MdeModulePkg: Move to new location

Contributed-under: TianoCore Contribution Agreement 1.0 Signed-off-by: Guo Mang <mang.guo@intel.com>
author: Guo Mang <mang.guo@intel.com> 2016-12-22 15:55:38 +0800
committer: Guo Mang <mang.guo@intel.com> 2016-12-26 19:14:37 +0800
commit: 7f05fa00f73038b425002566d3afe6c3ade2ccdb (patch)
tree: 297e208d4ade33a8bb3d5d20f72c53e0d134e003 /Core
parent: ed2ecce34b3830562c4239093a41ba92d76d5f31 (diff)
download: edk2-platforms-7f05fa00f73038b425002566d3afe6c3ade2ccdb.tar.xz
1441 files changed, 541828 insertions, 0 deletions
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.c b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.c
new file mode 100644
index 0000000000..be1341061e
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.c
@@ -0,0 +1,1079 @@
+/** @file
+  The application to show the Boot Manager Menu.
+
+Copyright (c) 2011 - 2015, 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 "BootManagerMenu.h"
+
+EFI_HII_HANDLE gStringPackHandle;
+
+BOOLEAN   mModeInitialized = FALSE;
+
+//
+// Boot video resolution and text mode.
+//
+UINT32    mBootHorizontalResolution    = 0;
+UINT32    mBootVerticalResolution      = 0;
+UINT32    mBootTextModeColumn          = 0;
+UINT32    mBootTextModeRow             = 0;
+//
+// BIOS setup video resolution and text mode.
+//
+UINT32    mSetupTextModeColumn         = 0;
+UINT32    mSetupTextModeRow            = 0;
+UINT32    mSetupHorizontalResolution   = 0;
+UINT32    mSetupVerticalResolution     = 0;
+
+/**
+  Prints a unicode string to the default console, at
+  the supplied cursor position, using L"%s" format.
+
+  @param  Column     The cursor position to print the string at.
+  @param  Row        The cursor position to print the string at
+  @param  String     String pointer.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+PrintStringAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *String
+  )
+{
+
+  gST->ConOut->SetCursorPosition (gST->ConOut, Column, Row);
+  return Print (L"%s", String);
+}
+
+/**
+  Prints a chracter to the default console, at
+  the supplied cursor position, using L"%c" format.
+
+  @param  Column     The cursor position to print the string at.
+  @param  Row        The cursor position to print the string at.
+  @param  Character  Character to print.
+
+  @return Length of string printed to the console.
+
+**/
+UINTN
+PrintCharAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  CHAR16       Character
+  )
+{
+  gST->ConOut->SetCursorPosition (gST->ConOut, Column, Row);
+  return Print (L"%c", Character);
+}
+
+/**
+  Count the storage space of a Unicode string which uses current lanaguag to get 
+  from input string ID.
+
+  @param StringId          The input string to be counted.
+
+  @return Storage space for the input string.
+
+**/
+UINTN
+GetLineWidth (
+  IN EFI_STRING_ID       StringId
+  )
+{  
+  UINTN        Index;
+  UINTN        IncrementValue;
+  EFI_STRING   String;
+  UINTN        LineWidth;
+  
+  LineWidth = 0;
+  String = HiiGetString (gStringPackHandle, StringId, NULL); 
+  
+  if (String != NULL) {
+    Index           = 0;
+    IncrementValue  = 1;
+    
+    do {
+      //
+      // Advance to the null-terminator or to the first width directive
+      //
+      for (;
+           (String[Index] != NARROW_CHAR) && (String[Index] != WIDE_CHAR) && (String[Index] != 0);
+           Index++, LineWidth = LineWidth + IncrementValue
+          )
+        ;
+    
+      //
+      // We hit the null-terminator, we now have a count
+      //
+      if (String[Index] == 0) {
+        break;
+      }
+      //
+      // We encountered a narrow directive - strip it from the size calculation since it doesn't get printed
+      // and also set the flag that determines what we increment by.(if narrow, increment by 1, if wide increment by 2)
+      //
+      if (String[Index] == NARROW_CHAR) {
+        //
+        // Skip to the next character
+        //
+        Index++;
+        IncrementValue = 1;
+      } else {
+        //
+        // Skip to the next character
+        //
+        Index++;
+        IncrementValue = 2;
+      }
+    } while (String[Index] != 0);   
+    FreePool (String);
+  }
+  
+  return LineWidth;  
+}
+
+/**
+  This function uses calculate the boot menu location, size and scroll bar information.
+
+  @param  BootMenuData            The boot menu data to be proccessed.
+
+  @return EFI_SUCCESS             calculate boot menu information successful.
+  @retval EFI_INVALID_PARAMETER   Input parameter is invalid   
+
+**/
+EFI_STATUS 
+InitializeBootMenuScreen (
+  IN OUT  BOOT_MENU_POPUP_DATA  *BootMenuData
+  )
+{
+  UINTN         MaxStrWidth;
+  UINTN         StrWidth;
+  UINTN         Index;
+  UINTN         Column;
+  UINTN         Row;
+  UINTN         MaxPrintRows;
+  UINTN         UnSelectableItmes;
+
+  if (BootMenuData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Get maximum string width
+  //
+  MaxStrWidth = 0;  
+  for (Index = 0; Index < TITLE_TOKEN_COUNT; Index++) {    
+    StrWidth = GetLineWidth (BootMenuData->TitleToken[Index]);
+    MaxStrWidth = MaxStrWidth > StrWidth ? MaxStrWidth : StrWidth;
+  }
+   
+  for (Index = 0; Index < BootMenuData->ItemCount; Index++) {
+    StrWidth = GetLineWidth (BootMenuData->PtrTokens[Index]);
+    MaxStrWidth = MaxStrWidth > StrWidth ? MaxStrWidth : StrWidth; 
+  } 
+  
+  for (Index = 0; Index < HELP_TOKEN_COUNT; Index++) {    
+    StrWidth = GetLineWidth (BootMenuData->HelpToken[Index]);
+    MaxStrWidth = MaxStrWidth > StrWidth ? MaxStrWidth : StrWidth;
+  }  
+  //
+  // query current row and column to calculate boot menu location
+  //
+  gST->ConOut->QueryMode (
+                 gST->ConOut,
+                 gST->ConOut->Mode->Mode,
+                 &Column,
+                 &Row
+                 ); 
+                 
+  MaxPrintRows = Row - 6;    
+  UnSelectableItmes = TITLE_TOKEN_COUNT + 2 + HELP_TOKEN_COUNT + 2;           
+  BootMenuData->MenuScreen.Width = MaxStrWidth + 8;
+  if (BootMenuData->ItemCount + UnSelectableItmes > MaxPrintRows) {
+    BootMenuData->MenuScreen.Height = MaxPrintRows;
+    BootMenuData->ScrollBarControl.HasScrollBar = TRUE;
+    BootMenuData->ScrollBarControl.ItemCountPerScreen = MaxPrintRows - UnSelectableItmes;
+    BootMenuData->ScrollBarControl.FirstItem = 0;
+    BootMenuData->ScrollBarControl.LastItem = MaxPrintRows - UnSelectableItmes - 1;
+  } else {
+    BootMenuData->MenuScreen.Height = BootMenuData->ItemCount + UnSelectableItmes;
+    BootMenuData->ScrollBarControl.HasScrollBar = FALSE;
+    BootMenuData->ScrollBarControl.ItemCountPerScreen = BootMenuData->ItemCount;
+    BootMenuData->ScrollBarControl.FirstItem = 0;
+    BootMenuData->ScrollBarControl.LastItem = BootMenuData->ItemCount - 1;    
+  }
+  BootMenuData->MenuScreen.StartCol = (Column -  BootMenuData->MenuScreen.Width) / 2;              
+  BootMenuData->MenuScreen.StartRow = (Row -  BootMenuData->MenuScreen.Height) / 2;  
+
+  return EFI_SUCCESS;
+}
+/**
+  This funciton uses check boot option is wheher setup application or no
+
+  @param   BootOption   Pointer to EFI_BOOT_MANAGER_LOAD_OPTION array.
+  
+  @retval  TRUE         This boot option is setup application.
+  @retval  FALSE        This boot options isn't setup application
+
+**/
+BOOLEAN
+IsBootManagerMenu (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION    *BootOption
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION        BootManagerMenu;
+
+  Status = EfiBootManagerGetBootManagerMenu (&BootManagerMenu);
+  if (!EFI_ERROR (Status)) {
+    EfiBootManagerFreeLoadOption (&BootManagerMenu);
+  }
+
+  return (BOOLEAN) (!EFI_ERROR (Status) && (BootOption->OptionNumber == BootManagerMenu.OptionNumber));
+}
+
+/**
+  Return whether to ignore the boot option.
+
+  @param BootOption  Pointer to EFI_BOOT_MANAGER_LOAD_OPTION to check.
+
+  @retval TRUE  Ignore the boot optin.
+  @retval FALSE Do not ignore the boot option.
+**/
+BOOLEAN
+IgnoreBootOption (
+  IN   EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_DEVICE_PATH_PROTOCOL      *ImageDevicePath;
+
+  //
+  // Ignore myself.
+  //
+  Status = gBS->HandleProtocol (gImageHandle, &gEfiLoadedImageDevicePathProtocolGuid, (VOID **) &ImageDevicePath);
+  ASSERT_EFI_ERROR (Status);
+  if (CompareMem (BootOption->FilePath, ImageDevicePath, GetDevicePathSize (ImageDevicePath)) == 0) {
+    return TRUE;
+  }
+
+  //
+  // Do not ignore Boot Manager Menu.
+  //
+  if (IsBootManagerMenu (BootOption)) {
+    return FALSE;
+  }
+
+  //
+  // Ignore the hidden/inactive boot option.
+  //
+  if (((BootOption->Attributes & LOAD_OPTION_HIDDEN) != 0) || ((BootOption->Attributes & LOAD_OPTION_ACTIVE) == 0)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  This funciton uses to initialize boot menu data
+
+  @param   BootOption             Pointer to EFI_BOOT_MANAGER_LOAD_OPTION array.
+  @param   BootOptionCount        Number of boot option.
+  @param   BootMenuData           The Input BootMenuData to be initialized.
+  
+  @retval  EFI_SUCCESS            Initialize boot menu data successful.
+  @retval  EFI_INVALID_PARAMETER  Input parameter is invalid.   
+
+**/
+EFI_STATUS
+InitializeBootMenuData (
+  IN   EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption,
+  IN   UINTN                         BootOptionCount,
+  OUT  BOOT_MENU_POPUP_DATA          *BootMenuData
+  )
+{
+  UINTN                         Index;
+  UINTN                         StrIndex;
+      
+  if (BootOption == NULL || BootMenuData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BootMenuData->TitleToken[0] = STRING_TOKEN (STR_BOOT_POPUP_MENU_TITLE_STRING);
+  BootMenuData->PtrTokens     = AllocateZeroPool (BootOptionCount * sizeof (EFI_STRING_ID));
+  ASSERT (BootMenuData->PtrTokens != NULL);
+
+  //
+  // Skip boot option which created by BootNext Variable
+  //
+  for (StrIndex = 0, Index = 0; Index < BootOptionCount; Index++) {
+    if (IgnoreBootOption (&BootOption[Index])) {
+      continue;
+    }
+
+    ASSERT (BootOption[Index].Description != NULL);
+    BootMenuData->PtrTokens[StrIndex++] = HiiSetString (
+                                            gStringPackHandle, 
+                                            0,
+                                            BootOption[Index].Description,
+                                            NULL
+                                            );
+  }
+
+  BootMenuData->ItemCount           = StrIndex;   
+  BootMenuData->HelpToken[0] = STRING_TOKEN (STR_BOOT_POPUP_MENU_HELP1_STRING);
+  BootMenuData->HelpToken[1] = STRING_TOKEN (STR_BOOT_POPUP_MENU_HELP2_STRING);
+  BootMenuData->HelpToken[2] = STRING_TOKEN (STR_BOOT_POPUP_MENU_HELP3_STRING);
+  InitializeBootMenuScreen (BootMenuData);
+  BootMenuData->SelectItem = 0;
+  return EFI_SUCCESS;
+}    
+
+/**
+  This function uses input select item to highlight selected item
+  and set current selected item in BootMenuData
+
+  @param  WantSelectItem          The user wants to select item.
+  @param  BootMenuData            The boot menu data to be proccessed
+
+  @return EFI_SUCCESS             Highlight selected item and update current selected 
+                                  item successful 
+  @retval EFI_INVALID_PARAMETER   Input parameter is invalid   
+**/
+EFI_STATUS
+BootMenuSelectItem (
+  IN     UINTN                 WantSelectItem,
+  IN OUT BOOT_MENU_POPUP_DATA  *BootMenuData
+  )
+{
+  INT32                 SavedAttribute;
+  EFI_STRING            String;
+  UINTN                 StartCol;  
+  UINTN                 StartRow;
+  UINTN                 PrintCol;
+  UINTN                 PrintRow;
+  UINTN                 TopShadeNum;
+  UINTN                 LowShadeNum;
+  UINTN                 FirstItem;
+  UINTN                 LastItem;
+  UINTN                 ItemCountPerScreen;
+  UINTN                 Index;
+  BOOLEAN               RePaintItems;
+  
+  if (BootMenuData == NULL || WantSelectItem >= BootMenuData->ItemCount) {
+    return EFI_INVALID_PARAMETER;
+  }
+  SavedAttribute = gST->ConOut->Mode->Attribute;
+  RePaintItems = FALSE;
+  StartCol = BootMenuData->MenuScreen.StartCol;
+  StartRow = BootMenuData->MenuScreen.StartRow;
+  //
+  // print selectable items again and adjust scroll bar if need
+  //         
+  if (BootMenuData->ScrollBarControl.HasScrollBar &&
+      (WantSelectItem < BootMenuData->ScrollBarControl.FirstItem ||
+      WantSelectItem > BootMenuData->ScrollBarControl.LastItem ||
+      WantSelectItem == BootMenuData->SelectItem)) {          
+    ItemCountPerScreen   = BootMenuData->ScrollBarControl.ItemCountPerScreen;
+    //
+    // Set first item and last item
+    //     
+    if (WantSelectItem < BootMenuData->ScrollBarControl.FirstItem) {
+      BootMenuData->ScrollBarControl.FirstItem = WantSelectItem;
+      BootMenuData->ScrollBarControl.LastItem = WantSelectItem + ItemCountPerScreen - 1;  
+    } else if (WantSelectItem > BootMenuData->ScrollBarControl.LastItem) {
+      BootMenuData->ScrollBarControl.FirstItem = WantSelectItem - ItemCountPerScreen + 1; 
+      BootMenuData->ScrollBarControl.LastItem = WantSelectItem;
+    }
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_WHITE | EFI_BACKGROUND_BLUE);
+    FirstItem = BootMenuData->ScrollBarControl.FirstItem;
+    LastItem  = BootMenuData->ScrollBarControl.LastItem;
+    TopShadeNum = 0;
+    if (FirstItem != 0) {
+      TopShadeNum = (FirstItem * ItemCountPerScreen) / BootMenuData->ItemCount;
+      if ((FirstItem * ItemCountPerScreen) % BootMenuData->ItemCount != 0) {
+        TopShadeNum++;
+      }
+      PrintCol = StartCol  + BootMenuData->MenuScreen.Width - 2;
+      PrintRow = StartRow + TITLE_TOKEN_COUNT + 2;  
+      for (Index = 0; Index < TopShadeNum; Index++, PrintRow++) {
+        PrintCharAt (PrintCol, PrintRow, BLOCKELEMENT_LIGHT_SHADE);
+      }
+    }
+    LowShadeNum = 0;
+    if (LastItem != BootMenuData->ItemCount - 1) {
+      LowShadeNum = ((BootMenuData->ItemCount - 1 - LastItem) * ItemCountPerScreen) / BootMenuData->ItemCount;
+      if (((BootMenuData->ItemCount - 1 - LastItem) * ItemCountPerScreen) % BootMenuData->ItemCount != 0) {
+        LowShadeNum++;
+      }
+      PrintCol = StartCol  + BootMenuData->MenuScreen.Width - 2;
+      PrintRow = StartRow + TITLE_TOKEN_COUNT + 2 + ItemCountPerScreen - LowShadeNum;  
+      for (Index = 0; Index < LowShadeNum; Index++, PrintRow++) {
+        PrintCharAt (PrintCol, PrintRow, BLOCKELEMENT_LIGHT_SHADE);
+      } 
+    }
+    PrintCol = StartCol  + BootMenuData->MenuScreen.Width - 2;
+    PrintRow = StartRow + TITLE_TOKEN_COUNT + 2 + TopShadeNum;  
+    for (Index = TopShadeNum; Index < ItemCountPerScreen - LowShadeNum; Index++, PrintRow++) {
+      PrintCharAt (PrintCol, PrintRow, BLOCKELEMENT_FULL_BLOCK);
+    }      
+
+
+    //
+    // Clear selectable items first
+    //
+    PrintCol = StartCol  + 1;
+    PrintRow = StartRow + TITLE_TOKEN_COUNT + 2;  
+    String = AllocateZeroPool ((BootMenuData->MenuScreen.Width - 2) * sizeof (CHAR16));
+    ASSERT (String != NULL);
+    for (Index = 0; Index < BootMenuData->MenuScreen.Width - 3; Index++) {
+      String[Index] = 0x20;
+    }      
+    for (Index = 0; Index < ItemCountPerScreen; Index++) {        
+      PrintStringAt (PrintCol, PrintRow + Index, String); 
+    }
+    FreePool (String);
+    //
+    // print selectable items  
+    //
+    for (Index = 0; Index < ItemCountPerScreen; Index++, PrintRow++) {
+      String = HiiGetString (gStringPackHandle, BootMenuData->PtrTokens[Index + FirstItem], NULL);
+      PrintStringAt (PrintCol, PrintRow, String);
+      FreePool (String); 
+    }
+    RePaintItems = TRUE;
+  }
+  
+  //
+  // Print want to select item 
+  //
+  FirstItem = BootMenuData->ScrollBarControl.FirstItem;
+  gST->ConOut->SetAttribute (gST->ConOut, EFI_WHITE | EFI_BACKGROUND_BLACK);
+  String = HiiGetString (gStringPackHandle, BootMenuData->PtrTokens[WantSelectItem], NULL);
+  PrintCol = StartCol  + 1;  
+  PrintRow = StartRow + TITLE_TOKEN_COUNT + 2 + WantSelectItem - FirstItem;  
+  PrintStringAt (PrintCol, PrintRow, String);
+  FreePool (String);
+  
+  //
+  // if Want Select and selected item isn't the same and doesn't re-draw selectable 
+  // items, clear select item
+  //
+  if (WantSelectItem != BootMenuData->SelectItem && !RePaintItems) {
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_WHITE | EFI_BACKGROUND_BLUE);
+    String = HiiGetString (gStringPackHandle, BootMenuData->PtrTokens[BootMenuData->SelectItem], NULL);
+    PrintCol = StartCol  + 1;  
+    PrintRow = StartRow + 3 + BootMenuData->SelectItem - FirstItem;  
+    PrintStringAt (PrintCol, PrintRow, String);
+    FreePool (String);    
+  }
+
+  gST->ConOut->SetAttribute (gST->ConOut, SavedAttribute);
+  BootMenuData->SelectItem = WantSelectItem;
+  return EFI_SUCCESS;
+}
+
+/**
+  This funciton uses to draw boot popup menu
+
+  @param   BootMenuData           The Input BootMenuData to be processed.
+  
+  @retval  EFI_SUCCESS            Draw boot popup menu successful.
+
+**/
+EFI_STATUS 
+DrawBootPopupMenu (
+  IN  BOOT_MENU_POPUP_DATA  *BootMenuData
+  )
+{
+  EFI_STRING            String;
+  UINTN                 Index;
+  UINTN                 Width;  
+  UINTN                 StartCol;
+  UINTN                 StartRow;
+  UINTN                 PrintRow;
+  UINTN                 PrintCol;
+  UINTN                 LineWidth;
+  INT32                 SavedAttribute; 
+  UINTN                 ItemCountPerScreen;  
+
+  gST->ConOut->ClearScreen (gST->ConOut);
+  
+  SavedAttribute = gST->ConOut->Mode->Attribute;
+  gST->ConOut->SetAttribute (gST->ConOut, EFI_WHITE | EFI_BACKGROUND_BLUE);
+  Width    = BootMenuData->MenuScreen.Width;
+  StartCol = BootMenuData->MenuScreen.StartCol;
+  StartRow = BootMenuData->MenuScreen.StartRow;
+  ItemCountPerScreen = BootMenuData->ScrollBarControl.ItemCountPerScreen;
+  PrintRow = StartRow;
+ 
+  gST->ConOut->EnableCursor (gST->ConOut, FALSE);
+  //
+  // Draw Boot popup menu screen
+  //
+  PrintCharAt (StartCol, PrintRow, BOXDRAW_DOWN_RIGHT);
+  for (Index = 1; Index < Width - 1; Index++) {
+    PrintCharAt (StartCol + Index, PrintRow, BOXDRAW_HORIZONTAL); 
+  }
+  PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_DOWN_LEFT);
+  
+  //
+  // Draw the screen for title
+  //
+  String = AllocateZeroPool ((Width - 1) * sizeof (CHAR16));
+  ASSERT (String != NULL);
+  for (Index = 0; Index < Width - 2; Index++) {
+    String[Index] = 0x20;
+  }
+
+  for (Index = 0; Index < TITLE_TOKEN_COUNT; Index++) {
+    PrintRow++;
+    PrintCharAt (StartCol, PrintRow, BOXDRAW_VERTICAL);  
+    PrintStringAt (StartCol + 1, PrintRow, String);
+    PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_VERTICAL);
+  }
+  
+  PrintRow++;
+  PrintCharAt (StartCol, PrintRow, BOXDRAW_VERTICAL_RIGHT);
+  for (Index = 1; Index < Width - 1; Index++) {
+    PrintCharAt (StartCol + Index, PrintRow, BOXDRAW_HORIZONTAL); 
+  }
+  PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_VERTICAL_LEFT);  
+  
+  //
+  // Draw screen for selectable items
+  //
+  for (Index = 0; Index < ItemCountPerScreen; Index++) {
+    PrintRow++;
+    PrintCharAt (StartCol, PrintRow, BOXDRAW_VERTICAL);
+    PrintStringAt (StartCol + 1, PrintRow, String);
+    PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_VERTICAL);
+  }  
+
+  PrintRow++;
+  PrintCharAt (StartCol, PrintRow, BOXDRAW_VERTICAL_RIGHT);
+  for (Index = 1; Index < Width - 1; Index++) {
+    PrintCharAt (StartCol + Index, PrintRow, BOXDRAW_HORIZONTAL); 
+  }
+  PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_VERTICAL_LEFT);
+  
+  //
+  // Draw screen for Help
+  //
+  for (Index = 0; Index < HELP_TOKEN_COUNT; Index++) {
+    PrintRow++;
+    PrintCharAt (StartCol, PrintRow, BOXDRAW_VERTICAL);
+    PrintStringAt (StartCol + 1, PrintRow, String);
+    PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_VERTICAL);
+  }
+  FreePool (String);  
+    
+  PrintRow++;  
+  PrintCharAt (StartCol, PrintRow, BOXDRAW_UP_RIGHT);
+  for (Index = 1; Index < Width - 1; Index++) {
+    PrintCharAt (StartCol + Index, PrintRow, BOXDRAW_HORIZONTAL); 
+  }
+  PrintCharAt (StartCol + Width - 1, PrintRow, BOXDRAW_UP_LEFT);        
+  
+  
+  //
+  // print title strings
+  //
+  PrintRow = StartRow + 1;
+  for (Index = 0; Index < TITLE_TOKEN_COUNT; Index++, PrintRow++) {
+    String = HiiGetString (gStringPackHandle, BootMenuData->TitleToken[Index], NULL);
+    LineWidth = GetLineWidth (BootMenuData->TitleToken[Index]);      
+    PrintCol = StartCol + (Width - LineWidth) / 2;
+    PrintStringAt (PrintCol, PrintRow, String);
+    FreePool (String);
+  }
+  
+  //
+  // print selectable items
+  //
+  PrintCol = StartCol + 1;
+  PrintRow = StartRow + TITLE_TOKEN_COUNT + 2;  
+  for (Index = 0; Index < ItemCountPerScreen; Index++, PrintRow++) {
+    String = HiiGetString (gStringPackHandle, BootMenuData->PtrTokens[Index], NULL);
+    PrintStringAt (PrintCol, PrintRow, String);
+    FreePool (String); 
+  }
+  
+  //
+  // Print Help strings
+  //
+  PrintRow++;
+  for (Index = 0; Index < HELP_TOKEN_COUNT; Index++, PrintRow++) {
+    String = HiiGetString (gStringPackHandle, BootMenuData->HelpToken[Index], NULL);
+    LineWidth = GetLineWidth (BootMenuData->HelpToken[Index]);
+    PrintCol = StartCol + (Width - LineWidth) / 2;
+    PrintStringAt (PrintCol, PrintRow, String);
+    FreePool (String);
+  }
+  
+  //
+  // Print scroll bar if has scroll bar
+  //
+  if (BootMenuData->ScrollBarControl.HasScrollBar) {
+    PrintCol = StartCol + Width - 2;
+    PrintRow = StartRow + 2; 
+    PrintCharAt (PrintCol, PrintRow, GEOMETRICSHAPE_UP_TRIANGLE); 
+    PrintCharAt (PrintCol + 1, PrintRow, BOXDRAW_VERTICAL); 
+    PrintRow += (ItemCountPerScreen + 1);    
+    PrintCharAt (PrintCol, PrintRow, GEOMETRICSHAPE_DOWN_TRIANGLE);
+    PrintCharAt (PrintCol + 1, PrintRow, BOXDRAW_VERTICAL); 
+  }  
+    
+  gST->ConOut->SetAttribute (gST->ConOut, SavedAttribute);
+  //
+  // Print Selected item
+  //
+  BootMenuSelectItem (BootMenuData->SelectItem, BootMenuData);
+  return EFI_SUCCESS;
+}
+
+/**
+  This funciton uses to boot from selected item 
+
+  @param   BootOptions            Pointer to EFI_BOOT_MANAGER_LOAD_OPTION array.
+  @param   BootOptionCount        Number of boot option.
+  @param   SelectItem             Current selected item.
+**/
+VOID
+BootFromSelectOption (
+  IN   EFI_BOOT_MANAGER_LOAD_OPTION  *BootOptions,
+  IN   UINTN                         BootOptionCount, 
+  IN   UINTN                         SelectItem
+  )
+{
+  UINTN                 ItemNum;
+  UINTN                 Index;
+
+  ASSERT (BootOptions != NULL);
+
+  for (ItemNum = 0, Index = 0; Index < BootOptionCount; Index++) {
+    if (IgnoreBootOption (&BootOptions[Index])) {
+      continue;
+    }
+
+    if (ItemNum++ == SelectItem) {
+      EfiBootManagerBoot (&BootOptions[Index]);
+      break;
+    }
+  }
+}
+
+/**
+  This function will change video resolution and text mode
+  according to defined setup mode or defined boot mode  
+
+  @param  IsSetupMode   Indicate mode is changed to setup mode or boot mode. 
+
+  @retval  EFI_SUCCESS  Mode is changed successfully.
+  @retval  Others             Mode failed to be changed.
+
+**/
+EFI_STATUS
+EFIAPI
+BdsSetConsoleMode (
+  BOOLEAN  IsSetupMode
+  )
+{
+  EFI_GRAPHICS_OUTPUT_PROTOCOL          *GraphicsOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL       *SimpleTextOut;
+  UINTN                                 SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  *Info;
+  UINT32                                MaxGopMode;
+  UINT32                                MaxTextMode;
+  UINT32                                ModeNumber;
+  UINT32                                NewHorizontalResolution;
+  UINT32                                NewVerticalResolution;
+  UINT32                                NewColumns;
+  UINT32                                NewRows;
+  UINTN                                 HandleCount;
+  EFI_HANDLE                            *HandleBuffer;
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  UINTN                                 CurrentColumn;
+  UINTN                                 CurrentRow;  
+
+  MaxGopMode  = 0;
+  MaxTextMode = 0;
+
+  //
+  // Get current video resolution and text mode 
+  //
+  Status = gBS->HandleProtocol (
+                  gST->ConsoleOutHandle,
+                  &gEfiGraphicsOutputProtocolGuid,
+                  (VOID**)&GraphicsOutput
+                  );
+  if (EFI_ERROR (Status)) {
+    GraphicsOutput = NULL;
+  }
+
+  Status = gBS->HandleProtocol (
+                  gST->ConsoleOutHandle,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  (VOID**)&SimpleTextOut
+                  );
+  if (EFI_ERROR (Status)) {
+    SimpleTextOut = NULL;
+  }  
+
+  if ((GraphicsOutput == NULL) || (SimpleTextOut == NULL)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (IsSetupMode) {
+    //
+    // The requried resolution and text mode is setup mode.
+    //
+    NewHorizontalResolution = mSetupHorizontalResolution;
+    NewVerticalResolution   = mSetupVerticalResolution;
+    NewColumns              = mSetupTextModeColumn;
+    NewRows                 = mSetupTextModeRow;
+  } else {
+    //
+    // The required resolution and text mode is boot mode.
+    //
+    NewHorizontalResolution = mBootHorizontalResolution;
+    NewVerticalResolution   = mBootVerticalResolution;
+    NewColumns              = mBootTextModeColumn;
+    NewRows                 = mBootTextModeRow;   
+  }
+  
+  if (GraphicsOutput != NULL) {
+    MaxGopMode  = GraphicsOutput->Mode->MaxMode;
+  } 
+
+  if (SimpleTextOut != NULL) {
+    MaxTextMode = SimpleTextOut->Mode->MaxMode;
+  }
+
+  //
+  // 1. If current video resolution is same with required video resolution,
+  //    video resolution need not be changed.
+  //    1.1. If current text mode is same with required text mode, text mode need not be changed.
+  //    1.2. If current text mode is different from required text mode, text mode need be changed.
+  // 2. If current video resolution is different from required video resolution, we need restart whole console drivers.
+  //
+  for (ModeNumber = 0; ModeNumber < MaxGopMode; ModeNumber++) {
+    Status = GraphicsOutput->QueryMode (
+                       GraphicsOutput,
+                       ModeNumber,
+                       &SizeOfInfo,
+                       &Info
+                       );
+    if (!EFI_ERROR (Status)) {
+      if ((Info->HorizontalResolution == NewHorizontalResolution) &&
+          (Info->VerticalResolution == NewVerticalResolution)) {
+        if ((GraphicsOutput->Mode->Info->HorizontalResolution == NewHorizontalResolution) &&
+            (GraphicsOutput->Mode->Info->VerticalResolution == NewVerticalResolution)) {
+          //
+          // Current resolution is same with required resolution, check if text mode need be set
+          //
+          Status = SimpleTextOut->QueryMode (SimpleTextOut, SimpleTextOut->Mode->Mode, &CurrentColumn, &CurrentRow);
+          ASSERT_EFI_ERROR (Status);
+          if (CurrentColumn == NewColumns && CurrentRow == NewRows) {
+            //
+            // If current text mode is same with required text mode. Do nothing
+            //
+            FreePool (Info);
+            return EFI_SUCCESS;
+          } else {
+            //
+            // If current text mode is different from requried text mode.  Set new video mode
+            //
+            for (Index = 0; Index < MaxTextMode; Index++) {
+              Status = SimpleTextOut->QueryMode (SimpleTextOut, Index, &CurrentColumn, &CurrentRow);
+              if (!EFI_ERROR(Status)) {
+                if ((CurrentColumn == NewColumns) && (CurrentRow == NewRows)) {
+                  //
+                  // Required text mode is supported, set it.
+                  //
+                  Status = SimpleTextOut->SetMode (SimpleTextOut, Index);
+                  ASSERT_EFI_ERROR (Status);
+                  //
+                  // Update text mode PCD.
+                  //
+                  Status = PcdSet32S (PcdConOutColumn, mSetupTextModeColumn);
+                  ASSERT_EFI_ERROR (Status);
+                  Status = PcdSet32S (PcdConOutRow, mSetupTextModeRow);
+                  ASSERT_EFI_ERROR (Status);
+                  FreePool (Info);
+                  return EFI_SUCCESS;
+                }
+              }
+            }
+            if (Index == MaxTextMode) {
+              //
+              // If requried text mode is not supported, return error.
+              //
+              FreePool (Info);
+              return EFI_UNSUPPORTED;
+            }
+          }
+        } else {
+          //
+          // If current video resolution is not same with the new one, set new video resolution.
+          // In this case, the driver which produces simple text out need be restarted.
+          //
+          Status = GraphicsOutput->SetMode (GraphicsOutput, ModeNumber);
+          if (!EFI_ERROR (Status)) {
+            FreePool (Info);
+            break;
+          }
+        }
+      }
+      FreePool (Info);
+    }
+  }
+
+  if (ModeNumber == MaxGopMode) {
+    //
+    // If the resolution is not supported, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Set PCD to Inform GraphicsConsole to change video resolution.
+  // Set PCD to Inform Consplitter to change text mode.
+  //
+  Status = PcdSet32S (PcdVideoHorizontalResolution, NewHorizontalResolution);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdVideoVerticalResolution, NewVerticalResolution);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdConOutColumn, NewColumns);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdConOutRow, NewRows);
+  ASSERT_EFI_ERROR (Status);
+  
+  //
+  // Video mode is changed, so restart graphics console driver and higher level driver.
+  // Reconnect graphics console driver and higher level driver.
+  // Locate all the handles with GOP protocol and reconnect it.
+  //
+  Status = gBS->LocateHandleBuffer (
+                   ByProtocol,
+                   &gEfiSimpleTextOutProtocolGuid,
+                   NULL,
+                   &HandleCount,
+                   &HandleBuffer
+                   );
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      gBS->DisconnectController (HandleBuffer[Index], NULL, NULL);
+    }
+    for (Index = 0; Index < HandleCount; Index++) {
+      gBS->ConnectController (HandleBuffer[Index], NULL, NULL, TRUE);
+    }
+    if (HandleBuffer != NULL) {
+      FreePool (HandleBuffer);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Display the boot popup menu and allow user select boot item.
+
+  @param   ImageHandle     The image handle.
+  @param   SystemTable     The system table.
+  
+  @retval  EFI_SUCCESS          Boot from selected boot option, and return success from boot option
+  @retval  EFI_NOT_FOUND        User select to enter setup or can not find boot option
+  
+**/
+EFI_STATUS
+EFIAPI
+BootManagerMenuEntry (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  )
+{
+  EFI_BOOT_MANAGER_LOAD_OPTION    *BootOption;
+  UINTN                           BootOptionCount;  
+  EFI_STATUS                      Status;
+  BOOT_MENU_POPUP_DATA            BootMenuData;
+  UINTN                           Index;
+  EFI_INPUT_KEY                   Key;
+  BOOLEAN                         ExitApplication;
+  UINTN                           SelectItem;
+  EFI_BOOT_LOGO_PROTOCOL          *BootLogo;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *SimpleTextOut;
+  UINTN                           BootTextColumn;
+  UINTN                           BootTextRow;
+
+  //
+  // Set Logo status invalid when boot manager menu is launched
+  //
+  BootLogo = NULL;
+  Status = gBS->LocateProtocol (&gEfiBootLogoProtocolGuid, NULL, (VOID **) &BootLogo);
+  if (!EFI_ERROR (Status) && (BootLogo != NULL)) {
+    Status = BootLogo->SetBootLogo (BootLogo, NULL, 0, 0, 0, 0);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  gBS->SetWatchdogTimer (0x0000, 0x0000, 0x0000, NULL);
+
+  gStringPackHandle = HiiAddPackages (
+                         &gEfiCallerIdGuid,
+                         gImageHandle,
+                         BootManagerMenuAppStrings,
+                         NULL
+                         );
+  ASSERT (gStringPackHandle != NULL);
+
+  //
+  // Connect all prior to entering the platform setup menu.
+  //
+  EfiBootManagerConnectAll ();
+  EfiBootManagerRefreshAllBootOption ();
+
+  BootOption = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+  if (!mModeInitialized) {
+    //
+    // After the console is ready, get current video resolution 
+    // and text mode before launching setup at first time.
+    //
+    Status = gBS->HandleProtocol (
+                    gST->ConsoleOutHandle,
+                    &gEfiGraphicsOutputProtocolGuid,
+                    (VOID**)&GraphicsOutput
+                    );
+    if (EFI_ERROR (Status)) {
+      GraphicsOutput = NULL;
+    }
+    
+    Status = gBS->HandleProtocol (
+                    gST->ConsoleOutHandle,
+                    &gEfiSimpleTextOutProtocolGuid,
+                    (VOID**)&SimpleTextOut
+                    );
+    if (EFI_ERROR (Status)) {
+      SimpleTextOut = NULL;
+    }  
+
+    if (GraphicsOutput != NULL) {
+      //
+      // Get current video resolution and text mode.
+      //
+      mBootHorizontalResolution = GraphicsOutput->Mode->Info->HorizontalResolution;
+      mBootVerticalResolution   = GraphicsOutput->Mode->Info->VerticalResolution;
+    }
+
+    if (SimpleTextOut != NULL) {
+      Status = SimpleTextOut->QueryMode (
+                                SimpleTextOut,
+                                SimpleTextOut->Mode->Mode,
+                                &BootTextColumn,
+                                &BootTextRow
+                                );
+      mBootTextModeColumn = (UINT32)BootTextColumn;
+      mBootTextModeRow    = (UINT32)BootTextRow;
+    }
+
+    //
+    // Get user defined text mode for setup.
+    //  
+    mSetupHorizontalResolution = PcdGet32 (PcdSetupVideoHorizontalResolution);
+    mSetupVerticalResolution   = PcdGet32 (PcdSetupVideoVerticalResolution);      
+    mSetupTextModeColumn       = PcdGet32 (PcdSetupConOutColumn);
+    mSetupTextModeRow          = PcdGet32 (PcdSetupConOutRow);
+    mModeInitialized           = TRUE;
+  }
+  
+  //
+  // Set back to conventional setup resolution
+  //
+  BdsSetConsoleMode (TRUE);
+
+  //
+  // Initialize Boot menu data
+  //
+  Status = InitializeBootMenuData (BootOption, BootOptionCount, &BootMenuData);
+  //
+  // According to boot menu data to draw boot popup menu
+  //
+  DrawBootPopupMenu (&BootMenuData);
+  
+  //
+  // check user input to determine want to re-draw or boot from user selected item
+  //
+  ExitApplication = FALSE;
+  while (!ExitApplication) {
+    gBS->WaitForEvent (1, &gST->ConIn->WaitForKey, &Index);
+    Status = gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+    if (!EFI_ERROR (Status)) {
+      switch (Key.UnicodeChar) {
+      
+      case CHAR_NULL:        
+        switch (Key.ScanCode) {          
+          
+        case SCAN_UP:
+          SelectItem = BootMenuData.SelectItem == 0 ? BootMenuData.ItemCount - 1 : BootMenuData.SelectItem - 1;
+          BootMenuSelectItem (SelectItem, &BootMenuData); 
+          break;
+        
+        case SCAN_DOWN:
+          SelectItem = BootMenuData.SelectItem == BootMenuData.ItemCount - 1 ? 0 : BootMenuData.SelectItem + 1;
+          BootMenuSelectItem (SelectItem, &BootMenuData); 
+          break;
+
+        case SCAN_ESC:
+          gST->ConOut->ClearScreen (gST->ConOut);
+          ExitApplication = TRUE;
+          //
+          // Set boot resolution for normal boot
+          //
+          BdsSetConsoleMode (FALSE);
+          break;
+          
+        default:
+          break;
+        }
+        break;
+        
+      case CHAR_CARRIAGE_RETURN:
+        gST->ConOut->ClearScreen (gST->ConOut);
+        //
+        // Set boot resolution for normal boot
+        //
+        BdsSetConsoleMode (FALSE);
+        BootFromSelectOption (BootOption, BootOptionCount, BootMenuData.SelectItem);
+        //
+        // Back to boot manager menu again, set back to setup resolution
+        //
+        BdsSetConsoleMode (TRUE);
+        DrawBootPopupMenu (&BootMenuData);
+        break;
+        
+      default:
+        break;
+      }
+    }
+  }
+  EfiBootManagerFreeLoadOptions (BootOption, BootOptionCount);
+  FreePool (BootMenuData.PtrTokens);
+
+  HiiRemovePackages (gStringPackHandle);
+
+  return Status;
+  
+}
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.h b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.h
new file mode 100644
index 0000000000..26d9a31865
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenu.h
@@ -0,0 +1,60 @@
+/** @file
+  FrontPage routines to handle the callbacks and browser calls
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+
+#ifndef _BOOT_MANAGER_MENU_H_
+#define _BOOT_MANAGER_MENU_H_
+
+#include <Uefi.h>
+#include <Guid/MdeModuleHii.h>
+#include <Library/UefiBootManagerLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/HiiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DevicePathLib.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/BootLogo.h>
+
+#define TITLE_TOKEN_COUNT   1
+#define HELP_TOKEN_COUNT    3
+
+typedef struct _BOOT_MENU_SCREEN {
+  UINTN        StartCol;
+  UINTN        StartRow;
+  UINTN        Width;
+  UINTN        Height;
+} BOOT_MENU_SCREEN; 
+
+typedef struct _BOOT_MENU_SCROLL_BAR_CONTROL {
+  BOOLEAN      HasScrollBar;
+  UINTN        ItemCountPerScreen;
+  UINTN        FirstItem;
+  UINTN        LastItem;
+} BOOT_MENU_SCROLL_BAR_CONTROL; 
+
+typedef struct _BOOT_MENU_POPUP_DATA {
+  EFI_STRING_ID                   TitleToken[TITLE_TOKEN_COUNT]; // Title string ID
+  UINTN                           ItemCount;                     // Selectable item count
+  EFI_STRING_ID                   *PtrTokens;                    // All of selectable items string ID
+  EFI_STRING_ID                   HelpToken[HELP_TOKEN_COUNT];   // All of help string ID
+  UINTN                           SelectItem;                    // Current select  item	
+  BOOT_MENU_SCREEN                MenuScreen;                    // Boot menu screen information
+  BOOT_MENU_SCROLL_BAR_CONTROL    ScrollBarControl;              // Boot menu scroll bar inoformation
+} BOOT_MENU_POPUP_DATA;
+
+#endif 
+
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.inf b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.inf
new file mode 100644
index 0000000000..dd60ef4cd8
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.inf
@@ -0,0 +1,68 @@
+## @file
+#  The application to show the Boot Manager Menu.
+#
+#  The application pops up a menu showing all the boot options referenced by
+#  BootOrder NV variable and user can choose to boot from one of them.
+#  
+#  Copyright (c) 2011 - 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BootManagerMenuApp
+  MODULE_UNI_FILE                = BootManagerMenuApp.uni
+  FILE_GUID                      = EEC25BDC-67F2-4D95-B1D5-F81B2039D11D
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 1.0 
+  ENTRY_POINT                    = BootManagerMenuEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BootManagerMenu.c
+  BootManagerMenu.h
+  BootManagerMenuStrings.uni
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  HiiLib
+  DebugLib
+  UefiLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiApplicationEntryPoint
+  UefiBootManagerLib
+  
+[Guids]
+
+[Protocols]
+  gEfiBootLogoProtocolGuid                      ## CONSUMES
+  gEfiLoadedImageDevicePathProtocolGuid         ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutRow                          ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutColumn                       ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoHorizontalResolution          ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoVerticalResolution            ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutColumn                  ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutRow                     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoHorizontalResolution     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoVerticalResolution       ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  BootManagerMenuAppExtra.uni
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.uni b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.uni
new file mode 100644
index 0000000000..a93c11cb5e
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.uni
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuAppExtra.uni b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuAppExtra.uni
new file mode 100644
index 0000000000..f26cc6b249
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuAppExtra.uni
diff --git a/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuStrings.uni b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuStrings.uni
new file mode 100644
index 0000000000..04fdd30fa7
--- /dev/null
+++ b/Core/MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuStrings.uni
diff --git a/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.c b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.c
new file mode 100644
index 0000000000..1821b2dd76
--- /dev/null
+++ b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.c
@@ -0,0 +1,57 @@
+/** @file
+  This sample application bases on HelloWorld PCD setting 
+  to print "UEFI Hello World!" to the UEFI Console.
+
+  Copyright (c) 2006 - 2008, 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 <Uefi.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiApplicationEntryPoint.h>
+
+
+/**
+  The user Entry Point for Application. The user code starts with this function
+  as the real entry point for the application.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+UefiMain (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+	UINT32 Index;
+	
+	Index = 0;
+  
+  //
+  // Three PCD type (FeatureFlag, UINT32 and String) are used as the sample.
+  //
+  if (FeaturePcdGet (PcdHelloWorldPrintEnable)) {
+  	for (Index = 0; Index < PcdGet32 (PcdHelloWorldPrintTimes); Index ++) {
+  	  //
+  	  // Use UefiLib Print API to print string to UEFI console
+  	  //
+    	Print ((CHAR16*)PcdGetPtr (PcdHelloWorldPrintString));
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.inf b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.inf
new file mode 100644
index 0000000000..6514ef32ec
--- /dev/null
+++ b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.inf
@@ -0,0 +1,56 @@
+## @file
+#  Sample UEFI Application Reference EDKII Module.
+#
+#  This is a sample shell application that will print "UEFI Hello World!" to the
+#  UEFI Console based on PCD setting.
+#
+#  It demos how to use EDKII PCD mechanism to make code more flexible.
+#
+#  Copyright (c) 2008 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = HelloWorld
+  MODULE_UNI_FILE                = HelloWorld.uni
+  FILE_GUID                      = 6987936E-ED34-44db-AE97-1FA5E4ED2116
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = UefiMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  HelloWorld.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiApplicationEntryPoint
+  UefiLib
+  PcdLib
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintEnable   ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintString || gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintEnable    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintTimes  || gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintEnable    ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  HelloWorldExtra.uni
diff --git a/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.uni b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.uni
new file mode 100644
index 0000000000..c24be4c135
--- /dev/null
+++ b/Core/MdeModulePkg/Application/HelloWorld/HelloWorld.uni
diff --git a/Core/MdeModulePkg/Application/HelloWorld/HelloWorldExtra.uni b/Core/MdeModulePkg/Application/HelloWorld/HelloWorldExtra.uni
new file mode 100644
index 0000000000..c9ef65c371
--- /dev/null
+++ b/Core/MdeModulePkg/Application/HelloWorld/HelloWorldExtra.uni
diff --git a/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.c b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.c
new file mode 100644
index 0000000000..ea2a00bd83
--- /dev/null
+++ b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.c
@@ -0,0 +1,804 @@
+/** @file
+  
+  Copyright (c) 2014 - 2016, 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 <Base.h>
+#include <Uefi.h>
+#include <PiDxe.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiApplicationEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/PrintLib.h>
+
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/SmmAccess2.h>
+
+#include <Guid/ZeroGuid.h>
+#include <Guid/MemoryProfile.h>
+#include <Guid/PiSmmCommunicationRegionTable.h>
+
+CHAR16 *mActionString[] = {
+  L"Unknown",
+  L"AllocatePages",
+  L"FreePages",
+  L"AllocatePool",
+  L"FreePool",
+};
+
+CHAR16 *mMemoryTypeString[] = {
+  L"EfiReservedMemoryType",
+  L"EfiLoaderCode",
+  L"EfiLoaderData",
+  L"EfiBootServicesCode",
+  L"EfiBootServicesData",
+  L"EfiRuntimeServicesCode",
+  L"EfiRuntimeServicesData",
+  L"EfiConventionalMemory",
+  L"EfiUnusableMemory",
+  L"EfiACPIReclaimMemory",
+  L"EfiACPIMemoryNVS",
+  L"EfiMemoryMappedIO",
+  L"EfiMemoryMappedIOPortSpace",
+  L"EfiPalCode",
+  L"EfiPersistentMemory",
+  L"EfiOSReserved",
+  L"EfiOemReserved",
+};
+
+CHAR16 *mSubsystemString[] = {
+  L"Unknown",
+  L"NATIVE",
+  L"WINDOWS_GUI",
+  L"WINDOWS_CUI",
+  L"Unknown",
+  L"Unknown",
+  L"Unknown",
+  L"POSIX_CUI",
+  L"Unknown",
+  L"WINDOWS_CE_GUI",
+  L"EFI_APPLICATION",
+  L"EFI_BOOT_SERVICE_DRIVER",
+  L"EFI_RUNTIME_DRIVER",
+  L"EFI_ROM",
+  L"XBOX",
+  L"Unknown",
+};
+
+CHAR16 *mFileTypeString[] = {
+  L"Unknown",
+  L"RAW",
+  L"FREEFORM",
+  L"SECURITY_CORE",
+  L"PEI_CORE",
+  L"DXE_CORE",
+  L"PEIM",
+  L"DRIVER",
+  L"COMBINED_PEIM_DRIVER",
+  L"APPLICATION",
+  L"SMM",
+  L"FIRMWARE_VOLUME_IMAGE",
+  L"COMBINED_SMM_DXE",
+  L"SMM_CORE",
+};
+
+#define PROFILE_NAME_STRING_LENGTH  36
+CHAR16 mNameString[PROFILE_NAME_STRING_LENGTH + 1];
+
+/** 
+  Get the file name portion of the Pdb File Name.
+  
+  The portion of the Pdb File Name between the last backslash and
+  either a following period or the end of the string is converted
+  to Unicode and copied into UnicodeBuffer.  The name is truncated,
+  if necessary, to ensure that UnicodeBuffer is not overrun.
+  
+  @param[in]  PdbFileName     Pdb file name.
+  @param[out] UnicodeBuffer   The resultant Unicode File Name.
+  
+**/
+VOID
+GetShortPdbFileName (
+  IN  CHAR8     *PdbFileName,
+  OUT CHAR16    *UnicodeBuffer
+  )
+{
+  UINTN IndexA;     // Current work location within an ASCII string.
+  UINTN IndexU;     // Current work location within a Unicode string.
+  UINTN StartIndex;
+  UINTN EndIndex;
+
+  ZeroMem (UnicodeBuffer, (PROFILE_NAME_STRING_LENGTH + 1) * sizeof (CHAR16));
+
+  if (PdbFileName == NULL) {
+    StrnCpyS (UnicodeBuffer, PROFILE_NAME_STRING_LENGTH + 1, L" ", 1);
+  } else {
+    StartIndex = 0;
+    for (EndIndex = 0; PdbFileName[EndIndex] != 0; EndIndex++);
+    for (IndexA = 0; PdbFileName[IndexA] != 0; IndexA++) {
+      if (PdbFileName[IndexA] == '\\') {
+        StartIndex = IndexA + 1;
+      }
+
+      if (PdbFileName[IndexA] == '.') {
+        EndIndex = IndexA;
+      }
+    }
+
+    IndexU = 0;
+    for (IndexA = StartIndex; IndexA < EndIndex; IndexA++) {
+      UnicodeBuffer[IndexU] = (CHAR16) PdbFileName[IndexA];
+      IndexU++;
+      if (IndexU >= PROFILE_NAME_STRING_LENGTH) {
+        UnicodeBuffer[PROFILE_NAME_STRING_LENGTH] = 0;
+        break;
+      }
+    }
+  }
+}
+
+/** 
+  Get a human readable name for an image.
+  The following methods will be tried orderly:
+    1. Image PDB
+    2. FFS UI section
+    3. Image GUID
+
+  @param[in] DriverInfo Pointer to memory profile driver info.
+
+  @post The resulting Unicode name string is stored in the mNameString global array.
+
+**/
+VOID
+GetDriverNameString (
+ IN MEMORY_PROFILE_DRIVER_INFO  *DriverInfo
+ )
+{
+  EFI_STATUS                  Status;
+  CHAR8                       *PdbFileName;
+  CHAR16                      *NameString;
+  UINTN                       StringSize;
+
+  //
+  // Method 1: Get the name string from image PDB
+  //
+  if ((DriverInfo->ImageBase != 0) && (DriverInfo->FileType != EFI_FV_FILETYPE_SMM) && (DriverInfo->FileType != EFI_FV_FILETYPE_SMM_CORE)) {
+    PdbFileName = PeCoffLoaderGetPdbPointer ((VOID *) (UINTN) DriverInfo->ImageBase);
+
+    if (PdbFileName != NULL) {
+      GetShortPdbFileName (PdbFileName, mNameString);
+      return;
+    }
+  }
+
+  if (!CompareGuid (&DriverInfo->FileName, &gZeroGuid)) {
+    //
+    // Try to get the image's FFS UI section by image GUID
+    //
+    NameString = NULL;
+    StringSize = 0;
+    Status = GetSectionFromAnyFv (
+              &DriverInfo->FileName,
+              EFI_SECTION_USER_INTERFACE,
+              0,
+              (VOID **) &NameString,
+              &StringSize
+              );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Method 2: Get the name string from FFS UI section
+      //
+      StrnCpyS (mNameString, PROFILE_NAME_STRING_LENGTH + 1, NameString, PROFILE_NAME_STRING_LENGTH);
+      mNameString[PROFILE_NAME_STRING_LENGTH] = 0;
+      FreePool (NameString);
+      return;
+    }
+  }
+
+  //
+  // Method 3: Get the name string from image GUID
+  //
+  UnicodeSPrint (mNameString, sizeof (mNameString), L"%g", &DriverInfo->FileName);
+}
+
+/**
+  Memory type to string.
+
+  @param[in] MemoryType Memory type.
+
+  @return Pointer to string.
+
+**/
+CHAR16 *
+ProfileMemoryTypeToStr (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  UINTN     Index;
+
+  if ((UINT32) MemoryType >= 0x80000000) {
+    //
+    // OS reserved memory type.
+    //
+    Index = EfiMaxMemoryType;
+  } else if ((UINT32) MemoryType >= 0x70000000) {
+    //
+    // OEM reserved memory type.
+    //
+    Index = EfiMaxMemoryType + 1;
+  } else {
+    Index = MemoryType;
+  }
+
+  return mMemoryTypeString[Index];
+}
+
+/**
+  Dump memory profile allocate information.
+
+  @param[in] DriverInfo         Pointer to memory profile driver info.
+  @param[in] AllocIndex         Memory profile alloc info index.
+  @param[in] AllocInfo          Pointer to memory profile alloc info.
+
+  @return Pointer to next memory profile alloc info.
+
+**/
+MEMORY_PROFILE_ALLOC_INFO *
+DumpMemoryProfileAllocInfo (
+  IN MEMORY_PROFILE_DRIVER_INFO *DriverInfo,
+  IN UINTN                      AllocIndex,
+  IN MEMORY_PROFILE_ALLOC_INFO  *AllocInfo
+  )
+{
+  if (AllocInfo->Header.Signature != MEMORY_PROFILE_ALLOC_INFO_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"    MEMORY_PROFILE_ALLOC_INFO (0x%x)\n", AllocIndex);
+  Print (L"      Signature     - 0x%08x\n", AllocInfo->Header.Signature);
+  Print (L"      Length        - 0x%04x\n", AllocInfo->Header.Length);
+  Print (L"      Revision      - 0x%04x\n", AllocInfo->Header.Revision);  
+  Print (L"      CallerAddress - 0x%016lx (Offset: 0x%08x)\n", AllocInfo->CallerAddress, (UINTN) (AllocInfo->CallerAddress - DriverInfo->ImageBase));
+  Print (L"      SequenceId    - 0x%08x\n", AllocInfo->SequenceId);
+  Print (L"      Action        - 0x%08x (%s)\n", AllocInfo->Action, mActionString[(AllocInfo->Action < sizeof(mActionString)/sizeof(mActionString[0])) ? AllocInfo->Action : 0]);
+  Print (L"      MemoryType    - 0x%08x (%s)\n", AllocInfo->MemoryType, ProfileMemoryTypeToStr (AllocInfo->MemoryType));
+  Print (L"      Buffer        - 0x%016lx\n", AllocInfo->Buffer);
+  Print (L"      Size          - 0x%016lx\n", AllocInfo->Size);
+
+  return (MEMORY_PROFILE_ALLOC_INFO *) ((UINTN) AllocInfo + AllocInfo->Header.Length);
+}
+
+/**
+  Dump memory profile driver information.
+
+  @param[in] DriverIndex        Memory profile driver info index.
+  @param[in] DriverInfo         Pointer to memory profile driver info.
+
+  @return Pointer to next memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO *
+DumpMemoryProfileDriverInfo (
+  IN UINTN                      DriverIndex,
+  IN MEMORY_PROFILE_DRIVER_INFO *DriverInfo
+  )
+{
+  UINTN                         TypeIndex;
+  MEMORY_PROFILE_ALLOC_INFO     *AllocInfo;
+  UINTN                         AllocIndex;
+
+  if (DriverInfo->Header.Signature != MEMORY_PROFILE_DRIVER_INFO_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"  MEMORY_PROFILE_DRIVER_INFO (0x%x)\n", DriverIndex);
+  Print (L"    Signature               - 0x%08x\n", DriverInfo->Header.Signature);
+  Print (L"    Length                  - 0x%04x\n", DriverInfo->Header.Length);
+  Print (L"    Revision                - 0x%04x\n", DriverInfo->Header.Revision);  
+  GetDriverNameString (DriverInfo);
+  Print (L"    FileName                - %s\n", &mNameString);
+  Print (L"    ImageBase               - 0x%016lx\n", DriverInfo->ImageBase);
+  Print (L"    ImageSize               - 0x%016lx\n", DriverInfo->ImageSize);
+  Print (L"    EntryPoint              - 0x%016lx\n", DriverInfo->EntryPoint);
+  Print (L"    ImageSubsystem          - 0x%04x (%s)\n", DriverInfo->ImageSubsystem, mSubsystemString[(DriverInfo->ImageSubsystem < sizeof(mSubsystemString)/sizeof(mSubsystemString[0])) ? DriverInfo->ImageSubsystem : 0]);
+  Print (L"    FileType                - 0x%02x (%s)\n", DriverInfo->FileType, mFileTypeString[(DriverInfo->FileType < sizeof(mFileTypeString)/sizeof(mFileTypeString[0])) ? DriverInfo->FileType : 0]);
+  Print (L"    CurrentUsage            - 0x%016lx\n", DriverInfo->CurrentUsage);
+  Print (L"    PeakUsage               - 0x%016lx\n", DriverInfo->PeakUsage);
+  for (TypeIndex = 0; TypeIndex < sizeof (DriverInfo->CurrentUsageByType) / sizeof (DriverInfo->CurrentUsageByType[0]); TypeIndex++) {
+    if ((DriverInfo->CurrentUsageByType[TypeIndex] != 0) ||
+        (DriverInfo->PeakUsageByType[TypeIndex] != 0)) {
+      Print (L"    CurrentUsage[0x%02x]      - 0x%016lx (%s)\n", TypeIndex, DriverInfo->CurrentUsageByType[TypeIndex], mMemoryTypeString[TypeIndex]);
+      Print (L"    PeakUsage[0x%02x]         - 0x%016lx (%s)\n", TypeIndex, DriverInfo->PeakUsageByType[TypeIndex], mMemoryTypeString[TypeIndex]);
+    }
+  }
+  Print (L"    AllocRecordCount        - 0x%08x\n", DriverInfo->AllocRecordCount);
+
+  AllocInfo = (MEMORY_PROFILE_ALLOC_INFO *) ((UINTN) DriverInfo + DriverInfo->Header.Length);
+  for (AllocIndex = 0; AllocIndex < DriverInfo->AllocRecordCount; AllocIndex++) {
+    AllocInfo = DumpMemoryProfileAllocInfo (DriverInfo, AllocIndex, AllocInfo);
+    if (AllocInfo == NULL) {
+      return NULL;
+    }
+  }
+  return (MEMORY_PROFILE_DRIVER_INFO *) AllocInfo;
+}
+
+/**
+  Dump memory profile context information.
+
+  @param[in] Context            Pointer to memory profile context.
+
+  @return Pointer to the end of memory profile context buffer.
+
+**/
+VOID *
+DumpMemoryProfileContext (
+  IN MEMORY_PROFILE_CONTEXT     *Context
+  )
+{
+  UINTN                         TypeIndex;
+  MEMORY_PROFILE_DRIVER_INFO    *DriverInfo;
+  UINTN                         DriverIndex;
+
+  if (Context->Header.Signature != MEMORY_PROFILE_CONTEXT_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"MEMORY_PROFILE_CONTEXT\n");
+  Print (L"  Signature                     - 0x%08x\n", Context->Header.Signature);
+  Print (L"  Length                        - 0x%04x\n", Context->Header.Length);
+  Print (L"  Revision                      - 0x%04x\n", Context->Header.Revision);  
+  Print (L"  CurrentTotalUsage             - 0x%016lx\n", Context->CurrentTotalUsage);
+  Print (L"  PeakTotalUsage                - 0x%016lx\n", Context->PeakTotalUsage);
+  for (TypeIndex = 0; TypeIndex < sizeof (Context->CurrentTotalUsageByType) / sizeof (Context->CurrentTotalUsageByType[0]); TypeIndex++) {
+    if ((Context->CurrentTotalUsageByType[TypeIndex] != 0) ||
+        (Context->PeakTotalUsageByType[TypeIndex] != 0)) {
+      Print (L"  CurrentTotalUsage[0x%02x]       - 0x%016lx (%s)\n", TypeIndex, Context->CurrentTotalUsageByType[TypeIndex], mMemoryTypeString[TypeIndex]);
+      Print (L"  PeakTotalUsage[0x%02x]          - 0x%016lx (%s)\n", TypeIndex, Context->PeakTotalUsageByType[TypeIndex], mMemoryTypeString[TypeIndex]);
+    }
+  }
+  Print (L"  TotalImageSize                - 0x%016lx\n", Context->TotalImageSize);
+  Print (L"  ImageCount                    - 0x%08x\n", Context->ImageCount);
+  Print (L"  SequenceCount                 - 0x%08x\n", Context->SequenceCount);
+
+  DriverInfo = (MEMORY_PROFILE_DRIVER_INFO *) ((UINTN) Context + Context->Header.Length);
+  for (DriverIndex = 0; DriverIndex < Context->ImageCount; DriverIndex++) {
+    DriverInfo = DumpMemoryProfileDriverInfo (DriverIndex, DriverInfo);
+    if (DriverInfo == NULL) {
+      return NULL;
+    }
+  }
+  return (VOID *) DriverInfo;
+}
+
+/**
+  Dump memory profile descriptor information.
+
+  @param[in] DescriptorIndex    Memory profile descriptor index.
+  @param[in] Descriptor         Pointer to memory profile descriptor.
+
+  @return Pointer to next memory profile descriptor.
+
+**/
+MEMORY_PROFILE_DESCRIPTOR *
+DumpMemoryProfileDescriptor (
+  IN UINTN                      DescriptorIndex,
+  IN MEMORY_PROFILE_DESCRIPTOR  *Descriptor
+  )
+{
+  if (Descriptor->Header.Signature != MEMORY_PROFILE_DESCRIPTOR_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"  MEMORY_PROFILE_DESCRIPTOR (0x%x)\n", DescriptorIndex);
+  Print (L"    Signature               - 0x%08x\n", Descriptor->Header.Signature);
+  Print (L"    Length                  - 0x%04x\n", Descriptor->Header.Length);
+  Print (L"    Revision                - 0x%04x\n", Descriptor->Header.Revision);  
+  Print (L"    Address                 - 0x%016lx\n", Descriptor->Address);
+  Print (L"    Size                    - 0x%016lx\n", Descriptor->Size);
+
+  return (MEMORY_PROFILE_DESCRIPTOR *) ((UINTN) Descriptor + Descriptor->Header.Length);
+}
+
+/**
+  Dump memory profile free memory information.
+
+  @param[in] FreeMemory         Pointer to memory profile free memory.
+
+  @return Pointer to the end of memory profile free memory buffer.
+
+**/
+VOID *
+DumpMemoryProfileFreeMemory (
+  IN MEMORY_PROFILE_FREE_MEMORY *FreeMemory
+  )
+{
+  MEMORY_PROFILE_DESCRIPTOR     *Descriptor;
+  UINTN                         DescriptorIndex;
+
+  if (FreeMemory->Header.Signature != MEMORY_PROFILE_FREE_MEMORY_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"MEMORY_PROFILE_FREE_MEMORY\n");
+  Print (L"  Signature                     - 0x%08x\n", FreeMemory->Header.Signature);
+  Print (L"  Length                        - 0x%04x\n", FreeMemory->Header.Length);
+  Print (L"  Revision                      - 0x%04x\n", FreeMemory->Header.Revision);  
+  Print (L"  TotalFreeMemoryPages          - 0x%016lx\n", FreeMemory->TotalFreeMemoryPages);
+  Print (L"  FreeMemoryEntryCount          - 0x%08x\n", FreeMemory->FreeMemoryEntryCount);
+
+  Descriptor = (MEMORY_PROFILE_DESCRIPTOR *) ((UINTN) FreeMemory + FreeMemory->Header.Length);
+  for (DescriptorIndex = 0; DescriptorIndex < FreeMemory->FreeMemoryEntryCount; DescriptorIndex++) {
+    Descriptor = DumpMemoryProfileDescriptor (DescriptorIndex, Descriptor);
+    if (Descriptor == NULL) {
+      return NULL;
+    }
+  }
+
+  return (VOID *) Descriptor;
+}
+
+/**
+  Dump memory profile memory range information.
+
+  @param[in] MemoryRange        Pointer to memory profile memory range.
+
+  @return Pointer to the end of memory profile memory range buffer.
+
+**/
+VOID *
+DumpMemoryProfileMemoryRange (
+  IN MEMORY_PROFILE_MEMORY_RANGE    *MemoryRange
+  )
+{
+  MEMORY_PROFILE_DESCRIPTOR     *Descriptor;
+  UINTN                         DescriptorIndex;
+
+  if (MemoryRange->Header.Signature != MEMORY_PROFILE_MEMORY_RANGE_SIGNATURE) {
+    return NULL;
+  }
+  Print (L"MEMORY_PROFILE_MEMORY_RANGE\n");
+  Print (L"  Signature                     - 0x%08x\n", MemoryRange->Header.Signature);
+  Print (L"  Length                        - 0x%04x\n", MemoryRange->Header.Length);
+  Print (L"  Revision                      - 0x%04x\n", MemoryRange->Header.Revision);  
+  Print (L"  MemoryRangeCount              - 0x%08x\n", MemoryRange->MemoryRangeCount);
+
+  Descriptor = (MEMORY_PROFILE_DESCRIPTOR *) ((UINTN) MemoryRange + MemoryRange->Header.Length);
+  for (DescriptorIndex = 0; DescriptorIndex < MemoryRange->MemoryRangeCount; DescriptorIndex++) {
+    Descriptor = DumpMemoryProfileDescriptor (DescriptorIndex, Descriptor);
+    if (Descriptor == NULL) {
+      return NULL;
+    }
+  }
+
+  return (VOID *) Descriptor;
+}
+
+/**
+  Scan memory profile by Signature.
+
+  @param[in] ProfileBuffer      Memory profile base address.
+  @param[in] ProfileSize        Memory profile size.
+  @param[in] Signature          Signature.
+
+  @return Pointer to the stucture with the signature.
+
+**/
+VOID *
+ScanMemoryProfileBySignature (
+  IN PHYSICAL_ADDRESS           ProfileBuffer,
+  IN UINT64                     ProfileSize,
+  IN UINT32                     Signature
+  )
+{
+  MEMORY_PROFILE_COMMON_HEADER  *CommonHeader;
+  UINTN                          ProfileEnd;
+
+  ProfileEnd = (UINTN) (ProfileBuffer + ProfileSize);
+  CommonHeader = (MEMORY_PROFILE_COMMON_HEADER *) (UINTN) ProfileBuffer;
+  while ((UINTN) CommonHeader < ProfileEnd) {
+    if (CommonHeader->Signature == Signature) {
+      //
+      // Found it.
+      //
+      return (VOID *) CommonHeader;
+    }
+    CommonHeader = (MEMORY_PROFILE_COMMON_HEADER *) ((UINTN) CommonHeader + CommonHeader->Length);
+  }
+
+  return NULL;
+}
+
+/**
+  Dump memory profile information.
+
+  @param[in] ProfileBuffer      Memory profile base address.
+  @param[in] ProfileSize        Memory profile size.
+
+**/
+VOID
+DumpMemoryProfile (
+  IN PHYSICAL_ADDRESS           ProfileBuffer,
+  IN UINT64                     ProfileSize
+  )
+{
+  MEMORY_PROFILE_CONTEXT        *Context;
+  MEMORY_PROFILE_FREE_MEMORY    *FreeMemory;
+  MEMORY_PROFILE_MEMORY_RANGE   *MemoryRange;
+
+  Context = (MEMORY_PROFILE_CONTEXT *) ScanMemoryProfileBySignature (ProfileBuffer, ProfileSize, MEMORY_PROFILE_CONTEXT_SIGNATURE);
+  if (Context != NULL) {
+    DumpMemoryProfileContext (Context);
+  }
+
+  FreeMemory = (MEMORY_PROFILE_FREE_MEMORY *) ScanMemoryProfileBySignature (ProfileBuffer, ProfileSize, MEMORY_PROFILE_FREE_MEMORY_SIGNATURE);
+  if (FreeMemory != NULL) {
+    DumpMemoryProfileFreeMemory (FreeMemory);
+  }
+
+  MemoryRange = (MEMORY_PROFILE_MEMORY_RANGE *) ScanMemoryProfileBySignature (ProfileBuffer, ProfileSize, MEMORY_PROFILE_MEMORY_RANGE_SIGNATURE);
+  if (MemoryRange != NULL) {
+    DumpMemoryProfileMemoryRange (MemoryRange);
+  }
+}
+
+/**
+  Get and dump UEFI memory profile data.
+
+  @return EFI_SUCCESS   Get the memory profile data successfully.
+  @return other         Fail to get the memory profile data.
+
+**/
+EFI_STATUS
+GetUefiMemoryProfileData (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EDKII_MEMORY_PROFILE_PROTOCOL *ProfileProtocol;
+  VOID                          *Data;
+  UINT64                        Size;
+
+  Status = gBS->LocateProtocol (&gEdkiiMemoryProfileGuid, NULL, (VOID **) &ProfileProtocol);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UefiMemoryProfile: Locate MemoryProfile protocol - %r\n", Status));
+    return Status;
+  }
+
+  Size = 0;
+  Data = NULL;
+  Status = ProfileProtocol->GetData (
+                              ProfileProtocol,
+                              &Size,
+                              Data
+                              );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    Print (L"UefiMemoryProfile: GetData - %r\n", Status);
+    return Status;
+  }
+
+  //
+  // Add one sizeof (MEMORY_PROFILE_ALLOC_INFO) to Size for this AllocatePool action.
+  //
+  Size = Size + sizeof (MEMORY_PROFILE_ALLOC_INFO);
+  Data = AllocateZeroPool ((UINTN) Size);
+  if (Data == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    Print (L"UefiMemoryProfile: AllocateZeroPool (0x%x) - %r\n", Size, Status);
+    return Status;
+  }
+
+  Status = ProfileProtocol->GetData (
+                              ProfileProtocol,
+                              &Size,
+                              Data
+                              );
+  if (EFI_ERROR (Status)) {
+    FreePool (Data);
+    Print (L"UefiMemoryProfile: GetData - %r\n", Status);
+    return Status;
+  }
+
+
+  Print (L"UefiMemoryProfileSize - 0x%x\n", Size);
+  Print (L"======= UefiMemoryProfile begin =======\n");
+  DumpMemoryProfile ((PHYSICAL_ADDRESS) (UINTN) Data, Size);
+  Print (L"======= UefiMemoryProfile end =======\n\n\n");
+
+  FreePool (Data);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get and dump SMRAM profile data.
+
+  @return EFI_SUCCESS   Get the SMRAM profile data successfully.
+  @return other         Fail to get the SMRAM profile data.
+
+**/
+EFI_STATUS
+GetSmramProfileData (
+  VOID
+  )
+{
+  EFI_STATUS                                    Status;
+  UINTN                                         CommSize;
+  UINT8                                         *CommBuffer;
+  EFI_SMM_COMMUNICATE_HEADER                    *CommHeader;
+  SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO      *CommGetProfileInfo;
+  SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET *CommGetProfileData;
+  UINTN                                         ProfileSize;
+  VOID                                          *ProfileBuffer;
+  EFI_SMM_COMMUNICATION_PROTOCOL                *SmmCommunication;
+  UINTN                                         MinimalSizeNeeded;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE       *PiSmmCommunicationRegionTable;
+  UINT32                                        Index;
+  EFI_MEMORY_DESCRIPTOR                         *Entry;
+  VOID                                          *Buffer;
+  UINTN                                         Size;
+  UINTN                                         Offset;
+
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &SmmCommunication);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfile: Locate SmmCommunication protocol - %r\n", Status));
+    return Status;
+  }
+
+  MinimalSizeNeeded = sizeof (EFI_GUID) +
+                      sizeof (UINTN) +
+                      MAX (sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO),
+                           sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET));
+  MinimalSizeNeeded += MAX (sizeof (MEMORY_PROFILE_CONTEXT),
+                            MAX (sizeof (MEMORY_PROFILE_DRIVER_INFO),
+                                 MAX (sizeof (MEMORY_PROFILE_ALLOC_INFO),
+                                      MAX (sizeof (MEMORY_PROFILE_DESCRIPTOR),
+                                           MAX (sizeof (MEMORY_PROFILE_FREE_MEMORY),
+                                                sizeof (MEMORY_PROFILE_MEMORY_RANGE))))));
+
+  Status = EfiGetSystemConfigurationTable (
+             &gEdkiiPiSmmCommunicationRegionTableGuid,
+             (VOID **) &PiSmmCommunicationRegionTable
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfile: Get PiSmmCommunicationRegionTable - %r\n", Status));
+    return Status;
+  }
+  ASSERT (PiSmmCommunicationRegionTable != NULL);
+  Entry = (EFI_MEMORY_DESCRIPTOR *) (PiSmmCommunicationRegionTable + 1);
+  Size = 0;
+  for (Index = 0; Index < PiSmmCommunicationRegionTable->NumberOfEntries; Index++) {
+    if (Entry->Type == EfiConventionalMemory) {
+      Size = EFI_PAGES_TO_SIZE ((UINTN) Entry->NumberOfPages);
+      if (Size >= MinimalSizeNeeded) {
+        break;
+      }
+    }
+    Entry = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) Entry + PiSmmCommunicationRegionTable->DescriptorSize);
+  }
+  ASSERT (Index < PiSmmCommunicationRegionTable->NumberOfEntries);
+  CommBuffer = (UINT8 *) (UINTN) Entry->PhysicalStart;
+
+  //
+  // Get Size
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *) &CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEdkiiMemoryProfileGuid, sizeof (gEdkiiMemoryProfileGuid));
+  CommHeader->MessageLength = sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO);
+
+  CommGetProfileInfo = (SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO *) &CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  CommGetProfileInfo->Header.Command      = SMRAM_PROFILE_COMMAND_GET_PROFILE_INFO;
+  CommGetProfileInfo->Header.DataLength   = sizeof (*CommGetProfileInfo);
+  CommGetProfileInfo->Header.ReturnStatus = (UINT64)-1;
+  CommGetProfileInfo->ProfileSize         = 0;
+
+  CommSize = sizeof (EFI_GUID) + sizeof (UINTN) + CommHeader->MessageLength;
+  Status = SmmCommunication->Communicate (SmmCommunication, CommBuffer, &CommSize);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfile: SmmCommunication - %r\n", Status));
+    return Status;
+  }
+
+  if (CommGetProfileInfo->Header.ReturnStatus != 0) {
+    Print (L"SmramProfile: GetProfileInfo - 0x%0x\n", CommGetProfileInfo->Header.ReturnStatus);
+    return EFI_SUCCESS;
+  }
+
+  ProfileSize = (UINTN) CommGetProfileInfo->ProfileSize;
+
+  //
+  // Get Data
+  //
+  ProfileBuffer = AllocateZeroPool (ProfileSize);
+  if (ProfileBuffer == 0) {
+    Status = EFI_OUT_OF_RESOURCES;
+    Print (L"SmramProfile: AllocateZeroPool (0x%x) for profile buffer - %r\n", ProfileSize, Status);
+    return Status;
+  }
+
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *) &CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEdkiiMemoryProfileGuid, sizeof(gEdkiiMemoryProfileGuid));
+  CommHeader->MessageLength = sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET);
+
+  CommGetProfileData = (SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET *) &CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  CommGetProfileData->Header.Command      = SMRAM_PROFILE_COMMAND_GET_PROFILE_DATA_BY_OFFSET;
+  CommGetProfileData->Header.DataLength   = sizeof (*CommGetProfileData);
+  CommGetProfileData->Header.ReturnStatus = (UINT64)-1;
+
+  CommSize = sizeof (EFI_GUID) + sizeof (UINTN) + CommHeader->MessageLength;
+  Buffer = (UINT8 *) CommHeader + CommSize;
+  Size -= CommSize;
+
+  CommGetProfileData->ProfileBuffer       = (PHYSICAL_ADDRESS) (UINTN) Buffer;
+  CommGetProfileData->ProfileOffset       = 0;
+  while (CommGetProfileData->ProfileOffset < ProfileSize) {
+    Offset = (UINTN) CommGetProfileData->ProfileOffset;
+    if (Size <= (ProfileSize - CommGetProfileData->ProfileOffset)) {
+      CommGetProfileData->ProfileSize = (UINT64) Size;
+    } else {
+      CommGetProfileData->ProfileSize = (UINT64) (ProfileSize - CommGetProfileData->ProfileOffset);
+    }
+    Status = SmmCommunication->Communicate (SmmCommunication, CommBuffer, &CommSize);
+    ASSERT_EFI_ERROR (Status);
+
+    if (CommGetProfileData->Header.ReturnStatus != 0) {
+      FreePool (ProfileBuffer);
+      Print (L"GetProfileData - 0x%x\n", CommGetProfileData->Header.ReturnStatus);
+      return EFI_SUCCESS;
+    }
+    CopyMem ((UINT8 *) ProfileBuffer + Offset, (VOID *) (UINTN) CommGetProfileData->ProfileBuffer, (UINTN) CommGetProfileData->ProfileSize);
+  }
+
+
+  Print (L"SmramProfileSize - 0x%x\n", ProfileSize);
+  Print (L"======= SmramProfile begin =======\n");
+  DumpMemoryProfile ((PHYSICAL_ADDRESS) (UINTN) ProfileBuffer, ProfileSize);
+  Print (L"======= SmramProfile end =======\n\n\n");
+
+  FreePool (ProfileBuffer);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user Entry Point for Application. The user code starts with this function
+  as the real entry point for the image goes into a library that calls this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+UefiMain (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                    Status;
+
+  Status = GetUefiMemoryProfileData ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "GetUefiMemoryProfileData - %r\n", Status));
+  }
+
+  Status = GetSmramProfileData ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "GetSmramProfileData - %r\n", Status));
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.inf b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.inf
new file mode 100644
index 0000000000..a1bc6752f0
--- /dev/null
+++ b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.inf
@@ -0,0 +1,63 @@
+## @file
+#  Shell application to dump UEFI memory and SMRAM profile information.
+#
+#  Note that if the feature is not enabled by setting PcdMemoryProfilePropertyMask,
+#  the application will not display memory profile information.
+#
+#  Copyright (c) 2014 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = MemoryProfileInfo
+  MODULE_UNI_FILE                = MemoryProfileInfo.uni
+  FILE_GUID                      = 21429B90-5F67-4e93-AF55-1D314D646E12
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = UefiMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  MemoryProfileInfo.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiApplicationEntryPoint
+  BaseLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  DebugLib
+  UefiLib
+  MemoryAllocationLib
+  DxeServicesLib
+  PeCoffGetEntryPointLib
+  PrintLib
+  
+[Guids]
+  ## SOMETIMES_CONSUMES   ## GUID # Locate protocol
+  ## SOMETIMES_CONSUMES   ## GUID # SmiHandlerRegister
+  gEdkiiMemoryProfileGuid
+  gZeroGuid                            ## SOMETIMES_CONSUMES   ## GUID
+  gEdkiiPiSmmCommunicationRegionTableGuid    ## SOMETIMES_CONSUMES ## SystemTable
+
+[Protocols]
+  gEfiSmmCommunicationProtocolGuid     ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  MemoryProfileInfoExtra.uni
diff --git a/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.uni b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.uni
new file mode 100644
index 0000000000..fe4882605d
--- /dev/null
+++ b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.uni
diff --git a/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfoExtra.uni b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfoExtra.uni
new file mode 100644
index 0000000000..32905119ec
--- /dev/null
+++ b/Core/MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfoExtra.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/Bm.vfr b/Core/MdeModulePkg/Application/UiApp/BootMaint/Bm.vfr
new file mode 100644
index 0000000000..de1ffba9cb
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/Bm.vfr
@@ -0,0 +1,360 @@
+///** @file
+//  
+//    Boot Maintenance Utility Formset
+//  
+//  Copyright (c) 2004 - 2015, 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 "FormGuid.h"
+
+formset
+  guid = BOOT_MAINT_FORMSET_GUID,
+  title = STRING_TOKEN(STR_FORM_MAIN_TITLE),
+  help = STRING_TOKEN(STR_NULL_STRING),
+  classguid = BOOT_MAINT_FORMSET_GUID,
+
+  varstore BMM_FAKE_NV_DATA,
+    varid = VARSTORE_ID_BOOT_MAINT,
+    name = BmmData,
+    guid = BOOT_MAINT_FORMSET_GUID;
+
+  form formid = FORM_MAIN_ID,
+       title = STRING_TOKEN(STR_FORM_MAIN_TITLE);
+
+    goto FORM_BOOT_SETUP_ID,
+         prompt = STRING_TOKEN(STR_FORM_BOOT_SETUP_TITLE),
+         help = STRING_TOKEN(STR_FORM_BOOT_SETUP_HELP),
+         flags = INTERACTIVE,
+         key = FORM_BOOT_SETUP_ID;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    goto FORM_DRIVER_SETUP_ID,
+         prompt = STRING_TOKEN(STR_FORM_DRIVER_SETUP_TITLE),
+         help = STRING_TOKEN(STR_FORM_DRIVER_SETUP_HELP),
+         flags = INTERACTIVE,
+         key = FORM_DRIVER_SETUP_ID;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    goto FORM_CON_MAIN_ID,
+         prompt = STRING_TOKEN(STR_FORM_CON_MAIN_TITLE),
+         help = STRING_TOKEN(STR_FORM_CON_MAIN_HELP),
+         flags = INTERACTIVE,
+         key = FORM_CON_MAIN_ID;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    goto
+         formsetguid = FILE_EXPLORE_FORMSET_GUID,
+         formid = 0,
+         question = 0,
+         prompt = STRING_TOKEN(STR_BOOT_FROM_FILE),
+         help   = STRING_TOKEN(STR_BOOT_FROM_FILE_HELP),
+         flags  = INTERACTIVE,
+         key    = KEY_VALUE_BOOT_FROM_FILE;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+//    label FORM_MAIN_ID;
+
+    goto FORM_BOOT_NEXT_ID,
+         prompt = STRING_TOKEN(STR_FORM_BOOT_NEXT_TITLE),
+         help = STRING_TOKEN(STR_FORM_BOOT_NEXT_HELP),
+         flags = INTERACTIVE,
+         key = FORM_BOOT_NEXT_ID;
+
+    goto FORM_TIME_OUT_ID,
+         prompt = STRING_TOKEN(STR_FORM_TIME_OUT_TITLE),
+         help = STRING_TOKEN(STR_FORM_TIME_OUT_HELP),
+         flags = INTERACTIVE,
+         key = FORM_TIME_OUT_ID;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    text
+         help   = STRING_TOKEN(STR_RESET),
+         text   = STRING_TOKEN(STR_RESET),
+         flags  = INTERACTIVE,
+         key    = FORM_RESET;
+         
+    label LABEL_BMM_PLATFORM_INFORMATION;
+    //
+    // This is where we will dynamically add a Action type op-code to show 
+    // the platform information.
+    //
+    
+    //
+    // This is where we will dynamically add a Action type op-code to show 
+    // the advanced menu.
+    //
+    
+    //
+    // This is where we will dynamically add a Action type op-code to show 
+    // the intel test menu.
+    //
+    label LABEL_END; 
+  endform;
+
+  form formid = FORM_BOOT_SETUP_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_SETUP_TITLE);
+
+       goto FORM_MAIN_ID,
+            prompt = STRING_TOKEN(STR_FORM_GOTO_MAIN),
+            help = STRING_TOKEN(STR_FORM_GOTO_MAIN);
+            //flags = INTERACTIVE,
+            //key = FORM_MAIN_ID;
+
+       goto
+            formsetguid = FILE_EXPLORE_FORMSET_GUID,
+            formid = 0,
+            question = 0,
+            prompt = STRING_TOKEN(STR_FORM_BOOT_ADD_TITLE),
+            help = STRING_TOKEN(STR_FORM_BOOT_ADD_HELP),
+            flags = INTERACTIVE,
+            key = FORM_BOOT_ADD_ID;
+
+       goto FORM_BOOT_DEL_ID,
+            prompt = STRING_TOKEN(STR_FORM_BOOT_DEL_TITLE),
+            help = STRING_TOKEN(STR_FORM_BOOT_IMMEDIATE_HELP),
+            flags = INTERACTIVE,
+            key = FORM_BOOT_DEL_ID;
+
+       goto FORM_BOOT_CHG_ID,
+            prompt = STRING_TOKEN(STR_FORM_BOOT_CHG_TITLE),
+            help = STRING_TOKEN(STR_FORM_BOOT_IMMEDIATE_HELP),
+            flags = INTERACTIVE,
+            key = FORM_BOOT_CHG_ID;
+  endform;
+
+  form formid = FORM_DRIVER_SETUP_ID,
+       title = STRING_TOKEN(STR_FORM_DRIVER_SETUP_TITLE);
+
+       goto FORM_MAIN_ID,
+            prompt = STRING_TOKEN(STR_FORM_GOTO_MAIN),
+            help = STRING_TOKEN(STR_FORM_GOTO_MAIN);
+            //help = STRING_TOKEN(STR_FORM_GOTO_MAIN),
+            //flags = INTERACTIVE,
+            //key = FORM_MAIN_ID;
+
+       goto FORM_DRV_ADD_ID,
+            prompt = STRING_TOKEN(STR_FORM_DRV_ADD_TITLE),
+            help = STRING_TOKEN(STR_FORM_DRV_ADD_HELP),
+            flags = INTERACTIVE,
+            key = FORM_DRV_ADD_ID;
+
+       goto FORM_DRV_DEL_ID,
+            prompt = STRING_TOKEN(STR_FORM_DRV_DEL_TITLE),
+            help = STRING_TOKEN(STR_FORM_NEXT_BOOT_HELP),
+            flags = INTERACTIVE,
+            key = FORM_DRV_DEL_ID;
+
+       goto FORM_DRV_CHG_ID,
+            prompt = STRING_TOKEN(STR_FORM_DRV_CHG_TITLE),
+            help = STRING_TOKEN(STR_FORM_NEXT_BOOT_HELP),
+            flags = INTERACTIVE,
+            key = FORM_DRV_CHG_ID;
+  endform;
+
+  form formid = FORM_BOOT_DEL_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_DEL_TITLE);
+
+       label FORM_BOOT_DEL_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_BOOT_CHG_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_CHG_TITLE);
+
+       label FORM_BOOT_CHG_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_BOOT_NEXT_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_NEXT_TITLE);
+
+       label FORM_BOOT_NEXT_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_TIME_OUT_ID,
+       title = STRING_TOKEN(STR_FORM_TIME_OUT_TITLE);
+
+       label FORM_TIME_OUT_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_MEMORY_CHECK_ID,
+       title = STRING_TOKEN(STR_FORM_MEMORY_CHECK_TITLE);
+
+       label FORM_MEMORY_CHECK_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_UEFI_OPTIMIZED_BOOT_ID,
+       title = STRING_TOKEN(STR_FORM_UEFI_OPTIMIZED_BOOT_TITLE);
+
+       label FORM_UEFI_OPTIMIZED_BOOT_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_DRV_ADD_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_ADD_TITLE);
+
+       goto FORM_MAIN_ID,
+            prompt = STRING_TOKEN(STR_FORM_GOTO_MAIN),
+            help = STRING_TOKEN(STR_FORM_GOTO_MAIN);
+            //flags = INTERACTIVE,
+            //key = FORM_MAIN_ID;
+
+       goto
+            formsetguid = FILE_EXPLORE_FORMSET_GUID,
+            formid = 0,
+            question = 0,
+            prompt = STRING_TOKEN(STR_FORM_DRV_ADD_FILE_TITLE),
+            help = STRING_TOKEN(STR_FORM_DRV_ADD_FILE_TITLE),
+            flags = INTERACTIVE,
+            key = FORM_DRV_ADD_FILE_ID;
+
+  endform;
+
+  form formid = FORM_DRV_DEL_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_DEL_TITLE);
+
+       label FORM_DRV_DEL_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_DRV_CHG_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_CHG_TITLE);
+
+       label FORM_DRV_CHG_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_CON_MAIN_ID,
+       title = STRING_TOKEN(STR_FORM_CON_MAIN_TITLE);
+
+       goto FORM_MAIN_ID,
+       prompt = STRING_TOKEN(STR_FORM_GOTO_MAIN),
+       help = STRING_TOKEN(STR_FORM_GOTO_MAIN);
+       //flags = INTERACTIVE,
+       //key = FORM_MAIN_ID;
+
+       goto FORM_CON_IN_ID,
+       prompt = STRING_TOKEN(STR_FORM_CON_IN_TITLE),
+       help = STRING_TOKEN(STR_FORM_CON_IN_HELP),
+       flags = INTERACTIVE,
+       key = FORM_CON_IN_ID;
+
+       goto FORM_CON_OUT_ID,
+       prompt = STRING_TOKEN(STR_FORM_CON_OUT_TITLE),
+       help = STRING_TOKEN(STR_FORM_CON_OUT_HELP),
+       flags = INTERACTIVE,
+       key = FORM_CON_OUT_ID;
+
+       goto FORM_CON_ERR_ID,
+       prompt = STRING_TOKEN(STR_FORM_STD_ERR_TITLE),
+       help = STRING_TOKEN(STR_FORM_STD_ERR_HELP),
+       flags = INTERACTIVE,
+       key = FORM_CON_ERR_ID;
+
+       goto FORM_CON_MODE_ID,
+       prompt = STRING_TOKEN(STR_FORM_MODE_TITLE),
+       help = STRING_TOKEN(STR_FORM_MODE_HELP),
+       flags = INTERACTIVE,
+       key = FORM_CON_MODE_ID;
+
+       goto FORM_CON_COM_ID,
+       prompt = STRING_TOKEN(STR_FORM_COM_TITLE),
+       help = STRING_TOKEN(STR_FORM_COM_HELP),
+       flags = INTERACTIVE,
+       key = FORM_CON_COM_ID;
+  endform;
+
+  form formid = FORM_CON_MODE_ID,
+       title = STRING_TOKEN(STR_FORM_MODE_TITLE);
+
+       label FORM_CON_MODE_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_CON_COM_ID,
+       title = STRING_TOKEN(STR_FORM_COM_TITLE);
+
+       label FORM_CON_COM_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_CON_COM_SETUP_ID,
+       title = STRING_TOKEN(STR_CON_COM_SETUP);
+
+       label FORM_CON_COM_SETUP_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_FILE_SEEK_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_ADD_TITLE);
+
+       label FORM_FILE_SEEK_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_FILE_NEW_SEEK_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_ADD_TITLE);
+
+       label FORM_FILE_NEW_SEEK_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_DRV_ADD_HANDLE_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_ADD_HANDLE_TITLE);
+
+       label FORM_DRV_ADD_HANDLE_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_DRV_ADD_HANDLE_DESC_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_ADD_DESC_TITLE);
+
+       label FORM_DRV_ADD_HANDLE_DESC_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_CON_IN_ID,
+       title = STRING_TOKEN(STR_FORM_CON_IN_TITLE);
+
+       label FORM_CON_IN_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_CON_OUT_ID,
+       title = STRING_TOKEN(STR_FORM_CON_OUT_TITLE);
+
+       label FORM_CON_OUT_ID;
+       label LABEL_END;
+
+  endform;
+
+  form formid = FORM_CON_ERR_ID,
+       title = STRING_TOKEN(STR_FORM_STD_ERR_TITLE);
+
+       label FORM_CON_ERR_ID;
+       label LABEL_END;
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/BmLib.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/BmLib.c
new file mode 100644
index 0000000000..82f677de63
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/BmLib.c
@@ -0,0 +1,369 @@
+/** @file
+  Utility routines used by boot maintenance modules.
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+/**
+
+  Find the first instance of this Protocol
+  in the system and return it's interface.
+
+
+  @param ProtocolGuid    Provides the protocol to search for
+  @param Interface       On return, a pointer to the first interface
+                         that matches ProtocolGuid
+
+  @retval  EFI_SUCCESS      A protocol instance matching ProtocolGuid was found
+  @retval  EFI_NOT_FOUND    No protocol instances were found that match ProtocolGuid
+
+**/
+EFI_STATUS
+EfiLibLocateProtocol (
+  IN  EFI_GUID    *ProtocolGuid,
+  OUT VOID        **Interface
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->LocateProtocol (
+                  ProtocolGuid,
+                  NULL,
+                  (VOID **) Interface
+                  );
+  return Status;
+}
+
+/**
+
+  Function opens and returns a file handle to the root directory of a volume.
+
+  @param DeviceHandle    A handle for a device
+
+  @return A valid file handle or NULL is returned
+
+**/
+EFI_FILE_HANDLE
+EfiLibOpenRoot (
+  IN EFI_HANDLE                   DeviceHandle
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *Volume;
+  EFI_FILE_HANDLE                 File;
+
+  File = NULL;
+
+  //
+  // File the file system interface to the device
+  //
+  Status = gBS->HandleProtocol (
+                  DeviceHandle,
+                  &gEfiSimpleFileSystemProtocolGuid,
+                  (VOID *) &Volume
+                  );
+
+  //
+  // Open the root directory of the volume
+  //
+  if (!EFI_ERROR (Status)) {
+    Status = Volume->OpenVolume (
+                      Volume,
+                      &File
+                      );
+  }
+  //
+  // Done
+  //
+  return EFI_ERROR (Status) ? NULL : File;
+}
+
+/**
+
+  Helper function called as part of the code needed
+  to allocate the proper sized buffer for various
+  EFI interfaces.
+
+
+  @param Status          Current status
+  @param Buffer          Current allocated buffer, or NULL
+  @param BufferSize      Current buffer size needed
+
+  @retval  TRUE  if the buffer was reallocated and the caller
+                 should try the API again.
+  @retval  FALSE The caller should not call this function again.
+
+**/
+BOOLEAN
+EfiGrowBuffer (
+  IN OUT EFI_STATUS   *Status,
+  IN OUT VOID         **Buffer,
+  IN UINTN            BufferSize
+  )
+{
+  BOOLEAN TryAgain;
+
+  //
+  // If this is an initial request, buffer will be null with a new buffer size
+  //
+  if ((*Buffer == NULL) && (BufferSize != 0)) {
+    *Status = EFI_BUFFER_TOO_SMALL;
+  }
+  //
+  // If the status code is "buffer too small", resize the buffer
+  //
+  TryAgain = FALSE;
+  if (*Status == EFI_BUFFER_TOO_SMALL) {
+
+    if (*Buffer != NULL) {
+      FreePool (*Buffer);
+    }
+
+    *Buffer = AllocateZeroPool (BufferSize);
+
+    if (*Buffer != NULL) {
+      TryAgain = TRUE;
+    } else {
+      *Status = EFI_OUT_OF_RESOURCES;
+    }
+  }
+  //
+  // If there's an error, free the buffer
+  //
+  if (!TryAgain && EFI_ERROR (*Status) && (*Buffer != NULL)) {
+    FreePool (*Buffer);
+    *Buffer = NULL;
+  }
+
+  return TryAgain;
+}
+
+
+/**
+  Function deletes the variable specified by VarName and VarGuid.
+
+  @param VarName           A Null-terminated Unicode string that is
+                           the name of the vendor's variable.
+                         
+  @param VarGuid           A unique identifier for the vendor.
+
+  @retval  EFI_SUCCESS           The variable was found and removed
+  @retval  EFI_UNSUPPORTED       The variable store was inaccessible
+  @retval  EFI_NOT_FOUND         The variable was not found
+
+**/
+EFI_STATUS
+EfiLibDeleteVariable (
+  IN CHAR16   *VarName,
+  IN EFI_GUID *VarGuid
+  )
+{
+  return gRT->SetVariable (
+                VarName,
+                VarGuid,
+                0,
+                0,
+                NULL
+                );
+}
+
+/**
+
+  Function gets the file system information from an open file descriptor,
+  and stores it in a buffer allocated from pool.
+
+
+  @param FHand           The file handle.
+
+  @return                A pointer to a buffer with file information.
+  @retval                NULL is returned if failed to get Vaolume Label Info.
+
+**/
+EFI_FILE_SYSTEM_VOLUME_LABEL *
+EfiLibFileSystemVolumeLabelInfo (
+  IN EFI_FILE_HANDLE      FHand
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_FILE_SYSTEM_VOLUME_LABEL      *Buffer;
+  UINTN                             BufferSize;
+  //
+  // Initialize for GrowBuffer loop
+  //
+  Buffer      = NULL;
+  BufferSize  = SIZE_OF_EFI_FILE_SYSTEM_VOLUME_LABEL + 200;
+
+  //
+  // Call the real function
+  //
+  while (EfiGrowBuffer (&Status, (VOID **) &Buffer, BufferSize)) {
+    Status = FHand->GetInfo (
+                      FHand,
+                      &gEfiFileSystemVolumeLabelInfoIdGuid,
+                      &BufferSize,
+                      Buffer
+                      );
+  }
+
+  return Buffer;
+}
+
+/**
+  Duplicate a string.
+
+  @param Src             The source.
+
+  @return A new string which is duplicated copy of the source.
+  @retval NULL If there is not enough memory.
+
+**/
+CHAR16 *
+EfiStrDuplicate (
+  IN CHAR16   *Src
+  )
+{
+  CHAR16  *Dest;
+  UINTN   Size;
+
+  Size  = StrSize (Src);
+  Dest  = AllocateZeroPool (Size);
+  ASSERT (Dest != NULL);
+  if (Dest != NULL) {
+    CopyMem (Dest, Src, Size);
+  }
+
+  return Dest;
+}
+
+/**
+
+  Function gets the file information from an open file descriptor, and stores it
+  in a buffer allocated from pool.
+
+  @param FHand           File Handle.
+
+  @return                A pointer to a buffer with file information or NULL is returned
+
+**/
+EFI_FILE_INFO *
+EfiLibFileInfo (
+  IN EFI_FILE_HANDLE      FHand
+  )
+{
+  EFI_STATUS    Status;
+  EFI_FILE_INFO *Buffer;
+  UINTN         BufferSize;
+
+  //
+  // Initialize for GrowBuffer loop
+  //
+  Buffer      = NULL;
+  BufferSize  = SIZE_OF_EFI_FILE_INFO + 200;
+
+  //
+  // Call the real function
+  //
+  while (EfiGrowBuffer (&Status, (VOID **) &Buffer, BufferSize)) {
+    Status = FHand->GetInfo (
+                      FHand,
+                      &gEfiFileInfoGuid,
+                      &BufferSize,
+                      Buffer
+                      );
+  }
+
+  return Buffer;
+}
+
+/**
+  Function is used to determine the number of device path instances
+  that exist in a device path.
+
+
+  @param DevicePath      A pointer to a device path data structure.
+
+  @return This function counts and returns the number of device path instances
+          in DevicePath.
+
+**/
+UINTN
+EfiDevicePathInstanceCount (
+  IN EFI_DEVICE_PATH_PROTOCOL      *DevicePath
+  )
+{
+  UINTN Count;
+  UINTN Size;
+
+  Count = 0;
+  while (GetNextDevicePathInstance (&DevicePath, &Size) != NULL) {
+    Count += 1;
+  }
+
+  return Count;
+}
+
+/**
+  Adjusts the size of a previously allocated buffer.
+
+
+  @param OldPool         - A pointer to the buffer whose size is being adjusted.
+  @param OldSize         - The size of the current buffer.
+  @param NewSize         - The size of the new buffer.
+
+  @return   The newly allocated buffer.
+  @retval   NULL  Allocation failed.
+
+**/
+VOID *
+EfiReallocatePool (
+  IN VOID                 *OldPool,
+  IN UINTN                OldSize,
+  IN UINTN                NewSize
+  )
+{
+  VOID  *NewPool;
+
+  NewPool = NULL;
+  if (NewSize != 0) {
+    NewPool = AllocateZeroPool (NewSize);
+  }
+
+  if (OldPool != NULL) {
+    if (NewPool != NULL) {
+      CopyMem (NewPool, OldPool, OldSize < NewSize ? OldSize : NewSize);
+    }
+
+    FreePool (OldPool);
+  }
+
+  return NewPool;
+}
+
+/**
+  Get a string from the Data Hub record based on 
+  a device path.
+
+  @param DevPath         The device Path.
+
+  @return A string located from the Data Hub records based on
+          the device path.
+  @retval NULL  If failed to get the String from Data Hub.
+
+**/
+UINT16 *
+EfiLibStrFromDatahub (
+  IN EFI_DEVICE_PATH_PROTOCOL                 *DevPath
+  )
+{
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/Bmstring.uni b/Core/MdeModulePkg/Application/UiApp/BootMaint/Bmstring.uni
new file mode 100644
index 0000000000..f91d599289
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/Bmstring.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.c
new file mode 100644
index 0000000000..98c46668f1
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.c
@@ -0,0 +1,1376 @@
+/** @file
+  The functions for Boot Maintainence Main menu.
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+EFI_DEVICE_PATH_PROTOCOL  EndDevicePath[] = {
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      END_DEVICE_PATH_LENGTH,
+      0
+    }
+  }
+};
+
+HII_VENDOR_DEVICE_PATH  mBmmHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    //
+    // {165A028F-0BB2-4b5f-8747-77592E3F6499}
+    //
+    { 0x165a028f, 0xbb2, 0x4b5f, { 0x87, 0x47, 0x77, 0x59, 0x2e, 0x3f, 0x64, 0x99 } }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+HII_VENDOR_DEVICE_PATH  mFeHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    //
+    // {91DB4238-B0C8-472e-BBCF-F3A6541010F4}
+    //
+    { 0x91db4238, 0xb0c8, 0x472e, { 0xbb, 0xcf, 0xf3, 0xa6, 0x54, 0x10, 0x10, 0xf4 } }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+EFI_GUID mBootMaintGuid          = BOOT_MAINT_FORMSET_GUID;
+EFI_GUID mFileExplorerGuid       = FILE_EXPLORE_FORMSET_GUID;
+
+CHAR16  mBootMaintStorageName[]     = L"BmmData";
+CHAR16  mFileExplorerStorageName[]  = L"FeData";
+BMM_CALLBACK_DATA *mBmmCallbackInfo = NULL;
+BOOLEAN  mAllMenuInit               = FALSE;
+BOOLEAN  mEnterFileExplorer         = FALSE;
+
+/**
+  Init all memu.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+InitAllMenu (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+/**
+  Free up all Menu Option list.
+
+**/
+VOID
+FreeAllMenu (
+  VOID
+  );
+
+/**
+  Create string tokens for a menu from its help strings and display strings
+
+  @param CallbackData       The BMM context data.
+  @param HiiHandle          Hii Handle of the package to be updated.
+  @param MenuOption         The Menu whose string tokens need to be created
+
+  @retval  EFI_SUCCESS      String tokens created successfully
+  @retval  others           contain some errors
+**/
+EFI_STATUS
+CreateMenuStringToken (
+  IN BMM_CALLBACK_DATA                *CallbackData,
+  IN EFI_HII_HANDLE                   HiiHandle,
+  IN BM_MENU_OPTION                   *MenuOption
+  )
+{
+  BM_MENU_ENTRY *NewMenuEntry;
+  UINTN         Index;
+
+  for (Index = 0; Index < MenuOption->MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (MenuOption, Index);
+
+    NewMenuEntry->DisplayStringToken = HiiSetString (
+                                         HiiHandle,
+                                         0,
+                                         NewMenuEntry->DisplayString,
+                                         NULL
+                                         );
+
+    if (NULL == NewMenuEntry->HelpString) {
+      NewMenuEntry->HelpStringToken = NewMenuEntry->DisplayStringToken;
+    } else {
+      NewMenuEntry->HelpStringToken = HiiSetString (
+                                        HiiHandle,
+                                        0,
+                                        NewMenuEntry->HelpString,
+                                        NULL
+                                        );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Request         A null-terminated Unicode string in <ConfigRequest> format.
+  @param Progress        On return, points to a character in the Request string.
+                         Points to the string's null terminator if request was successful.
+                         Points to the most recent '&' before the first failing name/value
+                         pair (or the beginning of the string if the failure is in the
+                         first name/value pair) if the request was not successful.
+  @param Results         A null-terminated Unicode string in <ConfigAltResp> format which
+                         has all values filled in for the names in the Request string.
+                         String to be allocated by the called function.
+
+  @retval  EFI_SUCCESS            The Results is filled with the requested values.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval  EFI_INVALID_PARAMETER  Request is NULL, illegal syntax, or unknown name.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+BootMaintExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS         Status;
+  UINTN              BufferSize;
+  BMM_CALLBACK_DATA  *Private;
+  EFI_STRING                       ConfigRequestHdr;
+  EFI_STRING                       ConfigRequest;
+  BOOLEAN                          AllocatedRequest;
+  UINTN                            Size;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Request;
+  if ((Request != NULL) && !HiiIsConfigHdrMatch (Request, &mBootMaintGuid, mBootMaintStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  ConfigRequestHdr = NULL;
+  ConfigRequest    = NULL;
+  AllocatedRequest = FALSE;
+  Size             = 0;
+
+  Private = BMM_CALLBACK_DATA_FROM_THIS (This);
+  //
+  // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+  //
+  BufferSize = sizeof (BMM_FAKE_NV_DATA);
+  ConfigRequest = Request;
+  if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+    //
+    // Request has no request element, construct full request string.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&mBootMaintGuid, mBootMaintStorageName, Private->BmmDriverHandle);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+  }
+
+  Status = gHiiConfigRouting->BlockToConfig (
+                                gHiiConfigRouting,
+                                ConfigRequest,
+                                (UINT8 *) &Private->BmmFakeNvData,
+                                BufferSize,
+                                Results,
+                                Progress
+                                );
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+  }
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This                Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration       A null-terminated Unicode string in
+                                  <ConfigString> format.   
+  @param[out] Progress            A pointer to a string filled in with the
+                                  offset of the most recent '&' before the
+                                  first failing name / value pair (or the
+                                  beginn ing of the string if the failure
+                                  is in the first name / value pair) or
+                                  the terminating NULL if all was
+                                  successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are
+                                  awaiting distribution.  
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETERS  Passing in a NULL for the
+                                  Results parameter would result
+                                  in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data
+                                  was not found.
+**/
+EFI_STATUS
+EFIAPI
+BootMaintRouteConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL *This,
+  IN CONST EFI_STRING                     Configuration,
+  OUT EFI_STRING                          *Progress
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           BufferSize;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL *ConfigRouting;
+  BMM_FAKE_NV_DATA                *NewBmmData;
+  BMM_FAKE_NV_DATA                *OldBmmData;
+  BM_CONSOLE_CONTEXT              *NewConsoleContext;
+  BM_TERMINAL_CONTEXT             *NewTerminalContext;
+  BM_MENU_ENTRY                   *NewMenuEntry;
+  BM_LOAD_CONTEXT                 *NewLoadContext;
+  UINT16                          Index;
+  BOOLEAN                         TerminalAttChange;
+  BMM_CALLBACK_DATA               *Private; 
+
+  if (Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Configuration;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check routing data in <ConfigHdr>.
+  // Note: there is no name for Name/Value storage, only GUID will be checked
+  //
+  if (!HiiIsConfigHdrMatch (Configuration, &mBootMaintGuid, mBootMaintStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiConfigRoutingProtocolGuid, 
+                  NULL, 
+                  (VOID **)&ConfigRouting
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Private = BMM_CALLBACK_DATA_FROM_THIS (This);  
+  //
+  // Get Buffer Storage data from EFI variable
+  //
+  BufferSize = sizeof (BMM_FAKE_NV_DATA);
+  OldBmmData = &Private->BmmOldFakeNVData;
+  NewBmmData = &Private->BmmFakeNvData;
+  //
+  // Convert <ConfigResp> to buffer data by helper function ConfigToBlock()
+  //
+  Status = ConfigRouting->ConfigToBlock (
+                            ConfigRouting,
+                            Configuration,
+                            (UINT8 *) NewBmmData,
+                            &BufferSize,
+                            Progress
+                            );
+  ASSERT_EFI_ERROR (Status);    
+  //
+  // Compare new and old BMM configuration data and only do action for modified item to 
+  // avoid setting unnecessary non-volatile variable
+  //
+  
+  //
+  // Check data which located in BMM main page and save the settings if need
+  //         
+  if (CompareMem (&NewBmmData->BootNext, &OldBmmData->BootNext, sizeof (NewBmmData->BootNext)) != 0) {
+    Status = Var_UpdateBootNext (Private);
+  }
+  
+  //
+  // Check data which located in Boot Options Menu and save the settings if need
+  //      
+  if (CompareMem (NewBmmData->BootOptionDel, OldBmmData->BootOptionDel, sizeof (NewBmmData->BootOptionDel)) != 0) {  
+    for (Index = 0; 
+         ((Index < BootOptionMenu.MenuNumber) && (Index < (sizeof (NewBmmData->BootOptionDel) / sizeof (NewBmmData->BootOptionDel[0])))); 
+         Index ++) {
+      NewMenuEntry            = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+      NewLoadContext          = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+      NewLoadContext->Deleted = NewBmmData->BootOptionDel[Index];
+      NewBmmData->BootOptionDel[Index] = FALSE;
+      NewBmmData->BootOptionDelMark[Index] = FALSE;
+    }
+    
+    Var_DelBootOption ();
+  }
+  
+  if (CompareMem (NewBmmData->BootOptionOrder, OldBmmData->BootOptionOrder, sizeof (NewBmmData->BootOptionOrder)) != 0) {  
+    Status = Var_UpdateBootOrder (Private);
+  }
+
+  if (CompareMem (&NewBmmData->BootTimeOut, &OldBmmData->BootTimeOut, sizeof (NewBmmData->BootTimeOut)) != 0){
+    Status = gRT->SetVariable(
+                    L"Timeout",
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    sizeof(UINT16),
+                    &(NewBmmData->BootTimeOut)
+                    );
+    ASSERT_EFI_ERROR(Status);
+
+    Private->BmmOldFakeNVData.BootTimeOut = NewBmmData->BootTimeOut;
+  }
+
+  //
+  // Check data which located in Driver Options Menu and save the settings if need
+  //              
+  if (CompareMem (NewBmmData->DriverOptionDel, OldBmmData->DriverOptionDel, sizeof (NewBmmData->DriverOptionDel)) != 0) {       
+    for (Index = 0; 
+         ((Index < DriverOptionMenu.MenuNumber) && (Index < (sizeof (NewBmmData->DriverOptionDel) / sizeof (NewBmmData->DriverOptionDel[0])))); 
+         Index++) {
+      NewMenuEntry            = BOpt_GetMenuEntry (&DriverOptionMenu, Index);
+      NewLoadContext          = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+      NewLoadContext->Deleted = NewBmmData->DriverOptionDel[Index];
+      NewBmmData->DriverOptionDel[Index] = FALSE;
+      NewBmmData->DriverOptionDelMark[Index] = FALSE;
+    }
+    Var_DelDriverOption ();  
+  }
+  
+  if (CompareMem (NewBmmData->DriverOptionOrder, OldBmmData->DriverOptionOrder, sizeof (NewBmmData->DriverOptionOrder)) != 0) {  
+    Status = Var_UpdateDriverOrder (Private);
+  }
+
+  if (CompareMem (&NewBmmData->ConsoleOutMode, &OldBmmData->ConsoleOutMode, sizeof (NewBmmData->ConsoleOutMode)) != 0){
+    Var_UpdateConMode(Private);
+  }
+
+  TerminalAttChange = FALSE;
+  for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+
+    //
+    // only need update modified items
+    //
+    if (CompareMem (&NewBmmData->COMBaudRate[Index], &OldBmmData->COMBaudRate[Index], sizeof (NewBmmData->COMBaudRate[Index])) == 0 &&
+         CompareMem (&NewBmmData->COMDataRate[Index], &OldBmmData->COMDataRate[Index], sizeof (NewBmmData->COMDataRate[Index])) == 0 &&
+         CompareMem (&NewBmmData->COMStopBits[Index], &OldBmmData->COMStopBits[Index], sizeof (NewBmmData->COMStopBits[Index])) == 0 &&
+         CompareMem (&NewBmmData->COMParity[Index], &OldBmmData->COMParity[Index], sizeof (NewBmmData->COMParity[Index])) == 0 &&
+         CompareMem (&NewBmmData->COMTerminalType[Index], &OldBmmData->COMTerminalType[Index], sizeof (NewBmmData->COMTerminalType[Index])) == 0 &&
+         CompareMem (&NewBmmData->COMFlowControl[Index], &OldBmmData->COMFlowControl[Index], sizeof (NewBmmData->COMFlowControl[Index])) == 0) {
+      continue;
+    }
+
+    NewMenuEntry = BOpt_GetMenuEntry (&TerminalMenu, Index);
+    ASSERT (NewMenuEntry != NULL);
+    NewTerminalContext = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+    NewTerminalContext->BaudRateIndex = NewBmmData->COMBaudRate[Index];
+    ASSERT (NewBmmData->COMBaudRate[Index] < (sizeof (BaudRateList) / sizeof (BaudRateList[0])));
+    NewTerminalContext->BaudRate      = BaudRateList[NewBmmData->COMBaudRate[Index]].Value;
+    NewTerminalContext->DataBitsIndex = NewBmmData->COMDataRate[Index];
+    ASSERT (NewBmmData->COMDataRate[Index] < (sizeof (DataBitsList) / sizeof (DataBitsList[0])));
+    NewTerminalContext->DataBits      = (UINT8) DataBitsList[NewBmmData->COMDataRate[Index]].Value;
+    NewTerminalContext->StopBitsIndex = NewBmmData->COMStopBits[Index];
+    ASSERT (NewBmmData->COMStopBits[Index] < (sizeof (StopBitsList) / sizeof (StopBitsList[0])));
+    NewTerminalContext->StopBits      = (UINT8) StopBitsList[NewBmmData->COMStopBits[Index]].Value;
+    NewTerminalContext->ParityIndex   = NewBmmData->COMParity[Index];
+    ASSERT (NewBmmData->COMParity[Index] < (sizeof (ParityList) / sizeof (ParityList[0])));
+    NewTerminalContext->Parity        = (UINT8) ParityList[NewBmmData->COMParity[Index]].Value;
+    NewTerminalContext->TerminalType  = NewBmmData->COMTerminalType[Index];
+    NewTerminalContext->FlowControl   = NewBmmData->COMFlowControl[Index];
+    ChangeTerminalDevicePath (
+      NewTerminalContext->DevicePath,
+      FALSE
+      );
+    TerminalAttChange = TRUE;
+  }
+  if (TerminalAttChange) {
+    Var_UpdateConsoleInpOption ();
+    Var_UpdateConsoleOutOption ();
+    Var_UpdateErrorOutOption ();
+  }
+  //
+  // Check data which located in Console Options Menu and save the settings if need
+  //
+  if (CompareMem (NewBmmData->ConsoleInCheck, OldBmmData->ConsoleInCheck, sizeof (NewBmmData->ConsoleInCheck)) != 0){
+    for (Index = 0; Index < ConsoleInpMenu.MenuNumber; Index++){
+      NewMenuEntry                = BOpt_GetMenuEntry(&ConsoleInpMenu, Index);
+      NewConsoleContext           = (BM_CONSOLE_CONTEXT *)NewMenuEntry->VariableContext;
+      ASSERT (Index < MAX_MENU_NUMBER);
+      NewConsoleContext->IsActive = NewBmmData->ConsoleInCheck[Index];
+    }
+    for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+      NewMenuEntry                = BOpt_GetMenuEntry (&TerminalMenu, Index);
+      NewTerminalContext          = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+      ASSERT (Index + ConsoleInpMenu.MenuNumber < MAX_MENU_NUMBER);
+      NewTerminalContext->IsConIn = NewBmmData->ConsoleInCheck[Index + ConsoleInpMenu.MenuNumber];
+    }
+    Var_UpdateConsoleInpOption();
+  }
+
+  if (CompareMem (NewBmmData->ConsoleOutCheck, OldBmmData->ConsoleOutCheck, sizeof (NewBmmData->ConsoleOutCheck)) != 0){
+    for (Index = 0; Index < ConsoleOutMenu.MenuNumber; Index++){
+      NewMenuEntry                = BOpt_GetMenuEntry(&ConsoleOutMenu, Index);
+      NewConsoleContext           = (BM_CONSOLE_CONTEXT *)NewMenuEntry->VariableContext;
+      ASSERT (Index < MAX_MENU_NUMBER);
+      NewConsoleContext->IsActive = NewBmmData->ConsoleOutCheck[Index];
+    }
+    for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+      NewMenuEntry                = BOpt_GetMenuEntry (&TerminalMenu, Index);
+      NewTerminalContext          = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+      ASSERT (Index + ConsoleOutMenu.MenuNumber < MAX_MENU_NUMBER);
+      NewTerminalContext->IsConOut = NewBmmData->ConsoleOutCheck[Index + ConsoleOutMenu.MenuNumber];
+    }
+    Var_UpdateConsoleOutOption();
+  }
+
+  if (CompareMem (NewBmmData->ConsoleErrCheck, OldBmmData->ConsoleErrCheck, sizeof (NewBmmData->ConsoleErrCheck)) != 0){
+    for (Index = 0; Index < ConsoleErrMenu.MenuNumber; Index++){
+      NewMenuEntry                = BOpt_GetMenuEntry(&ConsoleErrMenu, Index);
+      NewConsoleContext           = (BM_CONSOLE_CONTEXT *)NewMenuEntry->VariableContext;
+      ASSERT (Index < MAX_MENU_NUMBER);
+      NewConsoleContext->IsActive = NewBmmData->ConsoleErrCheck[Index];
+    }
+    for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+      NewMenuEntry                = BOpt_GetMenuEntry (&TerminalMenu, Index);
+      NewTerminalContext          = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+      ASSERT (Index + ConsoleErrMenu.MenuNumber < MAX_MENU_NUMBER);
+      NewTerminalContext->IsStdErr = NewBmmData->ConsoleErrCheck[Index + ConsoleErrMenu.MenuNumber];
+    }
+    Var_UpdateErrorOutOption();
+  }
+
+  //
+  // After user do the save action, need to update OldBmmData.
+  //
+  CopyMem (OldBmmData, NewBmmData, sizeof (BMM_FAKE_NV_DATA));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action             Specifies the type of action taken by the browser.
+  @param QuestionId         A unique value which is sent to the original exporting driver
+                            so that it can identify the type of data to expect.
+  @param Type               The type of value for the question.
+  @param Value              A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest      On return, points to the action requested by the callback function.
+
+  @retval EFI_SUCCESS           The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR      The variable could not be saved.
+  @retval EFI_UNSUPPORTED       The specified Action is not supported by the callback.
+  @retval EFI_INVALID_PARAMETER The parameter of Value or ActionRequest is invalid.
+**/
+EFI_STATUS
+EFIAPI
+BootMaintCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL         *This,
+  IN        EFI_BROWSER_ACTION                     Action,
+  IN        EFI_QUESTION_ID                        QuestionId,
+  IN        UINT8                                  Type,
+  IN        EFI_IFR_TYPE_VALUE                     *Value,
+  OUT       EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  BMM_CALLBACK_DATA *Private;
+  BM_MENU_ENTRY     *NewMenuEntry;
+  BMM_FAKE_NV_DATA  *CurrentFakeNVMap;
+  UINTN             Index;
+
+  //
+  //Chech whether exit from FileExplorer and reenter BM,if yes,reclaim string depositories
+  //
+  if (Action == EFI_BROWSER_ACTION_FORM_OPEN){
+    if(mEnterFileExplorer ){
+      ReclaimStringDepository();
+      mEnterFileExplorer = FALSE;
+    }
+  }
+
+  if (Action != EFI_BROWSER_ACTION_CHANGING && Action != EFI_BROWSER_ACTION_CHANGED) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Private        = BMM_CALLBACK_DATA_FROM_THIS (This);
+
+  //
+  // Retrive uncommitted data from Form Browser
+  //
+  CurrentFakeNVMap = &Private->BmmFakeNvData;
+  HiiGetBrowserData (&mBootMaintGuid, mBootMaintStorageName, sizeof (BMM_FAKE_NV_DATA), (UINT8 *) CurrentFakeNVMap);
+
+  if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if (Value == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    UpdatePageId (Private, QuestionId);
+
+    if (QuestionId < FILE_OPTION_OFFSET) {
+      if (QuestionId < CONFIG_OPTION_OFFSET) {
+        switch (QuestionId) {
+        case FORM_BOOT_ADD_ID:
+          // Leave Bm and enter FileExplorer.
+          Private->FeCurrentState = FileExplorerStateAddBootOption;
+          Private->FeDisplayContext = FileExplorerDisplayUnknown;
+          ReclaimStringDepository (); 
+          UpdateFileExplorer(Private, 0);
+          mEnterFileExplorer = TRUE;
+          break;
+
+        case FORM_DRV_ADD_FILE_ID:
+          // Leave Bm and enter FileExplorer.
+          Private->FeCurrentState = FileExplorerStateAddDriverOptionState;
+          Private->FeDisplayContext = FileExplorerDisplayUnknown;
+          ReclaimStringDepository ();
+          UpdateFileExplorer(Private, 0);
+          mEnterFileExplorer = TRUE;
+          break;
+
+        case FORM_DRV_ADD_HANDLE_ID:
+          CleanUpPage (FORM_DRV_ADD_HANDLE_ID, Private);
+          UpdateDrvAddHandlePage (Private);
+          break;
+
+        case FORM_BOOT_DEL_ID:
+          CleanUpPage (FORM_BOOT_DEL_ID, Private);
+          UpdateBootDelPage (Private);
+          break;
+
+        case FORM_BOOT_CHG_ID:
+        case FORM_DRV_CHG_ID:
+          UpdatePageBody (QuestionId, Private);
+          break;
+
+        case FORM_DRV_DEL_ID:
+          CleanUpPage (FORM_DRV_DEL_ID, Private);
+          UpdateDrvDelPage (Private);
+          break;
+
+        case FORM_BOOT_NEXT_ID:
+          CleanUpPage (FORM_BOOT_NEXT_ID, Private);
+          UpdateBootNextPage (Private);
+          break;
+
+        case FORM_TIME_OUT_ID:
+          CleanUpPage (FORM_TIME_OUT_ID, Private);
+          UpdateTimeOutPage (Private);
+          break;
+
+        case FORM_CON_IN_ID:
+        case FORM_CON_OUT_ID:
+        case FORM_CON_ERR_ID:
+          UpdatePageBody (QuestionId, Private);
+          break;
+
+        case FORM_CON_MODE_ID:
+          CleanUpPage (FORM_CON_MODE_ID, Private);
+          UpdateConModePage (Private);
+          break;
+
+        case FORM_CON_COM_ID:
+          CleanUpPage (FORM_CON_COM_ID, Private);
+          UpdateConCOMPage (Private);
+          break;
+
+        default:
+          break;
+        }
+      } else if ((QuestionId >= TERMINAL_OPTION_OFFSET) && (QuestionId < CONSOLE_OPTION_OFFSET)) {
+        Index                  = (UINT16) (QuestionId - TERMINAL_OPTION_OFFSET);
+        Private->CurrentTerminal  = Index;
+
+        CleanUpPage (FORM_CON_COM_SETUP_ID, Private);
+        UpdateTerminalPage (Private);
+
+      } else if (QuestionId >= HANDLE_OPTION_OFFSET) {
+        Index                  = (UINT16) (QuestionId - HANDLE_OPTION_OFFSET);
+
+        NewMenuEntry            = BOpt_GetMenuEntry (&DriverMenu, Index);
+        ASSERT (NewMenuEntry != NULL);
+        Private->HandleContext  = (BM_HANDLE_CONTEXT *) NewMenuEntry->VariableContext;
+
+        CleanUpPage (FORM_DRV_ADD_HANDLE_DESC_ID, Private);
+
+        Private->MenuEntry                  = NewMenuEntry;
+        Private->LoadContext->FilePathList  = Private->HandleContext->DevicePath;
+
+        UpdateDriverAddHandleDescPage (Private);
+      }
+    }
+    if (QuestionId == KEY_VALUE_BOOT_FROM_FILE){
+      // Leave Bm and enter FileExplorer.
+      Private->FeCurrentState = FileExplorerStateBootFromFile;
+      Private->FeDisplayContext = FileExplorerDisplayUnknown;
+      ReclaimStringDepository ();
+      UpdateFileExplorer(Private, 0);
+      mEnterFileExplorer = TRUE;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    if ((Value == NULL) || (ActionRequest == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((QuestionId >= BOOT_OPTION_DEL_QUESTION_ID) && (QuestionId < BOOT_OPTION_DEL_QUESTION_ID + MAX_MENU_NUMBER)) {
+      if (Value->b){
+        //
+        // Means user try to delete this boot option but not press F10 or "Commit Changes and Exit" menu.
+        //
+        CurrentFakeNVMap->BootOptionDelMark[QuestionId - BOOT_OPTION_DEL_QUESTION_ID] = TRUE;
+      } else {
+        //
+        // Means user remove the old check status.
+        //
+        CurrentFakeNVMap->BootOptionDelMark[QuestionId - BOOT_OPTION_DEL_QUESTION_ID] = FALSE;
+      }
+    } else if ((QuestionId >= DRIVER_OPTION_DEL_QUESTION_ID) && (QuestionId < DRIVER_OPTION_DEL_QUESTION_ID + MAX_MENU_NUMBER)) {
+      if (Value->b){
+        CurrentFakeNVMap->DriverOptionDelMark[QuestionId - DRIVER_OPTION_DEL_QUESTION_ID] = TRUE;
+      } else {
+        CurrentFakeNVMap->DriverOptionDelMark[QuestionId - DRIVER_OPTION_DEL_QUESTION_ID] = FALSE;
+      }
+    } else {
+      switch (QuestionId) {
+      case KEY_VALUE_SAVE_AND_EXIT:
+      case KEY_VALUE_NO_SAVE_AND_EXIT:
+        if (QuestionId == KEY_VALUE_SAVE_AND_EXIT) {
+          *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_SUBMIT_EXIT;
+        } else if (QuestionId == KEY_VALUE_NO_SAVE_AND_EXIT) {
+          DiscardChangeHandler (Private, CurrentFakeNVMap);
+          *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD_EXIT;
+        }
+
+        break;
+ 
+      case FORM_RESET:
+        gRT->ResetSystem (EfiResetCold, EFI_SUCCESS, 0, NULL);
+        return EFI_UNSUPPORTED;
+
+      default:
+        break;
+      }
+    }
+  }
+
+  //
+  // Pass changed uncommitted data back to Form Browser
+  //
+  HiiSetBrowserData (&mBootMaintGuid, mBootMaintStorageName, sizeof (BMM_FAKE_NV_DATA), (UINT8 *) CurrentFakeNVMap, NULL);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Discard all changes done to the BMM pages such as Boot Order change,
+  Driver order change.
+
+  @param Private            The BMM context data.
+  @param CurrentFakeNVMap   The current Fack NV Map.
+
+**/
+VOID
+DiscardChangeHandler (
+  IN  BMM_CALLBACK_DATA               *Private,
+  IN  BMM_FAKE_NV_DATA                *CurrentFakeNVMap
+  )
+{
+  UINT16  Index;
+
+  switch (Private->BmmPreviousPageId) {
+  case FORM_BOOT_CHG_ID:
+    CopyMem (CurrentFakeNVMap->BootOptionOrder, Private->BmmOldFakeNVData.BootOptionOrder, sizeof (CurrentFakeNVMap->BootOptionOrder));
+    break;
+	
+  case FORM_DRV_CHG_ID:
+    CopyMem (CurrentFakeNVMap->DriverOptionOrder, Private->BmmOldFakeNVData.DriverOptionOrder, sizeof (CurrentFakeNVMap->DriverOptionOrder));
+    break;
+
+  case FORM_BOOT_DEL_ID:
+    ASSERT (BootOptionMenu.MenuNumber <= (sizeof (CurrentFakeNVMap->BootOptionDel) / sizeof (CurrentFakeNVMap->BootOptionDel[0])));
+    for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+      CurrentFakeNVMap->BootOptionDel[Index] = FALSE;
+    }
+    break;
+
+  case FORM_DRV_DEL_ID:
+    ASSERT (DriverOptionMenu.MenuNumber <= (sizeof (CurrentFakeNVMap->DriverOptionDel) / sizeof (CurrentFakeNVMap->DriverOptionDel[0])));
+    for (Index = 0; Index < DriverOptionMenu.MenuNumber; Index++) {
+      CurrentFakeNVMap->DriverOptionDel[Index] = FALSE;
+    }
+    break;
+
+  case FORM_BOOT_NEXT_ID:
+    CurrentFakeNVMap->BootNext = Private->BmmOldFakeNVData.BootNext;
+    break;
+
+  case FORM_TIME_OUT_ID:
+    CurrentFakeNVMap->BootTimeOut = Private->BmmOldFakeNVData.BootTimeOut;
+    break;
+
+  case FORM_DRV_ADD_HANDLE_DESC_ID:
+  case FORM_DRV_ADD_FILE_ID:
+  case FORM_DRV_ADD_HANDLE_ID:
+    CurrentFakeNVMap->DriverAddHandleDesc[0]          = 0x0000;
+    CurrentFakeNVMap->DriverAddHandleOptionalData[0]  = 0x0000;
+    break;
+
+  default:
+    break;
+  }
+}
+
+
+/**
+  Create dynamic code for BMM.
+
+  @param  BmmCallbackInfo        The BMM context data.
+
+**/
+VOID
+InitializeDrivers(
+  IN BMM_CALLBACK_DATA        *BmmCallbackInfo
+  )
+{
+  EFI_HII_HANDLE              HiiHandle;
+  VOID                        *StartOpCodeHandle;
+  VOID                        *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL          *StartLabel;
+  EFI_IFR_GUID_LABEL          *EndLabel;
+  UINTN                       Index;  
+  EFI_STRING_ID               FormSetTitle;
+  EFI_STRING_ID               FormSetHelp;  
+  EFI_STRING                  String;
+  EFI_STRING_ID               Token;
+  EFI_STRING_ID               TokenHelp;  
+  EFI_HII_HANDLE              *HiiHandles;
+  UINTN                       SkipCount;
+  EFI_GUID                    FormSetGuid;
+  CHAR16                      *DevicePathStr;
+  EFI_STRING_ID               DevicePathId;
+
+  HiiHandle = BmmCallbackInfo->BmmHiiHandle;
+  //
+  // Allocate space for creation of UpdateData Buffer
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = LABEL_BMM_PLATFORM_INFORMATION;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  //
+  // Get all the Hii handles
+  //
+  HiiHandles = HiiGetHiiHandles (NULL);
+  ASSERT (HiiHandles != NULL);
+
+  //
+  // Search for formset of each class type
+  //
+  SkipCount = 0;
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    if (!ExtractDisplayedHiiFormFromHiiHandle (HiiHandles[Index], &gEfiIfrBootMaintenanceGuid, SkipCount, &FormSetTitle, &FormSetHelp, &FormSetGuid)) {
+      SkipCount = 0;
+      continue;
+    }
+    String = HiiGetString (HiiHandles[Index], FormSetTitle, NULL);
+    if (String == NULL) {
+      String = HiiGetString (HiiHandle, STR_MISSING_STRING, NULL);
+      ASSERT (String != NULL);
+    }
+    Token = HiiSetString (HiiHandle, 0, String, NULL);
+    FreePool (String);
+
+    String = HiiGetString (HiiHandles[Index], FormSetHelp, NULL);
+    if (String == NULL) {
+      String = HiiGetString (HiiHandle, STR_MISSING_STRING, NULL);
+      ASSERT (String != NULL);
+    }
+    TokenHelp = HiiSetString (HiiHandle, 0, String, NULL);
+    FreePool (String);
+
+    DevicePathStr = ExtractDevicePathFromHiiHandle(HiiHandles[Index]);
+    DevicePathId  = 0;
+    if (DevicePathStr != NULL){
+      DevicePathId = HiiSetString (HiiHandle, 0, DevicePathStr, NULL);
+      FreePool (DevicePathStr);
+    }
+
+    HiiCreateGotoExOpCode (
+      StartOpCodeHandle,
+      0,
+      Token,
+      TokenHelp,
+      0,
+      (EFI_QUESTION_ID) (Index + FRONT_PAGE_KEY_OFFSET),
+      0,
+      &FormSetGuid,
+      DevicePathId
+      );
+    //
+    //One packagelist may has more than one form package,
+    //Index-- means keep current HiiHandle and still extract from the packagelist, 
+    //SkipCount++ means skip the formset which was found before in the same form package. 
+    //
+    SkipCount++;
+    Index--;
+  }
+
+  HiiUpdateForm (
+    HiiHandle,
+    &mBootMaintGuid,
+    FORM_MAIN_ID,
+    StartOpCodeHandle,
+    EndOpCodeHandle
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);  
+  FreePool (HiiHandles);
+}
+
+/**
+   Create dynamic code for BMM and initialize all of BMM configuration data in BmmFakeNvData and
+   BmmOldFakeNVData member in BMM context data.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+InitializeBmmConfig (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT16          Index;
+
+  ASSERT (CallbackData != NULL);
+
+  InitializeDrivers (CallbackData);
+
+  //
+  // Initialize data which located in BMM main page
+  //
+  CallbackData->BmmFakeNvData.BootNext = (UINT16) (BootOptionMenu.MenuNumber);
+  for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry    = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+    NewLoadContext  = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+
+    if (NewLoadContext->IsBootNext) {
+      CallbackData->BmmFakeNvData.BootNext = Index;
+      break;
+    }
+  }
+
+  CallbackData->BmmFakeNvData.BootTimeOut = PcdGet16 (PcdPlatformBootTimeOut);
+
+  //
+  // Initialize data which located in Boot Options Menu
+  //
+  GetBootOrder (CallbackData);
+
+  //
+  // Initialize data which located in Driver Options Menu
+  //
+  GetDriverOrder (CallbackData);
+
+  //
+  // Initialize data which located in Console Options Menu
+  //  
+  GetConsoleOutMode (CallbackData);
+  GetConsoleInCheck (CallbackData);
+  GetConsoleOutCheck (CallbackData);
+  GetConsoleErrCheck (CallbackData);
+  GetTerminalAttribute (CallbackData);
+
+  //
+  // Backup Initialize BMM configuartion data to BmmOldFakeNVData
+  //
+  CopyMem (&CallbackData->BmmOldFakeNVData, &CallbackData->BmmFakeNvData, sizeof (BMM_FAKE_NV_DATA));
+}
+
+/**
+  Initialize the Boot Maintenance Utitliy.
+
+**/
+VOID
+InitializeBM (
+  VOID
+  )
+{
+  BMM_CALLBACK_DATA           *BmmCallbackInfo;
+
+  BmmCallbackInfo = mBmmCallbackInfo;
+  BmmCallbackInfo->BmmPreviousPageId             = FORM_MAIN_ID;
+  BmmCallbackInfo->BmmCurrentPageId              = FORM_MAIN_ID;
+  BmmCallbackInfo->FeCurrentState                = FileExplorerStateInActive;
+  BmmCallbackInfo->FeDisplayContext              = FileExplorerDisplayUnknown;
+
+  InitAllMenu (BmmCallbackInfo);
+
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &ConsoleInpMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &ConsoleOutMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &ConsoleErrMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &BootOptionMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &DriverOptionMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &TerminalMenu);
+  CreateMenuStringToken (BmmCallbackInfo, BmmCallbackInfo->BmmHiiHandle, &DriverMenu);
+
+  InitializeBmmConfig(BmmCallbackInfo);
+}
+
+/**
+  Initialized all Menu Option List.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+InitAllMenu (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  InitializeListHead (&BootOptionMenu.Head);
+  InitializeListHead (&DriverOptionMenu.Head);
+  BOpt_GetBootOptions (CallbackData);
+  BOpt_GetDriverOptions (CallbackData);
+  InitializeListHead (&FsOptionMenu.Head);
+  BOpt_FindDrivers ();
+  InitializeListHead (&DirectoryMenu.Head);
+  InitializeListHead (&ConsoleInpMenu.Head);
+  InitializeListHead (&ConsoleOutMenu.Head);
+  InitializeListHead (&ConsoleErrMenu.Head);
+  InitializeListHead (&TerminalMenu.Head);
+  LocateSerialIo ();
+  GetAllConsoles ();
+  mAllMenuInit = TRUE;
+}
+
+/**
+  Free up all Menu Option list.
+
+**/
+VOID
+FreeAllMenu (
+  VOID
+  )
+{
+  if (!mAllMenuInit){
+    return;
+  }
+  BOpt_FreeMenu (&DirectoryMenu);
+  BOpt_FreeMenu (&FsOptionMenu);
+  BOpt_FreeMenu (&BootOptionMenu);
+  BOpt_FreeMenu (&DriverOptionMenu);
+  BOpt_FreeMenu (&DriverMenu);
+  FreeAllConsoles ();
+  mAllMenuInit = FALSE;
+}
+
+/**
+  Initialize all the string depositories.
+
+**/
+VOID
+InitializeStringDepository (
+  VOID
+  )
+{
+  STRING_DEPOSITORY *StringDepository;
+  StringDepository              = AllocateZeroPool (sizeof (STRING_DEPOSITORY) * STRING_DEPOSITORY_NUMBER);
+  FileOptionStrDepository       = StringDepository++;
+  ConsoleOptionStrDepository    = StringDepository++;
+  BootOptionStrDepository       = StringDepository++;
+  BootOptionHelpStrDepository   = StringDepository++;
+  DriverOptionStrDepository     = StringDepository++;
+  DriverOptionHelpStrDepository = StringDepository++;
+  TerminalStrDepository         = StringDepository;
+}
+
+/**
+  Fetch a usable string node from the string depository and return the string token.
+
+  @param CallbackData       The BMM context data.
+  @param StringDepository   The string repository.
+
+  @retval  EFI_STRING_ID           String token.
+
+**/
+EFI_STRING_ID
+GetStringTokenFromDepository (
+  IN   BMM_CALLBACK_DATA     *CallbackData,
+  IN   STRING_DEPOSITORY     *StringDepository
+  )
+{
+  STRING_LIST_NODE  *CurrentListNode;
+  STRING_LIST_NODE  *NextListNode;
+
+  CurrentListNode = StringDepository->CurrentNode;
+
+  if ((NULL != CurrentListNode) && (NULL != CurrentListNode->Next)) {
+    //
+    // Fetch one reclaimed node from the list.
+    //
+    NextListNode = StringDepository->CurrentNode->Next;
+  } else {
+    //
+    // If there is no usable node in the list, update the list.
+    //
+    NextListNode = AllocateZeroPool (sizeof (STRING_LIST_NODE));
+    ASSERT (NextListNode != NULL);
+    NextListNode->StringToken = HiiSetString (CallbackData->BmmHiiHandle, 0, L" ", NULL);
+    ASSERT (NextListNode->StringToken != 0);
+
+    StringDepository->TotalNodeNumber++;
+
+    if (NULL == CurrentListNode) {
+      StringDepository->ListHead = NextListNode;
+    } else {
+      CurrentListNode->Next = NextListNode;
+    }
+  }
+
+  StringDepository->CurrentNode = NextListNode;
+
+  return StringDepository->CurrentNode->StringToken;
+}
+
+/**
+  Reclaim string depositories by moving the current node pointer to list head..
+
+**/
+VOID
+ReclaimStringDepository (
+  VOID
+  )
+{
+  UINTN             DepositoryIndex;
+  STRING_DEPOSITORY *StringDepository;
+
+  StringDepository = FileOptionStrDepository;
+  for (DepositoryIndex = 0; DepositoryIndex < STRING_DEPOSITORY_NUMBER; DepositoryIndex++) {
+    StringDepository->CurrentNode = StringDepository->ListHead;
+    StringDepository++;
+  }
+}
+
+/**
+  Release resource for all the string depositories.
+
+**/
+VOID
+CleanUpStringDepository (
+  VOID
+  )
+{
+  UINTN             NodeIndex;
+  UINTN             DepositoryIndex;
+  STRING_LIST_NODE  *CurrentListNode;
+  STRING_LIST_NODE  *NextListNode;
+  STRING_DEPOSITORY *StringDepository;
+
+  //
+  // Release string list nodes.
+  //
+  StringDepository = FileOptionStrDepository;
+  for (DepositoryIndex = 0; DepositoryIndex < STRING_DEPOSITORY_NUMBER; DepositoryIndex++) {
+    CurrentListNode = StringDepository->ListHead;
+    for (NodeIndex = 0; NodeIndex < StringDepository->TotalNodeNumber; NodeIndex++) {
+      NextListNode = CurrentListNode->Next;
+      FreePool (CurrentListNode);
+      CurrentListNode = NextListNode;
+    }
+
+    StringDepository++;
+  }
+  //
+  // Release string depository.
+  //
+  FreePool (FileOptionStrDepository);
+}
+
+/**
+  Install BootMaint and FileExplorer HiiPackages.
+
+**/
+VOID
+InitBootMaintenance(
+  VOID
+  )
+{
+  BMM_CALLBACK_DATA        *BmmCallbackInfo;
+  EFI_STATUS               Status;
+  UINT8                    *Ptr;
+
+  Status = EFI_SUCCESS;
+
+  if (!gConnectAllHappened){
+    EfiBootManagerConnectAll();
+    gConnectAllHappened = TRUE;
+  }
+
+  EfiBootManagerRefreshAllBootOption ();
+
+  //
+  // Create CallbackData structures for Driver Callback
+  //
+  BmmCallbackInfo = AllocateZeroPool (sizeof (BMM_CALLBACK_DATA));
+  ASSERT (BmmCallbackInfo != NULL);
+
+  //
+  // Create LoadOption in BmmCallbackInfo for Driver Callback
+  //
+  Ptr = AllocateZeroPool (sizeof (BM_LOAD_CONTEXT) + sizeof (BM_FILE_CONTEXT) + sizeof (BM_HANDLE_CONTEXT) + sizeof (BM_MENU_ENTRY));
+  ASSERT (Ptr != NULL);
+
+  //
+  // Initialize Bmm callback data.
+  //
+  BmmCallbackInfo->LoadContext = (BM_LOAD_CONTEXT *) Ptr;
+  Ptr += sizeof (BM_LOAD_CONTEXT);
+
+  BmmCallbackInfo->FileContext = (BM_FILE_CONTEXT *) Ptr;
+  Ptr += sizeof (BM_FILE_CONTEXT);
+
+  BmmCallbackInfo->HandleContext = (BM_HANDLE_CONTEXT *) Ptr;
+  Ptr += sizeof (BM_HANDLE_CONTEXT);
+
+  BmmCallbackInfo->MenuEntry     = (BM_MENU_ENTRY *) Ptr;
+
+  BmmCallbackInfo->Signature                     = BMM_CALLBACK_DATA_SIGNATURE;
+  BmmCallbackInfo->BmmConfigAccess.ExtractConfig = BootMaintExtractConfig;
+  BmmCallbackInfo->BmmConfigAccess.RouteConfig   = BootMaintRouteConfig;
+  BmmCallbackInfo->BmmConfigAccess.Callback      = BootMaintCallback;
+  BmmCallbackInfo->BmmPreviousPageId             = FORM_MAIN_ID;
+  BmmCallbackInfo->BmmCurrentPageId              = FORM_MAIN_ID;
+  BmmCallbackInfo->FeConfigAccess.ExtractConfig  = FakeExtractConfig;
+  BmmCallbackInfo->FeConfigAccess.RouteConfig    = FileExplorerRouteConfig;
+  BmmCallbackInfo->FeConfigAccess.Callback       = FileExplorerCallback;
+  BmmCallbackInfo->FeCurrentState                = FileExplorerStateInActive;
+  BmmCallbackInfo->FeDisplayContext              = FileExplorerDisplayUnknown;
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &BmmCallbackInfo->BmmDriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mBmmHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &BmmCallbackInfo->BmmConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &BmmCallbackInfo->FeDriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mFeHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &BmmCallbackInfo->FeConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Post our Boot Maint VFR binary to the HII database.
+  //
+  BmmCallbackInfo->BmmHiiHandle = HiiAddPackages (
+                                    &mBootMaintGuid,
+                                    BmmCallbackInfo->BmmDriverHandle,
+                                    BmBin,
+                                    UiAppStrings,
+                                    NULL
+                                    );
+  ASSERT (BmmCallbackInfo->BmmHiiHandle != NULL);
+
+  //
+  // Post our File Explorer VFR binary to the HII database.
+  //
+  BmmCallbackInfo->FeHiiHandle = HiiAddPackages (
+                                   &mFileExplorerGuid,
+                                   BmmCallbackInfo->FeDriverHandle,
+                                   FEBin,
+                                   UiAppStrings,
+                                   NULL
+                                   );
+  ASSERT (BmmCallbackInfo->FeHiiHandle != NULL);
+
+  //
+  // Init OpCode Handle and Allocate space for creation of Buffer
+  //
+  mStartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (mStartOpCodeHandle != NULL);
+
+  mEndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (mEndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  mStartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (mStartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  mStartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  mEndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (mEndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  mEndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  mEndLabel->Number       = LABEL_END;
+
+  mBmmCallbackInfo = BmmCallbackInfo;
+
+  InitializeStringDepository ();
+
+}
+
+/**
+  Remove the installed BootMaint and FileExplorer HiiPackages.
+
+**/
+VOID
+FreeBMPackage(
+  VOID
+  )
+{
+  BMM_CALLBACK_DATA         *BmmCallbackInfo;
+
+  if (mStartOpCodeHandle != NULL) {
+    HiiFreeOpCodeHandle (mStartOpCodeHandle);
+  }
+
+  if (mEndOpCodeHandle != NULL) {
+    HiiFreeOpCodeHandle (mEndOpCodeHandle);
+  }
+
+  FreeAllMenu ();
+  CleanUpStringDepository ();
+
+  BmmCallbackInfo = mBmmCallbackInfo;
+
+  //
+  // Remove our IFR data from HII database
+  //
+  HiiRemovePackages (BmmCallbackInfo->BmmHiiHandle);
+  HiiRemovePackages (BmmCallbackInfo->FeHiiHandle);
+
+  gBS->UninstallMultipleProtocolInterfaces (
+         BmmCallbackInfo->FeDriverHandle,
+         &gEfiDevicePathProtocolGuid,
+         &mFeHiiVendorDevicePath,
+         &gEfiHiiConfigAccessProtocolGuid,
+         &BmmCallbackInfo->FeConfigAccess,
+         NULL
+         );
+
+  gBS->UninstallMultipleProtocolInterfaces (
+         BmmCallbackInfo->BmmDriverHandle,
+         &gEfiDevicePathProtocolGuid,
+         &mBmmHiiVendorDevicePath,
+         &gEfiHiiConfigAccessProtocolGuid,
+         &BmmCallbackInfo->BmmConfigAccess,
+         NULL
+         );
+
+  FreePool (BmmCallbackInfo->LoadContext);
+  FreePool (BmmCallbackInfo);
+}
+
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.h b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.h
new file mode 100644
index 0000000000..23d2d2b6d1
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootMaint.h
@@ -0,0 +1,1567 @@
+/** @file
+  Header file for boot maintenance module.
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _BOOT_MAINT_H_
+#define _BOOT_MAINT_H_
+
+#include "Ui.h"
+#include "FormGuid.h"
+#include "FrontPage.h"
+#include <Guid/TtyTerm.h>
+
+//
+// Constants which are variable names used to access variables
+//
+
+#define VAR_CON_OUT_MODE L"ConOutMode"
+
+
+//
+// String Contant
+//
+#define STR_FLOPPY       L"Floppy Drive #%02x"
+#define STR_HARDDISK     L"HardDisk Drive #%02x"
+#define STR_CDROM        L"ATAPI CDROM Drive #%02x"
+#define STR_NET          L"NET Drive #%02x"
+#define STR_BEV          L"BEV Drive #%02x"
+#define STR_FLOPPY_HELP   L"Select Floppy Drive #%02x"
+#define STR_HARDDISK_HELP L"Select HardDisk Drive #%02x"
+#define STR_CDROM_HELP    L"Select ATAPI CDROM Drive #%02x"
+#define STR_NET_HELP      L"NET Drive #%02x"
+#define STR_BEV_HELP      L"BEV Drive #%02x"
+
+//
+// Variable created with this flag will be "Efi:...."
+//
+#define VAR_FLAG  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE
+
+extern EFI_GUID mBootMaintGuid;
+extern EFI_GUID mFileExplorerGuid;
+extern CHAR16   mFileExplorerStorageName[];
+extern CHAR16   mBootMaintStorageName[];
+//
+// These are the VFR compiler generated data representing our VFR data.
+//
+extern UINT8    BmBin[];
+extern UINT8    FEBin[];
+
+//
+// Below are the number of options in Baudrate, Databits,
+// Parity and Stopbits selection for serial ports.
+//
+#define BM_COM_ATTR_BUADRATE  19
+#define BM_COM_ATTR_DATABITS  4
+#define BM_COM_ATTR_PARITY    5
+#define BM_COM_ATTR_STOPBITS  3
+
+//
+// Callback function helper
+//
+#define BMM_CALLBACK_DATA_SIGNATURE     SIGNATURE_32 ('C', 'b', 'c', 'k')
+#define BMM_CALLBACK_DATA_FROM_THIS(a)  CR (a, BMM_CALLBACK_DATA, BmmConfigAccess, BMM_CALLBACK_DATA_SIGNATURE)
+
+#define FE_CALLBACK_DATA_FROM_THIS(a)   CR (a, BMM_CALLBACK_DATA, FeConfigAccess, BMM_CALLBACK_DATA_SIGNATURE)
+
+//
+// Enumeration type definition
+//
+typedef UINT8 BBS_TYPE;
+
+typedef enum _TYPE_OF_TERMINAL {
+  TerminalTypePcAnsi                             = 0,
+  TerminalTypeVt100,
+  TerminalTypeVt100Plus,
+  TerminalTypeVtUtf8,
+  TerminalTypeTtyTerm
+} TYPE_OF_TERMINAL;
+
+typedef enum _FILE_EXPLORER_STATE {
+  FileExplorerStateInActive                      = 0,
+  FileExplorerStateBootFromFile,
+  FileExplorerStateAddBootOption,
+  FileExplorerStateAddDriverOptionState,
+  FileExplorerStateUnknown
+} FILE_EXPLORER_STATE;
+
+typedef enum _FILE_EXPLORER_DISPLAY_CONTEXT {
+  FileExplorerDisplayFileSystem,
+  FileExplorerDisplayDirectory,
+  FileExplorerDisplayUnknown
+} FILE_EXPLORER_DISPLAY_CONTEXT;
+
+//
+// All of the signatures that will be used in list structure
+//
+#define BM_MENU_OPTION_SIGNATURE      SIGNATURE_32 ('m', 'e', 'n', 'u')
+#define BM_LOAD_OPTION_SIGNATURE      SIGNATURE_32 ('l', 'o', 'a', 'd')
+#define BM_CONSOLE_OPTION_SIGNATURE   SIGNATURE_32 ('c', 'n', 's', 'l')
+#define BM_FILE_OPTION_SIGNATURE      SIGNATURE_32 ('f', 'i', 'l', 'e')
+#define BM_HANDLE_OPTION_SIGNATURE    SIGNATURE_32 ('h', 'n', 'd', 'l')
+#define BM_TERMINAL_OPTION_SIGNATURE  SIGNATURE_32 ('t', 'r', 'm', 'l')
+#define BM_MENU_ENTRY_SIGNATURE       SIGNATURE_32 ('e', 'n', 't', 'r')
+
+#define BM_LOAD_CONTEXT_SELECT        0x0
+#define BM_CONSOLE_CONTEXT_SELECT     0x1
+#define BM_FILE_CONTEXT_SELECT        0x2
+#define BM_HANDLE_CONTEXT_SELECT      0x3
+#define BM_TERMINAL_CONTEXT_SELECT    0x5
+
+#define BM_CONSOLE_IN_CONTEXT_SELECT  0x6
+#define BM_CONSOLE_OUT_CONTEXT_SELECT 0x7
+#define BM_CONSOLE_ERR_CONTEXT_SELECT 0x8
+
+//
+// Buffer size for update data
+//
+#define UPDATE_DATA_SIZE        0x100000
+
+//
+// Namespace of callback keys used in display and file system navigation
+//
+#define MAX_BBS_OFFSET          0xE000
+#define NET_OPTION_OFFSET       0xD800
+#define BEV_OPTION_OFFSET       0xD000
+#define FD_OPTION_OFFSET        0xC000
+#define HD_OPTION_OFFSET        0xB000
+#define CD_OPTION_OFFSET        0xA000
+#define FILE_OPTION_OFFSET      0x8000
+#define FILE_OPTION_MASK        0x7FFF
+#define HANDLE_OPTION_OFFSET    0x7000
+#define CONSOLE_OPTION_OFFSET   0x6000
+#define TERMINAL_OPTION_OFFSET  0x5000
+#define CONFIG_OPTION_OFFSET    0x1200
+#define KEY_VALUE_OFFSET        0x1100
+#define FORM_ID_OFFSET          0x1000
+
+//
+// VarOffset that will be used to create question
+// all these values are computed from the structure
+// defined below
+//
+#define VAR_OFFSET(Field)              ((UINT16) ((UINTN) &(((BMM_FAKE_NV_DATA *) 0)->Field)))
+
+//
+// Question Id of Zero is invalid, so add an offset to it
+//
+#define QUESTION_ID(Field)             (VAR_OFFSET (Field) + CONFIG_OPTION_OFFSET)
+
+#define BOOT_TIME_OUT_VAR_OFFSET        VAR_OFFSET (BootTimeOut)
+#define BOOT_NEXT_VAR_OFFSET            VAR_OFFSET (BootNext)
+#define COM1_BAUD_RATE_VAR_OFFSET       VAR_OFFSET (COM1BaudRate)
+#define COM1_DATA_RATE_VAR_OFFSET       VAR_OFFSET (COM1DataRate)
+#define COM1_STOP_BITS_VAR_OFFSET       VAR_OFFSET (COM1StopBits)
+#define COM1_PARITY_VAR_OFFSET          VAR_OFFSET (COM1Parity)
+#define COM1_TERMINAL_VAR_OFFSET        VAR_OFFSET (COM2TerminalType)
+#define COM2_BAUD_RATE_VAR_OFFSET       VAR_OFFSET (COM2BaudRate)
+#define COM2_DATA_RATE_VAR_OFFSET       VAR_OFFSET (COM2DataRate)
+#define COM2_STOP_BITS_VAR_OFFSET       VAR_OFFSET (COM2StopBits)
+#define COM2_PARITY_VAR_OFFSET          VAR_OFFSET (COM2Parity)
+#define COM2_TERMINAL_VAR_OFFSET        VAR_OFFSET (COM2TerminalType)
+#define DRV_ADD_HANDLE_DESC_VAR_OFFSET  VAR_OFFSET (DriverAddHandleDesc)
+#define DRV_ADD_ACTIVE_VAR_OFFSET       VAR_OFFSET (DriverAddActive)
+#define DRV_ADD_RECON_VAR_OFFSET        VAR_OFFSET (DriverAddForceReconnect)
+#define CON_IN_COM1_VAR_OFFSET          VAR_OFFSET (ConsoleInputCOM1)
+#define CON_IN_COM2_VAR_OFFSET          VAR_OFFSET (ConsoleInputCOM2)
+#define CON_OUT_COM1_VAR_OFFSET         VAR_OFFSET (ConsoleOutputCOM1)
+#define CON_OUT_COM2_VAR_OFFSET         VAR_OFFSET (ConsoleOutputCOM2)
+#define CON_ERR_COM1_VAR_OFFSET         VAR_OFFSET (ConsoleErrorCOM1)
+#define CON_ERR_COM2_VAR_OFFSET         VAR_OFFSET (ConsoleErrorCOM2)
+#define CON_MODE_VAR_OFFSET             VAR_OFFSET (ConsoleOutMode)
+#define CON_DEVICE_VAR_OFFSET           VAR_OFFSET (ConsoleCheck)
+#define CON_IN_DEVICE_VAR_OFFSET        VAR_OFFSET (ConsoleInCheck)
+#define CON_OUT_DEVICE_VAR_OFFSET       VAR_OFFSET (ConsoleOutCheck)
+#define CON_ERR_DEVICE_VAR_OFFSET       VAR_OFFSET (ConsoleErrCheck)
+#define BOOT_OPTION_ORDER_VAR_OFFSET    VAR_OFFSET (BootOptionOrder)
+#define DRIVER_OPTION_ORDER_VAR_OFFSET  VAR_OFFSET (DriverOptionOrder)
+#define BOOT_OPTION_DEL_VAR_OFFSET      VAR_OFFSET (BootOptionDel)
+#define DRIVER_OPTION_DEL_VAR_OFFSET    VAR_OFFSET (DriverOptionDel)
+#define DRIVER_ADD_OPTION_VAR_OFFSET    VAR_OFFSET (DriverAddHandleOptionalData)
+#define COM_BAUD_RATE_VAR_OFFSET        VAR_OFFSET (COMBaudRate)
+#define COM_DATA_RATE_VAR_OFFSET        VAR_OFFSET (COMDataRate)
+#define COM_STOP_BITS_VAR_OFFSET        VAR_OFFSET (COMStopBits)
+#define COM_PARITY_VAR_OFFSET           VAR_OFFSET (COMParity)
+#define COM_TERMINAL_VAR_OFFSET         VAR_OFFSET (COMTerminalType)
+#define COM_FLOWCONTROL_VAR_OFFSET      VAR_OFFSET (COMFlowControl)
+
+#define BOOT_TIME_OUT_QUESTION_ID       QUESTION_ID (BootTimeOut)
+#define BOOT_NEXT_QUESTION_ID           QUESTION_ID (BootNext)
+#define COM1_BAUD_RATE_QUESTION_ID      QUESTION_ID (COM1BaudRate)
+#define COM1_DATA_RATE_QUESTION_ID      QUESTION_ID (COM1DataRate)
+#define COM1_STOP_BITS_QUESTION_ID      QUESTION_ID (COM1StopBits)
+#define COM1_PARITY_QUESTION_ID         QUESTION_ID (COM1Parity)
+#define COM1_TERMINAL_QUESTION_ID       QUESTION_ID (COM2TerminalType)
+#define COM2_BAUD_RATE_QUESTION_ID      QUESTION_ID (COM2BaudRate)
+#define COM2_DATA_RATE_QUESTION_ID      QUESTION_ID (COM2DataRate)
+#define COM2_STOP_BITS_QUESTION_ID      QUESTION_ID (COM2StopBits)
+#define COM2_PARITY_QUESTION_ID         QUESTION_ID (COM2Parity)
+#define COM2_TERMINAL_QUESTION_ID       QUESTION_ID (COM2TerminalType)
+#define DRV_ADD_HANDLE_DESC_QUESTION_ID QUESTION_ID (DriverAddHandleDesc)
+#define DRV_ADD_ACTIVE_QUESTION_ID      QUESTION_ID (DriverAddActive)
+#define DRV_ADD_RECON_QUESTION_ID       QUESTION_ID (DriverAddForceReconnect)
+#define CON_IN_COM1_QUESTION_ID         QUESTION_ID (ConsoleInputCOM1)
+#define CON_IN_COM2_QUESTION_ID         QUESTION_ID (ConsoleInputCOM2)
+#define CON_OUT_COM1_QUESTION_ID        QUESTION_ID (ConsoleOutputCOM1)
+#define CON_OUT_COM2_QUESTION_ID        QUESTION_ID (ConsoleOutputCOM2)
+#define CON_ERR_COM1_QUESTION_ID        QUESTION_ID (ConsoleErrorCOM1)
+#define CON_ERR_COM2_QUESTION_ID        QUESTION_ID (ConsoleErrorCOM2)
+#define CON_MODE_QUESTION_ID            QUESTION_ID (ConsoleOutMode)
+#define CON_DEVICE_QUESTION_ID          QUESTION_ID (ConsoleCheck)
+#define CON_IN_DEVICE_QUESTION_ID       QUESTION_ID (ConsoleInCheck)
+#define CON_OUT_DEVICE_QUESTION_ID      QUESTION_ID (ConsoleOutCheck)
+#define CON_ERR_DEVICE_QUESTION_ID      QUESTION_ID (ConsoleErrCheck)
+#define BOOT_OPTION_ORDER_QUESTION_ID   QUESTION_ID (BootOptionOrder)
+#define DRIVER_OPTION_ORDER_QUESTION_ID QUESTION_ID (DriverOptionOrder)
+#define BOOT_OPTION_DEL_QUESTION_ID     QUESTION_ID (BootOptionDel)
+#define DRIVER_OPTION_DEL_QUESTION_ID   QUESTION_ID (DriverOptionDel)
+#define DRIVER_ADD_OPTION_QUESTION_ID   QUESTION_ID (DriverAddHandleOptionalData)
+#define COM_BAUD_RATE_QUESTION_ID       QUESTION_ID (COMBaudRate)
+#define COM_DATA_RATE_QUESTION_ID       QUESTION_ID (COMDataRate)
+#define COM_STOP_BITS_QUESTION_ID       QUESTION_ID (COMStopBits)
+#define COM_PARITY_QUESTION_ID          QUESTION_ID (COMParity)
+#define COM_TERMINAL_QUESTION_ID        QUESTION_ID (COMTerminalType)
+#define COM_FLOWCONTROL_QUESTION_ID     QUESTION_ID (COMFlowControl)
+
+#define STRING_DEPOSITORY_NUMBER        8
+
+///
+/// Serial Ports attributes, first one is the value for
+/// return from callback function, stringtoken is used to
+/// display the value properly
+///
+typedef struct {
+  UINTN   Value;
+  UINT16  StringToken;
+} COM_ATTR;
+
+typedef struct {
+  UINT64                    BaudRate;
+  UINT8                     DataBits;
+  UINT8                     Parity;
+  UINT8                     StopBits;
+
+  UINT8                     BaudRateIndex;
+  UINT8                     DataBitsIndex;
+  UINT8                     ParityIndex;
+  UINT8                     StopBitsIndex;
+
+  UINT8                     FlowControl;
+
+  UINT8                     IsConIn;
+  UINT8                     IsConOut;
+  UINT8                     IsStdErr;
+  UINT8                     TerminalType;
+
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+} BM_TERMINAL_CONTEXT;
+
+typedef struct {
+  BOOLEAN                   IsBootNext;
+  BOOLEAN                   LoadOptionModified;
+  BOOLEAN                   Deleted;
+
+  BOOLEAN                   IsLegacy;
+  BOOLEAN                   IsActive;
+  BOOLEAN                   ForceReconnect;
+  UINTN                     OptionalDataSize;
+
+  UINTN                     LoadOptionSize;
+  UINT8                     *LoadOption;
+
+  UINT32                    Attributes;
+  UINT16                    FilePathListLength;
+  UINT16                    *Description;
+  EFI_DEVICE_PATH_PROTOCOL  *FilePathList;
+  UINT8                     *OptionalData;
+
+  UINT16                    BbsIndex;
+} BM_LOAD_CONTEXT;
+
+typedef struct {
+
+  BOOLEAN                   IsActive;
+
+  BOOLEAN                   IsTerminal;
+
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+} BM_CONSOLE_CONTEXT;
+
+typedef struct {
+  UINTN   Column;
+  UINTN   Row;
+} CONSOLE_OUT_MODE;
+
+typedef struct {
+  EFI_HANDLE                        Handle;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  EFI_FILE_HANDLE                   FHandle;
+  UINT16                            *FileName;
+  EFI_FILE_SYSTEM_VOLUME_LABEL      *Info;
+
+  BOOLEAN                           IsRoot;
+  BOOLEAN                           IsDir;
+  BOOLEAN                           IsRemovableMedia;
+  BOOLEAN                           IsLoadFile;
+  BOOLEAN                           IsBootLegacy;
+} BM_FILE_CONTEXT;
+
+typedef struct {
+  EFI_HANDLE                Handle;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+} BM_HANDLE_CONTEXT;
+
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Head;
+  UINTN           MenuNumber;
+} BM_MENU_OPTION;
+
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Link;
+  UINTN           OptionNumber;
+  UINT16          *DisplayString;
+  UINT16          *HelpString;
+  EFI_STRING_ID   DisplayStringToken;
+  EFI_STRING_ID   HelpStringToken;
+  UINTN           ContextSelection;
+  VOID            *VariableContext;
+} BM_MENU_ENTRY;
+
+typedef struct {
+  //
+  // Shared callback data.
+  //
+  UINTN                          Signature;
+
+  BM_MENU_ENTRY                  *MenuEntry;
+  BM_HANDLE_CONTEXT              *HandleContext;
+  BM_FILE_CONTEXT                *FileContext;
+  BM_LOAD_CONTEXT                *LoadContext;
+  BM_TERMINAL_CONTEXT            *TerminalContext;
+  UINTN                          CurrentTerminal;
+  BBS_TYPE                       BbsType;
+
+  //
+  // BMM main formset callback data.
+  //
+  EFI_HII_HANDLE                 BmmHiiHandle;
+  EFI_HANDLE                     BmmDriverHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL BmmConfigAccess;
+  EFI_FORM_ID                    BmmCurrentPageId;
+  EFI_FORM_ID                    BmmPreviousPageId;
+  BOOLEAN                        BmmAskSaveOrNot;
+  BMM_FAKE_NV_DATA               BmmFakeNvData;
+  BMM_FAKE_NV_DATA               BmmOldFakeNVData;
+
+  //
+  // File explorer formset callback data.
+  //
+  EFI_HII_HANDLE                 FeHiiHandle;
+  EFI_HANDLE                     FeDriverHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL FeConfigAccess;
+  FILE_EXPLORER_STATE            FeCurrentState;
+  FILE_EXPLORER_DISPLAY_CONTEXT  FeDisplayContext;
+  FILE_EXPLORER_NV_DATA          FeFakeNvData;
+} BMM_CALLBACK_DATA;
+
+typedef struct _STRING_LIST_NODE STRING_LIST_NODE;
+
+struct _STRING_LIST_NODE {
+  EFI_STRING_ID             StringToken;
+  STRING_LIST_NODE          *Next;
+};
+
+typedef struct _STRING_DEPOSITORY {
+  UINTN             TotalNodeNumber;
+  STRING_LIST_NODE  *CurrentNode;
+  STRING_LIST_NODE  *ListHead;
+} STRING_DEPOSITORY;
+
+//
+// #pragma pack()
+//
+// For initializing File System menu
+//
+
+/**
+  This function build the FsOptionMenu list which records all
+  available file system in the system. They includes all instances
+  of EFI_SIMPLE_FILE_SYSTEM_PROTOCOL, all instances of EFI_LOAD_FILE_SYSTEM
+  and all type of legacy boot device.
+
+  @param CallbackData    BMM context data
+
+  @retval  EFI_SUCCESS             Success find the file system
+  @retval  EFI_OUT_OF_RESOURCES    Can not create menu entry
+
+**/
+EFI_STATUS
+BOpt_FindFileSystem (
+  IN BMM_CALLBACK_DATA          *CallbackData
+  );
+
+/**
+  Find files under current directory
+  All files and sub-directories in current directory
+  will be stored in DirectoryMenu for future use.
+
+  @param CallbackData  The BMM context data.
+  @param MenuEntry     The Menu Entry.
+
+  @retval EFI_SUCCESS         Get files from current dir successfully.
+  @return Other value if can't get files from current dir.
+
+**/
+EFI_STATUS
+BOpt_FindFiles (
+  IN BMM_CALLBACK_DATA          *CallbackData,
+  IN BM_MENU_ENTRY              *MenuEntry
+  );
+
+/**
+
+  Find drivers that will be added as Driver#### variables from handles
+  in current system environment
+  All valid handles in the system except those consume SimpleFs, LoadFile
+  are stored in DriverMenu for future use.
+
+  @retval EFI_SUCCESS The function complets successfully.
+  @return Other value if failed to build the DriverMenu.
+
+**/
+EFI_STATUS
+BOpt_FindDrivers (
+  VOID
+  );
+
+/**
+
+  Build the BootOptionMenu according to BootOrder Variable.
+  This Routine will access the Boot#### to get EFI_LOAD_OPTION.
+
+  @param CallbackData The BMM context data.
+
+  @return The number of the Var Boot####.
+
+**/
+EFI_STATUS
+BOpt_GetBootOptions (
+  IN  BMM_CALLBACK_DATA         *CallbackData
+  );
+
+/**
+
+  Build up all DriverOptionMenu
+
+  @param CallbackData The BMM context data.
+
+  @return EFI_SUCESS The functin completes successfully.
+  @retval EFI_OUT_OF_RESOURCES Not enough memory to compete the operation.
+  
+
+**/
+EFI_STATUS
+BOpt_GetDriverOptions (
+  IN  BMM_CALLBACK_DATA         *CallbackData
+  );
+
+/**
+  Free resources allocated in Allocate Rountine.
+
+  @param FreeMenu        Menu to be freed
+
+**/
+VOID
+BOpt_FreeMenu (
+  BM_MENU_OPTION        *FreeMenu
+  );
+
+
+/**
+
+  Append file name to existing file name.
+
+  @param Str1  The existing file name
+  @param Str2  The file name to be appended
+
+  @return Allocate a new string to hold the appended result.
+          Caller is responsible to free the returned string.
+
+**/
+CHAR16*
+BOpt_AppendFileName (
+  IN  CHAR16  *Str1,
+  IN  CHAR16  *Str2
+  );
+
+/**
+
+  Check whether current FileName point to a valid
+  Efi Image File.
+
+  @param FileName  File need to be checked.
+
+  @retval TRUE  Is Efi Image
+  @retval FALSE Not a valid Efi Image
+
+**/
+BOOLEAN
+BOpt_IsEfiImageName (
+  IN UINT16  *FileName
+  );
+
+/**
+
+  Check whether current FileName point to a valid Efi Application
+
+  @param Dir       Pointer to current Directory
+  @param FileName  Pointer to current File name.
+
+  @retval TRUE      Is a valid Efi Application
+  @retval FALSE     not a valid Efi Application
+
+**/
+BOOLEAN
+BOpt_IsEfiApp (
+  IN EFI_FILE_HANDLE Dir,
+  IN UINT16          *FileName
+  );
+
+/**
+
+  Get the Option Number that has not been allocated for use.
+
+  @param Type  The type of Option.
+
+  @return The available Option Number.
+
+**/
+UINT16
+BOpt_GetOptionNumber (
+  CHAR16        *Type
+  );
+
+/**
+
+  Get the Option Number for Boot#### that does not used.
+
+  @return The available Option Number.
+
+**/
+UINT16
+BOpt_GetBootOptionNumber (
+  VOID
+  );
+
+/**
+
+Get the Option Number for Driver#### that does not used.
+
+@return The unused Option Number.
+
+**/
+UINT16
+BOpt_GetDriverOptionNumber (
+  VOID
+  );
+
+/**
+  Create a menu entry give a Menu type.
+
+  @param MenuType        The Menu type to be created.
+
+
+  @retval NULL           If failed to create the menu.
+  @return                The menu.
+
+**/
+BM_MENU_ENTRY                     *
+BOpt_CreateMenuEntry (
+  UINTN           MenuType
+  );
+
+/**
+  Free up all resource allocated for a BM_MENU_ENTRY.
+
+  @param MenuEntry   A pointer to BM_MENU_ENTRY.
+
+**/
+VOID
+BOpt_DestroyMenuEntry (
+  BM_MENU_ENTRY         *MenuEntry
+  );
+
+/**
+  Get the Menu Entry from the list in Menu Entry List.
+
+  If MenuNumber is great or equal to the number of Menu
+  Entry in the list, then ASSERT.
+
+  @param MenuOption      The Menu Entry List to read the menu entry.
+  @param MenuNumber      The index of Menu Entry.
+
+  @return The Menu Entry.
+
+**/
+BM_MENU_ENTRY                     *
+BOpt_GetMenuEntry (
+  BM_MENU_OPTION      *MenuOption,
+  UINTN               MenuNumber
+  );
+
+/**
+  Get option number according to Boot#### and BootOrder variable. 
+  The value is saved as #### + 1.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID  
+GetBootOrder (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+/**
+  Get driver option order from globalc DriverOptionMenu.
+
+  @param CallbackData    The BMM context data.
+  
+**/
+VOID  
+GetDriverOrder (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+//
+// Locate all serial io devices for console
+//
+/**
+  Build a list containing all serial devices.
+
+  @retval EFI_SUCCESS The function complete successfully.
+  @retval EFI_UNSUPPORTED No serial ports present.
+
+**/
+EFI_STATUS
+LocateSerialIo (
+  VOID
+  );
+
+//
+// Initializing Console menu
+//
+/**
+  Build up ConsoleOutMenu, ConsoleInpMenu and ConsoleErrMenu
+
+  @retval EFI_SUCCESS    The function always complete successfully.
+
+**/
+EFI_STATUS
+GetAllConsoles(
+  VOID
+  );
+
+//
+// Get current mode information
+//
+/**
+  Get mode number according to column and row
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+GetConsoleOutMode (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+//
+// Cleaning up console menu
+//
+/**
+  Free ConsoleOutMenu, ConsoleInpMenu and ConsoleErrMenu
+
+  @retval EFI_SUCCESS    The function always complete successfully.
+**/
+EFI_STATUS
+FreeAllConsoles (
+  VOID
+  );
+
+/**
+  Update the device path that describing a terminal device
+  based on the new BaudRate, Data Bits, parity and Stop Bits
+  set.
+
+  @param DevicePath     The devicepath protocol instance wanted to be updated.
+
+**/
+VOID
+ChangeVariableDevicePath (
+  IN OUT EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  );
+
+/**
+  Update the multi-instance device path of Terminal Device based on
+  the global TerminalMenu. If ChangeTernimal is TRUE, the terminal 
+  device path in the Terminal Device in TerminalMenu is also updated.
+
+  @param DevicePath      The multi-instance device path.
+  @param ChangeTerminal  TRUE, then device path in the Terminal Device 
+                         in TerminalMenu is also updated; FALSE, no update.
+
+  @return EFI_SUCCESS    The function completes successfully.
+
+**/
+EFI_STATUS
+ChangeTerminalDevicePath (
+  IN OUT EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN BOOLEAN                   ChangeTerminal
+  );
+
+//
+// Variable operation by menu selection
+//
+/**
+  This function create a currently loaded Boot Option from 
+  the BMM. It then appends this Boot Option to the end of 
+  the "BootOrder" list. It also append this Boot Opotion to the end
+  of BootOptionMenu.
+
+  @param CallbackData           The BMM context data.
+  @param NvRamMap               The file explorer formset internal state.
+
+  @retval EFI_OUT_OF_RESOURCES  If not enought memory to complete the operation.
+  @retval EFI_SUCCESS           If function completes successfully.
+
+**/
+EFI_STATUS
+Var_UpdateBootOption (
+  IN  BMM_CALLBACK_DATA                   *CallbackData,
+  IN  FILE_EXPLORER_NV_DATA               *NvRamMap
+  );
+
+/**
+  Delete Boot Option that represent a Deleted state in BootOptionMenu.
+  After deleting this boot option, call Var_ChangeBootOrder to
+  make sure BootOrder is in valid state.
+
+  @retval EFI_SUCCESS   If all boot load option EFI Variables corresponding to  
+                        BM_LOAD_CONTEXT marked for deletion is deleted
+  @return Others        If failed to update the "BootOrder" variable after deletion. 
+
+**/
+EFI_STATUS
+Var_DelBootOption (
+  VOID
+  );
+
+/**
+  After any operation on Boot####, there will be a discrepancy in BootOrder.
+  Since some are missing but in BootOrder, while some are present but are
+  not reflected by BootOrder. Then a function rebuild BootOrder from
+  scratch by content from BootOptionMenu is needed.
+
+  @retval  EFI_SUCCESS  The boot order is updated successfully.
+  @return  other than EFI_SUCCESS if failed to change the "BootOrder" EFI Variable.
+
+**/
+EFI_STATUS
+Var_ChangeBootOrder (
+  VOID
+  );
+
+/**
+  This function create a currently loaded Drive Option from 
+  the BMM. It then appends this Driver Option to the end of 
+  the "DriverOrder" list. It append this Driver Opotion to the end
+  of DriverOptionMenu.
+
+  @param CallbackData    The BMM context data.
+  @param HiiHandle       The HII handle associated with the BMM formset.
+  @param DescriptionData The description of this driver option.
+  @param OptionalData    The optional load option.
+  @param ForceReconnect  If to force reconnect.
+
+  @retval EFI_OUT_OF_RESOURCES If not enought memory to complete the operation.
+  @retval EFI_SUCCESS          If function completes successfully.
+
+**/
+EFI_STATUS
+Var_UpdateDriverOption (
+  IN  BMM_CALLBACK_DATA         *CallbackData,
+  IN  EFI_HII_HANDLE            HiiHandle,
+  IN  UINT16                    *DescriptionData,
+  IN  UINT16                    *OptionalData,
+  IN  UINT8                     ForceReconnect
+  );
+
+/**
+  Delete Load Option that represent a Deleted state in BootOptionMenu.
+  After deleting this Driver option, call Var_ChangeDriverOrder to
+  make sure DriverOrder is in valid state.
+
+  @retval EFI_SUCCESS Load Option is successfully updated.
+  @return Other value than EFI_SUCCESS if failed to update "Driver Order" EFI
+          Variable.
+
+**/
+EFI_STATUS
+Var_DelDriverOption (
+  VOID
+  );
+
+/**
+  After any operation on Driver####, there will be a discrepancy in
+  DriverOrder. Since some are missing but in DriverOrder, while some
+  are present but are not reflected by DriverOrder. Then a function
+  rebuild DriverOrder from scratch by content from DriverOptionMenu is
+  needed.
+
+  @retval  EFI_SUCCESS  The driver order is updated successfully.
+  @return  other than EFI_SUCCESS if failed to set the "DriverOrder" EFI Variable.
+
+**/
+EFI_STATUS
+Var_ChangeDriverOrder (
+  VOID
+  );
+
+/**
+  This function delete and build multi-instance device path ConIn
+  console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+**/
+EFI_STATUS
+Var_UpdateConsoleInpOption (
+  VOID
+  );
+
+/**
+  This function delete and build multi-instance device path ConOut console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+**/
+EFI_STATUS
+Var_UpdateConsoleOutOption (
+  VOID
+  );
+
+/**
+  This function delete and build multi-instance device path ErrOut console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+**/
+EFI_STATUS
+Var_UpdateErrorOutOption (
+  VOID
+  );
+
+/**
+  This function delete and build Out of Band console device.
+  
+  @param   MenuIndex   Menu index which user select in the terminal menu list.
+  
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.  
+**/
+EFI_STATUS
+Var_UpdateOutOfBandOption (
+  IN  UINT16           MenuIndex
+  );
+
+/**
+  Update the device path of "ConOut", "ConIn" and "ErrOut" based on the new BaudRate, Data Bits, 
+  parity and stop Bits set.
+
+**/
+VOID
+Var_UpdateAllConsoleOption (
+  VOID
+  );
+
+/**
+  This function update the "BootNext" EFI Variable. If there is no "BootNex" specified in BMM, 
+  this EFI Variable is deleted.
+  It also update the BMM context data specified the "BootNext" value.
+
+  @param CallbackData    The BMM context data.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateBootNext (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  );
+
+/**
+  This function update the "BootOrder" EFI Variable based on BMM Formset's NV map. It then refresh 
+  BootOptionMenu with the new "BootOrder" list.
+
+  @param CallbackData           The BMM context data.
+
+  @retval EFI_SUCCESS           The function complete successfully.
+  @retval EFI_OUT_OF_RESOURCES  Not enough memory to complete the function.
+  @return not The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateBootOrder (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  );
+
+/**
+  This function update the "DriverOrder" EFI Variable based on
+  BMM Formset's NV map. It then refresh DriverOptionMenu
+  with the new "DriverOrder" list.
+
+  @param CallbackData    The BMM context data.
+
+  @retval EFI_SUCCESS           The function complete successfully.
+  @retval EFI_OUT_OF_RESOURCES  Not enough memory to complete the function.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateDriverOrder (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  );
+
+/**
+  Update the Text Mode of Console.
+
+  @param CallbackData  The context data for BMM.
+
+  @retval EFI_SUCCSS If the Text Mode of Console is updated.
+  @return Other value if the Text Mode of Console is not updated.
+
+**/
+EFI_STATUS
+Var_UpdateConMode (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  );
+
+//
+// Following are page create and refresh functions
+//
+/**
+  Refresh the global UpdateData structure.
+
+**/
+VOID
+RefreshUpdateData (
+  VOID
+  );
+
+/**
+  Clean up the dynamic opcode at label and form specified by
+  both LabelId. 
+
+  @param LabelId         It is both the Form ID and Label ID for
+                         opcode deletion.
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+CleanUpPage (
+  IN UINT16                           LabelId,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create a lit of boot option from global BootOptionMenu. It
+  allow user to delete the boot option.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateBootDelPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create a lit of driver option from global DriverMenu.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateDrvAddHandlePage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create a lit of driver option from global DriverOptionMenu. It
+  allow user to delete the driver option.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateDrvDelPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Prepare the page to allow user to add description for a Driver Option.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateDriverAddHandleDescPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Dispatch the correct update page function to call based on the UpdatePageId.
+
+  @param UpdatePageId    The form ID.
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdatePageBody (
+  IN UINT16                           UpdatePageId,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create the dynamic page to allow user to set the "BootNext" vaule.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateBootNextPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create the dynamic page to allow user to set the "TimeOut" vaule.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateTimeOutPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create the dynamic page which allows user to set the property such as Baud Rate, Data Bits,
+  Parity, Stop Bits, Terminal Type.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateTerminalPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Refresh the text mode page
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateConModePage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Create a list of Goto Opcode for all terminal devices logged
+  by TerminaMenu. This list will be inserted to form FORM_CON_COM_SETUP_ID.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateConCOMPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  );
+
+/**
+  Function opens and returns a file handle to the root directory of a volume.
+
+  @param DeviceHandle    A handle for a device
+  @return A valid file handle or NULL is returned
+**/
+EFI_FILE_HANDLE
+EfiLibOpenRoot (
+  IN EFI_HANDLE       DeviceHandle
+  );
+
+/**
+  Function gets the file system information from an open file descriptor,
+  and stores it in a buffer allocated from pool.
+
+  @param FHand           The file handle.
+
+  @return                A pointer to a buffer with file information.
+                         NULL is returned if failed to get Vaolume Label Info.
+**/
+EFI_FILE_SYSTEM_VOLUME_LABEL *
+EfiLibFileSystemVolumeLabelInfo (
+  IN EFI_FILE_HANDLE      FHand
+  );
+
+/**
+
+  Function gets the file information from an open file descriptor, and stores it
+  in a buffer allocated from pool.
+
+  @param FHand           File Handle.
+
+  @return                A pointer to a buffer with file information or NULL is returned
+
+**/
+EFI_FILE_INFO *
+EfiLibFileInfo (
+  IN EFI_FILE_HANDLE      FHand
+  );
+
+/**
+  Find the first instance of this Protocol in the system and return it's interface.
+
+  @param ProtocolGuid    Provides the protocol to search for
+  @param Interface       On return, a pointer to the first interface
+                         that matches ProtocolGuid
+
+  @retval  EFI_SUCCESS      A protocol instance matching ProtocolGuid was found
+  @retval  EFI_NOT_FOUND    No protocol instances were found that match ProtocolGuid
+
+**/
+EFI_STATUS
+EfiLibLocateProtocol (
+  IN  EFI_GUID        *ProtocolGuid,
+  OUT VOID            **Interface
+  );
+
+/**
+  Adjusts the size of a previously allocated buffer.
+
+  @param OldPool         A pointer to the buffer whose size is being adjusted.
+  @param OldSize         The size of the current buffer.
+  @param NewSize         The size of the new buffer.
+
+  @return   The newly allocated buffer. if NULL, allocation failed.
+
+**/
+VOID*
+EfiReallocatePool (
+  IN VOID                 *OldPool,
+  IN UINTN                OldSize,
+  IN UINTN                NewSize
+  );
+
+/**
+  Function deletes the variable specified by VarName and VarGuid.
+
+
+  @param VarName            A Null-terminated Unicode string that is
+                            the name of the vendor's variable.
+                         
+  @param VarGuid            A unique identifier for the vendor.
+
+  @retval  EFI_SUCCESS           The variable was found and removed
+  @retval  EFI_UNSUPPORTED       The variable store was inaccessible
+  @retval  EFI_OUT_OF_RESOURCES  The temporary buffer was not available
+  @retval  EFI_NOT_FOUND         The variable was not found
+
+**/
+EFI_STATUS
+EfiLibDeleteVariable (
+  IN CHAR16   *VarName,
+  IN EFI_GUID *VarGuid
+  );
+
+/**
+  Duplicate a string.
+
+  @param Src             The source.
+
+  @return A new string which is duplicated copy of the source.
+  @retval NULL If there is not enough memory.
+
+**/
+CHAR16                            *
+EfiStrDuplicate (
+  IN CHAR16   *Src
+  );
+
+/**
+  Function is used to determine the number of device path instances
+  that exist in a device path.
+
+
+  @param DevicePath      A pointer to a device path data structure.
+
+  @return This function counts and returns the number of device path instances
+          in DevicePath.
+
+**/
+UINTN
+EfiDevicePathInstanceCount (
+  IN EFI_DEVICE_PATH_PROTOCOL      *DevicePath
+  );
+
+/**
+  Create string tokens for a menu from its help strings and display strings
+
+
+  @param CallbackData    The BMM context data.
+  @param HiiHandle       Hii Handle of the package to be updated.
+  @param MenuOption      The Menu whose string tokens need to be created
+
+  @retval  EFI_SUCCESS      string tokens created successfully
+  @retval  others           contain some errors
+
+**/
+EFI_STATUS
+CreateMenuStringToken (
+  IN BMM_CALLBACK_DATA                *CallbackData,
+  IN EFI_HII_HANDLE                   HiiHandle,
+  IN BM_MENU_OPTION                   *MenuOption
+  );
+
+/**
+  Get a string from the Data Hub record based on 
+  a device path.
+
+  @param DevPath         The device Path.
+
+  @return A string located from the Data Hub records based on
+          the device path.
+  @retval NULL  If failed to get the String from Data Hub.
+
+**/
+UINT16 *
+EfiLibStrFromDatahub (
+  IN EFI_DEVICE_PATH_PROTOCOL                 *DevPath
+  );
+
+/**
+  Get the index number (#### in Boot####) for the boot option pointed to a BBS legacy device type
+  specified by DeviceType.
+
+  @param DeviceType      The legacy device type. It can be floppy, network, harddisk, cdrom,
+                         etc.
+  @param OptionIndex     Returns the index number (#### in Boot####).
+  @param OptionSize      Return the size of the Boot### variable.
+
+**/
+VOID *
+GetLegacyBootOptionVar (
+  IN  UINTN                            DeviceType,
+  OUT UINTN                            *OptionIndex,
+  OUT UINTN                            *OptionSize
+  );
+
+/**
+  Initialize the Boot Maintenance Utitliy.
+
+**/
+VOID
+InitializeBM (
+  VOID
+  );
+
+
+/**
+  Intialize all the string depositories.
+
+**/
+VOID
+InitializeStringDepository (
+  VOID
+  );
+
+/**
+  Fetch a usable string node from the string depository and return the string token.
+
+
+  @param CallbackData       The BMM context data.
+  @param StringDepository   Pointer of the string depository.
+
+  @retval  EFI_STRING_ID    String token.
+
+**/
+EFI_STRING_ID
+GetStringTokenFromDepository (
+  IN   BMM_CALLBACK_DATA     *CallbackData,
+  IN   STRING_DEPOSITORY     *StringDepository
+  );
+
+/**
+  Reclaim string depositories by moving the current node pointer to list head..
+**/
+VOID
+ReclaimStringDepository (
+  VOID
+  );
+
+/**
+  Release resource for all the string depositories.
+
+**/
+VOID
+CleanUpStringDepository (
+  VOID
+  );
+
+/**
+  Discard all changes done to the BMM pages such as Boot Order change,
+  Driver order change.
+
+  @param Private         The BMM context data.
+  @param CurrentFakeNVMap The current Fack NV Map.
+
+**/
+VOID
+DiscardChangeHandler (
+  IN  BMM_CALLBACK_DATA               *Private,
+  IN  BMM_FAKE_NV_DATA                *CurrentFakeNVMap
+  );
+
+/**
+  Dispatch the display to the next page based on NewPageId.
+
+  @param Private         The BMM context data.
+  @param NewPageId       The original page ID.
+
+**/
+VOID
+UpdatePageId (
+  BMM_CALLBACK_DATA              *Private,
+  UINT16                         NewPageId
+  );
+
+/**
+  Boot a file selected by user at File Expoloer of BMM.
+
+  @param FileContext     The file context data, which contains the device path
+                         of the file to be boot from.
+
+  @retval EFI_SUCCESS    The function completed successfull.
+  @return                 Other value if the boot from the file fails.
+
+**/
+EFI_STATUS
+BootThisFile (
+  IN BM_FILE_CONTEXT                   *FileContext
+  );
+
+/**
+  Update the file explower page with the refershed file system.
+
+
+  @param CallbackData    BMM context data
+  @param KeyValue        Key value to identify the type of data to expect.
+
+  @retval  TRUE           Inform the caller to create a callback packet to exit file explorer.
+  @retval  FALSE          Indicate that there is no need to exit file explorer.
+
+**/
+BOOLEAN
+UpdateFileExplorer (
+  IN BMM_CALLBACK_DATA            *CallbackData,
+  IN UINT16                       KeyValue
+  );
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This                Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration       A null-terminated Unicode string in
+                                  <ConfigString> format.   
+  @param[out] Progress            A pointer to a string filled in with the
+                                  offset of the most recent '&' before the
+                                  first failing name / value pair (or the
+                                  beginn ing of the string if the failure
+                                  is in the first name / value pair) or
+                                  the terminating NULL if all was
+                                  successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are
+                                  awaiting distribution.  
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETERS  Passing in a NULL for the
+                                  Results parameter would result
+                                  in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data
+                                  was not found.
+**/
+EFI_STATUS
+EFIAPI
+FileExplorerRouteConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL *This,
+  IN CONST EFI_STRING                     Configuration,
+  OUT EFI_STRING                          *Progress
+  );
+
+/**
+  This function processes the results of changes in configuration.
+  When user select a interactive opcode, this callback will be triggered.
+  Based on the Question(QuestionId) that triggers the callback, the corresponding
+  actions is performed. It handles:
+
+  1) the addition of boot option.
+  2) the addition of driver option.
+  3) exit from file browser
+  4) update of file content if a dir is selected.
+  5) boot the file if a file is selected in "boot from file"
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the variable and its data.
+  @retval  EFI_DEVICE_ERROR      The variable could not be saved.
+  @retval  EFI_UNSUPPORTED       The specified Action is not supported by the callback.
+
+**/
+EFI_STATUS
+EFIAPI
+FileExplorerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  );
+
+/**
+  Remove the installed BootMaint and FileExplorer HiiPackages.
+
+**/
+VOID
+FreeBMPackage(
+  VOID
+  );
+
+/**
+  Install BootMaint and FileExplorer HiiPackages.
+
+**/
+VOID
+InitBootMaintenance(
+  VOID
+  );
+
+/**
+
+  Initialize console input device check box to ConsoleInCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/  
+VOID  
+GetConsoleInCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+/**
+
+  Initialize console output device check box to ConsoleOutCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/      
+VOID    
+GetConsoleOutCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+/**
+
+  Initialize standard error output device check box to ConsoleErrCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/        
+VOID  
+GetConsoleErrCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+/**
+
+  Initialize terminal attributes (baudrate, data rate, stop bits, parity and terminal type)
+  to BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/        
+VOID  
+GetTerminalAttribute (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  );
+
+//
+// Global variable in this program (defined in data.c)
+//
+extern BM_MENU_OPTION             BootOptionMenu;
+extern BM_MENU_OPTION             DriverOptionMenu;
+extern BM_MENU_OPTION             FsOptionMenu;
+extern BM_MENU_OPTION             ConsoleInpMenu;
+extern BM_MENU_OPTION             ConsoleOutMenu;
+extern BM_MENU_OPTION             ConsoleErrMenu;
+extern BM_MENU_OPTION             DirectoryMenu;
+extern BM_MENU_OPTION             DriverMenu;
+extern BM_MENU_OPTION             TerminalMenu;
+extern UINT16                     TerminalType[5];
+extern COM_ATTR                   BaudRateList[19];
+extern COM_ATTR                   DataBitsList[4];
+extern COM_ATTR                   ParityList[5];
+extern COM_ATTR                   StopBitsList[3];
+extern EFI_GUID                   TerminalTypeGuid[5];
+extern STRING_DEPOSITORY          *FileOptionStrDepository;
+extern STRING_DEPOSITORY          *ConsoleOptionStrDepository;
+extern STRING_DEPOSITORY          *BootOptionStrDepository;
+extern STRING_DEPOSITORY          *BootOptionHelpStrDepository;
+extern STRING_DEPOSITORY          *DriverOptionStrDepository;
+extern STRING_DEPOSITORY          *DriverOptionHelpStrDepository;
+extern STRING_DEPOSITORY          *TerminalStrDepository;
+extern EFI_DEVICE_PATH_PROTOCOL   EndDevicePath[];
+extern UINT16                     mFlowControlType[2];
+extern UINT32                     mFlowControlValue[2];
+
+//
+// Shared IFR form update data
+//
+extern VOID                        *mStartOpCodeHandle;
+extern VOID                        *mEndOpCodeHandle;
+extern EFI_IFR_GUID_LABEL          *mStartLabel;
+extern EFI_IFR_GUID_LABEL          *mEndLabel;
+
+#endif
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/BootOption.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootOption.c
new file mode 100644
index 0000000000..5213e3cc98
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/BootOption.c
@@ -0,0 +1,1467 @@
+/** @file
+  Provide boot option support for Application "BootMaint"
+
+  Include file system navigation, system handle selection
+
+  Boot option manipulation
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+///
+/// Define the maximum characters that will be accepted.
+///
+#define MAX_CHAR            480
+
+/**
+  Create a menu entry by given menu type.
+
+  @param MenuType        The Menu type to be created.
+
+  @retval NULL           If failed to create the menu.
+  @return the new menu entry.
+
+**/
+BM_MENU_ENTRY *
+BOpt_CreateMenuEntry (
+  UINTN           MenuType
+  )
+{
+  BM_MENU_ENTRY *MenuEntry;
+  UINTN         ContextSize;
+
+  //
+  // Get context size according to menu type
+  //
+  switch (MenuType) {
+  case BM_LOAD_CONTEXT_SELECT:
+    ContextSize = sizeof (BM_LOAD_CONTEXT);
+    break;
+
+  case BM_FILE_CONTEXT_SELECT:
+    ContextSize = sizeof (BM_FILE_CONTEXT);
+    break;
+
+  case BM_CONSOLE_CONTEXT_SELECT:
+    ContextSize = sizeof (BM_CONSOLE_CONTEXT);
+    break;
+
+  case BM_TERMINAL_CONTEXT_SELECT:
+    ContextSize = sizeof (BM_TERMINAL_CONTEXT);
+    break;
+
+  case BM_HANDLE_CONTEXT_SELECT:
+    ContextSize = sizeof (BM_HANDLE_CONTEXT);
+    break;
+
+  default:
+    ContextSize = 0;
+    break;
+  }
+
+  if (ContextSize == 0) {
+    return NULL;
+  }
+
+  //
+  // Create new menu entry
+  //
+  MenuEntry = AllocateZeroPool (sizeof (BM_MENU_ENTRY));
+  if (MenuEntry == NULL) {
+    return NULL;
+  }
+
+  MenuEntry->VariableContext = AllocateZeroPool (ContextSize);
+  if (MenuEntry->VariableContext == NULL) {
+    FreePool (MenuEntry);
+    return NULL;
+  }
+
+  MenuEntry->Signature        = BM_MENU_ENTRY_SIGNATURE;
+  MenuEntry->ContextSelection = MenuType;
+  return MenuEntry;
+}
+
+/**
+  Free up all resource allocated for a BM_MENU_ENTRY.
+
+  @param MenuEntry   A pointer to BM_MENU_ENTRY.
+
+**/
+VOID
+BOpt_DestroyMenuEntry (
+  BM_MENU_ENTRY         *MenuEntry
+  )
+{
+  BM_LOAD_CONTEXT           *LoadContext;
+  BM_FILE_CONTEXT           *FileContext;
+  BM_CONSOLE_CONTEXT        *ConsoleContext;
+  BM_TERMINAL_CONTEXT       *TerminalContext;
+  BM_HANDLE_CONTEXT         *HandleContext;
+
+  //
+  //  Select by the type in Menu entry for current context type
+  //
+  switch (MenuEntry->ContextSelection) {
+  case BM_LOAD_CONTEXT_SELECT:
+    LoadContext = (BM_LOAD_CONTEXT *) MenuEntry->VariableContext;
+    FreePool (LoadContext->FilePathList);
+    FreePool (LoadContext->LoadOption);
+    if (LoadContext->OptionalData != NULL) {
+      FreePool (LoadContext->OptionalData);
+    }
+    FreePool (LoadContext);
+    break;
+
+  case BM_FILE_CONTEXT_SELECT:
+    FileContext = (BM_FILE_CONTEXT *) MenuEntry->VariableContext;
+
+    if (!FileContext->IsRoot) {
+      FreePool (FileContext->DevicePath);
+    } else {
+      if (FileContext->FHandle != NULL) {
+        FileContext->FHandle->Close (FileContext->FHandle);
+      }
+    }
+
+    if (FileContext->FileName != NULL) {
+      FreePool (FileContext->FileName);
+    }
+    if (FileContext->Info != NULL) {
+      FreePool (FileContext->Info);
+    }
+    FreePool (FileContext);
+    break;
+
+  case BM_CONSOLE_CONTEXT_SELECT:
+    ConsoleContext = (BM_CONSOLE_CONTEXT *) MenuEntry->VariableContext;
+    FreePool (ConsoleContext->DevicePath);
+    FreePool (ConsoleContext);
+    break;
+
+  case BM_TERMINAL_CONTEXT_SELECT:
+    TerminalContext = (BM_TERMINAL_CONTEXT *) MenuEntry->VariableContext;
+    FreePool (TerminalContext->DevicePath);
+    FreePool (TerminalContext);
+    break;
+
+  case BM_HANDLE_CONTEXT_SELECT:
+    HandleContext = (BM_HANDLE_CONTEXT *) MenuEntry->VariableContext;
+    FreePool (HandleContext);
+    break;
+
+  default:
+    break;
+  }
+
+  FreePool (MenuEntry->DisplayString);
+  if (MenuEntry->HelpString != NULL) {
+    FreePool (MenuEntry->HelpString);
+  }
+
+  FreePool (MenuEntry);
+}
+
+/**
+  Get the Menu Entry from the list in Menu Entry List.
+
+  If MenuNumber is great or equal to the number of Menu
+  Entry in the list, then ASSERT.
+
+  @param MenuOption      The Menu Entry List to read the menu entry.
+  @param MenuNumber      The index of Menu Entry.
+
+  @return The Menu Entry.
+
+**/
+BM_MENU_ENTRY *
+BOpt_GetMenuEntry (
+  BM_MENU_OPTION      *MenuOption,
+  UINTN               MenuNumber
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  UINTN           Index;
+  LIST_ENTRY      *List;
+
+  ASSERT (MenuNumber < MenuOption->MenuNumber);
+
+  List = MenuOption->Head.ForwardLink;
+  for (Index = 0; Index < MenuNumber; Index++) {
+    List = List->ForwardLink;
+  }
+
+  NewMenuEntry = CR (List, BM_MENU_ENTRY, Link, BM_MENU_ENTRY_SIGNATURE);
+
+  return NewMenuEntry;
+}
+
+/**
+  This function build the FsOptionMenu list which records all
+  available file system in the system. They includes all instances
+  of EFI_SIMPLE_FILE_SYSTEM_PROTOCOL, all instances of EFI_LOAD_FILE_SYSTEM
+  and all type of legacy boot device.
+
+  @param CallbackData    BMM context data
+
+  @retval  EFI_SUCCESS             Success find the file system
+  @retval  EFI_OUT_OF_RESOURCES    Can not create menu entry
+
+**/
+EFI_STATUS
+BOpt_FindFileSystem (
+  IN BMM_CALLBACK_DATA          *CallbackData
+  )
+{
+  UINTN                     NoBlkIoHandles;
+  UINTN                     NoSimpleFsHandles;
+  UINTN                     NoLoadFileHandles;
+  EFI_HANDLE                *BlkIoHandle;
+  EFI_HANDLE                *SimpleFsHandle;
+  EFI_HANDLE                *LoadFileHandle;
+  UINT16                    *VolumeLabel;
+  EFI_BLOCK_IO_PROTOCOL     *BlkIo;
+  UINTN                     Index;
+  EFI_STATUS                Status;
+  BM_MENU_ENTRY             *MenuEntry;
+  BM_FILE_CONTEXT           *FileContext;
+  UINT16                    *TempStr;
+  UINTN                     OptionNumber;
+  VOID                      *Buffer;
+  BOOLEAN                   RemovableMedia;
+
+  NoSimpleFsHandles = 0;
+  NoLoadFileHandles = 0;
+  OptionNumber      = 0;
+  InitializeListHead (&FsOptionMenu.Head);
+
+  //
+  // Locate Handles that support BlockIo protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiBlockIoProtocolGuid,
+                  NULL,
+                  &NoBlkIoHandles,
+                  &BlkIoHandle
+                  );
+  if (!EFI_ERROR (Status)) {
+
+    for (Index = 0; Index < NoBlkIoHandles; Index++) {
+      Status = gBS->HandleProtocol (
+                      BlkIoHandle[Index],
+                      &gEfiBlockIoProtocolGuid,
+                      (VOID **) &BlkIo
+                      );
+
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Issue a dummy read to trigger reinstall of BlockIo protocol for removable media
+      //
+      if (BlkIo->Media->RemovableMedia) {
+        Buffer = AllocateZeroPool (BlkIo->Media->BlockSize);
+        if (NULL == Buffer) {
+          FreePool (BlkIoHandle);
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        BlkIo->ReadBlocks (
+                BlkIo,
+                BlkIo->Media->MediaId,
+                0,
+                BlkIo->Media->BlockSize,
+                Buffer
+                );
+        FreePool (Buffer);
+      }
+    }
+    FreePool (BlkIoHandle);
+  }
+
+  //
+  // Locate Handles that support Simple File System protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiSimpleFileSystemProtocolGuid,
+                  NULL,
+                  &NoSimpleFsHandles,
+                  &SimpleFsHandle
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Find all the instances of the File System prototocol
+    //
+    for (Index = 0; Index < NoSimpleFsHandles; Index++) {
+      Status = gBS->HandleProtocol (
+                      SimpleFsHandle[Index],
+                      &gEfiBlockIoProtocolGuid,
+                      (VOID **) &BlkIo
+                      );
+      if (EFI_ERROR (Status)) {
+        //
+        // If no block IO exists assume it's NOT a removable media
+        //
+        RemovableMedia = FALSE;
+      } else {
+        //
+        // If block IO exists check to see if it's remobable media
+        //
+        RemovableMedia = BlkIo->Media->RemovableMedia;
+      }
+
+      //
+      // Allocate pool for this load option
+      //
+      MenuEntry = BOpt_CreateMenuEntry (BM_FILE_CONTEXT_SELECT);
+      if (NULL == MenuEntry) {
+        FreePool (SimpleFsHandle);
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      FileContext = (BM_FILE_CONTEXT *) MenuEntry->VariableContext;
+
+      FileContext->Handle     = SimpleFsHandle[Index];
+      MenuEntry->OptionNumber = Index;
+      FileContext->FHandle    = EfiLibOpenRoot (FileContext->Handle);
+      if (FileContext->FHandle == NULL) {
+        BOpt_DestroyMenuEntry (MenuEntry);
+        continue;
+      }
+
+      MenuEntry->HelpString = UiDevicePathToStr (DevicePathFromHandle (FileContext->Handle));
+      FileContext->Info = EfiLibFileSystemVolumeLabelInfo (FileContext->FHandle);
+      FileContext->FileName = EfiStrDuplicate (L"\\");
+      FileContext->DevicePath = FileDevicePath (
+                                  FileContext->Handle,
+                                  FileContext->FileName
+                                  );
+      FileContext->IsDir            = TRUE;
+      FileContext->IsRoot           = TRUE;
+      FileContext->IsRemovableMedia = RemovableMedia;
+      FileContext->IsLoadFile       = FALSE;
+
+      //
+      // Get current file system's Volume Label
+      //
+      if (FileContext->Info == NULL) {
+        VolumeLabel = L"NO FILE SYSTEM INFO";
+      } else {
+        if (FileContext->Info->VolumeLabel == NULL) {
+          VolumeLabel = L"NULL VOLUME LABEL";
+        } else {
+          VolumeLabel = FileContext->Info->VolumeLabel;
+          if (*VolumeLabel == 0x0000) {
+            VolumeLabel = L"NO VOLUME LABEL";
+          }
+        }
+      }
+
+      TempStr                   = MenuEntry->HelpString;
+      MenuEntry->DisplayString  = AllocateZeroPool (MAX_CHAR);
+      ASSERT (MenuEntry->DisplayString != NULL);
+      UnicodeSPrint (
+        MenuEntry->DisplayString,
+        MAX_CHAR,
+        L"%s, [%s]",
+        VolumeLabel,
+        TempStr
+        );
+      OptionNumber++;
+      InsertTailList (&FsOptionMenu.Head, &MenuEntry->Link);
+    }
+  }
+
+  if (NoSimpleFsHandles != 0) {
+    FreePool (SimpleFsHandle);
+  }
+  //
+  // Searching for handles that support Load File protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiLoadFileProtocolGuid,
+                  NULL,
+                  &NoLoadFileHandles,
+                  &LoadFileHandle
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < NoLoadFileHandles; Index++) {
+      MenuEntry = BOpt_CreateMenuEntry (BM_FILE_CONTEXT_SELECT);
+      if (NULL == MenuEntry) {
+        FreePool (LoadFileHandle);
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      FileContext                   = (BM_FILE_CONTEXT *) MenuEntry->VariableContext;
+      FileContext->IsRemovableMedia = FALSE;
+      FileContext->IsLoadFile       = TRUE;
+      FileContext->Handle           = LoadFileHandle[Index];
+      FileContext->IsRoot           = TRUE;
+
+      FileContext->DevicePath       = DevicePathFromHandle (FileContext->Handle);
+      FileContext->FileName         = UiDevicePathToStr (FileContext->DevicePath);
+
+      MenuEntry->HelpString     = UiDevicePathToStr (FileContext->DevicePath);
+
+      TempStr                   = MenuEntry->HelpString;
+      MenuEntry->DisplayString  = AllocateZeroPool (MAX_CHAR);
+      ASSERT (MenuEntry->DisplayString != NULL);
+      UnicodeSPrint (
+        MenuEntry->DisplayString,
+        MAX_CHAR,
+        L"Load File [%s]",
+        TempStr
+        );
+
+      MenuEntry->OptionNumber = OptionNumber;
+      OptionNumber++;
+      InsertTailList (&FsOptionMenu.Head, &MenuEntry->Link);
+    }
+  }
+
+  if (NoLoadFileHandles != 0) {
+    FreePool (LoadFileHandle);
+  }
+
+  //
+  // Remember how many file system options are here
+  //
+  FsOptionMenu.MenuNumber = OptionNumber;
+  return EFI_SUCCESS;
+}
+
+/**
+  Free resources allocated in Allocate Rountine.
+
+  @param FreeMenu        Menu to be freed
+**/
+VOID
+BOpt_FreeMenu (
+  BM_MENU_OPTION        *FreeMenu
+  )
+{
+  BM_MENU_ENTRY *MenuEntry;
+  while (!IsListEmpty (&FreeMenu->Head)) {
+    MenuEntry = CR (
+                  FreeMenu->Head.ForwardLink,
+                  BM_MENU_ENTRY,
+                  Link,
+                  BM_MENU_ENTRY_SIGNATURE
+                  );
+    RemoveEntryList (&MenuEntry->Link);
+    BOpt_DestroyMenuEntry (MenuEntry);
+  }
+  FreeMenu->MenuNumber = 0;
+}
+
+/**
+  Find files under current directory
+  All files and sub-directories in current directory
+  will be stored in DirectoryMenu for future use.
+
+  @param CallbackData  The BMM context data.
+  @param MenuEntry     The Menu Entry.
+
+  @retval EFI_SUCCESS         Get files from current dir successfully.
+  @return Other value if can't get files from current dir.
+
+**/
+EFI_STATUS
+BOpt_FindFiles (
+  IN BMM_CALLBACK_DATA          *CallbackData,
+  IN BM_MENU_ENTRY              *MenuEntry
+  )
+{
+  EFI_FILE_HANDLE NewDir;
+  EFI_FILE_HANDLE Dir;
+  EFI_FILE_INFO   *DirInfo;
+  UINTN           BufferSize;
+  UINTN           DirBufferSize;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_FILE_CONTEXT *FileContext;
+  BM_FILE_CONTEXT *NewFileContext;
+  UINTN           Pass;
+  EFI_STATUS      Status;
+  UINTN           OptionNumber;
+
+  FileContext   = (BM_FILE_CONTEXT *) MenuEntry->VariableContext;
+  Dir           = FileContext->FHandle;
+  OptionNumber  = 0;
+  //
+  // Open current directory to get files from it
+  //
+  Status = Dir->Open (
+                  Dir,
+                  &NewDir,
+                  FileContext->FileName,
+                  EFI_FILE_READ_ONLY,
+                  0
+                  );
+  if (!FileContext->IsRoot) {
+    Dir->Close (Dir);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DirInfo = EfiLibFileInfo (NewDir);
+  if (DirInfo == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if ((DirInfo->Attribute & EFI_FILE_DIRECTORY) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FileContext->DevicePath = FileDevicePath (
+                              FileContext->Handle,
+                              FileContext->FileName
+                              );
+
+  DirBufferSize = sizeof (EFI_FILE_INFO) + 1024;
+  DirInfo       = AllocateZeroPool (DirBufferSize);
+  if (DirInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Get all files in current directory
+  // Pass 1 to get Directories
+  // Pass 2 to get files that are EFI images
+  //
+  for (Pass = 1; Pass <= 2; Pass++) {
+    NewDir->SetPosition (NewDir, 0);
+    for (;;) {
+      BufferSize  = DirBufferSize;
+      Status      = NewDir->Read (NewDir, &BufferSize, DirInfo);
+      if (EFI_ERROR (Status) || BufferSize == 0) {
+        break;
+      }
+
+      if (((DirInfo->Attribute & EFI_FILE_DIRECTORY) != 0 && Pass == 2) ||
+          ((DirInfo->Attribute & EFI_FILE_DIRECTORY) == 0 && Pass == 1)
+          ) {
+        //
+        // Pass 1 is for Directories
+        // Pass 2 is for file names
+        //
+        continue;
+      }
+
+      if (!(BOpt_IsEfiImageName (DirInfo->FileName) || (DirInfo->Attribute & EFI_FILE_DIRECTORY) != 0)) {
+        //
+        // Slip file unless it is a directory entry or a .EFI file
+        //
+        continue;
+      }
+
+      NewMenuEntry = BOpt_CreateMenuEntry (BM_FILE_CONTEXT_SELECT);
+      if (NULL == NewMenuEntry) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      NewFileContext          = (BM_FILE_CONTEXT *) NewMenuEntry->VariableContext;
+      NewFileContext->Handle  = FileContext->Handle;
+      NewFileContext->FileName = BOpt_AppendFileName (
+                                  FileContext->FileName,
+                                  DirInfo->FileName
+                                  );
+      NewFileContext->FHandle = NewDir;
+      NewFileContext->DevicePath = FileDevicePath (
+                                    NewFileContext->Handle,
+                                    NewFileContext->FileName
+                                    );
+      NewMenuEntry->HelpString = NULL;
+
+      MenuEntry->DisplayStringToken = GetStringTokenFromDepository (
+                                        CallbackData,
+                                        FileOptionStrDepository
+                                        );
+
+      NewFileContext->IsDir = (BOOLEAN) ((DirInfo->Attribute & EFI_FILE_DIRECTORY) == EFI_FILE_DIRECTORY);
+
+      if (NewFileContext->IsDir) {
+        BufferSize                  = StrLen (DirInfo->FileName) * 2 + 6;
+        NewMenuEntry->DisplayString = AllocateZeroPool (BufferSize);
+
+        UnicodeSPrint (
+          NewMenuEntry->DisplayString,
+          BufferSize,
+          L"<%s>",
+          DirInfo->FileName
+          );
+
+      } else {
+        NewMenuEntry->DisplayString = EfiStrDuplicate (DirInfo->FileName);
+      }
+
+      NewFileContext->IsRoot            = FALSE;
+      NewFileContext->IsLoadFile        = FALSE;
+      NewFileContext->IsRemovableMedia  = FALSE;
+
+      NewMenuEntry->OptionNumber        = OptionNumber;
+      OptionNumber++;
+      InsertTailList (&DirectoryMenu.Head, &NewMenuEntry->Link);
+    }
+  }
+
+  DirectoryMenu.MenuNumber = OptionNumber;
+  FreePool (DirInfo);
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Build the BootOptionMenu according to BootOrder Variable.
+  This Routine will access the Boot#### to get EFI_LOAD_OPTION.
+
+  @param CallbackData The BMM context data.
+
+  @return EFI_NOT_FOUND Fail to find "BootOrder" variable.
+  @return EFI_SUCESS    Success build boot option menu.
+
+**/
+EFI_STATUS
+BOpt_GetBootOptions (
+  IN  BMM_CALLBACK_DATA         *CallbackData
+  )
+{
+  UINTN                         Index;
+  UINT16                        BootString[10];
+  UINT8                         *LoadOptionFromVar;
+  UINT8                         *LoadOption;
+  UINTN                         BootOptionSize;
+  BOOLEAN                       BootNextFlag;
+  UINT16                        *BootOrderList;
+  UINTN                         BootOrderListSize;
+  UINT16                        *BootNext;
+  UINTN                         BootNextSize;
+  BM_MENU_ENTRY                 *NewMenuEntry;
+  BM_LOAD_CONTEXT               *NewLoadContext;
+  UINT8                         *LoadOptionPtr;
+  UINTN                         StringSize;
+  UINTN                         OptionalDataSize;
+  UINT8                         *LoadOptionEnd;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  UINTN                         MenuCount;
+  UINT8                         *Ptr;
+  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption;
+  UINTN                         BootOptionCount;  
+  
+  MenuCount         = 0;
+  BootOrderListSize = 0;
+  BootNextSize      = 0;
+  BootOrderList     = NULL;
+  BootNext          = NULL;
+  LoadOptionFromVar = NULL;
+  BOpt_FreeMenu (&BootOptionMenu);
+  InitializeListHead (&BootOptionMenu.Head);
+
+  //
+  // Get the BootOrder from the Var
+  //
+  GetEfiGlobalVariable2 (L"BootOrder", (VOID **) &BootOrderList, &BootOrderListSize);
+  if (BootOrderList == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  
+  //
+  // Get the BootNext from the Var
+  //
+  GetEfiGlobalVariable2 (L"BootNext", (VOID **) &BootNext, &BootNextSize);
+  if (BootNext != NULL) {
+    if (BootNextSize != sizeof (UINT16)) {
+      FreePool (BootNext);
+      BootNext = NULL;
+    }
+  }
+  BootOption = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+  for (Index = 0; Index < BootOrderListSize / sizeof (UINT16); Index++) {
+    //
+    // Don't display the hidden/inactive boot option
+    //
+    if (((BootOption[Index].Attributes & LOAD_OPTION_HIDDEN) != 0) || ((BootOption[Index].Attributes & LOAD_OPTION_ACTIVE) == 0)) {
+      continue;
+    }
+      
+    UnicodeSPrint (BootString, sizeof (BootString), L"Boot%04x", BootOrderList[Index]);
+    //
+    //  Get all loadoptions from the VAR
+    //
+    GetEfiGlobalVariable2 (BootString, (VOID **) &LoadOptionFromVar, &BootOptionSize);
+    if (LoadOptionFromVar == NULL) {
+      continue;
+    }
+
+    LoadOption = AllocateZeroPool (BootOptionSize);
+    if (LoadOption == NULL) {
+      continue;
+    }
+
+    CopyMem (LoadOption, LoadOptionFromVar, BootOptionSize);
+    FreePool (LoadOptionFromVar);
+
+    if (BootNext != NULL) {
+      BootNextFlag = (BOOLEAN) (*BootNext == BootOrderList[Index]);
+    } else {
+      BootNextFlag = FALSE;
+    }
+
+    NewMenuEntry = BOpt_CreateMenuEntry (BM_LOAD_CONTEXT_SELECT);
+    ASSERT (NULL != NewMenuEntry);
+
+    NewLoadContext                      = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+
+    LoadOptionPtr                       = LoadOption;
+    LoadOptionEnd                       = LoadOption + BootOptionSize;
+
+    NewMenuEntry->OptionNumber          = BootOrderList[Index];
+    NewLoadContext->LoadOptionModified  = FALSE;
+    NewLoadContext->Deleted             = FALSE;
+    NewLoadContext->IsBootNext          = BootNextFlag;
+
+    //
+    // Is a Legacy Device?
+    //
+    Ptr = (UINT8 *) LoadOption;
+
+    //
+    // Attribute = *(UINT32 *)Ptr;
+    //
+    Ptr += sizeof (UINT32);
+
+    //
+    // FilePathSize = *(UINT16 *)Ptr;
+    //
+    Ptr += sizeof (UINT16);
+
+    //
+    // Description = (CHAR16 *)Ptr;
+    //
+    Ptr += StrSize ((CHAR16 *) Ptr);
+
+    //
+    // Now Ptr point to Device Path
+    //
+    DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) Ptr;
+    if ((BBS_DEVICE_PATH == DevicePath->Type) && (BBS_BBS_DP == DevicePath->SubType)) {
+      NewLoadContext->IsLegacy = TRUE;
+    } else {
+      NewLoadContext->IsLegacy = FALSE;
+    }
+    //
+    // LoadOption is a pointer type of UINT8
+    // for easy use with following LOAD_OPTION
+    // embedded in this struct
+    //
+    NewLoadContext->LoadOption      = LoadOption;
+    NewLoadContext->LoadOptionSize  = BootOptionSize;
+
+    NewLoadContext->Attributes      = *(UINT32 *) LoadOptionPtr;
+    NewLoadContext->IsActive        = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_ACTIVE);
+
+    NewLoadContext->ForceReconnect  = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_FORCE_RECONNECT);
+
+    LoadOptionPtr += sizeof (UINT32);
+
+    NewLoadContext->FilePathListLength = *(UINT16 *) LoadOptionPtr;
+    LoadOptionPtr += sizeof (UINT16);
+    
+    StringSize = StrSize((UINT16*)LoadOptionPtr);
+
+    NewLoadContext->Description = AllocateZeroPool (StrSize((UINT16*)LoadOptionPtr));
+    ASSERT (NewLoadContext->Description != NULL);
+    StrCpyS (NewLoadContext->Description, StrSize((UINT16*)LoadOptionPtr) / sizeof (UINT16), (UINT16*)LoadOptionPtr);
+    
+    ASSERT (NewLoadContext->Description != NULL);
+    NewMenuEntry->DisplayString = NewLoadContext->Description;
+
+    LoadOptionPtr += StringSize;
+
+    NewLoadContext->FilePathList = AllocateZeroPool (NewLoadContext->FilePathListLength);
+    ASSERT (NewLoadContext->FilePathList != NULL);
+    CopyMem (
+      NewLoadContext->FilePathList,
+      (EFI_DEVICE_PATH_PROTOCOL *) LoadOptionPtr,
+      NewLoadContext->FilePathListLength
+      );
+
+    NewMenuEntry->HelpString = UiDevicePathToStr (NewLoadContext->FilePathList);
+    NewMenuEntry->DisplayStringToken = GetStringTokenFromDepository (
+                                        CallbackData,
+                                        BootOptionStrDepository
+                                        );
+    NewMenuEntry->HelpStringToken = GetStringTokenFromDepository (
+                                      CallbackData,
+                                      BootOptionHelpStrDepository
+                                      );
+    LoadOptionPtr += NewLoadContext->FilePathListLength;
+
+    if (LoadOptionPtr < LoadOptionEnd) {
+      OptionalDataSize = BootOptionSize -
+        sizeof (UINT32) -
+        sizeof (UINT16) -
+        StringSize -
+        NewLoadContext->FilePathListLength;
+
+      NewLoadContext->OptionalData = AllocateZeroPool (OptionalDataSize);
+      ASSERT (NewLoadContext->OptionalData != NULL);
+      CopyMem (
+        NewLoadContext->OptionalData,
+        LoadOptionPtr,
+        OptionalDataSize
+        );
+
+      NewLoadContext->OptionalDataSize = OptionalDataSize;
+    }
+
+    InsertTailList (&BootOptionMenu.Head, &NewMenuEntry->Link);
+    MenuCount++;
+  }
+  EfiBootManagerFreeLoadOptions (BootOption, BootOptionCount);
+
+  if (BootNext != NULL) {
+    FreePool (BootNext);
+  }
+  if (BootOrderList != NULL) {
+    FreePool (BootOrderList);
+  }
+  BootOptionMenu.MenuNumber = MenuCount;
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Append file name to existing file name.
+
+  @param Str1  The existing file name
+  @param Str2  The file name to be appended
+
+  @return Allocate a new string to hold the appended result.
+          Caller is responsible to free the returned string.
+
+**/
+CHAR16 *
+BOpt_AppendFileName (
+  IN  CHAR16  *Str1,
+  IN  CHAR16  *Str2
+  )
+{
+  UINTN   DestMax;
+  CHAR16  *Str;
+  CHAR16  *TmpStr;
+  CHAR16  *Ptr;
+  CHAR16  *LastSlash;
+
+  DestMax = (StrSize (Str1) + StrSize (Str2) + sizeof (CHAR16)) / sizeof (CHAR16);
+  Str   = AllocateZeroPool (DestMax * sizeof (CHAR16));
+  ASSERT (Str != NULL);
+
+  TmpStr = AllocateZeroPool (DestMax * sizeof (CHAR16)); 
+  ASSERT (TmpStr != NULL);
+
+  StrCatS (Str, DestMax, Str1);
+  if (!((*Str == '\\') && (*(Str + 1) == 0))) {
+    StrCatS (Str, DestMax, L"\\");
+  }
+
+  StrCatS (Str, DestMax, Str2);
+
+  Ptr       = Str;
+  LastSlash = Str;
+  while (*Ptr != 0) {
+    if (*Ptr == '\\' && *(Ptr + 1) == '.' && *(Ptr + 2) == '.' && *(Ptr + 3) == L'\\') {
+      //
+      // Convert "\Name\..\" to "\"
+      // DO NOT convert the .. if it is at the end of the string. This will
+      // break the .. behavior in changing directories.
+      //
+
+      //
+      // Use TmpStr as a backup, as StrCpyS in BaseLib does not handle copy of two strings 
+      // that overlap.
+      //
+      StrCpyS (TmpStr, DestMax, Ptr + 3);
+      StrCpyS (LastSlash, DestMax - (UINTN) (LastSlash - Str), TmpStr);
+      Ptr = LastSlash;
+    } else if (*Ptr == '\\' && *(Ptr + 1) == '.' && *(Ptr + 2) == '\\') {
+      //
+      // Convert a "\.\" to a "\"
+      //
+
+      //
+      // Use TmpStr as a backup, as StrCpyS in BaseLib does not handle copy of two strings 
+      // that overlap.
+      //
+      StrCpyS (TmpStr, DestMax, Ptr + 2);
+      StrCpyS (Ptr, DestMax - (UINTN) (Ptr - Str), TmpStr);
+      Ptr = LastSlash;
+    } else if (*Ptr == '\\') {
+      LastSlash = Ptr;
+    }
+
+    Ptr++;
+  }
+
+  FreePool (TmpStr);
+  
+  return Str;
+}
+
+/**
+
+  Check whether current FileName point to a valid
+  Efi Image File.
+
+  @param FileName  File need to be checked.
+
+  @retval TRUE  Is Efi Image
+  @retval FALSE Not a valid Efi Image
+
+**/
+BOOLEAN
+BOpt_IsEfiImageName (
+  IN UINT16  *FileName
+  )
+{
+  //
+  // Search for ".efi" extension
+  //
+  while (*FileName != L'\0') {
+    if (FileName[0] == '.') {
+      if (FileName[1] == 'e' || FileName[1] == 'E') {
+        if (FileName[2] == 'f' || FileName[2] == 'F') {
+          if (FileName[3] == 'i' || FileName[3] == 'I') {
+            return TRUE;
+          } else if (FileName[3] == 0x0000) {
+            return FALSE;
+          }
+        } else if (FileName[2] == 0x0000) {
+          return FALSE;
+        }
+      } else if (FileName[1] == 0x0000) {
+        return FALSE;
+      }
+    }
+
+    FileName += 1;
+  }
+
+  return FALSE;
+}
+
+/**
+
+  Check whether current FileName point to a valid Efi Application
+
+  @param Dir       Pointer to current Directory
+  @param FileName  Pointer to current File name.
+
+  @retval TRUE      Is a valid Efi Application
+  @retval FALSE     not a valid Efi Application
+
+**/
+BOOLEAN
+BOpt_IsEfiApp (
+  IN EFI_FILE_HANDLE Dir,
+  IN UINT16          *FileName
+  )
+{
+  UINTN                       BufferSize;
+  EFI_IMAGE_DOS_HEADER        DosHdr;
+  UINT16                      Subsystem;
+  EFI_FILE_HANDLE             File;
+  EFI_STATUS                  Status;
+  EFI_IMAGE_OPTIONAL_HEADER_UNION PeHdr;
+
+  Status = Dir->Open (Dir, &File, FileName, EFI_FILE_MODE_READ, 0);
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  BufferSize = sizeof (EFI_IMAGE_DOS_HEADER);
+  File->Read (File, &BufferSize, &DosHdr);
+  if (DosHdr.e_magic != EFI_IMAGE_DOS_SIGNATURE) {
+    File->Close (File);
+    return FALSE;
+  }
+
+  File->SetPosition (File, DosHdr.e_lfanew);
+  BufferSize = sizeof (EFI_IMAGE_OPTIONAL_HEADER_UNION);
+  File->Read (File, &BufferSize, &PeHdr);
+  if (PeHdr.Pe32.Signature != EFI_IMAGE_NT_SIGNATURE) {
+    File->Close (File);
+    return FALSE;
+  }
+  //
+  // Determine PE type and read subsytem
+  //
+  if (PeHdr.Pe32.OptionalHeader.Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+    Subsystem = PeHdr.Pe32.OptionalHeader.Subsystem;
+  } else if (PeHdr.Pe32.OptionalHeader.Magic == EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC) {
+    Subsystem = PeHdr.Pe32Plus.OptionalHeader.Subsystem;
+  } else {
+    return FALSE;
+  }
+
+  if (Subsystem == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION) {
+    File->Close (File);
+    return TRUE;
+  } else {
+    File->Close (File);
+    return FALSE;
+  }
+}
+
+/**
+
+  Find drivers that will be added as Driver#### variables from handles
+  in current system environment
+  All valid handles in the system except those consume SimpleFs, LoadFile
+  are stored in DriverMenu for future use.
+
+  @retval EFI_SUCCESS The function complets successfully.
+  @return Other value if failed to build the DriverMenu.
+
+**/
+EFI_STATUS
+BOpt_FindDrivers (
+  VOID
+  )
+{
+  UINTN                           NoDevicePathHandles;
+  EFI_HANDLE                      *DevicePathHandle;
+  UINTN                           Index;
+  EFI_STATUS                      Status;
+  BM_MENU_ENTRY                   *NewMenuEntry;
+  BM_HANDLE_CONTEXT               *NewHandleContext;
+  EFI_HANDLE                      CurHandle;
+  UINTN                           OptionNumber;
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *SimpleFs;
+  EFI_LOAD_FILE_PROTOCOL          *LoadFile;
+
+  SimpleFs  = NULL;
+  LoadFile  = NULL;
+
+  InitializeListHead (&DriverMenu.Head);
+
+  //
+  // At first, get all handles that support Device Path
+  // protocol which is the basic requirement for
+  // Driver####
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDevicePathProtocolGuid,
+                  NULL,
+                  &NoDevicePathHandles,
+                  &DevicePathHandle
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  OptionNumber = 0;
+  for (Index = 0; Index < NoDevicePathHandles; Index++) {
+    CurHandle = DevicePathHandle[Index];
+
+    Status = gBS->HandleProtocol (
+                    CurHandle,
+                    &gEfiSimpleFileSystemProtocolGuid,
+                    (VOID **) &SimpleFs
+                    );
+    if (Status == EFI_SUCCESS) {
+      continue;
+    }
+
+    Status = gBS->HandleProtocol (
+                    CurHandle,
+                    &gEfiLoadFileProtocolGuid,
+                    (VOID **) &LoadFile
+                    );
+    if (Status == EFI_SUCCESS) {
+      continue;
+    }
+
+    NewMenuEntry = BOpt_CreateMenuEntry (BM_HANDLE_CONTEXT_SELECT);
+    if (NULL == NewMenuEntry) {
+      FreePool (DevicePathHandle);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NewHandleContext              = (BM_HANDLE_CONTEXT *) NewMenuEntry->VariableContext;
+    NewHandleContext->Handle      = CurHandle;
+    NewHandleContext->DevicePath  = DevicePathFromHandle (CurHandle);
+    NewMenuEntry->DisplayString = UiDevicePathToStr (NewHandleContext->DevicePath);
+    NewMenuEntry->HelpString    = NULL;
+    NewMenuEntry->OptionNumber  = OptionNumber;
+    OptionNumber++;
+    InsertTailList (&DriverMenu.Head, &NewMenuEntry->Link);
+
+  }
+
+  if (DevicePathHandle != NULL) {
+    FreePool (DevicePathHandle);
+  }
+
+  DriverMenu.MenuNumber = OptionNumber;
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Get the Option Number that has not been allocated for use.
+
+  @param Type  The type of Option.
+
+  @return The available Option Number.
+
+**/
+UINT16
+BOpt_GetOptionNumber (
+  CHAR16        *Type
+  )
+{
+  UINT16        *OrderList;
+  UINTN         OrderListSize;
+  UINTN         Index;
+  CHAR16        StrTemp[20];
+  UINT16        *OptionBuffer;
+  UINT16        OptionNumber;
+  UINTN         OptionSize;
+
+  OrderListSize = 0;
+  OrderList     = NULL;
+  OptionNumber  = 0;
+  Index         = 0;
+
+  UnicodeSPrint (StrTemp, sizeof (StrTemp), L"%sOrder", Type);
+
+  GetEfiGlobalVariable2 (StrTemp, (VOID **) &OrderList, &OrderListSize);
+  for (OptionNumber = 0; ; OptionNumber++) {
+    if (OrderList != NULL) {
+      for (Index = 0; Index < OrderListSize / sizeof (UINT16); Index++) {
+        if (OptionNumber == OrderList[Index]) {
+          break;
+        }
+      }
+    }
+
+    if (Index < OrderListSize / sizeof (UINT16)) {
+      //
+      // The OptionNumber occurs in the OrderList, continue to use next one
+      //
+      continue;
+    }
+    UnicodeSPrint (StrTemp, sizeof (StrTemp), L"%s%04x", Type, (UINTN) OptionNumber);
+    DEBUG((EFI_D_ERROR,"Option = %s\n", StrTemp));
+    GetEfiGlobalVariable2 (StrTemp, (VOID **) &OptionBuffer, &OptionSize);
+    if (NULL == OptionBuffer) {
+      //
+      // The Boot[OptionNumber] / Driver[OptionNumber] NOT occurs, we found it
+      //
+      break;
+    }
+  }
+
+  return OptionNumber;
+}
+
+/**
+
+  Get the Option Number for Boot#### that does not used.
+
+  @return The available Option Number.
+
+**/
+UINT16
+BOpt_GetBootOptionNumber (
+  VOID
+  )
+{
+  return BOpt_GetOptionNumber (L"Boot");
+}
+
+/**
+
+  Get the Option Number for Driver#### that does not used.
+
+  @return The unused Option Number.
+
+**/
+UINT16
+BOpt_GetDriverOptionNumber (
+  VOID
+  )
+{
+  return BOpt_GetOptionNumber (L"Driver");
+}
+
+/**
+
+  Build up all DriverOptionMenu
+
+  @param CallbackData The BMM context data.
+
+  @retval EFI_SUCESS           The functin completes successfully.
+  @retval EFI_OUT_OF_RESOURCES Not enough memory to compete the operation.
+  @retval EFI_NOT_FOUND        Fail to get "DriverOrder" variable.
+
+**/
+EFI_STATUS
+BOpt_GetDriverOptions (
+  IN  BMM_CALLBACK_DATA         *CallbackData
+  )
+{
+  UINTN           Index;
+  UINT16          DriverString[12];
+  UINT8           *LoadOptionFromVar;
+  UINT8           *LoadOption;
+  UINTN           DriverOptionSize;
+
+  UINT16          *DriverOrderList;
+  UINTN           DriverOrderListSize;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT8           *LoadOptionPtr;
+  UINTN           StringSize;
+  UINTN           OptionalDataSize;
+  UINT8           *LoadOptionEnd;
+
+  DriverOrderListSize = 0;
+  DriverOrderList     = NULL;
+  DriverOptionSize    = 0;
+  LoadOptionFromVar   = NULL;
+  BOpt_FreeMenu (&DriverOptionMenu);
+  InitializeListHead (&DriverOptionMenu.Head);
+  //
+  // Get the DriverOrder from the Var
+  //
+  GetEfiGlobalVariable2 (L"DriverOrder", (VOID **) &DriverOrderList, &DriverOrderListSize);
+  if (DriverOrderList == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  
+  for (Index = 0; Index < DriverOrderListSize / sizeof (UINT16); Index++) {
+    UnicodeSPrint (
+      DriverString,
+      sizeof (DriverString),
+      L"Driver%04x",
+      DriverOrderList[Index]
+      );
+    //
+    //  Get all loadoptions from the VAR
+    //
+    GetEfiGlobalVariable2 (DriverString, (VOID **) &LoadOptionFromVar, &DriverOptionSize);
+    if (LoadOptionFromVar == NULL) {
+      continue;
+    }
+
+    LoadOption = AllocateZeroPool (DriverOptionSize);
+    if (LoadOption == NULL) {
+      continue;
+    }
+
+    CopyMem (LoadOption, LoadOptionFromVar, DriverOptionSize);
+    FreePool (LoadOptionFromVar);
+
+    NewMenuEntry = BOpt_CreateMenuEntry (BM_LOAD_CONTEXT_SELECT);
+    if (NULL == NewMenuEntry) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NewLoadContext                      = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    LoadOptionPtr                       = LoadOption;
+    LoadOptionEnd                       = LoadOption + DriverOptionSize;
+    NewMenuEntry->OptionNumber          = DriverOrderList[Index];
+    NewLoadContext->LoadOptionModified  = FALSE;
+    NewLoadContext->Deleted             = FALSE;
+    NewLoadContext->IsLegacy            = FALSE;
+
+    //
+    // LoadOption is a pointer type of UINT8
+    // for easy use with following LOAD_OPTION
+    // embedded in this struct
+    //
+    NewLoadContext->LoadOption      = LoadOption;
+    NewLoadContext->LoadOptionSize  = DriverOptionSize;
+
+    NewLoadContext->Attributes      = *(UINT32 *) LoadOptionPtr;
+    NewLoadContext->IsActive        = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_ACTIVE);
+
+    NewLoadContext->ForceReconnect  = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_FORCE_RECONNECT);
+
+    LoadOptionPtr += sizeof (UINT32);
+
+    NewLoadContext->FilePathListLength = *(UINT16 *) LoadOptionPtr;
+    LoadOptionPtr += sizeof (UINT16);
+
+    StringSize                  = StrSize ((UINT16 *) LoadOptionPtr);
+    NewLoadContext->Description = AllocateZeroPool (StringSize);
+    ASSERT (NewLoadContext->Description != NULL);
+    CopyMem (
+      NewLoadContext->Description,
+      (UINT16 *) LoadOptionPtr,
+      StringSize
+      );
+    NewMenuEntry->DisplayString = NewLoadContext->Description;
+
+    LoadOptionPtr += StringSize;
+
+    NewLoadContext->FilePathList = AllocateZeroPool (NewLoadContext->FilePathListLength);
+    ASSERT (NewLoadContext->FilePathList != NULL);
+    CopyMem (
+      NewLoadContext->FilePathList,
+      (EFI_DEVICE_PATH_PROTOCOL *) LoadOptionPtr,
+      NewLoadContext->FilePathListLength
+      );
+
+    NewMenuEntry->HelpString = UiDevicePathToStr (NewLoadContext->FilePathList);
+    NewMenuEntry->DisplayStringToken = GetStringTokenFromDepository (
+                                        CallbackData,
+                                        DriverOptionStrDepository
+                                        );
+    NewMenuEntry->HelpStringToken = GetStringTokenFromDepository (
+                                      CallbackData,
+                                      DriverOptionHelpStrDepository
+                                      );
+    LoadOptionPtr += NewLoadContext->FilePathListLength;
+
+    if (LoadOptionPtr < LoadOptionEnd) {
+      OptionalDataSize = DriverOptionSize -
+        sizeof (UINT32) -
+        sizeof (UINT16) -
+        StringSize -
+        NewLoadContext->FilePathListLength;
+
+      NewLoadContext->OptionalData = AllocateZeroPool (OptionalDataSize);
+      ASSERT (NewLoadContext->OptionalData != NULL);
+      CopyMem (
+        NewLoadContext->OptionalData,
+        LoadOptionPtr,
+        OptionalDataSize
+        );
+
+      NewLoadContext->OptionalDataSize = OptionalDataSize;
+    }
+
+    InsertTailList (&DriverOptionMenu.Head, &NewMenuEntry->Link);
+
+  }
+
+  if (DriverOrderList != NULL) {
+    FreePool (DriverOrderList);
+  }
+  DriverOptionMenu.MenuNumber = Index;
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Get option number according to Boot#### and BootOrder variable. 
+  The value is saved as #### + 1.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID  
+GetBootOrder (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  BMM_FAKE_NV_DATA          *BmmConfig;
+  UINT16                    Index;
+  UINT16                    OptionOrderIndex; 
+  UINTN                     DeviceType;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_LOAD_CONTEXT           *NewLoadContext;  
+
+  ASSERT (CallbackData != NULL);
+  
+  DeviceType = (UINTN) -1;   
+  BmmConfig  = &CallbackData->BmmFakeNvData;  
+  ZeroMem (BmmConfig->BootOptionOrder, sizeof (BmmConfig->BootOptionOrder));
+  
+  for (Index = 0, OptionOrderIndex = 0; ((Index < BootOptionMenu.MenuNumber) &&
+       (OptionOrderIndex < (sizeof (BmmConfig->BootOptionOrder) / sizeof (BmmConfig->BootOptionOrder[0]))));
+       Index++) {
+    NewMenuEntry   = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+    NewLoadContext = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+
+    if (NewLoadContext->IsLegacy) {
+      if (((BBS_BBS_DEVICE_PATH *) NewLoadContext->FilePathList)->DeviceType != DeviceType) {
+        DeviceType = ((BBS_BBS_DEVICE_PATH *) NewLoadContext->FilePathList)->DeviceType;
+      } else {
+        //
+        // Only show one legacy boot option for the same device type
+        // assuming the boot options are grouped by the device type
+        //
+        continue;
+      }
+    }
+    BmmConfig->BootOptionOrder[OptionOrderIndex++] = (UINT32) (NewMenuEntry->OptionNumber + 1);
+  }  
+}
+
+/**
+  Get driver option order from globalc DriverOptionMenu.
+
+  @param CallbackData    The BMM context data.
+  
+**/
+VOID  
+GetDriverOrder (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  BMM_FAKE_NV_DATA          *BmmConfig;
+  UINT16                    Index;
+  UINT16                    OptionOrderIndex; 
+  UINTN                     DeviceType;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_LOAD_CONTEXT           *NewLoadContext;  
+
+
+  ASSERT (CallbackData != NULL);
+  
+  DeviceType = (UINTN) -1;   
+  BmmConfig  = &CallbackData->BmmFakeNvData;  
+  ZeroMem (BmmConfig->DriverOptionOrder, sizeof (BmmConfig->DriverOptionOrder));
+  
+  for (Index = 0, OptionOrderIndex = 0; ((Index < DriverOptionMenu.MenuNumber) &&
+       (OptionOrderIndex < (sizeof (BmmConfig->DriverOptionOrder) / sizeof (BmmConfig->DriverOptionOrder[0]))));
+       Index++) {
+    NewMenuEntry   = BOpt_GetMenuEntry (&DriverOptionMenu, Index);
+    NewLoadContext = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+
+    if (NewLoadContext->IsLegacy) {
+      if (((BBS_BBS_DEVICE_PATH *) NewLoadContext->FilePathList)->DeviceType != DeviceType) {
+        DeviceType = ((BBS_BBS_DEVICE_PATH *) NewLoadContext->FilePathList)->DeviceType;
+      } else {
+        //
+        // Only show one legacy boot option for the same device type
+        // assuming the boot options are grouped by the device type
+        //
+        continue;
+      }
+    }
+    BmmConfig->DriverOptionOrder[OptionOrderIndex++] = (UINT32) (NewMenuEntry->OptionNumber + 1);
+  }  
+}  
+  
+
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/ConsoleOption.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/ConsoleOption.c
new file mode 100644
index 0000000000..58ce8b7f3a
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/ConsoleOption.c
@@ -0,0 +1,1147 @@
+/** @file
+  handles console redirection from boot manager
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+
+/**
+  Function compares a device path data structure to that of all the nodes of a
+  second device path instance.
+
+  @param  Multi                 A pointer to a multi-instance device path data
+                                structure.
+  @param  Single                A pointer to a single-instance device path data
+                                structure.
+
+  @retval TRUE                  If the Single device path is contained within Multi device path.
+  @retval FALSE                 The Single device path is not match within Multi device path.
+
+**/
+BOOLEAN
+MatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathInst;
+  UINTN                     Size;
+
+  if (Multi == NULL || Single  == NULL) {
+    return FALSE;
+  }
+
+  DevicePath     = Multi;
+  DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+
+  //
+  // Search for the match of 'Single' in 'Multi'
+  //
+  while (DevicePathInst != NULL) {
+    //
+    // If the single device path is found in multiple device paths,
+    // return success
+    //
+    if (CompareMem (Single, DevicePathInst, Size) == 0) {
+      FreePool (DevicePathInst);
+      return TRUE;
+    }
+
+    FreePool (DevicePathInst);
+    DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether the device path node is ISA Serial Node.
+
+  @param Acpi           Device path node to be checked
+
+  @retval TRUE          It's ISA Serial Node.
+  @retval FALSE         It's NOT ISA Serial Node.
+
+**/
+BOOLEAN
+IsIsaSerialNode (
+  IN ACPI_HID_DEVICE_PATH *Acpi
+  )
+{
+  return (BOOLEAN) (
+      (DevicePathType (Acpi) == ACPI_DEVICE_PATH) &&
+      (DevicePathSubType (Acpi) == ACPI_DP) &&
+      (ReadUnaligned32 (&Acpi->HID) == EISA_PNP_ID (0x0501))
+      );
+}
+
+/**
+  Update Com Ports attributes from DevicePath
+
+  @param DevicePath      DevicePath that contains Com ports
+
+  @retval EFI_SUCCESS   The update is successful.
+
+**/
+EFI_STATUS
+UpdateComAttributeFromVariable (
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  );
+
+/**
+  Update the multi-instance device path of Terminal Device based on
+  the global TerminalMenu. If ChangeTernimal is TRUE, the terminal 
+  device path in the Terminal Device in TerminalMenu is also updated.
+
+  @param DevicePath      The multi-instance device path.
+  @param ChangeTerminal  TRUE, then device path in the Terminal Device 
+                         in TerminalMenu is also updated; FALSE, no update.
+
+  @return EFI_SUCCESS    The function completes successfully.
+
+**/
+EFI_STATUS
+ChangeTerminalDevicePath (
+  IN OUT    EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN        BOOLEAN                   ChangeTerminal
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  EFI_DEVICE_PATH_PROTOCOL  *Node1;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+  UART_DEVICE_PATH          *Uart;
+  UART_DEVICE_PATH          *Uart1;
+  UINTN                     Com;
+  BM_TERMINAL_CONTEXT       *NewTerminalContext;
+  BM_MENU_ENTRY             *NewMenuEntry;
+
+  Node  = DevicePath;
+  Node  = NextDevicePathNode (Node);
+  Com   = 0;
+  while (!IsDevicePathEnd (Node)) {
+    Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+    if (IsIsaSerialNode (Acpi)) {
+      CopyMem (&Com, &Acpi->UID, sizeof (UINT32));
+    }
+
+    NewMenuEntry = BOpt_GetMenuEntry (&TerminalMenu, Com);
+
+    NewTerminalContext = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+    if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node) == MSG_UART_DP)) {
+      Uart = (UART_DEVICE_PATH *) Node;
+      CopyMem (
+        &Uart->BaudRate,
+        &NewTerminalContext->BaudRate,
+        sizeof (UINT64)
+        );
+
+      CopyMem (
+        &Uart->DataBits,
+        &NewTerminalContext->DataBits,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &Uart->Parity,
+        &NewTerminalContext->Parity,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &Uart->StopBits,
+        &NewTerminalContext->StopBits,
+        sizeof (UINT8)
+        );
+      //
+      // Change the device path in the ComPort
+      //
+      if (ChangeTerminal) {
+        Node1 = NewTerminalContext->DevicePath;
+        Node1 = NextDevicePathNode (Node1);
+        while (!IsDevicePathEnd (Node1)) {
+          if ((DevicePathType (Node1) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node1) == MSG_UART_DP)) {
+            Uart1 = (UART_DEVICE_PATH *) Node1;
+            CopyMem (
+              &Uart1->BaudRate,
+              &NewTerminalContext->BaudRate,
+              sizeof (UINT64)
+              );
+
+            CopyMem (
+              &Uart1->DataBits,
+              &NewTerminalContext->DataBits,
+              sizeof (UINT8)
+              );
+
+            CopyMem (
+              &Uart1->Parity,
+              &NewTerminalContext->Parity,
+              sizeof (UINT8)
+              );
+
+            CopyMem (
+              &Uart1->StopBits,
+              &NewTerminalContext->StopBits,
+              sizeof (UINT8)
+              );
+            break;
+          }
+          //
+          // end if
+          //
+          Node1 = NextDevicePathNode (Node1);
+        }
+        //
+        // end while
+        //
+        break;
+      }
+    }
+
+    Node = NextDevicePathNode (Node);
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Update the device path that describing a terminal device
+  based on the new BaudRate, Data Bits, parity and Stop Bits
+  set.
+
+  @param DevicePath terminal device's path
+
+**/
+VOID
+ChangeVariableDevicePath (
+  IN OUT EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+  UART_DEVICE_PATH          *Uart;
+  UINTN                     Com;
+  BM_TERMINAL_CONTEXT       *NewTerminalContext;
+  BM_MENU_ENTRY             *NewMenuEntry;
+
+  Node  = DevicePath;
+  Node  = NextDevicePathNode (Node);
+  Com   = 0;
+  while (!IsDevicePathEnd (Node)) {
+    Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+    if (IsIsaSerialNode (Acpi)) {
+      CopyMem (&Com, &Acpi->UID, sizeof (UINT32));
+    }
+
+    if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node) == MSG_UART_DP)) {
+      NewMenuEntry = BOpt_GetMenuEntry (
+                      &TerminalMenu,
+                      Com
+                      );
+      ASSERT (NewMenuEntry != NULL);
+      NewTerminalContext  = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+      Uart                = (UART_DEVICE_PATH *) Node;
+      CopyMem (
+        &Uart->BaudRate,
+        &NewTerminalContext->BaudRate,
+        sizeof (UINT64)
+        );
+
+      CopyMem (
+        &Uart->DataBits,
+        &NewTerminalContext->DataBits,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &Uart->Parity,
+        &NewTerminalContext->Parity,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &Uart->StopBits,
+        &NewTerminalContext->StopBits,
+        sizeof (UINT8)
+        );
+    }
+
+    Node = NextDevicePathNode (Node);
+  }
+}
+
+/**
+  Retrieve ACPI UID of UART from device path
+
+  @param Handle          The handle for the UART device.
+  @param AcpiUid         The ACPI UID on output.
+
+  @retval  TRUE   Find valid UID from device path
+  @retval  FALSE  Can't find
+
+**/
+BOOLEAN
+RetrieveUartUid (
+  IN EFI_HANDLE   Handle,
+  IN OUT UINT32   *AcpiUid
+  )
+{
+  EFI_STATUS                Status;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  Status = gBS->HandleProtocol (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  Acpi = NULL;
+  for (; !IsDevicePathEnd (DevicePath); DevicePath = NextDevicePathNode (DevicePath)) {
+    if ((DevicePathType (DevicePath) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (DevicePath) == MSG_UART_DP)) {
+      break;
+    }
+    //
+    // Acpi points to the node before the Uart node
+    //
+    Acpi = (ACPI_HID_DEVICE_PATH *) DevicePath;
+  }
+
+  if ((Acpi != NULL) && IsIsaSerialNode (Acpi)) {
+    if (AcpiUid != NULL) {
+      CopyMem (AcpiUid, &Acpi->UID, sizeof (UINT32));
+    }
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Sort Uart handles array with Acpi->UID from low to high.
+
+  @param Handles         EFI_SERIAL_IO_PROTOCOL handle buffer
+  @param NoHandles       EFI_SERIAL_IO_PROTOCOL handle count
+**/
+VOID
+SortedUartHandle (
+  IN  EFI_HANDLE *Handles,
+  IN  UINTN      NoHandles
+  )
+{
+  UINTN       Index1;
+  UINTN       Index2;
+  UINTN       Position;
+  UINT32      AcpiUid1;
+  UINT32      AcpiUid2;
+  UINT32      TempAcpiUid;
+  EFI_HANDLE  TempHandle;
+
+  for (Index1 = 0; Index1 < NoHandles-1; Index1++) {
+    if (!RetrieveUartUid (Handles[Index1], &AcpiUid1)) {
+      continue;
+    }
+    TempHandle  = Handles[Index1];
+    Position    = Index1;
+    TempAcpiUid = AcpiUid1;
+
+    for (Index2 = Index1+1; Index2 < NoHandles; Index2++) {
+      if (!RetrieveUartUid (Handles[Index2], &AcpiUid2)) {
+        continue;
+      }
+      if (AcpiUid2 < TempAcpiUid) {
+        TempAcpiUid = AcpiUid2;
+        TempHandle  = Handles[Index2];
+        Position    = Index2;
+      }
+    }
+    Handles[Position] = Handles[Index1];
+    Handles[Index1]   = TempHandle;
+  }
+}
+
+/**
+  Test whether DevicePath is a valid Terminal
+
+
+  @param DevicePath      DevicePath to be checked
+  @param Termi           If DevicePath is valid Terminal, terminal type is returned.
+  @param Com             If DevicePath is valid Terminal, Com Port type is returned.
+
+  @retval  TRUE         If DevicePath point to a Terminal.
+  @retval  FALSE        If DevicePath does not point to a Terminal.
+
+**/
+BOOLEAN
+IsTerminalDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *DevicePath,
+  OUT TYPE_OF_TERMINAL         *Termi,
+  OUT UINTN                    *Com
+  );
+
+/**
+  Build a list containing all serial devices.
+
+
+  @retval EFI_SUCCESS The function complete successfully.
+  @retval EFI_UNSUPPORTED No serial ports present.
+
+**/
+EFI_STATUS
+LocateSerialIo (
+  VOID
+  )
+{
+  UINTN                     Index;
+  UINTN                     Index2;
+  UINTN                     NoHandles;
+  EFI_HANDLE                *Handles;
+  EFI_STATUS                Status;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_SERIAL_IO_PROTOCOL    *SerialIo;
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  EFI_DEVICE_PATH_PROTOCOL  *OutDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *InpDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *ErrDevicePath;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_TERMINAL_CONTEXT       *NewTerminalContext;
+  EFI_DEVICE_PATH_PROTOCOL  *NewDevicePath;
+  VENDOR_DEVICE_PATH        Vendor;
+
+  //
+  // Get all handles that have SerialIo protocol installed
+  //
+  InitializeListHead (&TerminalMenu.Head);
+  TerminalMenu.MenuNumber = 0;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiSerialIoProtocolGuid,
+                  NULL,
+                  &NoHandles,
+                  &Handles
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // No serial ports present
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Sort Uart handles array with Acpi->UID from low to high
+  // then Terminal menu can be built from low Acpi->UID to high Acpi->UID
+  //
+  SortedUartHandle (Handles, NoHandles);
+
+  for (Index = 0; Index < NoHandles; Index++) {
+    //
+    // Check to see whether the handle has DevicePath Protocol installed
+    //
+    gBS->HandleProtocol (
+          Handles[Index],
+          &gEfiDevicePathProtocolGuid,
+          (VOID **) &DevicePath
+          );
+
+    Acpi = NULL;
+    for (Node = DevicePath; !IsDevicePathEnd (Node); Node = NextDevicePathNode (Node)) {
+      if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node) == MSG_UART_DP)) {
+        break;
+      }
+      //
+      // Acpi points to the node before Uart node
+      //
+      Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+    }
+
+    if ((Acpi != NULL) && IsIsaSerialNode (Acpi)) {
+      NewMenuEntry = BOpt_CreateMenuEntry (BM_TERMINAL_CONTEXT_SELECT);
+      if (NewMenuEntry == NULL) {
+        FreePool (Handles);
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      NewTerminalContext = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+      CopyMem (&NewMenuEntry->OptionNumber, &Acpi->UID, sizeof (UINT32));
+      NewTerminalContext->DevicePath = DuplicateDevicePath (DevicePath);
+      //
+      // BugBug: I have no choice, calling EfiLibStrFromDatahub will hang the system!
+      // coz' the misc data for each platform is not correct, actually it's the device path stored in
+      // datahub which is not completed, so a searching for end of device path will enter a
+      // dead-loop.
+      //
+      NewMenuEntry->DisplayString = EfiLibStrFromDatahub (DevicePath);
+      if (NULL == NewMenuEntry->DisplayString) {
+        NewMenuEntry->DisplayString = UiDevicePathToStr (DevicePath);
+      }
+
+      NewMenuEntry->HelpString = NULL;
+
+      gBS->HandleProtocol (
+            Handles[Index],
+            &gEfiSerialIoProtocolGuid,
+            (VOID **) &SerialIo
+            );
+
+      CopyMem (
+        &NewTerminalContext->BaudRate,
+        &SerialIo->Mode->BaudRate,
+        sizeof (UINT64)
+        );
+
+      CopyMem (
+        &NewTerminalContext->DataBits,
+        &SerialIo->Mode->DataBits,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &NewTerminalContext->Parity,
+        &SerialIo->Mode->Parity,
+        sizeof (UINT8)
+        );
+
+      CopyMem (
+        &NewTerminalContext->StopBits,
+        &SerialIo->Mode->StopBits,
+        sizeof (UINT8)
+        );
+      InsertTailList (&TerminalMenu.Head, &NewMenuEntry->Link);
+      TerminalMenu.MenuNumber++;
+    }
+  }
+  if (Handles != NULL) {
+    FreePool (Handles);
+  }
+
+  //
+  // Get L"ConOut", L"ConIn" and L"ErrOut" from the Var
+  //
+  GetEfiGlobalVariable2 (L"ConOut", (VOID**)&OutDevicePath, NULL);
+  GetEfiGlobalVariable2 (L"ConIn", (VOID**)&InpDevicePath, NULL);
+  GetEfiGlobalVariable2 (L"ErrOut", (VOID**)&ErrDevicePath, NULL);
+  if (OutDevicePath != NULL) {
+    UpdateComAttributeFromVariable (OutDevicePath);
+  }
+
+  if (InpDevicePath != NULL) {
+    UpdateComAttributeFromVariable (InpDevicePath);
+  }
+
+  if (ErrDevicePath != NULL) {
+    UpdateComAttributeFromVariable (ErrDevicePath);
+  }
+
+  for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&TerminalMenu, Index);
+    if (NULL == NewMenuEntry) {
+      return EFI_NOT_FOUND;
+    }
+
+    NewTerminalContext                = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+
+    NewTerminalContext->TerminalType  = 0;
+    NewTerminalContext->IsConIn       = FALSE;
+    NewTerminalContext->IsConOut      = FALSE;
+    NewTerminalContext->IsStdErr      = FALSE;
+
+    Vendor.Header.Type                = MESSAGING_DEVICE_PATH;
+    Vendor.Header.SubType             = MSG_VENDOR_DP;
+
+    for (Index2 = 0; Index2 < (sizeof (TerminalTypeGuid) / sizeof (TerminalTypeGuid[0])); Index2++) {
+      CopyMem (&Vendor.Guid, &TerminalTypeGuid[Index2], sizeof (EFI_GUID));
+      SetDevicePathNodeLength (&Vendor.Header, sizeof (VENDOR_DEVICE_PATH));
+      NewDevicePath = AppendDevicePathNode (
+                        NewTerminalContext->DevicePath,
+                        (EFI_DEVICE_PATH_PROTOCOL *) &Vendor
+                        );
+      if (NewMenuEntry->HelpString != NULL) {
+        FreePool (NewMenuEntry->HelpString);
+      }
+      //
+      // NewMenuEntry->HelpString = UiDevicePathToStr (NewDevicePath);
+      // NewMenuEntry->DisplayString = NewMenuEntry->HelpString;
+      //
+      NewMenuEntry->HelpString = NULL;
+
+      if (MatchDevicePaths (OutDevicePath, NewDevicePath)) {
+        NewTerminalContext->IsConOut      = TRUE;
+        NewTerminalContext->TerminalType  = (UINT8) Index2;
+      }
+
+      if (MatchDevicePaths (InpDevicePath, NewDevicePath)) {
+        NewTerminalContext->IsConIn       = TRUE;
+        NewTerminalContext->TerminalType  = (UINT8) Index2;
+      }
+
+      if (MatchDevicePaths (ErrDevicePath, NewDevicePath)) {
+        NewTerminalContext->IsStdErr      = TRUE;
+        NewTerminalContext->TerminalType  = (UINT8) Index2;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update Com Ports attributes from DevicePath
+
+  @param DevicePath      DevicePath that contains Com ports
+
+  @retval EFI_SUCCESS   The update is successful.
+  @retval EFI_NOT_FOUND Can not find specific menu entry
+**/
+EFI_STATUS
+UpdateComAttributeFromVariable (
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  EFI_DEVICE_PATH_PROTOCOL  *SerialNode;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+  UART_DEVICE_PATH          *Uart;
+  UART_DEVICE_PATH          *Uart1;
+  UINTN                     TerminalNumber;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_TERMINAL_CONTEXT       *NewTerminalContext;
+  UINTN                     Index;
+
+  Node            = DevicePath;
+  Node            = NextDevicePathNode (Node);
+  TerminalNumber  = 0;
+  for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+    while (!IsDevicePathEnd (Node)) {
+      Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+      if (IsIsaSerialNode (Acpi)) {
+        CopyMem (&TerminalNumber, &Acpi->UID, sizeof (UINT32));
+      }
+
+      if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node) == MSG_UART_DP)) {
+        Uart          = (UART_DEVICE_PATH *) Node;
+        NewMenuEntry  = BOpt_GetMenuEntry (&TerminalMenu, TerminalNumber);
+        if (NULL == NewMenuEntry) {
+          return EFI_NOT_FOUND;
+        }
+
+        NewTerminalContext = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+        CopyMem (
+          &NewTerminalContext->BaudRate,
+          &Uart->BaudRate,
+          sizeof (UINT64)
+          );
+
+        CopyMem (
+          &NewTerminalContext->DataBits,
+          &Uart->DataBits,
+          sizeof (UINT8)
+          );
+
+        CopyMem (
+          &NewTerminalContext->Parity,
+          &Uart->Parity,
+          sizeof (UINT8)
+          );
+
+        CopyMem (
+          &NewTerminalContext->StopBits,
+          &Uart->StopBits,
+          sizeof (UINT8)
+          );
+
+        SerialNode  = NewTerminalContext->DevicePath;
+        SerialNode  = NextDevicePathNode (SerialNode);
+        while (!IsDevicePathEnd (SerialNode)) {
+          if ((DevicePathType (SerialNode) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (SerialNode) == MSG_UART_DP)) {
+            //
+            // Update following device paths according to
+            // previous acquired uart attributes
+            //
+            Uart1 = (UART_DEVICE_PATH *) SerialNode;
+            CopyMem (
+              &Uart1->BaudRate,
+              &NewTerminalContext->BaudRate,
+              sizeof (UINT64)
+              );
+
+            CopyMem (
+              &Uart1->DataBits,
+              &NewTerminalContext->DataBits,
+              sizeof (UINT8)
+              );
+            CopyMem (
+              &Uart1->Parity,
+              &NewTerminalContext->Parity,
+              sizeof (UINT8)
+              );
+            CopyMem (
+              &Uart1->StopBits,
+              &NewTerminalContext->StopBits,
+              sizeof (UINT8)
+              );
+
+            break;
+          }
+
+          SerialNode = NextDevicePathNode (SerialNode);
+        }
+        //
+        // end while
+        //
+      }
+
+      Node = NextDevicePathNode (Node);
+    }
+    //
+    // end while
+    //
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Build up Console Menu based on types passed in. The type can
+  be BM_CONSOLE_IN_CONTEXT_SELECT, BM_CONSOLE_OUT_CONTEXT_SELECT
+  and BM_CONSOLE_ERR_CONTEXT_SELECT.
+
+  @param ConsoleMenuType Can be BM_CONSOLE_IN_CONTEXT_SELECT, BM_CONSOLE_OUT_CONTEXT_SELECT
+                         and BM_CONSOLE_ERR_CONTEXT_SELECT.
+
+  @retval EFI_UNSUPPORTED The type passed in is not in the 3 types defined.
+  @retval EFI_NOT_FOUND   If the EFI Variable defined in UEFI spec with name "ConOutDev", 
+                          "ConInDev" or "ConErrDev" doesn't exists.
+  @retval EFI_OUT_OF_RESOURCES Not enough resource to complete the operations.
+  @retval EFI_SUCCESS          Function completes successfully.
+
+**/
+EFI_STATUS
+GetConsoleMenu (
+  IN UINTN              ConsoleMenuType
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *AllDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *MultiDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathInst;
+  UINTN                     Size;
+  UINTN                     AllCount;
+  UINTN                     Index;
+  UINTN                     Index2;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_CONSOLE_CONTEXT        *NewConsoleContext;
+  TYPE_OF_TERMINAL          Terminal;
+  UINTN                     Com;
+  BM_MENU_OPTION            *ConsoleMenu;
+
+  DevicePath    = NULL;
+  AllDevicePath = NULL;
+  AllCount      = 0;
+  switch (ConsoleMenuType) {
+  case BM_CONSOLE_IN_CONTEXT_SELECT:
+    ConsoleMenu   = &ConsoleInpMenu;
+    GetEfiGlobalVariable2 (L"ConIn", (VOID**)&DevicePath, NULL);
+    GetEfiGlobalVariable2 (L"ConInDev", (VOID**)&AllDevicePath, NULL);
+    break;
+
+  case BM_CONSOLE_OUT_CONTEXT_SELECT:
+    ConsoleMenu   = &ConsoleOutMenu;
+    GetEfiGlobalVariable2 (L"ConOut", (VOID**)&DevicePath, NULL);
+    GetEfiGlobalVariable2 (L"ConOutDev", (VOID**)&AllDevicePath, NULL);
+    break;
+
+  case BM_CONSOLE_ERR_CONTEXT_SELECT:
+    ConsoleMenu   = &ConsoleErrMenu;
+    GetEfiGlobalVariable2 (L"ErrOut", (VOID**)&DevicePath, NULL);
+    GetEfiGlobalVariable2 (L"ErrOutDev", (VOID**)&AllDevicePath, NULL);
+    break;
+
+  default:
+    return EFI_UNSUPPORTED;
+  }
+
+  if (NULL == AllDevicePath) {
+    return EFI_NOT_FOUND;
+  }
+
+  InitializeListHead (&ConsoleMenu->Head);
+
+  AllCount                = EfiDevicePathInstanceCount (AllDevicePath);
+  ConsoleMenu->MenuNumber = 0;
+  //
+  // Following is menu building up for Console Devices selected.
+  //
+  MultiDevicePath = AllDevicePath;
+  Index2          = 0;
+  for (Index = 0; Index < AllCount; Index++) {
+    DevicePathInst  = GetNextDevicePathInstance (&MultiDevicePath, &Size);
+
+    NewMenuEntry    = BOpt_CreateMenuEntry (BM_CONSOLE_CONTEXT_SELECT);
+    if (NULL == NewMenuEntry) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NewConsoleContext             = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;
+    NewMenuEntry->OptionNumber    = Index2;
+
+    NewConsoleContext->DevicePath = DuplicateDevicePath (DevicePathInst);
+    ASSERT (NewConsoleContext->DevicePath != NULL);
+    NewMenuEntry->DisplayString   = EfiLibStrFromDatahub (NewConsoleContext->DevicePath);
+    if (NULL == NewMenuEntry->DisplayString) {
+      NewMenuEntry->DisplayString = UiDevicePathToStr (NewConsoleContext->DevicePath);
+    }
+
+    NewConsoleContext->IsTerminal = IsTerminalDevicePath (
+                                      NewConsoleContext->DevicePath,
+                                      &Terminal,
+                                      &Com
+                                      );
+
+    NewConsoleContext->IsActive = MatchDevicePaths (
+                                    DevicePath,
+                                    NewConsoleContext->DevicePath
+                                    );
+
+    if (NewConsoleContext->IsTerminal) {
+      BOpt_DestroyMenuEntry (NewMenuEntry);
+    } else {
+      Index2++;
+      ConsoleMenu->MenuNumber++;
+      InsertTailList (&ConsoleMenu->Head, &NewMenuEntry->Link);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Build up ConsoleOutMenu, ConsoleInpMenu and ConsoleErrMenu
+
+  @retval EFI_SUCCESS    The function always complete successfully.
+
+**/
+EFI_STATUS
+GetAllConsoles (
+  VOID
+  )
+{
+  GetConsoleMenu (BM_CONSOLE_IN_CONTEXT_SELECT);
+  GetConsoleMenu (BM_CONSOLE_OUT_CONTEXT_SELECT);
+  GetConsoleMenu (BM_CONSOLE_ERR_CONTEXT_SELECT);
+  return EFI_SUCCESS;
+}
+
+/**
+  Free ConsoleOutMenu, ConsoleInpMenu and ConsoleErrMenu
+
+  @retval EFI_SUCCESS    The function always complete successfully.
+**/
+EFI_STATUS
+FreeAllConsoles (
+  VOID
+  )
+{
+  BOpt_FreeMenu (&ConsoleOutMenu);
+  BOpt_FreeMenu (&ConsoleInpMenu);
+  BOpt_FreeMenu (&ConsoleErrMenu);
+  BOpt_FreeMenu (&TerminalMenu);
+  return EFI_SUCCESS;
+}
+
+/**
+  Test whether DevicePath is a valid Terminal
+
+
+  @param DevicePath      DevicePath to be checked
+  @param Termi           If DevicePath is valid Terminal, terminal type is returned.
+  @param Com             If DevicePath is valid Terminal, Com Port type is returned.
+
+  @retval  TRUE         If DevicePath point to a Terminal.
+  @retval  FALSE        If DevicePath does not point to a Terminal.
+
+**/
+BOOLEAN
+IsTerminalDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *DevicePath,
+  OUT TYPE_OF_TERMINAL         *Termi,
+  OUT UINTN                    *Com
+  )
+{
+  BOOLEAN                   IsTerminal;
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  VENDOR_DEVICE_PATH        *Vendor;
+  UART_DEVICE_PATH          *Uart;
+  ACPI_HID_DEVICE_PATH      *Acpi;
+
+  IsTerminal = FALSE;
+
+  Uart   = NULL;
+  Vendor = NULL;
+  Acpi   = NULL;
+  for (Node = DevicePath; !IsDevicePathEnd (Node); Node = NextDevicePathNode (Node)) {
+    //
+    // Vendor points to the node before the End node
+    //
+    Vendor = (VENDOR_DEVICE_PATH *) Node;
+
+    if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) && (DevicePathSubType (Node) == MSG_UART_DP)) {
+      Uart = (UART_DEVICE_PATH *) Node;
+    }
+
+    if (Uart == NULL) {
+      //
+      // Acpi points to the node before the UART node
+      //
+      Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+    }
+  }
+
+  if (Vendor == NULL ||
+      DevicePathType (Vendor) != MESSAGING_DEVICE_PATH ||
+      DevicePathSubType (Vendor) != MSG_VENDOR_DP ||
+      Uart == NULL) {
+    return FALSE;
+  }
+
+  //
+  // There are four kinds of Terminal types
+  // check to see whether this devicepath
+  // is one of that type
+  //
+  if (CompareGuid (&Vendor->Guid, &TerminalTypeGuid[0])) {
+    *Termi      = TerminalTypePcAnsi;
+    IsTerminal  = TRUE;
+  } else {
+    if (CompareGuid (&Vendor->Guid, &TerminalTypeGuid[1])) {
+      *Termi      = TerminalTypeVt100;
+      IsTerminal  = TRUE;
+    } else {
+      if (CompareGuid (&Vendor->Guid, &TerminalTypeGuid[2])) {
+        *Termi      = TerminalTypeVt100Plus;
+        IsTerminal  = TRUE;
+      } else {
+        if (CompareGuid (&Vendor->Guid, &TerminalTypeGuid[3])) {
+          *Termi      = TerminalTypeVtUtf8;
+          IsTerminal  = TRUE;
+        } else {
+          if (CompareGuid (&Vendor->Guid, &TerminalTypeGuid[4])) {
+            *Termi      = TerminalTypeTtyTerm;
+            IsTerminal  = TRUE;
+          } else {
+            IsTerminal = FALSE;
+          }
+        }
+      }
+    }
+  }
+
+  if (!IsTerminal) {
+    return FALSE;
+  }
+
+  if ((Acpi != NULL) && IsIsaSerialNode (Acpi)) {
+    CopyMem (Com, &Acpi->UID, sizeof (UINT32));
+  } else {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Get mode number according to column and row
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+GetConsoleOutMode (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  UINTN                         Col;
+  UINTN                         Row;
+  UINTN                         CurrentCol;
+  UINTN                         CurrentRow;
+  UINTN                         Mode;
+  UINTN                         MaxMode;
+  EFI_STATUS                    Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *ConOut;
+
+  ConOut   = gST->ConOut;
+  MaxMode  = (UINTN) (ConOut->Mode->MaxMode);
+
+  CurrentCol = PcdGet32 (PcdSetupConOutColumn);
+  CurrentRow = PcdGet32 (PcdSetupConOutRow);
+  for (Mode = 0; Mode < MaxMode; Mode++) {
+    Status = ConOut->QueryMode (ConOut, Mode, &Col, &Row);
+    if (!EFI_ERROR(Status)) {
+      if (CurrentCol == Col && CurrentRow == Row) {
+        CallbackData->BmmFakeNvData.ConsoleOutMode = (UINT16) Mode;
+        break;
+      }
+    }
+  }
+}
+
+/**
+
+  Initialize console input device check box to ConsoleInCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/  
+VOID  
+GetConsoleInCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  UINT16              Index;
+  BM_MENU_ENTRY       *NewMenuEntry; 
+  UINT8               *ConInCheck;
+  BM_CONSOLE_CONTEXT  *NewConsoleContext;
+
+  ASSERT (CallbackData != NULL);
+
+  ConInCheck = &CallbackData->BmmFakeNvData.ConsoleInCheck[0];
+  for (Index = 0; ((Index < ConsoleInpMenu.MenuNumber) && \
+       (Index < MAX_MENU_NUMBER)) ; Index++) {  
+    NewMenuEntry      = BOpt_GetMenuEntry (&ConsoleInpMenu, Index);
+    NewConsoleContext = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;  
+    ConInCheck[Index] = NewConsoleContext->IsActive;
+  }
+}
+
+/**
+
+  Initialize console output device check box to ConsoleOutCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/      
+VOID    
+GetConsoleOutCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  UINT16              Index;
+  BM_MENU_ENTRY       *NewMenuEntry; 
+  UINT8               *ConOutCheck;
+  BM_CONSOLE_CONTEXT  *NewConsoleContext;
+  
+  ASSERT (CallbackData != NULL);
+  ConOutCheck = &CallbackData->BmmFakeNvData.ConsoleOutCheck[0];
+  for (Index = 0; ((Index < ConsoleOutMenu.MenuNumber) && \
+       (Index < MAX_MENU_NUMBER)) ; Index++) {  
+    NewMenuEntry      = BOpt_GetMenuEntry (&ConsoleOutMenu, Index);
+    NewConsoleContext = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;  
+    ConOutCheck[Index] = NewConsoleContext->IsActive;
+  }
+}
+
+/**
+
+  Initialize standard error output device check box to ConsoleErrCheck[MAX_MENU_NUMBER]
+  in BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/        
+VOID  
+GetConsoleErrCheck (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  UINT16              Index;
+  BM_MENU_ENTRY       *NewMenuEntry; 
+  UINT8               *ConErrCheck;
+  BM_CONSOLE_CONTEXT  *NewConsoleContext;
+
+  ASSERT (CallbackData != NULL);
+  ConErrCheck = &CallbackData->BmmFakeNvData.ConsoleErrCheck[0];
+  for (Index = 0; ((Index < ConsoleErrMenu.MenuNumber) && \
+       (Index < MAX_MENU_NUMBER)) ; Index++) {  
+    NewMenuEntry      = BOpt_GetMenuEntry (&ConsoleErrMenu, Index);
+    NewConsoleContext = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;  
+    ConErrCheck[Index] = NewConsoleContext->IsActive;
+  }
+}
+
+/**
+
+  Initialize terminal attributes (baudrate, data rate, stop bits, parity and terminal type)
+  to BMM_FAKE_NV_DATA structure.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID  
+GetTerminalAttribute (
+  IN  BMM_CALLBACK_DATA    *CallbackData
+  )
+{
+  BMM_FAKE_NV_DATA     *CurrentFakeNVMap;
+  BM_MENU_ENTRY        *NewMenuEntry;
+  BM_TERMINAL_CONTEXT  *NewTerminalContext;    
+  UINT16               TerminalIndex;  
+  UINT8                AttributeIndex;
+
+  ASSERT (CallbackData != NULL);
+
+  CurrentFakeNVMap = &CallbackData->BmmFakeNvData;     
+  for (TerminalIndex = 0; ((TerminalIndex < TerminalMenu.MenuNumber) && \
+       (TerminalIndex < MAX_MENU_NUMBER)); TerminalIndex++) {  
+    NewMenuEntry        = BOpt_GetMenuEntry (&TerminalMenu, TerminalIndex);
+    NewTerminalContext  = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+    for (AttributeIndex = 0; AttributeIndex < sizeof (BaudRateList) / sizeof (BaudRateList [0]); AttributeIndex++) {
+      if (NewTerminalContext->BaudRate == (UINT64) (BaudRateList[AttributeIndex].Value)) {
+        NewTerminalContext->BaudRateIndex = AttributeIndex;
+        break;
+      }
+    }
+    for (AttributeIndex = 0; AttributeIndex < sizeof (DataBitsList) / sizeof (DataBitsList[0]); AttributeIndex++) {
+      if (NewTerminalContext->DataBits == (UINT64) (DataBitsList[AttributeIndex].Value)) {
+        NewTerminalContext->DataBitsIndex = AttributeIndex;
+        break;
+      }
+    }    
+
+    for (AttributeIndex = 0; AttributeIndex < sizeof (ParityList) / sizeof (ParityList[0]); AttributeIndex++) {
+      if (NewTerminalContext->Parity == (UINT64) (ParityList[AttributeIndex].Value)) {
+        NewTerminalContext->ParityIndex = AttributeIndex;
+        break;
+      }
+    }
+
+    for (AttributeIndex = 0; AttributeIndex < sizeof (StopBitsList) / sizeof (StopBitsList[0]); AttributeIndex++) {
+      if (NewTerminalContext->StopBits == (UINT64) (StopBitsList[AttributeIndex].Value)) {
+        NewTerminalContext->StopBitsIndex = AttributeIndex;
+        break;
+      }
+    }
+    CurrentFakeNVMap->COMBaudRate[TerminalIndex]     = NewTerminalContext->BaudRateIndex;
+    CurrentFakeNVMap->COMDataRate[TerminalIndex]     = NewTerminalContext->DataBitsIndex;
+    CurrentFakeNVMap->COMStopBits[TerminalIndex]     = NewTerminalContext->StopBitsIndex;
+    CurrentFakeNVMap->COMParity[TerminalIndex]       = NewTerminalContext->ParityIndex; 
+    CurrentFakeNVMap->COMTerminalType[TerminalIndex] = NewTerminalContext->TerminalType;
+    CurrentFakeNVMap->COMFlowControl[TerminalIndex]  = NewTerminalContext->FlowControl;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/Data.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/Data.c
new file mode 100644
index 0000000000..4f05ac5fcc
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/Data.c
@@ -0,0 +1,289 @@
+/** @file
+  Define some data used for Boot Maint
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+VOID                *mStartOpCodeHandle = NULL;
+VOID                *mEndOpCodeHandle = NULL;
+EFI_IFR_GUID_LABEL  *mStartLabel = NULL;
+EFI_IFR_GUID_LABEL  *mEndLabel = NULL;
+
+STRING_DEPOSITORY   *FileOptionStrDepository;
+STRING_DEPOSITORY   *ConsoleOptionStrDepository;
+STRING_DEPOSITORY   *BootOptionStrDepository;
+STRING_DEPOSITORY   *BootOptionHelpStrDepository;
+STRING_DEPOSITORY   *DriverOptionStrDepository;
+STRING_DEPOSITORY   *DriverOptionHelpStrDepository;
+STRING_DEPOSITORY   *TerminalStrDepository;
+
+///
+/// Terminal type string token storage
+///
+UINT16              TerminalType[] = {
+  STRING_TOKEN(STR_COM_TYPE_0),
+  STRING_TOKEN(STR_COM_TYPE_1),
+  STRING_TOKEN(STR_COM_TYPE_2),
+  STRING_TOKEN(STR_COM_TYPE_3),
+  STRING_TOKEN(STR_COM_TYPE_4),
+};
+
+///
+/// Flow Control type string token storage
+///
+UINT16              mFlowControlType[2] = {
+  STRING_TOKEN(STR_NONE_FLOW_CONTROL),
+  STRING_TOKEN(STR_HARDWARE_FLOW_CONTROL)
+};
+
+UINT32              mFlowControlValue[2] = {
+  0,
+  UART_FLOW_CONTROL_HARDWARE
+};
+
+///
+/// File system selection menu
+///
+BM_MENU_OPTION      FsOptionMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Console Input Device Selection Menu
+///
+BM_MENU_OPTION      ConsoleInpMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Console Output Device Selection Menu
+///
+BM_MENU_OPTION      ConsoleOutMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Error Output Device Selection Menu
+///
+BM_MENU_OPTION      ConsoleErrMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Boot Option from variable Menu
+///
+BM_MENU_OPTION      BootOptionMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Driver Option from variable menu
+///
+BM_MENU_OPTION      DriverOptionMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Files and sub-directories in current directory menu
+///
+BM_MENU_OPTION      DirectoryMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Handles in current system selection menu
+///
+BM_MENU_OPTION      DriverMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+BM_MENU_OPTION      TerminalMenu = {
+  BM_MENU_OPTION_SIGNATURE,
+  {NULL},
+  0
+};
+
+///
+/// Value and string token correspondency for BaudRate
+///
+COM_ATTR            BaudRateList[19] = {
+  {
+    115200,
+    STRING_TOKEN(STR_COM_BAUD_RATE_0)
+  },
+  {
+    57600,
+    STRING_TOKEN(STR_COM_BAUD_RATE_1)
+  },
+  {
+    38400,
+    STRING_TOKEN(STR_COM_BAUD_RATE_2)
+  },
+  {
+    19200,
+    STRING_TOKEN(STR_COM_BAUD_RATE_3)
+  },
+  {
+    9600,
+    STRING_TOKEN(STR_COM_BAUD_RATE_4)
+  },
+  {
+    7200,
+    STRING_TOKEN(STR_COM_BAUD_RATE_5)
+  },
+  {
+    4800,
+    STRING_TOKEN(STR_COM_BAUD_RATE_6)
+  },
+  {
+    3600,
+    STRING_TOKEN(STR_COM_BAUD_RATE_7)
+  },
+  {
+    2400,
+    STRING_TOKEN(STR_COM_BAUD_RATE_8)
+  },
+  {
+    2000,
+    STRING_TOKEN(STR_COM_BAUD_RATE_9)
+  },
+  {
+    1800,
+    STRING_TOKEN(STR_COM_BAUD_RATE_10)
+  },
+  {
+    1200,
+    STRING_TOKEN(STR_COM_BAUD_RATE_11)
+  },
+  {
+    600,
+    STRING_TOKEN(STR_COM_BAUD_RATE_12)
+  },
+  {
+    300,
+    STRING_TOKEN(STR_COM_BAUD_RATE_13)
+  },
+  {
+    150,
+    STRING_TOKEN(STR_COM_BAUD_RATE_14)
+  },
+  {
+    134,
+    STRING_TOKEN(STR_COM_BAUD_RATE_15)
+  },
+  {
+    110,
+    STRING_TOKEN(STR_COM_BAUD_RATE_16)
+  },
+  {
+    75,
+    STRING_TOKEN(STR_COM_BAUD_RATE_17)
+  },
+  {
+    50,
+    STRING_TOKEN(STR_COM_BAUD_RATE_18)
+  }
+};
+
+///
+/// Value and string token correspondency for DataBits
+///
+COM_ATTR            DataBitsList[4] = {
+  {
+    5,
+    STRING_TOKEN(STR_COM_DATA_BITS_0)
+  },
+  {
+    6,
+    STRING_TOKEN(STR_COM_DATA_BITS_1)
+  },
+  {
+    7,
+    STRING_TOKEN(STR_COM_DATA_BITS_2)
+  },
+  {
+    8,
+    STRING_TOKEN(STR_COM_DATA_BITS_3)
+  }
+};
+
+///
+/// Value and string token correspondency for Parity
+///
+COM_ATTR            ParityList[5] = {
+  {
+    NoParity,
+    STRING_TOKEN(STR_COM_PAR_0)
+  },
+  {
+    EvenParity,
+    STRING_TOKEN(STR_COM_PAR_1)
+  },
+  {
+    OddParity,
+    STRING_TOKEN(STR_COM_PAR_2)
+  },
+  {
+    MarkParity,
+    STRING_TOKEN(STR_COM_PAR_3)
+  },
+  {
+    SpaceParity,
+    STRING_TOKEN(STR_COM_PAR_4)
+  }
+};
+
+///
+/// Value and string token correspondency for Baudreate
+///
+COM_ATTR            StopBitsList[3] = {
+  {
+    OneStopBit,
+    STRING_TOKEN(STR_COM_STOP_BITS_0)
+  },
+  {
+    OneFiveStopBits,
+    STRING_TOKEN(STR_COM_STOP_BITS_1)
+  },
+  {
+    TwoStopBits,
+    STRING_TOKEN(STR_COM_STOP_BITS_2)
+  }
+};
+
+///
+/// Guid for messaging path, used in Serial port setting.
+///
+EFI_GUID            TerminalTypeGuid[] = {
+  DEVICE_PATH_MESSAGING_PC_ANSI,
+  DEVICE_PATH_MESSAGING_VT_100,
+  DEVICE_PATH_MESSAGING_VT_100_PLUS,
+  DEVICE_PATH_MESSAGING_VT_UTF8,
+  EFI_TTY_TERM_GUID
+};
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/FE.vfr b/Core/MdeModulePkg/Application/UiApp/BootMaint/FE.vfr
new file mode 100644
index 0000000000..1401d21721
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/FE.vfr
@@ -0,0 +1,131 @@
+///** @file
+//  
+//    File Explorer Formset
+//  
+//  Copyright (c) 2004 - 2015, 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 "FormGuid.h"
+
+formset
+  guid = FILE_EXPLORE_FORMSET_GUID,
+  title = STRING_TOKEN(STR_FILE_EXPLORER_TITLE),
+  help = STRING_TOKEN(STR_NULL_STRING),
+  classguid = FILE_EXPLORE_FORMSET_GUID,
+
+  varstore FILE_EXPLORER_NV_DATA,
+    varid = VARSTORE_ID_BOOT_MAINT,
+    name = FeData,
+    guid = FILE_EXPLORE_FORMSET_GUID;
+
+  form formid = FORM_FILE_EXPLORER_ID,
+       title = STRING_TOKEN(STR_FILE_EXPLORER_TITLE);
+
+       label FORM_FILE_EXPLORER_ID;
+       label LABEL_END;
+  endform;
+
+  form formid = FORM_BOOT_ADD_DESCRIPTION_ID,
+       title = STRING_TOKEN(STR_FORM_BOOT_ADD_DESC_TITLE);
+
+       label FORM_BOOT_ADD_DESCRIPTION_ID;
+       label LABEL_END;
+
+       subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+       string    varid    = FeData.BootDescriptionData,
+                 questionid = KEY_VALUE_BOOT_DESCRIPTION,
+                 prompt   = STRING_TOKEN(STR_LOAD_OPTION_DESC),
+                 help     = STRING_TOKEN(STR_NULL_STRING),
+                 flags    = INTERACTIVE,
+                 minsize  = 6,
+                 maxsize  = 75,
+       endstring;
+
+       string    varid    = FeData.BootOptionalData,
+                 questionid = KEY_VALUE_BOOT_OPTION,
+                 prompt   = STRING_TOKEN(STR_OPTIONAL_DATA),
+                 help     = STRING_TOKEN(STR_NULL_STRING),
+                 flags    = INTERACTIVE,
+                 minsize  = 0,
+                 maxsize  = 120,
+       endstring;
+
+       subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+       text
+         help   = STRING_TOKEN(STR_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NULL_STRING),
+         flags  = INTERACTIVE,
+         key    = KEY_VALUE_SAVE_AND_EXIT_BOOT;
+
+       text
+         help   = STRING_TOKEN(STR_NO_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NO_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NULL_STRING),
+         flags  = INTERACTIVE,
+         key    = KEY_VALUE_NO_SAVE_AND_EXIT_BOOT;
+
+  endform;
+
+  form formid = FORM_DRIVER_ADD_FILE_DESCRIPTION_ID,
+       title = STRING_TOKEN(STR_FORM_DRV_ADD_DESC_TITLE);
+
+       label FORM_DRIVER_ADD_FILE_DESCRIPTION_ID;
+       label LABEL_END;
+
+       subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+       string    varid    = FeData.DriverDescriptionData,
+                 questionid = KEY_VALUE_DRIVER_DESCRIPTION,
+                 prompt   = STRING_TOKEN(STR_LOAD_OPTION_DESC),
+                 help     = STRING_TOKEN(STR_NULL_STRING),
+                 flags    = INTERACTIVE,
+                 minsize  = 6,
+                 maxsize  = 75,
+       endstring;
+
+       string    varid    = FeData.DriverOptionalData,
+                 questionid = KEY_VALUE_DRIVER_OPTION,
+                 prompt   = STRING_TOKEN(STR_OPTIONAL_DATA),
+                 help     = STRING_TOKEN(STR_NULL_STRING),
+                 flags    = INTERACTIVE,
+                 minsize  = 0,
+                 maxsize  = 120,
+       endstring;
+
+       checkbox varid    = FeData.ForceReconnect,
+               prompt   = STRING_TOKEN(STR_LOAD_OPTION_FORCE_RECON),
+               help     = STRING_TOKEN(STR_LOAD_OPTION_FORCE_RECON),
+               flags    = CHECKBOX_DEFAULT,
+               key      = 0,
+       endcheckbox;
+
+       subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+       text
+         help   = STRING_TOKEN(STR_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NULL_STRING),
+         flags  = INTERACTIVE,
+         key    = KEY_VALUE_SAVE_AND_EXIT_DRIVER;  //BUGBUB: allow duplicate key in one formset???
+
+       text
+         help   = STRING_TOKEN(STR_NO_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NO_SAVE_AND_EXIT),
+         text   = STRING_TOKEN(STR_NULL_STRING),
+         flags  = INTERACTIVE,
+         key    = KEY_VALUE_NO_SAVE_AND_EXIT_DRIVER;
+
+  endform;
+
+endformset;
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/FileExplorer.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/FileExplorer.c
new file mode 100644
index 0000000000..e4a68ef274
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/FileExplorer.c
@@ -0,0 +1,449 @@
+/** @file
+  File explorer related functions.
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+/**
+  Update the File Explore page.
+
+  @param CallbackData    The BMM context data.
+  @param MenuOption      Pointer to menu options to display.
+
+**/
+VOID
+UpdateFileExplorePage (
+  IN BMM_CALLBACK_DATA            *CallbackData,
+  BM_MENU_OPTION                  *MenuOption
+  )
+{
+  UINTN           Index;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_FILE_CONTEXT *NewFileContext;
+  EFI_FORM_ID     FormId;
+
+  NewMenuEntry    = NULL;
+  NewFileContext  = NULL;
+  FormId          = 0;
+
+  RefreshUpdateData ();
+  mStartLabel->Number = FORM_FILE_EXPLORER_ID;
+
+  for (Index = 0; Index < MenuOption->MenuNumber; Index++) {
+    NewMenuEntry    = BOpt_GetMenuEntry (MenuOption, Index);
+    NewFileContext  = (BM_FILE_CONTEXT *) NewMenuEntry->VariableContext;
+
+    if (NewFileContext->IsBootLegacy) {
+      continue;
+    }
+
+    if ((NewFileContext->IsDir) || (FileExplorerStateBootFromFile == CallbackData->FeCurrentState)) {
+      //
+      // Create Text opcode for directory, also create Text opcode for file in FileExplorerStateBootFromFile.
+      //
+      HiiCreateActionOpCode (
+        mStartOpCodeHandle,
+        (UINT16) (FILE_OPTION_OFFSET + Index),
+        NewMenuEntry->DisplayStringToken,
+        STRING_TOKEN (STR_NULL_STRING),
+        EFI_IFR_FLAG_CALLBACK,
+        0
+        );
+    } else {
+      //
+      // Create Goto opcode for file in FileExplorerStateAddBootOption or FileExplorerStateAddDriverOptionState.
+      //
+      if (FileExplorerStateAddBootOption == CallbackData->FeCurrentState) {
+        FormId = FORM_BOOT_ADD_DESCRIPTION_ID;
+      } else if (FileExplorerStateAddDriverOptionState == CallbackData->FeCurrentState) {
+        FormId = FORM_DRIVER_ADD_FILE_DESCRIPTION_ID;
+      }
+
+      HiiCreateGotoOpCode (
+        mStartOpCodeHandle,
+        FormId,
+        NewMenuEntry->DisplayStringToken,
+        STRING_TOKEN (STR_NULL_STRING),
+        EFI_IFR_FLAG_CALLBACK,
+        (UINT16) (FILE_OPTION_OFFSET + Index)
+        );
+    }
+  }
+
+  HiiUpdateForm (
+    CallbackData->FeHiiHandle,
+    &mFileExplorerGuid,
+    FORM_FILE_EXPLORER_ID,
+    mStartOpCodeHandle, // Label FORM_FILE_EXPLORER_ID
+    mEndOpCodeHandle    // LABEL_END
+    );
+}
+
+/**
+  Update the file explower page with the refershed file system.
+
+
+  @param CallbackData    BMM context data
+  @param KeyValue        Key value to identify the type of data to expect.
+
+  @retval  TRUE           Inform the caller to create a callback packet to exit file explorer.
+  @retval  FALSE          Indicate that there is no need to exit file explorer.
+
+**/
+BOOLEAN
+UpdateFileExplorer (
+  IN BMM_CALLBACK_DATA            *CallbackData,
+  IN UINT16                       KeyValue
+  )
+{
+  UINT16          FileOptionMask;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_FILE_CONTEXT *NewFileContext;
+  EFI_FORM_ID     FormId;
+  BOOLEAN         ExitFileExplorer;
+  EFI_STATUS      Status;
+
+  NewMenuEntry      = NULL;
+  NewFileContext    = NULL;
+  ExitFileExplorer  = FALSE;
+
+  FileOptionMask    = (UINT16) (FILE_OPTION_MASK & KeyValue);
+
+  if (FileExplorerDisplayUnknown == CallbackData->FeDisplayContext) {
+    //
+    // First in, display file system.
+    //
+    BOpt_FreeMenu (&FsOptionMenu);
+    BOpt_FindFileSystem (CallbackData);
+    CreateMenuStringToken (CallbackData, CallbackData->FeHiiHandle, &FsOptionMenu);
+
+    UpdateFileExplorePage (CallbackData, &FsOptionMenu);
+
+    CallbackData->FeDisplayContext = FileExplorerDisplayFileSystem;
+  } else {
+    if (FileExplorerDisplayFileSystem == CallbackData->FeDisplayContext) {
+      NewMenuEntry = BOpt_GetMenuEntry (&FsOptionMenu, FileOptionMask);
+    } else if (FileExplorerDisplayDirectory == CallbackData->FeDisplayContext) {
+      NewMenuEntry = BOpt_GetMenuEntry (&DirectoryMenu, FileOptionMask);
+    }
+
+    NewFileContext                  = (BM_FILE_CONTEXT *) NewMenuEntry->VariableContext;
+
+    if (NewFileContext->IsDir ) {
+      CallbackData->FeDisplayContext = FileExplorerDisplayDirectory;
+
+      RemoveEntryList (&NewMenuEntry->Link);
+      BOpt_FreeMenu (&DirectoryMenu);
+      Status = BOpt_FindFiles (CallbackData, NewMenuEntry);
+       if (EFI_ERROR (Status)) {
+         ExitFileExplorer = TRUE;
+         goto exit;
+       }
+      CreateMenuStringToken (CallbackData, CallbackData->FeHiiHandle, &DirectoryMenu);
+      BOpt_DestroyMenuEntry (NewMenuEntry);
+
+      UpdateFileExplorePage (CallbackData, &DirectoryMenu);
+
+    } else {
+      switch (CallbackData->FeCurrentState) {
+      case FileExplorerStateBootFromFile:
+        //
+        // Here boot from file
+        //
+        BootThisFile (NewFileContext);
+        ExitFileExplorer = TRUE;
+        break;
+
+      case FileExplorerStateAddBootOption:
+      case FileExplorerStateAddDriverOptionState:
+        if (FileExplorerStateAddBootOption == CallbackData->FeCurrentState) {
+          FormId = FORM_BOOT_ADD_DESCRIPTION_ID;
+          if (!CallbackData->FeFakeNvData.BootOptionChanged) {
+            ZeroMem (CallbackData->FeFakeNvData.BootDescriptionData, sizeof (CallbackData->FeFakeNvData.BootDescriptionData));
+            ZeroMem (CallbackData->FeFakeNvData.BootOptionalData, sizeof (CallbackData->FeFakeNvData.BootOptionalData));
+          }
+        } else {
+          FormId = FORM_DRIVER_ADD_FILE_DESCRIPTION_ID;
+          if (!CallbackData->FeFakeNvData.DriverOptionChanged) {
+            ZeroMem (CallbackData->FeFakeNvData.DriverDescriptionData, sizeof (CallbackData->FeFakeNvData.DriverDescriptionData));
+            ZeroMem (CallbackData->FeFakeNvData.DriverOptionalData, sizeof (CallbackData->FeFakeNvData.DriverOptionalData));
+          }
+        }
+
+        CallbackData->MenuEntry = NewMenuEntry;
+        CallbackData->LoadContext->FilePathList = ((BM_FILE_CONTEXT *) (CallbackData->MenuEntry->VariableContext))->DevicePath;
+
+        //
+        // Create Subtitle op-code for the display string of the option.
+        //
+        RefreshUpdateData ();
+        mStartLabel->Number = FormId;
+
+        HiiCreateSubTitleOpCode (
+          mStartOpCodeHandle,
+          NewMenuEntry->DisplayStringToken,
+          0,
+          0,
+          0
+          );
+
+        HiiUpdateForm (
+          CallbackData->FeHiiHandle,
+          &mFileExplorerGuid,
+          FormId,
+          mStartOpCodeHandle, // Label FormId
+          mEndOpCodeHandle    // LABEL_END
+          );
+        break;
+
+      default:
+        break;
+      }
+    }
+  }
+  exit:
+  return ExitFileExplorer;
+}
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This                Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration       A null-terminated Unicode string in
+                                  <ConfigString> format.   
+  @param[out] Progress            A pointer to a string filled in with the
+                                  offset of the most recent '&' before the
+                                  first failing name / value pair (or the
+                                  beginn ing of the string if the failure
+                                  is in the first name / value pair) or
+                                  the terminating NULL if all was
+                                  successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are
+                                  awaiting distribution.  
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETERS  Passing in a NULL for the
+                                  Results parameter would result
+                                  in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data
+                                  was not found.
+**/
+EFI_STATUS
+EFIAPI
+FileExplorerRouteConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL *This,
+  IN CONST EFI_STRING                     Configuration,
+  OUT EFI_STRING                          *Progress
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           BufferSize;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL *ConfigRouting;
+  FILE_EXPLORER_NV_DATA           *FeData;
+  BMM_CALLBACK_DATA               *Private;
+
+  if (Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Configuration;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check routing data in <ConfigHdr>.
+  // Note: there is no name for Name/Value storage, only GUID will be checked
+  //
+  if (!HiiIsConfigHdrMatch (Configuration, &mFileExplorerGuid, mFileExplorerStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiConfigRoutingProtocolGuid, 
+                  NULL, 
+                  (VOID**) &ConfigRouting
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private = FE_CALLBACK_DATA_FROM_THIS (This);
+  //
+  // Get Buffer Storage data from EFI variable
+  //
+  BufferSize = sizeof (FILE_EXPLORER_NV_DATA );
+  FeData = &Private->FeFakeNvData;
+
+  //
+  // Convert <ConfigResp> to buffer data by helper function ConfigToBlock()
+  //
+  Status = ConfigRouting->ConfigToBlock (
+                            ConfigRouting,
+                            Configuration,
+                            (UINT8 *) FeData,
+                            &BufferSize,
+                            Progress
+                            );
+  ASSERT_EFI_ERROR (Status);
+
+  if (FeData->BootDescriptionData[0] != 0x00 || FeData->BootOptionalData[0] != 0x00) {
+    Status = Var_UpdateBootOption (Private, FeData);
+    Private->FeFakeNvData.BootOptionChanged = FALSE;
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    BOpt_GetBootOptions (Private);
+    CreateMenuStringToken (Private, Private->FeHiiHandle, &BootOptionMenu);
+  }
+
+  if (FeData->DriverDescriptionData[0] != 0x00 || FeData->DriverOptionalData[0] != 0x00) {
+    Status = Var_UpdateDriverOption (
+              Private,
+              Private->FeHiiHandle,
+              FeData->DriverDescriptionData,
+              FeData->DriverOptionalData,
+              FeData->ForceReconnect
+              );
+    Private->FeFakeNvData.DriverOptionChanged = FALSE;
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    BOpt_GetDriverOptions (Private);
+    CreateMenuStringToken (Private, Private->FeHiiHandle, &DriverOptionMenu);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function processes the results of changes in configuration.
+  When user select a interactive opcode, this callback will be triggered.
+  Based on the Question(QuestionId) that triggers the callback, the corresponding
+  actions is performed. It handles:
+
+  1) the addition of boot option.
+  2) the addition of driver option.
+  3) exit from file browser
+  4) update of file content if a dir is selected.
+  5) boot the file if a file is selected in "boot from file"
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the variable and its data.
+  @retval  EFI_DEVICE_ERROR      The variable could not be saved.
+  @retval  EFI_UNSUPPORTED       The specified Action is not supported by the callback.
+  @retval  EFI_INVALID_PARAMETER If paramter Value or ActionRequest is NULL.
+**/
+EFI_STATUS
+EFIAPI
+FileExplorerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  BMM_CALLBACK_DATA     *Private;
+  FILE_EXPLORER_NV_DATA *NvRamMap;
+  EFI_STATUS            Status;
+
+  if (Action != EFI_BROWSER_ACTION_CHANGING && Action != EFI_BROWSER_ACTION_CHANGED) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Status         = EFI_SUCCESS;
+  Private        = FE_CALLBACK_DATA_FROM_THIS (This);
+
+  //
+  // Retrieve uncommitted data from Form Browser
+  //
+  NvRamMap = &Private->FeFakeNvData;
+  HiiGetBrowserData (&mFileExplorerGuid, mFileExplorerStorageName, sizeof (FILE_EXPLORER_NV_DATA), (UINT8 *) NvRamMap);
+
+  if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    if ((Value == NULL) || (ActionRequest == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (QuestionId == KEY_VALUE_SAVE_AND_EXIT_BOOT){
+      NvRamMap->BootOptionChanged = FALSE;
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_SUBMIT;
+    } else if (QuestionId == KEY_VALUE_SAVE_AND_EXIT_DRIVER){
+      NvRamMap->DriverOptionChanged = FALSE;
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_SUBMIT;
+    } else if (QuestionId == KEY_VALUE_NO_SAVE_AND_EXIT_BOOT) {
+      //
+      // Discard changes and exit formset
+      //
+      NvRamMap->BootOptionalData[0]     = 0x0000;
+      NvRamMap->BootDescriptionData[0]  = 0x0000;
+      NvRamMap->BootOptionChanged = FALSE;
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_EXIT;
+    } else if ( QuestionId == KEY_VALUE_NO_SAVE_AND_EXIT_DRIVER){
+      NvRamMap->BootOptionalData[0]     = 0x0000;
+      NvRamMap->BootDescriptionData[0]  = 0x0000;
+      NvRamMap->DriverOptionChanged = FALSE;
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_EXIT;
+    } else if (QuestionId == KEY_VALUE_BOOT_DESCRIPTION || QuestionId == KEY_VALUE_BOOT_OPTION){
+      NvRamMap->BootOptionChanged = TRUE;
+    } else if (QuestionId == KEY_VALUE_DRIVER_DESCRIPTION || QuestionId == KEY_VALUE_DRIVER_OPTION){
+      NvRamMap->DriverOptionChanged = TRUE;
+    } else if (QuestionId < FILE_OPTION_OFFSET) {
+      //
+      // Exit File Explorer formset
+      //
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_EXIT;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if (Value == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    if (QuestionId >= FILE_OPTION_OFFSET) {
+      UpdateFileExplorer (Private, QuestionId);
+    }
+  }
+
+  //
+  // Pass changed uncommitted data back to Form Browser
+  //
+  HiiSetBrowserData (&mFileExplorerGuid, mFileExplorerStorageName, sizeof (FILE_EXPLORER_NV_DATA), (UINT8 *) NvRamMap, NULL);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/FormGuid.h b/Core/MdeModulePkg/Application/UiApp/BootMaint/FormGuid.h
new file mode 100644
index 0000000000..257c1fdde7
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/FormGuid.h
@@ -0,0 +1,223 @@
+/** @file
+  Formset guids, form id and VarStore data structure for Boot Maintenance Manager.
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+#ifndef _FORM_GUID_H_
+#define _FORM_GUID_H_
+
+#define BOOT_MAINT_FORMSET_GUID \
+  { \
+  0x642237c7, 0x35d4, 0x472d, {0x83, 0x65, 0x12, 0xe0, 0xcc, 0xf2, 0x7a, 0x22} \
+  }
+
+#define FILE_EXPLORE_FORMSET_GUID \
+  { \
+  0x1f2d63e1, 0xfebd, 0x4dc7, {0x9c, 0xc5, 0xba, 0x2b, 0x1c, 0xef, 0x9c, 0x5b} \
+  }
+
+#define FORM_MAIN_ID                         0x1001
+#define FORM_BOOT_ADD_ID                     0x1002
+#define FORM_BOOT_DEL_ID                     0x1003
+#define FORM_BOOT_CHG_ID                     0x1004
+#define FORM_DRV_ADD_ID                      0x1005
+#define FORM_DRV_DEL_ID                      0x1006
+#define FORM_DRV_CHG_ID                      0x1007
+#define FORM_CON_MAIN_ID                     0x1008
+#define FORM_CON_IN_ID                       0x1009
+#define FORM_CON_OUT_ID                      0x100A
+#define FORM_CON_ERR_ID                      0x100B
+#define FORM_FILE_SEEK_ID                    0x100C
+#define FORM_FILE_NEW_SEEK_ID                0x100D
+#define FORM_DRV_ADD_FILE_ID                 0x100E
+#define FORM_DRV_ADD_HANDLE_ID               0x100F
+#define FORM_DRV_ADD_HANDLE_DESC_ID          0x1010
+#define FORM_BOOT_NEXT_ID                    0x1011
+#define FORM_TIME_OUT_ID                     0x1012
+#define FORM_BOOT_SETUP_ID                   0x1014
+#define FORM_DRIVER_SETUP_ID                 0x1015
+#define FORM_BOOT_LEGACY_DEVICE_ID           0x1016
+#define FORM_CON_COM_ID                      0x1017
+#define FORM_CON_COM_SETUP_ID                0x1018
+#define FORM_FILE_EXPLORER_ID                0x101E
+#define FORM_BOOT_ADD_DESCRIPTION_ID         0x101F
+#define FORM_DRIVER_ADD_FILE_DESCRIPTION_ID  0x1020
+#define FORM_CON_MODE_ID                     0x1021
+#define FORM_MEMORY_CHECK_ID                 0x1022
+#define FORM_UEFI_OPTIMIZED_BOOT_ID          0x1023
+
+#define MAXIMUM_FORM_ID                      0x10FF
+
+#define KEY_VALUE_COM_SET_BAUD_RATE          0x1101
+#define KEY_VALUE_COM_SET_DATA_BITS          0x1102
+#define KEY_VALUE_COM_SET_STOP_BITS          0x1103
+#define KEY_VALUE_COM_SET_PARITY             0x1104
+#define KEY_VALUE_COM_SET_TERMI_TYPE         0x1105
+#define KEY_VALUE_MAIN_BOOT_NEXT             0x1106
+#define KEY_VALUE_BOOT_ADD_DESC_DATA         0x1107
+#define KEY_VALUE_BOOT_ADD_OPT_DATA          0x1108
+#define KEY_VALUE_DRIVER_ADD_DESC_DATA       0x1109
+#define KEY_VALUE_DRIVER_ADD_OPT_DATA        0x110A
+#define KEY_VALUE_SAVE_AND_EXIT              0x110B
+#define KEY_VALUE_NO_SAVE_AND_EXIT           0x110C
+#define KEY_VALUE_BOOT_FROM_FILE             0x110D
+#define FORM_RESET                           0x110E
+#define KEY_VALUE_BOOT_DESCRIPTION           0x110F
+#define KEY_VALUE_BOOT_OPTION                0x1110
+#define KEY_VALUE_DRIVER_DESCRIPTION         0x1111
+#define KEY_VALUE_DRIVER_OPTION              0x1112
+
+#define MAXIMUM_NORMAL_KEY_VALUE             0x11FF
+
+//
+// Varstore ID defined for Buffer Storage
+//
+#define VARSTORE_ID_BOOT_MAINT               0x1000
+#define VARSTORE_ID_FILE_EXPLORER            0x1001
+
+//
+// End Label
+//
+#define LABEL_BMM_PLATFORM_INFORMATION       0xfffe
+#define LABEL_END                            0xffff
+#define MAX_MENU_NUMBER                      100
+
+///
+/// This is the structure that will be used to store the
+/// question's current value. Use it at initialize time to
+/// set default value for each question. When using at run
+/// time, this map is returned by the callback function,
+/// so dynamically changing the question's value will be
+/// possible through this mechanism
+///
+typedef struct {
+  //
+  // Three questions displayed at the main page
+  // for Timeout, BootNext, Variables respectively
+  //
+  UINT16  BootTimeOut;
+  UINT16  BootNext;
+
+  //
+  // This is the COM1 Attributes value storage
+  //
+  UINT8   COM1BaudRate;
+  UINT8   COM1DataRate;
+  UINT8   COM1StopBits;
+  UINT8   COM1Parity;
+  UINT8   COM1TerminalType;
+
+  //
+  // This is the COM2 Attributes value storage
+  //
+  UINT8   COM2BaudRate;
+  UINT8   COM2DataRate;
+  UINT8   COM2StopBits;
+  UINT8   COM2Parity;
+  UINT8   COM2TerminalType;
+
+  //
+  // Driver Option Add Handle page storage
+  //
+  UINT16  DriverAddHandleDesc[MAX_MENU_NUMBER];
+  UINT16  DriverAddHandleOptionalData[MAX_MENU_NUMBER];
+  UINT8   DriverAddActive;
+  UINT8   DriverAddForceReconnect;
+
+  //
+  // Console Input/Output/Errorout using COM port check storage
+  //
+  UINT8   ConsoleInputCOM1;
+  UINT8   ConsoleInputCOM2;
+  UINT8   ConsoleOutputCOM1;
+  UINT8   ConsoleOutputCOM2;
+  UINT8   ConsoleErrorCOM1;
+  UINT8   ConsoleErrorCOM2;
+
+  //
+  // At most 100 input/output/errorout device for console storage
+  //
+  UINT8   ConsoleCheck[MAX_MENU_NUMBER];
+  
+  //
+  // At most 100 input/output/errorout device for console storage
+  //
+  UINT8   ConsoleInCheck[MAX_MENU_NUMBER];
+  UINT8   ConsoleOutCheck[MAX_MENU_NUMBER];
+  UINT8   ConsoleErrCheck[MAX_MENU_NUMBER];
+
+  //
+  // Boot or Driver Option Order storage
+  // The value is the OptionNumber+1 because the order list value cannot be 0
+  // Use UINT32 to hold the potential value 0xFFFF+1=0x10000
+  //
+  UINT32  BootOptionOrder[MAX_MENU_NUMBER];
+  UINT32  DriverOptionOrder[MAX_MENU_NUMBER];
+  //
+  // Boot or Driver Option Delete storage
+  //
+  BOOLEAN BootOptionDel[MAX_MENU_NUMBER];
+  BOOLEAN DriverOptionDel[MAX_MENU_NUMBER];
+  BOOLEAN BootOptionDelMark[MAX_MENU_NUMBER];
+  BOOLEAN DriverOptionDelMark[MAX_MENU_NUMBER];
+
+  //
+  // This is the Terminal Attributes value storage
+  //
+  UINT8   COMBaudRate[MAX_MENU_NUMBER];
+  UINT8   COMDataRate[MAX_MENU_NUMBER];
+  UINT8   COMStopBits[MAX_MENU_NUMBER];
+  UINT8   COMParity[MAX_MENU_NUMBER];
+  UINT8   COMTerminalType[MAX_MENU_NUMBER];
+  UINT8   COMFlowControl[MAX_MENU_NUMBER];
+
+  //
+  // We use DisableMap array to record the enable/disable state of each boot device
+  // It should be taken as a bit array, from left to right there are totally 256 bits
+  // the most left one stands for BBS table item 0, and the most right one stands for item 256
+  // If the bit is 1, it means the boot device has been disabled.
+  //
+  UINT8   DisableMap[32];
+
+  //
+  // Console Output Text Mode
+  //
+  UINT16  ConsoleOutMode;
+
+  //
+  //  UINT16                    PadArea[10];
+  //
+} BMM_FAKE_NV_DATA;
+
+//
+// Key used by File Explorer forms
+//
+#define KEY_VALUE_SAVE_AND_EXIT_BOOT           0x1000
+#define KEY_VALUE_NO_SAVE_AND_EXIT_BOOT        0x1001
+#define KEY_VALUE_SAVE_AND_EXIT_DRIVER         0x1002
+#define KEY_VALUE_NO_SAVE_AND_EXIT_DRIVER      0x1003
+
+///
+/// This is the data structure used by File Explorer formset
+///
+typedef struct {
+  UINT16  BootDescriptionData[MAX_MENU_NUMBER];
+  UINT16  BootOptionalData[127];
+  UINT16  DriverDescriptionData[MAX_MENU_NUMBER];
+  UINT16  DriverOptionalData[127];
+  BOOLEAN BootOptionChanged;
+  BOOLEAN DriverOptionChanged;
+  UINT8   Active;
+  UINT8   ForceReconnect;
+} FILE_EXPLORER_NV_DATA;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/UpdatePage.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/UpdatePage.c
new file mode 100644
index 0000000000..73f451c43c
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/UpdatePage.c
@@ -0,0 +1,1243 @@
+/** @file
+Dynamically update the pages.
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+/**
+  Refresh the global UpdateData structure.
+
+**/
+VOID
+RefreshUpdateData (
+  VOID
+  )
+{
+  //
+  // Free current updated date
+  //  
+  if (mStartOpCodeHandle != NULL) {
+    HiiFreeOpCodeHandle (mStartOpCodeHandle);
+  }
+
+  //
+  // Create new OpCode Handle
+  //
+  mStartOpCodeHandle = HiiAllocateOpCodeHandle ();
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  mStartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (mStartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  mStartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+
+}
+
+/**
+  Add a "Go back to main page" tag in front of the form when there are no
+  "Apply changes" and "Discard changes" tags in the end of the form.
+ 
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdatePageStart (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  RefreshUpdateData ();
+  mStartLabel->Number = CallbackData->BmmCurrentPageId;
+
+  if (!(CallbackData->BmmAskSaveOrNot)) {
+    //
+    // Add a "Go back to main page" tag in front of the form when there are no
+    // "Apply changes" and "Discard changes" tags in the end of the form.
+    //
+    HiiCreateGotoOpCode (
+      mStartOpCodeHandle,
+      FORM_MAIN_ID,
+      STRING_TOKEN (STR_FORM_GOTO_MAIN),
+      STRING_TOKEN (STR_FORM_GOTO_MAIN),
+      0,
+      FORM_MAIN_ID
+      );
+  }
+
+}
+
+/**
+  Create the "Apply changes" and "Discard changes" tags. And
+  ensure user can return to the main page.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdatePageEnd (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  //
+  // Create the "Apply changes" and "Discard changes" tags.
+  //
+  if (CallbackData->BmmAskSaveOrNot) {
+    HiiCreateSubTitleOpCode (
+      mStartOpCodeHandle,
+      STRING_TOKEN (STR_NULL_STRING),
+      0,
+      0,
+      0
+      );
+
+    HiiCreateActionOpCode (
+      mStartOpCodeHandle,
+      KEY_VALUE_SAVE_AND_EXIT,
+      STRING_TOKEN (STR_SAVE_AND_EXIT),
+      STRING_TOKEN (STR_NULL_STRING),
+      EFI_IFR_FLAG_CALLBACK,
+      0
+      );
+  }
+
+  //
+  // Ensure user can return to the main page.
+  //
+  HiiCreateActionOpCode (
+    mStartOpCodeHandle,
+    KEY_VALUE_NO_SAVE_AND_EXIT,
+    STRING_TOKEN (STR_NO_SAVE_AND_EXIT),
+    STRING_TOKEN (STR_NULL_STRING),
+    EFI_IFR_FLAG_CALLBACK,
+    0
+    );
+
+  HiiUpdateForm (
+    CallbackData->BmmHiiHandle,
+    &mBootMaintGuid,
+    CallbackData->BmmCurrentPageId,
+    mStartOpCodeHandle, // Label CallbackData->BmmCurrentPageId
+    mEndOpCodeHandle    // LABEL_END
+    );
+}
+
+/**
+  Clean up the dynamic opcode at label and form specified by both LabelId. 
+
+  @param LabelId         It is both the Form ID and Label ID for opcode deletion.
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+CleanUpPage (
+  IN UINT16                           LabelId,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  RefreshUpdateData ();
+
+  //
+  // Remove all op-codes from dynamic page
+  //
+  mStartLabel->Number = LabelId;
+  HiiUpdateForm (
+    CallbackData->BmmHiiHandle,
+    &mBootMaintGuid,
+    LabelId,
+    mStartOpCodeHandle, // Label LabelId
+    mEndOpCodeHandle    // LABEL_END
+    );
+}
+
+/**
+  Boot a file selected by user at File Expoloer of BMM.
+
+  @param FileContext     The file context data, which contains the device path
+                         of the file to be boot from.
+
+  @retval EFI_SUCCESS    The function completed successfull.
+  @return Other value if the boot from the file fails.
+
+**/
+EFI_STATUS
+BootThisFile (
+  IN BM_FILE_CONTEXT                   *FileContext
+  )
+{
+  EFI_BOOT_MANAGER_LOAD_OPTION BootOption;
+
+  EfiBootManagerInitializeLoadOption (
+    &BootOption,
+    0,
+    LoadOptionTypeBoot,
+    LOAD_OPTION_ACTIVE,
+    FileContext->FileName,
+    FileContext->DevicePath,
+    NULL,
+    0
+    );
+  //
+  // Since current no boot from removable media directly is allowed */
+  //
+  gST->ConOut->ClearScreen (gST->ConOut);
+
+  BdsSetConsoleMode (FALSE);
+  EfiBootManagerBoot (&BootOption);
+  BdsSetConsoleMode (TRUE);
+
+  EfiBootManagerFreeLoadOption (&BootOption);
+
+  return BootOption.Status;
+
+}
+
+/**
+  Create a list of Goto Opcode for all terminal devices logged
+  by TerminaMenu. This list will be inserted to form FORM_CON_COM_SETUP_ID.
+
+  @param CallbackData    The BMM context data.
+**/
+VOID
+UpdateConCOMPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY       *NewMenuEntry;
+  UINT16              Index;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&TerminalMenu, Index);
+
+    HiiCreateGotoOpCode (
+      mStartOpCodeHandle,
+      FORM_CON_COM_SETUP_ID,
+      NewMenuEntry->DisplayStringToken,
+      STRING_TOKEN (STR_NULL_STRING),
+      EFI_IFR_FLAG_CALLBACK,
+      (UINT16) (TERMINAL_OPTION_OFFSET + Index)
+      );
+  }
+
+  UpdatePageEnd (CallbackData);
+}
+
+
+/**
+  Create a list of boot option from global BootOptionMenu. It
+  allow user to delete the boot option.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateBootDelPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT16          Index;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+  CreateMenuStringToken (CallbackData, CallbackData->BmmHiiHandle, &BootOptionMenu);
+
+  ASSERT (BootOptionMenu.MenuNumber <= (sizeof (CallbackData->BmmFakeNvData.BootOptionDel) / sizeof (CallbackData->BmmFakeNvData.BootOptionDel[0])));
+  for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry    = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+    NewLoadContext  = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    if (NewLoadContext->IsLegacy) {
+      continue;
+    }
+
+    NewLoadContext->Deleted = FALSE;
+
+    if (CallbackData->BmmFakeNvData.BootOptionDel[Index] && !CallbackData->BmmFakeNvData.BootOptionDelMark[Index]) {
+      //
+      // CallbackData->BmmFakeNvData.BootOptionDel[Index] == TRUE means browser knows this boot option is selected
+      // CallbackData->BmmFakeNvData.BootOptionDelMark[Index] = FALSE means BDS knows the selected boot option has
+      // deleted, browser maintains old useless info. So clear this info here, and later update this info to browser
+      // through HiiSetBrowserData function.
+      //
+      CallbackData->BmmFakeNvData.BootOptionDel[Index] = FALSE;
+    }
+
+    //
+    // Check to see if the current boot option devicepath is the ShellDevice 
+    // path. If it is keep only UEFI Shell in the delete boot option list 
+    // or else continue
+    // 
+    //if ((NULL != NewLoadContext->FilePathList) && (TRUE == IsShellNodeDevicePath(NewLoadContext->FilePathList))) {
+    //  NewLoadContext->Deleted = FALSE;
+    //  CallbackData->BmmFakeNvData.OptionDel[Index] = FALSE;
+
+      HiiCreateCheckBoxOpCode (
+        mStartOpCodeHandle,
+        (EFI_QUESTION_ID) (BOOT_OPTION_DEL_QUESTION_ID + Index),
+        VARSTORE_ID_BOOT_MAINT,
+        (UINT16) (BOOT_OPTION_DEL_VAR_OFFSET + Index),
+        NewMenuEntry->DisplayStringToken,
+        NewMenuEntry->HelpStringToken,
+        EFI_IFR_FLAG_CALLBACK,
+        0,
+        NULL
+        );
+    //} else {
+    //    continue;
+    //}
+  }
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Create a lit of driver option from global DriverMenu.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateDrvAddHandlePage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY *NewMenuEntry;
+  UINT16        Index;
+
+  CallbackData->BmmAskSaveOrNot = FALSE;
+
+  UpdatePageStart (CallbackData);
+
+  for (Index = 0; Index < DriverMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&DriverMenu, Index);
+
+    HiiCreateGotoOpCode (
+      mStartOpCodeHandle,
+      FORM_DRV_ADD_HANDLE_DESC_ID,
+      NewMenuEntry->DisplayStringToken,
+      STRING_TOKEN (STR_NULL_STRING),
+      EFI_IFR_FLAG_CALLBACK,
+      (UINT16) (HANDLE_OPTION_OFFSET + Index)
+      );
+  }
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Create a lit of driver option from global DriverOptionMenu. It
+  allow user to delete the driver option.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateDrvDelPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT16          Index;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  CreateMenuStringToken (CallbackData, CallbackData->BmmHiiHandle, &DriverOptionMenu);
+  
+  ASSERT (DriverOptionMenu.MenuNumber <= (sizeof (CallbackData->BmmFakeNvData.DriverOptionDel) / sizeof (CallbackData->BmmFakeNvData.DriverOptionDel[0])));
+  for (Index = 0; Index < DriverOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry            = BOpt_GetMenuEntry (&DriverOptionMenu, Index);
+
+    NewLoadContext          = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    NewLoadContext->Deleted = FALSE;
+
+    if (CallbackData->BmmFakeNvData.DriverOptionDel[Index] && !CallbackData->BmmFakeNvData.DriverOptionDelMark[Index]) {
+      //
+      // CallbackData->BmmFakeNvData.BootOptionDel[Index] == TRUE means browser knows this boot option is selected
+      // CallbackData->BmmFakeNvData.BootOptionDelMark[Index] = FALSE means BDS knows the selected boot option has
+      // deleted, browser maintains old useless info. So clear this info here, and later update this info to browser
+      // through HiiSetBrowserData function.
+      //
+      CallbackData->BmmFakeNvData.DriverOptionDel[Index] = FALSE;
+    }
+    HiiCreateCheckBoxOpCode (
+      mStartOpCodeHandle,
+      (EFI_QUESTION_ID) (DRIVER_OPTION_DEL_QUESTION_ID + Index),
+      VARSTORE_ID_BOOT_MAINT,
+      (UINT16) (DRIVER_OPTION_DEL_VAR_OFFSET + Index),
+      NewMenuEntry->DisplayStringToken,
+      NewMenuEntry->HelpStringToken,
+      EFI_IFR_FLAG_CALLBACK,
+      0,
+      NULL
+      );
+  }
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Prepare the page to allow user to add description for 
+  a Driver Option.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateDriverAddHandleDescPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY *NewMenuEntry;
+
+  CallbackData->BmmFakeNvData.DriverAddActive          = 0x01;
+  CallbackData->BmmFakeNvData.DriverAddForceReconnect  = 0x00;
+  CallbackData->BmmAskSaveOrNot                        = TRUE;
+  NewMenuEntry = CallbackData->MenuEntry;
+
+  UpdatePageStart (CallbackData);
+
+  HiiCreateSubTitleOpCode (
+    mStartOpCodeHandle,
+    NewMenuEntry->DisplayStringToken,
+    0,
+    0,
+    0
+    );
+
+  HiiCreateStringOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) DRV_ADD_HANDLE_DESC_QUESTION_ID,
+    VARSTORE_ID_BOOT_MAINT,
+    DRV_ADD_HANDLE_DESC_VAR_OFFSET,
+    STRING_TOKEN (STR_LOAD_OPTION_DESC),
+    STRING_TOKEN (STR_NULL_STRING),
+    0,
+    0,
+    6,
+    75,
+    NULL
+    );
+
+  HiiCreateCheckBoxOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) DRV_ADD_RECON_QUESTION_ID,
+    VARSTORE_ID_BOOT_MAINT,
+    DRV_ADD_RECON_VAR_OFFSET,
+    STRING_TOKEN (STR_LOAD_OPTION_FORCE_RECON),
+    STRING_TOKEN (STR_LOAD_OPTION_FORCE_RECON),
+    0,
+    0,
+    NULL
+    );
+
+  HiiCreateStringOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) DRIVER_ADD_OPTION_QUESTION_ID,
+    VARSTORE_ID_BOOT_MAINT,
+    DRIVER_ADD_OPTION_VAR_OFFSET,
+    STRING_TOKEN (STR_OPTIONAL_DATA),
+    STRING_TOKEN (STR_NULL_STRING),
+    0,
+    0,
+    6,
+    75,
+    NULL
+    );
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Update console page.
+
+  @param UpdatePageId    The form ID to be updated.
+  @param ConsoleMenu     The console menu list.
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateConsolePage (
+  IN UINT16                           UpdatePageId,
+  IN BM_MENU_OPTION                   *ConsoleMenu,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY       *NewMenuEntry;
+  BM_CONSOLE_CONTEXT  *NewConsoleContext;
+  BM_TERMINAL_CONTEXT *NewTerminalContext;
+  UINT16              Index;
+  UINT16              Index2;
+  UINT8               CheckFlags;
+  UINT8               *ConsoleCheck;
+  UINT8               *OldConsoleCheck;
+  UINTN               ConsoleCheckSize;
+  EFI_QUESTION_ID     QuestionIdBase;
+  UINT16              VariableOffsetBase;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  ConsoleCheck       = NULL;
+  OldConsoleCheck    = NULL;
+  QuestionIdBase     = 0;
+  VariableOffsetBase = 0;
+  ConsoleCheckSize   = 0;
+
+  switch (UpdatePageId) {
+  case FORM_CON_IN_ID:
+    ConsoleCheck       = &CallbackData->BmmFakeNvData.ConsoleInCheck[0];
+    OldConsoleCheck    = &CallbackData->BmmOldFakeNVData.ConsoleInCheck[0];
+    ConsoleCheckSize   = sizeof (CallbackData->BmmFakeNvData.ConsoleInCheck);
+    QuestionIdBase     = CON_IN_DEVICE_QUESTION_ID;
+    VariableOffsetBase = CON_IN_DEVICE_VAR_OFFSET;
+    break;
+
+  case FORM_CON_OUT_ID:
+    ConsoleCheck       = &CallbackData->BmmFakeNvData.ConsoleOutCheck[0];
+    OldConsoleCheck    = &CallbackData->BmmOldFakeNVData.ConsoleOutCheck[0];
+    ConsoleCheckSize   = sizeof (CallbackData->BmmFakeNvData.ConsoleOutCheck);
+    QuestionIdBase     = CON_OUT_DEVICE_QUESTION_ID;
+    VariableOffsetBase = CON_OUT_DEVICE_VAR_OFFSET;
+    break;
+
+  case FORM_CON_ERR_ID:
+    ConsoleCheck       = &CallbackData->BmmFakeNvData.ConsoleErrCheck[0];
+    OldConsoleCheck    = &CallbackData->BmmOldFakeNVData.ConsoleErrCheck[0];
+    ConsoleCheckSize   = sizeof (CallbackData->BmmFakeNvData.ConsoleErrCheck);
+    QuestionIdBase     = CON_ERR_DEVICE_QUESTION_ID;
+    VariableOffsetBase = CON_ERR_DEVICE_VAR_OFFSET;
+    break;
+  }
+  ASSERT (ConsoleCheck != NULL);
+
+  for (Index = 0; ((Index < ConsoleMenu->MenuNumber) && \
+       (Index < MAX_MENU_NUMBER)) ; Index++) {
+    CheckFlags = 0;
+    NewMenuEntry = BOpt_GetMenuEntry (ConsoleMenu, Index);
+    NewConsoleContext = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;
+    if (NewConsoleContext->IsActive) {
+      CheckFlags |= EFI_IFR_CHECKBOX_DEFAULT;
+      ConsoleCheck[Index] = TRUE;
+    } else {
+      ConsoleCheck[Index] = FALSE;
+    }
+    HiiCreateCheckBoxOpCode (
+      mStartOpCodeHandle,
+      (EFI_QUESTION_ID) (QuestionIdBase + Index),
+      VARSTORE_ID_BOOT_MAINT,
+      (UINT16) (VariableOffsetBase + Index),
+      NewMenuEntry->DisplayStringToken,
+      NewMenuEntry->HelpStringToken,
+      0,
+      CheckFlags,
+      NULL
+      );
+  }
+
+  for (Index2 = 0; ((Index2 < TerminalMenu.MenuNumber) && \
+       (Index2 < MAX_MENU_NUMBER)); Index2++) {
+    CheckFlags          = 0;
+    NewMenuEntry        = BOpt_GetMenuEntry (&TerminalMenu, Index2);
+    NewTerminalContext  = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+
+    ASSERT (Index < MAX_MENU_NUMBER);
+    if (((NewTerminalContext->IsConIn != 0) && (UpdatePageId == FORM_CON_IN_ID)) ||
+        ((NewTerminalContext->IsConOut != 0) && (UpdatePageId == FORM_CON_OUT_ID)) ||
+        ((NewTerminalContext->IsStdErr != 0) && (UpdatePageId == FORM_CON_ERR_ID))
+        ) {
+      CheckFlags |= EFI_IFR_CHECKBOX_DEFAULT;
+      ConsoleCheck[Index] = TRUE;
+    } else {
+      ConsoleCheck[Index] = FALSE;
+    }
+    HiiCreateCheckBoxOpCode (
+      mStartOpCodeHandle,
+      (EFI_QUESTION_ID) (QuestionIdBase + Index),
+      VARSTORE_ID_BOOT_MAINT,
+      (UINT16) (VariableOffsetBase + Index),
+      NewMenuEntry->DisplayStringToken,
+      NewMenuEntry->HelpStringToken,
+      0,
+      CheckFlags,
+      NULL
+      );
+
+    Index++;
+  }
+
+  CopyMem (OldConsoleCheck, ConsoleCheck, ConsoleCheckSize);
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Update the page's NV Map if user has changed the order
+  a list. This list can be Boot Order or Driver Order.
+
+  @param UpdatePageId    The form ID to be updated.
+  @param OptionMenu      The new list.
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateOrderPage (
+  IN UINT16                           UpdatePageId,
+  IN BM_MENU_OPTION                   *OptionMenu,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY     *NewMenuEntry;
+  UINT16            Index;
+  UINT16            OptionIndex;
+  VOID              *OptionsOpCodeHandle;
+  BOOLEAN           BootOptionFound;
+  UINT32            *OptionOrder;
+  EFI_QUESTION_ID   QuestionId;
+  UINT16            VarOffset;
+
+
+  UpdatePageStart (CallbackData);
+
+  CreateMenuStringToken (CallbackData, CallbackData->BmmHiiHandle, OptionMenu);
+  
+  OptionOrder = NULL;
+  QuestionId = 0;
+  VarOffset = 0;
+  switch (UpdatePageId) { 
+  
+  case FORM_BOOT_CHG_ID:
+    GetBootOrder (CallbackData);
+    OptionOrder = CallbackData->BmmFakeNvData.BootOptionOrder;
+    QuestionId = BOOT_OPTION_ORDER_QUESTION_ID;
+    VarOffset = BOOT_OPTION_ORDER_VAR_OFFSET;
+    break;
+    
+  case FORM_DRV_CHG_ID:
+    GetDriverOrder (CallbackData);
+    OptionOrder = CallbackData->BmmFakeNvData.DriverOptionOrder;
+    QuestionId = DRIVER_OPTION_ORDER_QUESTION_ID;
+    VarOffset = DRIVER_OPTION_ORDER_VAR_OFFSET;
+    break;
+  }  
+  ASSERT (OptionOrder != NULL);   
+  
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+  
+  NewMenuEntry = NULL;
+  for (OptionIndex = 0; (OptionOrder[OptionIndex] != 0 && OptionIndex < MAX_MENU_NUMBER); OptionIndex++) {
+    BootOptionFound = FALSE;
+    for (Index = 0; Index < OptionMenu->MenuNumber; Index++) {
+      NewMenuEntry   = BOpt_GetMenuEntry (OptionMenu, Index);
+      if ((UINT32) (NewMenuEntry->OptionNumber + 1) == OptionOrder[OptionIndex]) {
+        BootOptionFound = TRUE;
+        break;
+      }
+    }
+    if (BootOptionFound) {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        NewMenuEntry->DisplayStringToken,
+        0,
+        EFI_IFR_TYPE_NUM_SIZE_32,
+        OptionOrder[OptionIndex]
+        );
+    }
+  }
+  
+  if (OptionMenu->MenuNumber > 0) {
+    HiiCreateOrderedListOpCode (                   
+      mStartOpCodeHandle,                          // Container for dynamic created opcodes     
+      QuestionId,                                  // Question ID                               
+      VARSTORE_ID_BOOT_MAINT,                      // VarStore ID                               
+      VarOffset,                                   // Offset in Buffer Storage                  
+      STRING_TOKEN (STR_CHANGE_ORDER),             // Question prompt text                      
+      STRING_TOKEN (STR_CHANGE_ORDER),             // Question help text                        
+      0,                                           // Question flag                             
+      0,                                           // Ordered list flag, e.g. EFI_IFR_UNIQUE_SET
+      EFI_IFR_TYPE_NUM_SIZE_32,                    // Data type of Question value               
+      100,                                         // Maximum container                         
+      OptionsOpCodeHandle,                         // Option Opcode list                        
+      NULL                                         // Default Opcode is NULL                    
+      );
+  }
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+
+  UpdatePageEnd (CallbackData);
+
+}
+
+/**
+  Create the dynamic page to allow user to set
+  the "BootNext" value.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateBootNextPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINTN           NumberOfOptions;
+  UINT16          Index;
+  VOID            *OptionsOpCodeHandle;
+
+  NumberOfOptions               = BootOptionMenu.MenuNumber;
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+  CreateMenuStringToken (CallbackData, CallbackData->BmmHiiHandle, &BootOptionMenu);
+
+  if (NumberOfOptions > 0) {
+    OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+    ASSERT (OptionsOpCodeHandle != NULL);
+
+    CallbackData->BmmFakeNvData.BootNext = (UINT16) (BootOptionMenu.MenuNumber);
+
+    for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+      NewMenuEntry    = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+      NewLoadContext  = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+
+      if (NewLoadContext->IsBootNext) {
+        HiiCreateOneOfOptionOpCode (
+          OptionsOpCodeHandle,
+          NewMenuEntry->DisplayStringToken,
+          EFI_IFR_OPTION_DEFAULT,
+          EFI_IFR_TYPE_NUM_SIZE_16,
+          Index
+          );
+        CallbackData->BmmFakeNvData.BootNext = Index;
+      } else {
+        HiiCreateOneOfOptionOpCode (
+          OptionsOpCodeHandle,
+          NewMenuEntry->DisplayStringToken,
+          0,
+          EFI_IFR_TYPE_NUM_SIZE_16,
+          Index
+          );
+      }
+    }
+
+    if (CallbackData->BmmFakeNvData.BootNext == Index) {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        STRING_TOKEN (STR_NONE),
+        EFI_IFR_OPTION_DEFAULT,
+        EFI_IFR_TYPE_NUM_SIZE_16,
+        Index
+        );
+    } else {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        STRING_TOKEN (STR_NONE),
+        0,
+        EFI_IFR_TYPE_NUM_SIZE_16,
+        Index
+        );
+    }      
+
+    HiiCreateOneOfOpCode (
+      mStartOpCodeHandle,
+      (EFI_QUESTION_ID) BOOT_NEXT_QUESTION_ID,
+      VARSTORE_ID_BOOT_MAINT,
+      BOOT_NEXT_VAR_OFFSET,
+      STRING_TOKEN (STR_BOOT_NEXT),
+      STRING_TOKEN (STR_BOOT_NEXT_HELP),
+      0,
+      EFI_IFR_NUMERIC_SIZE_2,
+      OptionsOpCodeHandle,
+      NULL
+      );
+
+    HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  }
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Create the dynamic page to allow user to set the "TimeOut" value.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateTimeOutPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  UINT16  BootTimeOut;
+  VOID    *DefaultOpCodeHandle;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  BootTimeOut = PcdGet16 (PcdPlatformBootTimeOut);
+
+  DefaultOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (DefaultOpCodeHandle != NULL);
+  HiiCreateDefaultOpCode (DefaultOpCodeHandle, EFI_HII_DEFAULT_CLASS_STANDARD, EFI_IFR_TYPE_NUM_SIZE_16, BootTimeOut);
+
+  HiiCreateNumericOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) BOOT_TIME_OUT_QUESTION_ID,
+    VARSTORE_ID_BOOT_MAINT,
+    BOOT_TIME_OUT_VAR_OFFSET,
+    STRING_TOKEN (STR_NUM_AUTO_BOOT),
+    STRING_TOKEN (STR_HLP_AUTO_BOOT),
+    0,
+    EFI_IFR_NUMERIC_SIZE_2 | EFI_IFR_DISPLAY_UINT_DEC,
+    0,
+    65535,
+    0,
+    DefaultOpCodeHandle
+    );
+  
+  HiiFreeOpCodeHandle (DefaultOpCodeHandle);
+
+  CallbackData->BmmFakeNvData.BootTimeOut = BootTimeOut;
+
+  UpdatePageEnd (CallbackData);
+}
+
+
+/**
+  Refresh the text mode page.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateConModePage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  UINTN                         Mode;
+  UINTN                         Index;
+  UINTN                         Col;
+  UINTN                         Row;
+  CHAR16                        ModeString[50];
+  CHAR16                        *PStr;
+  UINTN                         MaxMode;
+  UINTN                         ValidMode;
+  EFI_STRING_ID                 *ModeToken;
+  EFI_STATUS                    Status;
+  VOID                          *OptionsOpCodeHandle;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *ConOut;
+
+  ConOut    = gST->ConOut;
+  Index     = 0;
+  ValidMode = 0;
+  MaxMode   = (UINTN) (ConOut->Mode->MaxMode);
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  //
+  // Check valid mode
+  //
+  for (Mode = 0; Mode < MaxMode; Mode++) {
+    Status = ConOut->QueryMode (ConOut, Mode, &Col, &Row);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    ValidMode++;
+  }
+
+  if (ValidMode == 0) {
+    return;
+  }
+
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  ModeToken           = AllocateZeroPool (sizeof (EFI_STRING_ID) * ValidMode);
+  ASSERT(ModeToken != NULL);
+
+  //
+  // Determin which mode should be the first entry in menu
+  //
+  GetConsoleOutMode (CallbackData);
+
+  //
+  // Build text mode options
+  //
+  for (Mode = 0; Mode < MaxMode; Mode++) {
+    Status = ConOut->QueryMode (ConOut, Mode, &Col, &Row);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    
+    //
+    // Build mode string Column x Row
+    //
+    UnicodeValueToString (ModeString, 0, Col, 0);
+    PStr = &ModeString[0];
+    StrnCatS (PStr, sizeof (ModeString) / sizeof (ModeString[0]), L" x ", StrLen(L" x ") + 1);
+    PStr = PStr + StrLen (PStr);
+    UnicodeValueToString (PStr , 0, Row, 0);
+
+    ModeToken[Index] = HiiSetString (CallbackData->BmmHiiHandle, 0, ModeString, NULL);
+
+    if (Mode == CallbackData->BmmFakeNvData.ConsoleOutMode) {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        ModeToken[Index],
+        EFI_IFR_OPTION_DEFAULT,
+        EFI_IFR_TYPE_NUM_SIZE_16,
+        (UINT16) Mode
+        );
+    } else {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        ModeToken[Index],
+        0,
+        EFI_IFR_TYPE_NUM_SIZE_16,
+        (UINT16) Mode
+        );
+    }
+    Index++;
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) CON_MODE_QUESTION_ID,
+    VARSTORE_ID_BOOT_MAINT,
+    CON_MODE_VAR_OFFSET,
+    STRING_TOKEN (STR_CON_MODE_SETUP),
+    STRING_TOKEN (STR_CON_MODE_SETUP),
+    EFI_IFR_FLAG_RESET_REQUIRED,
+    EFI_IFR_NUMERIC_SIZE_2,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  FreePool (ModeToken);
+
+  UpdatePageEnd (CallbackData);
+}
+
+ /**
+  Create the dynamic page which allows user to set the property such as Baud Rate, Data Bits,
+  Parity, Stop Bits, Terminal Type.
+
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdateTerminalPage (
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  UINT8               Index;
+  UINT8               CheckFlags;
+  BM_MENU_ENTRY       *NewMenuEntry;
+  VOID                *OptionsOpCodeHandle;
+  UINTN               CurrentTerminal;
+
+  CallbackData->BmmAskSaveOrNot = TRUE;
+
+  UpdatePageStart (CallbackData);
+
+  CurrentTerminal = CallbackData->CurrentTerminal;
+  NewMenuEntry = BOpt_GetMenuEntry (
+                  &TerminalMenu,
+                  CurrentTerminal
+                  );
+
+  if (NewMenuEntry == NULL) {
+    return ;
+  }
+
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (BaudRateList) / sizeof (BaudRateList [0]); Index++) {
+    CheckFlags = 0;
+    if (BaudRateList[Index].Value == 115200) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      BaudRateList[Index].StringToken,
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      Index
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_BAUD_RATE_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_BAUD_RATE_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_BAUD_RATE),
+    STRING_TOKEN (STR_COM_BAUD_RATE),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+  
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (DataBitsList) / sizeof (DataBitsList[0]); Index++) {
+    CheckFlags = 0;
+
+    if (DataBitsList[Index].Value == 8) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      DataBitsList[Index].StringToken,
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      Index
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_DATA_RATE_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_DATA_RATE_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_DATA_BITS),
+    STRING_TOKEN (STR_COM_DATA_BITS),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (ParityList) / sizeof (ParityList[0]); Index++) {
+    CheckFlags = 0;
+    if (ParityList[Index].Value ==  NoParity) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      ParityList[Index].StringToken,
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      Index
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_PARITY_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_PARITY_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_PARITY),
+    STRING_TOKEN (STR_COM_PARITY),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (StopBitsList) / sizeof (StopBitsList[0]); Index++) {
+    CheckFlags = 0;
+    if (StopBitsList[Index].Value == OneStopBit) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      StopBitsList[Index].StringToken,
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      Index
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_STOP_BITS_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_STOP_BITS_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_STOP_BITS),
+    STRING_TOKEN (STR_COM_STOP_BITS),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (TerminalType) / sizeof (TerminalType[0]); Index++) {
+    CheckFlags = 0;
+    if (Index == 0) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      (EFI_STRING_ID) TerminalType[Index],
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      Index
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_TERMINAL_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_TERMINAL_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_TERMI_TYPE),
+    STRING_TOKEN (STR_COM_TERMI_TYPE),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  for (Index = 0; Index < sizeof (mFlowControlType) / sizeof (mFlowControlType[0]); Index++) {
+  CheckFlags = 0;
+    if (Index == 0) {
+      CheckFlags |= EFI_IFR_OPTION_DEFAULT;
+    }
+    HiiCreateOneOfOptionOpCode (
+      OptionsOpCodeHandle,
+      (EFI_STRING_ID) mFlowControlType[Index],
+      CheckFlags,
+      EFI_IFR_TYPE_NUM_SIZE_8,
+      mFlowControlValue[Index]
+      );
+  }
+
+  HiiCreateOneOfOpCode (
+    mStartOpCodeHandle,
+    (EFI_QUESTION_ID) (COM_FLOWCONTROL_QUESTION_ID + CurrentTerminal),
+    VARSTORE_ID_BOOT_MAINT,
+    (UINT16) (COM_FLOWCONTROL_VAR_OFFSET + CurrentTerminal),
+    STRING_TOKEN (STR_COM_FLOW_CONTROL),
+    STRING_TOKEN (STR_COM_FLOW_CONTROL),
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+
+  UpdatePageEnd (CallbackData);
+}
+
+/**
+  Dispatch the correct update page function to call based on
+  the UpdatePageId.
+
+  @param UpdatePageId    The form ID.
+  @param CallbackData    The BMM context data.
+
+**/
+VOID
+UpdatePageBody (
+  IN UINT16                           UpdatePageId,
+  IN BMM_CALLBACK_DATA                *CallbackData
+  )
+{
+  CleanUpPage (UpdatePageId, CallbackData);
+  switch (UpdatePageId) {
+  case FORM_CON_IN_ID:
+    UpdateConsolePage (UpdatePageId, &ConsoleInpMenu, CallbackData);
+    break;
+
+  case FORM_CON_OUT_ID:
+    UpdateConsolePage (UpdatePageId, &ConsoleOutMenu, CallbackData);
+    break;
+
+  case FORM_CON_ERR_ID:
+    UpdateConsolePage (UpdatePageId, &ConsoleErrMenu, CallbackData);
+    break;
+
+  case FORM_BOOT_CHG_ID:
+    UpdateOrderPage (UpdatePageId, &BootOptionMenu, CallbackData);
+    break;
+
+  case FORM_DRV_CHG_ID:
+    UpdateOrderPage (UpdatePageId, &DriverOptionMenu, CallbackData);
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Dispatch the display to the next page based on NewPageId.
+
+  @param Private         The BMM context data.
+  @param NewPageId       The original page ID.
+
+**/
+VOID
+UpdatePageId (
+  BMM_CALLBACK_DATA              *Private,
+  UINT16                         NewPageId
+  )
+{
+  if ((NewPageId < FILE_OPTION_OFFSET) && (NewPageId >= HANDLE_OPTION_OFFSET)) {
+    //
+    // If we select a handle to add driver option, advance to the add handle description page.
+    //
+    NewPageId = FORM_DRV_ADD_HANDLE_DESC_ID;
+  } else if ((NewPageId == KEY_VALUE_SAVE_AND_EXIT) || (NewPageId == KEY_VALUE_NO_SAVE_AND_EXIT)) {
+    //
+    // Return to main page after "Save Changes" or "Discard Changes".
+    //
+    NewPageId = FORM_MAIN_ID;
+  } else if ((NewPageId >= TERMINAL_OPTION_OFFSET) && (NewPageId < CONSOLE_OPTION_OFFSET)) {
+    NewPageId = FORM_CON_COM_SETUP_ID;
+  }
+
+  if ((NewPageId > 0) && (NewPageId < MAXIMUM_FORM_ID)) {
+    Private->BmmPreviousPageId  = Private->BmmCurrentPageId;
+    Private->BmmCurrentPageId   = NewPageId;
+  }
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMaint/Variable.c b/Core/MdeModulePkg/Application/UiApp/BootMaint/Variable.c
new file mode 100644
index 0000000000..a8d9cd5efb
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMaint/Variable.c
@@ -0,0 +1,1074 @@
+/** @file
+  Variable operation that will be used by bootmaint
+
+Copyright (c) 2004 - 2015, 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 "BootMaint.h"
+
+/**
+  Delete Boot Option that represent a Deleted state in BootOptionMenu.
+  After deleting this boot option, call Var_ChangeBootOrder to
+  make sure BootOrder is in valid state.
+
+  @retval EFI_SUCCESS   If all boot load option EFI Variables corresponding to  
+                        BM_LOAD_CONTEXT marked for deletion is deleted.
+  @retval EFI_NOT_FOUND If can not find the boot option want to be deleted.
+  @return Others        If failed to update the "BootOrder" variable after deletion. 
+
+**/
+EFI_STATUS
+Var_DelBootOption (
+  VOID
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT16          BootString[10];
+  EFI_STATUS      Status;
+  UINTN           Index;
+  UINTN           Index2;
+
+  Status  = EFI_SUCCESS;
+  Index2  = 0;
+  for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&BootOptionMenu, (Index - Index2));
+    if (NULL == NewMenuEntry) {
+      return EFI_NOT_FOUND;
+    }
+
+    NewLoadContext = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    if (!NewLoadContext->Deleted) {
+      continue;
+    }
+
+    UnicodeSPrint (
+      BootString,
+      sizeof (BootString),
+      L"Boot%04x",
+      NewMenuEntry->OptionNumber
+      );
+
+    EfiLibDeleteVariable (BootString, &gEfiGlobalVariableGuid);
+    Index2++;
+    //
+    // If current Load Option is the same as BootNext,
+    // must delete BootNext in order to make sure
+    // there will be no panic on next boot
+    //
+    if (NewLoadContext->IsBootNext) {
+      EfiLibDeleteVariable (L"BootNext", &gEfiGlobalVariableGuid);
+    }
+
+    RemoveEntryList (&NewMenuEntry->Link);
+    BOpt_DestroyMenuEntry (NewMenuEntry);
+    NewMenuEntry = NULL;
+  }
+
+  BootOptionMenu.MenuNumber -= Index2;
+
+  Status = Var_ChangeBootOrder ();
+  return Status;
+}
+
+/**
+  After any operation on Boot####, there will be a discrepancy in BootOrder.
+  Since some are missing but in BootOrder, while some are present but are
+  not reflected by BootOrder. Then a function rebuild BootOrder from
+  scratch by content from BootOptionMenu is needed.
+
+
+  
+
+  @retval  EFI_SUCCESS  The boot order is updated successfully.
+  @return               EFI_STATUS other than EFI_SUCCESS if failed to
+                        Set the "BootOrder" EFI Variable.
+
+**/
+EFI_STATUS
+Var_ChangeBootOrder (
+  VOID
+  )
+{
+
+  EFI_STATUS    Status;
+  BM_MENU_ENTRY *NewMenuEntry;
+  UINT16        *BootOrderList;
+  UINT16        *BootOrderListPtr;
+  UINTN         BootOrderListSize;
+  UINTN         Index;
+
+  BootOrderList     = NULL;
+  BootOrderListSize = 0;
+
+  //
+  // First check whether BootOrder is present in current configuration
+  //
+  GetEfiGlobalVariable2 (L"BootOrder", (VOID **) &BootOrderList, &BootOrderListSize);
+
+  //
+  // If exists, delete it to hold new BootOrder
+  //
+  if (BootOrderList != NULL) {
+    EfiLibDeleteVariable (L"BootOrder", &gEfiGlobalVariableGuid);
+    FreePool (BootOrderList);
+    BootOrderList = NULL;
+  }
+  //
+  // Maybe here should be some check method to ensure that
+  // no new added boot options will be added
+  // but the setup engine now will give only one callback
+  // that is to say, user are granted only one chance to
+  // decide whether the boot option will be added or not
+  // there should be no indictor to show whether this
+  // is a "new" boot option
+  //
+  BootOrderListSize = BootOptionMenu.MenuNumber;
+
+  if (BootOrderListSize > 0) {
+    BootOrderList = AllocateZeroPool (BootOrderListSize * sizeof (UINT16));
+    ASSERT (BootOrderList != NULL);
+    BootOrderListPtr = BootOrderList;
+
+    //
+    // Get all current used Boot#### from BootOptionMenu.
+    // OptionNumber in each BM_LOAD_OPTION is really its
+    // #### value.
+    //
+    for (Index = 0; Index < BootOrderListSize; Index++) {
+      NewMenuEntry    = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+      *BootOrderList  = (UINT16) NewMenuEntry->OptionNumber;
+      BootOrderList++;
+    }
+
+    BootOrderList = BootOrderListPtr;
+
+    //
+    // After building the BootOrderList, write it back
+    //
+    Status = gRT->SetVariable (
+                    L"BootOrder",
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    BootOrderListSize * sizeof (UINT16),
+                    BootOrderList
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Delete Load Option that represent a Deleted state in BootOptionMenu.
+  After deleting this Driver option, call Var_ChangeDriverOrder to
+  make sure DriverOrder is in valid state.
+
+  @retval EFI_SUCCESS       Load Option is successfully updated.
+  @retval EFI_NOT_FOUND     Fail to find the driver option want to be deleted.
+  @return Other value than EFI_SUCCESS if failed to update "Driver Order" EFI
+          Variable.
+
+**/
+EFI_STATUS
+Var_DelDriverOption (
+  VOID
+  )
+{
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  UINT16          DriverString[12];
+  EFI_STATUS      Status;
+  UINTN           Index;
+  UINTN           Index2;
+
+  Status  = EFI_SUCCESS;
+  Index2  = 0;
+  for (Index = 0; Index < DriverOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&DriverOptionMenu, (Index - Index2));
+    if (NULL == NewMenuEntry) {
+      return EFI_NOT_FOUND;
+    }
+
+    NewLoadContext = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    if (!NewLoadContext->Deleted) {
+      continue;
+    }
+
+    UnicodeSPrint (
+      DriverString,
+      sizeof (DriverString),
+      L"Driver%04x",
+      NewMenuEntry->OptionNumber
+      );
+
+    EfiLibDeleteVariable (DriverString, &gEfiGlobalVariableGuid);
+    Index2++;
+
+    RemoveEntryList (&NewMenuEntry->Link);
+    BOpt_DestroyMenuEntry (NewMenuEntry);
+    NewMenuEntry = NULL;
+  }
+
+  DriverOptionMenu.MenuNumber -= Index2;
+
+  Status = Var_ChangeDriverOrder ();
+  return Status;
+}
+
+/**
+  After any operation on Driver####, there will be a discrepancy in
+  DriverOrder. Since some are missing but in DriverOrder, while some
+  are present but are not reflected by DriverOrder. Then a function
+  rebuild DriverOrder from scratch by content from DriverOptionMenu is
+  needed.
+
+  @retval  EFI_SUCCESS  The driver order is updated successfully.
+  @return  Other status than EFI_SUCCESS if failed to set the "DriverOrder" EFI Variable.
+
+**/
+EFI_STATUS
+Var_ChangeDriverOrder (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+  BM_MENU_ENTRY *NewMenuEntry;
+  UINT16        *DriverOrderList;
+  UINT16        *DriverOrderListPtr;
+  UINTN         DriverOrderListSize;
+  UINTN         Index;
+
+  DriverOrderList     = NULL;
+  DriverOrderListSize = 0;
+
+  //
+  // First check whether DriverOrder is present in current configuration
+  //
+  GetEfiGlobalVariable2 (L"DriverOrder", (VOID **) &DriverOrderList, &DriverOrderListSize);
+  //
+  // If exists, delete it to hold new DriverOrder
+  //
+  if (DriverOrderList != NULL) {
+    EfiLibDeleteVariable (L"DriverOrder", &gEfiGlobalVariableGuid);
+    FreePool (DriverOrderList);
+    DriverOrderList = NULL;
+  }
+
+  DriverOrderListSize = DriverOptionMenu.MenuNumber;
+
+  if (DriverOrderListSize > 0) {
+    DriverOrderList = AllocateZeroPool (DriverOrderListSize * sizeof (UINT16));
+    ASSERT (DriverOrderList != NULL);
+    DriverOrderListPtr = DriverOrderList;
+
+    //
+    // Get all current used Driver#### from DriverOptionMenu.
+    // OptionNumber in each BM_LOAD_OPTION is really its
+    // #### value.
+    //
+    for (Index = 0; Index < DriverOrderListSize; Index++) {
+      NewMenuEntry      = BOpt_GetMenuEntry (&DriverOptionMenu, Index);
+      *DriverOrderList  = (UINT16) NewMenuEntry->OptionNumber;
+      DriverOrderList++;
+    }
+
+    DriverOrderList = DriverOrderListPtr;
+
+    //
+    // After building the DriverOrderList, write it back
+    //
+    Status = gRT->SetVariable (
+                    L"DriverOrder",
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    DriverOrderListSize * sizeof (UINT16),
+                    DriverOrderList
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Update the device path of "ConOut", "ConIn" and "ErrOut" 
+  based on the new BaudRate, Data Bits, parity and Stop Bits
+  set.
+
+**/
+VOID
+Var_UpdateAllConsoleOption (
+  VOID
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *OutDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *InpDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *ErrDevicePath;
+  EFI_STATUS                Status;
+
+  GetEfiGlobalVariable2 (L"ConOut", (VOID**)&OutDevicePath, NULL);
+  GetEfiGlobalVariable2 (L"ConIn", (VOID**)&InpDevicePath, NULL);
+  GetEfiGlobalVariable2 (L"ErrOut", (VOID**)&ErrDevicePath, NULL);
+  if (OutDevicePath != NULL) {
+    ChangeVariableDevicePath (OutDevicePath);
+    Status = gRT->SetVariable (
+                    L"ConOut",
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    GetDevicePathSize (OutDevicePath),
+                    OutDevicePath
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  }
+
+  if (InpDevicePath != NULL) {
+    ChangeVariableDevicePath (InpDevicePath);
+    Status = gRT->SetVariable (
+                    L"ConIn",
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    GetDevicePathSize (InpDevicePath),
+                    InpDevicePath
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  }
+
+  if (ErrDevicePath != NULL) {
+    ChangeVariableDevicePath (ErrDevicePath);
+    Status = gRT->SetVariable (
+                    L"ErrOut",
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    GetDevicePathSize (ErrDevicePath),
+                    ErrDevicePath
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  }
+}
+
+/**
+  This function delete and build multi-instance device path for
+  specified type of console device.
+
+  This function clear the EFI variable defined by ConsoleName and
+  gEfiGlobalVariableGuid. It then build the multi-instance device
+  path by appending the device path of the Console (In/Out/Err) instance 
+  in ConsoleMenu. Then it scan all corresponding console device by
+  scanning Terminal (built from device supporting Serial I/O instances)
+  devices in TerminalMenu. At last, it save a EFI variable specifed
+  by ConsoleName and gEfiGlobalVariableGuid.
+
+  @param ConsoleName     The name for the console device type. They are
+                         usually "ConIn", "ConOut" and "ErrOut".
+  @param ConsoleMenu     The console memu which is a list of console devices.
+  @param UpdatePageId    The flag specifying which type of console device
+                         to be processed.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateConsoleOption (
+  IN UINT16                     *ConsoleName,
+  IN BM_MENU_OPTION             *ConsoleMenu,
+  IN UINT16                     UpdatePageId
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *ConDevicePath;
+  BM_MENU_ENTRY             *NewMenuEntry;
+  BM_CONSOLE_CONTEXT        *NewConsoleContext;
+  BM_TERMINAL_CONTEXT       *NewTerminalContext;
+  EFI_STATUS                Status;
+  VENDOR_DEVICE_PATH        Vendor;
+  EFI_DEVICE_PATH_PROTOCOL  *TerminalDevicePath;
+  UINTN                     Index;
+
+  GetEfiGlobalVariable2 (ConsoleName, (VOID**)&ConDevicePath, NULL);
+  if (ConDevicePath != NULL) {
+    EfiLibDeleteVariable (ConsoleName, &gEfiGlobalVariableGuid);
+    FreePool (ConDevicePath);
+    ConDevicePath = NULL;
+  };
+
+  //
+  // First add all console input device from console input menu
+  //
+  for (Index = 0; Index < ConsoleMenu->MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (ConsoleMenu, Index);
+
+    NewConsoleContext = (BM_CONSOLE_CONTEXT *) NewMenuEntry->VariableContext;
+    if (NewConsoleContext->IsActive) {
+      ConDevicePath = AppendDevicePathInstance (
+                        ConDevicePath,
+                        NewConsoleContext->DevicePath
+                        );
+    }
+  }
+
+  for (Index = 0; Index < TerminalMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&TerminalMenu, Index);
+
+    NewTerminalContext = (BM_TERMINAL_CONTEXT *) NewMenuEntry->VariableContext;
+    if (((NewTerminalContext->IsConIn != 0) && (UpdatePageId == FORM_CON_IN_ID)) ||
+        ((NewTerminalContext->IsConOut != 0)  && (UpdatePageId == FORM_CON_OUT_ID)) ||
+        ((NewTerminalContext->IsStdErr  != 0) && (UpdatePageId == FORM_CON_ERR_ID))
+        ) {
+      Vendor.Header.Type    = MESSAGING_DEVICE_PATH;
+      Vendor.Header.SubType = MSG_VENDOR_DP;
+      
+      ASSERT (NewTerminalContext->TerminalType < (sizeof (TerminalTypeGuid) / sizeof (TerminalTypeGuid[0])));
+      CopyMem (
+        &Vendor.Guid,
+        &TerminalTypeGuid[NewTerminalContext->TerminalType],
+        sizeof (EFI_GUID)
+        );
+      SetDevicePathNodeLength (&Vendor.Header, sizeof (VENDOR_DEVICE_PATH));
+      TerminalDevicePath = AppendDevicePathNode (
+                            NewTerminalContext->DevicePath,
+                            (EFI_DEVICE_PATH_PROTOCOL *) &Vendor
+                            );
+      ASSERT (TerminalDevicePath != NULL);
+      ChangeTerminalDevicePath (TerminalDevicePath, TRUE);
+      ConDevicePath = AppendDevicePathInstance (
+                        ConDevicePath,
+                        TerminalDevicePath
+                        );
+    }
+  }
+
+  if (ConDevicePath != NULL) {
+    Status = gRT->SetVariable (
+                    ConsoleName,
+                    &gEfiGlobalVariableGuid,
+                    VAR_FLAG,
+                    GetDevicePathSize (ConDevicePath),
+                    ConDevicePath
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  This function delete and build multi-instance device path ConIn
+  console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+**/
+EFI_STATUS
+Var_UpdateConsoleInpOption (
+  VOID
+  )
+{
+  return Var_UpdateConsoleOption (L"ConIn", &ConsoleInpMenu, FORM_CON_IN_ID);
+}
+
+/**
+  This function delete and build multi-instance device path ConOut
+  console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+**/
+EFI_STATUS
+Var_UpdateConsoleOutOption (
+  VOID
+  )
+{
+  return Var_UpdateConsoleOption (L"ConOut", &ConsoleOutMenu, FORM_CON_OUT_ID);
+}
+
+/**
+  This function delete and build multi-instance device path ErrOut
+  console device.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.  
+**/
+EFI_STATUS
+Var_UpdateErrorOutOption (
+  VOID
+  )
+{
+  return Var_UpdateConsoleOption (L"ErrOut", &ConsoleErrMenu, FORM_CON_ERR_ID);
+}
+
+/**
+  This function create a currently loaded Drive Option from 
+  the BMM. It then appends this Driver Option to the end of 
+  the "DriverOrder" list. It append this Driver Opotion to the end
+  of DriverOptionMenu.
+
+  @param CallbackData    The BMM context data.
+  @param HiiHandle       The HII handle associated with the BMM formset.
+  @param DescriptionData The description of this driver option.
+  @param OptionalData    The optional load option.
+  @param ForceReconnect  If to force reconnect.
+
+  @retval EFI_OUT_OF_RESOURCES If not enought memory to complete the operation.
+  @retval EFI_SUCCESS          If function completes successfully.
+
+**/
+EFI_STATUS
+Var_UpdateDriverOption (
+  IN  BMM_CALLBACK_DATA         *CallbackData,
+  IN  EFI_HII_HANDLE            HiiHandle,
+  IN  UINT16                    *DescriptionData,
+  IN  UINT16                    *OptionalData,
+  IN  UINT8                     ForceReconnect
+  )
+{
+  UINT16          Index;
+  UINT16          *DriverOrderList;
+  UINT16          *NewDriverOrderList;
+  UINT16          DriverString[12];
+  UINTN           DriverOrderListSize;
+  VOID            *Buffer;
+  UINTN           BufferSize;
+  UINT8           *Ptr;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  BOOLEAN         OptionalDataExist;
+  EFI_STATUS      Status;
+
+  OptionalDataExist = FALSE;
+
+  Index             = BOpt_GetDriverOptionNumber ();
+  UnicodeSPrint (
+    DriverString,
+    sizeof (DriverString),
+    L"Driver%04x",
+    Index
+    );
+
+  if (*DescriptionData == 0x0000) {
+    StrCpyS (DescriptionData, MAX_MENU_NUMBER, DriverString);
+  }
+
+  BufferSize = sizeof (UINT32) + sizeof (UINT16) + StrSize (DescriptionData);
+  BufferSize += GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+
+  if (*OptionalData != 0x0000) {
+    OptionalDataExist = TRUE;
+    BufferSize += StrSize (OptionalData);
+  }
+
+  Buffer = AllocateZeroPool (BufferSize);
+  if (NULL == Buffer) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewMenuEntry = BOpt_CreateMenuEntry (BM_LOAD_CONTEXT_SELECT);
+  if (NULL == NewMenuEntry) {
+    FreePool (Buffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewLoadContext                  = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+  NewLoadContext->Deleted         = FALSE;
+  NewLoadContext->LoadOptionSize  = BufferSize;
+  Ptr = (UINT8 *) Buffer;
+  NewLoadContext->LoadOption = Ptr;
+  *((UINT32 *) Ptr) = LOAD_OPTION_ACTIVE | (ForceReconnect << 1);
+  NewLoadContext->Attributes = *((UINT32 *) Ptr);
+  NewLoadContext->IsActive = TRUE;
+  NewLoadContext->ForceReconnect = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_FORCE_RECONNECT);
+
+  Ptr += sizeof (UINT32);
+  *((UINT16 *) Ptr) = (UINT16) GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+  NewLoadContext->FilePathListLength = *((UINT16 *) Ptr);
+
+  Ptr += sizeof (UINT16);
+  CopyMem (
+    Ptr,
+    DescriptionData,
+    StrSize (DescriptionData)
+    );
+
+  NewLoadContext->Description = AllocateZeroPool (StrSize (DescriptionData));
+  ASSERT (NewLoadContext->Description != NULL);
+  NewMenuEntry->DisplayString = NewLoadContext->Description;
+  CopyMem (
+    NewLoadContext->Description,
+    (VOID *) Ptr,
+    StrSize (DescriptionData)
+    );
+
+  Ptr += StrSize (DescriptionData);
+  CopyMem (
+    Ptr,
+    CallbackData->LoadContext->FilePathList,
+    GetDevicePathSize (CallbackData->LoadContext->FilePathList)
+    );
+
+  NewLoadContext->FilePathList = AllocateZeroPool (GetDevicePathSize (CallbackData->LoadContext->FilePathList));
+  ASSERT (NewLoadContext->FilePathList != NULL);
+
+  CopyMem (
+    NewLoadContext->FilePathList,
+    (VOID *) Ptr,
+    GetDevicePathSize (CallbackData->LoadContext->FilePathList)
+    );
+
+  NewMenuEntry->HelpString    = UiDevicePathToStr (NewLoadContext->FilePathList);
+  NewMenuEntry->OptionNumber  = Index;
+  NewMenuEntry->DisplayStringToken = GetStringTokenFromDepository (
+                                      CallbackData,
+                                      DriverOptionStrDepository
+                                      );
+  NewMenuEntry->DisplayStringToken = HiiSetString (HiiHandle, 0, NewMenuEntry->DisplayString, NULL);
+
+  NewMenuEntry->HelpStringToken = GetStringTokenFromDepository (
+                                    CallbackData,
+                                    DriverOptionHelpStrDepository
+                                    );
+  NewMenuEntry->HelpStringToken = HiiSetString (HiiHandle, 0, NewMenuEntry->HelpString, NULL);
+
+  if (OptionalDataExist) {
+    Ptr += (UINT8) GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+
+    CopyMem (
+      Ptr,
+      OptionalData,
+      StrSize (OptionalData)
+      );
+  }
+
+  Status = gRT->SetVariable (
+                  DriverString,
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  BufferSize,
+                  Buffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+  GetEfiGlobalVariable2 (L"DriverOrder", (VOID **) &DriverOrderList, &DriverOrderListSize);
+  NewDriverOrderList = AllocateZeroPool (DriverOrderListSize + sizeof (UINT16));
+  ASSERT (NewDriverOrderList != NULL);
+  if (DriverOrderList != NULL){
+    CopyMem (NewDriverOrderList, DriverOrderList, DriverOrderListSize);
+  }
+  NewDriverOrderList[DriverOrderListSize / sizeof (UINT16)] = Index;
+  if (DriverOrderList != NULL) {
+    EfiLibDeleteVariable (L"DriverOrder", &gEfiGlobalVariableGuid);
+  }
+
+  Status = gRT->SetVariable (
+                  L"DriverOrder",
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  DriverOrderListSize + sizeof (UINT16),
+                  NewDriverOrderList
+                  );
+  ASSERT_EFI_ERROR (Status);
+  if (DriverOrderList != NULL) {
+    FreePool (DriverOrderList);
+  }
+  DriverOrderList = NULL;
+  FreePool (NewDriverOrderList);
+  InsertTailList (&DriverOptionMenu.Head, &NewMenuEntry->Link);
+  DriverOptionMenu.MenuNumber++;
+
+  *DescriptionData  = 0x0000;
+  *OptionalData     = 0x0000;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function create a currently loaded Boot Option from 
+  the BMM. It then appends this Boot Option to the end of 
+  the "BootOrder" list. It also append this Boot Opotion to the end
+  of BootOptionMenu.
+
+  @param CallbackData    The BMM context data.
+  @param NvRamMap        The file explorer formset internal state.
+
+  @retval EFI_OUT_OF_RESOURCES If not enought memory to complete the operation.
+  @retval EFI_SUCCESS          If function completes successfully.
+
+**/
+EFI_STATUS
+Var_UpdateBootOption (
+  IN  BMM_CALLBACK_DATA                   *CallbackData,
+  IN  FILE_EXPLORER_NV_DATA               *NvRamMap
+  )
+{
+  UINT16          *BootOrderList;
+  UINT16          *NewBootOrderList;
+  UINTN           BootOrderListSize;
+  UINT16          BootString[10];
+  VOID            *Buffer;
+  UINTN           BufferSize;
+  UINT8           *Ptr;
+  UINT16          Index;
+  BM_MENU_ENTRY   *NewMenuEntry;
+  BM_LOAD_CONTEXT *NewLoadContext;
+  BOOLEAN         OptionalDataExist;
+  EFI_STATUS      Status;
+
+  OptionalDataExist = FALSE;
+
+  Index = BOpt_GetBootOptionNumber () ;
+  UnicodeSPrint (BootString, sizeof (BootString), L"Boot%04x", Index);
+
+  if (NvRamMap->BootDescriptionData[0] == 0x0000) {
+    StrCpyS (NvRamMap->BootDescriptionData, sizeof (NvRamMap->BootDescriptionData) / sizeof (NvRamMap->BootDescriptionData[0]), BootString);
+  }
+
+  BufferSize = sizeof (UINT32) + sizeof (UINT16) + StrSize (NvRamMap->BootDescriptionData);
+  BufferSize += GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+
+  if (NvRamMap->BootOptionalData[0] != 0x0000) {
+    OptionalDataExist = TRUE;
+    BufferSize += StrSize (NvRamMap->BootOptionalData);
+  }
+
+  Buffer = AllocateZeroPool (BufferSize);
+  if (NULL == Buffer) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewMenuEntry = BOpt_CreateMenuEntry (BM_LOAD_CONTEXT_SELECT);
+  if (NULL == NewMenuEntry) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewLoadContext                  = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+  NewLoadContext->Deleted         = FALSE;
+  NewLoadContext->LoadOptionSize  = BufferSize;
+  Ptr = (UINT8 *) Buffer;
+  NewLoadContext->LoadOption = Ptr;
+  *((UINT32 *) Ptr) = LOAD_OPTION_ACTIVE;
+  NewLoadContext->Attributes = *((UINT32 *) Ptr);
+  NewLoadContext->IsActive = TRUE;
+  NewLoadContext->ForceReconnect = (BOOLEAN) (NewLoadContext->Attributes & LOAD_OPTION_FORCE_RECONNECT);
+
+  Ptr += sizeof (UINT32);
+  *((UINT16 *) Ptr) = (UINT16) GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+  NewLoadContext->FilePathListLength = *((UINT16 *) Ptr);
+  Ptr += sizeof (UINT16);
+
+  CopyMem (
+    Ptr,
+    NvRamMap->BootDescriptionData,
+    StrSize (NvRamMap->BootDescriptionData)
+    );
+
+  NewLoadContext->Description = AllocateZeroPool (StrSize (NvRamMap->BootDescriptionData));
+  ASSERT (NewLoadContext->Description != NULL);
+
+  NewMenuEntry->DisplayString = NewLoadContext->Description;
+  CopyMem (
+    NewLoadContext->Description,
+    (VOID *) Ptr,
+    StrSize (NvRamMap->BootDescriptionData)
+    );
+
+  Ptr += StrSize (NvRamMap->BootDescriptionData);
+  CopyMem (
+    Ptr,
+    CallbackData->LoadContext->FilePathList,
+    GetDevicePathSize (CallbackData->LoadContext->FilePathList)
+    );
+
+  NewLoadContext->FilePathList = AllocateZeroPool (GetDevicePathSize (CallbackData->LoadContext->FilePathList));
+  ASSERT (NewLoadContext->FilePathList != NULL);
+
+  CopyMem (
+    NewLoadContext->FilePathList,
+    (VOID *) Ptr,
+    GetDevicePathSize (CallbackData->LoadContext->FilePathList)
+    );
+
+  NewMenuEntry->HelpString    = UiDevicePathToStr (NewLoadContext->FilePathList);
+  NewMenuEntry->OptionNumber  = Index;
+  NewMenuEntry->DisplayStringToken = GetStringTokenFromDepository (
+                                      CallbackData,
+                                      BootOptionStrDepository
+                                      );
+  NewMenuEntry->DisplayStringToken = HiiSetString (CallbackData->FeHiiHandle, 0, NewMenuEntry->DisplayString, NULL);
+
+  NewMenuEntry->HelpStringToken = GetStringTokenFromDepository (
+                                    CallbackData,
+                                    BootOptionHelpStrDepository
+                                    );
+  NewMenuEntry->HelpStringToken = HiiSetString (CallbackData->FeHiiHandle, 0, NewMenuEntry->HelpString, NULL);
+
+  if (OptionalDataExist) {
+    Ptr += (UINT8) GetDevicePathSize (CallbackData->LoadContext->FilePathList);
+
+    CopyMem (Ptr, NvRamMap->BootOptionalData, StrSize (NvRamMap->BootOptionalData));
+  }
+
+  Status = gRT->SetVariable (
+                  BootString,
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  BufferSize,
+                  Buffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  GetEfiGlobalVariable2 (L"BootOrder", (VOID **) &BootOrderList, &BootOrderListSize);
+  NewBootOrderList = AllocateZeroPool (BootOrderListSize + sizeof (UINT16));
+  ASSERT (NewBootOrderList != NULL);
+  if (BootOrderList != NULL){
+    CopyMem (NewBootOrderList, BootOrderList, BootOrderListSize);
+  }
+  NewBootOrderList[BootOrderListSize / sizeof (UINT16)] = Index;
+
+  if (BootOrderList != NULL) {
+    FreePool (BootOrderList);
+  }
+
+  Status = gRT->SetVariable (
+                  L"BootOrder",
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  BootOrderListSize + sizeof (UINT16),
+                  NewBootOrderList
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  FreePool (NewBootOrderList);
+  NewBootOrderList = NULL;
+  InsertTailList (&BootOptionMenu.Head, &NewMenuEntry->Link);
+  BootOptionMenu.MenuNumber++;
+
+  NvRamMap->BootDescriptionData[0]  = 0x0000;
+  NvRamMap->BootOptionalData[0]     = 0x0000;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function update the "BootNext" EFI Variable. If there is 
+  no "BootNext" specified in BMM, this EFI Variable is deleted.
+  It also update the BMM context data specified the "BootNext"
+  vaule.
+
+  @param CallbackData    The BMM context data.
+
+  @retval EFI_SUCCESS    The function complete successfully.
+  @return                The EFI variable can be saved. See gRT->SetVariable 
+                         for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateBootNext (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  )
+{
+  BM_MENU_ENTRY     *NewMenuEntry;
+  BM_LOAD_CONTEXT   *NewLoadContext;
+  BMM_FAKE_NV_DATA  *CurrentFakeNVMap;
+  UINT16            Index;
+  EFI_STATUS        Status;
+
+  Status            = EFI_SUCCESS;
+  CurrentFakeNVMap  = &CallbackData->BmmFakeNvData;
+  for (Index = 0; Index < BootOptionMenu.MenuNumber; Index++) {
+    NewMenuEntry = BOpt_GetMenuEntry (&BootOptionMenu, Index);
+    ASSERT (NULL != NewMenuEntry);
+
+    NewLoadContext              = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+    NewLoadContext->IsBootNext  = FALSE;
+  }
+
+  if (CurrentFakeNVMap->BootNext == BootOptionMenu.MenuNumber) {
+    EfiLibDeleteVariable (L"BootNext", &gEfiGlobalVariableGuid);
+    return EFI_SUCCESS;
+  }
+
+  NewMenuEntry = BOpt_GetMenuEntry (
+                  &BootOptionMenu,
+                  CurrentFakeNVMap->BootNext
+                  );
+  ASSERT (NewMenuEntry != NULL);
+
+  NewLoadContext = (BM_LOAD_CONTEXT *) NewMenuEntry->VariableContext;
+  Status = gRT->SetVariable (
+                  L"BootNext",
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  sizeof (UINT16),
+                  &NewMenuEntry->OptionNumber
+                  );
+  NewLoadContext->IsBootNext              = TRUE;
+  CallbackData->BmmOldFakeNVData.BootNext = CurrentFakeNVMap->BootNext;
+  return Status;
+}
+
+/**
+  This function update the "BootOrder" EFI Variable based on
+  BMM Formset's NV map. It then refresh BootOptionMenu
+  with the new "BootOrder" list.
+
+  @param CallbackData    The BMM context data.
+
+  @retval EFI_SUCCESS             The function complete successfully.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to complete the function.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateBootOrder (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  )
+{
+  EFI_STATUS  Status;
+  UINT16      Index;
+  UINT16      OrderIndex;
+  UINT16      *BootOrder;
+  UINTN       BootOrderSize;
+  UINT16      OptionNumber;
+
+  //
+  // First check whether BootOrder is present in current configuration
+  //
+  GetEfiGlobalVariable2 (L"BootOrder", (VOID **) &BootOrder, &BootOrderSize);
+  if (BootOrder == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ASSERT (BootOptionMenu.MenuNumber <= (sizeof (CallbackData->BmmFakeNvData.BootOptionOrder) / sizeof (CallbackData->BmmFakeNvData.BootOptionOrder[0])));
+
+  //
+  // OptionOrder is subset of BootOrder
+  //
+  for (OrderIndex = 0; (OrderIndex < BootOptionMenu.MenuNumber) && (CallbackData->BmmFakeNvData.BootOptionOrder[OrderIndex] != 0); OrderIndex++) {
+    for (Index = OrderIndex; Index < BootOrderSize / sizeof (UINT16); Index++) {
+      if ((BootOrder[Index] == (UINT16) (CallbackData->BmmFakeNvData.BootOptionOrder[OrderIndex] - 1)) && (OrderIndex != Index)) {
+        OptionNumber = BootOrder[Index];
+        CopyMem (&BootOrder[OrderIndex + 1], &BootOrder[OrderIndex], (Index - OrderIndex) * sizeof (UINT16));
+        BootOrder[OrderIndex] = OptionNumber;
+      }
+    }
+  }
+
+  Status = gRT->SetVariable (
+                  L"BootOrder",
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  BootOrderSize,
+                  BootOrder
+                  );
+  FreePool (BootOrder);
+
+  BOpt_FreeMenu (&BootOptionMenu);
+  BOpt_GetBootOptions (CallbackData);
+
+  return Status;
+
+}
+
+/**
+  This function update the "DriverOrder" EFI Variable based on
+  BMM Formset's NV map. It then refresh DriverOptionMenu
+  with the new "DriverOrder" list.
+
+  @param CallbackData    The BMM context data.
+
+  @retval EFI_SUCCESS           The function complete successfully.
+  @retval EFI_OUT_OF_RESOURCES  Not enough memory to complete the function.
+  @return The EFI variable can not be saved. See gRT->SetVariable for detail return information.
+
+**/
+EFI_STATUS
+Var_UpdateDriverOrder (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  )
+{
+  EFI_STATUS  Status;
+  UINT16      Index;
+  UINT16      *DriverOrderList;
+  UINT16      *NewDriverOrderList;
+  UINTN       DriverOrderListSize;
+
+  DriverOrderList     = NULL;
+  DriverOrderListSize = 0;
+
+  //
+  // First check whether DriverOrder is present in current configuration
+  //
+  GetEfiGlobalVariable2 (L"DriverOrder", (VOID **) &DriverOrderList, &DriverOrderListSize);
+  NewDriverOrderList = AllocateZeroPool (DriverOrderListSize);
+
+  if (NewDriverOrderList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // If exists, delete it to hold new DriverOrder
+  //
+  if (DriverOrderList != NULL) {
+    EfiLibDeleteVariable (L"DriverOrder", &gEfiGlobalVariableGuid);
+    FreePool (DriverOrderList);
+  }
+
+  ASSERT (DriverOptionMenu.MenuNumber <= (sizeof (CallbackData->BmmFakeNvData.DriverOptionOrder) / sizeof (CallbackData->BmmFakeNvData.DriverOptionOrder[0])));
+  for (Index = 0; Index < DriverOptionMenu.MenuNumber; Index++) {
+    NewDriverOrderList[Index] = (UINT16) (CallbackData->BmmFakeNvData.DriverOptionOrder[Index] - 1);
+  }
+
+  Status = gRT->SetVariable (
+                  L"DriverOrder",
+                  &gEfiGlobalVariableGuid,
+                  VAR_FLAG,
+                  DriverOrderListSize,
+                  NewDriverOrderList
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  BOpt_FreeMenu (&DriverOptionMenu);
+  BOpt_GetDriverOptions (CallbackData);
+  return EFI_SUCCESS;
+}
+
+/**
+  Update the Text Mode of Console.
+
+  @param CallbackData  The context data for BMM.
+
+  @retval EFI_SUCCSS If the Text Mode of Console is updated.
+  @return Other value if the Text Mode of Console is not updated.
+
+**/
+EFI_STATUS
+Var_UpdateConMode (
+  IN BMM_CALLBACK_DATA            *CallbackData
+  )
+{
+  EFI_STATUS        Status;
+  UINTN             Mode;
+  CONSOLE_OUT_MODE  ModeInfo;
+
+  Mode = CallbackData->BmmFakeNvData.ConsoleOutMode;
+
+  Status = gST->ConOut->QueryMode (gST->ConOut, Mode, &(ModeInfo.Column), &(ModeInfo.Row));
+  if (!EFI_ERROR(Status)) {
+    Status = PcdSet32S (PcdSetupConOutColumn, (UINT32) ModeInfo.Column);
+    if (!EFI_ERROR (Status)) {
+      Status = PcdSet32S (PcdSetupConOutRow, (UINT32) ModeInfo.Row);
+    }
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.c b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.c
new file mode 100644
index 0000000000..4390deef24
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.c
@@ -0,0 +1,399 @@
+/** @file
+  The platform boot manager reference implementation
+
+Copyright (c) 2004 - 2015, 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 "BootManager.h"
+
+EFI_GUID           mBootManagerGuid = BOOT_MANAGER_FORMSET_GUID;
+CHAR16             *mDeviceTypeStr[] = {
+  L"Legacy BEV",
+  L"Legacy Floppy",
+  L"Legacy Hard Drive",
+  L"Legacy CD ROM",
+  L"Legacy PCMCIA",
+  L"Legacy USB",
+  L"Legacy Embedded Network",
+  L"Legacy Unknown Device"
+};
+
+
+HII_VENDOR_DEVICE_PATH  mBootManagerHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    //
+    // {1DDDBE15-481D-4d2b-8277-B191EAF66525}
+    //
+    { 0x1dddbe15, 0x481d, 0x4d2b, { 0x82, 0x77, 0xb1, 0x91, 0xea, 0xf6, 0x65, 0x25 } }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+BOOT_MANAGER_CALLBACK_DATA  gBootManagerPrivate = {
+  BOOT_MANAGER_CALLBACK_DATA_SIGNATURE,
+  NULL,
+  NULL,
+  {
+    FakeExtractConfig,
+    FakeRouteConfig,
+    BootManagerCallback
+  }
+};
+
+/**
+  This call back function is registered with Boot Manager formset.
+  When user selects a boot option, this call back function will
+  be triggered. The boot option is saved for later processing.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+BootManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  UINTN                         Index;
+  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption;
+  UINTN                         BootOptionCount;
+  UINT16                        KeyCount;
+  EFI_INPUT_KEY                 Key;
+  EFI_BOOT_MANAGER_LOAD_OPTION  Option;
+
+  if (Action != EFI_BROWSER_ACTION_CHANGED) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((Value == NULL) || (ActionRequest == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Initialize the key count
+  //
+  KeyCount = 0;
+  Option.Attributes = 0;
+  BootOption = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    KeyCount++;
+
+    EfiBootManagerInitializeLoadOption (
+      &Option,
+      BootOption[Index].OptionNumber,
+      BootOption[Index].OptionType,
+      BootOption[Index].Attributes,
+      BootOption[Index].Description,
+      BootOption[Index].FilePath,
+      BootOption[Index].OptionalData,
+      BootOption[Index].OptionalDataSize
+      );
+
+    //
+    // Is this device the one chosen?
+    //
+    if (KeyCount == QuestionId) {
+      //
+      // Clear  the  screen  before.
+      //
+      gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+      gST->ConOut->ClearScreen (gST->ConOut);
+      //
+      // Check any reset required change is applied? if yes, reset system
+      //
+      SetupResetReminder();
+      //
+      // Parse the selected option.
+      //
+      BdsSetConsoleMode(FALSE);
+      EfiBootManagerBoot (&Option);
+      BdsSetConsoleMode(TRUE);
+
+      if (EFI_ERROR (Option.Status)) {
+        gST->ConOut->OutputString (
+                      gST->ConOut,
+                      GetStringById (STRING_TOKEN (STR_ANY_KEY_CONTINUE))
+                      );
+        gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+      }
+      break;
+    }
+  }
+  EfiBootManagerFreeLoadOptions (BootOption, BootOptionCount);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register HII packages to HII database.
+
+**/
+VOID
+InitializeBootManager  (
+  VOID
+  )
+{
+  EFI_STATUS      Status;
+
+  if (!gConnectAllHappened){
+    EfiBootManagerConnectAll();
+    gConnectAllHappened = TRUE;
+  }
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  //
+  gBootManagerPrivate.DriverHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &gBootManagerPrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mBootManagerHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gBootManagerPrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Publish our HII data
+  //
+  gBootManagerPrivate.HiiHandle = HiiAddPackages (
+                                    &mBootManagerGuid,
+                                    gBootManagerPrivate.DriverHandle,
+                                    BootManagerVfrBin,
+                                    UiAppStrings,
+                                    NULL
+                                    );
+  ASSERT(gBootManagerPrivate.HiiHandle != NULL);
+}
+
+
+/**
+  Enumerate possible boot options.
+
+**/
+VOID
+EnumerateBootOptions  (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  EFI_DEVICE_PATH_PROTOCOL      *ImageDevicePath;
+  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption;
+  UINTN                         BootOptionCount;
+  EFI_STRING_ID                 Token;
+  CHAR16                        *HelpString;
+  EFI_STRING_ID                 HelpToken;
+  UINT16                        *TempStr;
+  EFI_HII_HANDLE                HiiHandle;
+  UINTN                         TempSize;
+  VOID                          *StartOpCodeHandle;
+  VOID                          *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL            *StartLabel;
+  EFI_IFR_GUID_LABEL            *EndLabel;
+  UINT16                        DeviceType;
+  BOOLEAN                       IsLegacyOption;
+  BOOLEAN                       NeedEndOp;
+  UINT16                        KeyInput;
+  UINTN                         DestMax;
+
+  DeviceType = (UINT16) -1;
+
+  Status = gBS->HandleProtocol (gImageHandle, &gEfiLoadedImageDevicePathProtocolGuid, (VOID **) &ImageDevicePath);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // for better user experience
+  // 1. User changes HD configuration (e.g.: unplug HDD), here we have a chance to remove the HDD boot option
+  // 2. User enables/disables UEFI PXE, here we have a chance to add/remove EFI Network boot option
+  //
+  EfiBootManagerRefreshAllBootOption ();
+
+  BootOption = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+  HiiHandle = gBootManagerPrivate.HiiHandle;
+
+  //
+  // Allocate space for creation of UpdateData Buffer
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = LABEL_BOOT_OPTION;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_BOOT_OPTION_END;
+
+  KeyInput = 0;
+  NeedEndOp = FALSE;
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    //
+    // At this stage we are creating a menu entry, thus the Keys are reproduceable
+    //
+    KeyInput++;
+
+    //
+    // Don't display the hidden/inactive boot option
+    //
+    if (((BootOption[Index].Attributes & LOAD_OPTION_HIDDEN) != 0) || ((BootOption[Index].Attributes & LOAD_OPTION_ACTIVE) == 0)) {
+      continue;
+    }
+
+    //
+    // Don't display myself
+    //
+    if (CompareMem (BootOption[Index].FilePath, ImageDevicePath, GetDevicePathSize (ImageDevicePath)) == 0) {
+      continue;
+    }
+
+    //
+    // Group the legacy boot option in the sub title created dynamically
+    //
+    IsLegacyOption = (BOOLEAN) (
+                       (DevicePathType (BootOption[Index].FilePath) == BBS_DEVICE_PATH) &&
+                       (DevicePathSubType (BootOption[Index].FilePath) == BBS_BBS_DP)
+                       );
+
+    if (!IsLegacyOption && NeedEndOp) {
+      NeedEndOp = FALSE;
+      HiiCreateEndOpCode (StartOpCodeHandle);
+    }
+    
+    if (IsLegacyOption && DeviceType != ((BBS_BBS_DEVICE_PATH *) BootOption[Index].FilePath)->DeviceType) {
+      if (NeedEndOp) {
+        HiiCreateEndOpCode (StartOpCodeHandle);
+      }
+
+      DeviceType = ((BBS_BBS_DEVICE_PATH *) BootOption[Index].FilePath)->DeviceType;
+      Token      = HiiSetString (
+                     HiiHandle,
+                     0,
+                     mDeviceTypeStr[
+                       MIN (DeviceType & 0xF, sizeof (mDeviceTypeStr) / sizeof (mDeviceTypeStr[0]) - 1)
+                       ],
+                     NULL
+                     );
+      HiiCreateSubTitleOpCode (StartOpCodeHandle, Token, 0, 0, 1);
+      NeedEndOp = TRUE;
+    }
+
+    ASSERT (BootOption[Index].Description != NULL);
+    
+    Token = HiiSetString (HiiHandle, 0, BootOption[Index].Description, NULL);
+
+    TempStr = UiDevicePathToStr (BootOption[Index].FilePath);
+    TempSize = StrSize (TempStr);
+    DestMax = (TempSize + StrSize (L"Device Path : ")) / sizeof(CHAR16);
+    HelpString = AllocateZeroPool (TempSize + StrSize (L"Device Path : "));
+    ASSERT (HelpString != NULL);
+    StrCatS (HelpString, DestMax, L"Device Path : ");
+    StrCatS (HelpString, DestMax, TempStr);
+
+    HelpToken = HiiSetString (HiiHandle, 0, HelpString, NULL);
+
+    HiiCreateActionOpCode (
+      StartOpCodeHandle,
+      KeyInput,
+      Token,
+      HelpToken,
+      EFI_IFR_FLAG_CALLBACK,
+      0
+      );
+  }
+
+  if (NeedEndOp) {
+    HiiCreateEndOpCode (StartOpCodeHandle);
+  }
+
+  HiiUpdateForm (
+    HiiHandle,
+    &mBootManagerGuid,
+    BOOT_MANAGER_FORM_ID,
+    StartOpCodeHandle,
+    EndOpCodeHandle
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+  
+  EfiBootManagerFreeLoadOptions (BootOption, BootOptionCount);
+}
+
+
+/**
+  Remove the installed  packages from the HII Database. 
+
+**/
+VOID
+FreeBootManager (
+  VOID
+  )
+{
+  EFI_STATUS      Status;
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gBootManagerPrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mBootManagerHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gBootManagerPrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  HiiRemovePackages (gBootManagerPrivate.HiiHandle);
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.h b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.h
new file mode 100644
index 0000000000..a85973ed24
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManager.h
@@ -0,0 +1,112 @@
+/** @file
+  The platform boot manager reference implement
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _EFI_BOOT_MANAGER_H_
+#define _EFI_BOOT_MANAGER_H_
+
+#include "Ui.h"
+#include "FrontPage.h"
+
+//
+// These are defined as the same with vfr file
+//
+#define BOOT_MANAGER_FORMSET_GUID \
+  { \
+  0x847bc3fe, 0xb974, 0x446d, {0x94, 0x49, 0x5a, 0xd5, 0x41, 0x2e, 0x99, 0x3b} \
+  }
+
+#define BOOT_MANAGER_FORM_ID     0x1000
+
+#define LABEL_BOOT_OPTION        0x00
+#define LABEL_BOOT_OPTION_END    0x01
+
+//
+// These are the VFR compiler generated data representing our VFR data.
+//
+extern UINT8 BootManagerVfrBin[];
+
+#define BOOT_MANAGER_CALLBACK_DATA_SIGNATURE  SIGNATURE_32 ('B', 'M', 'C', 'B')
+
+typedef struct {
+  UINTN                           Signature;
+
+  //
+  // HII relative handles
+  //
+  EFI_HII_HANDLE                  HiiHandle;
+  EFI_HANDLE                      DriverHandle;
+
+  //
+  // Produced protocols
+  //
+  EFI_HII_CONFIG_ACCESS_PROTOCOL   ConfigAccess;
+} BOOT_MANAGER_CALLBACK_DATA;
+
+/**
+  This call back function is registered with Boot Manager formset.
+  When user selects a boot option, this call back function will
+  be triggered. The boot option is saved for later processing.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+BootManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  );
+
+/**
+  Register HII packages to HII database.
+
+**/
+VOID
+InitializeBootManager  (
+  VOID
+  );
+
+/**
+  Enumerate possible boot options.
+
+**/
+VOID
+EnumerateBootOptions  (
+  VOID
+  );
+
+/**
+  Remove the installed  packages from the HII Database. 
+
+**/
+VOID
+FreeBootManager (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerStrings.uni b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerStrings.uni
new file mode 100644
index 0000000000..013c15176a
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerStrings.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerVfr.Vfr b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerVfr.Vfr
new file mode 100644
index 0000000000..a5465f4226
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/BootMngr/BootManagerVfr.Vfr
@@ -0,0 +1,50 @@
+///** @file
+//  
+//    Browser formset.
+//  
+//  Copyright (c) 2004 - 2015, 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.
+//  
+//**/
+
+#define FORMSET_GUID  { 0x847bc3fe, 0xb974, 0x446d, 0x94, 0x49, 0x5a, 0xd5, 0x41, 0x2e, 0x99, 0x3b }
+
+#define BOOT_MANAGER_FORM_ID     0x1000
+
+#define LABEL_BOOT_OPTION        0x00
+#define LABEL_BOOT_OPTION_END    0x01
+
+#define BOOT_MANAGER_CLASS       0x00
+#define BOOT_MANAGER_SUBCLASS    0x00
+
+formset
+  guid      = FORMSET_GUID,
+  title     = STRING_TOKEN(STR_BM_BANNER),
+  help      = STRING_TOKEN(STR_LAST_STRING),
+  classguid = FORMSET_GUID,
+
+  form formid = BOOT_MANAGER_FORM_ID,
+       title  = STRING_TOKEN(STR_BM_BANNER);
+
+    subtitle text = STRING_TOKEN(STR_LAST_STRING);
+    subtitle text = STRING_TOKEN(STR_BOOT_OPTION_BANNER);
+    subtitle text = STRING_TOKEN(STR_LAST_STRING);
+
+    //
+    // This is where we will dynamically add choices for the Boot Manager
+    //
+    label LABEL_BOOT_OPTION;
+    label LABEL_BOOT_OPTION_END;
+
+    subtitle text = STRING_TOKEN(STR_LAST_STRING);
+    subtitle text = STRING_TOKEN(STR_HELP_FOOTER);
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.c b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.c
new file mode 100644
index 0000000000..18b2bd7e99
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.c
@@ -0,0 +1,794 @@
+/** @file
+  The platform device manager reference implementation
+
+Copyright (c) 2004 - 2015, 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 "DeviceManager.h"
+
+DEVICE_MANAGER_CALLBACK_DATA  gDeviceManagerPrivate = {
+  DEVICE_MANAGER_CALLBACK_DATA_SIGNATURE,
+  NULL,
+  NULL,
+  {
+    FakeExtractConfig,
+    FakeRouteConfig,
+    DeviceManagerCallback
+  }
+};
+
+#define  MAX_MAC_ADDRESS_NODE_LIST_LEN    10
+
+EFI_GUID mDeviceManagerGuid = DEVICE_MANAGER_FORMSET_GUID;
+
+//
+// Which Mac Address string is select
+// it will decide what menu need to show in the NETWORK_DEVICE_FORM_ID form.
+//
+EFI_STRING  mSelectedMacAddrString;
+
+//
+// The Mac Address show in the NETWORK_DEVICE_LIST_FORM_ID
+//
+MAC_ADDRESS_NODE_LIST  mMacDeviceList;
+
+DEVICE_MANAGER_MENU_ITEM  mDeviceManagerMenuItemTable[] = {
+  { STRING_TOKEN (STR_DISK_DEVICE),     EFI_DISK_DEVICE_CLASS },
+  { STRING_TOKEN (STR_VIDEO_DEVICE),    EFI_VIDEO_DEVICE_CLASS },
+  { STRING_TOKEN (STR_NETWORK_DEVICE),  EFI_NETWORK_DEVICE_CLASS },
+  { STRING_TOKEN (STR_INPUT_DEVICE),    EFI_INPUT_DEVICE_CLASS },
+  { STRING_TOKEN (STR_ON_BOARD_DEVICE), EFI_ON_BOARD_DEVICE_CLASS },
+  { STRING_TOKEN (STR_OTHER_DEVICE),    EFI_OTHER_DEVICE_CLASS }
+};
+
+HII_VENDOR_DEVICE_PATH  mDeviceManagerHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    //
+    // {102579A0-3686-466e-ACD8-80C087044F4A}
+    //
+    { 0x102579a0, 0x3686, 0x466e, { 0xac, 0xd8, 0x80, 0xc0, 0x87, 0x4, 0x4f, 0x4a } }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  This function is invoked if user selected a interactive opcode from Device Manager's
+  Formset. If user set VBIOS, the new value is saved to EFI variable.
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+DeviceManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  UINTN CurIndex;
+  
+  if (Action != EFI_BROWSER_ACTION_CHANGING) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((Value == NULL) || (ActionRequest == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((QuestionId < MAX_KEY_SECTION_LEN + NETWORK_DEVICE_LIST_KEY_OFFSET) && (QuestionId >= NETWORK_DEVICE_LIST_KEY_OFFSET)) {
+    //
+    // If user select the mac address, need to record mac address string to support next form show.
+    //
+    for (CurIndex = 0; CurIndex < mMacDeviceList.CurListLen; CurIndex ++) {
+      if (mMacDeviceList.NodeList[CurIndex].QuestionId == QuestionId) {
+         mSelectedMacAddrString = HiiGetString (gDeviceManagerPrivate.HiiHandle, mMacDeviceList.NodeList[CurIndex].PromptId, NULL);
+      }
+    }
+    CreateDeviceManagerForm(NETWORK_DEVICE_FORM_ID);
+  } else if(QuestionId == QUESTION_NETWORK_DEVICE_ID){
+    CreateDeviceManagerForm(NETWORK_DEVICE_LIST_FORM_ID);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the mac address string from the device path.
+  if the device path has the vlan, get the vanid also.
+  
+  @param MacAddressNode              Device path begin with mac address 
+  @param PBuffer                     Output string buffer contain mac address.
+
+**/
+BOOLEAN 
+GetMacAddressString(
+  IN  MAC_ADDR_DEVICE_PATH   *MacAddressNode,
+  OUT CHAR16                 **PBuffer
+  )
+{
+  UINTN                 HwAddressSize;
+  UINTN                 Index;
+  UINT8                 *HwAddress;
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  UINT16                VlanId;
+  CHAR16                *String;
+  UINTN                 BufferLen;
+
+  VlanId = 0;
+  String = NULL;
+  ASSERT(MacAddressNode != NULL);
+
+  HwAddressSize = sizeof (EFI_MAC_ADDRESS);
+  if (MacAddressNode->IfType == 0x01 || MacAddressNode->IfType == 0x00) {
+    HwAddressSize = 6;
+  }
+
+  //
+  // The output format is MAC:XX:XX:XX:...\XXXX
+  // The size is the Number size + ":" size + Vlan size(\XXXX) + End
+  //
+  BufferLen = (4 + 2 * HwAddressSize + (HwAddressSize - 1) + 5 + 1) * sizeof (CHAR16);
+  String = AllocateZeroPool (BufferLen);
+  if (String == NULL) {
+    return FALSE;
+  }
+
+  *PBuffer = String;
+  StrCpyS(String, BufferLen / sizeof (CHAR16), L"MAC:");
+  String += 4;
+  
+  //
+  // Convert the MAC address into a unicode string.
+  //
+  HwAddress = &MacAddressNode->MacAddress.Addr[0];
+  for (Index = 0; Index < HwAddressSize; Index++) {
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(HwAddress++), 2);
+    if (Index < HwAddressSize - 1) {
+      *String++ = L':';
+    }
+  }
+  
+  //
+  // If VLAN is configured, it will need extra 5 characters like "\0005".
+  // Plus one unicode character for the null-terminator.
+  //
+  Node = (EFI_DEVICE_PATH_PROTOCOL  *)MacAddressNode;
+  while (!IsDevicePathEnd (Node)) {
+    if (Node->Type == MESSAGING_DEVICE_PATH && Node->SubType == MSG_VLAN_DP) {
+      VlanId = ((VLAN_DEVICE_PATH *) Node)->VlanId;
+    }
+    Node = NextDevicePathNode (Node);
+  }
+
+  if (VlanId != 0) {
+    *String++ = L'\\';
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, VlanId, 4);
+  }
+
+  //
+  // Null terminate the Unicode string
+  //
+  *String = L'\0';
+
+  return TRUE;
+}
+
+/**
+  Save question id and prompt id to the mac device list.
+  If the same mac address has saved yet, no need to add more.
+
+  @param MacAddrString               Mac address string.
+
+  @retval  EFI_SUCCESS               Add the item is successful.
+  @return  Other values if failed to Add the item.
+**/
+BOOLEAN 
+AddIdToMacDeviceList (
+  IN  EFI_STRING        MacAddrString
+  )
+{
+  MENU_INFO_ITEM *TempDeviceList;
+  UINTN          Index;
+  EFI_STRING     StoredString;
+  EFI_STRING_ID  PromptId;
+  EFI_HII_HANDLE HiiHandle;
+
+  HiiHandle =   gDeviceManagerPrivate.HiiHandle;
+  TempDeviceList = NULL;
+
+  for (Index = 0; Index < mMacDeviceList.CurListLen; Index ++) {
+    StoredString = HiiGetString (HiiHandle, mMacDeviceList.NodeList[Index].PromptId, NULL);
+    if (StoredString == NULL) {
+      return FALSE;
+    }
+
+    //
+    // Already has save the same mac address to the list.
+    //
+    if (StrCmp (MacAddrString, StoredString) == 0) {
+      return FALSE;
+    }
+  }
+
+  PromptId = HiiSetString(HiiHandle, 0, MacAddrString, NULL);
+  //
+  // If not in the list, save it.
+  //
+  if (mMacDeviceList.MaxListLen > mMacDeviceList.CurListLen + 1) {
+    mMacDeviceList.NodeList[mMacDeviceList.CurListLen].PromptId = PromptId;
+    mMacDeviceList.NodeList[mMacDeviceList.CurListLen].QuestionId = (EFI_QUESTION_ID) (mMacDeviceList.CurListLen + NETWORK_DEVICE_LIST_KEY_OFFSET);
+  } else {
+    mMacDeviceList.MaxListLen += MAX_MAC_ADDRESS_NODE_LIST_LEN;
+    if (mMacDeviceList.CurListLen != 0) {
+      TempDeviceList = (MENU_INFO_ITEM *)AllocateCopyPool (sizeof (MENU_INFO_ITEM) * mMacDeviceList.MaxListLen, (VOID *)mMacDeviceList.NodeList);
+    } else {
+      TempDeviceList = (MENU_INFO_ITEM *)AllocatePool (sizeof (MENU_INFO_ITEM) * mMacDeviceList.MaxListLen);
+    }
+    
+    if (TempDeviceList == NULL) {
+      return FALSE;
+    }
+    TempDeviceList[mMacDeviceList.CurListLen].PromptId = PromptId;  
+    TempDeviceList[mMacDeviceList.CurListLen].QuestionId = (EFI_QUESTION_ID) (mMacDeviceList.CurListLen + NETWORK_DEVICE_LIST_KEY_OFFSET);
+    
+    if (mMacDeviceList.CurListLen > 0) {
+      FreePool(mMacDeviceList.NodeList);
+    }
+    
+    mMacDeviceList.NodeList = TempDeviceList;
+  }
+  mMacDeviceList.CurListLen ++;
+
+  return TRUE;
+}
+
+/**
+  Check the devcie path, try to find whether it has mac address path.
+
+  In this function, first need to check whether this path has mac address path.
+  second, when the mac address device path has find, also need to deicide whether
+  need to add this mac address relate info to the menu.
+
+  @param    *Node            Input device which need to be check.
+  @param    NextShowFormId   FormId Which need to be show.
+  @param    *NeedAddItem     Whether need to add the menu in the network device list.
+
+  @retval  TRUE             Has mac address device path.
+  @retval  FALSE            NOT Has mac address device path.  
+
+**/
+BOOLEAN
+IsMacAddressDevicePath (
+  IN  VOID          *Node,
+  IN EFI_FORM_ID    NextShowFormId,
+  OUT BOOLEAN       *NeedAddItem
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL   *DevicePath;
+  CHAR16                     *Buffer;
+  BOOLEAN                    ReturnVal;
+  
+  ASSERT (Node != NULL);
+  *NeedAddItem = FALSE;
+  ReturnVal    = FALSE;
+  Buffer    = NULL;
+
+  DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) Node;
+
+  //
+  // find the partition device path node
+  //
+  while (!IsDevicePathEnd (DevicePath)) {
+    if ((DevicePathType (DevicePath) == MESSAGING_DEVICE_PATH) &&
+       (DevicePathSubType (DevicePath) == MSG_MAC_ADDR_DP)) {
+      ReturnVal = TRUE;
+      
+      if (DEVICE_MANAGER_FORM_ID == NextShowFormId) {
+        *NeedAddItem = TRUE;
+        break;
+      } 
+      
+      if (!GetMacAddressString((MAC_ADDR_DEVICE_PATH*)DevicePath, &Buffer)) {
+        break;
+      }
+
+      if (NETWORK_DEVICE_FORM_ID == NextShowFormId) {
+        if (StrCmp (Buffer, mSelectedMacAddrString) == 0) {
+          *NeedAddItem = TRUE;
+        }
+        break;
+      }
+
+      if (NETWORK_DEVICE_LIST_FORM_ID == NextShowFormId) {
+        //
+        // Same handle may has two network child handle, so the questionid 
+        // has the offset of SAME_HANDLE_KEY_OFFSET.
+        //
+        if (AddIdToMacDeviceList (Buffer)) {
+          *NeedAddItem = TRUE;
+        }
+        break;
+      }
+    } 
+    DevicePath = NextDevicePathNode (DevicePath);
+  }
+
+  if (Buffer != NULL) {
+    FreePool (Buffer);
+  }
+
+  return ReturnVal;
+}
+
+/**
+  Check to see if the device path is for the network device.
+
+  @param Handle          The HII handle which include the mac address device path.
+  @param NextShowFormId  The FormId of the form which will be show next time.
+  @param ItemCount       The new add Mac address item count.
+
+  @retval  TRUE          Need to add new item in the menu.
+  @return  FALSE         Do not need to add the menu about the network.
+
+**/
+BOOLEAN 
+IsNeedAddNetworkMenu (
+  IN      EFI_HII_HANDLE      Handle,
+  IN      EFI_FORM_ID         NextShowFormId,
+  OUT     UINTN               *ItemCount
+  )
+{
+  EFI_STATUS     Status;
+  UINTN          EntryCount;
+  UINTN          Index;  
+  EFI_HANDLE     DriverHandle;
+  EFI_HANDLE     ControllerHandle;
+  EFI_DEVICE_PATH_PROTOCOL   *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL   *TmpDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL   *ChildDevicePath;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY   *OpenInfoBuffer;
+  BOOLEAN        IsNeedAdd;
+
+  IsNeedAdd  = FALSE;
+  OpenInfoBuffer = NULL;
+  if ((Handle == NULL) || (ItemCount == NULL)) {
+    return FALSE;
+  }
+  *ItemCount = 0;
+
+  Status = gHiiDatabase->GetPackageListHandle (gHiiDatabase, Handle, &DriverHandle);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Get the device path by the got Driver handle .
+  //
+  Status = gBS->HandleProtocol (DriverHandle, &gEfiDevicePathProtocolGuid, (VOID **) &DevicePath);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  TmpDevicePath = DevicePath;
+
+  // 
+  // Check whether this device path include mac address device path.
+  // If this path has mac address path, get the value whether need 
+  // add this info to the menu and return.
+  // Else check more about the child handle devcie path.
+  //
+  if (IsMacAddressDevicePath(TmpDevicePath, NextShowFormId,&IsNeedAdd)) {
+    if ((NETWORK_DEVICE_LIST_FORM_ID == NextShowFormId) && IsNeedAdd) {
+      (*ItemCount) = 1;
+    }
+    return IsNeedAdd;
+  }
+
+  //
+  // Search whether this path is the controller path, not he child handle path.
+  // And the child handle has the network devcie connected.
+  //
+  TmpDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath(&gEfiDevicePathProtocolGuid, &TmpDevicePath, &ControllerHandle);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if (!IsDevicePathEnd (TmpDevicePath)) {
+    return FALSE;    
+  }
+
+  //
+  // Retrieve the list of agents that are consuming the specific protocol
+  // on ControllerHandle.
+  // The buffer point by OpenInfoBuffer need be free at this function.
+  //
+  Status = gBS->OpenProtocolInformation (
+                  ControllerHandle,
+                  &gEfiPciIoProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  //
+  // Inspect if ChildHandle is one of the agents.
+  //
+  Status = EFI_UNSUPPORTED;
+  for (Index = 0; Index < EntryCount; Index++) {
+    //
+    // Query all the children created by the controller handle's driver
+    //
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+                      OpenInfoBuffer[Index].ControllerHandle,
+                      &gEfiDevicePathProtocolGuid,
+                      (VOID **) &ChildDevicePath,
+                      NULL,
+                      NULL,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      // 
+      // Check whether this device path include mac address device path.
+      //
+      if (!IsMacAddressDevicePath(ChildDevicePath, NextShowFormId,&IsNeedAdd)) {
+        //
+        // If this path not has mac address path, check the other.
+        //
+        continue;
+      } else {
+        //
+        // If need to update the NETWORK_DEVICE_LIST_FORM, try to get more.
+        //
+        if ((NETWORK_DEVICE_LIST_FORM_ID == NextShowFormId)) {
+          if (IsNeedAdd) {
+            (*ItemCount) += 1;
+          }
+          continue;
+        } else {
+          //
+          // If need to update other form, return whether need to add to the menu.
+          //          
+          goto Done;
+        }
+      }
+    }
+  }
+
+Done:
+  if (OpenInfoBuffer != NULL) {
+    FreePool (OpenInfoBuffer);  
+  }
+  return IsNeedAdd; 
+}
+
+/**
+  This  function  registers  HII  packages  to  HII  database.
+
+**/
+VOID
+InitializeDeviceManager (
+  VOID
+)
+{
+  EFI_STATUS                  Status;
+
+  if (!gConnectAllHappened){
+    EfiBootManagerConnectAll();
+    gConnectAllHappened = TRUE;
+  }
+
+  gDeviceManagerPrivate.DriverHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &gDeviceManagerPrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mDeviceManagerHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gDeviceManagerPrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Publish our HII data.
+  //
+  gDeviceManagerPrivate.HiiHandle = HiiAddPackages (
+                  &mDeviceManagerGuid,
+                  gDeviceManagerPrivate.DriverHandle,
+                  DeviceManagerVfrBin,
+                  UiAppStrings,
+                  NULL
+                  );
+  ASSERT (gDeviceManagerPrivate.HiiHandle != NULL);
+}
+
+/**
+  Remove the installed packages from the HII Database. 
+
+**/
+VOID
+FreeDeviceManager(
+  VOID
+)
+{
+  EFI_STATUS                  Status;
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gDeviceManagerPrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mDeviceManagerHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gDeviceManagerPrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  HiiRemovePackages (gDeviceManagerPrivate.HiiHandle);
+}
+
+/**
+  Dynamic create Hii information for Device Manager.
+
+  @param   NextShowFormId     The FormId which need to be show.
+
+**/
+VOID
+CreateDeviceManagerForm(
+  IN EFI_FORM_ID      NextShowFormId
+)
+{
+  UINTN                       Index;
+  EFI_STRING                  String;
+  EFI_STRING_ID               Token;
+  EFI_STRING_ID               TokenHelp;
+  EFI_HII_HANDLE              *HiiHandles;
+  EFI_HII_HANDLE              HiiHandle;
+  UINTN                       SkipCount;
+  EFI_STRING_ID               FormSetTitle;
+  EFI_STRING_ID               FormSetHelp;
+  EFI_GUID                    FormSetGuid;
+  VOID                        *StartOpCodeHandle;
+  VOID                        *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL          *StartLabel;
+  EFI_IFR_GUID_LABEL          *EndLabel;
+  BOOLEAN                     AddNetworkMenu;
+  UINTN                       AddItemCount;
+  UINTN                       NewStringLen;
+  EFI_STRING                  NewStringTitle;
+  CHAR16                      *DevicePathStr;
+  EFI_STRING_ID               DevicePathId;
+
+  HiiHandle = gDeviceManagerPrivate.HiiHandle;
+  AddNetworkMenu = FALSE;
+  AddItemCount = 0;
+
+  //
+  // If need show the Network device list form, clear the old save list first.
+  //
+  if ((NextShowFormId == NETWORK_DEVICE_LIST_FORM_ID) && (mMacDeviceList.CurListLen > 0)) {
+    mMacDeviceList.CurListLen = 0;
+  }
+
+  //
+  // Update the network device form titile.
+  //
+  if (NextShowFormId == NETWORK_DEVICE_FORM_ID) {
+    String = HiiGetString (HiiHandle, STRING_TOKEN (STR_FORM_NETWORK_DEVICE_TITLE), NULL);
+    NewStringLen = StrLen(mSelectedMacAddrString) * 2;
+    NewStringLen += (StrLen(String) + 2) * 2;
+    NewStringTitle = AllocatePool (NewStringLen);
+    UnicodeSPrint (NewStringTitle, NewStringLen, L"%s %s", String, mSelectedMacAddrString);
+    HiiSetString (HiiHandle, STRING_TOKEN (STR_FORM_NETWORK_DEVICE_TITLE), NewStringTitle, NULL);    
+    FreePool (String);
+    FreePool (NewStringTitle);
+  }
+
+  //
+  // Allocate space for creation of UpdateData Buffer
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  //
+  // According to the next show Form id(mNextShowFormId) to decide which form need to update.
+  //
+  StartLabel->Number       = (UINT16) (LABEL_FORM_ID_OFFSET + NextShowFormId);
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  //
+  // Get all the Hii handles
+  //
+  HiiHandles = HiiGetHiiHandles (NULL);
+  ASSERT (HiiHandles != NULL);
+
+  //
+  // Search for formset of each class type
+  //
+  SkipCount = 0;
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    //
+    //  The QuestionId in the form which will call the driver form has this asssumption.
+    //  QuestionId = Handle Index + NETWORK_DEVICE_LIST_KEY_OFFSET;
+    //  Different QuestionId at least has the section of NETWORK_DEVICE_LIST_KEY_OFFSET.
+    //
+    ASSERT(Index < MAX_KEY_SECTION_LEN);
+    if (!ExtractDisplayedHiiFormFromHiiHandle (HiiHandles[Index], &gEfiHiiPlatformSetupFormsetGuid, SkipCount, &FormSetTitle, &FormSetHelp, &FormSetGuid)) {
+      SkipCount = 0;
+      continue;
+    }
+
+    String = HiiGetString (HiiHandles[Index], FormSetTitle, NULL);
+    if (String == NULL) {
+      String = HiiGetString (HiiHandle, STR_MISSING_STRING, NULL);
+      ASSERT (String != NULL);
+    }
+    Token = HiiSetString (HiiHandle, 0, String, NULL);
+    FreePool (String);
+
+    String = HiiGetString (HiiHandles[Index], FormSetHelp, NULL);
+    if (String == NULL) {
+      String = HiiGetString (HiiHandle, STR_MISSING_STRING, NULL);
+      ASSERT (String != NULL);
+    }
+    TokenHelp = HiiSetString (HiiHandle, 0, String, NULL);
+    FreePool (String);
+
+    //
+    // Network device process
+    // 
+    if (IsNeedAddNetworkMenu (HiiHandles[Index], NextShowFormId,&AddItemCount)) {
+      if (NextShowFormId == DEVICE_MANAGER_FORM_ID) {
+        //
+        // Only show one menu item "Network Config" in the device manger form.
+        //
+        if (!AddNetworkMenu) {
+          AddNetworkMenu = TRUE;
+          HiiCreateGotoOpCode (
+            StartOpCodeHandle,
+            NETWORK_DEVICE_LIST_FORM_ID,
+            STRING_TOKEN (STR_FORM_NETWORK_DEVICE_LIST_TITLE),
+            STRING_TOKEN (STR_FORM_NETWORK_DEVICE_LIST_HELP),
+            EFI_IFR_FLAG_CALLBACK,
+            (EFI_QUESTION_ID) QUESTION_NETWORK_DEVICE_ID
+            );
+        }
+      } else if (NextShowFormId == NETWORK_DEVICE_LIST_FORM_ID) {
+        //
+        // In network device list form, same mac address device only show one menu.
+        //
+        while (AddItemCount > 0) {
+            HiiCreateGotoOpCode (
+              StartOpCodeHandle,
+              NETWORK_DEVICE_FORM_ID,
+              mMacDeviceList.NodeList[mMacDeviceList.CurListLen - AddItemCount].PromptId,
+              STRING_TOKEN (STR_NETWORK_DEVICE_HELP),
+              EFI_IFR_FLAG_CALLBACK,
+              mMacDeviceList.NodeList[mMacDeviceList.CurListLen - AddItemCount].QuestionId
+              );
+            AddItemCount -= 1;
+          }
+      } else if (NextShowFormId == NETWORK_DEVICE_FORM_ID) {
+        //
+        // In network device form, only the selected mac address device need to be show.
+        //
+        DevicePathStr = ExtractDevicePathFromHiiHandle(HiiHandles[Index]);
+        DevicePathId  = 0;
+        if (DevicePathStr != NULL){
+          DevicePathId =  HiiSetString (HiiHandle, 0, DevicePathStr, NULL);
+          FreePool(DevicePathStr);
+        }
+        HiiCreateGotoExOpCode (
+          StartOpCodeHandle,
+          0,
+          Token,
+          TokenHelp,
+          0,
+          (EFI_QUESTION_ID) (Index + DEVICE_KEY_OFFSET),
+          0,
+          &FormSetGuid,    
+          DevicePathId
+          );
+      }
+    } else {
+      //
+      // 
+      // Not network device process, only need to show at device manger form.
+      //
+      if (NextShowFormId == DEVICE_MANAGER_FORM_ID) {
+        DevicePathStr = ExtractDevicePathFromHiiHandle(HiiHandles[Index]);
+        DevicePathId  = 0;
+        if (DevicePathStr != NULL){
+          DevicePathId =  HiiSetString (HiiHandle, 0, DevicePathStr, NULL);
+          FreePool(DevicePathStr);
+        }
+        HiiCreateGotoExOpCode (
+          StartOpCodeHandle,
+          0,
+          Token,
+          TokenHelp,
+          0,
+          (EFI_QUESTION_ID) (Index + DEVICE_KEY_OFFSET),
+          0,
+          &FormSetGuid,
+          DevicePathId
+          );
+      }
+    }
+    //
+    //One packagelist may has more than one form package,
+    //Index-- means keep current HiiHandle and still extract from the packagelist, 
+    //SkipCount++ means skip the formset which was found before in the same form package. 
+    //
+    SkipCount++;
+    Index--;
+  }
+
+  HiiUpdateForm (
+    HiiHandle,
+    &mDeviceManagerGuid,
+    NextShowFormId,
+    StartOpCodeHandle,
+    EndOpCodeHandle
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+  FreePool (HiiHandles);
+}
+
diff --git a/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.h b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.h
new file mode 100644
index 0000000000..9c1c34160a
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManager.h
@@ -0,0 +1,155 @@
+/** @file
+  The platform device manager reference implement
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _DEVICE_MANAGER_H_
+#define _DEVICE_MANAGER_H_
+
+#include "Ui.h"
+#include "FrontPage.h"
+#include <Protocol/PciIo.h>
+
+//
+// These are defined as the same with vfr file
+//
+#define DEVICE_MANAGER_FORMSET_GUID  \
+  { \
+  0x3ebfa8e6, 0x511d, 0x4b5b, {0xa9, 0x5f, 0xfb, 0x38, 0x26, 0xf, 0x1c, 0x27} \
+  }
+
+#define LABEL_DEVICES_LIST                   0x1100
+#define LABEL_NETWORK_DEVICE_LIST_ID         0x1101
+#define LABEL_NETWORK_DEVICE_ID              0x1102
+#define LABEL_END                            0xffff
+#define LABEL_FORM_ID_OFFSET                 0x0100
+
+#define LABEL_VBIOS                          0x0040
+
+#define DEVICE_MANAGER_FORM_ID               0x1000
+#define NETWORK_DEVICE_LIST_FORM_ID          0x1001
+#define NETWORK_DEVICE_FORM_ID               0x1002
+#define DEVICE_KEY_OFFSET                    0x4000
+#define NETWORK_DEVICE_LIST_KEY_OFFSET       0x2000
+#define DEVICE_MANAGER_KEY_VBIOS             0x3000
+#define MAX_KEY_SECTION_LEN                  0x1000
+
+#define QUESTION_NETWORK_DEVICE_ID           0x3FFF
+//
+// These are the VFR compiler generated data representing our VFR data.
+//
+extern UINT8  DeviceManagerVfrBin[];
+
+#define DEVICE_MANAGER_CALLBACK_DATA_SIGNATURE  SIGNATURE_32 ('D', 'M', 'C', 'B')
+
+
+typedef struct {
+  UINTN                           Signature;
+
+  ///
+  /// Device Manager HII relative handles
+  ///
+  EFI_HII_HANDLE                  HiiHandle;
+
+  EFI_HANDLE                      DriverHandle;
+
+  ///
+  /// Device Manager Produced protocols
+  ///
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  ConfigAccess;
+
+  ///
+  /// Configuration data
+  ///
+  UINT8                           VideoBios;
+} DEVICE_MANAGER_CALLBACK_DATA;
+
+typedef struct {
+  EFI_STRING_ID    PromptId;
+  EFI_QUESTION_ID  QuestionId;
+}MENU_INFO_ITEM;
+
+typedef struct {
+  UINTN           CurListLen;
+  UINTN           MaxListLen;
+  MENU_INFO_ITEM  *NodeList;
+} MAC_ADDRESS_NODE_LIST;
+
+#define DEVICE_MANAGER_CALLBACK_DATA_FROM_THIS(a) \
+  CR (a, \
+      DEVICE_MANAGER_CALLBACK_DATA, \
+      ConfigAccess, \
+      DEVICE_MANAGER_CALLBACK_DATA_SIGNATURE \
+      )
+typedef struct {
+  EFI_STRING_ID  StringId;
+  UINT16         Class;
+} DEVICE_MANAGER_MENU_ITEM;
+
+/**
+  This function is invoked if user selected a interactive opcode from Device Manager's
+  Formset. If user set VBIOS, the new value is saved to EFI variable.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+DeviceManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  );
+
+/**
+  This  function  registers  HII  packages  to  HII  database.
+
+**/
+VOID
+InitializeDeviceManager (
+  VOID
+  );
+
+/**
+  Remove the installed  packages from the HII Database. 
+
+**/
+VOID
+FreeDeviceManager(
+  VOID
+  );
+
+/**
+  Dynamic create Hii information for Device Manager.
+
+  @param   NextShowFormId     The FormId which need to be show.
+
+**/
+VOID
+CreateDeviceManagerForm(
+  IN EFI_FORM_ID      NextShowFormId
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerStrings.uni b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerStrings.uni
new file mode 100644
index 0000000000..551df81736
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerStrings.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerVfr.Vfr b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerVfr.Vfr
new file mode 100644
index 0000000000..2c575f56b0
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DeviceManagerVfr.Vfr
@@ -0,0 +1,102 @@
+///** @file
+//  
+//    Device Manager formset.
+//  
+//  Copyright (c) 2004 - 2015, 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.
+//  
+//**/
+
+#define FORMSET_GUID  { 0x3ebfa8e6, 0x511d, 0x4b5b, 0xa9, 0x5f, 0xfb, 0x38, 0x26, 0xf, 0x1c, 0x27 }
+
+#define EFI_DISK_DEVICE_CLASS              0x0001
+#define EFI_VIDEO_DEVICE_CLASS             0x0002
+#define EFI_NETWORK_DEVICE_CLASS           0x0004
+#define EFI_INPUT_DEVICE_CLASS             0x0008
+#define EFI_ON_BOARD_DEVICE_CLASS          0x0010
+#define EFI_OTHER_DEVICE_CLASS             0x0020
+#define LABEL_VBIOS                        0x0040
+#define LABEL_DEVICES_LIST                 0x1100
+#define LABEL_NETWORK_DEVICE_LIST_ID       0x1101
+#define LABEL_NETWORK_DEVICE_ID            0x1102
+#define LABEL_END                          0xffff
+
+#define DEVICE_MANAGER_CLASS               0x0000
+#define FRONT_PAGE_SUBCLASS                0x0003
+
+#define DEVICE_MANAGER_FORM_ID             0x1000
+#define NETWORK_DEVICE_LIST_FORM_ID        0x1001
+#define NETWORK_DEVICE_FORM_ID             0x1002
+
+formset
+  guid      = FORMSET_GUID,
+  title     = STRING_TOKEN(STR_EDKII_MENU_TITLE),
+  help      = STRING_TOKEN(STR_EMPTY_STRING),
+  classguid = FORMSET_GUID,
+
+  form formid = DEVICE_MANAGER_FORM_ID,
+       title  = STRING_TOKEN(STR_EDKII_MENU_TITLE);
+
+    subtitle text = STRING_TOKEN(STR_DEVICES_LIST);
+    //
+    // This is where devices get added to the device manager hierarchy
+    //
+    label EFI_DISK_DEVICE_CLASS;
+//    label LABEL_END;  // Since next opcode is a label, so this one could be omitted to save code size
+
+    label EFI_VIDEO_DEVICE_CLASS;
+//    label LABEL_END;
+
+    label EFI_NETWORK_DEVICE_CLASS;
+//    label LABEL_END;
+
+    label EFI_INPUT_DEVICE_CLASS;
+//    label LABEL_END;
+
+    label EFI_ON_BOARD_DEVICE_CLASS;
+//    label LABEL_END;
+
+//    label EFI_OTHER_DEVICE_CLASS;
+
+    label LABEL_DEVICES_LIST;
+    label LABEL_END;
+      
+    subtitle text = STRING_TOKEN(STR_EMPTY_STRING);
+
+    label LABEL_VBIOS;
+    label LABEL_END;
+    
+    subtitle text = STRING_TOKEN(STR_EMPTY_STRING);
+    subtitle text = STRING_TOKEN(STR_EXIT_STRING);
+
+  endform;
+
+  form formid = NETWORK_DEVICE_LIST_FORM_ID,
+       title = STRING_TOKEN(STR_FORM_NETWORK_DEVICE_LIST_TITLE);
+
+    subtitle text = STRING_TOKEN(STR_NETWORK_DEVICE_LIST_STRING);
+
+       label LABEL_NETWORK_DEVICE_LIST_ID;
+       label LABEL_END;
+       subtitle text = STRING_TOKEN(STR_EMPTY_STRING);
+    subtitle text = STRING_TOKEN(STR_EXIT_STRING);
+  endform;
+
+  form formid = NETWORK_DEVICE_FORM_ID,
+       title = STRING_TOKEN(STR_FORM_NETWORK_DEVICE_TITLE);
+
+    subtitle text = STRING_TOKEN(STR_NETWORK_DEVICE_STRING);
+
+       label LABEL_NETWORK_DEVICE_ID;
+       label LABEL_END;
+       subtitle text = STRING_TOKEN(STR_EMPTY_STRING);
+    subtitle text = STRING_TOKEN(STR_EXIT_STRING);
+  endform;
+endformset;
+
diff --git a/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DriverHealthVfr.Vfr b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DriverHealthVfr.Vfr
new file mode 100644
index 0000000000..aa540563e7
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/DeviceMngr/DriverHealthVfr.Vfr
@@ -0,0 +1,45 @@
+///** @file
+//  
+//    Driver Health formset.
+//  
+//  Copyright (c) 2004 - 2015, 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.
+//  
+//**/
+
+#define DRIVER_HEALTH_FORMSET_GUID  { 0xf76e0a70, 0xb5ed, 0x4c38, 0xac, 0x9a, 0xe5, 0xf5, 0x4b, 0xf1, 0x6e, 0x34 }
+
+#define LABEL_DRIVER_HEALTH                0x2000
+#define LABEL_DRIVER_HEALTH_END            0x2001
+#define LABEL_DRIVER_HEALTH_REAPIR_ALL     0x3000
+#define LABEL_DRIVER_HEALTH_REAPIR_ALL_END 0x3001
+
+#define DRIVER_HEALTH_FORM_ID              0x1003
+
+formset
+  guid      = DRIVER_HEALTH_FORMSET_GUID,
+  title     = STRING_TOKEN(STR_DH_BANNER),
+  help      = STRING_TOKEN(STR_EMPTY_STRING),
+  classguid = DRIVER_HEALTH_FORMSET_GUID,
+ 
+  form formid = DRIVER_HEALTH_FORM_ID,
+      title  = STRING_TOKEN(STR_DH_BANNER);
+
+      label LABEL_DRIVER_HEALTH;
+      label LABEL_DRIVER_HEALTH_END;  
+            
+      subtitle text = STRING_TOKEN(STR_LAST_STRING);
+      label LABEL_DRIVER_HEALTH_REAPIR_ALL;
+      label LABEL_DRIVER_HEALTH_REAPIR_ALL_END;
+            
+      subtitle text = STRING_TOKEN(STR_LAST_STRING);
+      subtitle text = STRING_TOKEN(STR_HELP_FOOTER);
+      subtitle text = STRING_TOKEN(STR_LAST_STRING);      
+  endform;   
+endformset;
diff --git a/Core/MdeModulePkg/Application/UiApp/FormsetGuid.h b/Core/MdeModulePkg/Application/UiApp/FormsetGuid.h
new file mode 100644
index 0000000000..68a5a3f481
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/FormsetGuid.h
@@ -0,0 +1,51 @@
+/** @file
+  FrontPage routines to handle the callbacks and browser calls
+
+Copyright (c) 2013 - 2015, 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.
+
+**/
+
+
+//**/
+#ifndef _FORMSET_GUID_H_
+#define _FORMSET_GUID_H_
+
+#include "BootMaint/FormGuid.h"
+
+#define FRONT_PAGE_FORMSET_GUID  { 0x9e0c30bc, 0x3f06, 0x4ba6, 0x82, 0x88, 0x9, 0x17, 0x9b, 0x85, 0x5d, 0xbe }
+
+// Used by Boot manager form
+#define BOOT_MANAGER_FORMSET_GUID  { 0x847bc3fe, 0xb974, 0x446d, 0x94, 0x49, 0x5a, 0xd5, 0x41, 0x2e, 0x99, 0x3b }
+
+#define BOOT_MANAGER_FORM_ID     0x1000
+
+
+// Used by Device manager form.
+#define DEVICE_MANAGER_FORMSET_GUID  { 0x3ebfa8e6, 0x511d, 0x4b5b, 0xa9, 0x5f, 0xfb, 0x38, 0x26, 0xf, 0x1c, 0x27 }
+
+#define DEVICE_MANAGER_FORM_ID             0x2000
+#define NETWORK_DEVICE_LIST_FORM_ID        0x2001
+#define NETWORK_DEVICE_FORM_ID             0x2002
+
+#define DEVICE_KEY_OFFSET                  0x2000
+#define NETWORK_DEVICE_LIST_KEY_OFFSET     0x2100
+#define MAX_KEY_SECTION_LEN                0x0100
+
+#define QUESTION_NETWORK_DEVICE_ID         0x2FFF
+
+#define LABEL_DEVICES_LIST                 0x2100
+#define LABEL_NETWORK_DEVICE_LIST_ID       0x2101
+#define LABEL_NETWORK_DEVICE_ID            0x2102
+#define LABEL_END                          0xffff
+#define LABEL_FORM_ID_OFFSET               0x0100
+#define LABEL_VBIOS                        0x0040
+
+
+#endif
diff --git a/Core/MdeModulePkg/Application/UiApp/FrontPage.c b/Core/MdeModulePkg/Application/UiApp/FrontPage.c
new file mode 100644
index 0000000000..906c5900de
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/FrontPage.c
@@ -0,0 +1,1606 @@
+/** @file
+  FrontPage routines to handle the callbacks and browser calls
+
+Copyright (c) 2004 - 2015, 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 "FrontPage.h"
+#include "Language.h"
+#define MAX_STRING_LEN        200
+
+EFI_GUID  mFrontPageGuid      = FRONT_PAGE_FORMSET_GUID;
+
+BOOLEAN   gConnectAllHappened = FALSE;
+BOOLEAN   mFeaturerSwitch = TRUE;
+BOOLEAN   mResetRequired  = FALSE;
+BOOLEAN   mEnterBmm       = FALSE;
+
+EFI_FORM_BROWSER2_PROTOCOL      *gFormBrowser2;
+CHAR8     *mLanguageString;
+BOOLEAN   mModeInitialized = FALSE;
+//
+// Boot video resolution and text mode.
+//
+UINT32    mBootHorizontalResolution    = 0;
+UINT32    mBootVerticalResolution      = 0;
+UINT32    mBootTextModeColumn          = 0;
+UINT32    mBootTextModeRow             = 0;
+//
+// BIOS setup video resolution and text mode.
+//
+UINT32    mSetupTextModeColumn         = 0;
+UINT32    mSetupTextModeRow            = 0;
+UINT32    mSetupHorizontalResolution   = 0;
+UINT32    mSetupVerticalResolution     = 0;
+
+FRONT_PAGE_CALLBACK_DATA  gFrontPagePrivate = {
+  FRONT_PAGE_CALLBACK_DATA_SIGNATURE,
+  NULL,
+  NULL,
+  NULL,
+  {
+    FakeExtractConfig,
+    FakeRouteConfig,
+    FrontPageCallback
+  }
+};
+
+HII_VENDOR_DEVICE_PATH  mFrontPageHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    //
+    // {8E6D99EE-7531-48f8-8745-7F6144468FF2}
+    //
+    { 0x8e6d99ee, 0x7531, 0x48f8, { 0x87, 0x45, 0x7f, 0x61, 0x44, 0x46, 0x8f, 0xf2 } }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  Update the banner information for the Front Page based on Smbios information.
+
+**/
+VOID
+UpdateFrontPageStrings (
+  VOID
+  );
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Request         A null-terminated Unicode string in <ConfigRequest> format.
+  @param Progress        On return, points to a character in the Request string.
+                         Points to the string's null terminator if request was successful.
+                         Points to the most recent '&' before the first failing name/value
+                         pair (or the beginning of the string if the failure is in the
+                         first name/value pair) if the request was not successful.
+  @param Results         A null-terminated Unicode string in <ConfigAltResp> format which
+                         has all values filled in for the names in the Request string.
+                         String to be allocated by the called function.
+
+  @retval  EFI_SUCCESS            The Results is filled with the requested values.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval  EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FakeExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Request;
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Configuration   A null-terminated Unicode string in <ConfigResp> format.
+  @param Progress        A pointer to a string filled in with the offset of the most
+                         recent '&' before the first failing name/value pair (or the
+                         beginning of the string if the failure is in the first
+                         name/value pair) or the terminating NULL if all was successful.
+
+  @retval  EFI_SUCCESS            The Results is processed successfully.
+  @retval  EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FakeRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Configuration;
+  if (!HiiIsConfigHdrMatch (Configuration, &mBootMaintGuid, mBootMaintStorageName)
+      && !HiiIsConfigHdrMatch (Configuration, &mFileExplorerGuid, mFileExplorerStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Progress = Configuration + StrLen (Configuration);
+  return EFI_SUCCESS;
+}
+
+/**
+  Create oneof options for language.
+
+**/
+VOID
+InitializeLanguage (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  CHAR8                       *LangCode;
+  CHAR8                       *Lang;
+  UINTN                       LangSize;
+  CHAR8                       *CurrentLang;
+  UINTN                       OptionCount;
+  CHAR16                      *StringBuffer;
+  EFI_HII_HANDLE              HiiHandle;
+  VOID                        *OptionsOpCodeHandle;
+  VOID                        *StartOpCodeHandle;
+  VOID                        *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL          *StartLabel;
+  EFI_IFR_GUID_LABEL          *EndLabel;    
+  EFI_HII_STRING_PROTOCOL     *HiiString;
+  UINTN                       StringSize;
+
+  Lang         = NULL;
+  StringBuffer = NULL;
+
+  //
+  // Init OpCode Handle and Allocate space for creation of UpdateData Buffer
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = LABEL_SELECT_LANGUAGE;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+  //
+  // Collect the languages from what our current Language support is based on our VFR
+  //
+  HiiHandle = gFrontPagePrivate.HiiHandle;
+
+  GetEfiGlobalVariable2 (L"PlatformLang", (VOID**)&CurrentLang, NULL);
+
+  if (mLanguageString == NULL) {
+    //
+    // Get Support language list from variable.
+    //
+    GetEfiGlobalVariable2 (L"PlatformLangCodes", (VOID**)&mLanguageString, NULL);
+    if (mLanguageString == NULL) {
+      mLanguageString = AllocateCopyPool (
+                                 AsciiStrSize ((CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultPlatformLangCodes)),
+                                 (CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultPlatformLangCodes)
+                                 );
+      ASSERT (mLanguageString != NULL);
+    }
+  }
+
+  if (gFrontPagePrivate.LanguageToken == NULL) {
+    //
+    // Count the language list number.
+    //  
+    LangCode      = mLanguageString;
+    Lang          = AllocatePool (AsciiStrSize (mLanguageString));
+    ASSERT (Lang != NULL);
+    OptionCount = 0;
+    while (*LangCode != 0) {
+      GetNextLanguage (&LangCode, Lang);
+      OptionCount ++;
+    }
+
+    //
+    // Allocate extra 1 as the end tag.
+    //
+    gFrontPagePrivate.LanguageToken = AllocateZeroPool ((OptionCount + 1) * sizeof (EFI_STRING_ID));
+    ASSERT (gFrontPagePrivate.LanguageToken != NULL);
+
+    Status = gBS->LocateProtocol (&gEfiHiiStringProtocolGuid, NULL, (VOID **) &HiiString);
+    ASSERT_EFI_ERROR (Status);
+
+    LangCode     = mLanguageString;
+    OptionCount  = 0;
+    while (*LangCode != 0) {
+      GetNextLanguage (&LangCode, Lang);
+
+      StringSize = 0;
+      Status = HiiString->GetString (HiiString, Lang, HiiHandle, PRINTABLE_LANGUAGE_NAME_STRING_ID, StringBuffer, &StringSize, NULL);
+      if (Status == EFI_BUFFER_TOO_SMALL) {
+        StringBuffer = AllocateZeroPool (StringSize);
+        ASSERT (StringBuffer != NULL);
+        Status = HiiString->GetString (HiiString, Lang, HiiHandle, PRINTABLE_LANGUAGE_NAME_STRING_ID, StringBuffer, &StringSize, NULL);
+        ASSERT_EFI_ERROR (Status);
+      }
+
+      if (EFI_ERROR (Status)) {
+        LangSize = AsciiStrSize (Lang);
+        StringBuffer = AllocatePool (LangSize * sizeof (CHAR16));
+        ASSERT (StringBuffer != NULL);
+        AsciiStrToUnicodeStrS (Lang, StringBuffer, LangSize);
+      }
+
+      ASSERT (StringBuffer != NULL);
+      gFrontPagePrivate.LanguageToken[OptionCount] = HiiSetString (HiiHandle, 0, StringBuffer, NULL);
+      FreePool (StringBuffer);
+
+      OptionCount++;
+    }
+  }
+
+  ASSERT (gFrontPagePrivate.LanguageToken != NULL);
+  LangCode     = mLanguageString;
+  OptionCount  = 0;
+  if (Lang == NULL) {
+    Lang = AllocatePool (AsciiStrSize (mLanguageString));
+    ASSERT (Lang != NULL);
+  }
+  while (*LangCode != 0) {
+    GetNextLanguage (&LangCode, Lang);
+
+    if (CurrentLang != NULL && AsciiStrCmp (Lang, CurrentLang) == 0) {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        gFrontPagePrivate.LanguageToken[OptionCount],
+        EFI_IFR_OPTION_DEFAULT,
+        EFI_IFR_NUMERIC_SIZE_1,
+        (UINT8) OptionCount
+        );
+    } else {
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        gFrontPagePrivate.LanguageToken[OptionCount],
+        0,
+        EFI_IFR_NUMERIC_SIZE_1,
+        (UINT8) OptionCount
+        );
+    }
+
+    OptionCount++;
+  }
+
+  if (CurrentLang != NULL) {
+    FreePool (CurrentLang);
+  }
+  FreePool (Lang);
+
+  HiiCreateOneOfOpCode (
+    StartOpCodeHandle,
+    FRONT_PAGE_KEY_LANGUAGE,
+    0,
+    0,
+    STRING_TOKEN (STR_LANGUAGE_SELECT),
+    STRING_TOKEN (STR_LANGUAGE_SELECT_HELP),
+    EFI_IFR_FLAG_CALLBACK,
+    EFI_IFR_NUMERIC_SIZE_1,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  Status = HiiUpdateForm (
+             HiiHandle,
+             &mFrontPageGuid,
+             FRONT_PAGE_FORM_ID,
+             StartOpCodeHandle, // LABEL_SELECT_LANGUAGE
+             EndOpCodeHandle    // LABEL_END
+             );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the variable and its data.
+  @retval  EFI_DEVICE_ERROR      The variable could not be saved.
+  @retval  EFI_UNSUPPORTED       The specified Action is not supported by the callback.
+
+**/
+EFI_STATUS
+EFIAPI
+FrontPageCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  CHAR8                         *LangCode;
+  CHAR8                         *Lang;
+  UINTN                         Index;
+  EFI_STATUS                    Status;
+
+  //
+  //Chech whether exit from BMM and reenter frontpage,if yes,reclaim string depositories
+  //
+  if (Action == EFI_BROWSER_ACTION_FORM_OPEN){
+    if (mEnterBmm){
+      ReclaimStringDepository();
+      mEnterBmm = FALSE;
+    }
+  }
+
+  if (Action != EFI_BROWSER_ACTION_CHANGING && Action != EFI_BROWSER_ACTION_CHANGED) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    if ((Value == NULL) || (ActionRequest == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    switch (QuestionId) {
+    case FRONT_PAGE_KEY_CONTINUE:
+      //
+      // This is the continue - clear the screen and return an error to get out of FrontPage loop
+      //
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_EXIT;
+      break;
+
+    case FRONT_PAGE_KEY_LANGUAGE:
+      //
+      // Allocate working buffer for RFC 4646 language in supported LanguageString.
+      //
+      Lang = AllocatePool (AsciiStrSize (mLanguageString));
+      ASSERT (Lang != NULL);  
+
+      Index = 0;
+      LangCode = mLanguageString;
+      while (*LangCode != 0) {
+        GetNextLanguage (&LangCode, Lang);
+
+        if (Index == Value->u8) {
+          break;
+        }
+
+        Index++;
+      }
+
+      if (Index == Value->u8) {
+        Status = gRT->SetVariable (
+                        L"PlatformLang",
+                        &gEfiGlobalVariableGuid,
+                        EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                        AsciiStrSize (Lang),
+                        Lang
+                        );
+        ASSERT_EFI_ERROR(Status);
+      } else {
+        ASSERT (FALSE);
+      }
+      FreePool (Lang);
+      //
+      //Current language of platform is changed,recreate oneof options for language.
+      //
+      InitializeLanguage();
+      break;
+
+    default:
+      break;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if (Value == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // The first 4 entries in the Front Page are to be GUARANTEED to remain constant so IHV's can
+    // describe to their customers in documentation how to find their setup information (namely
+    // under the device manager and specific buckets)
+    //
+    switch (QuestionId) {
+    case FRONT_PAGE_KEY_BOOT_MANAGER:
+      //
+      // Boot Manager
+      //
+      EnumerateBootOptions ();
+      break;
+
+    case FRONT_PAGE_KEY_DEVICE_MANAGER:
+      //
+      // Device Manager
+      //
+      CreateDeviceManagerForm(DEVICE_MANAGER_FORM_ID);
+      break;
+
+    case FRONT_PAGE_KEY_BOOT_MAINTAIN:
+      //
+      // Boot Maintenance Manager
+      //
+      InitializeBM ();
+      mEnterBmm  = TRUE;
+      break;
+
+    default:
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize HII information for the FrontPage
+
+
+  @retval  EFI_SUCCESS        The operation is successful.
+  @retval  EFI_DEVICE_ERROR   If the dynamic opcode creation failed.
+
+**/
+EFI_STATUS
+InitializeFrontPage (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+
+  //
+  // Locate Hii relative protocols
+  //
+  Status = gBS->LocateProtocol (&gEfiFormBrowser2ProtocolGuid, NULL, (VOID **) &gFormBrowser2);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  //
+  gFrontPagePrivate.DriverHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &gFrontPagePrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mFrontPageHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gFrontPagePrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Publish our HII data
+  //
+  gFrontPagePrivate.HiiHandle = HiiAddPackages (
+                                  &mFrontPageGuid,
+                                  gFrontPagePrivate.DriverHandle,
+                                  FrontPageVfrBin,
+                                  UiAppStrings,
+                                  NULL
+                                  );
+  ASSERT (gFrontPagePrivate.HiiHandle != NULL);
+
+  //
+  //Updata Front Page strings
+  //
+  UpdateFrontPageStrings ();
+
+  //
+  // Initialize laguage options
+  //
+  InitializeLanguage ();
+
+  return Status;
+}
+
+/**
+  Call the browser and display the front page
+
+  @return   Status code that will be returned by
+            EFI_FORM_BROWSER2_PROTOCOL.SendForm ().
+
+**/
+EFI_STATUS
+CallFrontPage (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_BROWSER_ACTION_REQUEST  ActionRequest;
+
+  //
+  // Begin waiting for USER INPUT
+  //
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_PC_INPUT_WAIT)
+    );
+
+  ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+  Status = gFormBrowser2->SendForm (
+                            gFormBrowser2,
+                            &gFrontPagePrivate.HiiHandle,
+                            1,
+                            &mFrontPageGuid,
+                            0,
+                            NULL,
+                            &ActionRequest
+                            );
+  //
+  // Check whether user change any option setting which needs a reset to be effective
+  //
+  if (ActionRequest == EFI_BROWSER_ACTION_REQUEST_RESET) {
+    EnableResetRequired ();
+  }
+
+  return Status;
+}
+
+/**
+  Remove the installed packages from the HiiDatabase.
+
+**/
+VOID
+FreeFrontPage(
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gFrontPagePrivate.DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mFrontPageHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &gFrontPagePrivate.ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Publish our HII data
+  //
+  HiiRemovePackages (gFrontPagePrivate.HiiHandle);
+  if (gFrontPagePrivate.LanguageToken != NULL) {
+    FreePool (gFrontPagePrivate.LanguageToken);
+    gFrontPagePrivate.LanguageToken = NULL;
+  }
+}
+
+/**
+  Convert Processor Frequency Data to a string.
+
+  @param ProcessorFrequency The frequency data to process
+  @param Base10Exponent     The exponent based on 10
+  @param String             The string that is created
+
+**/
+VOID
+ConvertProcessorToString (
+  IN  UINT16                               ProcessorFrequency,
+  IN  UINT16                               Base10Exponent,
+  OUT CHAR16                               **String
+  )
+{
+  CHAR16  *StringBuffer;
+  UINTN   Index;
+  UINTN   DestMax;
+  UINT32  FreqMhz;
+
+  if (Base10Exponent >= 6) {
+    FreqMhz = ProcessorFrequency;
+    for (Index = 0; Index < (UINTN) (Base10Exponent - 6); Index++) {
+      FreqMhz *= 10;
+    }
+  } else {
+    FreqMhz = 0;
+  }
+  DestMax = 0x20 / sizeof (CHAR16);
+  StringBuffer = AllocateZeroPool (0x20);
+  ASSERT (StringBuffer != NULL);
+  Index = UnicodeValueToString (StringBuffer, LEFT_JUSTIFY, FreqMhz / 1000, 3);
+  StrCatS (StringBuffer, DestMax, L".");
+  UnicodeValueToString (StringBuffer + Index + 1, PREFIX_ZERO, (FreqMhz % 1000) / 10, 2);
+  StrCatS (StringBuffer, DestMax, L" GHz");
+  *String = (CHAR16 *) StringBuffer;
+  return ;
+}
+
+
+/**
+  Convert Memory Size to a string.
+
+  @param MemorySize      The size of the memory to process
+  @param String          The string that is created
+
+**/
+VOID
+ConvertMemorySizeToString (
+  IN  UINT32          MemorySize,
+  OUT CHAR16          **String
+  )
+{
+  CHAR16  *StringBuffer;
+
+  StringBuffer = AllocateZeroPool (0x24);
+  ASSERT (StringBuffer != NULL);
+  UnicodeValueToString (StringBuffer, LEFT_JUSTIFY, MemorySize, 10);
+  StrCatS (StringBuffer, 0x24 / sizeof (CHAR16), L" MB RAM");
+
+  *String = (CHAR16 *) StringBuffer;
+
+  return ;
+}
+
+/**
+
+  Acquire the string associated with the Index from smbios structure and return it.
+  The caller is responsible for free the string buffer.
+
+  @param    OptionalStrStart  The start position to search the string
+  @param    Index             The index of the string to extract
+  @param    String            The string that is extracted
+
+  @retval   EFI_SUCCESS       The function returns EFI_SUCCESS always.
+
+**/
+EFI_STATUS
+GetOptionalStringByIndex (
+  IN      CHAR8                   *OptionalStrStart,
+  IN      UINT8                   Index,
+  OUT     CHAR16                  **String
+  )
+{
+  UINTN          StrSize;
+
+  if (Index == 0) {
+    *String = AllocateZeroPool (sizeof (CHAR16));
+    return EFI_SUCCESS;
+  }
+
+  StrSize = 0;
+  do {
+    Index--;
+    OptionalStrStart += StrSize;
+    StrSize           = AsciiStrSize (OptionalStrStart);
+  } while (OptionalStrStart[StrSize] != 0 && Index != 0);
+
+  if ((Index != 0) || (StrSize == 1)) {
+    //
+    // Meet the end of strings set but Index is non-zero, or
+    // Find an empty string
+    //
+    *String = GetStringById (STRING_TOKEN (STR_MISSING_STRING));
+  } else {
+    *String = AllocatePool (StrSize * sizeof (CHAR16));
+    AsciiStrToUnicodeStrS (OptionalStrStart, *String, StrSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Update the banner information for the Front Page based on Smbios information.
+**/
+VOID
+UpdateFrontPageStrings (
+  VOID
+  )
+{
+  UINT8                             StrIndex;
+  CHAR16                            *NewString;
+  CHAR16                            *FirmwareVersionString;
+  BOOLEAN                           Find[5];
+  EFI_STATUS                        Status;
+  EFI_STRING_ID                     TokenToUpdate;
+  EFI_SMBIOS_HANDLE                 SmbiosHandle;
+  EFI_SMBIOS_PROTOCOL               *Smbios;
+  SMBIOS_TABLE_TYPE0                *Type0Record;
+  SMBIOS_TABLE_TYPE1                *Type1Record;
+  SMBIOS_TABLE_TYPE4                *Type4Record;
+  SMBIOS_TABLE_TYPE19               *Type19Record;
+  EFI_SMBIOS_TABLE_HEADER           *Record;
+
+  ZeroMem (Find, sizeof (Find));
+
+  //
+  // Update Front Page strings
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmbiosProtocolGuid,
+                  NULL,
+                  (VOID **) &Smbios
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  SmbiosHandle = SMBIOS_HANDLE_PI_RESERVED;
+  do {
+    Status = Smbios->GetNext (Smbios, &SmbiosHandle, NULL, &Record, NULL);
+    if (EFI_ERROR(Status)) {
+      break;
+    }
+
+    if (Record->Type == EFI_SMBIOS_TYPE_BIOS_INFORMATION) {
+      Type0Record = (SMBIOS_TABLE_TYPE0 *) Record;
+      StrIndex = Type0Record->BiosVersion;
+      GetOptionalStringByIndex ((CHAR8*)((UINT8*)Type0Record + Type0Record->Hdr.Length), StrIndex, &NewString);
+      TokenToUpdate = STRING_TOKEN (STR_FRONT_PAGE_BIOS_VERSION);
+      FirmwareVersionString = (CHAR16 *) PcdGetPtr (PcdFirmwareVersionString);
+      if (*FirmwareVersionString != 0x0000 ) {
+        FreePool (NewString);
+        NewString = (CHAR16 *) PcdGetPtr (PcdFirmwareVersionString);
+        HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+      } else {
+        HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+        FreePool (NewString);
+      }
+      Find[0] = TRUE;
+    }  
+
+    if (Record->Type == EFI_SMBIOS_TYPE_SYSTEM_INFORMATION) {
+      Type1Record = (SMBIOS_TABLE_TYPE1 *) Record;
+      StrIndex = Type1Record->ProductName;
+      GetOptionalStringByIndex ((CHAR8*)((UINT8*)Type1Record + Type1Record->Hdr.Length), StrIndex, &NewString);
+      TokenToUpdate = STRING_TOKEN (STR_FRONT_PAGE_COMPUTER_MODEL);
+      HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+      FreePool (NewString);
+      Find[1] = TRUE;
+    }
+
+    if ((Record->Type == EFI_SMBIOS_TYPE_PROCESSOR_INFORMATION) && !Find[2]) {
+      Type4Record = (SMBIOS_TABLE_TYPE4 *) Record;
+      //
+      // The information in the record should be only valid when the CPU Socket is populated. 
+      //
+      if ((Type4Record->Status & SMBIOS_TYPE4_CPU_SOCKET_POPULATED) == SMBIOS_TYPE4_CPU_SOCKET_POPULATED) {
+        StrIndex = Type4Record->ProcessorVersion;
+        GetOptionalStringByIndex ((CHAR8*)((UINT8*)Type4Record + Type4Record->Hdr.Length), StrIndex, &NewString);
+        TokenToUpdate = STRING_TOKEN (STR_FRONT_PAGE_CPU_MODEL);
+        HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+        FreePool (NewString);
+        Find[2] = TRUE;
+      }
+    }    
+
+    if ((Record->Type == EFI_SMBIOS_TYPE_PROCESSOR_INFORMATION) && !Find[3]) {
+      Type4Record = (SMBIOS_TABLE_TYPE4 *) Record;
+      //
+      // The information in the record should be only valid when the CPU Socket is populated. 
+      //
+      if ((Type4Record->Status & SMBIOS_TYPE4_CPU_SOCKET_POPULATED) == SMBIOS_TYPE4_CPU_SOCKET_POPULATED) {
+        ConvertProcessorToString(Type4Record->CurrentSpeed, 6, &NewString);
+        TokenToUpdate = STRING_TOKEN (STR_FRONT_PAGE_CPU_SPEED);
+        HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+        FreePool (NewString);
+        Find[3] = TRUE;
+      }
+    } 
+
+    if ( Record->Type == EFI_SMBIOS_TYPE_MEMORY_ARRAY_MAPPED_ADDRESS ) {
+      Type19Record = (SMBIOS_TABLE_TYPE19 *) Record;
+      ConvertMemorySizeToString (
+        (UINT32)(RShiftU64((Type19Record->EndingAddress - Type19Record->StartingAddress + 1), 10)),
+        &NewString
+        );
+      TokenToUpdate = STRING_TOKEN (STR_FRONT_PAGE_MEMORY_SIZE);
+      HiiSetString (gFrontPagePrivate.HiiHandle, TokenToUpdate, NewString, NULL);
+      FreePool (NewString);
+      Find[4] = TRUE;  
+    }
+  } while ( !(Find[0] && Find[1] && Find[2] && Find[3] && Find[4]));
+  return ;
+}
+
+/**
+  This function will change video resolution and text mode
+  according to defined setup mode or defined boot mode  
+
+  @param  IsSetupMode   Indicate mode is changed to setup mode or boot mode. 
+
+  @retval  EFI_SUCCESS  Mode is changed successfully.
+  @retval  Others             Mode failed to be changed.
+
+**/
+EFI_STATUS
+EFIAPI
+BdsSetConsoleMode (
+  BOOLEAN  IsSetupMode
+  )
+{
+  EFI_GRAPHICS_OUTPUT_PROTOCOL          *GraphicsOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL       *SimpleTextOut;
+  UINTN                                 SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  *Info;
+  UINT32                                MaxGopMode;
+  UINT32                                MaxTextMode;
+  UINT32                                ModeNumber;
+  UINT32                                NewHorizontalResolution;
+  UINT32                                NewVerticalResolution;
+  UINT32                                NewColumns;
+  UINT32                                NewRows;
+  UINTN                                 HandleCount;
+  EFI_HANDLE                            *HandleBuffer;
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  UINTN                                 CurrentColumn;
+  UINTN                                 CurrentRow;  
+
+  MaxGopMode  = 0;
+  MaxTextMode = 0;
+
+  //
+  // Get current video resolution and text mode 
+  //
+  Status = gBS->HandleProtocol (
+                  gST->ConsoleOutHandle,
+                  &gEfiGraphicsOutputProtocolGuid,
+                  (VOID**)&GraphicsOutput
+                  );
+  if (EFI_ERROR (Status)) {
+    GraphicsOutput = NULL;
+  }
+
+  Status = gBS->HandleProtocol (
+                  gST->ConsoleOutHandle,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  (VOID**)&SimpleTextOut
+                  );
+  if (EFI_ERROR (Status)) {
+    SimpleTextOut = NULL;
+  }  
+
+  if ((GraphicsOutput == NULL) || (SimpleTextOut == NULL)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (IsSetupMode) {
+    //
+    // The requried resolution and text mode is setup mode.
+    //
+    NewHorizontalResolution = mSetupHorizontalResolution;
+    NewVerticalResolution   = mSetupVerticalResolution;
+    NewColumns              = mSetupTextModeColumn;
+    NewRows                 = mSetupTextModeRow;
+  } else {
+    //
+    // The required resolution and text mode is boot mode.
+    //
+    NewHorizontalResolution = mBootHorizontalResolution;
+    NewVerticalResolution   = mBootVerticalResolution;
+    NewColumns              = mBootTextModeColumn;
+    NewRows                 = mBootTextModeRow;   
+  }
+  
+  if (GraphicsOutput != NULL) {
+    MaxGopMode  = GraphicsOutput->Mode->MaxMode;
+  } 
+
+  if (SimpleTextOut != NULL) {
+    MaxTextMode = SimpleTextOut->Mode->MaxMode;
+  }
+
+  //
+  // 1. If current video resolution is same with required video resolution,
+  //    video resolution need not be changed.
+  //    1.1. If current text mode is same with required text mode, text mode need not be changed.
+  //    1.2. If current text mode is different from required text mode, text mode need be changed.
+  // 2. If current video resolution is different from required video resolution, we need restart whole console drivers.
+  //
+  for (ModeNumber = 0; ModeNumber < MaxGopMode; ModeNumber++) {
+    Status = GraphicsOutput->QueryMode (
+                       GraphicsOutput,
+                       ModeNumber,
+                       &SizeOfInfo,
+                       &Info
+                       );
+    if (!EFI_ERROR (Status)) {
+      if ((Info->HorizontalResolution == NewHorizontalResolution) &&
+          (Info->VerticalResolution == NewVerticalResolution)) {
+        if ((GraphicsOutput->Mode->Info->HorizontalResolution == NewHorizontalResolution) &&
+            (GraphicsOutput->Mode->Info->VerticalResolution == NewVerticalResolution)) {
+          //
+          // Current resolution is same with required resolution, check if text mode need be set
+          //
+          Status = SimpleTextOut->QueryMode (SimpleTextOut, SimpleTextOut->Mode->Mode, &CurrentColumn, &CurrentRow);
+          ASSERT_EFI_ERROR (Status);
+          if (CurrentColumn == NewColumns && CurrentRow == NewRows) {
+            //
+            // If current text mode is same with required text mode. Do nothing
+            //
+            FreePool (Info);
+            return EFI_SUCCESS;
+          } else {
+            //
+            // If current text mode is different from requried text mode.  Set new video mode
+            //
+            for (Index = 0; Index < MaxTextMode; Index++) {
+              Status = SimpleTextOut->QueryMode (SimpleTextOut, Index, &CurrentColumn, &CurrentRow);
+              if (!EFI_ERROR(Status)) {
+                if ((CurrentColumn == NewColumns) && (CurrentRow == NewRows)) {
+                  //
+                  // Required text mode is supported, set it.
+                  //
+                  Status = SimpleTextOut->SetMode (SimpleTextOut, Index);
+                  ASSERT_EFI_ERROR (Status);
+                  //
+                  // Update text mode PCD.
+                  //
+                  Status = PcdSet32S (PcdConOutColumn, mSetupTextModeColumn);
+                  ASSERT_EFI_ERROR (Status);
+                  Status = PcdSet32S (PcdConOutRow, mSetupTextModeRow);
+                  ASSERT_EFI_ERROR (Status);
+                  FreePool (Info);
+                  return EFI_SUCCESS;
+                }
+              }
+            }
+            if (Index == MaxTextMode) {
+              //
+              // If requried text mode is not supported, return error.
+              //
+              FreePool (Info);
+              return EFI_UNSUPPORTED;
+            }
+          }
+        } else {
+          //
+          // If current video resolution is not same with the new one, set new video resolution.
+          // In this case, the driver which produces simple text out need be restarted.
+          //
+          Status = GraphicsOutput->SetMode (GraphicsOutput, ModeNumber);
+          if (!EFI_ERROR (Status)) {
+            FreePool (Info);
+            break;
+          }
+        }
+      }
+      FreePool (Info);
+    }
+  }
+
+  if (ModeNumber == MaxGopMode) {
+    //
+    // If the resolution is not supported, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Set PCD to Inform GraphicsConsole to change video resolution.
+  // Set PCD to Inform Consplitter to change text mode.
+  //
+  Status = PcdSet32S (PcdVideoHorizontalResolution, NewHorizontalResolution);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdVideoVerticalResolution, NewVerticalResolution);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdConOutColumn, NewColumns);
+  ASSERT_EFI_ERROR (Status);
+  Status = PcdSet32S (PcdConOutRow, NewRows);
+  ASSERT_EFI_ERROR (Status);
+  
+  
+  //
+  // Video mode is changed, so restart graphics console driver and higher level driver.
+  // Reconnect graphics console driver and higher level driver.
+  // Locate all the handles with GOP protocol and reconnect it.
+  //
+  Status = gBS->LocateHandleBuffer (
+                   ByProtocol,
+                   &gEfiSimpleTextOutProtocolGuid,
+                   NULL,
+                   &HandleCount,
+                   &HandleBuffer
+                   );
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      gBS->DisconnectController (HandleBuffer[Index], NULL, NULL);
+    }
+    for (Index = 0; Index < HandleCount; Index++) {
+      gBS->ConnectController (HandleBuffer[Index], NULL, NULL, TRUE);
+    }
+    if (HandleBuffer != NULL) {
+      FreePool (HandleBuffer);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user Entry Point for Application. The user code starts with this function
+  as the real entry point for the image goes into a library that calls this 
+  function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUserInterface (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_HII_HANDLE                     HiiHandle;
+  EFI_STATUS                         Status;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL       *GraphicsOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *SimpleTextOut;
+  UINTN                              BootTextColumn;
+  UINTN                              BootTextRow;
+
+  if (!mModeInitialized) {
+    //
+    // After the console is ready, get current video resolution 
+    // and text mode before launching setup at first time.
+    //
+    Status = gBS->HandleProtocol (
+                    gST->ConsoleOutHandle,
+                    &gEfiGraphicsOutputProtocolGuid,
+                    (VOID**)&GraphicsOutput
+                    );
+    if (EFI_ERROR (Status)) {
+      GraphicsOutput = NULL;
+    }
+    
+    Status = gBS->HandleProtocol (
+                    gST->ConsoleOutHandle,
+                    &gEfiSimpleTextOutProtocolGuid,
+                    (VOID**)&SimpleTextOut
+                    );
+    if (EFI_ERROR (Status)) {
+      SimpleTextOut = NULL;
+    }  
+
+    if (GraphicsOutput != NULL) {
+      //
+      // Get current video resolution and text mode.
+      //
+      mBootHorizontalResolution = GraphicsOutput->Mode->Info->HorizontalResolution;
+      mBootVerticalResolution   = GraphicsOutput->Mode->Info->VerticalResolution;
+    }
+
+    if (SimpleTextOut != NULL) {
+      Status = SimpleTextOut->QueryMode (
+                                SimpleTextOut,
+                                SimpleTextOut->Mode->Mode,
+                                &BootTextColumn,
+                                &BootTextRow
+                                );
+      mBootTextModeColumn = (UINT32)BootTextColumn;
+      mBootTextModeRow    = (UINT32)BootTextRow;
+    }
+
+    //
+    // Get user defined text mode for setup.
+    //  
+    mSetupHorizontalResolution = PcdGet32 (PcdSetupVideoHorizontalResolution);
+    mSetupVerticalResolution   = PcdGet32 (PcdSetupVideoVerticalResolution);      
+    mSetupTextModeColumn       = PcdGet32 (PcdSetupConOutColumn);
+    mSetupTextModeRow          = PcdGet32 (PcdSetupConOutRow);
+
+    mModeInitialized           = TRUE;
+  }
+
+  gBS->SetWatchdogTimer (0x0000, 0x0000, 0x0000, NULL);
+  gST->ConOut->ClearScreen (gST->ConOut);
+  
+  //
+  // Install customized fonts needed by Front Page
+  //
+  
+  HiiHandle = ExportFonts ();
+  ASSERT (HiiHandle != NULL);
+
+  InitializeStringSupport ();
+
+  BdsSetConsoleMode (TRUE);
+  UiEntry (FALSE);
+  BdsSetConsoleMode (FALSE);
+
+  UninitializeStringSupport ();
+  HiiRemovePackages (HiiHandle);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is the main entry of the UI entry.
+  The function will present the main menu of the system UI.
+
+  @param ConnectAllHappened Caller passes the value to UI to avoid unnecessary connect-all.
+
+**/
+VOID
+EFIAPI
+UiEntry (
+  IN BOOLEAN                      ConnectAllHappened
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_BOOT_LOGO_PROTOCOL        *BootLogo;
+
+  //
+  // Indicate if the connect all has been performed before.
+  //
+  if (ConnectAllHappened) {
+    gConnectAllHappened = TRUE;
+  }
+
+  //
+  // The boot option enumeration time is acceptable in Ui driver
+  //
+  EfiBootManagerRefreshAllBootOption ();
+
+  //
+  // Boot Logo is corrupted, report it using Boot Logo protocol.
+  //
+  Status = gBS->LocateProtocol (&gEfiBootLogoProtocolGuid, NULL, (VOID **) &BootLogo);
+  if (!EFI_ERROR (Status) && (BootLogo != NULL)) {
+    BootLogo->SetBootLogo (BootLogo, NULL, 0, 0, 0, 0);
+  }
+
+  InitializeFrontPage ();
+  InitializeDeviceManager ();
+  InitializeBootManager ();
+  InitBootMaintenance();
+
+  CallFrontPage ();
+
+  FreeBMPackage ();
+  FreeBootManager ();
+  FreeDeviceManager ();
+  FreeFrontPage ();
+
+  if (mLanguageString != NULL) {
+    FreePool (mLanguageString);
+    mLanguageString = NULL;
+  }
+
+  //
+  //Will leave browser, check any reset required change is applied? if yes, reset system
+  //
+  SetupResetReminder ();
+}
+
+/**
+  Extract device path for given HII handle and class guid.
+
+  @param Handle          The HII handle.
+
+  @retval  NULL          Fail to get the device path string.
+  @return  PathString    Get the device path string.
+
+**/
+CHAR16 *
+ExtractDevicePathFromHiiHandle (
+  IN      EFI_HII_HANDLE      Handle
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_HANDLE                       DriverHandle;
+  EFI_DEVICE_PATH_PROTOCOL         *DevicePath;  
+  EFI_DEVICE_PATH_TO_TEXT_PROTOCOL *PathToText;
+  CHAR16                           *NewString;
+
+  ASSERT (Handle != NULL);
+
+  if (Handle == NULL) {
+    return NULL;
+  }
+
+  Status = gHiiDatabase->GetPackageListHandle (gHiiDatabase, Handle, &DriverHandle);
+ if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Get the device path by the got Driver handle .
+  //
+  Status = gBS->HandleProtocol (DriverHandle, &gEfiDevicePathProtocolGuid, (VOID **) &DevicePath);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+ }
+
+  Status = gBS->LocateProtocol (
+                  &gEfiDevicePathToTextProtocolGuid,
+                  NULL,
+                  (VOID **) &PathToText
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Get device path string.
+  //
+  NewString = PathToText->ConvertDevicePathToText(DevicePath, FALSE, FALSE);
+
+  return NewString;
+}
+
+/**
+  Extract the displayed formset for given HII handle and class guid.
+
+  @param Handle          The HII handle.
+  @param SetupClassGuid  The class guid specifies which form set will be displayed.
+  @param SkipCount       Skip some formsets which has processed before.
+  @param FormSetTitle    Formset title string.
+  @param FormSetHelp     Formset help string.
+  @param FormSetGuid     Formset Guid.
+
+  @retval  TRUE          The formset for given HII handle will be displayed.
+  @return  FALSE         The formset for given HII handle will not be displayed.
+
+**/
+BOOLEAN
+ExtractDisplayedHiiFormFromHiiHandle (
+  IN      EFI_HII_HANDLE      Handle,
+  IN      EFI_GUID            *SetupClassGuid,
+  IN      UINTN               SkipCount,
+  OUT     EFI_STRING_ID       *FormSetTitle,
+  OUT     EFI_STRING_ID       *FormSetHelp,
+  OUT     EFI_GUID            *FormSetGuid
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        BufferSize;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINT8                        *Package;
+  UINT8                        *OpCodeData;
+  UINT32                       Offset;
+  UINT32                       Offset2;
+  UINT32                       PackageListLength;
+  EFI_HII_PACKAGE_HEADER       PackageHeader;
+  EFI_GUID                     *ClassGuid;
+  UINT8                        ClassGuidNum;
+  BOOLEAN                      FoundAndSkip;
+
+  ASSERT (Handle != NULL);
+  ASSERT (SetupClassGuid != NULL && FormSetTitle != NULL && FormSetHelp != NULL && FormSetGuid != NULL);
+
+  *FormSetTitle = 0;
+  *FormSetHelp  = 0;
+  ClassGuidNum  = 0;
+  ClassGuid     = NULL;
+  FoundAndSkip  = FALSE;
+
+  //
+  // Get HII PackageList
+  //
+  BufferSize = 0;
+  HiiPackageList = NULL;
+  Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  //
+  // Handle is a invalid handle. Check if Handle is corrupted.
+  //
+  ASSERT (Status != EFI_NOT_FOUND);
+  //
+  // The return status should always be EFI_BUFFER_TOO_SMALL as input buffer's size is 0.
+  //
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+  
+  HiiPackageList = AllocatePool (BufferSize);
+  ASSERT (HiiPackageList != NULL);
+
+  Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  //
+  // Get Form package from this HII package List
+  //
+  Offset = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  PackageListLength = ReadUnaligned32 (&HiiPackageList->PackageLength);
+
+  while (Offset < PackageListLength) {
+    Package = ((UINT8 *) HiiPackageList) + Offset;
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+    Offset += PackageHeader.Length;
+
+    if (PackageHeader.Type == EFI_HII_PACKAGE_FORMS) {
+      //
+      // Search FormSet Opcode in this Form Package
+      //
+      Offset2 = sizeof (EFI_HII_PACKAGE_HEADER);
+      while (Offset2 < PackageHeader.Length) {
+        OpCodeData = Package + Offset2;
+        Offset2 += ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+
+        if (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode == EFI_IFR_FORM_SET_OP) {
+          if (((EFI_IFR_OP_HEADER *) OpCodeData)->Length > OFFSET_OF (EFI_IFR_FORM_SET, Flags)) {
+            //
+            // Find FormSet OpCode
+            //
+            ClassGuidNum = (UINT8) (((EFI_IFR_FORM_SET *) OpCodeData)->Flags & 0x3);
+            ClassGuid = (EFI_GUID *) (VOID *)(OpCodeData + sizeof (EFI_IFR_FORM_SET));
+            while (ClassGuidNum-- > 0) {
+              if (CompareGuid (SetupClassGuid, ClassGuid)) {
+                //
+                // Check whether need to skip the formset.
+                //
+                if (SkipCount != 0) {
+                  SkipCount--;
+                  FoundAndSkip = TRUE;
+                  break;
+                }
+                CopyMem (FormSetTitle, &((EFI_IFR_FORM_SET *) OpCodeData)->FormSetTitle, sizeof (EFI_STRING_ID));
+                CopyMem (FormSetHelp, &((EFI_IFR_FORM_SET *) OpCodeData)->Help, sizeof (EFI_STRING_ID));
+                CopyGuid (FormSetGuid, (CONST EFI_GUID *)(&((EFI_IFR_FORM_SET *) OpCodeData)->Guid));
+                FreePool (HiiPackageList);
+                return TRUE;
+              }
+              ClassGuid ++;
+            }
+            if (FoundAndSkip) {
+              break;
+            }
+           } else if (CompareGuid (SetupClassGuid, &gEfiHiiPlatformSetupFormsetGuid)) {
+             //
+             //  Check whether need to skip the formset.
+             //
+             if (SkipCount != 0) {
+               SkipCount--;
+               break;
+             }
+             CopyMem (FormSetTitle, &((EFI_IFR_FORM_SET *) OpCodeData)->FormSetTitle, sizeof (EFI_STRING_ID));
+             CopyMem (FormSetHelp, &((EFI_IFR_FORM_SET *) OpCodeData)->Help, sizeof (EFI_STRING_ID));
+             CopyGuid (FormSetGuid, (CONST EFI_GUID *)(&((EFI_IFR_FORM_SET *) OpCodeData)->Guid));
+             FreePool (HiiPackageList);
+             return TRUE;
+          }
+        }
+      }
+    }
+  }
+
+  FreePool (HiiPackageList);
+
+  return FALSE;
+}
+
+//
+//  Following are BDS Lib functions which contain all the code about setup browser reset reminder feature.
+//  Setup Browser reset reminder feature is that an reset reminder will be given before user leaves the setup browser  if
+//  user change any option setting which needs a reset to be effective, and  the reset will be applied according to  the user selection.
+//
+
+
+/**
+  Enable the setup browser reset reminder feature.
+  This routine is used in platform tip. If the platform policy need the feature, use the routine to enable it.
+
+**/
+VOID
+EFIAPI
+EnableResetReminderFeature (
+  VOID
+  )
+{
+  mFeaturerSwitch = TRUE;
+}
+
+
+/**
+  Disable the setup browser reset reminder feature.
+  This routine is used in platform tip. If the platform policy do not want the feature, use the routine to disable it.
+
+**/
+VOID
+EFIAPI
+DisableResetReminderFeature (
+  VOID
+  )
+{
+  mFeaturerSwitch = FALSE;
+}
+
+
+/**
+  Record the info that  a reset is required.
+  A  module boolean variable is used to record whether a reset is required.
+
+**/
+VOID
+EFIAPI
+EnableResetRequired (
+  VOID
+  )
+{
+  mResetRequired = TRUE;
+}
+
+
+/**
+  Record the info that  no reset is required.
+  A  module boolean variable is used to record whether a reset is required.
+
+**/
+VOID
+EFIAPI
+DisableResetRequired (
+  VOID
+  )
+{
+  mResetRequired = FALSE;
+}
+
+
+/**
+  Check whether platform policy enable the reset reminder feature. The default is enabled.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetReminderFeatureEnable (
+  VOID
+  )
+{
+  return mFeaturerSwitch;
+}
+
+
+/**
+  Check if  user changed any option setting which needs a system reset to be effective.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetRequired (
+  VOID
+  )
+{
+  return mResetRequired;
+}
+
+
+/**
+  Check whether a reset is needed, and finish the reset reminder feature.
+  If a reset is needed, Popup a menu to notice user, and finish the feature
+  according to the user selection.
+
+**/
+VOID
+EFIAPI
+SetupResetReminder (
+  VOID
+  )
+{
+  EFI_INPUT_KEY                 Key;
+  CHAR16                        *StringBuffer1;
+  CHAR16                        *StringBuffer2;
+
+
+  //
+  //check any reset required change is applied? if yes, reset system
+  //
+  if (IsResetReminderFeatureEnable ()) {
+    if (IsResetRequired ()) {
+
+      StringBuffer1 = AllocateZeroPool (MAX_STRING_LEN * sizeof (CHAR16));
+      ASSERT (StringBuffer1 != NULL);
+      StringBuffer2 = AllocateZeroPool (MAX_STRING_LEN * sizeof (CHAR16));
+      ASSERT (StringBuffer2 != NULL);
+      StrCpyS (StringBuffer1, MAX_STRING_LEN, L"Configuration changed. Reset to apply it Now.");
+      StrCpyS (StringBuffer2, MAX_STRING_LEN, L"Press ENTER to reset");
+      //
+      // Popup a menu to notice user
+      //
+      do {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, StringBuffer1, StringBuffer2, NULL);
+      } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+      FreePool (StringBuffer1);
+      FreePool (StringBuffer2);
+
+      gRT->ResetSystem (EfiResetCold, EFI_SUCCESS, 0, NULL);
+    }
+  }
+}
+
+
+/**
+  This function converts an input device structure to a Unicode string.
+
+  @param DevPath                  A pointer to the device path structure.
+
+  @return A new allocated Unicode string that represents the device path.
+
+**/
+CHAR16 *
+UiDevicePathToStr (
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevPath
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           *ToText;
+  EFI_DEVICE_PATH_TO_TEXT_PROTOCOL *DevPathToText;
+
+  if (DevPath == NULL) {
+    return NULL;
+  }
+
+  Status = gBS->LocateProtocol (
+                  &gEfiDevicePathToTextProtocolGuid,
+                  NULL,
+                  (VOID **) &DevPathToText
+                  );
+  ASSERT_EFI_ERROR (Status);
+  ToText = DevPathToText->ConvertDevicePathToText (
+                            DevPath,
+                            FALSE,
+                            TRUE
+                            );
+  ASSERT (ToText != NULL);
+  return ToText;
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/FrontPage.h b/Core/MdeModulePkg/Application/UiApp/FrontPage.h
new file mode 100644
index 0000000000..52545d63c4
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/FrontPage.h
@@ -0,0 +1,277 @@
+/** @file
+  FrontPage routines to handle the callbacks and browser calls
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _FRONT_PAGE_H_
+#define _FRONT_PAGE_H_
+
+#include "DeviceMngr/DeviceManager.h"
+#include "BootMaint/BootMaint.h"
+#include "BootMngr/BootManager.h"
+#include "String.h"
+
+#include <Protocol/BootLogo.h>
+//
+// These are the VFR compiler generated data representing our VFR data.
+//
+extern UINT8  FrontPageVfrBin[];
+
+extern EFI_FORM_BROWSER2_PROTOCOL      *gFormBrowser2;
+
+extern BOOLEAN  gConnectAllHappened;
+
+#define SMBIOS_TYPE4_CPU_SOCKET_POPULATED BIT6
+
+///
+/// The size of a 3 character ISO639 language code.
+///
+#define ISO_639_2_ENTRY_SIZE   3
+
+//
+// This is the VFR compiler generated header file which defines the
+// string identifiers.
+//
+#define PRINTABLE_LANGUAGE_NAME_STRING_ID     0x0001
+
+//
+// These are defined as the same with vfr file
+//
+#define FRONT_PAGE_FORM_ID             0x1000
+#define INVALID_FORM_ID                0x0FFF
+
+#define FRONT_PAGE_KEY_SECTION         0x1000
+#define FRONT_PAGE_KEY_OFFSET          0x4000
+#define FRONT_PAGE_KEY_CONTINUE        0x1000
+#define FRONT_PAGE_KEY_LANGUAGE        0x1234
+#define FRONT_PAGE_KEY_BOOT_MANAGER    0x1064
+#define FRONT_PAGE_KEY_DEVICE_MANAGER  0x8567
+#define FRONT_PAGE_KEY_BOOT_MAINTAIN   0x9876
+
+#define LABEL_SELECT_LANGUAGE          0x1000
+#define LABEL_PLATFORM_INFORMATION     0x1001
+#define LABEL_END                      0xffff
+
+#define FRONT_PAGE_FORMSET_GUID \
+  { \
+    0x9e0c30bc, 0x3f06, 0x4ba6, {0x82, 0x88, 0x9, 0x17, 0x9b, 0x85, 0x5d, 0xbe} \
+  }
+
+#define FRONT_PAGE_CALLBACK_DATA_SIGNATURE  SIGNATURE_32 ('F', 'P', 'C', 'B')
+
+typedef struct {
+  UINTN                           Signature;
+
+  //
+  // HII relative handles
+  //
+  EFI_HII_HANDLE                  HiiHandle;
+  EFI_HANDLE                      DriverHandle;
+  EFI_STRING_ID                   *LanguageToken;
+
+  //
+  // Produced protocols
+  //
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  ConfigAccess;
+} FRONT_PAGE_CALLBACK_DATA;
+
+
+#define EFI_FP_CALLBACK_DATA_FROM_THIS(a) \
+  CR (a, \
+      FRONT_PAGE_CALLBACK_DATA, \
+      ConfigAccess, \
+      FRONT_PAGE_CALLBACK_DATA_SIGNATURE \
+      )
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+
+  @param This            - Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Request         - A null-terminated Unicode string in <ConfigRequest> format.
+  @param Progress        - On return, points to a character in the Request string.
+                         Points to the string's null terminator if request was successful.
+                         Points to the most recent '&' before the first failing name/value
+                         pair (or the beginning of the string if the failure is in the
+                         first name/value pair) if the request was not successful.
+  @param Results         - A null-terminated Unicode string in <ConfigAltResp> format which
+                         has all values filled in for the names in the Request string.
+                         String to be allocated by the called function.
+
+  @retval  EFI_SUCCESS            The Results is filled with the requested values.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval  EFI_INVALID_PARAMETER  Request is NULL, illegal syntax, or unknown name.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FakeExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  );
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            - Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Configuration   - A null-terminated Unicode string in <ConfigResp> format.
+  @param Progress        - A pointer to a string filled in with the offset of the most
+                         recent '&' before the first failing name/value pair (or the
+                         beginning of the string if the failure is in the first
+                         name/value pair) or the terminating NULL if all was successful.
+
+  @retval  EFI_SUCCESS            The Results is processed successfully.
+  @retval  EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FakeRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  );
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            - Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          - Specifies the type of action taken by the browser.
+  @param QuestionId      - A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            - The type of value for the question.
+  @param Value           - A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   - On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the variable and its data.
+  @retval  EFI_DEVICE_ERROR      The variable could not be saved.
+  @retval  EFI_UNSUPPORTED       The specified Action is not supported by the callback.
+
+**/
+EFI_STATUS
+EFIAPI
+FrontPageCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  );
+
+/**
+  Initialize HII information for the FrontPage
+
+  @retval  EFI_SUCCESS        The operation is successful.
+  @retval  EFI_DEVICE_ERROR   If the dynamic opcode creation failed.
+
+**/
+EFI_STATUS
+InitializeFrontPage (
+  VOID
+  );
+
+/**
+  Acquire the string associated with the ProducerGuid and return it.
+
+
+  @param ProducerGuid    - The Guid to search the HII database for
+  @param Token           - The token value of the string to extract
+  @param String          - The string that is extracted
+
+  @retval  EFI_SUCCESS  The function returns EFI_SUCCESS always.
+
+**/
+EFI_STATUS
+GetProducerString (
+  IN      EFI_GUID                  *ProducerGuid,
+  IN      EFI_STRING_ID             Token,
+  OUT     CHAR16                    **String
+  );
+
+/**
+  This function is the main entry of the UI entry.
+  The function will present the main menu of the system UI.
+
+  @param ConnectAllHappened Caller passes the value to UI to avoid unnecessary connect-all.
+
+**/
+VOID
+EFIAPI
+UiEntry (
+  IN BOOLEAN                      ConnectAllHappened
+  );
+
+/**
+  Extract the displayed formset for given HII handle and class guid.
+
+  @param Handle          The HII handle.
+  @param SetupClassGuid  The class guid specifies which form set will be displayed.
+  @param SkipCount       Skip some formsets which has processed before.
+  @param FormSetTitle    Formset title string.
+  @param FormSetHelp     Formset help string.
+  @param FormSetGuid     Formset Guid.
+
+  @retval  TRUE          The formset for given HII handle will be displayed.
+  @return  FALSE         The formset for given HII handle will not be displayed.
+
+**/
+BOOLEAN
+ExtractDisplayedHiiFormFromHiiHandle (
+  IN      EFI_HII_HANDLE      Handle,
+  IN      EFI_GUID            *SetupClassGuid,
+  IN      UINTN               SkipCount,
+  OUT     EFI_STRING_ID       *FormSetTitle,
+  OUT     EFI_STRING_ID       *FormSetHelp,
+  OUT     EFI_GUID            *FormSetGuid
+  );
+
+/**
+  Extract device path for given HII handle and class guid.
+
+  @param Handle          The HII handle.
+
+  @retval  NULL          Fail to get the device path string.
+  @return  PathString    Get the device path string.
+
+**/
+CHAR16 *
+ExtractDevicePathFromHiiHandle (
+  IN      EFI_HII_HANDLE      Handle
+  );
+
+/**
+  This function will change video resolution and text mode
+  according to defined setup mode or defined boot mode  
+
+  @param  IsSetupMode   Indicate mode is changed to setup mode or boot mode. 
+
+  @retval  EFI_SUCCESS  Mode is changed successfully.
+  @retval  Others             Mode failed to be changed.
+
+**/
+EFI_STATUS
+EFIAPI
+BdsSetConsoleMode (
+  BOOLEAN  IsSetupMode
+  );
+
+#endif // _FRONT_PAGE_H_
+
diff --git a/Core/MdeModulePkg/Application/UiApp/FrontPageStrings.uni b/Core/MdeModulePkg/Application/UiApp/FrontPageStrings.uni
new file mode 100644
index 0000000000..37df7afc8e
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/FrontPageStrings.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/FrontPageVfr.Vfr b/Core/MdeModulePkg/Application/UiApp/FrontPageVfr.Vfr
new file mode 100644
index 0000000000..82d241024b
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/FrontPageVfr.Vfr
@@ -0,0 +1,149 @@
+///** @file
+//  
+//    Browser formset.
+//  
+//  Copyright (c) 2007 - 2015, 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.
+//  
+//**/
+
+#define FORMSET_GUID  { 0x9e0c30bc, 0x3f06, 0x4ba6, 0x82, 0x88, 0x9, 0x17, 0x9b, 0x85, 0x5d, 0xbe }
+#include "FormsetGuid.h"
+
+#define FRONT_PAGE_CLASS               0x0000
+#define FRONT_PAGE_SUBCLASS            0x0002
+
+#define FRONT_PAGE_FORM_ID             0x1000
+
+#define FRONT_PAGE_ITEM_ONE            0x0001
+#define FRONT_PAGE_ITEM_TWO            0x0002
+#define FRONT_PAGE_ITEM_THREE          0x0003
+#define FRONT_PAGE_ITEM_FOUR           0x0004
+#define FRONT_PAGE_ITEM_FIVE           0x0005
+
+#define FRONT_PAGE_KEY_CONTINUE        0x1000
+#define FRONT_PAGE_KEY_LANGUAGE        0x1234
+#define FRONT_PAGE_KEY_BOOT_MANAGER    0x1064
+#define FRONT_PAGE_KEY_DEVICE_MANAGER  0x8567
+#define FRONT_PAGE_KEY_BOOT_MAINTAIN   0x9876
+
+#define LABEL_SELECT_LANGUAGE          0x1000
+#define LABEL_TIMEOUT                  0x2000
+#define LABEL_END                      0xffff
+
+formset
+  guid     = FORMSET_GUID,
+  title    = STRING_TOKEN(STR_FRONT_PAGE_TITLE),
+  help     = STRING_TOKEN(STR_NULL_STRING),
+  classguid = FORMSET_GUID,
+
+  form formid = FRONT_PAGE_FORM_ID,
+       title  = STRING_TOKEN(STR_FRONT_PAGE_TITLE);
+
+    banner
+      title = STRING_TOKEN(STR_FRONT_PAGE_COMPUTER_MODEL),
+      line  1,
+      align left;
+
+    banner
+      title = STRING_TOKEN(STR_FRONT_PAGE_CPU_MODEL),
+      line  2,
+      align left;
+
+    banner
+      title = STRING_TOKEN(STR_FRONT_PAGE_CPU_SPEED),
+      line  2,
+      align right;
+
+    banner
+      title = STRING_TOKEN(STR_FRONT_PAGE_BIOS_VERSION),
+      line  3,
+      align left;
+
+    banner
+      title = STRING_TOKEN(STR_FRONT_PAGE_MEMORY_SIZE),
+      line  3,
+      align right;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_0_LEFT),
+//      line  0,
+//      align left;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_0_RIGHT),
+//      line  0,
+//      align right;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_1_LEFT),
+//      line  1,
+//      align left;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_1_RIGHT),
+//      line  1,
+//      align right;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_2_LEFT),
+//      line  2,
+//      align left;
+
+//    banner
+//      title = STRING_TOKEN(STR_FRONT_PAGE_BANNER_3_LEFT),
+//      line  3,
+//      align left;
+
+
+    text
+      help    = STRING_TOKEN(STR_CONTINUE_HELP),
+      text    = STRING_TOKEN(STR_CONTINUE_PROMPT),
+      flags   = INTERACTIVE,
+      key     = FRONT_PAGE_KEY_CONTINUE;
+
+    label LABEL_SELECT_LANGUAGE;
+    //
+    // This is where we will dynamically add a OneOf type op-code to select
+    // Languages from the currently available choices
+    //
+    label LABEL_END;
+          
+    goto 
+      formsetguid = DEVICE_MANAGER_FORMSET_GUID,
+      formid      = 0,
+      question    = 0,
+      prompt  = STRING_TOKEN(STR_EDKII_MENU),
+      help    = STRING_TOKEN(STR_EDKII_MENU_HELP),
+      flags   = INTERACTIVE,
+      key     = FRONT_PAGE_KEY_DEVICE_MANAGER;
+
+               
+    goto 
+      formsetguid = BOOT_MANAGER_FORMSET_GUID  ,
+      formid      = 0,
+      question    = 0,
+      prompt      = STRING_TOKEN(STR_BOOT_MANAGER),
+      help        = STRING_TOKEN(STR_BOOT_MANAGER_HELP),
+      flags       = INTERACTIVE,
+      key         = FRONT_PAGE_KEY_BOOT_MANAGER;
+
+
+    goto
+      formsetguid = BOOT_MAINT_FORMSET_GUID,
+      formid      = 0,
+      question    = 0,
+      prompt  = STRING_TOKEN(STR_BOOT_MAINT_MANAGER),
+      help    = STRING_TOKEN(STR_BOOT_MAINT_MANAGER_HELP),
+      flags   = INTERACTIVE,
+      key     = FRONT_PAGE_KEY_BOOT_MAINTAIN;
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Application/UiApp/Language.c b/Core/MdeModulePkg/Application/UiApp/Language.c
new file mode 100644
index 0000000000..df7a0b3561
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/Language.c
@@ -0,0 +1,309 @@
+/** @file
+  Language settings
+
+Copyright (c) 2004 - 2015, 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 "Language.h"
+#include "FrontPage.h"
+
+EFI_GUID  mFontPackageGuid = {
+  0x78941450, 0x90ab, 0x4fb1, {0xb7, 0x5f, 0x58, 0x92, 0x14, 0xe2, 0x4a, 0xc}
+};
+
+#define NARROW_GLYPH_NUMBER 8
+#define WIDE_GLYPH_NUMBER   75
+
+typedef struct {
+  ///
+  /// This 4-bytes total array length is required by HiiAddPackages()
+  ///
+  UINT32                 Length;
+
+  //
+  // This is the Font package definition
+  //
+  EFI_HII_PACKAGE_HEADER Header;
+  UINT16                 NumberOfNarrowGlyphs;
+  UINT16                 NumberOfWideGlyphs;
+  EFI_NARROW_GLYPH       NarrowArray[NARROW_GLYPH_NUMBER];
+  EFI_WIDE_GLYPH         WideArray[WIDE_GLYPH_NUMBER];
+} FONT_PACK_BIN;
+
+FONT_PACK_BIN mFontBin = {
+  sizeof (FONT_PACK_BIN),
+  {
+    sizeof (FONT_PACK_BIN) - sizeof (UINT32),
+    EFI_HII_PACKAGE_SIMPLE_FONTS,
+  },
+  NARROW_GLYPH_NUMBER,
+  0,
+  {     // Narrow Glyphs
+    {
+      0x05d0,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x4E,
+        0x6E,
+        0x62,
+        0x32,
+        0x32,
+        0x3C,
+        0x68,
+        0x4C,
+        0x4C,
+        0x46,
+        0x76,
+        0x72,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d1,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x78,
+        0x7C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x7E,
+        0x7E,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d2,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x78,
+        0x7C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x1C,
+        0x3E,
+        0x66,
+        0x66,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d3,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x7E,
+        0x7E,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d4,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x7C,
+        0x7E,
+        0x06,
+        0x06,
+        0x06,
+        0x06,
+        0x66,
+        0x66,
+        0x66,
+        0x66,
+        0x66,
+        0x66,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d5,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x3C,
+        0x3C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x0C,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x05d6,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x38,
+        0x38,
+        0x1E,
+        0x1E,
+        0x18,
+        0x18,
+        0x18,
+        0x18,
+        0x18,
+        0x18,
+        0x18,
+        0x18,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    },
+    {
+      0x0000,
+      0x00,
+      {
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00,
+        0x00
+      }
+    }
+  }
+};
+
+/**
+  Routine to export glyphs to the HII database.  This is in addition to whatever is defined in the Graphics Console driver.
+
+**/
+EFI_HII_HANDLE
+ExportFonts (
+  VOID
+  )
+{
+  return HiiAddPackages (
+           &mFontPackageGuid,
+           gImageHandle,
+           &mFontBin,
+           NULL
+           );
+}
+
+/**
+  Get next language from language code list (with separator ';').
+
+  If LangCode is NULL, then ASSERT.
+  If Lang is NULL, then ASSERT.
+
+  @param  LangCode    On input: point to first language in the list. On
+                                 output: point to next language in the list, or
+                                 NULL if no more language in the list.
+  @param  Lang           The first language in the list.
+
+**/
+VOID
+EFIAPI
+GetNextLanguage (
+  IN OUT CHAR8      **LangCode,
+  OUT CHAR8         *Lang
+  )
+{
+  UINTN  Index;
+  CHAR8  *StringPtr;
+
+  ASSERT (LangCode != NULL);
+  ASSERT (*LangCode != NULL);
+  ASSERT (Lang != NULL);
+
+  Index = 0;
+  StringPtr = *LangCode;
+  while (StringPtr[Index] != 0 && StringPtr[Index] != ';') {
+    Index++;
+  }
+
+  CopyMem (Lang, StringPtr, Index);
+  Lang[Index] = 0;
+
+  if (StringPtr[Index] == ';') {
+    Index++;
+  }
+  *LangCode = StringPtr + Index;
+}
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Application/UiApp/Language.h b/Core/MdeModulePkg/Application/UiApp/Language.h
new file mode 100644
index 0000000000..274de5ce2f
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/Language.h
@@ -0,0 +1,48 @@
+/** @file
+  Language setting
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _LANGUAGE_H_
+#define _LANGUAGE_H_
+
+#include "String.h"
+
+/**
+  Get next language from language code list (with separator ';').
+
+  If LangCode is NULL, then ASSERT.
+  If Lang is NULL, then ASSERT.
+
+  @param  LangCode    On input: point to first language in the list. On
+                                 output: point to next language in the list, or
+                                 NULL if no more language in the list.
+  @param  Lang           The first language in the list.
+
+**/
+VOID
+EFIAPI
+GetNextLanguage (
+  IN OUT CHAR8      **LangCode,
+  OUT CHAR8         *Lang
+  );
+
+/**
+  Routine to export glyphs to the HII database.  This is in addition to whatever is defined in the Graphics Console driver.
+
+**/
+EFI_HII_HANDLE
+ExportFonts (
+  VOID
+  );
+
+#endif // _LANGUAGE_H_
diff --git a/Core/MdeModulePkg/Application/UiApp/String.c b/Core/MdeModulePkg/Application/UiApp/String.c
new file mode 100644
index 0000000000..7884ae3fce
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/String.c
@@ -0,0 +1,71 @@
+/** @file
+  String support
+
+Copyright (c) 2004 - 2015, 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 "Ui.h"
+#include "Language.h"
+#include "FrontPage.h"
+
+EFI_HII_HANDLE gStringPackHandle;
+
+EFI_GUID mUiStringPackGuid = {
+  0x136a3048, 0x752a, 0x4bf6, { 0xa7, 0x57, 0x9, 0x36, 0x11, 0x95, 0x38, 0xed }
+};
+
+/**
+  Initialize HII global accessor for string support.
+
+**/
+VOID
+InitializeStringSupport (
+  VOID
+  )
+{
+  gStringPackHandle = HiiAddPackages (
+                         &mUiStringPackGuid,
+                         gImageHandle,
+                         UiAppStrings,
+                         NULL
+                         );
+  ASSERT (gStringPackHandle != NULL);
+}
+
+/**
+  Remove the string package.
+
+**/
+VOID
+UninitializeStringSupport (
+  VOID
+  )
+{
+  HiiRemovePackages (gStringPackHandle);
+}
+
+/**
+  Get string by string id from HII Interface
+
+
+  @param Id              String ID.
+
+  @retval  CHAR16 *  String from ID.
+  @retval  NULL      If error occurs.
+
+**/
+CHAR16 *
+GetStringById (
+  IN  EFI_STRING_ID   Id
+  )
+{
+  return HiiGetString (gStringPackHandle, Id, NULL);
+}
diff --git a/Core/MdeModulePkg/Application/UiApp/String.h b/Core/MdeModulePkg/Application/UiApp/String.h
new file mode 100644
index 0000000000..b8cb8762ec
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/String.h
@@ -0,0 +1,80 @@
+/** @file
+  String support
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _STRING_H_
+#define _STRING_H_
+
+extern EFI_HII_HANDLE gStringPackHandle;
+
+//
+// This is the VFR compiler generated header file which defines the
+// string identifiers.
+//
+
+extern UINT8  BdsDxeStrings[];
+
+//
+// String Definition Guid for BDS Platform
+//
+#define EFI_BDS_PLATFORM_GUID \
+  { \
+    0x7777E939, 0xD57E, 0x4DCB, 0xA0, 0x8E, 0x64, 0xD7, 0x98, 0x57, 0x1E, 0x0F \
+  }
+
+/**
+  Get string by string id from HII Interface
+
+
+  @param Id              String ID.
+
+  @retval  CHAR16 *  String from ID.
+  @retval  NULL      If error occurs.
+
+**/
+CHAR16 *
+GetStringById (
+  IN  EFI_STRING_ID   Id
+  );
+
+/**
+  Initialize HII global accessor for string support.
+
+**/
+VOID
+InitializeStringSupport (
+  VOID
+  );
+
+/**
+  Remove the string package.
+
+**/
+VOID
+UninitializeStringSupport (
+  VOID
+  );
+
+/**
+  Call the browser and display the front page
+
+  @return   Status code that will be returned by
+            EFI_FORM_BROWSER2_PROTOCOL.SendForm ().
+
+**/
+EFI_STATUS
+CallFrontPage (
+  VOID
+  );
+
+#endif // _STRING_H_
diff --git a/Core/MdeModulePkg/Application/UiApp/Strings.uni b/Core/MdeModulePkg/Application/UiApp/Strings.uni
new file mode 100644
index 0000000000..c1994d7b59
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/Strings.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/Ui.h b/Core/MdeModulePkg/Application/UiApp/Ui.h
new file mode 100644
index 0000000000..8159878007
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/Ui.h
@@ -0,0 +1,178 @@
+/** @file
+  FrontPage routines to handle the callbacks and browser calls
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+
+#ifndef _BDS_MODULE_H_
+#define _BDS_MODULE_H_
+
+#include <PiDxe.h>
+#include <IndustryStandard/PeImage.h>
+#include <IndustryStandard/SmBios.h>
+
+#include <Guid/MdeModuleHii.h>
+#include <Guid/FileSystemVolumeLabelInfo.h>
+#include <Guid/HiiPlatformSetupFormset.h>
+#include <Guid/GlobalVariable.h>
+#include <Guid/CapsuleVendor.h>
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/FileInfo.h>
+#include <Guid/HiiBootMaintenanceFormset.h>
+
+#include <Protocol/LoadFile.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/Smbios.h>
+#include <Protocol/UgaDraw.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/GenericMemoryTest.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/SimpleFileSystem.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/SerialIo.h>
+#include <Protocol/SimpleTextInEx.h>
+#include <Protocol/DriverHealth.h>
+#include <Protocol/DevicePathToText.h>
+
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/PrintLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/HobLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+#include <Library/CapsuleLib.h>
+#include <Library/HiiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiHiiServicesLib.h>
+
+#include <Library/UefiBootManagerLib.h>
+
+#pragma pack(1)
+
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} HII_VENDOR_DEVICE_PATH;
+
+#pragma pack()
+
+
+//
+//The interface functions related to the Setup Browser Reset Reminder feature
+//
+/**
+  Enable the setup browser reset reminder feature.
+  This routine is used in a platform tip. If the platform policy needs the feature, use the routine to enable it.
+
+**/
+VOID
+EFIAPI
+EnableResetReminderFeature (
+  VOID
+  );
+
+/**
+  Disable the setup browser reset reminder feature.
+  This routine is used in a platform tip. If the platform policy does not want the feature, use the routine to disable it.
+
+**/
+VOID
+EFIAPI
+DisableResetReminderFeature (
+  VOID
+  );
+
+/**
+  Record the info that a reset is required.
+  A module boolean variable is used to record whether a reset is required.
+
+**/
+VOID
+EFIAPI
+EnableResetRequired (
+  VOID
+  );
+
+
+/**
+  Record the info that no reset is required.
+  A module boolean variable is used to record whether a reset is required.
+
+**/
+VOID
+EFIAPI
+DisableResetRequired (
+  VOID
+  );
+
+/**
+  Check whether platform policy enables the reset reminder feature. The default is enabled.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetReminderFeatureEnable (
+  VOID
+  );
+
+/**
+  Check if the user changed any option setting that needs a system reset to be effective.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetRequired (
+  VOID
+  );
+
+/**
+  Check whether a reset is needed, and finish the reset reminder feature.
+  If a reset is needed, pop up a menu to notice user, and finish the feature
+  according to the user selection.
+
+**/
+VOID
+EFIAPI
+SetupResetReminder (
+  VOID
+  );
+
+/**
+  This function converts an input device structure to a Unicode string.
+
+  @param DevPath                  A pointer to the device path structure.
+
+  @return A new allocated Unicode string that represents the device path.
+
+**/
+CHAR16 *
+UiDevicePathToStr (
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevPath
+  );
+#endif
diff --git a/Core/MdeModulePkg/Application/UiApp/UiApp.inf b/Core/MdeModulePkg/Application/UiApp/UiApp.inf
new file mode 100644
index 0000000000..9553c7bab5
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/UiApp.inf
@@ -0,0 +1,151 @@
+## @file
+#  UiApp module is driver for BDS phase.
+#
+#  Copyright (c) 2011 - 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UiApp
+  MODULE_UNI_FILE                = UiApp.uni
+  FILE_GUID                      = 462CAA21-7614-4503-836E-8AB6F4662331
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeUserInterface
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FrontPage.h
+  Ui.h
+  BootMngr/BootManager.h
+  BootMaint/BootMaint.h
+  BootMaint/FormGuid.h
+
+  DeviceMngr/DeviceManager.h
+  DeviceMngr/DeviceManagerVfr.Vfr
+  DeviceMngr/DeviceManagerStrings.uni
+  DeviceMngr/DeviceManager.c
+  BootMngr/BootManagerVfr.Vfr
+  BootMngr/BootManagerStrings.uni
+  BootMngr/BootManager.c
+  BootMaint/FE.vfr
+  BootMaint/FileExplorer.c
+  BootMaint/BootMaint.c
+  BootMaint/UpdatePage.c
+  BootMaint/Variable.c
+  BootMaint/Data.c
+  BootMaint/ConsoleOption.c
+  BootMaint/BootOption.c
+  BootMaint/BmLib.c
+  BootMaint/Bm.vfr
+  BootMaint/Bmstring.uni
+  FrontPageVfr.Vfr
+  FrontPageStrings.uni
+  Strings.uni
+  FrontPage.c
+  String.c
+  String.h
+  Language.c
+  Language.h
+  FormsetGuid.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  BaseLib
+  HobLib
+  UefiRuntimeServicesTableLib
+  ReportStatusCodeLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  PrintLib
+  HiiLib
+  UefiApplicationEntryPoint
+  PcdLib
+  UefiHiiServicesLib
+  UefiBootManagerLib
+  
+[Guids]
+  gEfiGlobalVariableGuid                        ## SOMETIMES_PRODUCES ## Variable:L"BootNext" (The number of next boot option)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"BootXX" (Boot option variable)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"PlatformLang" (Platform supported languange in Rfc4646 format)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"Lang" (Platform supported languange in Iso639 format)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"KeyXX" (Hotkey option variable)
+                                                ## PRODUCES           ## Variable:L"HwErrRecSupport" (The level of platform supported hardware Error Record Persistence)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"BootOptionSupport" (The feature supported in boot option menu, value could be: EFI_BOOT_OPTION_SUPPORT_KEY, EFI_BOOT_OPTION_SUPPORT_APP
+                                                ## SOMETIMES_PRODUCES (not PcdUefiVariableDefaultLangDeprecate)## Variable:L"LangCodes" (Value of PcdUefiVariableDefaultLangCodes)
+                                                ## PRODUCES           ## Variable:L"PlatformLangCodes" (Value of PcdUefiVariableDefaultPlatformLangCodes)
+                                                ## PRODUCES           ## Variable:L"Timeout" (The time out value in second of showing progress bar)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"BootOrder" (The boot option array)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"DriverOrder" (The driver order list)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConIn" (The device path of console in device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConOut" (The device path of console out device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ErrOut" (The device path of error out device)
+  gEfiFileSystemVolumeLabelInfoIdGuid           ## CONSUMES ## GUID (Indicate the information type is volume)
+  gEfiFileInfoGuid                              ## CONSUMES ## GUID (Indicate the information type is file)
+  gEfiHiiPlatformSetupFormsetGuid               ## CONSUMES ## GUID (Indicate the formset class guid to be displayed)
+  gEfiIfrTianoGuid                              ## CONSUMES ## GUID (Extended IFR Guid Opcode)
+  gEfiHiiDriverHealthFormsetGuid                ## CONSUMES ## GUID (Indicate the Driver Health formset class guid to be displayed)
+  gEfiIfrBootMaintenanceGuid                    ## CONSUMES ## GUID
+  
+[Protocols]
+  gEfiSimpleFileSystemProtocolGuid              ## CONSUMES
+  gEfiLoadFileProtocolGuid                      ## CONSUMES
+  gEfiSmbiosProtocolGuid                        ## CONSUMES
+  gEfiGenericMemTestProtocolGuid                ## CONSUMES
+  gEfiUgaDrawProtocolGuid |PcdUgaConsumeSupport ## SOMETIMES_CONSUMES
+  gEfiBlockIoProtocolGuid                       ## CONSUMES
+  gEfiGraphicsOutputProtocolGuid                ## SOMETIMES_CONSUMES
+  gEfiSimpleTextInputExProtocolGuid             ## CONSUMES
+  gEfiHiiConfigAccessProtocolGuid               ## CONSUMES
+  gEfiFormBrowser2ProtocolGuid                  ## CONSUMES
+  gEfiSerialIoProtocolGuid                      ## CONSUMES
+  gEfiDevicePathProtocolGuid                    ## CONSUMES
+  gEfiPciIoProtocolGuid                         ## CONSUMES
+  gEfiDevicePathToTextProtocolGuid              ## CONSUMES
+  gEfiBootLogoProtocolGuid                      ## CONSUMES
+  gEfiLoadedImageDevicePathProtocolGuid         ## CONSUMES
+
+[FeaturePcd]
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLangCodes  ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLang       ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutRow                       ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutColumn                    ## PRODUCES
+  ## CONSUMES
+  ## PRODUCES
+  gEfiMdePkgTokenSpaceGuid.PcdPlatformBootTimeOut
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoHorizontalResolution       ## PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoVerticalResolution         ## PRODUCES
+  ## CONSUMES
+  ## SOMETIMES_PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutColumn
+  ## CONSUMES
+  ## SOMETIMES_PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutRow
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoHorizontalResolution  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoVerticalResolution    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareVersionString           ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UiAppExtra.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/UiApp.uni b/Core/MdeModulePkg/Application/UiApp/UiApp.uni
new file mode 100644
index 0000000000..020fb2d9e4
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/UiApp.uni
diff --git a/Core/MdeModulePkg/Application/UiApp/UiAppExtra.uni b/Core/MdeModulePkg/Application/UiApp/UiAppExtra.uni
new file mode 100644
index 0000000000..f2337ecbc5
--- /dev/null
+++ b/Core/MdeModulePkg/Application/UiApp/UiAppExtra.uni
diff --git a/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.c b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.c
new file mode 100644
index 0000000000..a88b7f1ec4
--- /dev/null
+++ b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.c
@@ -0,0 +1,277 @@
+/** @file
+  If the Variable services have PcdVariableCollectStatistics set to TRUE then
+  this utility will print out the statistics information. You can use console
+  redirection to capture the data.
+
+  Copyright (c) 2006 - 2016, 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 <Uefi.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiApplicationEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include <Guid/VariableFormat.h>
+#include <Guid/SmmVariableCommon.h>
+#include <Guid/PiSmmCommunicationRegionTable.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/SmmVariable.h>
+
+EFI_SMM_COMMUNICATION_PROTOCOL  *mSmmCommunication = NULL;
+
+/**
+  This function get the variable statistics data from SMM variable driver.
+
+  @param[in, out] SmmCommunicateHeader In input, a pointer to a collection of data that will
+                                       be passed into an SMM environment. In output, a pointer
+                                       to a collection of data that comes from an SMM environment.
+  @param[in, out] SmmCommunicateSize   The size of the SmmCommunicateHeader.
+
+  @retval EFI_SUCCESS               Get the statistics data information.
+  @retval EFI_NOT_FOUND             Not found.
+  @retval EFI_BUFFER_TO_SMALL       DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+GetVariableStatisticsData (
+  IN OUT  EFI_SMM_COMMUNICATE_HEADER  *SmmCommunicateHeader,
+  IN OUT  UINTN                       *SmmCommunicateSize
+  )
+{
+  EFI_STATUS                          Status;
+  SMM_VARIABLE_COMMUNICATE_HEADER     *SmmVariableFunctionHeader;
+
+  CopyGuid (&SmmCommunicateHeader->HeaderGuid, &gEfiSmmVariableProtocolGuid);
+  SmmCommunicateHeader->MessageLength = *SmmCommunicateSize - OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data);
+
+  SmmVariableFunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *) &SmmCommunicateHeader->Data[0];
+  SmmVariableFunctionHeader->Function = SMM_VARIABLE_FUNCTION_GET_STATISTICS;
+
+  Status = mSmmCommunication->Communicate (mSmmCommunication, SmmCommunicateHeader, SmmCommunicateSize);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SmmVariableFunctionHeader->ReturnStatus;
+  return Status;
+}
+
+/**
+
+  This function get and print the variable statistics data from SMM variable driver.
+
+  @retval EFI_SUCCESS               Print the statistics information successfully.
+  @retval EFI_NOT_FOUND             Not found the statistics information.
+
+**/
+EFI_STATUS
+PrintInfoFromSmm (
+  VOID
+  )
+{
+  EFI_STATUS                                     Status;
+  VARIABLE_INFO_ENTRY                            *VariableInfo;
+  EFI_SMM_COMMUNICATE_HEADER                     *CommBuffer;
+  UINTN                                          RealCommSize;
+  UINTN                                          CommSize;
+  SMM_VARIABLE_COMMUNICATE_HEADER                *FunctionHeader;
+  EFI_SMM_VARIABLE_PROTOCOL                      *Smmvariable;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE        *PiSmmCommunicationRegionTable;
+  UINT32                                         Index;
+  EFI_MEMORY_DESCRIPTOR                          *Entry;
+  UINTN                                          Size;
+  UINTN                                          MaxSize;
+
+  Status = gBS->LocateProtocol (&gEfiSmmVariableProtocolGuid, NULL, (VOID **) &Smmvariable);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &mSmmCommunication);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CommBuffer = NULL;
+  RealCommSize = 0;
+  Status = EfiGetSystemConfigurationTable (
+             &gEdkiiPiSmmCommunicationRegionTableGuid,
+             (VOID **) &PiSmmCommunicationRegionTable
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (PiSmmCommunicationRegionTable != NULL);
+  Entry = (EFI_MEMORY_DESCRIPTOR *) (PiSmmCommunicationRegionTable + 1);
+  Size = 0;
+  MaxSize = 0;
+  for (Index = 0; Index < PiSmmCommunicationRegionTable->NumberOfEntries; Index++) {
+    if (Entry->Type == EfiConventionalMemory) {
+      Size = EFI_PAGES_TO_SIZE ((UINTN) Entry->NumberOfPages);
+      if (Size > (SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + sizeof (VARIABLE_INFO_ENTRY))) {
+        if (Size > MaxSize) {
+          MaxSize = Size;
+          RealCommSize = MaxSize;
+          CommBuffer = (EFI_SMM_COMMUNICATE_HEADER *) (UINTN) Entry->PhysicalStart;
+        }
+      }
+    }
+    Entry = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) Entry + PiSmmCommunicationRegionTable->DescriptorSize);
+  }
+  ASSERT (CommBuffer != NULL);
+  ZeroMem (CommBuffer, RealCommSize);
+
+  Print (L"Non-Volatile SMM Variables:\n");
+  do {
+    CommSize = RealCommSize;
+    Status = GetVariableStatisticsData (CommBuffer, &CommSize);
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      Print (L"The generic SMM communication buffer provided by SmmCommunicationRegionTable is too small\n");
+      return Status;
+    }
+
+    if (EFI_ERROR (Status) || (CommSize <= SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE)) {
+      break;
+    }
+
+    FunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *) CommBuffer->Data;
+    VariableInfo   = (VARIABLE_INFO_ENTRY *) FunctionHeader->Data;
+
+    if (!VariableInfo->Volatile) {
+      Print (
+          L"%g R%03d(%03d) W%03d D%03d:%s\n",
+          &VariableInfo->VendorGuid,
+          VariableInfo->ReadCount,
+          VariableInfo->CacheCount,
+          VariableInfo->WriteCount,
+          VariableInfo->DeleteCount,
+          (CHAR16 *)(VariableInfo + 1)
+          );
+    }
+  } while (TRUE);
+
+  Print (L"Volatile SMM Variables:\n");
+  ZeroMem (CommBuffer, RealCommSize);
+  do {
+    CommSize = RealCommSize;
+    Status = GetVariableStatisticsData (CommBuffer, &CommSize);
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      Print (L"The generic SMM communication buffer provided by SmmCommunicationRegionTable is too small\n");
+      return Status;
+    }
+
+    if (EFI_ERROR (Status) || (CommSize <= SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE)) {
+      break;
+    }
+
+    FunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *) CommBuffer->Data;
+    VariableInfo   = (VARIABLE_INFO_ENTRY *) FunctionHeader->Data;
+
+    if (VariableInfo->Volatile) {
+      Print (
+          L"%g R%03d(%03d) W%03d D%03d:%s\n",
+          &VariableInfo->VendorGuid,
+          VariableInfo->ReadCount,
+          VariableInfo->CacheCount,
+          VariableInfo->WriteCount,
+          VariableInfo->DeleteCount,
+          (CHAR16 *)(VariableInfo + 1)
+          );
+    }
+  } while (TRUE);
+
+  return Status;
+}
+
+/**
+  The user Entry Point for Application. The user code starts with this function
+  as the real entry point for the image goes into a library that calls this
+  function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+UefiMain (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+  VARIABLE_INFO_ENTRY   *VariableInfo;
+  VARIABLE_INFO_ENTRY   *Entry;
+
+  Status = EfiGetSystemConfigurationTable (&gEfiVariableGuid, (VOID **)&Entry);
+  if (EFI_ERROR (Status) || (Entry == NULL)) {
+    Status = EfiGetSystemConfigurationTable (&gEfiAuthenticatedVariableGuid, (VOID **)&Entry);
+  }
+
+  if (EFI_ERROR (Status) || (Entry == NULL)) {
+    Status = PrintInfoFromSmm ();
+    if (!EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (!EFI_ERROR (Status) && (Entry != NULL)) {
+    Print (L"Non-Volatile EFI Variables:\n");
+    VariableInfo = Entry;
+    do {
+      if (!VariableInfo->Volatile) {
+        Print (
+          L"%g R%03d(%03d) W%03d D%03d:%s\n",
+          &VariableInfo->VendorGuid,
+          VariableInfo->ReadCount,
+          VariableInfo->CacheCount,
+          VariableInfo->WriteCount,
+          VariableInfo->DeleteCount,
+          VariableInfo->Name
+          );
+      }
+
+      VariableInfo = VariableInfo->Next;
+    } while (VariableInfo != NULL);
+
+    Print (L"Volatile EFI Variables:\n");
+    VariableInfo = Entry;
+    do {
+      if (VariableInfo->Volatile) {
+        Print (
+          L"%g R%03d(%03d) W%03d D%03d:%s\n",
+          &VariableInfo->VendorGuid,
+          VariableInfo->ReadCount,
+          VariableInfo->CacheCount,
+          VariableInfo->WriteCount,
+          VariableInfo->DeleteCount,
+          VariableInfo->Name
+          );
+      }
+      VariableInfo = VariableInfo->Next;
+    } while (VariableInfo != NULL);
+
+  } else {
+    Print (L"Warning: Variable Dxe/Smm driver doesn't enable the feature of statistical information!\n");
+    Print (L"If you want to see this info, please:\n");
+    Print (L"  1. Set PcdVariableCollectStatistics as TRUE\n");
+    Print (L"  2. Rebuild Variable Dxe/Smm driver\n");
+    Print (L"  3. Run \"VariableInfo\" cmd again\n");
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.inf b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.inf
new file mode 100644
index 0000000000..484ad32a4e
--- /dev/null
+++ b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.inf
@@ -0,0 +1,61 @@
+## @file
+#  A shell application that displays statistical information about variable usage.
+#
+#  This application can display statistical information about variable usage for SMM variable
+#  driver and non-SMM variable driver.
+#  Note that if Variable Dxe/Smm driver doesn't enable the feature by setting PcdVariableCollectStatistics
+#  as TRUE, the application will not display variable statistical information.
+#
+#  Copyright (c) 2007 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VariableInfo
+  MODULE_UNI_FILE                = VariableInfo.uni
+  FILE_GUID                      = 202A2922-8C27-4943-9855-26180BF9F113
+  MODULE_TYPE                    = UEFI_APPLICATION
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = UefiMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  VariableInfo.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiApplicationEntryPoint
+  UefiLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  MemoryAllocationLib
+
+[Protocols]
+  gEfiSmmCommunicationProtocolGuid   ## SOMETIMES_CONSUMES
+
+  ## UNDEFINED            # Used to do smm communication
+  ## SOMETIMES_CONSUMES
+  gEfiSmmVariableProtocolGuid
+
+[Guids]
+  gEfiAuthenticatedVariableGuid              ## SOMETIMES_CONSUMES ## SystemTable
+  gEfiVariableGuid                           ## SOMETIMES_CONSUMES ## SystemTable
+  gEdkiiPiSmmCommunicationRegionTableGuid    ## SOMETIMES_CONSUMES ## SystemTable
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  VariableInfoExtra.uni
diff --git a/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.uni b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.uni
new file mode 100644
index 0000000000..138a6fed4d
--- /dev/null
+++ b/Core/MdeModulePkg/Application/VariableInfo/VariableInfo.uni
diff --git a/Core/MdeModulePkg/Application/VariableInfo/VariableInfoExtra.uni b/Core/MdeModulePkg/Application/VariableInfo/VariableInfoExtra.uni
new file mode 100644
index 0000000000..f23f161062
--- /dev/null
+++ b/Core/MdeModulePkg/Application/VariableInfo/VariableInfoExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
new file mode 100644
index 0000000000..4d1b5848a5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
@@ -0,0 +1,2522 @@
+/** @file
+  The file for AHCI mode of ATA host controller.
+
+  Copyright (c) 2010 - 2014, 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 "AtaAtapiPassThru.h"
+
+/**
+  Read AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+
+  @return The register content read.
+
+**/
+UINT32
+EFIAPI
+AhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data = 0;
+
+  PciIo->Mem.Read (
+               PciIo,
+               EfiPciIoWidthUint32,
+               EFI_AHCI_BAR_INDEX,
+               (UINT64) Offset,
+               1,
+               &Data
+               );
+
+  return Data;
+}
+
+/**
+  Write AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  Data         The data used to write down.
+
+**/
+VOID
+EFIAPI
+AhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               EFI_AHCI_BAR_INDEX,
+               (UINT64) Offset,
+               1,
+               &Data
+               );
+
+  return ;
+}
+
+/**
+  Do AND operation with the value of AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  AndData      The data used to do AND operation.
+
+**/
+VOID
+EFIAPI
+AhciAndReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               AndData
+  )
+{
+  UINT32 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data  = AhciReadReg (PciIo, Offset);
+
+  Data &= AndData;
+
+  AhciWriteReg (PciIo, Offset, Data);
+}
+
+/**
+  Do OR operation with the value of AHCI Operation register.
+
+  @param  PciIo        The PCI IO protocol instance.
+  @param  Offset       The operation register offset.
+  @param  OrData       The data used to do OR operation.
+
+**/
+VOID
+EFIAPI
+AhciOrReg (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32               Offset,
+  IN UINT32               OrData
+  )
+{
+  UINT32 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data  = AhciReadReg (PciIo, Offset);
+
+  Data |= OrData;
+
+  AhciWriteReg (PciIo, Offset, Data);
+}
+
+/**
+  Wait for the value of the specified MMIO register set to the test value.
+
+  @param  PciIo             The PCI IO protocol instance.
+  @param  Offset            The MMIO address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The MMIO setting is time out.
+  @retval EFI_SUCCESS       The MMIO is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciWaitMmioSet (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINTN                     Offset,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Value = AhciReadReg (PciIo, (UINT32) Offset) & MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param  Address           The system memory address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciWaitMemSet (
+  IN  EFI_PHYSICAL_ADDRESS      Address,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay =  DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    //
+    // Access sytem memory to see if the value is the tested one.
+    //
+    // The system memory pointed by Address will be updated by the
+    // SATA Host Controller, "volatile" is introduced to prevent
+    // compiler from optimizing the access to the memory address
+    // to only read once.
+    //
+    Value  = *(volatile UINT32 *) (UINTN) Address;
+    Value &= MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Check the memory status to the test value.
+
+  @param[in]       Address           The memory address to test.
+  @param[in]       MaskValue         The mask value of memory.
+  @param[in]       TestValue         The test value of memory.
+  @param[in, out]  Task              Optional. Pointer to the ATA_NONBLOCK_TASK used by
+                                     non-blocking mode. If NULL, then just try once.
+
+  @retval EFI_NOTREADY      The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting retry times out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCheckMemSet (
+  IN     UINTN                     Address,
+  IN     UINT32                    MaskValue,
+  IN     UINT32                    TestValue,
+  IN OUT ATA_NONBLOCK_TASK         *Task
+  )
+{
+  UINT32     Value;
+
+  if (Task != NULL) {
+    Task->RetryTimes--;
+  }
+
+  Value  = *(volatile UINT32 *) Address;
+  Value &= MaskValue;
+
+  if (Value == TestValue) {
+    return EFI_SUCCESS;
+  }
+
+  if ((Task != NULL) && !Task->InfiniteWait && (Task->RetryTimes == 0)) {
+    return EFI_TIMEOUT;
+  } else {
+    return EFI_NOT_READY;
+  }
+}
+
+/**
+  Check if the device is still on port. It also checks if the AHCI controller
+  supports the address and data count will be transferred.
+
+  @param  PciIo            The PCI IO protocol instance.
+  @param  Port             The number of port.
+
+  @retval EFI_SUCCESS      The device is attached to port and the transfer data is
+                           supported by AHCI controller.
+  @retval EFI_UNSUPPORTED  The transfer address and count is not supported by AHCI
+                           controller.
+  @retval EFI_NOT_READY    The physical communication between AHCI controller and device
+                           is not ready.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCheckDeviceStatus (
+  IN  EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN  UINT8                  Port
+  )
+{
+  UINT32      Data;
+  UINT32      Offset;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+
+  Data   = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_SSTS_DET_MASK;
+
+  if (Data == EFI_AHCI_PORT_SSTS_DET_PCE) {
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+
+  Clear the port interrupt and error status. It will also clear
+  HBA interrupt status.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+
+**/
+VOID
+EFIAPI
+AhciClearPortStatus (
+  IN  EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN  UINT8                  Port
+  )
+{
+  UINT32 Offset;
+
+  //
+  // Clear any error status
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+  AhciWriteReg (PciIo, Offset, AhciReadReg (PciIo, Offset));
+
+  //
+  // Clear any port interrupt status
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_IS;
+  AhciWriteReg (PciIo, Offset, AhciReadReg (PciIo, Offset));
+
+  //
+  // Clear any HBA interrupt status
+  //
+  AhciWriteReg (PciIo, EFI_AHCI_IS_OFFSET, AhciReadReg (PciIo, EFI_AHCI_IS_OFFSET));
+}
+
+/**
+  This function is used to dump the Status Registers and if there is ERR bit set
+  in the Status Register, the Error Register's value is also be dumped.
+
+  @param  PciIo            The PCI IO protocol instance.
+  @param  Port             The number of port.
+  @param  AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+AhciDumpPortStatus (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     UINT8                      Port,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock
+  )
+{
+  UINT32               Offset;
+  UINT32               Data;
+
+  ASSERT (PciIo != NULL);
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  if (AtaStatusBlock != NULL) {
+    ZeroMem (AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+    AtaStatusBlock->AtaStatus  = (UINT8)Data;
+    if ((AtaStatusBlock->AtaStatus & BIT0) != 0) {
+      AtaStatusBlock->AtaError = (UINT8)(Data >> 8);
+    }
+  }
+}
+
+
+/**
+  Enable the FIS running for giving port.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+  @param      Timeout        The timeout value of enabling FIS, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The FIS enable setting fails.
+  @retval EFI_TIMEOUT        The FIS enable setting is time out.
+  @retval EFI_SUCCESS        The FIS enable successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciEnableFisReceive (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_FRE);
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_FR,
+           EFI_AHCI_PORT_CMD_FR,
+           Timeout
+           );
+}
+
+/**
+  Disable the FIS running for giving port.
+
+  @param      PciIo          The PCI IO protocol instance.
+  @param      Port           The number of port.
+  @param      Timeout        The timeout value of disabling FIS, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The FIS disable setting fails.
+  @retval EFI_TIMEOUT        The FIS disable setting is time out.
+  @retval EFI_UNSUPPORTED    The port is in running state.
+  @retval EFI_SUCCESS        The FIS disable successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDisableFisReceive (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+  UINT32 Data;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  //
+  // Before disabling Fis receive, the DMA engine of the port should NOT be in running status.
+  //
+  if ((Data & (EFI_AHCI_PORT_CMD_ST | EFI_AHCI_PORT_CMD_CR)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check if the Fis receive DMA engine for the port is running.
+  //
+  if ((Data & EFI_AHCI_PORT_CMD_FR) != EFI_AHCI_PORT_CMD_FR) {
+    return EFI_SUCCESS;
+  }
+
+  AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_FRE));
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_FR,
+           0,
+           Timeout
+           );
+}
+
+
+
+/**
+  Build the command list, command table and prepare the fis receiver.
+
+  @param    PciIo                 The PCI IO protocol instance.
+  @param    AhciRegisters         The pointer to the EFI_AHCI_REGISTERS.
+  @param    Port                  The number of port.
+  @param    PortMultiplier        The timeout value of stop.
+  @param    CommandFis            The control fis will be used for the transfer.
+  @param    CommandList           The command list will be used for the transfer.
+  @param    AtapiCommand          The atapi command will be used for the transfer.
+  @param    AtapiCommandLength    The length of the atapi command.
+  @param    CommandSlotNumber     The command slot will be used for the transfer.
+  @param    DataPhysicalAddr      The pointer to the data buffer pci bus master address.
+  @param    DataLength            The data count to be transferred.
+
+**/
+VOID
+EFIAPI
+AhciBuildCommand (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_COMMAND_FIS       *CommandFis,
+  IN     EFI_AHCI_COMMAND_LIST      *CommandList,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     UINT8                      CommandSlotNumber,
+  IN OUT VOID                       *DataPhysicalAddr,
+  IN     UINT32                     DataLength
+  )
+{
+  UINT64     BaseAddr;
+  UINT32     PrdtNumber;
+  UINT32     PrdtIndex;
+  UINTN      RemainedData;
+  UINTN      MemAddr;
+  DATA_64    Data64;
+  UINT32     Offset;
+
+  //
+  // Filling the PRDT
+  //
+  PrdtNumber = (UINT32)DivU64x32 (((UINT64)DataLength + EFI_AHCI_MAX_DATA_PER_PRDT - 1), EFI_AHCI_MAX_DATA_PER_PRDT);
+
+  //
+  // According to AHCI 1.3 spec, a PRDT entry can point to a maximum 4MB data block.
+  // It also limits that the maximum amount of the PRDT entry in the command table
+  // is 65535.
+  //
+  ASSERT (PrdtNumber <= 65535);
+
+  Data64.Uint64 = (UINTN) (AhciRegisters->AhciRFis) + sizeof (EFI_AHCI_RECEIVED_FIS) * Port;
+
+  BaseAddr = Data64.Uint64;
+
+  ZeroMem ((VOID *)((UINTN) BaseAddr), sizeof (EFI_AHCI_RECEIVED_FIS));
+
+  ZeroMem (AhciRegisters->AhciCommandTable, sizeof (EFI_AHCI_COMMAND_TABLE));
+
+  CommandFis->AhciCFisPmNum = PortMultiplier;
+
+  CopyMem (&AhciRegisters->AhciCommandTable->CommandFis, CommandFis, sizeof (EFI_AHCI_COMMAND_FIS));
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  if (AtapiCommand != NULL) {
+    CopyMem (
+      &AhciRegisters->AhciCommandTable->AtapiCmd,
+      AtapiCommand,
+      AtapiCommandLength
+      );
+
+    CommandList->AhciCmdA = 1;
+    CommandList->AhciCmdP = 1;
+
+    AhciOrReg (PciIo, Offset, (EFI_AHCI_PORT_CMD_DLAE | EFI_AHCI_PORT_CMD_ATAPI));
+  } else {
+    AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_DLAE | EFI_AHCI_PORT_CMD_ATAPI));
+  }
+
+  RemainedData = (UINTN) DataLength;
+  MemAddr      = (UINTN) DataPhysicalAddr;
+  CommandList->AhciCmdPrdtl = PrdtNumber;
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainedData < EFI_AHCI_MAX_DATA_PER_PRDT) {
+      AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbc = (UINT32)RemainedData - 1;
+    } else {
+      AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbc = EFI_AHCI_MAX_DATA_PER_PRDT - 1;
+    }
+
+    Data64.Uint64 = (UINT64)MemAddr;
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDba  = Data64.Uint32.Lower32;
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtIndex].AhciPrdtDbau = Data64.Uint32.Upper32;
+    RemainedData -= EFI_AHCI_MAX_DATA_PER_PRDT;
+    MemAddr      += EFI_AHCI_MAX_DATA_PER_PRDT;
+  }
+
+  //
+  // Set the last PRDT to Interrupt On Complete
+  //
+  if (PrdtNumber > 0) {
+    AhciRegisters->AhciCommandTable->PrdtTable[PrdtNumber - 1].AhciPrdtIoc = 1;
+  }
+
+  CopyMem (
+    (VOID *) ((UINTN) AhciRegisters->AhciCmdList + (UINTN) CommandSlotNumber * sizeof (EFI_AHCI_COMMAND_LIST)),
+    CommandList,
+    sizeof (EFI_AHCI_COMMAND_LIST)
+    );
+
+  Data64.Uint64 = (UINT64)(UINTN) AhciRegisters->AhciCommandTablePciAddr;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtba  = Data64.Uint32.Lower32;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtbau = Data64.Uint32.Upper32;
+  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdPmp   = PortMultiplier;
+
+}
+
+/**
+  Buid a command FIS.
+
+  @param  CmdFis            A pointer to the EFI_AHCI_COMMAND_FIS data structure.
+  @param  AtaCommandBlock   A pointer to the AhciBuildCommandFis data structure.
+
+**/
+VOID
+EFIAPI
+AhciBuildCommandFis (
+  IN OUT EFI_AHCI_COMMAND_FIS    *CmdFis,
+  IN     EFI_ATA_COMMAND_BLOCK   *AtaCommandBlock
+  )
+{
+  ZeroMem (CmdFis, sizeof (EFI_AHCI_COMMAND_FIS));
+
+  CmdFis->AhciCFisType = EFI_AHCI_FIS_REGISTER_H2D;
+  //
+  // Indicator it's a command
+  //
+  CmdFis->AhciCFisCmdInd      = 0x1;
+  CmdFis->AhciCFisCmd         = AtaCommandBlock->AtaCommand;
+
+  CmdFis->AhciCFisFeature     = AtaCommandBlock->AtaFeatures;
+  CmdFis->AhciCFisFeatureExp  = AtaCommandBlock->AtaFeaturesExp;
+
+  CmdFis->AhciCFisSecNum      = AtaCommandBlock->AtaSectorNumber;
+  CmdFis->AhciCFisSecNumExp   = AtaCommandBlock->AtaSectorNumberExp;
+
+  CmdFis->AhciCFisClyLow      = AtaCommandBlock->AtaCylinderLow;
+  CmdFis->AhciCFisClyLowExp   = AtaCommandBlock->AtaCylinderLowExp;
+
+  CmdFis->AhciCFisClyHigh     = AtaCommandBlock->AtaCylinderHigh;
+  CmdFis->AhciCFisClyHighExp  = AtaCommandBlock->AtaCylinderHighExp;
+
+  CmdFis->AhciCFisSecCount    = AtaCommandBlock->AtaSectorCount;
+  CmdFis->AhciCFisSecCountExp = AtaCommandBlock->AtaSectorCountExp;
+
+  CmdFis->AhciCFisDevHead     = (UINT8) (AtaCommandBlock->AtaDeviceHead | 0xE0);
+}
+
+/**
+  Start a PIO data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The PIO data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The PIO data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPioTransfer (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         FisBaseAddr;
+  UINTN                         Offset;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  UINTN                         MapLength;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  UINT64                        Delay;
+  EFI_AHCI_COMMAND_FIS          CFis;
+  EFI_AHCI_COMMAND_LIST         CmdList;
+  UINT32                        PortTfd;
+  UINT32                        PrdCount;
+  BOOLEAN                       InfiniteWait;
+  BOOLEAN                       PioFisReceived;
+  BOOLEAN                       D2hFisReceived;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  if (Read) {
+    Flag = EfiPciIoOperationBusMasterWrite;
+  } else {
+    Flag = EfiPciIoOperationBusMasterRead;
+  }
+
+  //
+  // construct command list and command table with pci bus address
+  //
+  MapLength = DataCount;
+  Status = PciIo->Map (
+                    PciIo,
+                    Flag,
+                    MemoryAddr,
+                    &MapLength,
+                    &PhyAddr,
+                    &Map
+                    );
+
+  if (EFI_ERROR (Status) || (DataCount != MapLength)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // Package read needed
+  //
+  AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+  CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+  CmdList.AhciCmdW   = Read ? 0 : 1;
+
+  AhciBuildCommand (
+    PciIo,
+    AhciRegisters,
+    Port,
+    PortMultiplier,
+    &CFis,
+    &CmdList,
+    AtapiCommand,
+    AtapiCommandLength,
+    0,
+    (VOID *)(UINTN)PhyAddr,
+    DataCount
+    );
+
+  Status = AhciStartCommand (
+             PciIo,
+             Port,
+             0,
+             Timeout
+             );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Check the status and wait the driver sending data
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+
+  if (Read && (AtapiCommand == 0)) {
+    //
+    // Wait device sends the PIO setup fis before data transfer
+    //
+    Status = EFI_TIMEOUT;
+    Delay  = DivU64x32 (Timeout, 1000) + 1;
+    do {
+      PioFisReceived = FALSE;
+      D2hFisReceived = FALSE;
+      Offset = FisBaseAddr + EFI_AHCI_PIO_FIS_OFFSET;
+      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_PIO_SETUP, NULL);
+      if (!EFI_ERROR (Status)) {
+        PioFisReceived = TRUE;
+      }
+      //
+      // According to SATA 2.6 spec section 11.7, D2h FIS means an error encountered.
+      // But Qemu and Marvel 9230 sata controller may just receive a D2h FIS from device
+      // after the transaction is finished successfully.
+      // To get better device compatibilities, we further check if the PxTFD's ERR bit is set.
+      // By this way, we can know if there is a real error happened.
+      //
+      Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_REGISTER_D2H, NULL);
+      if (!EFI_ERROR (Status)) {
+        D2hFisReceived = TRUE;
+      }
+
+      if (PioFisReceived || D2hFisReceived) {
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+        PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+        //
+        // PxTFD will be updated if there is a D2H or SetupFIS received. 
+        //
+        if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+          Status = EFI_DEVICE_ERROR;
+          break;
+        }
+
+        PrdCount = *(volatile UINT32 *) (&(AhciRegisters->AhciCmdList[0].AhciCmdPrdbc));
+        if (PrdCount == DataCount) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      }
+
+      //
+      // Stall for 100 microseconds.
+      //
+      MicroSecondDelay(100);
+
+      Delay--;
+      if (Delay == 0) {
+        Status = EFI_TIMEOUT;
+      }
+    } while (InfiniteWait || (Delay > 0));
+  } else {
+    //
+    // Wait for D2H Fis is received
+    //
+    Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+    Status = AhciWaitMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Timeout
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+    PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+    if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+Exit:
+  AhciStopCommand (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDisableFisReceive (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  PciIo->Unmap (
+    PciIo,
+    Map
+    );
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+
+  return Status;
+}
+
+/**
+  Start a DMA data transfer on specific port
+
+  @param[in]       Instance            The ATA_ATAPI_PASS_THRU_INSTANCE protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The DMA data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The DMA data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDmaTransfer (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Offset;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  UINTN                         MapLength;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  EFI_AHCI_COMMAND_FIS          CFis;
+  EFI_AHCI_COMMAND_LIST         CmdList;
+  UINTN                         FisBaseAddr;
+  UINT32                        PortTfd;
+
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_TPL                       OldTpl;
+
+  Map   = NULL;
+  PciIo = Instance->PciIo;
+
+  if (PciIo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Before starting the Blocking BlockIO operation, push to finish all non-blocking
+  // BlockIO tasks.
+  // Delay 100us to simulate the blocking time out checking.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  while ((Task == NULL) && (!IsListEmpty (&Instance->NonBlockingTaskList))) {
+    AsyncNonBlockingTransferRoutine (NULL, Instance);
+    //
+    // Stall for 100us.
+    //
+    MicroSecondDelay (100);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  if ((Task == NULL) || ((Task != NULL) && (!Task->IsStart))) {
+    //
+    // Mark the Task to indicate that it has been started.
+    //
+    if (Task != NULL) {
+      Task->IsStart      = TRUE;
+    }
+    if (Read) {
+      Flag = EfiPciIoOperationBusMasterWrite;
+    } else {
+      Flag = EfiPciIoOperationBusMasterRead;
+    }
+
+    //
+    // Construct command list and command table with pci bus address.
+    //
+    MapLength = DataCount;
+    Status = PciIo->Map (
+                      PciIo,
+                      Flag,
+                      MemoryAddr,
+                      &MapLength,
+                      &PhyAddr,
+                      &Map
+                      );
+
+    if (EFI_ERROR (Status) || (DataCount != MapLength)) {
+      return EFI_BAD_BUFFER_SIZE;
+    }
+
+    if (Task != NULL) {
+      Task->Map = Map;
+    }
+    //
+    // Package read needed
+    //
+    AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+    ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+    CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+    CmdList.AhciCmdW   = Read ? 0 : 1;
+
+    AhciBuildCommand (
+      PciIo,
+      AhciRegisters,
+      Port,
+      PortMultiplier,
+      &CFis,
+      &CmdList,
+      AtapiCommand,
+      AtapiCommandLength,
+      0,
+      (VOID *)(UINTN)PhyAddr,
+      DataCount
+      );
+
+    Status = AhciStartCommand (
+               PciIo,
+               Port,
+               0,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  }
+
+  //
+  // Wait for command compelte
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Offset      = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+  if (Task != NULL) {
+    //
+    // For Non-blocking
+    //
+    Status = AhciCheckMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Task
+               );
+  } else {
+    Status = AhciWaitMemSet (
+               Offset,
+               EFI_AHCI_FIS_TYPE_MASK,
+               EFI_AHCI_FIS_REGISTER_D2H,
+               Timeout
+               );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+  if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  //
+  // For Blocking mode, the command should be stopped, the Fis should be disabled
+  // and the PciIo should be unmapped.
+  // For non-blocking mode, only when a error is happened (if the return status is
+  // EFI_NOT_READY that means the command doesn't finished, try again.), first do the
+  // context cleanup, then set the packet's Asb status.
+  //
+  if (Task == NULL ||
+      ((Task != NULL) && (Status != EFI_NOT_READY))
+     ) {
+    AhciStopCommand (
+      PciIo,
+      Port,
+      Timeout
+      );
+
+    AhciDisableFisReceive (
+      PciIo,
+      Port,
+      Timeout
+      );
+
+    PciIo->Unmap (
+             PciIo,
+             (Task != NULL) ? Task->Map : Map
+             );
+
+    if (Task != NULL) {
+      Task->Packet->Asb->AtaStatus = 0x01;
+    }
+  }
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+  return Status;
+}
+
+/**
+  Start a non data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The non data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The non data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciNonDataTransfer (
+  IN     EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN     EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN     UINT8                         Port,
+  IN     UINT8                         PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND        *AtapiCommand OPTIONAL,
+  IN     UINT8                         AtapiCommandLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        FisBaseAddr;
+  UINTN                        Offset;
+  UINT32                       PortTfd;
+  EFI_AHCI_COMMAND_FIS         CFis;
+  EFI_AHCI_COMMAND_LIST        CmdList;
+
+  //
+  // Package read needed
+  //
+  AhciBuildCommandFis (&CFis, AtaCommandBlock);
+
+  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
+
+  CmdList.AhciCmdCfl = EFI_AHCI_FIS_REGISTER_H2D_LENGTH / 4;
+
+  AhciBuildCommand (
+    PciIo,
+    AhciRegisters,
+    Port,
+    PortMultiplier,
+    &CFis,
+    &CmdList,
+    AtapiCommand,
+    AtapiCommandLength,
+    0,
+    NULL,
+    0
+    );
+
+  Status = AhciStartCommand (
+             PciIo,
+             Port,
+             0,
+             Timeout
+             );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Wait device sends the Response Fis
+  //
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Offset      = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+  Status      = AhciWaitMemSet (
+                  Offset,
+                  EFI_AHCI_FIS_TYPE_MASK,
+                  EFI_AHCI_FIS_REGISTER_D2H,
+                  Timeout
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
+  if ((PortTfd & EFI_AHCI_PORT_TFD_ERR) != 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  AhciStopCommand (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDisableFisReceive (
+    PciIo,
+    Port,
+    Timeout
+    );
+
+  AhciDumpPortStatus (PciIo, Port, AtaStatusBlock);
+
+  return Status;
+}
+
+/**
+  Stop command running for giving port
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of stop, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The command stop unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command stop successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStopCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32 Offset;
+  UINT32 Data;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  Data   = AhciReadReg (PciIo, Offset);
+
+  if ((Data & (EFI_AHCI_PORT_CMD_ST |  EFI_AHCI_PORT_CMD_CR)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if ((Data & EFI_AHCI_PORT_CMD_ST) != 0) {
+    AhciAndReg (PciIo, Offset, (UINT32)~(EFI_AHCI_PORT_CMD_ST));
+  }
+
+  return AhciWaitMmioSet (
+           PciIo,
+           Offset,
+           EFI_AHCI_PORT_CMD_CR,
+           0,
+           Timeout
+           );
+}
+
+/**
+  Start command for give slot on specific port.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  CommandSlot        The number of Command Slot.
+  @param  Timeout            The timeout value of start, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The command start unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command start successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStartCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT8                     CommandSlot,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     CmdSlotBit;
+  EFI_STATUS Status;
+  UINT32     PortStatus;
+  UINT32     StartCmd;
+  UINT32     PortTfd;
+  UINT32     Offset;
+  UINT32     Capability;
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg(PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+
+  CmdSlotBit = (UINT32) (1 << CommandSlot);
+
+  AhciClearPortStatus (
+    PciIo,
+    Port
+    );
+
+  Status = AhciEnableFisReceive (
+             PciIo,
+             Port,
+             Timeout
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  PortStatus = AhciReadReg (PciIo, Offset);
+
+  StartCmd = 0;
+  if ((PortStatus & EFI_AHCI_PORT_CMD_ALPE) != 0) {
+    StartCmd = AhciReadReg (PciIo, Offset);
+    StartCmd &= ~EFI_AHCI_PORT_CMD_ICC_MASK;
+    StartCmd |= EFI_AHCI_PORT_CMD_ACTIVE;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, Offset);
+
+  if ((PortTfd & (EFI_AHCI_PORT_TFD_BSY | EFI_AHCI_PORT_TFD_DRQ)) != 0) {
+    if ((Capability & BIT24) != 0) {
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_CLO);
+
+      AhciWaitMmioSet (
+        PciIo,
+        Offset,
+        EFI_AHCI_PORT_CMD_CLO,
+        0,
+        Timeout
+        );
+    }
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_ST | StartCmd);
+
+  //
+  // Setting the command
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CI;
+  AhciAndReg (PciIo, Offset, 0);
+  AhciOrReg (PciIo, Offset, CmdSlotBit);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Do AHCI port reset.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of reset, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   The port reset unsuccessfully
+  @retval EFI_TIMEOUT        The reset operation is time out.
+  @retval EFI_SUCCESS        The port reset successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPortReset (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  )
+{
+  EFI_STATUS      Status;
+  UINT32          Offset;
+
+  AhciClearPortStatus (PciIo, Port);
+
+  AhciStopCommand (PciIo, Port, Timeout);
+
+  AhciDisableFisReceive (PciIo, Port, Timeout);
+
+  AhciEnableFisReceive (PciIo, Port, Timeout);
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SCTL;
+
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_SCTL_DET_INIT);
+
+  //
+  // wait 5 millisecond before de-assert DET
+  //
+  MicroSecondDelay (5000);
+
+  AhciAndReg (PciIo, Offset, (UINT32)EFI_AHCI_PORT_SCTL_MASK);
+
+  //
+  // wait 5 millisecond before de-assert DET
+  //
+  MicroSecondDelay (5000);
+
+  //
+  // Wait for communication to be re-established
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+  Status = AhciWaitMmioSet (
+             PciIo,
+             Offset,
+             EFI_AHCI_PORT_SSTS_DET_MASK,
+             EFI_AHCI_PORT_SSTS_DET_PCE,
+             Timeout
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_ERR_CLEAR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Do AHCI HBA reset.
+
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Timeout            The timeout value of reset, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR   AHCI controller is failed to complete hardware reset.
+  @retval EFI_TIMEOUT        The reset operation is time out.
+  @retval EFI_SUCCESS        AHCI controller is reset successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciReset (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT64                 Delay;
+  UINT32                 Value;
+  UINT32                 Capability;
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg (PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  
+  //
+  // Enable AE before accessing any AHCI registers if Supports AHCI Mode Only is not set
+  //
+  if ((Capability & EFI_AHCI_CAP_SAM) == 0) {
+    AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_ENABLE);
+  }
+
+  AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_RESET);
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+
+  do {
+    Value = AhciReadReg(PciIo, EFI_AHCI_GHC_OFFSET);
+
+    if ((Value & EFI_AHCI_GHC_RESET) == 0) {
+      break;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay(100);
+
+    Delay--;
+  } while (Delay > 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send SMART Return Status command to check if the execution of SMART cmd is successful or not.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  AtaStatusBlock      A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS     Successfully get the return status of S.M.A.R.T command execution.
+  @retval Others          Fail to get return status data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciAtaSmartReturnStatusCheck (
+  IN EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN EFI_AHCI_REGISTERS          *AhciRegisters,
+  IN UINT8                       Port,
+  IN UINT8                       PortMultiplier,
+  IN OUT EFI_ATA_STATUS_BLOCK    *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+  UINT8                   LBAMid;
+  UINT8                   LBAHigh;
+  UINTN                   FisBaseAddr;
+  UINT32                  Value;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+  AtaCommandBlock.AtaFeatures     = ATA_SMART_RETURN_STATUS;
+  AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+  AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+  //
+  // Send S.M.A.R.T Read Return Status command to device
+  //
+  Status = AhciNonDataTransfer (
+             PciIo,
+             AhciRegisters,
+             (UINT8)Port,
+             (UINT8)PortMultiplier,
+             NULL,
+             0,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLED)
+      );
+    return EFI_DEVICE_ERROR;
+  }
+
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_ENABLE)
+    );
+
+  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + Port * sizeof (EFI_AHCI_RECEIVED_FIS);
+
+  Value = *(UINT32 *) (FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET);
+
+  if ((Value & EFI_AHCI_FIS_TYPE_MASK) == EFI_AHCI_FIS_REGISTER_D2H) {
+    LBAMid  = ((UINT8 *)(UINTN)(FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET))[5];
+    LBAHigh = ((UINT8 *)(UINTN)(FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET))[6];
+
+    if ((LBAMid == 0x4f) && (LBAHigh == 0xc2)) {
+      //
+      // The threshold exceeded condition is not detected by the device
+      //
+      DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is not detected\n"));
+      REPORT_STATUS_CODE (
+            EFI_PROGRESS_CODE,
+            (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_UNDERTHRESHOLD)
+            );
+    } else if ((LBAMid == 0xf4) && (LBAHigh == 0x2c)) {
+      //
+      // The threshold exceeded condition is detected by the device
+      //
+      DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is detected\n"));
+      REPORT_STATUS_CODE (
+           EFI_PROGRESS_CODE,
+           (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_OVERTHRESHOLD)
+           );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable SMART command of the disk if supported.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  IdentifyData        A pointer to data buffer which is used to contain IDENTIFY data.
+  @param  AtaStatusBlock      A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+AhciAtaSmartSupport (
+  IN EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN UINT8                         Port,
+  IN UINT8                         PortMultiplier,
+  IN EFI_IDENTIFY_DATA             *IdentifyData,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock
+  )
+{
+  EFI_STATUS               Status;
+  EFI_ATA_COMMAND_BLOCK    AtaCommandBlock;
+
+  //
+  // Detect if the device supports S.M.A.R.T.
+  //
+  if ((IdentifyData->AtaData.command_set_supported_82 & 0x0001) != 0x0001) {
+    //
+    // S.M.A.R.T is not supported by the device
+    //
+    DEBUG ((EFI_D_INFO, "S.M.A.R.T feature is not supported at port [%d] PortMultiplier [%d]!\n",
+            Port, PortMultiplier));
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_NOTSUPPORTED)
+      );
+  } else {
+    //
+    // Check if the feature is enabled. If not, then enable S.M.A.R.T.
+    //
+    if ((IdentifyData->AtaData.command_set_feature_enb_85 & 0x0001) != 0x0001) {
+
+      REPORT_STATUS_CODE (
+        EFI_PROGRESS_CODE,
+        (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLE)
+        );
+
+      ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+      AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+      AtaCommandBlock.AtaFeatures     = ATA_SMART_ENABLE_OPERATION;
+      AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+      AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+      //
+      // Send S.M.A.R.T Enable command to device
+      //
+      Status = AhciNonDataTransfer (
+                 PciIo,
+                 AhciRegisters,
+                 (UINT8)Port,
+                 (UINT8)PortMultiplier,
+                 NULL,
+                 0,
+                 &AtaCommandBlock,
+                 AtaStatusBlock,
+                 ATA_ATAPI_TIMEOUT,
+                 NULL
+                 );
+
+
+      if (!EFI_ERROR (Status)) {
+        //
+        // Send S.M.A.R.T AutoSave command to device
+        //
+        ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+        AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+        AtaCommandBlock.AtaFeatures     = 0xD2;
+        AtaCommandBlock.AtaSectorCount  = 0xF1;
+        AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+        AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+
+        Status = AhciNonDataTransfer (
+                   PciIo,
+                   AhciRegisters,
+                   (UINT8)Port,
+                   (UINT8)PortMultiplier,
+                   NULL,
+                   0,
+                   &AtaCommandBlock,
+                   AtaStatusBlock,
+                   ATA_ATAPI_TIMEOUT,
+                   NULL
+                   );
+
+        if (!EFI_ERROR (Status)) {
+          Status = AhciAtaSmartReturnStatusCheck (
+                     PciIo,
+                     AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplier,
+                     AtaStatusBlock
+                     );
+        }
+      }
+    }
+    DEBUG ((EFI_D_INFO, "Enabled S.M.A.R.T feature at port [%d] PortMultiplier [%d]!\n",
+            Port, PortMultiplier));
+  }
+
+  return ;
+}
+
+/**
+  Send Buffer cmd to specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Buffer              The data buffer to store IDENTIFY PACKET data.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciIdentify (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN EFI_AHCI_REGISTERS       *AhciRegisters,
+  IN UINT8                    Port,
+  IN UINT8                    PortMultiplier,
+  IN OUT EFI_IDENTIFY_DATA    *Buffer
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+
+  if (PciIo == NULL || AhciRegisters == NULL || Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_IDENTIFY_DRIVE;
+  AtaCommandBlock.AtaSectorCount = 1;
+
+  Status = AhciPioTransfer (
+             PciIo,
+             AhciRegisters,
+             Port,
+             PortMultiplier,
+             NULL,
+             0,
+             TRUE,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send Buffer cmd to specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Buffer              The data buffer to store IDENTIFY PACKET data.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciIdentifyPacket (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN EFI_AHCI_REGISTERS       *AhciRegisters,
+  IN UINT8                    Port,
+  IN UINT8                    PortMultiplier,
+  IN OUT EFI_IDENTIFY_DATA    *Buffer
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+
+  if (PciIo == NULL || AhciRegisters == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_IDENTIFY_DEVICE;
+  AtaCommandBlock.AtaSectorCount = 1;
+
+  Status = AhciPioTransfer (
+             PciIo,
+             AhciRegisters,
+             Port,
+             PortMultiplier,
+             NULL,
+             0,
+             TRUE,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send SET FEATURE cmd on specific device.
+
+  @param  PciIo               The PCI IO protocol instance.
+  @param  AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param  Port                The number of port.
+  @param  PortMultiplier      The timeout value of stop.
+  @param  Feature             The data to send Feature register.
+  @param  FeatureSpecificData The specific data for SET FEATURE cmd.
+
+  @retval EFI_DEVICE_ERROR    The cmd abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for executing.
+  @retval EFI_SUCCESS         The cmd executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDeviceSetFeature (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN EFI_AHCI_REGISTERS     *AhciRegisters,
+  IN UINT8                  Port,
+  IN UINT8                  PortMultiplier,
+  IN UINT16                 Feature,
+  IN UINT32                 FeatureSpecificData
+  )
+{
+  EFI_STATUS               Status;
+  EFI_ATA_COMMAND_BLOCK    AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK     AtaStatusBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SET_FEATURES;
+  AtaCommandBlock.AtaFeatures     = (UINT8) Feature;
+  AtaCommandBlock.AtaFeaturesExp  = (UINT8) (Feature >> 8);
+  AtaCommandBlock.AtaSectorCount  = (UINT8) FeatureSpecificData;
+  AtaCommandBlock.AtaSectorNumber = (UINT8) (FeatureSpecificData >> 8);
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (FeatureSpecificData >> 16);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (FeatureSpecificData >> 24);
+
+  Status = AhciNonDataTransfer (
+             PciIo,
+             AhciRegisters,
+             (UINT8)Port,
+             (UINT8)PortMultiplier,
+             NULL,
+             0,
+             &AtaCommandBlock,
+             &AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command
+  with PIO Protocol.
+
+  @param PciIo              The PCI IO protocol instance.
+  @param AhciRegisters      The pointer to the EFI_AHCI_REGISTERS.
+  @param Port               The number of port.
+  @param PortMultiplier     The number of port multiplier.
+  @param Packet             A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_AHCI_REGISTERS                            *AhciRegisters,
+  IN  UINT8                                         Port,
+  IN  UINT8                                         PortMultiplier,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  )
+{
+  EFI_STATUS                   Status;
+  VOID                         *Buffer;
+  UINT32                       Length;
+  EFI_ATA_COMMAND_BLOCK        AtaCommandBlock;
+  EFI_ATA_STATUS_BLOCK         AtaStatusBlock;
+  BOOLEAN                      Read;
+
+  if (Packet == NULL || Packet->Cdb == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+  ZeroMem (&AtaStatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+  AtaCommandBlock.AtaCommand      = ATA_CMD_PACKET;
+  //
+  // No OVL; No DMA
+  //
+  AtaCommandBlock.AtaFeatures     = 0x00;
+  //
+  // set the transfersize to ATAPI_MAX_BYTE_COUNT to let the device
+  // determine how many data should be transferred.
+  //
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8);
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    Buffer = Packet->InDataBuffer;
+    Length = Packet->InTransferLength;
+    Read = TRUE;
+  } else {
+    Buffer = Packet->OutDataBuffer;
+    Length = Packet->OutTransferLength;
+    Read = FALSE;
+  }
+
+  if (Length == 0) {
+    Status = AhciNonDataTransfer (
+               PciIo,
+               AhciRegisters,
+               Port,
+               PortMultiplier,
+               Packet->Cdb,
+               Packet->CdbLength,
+               &AtaCommandBlock,
+               &AtaStatusBlock,
+               Packet->Timeout,
+               NULL
+               );
+  } else {
+    Status = AhciPioTransfer (
+               PciIo,
+               AhciRegisters,
+               Port,
+               PortMultiplier,
+               Packet->Cdb,
+               Packet->CdbLength,
+               Read,
+               &AtaCommandBlock,
+               &AtaStatusBlock,
+               Buffer,
+               Length,
+               Packet->Timeout,
+               NULL
+               );
+  }
+  return Status;
+}
+
+/**
+  Allocate transfer-related data struct which is used at AHCI mode.
+
+  @param  PciIo                 The PCI IO protocol instance.
+  @param  AhciRegisters         The pointer to the EFI_AHCI_REGISTERS.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciCreateTransferDescriptor (
+  IN     EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN OUT EFI_AHCI_REGISTERS     *AhciRegisters
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Bytes;
+  VOID                  *Buffer;
+
+  UINT32                Capability;
+  UINT32                PortImplementBitMap;
+  UINT8                 MaxPortNumber;
+  UINT8                 MaxCommandSlotNumber;
+  BOOLEAN               Support64Bit;
+  UINT64                MaxReceiveFisSize;
+  UINT64                MaxCommandListSize;
+  UINT64                MaxCommandTableSize;
+  EFI_PHYSICAL_ADDRESS  AhciRFisPciAddr;
+  EFI_PHYSICAL_ADDRESS  AhciCmdListPciAddr;
+  EFI_PHYSICAL_ADDRESS  AhciCommandTablePciAddr;
+
+  Buffer = NULL;
+  //
+  // Collect AHCI controller information
+  //
+  Capability           = AhciReadReg(PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  //
+  // Get the number of command slots per port supported by this HBA.
+  //
+  MaxCommandSlotNumber = (UINT8) (((Capability & 0x1F00) >> 8) + 1);
+  Support64Bit         = (BOOLEAN) (((Capability & BIT31) != 0) ? TRUE : FALSE);
+  
+  PortImplementBitMap  = AhciReadReg(PciIo, EFI_AHCI_PI_OFFSET);
+  //
+  // Get the highest bit of implemented ports which decides how many bytes are allocated for recived FIS.
+  //
+  MaxPortNumber        = (UINT8)(UINTN)(HighBitSet32(PortImplementBitMap) + 1);
+  if (MaxPortNumber == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  MaxReceiveFisSize    = MaxPortNumber * sizeof (EFI_AHCI_RECEIVED_FIS);
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxReceiveFisSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxReceiveFisSize);
+
+  AhciRegisters->AhciRFis          = Buffer;
+  AhciRegisters->MaxReceiveFisSize = MaxReceiveFisSize;
+  Bytes  = (UINTN)MaxReceiveFisSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciRFisPciAddr,
+                    &AhciRegisters->MapRFis
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxReceiveFisSize)) {
+    //
+    // Map error or unable to map the whole RFis buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error6;
+  }
+
+  if ((!Support64Bit) && (AhciRFisPciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error5;
+  }
+  AhciRegisters->AhciRFisPciAddr = (EFI_AHCI_RECEIVED_FIS *)(UINTN)AhciRFisPciAddr;
+
+  //
+  // Allocate memory for command list
+  // Note that the implemenation is a single task model which only use a command list for all ports.
+  //
+  Buffer = NULL;
+  MaxCommandListSize = MaxCommandSlotNumber * sizeof (EFI_AHCI_COMMAND_LIST);
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxCommandListSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Free mapped resource.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error5;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxCommandListSize);
+
+  AhciRegisters->AhciCmdList        = Buffer;
+  AhciRegisters->MaxCommandListSize = MaxCommandListSize;
+  Bytes  = (UINTN)MaxCommandListSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciCmdListPciAddr,
+                    &AhciRegisters->MapCmdList
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxCommandListSize)) {
+    //
+    // Map error or unable to map the whole cmd list buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error4;
+  }
+
+  if ((!Support64Bit) && (AhciCmdListPciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error3;
+  }
+  AhciRegisters->AhciCmdListPciAddr = (EFI_AHCI_COMMAND_LIST *)(UINTN)AhciCmdListPciAddr;
+
+  //
+  // Allocate memory for command table
+  // According to AHCI 1.3 spec, a PRD table can contain maximum 65535 entries.
+  //
+  Buffer = NULL;
+  MaxCommandTableSize = sizeof (EFI_AHCI_COMMAND_TABLE);
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES ((UINTN) MaxCommandTableSize),
+                    &Buffer,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Free mapped resource.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error3;
+  }
+
+  ZeroMem (Buffer, (UINTN)MaxCommandTableSize);
+
+  AhciRegisters->AhciCommandTable    = Buffer;
+  AhciRegisters->MaxCommandTableSize = MaxCommandTableSize;
+  Bytes  = (UINTN)MaxCommandTableSize;
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buffer,
+                    &Bytes,
+                    &AhciCommandTablePciAddr,
+                    &AhciRegisters->MapCommandTable
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != MaxCommandTableSize)) {
+    //
+    // Map error or unable to map the whole cmd list buffer into a contiguous region.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error2;
+  }
+
+  if ((!Support64Bit) && (AhciCommandTablePciAddr > 0x100000000ULL)) {
+    //
+    // The AHCI HBA doesn't support 64bit addressing, so should not get a >4G pci bus master address.
+    //
+    Status = EFI_DEVICE_ERROR;
+    goto Error1;
+  }
+  AhciRegisters->AhciCommandTablePciAddr = (EFI_AHCI_COMMAND_TABLE *)(UINTN)AhciCommandTablePciAddr;
+
+  return EFI_SUCCESS;
+  //
+  // Map error or unable to map the whole CmdList buffer into a contiguous region.
+  //
+Error1:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapCommandTable
+           );
+Error2:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxCommandTableSize),
+           AhciRegisters->AhciCommandTable
+           );
+Error3:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapCmdList
+           );
+Error4:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxCommandListSize),
+           AhciRegisters->AhciCmdList
+           );
+Error5:
+  PciIo->Unmap (
+           PciIo,
+           AhciRegisters->MapRFis
+           );
+Error6:
+  PciIo->FreeBuffer (
+           PciIo,
+           EFI_SIZE_TO_PAGES ((UINTN) MaxReceiveFisSize),
+           AhciRegisters->AhciRFis
+           );
+
+  return Status;
+}
+
+/**
+  Initialize ATA host controller at AHCI mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_PCI_IO_PROTOCOL              *PciIo;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL *IdeInit;
+  UINT32                           Capability;
+  UINT8                            MaxPortNumber;
+  UINT32                           PortImplementBitMap;
+
+  EFI_AHCI_REGISTERS               *AhciRegisters;
+
+  UINT8                            Port;
+  DATA_64                          Data64;
+  UINT32                           Offset;
+  UINT32                           Data;
+  EFI_IDENTIFY_DATA                Buffer;
+  EFI_ATA_DEVICE_TYPE              DeviceType;
+  EFI_ATA_COLLECTIVE_MODE          *SupportedModes;
+  EFI_ATA_TRANSFER_MODE            TransferMode;
+  UINT32                           PhyDetectDelay;
+
+  if (Instance == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PciIo   = Instance->PciIo;
+  IdeInit = Instance->IdeControllerInit;
+
+  Status = AhciReset (PciIo, EFI_AHCI_BUS_RESET_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Collect AHCI controller information
+  //
+  Capability = AhciReadReg (PciIo, EFI_AHCI_CAPABILITY_OFFSET);
+  
+  //
+  // Enable AE before accessing any AHCI registers if Supports AHCI Mode Only is not set
+  //
+  if ((Capability & EFI_AHCI_CAP_SAM) == 0) {
+    AhciOrReg (PciIo, EFI_AHCI_GHC_OFFSET, EFI_AHCI_GHC_ENABLE);
+  }
+  
+  //
+  // Get the number of command slots per port supported by this HBA.
+  //
+  MaxPortNumber        = (UINT8) ((Capability & 0x1F) + 1);
+
+  //
+  // Get the bit map of those ports exposed by this HBA.
+  // It indicates which ports that the HBA supports are available for software to use.
+  //
+  PortImplementBitMap  = AhciReadReg(PciIo, EFI_AHCI_PI_OFFSET);
+
+  AhciRegisters = &Instance->AhciRegisters;
+  Status = AhciCreateTransferDescriptor (PciIo, AhciRegisters);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Port = 0; Port < EFI_AHCI_MAX_PORTS; Port ++) {
+    if ((PortImplementBitMap & (BIT0 << Port)) != 0) {
+      //
+      // According to AHCI spec, MaxPortNumber should be equal or greater than the number of implemented ports.
+      //
+      if ((MaxPortNumber--) == 0) {
+        //
+        // Should never be here.
+        //
+        ASSERT (FALSE);
+        return EFI_SUCCESS;
+      }
+
+      IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelEnumeration, Port);
+
+      //
+      // Initialize FIS Base Address Register and Command List Base Address Register for use.
+      //
+      Data64.Uint64 = (UINTN) (AhciRegisters->AhciRFisPciAddr) + sizeof (EFI_AHCI_RECEIVED_FIS) * Port;
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_FB;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Lower32);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_FBU;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Upper32);
+
+      Data64.Uint64 = (UINTN) (AhciRegisters->AhciCmdListPciAddr);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CLB;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Lower32);
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CLBU;
+      AhciWriteReg (PciIo, Offset, Data64.Uint32.Upper32);
+
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      Data = AhciReadReg (PciIo, Offset);
+      if ((Data & EFI_AHCI_PORT_CMD_CPD) != 0) {
+        AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_POD);
+      }
+
+      if ((Capability & EFI_AHCI_CAP_SSS) != 0) {
+        AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_SUD);
+      }
+
+      //
+      // Disable aggressive power management.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SCTL;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_SCTL_IPM_INIT);
+      //
+      // Disable the reporting of the corresponding interrupt to system software.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_IE;
+      AhciAndReg (PciIo, Offset, 0);
+
+      //
+      // Now inform the IDE Controller Init Module.
+      //
+      IdeInit->NotifyPhase (IdeInit, EfiIdeBusBeforeDevicePresenceDetection, Port);
+
+      //
+      // Enable FIS Receive DMA engine for the first D2H FIS.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+      AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_CMD_FRE);
+      Status = AhciWaitMmioSet (
+                 PciIo,
+                 Offset,
+                 EFI_AHCI_PORT_CMD_FR,
+                 EFI_AHCI_PORT_CMD_FR,
+                 EFI_AHCI_PORT_CMD_FR_CLEAR_TIMEOUT
+                 );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Wait no longer than 10 ms to wait the Phy to detect the presence of a device.
+      // It's the requirment from SATA1.0a spec section 5.2.
+      //
+      PhyDetectDelay = EFI_AHCI_BUS_PHY_DETECT_TIMEOUT;
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+      do {
+        Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_SSTS_DET_MASK;
+        if ((Data == EFI_AHCI_PORT_SSTS_DET_PCE) || (Data == EFI_AHCI_PORT_SSTS_DET)) {
+          break;
+        }
+
+        MicroSecondDelay (1000);
+        PhyDetectDelay--;
+      } while (PhyDetectDelay > 0);
+
+      if (PhyDetectDelay == 0) {
+        //
+        // No device detected at this port.
+        // Clear PxCMD.SUD for those ports at which there are no device present.
+        //
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+        AhciAndReg (PciIo, Offset, (UINT32) ~(EFI_AHCI_PORT_CMD_SUD));
+        continue;
+      }
+
+      //
+      // According to SATA1.0a spec section 5.2, we need to wait for PxTFD.BSY and PxTFD.DRQ
+      // and PxTFD.ERR to be zero. The maximum wait time is 16s which is defined at ATA spec.
+      //
+      PhyDetectDelay = 16 * 1000;
+      do {
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+        if (AhciReadReg(PciIo, Offset) != 0) {
+          AhciWriteReg (PciIo, Offset, AhciReadReg(PciIo, Offset));
+        }
+        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+
+        Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_TFD_MASK;
+        if (Data == 0) {
+          break;
+        }
+
+        MicroSecondDelay (1000);
+        PhyDetectDelay--;
+      } while (PhyDetectDelay > 0);
+
+      if (PhyDetectDelay == 0) {
+        continue;
+      }
+
+      //
+      // When the first D2H register FIS is received, the content of PxSIG register is updated.
+      //
+      Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SIG;
+      Status = AhciWaitMmioSet (
+                 PciIo,
+                 Offset,
+                 0x0000FFFF,
+                 0x00000101,
+                 EFI_TIMER_PERIOD_SECONDS(16)
+                 );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      Data = AhciReadReg (PciIo, Offset);
+      if ((Data & EFI_AHCI_ATAPI_SIG_MASK) == EFI_AHCI_ATAPI_DEVICE_SIG) {
+        Status = AhciIdentifyPacket (PciIo, AhciRegisters, Port, 0, &Buffer);
+
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+
+        DeviceType = EfiIdeCdrom;
+      } else if ((Data & EFI_AHCI_ATAPI_SIG_MASK) == EFI_AHCI_ATA_DEVICE_SIG) {
+        Status = AhciIdentify (PciIo, AhciRegisters, Port, 0, &Buffer);
+
+        if (EFI_ERROR (Status)) {
+          REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_EC_NOT_DETECTED));
+          continue;
+        }
+
+        DeviceType = EfiIdeHarddisk;
+      } else {
+        continue;
+      }
+      DEBUG ((EFI_D_INFO, "port [%d] port mulitplier [%d] has a [%a]\n",
+              Port, 0, DeviceType == EfiIdeCdrom ? "cdrom" : "harddisk"));
+
+      //
+      // If the device is a hard disk, then try to enable S.M.A.R.T feature
+      //
+      if ((DeviceType == EfiIdeHarddisk) && PcdGetBool (PcdAtaSmartEnable)) {
+        AhciAtaSmartSupport (
+          PciIo,
+          AhciRegisters,
+          Port,
+          0,
+          &Buffer,
+          NULL
+          );
+      }
+
+      //
+      // Submit identify data to IDE controller init driver
+      //
+      IdeInit->SubmitData (IdeInit, Port, 0, &Buffer);
+
+      //
+      // Now start to config ide device parameter and transfer mode.
+      //
+      Status = IdeInit->CalculateMode (
+                          IdeInit,
+                          Port,
+                          0,
+                          &SupportedModes
+                          );
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Calculate Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+
+      //
+      // Set best supported PIO mode on this IDE device
+      //
+      if (SupportedModes->PioMode.Mode <= EfiAtaPioMode2) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_DEFAULT_PIO;
+      } else {
+        TransferMode.ModeCategory = EFI_ATA_MODE_FLOW_PIO;
+      }
+
+      TransferMode.ModeNumber = (UINT8) (SupportedModes->PioMode.Mode);
+
+      //
+      // Set supported DMA mode on this IDE device. Note that UDMA & MDMA cann't
+      // be set together. Only one DMA mode can be set to a device. If setting
+      // DMA mode operation fails, we can continue moving on because we only use
+      // PIO mode at boot time. DMA modes are used by certain kind of OS booting
+      //
+      if (SupportedModes->UdmaMode.Valid) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_UDMA;
+        TransferMode.ModeNumber = (UINT8) (SupportedModes->UdmaMode.Mode);
+      } else if (SupportedModes->MultiWordDmaMode.Valid) {
+        TransferMode.ModeCategory = EFI_ATA_MODE_MDMA;
+        TransferMode.ModeNumber = (UINT8) SupportedModes->MultiWordDmaMode.Mode;
+      }
+
+      Status = AhciDeviceSetFeature (PciIo, AhciRegisters, Port, 0, 0x03, (UINT32)(*(UINT8 *)&TransferMode));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+
+      //
+      // Found a ATA or ATAPI device, add it into the device list.
+      //
+      CreateNewDeviceInfo (Instance, Port, 0, DeviceType, &Buffer);
+      if (DeviceType == EfiIdeHarddisk) {
+        REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_PC_ENABLE));
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
new file mode 100644
index 0000000000..6401fb2e9f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
@@ -0,0 +1,370 @@
+/** @file
+  Header file for AHCI mode of ATA host controller.
+  
+  Copyright (c) 2010 - 2014, 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.             
+
+**/
+#ifndef __ATA_HC_AHCI_MODE_H__
+#define __ATA_HC_AHCI_MODE_H__
+
+#define EFI_AHCI_BAR_INDEX                     0x05
+
+#define EFI_AHCI_CAPABILITY_OFFSET             0x0000
+#define   EFI_AHCI_CAP_SAM                     BIT18
+#define   EFI_AHCI_CAP_SSS                     BIT27
+#define   EFI_AHCI_CAP_S64A                    BIT31
+#define EFI_AHCI_GHC_OFFSET                    0x0004
+#define   EFI_AHCI_GHC_RESET                   BIT0
+#define   EFI_AHCI_GHC_IE                      BIT1
+#define   EFI_AHCI_GHC_ENABLE                  BIT31
+#define EFI_AHCI_IS_OFFSET                     0x0008
+#define EFI_AHCI_PI_OFFSET                     0x000C
+
+#define EFI_AHCI_MAX_PORTS                     32
+
+typedef struct {
+  UINT32  Lower32;
+  UINT32  Upper32;
+} DATA_32;
+
+typedef union {
+  DATA_32   Uint32;
+  UINT64    Uint64;
+} DATA_64;
+
+//
+// Refer SATA1.0a spec section 5.2, the Phy detection time should be less than 10ms.
+//
+#define  EFI_AHCI_BUS_PHY_DETECT_TIMEOUT       10
+//
+// Refer SATA1.0a spec, the FIS enable time should be less than 500ms.
+//
+#define  EFI_AHCI_PORT_CMD_FR_CLEAR_TIMEOUT    EFI_TIMER_PERIOD_MILLISECONDS(500)
+//
+// Refer SATA1.0a spec, the bus reset time should be less than 1s.
+//
+#define  EFI_AHCI_BUS_RESET_TIMEOUT            EFI_TIMER_PERIOD_SECONDS(1)
+
+#define  EFI_AHCI_ATAPI_DEVICE_SIG             0xEB140000
+#define  EFI_AHCI_ATA_DEVICE_SIG               0x00000000
+#define  EFI_AHCI_PORT_MULTIPLIER_SIG          0x96690000
+#define  EFI_AHCI_ATAPI_SIG_MASK               0xFFFF0000
+
+//
+// Each PRDT entry can point to a memory block up to 4M byte
+//
+#define EFI_AHCI_MAX_DATA_PER_PRDT             0x400000
+
+#define EFI_AHCI_FIS_REGISTER_H2D              0x27      //Register FIS - Host to Device
+#define   EFI_AHCI_FIS_REGISTER_H2D_LENGTH     20 
+#define EFI_AHCI_FIS_REGISTER_D2H              0x34      //Register FIS - Device to Host
+#define   EFI_AHCI_FIS_REGISTER_D2H_LENGTH     20
+#define EFI_AHCI_FIS_DMA_ACTIVATE              0x39      //DMA Activate FIS - Device to Host
+#define   EFI_AHCI_FIS_DMA_ACTIVATE_LENGTH     4
+#define EFI_AHCI_FIS_DMA_SETUP                 0x41      //DMA Setup FIS - Bi-directional
+#define   EFI_AHCI_FIS_DMA_SETUP_LENGTH        28
+#define EFI_AHCI_FIS_DATA                      0x46      //Data FIS - Bi-directional
+#define EFI_AHCI_FIS_BIST                      0x58      //BIST Activate FIS - Bi-directional
+#define   EFI_AHCI_FIS_BIST_LENGTH             12
+#define EFI_AHCI_FIS_PIO_SETUP                 0x5F      //PIO Setup FIS - Device to Host
+#define   EFI_AHCI_FIS_PIO_SETUP_LENGTH        20
+#define EFI_AHCI_FIS_SET_DEVICE                0xA1      //Set Device Bits FIS - Device to Host
+#define   EFI_AHCI_FIS_SET_DEVICE_LENGTH       8
+
+#define EFI_AHCI_D2H_FIS_OFFSET                0x40
+#define EFI_AHCI_DMA_FIS_OFFSET                0x00
+#define EFI_AHCI_PIO_FIS_OFFSET                0x20
+#define EFI_AHCI_SDB_FIS_OFFSET                0x58
+#define EFI_AHCI_FIS_TYPE_MASK                 0xFF
+#define EFI_AHCI_U_FIS_OFFSET                  0x60
+
+//
+// Port register
+//
+#define EFI_AHCI_PORT_START                    0x0100
+#define EFI_AHCI_PORT_REG_WIDTH                0x0080
+#define EFI_AHCI_PORT_CLB                      0x0000
+#define EFI_AHCI_PORT_CLBU                     0x0004
+#define EFI_AHCI_PORT_FB                       0x0008
+#define EFI_AHCI_PORT_FBU                      0x000C
+#define EFI_AHCI_PORT_IS                       0x0010
+#define   EFI_AHCI_PORT_IS_DHRS                BIT0
+#define   EFI_AHCI_PORT_IS_PSS                 BIT1
+#define   EFI_AHCI_PORT_IS_SSS                 BIT2
+#define   EFI_AHCI_PORT_IS_SDBS                BIT3
+#define   EFI_AHCI_PORT_IS_UFS                 BIT4
+#define   EFI_AHCI_PORT_IS_DPS                 BIT5
+#define   EFI_AHCI_PORT_IS_PCS                 BIT6
+#define   EFI_AHCI_PORT_IS_DIS                 BIT7
+#define   EFI_AHCI_PORT_IS_PRCS                BIT22
+#define   EFI_AHCI_PORT_IS_IPMS                BIT23
+#define   EFI_AHCI_PORT_IS_OFS                 BIT24
+#define   EFI_AHCI_PORT_IS_INFS                BIT26
+#define   EFI_AHCI_PORT_IS_IFS                 BIT27
+#define   EFI_AHCI_PORT_IS_HBDS                BIT28
+#define   EFI_AHCI_PORT_IS_HBFS                BIT29
+#define   EFI_AHCI_PORT_IS_TFES                BIT30
+#define   EFI_AHCI_PORT_IS_CPDS                BIT31
+#define   EFI_AHCI_PORT_IS_CLEAR               0xFFFFFFFF
+#define   EFI_AHCI_PORT_IS_FIS_CLEAR           0x0000000F
+
+#define EFI_AHCI_PORT_IE                       0x0014
+#define EFI_AHCI_PORT_CMD                      0x0018
+#define   EFI_AHCI_PORT_CMD_ST_MASK            0xFFFFFFFE
+#define   EFI_AHCI_PORT_CMD_ST                 BIT0
+#define   EFI_AHCI_PORT_CMD_SUD                BIT1
+#define   EFI_AHCI_PORT_CMD_POD                BIT2
+#define   EFI_AHCI_PORT_CMD_CLO                BIT3
+#define   EFI_AHCI_PORT_CMD_CR                 BIT15
+#define   EFI_AHCI_PORT_CMD_FRE                BIT4
+#define   EFI_AHCI_PORT_CMD_FR                 BIT14
+#define   EFI_AHCI_PORT_CMD_MASK               ~(EFI_AHCI_PORT_CMD_ST | EFI_AHCI_PORT_CMD_FRE | EFI_AHCI_PORT_CMD_COL)
+#define   EFI_AHCI_PORT_CMD_PMA                BIT17
+#define   EFI_AHCI_PORT_CMD_HPCP               BIT18
+#define   EFI_AHCI_PORT_CMD_MPSP               BIT19
+#define   EFI_AHCI_PORT_CMD_CPD                BIT20
+#define   EFI_AHCI_PORT_CMD_ESP                BIT21
+#define   EFI_AHCI_PORT_CMD_ATAPI              BIT24
+#define   EFI_AHCI_PORT_CMD_DLAE               BIT25
+#define   EFI_AHCI_PORT_CMD_ALPE               BIT26
+#define   EFI_AHCI_PORT_CMD_ASP                BIT27
+#define   EFI_AHCI_PORT_CMD_ICC_MASK           (BIT28 | BIT29 | BIT30 | BIT31)
+#define   EFI_AHCI_PORT_CMD_ACTIVE             (1 << 28 )
+#define EFI_AHCI_PORT_TFD                      0x0020
+#define   EFI_AHCI_PORT_TFD_MASK               (BIT7 | BIT3 | BIT0)
+#define   EFI_AHCI_PORT_TFD_BSY                BIT7
+#define   EFI_AHCI_PORT_TFD_DRQ                BIT3
+#define   EFI_AHCI_PORT_TFD_ERR                BIT0
+#define   EFI_AHCI_PORT_TFD_ERR_MASK           0x00FF00
+#define EFI_AHCI_PORT_SIG                      0x0024
+#define EFI_AHCI_PORT_SSTS                     0x0028
+#define   EFI_AHCI_PORT_SSTS_DET_MASK          0x000F
+#define   EFI_AHCI_PORT_SSTS_DET               0x0001
+#define   EFI_AHCI_PORT_SSTS_DET_PCE           0x0003
+#define   EFI_AHCI_PORT_SSTS_SPD_MASK          0x00F0
+#define EFI_AHCI_PORT_SCTL                     0x002C
+#define   EFI_AHCI_PORT_SCTL_DET_MASK          0x000F
+#define   EFI_AHCI_PORT_SCTL_MASK              (~EFI_AHCI_PORT_SCTL_DET_MASK)
+#define   EFI_AHCI_PORT_SCTL_DET_INIT          0x0001
+#define   EFI_AHCI_PORT_SCTL_DET_PHYCOMM       0x0003
+#define   EFI_AHCI_PORT_SCTL_SPD_MASK          0x00F0
+#define   EFI_AHCI_PORT_SCTL_IPM_MASK          0x0F00
+#define   EFI_AHCI_PORT_SCTL_IPM_INIT          0x0300
+#define   EFI_AHCI_PORT_SCTL_IPM_PSD           0x0100
+#define   EFI_AHCI_PORT_SCTL_IPM_SSD           0x0200
+#define EFI_AHCI_PORT_SERR                     0x0030
+#define   EFI_AHCI_PORT_SERR_RDIE              BIT0
+#define   EFI_AHCI_PORT_SERR_RCE               BIT1
+#define   EFI_AHCI_PORT_SERR_TDIE              BIT8
+#define   EFI_AHCI_PORT_SERR_PCDIE             BIT9
+#define   EFI_AHCI_PORT_SERR_PE                BIT10
+#define   EFI_AHCI_PORT_SERR_IE                BIT11
+#define   EFI_AHCI_PORT_SERR_PRC               BIT16
+#define   EFI_AHCI_PORT_SERR_PIE               BIT17
+#define   EFI_AHCI_PORT_SERR_CW                BIT18
+#define   EFI_AHCI_PORT_SERR_BDE               BIT19
+#define   EFI_AHCI_PORT_SERR_DE                BIT20
+#define   EFI_AHCI_PORT_SERR_CRCE              BIT21
+#define   EFI_AHCI_PORT_SERR_HE                BIT22
+#define   EFI_AHCI_PORT_SERR_LSE               BIT23
+#define   EFI_AHCI_PORT_SERR_TSTE              BIT24
+#define   EFI_AHCI_PORT_SERR_UFT               BIT25
+#define   EFI_AHCI_PORT_SERR_EX                BIT26
+#define   EFI_AHCI_PORT_ERR_CLEAR              0xFFFFFFFF
+#define EFI_AHCI_PORT_SACT                     0x0034
+#define EFI_AHCI_PORT_CI                       0x0038
+#define EFI_AHCI_PORT_SNTF                     0x003C
+
+
+#pragma pack(1)
+//
+// Command List structure includes total 32 entries.
+// The entry data structure is listed at the following.
+//
+typedef struct {
+  UINT32   AhciCmdCfl:5;      //Command FIS Length
+  UINT32   AhciCmdA:1;        //ATAPI
+  UINT32   AhciCmdW:1;        //Write
+  UINT32   AhciCmdP:1;        //Prefetchable
+  UINT32   AhciCmdR:1;        //Reset
+  UINT32   AhciCmdB:1;        //BIST
+  UINT32   AhciCmdC:1;        //Clear Busy upon R_OK
+  UINT32   AhciCmdRsvd:1;
+  UINT32   AhciCmdPmp:4;      //Port Multiplier Port
+  UINT32   AhciCmdPrdtl:16;   //Physical Region Descriptor Table Length
+  UINT32   AhciCmdPrdbc;      //Physical Region Descriptor Byte Count
+  UINT32   AhciCmdCtba;       //Command Table Descriptor Base Address
+  UINT32   AhciCmdCtbau;      //Command Table Descriptor Base Address Upper 32-BITs
+  UINT32   AhciCmdRsvd1[4]; 
+} EFI_AHCI_COMMAND_LIST;
+
+//
+// This is a software constructed FIS.
+// For data transfer operations, this is the H2D Register FIS format as 
+// specified in the Serial ATA Revision 2.6 specification.
+//
+typedef struct {
+  UINT8    AhciCFisType;
+  UINT8    AhciCFisPmNum:4;
+  UINT8    AhciCFisRsvd:1;
+  UINT8    AhciCFisRsvd1:1;
+  UINT8    AhciCFisRsvd2:1;
+  UINT8    AhciCFisCmdInd:1;
+  UINT8    AhciCFisCmd;
+  UINT8    AhciCFisFeature;
+  UINT8    AhciCFisSecNum;
+  UINT8    AhciCFisClyLow;
+  UINT8    AhciCFisClyHigh;
+  UINT8    AhciCFisDevHead;
+  UINT8    AhciCFisSecNumExp;
+  UINT8    AhciCFisClyLowExp;
+  UINT8    AhciCFisClyHighExp;
+  UINT8    AhciCFisFeatureExp;
+  UINT8    AhciCFisSecCount;
+  UINT8    AhciCFisSecCountExp;
+  UINT8    AhciCFisRsvd3;
+  UINT8    AhciCFisControl;
+  UINT8    AhciCFisRsvd4[4];
+  UINT8    AhciCFisRsvd5[44];
+} EFI_AHCI_COMMAND_FIS;
+
+//
+// ACMD: ATAPI command (12 or 16 bytes)
+//
+typedef struct {
+  UINT8    AtapiCmd[0x10];
+} EFI_AHCI_ATAPI_COMMAND;
+
+//
+// Physical Region Descriptor Table includes up to 65535 entries
+// The entry data structure is listed at the following.
+// the actual entry number comes from the PRDTL field in the command
+// list entry for this command slot. 
+//
+typedef struct {
+  UINT32   AhciPrdtDba;       //Data Base Address
+  UINT32   AhciPrdtDbau;      //Data Base Address Upper 32-BITs
+  UINT32   AhciPrdtRsvd;
+  UINT32   AhciPrdtDbc:22;    //Data Byte Count
+  UINT32   AhciPrdtRsvd1:9;
+  UINT32   AhciPrdtIoc:1;     //Interrupt on Completion
+} EFI_AHCI_COMMAND_PRDT;
+
+//
+// Command table data strucute which is pointed to by the entry in the command list
+//
+typedef struct {
+  EFI_AHCI_COMMAND_FIS      CommandFis;       // A software constructed FIS.
+  EFI_AHCI_ATAPI_COMMAND    AtapiCmd;         // 12 or 16 bytes ATAPI cmd.
+  UINT8                     Reserved[0x30];
+  EFI_AHCI_COMMAND_PRDT     PrdtTable[65535];     // The scatter/gather list for data transfer
+} EFI_AHCI_COMMAND_TABLE;
+
+//
+// Received FIS structure
+//
+typedef struct {
+  UINT8    AhciDmaSetupFis[0x1C];         // Dma Setup Fis: offset 0x00
+  UINT8    AhciDmaSetupFisRsvd[0x04];
+  UINT8    AhciPioSetupFis[0x14];         // Pio Setup Fis: offset 0x20
+  UINT8    AhciPioSetupFisRsvd[0x0C];     
+  UINT8    AhciD2HRegisterFis[0x14];      // D2H Register Fis: offset 0x40
+  UINT8    AhciD2HRegisterFisRsvd[0x04];
+  UINT64   AhciSetDeviceBitsFis;          // Set Device Bits Fix: offset 0x58
+  UINT8    AhciUnknownFis[0x40];          // Unkonwn Fis: offset 0x60
+  UINT8    AhciUnknownFisRsvd[0x60];      
+} EFI_AHCI_RECEIVED_FIS; 
+
+#pragma pack()
+
+typedef struct {
+  EFI_AHCI_RECEIVED_FIS     *AhciRFis;
+  EFI_AHCI_COMMAND_LIST     *AhciCmdList;
+  EFI_AHCI_COMMAND_TABLE    *AhciCommandTable;
+  EFI_AHCI_RECEIVED_FIS     *AhciRFisPciAddr;
+  EFI_AHCI_COMMAND_LIST     *AhciCmdListPciAddr;
+  EFI_AHCI_COMMAND_TABLE    *AhciCommandTablePciAddr;
+  UINT64                    MaxCommandListSize;
+  UINT64                    MaxCommandTableSize;
+  UINT64                    MaxReceiveFisSize;
+  VOID                      *MapRFis;
+  VOID                      *MapCmdList;
+  VOID                      *MapCommandTable;
+} EFI_AHCI_REGISTERS;
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command 
+  with PIO Protocol.
+
+  @param PciIo              The PCI IO protocol instance.
+  @param AhciRegisters      The pointer to the EFI_AHCI_REGISTERS.
+  @param Port               The number of port.     
+  @param PortMultiplier     The number of port multiplier.
+  @param Packet             A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_AHCI_REGISTERS                            *AhciRegisters,
+  IN  UINT8                                         Port,
+  IN  UINT8                                         PortMultiplier,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  );
+
+/**
+  Start command for give slot on specific port.
+    
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  CommandSlot        The number of CommandSlot.
+  @param  Timeout            The timeout value of start, uses 100ns as a unit.
+   
+  @retval EFI_DEVICE_ERROR   The command start unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command start successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStartCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT8                     CommandSlot,
+  IN  UINT64                    Timeout
+  );
+
+/**
+  Stop command running for giving port
+    
+  @param  PciIo              The PCI IO protocol instance.
+  @param  Port               The number of port.
+  @param  Timeout            The timeout value of stop, uses 100ns as a unit.
+   
+  @retval EFI_DEVICE_ERROR   The command stop unsuccessfully.
+  @retval EFI_TIMEOUT        The operation is time out.
+  @retval EFI_SUCCESS        The command stop successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciStopCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  UINT8                     Port,
+  IN  UINT64                    Timeout
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c
new file mode 100644
index 0000000000..870900f106
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.c
@@ -0,0 +1,2604 @@
+/** @file
+  This file implements ATA_PASSTHRU_PROCTOCOL and EXT_SCSI_PASSTHRU_PROTOCOL interfaces
+  for managed ATA controllers.
+
+  Copyright (c) 2010 - 2015, 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 "AtaAtapiPassThru.h"
+
+//
+//  EFI_DRIVER_BINDING_PROTOCOL instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gAtaAtapiPassThruDriverBinding = {
+  AtaAtapiPassThruSupported,
+  AtaAtapiPassThruStart,
+  AtaAtapiPassThruStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+ATA_ATAPI_PASS_THRU_INSTANCE gAtaAtapiPassThruInstanceTemplate = {
+  ATA_ATAPI_PASS_THRU_SIGNATURE,
+  0,                  // Controller Handle
+  NULL,               // PciIo Protocol
+  NULL,               // IdeControllerInit Protocol
+  {                   // AtaPassThruMode
+    //
+    // According to UEFI2.3 spec Section 12.10, Drivers for non-RAID ATA controllers should set
+    // both EFI_ATA_PASS_THRU_ATTRIBUTES_PHYSICAL and EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL
+    // bits.
+    // Note that the driver doesn't support AtaPassThru non blocking I/O.
+    //
+    EFI_ATA_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_ATA_PASS_THRU_ATTRIBUTES_NONBLOCKIO,
+    //
+    // IoAlign
+    //
+    sizeof (UINTN)
+  },
+  {                   // AtaPassThru
+    NULL,
+    AtaPassThruPassThru,
+    AtaPassThruGetNextPort,
+    AtaPassThruGetNextDevice,
+    AtaPassThruBuildDevicePath,
+    AtaPassThruGetDevice,
+    AtaPassThruResetPort,
+    AtaPassThruResetDevice
+  },
+  {                   // ExtScsiPassThruMode
+    //
+    // AdapterId
+    //
+    0,
+    //
+    // According to UEFI2.3 spec Section 14.7, Drivers for non-RAID SCSI controllers should set
+    // both EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL and EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL
+    // bits.
+    // Note that the driver doesn't support ExtScsiPassThru non blocking I/O.
+    //
+    EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL,
+    //
+    // IoAlign
+    //
+    sizeof (UINTN)
+  },
+  {                   // ExtScsiPassThru
+    NULL,
+    ExtScsiPassThruPassThru,
+    ExtScsiPassThruGetNextTargetLun,
+    ExtScsiPassThruBuildDevicePath,
+    ExtScsiPassThruGetTargetLun,
+    ExtScsiPassThruResetChannel,
+    ExtScsiPassThruResetTargetLun,
+    ExtScsiPassThruGetNextTarget
+  },
+  EfiAtaUnknownMode,  // Work Mode
+  {                   // IdeRegisters
+    {0},
+    {0}
+  },
+  {                   // AhciRegisters
+    0
+  },
+  {                   // DeviceList
+    NULL,
+    NULL
+  },
+  0,                  // OriginalAttributes
+  0,                  // PreviousPort
+  0,                  // PreviousPortMultiplier
+  0,                  // PreviousTargetId
+  0,                  // PreviousLun
+  NULL,               // Timer event
+  {                   // NonBlocking TaskList
+    NULL,
+    NULL
+  }
+};
+
+ATAPI_DEVICE_PATH    mAtapiDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_ATAPI_DP,
+    {
+      (UINT8) (sizeof (ATAPI_DEVICE_PATH)),
+      (UINT8) ((sizeof (ATAPI_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0,
+  0
+};
+
+SATA_DEVICE_PATH    mSataDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_SATA_DP,
+    {
+      (UINT8) (sizeof (SATA_DEVICE_PATH)),
+      (UINT8) ((sizeof (SATA_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0,
+  0
+};
+
+UINT8 mScsiId[TARGET_MAX_BYTES] = {
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF,
+  0xFF, 0xFF, 0xFF, 0xFF
+};
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Instance           Pointer to the ATA_ATAPI_PASS_THRU_INSTANCE.
+  @param[in]      Task               Optional. Pointer to the ATA_NONBLOCK_TASK
+                                     used by non-blocking mode.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For
+                                     bi-directional commands, InTransferLength bytes
+                                     were transferred from InDataBuffer. For
+                                     write and bi-directional commands, OutTransferLength
+                                     bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number
+                                     of bytes that could be transferred is returned
+                                     in InTransferLength. For write and bi-directional
+                                     commands, OutTransferLength bytes were transferred
+                                     by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because
+                                     there are too many ATA commands already
+                                     queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting
+                                     to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents
+                                     of Acb are invalid. The ATA command was
+                                     not sent, so no additional status information
+                                     is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThruExecute (
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE     *Instance,
+  IN     ATA_NONBLOCK_TASK                *Task OPTIONAL
+  )
+{
+  EFI_ATA_PASS_THRU_CMD_PROTOCOL  Protocol;
+  EFI_ATA_HC_WORK_MODE            Mode;
+  EFI_STATUS                      Status;
+
+  Protocol = Packet->Protocol;
+
+  Mode = Instance->Mode;
+  switch (Mode) {
+    case EfiAtaIdeMode:
+      //
+      // Reassign IDE mode io port registers' base addresses
+      //
+      Status = GetIdeRegisterIoAddr (Instance->PciIo, Instance->IdeRegisters);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      switch (Protocol) {
+        case EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA:
+          Status = AtaNonDataCommandIn (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN:
+          Status = AtaPioDataInOut (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT:
+          Status = AtaPioDataInOut (
+                     Instance->PciIo,
+                     &Instance->IdeRegisters[Port],
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN:
+          Status = AtaUdmaInOut (
+                     Instance,
+                     &Instance->IdeRegisters[Port],
+                     TRUE,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT:
+          Status = AtaUdmaInOut (
+                     Instance,
+                     &Instance->IdeRegisters[Port],
+                     FALSE,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        default :
+          return EFI_UNSUPPORTED;
+      }
+      break;
+    case EfiAtaAhciMode :
+      switch (Protocol) {
+        case EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA:
+          Status = AhciNonDataTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN:
+          Status = AhciPioTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT:
+          Status = AhciPioTransfer (
+                     Instance->PciIo,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN:
+          Status = AhciDmaTransfer (
+                     Instance,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     TRUE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->InDataBuffer,
+                     Packet->InTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        case EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT:
+          Status = AhciDmaTransfer (
+                     Instance,
+                     &Instance->AhciRegisters,
+                     (UINT8)Port,
+                     (UINT8)PortMultiplierPort,
+                     NULL,
+                     0,
+                     FALSE,
+                     Packet->Acb,
+                     Packet->Asb,
+                     Packet->OutDataBuffer,
+                     Packet->OutTransferLength,
+                     Packet->Timeout,
+                     Task
+                     );
+          break;
+        default :
+          return EFI_UNSUPPORTED;
+      }
+      break;
+
+    default:
+      Status = EFI_DEVICE_ERROR;
+      break;
+  }
+
+  return Status;
+}
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AsyncNonBlockingTransferRoutine (
+  EFI_EVENT  Event,
+  VOID*      Context
+  )
+{
+  LIST_ENTRY                   *Entry;
+  LIST_ENTRY                   *EntryHeader;
+  ATA_NONBLOCK_TASK            *Task;
+  EFI_STATUS                   Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE *Instance;
+
+  Instance   = (ATA_ATAPI_PASS_THRU_INSTANCE *) Context;
+  EntryHeader = &Instance->NonBlockingTaskList;
+  //
+  // Get the Taks from the Taks List and execute it, until there is
+  // no task in the list or the device is busy with task (EFI_NOT_READY).
+  //
+  while (TRUE) {
+    if (!IsListEmpty (EntryHeader)) {
+      Entry = GetFirstNode (EntryHeader);
+      Task  = ATA_NON_BLOCK_TASK_FROM_ENTRY (Entry);
+    } else {
+      return;
+    }
+
+    Status = AtaPassThruPassThruExecute (
+               Task->Port,
+               Task->PortMultiplier,
+               Task->Packet,
+               Instance,
+               Task
+               );
+
+    //
+    // If the data transfer meet a error, remove all tasks in the list since these tasks are
+    // associated with one task from Ata Bus and signal the event with error status.
+    //
+    if ((Status != EFI_NOT_READY) && (Status != EFI_SUCCESS)) {
+      DestroyAsynTaskList (Instance, TRUE);
+      break;
+    }
+
+    //
+    // For Non blocking mode, the Status of EFI_NOT_READY means the operation
+    // is not finished yet. Otherwise the operation is successful.
+    //
+    if (Status == EFI_NOT_READY) {
+      break;
+    } else {
+      RemoveEntryList (&Task->Link);
+      gBS->SignalEvent (Task->Event);
+      FreePool (Task);
+    }
+  }
+}
+
+/**
+  The Entry Point of module.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAtaAtapiPassThru (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gAtaAtapiPassThruDriverBinding,
+             ImageHandle,
+             &gAtaAtapiPassThruComponentName,
+             &gAtaAtapiPassThruComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Because ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  PCI_TYPE00                        PciData;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+
+  //
+  // SATA Controller is a device driver, and should ingore the
+  // "RemainingDevicePath" according to UEFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIdeControllerInitProtocolGuid,
+                  (VOID **) &IdeControllerInit,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiIdeControllerInitProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Now test the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and
+  // Sub Class (offset 0x0A). This controller should be an ATA controller
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (PciData.Hdr.ClassCode),
+                        PciData.Hdr.ClassCode
+                        );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (IS_PCI_IDE (&PciData) || IS_PCI_SATADPA (&PciData)) {
+    return EFI_SUCCESS;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UINT64                            Supports;
+  UINT64                            OriginalPciAttributes;
+
+  Status                = EFI_SUCCESS;
+  IdeControllerInit     = NULL;
+  Instance              = NULL;
+  OriginalPciAttributes = 0;
+
+  DEBUG ((EFI_D_INFO, "==AtaAtapiPassThru Start== Controller = %x\n", Controller));
+
+  Status  = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiIdeControllerInitProtocolGuid,
+                   (VOID **) &IdeControllerInit,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Open Ide_Controller_Init Error, Status=%r", Status));
+    goto ErrorExit;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Get Pci_Io Protocol Error, Status=%r", Status));
+    goto ErrorExit;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Allocate a buffer to store the ATA_ATAPI_PASS_THRU_INSTANCE data structure
+  //
+  Instance = AllocateCopyPool (sizeof (ATA_ATAPI_PASS_THRU_INSTANCE), &gAtaAtapiPassThruInstanceTemplate);
+  if (Instance == NULL) {
+    goto ErrorExit;
+  }
+
+  Instance->ControllerHandle      = Controller;
+  Instance->IdeControllerInit     = IdeControllerInit;
+  Instance->PciIo                 = PciIo;
+  Instance->OriginalPciAttributes = OriginalPciAttributes;
+  Instance->AtaPassThru.Mode      = &Instance->AtaPassThruMode;
+  Instance->ExtScsiPassThru.Mode  = &Instance->ExtScsiPassThruMode;
+  InitializeListHead(&Instance->DeviceList);
+  InitializeListHead(&Instance->NonBlockingTaskList);
+
+  Instance->TimerEvent = NULL;
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  AsyncNonBlockingTransferRoutine,
+                  Instance,
+                  &Instance->TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Set 1ms timer.
+  //
+  Status = gBS->SetTimer (Instance->TimerEvent, TimerPeriodic, 10000);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Enumerate all inserted ATA devices.
+  //
+  Status = EnumerateAttachedDevice (Instance);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiAtaPassThruProtocolGuid, &(Instance->AtaPassThru),
+                  &gEfiExtScsiPassThruProtocolGuid, &(Instance->ExtScsiPassThru),
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+
+ErrorExit:
+  if (IdeControllerInit != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiIdeControllerInitProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  if ((Instance != NULL) && (Instance->TimerEvent != NULL)) {
+    gBS->CloseEvent (Instance->TimerEvent);
+  }
+
+  //
+  // Remove all inserted ATA devices.
+  //
+  DestroyDeviceInfoList(Instance);
+
+  if (Instance != NULL) {
+    FreePool (Instance);
+  }
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                        Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_AHCI_REGISTERS                *AhciRegisters;
+  UINT64                            Supports;
+
+  DEBUG ((EFI_D_INFO, "==AtaAtapiPassThru Stop== Controller = %x\n", Controller));
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (AtaPassThru);
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid, &(Instance->AtaPassThru),
+                  &gEfiExtScsiPassThruProtocolGuid, &(Instance->ExtScsiPassThru),
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Close protocols opened by AtaAtapiPassThru controller driver
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiIdeControllerInitProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Close Non-Blocking timer and free Task list.
+  //
+  if (Instance->TimerEvent != NULL) {
+    gBS->CloseEvent (Instance->TimerEvent);
+    Instance->TimerEvent = NULL;
+  }
+  DestroyAsynTaskList (Instance, FALSE);
+  //
+  // Free allocated resource
+  //
+  DestroyDeviceInfoList (Instance);
+
+  //
+  // If the current working mode is AHCI mode, then pre-allocated resource
+  // for AHCI initialization should be released.
+  //
+  PciIo = Instance->PciIo;
+
+  if (Instance->Mode == EfiAtaAhciMode) {
+    AhciRegisters = &Instance->AhciRegisters;
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapCommandTable
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxCommandTableSize),
+             AhciRegisters->AhciCommandTable
+             );
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapCmdList
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxCommandListSize),
+             AhciRegisters->AhciCmdList
+             );
+    PciIo->Unmap (
+             PciIo,
+             AhciRegisters->MapRFis
+             );
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES ((UINTN) AhciRegisters->MaxReceiveFisSize),
+             AhciRegisters->AhciRFis
+             );
+  }
+
+  //
+  // Disable this ATA host controller.
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    PciIo->Attributes (
+             PciIo,
+             EfiPciIoAttributeOperationDisable,
+             Supports,
+             NULL
+             );
+  }
+
+  //
+  // Restore original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    Instance->OriginalPciAttributes,
+                    NULL
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  FreePool (Instance);
+
+  return Status;
+}
+
+/**
+  Traverse the attached ATA devices list to find out the device to access.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+
+  @retval     The pointer to the data structure of the device info to access.
+
+**/
+LIST_ENTRY *
+EFIAPI
+SearchDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+  LIST_ENTRY           *Node;
+
+  Node = GetFirstNode (&Instance->DeviceList);
+  while (!IsNull (&Instance->DeviceList, Node)) {
+    DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+    if ((DeviceInfo->Type == DeviceType) &&
+        (Port == DeviceInfo->Port) &&
+        (PortMultiplier == DeviceInfo->PortMultiplier)) {
+      return Node;
+    }
+
+    Node = GetNextNode (&Instance->DeviceList, Node);
+  }
+
+  return NULL;
+}
+
+/**
+  Allocate device info data structure to contain device info.
+  And insert the data structure to the tail of device list for tracing.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+  @param[in]  IdentifyData        The data buffer to store the output of the IDENTIFY cmd.
+
+  @retval EFI_SUCCESS             Successfully insert the ata device to the tail of device list.
+  @retval EFI_OUT_OF_RESOURCES    Can not allocate enough resource for use.
+
+**/
+EFI_STATUS
+EFIAPI
+CreateNewDeviceInfo (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE   *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType,
+  IN  EFI_IDENTIFY_DATA              *IdentifyData
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+
+  DeviceInfo = AllocateZeroPool (sizeof (EFI_ATA_DEVICE_INFO));
+
+  if (DeviceInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DeviceInfo->Signature      = ATA_ATAPI_DEVICE_SIGNATURE;
+  DeviceInfo->Port           = Port;
+  DeviceInfo->PortMultiplier = PortMultiplier;
+  DeviceInfo->Type           = DeviceType;
+
+  if (IdentifyData != NULL) {
+    DeviceInfo->IdentifyData = AllocateCopyPool (sizeof (EFI_IDENTIFY_DATA), IdentifyData);
+    if (DeviceInfo->IdentifyData == NULL) {
+      FreePool (DeviceInfo);
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  InsertTailList (&Instance->DeviceList, &DeviceInfo->Link);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Destroy all attached ATA devices info.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+VOID
+EFIAPI
+DestroyDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance
+  )
+{
+  EFI_ATA_DEVICE_INFO  *DeviceInfo;
+  LIST_ENTRY           *Node;
+
+  Node = GetFirstNode (&Instance->DeviceList);
+  while (!IsNull (&Instance->DeviceList, Node)) {
+    DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+    Node = GetNextNode (&Instance->DeviceList, Node);
+
+    RemoveEntryList (&DeviceInfo->Link);
+    if (DeviceInfo->IdentifyData != NULL) {
+      FreePool (DeviceInfo->IdentifyData);
+    }
+    FreePool (DeviceInfo);
+  }
+}
+
+/**
+  Destroy all pending non blocking tasks.
+
+  @param[in]  Instance    A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  IsSigEvent  Indicate whether signal the task event when remove the
+                          task.
+
+**/
+VOID
+EFIAPI
+DestroyAsynTaskList (
+  IN ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN BOOLEAN                       IsSigEvent
+  )
+{
+  LIST_ENTRY           *Entry;
+  LIST_ENTRY           *DelEntry;
+  ATA_NONBLOCK_TASK    *Task;
+  EFI_TPL              OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  if (!IsListEmpty (&Instance->NonBlockingTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for (Entry = (&Instance->NonBlockingTaskList)->ForwardLink;
+        Entry != (&Instance->NonBlockingTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      Task     = ATA_NON_BLOCK_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      if (IsSigEvent) {
+        Task->Packet->Asb->AtaStatus = 0x01;
+        gBS->SignalEvent (Task->Event);
+      }
+      FreePool (Task);
+    }
+  }
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Enumerate all attached ATA devices at IDE mode or AHCI mode separately.
+
+  The function is designed to enumerate all attached ATA devices.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+  @retval EFI_SUCCESS           Successfully enumerate attached ATA devices.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EnumerateAttachedDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance
+  )
+{
+  EFI_STATUS                   Status;
+  PCI_TYPE00                   PciData;
+  UINT8                        ClassCode;
+
+  Status = EFI_SUCCESS;
+
+  Status = Instance->PciIo->Pci.Read (
+                                  Instance->PciIo,
+                                  EfiPciIoWidthUint8,
+                                  PCI_CLASSCODE_OFFSET,
+                                  sizeof (PciData.Hdr.ClassCode),
+                                  PciData.Hdr.ClassCode
+                                  );
+  ASSERT_EFI_ERROR (Status);
+
+  ClassCode = PciData.Hdr.ClassCode[1];
+
+  switch (ClassCode) {
+    case PCI_CLASS_MASS_STORAGE_IDE :
+      //
+      // The ATA controller is working at IDE mode
+      //
+      Instance->Mode = EfiAtaIdeMode;
+
+      Status = IdeModeInitialization (Instance);
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      }
+      break;
+    case PCI_CLASS_MASS_STORAGE_SATADPA :
+      //
+      // The ATA controller is working at AHCI mode
+      //
+      Instance->Mode = EfiAtaAhciMode;
+
+      Status = AhciModeInitialization (Instance);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      }
+
+      break;
+    default :
+      Status = EFI_UNSUPPORTED;
+  }
+
+Done:
+  return Status;
+}
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Event               If non-blocking I/O is not supported then Event is ignored, and blocking
+                                     I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                     Event is not NULL and non blocking I/O is supported, then non-blocking
+                                     I/O is performed, and Event will be signaled when the ATA command completes.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For bi-directional commands,
+                                     InTransferLength bytes were transferred from InDataBuffer. For write and
+                                     bi-directional commands, OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number of bytes that could be transferred
+                                     is returned in InTransferLength. For write and bi-directional commands,
+                                     OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because there are too many ATA commands
+                                     already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents of Acb are invalid. The ATA
+                                     command was not sent, so no additional status information is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThru (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL       *This,
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     EFI_EVENT                        Event OPTIONAL
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  EFI_IDENTIFY_DATA               *IdentifyData;
+  UINT64                          Capacity;
+  UINT32                          MaxSectorCount;
+  ATA_NONBLOCK_TASK               *Task;
+  EFI_TPL                         OldTpl;
+  UINT32                          BlockSize;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->Asb, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Node = SearchDeviceInfoList (Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    Node = SearchDeviceInfoList(Instance, Port, PortMultiplierPort, EfiIdeCdrom);
+    if (Node == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Check whether this device needs 48-bit addressing (ATAPI-6 ata device).
+  // Per ATA-6 spec, word83: bit15 is zero and bit14 is one.
+  // If bit10 is one, it means the ata device support 48-bit addressing.
+  //
+  DeviceInfo     = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+  IdentifyData   = DeviceInfo->IdentifyData;
+  MaxSectorCount = 0x100;
+  if ((IdentifyData->AtaData.command_set_supported_83 & (BIT10 | BIT15 | BIT14)) == 0x4400) {
+    Capacity = *((UINT64 *)IdentifyData->AtaData.maximum_lba_for_48bit_addressing);
+    if (Capacity > 0xFFFFFFF) {
+      //
+      // Capacity exceeds 120GB. 48-bit addressing is really needed
+      // In this case, the max sector count is 0x10000
+      //
+      MaxSectorCount = 0x10000;
+    }
+  }
+
+  BlockSize = 0x200;
+  if ((IdentifyData->AtaData.phy_logic_sector_support & (BIT14 | BIT15)) == BIT14) {
+    //
+    // Check logical block size
+    //
+    if ((IdentifyData->AtaData.phy_logic_sector_support & BIT12) != 0) {
+      BlockSize = (UINT32) (((IdentifyData->AtaData.logic_sector_size_hi << 16) | IdentifyData->AtaData.logic_sector_size_lo) * sizeof (UINT16));
+    }
+  }
+
+  //
+  // convert the transfer length from sector count to byte.
+  //
+  if (((Packet->Length & EFI_ATA_PASS_THRU_LENGTH_BYTES) == 0) &&
+       (Packet->InTransferLength != 0)) {
+    Packet->InTransferLength = Packet->InTransferLength * BlockSize;
+  }
+
+  //
+  // convert the transfer length from sector count to byte.
+  //
+  if (((Packet->Length & EFI_ATA_PASS_THRU_LENGTH_BYTES) == 0) &&
+       (Packet->OutTransferLength != 0)) {
+    Packet->OutTransferLength = Packet->OutTransferLength * BlockSize;
+  }
+
+  //
+  // If the data buffer described by InDataBuffer/OutDataBuffer and InTransferLength/OutTransferLength
+  // is too big to be transferred in a single command, then no data is transferred and EFI_BAD_BUFFER_SIZE
+  // is returned.
+  //
+  if (((Packet->InTransferLength != 0) && (Packet->InTransferLength > MaxSectorCount * BlockSize)) ||
+      ((Packet->OutTransferLength != 0) && (Packet->OutTransferLength > MaxSectorCount * BlockSize))) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // For non-blocking mode, queue the Task into the list.
+  //
+  if (Event != NULL) {
+    Task = AllocateZeroPool (sizeof (ATA_NONBLOCK_TASK));
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Task->Signature      = ATA_NONBLOCKING_TASK_SIGNATURE;
+    Task->Port           = Port;
+    Task->PortMultiplier = PortMultiplierPort;
+    Task->Packet         = Packet;
+    Task->Event          = Event;
+    Task->IsStart        = FALSE;
+    Task->RetryTimes     = DivU64x32(Packet->Timeout, 1000) + 1;
+    if (Packet->Timeout == 0) {
+      Task->InfiniteWait = TRUE;
+    } else {
+      Task->InfiniteWait = FALSE;
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    InsertTailList (&Instance->NonBlockingTaskList, &Task->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    return EFI_SUCCESS;
+  } else {
+    return AtaPassThruPassThruExecute (
+             Port,
+             PortMultiplierPort,
+             Packet,
+             Instance,
+             NULL
+             );
+  }
+}
+
+/**
+  Used to retrieve the list of legal port numbers for ATA devices on an ATA controller.
+  These can either be the list of ports where ATA devices are actually present or the
+  list of legal port numbers for the ATA controller. Regardless, the caller of this
+  function must probe the port number returned to see if an ATA device is actually
+  present at that location on the ATA controller.
+
+  The GetNextPort() function retrieves the port number on an ATA controller. If on input
+  Port is 0xFFFF, then the port number of the first port on the ATA controller is returned
+  in Port and EFI_SUCCESS is returned.
+
+  If Port is a port number that was returned on a previous call to GetNextPort(), then the
+  port number of the next port on the ATA controller is returned in Port, and EFI_SUCCESS
+  is returned. If Port is not 0xFFFF and Port was not returned on a previous call to
+  GetNextPort(), then EFI_INVALID_PARAMETER is returned.
+
+  If Port is the port number of the last port on the ATA controller, then EFI_NOT_FOUND is
+  returned.
+
+  @param[in]      This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in, out] Port          On input, a pointer to the port number on the ATA controller.
+                                On output, a pointer to the next port number on the ATA
+                                controller. An input value of 0xFFFF retrieves the first port
+                                number on the ATA controller.
+
+  @retval EFI_SUCCESS           The next port number on the ATA controller was returned in Port.
+  @retval EFI_NOT_FOUND         There are no more ports on this ATA controller.
+  @retval EFI_INVALID_PARAMETER Port is not 0xFFFF and Port was not returned on a previous call
+                                to GetNextPort().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN OUT UINT16                 *Port
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Port == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Port == 0xFFFF) {
+    //
+    // If the Port is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeHarddisk) {
+        *Port = DeviceInfo->Port;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else if (*Port == Instance->PreviousPort) {
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port > *Port)){
+        *Port = DeviceInfo->Port;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // Port is not equal to 0xFFFF and also not equal to previous return value
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+Exit:
+  //
+  // Update the PreviousPort and PreviousPortMultiplier.
+  //
+  Instance->PreviousPort = *Port;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal port multiplier port numbers for ATA devices on a port of an ATA
+  controller. These can either be the list of port multiplier ports where ATA devices are actually
+  present on port or the list of legal port multiplier ports on that port. Regardless, the caller of this
+  function must probe the port number and port multiplier port number returned to see if an ATA
+  device is actually present.
+
+  The GetNextDevice() function retrieves the port multiplier port number of an ATA device
+  present on a port of an ATA controller.
+
+  If PortMultiplierPort points to a port multiplier port number value that was returned on a
+  previous call to GetNextDevice(), then the port multiplier port number of the next ATA device
+  on the port of the ATA controller is returned in PortMultiplierPort, and EFI_SUCCESS is
+  returned.
+
+  If PortMultiplierPort points to 0xFFFF, then the port multiplier port number of the first
+  ATA device on port of the ATA controller is returned in PortMultiplierPort and
+  EFI_SUCCESS is returned.
+
+  If PortMultiplierPort is not 0xFFFF and the value pointed to by PortMultiplierPort
+  was not returned on a previous call to GetNextDevice(), then EFI_INVALID_PARAMETER
+  is returned.
+
+  If PortMultiplierPort is the port multiplier port number of the last ATA device on the port of
+  the ATA controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number present on the ATA controller.
+  @param[in, out] PortMultiplierPort On input, a pointer to the port multiplier port number of an
+                                     ATA device present on the ATA controller.
+                                     If on input a PortMultiplierPort of 0xFFFF is specified,
+                                     then the port multiplier port number of the first ATA device
+                                     is returned. On output, a pointer to the port multiplier port
+                                     number of the next ATA device present on an ATA controller.
+
+  @retval EFI_SUCCESS                The port multiplier port number of the next ATA device on the port
+                                     of the ATA controller was returned in PortMultiplierPort.
+  @retval EFI_NOT_FOUND              There are no more ATA devices on this port of the ATA controller.
+  @retval EFI_INVALID_PARAMETER      PortMultiplierPort is not 0xFFFF, and PortMultiplierPort was not
+                                     returned on a previous call to GetNextDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN OUT UINT16                 *PortMultiplierPort
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (PortMultiplierPort == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*PortMultiplierPort == 0xFFFF) {
+    //
+    // If the PortMultiplierPort is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port == Port)){
+        *PortMultiplierPort = DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else if (*PortMultiplierPort == Instance->PreviousPortMultiplier) {
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeHarddisk) &&
+           (DeviceInfo->Port == Port) &&
+           (DeviceInfo->PortMultiplier > *PortMultiplierPort)){
+        *PortMultiplierPort = DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // PortMultiplierPort is not equal to 0xFFFF and also not equal to previous return value
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+Exit:
+  //
+  // Update the PreviousPort and PreviousPortMultiplier.
+  //
+  Instance->PreviousPortMultiplier = *PortMultiplierPort;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to allocate and build a device path node for an ATA device on an ATA controller.
+
+  The BuildDevicePath() function allocates and builds a single device node for the ATA
+  device specified by Port and PortMultiplierPort. If the ATA device specified by Port and
+  PortMultiplierPort is not present on the ATA controller, then EFI_NOT_FOUND is returned.
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned. If there are not enough
+  resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of
+  DevicePath are initialized to describe the ATA device specified by Port and PortMultiplierPort,
+  and EFI_SUCCESS is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               Port specifies the port number of the ATA device for which a
+                                     device path node is to be allocated and built.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device for which a
+                                     device path node is to be allocated and built. If there is no
+                                     port multiplier, then specify 0.
+  @param[in, out] DevicePath         A pointer to a single device path node that describes the ATA
+                                     device specified by Port and PortMultiplierPort. This function
+                                     is responsible for allocating the buffer DevicePath with the
+                                     boot service AllocatePool(). It is the caller's responsibility
+                                     to free DevicePath when the caller is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the ATA device specified by
+                                     Port and PortMultiplierPort was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The ATA device specified by Port and PortMultiplierPort does not
+                                     exist on the ATA controller.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruBuildDevicePath (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    DevicePathNode = AllocateCopyPool (sizeof (ATAPI_DEVICE_PATH), &mAtapiDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DevicePathNode->Atapi.PrimarySecondary = (UINT8) Port;
+    DevicePathNode->Atapi.SlaveMaster      = (UINT8) PortMultiplierPort;
+    DevicePathNode->Atapi.Lun              = 0;
+  } else {
+    DevicePathNode = AllocateCopyPool (sizeof (SATA_DEVICE_PATH), &mSataDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Sata.HBAPortNumber            = Port;
+    DevicePathNode->Sata.PortMultiplierPortNumber = PortMultiplierPort;
+    DevicePathNode->Sata.Lun                      = 0;
+  }
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a port number and port multiplier port number.
+
+  The GetDevice() function determines the port and port multiplier port number associated with
+  the ATA device described by DevicePath. If DevicePath is a device path node type that the
+  ATA Pass Thru driver supports, then the ATA Pass Thru driver will attempt to translate the contents
+  DevicePath into a port number and port multiplier port number.
+
+  If this translation is successful, then that port number and port multiplier port number are returned
+  in Port and PortMultiplierPort, and EFI_SUCCESS is returned.
+
+  If DevicePath, Port, or PortMultiplierPort are NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If DevicePath is not a device path node type that the ATA Pass Thru driver supports, then
+  EFI_UNSUPPORTED is returned.
+
+  If DevicePath is a device path node type that the ATA Pass Thru driver supports, but there is not
+  a valid translation from DevicePath to a port number and port multiplier port number, then
+  EFI_NOT_FOUND is returned.
+
+  @param[in]  This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an ATA device on the
+                                  ATA controller.
+  @param[out] Port                On return, points to the port number of an ATA device on the ATA controller.
+  @param[out] PortMultiplierPort  On return, points to the port multiplier port number of an ATA device
+                                  on the ATA controller.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to a port number and port multiplier
+                                  port number, and they were returned in Port and PortMultiplierPort.
+  @retval EFI_INVALID_PARAMETER   DevicePath is NULL.
+  @retval EFI_INVALID_PARAMETER   Port is NULL.
+  @retval EFI_INVALID_PARAMETER   PortMultiplierPort is NULL.
+  @retval EFI_UNSUPPORTED         This driver does not support the device path node type in DevicePath.
+  @retval EFI_NOT_FOUND           A valid translation from DevicePath to a port number and port multiplier
+                                  port number does not exist.
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetDevice (
+  IN  EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  OUT UINT16                     *Port,
+  OUT UINT16                     *PortMultiplierPort
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Port == NULL || PortMultiplierPort == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH or ATAPI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) ||
+      ((DevicePath->SubType != MSG_SATA_DP) &&
+      (DevicePath->SubType != MSG_ATAPI_DP)) ||
+      ((DevicePathNodeLength(DevicePath) != sizeof(ATAPI_DEVICE_PATH)) &&
+      (DevicePathNodeLength(DevicePath) != sizeof(SATA_DEVICE_PATH)))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    *Port               = DevicePathNode->Atapi.PrimarySecondary;
+    *PortMultiplierPort = DevicePathNode->Atapi.SlaveMaster;
+  } else {
+    *Port               = DevicePathNode->Sata.HBAPortNumber;
+    *PortMultiplierPort = DevicePathNode->Sata.PortMultiplierPortNumber;
+  }
+
+  Node = SearchDeviceInfoList(Instance, *Port, *PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a specific port on the ATA controller. This operation also resets all the ATA devices
+  connected to the port.
+
+  The ResetChannel() function resets an a specific port on an ATA controller. This operation
+  resets all the ATA devices connected to that port. If this ATA controller does not support
+  a reset port operation, then EFI_UNSUPPORTED is returned.
+
+  If a device error occurs while executing that port reset operation, then EFI_DEVICE_ERROR is
+  returned.
+
+  If a timeout occurs during the execution of the port reset operation, then EFI_TIMEOUT is returned.
+
+  If the port reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in]  This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  Port          The port number on the ATA controller.
+
+  @retval EFI_SUCCESS       The ATA controller port was reset.
+  @retval EFI_UNSUPPORTED   The ATA controller does not support a port reset operation.
+  @retval EFI_DEVICE_ERROR  A device error occurred while attempting to reset the ATA port.
+  @retval EFI_TIMEOUT       A timeout occurred while attempting to reset the ATA port.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset port operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets an ATA device that is connected to an ATA controller.
+
+  The ResetDevice() function resets the ATA device specified by Port and PortMultiplierPort.
+  If this ATA controller does not support a device reset operation, then EFI_UNSUPPORTED is
+  returned.
+
+  If Port or PortMultiplierPort are not in a valid range for this ATA controller, then
+  EFI_INVALID_PARAMETER is returned.
+
+  If a device error occurs while executing that device reset operation, then EFI_DEVICE_ERROR
+  is returned.
+
+  If a timeout occurs during the execution of the device reset operation, then EFI_TIMEOUT is
+  returned.
+
+  If the device reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in] This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in] Port                Port represents the port number of the ATA device to be reset.
+  @param[in] PortMultiplierPort  The port multiplier port number of the ATA device to reset.
+                                 If there is no port multiplier, then specify 0.
+  @retval EFI_SUCCESS            The ATA device specified by Port and PortMultiplierPort was reset.
+  @retval EFI_UNSUPPORTED        The ATA controller does not support a device reset operation.
+  @retval EFI_INVALID_PARAMETER  Port or PortMultiplierPort are invalid.
+  @retval EFI_DEVICE_ERROR       A device error occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+  @retval EFI_TIMEOUT            A timeout occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN UINT16                     PortMultiplierPort
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  Node = SearchDeviceInfoList (Instance, Port, PortMultiplierPort, EfiIdeHarddisk);
+
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Return success directly then upper layer driver could think reset device operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Sumbit ATAPI request sense command.
+
+  @param[in] This            A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in] Target          The Target is an array of size TARGET_MAX_BYTES and it represents
+                             the id of the SCSI device to send the SCSI Request Packet. Each
+                             transport driver may choose to utilize a subset of this size to suit the needs
+                             of transport target representation. For example, a Fibre Channel driver
+                             may use only 8 bytes (WWN) to represent an FC target.
+  @param[in] Lun             The LUN of the SCSI device to send the SCSI Request Packet.
+  @param[in] SenseData       A pointer to store sense data.
+  @param[in] SenseDataLength The sense data length.
+  @param[in] Timeout         The timeout value to execute this cmd, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS        Send out the ATAPI packet command successfully.
+  @retval EFI_DEVICE_ERROR   The device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketRequestSense (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL         *This,
+  IN  UINT8                                   *Target,
+  IN  UINT64                                  Lun,
+  IN  VOID                                    *SenseData,
+  IN  UINT8                                   SenseDataLength,
+  IN  UINT64                                  Timeout
+  )
+{
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  Packet;
+  UINT8                                       Cdb[12];
+  EFI_STATUS                                  Status;
+
+  ZeroMem (&Packet, sizeof (EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, 12);
+
+  Cdb[0] = ATA_CMD_REQUEST_SENSE;
+  Cdb[4] = SenseDataLength;
+
+  Packet.Timeout          = Timeout;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = 12;
+  Packet.DataDirection    = EFI_EXT_SCSI_DATA_DIRECTION_READ;
+  Packet.InDataBuffer     = SenseData;
+  Packet.InTransferLength = SenseDataLength;
+
+  Status = ExtScsiPassThruPassThru (This, Target, Lun, &Packet, NULL);
+
+  return Status;
+}
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+  EFI_ATA_HC_WORK_MODE            Mode;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  BOOLEAN                         SenseReq;
+  EFI_SCSI_SENSE_DATA             *PtrSenseData;
+  UINTN                           SenseDataLen;
+  EFI_STATUS                      SenseStatus;
+
+  SenseDataLen = 0;
+  Instance     = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((Packet == NULL) || (Packet->Cdb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Don't support variable length CDB
+  //
+  if ((Packet->CdbLength != 6) && (Packet->CdbLength != 10) &&
+      (Packet->CdbLength != 12) && (Packet->CdbLength != 16)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->SenseData, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For ATAPI device, doesn't support multiple LUN device.
+  //
+  if (Lun != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The layout of Target array:
+  //  ________________________________________________________________________
+  // |       Byte 0        |       Byte 1        | ... | TARGET_MAX_BYTES - 1 |
+  // |_____________________|_____________________|_____|______________________|
+  // |                     | The port multiplier |     |                      |
+  // |   The port number   |    port number      | N/A |         N/A          |
+  // |_____________________|_____________________|_____|______________________|
+  //
+  // For ATAPI device, 2 bytes is enough to represent the location of SCSI device.
+  //
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom);
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+  //
+  // ATA_CMD_IDENTIFY_DEVICE cmd is a ATA cmd but not a SCSI cmd.
+  // Normally it should NOT be passed down through ExtScsiPassThru protocol interface.
+  // But to response EFI_DISK_INFO.Identify() request from ScsiDisk, we should handle this command.
+  //
+  if (*((UINT8*)Packet->Cdb) == ATA_CMD_IDENTIFY_DEVICE) {
+    CopyMem (Packet->InDataBuffer, DeviceInfo->IdentifyData, sizeof (EFI_IDENTIFY_DATA));
+    //
+    // For IDENTIFY DEVICE cmd, we don't need to get sense data.
+    //
+    Packet->SenseDataLength = 0;
+    return EFI_SUCCESS;
+  }
+
+  Mode = Instance->Mode;
+  switch (Mode) {
+    case EfiAtaIdeMode:
+      //
+      // Reassign IDE mode io port registers' base addresses
+      //
+      Status = GetIdeRegisterIoAddr (Instance->PciIo, Instance->IdeRegisters);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Status = AtaPacketCommandExecute (Instance->PciIo, &Instance->IdeRegisters[Port], Port, PortMultiplier, Packet);
+      break;
+    case EfiAtaAhciMode:
+      Status = AhciPacketCommandExecute (Instance->PciIo, &Instance->AhciRegisters, Port, PortMultiplier, Packet);
+      break;
+    default :
+      Status = EFI_DEVICE_ERROR;
+      break;
+  }
+
+  //
+  // If the cmd doesn't get executed correctly, then check sense data.
+  //
+  if (EFI_ERROR (Status) && (Packet->SenseDataLength != 0) && (*((UINT8*)Packet->Cdb) != ATA_CMD_REQUEST_SENSE)) {
+    PtrSenseData = AllocateAlignedPages (EFI_SIZE_TO_PAGES (sizeof (EFI_SCSI_SENSE_DATA)), This->Mode->IoAlign);
+    if (PtrSenseData == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    for (SenseReq = TRUE; SenseReq;) {
+      SenseStatus = AtaPacketRequestSense (
+                      This,
+                      Target,
+                      Lun,
+                      PtrSenseData,
+                      sizeof (EFI_SCSI_SENSE_DATA),
+                      Packet->Timeout
+                      );
+      if (EFI_ERROR (SenseStatus)) {
+        break;
+      }
+
+      CopyMem ((UINT8*)Packet->SenseData + SenseDataLen, PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+      SenseDataLen += sizeof (EFI_SCSI_SENSE_DATA);
+
+      //
+      // no more sense key or number of sense keys exceeds predefined,
+      // skip the loop.
+      //
+      if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) ||
+          (SenseDataLen + sizeof (EFI_SCSI_SENSE_DATA) > Packet->SenseDataLength)) {
+        SenseReq = FALSE;
+      }
+    }
+    FreeAlignedPages (PtrSenseData, EFI_SIZE_TO_PAGES (sizeof (EFI_SCSI_SENSE_DATA)));
+  }
+  //
+  // Update the SenseDataLength field to the data length received.
+  //
+  Packet->SenseDataLength = (UINT8)SenseDataLen;
+  return Status;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  UINT8                           *Target8;
+  UINT16                          *Target16;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Target8  = *Target;
+  Target16 = (UINT16 *)*Target;
+
+  if (CompareMem(Target8, mScsiId, TARGET_MAX_BYTES) != 0) {
+    //
+    // For ATAPI device, we use 2 least significant bytes to represent the location of SCSI device.
+    // So the higher bytes in Target array should be 0xFF.
+    //
+    if (CompareMem (&Target8[2], &mScsiId[2], TARGET_MAX_BYTES - 2) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // When Target is not all 0xFF's, compare 2 least significant bytes with
+    // previous target id to see if it is returned by previous call.
+    //
+    if ((*Target16 != Instance->PreviousTargetId) ||
+        (*Lun != Instance->PreviousLun)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Traverse the whole device list to find the next cdrom closed to
+    // the device signified by Target[0] and Target[1].
+    //
+    // Note that we here use a tricky way to find the next cdrom :
+    // All ata devices are detected and inserted into the device list
+    // sequentially.
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeCdrom) &&
+         ((Target8[0] < DeviceInfo->Port) ||
+          ((Target8[0] == DeviceInfo->Port) &&
+           (Target8[1] < DeviceInfo->PortMultiplier)))) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // If the array is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeCdrom) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+Exit:
+  *Lun = 0;
+
+  //
+  // Update the PreviousTargetId.
+  //
+  Instance->PreviousTargetId = *Target16;
+  Instance->PreviousLun      = *Lun;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // can not build device path for the SCSI Host Controller.
+  //
+  if (Lun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom) == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    DevicePathNode = AllocateCopyPool (sizeof (ATAPI_DEVICE_PATH), &mAtapiDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Atapi.PrimarySecondary = Port;
+    DevicePathNode->Atapi.SlaveMaster      = PortMultiplier;
+    DevicePathNode->Atapi.Lun              = (UINT16) Lun;
+  } else {
+    DevicePathNode = AllocateCopyPool (sizeof (SATA_DEVICE_PATH), &mSataDevicePathTemplate);
+    if (DevicePathNode == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    DevicePathNode->Sata.HBAPortNumber            = Port;
+    DevicePathNode->Sata.PortMultiplierPortNumber = PortMultiplier;
+    DevicePathNode->Sata.Lun                      = (UINT16) Lun;
+  }
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  )
+{
+  EFI_DEV_PATH                    *DevicePathNode;
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) ||
+      ((DevicePath->SubType != MSG_ATAPI_DP) &&
+      (DevicePath->SubType != MSG_SATA_DP)) ||
+      ((DevicePathNodeLength(DevicePath) != sizeof(ATAPI_DEVICE_PATH)) &&
+      (DevicePathNodeLength(DevicePath) != sizeof(SATA_DEVICE_PATH)))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  SetMem (*Target, TARGET_MAX_BYTES, 0xFF);
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    (*Target)[0] = (UINT8) DevicePathNode->Atapi.PrimarySecondary;
+    (*Target)[1] = (UINT8) DevicePathNode->Atapi.SlaveMaster;
+    *Lun         = (UINT8) DevicePathNode->Atapi.Lun;
+  } else {
+    (*Target)[0] = (UINT8) DevicePathNode->Sata.HBAPortNumber;
+    (*Target)[1] = (UINT8) DevicePathNode->Sata.PortMultiplierPortNumber;
+    *Lun         = (UINT8) DevicePathNode->Sata.Lun;
+  }
+
+  Node = SearchDeviceInfoList(Instance, (*Target)[0], (*Target)[1], EfiIdeCdrom);
+
+  if (Node == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (*Lun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset channel operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  UINT8                           Port;
+  UINT8                           PortMultiplier;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+  //
+  // For ATAPI device, doesn't support multiple LUN device.
+  //
+  if (Lun != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // The layout of Target array:
+  //  ________________________________________________________________________
+  // |       Byte 0        |       Byte 1        | ... | TARGET_MAX_BYTES - 1 |
+  // |_____________________|_____________________|_____|______________________|
+  // |                     | The port multiplier |     |                      |
+  // |   The port number   |    port number      | N/A |         N/A          |
+  // |_____________________|_____________________|_____|______________________|
+  //
+  // For ATAPI device, 2 bytes is enough to represent the location of SCSI device.
+  //
+  Port           = Target[0];
+  PortMultiplier = Target[1];
+
+  Node = SearchDeviceInfoList(Instance, Port, PortMultiplier, EfiIdeCdrom);
+  if (Node == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Return success directly then upper layer driver could think reset target LUN operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  )
+{
+  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance;
+  LIST_ENTRY                      *Node;
+  EFI_ATA_DEVICE_INFO             *DeviceInfo;
+  UINT8                           *Target8;
+  UINT16                          *Target16;
+
+  Instance = EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || *Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Target8  = *Target;
+  Target16 = (UINT16 *)*Target;
+
+  if (CompareMem(Target8, mScsiId, TARGET_MAX_BYTES) != 0) {
+    //
+    // For ATAPI device, we use 2 least significant bytes to represent the location of SCSI device.
+    // So the higher bytes in Target array should be 0xFF.
+    //
+    if (CompareMem (&Target8[2], &mScsiId[2], TARGET_MAX_BYTES - 2) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // When Target is not all 0xFF's, compare 2 least significant bytes with
+    // previous target id to see if it is returned by previous call.
+    //
+    if (*Target16 != Instance->PreviousTargetId) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Traverse the whole device list to find the next cdrom closed to
+    // the device signified by Target[0] and Target[1].
+    //
+    // Note that we here use a tricky way to find the next cdrom :
+    // All ata devices are detected and inserted into the device list
+    // sequentially.
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if ((DeviceInfo->Type == EfiIdeCdrom) &&
+         ((Target8[0] < DeviceInfo->Port) ||
+          ((Target8[0] == DeviceInfo->Port) &&
+           (Target8[1] < DeviceInfo->PortMultiplier)))) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  } else {
+    //
+    // If the array is all 0xFF's, start to traverse the device list from the beginning
+    //
+    Node = GetFirstNode (&Instance->DeviceList);
+
+    while (!IsNull (&Instance->DeviceList, Node)) {
+      DeviceInfo = ATA_ATAPI_DEVICE_INFO_FROM_THIS (Node);
+
+      if (DeviceInfo->Type == EfiIdeCdrom) {
+        Target8[0] = (UINT8)DeviceInfo->Port;
+        Target8[1] = (UINT8)DeviceInfo->PortMultiplier;
+        goto Exit;
+      }
+
+      Node = GetNextNode (&Instance->DeviceList, Node);
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+Exit:
+  //
+  // Update the PreviousTargetId.
+  //
+  Instance->PreviousTargetId = *Target16;
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h
new file mode 100644
index 0000000000..6f3407c01a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.h
@@ -0,0 +1,1300 @@
+/** @file
+  Header file for ATA/ATAPI PASS THRU driver.
+
+  Copyright (c) 2010 - 2012, 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.
+
+**/
+#ifndef __ATA_ATAPI_PASS_THRU_H__
+#define __ATA_ATAPI_PASS_THRU_H__
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Atapi.h>
+#include <IndustryStandard/Scsi.h>
+
+#include <Protocol/PciIo.h>
+#include <Protocol/IdeControllerInit.h>
+#include <Protocol/AtaPassThru.h>
+#include <Protocol/ScsiPassThruExt.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PciLib.h>
+#include <Library/PcdLib.h>
+#include <Library/TimerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/DevicePathLib.h>
+
+#include "IdeMode.h"
+#include "AhciMode.h"
+
+extern EFI_DRIVER_BINDING_PROTOCOL  gAtaAtapiPassThruDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gAtaAtapiPassThruComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gAtaAtapiPassThruComponentName2;
+
+#define ATA_ATAPI_PASS_THRU_SIGNATURE  SIGNATURE_32 ('a', 'a', 'p', 't')
+#define ATA_ATAPI_DEVICE_SIGNATURE     SIGNATURE_32 ('a', 'd', 'e', 'v')
+#define ATA_NONBLOCKING_TASK_SIGNATURE  SIGNATURE_32 ('a', 't', 's', 'k')
+
+typedef struct _ATA_NONBLOCK_TASK ATA_NONBLOCK_TASK;
+
+typedef enum {
+  EfiAtaIdeMode,
+  EfiAtaAhciMode,
+  EfiAtaRaidMode,
+  EfiAtaUnknownMode
+} EFI_ATA_HC_WORK_MODE;
+
+typedef enum {
+  EfiIdeCdrom,                  /* ATAPI CDROM */
+  EfiIdeHarddisk,               /* Hard Disk */
+  EfiPortMultiplier,            /* Port Multiplier */
+  EfiIdeUnknown
+} EFI_ATA_DEVICE_TYPE;
+
+//
+// Ahci mode device info
+//
+typedef struct {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+
+  UINT16                            Port;
+  UINT16                            PortMultiplier;
+  EFI_ATA_DEVICE_TYPE               Type;
+
+  EFI_IDENTIFY_DATA                 *IdentifyData;
+} EFI_ATA_DEVICE_INFO;
+
+typedef struct {
+  UINT32                            Signature;
+
+  EFI_HANDLE                        ControllerHandle;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeControllerInit;
+
+  EFI_ATA_PASS_THRU_MODE            AtaPassThruMode;
+  EFI_ATA_PASS_THRU_PROTOCOL        AtaPassThru;
+  EFI_EXT_SCSI_PASS_THRU_MODE       ExtScsiPassThruMode;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   ExtScsiPassThru;
+
+  EFI_ATA_HC_WORK_MODE              Mode;
+
+  EFI_IDE_REGISTERS                 IdeRegisters[EfiIdeMaxChannel];
+  EFI_AHCI_REGISTERS                AhciRegisters;
+
+  //
+  // The attached device list
+  //
+  LIST_ENTRY                        DeviceList;
+  UINT64                            OriginalPciAttributes;
+
+  //
+  // For AtaPassThru protocol, using the following bytes to record the previous call in
+  // GetNextPort()/GetNextDevice().
+  //
+  UINT16                            PreviousPort;
+  UINT16                            PreviousPortMultiplier;
+  //
+  // For ExtScsiPassThru protocol, using the following bytes to record the previous call in
+  // GetNextTarget()/GetNextTargetLun().
+  //
+  UINT16                            PreviousTargetId;
+  UINT64                            PreviousLun;
+
+  //
+  // For Non-blocking.
+  //
+  EFI_EVENT                         TimerEvent;
+  LIST_ENTRY                        NonBlockingTaskList;
+} ATA_ATAPI_PASS_THRU_INSTANCE;
+
+//
+// Task for Non-blocking mode.
+//
+struct _ATA_NONBLOCK_TASK {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+
+  UINT16                            Port;
+  UINT16                            PortMultiplier;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  BOOLEAN                           IsStart;
+  EFI_EVENT                         Event;
+  UINT64                            RetryTimes;
+  BOOLEAN                           InfiniteWait;
+  VOID                              *Map;            // Pointer to map.
+  VOID                              *TableMap;       // Pointer to PRD table map.
+  EFI_ATA_DMA_PRD                   *MapBaseAddress; //  Pointer to range Base address for Map.
+  UINTN                             PageCount;       //  The page numbers used by PCIO freebuffer.
+};
+
+//
+// Timeout value which uses 100ns as a unit.
+// It means 3 second span.
+//
+#define ATA_ATAPI_TIMEOUT           EFI_TIMER_PERIOD_SECONDS(3)
+
+#define IS_ALIGNED(addr, size)      (((UINTN) (addr) & (size - 1)) == 0)
+
+#define ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      ATA_ATAPI_PASS_THRU_INSTANCE, \
+      AtaPassThru, \
+      ATA_ATAPI_PASS_THRU_SIGNATURE \
+      )
+
+#define EXT_SCSI_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      ATA_ATAPI_PASS_THRU_INSTANCE, \
+      ExtScsiPassThru, \
+      ATA_ATAPI_PASS_THRU_SIGNATURE \
+      )
+
+#define ATA_ATAPI_DEVICE_INFO_FROM_THIS(a) \
+  CR (a, \
+      EFI_ATA_DEVICE_INFO, \
+      Link, \
+      ATA_ATAPI_DEVICE_SIGNATURE \
+      );
+
+#define ATA_NON_BLOCK_TASK_FROM_ENTRY(a) \
+  CR (a, \
+      ATA_NONBLOCK_TASK, \
+      Link, \
+      ATA_NONBLOCKING_TASK_SIGNATURE \
+      );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Because ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  );
+
+/**
+  Traverse the attached ATA devices list to find out the device to access.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+
+  @retval     The pointer to the data structure of the device info to access.
+
+**/
+LIST_ENTRY *
+EFIAPI
+SearchDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType
+  );
+
+/**
+  Allocate device info data structure to contain device info.
+  And insert the data structure to the tail of device list for tracing.
+
+  @param[in]  Instance            A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  Port                The port number of the ATA device to send the command.
+  @param[in]  PortMultiplierPort  The port multiplier port number of the ATA device to send the command.
+                                  If there is no port multiplier, then specify 0.
+  @param[in]  DeviceType          The device type of the ATA device.
+  @param[in]  IdentifyData        The data buffer to store the output of the IDENTIFY cmd.
+
+  @retval EFI_SUCCESS             Successfully insert the ata device to the tail of device list.
+  @retval EFI_OUT_OF_RESOURCES    Can not allocate enough resource for use.
+
+**/
+EFI_STATUS
+EFIAPI
+CreateNewDeviceInfo (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT16                         Port,
+  IN  UINT16                         PortMultiplier,
+  IN  EFI_ATA_DEVICE_TYPE            DeviceType,
+  IN  EFI_IDENTIFY_DATA              *IdentifyData
+  );
+
+/**
+  Destroy all attached ATA devices info.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+VOID
+EFIAPI
+DestroyDeviceInfoList (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance
+  );
+
+/**
+  Destroy all pending non blocking tasks.
+
+  @param[in]  Instance    A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+  @param[in]  IsSigEvent  Indicate whether signal the task event when remove the
+                          task.
+
+**/
+VOID
+EFIAPI
+DestroyAsynTaskList (
+  IN ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN BOOLEAN                       IsSigEvent
+  );
+
+/**
+  Enumerate all attached ATA devices at IDE mode or AHCI mode separately.
+
+  The function is designed to enumerate all attached ATA devices.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+  @retval EFI_SUCCESS           Successfully enumerate attached ATA devices.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EnumerateAttachedDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE      *Instance
+  );
+
+/**
+  Call back funtion when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AsyncNonBlockingTransferRoutine (
+  EFI_EVENT  Event,
+  VOID*      Context
+  );
+
+/**
+  Sends an ATA command to an ATA device that is attached to the ATA controller. This function
+  supports both blocking I/O and non-blocking I/O. The blocking I/O functionality is required,
+  and the non-blocking I/O functionality is optional.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number of the ATA device to send the command.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device to send the command.
+                                     If there is no port multiplier, then specify 0.
+  @param[in, out] Packet             A pointer to the ATA command to send to the ATA device specified by Port
+                                     and PortMultiplierPort.
+  @param[in]      Event              If non-blocking I/O is not supported then Event is ignored, and blocking
+                                     I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                     Event is not NULL and non blocking I/O is supported, then non-blocking
+                                     I/O is performed, and Event will be signaled when the ATA command completes.
+
+  @retval EFI_SUCCESS                The ATA command was sent by the host. For bi-directional commands,
+                                     InTransferLength bytes were transferred from InDataBuffer. For write and
+                                     bi-directional commands, OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The ATA command was not executed. The number of bytes that could be transferred
+                                     is returned in InTransferLength. For write and bi-directional commands,
+                                     OutTransferLength bytes were transferred by OutDataBuffer.
+  @retval EFI_NOT_READY              The ATA command could not be sent because there are too many ATA commands
+                                     already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the ATA command.
+  @retval EFI_INVALID_PARAMETER      Port, PortMultiplierPort, or the contents of Acb are invalid. The ATA
+                                     command was not sent, so no additional status information is available.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruPassThru (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL       *This,
+  IN     UINT16                           Port,
+  IN     UINT16                           PortMultiplierPort,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *Packet,
+  IN     EFI_EVENT                        Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal port numbers for ATA devices on an ATA controller.
+  These can either be the list of ports where ATA devices are actually present or the
+  list of legal port numbers for the ATA controller. Regardless, the caller of this
+  function must probe the port number returned to see if an ATA device is actually
+  present at that location on the ATA controller.
+
+  The GetNextPort() function retrieves the port number on an ATA controller. If on input
+  Port is 0xFFFF, then the port number of the first port on the ATA controller is returned
+  in Port and EFI_SUCCESS is returned.
+
+  If Port is a port number that was returned on a previous call to GetNextPort(), then the
+  port number of the next port on the ATA controller is returned in Port, and EFI_SUCCESS
+  is returned. If Port is not 0xFFFF and Port was not returned on a previous call to
+  GetNextPort(), then EFI_INVALID_PARAMETER is returned.
+
+  If Port is the port number of the last port on the ATA controller, then EFI_NOT_FOUND is
+  returned.
+
+  @param[in]      This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in, out] Port          On input, a pointer to the port number on the ATA controller.
+                                On output, a pointer to the next port number on the ATA
+                                controller. An input value of 0xFFFF retrieves the first port
+                                number on the ATA controller.
+
+  @retval EFI_SUCCESS           The next port number on the ATA controller was returned in Port.
+  @retval EFI_NOT_FOUND         There are no more ports on this ATA controller.
+  @retval EFI_INVALID_PARAMETER Port is not 0xFFFF and Port was not returned on a previous call
+                                to GetNextPort().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN OUT UINT16                 *Port
+  );
+
+/**
+  Used to retrieve the list of legal port multiplier port numbers for ATA devices on a port of an ATA
+  controller. These can either be the list of port multiplier ports where ATA devices are actually
+  present on port or the list of legal port multiplier ports on that port. Regardless, the caller of this
+  function must probe the port number and port multiplier port number returned to see if an ATA
+  device is actually present.
+
+  The GetNextDevice() function retrieves the port multiplier port number of an ATA device
+  present on a port of an ATA controller.
+
+  If PortMultiplierPort points to a port multiplier port number value that was returned on a
+  previous call to GetNextDevice(), then the port multiplier port number of the next ATA device
+  on the port of the ATA controller is returned in PortMultiplierPort, and EFI_SUCCESS is
+  returned.
+
+  If PortMultiplierPort points to 0xFFFF, then the port multiplier port number of the first
+  ATA device on port of the ATA controller is returned in PortMultiplierPort and
+  EFI_SUCCESS is returned.
+
+  If PortMultiplierPort is not 0xFFFF and the value pointed to by PortMultiplierPort
+  was not returned on a previous call to GetNextDevice(), then EFI_INVALID_PARAMETER
+  is returned.
+
+  If PortMultiplierPort is the port multiplier port number of the last ATA device on the port of
+  the ATA controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               The port number present on the ATA controller.
+  @param[in, out] PortMultiplierPort On input, a pointer to the port multiplier port number of an
+                                     ATA device present on the ATA controller.
+                                     If on input a PortMultiplierPort of 0xFFFF is specified,
+                                     then the port multiplier port number of the first ATA device
+                                     is returned. On output, a pointer to the port multiplier port
+                                     number of the next ATA device present on an ATA controller.
+
+  @retval EFI_SUCCESS                The port multiplier port number of the next ATA device on the port
+                                     of the ATA controller was returned in PortMultiplierPort.
+  @retval EFI_NOT_FOUND              There are no more ATA devices on this port of the ATA controller.
+  @retval EFI_INVALID_PARAMETER      PortMultiplierPort is not 0xFFFF, and PortMultiplierPort was not
+                                     returned on a previous call to GetNextDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetNextDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN OUT UINT16                 *PortMultiplierPort
+  );
+
+/**
+  Used to allocate and build a device path node for an ATA device on an ATA controller.
+
+  The BuildDevicePath() function allocates and builds a single device node for the ATA
+  device specified by Port and PortMultiplierPort. If the ATA device specified by Port and
+  PortMultiplierPort is not present on the ATA controller, then EFI_NOT_FOUND is returned.
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned. If there are not enough
+  resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of
+  DevicePath are initialized to describe the ATA device specified by Port and PortMultiplierPort,
+  and EFI_SUCCESS is returned.
+
+  @param[in]      This               A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]      Port               Port specifies the port number of the ATA device for which a
+                                     device path node is to be allocated and built.
+  @param[in]      PortMultiplierPort The port multiplier port number of the ATA device for which a
+                                     device path node is to be allocated and built. If there is no
+                                     port multiplier, then specify 0.
+  @param[in, out] DevicePath         A pointer to a single device path node that describes the ATA
+                                     device specified by Port and PortMultiplierPort. This function
+                                     is responsible for allocating the buffer DevicePath with the
+                                     boot service AllocatePool(). It is the caller's responsibility
+                                     to free DevicePath when the caller is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the ATA device specified by
+                                     Port and PortMultiplierPort was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The ATA device specified by Port and PortMultiplierPort does not
+                                     exist on the ATA controller.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruBuildDevicePath (
+  IN     EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a port number and port multiplier port number.
+
+  The GetDevice() function determines the port and port multiplier port number associated with
+  the ATA device described by DevicePath. If DevicePath is a device path node type that the
+  ATA Pass Thru driver supports, then the ATA Pass Thru driver will attempt to translate the contents
+  DevicePath into a port number and port multiplier port number.
+
+  If this translation is successful, then that port number and port multiplier port number are returned
+  in Port and PortMultiplierPort, and EFI_SUCCESS is returned.
+
+  If DevicePath, Port, or PortMultiplierPort are NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If DevicePath is not a device path node type that the ATA Pass Thru driver supports, then
+  EFI_UNSUPPORTED is returned.
+
+  If DevicePath is a device path node type that the ATA Pass Thru driver supports, but there is not
+  a valid translation from DevicePath to a port number and port multiplier port number, then
+  EFI_NOT_FOUND is returned.
+
+  @param[in]  This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an ATA device on the
+                                  ATA controller.
+  @param[out] Port                On return, points to the port number of an ATA device on the ATA controller.
+  @param[out] PortMultiplierPort  On return, points to the port multiplier port number of an ATA device
+                                  on the ATA controller.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to a port number and port multiplier
+                                  port number, and they were returned in Port and PortMultiplierPort.
+  @retval EFI_INVALID_PARAMETER   DevicePath is NULL.
+  @retval EFI_INVALID_PARAMETER   Port is NULL.
+  @retval EFI_INVALID_PARAMETER   PortMultiplierPort is NULL.
+  @retval EFI_UNSUPPORTED         This driver does not support the device path node type in DevicePath.
+  @retval EFI_NOT_FOUND           A valid translation from DevicePath to a port number and port multiplier
+                                  port number does not exist.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruGetDevice (
+  IN  EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  OUT UINT16                     *Port,
+  OUT UINT16                     *PortMultiplierPort
+  );
+
+/**
+  Resets a specific port on the ATA controller. This operation also resets all the ATA devices
+  connected to the port.
+
+  The ResetChannel() function resets an a specific port on an ATA controller. This operation
+  resets all the ATA devices connected to that port. If this ATA controller does not support
+  a reset port operation, then EFI_UNSUPPORTED is returned.
+
+  If a device error occurs while executing that port reset operation, then EFI_DEVICE_ERROR is
+  returned.
+
+  If a timeout occurs during the execution of the port reset operation, then EFI_TIMEOUT is returned.
+
+  If the port reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in]  This          A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in]  Port          The port number on the ATA controller.
+
+  @retval EFI_SUCCESS       The ATA controller port was reset.
+  @retval EFI_UNSUPPORTED   The ATA controller does not support a port reset operation.
+  @retval EFI_DEVICE_ERROR  A device error occurred while attempting to reset the ATA port.
+  @retval EFI_TIMEOUT       A timeout occurred while attempting to reset the ATA port.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetPort (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port
+  );
+
+/**
+  Resets an ATA device that is connected to an ATA controller.
+
+  The ResetDevice() function resets the ATA device specified by Port and PortMultiplierPort.
+  If this ATA controller does not support a device reset operation, then EFI_UNSUPPORTED is
+  returned.
+
+  If Port or PortMultiplierPort are not in a valid range for this ATA controller, then
+  EFI_INVALID_PARAMETER is returned.
+
+  If a device error occurs while executing that device reset operation, then EFI_DEVICE_ERROR
+  is returned.
+
+  If a timeout occurs during the execution of the device reset operation, then EFI_TIMEOUT is
+  returned.
+
+  If the device reset operation is completed, then EFI_SUCCESS is returned.
+
+  @param[in] This                A pointer to the EFI_ATA_PASS_THRU_PROTOCOL instance.
+  @param[in] Port                Port represents the port number of the ATA device to be reset.
+  @param[in] PortMultiplierPort  The port multiplier port number of the ATA device to reset.
+                                 If there is no port multiplier, then specify 0.
+  @retval EFI_SUCCESS            The ATA device specified by Port and PortMultiplierPort was reset.
+  @retval EFI_UNSUPPORTED        The ATA controller does not support a device reset operation.
+  @retval EFI_INVALID_PARAMETER  Port or PortMultiplierPort are invalid.
+  @retval EFI_DEVICE_ERROR       A device error occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+  @retval EFI_TIMEOUT            A timeout occurred while attempting to reset the ATA device
+                                 specified by Port and PortMultiplierPort.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPassThruResetDevice (
+  IN EFI_ATA_PASS_THRU_PROTOCOL *This,
+  IN UINT16                     Port,
+  IN UINT16                     PortMultiplierPort
+  );
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  );
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  );
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  );
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtScsiPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  );
+
+/**
+  Initialize ATA host controller at IDE mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  );
+
+/**
+  Initialize ATA host controller at AHCI mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  );
+
+/**
+  Start a non data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The non data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The non data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciNonDataTransfer (
+  IN     EFI_PCI_IO_PROTOCOL           *PciIo,
+  IN     EFI_AHCI_REGISTERS            *AhciRegisters,
+  IN     UINT8                         Port,
+  IN     UINT8                         PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND        *AtapiCommand OPTIONAL,
+  IN     UINT8                         AtapiCommandLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  );
+
+/**
+  Start a DMA data transfer on specific port
+
+  @param[in]       Instance            The ATA_ATAPI_PASS_THRU_INSTANCE protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The DMA data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The DMA data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciDmaTransfer (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE *Instance,
+  IN     EFI_AHCI_REGISTERS           *AhciRegisters,
+  IN     UINT8                        Port,
+  IN     UINT8                        PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND       *AtapiCommand OPTIONAL,
+  IN     UINT8                        AtapiCommandLength,
+  IN     BOOLEAN                      Read,
+  IN     EFI_ATA_COMMAND_BLOCK        *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK         *AtaStatusBlock,
+  IN OUT VOID                         *MemoryAddr,
+  IN     UINT32                       DataCount,
+  IN     UINT64                       Timeout,
+  IN     ATA_NONBLOCK_TASK            *Task
+  );
+
+/**
+  Start a PIO data transfer on specific port.
+
+  @param[in]       PciIo               The PCI IO protocol instance.
+  @param[in]       AhciRegisters       The pointer to the EFI_AHCI_REGISTERS.
+  @param[in]       Port                The number of port.
+  @param[in]       PortMultiplier      The timeout value of stop.
+  @param[in]       AtapiCommand        The atapi command will be used for the
+                                       transfer.
+  @param[in]       AtapiCommandLength  The length of the atapi command.
+  @param[in]       Read                The transfer direction.
+  @param[in]       AtaCommandBlock     The EFI_ATA_COMMAND_BLOCK data.
+  @param[in, out]  AtaStatusBlock      The EFI_ATA_STATUS_BLOCK data.
+  @param[in, out]  MemoryAddr          The pointer to the data buffer.
+  @param[in]       DataCount           The data count to be transferred.
+  @param[in]       Timeout             The timeout value of non data transfer, uses 100ns as a unit.
+  @param[in]       Task                Optional. Pointer to the ATA_NONBLOCK_TASK
+                                       used by non-blocking mode.
+
+  @retval EFI_DEVICE_ERROR    The PIO data transfer abort with error occurs.
+  @retval EFI_TIMEOUT         The operation is time out.
+  @retval EFI_UNSUPPORTED     The device is not ready for transfer.
+  @retval EFI_SUCCESS         The PIO data transfer executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AhciPioTransfer (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     EFI_AHCI_REGISTERS         *AhciRegisters,
+  IN     UINT8                      Port,
+  IN     UINT8                      PortMultiplier,
+  IN     EFI_AHCI_ATAPI_COMMAND     *AtapiCommand OPTIONAL,
+  IN     UINT8                      AtapiCommandLength,
+  IN     BOOLEAN                    Read,
+  IN     EFI_ATA_COMMAND_BLOCK      *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK       *AtaStatusBlock,
+  IN OUT VOID                       *MemoryAddr,
+  IN     UINT32                     DataCount,
+  IN     UINT64                     Timeout,
+  IN     ATA_NONBLOCK_TASK          *Task
+  );
+
+/**
+  Send ATA command into device with NON_DATA protocol
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE
+                                   data structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data
+                                   structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_ABORTED Command failed
+  @retval  EFI_DEVICE_ERROR Device status error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaNonDataCommandIn (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  );
+
+/**
+  Perform an ATA Udma operation (Read, ReadExt, Write, WriteExt).
+
+  @param[in]      Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      Read             Flag used to determine the data transfer
+                                   direction. Read equals 1, means data transferred
+                                   from device to host;Read equals 0, means data
+                                   transferred from host to device.
+  @param[in]      DataBuffer       A pointer to the source buffer for the data.
+  @param[in]      DataLength       The length of  the data.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS          the operation is successful.
+  @retval EFI_OUT_OF_RESOURCES Build PRD table failed
+  @retval EFI_UNSUPPORTED      Unknown channel or operations command
+  @retval EFI_DEVICE_ERROR     Ata command execute failed
+
+**/
+EFI_STATUS
+EFIAPI
+AtaUdmaInOut (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     EFI_IDE_REGISTERS             *IdeRegisters,
+  IN     BOOLEAN                       Read,
+  IN     VOID                          *DataBuffer,
+  IN     UINT64                        DataLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  );
+
+/**
+  This function is used to send out ATA commands conforms to the PIO Data In Protocol.
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in, out] Buffer           A pointer to the source buffer for the data.
+  @param[in]      ByteCount        The length of  the data.
+  @param[in] Read                  Flag used to determine the data transfer direction.
+                                   Read equals 1, means data transferred from device
+                                   to host;Read equals 0, means data transferred
+                                   from host to device.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS      send out the ATA command and device send required data successfully.
+  @retval EFI_DEVICE_ERROR command sent failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPioDataInOut (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf
new file mode 100644
index 0000000000..82d5f7a46c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf
@@ -0,0 +1,78 @@
+## @file
+#  AtaAtapiPassThru driver to provide native IDE/AHCI mode support.
+#
+#  This driver installs AtaPassThru and ExtScsiPassThru protocol in each ide/sata controller
+#  to access to all attached Ata/Atapi devices.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AtaAtapiPassThruDxe
+  MODULE_UNI_FILE                = AtaAtapiPassThruDxe.uni
+  FILE_GUID                      = 5E523CB4-D397-4986-87BD-A6DD8B22F455
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeAtaAtapiPassThru
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gAtaAtapiPassThruDriverBinding                         
+#  COMPONENT_NAME                =  gAtaAtapiPassThruComponentName
+#  COMPONENT_NAME2               =  gAtaAtapiPassThruComponentName2                         
+#
+#
+
+[Sources]
+  AtaAtapiPassThru.c
+  AtaAtapiPassThru.h
+  AhciMode.c
+  AhciMode.h
+  IdeMode.c
+  IdeMode.h
+  ComponentName.c
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  TimerLib
+  ReportStatusCodeLib
+  PcdLib
+
+[Protocols]
+  gEfiAtaPassThruProtocolGuid                   ## BY_START
+  gEfiExtScsiPassThruProtocolGuid               ## BY_START
+  gEfiIdeControllerInitProtocolGuid             ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiPciIoProtocolGuid                         ## TO_START
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAtaSmartEnable   ## SOMETIMES_CONSUMES
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AtaAtapiPassThruDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni
new file mode 100644
index 0000000000..0285409ec5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni
new file mode 100644
index 0000000000..d89350dfe3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThruDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c
new file mode 100644
index 0000000000..c3f7738d45
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/ComponentName.c
@@ -0,0 +1,251 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for AtaAtapiPassThru driver.
+  
+  Copyright (c) 2010 - 2011, 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 "AtaAtapiPassThru.h"
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruDriverNameTable[] = {
+  { "eng;en", L"AtaAtapiPassThru Driver" },
+  { NULL , NULL }
+};
+
+//
+// Controller name table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruIdeControllerNameTable[] = {
+  { "eng;en", L"IDE Controller" },
+  { NULL , NULL }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaAtapiPassThruAhciControllerNameTable[] = {
+  { "eng;en", L"AHCI Controller" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gAtaAtapiPassThruComponentName = {
+  AtaAtapiPassThruComponentNameGetDriverName,
+  AtaAtapiPassThruComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gAtaAtapiPassThruComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) AtaAtapiPassThruComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) AtaAtapiPassThruComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mAtaAtapiPassThruDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gAtaAtapiPassThruComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaAtapiPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+  VOID                          *Interface;
+  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gAtaAtapiPassThruDriverBinding.DriverBindingHandle,
+             &gEfiIdeControllerInitProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // AtaPassThru and ExtScsiPassThru should also be installed at the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiAtaPassThruProtocolGuid,
+                  &Interface,
+                  gAtaAtapiPassThruDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = ATA_PASS_THRU_PRIVATE_DATA_FROM_THIS (Interface);
+
+  if (Instance->Mode == EfiAtaIdeMode) {
+    ControllerNameTable = mAtaAtapiPassThruIdeControllerNameTable;
+  } else if (Instance->Mode == EfiAtaAhciMode) {
+    ControllerNameTable = mAtaAtapiPassThruAhciControllerNameTable;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gAtaAtapiPassThruComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c
new file mode 100644
index 0000000000..420ad273dc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.c
@@ -0,0 +1,2890 @@
+/** @file
+  Header file for AHCI mode of ATA host controller.
+
+  Copyright (c) 2010 - 2015, 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 "AtaAtapiPassThru.h"
+
+/**
+  read a one-byte data from a IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE Port number
+
+  @return  the one-byte data read from IDE port
+**/
+UINT8
+EFIAPI
+IdeReadPortB (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port
+  )
+{
+  UINT8 Data;
+
+  ASSERT (PciIo != NULL);
+
+  Data = 0;
+  //
+  // perform 1-byte data read from register
+  //
+  PciIo->Io.Read (
+              PciIo,
+              EfiPciIoWidthUint8,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+  return Data;
+}
+
+/**
+  write a 1-byte data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortB (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT8                 Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 1-byte data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint8,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  write a 1-word data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortW (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT16                Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 1-word data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  write a 2-word data to a specific IDE port.
+
+  @param  PciIo  A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port   The IDE port to be writen
+  @param  Data   The data to write to the port
+**/
+VOID
+EFIAPI
+IdeWritePortDW (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINT32                Data
+  )
+{
+  ASSERT (PciIo != NULL);
+
+  //
+  // perform 2-word data write to register
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthUint32,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              1,
+              &Data
+              );
+}
+
+/**
+  Write multiple words of data to the IDE data port.
+  Call the IO abstraction once to do the complete read,
+  not one word at a time
+
+  @param  PciIo      A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port       IO port to read
+  @param  Count      No. of UINT16's to read
+  @param  Buffer     Pointer to the data buffer for read
+
+**/
+VOID
+EFIAPI
+IdeWritePortWMultiple (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINTN                 Count,
+  IN  VOID                  *Buffer
+  )
+{
+  ASSERT (PciIo  != NULL);
+  ASSERT (Buffer != NULL);
+
+  //
+  // perform UINT16 data write to the FIFO
+  //
+  PciIo->Io.Write (
+              PciIo,
+              EfiPciIoWidthFifoUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              Count,
+              (UINT16 *) Buffer
+              );
+
+}
+
+/**
+  Reads multiple words of data from the IDE data port.
+  Call the IO abstraction once to do the complete read,
+  not one word at a time
+
+  @param  PciIo    A pointer to EFI_PCI_IO_PROTOCOL data structure
+  @param  Port     IO port to read
+  @param  Count    Number of UINT16's to read
+  @param  Buffer   Pointer to the data buffer for read
+
+**/
+VOID
+EFIAPI
+IdeReadPortWMultiple (
+  IN  EFI_PCI_IO_PROTOCOL   *PciIo,
+  IN  UINT16                Port,
+  IN  UINTN                 Count,
+  IN  VOID                  *Buffer
+  )
+{
+  ASSERT (PciIo  != NULL);
+  ASSERT (Buffer != NULL);
+
+  //
+  // Perform UINT16 data read from FIFO
+  //
+  PciIo->Io.Read (
+              PciIo,
+              EfiPciIoWidthFifoUint16,
+              EFI_PCI_IO_PASS_THROUGH_BAR,
+              (UINT64) Port,
+              Count,
+              (UINT16 *) Buffer
+              );
+
+}
+
+/**
+  This function is used to analyze the Status Register and print out
+  some debug information and if there is ERR bit set in the Status
+  Register, the Error Register's value is also be parsed and print out.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+DumpAllIdeRegisters (
+  IN     EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN     EFI_IDE_REGISTERS        *IdeRegisters,
+  IN OUT EFI_ATA_STATUS_BLOCK     *AtaStatusBlock
+  )
+{
+  EFI_ATA_STATUS_BLOCK StatusBlock;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  ZeroMem (&StatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+
+  StatusBlock.AtaStatus          = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+  StatusBlock.AtaError           = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+  StatusBlock.AtaSectorCount     = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+  StatusBlock.AtaSectorCountExp  = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+  StatusBlock.AtaSectorNumber    = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+  StatusBlock.AtaSectorNumberExp = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+  StatusBlock.AtaCylinderLow     = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+  StatusBlock.AtaCylinderLowExp  = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+  StatusBlock.AtaCylinderHigh    = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+  StatusBlock.AtaCylinderHighExp = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+  StatusBlock.AtaDeviceHead      = IdeReadPortB (PciIo, IdeRegisters->Head);
+
+  if (AtaStatusBlock != NULL) {
+    //
+    // Dump the content of all ATA registers.
+    //
+    CopyMem (AtaStatusBlock, &StatusBlock, sizeof (EFI_ATA_STATUS_BLOCK));
+  }
+
+  DEBUG_CODE_BEGIN ();
+  if ((StatusBlock.AtaStatus & ATA_STSREG_DWF) != 0) {
+    DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Write Fault\n", StatusBlock.AtaStatus));
+  }
+
+  if ((StatusBlock.AtaStatus & ATA_STSREG_CORR) != 0) {
+    DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Corrected Data\n", StatusBlock.AtaStatus));
+  }
+
+  if ((StatusBlock.AtaStatus & ATA_STSREG_ERR) != 0) {
+    if ((StatusBlock.AtaError & ATA_ERRREG_BBK) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Bad Block Detected\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_UNC) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Uncorrectable Data\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_MC) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Media Change\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_ABRT) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Abort\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_TK0NF) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Track 0 Not Found\n", StatusBlock.AtaError));
+    }
+
+    if ((StatusBlock.AtaError & ATA_ERRREG_AMNF) != 0) {
+      DEBUG ((EFI_D_ERROR, "CheckRegisterStatus()-- %02x : Error : Address Mark Not Found\n", StatusBlock.AtaError));
+    }
+  }
+  DEBUG_CODE_END ();
+}
+
+/**
+  This function is used to analyze the Status Register at the condition that BSY is zero.
+  if there is ERR bit set in the Status Register, then return error.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_SUCCESS       No err information in the Status Register.
+  @retval EFI_DEVICE_ERROR  Any err information in the Status Register.
+
+**/
+EFI_STATUS
+EFIAPI
+CheckStatusRegister (
+  IN  EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN  EFI_IDE_REGISTERS        *IdeRegisters
+  )
+{
+  UINT8           StatusRegister;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+  if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+    if ((StatusRegister & (ATA_STSREG_ERR | ATA_STSREG_DWF | ATA_STSREG_CORR)) == 0) {
+      return EFI_SUCCESS;
+    } else {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to poll for the DRQ bit clear in the Status
+  Register. DRQ is cleared when the device is finished transferring data.
+  So this function is called after data transfer is finished.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS     DRQ bit clear within the time out.
+
+  @retval EFI_TIMEOUT     DRQ bit not clear within the time out.
+
+  @note
+  Read Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQClear (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+/**
+  This function is used to poll for the DRQ bit clear in the Alternate
+  Status Register. DRQ is cleared when the device is finished
+  transferring data. So this function is called after data transfer
+  is finished.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS     DRQ bit clear within the time out.
+
+  @retval EFI_TIMEOUT     DRQ bit not clear within the time out.
+  @note   Read Alternate Status Register will not clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQClear2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRQ bit set in the
+  Status Register.
+  DRQ is set when the device is ready to transfer data. So this function
+  is called after the command is sent to the device and before required
+  data is transferred.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          DRQ bit set within the time out.
+  @retval EFI_TIMEOUT          DRQ bit not set within the time out.
+  @retval EFI_ABORTED          DRQ bit not set caused by the command abort.
+
+  @note  Read Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQReady (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    //
+    // Read Status Register will clear interrupt
+    //
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear or ERR is set
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((StatusRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+/**
+  This function is used to poll for the DRQ bit set in the Alternate Status Register.
+  DRQ is set when the device is ready to transfer data. So this function is called after
+  the command is sent to the device and before required data is transferred.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS           DRQ bit set within the time out.
+  @retval EFI_TIMEOUT           DRQ bit not set within the time out.
+  @retval EFI_ABORTED           DRQ bit not set caused by the command abort.
+  @note  Read Alternate Status Register will not clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRQReady2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+
+  do {
+    //
+    // Read Alternate Status Register will not clear interrupt status
+    //
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    //
+    // Wait for BSY == 0, then judge if DRQ is clear or ERR is set
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((AltRegister & ATA_STSREG_DRQ) == ATA_STSREG_DRQ) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRDY bit set in the Status Register. DRDY
+  bit is set when the device is ready to accept command. Most ATA commands must be
+  sent after DRDY set except the ATAPI Packet Command.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS         DRDY bit set within the time out.
+  @retval EFI_TIMEOUT         DRDY bit not set within the time out.
+
+  @note  Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+DRDYReady (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+    //
+    // Wait for BSY == 0, then judge if DRDY is set or ERR is set
+    //
+    if ((StatusRegister & ATA_STSREG_BSY) == 0) {
+      if ((StatusRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((StatusRegister & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the DRDY bit set in the Alternate Status Register.
+  DRDY bit is set when the device is ready to accept command. Most ATA commands must
+  be sent after DRDY set except the ATAPI Packet Command.
+
+  @param PciIo            A pointer to EFI_PCI_IO_PROTOCOL data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS      DRDY bit set within the time out.
+  @retval EFI_TIMEOUT      DRDY bit not set within the time out.
+
+  @note  Read Alternate Status Register will clear interrupt status.
+
+**/
+EFI_STATUS
+EFIAPI
+DRDYReady2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltRegister;
+  UINT8   ErrorRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    //
+    // Wait for BSY == 0, then judge if DRDY is set or ERR is set
+    //
+    if ((AltRegister & ATA_STSREG_BSY) == 0) {
+      if ((AltRegister & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+        ErrorRegister = IdeReadPortB (PciIo, IdeRegisters->ErrOrFeature);
+
+        if ((ErrorRegister & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+          return EFI_ABORTED;
+        }
+        return EFI_DEVICE_ERROR;
+      }
+
+      if ((AltRegister & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the BSY bit clear in the Status Register. BSY
+  is clear when the device is not busy. Every command must be sent after device is not busy.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          BSY bit clear within the time out.
+  @retval EFI_TIMEOUT          BSY bit not clear within the time out.
+
+  @note Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+WaitForBSYClear (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   StatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    StatusRegister = IdeReadPortB (PciIo, IdeRegisters->CmdOrStatus);
+
+    if ((StatusRegister & ATA_STSREG_BSY) == 0x00) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  This function is used to poll for the BSY bit clear in the Status Register. BSY
+  is clear when the device is not busy. Every command must be sent after device is not busy.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS          BSY bit clear within the time out.
+  @retval EFI_TIMEOUT          BSY bit not clear within the time out.
+
+  @note Read Status Register will clear interrupt status.
+**/
+EFI_STATUS
+EFIAPI
+WaitForBSYClear2 (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT64  Delay;
+  UINT8   AltStatusRegister;
+  BOOLEAN InfiniteWait;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32(Timeout, 1000) + 1;
+  do {
+    AltStatusRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+
+    if ((AltStatusRegister & ATA_STSREG_BSY) == 0x00) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Get IDE i/o port registers' base addresses by mode.
+
+  In 'Compatibility' mode, use fixed addresses.
+  In Native-PCI mode, get base addresses from BARs in the PCI IDE controller's
+  Configuration Space.
+
+  The steps to get IDE i/o port registers' base addresses for each channel
+  as follows:
+
+  1. Examine the Programming Interface byte of the Class Code fields in PCI IDE
+  controller's Configuration Space to determine the operating mode.
+
+  2. a) In 'Compatibility' mode, use fixed addresses shown in the Table 1 below.
+   ___________________________________________
+  |           | Command Block | Control Block |
+  |  Channel  |   Registers   |   Registers   |
+  |___________|_______________|_______________|
+  |  Primary  |  1F0h - 1F7h  |  3F6h - 3F7h  |
+  |___________|_______________|_______________|
+  | Secondary |  170h - 177h  |  376h - 377h  |
+  |___________|_______________|_______________|
+
+  Table 1. Compatibility resource mappings
+
+  b) In Native-PCI mode, IDE registers are mapped into IO space using the BARs
+  in IDE controller's PCI Configuration Space, shown in the Table 2 below.
+   ___________________________________________________
+  |           |   Command Block   |   Control Block   |
+  |  Channel  |     Registers     |     Registers     |
+  |___________|___________________|___________________|
+  |  Primary  | BAR at offset 0x10| BAR at offset 0x14|
+  |___________|___________________|___________________|
+  | Secondary | BAR at offset 0x18| BAR at offset 0x1C|
+  |___________|___________________|___________________|
+
+  Table 2. BARs for Register Mapping
+
+  @param[in]      PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in, out] IdeRegisters    Pointer to EFI_IDE_REGISTERS which is used to
+                                 store the IDE i/o port registers' base addresses
+
+  @retval EFI_UNSUPPORTED        Return this value when the BARs is not IO type
+  @retval EFI_SUCCESS            Get the Base address successfully
+  @retval Other                  Read the pci configureation data error
+
+**/
+EFI_STATUS
+EFIAPI
+GetIdeRegisterIoAddr (
+  IN     EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN OUT EFI_IDE_REGISTERS           *IdeRegisters
+  )
+{
+  EFI_STATUS        Status;
+  PCI_TYPE00        PciData;
+  UINT16            CommandBlockBaseAddr;
+  UINT16            ControlBlockBaseAddr;
+  UINT16            BusMasterBaseAddr;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        0,
+                        sizeof (PciData),
+                        &PciData
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  BusMasterBaseAddr    = (UINT16) ((PciData.Device.Bar[4] & 0x0000fff0));
+
+  if ((PciData.Hdr.ClassCode[0] & IDE_PRIMARY_OPERATING_MODE) == 0) {
+    CommandBlockBaseAddr = 0x1f0;
+    ControlBlockBaseAddr = 0x3f6;
+  } else {
+    //
+    // The BARs should be of IO type
+    //
+    if ((PciData.Device.Bar[0] & BIT0) == 0 ||
+        (PciData.Device.Bar[1] & BIT0) == 0) {
+      return EFI_UNSUPPORTED;
+    }
+
+    CommandBlockBaseAddr = (UINT16) (PciData.Device.Bar[0] & 0x0000fff8);
+    ControlBlockBaseAddr = (UINT16) ((PciData.Device.Bar[1] & 0x0000fffc) + 2);
+  }
+
+  //
+  // Calculate IDE primary channel I/O register base address.
+  //
+  IdeRegisters[EfiIdePrimary].Data              = CommandBlockBaseAddr;
+  IdeRegisters[EfiIdePrimary].ErrOrFeature      = (UINT16) (CommandBlockBaseAddr + 0x01);
+  IdeRegisters[EfiIdePrimary].SectorCount       = (UINT16) (CommandBlockBaseAddr + 0x02);
+  IdeRegisters[EfiIdePrimary].SectorNumber      = (UINT16) (CommandBlockBaseAddr + 0x03);
+  IdeRegisters[EfiIdePrimary].CylinderLsb       = (UINT16) (CommandBlockBaseAddr + 0x04);
+  IdeRegisters[EfiIdePrimary].CylinderMsb       = (UINT16) (CommandBlockBaseAddr + 0x05);
+  IdeRegisters[EfiIdePrimary].Head              = (UINT16) (CommandBlockBaseAddr + 0x06);
+  IdeRegisters[EfiIdePrimary].CmdOrStatus       = (UINT16) (CommandBlockBaseAddr + 0x07);
+  IdeRegisters[EfiIdePrimary].AltOrDev          = ControlBlockBaseAddr;
+  IdeRegisters[EfiIdePrimary].BusMasterBaseAddr = BusMasterBaseAddr;
+
+  if ((PciData.Hdr.ClassCode[0] & IDE_SECONDARY_OPERATING_MODE) == 0) {
+    CommandBlockBaseAddr = 0x170;
+    ControlBlockBaseAddr = 0x376;
+  } else {
+    //
+    // The BARs should be of IO type
+    //
+    if ((PciData.Device.Bar[2] & BIT0) == 0 ||
+        (PciData.Device.Bar[3] & BIT0) == 0) {
+      return EFI_UNSUPPORTED;
+    }
+
+    CommandBlockBaseAddr = (UINT16) (PciData.Device.Bar[2] & 0x0000fff8);
+    ControlBlockBaseAddr = (UINT16) ((PciData.Device.Bar[3] & 0x0000fffc) + 2);
+  }
+
+  //
+  // Calculate IDE secondary channel I/O register base address.
+  //
+  IdeRegisters[EfiIdeSecondary].Data              = CommandBlockBaseAddr;
+  IdeRegisters[EfiIdeSecondary].ErrOrFeature      = (UINT16) (CommandBlockBaseAddr + 0x01);
+  IdeRegisters[EfiIdeSecondary].SectorCount       = (UINT16) (CommandBlockBaseAddr + 0x02);
+  IdeRegisters[EfiIdeSecondary].SectorNumber      = (UINT16) (CommandBlockBaseAddr + 0x03);
+  IdeRegisters[EfiIdeSecondary].CylinderLsb       = (UINT16) (CommandBlockBaseAddr + 0x04);
+  IdeRegisters[EfiIdeSecondary].CylinderMsb       = (UINT16) (CommandBlockBaseAddr + 0x05);
+  IdeRegisters[EfiIdeSecondary].Head              = (UINT16) (CommandBlockBaseAddr + 0x06);
+  IdeRegisters[EfiIdeSecondary].CmdOrStatus       = (UINT16) (CommandBlockBaseAddr + 0x07);
+  IdeRegisters[EfiIdeSecondary].AltOrDev          = ControlBlockBaseAddr;
+  IdeRegisters[EfiIdeSecondary].BusMasterBaseAddr = (UINT16) (BusMasterBaseAddr + 0x8);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to implement the Soft Reset on the specified device. But,
+  the ATA Soft Reset mechanism is so strong a reset method that it will force
+  resetting on both devices connected to the same cable.
+
+  It is called by IdeBlkIoReset(), a interface function of Block
+  I/O protocol.
+
+  This function can also be used by the ATAPI device to perform reset when
+  ATAPI Reset command is failed.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_SUCCESS       Soft reset completes successfully.
+  @retval EFI_DEVICE_ERROR  Any step during the reset process is failed.
+
+  @note  The registers initial values after ATA soft reset are different
+         to the ATA device and ATAPI device.
+**/
+EFI_STATUS
+EFIAPI
+AtaSoftReset (
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN  EFI_IDE_REGISTERS    *IdeRegisters,
+  IN  UINT64               Timeout
+  )
+{
+  UINT8 DeviceControl;
+
+  DeviceControl = 0;
+  //
+  // disable Interrupt and set SRST bit to initiate soft reset
+  //
+  DeviceControl = ATA_CTLREG_SRST | ATA_CTLREG_IEN_L;
+
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+  //
+  // SRST should assert for at least 5 us, we use 10 us for
+  // better compatibility
+  //
+  MicroSecondDelay (10);
+
+  //
+  // Enable interrupt to support UDMA, and clear SRST bit
+  //
+  DeviceControl = 0;
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+  //
+  // Wait for at least 10 ms to check BSY status, we use 10 ms
+  // for better compatibility
+  //
+  MicroSecondDelay (10000);
+
+  //
+  // slave device needs at most 31ms to clear BSY
+  //
+  if (WaitForBSYClear (PciIo, IdeRegisters, Timeout) == EFI_TIMEOUT) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send ATA Ext command into device with NON_DATA protocol.
+
+  @param PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param Timeout          The time to complete the command, uses 100ns as a unit.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_DEVICE_ERROR Error executing commands on this device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIssueCommand (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN  UINT64                    Timeout
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       DeviceHead;
+  UINT8       AtaCommand;
+
+  ASSERT (PciIo != NULL);
+  ASSERT (IdeRegisters != NULL);
+  ASSERT (AtaCommandBlock != NULL);
+
+  DeviceHead = AtaCommandBlock->AtaDeviceHead;
+  AtaCommand = AtaCommandBlock->AtaCommand;
+
+  Status = WaitForBSYClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Select device (bit4), set LBA mode(bit6) (use 0xe0 for compatibility)
+  //
+  IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8) (0xe0 | DeviceHead));
+
+  //
+  // set all the command parameters
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  Status = DRQClear2 (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Fill the feature register, which is a two-byte FIFO. Need write twice.
+  //
+  IdeWritePortB (PciIo, IdeRegisters->ErrOrFeature, AtaCommandBlock->AtaFeaturesExp);
+  IdeWritePortB (PciIo, IdeRegisters->ErrOrFeature, AtaCommandBlock->AtaFeatures);
+
+  //
+  // Fill the sector count register, which is a two-byte FIFO. Need write twice.
+  //
+  IdeWritePortB (PciIo, IdeRegisters->SectorCount, AtaCommandBlock->AtaSectorCountExp);
+  IdeWritePortB (PciIo, IdeRegisters->SectorCount, AtaCommandBlock->AtaSectorCount);
+
+  //
+  // Fill the start LBA registers, which are also two-byte FIFO
+  //
+  IdeWritePortB (PciIo, IdeRegisters->SectorNumber, AtaCommandBlock->AtaSectorNumberExp);
+  IdeWritePortB (PciIo, IdeRegisters->SectorNumber, AtaCommandBlock->AtaSectorNumber);
+
+  IdeWritePortB (PciIo, IdeRegisters->CylinderLsb, AtaCommandBlock->AtaCylinderLowExp);
+  IdeWritePortB (PciIo, IdeRegisters->CylinderLsb, AtaCommandBlock->AtaCylinderLow);
+
+  IdeWritePortB (PciIo, IdeRegisters->CylinderMsb, AtaCommandBlock->AtaCylinderHighExp);
+  IdeWritePortB (PciIo, IdeRegisters->CylinderMsb, AtaCommandBlock->AtaCylinderHigh);
+
+  //
+  // Send command via Command Register
+  //
+  IdeWritePortB (PciIo, IdeRegisters->CmdOrStatus, AtaCommand);
+
+  //
+  // Stall at least 400 microseconds.
+  //
+  MicroSecondDelay (400);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to send out ATA commands conforms to the PIO Data In Protocol.
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in, out] Buffer           A pointer to the source buffer for the data.
+  @param[in]      ByteCount        The length of the data.
+  @param[in]      Read             Flag used to determine the data transfer direction.
+                                   Read equals 1, means data transferred from device
+                                   to host;Read equals 0, means data transferred
+                                   from host to device.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS      send out the ATA command and device send required data successfully.
+  @retval EFI_DEVICE_ERROR command sent failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPioDataInOut (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  )
+{
+  UINTN       WordCount;
+  UINTN       Increment;
+  UINT16      *Buffer16;
+  EFI_STATUS  Status;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (Buffer == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Issue ATA command
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Buffer16 = (UINT16 *) Buffer;
+
+  //
+  // According to PIO data in protocol, host can perform a series of reads to
+  // the data register after each time device set DRQ ready;
+  // The data size of "a series of read" is command specific.
+  // For most ATA command, data size received from device will not exceed
+  // 1 sector, hence the data size for "a series of read" can be the whole data
+  // size of one command request.
+  // For ATA command such as Read Sector command, the data size of one ATA
+  // command request is often larger than 1 sector, according to the
+  // Read Sector command, the data size of "a series of read" is exactly 1
+  // sector.
+  // Here for simplification reason, we specify the data size for
+  // "a series of read" to 1 sector (256 words) if data size of one ATA command
+  // request is larger than 256 words.
+  //
+  Increment = 256;
+
+  //
+  // used to record bytes of currently transfered data
+  //
+  WordCount = 0;
+
+  while (WordCount < RShiftU64(ByteCount, 1)) {
+    //
+    // Poll DRQ bit set, data transfer can be performed only when DRQ is ready
+    //
+    Status = DRQReady2 (PciIo, IdeRegisters, Timeout);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    //
+    // Get the byte count for one series of read
+    //
+    if ((WordCount + Increment) > RShiftU64(ByteCount, 1)) {
+      Increment = (UINTN)(RShiftU64(ByteCount, 1) - WordCount);
+    }
+
+    if (Read) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        Increment,
+        Buffer16
+        );
+    } else {
+      IdeWritePortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        Increment,
+        Buffer16
+        );
+    }
+
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    WordCount += Increment;
+    Buffer16  += Increment;
+  }
+
+  Status = DRQClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+Exit:
+  //
+  // Dump All Ide registers to ATA_STATUS_BLOCK
+  //
+  DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+
+  //
+  // Not support the Non-blocking now,just do the blocking process.
+  //
+  return Status;
+}
+
+/**
+  Send ATA command into device with NON_DATA protocol
+
+  @param[in]      PciIo            A pointer to ATA_ATAPI_PASS_THRU_INSTANCE
+                                   data structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data
+                                   structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval  EFI_SUCCESS Reading succeed
+  @retval  EFI_ABORTED Command failed
+  @retval  EFI_DEVICE_ERROR Device status error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaNonDataCommandIn (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN     EFI_ATA_COMMAND_BLOCK     *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK      *AtaStatusBlock,
+  IN     UINT64                    Timeout,
+  IN     ATA_NONBLOCK_TASK         *Task
+  )
+{
+  EFI_STATUS  Status;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Issue ATA command
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  //
+  // Wait for command completion
+  //
+  Status = WaitForBSYClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+Exit:
+  //
+  // Dump All Ide registers to ATA_STATUS_BLOCK
+  //
+  DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+
+  //
+  // Not support the Non-blocking now,just do the blocking process.
+  //
+  return Status;
+}
+
+/**
+  Wait for memory to be set.
+
+  @param[in]  PciIo           The PCI IO protocol instance.
+  @param[in]  IdeRegisters    A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]  Timeout         The time to complete the command, uses 100ns as a unit.
+
+  @retval EFI_DEVICE_ERROR  The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting is time out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+AtaUdmStatusWait (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters,
+  IN  UINT64                    Timeout
+ )
+{
+  UINT8                         RegisterValue;
+  EFI_STATUS                    Status;
+  UINT16                        IoPortForBmis;
+  UINT64                        Delay;
+  BOOLEAN                       InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 1000) + 1;
+
+  do {
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+    RegisterValue = IdeReadPortB (PciIo, IoPortForBmis);
+    if (((RegisterValue & BMIS_ERROR) != 0) || (Timeout == 0)) {
+      DEBUG ((EFI_D_ERROR, "ATA UDMA operation fails\n"));
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    if ((RegisterValue & BMIS_INTERRUPT) != 0) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+    //
+    // Stall for 100 microseconds.
+    //
+    MicroSecondDelay (100);
+    Delay--;
+  } while (InfiniteWait || (Delay > 0));
+
+  return Status;
+}
+
+/**
+  Check if the memory to be set.
+
+  @param[in]  PciIo           The PCI IO protocol instance.
+  @param[in]  Task            Optional. Pointer to the ATA_NONBLOCK_TASK
+                              used by non-blocking mode.
+  @param[in]  IdeRegisters    A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_DEVICE_ERROR  The memory setting met a issue.
+  @retval EFI_NOT_READY     The memory is not set.
+  @retval EFI_TIMEOUT       The memory setting is time out.
+  @retval EFI_SUCCESS       The memory is correct set.
+
+**/
+EFI_STATUS
+AtaUdmStatusCheck (
+  IN     EFI_PCI_IO_PROTOCOL        *PciIo,
+  IN     ATA_NONBLOCK_TASK          *Task,
+  IN     EFI_IDE_REGISTERS          *IdeRegisters
+ )
+{
+  UINT8          RegisterValue;
+  UINT16         IoPortForBmis;
+  EFI_STATUS     Status;
+
+  Task->RetryTimes--;
+
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+  RegisterValue = IdeReadPortB (PciIo, IoPortForBmis);
+
+  if ((RegisterValue & BMIS_ERROR) != 0) {
+    DEBUG ((EFI_D_ERROR, "ATA UDMA operation fails\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((RegisterValue & BMIS_INTERRUPT) != 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (!Task->InfiniteWait && (Task->RetryTimes == 0)) {
+    return EFI_TIMEOUT;
+  } else {
+    //
+    // The memory is not set.
+    //
+    return EFI_NOT_READY;
+  }
+}
+
+/**
+  Perform an ATA Udma operation (Read, ReadExt, Write, WriteExt).
+
+  @param[in]      Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data
+                                   structure.
+  @param[in]      IdeRegisters     A pointer to EFI_IDE_REGISTERS data structure.
+  @param[in]      Read             Flag used to determine the data transfer
+                                   direction. Read equals 1, means data transferred
+                                   from device to host;Read equals 0, means data
+                                   transferred from host to device.
+  @param[in]      DataBuffer       A pointer to the source buffer for the data.
+  @param[in]      DataLength       The length of  the data.
+  @param[in]      AtaCommandBlock  A pointer to EFI_ATA_COMMAND_BLOCK data structure.
+  @param[in, out] AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+  @param[in]      Timeout          The time to complete the command, uses 100ns as a unit.
+  @param[in]      Task             Optional. Pointer to the ATA_NONBLOCK_TASK
+                                   used by non-blocking mode.
+
+  @retval EFI_SUCCESS          the operation is successful.
+  @retval EFI_OUT_OF_RESOURCES Build PRD table failed
+  @retval EFI_UNSUPPORTED      Unknown channel or operations command
+  @retval EFI_DEVICE_ERROR     Ata command execute failed
+
+**/
+EFI_STATUS
+EFIAPI
+AtaUdmaInOut (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     EFI_IDE_REGISTERS             *IdeRegisters,
+  IN     BOOLEAN                       Read,
+  IN     VOID                          *DataBuffer,
+  IN     UINT64                        DataLength,
+  IN     EFI_ATA_COMMAND_BLOCK         *AtaCommandBlock,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock,
+  IN     UINT64                        Timeout,
+  IN     ATA_NONBLOCK_TASK             *Task
+  )
+{
+  EFI_STATUS                    Status;
+  UINT16                        IoPortForBmic;
+  UINT16                        IoPortForBmis;
+  UINT16                        IoPortForBmid;
+
+  UINTN                         PrdTableSize;
+  EFI_PHYSICAL_ADDRESS          PrdTableMapAddr;
+  VOID                          *PrdTableMap;
+  EFI_PHYSICAL_ADDRESS          PrdTableBaseAddr;
+  EFI_ATA_DMA_PRD               *TempPrdBaseAddr;
+  UINTN                         PrdTableNum;
+
+  UINT8                         RegisterValue;
+  UINTN                         PageCount;
+  UINTN                         ByteCount;
+  UINTN                         ByteRemaining;
+  UINT8                         DeviceControl;
+
+  VOID                          *BufferMap;
+  EFI_PHYSICAL_ADDRESS          BufferMapAddress;
+  EFI_PCI_IO_PROTOCOL_OPERATION PciIoOperation;
+
+  UINT8                         DeviceHead;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_TPL                       OldTpl;
+
+  UINTN                         AlignmentMask;
+  UINTN                         RealPageCount;
+  EFI_PHYSICAL_ADDRESS          BaseAddr;
+  EFI_PHYSICAL_ADDRESS          BaseMapAddr;
+
+  Status        = EFI_SUCCESS;
+  PrdTableMap   = NULL;
+  BufferMap     = NULL;
+  PageCount     = 0;
+  RealPageCount = 0;
+  BaseAddr      = 0;
+  PciIo         = Instance->PciIo;
+
+  if ((PciIo == NULL) || (IdeRegisters == NULL) || (DataBuffer == NULL) || (AtaCommandBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Before starting the Blocking BlockIO operation, push to finish all non-blocking
+  // BlockIO tasks.
+  // Delay 1ms to simulate the blocking time out checking.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  while ((Task == NULL) && (!IsListEmpty (&Instance->NonBlockingTaskList))) {
+    AsyncNonBlockingTransferRoutine (NULL, Instance);
+    //
+    // Stall for 1 milliseconds.
+    //
+    MicroSecondDelay (1000);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  //
+  // The data buffer should be even alignment
+  //
+  if (((UINTN)DataBuffer & 0x1) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Set relevant IO Port address.
+  //
+  IoPortForBmic = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIC_OFFSET);
+  IoPortForBmis = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMIS_OFFSET);
+  IoPortForBmid = (UINT16) (IdeRegisters->BusMasterBaseAddr + BMID_OFFSET);
+
+  //
+  // For Blocking mode, start the command.
+  // For non-blocking mode, when the command is not started, start it, otherwise
+  // go to check the status.
+  //
+  if (((Task != NULL) && (!Task->IsStart)) || (Task == NULL)) {
+    //
+    // Calculate the number of PRD entry.
+    // Every entry in PRD table can specify a 64K memory region.
+    //
+    PrdTableNum   = (UINTN)(RShiftU64(DataLength, 16) + 1);
+
+    //
+    // Make sure that the memory region of PRD table is not cross 64K boundary
+    //
+    PrdTableSize = PrdTableNum * sizeof (EFI_ATA_DMA_PRD);
+    if (PrdTableSize > 0x10000) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Allocate buffer for PRD table initialization.
+    // Note Ide Bus Master spec said the descriptor table must be aligned on a 4 byte
+    // boundary and the table cannot cross a 64K boundary in memory.
+    //
+    PageCount     = EFI_SIZE_TO_PAGES (PrdTableSize);
+    RealPageCount = PageCount + EFI_SIZE_TO_PAGES (SIZE_64KB);
+
+    //
+    // Make sure that PageCount plus EFI_SIZE_TO_PAGES (SIZE_64KB) does not overflow.
+    //
+    ASSERT (RealPageCount > PageCount);
+
+    Status    = PciIo->AllocateBuffer (
+                         PciIo,
+                         AllocateAnyPages,
+                         EfiBootServicesData,
+                         RealPageCount,
+                         (VOID **)&BaseAddr,
+                         0
+                         );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ByteCount = EFI_PAGES_TO_SIZE (RealPageCount);
+    Status    = PciIo->Map (
+                         PciIo,
+                         EfiPciIoOperationBusMasterCommonBuffer,
+                         (VOID*)(UINTN)BaseAddr,
+                         &ByteCount,
+                         &BaseMapAddr,
+                         &PrdTableMap
+                         );
+    if (EFI_ERROR (Status) || (ByteCount != EFI_PAGES_TO_SIZE (RealPageCount))) {
+      //
+      // If the data length actually mapped is not equal to the requested amount,
+      // it means the DMA operation may be broken into several discontinuous smaller chunks.
+      // Can't handle this case.
+      //
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ZeroMem ((VOID *) ((UINTN) BaseAddr), ByteCount);
+
+    //
+    // Calculate the 64K align address as PRD Table base address.
+    //
+    AlignmentMask    = SIZE_64KB - 1;
+    PrdTableBaseAddr = ((UINTN) BaseAddr + AlignmentMask) & ~AlignmentMask;
+    PrdTableMapAddr  = ((UINTN) BaseMapAddr + AlignmentMask) & ~AlignmentMask;
+
+    //
+    // Map the host address of DataBuffer to DMA master address.
+    //
+    if (Read) {
+      PciIoOperation = EfiPciIoOperationBusMasterWrite;
+    } else {
+      PciIoOperation = EfiPciIoOperationBusMasterRead;
+    }
+
+    ByteCount = (UINTN)DataLength;
+    Status    = PciIo->Map (
+                         PciIo,
+                         PciIoOperation,
+                         DataBuffer,
+                         &ByteCount,
+                         &BufferMapAddress,
+                         &BufferMap
+                         );
+    if (EFI_ERROR (Status) || (ByteCount != DataLength)) {
+      PciIo->Unmap (PciIo, PrdTableMap);
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // According to Ata spec, it requires the buffer address and size to be even.
+    //
+    ASSERT ((BufferMapAddress & 0x1) == 0);
+    ASSERT ((ByteCount & 0x1) == 0);
+
+    //
+    // Fill the PRD table with appropriate bus master address of data buffer and data length.
+    //
+    ByteRemaining   = ByteCount;
+    TempPrdBaseAddr = (EFI_ATA_DMA_PRD*)(UINTN)PrdTableBaseAddr;
+    while (ByteRemaining != 0) {
+      if (ByteRemaining <= 0x10000) {
+        TempPrdBaseAddr->RegionBaseAddr = (UINT32) ((UINTN) BufferMapAddress);
+        TempPrdBaseAddr->ByteCount      = (UINT16) ByteRemaining;
+        TempPrdBaseAddr->EndOfTable     = 0x8000;
+        break;
+      }
+
+      TempPrdBaseAddr->RegionBaseAddr = (UINT32) ((UINTN) BufferMapAddress);
+      TempPrdBaseAddr->ByteCount      = (UINT16) 0x0;
+
+      ByteRemaining    -= 0x10000;
+      BufferMapAddress += 0x10000;
+      TempPrdBaseAddr++;
+    }
+
+    //
+    // Start to enable the DMA operation
+    //
+    DeviceHead = AtaCommandBlock->AtaDeviceHead;
+
+    IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)(0xe0 | DeviceHead));
+
+    //
+    // Enable interrupt to support UDMA
+    //
+    DeviceControl = 0;
+    IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+
+    //
+    // Read BMIS register and clear ERROR and INTR bit
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmis);
+    RegisterValue |= (BMIS_INTERRUPT | BMIS_ERROR);
+    IdeWritePortB (PciIo, IoPortForBmis, RegisterValue);
+
+    //
+    // Set the base address to BMID register
+    //
+    IdeWritePortDW (PciIo, IoPortForBmid, (UINT32)PrdTableMapAddr);
+
+    //
+    // Set BMIC register to identify the operation direction
+    //
+    RegisterValue = IdeReadPortB(PciIo, IoPortForBmic);
+    if (Read) {
+      RegisterValue |= BMIC_NREAD;
+    } else {
+      RegisterValue &= ~((UINT8) BMIC_NREAD);
+    }
+    IdeWritePortB (PciIo, IoPortForBmic, RegisterValue);
+
+    if (Task != NULL) {
+      Task->Map            = BufferMap;
+      Task->TableMap       = PrdTableMap;
+      Task->MapBaseAddress = (EFI_ATA_DMA_PRD*)(UINTN)BaseAddr;
+      Task->PageCount      = RealPageCount;
+      Task->IsStart        = TRUE;
+    }
+
+    //
+    // Issue ATA command
+    //
+    Status = AtaIssueCommand (PciIo, IdeRegisters, AtaCommandBlock, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+    //
+    // Set START bit of BMIC register
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmic);
+    RegisterValue |= BMIC_START;
+    IdeWritePortB(PciIo, IoPortForBmic, RegisterValue);
+
+  }
+
+  //
+  // Check the INTERRUPT and ERROR bit of BMIS
+  //
+  if (Task != NULL) {
+    Status = AtaUdmStatusCheck (PciIo, Task, IdeRegisters);
+  } else {
+    Status = AtaUdmStatusWait (PciIo, IdeRegisters, Timeout);
+  }
+
+  //
+  // For blocking mode, clear registers and free buffers.
+  // For non blocking mode, when the related registers have been set or time
+  // out, or a error has been happened, it needs to clear the register and free
+  // buffer.
+  //
+  if ((Task == NULL) || Status != EFI_NOT_READY) {
+    //
+    // Read BMIS register and clear ERROR and INTR bit
+    //
+    RegisterValue  = IdeReadPortB (PciIo, IoPortForBmis);
+    RegisterValue |= (BMIS_INTERRUPT | BMIS_ERROR);
+    IdeWritePortB (PciIo, IoPortForBmis, RegisterValue);
+
+    //
+    // Read Status Register of IDE device to clear interrupt
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IdeRegisters->CmdOrStatus);
+
+    //
+    // Clear START bit of BMIC register
+    //
+    RegisterValue  = IdeReadPortB(PciIo, IoPortForBmic);
+    RegisterValue &= ~((UINT8) BMIC_START);
+    IdeWritePortB (PciIo, IoPortForBmic, RegisterValue);
+
+    //
+    // Disable interrupt of Select device
+    //
+    DeviceControl  = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    DeviceControl |= ATA_CTLREG_IEN_L;
+    IdeWritePortB (PciIo, IdeRegisters->AltOrDev, DeviceControl);
+    //
+    // Stall for 10 milliseconds.
+    //
+    MicroSecondDelay (10000);
+
+  }
+
+Exit:
+  //
+  // Free all allocated resource
+  //
+  if ((Task == NULL) || Status != EFI_NOT_READY) {
+    if (Task != NULL) {
+      PciIo->Unmap (PciIo, Task->TableMap);
+      PciIo->FreeBuffer (PciIo, Task->PageCount, Task->MapBaseAddress);
+      PciIo->Unmap (PciIo, Task->Map);
+    } else {
+      PciIo->Unmap (PciIo, PrdTableMap);
+      PciIo->FreeBuffer (PciIo, RealPageCount, (VOID*)(UINTN)BaseAddr);
+      PciIo->Unmap (PciIo, BufferMap);
+    }
+
+    //
+    // Dump All Ide registers to ATA_STATUS_BLOCK
+    //
+    DumpAllIdeRegisters (PciIo, IdeRegisters, AtaStatusBlock);
+  }
+
+  return Status;
+}
+
+/**
+  This function reads the pending data in the device.
+
+  @param PciIo         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters  A pointer to EFI_IDE_REGISTERS data structure.
+
+  @retval EFI_SUCCESS   Successfully read.
+  @retval EFI_NOT_READY The BSY is set avoiding reading.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketReadPendingData (
+  IN  EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN  EFI_IDE_REGISTERS         *IdeRegisters
+  )
+{
+  UINT8     AltRegister;
+  UINT16    TempWordBuffer;
+
+  AltRegister = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+  if ((AltRegister & ATA_STSREG_BSY) == ATA_STSREG_BSY) {
+    return EFI_NOT_READY;
+  }
+
+  if ((AltRegister & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+    TempWordBuffer = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    while ((TempWordBuffer & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        1,
+        &TempWordBuffer
+        );
+      TempWordBuffer = IdeReadPortB (PciIo, IdeRegisters->AltOrDev);
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is called by AtaPacketCommandExecute().
+  It is used to transfer data between host and device. The data direction is specified
+  by the fourth parameter.
+
+  @param PciIo         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeRegisters  A pointer to EFI_IDE_REGISTERS data structure.
+  @param Buffer        Buffer contained data transferred between host and device.
+  @param ByteCount     Data size in byte unit of the buffer.
+  @param Read          Flag used to determine the data transfer direction.
+                       Read equals 1, means data transferred from device to host;
+                       Read equals 0, means data transferred from host to device.
+  @param Timeout       Timeout value for wait DRQ ready before each data stream's transfer
+                       , uses 100ns as a unit.
+
+  @retval EFI_SUCCESS      data is transferred successfully.
+  @retval EFI_DEVICE_ERROR the device failed to transfer data.
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketReadWrite (
+  IN     EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN     EFI_IDE_REGISTERS         *IdeRegisters,
+  IN OUT VOID                      *Buffer,
+  IN     UINT64                    ByteCount,
+  IN     BOOLEAN                   Read,
+  IN     UINT64                    Timeout
+  )
+{
+  UINT32      RequiredWordCount;
+  UINT32      ActualWordCount;
+  UINT32      WordCount;
+  EFI_STATUS  Status;
+  UINT16      *PtrBuffer;
+
+  //
+  // No data transfer is premitted.
+  //
+  if (ByteCount == 0) {
+    return EFI_SUCCESS;
+  }
+
+  PtrBuffer         = Buffer;
+  RequiredWordCount = (UINT32)RShiftU64(ByteCount, 1);
+  //
+  // ActuralWordCount means the word count of data really transferred.
+  //
+  ActualWordCount = 0;
+
+  while (ActualWordCount < RequiredWordCount) {
+    //
+    // before each data transfer stream, the host should poll DRQ bit ready,
+    // to see whether indicates device is ready to transfer data.
+    //
+    Status = DRQReady2 (PciIo, IdeRegisters, Timeout);
+    if (EFI_ERROR (Status)) {
+      return CheckStatusRegister (PciIo, IdeRegisters);
+    }
+
+    //
+    // get current data transfer size from Cylinder Registers.
+    //
+    WordCount  = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb) << 8;
+    WordCount  = WordCount | IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+    WordCount  = WordCount & 0xffff;
+    WordCount /= 2;
+
+    WordCount = MIN (WordCount, (RequiredWordCount - ActualWordCount));
+
+    if (Read) {
+      IdeReadPortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        WordCount,
+        PtrBuffer
+        );
+    } else {
+      IdeWritePortWMultiple (
+        PciIo,
+        IdeRegisters->Data,
+        WordCount,
+        PtrBuffer
+        );
+    }
+
+    //
+    // read status register to check whether error happens.
+    //
+    Status = CheckStatusRegister (PciIo, IdeRegisters);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    PtrBuffer       += WordCount;
+    ActualWordCount += WordCount;
+  }
+
+  if (Read) {
+    //
+    // In the case where the drive wants to send more data than we need to read,
+    // the DRQ bit will be set and cause delays from DRQClear2().
+    // We need to read data from the drive until it clears DRQ so we can move on.
+    //
+    AtaPacketReadPendingData (PciIo, IdeRegisters);
+  }
+
+  //
+  // read status register to check whether error happens.
+  //
+  Status = CheckStatusRegister (PciIo, IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // After data transfer is completed, normally, DRQ bit should clear.
+  //
+  Status = DRQClear (PciIo, IdeRegisters, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command
+  with PIO Data In Protocol.
+
+  @param[in] PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                            store the IDE i/o port registers' base addresses
+  @param[in] Channel        The channel number of device.
+  @param[in] Device         The device number of device.
+  @param[in] Packet         A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_IDE_REGISTERS                             *IdeRegisters,
+  IN  UINT8                                         Channel,
+  IN  UINT8                                         Device,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  )
+{
+  EFI_ATA_COMMAND_BLOCK       AtaCommandBlock;
+  EFI_STATUS                  Status;
+  UINT8                       Count;
+  UINT8                       PacketCommand[12];
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  //
+  // Fill ATAPI Command Packet according to CDB.
+  // For Atapi cmd, its length should be less than or equal to 12 bytes.
+  //
+  if (Packet->CdbLength > 12) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (PacketCommand, 12);
+  CopyMem (PacketCommand, Packet->Cdb, Packet->CdbLength);
+
+  //
+  // No OVL; No DMA
+  //
+  AtaCommandBlock.AtaFeatures = 0x00;
+  //
+  // set the transfersize to ATAPI_MAX_BYTE_COUNT to let the device
+  // determine how many data should be transferred.
+  //
+  AtaCommandBlock.AtaCylinderLow  = (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff);
+  AtaCommandBlock.AtaCylinderHigh = (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8);
+  AtaCommandBlock.AtaDeviceHead   = (UINT8) (Device << 0x4);
+  AtaCommandBlock.AtaCommand      = ATA_CMD_PACKET;
+
+  IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)(0xe0 | (Device << 0x4)));
+  //
+  //  Disable interrupt
+  //
+  IdeWritePortB (PciIo, IdeRegisters->AltOrDev, ATA_DEFAULT_CTL);
+
+  //
+  // Issue ATA PACKET command firstly
+  //
+  Status = AtaIssueCommand (PciIo, IdeRegisters, &AtaCommandBlock, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = DRQReady (PciIo, IdeRegisters, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Send out ATAPI command packet
+  //
+  for (Count = 0; Count < 6; Count++) {
+    IdeWritePortW (PciIo, IdeRegisters->Data, *((UINT16*)PacketCommand + Count));
+    //
+    // Stall for 10 microseconds.
+    //
+    MicroSecondDelay (10);
+  }
+
+  //
+  // Read/Write the data of ATAPI Command
+  //
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    Status = AtaPacketReadWrite (
+               PciIo,
+               IdeRegisters,
+               Packet->InDataBuffer,
+               Packet->InTransferLength,
+               TRUE,
+               Packet->Timeout
+               );
+  } else {
+    Status = AtaPacketReadWrite (
+               PciIo,
+               IdeRegisters,
+               Packet->OutDataBuffer,
+               Packet->OutTransferLength,
+               FALSE,
+               Packet->Timeout
+               );
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the calculated Best transfer mode to a detected device.
+
+  @param Instance               A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel                The channel number of device.
+  @param Device                 The device number of device.
+  @param TransferMode           A pointer to EFI_ATA_TRANSFER_MODE data structure.
+  @param AtaStatusBlock         A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Set transfer mode successfully.
+  @retval EFI_DEVICE_ERROR     Set transfer mode failed.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDeviceTransferMode (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_ATA_TRANSFER_MODE         *TransferMode,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_SET_FEATURES;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8)(Device << 0x4);
+  AtaCommandBlock.AtaFeatures    = 0x03;
+  AtaCommandBlock.AtaSectorCount = *((UINT8 *)TransferMode);
+
+  //
+  // Send SET FEATURE command (sub command 0x03) to set pio mode.
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Set drive parameters for devices not support PACKETS command.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param DriveParameters  A pointer to EFI_ATA_DRIVE_PARMS data structure.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Set drive parameter successfully.
+  @retval EFI_DEVICE_ERROR     Set drive parameter failed.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDriveParameters (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_ATA_DRIVE_PARMS           *DriveParameters,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand     = ATA_CMD_INIT_DRIVE_PARAM;
+  AtaCommandBlock.AtaSectorCount = DriveParameters->Sector;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8) ((Device << 0x4) + DriveParameters->Heads);
+
+  //
+  // Send Init drive parameters
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  //
+  // Send Set Multiple parameters
+  //
+  AtaCommandBlock.AtaCommand     = ATA_CMD_SET_MULTIPLE_MODE;
+  AtaCommandBlock.AtaSectorCount = DriveParameters->MultipleSector;
+  AtaCommandBlock.AtaDeviceHead  = (UINT8)(Device << 0x4);
+
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  Send SMART Return Status command to check if the execution of SMART cmd is successful or not.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS     Successfully get the return status of S.M.A.R.T command execution.
+  @retval Others          Fail to get return status data.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeAtaSmartReturnStatusCheck (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+  UINT8                   LBAMid;
+  UINT8                   LBAHigh;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+  AtaCommandBlock.AtaFeatures     = ATA_SMART_RETURN_STATUS;
+  AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+  AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+  AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+  //
+  // Send S.M.A.R.T Read Return Status command to device
+  //
+  Status = AtaNonDataCommandIn (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLED)
+      );
+    return EFI_DEVICE_ERROR;
+  }
+
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_ENABLE)
+    );
+
+  LBAMid  = IdeReadPortB (Instance->PciIo, Instance->IdeRegisters[Channel].CylinderLsb);
+  LBAHigh = IdeReadPortB (Instance->PciIo, Instance->IdeRegisters[Channel].CylinderMsb);
+
+  if ((LBAMid == 0x4f) && (LBAHigh == 0xc2)) {
+    //
+    // The threshold exceeded condition is not detected by the device
+    //
+    DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is not detected\n"));
+    REPORT_STATUS_CODE (
+          EFI_PROGRESS_CODE,
+          (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_UNDERTHRESHOLD)
+          );
+  } else if ((LBAMid == 0xf4) && (LBAHigh == 0x2c)) {
+    //
+    // The threshold exceeded condition is detected by the device
+    //
+    DEBUG ((EFI_D_INFO, "The S.M.A.R.T threshold exceeded condition is detected\n"));
+    REPORT_STATUS_CODE (
+         EFI_PROGRESS_CODE,
+         (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_OVERTHRESHOLD)
+         );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable SMART command of the disk if supported.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param IdentifyData     A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+**/
+VOID
+EFIAPI
+IdeAtaSmartSupport (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN     EFI_IDENTIFY_DATA             *IdentifyData,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS              Status;
+  EFI_ATA_COMMAND_BLOCK   AtaCommandBlock;
+
+  //
+  // Detect if the device supports S.M.A.R.T.
+  //
+  if ((IdentifyData->AtaData.command_set_supported_82 & 0x0001) != 0x0001) {
+    //
+    // S.M.A.R.T is not supported by the device
+    //
+    DEBUG ((EFI_D_INFO, "S.M.A.R.T feature is not supported at [%a] channel [%a] device!\n",
+            (Channel == 1) ? "secondary" : "primary", (Device == 1) ? "slave" : "master"));
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_NOTSUPPORTED)
+      );
+  } else {
+    //
+    // Check if the feature is enabled. If not, then enable S.M.A.R.T.
+    //
+    if ((IdentifyData->AtaData.command_set_feature_enb_85 & 0x0001) != 0x0001) {
+
+      REPORT_STATUS_CODE (
+        EFI_PROGRESS_CODE,
+        (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_ATA_BUS_SMART_DISABLE)
+        );
+
+      ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+      AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+      AtaCommandBlock.AtaFeatures     = ATA_SMART_ENABLE_OPERATION;
+      AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+      AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+      AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+      //
+      // Send S.M.A.R.T Enable command to device
+      //
+      Status = AtaNonDataCommandIn (
+                 Instance->PciIo,
+                 &Instance->IdeRegisters[Channel],
+                 &AtaCommandBlock,
+                 AtaStatusBlock,
+                 ATA_ATAPI_TIMEOUT,
+                 NULL
+                 );
+
+      if (!EFI_ERROR (Status)) {
+        //
+        // Send S.M.A.R.T AutoSave command to device
+        //
+        ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+        AtaCommandBlock.AtaCommand      = ATA_CMD_SMART;
+        AtaCommandBlock.AtaFeatures     = 0xD2;
+        AtaCommandBlock.AtaSectorCount  = 0xF1;
+        AtaCommandBlock.AtaCylinderLow  = ATA_CONSTANT_4F;
+        AtaCommandBlock.AtaCylinderHigh = ATA_CONSTANT_C2;
+        AtaCommandBlock.AtaDeviceHead   = (UINT8) ((Device << 0x4) | 0xe0);
+
+        Status = AtaNonDataCommandIn (
+                   Instance->PciIo,
+                   &Instance->IdeRegisters[Channel],
+                   &AtaCommandBlock,
+                   AtaStatusBlock,
+                   ATA_ATAPI_TIMEOUT,
+                   NULL
+                   );
+        if (!EFI_ERROR (Status)) {
+          Status = IdeAtaSmartReturnStatusCheck (
+                     Instance,
+                     Channel,
+                     Device,
+                     AtaStatusBlock
+                     );
+        }
+      }
+    }
+
+    DEBUG ((EFI_D_INFO, "Enabled S.M.A.R.T feature at [%a] channel [%a] device!\n",
+           (Channel == 1) ? "secondary" : "primary", (Device == 1) ? "slave" : "master"));
+
+  }
+
+  return ;
+}
+
+
+/**
+  Sends out an ATA Identify Command to the specified device.
+
+  This function is called by DiscoverIdeDevice() during its device
+  identification. It sends out the ATA Identify Command to the
+  specified device. Only ATA device responses to this command. If
+  the command succeeds, it returns the Identify data structure which
+  contains information about the device. This function extracts the
+  information it needs to fill the IDE_BLK_IO_DEV data structure,
+  including device type, media block size, media capacity, and etc.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param Buffer           A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Identify ATA device successfully.
+  @retval EFI_DEVICE_ERROR     ATA Identify Device Command failed or device is not ATA device.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIdentify (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_IDENTIFY_DATA             *Buffer,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS             Status;
+  EFI_ATA_COMMAND_BLOCK  AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand    = ATA_CMD_IDENTIFY_DRIVE;
+  AtaCommandBlock.AtaDeviceHead = (UINT8)(Device << 0x4);
+
+  Status = AtaPioDataInOut (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             TRUE,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+/**
+  This function is called by DiscoverIdeDevice() during its device
+  identification.
+  Its main purpose is to get enough information for the device media
+  to fill in the Media data structure of the Block I/O Protocol interface.
+
+  There are 5 steps to reach such objective:
+  1. Sends out the ATAPI Identify Command to the specified device.
+  Only ATAPI device responses to this command. If the command succeeds,
+  it returns the Identify data structure which filled with information
+  about the device. Since the ATAPI device contains removable media,
+  the only meaningful information is the device module name.
+  2. Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return inquiry data of the device, which contains
+  the device type information.
+  3. Allocate sense data space for future use. We don't detect the media
+  presence here to improvement boot performance, especially when CD
+  media is present. The media detection will be performed just before
+  each BLK_IO read/write
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param Channel          The channel number of device.
+  @param Device           The device number of device.
+  @param Buffer           A pointer to data buffer which is used to contain IDENTIFY data.
+  @param AtaStatusBlock   A pointer to EFI_ATA_STATUS_BLOCK data structure.
+
+  @retval EFI_SUCCESS          Identify ATAPI device successfully.
+  @retval EFI_DEVICE_ERROR     ATA Identify Packet Device Command failed or device type
+                               is not supported by this IDE driver.
+  @retval EFI_OUT_OF_RESOURCES Allocate memory failed.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaIdentifyPacket (
+  IN     ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN     UINT8                         Channel,
+  IN     UINT8                         Device,
+  IN OUT EFI_IDENTIFY_DATA             *Buffer,
+  IN OUT EFI_ATA_STATUS_BLOCK          *AtaStatusBlock
+  )
+{
+  EFI_STATUS             Status;
+  EFI_ATA_COMMAND_BLOCK  AtaCommandBlock;
+
+  ZeroMem (&AtaCommandBlock, sizeof (EFI_ATA_COMMAND_BLOCK));
+
+  AtaCommandBlock.AtaCommand    = ATA_CMD_IDENTIFY_DEVICE;
+  AtaCommandBlock.AtaDeviceHead = (UINT8)(Device << 0x4);
+
+  //
+  // Send ATAPI Identify Command to get IDENTIFY data.
+  //
+  Status = AtaPioDataInOut (
+             Instance->PciIo,
+             &Instance->IdeRegisters[Channel],
+             (VOID *) Buffer,
+             sizeof (EFI_IDENTIFY_DATA),
+             TRUE,
+             &AtaCommandBlock,
+             AtaStatusBlock,
+             ATA_ATAPI_TIMEOUT,
+             NULL
+             );
+
+  return Status;
+}
+
+
+/**
+  This function is used for detect whether the IDE device exists in the
+  specified Channel as the specified Device Number.
+
+  There is two IDE channels: one is Primary Channel, the other is
+  Secondary Channel.(Channel is the logical name for the physical "Cable".)
+  Different channel has different register group.
+
+  On each IDE channel, at most two IDE devices attach,
+  one is called Device 0 (Master device), the other is called Device 1
+  (Slave device). The devices on the same channel co-use the same register
+  group, so before sending out a command for a specified device via command
+  register, it is a must to select the current device to accept the command
+  by set the device number in the Head/Device Register.
+
+  @param Instance         A pointer to ATA_ATAPI_PASS_THRU_INSTANCE data structure.
+  @param IdeChannel       The channel number of device.
+
+  @retval EFI_SUCCESS successfully detects device.
+  @retval other       any failure during detection process will return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+DetectAndConfigIdeDevice (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE  *Instance,
+  IN  UINT8                         IdeChannel
+  )
+{
+  EFI_STATUS                        Status;
+  UINT8                             SectorCountReg;
+  UINT8                             LBALowReg;
+  UINT8                             LBAMidReg;
+  UINT8                             LBAHighReg;
+  EFI_ATA_DEVICE_TYPE               DeviceType;
+  UINT8                             IdeDevice;
+  EFI_IDE_REGISTERS                 *IdeRegisters;
+  EFI_IDENTIFY_DATA                 Buffer;
+
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeInit;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+
+  EFI_ATA_COLLECTIVE_MODE           *SupportedModes;
+  EFI_ATA_TRANSFER_MODE             TransferMode;
+  EFI_ATA_DRIVE_PARMS               DriveParameters;
+
+  IdeRegisters = &Instance->IdeRegisters[IdeChannel];
+  IdeInit      = Instance->IdeControllerInit;
+  PciIo        = Instance->PciIo;
+
+  for (IdeDevice = 0; IdeDevice < EfiIdeMaxDevice; IdeDevice++) {
+    //
+    // Send ATA Device Execut Diagnostic command.
+    // This command should work no matter DRDY is ready or not
+    //
+    IdeWritePortB (PciIo, IdeRegisters->CmdOrStatus, ATA_CMD_EXEC_DRIVE_DIAG);
+
+    Status = WaitForBSYClear (PciIo, IdeRegisters, 350000000);
+    if (EFI_ERROR (Status)) {
+      DEBUG((EFI_D_ERROR, "New detecting method: Send Execute Diagnostic Command: WaitForBSYClear: Status: %d\n", Status));
+      continue;
+    }
+
+    //
+    // Select Master or Slave device to get the return signature for ATA DEVICE DIAGNOSTIC cmd.
+    //
+    IdeWritePortB (PciIo, IdeRegisters->Head, (UINT8)((IdeDevice << 4) | 0xe0));
+    //
+    // Stall for 1 milliseconds.
+    //
+    MicroSecondDelay (1000);
+
+    SectorCountReg = IdeReadPortB (PciIo, IdeRegisters->SectorCount);
+    LBALowReg      = IdeReadPortB (PciIo, IdeRegisters->SectorNumber);
+    LBAMidReg      = IdeReadPortB (PciIo, IdeRegisters->CylinderLsb);
+    LBAHighReg     = IdeReadPortB (PciIo, IdeRegisters->CylinderMsb);
+
+    //
+    // Refer to ATA/ATAPI 4 Spec, section 9.1
+    //
+    if ((SectorCountReg == 0x1) && (LBALowReg == 0x1) && (LBAMidReg == 0x0) && (LBAHighReg == 0x0)) {
+      DeviceType = EfiIdeHarddisk;
+    } else if ((LBAMidReg == 0x14) && (LBAHighReg == 0xeb)) {
+      DeviceType = EfiIdeCdrom;
+    } else {
+      continue;
+    }
+
+    //
+    // Send IDENTIFY cmd to the device to test if it is really attached.
+    //
+    if (DeviceType == EfiIdeHarddisk) {
+      Status = AtaIdentify (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      //
+      // if identifying ata device is failure, then try to send identify packet cmd.
+      //
+      if (EFI_ERROR (Status)) {
+        REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_EC_NOT_DETECTED));
+
+        DeviceType = EfiIdeCdrom;
+        Status     = AtaIdentifyPacket (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      }
+    } else {
+      Status = AtaIdentifyPacket (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      //
+      // if identifying atapi device is failure, then try to send identify cmd.
+      //
+      if (EFI_ERROR (Status)) {
+        DeviceType = EfiIdeHarddisk;
+        Status     = AtaIdentify (Instance, IdeChannel, IdeDevice, &Buffer, NULL);
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      //
+      // No device is found at this port
+      //
+      continue;
+    }
+
+    DEBUG ((EFI_D_INFO, "[%a] channel [%a] [%a] device\n",
+            (IdeChannel == 1) ? "secondary" : "primary  ", (IdeDevice == 1) ? "slave " : "master",
+            DeviceType == EfiIdeCdrom ? "cdrom   " : "harddisk"));
+    //
+    // If the device is a hard disk, then try to enable S.M.A.R.T feature
+    //
+    if ((DeviceType == EfiIdeHarddisk) && PcdGetBool (PcdAtaSmartEnable)) {
+      IdeAtaSmartSupport (
+        Instance,
+        IdeChannel,
+        IdeDevice,
+        &Buffer,
+        NULL
+        );
+    }
+
+    //
+    // Submit identify data to IDE controller init driver
+    //
+    IdeInit->SubmitData (IdeInit, IdeChannel, IdeDevice, &Buffer);
+
+    //
+    // Now start to config ide device parameter and transfer mode.
+    //
+    Status = IdeInit->CalculateMode (
+                        IdeInit,
+                        IdeChannel,
+                        IdeDevice,
+                        &SupportedModes
+                        );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Calculate Mode Fail, Status = %r\n", Status));
+      continue;
+    }
+
+    //
+    // Set best supported PIO mode on this IDE device
+    //
+    if (SupportedModes->PioMode.Mode <= EfiAtaPioMode2) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_DEFAULT_PIO;
+    } else {
+      TransferMode.ModeCategory = EFI_ATA_MODE_FLOW_PIO;
+    }
+
+    TransferMode.ModeNumber = (UINT8) (SupportedModes->PioMode.Mode);
+
+    if (SupportedModes->ExtModeCount == 0){
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    }
+
+    //
+    // Set supported DMA mode on this IDE device. Note that UDMA & MDMA cann't
+    // be set together. Only one DMA mode can be set to a device. If setting
+    // DMA mode operation fails, we can continue moving on because we only use
+    // PIO mode at boot time. DMA modes are used by certain kind of OS booting
+    //
+    if (SupportedModes->UdmaMode.Valid) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_UDMA;
+      TransferMode.ModeNumber   = (UINT8) (SupportedModes->UdmaMode.Mode);
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    } else if (SupportedModes->MultiWordDmaMode.Valid) {
+      TransferMode.ModeCategory = EFI_ATA_MODE_MDMA;
+      TransferMode.ModeNumber   = (UINT8) SupportedModes->MultiWordDmaMode.Mode;
+      Status = SetDeviceTransferMode (Instance, IdeChannel, IdeDevice, &TransferMode, NULL);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Set transfer Mode Fail, Status = %r\n", Status));
+        continue;
+      }
+    }
+
+    //
+    // Set Parameters for the device:
+    // 1) Init
+    // 2) Establish the block count for READ/WRITE MULTIPLE (EXT) command
+    //
+    if (DeviceType == EfiIdeHarddisk) {
+      //
+      // Init driver parameters
+      //
+      DriveParameters.Sector         = (UINT8) ((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->sectors_per_track;
+      DriveParameters.Heads          = (UINT8) (((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->heads - 1);
+      DriveParameters.MultipleSector = (UINT8) ((ATA5_IDENTIFY_DATA *)(&Buffer.AtaData))->multi_sector_cmd_max_sct_cnt;
+
+      Status = SetDriveParameters (Instance, IdeChannel, IdeDevice, &DriveParameters, NULL);
+    }
+
+    //
+    // Set IDE controller Timing Blocks in the PCI Configuration Space
+    //
+    IdeInit->SetTiming (IdeInit, IdeChannel, IdeDevice, SupportedModes);
+
+    //
+    // IDE controller and IDE device timing is configured successfully.
+    // Now insert the device into device list.
+    //
+    Status = CreateNewDeviceInfo (Instance, IdeChannel, IdeDevice, DeviceType, &Buffer);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (DeviceType == EfiIdeHarddisk) {
+      REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_PERIPHERAL_FIXED_MEDIA | EFI_P_PC_ENABLE));
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize ATA host controller at IDE mode.
+
+  The function is designed to initialize ATA host controller.
+
+  @param[in]  Instance          A pointer to the ATA_ATAPI_PASS_THRU_INSTANCE instance.
+
+**/
+EFI_STATUS
+EFIAPI
+IdeModeInitialization (
+  IN  ATA_ATAPI_PASS_THRU_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_IDE_CONTROLLER_INIT_PROTOCOL  *IdeInit;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UINT8                             Channel;
+  UINT8                             IdeChannel;
+  BOOLEAN                           ChannelEnabled;
+  UINT8                             MaxDevices;
+
+  IdeInit = Instance->IdeControllerInit;
+  PciIo   = Instance->PciIo;
+  Channel = IdeInit->ChannelCount;
+
+  //
+  // Obtain IDE IO port registers' base addresses
+  //
+  Status = GetIdeRegisterIoAddr (PciIo, Instance->IdeRegisters);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  for (IdeChannel = 0; IdeChannel < Channel; IdeChannel++) {
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelEnumeration, IdeChannel);
+
+    //
+    // now obtain channel information fron IdeControllerInit protocol.
+    //
+    Status = IdeInit->GetChannelInfo (
+                        IdeInit,
+                        IdeChannel,
+                        &ChannelEnabled,
+                        &MaxDevices
+                        );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "[GetChannel, Status=%x]", Status));
+      continue;
+    }
+
+    if (!ChannelEnabled) {
+      continue;
+    }
+
+    ASSERT (MaxDevices <= 2);
+    //
+    // Now inform the IDE Controller Init Module.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBeforeChannelReset, IdeChannel);
+
+    //
+    // No reset channel function implemented.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeAfterChannelReset, IdeChannel);
+
+    //
+    // Now inform the IDE Controller Init Module.
+    //
+    IdeInit->NotifyPhase (IdeInit, EfiIdeBusBeforeDevicePresenceDetection, IdeChannel);
+
+    //
+    // Detect all attached ATA devices and set the transfer mode for each device.
+    //
+    DetectAndConfigIdeDevice (Instance, IdeChannel);
+  }
+
+  //
+  // All configurations done! Notify IdeController to do post initialization
+  // work such as saving IDE controller PCI settings for S3 resume
+  //
+  IdeInit->NotifyPhase (IdeInit, EfiIdeBusPhaseMaximum, 0);
+
+ErrorExit:
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h
new file mode 100644
index 0000000000..67fff64658
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/IdeMode.h
@@ -0,0 +1,204 @@
+/** @file
+  Header file for IDE mode of ATA host controller.
+  
+  Copyright (c) 2010 - 2011, 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.             
+
+**/
+#ifndef __ATA_HC_IDE_MODE_H__
+#define __ATA_HC_IDE_MODE_H__
+
+typedef enum {
+  EfiIdePrimary    = 0,
+  EfiIdeSecondary  = 1,
+  EfiIdeMaxChannel = 2
+} EFI_IDE_CHANNEL;
+
+typedef enum {
+  EfiIdeMaster    = 0,
+  EfiIdeSlave     = 1,
+  EfiIdeMaxDevice = 2
+} EFI_IDE_DEVICE;
+
+///
+/// PIO mode definition
+///
+typedef enum {
+  EfiAtaPioModeBelow2,
+  EfiAtaPioMode2,
+  EfiAtaPioMode3,
+  EfiAtaPioMode4
+} EFI_ATA_PIO_MODE;
+
+//
+// Multi word DMA definition
+//
+typedef enum {
+  EfiAtaMdmaMode0,
+  EfiAtaMdmaMode1,
+  EfiAtaMdmaMode2
+} EFI_ATA_MDMA_MODE;
+
+//
+// UDMA mode definition
+//
+typedef enum {
+  EfiAtaUdmaMode0,
+  EfiAtaUdmaMode1,
+  EfiAtaUdmaMode2,
+  EfiAtaUdmaMode3,
+  EfiAtaUdmaMode4,
+  EfiAtaUdmaMode5
+} EFI_ATA_UDMA_MODE;
+
+//
+// Bus Master Reg
+//
+#define BMIC_NREAD      BIT3
+#define BMIC_START      BIT0
+#define BMIS_INTERRUPT  BIT2
+#define BMIS_ERROR      BIT1
+
+#define BMIC_OFFSET    0x00
+#define BMIS_OFFSET    0x02
+#define BMID_OFFSET    0x04
+
+//
+// IDE transfer mode
+//
+#define EFI_ATA_MODE_DEFAULT_PIO 0x00
+#define EFI_ATA_MODE_FLOW_PIO    0x01
+#define EFI_ATA_MODE_MDMA        0x04
+#define EFI_ATA_MODE_UDMA        0x08
+
+typedef struct {
+  UINT32  RegionBaseAddr;
+  UINT16  ByteCount;
+  UINT16  EndOfTable;
+} EFI_ATA_DMA_PRD;
+
+typedef struct {
+  UINT8 ModeNumber   : 3;
+  UINT8 ModeCategory : 5;
+} EFI_ATA_TRANSFER_MODE;
+
+typedef struct {
+  UINT8 Sector;
+  UINT8 Heads;
+  UINT8 MultipleSector;
+} EFI_ATA_DRIVE_PARMS;
+
+//
+// IDE registers set
+//
+typedef struct {
+  UINT16                          Data;
+  UINT16                          ErrOrFeature;
+  UINT16                          SectorCount;
+  UINT16                          SectorNumber;
+  UINT16                          CylinderLsb;
+  UINT16                          CylinderMsb;
+  UINT16                          Head;
+  UINT16                          CmdOrStatus;
+  UINT16                          AltOrDev;
+
+  UINT16                          BusMasterBaseAddr;
+} EFI_IDE_REGISTERS;
+
+//
+// Bit definitions in Programming Interface byte of the Class Code field
+// in PCI IDE controller's Configuration Space
+//
+#define IDE_PRIMARY_OPERATING_MODE            BIT0
+#define IDE_PRIMARY_PROGRAMMABLE_INDICATOR    BIT1
+#define IDE_SECONDARY_OPERATING_MODE          BIT2
+#define IDE_SECONDARY_PROGRAMMABLE_INDICATOR  BIT3
+
+/**
+  Get IDE i/o port registers' base addresses by mode. 
+
+  In 'Compatibility' mode, use fixed addresses.
+  In Native-PCI mode, get base addresses from BARs in the PCI IDE controller's
+  Configuration Space.
+
+  The steps to get IDE i/o port registers' base addresses for each channel
+  as follows:
+
+  1. Examine the Programming Interface byte of the Class Code fields in PCI IDE
+  controller's Configuration Space to determine the operating mode.
+
+  2. a) In 'Compatibility' mode, use fixed addresses shown in the Table 1 below.
+   ___________________________________________
+  |           | Command Block | Control Block |
+  |  Channel  |   Registers   |   Registers   |
+  |___________|_______________|_______________|
+  |  Primary  |  1F0h - 1F7h  |  3F6h - 3F7h  |
+  |___________|_______________|_______________|
+  | Secondary |  170h - 177h  |  376h - 377h  |
+  |___________|_______________|_______________|
+
+  Table 1. Compatibility resource mappings
+  
+  b) In Native-PCI mode, IDE registers are mapped into IO space using the BARs
+  in IDE controller's PCI Configuration Space, shown in the Table 2 below.
+   ___________________________________________________
+  |           |   Command Block   |   Control Block   |
+  |  Channel  |     Registers     |     Registers     |
+  |___________|___________________|___________________|
+  |  Primary  | BAR at offset 0x10| BAR at offset 0x14|
+  |___________|___________________|___________________|
+  | Secondary | BAR at offset 0x18| BAR at offset 0x1C|
+  |___________|___________________|___________________|
+
+  Table 2. BARs for Register Mapping
+
+  @param[in]      PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in, out] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                                 store the IDE i/o port registers' base addresses
+           
+  @retval EFI_UNSUPPORTED        Return this value when the BARs is not IO type
+  @retval EFI_SUCCESS            Get the Base address successfully
+  @retval Other                  Read the pci configureation data error
+
+**/
+EFI_STATUS
+EFIAPI
+GetIdeRegisterIoAddr (
+  IN     EFI_PCI_IO_PROTOCOL         *PciIo,
+  IN OUT EFI_IDE_REGISTERS           *IdeRegisters
+  );
+
+/**
+  This function is used to send out ATAPI commands conforms to the Packet Command 
+  with PIO Data In Protocol.
+
+  @param[in] PciIo          Pointer to the EFI_PCI_IO_PROTOCOL instance
+  @param[in] IdeRegisters   Pointer to EFI_IDE_REGISTERS which is used to
+                            store the IDE i/o port registers' base addresses
+  @param[in] Channel        The channel number of device.
+  @param[in] Device         The device number of device.
+  @param[in] Packet         A pointer to EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+
+  @retval EFI_SUCCESS       send out the ATAPI packet command successfully
+                            and device sends data successfully.
+  @retval EFI_DEVICE_ERROR  the device failed to send data.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaPacketCommandExecute (
+  IN  EFI_PCI_IO_PROTOCOL                           *PciIo,
+  IN  EFI_IDE_REGISTERS                             *IdeRegisters,
+  IN  UINT8                                         Channel,
+  IN  UINT8                                         Device,
+  IN  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c
new file mode 100644
index 0000000000..e7449f9714
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.c
@@ -0,0 +1,1718 @@
+/** @file
+  This file implements protocol interfaces for ATA bus driver.
+
+  This file implements protocol interfaces: Driver Binding protocol,
+  Block IO protocol and DiskInfo protocol.
+
+  Copyright (c) 2009 - 2015, 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 "AtaBus.h"
+
+//
+// ATA Bus Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gAtaBusDriverBinding = {
+  AtaBusDriverBindingSupported,
+  AtaBusDriverBindingStart,
+  AtaBusDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for ATA Child Device.
+//
+ATA_DEVICE gAtaDeviceTemplate = {
+  ATA_DEVICE_SIGNATURE,        // Signature
+  NULL,                        // Handle
+  {                            // BlockIo
+    EFI_BLOCK_IO_PROTOCOL_REVISION,
+    NULL,
+    AtaBlockIoReset,
+    AtaBlockIoReadBlocks,
+    AtaBlockIoWriteBlocks,
+    AtaBlockIoFlushBlocks
+  },
+  {                            // BlockIo2
+    NULL,
+    AtaBlockIoResetEx,
+    AtaBlockIoReadBlocksEx,
+    AtaBlockIoWriteBlocksEx,
+    AtaBlockIoFlushBlocksEx
+  },
+  {                            // BlockMedia
+    0,                         // MediaId
+    FALSE,                     // RemovableMedia
+    TRUE,                      // MediaPresent
+    FALSE,                     // LogicPartition
+    FALSE,                     // ReadOnly
+    FALSE,                     // WritingCache
+    0x200,                     // BlockSize
+    0,                         // IoAlign
+    0,                         // LastBlock
+    0,                         // LowestAlignedLba
+    1                          // LogicalBlocksPerPhysicalBlock
+  },
+  {                            // DiskInfo
+    EFI_DISK_INFO_IDE_INTERFACE_GUID,
+    AtaDiskInfoInquiry,
+    AtaDiskInfoIdentify,
+    AtaDiskInfoSenseData,
+    AtaDiskInfoWhichIde
+  },
+  NULL,                        // DevicePath
+  {
+    AtaStorageSecurityReceiveData,
+    AtaStorageSecuritySendData
+  },
+  NULL,                        // AtaBusDriverData
+  0,                           // Port
+  0,                           // PortMultiplierPort
+  { 0, },                      // Packet
+  {{ 0}, },                    // Acb
+  NULL,                        // Asb
+  FALSE,                       // UdmaValid
+  FALSE,                       // Lba48Bit
+  NULL,                        // IdentifyData
+  NULL,                        // ControllerNameTable
+  {L'\0', },                   // ModelName
+  {NULL, NULL},                // AtaTaskList
+  {NULL, NULL},                // AtaSubTaskList
+  FALSE                        // Abort
+};
+
+/**
+  Allocates an aligned buffer for ATA device.
+
+  This function allocates an aligned buffer for the ATA device to perform
+  ATA pass through operations. The alignment requirement is from ATA pass
+  through interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN ATA_DEVICE               *AtaDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), AtaDevice->AtaBusDriverData->AtaPassThru->Mode->IoAlign);
+}
+
+/**
+  Frees an aligned buffer for ATA device.
+
+  This function frees an aligned buffer for the ATA device to perform
+  ATA pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+
+/**
+  Release all the resources allocated for the ATA device.
+
+  This function releases all the resources allocated for the ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+**/
+VOID
+ReleaseAtaResources (
+  IN ATA_DEVICE  *AtaDevice
+  )
+{
+  ATA_BUS_ASYN_SUB_TASK *SubTask;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *DelEntry;
+  EFI_TPL               OldTpl;
+
+  FreeUnicodeStringTable (AtaDevice->ControllerNameTable);
+  FreeAlignedBuffer (AtaDevice->Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+  FreeAlignedBuffer (AtaDevice->IdentifyData, sizeof (ATA_IDENTIFY_DATA));
+  if (AtaDevice->DevicePath != NULL) {
+    FreePool (AtaDevice->DevicePath);
+  }
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  if (!IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for(Entry = AtaDevice->AtaSubTaskList.ForwardLink;
+        Entry != (&AtaDevice->AtaSubTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      SubTask  = ATA_ASYN_SUB_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      FreeAtaSubTask (SubTask);
+    }
+  }
+  if (!IsListEmpty (&AtaDevice->AtaTaskList)) {
+    //
+    // Free the Subtask list.
+    //
+    for(Entry = AtaDevice->AtaTaskList.ForwardLink;
+        Entry != (&AtaDevice->AtaTaskList);
+       ) {
+      DelEntry = Entry;
+      Entry    = Entry->ForwardLink;
+      AtaTask  = ATA_ASYN_TASK_FROM_ENTRY (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      FreePool (AtaTask);
+    }
+  }
+  gBS->RestoreTPL (OldTpl);
+  FreePool (AtaDevice);
+}
+
+
+/**
+  Registers an ATA device.
+
+  This function allocates an ATA device structure for the ATA device specified by
+  Port and PortMultiplierPort if the ATA device is identified as a valid one.
+  Then it will create child handle and install Block IO and Disk Info protocol on
+  it.
+
+  @param  AtaBusDriverData      The parent ATA bus driver data structure.
+  @param  Port                  The port number of the ATA device.
+  @param  PortMultiplierPort    The port multiplier port number of the ATA device.
+
+  @retval EFI_SUCCESS           The ATA device is successfully registered.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to allocate the ATA device
+                                and related data structures.
+  @return Others                Some error occurs when registering the ATA device.
+**/
+EFI_STATUS
+RegisterAtaDevice (
+  IN OUT ATA_BUS_DRIVER_DATA        *AtaBusDriverData,
+  IN     UINT16                     Port,
+  IN     UINT16                     PortMultiplierPort
+  )
+{
+  EFI_STATUS                        Status;
+  ATA_DEVICE                        *AtaDevice;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_DEVICE_PATH_PROTOCOL          *NewDevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath;
+  EFI_HANDLE                        DeviceHandle;
+
+  AtaDevice         = NULL;
+  NewDevicePathNode = NULL;
+  DevicePath        = NULL;
+  RemainingDevicePath = NULL;
+
+  //
+  // Build device path
+  //
+  AtaPassThru = AtaBusDriverData->AtaPassThru;
+  Status = AtaPassThru->BuildDevicePath (AtaPassThru, Port, PortMultiplierPort, &NewDevicePathNode);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  DevicePath = AppendDevicePathNode (AtaBusDriverData->ParentDevicePath, NewDevicePathNode);
+  if (DevicePath == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  DeviceHandle = NULL;
+  RemainingDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+  if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+    Status = EFI_ALREADY_STARTED;
+    FreePool (DevicePath);
+    goto Done;
+  }
+
+  //
+  // Allocate ATA device from the template.
+  //
+  AtaDevice = AllocateCopyPool (sizeof (ATA_DEVICE), &gAtaDeviceTemplate);
+  if (AtaDevice == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Initializes ATA device structures and allocates the required buffer.
+  //
+  AtaDevice->BlockIo.Media      = &AtaDevice->BlockMedia;
+  AtaDevice->BlockIo2.Media     = &AtaDevice->BlockMedia;
+  AtaDevice->AtaBusDriverData   = AtaBusDriverData;
+  AtaDevice->DevicePath         = DevicePath;
+  AtaDevice->Port               = Port;
+  AtaDevice->PortMultiplierPort = PortMultiplierPort;
+  AtaDevice->Asb = AllocateAlignedBuffer (AtaDevice, sizeof (EFI_ATA_STATUS_BLOCK));
+  if (AtaDevice->Asb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  AtaDevice->IdentifyData = AllocateAlignedBuffer (AtaDevice, sizeof (ATA_IDENTIFY_DATA));
+  if (AtaDevice->IdentifyData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Initial Ata Task List
+  //
+  InitializeListHead (&AtaDevice->AtaTaskList);
+  InitializeListHead (&AtaDevice->AtaSubTaskList);
+
+  //
+  // Report Status Code to indicate the ATA device will be enabled
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_ENABLE),
+    AtaBusDriverData->ParentDevicePath
+    );
+
+  //
+  // Try to identify the ATA device via the ATA pass through command.
+  //
+  Status = DiscoverAtaDevice (AtaDevice);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Build controller name for Component Name (2) protocol.
+  //
+  Status = AddUnicodeString2 (
+             "eng",
+             gAtaBusComponentName.SupportedLanguages,
+             &AtaDevice->ControllerNameTable,
+             AtaDevice->ModelName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = AddUnicodeString2 (
+             "en",
+             gAtaBusComponentName2.SupportedLanguages,
+             &AtaDevice->ControllerNameTable,
+             AtaDevice->ModelName,
+             FALSE
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Update to AHCI interface GUID based on device path node. The default one
+  // is IDE interface GUID copied from template.
+  //
+  if (NewDevicePathNode->SubType == MSG_SATA_DP) {
+    CopyGuid (&AtaDevice->DiskInfo.Interface, &gEfiDiskInfoAhciInterfaceGuid);
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &AtaDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  AtaDevice->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &AtaDevice->BlockIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &AtaDevice->BlockIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &AtaDevice->DiskInfo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // See if the ata device support trust computing feature or not.
+  // If yes, then install Storage Security Protocol at the ata device handle.
+  //
+  if ((AtaDevice->IdentifyData->trusted_computing_support & BIT0) != 0) {
+    DEBUG ((EFI_D_INFO, "Found TCG support in Port %x PortMultiplierPort %x\n", Port, PortMultiplierPort));
+    Status = gBS->InstallProtocolInterface (
+                    &AtaDevice->Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &AtaDevice->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    DEBUG ((EFI_D_INFO, "Successfully Install Storage Security Protocol on the ATA device\n"));
+  }
+
+  gBS->OpenProtocol (
+         AtaBusDriverData->Controller,
+         &gEfiAtaPassThruProtocolGuid,
+         (VOID **) &AtaPassThru,
+         AtaBusDriverData->DriverBindingHandle,
+         AtaDevice->Handle,
+         EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+         );
+
+Done:
+  if (NewDevicePathNode != NULL) {
+    FreePool (NewDevicePathNode);
+  }
+
+  if (EFI_ERROR (Status) && (AtaDevice != NULL)) {
+    ReleaseAtaResources (AtaDevice);
+    DEBUG ((EFI_D_ERROR | EFI_D_INIT, "Failed to initialize Port %x PortMultiplierPort %x, status = %r\n", Port, PortMultiplierPort, Status));
+  }
+  return Status;
+}
+
+
+/**
+  Unregisters an ATA device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the ATA device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the ATA device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The ATA device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the ATA device.
+
+**/
+EFI_STATUS
+UnregisterAtaDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                               Status;
+  EFI_BLOCK_IO_PROTOCOL                    *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL                   *BlockIo2;
+  ATA_DEVICE                               *AtaDevice;
+  EFI_ATA_PASS_THRU_PROTOCOL               *AtaPassThru;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *StorageSecurity;
+
+  BlockIo2             =     NULL;
+  BlockIo              =     NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Locate BlockIo2 protocol
+    //
+    Status = gBS->OpenProtocol (
+                    Handle,
+                    &gEfiBlockIo2ProtocolGuid,
+                    (VOID **) &BlockIo2,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Get AtaDevice data.
+  //
+  if (BlockIo != NULL) {
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (BlockIo);
+  } else {
+    ASSERT (BlockIo2 != NULL);
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (BlockIo2);
+  }
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiAtaPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The Ata Bus driver installs the BlockIo and BlockIo2 in the DriverBindingStart().
+  // Here should uninstall both of them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  AtaDevice->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &AtaDevice->BlockIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &AtaDevice->BlockIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &AtaDevice->DiskInfo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->OpenProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          (VOID **) &AtaPassThru,
+          This->DriverBindingHandle,
+          Handle,
+          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+          );
+    return Status;
+  }
+
+  //
+  // If Storage Security Command Protocol is installed, then uninstall this protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiStorageSecurityCommandProtocolGuid,
+                  (VOID **) &StorageSecurity,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallProtocolInterface (
+                    Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    &AtaDevice->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        (VOID **) &AtaPassThru,
+        This->DriverBindingHandle,
+        Handle,
+        EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+        );
+      return Status;
+    }
+  }
+
+  ReleaseAtaResources (AtaDevice);
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  UINT16                            Port;
+  UINT16                            PortMultiplierPort;
+
+  //
+  // Test EFI_ATA_PASS_THRU_PROTOCOL on controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Test to see if this ATA Pass Thru Protocol is for a LOGICAL channel
+  //
+  if ((AtaPassThru->Mode->Attributes & EFI_ATA_PASS_THRU_ATTRIBUTES_LOGICAL) == 0) {
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Test RemainingDevicePath is valid or not.
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = AtaPassThru->GetDevice (AtaPassThru, RemainingDevicePath, &Port, &PortMultiplierPort);
+    if (EFI_ERROR (Status)) {
+      //
+      // Close the I/O Abstraction(s) used to perform the supported test
+      //
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiAtaPassThruProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      return Status;
+    }
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+  EFI_DEVICE_PATH_PROTOCOL          *ParentDevicePath;
+  ATA_BUS_DRIVER_DATA               *AtaBusDriverData;
+  UINT16                            Port;
+  UINT16                            PortMultiplierPort;
+
+  AtaBusDriverData = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Report Status Code to indicate ATA bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAtaPassThruProtocolGuid,
+                  (VOID **) &AtaPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Check EFI_ALREADY_STARTED to reuse the original ATA_BUS_DRIVER_DATA.
+  //
+  if (Status != EFI_ALREADY_STARTED) {
+    AtaBusDriverData = AllocateZeroPool (sizeof (ATA_BUS_DRIVER_DATA));
+    if (AtaBusDriverData == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ErrorExit;
+    }
+
+    AtaBusDriverData->AtaPassThru = AtaPassThru;
+    AtaBusDriverData->Controller  = Controller;
+    AtaBusDriverData->ParentDevicePath = ParentDevicePath;
+    AtaBusDriverData->DriverBindingHandle = This->DriverBindingHandle;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    AtaBusDriverData,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto ErrorExit;
+    }
+
+  } else {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &AtaBusDriverData,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AtaBusDriverData = NULL;
+      goto ErrorExit;
+    }
+  }
+
+  //
+  // Report Status Code to indicate detecting devices on bus
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_DETECT),
+    ParentDevicePath
+    );
+
+  if (RemainingDevicePath == NULL) {
+    Port = 0xFFFF;
+    while (TRUE) {
+      Status = AtaPassThru->GetNextPort (AtaPassThru, &Port);
+      if (EFI_ERROR (Status)) {
+        //
+        // We cannot find more legal port then we are done.
+        //
+        break;
+      }
+
+      PortMultiplierPort = 0xFFFF;
+      while (TRUE) {
+        Status = AtaPassThru->GetNextDevice (AtaPassThru, Port, &PortMultiplierPort);
+        if (EFI_ERROR (Status)) {
+          //
+          // We cannot find more legal port multiplier port number for ATA device
+          // on the port, then we are done.
+          //
+          break;
+        }
+        RegisterAtaDevice (AtaBusDriverData, Port, PortMultiplierPort);
+      }
+    }
+    Status = EFI_SUCCESS;
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    Status = AtaPassThru->GetDevice (AtaPassThru, RemainingDevicePath, &Port, &PortMultiplierPort);
+    if (!EFI_ERROR (Status)) {
+      Status = RegisterAtaDevice (AtaBusDriverData,Port, PortMultiplierPort);
+    }
+  }
+
+  return Status;
+
+ErrorExit:
+
+  if (AtaBusDriverData != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiCallerIdGuid,
+           AtaBusDriverData,
+           NULL
+           );
+    FreePool (AtaBusDriverData);
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiAtaPassThruProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+  ATA_BUS_DRIVER_DATA         *AtaBusDriverData;
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &AtaBusDriverData,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiCallerIdGuid,
+            AtaBusDriverData,
+            NULL
+            );
+      FreePool (AtaBusDriverData);
+    }
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiAtaPassThruProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = UnregisterAtaDevice (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_STATUS      Status;
+  ATA_DEVICE      *AtaDevice;
+  EFI_TPL         OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (This);
+
+  Status = ResetAtaDevice (AtaDevice);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Read/Write BufferSize bytes from Lba from/into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context. Either be
+                              block I/O or block I/O2.
+  @param[in]       MediaId    The media ID that the read/write request is for.
+  @param[in]       Lba        The starting logical block address to be read/written.
+                              The caller is responsible for reading/writing to only
+                              legitimate locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination/source buffer for the data.
+  @param[in]       IsBlockIo2 Indicate the calling is from BlockIO or BlockIO2. TURE is
+                              from BlockIO2, FALSE is for BlockIO.
+  @param[in]       IsWrite    Indicates whether it is a write operation.
+
+  @retval EFI_SUCCESS           The data was read/written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be read/written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read/write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read/write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+BlockIoReadWrite (
+  IN     VOID                    *This,
+  IN     UINT32                  MediaId,
+  IN     EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token,
+  IN     UINTN                   BufferSize,
+  OUT    VOID                    *Buffer,
+  IN     BOOLEAN                 IsBlockIo2,
+  IN     BOOLEAN                 IsWrite
+  )
+{
+  ATA_DEVICE                        *AtaDevice;
+  EFI_STATUS                        Status;
+  EFI_TPL                           OldTpl;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+
+  if (IsBlockIo2) {
+   Media     = ((EFI_BLOCK_IO2_PROTOCOL *) This)->Media;
+   AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (This);
+  } else {
+   Media     = ((EFI_BLOCK_IO_PROTOCOL *) This)->Media;
+   AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (This);
+  }
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  //
+  // Check parameters.
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Invoke low level AtaDevice Access Routine.
+  //
+  Status = AccessAtaDevice (AtaDevice, Buffer, Lba, NumberOfBlocks, IsWrite, Token);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, NULL, BufferSize, Buffer, FALSE, FALSE);
+}
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, NULL, BufferSize, Buffer, FALSE, TRUE);
+}
+
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  //
+  // return directly
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the Block Device.
+
+  @param[in]  This                 Indicates a pointer to the calling context.
+  @param[in]  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_STATUS      Status;
+  ATA_DEVICE      *AtaDevice;
+  EFI_TPL         OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  AtaDevice = ATA_DEVICE_FROM_BLOCK_IO2 (This);
+
+  AtaTerminateNonBlockingTask (AtaDevice);
+
+  Status = ResetAtaDevice (AtaDevice);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    Id of the media, changes every time the media is replaced.
+  @param[in]       Lba        The starting Logical Block Address to read from.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination buffer for the data. The caller is
+                              responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The read request was queued if Event is not NULL.
+                                The data was read correctly from the device if
+                                the Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of the
+                                intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, Token, BufferSize, Buffer, TRUE, FALSE);
+}
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    The media ID that the write request is for.
+  @param[in]       Lba        The starting logical block address to be written. The
+                              caller is responsible for writing to only legitimate
+                              locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]       Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  return BlockIoReadWrite ((VOID *) This, MediaId, Lba, Token, BufferSize, Buffer, TRUE, TRUE);
+}
+
+
+/**
+  Flush the Block Device.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token
+  )
+{
+  //
+  // Signal event and return directly.
+  //
+  if (Token != NULL && Token->Event != NULL) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    gBS->SignalEvent (Token->Event);
+  }
+  return EFI_SUCCESS;
+}
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData      Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize  Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS                      Status;
+  ATA_DEVICE                      *AtaDevice;
+
+  AtaDevice = ATA_DEVICE_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (ATA_IDENTIFY_DATA)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, AtaDevice->IdentifyData, sizeof (ATA_IDENTIFY_DATA));
+  }
+  *IdentifyDataSize = sizeof (ATA_IDENTIFY_DATA);
+
+  return Status;
+}
+
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used by the IDE bus driver to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData        Pointer to the SenseData.
+  @param[in, out] SenseDataSize    Size of SenseData in bytes.
+  @param[out]     SenseDataNumber  Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  ATA_DEVICE                   *AtaDevice;
+
+  AtaDevice       = ATA_DEVICE_FROM_DISK_INFO (This);
+  *IdeChannel     = AtaDevice->Port;
+  *IdeDevice      = AtaDevice->PortMultiplierPort;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecurityReceiveData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  )
+{
+  EFI_STATUS                       Status;
+  ATA_DEVICE                       *Private;
+  EFI_TPL                          OldTpl;
+
+  DEBUG ((EFI_D_INFO, "EFI Storage Security Protocol - Read\n"));
+  if ((PayloadBuffer == NULL || PayloadTransferSize == NULL) && PayloadBufferSize != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = ATA_DEVICE_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Private->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Private->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Status = TrustTransferAtaDevice (
+             Private,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             FALSE,
+             Timeout,
+             PayloadTransferSize
+             );
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  ATA_DEVICE                       *Private;
+  EFI_TPL                          OldTpl;
+
+  DEBUG ((EFI_D_INFO, "EFI Storage Security Protocol - Send\n"));
+  if ((PayloadBuffer == NULL) && (PayloadBufferSize != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = ATA_DEVICE_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Private->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  Status = TrustTransferAtaDevice (
+             Private,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             TRUE,
+             Timeout,
+             NULL
+             );
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The user Entry Point for module AtaBus. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAtaBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gAtaBusDriverBinding,
+             ImageHandle,
+             &gAtaBusComponentName,
+             &gAtaBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h
new file mode 100644
index 0000000000..26ec70d30a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBus.h
@@ -0,0 +1,1081 @@
+/** @file
+  Master header file for ATA Bus Driver.
+
+  This file defines common data structures, macro definitions and some module
+  internal function header files.
+
+  Copyright (c) 2009 - 2015, 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.
+
+**/
+
+#ifndef _ATA_BUS_H_
+#define _ATA_BUS_H_
+
+#include <Uefi.h>
+
+#include <Protocol/AtaPassThru.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Protocol/DiskInfo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/StorageSecurityCommand.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/TimerLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Atapi.h>
+
+//
+// Time out value for ATA pass through protocol
+//
+#define ATA_TIMEOUT                       EFI_TIMER_PERIOD_SECONDS (3)
+
+//
+// Maximum number of times to retry ATA command
+//
+#define MAX_RETRY_TIMES                   3
+
+//
+// The maximum total sectors count in 28 bit addressing mode
+//
+#define MAX_28BIT_ADDRESSING_CAPACITY     0xfffffff
+
+//
+// The maximum ATA transaction sector count in 28 bit addressing mode.
+//
+#define MAX_28BIT_TRANSFER_BLOCK_NUM      0x100
+
+//
+// The maximum ATA transaction sector count in 48 bit addressing mode.
+//
+//#define MAX_48BIT_TRANSFER_BLOCK_NUM      0x10000
+
+//
+// BugBug: if the TransferLength is equal with 0x10000 (the 48bit max length),
+// there is a bug that even the register interrupt bit has been sit, the buffer
+// seems not ready. Change the Maximum Sector Numbers to 0xFFFF to work round
+// this issue.
+//
+#define MAX_48BIT_TRANSFER_BLOCK_NUM      0xFFFF
+
+//
+// The maximum model name in ATA identify data
+//
+#define MAX_MODEL_NAME_LEN                40
+
+#define ATA_TASK_SIGNATURE                SIGNATURE_32 ('A', 'T', 'S', 'K')
+#define ATA_DEVICE_SIGNATURE              SIGNATURE_32 ('A', 'B', 'I', 'D')
+#define ATA_SUB_TASK_SIGNATURE            SIGNATURE_32 ('A', 'S', 'T', 'S')
+#define IS_ALIGNED(addr, size)            (((UINTN) (addr) & (size - 1)) == 0)
+
+//
+// ATA bus data structure for ATA controller
+//
+typedef struct {
+  EFI_ATA_PASS_THRU_PROTOCOL  *AtaPassThru;
+  EFI_HANDLE                  Controller;
+  EFI_DEVICE_PATH_PROTOCOL    *ParentDevicePath;
+  EFI_HANDLE                  DriverBindingHandle;
+} ATA_BUS_DRIVER_DATA;
+
+//
+// ATA device data structure for each child device
+//
+typedef struct {
+  UINT32                                Signature;
+
+  EFI_HANDLE                            Handle;
+  EFI_BLOCK_IO_PROTOCOL                 BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL                BlockIo2;
+  EFI_BLOCK_IO_MEDIA                    BlockMedia;
+  EFI_DISK_INFO_PROTOCOL                DiskInfo;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL StorageSecurity;
+
+  ATA_BUS_DRIVER_DATA                   *AtaBusDriverData;
+  UINT16                                Port;
+  UINT16                                PortMultiplierPort;
+
+  //
+  // Buffer for the execution of ATA pass through protocol
+  //
+  EFI_ATA_PASS_THRU_COMMAND_PACKET      Packet;
+  EFI_ATA_COMMAND_BLOCK                 Acb;
+  EFI_ATA_STATUS_BLOCK                  *Asb;
+
+  BOOLEAN                               UdmaValid;
+  BOOLEAN                               Lba48Bit;
+
+  //
+  // Cached data for ATA identify data
+  //
+  ATA_IDENTIFY_DATA                     *IdentifyData;
+
+  EFI_UNICODE_STRING_TABLE              *ControllerNameTable;
+  CHAR16                                ModelName[MAX_MODEL_NAME_LEN + 1];
+
+  LIST_ENTRY                            AtaTaskList;
+  LIST_ENTRY                            AtaSubTaskList;
+  BOOLEAN                               Abort;
+} ATA_DEVICE;
+
+//
+// Sub-Task for the non blocking I/O
+//
+typedef struct {
+  UINT32                            Signature;
+  ATA_DEVICE                        *AtaDevice;
+  EFI_BLOCK_IO2_TOKEN               *Token;
+  UINTN                             *UnsignalledEventCount;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  Packet;
+  BOOLEAN                           *IsError;// Indicate whether meeting error during source allocation for new task.
+  LIST_ENTRY                        TaskEntry;
+} ATA_BUS_ASYN_SUB_TASK;
+
+//
+// Task for the non blocking I/O
+//
+typedef struct {
+  UINT32                            Signature;
+  EFI_BLOCK_IO2_TOKEN               *Token;
+  ATA_DEVICE                        *AtaDevice;
+  UINT8                             *Buffer;
+  EFI_LBA                           StartLba;
+  UINTN                             NumberOfBlocks;
+  BOOLEAN                           IsWrite;
+  LIST_ENTRY                        TaskEntry;
+} ATA_BUS_ASYN_TASK;
+
+#define ATA_DEVICE_FROM_BLOCK_IO(a)         CR (a, ATA_DEVICE, BlockIo, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_BLOCK_IO2(a)        CR (a, ATA_DEVICE, BlockIo2, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_DISK_INFO(a)        CR (a, ATA_DEVICE, DiskInfo, ATA_DEVICE_SIGNATURE)
+#define ATA_DEVICE_FROM_STORAGE_SECURITY(a) CR (a, ATA_DEVICE, StorageSecurity, ATA_DEVICE_SIGNATURE)
+#define ATA_ASYN_SUB_TASK_FROM_ENTRY(a)     CR (a, ATA_BUS_ASYN_SUB_TASK, TaskEntry, ATA_SUB_TASK_SIGNATURE)
+#define ATA_ASYN_TASK_FROM_ENTRY(a)         CR (a, ATA_BUS_ASYN_TASK, TaskEntry, ATA_TASK_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL        gAtaBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL        gAtaBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL       gAtaBusComponentName2;
+
+/**
+  Allocates an aligned buffer for ATA device.
+
+  This function allocates an aligned buffer for the ATA device to perform
+  ATA pass through operations. The alignment requirement is from ATA pass
+  through interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN ATA_DEVICE               *AtaDevice,
+  IN UINTN                    BufferSize
+  );
+
+/**
+  Frees an aligned buffer for ATA device.
+
+  This function frees an aligned buffer for the ATA device to perform
+  ATA pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  );
+
+/**
+  Free SubTask.
+
+  @param[in, out]  Task      Pointer to task to be freed.
+
+**/
+VOID
+EFIAPI
+FreeAtaSubTask (
+  IN OUT ATA_BUS_ASYN_SUB_TASK  *Task
+  );
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.ResetDevice().
+
+  This function wraps the ResetDevice() invocation for ATA pass through function
+  for an ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+ResetAtaDevice (
+  IN ATA_DEVICE                           *AtaDevice
+  );
+
+
+/**
+  Discovers whether it is a valid ATA device.
+
+  This function issues ATA_CMD_IDENTIFY_DRIVE command to the ATA device to identify it.
+  If the command is executed successfully, it then identifies it and initializes
+  the Media information in Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+  @return others            Some error occurs when discovering the ATA device.
+
+**/
+EFI_STATUS
+DiscoverAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  );
+
+/**
+  Read or write a number of blocks from ATA device.
+
+  This function performs ATA pass through transactions to read/write data from/to
+  ATA device. It may separate the read/write request into several ATA pass through
+  transactions.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       NumberOfBlocks  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in, out]  Token           A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+AccessAtaDevice(
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT UINT8                      *Buffer,
+  IN EFI_LBA                        StartLba,
+  IN UINTN                          NumberOfBlocks,
+  IN BOOLEAN                        IsWrite,
+  IN OUT EFI_BLOCK_IO2_TOKEN        *Token
+  );
+
+/**
+  Trust transfer data from/to ATA device.
+
+  This function performs one ATA pass through transaction to do a trust transfer from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param  AtaDevice                    The ATA child device involved for the operation.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+EFIAPI
+TrustTransferAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT VOID                       *Buffer,
+  IN UINT8                          SecurityProtocolId,
+  IN UINT16                         SecurityProtocolSpecificData,
+  IN UINTN                          TransferLength,
+  IN BOOLEAN                        IsTrustSend,
+  IN UINT64                         Timeout,
+  OUT UINTN                         *TransferLengthOut
+  );
+
+//
+// Protocol interface prototypes
+//
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+/**
+  Reset the Block Device throught Block I/O2 protocol.
+
+  @param[in]  This                 Indicates a pointer to the calling context.
+  @param[in]  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    Id of the media, changes every time the media is replaced.
+  @param[in]       Lba        The starting Logical Block Address to read from.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[out]      Buffer     A pointer to the destination buffer for the data. The caller is
+                              responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The read request was queued if Event is not NULL.
+                                The data was read correctly from the device if
+                                the Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of the
+                                intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoReadBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    The media ID that the write request is for.
+  @param[in]       Lba        The starting logical block address to be written. The
+                              caller is responsible for writing to only legitimate
+                              locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]       Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoWriteBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBlockIoFlushBlocksEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN  *Token
+  );
+
+/**
+  Terminate any in-flight non-blocking I/O requests by signaling an EFI_ABORTED
+  in the TransactionStatus member of the EFI_BLOCK_IO2_TOKEN for the non-blocking
+  I/O. After that it is safe to free any Token or Buffer data structures that
+  were allocated to initiate the non-blockingI/O requests that were in-flight for
+  this device.
+
+  @param[in]  AtaDevice     The ATA child device involved for the operation.
+
+**/
+VOID
+EFIAPI
+AtaTerminateNonBlockingTask (
+  IN ATA_DEVICE               *AtaDevice
+  );
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData      Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize  Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used by the IDE bus driver to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This             Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData        Pointer to the SenseData.
+  @param[in, out] SenseDataSize    Size of SenseData in bytes.
+  @param[out]     SenseDataNumber  Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecurityReceiveData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  );
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  );
+
+/**
+  Send TPer Reset command to reset eDrive to lock all protected bands.
+  Typically, there are 2 mechanism for resetting eDrive. They are:
+  1. TPer Reset through IEEE 1667 protocol.
+  2. TPer Reset through native TCG protocol.
+  This routine will detect what protocol the attached eDrive comform to, TCG or
+  IEEE 1667 protocol. Then send out TPer Reset command separately.
+
+  @param[in] AtaDevice    ATA_DEVICE pointer.
+
+**/
+VOID
+InitiateTPerReset (
+  IN   ATA_DEVICE       *AtaDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf
new file mode 100644
index 0000000000..4aab75bab7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf
@@ -0,0 +1,77 @@
+## @file
+#  ATA Bus driver to enumerate and identfy ATA devices.
+#
+#  This driver follows UEFI driver model and layers on ATA Pass Thru protocol defined
+#  in UEFI spec 2.2. It installs Block IO and Disk Info protocol for each ATA device
+#  it enumerates and identifies successfully.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AtaBusDxe
+  MODULE_UNI_FILE                = AtaBusDxe.uni
+  FILE_GUID                      = 19DF145A-B1D4-453f-8507-38816676D7F6
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeAtaBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gAtaBusDriverBinding
+#  COMPONENT_NAME                =  gAtaBusComponentName
+#  COMPONENT_NAME2               =  gAtaBusComponentName2
+#
+#
+
+[Sources]
+  AtaBus.h
+  AtaBus.c
+  AtaPassThruExecute.c
+  ComponentName.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  TimerLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiDiskInfoIdeInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoAhciInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+
+[Protocols]
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiBlockIo2ProtocolGuid                      ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid
+  gEfiAtaPassThruProtocolGuid                   ## TO_START
+  gEfiStorageSecurityCommandProtocolGuid        ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AtaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni
new file mode 100644
index 0000000000..45a8be7713
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni
new file mode 100644
index 0000000000..269bbf9780
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c
new file mode 100644
index 0000000000..a3008f92d9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/AtaPassThruExecute.c
@@ -0,0 +1,1072 @@
+/** @file
+  This file implements ATA pass through transaction for ATA bus driver.
+
+  This file implements the low level execution of ATA pass through transaction.
+  It transforms the high level identity, read/write, reset command to ATA pass
+  through command and protocol.
+
+  NOTE: This file also implements the StorageSecurityCommandProtocol(SSP). For input
+  parameter SecurityProtocolSpecificData, ATA spec has no explicitly definition
+  for Security Protocol Specific layout. This implementation uses big endian for
+  Cylinder register.
+
+  Copyright (c) 2009 - 2013, 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 "AtaBus.h"
+
+#define ATA_CMD_TRUST_NON_DATA    0x5B
+#define ATA_CMD_TRUST_RECEIVE     0x5C
+#define ATA_CMD_TRUST_RECEIVE_DMA 0x5D
+#define ATA_CMD_TRUST_SEND        0x5E
+#define ATA_CMD_TRUST_SEND_DMA    0x5F
+
+//
+// Look up table (UdmaValid, IsWrite) for EFI_ATA_PASS_THRU_CMD_PROTOCOL
+//
+EFI_ATA_PASS_THRU_CMD_PROTOCOL mAtaPassThruCmdProtocols[][2] = {
+  {
+    EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN,
+    EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT
+  },
+  {
+    EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_IN,
+    EFI_ATA_PASS_THRU_PROTOCOL_UDMA_DATA_OUT,
+  }
+};
+
+//
+// Look up table (UdmaValid, Lba48Bit, IsIsWrite) for ATA_CMD
+//
+UINT8 mAtaCommands[][2][2] = {
+  {
+    {
+      ATA_CMD_READ_SECTORS,            // 28-bit LBA; PIO read
+      ATA_CMD_WRITE_SECTORS            // 28-bit LBA; PIO write
+    },
+    {
+      ATA_CMD_READ_SECTORS_EXT,        // 48-bit LBA; PIO read
+      ATA_CMD_WRITE_SECTORS_EXT        // 48-bit LBA; PIO write
+    }
+  },
+  {
+    {
+      ATA_CMD_READ_DMA,                // 28-bit LBA; DMA read
+      ATA_CMD_WRITE_DMA                // 28-bit LBA; DMA write
+    },
+    {
+      ATA_CMD_READ_DMA_EXT,            // 48-bit LBA; DMA read
+      ATA_CMD_WRITE_DMA_EXT            // 48-bit LBA; DMA write
+    }
+  }
+};
+
+//
+// Look up table (UdmaValid, IsTrustSend) for ATA_CMD
+//
+UINT8 mAtaTrustCommands[2][2] = {
+  {
+    ATA_CMD_TRUST_RECEIVE,            // PIO read
+    ATA_CMD_TRUST_SEND                // PIO write
+  },
+  {
+    ATA_CMD_TRUST_RECEIVE_DMA,        // DMA read
+    ATA_CMD_TRUST_SEND_DMA            // DMA write
+  }
+};
+
+
+//
+// Look up table (Lba48Bit) for maximum transfer block number
+//
+UINTN mMaxTransferBlockNumber[] = {
+  MAX_28BIT_TRANSFER_BLOCK_NUM,
+  MAX_48BIT_TRANSFER_BLOCK_NUM
+};
+
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+  This function wraps the PassThru() invocation for ATA pass through function
+  for an ATA device. It assembles the ATA pass through command packet for ATA
+  transaction.
+
+  @param[in, out]  AtaDevice   The ATA child device involved for the operation.
+  @param[in, out]  TaskPacket  Pointer to a Pass Thru Command Packet. Optional,
+                               if it is NULL, blocking mode, and use the packet
+                               in AtaDevice. If it is not NULL, non blocking mode,
+                               and pass down this Packet.
+  @param[in, out]  Event       If Event is NULL, then blocking I/O is performed.
+                               If Event is not NULL and non-blocking I/O is
+                               supported,then non-blocking I/O is performed,
+                               and Event will be signaled when the write
+                               request is completed.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+AtaDevicePassThru (
+  IN OUT ATA_DEVICE                       *AtaDevice,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *TaskPacket, OPTIONAL
+  IN OUT EFI_EVENT                        Event OPTIONAL
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_ATA_PASS_THRU_PROTOCOL              *AtaPassThru;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET        *Packet;
+
+  //
+  // Assemble packet. If it is non blocking mode, the Ata driver should keep each
+  // subtask and clean them when the event is signaled.
+  //
+  if (TaskPacket != NULL) {
+    Packet = TaskPacket;
+    Packet->Asb = AllocateAlignedBuffer (AtaDevice, sizeof (EFI_ATA_STATUS_BLOCK));
+    if (Packet->Asb == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    CopyMem (Packet->Asb, AtaDevice->Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+    Packet->Acb = AllocateCopyPool (sizeof (EFI_ATA_COMMAND_BLOCK), &AtaDevice->Acb);
+  } else {
+    Packet = &AtaDevice->Packet;
+    Packet->Asb = AtaDevice->Asb;
+    Packet->Acb = &AtaDevice->Acb;
+  }
+
+  AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+
+  Status = AtaPassThru->PassThru (
+                          AtaPassThru,
+                          AtaDevice->Port,
+                          AtaDevice->PortMultiplierPort,
+                          Packet,
+                          Event
+                          );
+  //
+  // Ensure ATA pass through caller and callee have the same
+  // interpretation of ATA pass through protocol.
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+  ASSERT (Status != EFI_BAD_BUFFER_SIZE);
+
+  return Status;
+}
+
+
+/**
+  Wrapper for EFI_ATA_PASS_THRU_PROTOCOL.ResetDevice().
+
+  This function wraps the ResetDevice() invocation for ATA pass through function
+  for an ATA device.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The return status from EFI_ATA_PASS_THRU_PROTOCOL.PassThru().
+
+**/
+EFI_STATUS
+ResetAtaDevice (
+  IN ATA_DEVICE                           *AtaDevice
+  )
+{
+  EFI_ATA_PASS_THRU_PROTOCOL              *AtaPassThru;
+
+  AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    AtaDevice->AtaBusDriverData->ParentDevicePath
+    );
+
+  return AtaPassThru->ResetDevice (
+                        AtaPassThru,
+                        AtaDevice->Port,
+                        AtaDevice->PortMultiplierPort
+                        );
+}
+
+
+/**
+  Prints ATA model name to ATA device structure.
+
+  This function converts ATA device model name from ATA identify data
+  to a string in ATA device structure. It needs to change the character
+  order in the original model name string.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+**/
+VOID
+PrintAtaModelName (
+  IN OUT ATA_DEVICE  *AtaDevice
+  )
+{
+  UINTN   Index;
+  CHAR8   *Source;
+  CHAR16  *Destination;
+
+  Source = AtaDevice->IdentifyData->ModelName;
+  Destination = AtaDevice->ModelName;
+
+  //
+  // Swap the byte order in the original module name.
+  //
+  for (Index = 0; Index < MAX_MODEL_NAME_LEN; Index += 2) {
+    Destination[Index]      = Source[Index + 1];
+    Destination[Index + 1]  = Source[Index];
+  }
+  AtaDevice->ModelName[MAX_MODEL_NAME_LEN] = L'\0';
+}
+
+
+/**
+  Gets ATA device Capacity according to ATA 6.
+
+  This function returns the capacity of the ATA device if it follows
+  ATA 6 to support 48 bit addressing.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @return The capacity of the ATA device or 0 if the device does not support
+          48-bit addressing defined in ATA 6.
+
+**/
+EFI_LBA
+GetAtapi6Capacity (
+  IN ATA_DEVICE                 *AtaDevice
+  )
+{
+  EFI_LBA                       Capacity;
+  EFI_LBA                       TmpLba;
+  UINTN                         Index;
+  ATA_IDENTIFY_DATA             *IdentifyData;
+
+  IdentifyData = AtaDevice->IdentifyData;
+  if ((IdentifyData->command_set_supported_83 & BIT10) == 0) {
+    //
+    // The device doesn't support 48 bit addressing
+    //
+    return 0;
+  }
+
+  //
+  // 48 bit address feature set is supported, get maximum capacity
+  //
+  Capacity = 0;
+  for (Index = 0; Index < 4; Index++) {
+    //
+    // Lower byte goes first: word[100] is the lowest word, word[103] is highest
+    //
+    TmpLba = IdentifyData->maximum_lba_for_48bit_addressing[Index];
+    Capacity |= LShiftU64 (TmpLba, 16 * Index);
+  }
+
+  return Capacity;
+}
+
+
+/**
+  Identifies ATA device via the Identify data.
+
+  This function identifies the ATA device and initializes the Media information in
+  Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_UNSUPPORTED   The device is not a valid ATA device (hard disk).
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+
+**/
+EFI_STATUS
+IdentifyAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  )
+{
+  ATA_IDENTIFY_DATA                 *IdentifyData;
+  EFI_BLOCK_IO_MEDIA                *BlockMedia;
+  EFI_LBA                           Capacity;
+  UINT16                            PhyLogicSectorSupport;
+  UINT16                            UdmaMode;
+
+  IdentifyData = AtaDevice->IdentifyData;
+
+  if ((IdentifyData->config & BIT15) != 0) {
+    //
+    // This is not an hard disk
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  DEBUG ((EFI_D_INFO, "AtaBus - Identify Device: Port %x PortMultiplierPort %x\n", AtaDevice->Port, AtaDevice->PortMultiplierPort));
+
+  //
+  // Check whether the WORD 88 (supported UltraDMA by drive) is valid
+  //
+  if ((IdentifyData->field_validity & BIT2) != 0) {
+    UdmaMode = IdentifyData->ultra_dma_mode;
+    if ((UdmaMode & (BIT0 | BIT1 | BIT2 | BIT3 | BIT4 | BIT5 | BIT6)) != 0) {
+      //
+      // If BIT0~BIT6 is selected, then UDMA is supported
+      //
+      AtaDevice->UdmaValid = TRUE;
+    }
+  }
+
+  Capacity = GetAtapi6Capacity (AtaDevice);
+  if (Capacity > MAX_28BIT_ADDRESSING_CAPACITY) {
+    //
+    // Capacity exceeds 120GB. 48-bit addressing is really needed
+    //
+    AtaDevice->Lba48Bit = TRUE;
+  } else {
+    //
+    // This is a hard disk <= 120GB capacity, treat it as normal hard disk
+    //
+    Capacity = ((UINT32)IdentifyData->user_addressable_sectors_hi << 16) | IdentifyData->user_addressable_sectors_lo;
+    AtaDevice->Lba48Bit = FALSE;
+  }
+
+  //
+  // Block Media Information:
+  //
+  BlockMedia = &AtaDevice->BlockMedia;
+  BlockMedia->LastBlock = Capacity - 1;
+  BlockMedia->IoAlign = AtaDevice->AtaBusDriverData->AtaPassThru->Mode->IoAlign;
+  //
+  // Check whether Long Physical Sector Feature is supported
+  //
+  PhyLogicSectorSupport = IdentifyData->phy_logic_sector_support;
+  if ((PhyLogicSectorSupport & (BIT14 | BIT15)) == BIT14) {
+    //
+    // Check whether one physical block contains multiple physical blocks
+    //
+    if ((PhyLogicSectorSupport & BIT13) != 0) {
+      BlockMedia->LogicalBlocksPerPhysicalBlock = (UINT32) (1 << (PhyLogicSectorSupport & 0x000f));
+      //
+      // Check lowest alignment of logical blocks within physical block
+      //
+      if ((IdentifyData->alignment_logic_in_phy_blocks & (BIT14 | BIT15)) == BIT14) {
+        BlockMedia->LowestAlignedLba = (EFI_LBA) ((BlockMedia->LogicalBlocksPerPhysicalBlock - ((UINT32)IdentifyData->alignment_logic_in_phy_blocks & 0x3fff)) %
+          BlockMedia->LogicalBlocksPerPhysicalBlock);
+      }
+    }
+    //
+    // Check logical block size
+    //
+    if ((PhyLogicSectorSupport & BIT12) != 0) {
+      BlockMedia->BlockSize = (UINT32) (((IdentifyData->logic_sector_size_hi << 16) | IdentifyData->logic_sector_size_lo) * sizeof (UINT16));
+    }
+    AtaDevice->BlockIo.Revision = EFI_BLOCK_IO_PROTOCOL_REVISION2;
+  }
+  //
+  // Get ATA model name from identify data structure.
+  //
+  PrintAtaModelName (AtaDevice);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Discovers whether it is a valid ATA device.
+
+  This function issues ATA_CMD_IDENTIFY_DRIVE command to the ATA device to identify it.
+  If the command is executed successfully, it then identifies it and initializes
+  the Media information in Block IO protocol interface.
+
+  @param  AtaDevice         The ATA child device involved for the operation.
+
+  @retval EFI_SUCCESS       The device is successfully identified and Media information
+                            is correctly initialized.
+  @return others            Some error occurs when discovering the ATA device.
+
+**/
+EFI_STATUS
+DiscoverAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  UINTN                             Retry;
+
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  Acb->AtaCommand = ATA_CMD_IDENTIFY_DRIVE;
+  Acb->AtaDeviceHead = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  Packet->InDataBuffer = AtaDevice->IdentifyData;
+  Packet->InTransferLength = sizeof (ATA_IDENTIFY_DATA);
+  Packet->Protocol = EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN;
+  Packet->Length   = EFI_ATA_PASS_THRU_LENGTH_BYTES | EFI_ATA_PASS_THRU_LENGTH_SECTOR_COUNT;
+  Packet->Timeout  = ATA_TIMEOUT;
+
+  Retry = MAX_RETRY_TIMES;
+  do {
+    Status = AtaDevicePassThru (AtaDevice, NULL, NULL);
+    if (!EFI_ERROR (Status)) {
+      //
+      // The command is issued successfully
+      //
+      Status = IdentifyAtaDevice (AtaDevice);
+      return Status;
+    }
+  } while (Retry-- > 0);
+
+  return Status;
+}
+
+/**
+  Transfer data from ATA device.
+
+  This function performs one ATA pass through transaction to transfer data from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  TaskPacket      Pointer to a Pass Thru Command Packet. Optional,
+                                   if it is NULL, blocking mode, and use the packet
+                                   in AtaDevice. If it is not NULL, non blocking mode,
+                                   and pass down this Packet.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       TransferLength  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in]       Event           If Event is NULL, then blocking I/O is performed.
+                                   If Event is not NULL and non-blocking I/O is
+                                   supported,then non-blocking I/O is performed,
+                                   and Event will be signaled when the write
+                                   request is completed.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+TransferAtaDevice (
+  IN OUT ATA_DEVICE                       *AtaDevice,
+  IN OUT EFI_ATA_PASS_THRU_COMMAND_PACKET *TaskPacket, OPTIONAL
+  IN OUT VOID                             *Buffer,
+  IN EFI_LBA                              StartLba,
+  IN UINT32                               TransferLength,
+  IN BOOLEAN                              IsWrite,
+  IN EFI_EVENT                            Event OPTIONAL
+  )
+{
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+
+  //
+  // Ensure AtaDevice->UdmaValid, AtaDevice->Lba48Bit and IsWrite are valid boolean values
+  //
+  ASSERT ((UINTN) AtaDevice->UdmaValid < 2);
+  ASSERT ((UINTN) AtaDevice->Lba48Bit < 2);
+  ASSERT ((UINTN) IsWrite < 2);
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  Acb->AtaCommand = mAtaCommands[AtaDevice->UdmaValid][AtaDevice->Lba48Bit][IsWrite];
+  Acb->AtaSectorNumber = (UINT8) StartLba;
+  Acb->AtaCylinderLow = (UINT8) RShiftU64 (StartLba, 8);
+  Acb->AtaCylinderHigh = (UINT8) RShiftU64 (StartLba, 16);
+  Acb->AtaDeviceHead = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+  Acb->AtaSectorCount = (UINT8) TransferLength;
+  if (AtaDevice->Lba48Bit) {
+    Acb->AtaSectorNumberExp = (UINT8) RShiftU64 (StartLba, 24);
+    Acb->AtaCylinderLowExp = (UINT8) RShiftU64 (StartLba, 32);
+    Acb->AtaCylinderHighExp = (UINT8) RShiftU64 (StartLba, 40);
+    Acb->AtaSectorCountExp = (UINT8) (TransferLength >> 8);
+  } else {
+    Acb->AtaDeviceHead = (UINT8) (Acb->AtaDeviceHead | RShiftU64 (StartLba, 24));
+  }
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  if (TaskPacket != NULL) {
+    Packet = ZeroMem (TaskPacket, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  } else {
+    Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  }
+
+  if (IsWrite) {
+    Packet->OutDataBuffer = Buffer;
+    Packet->OutTransferLength = TransferLength;
+  } else {
+    Packet->InDataBuffer = Buffer;
+    Packet->InTransferLength = TransferLength;
+  }
+
+  Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsWrite];
+  Packet->Length = EFI_ATA_PASS_THRU_LENGTH_SECTOR_COUNT;
+  //
+  // |------------------------|-----------------|------------------------|-----------------|
+  // | ATA PIO Transfer Mode  |  Transfer Rate  | ATA DMA Transfer Mode  |  Transfer Rate  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 0       |  3.3Mbytes/sec  | Single-word DMA Mode 0 |  2.1Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 1       |  5.2Mbytes/sec  | Single-word DMA Mode 1 |  4.2Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 2       |  8.3Mbytes/sec  | Single-word DMA Mode 2 |  8.4Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 3       | 11.1Mbytes/sec  | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  // |       PIO Mode 4       | 16.6Mbytes/sec  | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |
+  // |------------------------|-----------------|------------------------|-----------------|
+  //
+  // As AtaBus is used to manage ATA devices, we have to use the lowest transfer rate to
+  // calculate the possible maximum timeout value for each read/write operation.
+  // The timout value is rounded up to nearest integar and here an additional 30s is added
+  // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+  // commands in the Standby/Idle mode.
+  //
+  if (AtaDevice->UdmaValid) {
+    //
+    // Calculate the maximum timeout value for DMA read/write operation.
+    //
+    Packet->Timeout  = EFI_TIMER_PERIOD_SECONDS (DivU64x32 (MultU64x32 (TransferLength, AtaDevice->BlockMedia.BlockSize), 2100000) + 31);
+  } else {
+    //
+    // Calculate the maximum timeout value for PIO read/write operation
+    //
+    Packet->Timeout  = EFI_TIMER_PERIOD_SECONDS (DivU64x32 (MultU64x32 (TransferLength, AtaDevice->BlockMedia.BlockSize), 3300000) + 31);
+  }
+
+  return AtaDevicePassThru (AtaDevice, TaskPacket, Event);
+}
+
+/**
+  Free SubTask.
+
+  @param[in, out]  Task      Pointer to task to be freed.
+
+**/
+VOID
+EFIAPI
+FreeAtaSubTask (
+  IN OUT ATA_BUS_ASYN_SUB_TASK  *Task
+  )
+{
+  if (Task->Packet.Asb != NULL) {
+    FreeAlignedBuffer (Task->Packet.Asb, sizeof (EFI_ATA_STATUS_BLOCK));
+  }
+  if (Task->Packet.Acb != NULL) {
+    FreePool (Task->Packet.Acb);
+  }
+
+  FreePool (Task);
+}
+
+/**
+  Terminate any in-flight non-blocking I/O requests by signaling an EFI_ABORTED
+  in the TransactionStatus member of the EFI_BLOCK_IO2_TOKEN for the non-blocking
+  I/O. After that it is safe to free any Token or Buffer data structures that
+  were allocated to initiate the non-blockingI/O requests that were in-flight for
+  this device.
+
+  @param[in]  AtaDevice     The ATA child device involved for the operation.
+
+**/
+VOID
+EFIAPI
+AtaTerminateNonBlockingTask (
+  IN ATA_DEVICE               *AtaDevice
+  )
+{
+  BOOLEAN               SubTaskEmpty;
+  EFI_TPL               OldTpl;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *List;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  //
+  // Abort all executing tasks from now.
+  //
+  AtaDevice->Abort = TRUE;
+
+  List = &AtaDevice->AtaTaskList;
+  for (Entry = GetFirstNode (List); !IsNull (List, Entry);) {
+    AtaTask  = ATA_ASYN_TASK_FROM_ENTRY (Entry);
+    AtaTask->Token->TransactionStatus = EFI_ABORTED;
+    gBS->SignalEvent (AtaTask->Token->Event);
+
+    Entry = RemoveEntryList (Entry);
+    FreePool (AtaTask);
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  do {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    //
+    // Wait for executing subtasks done.
+    //
+    SubTaskEmpty = IsListEmpty (&AtaDevice->AtaSubTaskList);
+    gBS->RestoreTPL (OldTpl);
+  } while (!SubTaskEmpty);
+
+  //
+  // Aborting operation has been done. From now on, don't need to abort normal operation.
+  //  
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  AtaDevice->Abort = FALSE;
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Call back funtion when the event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the
+                        Event.
+
+**/
+VOID
+EFIAPI
+AtaNonBlockingCallBack (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  ATA_BUS_ASYN_SUB_TASK *Task;
+  ATA_BUS_ASYN_TASK     *AtaTask;
+  ATA_DEVICE            *AtaDevice;
+  LIST_ENTRY            *Entry;
+  EFI_STATUS            Status;
+
+  Task = (ATA_BUS_ASYN_SUB_TASK *) Context;
+  gBS->CloseEvent (Event);
+
+  AtaDevice = Task->AtaDevice;
+
+  //
+  // Check the command status.
+  // If there is error during the sub task source allocation, the error status
+  // should be returned to the caller directly, so here the Task->Token may already
+  // be deleted by the caller and no need to update the status.
+  //
+  if ((!(*Task->IsError)) && ((Task->Packet.Asb->AtaStatus & 0x01) == 0x01)) {
+    Task->Token->TransactionStatus = EFI_DEVICE_ERROR;
+  }
+
+  if (AtaDevice->Abort) {
+    Task->Token->TransactionStatus = EFI_ABORTED;
+  }
+
+  DEBUG ((
+    EFI_D_BLKIO,
+    "NON-BLOCKING EVENT FINISHED!- STATUS = %r\n",
+    Task->Token->TransactionStatus
+    ));
+
+  //
+  // Reduce the SubEventCount, till it comes to zero.
+  //
+  (*Task->UnsignalledEventCount) --;
+  DEBUG ((EFI_D_BLKIO, "UnsignalledEventCount = %d\n", *Task->UnsignalledEventCount));
+
+  //
+  // Remove the SubTask from the Task list.
+  //
+  RemoveEntryList (&Task->TaskEntry);
+  if ((*Task->UnsignalledEventCount) == 0) {
+    //
+    // All Sub tasks are done, then signal the upper layer event.
+    // Except there is error during the sub task source allocation.
+    //
+    if (!(*Task->IsError)) {
+      gBS->SignalEvent (Task->Token->Event);
+      DEBUG ((EFI_D_BLKIO, "Signal the upper layer event!\n"));
+    }
+
+    FreePool (Task->UnsignalledEventCount);
+    FreePool (Task->IsError);
+
+
+    //
+    // Finish all subtasks and move to the next task in AtaTaskList.
+    //
+    if (!IsListEmpty (&AtaDevice->AtaTaskList)) {
+      Entry   = GetFirstNode (&AtaDevice->AtaTaskList);
+      AtaTask = ATA_ASYN_TASK_FROM_ENTRY (Entry);
+      DEBUG ((EFI_D_BLKIO, "Start to embark a new Ata Task\n"));
+      DEBUG ((EFI_D_BLKIO, "AtaTask->NumberOfBlocks = %x; AtaTask->Token=%x\n", AtaTask->NumberOfBlocks, AtaTask->Token));
+      Status = AccessAtaDevice (
+                 AtaTask->AtaDevice,
+                 AtaTask->Buffer,
+                 AtaTask->StartLba,
+                 AtaTask->NumberOfBlocks,
+                 AtaTask->IsWrite,
+                 AtaTask->Token
+                 );
+      if (EFI_ERROR (Status)) {
+        AtaTask->Token->TransactionStatus = Status;
+        gBS->SignalEvent (AtaTask->Token->Event);
+      }
+      RemoveEntryList (Entry);
+      FreePool (AtaTask);
+    }
+  }
+
+  DEBUG ((
+    EFI_D_BLKIO,
+    "PACKET INFO: Write=%s, Length=%x, LowCylinder=%x, HighCylinder=%x, SectionNumber=%x\n",
+    Task->Packet.OutDataBuffer != NULL ? L"YES" : L"NO",
+    Task->Packet.OutDataBuffer != NULL ? Task->Packet.OutTransferLength : Task->Packet.InTransferLength,
+    Task->Packet.Acb->AtaCylinderLow,
+    Task->Packet.Acb->AtaCylinderHigh,
+    Task->Packet.Acb->AtaSectorCount
+    ));
+
+  //
+  // Free the buffer of SubTask.
+  //
+  FreeAtaSubTask (Task);
+}
+
+/**
+  Read or write a number of blocks from ATA device.
+
+  This function performs ATA pass through transactions to read/write data from/to
+  ATA device. It may separate the read/write request into several ATA pass through
+  transactions.
+
+  @param[in, out]  AtaDevice       The ATA child device involved for the operation.
+  @param[in, out]  Buffer          The pointer to the current transaction buffer.
+  @param[in]       StartLba        The starting logical block address to be accessed.
+  @param[in]       NumberOfBlocks  The block number or sector count of the transfer.
+  @param[in]       IsWrite         Indicates whether it is a write operation.
+  @param[in, out]  Token           A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+AccessAtaDevice(
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT UINT8                      *Buffer,
+  IN EFI_LBA                        StartLba,
+  IN UINTN                          NumberOfBlocks,
+  IN BOOLEAN                        IsWrite,
+  IN OUT EFI_BLOCK_IO2_TOKEN        *Token
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             MaxTransferBlockNumber;
+  UINTN                             TransferBlockNumber;
+  UINTN                             BlockSize;
+  ATA_BUS_ASYN_SUB_TASK             *SubTask;
+  UINTN                             *EventCount;
+  UINTN                             TempCount;
+  ATA_BUS_ASYN_TASK                 *AtaTask;
+  EFI_EVENT                         SubEvent;
+  UINTN                             Index;
+  BOOLEAN                           *IsError;
+  EFI_TPL                           OldTpl;
+
+  TempCount  = 0;
+  Status     = EFI_SUCCESS;
+  EventCount = NULL;
+  IsError    = NULL;
+  Index      = 0;
+  SubTask    = NULL;
+  SubEvent   = NULL;
+  AtaTask    = NULL;
+
+  //
+  // Ensure AtaDevice->Lba48Bit is a valid boolean value
+  //
+  ASSERT ((UINTN) AtaDevice->Lba48Bit < 2);
+  MaxTransferBlockNumber = mMaxTransferBlockNumber[AtaDevice->Lba48Bit];
+  BlockSize              = AtaDevice->BlockMedia.BlockSize;
+
+  //
+  // Initial the return status and shared account for Non Blocking.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+    if (!IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+      AtaTask = AllocateZeroPool (sizeof (ATA_BUS_ASYN_TASK));
+      if (AtaTask == NULL) {
+        gBS->RestoreTPL (OldTpl);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      AtaTask->AtaDevice      = AtaDevice;
+      AtaTask->Buffer         = Buffer;
+      AtaTask->IsWrite        = IsWrite;
+      AtaTask->NumberOfBlocks = NumberOfBlocks;
+      AtaTask->Signature      = ATA_TASK_SIGNATURE;
+      AtaTask->StartLba       = StartLba;
+      AtaTask->Token          = Token;
+
+      InsertTailList (&AtaDevice->AtaTaskList, &AtaTask->TaskEntry);
+      gBS->RestoreTPL (OldTpl);
+      return EFI_SUCCESS;
+    }
+    gBS->RestoreTPL (OldTpl);
+
+    Token->TransactionStatus = EFI_SUCCESS;
+    EventCount = AllocateZeroPool (sizeof (UINTN));
+    if (EventCount == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    IsError = AllocateZeroPool (sizeof (BOOLEAN));
+    if (IsError == NULL) {
+      FreePool (EventCount);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DEBUG ((EFI_D_BLKIO, "Allocation IsError Addr=%x\n", IsError));
+    *IsError = FALSE;
+    TempCount   = (NumberOfBlocks + MaxTransferBlockNumber - 1) / MaxTransferBlockNumber;
+    *EventCount = TempCount;
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, NumberOfBlocks=%x\n", NumberOfBlocks));
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, MaxTransferBlockNumber=%x\n", MaxTransferBlockNumber));
+    DEBUG ((EFI_D_BLKIO, "AccessAtaDevice, EventCount=%x\n", TempCount));
+  } else {
+    while (!IsListEmpty (&AtaDevice->AtaTaskList) || !IsListEmpty (&AtaDevice->AtaSubTaskList)) {
+      //
+      // Stall for 100us.
+      //
+      MicroSecondDelay (100);
+    }
+  }
+
+  do {
+    if (NumberOfBlocks > MaxTransferBlockNumber) {
+      TransferBlockNumber = MaxTransferBlockNumber;
+      NumberOfBlocks     -= MaxTransferBlockNumber;
+    } else  {
+      TransferBlockNumber = NumberOfBlocks;
+      NumberOfBlocks      = 0;
+    }
+
+    //
+    // Create sub event for the sub ata task. Non-blocking mode.
+    //
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      SubTask  = NULL;
+      SubEvent = NULL;
+
+      SubTask = AllocateZeroPool (sizeof (ATA_BUS_ASYN_SUB_TASK));
+      if (SubTask == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto EXIT;
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      SubTask->UnsignalledEventCount = EventCount;
+      SubTask->Signature             = ATA_SUB_TASK_SIGNATURE;
+      SubTask->AtaDevice             = AtaDevice;
+      SubTask->Token                 = Token;
+      SubTask->IsError               = IsError;
+      InsertTailList (&AtaDevice->AtaSubTaskList, &SubTask->TaskEntry);
+      gBS->RestoreTPL (OldTpl);
+
+      Status = gBS->CreateEvent (
+                      EVT_NOTIFY_SIGNAL,
+                      TPL_NOTIFY,
+                      AtaNonBlockingCallBack,
+                      SubTask,
+                      &SubEvent
+                      );
+      //
+      // If resource allocation fail, the un-signalled event count should equal to
+      // the original one minus the unassigned subtasks number.
+      //
+      if (EFI_ERROR (Status)) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto EXIT;
+      }
+
+      Status = TransferAtaDevice (AtaDevice, &SubTask->Packet, Buffer, StartLba, (UINT32) TransferBlockNumber, IsWrite, SubEvent);
+    } else {
+      //
+      // Blocking Mode.
+      //
+      DEBUG ((EFI_D_BLKIO, "Blocking AccessAtaDevice, TransferBlockNumber=%x; StartLba = %x\n", TransferBlockNumber, StartLba));
+      Status = TransferAtaDevice (AtaDevice, NULL, Buffer, StartLba, (UINT32) TransferBlockNumber, IsWrite, NULL);
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    Index++;
+    StartLba += TransferBlockNumber;
+    Buffer   += TransferBlockNumber * BlockSize;
+  } while (NumberOfBlocks > 0);
+
+EXIT:
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    //
+    // Release resource at non-blocking mode.
+    //
+    if (EFI_ERROR (Status)) {
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      Token->TransactionStatus = Status;
+      *EventCount = (*EventCount) - (TempCount - Index);
+      *IsError    = TRUE;
+
+      if (*EventCount == 0) {
+        FreePool (EventCount);
+        FreePool (IsError);
+      }
+
+      if (SubTask != NULL) {
+        RemoveEntryList (&SubTask->TaskEntry);
+        FreeAtaSubTask (SubTask);
+      }
+
+      if (SubEvent != NULL) {
+        gBS->CloseEvent (SubEvent);
+      }
+      gBS->RestoreTPL (OldTpl);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Trust transfer data from/to ATA device.
+
+  This function performs one ATA pass through transaction to do a trust transfer from/to
+  ATA device. It chooses the appropriate ATA command and protocol to invoke PassThru
+  interface of ATA pass through.
+
+  @param  AtaDevice                    The ATA child device involved for the operation.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+EFIAPI
+TrustTransferAtaDevice (
+  IN OUT ATA_DEVICE                 *AtaDevice,
+  IN OUT VOID                       *Buffer,
+  IN UINT8                          SecurityProtocolId,
+  IN UINT16                         SecurityProtocolSpecificData,
+  IN UINTN                          TransferLength,
+  IN BOOLEAN                        IsTrustSend,
+  IN UINT64                         Timeout,
+  OUT UINTN                         *TransferLengthOut
+  )
+{
+  EFI_ATA_COMMAND_BLOCK             *Acb;
+  EFI_ATA_PASS_THRU_COMMAND_PACKET  *Packet;
+  EFI_STATUS                        Status;
+  VOID                              *NewBuffer;
+  EFI_ATA_PASS_THRU_PROTOCOL        *AtaPassThru;
+
+  //
+  // Ensure AtaDevice->UdmaValid and IsTrustSend are valid boolean values
+  //
+  ASSERT ((UINTN) AtaDevice->UdmaValid < 2);
+  ASSERT ((UINTN) IsTrustSend < 2);
+  //
+  // Prepare for ATA command block.
+  //
+  Acb = ZeroMem (&AtaDevice->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
+  if (TransferLength == 0) {
+    Acb->AtaCommand    = ATA_CMD_TRUST_NON_DATA;
+  } else {
+    Acb->AtaCommand    = mAtaTrustCommands[AtaDevice->UdmaValid][IsTrustSend];
+  }
+  Acb->AtaFeatures      = SecurityProtocolId;
+  Acb->AtaSectorCount   = (UINT8) (TransferLength / 512);
+  Acb->AtaSectorNumber  = (UINT8) ((TransferLength / 512) >> 8);
+  //
+  // NOTE: ATA Spec has no explicitly definition for Security Protocol Specific layout.
+  // Here use big endian for Cylinder register.
+  //
+  Acb->AtaCylinderHigh  = (UINT8) SecurityProtocolSpecificData;
+  Acb->AtaCylinderLow   = (UINT8) (SecurityProtocolSpecificData >> 8);
+  Acb->AtaDeviceHead    = (UINT8) (BIT7 | BIT6 | BIT5 | (AtaDevice->PortMultiplierPort << 4));
+
+  //
+  // Prepare for ATA pass through packet.
+  //
+  Packet = ZeroMem (&AtaDevice->Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
+  if (TransferLength == 0) {
+    Packet->InTransferLength  = 0;
+    Packet->OutTransferLength = 0;
+    Packet->Protocol = EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA;
+  } else if (IsTrustSend) {
+    //
+    // Check the alignment of the incoming buffer prior to invoking underlying ATA PassThru
+    //
+    AtaPassThru = AtaDevice->AtaBusDriverData->AtaPassThru;
+    if ((AtaPassThru->Mode->IoAlign > 1) && !IS_ALIGNED (Buffer, AtaPassThru->Mode->IoAlign)) {
+      NewBuffer = AllocateAlignedBuffer (AtaDevice, TransferLength);
+      if (NewBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      CopyMem (NewBuffer, Buffer, TransferLength);
+      FreePool (Buffer);
+      Buffer = NewBuffer;
+    }
+    Packet->OutDataBuffer = Buffer;
+    Packet->OutTransferLength = (UINT32) TransferLength;
+    Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsTrustSend];
+  } else {
+    Packet->InDataBuffer = Buffer;
+    Packet->InTransferLength = (UINT32) TransferLength;
+    Packet->Protocol = mAtaPassThruCmdProtocols[AtaDevice->UdmaValid][IsTrustSend];
+  }
+  Packet->Length   = EFI_ATA_PASS_THRU_LENGTH_BYTES;
+  Packet->Timeout  = Timeout;
+
+  Status = AtaDevicePassThru (AtaDevice, NULL, NULL);
+  if (TransferLengthOut != NULL) {
+    if (! IsTrustSend) {
+      *TransferLengthOut = Packet->InTransferLength;
+    }
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c
new file mode 100644
index 0000000000..ff9d727e6c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ata/AtaBusDxe/ComponentName.c
@@ -0,0 +1,238 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for ConPlatform driver.
+  
+  Copyright (c) 2009 - 2011, 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 "AtaBus.h"
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaBusDriverNameTable[] = {
+  { "eng;en", L"ATA Bus Driver" },
+  { NULL , NULL }
+};
+
+//
+// Controller name table
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mAtaBusControllerNameTable[] = {
+  { "eng;en", L"ATA Controller" },
+  { NULL , NULL }
+};
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gAtaBusComponentName = {
+  AtaBusComponentNameGetDriverName,
+  AtaBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gAtaBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) AtaBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) AtaBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mAtaBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gAtaBusComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+AtaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                Status;
+  EFI_BLOCK_IO_PROTOCOL     *BlockIo;
+  ATA_DEVICE                *AtaDevice;
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gAtaBusDriverBinding.DriverBindingHandle,
+             &gEfiAtaPassThruProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = mAtaBusControllerNameTable;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               &gEfiAtaPassThruProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Get the child context
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    gAtaBusDriverBinding.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+    AtaDevice = ATA_DEVICE_FROM_BLOCK_IO (BlockIo);
+    ControllerNameTable =AtaDevice->ControllerNameTable;
+  }
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gAtaBusComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c
new file mode 100644
index 0000000000..cad5ad7d44
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBus.c
@@ -0,0 +1,1498 @@
+/** @file
+  This file implements I2C IO Protocol which enables the user to manipulate a single 
+  I2C device independent of the host controller and I2C design.
+
+  Copyright (c) 2013 - 2015, 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 "I2cDxe.h"
+
+//
+//  EFI_DRIVER_BINDING_PROTOCOL instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gI2cBusDriverBinding = {
+  I2cBusDriverSupported,
+  I2cBusDriverStart,
+  I2cBusDriverStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for I2C Bus Child Device.
+//
+I2C_DEVICE_CONTEXT gI2cDeviceContextTemplate = {
+  I2C_DEVICE_SIGNATURE,
+  NULL,
+  {                     // I2cIo Protocol
+    I2cBusQueueRequest, // QueueRequest
+    NULL,               // DeviceGuid
+    0,                  // DeviceIndex
+    0,                  // HardwareRevision
+    NULL                // I2cControllerCapabilities
+  },
+  NULL,                 // DevicePath
+  NULL,                 // I2cDevice
+  NULL,                 // I2cBusContext
+};
+
+//
+// Template for controller device path node.
+//
+CONTROLLER_DEVICE_PATH gControllerDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_CONTROLLER_DP,
+    {
+      (UINT8) (sizeof (CONTROLLER_DEVICE_PATH)),
+      (UINT8) ((sizeof (CONTROLLER_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+//
+// Template for vendor device path node.
+//
+VENDOR_DEVICE_PATH gVendorDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_VENDOR_DP,
+    {
+      (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+      (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+    }
+  },
+  { 0x0, 0x0, 0x0, { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 }}
+};
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mI2cBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"I2C Bus Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gI2cBusComponentName = {
+  (EFI_COMPONENT_NAME_GET_DRIVER_NAME) I2cBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME_GET_CONTROLLER_NAME) I2cBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gI2cBusComponentName2 = {
+  I2cBusComponentNameGetDriverName,
+  I2cBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mI2cBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This != &gI2cBusComponentName2)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Check if the child of I2C controller has been created.
+
+  @param[in] This                         A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.   
+  @param[in] Controller                   I2C controller handle.
+  @param[in] RemainingDevicePath          A pointer to the remaining portion of a device path.
+  @param[in] RemainingHasControllerNode   Indicate if RemainingDevicePath contains CONTROLLER_DEVICE_PATH.
+  @param[in] RemainingControllerNumber    Controller number in CONTROLLER_DEVICE_PATH.
+  
+  @retval EFI_SUCCESS                     The child of I2C controller is not created.
+  @retval Others                          The child of I2C controller has been created or other errors happen.
+
+**/
+EFI_STATUS
+CheckRemainingDevicePath (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath,
+  IN BOOLEAN                      RemainingHasControllerNode,
+  IN UINT32                       RemainingControllerNumber
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_DEVICE_PATH_PROTOCOL                *SystemDevicePath;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY     *OpenInfoBuffer;
+  UINTN                                   EntryCount;
+  UINTN                                   Index;
+  BOOLEAN                                 SystemHasControllerNode;
+  UINT32                                  SystemControllerNumber;  
+
+  SystemHasControllerNode = FALSE;
+  SystemControllerNumber    = 0;
+  
+  Status = gBS->OpenProtocolInformation (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  for (Index = 0; Index < EntryCount; Index++) {
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+                      OpenInfoBuffer[Index].ControllerHandle,
+                      &gEfiDevicePathProtocolGuid,
+                      (VOID **) &SystemDevicePath,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (!EFI_ERROR (Status)) {
+        //
+        // Find vendor device path node and compare
+        //    
+        while (!IsDevicePathEnd (SystemDevicePath)) {
+          if ((DevicePathType (SystemDevicePath) == HARDWARE_DEVICE_PATH) &&
+              (DevicePathSubType (SystemDevicePath) == HW_VENDOR_DP)) {
+            //
+            // Check if vendor device path is same between system device path and remaining device path
+            //
+            if (CompareMem (SystemDevicePath, RemainingDevicePath, sizeof (VENDOR_DEVICE_PATH)) == 0) {
+              //
+              // Get controller node appended after vendor node
+              //
+              SystemDevicePath = NextDevicePathNode (SystemDevicePath);
+              if ((DevicePathType (SystemDevicePath) == HARDWARE_DEVICE_PATH) &&
+                  (DevicePathSubType (SystemDevicePath) == HW_CONTROLLER_DP)) {
+                SystemHasControllerNode = TRUE;
+                SystemControllerNumber    = ((CONTROLLER_DEVICE_PATH *) SystemDevicePath)->ControllerNumber;
+              } else {
+                SystemHasControllerNode = FALSE;
+                SystemControllerNumber    = 0;
+              }
+              if (((SystemHasControllerNode)  && (!RemainingHasControllerNode) && (SystemControllerNumber == 0)) ||
+                  ((!SystemHasControllerNode) && (RemainingHasControllerNode)  && (RemainingControllerNumber == 0)) ||
+                  ((SystemHasControllerNode)  && (RemainingHasControllerNode)  && (SystemControllerNumber == RemainingControllerNumber)) ||
+                  ((!SystemHasControllerNode) && (!RemainingHasControllerNode))) {
+                  DEBUG ((EFI_D_ERROR, "This I2C device has been already started.\n"));
+                  Status = EFI_UNSUPPORTED;
+                  break;
+              }
+            }
+          }
+          SystemDevicePath = NextDevicePathNode (SystemDevicePath);
+        }
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+      }
+    }
+  }
+  FreePool (OpenInfoBuffer);
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_I2C_ENUMERATE_PROTOCOL          *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL               *I2cHost;
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *DevPathNode;
+  BOOLEAN                             RemainingHasControllerNode;
+  UINT32                              RemainingControllerNumber;
+
+  RemainingHasControllerNode = FALSE;
+  RemainingControllerNumber    = 0;
+
+  //
+  //  Determine if the I2c Enumerate Protocol is available
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cEnumerateProtocolGuid,
+                  (VOID **) &I2cEnumerate,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cEnumerateProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // Check if the first node of RemainingDevicePath is a hardware vendor device path
+    //
+    if ((DevicePathType (RemainingDevicePath) != HARDWARE_DEVICE_PATH) ||
+        (DevicePathSubType (RemainingDevicePath) != HW_VENDOR_DP)) {
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Check if the second node of RemainingDevicePath is a controller node
+    //
+    DevPathNode = NextDevicePathNode (RemainingDevicePath);
+    if (!IsDevicePathEnd (DevPathNode)) {
+      if ((DevicePathType (DevPathNode) != HARDWARE_DEVICE_PATH) ||
+          (DevicePathSubType (DevPathNode) != HW_CONTROLLER_DP)) {
+        return EFI_UNSUPPORTED;
+      } else {
+        RemainingHasControllerNode = TRUE;
+        RemainingControllerNumber    = ((CONTROLLER_DEVICE_PATH *) DevPathNode)->ControllerNumber;
+      }
+    }
+  }
+
+  //
+  // Determine if the I2C Host Protocol is available
+  //
+  Status = gBS->OpenProtocol ( 
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID **) &I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+
+
+  if (Status == EFI_ALREADY_STARTED) {
+    if ((RemainingDevicePath == NULL) || 
+        ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath))) {
+      //
+      // If RemainingDevicePath is NULL or is the End of Device Path Node, return EFI_SUCCESS.
+      //
+      Status = EFI_SUCCESS;
+    } else {
+      //
+      // Test if the child with the RemainingDevicePath has already been created.
+      //    
+      Status = CheckRemainingDevicePath (
+                 This,
+                 Controller,
+                 RemainingDevicePath,
+                 RemainingHasControllerNode,
+                 RemainingControllerNumber
+                 );
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_I2C_ENUMERATE_PROTOCOL *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL      *I2cHost;
+  I2C_BUS_CONTEXT            *I2cBusContext;
+  EFI_STATUS                 Status;
+  EFI_DEVICE_PATH_PROTOCOL   *ParentDevicePath;
+
+  I2cBusContext     = NULL;
+  ParentDevicePath  = NULL;
+  I2cEnumerate      = NULL;
+  I2cHost           = NULL;
+
+  //
+  //  Determine if the I2C controller is available
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID**)&I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open I2C host error, Status = %r\n", Status));
+    return Status;
+  }
+
+  if (Status == EFI_ALREADY_STARTED) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &I2cBusContext,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: open private protocol error, Status = %r.\n", Status));
+      return Status;
+    }
+  }
+
+  //
+  //  Get the I2C bus enumeration API
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cEnumerateProtocolGuid,
+                  (VOID**)&I2cEnumerate,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open I2C enumerate error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiDevicePathProtocolGuid,
+                   (VOID **) &ParentDevicePath,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: open device path error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  if ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // don't create any child device and return EFI_SUCESS
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Allocate the buffer for I2C_BUS_CONTEXT if it is not allocated before.
+  //
+  if (I2cBusContext == NULL) {
+    //
+    //  Allocate the I2C context structure for the current I2C controller
+    //
+    I2cBusContext = AllocateZeroPool (sizeof (I2C_BUS_CONTEXT));
+    if (I2cBusContext == NULL) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: there is no enough memory to allocate.\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Error;
+    }
+    
+    /*
+       +----------------+
+    .->| I2C_BUS_CONTEXT|<----- This file Protocol (gEfiCallerIdGuid) installed on I2C Controller handle
+    |  +----------------+
+    |
+    |  +----------------------------+
+    |  | I2C_DEVICE_CONTEXT         |
+    `--|                            |
+       |                            |
+       | I2C IO Protocol Structure  | <----- I2C IO Protocol
+       |                            |
+       +----------------------------+
+    
+    */
+    I2cBusContext->I2cHost      = I2cHost;
+    I2cBusContext->I2cEnumerate = I2cEnumerate;
+    //
+    // Parent controller used to create children
+    //
+    I2cBusContext->Controller   = Controller;
+    //
+    // Parent controller device path used to create children device path
+    //
+    I2cBusContext->ParentDevicePath = ParentDevicePath;
+    
+    I2cBusContext->DriverBindingHandle = This->DriverBindingHandle;
+    
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    I2cBusContext,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "I2cBus: install private protocol error, Status = %r.\n", Status));
+      goto Error;
+    }
+  }
+
+  //
+  //  Start the driver
+  //
+  Status = RegisterI2cDevice (I2cBusContext, Controller, RemainingDevicePath);
+
+  return Status;
+
+Error:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cBus: Start() function failed, Status = %r\n", Status));
+    if (ParentDevicePath != NULL) {
+      gBS->CloseProtocol ( 
+            Controller,
+            &gEfiDevicePathProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+
+    if (I2cHost != NULL) {
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiI2cHostProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+
+    if (I2cEnumerate != NULL) {
+      gBS->CloseProtocol ( 
+            Controller,
+            &gEfiI2cEnumerateProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+    
+    if (I2cBusContext != NULL) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      &Controller,
+                      gEfiCallerIdGuid,
+                      I2cBusContext,
+                      NULL
+                      );
+      FreePool (I2cBusContext);
+    }
+  }
+
+  //
+  //  Return the operation status.
+  //
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  I2C_BUS_CONTEXT             *I2cBusContext;
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+
+  if (NumberOfChildren == 0) {
+    gBS->CloseProtocol ( 
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiI2cEnumerateProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &I2cBusContext,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiCallerIdGuid,
+            I2cBusContext,
+            NULL
+            );
+      //
+      // No more child now, free bus context data.
+      //
+      FreePool (I2cBusContext);
+    }
+    return Status;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = UnRegisterI2cDevice (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Enumerate the I2C bus
+
+  This routine walks the platform specific data describing the
+  I2C bus to create the I2C devices where driver GUIDs were
+  specified.
+
+  @param[in] I2cBusContext            Address of an I2C_BUS_CONTEXT structure
+  @param[in] Controller               Handle to the controller
+  @param[in] RemainingDevicePath      A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS       The bus is successfully configured
+
+**/
+EFI_STATUS
+RegisterI2cDevice (
+  IN I2C_BUS_CONTEXT            *I2cBusContext,
+  IN EFI_HANDLE                 Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL   *RemainingDevicePath
+  )
+{
+  I2C_DEVICE_CONTEXT                *I2cDeviceContext;
+  EFI_STATUS                        Status;
+  CONST EFI_I2C_DEVICE              *Device;
+  CONST EFI_I2C_DEVICE              *TempDevice;
+  UINT32                            RemainingPathDeviceIndex;
+  EFI_DEVICE_PATH_PROTOCOL          *DevPathNode;
+  BOOLEAN                           BuildControllerNode;
+  UINTN                             Count;
+
+  Status                   = EFI_SUCCESS;
+  BuildControllerNode      = TRUE;
+
+  //
+  // Default DeviceIndex
+  //
+  RemainingPathDeviceIndex = 0;
+  
+  //
+  // Determine the controller number in Controller Node Device Path when RemainingDevicePath is not NULL.
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if there is a controller node appended after vendor node
+    //    
+    DevPathNode = NextDevicePathNode (RemainingDevicePath);
+    if ((DevicePathType (DevPathNode) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType(DevPathNode) == HW_CONTROLLER_DP)) {
+      //
+      // RemainingDevicePath != NULL and RemainingDevicePath contains Controller Node,
+      // add Controller Node to Device Path on child handle.
+      //
+      RemainingPathDeviceIndex = ((CONTROLLER_DEVICE_PATH *) DevPathNode)->ControllerNumber;
+    } else {
+      //
+      // RemainingDevicePath != NULL and RemainingDevicePath does not contain Controller Node,
+      // do not add controller node to Device Path on child handle.
+      //
+      BuildControllerNode = FALSE;
+    }
+  }
+
+  //
+  //  Walk the list of I2C devices on this bus
+  //
+  Device = NULL;
+  while (TRUE) {
+    //
+    //  Get the next I2C device
+    //
+    Status = I2cBusContext->I2cEnumerate->Enumerate (I2cBusContext->I2cEnumerate, &Device);
+    if (EFI_ERROR (Status) || Device == NULL) {
+      if (RemainingDevicePath != NULL) {
+        Status = EFI_NOT_FOUND;
+      } else {
+        Status = EFI_SUCCESS;
+      }
+      break;
+    }
+
+    //
+    //  Determine if the device info is valid
+    //
+    if ((Device->DeviceGuid == NULL) || (Device->SlaveAddressCount == 0) || (Device->SlaveAddressArray == NULL)) {
+      DEBUG ((EFI_D_ERROR, "Invalid EFI_I2C_DEVICE reported by I2c Enumerate protocol.\n"));
+      continue;
+    }
+
+    if (RemainingDevicePath == NULL) {
+      if (Device->DeviceIndex == 0) {
+        //
+        // Determine if the controller node is necessary when controller number is zero in I2C device
+        //
+        TempDevice = NULL;
+        Count      = 0;
+        while (TRUE) {
+          //
+          //  Get the next I2C device
+          //
+          Status = I2cBusContext->I2cEnumerate->Enumerate (I2cBusContext->I2cEnumerate, &TempDevice);
+          if (EFI_ERROR (Status) || TempDevice == NULL) {
+            Status = EFI_SUCCESS;
+            break;
+          }
+          if (CompareGuid (Device->DeviceGuid, TempDevice->DeviceGuid)) {
+            Count++;
+          }
+        }
+        if (Count == 1) {
+          //
+          // RemainingDevicePath == NULL and only DeviceIndex 0 is present on the I2C bus,
+          // do not add Controller Node to Device Path on child handle.
+          //
+          BuildControllerNode = FALSE;
+        }
+      }
+    } else {
+      //
+      // Find I2C device reported in Remaining Device Path
+      //
+      if ((!CompareGuid (&((VENDOR_DEVICE_PATH *)RemainingDevicePath)->Guid, Device->DeviceGuid)) ||
+          (RemainingPathDeviceIndex != Device->DeviceIndex)) {
+        continue; 
+      }
+    }
+
+    //
+    // Build the device context for current I2C device.
+    //
+    I2cDeviceContext = NULL;
+    I2cDeviceContext = AllocateCopyPool (sizeof (I2C_DEVICE_CONTEXT), &gI2cDeviceContextTemplate);
+    ASSERT (I2cDeviceContext != NULL);
+    if (I2cDeviceContext == NULL) {
+      continue;
+    }
+
+    //
+    //  Initialize the specific device context
+    //
+    I2cDeviceContext->I2cBusContext          = I2cBusContext;
+    I2cDeviceContext->I2cDevice              = Device;
+    I2cDeviceContext->I2cIo.DeviceGuid       = Device->DeviceGuid;
+    I2cDeviceContext->I2cIo.DeviceIndex      = Device->DeviceIndex;
+    I2cDeviceContext->I2cIo.HardwareRevision = Device->HardwareRevision;
+    I2cDeviceContext->I2cIo.I2cControllerCapabilities = I2cBusContext->I2cHost->I2cControllerCapabilities;
+
+    //
+    //  Build the device path
+    //
+    Status = I2cBusDevicePathAppend (I2cDeviceContext, BuildControllerNode);
+    ASSERT_EFI_ERROR (Status);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    //  Install the protocol
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+              &I2cDeviceContext->Handle,
+              &gEfiI2cIoProtocolGuid,
+              &I2cDeviceContext->I2cIo,
+              &gEfiDevicePathProtocolGuid,
+              I2cDeviceContext->DevicePath,
+              NULL );
+    if (EFI_ERROR (Status)) {
+      //
+      // Free resources for this I2C device
+      //
+      ReleaseI2cDeviceContext (I2cDeviceContext);
+      continue;
+    }
+    
+    //
+    // Create the child handle
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiI2cHostProtocolGuid,
+                    (VOID **) &I2cBusContext->I2cHost,
+                    I2cBusContext->DriverBindingHandle,
+                    I2cDeviceContext->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      I2cDeviceContext->Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      I2cDeviceContext->DevicePath,
+                      &gEfiI2cIoProtocolGuid,
+                      &I2cDeviceContext->I2cIo,
+                      NULL
+                      );
+      //
+      // Free resources for this I2C device
+      //
+      ReleaseI2cDeviceContext (I2cDeviceContext);
+      continue;      
+    }
+
+    if (RemainingDevicePath != NULL) {
+      //
+      // Child has been created successfully
+      //
+      break;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Queue an I2C transaction for execution on the I2C device.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  This routine queues an I2C transaction to the I2C controller for
+  execution on the I2C bus.
+
+  When Event is NULL, QueueRequest() operates synchronously and returns
+  the I2C completion status as its return value.
+
+  When Event is not NULL, QueueRequest() synchronously returns EFI_SUCCESS
+  indicating that the asynchronous I2C transaction was queued.  The values
+  above are returned in the buffer pointed to by I2cStatus upon the
+  completion of the I2C transaction when I2cStatus is not NULL.
+
+  The upper layer driver writer provides the following to the platform
+  vendor:
+  
+  1.  Vendor specific GUID for the I2C part
+  2.  Guidance on proper construction of the slave address array when the
+      I2C device uses more than one slave address.  The I2C bus protocol
+      uses the SlaveAddressIndex to perform relative to physical address
+      translation to access the blocks of hardware within the I2C device.
+
+  @param[in] This               Pointer to an EFI_I2C_IO_PROTOCOL structure.
+  @param[in] SlaveAddressIndex  Index value into an array of slave addresses
+                                for the I2C device.  The values in the array
+                                are specified by the board designer, with the
+                                third party I2C device driver writer providing
+                                the slave address order.
+
+                                For devices that have a single slave address,
+                                this value must be zero.  If the I2C device
+                                uses more than one slave address then the
+                                third party (upper level) I2C driver writer
+                                needs to specify the order of entries in the
+                                slave address array.
+
+                                \ref ThirdPartyI2cDrivers "Third Party I2C
+                                Drivers" section in I2cMaster.h.
+  @param[in] Event              Event to signal for asynchronous transactions,
+                                NULL for synchronous transactions
+  @param[in] RequestPacket      Pointer to an EFI_I2C_REQUEST_PACKET structure
+                                describing the I2C transaction
+  @param[out] I2cStatus         Optional buffer to receive the I2C transaction
+                                completion status
+
+  @retval EFI_SUCCESS           The asynchronous transaction was successfully
+                                queued when Event is not NULL.
+  @retval EFI_SUCCESS           The transaction completed successfully when
+                                Event is NULL.
+  @retval EFI_BAD_BUFFER_SIZE   The RequestPacket->LengthInBytes value is too
+                                large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the
+                                transaction.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        The EFI_I2C_HOST_PROTOCOL could not set the
+                                bus configuration required to access this I2C
+                                device.
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the slave
+                                address selected by SlaveAddressIndex.
+                                EFI_DEVICE_ERROR will be returned if the
+                                controller cannot distinguish when the NACK
+                                occurred.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C transaction
+  @retval EFI_UNSUPPORTED       The controller does not support the requested
+                                transaction.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusQueueRequest (
+  IN CONST EFI_I2C_IO_PROTOCOL  *This,
+  IN UINTN                      SlaveAddressIndex,
+  IN EFI_EVENT                  Event               OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET     *RequestPacket,
+  OUT EFI_STATUS                *I2cStatus          OPTIONAL
+  )
+{
+  CONST EFI_I2C_DEVICE        *I2cDevice;
+  I2C_BUS_CONTEXT             *I2cBusContext;
+  CONST EFI_I2C_HOST_PROTOCOL *I2cHost;
+  I2C_DEVICE_CONTEXT          *I2cDeviceContext;
+  EFI_STATUS                  Status;
+
+  if (RequestPacket == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Validate the I2C slave index
+  //
+  I2cDeviceContext = I2C_DEVICE_CONTEXT_FROM_PROTOCOL (This);
+  I2cDevice        = I2cDeviceContext->I2cDevice;
+  if ( SlaveAddressIndex >= I2cDevice->SlaveAddressCount ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Locate the I2c Host Protocol to queue request
+  //
+  I2cBusContext = I2cDeviceContext->I2cBusContext;
+  I2cHost       = I2cBusContext->I2cHost;
+
+  //
+  //  Start the I2C operation
+  //
+  Status = I2cHost->QueueRequest (
+                      I2cHost,
+                      I2cDevice->I2cBusConfiguration,
+                      I2cDevice->SlaveAddressArray [SlaveAddressIndex],
+                      Event,
+                      RequestPacket,
+                      I2cStatus
+                      );
+
+  return Status;
+}
+
+/**
+  Release all the resources allocated for the I2C device.
+
+  This function releases all the resources allocated for the I2C device.
+
+  @param  I2cDeviceContext         The I2C child device involved for the operation.
+
+**/
+VOID
+ReleaseI2cDeviceContext (
+  IN I2C_DEVICE_CONTEXT          *I2cDeviceContext
+  )
+{
+  if (I2cDeviceContext == NULL) {
+    return;
+  }
+  
+  if (I2cDeviceContext->DevicePath != NULL) {
+    FreePool (I2cDeviceContext->DevicePath);
+  }
+
+  FreePool (I2cDeviceContext);
+}
+
+/**
+  Unregister an I2C device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the I2C device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the I2C device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The I2C device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the I2C device.
+
+**/
+EFI_STATUS
+UnRegisterI2cDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                  Status;
+  I2C_DEVICE_CONTEXT          *I2cDeviceContext;
+  EFI_I2C_IO_PROTOCOL         *I2cIo;
+  EFI_I2C_HOST_PROTOCOL       *I2cHost;
+
+  I2cIo = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiI2cIoProtocolGuid,
+                  (VOID **) &I2cIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get I2c device context data.
+  //
+  I2cDeviceContext = I2C_DEVICE_CONTEXT_FROM_PROTOCOL (I2cIo);
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiI2cHostProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The I2C Bus driver installs the I2C Io and Device Path Protocol in the DriverBindingStart().
+  // Here should uninstall them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  I2cDeviceContext->DevicePath,
+                  &gEfiI2cIoProtocolGuid,
+                  &I2cDeviceContext->I2cIo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Keep parent and child relationship
+    //
+    gBS->OpenProtocol (
+          Controller,
+          &gEfiI2cHostProtocolGuid,
+          (VOID **) &I2cHost,
+          This->DriverBindingHandle,
+          Handle,
+          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+          );
+    return Status;
+  }
+  
+  //
+  // Free resources for this I2C device
+  //
+  ReleaseI2cDeviceContext (I2cDeviceContext);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Create a path for the I2C device
+
+  Append the I2C slave path to the I2C master controller path.
+
+  @param[in] I2cDeviceContext           Address of an I2C_DEVICE_CONTEXT structure.
+  @param[in] BuildControllerNode        Flag to build controller node in device path.
+
+  @retval EFI_SUCCESS           The I2C device path is built successfully.
+  @return Others                It is failed to built device path.
+
+**/
+EFI_STATUS
+I2cBusDevicePathAppend (
+  IN I2C_DEVICE_CONTEXT     *I2cDeviceContext,
+  IN BOOLEAN                BuildControllerNode
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *PreviousDevicePath;
+  
+  PreviousDevicePath = NULL;
+
+  //
+  // Build vendor device path
+  //  
+  CopyMem (&gVendorDevicePathTemplate.Guid, I2cDeviceContext->I2cDevice->DeviceGuid, sizeof (EFI_GUID));
+  I2cDeviceContext->DevicePath                    = AppendDevicePathNode (
+                                                      I2cDeviceContext->I2cBusContext->ParentDevicePath,
+                                                      (EFI_DEVICE_PATH_PROTOCOL *) &gVendorDevicePathTemplate
+                                                      );
+  ASSERT (I2cDeviceContext->DevicePath != NULL);
+  if (I2cDeviceContext->DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  if ((BuildControllerNode) && (I2cDeviceContext->DevicePath != NULL)) {
+    //
+    // Build the final I2C device path with controller node
+    //
+    PreviousDevicePath = I2cDeviceContext->DevicePath;
+    gControllerDevicePathTemplate.ControllerNumber = I2cDeviceContext->I2cDevice->DeviceIndex; 
+    I2cDeviceContext->DevicePath          = AppendDevicePathNode (
+                                              I2cDeviceContext->DevicePath,
+                                              (EFI_DEVICE_PATH_PROTOCOL *) &gControllerDevicePathTemplate
+                                              );
+    gBS->FreePool (PreviousDevicePath);
+    ASSERT (I2cDeviceContext->DevicePath != NULL);
+    if (I2cDeviceContext->DevicePath == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user entry point for the I2C bus module. The user code starts with
+  this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gI2cBusDriverBinding,
+             NULL,
+             &gI2cBusComponentName,
+             &gI2cBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C bus module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of all I2C Controller handles in the handle database.
+  // If there is an error getting the list, then the unload
+  // operation fails.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiI2cHostProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by Driver BindingHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      gI2cBusDriverBinding.DriverBindingHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gI2cBusDriverBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cBusDriverBinding.DriverBindingHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  gI2cBusDriverBinding.DriverBindingHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cBusDriverBinding.DriverBindingHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf
new file mode 100644
index 0000000000..4dbe2f5e68
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf
@@ -0,0 +1,58 @@
+## @file
+#  This driver enumerates I2C devices on I2C bus and produce I2C IO Protocol on I2C devices.
+#
+#  Copyright (c) 2013 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cBusDxe
+  MODULE_UNI_FILE                = I2cBusDxe.uni
+  FILE_GUID                      = 0C34B372-2622-4A13-A46E-BFD0DEB48BFF
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2cBus
+  UNLOAD_IMAGE                   = I2cBusUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.h
+  I2cBus.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cIoProtocolGuid                             ## BY_START
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiI2cEnumerateProtocolGuid                      ## TO_START
+  gEfiI2cHostProtocolGuid                           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cBusDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni
new file mode 100644
index 0000000000..f0a1f5e650
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni
new file mode 100644
index 0000000000..13381e6282
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c
new file mode 100644
index 0000000000..fb985f96e2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.c
@@ -0,0 +1,75 @@
+/** @file
+  This file implements the entrypoint and unload function for I2C DXE module.
+
+  Copyright (c) 2013, 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 "I2cDxe.h"
+
+/**
+  The user Entry Point for I2C module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2c(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = InitializeI2cHost ( ImageHandle, SystemTable );
+  if ( !EFI_ERROR ( Status ))
+  {
+    Status = InitializeI2cBus ( ImageHandle, SystemTable );
+  }
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+
+  //
+  //  Disconnect the drivers
+  //
+  Status = I2cBusUnload ( ImageHandle );
+  if ( !EFI_ERROR ( Status )) {
+    Status = I2cHostUnload ( ImageHandle );
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h
new file mode 100644
index 0000000000..1490d4283a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.h
@@ -0,0 +1,1097 @@
+/** @file
+  Private data structures for the I2C DXE driver.
+
+  This file defines common data structures, macro definitions and some module
+  internal function header files.
+
+  Copyright (c) 2013, 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.
+
+**/
+
+#ifndef __I2C_DXE_H__
+#define __I2C_DXE_H__
+
+#include <Uefi.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/TimerLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+
+#include <Protocol/DriverBinding.h>
+#include <Protocol/I2cEnumerate.h>
+#include <Protocol/I2cHost.h>
+#include <Protocol/I2cIo.h>
+#include <Protocol/I2cMaster.h>
+#include <Protocol/I2cBusConfigurationManagement.h>
+#include <Protocol/LoadedImage.h>
+
+#define I2C_DEVICE_SIGNATURE          SIGNATURE_32 ('I', '2', 'C', 'D')
+#define I2C_HOST_SIGNATURE            SIGNATURE_32 ('I', '2', 'C', 'H')
+#define I2C_REQUEST_SIGNATURE         SIGNATURE_32 ('I', '2', 'C', 'R')
+
+//
+// Synchronize access to the list of requests
+//
+#define TPL_I2C_SYNC                  TPL_NOTIFY
+
+//
+//  I2C bus context
+//
+typedef struct {
+  EFI_I2C_ENUMERATE_PROTOCOL       *I2cEnumerate;
+  EFI_I2C_HOST_PROTOCOL            *I2cHost;
+  EFI_HANDLE                       Controller;
+  EFI_DEVICE_PATH_PROTOCOL         *ParentDevicePath;
+  EFI_HANDLE                       DriverBindingHandle;  
+} I2C_BUS_CONTEXT;
+
+//
+// I2C device context
+//
+typedef struct {
+  //
+  // Structure identification
+  //
+  UINT32                        Signature;
+
+  //
+  // I2c device handle
+  //
+  EFI_HANDLE                    Handle;
+
+  //
+  // Upper level API to support the I2C device I/O
+  //
+  EFI_I2C_IO_PROTOCOL           I2cIo;
+
+  //
+  // Device path for this device
+  //
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+
+  //
+  // Platform specific data for this device
+  //
+  CONST EFI_I2C_DEVICE          *I2cDevice;
+
+  //
+  // Context for the common I/O support including the
+  // lower level API to the host controller.
+  //
+  I2C_BUS_CONTEXT               *I2cBusContext;
+} I2C_DEVICE_CONTEXT;
+
+#define I2C_DEVICE_CONTEXT_FROM_PROTOCOL(a) CR (a, I2C_DEVICE_CONTEXT, I2cIo, I2C_DEVICE_SIGNATURE)
+
+//
+// I2C Request
+//
+typedef struct {
+  //
+  // Signature
+  //
+  UINT32                            Signature;
+  
+  //
+  // Next request in the pending request list
+  //
+  LIST_ENTRY                        Link;
+
+  //
+  // I2C bus configuration for the operation
+  //
+  UINTN                             I2cBusConfiguration;
+
+  //
+  // I2C slave address for the operation
+  //
+  UINTN                             SlaveAddress;
+
+  //
+  // Event to set for asynchronous operations, NULL for
+  // synchronous operations
+  //
+  EFI_EVENT                         Event;
+
+  //
+  // I2C operation description
+  //
+  EFI_I2C_REQUEST_PACKET            *RequestPacket;
+
+  //
+  // Optional buffer to receive the I2C operation completion status
+  //
+  EFI_STATUS                        *Status;
+} I2C_REQUEST;
+
+#define I2C_REQUEST_FROM_ENTRY(a)         CR (a, I2C_REQUEST, Link, I2C_REQUEST_SIGNATURE);
+
+//
+// I2C host context
+//
+typedef struct {
+  //
+  // Structure identification
+  //
+  UINTN Signature;
+
+  //
+  // Current I2C bus configuration
+  //
+  UINTN I2cBusConfiguration;
+
+  //
+  // I2C bus configuration management event
+  //
+  EFI_EVENT I2cBusConfigurationEvent;
+
+  //
+  // I2C operation completion event
+  //
+  EFI_EVENT I2cEvent;
+
+  //
+  // I2C operation and I2C bus configuration management status
+  //
+  EFI_STATUS Status;
+
+  //
+  // I2C bus configuration management operation pending
+  //
+  BOOLEAN I2cBusConfigurationManagementPending;
+
+  //
+  // I2C request list maintained by I2C Host
+  //
+  LIST_ENTRY                  RequestList;
+
+  //
+  // Upper level API
+  //
+  EFI_I2C_HOST_PROTOCOL       I2cHost;
+
+  //
+  // I2C bus configuration management protocol
+  //
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL *I2cBusConfigurationManagement;
+
+  //
+  // Lower level API for I2C master (controller)
+  //
+  EFI_I2C_MASTER_PROTOCOL *I2cMaster;
+} I2C_HOST_CONTEXT;
+
+#define I2C_HOST_CONTEXT_FROM_PROTOCOL(a) CR (a, I2C_HOST_CONTEXT, I2cHost, I2C_HOST_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_COMPONENT_NAME_PROTOCOL    gI2cBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gI2cBusComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL    gI2cBusDriverBinding;
+
+extern EFI_COMPONENT_NAME_PROTOCOL    gI2cHostComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gI2cHostComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL    gI2cHostDriverBinding;
+
+/**
+  Start the I2C driver
+
+  This routine allocates the necessary resources for the driver.
+
+  This routine is called by I2cBusDriverStart to complete the driver
+  initialization.
+
+  @param[in] I2cBus                   Address of an I2C_BUS_CONTEXT structure
+  @param[in] Controller               Handle to the controller
+  @param[in] RemainingDevicePath      A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS                 Driver API properly initialized
+  
+**/
+EFI_STATUS
+RegisterI2cDevice (
+  IN I2C_BUS_CONTEXT           *I2cBus,
+  IN EFI_HANDLE                 Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath
+  );
+
+/**
+  Unregister an I2C device.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the I2C device.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the I2C device.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The I2C device is successfully unregistered.
+  @return Others                Some error occurs when unregistering the I2C device.
+
+**/
+EFI_STATUS
+UnRegisterI2cDevice (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  );
+
+/**
+  Create a path for the I2C device
+
+  Append the I2C slave path to the I2C master controller path.
+
+  @param[in] I2cDeviceContext           Address of an I2C_DEVICE_CONTEXT structure.
+  @param[in] BuildControllerNode        Flag to build controller node in device path.
+
+  @retval EFI_SUCCESS           The I2C device path is built successfully.
+  @return Others                It is failed to built device path.
+
+**/
+EFI_STATUS
+I2cBusDevicePathAppend (
+  IN I2C_DEVICE_CONTEXT     *I2cDeviceContext,
+  IN BOOLEAN                BuildControllerNode
+  );
+
+/**
+  Queue an I2C transaction for execution on the I2C device.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  This routine queues an I2C transaction to the I2C controller for
+  execution on the I2C bus.
+
+  When Event is NULL, QueueRequest() operates synchronously and returns
+  the I2C completion status as its return value.
+
+  When Event is not NULL, QueueRequest() synchronously returns EFI_SUCCESS
+  indicating that the asynchronous I2C transaction was queued.  The values
+  above are returned in the buffer pointed to by I2cStatus upon the
+  completion of the I2C transaction when I2cStatus is not NULL.
+
+  The upper layer driver writer provides the following to the platform
+  vendor:
+  
+  1.  Vendor specific GUID for the I2C part
+  2.  Guidance on proper construction of the slave address array when the
+      I2C device uses more than one slave address.  The I2C bus protocol
+      uses the SlaveAddressIndex to perform relative to physical address
+      translation to access the blocks of hardware within the I2C device.
+
+  @param[in] This               Pointer to an EFI_I2C_IO_PROTOCOL structure.
+  @param[in] SlaveAddressIndex  Index value into an array of slave addresses
+                                for the I2C device.  The values in the array
+                                are specified by the board designer, with the
+                                third party I2C device driver writer providing
+                                the slave address order.
+
+                                For devices that have a single slave address,
+                                this value must be zero.  If the I2C device
+                                uses more than one slave address then the
+                                third party (upper level) I2C driver writer
+                                needs to specify the order of entries in the
+                                slave address array.
+
+                                \ref ThirdPartyI2cDrivers "Third Party I2C
+                                Drivers" section in I2cMaster.h.
+  @param[in] Event              Event to signal for asynchronous transactions,
+                                NULL for synchronous transactions
+  @param[in] RequestPacket      Pointer to an EFI_I2C_REQUEST_PACKET structure
+                                describing the I2C transaction
+  @param[out] I2cStatus         Optional buffer to receive the I2C transaction
+                                completion status
+
+  @retval EFI_SUCCESS           The asynchronous transaction was successfully
+                                queued when Event is not NULL.
+  @retval EFI_SUCCESS           The transaction completed successfully when
+                                Event is NULL.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                binding Stop() routine was called.
+  @retval EFI_BAD_BUFFER_SIZE   The RequestPacket->LengthInBytes value is too
+                                large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the
+                                transaction.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NOT_FOUND         Reserved bit set in the SlaveAddress parameter
+  @retval EFI_NO_MAPPING        The EFI_I2C_HOST_PROTOCOL could not set the
+                                bus configuration required to access this I2C
+                                device.
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the slave
+                                address selected by SlaveAddressIndex.
+                                EFI_DEVICE_ERROR will be returned if the
+                                controller cannot distinguish when the NACK
+                                occurred.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C transaction
+  @retval EFI_UNSUPPORTED       The controller does not support the requested
+                                transaction.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusQueueRequest (
+  IN CONST EFI_I2C_IO_PROTOCOL  *This,
+  IN UINTN                      SlaveAddressIndex,
+  IN EFI_EVENT                  Event               OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET     *RequestPacket,
+  OUT EFI_STATUS                *I2cStatus          OPTIONAL
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+  
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  The user entry point for the I2C bus module. The user code starts with
+  this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  This is the unload handle for I2C bus module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cBusUnload (
+  IN EFI_HANDLE             ImageHandle
+  );
+
+/**
+  Release all the resources allocated for the I2C device.
+
+  This function releases all the resources allocated for the I2C device.
+
+  @param  I2cDeviceContext         The I2C child device involved for the operation.
+
+**/
+VOID
+ReleaseI2cDeviceContext (
+  IN I2C_DEVICE_CONTEXT          *I2cDeviceContext
+  );
+
+/**
+  Complete the current request
+
+  @param[in] I2cHost  Address of an I2C_HOST_CONTEXT structure.
+  @param[in] Status   Status of the I<sub>2</sub>C operation.
+
+  @return This routine returns the input status value.
+
+**/
+EFI_STATUS
+I2cHostRequestComplete (
+  I2C_HOST_CONTEXT *I2cHost,
+  EFI_STATUS       Status
+  );
+
+/**
+  Enable access to the I2C bus configuration
+
+  @param[in] I2cHostContext     Address of an I2C_HOST_CONTEXT structure
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+I2cHostRequestEnable (
+  I2C_HOST_CONTEXT *I2cHost
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  );
+  
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+  
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Handle the bus available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostRequestCompleteEvent (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  );
+
+/**
+  Handle the I2C bus configuration available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostI2cBusConfigurationAvailable (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  );
+
+/**
+  Queue an I2C operation for execution on the I2C controller.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  N.B. The typical consumers of this API are the I2C bus driver and
+  on rare occasions the I2C test application.  Extreme care must be
+  taken by other consumers of this API to prevent confusing the
+  third party I2C drivers due to a state change at the I2C device
+  which the third party I2C drivers did not initiate.  I2C platform
+  drivers may use this API within these guidelines.
+
+  This layer uses the concept of I2C bus configurations to describe
+  the I2C bus.  An I2C bus configuration is defined as a unique
+  setting of the multiplexers and switches in the I2C bus which
+  enable access to one or more I2C devices.  When using a switch
+  to divide a bus, due to speed differences, the I2C platform layer
+  would define an I2C bus configuration for the I2C devices on each
+  side of the switch.  When using a multiplexer, the I2C platform
+  layer defines an I2C bus configuration for each of the selector
+  values required to control the multiplexer.  See Figure 1 in the
+  <a href="http://www.nxp.com/documents/user_manual/UM10204.pdf">I<sup>2</sup>C
+  Specification</a> for a complex I2C bus configuration.
+
+  The I2C host driver processes all operations in FIFO order.  Prior to
+  performing the operation, the I2C host driver calls the I2C platform
+  driver to reconfigure the switches and multiplexers in the I2C bus
+  enabling access to the specified I2C device.  The I2C platform driver
+  also selects the maximum bus speed for the device.  After the I2C bus
+  is configured, the I2C host driver calls the I2C port driver to
+  initialize the I2C controller and start the I2C operation.
+
+  @param[in] This             Address of an EFI_I2C_HOST_PROTOCOL instance.
+  @param[in] I2cBusConfiguration  I2C bus configuration to access the I2C
+                                  device.
+  @param[in] SlaveAddress     Address of the device on the I2C bus.
+  @param[in] Event            Event to set for asynchronous operations,
+                              NULL for synchronous operations
+  @param[in] RequestPacket    Address of an EFI_I2C_REQUEST_PACKET
+                              structure describing the I2C operation
+  @param[out] I2cStatus       Optional buffer to receive the I2C operation
+                              completion status
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_INVALID_PARAMETER TPL is too high
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status pointed to by
+                                the request packet.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostQueueRequest (
+  IN CONST EFI_I2C_HOST_PROTOCOL  *This,
+  IN UINTN                        I2cBusConfiguration,
+  IN UINTN                        SlaveAddress,
+  IN EFI_EVENT                    Event            OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET       *RequestPacket,
+  OUT EFI_STATUS                  *I2cStatus       OPTIONAL
+  );
+
+/**
+  The user Entry Point for I2C host module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cHost(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  This is the unload handle for I2C host module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostUnload (
+  IN EFI_HANDLE             ImageHandle
+  );
+
+#endif  //  __I2C_DXE_H__
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf
new file mode 100644
index 0000000000..5cd53b2fc4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf
@@ -0,0 +1,67 @@
+## @file
+#  I2c Dxe driver includes both I2c Bus and Host functionality.
+#
+#  This driver produce I2C Host Protocol on I2C controller handle, enumerate I2C
+#  devices on I2C bus and produce I2C IO Protocol on I2C devices.
+#
+#  Copyright (c) 2013 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cDxe
+  MODULE_UNI_FILE                = I2cDxe.uni
+  FILE_GUID                      = ECA2AE9E-7594-4901-871C-449DA1A11660
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2c
+  UNLOAD_IMAGE                   = I2cUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.c
+  I2cDxe.h
+  I2cHost.c
+  I2cBus.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cIoProtocolGuid                             ## BY_START
+  ## BY_START
+  ## TO_START
+  gEfiI2cHostProtocolGuid
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiI2cMasterProtocolGuid                         ## TO_START
+  gEfiI2cEnumerateProtocolGuid                      ## TO_START
+  gEfiI2cBusConfigurationManagementProtocolGuid     ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni
new file mode 100644
index 0000000000..03f9776d1f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni
new file mode 100644
index 0000000000..9496661f9e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c
new file mode 100644
index 0000000000..68dd931887
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHost.c
@@ -0,0 +1,1228 @@
+/** @file
+  This file implements I2C Host Protocol which provides callers with the ability to 
+  do I/O transactions to all of the devices on the I2C bus.
+
+  Copyright (c) 2014, Hewlett-Packard Development Company, L.P.<BR>
+  Copyright (c) 2013 - 2015, 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 "I2cDxe.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gI2cHostDriverBinding = {
+  I2cHostDriverSupported,
+  I2cHostDriverStart,
+  I2cHostDriverStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mI2cHostDriverNameTable[] = {
+  { "eng;en", L"I2c Host Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gI2cHostComponentName = {
+  (EFI_COMPONENT_NAME_GET_DRIVER_NAME) I2cHostComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME_GET_CONTROLLER_NAME) I2cHostComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gI2cHostComponentName2 = {
+  I2cHostComponentNameGetDriverName,
+  I2cHostComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mI2cHostDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This != &gI2cHostComponentName2)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME2_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_I2C_MASTER_PROTOCOL                       *I2cMaster;
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL *I2cBusConfigurationManagement;
+  EFI_STATUS                                    Status;
+
+  //
+  //  Locate I2C Bus Configuration Management Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  (VOID **)&I2cBusConfigurationManagement,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  //
+  //  Locate I2C Master Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cMasterProtocolGuid,
+                  (VOID **)&I2cMaster,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                                          Status;
+  EFI_I2C_MASTER_PROTOCOL                             *I2cMaster;
+  EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL       *I2cBusConfigurationManagement;
+  I2C_HOST_CONTEXT                                    *I2cHostContext;
+
+  I2cMaster                     = NULL;
+  I2cHostContext                = NULL;
+  I2cBusConfigurationManagement = NULL;
+
+  //
+  // Locate I2C Bus Configuration Management Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cBusConfigurationManagementProtocolGuid,
+                  (VOID **)&I2cBusConfigurationManagement,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Open I2C bus configuration error, Status = %r\n", Status));
+    return Status;
+  }
+
+  //
+  // Locate I2C Master Protocol
+  //
+  Status = gBS->OpenProtocol ( 
+                  Controller,
+                  &gEfiI2cMasterProtocolGuid,
+                  (VOID **)&I2cMaster,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Open I2C master error, Status = %r\n", Status));
+    goto Exit;
+  }
+
+  //
+  // Allocate the I2C Host Context structure
+  //
+  I2cHostContext = AllocateZeroPool (sizeof (I2C_HOST_CONTEXT));
+  if (I2cHostContext == NULL) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: there is no enough memory to allocate.\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Initialize the context structure for the current I2C Controller
+  //
+  I2cHostContext->Signature                     = I2C_HOST_SIGNATURE;
+  I2cHostContext->I2cMaster                     = I2cMaster;
+  I2cHostContext->I2cBusConfigurationManagement = I2cBusConfigurationManagement;
+  I2cHostContext->I2cBusConfiguration           = (UINTN) -1;
+  InitializeListHead(&I2cHostContext->RequestList);
+
+  //
+  // Reset the controller
+  //
+  Status = I2cMaster->Reset (I2cMaster);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: I2C controller reset failed!\n"));
+    goto Exit;
+  }
+
+  //
+  // Create the I2C transaction complete event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_I2C_SYNC,
+                  I2cHostRequestCompleteEvent,
+                  I2cHostContext,
+                  &I2cHostContext->I2cEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: create complete event error, Status = %r\n", Status));
+    goto Exit;
+  }
+
+  //
+  // Get the bus management event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_I2C_SYNC,
+                  I2cHostI2cBusConfigurationAvailable,
+                  I2cHostContext,
+                  &I2cHostContext->I2cBusConfigurationEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: create bus available event error, Status = %r\n", Status));
+    goto Exit;
+  }  
+
+  //
+  // Build the I2C host protocol for the current I2C controller
+  //
+  I2cHostContext->I2cHost.QueueRequest              = I2cHostQueueRequest;
+  I2cHostContext->I2cHost.I2cControllerCapabilities = I2cMaster->I2cControllerCapabilities;
+
+  //
+  //  Install the driver protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  &I2cHostContext->I2cHost,
+                  NULL
+                  );
+Exit:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: Start() function failed, Status = %r\n", Status));
+    if (I2cBusConfigurationManagement != NULL) {
+      gBS->CloseProtocol (
+                      Controller,
+                      &gEfiI2cBusConfigurationManagementProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );      
+    }
+    
+    if ((I2cHostContext != NULL) && (I2cHostContext->I2cEvent != NULL)) {
+      gBS->CloseEvent (I2cHostContext->I2cEvent);
+      I2cHostContext->I2cEvent = NULL;
+    }
+
+    if ((I2cHostContext != NULL) && (I2cHostContext->I2cBusConfigurationEvent != NULL)) {
+      gBS->CloseEvent (I2cHostContext->I2cBusConfigurationEvent);
+      I2cHostContext->I2cBusConfigurationEvent = NULL;
+    }
+
+    //
+    //  Release the context structure upon failure
+    //
+    if (I2cHostContext != NULL) {
+      FreePool (I2cHostContext);
+    }
+  }
+
+  //
+  //  Return the operation status.
+  //
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed, or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;  
+  I2C_HOST_CONTEXT            *I2cHostContext;
+  EFI_I2C_HOST_PROTOCOL       *I2cHost;
+  EFI_TPL                     TplPrevious;
+
+  TplPrevious = EfiGetCurrentTpl ();
+  if (TplPrevious > TPL_I2C_SYNC) {
+    DEBUG ((EFI_D_ERROR, "I2cHost: TPL %d is too high in Stop.\n", TplPrevious));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiI2cHostProtocolGuid,
+                  (VOID **) &I2cHost,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  I2cHostContext = I2C_HOST_CONTEXT_FROM_PROTOCOL (I2cHost);
+
+  //
+  // Raise TPL for critical section
+  //
+  TplPrevious = gBS->RaiseTPL (TPL_I2C_SYNC);
+  
+  //
+  // If there is pending request or pending bus configuration, do not stop
+  //
+  Status = EFI_DEVICE_ERROR;
+  if (( !I2cHostContext->I2cBusConfigurationManagementPending )
+    && IsListEmpty (&I2cHostContext->RequestList)) {
+    
+    //
+    //  Remove the I2C host protocol
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiI2cHostProtocolGuid,
+                    I2cHost,
+                    NULL
+                    );
+  }
+  
+  //
+  // Leave critical section
+  //
+  gBS->RestoreTPL (TplPrevious);
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiI2cBusConfigurationManagementProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    //
+    // Release I2c Host resources
+    //
+    if (I2cHostContext->I2cBusConfigurationEvent != NULL) {
+      gBS->CloseEvent (I2cHostContext->I2cBusConfigurationEvent);
+      I2cHostContext->I2cBusConfigurationEvent = NULL;
+    }
+    
+    if (I2cHostContext->I2cEvent != NULL) {
+      gBS->CloseEvent (I2cHostContext->I2cEvent);
+      I2cHostContext->I2cEvent = NULL;
+    }
+    
+    FreePool (I2cHostContext);
+  }
+
+  //
+  //  Return the stop status
+  //
+  return Status;
+}
+
+/**
+  Handle the I2C bus configuration available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostI2cBusConfigurationAvailable (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  )
+{
+  I2C_HOST_CONTEXT            *I2cHostContext;
+  EFI_I2C_MASTER_PROTOCOL     *I2cMaster;
+  I2C_REQUEST                 *I2cRequest;
+  LIST_ENTRY                  *EntryHeader;
+  LIST_ENTRY                  *Entry;
+  EFI_STATUS                  Status;
+
+  //
+  // Mark this I2C bus configuration management operation as complete
+  //
+  I2cHostContext = (I2C_HOST_CONTEXT *)Context;
+  I2cMaster      = I2cHostContext->I2cMaster;
+  ASSERT (I2cMaster != NULL);
+  //
+  // Clear flag to indicate I2C bus configuration is finished
+  //
+  I2cHostContext->I2cBusConfigurationManagementPending = FALSE;
+
+  //
+  //  Validate the completion status
+  //
+  if (EFI_ERROR (I2cHostContext->Status)) {
+    //
+    // Setting I2C bus configuration failed before
+    //
+    I2cHostRequestComplete (I2cHostContext, I2cHostContext->Status);
+
+    //
+    // Unknown I2C bus configuration
+    // Force next operation to enable the I2C bus configuration
+    //
+    I2cHostContext->I2cBusConfiguration = (UINTN) -1;
+    
+    //
+    // Do not continue current I2C request
+    //
+    return;
+  }
+
+  //
+  // Get the first request in the link with FIFO order
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  //
+  // Update the I2C bus configuration of the current I2C request
+  //
+  I2cHostContext->I2cBusConfiguration = I2cRequest->I2cBusConfiguration;
+
+  //
+  // Start an I2C operation on the host, the status is returned by I2cHostContext->Status
+  //
+  Status = I2cMaster->StartRequest ( 
+                        I2cMaster,
+                        I2cRequest->SlaveAddress,
+                        I2cRequest->RequestPacket,
+                        I2cHostContext->I2cEvent,
+                        &I2cHostContext->Status
+                        );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG((DEBUG_ERROR, "I2cHostI2cBusConfigurationAvailable: Error starting I2C operation, %r\n", Status));
+  }
+}
+
+/**
+  Complete the current request
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] I2cHostContext  Address of an I2C_HOST_CONTEXT structure.
+  @param[in] Status          Status of the I2C operation.
+
+  @return This routine returns the input status value.
+
+**/
+EFI_STATUS
+I2cHostRequestComplete (
+  I2C_HOST_CONTEXT *I2cHostContext,
+  EFI_STATUS       Status
+  )
+{
+  I2C_REQUEST *I2cRequest;
+  LIST_ENTRY  *EntryHeader;
+  LIST_ENTRY  *Entry;
+
+  //
+  // Remove the current I2C request from the list
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  //
+  // Save the status for QueueRequest
+  //
+  if ( NULL != I2cRequest->Status ) {
+    *I2cRequest->Status = Status;
+  }
+
+  //
+  //  Notify the user of the I2C request completion
+  //
+  if ( NULL != I2cRequest->Event ) {
+    gBS->SignalEvent (I2cRequest->Event);
+  }
+
+  //
+  // Done with this request, remove the current request from list
+  //
+  RemoveEntryList (&I2cRequest->Link);
+  FreePool (I2cRequest->RequestPacket);
+  FreePool (I2cRequest);
+
+  //
+  // If there is more I2C request, start next one
+  //
+  if(!IsListEmpty (EntryHeader)) {
+    I2cHostRequestEnable (I2cHostContext);
+  }
+  
+  return Status;
+}
+
+/**
+  Handle the bus available event
+
+  This routine is called at TPL_I2C_SYNC.
+
+  @param[in] Event    Address of an EFI_EVENT handle
+  @param[in] Context  Address of an I2C_HOST_CONTEXT structure
+
+**/
+VOID
+EFIAPI
+I2cHostRequestCompleteEvent (
+  IN EFI_EVENT Event,
+  IN VOID *Context
+  )
+{
+  I2C_HOST_CONTEXT *I2cHostContext;
+
+  //
+  // Handle the completion event
+  //
+  I2cHostContext = (I2C_HOST_CONTEXT *)Context;
+  I2cHostRequestComplete (I2cHostContext, I2cHostContext->Status);
+}
+
+/**
+  Enable access to the I2C bus configuration
+
+  @param[in] I2cHostContext     Address of an I2C_HOST_CONTEXT structure
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ABORTED           The request did not complete because the driver
+                                was shutdown.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+I2cHostRequestEnable (
+  I2C_HOST_CONTEXT *I2cHostContext
+  )
+{
+  UINTN                                                 I2cBusConfiguration;
+  CONST EFI_I2C_BUS_CONFIGURATION_MANAGEMENT_PROTOCOL   *I2cBusConfigurationManagement;
+  I2C_REQUEST                                           *I2cRequest;
+  EFI_STATUS                                            Status;
+  EFI_TPL                                               TplPrevious;
+  LIST_ENTRY                                            *EntryHeader;
+  LIST_ENTRY                                            *Entry;
+
+  //
+  //  Assume pending request
+  //
+  Status = EFI_NOT_READY;
+
+  I2cBusConfigurationManagement = I2cHostContext->I2cBusConfigurationManagement;
+
+  //
+  //  Validate the I2c bus configuration
+  //
+  EntryHeader = &I2cHostContext->RequestList;
+  Entry       = GetFirstNode (EntryHeader);
+  I2cRequest = I2C_REQUEST_FROM_ENTRY (Entry);
+
+  I2cBusConfiguration = I2cRequest->I2cBusConfiguration;
+
+  if (I2cHostContext->I2cBusConfiguration != I2cBusConfiguration ) {
+    //
+    // Set flag to indicate I2C bus configuration is in progress
+    //
+    I2cHostContext->I2cBusConfigurationManagementPending = TRUE;
+    //
+    //  Update bus configuration for this device's requesting bus configuration
+    //
+    Status = I2cBusConfigurationManagement->EnableI2cBusConfiguration (
+                I2cBusConfigurationManagement,
+                I2cBusConfiguration,
+                I2cHostContext->I2cBusConfigurationEvent,
+                &I2cHostContext->Status
+                );
+  } else {
+    //
+    //  I2C bus configuration is same, no need change configuration and start I2c transaction directly
+    //
+    TplPrevious = gBS->RaiseTPL ( TPL_I2C_SYNC );
+
+    //
+    //  Same I2C bus configuration
+    //
+    I2cHostContext->Status = EFI_SUCCESS;
+    I2cHostI2cBusConfigurationAvailable (I2cHostContext->I2cBusConfigurationEvent, I2cHostContext);
+
+    //
+    //  Release the thread synchronization
+    //
+    gBS->RestoreTPL ( TplPrevious );
+  }
+  return Status;
+}
+
+/**
+  Queue an I2C operation for execution on the I2C controller.
+
+  This routine must be called at or below TPL_NOTIFY.  For synchronous
+  requests this routine must be called at or below TPL_CALLBACK.
+
+  N.B. The typical consumers of this API are the I2C bus driver and
+  on rare occasions the I2C test application.  Extreme care must be
+  taken by other consumers of this API to prevent confusing the
+  third party I2C drivers due to a state change at the I2C device
+  which the third party I2C drivers did not initiate.  I2C platform
+  drivers may use this API within these guidelines.
+
+  This layer uses the concept of I2C bus configurations to describe
+  the I2C bus.  An I2C bus configuration is defined as a unique
+  setting of the multiplexers and switches in the I2C bus which
+  enable access to one or more I2C devices.  When using a switch
+  to divide a bus, due to speed differences, the I2C platform layer
+  would define an I2C bus configuration for the I2C devices on each
+  side of the switch.  When using a multiplexer, the I2C platform
+  layer defines an I2C bus configuration for each of the selector
+  values required to control the multiplexer.  See Figure 1 in the
+  <a href="http://www.nxp.com/documents/user_manual/UM10204.pdf">I<sup>2</sup>C
+  Specification</a> for a complex I2C bus configuration.
+
+  The I2C host driver processes all operations in FIFO order.  Prior to
+  performing the operation, the I2C host driver calls the I2C platform
+  driver to reconfigure the switches and multiplexers in the I2C bus
+  enabling access to the specified I2C device.  The I2C platform driver
+  also selects the maximum bus speed for the device.  After the I2C bus
+  is configured, the I2C host driver calls the I2C port driver to
+  initialize the I2C controller and start the I2C operation.
+
+  @param[in] This             Address of an EFI_I2C_HOST_PROTOCOL instance.
+  @param[in] I2cBusConfiguration  I2C bus configuration to access the I2C
+                                  device.
+  @param[in] SlaveAddress     Address of the device on the I2C bus.
+  @param[in] Event            Event to set for asynchronous operations,
+                              NULL for synchronous operations
+  @param[in] RequestPacket    Address of an EFI_I2C_REQUEST_PACKET
+                              structure describing the I2C operation
+  @param[out] I2cStatus       Optional buffer to receive the I2C operation
+                              completion status
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_BAD_BUFFER_SIZE   The WriteBytes or ReadBytes buffer size is too large.
+  @retval EFI_DEVICE_ERROR      There was an I2C error (NACK) during the operation.
+                                This could indicate the slave device is not present.
+  @retval EFI_INVALID_PARAMETER RequestPacket is NULL
+  @retval EFI_INVALID_PARAMETER TPL is too high
+  @retval EFI_NO_MAPPING        Invalid I2cBusConfiguration value
+  @retval EFI_NO_RESPONSE       The I2C device is not responding to the
+                                slave address.  EFI_DEVICE_ERROR may also be
+                                returned if the controller can not distinguish
+                                when the NACK occurred.
+  @retval EFI_NOT_FOUND         I2C slave address exceeds maximum address
+  @retval EFI_NOT_READY         I2C bus is busy or operation pending, wait for
+                                the event and then read status pointed to by
+                                the request packet.
+  @retval EFI_OUT_OF_RESOURCES  Insufficient memory for I2C operation
+  @retval EFI_TIMEOUT           The transaction did not complete within an internally
+                                specified timeout period.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostQueueRequest (
+  IN CONST EFI_I2C_HOST_PROTOCOL  *This,
+  IN UINTN                        I2cBusConfiguration,
+  IN UINTN                        SlaveAddress,
+  IN EFI_EVENT                    Event            OPTIONAL,
+  IN EFI_I2C_REQUEST_PACKET       *RequestPacket,
+  OUT EFI_STATUS                  *I2cStatus       OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  EFI_EVENT         SyncEvent;
+  EFI_TPL           TplPrevious;
+  I2C_REQUEST       *I2cRequest;
+  I2C_HOST_CONTEXT  *I2cHostContext;
+  BOOLEAN           FirstRequest;
+  UINTN             RequestPacketSize;
+  UINTN             StartBit;
+
+  SyncEvent    = NULL;
+  FirstRequest = FALSE;
+  Status       = EFI_SUCCESS;
+
+  if (RequestPacket == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  if ((SlaveAddress & I2C_ADDRESSING_10_BIT) != 0) {
+    //
+    // 10-bit address, bits 0-9 are used for 10-bit I2C slave addresses,
+    // bits 10-30 are reserved bits and must be zero
+    //
+    StartBit = 10;
+  } else {
+    //
+    // 7-bit address, Bits 0-6 are used for 7-bit I2C slave addresses,
+    // bits 7-30 are reserved bits and must be zero
+    //
+    StartBit = 7;
+  }
+
+  if (BitFieldRead32 ((UINT32)SlaveAddress, StartBit, 30) != 0) {
+    //
+    // Reserved bit set in the SlaveAddress parameter
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  I2cHostContext = I2C_HOST_CONTEXT_FROM_PROTOCOL (This);
+
+  if (Event == NULL) {
+    //
+    // For synchronous transaction, register an event used to wait for finishing synchronous transaction
+    //
+    Status = gBS->CreateEvent ( 
+                0,
+                TPL_I2C_SYNC,
+                NULL,
+                NULL,
+                &SyncEvent
+                );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+ 
+  //
+  // TPL should be at or below TPL_NOTIFY.
+  // For synchronous requests this routine must be called at or below TPL_CALLBACK.
+  //
+  TplPrevious = EfiGetCurrentTpl ();
+  if ((TplPrevious > TPL_I2C_SYNC) || ((Event == NULL) && (TplPrevious > TPL_CALLBACK))) {
+    DEBUG ((EFI_D_ERROR, "ERROR - TPL %d is too high!\n", TplPrevious));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Allocate the request structure
+  //
+  I2cRequest = AllocateZeroPool (sizeof (I2C_REQUEST));
+  if (I2cRequest == NULL) {
+    DEBUG ((EFI_D_ERROR, "WARNING - Failed to allocate I2C_REQUEST!\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize the request
+  //
+  I2cRequest->Signature           = I2C_REQUEST_SIGNATURE;
+  I2cRequest->I2cBusConfiguration = I2cBusConfiguration;
+  I2cRequest->SlaveAddress        = SlaveAddress;
+  I2cRequest->Event               = (Event == NULL) ? SyncEvent : Event;
+  I2cRequest->Status              = I2cStatus;
+
+  //
+  // Copy request packet into private buffer, as RequestPacket may be freed during asynchronous transaction
+  //
+  RequestPacketSize = sizeof (UINTN) + RequestPacket->OperationCount * sizeof (EFI_I2C_OPERATION);
+  I2cRequest->RequestPacket = AllocateZeroPool (RequestPacketSize);
+  ASSERT (I2cRequest->RequestPacket != NULL);
+  CopyMem (I2cRequest->RequestPacket, RequestPacket, RequestPacketSize);
+
+  //
+  // Synchronize with the other threads
+  //
+  gBS->RaiseTPL ( TPL_I2C_SYNC );
+  
+  FirstRequest = IsListEmpty (&I2cHostContext->RequestList);
+  
+  //
+  // Insert new I2C request in the list
+  //
+  InsertTailList (&I2cHostContext->RequestList, &I2cRequest->Link);
+
+  //
+  // Release the thread synchronization
+  //
+  gBS->RestoreTPL (TplPrevious);
+  
+  if (FirstRequest) {
+    //
+    // Start the first I2C request, then the subsequent of I2C request will continue
+    //
+    Status = I2cHostRequestEnable (I2cHostContext);
+  }
+
+  if (Event != NULL) {
+    //
+    // For asynchronous, return EFI_SUCCESS indicating that the asynchronously I2C transaction was queued.
+    // No real I2C operation status in I2cStatus
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // For synchronous transaction, wait for the operation completion
+  //
+  do {
+    Status = gBS->CheckEvent (SyncEvent);
+  } while (Status == EFI_NOT_READY);
+
+  //
+  // Get the I2C operation status
+  //
+  Status = I2cHostContext->Status;
+
+  //
+  // Return the I2C operation status
+  //
+  if (I2cStatus != NULL) {
+    *I2cStatus = Status;
+  }
+
+  //
+  // Close the event if necessary
+  //
+  if (SyncEvent != NULL) {
+    gBS->CloseEvent (SyncEvent);
+  }
+
+  return Status;
+}
+
+/**
+  The user Entry Point for I2C host module. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeI2cHost(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gI2cHostDriverBinding,
+             ImageHandle,
+             &gI2cHostComponentName,
+             &gI2cHostComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
+
+/**
+  This is the unload handle for I2C host module.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in] ImageHandle           The drivers' driver image.
+
+  @retval    EFI_SUCCESS           The image is unloaded.
+  @retval    Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+I2cHostUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of all I2C Controller handles in the handle database.
+  // If there is an error getting the list, then the unload
+  // operation fails.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiI2cHostProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by ImageHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      ImageHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gI2cHostDriverBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cHostDriverBinding.DriverBindingHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  gI2cHostDriverBinding.DriverBindingHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           gI2cHostDriverBinding.DriverBindingHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf
new file mode 100644
index 0000000000..2fb1085c6d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf
@@ -0,0 +1,55 @@
+## @file
+#  This driver produce I2C Host Protocol on I2C controller handle.
+#
+#  Copyright (c) 2013 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = I2cHostDxe
+  MODULE_UNI_FILE                = I2cHostDxe.uni
+  FILE_GUID                      = CDEC3671-816E-43DC-A002-DCD645229338
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeI2cHost
+  UNLOAD_IMAGE                   = I2cHostUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  I2cDxe.h
+  I2cHost.c
+
+[LibraryClasses]
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Protocols]
+  gEfiI2cHostProtocolGuid                           ## BY_START
+  gEfiI2cMasterProtocolGuid                         ## TO_START
+  gEfiI2cBusConfigurationManagementProtocolGuid     ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  I2cHostDxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni
new file mode 100644
index 0000000000..a5a804303a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.uni
diff --git a/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni
new file mode 100644
index 0000000000..d1f60d4f09
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c
new file mode 100644
index 0000000000..ff7bbef278
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.c
@@ -0,0 +1,180 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for IsaBusDxe driver.
+  
+  Copyright (c) 2015, 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 "ComponentName.h"
+#include <Library/UefiLib.h>
+
+//
+// Driver name table 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE     mIsaBusDriverNameTable[] = {
+  { "eng;en", L"PI ISA BUS Driver" },
+  { NULL , NULL }
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gIsaBusComponentName = {
+  IsaBusComponentNameGetDriverName,
+  IsaBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gIsaBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) IsaBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) IsaBusComponentNameGetControllerName,
+  "en"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mIsaBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gIsaBusComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h
new file mode 100644
index 0000000000..f533971833
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/ComponentName.h
@@ -0,0 +1,151 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for IsaBusDxe driver.
+  
+  Copyright (c) 2015, 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.             
+
+**/
+
+#ifndef _ISA_BUS_COMPONENT_NAME_H_
+#define _ISA_BUS_COMPONENT_NAME_H_
+
+#include <Uefi.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+
+extern EFI_COMPONENT_NAME_PROTOCOL  gIsaBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gIsaBusComponentName2;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c
new file mode 100644
index 0000000000..7067c6e6f0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.c
@@ -0,0 +1,461 @@
+/** @file
+  This file consumes the ISA Host Controller protocol produced by the ISA Host
+  Controller and installs the ISA Host Controller Service Binding protocol
+  on the ISA Host Controller's handle.
+
+  Copyright (c) 2015, 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 "IsaBusDxe.h"
+#include "ComponentName.h"
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided, 
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This 
+                                   parameter is ignored by device drivers, and is optional for bus 
+                                   drivers. For bus drivers, if this parameter is not NULL, then 
+                                   the bus driver must determine if the bus controller specified 
+                                   by ControllerHandle and the child controller specified 
+                                   by RemainingDevicePath are both supported by this 
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  VOID                              *Instance;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcProtocolGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiIsaHcProtocolGuid,
+      This->DriverBindingHandle,
+      Controller
+      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      This->DriverBindingHandle,
+      Controller
+      );
+  }
+
+  return Status;
+}
+
+ISA_BUS_CHILD_PRIVATE_DATA mIsaBusChildPrivateTemplate = {
+  ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE,
+  FALSE
+};
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                    *ChildHandle
+  )
+{
+  EFI_STATUS                           Status;
+  ISA_BUS_PRIVATE_DATA                 *Private;
+  EFI_ISA_HC_PROTOCOL                  *IsaHc;
+  ISA_BUS_CHILD_PRIVATE_DATA           *Child;
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (This);
+
+  Child = AllocateCopyPool (sizeof (mIsaBusChildPrivateTemplate), &mIsaBusChildPrivateTemplate);
+  if (Child == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiIsaHcProtocolGuid, Private->IsaHc,
+                  &gEfiCallerIdGuid,      Child,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Child);
+    return Status;
+  }
+
+  return gBS->OpenProtocol (
+                Private->IsaHcHandle,
+                &gEfiIsaHcProtocolGuid,
+                (VOID **) &IsaHc,
+                gIsaBusDriverBinding.DriverBindingHandle,
+                *ChildHandle,
+                EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                );
+}
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                       ChildHandle
+  )
+{
+  EFI_STATUS                           Status;
+  ISA_BUS_PRIVATE_DATA                 *Private;
+  EFI_ISA_HC_PROTOCOL                  *IsaHc;
+  ISA_BUS_CHILD_PRIVATE_DATA           *Child;
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (This);
+
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &Child,
+                  gIsaBusDriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ASSERT (Child->Signature == ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE);
+
+  if (Child->InDestroying) {
+    return EFI_SUCCESS;
+  }
+
+  Child->InDestroying = TRUE;
+  Status = gBS->CloseProtocol (
+                  Private->IsaHcHandle,
+                  &gEfiIsaHcProtocolGuid,
+                  gIsaBusDriverBinding.DriverBindingHandle,
+                  ChildHandle
+                  );
+  ASSERT_EFI_ERROR (Status);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandle,
+                    &gEfiIsaHcProtocolGuid, Private->IsaHc,
+                    &gEfiCallerIdGuid,      Child,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+             Private->IsaHcHandle,
+             &gEfiIsaHcProtocolGuid,
+             (VOID **) &IsaHc,
+             gIsaBusDriverBinding.DriverBindingHandle,
+             ChildHandle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    Child->InDestroying = FALSE;
+  } else {
+    FreePool (Child);
+  }
+
+  return Status;
+}
+
+ISA_BUS_PRIVATE_DATA   mIsaBusPrivateTemplate = {
+  ISA_BUS_PRIVATE_DATA_SIGNATURE,
+  {
+    IsaBusCreateChild,
+    IsaBusDestroyChild
+  }
+};
+
+/**
+  Starts a device controller or a bus controller.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This 
+                                   parameter is ignored by device drivers, and is optional for bus 
+                                   drivers. For a bus driver, if this parameter is NULL, then handles 
+                                   for all the children of Controller are created by this driver.  
+                                   If this parameter is not NULL and the first Device Path Node is 
+                                   not the End of Device Path Node, then only the handle for the 
+                                   child device specified by the first Device Path Node of 
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is 
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  ISA_BUS_PRIVATE_DATA              *Private;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcProtocolGuid,
+                  (VOID **) &mIsaBusPrivateTemplate.IsaHc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiIsaHcProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    return Status;
+  }
+
+  Private = AllocateCopyPool (sizeof (mIsaBusPrivateTemplate), &mIsaBusPrivateTemplate);
+  ASSERT (Private != NULL);
+
+  Private->IsaHcHandle = Controller;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiIsaHcServiceBindingProtocolGuid, &Private->ServiceBinding,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+IsaBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_SERVICE_BINDING_PROTOCOL       *ServiceBinding;
+  ISA_BUS_PRIVATE_DATA               *Private;
+  UINTN                              Index;
+  BOOLEAN                            AllChildrenStopped;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiIsaHcServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private = ISA_BUS_PRIVATE_DATA_FROM_THIS (ServiceBinding);
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiIsaHcServiceBindingProtocolGuid, &Private->ServiceBinding,
+                    NULL
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiIsaHcProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      FreePool (Private);
+    }
+
+    return Status;
+  }
+
+  AllChildrenStopped = TRUE;
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    Status = ServiceBinding->DestroyChild (ServiceBinding, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  return AllChildrenStopped ? EFI_SUCCESS : EFI_DEVICE_ERROR;
+}
+
+//
+// ISA Bus Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gIsaBusDriverBinding = {
+  IsaBusDriverBindingSupported,
+  IsaBusDriverBindingStart,
+  IsaBusDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+/**
+  Entry point of the IsaBusDxe driver.
+  
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+**/
+EFI_STATUS
+EFIAPI
+InitializeIsaBus (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS           Status;
+  
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gIsaBusDriverBinding,
+             ImageHandle,
+             &gIsaBusComponentName,
+             &gIsaBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h
new file mode 100644
index 0000000000..86f6f56a61
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.h
@@ -0,0 +1,46 @@
+/** @file
+  Header file for the ISA BUS driver.
+
+  Copyright (c) 2015, 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.
+
+
+**/
+
+#ifndef _ISA_BUS_H_
+#define _ISA_BUS_H_
+
+#include <Uefi.h>
+#include <Protocol/IsaHc.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/ServiceBinding.h>
+
+typedef struct {
+  UINT32                          Signature;
+  EFI_SERVICE_BINDING_PROTOCOL    ServiceBinding;
+  EFI_ISA_HC_PROTOCOL             *IsaHc;       ///< ISA HC protocol produced by the ISA Host Controller driver
+  EFI_HANDLE                      IsaHcHandle;  ///< ISA HC handle created by the ISA Host Controller driver
+} ISA_BUS_PRIVATE_DATA;
+#define ISA_BUS_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('_', 'i', 's', 'b')
+#define ISA_BUS_PRIVATE_DATA_FROM_THIS(a) CR (a, ISA_BUS_PRIVATE_DATA, ServiceBinding, ISA_BUS_PRIVATE_DATA_SIGNATURE)
+
+typedef struct {
+  UINT32                          Signature;
+  BOOLEAN                         InDestroying; ///< Flag to avoid DestroyChild() re-entry.
+} ISA_BUS_CHILD_PRIVATE_DATA;
+#define ISA_BUS_CHILD_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('_', 'i', 's', 'c')
+
+extern EFI_DRIVER_BINDING_PROTOCOL gIsaBusDriverBinding;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf
new file mode 100644
index 0000000000..1d9e22ec4c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf
@@ -0,0 +1,65 @@
+## @file
+#  ISA Bus driver to manage the child devices attached to the ISA Host Controller.
+#
+#  This driver follows UEFI driver model and layers on ISA HC protocol defined
+#  in PI spec 1.2.1. It consumes the ISA Host Controller protocol produced by
+#  the ISA Host Controller and installs the ISA Host Controller Service Binding
+#  protocol on the ISA Host Controller's handle.
+#
+#  Copyright (c) 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IsaBusDxe
+  MODULE_UNI_FILE                = IsaBusDxe.uni
+  FILE_GUID                      = DCBE6D66-D928-4138-8041-358F35CBCF80
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeIsaBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gIsaBusDriverBinding
+#  COMPONENT_NAME                =  gIsaBusComponentName
+#  COMPONENT_NAME2               =  gIsaBusComponentName2
+#
+
+[Sources]
+  IsaBusDxe.h
+  IsaBusDxe.c
+  ComponentName.h
+  ComponentName.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  DebugLib
+
+[Protocols]
+  ## CONSUMES 
+  ## PRODUCES
+  gEfiIsaHcProtocolGuid                 
+  gEfiIsaHcServiceBindingProtocolGuid   ## PRODUCES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IsaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni
new file mode 100644
index 0000000000..5025c25300
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni
new file mode 100644
index 0000000000..44354a0f99
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c
new file mode 100644
index 0000000000..3181b9da5b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.c
@@ -0,0 +1,225 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for EHCI driver.
+
+Copyright (c) 2006 - 2011, 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 "Ehci.h"
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gEhciComponentName = {
+  EhciComponentNameGetDriverName,
+  EhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gEhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) EhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) EhciComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mEhciDriverNameTable[] = {
+  { "eng;en", L"Usb Ehci Driver" },
+  { NULL , NULL }
+};
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mEhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gEhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  USB2_HC_DEV          *EhciDev;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gEhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gEhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  EhciDev = EHC_FROM_THIS (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           EhciDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gEhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h
new file mode 100644
index 0000000000..1a17d0bb49
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/ComponentName.h
@@ -0,0 +1,147 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2008 - 2011, 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.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+EhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c
new file mode 100644
index 0000000000..4e9e05f0e4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.c
@@ -0,0 +1,2106 @@
+/** @file  
+  The Ehci controller driver.
+
+  EhciDxe driver is responsible for managing the behavior of EHCI controller. 
+  It implements the interfaces of monitoring the status of all ports and transferring 
+  Control, Bulk, Interrupt and Isochronous requests to Usb2.0 device.
+
+  Note that EhciDxe driver is enhanced to guarantee that the EHCI controller get attached
+  to the EHCI controller before a UHCI or OHCI driver attaches to the companion UHCI or 
+  OHCI controller.  This way avoids the control transfer on a shared port between EHCI 
+  and companion host controller when UHCI or OHCI gets attached earlier than EHCI and a 
+  USB 2.0 device inserts.
+
+Copyright (c) 2006 - 2015, 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 "Ehci.h"
+
+//
+// Two arrays used to translate the EHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {PORTSC_CONN,     USB_PORT_STAT_CONNECTION},
+  {PORTSC_ENABLED,  USB_PORT_STAT_ENABLE},
+  {PORTSC_SUSPEND,  USB_PORT_STAT_SUSPEND},
+  {PORTSC_OVERCUR,  USB_PORT_STAT_OVERCURRENT},
+  {PORTSC_RESET,    USB_PORT_STAT_RESET},
+  {PORTSC_POWER,    USB_PORT_STAT_POWER},
+  {PORTSC_OWNER,    USB_PORT_STAT_OWNER}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {PORTSC_CONN_CHANGE,    USB_PORT_STAT_C_CONNECTION},
+  {PORTSC_ENABLE_CHANGE,  USB_PORT_STAT_C_ENABLE},
+  {PORTSC_OVERCUR_CHANGE, USB_PORT_STAT_C_OVERCURRENT}
+};
+
+EFI_DRIVER_BINDING_PROTOCOL
+gEhciDriverBinding = {
+  EhcDriverBindingSupported,
+  EhcDriverBindingStart,
+  EhcDriverBindingStop,
+  0x30,
+  NULL,
+  NULL
+};
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  This EFI_USB_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+
+  if ((MaxSpeed == NULL) || (PortNumber == NULL) || (Is64BitCapable == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *MaxSpeed       = EFI_USB_SPEED_HIGH;
+  *PortNumber     = (UINT8) (Ehc->HcStructParams & HCSP_NPORTS);
+  *Is64BitCapable = (UINT8) (Ehc->HcCapParams & HCCP_64BIT);
+
+  DEBUG ((EFI_D_INFO, "EhcGetCapability: %d ports, 64 bit %d\n", *PortNumber, *Is64BitCapable));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcReset (
+  IN EFI_USB2_HC_PROTOCOL *This,
+  IN UINT16               Attributes
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT32                  DbgCtrlStatus;
+
+  Ehc = EHC_FROM_THIS (This);
+
+  if (Ehc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Ehc->DevicePath
+      );
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+  //
+  // Flow through, same behavior as Host Controller Reset
+  //
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    //
+    // Host Controller must be Halt when Reset it
+    //
+    if (Ehc->DebugPortNum != 0) {
+      DbgCtrlStatus = EhcReadDbgRegister(Ehc, 0);
+      if ((DbgCtrlStatus & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) == (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+        Status = EFI_SUCCESS;
+        goto ON_EXIT;
+      }
+    }
+
+    if (!EhcIsHalt (Ehc)) {
+      Status = EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto ON_EXIT;
+      }
+    }
+
+    //
+    // Clean up the asynchronous transfers, currently only
+    // interrupt supports asynchronous operation.
+    //
+    EhciDelAllAsyncIntTransfers (Ehc);
+    EhcAckAllInterrupt (Ehc);
+    EhcFreeSched (Ehc);
+
+    Status = EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+
+    Status = EhcInitHC (Ehc);
+    break;
+
+  case EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG:
+  case EFI_USB_HC_RESET_HOST_WITH_DEBUG:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcReset: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetState (
+  IN   EFI_USB2_HC_PROTOCOL  *This,
+  OUT  EFI_USB_HC_STATE      *State
+  )
+{
+  EFI_TPL                 OldTpl;
+  USB2_HC_DEV             *Ehc;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  if (EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    *State = EfiUsbHcStateHalt;
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  DEBUG ((EFI_D_INFO, "EhcGetState: current state %d\n", *State));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetState (
+  IN EFI_USB2_HC_PROTOCOL *This,
+  IN EFI_USB_HC_STATE     State
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  EFI_USB_HC_STATE        CurState;
+
+  Status = EhcGetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    if (EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    //
+    // Software must not write a one to this field unless the host controller
+    // is in the Halted state. Doing so will yield undefined results.
+    // refers to Spec[EHCI1.0-2.3.1]
+    //
+    if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "EhcSetState: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortStatus (
+  IN   EFI_USB2_HC_PROTOCOL  *This,
+  IN   UINT8                 PortNumber,
+  OUT  EFI_USB_PORT_STATUS   *PortStatus
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+  UINT32                  DbgCtrlStatus;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                        = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  if ((Ehc->DebugPortNum != 0) && (PortNumber == (Ehc->DebugPortNum - 1))) {
+    DbgCtrlStatus = EhcReadDbgRegister(Ehc, 0);
+    if ((DbgCtrlStatus & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) == (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+      goto ON_EXIT;
+    }
+  }
+
+  State                         = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Identify device speed. If in K state, it is low speed.
+  // If the port is enabled after reset, the device is of
+  // high speed. The USB bus driver should retrieve the actual
+  // port speed after reset.
+  //
+  if (EHC_BIT_IS_SET (State, PORTSC_LINESTATE_K)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+
+  } else if (EHC_BIT_IS_SET (State, PORTSC_ENABLED)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+  }
+
+  //
+  // Convert the EHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetRootHubPortFeature (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  State   = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Sofeware can't set this bit, Port can only be enable by
+    // EHCI as a part of the reset and enable
+    //
+    State |= PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= PORTSC_SUSPEND;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (EhcIsHalt (Ehc)) {
+      Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "EhcSetRootHubPortFeature :failed to start HC - %r\n", Status));
+        break;
+      }
+    }
+
+    //
+    // Set one to PortReset bit must also set zero to PortEnable bit
+    //
+    State |= PORTSC_RESET;
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Set port power bit when PPC is 1
+    //
+    if ((Ehc->HcCapParams & HCSP_PPC) == HCSP_PPC) {
+      State |= PORTSC_POWER;
+      EhcWriteOpReg (Ehc, Offset, State);
+    }
+    break;
+
+  case EfiUsbPortOwner:
+    State |= PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcSetRootHubPortFeature: exit status %r\n", Status));
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcClearRootHubPortFeature (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  OldTpl    = gBS->RaiseTPL (EHC_TPL);
+  Ehc       = EHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = EHC_PORT_STAT_OFFSET + (4 * PortNumber);
+  State   = EhcReadOpReg (Ehc, Offset);
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Clear PORT_ENABLE feature means disable port.
+    //
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // A write of zero to this bit is ignored by the host
+    // controller. The host controller will unconditionally
+    // set this bit to a zero when:
+    //   1. software sets the Forct Port Resume bit to a zero from a one.
+    //   2. software sets the Port Reset bit to a one frome a zero.
+    //
+    State &= ~PORSTSC_RESUME;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Clear PORT_RESET means clear the reset signal.
+    //
+    State &= ~PORTSC_RESET;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // Clear port owner means this port owned by EHC
+    //
+    State &= ~PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= PORTSC_CONN_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= PORTSC_ENABLE_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= PORTSC_OVERCUR_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Clear port power bit when PPC is 1
+    //
+    if ((Ehc->HcCapParams & HCSP_PPC) == HCSP_PPC) {
+      State &= ~PORTSC_POWER;
+      EhcWriteOpReg (Ehc, Offset, State);
+    }
+    break;
+  case EfiUsbPortSuspendChange:
+  case EfiUsbPortResetChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "EhcClearRootHubPortFeature: exit status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  TimeOut               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcControlTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  UINT8                   Endpoint;
+  EFI_STATUS              Status;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: HC halted at entrance\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  //
+  // Encode the direction in address, although default control
+  // endpoint is bidirectional. EhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          0,
+          MaximumPacketLength,
+          Translator,
+          EHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: failed to create URB"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  //
+  // Get the status from URB. The result is updated in EhcCheckUrbResult
+  // which is called by EhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcBulkTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: HC is halted\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: failed to create URB\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcAsyncInterruptTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                  * This,
+  IN  UINT8                                 DeviceAddress,
+  IN  UINT8                                 EndPointAddress,
+  IN  UINT8                                 DeviceSpeed,
+  IN  UINTN                                 MaximumPacketLength,
+  IN  BOOLEAN                               IsNewTransfer,
+  IN  OUT UINT8                             *DataToggle,
+  IN  UINTN                                 PollingInterval,
+  IN  UINTN                                 DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR    * Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK       CallBackFunction,
+  IN  VOID                                  *Context OPTIONAL
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  URB                     *Urb;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT8                   *Data;
+
+  //
+  // Validate parameters
+  //
+  if (!EHCI_IS_DATAIN (EndPointAddress)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsNewTransfer) {
+    if (DataLength == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((*DataToggle != 1) && (*DataToggle != 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((PollingInterval > 255) || (PollingInterval < 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = EHC_FROM_THIS (This);
+
+  //
+  // Delete Async interrupt transfer request. DataToggle will return
+  // the next data toggle to use.
+  //
+  if (!IsNewTransfer) {
+    Status = EhciDelAsyncIntTransfer (Ehc, DeviceAddress, EndPointAddress, DataToggle);
+
+    DEBUG ((EFI_D_INFO, "EhcAsyncInterruptTransfer: remove old transfer - %r\n", Status));
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: HC is halt\n"));
+    EhcAckAllInterrupt (Ehc);
+
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  Data = AllocatePool (DataLength);
+
+  if (Data == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: failed to allocate buffer\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_INT_TRANSFER_ASYNC,
+          NULL,
+          Data,
+          DataLength,
+          CallBackFunction,
+          Context,
+          PollingInterval
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcAsyncInterruptTransfer: failed to create URB\n"));
+
+    gBS->FreePool (Data);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  //
+  // New asynchronous transfer must inserted to the head.
+  // Check the comments in EhcMoniteAsyncRequests
+  //
+  EhcLinkQhToPeriod (Ehc, Urb->Qh);
+  InsertHeadList (&Ehc->AsyncIntTransfers, &Urb->UrbList);
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  TimeOut               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSyncInterruptTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+
+  //
+  // Validates parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8))  ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 3072))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (EHC_TPL);
+  Ehc             = EHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: HC is halt\n"));
+
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_INT_TRANSFER_SYNC,
+          NULL,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: failed to create URB\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToPeriod (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+ON_EXIT:
+  Ehc->PciIo->Flush (Ehc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcIsochronousTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcAsyncIsochronousTransfer (
+  IN  EFI_USB2_HC_PROTOCOL                *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  UINT8                               DataBuffersNumber,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN  VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle       EFI_HANDLE.
+  @param  SystemTable       EFI_SYSTEM_TABLE.
+
+  @return EFI_SUCCESS       Success.
+          EFI_DEVICE_ERROR  Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gEhciDriverBinding,
+           ImageHandle,
+           &gEhciComponentName,
+           &gEhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  USB_CLASSC              UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to Ehci type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) || (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB)
+      || ((UsbClassCReg.ProgInterface != PCI_IF_EHCI) && (UsbClassCReg.ProgInterface != PCI_IF_UHCI) && (UsbClassCReg.ProgInterface != PCI_IF_OHCI))) {
+
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Get the usb debug port related information.
+
+  @param  Ehc                The EHCI device.
+
+  @retval RETURN_SUCCESS     Get debug port number, bar and offset successfully.
+  @retval Others             The usb host controller does not supported usb debug port capability.
+
+**/
+EFI_STATUS
+EhcGetUsbDebugPortInfo (
+  IN  USB2_HC_DEV     *Ehc
+ )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT16              PciStatus;
+  UINT8               CapabilityPtr;
+  UINT8               CapabilityId;
+  UINT16              DebugPort;
+  EFI_STATUS          Status;
+
+  ASSERT (Ehc->PciIo != NULL);
+  PciIo = Ehc->PciIo;
+
+  //
+  // Detect if the EHCI host controller support Capaility Pointer.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_PRIMARY_STATUS_OFFSET,
+                        sizeof (UINT16),
+                        &PciStatus
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((PciStatus & EFI_PCI_STATUS_CAPABILITY) == 0) {
+    //
+    // The Pci Device Doesn't Support Capability Pointer.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Pointer To Capability List
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CAPBILITY_POINTER_OFFSET,
+                        1,
+                        &CapabilityPtr
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Find Capability ID 0xA, Which Is For Debug Port
+  //
+  while (CapabilityPtr != 0) {
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint8,
+                          CapabilityPtr,
+                          1,
+                          &CapabilityId
+                          );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (CapabilityId == EHC_DEBUG_PORT_CAP_ID) {
+      break;
+    }
+
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint8,
+                          CapabilityPtr + 1,
+                          1,
+                          &CapabilityPtr
+                          );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // No Debug Port Capability Found
+  //
+  if (CapabilityPtr == 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get The Base Address Of Debug Port Register In Debug Port Capability Register
+  //
+  Status = PciIo->Pci.Read (
+                        Ehc->PciIo,
+                        EfiPciIoWidthUint8,
+                        CapabilityPtr + 2,
+                        sizeof (UINT16),
+                        &DebugPort
+                        );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ehc->DebugPortOffset = DebugPort & 0x1FFF;
+  Ehc->DebugPortBarNum = (UINT8)((DebugPort >> 13) - 1);
+  Ehc->DebugPortNum    = (UINT8)((Ehc->HcStructParams & 0x00F00000) >> 20);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create and initialize a USB2_HC_DEV.
+
+  @param  PciIo                  The PciIo on this device.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  Original PCI attributes.
+
+  @return  The allocated and initialized USB2_HC_DEV structure if created,
+           otherwise NULL.
+
+**/
+USB2_HC_DEV *
+EhcCreateUsb2Hc (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_STATUS              Status;
+
+  Ehc = AllocateZeroPool (sizeof (USB2_HC_DEV));
+
+  if (Ehc == NULL) {
+    return NULL;
+  }
+
+  //
+  // Init EFI_USB2_HC_PROTOCOL interface and private data structure
+  //
+  Ehc->Signature                        = USB2_HC_DEV_SIGNATURE;
+
+  Ehc->Usb2Hc.GetCapability             = EhcGetCapability;
+  Ehc->Usb2Hc.Reset                     = EhcReset;
+  Ehc->Usb2Hc.GetState                  = EhcGetState;
+  Ehc->Usb2Hc.SetState                  = EhcSetState;
+  Ehc->Usb2Hc.ControlTransfer           = EhcControlTransfer;
+  Ehc->Usb2Hc.BulkTransfer              = EhcBulkTransfer;
+  Ehc->Usb2Hc.AsyncInterruptTransfer    = EhcAsyncInterruptTransfer;
+  Ehc->Usb2Hc.SyncInterruptTransfer     = EhcSyncInterruptTransfer;
+  Ehc->Usb2Hc.IsochronousTransfer       = EhcIsochronousTransfer;
+  Ehc->Usb2Hc.AsyncIsochronousTransfer  = EhcAsyncIsochronousTransfer;
+  Ehc->Usb2Hc.GetRootHubPortStatus      = EhcGetRootHubPortStatus;
+  Ehc->Usb2Hc.SetRootHubPortFeature     = EhcSetRootHubPortFeature;
+  Ehc->Usb2Hc.ClearRootHubPortFeature   = EhcClearRootHubPortFeature;
+  Ehc->Usb2Hc.MajorRevision             = 0x2;
+  Ehc->Usb2Hc.MinorRevision             = 0x0;
+
+  Ehc->PciIo                 = PciIo;
+  Ehc->DevicePath            = DevicePath;
+  Ehc->OriginalPciAttributes = OriginalPciAttributes;
+
+  InitializeListHead (&Ehc->AsyncIntTransfers);
+
+  Ehc->HcStructParams = EhcReadCapRegister (Ehc, EHC_HCSPARAMS_OFFSET);
+  Ehc->HcCapParams    = EhcReadCapRegister (Ehc, EHC_HCCPARAMS_OFFSET);
+  Ehc->CapLen         = EhcReadCapRegister (Ehc, EHC_CAPLENGTH_OFFSET) & 0x0FF;
+
+  DEBUG ((EFI_D_INFO, "EhcCreateUsb2Hc: capability length %d\n", Ehc->CapLen));
+
+  //
+  // EHCI Controllers with a CapLen of 0 are ignored.
+  //
+  if (Ehc->CapLen == 0) {
+    gBS->FreePool (Ehc);
+    return NULL;
+  }
+  
+  EhcGetUsbDebugPortInfo (Ehc);
+
+  //
+  // Create AsyncRequest Polling Timer
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  EhcMonitorAsyncRequests,
+                  Ehc,
+                  &Ehc->PollTimer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (Ehc);
+    return NULL;
+  }
+
+  return Ehc;
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+EhcExitBootService (
+  EFI_EVENT                      Event,
+  VOID                           *Context
+  )
+
+{
+  USB2_HC_DEV   *Ehc;
+
+  Ehc = (USB2_HC_DEV *) Context;
+
+  //
+  // Reset the Host Controller
+  //
+  EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+}
+
+
+/**
+  Starting the Usb EHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  USB2_HC_DEV             *Ehc;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  EFI_PCI_IO_PROTOCOL     *Instance;
+  UINT64                  Supports;
+  UINT64                  OriginalPciAttributes;
+  BOOLEAN                 PciAttributesSaved;
+  USB_CLASSC              UsbClassCReg;
+  EFI_HANDLE              *HandleBuffer;
+  UINTN                   NumberOfHandles;
+  UINTN                   Index;
+  UINTN                   CompanionSegmentNumber;
+  UINTN                   CompanionBusNumber;
+  UINTN                   CompanionDeviceNumber;
+  UINTN                   CompanionFunctionNumber;
+  UINTN                   EhciSegmentNumber;
+  UINTN                   EhciBusNumber;
+  UINTN                   EhciDeviceNumber;
+  UINTN                   EhciFunctionNumber;
+  UINT32                  State;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open the PciIo Protocol, then enable the USB host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to enable controller\n"));
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Get the Pci device class code.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto CLOSE_PCIIO;
+  }
+  //
+  // Determine if the device is UHCI or OHCI host controller or not. If yes, then find out the 
+  // companion usb ehci host controller and force EHCI driver get attached to it before
+  // UHCI or OHCI driver attaches to UHCI or OHCI host controller.
+  //
+  if ((UsbClassCReg.ProgInterface == PCI_IF_UHCI || UsbClassCReg.ProgInterface == PCI_IF_OHCI) &&
+       (UsbClassCReg.BaseCode == PCI_CLASS_SERIAL) && 
+       (UsbClassCReg.SubClassCode == PCI_CLASS_SERIAL_USB)) {
+    Status = PciIo->GetLocation (
+                    PciIo,
+                    &CompanionSegmentNumber,
+                    &CompanionBusNumber,
+                    &CompanionDeviceNumber,
+                    &CompanionFunctionNumber
+                    );
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PCIIO;
+    }
+
+    Status = gBS->LocateHandleBuffer (
+                    ByProtocol,
+                    &gEfiPciIoProtocolGuid,
+                    NULL,
+                    &NumberOfHandles,
+                    &HandleBuffer
+                    );
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PCIIO;
+    }
+
+    for (Index = 0; Index < NumberOfHandles; Index++) {
+      //
+      // Get the device path on this handle
+      //
+      Status = gBS->HandleProtocol (
+                    HandleBuffer[Index],
+                    &gEfiPciIoProtocolGuid,
+                    (VOID **)&Instance
+                    );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = Instance->Pci.Read (
+                    Instance,
+                    EfiPciIoWidthUint8,
+                    PCI_CLASSCODE_OFFSET,
+                    sizeof (USB_CLASSC) / sizeof (UINT8),
+                    &UsbClassCReg
+                    );
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_UNSUPPORTED;
+        goto CLOSE_PCIIO;
+      }
+
+      if ((UsbClassCReg.ProgInterface == PCI_IF_EHCI) &&
+           (UsbClassCReg.BaseCode == PCI_CLASS_SERIAL) && 
+           (UsbClassCReg.SubClassCode == PCI_CLASS_SERIAL_USB)) {
+        Status = Instance->GetLocation (
+                    Instance,
+                    &EhciSegmentNumber,
+                    &EhciBusNumber,
+                    &EhciDeviceNumber,
+                    &EhciFunctionNumber
+                    );
+        if (EFI_ERROR (Status)) {
+          goto CLOSE_PCIIO;
+        }
+        //
+        // Currently, the judgment on the companion usb host controller is through the
+        // same bus number, which may vary on different platform.
+        //
+        if (EhciBusNumber == CompanionBusNumber) {
+          gBS->CloseProtocol (
+                    Controller,
+                    &gEfiPciIoProtocolGuid,
+                    This->DriverBindingHandle,
+                    Controller
+                    );
+          EhcDriverBindingStart(This, HandleBuffer[Index], NULL);
+        }
+      }
+    }
+    Status = EFI_NOT_FOUND;
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Create then install USB2_HC_PROTOCOL
+  //
+  Ehc = EhcCreateUsb2Hc (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Ehc == NULL) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to create USB2_HC\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto CLOSE_PCIIO;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Ehc->Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to install USB2_HC Protocol\n"));
+    goto FREE_POOL;
+  }
+
+  //
+  // Robustnesss improvement such as for Duet platform
+  // Default is not required.
+  //
+  if (FeaturePcdGet (PcdTurnOffUsbLegacySupport)) {
+    EhcClearLegacySupport (Ehc);
+  }
+
+  if (Ehc->DebugPortNum != 0) {
+    State = EhcReadDbgRegister(Ehc, 0);
+    if ((State & (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) != (USB_DEBUG_PORT_IN_USE | USB_DEBUG_PORT_OWNER)) {
+      EhcResetHC (Ehc, EHC_RESET_TIMEOUT);
+    }
+  }
+
+  Status = EhcInitHC (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to init host controller\n"));
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->SetTimer (Ehc->PollTimer, TimerPeriodic, EHC_ASYNC_POLL_INTERVAL);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcDriverBindingStart: failed to start async interrupt monitor\n"));
+
+    EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  EhcExitBootService,
+                  Ehc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Ehc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Install the component name protocol, don't fail the start
+  // because of something for display.
+  //
+  AddUnicodeString2 (
+    "eng",
+    gEhciComponentName.SupportedLanguages,
+    &Ehc->ControllerNameTable,
+    L"Enhanced Host Controller (USB 2.0)",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gEhciComponentName2.SupportedLanguages,
+    &Ehc->ControllerNameTable,
+    L"Enhanced Host Controller (USB 2.0)",
+    FALSE
+    );
+
+
+  DEBUG ((EFI_D_INFO, "EhcDriverBindingStart: EHCI started for controller @ %p\n", Controller));
+  return EFI_SUCCESS;
+
+UNINSTALL_USBHC:
+  gBS->UninstallProtocolInterface (
+         Controller,
+         &gEfiUsb2HcProtocolGuid,
+         &Ehc->Usb2Hc
+         );
+
+FREE_POOL:
+  EhcFreeSched (Ehc);
+  gBS->CloseEvent (Ehc->PollTimer);
+  gBS->FreePool (Ehc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB2_HC_DEV           *Ehc;
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ehc   = EHC_FROM_THIS (Usb2Hc);
+  PciIo = Ehc->PciIo;
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Stop AsyncRequest Polling timer then stop the EHCI driver
+  // and uninstall the EHCI protocl.
+  //
+  gBS->SetTimer (Ehc->PollTimer, TimerCancel, EHC_ASYNC_POLL_INTERVAL);
+  EhcHaltHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (Ehc->PollTimer != NULL) {
+    gBS->CloseEvent (Ehc->PollTimer);
+  }
+
+  if (Ehc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Ehc->ExitBootServiceEvent);
+  }
+
+  EhcFreeSched (Ehc);
+
+  if (Ehc->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (Ehc->ControllerNameTable);
+  }
+
+  //
+  // Disable routing of all ports to EHCI controller, so all ports are 
+  // routed back to the UHCI or OHCI controller.
+  //
+  EhcClearOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+                  PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Ehc->OriginalPciAttributes,
+                  NULL
+                  );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  FreePool (Ehc);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h
new file mode 100644
index 0000000000..7177658092
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/Ehci.h
@@ -0,0 +1,248 @@
+/** @file
+
+  Provides some data struct used by EHCI controller driver.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _EFI_EHCI_H_
+#define _EFI_EHCI_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB2_HC_DEV  USB2_HC_DEV;
+
+#include "UsbHcMem.h"
+#include "EhciReg.h"
+#include "EhciUrb.h"
+#include "EhciSched.h"
+#include "EhciDebug.h"
+#include "ComponentName.h"
+
+//
+// EHC timeout experience values
+//
+
+#define EHC_1_MICROSECOND            1
+#define EHC_1_MILLISECOND            (1000 * EHC_1_MICROSECOND)
+#define EHC_1_SECOND                 (1000 * EHC_1_MILLISECOND)
+
+//
+// EHCI register operation timeout, set by experience
+//
+#define EHC_RESET_TIMEOUT            (1 * EHC_1_SECOND)
+#define EHC_GENERIC_TIMEOUT          (10 * EHC_1_MILLISECOND)
+
+//
+// Wait for roothub port power stable, refers to Spec[EHCI1.0-2.3.9]
+//
+#define EHC_ROOT_PORT_RECOVERY_STALL (20 * EHC_1_MILLISECOND)
+
+//
+// Sync and Async transfer polling interval, set by experience,
+// and the unit of Async is 100us, means 1ms as interval.
+//
+#define EHC_SYNC_POLL_INTERVAL       (1 * EHC_1_MILLISECOND)
+#define EHC_ASYNC_POLL_INTERVAL      EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// EHCI debug port control status register bit definition
+//
+#define USB_DEBUG_PORT_IN_USE        BIT10
+#define USB_DEBUG_PORT_ENABLE        BIT28
+#define USB_DEBUG_PORT_OWNER         BIT30
+
+//
+// EHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  EHC_TPL                     TPL_NOTIFY
+
+//
+//Iterate through the doule linked list. NOT delete safe
+//
+#define EFI_LIST_FOR_EACH(Entry, ListHead)    \
+  for(Entry = (ListHead)->ForwardLink; Entry != (ListHead); Entry = Entry->ForwardLink)
+
+//
+//Iterate through the doule linked list. This is delete-safe.
+//Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+
+#define EHC_LOW_32BIT(Addr64)     ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define EHC_HIGH_32BIT(Addr64)    ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define EHC_BIT_IS_SET(Data, Bit) ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define EHC_REG_BIT_IS_SET(Ehc, Offset, Bit) \
+          (EHC_BIT_IS_SET(EhcReadOpReg ((Ehc), (Offset)), (Bit)))
+
+#define USB2_HC_DEV_SIGNATURE  SIGNATURE_32 ('e', 'h', 'c', 'i')
+#define EHC_FROM_THIS(a)       CR(a, USB2_HC_DEV, Usb2Hc, USB2_HC_DEV_SIGNATURE)
+
+struct _USB2_HC_DEV {
+  UINTN                     Signature;
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINT64                    OriginalPciAttributes;
+  USBHC_MEM_POOL            *MemPool;
+
+  //
+  // Schedule data shared between asynchronous and periodic
+  // transfers:
+  // ShortReadStop, as its name indicates, is used to terminate
+  // the short read except the control transfer. EHCI follows
+  // the alternative next QTD point when a short read happens.
+  // For control transfer, even the short read happens, try the
+  // status stage.
+  //
+  EHC_QTD                  *ShortReadStop;
+  EFI_EVENT                 PollTimer;
+
+  //
+  // ExitBootServicesEvent is used to stop the EHC DMA operation 
+  // after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+
+  //
+  // Asynchronous(bulk and control) transfer schedule data:
+  // ReclaimHead is used as the head of the asynchronous transfer
+  // list. It acts as the reclamation header.
+  //
+  EHC_QH                   *ReclaimHead;
+
+  //
+  // Peroidic (interrupt) transfer schedule data:
+  //
+  VOID                      *PeriodFrame;     // the buffer pointed by this pointer is used to store pci bus address of the QH descriptor.
+  VOID                      *PeriodFrameHost; // the buffer pointed by this pointer is used to store host memory address of the QH descriptor.
+  VOID                      *PeriodFrameMap;
+
+  EHC_QH                    *PeriodOne;
+  LIST_ENTRY                AsyncIntTransfers;
+
+  //
+  // EHCI configuration data
+  //
+  UINT32                    HcStructParams; // Cache of HC structure parameter, EHC_HCSPARAMS_OFFSET
+  UINT32                    HcCapParams;    // Cache of HC capability parameter, HCCPARAMS
+  UINT32                    CapLen;         // Capability length
+
+  //
+  // Misc
+  //
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // EHCI debug port info
+  //
+  UINT16                    DebugPortOffset; // The offset of debug port mmio register
+  UINT8                     DebugPortBarNum; // The bar number of debug port mmio register
+  UINT8                     DebugPortNum;    // The port number of usb debug port
+};
+
+
+extern EFI_DRIVER_BINDING_PROTOCOL      gEhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL      gEhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL     gEhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb EHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c
new file mode 100644
index 0000000000..76368b4748
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.c
@@ -0,0 +1,258 @@
+/** @file
+
+  This file provides the information dump support for EHCI when in debug mode.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+/**
+  Dump the status byte in QTD/QH to a more friendly format.
+
+  @param  State    The state in the QTD/QH.
+
+**/
+VOID
+EhcDumpStatus (
+  IN UINT32               State
+  )
+{
+  if (EHC_BIT_IS_SET (State, QTD_STAT_DO_PING)) {
+    DEBUG ((EFI_D_VERBOSE, "  Do_Ping"));
+  } else {
+    DEBUG ((EFI_D_VERBOSE, "  Do_Out"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_DO_CS)) {
+    DEBUG ((EFI_D_VERBOSE, "  Do_CS"));
+  } else {
+    DEBUG ((EFI_D_VERBOSE, "  Do_SS"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Transfer_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Babble_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+    DEBUG ((EFI_D_VERBOSE, "  Buffer_Error"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+    DEBUG ((EFI_D_VERBOSE, "  Halted"));
+  }
+
+  if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+    DEBUG ((EFI_D_VERBOSE, "  Active"));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+}
+
+
+/**
+  Dump the fields of a QTD.
+
+  @param  Qtd      The QTD to dump.
+  @param  Msg      The message to print before the dump.
+
+**/
+VOID
+EhcDumpQtd (
+  IN EHC_QTD              *Qtd,
+  IN CHAR8                *Msg
+  )
+{
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+
+  if (Msg != NULL) {
+    DEBUG ((EFI_D_VERBOSE, Msg));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Queue TD @ 0x%p, data length %d\n", Qtd, (UINT32)Qtd->DataLen));
+
+  QtdHw = &Qtd->QtdHw;
+
+  DEBUG ((EFI_D_VERBOSE, "Next QTD     : %x\n", QtdHw->NextQtd));
+  DEBUG ((EFI_D_VERBOSE, "AltNext QTD  : %x\n", QtdHw->AltNext));
+  DEBUG ((EFI_D_VERBOSE, "Status       : %x\n", QtdHw->Status));
+  EhcDumpStatus (QtdHw->Status);
+
+  if (QtdHw->Pid == QTD_PID_SETUP) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : Setup\n"));
+
+  } else if (QtdHw->Pid == QTD_PID_INPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : IN\n"));
+
+  } else if (QtdHw->Pid == QTD_PID_OUTPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID          : OUT\n"));
+
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Error Count  : %d\n", QtdHw->ErrCnt));
+  DEBUG ((EFI_D_VERBOSE, "Current Page : %d\n", QtdHw->CurPage));
+  DEBUG ((EFI_D_VERBOSE, "IOC          : %d\n", QtdHw->Ioc));
+  DEBUG ((EFI_D_VERBOSE, "Total Bytes  : %d\n", QtdHw->TotalBytes));
+  DEBUG ((EFI_D_VERBOSE, "Data Toggle  : %d\n", QtdHw->DataToggle));
+
+  for (Index = 0; Index < 5; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "Page[%d]      : 0x%x\n", (UINT32)Index, QtdHw->Page[Index]));
+  }
+}
+
+
+/**
+  Dump the queue head.
+
+  @param  Qh       The queue head to dump.
+  @param  Msg      The message to print before the dump.
+  @param  DumpBuf  Whether to dump the memory buffer of the associated QTD.
+
+**/
+VOID
+EhcDumpQh (
+  IN EHC_QH               *Qh,
+  IN CHAR8                *Msg,
+  IN BOOLEAN              DumpBuf
+  )
+{
+  EHC_QTD                 *Qtd;
+  QH_HW                   *QhHw;
+  LIST_ENTRY              *Entry;
+  UINTN                   Index;
+
+  if (Msg != NULL) {
+    DEBUG ((EFI_D_VERBOSE, Msg));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Queue head @ 0x%p, interval %ld, next qh %p\n",
+                                Qh, (UINT64)Qh->Interval, Qh->NextQh));
+
+  QhHw = &Qh->QhHw;
+
+  DEBUG ((EFI_D_VERBOSE, "Hoziontal link: %x\n", QhHw->HorizonLink));
+  DEBUG ((EFI_D_VERBOSE, "Device address: %d\n", QhHw->DeviceAddr));
+  DEBUG ((EFI_D_VERBOSE, "Inactive      : %d\n", QhHw->Inactive));
+  DEBUG ((EFI_D_VERBOSE, "EP number     : %d\n", QhHw->EpNum));
+  DEBUG ((EFI_D_VERBOSE, "EP speed      : %d\n", QhHw->EpSpeed));
+  DEBUG ((EFI_D_VERBOSE, "DT control    : %d\n", QhHw->DtCtrl));
+  DEBUG ((EFI_D_VERBOSE, "Reclaim head  : %d\n", QhHw->ReclaimHead));
+  DEBUG ((EFI_D_VERBOSE, "Max packet len: %d\n", QhHw->MaxPacketLen));
+  DEBUG ((EFI_D_VERBOSE, "Ctrl EP       : %d\n", QhHw->CtrlEp));
+  DEBUG ((EFI_D_VERBOSE, "Nak reload    : %d\n", QhHw->NakReload));
+
+  DEBUG ((EFI_D_VERBOSE, "SMask         : %x\n", QhHw->SMask));
+  DEBUG ((EFI_D_VERBOSE, "CMask         : %x\n", QhHw->CMask));
+  DEBUG ((EFI_D_VERBOSE, "Hub address   : %d\n", QhHw->HubAddr));
+  DEBUG ((EFI_D_VERBOSE, "Hub port      : %d\n", QhHw->PortNum));
+  DEBUG ((EFI_D_VERBOSE, "Multiplier    : %d\n", QhHw->Multiplier));
+
+  DEBUG ((EFI_D_VERBOSE, "Cur QTD       : %x\n", QhHw->CurQtd));
+
+  DEBUG ((EFI_D_VERBOSE, "Next QTD      : %x\n", QhHw->NextQtd));
+  DEBUG ((EFI_D_VERBOSE, "AltNext QTD   : %x\n", QhHw->AltQtd));
+  DEBUG ((EFI_D_VERBOSE, "Status        : %x\n", QhHw->Status));
+
+  EhcDumpStatus (QhHw->Status);
+
+  if (QhHw->Pid == QTD_PID_SETUP) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : Setup\n"));
+
+  } else if (QhHw->Pid == QTD_PID_INPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : IN\n"));
+
+  } else if (QhHw->Pid == QTD_PID_OUTPUT) {
+    DEBUG ((EFI_D_VERBOSE, "PID           : OUT\n"));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "Error Count   : %d\n", QhHw->ErrCnt));
+  DEBUG ((EFI_D_VERBOSE, "Current Page  : %d\n", QhHw->CurPage));
+  DEBUG ((EFI_D_VERBOSE, "IOC           : %d\n", QhHw->Ioc));
+  DEBUG ((EFI_D_VERBOSE, "Total Bytes   : %d\n", QhHw->TotalBytes));
+  DEBUG ((EFI_D_VERBOSE, "Data Toggle   : %d\n", QhHw->DataToggle));
+
+  for (Index = 0; Index < 5; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "Page[%d]       : 0x%x\n", Index, QhHw->Page[Index]));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+    EhcDumpQtd (Qtd, NULL);
+
+    if (DumpBuf && (Qtd->DataLen != 0)) {
+      EhcDumpBuf (Qtd->Data, Qtd->DataLen);
+    }
+  }
+}
+
+
+/**
+  Dump the buffer in the form of hex.
+
+  @param  Buf      The buffer to dump.
+  @param  Len      The length of buffer.
+
+**/
+VOID
+EhcDumpBuf (
+  IN UINT8                *Buf,
+  IN UINTN                Len
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Len; Index++) {
+    if (Index % 16 == 0) {
+      DEBUG ((EFI_D_VERBOSE,"\n"));
+    }
+
+    DEBUG ((EFI_D_VERBOSE, "%02x ", Buf[Index]));
+  }
+
+  DEBUG ((EFI_D_VERBOSE, "\n"));
+}
+
+/**
+  Dump the EHCI status registers.
+
+  @param  Ehc    USB EHCI Host Controller instance
+
+**/
+VOID
+EhcDumpRegs (
+  IN  USB2_HC_DEV         *Ehc
+  )
+{
+  UINT8   Index;
+
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CAPLENGTH_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_CAPLENGTH_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_HCSPARAMS_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_HCSPARAMS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_HCCPARAMS_OFFSET   = 0x%08x\n", EhcReadCapRegister (Ehc, EHC_HCCPARAMS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBCMD_OFFSET      = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBCMD_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBSTS_OFFSET      = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_USBINTR_OFFSET     = 0x%08x\n", EhcReadOpReg (Ehc, EHC_USBINTR_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_FRINDEX_OFFSET     = 0x%08x\n", EhcReadOpReg (Ehc, EHC_FRINDEX_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CTRLDSSEG_OFFSET   = 0x%08x\n", EhcReadOpReg (Ehc,  EHC_CTRLDSSEG_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_FRAME_BASE_OFFSET  = 0x%08x\n", EhcReadOpReg (Ehc,  EHC_FRAME_BASE_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_ASYNC_HEAD_OFFSET  = 0x%08x\n", EhcReadOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET)));
+  DEBUG ((EFI_D_VERBOSE, "  EHC_CONFIG_FLAG_OFFSET = 0x%08x\n", EhcReadOpReg (Ehc, EHC_CONFIG_FLAG_OFFSET)));
+  for (Index = 0; Index < (UINT8) (Ehc->HcStructParams & HCSP_NPORTS); Index++) {
+    DEBUG ((EFI_D_VERBOSE, "  EHC_PORT_STAT_OFFSET(%d)  = 0x%08x\n", Index, EhcReadOpReg (Ehc, EHC_PORT_STAT_OFFSET + (4 * Index))));
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h
new file mode 100644
index 0000000000..bc84bb7864
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDebug.h
@@ -0,0 +1,75 @@
+/** @file
+
+  This file contains the definination for host controller debug support routines.
+
+Copyright (c) 2007 - 2009, 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.
+
+**/
+
+#ifndef _EFI_EHCI_DEBUG_H_
+#define _EFI_EHCI_DEBUG_H_
+
+
+/**
+  Dump the fields of a QTD.
+
+  @param  Qtd      The QTD to dump.
+  @param  Msg      The message to print before the dump.
+
+**/
+VOID
+EhcDumpQtd (
+  IN EHC_QTD              *Qtd,
+  IN CHAR8                *Msg
+  );
+
+
+/**
+  Dump the queue head.
+
+  @param  Qh       The queue head to dump.
+  @param  Msg      The message to print before the dump.
+  @param  DumpBuf  Whether to dump the memory buffer of the associated QTD.
+
+**/
+VOID
+EhcDumpQh (
+  IN EHC_QH               *Qh,
+  IN CHAR8                *Msg,
+  IN BOOLEAN              DumpBuf
+  );
+
+
+/**
+  Dump the buffer in the form of hex.
+
+  @param  Buf      The buffer to dump.
+  @param  Len      The length of buffer.
+
+**/
+VOID
+EhcDumpBuf (
+  IN UINT8                *Buf,
+  IN UINTN                Len
+  );
+
+
+/**
+  Dump the EHCI status registers.
+
+  @param  Ehc    USB EHCI Host Controller instance
+
+**/
+VOID
+EhcDumpRegs (
+  IN  USB2_HC_DEV         *Ehc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf
new file mode 100644
index 0000000000..238923e828
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf
@@ -0,0 +1,90 @@
+## @file
+#  The EhciDxe driver is responsible for managing the behavior of EHCI controller.
+#  It implements the interfaces of monitoring the status of all ports and transferring 
+#  Control, Bulk, Interrupt and Isochronous requests to Usb2.0 device.
+#
+#  Note that EhciDxe driver is enhanced to guarantee that the EHCI controller get attached
+#  to the EHCI controller before the UHCI driver attaches to the companion UHCI controller. 
+#  This way avoids the control transfer on a shared port between EHCI and companion host
+#  controller when UHCI gets attached earlier than EHCI and a USB 2.0 device inserts.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EhciDxe
+  MODULE_UNI_FILE                = EhciDxe.uni
+  FILE_GUID                      = BDFE430E-8F2A-4db0-9991-6F856594777E
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = EhcDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gEhciDriverBinding                        
+#  COMPONENT_NAME                =  gEhciComponentName
+#  COMPONENT_NAME2               =  gEhciComponentName2
+#
+
+[Sources]
+  UsbHcMem.h
+  EhciUrb.c
+  EhciReg.h
+  UsbHcMem.c
+  EhciSched.c
+  EhciDebug.c
+  EhciReg.c
+  EhciDebug.h
+  ComponentName.c
+  ComponentName.h
+  EhciUrb.h
+  Ehci.h
+  EhciSched.h
+  Ehci.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTurnOffUsbLegacySupport  ## CONSUMES
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  PcdLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni
new file mode 100644
index 0000000000..0ddb7ab705
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni
new file mode 100644
index 0000000000..0039424c15
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
new file mode 100644
index 0000000000..88a66aee71
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.c
@@ -0,0 +1,658 @@
+/** @file
+
+  The EHCI register operation routines.
+
+Copyright (c) 2007 - 2012, 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 "Ehci.h"
+
+
+/**
+  Read EHCI capability register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Capability register address.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadCapRegister: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read EHCI debug port register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Debug port register offset.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadDbgRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             Ehc->DebugPortBarNum,
+                             (UINT64) (Ehc->DebugPortOffset + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadDbgRegister: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Read EHCI Operation register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The operation register offset.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Ehc->CapLen != 0);
+
+  Status = Ehc->PciIo->Mem.Read (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) (Ehc->CapLen + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Write  the data to the EHCI operation register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       EHCI operation register offset.
+  @param  Data         The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Ehc->CapLen != 0);
+
+  Status = Ehc->PciIo->Mem.Write (
+                             Ehc->PciIo,
+                             EfiPciIoWidthUint32,
+                             EHC_BAR_INDEX,
+                             (UINT64) (Ehc->CapLen + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data |= Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data &= ~Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operation register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in millisecond).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+EhcWaitOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+
+  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_INTERVAL + 1; Index++) {
+    if (EHC_REG_BIT_IS_SET (Ehc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (EHC_SYNC_POLL_INTERVAL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Add support for UEFI Over Legacy (UoL) feature, stop
+  the legacy USB SMI support.
+
+  @param  Ehc          The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  UINT32                    ExtendCap;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT32                    Value;
+  UINT32                    TimeOut;
+
+  DEBUG ((EFI_D_INFO, "EhcClearLegacySupport: called to clear legacy support\n"));
+
+  PciIo     = Ehc->PciIo;
+  ExtendCap = (Ehc->HcCapParams >> 8) & 0xFF;
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap + 0x4, 1, &Value);
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  Value |= (0x1 << 24);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+
+  TimeOut = 40;
+  while (TimeOut-- != 0) {
+    gBS->Stall (500);
+
+    PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+
+    if ((Value & 0x01010000) == 0x01000000) {
+      break;
+    }
+  }
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap, 1, &Value);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, ExtendCap + 0x4, 1, &Value);
+}
+
+
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  Synchronized with the hardware.
+  @retval EFI_TIMEOUT  Time out happened while waiting door bell to set.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_IAAD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_IAA, TRUE, Timeout);
+
+  //
+  // ACK the IAA bit in USBSTS register. Make sure other
+  // interrupt bits are not ACKed. These bits are WC (Write Clean).
+  //
+  Data  = EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET);
+  Data &= ~USBSTS_INTACK_MASK;
+  Data |= USBSTS_IAA;
+
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, Data);
+
+  return Status;
+}
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Ehc          The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  USB2_HC_DEV         *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, USBSTS_INTACK_MASK);
+}
+
+
+/**
+  Enable the periodic schedule then wait EHC to
+  actually enable it.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The periodical schedule is enabled.
+  @retval EFI_TIMEOUT  Time out happened while enabling periodic schedule.
+
+**/
+EFI_STATUS
+EhcEnablePeriodSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Disable periodic schedule.
+
+  @param  Ehc               The EHCI device.
+  @param  Timeout           Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS       Periodic schedule is disabled.
+  @retval EFI_DEVICE_ERROR  Fail to disable periodic schedule.
+
+**/
+EFI_STATUS
+EhcDisablePeriodSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, FALSE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Enable asynchrounous schedule.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI asynchronous schedule is enabled.
+  @return Others       Failed to enable the asynchronous scheudle.
+
+**/
+EFI_STATUS
+EhcEnableAsyncSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Disable asynchrounous schedule.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The asynchronous schedule is disabled.
+  @return Others       Failed to disable the asynchronous schedule.
+
+**/
+EFI_STATUS
+EhcDisableAsyncSchd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS  Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, FALSE, Timeout);
+  return Status;
+}
+
+
+
+/**
+  Whether Ehc is halted.
+
+  @param  Ehc          The EHCI device.
+
+  @retval TRUE         The controller is halted.
+  @retval FALSE        It isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT);
+}
+
+
+/**
+  Whether system error occurred.
+
+  @param  Ehc          The EHCI device.
+
+  @return TRUE         System error happened.
+  @return FALSE        No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR);
+}
+
+
+/**
+  Reset the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The host controller is reset.
+  @return Others       Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    Status = EhcHaltHC (Ehc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET, FALSE, Timeout);
+  return Status;
+}
+
+
+/**
+  Halt the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI is halt.
+  @retval EFI_TIMEOUT  Failed to halt the controller before Timeout.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN USB2_HC_DEV         *Ehc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Set the EHCI to run.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @retval EFI_SUCCESS  The EHCI is running.
+  @return Others       Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
+
+/**
+  Initialize the HC hardware.
+  EHCI spec lists the five things to do to initialize the hardware:
+  1. Program CTRLDSSEGMENT
+  2. Set USBINTR to enable interrupts
+  3. Set periodic list base
+  4. Set USBCMD, interrupt threshold, frame list size etc
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI
+
+  @param  Ehc          The EHCI device.
+
+  @return EFI_SUCCESS  The EHCI has come out of halt state.
+  @return EFI_TIMEOUT  Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Index;
+
+  // This ASSERT crashes the BeagleBoard. There is some issue in the USB stack.
+  // This ASSERT needs to be removed so the BeagleBoard will boot. When we fix
+  // the USB stack we can put this ASSERT back in
+  // ASSERT (EhcIsHalt (Ehc));
+
+  //
+  // Allocate the periodic frame and associated memeory
+  // management facilities if not already done.
+  //
+  if (Ehc->PeriodFrame != NULL) {
+    EhcFreeSched (Ehc);
+  }
+
+  Status = EhcInitSched (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // 1. Clear USBINTR to disable all the interrupt. UEFI works by polling
+  //
+  EhcWriteOpReg (Ehc, EHC_USBINTR_OFFSET, 0);
+
+  //
+  // 2. Start the Host Controller
+  //
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+
+  //
+  // 3. Power up all ports if EHCI has Port Power Control (PPC) support
+  //
+  if (Ehc->HcStructParams & HCSP_PPC) {
+    for (Index = 0; Index < (UINT8) (Ehc->HcStructParams & HCSP_NPORTS); Index++) {
+      EhcSetOpRegBit (Ehc, (UINT32) (EHC_PORT_STAT_OFFSET + (4 * Index)), PORTSC_POWER);
+    }
+  }
+
+  //
+  // Wait roothub port power stable
+  //
+  gBS->Stall (EHC_ROOT_PORT_RECOVERY_STALL);
+
+  //
+  // 4. Set all ports routing to EHC
+  //
+  EhcSetOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable period schedule\n"));
+    return Status;
+  }
+
+  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcInitHC: failed to enable async schedule\n"));
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h
new file mode 100644
index 0000000000..2347ee125f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciReg.h
@@ -0,0 +1,363 @@
+/** @file
+
+  This file contains the definination for host controller register operation routines.
+
+Copyright (c) 2007 - 2012, 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.
+
+**/
+
+#ifndef _EFI_EHCI_REG_H_
+#define _EFI_EHCI_REG_H_
+
+//
+// EHCI register offset
+//
+
+
+//
+// Capability register offset
+//
+#define EHC_CAPLENGTH_OFFSET    0    // Capability register length offset
+#define EHC_HCSPARAMS_OFFSET    0x04 // Structural Parameters 04-07h
+#define EHC_HCCPARAMS_OFFSET    0x08 // Capability parameters offset
+
+//
+// Capability register bit definition
+//
+#define HCSP_NPORTS             0x0F // Number of root hub port
+#define HCSP_PPC                0x10 // Port Power Control
+#define HCCP_64BIT              0x01 // 64-bit addressing capability
+
+//
+// Operational register offset
+//
+#define EHC_USBCMD_OFFSET       0x0  // USB command register offset
+#define EHC_USBSTS_OFFSET       0x04 // Statue register offset
+#define EHC_USBINTR_OFFSET      0x08 // USB interrutp offset
+#define EHC_FRINDEX_OFFSET      0x0C // Frame index offset
+#define EHC_CTRLDSSEG_OFFSET    0x10 // Control data structure segment offset
+#define EHC_FRAME_BASE_OFFSET   0x14 // Frame list base address offset
+#define EHC_ASYNC_HEAD_OFFSET   0x18 // Next asynchronous list address offset
+#define EHC_CONFIG_FLAG_OFFSET  0x40 // Configure flag register offset
+#define EHC_PORT_STAT_OFFSET    0x44 // Port status/control offset
+
+#define EHC_FRAME_LEN           1024
+
+//
+// Register bit definition
+//
+#define CONFIGFLAG_ROUTE_EHC    0x01 // Route port to EHC
+
+#define USBCMD_RUN              0x01   // Run/stop
+#define USBCMD_RESET            0x02   // Start the host controller reset
+#define USBCMD_ENABLE_PERIOD    0x10   // Enable periodic schedule
+#define USBCMD_ENABLE_ASYNC     0x20   // Enable asynchronous schedule
+#define USBCMD_IAAD             0x40   // Interrupt on async advance doorbell
+
+#define USBSTS_IAA              0x20   // Interrupt on async advance
+#define USBSTS_PERIOD_ENABLED   0x4000 // Periodic schedule status
+#define USBSTS_ASYNC_ENABLED    0x8000 // Asynchronous schedule status
+#define USBSTS_HALT             0x1000 // Host controller halted
+#define USBSTS_SYS_ERROR        0x10   // Host system error
+#define USBSTS_INTACK_MASK      0x003F // Mask for the interrupt ACK, the WC
+                                       // (write clean) bits in USBSTS register
+
+#define PORTSC_CONN             0x01   // Current Connect Status
+#define PORTSC_CONN_CHANGE      0x02   // Connect Status Change
+#define PORTSC_ENABLED          0x04   // Port Enable / Disable
+#define PORTSC_ENABLE_CHANGE    0x08   // Port Enable / Disable Change
+#define PORTSC_OVERCUR          0x10   // Over current Active
+#define PORTSC_OVERCUR_CHANGE   0x20   // Over current Change
+#define PORSTSC_RESUME          0x40   // Force Port Resume
+#define PORTSC_SUSPEND          0x80   // Port Suspend State
+#define PORTSC_RESET            0x100  // Port Reset
+#define PORTSC_LINESTATE_K      0x400  // Line Status K-state
+#define PORTSC_LINESTATE_J      0x800  // Line Status J-state
+#define PORTSC_POWER            0x1000 // Port Power
+#define PORTSC_OWNER            0x2000 // Port Owner
+#define PORTSC_CHANGE_MASK      0x2A   // Mask of the port change bits,
+                                       // they are WC (write clean)
+//
+// PCI Configuration Registers
+//
+#define EHC_BAR_INDEX           0      // how many bytes away from USB_BASE to 0x10
+
+//
+// Debug port capability id
+//
+#define EHC_DEBUG_PORT_CAP_ID   0x0A
+
+#define EHC_LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define EHC_ADDR(High, QhHw32)   \
+        ((VOID *) (UINTN) (LShiftU64 ((High), 32) | ((QhHw32) & 0xFFFFFFF0)))
+
+#define EHCI_IS_DATAIN(EndpointAddr) EHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT16                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Ehci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   ProgInterface;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+/**
+  Read EHCI capability register.
+
+  @param  Ehc     The EHCI device.
+  @param  Offset  Capability register address.
+
+  @return The register content.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read EHCI debug port register.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       Debug port register address.
+
+  @return The register content read.
+  @retval If err, return 0xffff.
+
+**/
+UINT32
+EhcReadDbgRegister (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read EHCI Operation register.
+
+  @param  Ehc      The EHCI device.
+  @param  Offset   The operation register offset.
+
+  @return The register content.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32              Offset
+  );
+
+
+/**
+  Write  the data to the EHCI operation register.
+
+  @param  Ehc      The EHCI device.
+  @param  Offset   EHCI operation register offset.
+  @param  Data     The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Ehc          The EHCI device.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Add support for UEFI Over Legacy (UoL) feature, stop
+  the legacy USB SMI support.
+
+  @param  Ehc      The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  Synchronized with the hardware.
+  @retval EFI_TIMEOUT  Time out happened while waiting door bell to set.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT32               Timeout
+  );
+
+
+/**
+  Clear all the interrutp status bits, these bits are Write-Clean.
+
+  @param  Ehc      The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  USB2_HC_DEV         *Ehc
+  );
+
+
+
+/**
+  Whether Ehc is halted.
+
+  @param  Ehc     The EHCI device.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Whether system error occurred.
+
+  @param  Ehc      The EHCI device.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Reset the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The host controller is reset.
+  @return Others       Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  );
+
+
+/**
+  Halt the host controller.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @return EFI_SUCCESS  The EHCI is halt.
+  @return EFI_TIMEOUT  Failed to halt the controller before Timeout.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN USB2_HC_DEV         *Ehc,
+  IN UINT32              Timeout
+  );
+
+
+/**
+  Set the EHCI to run.
+
+  @param  Ehc          The EHCI device.
+  @param  Timeout      Time to wait before abort.
+
+  @return EFI_SUCCESS  The EHCI is running.
+  @return Others       Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT32               Timeout
+  );
+
+
+
+/**
+  Initialize the HC hardware.
+  EHCI spec lists the five things to do to initialize the hardware:
+  1. Program CTRLDSSEGMENT
+  2. Set USBINTR to enable interrupts
+  3. Set periodic list base
+  4. Set USBCMD, interrupt threshold, frame list size etc
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI
+
+  @param  Ehc          The EHCI device.
+
+  @return EFI_SUCCESS  The EHCI has come out of halt state.
+  @return EFI_TIMEOUT  Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c
new file mode 100644
index 0000000000..5594e6699e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.c
@@ -0,0 +1,1052 @@
+/** @file
+
+  EHCI transfer scheduling routines.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+
+/**
+  Create helper QTD/QH for the EHCI device.
+
+  @param  Ehc                   The EHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for helper QTD/QH.
+  @retval EFI_SUCCESS           Helper QH/QTD are created.
+
+**/
+EFI_STATUS
+EhcCreateHelpQ (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  USB_ENDPOINT            Ep;
+  EHC_QH                  *Qh;
+  QH_HW                   *QhHw;
+  EHC_QTD                 *Qtd;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  //
+  // Create an inactive Qtd to terminate the short packet read.
+  //
+  Qtd = EhcCreateQtd (Ehc, NULL, NULL, 0, QTD_PID_INPUT, 0, 64);
+
+  if (Qtd == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qtd->QtdHw.Status   = QTD_STAT_HALTED;
+  Ehc->ShortReadStop  = Qtd;
+
+  //
+  // Create a QH to act as the EHC reclamation header.
+  // Set the header to loopback to itself.
+  //
+  Ep.DevAddr    = 0;
+  Ep.EpAddr     = 1;
+  Ep.Direction  = EfiUsbDataIn;
+  Ep.DevSpeed   = EFI_USB_SPEED_HIGH;
+  Ep.MaxPacket  = 64;
+  Ep.HubAddr    = 0;
+  Ep.HubPort    = 0;
+  Ep.Toggle     = 0;
+  Ep.Type       = EHC_BULK_TRANSFER;
+  Ep.PollRate   = 1;
+
+  Qh            = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  PciAddr           = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+  QhHw              = &Qh->QhHw;
+  QhHw->HorizonLink = QH_LINK (PciAddr + OFFSET_OF(EHC_QH, QhHw), EHC_TYPE_QH, FALSE);
+  QhHw->Status      = QTD_STAT_HALTED;
+  QhHw->ReclaimHead = 1;
+  Qh->NextQh        = Qh;
+  Ehc->ReclaimHead  = Qh;
+
+  //
+  // Create a dummy QH to act as the terminator for periodical schedule
+  //
+  Ep.EpAddr   = 2;
+  Ep.Type     = EHC_INT_TRANSFER_SYNC;
+
+  Qh          = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qh->QhHw.Status = QTD_STAT_HALTED;
+  Ehc->PeriodOne  = Qh;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize the schedule data structure such as frame list.
+
+  @param  Ehc                   The EHCI device to init schedule data.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  VOID                  *Buf;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+  VOID                  *Map;
+  UINTN                 Pages;
+  UINTN                 Bytes;
+  UINTN                 Index;
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PciAddr;
+
+  //
+  // First initialize the periodical schedule data:
+  // 1. Allocate and map the memory for the frame list
+  // 2. Create the help QTD/QH
+  // 3. Initialize the frame entries
+  // 4. Set the frame list register
+  //
+  PciIo = Ehc->PciIo;
+
+  Bytes = 4096;
+  Pages = EFI_SIZE_TO_PAGES (Bytes);
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &Buf,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    Buf,
+                    &Bytes,
+                    &PhyAddr,
+                    &Map
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != 4096)) {
+    PciIo->FreeBuffer (PciIo, Pages, Buf);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ehc->PeriodFrame      = Buf;
+  Ehc->PeriodFrameMap   = Map;
+
+  //
+  // Program the FRAMELISTBASE register with the low 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (PhyAddr));
+  //
+  // Program the CTRLDSSEGMENT register with the high 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_CTRLDSSEG_OFFSET, EHC_HIGH_32BIT (PhyAddr));
+
+  //
+  // Init memory pool management then create the helper
+  // QTD/QH. If failed, previously allocated resources
+  // will be freed by EhcFreeSched
+  //
+  Ehc->MemPool = UsbHcInitMemPool (
+                   PciIo,
+                   EHC_BIT_IS_SET (Ehc->HcCapParams, HCCP_64BIT),
+                   EHC_HIGH_32BIT (PhyAddr)
+                   );
+
+  if (Ehc->MemPool == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit1;
+  }
+
+  Status = EhcCreateHelpQ (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize the frame list entries then set the registers
+  //
+  Ehc->PeriodFrameHost      = AllocateZeroPool (EHC_FRAME_LEN * sizeof (UINTN));
+  if (Ehc->PeriodFrameHost == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  PciAddr  = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index++) {
+    //
+    // Store the pci bus address of the QH in period frame list which will be accessed by pci bus master.
+    //
+    ((UINT32 *)(Ehc->PeriodFrame))[Index] = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    //
+    // Store the host address of the QH in period frame list which will be accessed by host.
+    //
+    ((UINTN *)(Ehc->PeriodFrameHost))[Index] = (UINTN)Ehc->PeriodOne;
+  }
+
+  //
+  // Second initialize the asynchronous schedule:
+  // Only need to set the AsynListAddr register to
+  // the reclamation header
+  //
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, EHC_LOW_32BIT (PciAddr));
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+ErrorExit1:
+  PciIo->FreeBuffer (PciIo, Pages, Buf);
+  PciIo->Unmap (PciIo, Map);
+
+  return Status;
+}
+
+
+/**
+  Free the schedule data. It may be partially initialized.
+
+  @param  Ehc                   The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, 0);
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, 0);
+
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+  if (Ehc->MemPool != NULL) {
+    UsbHcFreeMemPool (Ehc->MemPool);
+    Ehc->MemPool = NULL;
+  }
+
+  if (Ehc->PeriodFrame != NULL) {
+    PciIo = Ehc->PciIo;
+    ASSERT (PciIo != NULL);
+
+    PciIo->Unmap (PciIo, Ehc->PeriodFrameMap);
+
+    PciIo->FreeBuffer (
+             PciIo,
+             EFI_SIZE_TO_PAGES (EFI_PAGE_SIZE),
+             Ehc->PeriodFrame
+             );
+
+    Ehc->PeriodFrame = NULL;
+  }
+
+  if (Ehc->PeriodFrameHost != NULL) {
+    FreePool (Ehc->PeriodFrameHost);
+    Ehc->PeriodFrameHost = NULL;
+  }
+}
+
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  EHC_QH                  *Head;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  //
+  // Append the queue head after the reclaim header, then
+  // fix the hardware visiable parts (EHCI R1.0 page 72).
+  // ReclaimHead is always linked to the EHCI's AsynListAddr.
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Qh->NextQh              = Head->NextQh;
+  Head->NextQh            = Qh;
+
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh->NextQh, sizeof (EHC_QH));
+  Qh->QhHw.HorizonLink    = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Head->NextQh, sizeof (EHC_QH));
+  Head->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+}
+
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  EHC_QH                  *Head;
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  ASSERT (Ehc->ReclaimHead->NextQh == Qh);
+
+  //
+  // Remove the QH from reclamation head, then update the hardware
+  // visiable part: Only need to loopback the ReclaimHead. The Qh
+  // is pointing to ReclaimHead (which is staill in the list).
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Head->NextQh            = Qh->NextQh;
+  Qh->NextQh              = NULL;
+
+  PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Head->NextQh, sizeof (EHC_QH));
+  Head->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+
+  //
+  // Set and wait the door bell to synchronize with the hardware
+  //
+  Status = EhcSetAndWaitDoorBell (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "EhcUnlinkQhFromAsync: Failed to synchronize with doorbell\n"));
+  }
+}
+
+
+/**
+  Link a queue head for interrupt transfer to the periodic
+  schedule frame list. This code is very much the same as
+  that in UHCI.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  UINTN                   Index;
+  EHC_QH                  *Prev;
+  EHC_QH                  *Next;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index += Qh->Interval) {
+    //
+    // First QH can't be NULL because we always keep PeriodOne
+    // heads on the frame list
+    //
+    ASSERT (!EHC_LINK_TERMINATED (((UINT32*)Ehc->PeriodFrame)[Index]));
+    Next  = (EHC_QH*)((UINTN*)Ehc->PeriodFrameHost)[Index];
+    Prev  = NULL;
+
+    //
+    // Now, insert the queue head (Qh) into this frame:
+    // 1. Find a queue head with the same poll interval, just insert
+    //    Qh after this queue head, then we are done.
+    //
+    // 2. Find the position to insert the queue head into:
+    //      Previous head's interval is bigger than Qh's
+    //      Next head's interval is less than Qh's
+    //    Then, insert the Qh between then
+    //
+    while (Next->Interval > Qh->Interval) {
+      Prev  = Next;
+      Next  = Next->NextQh;
+    }
+
+    ASSERT (Next != NULL);
+
+    //
+    // The entry may have been linked into the frame by early insertation.
+    // For example: if insert a Qh with Qh.Interval == 4, and there is a Qh
+    // with Qh.Interval == 8 on the frame. If so, we are done with this frame.
+    // It isn't necessary to compare all the QH with the same interval to
+    // Qh. This is because if there is other QH with the same interval, Qh
+    // should has been inserted after that at Frames[0] and at Frames[0] it is
+    // impossible for (Next == Qh)
+    //
+    if (Next == Qh) {
+      continue;
+    }
+
+    if (Next->Interval == Qh->Interval) {
+      //
+      // If there is a QH with the same interval, it locates at
+      // Frames[0], and we can simply insert it after this QH. We
+      // are all done.
+      //
+      ASSERT ((Index == 0) && (Qh->NextQh == NULL));
+
+      Prev                    = Next;
+      Next                    = Next->NextQh;
+
+      Qh->NextQh              = Next;
+      Prev->NextQh            = Qh;
+
+      Qh->QhHw.HorizonLink    = Prev->QhHw.HorizonLink;
+      PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+      Prev->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+      break;
+    }
+
+    //
+    // OK, find the right position, insert it in. If Qh's next
+    // link has already been set, it is in position. This is
+    // guarranted by 2^n polling interval.
+    //
+    if (Qh->NextQh == NULL) {
+      Qh->NextQh              = Next;
+      PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Next, sizeof (EHC_QH));
+      Qh->QhHw.HorizonLink    = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    }
+
+    PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Qh, sizeof (EHC_QH));
+
+    if (Prev == NULL) {
+      ((UINT32*)Ehc->PeriodFrame)[Index]     = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+      ((UINTN*)Ehc->PeriodFrameHost)[Index]  = (UINTN)Qh;
+    } else {
+      Prev->NextQh            = Qh;
+      Prev->QhHw.HorizonLink  = QH_LINK (PciAddr, EHC_TYPE_QH, FALSE);
+    }
+  }
+}
+
+
+/**
+  Unlink an interrupt queue head from the periodic
+  schedule frame list.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qh                    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  )
+{
+  UINTN                   Index;
+  EHC_QH                  *Prev;
+  EHC_QH                  *This;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index += Qh->Interval) {
+    //
+    // Frame link can't be NULL because we always keep PeroidOne
+    // on the frame list
+    //
+    ASSERT (!EHC_LINK_TERMINATED (((UINT32*)Ehc->PeriodFrame)[Index]));
+    This  = (EHC_QH*)((UINTN*)Ehc->PeriodFrameHost)[Index];
+    Prev  = NULL;
+
+    //
+    // Walk through the frame's QH list to find the
+    // queue head to remove
+    //
+    while ((This != NULL) && (This != Qh)) {
+      Prev  = This;
+      This  = This->NextQh;
+    }
+
+    //
+    // Qh may have already been unlinked from this frame
+    // by early action. See the comments in EhcLinkQhToPeriod.
+    //
+    if (This == NULL) {
+      continue;
+    }
+
+    if (Prev == NULL) {
+      //
+      // Qh is the first entry in the frame
+      //
+      ((UINT32*)Ehc->PeriodFrame)[Index] = Qh->QhHw.HorizonLink;
+      ((UINTN*)Ehc->PeriodFrameHost)[Index] = (UINTN)Qh->NextQh;
+    } else {
+      Prev->NextQh            = Qh->NextQh;
+      Prev->QhHw.HorizonLink  = Qh->QhHw.HorizonLink;
+    }
+  }
+}
+
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+EhcCheckUrbResult (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINT8                   State;
+  BOOLEAN                 Finished;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  ASSERT ((Ehc != NULL) && (Urb != NULL) && (Urb->Qh != NULL));
+
+  Finished        = TRUE;
+  Urb->Completed  = 0;
+
+  Urb->Result     = EFI_USB_NOERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto ON_EXIT;
+  }
+
+  EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+    Qtd   = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+    QtdHw = &Qtd->QtdHw;
+    State = (UINT8) QtdHw->Status;
+
+    if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+      //
+      // EHCI will halt the queue head when met some error.
+      // If it is halted, the result of URB is finialized.
+      //
+      if ((State & QTD_STAT_ERR_MASK) == 0) {
+        Urb->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BABBLE;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR) && (QtdHw->ErrCnt == 0)) {
+        Urb->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+
+    } else if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+      //
+      // The QTD is still active, no need to check furthur.
+      //
+      Urb->Result |= EFI_USB_ERR_NOTEXECUTE;
+
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // This QTD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a setup.
+      //
+      if (QtdHw->Pid != QTD_PID_SETUP) {
+        Urb->Completed += Qtd->DataLen - QtdHw->TotalBytes;
+      }
+
+      if ((QtdHw->TotalBytes != 0) && (QtdHw->Pid == QTD_PID_INPUT)) {
+        EhcDumpQh (Urb->Qh, "Short packet read", FALSE);
+
+        //
+        // Short packet read condition. If it isn't a setup transfer,
+        // no need to check furthur: the queue head will halt at the
+        // ShortReadStop. If it is a setup transfer, need to check the
+        // Status Stage of the setup transfer to get the finial result
+        //
+        PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+        if (QtdHw->AltNext == QTD_LINK (PciAddr, FALSE)) {
+          DEBUG ((EFI_D_VERBOSE, "EhcCheckUrbResult: Short packet read, break\n"));
+
+          Finished = TRUE;
+          goto ON_EXIT;
+        }
+
+        DEBUG ((EFI_D_VERBOSE, "EhcCheckUrbResult: Short packet read, continue\n"));
+      }
+    }
+  }
+
+ON_EXIT:
+  //
+  // Return the data toggle set by EHCI hardware, bulk and interrupt
+  // transfer will use this to initialize the next transaction. For
+  // Control transfer, it always start a new data toggle sequence for
+  // new transfer.
+  //
+  // NOTICE: don't move DT update before the loop, otherwise there is
+  // a race condition that DT is wrong.
+  //
+  Urb->DataToggle = (UINT8) Urb->Qh->QhHw.DataToggle;
+
+  return Finished;
+}
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb,
+  IN  UINTN               TimeOut
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINTN                   Loop;
+  BOOLEAN                 Finished;
+  BOOLEAN                 InfiniteLoop;
+
+  Status       = EFI_SUCCESS;
+  Loop         = TimeOut * EHC_1_MILLISECOND;
+  Finished     = FALSE;
+  InfiniteLoop = FALSE;
+
+  //
+  // According to UEFI spec section 16.2.4, If Timeout is 0, then the caller
+  // must wait for the function to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR
+  // is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Loop); Index++) {
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (Finished) {
+      break;
+    }
+
+    gBS->Stall (EHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    DEBUG ((EFI_D_ERROR, "EhcExecTransfer: transfer not finished in %dms\n", (UINT32)TimeOut));
+    EhcDumpQh (Urb->Qh, NULL, FALSE);
+
+    Status = EFI_TIMEOUT;
+
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    DEBUG ((EFI_D_ERROR, "EhcExecTransfer: transfer failed with %x\n", Urb->Result));
+    EhcDumpQh (Urb->Qh, NULL, FALSE);
+
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Ehc                   The EHCI device.
+  @param  DevAddr               The address of the target device.
+  @param  EpNum                 The endpoint of the target.
+  @param  DataToggle            Return the next data toggle to use.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+EhciDelAsyncIntTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EpNum,
+  OUT UINT8               *DataToggle
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+  EFI_USB_DATA_DIRECTION  Direction;
+
+  Direction = (((EpNum & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  EpNum    &= 0x0F;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    if ((Urb->Ep.DevAddr == DevAddr) && (Urb->Ep.EpAddr == EpNum) &&
+        (Urb->Ep.Direction == Direction)) {
+      //
+      // Check the URB status to retrieve the next data toggle
+      // from the associated queue head.
+      //
+      EhcCheckUrbResult (Ehc, Urb);
+      *DataToggle = Urb->DataToggle;
+
+      EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+      RemoveEntryList (&Urb->UrbList);
+
+      gBS->FreePool (Urb->Data);
+      EhcFreeUrb (Ehc, Urb);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Ehc                   The EHCI device.
+
+**/
+VOID
+EhciDelAllAsyncIntTransfers (
+  IN USB2_HC_DEV          *Ehc
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    EhcUnlinkQhFromPeriod (Ehc, Urb->Qh);
+    RemoveEntryList (&Urb->UrbList);
+
+    gBS->FreePool (Urb->Data);
+    EhcFreeUrb (Ehc, Urb);
+  }
+}
+
+
+/**
+  Flush data from PCI controller specific address to mapped system
+  memory address.
+
+  @param  Ehc                The EHCI device.
+  @param  Urb                The URB to unmap.
+
+  @retval EFI_SUCCESS        Success to flush data to mapped system memory.
+  @retval EFI_DEVICE_ERROR   Fail to flush data to mapped system memory.
+
+**/
+EFI_STATUS
+EhcFlushAsyncIntMap (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  UINTN                         Len;
+  VOID                          *Map;
+
+  PciIo = Ehc->PciIo;
+  Len   = Urb->DataLen;
+
+  if (Urb->Ep.Direction == EfiUsbDataIn) {
+    MapOp = EfiPciIoOperationBusMasterWrite;
+  } else {
+    MapOp = EfiPciIoOperationBusMasterRead;
+  }
+
+  Status = PciIo->Unmap (PciIo, Urb->DataMap);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataMap = NULL;
+
+  Status = PciIo->Map (PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+  if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+  Urb->DataMap  = Map;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Update the queue head for next round of asynchronous transfer.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to update.
+
+**/
+VOID
+EhcUpdateAsyncRequest (
+  IN  USB2_HC_DEV         *Ehc,
+  IN URB                  *Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  EHC_QTD                 *FirstQtd;
+  QH_HW                   *QhHw;
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  EFI_PHYSICAL_ADDRESS    PciAddr;
+
+  Qtd = NULL;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    FirstQtd = NULL;
+
+    EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+      Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+      if (FirstQtd == NULL) {
+        FirstQtd = Qtd;
+      }
+
+      //
+      // Update the QTD for next round of transfer. Host control
+      // may change dt/Total Bytes to Transfer/C_Page/Cerr/Status/
+      // Current Offset. These fields need to be updated. DT isn't
+      // used by interrupt transfer. It uses DT in queue head.
+      // Current Offset is in Page[0], only need to reset Page[0]
+      // to initial data buffer.
+      //
+      QtdHw             = &Qtd->QtdHw;
+      QtdHw->Status     = QTD_STAT_ACTIVE;
+      QtdHw->ErrCnt     = QTD_MAX_ERR;
+      QtdHw->CurPage    = 0;
+      QtdHw->TotalBytes = (UINT32) Qtd->DataLen;
+      //
+      // calculate physical address by offset.
+      //
+      PciAddr = (UINTN)Urb->DataPhy + ((UINTN)Qtd->Data - (UINTN)Urb->Data); 
+      QtdHw->Page[0]    = EHC_LOW_32BIT (PciAddr);
+      QtdHw->PageHigh[0]= EHC_HIGH_32BIT (PciAddr);
+    }
+
+    //
+    // Update QH for next round of transfer. Host control only
+    // touch the fields in transfer overlay area. Only need to
+    // zero out the overlay area and set NextQtd to the first
+    // QTD. DateToggle bit is left untouched.
+    //
+    QhHw              = &Urb->Qh->QhHw;
+    QhHw->CurQtd      = QTD_LINK (0, TRUE);
+    QhHw->AltQtd      = 0;
+
+    QhHw->Status      = 0;
+    QhHw->Pid         = 0;
+    QhHw->ErrCnt      = 0;
+    QhHw->CurPage     = 0;
+    QhHw->Ioc         = 0;
+    QhHw->TotalBytes  = 0;
+
+    for (Index = 0; Index < 5; Index++) {
+      QhHw->Page[Index]     = 0;
+      QhHw->PageHigh[Index] = 0;
+    }
+
+    PciAddr = UsbHcGetPciAddressForHostMem (Ehc->MemPool, FirstQtd, sizeof (EHC_QTD));
+    QhHw->NextQtd = QTD_LINK (PciAddr, FALSE);
+  }
+
+  return ;
+}
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB2_HC_DEV.
+
+**/
+VOID
+EFIAPI
+EhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB2_HC_DEV             *Ehc;
+  EFI_TPL                 OldTpl;
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  BOOLEAN                 Finished;
+  UINT8                   *ProcBuf;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+
+  OldTpl  = gBS->RaiseTPL (EHC_TPL);
+  Ehc     = (USB2_HC_DEV *) Context;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Ehc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    //
+    // Check the result of URB execution. If it is still
+    // active, check the next one.
+    //
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (!Finished) {
+      continue;
+    }
+
+    //
+    // Flush any PCI posted write transactions from a PCI host
+    // bridge to system memory.
+    //
+    Status = EhcFlushAsyncIntMap (Ehc, Urb);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "EhcMonitorAsyncRequests: Fail to Flush AsyncInt Mapped Memeory\n"));
+    }
+
+    //
+    // Allocate a buffer then copy the transferred data for user.
+    // If failed to allocate the buffer, update the URB for next
+    // round of transfer. Ignore the data of this round.
+    //
+    ProcBuf = NULL;
+
+    if (Urb->Result == EFI_USB_NOERROR) {
+      ASSERT (Urb->Completed <= Urb->DataLen);
+
+      ProcBuf = AllocatePool (Urb->Completed);
+
+      if (ProcBuf == NULL) {
+        EhcUpdateAsyncRequest (Ehc, Urb);
+        continue;
+      }
+
+      CopyMem (ProcBuf, Urb->Data, Urb->Completed);
+    }
+
+    EhcUpdateAsyncRequest (Ehc, Urb);
+
+    //
+    // Leave error recovery to its related device driver. A
+    // common case of the error recovery is to re-submit the
+    // interrupt transfer which is linked to the head of the
+    // list. This function scans from head to tail. So the
+    // re-submitted interrupt transfer's callback function
+    // will not be called again in this round. Don't touch this
+    // URB after the callback, it may have been removed by the
+    // callback.
+    //
+    if (Urb->Callback != NULL) {
+      //
+      // Restore the old TPL, USB bus maybe connect device in
+      // his callback. Some drivers may has a lower TPL restriction.
+      //
+      gBS->RestoreTPL (OldTpl);
+      (Urb->Callback) (ProcBuf, Urb->Completed, Urb->Context, Urb->Result);
+      OldTpl = gBS->RaiseTPL (EHC_TPL);
+    }
+
+    if (ProcBuf != NULL) {
+      FreePool (ProcBuf);
+    }
+  }
+
+  gBS->RestoreTPL (OldTpl);
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h
new file mode 100644
index 0000000000..c03bd619d7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciSched.h
@@ -0,0 +1,180 @@
+/** @file
+
+  This file contains the definination for host controller schedule routines.
+
+Copyright (c) 2007 - 2009, 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.
+
+**/
+
+#ifndef _EFI_EHCI_SCHED_H_
+#define _EFI_EHCI_SCHED_H_
+
+
+/**
+  Initialize the schedule data structure such as frame list.
+
+  @param Ehc                    The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Free the schedule data. It may be partially initialized.
+
+  @param  Ehc            The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Link a queue head for interrupt transfer to the periodic
+  schedule frame list. This code is very much the same as
+  that in UHCI.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to link.
+
+**/
+VOID
+EhcLinkQhToPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+/**
+  Unlink an interrupt queue head from the periodic
+  schedule frame list.
+
+  @param  Ehc            The EHCI device.
+  @param  Qh             The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromPeriod (
+  IN USB2_HC_DEV          *Ehc,
+  IN EHC_QH               *Qh
+  );
+
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  URB                 *Urb,
+  IN  UINTN               TimeOut
+  );
+
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Ehc            The EHCI device.
+  @param  DevAddr        The address of the target device.
+  @param  EpNum          The endpoint of the target.
+  @param  DataToggle     Return the next data toggle to use.
+
+  @retval EFI_SUCCESS    An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND  No transfer for the device is found.
+
+**/
+EFI_STATUS
+EhciDelAsyncIntTransfer (
+  IN  USB2_HC_DEV         *Ehc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EpNum,
+  OUT UINT8               *DataToggle
+  );
+
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Ehc            The EHCI device.
+
+**/
+VOID
+EhciDelAllAsyncIntTransfers (
+  IN USB2_HC_DEV          *Ehc
+  );
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event          Interrupt event.
+  @param  Context        Pointer to USB2_HC_DEV.
+
+**/
+VOID
+EFIAPI
+EhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c
new file mode 100644
index 0000000000..6afb327df1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.c
@@ -0,0 +1,657 @@
+/** @file
+
+    This file contains URB request, each request is warpped in a
+    URB (Usb Request Block).
+
+Copyright (c) 2007 - 2010, 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 "Ehci.h"
+
+
+/**
+  Create a single QTD to hold the data.
+
+  @param  Ehc                   The EHCI device.
+  @param  Data                  The cpu memory address of current data not associated with a QTD.
+  @param  DataPhy               The pci bus address of current data not associated with a QTD.
+  @param  DataLen               The length of the data.
+  @param  PktId                 Packet ID to use in the QTD.
+  @param  Toggle                Data toggle to use in the QTD.
+  @param  MaxPacket             Maximu packet length of the endpoint.
+
+  @return Created QTD or NULL if failed to create one.
+
+**/
+EHC_QTD *
+EhcCreateQtd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+{
+  EHC_QTD                 *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  UINTN                   Len;
+  UINTN                   ThisBufLen;
+
+  ASSERT (Ehc != NULL);
+
+  Qtd = UsbHcAllocateMem (Ehc->MemPool, sizeof (EHC_QTD));
+
+  if (Qtd == NULL) {
+    return NULL;
+  }
+
+  Qtd->Signature    = EHC_QTD_SIG;
+  Qtd->Data         = Data;
+  Qtd->DataLen      = 0;
+
+  InitializeListHead (&Qtd->QtdList);
+
+  QtdHw             = &Qtd->QtdHw;
+  QtdHw->NextQtd    = QTD_LINK (NULL, TRUE);
+  QtdHw->AltNext    = QTD_LINK (NULL, TRUE);
+  QtdHw->Status     = QTD_STAT_ACTIVE;
+  QtdHw->Pid        = PktId;
+  QtdHw->ErrCnt     = QTD_MAX_ERR;
+  QtdHw->Ioc        = 0;
+  QtdHw->TotalBytes = 0;
+  QtdHw->DataToggle = Toggle;
+
+  //
+  // Fill in the buffer points
+  //
+  if (Data != NULL) {
+    Len = 0;
+
+    for (Index = 0; Index <= QTD_MAX_BUFFER; Index++) {
+      //
+      // Set the buffer point (Check page 41 EHCI Spec 1.0). No need to
+      // compute the offset and clear Reserved fields. This is already
+      // done in the data point.
+      //
+      QtdHw->Page[Index]      = EHC_LOW_32BIT (DataPhy);
+      QtdHw->PageHigh[Index]  = EHC_HIGH_32BIT (DataPhy);
+
+      ThisBufLen              = QTD_BUF_LEN - (EHC_LOW_32BIT (DataPhy) & QTD_BUF_MASK);
+
+      if (Len + ThisBufLen >= DataLen) {
+        Len = DataLen;
+        break;
+      }
+
+      Len += ThisBufLen;
+      Data += ThisBufLen;
+      DataPhy += ThisBufLen;
+    }
+
+    //
+    // Need to fix the last pointer if the Qtd can't hold all the
+    // user's data to make sure that the length is in the unit of
+    // max packets. If it can hold all the data, there is no such
+    // need.
+    //
+    if (Len < DataLen) {
+      Len = Len - Len % MaxPacket;
+    }
+
+    QtdHw->TotalBytes = (UINT32) Len;
+    Qtd->DataLen      = Len;
+  }
+
+  return Qtd;
+}
+
+
+
+/**
+  Initialize the queue head for interrupt transfer,
+  that is, initialize the following three fields:
+  1. SplitXState in the Status field
+  2. Microframe S-mask
+  3. Microframe C-mask
+
+  @param  Ep                    The queue head's related endpoint.
+  @param  QhHw                  The queue head to initialize.
+
+**/
+VOID
+EhcInitIntQh (
+  IN USB_ENDPOINT         *Ep,
+  IN QH_HW                *QhHw
+  )
+{
+  //
+  // Because UEFI interface can't utilitize an endpoint with
+  // poll rate faster than 1ms, only need to set one bit in
+  // the queue head. simple. But it may be changed later. If
+  // sub-1ms interrupt is supported, need to update the S-Mask
+  // here
+  //
+  if (Ep->DevSpeed == EFI_USB_SPEED_HIGH) {
+    QhHw->SMask = QH_MICROFRAME_0;
+    return ;
+  }
+
+  //
+  // For low/full speed device, the transfer must go through
+  // the split transaction. Need to update three fields
+  // 1. SplitXState in the status
+  // 2. Microframe S-Mask
+  // 3. Microframe C-Mask
+  // UEFI USB doesn't exercise admission control. It simplely
+  // schedule the high speed transactions in microframe 0, and
+  // full/low speed transactions at microframe 1. This also
+  // avoid the use of FSTN.
+  //
+  QhHw->SMask = QH_MICROFRAME_1;
+  QhHw->CMask = QH_MICROFRAME_3 | QH_MICROFRAME_4 | QH_MICROFRAME_5;
+}
+
+
+
+/**
+  Allocate and initialize a EHCI queue head.
+
+  @param  Ehci                  The EHCI device.
+  @param  Ep                    The endpoint to create queue head for.
+
+  @return Created queue head or NULL if failed to create one.
+
+**/
+EHC_QH *
+EhcCreateQh (
+  IN USB2_HC_DEV          *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+{
+  EHC_QH                  *Qh;
+  QH_HW                   *QhHw;
+
+  Qh = UsbHcAllocateMem (Ehci->MemPool, sizeof (EHC_QH));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->Signature       = EHC_QH_SIG;
+  Qh->NextQh          = NULL;
+  Qh->Interval        = Ep->PollRate;
+
+  InitializeListHead (&Qh->Qtds);
+
+  QhHw                = &Qh->QhHw;
+  QhHw->HorizonLink   = QH_LINK (NULL, 0, TRUE);
+  QhHw->DeviceAddr    = Ep->DevAddr;
+  QhHw->Inactive      = 0;
+  QhHw->EpNum         = Ep->EpAddr;
+  QhHw->EpSpeed       = Ep->DevSpeed;
+  QhHw->DtCtrl        = 0;
+  QhHw->ReclaimHead   = 0;
+  QhHw->MaxPacketLen  = (UINT32) Ep->MaxPacket;
+  QhHw->CtrlEp        = 0;
+  QhHw->NakReload     = QH_NAK_RELOAD;
+  QhHw->HubAddr       = Ep->HubAddr;
+  QhHw->PortNum       = Ep->HubPort;
+  QhHw->Multiplier    = 1;
+  QhHw->DataToggle    = Ep->Toggle;
+
+  if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+    QhHw->Status |= QTD_STAT_DO_SS;
+  }
+
+  switch (Ep->Type) {
+  case EHC_CTRL_TRANSFER:
+    //
+    // Special initialization for the control transfer:
+    // 1. Control transfer initialize data toggle from each QTD
+    // 2. Set the Control Endpoint Flag (C) for low/full speed endpoint.
+    //
+    QhHw->DtCtrl = 1;
+
+    if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+      QhHw->CtrlEp = 1;
+    }
+    break;
+
+  case EHC_INT_TRANSFER_ASYNC:
+  case EHC_INT_TRANSFER_SYNC:
+    //
+    // Special initialization for the interrupt transfer
+    // to set the S-Mask and C-Mask
+    //
+    QhHw->NakReload = 0;
+    EhcInitIntQh (Ep, QhHw);
+    break;
+
+  case EHC_BULK_TRANSFER:
+    if ((Ep->DevSpeed == EFI_USB_SPEED_HIGH) && (Ep->Direction == EfiUsbDataOut)) {
+      QhHw->Status |= QTD_STAT_DO_PING;
+    }
+
+    break;
+  }
+
+  return Qh;
+}
+
+
+/**
+  Convert the poll interval from application to that
+  be used by EHCI interface data structure. Only need
+  to get the max 2^n that is less than interval. UEFI
+  can't support high speed endpoint with a interval less
+  than 8 microframe because interval is specified in
+  the unit of ms (millisecond).
+
+  @param  Interval              The interval to convert.
+
+  @return The converted interval.
+
+**/
+UINTN
+EhcConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  if (Interval == 0) {
+    return 1;
+  }
+
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+
+/**
+  Free a list of QTDs.
+
+  @param  Ehc                   The EHCI device.
+  @param  Qtds                  The list head of the QTD.
+
+**/
+VOID
+EhcFreeQtds (
+  IN USB2_HC_DEV          *Ehc,
+  IN LIST_ENTRY           *Qtds
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  EHC_QTD                 *Qtd;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+    RemoveEntryList (&Qtd->QtdList);
+    UsbHcFreeMem (Ehc->MemPool, Qtd, sizeof (EHC_QTD));
+  }
+}
+
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  )
+{
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+
+  PciIo = Ehc->PciIo;
+
+  if (Urb->RequestPhy != NULL) {
+    PciIo->Unmap (PciIo, Urb->RequestMap);
+  }
+
+  if (Urb->DataMap != NULL) {
+    PciIo->Unmap (PciIo, Urb->DataMap);
+  }
+
+  if (Urb->Qh != NULL) {
+    //
+    // Ensure that this queue head has been unlinked from the
+    // schedule data structures. Free all the associated QTDs
+    //
+    EhcFreeQtds (Ehc, &Urb->Qh->Qtds);
+    UsbHcFreeMem (Ehc->MemPool, Urb->Qh, sizeof (EHC_QH));
+  }
+
+  gBS->FreePool (Urb);
+}
+
+
+/**
+  Create a list of QTDs for the URB.
+
+  @param  Ehc                   The EHCI device.
+  @param  Urb                   The URB to create QTDs for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for QTD.
+  @retval EFI_SUCCESS           The QTDs are allocated for the URB.
+
+**/
+EFI_STATUS
+EhcCreateQtds (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  )
+{
+  USB_ENDPOINT            *Ep;
+  EHC_QH                  *Qh;
+  EHC_QTD                 *Qtd;
+  EHC_QTD                 *StatusQtd;
+  EHC_QTD                 *NextQtd;
+  LIST_ENTRY              *Entry;
+  UINT32                  AlterNext;
+  UINT8                   Toggle;
+  UINTN                   Len;
+  UINT8                   Pid;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+
+  ASSERT ((Urb != NULL) && (Urb->Qh != NULL));
+
+  //
+  // EHCI follows the alternet next QTD pointer if it meets
+  // a short read and the AlterNext pointer is valid. UEFI
+  // EHCI driver should terminate the transfer except the
+  // control transfer.
+  //
+  Toggle    = 0;
+  Qh        = Urb->Qh;
+  Ep        = &Urb->Ep;
+  StatusQtd = NULL;
+  AlterNext = QTD_LINK (NULL, TRUE);
+
+  PhyAddr   = UsbHcGetPciAddressForHostMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (EHC_QTD));
+  if (Ep->Direction == EfiUsbDataIn) {
+    AlterNext = QTD_LINK (PhyAddr, FALSE);
+  }
+
+  //
+  // Build the Setup and status packets for control transfer
+  //
+  if (Urb->Ep.Type == EHC_CTRL_TRANSFER) {
+    Len = sizeof (EFI_USB_DEVICE_REQUEST);
+    Qtd = EhcCreateQtd (Ehc, (UINT8 *)Urb->Request, (UINT8 *)Urb->RequestPhy, Len, QTD_PID_SETUP, 0, Ep->MaxPacket);
+
+    if (Qtd == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Create the status packet now. Set the AlterNext to it. So, when
+    // EHCI meets a short control read, it can resume at the status stage.
+    // Use the opposite direction of the data stage, or IN if there is
+    // no data stage.
+    //
+    if (Ep->Direction == EfiUsbDataIn) {
+      Pid = QTD_PID_OUTPUT;
+    } else {
+      Pid = QTD_PID_INPUT;
+    }
+
+    StatusQtd = EhcCreateQtd (Ehc, NULL, NULL, 0, Pid, 1, Ep->MaxPacket);
+
+    if (StatusQtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      PhyAddr   = UsbHcGetPciAddressForHostMem (Ehc->MemPool, StatusQtd, sizeof (EHC_QTD));
+      AlterNext = QTD_LINK (PhyAddr, FALSE);
+    }
+
+    Toggle = 1;
+  }
+
+  //
+  // Build the data packets for all the transfers
+  //
+  if (Ep->Direction == EfiUsbDataIn) {
+    Pid = QTD_PID_INPUT;
+  } else {
+    Pid = QTD_PID_OUTPUT;
+  }
+
+  Qtd = NULL;
+  Len = 0;
+
+  while (Len < Urb->DataLen) {
+    Qtd = EhcCreateQtd (
+            Ehc,
+            (UINT8 *) Urb->Data + Len,
+            (UINT8 *) Urb->DataPhy + Len,
+            Urb->DataLen - Len,
+            Pid,
+            Toggle,
+            Ep->MaxPacket
+            );
+
+    if (Qtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    Qtd->QtdHw.AltNext = AlterNext;
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Switch the Toggle bit if odd number of packets are included in the QTD.
+    //
+    if (((Qtd->DataLen + Ep->MaxPacket - 1) / Ep->MaxPacket) % 2) {
+      Toggle = (UINT8) (1 - Toggle);
+    }
+
+    Len += Qtd->DataLen;
+  }
+
+  //
+  // Insert the status packet for control transfer
+  //
+  if (Ep->Type == EHC_CTRL_TRANSFER) {
+    InsertTailList (&Qh->Qtds, &StatusQtd->QtdList);
+  }
+
+  //
+  // OK, all the QTDs needed are created. Now, fix the NextQtd point
+  //
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, EHC_QTD, QtdList);
+
+    //
+    // break if it is the last entry on the list
+    //
+    if (Entry->ForwardLink == &Qh->Qtds) {
+      break;
+    }
+
+    NextQtd             = EFI_LIST_CONTAINER (Entry->ForwardLink, EHC_QTD, QtdList);
+    PhyAddr             = UsbHcGetPciAddressForHostMem (Ehc->MemPool, NextQtd, sizeof (EHC_QTD));
+    Qtd->QtdHw.NextQtd  = QTD_LINK (PhyAddr, FALSE);
+  }
+
+  //
+  // Link the QTDs to the queue head
+  //
+  NextQtd           = EFI_LIST_CONTAINER (Qh->Qtds.ForwardLink, EHC_QTD, QtdList);
+  PhyAddr           = UsbHcGetPciAddressForHostMem (Ehc->MemPool, NextQtd, sizeof (EHC_QTD));
+  Qh->QhHw.NextQtd  = QTD_LINK (PhyAddr, FALSE);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  EhcFreeQtds (Ehc, &Qh->Qtds);
+  return EFI_OUT_OF_RESOURCES;
+}
+
+
+/**
+  Create a new URB and its associated QTD.
+
+  @param  Ehc                   The EHCI device.
+  @param  DevAddr               The device address.
+  @param  EpAddr                Endpoint addrress & its direction.
+  @param  DevSpeed              The device speed.
+  @param  Toggle                Initial data toggle to use.
+  @param  MaxPacket             The max packet length of the endpoint.
+  @param  Hub                   The transaction translator to use.
+  @param  Type                  The transaction type.
+  @param  Request               The standard USB request for control transfer.
+  @param  Data                  The user data to transfer.
+  @param  DataLen               The length of data buffer.
+  @param  Callback              The function to call when data is transferred.
+  @param  Context               The context to the callback.
+  @param  Interval              The interval for interrupt transfer.
+
+  @return Created URB or NULL.
+
+**/
+URB *
+EhcCreateUrb (
+  IN USB2_HC_DEV                        *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  EFI_STATUS                    Status;
+  UINTN                         Len;
+  URB                           *Urb;
+  VOID                          *Map;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = EHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep              = &Urb->Ep;
+  Ep->DevAddr     = DevAddr;
+  Ep->EpAddr      = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction   = (((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  Ep->DevSpeed    = DevSpeed;
+  Ep->MaxPacket   = MaxPacket;
+
+  Ep->HubAddr     = 0;
+  Ep->HubPort     = 0;
+
+  if (DevSpeed != EFI_USB_SPEED_HIGH) {
+    ASSERT (Hub != NULL);
+
+    Ep->HubAddr   = Hub->TranslatorHubAddress;
+    Ep->HubPort   = Hub->TranslatorPortNumber;
+  }
+
+  Ep->Toggle      = Toggle;
+  Ep->Type        = Type;
+  Ep->PollRate    = EhcConvertPollRate (Interval);
+
+  Urb->Request    = Request;
+  Urb->Data       = Data;
+  Urb->DataLen    = DataLen;
+  Urb->Callback   = Callback;
+  Urb->Context    = Context;
+
+  PciIo           = Ehc->PciIo;
+  Urb->Qh         = EhcCreateQh (Ehc, &Urb->Ep);
+
+  if (Urb->Qh == NULL) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Map the request and user data
+  //
+  if (Request != NULL) {
+    Len     = sizeof (EFI_USB_DEVICE_REQUEST);
+    MapOp   = EfiPciIoOperationBusMasterRead;
+    Status  = PciIo->Map (PciIo, MapOp, Request, &Len, &PhyAddr, &Map);
+
+    if (EFI_ERROR (Status) || (Len != sizeof (EFI_USB_DEVICE_REQUEST))) {
+      goto ON_ERROR;
+    }
+
+    Urb->RequestPhy = (VOID *) ((UINTN) PhyAddr);
+    Urb->RequestMap = Map;
+  }
+
+  if (Data != NULL) {
+    Len     = DataLen;
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      MapOp = EfiPciIoOperationBusMasterWrite;
+    } else {
+      MapOp = EfiPciIoOperationBusMasterRead;
+    }
+
+    Status  = PciIo->Map (PciIo, MapOp, Data, &Len, &PhyAddr, &Map);
+
+    if (EFI_ERROR (Status) || (Len != DataLen)) {
+      goto ON_ERROR;
+    }
+
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  Status = EhcCreateQtds (Ehc, Urb);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Urb;
+
+ON_ERROR:
+  EhcFreeUrb (Ehc, Urb);
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h
new file mode 100644
index 0000000000..02e9af81be
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/EhciUrb.h
@@ -0,0 +1,336 @@
+/** @file
+
+    This file contains URB request, each request is warpped in a
+    URB (Usb Request Block).
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_URB_H_
+#define _EFI_EHCI_URB_H_
+
+
+typedef struct _EHC_QTD  EHC_QTD;
+typedef struct _EHC_QH   EHC_QH;
+typedef struct _URB      URB;
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define EHC_CTRL_TRANSFER       0x01
+#define EHC_BULK_TRANSFER       0x02
+#define EHC_INT_TRANSFER_SYNC   0x04
+#define EHC_INT_TRANSFER_ASYNC  0x08
+
+#define EHC_QTD_SIG             SIGNATURE_32 ('U', 'S', 'B', 'T')
+#define EHC_QH_SIG              SIGNATURE_32 ('U', 'S', 'B', 'H')
+#define EHC_URB_SIG             SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Hardware related bit definitions
+//
+#define EHC_TYPE_ITD            0x00
+#define EHC_TYPE_QH             0x02
+#define EHC_TYPE_SITD           0x04
+#define EHC_TYPE_FSTN           0x06
+
+#define QH_NAK_RELOAD           3
+#define QH_HSHBW_MULTI          1
+
+#define QTD_MAX_ERR             3
+#define QTD_PID_OUTPUT          0x00
+#define QTD_PID_INPUT           0x01
+#define QTD_PID_SETUP           0x02
+
+#define QTD_STAT_DO_OUT         0
+#define QTD_STAT_DO_SS          0
+#define QTD_STAT_DO_PING        0x01
+#define QTD_STAT_DO_CS          0x02
+#define QTD_STAT_TRANS_ERR      0x08
+#define QTD_STAT_BABBLE_ERR     0x10
+#define QTD_STAT_BUFF_ERR       0x20
+#define QTD_STAT_HALTED         0x40
+#define QTD_STAT_ACTIVE         0x80
+#define QTD_STAT_ERR_MASK       (QTD_STAT_TRANS_ERR | QTD_STAT_BABBLE_ERR | QTD_STAT_BUFF_ERR)
+
+#define QTD_MAX_BUFFER          4
+#define QTD_BUF_LEN             4096
+#define QTD_BUF_MASK            0x0FFF
+
+#define QH_MICROFRAME_0         0x01
+#define QH_MICROFRAME_1         0x02
+#define QH_MICROFRAME_2         0x04
+#define QH_MICROFRAME_3         0x08
+#define QH_MICROFRAME_4         0x10
+#define QH_MICROFRAME_5         0x20
+#define QH_MICROFRAME_6         0x40
+#define QH_MICROFRAME_7         0x80
+
+#define USB_ERR_SHORT_PACKET    0x200
+
+//
+// Fill in the hardware link point: pass in a EHC_QH/QH_HW
+// pointer to QH_LINK; A EHC_QTD/QTD_HW pointer to QTD_LINK
+//
+#define QH_LINK(Addr, Type, Term) \
+          ((UINT32) ((EHC_LOW_32BIT (Addr) & 0xFFFFFFE0) | (Type) | ((Term) ? 1 : 0)))
+
+#define QTD_LINK(Addr, Term)      QH_LINK((Addr), 0, (Term))
+
+//
+// The defination of EHCI hardware used data structure for
+// little endian architecture. The QTD and QH structures
+// are required to be 32 bytes aligned. Don't add members
+// to the head of the associated software strucuture.
+//
+#pragma pack(1)
+typedef struct {
+  UINT32                  NextQtd;
+  UINT32                  AltNext;
+
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QTD_HW;
+
+typedef struct {
+  UINT32                  HorizonLink;
+  //
+  // Endpoint capabilities/Characteristics DWord 1 and DWord 2
+  //
+  UINT32                  DeviceAddr   : 7;
+  UINT32                  Inactive     : 1;
+  UINT32                  EpNum        : 4;
+  UINT32                  EpSpeed      : 2;
+  UINT32                  DtCtrl       : 1;
+  UINT32                  ReclaimHead  : 1;
+  UINT32                  MaxPacketLen : 11;
+  UINT32                  CtrlEp       : 1;
+  UINT32                  NakReload    : 4;
+
+  UINT32                  SMask        : 8;
+  UINT32                  CMask        : 8;
+  UINT32                  HubAddr      : 7;
+  UINT32                  PortNum      : 7;
+  UINT32                  Multiplier   : 2;
+
+  //
+  // Transaction execution overlay area
+  //
+  UINT32                  CurQtd;
+  UINT32                  NextQtd;
+  UINT32                  AltQtd;
+
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QH_HW;
+#pragma pack()
+
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  UINT8                   DevAddr;
+  UINT8                   EpAddr;     // Endpoint address, no direction encoded in
+  EFI_USB_DATA_DIRECTION  Direction;
+  UINT8                   DevSpeed;
+  UINTN                   MaxPacket;
+  UINT8                   HubAddr;
+  UINT8                   HubPort;
+  UINT8                   Toggle;     // Data toggle, not used for control transfer
+  UINTN                   Type;
+  UINTN                   PollRate;   // Polling interval used by EHCI
+} USB_ENDPOINT;
+
+//
+// Software QTD strcture, this is used to manage all the
+// QTD generated from a URB. Don't add fields before QtdHw.
+//
+struct _EHC_QTD {
+  QTD_HW                  QtdHw;
+  UINT32                  Signature;
+  LIST_ENTRY              QtdList;   // The list of QTDs to one end point
+  UINT8                   *Data;     // Buffer of the original data
+  UINTN                   DataLen;   // Original amount of data in this QTD
+};
+
+//
+// Software QH structure. All three different transaction types
+// supported by UEFI USB, that is the control/bulk/interrupt
+// transfers use the queue head and queue token strcuture.
+//
+// Interrupt QHs are linked to periodic frame list in the reversed
+// 2^N tree. Each interrupt QH is linked to the list starting at
+// frame 0. There is a dummy interrupt QH linked to each frame as
+// a sentinental whose polling interval is 1. Synchronous interrupt
+// transfer is linked after this dummy QH.
+//
+// For control/bulk transfer, only synchronous (in the sense of UEFI)
+// transfer is supported. A dummy QH is linked to EHCI AsyncListAddr
+// as the reclamation header. New transfer is inserted after this QH.
+//
+struct _EHC_QH {
+  QH_HW                   QhHw;
+  UINT32                  Signature;
+  EHC_QH                  *NextQh;    // The queue head pointed to by horizontal link
+  LIST_ENTRY              Qtds;       // The list of QTDs to this queue head
+  UINTN                   Interval;
+};
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+struct _URB {
+  UINT32                          Signature;
+  LIST_ENTRY                      UrbList;
+
+  //
+  // Transaction information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;     // Control transfer only
+  VOID                            *RequestPhy;  // Address of the mapped request
+  VOID                            *RequestMap;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;     // Address of the mapped user data
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+
+  //
+  // Schedule data
+  //
+  EHC_QH                          *Qh;
+
+  //
+  // Transaction result
+  //
+  UINT32                          Result;
+  UINTN                           Completed;    // completed data length
+  UINT8                           DataToggle;
+};
+
+
+
+/**
+  Create a single QTD to hold the data.
+
+  @param  Ehc        The EHCI device.
+  @param  Data       The cpu memory address of current data not associated with a QTD.
+  @param  DataPhy    The pci bus address of current data not associated with a QTD.
+  @param  DataLen    The length of the data.
+  @param  PktId      Packet ID to use in the QTD.
+  @param  Toggle     Data toggle to use in the QTD.
+  @param  MaxPacket  Maximu packet length of the endpoint.
+
+  @return Created QTD or NULL if failed to create one.
+
+**/
+EHC_QTD *
+EhcCreateQtd (
+  IN USB2_HC_DEV          *Ehc,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  );
+
+
+
+/**
+  Allocate and initialize a EHCI queue head.
+
+  @param  Ehci       The EHCI device.
+  @param  Ep         The endpoint to create queue head for.
+
+  @return Created queue head or NULL if failed to create one.
+
+**/
+EHC_QH *
+EhcCreateQh (
+  IN USB2_HC_DEV          *Ehci,
+  IN USB_ENDPOINT         *Ep
+  );
+
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+
+  @param  Ehc        The EHCI device.
+  @param  Urb        The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN USB2_HC_DEV          *Ehc,
+  IN URB                  *Urb
+  );
+
+
+/**
+  Create a new URB and its associated QTD.
+
+  @param  Ehc        The EHCI device.
+  @param  DevAddr    The device address.
+  @param  EpAddr     Endpoint addrress & its direction.
+  @param  DevSpeed   The device speed.
+  @param  Toggle     Initial data toggle to use.
+  @param  MaxPacket  The max packet length of the endpoint.
+  @param  Hub        The transaction translator to use.
+  @param  Type       The transaction type.
+  @param  Request    The standard USB request for control transfer.
+  @param  Data       The user data to transfer.
+  @param  DataLen    The length of data buffer.
+  @param  Callback   The function to call when data is transferred.
+  @param  Context    The context to the callback.
+  @param  Interval   The interval for interrupt transfer.
+
+  @return Created URB or NULL.
+
+**/
+URB *
+EhcCreateUrb (
+  IN USB2_HC_DEV                        *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..28be3803f1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.c
@@ -0,0 +1,566 @@
+/** @file
+
+  Routine procedures for memory allocate/free.
+
+Copyright (c) 2007 - 2013, 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 "Ehci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  //
+  // Check whether the data structure used by the host controller
+  // should be restricted into the same 4G
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+    PciIo->Unmap (PciIo, Mapping);
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  UINTN                   Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->BufHost <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->BufHost;
+  PhyAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->Buf + Offset);
+  return PhyAddr;
+}
+
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                The PciIo that can be used to access the host controller.
+  @param  Check4G              Whether the host controller requires allocated memory
+                               from one 4G address space.
+  @param  Which4G              The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS          The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE  Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool              The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..84ced8d580
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciDxe/UsbHcMem.h
@@ -0,0 +1,157 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines.
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+  @param  Check4G             Whether the host controller requires allocated memory
+                              from one 4G address space.
+  @param  Which4G             The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param   Pool               The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
new file mode 100644
index 0000000000..09769eaf24
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.c
@@ -0,0 +1,1275 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2010 - 2013, 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 "EhcPeim.h"
+
+//
+// Two arrays used to translate the EHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {PORTSC_CONN,     USB_PORT_STAT_CONNECTION},
+  {PORTSC_ENABLED,  USB_PORT_STAT_ENABLE},
+  {PORTSC_SUSPEND,  USB_PORT_STAT_SUSPEND},
+  {PORTSC_OVERCUR,  USB_PORT_STAT_OVERCURRENT},
+  {PORTSC_RESET,    USB_PORT_STAT_RESET},
+  {PORTSC_POWER,    USB_PORT_STAT_POWER},
+  {PORTSC_OWNER,    USB_PORT_STAT_OWNER}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {PORTSC_CONN_CHANGE,    USB_PORT_STAT_C_CONNECTION},
+  {PORTSC_ENABLE_CHANGE,  USB_PORT_STAT_C_ENABLE},
+  {PORTSC_OVERCUR_CHANGE, USB_PORT_STAT_C_OVERCURRENT}
+};
+
+/**
+  Read Ehc Operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+ 
+  ASSERT (Ehc->CapLen != 0);
+
+  Data = MmioRead32 (Ehc->UsbHostControllerBaseAddress + Ehc->CapLen + Offset);
+  
+  return Data;
+}
+
+/**
+  Write the data to the EHCI operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    EHCI operation register offset.
+  @param  Data      The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+
+  ASSERT (Ehc->CapLen != 0);
+
+  MmioWrite32(Ehc->UsbHostControllerBaseAddress + Ehc->CapLen + Offset, Data);
+
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The offset of the operational register.
+  @param  Bit       The bit mask of the register to set.
+
+**/
+VOID
+EhcSetOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data |= Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The offset of the operational register.
+  @param  Bit       The bit mask of the register to clear.
+
+**/
+VOID
+EhcClearOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = EhcReadOpReg (Ehc, Offset);
+  Data &= ~Bit;
+  EhcWriteOpReg (Ehc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit 
+  to become set (or clear).
+  
+  @param  Ehc           The EHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in millisecond).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+EhcWaitOpRegBit (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+
+  for (Index = 0; Index < Timeout / EHC_SYNC_POLL_INTERVAL + 1; Index++) {
+    if (EHC_REG_BIT_IS_SET (Ehc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    MicroSecondDelay (EHC_SYNC_POLL_INTERVAL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Read EHCI capability register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  
+  Data = MmioRead32(Ehc->UsbHostControllerBaseAddress + Offset);
+  
+  return Data;
+}
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while waiting door bell to set.
+  @retval EFI_SUCCESS       Synchronized with the hardware.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_IAAD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_IAA, TRUE, Timeout);
+
+  //
+  // ACK the IAA bit in USBSTS register. Make sure other
+  // interrupt bits are not ACKed. These bits are WC (Write Clean).
+  //
+  Data  = EhcReadOpReg (Ehc, EHC_USBSTS_OFFSET);
+  Data &= ~USBSTS_INTACK_MASK;
+  Data |= USBSTS_IAA;
+
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, Data);
+
+  return Status;
+}
+
+/**
+  Clear all the interrutp status bits, these bits 
+  are Write-Clean.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  PEI_USB2_HC_DEV         *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_USBSTS_OFFSET, USBSTS_INTACK_MASK);
+}
+
+/**
+  Enable the periodic schedule then wait EHC to 
+  actually enable it.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while enabling periodic schedule.
+  @retval EFI_SUCCESS       The periodical schedule is enabled.
+
+**/
+EFI_STATUS
+EhcEnablePeriodSchd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_PERIOD);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_PERIOD_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Enable asynchrounous schedule.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   Time to wait before abort.
+
+  @retval EFI_SUCCESS       The EHCI asynchronous schedule is enabled.
+  @retval Others            Failed to enable the asynchronous scheudle.
+
+**/
+EFI_STATUS
+EhcEnableAsyncSchd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_ENABLE_ASYNC);
+
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_ASYNC_ENABLED, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Check whether Ehc is halted.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      The controller is halted.
+  @retval FALSE     The controller isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT);
+}
+
+/**
+  Check whether system error occurred.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      System error happened.
+  @retval FALSE     No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  return EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_SYS_ERROR);
+}
+
+/**
+  Reset the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT     The transfer failed due to time out.
+  @retval Others          Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!EHC_REG_BIT_IS_SET (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT)) {
+    Status = EhcHaltHC (Ehc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RESET, FALSE, Timeout);
+  return Status;
+}
+
+/**
+  Halt the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_TIMEOUT     Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS     The EHCI is halt.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN PEI_USB2_HC_DEV     *Ehc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcClearOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+/**
+  Set the EHCI to run.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_SUCCESS     The EHCI is running.
+  @retval Others          Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+  Status = EhcWaitOpRegBit (Ehc, EHC_USBSTS_OFFSET, USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
+/**
+  Power On All EHCI Ports.
+  
+  @param  Ehc             The EHCI device.
+
+**/
+VOID
+EhcPowerOnAllPorts (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+{
+  UINT8 PortNumber;
+  UINT8 Index;
+
+  PortNumber = (UINT8)(Ehc->HcStructParams & HCSP_NPORTS);
+  for (Index = 0; Index < PortNumber; Index++) {
+    EhcSetOpRegBit (Ehc, EHC_PORT_STAT_OFFSET + 4 * Index, PORTSC_POWER);
+  }
+}
+
+/**
+  Initialize the HC hardware. 
+  EHCI spec lists the five things to do to initialize the hardware.
+  1. Program CTRLDSSEGMENT.
+  2. Set USBINTR to enable interrupts.
+  3. Set periodic list base.
+  4. Set USBCMD, interrupt threshold, frame list size etc.
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI.
+  
+  @param  Ehc             The EHCI device.
+
+  @retval EFI_SUCCESS     The EHCI has come out of halt state.
+  @retval EFI_TIMEOUT     Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  UINTN               PageNumber;
+  
+  ASSERT (EhcIsHalt (Ehc));
+
+  //
+  // Allocate the periodic frame and associated memeory
+  // management facilities if not already done.
+  //
+  if (Ehc->PeriodFrame != NULL) {
+    EhcFreeSched (Ehc);
+  }
+  PageNumber =  sizeof(PEI_URB)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+  Ehc->Urb = (PEI_URB *) ((UINTN) TempPtr);
+  if (Ehc->Urb  == NULL) {
+    return Status;
+  }
+
+  EhcPowerOnAllPorts (Ehc);  
+  MicroSecondDelay (EHC_ROOT_PORT_RECOVERY_STALL);
+  
+  Status = EhcInitSched (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // 1. Program the CTRLDSSEGMENT register with the high 32 bit addr
+  //
+  EhcWriteOpReg (Ehc, EHC_CTRLDSSEG_OFFSET, Ehc->High32bitAddr);
+
+  //
+  // 2. Clear USBINTR to disable all the interrupt. UEFI works by polling
+  //
+  EhcWriteOpReg (Ehc, EHC_USBINTR_OFFSET, 0);
+
+  //
+  // 3. Program periodic frame list, already done in EhcInitSched
+  // 4. Start the Host Controller
+  //
+  EhcSetOpRegBit (Ehc, EHC_USBCMD_OFFSET, USBCMD_RUN);
+
+  //
+  // 5. Set all ports routing to EHC
+  //
+  EhcSetOpRegBit (Ehc, EHC_CONFIG_FLAG_OFFSET, CONFIGFLAG_ROUTE_EHC);
+
+  //
+  // Wait roothub port power stable
+  //
+  MicroSecondDelay (EHC_ROOT_PORT_RECOVERY_STALL);
+
+  Status = EhcEnablePeriodSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = EhcEnableAsyncSchd (Ehc, EHC_GENERIC_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support 
+                                bulk transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator            A pointr to the transaction translator data.                                
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcBulkTransfer (
+  IN EFI_PEI_SERVICES                     **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI         *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               EndPointAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  PEI_URB                 *Urb;
+  EFI_STATUS              Status;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) || 
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc =PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(This);
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          *DataToggle,
+          MaximumPacketLength,
+          Translator,
+          EHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+  *DataToggle     = Urb->DataToggle;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  OUT UINT8                                 *PortNumber
+  )
+{
+
+  PEI_USB2_HC_DEV             *EhcDev;
+  EhcDev = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }  
+  
+  *PortNumber = (UINT8)(EhcDev->HcStructParams & HCSP_NPORTS);
+  return EFI_SUCCESS;
+  
+}
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN  UINT8                 PortNumber,
+  IN  EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  Ehc       = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = EHC_PORT_STAT_OFFSET + (4 * PortNumber);
+  State   = EhcReadOpReg (Ehc, Offset);
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Clear PORT_ENABLE feature means disable port.
+    //
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // A write of zero to this bit is ignored by the host
+    // controller. The host controller will unconditionally
+    // set this bit to a zero when:
+    //   1. software sets the Forct Port Resume bit to a zero from a one.
+    //   2. software sets the Port Reset bit to a one frome a zero.
+    //
+    State &= ~PORSTSC_RESUME;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Clear PORT_RESET means clear the reset signal.
+    //
+    State &= ~PORTSC_RESET;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // Clear port owner means this port owned by EHC
+    //
+    State &= ~PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= PORTSC_CONN_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= PORTSC_ENABLE_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= PORTSC_OVERCUR_CHANGE;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+  case EfiUsbPortSuspendChange:
+  case EfiUsbPortResetChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  PortNumber,
+  IN EFI_USB_PORT_FEATURE                   PortFeature
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+
+  Ehc       = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset  = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  State   = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~PORTSC_CHANGE_MASK;
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Sofeware can't set this bit, Port can only be enable by
+    // EHCI as a part of the reset and enable
+    //
+    State |= PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= PORTSC_SUSPEND;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (EhcIsHalt (Ehc)) {
+      Status = EhcRunHC (Ehc, EHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        break;
+      }
+    }
+    
+    //
+    // Set one to PortReset bit must also set zero to PortEnable bit
+    //
+    State |= PORTSC_RESET;
+    State &= ~PORTSC_ENABLED;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Not supported, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortOwner:
+    State |= PORTSC_OWNER;
+    EhcWriteOpReg (Ehc, Offset, State);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN  UINT8                                 PortNumber,
+  OUT EFI_USB_PORT_STATUS                   *PortStatus
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc       =  PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Ehc->HcStructParams & HCSP_NPORTS);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                        = (UINT32) (EHC_PORT_STAT_OFFSET + (4 * PortNumber));
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  State                         = EhcReadOpReg (Ehc, Offset);
+
+  //
+  // Identify device speed. If in K state, it is low speed.
+  // If the port is enabled after reset, the device is of 
+  // high speed. The USB bus driver should retrieve the actual
+  // port speed after reset. 
+  //
+  if (EHC_BIT_IS_SET (State, PORTSC_LINESTATE_K)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+
+  } else if (EHC_BIT_IS_SET (State, PORTSC_ENABLED)) {
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+  }
+  
+  //
+  // Convert the EHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (EHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+                                 If Timeout is 0, then the caller must wait for the function
+                                 to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator             Transaction translator to be used by this device.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcControlTransfer (
+  IN  EFI_PEI_SERVICES                    **PeiServices,
+  IN  PEI_USB2_HOST_CONTROLLER_PPI        *This,
+  IN  UINT8                               DeviceAddress,
+  IN  UINT8                               DeviceSpeed,
+  IN  UINTN                               MaximumPacketLength,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *TransferResult
+  )
+{
+  PEI_USB2_HC_DEV         *Ehc;
+  PEI_URB                 *Urb;
+  UINT8                   Endpoint;
+  EFI_STATUS              Status;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) && 
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) && 
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ehc             = PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    EhcAckAllInterrupt (Ehc);
+    goto ON_EXIT;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  //
+  // Encode the direction in address, although default control
+  // endpoint is bidirectional. EhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = EhcCreateUrb (
+          Ehc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          0,
+          MaximumPacketLength,
+          Translator,
+          EHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL,
+          1
+          );
+
+  if (Urb == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  EhcLinkQhToAsync (Ehc, Urb->Qh);
+  Status = EhcExecTransfer (Ehc, Urb, TimeOut);
+  EhcUnlinkQhFromAsync (Ehc, Urb->Qh);
+
+  //
+  // Get the status from URB. The result is updated in EhcCheckUrbResult
+  // which is called by EhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (*TransferResult == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  EhcAckAllInterrupt (Ehc);
+  EhcFreeUrb (Ehc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            PPI successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+EhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE     FileHandle,
+  IN CONST EFI_PEI_SERVICES  **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *ChipSetUsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  PEI_USB2_HC_DEV             *EhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &ChipSetUsbControllerPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Index = 0;
+  while (TRUE) {
+    Status = ChipSetUsbControllerPpi->GetUsbController (
+                                        (EFI_PEI_SERVICES **) PeiServices,
+                                        ChipSetUsbControllerPpi,
+                                        Index,
+                                        &ControllerType,
+                                        &BaseAddress
+                                        );
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    
+    //
+    // This PEIM is for UHC type controller.
+    //
+    if (ControllerType != PEI_EHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = sizeof (PEI_USB2_HC_DEV) / PAGESIZE + 1;
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesCode,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ZeroMem((VOID *)(UINTN)TempPtr, MemPages*PAGESIZE);
+    EhcDev = (PEI_USB2_HC_DEV *) ((UINTN) TempPtr);
+
+    EhcDev->Signature = USB2_HC_DEV_SIGNATURE;
+
+    EhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+
+
+    EhcDev->HcStructParams = EhcReadCapRegister (EhcDev, EHC_HCSPARAMS_OFFSET);
+    EhcDev->HcCapParams    = EhcReadCapRegister (EhcDev, EHC_HCCPARAMS_OFFSET);
+    EhcDev->CapLen         = EhcReadCapRegister (EhcDev, EHC_CAPLENGTH_OFFSET) & 0x0FF;
+    //
+    // Initialize Uhc's hardware
+    //
+    Status = InitializeUsbHC (EhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    EhcDev->Usb2HostControllerPpi.ControlTransfer          = EhcControlTransfer;
+    EhcDev->Usb2HostControllerPpi.BulkTransfer             = EhcBulkTransfer;
+    EhcDev->Usb2HostControllerPpi.GetRootHubPortNumber     = EhcGetRootHubPortNumber;
+    EhcDev->Usb2HostControllerPpi.GetRootHubPortStatus     = EhcGetRootHubPortStatus;
+    EhcDev->Usb2HostControllerPpi.SetRootHubPortFeature    = EhcSetRootHubPortFeature;
+    EhcDev->Usb2HostControllerPpi.ClearRootHubPortFeature  = EhcClearRootHubPortFeature;
+
+    EhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    EhcDev->PpiDescriptor.Guid = &gPeiUsb2HostControllerPpiGuid;
+    EhcDev->PpiDescriptor.Ppi = &EhcDev->Usb2HostControllerPpi;
+
+    Status = PeiServicesInstallPpi (&EhcDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      Index++;
+      continue;
+    }
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  @param  EhcDev                 EHCI Device.
+
+  @retval EFI_SUCCESS            EHCI successfully initialized.
+  @retval EFI_ABORTED            EHCI was failed to be initialized.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN PEI_USB2_HC_DEV      *EhcDev  
+  )
+{
+  EFI_STATUS  Status;
+
+   	
+  EhcResetHC (EhcDev, EHC_RESET_TIMEOUT);
+
+  Status = EhcInitHC (EhcDev);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;      
+  }
+  
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
new file mode 100644
index 0000000000..74037769a4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhcPeim.h
@@ -0,0 +1,224 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010 - 2015, 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.
+
+**/
+
+#ifndef _RECOVERY_EHC_H_
+#define _RECOVERY_EHC_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/Usb2HostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+
+typedef struct _PEI_USB2_HC_DEV PEI_USB2_HC_DEV;
+
+#define EFI_LIST_ENTRY LIST_ENTRY
+
+#include "UsbHcMem.h"
+#include "EhciReg.h"
+#include "EhciUrb.h"
+#include "EhciSched.h"
+
+#define EFI_USB_SPEED_FULL 0x0000
+#define EFI_USB_SPEED_LOW  0x0001
+#define EFI_USB_SPEED_HIGH 0x0002
+
+#define PAGESIZE           4096
+
+#define EHC_1_MICROSECOND            1
+#define EHC_1_MILLISECOND            (1000 * EHC_1_MICROSECOND)
+#define EHC_1_SECOND                 (1000 * EHC_1_MILLISECOND)
+
+//
+// EHCI register operation timeout, set by experience
+//
+#define EHC_RESET_TIMEOUT            (1 * EHC_1_SECOND)
+#define EHC_GENERIC_TIMEOUT          (10 * EHC_1_MILLISECOND)
+
+
+//
+// Wait for roothub port power stable, refers to Spec[EHCI1.0-2.3.9]
+//
+#define EHC_ROOT_PORT_RECOVERY_STALL (20 * EHC_1_MILLISECOND)
+
+//
+// Sync transfer polling interval, set by experience.
+//
+#define EHC_SYNC_POLL_INTERVAL       (6 * EHC_1_MILLISECOND)
+
+//
+//Iterate through the doule linked list. NOT delete safe
+//
+#define EFI_LIST_FOR_EACH(Entry, ListHead)    \
+  for(Entry = (ListHead)->ForwardLink; Entry != (ListHead); Entry = Entry->ForwardLink)
+
+//
+//Iterate through the doule linked list. This is delete-safe.
+//Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+
+#define EHC_LOW_32BIT(Addr64)     ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define EHC_HIGH_32BIT(Addr64)    ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define EHC_BIT_IS_SET(Data, Bit) ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define EHC_REG_BIT_IS_SET(Ehc, Offset, Bit) \
+          (EHC_BIT_IS_SET(EhcReadOpReg ((Ehc), (Offset)), (Bit)))
+
+#define USB2_HC_DEV_SIGNATURE  SIGNATURE_32 ('e', 'h', 'c', 'i')
+
+struct _PEI_USB2_HC_DEV {
+  UINTN                               Signature;
+  PEI_USB2_HOST_CONTROLLER_PPI        Usb2HostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR              PpiDescriptor;                    
+  UINT32                              UsbHostControllerBaseAddress;
+  PEI_URB                             *Urb;
+  USBHC_MEM_POOL                      *MemPool;
+
+  //
+  // Schedule data shared between asynchronous and periodic
+  // transfers:
+  // ShortReadStop, as its name indicates, is used to terminate
+  // the short read except the control transfer. EHCI follows
+  // the alternative next QTD point when a short read happens.
+  // For control transfer, even the short read happens, try the
+  // status stage.
+  //
+  PEI_EHC_QTD                         *ShortReadStop;
+  EFI_EVENT                           PollTimer;
+  
+  //
+  // Asynchronous(bulk and control) transfer schedule data: 
+  // ReclaimHead is used as the head of the asynchronous transfer
+  // list. It acts as the reclamation header. 
+  //
+  PEI_EHC_QH                          *ReclaimHead;
+  
+  //
+  // Peroidic (interrupt) transfer schedule data:
+  //
+  VOID                                *PeriodFrame;     // Mapped as common buffer 
+  VOID                                *PeriodFrameHost;
+  VOID                                *PeriodFrameMap;
+                                      
+  PEI_EHC_QH                          *PeriodOne;
+  EFI_LIST_ENTRY                      AsyncIntTransfers;
+
+  //
+  // EHCI configuration data
+  //
+  UINT32                              HcStructParams; // Cache of HC structure parameter, EHC_HCSPARAMS_OFFSET
+  UINT32                              HcCapParams;    // Cache of HC capability parameter, HCCPARAMS
+  UINT32                              CapLen;         // Capability length
+  UINT32                              High32bitAddr;
+};
+
+#define PEI_RECOVERY_USB_EHC_DEV_FROM_EHCI_THIS(a)  CR (a, PEI_USB2_HC_DEV, Usb2HostControllerPpi, USB2_HC_DEV_SIGNATURE)
+
+/**
+  @param  EhcDev                 EHCI Device.
+
+  @retval EFI_SUCCESS            EHCI successfully initialized.
+  @retval EFI_ABORTED            EHCI was failed to be initialized.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN PEI_USB2_HC_DEV      *EhcDev  
+  );
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Ehc                   The EHCI device.
+  @param  Check4G               Whether the host controller requires allocated memory 
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+;
+            
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The USB memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+;
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Ehc       The EHCI device.
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+;
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
new file mode 100644
index 0000000000..7083f86681
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
@@ -0,0 +1,70 @@
+## @file
+# The EhcPeim driver is responsible for managing EHCI host controller at PEI phase.
+#
+# It produces gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+# which is used to enable recovery function from USB Drivers.
+#
+# Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EhciPei
+  MODULE_UNI_FILE                = EhciPei.uni
+  FILE_GUID                      = BAB4F20F-0981-4b5f-A047-6EF83BEEAB3C
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = EhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  EhcPeim.c
+  EhcPeim.h
+  EhciUrb.c
+  EhciSched.c
+  UsbHcMem.c
+  EhciReg.h
+  EhciSched.h
+  EhciUrb.h
+  UsbHcMem.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+
+
+[Ppis]
+  gPeiUsb2HostControllerPpiGuid                 ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni
new file mode 100644
index 0000000000..c0f6890275
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni
new file mode 100644
index 0000000000..3f5163896d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h
new file mode 100644
index 0000000000..34c61d8a94
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciReg.h
@@ -0,0 +1,310 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_REG_H_
+#define _EFI_EHCI_REG_H_
+
+
+
+//
+// Capability register offset
+//
+#define EHC_CAPLENGTH_OFFSET    0    // Capability register length offset
+#define EHC_HCSPARAMS_OFFSET    0x04 // Structural Parameters 04-07h
+#define EHC_HCCPARAMS_OFFSET    0x08 // Capability parameters offset
+
+//
+// Capability register bit definition
+//
+#define HCSP_NPORTS             0x0F // Number of root hub port
+#define HCCP_64BIT              0x01 // 64-bit addressing capability
+
+//
+// Operational register offset
+//
+#define EHC_USBCMD_OFFSET       0x0  // USB command register offset
+#define EHC_USBSTS_OFFSET       0x04 // Statue register offset
+#define EHC_USBINTR_OFFSET      0x08 // USB interrutp offset
+#define EHC_FRINDEX_OFFSET      0x0C // Frame index offset
+#define EHC_CTRLDSSEG_OFFSET    0x10 // Control data structure segment offset
+#define EHC_FRAME_BASE_OFFSET   0x14 // Frame list base address offset
+#define EHC_ASYNC_HEAD_OFFSET   0x18 // Next asynchronous list address offset
+#define EHC_CONFIG_FLAG_OFFSET  0x40 // Configure flag register offset
+#define EHC_PORT_STAT_OFFSET    0x44 // Port status/control offset
+
+#define EHC_FRAME_LEN           1024
+
+//
+// Register bit definition
+//
+#define CONFIGFLAG_ROUTE_EHC    0x01 // Route port to EHC
+
+#define USBCMD_RUN              0x01   // Run/stop
+#define USBCMD_RESET            0x02   // Start the host controller reset
+#define USBCMD_ENABLE_PERIOD    0x10   // Enable periodic schedule
+#define USBCMD_ENABLE_ASYNC     0x20   // Enable asynchronous schedule
+#define USBCMD_IAAD             0x40   // Interrupt on async advance doorbell
+
+#define USBSTS_IAA              0x20   // Interrupt on async advance
+#define USBSTS_PERIOD_ENABLED   0x4000 // Periodic schedule status
+#define USBSTS_ASYNC_ENABLED    0x8000 // Asynchronous schedule status
+#define USBSTS_HALT             0x1000 // Host controller halted
+#define USBSTS_SYS_ERROR        0x10   // Host system error
+#define USBSTS_INTACK_MASK      0x003F // Mask for the interrupt ACK, the WC
+                                       // (write clean) bits in USBSTS register
+
+#define PORTSC_CONN             0x01   // Current Connect Status
+#define PORTSC_CONN_CHANGE      0x02   // Connect Status Change
+#define PORTSC_ENABLED          0x04   // Port Enable / Disable
+#define PORTSC_ENABLE_CHANGE    0x08   // Port Enable / Disable Change
+#define PORTSC_OVERCUR          0x10   // Over current Active
+#define PORTSC_OVERCUR_CHANGE   0x20   // Over current Change
+#define PORSTSC_RESUME          0x40   // Force Port Resume
+#define PORTSC_SUSPEND          0x80   // Port Suspend State
+#define PORTSC_RESET            0x100  // Port Reset
+#define PORTSC_LINESTATE_K      0x400  // Line Status K-state
+#define PORTSC_LINESTATE_J      0x800  // Line Status J-state
+#define PORTSC_POWER            0x1000 // Port Power
+#define PORTSC_OWNER            0x2000 // Port Owner
+#define PORTSC_CHANGE_MASK      0x2A   // Mask of the port change bits,
+                                       // they are WC (write clean)
+//
+// PCI Configuration Registers
+//
+#define EHC_BAR_INDEX           0      // how many bytes away from USB_BASE to 0x10
+
+#define EHC_LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define EHC_ADDR(High, QhHw32)   \
+        ((VOID *) (UINTN) (LShiftU64 ((High), 32) | ((QhHw32) & 0xFFFFFFF0)))
+
+#define EHCI_IS_DATAIN(EndpointAddr) EHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+//
+// Structure to map the hardware port states to the 
+// UEFI's port states. 
+//
+typedef struct {
+  UINT16                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Ehci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Pi;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+
+/**
+  Read EHCI capability register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadCapRegister (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+;
+
+/**
+  Read Ehc Operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+EhcReadOpReg (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  UINT32              Offset
+  )
+;
+
+/**
+  Write the data to the EHCI operation register.
+  
+  @param  Ehc       The EHCI device.
+  @param  Offset    EHCI operation register offset.
+  @param  Data      The data to write.
+
+**/
+VOID
+EhcWriteOpReg (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+;
+
+/**
+  Stop the legacy USB SMI support.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcClearLegacySupport (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Set door bell and wait it to be ACKed by host controller.
+  This function is used to synchronize with the hardware.
+  
+  @param  Ehc       The EHCI device.
+  @param  Timeout   The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT       Time out happened while waiting door bell to set.
+  @retval EFI_SUCCESS       Synchronized with the hardware.
+
+**/
+EFI_STATUS
+EhcSetAndWaitDoorBell (
+  IN  PEI_USB2_HC_DEV      *Ehc,
+  IN  UINT32               Timeout
+  )
+;
+
+/**
+  Clear all the interrutp status bits, these bits 
+  are Write-Clean.
+  
+  @param  Ehc       The EHCI device.
+
+**/
+VOID
+EhcAckAllInterrupt (
+  IN  PEI_USB2_HC_DEV     *Ehc
+  )
+;
+
+/**
+  Check whether Ehc is halted.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      The controller is halted.
+  @retval FALSE     The controller isn't halted.
+
+**/
+BOOLEAN
+EhcIsHalt (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Check whether system error occurred.
+  
+  @param  Ehc       The EHCI device.
+
+  @retval TRUE      System error happened.
+  @retval FALSE     No system error.
+
+**/
+BOOLEAN
+EhcIsSysError (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+/**
+  Reset the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT     The transfer failed due to time out.
+  @retval Others          Failed to reset the host.
+
+**/
+EFI_STATUS
+EhcResetHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+;
+
+/**
+  Halt the host controller.
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_TIMEOUT     Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS     The EHCI is halt.
+
+**/
+EFI_STATUS
+EhcHaltHC (
+  IN PEI_USB2_HC_DEV     *Ehc,
+  IN UINT32              Timeout
+  )
+;
+
+/**
+  Set the EHCI to run
+  
+  @param  Ehc             The EHCI device.
+  @param  Timeout         Time to wait before abort.
+
+  @retval EFI_SUCCESS     The EHCI is running.
+  @retval Others          Failed to set the EHCI to run.
+
+**/
+EFI_STATUS
+EhcRunHC (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT32               Timeout
+  )
+;
+
+/**
+  Initialize the HC hardware. 
+  EHCI spec lists the five things to do to initialize the hardware.
+  1. Program CTRLDSSEGMENT.
+  2. Set USBINTR to enable interrupts.
+  3. Set periodic list base.
+  4. Set USBCMD, interrupt threshold, frame list size etc.
+  5. Write 1 to CONFIGFLAG to route all ports to EHCI.
+  
+  @param  Ehc             The EHCI device.
+
+  @retval EFI_SUCCESS     The EHCI has come out of halt state.
+  @retval EFI_TIMEOUT     Time out happened.
+
+**/
+EFI_STATUS
+EhcInitHC (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
new file mode 100644
index 0000000000..e992d4f287
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.c
@@ -0,0 +1,461 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2010 - 2013, 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 "EhcPeim.h"
+
+/**
+  Create helper QTD/QH for the EHCI device.
+  
+  @param  Ehc         The EHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for helper QTD/QH.
+  @retval EFI_SUCCESS           Helper QH/QTD are created.
+
+**/
+EFI_STATUS
+EhcCreateHelpQ (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  USB_ENDPOINT            Ep;
+  PEI_EHC_QH              *Qh;
+  QH_HW                   *QhHw;
+  PEI_EHC_QTD             *Qtd;
+
+  //
+  // Create an inactive Qtd to terminate the short packet read.
+  //
+  Qtd = EhcCreateQtd (Ehc, NULL, 0, QTD_PID_INPUT, 0, 64);
+
+  if (Qtd == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qtd->QtdHw.Status   = QTD_STAT_HALTED;
+  Ehc->ShortReadStop  = Qtd;
+
+  //
+  // Create a QH to act as the EHC reclamation header.
+  // Set the header to loopback to itself.
+  //
+  Ep.DevAddr    = 0;
+  Ep.EpAddr     = 1;
+  Ep.Direction  = EfiUsbDataIn;
+  Ep.DevSpeed   = EFI_USB_SPEED_HIGH;
+  Ep.MaxPacket  = 64;
+  Ep.HubAddr    = 0;
+  Ep.HubPort    = 0;
+  Ep.Toggle     = 0;
+  Ep.Type       = EHC_BULK_TRANSFER;
+  Ep.PollRate   = 1;
+
+  Qh            = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  QhHw              = &Qh->QhHw;
+  QhHw->HorizonLink = QH_LINK (QhHw, EHC_TYPE_QH, FALSE);
+  QhHw->Status      = QTD_STAT_HALTED;
+  QhHw->ReclaimHead = 1;
+  Ehc->ReclaimHead  = Qh;
+
+  //
+  // Create a dummy QH to act as the terminator for periodical schedule
+  //
+  Ep.EpAddr   = 2;
+  Ep.Type     = EHC_INT_TRANSFER_SYNC;
+
+  Qh          = EhcCreateQh (Ehc, &Ep);
+
+  if (Qh == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Qh->QhHw.Status = QTD_STAT_HALTED;
+  Ehc->PeriodOne  = Qh;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the schedule data structure such as frame list.
+  
+  @param  Ehc                   The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+  VOID                  *Map;
+  UINTN                 Index;
+  UINT32                *Desc;
+  EFI_STATUS            Status;
+
+  //
+  // First initialize the periodical schedule data:
+  // 1. Allocate and map the memory for the frame list
+  // 2. Create the help QTD/QH
+  // 3. Initialize the frame entries
+  // 4. Set the frame list register
+  //
+  //
+  // The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &PhyAddr
+             );
+
+  Map = NULL;
+  Ehc->PeriodFrameHost  = (VOID *)(UINTN)PhyAddr;
+  Ehc->PeriodFrame      = (VOID *)(UINTN)PhyAddr;
+  Ehc->PeriodFrameMap   = Map;
+  Ehc->High32bitAddr    = EHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // Init memory pool management then create the helper
+  // QTD/QH. If failed, previously allocated resources
+  // will be freed by EhcFreeSched
+  //
+  Ehc->MemPool = UsbHcInitMemPool (
+                   Ehc,
+                   EHC_BIT_IS_SET (Ehc->HcCapParams, HCCP_64BIT),
+                   Ehc->High32bitAddr
+                   );
+
+  if (Ehc->MemPool == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = EhcCreateHelpQ (Ehc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Initialize the frame list entries then set the registers
+  //
+  Desc = (UINT32 *) Ehc->PeriodFrame;
+
+  for (Index = 0; Index < EHC_FRAME_LEN; Index++) {
+    Desc[Index] = QH_LINK (Ehc->PeriodOne, EHC_TYPE_QH, FALSE);
+  }
+
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, EHC_LOW_32BIT (Ehc->PeriodFrame));
+
+  //
+  // Second initialize the asynchronous schedule:
+  // Only need to set the AsynListAddr register to
+  // the reclamation header
+  //
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, EHC_LOW_32BIT (Ehc->ReclaimHead));
+  return EFI_SUCCESS;
+}
+
+/**
+  Free the schedule data. It may be partially initialized.
+  
+  @param  Ehc   The EHCI device. 
+
+**/
+VOID
+EhcFreeSched (
+  IN PEI_USB2_HC_DEV      *Ehc
+  )
+{
+  EhcWriteOpReg (Ehc, EHC_FRAME_BASE_OFFSET, 0);
+  EhcWriteOpReg (Ehc, EHC_ASYNC_HEAD_OFFSET, 0);
+
+  if (Ehc->PeriodOne != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->PeriodOne, sizeof (PEI_EHC_QH));
+    Ehc->PeriodOne = NULL;
+  }
+
+  if (Ehc->ReclaimHead != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ReclaimHead, sizeof (PEI_EHC_QH));
+    Ehc->ReclaimHead = NULL;
+  }
+
+  if (Ehc->ShortReadStop != NULL) {
+    UsbHcFreeMem (Ehc->MemPool, Ehc->ShortReadStop, sizeof (PEI_EHC_QTD));
+    Ehc->ShortReadStop = NULL;
+  }
+
+  if (Ehc->MemPool != NULL) {
+    UsbHcFreeMemPool (Ehc->MemPool);
+    Ehc->MemPool = NULL;
+  }
+
+  if (Ehc->PeriodFrame != NULL) {
+    Ehc->PeriodFrame = NULL;
+  }
+}
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+{
+  PEI_EHC_QH               *Head;
+
+  //
+  // Append the queue head after the reclaim header, then
+  // fix the hardware visiable parts (EHCI R1.0 page 72).
+  // ReclaimHead is always linked to the EHCI's AsynListAddr.
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Qh->NextQh              = Head->NextQh;
+  Head->NextQh            = Qh;
+
+  Qh->QhHw.HorizonLink    = QH_LINK (Head, EHC_TYPE_QH, FALSE);;
+  Head->QhHw.HorizonLink  = QH_LINK (Qh, EHC_TYPE_QH, FALSE);
+}
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+{
+  PEI_EHC_QH              *Head;
+
+  ASSERT (Ehc->ReclaimHead->NextQh == Qh);
+
+  //
+  // Remove the QH from reclamation head, then update the hardware
+  // visiable part: Only need to loopback the ReclaimHead. The Qh
+  // is pointing to ReclaimHead (which is staill in the list).
+  //
+  Head                    = Ehc->ReclaimHead;
+
+  Head->NextQh            = Qh->NextQh;
+  Qh->NextQh              = NULL;
+
+  Head->QhHw.HorizonLink  = QH_LINK (Head, EHC_TYPE_QH, FALSE);
+
+  //
+  // Set and wait the door bell to synchronize with the hardware
+  //
+  EhcSetAndWaitDoorBell (Ehc, EHC_GENERIC_TIMEOUT);
+  
+  return;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly. 
+
+  @param Ehc   The EHCI device.
+  @param Urb   The URB to check result.
+
+  @retval TRUE    URB transfer is finialized.
+  @retval FALSE   URB transfer is not finialized.
+
+**/
+BOOLEAN
+EhcCheckUrbResult (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb
+  )
+{
+  EFI_LIST_ENTRY          *Entry;
+  PEI_EHC_QTD             *Qtd;
+  QTD_HW                  *QtdHw;
+  UINT8                   State;
+  BOOLEAN                 Finished;
+
+  ASSERT ((Ehc != NULL) && (Urb != NULL) && (Urb->Qh != NULL));
+
+  Finished        = TRUE;
+  Urb->Completed  = 0;
+
+  Urb->Result     = EFI_USB_NOERROR;
+
+  if (EhcIsHalt (Ehc) || EhcIsSysError (Ehc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto ON_EXIT;
+  }
+
+  EFI_LIST_FOR_EACH (Entry, &Urb->Qh->Qtds) {
+    Qtd   = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+    QtdHw = &Qtd->QtdHw;
+    State = (UINT8) QtdHw->Status;
+
+    if (EHC_BIT_IS_SET (State, QTD_STAT_HALTED)) {
+      //
+      // EHCI will halt the queue head when met some error.
+      // If it is halted, the result of URB is finialized.
+      //
+      if ((State & QTD_STAT_ERR_MASK) == 0) {
+        Urb->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BABBLE_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BABBLE;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_BUFF_ERR)) {
+        Urb->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if (EHC_BIT_IS_SET (State, QTD_STAT_TRANS_ERR) && (QtdHw->ErrCnt == 0)) {
+        Urb->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+      
+    } else if (EHC_BIT_IS_SET (State, QTD_STAT_ACTIVE)) {
+      //
+      // The QTD is still active, no need to check furthur.
+      //
+      Urb->Result |= EFI_USB_ERR_NOTEXECUTE;
+      
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // This QTD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a setup.
+      //
+      if (QtdHw->Pid != QTD_PID_SETUP) {
+        Urb->Completed += Qtd->DataLen - QtdHw->TotalBytes;
+      }
+
+      if ((QtdHw->TotalBytes != 0) && (QtdHw->Pid == QTD_PID_INPUT)) {
+        //EHC_DUMP_QH ((Urb->Qh, "Short packet read", FALSE));
+
+        //
+        // Short packet read condition. If it isn't a setup transfer,
+        // no need to check furthur: the queue head will halt at the
+        // ShortReadStop. If it is a setup transfer, need to check the
+        // Status Stage of the setup transfer to get the finial result
+        //
+        if (QtdHw->AltNext == QTD_LINK (Ehc->ShortReadStop, FALSE)) {
+          
+          Finished = TRUE;
+          goto ON_EXIT;
+        }
+      }
+    }
+  }
+
+ON_EXIT:
+  //
+  // Return the data toggle set by EHCI hardware, bulk and interrupt
+  // transfer will use this to initialize the next transaction. For
+  // Control transfer, it always start a new data toggle sequence for
+  // new transfer.
+  //
+  // NOTICE: don't move DT update before the loop, otherwise there is
+  // a race condition that DT is wrong.
+  //
+  Urb->DataToggle = (UINT8) Urb->Qh->QhHw.DataToggle;
+
+  return Finished;
+}
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+  
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb,
+  IN  UINTN               TimeOut
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINTN                   Loop;
+  BOOLEAN                 Finished;
+  BOOLEAN                 InfiniteLoop;
+
+  Status    = EFI_SUCCESS;
+  Loop      = TimeOut * EHC_1_MILLISECOND;
+  Finished     = FALSE;
+  InfiniteLoop = FALSE;
+
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Loop); Index++) {
+    Finished = EhcCheckUrbResult (Ehc, Urb);
+
+    if (Finished) {
+      break;
+    }
+
+    MicroSecondDelay (EHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    Status = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h
new file mode 100644
index 0000000000..6cc52f8d0a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciSched.h
@@ -0,0 +1,100 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010 - 2011, 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.
+
+**/
+
+#ifndef _EFI_EHCI_SCHED_H_
+#define _EFI_EHCI_SCHED_H_
+
+/**
+  Initialize the schedule data structure such as frame list.
+  
+  @param  Ehc   The EHCI device to init schedule data for.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to init schedule data.
+  @retval EFI_SUCCESS           The schedule data is initialized.
+
+**/
+EFI_STATUS
+EhcInitSched (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+;
+
+/**
+  Free the schedule data. It may be partially initialized.
+  
+  @param  Ehc   The EHCI device.
+
+**/
+VOID
+EhcFreeSched (
+  IN PEI_USB2_HC_DEV          *Ehc
+  )
+;
+
+/**
+  Link the queue head to the asynchronous schedule list.
+  UEFI only supports one CTRL/BULK transfer at a time
+  due to its interfaces. This simplifies the AsynList
+  management: A reclamation header is always linked to
+  the AsyncListAddr, the only active QH is appended to it.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to link.
+
+**/
+VOID
+EhcLinkQhToAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+;
+
+/**
+  Unlink a queue head from the asynchronous schedule list.
+  Need to synchronize with hardware.
+  
+  @param  Ehc   The EHCI device.
+  @param  Qh    The queue head to unlink.
+
+**/
+VOID
+EhcUnlinkQhFromAsync (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_EHC_QH           *Qh
+  )
+;
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+  
+  @param  Ehc               The EHCI device.
+  @param  Urb               The URB to execute.
+  @param  TimeOut           The time to wait before abort, in millisecond.
+
+  @retval EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @retval EFI_TIMEOUT       The transfer failed due to time out.
+  @retval EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+EhcExecTransfer (
+  IN  PEI_USB2_HC_DEV     *Ehc,
+  IN  PEI_URB             *Urb,
+  IN  UINTN               TimeOut
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
new file mode 100644
index 0000000000..597a4947f5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.c
@@ -0,0 +1,610 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2010, 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 "EhcPeim.h"
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+  
+  @param  Ehc         The EHCI device.
+  @param  Data        Current data not associated with a QTD.
+  @param  DataLen     The length of the data.
+  @param  PktId       Packet ID to use in the QTD.
+  @param  Toggle      Data toggle to use in the QTD.
+  @param  MaxPacket   Maximu packet length of the endpoint.
+
+  @retval the pointer to the created QTD or NULL if failed to create one.
+
+**/
+PEI_EHC_QTD *
+EhcCreateQtd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT8                *Data,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+{
+  PEI_EHC_QTD             *Qtd;
+  QTD_HW                  *QtdHw;
+  UINTN                   Index;
+  UINTN                   Len;
+  UINTN                   ThisBufLen;
+
+  ASSERT (Ehc != NULL);
+
+  Qtd = UsbHcAllocateMem (Ehc, Ehc->MemPool, sizeof (PEI_EHC_QTD));
+
+  if (Qtd == NULL) {
+    return NULL;
+  }
+
+  Qtd->Signature    = EHC_QTD_SIG;
+  Qtd->Data         = Data;
+  Qtd->DataLen      = 0;
+
+  InitializeListHead (&Qtd->QtdList);
+
+  QtdHw             = &Qtd->QtdHw;
+  QtdHw->NextQtd    = QTD_LINK (NULL, TRUE);
+  QtdHw->AltNext    = QTD_LINK (NULL, TRUE);
+  QtdHw->Status     = QTD_STAT_ACTIVE;
+  QtdHw->Pid        = PktId;
+  QtdHw->ErrCnt     = QTD_MAX_ERR;
+  QtdHw->Ioc        = 0;
+  QtdHw->TotalBytes = 0;
+  QtdHw->DataToggle = Toggle;
+
+  //
+  // Fill in the buffer points
+  //
+  if (Data != NULL) {
+    Len = 0;
+
+    for (Index = 0; Index <= QTD_MAX_BUFFER; Index++) {
+      //
+      // Set the buffer point (Check page 41 EHCI Spec 1.0). No need to
+      // compute the offset and clear Reserved fields. This is already
+      // done in the data point.
+      //
+      QtdHw->Page[Index]      = EHC_LOW_32BIT (Data);
+      QtdHw->PageHigh[Index]  = EHC_HIGH_32BIT (Data);
+
+      ThisBufLen              = QTD_BUF_LEN - (EHC_LOW_32BIT (Data) & QTD_BUF_MASK);
+
+      if (Len + ThisBufLen >= DataLen) {
+        Len = DataLen;
+        break;
+      }
+
+      Len += ThisBufLen;
+      Data += ThisBufLen;
+    }
+    
+    //
+    // Need to fix the last pointer if the Qtd can't hold all the
+    // user's data to make sure that the length is in the unit of
+    // max packets. If it can hold all the data, there is no such
+    // need.
+    //
+    if (Len < DataLen) {
+      Len = Len - Len % MaxPacket;
+    }
+
+    QtdHw->TotalBytes = (UINT32) Len;
+    Qtd->DataLen      = Len;
+  }
+
+  return Qtd;
+}
+
+/**
+  Initialize the queue head for interrupt transfer,
+  that is, initialize the following three fields:
+    1. SplitXState in the Status field.
+    2. Microframe S-mask.
+    3. Microframe C-mask.
+  
+  @param  Ep    The queue head's related endpoint.
+  @param  QhHw  The queue head to initialize.
+
+**/
+VOID
+EhcInitIntQh (
+  IN USB_ENDPOINT         *Ep,
+  IN QH_HW                *QhHw
+  )
+{
+  //
+  // Because UEFI interface can't utilitize an endpoint with
+  // poll rate faster than 1ms, only need to set one bit in
+  // the queue head. simple. But it may be changed later. If
+  // sub-1ms interrupt is supported, need to update the S-Mask
+  // here
+  //
+  if (Ep->DevSpeed == EFI_USB_SPEED_HIGH) {
+    QhHw->SMask = QH_MICROFRAME_0;
+    return ;
+  }
+  
+  //
+  // For low/full speed device, the transfer must go through
+  // the split transaction. Need to update three fields
+  // 1. SplitXState in the status
+  // 2. Microframe S-Mask
+  // 3. Microframe C-Mask
+  // UEFI USB doesn't exercise admission control. It simplely
+  // schedule the high speed transactions in microframe 0, and
+  // full/low speed transactions at microframe 1. This also
+  // avoid the use of FSTN.
+  //
+  QhHw->SMask = QH_MICROFRAME_1;
+  QhHw->CMask = QH_MICROFRAME_3 | QH_MICROFRAME_4 | QH_MICROFRAME_5;
+}
+
+/**
+  Allocate and initialize a EHCI queue head.
+  
+  @param  Ehci      The EHCI device.
+  @param  Ep        The endpoint to create queue head for.
+
+  @retval the pointer to the created queue head or NULL if failed to create one.
+
+**/
+PEI_EHC_QH *
+EhcCreateQh (
+  IN PEI_USB2_HC_DEV      *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+{
+  PEI_EHC_QH              *Qh;
+  QH_HW                   *QhHw;
+
+  Qh = UsbHcAllocateMem (Ehci, Ehci->MemPool, sizeof (PEI_EHC_QH));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->Signature       = EHC_QH_SIG;
+  Qh->NextQh          = NULL;
+  Qh->Interval        = Ep->PollRate;
+  
+  InitializeListHead (&Qh->Qtds);
+
+  QhHw                = &Qh->QhHw;
+  QhHw->HorizonLink   = QH_LINK (NULL, 0, TRUE);
+  QhHw->DeviceAddr    = Ep->DevAddr;
+  QhHw->Inactive      = 0;
+  QhHw->EpNum         = Ep->EpAddr;
+  QhHw->EpSpeed       = Ep->DevSpeed;
+  QhHw->DtCtrl        = 0;
+  QhHw->ReclaimHead   = 0;
+  QhHw->MaxPacketLen  = (UINT32) Ep->MaxPacket;
+  QhHw->CtrlEp        = 0;
+  QhHw->NakReload     = QH_NAK_RELOAD;
+  QhHw->HubAddr       = Ep->HubAddr;
+  QhHw->PortNum       = Ep->HubPort;
+  QhHw->Multiplier    = 1;
+  QhHw->DataToggle    = Ep->Toggle;
+
+  if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+    QhHw->Status |= QTD_STAT_DO_SS;
+  }
+
+  switch (Ep->Type) {
+  case EHC_CTRL_TRANSFER:
+    //
+    // Special initialization for the control transfer:
+    // 1. Control transfer initialize data toggle from each QTD
+    // 2. Set the Control Endpoint Flag (C) for low/full speed endpoint.
+    //
+    QhHw->DtCtrl = 1;
+
+    if (Ep->DevSpeed != EFI_USB_SPEED_HIGH) {
+      QhHw->CtrlEp = 1;
+    }
+    break;
+
+  case EHC_INT_TRANSFER_ASYNC:
+  case EHC_INT_TRANSFER_SYNC:
+    //
+    // Special initialization for the interrupt transfer
+    // to set the S-Mask and C-Mask
+    //
+    QhHw->NakReload = 0;
+    EhcInitIntQh (Ep, QhHw);
+    break;
+
+  case EHC_BULK_TRANSFER:
+    if ((Ep->DevSpeed == EFI_USB_SPEED_HIGH) && (Ep->Direction == EfiUsbDataOut)) {
+      QhHw->Status |= QTD_STAT_DO_PING;
+    }
+
+    break;
+  }
+
+  return Qh;
+}
+
+/**
+  Convert the poll interval from application to that
+  be used by EHCI interface data structure. Only need
+  to get the max 2^n that is less than interval. UEFI
+  can't support high speed endpoint with a interval less
+  than 8 microframe because interval is specified in
+  the unit of ms (millisecond).
+  
+  @param Interval       The interval to convert.
+
+  @retval The converted interval.
+
+**/
+UINTN
+EhcConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  if (Interval == 0) {
+    return 1;
+  }
+  
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+/**
+  Free a list of QTDs.
+  
+  @param  Ehc         The EHCI device.
+  @param  Qtds        The list head of the QTD.
+
+**/
+VOID
+EhcFreeQtds (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN EFI_LIST_ENTRY       *Qtds
+  )
+{
+  EFI_LIST_ENTRY          *Entry;
+  EFI_LIST_ENTRY          *Next;
+  PEI_EHC_QTD             *Qtd;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+
+    RemoveEntryList (&Qtd->QtdList);
+    UsbHcFreeMem (Ehc->MemPool, Qtd, sizeof (PEI_EHC_QTD));
+  }
+}
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+{
+  if (Urb->Qh != NULL) {
+    //
+    // Ensure that this queue head has been unlinked from the
+    // schedule data structures. Free all the associated QTDs
+    //
+    EhcFreeQtds (Ehc, &Urb->Qh->Qtds);
+    UsbHcFreeMem (Ehc->MemPool, Urb->Qh, sizeof (PEI_EHC_QH));
+  }
+}
+
+/**
+  Create a list of QTDs for the URB.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to create QTDs for.
+
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate resource for QTD.
+  @retval EFI_SUCCESS             The QTDs are allocated for the URB.
+
+**/
+EFI_STATUS
+EhcCreateQtds (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+{
+  USB_ENDPOINT            *Ep;
+  PEI_EHC_QH              *Qh;
+  PEI_EHC_QTD             *Qtd;
+  PEI_EHC_QTD             *StatusQtd;
+  PEI_EHC_QTD             *NextQtd;
+  EFI_LIST_ENTRY          *Entry;
+  UINT32                  AlterNext;
+  UINT8                   Toggle;
+  UINTN                   Len;
+  UINT8                   Pid;
+
+  ASSERT ((Urb != NULL) && (Urb->Qh != NULL));
+
+  //
+  // EHCI follows the alternet next QTD pointer if it meets
+  // a short read and the AlterNext pointer is valid. UEFI
+  // EHCI driver should terminate the transfer except the
+  // control transfer.
+  //
+  Toggle    = 0;
+  Qh        = Urb->Qh;
+  Ep        = &Urb->Ep;
+  StatusQtd = NULL;
+  AlterNext = QTD_LINK (NULL, TRUE);
+
+  if (Ep->Direction == EfiUsbDataIn) {
+    AlterNext = QTD_LINK (Ehc->ShortReadStop, FALSE);
+  }
+  
+  //
+  // Build the Setup and status packets for control transfer
+  //
+  if (Urb->Ep.Type == EHC_CTRL_TRANSFER) {
+    Len = sizeof (EFI_USB_DEVICE_REQUEST);
+    Qtd = EhcCreateQtd (Ehc, Urb->RequestPhy, Len, QTD_PID_SETUP, 0, Ep->MaxPacket);
+
+    if (Qtd == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Create the status packet now. Set the AlterNext to it. So, when
+    // EHCI meets a short control read, it can resume at the status stage.
+    // Use the opposite direction of the data stage, or IN if there is
+    // no data stage.
+    //
+    if (Ep->Direction == EfiUsbDataIn) {
+      Pid = QTD_PID_OUTPUT;
+    } else {
+      Pid = QTD_PID_INPUT;
+    }
+
+    StatusQtd = EhcCreateQtd (Ehc, NULL, 0, Pid, 1, Ep->MaxPacket);
+
+    if (StatusQtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    if (Ep->Direction == EfiUsbDataIn) {
+      AlterNext = QTD_LINK (StatusQtd, FALSE);
+    }
+
+    Toggle = 1;
+  }
+
+  //
+  // Build the data packets for all the transfers
+  //
+  if (Ep->Direction == EfiUsbDataIn) {
+    Pid = QTD_PID_INPUT;
+  } else {
+    Pid = QTD_PID_OUTPUT;
+  }
+
+  Qtd = NULL;
+  Len = 0;
+
+  while (Len < Urb->DataLen) {
+    Qtd = EhcCreateQtd (
+            Ehc,
+            (UINT8 *) Urb->DataPhy + Len,
+            Urb->DataLen - Len,
+            Pid,
+            Toggle,
+            Ep->MaxPacket
+            );
+
+    if (Qtd == NULL) {
+      goto ON_ERROR;
+    }
+
+    Qtd->QtdHw.AltNext = AlterNext;
+    InsertTailList (&Qh->Qtds, &Qtd->QtdList);
+
+    //
+    // Switch the Toggle bit if odd number of packets are included in the QTD.
+    //
+    if (((Qtd->DataLen + Ep->MaxPacket - 1) / Ep->MaxPacket) % 2) {
+      Toggle = (UINT8) (1 - Toggle);
+    }
+
+    Len += Qtd->DataLen;
+  }
+  
+  //
+  // Insert the status packet for control transfer
+  //
+  if (Ep->Type == EHC_CTRL_TRANSFER) {
+    InsertTailList (&Qh->Qtds, &StatusQtd->QtdList);
+  }
+
+  //
+  // OK, all the QTDs needed are created. Now, fix the NextQtd point
+  //
+  EFI_LIST_FOR_EACH (Entry, &Qh->Qtds) {
+    Qtd = EFI_LIST_CONTAINER (Entry, PEI_EHC_QTD, QtdList);
+
+    //
+    // break if it is the last entry on the list
+    //
+    if (Entry->ForwardLink == &Qh->Qtds) {
+      break;
+    }
+
+    NextQtd             = EFI_LIST_CONTAINER (Entry->ForwardLink, PEI_EHC_QTD, QtdList);
+    Qtd->QtdHw.NextQtd  = QTD_LINK (NextQtd, FALSE);
+  }
+
+  //
+  // Link the QTDs to the queue head
+  //
+  NextQtd           = EFI_LIST_CONTAINER (Qh->Qtds.ForwardLink, PEI_EHC_QTD, QtdList);
+  Qh->QhHw.NextQtd  = QTD_LINK (NextQtd, FALSE);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  EhcFreeQtds (Ehc, &Qh->Qtds);
+  return EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Create a new URB and its associated QTD.
+  
+  @param  Ehc               The EHCI device.
+  @param  DevAddr           The device address.
+  @param  EpAddr            Endpoint addrress & its direction.
+  @param  DevSpeed          The device speed.
+  @param  Toggle            Initial data toggle to use.
+  @param  MaxPacket         The max packet length of the endpoint.
+  @param  Hub               The transaction translator to use.
+  @param  Type              The transaction type.
+  @param  Request           The standard USB request for control transfer.
+  @param  Data              The user data to transfer.
+  @param  DataLen           The length of data buffer.
+  @param  Callback          The function to call when data is transferred.
+  @param  Context           The context to the callback.
+  @param  Interval          The interval for interrupt transfer.
+
+  @retval the pointer to the created URB or NULL.
+
+**/
+PEI_URB *
+EhcCreateUrb (
+  IN PEI_USB2_HC_DEV                    *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,  
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_STATUS                    Status;
+  UINTN                         Len;
+  PEI_URB                       *Urb;
+  VOID                          *Map;
+
+    
+  Map = NULL;
+  
+  Urb = Ehc->Urb;
+  Urb->Signature  = EHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep              = &Urb->Ep;
+  Ep->DevAddr     = DevAddr;
+  Ep->EpAddr      = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction   = (((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut);
+  Ep->DevSpeed    = DevSpeed;
+  Ep->MaxPacket   = MaxPacket;
+
+  Ep->HubAddr     = 0;
+  Ep->HubPort     = 0;
+
+  if (DevSpeed != EFI_USB_SPEED_HIGH) {
+    ASSERT (Hub != NULL);
+
+    Ep->HubAddr   = Hub->TranslatorHubAddress;
+    Ep->HubPort   = Hub->TranslatorPortNumber;
+  }
+
+  Ep->Toggle      = Toggle;
+  Ep->Type        = Type;
+  Ep->PollRate    = EhcConvertPollRate (Interval);
+
+  Urb->Request    = Request;
+  Urb->Data       = Data;
+  Urb->DataLen    = DataLen;
+  Urb->Callback   = Callback;
+  Urb->Context    = Context;
+  Urb->Qh         = EhcCreateQh (Ehc, &Urb->Ep);
+
+  if (Urb->Qh == NULL) {
+    goto ON_ERROR;
+  }
+  
+  //
+  // Map the request and user data
+  //
+  if (Request != NULL) {
+    Len     = sizeof (EFI_USB_DEVICE_REQUEST);
+    PhyAddr =  (EFI_PHYSICAL_ADDRESS) (UINTN) Request ;
+    if ( (Len != sizeof (EFI_USB_DEVICE_REQUEST))) {
+      goto ON_ERROR;
+    }
+
+    Urb->RequestPhy = (VOID *) ((UINTN) PhyAddr);
+    Urb->RequestMap = Map;
+  }
+
+  if (Data != NULL) {
+    Len      = DataLen;
+    PhyAddr  =  (EFI_PHYSICAL_ADDRESS) (UINTN) Data ;
+    if ( (Len != DataLen)) {
+      goto ON_ERROR;
+    }
+
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  Status = EhcCreateQtds (Ehc, Urb);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Urb;
+
+ON_ERROR:
+  EhcFreeUrb (Ehc, Urb);
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h
new file mode 100644
index 0000000000..3fe93fb294
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/EhciUrb.h
@@ -0,0 +1,331 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_URB_H_
+#define _EFI_EHCI_URB_H_
+
+typedef struct _PEI_EHC_QTD PEI_EHC_QTD;
+typedef struct _PEI_EHC_QH  PEI_EHC_QH;
+typedef struct _PEI_URB     PEI_URB;
+
+#define EHC_CTRL_TRANSFER       0x01
+#define EHC_BULK_TRANSFER       0x02
+#define EHC_INT_TRANSFER_SYNC   0x04
+#define EHC_INT_TRANSFER_ASYNC  0x08
+
+#define EHC_QTD_SIG             SIGNATURE_32 ('U', 'S', 'B', 'T')
+#define EHC_QH_SIG              SIGNATURE_32 ('U', 'S', 'B', 'H')
+#define EHC_URB_SIG             SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Hardware related bit definitions
+//
+#define EHC_TYPE_ITD            0x00
+#define EHC_TYPE_QH             0x02
+#define EHC_TYPE_SITD           0x04
+#define EHC_TYPE_FSTN           0x06
+
+#define QH_NAK_RELOAD           3
+#define QH_HSHBW_MULTI          1
+
+#define QTD_MAX_ERR             3
+#define QTD_PID_OUTPUT          0x00
+#define QTD_PID_INPUT           0x01
+#define QTD_PID_SETUP           0x02
+
+#define QTD_STAT_DO_OUT         0
+#define QTD_STAT_DO_SS          0
+#define QTD_STAT_DO_PING        0x01
+#define QTD_STAT_DO_CS          0x02
+#define QTD_STAT_TRANS_ERR      0x08
+#define QTD_STAT_BABBLE_ERR     0x10
+#define QTD_STAT_BUFF_ERR       0x20
+#define QTD_STAT_HALTED         0x40
+#define QTD_STAT_ACTIVE         0x80
+#define QTD_STAT_ERR_MASK       (QTD_STAT_TRANS_ERR | QTD_STAT_BABBLE_ERR | QTD_STAT_BUFF_ERR)
+
+#define QTD_MAX_BUFFER          4
+#define QTD_BUF_LEN             4096
+#define QTD_BUF_MASK            0x0FFF
+
+#define QH_MICROFRAME_0         0x01
+#define QH_MICROFRAME_1         0x02
+#define QH_MICROFRAME_2         0x04
+#define QH_MICROFRAME_3         0x08
+#define QH_MICROFRAME_4         0x10
+#define QH_MICROFRAME_5         0x20
+#define QH_MICROFRAME_6         0x40
+#define QH_MICROFRAME_7         0x80
+
+#define USB_ERR_SHORT_PACKET    0x200
+
+//
+// Fill in the hardware link point: pass in a EHC_QH/QH_HW 
+// pointer to QH_LINK; A EHC_QTD/QTD_HW pointer to QTD_LINK
+//
+#define QH_LINK(Addr, Type, Term) \
+          ((UINT32) ((EHC_LOW_32BIT (Addr) & 0xFFFFFFE0) | (Type) | ((Term) ? 1 : 0)))
+
+#define QTD_LINK(Addr, Term)      QH_LINK((Addr), 0, (Term))
+
+//
+// The defination of EHCI hardware used data structure for 
+// little endian architecture. The QTD and QH structures 
+// are required to be 32 bytes aligned. Don't add members 
+// to the head of the associated software strucuture.
+//
+#pragma pack(1)
+typedef struct {
+  UINT32                  NextQtd;
+  UINT32                  AltNext;
+  
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QTD_HW;
+
+typedef struct {
+  UINT32                  HorizonLink;  
+  //
+  // Endpoint capabilities/Characteristics DWord 1 and DWord 2
+  //
+  UINT32                  DeviceAddr   : 7;
+  UINT32                  Inactive     : 1;
+  UINT32                  EpNum        : 4;
+  UINT32                  EpSpeed      : 2;
+  UINT32                  DtCtrl       : 1;
+  UINT32                  ReclaimHead  : 1;
+  UINT32                  MaxPacketLen : 11;
+  UINT32                  CtrlEp       : 1;
+  UINT32                  NakReload    : 4;
+
+  UINT32                  SMask        : 8;
+  UINT32                  CMask        : 8;
+  UINT32                  HubAddr      : 7;
+  UINT32                  PortNum      : 7;
+  UINT32                  Multiplier   : 2;
+
+  //
+  // Transaction execution overlay area
+  //
+  UINT32                  CurQtd;
+  UINT32                  NextQtd;
+  UINT32                  AltQtd;
+  
+  UINT32                  Status       : 8;
+  UINT32                  Pid          : 2;
+  UINT32                  ErrCnt       : 2;
+  UINT32                  CurPage      : 3;
+  UINT32                  Ioc          : 1;
+  UINT32                  TotalBytes   : 15;
+  UINT32                  DataToggle   : 1;
+
+  UINT32                  Page[5];
+  UINT32                  PageHigh[5];
+} QH_HW;
+#pragma pack()
+
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  UINT8                   DevAddr;
+  UINT8                   EpAddr;     // Endpoint address, no direction encoded in
+  EFI_USB_DATA_DIRECTION  Direction;
+  UINT8                   DevSpeed;
+  UINTN                   MaxPacket;
+  UINT8                   HubAddr;
+  UINT8                   HubPort;
+  UINT8                   Toggle;     // Data toggle, not used for control transfer
+  UINTN                   Type;
+  UINTN                   PollRate;   // Polling interval used by EHCI
+} USB_ENDPOINT;
+
+//
+// Software QTD strcture, this is used to manage all the 
+// QTD generated from a URB. Don't add fields before QtdHw.
+//
+struct _PEI_EHC_QTD {
+  QTD_HW                  QtdHw;
+  UINT32                  Signature;
+  EFI_LIST_ENTRY          QtdList;   // The list of QTDs to one end point
+  UINT8                   *Data;     // Buffer of the original data
+  UINTN                   DataLen;   // Original amount of data in this QTD
+};
+
+
+
+//
+// Software QH structure. All three different transaction types 
+// supported by UEFI USB, that is the control/bulk/interrupt 
+// transfers use the queue head and queue token strcuture. 
+//
+// Interrupt QHs are linked to periodic frame list in the reversed
+// 2^N tree. Each interrupt QH is linked to the list starting at 
+// frame 0. There is a dummy interrupt QH linked to each frame as
+// a sentinental whose polling interval is 1. Synchronous interrupt
+// transfer is linked after this dummy QH. 
+// 
+// For control/bulk transfer, only synchronous (in the sense of UEFI) 
+// transfer is supported. A dummy QH is linked to EHCI AsyncListAddr
+// as the reclamation header. New transfer is inserted after this QH.
+//
+struct _PEI_EHC_QH {
+  QH_HW                   QhHw;
+  UINT32                  Signature;
+  PEI_EHC_QH              *NextQh;    // The queue head pointed to by horizontal link
+  EFI_LIST_ENTRY          Qtds;       // The list of QTDs to this queue head
+  UINTN                   Interval; 
+};
+
+//
+// URB (Usb Request Block) contains information for all kinds of 
+// usb requests.
+//
+struct _PEI_URB {
+  UINT32                          Signature;
+  EFI_LIST_ENTRY                  UrbList;
+  
+  //
+  // Transaction information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;     // Control transfer only
+  VOID                            *RequestPhy;  // Address of the mapped request
+  VOID                            *RequestMap;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;     // Address of the mapped user data
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback; 
+  VOID                            *Context;
+
+  //
+  // Schedule data
+  //
+  PEI_EHC_QH                      *Qh;
+  
+  //
+  // Transaction result
+  //
+  UINT32                          Result;
+  UINTN                           Completed;    // completed data length
+  UINT8                           DataToggle;
+};
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+  
+  @param  Ehc         The EHCI device.
+  @param  Data        Current data not associated with a QTD.
+  @param  DataLen     The length of the data.
+  @param  PktId       Packet ID to use in the QTD.
+  @param  Toggle      Data toggle to use in the QTD.
+  @param  MaxPacket   Maximu packet length of the endpoint.
+
+  @retval the pointer to the created QTD or NULL if failed to create one.
+
+**/
+PEI_EHC_QTD *
+EhcCreateQtd (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN UINT8                *Data,
+  IN UINTN                DataLen,
+  IN UINT8                PktId,
+  IN UINT8                Toggle,
+  IN UINTN                MaxPacket
+  )
+;
+
+/**
+  Allocate and initialize a EHCI queue head.
+  
+  @param  Ehci      The EHCI device.
+  @param  Ep        The endpoint to create queue head for.
+
+  @retval the pointer to the created queue head or NULL if failed to create one.
+
+**/
+PEI_EHC_QH *
+EhcCreateQh (
+  IN PEI_USB2_HC_DEV      *Ehci,
+  IN USB_ENDPOINT         *Ep
+  )
+;
+
+/**
+  Free an allocated URB. It is possible for it to be partially inited.
+  
+  @param  Ehc         The EHCI device.
+  @param  Urb         The URB to free.
+
+**/
+VOID
+EhcFreeUrb (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN PEI_URB              *Urb
+  )
+;
+
+/**
+  Create a new URB and its associated QTD.
+  
+  @param  Ehc               The EHCI device.
+  @param  DevAddr           The device address.
+  @param  EpAddr            Endpoint addrress & its direction.
+  @param  DevSpeed          The device speed.
+  @param  Toggle            Initial data toggle to use.
+  @param  MaxPacket         The max packet length of the endpoint.
+  @param  Hub               The transaction translator to use.
+  @param  Type              The transaction type.
+  @param  Request           The standard USB request for control transfer.
+  @param  Data              The user data to transfer.
+  @param  DataLen           The length of data buffer.
+  @param  Callback          The function to call when data is transferred.
+  @param  Context           The context to the callback.
+  @param  Interval          The interval for interrupt transfer.
+
+  @retval the pointer to the created URB or NULL.
+
+**/
+PEI_URB *
+EhcCreateUrb (
+  IN PEI_USB2_HC_DEV                    *Ehc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,  
+  IN UINT8                              DevSpeed,
+  IN UINT8                              Toggle,
+  IN UINTN                              MaxPacket,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR *Hub,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context,
+  IN UINTN                              Interval
+  )
+;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
new file mode 100644
index 0000000000..6b3755852b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.c
@@ -0,0 +1,493 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2010, 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 "EhcPeim.h"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Ehc            The EHCI device.
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  EFI_STATUS              Status;
+  UINTN               PageNumber;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  Mapping = NULL;
+  PageNumber =  sizeof(USBHC_MEM_BLOCK)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+      return NULL;
+  }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);
+  
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+  
+  Block = (USBHC_MEM_BLOCK*)(UINTN)TempPtr;
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  
+  PageNumber =  (Block->BitsLen)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+             &TempPtr
+             );
+  
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);	
+
+  Block->Bits  = (UINT8 *)(UINTN)TempPtr;
+
+  
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             Pages,
+             &TempPtr
+             );
+  ZeroMem ((VOID   *)(UINTN)TempPtr, Pages*EFI_PAGE_SIZE);
+
+  BufHost  = (VOID *)(UINTN)TempPtr;
+  MappedAddr = (EFI_PHYSICAL_ADDRESS) (UINTN) BufHost;
+  //
+  // Check whether the data structure used by the host controller
+  // should be restricted into the same 4G
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+      return NULL;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+  Block->Next      = NULL;
+
+  return Block;
+
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+  
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Ehc                   The EHCI device.
+  @param  Check4G               Whether the host controller requires allocated memory.
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+  UINTN               PageNumber;
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  
+  PageNumber =  sizeof(USBHC_MEM_POOL)/PAGESIZE +1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             PageNumber,
+              &TempPtr
+             );
+
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+  ZeroMem ((VOID   *)(UINTN)TempPtr, PageNumber*EFI_PAGE_SIZE);	
+
+  Pool = (USBHC_MEM_POOL *) ((UINTN) TempPtr);	
+
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Ehc, Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The USB memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Ehc       The EHCI device.
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN PEI_USB2_HC_DEV      *Ehc,
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+  NewBlock = UsbHcAllocMemBlock (Ehc,Pool, Pages);
+
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+  
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
new file mode 100644
index 0000000000..586d12af96
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/EhciPei/UsbHcMem.h
@@ -0,0 +1,77 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#include <Uefi.h>
+#include <IndustryStandard/Pci22.h>
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen; 
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;     
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB 
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory. 
+//
+typedef struct _USBHC_MEM_POOL {
+  BOOLEAN                 Check4G;      
+  UINT32                  Which4G;      
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)       
+
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c
new file mode 100644
index 0000000000..be1b829200
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.c
@@ -0,0 +1,2501 @@
+/** @file
+PEIM to produce gEfiPeiVirtualBlockIoPpiGuid & gEfiPeiVirtualBlockIo2PpiGuid PPI for
+ATA controllers in the platform.
+
+This PPI can be consumed by PEIM which produce gEfiPeiDeviceRecoveryModulePpiGuid
+for Atapi CD ROM device.
+
+Copyright (c) 2006 - 2015, 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 "AtapiPeim.h"
+
+/**
+  Initializes the Atapi Block Io PPI.  
+  
+  @param[in]  FileHandle           Handle of the file being invoked.
+  @param[in]  PeiServices          Describes the list of possible PEI Services.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+  @retval     EFI_OUT_OF_RESOURCES Not enough memory to allocate.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiPeimEntry (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  PEI_ATA_CONTROLLER_PPI  *AtaControllerPpi;
+  EFI_STATUS              Status;
+  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev;
+
+  Status = PeiServicesRegisterForShadow (FileHandle);
+  if (!EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PeiServicesLocatePpi (
+              &gPeiAtaControllerPpiGuid,
+              0,
+              NULL,
+              (VOID **) &AtaControllerPpi
+              );
+  ASSERT_EFI_ERROR (Status);
+
+  AtapiBlkIoDev = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (*AtapiBlkIoDev)));
+  if (AtapiBlkIoDev == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  AtapiBlkIoDev->Signature        = ATAPI_BLK_IO_DEV_SIGNATURE;
+  AtapiBlkIoDev->AtaControllerPpi = AtaControllerPpi;
+
+  //
+  // atapi device enumeration and build private data
+  //
+  AtapiEnumerateDevices (AtapiBlkIoDev);
+
+  AtapiBlkIoDev->AtapiBlkIo.GetNumberOfBlockDevices = AtapiGetNumberOfBlockDevices;
+  AtapiBlkIoDev->AtapiBlkIo.GetBlockDeviceMediaInfo = AtapiGetBlockDeviceMediaInfo;
+  AtapiBlkIoDev->AtapiBlkIo.ReadBlocks              = AtapiReadBlocks;
+  AtapiBlkIoDev->AtapiBlkIo2.Revision                = EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION;
+  AtapiBlkIoDev->AtapiBlkIo2.GetNumberOfBlockDevices = AtapiGetNumberOfBlockDevices2;
+  AtapiBlkIoDev->AtapiBlkIo2.GetBlockDeviceMediaInfo = AtapiGetBlockDeviceMediaInfo2;
+  AtapiBlkIoDev->AtapiBlkIo2.ReadBlocks              = AtapiReadBlocks2;
+
+  AtapiBlkIoDev->PpiDescriptor.Flags                = EFI_PEI_PPI_DESCRIPTOR_PPI;
+  AtapiBlkIoDev->PpiDescriptor.Guid                 = &gEfiPeiVirtualBlockIoPpiGuid;
+  AtapiBlkIoDev->PpiDescriptor.Ppi                  = &AtapiBlkIoDev->AtapiBlkIo;
+
+  AtapiBlkIoDev->PpiDescriptor2.Flags                = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+  AtapiBlkIoDev->PpiDescriptor2.Guid                 = &gEfiPeiVirtualBlockIo2PpiGuid;
+  AtapiBlkIoDev->PpiDescriptor2.Ppi                  = &AtapiBlkIoDev->AtapiBlkIo2;
+
+  DEBUG ((EFI_D_INFO, "Atatpi Device Count is %d\n", AtapiBlkIoDev->DeviceCount));
+  if (AtapiBlkIoDev->DeviceCount != 0) {
+    Status = PeiServicesInstallPpi (&AtapiBlkIoDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI   *This,
+  OUT  UINTN                             *NumberBlockDevices
+  )
+{
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  AtapiBlkIoDev       = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  *NumberBlockDevices = AtapiBlkIoDev->DeviceCount;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS           Media information about the specified block device 
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware 
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI        *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO_MEDIA               *MediaInfo
+  )
+{
+  UINTN             DeviceCount;
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+  EFI_STATUS        Status;
+  UINTN             Index;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL || MediaInfo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  DeviceCount   = AtapiBlkIoDev->DeviceCount;
+
+  //
+  // DeviceIndex is a value from 1 to NumberBlockDevices.
+  //
+  if ((DeviceIndex < 1) || (DeviceIndex > DeviceCount) || (DeviceIndex > MAX_IDE_DEVICES)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Index = DeviceIndex - 1;
+
+  //
+  // probe media and retrieve latest media information
+  //
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DevicePosition is %d\n", AtapiBlkIoDev->DeviceInfo[Index].DevicePosition));  
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DeviceType is   %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.DeviceType));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo MediaPresent is %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.MediaPresent));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo BlockSize is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.BlockSize));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo LastBlock is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.LastBlock));
+
+  Status = DetectMedia (
+             AtapiBlkIoDev,
+             AtapiBlkIoDev->DeviceInfo[Index].DevicePosition,
+             &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo,
+             &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo2
+             );
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DevicePosition is %d\n", AtapiBlkIoDev->DeviceInfo[Index].DevicePosition));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo DeviceType is   %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.DeviceType));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo MediaPresent is %d\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.MediaPresent));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo BlockSize is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.BlockSize));
+  DEBUG ((EFI_D_INFO, "Atatpi GetInfo LastBlock is  0x%x\n", AtapiBlkIoDev->DeviceInfo[Index].MediaInfo.LastBlock));
+  
+  //
+  // Get media info from AtapiBlkIoDev
+  //
+  CopyMem (MediaInfo, &AtapiBlkIoDev->DeviceInfo[Index].MediaInfo, sizeof(EFI_PEI_BLOCK_IO_MEDIA));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  )
+{
+
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_STATUS              Status;
+  UINTN                   NumberOfBlocks;
+  UINTN                   BlockSize;
+  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS (This);
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  Status = AtapiGetBlockDeviceMediaInfo (
+            PeiServices,
+            This,
+            DeviceIndex,
+            &MediaInfo
+            );
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (!MediaInfo.MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  BlockSize = MediaInfo.BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks = BufferSize / BlockSize;
+
+  if ((StartLBA + NumberOfBlocks - 1) > AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].MediaInfo2.LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ReadSectors (
+            AtapiBlkIoDev,
+            AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].DevicePosition,
+            Buffer,
+            StartLBA,
+            NumberOfBlocks,
+            BlockSize
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  OUT  UINTN                             *NumberBlockDevices
+  )
+{
+  EFI_STATUS        Status;
+  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiGetNumberOfBlockDevices (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             NumberBlockDevices
+             );
+
+  return Status;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS           Media information about the specified block device
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo2 (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI       *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO2_MEDIA              *MediaInfo
+  )
+{
+  ATAPI_BLK_IO_DEV           *AtapiBlkIoDev;
+  EFI_STATUS                 Status;
+  EFI_PEI_BLOCK_IO_MEDIA     Media;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL || MediaInfo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiGetBlockDeviceMediaInfo (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             DeviceIndex,
+             &Media
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get media info from AtapiBlkIoDev
+  //
+  CopyMem (MediaInfo, &AtapiBlkIoDev->DeviceInfo[DeviceIndex - 1].MediaInfo2, sizeof(EFI_PEI_BLOCK_IO2_MEDIA));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  )
+{
+  EFI_STATUS          Status;
+  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev;
+
+  AtapiBlkIoDev = NULL;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AtapiBlkIoDev = PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS (This);
+
+  Status = AtapiReadBlocks (
+             PeiServices,
+             &AtapiBlkIoDev->AtapiBlkIo,
+             DeviceIndex,
+             StartLBA,
+             BufferSize,
+             Buffer
+             );
+
+  return Status;
+}
+
+
+/**
+  Enumerate Atapi devices.
+
+  This function is used to enumerate Atatpi device in Ide channel.
+
+  @param[in]  AtapiBlkIoDev  A pointer to atapi block IO device
+
+**/
+VOID
+AtapiEnumerateDevices (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev
+  )
+{
+  UINT8                   Index1;
+  UINT8                   Index2;
+  UINTN                   DevicePosition;
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_PEI_BLOCK_IO2_MEDIA MediaInfo2;
+  EFI_STATUS              Status;
+  UINTN                   DeviceCount;
+  UINT16                  CommandBlockBaseAddr;
+  UINT16                  ControlBlockBaseAddr;
+  UINT32                  IdeEnabledNumber;
+  IDE_REGS_BASE_ADDR      IdeRegsBaseAddr[MAX_IDE_CHANNELS];
+
+  DeviceCount = 0;
+  DevicePosition = 0;
+
+  //
+  // Scan IDE bus for ATAPI devices
+  //
+
+  //
+  // Enable Sata and IDE controller.
+  //
+  AtapiBlkIoDev->AtaControllerPpi->EnableAtaChannel (
+                                  (EFI_PEI_SERVICES **) GetPeiServicesTablePointer(),
+                                  AtapiBlkIoDev->AtaControllerPpi,
+                                  PEI_ICH_IDE_PRIMARY | PEI_ICH_IDE_SECONDARY
+                                  );
+
+  //
+  // Allow SATA Devices to spin-up. This is needed if 
+  // SEC and PEI phase is too short, for example Release Build.
+  //
+  DEBUG ((EFI_D_INFO, "Delay for %d seconds for SATA devices to spin-up\n", PcdGet16 (PcdSataSpinUpDelayInSecForRecoveryPath)));
+  MicroSecondDelay (PcdGet16 (PcdSataSpinUpDelayInSecForRecoveryPath) * 1000 * 1000); //
+
+  //
+  // Get four channels (primary or secondary Pata, Sata Channel) Command and Control Regs Base address.
+  //
+  IdeEnabledNumber = AtapiBlkIoDev->AtaControllerPpi->GetIdeRegsBaseAddr (
+                                                      (EFI_PEI_SERVICES **) GetPeiServicesTablePointer(),
+                                                      AtapiBlkIoDev->AtaControllerPpi,
+                                                      IdeRegsBaseAddr
+                                                      );
+
+  //
+  // Using Command and Control Regs Base Address to fill other registers.
+  //
+  for (Index1 = 0; Index1 < IdeEnabledNumber; Index1 ++) { 
+    CommandBlockBaseAddr               = IdeRegsBaseAddr[Index1].CommandBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Data         = CommandBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Reg1.Feature = (UINT16) (CommandBlockBaseAddr + 0x1);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].SectorCount  = (UINT16) (CommandBlockBaseAddr + 0x2);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].SectorNumber = (UINT16) (CommandBlockBaseAddr + 0x3);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].CylinderLsb  = (UINT16) (CommandBlockBaseAddr + 0x4);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].CylinderMsb  = (UINT16) (CommandBlockBaseAddr + 0x5);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Head         = (UINT16) (CommandBlockBaseAddr + 0x6);
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Reg.Command  = (UINT16) (CommandBlockBaseAddr + 0x7);
+
+    ControlBlockBaseAddr                = IdeRegsBaseAddr[Index1].ControlBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].Alt.DeviceControl = ControlBlockBaseAddr;
+    AtapiBlkIoDev->IdeIoPortReg[Index1].DriveAddress      = (UINT16) (ControlBlockBaseAddr + 0x1);
+    
+    //
+    // Scan IDE bus for ATAPI devices IDE or Sata device
+    //
+    for (Index2 = IdeMaster; Index2 < IdeMaxDevice; Index2++) {
+      //
+      // Pata & Sata, Primary & Secondary channel, Master & Slave device
+      //
+      DevicePosition = (UINTN) (Index1 * 2 + Index2);
+
+      if (DiscoverAtapiDevice (AtapiBlkIoDev, DevicePosition, &MediaInfo, &MediaInfo2)) {
+        //
+        // ATAPI Device at DevicePosition is found.
+        //
+        AtapiBlkIoDev->DeviceInfo[DeviceCount].DevicePosition = DevicePosition;
+        //
+        // Retrieve Media Info
+        //
+        Status  = DetectMedia (AtapiBlkIoDev, DevicePosition, &MediaInfo, &MediaInfo2);
+        CopyMem (&(AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo), &MediaInfo, sizeof (MediaInfo));
+        CopyMem (&(AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2), &MediaInfo2, sizeof (MediaInfo2));
+
+        DEBUG ((EFI_D_INFO, "Atatpi Device Position is %d\n", DevicePosition));
+        DEBUG ((EFI_D_INFO, "Atatpi DeviceType is   %d\n", MediaInfo.DeviceType));
+        DEBUG ((EFI_D_INFO, "Atatpi MediaPresent is %d\n", MediaInfo.MediaPresent));
+        DEBUG ((EFI_D_INFO, "Atatpi BlockSize is  0x%x\n", MediaInfo.BlockSize));
+
+        if (EFI_ERROR (Status)) {
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo.MediaPresent = FALSE;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo.LastBlock    = 0;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2.MediaPresent = FALSE;
+          AtapiBlkIoDev->DeviceInfo[DeviceCount].MediaInfo2.LastBlock    = 0;
+        }
+        DeviceCount += 1;
+      }
+    }
+  }
+
+  AtapiBlkIoDev->DeviceCount = DeviceCount;
+}
+
+/**
+  Detect Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval TRUE                Atapi device exists in specified position.
+  @retval FALSE               Atapi device does not exist in specified position.
+
+**/
+BOOLEAN
+DiscoverAtapiDevice (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  )
+{
+  EFI_STATUS  Status;
+
+  if (!DetectIDEController (AtapiBlkIoDev, DevicePosition)) {
+    return FALSE;
+  }
+  //
+  // test if it is an ATAPI device (only supported device)
+  //
+  if (ATAPIIdentify (AtapiBlkIoDev, DevicePosition) == EFI_SUCCESS) {
+
+    Status = Inquiry (AtapiBlkIoDev, DevicePosition, MediaInfo, MediaInfo2);
+    if (!EFI_ERROR (Status)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check power mode of Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  AtaCommand      The Ata Command passed in.
+
+  @retval EFI_SUCCESS         The Atapi device support power mode.
+  @retval EFI_NOT_FOUND       The Atapi device not found.
+  @retval EFI_TIMEOUT         Atapi command transaction is time out.
+  @retval EFI_ABORTED         Atapi command abort.
+
+**/
+EFI_STATUS
+CheckPowerMode (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  UINT8               AtaCommand
+  )
+{
+  UINT8       Channel;
+  UINT8       Device;
+  UINT16      StatusRegister;
+  UINT16      HeadRegister;
+  UINT16      CommandRegister;
+  UINT16      ErrorRegister;
+  UINT16      SectorCountRegister;
+  EFI_STATUS  Status;
+  UINT8       StatusValue;
+  UINT8       ErrorValue;
+  UINT8       SectorCountValue;
+
+  Channel             = (UINT8) (DevicePosition / 2);
+  Device              = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusRegister      = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadRegister        = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandRegister     = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  ErrorRegister       = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg1.Error;
+  SectorCountRegister = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorCount;
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+  //
+  // refresh the SectorCount register
+  //
+  SectorCountValue = 0x55;
+  IoWrite8 (SectorCountRegister, SectorCountValue);
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+  Status = DRDYReady (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 100);
+
+  //
+  // select device
+  //
+  IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+  //
+  // send 'check power' commandd via Command Register
+  //
+  IoWrite8 (CommandRegister, AtaCommand);
+
+  Status = WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 3000);
+  if (EFI_ERROR (Status)) {
+    return EFI_TIMEOUT;
+  }
+
+  StatusValue = IoRead8 (StatusRegister);
+
+  //
+  // command returned status is DRDY, indicating device supports the command,
+  // so device is present.
+  //
+  if ((StatusValue & ATA_STSREG_DRDY) == ATA_STSREG_DRDY) {
+    return EFI_SUCCESS;
+  }
+
+  SectorCountValue = IoRead8 (SectorCountRegister);
+
+  //
+  // command returned status is ERR & ABRT_ERR, indicating device does not support
+  // the command, so device is present.
+  //
+  if ((StatusValue & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+    ErrorValue = IoRead8 (ErrorRegister);
+    if ((ErrorValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+      return EFI_ABORTED;
+    } else {
+      //
+      // According to spec, no other error code is valid
+      //
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  if ((SectorCountValue == 0x00) || (SectorCountValue == 0x80) || (SectorCountValue == 0xff)) {
+    //
+    // Write SectorCount 0x55 but return valid state value. Maybe no device
+    // exists or some slow kind of ATAPI device exists.
+    //
+    IoWrite8 (HeadRegister, (UINT8) ((Device << 4) | 0xe0));
+
+    //
+    // write 0x55 and 0xaa to SectorCounter register,
+    // if the data could be written into the register,
+    // indicating the device is present, otherwise the device is not present.
+    //
+    SectorCountValue = 0x55;
+    IoWrite8 (SectorCountRegister, SectorCountValue);
+    MicroSecondDelay (10000);
+
+    SectorCountValue = IoRead8 (SectorCountRegister);
+    if (SectorCountValue != 0x55) {
+      return EFI_NOT_FOUND;
+    }
+    //
+    // Send a "ATAPI TEST UNIT READY" command ... slow but accurate
+    //
+    Status = TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    return Status;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Detect if an IDE controller exists in specified position.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+
+  @retval TRUE         The Atapi device exists.
+  @retval FALSE        The Atapi device does not present.
+
+**/
+BOOLEAN
+DetectIDEController (
+  IN  ATAPI_BLK_IO_DEV   *AtapiBlkIoDev,
+  IN  UINTN              DevicePosition
+  )
+{
+  UINT8       Channel;
+  EFI_STATUS  Status;
+  UINT8       AtaCommand;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+  //
+  //  Wait 31 seconds for BSY clear
+  //
+  Status = WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Send 'check power' command for IDE device
+  //
+  AtaCommand  = 0xE5;
+  Status      = CheckPowerMode (AtapiBlkIoDev, DevicePosition, AtaCommand);
+  if ((Status == EFI_ABORTED) || (Status == EFI_SUCCESS)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Wait specified time interval to poll for BSY bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        BSY bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        BSY bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+WaitForBSYClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    StatusValue = IoRead8 (StatusRegister);
+    if ((StatusValue & ATA_STSREG_BSY) == 0x00) {
+      break;
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRDY bit set in the Status register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRDY bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRDY bit is not set in the specified time interval.
+
+**/
+EFI_STATUS
+DRDYReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    StatusValue = IoRead8 (StatusRegister);
+    //
+    //  BSY == 0 , DRDY == 1
+    //
+    if ((StatusValue & (ATA_STSREG_DRDY | ATA_STSREG_BSY)) == ATA_STSREG_DRDY) {
+      break;
+    }
+
+  if ((StatusValue & (ATA_STSREG_ERR | ATA_STSREG_BSY)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    StatusValue = IoRead8 (StatusRegister);
+
+    //
+    // wait for BSY == 0 and DRQ == 0
+    //
+    if ((StatusValue & (ATA_STSREG_DRQ | ATA_STSREG_BSY)) == 0) {
+      break;
+    }
+
+  if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Alternate Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  AltStatusRegister;
+  UINT8   AltStatusValue;
+  UINT8   ErrValue;
+
+  AltStatusValue    = 0;
+
+  AltStatusRegister = IdeIoRegisters->Alt.AltStatus;
+
+  Delay             = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    AltStatusValue = IoRead8 (AltStatusRegister);
+
+    //
+    // wait for BSY == 0 and DRQ == 0
+    //
+    if ((AltStatusValue & (ATA_STSREG_DRQ | ATA_STSREG_BSY)) == 0) {
+      break;
+    }
+
+  if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+    ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+    if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+    return EFI_ABORTED;
+    }
+  }
+  
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  StatusRegister;
+  UINT8   StatusValue;
+  UINT8   ErrValue;
+
+  StatusValue     = 0;
+  ErrValue        = 0;
+
+  StatusRegister  = IdeIoRegisters->Reg.Status;
+
+  Delay           = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+    //
+    //  read Status Register will clear interrupt
+    //
+    StatusValue = IoRead8 (StatusRegister);
+
+    //
+    //  BSY==0,DRQ==1
+    //
+    if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      break;
+    }
+
+    if ((StatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+
+      ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+      if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+        return EFI_ABORTED;
+      }
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Alternate Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  )
+{
+  UINTN   Delay;
+  UINT16  AltStatusRegister;
+  UINT8   AltStatusValue;
+  UINT8   ErrValue;
+
+  AltStatusValue    = 0;
+
+  AltStatusRegister = IdeIoRegisters->Alt.AltStatus;
+
+  Delay             = ((TimeoutInMilliSeconds * STALL_1_MILLI_SECOND) / 250) + 1;
+  do {
+
+    AltStatusValue = IoRead8 (AltStatusRegister);
+
+    //
+    //  BSY==0,DRQ==1
+    //
+    if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_DRQ)) == ATA_STSREG_DRQ) {
+      break;
+    }
+
+    if ((AltStatusValue & (ATA_STSREG_BSY | ATA_STSREG_ERR)) == ATA_STSREG_ERR) {
+
+      ErrValue = IoRead8 (IdeIoRegisters->Reg1.Error);
+      if ((ErrValue & ATA_ERRREG_ABRT) == ATA_ERRREG_ABRT) {
+        return EFI_ABORTED;
+      }
+    }
+    MicroSecondDelay (250);
+
+    Delay--;
+  } while (Delay != 0);
+
+  if (Delay == 0) {
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if there is an error in Status Register.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  StatusReg         The address to IDE IO registers.
+
+  @retval EFI_SUCCESS        Operation success.
+  @retval EFI_DEVICE_ERROR   Device error.
+
+**/
+EFI_STATUS
+CheckErrorStatus (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINT16              StatusReg
+  )
+{
+  UINT8 StatusValue;
+
+  StatusValue = IoRead8 (StatusReg);
+
+  if ((StatusValue & (ATA_STSREG_ERR | ATA_STSREG_DWF | ATA_STSREG_CORR)) == 0) {
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_DEVICE_ERROR;
+
+}
+
+/**
+  Idendify Atapi devices.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        Identify successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be identified successfully.
+
+**/
+EFI_STATUS
+ATAPIIdentify (
+  IN  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev,
+  IN  UINTN                   DevicePosition
+  )
+{
+  ATAPI_IDENTIFY_DATA  AtapiIdentifyData;
+  UINT8                Channel;
+  UINT8                Device;
+  UINT16               StatusReg;
+  UINT16               HeadReg;
+  UINT16               CommandReg;
+  UINT16               DataReg;
+  UINT16               SectorCountReg;
+  UINT16               SectorNumberReg;
+  UINT16               CylinderLsbReg;
+  UINT16               CylinderMsbReg;
+
+  UINT32               WordCount;
+  UINT32               Increment;
+  UINT32               Index;
+  UINT32               ByteCount;
+  UINT16               *Buffer16;
+
+  EFI_STATUS           Status;
+
+  ByteCount       = sizeof (AtapiIdentifyData);
+  Buffer16        = (UINT16 *) &AtapiIdentifyData;
+
+  Channel         = (UINT8) (DevicePosition / 2);
+  Device          = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusReg       = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandReg      = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  DataReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Data;
+  SectorCountReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorCount;
+  SectorNumberReg = AtapiBlkIoDev->IdeIoPortReg[Channel].SectorNumber;
+  CylinderLsbReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderLsb;
+  CylinderMsbReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderMsb;
+
+  //
+  // Send ATAPI Identify Command to get IDENTIFY data.
+  //
+  if (WaitForBSYClear (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // select device via Head/Device register.
+  // Before write Head/Device register, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), ATATIMEOUT) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  //  e0:1110,0000-- bit7 and bit5 are reserved bits.
+  //           bit6 set means LBA mode
+  //
+  IoWrite8 (HeadReg, (UINT8) ((Device << 4) | 0xe0));
+
+  //
+  // set all the command parameters
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  IoWrite8 (SectorCountReg, 0);
+  IoWrite8 (SectorNumberReg, 0);
+  IoWrite8 (CylinderLsbReg, 0);
+  IoWrite8 (CylinderMsbReg, 0);
+
+  //
+  // send command via Command Register
+  //
+  IoWrite8 (CommandReg, ATA_CMD_IDENTIFY_DEVICE);
+
+  //
+  // According to PIO data in protocol, host can perform a series of reads to the
+  // data register after each time device set DRQ ready;
+  // The data size of "a series of read" is command specific.
+  // For most ATA command, data size received from device will not exceed 1 sector,
+  // hense the data size for "a series of read" can be the whole data size of one command request.
+  // For ATA command such as Read Sector command, whole data size of one ATA command request is often larger
+  // than 1 sector, according to the Read Sector command, the data size of "a series of read" is exactly
+  // 1 sector.
+  // Here for simplification reason, we specify the data size for "a series of read" to
+  // 1 sector (256 words) if whole data size of one ATA commmand request is larger than 256 words.
+  //
+  Increment = 256;
+  //
+  // 256 words
+  //
+  WordCount = 0;
+  //
+  // WordCount is used to record bytes of currently transfered data
+  //
+  while (WordCount < ByteCount / 2) {
+    //
+    // Poll DRQ bit set, data transfer can be performed only when DRQ is ready.
+    //
+    Status = DRQReady2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), ATATIMEOUT);
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    if (CheckErrorStatus (AtapiBlkIoDev, StatusReg) != EFI_SUCCESS) {
+
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // Get the byte count for one series of read
+    //
+    if ((WordCount + Increment) > ByteCount / 2) {
+      Increment = ByteCount / 2 - WordCount;
+    }
+    //
+    // perform a series of read without check DRQ ready
+    //
+    for (Index = 0; Index < Increment; Index++) {
+      *Buffer16++ = IoRead16 (DataReg);
+    }
+
+    WordCount += Increment;
+
+  }
+  //
+  // while
+  //
+  if (DRQClear (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+TestUnitReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.TestUnitReady.opcode = ATA_CMD_TEST_UNIT_READY;
+
+  //
+  // send command packet
+  //
+  Status = AtapiPacketCommandIn (AtapiBlkIoDev, DevicePosition, &Packet, NULL, 0, ATAPITIMEOUT);
+  return Status;
+}
+
+/**
+  Send out ATAPI commands conforms to the Packet Command with PIO Data In Protocol.
+  
+  @param[in]  AtapiBlkIoDev         A pointer to atapi block IO device.
+  @param[in]  DevicePosition        An integer to signify device position.
+  @param[in]  Packet                A pointer to ATAPI command packet.
+  @param[in]  Buffer                Buffer to contain requested transfer data from device.
+  @param[in]  ByteCount             Requested transfer data length.
+  @param[in]  TimeoutInMilliSeconds Time out value, in unit of milliseconds.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+
+**/
+EFI_STATUS
+AtapiPacketCommandIn (
+  IN  ATAPI_BLK_IO_DEV      *AtapiBlkIoDev,
+  IN  UINTN                 DevicePosition,
+  IN  ATAPI_PACKET_COMMAND  *Packet,
+  IN  UINT16                *Buffer,
+  IN  UINT32                ByteCount,
+  IN  UINTN                 TimeoutInMilliSeconds
+  )
+{
+  UINT8       Channel;
+  UINT8       Device;
+  UINT16      StatusReg;
+  UINT16      HeadReg;
+  UINT16      CommandReg;
+  UINT16      FeatureReg;
+  UINT16      CylinderLsbReg;
+  UINT16      CylinderMsbReg;
+  UINT16      DeviceControlReg;
+  UINT16      DataReg;
+  EFI_STATUS  Status;
+  UINT32      Count;
+  UINT16      *CommandIndex;
+  UINT16      *PtrBuffer;
+  UINT32      Index;
+  UINT8       StatusValue;
+  UINT32      WordCount;
+
+  //
+  // required transfer data in word unit.
+  //
+  UINT32      RequiredWordCount;
+
+  //
+  // actual transfer data in word unit.
+  //
+  UINT32      ActualWordCount;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+  Device            = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  StatusReg         = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Status;
+  HeadReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+  CommandReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  FeatureReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg1.Feature;
+  CylinderLsbReg    = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderLsb;
+  CylinderMsbReg    = AtapiBlkIoDev->IdeIoPortReg[Channel].CylinderMsb;
+  DeviceControlReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].Alt.DeviceControl;
+  DataReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Data;
+
+  //
+  // Set all the command parameters by fill related registers.
+  // Before write to all the following registers, BSY and DRQ must be 0.
+  //
+  if (DRQClear2 (
+        AtapiBlkIoDev,
+        &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+        ATATIMEOUT
+        ) != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Select device via Device/Head Register.
+  // DEFAULT_CMD: 0xa0 (1010,0000)
+  //
+  IoWrite8 (HeadReg, (UINT8) ((Device << 4) | ATA_DEFAULT_CMD));
+
+  //
+  // No OVL; No DMA
+  //
+  IoWrite8 (FeatureReg, 0x00);
+
+  //
+  // set the transfersize to MAX_ATAPI_BYTE_COUNT to let the device
+  // determine how many data should be transfered.
+  //
+  IoWrite8 (CylinderLsbReg, (UINT8) (ATAPI_MAX_BYTE_COUNT & 0x00ff));
+  IoWrite8 (CylinderMsbReg, (UINT8) (ATAPI_MAX_BYTE_COUNT >> 8));
+
+  //
+  //  DEFAULT_CTL:0x0a (0000,1010)
+  //  Disable interrupt
+  //
+  IoWrite8 (DeviceControlReg, ATA_DEFAULT_CTL);
+
+  //
+  // Send Packet command to inform device
+  // that the following data bytes are command packet.
+  //
+  IoWrite8 (CommandReg, ATA_CMD_PACKET);
+
+  Status = DRQReady (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), TimeoutInMilliSeconds);
+  if (Status != EFI_SUCCESS) {
+    return Status;
+  }
+  //
+  // Send out command packet
+  //
+  CommandIndex = Packet->Data16;
+  for (Count = 0; Count < 6; Count++, CommandIndex++) {
+    IoWrite16 (DataReg, *CommandIndex);
+    MicroSecondDelay (10);
+  }
+
+  StatusValue = IoRead8 (StatusReg);
+  if ((StatusValue & ATA_STSREG_ERR) == ATA_STSREG_ERR) {
+    //
+    // Trouble! Something's wrong here... Wait some time and return. 3 second is
+    // supposed to be long enough for a device reset latency or error recovery
+    //
+    MicroSecondDelay (3000000);
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Buffer == NULL || ByteCount == 0) {
+    return EFI_SUCCESS;
+  }
+  //
+  // call PioReadWriteData() function to get
+  // requested transfer data form device.
+  //
+  PtrBuffer         = Buffer;
+  RequiredWordCount = ByteCount / 2;
+  //
+  // ActuralWordCount means the word count of data really transfered.
+  //
+  ActualWordCount = 0;
+
+  Status          = EFI_SUCCESS;
+  while ((Status == EFI_SUCCESS) && (ActualWordCount < RequiredWordCount)) {
+    //
+    // before each data transfer stream, the host should poll DRQ bit ready,
+    // which informs device is ready to transfer data.
+    //
+    if (DRQReady2 (
+          AtapiBlkIoDev,
+          &(AtapiBlkIoDev->IdeIoPortReg[Channel]),
+          TimeoutInMilliSeconds
+          ) != EFI_SUCCESS) {
+      return CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+    }
+    //
+    // read Status Register will clear interrupt
+    //
+    StatusValue = IoRead8 (StatusReg);
+
+    //
+    // get current data transfer size from Cylinder Registers.
+    //
+    WordCount = IoRead8 (CylinderMsbReg) << 8;
+    WordCount = WordCount | IoRead8 (CylinderLsbReg);
+    WordCount = WordCount & 0xffff;
+    WordCount /= 2;
+
+    //
+    // perform a series data In/Out.
+    //
+    for (Index = 0; (Index < WordCount) && (ActualWordCount < RequiredWordCount); Index++, ActualWordCount++) {
+
+      *PtrBuffer = IoRead16 (DataReg);
+
+      PtrBuffer++;
+
+    }
+
+    if (((ATAPI_REQUEST_SENSE_CMD *) Packet)->opcode == ATA_CMD_REQUEST_SENSE && ActualWordCount >= 4) {
+      RequiredWordCount = MIN (
+                            RequiredWordCount,
+                            (UINT32) (4 + (((ATAPI_REQUEST_SENSE_DATA *) Buffer)->addnl_sense_length / 2))
+                            );
+    }
+
+  }
+  //
+  // After data transfer is completed, normally, DRQ bit should clear.
+  //
+  Status = DRQClear2 (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), TimeoutInMilliSeconds);
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // read status register to check whether error happens.
+  //
+  Status = CheckErrorStatus (AtapiBlkIoDev, StatusReg);
+  return Status;
+}
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return INQUIRY data of the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+  @retval EFI_UNSUPPORTED    Unsupported device type.
+
+**/
+EFI_STATUS
+Inquiry (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  )
+{
+  ATAPI_PACKET_COMMAND        Packet;
+  EFI_STATUS                  Status;
+  ATAPI_INQUIRY_DATA          Idata;
+
+  //
+  // prepare command packet for the ATAPI Inquiry Packet Command.
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  ZeroMem (&Idata, sizeof (ATAPI_INQUIRY_DATA));
+
+  Packet.Inquiry.opcode             = ATA_CMD_INQUIRY;
+  Packet.Inquiry.page_code          = 0;
+  Packet.Inquiry.allocation_length  = (UINT8) sizeof (ATAPI_INQUIRY_DATA);
+
+  //
+  // Send command packet and get requested Inquiry data.
+  //
+  Status = AtapiPacketCommandIn (
+            AtapiBlkIoDev,
+            DevicePosition,
+            &Packet,
+            (UINT16 *) (&Idata),
+            sizeof (ATAPI_INQUIRY_DATA),
+            ATAPITIMEOUT
+            //50
+            );
+
+  if (Status != EFI_SUCCESS) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Identify device type via INQUIRY data.
+  //
+  switch (Idata.peripheral_type & 0x1f) {
+  case 0x00:
+    //
+    // Magnetic Disk
+    //
+    MediaInfo->DeviceType   = IdeLS120;
+    MediaInfo->MediaPresent = FALSE;
+    MediaInfo->LastBlock    = 0;
+    MediaInfo->BlockSize    = 0x200;
+    MediaInfo2->InterfaceType  = MSG_ATAPI_DP;
+    MediaInfo2->RemovableMedia = TRUE;
+    MediaInfo2->MediaPresent   = FALSE;
+    MediaInfo2->ReadOnly       = FALSE;
+    MediaInfo2->BlockSize      = 0x200;
+    MediaInfo2->LastBlock      = 0;
+    break;
+
+  case 0x05:
+    //
+    // CD-ROM
+    //
+    MediaInfo->DeviceType   = IdeCDROM;
+    MediaInfo->MediaPresent = FALSE;
+    MediaInfo->LastBlock    = 0;
+    MediaInfo->BlockSize    = 0x800;
+    MediaInfo2->InterfaceType  = MSG_ATAPI_DP;
+    MediaInfo2->RemovableMedia = TRUE;
+    MediaInfo2->MediaPresent   = FALSE;
+    MediaInfo2->ReadOnly       = TRUE;
+    MediaInfo2->BlockSize      = 0x200;
+    MediaInfo2->LastBlock      = 0;
+    break;
+
+  default:
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Used before read/write blocks from/to ATAPI device media. 
+  Since ATAPI device media is removable, it is necessary to detect
+  whether media is present and get current present media's information.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate required resources.
+
+**/
+EFI_STATUS
+DetectMedia (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  )
+{
+
+  UINTN                     Index;
+  UINTN                     RetryNum;
+  UINTN                     MaxRetryNum;
+  ATAPI_REQUEST_SENSE_DATA  *SenseBuffers;
+  BOOLEAN                   NeedReadCapacity;
+  BOOLEAN                   NeedRetry;
+  EFI_STATUS                Status;
+  UINT8                     SenseCounts;
+
+  SenseBuffers = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (*SenseBuffers)));
+  if (SenseBuffers == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Test Unit Ready command is used to detect whether device is accessible,
+  // the device will produce corresponding Sense data.
+  //
+  for (Index = 0; Index < 2; Index++) {
+
+    Status = TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    if (Status != EFI_SUCCESS) {
+      Status = ResetDevice (AtapiBlkIoDev, DevicePosition, FALSE);
+
+      if (Status != EFI_SUCCESS) {
+        ResetDevice (AtapiBlkIoDev, DevicePosition, TRUE);
+      }
+
+    } else {
+      break;
+    }
+  }
+
+  SenseCounts       = MAX_SENSE_KEY_COUNT;
+  Status            = EFI_SUCCESS;
+  NeedReadCapacity  = TRUE;
+
+  for (Index = 0; Index < 5; Index++) {
+    SenseCounts = MAX_SENSE_KEY_COUNT;
+    Status = RequestSense (
+              AtapiBlkIoDev,
+              DevicePosition,
+              SenseBuffers,
+              &SenseCounts
+              );
+    DEBUG ((EFI_D_INFO, "Atapi Request Sense Count is %d\n", SenseCounts));
+    if (IsDeviceStateUnclear (SenseBuffers, SenseCounts) || IsNoMedia (SenseBuffers, SenseCounts)) {
+      //
+      // We are not sure whether the media is present or not, try again
+      //
+      TestUnitReady (AtapiBlkIoDev, DevicePosition);
+    } else {
+      break;
+    }
+  }
+
+  if (Status == EFI_SUCCESS) {
+
+    if (IsNoMedia (SenseBuffers, SenseCounts)) {
+
+      NeedReadCapacity        = FALSE;
+      MediaInfo->MediaPresent = FALSE;
+      MediaInfo->LastBlock    = 0;
+      MediaInfo2->MediaPresent = FALSE;
+      MediaInfo2->LastBlock    = 0;
+    }
+
+    if (IsMediaError (SenseBuffers, SenseCounts)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  if (NeedReadCapacity) {
+    //
+    // at most retry 5 times
+    //
+    MaxRetryNum = 5;
+    RetryNum    = 1;
+    //
+    // initial retry once
+    //
+    for (Index = 0; (Index < RetryNum) && (Index < MaxRetryNum); Index++) {
+
+      Status = ReadCapacity (AtapiBlkIoDev, DevicePosition, MediaInfo, MediaInfo2);
+      MicroSecondDelay (200000);
+      SenseCounts = MAX_SENSE_KEY_COUNT;
+
+      if (Status != EFI_SUCCESS) {
+
+        Status = RequestSense (AtapiBlkIoDev, DevicePosition, SenseBuffers, &SenseCounts);
+        //
+        // If Request Sense data failed, reset the device and retry.
+        //
+        if (Status != EFI_SUCCESS) {
+
+          Status = ResetDevice (AtapiBlkIoDev, DevicePosition, FALSE);
+          //
+          // if ATAPI soft reset fail,
+          // use stronger reset mechanism -- ATA soft reset.
+          //
+          if (Status != EFI_SUCCESS) {
+            ResetDevice (AtapiBlkIoDev, DevicePosition, TRUE);
+          }
+
+          RetryNum++;
+          //
+          // retry once more
+          //
+          continue;
+        }
+        //
+        // No Media
+        //
+        if (IsNoMedia (SenseBuffers, SenseCounts)) {
+
+          MediaInfo->MediaPresent = FALSE;
+          MediaInfo->LastBlock    = 0;
+          MediaInfo2->MediaPresent = FALSE;
+          MediaInfo2->LastBlock    = 0;
+          break;
+        }
+
+        if (IsMediaError (SenseBuffers, SenseCounts)) {
+          return EFI_DEVICE_ERROR;
+        }
+
+        if (!IsDriveReady (SenseBuffers, SenseCounts, &NeedRetry)) {
+          //
+          // Drive not ready: if NeedRetry, then retry once more;
+          // else return error
+          //
+          if (NeedRetry) {
+            RetryNum++;
+            continue;
+          } else {
+            return EFI_DEVICE_ERROR;
+          }
+        }
+        //
+        // if read capacity fail not for above reasons, retry once more
+        //
+        RetryNum++;
+
+      }
+
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Reset specified Atapi device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in]  Extensive         If TRUE, use ATA soft reset, otherwise use Atapi soft reset.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ResetDevice (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev,
+  IN  UINTN             DevicePosition,
+  IN  BOOLEAN           Extensive
+  )
+{
+  UINT8   DevControl;
+  UINT8   Command;
+  UINT8   DeviceSelect;
+  UINT16  DeviceControlReg;
+  UINT16  CommandReg;
+  UINT16  HeadReg;
+  UINT8   Channel;
+  UINT8   Device;
+
+  Channel           = (UINT8) (DevicePosition / 2);
+  Device            = (UINT8) (DevicePosition % 2);
+
+  ASSERT (Channel < MAX_IDE_CHANNELS);
+
+  DeviceControlReg  = AtapiBlkIoDev->IdeIoPortReg[Channel].Alt.DeviceControl;
+  CommandReg        = AtapiBlkIoDev->IdeIoPortReg[Channel].Reg.Command;
+  HeadReg           = AtapiBlkIoDev->IdeIoPortReg[Channel].Head;
+
+  if (Extensive) {
+
+    DevControl = 0;
+    DevControl |= ATA_CTLREG_SRST;
+    //
+    // set SRST bit to initiate soft reset
+    //
+    DevControl |= BIT1;
+    //
+    // disable Interrupt
+    //
+    IoWrite8 (DeviceControlReg, DevControl);
+
+    //
+    // Wait 10us
+    //
+    MicroSecondDelay (10);
+
+    //
+    // Clear SRST bit
+    //
+    DevControl &= 0xfb;
+    //
+    // 0xfb:1111,1011
+    //
+    IoWrite8 (DeviceControlReg, DevControl);
+
+    //
+    // slave device needs at most 31s to clear BSY
+    //
+    if (WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000) == EFI_TIMEOUT) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } else {
+    //
+    // for ATAPI device, no need to wait DRDY ready after device selecting.
+    // bit7 and bit5 are both set to 1 for backward compatibility
+    //
+    DeviceSelect = (UINT8) (((BIT7 | BIT5) | (Device << 4)));
+    IoWrite8 (HeadReg, DeviceSelect);
+
+    Command = ATA_CMD_SOFT_RESET;
+    IoWrite8 (CommandReg, Command);
+
+    //
+    // BSY cleared is the only status return to the host by the device when reset is completed
+    // slave device needs at most 31s to clear BSY
+    //
+    if (WaitForBSYClear (AtapiBlkIoDev, &(AtapiBlkIoDev->IdeIoPortReg[Channel]), 31000) != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // stall 5 seconds to make the device status stable
+    //
+    MicroSecondDelay (STALL_1_SECONDS * 5);
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**  
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param[in]      AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]      DevicePosition  An integer to signify device position.
+  @param[in]      SenseBuffers    Pointer to sense buffer.
+  @param[in, out] SenseCounts     Length of sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+RequestSense (
+  IN  ATAPI_BLK_IO_DEV           *AtapiBlkIoDev,
+  IN  UINTN                      DevicePosition,
+  IN  ATAPI_REQUEST_SENSE_DATA   *SenseBuffers,
+  IN  OUT  UINT8                 *SenseCounts
+  )
+{
+  EFI_STATUS            Status;
+  ATAPI_REQUEST_SENSE_DATA    *Sense;
+  UINT16                *Ptr;
+  BOOLEAN               SenseReq;
+  ATAPI_PACKET_COMMAND  Packet;
+
+  ZeroMem (SenseBuffers, sizeof (ATAPI_REQUEST_SENSE_DATA) * (*SenseCounts));
+  //
+  // fill command packet for Request Sense Packet Command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.RequestSence.opcode            = ATA_CMD_REQUEST_SENSE;
+  Packet.RequestSence.allocation_length = (UINT8) sizeof (ATAPI_REQUEST_SENSE_DATA);
+
+  Ptr = (UINT16 *) SenseBuffers;
+  //
+  // initialize pointer
+  //
+  *SenseCounts = 0;
+  //
+  //  request sense data from device continiously until no sense data exists in the device.
+  //
+  for (SenseReq = TRUE; SenseReq;) {
+
+    Sense = (ATAPI_REQUEST_SENSE_DATA *) Ptr;
+
+    //
+    // send out Request Sense Packet Command and get one Sense data form device
+    //
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              Ptr,
+              sizeof (ATAPI_REQUEST_SENSE_DATA),
+              ATAPITIMEOUT
+              );
+    //
+    // failed to get Sense data
+    //
+    if (Status != EFI_SUCCESS) {
+      if (*SenseCounts == 0) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    (*SenseCounts)++;
+
+    if (*SenseCounts > MAX_SENSE_KEY_COUNT) {
+      return EFI_SUCCESS;
+    }
+    //
+    // We limit MAX sense data count to 20 in order to avoid dead loop. Some
+    // incompatible ATAPI devices don't retrive NO_SENSE when there is no media.
+    // In this case, dead loop occurs if we don't have a gatekeeper. 20 is
+    // supposed to be large enough for any ATAPI device.
+    //
+    if ((Sense->sense_key != ATA_SK_NO_SENSE) && ((*SenseCounts) < 20)) {
+
+      Ptr += sizeof (ATAPI_REQUEST_SENSE_DATA) / 2;
+      //
+      // Ptr is word based pointer
+      //
+    } else {
+      //
+      // when no sense key, skip out the loop
+      //
+      SenseReq = FALSE;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**  
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadCapacity (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  )
+{
+  EFI_STATUS                Status;
+  ATAPI_PACKET_COMMAND      Packet;
+
+  //
+  // used for capacity data returned from ATAPI device
+  //
+  ATAPI_READ_CAPACITY_DATA        Data;
+  ATAPI_READ_FORMAT_CAPACITY_DATA FormatData;
+
+  ZeroMem (&Data, sizeof (Data));
+  ZeroMem (&FormatData, sizeof (FormatData));
+
+  if (MediaInfo->DeviceType == IdeCDROM) {
+
+    ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+    Packet.Inquiry.opcode = ATA_CMD_READ_CAPACITY;
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) (&Data),
+              sizeof (ATAPI_READ_CAPACITY_DATA),
+              ATAPITIMEOUT
+              );
+
+  } else {
+    //
+    // DeviceType == IdeLS120
+    //
+    ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+    Packet.ReadFormatCapacity.opcode                = ATA_CMD_READ_FORMAT_CAPACITY;
+    Packet.ReadFormatCapacity.allocation_length_lo  = 12;
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) (&FormatData),
+              sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA),
+              ATAPITIMEOUT*10
+              );
+  }
+
+  if (Status == EFI_SUCCESS) {
+
+    if (MediaInfo->DeviceType == IdeCDROM) {
+
+      MediaInfo->LastBlock    = (Data.LastLba3 << 24) | (Data.LastLba2 << 16) | (Data.LastLba1 << 8) | Data.LastLba0;
+      MediaInfo->MediaPresent = TRUE;
+      //
+      // Because the user data portion in the sector of the Data CD supported
+      // is always 800h
+      //
+      MediaInfo->BlockSize     = 0x800;
+
+      MediaInfo2->LastBlock    = MediaInfo->LastBlock;
+      MediaInfo2->MediaPresent = MediaInfo->MediaPresent;
+      MediaInfo2->BlockSize    = (UINT32)MediaInfo->BlockSize;
+    }
+
+    if (MediaInfo->DeviceType == IdeLS120) {
+
+      if (FormatData.DesCode == 3) {
+        MediaInfo->MediaPresent = FALSE;
+        MediaInfo->LastBlock    = 0;
+        MediaInfo2->MediaPresent = FALSE;
+        MediaInfo2->LastBlock    = 0;
+      } else {
+        MediaInfo->LastBlock = (FormatData.LastLba3 << 24) |
+          (FormatData.LastLba2 << 16) |
+          (FormatData.LastLba1 << 8) |
+          FormatData.LastLba0;
+        MediaInfo->LastBlock--;
+
+        MediaInfo->MediaPresent = TRUE;
+
+        MediaInfo->BlockSize    = 0x200;
+
+        MediaInfo2->LastBlock    = MediaInfo->LastBlock;
+        MediaInfo2->MediaPresent = MediaInfo->MediaPresent;
+        MediaInfo2->BlockSize    = (UINT32)MediaInfo->BlockSize;
+
+      }
+    }
+
+    return EFI_SUCCESS;
+
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+}
+
+/**  
+  Perform read from disk in block unit.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  Buffer          Buffer to contain read data.
+  @param[in]  StartLba        Starting LBA address.
+  @param[in]  NumberOfBlocks  Number of blocks to read.
+  @param[in]  BlockSize       Size of each block.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  VOID                *Buffer,
+  IN  EFI_PEI_LBA         StartLba,
+  IN  UINTN               NumberOfBlocks,
+  IN  UINTN               BlockSize
+  )
+{
+
+  ATAPI_PACKET_COMMAND  Packet;
+  ATAPI_READ10_CMD      *Read10Packet;
+  EFI_STATUS            Status;
+  UINTN                 BlocksRemaining;
+  UINT32                Lba32;
+  UINT32                ByteCount;
+  UINT16                SectorCount;
+  VOID                  *PtrBuffer;
+  UINT16                MaxBlock;
+
+  //
+  // fill command packet for Read(10) command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Read10Packet  = &Packet.Read10;
+  Lba32         = (UINT32) StartLba;
+  PtrBuffer     = Buffer;
+
+  //
+  // limit the data bytes that can be transfered by one Read(10) Command
+  //
+  MaxBlock = (UINT16) (0x10000 / BlockSize);
+  //
+  // (64k bytes)
+  //
+  BlocksRemaining = NumberOfBlocks;
+
+  Status          = EFI_SUCCESS;
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      SectorCount = (UINT16) BlocksRemaining;
+    } else {
+      SectorCount = MaxBlock;
+    }
+    //
+    // fill the Packet data sturcture
+    //
+    Read10Packet->opcode = ATA_CMD_READ_10;
+
+    //
+    // Lba0 ~ Lba3 specify the start logical block address of the data transfer.
+    // Lba0 is MSB, Lba3 is LSB
+    //
+    Read10Packet->Lba3  = (UINT8) (Lba32 & 0xff);
+    Read10Packet->Lba2  = (UINT8) (Lba32 >> 8);
+    Read10Packet->Lba1  = (UINT8) (Lba32 >> 16);
+    Read10Packet->Lba0  = (UINT8) (Lba32 >> 24);
+
+    //
+    // TranLen0 ~ TranLen1 specify the transfer length in block unit.
+    // TranLen0 is MSB, TranLen is LSB
+    //
+    Read10Packet->TranLen1  = (UINT8) (SectorCount & 0xff);
+    Read10Packet->TranLen0  = (UINT8) (SectorCount >> 8);
+
+    ByteCount               = (UINT32) (SectorCount * BlockSize);
+
+    Status = AtapiPacketCommandIn (
+              AtapiBlkIoDev,
+              DevicePosition,
+              &Packet,
+              (UINT16 *) PtrBuffer,
+              ByteCount,
+              ATAPILONGTIMEOUT
+              );
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    Lba32 += SectorCount;
+    PtrBuffer = (UINT8 *) PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return Status;
+}
+
+/**  
+  Check if there is media according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseData,
+  IN  UINTN                     SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsNoMedia;
+
+  IsNoMedia = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    if ((SensePtr->sense_key == ATA_SK_NOT_READY) && (SensePtr->addnl_sense_code == ATA_ASC_NO_MEDIA)) {
+      IsNoMedia = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsNoMedia;
+}
+
+/**  
+  Check if device state is unclear according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Device state is unclear
+  @retval FALSE   Device state is clear  
+
+**/
+BOOLEAN
+IsDeviceStateUnclear (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   Unclear;
+
+  Unclear  = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    if (SensePtr->sense_key == 0x06) {
+      //
+      // Sense key is 0x06 means the device is just be reset or media just
+      // changed. The current state of the device is unclear.
+      //
+      Unclear = TRUE;
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return Unclear;
+}
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseData,
+  IN  UINTN                     SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsError;
+
+  IsError   = FALSE;
+
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_MEDIUM_ERROR:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_ERR1:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR2:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR3:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR4:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_UPSIDE_DOWN:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+/**  
+  Check if drive is ready according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+  @param[out] NeedRetry   Indicate if retry is needed.
+
+  @retval TRUE    Drive ready
+  @retval FALSE   Drive not ready
+
+**/
+BOOLEAN
+IsDriveReady (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts,
+  OUT BOOLEAN                     *NeedRetry
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   IsReady;
+
+  IsReady     = TRUE;
+  *NeedRetry  = FALSE;
+
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_NOT_READY:
+        switch (SensePtr->addnl_sense_code_qualifier) {
+        case ATA_ASCQ_IN_PROGRESS:
+          IsReady     = FALSE;
+          *NeedRetry  = TRUE;
+          break;
+
+        default:
+          IsReady     = FALSE;
+          *NeedRetry  = FALSE;
+          break;
+        }
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsReady;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h
new file mode 100644
index 0000000000..20b35c4afd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/AtapiPeim.h
@@ -0,0 +1,789 @@
+/** @file
+Private Include file for IdeBus PEIM.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _RECOVERY_ATAPI_H_
+#define _RECOVERY_ATAPI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+#include <Ppi/AtaController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/IoLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+
+
+#include <IndustryStandard/Atapi.h>
+
+#define MAX_SENSE_KEY_COUNT 6
+#define MAX_IDE_CHANNELS    4  // Ide and Sata Primary, Secondary Channel.
+#define MAX_IDE_DEVICES     8  // Ide, Sata Primary, Secondary and Master, Slave device.
+
+typedef enum {
+  IdePrimary    = 0,
+  IdeSecondary  = 1,
+  IdeMaxChannel = 2
+} EFI_IDE_CHANNEL;
+
+typedef enum {
+  IdeMaster     = 0,
+  IdeSlave      = 1,
+  IdeMaxDevice  = 2
+} EFI_IDE_DEVICE;
+
+//
+// IDE Registers
+//
+typedef union {
+  UINT16  Command;        /* when write */
+  UINT16  Status;         /* when read */
+} IDE_CMD_OR_STATUS;
+
+typedef union {
+  UINT16  Error;          /* when read */
+  UINT16  Feature;        /* when write */
+} IDE_ERROR_OR_FEATURE;
+
+typedef union {
+  UINT16  AltStatus;      /* when read */
+  UINT16  DeviceControl;  /* when write */
+} IDE_ALTSTATUS_OR_DEVICECONTROL;
+
+//
+// IDE registers set
+//
+typedef struct {
+  UINT16                          Data;
+  IDE_ERROR_OR_FEATURE            Reg1;
+  UINT16                          SectorCount;
+  UINT16                          SectorNumber;
+  UINT16                          CylinderLsb;
+  UINT16                          CylinderMsb;
+  UINT16                          Head;
+  IDE_CMD_OR_STATUS               Reg;
+
+  IDE_ALTSTATUS_OR_DEVICECONTROL  Alt;
+  UINT16                          DriveAddress;
+} IDE_BASE_REGISTERS;
+
+typedef struct {
+
+  UINTN                   DevicePosition;
+  EFI_PEI_BLOCK_IO_MEDIA  MediaInfo;
+  EFI_PEI_BLOCK_IO2_MEDIA MediaInfo2;
+
+} PEI_ATAPI_DEVICE_INFO;
+
+#define ATAPI_BLK_IO_DEV_SIGNATURE  SIGNATURE_32 ('a', 'b', 'i', 'o')
+typedef struct {
+  UINTN                           Signature;
+
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   AtapiBlkIo;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  AtapiBlkIo2;
+  EFI_PEI_PPI_DESCRIPTOR          PpiDescriptor;
+  EFI_PEI_PPI_DESCRIPTOR          PpiDescriptor2;
+  PEI_ATA_CONTROLLER_PPI          *AtaControllerPpi;
+
+  UINTN                           DeviceCount;
+  PEI_ATAPI_DEVICE_INFO           DeviceInfo[MAX_IDE_DEVICES];   //for max 8 device
+  IDE_BASE_REGISTERS              IdeIoPortReg[MAX_IDE_CHANNELS]; //for max 4 channel.
+} ATAPI_BLK_IO_DEV;
+
+#define PEI_RECOVERY_ATAPI_FROM_BLKIO_THIS(a) CR (a, ATAPI_BLK_IO_DEV, AtapiBlkIo, ATAPI_BLK_IO_DEV_SIGNATURE)
+#define PEI_RECOVERY_ATAPI_FROM_BLKIO2_THIS(a) CR (a, ATAPI_BLK_IO_DEV, AtapiBlkIo2, ATAPI_BLK_IO_DEV_SIGNATURE)
+
+
+#define STALL_1_MILLI_SECOND  1000  // stall 1 ms
+#define STALL_1_SECONDS       1000 * STALL_1_MILLI_SECOND
+
+//
+// Time Out Value For IDE Device Polling
+//
+// ATATIMEOUT is used for waiting time out for ATA device
+//
+#define ATATIMEOUT  1000  // 1 second
+// ATAPITIMEOUT is used for waiting operation
+// except read and write time out for ATAPI device
+//
+#define ATAPITIMEOUT  1000  // 1 second
+// ATAPILONGTIMEOUT is used for waiting read and
+// write operation timeout for ATAPI device
+//
+#define CDROMLONGTIMEOUT  2000  // 2 seconds
+#define ATAPILONGTIMEOUT  5000  // 5 seconds
+
+//
+// PEI Recovery Block I/O PPI
+//
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  OUT  UINTN                             *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS           Media information about the specified block device 
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware 
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI        *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO_MEDIA               *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO_PPI     *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetNumberOfBlockDevices2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  OUT  UINTN                             *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS           Media information about the specified block device
+                                was obtained successfully.
+  @retval EFI_DEVICE_ERROR      Cannot get the media information due to a hardware
+                                error.
+  @retval Others                Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiGetBlockDeviceMediaInfo2 (
+  IN   EFI_PEI_SERVICES                     **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI       *This,
+  IN   UINTN                                DeviceIndex,
+  OUT  EFI_PEI_BLOCK_IO2_MEDIA              *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+AtapiReadBlocks2 (
+  IN   EFI_PEI_SERVICES                  **PeiServices,
+  IN   EFI_PEI_RECOVERY_BLOCK_IO2_PPI    *This,
+  IN   UINTN                             DeviceIndex,
+  IN   EFI_PEI_LBA                       StartLBA,
+  IN   UINTN                             BufferSize,
+  OUT  VOID                              *Buffer
+  );
+
+//
+// Internal functions
+//
+
+/**
+  Enumerate Atapi devices.
+
+  This function is used to enumerate Atatpi device in Ide channel.
+
+  @param[in]  AtapiBlkIoDev  A pointer to atapi block IO device
+
+**/
+VOID
+AtapiEnumerateDevices (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev
+  );
+
+/**
+  Detect Atapi devices.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval TRUE                Atapi device exists in specified position.
+  @retval FALSE               Atapi device does not exist in specified position.
+
+**/
+BOOLEAN
+DiscoverAtapiDevice (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  );
+
+/**
+  Detect if an IDE controller exists in specified position.
+  
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+
+  @retval TRUE         The Atapi device exists.
+  @retval FALSE        The Atapi device does not present.
+
+**/
+BOOLEAN
+DetectIDEController (
+  IN  ATAPI_BLK_IO_DEV   *AtapiBlkIoDev,
+  IN  UINTN              DevicePosition
+  );
+
+/**
+  Wait specified time interval to poll for BSY bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        BSY bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        BSY bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+WaitForBSYClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRDY bit set in the Status register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRDY bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRDY bit is not set in the specified time interval.
+
+**/
+EFI_STATUS
+DRDYReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit clear in the Alternate Status Register.
+  
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is cleared in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not cleared in the specified time interval.
+
+**/
+EFI_STATUS
+DRQClear2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Wait specified time interval to poll for DRQ bit set in the Alternate Status Register.
+
+  @param[in]  AtapiBlkIoDev          A pointer to atapi block IO device.
+  @param[in]  IdeIoRegisters         A pointer to IDE IO registers.
+  @param[in]  TimeoutInMilliSeconds  Time specified in milliseconds.
+
+  @retval EFI_SUCCESS        DRQ bit is set in the specified time interval.
+  @retval EFI_TIMEOUT        DRQ bit is not set in the specified time interval.
+  @retval EFI_ABORTED        Operation Aborted.
+
+**/
+EFI_STATUS
+DRQReady2 (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  IDE_BASE_REGISTERS  *IdeIoRegisters,
+  IN  UINTN               TimeoutInMilliSeconds
+  );
+
+/**
+  Check if there is an error in Status Register.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  StatusReg         The address to IDE IO registers.
+
+  @retval EFI_SUCCESS        Operation success.
+  @retval EFI_DEVICE_ERROR   Device error.
+
+**/
+EFI_STATUS
+CheckErrorStatus (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINT16              StatusReg
+  );
+
+/**
+  Idendify Atapi devices.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        Identify successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be identified successfully.
+
+**/
+EFI_STATUS
+ATAPIIdentify (
+  IN  ATAPI_BLK_IO_DEV        *AtapiBlkIoDev,
+  IN  UINTN                   DevicePosition
+  );
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+TestUnitReady (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition
+  ) ;
+
+/**
+  Send out ATAPI commands conforms to the Packet Command with PIO Data In Protocol.
+  
+  @param[in]  AtapiBlkIoDev         A pointer to atapi block IO device.
+  @param[in]  DevicePosition        An integer to signify device position.
+  @param[in]  Packet                A pointer to ATAPI command packet.
+  @param[in]  Buffer                Buffer to contain requested transfer data from device.
+  @param[in]  ByteCount             Requested transfer data length.
+  @param[in]  TimeoutInMilliSeconds Time out value, in unit of milliseconds.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+
+**/
+EFI_STATUS
+AtapiPacketCommandIn (
+  IN  ATAPI_BLK_IO_DEV      *AtapiBlkIoDev,
+  IN  UINTN                 DevicePosition,
+  IN  ATAPI_PACKET_COMMAND  *Packet,
+  IN  UINT16                *Buffer,
+  IN  UINT32                ByteCount,
+  IN  UINTN                 TimeoutInMilliSeconds
+  );
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device.
+  This command will return INQUIRY data of the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[out] MediaInfo       The media information of the specified block media.
+  @param[out] MediaInfo2      The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS        Command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed command successfully.
+  @retval EFI_UNSUPPORTED    Unsupported device type.
+
+**/
+EFI_STATUS
+Inquiry (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  OUT EFI_PEI_BLOCK_IO_MEDIA        *MediaInfo,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA       *MediaInfo2
+  );
+
+/**  
+  Used before read/write blocks from/to ATAPI device media. 
+  Since ATAPI device media is removable, it is necessary to detect
+  whether media is present and get current present media's information.  
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in, out] MediaInfo     The media information of the specified block media.
+  @param[in, out] MediaInfo2    The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate required resources.
+
+**/
+EFI_STATUS
+DetectMedia (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  );
+
+/**  
+  Reset specified Atapi device.
+
+  @param[in]  AtapiBlkIoDev     A pointer to atapi block IO device.
+  @param[in]  DevicePosition    An integer to signify device position.
+  @param[in]  Extensive         If TRUE, use ATA soft reset, otherwise use Atapi soft reset.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ResetDevice (
+  IN  ATAPI_BLK_IO_DEV  *AtapiBlkIoDev,
+  IN  UINTN             DevicePosition,
+  IN  BOOLEAN           Extensive
+  );
+
+/**  
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param[in]      AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]      DevicePosition  An integer to signify device position.
+  @param[in]      SenseBuffers    Pointer to sense buffer.
+  @param[in, out] SenseCounts     Length of sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+RequestSense (
+  IN  ATAPI_BLK_IO_DEV          *AtapiBlkIoDev,
+  IN  UINTN                     DevicePosition,
+  IN  ATAPI_REQUEST_SENSE_DATA  *SenseBuffers,
+  IN OUT  UINT8                 *SenseCounts
+  );
+
+/**  
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in, out] MediaInfo   The media information of the specified block media.
+  @param[in, out] MediaInfo2  The media information 2 of the specified block media.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadCapacity (
+  IN  ATAPI_BLK_IO_DEV              *AtapiBlkIoDev,
+  IN  UINTN                         DevicePosition,
+  IN OUT EFI_PEI_BLOCK_IO_MEDIA     *MediaInfo,
+  IN OUT EFI_PEI_BLOCK_IO2_MEDIA    *MediaInfo2
+  );
+
+/**  
+  Perform read from disk in block unit.
+
+  @param[in]  AtapiBlkIoDev   A pointer to atapi block IO device.
+  @param[in]  DevicePosition  An integer to signify device position.
+  @param[in]  Buffer          Buffer to contain read data.
+  @param[in]  StartLba        Starting LBA address.
+  @param[in]  NumberOfBlocks  Number of blocks to read.
+  @param[in]  BlockSize       Size of each block.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN  ATAPI_BLK_IO_DEV    *AtapiBlkIoDev,
+  IN  UINTN               DevicePosition,
+  IN  VOID                *Buffer,
+  IN  EFI_PEI_LBA         StartLba,
+  IN  UINTN               NumberOfBlocks,
+  IN  UINTN               BlockSize
+  );
+
+/**  
+  Check if there is media according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if device state is unclear according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Device state is unclear
+  @retval FALSE   Device state is clear  
+
+**/
+BOOLEAN
+IsDeviceStateUnclear (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if drive is ready according to sense data.
+
+  @param[in]  SenseData   Pointer to sense data.
+  @param[in]  SenseCounts Count of sense data.
+  @param[out] NeedRetry   Indicate if retry is needed.
+
+  @retval TRUE    Drive ready
+  @retval FALSE   Drive not ready
+
+**/
+BOOLEAN
+IsDriveReady (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *NeedRetry
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf
new file mode 100644
index 0000000000..8bcef7bc6a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf
@@ -0,0 +1,69 @@
+## @file
+# PEIM to produce gEfiPeiVirtualBlockIoPpiGuid PPI for ATA controllers in the platform.
+# This PPI can be consumed by PEIM which produce gEfiPeiDeviceRecoveryModulePpiGuid
+# for Atapi CD ROM device.
+#
+# This module discovers CDROM devices in Legacy and native mode and installs block IO ppis for them.
+# Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IdeBusPei
+  MODULE_UNI_FILE                = IdeBusPei.uni
+  FILE_GUID                      = B7A5041A-78BA-49e3-B73B-54C757811FB6
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = AtapiPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  AtapiPeim.h
+  AtapiPeim.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  TimerLib
+  PeiServicesTablePointerLib
+  MemoryAllocationLib
+  PcdLib
+
+[Ppis]
+  gPeiAtaControllerPpiGuid                      ## CONSUMES
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSataSpinUpDelayInSecForRecoveryPath   ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid AND gPeiAtaControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IdeBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni
new file mode 100644
index 0000000000..75ab58e15d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni
new file mode 100644
index 0000000000..ec4663cf91
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c
new file mode 100644
index 0000000000..3d581b69fd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.c
@@ -0,0 +1,387 @@
+/** @file
+  This module is one template module for Incompatible PCI Device Support protocol.
+  It includes one incompatile pci devices list template.
+  
+  Incompatible PCI Device Support protocol allows the PCI bus driver to support
+  resource allocation for some PCI devices that do not comply with the PCI Specification.
+
+Copyright (c) 2009, 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 <PiDxe.h>
+#include <Protocol/IncompatiblePciDeviceSupport.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Acpi.h>
+
+typedef struct {
+  UINT64  VendorId;
+  UINT64  DeviceId;
+  UINT64  RevisionId;
+  UINT64  SubsystemVendorId;
+  UINT64  SubsystemDeviceId;
+} EFI_PCI_DEVICE_HEADER_INFO;
+
+typedef struct {
+  UINT64  ResType;
+  UINT64  GenFlag;
+  UINT64  SpecificFlag;
+  UINT64  AddrSpaceGranularity;
+  UINT64  AddrRangeMin;
+  UINT64  AddrRangeMax;
+  UINT64  AddrTranslationOffset;
+  UINT64  AddrLen;
+} EFI_PCI_RESOUCE_DESCRIPTOR;
+
+#define PCI_DEVICE_ID(VendorId, DeviceId, Revision, SubVendorId, SubDeviceId) \
+    VendorId, DeviceId, Revision, SubVendorId, SubDeviceId
+
+#define PCI_BAR_TYPE_IO   ACPI_ADDRESS_SPACE_TYPE_IO
+#define PCI_BAR_TYPE_MEM  ACPI_ADDRESS_SPACE_TYPE_MEM
+
+#define DEVICE_INF_TAG    0xFFF2
+#define DEVICE_RES_TAG    0xFFF1
+#define LIST_END_TAG      0x0000
+
+
+/**
+  Returns a list of ACPI resource descriptors that detail the special
+  resource configuration requirements for an incompatible PCI device.
+
+  @param  This                  Pointer to the EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL instance.
+  @param  VendorId              A unique ID to identify the manufacturer of the PCI device.
+  @param  DeviceId              A unique ID to identify the particular PCI device.
+  @param  RevisionId            A PCI device-specific revision identifier.
+  @param  SubsystemVendorId     Specifies the subsystem vendor ID.
+  @param  SubsystemDeviceId     Specifies the subsystem device ID.
+  @param  Configuration         A list of ACPI resource descriptors returned that detail
+                                the configuration requirement.
+
+  @retval EFI_SUCCESS           Successfully got ACPI resource for specified PCI device.
+  @retval EFI_INVALID_PARAMETER Configuration is NULL. 
+  @retval EFI_OUT_OF_RESOURCES  No memory available.
+  @retval EFI_UNSUPPORTED       The specified PCI device wasn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+PCheckDevice (
+  IN  EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *This,
+  IN  UINTN                                         VendorId,
+  IN  UINTN                                         DeviceId,
+  IN  UINTN                                         RevisionId,
+  IN  UINTN                                         SubsystemVendorId,
+  IN  UINTN                                         SubsystemDeviceId,
+  OUT VOID                                          **Configuration
+  );
+
+//
+// Handle onto which the Incompatible PCI Device List is installed
+//
+EFI_HANDLE                                    mIncompatiblePciDeviceSupportHandle = NULL;
+
+//
+// The Incompatible PCI Device Support Protocol instance produced by this driver
+//
+EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  mIncompatiblePciDeviceSupport = {
+  PCheckDevice
+};
+
+//
+// The incompatible PCI devices list template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINT64 mIncompatiblePciDeviceList[] = {
+  //
+  // DEVICE_INF_TAG,
+  // PCI_DEVICE_ID (VendorID, DeviceID, Revision, SubVendorId, SubDeviceId),
+  // DEVICE_RES_TAG,
+  // ResType,  GFlag , SFlag,   Granularity,  RangeMin,
+  // RangeMax, Offset, AddrLen
+  //
+  //
+  // Device Adaptec 9004
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x9004, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Adaptec 9005
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x9005, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device QLogic  1007
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x1077, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Agilent 103C
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x103C, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // Device Agilent 15BC
+  //
+  DEVICE_INF_TAG,
+  PCI_DEVICE_ID(0x15BC, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE, DEVICE_ID_NOCARE),
+  DEVICE_RES_TAG,
+  PCI_BAR_TYPE_IO,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_ACPI_UNUSED,
+  PCI_BAR_EVEN_ALIGN,
+  PCI_BAR_ALL,
+  PCI_BAR_NOCHANGE,
+  //
+  // The end of the list
+  //
+  LIST_END_TAG
+};
+
+
+/**
+  Entry point of the incompatible pci device support code. Setup an incompatible device list template
+  and install EFI Incompatible PCI Device Support protocol.
+
+  @param ImageHandle             A handle for the image that is initializing this driver.
+  @param SystemTable             A pointer to the EFI system table.
+
+  @retval EFI_SUCCESS            Installed EFI Incompatible PCI Device Support Protocol successfully.
+  @retval others                 Failed to install protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+IncompatiblePciDeviceSupportEntryPoint (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                         Status;
+
+  //
+  // Install EFI Incompatible PCI Device Support Protocol on a new handle
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mIncompatiblePciDeviceSupportHandle,
+                  &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mIncompatiblePciDeviceSupport
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Returns a list of ACPI resource descriptors that detail the special
+  resource configuration requirements for an incompatible PCI device.
+
+  @param  This                  Pointer to the EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL instance.
+  @param  VendorId              A unique ID to identify the manufacturer of the PCI device.
+  @param  DeviceId              A unique ID to identify the particular PCI device.
+  @param  RevisionId            A PCI device-specific revision identifier.
+  @param  SubsystemVendorId     Specifies the subsystem vendor ID.
+  @param  SubsystemDeviceId     Specifies the subsystem device ID.
+  @param  Configuration         A list of ACPI resource descriptors returned that detail
+                                the configuration requirement.
+
+  @retval EFI_SUCCESS           Successfully got ACPI resource for specified PCI device.
+  @retval EFI_INVALID_PARAMETER Configuration is NULL. 
+  @retval EFI_OUT_OF_RESOURCES  No memory available.
+  @retval EFI_UNSUPPORTED       The specified PCI device wasn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+PCheckDevice (
+  IN  EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *This,
+  IN  UINTN                                         VendorId,
+  IN  UINTN                                         DeviceId,
+  IN  UINTN                                         RevisionId,
+  IN  UINTN                                         SubsystemVendorId,
+  IN  UINTN                                         SubsystemDeviceId,
+  OUT VOID                                          **Configuration
+  )
+{
+  UINT64                            Tag;
+  UINT64                            *ListPtr;
+  UINT64                            *TempListPtr;
+  EFI_PCI_DEVICE_HEADER_INFO        *Header;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *AcpiPtr;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *OldAcpiPtr;
+  EFI_PCI_RESOUCE_DESCRIPTOR        *Dsc;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *PtrEnd;
+  UINTN                             Index;
+
+  //
+  // Validate the parameters
+  //
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Initialize the return value to NULL
+  //
+  * (VOID **) Configuration = NULL;
+
+  ListPtr                   = mIncompatiblePciDeviceList;
+  while (*ListPtr != LIST_END_TAG) {
+
+    Tag = *ListPtr;
+
+    switch (Tag) {
+    case DEVICE_INF_TAG:
+      Header  = (EFI_PCI_DEVICE_HEADER_INFO *) (ListPtr + 1);
+      ListPtr = ListPtr + 1 + sizeof (EFI_PCI_DEVICE_HEADER_INFO) / sizeof (UINT64);
+      //
+      // See if the Header matches the parameters passed in
+      //
+      if (Header->VendorId != DEVICE_ID_NOCARE) {
+        if (Header->VendorId != VendorId) {
+          continue;
+        }
+      }
+
+      if (Header->DeviceId != DEVICE_ID_NOCARE) {
+        if (DeviceId != Header->DeviceId) {
+          continue;
+        }
+      }
+
+      if (Header->RevisionId != DEVICE_ID_NOCARE) {
+        if (RevisionId != Header->RevisionId) {
+          continue;
+        }
+      }
+
+      if (Header->SubsystemVendorId != DEVICE_ID_NOCARE) {
+        if (SubsystemVendorId != Header->SubsystemVendorId) {
+          continue;
+        }
+      }
+
+      if (Header->SubsystemDeviceId != DEVICE_ID_NOCARE) {
+        if (SubsystemDeviceId != Header->SubsystemDeviceId) {
+          continue;
+        }
+      }
+      //
+      // Matched an item, so construct the ACPI descriptor for the resource.
+      //
+      //
+      // Count the resource items so that to allocate space
+      //
+      for (Index = 0, TempListPtr = ListPtr; *TempListPtr == DEVICE_RES_TAG; Index++) {
+        TempListPtr = TempListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+      }
+      //
+      // If there is at least one type of resource request,
+      // allocate an acpi resource node
+      //
+      if (Index == 0) {
+        return EFI_UNSUPPORTED;
+      }
+
+      AcpiPtr = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) * Index + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (AcpiPtr == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      OldAcpiPtr = AcpiPtr;
+      //
+      // Fill the EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR structure
+      // according to the EFI_PCI_RESOUCE_DESCRIPTOR structure
+      //
+      for (; *ListPtr == DEVICE_RES_TAG;) {
+
+        Dsc = (EFI_PCI_RESOUCE_DESCRIPTOR *) (ListPtr + 1);
+
+        AcpiPtr->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+        AcpiPtr->Len = (UINT16) sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+        AcpiPtr->ResType = (UINT8) Dsc->ResType;
+        AcpiPtr->GenFlag = (UINT8) Dsc->GenFlag;
+        AcpiPtr->SpecificFlag = (UINT8) Dsc->SpecificFlag;
+        AcpiPtr->AddrSpaceGranularity = Dsc->AddrSpaceGranularity;;
+        AcpiPtr->AddrRangeMin = Dsc->AddrRangeMin;
+        AcpiPtr->AddrRangeMax = Dsc->AddrRangeMax;
+        AcpiPtr->AddrTranslationOffset = Dsc->AddrTranslationOffset;
+        AcpiPtr->AddrLen = Dsc->AddrLen;
+
+        ListPtr = ListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+        AcpiPtr++;
+      }
+      //
+      // Put the checksum
+      //
+      PtrEnd                    = (EFI_ACPI_END_TAG_DESCRIPTOR *) (AcpiPtr);
+      PtrEnd->Desc              = ACPI_END_TAG_DESCRIPTOR;
+      PtrEnd->Checksum          = 0;
+
+      *(VOID **) Configuration  = OldAcpiPtr;
+
+      return EFI_SUCCESS;
+
+    case DEVICE_RES_TAG:
+      //
+      // Adjust the pointer to the next PCI resource descriptor item
+      //
+      ListPtr = ListPtr + 1 + ((sizeof (EFI_PCI_RESOUCE_DESCRIPTOR)) / sizeof (UINT64));
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni
new file mode 100644
index 0000000000..46732f797a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupport.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf
new file mode 100644
index 0000000000..a83edaec7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf
@@ -0,0 +1,53 @@
+## @file
+#  PCI Incompatible device support module template.
+#
+#  Installs EFI PCI Incompatible Device Support protocol and includes one incompatile 
+#  pci devices list template.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IncompatiblePciDeviceSupport
+  MODULE_UNI_FILE                = IncompatiblePciDeviceSupport.uni
+  FILE_GUID                      = AD70855E-0CC5-4abf-8979-BE762A949EA3
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = IncompatiblePciDeviceSupportEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  IncompatiblePciDeviceSupport.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  DebugLib
+
+[Protocols]
+  gEfiIncompatiblePciDeviceSupportProtocolGuid    ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IncompatiblePciDeviceSupportExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni
new file mode 100644
index 0000000000..0a3aaac3da
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c
new file mode 100644
index 0000000000..60849aea03
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/ComponentName.c
@@ -0,0 +1,233 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013, 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 "NvmExpress.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gNvmExpressComponentName = {
+  NvmExpressComponentNameGetDriverName,
+  NvmExpressComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gNvmExpressComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) NvmExpressComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) NvmExpressComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mNvmExpressDriverNameTable[] = {
+  { "eng;en", L"NVM Express Driver" },
+  { NULL, NULL }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mNvmExpressControllerNameTable[] = {
+  { "eng;en", L"NVM Express Controller" },
+  { NULL, NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL       *This,
+  IN  CHAR8                             *Language,
+  OUT CHAR16                            **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mNvmExpressDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gNvmExpressComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_BLOCK_IO_PROTOCOL               *BlockIo;
+  NVME_DEVICE_PRIVATE_DATA            *Device;
+  EFI_UNICODE_STRING_TABLE            *ControllerNameTable;
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gNvmExpressDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = mNvmExpressControllerNameTable;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               &gEfiPciIoProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Get the child context
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    gNvmExpressDriverBinding.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+    Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (BlockIo);
+    ControllerNameTable = Device->ControllerNameTable;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gNvmExpressComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c
new file mode 100644
index 0000000000..9938bf4541
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c
@@ -0,0 +1,1138 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+//
+// NVM Express Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gNvmExpressDriverBinding = {
+  NvmExpressDriverBindingSupported,
+  NvmExpressDriverBindingStart,
+  NvmExpressDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// NVM Express EFI Driver Supported EFI Version Protocol Instance
+//
+EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL gNvmExpressDriverSupportedEfiVersion = {
+  sizeof (EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL), // Size of Protocol structure.
+  0                                                   // Version number to be filled at start up.
+};
+
+//
+// Template for NVM Express Pass Thru Mode data structure.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_NVM_EXPRESS_PASS_THRU_MODE gEfiNvmExpressPassThruMode = {
+  EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_NVM_EXPRESS_PASS_THRU_ATTRIBUTES_CMD_SET_NVM,
+  sizeof (UINTN),
+  0x10100
+};
+
+/**
+  Check if the specified Nvm Express device namespace is active, and create child handles
+  for them with BlockIo and DiskInfo protocol instances.
+
+  @param[in] Private         The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param[in] NamespaceId     The NVM Express namespace ID  for which a device path node is to be
+                             allocated and built. Caller must set the NamespaceId to zero if the
+                             device path node will contain a valid UUID.
+
+  @retval EFI_SUCCESS        All the namespaces in the device are successfully enumerated.
+  @return Others             Some error occurs when enumerating the namespaces.
+
+**/
+EFI_STATUS
+EnumerateNvmeDevNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  UINT32                                NamespaceId
+  )
+{
+  NVME_ADMIN_NAMESPACE_DATA             *NamespaceData;
+  EFI_DEVICE_PATH_PROTOCOL              *NewDevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+  EFI_HANDLE                            DeviceHandle;
+  EFI_DEVICE_PATH_PROTOCOL              *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL              *RemainingDevicePath;
+  NVME_DEVICE_PRIVATE_DATA              *Device;
+  EFI_STATUS                            Status;
+  UINT32                                Lbads;
+  UINT32                                Flbas;
+  UINT32                                LbaFmtIdx;
+
+  NewDevicePathNode = NULL;
+  DevicePath        = NULL;
+  Device            = NULL;
+
+  //
+  // Allocate a buffer for Identify Namespace data
+  //
+  NamespaceData = AllocateZeroPool(sizeof (NVME_ADMIN_NAMESPACE_DATA));
+  if(NamespaceData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ParentDevicePath = Private->ParentDevicePath;
+  //
+  // Identify Namespace
+  //
+  Status = NvmeIdentifyNamespace (
+             Private,
+             NamespaceId,
+             (VOID *)NamespaceData
+             );
+  if (EFI_ERROR(Status)) {
+    goto Exit;
+  }
+  //
+  // Validate Namespace
+  //
+  if (NamespaceData->Ncap == 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    //
+    // allocate device private data for each discovered namespace
+    //
+    Device = AllocateZeroPool(sizeof(NVME_DEVICE_PRIVATE_DATA));
+    if (Device == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    //
+    // Initialize SSD namespace instance data
+    //
+    Device->Signature           = NVME_DEVICE_PRIVATE_DATA_SIGNATURE;
+    Device->NamespaceId         = NamespaceId;
+    Device->NamespaceUuid       = NamespaceData->Eui64;
+
+    Device->ControllerHandle    = Private->ControllerHandle;
+    Device->DriverBindingHandle = Private->DriverBindingHandle;
+    Device->Controller          = Private;
+
+    //
+    // Build BlockIo media structure
+    //
+    Device->Media.MediaId        = 0;
+    Device->Media.RemovableMedia = FALSE;
+    Device->Media.MediaPresent   = TRUE;
+    Device->Media.LogicalPartition = FALSE;
+    Device->Media.ReadOnly       = FALSE;
+    Device->Media.WriteCaching   = FALSE;
+
+    Flbas     = NamespaceData->Flbas;
+    LbaFmtIdx = Flbas & 0xF;
+    Lbads     = NamespaceData->LbaFormat[LbaFmtIdx].Lbads;
+    Device->Media.BlockSize = (UINT32)1 << Lbads;
+
+    Device->Media.LastBlock                     = NamespaceData->Nsze - 1;
+    Device->Media.LogicalBlocksPerPhysicalBlock = 1;
+    Device->Media.LowestAlignedLba              = 1;
+
+    //
+    // Create BlockIo Protocol instance
+    //
+    Device->BlockIo.Revision     = EFI_BLOCK_IO_PROTOCOL_REVISION2;
+    Device->BlockIo.Media        = &Device->Media;
+    Device->BlockIo.Reset        = NvmeBlockIoReset;
+    Device->BlockIo.ReadBlocks   = NvmeBlockIoReadBlocks;
+    Device->BlockIo.WriteBlocks  = NvmeBlockIoWriteBlocks;
+    Device->BlockIo.FlushBlocks  = NvmeBlockIoFlushBlocks;
+
+    //
+    // Create StorageSecurityProtocol Instance
+    //
+    Device->StorageSecurity.ReceiveData = NvmeStorageSecurityReceiveData;
+    Device->StorageSecurity.SendData    = NvmeStorageSecuritySendData;
+
+    //
+    // Create DiskInfo Protocol instance
+    //
+    InitializeDiskInfo (Device);
+
+    //
+    // Create a Nvm Express Namespace Device Path Node
+    //
+    Status = Private->Passthru.BuildDevicePath (
+                                 &Private->Passthru,
+                                 Device->NamespaceId,
+                                 &NewDevicePathNode
+                                 );
+
+    if (EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    //
+    // Append the SSD node to the controller's device path
+    //
+    DevicePath = AppendDevicePathNode (ParentDevicePath, NewDevicePathNode);
+    if (DevicePath == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    DeviceHandle = NULL;
+    RemainingDevicePath = DevicePath;
+    Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+    if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+      Status = EFI_ALREADY_STARTED;
+      FreePool (DevicePath);
+      goto Exit;
+    }
+
+    Device->DevicePath = DevicePath;
+
+    //
+    // Make sure the handle is NULL so we create a new handle
+    //
+    Device->DeviceHandle = NULL;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Device->DeviceHandle,
+                    &gEfiDevicePathProtocolGuid,
+                    Device->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &Device->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &Device->DiskInfo,
+                    NULL
+                    );
+
+    if(EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    //
+    // Check if the NVMe controller supports the Security Send and Security Receive commands
+    //
+    if ((Private->ControllerData->Oacs & SECURITY_SEND_RECEIVE_SUPPORTED) != 0) {
+      Status = gBS->InstallProtocolInterface (
+                      &Device->DeviceHandle,
+                      &gEfiStorageSecurityCommandProtocolGuid,
+                      EFI_NATIVE_INTERFACE,
+                      &Device->StorageSecurity
+                      );
+      if(EFI_ERROR(Status)) {
+        gBS->UninstallMultipleProtocolInterfaces (
+               &Device->DeviceHandle,
+               &gEfiDevicePathProtocolGuid,
+               Device->DevicePath,
+               &gEfiBlockIoProtocolGuid,
+               &Device->BlockIo,
+               &gEfiDiskInfoProtocolGuid,
+               &Device->DiskInfo,
+               NULL
+               );
+        goto Exit;
+      }
+    }
+
+    gBS->OpenProtocol (
+           Private->ControllerHandle,
+           &gEfiNvmExpressPassThruProtocolGuid,
+           (VOID **) &Private->Passthru,
+           Private->DriverBindingHandle,
+           Device->DeviceHandle,
+           EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+           );
+
+    //
+    // Dump NvmExpress Identify Namespace Data
+    //
+    DEBUG ((EFI_D_INFO, " == NVME IDENTIFY NAMESPACE [%d] DATA ==\n", NamespaceId));
+    DEBUG ((EFI_D_INFO, "    NSZE        : 0x%x\n", NamespaceData->Nsze));
+    DEBUG ((EFI_D_INFO, "    NCAP        : 0x%x\n", NamespaceData->Ncap));
+    DEBUG ((EFI_D_INFO, "    NUSE        : 0x%x\n", NamespaceData->Nuse));
+    DEBUG ((EFI_D_INFO, "    LBAF0.LBADS : 0x%x\n", (NamespaceData->LbaFormat[0].Lbads)));
+
+    //
+    // Build controller name for Component Name (2) protocol.
+    //
+    UnicodeSPrintAsciiFormat (Device->ModelName, sizeof (Device->ModelName), "%a-%a-%x", Private->ControllerData->Sn, Private->ControllerData->Mn, NamespaceData->Eui64);
+
+    AddUnicodeString2 (
+      "eng",
+      gNvmExpressComponentName.SupportedLanguages,
+      &Device->ControllerNameTable,
+      Device->ModelName,
+      TRUE
+      );
+
+    AddUnicodeString2 (
+      "en",
+      gNvmExpressComponentName2.SupportedLanguages,
+      &Device->ControllerNameTable,
+      Device->ModelName,
+      FALSE
+      );
+  }
+
+Exit:
+  if(NamespaceData != NULL) {
+    FreePool (NamespaceData);
+  }
+
+  if (NewDevicePathNode != NULL) {
+    FreePool (NewDevicePathNode);
+  }
+
+  if(EFI_ERROR(Status) && (Device != NULL) && (Device->DevicePath != NULL)) {
+    FreePool (Device->DevicePath);
+  }
+  if(EFI_ERROR(Status) && (Device != NULL)) {
+    FreePool (Device);
+  }
+  return Status;
+}
+
+/**
+  Discover all Nvm Express device namespaces, and create child handles for them with BlockIo
+  and DiskInfo protocol instances.
+
+  @param[in] Private         The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS        All the namespaces in the device are successfully enumerated.
+  @return Others             Some error occurs when enumerating the namespaces.
+
+**/
+EFI_STATUS
+DiscoverAllNamespaces (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS                            Status;
+  UINT32                                NamespaceId;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL    *Passthru;
+
+  NamespaceId   = 0xFFFFFFFF;
+  Passthru      = &Private->Passthru;
+
+  while (TRUE) {
+    Status = Passthru->GetNextNamespace (
+                         Passthru,
+                         (UINT32 *)&NamespaceId
+                         );
+
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    Status = EnumerateNvmeDevNamespace (
+               Private,
+               NamespaceId
+               );
+
+    if (EFI_ERROR(Status)) {
+      continue;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unregisters a Nvm Express device namespace.
+
+  This function removes the protocols installed on the controller handle and
+  frees the resources allocated for the namespace.
+
+  @param  This                  The pointer to EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The controller handle of the namespace.
+  @param  Handle                The child handle.
+
+  @retval EFI_SUCCESS           The namespace is successfully unregistered.
+  @return Others                Some error occurs when unregistering the namespace.
+
+**/
+EFI_STATUS
+UnregisterNvmeNamespace (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                               Status;
+  EFI_BLOCK_IO_PROTOCOL                    *BlockIo;
+  NVME_DEVICE_PRIVATE_DATA                 *Device;
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *StorageSecurity;
+
+  BlockIo = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Device  = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (BlockIo);
+  Private = Device->Controller;
+
+  //
+  // Close the child handle
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiNvmExpressPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Handle
+         );
+
+  //
+  // The Nvm Express driver installs the BlockIo and DiskInfo in the DriverBindingStart().
+  // Here should uninstall both of them.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  Device->DevicePath,
+                  &gEfiBlockIoProtocolGuid,
+                  &Device->BlockIo,
+                  &gEfiDiskInfoProtocolGuid,
+                  &Device->DiskInfo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->OpenProtocol (
+           Controller,
+           &gEfiNvmExpressPassThruProtocolGuid,
+           (VOID **) &Private->Passthru,
+           This->DriverBindingHandle,
+           Handle,
+           EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+           );
+    return Status;
+  }
+
+  //
+  // If Storage Security Command Protocol is installed, then uninstall this protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiStorageSecurityCommandProtocolGuid,
+                  (VOID **) &StorageSecurity,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallProtocolInterface (
+                    Handle,
+                    &gEfiStorageSecurityCommandProtocolGuid,
+                    &Device->StorageSecurity
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+        Controller,
+        &gEfiNvmExpressPassThruProtocolGuid,
+        (VOID **) &Private->Passthru,
+        This->DriverBindingHandle,
+        Handle,
+        EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+        );
+      return Status;
+    }
+  }
+
+  if(Device->DevicePath != NULL) {
+    FreePool (Device->DevicePath);
+  }
+
+  if (Device->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (Device->ControllerNameTable);
+  }
+
+  FreePool (Device);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEV_PATH_PTR          DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT8                     ClassCode[3];
+
+  //
+  // Check whether device path is valid
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node,
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      DevicePathNode.DevPath = RemainingDevicePath;
+
+      if ((DevicePathNode.DevPath->Type    != MESSAGING_DEVICE_PATH) ||
+          (DevicePathNode.DevPath->SubType != MSG_NVME_NAMESPACE_DP) ||
+          (DevicePathNodeLength(DevicePathNode.DevPath) != sizeof(NVME_NAMESPACE_DEVICE_PATH))) {
+         return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Attempt to Open PCI I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and Sub Class (offset 0x0A).
+  // This controller should be a Nvm Express controller.
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (ClassCode),
+                        ClassCode
+                        );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Examine Nvm Express controller PCI Configuration table fields
+  //
+  if ((ClassCode[0] != PCI_IF_NVMHCI) || (ClassCode[1] != PCI_CLASS_MASS_STORAGE_NVM) || (ClassCode[2] != PCI_CLASS_MASS_STORAGE)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+Done:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_PCI_IO_PROTOCOL                 *PciIo;
+  NVME_CONTROLLER_PRIVATE_DATA        *Private;
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+  UINT32                              NamespaceId;
+  EFI_PHYSICAL_ADDRESS                MappedAddr;
+  UINTN                               Bytes;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  *Passthru;
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: start\n"));
+
+  Private          = NULL;
+  Passthru         = NULL;
+  ParentDevicePath = NULL;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if ((EFI_ERROR (Status)) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  //
+  // Check EFI_ALREADY_STARTED to reuse the original NVME_CONTROLLER_PRIVATE_DATA.
+  //
+  if (Status != EFI_ALREADY_STARTED) {
+    Private = AllocateZeroPool (sizeof (NVME_CONTROLLER_PRIVATE_DATA));
+
+    if (Private == NULL) {
+      DEBUG ((EFI_D_ERROR, "NvmExpressDriverBindingStart: allocating pool for Nvme Private Data failed!\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    //
+    // 4 x 4kB aligned buffers will be carved out of this buffer.
+    // 1st 4kB boundary is the start of the admin submission queue.
+    // 2nd 4kB boundary is the start of the admin completion queue.
+    // 3rd 4kB boundary is the start of I/O submission queue #1.
+    // 4th 4kB boundary is the start of I/O completion queue #1.
+    //
+    // Allocate 4 pages of memory, then map it for bus master read and write.
+    //
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      4,
+                      (VOID**)&Private->Buffer,
+                      0
+                      );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Bytes = EFI_PAGES_TO_SIZE (4);
+    Status = PciIo->Map (
+                      PciIo,
+                      EfiPciIoOperationBusMasterCommonBuffer,
+                      Private->Buffer,
+                      &Bytes,
+                      &MappedAddr,
+                      &Private->Mapping
+                      );
+
+    if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (4))) {
+      goto Exit;
+    }
+
+    Private->BufferPciAddr = (UINT8 *)(UINTN)MappedAddr;
+    ZeroMem (Private->Buffer, EFI_PAGES_TO_SIZE (4));
+
+    Private->Signature = NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE;
+    Private->ControllerHandle          = Controller;
+    Private->ImageHandle               = This->DriverBindingHandle;
+    Private->DriverBindingHandle       = This->DriverBindingHandle;
+    Private->PciIo                     = PciIo;
+    Private->ParentDevicePath          = ParentDevicePath;
+    Private->Passthru.Mode             = &Private->PassThruMode;
+    Private->Passthru.PassThru         = NvmExpressPassThru;
+    Private->Passthru.GetNextNamespace = NvmExpressGetNextNamespace;
+    Private->Passthru.BuildDevicePath  = NvmExpressBuildDevicePath;
+    Private->Passthru.GetNamespace     = NvmExpressGetNamespace;
+    CopyMem (&Private->PassThruMode, &gEfiNvmExpressPassThruMode, sizeof (EFI_NVM_EXPRESS_PASS_THRU_MODE));
+
+    Status = NvmeControllerInit (Private);
+    if (EFI_ERROR(Status)) {
+      goto Exit;
+    }
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    &Private->Passthru,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  } else {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    (VOID **) &Passthru,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (Passthru);
+  }
+
+  if (RemainingDevicePath == NULL) {
+    //
+    // Enumerate all NVME namespaces in the controller
+    //
+    Status = DiscoverAllNamespaces (
+               Private
+               );
+
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // Enumerate the specified NVME namespace
+    //
+    Status = Private->Passthru.GetNamespace (
+                                 &Private->Passthru,
+                                 RemainingDevicePath,
+                                 &NamespaceId
+                                 );
+
+    if (!EFI_ERROR (Status)) {
+      Status = EnumerateNvmeDevNamespace (
+                 Private,
+                 NamespaceId
+                 );
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: end successfully\n"));
+  return EFI_SUCCESS;
+
+Exit:
+  if ((Private != NULL) && (Private->Mapping != NULL)) {
+    PciIo->Unmap (PciIo, Private->Mapping);
+  }
+
+  if ((Private != NULL) && (Private->Buffer != NULL)) {
+    PciIo->FreeBuffer (PciIo, 4, Private->Buffer);
+  }
+
+  if (Private != NULL) {
+    FreePool (Private);
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  DEBUG ((EFI_D_INFO, "NvmExpressDriverBindingStart: end with %r\n", Status));
+
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                          Status;
+  BOOLEAN                             AllChildrenStopped;
+  UINTN                               Index;
+  NVME_CONTROLLER_PRIVATE_DATA        *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  *PassThru;
+
+  if (NumberOfChildren == 0) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiNvmExpressPassThruProtocolGuid,
+                    (VOID **) &PassThru,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (!EFI_ERROR (Status)) {
+      Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (PassThru);
+      gBS->UninstallMultipleProtocolInterfaces (
+            Controller,
+            &gEfiNvmExpressPassThruProtocolGuid,
+            PassThru,
+            NULL
+            );
+
+      if (Private->Mapping != NULL) {
+        Private->PciIo->Unmap (Private->PciIo, Private->Mapping);
+      }
+
+      if (Private->Buffer != NULL) {
+        Private->PciIo->FreeBuffer (Private->PciIo, 4, Private->Buffer);
+      }
+
+      FreePool (Private->ControllerData);
+      FreePool (Private);
+    }
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    Status = UnregisterNvmeNamespace (This, Controller, ChildHandleBuffer[Index]);
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This is the unload handle for the NVM Express driver.
+
+  Disconnect the driver specified by ImageHandle from the NVMe device in the handle database.
+  Uninstall all the protocols installed in the driver.
+
+  @param[in]  ImageHandle       The drivers' driver image.
+
+  @retval EFI_SUCCESS           The image is unloaded.
+  @retval Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of the device handles managed by this driver.
+  // If there is an error getting the list, then means the driver
+  // doesn't manage any device. At this way, we would only close
+  // those protocols installed at image handle.
+  //
+  DeviceHandleBuffer = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiNvmExpressPassThruProtocolGuid,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Disconnect the driver specified by ImageHandle from all
+    // the devices in the handle database.
+    //
+    for (Index = 0; Index < DeviceHandleCount; Index++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index],
+                      ImageHandle,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto EXIT;
+      }
+    }
+  }
+
+  //
+  // Uninstall all the protocols installed in the driver entry point
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ImageHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gNvmExpressDriverBinding,
+                  &gEfiDriverSupportedEfiVersionProtocolGuid,
+                  &gNvmExpressDriverSupportedEfiVersion,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Note we have to one by one uninstall the following protocols.
+  // It's because some of them are optionally installed based on
+  // the following PCD settings.
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnosticsDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentNameDisable
+  //   gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable
+  //   gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable
+  //
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           ImageHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName
+           );
+  }
+
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallProtocolInterface (
+           ImageHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2
+           );
+  }
+
+  Status = EFI_SUCCESS;
+
+EXIT:
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+  return Status;
+}
+
+/**
+  The entry point for Nvm Express driver, used to install Nvm Express driver on the ImageHandle.
+
+  @param  ImageHandle   The firmware allocated handle for this driver image.
+  @param  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gNvmExpressDriverBinding,
+             ImageHandle,
+             &gNvmExpressComponentName,
+             &gNvmExpressComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install EFI Driver Supported EFI Version Protocol required for
+  // EFI drivers that are on PCI and other plug in cards.
+  //
+  gNvmExpressDriverSupportedEfiVersion.FirmwareVersion = 0x00020028;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ImageHandle,
+                  &gEfiDriverSupportedEfiVersionProtocolGuid,
+                  &gNvmExpressDriverSupportedEfiVersion,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h
new file mode 100644
index 0000000000..21c6255caa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h
@@ -0,0 +1,610 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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.
+
+**/
+
+#ifndef _EFI_NVM_EXPRESS_H_
+#define _EFI_NVM_EXPRESS_H_
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/NvmExpressPassthru.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/DiskInfo.h>
+#include <Protocol/DriverSupportedEfiVersion.h>
+#include <Protocol/StorageSecurityCommand.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+typedef struct _NVME_CONTROLLER_PRIVATE_DATA NVME_CONTROLLER_PRIVATE_DATA;
+typedef struct _NVME_DEVICE_PRIVATE_DATA     NVME_DEVICE_PRIVATE_DATA;
+
+#include "NvmExpressBlockIo.h"
+#include "NvmExpressDiskInfo.h"
+#include "NvmExpressHci.h"
+
+extern EFI_DRIVER_BINDING_PROTOCOL                gNvmExpressDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                gNvmExpressComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL               gNvmExpressComponentName2;
+extern EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL  gNvmExpressDriverSupportedEfiVersion;
+
+#define PCI_CLASS_MASS_STORAGE_NVM                0x08  // mass storage sub-class non-volatile memory.
+#define PCI_IF_NVMHCI                             0x02  // mass storage programming interface NVMHCI.
+
+#define NVME_ASQ_SIZE                             1     // Number of admin submission queue entries, which is 0-based
+#define NVME_ACQ_SIZE                             1     // Number of admin completion queue entries, which is 0-based
+
+#define NVME_CSQ_SIZE                             1     // Number of I/O submission queue entries, which is 0-based
+#define NVME_CCQ_SIZE                             1     // Number of I/O completion queue entries, which is 0-based
+
+#define NVME_MAX_QUEUES                           2     // Number of queues supported by the driver
+
+#define NVME_CONTROLLER_ID                        0
+
+//
+// Time out value for Nvme transaction execution
+//
+#define NVME_GENERIC_TIMEOUT                      EFI_TIMER_PERIOD_SECONDS (5)
+
+//
+// Unique signature for private data structure.
+//
+#define NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE    SIGNATURE_32 ('N','V','M','E')
+
+//
+// Nvme private data structure.
+//
+struct _NVME_CONTROLLER_PRIVATE_DATA {
+  UINT32                              Signature;
+
+  EFI_HANDLE                          ControllerHandle;
+  EFI_HANDLE                          ImageHandle;
+  EFI_HANDLE                          DriverBindingHandle;
+
+  EFI_PCI_IO_PROTOCOL                 *PciIo;
+  UINT64                              PciAttributes;
+
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+
+  EFI_NVM_EXPRESS_PASS_THRU_MODE      PassThruMode;
+  EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL  Passthru;
+
+  //
+  // pointer to identify controller data
+  //
+  NVME_ADMIN_CONTROLLER_DATA          *ControllerData;
+
+  //
+  // 6 x 4kB aligned buffers will be carved out of this buffer.
+  // 1st 4kB boundary is the start of the admin submission queue.
+  // 2nd 4kB boundary is the start of the I/O submission queue #1.
+  // 3rd 4kB boundary is the start of the admin completion queue.
+  // 4th 4kB boundary is the start of the I/O completion queue #1.
+  // 5th 4kB boundary is the start of the first PRP list page.
+  // 6th 4kB boundary is the start of the second PRP list page.
+  //
+  UINT8                               *Buffer;
+  UINT8                               *BufferPciAddr;
+
+  //
+  // Pointers to 4kB aligned submission & completion queues.
+  //
+  NVME_SQ                             *SqBuffer[NVME_MAX_QUEUES];
+  NVME_CQ                             *CqBuffer[NVME_MAX_QUEUES];
+  NVME_SQ                             *SqBufferPciAddr[NVME_MAX_QUEUES];
+  NVME_CQ                             *CqBufferPciAddr[NVME_MAX_QUEUES];
+
+  //
+  // Submission and completion queue indices.
+  //
+  NVME_SQTDBL                         SqTdbl[NVME_MAX_QUEUES];
+  NVME_CQHDBL                         CqHdbl[NVME_MAX_QUEUES];
+
+  UINT8                               Pt[NVME_MAX_QUEUES];
+  UINT16                              Cid[NVME_MAX_QUEUES];
+
+  //
+  // Nvme controller capabilities
+  //
+  NVME_CAP                            Cap;
+
+  VOID                                *Mapping;
+};
+
+#define NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU(a) \
+  CR (a, \
+      NVME_CONTROLLER_PRIVATE_DATA, \
+      Passthru, \
+      NVME_CONTROLLER_PRIVATE_DATA_SIGNATURE \
+      )
+
+//
+// Unique signature for private data structure.
+//
+#define NVME_DEVICE_PRIVATE_DATA_SIGNATURE     SIGNATURE_32 ('X','S','S','D')
+
+//
+// Nvme device private data structure
+//
+struct _NVME_DEVICE_PRIVATE_DATA {
+  UINT32                                   Signature;
+
+  EFI_HANDLE                               DeviceHandle;
+  EFI_HANDLE                               ControllerHandle;
+  EFI_HANDLE                               DriverBindingHandle;
+
+  EFI_DEVICE_PATH_PROTOCOL                 *DevicePath;
+
+  EFI_UNICODE_STRING_TABLE                 *ControllerNameTable;
+
+  UINT32                                   NamespaceId;
+  UINT64                                   NamespaceUuid;
+
+  EFI_BLOCK_IO_MEDIA                       Media;
+  EFI_BLOCK_IO_PROTOCOL                    BlockIo;
+  EFI_DISK_INFO_PROTOCOL                   DiskInfo;
+  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    StorageSecurity;
+
+  EFI_LBA                                  NumBlocks;
+
+  CHAR16                                   ModelName[80];
+  NVME_ADMIN_NAMESPACE_DATA                NamespaceData;
+
+  NVME_CONTROLLER_PRIVATE_DATA             *Controller;
+
+};
+
+//
+// Statments to retrieve the private data from produced protocols.
+//
+#define NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO(a) \
+  CR (a, \
+      NVME_DEVICE_PRIVATE_DATA, \
+      BlockIo, \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define NVME_DEVICE_PRIVATE_DATA_FROM_DISK_INFO(a) \
+  CR (a, \
+      NVME_DEVICE_PRIVATE_DATA, \
+      DiskInfo, \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY(a)\
+  CR (a,                                                 \
+      NVME_DEVICE_PRIVATE_DATA,                          \
+      StorageSecurity,                                   \
+      NVME_DEVICE_PRIVATE_DATA_SIGNATURE                 \
+      )
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Sends an NVM Express Command Packet to an NVM Express controller or namespace. This function supports
+  both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the nonblocking
+  I/O functionality is optional.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         Is a 32 bit Namespace ID to which the Express HCI command packet will be sent.
+                                     A value of 0 denotes the NVM Express controller, a value of all 0FFh in the namespace
+                                     ID specifies that the command packet should be sent to all valid namespaces.
+  @param[in,out] Packet              A pointer to the NVM Express HCI Command Packet to send to the NVMe namespace specified
+                                     by NamespaceId.
+  @param[in]     Event               If nonblocking I/O is not supported then Event is ignored, and blocking I/O is performed.
+                                     If Event is NULL, then blocking I/O is performed. If Event is not NULL and non blocking I/O
+                                     is supported, then nonblocking I/O is performed, and Event will be signaled when the NVM
+                                     Express Command Packet completes.
+
+  @retval EFI_SUCCESS                The NVM Express Command Packet was sent by the host. TransferLength bytes were transferred
+                                     to, or from DataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The NVM Express Command Packet was not executed. The number of bytes that could be transferred
+                                     is returned in TransferLength.
+  @retval EFI_NOT_READY              The NVM Express Command Packet could not be sent because the controller is not ready. The caller
+                                     may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the NVM Express Command Packet.
+  @retval EFI_INVALID_PARAMETER      Namespace, or the contents of EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET are invalid. The NVM
+                                     Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_UNSUPPORTED            The command described by the NVM Express Command Packet is not supported by the host adapter.
+                                     The NVM Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT                A timeout occurred while waiting for the NVM Express Command Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressPassThru (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET    *Packet,
+  IN     EFI_EVENT                                   Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the next namespace ID for this NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNextNamespace() function retrieves the next valid
+  namespace ID on this NVM Express controller.
+
+  If on input the value pointed to by NamespaceId is 0xFFFFFFFF, then the first valid namespace
+  ID defined on the NVM Express controller is returned in the location pointed to by NamespaceId
+  and a status of EFI_SUCCESS is returned.
+
+  If on input the value pointed to by NamespaceId is an invalid namespace ID other than 0xFFFFFFFF,
+  then EFI_INVALID_PARAMETER is returned.
+
+  If on input the value pointed to by NamespaceId is a valid namespace ID, then the next valid
+  namespace ID on the NVM Express controller is returned in the location pointed to by NamespaceId,
+  and EFI_SUCCESS is returned.
+
+  If the value pointed to by NamespaceId is the namespace ID of the last namespace on the NVM
+  Express controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]     This           A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in,out] NamespaceId    On input, a pointer to a legal NamespaceId for an NVM Express
+                                namespace present on the NVM Express controller. On output, a
+                                pointer to the next NamespaceId of an NVM Express namespace on
+                                an NVM Express controller. An input value of 0xFFFFFFFF retrieves
+                                the first NamespaceId for an NVM Express namespace present on an
+                                NVM Express controller.
+
+  @retval EFI_SUCCESS           The Namespace ID of the next Namespace was returned.
+  @retval EFI_NOT_FOUND         There are no more namespaces defined on this controller.
+  @retval EFI_INVALID_PARAMETER NamespaceId is an invalid value other than 0xFFFFFFFF.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNextNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN OUT UINT32                                      *NamespaceId
+  );
+
+/**
+  Used to translate a device path node to a namespace ID.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNamespace() function determines the namespace ID associated with the
+  namespace described by DevicePath.
+
+  If DevicePath is a device path node type that the NVM Express Pass Thru driver supports, then the NVM Express
+  Pass Thru driver will attempt to translate the contents DevicePath into a namespace ID.
+
+  If this translation is successful, then that namespace ID is returned in NamespaceId, and EFI_SUCCESS is returned
+
+  @param[in]  This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an NVM Express namespace on
+                                  the NVM Express controller.
+  @param[out] NamespaceId         The NVM Express namespace ID contained in the device path node.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to NamespaceId.
+  @retval EFI_INVALID_PARAMETER   If DevicePath or NamespaceId are NULL, then EFI_INVALID_PARAMETER is returned.
+  @retval EFI_UNSUPPORTED         If DevicePath is not a device path node type that the NVM Express Pass Thru driver
+                                  supports, then EFI_UNSUPPORTED is returned.
+  @retval EFI_NOT_FOUND           If DevicePath is a device path node type that the NVM Express Pass Thru driver
+                                  supports, but there is not a valid translation from DevicePath to a namespace ID,
+                                  then EFI_NOT_FOUND is returned.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     EFI_DEVICE_PATH_PROTOCOL                    *DevicePath,
+     OUT UINT32                                      *NamespaceId
+  );
+
+/**
+  Used to allocate and build a device path node for an NVM Express namespace on an NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.BuildDevicePath() function allocates and builds a single device
+  path node for the NVM Express namespace specified by NamespaceId.
+
+  If the NamespaceId is not valid, then EFI_NOT_FOUND is returned.
+
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If there are not enough resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of DevicePath are
+  initialized to describe the NVM Express namespace specified by NamespaceId, and EFI_SUCCESS is returned.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         The NVM Express namespace ID  for which a device path node is to be
+                                     allocated and built. Caller must set the NamespaceId to zero if the
+                                     device path node will contain a valid UUID.
+  @param[in,out] DevicePath          A pointer to a single device path node that describes the NVM Express
+                                     namespace specified by NamespaceId. This function is responsible for
+                                     allocating the buffer DevicePath with the boot service AllocatePool().
+                                     It is the caller's responsibility to free DevicePath when the caller
+                                     is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the NVM Express namespace specified
+                                     by NamespaceId was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The NamespaceId is not valid.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate the DevicePath node.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressBuildDevicePath (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL                    **DevicePath
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c
new file mode 100644
index 0000000000..6cb2f92e8d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.c
@@ -0,0 +1,883 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Read some sectors from the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer used to store the data read from the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be read.
+
+  @retval EFI_SUCCESS            Datum are read from the device.
+  @retval Others                 Fail to read all the datum.
+
+**/
+EFI_STATUS
+ReadSectors (
+  IN NVME_DEVICE_PRIVATE_DATA           *Device,
+  IN UINT64                             Buffer,
+  IN UINT64                             Lba,
+  IN UINT32                             Blocks
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  UINT32                                   Bytes;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT32                                   BlockSize;
+
+  Private    = Device->Controller;
+  BlockSize  = Device->Media.BlockSize;
+  Bytes      = Blocks * BlockSize;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_READ_OPC;
+  CommandPacket.NvmeCmd->Nsid        = Device->NamespaceId;
+  CommandPacket.TransferBuffer       = (VOID *)(UINTN)Buffer;
+
+  CommandPacket.TransferLength = Bytes;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  CommandPacket.NvmeCmd->Cdw10 = (UINT32)Lba;
+  CommandPacket.NvmeCmd->Cdw11 = (UINT32)(Lba >> 32);
+  CommandPacket.NvmeCmd->Cdw12 = (Blocks - 1) & 0xFFFF;
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID | CDW12_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Write some sectors to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer to be written into the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be written.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+WriteSectors (
+  IN NVME_DEVICE_PRIVATE_DATA      *Device,
+  IN UINT64                        Buffer,
+  IN UINT64                        Lba,
+  IN UINT32                        Blocks
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT32                                   Bytes;
+  UINT32                                   BlockSize;
+
+  Private    = Device->Controller;
+  BlockSize  = Device->Media.BlockSize;
+  Bytes      = Blocks * BlockSize;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_WRITE_OPC;
+  CommandPacket.NvmeCmd->Nsid  = Device->NamespaceId;
+  CommandPacket.TransferBuffer = (VOID *)(UINTN)Buffer;
+
+  CommandPacket.TransferLength = Bytes;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  CommandPacket.NvmeCmd->Cdw10 = (UINT32)Lba;
+  CommandPacket.NvmeCmd->Cdw11 = (UINT32)(Lba >> 32);
+  CommandPacket.NvmeCmd->Cdw12 = (Blocks - 1) & 0xFFFF;
+
+  CommandPacket.MetadataBuffer = NULL;
+  CommandPacket.MetadataLength = 0;
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID | CDW12_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Read some blocks from the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer used to store the data read from the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be read.
+
+  @retval EFI_SUCCESS            Datum are read from the device.
+  @retval Others                 Fail to read all the datum.
+
+**/
+EFI_STATUS
+NvmeRead (
+  IN     NVME_DEVICE_PRIVATE_DATA       *Device,
+     OUT VOID                           *Buffer,
+  IN     UINT64                         Lba,
+  IN     UINTN                          Blocks
+  )
+{
+  EFI_STATUS                       Status;
+  UINT32                           BlockSize;
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  UINT32                           MaxTransferBlocks;
+  UINTN                            OrginalBlocks;
+
+  Status        = EFI_SUCCESS;
+  Private       = Device->Controller;
+  BlockSize     = Device->Media.BlockSize;
+  OrginalBlocks = Blocks;
+
+  if (Private->ControllerData->Mdts != 0) {
+    MaxTransferBlocks = (1 << (Private->ControllerData->Mdts)) * (1 << (Private->Cap.Mpsmin + 12)) / BlockSize;
+  } else {
+    MaxTransferBlocks = 1024;
+  }
+
+  while (Blocks > 0) {
+    if (Blocks > MaxTransferBlocks) {
+      Status = ReadSectors (Device, (UINT64)(UINTN)Buffer, Lba, MaxTransferBlocks);
+
+      Blocks -= MaxTransferBlocks;
+      Buffer  = (VOID *)(UINTN)((UINT64)(UINTN)Buffer + MaxTransferBlocks * BlockSize);
+      Lba    += MaxTransferBlocks;
+    } else {
+      Status = ReadSectors (Device, (UINT64)(UINTN)Buffer, Lba, (UINT32)Blocks);
+      Blocks = 0;
+    }
+
+    if (EFI_ERROR(Status)) {
+      break;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmeRead()  Lba = 0x%08x, Original = 0x%08x, Remaining = 0x%08x, BlockSize = 0x%x Status = %r\n", Lba, OrginalBlocks, Blocks, BlockSize, Status));
+
+  return Status;
+}
+
+/**
+  Write some blocks to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+  @param  Buffer                 The buffer to be written into the device.
+  @param  Lba                    The start block number.
+  @param  Blocks                 Total block number to be written.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+NvmeWrite (
+  IN NVME_DEVICE_PRIVATE_DATA           *Device,
+  IN VOID                               *Buffer,
+  IN UINT64                             Lba,
+  IN UINTN                              Blocks
+  )
+{
+  EFI_STATUS                       Status;
+  UINT32                           BlockSize;
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  UINT32                           MaxTransferBlocks;
+  UINTN                            OrginalBlocks;
+
+  Status        = EFI_SUCCESS;
+  Private       = Device->Controller;
+  BlockSize     = Device->Media.BlockSize;
+  OrginalBlocks = Blocks;
+
+  if (Private->ControllerData->Mdts != 0) {
+    MaxTransferBlocks = (1 << (Private->ControllerData->Mdts)) * (1 << (Private->Cap.Mpsmin + 12)) / BlockSize;
+  } else {
+    MaxTransferBlocks = 1024;
+  }
+
+  while (Blocks > 0) {
+    if (Blocks > MaxTransferBlocks) {
+      Status = WriteSectors (Device, (UINT64)(UINTN)Buffer, Lba, MaxTransferBlocks);
+
+      Blocks -= MaxTransferBlocks;
+      Buffer  = (VOID *)(UINTN)((UINT64)(UINTN)Buffer + MaxTransferBlocks * BlockSize);
+      Lba    += MaxTransferBlocks;
+    } else {
+      Status = WriteSectors (Device, (UINT64)(UINTN)Buffer, Lba, (UINT32)Blocks);
+      Blocks = 0;
+    }
+
+    if (EFI_ERROR(Status)) {
+      break;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "NvmeWrite() Lba = 0x%08x, Original = 0x%08x, Remaining = 0x%08x, BlockSize = 0x%x Status = %r\n", Lba, OrginalBlocks, Blocks, BlockSize, Status));
+
+  return Status;
+}
+
+/**
+  Flushes all modified data to the device.
+
+  @param  Device                 The pointer to the NVME_DEVICE_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS            Datum are written into the buffer.
+  @retval Others                 Fail to write all the datum.
+
+**/
+EFI_STATUS
+NvmeFlush (
+  IN NVME_DEVICE_PRIVATE_DATA      *Device
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA             *Private;
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  Private = Device->Controller;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  CommandPacket.NvmeCmd->Cdw0.Opcode = NVME_IO_FLUSH_OPC;
+  CommandPacket.NvmeCmd->Nsid  = Device->NamespaceId;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_IO_QUEUE;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               Device->NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL                         OldTpl;
+  NVME_CONTROLLER_PRIVATE_DATA    *Private;
+  NVME_DEVICE_PRIVATE_DATA        *Device;
+  EFI_STATUS                      Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For Nvm Express subsystem, reset block device means reset controller.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device  = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Private = Device->Controller;
+
+  Status  = NvmeControllerInit (Private);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Media = This->Media;
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeRead (Device, Buffer, Lba, NumberOfBlocks);
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_BLOCK_IO_MEDIA                *Media;
+  UINTN                             BlockSize;
+  UINTN                             NumberOfBlocks;
+  UINTN                             IoAlign;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Media = This->Media;
+
+  if (MediaId != Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  BlockSize = Media->BlockSize;
+  if ((BufferSize % BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IoAlign = Media->IoAlign;
+  if (IoAlign > 0 && (((UINTN) Buffer & (IoAlign - 1)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeWrite (Device, Buffer, Lba, NumberOfBlocks);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data.
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  NVME_DEVICE_PRIVATE_DATA          *Device;
+  EFI_STATUS                        Status;
+  EFI_TPL                           OldTpl;
+
+  //
+  // Check parameters.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_BLOCK_IO (This);
+
+  Status = NvmeFlush (Device);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Trust transfer data from/to NVMe device.
+
+  This function performs one NVMe transaction to do a trust transfer from/to NVMe device.
+
+  @param  Private                      The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer                       The pointer to the current transaction buffer.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  TransferLength               The block number or sector count of the transfer.
+  @param  IsTrustSend                  Indicates whether it is a trust send operation or not.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  TransferLengthOut            A pointer to a buffer to store the size in bytes of the data
+                                       written to the buffer. Ignore it when IsTrustSend is TRUE.
+
+  @retval EFI_SUCCESS       The data transfer is complete successfully.
+  @return others            Some error occurs when transferring data.
+
+**/
+EFI_STATUS
+TrustTransferNvmeDevice (
+  IN OUT NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN OUT VOID                              *Buffer,
+  IN UINT8                                 SecurityProtocolId,
+  IN UINT16                                SecurityProtocolSpecificData,
+  IN UINTN                                 TransferLength,
+  IN BOOLEAN                               IsTrustSend,
+  IN UINT64                                Timeout,
+  OUT UINTN                                *TransferLengthOut
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  UINT16                                   SpecificData;
+
+  ZeroMem (&CommandPacket, sizeof (EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof (EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof (EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  // 
+  // Change Endianness of SecurityProtocolSpecificData
+  //
+  SpecificData = (((SecurityProtocolSpecificData << 8) & 0xFF00) | (SecurityProtocolSpecificData >> 8));
+
+  if (IsTrustSend) {
+    Command.Cdw0.Opcode          = NVME_ADMIN_SECURITY_SEND_CMD;
+    CommandPacket.TransferBuffer = Buffer;
+    CommandPacket.TransferLength = (UINT32)TransferLength;
+    CommandPacket.NvmeCmd->Cdw10 = (UINT32)((SecurityProtocolId << 24) | (SpecificData << 8));
+    CommandPacket.NvmeCmd->Cdw11 = (UINT32)TransferLength;
+  } else {
+    Command.Cdw0.Opcode          = NVME_ADMIN_SECURITY_RECEIVE_CMD;
+    CommandPacket.TransferBuffer = Buffer;
+    CommandPacket.TransferLength = (UINT32)TransferLength;
+    CommandPacket.NvmeCmd->Cdw10 = (UINT32)((SecurityProtocolId << 24) | (SpecificData << 8));
+    CommandPacket.NvmeCmd->Cdw11 = (UINT32)TransferLength;
+  }
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+  CommandPacket.NvmeCmd->Nsid  = NVME_CONTROLLER_ID;
+  CommandPacket.CommandTimeout = Timeout;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NVME_CONTROLLER_ID,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  if (!IsTrustSend) {
+    if (EFI_ERROR (Status))  {
+      *TransferLengthOut = 0;
+    } else {
+      *TransferLengthOut = (UINTN) TransferLength;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecurityReceiveData (
+  IN  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN  UINT32                                   MediaId,
+  IN  UINT64                                   Timeout,
+  IN  UINT8                                    SecurityProtocolId,
+  IN  UINT16                                   SecurityProtocolSpecificData,
+  IN  UINTN                                    PayloadBufferSize,
+  OUT VOID                                     *PayloadBuffer,
+  OUT UINTN                                    *PayloadTransferSize
+  )
+{
+  EFI_STATUS                       Status;
+  NVME_DEVICE_PRIVATE_DATA         *Device;
+  
+  Status  = EFI_SUCCESS;
+
+  if ((PayloadBuffer == NULL) || (PayloadTransferSize == NULL) || (PayloadBufferSize == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Device->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Device->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  Status = TrustTransferNvmeDevice (
+             Device->Controller,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             FALSE,
+             Timeout,
+             PayloadTransferSize
+             );
+
+  return Status;
+}
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the send data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  )
+{
+  EFI_STATUS                       Status; 
+  NVME_DEVICE_PRIVATE_DATA         *Device;
+
+  Status  = EFI_SUCCESS;
+
+  if ((PayloadBuffer == NULL) && (PayloadBufferSize != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_STORAGE_SECURITY (This);
+
+  if (MediaId != Device->BlockIo.Media->MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  if (!Device->BlockIo.Media->MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  Status = TrustTransferNvmeDevice (
+             Device->Controller,
+             PayloadBuffer,
+             SecurityProtocolId,
+             SecurityProtocolSpecificData,
+             PayloadBufferSize,
+             TRUE,
+             Timeout,
+             NULL
+             );
+
+  return Status;
+}
+
+
+
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h
new file mode 100644
index 0000000000..1c71a81ec5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressBlockIo.h
@@ -0,0 +1,269 @@
+/** @file
+  Header file for EFI_BLOCK_IO_PROTOCOL interface.
+
+Copyright (c) 2013 - 2015, 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.
+
+**/
+
+#ifndef _EFI_NVME_BLOCKIO_H_
+#define _EFI_NVME_BLOCKIO_H_
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the destination buffer for the data. The caller is
+                     responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    The media ID that the write request is for.
+  @param  Lba        The starting logical block address to be written. The caller is
+                     responsible for writing to only legitimate locations.
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid,
+                                or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param  This              Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writing back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeBlockIoFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+/**
+  Send a security protocol command to a device that receives data and/or the result
+  of one or more commands sent by SendData.
+
+  The ReceiveData function sends a security protocol command to the given MediaId.
+  The security protocol command sent is defined by SecurityProtocolId and contains
+  the security protocol specific data SecurityProtocolSpecificData. The function
+  returns the data from the security protocol command in PayloadBuffer.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL IN command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED RECEIVE commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero.
+
+  If the PayloadBufferSize is zero, the security protocol command is sent using the
+  Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBufferSize is too small to store the available data from the security
+  protocol command, the function shall copy PayloadBufferSize bytes into the
+  PayloadBuffer and return EFI_WARN_BUFFER_TOO_SMALL.
+
+  If PayloadBuffer or PayloadTransferSize is NULL and PayloadBufferSize is non-zero,
+  the function shall return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function shall
+  return EFI_UNSUPPORTED. If there is no media in the device, the function returns
+  EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the device,
+  the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall
+  return EFI_SUCCESS. If the security protocol command completes with an error, the
+  function shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command. The caller is responsible for having
+                                       either implicit or explicit ownership of the buffer.
+  @param  PayloadTransferSize          A pointer to a buffer to store the size in bytes of the
+                                       data written to the payload data buffer.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL    The PayloadBufferSize was too small to store the available
+                                       data from the device. The PayloadBuffer contains the truncated data.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer or PayloadTransferSize is NULL and
+                                       PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecurityReceiveData (
+  IN  EFI_STORAGE_SECURITY_COMMAND_PROTOCOL   *This,
+  IN  UINT32                                  MediaId,
+  IN  UINT64                                  Timeout,
+  IN  UINT8                                   SecurityProtocolId,
+  IN  UINT16                                  SecurityProtocolSpecificData,
+  IN  UINTN                                   PayloadBufferSize,
+  OUT VOID                                    *PayloadBuffer,
+  OUT UINTN                                   *PayloadTransferSize
+  );
+
+/**
+  Send a security protocol command to a device.
+
+  The SendData function sends a security protocol command containing the payload
+  PayloadBuffer to the given MediaId. The security protocol command sent is
+  defined by SecurityProtocolId and contains the security protocol specific data
+  SecurityProtocolSpecificData. If the underlying protocol command requires a
+  specific padding for the command payload, the SendData function shall add padding
+  bytes to the command payload to satisfy the padding requirements.
+
+  For devices supporting the SCSI command set, the security protocol command is sent
+  using the SECURITY PROTOCOL OUT command defined in SPC-4.
+
+  For devices supporting the ATA command set, the security protocol command is sent
+  using one of the TRUSTED SEND commands defined in ATA8-ACS if PayloadBufferSize
+  is non-zero. If the PayloadBufferSize is zero, the security protocol command is
+  sent using the Trusted Non-Data command defined in ATA8-ACS.
+
+  If PayloadBuffer is NULL and PayloadBufferSize is non-zero, the function shall
+  return EFI_INVALID_PARAMETER.
+
+  If the given MediaId does not support security protocol commands, the function
+  shall return EFI_UNSUPPORTED. If there is no media in the device, the function
+  returns EFI_NO_MEDIA. If the MediaId is not the ID for the current media in the
+  device, the function returns EFI_MEDIA_CHANGED.
+
+  If the security protocol fails to complete within the Timeout period, the function
+  shall return EFI_TIMEOUT.
+
+  If the security protocol command completes without an error, the function shall return
+  EFI_SUCCESS. If the security protocol command completes with an error, the function
+  shall return EFI_DEVICE_ERROR.
+
+  @param  This                         Indicates a pointer to the calling context.
+  @param  MediaId                      ID of the medium to receive data from.
+  @param  Timeout                      The timeout, in 100ns units, to use for the execution
+                                       of the security protocol command. A Timeout value of 0
+                                       means that this function will wait indefinitely for the
+                                       security protocol command to execute. If Timeout is greater
+                                       than zero, then this function will return EFI_TIMEOUT
+                                       if the time required to execute the receive data command
+                                       is greater than Timeout.
+  @param  SecurityProtocolId           The value of the "Security Protocol" parameter of
+                                       the security protocol command to be sent.
+  @param  SecurityProtocolSpecificData The value of the "Security Protocol Specific" parameter
+                                       of the security protocol command to be sent.
+  @param  PayloadBufferSize            Size in bytes of the payload data buffer.
+  @param  PayloadBuffer                A pointer to a destination buffer to store the security
+                                       protocol command specific payload data for the security
+                                       protocol command.
+
+  @retval EFI_SUCCESS                  The security protocol command completed successfully.
+  @retval EFI_UNSUPPORTED              The given MediaId does not support security protocol commands.
+  @retval EFI_DEVICE_ERROR             The security protocol command completed with an error.
+  @retval EFI_NO_MEDIA                 There is no media in the device.
+  @retval EFI_MEDIA_CHANGED            The MediaId is not for the current media.
+  @retval EFI_INVALID_PARAMETER        The PayloadBuffer is NULL and PayloadBufferSize is non-zero.
+  @retval EFI_TIMEOUT                  A timeout occurred while waiting for the security
+                                       protocol command to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmeStorageSecuritySendData (
+  IN EFI_STORAGE_SECURITY_COMMAND_PROTOCOL    *This,
+  IN UINT32                                   MediaId,
+  IN UINT64                                   Timeout,
+  IN UINT8                                    SecurityProtocolId,
+  IN UINT16                                   SecurityProtocolSpecificData,
+  IN UINTN                                    PayloadBufferSize,
+  IN VOID                                     *PayloadBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c
new file mode 100644
index 0000000000..ea108a851d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.c
@@ -0,0 +1,162 @@
+/** @file
+  This file is used to implement the EFI_DISK_INFO_PROTOCOL interface..
+
+  Copyright (c) 2013, 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 "NvmExpress.h"
+
+EFI_DISK_INFO_PROTOCOL gNvmExpressDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_NVME_INTERFACE_GUID,
+  NvmExpressDiskInfoInquiry,
+  NvmExpressDiskInfoIdentify,
+  NvmExpressDiskInfoSenseData,
+  NvmExpressDiskInfoWhichIde
+};
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with NVME interface GUID.
+
+  @param[in]  Device  The pointer of NVME_DEVICE_PRIVATE_DATA.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  NVME_DEVICE_PRIVATE_DATA    *Device
+  )
+{
+  CopyMem (&Device->DiskInfo, &gNvmExpressDiskInfoProtocolTemplate, sizeof (EFI_DISK_INFO_PROTOCOL));
+}
+
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS                      Status;
+  NVME_DEVICE_PRIVATE_DATA        *Device;
+
+  Device = NVME_DEVICE_PRIVATE_DATA_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (Device->NamespaceData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, &Device->NamespaceData, sizeof (Device->NamespaceData));
+  }
+  *IdentifyDataSize = sizeof (Device->NamespaceData);
+  return Status;
+}
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h
new file mode 100644
index 0000000000..f07cfe3474
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDiskInfo.h
@@ -0,0 +1,129 @@
+/** @file
+  Header file for EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2013, 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.
+
+**/
+
+#ifndef _NVME_DISKINFO_H_
+#define _NVME_DISKINFO_H_
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with NVME interface GUID.
+
+  @param[in]  Device  The pointer of NVME_DEVICE_PRIVATE_DATA.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  NVME_DEVICE_PRIVATE_DATA    *Device
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+/**
+  Provides sense data information for the controller type.
+
+  This function is used to get sense data.
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf
new file mode 100644
index 0000000000..00acf2b5cf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf
@@ -0,0 +1,79 @@
+## @file
+#  NVM Express Host Controller Module.
+#
+#  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+#  NVM Express specification.
+#
+#  Copyright (c) 2013 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = NvmExpressDxe
+  MODULE_UNI_FILE                = NvmExpressDxe.uni
+  FILE_GUID                      = 5BE3BDF4-53CF-46a3-A6A9-73C34A6E5EE3
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = NvmExpressDriverEntry
+  UNLOAD_IMAGE                   = NvmExpressUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gNvmExpressDriverBinding
+#  COMPONENT_NAME                =  gNvmExpressComponentName
+#  COMPONENT_NAME2               =  gNvmExpressComponentName2
+
+[Sources]
+  NvmExpressBlockIo.c
+  NvmExpressBlockIo.h
+  ComponentName.c
+  NvmExpress.c
+  NvmExpress.h
+  NvmExpressDiskInfo.c
+  NvmExpressDiskInfo.h
+  NvmExpressHci.c
+  NvmExpressHci.h
+  NvmExpressPassthru.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  DevicePathLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiLib
+  PrintLib
+
+[Protocols]
+  gEfiPciIoProtocolGuid                       ## TO_START
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiNvmExpressPassThruProtocolGuid          ## BY_START
+  gEfiBlockIoProtocolGuid                     ## BY_START
+  gEfiDiskInfoProtocolGuid                    ## BY_START
+  gEfiStorageSecurityCommandProtocolGuid      ## BY_START
+  gEfiDriverSupportedEfiVersionProtocolGuid   ## PRODUCES
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER ## SOMETIMES_CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  NvmExpressDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni
new file mode 100644
index 0000000000..36fbc2acfe
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni
new file mode 100644
index 0000000000..2f8bf5922d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c
new file mode 100644
index 0000000000..f6b6288f65
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c
@@ -0,0 +1,981 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Read Nvm Express controller capability register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cap              The buffer used to store capability register content.
+
+  @return EFI_SUCCESS      Successfully read the controller capability register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller capability register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerCapabilities (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CAP                         *Cap
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CAP_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Cap, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express controller configuration register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cc               The buffer used to store configuration register content.
+
+  @return EFI_SUCCESS      Successfully read the controller configuration register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller configuration register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerConfiguration (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CC                          *Cc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CC_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Cc, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express controller configuration register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Cc               The buffer used to store the content to be written into configuration register.
+
+  @return EFI_SUCCESS      Successfully write data into the controller configuration register.
+  @return EFI_DEVICE_ERROR Fail to write data into the controller configuration register.
+
+**/
+EFI_STATUS
+WriteNvmeControllerConfiguration (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CC                          *Cc
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned32 ((UINT32*)Cc);
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CC_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Cc.En: %d\n", Cc->En));
+  DEBUG ((EFI_D_INFO, "Cc.Css: %d\n", Cc->Css));
+  DEBUG ((EFI_D_INFO, "Cc.Mps: %d\n", Cc->Mps));
+  DEBUG ((EFI_D_INFO, "Cc.Ams: %d\n", Cc->Ams));
+  DEBUG ((EFI_D_INFO, "Cc.Shn: %d\n", Cc->Shn));
+  DEBUG ((EFI_D_INFO, "Cc.Iosqes: %d\n", Cc->Iosqes));
+  DEBUG ((EFI_D_INFO, "Cc.Iocqes: %d\n", Cc->Iocqes));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express controller status register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Csts             The buffer used to store status register content.
+
+  @return EFI_SUCCESS      Successfully read the controller status register content.
+  @return EFI_DEVICE_ERROR Fail to read the controller status register.
+
+**/
+EFI_STATUS
+ReadNvmeControllerStatus (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_CSTS                        *Csts
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_CSTS_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Csts, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin queue attributes register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Aqa              The buffer used to store admin queue attributes register content.
+
+  @return EFI_SUCCESS      Successfully read the admin queue attributes register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin queue attributes register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminQueueAttributes (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_AQA                         *Aqa
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_AQA_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned32 ((UINT32*)Aqa, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin queue attributes register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Aqa              The buffer used to store the content to be written into admin queue attributes register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin queue attributes register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin queue attributes register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminQueueAttributes (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_AQA                         *Aqa
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT32                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned32 ((UINT32*)Aqa);
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_AQA_OFFSET,
+                        1,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Aqa.Asqs: %d\n", Aqa->Asqs));
+  DEBUG ((EFI_D_INFO, "Aqa.Acqs: %d\n", Aqa->Acqs));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin submission queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Asq              The buffer used to store admin submission queue base address register content.
+
+  @return EFI_SUCCESS      Successfully read the admin submission queue base address register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin submission queue base address register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminSubmissionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ASQ                         *Asq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ASQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Asq, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin submission queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Asq              The buffer used to store the content to be written into admin submission queue base address register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin submission queue base address register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin submission queue base address register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminSubmissionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ASQ                         *Asq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned64 ((UINT64*)Asq);
+
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ASQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Asq.Asqb: %lx\n", Asq->Asqb));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Nvm Express admin completion queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Acq              The buffer used to store admin completion queue base address register content.
+
+  @return EFI_SUCCESS      Successfully read the admin completion queue base address register content.
+  @return EFI_DEVICE_ERROR Fail to read the admin completion queue base address register.
+
+**/
+EFI_STATUS
+ReadNvmeAdminCompletionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ACQ                         *Acq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+
+  Status = PciIo->Mem.Read (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ACQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  WriteUnaligned64 ((UINT64*)Acq, Data);
+  return EFI_SUCCESS;
+}
+
+/**
+  Write Nvm Express admin completion queue base address register.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Acq              The buffer used to store the content to be written into admin completion queue base address register.
+
+  @return EFI_SUCCESS      Successfully write data into the admin completion queue base address register.
+  @return EFI_DEVICE_ERROR Fail to write data into the admin completion queue base address register.
+
+**/
+EFI_STATUS
+WriteNvmeAdminCompletionQueueBaseAddress (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private,
+  IN NVME_ACQ                         *Acq
+  )
+{
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  EFI_STATUS            Status;
+  UINT64                Data;
+
+  PciIo  = Private->PciIo;
+  Data   = ReadUnaligned64 ((UINT64*)Acq);
+
+  Status = PciIo->Mem.Write (
+                        PciIo,
+                        EfiPciIoWidthUint32,
+                        NVME_BAR,
+                        NVME_ACQ_OFFSET,
+                        2,
+                        &Data
+                        );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Acq.Acqb: %lxh\n", Acq->Acqb));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Disable the Nvm Express controller.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully disable the controller.
+  @return EFI_DEVICE_ERROR Fail to disable the controller.
+
+**/
+EFI_STATUS
+NvmeDisableController (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private
+  )
+{
+  NVME_CC                Cc;
+  NVME_CSTS              Csts;
+  EFI_STATUS             Status;
+  UINT32                 Index;
+  UINT8                  Timeout;
+
+  //
+  // Read Controller Configuration Register.
+  //
+  Status = ReadNvmeControllerConfiguration (Private, &Cc);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Cc.En = 0;
+
+  //
+  // Disable the controller.
+  //
+  Status = WriteNvmeControllerConfiguration (Private, &Cc);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Cap.To specifies max delay time in 500ms increments for Csts.Rdy to transition from 1 to 0 after
+  // Cc.Enable transition from 1 to 0. Loop produces a 1 millisecond delay per itteration, up to 500 * Cap.To.
+  //
+  if (Private->Cap.To == 0) {
+    Timeout = 1;
+  } else {
+    Timeout = Private->Cap.To;
+  }
+
+  for(Index = (Timeout * 500); Index != 0; --Index) {
+    gBS->Stall(1000);
+
+    //
+    // Check if the controller is initialized
+    //
+    Status = ReadNvmeControllerStatus (Private, &Csts);
+
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    if (Csts.Rdy == 0) {
+      break;
+    }
+  }
+
+  if (Index == 0) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "NVMe controller is disabled with status [%r].\n", Status));
+  return Status;
+}
+
+/**
+  Enable the Nvm Express controller.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully enable the controller.
+  @return EFI_DEVICE_ERROR Fail to enable the controller.
+  @return EFI_TIMEOUT      Fail to enable the controller in given time slot.
+
+**/
+EFI_STATUS
+NvmeEnableController (
+  IN NVME_CONTROLLER_PRIVATE_DATA     *Private
+  )
+{
+  NVME_CC                Cc;
+  NVME_CSTS              Csts;
+  EFI_STATUS             Status;
+  UINT32                 Index;
+  UINT8                  Timeout;
+
+  //
+  // Enable the controller.
+  // CC.AMS, CC.MPS and CC.CSS are all set to 0.
+  //
+  ZeroMem (&Cc, sizeof (NVME_CC));
+  Cc.En     = 1;
+  Cc.Iosqes = 6;
+  Cc.Iocqes = 4;
+
+  Status = WriteNvmeControllerConfiguration (Private, &Cc);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Cap.To specifies max delay time in 500ms increments for Csts.Rdy to set after
+  // Cc.Enable. Loop produces a 1 millisecond delay per itteration, up to 500 * Cap.To.
+  //
+  if (Private->Cap.To == 0) {
+    Timeout = 1;
+  } else {
+    Timeout = Private->Cap.To;
+  }
+
+  for(Index = (Timeout * 500); Index != 0; --Index) {
+    gBS->Stall(1000);
+
+    //
+    // Check if the controller is initialized
+    //
+    Status = ReadNvmeControllerStatus (Private, &Csts);
+
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    if (Csts.Rdy) {
+      break;
+    }
+  }
+
+  if (Index == 0) {
+    Status = EFI_TIMEOUT;
+  }
+
+  DEBUG ((EFI_D_INFO, "NVMe controller is enabled with status [%r].\n", Status));
+  return Status;
+}
+
+/**
+  Get identify controller data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer           The buffer used to store the identify controller data.
+
+  @return EFI_SUCCESS      Successfully get the identify controller data.
+  @return EFI_DEVICE_ERROR Fail to get the identify controller data.
+
+**/
+EFI_STATUS
+NvmeIdentifyController (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  IN VOID                               *Buffer
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  Command.Cdw0.Opcode = NVME_ADMIN_IDENTIFY_CMD;
+  //
+  // According to Nvm Express 1.1 spec Figure 38, When not used, the field shall be cleared to 0h.
+  // For the Identify command, the Namespace Identifier is only used for the Namespace data structure.
+  //
+  Command.Nsid        = 0;
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+  CommandPacket.TransferBuffer = Buffer;
+  CommandPacket.TransferLength = sizeof (NVME_ADMIN_CONTROLLER_DATA);
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+  //
+  // Set bit 0 (Cns bit) to 1 to identify a controller
+  //
+  Command.Cdw10                = 1;
+  Command.Flags                = CDW10_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NVME_CONTROLLER_ID,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Get specified identify namespace data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  NamespaceId      The specified namespace identifier.
+  @param  Buffer           The buffer used to store the identify namespace data.
+
+  @return EFI_SUCCESS      Successfully get the identify namespace data.
+  @return EFI_DEVICE_ERROR Fail to get the identify namespace data.
+
+**/
+EFI_STATUS
+NvmeIdentifyNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN UINT32                            NamespaceId,
+  IN VOID                              *Buffer
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_IDENTIFY_CMD;
+  Command.Nsid        = NamespaceId;
+  CommandPacket.TransferBuffer = Buffer;
+  CommandPacket.TransferLength = sizeof (NVME_ADMIN_NAMESPACE_DATA);
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+  //
+  // Set bit 0 (Cns bit) to 1 to identify a namespace
+  //
+  CommandPacket.NvmeCmd->Cdw10 = 0;
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               NamespaceId,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Create io completion queue.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully create io completion queue.
+  @return EFI_DEVICE_ERROR Fail to create io completion queue.
+
+**/
+EFI_STATUS
+NvmeCreateIoCompletionQueue (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  NVME_ADMIN_CRIOCQ                        CrIoCq;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+  ZeroMem (&CrIoCq, sizeof(NVME_ADMIN_CRIOCQ));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_CRIOCQ_CMD;
+  CommandPacket.TransferBuffer = Private->CqBufferPciAddr[1];
+  CommandPacket.TransferLength = EFI_PAGE_SIZE;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  CrIoCq.Qid   = NVME_IO_QUEUE;
+  CrIoCq.Qsize = NVME_CCQ_SIZE;
+  CrIoCq.Pc    = 1;
+  CopyMem (&CommandPacket.NvmeCmd->Cdw10, &CrIoCq, sizeof (NVME_ADMIN_CRIOCQ));
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               0,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Create io submission queue.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @return EFI_SUCCESS      Successfully create io submission queue.
+  @return EFI_DEVICE_ERROR Fail to create io submission queue.
+
+**/
+EFI_STATUS
+NvmeCreateIoSubmissionQueue (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                  Command;
+  EFI_NVM_EXPRESS_COMPLETION               Completion;
+  EFI_STATUS                               Status;
+  NVME_ADMIN_CRIOSQ                        CrIoSq;
+
+  ZeroMem (&CommandPacket, sizeof(EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof(EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+  ZeroMem (&CrIoSq, sizeof(NVME_ADMIN_CRIOSQ));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+
+  Command.Cdw0.Opcode = NVME_ADMIN_CRIOSQ_CMD;
+  CommandPacket.TransferBuffer = Private->SqBufferPciAddr[1];
+  CommandPacket.TransferLength = EFI_PAGE_SIZE;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+
+  CrIoSq.Qid   = NVME_IO_QUEUE;
+  CrIoSq.Qsize = NVME_CSQ_SIZE;
+  CrIoSq.Pc    = 1;
+  CrIoSq.Cqid  = NVME_IO_QUEUE;
+  CrIoSq.Qprio = 0;
+  CopyMem (&CommandPacket.NvmeCmd->Cdw10, &CrIoSq, sizeof (NVME_ADMIN_CRIOSQ));
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               0,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  return Status;
+}
+
+/**
+  Initialize the Nvm Express controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+NvmeControllerInit (
+  IN NVME_CONTROLLER_PRIVATE_DATA    *Private
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  UINT64                          Supports;
+  NVME_AQA                        Aqa;
+  NVME_ASQ                        Asq;
+  NVME_ACQ                        Acq;
+
+  //
+  // Save original PCI attributes and enable this controller.
+  //
+  PciIo  = Private->PciIo;
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &Private->PciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status    = PciIo->Attributes (
+                         PciIo,
+                         EfiPciIoAttributeOperationEnable,
+                         Supports,
+                         NULL
+                         );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "NvmeControllerInit: failed to enable controller\n"));
+    return Status;
+  }
+
+  //
+  // Read the Controller Capabilities register and verify that the NVM command set is supported
+  //
+  Status = ReadNvmeControllerCapabilities (Private, &Private->Cap);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Private->Cap.Css != 0x01) {
+    DEBUG ((EFI_D_INFO, "NvmeControllerInit: the controller doesn't support NVMe command set\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Currently the driver only supports 4k page size.
+  //
+  ASSERT ((Private->Cap.Mpsmin + 12) <= EFI_PAGE_SHIFT);
+
+  Private->Cid[0] = 0;
+  Private->Cid[1] = 0;
+
+  Status = NvmeDisableController (Private);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // set number of entries admin submission & completion queues.
+  //
+  Aqa.Asqs  = NVME_ASQ_SIZE;
+  Aqa.Rsvd1 = 0;
+  Aqa.Acqs  = NVME_ACQ_SIZE;
+  Aqa.Rsvd2 = 0;
+
+  //
+  // Address of admin submission queue.
+  //
+  Asq.Rsvd1 = 0;
+  Asq.Asqb  = (UINT64)(UINTN)(Private->BufferPciAddr) >> 12;
+
+  //
+  // Address of admin completion queue.
+  //
+  Acq.Rsvd1 = 0;
+  Acq.Acqb  = (UINT64)(UINTN)(Private->BufferPciAddr + EFI_PAGE_SIZE) >> 12;
+
+  //
+  // Address of I/O submission & completion queue.
+  //
+  Private->SqBuffer[0]        = (NVME_SQ *)(UINTN)(Private->Buffer);
+  Private->SqBufferPciAddr[0] = (NVME_SQ *)(UINTN)(Private->BufferPciAddr);
+  Private->CqBuffer[0]        = (NVME_CQ *)(UINTN)(Private->Buffer + 1 * EFI_PAGE_SIZE);
+  Private->CqBufferPciAddr[0] = (NVME_CQ *)(UINTN)(Private->BufferPciAddr + 1 * EFI_PAGE_SIZE);
+  Private->SqBuffer[1]        = (NVME_SQ *)(UINTN)(Private->Buffer + 2 * EFI_PAGE_SIZE);
+  Private->SqBufferPciAddr[1] = (NVME_SQ *)(UINTN)(Private->BufferPciAddr + 2 * EFI_PAGE_SIZE);
+  Private->CqBuffer[1]        = (NVME_CQ *)(UINTN)(Private->Buffer + 3 * EFI_PAGE_SIZE);
+  Private->CqBufferPciAddr[1] = (NVME_CQ *)(UINTN)(Private->BufferPciAddr + 3 * EFI_PAGE_SIZE);
+
+  DEBUG ((EFI_D_INFO, "Private->Buffer = [%016X]\n", (UINT64)(UINTN)Private->Buffer));
+  DEBUG ((EFI_D_INFO, "Admin Submission Queue size (Aqa.Asqs) = [%08X]\n", Aqa.Asqs));
+  DEBUG ((EFI_D_INFO, "Admin Completion Queue size (Aqa.Acqs) = [%08X]\n", Aqa.Acqs));
+  DEBUG ((EFI_D_INFO, "Admin Submission Queue (SqBuffer[0]) = [%016X]\n", Private->SqBuffer[0]));
+  DEBUG ((EFI_D_INFO, "Admin Completion Queue (CqBuffer[0]) = [%016X]\n", Private->CqBuffer[0]));
+  DEBUG ((EFI_D_INFO, "I/O   Submission Queue (SqBuffer[1]) = [%016X]\n", Private->SqBuffer[1]));
+  DEBUG ((EFI_D_INFO, "I/O   Completion Queue (CqBuffer[1]) = [%016X]\n", Private->CqBuffer[1]));
+
+  //
+  // Program admin queue attributes.
+  //
+  Status = WriteNvmeAdminQueueAttributes (Private, &Aqa);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Program admin submission queue address.
+  //
+  Status = WriteNvmeAdminSubmissionQueueBaseAddress (Private, &Asq);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Program admin completion queue address.
+  //
+  Status = WriteNvmeAdminCompletionQueueBaseAddress (Private, &Acq);
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Status = NvmeEnableController (Private);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Allocate buffer for Identify Controller data
+  //
+  Private->ControllerData = (NVME_ADMIN_CONTROLLER_DATA *)AllocateZeroPool (sizeof(NVME_ADMIN_CONTROLLER_DATA));
+
+  if (Private->ControllerData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Get current Identify Controller Data
+  //
+  Status = NvmeIdentifyController (Private, Private->ControllerData);
+
+  if (EFI_ERROR(Status)) {
+    FreePool(Private->ControllerData);
+    Private->ControllerData = NULL;
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Dump NvmExpress Identify Controller Data
+  //
+  Private->ControllerData->Sn[19] = 0;
+  Private->ControllerData->Mn[39] = 0;
+  DEBUG ((EFI_D_INFO, " == NVME IDENTIFY CONTROLLER DATA ==\n"));
+  DEBUG ((EFI_D_INFO, "    PCI VID   : 0x%x\n", Private->ControllerData->Vid));
+  DEBUG ((EFI_D_INFO, "    PCI SSVID : 0x%x\n", Private->ControllerData->Ssvid));
+  DEBUG ((EFI_D_INFO, "    SN        : %a\n",   (CHAR8 *)(Private->ControllerData->Sn)));
+  DEBUG ((EFI_D_INFO, "    MN        : %a\n",   (CHAR8 *)(Private->ControllerData->Mn)));
+  DEBUG ((EFI_D_INFO, "    FR        : 0x%x\n", *((UINT64*)Private->ControllerData->Fr)));
+  DEBUG ((EFI_D_INFO, "    RAB       : 0x%x\n", Private->ControllerData->Rab));
+  DEBUG ((EFI_D_INFO, "    IEEE      : 0x%x\n", *(UINT32*)Private->ControllerData->Ieee_oui));
+  DEBUG ((EFI_D_INFO, "    AERL      : 0x%x\n", Private->ControllerData->Aerl));
+  DEBUG ((EFI_D_INFO, "    SQES      : 0x%x\n", Private->ControllerData->Sqes));
+  DEBUG ((EFI_D_INFO, "    CQES      : 0x%x\n", Private->ControllerData->Cqes));
+  DEBUG ((EFI_D_INFO, "    NN        : 0x%x\n", Private->ControllerData->Nn));
+
+  //
+  // Create one I/O completion queue.
+  //
+  Status = NvmeCreateIoCompletionQueue (Private);
+  if (EFI_ERROR(Status)) {
+   return Status;
+  }
+
+  //
+  // Create one I/O Submission queue.
+  //
+  Status = NvmeCreateIoSubmissionQueue (Private);
+  if (EFI_ERROR(Status)) {
+   return Status;
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h
new file mode 100644
index 0000000000..06c1db7d8e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h
@@ -0,0 +1,809 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  Copyright (c) 2013 - 2015, 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.
+
+**/
+
+#ifndef _NVME_HCI_H_
+#define _NVME_HCI_H_
+
+#define NVME_BAR                 0
+
+//
+// controller register offsets
+//
+#define NVME_CAP_OFFSET          0x0000  // Controller Capabilities
+#define NVME_VER_OFFSET          0x0008  // Version
+#define NVME_INTMS_OFFSET        0x000c  // Interrupt Mask Set
+#define NVME_INTMC_OFFSET        0x0010  // Interrupt Mask Clear
+#define NVME_CC_OFFSET           0x0014  // Controller Configuration
+#define NVME_CSTS_OFFSET         0x001c  // Controller Status
+#define NVME_NSSR_OFFSET         0x0020  // NVM Subsystem Reset
+#define NVME_AQA_OFFSET          0x0024  // Admin Queue Attributes
+#define NVME_ASQ_OFFSET          0x0028  // Admin Submission Queue Base Address
+#define NVME_ACQ_OFFSET          0x0030  // Admin Completion Queue Base Address
+#define NVME_SQ0_OFFSET          0x1000  // Submission Queue 0 (admin) Tail Doorbell
+#define NVME_CQ0_OFFSET          0x1004  // Completion Queue 0 (admin) Head Doorbell
+
+//
+// These register offsets are defined as 0x1000 + (N * (4 << CAP.DSTRD))
+// Get the doorbell stride bit shift value from the controller capabilities.
+//
+#define NVME_SQTDBL_OFFSET(QID, DSTRD)    0x1000 + ((2 * (QID)) * (4 << (DSTRD)))       // Submission Queue y (NVM) Tail Doorbell
+#define NVME_CQHDBL_OFFSET(QID, DSTRD)    0x1000 + (((2 * (QID)) + 1) * (4 << (DSTRD))) // Completion Queue y (NVM) Head Doorbell
+
+
+#pragma pack(1)
+
+//
+// 3.1.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT16 Mqes;      // Maximum Queue Entries Supported
+  UINT8  Cqr:1;     // Contiguous Queues Required
+  UINT8  Ams:2;     // Arbitration Mechanism Supported
+  UINT8  Rsvd1:5;
+  UINT8  To;        // Timeout
+  UINT16 Dstrd:4;
+  UINT16 Nssrs:1;   // NVM Subsystem Reset Supported NSSRS
+  UINT16 Css:4;     // Command Sets Supported - Bit 37
+  UINT16 Rsvd3:7;
+  UINT8  Mpsmin:4;
+  UINT8  Mpsmax:4;
+  UINT8  Rsvd4;
+} NVME_CAP;
+
+//
+// 3.1.2 Offset 08h: VS - Version
+//
+typedef struct {
+  UINT16 Mnr;       // Minor version number
+  UINT16 Mjr;       // Major version number
+} NVME_VER;
+
+//
+// 3.1.5 Offset 14h: CC - Controller Configuration
+//
+typedef struct {
+  UINT16 En:1;       // Enable
+  UINT16 Rsvd1:3;
+  UINT16 Css:3;      // I/O Command Set Selected
+  UINT16 Mps:4;      // Memory Page Size
+  UINT16 Ams:3;      // Arbitration Mechanism Selected
+  UINT16 Shn:2;      // Shutdown Notification
+  UINT8  Iosqes:4;   // I/O Submission Queue Entry Size
+  UINT8  Iocqes:4;   // I/O Completion Queue Entry Size
+  UINT8  Rsvd2;
+} NVME_CC;
+
+//
+// 3.1.6 Offset 1Ch: CSTS - Controller Status
+//
+typedef struct {
+  UINT32 Rdy:1;      // Ready
+  UINT32 Cfs:1;      // Controller Fatal Status
+  UINT32 Shst:2;     // Shutdown Status
+  UINT32 Nssro:1;    // NVM Subsystem Reset Occurred
+  UINT32 Rsvd1:27;
+} NVME_CSTS;
+
+//
+// 3.1.8 Offset 24h: AQA - Admin Queue Attributes
+//
+typedef struct {
+  UINT16 Asqs:12;    // Submission Queue Size
+  UINT16 Rsvd1:4;
+  UINT16 Acqs:12;    // Completion Queue Size
+  UINT16 Rsvd2:4;
+} NVME_AQA;
+
+//
+// 3.1.9 Offset 28h: ASQ - Admin Submission Queue Base Address
+//
+typedef struct {
+  UINT64 Rsvd1:12;
+  UINT64 Asqb:52;    // Admin Submission Queue Base Address
+} NVME_ASQ;
+
+//
+// 3.1.10 Offset 30h: ACQ - Admin Completion Queue Base Address
+//
+typedef struct {
+  UINT64 Rsvd1:12;
+  UINT64 Acqb:52;    // Admin Completion Queue Base Address
+} NVME_ACQ;
+
+//
+// 3.1.11 Offset (1000h + ((2y) * (4 << CAP.DSTRD))): SQyTDBL - Submission Queue y Tail Doorbell
+//
+typedef struct {
+  UINT16 Sqt;
+  UINT16 Rsvd1;
+} NVME_SQTDBL;
+
+//
+// 3.1.12 Offset (1000h + ((2y + 1) * (4 << CAP.DSTRD))): CQyHDBL - Completion Queue y Head Doorbell
+//
+typedef struct {
+  UINT16 Cqh;
+  UINT16 Rsvd1;
+} NVME_CQHDBL;
+
+//
+// NVM command set structures
+//
+// Read Command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting Sector Address */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Sectors */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Af:4;                /* Access Frequency */
+  UINT32 Al:2;                /* Access Latency */
+  UINT32 Sr:1;                /* Sequential Request */
+  UINT32 In:1;                /* Incompressible */
+  UINT32 Rsvd2:24;
+  //
+  // CDW 14
+  //
+  UINT32 Eilbrt;              /* Expected Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Elbat;               /* Expected Logical Block Application Tag */
+  UINT16 Elbatm;              /* Expected Logical Block Application Tag Mask */
+} NVME_READ;
+
+//
+// Write Command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting Sector Address */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Sectors */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Af:4;                /* Access Frequency */
+  UINT32 Al:2;                /* Access Latency */
+  UINT32 Sr:1;                /* Sequential Request */
+  UINT32 In:1;                /* Incompressible */
+  UINT32 Rsvd2:24;
+  //
+  // CDW 14
+  //
+  UINT32 Ilbrt;               /* Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Lbat;                /* Logical Block Application Tag */
+  UINT16 Lbatm;               /* Logical Block Application Tag Mask */
+} NVME_WRITE;
+
+//
+// Flush
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Flush;               /* Flush */
+} NVME_FLUSH;
+
+//
+// Write Uncorrectable command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT32 Nlb:16;              /* Number of  Logical Blocks */
+  UINT32 Rsvd1:16;
+} NVME_WRITE_UNCORRECTABLE;
+
+//
+// Write Zeroes command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Logical Blocks */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Rsvd2;
+  //
+  // CDW 14
+  //
+  UINT32 Ilbrt;               /* Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Lbat;                /* Logical Block Application Tag */
+  UINT16 Lbatm;               /* Logical Block Application Tag Mask */
+} NVME_WRITE_ZEROES;
+
+//
+// Compare command
+//
+typedef struct {
+  //
+  // CDW 10, 11
+  //
+  UINT64 Slba;                /* Starting LBA */
+  //
+  // CDW 12
+  //
+  UINT16 Nlb;                 /* Number of Logical Blocks */
+  UINT16 Rsvd1:10;
+  UINT16 Prinfo:4;            /* Protection Info Check */
+  UINT16 Fua:1;               /* Force Unit Access */
+  UINT16 Lr:1;                /* Limited Retry */
+  //
+  // CDW 13
+  //
+  UINT32 Rsvd2;
+  //
+  // CDW 14
+  //
+  UINT32 Eilbrt;              /* Expected Initial Logical Block Reference Tag */
+  //
+  // CDW 15
+  //
+  UINT16 Elbat;               /* Expected Logical Block Application Tag */
+  UINT16 Elbatm;              /* Expected Logical Block Application Tag Mask */
+} NVME_COMPARE;
+
+typedef union {
+  NVME_READ                   Read;
+  NVME_WRITE                  Write;
+  NVME_FLUSH                  Flush;
+  NVME_WRITE_UNCORRECTABLE    WriteUncorrectable;
+  NVME_WRITE_ZEROES           WriteZeros;
+  NVME_COMPARE                Compare;
+} NVME_CMD;
+
+typedef struct {
+  UINT16 Mp;                /* Maximum Power */
+  UINT8  Rsvd1;             /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Mps:1;             /* Max Power Scale */
+  UINT8  Nops:1;            /* Non-Operational State */
+  UINT8  Rsvd2:6;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Enlat;             /* Entry Latency */
+  UINT32 Exlat;             /* Exit Latency */
+  UINT8  Rrt:5;             /* Relative Read Throughput */
+  UINT8  Rsvd3:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rrl:5;             /* Relative Read Leatency */
+  UINT8  Rsvd4:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rwt:5;             /* Relative Write Throughput */
+  UINT8  Rsvd5:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rwl:5;             /* Relative Write Leatency */
+  UINT8  Rsvd6:3;           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8  Rsvd7[16];         /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_PSDESCRIPTOR;
+
+//
+//  Identify Controller Data
+//
+typedef struct {
+  //
+  // Controller Capabilities and Features 0-255
+  //
+  UINT16 Vid;                 /* PCI Vendor ID */
+  UINT16 Ssvid;               /* PCI sub-system vendor ID */
+  UINT8  Sn[20];              /* Product serial number */
+
+  UINT8  Mn[40];              /* Proeduct model number */
+  UINT8  Fr[8];               /* Firmware Revision */
+  UINT8  Rab;                 /* Recommended Arbitration Burst */
+  UINT8  Ieee_oui[3];         /* Organization Unique Identifier */
+  UINT8  Cmic;                /* Multi-interface Capabilities */
+  UINT8  Mdts;                /* Maximum Data Transfer Size */
+  UINT8  Cntlid[2];           /* Controller ID */
+  UINT8  Rsvd1[176];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // Admin Command Set Attributes
+  //
+  UINT16 Oacs;                /* Optional Admin Command Support */
+    #define NAMESPACE_MANAGEMENT_SUPPORTED  BIT3
+    #define FW_DOWNLOAD_ACTIVATE_SUPPORTED  BIT2
+    #define FORMAT_NVM_SUPPORTED            BIT1
+    #define SECURITY_SEND_RECEIVE_SUPPORTED BIT0
+  UINT8  Acl;                 /* Abort Command Limit */
+  UINT8  Aerl;                /* Async Event Request Limit */
+  UINT8  Frmw;                /* Firmware updates */
+  UINT8  Lpa;                 /* Log Page Attributes */
+  UINT8  Elpe;                /* Error Log Page Entries */
+  UINT8  Npss;                /* Number of Power States Support */
+  UINT8  Avscc;               /* Admin Vendor Specific Command Configuration */
+  UINT8  Apsta;               /* Autonomous Power State Transition Attributes */
+  UINT8  Rsvd2[246];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // NVM Command Set Attributes
+  //
+  UINT8  Sqes;                /* Submission Queue Entry Size */
+  UINT8  Cqes;                /* Completion Queue Entry Size */
+  UINT16 Rsvd3;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Nn;                  /* Number of Namespaces */
+  UINT16 Oncs;                /* Optional NVM Command Support */
+  UINT16 Fuses;               /* Fused Operation Support */
+  UINT8  Fna;                 /* Format NVM Attributes */
+  UINT8  Vwc;                 /* Volatile Write Cache */
+  UINT16 Awun;                /* Atomic Write Unit Normal */
+  UINT16 Awupf;               /* Atomic Write Unit Power Fail */
+  UINT8  Nvscc;               /* NVM Vendor Specific Command Configuration */
+  UINT8  Rsvd4;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT16 Acwu;                /* Atomic Compare & Write Unit */
+  UINT16 Rsvd5;               /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Sgls;                /* SGL Support  */
+  UINT8  Rsvd6[164];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // I/O Command set Attributes
+  //
+  UINT8 Rsvd7[1344];          /* Reserved as of Nvm Express 1.1 Spec */
+  //
+  // Power State Descriptors
+  //
+  NVME_PSDESCRIPTOR PsDescriptor[32];
+
+  UINT8  VendorData[1024];    /* Vendor specific data */
+} NVME_ADMIN_CONTROLLER_DATA;
+
+typedef struct {
+  UINT16 Ms;                /* Metadata Size */
+  UINT8  Lbads;             /* LBA Data Size */
+  UINT8  Rp:2;              /* Relative Performance */
+    #define LBAF_RP_BEST      00b
+    #define LBAF_RP_BETTER    01b
+    #define LBAF_RP_GOOD      10b
+    #define LBAF_RP_DEGRADED  11b
+  UINT8  Rsvd1:6;           /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_LBAFORMAT;
+
+//
+// Identify Namespace Data
+//
+typedef struct {
+  //
+  // NVM Command Set Specific
+  //
+  UINT64 Nsze;                /* Namespace Size (total number of blocks in formatted namespace) */
+  UINT64 Ncap;                /* Namespace Capacity (max number of logical blocks) */
+  UINT64 Nuse;                /* Namespace Utilization */
+  UINT8  Nsfeat;              /* Namespace Features */
+  UINT8  Nlbaf;               /* Number of LBA Formats */
+  UINT8  Flbas;               /* Formatted LBA size */
+  UINT8  Mc;                  /* Metadata Capabilities */
+  UINT8  Dpc;                 /* End-to-end Data Protection capabilities */
+  UINT8  Dps;                 /* End-to-end Data Protection Type Settings */
+  UINT8  Nmic;                /* Namespace Multi-path I/O and Namespace Sharing Capabilities */
+  UINT8  Rescap;              /* Reservation Capabilities */
+  UINT8  Rsvd1[88];           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT64 Eui64;               /* IEEE Extended Unique Identifier */
+  //
+  // LBA Format
+  //
+  NVME_LBAFORMAT LbaFormat[16];
+
+  UINT8 Rsvd2[192];           /* Reserved as of Nvm Express 1.1 Spec */
+  UINT8 VendorData[3712];     /* Vendor specific data */
+} NVME_ADMIN_NAMESPACE_DATA;
+
+//
+// NvmExpress Admin Identify Cmd
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Cns:2;
+  UINT32 Rsvd1:30;
+} NVME_ADMIN_IDENTIFY;
+
+//
+// NvmExpress Admin Create I/O Completion Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Qid:16;              /* Queue Identifier */
+  UINT32 Qsize:16;            /* Queue Size */
+
+  //
+  // CDW 11
+  //
+  UINT32 Pc:1;                /* Physically Contiguous */
+  UINT32 Ien:1;               /* Interrupts Enabled */
+  UINT32 Rsvd1:14;            /* reserved as of Nvm Express 1.1 Spec */
+  UINT32 Iv:16;               /* Interrupt Vector for MSI-X or MSI*/
+} NVME_ADMIN_CRIOCQ;
+
+//
+// NvmExpress Admin Create I/O Submission Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Qid:16;              /* Queue Identifier */
+  UINT32 Qsize:16;            /* Queue Size */
+
+  //
+  // CDW 11
+  //
+  UINT32 Pc:1;                /* Physically Contiguous */
+  UINT32 Qprio:2;             /* Queue Priority */
+  UINT32 Rsvd1:13;            /* Reserved as of Nvm Express 1.1 Spec */
+  UINT32 Cqid:16;             /* Completion Queue ID */
+} NVME_ADMIN_CRIOSQ;
+
+//
+// NvmExpress Admin Delete I/O Completion Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT16 Qid;
+  UINT16 Rsvd1;
+} NVME_ADMIN_DEIOCQ;
+
+//
+// NvmExpress Admin Delete I/O Submission Queue
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT16 Qid;
+  UINT16 Rsvd1;
+} NVME_ADMIN_DEIOSQ;
+
+//
+// NvmExpress Admin Abort Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Sqid:16;             /* Submission Queue identifier */
+  UINT32 Cid:16;              /* Command Identifier */
+} NVME_ADMIN_ABORT;
+
+//
+// NvmExpress Admin Firmware Activate Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fs:3;                /* Submission Queue identifier */
+  UINT32 Aa:2;                /* Command Identifier */
+  UINT32 Rsvd1:27;
+} NVME_ADMIN_FIRMWARE_ACTIVATE;
+
+//
+// NvmExpress Admin Firmware Image Download Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Numd;                /* Number of Dwords */
+  //
+  // CDW 11
+  //
+  UINT32 Ofst;                /* Offset */
+} NVME_ADMIN_FIRMWARE_IMAGE_DOWNLOAD;
+
+//
+// NvmExpress Admin Get Features Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fid:8;                /* Feature Identifier */
+  UINT32 Sel:3;                /* Select */
+  UINT32 Rsvd1:21;
+} NVME_ADMIN_GET_FEATURES;
+
+//
+// NvmExpress Admin Get Log Page Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Lid:8;               /* Log Page Identifier */
+    #define LID_ERROR_INFO   0x1
+    #define LID_SMART_INFO   0x2
+    #define LID_FW_SLOT_INFO 0x3
+  UINT32 Rsvd1:8;
+  UINT32 Numd:12;             /* Number of Dwords */
+  UINT32 Rsvd2:4;             /* Reserved as of Nvm Express 1.1 Spec */
+} NVME_ADMIN_GET_LOG_PAGE;
+
+//
+// NvmExpress Admin Set Features Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Fid:8;               /* Feature Identifier */
+  UINT32 Rsvd1:23;
+  UINT32 Sv:1;                /* Save */
+} NVME_ADMIN_SET_FEATURES;
+
+//
+// NvmExpress Admin Format NVM Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Lbaf:4;              /* LBA Format */
+  UINT32 Ms:1;                /* Metadata Settings */
+  UINT32 Pi:3;                /* Protection Information */
+  UINT32 Pil:1;               /* Protection Information Location */
+  UINT32 Ses:3;               /* Secure Erase Settings */
+  UINT32 Rsvd1:20;
+} NVME_ADMIN_FORMAT_NVM;
+
+//
+// NvmExpress Admin Security Receive Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Rsvd1:8;
+  UINT32 Spsp:16;             /* SP Specific */
+  UINT32 Secp:8;              /* Security Protocol */
+  //
+  // CDW 11
+  //
+  UINT32 Al;                  /* Allocation Length */
+} NVME_ADMIN_SECURITY_RECEIVE;
+
+//
+// NvmExpress Admin Security Send Command
+//
+typedef struct {
+  //
+  // CDW 10
+  //
+  UINT32 Rsvd1:8;
+  UINT32 Spsp:16;             /* SP Specific */
+  UINT32 Secp:8;              /* Security Protocol */
+  //
+  // CDW 11
+  //
+  UINT32 Tl;                  /* Transfer Length */
+} NVME_ADMIN_SECURITY_SEND;
+
+typedef union {
+  NVME_ADMIN_IDENTIFY                   Identify;
+  NVME_ADMIN_CRIOCQ                     CrIoCq;
+  NVME_ADMIN_CRIOSQ                     CrIoSq;
+  NVME_ADMIN_DEIOCQ                     DeIoCq;
+  NVME_ADMIN_DEIOSQ                     DeIoSq;
+  NVME_ADMIN_ABORT                      Abort;
+  NVME_ADMIN_FIRMWARE_ACTIVATE          Activate;
+  NVME_ADMIN_FIRMWARE_IMAGE_DOWNLOAD    FirmwareImageDownload;
+  NVME_ADMIN_GET_FEATURES               GetFeatures;
+  NVME_ADMIN_GET_LOG_PAGE               GetLogPage;
+  NVME_ADMIN_SET_FEATURES               SetFeatures;
+  NVME_ADMIN_FORMAT_NVM                 FormatNvm;
+  NVME_ADMIN_SECURITY_RECEIVE           SecurityReceive;
+  NVME_ADMIN_SECURITY_SEND              SecuritySend;
+} NVME_ADMIN_CMD;
+
+typedef struct {
+  UINT32 Cdw10;
+  UINT32 Cdw11;
+  UINT32 Cdw12;
+  UINT32 Cdw13;
+  UINT32 Cdw14;
+  UINT32 Cdw15;
+} NVME_RAW;
+
+typedef union {
+  NVME_ADMIN_CMD Admin;   // Union of Admin commands
+  NVME_CMD       Nvm;     // Union of Nvm commands
+  NVME_RAW       Raw;
+} NVME_PAYLOAD;
+
+//
+// Submission Queue
+//
+typedef struct {
+  //
+  // CDW 0, Common to all comnmands
+  //
+  UINT8  Opc;               // Opcode
+  UINT8  Fuse:2;            // Fused Operation
+  UINT8  Rsvd1:5;
+  UINT8  Psdt:1;            // PRP or SGL for Data Transfer
+  UINT16 Cid;               // Command Identifier
+
+  //
+  // CDW 1
+  //
+  UINT32 Nsid;              // Namespace Identifier
+
+  //
+  // CDW 2,3
+  //
+  UINT64 Rsvd2;
+
+  //
+  // CDW 4,5
+  //
+  UINT64 Mptr;              // Metadata Pointer
+
+  //
+  // CDW 6-9
+  //
+  UINT64 Prp[2];            // First and second PRP entries
+
+  NVME_PAYLOAD Payload;
+
+} NVME_SQ;
+
+//
+// Completion Queue
+//
+typedef struct {
+  //
+  // CDW 0
+  //
+  UINT32 Dword0;
+  //
+  // CDW 1
+  //
+  UINT32 Rsvd1;
+  //
+  // CDW 2
+  //
+  UINT16 Sqhd;              // Submission Queue Head Pointer
+  UINT16 Sqid;              // Submission Queue Identifier
+  //
+  // CDW 3
+  //
+  UINT16 Cid;               // Command Identifier
+  UINT16 Pt:1;              // Phase Tag
+  UINT16 Sc:8;              // Status Code
+  UINT16 Sct:3;             // Status Code Type
+  UINT16 Rsvd2:2;
+  UINT16 Mo:1;              // More
+  UINT16 Dnr:1;             // Do Not Retry
+} NVME_CQ;
+
+//
+// Nvm Express Admin cmd opcodes
+//
+#define NVME_ADMIN_DEIOSQ_CMD                0x00
+#define NVME_ADMIN_CRIOSQ_CMD                0x01
+#define NVME_ADMIN_GET_LOG_PAGE_CMD          0x02
+#define NVME_ADMIN_DEIOCQ_CMD                0x04
+#define NVME_ADMIN_CRIOCQ_CMD                0x05
+#define NVME_ADMIN_IDENTIFY_CMD              0x06
+#define NVME_ADMIN_ABORT_CMD                 0x08
+#define NVME_ADMIN_SET_FEATURES_CMD          0x09
+#define NVME_ADMIN_GET_FEATURES_CMD          0x0A
+#define NVME_ADMIN_ASYNC_EVENT_REQUEST_CMD   0x0C
+#define NVME_ADMIN_NAMESACE_MANAGEMENT_CMD   0x0D
+#define NVME_ADMIN_FW_COMMIT_CMD             0x10
+#define NVME_ADMIN_FW_IAMGE_DOWNLOAD_CMD     0x11
+#define NVME_ADMIN_NAMESACE_ATTACHMENT_CMD   0x15
+#define NVME_ADMIN_FORMAT_NVM_CMD            0x80
+#define NVME_ADMIN_SECURITY_SEND_CMD         0x81
+#define NVME_ADMIN_SECURITY_RECEIVE_CMD      0x82
+
+#define NVME_IO_FLUSH_OPC                    0
+#define NVME_IO_WRITE_OPC                    1
+#define NVME_IO_READ_OPC                     2
+
+//
+// Offset from the beginning of private data queue buffer
+//
+#define NVME_ASQ_BUF_OFFSET                  EFI_PAGE_SIZE
+
+/**
+  Initialize the Nvm Express controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+NvmeControllerInit (
+  IN NVME_CONTROLLER_PRIVATE_DATA    *Private
+  );
+
+/**
+  Get identify controller data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  Buffer           The buffer used to store the identify controller data.
+
+  @return EFI_SUCCESS      Successfully get the identify controller data.
+  @return EFI_DEVICE_ERROR Fail to get the identify controller data.
+
+**/
+EFI_STATUS
+NvmeIdentifyController (
+  IN NVME_CONTROLLER_PRIVATE_DATA       *Private,
+  IN VOID                               *Buffer
+  );
+
+/**
+  Get specified identify namespace data.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  NamespaceId      The specified namespace identifier.
+  @param  Buffer           The buffer used to store the identify namespace data.
+
+  @return EFI_SUCCESS      Successfully get the identify namespace data.
+  @return EFI_DEVICE_ERROR Fail to get the identify namespace data.
+
+**/
+EFI_STATUS
+NvmeIdentifyNamespace (
+  IN NVME_CONTROLLER_PRIVATE_DATA      *Private,
+  IN UINT32                            NamespaceId,
+  IN VOID                              *Buffer
+  );
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c
new file mode 100644
index 0000000000..f9871527dd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressPassthru.c
@@ -0,0 +1,907 @@
+/** @file
+  NvmExpressDxe driver is used to manage non-volatile memory subsystem which follows
+  NVM Express specification.
+
+  (C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+  Copyright (c) 2013 - 2015, 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 "NvmExpress.h"
+
+/**
+  Dump the execution status from a given completion queue entry.
+
+  @param[in]     Cq               A pointer to the NVME_CQ item.
+
+**/
+VOID
+NvmeDumpStatus (
+  IN NVME_CQ             *Cq
+  )
+{
+  DEBUG ((EFI_D_VERBOSE, "Dump NVMe Completion Entry Status from [0x%x]:\n", Cq));
+
+  DEBUG ((EFI_D_VERBOSE, "  SQ Identifier : [0x%x], Phase Tag : [%d], Cmd Identifier : [0x%x]\n", Cq->Sqid, Cq->Pt, Cq->Cid));
+
+  DEBUG ((EFI_D_VERBOSE, "  NVMe Cmd Execution Result - "));
+
+  switch (Cq->Sct) {
+    case 0x0:
+      switch (Cq->Sc) {
+        case 0x0:
+          DEBUG ((EFI_D_VERBOSE, "Successful Completion\n"));
+          break;
+        case 0x1:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Command Opcode\n"));
+          break;
+        case 0x2:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Field in Command\n"));
+          break;
+        case 0x3:
+          DEBUG ((EFI_D_VERBOSE, "Command ID Conflict\n"));
+          break;
+        case 0x4:
+          DEBUG ((EFI_D_VERBOSE, "Data Transfer Error\n"));
+          break;
+        case 0x5:
+          DEBUG ((EFI_D_VERBOSE, "Commands Aborted due to Power Loss Notification\n"));
+          break;
+        case 0x6:
+          DEBUG ((EFI_D_VERBOSE, "Internal Device Error\n"));
+          break;
+        case 0x7:
+          DEBUG ((EFI_D_VERBOSE, "Command Abort Requested\n"));
+          break;
+        case 0x8:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to SQ Deletion\n"));
+          break;
+        case 0x9:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to Failed Fused Command\n"));
+          break;
+        case 0xA:
+          DEBUG ((EFI_D_VERBOSE, "Command Aborted due to Missing Fused Command\n"));
+          break;
+        case 0xB:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Namespace or Format\n"));
+          break;
+        case 0xC:
+          DEBUG ((EFI_D_VERBOSE, "Command Sequence Error\n"));
+          break;
+        case 0xD:
+          DEBUG ((EFI_D_VERBOSE, "Invalid SGL Last Segment Descriptor\n"));
+          break;
+        case 0xE:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Number of SGL Descriptors\n"));
+          break;
+        case 0xF:
+          DEBUG ((EFI_D_VERBOSE, "Data SGL Length Invalid\n"));
+          break;
+        case 0x10:
+          DEBUG ((EFI_D_VERBOSE, "Metadata SGL Length Invalid\n"));
+          break;
+        case 0x11:
+          DEBUG ((EFI_D_VERBOSE, "SGL Descriptor Type Invalid\n"));
+          break;
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "LBA Out of Range\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Capacity Exceeded\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "Namespace Not Ready\n"));
+          break;
+        case 0x83:
+          DEBUG ((EFI_D_VERBOSE, "Reservation Conflict\n"));
+          break;
+      }
+      break;
+
+    case 0x1:
+      switch (Cq->Sc) {
+        case 0x0:
+          DEBUG ((EFI_D_VERBOSE, "Completion Queue Invalid\n"));
+          break;
+        case 0x1:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Queue Identifier\n"));
+          break;
+        case 0x2:
+          DEBUG ((EFI_D_VERBOSE, "Maximum Queue Size Exceeded\n"));
+          break;
+        case 0x3:
+          DEBUG ((EFI_D_VERBOSE, "Abort Command Limit Exceeded\n"));
+          break;
+        case 0x5:
+          DEBUG ((EFI_D_VERBOSE, "Asynchronous Event Request Limit Exceeded\n"));
+          break;
+        case 0x6:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Firmware Slot\n"));
+          break;
+        case 0x7:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Firmware Image\n"));
+          break;
+        case 0x8:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Interrupt Vector\n"));
+          break;
+        case 0x9:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Log Page\n"));
+          break;
+        case 0xA:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Format\n"));
+          break;
+        case 0xB:
+          DEBUG ((EFI_D_VERBOSE, "Firmware Application Requires Conventional Reset\n"));
+          break;
+        case 0xC:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Queue Deletion\n"));
+          break;
+        case 0xD:
+          DEBUG ((EFI_D_VERBOSE, "Feature Identifier Not Saveable\n"));
+          break;
+        case 0xE:
+          DEBUG ((EFI_D_VERBOSE, "Feature Not Changeable\n"));
+          break;
+        case 0xF:
+          DEBUG ((EFI_D_VERBOSE, "Feature Not Namespace Specific\n"));
+          break;
+        case 0x10:
+          DEBUG ((EFI_D_VERBOSE, "Firmware Application Requires NVM Subsystem Reset\n"));
+          break;
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "Conflicting Attributes\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Invalid Protection Information\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "Attempted Write to Read Only Range\n"));
+          break;
+      }
+      break;
+
+    case 0x2:
+      switch (Cq->Sc) {
+        case 0x80:
+          DEBUG ((EFI_D_VERBOSE, "Write Fault\n"));
+          break;
+        case 0x81:
+          DEBUG ((EFI_D_VERBOSE, "Unrecovered Read Error\n"));
+          break;
+        case 0x82:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Guard Check Error\n"));
+          break;
+        case 0x83:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Application Tag Check Error\n"));
+          break;
+        case 0x84:
+          DEBUG ((EFI_D_VERBOSE, "End-to-end Reference Tag Check Error\n"));
+          break;
+        case 0x85:
+          DEBUG ((EFI_D_VERBOSE, "Compare Failure\n"));
+          break;
+        case 0x86:
+          DEBUG ((EFI_D_VERBOSE, "Access Denied\n"));
+          break;
+      }
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  Create PRP lists for data transfer which is larger than 2 memory pages.
+  Note here we calcuate the number of required PRP lists and allocate them at one time.
+
+  @param[in]     PciIo               A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param[in]     PhysicalAddr        The physical base address of data buffer.
+  @param[in]     Pages               The number of pages to be transfered.
+  @param[out]    PrpListHost         The host base address of PRP lists.
+  @param[in,out] PrpListNo           The number of PRP List.
+  @param[out]    Mapping             The mapping value returned from PciIo.Map().
+
+  @retval The pointer to the first PRP List of the PRP lists.
+
+**/
+VOID*
+NvmeCreatePrpList (
+  IN     EFI_PCI_IO_PROTOCOL          *PciIo,
+  IN     EFI_PHYSICAL_ADDRESS         PhysicalAddr,
+  IN     UINTN                        Pages,
+     OUT VOID                         **PrpListHost,
+  IN OUT UINTN                        *PrpListNo,
+     OUT VOID                         **Mapping
+  )
+{
+  UINTN                       PrpEntryNo;
+  UINT64                      PrpListBase;
+  UINTN                       PrpListIndex;
+  UINTN                       PrpEntryIndex;
+  UINT64                      Remainder;
+  EFI_PHYSICAL_ADDRESS        PrpListPhyAddr;
+  UINTN                       Bytes;
+  EFI_STATUS                  Status;
+
+  //
+  // The number of Prp Entry in a memory page.
+  //
+  PrpEntryNo = EFI_PAGE_SIZE / sizeof (UINT64);
+
+  //
+  // Calculate total PrpList number.
+  //
+  *PrpListNo = (UINTN)DivU64x64Remainder ((UINT64)Pages, (UINT64)PrpEntryNo - 1, &Remainder);
+  if (*PrpListNo == 0) {
+    *PrpListNo = 1;
+  } else if ((Remainder != 0) && (Remainder != 1)) {
+    *PrpListNo += 1;
+  } else if (Remainder == 1) {
+    Remainder = PrpEntryNo;
+  } else if (Remainder == 0) {
+    Remainder = PrpEntryNo - 1;
+  }
+
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    *PrpListNo,
+                    PrpListHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (*PrpListNo);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    *PrpListHost,
+                    &Bytes,
+                    &PrpListPhyAddr,
+                    Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (*PrpListNo))) {
+    DEBUG ((EFI_D_ERROR, "NvmeCreatePrpList: create PrpList failure!\n"));
+    goto EXIT;
+  }
+  //
+  // Fill all PRP lists except of last one.
+  //
+  ZeroMem (*PrpListHost, Bytes);
+  for (PrpListIndex = 0; PrpListIndex < *PrpListNo - 1; ++PrpListIndex) {
+    PrpListBase = *(UINT64*)PrpListHost + PrpListIndex * EFI_PAGE_SIZE;
+
+    for (PrpEntryIndex = 0; PrpEntryIndex < PrpEntryNo; ++PrpEntryIndex) {
+      if (PrpEntryIndex != PrpEntryNo - 1) {
+        //
+        // Fill all PRP entries except of last one.
+        //
+        *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PhysicalAddr;
+        PhysicalAddr += EFI_PAGE_SIZE;
+      } else {
+        //
+        // Fill last PRP entries with next PRP List pointer.
+        //
+        *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PrpListPhyAddr + (PrpListIndex + 1) * EFI_PAGE_SIZE;
+      }
+    }
+  }
+  //
+  // Fill last PRP list.
+  //
+  PrpListBase = *(UINT64*)PrpListHost + PrpListIndex * EFI_PAGE_SIZE;
+  for (PrpEntryIndex = 0; PrpEntryIndex < Remainder; ++PrpEntryIndex) {
+    *((UINT64*)(UINTN)PrpListBase + PrpEntryIndex) = PhysicalAddr;
+    PhysicalAddr += EFI_PAGE_SIZE;
+  }
+
+  return (VOID*)(UINTN)PrpListPhyAddr;
+
+EXIT:
+  PciIo->FreeBuffer (PciIo, *PrpListNo, *PrpListHost);
+  return NULL;
+}
+
+
+/**
+  Sends an NVM Express Command Packet to an NVM Express controller or namespace. This function supports
+  both blocking I/O and non-blocking I/O. The blocking I/O functionality is required, and the non-blocking
+  I/O functionality is optional.
+
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         A 32 bit namespace ID as defined in the NVMe specification to which the NVM Express Command
+                                     Packet will be sent.  A value of 0 denotes the NVM Express controller, a value of all 0xFF's
+                                     (all bytes are 0xFF) in the namespace ID specifies that the command packet should be sent to
+                                     all valid namespaces.
+  @param[in,out] Packet              A pointer to the NVM Express Command Packet.
+  @param[in]     Event               If non-blocking I/O is not supported then Event is ignored, and blocking I/O is performed.
+                                     If Event is NULL, then blocking I/O is performed. If Event is not NULL and non-blocking I/O
+                                     is supported, then non-blocking I/O is performed, and Event will be signaled when the NVM
+                                     Express Command Packet completes.
+
+  @retval EFI_SUCCESS                The NVM Express Command Packet was sent by the host. TransferLength bytes were transferred
+                                     to, or from DataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE        The NVM Express Command Packet was not executed. The number of bytes that could be transferred
+                                     is returned in TransferLength.
+  @retval EFI_NOT_READY              The NVM Express Command Packet could not be sent because the controller is not ready. The caller
+                                     may retry again later.
+  @retval EFI_DEVICE_ERROR           A device error occurred while attempting to send the NVM Express Command Packet.
+  @retval EFI_INVALID_PARAMETER      NamespaceId or the contents of EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET are invalid. The NVM
+                                     Express Command Packet was not sent, so no additional status information is available.
+  @retval EFI_UNSUPPORTED            The command described by the NVM Express Command Packet is not supported by the NVM Express
+                                     controller. The NVM Express Command Packet was not sent so no additional status information
+                                     is available.
+  @retval EFI_TIMEOUT                A timeout occurred while waiting for the NVM Express Command Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressPassThru (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET    *Packet,
+  IN     EFI_EVENT                                   Event OPTIONAL
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_STATUS                    Status;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  NVME_SQ                       *Sq;
+  NVME_CQ                       *Cq;
+  UINT8                         QueueType;
+  UINT32                        Bytes;
+  UINT16                        Offset;
+  EFI_EVENT                     TimerEvent;
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *MapData;
+  VOID                          *MapMeta;
+  VOID                          *MapPrpList;
+  UINTN                         MapLength;
+  UINT64                        *Prp;
+  VOID                          *PrpListHost;
+  UINTN                         PrpListNo;
+  UINT32                        Data;
+
+  //
+  // check the data fields in Packet parameter.
+  //
+  if ((This == NULL) || (Packet == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->NvmeCmd == NULL) || (Packet->NvmeCompletion == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Packet->QueueType != NVME_ADMIN_QUEUE && Packet->QueueType != NVME_IO_QUEUE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private     = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+  PciIo       = Private->PciIo;
+  MapData     = NULL;
+  MapMeta     = NULL;
+  MapPrpList  = NULL;
+  PrpListHost = NULL;
+  PrpListNo   = 0;
+  Prp         = NULL;
+  TimerEvent  = NULL;
+  Status      = EFI_SUCCESS;
+
+  QueueType = Packet->QueueType;
+  Sq  = Private->SqBuffer[QueueType] + Private->SqTdbl[QueueType].Sqt;
+  Cq  = Private->CqBuffer[QueueType] + Private->CqHdbl[QueueType].Cqh;
+
+  if (Packet->NvmeCmd->Nsid != NamespaceId) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (Sq, sizeof (NVME_SQ));
+  Sq->Opc  = (UINT8)Packet->NvmeCmd->Cdw0.Opcode;
+  Sq->Fuse = (UINT8)Packet->NvmeCmd->Cdw0.FusedOperation;
+  Sq->Cid  = Private->Cid[QueueType]++;
+  Sq->Nsid = Packet->NvmeCmd->Nsid;
+
+  //
+  // Currently we only support PRP for data transfer, SGL is NOT supported.
+  //
+  ASSERT (Sq->Psdt == 0);
+  if (Sq->Psdt != 0) {
+    DEBUG ((EFI_D_ERROR, "NvmExpressPassThru: doesn't support SGL mechanism\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  Sq->Prp[0] = (UINT64)(UINTN)Packet->TransferBuffer;
+  //
+  // If the NVMe cmd has data in or out, then mapping the user buffer to the PCI controller specific addresses.
+  // Note here we don't handle data buffer for CreateIOSubmitionQueue and CreateIOCompletionQueue cmds because
+  // these two cmds are special which requires their data buffer must support simultaneous access by both the
+  // processor and a PCI Bus Master. It's caller's responsbility to ensure this.
+  //
+  if (((Sq->Opc & (BIT0 | BIT1)) != 0) && (Sq->Opc != NVME_ADMIN_CRIOCQ_CMD) && (Sq->Opc != NVME_ADMIN_CRIOSQ_CMD)) {
+    if ((Sq->Opc & BIT0) != 0) {
+      Flag = EfiPciIoOperationBusMasterRead;
+    } else {
+      Flag = EfiPciIoOperationBusMasterWrite;
+    }
+
+    MapLength = Packet->TransferLength;
+    Status = PciIo->Map (
+                      PciIo,
+                      Flag,
+                      Packet->TransferBuffer,
+                      &MapLength,
+                      &PhyAddr,
+                      &MapData
+                      );
+    if (EFI_ERROR (Status) || (Packet->TransferLength != MapLength)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Sq->Prp[0] = PhyAddr;
+    Sq->Prp[1] = 0;
+
+    MapLength = Packet->MetadataLength;
+    if(Packet->MetadataBuffer != NULL) {
+      MapLength = Packet->MetadataLength;
+      Status = PciIo->Map (
+                        PciIo,
+                        Flag,
+                        Packet->MetadataBuffer,
+                        &MapLength,
+                        &PhyAddr,
+                        &MapMeta
+                        );
+      if (EFI_ERROR (Status) || (Packet->MetadataLength != MapLength)) {
+        PciIo->Unmap (
+                 PciIo,
+                 MapData
+                 );
+
+        return EFI_OUT_OF_RESOURCES;
+      }
+      Sq->Mptr = PhyAddr;
+    }
+  }
+  //
+  // If the buffer size spans more than two memory pages (page size as defined in CC.Mps),
+  // then build a PRP list in the second PRP submission queue entry.
+  //
+  Offset = ((UINT16)Sq->Prp[0]) & (EFI_PAGE_SIZE - 1);
+  Bytes  = Packet->TransferLength;
+
+  if ((Offset + Bytes) > (EFI_PAGE_SIZE * 2)) {
+    //
+    // Create PrpList for remaining data buffer.
+    //
+    PhyAddr = (Sq->Prp[0] + EFI_PAGE_SIZE) & ~(EFI_PAGE_SIZE - 1);
+    Prp = NvmeCreatePrpList (PciIo, PhyAddr, EFI_SIZE_TO_PAGES(Offset + Bytes) - 1, &PrpListHost, &PrpListNo, &MapPrpList);
+    if (Prp == NULL) {
+      goto EXIT;
+    }
+
+    Sq->Prp[1] = (UINT64)(UINTN)Prp;
+  } else if ((Offset + Bytes) > EFI_PAGE_SIZE) {
+    Sq->Prp[1] = (Sq->Prp[0] + EFI_PAGE_SIZE) & ~(EFI_PAGE_SIZE - 1);
+  }
+
+  if(Packet->NvmeCmd->Flags & CDW2_VALID) {
+    Sq->Rsvd2 = (UINT64)Packet->NvmeCmd->Cdw2;
+  }
+  if(Packet->NvmeCmd->Flags & CDW3_VALID) {
+    Sq->Rsvd2 |= LShiftU64 ((UINT64)Packet->NvmeCmd->Cdw3, 32);
+  }
+  if(Packet->NvmeCmd->Flags & CDW10_VALID) {
+    Sq->Payload.Raw.Cdw10 = Packet->NvmeCmd->Cdw10;
+  }
+  if(Packet->NvmeCmd->Flags & CDW11_VALID) {
+    Sq->Payload.Raw.Cdw11 = Packet->NvmeCmd->Cdw11;
+  }
+  if(Packet->NvmeCmd->Flags & CDW12_VALID) {
+    Sq->Payload.Raw.Cdw12 = Packet->NvmeCmd->Cdw12;
+  }
+  if(Packet->NvmeCmd->Flags & CDW13_VALID) {
+    Sq->Payload.Raw.Cdw13 = Packet->NvmeCmd->Cdw13;
+  }
+  if(Packet->NvmeCmd->Flags & CDW14_VALID) {
+    Sq->Payload.Raw.Cdw14 = Packet->NvmeCmd->Cdw14;
+  }
+  if(Packet->NvmeCmd->Flags & CDW15_VALID) {
+    Sq->Payload.Raw.Cdw15 = Packet->NvmeCmd->Cdw15;
+  }
+
+  //
+  // Ring the submission queue doorbell.
+  //
+  Private->SqTdbl[QueueType].Sqt ^= 1;
+  Data = ReadUnaligned32 ((UINT32*)&Private->SqTdbl[QueueType]);
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               NVME_BAR,
+               NVME_SQTDBL_OFFSET(QueueType, Private->Cap.Dstrd),
+               1,
+               &Data
+               );
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  Status = gBS->SetTimer(TimerEvent, TimerRelative, Packet->CommandTimeout);
+
+  if (EFI_ERROR(Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Wait for completion queue to get filled in.
+  //
+  Status = EFI_TIMEOUT;
+  while (EFI_ERROR (gBS->CheckEvent (TimerEvent))) {
+    if (Cq->Pt != Private->Pt[QueueType]) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+  }
+
+  //
+  // Check the NVMe cmd execution result
+  //
+  if (Status != EFI_TIMEOUT) {
+    if ((Cq->Sct == 0) && (Cq->Sc == 0)) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      //
+      // Copy the Respose Queue entry for this command to the callers response buffer
+      //
+      CopyMem(Packet->NvmeCompletion, Cq, sizeof(EFI_NVM_EXPRESS_COMPLETION));
+    
+      //
+      // Dump every completion entry status for debugging.
+      //
+      DEBUG_CODE_BEGIN();
+        NvmeDumpStatus(Cq);
+      DEBUG_CODE_END();
+    }
+  }
+
+  if ((Private->CqHdbl[QueueType].Cqh ^= 1) == 0) {
+    Private->Pt[QueueType] ^= 1;
+  }
+
+  Data = ReadUnaligned32 ((UINT32*)&Private->CqHdbl[QueueType]);
+  PciIo->Mem.Write (
+               PciIo,
+               EfiPciIoWidthUint32,
+               NVME_BAR,
+               NVME_CQHDBL_OFFSET(QueueType, Private->Cap.Dstrd),
+               1,
+               &Data
+               );
+
+EXIT:
+  if (MapData != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapData
+             );
+  }
+
+  if (MapMeta != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapMeta
+             );
+  }
+
+  if (MapPrpList != NULL) {
+    PciIo->Unmap (
+             PciIo,
+             MapPrpList
+             );
+  }
+
+  if (Prp != NULL) {
+    PciIo->FreeBuffer (PciIo, PrpListNo, PrpListHost);
+  }
+
+  if (TimerEvent != NULL) {
+    gBS->CloseEvent (TimerEvent);
+  }
+  return Status;
+}
+
+/**
+  Used to retrieve the next namespace ID for this NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNextNamespace() function retrieves the next valid
+  namespace ID on this NVM Express controller.
+
+  If on input the value pointed to by NamespaceId is 0xFFFFFFFF, then the first valid namespace
+  ID defined on the NVM Express controller is returned in the location pointed to by NamespaceId
+  and a status of EFI_SUCCESS is returned.
+
+  If on input the value pointed to by NamespaceId is an invalid namespace ID other than 0xFFFFFFFF,
+  then EFI_INVALID_PARAMETER is returned.
+
+  If on input the value pointed to by NamespaceId is a valid namespace ID, then the next valid
+  namespace ID on the NVM Express controller is returned in the location pointed to by NamespaceId,
+  and EFI_SUCCESS is returned.
+
+  If the value pointed to by NamespaceId is the namespace ID of the last namespace on the NVM
+  Express controller, then EFI_NOT_FOUND is returned.
+
+  @param[in]     This           A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in,out] NamespaceId    On input, a pointer to a legal NamespaceId for an NVM Express
+                                namespace present on the NVM Express controller. On output, a
+                                pointer to the next NamespaceId of an NVM Express namespace on
+                                an NVM Express controller. An input value of 0xFFFFFFFF retrieves
+                                the first NamespaceId for an NVM Express namespace present on an
+                                NVM Express controller.
+
+  @retval EFI_SUCCESS           The Namespace ID of the next Namespace was returned.
+  @retval EFI_NOT_FOUND         There are no more namespaces defined on this controller.
+  @retval EFI_INVALID_PARAMETER NamespaceId is an invalid value other than 0xFFFFFFFF.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNextNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN OUT UINT32                                      *NamespaceId
+  )
+{
+  NVME_CONTROLLER_PRIVATE_DATA     *Private;
+  NVME_ADMIN_NAMESPACE_DATA        *NamespaceData;
+  UINT32                           NextNamespaceId;
+  EFI_STATUS                       Status;
+
+  if ((This == NULL) || (NamespaceId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NamespaceData = NULL;
+  Status        = EFI_NOT_FOUND;
+
+  Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+  //
+  // If the NamespaceId input value is 0xFFFFFFFF, then get the first valid namespace ID
+  //
+  if (*NamespaceId == 0xFFFFFFFF) {
+    //
+    // Start with the first namespace ID
+    //
+    NextNamespaceId = 1;
+    //
+    // Allocate buffer for Identify Namespace data.
+    //
+    NamespaceData = (NVME_ADMIN_NAMESPACE_DATA *)AllocateZeroPool (sizeof (NVME_ADMIN_NAMESPACE_DATA));
+
+    if (NamespaceData == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = NvmeIdentifyNamespace (Private, NextNamespaceId, NamespaceData);
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+
+    *NamespaceId = NextNamespaceId;
+  } else {
+    if (*NamespaceId >= Private->ControllerData->Nn) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    NextNamespaceId = *NamespaceId + 1;
+    //
+    // Allocate buffer for Identify Namespace data.
+    //
+    NamespaceData = (NVME_ADMIN_NAMESPACE_DATA *)AllocateZeroPool (sizeof (NVME_ADMIN_NAMESPACE_DATA));
+    if (NamespaceData == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = NvmeIdentifyNamespace (Private, NextNamespaceId, NamespaceData);
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+
+    *NamespaceId = NextNamespaceId;
+  }
+
+Done:
+  if (NamespaceData != NULL) {
+    FreePool(NamespaceData);
+  }
+
+  return Status;
+}
+
+/**
+  Used to translate a device path node to a namespace ID.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.GetNamespace() function determines the namespace ID associated with the
+  namespace described by DevicePath.
+
+  If DevicePath is a device path node type that the NVM Express Pass Thru driver supports, then the NVM Express
+  Pass Thru driver will attempt to translate the contents DevicePath into a namespace ID.
+
+  If this translation is successful, then that namespace ID is returned in NamespaceId, and EFI_SUCCESS is returned
+
+  @param[in]  This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath          A pointer to the device path node that describes an NVM Express namespace on
+                                  the NVM Express controller.
+  @param[out] NamespaceId         The NVM Express namespace ID contained in the device path node.
+
+  @retval EFI_SUCCESS             DevicePath was successfully translated to NamespaceId.
+  @retval EFI_INVALID_PARAMETER   If DevicePath or NamespaceId are NULL, then EFI_INVALID_PARAMETER is returned.
+  @retval EFI_UNSUPPORTED         If DevicePath is not a device path node type that the NVM Express Pass Thru driver
+                                  supports, then EFI_UNSUPPORTED is returned.
+  @retval EFI_NOT_FOUND           If DevicePath is a device path node type that the NVM Express Pass Thru driver
+                                  supports, but there is not a valid translation from DevicePath to a namespace ID,
+                                  then EFI_NOT_FOUND is returned.
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressGetNamespace (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     EFI_DEVICE_PATH_PROTOCOL                    *DevicePath,
+     OUT UINT32                                      *NamespaceId
+  )
+{
+  NVME_NAMESPACE_DEVICE_PATH       *Node;
+
+  if ((This == NULL) || (DevicePath == NULL) || (NamespaceId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DevicePath->Type != MESSAGING_DEVICE_PATH) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Node = (NVME_NAMESPACE_DEVICE_PATH *)DevicePath;
+
+  if (DevicePath->SubType == MSG_NVME_NAMESPACE_DP) {
+    if (DevicePathNodeLength(DevicePath) != sizeof(NVME_NAMESPACE_DEVICE_PATH)) {
+      return EFI_NOT_FOUND;
+    }
+
+    *NamespaceId = Node->NamespaceId;
+
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Used to allocate and build a device path node for an NVM Express namespace on an NVM Express controller.
+
+  The EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL.BuildDevicePath() function allocates and builds a single device
+  path node for the NVM Express namespace specified by NamespaceId.
+
+  If the NamespaceId is not valid, then EFI_NOT_FOUND is returned.
+
+  If DevicePath is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If there are not enough resources to allocate the device path node, then EFI_OUT_OF_RESOURCES is returned.
+
+  Otherwise, DevicePath is allocated with the boot service AllocatePool(), the contents of DevicePath are
+  initialized to describe the NVM Express namespace specified by NamespaceId, and EFI_SUCCESS is returned.
+
+  @param[in]     This                A pointer to the EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL instance.
+  @param[in]     NamespaceId         The NVM Express namespace ID  for which a device path node is to be
+                                     allocated and built. Caller must set the NamespaceId to zero if the
+                                     device path node will contain a valid UUID.
+  @param[in,out] DevicePath          A pointer to a single device path node that describes the NVM Express
+                                     namespace specified by NamespaceId. This function is responsible for
+                                     allocating the buffer DevicePath with the boot service AllocatePool().
+                                     It is the caller's responsibility to free DevicePath when the caller
+                                     is finished with DevicePath.
+  @retval EFI_SUCCESS                The device path node that describes the NVM Express namespace specified
+                                     by NamespaceId was allocated and returned in DevicePath.
+  @retval EFI_NOT_FOUND              The NamespaceId is not valid.
+  @retval EFI_INVALID_PARAMETER      DevicePath is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources to allocate the DevicePath node.
+
+**/
+EFI_STATUS
+EFIAPI
+NvmExpressBuildDevicePath (
+  IN     EFI_NVM_EXPRESS_PASS_THRU_PROTOCOL          *This,
+  IN     UINT32                                      NamespaceId,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL                    **DevicePath
+  )
+{
+  NVME_NAMESPACE_DEVICE_PATH     *Node;
+  NVME_CONTROLLER_PRIVATE_DATA   *Private;
+  EFI_STATUS                     Status;
+  NVME_ADMIN_NAMESPACE_DATA      *NamespaceData;
+
+  //
+  // Validate parameters
+  //
+  if ((This == NULL) || (DevicePath == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NamespaceId == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status  = EFI_SUCCESS;
+  Private = NVME_CONTROLLER_PRIVATE_DATA_FROM_PASS_THRU (This);
+
+  Node = (NVME_NAMESPACE_DEVICE_PATH *)AllocateZeroPool (sizeof (NVME_NAMESPACE_DEVICE_PATH));
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Node->Header.Type    = MESSAGING_DEVICE_PATH;
+  Node->Header.SubType = MSG_NVME_NAMESPACE_DP;
+  SetDevicePathNodeLength (&Node->Header, sizeof (NVME_NAMESPACE_DEVICE_PATH));
+  Node->NamespaceId    = NamespaceId;
+
+  //
+  // Allocate a buffer for Identify Namespace data.
+  //
+  NamespaceData = NULL;
+  NamespaceData = AllocateZeroPool(sizeof (NVME_ADMIN_NAMESPACE_DATA));
+  if(NamespaceData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Get UUID from specified Identify Namespace data.
+  //
+  Status = NvmeIdentifyNamespace (
+             Private,
+             NamespaceId,
+             (VOID *)NamespaceData
+             );
+
+  if (EFI_ERROR(Status)) {
+    goto Exit;
+  }
+
+  Node->NamespaceUuid = NamespaceData->Eui64;
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *)Node;
+
+Exit:
+  if(NamespaceData != NULL) {
+    FreePool (NamespaceData);
+  }
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Node);
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c
new file mode 100644
index 0000000000..580e8bcc44
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.c
@@ -0,0 +1,176 @@
+/** @file
+  EFI Component Name functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2011, 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 "PciBus.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gPciBusComponentName = {
+  PciBusComponentNameGetDriverName,
+  PciBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gPciBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) PciBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) PciBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mPciBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"PCI Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mPciBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gPciBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h
new file mode 100644
index 0000000000..e74851ed35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/ComponentName.h
@@ -0,0 +1,152 @@
+/** @file
+  EFI Component Name functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+
+#ifndef _EFI_PCI_BUS_COMPONENT_NAME_H_
+#define _EFI_PCI_BUS_COMPONENT_NAME_H_
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gPciBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gPciBusComponentName2;
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c
new file mode 100644
index 0000000000..beecf79a35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.c
@@ -0,0 +1,409 @@
+/** @file
+  Driver Binding functions for PCI Bus module.
+
+  Single PCI bus driver instance will manager all PCI Root Bridges in one EFI based firmware,
+  since all PCI Root Bridges' resources need to be managed together.
+  Supported() function will try to get PCI Root Bridge IO Protocol.
+  Start() function will get PCI Host Bridge Resource Allocation Protocol to manage all
+  PCI Root Bridges. So it means platform needs install PCI Root Bridge IO protocol for each
+  PCI Root Bus and install PCI Host Bridge Resource Allocation Protocol.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// PCI Bus Driver Global Variables
+//
+EFI_DRIVER_BINDING_PROTOCOL                   gPciBusDriverBinding = {
+  PciBusDriverBindingSupported,
+  PciBusDriverBindingStart,
+  PciBusDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_HANDLE                                    gPciHostBrigeHandles[PCI_MAX_HOST_BRIDGE_NUM];
+EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL  *gEfiIncompatiblePciDeviceSupport = NULL;
+UINTN                                         gPciHostBridgeNumber = 0;
+BOOLEAN                                       gFullEnumeration     = TRUE;
+UINT64                                        gAllOne              = 0xFFFFFFFFFFFFFFFFULL;
+UINT64                                        gAllZero             = 0;
+
+EFI_PCI_PLATFORM_PROTOCOL                     *gPciPlatformProtocol;
+EFI_PCI_OVERRIDE_PROTOCOL                     *gPciOverrideProtocol;
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_PCI_HOTPLUG_REQUEST_PROTOCOL mPciHotPlugRequest = {
+  PciHotPlugRequestNotify
+};
+
+/**
+  The Entry Point for PCI Bus module. The user code starts with this function.
+
+  Installs driver module protocols and. Creates virtual device handles for ConIn,
+  ConOut, and StdErr. Installs Simple Text In protocol, Simple Text In Ex protocol,
+  Simple Pointer protocol, Absolute Pointer protocol on those virtual handlers.
+  Installs Graphics Output protocol and/or UGA Draw protocol if needed.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurred when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  Handle;
+
+  //
+  // Initializes PCI devices pool
+  //
+  InitializePciDevicePool ();
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gPciBusDriverBinding,
+             ImageHandle,
+             &gPciBusComponentName,
+             &gPciBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    //
+    // If Hot Plug is supported, install EFI PCI Hot Plug Request protocol.
+    //
+    Handle = NULL;
+    Status = gBS->InstallProtocolInterface (
+                    &Handle,
+                    &gEfiPciHotPlugRequestProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mPciHotPlugRequest
+                    );
+  }
+
+  return Status;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a gEfiPciRootBridgeIoProtocolGuid protocol can be supported.
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child.
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_DEVICE_PATH_PROTOCOL        *ParentDevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  EFI_DEV_PATH_PTR                Node;
+
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      Node.DevPath = RemainingDevicePath;
+      if (Node.DevPath->Type != HARDWARE_DEVICE_PATH ||
+          Node.DevPath->SubType != HW_PCI_DP         ||
+          DevicePathNodeLength(Node.DevPath) != sizeof(PCI_DEVICE_PATH)) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Check if Pci Root Bridge IO protocol is installed by platform
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciRootBridgeIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on ControllerHandle and enumerate Pci bus and start
+  all device under PCI bus.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child.
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, return EFI_SUCCESS
+    //
+    if (IsDevicePathEnd (RemainingDevicePath)) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  gBS->LocateProtocol (
+         &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+         NULL,
+         (VOID **) &gEfiIncompatiblePciDeviceSupport
+         );
+
+  //
+  // If PCI Platform protocol is available, get it now.
+  // If the platform implements this, it must be installed before BDS phase
+  //
+  gPciPlatformProtocol = NULL;
+  gBS->LocateProtocol (
+        &gEfiPciPlatformProtocolGuid,
+        NULL,
+        (VOID **) &gPciPlatformProtocol
+        );
+
+  //
+  // If PCI Platform protocol doesn't exist, try to Pci Override Protocol.
+  //
+  if (gPciPlatformProtocol == NULL) { 
+    gPciOverrideProtocol = NULL;
+    gBS->LocateProtocol (
+          &gEfiPciOverrideProtocolGuid,
+          NULL,
+          (VOID **) &gPciOverrideProtocol
+          );
+  }  
+
+  if (PcdGetBool (PcdPciDisableBusEnumeration)) {
+    gFullEnumeration = FALSE;
+  } else {
+    gFullEnumeration = (BOOLEAN) ((SearchHostBridgeHandle (Controller) ? FALSE : TRUE));
+  }
+
+  //
+  // Open Device Path Protocol for PCI root bridge
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );  
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report Status Code to indicate PCI bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_PCI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  //
+  // Enumerate the entire host bridge
+  // After enumeration, a database that records all the device information will be created
+  //
+  //
+  Status = PciEnumerator (Controller);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Start all the devices under the entire host bridge.
+  //
+  StartPciDevices (Controller);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on.
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  BOOLEAN     AllChildrenStopped;
+
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciRootBridgeIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    DestroyRootBridgeByHandle (
+      Controller
+      );
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Stop all the children
+  //
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    //
+    // De register all the pci device
+    //
+    Status = DeRegisterPciDevice (Controller, ChildHandleBuffer[Index]);
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h
new file mode 100644
index 0000000000..b99d3181ac
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBus.h
@@ -0,0 +1,404 @@
+/** @file
+  Header files and data structures needed by PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+
+#ifndef _EFI_PCI_BUS_H_
+#define _EFI_PCI_BUS_H_
+
+#include <PiDxe.h>
+
+#include <Protocol/LoadedImage.h>
+#include <Protocol/PciHostBridgeResourceAllocation.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/LoadFile2.h>
+#include <Protocol/PciRootBridgeIo.h>
+#include <Protocol/PciHotPlugRequest.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciPlatform.h>
+#include <Protocol/PciHotPlugInit.h>
+#include <Protocol/Decompress.h>
+#include <Protocol/BusSpecificDriverOverride.h>
+#include <Protocol/IncompatiblePciDeviceSupport.h>
+#include <Protocol/PciOverride.h>
+#include <Protocol/PciEnumerationComplete.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeCoffLib.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/PeImage.h>
+#include <IndustryStandard/Acpi.h>
+
+typedef struct _PCI_IO_DEVICE              PCI_IO_DEVICE;
+typedef struct _PCI_BAR                    PCI_BAR;
+
+#define EFI_PCI_RID(Bus, Device, Function)  (((UINT32)Bus << 8) + ((UINT32)Device << 3) + (UINT32)Function)
+#define EFI_PCI_BUS_OF_RID(RID)             ((UINT32)RID >> 8)
+
+#define     EFI_PCI_IOV_POLICY_ARI           0x0001
+#define     EFI_PCI_IOV_POLICY_SRIOV         0x0002
+#define     EFI_PCI_IOV_POLICY_MRIOV         0x0004
+
+typedef enum {
+  PciBarTypeUnknown = 0,
+  PciBarTypeIo16,
+  PciBarTypeIo32,
+  PciBarTypeMem32,
+  PciBarTypePMem32,
+  PciBarTypeMem64,
+  PciBarTypePMem64,
+  PciBarTypeIo,
+  PciBarTypeMem,
+  PciBarTypeMaxType
+} PCI_BAR_TYPE;
+
+#include "ComponentName.h"
+#include "PciIo.h"
+#include "PciCommand.h"
+#include "PciDeviceSupport.h"
+#include "PciEnumerator.h"
+#include "PciEnumeratorSupport.h"
+#include "PciDriverOverride.h"
+#include "PciRomTable.h"
+#include "PciOptionRomSupport.h"
+#include "PciPowerManagement.h"
+#include "PciHotPlugSupport.h"
+#include "PciLib.h"
+
+#define VGABASE1  0x3B0
+#define VGALIMIT1 0x3BB
+
+#define VGABASE2  0x3C0
+#define VGALIMIT2 0x3DF
+
+#define ISABASE   0x100
+#define ISALIMIT  0x3FF
+
+//
+// PCI BAR parameters
+//
+struct _PCI_BAR {
+  UINT64        BaseAddress;
+  UINT64        Length;
+  UINT64        Alignment;
+  PCI_BAR_TYPE  BarType;
+  BOOLEAN       Prefetchable;
+  UINT8         MemType;
+  UINT16        Offset;
+};
+
+//
+// defined in PCI Card Specification, 8.0
+//
+#define PCI_CARD_MEMORY_BASE_0                0x1C
+#define PCI_CARD_MEMORY_LIMIT_0               0x20
+#define PCI_CARD_MEMORY_BASE_1                0x24
+#define PCI_CARD_MEMORY_LIMIT_1               0x28
+#define PCI_CARD_IO_BASE_0_LOWER              0x2C
+#define PCI_CARD_IO_BASE_0_UPPER              0x2E
+#define PCI_CARD_IO_LIMIT_0_LOWER             0x30
+#define PCI_CARD_IO_LIMIT_0_UPPER             0x32
+#define PCI_CARD_IO_BASE_1_LOWER              0x34
+#define PCI_CARD_IO_BASE_1_UPPER              0x36
+#define PCI_CARD_IO_LIMIT_1_LOWER             0x38
+#define PCI_CARD_IO_LIMIT_1_UPPER             0x3A
+#define PCI_CARD_BRIDGE_CONTROL               0x3E
+
+#define PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE BIT8
+#define PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE BIT9
+
+#define RB_IO_RANGE                           1
+#define RB_MEM32_RANGE                        2
+#define RB_PMEM32_RANGE                       3
+#define RB_MEM64_RANGE                        4
+#define RB_PMEM64_RANGE                       5
+
+#define PPB_BAR_0                             0
+#define PPB_BAR_1                             1
+#define PPB_IO_RANGE                          2
+#define PPB_MEM32_RANGE                       3
+#define PPB_PMEM32_RANGE                      4
+#define PPB_PMEM64_RANGE                      5
+#define PPB_MEM64_RANGE                       0xFF
+
+#define P2C_BAR_0                             0
+#define P2C_MEM_1                             1
+#define P2C_MEM_2                             2
+#define P2C_IO_1                              3
+#define P2C_IO_2                              4
+
+#define EFI_BRIDGE_IO32_DECODE_SUPPORTED      0x0001
+#define EFI_BRIDGE_PMEM32_DECODE_SUPPORTED    0x0002
+#define EFI_BRIDGE_PMEM64_DECODE_SUPPORTED    0x0004
+#define EFI_BRIDGE_IO16_DECODE_SUPPORTED      0x0008
+#define EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED 0x0010
+#define EFI_BRIDGE_MEM64_DECODE_SUPPORTED     0x0020
+#define EFI_BRIDGE_MEM32_DECODE_SUPPORTED     0x0040
+
+#define PCI_MAX_HOST_BRIDGE_NUM               0x0010
+
+//
+// Define option for attribute
+//
+#define EFI_SET_SUPPORTS    0
+#define EFI_SET_ATTRIBUTES  1
+
+#define PCI_IO_DEVICE_SIGNATURE               SIGNATURE_32 ('p', 'c', 'i', 'o')
+
+struct _PCI_IO_DEVICE {
+  UINT32                                    Signature;
+  EFI_HANDLE                                Handle;
+  EFI_PCI_IO_PROTOCOL                       PciIo;
+  LIST_ENTRY                                Link;
+
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL PciDriverOverride;
+  EFI_DEVICE_PATH_PROTOCOL                  *DevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL           *PciRootBridgeIo;
+  EFI_LOAD_FILE2_PROTOCOL                   LoadFile2;
+
+  //
+  // PCI configuration space header type
+  //
+  PCI_TYPE00                                Pci;
+
+  //
+  // Bus number, Device number, Function number
+  //
+  UINT8                                     BusNumber;
+  UINT8                                     DeviceNumber;
+  UINT8                                     FunctionNumber;
+
+  //
+  // BAR for this PCI Device
+  //
+  PCI_BAR                                   PciBar[PCI_MAX_BAR];
+
+  //
+  // The bridge device this pci device is subject to
+  //
+  PCI_IO_DEVICE                             *Parent;
+
+  //
+  // A linked list for children Pci Device if it is bridge device
+  //
+  LIST_ENTRY                                ChildList;
+
+  //
+  // TURE if the PCI bus driver creates the handle for this PCI device
+  //
+  BOOLEAN                                   Registered;
+
+  //
+  // TRUE if the PCI bus driver successfully allocates the resource required by
+  // this PCI device
+  //
+  BOOLEAN                                   Allocated;
+
+  //
+  // The attribute this PCI device currently set
+  //
+  UINT64                                    Attributes;
+
+  //
+  // The attributes this PCI device actually supports
+  //
+  UINT64                                    Supports;
+
+  //
+  // The resource decode the bridge supports
+  //
+  UINT32                                    Decodes;
+
+  //
+  // TRUE if the ROM image is from the PCI Option ROM BAR
+  //
+  BOOLEAN                                   EmbeddedRom;
+
+  //
+  // The OptionRom Size
+  //
+  UINT64                                    RomSize;
+
+  //
+  // The OptionRom Size
+  //
+  UINT64                                    RomBase;
+
+  //
+  // TRUE if all OpROM (in device or in platform specific position) have been processed
+  //
+  BOOLEAN                                   AllOpRomProcessed;
+
+  //
+  // TRUE if there is any EFI driver in the OptionRom
+  //
+  BOOLEAN                                   BusOverride;
+
+  //
+  // A list tracking reserved resource on a bridge device
+  //
+  LIST_ENTRY                                ReservedResourceList;
+
+  //
+  // A list tracking image handle of platform specific overriding driver
+  //
+  LIST_ENTRY                                OptionRomDriverList;
+
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR         *ResourcePaddingDescriptors;
+  EFI_HPC_PADDING_ATTRIBUTES                PaddingAttributes;
+
+  //
+  // Bus number ranges for a PCI Root Bridge device
+  //
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR         *BusNumberRanges;
+
+  BOOLEAN                                   IsPciExp;
+  //
+  // For SR-IOV
+  //
+  UINT8                                     PciExpressCapabilityOffset;
+  UINT32                                    AriCapabilityOffset;
+  UINT32                                    SrIovCapabilityOffset;
+  UINT32                                    MrIovCapabilityOffset;
+  PCI_BAR                                   VfPciBar[PCI_MAX_BAR];
+  UINT32                                    SystemPageSize;
+  UINT16                                    InitialVFs;
+  UINT16                                    ReservedBusNum;
+  //
+  // Per PCI to PCI Bridge spec, I/O window is 4K aligned,
+  // but some chipsets support non-stardard I/O window aligments less than 4K.
+  // This field is used to support this case.
+  //
+  UINT16                                    BridgeIoAlignment;
+};
+
+#define PCI_IO_DEVICE_FROM_PCI_IO_THIS(a) \
+  CR (a, PCI_IO_DEVICE, PciIo, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_PCI_DRIVER_OVERRIDE_THIS(a) \
+  CR (a, PCI_IO_DEVICE, PciDriverOverride, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_LINK(a) \
+  CR (a, PCI_IO_DEVICE, Link, PCI_IO_DEVICE_SIGNATURE)
+
+#define PCI_IO_DEVICE_FROM_LOAD_FILE2_THIS(a) \
+  CR (a, PCI_IO_DEVICE, LoadFile2, PCI_IO_DEVICE_SIGNATURE)
+
+
+
+//
+// Global Variables
+//
+extern EFI_INCOMPATIBLE_PCI_DEVICE_SUPPORT_PROTOCOL *gEfiIncompatiblePciDeviceSupport;
+extern EFI_DRIVER_BINDING_PROTOCOL                  gPciBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                  gPciBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL                 gPciBusComponentName2;
+extern BOOLEAN                                      gFullEnumeration;
+extern UINTN                                        gPciHostBridgeNumber;
+extern EFI_HANDLE                                   gPciHostBrigeHandles[PCI_MAX_HOST_BRIDGE_NUM];
+extern UINT64                                       gAllOne;
+extern UINT64                                       gAllZero;
+extern EFI_PCI_PLATFORM_PROTOCOL                    *gPciPlatformProtocol;
+extern EFI_PCI_OVERRIDE_PROTOCOL                    *gPciOverrideProtocol;
+extern BOOLEAN                                      mReserveIsaAliases;
+extern BOOLEAN                                      mReserveVgaAliases;
+
+/**
+  Macro that checks whether device is a GFX device.
+
+  @param  _p      Specified device.
+
+  @retval TRUE    Device is a a GFX device.
+  @retval FALSE   Device is not a a GFX device.
+
+**/
+#define IS_PCI_GFX(_p)     IS_CLASS2 (_p, PCI_CLASS_DISPLAY, PCI_CLASS_DISPLAY_OTHER)
+
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a gEfiPciRootBridgeIoProtocolGuid protocol can be supported.
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child.
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start this driver on ControllerHandle and enumerate Pci bus and start
+  all device under PCI bus.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child.
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on.
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+PciBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf
new file mode 100644
index 0000000000..330ccc8cbf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf
@@ -0,0 +1,112 @@
+## @file
+#  The PCI bus driver will probe all PCI devices and allocate MMIO and IO space for these devices.
+#  Please use PCD feature flag PcdPciBusHotplugDeviceSupport to enable hot plug supporting.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciBusDxe
+  MODULE_UNI_FILE                = PciBusDxe.uni
+  FILE_GUID                      = 93B80004-9FB3-11d4-9A3A-0090273FC14D
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PciBusEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gPciBusDriverBinding
+#  COMPONENT_NAME                =  gPciBusComponentName
+#  COMPONENT_NAME2               =  gPciBusComponentName2
+#
+
+[Sources]
+  PciLib.c
+  PciIo.c
+  PciBus.c
+  PciDeviceSupport.c
+  ComponentName.c
+  ComponentName.h
+  PciCommand.c
+  PciResourceSupport.c
+  PciEnumeratorSupport.c
+  PciEnumerator.c
+  PciOptionRomSupport.c
+  PciDriverOverride.c
+  PciPowerManagement.c
+  PciPowerManagement.h
+  PciDriverOverride.h
+  PciRomTable.c
+  PciHotPlugSupport.c
+  PciLib.h
+  PciHotPlugSupport.h
+  PciRomTable.h
+  PciOptionRomSupport.h
+  PciEnumeratorSupport.h
+  PciEnumerator.h
+  PciResourceSupport.h
+  PciDeviceSupport.h
+  PciCommand.h
+  PciIo.h
+  PciBus.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  DevicePathLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  ReportStatusCodeLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  PeCoffLib
+
+[Protocols]
+  gEfiPciHotPlugRequestProtocolGuid               ## SOMETIMES_PRODUCES
+  gEfiPciIoProtocolGuid                           ## BY_START
+  gEfiDevicePathProtocolGuid                      ## BY_START
+  gEfiBusSpecificDriverOverrideProtocolGuid       ## BY_START
+  gEfiLoadedImageProtocolGuid                     ## SOMETIMES_CONSUMES
+  gEfiDecompressProtocolGuid                      ## SOMETIMES_CONSUMES
+  gEfiPciHotPlugInitProtocolGuid                  ## SOMETIMES_CONSUMES
+  gEfiPciHostBridgeResourceAllocationProtocolGuid ## TO_START
+  gEfiPciPlatformProtocolGuid                     ## SOMETIMES_CONSUMES
+  gEfiPciOverrideProtocolGuid                     ## SOMETIMES_CONSUMES 
+  gEfiPciEnumerationCompleteProtocolGuid          ## PRODUCES 
+  gEfiPciRootBridgeIoProtocolGuid                 ## TO_START
+  gEfiIncompatiblePciDeviceSupportProtocolGuid    ## SOMETIMES_CONSUMES
+  gEfiLoadFile2ProtocolGuid                       ## SOMETIMES_PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBusHotplugDeviceSupport  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBridgeIoAlignmentProbe   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnalignedPciIoEnable        ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSystemPageSize         ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSupport                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAriSupport                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMrIovSupport                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciDisableBusEnumeration    ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PciBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni
new file mode 100644
index 0000000000..9b34afdc9d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni
new file mode 100644
index 0000000000..63eff41a74
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c
new file mode 100644
index 0000000000..0bc1fbfeff
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.c
@@ -0,0 +1,260 @@
+/** @file
+  PCI command register operations supporting functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+/**
+  Operate the PCI register via PciIo function interface.
+
+  @param PciIoDevice    Pointer to instance of PCI_IO_DEVICE.
+  @param Command        Operator command.
+  @param Offset         The address within the PCI configuration space for the PCI controller.
+  @param Operation      Type of Operation.
+  @param PtrCommand     Return buffer holding old PCI command, if operation is not EFI_SET_REGISTER.
+
+  @return Status of PciIo operation.
+
+**/
+EFI_STATUS
+PciOperateRegister (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINT16        Command,
+  IN  UINT8         Offset,
+  IN  UINT8         Operation,
+  OUT UINT16        *PtrCommand
+  )
+{
+  UINT16              OldCommand;
+  EFI_STATUS          Status;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  OldCommand  = 0;
+  PciIo       = &PciIoDevice->PciIo;
+
+  if (Operation != EFI_SET_REGISTER) {
+    Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint16,
+                          Offset,
+                          1,
+                          &OldCommand
+                          );
+
+    if (Operation == EFI_GET_REGISTER) {
+      *PtrCommand = OldCommand;
+      return Status;
+    }
+  }
+
+  if (Operation == EFI_ENABLE_REGISTER) {
+    OldCommand = (UINT16) (OldCommand | Command);
+  } else if (Operation == EFI_DISABLE_REGISTER) {
+    OldCommand = (UINT16) (OldCommand & ~(Command));
+  } else {
+    OldCommand = Command;
+  }
+
+  return PciIo->Pci.Write (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      Offset,
+                      1,
+                      &OldCommand
+                      );
+}
+
+/**
+  Check the cpability supporting by given device.
+
+  @param PciIoDevice   Pointer to instance of PCI_IO_DEVICE.
+
+  @retval TRUE         Cpability supportted.
+  @retval FALSE        Cpability not supportted.
+
+**/
+BOOLEAN
+PciCapabilitySupport (
+  IN PCI_IO_DEVICE  *PciIoDevice
+  )
+{
+  if ((PciIoDevice->Pci.Hdr.Status & EFI_PCI_STATUS_CAPABILITY) != 0) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Locate capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocateCapabilityRegBlock (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINT8          CapId,
+  IN OUT UINT8      *Offset,
+  OUT UINT8         *NextRegBlock OPTIONAL
+  )
+{
+  UINT8   CapabilityPtr;
+  UINT16  CapabilityEntry;
+  UINT8   CapabilityID;
+
+  //
+  // To check the cpability of this device supports
+  //
+  if (!PciCapabilitySupport (PciIoDevice)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (*Offset != 0) {
+    CapabilityPtr = *Offset;
+  } else {
+
+    CapabilityPtr = 0;
+    if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+      PciIoDevice->PciIo.Pci.Read (
+                               &PciIoDevice->PciIo,
+                               EfiPciIoWidthUint8,
+                               EFI_PCI_CARDBUS_BRIDGE_CAPABILITY_PTR,
+                               1,
+                               &CapabilityPtr
+                               );
+    } else {
+
+      PciIoDevice->PciIo.Pci.Read (
+                               &PciIoDevice->PciIo,
+                               EfiPciIoWidthUint8,
+                               PCI_CAPBILITY_POINTER_OFFSET,
+                               1,
+                               &CapabilityPtr
+                               );
+    }
+  }
+
+  while ((CapabilityPtr >= 0x40) && ((CapabilityPtr & 0x03) == 0x00)) {
+    PciIoDevice->PciIo.Pci.Read (
+                             &PciIoDevice->PciIo,
+                             EfiPciIoWidthUint16,
+                             CapabilityPtr,
+                             1,
+                             &CapabilityEntry
+                             );
+
+    CapabilityID = (UINT8) CapabilityEntry;
+
+    if (CapabilityID == CapId) {
+      *Offset = CapabilityPtr;
+      if (NextRegBlock != NULL) {
+        *NextRegBlock = (UINT8) (CapabilityEntry >> 8);
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Certain PCI device may incorrectly have capability pointing to itself,
+    // break to avoid dead loop.
+    //
+    if (CapabilityPtr == (UINT8) (CapabilityEntry >> 8)) {
+      break;
+    }
+
+    CapabilityPtr = (UINT8) (CapabilityEntry >> 8);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Locate PciExpress capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocatePciExpressCapabilityRegBlock (
+  IN     PCI_IO_DEVICE *PciIoDevice,
+  IN     UINT16        CapId,
+  IN OUT UINT32        *Offset,
+     OUT UINT32        *NextRegBlock OPTIONAL
+  )
+{
+  EFI_STATUS           Status;
+  UINT32               CapabilityPtr;
+  UINT32               CapabilityEntry;
+  UINT16               CapabilityID;
+
+  //
+  // To check the capability of this device supports
+  //
+  if (!PciIoDevice->IsPciExp) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (*Offset != 0) {
+    CapabilityPtr = *Offset;
+  } else {
+    CapabilityPtr = EFI_PCIE_CAPABILITY_BASE_OFFSET;
+  }
+
+  while (CapabilityPtr != 0) {
+    //
+    // Mask it to DWORD alignment per PCI spec
+    //
+    CapabilityPtr &= 0xFFC;
+    Status = PciIoDevice->PciIo.Pci.Read (
+                                      &PciIoDevice->PciIo,
+                                      EfiPciIoWidthUint32,
+                                      CapabilityPtr,
+                                      1,
+                                      &CapabilityEntry
+                                      );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    CapabilityID = (UINT16) CapabilityEntry;
+
+    if (CapabilityID == CapId) {
+      *Offset = CapabilityPtr;
+      if (NextRegBlock != NULL) {
+        *NextRegBlock = (CapabilityEntry >> 20) & 0xFFF;
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    CapabilityPtr = (CapabilityEntry >> 20) & 0xFFF;
+  }
+
+  return EFI_NOT_FOUND;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h
new file mode 100644
index 0000000000..cc942d0d42
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciCommand.h
@@ -0,0 +1,238 @@
+/** @file
+  PCI command register operations supporting functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+
+#ifndef _EFI_PCI_COMMAND_H_
+#define _EFI_PCI_COMMAND_H_
+
+//
+// The PCI Command register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCI_COMMAND_BITS_OWNED                          ( \
+                EFI_PCI_COMMAND_IO_SPACE                    | \
+                EFI_PCI_COMMAND_MEMORY_SPACE                | \
+                EFI_PCI_COMMAND_BUS_MASTER                  | \
+                EFI_PCI_COMMAND_MEMORY_WRITE_AND_INVALIDATE | \
+                EFI_PCI_COMMAND_VGA_PALETTE_SNOOP           | \
+                EFI_PCI_COMMAND_FAST_BACK_TO_BACK             \
+                )
+
+//
+// The PCI Bridge Control register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCI_BRIDGE_CONTROL_BITS_OWNED                   ( \
+                EFI_PCI_BRIDGE_CONTROL_ISA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA_16               | \
+                EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK      \
+                )
+
+//
+// The PCCard Bridge Control register bits owned by PCI Bus driver.
+//
+// They should be cleared at the beginning. The other registers
+// are owned by chipset, we should not touch them.
+//
+#define EFI_PCCARD_BRIDGE_CONTROL_BITS_OWNED                ( \
+                EFI_PCI_BRIDGE_CONTROL_ISA                  | \
+                EFI_PCI_BRIDGE_CONTROL_VGA                  | \
+                EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK      \
+                )
+
+
+#define EFI_GET_REGISTER      1
+#define EFI_SET_REGISTER      2
+#define EFI_ENABLE_REGISTER   3
+#define EFI_DISABLE_REGISTER  4
+
+/**
+  Operate the PCI register via PciIo function interface.
+
+  @param PciIoDevice    Pointer to instance of PCI_IO_DEVICE.
+  @param Command        Operator command.
+  @param Offset         The address within the PCI configuration space for the PCI controller.
+  @param Operation      Type of Operation.
+  @param PtrCommand     Return buffer holding old PCI command, if operation is not EFI_SET_REGISTER.
+
+  @return Status of PciIo operation.
+
+**/
+EFI_STATUS
+PciOperateRegister (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINT16        Command,
+  IN  UINT8         Offset,
+  IN  UINT8         Operation,
+  OUT UINT16        *PtrCommand
+  );
+
+/**
+  Check the cpability supporting by given device.
+
+  @param PciIoDevice   Pointer to instance of PCI_IO_DEVICE.
+
+  @retval TRUE         Cpability supportted.
+  @retval FALSE        Cpability not supportted.
+
+**/
+BOOLEAN
+PciCapabilitySupport (
+  IN PCI_IO_DEVICE  *PciIoDevice
+  );
+
+/**
+  Locate capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocateCapabilityRegBlock (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINT8          CapId,
+  IN OUT UINT8      *Offset,
+  OUT UINT8         *NextRegBlock OPTIONAL
+  );
+
+/**
+  Locate PciExpress capability register block per capability ID.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param CapId             The capability ID.
+  @param Offset            A pointer to the offset returned.
+  @param NextRegBlock      A pointer to the next block returned.
+
+  @retval EFI_SUCCESS      Successfuly located capability register block.
+  @retval EFI_UNSUPPORTED  Pci device does not support capability.
+  @retval EFI_NOT_FOUND    Pci device support but can not find register block.
+
+**/
+EFI_STATUS
+LocatePciExpressCapabilityRegBlock (
+  IN     PCI_IO_DEVICE *PciIoDevice,
+  IN     UINT16        CapId,
+  IN OUT UINT32        *Offset,
+     OUT UINT32        *NextRegBlock OPTIONAL
+  );
+
+/**
+  Macro that reads command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[out]           Pointer to the 16-bit value read from command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_READ_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, 0, PCI_COMMAND_OFFSET, EFI_GET_REGISTER, b)
+
+/**
+  Macro that writes command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The 16-bit value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_SET_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_SET_REGISTER, NULL)
+
+/**
+  Macro that enables command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The enabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_ENABLE_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_ENABLE_REGISTER, NULL)
+
+/**
+  Macro that disalbes command register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The disabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_DISABLE_COMMAND_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_COMMAND_OFFSET, EFI_DISABLE_REGISTER, NULL)
+
+/**
+  Macro that reads PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[out]           The 16-bit value read from control register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_READ_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, 0, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_GET_REGISTER, b)
+
+/**
+  Macro that writes PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The 16-bit value written into control register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_SET_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_SET_REGISTER, NULL)
+
+/**
+  Macro that enables PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The enabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_ENABLE_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_ENABLE_REGISTER, NULL)
+
+/**
+ Macro that disalbes PCI bridge control register.
+
+  @param a[in]            Pointer to instance of PCI_IO_DEVICE.
+  @param b[in]            The disabled value written into command register.
+
+  @return status of PciIo operation
+
+**/
+#define PCI_DISABLE_BRIDGE_CONTROL_REGISTER(a,b) \
+        PciOperateRegister (a, b, PCI_BRIDGE_CONTROL_REGISTER_OFFSET, EFI_DISABLE_REGISTER, NULL)
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c
new file mode 100644
index 0000000000..c0227fa2b6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.c
@@ -0,0 +1,1155 @@
+/** @file
+  Supporting functions implementaion for PCI devices management.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// This device structure is serviced as a header.
+// Its next field points to the first root bridge device node.
+//
+LIST_ENTRY  mPciDevicePool;
+
+/**
+  Initialize the PCI devices pool.
+
+**/
+VOID
+InitializePciDevicePool (
+  VOID
+  )
+{
+  InitializeListHead (&mPciDevicePool);
+}
+
+/**
+  Insert a root bridge into PCI device pool.
+
+  @param RootBridge     A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+InsertRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  )
+{
+  InsertTailList (&mPciDevicePool, &(RootBridge->Link));
+}
+
+/**
+  This function is used to insert a PCI device node under
+  a bridge.
+
+  @param Bridge         The PCI bridge.
+  @param PciDeviceNode  The PCI device needs inserting.
+
+**/
+VOID
+InsertPciDevice (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_IO_DEVICE      *PciDeviceNode
+  )
+{
+  InsertTailList (&Bridge->ChildList, &(PciDeviceNode->Link));
+  PciDeviceNode->Parent = Bridge;
+}
+
+/**
+  Destroy root bridge and remove it from deivce tree.
+
+  @param RootBridge     The bridge want to be removed.
+
+**/
+VOID
+DestroyRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  )
+{
+  DestroyPciDeviceTree (RootBridge);
+
+  FreePciDevice (RootBridge);
+}
+
+/**
+  Destroy a pci device node.
+
+  All direct or indirect allocated resource for this node will be freed.
+
+  @param PciIoDevice  A pointer to the PCI_IO_DEVICE to be destoried.
+
+**/
+VOID
+FreePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  ASSERT (PciIoDevice != NULL);
+  //
+  // Assume all children have been removed underneath this device
+  //
+  if (PciIoDevice->ResourcePaddingDescriptors != NULL) {
+    FreePool (PciIoDevice->ResourcePaddingDescriptors);
+  }
+
+  if (PciIoDevice->DevicePath != NULL) {
+    FreePool (PciIoDevice->DevicePath);
+  }
+
+  FreePool (PciIoDevice);
+}
+
+/**
+  Destroy all the pci device node under the bridge.
+  Bridge itself is not included.
+
+  @param Bridge      A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+DestroyPciDeviceTree (
+  IN PCI_IO_DEVICE      *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+
+    //
+    // Remove this node from the linked list
+    //
+    RemoveEntryList (CurrentLink);
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      DestroyPciDeviceTree (Temp);
+    }
+
+    FreePciDevice (Temp);
+  }
+}
+
+/**
+  Destroy all device nodes under the root bridge
+  specified by Controller.
+
+  The root bridge itself is also included.
+
+  @param  Controller    Root bridge handle.
+
+  @retval EFI_SUCCESS   Destory all devcie nodes successfully.
+  @retval EFI_NOT_FOUND Cannot find any PCI device under specified
+                        root bridge.
+
+**/
+EFI_STATUS
+DestroyRootBridgeByHandle (
+  IN EFI_HANDLE        Controller
+  )
+{
+
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp->Handle == Controller) {
+
+      RemoveEntryList (CurrentLink);
+
+      DestroyPciDeviceTree (Temp);
+
+      FreePciDevice (Temp);
+
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This function registers the PCI IO device.
+
+  It creates a handle for this PCI IO device (if the handle does not exist), attaches
+  appropriate protocols onto the handle, does necessary initialization, and sets up
+  parent/child relationship with its bus controller.
+
+  @param Controller     An EFI handle for the PCI bus controller.
+  @param PciIoDevice    A PCI_IO_DEVICE pointer to the PCI IO device to be registered.
+  @param Handle         A pointer to hold the returned EFI handle for the PCI IO device.
+
+  @retval EFI_SUCCESS   The PCI device is successfully registered.
+  @retval other         An error occurred when registering the PCI device.
+
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN  EFI_HANDLE          Controller,
+  IN  PCI_IO_DEVICE       *PciIoDevice,
+  OUT EFI_HANDLE          *Handle      OPTIONAL
+  )
+{
+  EFI_STATUS          Status;
+  VOID                *PlatformOpRomBuffer;
+  UINTN               PlatformOpRomSize;
+  UINT8               PciExpressCapRegOffset;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT8               Data8;
+  BOOLEAN             HasEfiImage;
+
+  //
+  // Install the pciio protocol, device path protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &PciIoDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  PciIoDevice->DevicePath,
+                  &gEfiPciIoProtocolGuid,
+                  &PciIoDevice->PciIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Detect if PCI Express Device
+  //
+  PciExpressCapRegOffset = 0;
+  Status = LocateCapabilityRegBlock (
+             PciIoDevice,
+             EFI_PCI_CAPABILITY_ID_PCIEXP,
+             &PciExpressCapRegOffset,
+             NULL
+             );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice->IsPciExp = TRUE;
+  }
+
+  //
+  // Force Interrupt line to "Unknown" or "No Connection"
+  //
+  PciIo = &(PciIoDevice->PciIo);
+  Data8 = PCI_INT_LINE_UNKNOWN;
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &Data8);
+ 
+  //
+  // Process OpRom
+  //
+  if (!PciIoDevice->AllOpRomProcessed) {
+
+    //
+    // Get the OpRom provided by platform
+    //
+    if (gPciPlatformProtocol != NULL) {
+      Status = gPciPlatformProtocol->GetPciRom (
+                                       gPciPlatformProtocol,
+                                       PciIoDevice->Handle,
+                                       &PlatformOpRomBuffer,
+                                       &PlatformOpRomSize
+                                       );
+      if (!EFI_ERROR (Status)) {
+        PciIoDevice->EmbeddedRom    = FALSE;
+        PciIoDevice->RomSize        = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomSize  = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomImage = PlatformOpRomBuffer;
+        //
+        // For OpROM read from gPciPlatformProtocol:
+        // Add the Rom Image to internal database for later PCI light enumeration
+        //
+        PciRomAddImageMapping (
+          NULL,
+          PciIoDevice->PciRootBridgeIo->SegmentNumber,
+          PciIoDevice->BusNumber,
+          PciIoDevice->DeviceNumber,
+          PciIoDevice->FunctionNumber,
+          (UINT64) (UINTN) PciIoDevice->PciIo.RomImage,
+          PciIoDevice->PciIo.RomSize
+          );
+      }
+    } else if (gPciOverrideProtocol != NULL) {
+      Status = gPciOverrideProtocol->GetPciRom (
+                                       gPciOverrideProtocol,
+                                       PciIoDevice->Handle,
+                                       &PlatformOpRomBuffer,
+                                       &PlatformOpRomSize
+                                       );
+      if (!EFI_ERROR (Status)) {
+        PciIoDevice->EmbeddedRom    = FALSE;
+        PciIoDevice->RomSize        = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomSize  = PlatformOpRomSize;
+        PciIoDevice->PciIo.RomImage = PlatformOpRomBuffer;
+        //
+        // For OpROM read from gPciOverrideProtocol:
+        // Add the Rom Image to internal database for later PCI light enumeration
+        //
+        PciRomAddImageMapping (
+          NULL,
+          PciIoDevice->PciRootBridgeIo->SegmentNumber,
+          PciIoDevice->BusNumber,
+          PciIoDevice->DeviceNumber,
+          PciIoDevice->FunctionNumber,
+          (UINT64) (UINTN) PciIoDevice->PciIo.RomImage,
+          PciIoDevice->PciIo.RomSize
+          );
+      }   
+    }
+  }
+
+  //
+  // Determine if there are EFI images in the option rom
+  //
+  HasEfiImage = ContainEfiImage (PciIoDevice->PciIo.RomImage, PciIoDevice->PciIo.RomSize);
+
+  if (HasEfiImage) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &PciIoDevice->Handle,
+                    &gEfiLoadFile2ProtocolGuid,
+                    &PciIoDevice->LoadFile2,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             &PciIoDevice->Handle,
+             &gEfiDevicePathProtocolGuid,
+             PciIoDevice->DevicePath,
+             &gEfiPciIoProtocolGuid,
+             &PciIoDevice->PciIo,
+             NULL
+             );
+      return Status;
+    }
+  }
+
+
+  if (!PciIoDevice->AllOpRomProcessed) {
+
+    PciIoDevice->AllOpRomProcessed = TRUE;
+
+    //
+    // Dispatch the EFI OpRom for the PCI device.
+    // The OpRom is got from platform in the above code
+    // or loaded from device in the previous round of bus enumeration
+    //
+    if (HasEfiImage) {
+      ProcessOpRomImage (PciIoDevice);
+    }
+  }
+
+  if (PciIoDevice->BusOverride) {
+    //
+    // Install Bus Specific Driver Override Protocol
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &PciIoDevice->Handle,
+                    &gEfiBusSpecificDriverOverrideProtocolGuid,
+                    &PciIoDevice->PciDriverOverride,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             &PciIoDevice->Handle,
+             &gEfiDevicePathProtocolGuid,
+             PciIoDevice->DevicePath,
+             &gEfiPciIoProtocolGuid,
+             &PciIoDevice->PciIo,
+             NULL
+             );
+      if (HasEfiImage) {
+        gBS->UninstallMultipleProtocolInterfaces (
+               &PciIoDevice->Handle,
+               &gEfiLoadFile2ProtocolGuid,
+               &PciIoDevice->LoadFile2,
+               NULL
+               );
+      }
+
+      return Status;
+    }
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &(PciIoDevice->PciRootBridgeIo),
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  PciIoDevice->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Handle != NULL) {
+    *Handle = PciIoDevice->Handle;
+  }
+
+  //
+  // Indicate the pci device is registered
+  //
+  PciIoDevice->Registered = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to remove the whole PCI devices on the specified bridge from
+  the root bridge.
+
+  @param RootBridgeHandle   The root bridge device handle.
+  @param Bridge             The bridge device to be removed.
+
+**/
+VOID
+RemoveAllPciDeviceOnBridge (
+  EFI_HANDLE               RootBridgeHandle,
+  PCI_IO_DEVICE            *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+    Temp        = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    //
+    // Check if the current node has been deregistered before
+    // If it is not, then deregister it
+    //
+    if (Temp->Registered) {
+      DeRegisterPciDevice (RootBridgeHandle, Temp->Handle);
+    }
+
+    //
+    // Remove this node from the linked list
+    //
+    RemoveEntryList (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      RemoveAllPciDeviceOnBridge (RootBridgeHandle, Temp);
+    }
+
+    FreePciDevice (Temp);
+  }
+}
+
+/**
+  This function is used to de-register the PCI IO device.
+
+  That includes un-installing PciIo protocol from the specified PCI
+  device handle.
+
+  @param Controller    An EFI handle for the PCI bus controller.
+  @param Handle        PCI device handle.
+
+  @retval EFI_SUCCESS  The PCI device is successfully de-registered.
+  @retval other        An error occurred when de-registering the PCI device.
+
+**/
+EFI_STATUS
+DeRegisterPciDevice (
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  )
+
+{
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  EFI_STATUS                      Status;
+  PCI_IO_DEVICE                   *PciIoDevice;
+  PCI_IO_DEVICE                   *Node;
+  LIST_ENTRY                      *CurrentLink;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (PciIo);
+
+    //
+    // If it is already de-registered
+    //
+    if (!PciIoDevice->Registered) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // If it is PPB, first de-register its children
+    //
+
+    if (!IsListEmpty (&PciIoDevice->ChildList)) {
+
+      CurrentLink = PciIoDevice->ChildList.ForwardLink;
+
+      while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+        Node    = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+        Status  = DeRegisterPciDevice (Controller, Node->Handle);
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        CurrentLink = CurrentLink->ForwardLink;
+      }
+    }
+
+    //
+    // Close the child handle
+    //
+    Status = gBS->CloseProtocol (
+                    Controller,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    gPciBusDriverBinding.DriverBindingHandle,
+                    Handle
+                    );
+
+    //
+    // Un-install the Device Path protocol and PCI I/O protocol
+    // and Bus Specific Driver Override protocol if needed.
+    //
+    if (PciIoDevice->BusOverride) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      PciIoDevice->DevicePath,
+                      &gEfiPciIoProtocolGuid,
+                      &PciIoDevice->PciIo,
+                      &gEfiBusSpecificDriverOverrideProtocolGuid,
+                      &PciIoDevice->PciDriverOverride,
+                      NULL
+                      );
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      PciIoDevice->DevicePath,
+                      &gEfiPciIoProtocolGuid,
+                      &PciIoDevice->PciIo,
+                      NULL
+                      );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      //
+      // Try to uninstall LoadFile2 protocol if exists
+      //
+      Status = gBS->OpenProtocol (
+                      Handle,
+                      &gEfiLoadFile2ProtocolGuid,
+                      NULL,
+                      gPciBusDriverBinding.DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                      );
+      if (!EFI_ERROR (Status)) {
+        Status = gBS->UninstallMultipleProtocolInterfaces (
+                        Handle,
+                        &gEfiLoadFile2ProtocolGuid,
+                        &PciIoDevice->LoadFile2,
+                        NULL
+                        );
+      }
+      //
+      // Restore Status
+      //
+      Status = EFI_SUCCESS;
+    }
+
+
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+            Controller,
+            &gEfiPciRootBridgeIoProtocolGuid,
+            (VOID **) &PciRootBridgeIo,
+            gPciBusDriverBinding.DriverBindingHandle,
+            Handle,
+            EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+            );
+      return Status;
+    }
+
+    //
+    // The Device Driver should disable this device after disconnect
+    // so the Pci Bus driver will not touch this device any more.
+    // Restore the register field to the original value
+    //
+    PciIoDevice->Registered = FALSE;
+    PciIoDevice->Handle     = NULL;
+  } else {
+
+    //
+    // Handle may be closed before
+    //
+    return EFI_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start to manage the PCI device on the specified root bridge or PCI-PCI Bridge.
+
+  @param Controller          The root bridge handle.
+  @param RootBridge          A pointer to the PCI_IO_DEVICE.
+  @param RemainingDevicePath A pointer to the EFI_DEVICE_PATH_PROTOCOL.
+  @param NumberOfChildren    Children number.
+  @param ChildHandleBuffer   A pointer to the child handle buffer.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_UNSUPPORTED Device only support PCI-PCI bridge.
+  @retval EFI_NOT_FOUND   Can not find the specific device.
+  @retval EFI_SUCCESS     Success to start Pci devices on bridge.
+
+**/
+EFI_STATUS
+StartPciDevicesOnBridge (
+  IN EFI_HANDLE                          Controller,
+  IN PCI_IO_DEVICE                       *RootBridge,
+  IN EFI_DEVICE_PATH_PROTOCOL            *RemainingDevicePath,
+  IN OUT UINT8                           *NumberOfChildren,
+  IN OUT EFI_HANDLE                      *ChildHandleBuffer
+  )
+
+{
+  PCI_IO_DEVICE             *PciIoDevice;
+  EFI_DEV_PATH_PTR          Node;
+  EFI_DEVICE_PATH_PROTOCOL  *CurrentDevicePath;
+  EFI_STATUS                Status;
+  LIST_ENTRY                *CurrentLink;
+  UINT64                    Supports;
+
+  PciIoDevice = NULL;
+  CurrentLink = RootBridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+
+    PciIoDevice = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (RemainingDevicePath != NULL) {
+
+      Node.DevPath = RemainingDevicePath;
+
+      if (Node.Pci->Device != PciIoDevice->DeviceNumber ||
+          Node.Pci->Function != PciIoDevice->FunctionNumber) {
+        CurrentLink = CurrentLink->ForwardLink;
+        continue;
+      }
+
+      //
+      // Check if the device has been assigned with required resource
+      //
+      if (!PciIoDevice->Allocated) {
+        return EFI_NOT_READY;
+      }
+
+      //
+      // Check if the current node has been registered before
+      // If it is not, register it
+      //
+      if (!PciIoDevice->Registered) {
+        Status = RegisterPciDevice (
+                   Controller,
+                   PciIoDevice,
+                   NULL
+                   );
+
+      }
+
+      if (NumberOfChildren != NULL && ChildHandleBuffer != NULL && PciIoDevice->Registered) {
+        ChildHandleBuffer[*NumberOfChildren] = PciIoDevice->Handle;
+        (*NumberOfChildren)++;
+      }
+
+      //
+      // Get the next device path
+      //
+      CurrentDevicePath = NextDevicePathNode (RemainingDevicePath);
+      if (IsDevicePathEnd (CurrentDevicePath)) {
+        return EFI_SUCCESS;
+      }
+
+      //
+      // If it is a PPB
+      //
+      if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+        Status = StartPciDevicesOnBridge (
+                   Controller,
+                   PciIoDevice,
+                   CurrentDevicePath,
+                   NumberOfChildren,
+                   ChildHandleBuffer
+                   );
+
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationSupported,
+                             0,
+                             &Supports
+                             );
+        Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationEnable,
+                             Supports,
+                             NULL
+                             );
+
+        return Status;
+      } else {
+
+        //
+        // Currently, the PCI bus driver only support PCI-PCI bridge
+        //
+        return EFI_UNSUPPORTED;
+      }
+
+    } else {
+
+      //
+      // If remaining device path is NULL,
+      // try to enable all the pci devices under this bridge
+      //
+      if (!PciIoDevice->Registered && PciIoDevice->Allocated) {
+        Status = RegisterPciDevice (
+                   Controller,
+                   PciIoDevice,
+                   NULL
+                   );
+
+      }
+
+      if (NumberOfChildren != NULL && ChildHandleBuffer != NULL && PciIoDevice->Registered) {
+        ChildHandleBuffer[*NumberOfChildren] = PciIoDevice->Handle;
+        (*NumberOfChildren)++;
+      }
+
+      if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+        Status = StartPciDevicesOnBridge (
+                   Controller,
+                   PciIoDevice,
+                   RemainingDevicePath,
+                   NumberOfChildren,
+                   ChildHandleBuffer
+                   );
+
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationSupported,
+                             0,
+                             &Supports
+                             );
+        Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+        PciIoDevice->PciIo.Attributes (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoAttributeOperationEnable,
+                             Supports,
+                             NULL
+                             );
+
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+  }
+
+  if (PciIoDevice == NULL) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Start to manage all the PCI devices it found previously under
+  the entire host bridge.
+
+  @param Controller          The root bridge handle.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_SUCCESS     Success to start Pci device on host bridge.
+
+**/
+EFI_STATUS
+StartPciDevices (
+  IN EFI_HANDLE                         Controller
+  )
+{
+  PCI_IO_DEVICE     *RootBridge;
+  EFI_HANDLE        ThisHostBridge;
+  LIST_ENTRY        *CurrentLink;
+
+  RootBridge = GetRootBridgeByHandle (Controller);
+  ASSERT (RootBridge != NULL);
+  ThisHostBridge = RootBridge->PciRootBridgeIo->ParentHandle;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    RootBridge = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    //
+    // Locate the right root bridge to start
+    //
+    if (RootBridge->PciRootBridgeIo->ParentHandle == ThisHostBridge) {
+      StartPciDevicesOnBridge (
+         RootBridge->Handle,
+         RootBridge,
+         NULL,
+         NULL,
+         NULL
+         );
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create root bridge device.
+
+  @param RootBridgeHandle    Specified root bridge hanle.
+
+  @return The crated root bridge device instance, NULL means no
+          root bridge device instance created.
+
+**/
+PCI_IO_DEVICE *
+CreateRootBridge (
+  IN EFI_HANDLE                   RootBridgeHandle
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_IO_DEVICE                   *Dev;
+  EFI_DEVICE_PATH_PROTOCOL        *ParentDevicePath;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Dev = AllocateZeroPool (sizeof (PCI_IO_DEVICE));
+  if (Dev == NULL) {
+    return NULL;
+  }
+
+  Dev->Signature  = PCI_IO_DEVICE_SIGNATURE;
+  Dev->Handle     = RootBridgeHandle;
+  InitializeListHead (&Dev->ChildList);
+
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Dev);
+    return NULL;
+  }
+
+  //
+  // Record the root bridge parent device path
+  //
+  Dev->DevicePath = DuplicateDevicePath (ParentDevicePath);
+
+  //
+  // Get the pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePciDevice (Dev);
+    return NULL;
+  }
+
+  Dev->PciRootBridgeIo = PciRootBridgeIo;
+
+  //
+  // Initialize the PCI I/O instance structure
+  //
+  InitializePciIoInstance (Dev);
+  InitializePciDriverOverrideInstance (Dev);
+  InitializePciLoadFile2 (Dev);
+
+  //
+  // Initialize reserved resource list and
+  // option rom driver list
+  //
+  InitializeListHead (&Dev->ReservedResourceList);
+  InitializeListHead (&Dev->OptionRomDriverList);
+
+  return Dev;
+}
+
+/**
+  Get root bridge device instance by specific root bridge handle.
+
+  @param RootBridgeHandle    Given root bridge handle.
+
+  @return The root bridge device instance, NULL means no root bridge
+          device instance found.
+
+**/
+PCI_IO_DEVICE *
+GetRootBridgeByHandle (
+  EFI_HANDLE RootBridgeHandle
+  )
+{
+  PCI_IO_DEVICE   *RootBridgeDev;
+  LIST_ENTRY      *CurrentLink;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    RootBridgeDev = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (RootBridgeDev->Handle == RootBridgeHandle) {
+      return RootBridgeDev;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+/**
+  Judege whether Pci device existed.
+
+  @param Bridge       Parent bridege instance.
+  @param PciIoDevice  Device instance.
+
+  @retval TRUE        Pci device existed.
+  @retval FALSE       Pci device did not exist.
+
+**/
+BOOLEAN
+PciDeviceExisted (
+  IN PCI_IO_DEVICE    *Bridge,
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp == PciIoDevice) {
+      return TRUE;
+    }
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      if (PciDeviceExisted (Temp, PciIoDevice)) {
+        return TRUE;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the active VGA device on the same segment.
+
+  @param VgaDevice    PCI IO instance for the VGA device.
+
+  @return The active VGA device on the same segment.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheSameSegment (
+  IN PCI_IO_DEVICE        *VgaDevice
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = mPciDevicePool.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &mPciDevicePool) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (Temp->PciRootBridgeIo->SegmentNumber == VgaDevice->PciRootBridgeIo->SegmentNumber) {
+
+      Temp = ActiveVGADeviceOnTheRootBridge (Temp);
+
+      if (Temp != NULL) {
+        return Temp;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+/**
+  Get the active VGA device on the root bridge.
+
+  @param RootBridge  PCI IO instance for the root bridge.
+
+  @return The active VGA device.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheRootBridge (
+  IN PCI_IO_DEVICE        *RootBridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  CurrentLink = RootBridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_PCI_VGA(&Temp->Pci) &&
+        (Temp->Attributes &
+         (EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY |
+          EFI_PCI_IO_ATTRIBUTE_VGA_IO     |
+          EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      return Temp;
+    }
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+
+      Temp = ActiveVGADeviceOnTheRootBridge (Temp);
+
+      if (Temp != NULL) {
+        return Temp;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get HPC PCI address according to its device path.
+
+  @param RootBridge           Root bridege Io instance.
+  @param RemainingDevicePath  Given searching device path.
+  @param PciAddress           Buffer holding searched result.
+
+  @retval EFI_SUCCESS         PCI address was stored in PciAddress
+  @retval EFI_NOT_FOUND       Can not find the specific device path.
+
+**/
+EFI_STATUS
+GetHpcPciAddressFromRootBridge (
+  IN  PCI_IO_DEVICE                    *RootBridge,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *RemainingDevicePath,
+  OUT UINT64                           *PciAddress
+  )
+{
+  EFI_DEV_PATH_PTR          Node;
+  PCI_IO_DEVICE             *Temp;
+  EFI_DEVICE_PATH_PROTOCOL  *CurrentDevicePath;
+  LIST_ENTRY                *CurrentLink;
+  BOOLEAN                   MisMatch;
+
+  MisMatch          = FALSE;
+
+  CurrentDevicePath = RemainingDevicePath;
+  Node.DevPath      = CurrentDevicePath;
+  Temp              = NULL;
+
+  while (!IsDevicePathEnd (CurrentDevicePath)) {
+
+    CurrentLink   = RootBridge->ChildList.ForwardLink;
+    Node.DevPath  = CurrentDevicePath;
+
+    while (CurrentLink != NULL && CurrentLink != &RootBridge->ChildList) {
+      Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+      if (Node.Pci->Device   == Temp->DeviceNumber &&
+          Node.Pci->Function == Temp->FunctionNumber) {
+        RootBridge = Temp;
+        break;
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+
+    //
+    // Check if we find the bridge
+    //
+    if (CurrentLink == &RootBridge->ChildList) {
+
+      MisMatch = TRUE;
+      break;
+
+    }
+
+    CurrentDevicePath = NextDevicePathNode (CurrentDevicePath);
+  }
+
+  if (MisMatch) {
+
+    CurrentDevicePath = NextDevicePathNode (CurrentDevicePath);
+
+    if (IsDevicePathEnd (CurrentDevicePath)) {
+      *PciAddress = EFI_PCI_ADDRESS (RootBridge->BusNumber, Node.Pci->Device, Node.Pci->Function, 0);
+      return EFI_SUCCESS;
+    }
+
+    return EFI_NOT_FOUND;
+  }
+
+  if (Temp != NULL) {
+    *PciAddress = EFI_PCI_ADDRESS (Temp->BusNumber, Temp->DeviceNumber, Temp->FunctionNumber, 0);
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h
new file mode 100644
index 0000000000..1a01e72b1e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDeviceSupport.h
@@ -0,0 +1,289 @@
+/** @file
+  Supporting functions declaration for PCI devices management.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef _EFI_PCI_DEVICE_SUPPORT_H_
+#define _EFI_PCI_DEVICE_SUPPORT_H_
+
+/**
+  Initialize the PCI devices pool.
+
+**/
+VOID
+InitializePciDevicePool (
+  VOID
+  );
+
+/**
+  Insert a root bridge into PCI device pool.
+
+  @param RootBridge     A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+InsertRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  );
+
+/**
+  This function is used to insert a PCI device node under
+  a bridge.
+
+  @param Bridge         The PCI bridge.
+  @param PciDeviceNode  The PCI device needs inserting.
+
+**/
+VOID
+InsertPciDevice (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_IO_DEVICE      *PciDeviceNode
+  );
+
+/**
+  Destroy root bridge and remove it from deivce tree.
+
+  @param RootBridge     The bridge want to be removed.
+
+**/
+VOID
+DestroyRootBridge (
+  IN PCI_IO_DEVICE      *RootBridge
+  );
+
+/**
+  Destroy all the pci device node under the bridge.
+  Bridge itself is not included.
+
+  @param Bridge         A pointer to the PCI_IO_DEVICE.
+
+**/
+VOID
+DestroyPciDeviceTree (
+  IN PCI_IO_DEVICE      *Bridge
+  );
+
+/**
+  Destroy all device nodes under the root bridge
+  specified by Controller.
+
+  The root bridge itself is also included.
+
+  @param  Controller    Root bridge handle.
+
+  @retval EFI_SUCCESS   Destory all devcie nodes successfully.
+  @retval EFI_NOT_FOUND Cannot find any PCI device under specified
+                        root bridge.
+
+**/
+EFI_STATUS
+DestroyRootBridgeByHandle (
+  IN EFI_HANDLE        Controller
+  );
+
+/**
+  This function registers the PCI IO device.
+
+  It creates a handle for this PCI IO device (if the handle does not exist), attaches
+  appropriate protocols onto the handle, does necessary initialization, and sets up
+  parent/child relationship with its bus controller.
+
+  @param Controller     An EFI handle for the PCI bus controller.
+  @param PciIoDevice    A PCI_IO_DEVICE pointer to the PCI IO device to be registered.
+  @param Handle         A pointer to hold the returned EFI handle for the PCI IO device.
+
+  @retval EFI_SUCCESS   The PCI device is successfully registered.
+  @retval other         An error occurred when registering the PCI device.
+
+**/
+EFI_STATUS
+RegisterPciDevice (
+  IN  EFI_HANDLE          Controller,
+  IN  PCI_IO_DEVICE       *PciIoDevice,
+  OUT EFI_HANDLE          *Handle      OPTIONAL
+  );
+
+/**
+  This function is used to remove the whole PCI devices on the specified bridge from
+  the root bridge.
+
+  @param RootBridgeHandle   The root bridge device handle.
+  @param Bridge             The bridge device to be removed.
+
+**/
+VOID
+RemoveAllPciDeviceOnBridge (
+  EFI_HANDLE               RootBridgeHandle,
+  PCI_IO_DEVICE            *Bridge
+  );
+
+/**
+  This function is used to de-register the PCI IO device.
+
+  That includes un-installing PciIo protocol from the specified PCI
+  device handle.
+
+  @param Controller    An EFI handle for the PCI bus controller.
+  @param Handle        PCI device handle.
+
+  @retval EFI_SUCCESS  The PCI device is successfully de-registered.
+  @retval other        An error occurred when de-registering the PCI device.
+
+**/
+EFI_STATUS
+DeRegisterPciDevice (
+  IN  EFI_HANDLE                     Controller,
+  IN  EFI_HANDLE                     Handle
+  );
+
+/**
+  Start to manage the PCI device on the specified root bridge or PCI-PCI Bridge.
+
+  @param Controller          The root bridge handle.
+  @param RootBridge          A pointer to the PCI_IO_DEVICE.
+  @param RemainingDevicePath A pointer to the EFI_DEVICE_PATH_PROTOCOL.
+  @param NumberOfChildren    Children number.
+  @param ChildHandleBuffer   A pointer to the child handle buffer.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_UNSUPPORTED Device only support PCI-PCI bridge.
+  @retval EFI_NOT_FOUND   Can not find the specific device.
+  @retval EFI_SUCCESS     Success to start Pci devices on bridge.
+
+**/
+EFI_STATUS
+StartPciDevicesOnBridge (
+  IN EFI_HANDLE                          Controller,
+  IN PCI_IO_DEVICE                       *RootBridge,
+  IN EFI_DEVICE_PATH_PROTOCOL            *RemainingDevicePath,
+  IN OUT UINT8                           *NumberOfChildren,
+  IN OUT EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Start to manage all the PCI devices it found previously under
+  the entire host bridge.
+
+  @param Controller          The root bridge handle.
+
+  @retval EFI_NOT_READY   Device is not allocated.
+  @retval EFI_SUCCESS     Success to start Pci device on host bridge.
+
+**/
+EFI_STATUS
+StartPciDevices (
+  IN EFI_HANDLE                         Controller
+  );
+
+/**
+  Create root bridge device.
+
+  @param RootBridgeHandle    Specified root bridge hanle.
+
+  @return The crated root bridge device instance, NULL means no
+          root bridge device instance created.
+
+**/
+PCI_IO_DEVICE *
+CreateRootBridge (
+  IN EFI_HANDLE                   RootBridgeHandle
+  );
+
+/**
+  Get root bridge device instance by specific root bridge handle.
+
+  @param RootBridgeHandle    Given root bridge handle.
+
+  @return The root bridge device instance, NULL means no root bridge
+          device instance found.
+
+**/
+PCI_IO_DEVICE *
+GetRootBridgeByHandle (
+  EFI_HANDLE RootBridgeHandle
+  );
+
+
+/**
+  Judege whether Pci device existed.
+
+  @param Bridge       Parent bridege instance.
+  @param PciIoDevice  Device instance.
+
+  @retval TRUE        Pci device existed.
+  @retval FALSE       Pci device did not exist.
+
+**/
+BOOLEAN
+PciDeviceExisted (
+  IN PCI_IO_DEVICE    *Bridge,
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Get the active VGA device on the same segment.
+
+  @param VgaDevice    PCI IO instance for the VGA device.
+
+  @return The active VGA device on the same segment.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheSameSegment (
+  IN PCI_IO_DEVICE        *VgaDevice
+  );
+
+/**
+  Get the active VGA device on the root bridge.
+
+  @param RootBridge  PCI IO instance for the root bridge.
+
+  @return The active VGA device.
+
+**/
+PCI_IO_DEVICE *
+ActiveVGADeviceOnTheRootBridge (
+  IN PCI_IO_DEVICE        *RootBridge
+  );
+
+/**
+  Get HPC PCI address according to its device path.
+
+  @param RootBridge           Root bridege Io instance.
+  @param RemainingDevicePath  Given searching device path.
+  @param PciAddress           Buffer holding searched result.
+
+  @retval EFI_SUCCESS         PCI address was stored in PciAddress.
+  @retval EFI_NOT_FOUND       Can not find the specific device path.
+
+**/
+EFI_STATUS
+GetHpcPciAddressFromRootBridge (
+  IN  PCI_IO_DEVICE                    *RootBridge,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *RemainingDevicePath,
+  OUT UINT64                           *PciAddress
+  );
+
+/**
+  Destroy a pci device node.
+
+  All direct or indirect allocated resource for this node will be freed.
+
+  @param PciIoDevice  A pointer to the PCI_IO_DEVICE to be destoried.
+
+**/
+VOID
+FreePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c
new file mode 100644
index 0000000000..97f45e42d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.c
@@ -0,0 +1,143 @@
+/** @file
+  Functions implementation for Bus Specific Driver Override protoocl.
+
+Copyright (c) 2006 - 2009, 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 "PciBus.h"
+
+/**
+  Initializes a PCI Driver Override Instance.
+
+  @param  PciIoDevice   PCI Device instance.
+
+**/
+VOID
+InitializePciDriverOverrideInstance (
+  IN OUT PCI_IO_DEVICE          *PciIoDevice
+  )
+{
+  PciIoDevice->PciDriverOverride.GetDriver = GetDriver;
+}
+
+
+/**
+  Uses a bus specific algorithm to retrieve a driver image handle for a controller.
+
+  @param  This                  A pointer to the EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL instance.
+  @param  DriverImageHandle     On input, a pointer to the previous driver image handle returned
+                                by GetDriver(). On output, a pointer to the next driver
+                                image handle. Passing in a NULL, will return the first driver
+                                image handle.
+
+  @retval EFI_SUCCESS           A bus specific override driver is returned in DriverImageHandle.
+  @retval EFI_NOT_FOUND         The end of the list of override drivers was reached.
+                                A bus specific override driver is not returned in DriverImageHandle.
+  @retval EFI_INVALID_PARAMETER DriverImageHandle is not a handle that was returned on a
+                                previous call to GetDriver().
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriver (
+  IN EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN OUT EFI_HANDLE                                         *DriverImageHandle
+  )
+{
+  PCI_IO_DEVICE             *PciIoDevice;
+  LIST_ENTRY                *CurrentLink;
+  PCI_DRIVER_OVERRIDE_LIST  *Node;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_DRIVER_OVERRIDE_THIS (This);
+
+  CurrentLink = PciIoDevice->OptionRomDriverList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->OptionRomDriverList) {
+
+    Node = DRIVER_OVERRIDE_FROM_LINK (CurrentLink);
+
+    if (*DriverImageHandle == NULL) {
+
+      *DriverImageHandle = Node->DriverImageHandle;
+      return EFI_SUCCESS;
+    }
+
+    if (*DriverImageHandle == Node->DriverImageHandle) {
+
+      if (CurrentLink->ForwardLink == &PciIoDevice->OptionRomDriverList ||
+          CurrentLink->ForwardLink == NULL) {
+        return EFI_NOT_FOUND;
+      }
+
+      //
+      // Get next node
+      //
+      Node                = DRIVER_OVERRIDE_FROM_LINK (CurrentLink->ForwardLink);
+      *DriverImageHandle  = Node->DriverImageHandle;
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add an overriding driver image.
+
+  @param PciIoDevice        Instance of PciIo device.
+  @param DriverImageHandle  new added driver image.
+
+  @retval EFI_SUCCESS          Successfully added driver.
+  @retval EFI_OUT_OF_RESOURCES No memory resource for new driver instance.
+  @retval other                Some error occurred when locating gEfiLoadedImageProtocolGuid.
+
+**/
+EFI_STATUS
+AddDriver (
+  IN PCI_IO_DEVICE     *PciIoDevice,
+  IN EFI_HANDLE        DriverImageHandle
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_LOADED_IMAGE_PROTOCOL     *LoadedImage;
+  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext;
+  PCI_DRIVER_OVERRIDE_LIST      *Node;
+
+  Status = gBS->HandleProtocol (DriverImageHandle, &gEfiLoadedImageProtocolGuid, (VOID **) &LoadedImage);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Node = AllocatePool (sizeof (PCI_DRIVER_OVERRIDE_LIST));
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Node->Signature         = DRIVER_OVERRIDE_SIGNATURE;
+  Node->DriverImageHandle = DriverImageHandle;
+
+  InsertTailList (&PciIoDevice->OptionRomDriverList, &(Node->Link));
+
+  PciIoDevice->BusOverride  = TRUE;
+
+  ImageContext.Handle    = LoadedImage->ImageBase;
+  ImageContext.ImageRead = PeCoffLoaderImageReadFromMemory;
+
+  //
+  // Get information about the image
+  //
+  PeCoffLoaderGetImageInfo (&ImageContext);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h
new file mode 100644
index 0000000000..bf8efff8f1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciDriverOverride.h
@@ -0,0 +1,86 @@
+/** @file
+  Functions declaration for Bus Specific Driver Override protoocl.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+
+#ifndef _EFI_PCI_DRIVER_OVERRRIDE_H_
+#define _EFI_PCI_DRIVER_OVERRRIDE_H_
+
+#define DRIVER_OVERRIDE_SIGNATURE SIGNATURE_32 ('d', 'r', 'o', 'v')
+
+//
+// PCI driver override driver image list
+//
+typedef struct {
+  UINT32          Signature;
+  LIST_ENTRY      Link;
+  EFI_HANDLE      DriverImageHandle;
+} PCI_DRIVER_OVERRIDE_LIST;
+
+
+#define DRIVER_OVERRIDE_FROM_LINK(a) \
+  CR (a, PCI_DRIVER_OVERRIDE_LIST, Link, DRIVER_OVERRIDE_SIGNATURE)
+
+/**
+  Initializes a PCI Driver Override Instance.
+
+  @param  PciIoDevice   PCI Device instance.
+
+**/
+VOID
+InitializePciDriverOverrideInstance (
+  IN OUT PCI_IO_DEVICE          *PciIoDevice
+  );
+
+/**
+  Add an overriding driver image.
+
+  @param PciIoDevice        Instance of PciIo device.
+  @param DriverImageHandle  new added driver image.
+
+  @retval EFI_SUCCESS          Successfully added driver.
+  @retval EFI_OUT_OF_RESOURCES No memory resource for new driver instance.
+  @retval other                Some error occurred when locating gEfiLoadedImageProtocolGuid.
+
+**/
+EFI_STATUS
+AddDriver (
+  IN PCI_IO_DEVICE     *PciIoDevice,
+  IN EFI_HANDLE        DriverImageHandle
+  );
+
+
+/**
+  Uses a bus specific algorithm to retrieve a driver image handle for a controller.
+
+  @param  This                  A pointer to the EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL instance.
+  @param  DriverImageHandle     On input, a pointer to the previous driver image handle returned
+                                by GetDriver(). On output, a pointer to the next driver
+                                image handle. Passing in a NULL, will return the first driver
+                                image handle.
+
+  @retval EFI_SUCCESS           A bus specific override driver is returned in DriverImageHandle.
+  @retval EFI_NOT_FOUND         The end of the list of override drivers was reached.
+                                A bus specific override driver is not returned in DriverImageHandle.
+  @retval EFI_INVALID_PARAMETER DriverImageHandle is not a handle that was returned on a
+                                previous call to GetDriver().
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriver (
+  IN EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN OUT EFI_HANDLE                                         *DriverImageHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c
new file mode 100644
index 0000000000..7329143136
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.c
@@ -0,0 +1,2251 @@
+/** @file
+  PCI eunmeration implementation on entire PCI bus system for PCI Bus module.
+
+Copyright (c) 2006 - 2013, 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 "PciBus.h"
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  @param Controller  Parent controller handle.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumerator (
+  IN EFI_HANDLE                    Controller
+  )
+{
+  EFI_HANDLE                                        HostBridgeHandle;
+  EFI_STATUS                                        Status;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL                   *PciRootBridgeIo;
+
+  //
+  // If PCI bus has already done the full enumeration, never do it again
+  //
+  if (!gFullEnumeration) {
+    return PciEnumeratorLight (Controller);
+  }
+
+  //
+  // Get the rootbridge Io protocol to find the host bridge handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the host bridge handle
+  //
+  HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+  //
+  // Get the pci host bridge resource allocation protocol
+  //
+  Status = gBS->OpenProtocol (
+                  HostBridgeHandle,
+                  &gEfiPciHostBridgeResourceAllocationProtocolGuid,
+                  (VOID **) &PciResAlloc,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Notify the pci bus enumeration is about to begin
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginEnumeration);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Start the bus allocation phase
+  //
+  Status = PciHostBridgeEnumerator (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Submit the resource request
+  //
+  Status = PciHostBridgeResourceAllocator (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Notify the pci bus enumeration is about to complete
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeEndEnumeration);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Process P2C
+  //
+  Status = PciHostBridgeP2CProcess (PciResAlloc);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Process attributes for devices on this host bridge
+  //
+  Status = PciHostBridgeDeviceAttribute (PciResAlloc);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gFullEnumeration = FALSE;
+
+  Status = gBS->InstallProtocolInterface (
+                  &HostBridgeHandle,
+                  &gEfiPciEnumerationCompleteProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enumerate PCI root bridge.
+
+  @param PciResAlloc   Pointer to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+  @param RootBridgeDev Instance of root bridge device.
+
+  @retval EFI_SUCCESS  Successfully enumerated root bridge.
+  @retval other        Failed to enumerate root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc,
+  IN PCI_IO_DEVICE                                     *RootBridgeDev
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration1;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration2;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration3;
+  UINT8                             SubBusNumber;
+  UINT8                             StartBusNumber;
+  UINT8                             PaddedBusRange;
+  EFI_HANDLE                        RootBridgeHandle;
+  UINT8                             Desc;
+  UINT64                            AddrLen;
+  UINT64                            AddrRangeMin;
+
+  SubBusNumber    = 0;
+  StartBusNumber  = 0;
+  PaddedBusRange  = 0;
+
+  //
+  // Get the root bridge handle
+  //
+  RootBridgeHandle = RootBridgeDev->Handle;
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_IO_BUS_PCI | EFI_IOB_PCI_BUS_ENUM,
+    RootBridgeDev->DevicePath
+    );
+
+  //
+  // Get the Bus information
+  //
+  Status = PciResAlloc->StartBusEnumeration (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          (VOID **) &Configuration
+                          );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Configuration == NULL || Configuration->Desc == ACPI_END_TAG_DESCRIPTOR) {
+    return EFI_INVALID_PARAMETER;
+  }
+  RootBridgeDev->BusNumberRanges = Configuration;
+
+  //
+  // Sort the descriptors in ascending order
+  //
+  for (Configuration1 = Configuration; Configuration1->Desc != ACPI_END_TAG_DESCRIPTOR; Configuration1++) {
+    Configuration2 = Configuration1;
+    for (Configuration3 = Configuration1 + 1; Configuration3->Desc != ACPI_END_TAG_DESCRIPTOR; Configuration3++) {
+      if (Configuration2->AddrRangeMin > Configuration3->AddrRangeMin) {
+        Configuration2 = Configuration3;
+      }
+    }
+    //
+    // All other fields other than AddrRangeMin and AddrLen are ignored in a descriptor,
+    // so only need to swap these two fields.
+    //
+    if (Configuration2 != Configuration1) {
+      AddrRangeMin = Configuration1->AddrRangeMin;
+      Configuration1->AddrRangeMin = Configuration2->AddrRangeMin;
+      Configuration2->AddrRangeMin = AddrRangeMin;
+      
+      AddrLen = Configuration1->AddrLen;
+      Configuration1->AddrLen = Configuration2->AddrLen;
+      Configuration2->AddrLen = AddrLen;
+    }
+  }
+
+  //
+  // Get the bus number to start with
+  //
+  StartBusNumber = (UINT8) (Configuration->AddrRangeMin);
+
+  //
+  // Initialize the subordinate bus number
+  //
+  SubBusNumber = StartBusNumber;
+
+  //
+  // Reset all assigned PCI bus number
+  //
+  ResetAllPpbBusNumber (
+    RootBridgeDev,
+    StartBusNumber
+  );
+
+  //
+  // Assign bus number
+  //
+  Status = PciScanBus (
+            RootBridgeDev,
+            StartBusNumber,
+            &SubBusNumber,
+            &PaddedBusRange
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+
+  //
+  // Assign max bus number scanned
+  //
+
+  Status = PciAllocateBusNumber (RootBridgeDev, SubBusNumber, PaddedBusRange, &SubBusNumber);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }  
+
+  //
+  // Find the bus range which contains the higest bus number, then returns the number of buses
+  // that should be decoded.
+  //
+  while (Configuration->AddrRangeMin + Configuration->AddrLen - 1 < SubBusNumber) {
+    Configuration++;
+  }
+  AddrLen = Configuration->AddrLen;
+  Configuration->AddrLen = SubBusNumber - Configuration->AddrRangeMin + 1;
+
+  //
+  // Save the Desc field of the next descriptor. Mark the next descriptor as an END descriptor.
+  //
+  Configuration++;
+  Desc = Configuration->Desc;
+  Configuration->Desc = ACPI_END_TAG_DESCRIPTOR;
+  
+  //
+  // Set bus number
+  //
+  Status = PciResAlloc->SetBusNumbers (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          RootBridgeDev->BusNumberRanges
+                          );
+
+  //
+  // Restore changed fields
+  //
+  Configuration->Desc = Desc;
+  (Configuration - 1)->AddrLen = AddrLen;
+  
+  return Status;
+}
+
+/**
+  This routine is used to process all PCI devices' Option Rom
+  on a certain root bridge.
+
+  @param Bridge     Given parent's root bridge.
+  @param RomBase    Base address of ROM driver loaded from.
+  @param MaxLength  Maximum rom size.
+
+**/
+VOID
+ProcessOptionRom (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT64        RomBase,
+  IN UINT64        MaxLength
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+
+  //
+  // Go through bridges to reach all devices
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (!IsListEmpty (&Temp->ChildList)) {
+
+      //
+      // Go further to process the option rom under this bridge
+      //
+      ProcessOptionRom (Temp, RomBase, MaxLength);
+    }
+
+    if (Temp->RomSize != 0 && Temp->RomSize <= MaxLength) {
+
+      //
+      // Load and process the option rom
+      //
+      LoadOpRomImage (Temp, RomBase);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  This routine is used to assign bus number to the given PCI bus system
+
+  @param Bridge             Parent root bridge instance.
+  @param StartBusNumber     Number of beginning.
+  @param SubBusNumber       The number of sub bus.
+
+  @retval EFI_SUCCESS       Successfully assigned bus number.
+  @retval EFI_DEVICE_ERROR  Failed to assign bus number.
+
+**/
+EFI_STATUS
+PciAssignBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_TYPE00                      Pci;
+  UINT8                           Device;
+  UINT8                           Func;
+  UINT64                          Address;
+  UINTN                           SecondBus;
+  UINT16                          Register;
+  UINT8                           Register8;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+
+  SecondBus       = 0;
+  Register        = 0;
+
+  *SubBusNumber = StartBusNumber;
+
+  //
+  // First check to see whether the parent is ppb
+  //
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                PciRootBridgeIo,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func
+                );
+
+      if (!EFI_ERROR (Status)   &&
+          (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci))) {
+
+        //
+        // Reserved one bus for cardbus bridge
+        //
+        Status = PciAllocateBusNumber (Bridge, *SubBusNumber, 1, SubBusNumber);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        SecondBus = *SubBusNumber;
+
+        Register  = (UINT16) ((SecondBus << 8) | (UINT16) StartBusNumber);
+
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x18);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint16,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+
+        //
+        // Initialize SubBusNumber to SecondBus
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x1A);
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+        //
+        // If it is PPB, resursively search down this bridge
+        //
+        if (IS_PCI_BRIDGE (&Pci)) {
+
+          Register8 = 0xFF;
+          Status = PciRootBridgeIo->Pci.Write (
+                                          PciRootBridgeIo,
+                                          EfiPciWidthUint8,
+                                          Address,
+                                          1,
+                                          &Register8
+                                          );
+
+          Status = PciAssignBusNumber (
+                    Bridge,
+                    (UINT8) (SecondBus),
+                    SubBusNumber
+                    );
+
+          if (EFI_ERROR (Status)) {
+            return EFI_DEVICE_ERROR;
+          }
+        }
+
+        //
+        // Set the current maximum bus number under the PPB
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x1A);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine is used to determine the root bridge attribute by interfacing
+  the host bridge resource allocation protocol.
+
+  @param PciResAlloc    Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+  @param RootBridgeDev  Root bridge instance
+
+  @retval EFI_SUCCESS  Successfully got root bridge's attribute.
+  @retval other        Failed to get attribute.
+
+**/
+EFI_STATUS
+DetermineRootBridgeAttributes (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  IN PCI_IO_DEVICE                                    *RootBridgeDev
+  )
+{
+  UINT64      Attributes;
+  EFI_STATUS  Status;
+  EFI_HANDLE  RootBridgeHandle;
+
+  Attributes        = 0;
+  RootBridgeHandle  = RootBridgeDev->Handle;
+
+  //
+  // Get root bridge attribute by calling into pci host bridge resource allocation protocol
+  //
+  Status = PciResAlloc->GetAllocAttributes (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          &Attributes
+                          );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Here is the point where PCI bus driver calls HOST bridge allocation protocol
+  // Currently we hardcoded for ea815
+  //
+  if ((Attributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) {
+    RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED;
+  }
+
+  if ((Attributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) != 0) {
+    RootBridgeDev->Decodes |= EFI_BRIDGE_MEM64_DECODE_SUPPORTED;
+    RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM64_DECODE_SUPPORTED;
+  }
+
+  RootBridgeDev->Decodes |= EFI_BRIDGE_MEM32_DECODE_SUPPORTED;
+  RootBridgeDev->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+  RootBridgeDev->Decodes |= EFI_BRIDGE_IO16_DECODE_SUPPORTED;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get Max Option Rom size on specified bridge.
+
+  @param Bridge    Given bridge device instance.
+
+  @return Max size of option rom needed.
+
+**/
+UINT64
+GetMaxOptionRomSize (
+  IN PCI_IO_DEVICE   *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+  UINT64          MaxOptionRomSize;
+  UINT64          TempOptionRomSize;
+
+  MaxOptionRomSize = 0;
+
+  //
+  // Go through bridges to reach all devices
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (!IsListEmpty (&Temp->ChildList)) {
+
+      //
+      // Get max option rom size under this bridge
+      //
+      TempOptionRomSize = GetMaxOptionRomSize (Temp);
+
+      //
+      // Compare with the option rom size of the bridge
+      // Get the larger one
+      //
+      if (Temp->RomSize > TempOptionRomSize) {
+        TempOptionRomSize = Temp->RomSize;
+      }
+
+    } else {
+
+      //
+      // For devices get the rom size directly
+      //
+      TempOptionRomSize = Temp->RomSize;
+    }
+
+    //
+    // Get the largest rom size on this bridge
+    //
+    if (TempOptionRomSize > MaxOptionRomSize) {
+      MaxOptionRomSize = TempOptionRomSize;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return MaxOptionRomSize;
+}
+
+/**
+  Process attributes of devices on this host bridge
+
+  @param PciResAlloc Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Successfully process attribute.
+  @retval EFI_NOT_FOUND Can not find the specific root bridge device.
+  @retval other         Failed to determine the root bridge device's attribute.
+
+**/
+EFI_STATUS
+PciHostBridgeDeviceAttribute (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  EFI_HANDLE    RootBridgeHandle;
+  PCI_IO_DEVICE *RootBridgeDev;
+  EFI_STATUS    Status;
+
+  RootBridgeHandle = NULL;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Set the attributes for devcies behind the Root Bridge
+    //
+    Status = DetermineDeviceAttribute (RootBridgeDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource allocation status from the ACPI resource descriptor.
+
+  @param AcpiConfig       Point to Acpi configuration table.
+  @param IoResStatus      Return the status of I/O resource.
+  @param Mem32ResStatus   Return the status of 32-bit Memory resource.
+  @param PMem32ResStatus  Return the status of 32-bit Prefetchable Memory resource.
+  @param Mem64ResStatus   Return the status of 64-bit Memory resource.
+  @param PMem64ResStatus  Return the status of 64-bit Prefetchable Memory resource.
+
+**/
+VOID
+GetResourceAllocationStatus (
+  VOID        *AcpiConfig,
+  OUT UINT64  *IoResStatus,
+  OUT UINT64  *Mem32ResStatus,
+  OUT UINT64  *PMem32ResStatus,
+  OUT UINT64  *Mem64ResStatus,
+  OUT UINT64  *PMem64ResStatus
+  )
+{
+  UINT8                             *Temp;
+  UINT64                            ResStatus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *ACPIAddressDesc;
+
+  Temp = (UINT8 *) AcpiConfig;
+
+  while (*Temp == ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+
+    ACPIAddressDesc       = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Temp;
+    ResStatus = ACPIAddressDesc->AddrTranslationOffset;
+
+    switch (ACPIAddressDesc->ResType) {
+    case 0:
+      if (ACPIAddressDesc->AddrSpaceGranularity == 32) {
+        if (ACPIAddressDesc->SpecificFlag == 0x06) {
+          //
+          // Pmem32
+          //
+          *PMem32ResStatus = ResStatus;
+        } else {
+          //
+          // Mem32
+          //
+          *Mem32ResStatus = ResStatus;
+        }
+      }
+
+      if (ACPIAddressDesc->AddrSpaceGranularity == 64) {
+        if (ACPIAddressDesc->SpecificFlag == 0x06) {
+          //
+          // PMem64
+          //
+          *PMem64ResStatus = ResStatus;
+        } else {
+          //
+          // Mem64
+          //
+          *Mem64ResStatus = ResStatus;
+        }
+      }
+
+      break;
+
+    case 1:
+      //
+      // Io
+      //
+      *IoResStatus = ResStatus;
+      break;
+
+    default:
+      break;
+    }
+
+    Temp += sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+  }
+}
+
+/**
+  Remove a PCI device from device pool and mark its bar.
+
+  @param PciDevice Instance of Pci device.
+
+  @retval EFI_SUCCESS Successfully remove the PCI device.
+  @retval EFI_ABORTED Pci device is a root bridge or a PCI-PCI bridge.
+
+**/
+EFI_STATUS
+RejectPciDevice (
+  IN PCI_IO_DEVICE       *PciDevice
+  )
+{
+  PCI_IO_DEVICE   *Bridge;
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  //
+  // Remove the padding resource from a bridge
+  //
+  if ( IS_PCI_BRIDGE(&PciDevice->Pci) &&
+       PciDevice->ResourcePaddingDescriptors != NULL ) {
+    FreePool (PciDevice->ResourcePaddingDescriptors);
+    PciDevice->ResourcePaddingDescriptors = NULL;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip RB and PPB
+  //
+  if (IS_PCI_BRIDGE (&PciDevice->Pci) || (PciDevice->Parent == NULL)) {
+    return EFI_ABORTED;
+  }
+
+  if (IS_CARDBUS_BRIDGE (&PciDevice->Pci)) {
+    //
+    // Get the root bridge device
+    //
+    Bridge = PciDevice;
+    while (Bridge->Parent != NULL) {
+      Bridge = Bridge->Parent;
+    }
+
+    RemoveAllPciDeviceOnBridge (Bridge->Handle, PciDevice);
+
+    //
+    // Mark its bar
+    //
+    InitializeP2C (PciDevice);
+  }
+
+  //
+  // Remove the device
+  //
+  Bridge      = PciDevice->Parent;
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    if (Temp == PciDevice) {
+      InitializePciDevice (Temp);
+      RemoveEntryList (CurrentLink);
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_ABORTED;
+}
+
+/**
+  Determine whethter a PCI device can be rejected.
+
+  @param  PciResNode Pointer to Pci resource node instance.
+
+  @retval TRUE  The PCI device can be rejected.
+  @retval TRUE  The PCI device cannot be rejected.
+
+**/
+BOOLEAN
+IsRejectiveDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode
+  )
+{
+  PCI_IO_DEVICE *Temp;
+
+  Temp = PciResNode->PciDev;
+
+  //
+  // Ensure the device is present
+  //
+  if (Temp == NULL) {
+    return FALSE;
+  }
+
+  //
+  // PPB and RB should go ahead
+  //
+  if (IS_PCI_BRIDGE (&Temp->Pci) || (Temp->Parent == NULL)) {
+    return TRUE;
+  }
+
+  //
+  // Skip device on Bus0
+  //
+  if ((Temp->Parent != NULL) && (Temp->BusNumber == 0)) {
+    return FALSE;
+  }
+
+  //
+  // Skip VGA
+  //
+  if (IS_PCI_VGA (&Temp->Pci)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Compare two resource nodes and get the larger resource consumer.
+
+  @param PciResNode1  resource node 1 want to be compared
+  @param PciResNode2  resource node 2 want to be compared
+
+  @return Larger resource node.
+
+**/
+PCI_RESOURCE_NODE *
+GetLargerConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode1,
+  IN  PCI_RESOURCE_NODE   *PciResNode2
+  )
+{
+  if (PciResNode2 == NULL) {
+    return PciResNode1;
+  }
+
+  if ((IS_PCI_BRIDGE(&(PciResNode2->PciDev->Pci)) || (PciResNode2->PciDev->Parent == NULL)) \
+       && (PciResNode2->ResourceUsage != PciResUsagePadding) )
+  {
+    return PciResNode1;
+  }
+
+  if (PciResNode1 == NULL) {
+    return PciResNode2;
+  }
+
+  if ((PciResNode1->Length) > (PciResNode2->Length)) {
+    return PciResNode1;
+  }
+
+  return PciResNode2;
+}
+
+
+/**
+  Get the max resource consumer in the host resource pool.
+
+  @param ResPool  Pointer to resource pool node.
+
+  @return The max resource consumer in the host resource pool.
+
+**/
+PCI_RESOURCE_NODE *
+GetMaxResourceConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *ResPool
+  )
+{
+  PCI_RESOURCE_NODE *Temp;
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *PciResNode;
+  PCI_RESOURCE_NODE *PPBResNode;
+
+  PciResNode  = NULL;
+
+  CurrentLink = ResPool->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &ResPool->ChildList) {
+
+    Temp = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (!IsRejectiveDevice (Temp)) {
+      CurrentLink = CurrentLink->ForwardLink;
+      continue;
+    }
+
+    if ((IS_PCI_BRIDGE (&(Temp->PciDev->Pci)) || (Temp->PciDev->Parent == NULL)) \
+          && (Temp->ResourceUsage != PciResUsagePadding))
+    {
+      PPBResNode  = GetMaxResourceConsumerDevice (Temp);
+      PciResNode  = GetLargerConsumerDevice (PciResNode, PPBResNode);
+    } else {
+      PciResNode = GetLargerConsumerDevice (PciResNode, Temp);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return PciResNode;
+}
+
+/**
+  Adjust host bridge allocation so as to reduce resource requirement
+
+  @param IoPool           Pointer to instance of I/O resource Node.
+  @param Mem32Pool        Pointer to instance of 32-bit memory resource Node.
+  @param PMem32Pool       Pointer to instance of 32-bit Prefetchable memory resource node.
+  @param Mem64Pool        Pointer to instance of 64-bit memory resource node.
+  @param PMem64Pool       Pointer to instance of 64-bit Prefetchable memory resource node.
+  @param IoResStatus      Status of I/O resource Node.
+  @param Mem32ResStatus   Status of 32-bit memory resource Node.
+  @param PMem32ResStatus  Status of 32-bit Prefetchable memory resource node.
+  @param Mem64ResStatus   Status of 64-bit memory resource node.
+  @param PMem64ResStatus  Status of 64-bit Prefetchable memory resource node.
+
+  @retval EFI_SUCCESS     Successfully adjusted resoruce on host bridge.
+  @retval EFI_ABORTED     Host bridge hasn't this resource type or no resource be adjusted.
+
+**/
+EFI_STATUS
+PciHostBridgeAdjustAllocation (
+  IN  PCI_RESOURCE_NODE   *IoPool,
+  IN  PCI_RESOURCE_NODE   *Mem32Pool,
+  IN  PCI_RESOURCE_NODE   *PMem32Pool,
+  IN  PCI_RESOURCE_NODE   *Mem64Pool,
+  IN  PCI_RESOURCE_NODE   *PMem64Pool,
+  IN  UINT64              IoResStatus,
+  IN  UINT64              Mem32ResStatus,
+  IN  UINT64              PMem32ResStatus,
+  IN  UINT64              Mem64ResStatus,
+  IN  UINT64              PMem64ResStatus
+  )
+{
+  BOOLEAN                               AllocationAjusted;
+  PCI_RESOURCE_NODE                     *PciResNode;
+  PCI_RESOURCE_NODE                     *ResPool[5];
+  PCI_IO_DEVICE                         *RemovedPciDev[5];
+  UINT64                                ResStatus[5];
+  UINTN                                 RemovedPciDevNum;
+  UINTN                                 DevIndex;
+  UINTN                                 ResType;
+  EFI_STATUS                            Status;
+  EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD AllocFailExtendedData;
+
+  PciResNode = NULL;
+  ZeroMem (RemovedPciDev, 5 * sizeof (PCI_IO_DEVICE *));
+  RemovedPciDevNum  = 0;
+
+  ResPool[0]        = IoPool;
+  ResPool[1]        = Mem32Pool;
+  ResPool[2]        = PMem32Pool;
+  ResPool[3]        = Mem64Pool;
+  ResPool[4]        = PMem64Pool;
+
+  ResStatus[0]      = IoResStatus;
+  ResStatus[1]      = Mem32ResStatus;
+  ResStatus[2]      = PMem32ResStatus;
+  ResStatus[3]      = Mem64ResStatus;
+  ResStatus[4]      = PMem64ResStatus;
+
+  AllocationAjusted = FALSE;
+
+  for (ResType = 0; ResType < 5; ResType++) {
+
+    if (ResStatus[ResType] == EFI_RESOURCE_SATISFIED) {
+      continue;
+    }
+
+    if (ResStatus[ResType] == EFI_RESOURCE_NOT_SATISFIED) {
+      //
+      // Host bridge hasn't this resource type
+      //
+      return EFI_ABORTED;
+    }
+
+    //
+    // Hostbridge hasn't enough resource
+    //
+    PciResNode = GetMaxResourceConsumerDevice (ResPool[ResType]);
+    if (PciResNode == NULL) {
+      continue;
+    }
+
+    //
+    // Check if the device has been removed before
+    //
+    for (DevIndex = 0; DevIndex < RemovedPciDevNum; DevIndex++) {
+      if (PciResNode->PciDev == RemovedPciDev[DevIndex]) {
+        break;
+      }
+    }
+
+    if (DevIndex != RemovedPciDevNum) {
+      continue;
+    }
+
+    //
+    // Remove the device if it isn't in the array
+    //
+    Status = RejectPciDevice (PciResNode->PciDev);
+    if (Status == EFI_SUCCESS) {
+      DEBUG ((
+        EFI_D_ERROR,
+        "PciBus: [%02x|%02x|%02x] was rejected due to resource confliction.\n",
+        PciResNode->PciDev->BusNumber, PciResNode->PciDev->DeviceNumber, PciResNode->PciDev->FunctionNumber
+        ));
+
+      //
+      // Raise the EFI_IOB_EC_RESOURCE_CONFLICT status code
+      //
+      //
+      // Have no way to get ReqRes, AllocRes & Bar here
+      //
+      ZeroMem (&AllocFailExtendedData, sizeof (AllocFailExtendedData));
+      AllocFailExtendedData.DevicePathSize = (UINT16) sizeof (EFI_DEVICE_PATH_PROTOCOL);
+      AllocFailExtendedData.DevicePath     = (UINT8 *) PciResNode->PciDev->DevicePath;
+      AllocFailExtendedData.Bar            = PciResNode->Bar;
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+            EFI_PROGRESS_CODE,
+            EFI_IO_BUS_PCI | EFI_IOB_EC_RESOURCE_CONFLICT,
+            (VOID *) &AllocFailExtendedData,
+            sizeof (AllocFailExtendedData)
+            );
+
+      //
+      // Add it to the array and indicate at least a device has been rejected
+      //
+      RemovedPciDev[RemovedPciDevNum++] = PciResNode->PciDev;
+      AllocationAjusted                 = TRUE;
+    }
+  }
+  //
+  // End for
+  //
+
+  if (AllocationAjusted) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_ABORTED;
+  }
+}
+
+/**
+  Summary requests for all resource type, and contruct ACPI resource
+  requestor instance.
+
+  @param Bridge           detecting bridge
+  @param IoNode           Pointer to instance of I/O resource Node
+  @param Mem32Node        Pointer to instance of 32-bit memory resource Node
+  @param PMem32Node       Pointer to instance of 32-bit Pmemory resource node
+  @param Mem64Node        Pointer to instance of 64-bit memory resource node
+  @param PMem64Node       Pointer to instance of 64-bit Pmemory resource node
+  @param Config           Output buffer holding new constructed APCI resource requestor
+
+  @retval EFI_SUCCESS           Successfully constructed ACPI resource.
+  @retval EFI_OUT_OF_RESOURCES  No memory availabe.
+
+**/
+EFI_STATUS
+ConstructAcpiResourceRequestor (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_RESOURCE_NODE  *IoNode,
+  IN PCI_RESOURCE_NODE  *Mem32Node,
+  IN PCI_RESOURCE_NODE  *PMem32Node,
+  IN PCI_RESOURCE_NODE  *Mem64Node,
+  IN PCI_RESOURCE_NODE  *PMem64Node,
+  OUT VOID              **Config
+  )
+{
+  UINT8                             NumConfig;
+  UINT8                             Aperture;
+  UINT8                             *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *PtrEnd;
+
+  NumConfig = 0;
+  Aperture  = 0;
+
+  *Config  = NULL;
+
+  //
+  // if there is io request, add to the io aperture
+  //
+  if (ResourceRequestExisted (IoNode)) {
+    NumConfig++;
+    Aperture |= 0x01;
+  }
+
+  //
+  // if there is mem32 request, add to the mem32 aperture
+  //
+  if (ResourceRequestExisted (Mem32Node)) {
+    NumConfig++;
+    Aperture |= 0x02;
+  }
+
+  //
+  // if there is pmem32 request, add to the pmem32 aperture
+  //
+  if (ResourceRequestExisted (PMem32Node)) {
+    NumConfig++;
+    Aperture |= 0x04;
+  }
+
+  //
+  // if there is mem64 request, add to the mem64 aperture
+  //
+  if (ResourceRequestExisted (Mem64Node)) {
+    NumConfig++;
+    Aperture |= 0x08;
+  }
+
+  //
+  // if there is pmem64 request, add to the pmem64 aperture
+  //
+  if (ResourceRequestExisted (PMem64Node)) {
+    NumConfig++;
+    Aperture |= 0x10;
+  }
+
+  if (NumConfig != 0) {
+
+    //
+    // If there is at least one type of resource request,
+    // allocate a acpi resource node
+    //
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) * NumConfig + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Ptr = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+    //
+    // Deal with io aperture
+    //
+    if ((Aperture & 0x01) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Io
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_IO;
+      //
+      // non ISA range
+      //
+      Ptr->SpecificFlag = 1;
+      Ptr->AddrLen      = IoNode->Length;
+      Ptr->AddrRangeMax = IoNode->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with mem32 aperture
+    //
+    if ((Aperture & 0x02) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // Nonprefechable
+      //
+      Ptr->SpecificFlag = 0;
+      //
+      // 32 bit
+      //
+      Ptr->AddrSpaceGranularity = 32;
+      Ptr->AddrLen      = Mem32Node->Length;
+      Ptr->AddrRangeMax = Mem32Node->Alignment;
+
+      Ptr++;
+    }
+
+    //
+    // Deal with Pmem32 aperture
+    //
+    if ((Aperture & 0x04) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      Ptr->SpecificFlag = 0x6;
+      //
+      // 32 bit
+      //
+      Ptr->AddrSpaceGranularity = 32;
+      Ptr->AddrLen      = PMem32Node->Length;
+      Ptr->AddrRangeMax = PMem32Node->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with mem64 aperture
+    //
+    if ((Aperture & 0x08) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // nonprefechable
+      //
+      Ptr->SpecificFlag = 0;
+      //
+      // 64 bit
+      //
+      Ptr->AddrSpaceGranularity = 64;
+      Ptr->AddrLen      = Mem64Node->Length;
+      Ptr->AddrRangeMax = Mem64Node->Alignment;
+
+      Ptr++;
+    }
+    //
+    // Deal with Pmem64 aperture
+    //
+    if ((Aperture & 0x10) != 0) {
+      Ptr->Desc     = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Ptr->Len      = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+      //
+      // Mem
+      //
+      Ptr->ResType  = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      Ptr->SpecificFlag = 0x06;
+      //
+      // 64 bit
+      //
+      Ptr->AddrSpaceGranularity = 64;
+      Ptr->AddrLen      = PMem64Node->Length;
+      Ptr->AddrRangeMax = PMem64Node->Alignment;
+
+      Ptr++;
+    }
+
+    //
+    // put the checksum
+    //
+    PtrEnd            = (EFI_ACPI_END_TAG_DESCRIPTOR *) Ptr;
+
+    PtrEnd->Desc      = ACPI_END_TAG_DESCRIPTOR;
+    PtrEnd->Checksum  = 0;
+
+  } else {
+
+    //
+    // If there is no resource request
+    //
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Ptr               = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) (Configuration);
+    Ptr->Desc         = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+
+    PtrEnd            = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Ptr + 1);
+    PtrEnd->Desc      = ACPI_END_TAG_DESCRIPTOR;
+    PtrEnd->Checksum  = 0;
+  }
+
+  *Config = Configuration;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource base from an acpi configuration descriptor.
+
+  @param Config       An acpi configuration descriptor.
+  @param IoBase       Output of I/O resource base address.
+  @param Mem32Base    Output of 32-bit memory base address.
+  @param PMem32Base   Output of 32-bit prefetchable memory base address.
+  @param Mem64Base    Output of 64-bit memory base address.
+  @param PMem64Base   Output of 64-bit prefetchable memory base address.
+
+**/
+VOID
+GetResourceBase (
+  IN VOID     *Config,
+  OUT UINT64  *IoBase,
+  OUT UINT64  *Mem32Base,
+  OUT UINT64  *PMem32Base,
+  OUT UINT64  *Mem64Base,
+  OUT UINT64  *PMem64Base
+  )
+{
+  UINT8                             *Temp;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  UINT64                            ResStatus;
+
+  ASSERT (Config != NULL);
+
+  *IoBase     = 0xFFFFFFFFFFFFFFFFULL;
+  *Mem32Base  = 0xFFFFFFFFFFFFFFFFULL;
+  *PMem32Base = 0xFFFFFFFFFFFFFFFFULL;
+  *Mem64Base  = 0xFFFFFFFFFFFFFFFFULL;
+  *PMem64Base = 0xFFFFFFFFFFFFFFFFULL;
+
+  Temp        = (UINT8 *) Config;
+
+  while (*Temp == ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+
+    Ptr       = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Temp;
+    ResStatus = Ptr->AddrTranslationOffset;
+
+    if (ResStatus == EFI_RESOURCE_SATISFIED) {
+
+      switch (Ptr->ResType) {
+
+      //
+      // Memory type aperture
+      //
+      case 0:
+
+        //
+        // Check to see the granularity
+        //
+        if (Ptr->AddrSpaceGranularity == 32) {
+          if ((Ptr->SpecificFlag & 0x06) != 0) {
+            *PMem32Base = Ptr->AddrRangeMin;
+          } else {
+            *Mem32Base = Ptr->AddrRangeMin;
+          }
+        }
+
+        if (Ptr->AddrSpaceGranularity == 64) {
+          if ((Ptr->SpecificFlag & 0x06) != 0) {
+            *PMem64Base = Ptr->AddrRangeMin;
+          } else {
+            *Mem64Base = Ptr->AddrRangeMin;
+          }
+        }
+        break;
+
+      case 1:
+
+        //
+        // Io type aperture
+        //
+        *IoBase = Ptr->AddrRangeMin;
+        break;
+
+      default:
+        break;
+
+      }
+      //
+      // End switch
+      //
+    }
+    //
+    // End for
+    //
+    Temp += sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR);
+  }
+}
+
+/**
+  Enumerate pci bridge, allocate resource and determine attribute
+  for devices on this bridge.
+
+  @param BridgeDev    Pointer to instance of bridge device.
+
+  @retval EFI_SUCCESS Successfully enumerated PCI bridge.
+  @retval other       Failed to enumerate.
+
+**/
+EFI_STATUS
+PciBridgeEnumerator (
+  IN PCI_IO_DEVICE                                     *BridgeDev
+  )
+{
+  UINT8               SubBusNumber;
+  UINT8               StartBusNumber;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  EFI_STATUS          Status;
+
+  SubBusNumber    = 0;
+  StartBusNumber  = 0;
+  PciIo           = &(BridgeDev->PciIo);
+  Status          = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x19, 1, &StartBusNumber);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciAssignBusNumber (
+            BridgeDev,
+            StartBusNumber,
+            &SubBusNumber
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciPciDeviceInfoCollector (BridgeDev, StartBusNumber);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PciBridgeResourceAllocator (BridgeDev);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = DetermineDeviceAttribute (BridgeDev);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Allocate all kinds of resource for PCI bridge.
+
+  @param  Bridge      Pointer to bridge instance.
+
+  @retval EFI_SUCCESS Successfully allocated resource for PCI bridge.
+  @retval other       Failed to allocate resource for bridge.
+
+**/
+EFI_STATUS
+PciBridgeResourceAllocator (
+  IN PCI_IO_DEVICE  *Bridge
+  )
+{
+  PCI_RESOURCE_NODE *IoBridge;
+  PCI_RESOURCE_NODE *Mem32Bridge;
+  PCI_RESOURCE_NODE *PMem32Bridge;
+  PCI_RESOURCE_NODE *Mem64Bridge;
+  PCI_RESOURCE_NODE *PMem64Bridge;
+  UINT64            IoBase;
+  UINT64            Mem32Base;
+  UINT64            PMem32Base;
+  UINT64            Mem64Base;
+  UINT64            PMem64Base;
+  EFI_STATUS        Status;
+
+  IoBridge = CreateResourceNode (
+               Bridge,
+               0,
+               Bridge->BridgeIoAlignment,
+               0,
+               PciBarTypeIo16,
+               PciResUsageTypical
+               );
+
+  Mem32Bridge = CreateResourceNode (
+                  Bridge,
+                  0,
+                  0xFFFFF,
+                  0,
+                  PciBarTypeMem32,
+                  PciResUsageTypical
+                  );
+
+  PMem32Bridge = CreateResourceNode (
+                   Bridge,
+                   0,
+                   0xFFFFF,
+                   0,
+                   PciBarTypePMem32,
+                   PciResUsageTypical
+                   );
+
+  Mem64Bridge = CreateResourceNode (
+                  Bridge,
+                  0,
+                  0xFFFFF,
+                  0,
+                  PciBarTypeMem64,
+                  PciResUsageTypical
+                  );
+
+  PMem64Bridge = CreateResourceNode (
+                   Bridge,
+                   0,
+                   0xFFFFF,
+                   0,
+                   PciBarTypePMem64,
+                   PciResUsageTypical
+                   );
+
+  //
+  // Create resourcemap by going through all the devices subject to this root bridge
+  //
+  CreateResourceMap (
+    Bridge,
+    IoBridge,
+    Mem32Bridge,
+    PMem32Bridge,
+    Mem64Bridge,
+    PMem64Bridge
+    );
+
+  Status = GetResourceBaseFromBridge (
+             Bridge,
+             &IoBase,
+             &Mem32Base,
+             &PMem32Base,
+             &Mem64Base,
+             &PMem64Base
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program IO resources
+  //
+  ProgramResource (
+    IoBase,
+    IoBridge
+    );
+
+  //
+  // Program Mem32 resources
+  //
+  ProgramResource (
+    Mem32Base,
+    Mem32Bridge
+    );
+
+  //
+  // Program PMem32 resources
+  //
+  ProgramResource (
+    PMem32Base,
+    PMem32Bridge
+    );
+
+  //
+  // Program Mem64 resources
+  //
+  ProgramResource (
+    Mem64Base,
+    Mem64Bridge
+    );
+
+  //
+  // Program PMem64 resources
+  //
+  ProgramResource (
+    PMem64Base,
+    PMem64Bridge
+    );
+
+  DestroyResourceTree (IoBridge);
+  DestroyResourceTree (Mem32Bridge);
+  DestroyResourceTree (PMem32Bridge);
+  DestroyResourceTree (PMem64Bridge);
+  DestroyResourceTree (Mem64Bridge);
+
+  gBS->FreePool (IoBridge);
+  gBS->FreePool (Mem32Bridge);
+  gBS->FreePool (PMem32Bridge);
+  gBS->FreePool (PMem64Bridge);
+  gBS->FreePool (Mem64Bridge);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get resource base address for a pci bridge device.
+
+  @param Bridge     Given Pci driver instance.
+  @param IoBase     Output for base address of I/O type resource.
+  @param Mem32Base  Output for base address of 32-bit memory type resource.
+  @param PMem32Base Ooutput for base address of 32-bit Pmemory type resource.
+  @param Mem64Base  Output for base address of 64-bit memory type resource.
+  @param PMem64Base Output for base address of 64-bit Pmemory type resource.
+
+  @retval EFI_SUCCESS           Successfully got resource base address.
+  @retval EFI_OUT_OF_RESOURCES  PCI bridge is not available.
+
+**/
+EFI_STATUS
+GetResourceBaseFromBridge (
+  IN  PCI_IO_DEVICE *Bridge,
+  OUT UINT64        *IoBase,
+  OUT UINT64        *Mem32Base,
+  OUT UINT64        *PMem32Base,
+  OUT UINT64        *Mem64Base,
+  OUT UINT64        *PMem64Base
+  )
+{
+  if (!Bridge->Allocated) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  *IoBase     = gAllOne;
+  *Mem32Base  = gAllOne;
+  *PMem32Base = gAllOne;
+  *Mem64Base  = gAllOne;
+  *PMem64Base = gAllOne;
+
+  if (IS_PCI_BRIDGE (&Bridge->Pci)) {
+
+    if (Bridge->PciBar[PPB_IO_RANGE].Length > 0) {
+      *IoBase = Bridge->PciBar[PPB_IO_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_MEM32_RANGE].Length > 0) {
+      *Mem32Base = Bridge->PciBar[PPB_MEM32_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_PMEM32_RANGE].Length > 0) {
+      *PMem32Base = Bridge->PciBar[PPB_PMEM32_RANGE].BaseAddress;
+    }
+
+    if (Bridge->PciBar[PPB_PMEM64_RANGE].Length > 0) {
+      *PMem64Base = Bridge->PciBar[PPB_PMEM64_RANGE].BaseAddress;
+    } else {
+      *PMem64Base = gAllOne;
+    }
+
+  }
+
+  if (IS_CARDBUS_BRIDGE (&Bridge->Pci)) {
+    if (Bridge->PciBar[P2C_IO_1].Length > 0) {
+      *IoBase = Bridge->PciBar[P2C_IO_1].BaseAddress;
+    } else {
+      if (Bridge->PciBar[P2C_IO_2].Length > 0) {
+        *IoBase = Bridge->PciBar[P2C_IO_2].BaseAddress;
+      }
+    }
+
+    if (Bridge->PciBar[P2C_MEM_1].Length > 0) {
+      if (Bridge->PciBar[P2C_MEM_1].BarType == PciBarTypePMem32) {
+        *PMem32Base = Bridge->PciBar[P2C_MEM_1].BaseAddress;
+      }
+
+      if (Bridge->PciBar[P2C_MEM_1].BarType == PciBarTypeMem32) {
+        *Mem32Base = Bridge->PciBar[P2C_MEM_1].BaseAddress;
+      }
+    }
+
+    if (Bridge->PciBar[P2C_MEM_2].Length > 0) {
+      if (Bridge->PciBar[P2C_MEM_2].BarType == PciBarTypePMem32) {
+        *PMem32Base = Bridge->PciBar[P2C_MEM_2].BaseAddress;
+      }
+
+      if (Bridge->PciBar[P2C_MEM_2].BarType == PciBarTypeMem32) {
+        *Mem32Base = Bridge->PciBar[P2C_MEM_2].BaseAddress;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+   These are the notifications from the PCI bus driver that it is about to enter a certain
+   phase of the PCI enumeration process.
+
+   This member function can be used to notify the host bridge driver to perform specific actions,
+   including any chipset-specific initialization, so that the chipset is ready to enter the next phase.
+   Eight notification points are defined at this time. See belows:
+   EfiPciHostBridgeBeginEnumeration       Resets the host bridge PCI apertures and internal data
+                                          structures. The PCI enumerator should issue this notification
+                                          before starting a fresh enumeration process. Enumeration cannot
+                                          be restarted after sending any other notification such as
+                                          EfiPciHostBridgeBeginBusAllocation.
+   EfiPciHostBridgeBeginBusAllocation     The bus allocation phase is about to begin. No specific action is
+                                          required here. This notification can be used to perform any
+                                          chipset-specific programming.
+   EfiPciHostBridgeEndBusAllocation       The bus allocation and bus programming phase is complete. No
+                                          specific action is required here. This notification can be used to
+                                          perform any chipset-specific programming.
+   EfiPciHostBridgeBeginResourceAllocation
+                                          The resource allocation phase is about to begin. No specific
+                                          action is required here. This notification can be used to perform
+                                          any chipset-specific programming.
+   EfiPciHostBridgeAllocateResources      Allocates resources per previously submitted requests for all the PCI
+                                          root bridges. These resource settings are returned on the next call to
+                                          GetProposedResources(). Before calling NotifyPhase() with a Phase of
+                                          EfiPciHostBridgeAllocateResource, the PCI bus enumerator is responsible
+                                          for gathering I/O and memory requests for
+                                          all the PCI root bridges and submitting these requests using
+                                          SubmitResources(). This function pads the resource amount
+                                          to suit the root bridge hardware, takes care of dependencies between
+                                          the PCI root bridges, and calls the Global Coherency Domain (GCD)
+                                          with the allocation request. In the case of padding, the allocated range
+                                          could be bigger than what was requested.
+   EfiPciHostBridgeSetResources           Programs the host bridge hardware to decode previously allocated
+                                          resources (proposed resources) for all the PCI root bridges. After the
+                                          hardware is programmed, reassigning resources will not be supported.
+                                          The bus settings are not affected.
+   EfiPciHostBridgeFreeResources          Deallocates resources that were previously allocated for all the PCI
+                                          root bridges and resets the I/O and memory apertures to their initial
+                                          state. The bus settings are not affected. If the request to allocate
+                                          resources fails, the PCI enumerator can use this notification to
+                                          deallocate previous resources, adjust the requests, and retry
+                                          allocation.
+   EfiPciHostBridgeEndResourceAllocation  The resource allocation phase is completed. No specific action is
+                                          required here. This notification can be used to perform any chipsetspecific
+                                          programming.
+
+   @param[in] PciResAlloc         The instance pointer of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+   @param[in] Phase               The phase during enumeration
+
+   @retval EFI_NOT_READY          This phase cannot be entered at this time. For example, this error
+                                  is valid for a Phase of EfiPciHostBridgeAllocateResources if
+                                  SubmitResources() has not been called for one or more
+                                  PCI root bridges before this call
+   @retval EFI_DEVICE_ERROR       Programming failed due to a hardware error. This error is valid
+                                  for a Phase of EfiPciHostBridgeSetResources.
+   @retval EFI_INVALID_PARAMETER  Invalid phase parameter
+   @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a lack of resources.
+                                  This error is valid for a Phase of EfiPciHostBridgeAllocateResources if the
+                                  previously submitted resource requests cannot be fulfilled or
+                                  were only partially fulfilled.
+   @retval EFI_SUCCESS            The notification was accepted without any errors.
+
+**/
+EFI_STATUS
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE       Phase
+  )
+{
+  EFI_HANDLE                      HostBridgeHandle;
+  EFI_HANDLE                      RootBridgeHandle;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  EFI_STATUS                      Status;
+
+  HostBridgeHandle  = NULL;
+  RootBridgeHandle  = NULL;
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Get Host Bridge Handle.
+    //
+    PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle);
+
+    //
+    // Get the rootbridge Io protocol to find the host bridge handle
+    //
+    Status = gBS->HandleProtocol (
+                    RootBridgeHandle,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    (VOID **) &PciRootBridgeIo
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_NOT_FOUND;
+    }
+
+    HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+    //
+    // Call PlatformPci::PlatformNotify() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformNotify (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetEntry
+                            );
+  } else if (gPciOverrideProtocol != NULL){
+    //
+    // Get Host Bridge Handle.
+    //
+    PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle);
+
+    //
+    // Get the rootbridge Io protocol to find the host bridge handle
+    //
+    Status = gBS->HandleProtocol (
+                    RootBridgeHandle,
+                    &gEfiPciRootBridgeIoProtocolGuid,
+                    (VOID **) &PciRootBridgeIo
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_NOT_FOUND;
+    }
+
+    HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+
+    //
+    // Call PlatformPci::PhaseNotify() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformNotify (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetEntry
+                            );
+  }  
+
+  Status = PciResAlloc->NotifyPhase (
+                          PciResAlloc,
+                          Phase
+                          );
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PlatformNotify() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformNotify (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetExit
+                            );
+
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PhaseNotify() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformNotify (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            Phase,
+                            ChipsetExit
+                            );
+  }
+
+  return Status;
+}
+
+/**
+  Provides the hooks from the PCI bus driver to every PCI controller (device/function) at various
+  stages of the PCI enumeration process that allow the host bridge driver to preinitialize individual
+  PCI controllers before enumeration.
+
+  This function is called during the PCI enumeration process. No specific action is expected from this
+  member function. It allows the host bridge driver to preinitialize individual PCI controllers before
+  enumeration.
+
+  @param Bridge            Pointer to the EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Bus               The bus number of the pci device.
+  @param Device            The device number of the pci device.
+  @param Func              The function number of the pci device.
+  @param Phase             The phase of the PCI device enumeration.
+
+  @retval EFI_SUCCESS              The requested parameters were returned.
+  @retval EFI_INVALID_PARAMETER    RootBridgeHandle is not a valid root bridge handle.
+  @retval EFI_INVALID_PARAMETER    Phase is not a valid phase that is defined in
+                                   EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE.
+  @retval EFI_DEVICE_ERROR         Programming failed due to a hardware error. The PCI enumerator should
+                                   not enumerate this device, including its child devices if it is a PCI-to-PCI
+                                   bridge.
+
+**/
+EFI_STATUS
+PreprocessController (
+  IN PCI_IO_DEVICE                                    *Bridge,
+  IN UINT8                                            Bus,
+  IN UINT8                                            Device,
+  IN UINT8                                            Func,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE     Phase
+  )
+{
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS       RootBridgePciAddress;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc;
+  EFI_HANDLE                                        RootBridgeHandle;
+  EFI_HANDLE                                        HostBridgeHandle;
+  EFI_STATUS                                        Status;
+
+  //
+  // Get the host bridge handle
+  //
+  HostBridgeHandle = Bridge->PciRootBridgeIo->ParentHandle;
+
+  //
+  // Get the pci host bridge resource allocation protocol
+  //
+  Status = gBS->OpenProtocol (
+                  HostBridgeHandle,
+                  &gEfiPciHostBridgeResourceAllocationProtocolGuid,
+                  (VOID **) &PciResAlloc,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Root Brige Handle
+  //
+  while (Bridge->Parent != NULL) {
+    Bridge = Bridge->Parent;
+  }
+
+  RootBridgeHandle                      = Bridge->Handle;
+
+  RootBridgePciAddress.Register         = 0;
+  RootBridgePciAddress.Function         = Func;
+  RootBridgePciAddress.Device           = Device;
+  RootBridgePciAddress.Bus              = Bus;
+  RootBridgePciAddress.ExtendedRegister = 0;
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformPrepController (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetEntry
+                            );
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformPrepController (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetEntry
+                            );
+  }
+
+  Status = PciResAlloc->PreprocessController (
+                          PciResAlloc,
+                          RootBridgeHandle,
+                          RootBridgePciAddress,
+                          Phase
+                          );
+
+  if (gPciPlatformProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciPlatformProtocol->PlatformPrepController (
+                            gPciPlatformProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetExit
+                            );
+  } else if (gPciOverrideProtocol != NULL) {
+    //
+    // Call PlatformPci::PrepController() if the protocol is present.
+    //
+    gPciOverrideProtocol->PlatformPrepController (
+                            gPciOverrideProtocol,
+                            HostBridgeHandle,
+                            RootBridgeHandle,
+                            RootBridgePciAddress,
+                            Phase,
+                            ChipsetExit
+                            );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function allows the PCI bus driver to be notified to act as requested when a hot-plug event has
+  happened on the hot-plug controller. Currently, the operations include add operation and remove operation..
+
+  @param This                 A pointer to the hot plug request protocol.
+  @param Operation            The operation the PCI bus driver is requested to make.
+  @param Controller           The handle of the hot-plug controller.
+  @param RemainingDevicePath  The remaining device path for the PCI-like hot-plug device.
+  @param NumberOfChildren     The number of child handles.
+                              For a add operation, it is an output parameter.
+                              For a remove operation, it's an input parameter.
+  @param ChildHandleBuffer    The buffer which contains the child handles.
+
+  @retval EFI_INVALID_PARAMETER  Operation is not a legal value.
+                                 Controller is NULL or not a valid handle.
+                                 NumberOfChildren is NULL.
+                                 ChildHandleBuffer is NULL while Operation is add.
+  @retval EFI_OUT_OF_RESOURCES   There are no enough resources to start the devices.
+  @retval EFI_NOT_FOUND          Can not find bridge according to controller handle.
+  @retval EFI_SUCCESS            The handles for the specified device have been created or destroyed
+                                 as requested, and for an add operation, the new handles are
+                                 returned in ChildHandleBuffer.
+**/
+EFI_STATUS
+EFIAPI
+PciHotPlugRequestNotify (
+  IN EFI_PCI_HOTPLUG_REQUEST_PROTOCOL * This,
+  IN EFI_PCI_HOTPLUG_OPERATION        Operation,
+  IN EFI_HANDLE                       Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL         * RemainingDevicePath OPTIONAL,
+  IN OUT UINT8                        *NumberOfChildren,
+  IN OUT EFI_HANDLE                   * ChildHandleBuffer
+  )
+{
+  PCI_IO_DEVICE       *Bridge;
+  PCI_IO_DEVICE       *Temp;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINTN               Index;
+  EFI_HANDLE          RootBridgeHandle;
+  EFI_STATUS          Status;
+
+  //
+  // Check input parameter validity
+  //
+  if ((Controller == NULL) || (NumberOfChildren == NULL)){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Operation != EfiPciHotPlugRequestAdd) && (Operation != EfiPciHotplugRequestRemove)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Operation == EfiPciHotPlugRequestAdd){
+    if (ChildHandleBuffer == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else if ((Operation == EfiPciHotplugRequestRemove) && (*NumberOfChildren != 0)) {
+    if (ChildHandleBuffer == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Bridge = PCI_IO_DEVICE_FROM_PCI_IO_THIS (PciIo);
+
+  //
+  // Get root bridge handle
+  //
+  Temp = Bridge;
+  while (Temp->Parent != NULL) {
+    Temp = Temp->Parent;
+  }
+
+  RootBridgeHandle = Temp->Handle;
+
+  if (Operation == EfiPciHotPlugRequestAdd) {
+    //
+    // Report Status Code to indicate hot plug happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_PCI | EFI_IOB_PC_HOTPLUG),
+      Temp->DevicePath
+      );
+
+    if (NumberOfChildren != NULL) {
+      *NumberOfChildren = 0;
+    }
+
+    if (IsListEmpty (&Bridge->ChildList)) {
+
+      Status = PciBridgeEnumerator (Bridge);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+
+    Status = StartPciDevicesOnBridge (
+              RootBridgeHandle,
+              Bridge,
+              RemainingDevicePath,
+              NumberOfChildren,
+              ChildHandleBuffer
+              );
+
+    return Status;
+  }
+
+  if (Operation == EfiPciHotplugRequestRemove) {
+
+    if (*NumberOfChildren == 0) {
+      //
+      // Remove all devices on the bridge
+      //
+      RemoveAllPciDeviceOnBridge (RootBridgeHandle, Bridge);
+      return EFI_SUCCESS;
+
+    }
+
+    for (Index = 0; Index < *NumberOfChildren; Index++) {
+      //
+      // De register all the pci device
+      //
+      Status = DeRegisterPciDevice (RootBridgeHandle, ChildHandleBuffer[Index]);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+    }
+    //
+    // End for
+    //
+    return EFI_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Search hostbridge according to given handle
+
+  @param RootBridgeHandle  Host bridge handle.
+
+  @retval TRUE             Found host bridge handle.
+  @retval FALSE            Not found hot bridge handle.
+
+**/
+BOOLEAN
+SearchHostBridgeHandle (
+  IN EFI_HANDLE RootBridgeHandle
+  )
+{
+  EFI_HANDLE                      HostBridgeHandle;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  UINTN                           Index;
+  EFI_STATUS                      Status;
+
+  //
+  // Get the rootbridge Io protocol to find the host bridge handle
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  HostBridgeHandle = PciRootBridgeIo->ParentHandle;
+  for (Index = 0; Index < gPciHostBridgeNumber; Index++) {
+    if (HostBridgeHandle == gPciHostBrigeHandles[Index]) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Add host bridge handle to global variable for enumerating.
+
+  @param HostBridgeHandle   Host bridge handle.
+
+  @retval EFI_SUCCESS       Successfully added host bridge.
+  @retval EFI_ABORTED       Host bridge is NULL, or given host bridge
+                            has been in host bridge list.
+
+**/
+EFI_STATUS
+AddHostBridgeEnumerator (
+  IN EFI_HANDLE HostBridgeHandle
+  )
+{
+  UINTN Index;
+
+  if (HostBridgeHandle == NULL) {
+    return EFI_ABORTED;
+  }
+
+  for (Index = 0; Index < gPciHostBridgeNumber; Index++) {
+    if (HostBridgeHandle == gPciHostBrigeHandles[Index]) {
+      return EFI_ABORTED;
+    }
+  }
+
+  if (Index < PCI_MAX_HOST_BRIDGE_NUM) {
+    gPciHostBrigeHandles[Index] = HostBridgeHandle;
+    gPciHostBridgeNumber++;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h
new file mode 100644
index 0000000000..ff164d1f09
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumerator.h
@@ -0,0 +1,519 @@
+/** @file
+  PCI bus enumeration logic function declaration for PCI bus module.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef _EFI_PCI_ENUMERATOR_H_
+#define _EFI_PCI_ENUMERATOR_H_
+
+#include "PciResourceSupport.h"
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  @param Controller  Parent controller handle.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumerator (
+  IN EFI_HANDLE                    Controller
+  );
+
+/**
+  Enumerate PCI root bridge.
+
+  @param PciResAlloc   Pointer to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+  @param RootBridgeDev Instance of root bridge device.
+
+  @retval EFI_SUCCESS  Successfully enumerated root bridge.
+  @retval other        Failed to enumerate root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc,
+  IN PCI_IO_DEVICE                                     *RootBridgeDev
+  );
+
+/**
+  This routine is used to process all PCI devices' Option Rom
+  on a certain root bridge.
+
+  @param Bridge     Given parent's root bridge.
+  @param RomBase    Base address of ROM driver loaded from.
+  @param MaxLength  Maximum rom size.
+
+**/
+VOID
+ProcessOptionRom (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT64        RomBase,
+  IN UINT64        MaxLength
+  );
+
+/**
+  This routine is used to assign bus number to the given PCI bus system
+
+  @param Bridge             Parent root bridge instance.
+  @param StartBusNumber     Number of beginning.
+  @param SubBusNumber       The number of sub bus.
+
+  @retval EFI_SUCCESS       Successfully assigned bus number.
+  @retval EFI_DEVICE_ERROR  Failed to assign bus number.
+
+**/
+EFI_STATUS
+PciAssignBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber
+  );
+
+/**
+  This routine is used to determine the root bridge attribute by interfacing
+  the host bridge resource allocation protocol.
+
+  @param PciResAlloc    Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+  @param RootBridgeDev  Root bridge instance
+
+  @retval EFI_SUCCESS  Successfully got root bridge's attribute.
+  @retval other        Failed to get attribute.
+
+**/
+EFI_STATUS
+DetermineRootBridgeAttributes (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  IN PCI_IO_DEVICE                                    *RootBridgeDev
+  );
+
+/**
+  Get Max Option Rom size on specified bridge.
+
+  @param Bridge    Given bridge device instance.
+
+  @return Max size of option rom needed.
+
+**/
+UINT64
+GetMaxOptionRomSize (
+  IN PCI_IO_DEVICE   *Bridge
+  );
+
+/**
+  Process attributes of devices on this host bridge
+
+  @param PciResAlloc Protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Successfully process attribute.
+  @retval EFI_NOT_FOUND Can not find the specific root bridge device.
+  @retval other         Failed to determine the root bridge device's attribute.
+
+**/
+EFI_STATUS
+PciHostBridgeDeviceAttribute (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  Get resource allocation status from the ACPI resource descriptor.
+
+  @param AcpiConfig       Point to Acpi configuration table.
+  @param IoResStatus      Return the status of I/O resource.
+  @param Mem32ResStatus   Return the status of 32-bit Memory resource.
+  @param PMem32ResStatus  Return the status of 32-bit Prefetchable Memory resource.
+  @param Mem64ResStatus   Return the status of 64-bit Memory resource.
+  @param PMem64ResStatus  Return the status of 64-bit Prefetchable Memory resource.
+
+**/
+VOID
+GetResourceAllocationStatus (
+  VOID        *AcpiConfig,
+  OUT UINT64  *IoResStatus,
+  OUT UINT64  *Mem32ResStatus,
+  OUT UINT64  *PMem32ResStatus,
+  OUT UINT64  *Mem64ResStatus,
+  OUT UINT64  *PMem64ResStatus
+  );
+
+/**
+  Remove a PCI device from device pool and mark its bar.
+
+  @param PciDevice Instance of Pci device.
+
+  @retval EFI_SUCCESS Successfully remove the PCI device.
+  @retval EFI_ABORTED Pci device is a root bridge or a PCI-PCI bridge.
+
+**/
+EFI_STATUS
+RejectPciDevice (
+  IN PCI_IO_DEVICE       *PciDevice
+  );
+
+/**
+  Determine whethter a PCI device can be rejected.
+
+  @param  PciResNode Pointer to Pci resource node instance.
+
+  @retval TRUE  The PCI device can be rejected.
+  @retval TRUE  The PCI device cannot be rejected.
+
+**/
+BOOLEAN
+IsRejectiveDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode
+  );
+
+/**
+  Compare two resource nodes and get the larger resource consumer.
+
+  @param PciResNode1  resource node 1 want to be compared
+  @param PciResNode2  resource node 2 want to be compared
+
+  @return Larger resource node.
+
+**/
+PCI_RESOURCE_NODE *
+GetLargerConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *PciResNode1,
+  IN  PCI_RESOURCE_NODE   *PciResNode2
+  );
+
+/**
+  Get the max resource consumer in the host resource pool.
+
+  @param ResPool  Pointer to resource pool node.
+
+  @return The max resource consumer in the host resource pool.
+
+**/
+PCI_RESOURCE_NODE *
+GetMaxResourceConsumerDevice (
+  IN  PCI_RESOURCE_NODE   *ResPool
+  );
+
+/**
+  Adjust host bridge allocation so as to reduce resource requirement
+
+  @param IoPool           Pointer to instance of I/O resource Node.
+  @param Mem32Pool        Pointer to instance of 32-bit memory resource Node.
+  @param PMem32Pool       Pointer to instance of 32-bit Prefetchable memory resource node.
+  @param Mem64Pool        Pointer to instance of 64-bit memory resource node.
+  @param PMem64Pool       Pointer to instance of 64-bit Prefetchable memory resource node.
+  @param IoResStatus      Status of I/O resource Node.
+  @param Mem32ResStatus   Status of 32-bit memory resource Node.
+  @param PMem32ResStatus  Status of 32-bit Prefetchable memory resource node.
+  @param Mem64ResStatus   Status of 64-bit memory resource node.
+  @param PMem64ResStatus  Status of 64-bit Prefetchable memory resource node.
+
+  @retval EFI_SUCCESS     Successfully adjusted resoruce on host bridge.
+  @retval EFI_ABORTED     Host bridge hasn't this resource type or no resource be adjusted.
+
+**/
+EFI_STATUS
+PciHostBridgeAdjustAllocation (
+  IN  PCI_RESOURCE_NODE   *IoPool,
+  IN  PCI_RESOURCE_NODE   *Mem32Pool,
+  IN  PCI_RESOURCE_NODE   *PMem32Pool,
+  IN  PCI_RESOURCE_NODE   *Mem64Pool,
+  IN  PCI_RESOURCE_NODE   *PMem64Pool,
+  IN  UINT64              IoResStatus,
+  IN  UINT64              Mem32ResStatus,
+  IN  UINT64              PMem32ResStatus,
+  IN  UINT64              Mem64ResStatus,
+  IN  UINT64              PMem64ResStatus
+  );
+
+/**
+  Summary requests for all resource type, and contruct ACPI resource
+  requestor instance.
+
+  @param Bridge           detecting bridge
+  @param IoNode           Pointer to instance of I/O resource Node
+  @param Mem32Node        Pointer to instance of 32-bit memory resource Node
+  @param PMem32Node       Pointer to instance of 32-bit Pmemory resource node
+  @param Mem64Node        Pointer to instance of 64-bit memory resource node
+  @param PMem64Node       Pointer to instance of 64-bit Pmemory resource node
+  @param Config           Output buffer holding new constructed APCI resource requestor
+
+  @retval EFI_SUCCESS           Successfully constructed ACPI resource.
+  @retval EFI_OUT_OF_RESOURCES  No memory availabe.
+
+**/
+EFI_STATUS
+ConstructAcpiResourceRequestor (
+  IN PCI_IO_DEVICE      *Bridge,
+  IN PCI_RESOURCE_NODE  *IoNode,
+  IN PCI_RESOURCE_NODE  *Mem32Node,
+  IN PCI_RESOURCE_NODE  *PMem32Node,
+  IN PCI_RESOURCE_NODE  *Mem64Node,
+  IN PCI_RESOURCE_NODE  *PMem64Node,
+  OUT VOID              **Config
+  );
+
+/**
+  Get resource base from an acpi configuration descriptor.
+
+  @param Config       An acpi configuration descriptor.
+  @param IoBase       Output of I/O resource base address.
+  @param Mem32Base    Output of 32-bit memory base address.
+  @param PMem32Base   Output of 32-bit prefetchable memory base address.
+  @param Mem64Base    Output of 64-bit memory base address.
+  @param PMem64Base   Output of 64-bit prefetchable memory base address.
+
+**/
+VOID
+GetResourceBase (
+  IN VOID     *Config,
+  OUT UINT64  *IoBase,
+  OUT UINT64  *Mem32Base,
+  OUT UINT64  *PMem32Base,
+  OUT UINT64  *Mem64Base,
+  OUT UINT64  *PMem64Base
+  );
+
+/**
+  Enumerate pci bridge, allocate resource and determine attribute
+  for devices on this bridge.
+
+  @param BridgeDev    Pointer to instance of bridge device.
+
+  @retval EFI_SUCCESS Successfully enumerated PCI bridge.
+  @retval other       Failed to enumerate.
+
+**/
+EFI_STATUS
+PciBridgeEnumerator (
+  IN PCI_IO_DEVICE                                     *BridgeDev
+  );
+
+/**
+  Allocate all kinds of resource for PCI bridge.
+
+  @param  Bridge      Pointer to bridge instance.
+
+  @retval EFI_SUCCESS Successfully allocated resource for PCI bridge.
+  @retval other       Failed to allocate resource for bridge.
+
+**/
+EFI_STATUS
+PciBridgeResourceAllocator (
+  IN PCI_IO_DEVICE  *Bridge
+  );
+
+/**
+  Get resource base address for a pci bridge device.
+
+  @param Bridge     Given Pci driver instance.
+  @param IoBase     Output for base address of I/O type resource.
+  @param Mem32Base  Output for base address of 32-bit memory type resource.
+  @param PMem32Base Ooutput for base address of 32-bit Pmemory type resource.
+  @param Mem64Base  Output for base address of 64-bit memory type resource.
+  @param PMem64Base Output for base address of 64-bit Pmemory type resource.
+
+  @retval EFI_SUCCESS           Successfully got resource base address.
+  @retval EFI_OUT_OF_RESOURCES  PCI bridge is not available.
+
+**/
+EFI_STATUS
+GetResourceBaseFromBridge (
+  IN  PCI_IO_DEVICE *Bridge,
+  OUT UINT64        *IoBase,
+  OUT UINT64        *Mem32Base,
+  OUT UINT64        *PMem32Base,
+  OUT UINT64        *Mem64Base,
+  OUT UINT64        *PMem64Base
+  );
+
+/**
+  Process Option Rom on this host bridge
+
+  @param PciResAlloc Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval EFI_SUCCESS   Success process.
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+   These are the notifications from the PCI bus driver that it is about to enter a certain
+   phase of the PCI enumeration process.
+
+   This member function can be used to notify the host bridge driver to perform specific actions,
+   including any chipset-specific initialization, so that the chipset is ready to enter the next phase.
+   Eight notification points are defined at this time. See belows:
+   EfiPciHostBridgeBeginEnumeration       Resets the host bridge PCI apertures and internal data
+                                          structures. The PCI enumerator should issue this notification
+                                          before starting a fresh enumeration process. Enumeration cannot
+                                          be restarted after sending any other notification such as
+                                          EfiPciHostBridgeBeginBusAllocation.
+   EfiPciHostBridgeBeginBusAllocation     The bus allocation phase is about to begin. No specific action is
+                                          required here. This notification can be used to perform any
+                                          chipset-specific programming.
+   EfiPciHostBridgeEndBusAllocation       The bus allocation and bus programming phase is complete. No
+                                          specific action is required here. This notification can be used to
+                                          perform any chipset-specific programming.
+   EfiPciHostBridgeBeginResourceAllocation
+                                          The resource allocation phase is about to begin. No specific
+                                          action is required here. This notification can be used to perform
+                                          any chipset-specific programming.
+   EfiPciHostBridgeAllocateResources      Allocates resources per previously submitted requests for all the PCI
+                                          root bridges. These resource settings are returned on the next call to
+                                          GetProposedResources(). Before calling NotifyPhase() with a Phase of
+                                          EfiPciHostBridgeAllocateResource, the PCI bus enumerator is responsible
+                                          for gathering I/O and memory requests for
+                                          all the PCI root bridges and submitting these requests using
+                                          SubmitResources(). This function pads the resource amount
+                                          to suit the root bridge hardware, takes care of dependencies between
+                                          the PCI root bridges, and calls the Global Coherency Domain (GCD)
+                                          with the allocation request. In the case of padding, the allocated range
+                                          could be bigger than what was requested.
+   EfiPciHostBridgeSetResources           Programs the host bridge hardware to decode previously allocated
+                                          resources (proposed resources) for all the PCI root bridges. After the
+                                          hardware is programmed, reassigning resources will not be supported.
+                                          The bus settings are not affected.
+   EfiPciHostBridgeFreeResources          Deallocates resources that were previously allocated for all the PCI
+                                          root bridges and resets the I/O and memory apertures to their initial
+                                          state. The bus settings are not affected. If the request to allocate
+                                          resources fails, the PCI enumerator can use this notification to
+                                          deallocate previous resources, adjust the requests, and retry
+                                          allocation.
+   EfiPciHostBridgeEndResourceAllocation  The resource allocation phase is completed. No specific action is
+                                          required here. This notification can be used to perform any chipsetspecific
+                                          programming.
+
+   @param[in] PciResAlloc         The instance pointer of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+   @param[in] Phase               The phase during enumeration
+
+   @retval EFI_NOT_READY          This phase cannot be entered at this time. For example, this error
+                                  is valid for a Phase of EfiPciHostBridgeAllocateResources if
+                                  SubmitResources() has not been called for one or more
+                                  PCI root bridges before this call
+   @retval EFI_DEVICE_ERROR       Programming failed due to a hardware error. This error is valid
+                                  for a Phase of EfiPciHostBridgeSetResources.
+   @retval EFI_INVALID_PARAMETER  Invalid phase parameter
+   @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a lack of resources.
+                                  This error is valid for a Phase of EfiPciHostBridgeAllocateResources if the
+                                  previously submitted resource requests cannot be fulfilled or
+                                  were only partially fulfilled.
+   @retval EFI_SUCCESS            The notification was accepted without any errors.
+
+**/
+EFI_STATUS
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc,
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE       Phase
+  );
+
+/**
+  Provides the hooks from the PCI bus driver to every PCI controller (device/function) at various
+  stages of the PCI enumeration process that allow the host bridge driver to preinitialize individual
+  PCI controllers before enumeration.
+
+  This function is called during the PCI enumeration process. No specific action is expected from this
+  member function. It allows the host bridge driver to preinitialize individual PCI controllers before
+  enumeration.
+
+  @param Bridge            Pointer to the EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Bus               The bus number of the pci device.
+  @param Device            The device number of the pci device.
+  @param Func              The function number of the pci device.
+  @param Phase             The phase of the PCI device enumeration.
+
+  @retval EFI_SUCCESS              The requested parameters were returned.
+  @retval EFI_INVALID_PARAMETER    RootBridgeHandle is not a valid root bridge handle.
+  @retval EFI_INVALID_PARAMETER    Phase is not a valid phase that is defined in
+                                   EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE.
+  @retval EFI_DEVICE_ERROR         Programming failed due to a hardware error. The PCI enumerator should
+                                    not enumerate this device, including its child devices if it is a PCI-to-PCI
+                                    bridge.
+
+**/
+EFI_STATUS
+PreprocessController (
+  IN PCI_IO_DEVICE                                  *Bridge,
+  IN UINT8                                          Bus,
+  IN UINT8                                          Device,
+  IN UINT8                                          Func,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE   Phase
+  );
+
+/**
+  This function allows the PCI bus driver to be notified to act as requested when a hot-plug event has
+  happened on the hot-plug controller. Currently, the operations include add operation and remove operation..
+
+  @param This                 A pointer to the hot plug request protocol.
+  @param Operation            The operation the PCI bus driver is requested to make.
+  @param Controller           The handle of the hot-plug controller.
+  @param RemainingDevicePath  The remaining device path for the PCI-like hot-plug device.
+  @param NumberOfChildren     The number of child handles.
+                              For a add operation, it is an output parameter.
+                              For a remove operation, it's an input parameter.
+  @param ChildHandleBuffer    The buffer which contains the child handles.
+
+  @retval EFI_INVALID_PARAMETER  Operation is not a legal value.
+                                 Controller is NULL or not a valid handle.
+                                 NumberOfChildren is NULL.
+                                 ChildHandleBuffer is NULL while Operation is add.
+  @retval EFI_OUT_OF_RESOURCES   There are no enough resources to start the devices.
+  @retval EFI_NOT_FOUND          Can not find bridge according to controller handle.
+  @retval EFI_SUCCESS            The handles for the specified device have been created or destroyed
+                                 as requested, and for an add operation, the new handles are
+                                 returned in ChildHandleBuffer.
+**/
+EFI_STATUS
+EFIAPI
+PciHotPlugRequestNotify (
+  IN EFI_PCI_HOTPLUG_REQUEST_PROTOCOL * This,
+  IN EFI_PCI_HOTPLUG_OPERATION        Operation,
+  IN EFI_HANDLE                       Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL         * RemainingDevicePath OPTIONAL,
+  IN OUT UINT8                        *NumberOfChildren,
+  IN OUT EFI_HANDLE                   * ChildHandleBuffer
+  );
+
+/**
+  Search hostbridge according to given handle
+
+  @param RootBridgeHandle  Host bridge handle.
+
+  @retval TRUE             Found host bridge handle.
+  @retval FALSE            Not found hot bridge handle.
+
+**/
+BOOLEAN
+SearchHostBridgeHandle (
+  IN EFI_HANDLE RootBridgeHandle
+  );
+
+/**
+  Add host bridge handle to global variable for enumerating.
+
+  @param HostBridgeHandle   Host bridge handle.
+
+  @retval EFI_SUCCESS       Successfully added host bridge.
+  @retval EFI_ABORTED       Host bridge is NULL, or given host bridge
+                            has been in host bridge list.
+
+**/
+EFI_STATUS
+AddHostBridgeEnumerator (
+  IN EFI_HANDLE HostBridgeHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c
new file mode 100644
index 0000000000..b070eb1b5f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.c
@@ -0,0 +1,2717 @@
+/** @file
+  PCI emumeration support functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+extern CHAR16  *mBarTypeStr[];
+
+/**
+  This routine is used to check whether the pci device is present.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Output buffer for PCI device configuration space.
+  @param Bus               PCI bus NO.
+  @param Device            PCI device NO.
+  @param Func              PCI Func NO.
+
+  @retval EFI_NOT_FOUND    PCI device not present.
+  @retval EFI_SUCCESS      PCI device is found.
+
+**/
+EFI_STATUS
+PciDevicePresent (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *PciRootBridgeIo,
+  OUT PCI_TYPE00                          *Pci,
+  IN  UINT8                               Bus,
+  IN  UINT8                               Device,
+  IN  UINT8                               Func
+  )
+{
+  UINT64      Address;
+  EFI_STATUS  Status;
+
+  //
+  // Create PCI address map in terms of Bus, Device and Func
+  //
+  Address = EFI_PCI_ADDRESS (Bus, Device, Func, 0);
+
+  //
+  // Read the Vendor ID register
+  //
+  Status = PciRootBridgeIo->Pci.Read (
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  Pci
+                                  );
+
+  if (!EFI_ERROR (Status) && (Pci->Hdr).VendorId != 0xffff) {
+    //
+    // Read the entire config header for the device
+    //
+    Status = PciRootBridgeIo->Pci.Read (
+                                    PciRootBridgeIo,
+                                    EfiPciWidthUint32,
+                                    Address,
+                                    sizeof (PCI_TYPE00) / sizeof (UINT32),
+                                    Pci
+                                    );
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Collect all the resource information under this root bridge.
+
+  A database that records all the information about pci device subject to this
+  root bridge will then be created.
+
+  @param Bridge         Parent bridge instance.
+  @param StartBusNumber Bus number of begining.
+
+  @retval EFI_SUCCESS   PCI device is found.
+  @retval other         Some error occurred when reading PCI bridge information.
+
+**/
+EFI_STATUS
+PciPciDeviceInfoCollector (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  )
+{
+  EFI_STATUS          Status;
+  PCI_TYPE00          Pci;
+  UINT8               Device;
+  UINT8               Func;
+  UINT8               SecBus;
+  PCI_IO_DEVICE       *PciIoDevice;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  Status  = EFI_SUCCESS;
+  SecBus  = 0;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether PCI device is present
+      //
+      Status = PciDevicePresent (
+                 Bridge->PciRootBridgeIo,
+                 &Pci,
+                 (UINT8) StartBusNumber,
+                 (UINT8) Device,
+                 (UINT8) Func
+                 );
+      if (!EFI_ERROR (Status)) {
+
+        //
+        // Call back to host bridge function
+        //
+        PreprocessController (Bridge, (UINT8) StartBusNumber, Device, Func, EfiPciBeforeResourceCollection);
+
+        //
+        // Collect all the information about the PCI device discovered
+        //
+        Status = PciSearchDevice (
+                   Bridge,
+                   &Pci,
+                   (UINT8) StartBusNumber,
+                   Device,
+                   Func,
+                   &PciIoDevice
+                   );
+
+        //
+        // Recursively scan PCI busses on the other side of PCI-PCI bridges
+        //
+        //
+        if (!EFI_ERROR (Status) && (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci))) {
+
+          //
+          // If it is PPB, we need to get the secondary bus to continue the enumeration
+          //
+          PciIo   = &(PciIoDevice->PciIo);
+
+          Status  = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET, 1, &SecBus);
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          //
+          // Get resource padding for PPB
+          //
+          GetResourcePaddingPpb (PciIoDevice);
+
+          //
+          // Deep enumerate the next level bus
+          //
+          Status = PciPciDeviceInfoCollector (
+                     PciIoDevice,
+                     (UINT8) (SecBus)
+                     );
+
+        }
+
+        if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+          //
+          // Skip sub functions, this is not a multi function device
+          //
+          Func = PCI_MAX_FUNC;
+        }
+      }
+
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Seach required device and create PCI device instance.
+
+  @param Bridge     Parent bridge instance.
+  @param Pci        Input PCI device information block.
+  @param Bus        PCI bus NO.
+  @param Device     PCI device NO.
+  @param Func       PCI func  NO.
+  @param PciDevice  Output of searched PCI device instance.
+
+  @retval EFI_SUCCESS           Successfully created PCI device instance.
+  @retval EFI_OUT_OF_RESOURCES  Cannot get PCI device information.
+
+**/
+EFI_STATUS
+PciSearchDevice (
+  IN  PCI_IO_DEVICE                         *Bridge,
+  IN  PCI_TYPE00                            *Pci,
+  IN  UINT8                                 Bus,
+  IN  UINT8                                 Device,
+  IN  UINT8                                 Func,
+  OUT PCI_IO_DEVICE                         **PciDevice
+  )
+{
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = NULL;
+
+  DEBUG ((
+    EFI_D_INFO,
+    "PciBus: Discovered %s @ [%02x|%02x|%02x]\n",
+    IS_PCI_BRIDGE (Pci) ?     L"PPB" :
+    IS_CARDBUS_BRIDGE (Pci) ? L"P2C" :
+                              L"PCI",
+    Bus, Device, Func
+    ));
+
+  if (!IS_PCI_BRIDGE (Pci)) {
+
+    if (IS_CARDBUS_BRIDGE (Pci)) {
+      PciIoDevice = GatherP2CInfo (
+                      Bridge,
+                      Pci,
+                      Bus,
+                      Device,
+                      Func
+                      );
+      if ((PciIoDevice != NULL) && gFullEnumeration) {
+        InitializeP2C (PciIoDevice);
+      }
+    } else {
+
+      //
+      // Create private data for Pci Device
+      //
+      PciIoDevice = GatherDeviceInfo (
+                      Bridge,
+                      Pci,
+                      Bus,
+                      Device,
+                      Func
+                      );
+
+    }
+
+  } else {
+
+    //
+    // Create private data for PPB
+    //
+    PciIoDevice = GatherPpbInfo (
+                    Bridge,
+                    Pci,
+                    Bus,
+                    Device,
+                    Func
+                    );
+
+    //
+    // Special initialization for PPB including making the PPB quiet
+    //
+    if ((PciIoDevice != NULL) && gFullEnumeration) {
+      InitializePpb (PciIoDevice);
+    }
+  }
+
+  if (PciIoDevice == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Update the bar information for this PCI device so as to support some specific device
+  //
+  UpdatePciInfo (PciIoDevice);
+
+  if (PciIoDevice->DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Detect this function has option rom
+  //
+  if (gFullEnumeration) {
+
+    if (!IS_CARDBUS_BRIDGE (Pci)) {
+
+      GetOpRomInfo (PciIoDevice);
+
+    }
+
+    ResetPowerManagementFeature (PciIoDevice);
+
+  }
+
+  //
+  // Insert it into a global tree for future reference
+  //
+  InsertPciDevice (Bridge, PciIoDevice);
+
+  //
+  // Determine PCI device attributes
+  //
+
+  if (PciDevice != NULL) {
+    *PciDevice = PciIoDevice;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dump the PPB padding resource information.
+
+  @param PciIoDevice     PCI IO instance.
+  @param ResourceType    The desired resource type to dump.
+                         PciBarTypeUnknown means to dump all types of resources.
+**/
+VOID
+DumpPpbPaddingResource (
+  IN PCI_IO_DEVICE                    *PciIoDevice,
+  IN PCI_BAR_TYPE                     ResourceType
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  PCI_BAR_TYPE                      Type;
+
+  if (PciIoDevice->ResourcePaddingDescriptors == NULL) {
+    return;
+  }
+
+  if (ResourceType == PciBarTypeIo16 || ResourceType == PciBarTypeIo32) {
+    ResourceType = PciBarTypeIo;
+  }
+
+  for (Descriptor = PciIoDevice->ResourcePaddingDescriptors; Descriptor->Desc != ACPI_END_TAG_DESCRIPTOR; Descriptor++) {
+
+    Type = PciBarTypeUnknown;
+    if (Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_IO) {
+      Type = PciBarTypeIo;
+    } else if (Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+
+      if (Descriptor->AddrSpaceGranularity == 32) {
+        //
+        // prefechable
+        //
+        if (Descriptor->SpecificFlag == EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) {
+          Type = PciBarTypePMem32;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Descriptor->SpecificFlag == 0) {
+          Type = PciBarTypeMem32;
+        }
+      }
+
+      if (Descriptor->AddrSpaceGranularity == 64) {
+        //
+        // prefechable
+        //
+        if (Descriptor->SpecificFlag == EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) {
+          Type = PciBarTypePMem64;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Descriptor->SpecificFlag == 0) {
+          Type = PciBarTypeMem64;
+        }
+      }
+    }
+
+    if ((Type != PciBarTypeUnknown) && ((ResourceType == PciBarTypeUnknown) || (ResourceType == Type))) {
+      DEBUG ((
+        EFI_D_INFO,
+        "   Padding: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx\n",
+        mBarTypeStr[Type], Descriptor->AddrRangeMax, Descriptor->AddrLen
+        ));
+    }
+  }
+
+}
+
+/**
+  Dump the PCI BAR information.
+
+  @param PciIoDevice     PCI IO instance.
+**/
+VOID
+DumpPciBars (
+  IN PCI_IO_DEVICE                    *PciIoDevice
+  )
+{
+  UINTN                               Index;
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+    if (PciIoDevice->PciBar[Index].BarType == PciBarTypeUnknown) {
+      continue;
+    }
+
+    DEBUG ((
+      EFI_D_INFO,
+      "   BAR[%d]: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx;\tOffset = 0x%02x\n",
+      Index, mBarTypeStr[MIN (PciIoDevice->PciBar[Index].BarType, PciBarTypeMaxType)],
+      PciIoDevice->PciBar[Index].Alignment, PciIoDevice->PciBar[Index].Length, PciIoDevice->PciBar[Index].Offset
+      ));
+  }
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+    if ((PciIoDevice->VfPciBar[Index].BarType == PciBarTypeUnknown) && (PciIoDevice->VfPciBar[Index].Length == 0)) {
+      continue;
+    }
+
+    DEBUG ((
+      EFI_D_INFO,
+      " VFBAR[%d]: Type = %s; Alignment = 0x%lx;\tLength = 0x%lx;\tOffset = 0x%02x\n",
+      Index, mBarTypeStr[MIN (PciIoDevice->VfPciBar[Index].BarType, PciBarTypeMaxType)],
+      PciIoDevice->VfPciBar[Index].Alignment, PciIoDevice->VfPciBar[Index].Length, PciIoDevice->VfPciBar[Index].Offset
+      ));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+}
+
+/**
+  Create PCI device instance for PCI device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherDeviceInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  UINTN                           Offset;
+  UINTN                           BarIndex;
+  PCI_IO_DEVICE                   *PciIoDevice;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  //
+  // If it is a full enumeration, disconnect the device in advance
+  //
+  if (gFullEnumeration) {
+
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+  }
+
+  //
+  // Start to parse the bars
+  //
+  for (Offset = 0x10, BarIndex = 0; Offset <= 0x24 && BarIndex < PCI_MAX_BAR; BarIndex++) {
+    Offset = PciParseBar (PciIoDevice, Offset, BarIndex);
+  }
+
+  //
+  // Parse the SR-IOV VF bars
+  //
+  if (PcdGetBool (PcdSrIovSupport) && PciIoDevice->SrIovCapabilityOffset != 0) {
+    for (Offset = PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_BAR0, BarIndex = 0;
+         Offset <= PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_BAR5;
+         BarIndex++) {
+
+      ASSERT (BarIndex < PCI_MAX_BAR);
+      Offset = PciIovParseVfBar (PciIoDevice, Offset, BarIndex);
+    }
+  }
+
+  DEBUG_CODE (DumpPciBars (PciIoDevice););
+  return PciIoDevice;
+}
+
+/**
+  Create PCI device instance for PCI-PCI bridge.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherPpbInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE                   *PciIoDevice;
+  EFI_STATUS                      Status;
+  UINT8                           Value;
+  EFI_PCI_IO_PROTOCOL             *PciIo;
+  UINT8                           Temp;
+  UINT32                          PMemBaseLimit;
+  UINT16                          PrefetchableMemoryBase;
+  UINT16                          PrefetchableMemoryLimit;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  if (gFullEnumeration) {
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+    //
+    // Initalize the bridge control register
+    //
+    PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_BITS_OWNED);
+
+  }
+
+  //
+  // PPB can have two BARs
+  //
+  if (PciParseBar (PciIoDevice, 0x10, PPB_BAR_0) == 0x14) {
+    //
+    // Not 64-bit bar
+    //
+    PciParseBar (PciIoDevice, 0x14, PPB_BAR_1);
+  }
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Test whether it support 32 decode or not
+  //
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+
+  if (Value != 0) {
+    if ((Value & 0x01) != 0) {
+      PciIoDevice->Decodes |= EFI_BRIDGE_IO32_DECODE_SUPPORTED;
+    } else {
+      PciIoDevice->Decodes |= EFI_BRIDGE_IO16_DECODE_SUPPORTED;
+    }
+  }
+
+  //
+  // if PcdPciBridgeIoAlignmentProbe is TRUE, PCI bus driver probes
+  // PCI bridge supporting non-stardard I/O window alignment less than 4K.
+  //
+
+  PciIoDevice->BridgeIoAlignment = 0xFFF;
+  if (FeaturePcdGet (PcdPciBridgeIoAlignmentProbe)) {
+    //
+    // Check any bits of bit 3-1 of I/O Base Register are writable.
+    // if so, it is assumed non-stardard I/O window alignment is supported by this bridge.
+    // Per spec, bit 3-1 of I/O Base Register are reserved bits, so its content can't be assumed.
+    //
+    Value = (UINT8)(Temp ^ (BIT3 | BIT2 | BIT1));
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+    PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Value);
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &Temp);
+    Value = (UINT8)((Value ^ Temp) & (BIT3 | BIT2 | BIT1));
+    switch (Value) {
+      case BIT3:
+        PciIoDevice->BridgeIoAlignment = 0x7FF;
+        break;
+      case BIT3 | BIT2:
+        PciIoDevice->BridgeIoAlignment = 0x3FF;
+        break;
+      case BIT3 | BIT2 | BIT1:
+        PciIoDevice->BridgeIoAlignment = 0x1FF;
+        break;
+    }
+  }
+
+  Status = BarExisted (
+            PciIoDevice,
+            0x24,
+            NULL,
+            &PMemBaseLimit
+            );
+
+  //
+  // Test if it supports 64 memory or not
+  //
+  // The bottom 4 bits of both the Prefetchable Memory Base and Prefetchable Memory Limit
+  // registers:
+  //   0 - the bridge supports only 32 bit addresses.
+  //   1 - the bridge supports 64-bit addresses.
+  //
+  PrefetchableMemoryBase = (UINT16)(PMemBaseLimit & 0xffff);
+  PrefetchableMemoryLimit = (UINT16)(PMemBaseLimit >> 16);
+  if (!EFI_ERROR (Status) &&
+      (PrefetchableMemoryBase & 0x000f) == 0x0001 &&
+      (PrefetchableMemoryLimit & 0x000f) == 0x0001) {
+    Status = BarExisted (
+              PciIoDevice,
+              0x28,
+              NULL,
+              NULL
+              );
+
+    if (!EFI_ERROR (Status)) {
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM64_DECODE_SUPPORTED;
+    } else {
+      PciIoDevice->Decodes |= EFI_BRIDGE_PMEM32_DECODE_SUPPORTED;
+    }
+  }
+
+  //
+  // Memory 32 code is required for ppb
+  //
+  PciIoDevice->Decodes |= EFI_BRIDGE_MEM32_DECODE_SUPPORTED;
+
+  GetResourcePaddingPpb (PciIoDevice);
+
+  DEBUG_CODE (
+    DumpPpbPaddingResource (PciIoDevice, PciBarTypeUnknown);
+    DumpPciBars (PciIoDevice);
+  );
+
+  return PciIoDevice;
+}
+
+
+/**
+  Create PCI device instance for PCI Card bridge device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherP2CInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE                   *PciIoDevice;
+
+  PciIoDevice = CreatePciIoDevice (
+                  Bridge,
+                  Pci,
+                  Bus,
+                  Device,
+                  Func
+                  );
+
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  if (gFullEnumeration) {
+    PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_BITS_OWNED);
+
+    //
+    // Initalize the bridge control register
+    //
+    PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCCARD_BRIDGE_CONTROL_BITS_OWNED);
+  }
+
+  //
+  // P2C only has one bar that is in 0x10
+  //
+  PciParseBar (PciIoDevice, 0x10, P2C_BAR_0);
+
+  //
+  // Read PciBar information from the bar register
+  //
+  GetBackPcCardBar (PciIoDevice);
+  PciIoDevice->Decodes = EFI_BRIDGE_MEM32_DECODE_SUPPORTED  |
+                         EFI_BRIDGE_PMEM32_DECODE_SUPPORTED |
+                         EFI_BRIDGE_IO32_DECODE_SUPPORTED;
+
+  DEBUG_CODE (DumpPciBars (PciIoDevice););
+
+  return PciIoDevice;
+}
+
+/**
+  Create device path for pci deivce.
+
+  @param ParentDevicePath  Parent bridge's path.
+  @param PciIoDevice       Pci device instance.
+
+  @return Device path protocol instance for specific pci device.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CreatePciDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath,
+  IN  PCI_IO_DEVICE            *PciIoDevice
+  )
+{
+
+  PCI_DEVICE_PATH PciNode;
+
+  //
+  // Create PCI device path
+  //
+  PciNode.Header.Type     = HARDWARE_DEVICE_PATH;
+  PciNode.Header.SubType  = HW_PCI_DP;
+  SetDevicePathNodeLength (&PciNode.Header, sizeof (PciNode));
+
+  PciNode.Device          = PciIoDevice->DeviceNumber;
+  PciNode.Function        = PciIoDevice->FunctionNumber;
+  PciIoDevice->DevicePath = AppendDevicePathNode (ParentDevicePath, &PciNode.Header);
+
+  return PciIoDevice->DevicePath;
+}
+
+/**
+  Check whether the PCI IOV VF bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+VfBarExisted (
+  IN PCI_IO_DEVICE *PciIoDevice,
+  IN UINTN         Offset,
+  OUT UINT32       *BarLengthValue,
+  OUT UINT32       *OriginalBarValue
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT32              OriginalValue;
+  UINT32              Value;
+  EFI_TPL             OldTpl;
+
+  //
+  // Ensure it is called properly
+  //
+  ASSERT (PciIoDevice->SrIovCapabilityOffset != 0);
+  if (PciIoDevice->SrIovCapabilityOffset == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Preserve the original value
+  //
+
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &OriginalValue);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &Value);
+
+  //
+  // Write back the original value
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT32)Offset, 1, &OriginalValue);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (BarLengthValue != NULL) {
+    *BarLengthValue = Value;
+  }
+
+  if (OriginalBarValue != NULL) {
+    *OriginalBarValue = OriginalValue;
+  }
+
+  if (Value == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Check whether the bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+BarExisted (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINTN         Offset,
+  OUT UINT32        *BarLengthValue,
+  OUT UINT32        *OriginalBarValue
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT32              OriginalValue;
+  UINT32              Value;
+  EFI_TPL             OldTpl;
+
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Preserve the original value
+  //
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &OriginalValue);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &gAllOne);
+  PciIo->Pci.Read (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &Value);
+
+  //
+  // Write back the original value
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, (UINT8) Offset, 1, &OriginalValue);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (BarLengthValue != NULL) {
+    *BarLengthValue = Value;
+  }
+
+  if (OriginalBarValue != NULL) {
+    *OriginalBarValue = OriginalValue;
+  }
+
+  if (Value == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Test whether the device can support given attributes.
+
+  @param PciIoDevice      Pci device instance.
+  @param Command          Input command register value, and
+                          returned supported register value.
+  @param BridgeControl    Inout bridge control value for PPB or P2C, and
+                          returned supported bridge control value.
+  @param OldCommand       Returned and stored old command register offset.
+  @param OldBridgeControl Returned and stored old Bridge control value for PPB or P2C.
+
+**/
+VOID
+PciTestSupportedAttribute (
+  IN     PCI_IO_DEVICE                      *PciIoDevice,
+  IN OUT UINT16                             *Command,
+  IN OUT UINT16                             *BridgeControl,
+     OUT UINT16                             *OldCommand,
+     OUT UINT16                             *OldBridgeControl
+  )
+{
+  EFI_TPL OldTpl;
+
+  //
+  // Preserve the original value
+  //
+  PCI_READ_COMMAND_REGISTER (PciIoDevice, OldCommand);
+
+  //
+  // Raise TPL to high level to disable timer interrupt while the BAR is probed
+  //
+  OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  PCI_SET_COMMAND_REGISTER (PciIoDevice, *Command);
+  PCI_READ_COMMAND_REGISTER (PciIoDevice, Command);
+
+  //
+  // Write back the original value
+  //
+  PCI_SET_COMMAND_REGISTER (PciIoDevice, *OldCommand);
+
+  //
+  // Restore TPL to its original level
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+    //
+    // Preserve the original value
+    //
+    PCI_READ_BRIDGE_CONTROL_REGISTER (PciIoDevice, OldBridgeControl);
+
+    //
+    // Raise TPL to high level to disable timer interrupt while the BAR is probed
+    //
+    OldTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+    PCI_SET_BRIDGE_CONTROL_REGISTER (PciIoDevice, *BridgeControl);
+    PCI_READ_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+
+    //
+    // Write back the original value
+    //
+    PCI_SET_BRIDGE_CONTROL_REGISTER (PciIoDevice, *OldBridgeControl);
+
+    //
+    // Restore TPL to its original level
+    //
+    gBS->RestoreTPL (OldTpl);
+
+  } else {
+    *OldBridgeControl = 0;
+    *BridgeControl    = 0;
+  }
+}
+
+/**
+  Set the supported or current attributes of a PCI device.
+
+  @param PciIoDevice    Structure pointer for PCI device.
+  @param Command        Command register value.
+  @param BridgeControl  Bridge control value for PPB or P2C.
+  @param Option         Make a choice of EFI_SET_SUPPORTS or EFI_SET_ATTRIBUTES.
+
+**/
+VOID
+PciSetDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT16                             Command,
+  IN UINT16                             BridgeControl,
+  IN UINTN                              Option
+  )
+{
+  UINT64  Attributes;
+
+  Attributes = 0;
+
+  if ((Command & EFI_PCI_COMMAND_IO_SPACE) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_IO;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_MEMORY_SPACE) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_MEMORY;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_BUS_MASTER) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_BUS_MASTER;
+  }
+
+  if ((Command & EFI_PCI_COMMAND_VGA_PALETTE_SNOOP) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_ISA) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_ISA_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_VGA) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_IO;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+  }
+
+  if ((BridgeControl & EFI_PCI_BRIDGE_CONTROL_VGA_16) != 0) {
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_IO_16;
+    Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16;
+  }
+
+  if (Option == EFI_SET_SUPPORTS) {
+
+    Attributes |= (UINT64) (EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE |
+                  EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED        |
+                  EFI_PCI_IO_ATTRIBUTE_MEMORY_DISABLE       |
+                  EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE      |
+                  EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM         |
+                  EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+    if (IS_PCI_LPC (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_ISA_MOTHERBOARD_IO;
+        Attributes |= (mReserveIsaAliases ? (UINT64) EFI_PCI_IO_ATTRIBUTE_ISA_IO : \
+                                            (UINT64) EFI_PCI_IO_ATTRIBUTE_ISA_IO_16);
+    }
+
+    if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+      //
+      // For bridge, it should support IDE attributes
+      //
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_SECONDARY_IO;
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_PRIMARY_IO;
+
+      if (mReserveVgaAliases) {
+        Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 | \
+                                EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16);
+      } else {
+        Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_VGA_IO | \
+                                EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO);
+      }
+    } else {
+
+      if (IS_PCI_IDE (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_SECONDARY_IO;
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_IDE_PRIMARY_IO;
+      }
+
+      if (IS_PCI_VGA (&PciIoDevice->Pci)) {
+        Attributes |= EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY;
+        Attributes |= (mReserveVgaAliases ? (UINT64) EFI_PCI_IO_ATTRIBUTE_VGA_IO : \
+                                            (UINT64) EFI_PCI_IO_ATTRIBUTE_VGA_IO_16);
+      }
+    }
+
+    PciIoDevice->Supports = Attributes;
+    PciIoDevice->Supports &= ( (PciIoDevice->Parent->Supports) | \
+                               EFI_PCI_IO_ATTRIBUTE_IO | EFI_PCI_IO_ATTRIBUTE_MEMORY | \
+                               EFI_PCI_IO_ATTRIBUTE_BUS_MASTER );
+
+  } else {
+    //
+    // When this attribute is clear, the RomImage and RomSize fields in the PCI IO were
+    // initialized based on the PCI option ROM found through the ROM BAR of the PCI controller.
+    // When this attribute is set, the PCI option ROM described by the RomImage and RomSize
+    // fields is not from the the ROM BAR of the PCI controller.
+    //
+    if (!PciIoDevice->EmbeddedRom) {
+      Attributes |= EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM;
+    }
+    PciIoDevice->Attributes = Attributes;
+  }
+}
+
+/**
+  Determine if the device can support Fast Back to Back attribute.
+
+  @param PciIoDevice  Pci device instance.
+  @param StatusIndex  Status register value.
+
+  @retval EFI_SUCCESS       This device support Fast Back to Back attribute.
+  @retval EFI_UNSUPPORTED   This device doesn't support Fast Back to Back attribute.
+
+**/
+EFI_STATUS
+GetFastBackToBackSupport (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT8                              StatusIndex
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  EFI_STATUS          Status;
+  UINT32              StatusRegister;
+
+  //
+  // Read the status register
+  //
+  PciIo   = &PciIoDevice->PciIo;
+  Status  = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint16, StatusIndex, 1, &StatusRegister);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check the Fast B2B bit
+  //
+  if ((StatusRegister & EFI_PCI_FAST_BACK_TO_BACK_CAPABLE) != 0) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Process the option ROM for all the children of the specified parent PCI device.
+  It can only be used after the first full Option ROM process.
+
+  @param PciIoDevice Pci device instance.
+
+**/
+VOID
+ProcessOptionRomLight (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+
+  //
+  // For RootBridge, PPB , P2C, go recursively to traverse all its children
+  //
+  CurrentLink = PciIoDevice->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      ProcessOptionRomLight (Temp);
+    }
+
+    PciRomGetImageMapping (Temp);
+
+    //
+    // The OpRom has already been processed in the first round
+    //
+    Temp->AllOpRomProcessed = TRUE;
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  Determine the related attributes of all devices under a Root Bridge.
+
+  @param PciIoDevice   PCI device instance.
+
+**/
+EFI_STATUS
+DetermineDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+  UINT16          Command;
+  UINT16          BridgeControl;
+  UINT16          OldCommand;
+  UINT16          OldBridgeControl;
+  BOOLEAN         FastB2BSupport;
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+  EFI_STATUS      Status;
+
+  //
+  // For Root Bridge, just copy it by RootBridgeIo proctocol
+  // so as to keep consistent with the actual attribute
+  //
+  if (PciIoDevice->Parent == NULL) {
+    Status = PciIoDevice->PciRootBridgeIo->GetAttributes (
+                                            PciIoDevice->PciRootBridgeIo,
+                                            &PciIoDevice->Supports,
+                                            &PciIoDevice->Attributes
+                                            );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Assume the PCI Root Bridge supports DAC
+    //
+    PciIoDevice->Supports |= (UINT64)(EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE |
+                              EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM |
+                              EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+  } else {
+
+    //
+    // Set the attributes to be checked for common PCI devices and PPB or P2C
+    // Since some devices only support part of them, it is better to set the
+    // attribute according to its command or bridge control register
+    //
+    Command = EFI_PCI_COMMAND_IO_SPACE     |
+              EFI_PCI_COMMAND_MEMORY_SPACE |
+              EFI_PCI_COMMAND_BUS_MASTER   |
+              EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+
+    BridgeControl = EFI_PCI_BRIDGE_CONTROL_ISA | EFI_PCI_BRIDGE_CONTROL_VGA | EFI_PCI_BRIDGE_CONTROL_VGA_16;
+
+    //
+    // Test whether the device can support attributes above
+    //
+    PciTestSupportedAttribute (PciIoDevice, &Command, &BridgeControl, &OldCommand, &OldBridgeControl);
+
+    //
+    // Set the supported attributes for specified PCI device
+    //
+    PciSetDeviceAttribute (PciIoDevice, Command, BridgeControl, EFI_SET_SUPPORTS);
+
+    //
+    // Set the current attributes for specified PCI device
+    //
+    PciSetDeviceAttribute (PciIoDevice, OldCommand, OldBridgeControl, EFI_SET_ATTRIBUTES);
+
+    //
+    // Enable other supported attributes but not defined in PCI_IO_PROTOCOL
+    //
+    PCI_ENABLE_COMMAND_REGISTER (PciIoDevice, EFI_PCI_COMMAND_MEMORY_WRITE_AND_INVALIDATE);
+  }
+
+  FastB2BSupport = TRUE;
+
+  //
+  // P2C can not support FB2B on the secondary side
+  //
+  if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+    FastB2BSupport = FALSE;
+  }
+
+  //
+  // For RootBridge, PPB , P2C, go recursively to traverse all its children
+  //
+  CurrentLink = PciIoDevice->ChildList.ForwardLink;
+  while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+
+    Temp    = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+    Status  = DetermineDeviceAttribute (Temp);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Detect Fast Bact to Bact support for the device under the bridge
+    //
+    Status = GetFastBackToBackSupport (Temp, PCI_PRIMARY_STATUS_OFFSET);
+    if (FastB2BSupport && EFI_ERROR (Status)) {
+      FastB2BSupport = FALSE;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+  //
+  // Set or clear Fast Back to Back bit for the whole bridge
+  //
+  if (!IsListEmpty (&PciIoDevice->ChildList)) {
+
+    if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+
+      Status = GetFastBackToBackSupport (PciIoDevice, PCI_BRIDGE_STATUS_REGISTER_OFFSET);
+
+      if (EFI_ERROR (Status) || (!FastB2BSupport)) {
+        FastB2BSupport = FALSE;
+        PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK);
+      } else {
+        PCI_ENABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, EFI_PCI_BRIDGE_CONTROL_FAST_BACK_TO_BACK);
+      }
+    }
+
+    CurrentLink = PciIoDevice->ChildList.ForwardLink;
+    while (CurrentLink != NULL && CurrentLink != &PciIoDevice->ChildList) {
+      Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+      if (FastB2BSupport) {
+        PCI_ENABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_FAST_BACK_TO_BACK);
+      } else {
+        PCI_DISABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_FAST_BACK_TO_BACK);
+      }
+
+      CurrentLink = CurrentLink->ForwardLink;
+    }
+  }
+  //
+  // End for IsListEmpty
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine is used to update the bar information for those incompatible PCI device.
+
+  @param PciIoDevice      Input Pci device instance. Output Pci device instance with updated
+                          Bar information.
+
+  @retval EFI_SUCCESS     Successfully updated bar information.
+  @retval EFI_UNSUPPORTED Given PCI device doesn't belong to incompatible PCI device list.
+
+**/
+EFI_STATUS
+UpdatePciInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             BarIndex;
+  UINTN                             BarEndIndex;
+  BOOLEAN                           SetFlag;
+  VOID                              *Configuration;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+
+  Configuration = NULL;
+  Status        = EFI_SUCCESS;
+
+  if (gEfiIncompatiblePciDeviceSupport == NULL) {
+    //
+    // It can only be supported after the Incompatible PCI Device
+    // Support Protocol has been installed
+    //
+    Status = gBS->LocateProtocol (
+                    &gEfiIncompatiblePciDeviceSupportProtocolGuid,
+                    NULL,
+                    (VOID **) &gEfiIncompatiblePciDeviceSupport
+                    );
+  }
+  if (Status == EFI_SUCCESS) {
+      //
+      // Check whether the device belongs to incompatible devices from protocol or not
+      // If it is , then get its special requirement in the ACPI table
+      //
+      Status = gEfiIncompatiblePciDeviceSupport->CheckDevice (
+                                                   gEfiIncompatiblePciDeviceSupport,
+                                                   PciIoDevice->Pci.Hdr.VendorId,
+                                                   PciIoDevice->Pci.Hdr.DeviceId,
+                                                   PciIoDevice->Pci.Hdr.RevisionID,
+                                                   PciIoDevice->Pci.Device.SubsystemVendorID,
+                                                   PciIoDevice->Pci.Device.SubsystemID,
+                                                   &Configuration
+                                                   );
+
+  }
+
+  if (EFI_ERROR (Status) || Configuration == NULL ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Update PCI device information from the ACPI table
+  //
+  Ptr = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+  while (Ptr->Desc != ACPI_END_TAG_DESCRIPTOR) {
+
+    if (Ptr->Desc != ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+      //
+      // The format is not support
+      //
+      break;
+    }
+
+    BarIndex    = (UINTN) Ptr->AddrTranslationOffset;
+    BarEndIndex = BarIndex;
+
+    //
+    // Update all the bars in the device
+    //
+    if (BarIndex == PCI_BAR_ALL) {
+      BarIndex    = 0;
+      BarEndIndex = PCI_MAX_BAR - 1;
+    }
+
+    if (BarIndex > PCI_MAX_BAR) {
+      Ptr++;
+      continue;
+    }
+
+    for (; BarIndex <= BarEndIndex; BarIndex++) {
+      SetFlag = FALSE;
+      switch (Ptr->ResType) {
+      case ACPI_ADDRESS_SPACE_TYPE_MEM:
+
+        //
+        // Make sure the bar is memory type
+        //
+        if (CheckBarType (PciIoDevice, (UINT8) BarIndex, PciBarTypeMem)) {
+          SetFlag = TRUE;
+        }
+        break;
+
+      case ACPI_ADDRESS_SPACE_TYPE_IO:
+
+        //
+        // Make sure the bar is IO type
+        //
+        if (CheckBarType (PciIoDevice, (UINT8) BarIndex, PciBarTypeIo)) {
+          SetFlag = TRUE;
+        }
+        break;
+      }
+
+      if (SetFlag) {
+
+        //
+        // Update the new alignment for the device
+        //
+        SetNewAlign (&(PciIoDevice->PciBar[BarIndex].Alignment), Ptr->AddrRangeMax);
+
+        //
+        // Update the new length for the device
+        //
+        if (Ptr->AddrLen != PCI_BAR_NOCHANGE) {
+          PciIoDevice->PciBar[BarIndex].Length = Ptr->AddrLen;
+        }
+      }
+    }
+
+    Ptr++;
+  }
+
+  FreePool (Configuration);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine will update the alignment with the new alignment.
+
+  @param Alignment    Input Old alignment. Output updated alignment.
+  @param NewAlignment New alignment.
+
+**/
+VOID
+SetNewAlign (
+  IN OUT UINT64     *Alignment,
+  IN     UINT64     NewAlignment
+  )
+{
+  UINT64  OldAlignment;
+  UINTN   ShiftBit;
+
+  //
+  // The new alignment is the same as the original,
+  // so skip it
+  //
+  if (NewAlignment == PCI_BAR_OLD_ALIGN) {
+    return ;
+  }
+  //
+  // Check the validity of the parameter
+  //
+   if (NewAlignment != PCI_BAR_EVEN_ALIGN  &&
+       NewAlignment != PCI_BAR_SQUAD_ALIGN &&
+       NewAlignment != PCI_BAR_DQUAD_ALIGN ) {
+    *Alignment = NewAlignment;
+    return ;
+  }
+
+  OldAlignment  = (*Alignment) + 1;
+  ShiftBit      = 0;
+
+  //
+  // Get the first non-zero hex value of the length
+  //
+  while ((OldAlignment & 0x0F) == 0x00) {
+    OldAlignment = RShiftU64 (OldAlignment, 4);
+    ShiftBit += 4;
+  }
+
+  //
+  // Adjust the alignment to even, quad or double quad boundary
+  //
+  if (NewAlignment == PCI_BAR_EVEN_ALIGN) {
+    if ((OldAlignment & 0x01) != 0) {
+      OldAlignment = OldAlignment + 2 - (OldAlignment & 0x01);
+    }
+  } else if (NewAlignment == PCI_BAR_SQUAD_ALIGN) {
+    if ((OldAlignment & 0x03) != 0) {
+      OldAlignment = OldAlignment + 4 - (OldAlignment & 0x03);
+    }
+  } else if (NewAlignment == PCI_BAR_DQUAD_ALIGN) {
+    if ((OldAlignment & 0x07) != 0) {
+      OldAlignment = OldAlignment + 8 - (OldAlignment & 0x07);
+    }
+  }
+
+  //
+  // Update the old value
+  //
+  NewAlignment  = LShiftU64 (OldAlignment, ShiftBit) - 1;
+  *Alignment    = NewAlignment;
+
+  return ;
+}
+
+/**
+  Parse PCI IOV VF bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciIovParseVfBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  )
+{
+  UINT32      Value;
+  UINT32      OriginalValue;
+  UINT32      Mask;
+  EFI_STATUS  Status;
+
+  //
+  // Ensure it is called properly
+  //
+  ASSERT (PciIoDevice->SrIovCapabilityOffset != 0);
+  if (PciIoDevice->SrIovCapabilityOffset == 0) {
+    return 0;
+  }
+
+  OriginalValue = 0;
+  Value         = 0;
+
+  Status = VfBarExisted (
+            PciIoDevice,
+            Offset,
+            &Value,
+            &OriginalValue
+            );
+
+  if (EFI_ERROR (Status)) {
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->VfPciBar[BarIndex].Length      = 0;
+    PciIoDevice->VfPciBar[BarIndex].Alignment   = 0;
+
+    //
+    // Scan all the BARs anyway
+    //
+    PciIoDevice->VfPciBar[BarIndex].Offset = (UINT16) Offset;
+    return Offset + 4;
+  }
+
+  PciIoDevice->VfPciBar[BarIndex].Offset = (UINT16) Offset;
+  if ((Value & 0x01) != 0) {
+    //
+    // Device I/Os. Impossible
+    //
+    ASSERT (FALSE);
+    return Offset + 4;
+
+  } else {
+
+    Mask  = 0xfffffff0;
+
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = OriginalValue & Mask;
+
+    switch (Value & 0x07) {
+
+    //
+    //memory space; anywhere in 32 bit address space
+    //
+    case 0x00:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypePMem32;
+      } else {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypeMem32;
+      }
+
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      //
+      // Adjust Length
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length = MultU64x32 (PciIoDevice->VfPciBar[BarIndex].Length, PciIoDevice->InitialVFs);
+      //
+      // Adjust Alignment
+      //
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+
+    //
+    // memory space; anywhere in 64 bit address space
+    //
+    case 0x04:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypePMem64;
+      } else {
+        PciIoDevice->VfPciBar[BarIndex].BarType = PciBarTypeMem64;
+      }
+
+      //
+      // According to PCI 2.2,if the bar indicates a memory 64 decoding, next bar
+      // is regarded as an extension for the first bar. As a result
+      // the sizing will be conducted on combined 64 bit value
+      // Here just store the masked first 32bit value for future size
+      // calculation
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length    = Value & Mask;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      //
+      // Increment the offset to point to next DWORD
+      //
+      Offset += 4;
+
+      Status = VfBarExisted (
+                PciIoDevice,
+                Offset,
+                &Value,
+                &OriginalValue
+                );
+
+      if (EFI_ERROR (Status)) {
+        return Offset + 4;
+      }
+
+      //
+      // Fix the length to support some spefic 64 bit BAR
+      //
+      Value |= ((UINT32) -1 << HighBitSet32 (Value));
+
+      //
+      // Calculate the size of 64bit bar
+      //
+      PciIoDevice->VfPciBar[BarIndex].BaseAddress |= LShiftU64 ((UINT64) OriginalValue, 32);
+
+      PciIoDevice->VfPciBar[BarIndex].Length    = PciIoDevice->VfPciBar[BarIndex].Length | LShiftU64 ((UINT64) Value, 32);
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(PciIoDevice->VfPciBar[BarIndex].Length)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      //
+      // Adjust Length
+      //
+      PciIoDevice->VfPciBar[BarIndex].Length = MultU64x32 (PciIoDevice->VfPciBar[BarIndex].Length, PciIoDevice->InitialVFs);
+      //
+      // Adjust Alignment
+      //
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+
+    //
+    // reserved
+    //
+    default:
+      PciIoDevice->VfPciBar[BarIndex].BarType   = PciBarTypeUnknown;
+      PciIoDevice->VfPciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->VfPciBar[BarIndex].Length - 1;
+
+      if (PciIoDevice->VfPciBar[BarIndex].Alignment < PciIoDevice->SystemPageSize - 1) {
+        PciIoDevice->VfPciBar[BarIndex].Alignment = PciIoDevice->SystemPageSize - 1;
+      }
+
+      break;
+    }
+  }
+  
+  //
+  // Check the length again so as to keep compatible with some special bars
+  //
+  if (PciIoDevice->VfPciBar[BarIndex].Length == 0) {
+    PciIoDevice->VfPciBar[BarIndex].BarType     = PciBarTypeUnknown;
+    PciIoDevice->VfPciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->VfPciBar[BarIndex].Alignment   = 0;
+  }
+  
+  //
+  // Increment number of bar
+  //
+  return Offset + 4;
+}
+
+/**
+  Parse PCI bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciParseBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  )
+{
+  UINT32      Value;
+  UINT32      OriginalValue;
+  UINT32      Mask;
+  EFI_STATUS  Status;
+
+  OriginalValue = 0;
+  Value         = 0;
+
+  Status = BarExisted (
+             PciIoDevice,
+             Offset,
+             &Value,
+             &OriginalValue
+             );
+
+  if (EFI_ERROR (Status)) {
+    PciIoDevice->PciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->PciBar[BarIndex].Length      = 0;
+    PciIoDevice->PciBar[BarIndex].Alignment   = 0;
+
+    //
+    // Some devices don't fully comply to PCI spec 2.2. So be to scan all the BARs anyway
+    //
+    PciIoDevice->PciBar[BarIndex].Offset = (UINT8) Offset;
+    return Offset + 4;
+  }
+
+  PciIoDevice->PciBar[BarIndex].Offset = (UINT8) Offset;
+  if ((Value & 0x01) != 0) {
+    //
+    // Device I/Os
+    //
+    Mask = 0xfffffffc;
+
+    if ((Value & 0xFFFF0000) != 0) {
+      //
+      // It is a IO32 bar
+      //
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeIo32;
+      PciIoDevice->PciBar[BarIndex].Length    = ((~(Value & Mask)) + 1);
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+    } else {
+      //
+      // It is a IO16 bar
+      //
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeIo16;
+      PciIoDevice->PciBar[BarIndex].Length    = 0x0000FFFF & ((~(Value & Mask)) + 1);
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+    }
+    //
+    // Workaround. Some platforms inplement IO bar with 0 length
+    // Need to treat it as no-bar
+    //
+    if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+      PciIoDevice->PciBar[BarIndex].BarType = (PCI_BAR_TYPE) 0;
+    }
+
+    PciIoDevice->PciBar[BarIndex].Prefetchable  = FALSE;
+    PciIoDevice->PciBar[BarIndex].BaseAddress   = OriginalValue & Mask;
+
+  } else {
+
+    Mask  = 0xfffffff0;
+
+    PciIoDevice->PciBar[BarIndex].BaseAddress = OriginalValue & Mask;
+
+    switch (Value & 0x07) {
+
+    //
+    //memory space; anywhere in 32 bit address space
+    //
+    case 0x00:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypePMem32;
+      } else {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeMem32;
+      }
+
+      PciIoDevice->PciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+      break;
+
+    //
+    // memory space; anywhere in 64 bit address space
+    //
+    case 0x04:
+      if ((Value & 0x08) != 0) {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypePMem64;
+      } else {
+        PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeMem64;
+      }
+
+      //
+      // According to PCI 2.2,if the bar indicates a memory 64 decoding, next bar
+      // is regarded as an extension for the first bar. As a result
+      // the sizing will be conducted on combined 64 bit value
+      // Here just store the masked first 32bit value for future size
+      // calculation
+      //
+      PciIoDevice->PciBar[BarIndex].Length    = Value & Mask;
+      PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+
+      //
+      // Increment the offset to point to next DWORD
+      //
+      Offset += 4;
+
+      Status = BarExisted (
+                 PciIoDevice,
+                 Offset,
+                 &Value,
+                 &OriginalValue
+                 );
+
+      if (EFI_ERROR (Status)) {
+        //
+        // the high 32 bit does not claim any BAR, we need to re-check the low 32 bit BAR again
+        //
+        if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+          //
+          // some device implement MMIO bar with 0 length, need to treat it as no-bar
+          //
+          PciIoDevice->PciBar[BarIndex].BarType = PciBarTypeUnknown;
+          return Offset + 4;
+        }
+      }
+
+      //
+      // Fix the length to support some spefic 64 bit BAR
+      //
+      if (Value == 0) {
+        DEBUG ((EFI_D_INFO, "[PciBus]BAR probing for upper 32bit of MEM64 BAR returns 0, change to 0xFFFFFFFF.\n"));
+        Value = (UINT32) -1;
+      } else {
+        Value |= ((UINT32)(-1) << HighBitSet32 (Value));
+      }
+
+      //
+      // Calculate the size of 64bit bar
+      //
+      PciIoDevice->PciBar[BarIndex].BaseAddress |= LShiftU64 ((UINT64) OriginalValue, 32);
+
+      PciIoDevice->PciBar[BarIndex].Length    = PciIoDevice->PciBar[BarIndex].Length | LShiftU64 ((UINT64) Value, 32);
+      PciIoDevice->PciBar[BarIndex].Length    = (~(PciIoDevice->PciBar[BarIndex].Length)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+
+      break;
+
+    //
+    // reserved
+    //
+    default:
+      PciIoDevice->PciBar[BarIndex].BarType   = PciBarTypeUnknown;
+      PciIoDevice->PciBar[BarIndex].Length    = (~(Value & Mask)) + 1;
+      if (PciIoDevice->PciBar[BarIndex].Length < (SIZE_4KB)) {
+        //
+        // Force minimum 4KByte alignment for Virtualization technology for Directed I/O
+        //
+        PciIoDevice->PciBar[BarIndex].Alignment = (SIZE_4KB - 1);
+      } else {
+        PciIoDevice->PciBar[BarIndex].Alignment = PciIoDevice->PciBar[BarIndex].Length - 1;
+      }
+      break;
+    }
+  }
+
+  //
+  // Check the length again so as to keep compatible with some special bars
+  //
+  if (PciIoDevice->PciBar[BarIndex].Length == 0) {
+    PciIoDevice->PciBar[BarIndex].BarType     = PciBarTypeUnknown;
+    PciIoDevice->PciBar[BarIndex].BaseAddress = 0;
+    PciIoDevice->PciBar[BarIndex].Alignment   = 0;
+  }
+
+  //
+  // Increment number of bar
+  //
+  return Offset + 4;
+}
+
+/**
+  This routine is used to initialize the bar of a PCI device.
+
+  @param PciIoDevice Pci device instance.
+
+  @note It can be called typically when a device is going to be rejected.
+
+**/
+VOID
+InitializePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT8               Offset;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures
+  // Resource base is set to all ones so as to indicate its resource
+  // has not been alloacted
+  //
+  for (Offset = 0x10; Offset <= 0x24; Offset += sizeof (UINT32)) {
+    PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, Offset, 1, &gAllOne);
+  }
+}
+
+/**
+  This routine is used to initialize the bar of a PCI-PCI Bridge device.
+
+  @param  PciIoDevice PCI-PCI bridge device instance.
+
+**/
+VOID
+InitializePpb (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures including IO16
+  // Io32, pMem32, pMem64 to quiescent state
+  // Resource base all ones, Resource limit all zeros
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1C, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x1D, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x20, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x22, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x24, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x26, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x28, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x2C, 1, &gAllZero);
+
+  //
+  // Don't support use io32 as for now
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x30, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint16, 0x32, 1, &gAllZero);
+
+  //
+  // Force Interrupt line to zero for cards that come up randomly
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &gAllZero);
+}
+
+/**
+  This routine is used to initialize the bar of a PCI Card Bridge device.
+
+  @param PciIoDevice  PCI Card bridge device.
+
+**/
+VOID
+InitializeP2C (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &(PciIoDevice->PciIo);
+
+  //
+  // Put all the resource apertures including IO16
+  // Io32, pMem32, pMem64 to quiescent state(
+  // Resource base all ones, Resource limit all zeros
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x1c, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x20, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x24, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x28, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x2c, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x30, 1, &gAllZero);
+
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x34, 1, &gAllOne);
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, 0x38, 1, &gAllZero);
+
+  //
+  // Force Interrupt line to zero for cards that come up randomly
+  //
+  PciIo->Pci.Write (PciIo, EfiPciIoWidthUint8, 0x3C, 1, &gAllZero);
+}
+
+/**
+  Create and initiliaze general PCI I/O device instance for
+  PCI device/bridge device/hotplug bridge device.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Input Pci information block.
+  @param Bus               Device Bus NO.
+  @param Device            Device device NO.
+  @param Func              Device func NO.
+
+  @return Instance of PCI device. NULL means no instance created.
+
+**/
+PCI_IO_DEVICE *
+CreatePciIoDevice (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  )
+{
+  PCI_IO_DEVICE        *PciIoDevice;
+  EFI_PCI_IO_PROTOCOL  *PciIo;
+  EFI_STATUS           Status;
+
+  PciIoDevice = AllocateZeroPool (sizeof (PCI_IO_DEVICE));
+  if (PciIoDevice == NULL) {
+    return NULL;
+  }
+
+  PciIoDevice->Signature        = PCI_IO_DEVICE_SIGNATURE;
+  PciIoDevice->Handle           = NULL;
+  PciIoDevice->PciRootBridgeIo  = Bridge->PciRootBridgeIo;
+  PciIoDevice->DevicePath       = NULL;
+  PciIoDevice->BusNumber        = Bus;
+  PciIoDevice->DeviceNumber     = Device;
+  PciIoDevice->FunctionNumber   = Func;
+  PciIoDevice->Decodes          = 0;
+
+  if (gFullEnumeration) {
+    PciIoDevice->Allocated = FALSE;
+  } else {
+    PciIoDevice->Allocated = TRUE;
+  }
+
+  PciIoDevice->Registered         = FALSE;
+  PciIoDevice->Attributes         = 0;
+  PciIoDevice->Supports           = 0;
+  PciIoDevice->BusOverride        = FALSE;
+  PciIoDevice->AllOpRomProcessed  = FALSE;
+
+  PciIoDevice->IsPciExp           = FALSE;
+
+  CopyMem (&(PciIoDevice->Pci), Pci, sizeof (PCI_TYPE01));
+
+  //
+  // Initialize the PCI I/O instance structure
+  //
+  InitializePciIoInstance (PciIoDevice);
+  InitializePciDriverOverrideInstance (PciIoDevice);
+  InitializePciLoadFile2 (PciIoDevice);
+  PciIo = &PciIoDevice->PciIo;
+
+  //
+  // Create a device path for this PCI device and store it into its private data
+  //
+  CreatePciDevicePath (
+    Bridge->DevicePath,
+    PciIoDevice
+    );
+
+  //
+  // Detect if PCI Express Device
+  //
+  PciIoDevice->PciExpressCapabilityOffset = 0;
+  Status = LocateCapabilityRegBlock (
+             PciIoDevice,
+             EFI_PCI_CAPABILITY_ID_PCIEXP,
+             &PciIoDevice->PciExpressCapabilityOffset,
+             NULL
+             );
+  if (!EFI_ERROR (Status)) {
+    PciIoDevice->IsPciExp = TRUE;
+  }
+
+  if (PcdGetBool (PcdAriSupport)) {
+    //
+    // Check if the device is an ARI device.
+    //
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_ARI,
+               &PciIoDevice->AriCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // We need to enable ARI feature before calculate BusReservation,
+      // because FirstVFOffset and VFStride may change after that.
+      //
+      EFI_PCI_IO_PROTOCOL  *ParentPciIo;
+      UINT32               Data32;
+
+      //
+      // Check if its parent supports ARI forwarding.
+      //
+      ParentPciIo = &Bridge->PciIo;
+      ParentPciIo->Pci.Read (
+                          ParentPciIo, 
+                          EfiPciIoWidthUint32,
+                          Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CAPABILITIES_2_OFFSET,
+                          1,
+                          &Data32
+                          );
+      if ((Data32 & EFI_PCIE_CAPABILITY_DEVICE_CAPABILITIES_2_ARI_FORWARDING) != 0) {
+        //
+        // ARI forward support in bridge, so enable it.
+        //
+        ParentPciIo->Pci.Read (
+                            ParentPciIo,
+                            EfiPciIoWidthUint32,
+                            Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_OFFSET,
+                            1,
+                            &Data32
+                            );
+        if ((Data32 & EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_ARI_FORWARDING) == 0) {
+          Data32 |= EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_ARI_FORWARDING;
+          ParentPciIo->Pci.Write (
+                              ParentPciIo,
+                              EfiPciIoWidthUint32,
+                              Bridge->PciExpressCapabilityOffset + EFI_PCIE_CAPABILITY_DEVICE_CONTROL_2_OFFSET,
+                              1,
+                              &Data32
+                              );
+          DEBUG ((
+            EFI_D_INFO,
+            " ARI: forwarding enabled for PPB[%02x:%02x:%02x]\n",
+            Bridge->BusNumber,
+            Bridge->DeviceNumber,
+            Bridge->FunctionNumber
+            ));
+        }
+      }
+
+      DEBUG ((EFI_D_INFO, " ARI: CapOffset = 0x%x\n", PciIoDevice->AriCapabilityOffset));
+    }
+  }
+
+  //
+  // Initialization for SR-IOV
+  //
+
+  if (PcdGetBool (PcdSrIovSupport)) {
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_SRIOV,
+               &PciIoDevice->SrIovCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      UINT32    SupportedPageSize;
+      UINT16    VFStride;
+      UINT16    FirstVFOffset;
+      UINT16    Data16;
+      UINT32    PFRid;
+      UINT32    LastVF;
+
+      //
+      // If the SR-IOV device is an ARI device, then Set ARI Capable Hierarchy for the device.
+      //
+      if (PcdGetBool (PcdAriSupport) && PciIoDevice->AriCapabilityOffset != 0) {
+        PciIo->Pci.Read (
+                     PciIo,
+                     EfiPciIoWidthUint16,
+                     PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL,
+                     1,
+                     &Data16
+                     );
+        Data16 |= EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL_ARI_HIERARCHY;
+        PciIo->Pci.Write (
+                     PciIo,
+                     EfiPciIoWidthUint16,
+                     PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_CONTROL,
+                     1,
+                     &Data16
+                     );
+      }
+
+      //
+      // Calculate SystemPageSize
+      //
+
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint32,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_SUPPORTED_PAGE_SIZE,
+                   1,
+                   &SupportedPageSize
+                   );
+      PciIoDevice->SystemPageSize = (PcdGet32 (PcdSrIovSystemPageSize) & SupportedPageSize);
+      ASSERT (PciIoDevice->SystemPageSize != 0);
+
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint32,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_SYSTEM_PAGE_SIZE,
+                   1,
+                   &PciIoDevice->SystemPageSize
+                   );
+      //
+      // Adjust SystemPageSize for Alignment usage later
+      //
+      PciIoDevice->SystemPageSize <<= 12;
+
+      //
+      // Calculate BusReservation for PCI IOV
+      //
+
+      //
+      // Read First FirstVFOffset, InitialVFs, and VFStride
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_FIRSTVF,
+                   1,
+                   &FirstVFOffset
+                   );
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_INITIALVFS,
+                   1,
+                   &PciIoDevice->InitialVFs
+                   );
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PciIoDevice->SrIovCapabilityOffset + EFI_PCIE_CAPABILITY_ID_SRIOV_VFSTRIDE,
+                   1,
+                   &VFStride
+                   );
+      //
+      // Calculate LastVF
+      //
+      PFRid = EFI_PCI_RID(Bus, Device, Func);
+      LastVF = PFRid + FirstVFOffset + (PciIoDevice->InitialVFs - 1) * VFStride;
+
+      //
+      // Calculate ReservedBusNum for this PF
+      //
+      PciIoDevice->ReservedBusNum = (UINT16)(EFI_PCI_BUS_OF_RID (LastVF) - Bus + 1);
+
+      DEBUG ((
+        EFI_D_INFO,
+        " SR-IOV: SupportedPageSize = 0x%x; SystemPageSize = 0x%x; FirstVFOffset = 0x%x;\n",
+        SupportedPageSize, PciIoDevice->SystemPageSize >> 12, FirstVFOffset
+        ));
+      DEBUG ((
+        EFI_D_INFO,
+        "         InitialVFs = 0x%x; ReservedBusNum = 0x%x; CapOffset = 0x%x\n",
+        PciIoDevice->InitialVFs, PciIoDevice->ReservedBusNum, PciIoDevice->SrIovCapabilityOffset
+        ));
+    }
+  }
+
+  if (PcdGetBool (PcdMrIovSupport)) {
+    Status = LocatePciExpressCapabilityRegBlock (
+               PciIoDevice,
+               EFI_PCIE_CAPABILITY_ID_MRIOV,
+               &PciIoDevice->MrIovCapabilityOffset,
+               NULL
+               );
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, " MR-IOV: CapOffset = 0x%x\n", PciIoDevice->MrIovCapabilityOffset));
+    }
+  }
+
+  //
+  // Initialize the reserved resource list
+  //
+  InitializeListHead (&PciIoDevice->ReservedResourceList);
+
+  //
+  // Initialize the driver list
+  //
+  InitializeListHead (&PciIoDevice->OptionRomDriverList);
+
+  //
+  // Initialize the child list
+  //
+  InitializeListHead (&PciIoDevice->ChildList);
+
+  return PciIoDevice;
+}
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  It is only called on the second start on the same Root Bridge.
+
+  @param  Controller     Parent bridge handler.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumeratorLight (
+  IN EFI_HANDLE                    Controller
+  )
+{
+
+  EFI_STATUS                        Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  PCI_IO_DEVICE                     *RootBridgeDev;
+  UINT16                            MinBus;
+  UINT16                            MaxBus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+
+  MinBus      = 0;
+  MaxBus      = PCI_MAX_BUS;
+  Descriptors = NULL;
+
+  //
+  // If this root bridge has been already enumerated, then return successfully
+  //
+  if (GetRootBridgeByHandle (Controller) != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Open pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  Status = PciRootBridgeIo->Configuration (PciRootBridgeIo, (VOID **) &Descriptors);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (PciGetBusRange (&Descriptors, &MinBus, &MaxBus, NULL) == EFI_SUCCESS) {
+
+    //
+    // Create a device node for root bridge device with a NULL host bridge controller handle
+    //
+    RootBridgeDev = CreateRootBridge (Controller);
+
+    if (RootBridgeDev == NULL) {
+      Descriptors++;
+      continue;
+    }
+
+    //
+    // Record the root bridgeio protocol
+    //
+    RootBridgeDev->PciRootBridgeIo = PciRootBridgeIo;
+
+    Status = PciPciDeviceInfoCollector (
+               RootBridgeDev,
+               (UINT8) MinBus
+               );
+
+    if (!EFI_ERROR (Status)) {
+
+      //
+      // Remove those PCI devices which are rejected when full enumeration
+      //
+      RemoveRejectedPciDevices (RootBridgeDev->Handle, RootBridgeDev);
+
+      //
+      // Process option rom light
+      //
+      ProcessOptionRomLight (RootBridgeDev);
+
+      //
+      // Determine attributes for all devices under this root bridge
+      //
+      DetermineDeviceAttribute (RootBridgeDev);
+
+      //
+      // If successfully, insert the node into device pool
+      //
+      InsertRootBridge (RootBridgeDev);
+    } else {
+
+      //
+      // If unsuccessly, destroy the entire node
+      //
+      DestroyRootBridge (RootBridgeDev);
+    }
+
+    Descriptors++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get bus range from PCI resource descriptor list.
+
+  @param Descriptors  A pointer to the address space descriptor.
+  @param MinBus       The min bus returned.
+  @param MaxBus       The max bus returned.
+  @param BusRange     The bus range returned.
+
+  @retval EFI_SUCCESS    Successfully got bus range.
+  @retval EFI_NOT_FOUND  Can not find the specific bus.
+
+**/
+EFI_STATUS
+PciGetBusRange (
+  IN     EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  **Descriptors,
+  OUT    UINT16                             *MinBus,
+  OUT    UINT16                             *MaxBus,
+  OUT    UINT16                             *BusRange
+  )
+{
+  while ((*Descriptors)->Desc != ACPI_END_TAG_DESCRIPTOR) {
+    if ((*Descriptors)->ResType == ACPI_ADDRESS_SPACE_TYPE_BUS) {
+      if (MinBus != NULL) {
+        *MinBus = (UINT16) (*Descriptors)->AddrRangeMin;
+      }
+
+      if (MaxBus != NULL) {
+        *MaxBus = (UINT16) (*Descriptors)->AddrRangeMax;
+      }
+
+      if (BusRange != NULL) {
+        *BusRange = (UINT16) (*Descriptors)->AddrLen;
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    (*Descriptors)++;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This routine can be used to start the root bridge.
+
+  @param RootBridgeDev     Pci device instance.
+
+  @retval EFI_SUCCESS      This device started.
+  @retval other            Failed to get PCI Root Bridge I/O protocol.
+
+**/
+EFI_STATUS
+StartManagingRootBridge (
+  IN PCI_IO_DEVICE *RootBridgeDev
+  )
+{
+  EFI_HANDLE                      RootBridgeHandle;
+  EFI_STATUS                      Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  //
+  // Get the root bridge handle
+  //
+  RootBridgeHandle = RootBridgeDev->Handle;
+  PciRootBridgeIo  = NULL;
+
+  //
+  // Get the pci root bridge io protocol
+  //
+  Status = gBS->OpenProtocol (
+                  RootBridgeHandle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  (VOID **) &PciRootBridgeIo,
+                  gPciBusDriverBinding.DriverBindingHandle,
+                  RootBridgeHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  //
+  // Store the PciRootBridgeIo protocol into root bridge private data
+  //
+  RootBridgeDev->PciRootBridgeIo = PciRootBridgeIo;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  This routine can be used to check whether a PCI device should be rejected when light enumeration.
+
+  @param PciIoDevice  Pci device instance.
+
+  @retval TRUE      This device should be rejected.
+  @retval FALSE     This device shouldn't be rejected.
+
+**/
+BOOLEAN
+IsPciDeviceRejected (
+  IN PCI_IO_DEVICE *PciIoDevice
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      TestValue;
+  UINT32      OldValue;
+  UINT32      Mask;
+  UINT8       BarOffset;
+
+  //
+  // PPB should be skip!
+  //
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+    return FALSE;
+  }
+
+  if (IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+    //
+    // Only test base registers for P2C
+    //
+    for (BarOffset = 0x1C; BarOffset <= 0x38; BarOffset += 2 * sizeof (UINT32)) {
+
+      Mask    = (BarOffset < 0x2C) ? 0xFFFFF000 : 0xFFFFFFFC;
+      Status  = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      TestValue = TestValue & Mask;
+      if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+        //
+        // The bar isn't programed, so it should be rejected
+        //
+        return TRUE;
+      }
+    }
+
+    return FALSE;
+  }
+
+  for (BarOffset = 0x14; BarOffset <= 0x24; BarOffset += sizeof (UINT32)) {
+    //
+    // Test PCI devices
+    //
+    Status = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if ((TestValue & 0x01) != 0) {
+
+      //
+      // IO Bar
+      //
+      Mask      = 0xFFFFFFFC;
+      TestValue = TestValue & Mask;
+      if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+        return TRUE;
+      }
+
+    } else {
+
+      //
+      // Mem Bar
+      //
+      Mask      = 0xFFFFFFF0;
+      TestValue = TestValue & Mask;
+
+      if ((TestValue & 0x07) == 0x04) {
+
+        //
+        // Mem64 or PMem64
+        //
+        BarOffset += sizeof (UINT32);
+        if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+
+          //
+          // Test its high 32-Bit BAR
+          //
+          Status = BarExisted (PciIoDevice, BarOffset, &TestValue, &OldValue);
+          if (TestValue == OldValue) {
+            return TRUE;
+          }
+        }
+
+      } else {
+
+        //
+        // Mem32 or PMem32
+        //
+        if ((TestValue != 0) && (TestValue == (OldValue & Mask))) {
+          return TRUE;
+        }
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Reset all bus number from specific bridge.
+
+  @param Bridge           Parent specific bridge.
+  @param StartBusNumber   Start bus number.
+
+**/
+VOID
+ResetAllPpbBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  )
+{
+  EFI_STATUS                      Status;
+  PCI_TYPE00                      Pci;
+  UINT8                           Device;
+  UINT32                          Register;
+  UINT8                           Func;
+  UINT64                          Address;
+  UINT8                           SecondaryBus;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                 PciRootBridgeIo,
+                 &Pci,
+                 StartBusNumber,
+                 Device,
+                 Func
+                 );
+
+      if (!EFI_ERROR (Status) && (IS_PCI_BRIDGE (&Pci))) {
+
+        Register  = 0;
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0x18);
+        Status    = PciRootBridgeIo->Pci.Read (
+                                           PciRootBridgeIo,
+                                           EfiPciWidthUint32,
+                                           Address,
+                                           1,
+                                           &Register
+                                           );
+        SecondaryBus = (UINT8)(Register >> 8);
+
+        if (SecondaryBus != 0) {
+          ResetAllPpbBusNumber (Bridge, SecondaryBus);
+        }
+
+        //
+        // Reset register 18h, 19h, 1Ah on PCI Bridge
+        //
+        Register &= 0xFF000000;
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint32,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h
new file mode 100644
index 0000000000..42306e9a47
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciEnumeratorSupport.h
@@ -0,0 +1,476 @@
+/** @file
+  PCI emumeration support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _EFI_PCI_ENUMERATOR_SUPPORT_H_
+#define _EFI_PCI_ENUMERATOR_SUPPORT_H_
+
+/**
+  This routine is used to check whether the pci device is present.
+
+  @param PciRootBridgeIo   Pointer to instance of EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pci               Output buffer for PCI device configuration space.
+  @param Bus               PCI bus NO.
+  @param Device            PCI device NO.
+  @param Func              PCI Func NO.
+
+  @retval EFI_NOT_FOUND    PCI device not present.
+  @retval EFI_SUCCESS      PCI device is found.
+
+**/
+EFI_STATUS
+PciDevicePresent (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *PciRootBridgeIo,
+  OUT PCI_TYPE00                          *Pci,
+  IN  UINT8                               Bus,
+  IN  UINT8                               Device,
+  IN  UINT8                               Func
+  );
+
+/**
+  Collect all the resource information under this root bridge.
+
+  A database that records all the information about pci device subject to this
+  root bridge will then be created.
+
+  @param Bridge         Parent bridge instance.
+  @param StartBusNumber Bus number of begining.
+
+  @retval EFI_SUCCESS   PCI device is found.
+  @retval other         Some error occurred when reading PCI bridge information.
+
+**/
+EFI_STATUS
+PciPciDeviceInfoCollector (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  );
+
+/**
+  Seach required device and create PCI device instance.
+
+  @param Bridge     Parent bridge instance.
+  @param Pci        Input PCI device information block.
+  @param Bus        PCI bus NO.
+  @param Device     PCI device NO.
+  @param Func       PCI func  NO.
+  @param PciDevice  Output of searched PCI device instance.
+
+  @retval EFI_SUCCESS           Successfully created PCI device instance.
+  @retval EFI_OUT_OF_RESOURCES  Cannot get PCI device information.
+
+**/
+EFI_STATUS
+PciSearchDevice (
+  IN  PCI_IO_DEVICE                         *Bridge,
+  IN  PCI_TYPE00                            *Pci,
+  IN  UINT8                                 Bus,
+  IN  UINT8                                 Device,
+  IN  UINT8                                 Func,
+  OUT PCI_IO_DEVICE                         **PciDevice
+  );
+
+/**
+  Create PCI device instance for PCI device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherDeviceInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create PCI device instance for PCI-PCI bridge.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherPpbInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create PCI device instance for PCI Card bridge device.
+
+  @param Bridge   Parent bridge instance.
+  @param Pci      Input PCI device information block.
+  @param Bus      PCI device Bus NO.
+  @param Device   PCI device Device NO.
+  @param Func     PCI device's func NO.
+
+  @return  Created PCI device instance.
+
+**/
+PCI_IO_DEVICE *
+GatherP2CInfo (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  Create device path for pci deivce.
+
+  @param ParentDevicePath  Parent bridge's path.
+  @param PciIoDevice       Pci device instance.
+
+  @return device path protocol instance for specific pci device.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CreatePciDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath,
+  IN  PCI_IO_DEVICE            *PciIoDevice
+  );
+
+/**
+  Check whether the PCI IOV VF bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+VfBarExisted (
+  IN PCI_IO_DEVICE *PciIoDevice,
+  IN UINTN         Offset,
+  OUT UINT32       *BarLengthValue,
+  OUT UINT32       *OriginalBarValue
+  );
+
+/**
+  Check whether the bar is existed or not.
+
+  @param PciIoDevice       A pointer to the PCI_IO_DEVICE.
+  @param Offset            The offset.
+  @param BarLengthValue    The bar length value returned.
+  @param OriginalBarValue  The original bar value returned.
+
+  @retval EFI_NOT_FOUND    The bar doesn't exist.
+  @retval EFI_SUCCESS      The bar exist.
+
+**/
+EFI_STATUS
+BarExisted (
+  IN  PCI_IO_DEVICE *PciIoDevice,
+  IN  UINTN         Offset,
+  OUT UINT32        *BarLengthValue,
+  OUT UINT32        *OriginalBarValue
+  );
+
+/**
+  Test whether the device can support given attributes.
+
+  @param PciIoDevice      Pci device instance.
+  @param Command          Input command register value, and
+                          returned supported register value.
+  @param BridgeControl    Inout bridge control value for PPB or P2C, and
+                          returned supported bridge control value.
+  @param OldCommand       Returned and stored old command register offset.
+  @param OldBridgeControl Returned and stored old Bridge control value for PPB or P2C.
+
+**/
+VOID
+PciTestSupportedAttribute (
+  IN     PCI_IO_DEVICE                      *PciIoDevice,
+  IN OUT UINT16                             *Command,
+  IN OUT UINT16                             *BridgeControl,
+     OUT UINT16                             *OldCommand,
+     OUT UINT16                             *OldBridgeControl
+  );
+
+/**
+  Set the supported or current attributes of a PCI device.
+
+  @param PciIoDevice    Structure pointer for PCI device.
+  @param Command        Command register value.
+  @param BridgeControl  Bridge control value for PPB or P2C.
+  @param Option         Make a choice of EFI_SET_SUPPORTS or EFI_SET_ATTRIBUTES.
+
+**/
+VOID
+PciSetDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT16                             Command,
+  IN UINT16                             BridgeControl,
+  IN UINTN                              Option
+  );
+
+/**
+  Determine if the device can support Fast Back to Back attribute.
+
+  @param PciIoDevice  Pci device instance.
+  @param StatusIndex  Status register value.
+
+  @retval EFI_SUCCESS       This device support Fast Back to Back attribute.
+  @retval EFI_UNSUPPORTED   This device doesn't support Fast Back to Back attribute.
+
+**/
+EFI_STATUS
+GetFastBackToBackSupport (
+  IN PCI_IO_DEVICE                      *PciIoDevice,
+  IN UINT8                              StatusIndex
+  );
+
+/**
+  Determine the related attributes of all devices under a Root Bridge.
+
+  @param PciIoDevice   PCI device instance.
+
+**/
+EFI_STATUS
+DetermineDeviceAttribute (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  );
+
+/**
+  This routine is used to update the bar information for those incompatible PCI device.
+
+  @param PciIoDevice      Input Pci device instance. Output Pci device instance with updated
+                          Bar information.
+
+  @retval EFI_SUCCESS     Successfully updated bar information.
+  @retval EFI_UNSUPPORTED Given PCI device doesn't belong to incompatible PCI device list.
+
+**/
+EFI_STATUS
+UpdatePciInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine will update the alignment with the new alignment.
+
+  @param Alignment    Input Old alignment. Output updated alignment.
+  @param NewAlignment New alignment.
+
+**/
+VOID
+SetNewAlign (
+  IN OUT UINT64     *Alignment,
+  IN     UINT64     NewAlignment
+  );
+
+/**
+  Parse PCI bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciParseBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  );
+
+/**
+  Parse PCI IOV VF bar information and fill them into PCI device instance.
+
+  @param PciIoDevice  Pci device instance.
+  @param Offset       Bar offset.
+  @param BarIndex     Bar index.
+
+  @return Next bar offset.
+
+**/
+UINTN
+PciIovParseVfBar (
+  IN PCI_IO_DEVICE  *PciIoDevice,
+  IN UINTN          Offset,
+  IN UINTN          BarIndex
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI device.
+
+  @param PciIoDevice Pci device instance.
+
+  @note It can be called typically when a device is going to be rejected.
+
+**/
+VOID
+InitializePciDevice (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI-PCI Bridge device.
+
+  @param  PciIoDevice PCI-PCI bridge device instance.
+
+**/
+VOID
+InitializePpb (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  This routine is used to initialize the bar of a PCI Card Bridge device.
+
+  @param PciIoDevice  PCI Card bridge device.
+
+**/
+VOID
+InitializeP2C (
+  IN PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Create and initiliaze general PCI I/O device instance for
+  PCI device/bridge device/hotplug bridge device.
+
+  @param Bridge            Parent bridge instance.
+  @param Pci               Input Pci information block.
+  @param Bus               Device Bus NO.
+  @param Device            Device device NO.
+  @param Func              Device func NO.
+
+  @return Instance of PCI device. NULL means no instance created.
+
+**/
+PCI_IO_DEVICE *
+CreatePciIoDevice (
+  IN PCI_IO_DEVICE                    *Bridge,
+  IN PCI_TYPE00                       *Pci,
+  IN UINT8                            Bus,
+  IN UINT8                            Device,
+  IN UINT8                            Func
+  );
+
+/**
+  This routine is used to enumerate entire pci bus system
+  in a given platform.
+
+  It is only called on the second start on the same Root Bridge.
+
+  @param  Controller     Parent bridge handler.
+
+  @retval EFI_SUCCESS    PCI enumeration finished successfully.
+  @retval other          Some error occurred when enumerating the pci bus system.
+
+**/
+EFI_STATUS
+PciEnumeratorLight (
+  IN EFI_HANDLE                    Controller
+  );
+
+/**
+  Get bus range from PCI resource descriptor list.
+
+  @param Descriptors  A pointer to the address space descriptor.
+  @param MinBus       The min bus returned.
+  @param MaxBus       The max bus returned.
+  @param BusRange     The bus range returned.
+
+  @retval EFI_SUCCESS    Successfully got bus range.
+  @retval EFI_NOT_FOUND  Can not find the specific bus.
+
+**/
+EFI_STATUS
+PciGetBusRange (
+  IN     EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  **Descriptors,
+  OUT    UINT16                             *MinBus,
+  OUT    UINT16                             *MaxBus,
+  OUT    UINT16                             *BusRange
+  );
+
+/**
+  This routine can be used to start the root bridge.
+
+  @param RootBridgeDev     Pci device instance.
+
+  @retval EFI_SUCCESS      This device started.
+  @retval other            Failed to get PCI Root Bridge I/O protocol.
+
+**/
+EFI_STATUS
+StartManagingRootBridge (
+  IN PCI_IO_DEVICE *RootBridgeDev
+  );
+
+/**
+  This routine can be used to check whether a PCI device should be rejected when light enumeration.
+
+  @param PciIoDevice  Pci device instance.
+
+  @retval TRUE      This device should be rejected.
+  @retval FALSE     This device shouldn't be rejected.
+
+**/
+BOOLEAN
+IsPciDeviceRejected (
+  IN PCI_IO_DEVICE *PciIoDevice
+  );
+
+/**
+  Reset all bus number from specific bridge.
+
+  @param Bridge           Parent specific bridge.
+  @param StartBusNumber   Start bus number.
+
+**/
+VOID
+ResetAllPpbBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber
+  );
+
+/**
+  Dump the PPB padding resource information.
+
+  @param PciIoDevice     PCI IO instance.
+  @param ResourceType    The desired resource type to dump.
+                         PciBarTypeUnknown means to dump all types of resources.
+**/
+VOID
+DumpPpbPaddingResource (
+  IN PCI_IO_DEVICE                    *PciIoDevice,
+  IN PCI_BAR_TYPE                     ResourceType
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c
new file mode 100644
index 0000000000..257874b8b0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.c
@@ -0,0 +1,394 @@
+/** @file
+  PCI Hot Plug support functions implementation for PCI Bus module..
+
+Copyright (c) 2006 - 2010, 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 "PciBus.h"
+
+EFI_PCI_HOT_PLUG_INIT_PROTOCOL  *gPciHotPlugInit = NULL;
+EFI_HPC_LOCATION                *gPciRootHpcPool = NULL;
+UINTN                           gPciRootHpcCount = 0;
+ROOT_HPC_DATA                   *gPciRootHpcData = NULL;
+
+
+/**
+  Event notification function to set Hot Plug controller status.
+
+  @param  Event                    The event that invoke this function.
+  @param  Context                  The calling context, pointer to ROOT_HPC_DATA.
+
+**/
+VOID
+EFIAPI
+PciHPCInitialized (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  ROOT_HPC_DATA   *HpcData;
+
+  HpcData               = (ROOT_HPC_DATA *) Context;
+  HpcData->Initialized  = TRUE;
+}
+
+/**
+  Compare two device pathes to check if they are exactly same.
+
+  @param DevicePath1    A pointer to the first device path data structure.
+  @param DevicePath2    A pointer to the second device path data structure.
+
+  @retval TRUE    They are same.
+  @retval FALSE   They are not same.
+
+**/
+BOOLEAN
+EfiCompareDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath1,
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath2
+  )
+{
+  UINTN Size1;
+  UINTN Size2;
+
+  Size1 = GetDevicePathSize (DevicePath1);
+  Size2 = GetDevicePathSize (DevicePath2);
+
+  if (Size1 != Size2) {
+    return FALSE;
+  }
+
+  if (CompareMem (DevicePath1, DevicePath2, Size1) != 0) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Check hot plug support and initialize root hot plug private data.
+
+  If Hot Plug is supported by the platform, call PCI Hot Plug Init protocol
+  to get PCI Hot Plug controller's information and constructor the root hot plug
+  private data structure.
+
+  @retval EFI_SUCCESS           They are same.
+  @retval EFI_UNSUPPORTED       No PCI Hot Plug controler on the platform.
+  @retval EFI_OUT_OF_RESOURCES  No memory to constructor root hot plug private
+                                data structure.
+
+**/
+EFI_STATUS
+InitializeHotPlugSupport (
+  VOID
+  )
+{
+  EFI_STATUS        Status;
+  EFI_HPC_LOCATION  *HpcList;
+  UINTN             HpcCount;
+
+  //
+  // Locate the PciHotPlugInit Protocol
+  // If it doesn't exist, that means there is no
+  // hot plug controller supported on the platform
+  // the PCI Bus driver is running on. HotPlug Support
+  // is an optional feature, so absence of the protocol
+  // won't incur the penalty.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiPciHotPlugInitProtocolGuid,
+                  NULL,
+                  (VOID **) &gPciHotPlugInit
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gPciHotPlugInit->GetRootHpcList (
+                              gPciHotPlugInit,
+                              &HpcCount,
+                              &HpcList
+                              );
+
+  if (!EFI_ERROR (Status)) {
+
+    gPciRootHpcPool   = HpcList;
+    gPciRootHpcCount  = HpcCount;
+    gPciRootHpcData   = AllocateZeroPool (sizeof (ROOT_HPC_DATA) * gPciRootHpcCount);
+    if (gPciRootHpcData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test whether device path is for root pci hot plug bus.
+
+  @param HpbDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug bus.
+  @retval FALSE         The device path is not for root pci hot plug bus.
+
+**/
+BOOLEAN
+IsRootPciHotPlugBus (
+  IN  EFI_DEVICE_PATH_PROTOCOL        *HpbDevicePath,
+  OUT UINTN                           *HpIndex    OPTIONAL
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+    if (EfiCompareDevicePath (gPciRootHpcPool[Index].HpbDevicePath, HpbDevicePath)) {
+
+      if (HpIndex != NULL) {
+        *HpIndex = Index;
+      }
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Test whether device path is for root pci hot plug controller.
+
+  @param HpcDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug controller.
+  @retval FALSE         The device path is not for root pci hot plug controller.
+
+**/
+BOOLEAN
+IsRootPciHotPlugController (
+  IN EFI_DEVICE_PATH_PROTOCOL         *HpcDevicePath,
+  OUT UINTN                           *HpIndex
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+    if (EfiCompareDevicePath (gPciRootHpcPool[Index].HpcDevicePath, HpcDevicePath)) {
+
+      if (HpIndex != NULL) {
+        *HpIndex = Index;
+      }
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Creating event object for PCI Hot Plug controller.
+
+  @param  HpIndex   Index of hot plug device in global array.
+  @param  Event     The retuned event that invoke this function.
+
+  @return Status of create event invoken.
+
+**/
+EFI_STATUS
+CreateEventForHpc (
+  IN  UINTN      HpIndex,
+  OUT EFI_EVENT  *Event
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  PciHPCInitialized,
+                  gPciRootHpcData + HpIndex,
+                  &((gPciRootHpcData + HpIndex)->Event)
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    *Event = (gPciRootHpcData + HpIndex)->Event;
+  }
+
+  return Status;
+}
+
+/**
+  Wait for all root PCI Hot Plug controller finished initializing.
+
+  @param TimeoutInMicroSeconds  Microseconds to wait for all root HPCs' initialization.
+
+  @retval EFI_SUCCESS           All HPCs initialization finished.
+  @retval EFI_TIMEOUT           Not ALL HPCs initialization finished in Microseconds.
+
+**/
+EFI_STATUS
+AllRootHPCInitialized (
+  IN  UINTN           TimeoutInMicroSeconds
+  )
+{
+  UINT32  Delay;
+  UINTN   Index;
+
+  Delay = (UINT32) ((TimeoutInMicroSeconds / 30) + 1);
+
+  do {
+    for (Index = 0; Index < gPciRootHpcCount; Index++) {
+
+      if (gPciRootHpcData[Index].Found && !gPciRootHpcData[Index].Initialized) {
+        break;
+      }
+    }
+
+    if (Index == gPciRootHpcCount) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 30 microseconds..
+    //
+    gBS->Stall (30);
+
+    Delay--;
+
+  } while (Delay > 0);
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Check whether PCI-PCI bridge has PCI Hot Plug capability register block.
+
+  @param PciIoDevice    A Pointer to the PCI-PCI bridge.
+
+  @retval TRUE    PCI device is HPC.
+  @retval FALSE   PCI device is not HPC.
+
+**/
+BOOLEAN
+IsSHPC (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  )
+{
+
+  EFI_STATUS  Status;
+  UINT8       Offset;
+
+  if (PciIoDevice == NULL) {
+    return FALSE;
+  }
+
+  Offset = 0;
+  Status = LocateCapabilityRegBlock (
+            PciIoDevice,
+            EFI_PCI_CAPABILITY_ID_HOTPLUG,
+            &Offset,
+            NULL
+            );
+
+  //
+  // If the PCI-PCI bridge has the hot plug controller build-in,
+  // then return TRUE;
+  //
+  if (!EFI_ERROR (Status)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get resource padding if the specified PCI bridge is a hot plug bus.
+
+  @param PciIoDevice    PCI bridge instance.
+
+**/
+VOID
+GetResourcePaddingForHpb (
+  IN PCI_IO_DEVICE      *PciIoDevice
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HPC_STATE                     State;
+  UINT64                            PciAddress;
+  EFI_HPC_PADDING_ATTRIBUTES        Attributes;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+
+  if (IsPciHotPlugBus (PciIoDevice)) {
+    //
+    // If PCI-PCI bridge device is PCI Hot Plug bus.
+    //
+    PciAddress = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, 0);
+    Status = gPciHotPlugInit->GetResourcePadding (
+                                gPciHotPlugInit,
+                                PciIoDevice->DevicePath,
+                                PciAddress,
+                                &State,
+                                (VOID **) &Descriptors,
+                                &Attributes
+                                );
+
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+
+    if ((State & EFI_HPC_STATE_ENABLED) != 0 && (State & EFI_HPC_STATE_INITIALIZED) != 0) {
+      PciIoDevice->ResourcePaddingDescriptors = Descriptors;
+      PciIoDevice->PaddingAttributes          = Attributes;
+    }
+
+    return;
+  }
+}
+
+/**
+  Test whether PCI device is hot plug bus.
+
+  @param PciIoDevice  PCI device instance.
+
+  @retval TRUE    PCI device is a hot plug bus.
+  @retval FALSE   PCI device is not a hot plug bus.
+
+**/
+BOOLEAN
+IsPciHotPlugBus (
+  PCI_IO_DEVICE                       *PciIoDevice
+  )
+{
+  if (IsSHPC (PciIoDevice)) {
+    //
+    // If the PPB has the hot plug controller build-in,
+    // then return TRUE;
+    //
+    return TRUE;
+  }
+
+  //
+  // Otherwise, see if it is a Root HPC
+  //
+  if(IsRootPciHotPlugBus (PciIoDevice->DevicePath, NULL)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h
new file mode 100644
index 0000000000..1fb9ba9720
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciHotPlugSupport.h
@@ -0,0 +1,190 @@
+/** @file
+  PCI Hot Plug support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef _EFI_PCI_HOT_PLUG_SUPPORT_H_
+#define _EFI_PCI_HOT_PLUG_SUPPORT_H_
+
+//
+// stall 1 second, its unit is 100ns
+//
+#define STALL_1_SECOND        1000000
+
+//
+// PCI Hot Plug controller private data
+//
+typedef struct {
+  EFI_EVENT Event;
+  BOOLEAN   Found;
+  BOOLEAN   Initialized;
+  VOID      *Padding;
+} ROOT_HPC_DATA;
+
+//
+// Reference of some global variabes
+//
+extern EFI_PCI_HOT_PLUG_INIT_PROTOCOL *gPciHotPlugInit;
+extern EFI_HPC_LOCATION               *gPciRootHpcPool;
+extern ROOT_HPC_DATA                  *gPciRootHpcData;
+
+/**
+  Event notification function to set Hot Plug controller status.
+
+  @param  Event                    The event that invoke this function.
+  @param  Context                  The calling context, pointer to ROOT_HPC_DATA.
+
+**/
+VOID
+EFIAPI
+PciHPCInitialized (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  );
+
+/**
+  Compare two device pathes to check if they are exactly same.
+
+  @param DevicePath1    A pointer to the first device path data structure.
+  @param DevicePath2    A pointer to the second device path data structure.
+
+  @retval TRUE    They are same.
+  @retval FALSE   They are not same.
+
+**/
+BOOLEAN
+EfiCompareDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath1,
+  IN EFI_DEVICE_PATH_PROTOCOL *DevicePath2
+  );
+
+/**
+  Check hot plug support and initialize root hot plug private data.
+
+  If Hot Plug is supported by the platform, call PCI Hot Plug Init protocol
+  to get PCI Hot Plug controller's information and constructor the root hot plug
+  private data structure.
+
+  @retval EFI_SUCCESS           They are same.
+  @retval EFI_UNSUPPORTED       No PCI Hot Plug controler on the platform.
+  @retval EFI_OUT_OF_RESOURCES  No memory to constructor root hot plug private
+                                data structure.
+
+**/
+EFI_STATUS
+InitializeHotPlugSupport (
+  VOID
+  );
+
+/**
+  Test whether PCI device is hot plug bus.
+
+  @param PciIoDevice  PCI device instance.
+
+  @retval TRUE    PCI device is a hot plug bus.
+  @retval FALSE   PCI device is not a hot plug bus.
+
+**/
+BOOLEAN
+IsPciHotPlugBus (
+  PCI_IO_DEVICE                       *PciIoDevice
+  );
+
+/**
+  Test whether device path is for root pci hot plug bus.
+
+  @param HpbDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug bus.
+  @retval FALSE         The device path is not for root pci hot plug bus.
+
+**/
+BOOLEAN
+IsRootPciHotPlugBus (
+  IN  EFI_DEVICE_PATH_PROTOCOL        *HpbDevicePath,
+  OUT UINTN                           *HpIndex    OPTIONAL
+  );
+
+/**
+  Test whether device path is for root pci hot plug controller.
+
+  @param HpcDevicePath  A pointer to device path data structure to be tested.
+  @param HpIndex        If HpIndex is not NULL, return the index of root hot
+                        plug in global array when TRUE is retuned.
+
+  @retval TRUE          The device path is for root pci hot plug controller.
+  @retval FALSE         The device path is not for root pci hot plug controller.
+
+**/
+BOOLEAN
+IsRootPciHotPlugController (
+  IN EFI_DEVICE_PATH_PROTOCOL         *HpcDevicePath,
+  OUT UINTN                           *HpIndex
+  );
+
+/**
+  Creating event object for PCI Hot Plug controller.
+
+  @param  HpIndex   Index of hot plug device in global array.
+  @param  Event     The retuned event that invoke this function.
+
+  @return Status of create event invoken.
+
+**/
+EFI_STATUS
+CreateEventForHpc (
+  IN  UINTN      HpIndex,
+  OUT EFI_EVENT  *Event
+  );
+
+/**
+  Wait for all root PCI Hot Plug controller finished initializing.
+
+  @param TimeoutInMicroSeconds  Microseconds to wait for all root HPCs' initialization.
+
+  @retval EFI_SUCCESS           All HPCs initialization finished.
+  @retval EFI_TIMEOUT           Not ALL HPCs initialization finished in Microseconds.
+
+**/
+EFI_STATUS
+AllRootHPCInitialized (
+  IN  UINTN           TimeoutInMicroSeconds
+  );
+
+/**
+  Check whether PCI-PCI bridge has PCI Hot Plug capability register block.
+
+  @param PciIoDevice    A Pointer to the PCI-PCI bridge.
+
+  @retval TRUE    PCI device is HPC.
+  @retval FALSE   PCI device is not HPC.
+
+**/
+BOOLEAN
+IsSHPC (
+  IN PCI_IO_DEVICE                      *PciIoDevice
+  );
+
+/**
+  Get resource padding if the specified PCI bridge is a hot plug bus.
+
+  @param PciIoDevice    PCI bridge instance.
+
+**/
+VOID
+GetResourcePaddingForHpb (
+  IN PCI_IO_DEVICE      *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c
new file mode 100644
index 0000000000..416063268f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.c
@@ -0,0 +1,2043 @@
+/** @file
+  EFI PCI IO protocol functions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// Pci Io Protocol Interface
+//
+EFI_PCI_IO_PROTOCOL  mPciIoInterface = {
+  PciIoPollMem,
+  PciIoPollIo,
+  {
+    PciIoMemRead,
+    PciIoMemWrite
+  },
+  {
+    PciIoIoRead,
+    PciIoIoWrite
+  },
+  {
+    PciIoConfigRead,
+    PciIoConfigWrite
+  },
+  PciIoCopyMem,
+  PciIoMap,
+  PciIoUnmap,
+  PciIoAllocateBuffer,
+  PciIoFreeBuffer,
+  PciIoFlush,
+  PciIoGetLocation,
+  PciIoAttributes,
+  PciIoGetBarAttributes,
+  PciIoSetBarAttributes,
+  0,
+  NULL
+};
+
+/**
+  Initializes a PCI I/O Instance.
+
+  @param PciIoDevice    Pci device instance.
+
+**/
+VOID
+InitializePciIoInstance (
+  IN PCI_IO_DEVICE               *PciIoDevice
+  )
+{
+  CopyMem (&PciIoDevice->PciIo, &mPciIoInterface, sizeof (EFI_PCI_IO_PROTOCOL));
+}
+
+/**
+  Verifies access to a PCI Base Address Register (BAR).
+
+  @param PciIoDevice  Pci device instance.
+  @param BarIndex     The BAR index of the standard PCI Configuration header to use as the
+                      base address for the memory or I/O operation to perform.
+  @param Type         Operation type could be memory or I/O.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER Invalid Width/BarIndex or Bar type.
+  @retval EFI_SUCCESS           Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyBarAccess (
+  IN PCI_IO_DEVICE                   *PciIoDevice,
+  IN UINT8                           BarIndex,
+  IN PCI_BAR_TYPE                    Type,
+  IN IN EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN IN UINTN                        Count,
+  IN UINT64                          *Offset
+  )
+{
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BarIndex == EFI_PCI_IO_PASS_THROUGH_BAR) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // BarIndex 0-5 is legal
+  //
+  if (BarIndex >= PCI_MAX_BAR) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!CheckBarType (PciIoDevice, BarIndex, Type)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If Width is EfiPciIoWidthFifoUintX then convert to EfiPciIoWidthUintX
+  // If Width is EfiPciIoWidthFillUintX then convert to EfiPciIoWidthUintX
+  //
+  if (Width >= EfiPciIoWidthFifoUint8 && Width <= EfiPciIoWidthFifoUint64) {
+    Count = 1;
+  }
+
+  Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & 0x03);
+
+  if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PciIoDevice->PciBar[BarIndex].Length) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Offset = *Offset + PciIoDevice->PciBar[BarIndex].BaseAddress;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Verifies access to a PCI Configuration Header.
+
+  @param PciIoDevice  Pci device instance.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER  Invalid Width
+  @retval EFI_UNSUPPORTED        Offset overflowed.
+  @retval EFI_SUCCESS            Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyConfigAccess (
+  IN PCI_IO_DEVICE              *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN UINTN                      Count,
+  IN UINT64                     *Offset
+  )
+{
+  UINT64  ExtendOffset;
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If Width is EfiPciIoWidthFillUintX then convert to EfiPciIoWidthUintX
+  //
+  Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & 0x03);
+
+  if (PciIoDevice->IsPciExp) {
+    if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PCI_EXP_MAX_CONFIG_OFFSET) {
+      return EFI_UNSUPPORTED;
+    }
+
+    ExtendOffset  = LShiftU64 (*Offset, 32);
+    *Offset       = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, 0);
+    *Offset       = (*Offset) | ExtendOffset;
+
+  } else {
+    if ((*Offset + Count * (UINTN)(1 << Width)) - 1 >= PCI_MAX_CONFIG_OFFSET) {
+      return EFI_UNSUPPORTED;
+    }
+
+    *Offset = EFI_PCI_ADDRESS (PciIoDevice->BusNumber, PciIoDevice->DeviceNumber, PciIoDevice->FunctionNumber, *Offset);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollMem (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, 1, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Width > EfiPciIoWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Status  = PciIoMemRead (This, Width, BarIndex, Offset, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      if ((*Result & Mask) == Value || Delay == 0) {
+        return EFI_SUCCESS;
+      }
+      do {
+        //
+        // Stall 10 us = 100 * 100ns
+        //
+        gBS->Stall (10);
+
+        Status  = PciIoMemRead (This, Width, BarIndex, Offset, 1, Result);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((*Result & Mask) == Value) {
+          return EFI_SUCCESS;
+        }
+        if (Delay <= 100) {
+          return EFI_TIMEOUT;
+        }
+        Delay -= 100;
+      } while (TRUE);
+    }
+  }
+  
+  Status = PciIoDevice->PciRootBridgeIo->PollMem (
+                                           PciIoDevice->PciRootBridgeIo,
+                                           (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                           Offset,
+                                           Mask,
+                                           Value,
+                                           Delay,
+                                           Result
+                                           );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollIo (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width > EfiPciIoWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, 1, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Status  = PciIoIoRead (This, Width, BarIndex, Offset, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      if ((*Result & Mask) == Value || Delay == 0) {
+        return EFI_SUCCESS;
+      }
+      do {
+        //
+        // Stall 10 us = 100 * 100ns
+        //
+        gBS->Stall (10);
+
+        Status  = PciIoIoRead (This, Width, BarIndex, Offset, 1, Result);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((*Result & Mask) == Value) {
+          return EFI_SUCCESS;
+        }
+        if (Delay <= 100) {
+          return EFI_TIMEOUT;
+        }
+        Delay -= 100;
+      } while (TRUE);
+    }
+  }
+  
+  Status = PciIoDevice->PciRootBridgeIo->PollIo (
+                                           PciIoDevice->PciRootBridgeIo,
+                                           (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                           Offset,
+                                           Mask,
+                                           Value,
+                                           Delay,
+                                           Result
+                                           );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+  
+
+  Status = PciIoDevice->PciRootBridgeIo->Mem.Read (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeMem, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->Mem.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }    
+
+  Status = PciIoDevice->PciRootBridgeIo->Io.Read (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, BarIndex, PciBarTypeIo, Width, Count, &Offset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+
+  Status = PciIoDevice->PciRootBridgeIo->Io.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Offset,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        Address;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Address     = Offset;
+  Status      = PciIoVerifyConfigAccess (PciIoDevice, Width, Count, &Address);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }    
+
+  Status = PciIoDevice->PciRootBridgeIo->Pci.Read (
+                                               PciIoDevice->PciRootBridgeIo,
+                                               (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                               Address,
+                                               Count,
+                                               Buffer
+                                               );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_READ_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        Address;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Address     = Offset;
+  Status      = PciIoVerifyConfigAccess (PciIoDevice, Width, Count, &Address);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((Offset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+  
+  Status = PciIoDevice->PciRootBridgeIo->Pci.Write (
+                                              PciIoDevice->PciRootBridgeIo,
+                                              (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                              Address,
+                                              Count,
+                                              Buffer
+                                              );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_WRITE_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Enables a PCI driver to copy one region of PCI memory space to another region of PCI
+  memory space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  DestBarIndex          The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  DestOffset            The destination offset within the BAR specified by DestBarIndex to
+                                start the memory writes for the copy operation.
+  @param  SrcBarIndex           The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  SrcOffset             The source offset within the BAR specified by SrcBarIndex to start
+                                the memory reads for the copy operation.
+  @param  Count                 The number of memory operations to perform. Bytes moved is Width
+                                size * Count, starting at DestOffset and SrcOffset.
+
+  @retval EFI_SUCCESS           The data was copied from one memory region to another memory region.
+  @retval EFI_UNSUPPORTED       DestBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       SrcBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by DestOffset, Width, and Count
+                                is not valid for the PCI BAR specified by DestBarIndex.
+  @retval EFI_UNSUPPORTED       The address range specified by SrcOffset, Width, and Count is
+                                not valid for the PCI BAR specified by SrcBarIndex.
+  @retval EFI_INVALID_PARAMETER Width is invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoCopyMem (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN     UINT8                        DestBarIndex,
+  IN     UINT64                       DestOffset,
+  IN     UINT8                        SrcBarIndex,
+  IN     UINT64                       SrcOffset,
+  IN     UINTN                        Count
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Width >= EfiPciIoWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Width == EfiPciIoWidthFifoUint8  ||
+      Width == EfiPciIoWidthFifoUint16 ||
+      Width == EfiPciIoWidthFifoUint32 ||
+      Width == EfiPciIoWidthFifoUint64 ||
+      Width == EfiPciIoWidthFillUint8  ||
+      Width == EfiPciIoWidthFillUint16 ||
+      Width == EfiPciIoWidthFillUint32 ||
+      Width == EfiPciIoWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, DestBarIndex, PciBarTypeMem, Width, Count, &DestOffset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIoVerifyBarAccess (PciIoDevice, SrcBarIndex, PciBarTypeMem, Width, Count, &SrcOffset);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If request is not aligned, then convert request to EfiPciIoWithXXXUint8
+  //  
+  if (FeaturePcdGet (PcdUnalignedPciIoEnable)) {
+    if ((SrcOffset & ((1 << (Width & 0x03)) - 1)) != 0 || (DestOffset & ((1 << (Width & 0x03)) - 1)) != 0) {
+      Count *=  (UINTN)(1 << (Width & 0x03));
+      Width = (EFI_PCI_IO_PROTOCOL_WIDTH) (Width & (~0x03));
+    }
+  }  
+
+  Status = PciIoDevice->PciRootBridgeIo->CopyMem (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) Width,
+                                          DestOffset,
+                                          SrcOffset,
+                                          Count
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Provides the PCI controller-specific addresses needed to access system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMap (
+  IN     EFI_PCI_IO_PROTOCOL            *This,
+  IN     EFI_PCI_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                           *HostAddress,
+  IN OUT UINTN                          *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS           *DeviceAddress,
+  OUT    VOID                           **Mapping
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((UINT32)Operation >= EfiPciIoOperationMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (HostAddress == NULL || NumberOfBytes == NULL || DeviceAddress == NULL || Mapping == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((PciIoDevice->Attributes & EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE) != 0) {
+    Operation = (EFI_PCI_IO_PROTOCOL_OPERATION) (Operation + EfiPciOperationBusMasterRead64);
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->Map (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION) Operation,
+                                          HostAddress,
+                                          NumberOfBytes,
+                                          DeviceAddress,
+                                          Mapping
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoUnmap (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  IN  VOID                 *Mapping
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->Unmap (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Mapping
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Allocates pages that are suitable for an EfiPciIoOperationBusMasterCommonBuffer
+  mapping.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAllocateBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 Pages,
+  OUT VOID                  **HostAddress,
+  IN  UINT64                Attributes
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  if ((Attributes &
+      (~(EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE | EFI_PCI_ATTRIBUTE_MEMORY_CACHED))) != 0){
+    return EFI_UNSUPPORTED;
+  }
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if ((PciIoDevice->Attributes & EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE) != 0) {
+    Attributes |= EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE;
+  }
+
+  Status = PciIoDevice->PciRootBridgeIo->AllocateBuffer (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Type,
+                                          MemoryType,
+                                          Pages,
+                                          HostAddress,
+                                          Attributes
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFreeBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  UINTN                 Pages,
+  IN  VOID                  *HostAddress
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->FreeBuffer (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          Pages,
+                                          HostAddress
+                                          );
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The PCI posted write transactions were flushed from the PCI host
+                                bridge to system memory.
+  @retval EFI_DEVICE_ERROR      The PCI posted write transactions were not flushed from the PCI
+                                host bridge due to a hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFlush (
+  IN  EFI_PCI_IO_PROTOCOL  *This
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  Status = PciIoDevice->PciRootBridgeIo->Flush (
+                                           PciIoDevice->PciRootBridgeIo
+                                           );
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves this PCI controller's current PCI bus number, device number, and function number.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  SegmentNumber         The PCI controller's current PCI segment number.
+  @param  BusNumber             The PCI controller's current PCI bus number.
+  @param  DeviceNumber          The PCI controller's current PCI device number.
+  @param  FunctionNumber        The PCI controller's current PCI function number.
+
+  @retval EFI_SUCCESS           The PCI controller location was returned.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetLocation (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  OUT UINTN                *Segment,
+  OUT UINTN                *Bus,
+  OUT UINTN                *Device,
+  OUT UINTN                *Function
+  )
+{
+  PCI_IO_DEVICE *PciIoDevice;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if (Segment == NULL || Bus == NULL || Device == NULL || Function == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Segment  = PciIoDevice->PciRootBridgeIo->SegmentNumber;
+  *Bus      = PciIoDevice->BusNumber;
+  *Device   = PciIoDevice->DeviceNumber;
+  *Function = PciIoDevice->FunctionNumber;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check BAR type for PCI resource.
+
+  @param PciIoDevice   PCI device instance.
+  @param BarIndex      The BAR index of the standard PCI Configuration header to use as the
+                       base address for the memory or I/O operation to perform.
+  @param BarType       Memory or I/O.
+
+  @retval TRUE         Pci device's bar type is same with input BarType.
+  @retval TRUE         Pci device's bar type is not same with input BarType.
+
+**/
+BOOLEAN
+CheckBarType (
+  IN PCI_IO_DEVICE          *PciIoDevice,
+  IN UINT8                  BarIndex,
+  IN PCI_BAR_TYPE           BarType
+  )
+{
+  switch (BarType) {
+
+  case PciBarTypeMem:
+
+    if (PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeMem32  &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypePMem32 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypePMem64 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeMem64    ) {
+      return FALSE;
+    }
+
+    return TRUE;
+
+  case PciBarTypeIo:
+    if (PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeIo32 &&
+        PciIoDevice->PciBar[BarIndex].BarType != PciBarTypeIo16){
+      return FALSE;
+    }
+
+    return TRUE;
+
+  default:
+    break;
+  }
+
+  return FALSE;
+}
+
+/**
+  Set/Disable new attributes to a Root Bridge.
+
+  @param  PciIoDevice  Pci device instance.
+  @param  Attributes   New attribute want to be set.
+  @param  Operation    Set or Disable.
+
+  @retval  EFI_UNSUPPORTED  If root bridge does not support change attribute.
+  @retval  EFI_SUCCESS      Successfully set new attributs.
+
+**/
+EFI_STATUS
+ModifyRootBridgeAttributes (
+  IN  PCI_IO_DEVICE                            *PciIoDevice,
+  IN  UINT64                                   Attributes,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  )
+{
+  UINT64      PciRootBridgeSupports;
+  UINT64      PciRootBridgeAttributes;
+  UINT64      NewPciRootBridgeAttributes;
+  EFI_STATUS  Status;
+
+  //
+  // Get the current attributes of this PCI device's PCI Root Bridge
+  //
+  Status = PciIoDevice->PciRootBridgeIo->GetAttributes (
+                                          PciIoDevice->PciRootBridgeIo,
+                                          &PciRootBridgeSupports,
+                                          &PciRootBridgeAttributes
+                                          );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Mask off attributes not supported by PCI root bridge.
+  //
+  Attributes &= ~(UINT64)(EFI_PCI_IO_ATTRIBUTE_EMBEDDED_DEVICE |
+                          EFI_PCI_IO_ATTRIBUTE_EMBEDDED_ROM |
+                          EFI_PCI_IO_ATTRIBUTE_DUAL_ADDRESS_CYCLE);
+
+  //
+  // Record the new attribute of the Root Bridge
+  //
+  if (Operation == EfiPciIoAttributeOperationEnable) {
+    NewPciRootBridgeAttributes = PciRootBridgeAttributes | Attributes;
+  } else {
+    NewPciRootBridgeAttributes = PciRootBridgeAttributes & (~Attributes);
+  }
+
+  //
+  // Call the PCI Root Bridge to attempt to modify the attributes
+  //
+  if ((NewPciRootBridgeAttributes ^ PciRootBridgeAttributes) != 0) {
+
+    Status = PciIoDevice->PciRootBridgeIo->SetAttributes (
+                                            PciIoDevice->PciRootBridgeIo,
+                                            NewPciRootBridgeAttributes,
+                                            NULL,
+                                            NULL
+                                            );
+    if (EFI_ERROR (Status)) {
+      //
+      // The PCI Root Bridge could not modify the attributes, so return the error.
+      //
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // Also update the attributes for this Root Bridge structure
+  //
+  PciIoDevice->Attributes = NewPciRootBridgeAttributes;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether this device can be enable/disable to snoop.
+
+  @param PciIoDevice  Pci device instance.
+  @param Operation    Enable/Disable.
+
+  @retval EFI_UNSUPPORTED  Pci device is not GFX device or not support snoop.
+  @retval EFI_SUCCESS      Snoop can be supported.
+
+**/
+EFI_STATUS
+SupportPaletteSnoopAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  )
+{
+  PCI_IO_DEVICE *Temp;
+  UINT16        VGACommand;
+
+  //
+  // Snoop attribute can be only modified by GFX
+  //
+  if (!IS_PCI_GFX (&PciIoDevice->Pci)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get the boot VGA on the same segement
+  //
+  Temp = ActiveVGADeviceOnTheSameSegment (PciIoDevice);
+
+  if (Temp == NULL) {
+    //
+    // If there is no VGA device on the segement, set
+    // this graphics card to decode the palette range
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Check these two agents are on the same path
+  //
+  if (!PciDevicesOnTheSamePath (Temp, PciIoDevice)) {
+    //
+    // they are not on the same path, so snoop can be enabled or disabled
+    //
+    return EFI_SUCCESS;
+  }
+  //
+  // Check if they are on the same bus
+  //
+  if (Temp->Parent == PciIoDevice->Parent) {
+
+    PCI_READ_COMMAND_REGISTER (Temp, &VGACommand);
+
+    //
+    // If they are on the same bus, either one can
+    // be set to snoop, the other set to decode
+    //
+    if ((VGACommand & EFI_PCI_COMMAND_VGA_PALETTE_SNOOP) != 0) {
+      //
+      // VGA has set to snoop, so GFX can be only set to disable snoop
+      //
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        return EFI_UNSUPPORTED;
+      }
+    } else {
+      //
+      // VGA has disabled to snoop, so GFX can be only enabled
+      //
+      if (Operation == EfiPciIoAttributeOperationDisable) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If they are on  the same path but on the different bus
+  // The first agent is set to snoop, the second one set to
+  // decode
+  //
+
+  if (Temp->BusNumber < PciIoDevice->BusNumber) {
+    //
+    // GFX should be set to decode
+    //
+    if (Operation == EfiPciIoAttributeOperationDisable) {
+      PCI_ENABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+      Temp->Attributes |= EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+    } else {
+      return EFI_UNSUPPORTED;
+    }
+
+  } else {
+    //
+    // GFX should be set to snoop
+    //
+    if (Operation == EfiPciIoAttributeOperationEnable) {
+      PCI_DISABLE_COMMAND_REGISTER (Temp, EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+      Temp->Attributes &= (~(UINT64)EFI_PCI_COMMAND_VGA_PALETTE_SNOOP);
+    } else {
+      return EFI_UNSUPPORTED;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Performs an operation on the attributes that this PCI controller supports. The operations include
+  getting the set of supported attributes, retrieving the current attributes, setting the current
+  attributes, enabling attributes, and disabling attributes.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             The operation to perform on the attributes for this PCI controller.
+  @param  Attributes            The mask of attributes that are used for Set, Enable, and Disable
+                                operations.
+  @param  Result                A pointer to the result mask of attributes that are returned for the Get
+                                and Supported operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAttributes (
+  IN EFI_PCI_IO_PROTOCOL                       * This,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN  UINT64                                   Attributes,
+  OUT UINT64                                   *Result OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+
+  PCI_IO_DEVICE *PciIoDevice;
+  PCI_IO_DEVICE *UpStreamBridge;
+  PCI_IO_DEVICE *Temp;
+
+  UINT64        Supports;
+  UINT64        UpStreamAttributes;
+  UINT16        BridgeControl;
+  UINT16        Command;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  switch (Operation) {
+  case EfiPciIoAttributeOperationGet:
+    if (Result == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *Result = PciIoDevice->Attributes;
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationSupported:
+    if (Result == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *Result = PciIoDevice->Supports;
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationSet:
+    Status = PciIoDevice->PciIo.Attributes (
+                                  &(PciIoDevice->PciIo),
+                                  EfiPciIoAttributeOperationEnable,
+                                  Attributes,
+                                  NULL
+                                  );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    Status = PciIoDevice->PciIo.Attributes (
+                                  &(PciIoDevice->PciIo),
+                                  EfiPciIoAttributeOperationDisable,
+                                  (~Attributes) & (PciIoDevice->Supports),
+                                  NULL
+                                  );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    return EFI_SUCCESS;
+
+  case EfiPciIoAttributeOperationEnable:
+  case EfiPciIoAttributeOperationDisable:
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Just a trick for ENABLE attribute
+  // EFI_PCI_DEVICE_ENABLE is not defined in UEFI spec, which is the internal usage.
+  // So, this logic doesn't confrom to UEFI spec, which should be removed.
+  // But this trick logic is still kept for some binary drivers that depend on it.
+  //
+  if ((Attributes & EFI_PCI_DEVICE_ENABLE) == EFI_PCI_DEVICE_ENABLE) {
+    Attributes &= (PciIoDevice->Supports);
+
+    //
+    // Raise the EFI_P_PC_ENABLE Status code
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      EFI_IO_BUS_PCI | EFI_P_PC_ENABLE,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  //
+  // Check VGA and VGA16, they can not be set at the same time
+  //
+  if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO)) != 0) {
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // If no attributes can be supported, then return.
+  // Otherwise, set the attributes that it can support.
+  //
+  Supports = (PciIoDevice->Supports) & Attributes;
+  if (Supports != Attributes) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // For Root Bridge, just call RootBridgeIo to set attributes;
+  //
+  if (PciIoDevice->Parent == NULL) {
+    Status = ModifyRootBridgeAttributes (PciIoDevice, Attributes, Operation);
+    return Status;
+  }
+
+  Command       = 0;
+  BridgeControl = 0;
+
+  //
+  // For PPB & P2C, set relevant attribute bits
+  //
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci) || IS_CARDBUS_BRIDGE (&PciIoDevice->Pci)) {
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_IO | EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_VGA;
+    }
+
+    if ((Attributes & EFI_PCI_IO_ATTRIBUTE_ISA_IO) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_ISA;
+    }
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+      Command |= EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO;
+    }
+
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16 | EFI_PCI_IO_ATTRIBUTE_VGA_IO_16)) != 0) {
+      BridgeControl |= EFI_PCI_BRIDGE_CONTROL_VGA_16;
+    }
+
+  } else {
+    //
+    // Do with the attributes on VGA
+    // Only for VGA's legacy resource, we just can enable once.
+    //
+    if ((Attributes &
+        (EFI_PCI_IO_ATTRIBUTE_VGA_IO    |
+         EFI_PCI_IO_ATTRIBUTE_VGA_IO_16 |
+         EFI_PCI_IO_ATTRIBUTE_VGA_MEMORY)) != 0) {
+      //
+      // Check if a VGA has been enabled before enabling a new one
+      //
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        //
+        // Check if there have been an active VGA device on the same segment
+        //
+        Temp = ActiveVGADeviceOnTheSameSegment (PciIoDevice);
+        if (Temp != NULL && Temp != PciIoDevice) {
+          //
+          // An active VGA has been detected, so can not enable another
+          //
+          return EFI_UNSUPPORTED;
+        }
+      }
+    }
+
+    //
+    // Do with the attributes on GFX
+    //
+    if ((Attributes & (EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO_16)) != 0) {
+
+      if (Operation == EfiPciIoAttributeOperationEnable) {
+        //
+        // Check if snoop can be enabled in current configuration
+        //
+        Status = SupportPaletteSnoopAttributes (PciIoDevice, Operation);
+
+        if (EFI_ERROR (Status)) {
+
+          //
+          // Enable operation is forbidden, so mask the bit in attributes
+          // so as to keep consistent with the actual Status
+          //
+          // Attributes &= (~EFI_PCI_IO_ATTRIBUTE_VGA_PALETTE_IO);
+          //
+          //
+          //
+          return EFI_UNSUPPORTED;
+
+        }
+      }
+
+      //
+      // It can be supported, so get ready to set the bit
+      //
+      Command |= EFI_PCI_COMMAND_VGA_PALETTE_SNOOP;
+    }
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_IO) != 0) {
+    Command |= EFI_PCI_COMMAND_IO_SPACE;
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_MEMORY) != 0) {
+    Command |= EFI_PCI_COMMAND_MEMORY_SPACE;
+  }
+
+  if ((Attributes & EFI_PCI_IO_ATTRIBUTE_BUS_MASTER) != 0) {
+    Command |= EFI_PCI_COMMAND_BUS_MASTER;
+  }
+  //
+  // The upstream bridge should be also set to revelant attribute
+  // expect for IO, Mem and BusMaster
+  //
+  UpStreamAttributes = Attributes &
+                       (~(EFI_PCI_IO_ATTRIBUTE_IO     |
+                          EFI_PCI_IO_ATTRIBUTE_MEMORY |
+                          EFI_PCI_IO_ATTRIBUTE_BUS_MASTER
+                          )
+                        );
+  UpStreamBridge = PciIoDevice->Parent;
+
+  if (Operation == EfiPciIoAttributeOperationEnable) {
+    //
+    // Enable relevant attributes to command register and bridge control register
+    //
+    Status = PCI_ENABLE_COMMAND_REGISTER (PciIoDevice, Command);
+    if (BridgeControl != 0) {
+      Status = PCI_ENABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+    }
+
+    PciIoDevice->Attributes |= Attributes;
+
+    //
+    // Enable attributes of the upstream bridge
+    //
+    Status = UpStreamBridge->PciIo.Attributes (
+                                    &(UpStreamBridge->PciIo),
+                                    EfiPciIoAttributeOperationEnable,
+                                    UpStreamAttributes,
+                                    NULL
+                                    );
+  } else {
+
+    //
+    // Disable relevant attributes to command register and bridge control register
+    //
+    Status = PCI_DISABLE_COMMAND_REGISTER (PciIoDevice, Command);
+    if (BridgeControl != 0) {
+      Status = PCI_DISABLE_BRIDGE_CONTROL_REGISTER (PciIoDevice, BridgeControl);
+    }
+
+    PciIoDevice->Attributes &= (~Attributes);
+    Status = EFI_SUCCESS;
+
+  }
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      EFI_IO_BUS_PCI | EFI_IOB_EC_CONTROLLER_ERROR,
+      PciIoDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Gets the attributes that this PCI controller supports setting on a BAR using
+  SetBarAttributes(), and retrieves the list of resource descriptors for a BAR.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Supports              A pointer to the mask of attributes that this PCI controller supports
+                                setting for this BAR with SetBarAttributes().
+  @param  Resources             A pointer to the ACPI 2.0 resource descriptors that describe the current
+                                configuration of this BAR of the PCI controller.
+
+  @retval EFI_SUCCESS           If Supports is not NULL, then the attributes that the PCI
+                                controller supports are returned in Supports. If Resources
+                                is not NULL, then the ACPI 2.0 resource descriptors that the PCI
+                                controller is currently using are returned in Resources.
+  @retval EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources available to allocate
+                                Resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL             * This,
+  IN  UINT8                          BarIndex,
+  OUT UINT64                         *Supports, OPTIONAL
+  OUT VOID                           **Resources OPTIONAL
+  )
+{
+  UINT8                             *Configuration;
+  PCI_IO_DEVICE                     *PciIoDevice;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *AddressSpace;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  if (Supports == NULL && Resources == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BarIndex >= PCI_MAX_BAR) || (PciIoDevice->PciBar[BarIndex].BarType == PciBarTypeUnknown)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // This driver does not support modifications to the WRITE_COMBINE or
+  // CACHED attributes for BAR ranges.
+  //
+  if (Supports != NULL) {
+    *Supports = PciIoDevice->Supports & EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED & EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE;
+  }
+
+  if (Resources != NULL) {
+    Configuration = AllocateZeroPool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+    if (Configuration == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    AddressSpace = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+
+    AddressSpace->Desc         = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+    AddressSpace->Len          = (UINT16) (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3);
+
+    AddressSpace->AddrRangeMin = PciIoDevice->PciBar[BarIndex].BaseAddress;
+    AddressSpace->AddrLen      = PciIoDevice->PciBar[BarIndex].Length;
+    AddressSpace->AddrRangeMax = PciIoDevice->PciBar[BarIndex].Alignment;
+
+    switch (PciIoDevice->PciBar[BarIndex].BarType) {
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+      //
+      // Io
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_IO;
+      break;
+
+    case PciBarTypeMem32:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // 32 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 32;
+      break;
+
+    case PciBarTypePMem32:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      AddressSpace->SpecificFlag = 0x6;
+      //
+      // 32 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 32;
+      break;
+
+    case PciBarTypeMem64:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // 64 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 64;
+      break;
+
+    case PciBarTypePMem64:
+      //
+      // Mem
+      //
+      AddressSpace->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      //
+      // prefechable
+      //
+      AddressSpace->SpecificFlag = 0x6;
+      //
+      // 64 bit
+      //
+      AddressSpace->AddrSpaceGranularity = 64;
+      break;
+
+    default:
+      break;
+    }
+
+    //
+    // put the checksum
+    //
+    End           = (EFI_ACPI_END_TAG_DESCRIPTOR *) (AddressSpace + 1);
+    End->Desc     = ACPI_END_TAG_DESCRIPTOR;
+    End->Checksum = 0;
+
+    *Resources    = Configuration;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the attributes for a range of a BAR on a PCI controller.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Attributes            The mask of attributes to set for the resource range specified by
+                                BarIndex, Offset, and Length.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Offset                A pointer to the BAR relative base address of the resource range to be
+                                modified by the attributes specified by Attributes.
+  @param  Length                A pointer to the length of the resource range to be modified by the
+                                attributes specified by Attributes.
+
+  @retval EFI_SUCCESS           The set of attributes specified by Attributes for the resource
+                                range specified by BarIndex, Offset, and Length were
+                                set on the PCI controller, and the actual resource range is returned
+                                in Offset and Length.
+  @retval EFI_INVALID_PARAMETER Offset or Length is NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to set the attributes on the
+                                resource range specified by BarIndex, Offset, and
+                                Length.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoSetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     UINT64                       Attributes,
+  IN     UINT8                        BarIndex,
+  IN OUT UINT64                       *Offset,
+  IN OUT UINT64                       *Length
+  )
+{
+  EFI_STATUS    Status;
+  PCI_IO_DEVICE *PciIoDevice;
+  UINT64        NonRelativeOffset;
+  UINT64        Supports;
+
+  PciIoDevice = PCI_IO_DEVICE_FROM_PCI_IO_THIS (This);
+
+  //
+  // Make sure Offset and Length are not NULL
+  //
+  if (Offset == NULL || Length == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PciIoDevice->PciBar[BarIndex].BarType == PciBarTypeUnknown) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // This driver does not support setting the WRITE_COMBINE or the CACHED attributes.
+  // If Attributes is not 0, then return EFI_UNSUPPORTED.
+  //
+  Supports = PciIoDevice->Supports & EFI_PCI_IO_ATTRIBUTE_MEMORY_CACHED & EFI_PCI_IO_ATTRIBUTE_MEMORY_WRITE_COMBINE;
+
+  if (Attributes != (Attributes & Supports)) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Attributes must be supported.  Make sure the BAR range describd by BarIndex, Offset, and
+  // Length are valid for this PCI device.
+  //
+  NonRelativeOffset = *Offset;
+  Status = PciIoVerifyBarAccess (
+            PciIoDevice,
+            BarIndex,
+            PciBarTypeMem,
+            EfiPciIoWidthUint8,
+            (UINT32) *Length,
+            &NonRelativeOffset
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program parent bridge's attribute recurrently.
+
+  @param PciIoDevice  Child Pci device instance
+  @param Operation    The operation to perform on the attributes for this PCI controller.
+  @param Attributes   The mask of attributes that are used for Set, Enable, and Disable
+                      operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+UpStreamBridgesAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN UINT64                                   Attributes
+  )
+{
+  PCI_IO_DEVICE       *Parent;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  Parent = PciIoDevice->Parent;
+
+  while (Parent != NULL && IS_PCI_BRIDGE (&Parent->Pci)) {
+
+    //
+    // Get the PciIo Protocol
+    //
+    PciIo = &Parent->PciIo;
+
+    PciIo->Attributes (PciIo, Operation, Attributes, NULL);
+
+    Parent = Parent->Parent;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test whether two Pci devices has same parent bridge.
+
+  @param PciDevice1  The first pci device for testing.
+  @param PciDevice2  The second pci device for testing.
+
+  @retval TRUE       Two Pci device has the same parent bridge.
+  @retval FALSE      Two Pci device has not the same parent bridge.
+
+**/
+BOOLEAN
+PciDevicesOnTheSamePath (
+  IN PCI_IO_DEVICE        *PciDevice1,
+  IN PCI_IO_DEVICE        *PciDevice2
+  )
+{
+  BOOLEAN   Existed1;
+  BOOLEAN   Existed2;
+
+  if (PciDevice1->Parent == PciDevice2->Parent) {
+    return TRUE;
+  }
+
+  Existed1 = PciDeviceExisted (PciDevice1->Parent, PciDevice2);
+  Existed2 = PciDeviceExisted (PciDevice2->Parent, PciDevice1);
+
+  return (BOOLEAN) (Existed1 || Existed2);
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h
new file mode 100644
index 0000000000..ac2def5acf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciIo.h
@@ -0,0 +1,687 @@
+/** @file
+  EFI PCI IO protocol functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef _EFI_PCI_IO_PROTOCOL_H_
+#define _EFI_PCI_IO_PROTOCOL_H_
+
+/**
+  Initializes a PCI I/O Instance.
+
+  @param PciIoDevice    Pci device instance.
+
+**/
+VOID
+InitializePciIoInstance (
+  IN PCI_IO_DEVICE               *PciIoDevice
+  );
+
+/**
+  Verifies access to a PCI Base Address Register (BAR).
+
+  @param PciIoDevice  Pci device instance.
+  @param BarIndex     The BAR index of the standard PCI Configuration header to use as the
+                      base address for the memory or I/O operation to perform.
+  @param Type         Operation type could be memory or I/O.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER Invalid Width/BarIndex or Bar type.
+  @retval EFI_SUCCESS           Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyBarAccess (
+  IN PCI_IO_DEVICE                   *PciIoDevice,
+  IN UINT8                           BarIndex,
+  IN PCI_BAR_TYPE                    Type,
+  IN IN EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN IN UINTN                        Count,
+  IN UINT64                          *Offset
+  );
+
+/**
+  Verifies access to a PCI Configuration Header.
+
+  @param PciIoDevice  Pci device instance.
+  @param Width        Signifies the width of the memory or I/O operations.
+  @param Count        The number of memory or I/O operations to perform.
+  @param Offset       The offset within the PCI configuration space for the PCI controller.
+
+  @retval EFI_INVALID_PARAMETER  Invalid Width
+  @retval EFI_UNSUPPORTED        Offset overflowed.
+  @retval EFI_SUCCESS            Successfully verified.
+
+**/
+EFI_STATUS
+PciIoVerifyConfigAccess (
+  IN PCI_IO_DEVICE              *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN UINTN                      Count,
+  IN UINT64                     *Offset
+  );
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollMem (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  );
+
+/**
+  Reads from the memory space of a PCI controller. Returns either when the polling exit criteria is
+  satisfied or after a defined duration.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory operation.
+  @param  Mask                  Mask used for the polling criteria.
+  @param  Value                 The comparison value used for the polling exit criteria.
+  @param  Delay                 The number of 100 ns units to poll.
+  @param  Result                Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS           The last data returned from the access matched the poll exit criteria.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       Offset is not valid for the BarIndex of this PCI controller.
+  @retval EFI_TIMEOUT           Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoPollIo (
+  IN  EFI_PCI_IO_PROTOCOL        *This,
+  IN  EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT8                      BarIndex,
+  IN  UINT64                     Offset,
+  IN  UINT64                     Mask,
+  IN  UINT64                     Value,
+  IN  UINT64                     Delay,
+  OUT UINT64                     *Result
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMemWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in the PCI memory or I/O space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory or I/O operations.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for the memory or I/O operation to perform.
+  @param  Offset                The offset within the selected BAR to start the memory or I/O operation.
+  @param  Count                 The number of memory or I/O operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI BAR specified by BarIndex.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoIoWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT8                      BarIndex,
+  IN     UINT64                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigRead (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enable a PCI driver to access PCI controller registers in PCI configuration space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the PCI configuration space for the PCI controller.
+  @param  Count                 The number of PCI configuration operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results. For write
+                                operations, the source buffer to write data from.
+
+
+  @retval EFI_SUCCESS           The data was read from or written to the PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the PCI configuration header of the PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER Buffer is NULL or Width is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoConfigWrite (
+  IN     EFI_PCI_IO_PROTOCOL        *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT32                     Offset,
+  IN     UINTN                      Count,
+  IN OUT VOID                       *Buffer
+  );
+
+/**
+  Enables a PCI driver to copy one region of PCI memory space to another region of PCI
+  memory space.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  DestBarIndex          The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  DestOffset            The destination offset within the BAR specified by DestBarIndex to
+                                start the memory writes for the copy operation.
+  @param  SrcBarIndex           The BAR index in the standard PCI Configuration header to use as the
+                                base address for the memory operation to perform.
+  @param  SrcOffset             The source offset within the BAR specified by SrcBarIndex to start
+                                the memory reads for the copy operation.
+  @param  Count                 The number of memory operations to perform. Bytes moved is Width
+                                size * Count, starting at DestOffset and SrcOffset.
+
+  @retval EFI_SUCCESS           The data was copied from one memory region to another memory region.
+  @retval EFI_UNSUPPORTED       DestBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       SrcBarIndex not valid for this PCI controller.
+  @retval EFI_UNSUPPORTED       The address range specified by DestOffset, Width, and Count
+                                is not valid for the PCI BAR specified by DestBarIndex.
+  @retval EFI_UNSUPPORTED       The address range specified by SrcOffset, Width, and Count is
+                                not valid for the PCI BAR specified by SrcBarIndex.
+  @retval EFI_INVALID_PARAMETER Width is invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoCopyMem (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     EFI_PCI_IO_PROTOCOL_WIDTH    Width,
+  IN     UINT8                        DestBarIndex,
+  IN     UINT64                       DestOffset,
+  IN     UINT8                        SrcBarIndex,
+  IN     UINT64                       SrcOffset,
+  IN     UINTN                        Count
+  );
+
+/**
+  Provides the PCI controller-specific addresses needed to access system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to use to
+                                access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoMap (
+  IN     EFI_PCI_IO_PROTOCOL            *This,
+  IN     EFI_PCI_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                           *HostAddress,
+  IN OUT UINTN                          *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS           *DeviceAddress,
+  OUT    VOID                           **Mapping
+  );
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Mapping               The mapping value returned from Map().
+
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoUnmap (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  IN  VOID                 *Mapping
+  );
+
+/**
+  Allocates pages that are suitable for an EfiPciIoOperationBusMasterCommonBuffer
+  mapping.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.
+  @param  Pages                 The number of pages to allocate.
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAllocateBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 Pages,
+  OUT VOID                  **HostAddress,
+  IN  UINT64                Attributes
+  );
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Pages                 The number of pages to free.
+  @param  HostAddress           The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFreeBuffer (
+  IN  EFI_PCI_IO_PROTOCOL   *This,
+  IN  UINTN                 Pages,
+  IN  VOID                  *HostAddress
+  );
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system memory.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The PCI posted write transactions were flushed from the PCI host
+                                bridge to system memory.
+  @retval EFI_DEVICE_ERROR      The PCI posted write transactions were not flushed from the PCI
+                                host bridge due to a hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoFlush (
+  IN  EFI_PCI_IO_PROTOCOL  *This
+  );
+
+/**
+  Retrieves this PCI controller's current PCI bus number, device number, and function number.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  SegmentNumber         The PCI controller's current PCI segment number.
+  @param  BusNumber             The PCI controller's current PCI bus number.
+  @param  DeviceNumber          The PCI controller's current PCI device number.
+  @param  FunctionNumber        The PCI controller's current PCI function number.
+
+  @retval EFI_SUCCESS           The PCI controller location was returned.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetLocation (
+  IN  EFI_PCI_IO_PROTOCOL  *This,
+  OUT UINTN                *Segment,
+  OUT UINTN                *Bus,
+  OUT UINTN                *Device,
+  OUT UINTN                *Function
+  );
+
+/**
+  Check BAR type for PCI resource.
+
+  @param PciIoDevice   PCI device instance.
+  @param BarIndex      The BAR index of the standard PCI Configuration header to use as the
+                       base address for the memory or I/O operation to perform.
+  @param BarType       Memory or I/O.
+
+  @retval TRUE         Pci device's bar type is same with input BarType.
+  @retval TRUE         Pci device's bar type is not same with input BarType.
+
+**/
+BOOLEAN
+CheckBarType (
+  IN PCI_IO_DEVICE          *PciIoDevice,
+  IN UINT8                  BarIndex,
+  IN PCI_BAR_TYPE           BarType
+  );
+
+/**
+  Set/Disable new attributes to a Root Bridge.
+
+  @param  PciIoDevice  Pci device instance.
+  @param  Attributes   New attribute want to be set.
+  @param  Operation    Set or Disable.
+
+  @retval  EFI_UNSUPPORTED  If root bridge does not support change attribute.
+  @retval  EFI_SUCCESS      Successfully set new attributs.
+
+**/
+EFI_STATUS
+ModifyRootBridgeAttributes (
+  IN  PCI_IO_DEVICE                            *PciIoDevice,
+  IN  UINT64                                   Attributes,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  );
+
+/**
+  Check whether this device can be enable/disable to snoop.
+
+  @param PciIoDevice  Pci device instance.
+  @param Operation    Enable/Disable.
+
+  @retval EFI_UNSUPPORTED  Pci device is not GFX device or not support snoop.
+  @retval EFI_SUCCESS      Snoop can be supported.
+
+**/
+EFI_STATUS
+SupportPaletteSnoopAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation
+  );
+
+/**
+  Performs an operation on the attributes that this PCI controller supports. The operations include
+  getting the set of supported attributes, retrieving the current attributes, setting the current
+  attributes, enabling attributes, and disabling attributes.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Operation             The operation to perform on the attributes for this PCI controller.
+  @param  Attributes            The mask of attributes that are used for Set, Enable, and Disable
+                                operations.
+  @param  Result                A pointer to the result mask of attributes that are returned for the Get
+                                and Supported operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoAttributes (
+  IN EFI_PCI_IO_PROTOCOL                       * This,
+  IN  EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN  UINT64                                   Attributes,
+  OUT UINT64                                   *Result OPTIONAL
+  );
+
+/**
+  Gets the attributes that this PCI controller supports setting on a BAR using
+  SetBarAttributes(), and retrieves the list of resource descriptors for a BAR.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Supports              A pointer to the mask of attributes that this PCI controller supports
+                                setting for this BAR with SetBarAttributes().
+  @param  Resources             A pointer to the ACPI 2.0 resource descriptors that describe the current
+                                configuration of this BAR of the PCI controller.
+
+  @retval EFI_SUCCESS           If Supports is not NULL, then the attributes that the PCI
+                                controller supports are returned in Supports. If Resources
+                                is not NULL, then the ACPI 2.0 resource descriptors that the PCI
+                                controller is currently using are returned in Resources.
+  @retval EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources available to allocate
+                                Resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoGetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL             * This,
+  IN  UINT8                          BarIndex,
+  OUT UINT64                         *Supports, OPTIONAL
+  OUT VOID                           **Resources OPTIONAL
+  );
+
+/**
+  Sets the attributes for a range of a BAR on a PCI controller.
+
+  @param  This                  A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param  Attributes            The mask of attributes to set for the resource range specified by
+                                BarIndex, Offset, and Length.
+  @param  BarIndex              The BAR index of the standard PCI Configuration header to use as the
+                                base address for resource range. The legal range for this field is 0..5.
+  @param  Offset                A pointer to the BAR relative base address of the resource range to be
+                                modified by the attributes specified by Attributes.
+  @param  Length                A pointer to the length of the resource range to be modified by the
+                                attributes specified by Attributes.
+
+  @retval EFI_SUCCESS           The set of attributes specified by Attributes for the resource
+                                range specified by BarIndex, Offset, and Length were
+                                set on the PCI controller, and the actual resource range is returned
+                                in Offset and Length.
+  @retval EFI_INVALID_PARAMETER Offset or Length is NULL.
+  @retval EFI_UNSUPPORTED       BarIndex not valid for this PCI controller.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to set the attributes on the
+                                resource range specified by BarIndex, Offset, and
+                                Length.
+
+**/
+EFI_STATUS
+EFIAPI
+PciIoSetBarAttributes (
+  IN EFI_PCI_IO_PROTOCOL              *This,
+  IN     UINT64                       Attributes,
+  IN     UINT8                        BarIndex,
+  IN OUT UINT64                       *Offset,
+  IN OUT UINT64                       *Length
+  );
+
+/**
+  Program parent bridge's attribute recurrently.
+
+  @param PciIoDevice  Child Pci device instance
+  @param Operation    The operation to perform on the attributes for this PCI controller.
+  @param Attributes   The mask of attributes that are used for Set, Enable, and Disable
+                      operations.
+
+  @retval EFI_SUCCESS           The operation on the PCI controller's attributes was completed.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED       one or more of the bits set in
+                                Attributes are not supported by this PCI controller or one of
+                                its parent bridges when Operation is Set, Enable or Disable.
+
+**/
+EFI_STATUS
+UpStreamBridgesAttributes (
+  IN PCI_IO_DEVICE                            *PciIoDevice,
+  IN EFI_PCI_IO_PROTOCOL_ATTRIBUTE_OPERATION  Operation,
+  IN UINT64                                   Attributes
+  );
+
+/**
+  Test whether two Pci devices has same parent bridge.
+
+  @param PciDevice1  The first pci device for testing.
+  @param PciDevice2  The second pci device for testing.
+
+  @retval TRUE       Two Pci device has the same parent bridge.
+  @retval FALSE      Two Pci device has not the same parent bridge.
+
+**/
+BOOLEAN
+PciDevicesOnTheSamePath (
+  IN PCI_IO_DEVICE        *PciDevice1,
+  IN PCI_IO_DEVICE        *PciDevice2
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c
new file mode 100644
index 0000000000..f3407bcb63
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.c
@@ -0,0 +1,1649 @@
+/** @file
+  Internal library implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2016, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett Packard Enterprise Development LP<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 "PciBus.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+CHAR16 *mBarTypeStr[] = {
+  L"Unknow",
+  L"  Io16",
+  L"  Io32",
+  L" Mem32",
+  L"PMem32",
+  L" Mem64",
+  L"PMem64",
+  L"    Io",
+  L"   Mem",
+  L"Unknow"
+  };
+
+/**
+  Retrieve the PCI Card device BAR information via PciIo interface.
+
+  @param PciIoDevice        PCI Card device instance.
+
+**/
+VOID
+GetBackPcCardBar (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  )
+{
+  UINT32  Address;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return;
+  }
+
+  //
+  // Read PciBar information from the bar register
+  //
+  if (!gFullEnumeration) {
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_MEMORY_BASE_0,
+                             1,
+                             &Address
+                             );
+
+    (PciIoDevice->PciBar)[P2C_MEM_1].BaseAddress  = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_MEM_1].Length       = 0x2000000;
+    (PciIoDevice->PciBar)[P2C_MEM_1].BarType      = PciBarTypeMem32;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_MEMORY_BASE_1,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_MEM_2].BaseAddress  = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_MEM_2].Length       = 0x2000000;
+    (PciIoDevice->PciBar)[P2C_MEM_2].BarType      = PciBarTypePMem32;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_IO_BASE_0_LOWER,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_IO_1].BaseAddress = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_IO_1].Length      = 0x100;
+    (PciIoDevice->PciBar)[P2C_IO_1].BarType     = PciBarTypeIo16;
+
+    Address = 0;
+    PciIoDevice->PciIo.Pci.Read (
+                             &(PciIoDevice->PciIo),
+                             EfiPciIoWidthUint32,
+                             PCI_CARD_IO_BASE_1_LOWER,
+                             1,
+                             &Address
+                             );
+    (PciIoDevice->PciBar)[P2C_IO_2].BaseAddress = (UINT64) (Address);
+    (PciIoDevice->PciBar)[P2C_IO_2].Length      = 0x100;
+    (PciIoDevice->PciBar)[P2C_IO_2].BarType     = PciBarTypeIo16;
+
+  }
+
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    GetResourcePaddingForHpb (PciIoDevice);
+  }
+}
+
+/**
+  Remove rejected pci device from specific root bridge
+  handle.
+
+  @param RootBridgeHandle  Specific parent root bridge handle.
+  @param Bridge            Bridge device instance.
+
+**/
+VOID
+RemoveRejectedPciDevices (
+  IN EFI_HANDLE        RootBridgeHandle,
+  IN PCI_IO_DEVICE     *Bridge
+  )
+{
+  PCI_IO_DEVICE   *Temp;
+  LIST_ENTRY      *CurrentLink;
+  LIST_ENTRY      *LastLink;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return;
+  }
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+      //
+      // Remove rejected devices recusively
+      //
+      RemoveRejectedPciDevices (RootBridgeHandle, Temp);
+    } else {
+      //
+      // Skip rejection for all PPBs, while detect rejection for others
+      //
+      if (IsPciDeviceRejected (Temp)) {
+
+        //
+        // For P2C, remove all devices on it
+        //
+        if (!IsListEmpty (&Temp->ChildList)) {
+          RemoveAllPciDeviceOnBridge (RootBridgeHandle, Temp);
+        }
+
+        //
+        // Finally remove itself
+        //
+        LastLink = CurrentLink->BackLink;
+        RemoveEntryList (CurrentLink);
+        FreePciDevice (Temp);
+
+        CurrentLink = LastLink;
+      }
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+}
+
+/**
+  Dump the resourc map of the bridge device.
+
+  @param[in] BridgeResource   Resource descriptor of the bridge device.
+**/
+VOID
+DumpBridgeResource (
+  IN PCI_RESOURCE_NODE     *BridgeResource
+  )
+{
+  LIST_ENTRY               *Link;
+  PCI_RESOURCE_NODE        *Resource;
+  PCI_BAR                  *Bar;
+
+  if ((BridgeResource != NULL) && (BridgeResource->Length != 0)) {
+    DEBUG ((
+      EFI_D_INFO, "Type = %s; Base = 0x%lx;\tLength = 0x%lx;\tAlignment = 0x%lx\n",
+      mBarTypeStr[MIN (BridgeResource->ResType, PciBarTypeMaxType)],
+      BridgeResource->PciDev->PciBar[BridgeResource->Bar].BaseAddress,
+      BridgeResource->Length, BridgeResource->Alignment
+      ));
+    for ( Link = GetFirstNode (&BridgeResource->ChildList)
+        ; !IsNull (&BridgeResource->ChildList, Link)
+        ; Link = GetNextNode (&BridgeResource->ChildList, Link)
+        ) {
+      Resource = RESOURCE_NODE_FROM_LINK (Link);
+      if (Resource->ResourceUsage == PciResUsageTypical) {
+        Bar = Resource->Virtual ? Resource->PciDev->VfPciBar : Resource->PciDev->PciBar;
+        DEBUG ((
+          EFI_D_INFO, "   Base = 0x%lx;\tLength = 0x%lx;\tAlignment = 0x%lx;\tOwner = %s [%02x|%02x|%02x:",
+          Bar[Resource->Bar].BaseAddress, Resource->Length, Resource->Alignment,
+          IS_PCI_BRIDGE (&Resource->PciDev->Pci)     ? L"PPB" :
+          IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci) ? L"P2C" :
+                                                       L"PCI",
+          Resource->PciDev->BusNumber, Resource->PciDev->DeviceNumber,
+          Resource->PciDev->FunctionNumber
+          ));
+
+        if ((!IS_PCI_BRIDGE (&Resource->PciDev->Pci) && !IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci)) ||
+            (IS_PCI_BRIDGE (&Resource->PciDev->Pci) && (Resource->Bar < PPB_IO_RANGE)) ||
+            (IS_CARDBUS_BRIDGE (&Resource->PciDev->Pci) && (Resource->Bar < P2C_MEM_1))
+            ) {
+          //
+          // The resource requirement comes from the device itself.
+          //
+          DEBUG ((EFI_D_INFO, "%02x]", Bar[Resource->Bar].Offset));
+        } else {
+          //
+          // The resource requirement comes from the subordinate devices.
+          //
+          DEBUG ((EFI_D_INFO, "**]"));
+        }
+      } else {
+        DEBUG ((EFI_D_INFO, "   Base = Padding;\tLength = 0x%lx;\tAlignment = 0x%lx", Resource->Length, Resource->Alignment));
+      }
+      if (BridgeResource->ResType != Resource->ResType) {
+        DEBUG ((EFI_D_INFO, "; Type = %s", mBarTypeStr[MIN (Resource->ResType, PciBarTypeMaxType)]));
+      }
+      DEBUG ((EFI_D_INFO, "\n"));
+    }
+  }
+}
+
+/**
+  Find the corresponding resource node for the Device in child list of BridgeResource.
+  
+  @param[in]  Device          Pointer to PCI_IO_DEVICE.
+  @param[in]  BridgeResource  Pointer to PCI_RESOURCE_NODE.
+  @param[out] DeviceResources Pointer to a buffer to receive resources for the Device.
+  
+  @return Count of the resource descriptors returned.
+**/
+UINTN
+FindResourceNode (
+  IN  PCI_IO_DEVICE     *Device,
+  IN  PCI_RESOURCE_NODE *BridgeResource,
+  OUT PCI_RESOURCE_NODE **DeviceResources OPTIONAL
+  )
+{
+  LIST_ENTRY               *Link;
+  PCI_RESOURCE_NODE        *Resource;
+  UINTN                    Count;
+
+  Count = 0;
+  for ( Link = BridgeResource->ChildList.ForwardLink
+      ; Link != &BridgeResource->ChildList
+      ; Link = Link->ForwardLink
+      ) {
+    Resource = RESOURCE_NODE_FROM_LINK (Link);
+    if (Resource->PciDev == Device) {
+      if (DeviceResources != NULL) {
+        DeviceResources[Count] = Resource;
+      }
+      Count++;
+    }
+  }
+
+  return Count;
+}
+
+/**
+  Dump the resource map of all the devices under Bridge.
+  
+  @param[in] Bridge        Bridge device instance.
+  @param[in] Resources     Resource descriptors for the bridge device.
+  @param[in] ResourceCount Count of resource descriptors.
+**/
+VOID
+DumpResourceMap (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE **Resources,
+  IN UINTN             ResourceCount
+  )
+{
+  EFI_STATUS           Status;
+  LIST_ENTRY           *Link;
+  PCI_IO_DEVICE        *Device;
+  UINTN                Index;
+  CHAR16               *Str;
+  PCI_RESOURCE_NODE    **ChildResources;
+  UINTN                ChildResourceCount;
+
+  DEBUG ((EFI_D_INFO, "PciBus: Resource Map for "));
+
+  Status = gBS->OpenProtocol (
+                  Bridge->Handle,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  NULL,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((
+      EFI_D_INFO, "Bridge [%02x|%02x|%02x]\n",
+      Bridge->BusNumber, Bridge->DeviceNumber, Bridge->FunctionNumber
+      ));
+  } else {
+    Str = ConvertDevicePathToText (
+            DevicePathFromHandle (Bridge->Handle),
+            FALSE,
+            FALSE
+            );
+    DEBUG ((EFI_D_INFO, "Root Bridge %s\n", Str != NULL ? Str : L""));
+    if (Str != NULL) {
+      FreePool (Str);
+    }
+  }
+
+  for (Index = 0; Index < ResourceCount; Index++) {
+    DumpBridgeResource (Resources[Index]);
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+
+  for ( Link = Bridge->ChildList.ForwardLink
+      ; Link != &Bridge->ChildList
+      ; Link = Link->ForwardLink
+      ) {
+    Device = PCI_IO_DEVICE_FROM_LINK (Link);
+    if (IS_PCI_BRIDGE (&Device->Pci)) {
+
+      ChildResourceCount = 0;
+      for (Index = 0; Index < ResourceCount; Index++) {
+        ChildResourceCount += FindResourceNode (Device, Resources[Index], NULL);
+      }
+      ChildResources = AllocatePool (sizeof (PCI_RESOURCE_NODE *) * ChildResourceCount);
+      ASSERT (ChildResources != NULL);
+      ChildResourceCount = 0;
+      for (Index = 0; Index < ResourceCount; Index++) {
+        ChildResourceCount += FindResourceNode (Device, Resources[Index], &ChildResources[ChildResourceCount]);
+      }
+
+      DumpResourceMap (Device, ChildResources, ChildResourceCount);
+      FreePool (ChildResources);
+    }
+  }
+}
+
+/**
+  Submits the I/O and memory resource requirements for the specified PCI Host Bridge.
+
+  @param PciResAlloc  Point to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS           Successfully finished resource allocation.
+  @retval EFI_NOT_FOUND         Cannot get root bridge instance.
+  @retval EFI_OUT_OF_RESOURCES  Platform failed to program the resources if no hot plug supported.
+  @retval other                 Some error occurred when allocating resources for the PCI Host Bridge.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciHostBridgeResourceAllocator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  PCI_IO_DEVICE                                  *RootBridgeDev;
+  EFI_HANDLE                                     RootBridgeHandle;
+  VOID                                           *AcpiConfig;
+  EFI_STATUS                                     Status;
+  UINT64                                         IoBase;
+  UINT64                                         Mem32Base;
+  UINT64                                         PMem32Base;
+  UINT64                                         Mem64Base;
+  UINT64                                         PMem64Base;
+  UINT64                                         IoResStatus;
+  UINT64                                         Mem32ResStatus;
+  UINT64                                         PMem32ResStatus;
+  UINT64                                         Mem64ResStatus;
+  UINT64                                         PMem64ResStatus;
+  UINT64                                         MaxOptionRomSize;
+  PCI_RESOURCE_NODE                              *IoBridge;
+  PCI_RESOURCE_NODE                              *Mem32Bridge;
+  PCI_RESOURCE_NODE                              *PMem32Bridge;
+  PCI_RESOURCE_NODE                              *Mem64Bridge;
+  PCI_RESOURCE_NODE                              *PMem64Bridge;
+  PCI_RESOURCE_NODE                              IoPool;
+  PCI_RESOURCE_NODE                              Mem32Pool;
+  PCI_RESOURCE_NODE                              PMem32Pool;
+  PCI_RESOURCE_NODE                              Mem64Pool;
+  PCI_RESOURCE_NODE                              PMem64Pool;
+  BOOLEAN                                        ReAllocate;
+  EFI_DEVICE_HANDLE_EXTENDED_DATA_PAYLOAD        HandleExtendedData;
+  EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD  AllocFailExtendedData;
+
+  //
+  // Reallocate flag
+  //
+  ReAllocate = FALSE;
+
+  //
+  // It may try several times if the resource allocation fails
+  //
+  while (TRUE) {
+    //
+    // Initialize resource pool
+    //
+    InitializeResourcePool (&IoPool, PciBarTypeIo16);
+    InitializeResourcePool (&Mem32Pool, PciBarTypeMem32);
+    InitializeResourcePool (&PMem32Pool, PciBarTypePMem32);
+    InitializeResourcePool (&Mem64Pool, PciBarTypeMem64);
+    InitializeResourcePool (&PMem64Pool, PciBarTypePMem64);
+
+    RootBridgeDev     = NULL;
+    RootBridgeHandle  = 0;
+
+    while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+      //
+      // Get Root Bridge Device by handle
+      //
+      RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+      if (RootBridgeDev == NULL) {
+        return EFI_NOT_FOUND;
+      }
+
+      //
+      // Create the entire system resource map from the information collected by
+      // enumerator. Several resource tree was created
+      //
+
+      //
+      // If non-stardard PCI Bridge I/O window alignment is supported,
+      // set I/O aligment to minimum possible alignment for root bridge.
+      //
+      IoBridge = CreateResourceNode (
+                   RootBridgeDev,
+                   0,
+                   FeaturePcdGet (PcdPciBridgeIoAlignmentProbe) ? 0x1FF: 0xFFF,
+                   RB_IO_RANGE,
+                   PciBarTypeIo16,
+                   PciResUsageTypical
+                   );
+
+      Mem32Bridge = CreateResourceNode (
+                      RootBridgeDev,
+                      0,
+                      0xFFFFF,
+                      RB_MEM32_RANGE,
+                      PciBarTypeMem32,
+                      PciResUsageTypical
+                      );
+
+      PMem32Bridge = CreateResourceNode (
+                       RootBridgeDev,
+                       0,
+                       0xFFFFF,
+                       RB_PMEM32_RANGE,
+                       PciBarTypePMem32,
+                       PciResUsageTypical
+                       );
+
+      Mem64Bridge = CreateResourceNode (
+                      RootBridgeDev,
+                      0,
+                      0xFFFFF,
+                      RB_MEM64_RANGE,
+                      PciBarTypeMem64,
+                      PciResUsageTypical
+                      );
+
+      PMem64Bridge = CreateResourceNode (
+                       RootBridgeDev,
+                       0,
+                       0xFFFFF,
+                       RB_PMEM64_RANGE,
+                       PciBarTypePMem64,
+                       PciResUsageTypical
+                       );
+
+      //
+      // Create resourcemap by going through all the devices subject to this root bridge
+      //
+      CreateResourceMap (
+        RootBridgeDev,
+        IoBridge,
+        Mem32Bridge,
+        PMem32Bridge,
+        Mem64Bridge,
+        PMem64Bridge
+        );
+
+      //
+      // Get the max ROM size that the root bridge can process
+      //
+      RootBridgeDev->RomSize = Mem32Bridge->Length;
+
+      //
+      // Skip to enlarge the resource request during realloction
+      //
+      if (!ReAllocate) {
+        //
+        // Get Max Option Rom size for current root bridge
+        //
+        MaxOptionRomSize = GetMaxOptionRomSize (RootBridgeDev);
+
+        //
+        // Enlarger the mem32 resource to accomdate the option rom
+        // if the mem32 resource is not enough to hold the rom
+        //
+        if (MaxOptionRomSize > Mem32Bridge->Length) {
+
+          Mem32Bridge->Length     = MaxOptionRomSize;
+          RootBridgeDev->RomSize  = MaxOptionRomSize;
+
+          //
+          // Alignment should be adjusted as well
+          //
+          if (Mem32Bridge->Alignment < MaxOptionRomSize - 1) {
+            Mem32Bridge->Alignment = MaxOptionRomSize - 1;
+          }
+        }
+      }
+
+      //
+      // Based on the all the resource tree, contruct ACPI resource node to
+      // submit the resource aperture to pci host bridge protocol
+      //
+      Status = ConstructAcpiResourceRequestor (
+                 RootBridgeDev,
+                 IoBridge,
+                 Mem32Bridge,
+                 PMem32Bridge,
+                 Mem64Bridge,
+                 PMem64Bridge,
+                 &AcpiConfig
+                 );
+
+      //
+      // Insert these resource nodes into the database
+      //
+      InsertResourceNode (&IoPool, IoBridge);
+      InsertResourceNode (&Mem32Pool, Mem32Bridge);
+      InsertResourceNode (&PMem32Pool, PMem32Bridge);
+      InsertResourceNode (&Mem64Pool, Mem64Bridge);
+      InsertResourceNode (&PMem64Pool, PMem64Bridge);
+
+      if (Status == EFI_SUCCESS) {
+        //
+        // Submit the resource requirement
+        //
+        Status = PciResAlloc->SubmitResources (
+                                PciResAlloc,
+                                RootBridgeDev->Handle,
+                                AcpiConfig
+                                );
+        //
+        // If SubmitResources returns error, PciBus isn't able to start.
+        // It's a fatal error so assertion is added.
+        //
+        DEBUG ((EFI_D_INFO, "PciBus: HostBridge->SubmitResources() - %r\n", Status));
+        ASSERT_EFI_ERROR (Status);
+      }
+
+      //
+      // Free acpi resource node
+      //
+      if (AcpiConfig != NULL) {
+        FreePool (AcpiConfig);
+      }
+
+      if (EFI_ERROR (Status)) {
+        //
+        // Destroy all the resource tree
+        //
+        DestroyResourceTree (&IoPool);
+        DestroyResourceTree (&Mem32Pool);
+        DestroyResourceTree (&PMem32Pool);
+        DestroyResourceTree (&Mem64Pool);
+        DestroyResourceTree (&PMem64Pool);
+        return Status;
+      }
+    }
+    //
+    // End while, at least one Root Bridge should be found.
+    //
+    ASSERT (RootBridgeDev != NULL);
+
+    //
+    // Notify platform to start to program the resource
+    //
+    Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeAllocateResources);
+    DEBUG ((EFI_D_INFO, "PciBus: HostBridge->NotifyPhase(AllocateResources) - %r\n", Status));
+    if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+      //
+      // If Hot Plug is not supported
+      //
+      if (EFI_ERROR (Status)) {
+        //
+        // Allocation failed, then return
+        //
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Allocation succeed.
+      // Get host bridge handle for status report, and then skip the main while
+      //
+      HandleExtendedData.Handle = RootBridgeDev->PciRootBridgeIo->ParentHandle;
+
+      break;
+
+    } else {
+      //
+      // If Hot Plug is supported
+      //
+      if (!EFI_ERROR (Status)) {
+        //
+        // Allocation succeed, then continue the following
+        //
+        break;
+      }
+
+      //
+      // If the resource allocation is unsuccessful, free resources on bridge
+      //
+
+      RootBridgeDev     = NULL;
+      RootBridgeHandle  = 0;
+
+      IoResStatus       = EFI_RESOURCE_SATISFIED;
+      Mem32ResStatus    = EFI_RESOURCE_SATISFIED;
+      PMem32ResStatus   = EFI_RESOURCE_SATISFIED;
+      Mem64ResStatus    = EFI_RESOURCE_SATISFIED;
+      PMem64ResStatus   = EFI_RESOURCE_SATISFIED;
+
+      while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+        //
+        // Get RootBridg Device by handle
+        //
+        RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+        if (RootBridgeDev == NULL) {
+          return EFI_NOT_FOUND;
+        }
+
+        //
+        // Get host bridge handle for status report
+        //
+        HandleExtendedData.Handle = RootBridgeDev->PciRootBridgeIo->ParentHandle;
+
+        //
+        // Get acpi resource node for all the resource types
+        //
+        AcpiConfig = NULL;
+
+        Status = PciResAlloc->GetProposedResources (
+                                PciResAlloc,
+                                RootBridgeDev->Handle,
+                                &AcpiConfig
+                                );
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        if (AcpiConfig != NULL) {
+          //
+          // Adjust resource allocation policy for each RB
+          //
+          GetResourceAllocationStatus (
+            AcpiConfig,
+            &IoResStatus,
+            &Mem32ResStatus,
+            &PMem32ResStatus,
+            &Mem64ResStatus,
+            &PMem64ResStatus
+            );
+          FreePool (AcpiConfig);
+        }
+      }
+      //
+      // End while
+      //
+
+      //
+      // Raise the EFI_IOB_EC_RESOURCE_CONFLICT status code
+      //
+      //
+      // It is very difficult to follow the spec here
+      // Device path , Bar index can not be get here
+      //
+      ZeroMem (&AllocFailExtendedData, sizeof (AllocFailExtendedData));
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+            EFI_PROGRESS_CODE,
+            EFI_IO_BUS_PCI | EFI_IOB_EC_RESOURCE_CONFLICT,
+            (VOID *) &AllocFailExtendedData,
+            sizeof (AllocFailExtendedData)
+            );
+
+      Status = PciHostBridgeAdjustAllocation (
+                 &IoPool,
+                 &Mem32Pool,
+                 &PMem32Pool,
+                 &Mem64Pool,
+                 &PMem64Pool,
+                 IoResStatus,
+                 Mem32ResStatus,
+                 PMem32ResStatus,
+                 Mem64ResStatus,
+                 PMem64ResStatus
+                 );
+
+      //
+      // Destroy all the resource tree
+      //
+      DestroyResourceTree (&IoPool);
+      DestroyResourceTree (&Mem32Pool);
+      DestroyResourceTree (&PMem32Pool);
+      DestroyResourceTree (&Mem64Pool);
+      DestroyResourceTree (&PMem64Pool);
+
+      NotifyPhase (PciResAlloc, EfiPciHostBridgeFreeResources);
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      ReAllocate = TRUE;
+    }
+  }
+  //
+  // End main while
+  //
+
+  //
+  // Raise the EFI_IOB_PCI_RES_ALLOC status code
+  //
+  REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+        EFI_PROGRESS_CODE,
+        EFI_IO_BUS_PCI | EFI_IOB_PCI_RES_ALLOC,
+        (VOID *) &HandleExtendedData,
+        sizeof (HandleExtendedData)
+        );
+
+  //
+  // Notify pci bus driver starts to program the resource
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeSetResources);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  RootBridgeDev     = NULL;
+
+  RootBridgeHandle  = 0;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Get acpi resource node for all the resource types
+    //
+    AcpiConfig = NULL;
+    Status = PciResAlloc->GetProposedResources (
+                            PciResAlloc,
+                            RootBridgeDev->Handle,
+                            &AcpiConfig
+                            );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the resource base by interpreting acpi resource node
+    //
+    //
+    GetResourceBase (
+      AcpiConfig,
+      &IoBase,
+      &Mem32Base,
+      &PMem32Base,
+      &Mem64Base,
+      &PMem64Base
+      );
+
+    //
+    // Process option rom for this root bridge
+    //
+    ProcessOptionRom (RootBridgeDev, Mem32Base, RootBridgeDev->RomSize);
+
+    //
+    // Create the entire system resource map from the information collected by
+    // enumerator. Several resource tree was created
+    //
+    FindResourceNode (RootBridgeDev, &IoPool, &IoBridge);
+    FindResourceNode (RootBridgeDev, &Mem32Pool, &Mem32Bridge);
+    FindResourceNode (RootBridgeDev, &PMem32Pool, &PMem32Bridge);
+    FindResourceNode (RootBridgeDev, &Mem64Pool, &Mem64Bridge);
+    FindResourceNode (RootBridgeDev, &PMem64Pool, &PMem64Bridge);
+
+    ASSERT (IoBridge     != NULL);
+    ASSERT (Mem32Bridge  != NULL);
+    ASSERT (PMem32Bridge != NULL);
+    ASSERT (Mem64Bridge  != NULL);
+    ASSERT (PMem64Bridge != NULL);
+
+    //
+    // Program IO resources
+    //
+    ProgramResource (
+      IoBase,
+      IoBridge
+      );
+
+    //
+    // Program Mem32 resources
+    //
+    ProgramResource (
+      Mem32Base,
+      Mem32Bridge
+      );
+
+    //
+    // Program PMem32 resources
+    //
+    ProgramResource (
+      PMem32Base,
+      PMem32Bridge
+      );
+
+    //
+    // Program Mem64 resources
+    //
+    ProgramResource (
+      Mem64Base,
+      Mem64Bridge
+      );
+
+    //
+    // Program PMem64 resources
+    //
+    ProgramResource (
+      PMem64Base,
+      PMem64Bridge
+      );
+
+    IoBridge    ->PciDev->PciBar[IoBridge    ->Bar].BaseAddress = IoBase;
+    Mem32Bridge ->PciDev->PciBar[Mem32Bridge ->Bar].BaseAddress = Mem32Base;
+    PMem32Bridge->PciDev->PciBar[PMem32Bridge->Bar].BaseAddress = PMem32Base;
+    Mem64Bridge ->PciDev->PciBar[Mem64Bridge ->Bar].BaseAddress = Mem64Base;
+    PMem64Bridge->PciDev->PciBar[PMem64Bridge->Bar].BaseAddress = PMem64Base;
+
+    //
+    // Dump the resource map for current root bridge
+    //
+    DEBUG_CODE (
+      PCI_RESOURCE_NODE *Resources[5];
+      Resources[0] = IoBridge;
+      Resources[1] = Mem32Bridge;
+      Resources[2] = PMem32Bridge;
+      Resources[3] = Mem64Bridge;
+      Resources[4] = PMem64Bridge;
+      DumpResourceMap (RootBridgeDev, Resources, sizeof (Resources) / sizeof (Resources[0]));
+    );
+
+    FreePool (AcpiConfig);
+  }
+
+  //
+  // Destroy all the resource tree
+  //
+  DestroyResourceTree (&IoPool);
+  DestroyResourceTree (&Mem32Pool);
+  DestroyResourceTree (&PMem32Pool);
+  DestroyResourceTree (&Mem64Pool);
+  DestroyResourceTree (&PMem64Pool);
+
+  //
+  // Notify the resource allocation phase is to end
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeEndResourceAllocation);
+
+  return Status;
+}
+
+/**
+  Allocate NumberOfBuses buses and return the next available PCI bus number.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   Current available PCI bus number.
+  @param  NumberOfBuses    Number of buses enumerated below the StartBusNumber.
+  @param  NextBusNumber    Next available PCI bus number.
+
+  @retval EFI_SUCCESS           Available bus number resource is enough. Next available PCI bus number
+                                is returned in NextBusNumber.
+  @retval EFI_OUT_OF_RESOURCES  Available bus number resource is not enough for allocation.
+
+**/
+EFI_STATUS
+PciAllocateBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  IN UINT8                              NumberOfBuses,
+  OUT UINT8                             *NextBusNumber
+  )
+{
+  PCI_IO_DEVICE                      *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR  *BusNumberRanges;
+  UINT8                              NextNumber;
+  UINT64                             MaxNumberInRange;
+
+  //
+  // Get PCI Root Bridge device
+  //
+  RootBridge = Bridge;
+  while (RootBridge->Parent != NULL) {
+    RootBridge = RootBridge->Parent;
+  }
+
+  //
+  // Get next available PCI bus number
+  //
+  BusNumberRanges = RootBridge->BusNumberRanges;
+  while (BusNumberRanges->Desc != ACPI_END_TAG_DESCRIPTOR) {
+    MaxNumberInRange = BusNumberRanges->AddrRangeMin + BusNumberRanges->AddrLen - 1;
+    if (StartBusNumber >= BusNumberRanges->AddrRangeMin && StartBusNumber <=  MaxNumberInRange) {
+      NextNumber = (UINT8)(StartBusNumber + NumberOfBuses);
+      while (NextNumber > MaxNumberInRange) {
+        ++BusNumberRanges;
+        if (BusNumberRanges->Desc == ACPI_END_TAG_DESCRIPTOR) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+        NextNumber = (UINT8)(NextNumber + (BusNumberRanges->AddrRangeMin - (MaxNumberInRange + 1)));
+        MaxNumberInRange = BusNumberRanges->AddrRangeMin + BusNumberRanges->AddrLen - 1;
+      }
+      *NextBusNumber = NextNumber;
+      return EFI_SUCCESS;
+    }
+    BusNumberRanges++;
+  }
+  return EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Scan pci bus and assign bus number to the given PCI bus system.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   start point.
+  @param  SubBusNumber     Point to sub bus number.
+  @param  PaddedBusRange   Customized bus number.
+
+  @retval EFI_SUCCESS      Successfully scanned and assigned bus number.
+  @retval other            Some error occurred when scanning pci bus.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciScanBus (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber,
+  OUT UINT8                             *PaddedBusRange
+  )
+{
+  EFI_STATUS                        Status;
+  PCI_TYPE00                        Pci;
+  UINT8                             Device;
+  UINT8                             Func;
+  UINT64                            Address;
+  UINT8                             SecondBus;
+  UINT8                             PaddedSubBus;
+  UINT16                            Register;
+  UINTN                             HpIndex;
+  PCI_IO_DEVICE                     *PciDevice;
+  EFI_EVENT                         Event;
+  EFI_HPC_STATE                     State;
+  UINT64                            PciAddress;
+  EFI_HPC_PADDING_ATTRIBUTES        Attributes;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+  UINT16                            BusRange;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  BOOLEAN                           BusPadding;
+  UINT32                            TempReservedBusNum;
+
+  PciRootBridgeIo = Bridge->PciRootBridgeIo;
+  SecondBus       = 0;
+  Register        = 0;
+  State           = 0;
+  Attributes      = (EFI_HPC_PADDING_ATTRIBUTES) 0;
+  BusRange        = 0;
+  BusPadding      = FALSE;
+  PciDevice       = NULL;
+  PciAddress      = 0;
+
+  for (Device = 0; Device <= PCI_MAX_DEVICE; Device++) {
+    TempReservedBusNum = 0;
+    for (Func = 0; Func <= PCI_MAX_FUNC; Func++) {
+
+      //
+      // Check to see whether a pci device is present
+      //
+      Status = PciDevicePresent (
+                PciRootBridgeIo,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func
+                );
+
+      if (EFI_ERROR (Status) && Func == 0) {
+        //
+        // go to next device if there is no Function 0
+        //
+        break;
+      }
+
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Get the PCI device information
+      //
+      Status = PciSearchDevice (
+                Bridge,
+                &Pci,
+                StartBusNumber,
+                Device,
+                Func,
+                &PciDevice
+                );
+
+      ASSERT (!EFI_ERROR (Status));
+
+      PciAddress = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, 0);
+
+      if (!IS_PCI_BRIDGE (&Pci)) {
+        //
+        // PCI bridges will be called later
+        // Here just need for PCI device or PCI to cardbus controller
+        // EfiPciBeforeChildBusEnumeration for PCI Device Node
+        //
+        PreprocessController (
+            PciDevice,
+            PciDevice->BusNumber,
+            PciDevice->DeviceNumber,
+            PciDevice->FunctionNumber,
+            EfiPciBeforeChildBusEnumeration
+            );
+      }
+
+      if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+        //
+        // For Pci Hotplug controller devcie only
+        //
+        if (gPciHotPlugInit != NULL) {
+          //
+          // Check if it is a Hotplug PCI controller
+          //
+          if (IsRootPciHotPlugController (PciDevice->DevicePath, &HpIndex)) {
+            gPciRootHpcData[HpIndex].Found = TRUE;
+
+            if (!gPciRootHpcData[HpIndex].Initialized) {
+
+              Status = CreateEventForHpc (HpIndex, &Event);
+
+              ASSERT (!EFI_ERROR (Status));
+
+              Status = gPciHotPlugInit->InitializeRootHpc (
+                                          gPciHotPlugInit,
+                                          gPciRootHpcPool[HpIndex].HpcDevicePath,
+                                          PciAddress,
+                                          Event,
+                                          &State
+                                          );
+
+              PreprocessController (
+                PciDevice,
+                PciDevice->BusNumber,
+                PciDevice->DeviceNumber,
+                PciDevice->FunctionNumber,
+                EfiPciBeforeChildBusEnumeration
+              );
+            }
+          }
+        }
+      }
+
+      if (IS_PCI_BRIDGE (&Pci) || IS_CARDBUS_BRIDGE (&Pci)) {
+        //
+        // For PPB
+        //
+        if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+          //
+          // If Hot Plug is not supported,
+          // get the bridge information
+          //
+          Status = PciSearchDevice (
+                    Bridge,
+                    &Pci,
+                    StartBusNumber,
+                    Device,
+                    Func,
+                    &PciDevice
+                    );
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        } else {
+          //
+          // If Hot Plug is supported,
+          // Get the bridge information
+          //
+          BusPadding = FALSE;
+          if (gPciHotPlugInit != NULL) {
+
+            if (IsRootPciHotPlugBus (PciDevice->DevicePath, &HpIndex)) {
+
+              //
+              // If it is initialized, get the padded bus range
+              //
+              Status = gPciHotPlugInit->GetResourcePadding (
+                                          gPciHotPlugInit,
+                                          gPciRootHpcPool[HpIndex].HpbDevicePath,
+                                          PciAddress,
+                                          &State,
+                                          (VOID **) &Descriptors,
+                                          &Attributes
+                                          );
+
+              if (EFI_ERROR (Status)) {
+                return Status;
+              }
+
+              BusRange = 0;
+              Status = PciGetBusRange (
+                        &Descriptors,
+                        NULL,
+                        NULL,
+                        &BusRange
+                        );
+
+              FreePool (Descriptors);
+
+              if (EFI_ERROR (Status)) {
+                return Status;
+              }
+
+              BusPadding = TRUE;
+            }
+          }
+        }
+
+        Status = PciAllocateBusNumber (Bridge, *SubBusNumber, 1, SubBusNumber);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        SecondBus = *SubBusNumber;
+
+        Register  = (UINT16) ((SecondBus << 8) | (UINT16) StartBusNumber);
+        Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_PRIMARY_BUS_REGISTER_OFFSET);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint16,
+                                        Address,
+                                        1,
+                                        &Register
+                                        );
+
+
+        //
+        // If it is PPB, resursively search down this bridge
+        //
+        if (IS_PCI_BRIDGE (&Pci)) {
+
+          //
+          // Temporarily initialize SubBusNumber to maximum bus number to ensure the
+          // PCI configuration transaction to go through any PPB
+          //
+          Register  = 0xFF;
+          Address   = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+          Status = PciRootBridgeIo->Pci.Write (
+                                          PciRootBridgeIo,
+                                          EfiPciWidthUint8,
+                                          Address,
+                                          1,
+                                          &Register
+                                          );
+
+          //
+          // Nofify EfiPciBeforeChildBusEnumeration for PCI Brige
+          //
+          PreprocessController (
+            PciDevice,
+            PciDevice->BusNumber,
+            PciDevice->DeviceNumber,
+            PciDevice->FunctionNumber,
+            EfiPciBeforeChildBusEnumeration
+            );
+
+          Status = PciScanBus (
+                    PciDevice,
+                    SecondBus,
+                    SubBusNumber,
+                    PaddedBusRange
+                    );
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+
+        if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport) && BusPadding) {
+          //
+          // Ensure the device is enabled and initialized
+          //
+          if ((Attributes == EfiPaddingPciRootBridge) &&
+              (State & EFI_HPC_STATE_ENABLED) != 0    &&
+              (State & EFI_HPC_STATE_INITIALIZED) != 0) {
+            *PaddedBusRange = (UINT8) ((UINT8) (BusRange) + *PaddedBusRange);
+          } else {
+            //
+            // Reserve the larger one between the actual occupied bus number and padded bus number
+            //
+            Status = PciAllocateBusNumber (PciDevice, SecondBus, (UINT8) (BusRange), &PaddedSubBus);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+            *SubBusNumber = MAX (PaddedSubBus, *SubBusNumber);
+          }
+        }
+
+        //
+        // Set the current maximum bus number under the PPB
+        //
+        Address = EFI_PCI_ADDRESS (StartBusNumber, Device, Func, PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+
+        Status = PciRootBridgeIo->Pci.Write (
+                                        PciRootBridgeIo,
+                                        EfiPciWidthUint8,
+                                        Address,
+                                        1,
+                                        SubBusNumber
+                                        );
+      } else  {
+        //
+        // It is device. Check PCI IOV for Bus reservation
+        // Go through each function, just reserve the MAX ReservedBusNum for one device
+        //
+        if (PcdGetBool (PcdSrIovSupport) && PciDevice->SrIovCapabilityOffset != 0) {
+          if (TempReservedBusNum < PciDevice->ReservedBusNum) {
+
+            Status = PciAllocateBusNumber (PciDevice, *SubBusNumber, (UINT8) (PciDevice->ReservedBusNum - TempReservedBusNum), SubBusNumber);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+            TempReservedBusNum = PciDevice->ReservedBusNum;
+
+            if (Func == 0) {
+              DEBUG ((EFI_D_INFO, "PCI-IOV ScanBus - SubBusNumber - 0x%x\n", *SubBusNumber));
+            } else {
+              DEBUG ((EFI_D_INFO, "PCI-IOV ScanBus - SubBusNumber - 0x%x (Update)\n", *SubBusNumber));
+            }
+          }
+        }
+      }
+
+      if (Func == 0 && !IS_PCI_MULTI_FUNC (&Pci)) {
+
+        //
+        // Skip sub functions, this is not a multi function device
+        //
+
+        Func = PCI_MAX_FUNC;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process Option Rom on the specified root bridge.
+
+  @param Bridge  Pci root bridge device instance.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval other         Some error occurred when processing Option Rom on the root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeP2CProcess (
+  IN PCI_IO_DEVICE *Bridge
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  PCI_IO_DEVICE   *Temp;
+  EFI_HPC_STATE   State;
+  UINT64          PciAddress;
+  EFI_STATUS      Status;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    if (IS_CARDBUS_BRIDGE (&Temp->Pci)) {
+
+      if (gPciHotPlugInit != NULL && Temp->Allocated && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+
+        //
+        // Raise the EFI_IOB_PCI_HPC_INIT status code
+        //
+        REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+          EFI_PROGRESS_CODE,
+          EFI_IO_BUS_PCI | EFI_IOB_PCI_HPC_INIT,
+          Temp->DevicePath
+          );
+
+        PciAddress = EFI_PCI_ADDRESS (Temp->BusNumber, Temp->DeviceNumber, Temp->FunctionNumber, 0);
+        Status = gPciHotPlugInit->InitializeRootHpc (
+                                    gPciHotPlugInit,
+                                    Temp->DevicePath,
+                                    PciAddress,
+                                    NULL,
+                                    &State
+                                    );
+
+        if (!EFI_ERROR (Status)) {
+          Status = PciBridgeEnumerator (Temp);
+
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+
+        CurrentLink = CurrentLink->ForwardLink;
+        continue;
+
+      }
+    }
+
+    if (!IsListEmpty (&Temp->ChildList)) {
+      Status = PciRootBridgeP2CProcess (Temp);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process Option Rom on the specified host bridge.
+
+  @param PciResAlloc    Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval other         Some error occurred when processing Option Rom on the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  )
+{
+  EFI_HANDLE    RootBridgeHandle;
+  PCI_IO_DEVICE *RootBridgeDev;
+  EFI_STATUS    Status;
+
+  if (!FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    return EFI_SUCCESS;
+  }
+
+  RootBridgeHandle = NULL;
+
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // Get RootBridg Device by handle
+    //
+    RootBridgeDev = GetRootBridgeByHandle (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    Status = PciRootBridgeP2CProcess (RootBridgeDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is used to enumerate the entire host bridge
+  in a given platform.
+
+  @param PciResAlloc   A pointer to the PCI Host Resource Allocation protocol.
+
+  @retval EFI_SUCCESS            Successfully enumerated the host bridge.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory available.
+  @retval other                  Some error occurred when enumerating the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc
+  )
+{
+  EFI_HANDLE                        RootBridgeHandle;
+  PCI_IO_DEVICE                     *RootBridgeDev;
+  EFI_STATUS                        Status;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   *PciRootBridgeIo;
+  UINT16                            MinBus;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptors;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Configuration;
+  UINT8                             StartBusNumber;
+  LIST_ENTRY                        RootBridgeList;
+  LIST_ENTRY                        *Link;
+
+  if (FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    InitializeHotPlugSupport ();
+  }
+
+  InitializeListHead (&RootBridgeList);
+
+  //
+  // Notify the bus allocation phase is about to start
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginBusAllocation);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG((EFI_D_INFO, "PCI Bus First Scanning\n"));
+  RootBridgeHandle = NULL;
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // if a root bridge instance is found, create root bridge device for it
+    //
+
+    RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Enumerate all the buses under this root bridge
+    //
+    Status = PciRootBridgeEnumerator (
+              PciResAlloc,
+              RootBridgeDev
+              );
+
+    if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+      InsertTailList (&RootBridgeList, &(RootBridgeDev->Link));
+    } else {
+      DestroyRootBridge (RootBridgeDev);
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Notify the bus allocation phase is finished for the first time
+  //
+  NotifyPhase (PciResAlloc, EfiPciHostBridgeEndBusAllocation);
+
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    //
+    // Reset all assigned PCI bus number in all PPB
+    //
+    RootBridgeHandle = NULL;
+    Link = GetFirstNode (&RootBridgeList);
+    while ((PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) &&
+      (!IsNull (&RootBridgeList, Link))) {
+      RootBridgeDev = PCI_IO_DEVICE_FROM_LINK (Link);
+      //
+      // Get the Bus information
+      //
+      Status = PciResAlloc->StartBusEnumeration (
+                              PciResAlloc,
+                              RootBridgeHandle,
+                              (VOID **) &Configuration
+                              );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      //
+      // Get the bus number to start with
+      //
+      StartBusNumber  = (UINT8) (Configuration->AddrRangeMin);
+
+      ResetAllPpbBusNumber (
+        RootBridgeDev,
+        StartBusNumber
+      );
+
+      FreePool (Configuration);
+      Link = RemoveEntryList (Link);
+      DestroyRootBridge (RootBridgeDev);
+    }
+
+    //
+    // Wait for all HPC initialized
+    //
+    Status = AllRootHPCInitialized (STALL_1_SECOND * 15);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Some root HPC failed to initialize\n"));
+      return Status;
+    }
+
+    //
+    // Notify the bus allocation phase is about to start for the 2nd time
+    //
+    Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginBusAllocation);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DEBUG((EFI_D_INFO, "PCI Bus Second Scanning\n"));
+    RootBridgeHandle = NULL;
+    while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+      //
+      // if a root bridge instance is found, create root bridge device for it
+      //
+      RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+      if (RootBridgeDev == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      //
+      // Enumerate all the buses under this root bridge
+      //
+      Status = PciRootBridgeEnumerator (
+                PciResAlloc,
+                RootBridgeDev
+                );
+
+      DestroyRootBridge (RootBridgeDev);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+
+    //
+    // Notify the bus allocation phase is to end for the 2nd time
+    //
+    NotifyPhase (PciResAlloc, EfiPciHostBridgeEndBusAllocation);
+  }
+
+  //
+  // Notify the resource allocation phase is to start
+  //
+  Status = NotifyPhase (PciResAlloc, EfiPciHostBridgeBeginResourceAllocation);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  RootBridgeHandle = NULL;
+  while (PciResAlloc->GetNextRootBridge (PciResAlloc, &RootBridgeHandle) == EFI_SUCCESS) {
+
+    //
+    // if a root bridge instance is found, create root bridge device for it
+    //
+    RootBridgeDev = CreateRootBridge (RootBridgeHandle);
+
+    if (RootBridgeDev == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = StartManagingRootBridge (RootBridgeDev);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    PciRootBridgeIo = RootBridgeDev->PciRootBridgeIo;
+    Status          = PciRootBridgeIo->Configuration (PciRootBridgeIo, (VOID **) &Descriptors);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = PciGetBusRange (&Descriptors, &MinBus, NULL, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Determine root bridge attribute by calling interface of Pcihostbridge
+    // protocol
+    //
+    DetermineRootBridgeAttributes (
+      PciResAlloc,
+      RootBridgeDev
+      );
+
+    //
+    // Collect all the resource information under this root bridge
+    // A database that records all the information about pci device subject to this
+    // root bridge will then be created
+    //
+    Status = PciPciDeviceInfoCollector (
+              RootBridgeDev,
+              (UINT8) MinBus
+              );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    InsertRootBridge (RootBridgeDev);
+
+    //
+    // Record the hostbridge handle
+    //
+    AddHostBridgeEnumerator (RootBridgeDev->PciRootBridgeIo->ParentHandle);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h
new file mode 100644
index 0000000000..6c2ade3f36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciLib.h
@@ -0,0 +1,165 @@
+/** @file
+  Internal library declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _EFI_PCI_LIB_H_
+#define _EFI_PCI_LIB_H_
+
+
+typedef struct {
+  EFI_HANDLE            Handle;
+} EFI_DEVICE_HANDLE_EXTENDED_DATA_PAYLOAD;
+
+typedef struct {
+  UINT32                             Bar;
+  UINT16                             DevicePathSize;
+  UINT16                             ReqResSize;
+  UINT16                             AllocResSize;
+  UINT8                              *DevicePath;
+  UINT8                              *ReqRes;
+  UINT8                              *AllocRes;
+} EFI_RESOURCE_ALLOC_FAILURE_ERROR_DATA_PAYLOAD;
+
+
+/**
+  Retrieve the PCI Card device BAR information via PciIo interface.
+
+  @param PciIoDevice        PCI Card device instance.
+
+**/
+VOID
+GetBackPcCardBar (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  );
+
+/**
+  Remove rejected pci device from specific root bridge
+  handle.
+
+  @param RootBridgeHandle  Specific parent root bridge handle.
+  @param Bridge            Bridge device instance.
+
+**/
+VOID
+RemoveRejectedPciDevices (
+  IN EFI_HANDLE        RootBridgeHandle,
+  IN PCI_IO_DEVICE     *Bridge
+  );
+
+/**
+  Submits the I/O and memory resource requirements for the specified PCI Host Bridge.
+
+  @param PciResAlloc  Point to protocol instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS           Successfully finished resource allocation.
+  @retval EFI_NOT_FOUND         Cannot get root bridge instance.
+  @retval EFI_OUT_OF_RESOURCES  Platform failed to program the resources if no hot plug supported.
+  @retval other                 Some error occurred when allocating resources for the PCI Host Bridge.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciHostBridgeResourceAllocator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  Allocate NumberOfBuses buses and return the next available PCI bus number.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   Current available PCI bus number.
+  @param  NumberOfBuses    Number of buses enumerated below the StartBusNumber.
+  @param  NextBusNumber    Next available PCI bus number.
+
+  @retval EFI_SUCCESS           Available bus number resource is enough. Next available PCI bus number
+                                is returned in NextBusNumber.
+  @retval EFI_OUT_OF_RESOURCES  Available bus number resource is not enough for allocation.
+
+**/
+EFI_STATUS
+PciAllocateBusNumber (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  IN UINT8                              NumberOfBuses,
+  OUT UINT8                             *NextBusNumber
+  );
+
+/**
+  Scan pci bus and assign bus number to the given PCI bus system.
+
+  @param  Bridge           Bridge device instance.
+  @param  StartBusNumber   start point.
+  @param  SubBusNumber     Point to sub bus number.
+  @param  PaddedBusRange   Customized bus number.
+
+  @retval EFI_SUCCESS      Successfully scanned and assigned bus number.
+  @retval other            Some error occurred when scanning pci bus.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determine whether need support hotplug.
+
+**/
+EFI_STATUS
+PciScanBus (
+  IN PCI_IO_DEVICE                      *Bridge,
+  IN UINT8                              StartBusNumber,
+  OUT UINT8                             *SubBusNumber,
+  OUT UINT8                             *PaddedBusRange
+  );
+
+/**
+  Process Option Rom on the specified root bridge.
+
+  @param Bridge  Pci root bridge device instance.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval other         Some error occurred when processing Option Rom on the root bridge.
+
+**/
+EFI_STATUS
+PciRootBridgeP2CProcess (
+  IN PCI_IO_DEVICE *Bridge
+  );
+
+/**
+  Process Option Rom on the specified host bridge.
+
+  @param PciResAlloc    Pointer to instance of EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+
+  @retval EFI_SUCCESS   Success process.
+  @retval EFI_NOT_FOUND Can not find the root bridge instance.
+  @retval other         Some error occurred when processing Option Rom on the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeP2CProcess (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *PciResAlloc
+  );
+
+/**
+  This function is used to enumerate the entire host bridge
+  in a given platform.
+
+  @param PciResAlloc   A pointer to the PCI Host Resource Allocation protocol.
+
+  @retval EFI_SUCCESS            Successfully enumerated the host bridge.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory available.
+  @retval other                  Some error occurred when enumerating the host bridge.
+
+**/
+EFI_STATUS
+PciHostBridgeEnumerator (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  *PciResAlloc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c
new file mode 100644
index 0000000000..2bc4f8c5e8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.c
@@ -0,0 +1,783 @@
+/** @file
+  PCI Rom supporting funtions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+/**
+  Load the EFI Image from Option ROM
+
+  @param PciIoDevice   PCI IO device instance.
+  @param FilePath      The file path of the EFI Image
+  @param BufferSize    On input the size of Buffer in bytes. On output with a return
+                       code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                       On output with a return code of EFI_BUFFER_TOO_SMALL,
+                       the size of Buffer required to retrieve the requested file.
+  @param Buffer        The memory buffer to transfer the file to. If Buffer is NULL,
+                       then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+**/
+EFI_STATUS
+LocalLoadFile2 (
+  IN PCI_IO_DEVICE            *PciIoDevice,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  )
+{
+  EFI_STATUS                                Status;
+  MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH   *EfiOpRomImageNode;
+  EFI_PCI_EXPANSION_ROM_HEADER              *EfiRomHeader;
+  PCI_DATA_STRUCTURE                        *Pcir;
+  UINT32                                    ImageSize;
+  UINT8                                     *ImageBuffer;
+  UINT32                                    ImageLength;
+  UINT32                                    DestinationSize;
+  UINT32                                    ScratchSize;
+  VOID                                      *Scratch;
+  EFI_DECOMPRESS_PROTOCOL                   *Decompress;
+  UINT32                                    InitializationSize;
+
+  EfiOpRomImageNode = (MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH *) FilePath;
+  if ((EfiOpRomImageNode == NULL) ||
+      (DevicePathType (FilePath) != MEDIA_DEVICE_PATH) ||
+      (DevicePathSubType (FilePath) != MEDIA_RELATIVE_OFFSET_RANGE_DP) ||
+      (DevicePathNodeLength (FilePath) != sizeof (MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH)) ||
+      (!IsDevicePathEnd (NextDevicePathNode (FilePath))) ||
+      (EfiOpRomImageNode->StartingOffset > EfiOpRomImageNode->EndingOffset) ||
+      (EfiOpRomImageNode->EndingOffset >= PciIoDevice->RomSize) ||
+      (BufferSize == NULL)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  EfiRomHeader = (EFI_PCI_EXPANSION_ROM_HEADER *) (
+      (UINT8 *) PciIoDevice->PciIo.RomImage + EfiOpRomImageNode->StartingOffset
+      );
+  if (EfiRomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+    return EFI_NOT_FOUND;
+  }
+
+
+  Pcir = (PCI_DATA_STRUCTURE *) ((UINT8 *) EfiRomHeader + EfiRomHeader->PcirOffset);
+  ASSERT (Pcir->Signature == PCI_DATA_STRUCTURE_SIGNATURE);
+
+  if ((Pcir->CodeType == PCI_CODE_TYPE_EFI_IMAGE) &&
+      (EfiRomHeader->EfiSignature == EFI_PCI_EXPANSION_ROM_HEADER_EFISIGNATURE) &&
+      ((EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER) ||
+       (EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER)) &&
+      (EfiRomHeader->CompressionType <= EFI_PCI_EXPANSION_ROM_HEADER_COMPRESSED)
+       ) {
+
+    ImageSize = Pcir->ImageLength * 512;
+    InitializationSize = (UINT32) EfiRomHeader->InitializationSize * 512;
+    if (InitializationSize > ImageSize || EfiRomHeader->EfiImageHeaderOffset >=  InitializationSize) {
+      return EFI_NOT_FOUND;
+    }
+
+    ImageBuffer             = (UINT8 *) EfiRomHeader + EfiRomHeader->EfiImageHeaderOffset;
+    ImageLength             = InitializationSize - EfiRomHeader->EfiImageHeaderOffset;
+
+    if (EfiRomHeader->CompressionType != EFI_PCI_EXPANSION_ROM_HEADER_COMPRESSED) {
+      //
+      // Uncompressed: Copy the EFI Image directly to user's buffer
+      //
+      if (Buffer == NULL || *BufferSize < ImageLength) {
+        *BufferSize = ImageLength;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+
+      *BufferSize = ImageLength;
+      CopyMem (Buffer, ImageBuffer, ImageLength);
+      return EFI_SUCCESS;
+
+    } else {
+      //
+      // Compressed: Uncompress before copying
+      //
+      Status = gBS->LocateProtocol (&gEfiDecompressProtocolGuid, NULL, (VOID **) &Decompress);
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+      Status = Decompress->GetInfo (
+                             Decompress,
+                             ImageBuffer,
+                             ImageLength,
+                             &DestinationSize,
+                             &ScratchSize
+                             );
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      if (Buffer == NULL || *BufferSize < DestinationSize) {
+        *BufferSize = DestinationSize;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+
+      *BufferSize = DestinationSize;
+      Scratch = AllocatePool (ScratchSize);
+      if (Scratch == NULL) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      Status = Decompress->Decompress (
+                             Decompress,
+                             ImageBuffer,
+                             ImageLength,
+                             Buffer,
+                             DestinationSize,
+                             Scratch,
+                             ScratchSize
+                             );
+      FreePool (Scratch);
+
+      if (EFI_ERROR (Status)) {
+        return EFI_DEVICE_ERROR;
+      }
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Initialize a PCI LoadFile2 instance.
+
+  @param PciIoDevice   PCI IO Device.
+
+**/
+VOID
+InitializePciLoadFile2 (
+  IN PCI_IO_DEVICE       *PciIoDevice
+  )
+{
+  PciIoDevice->LoadFile2.LoadFile = LoadFile2;
+}
+
+/**
+  Causes the driver to load a specified file.
+
+  @param This        Indicates a pointer to the calling context.
+  @param FilePath    The device specific path of the file to load.
+  @param BootPolicy  Should always be FALSE.
+  @param BufferSize  On input the size of Buffer in bytes. On output with a return
+                     code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                     On output with a return code of EFI_BUFFER_TOO_SMALL,
+                     the size of Buffer required to retrieve the requested file.
+  @param Buffer      The memory buffer to transfer the file to. If Buffer is NULL,
+                     then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_UNSUPPORTED       BootPolicy is TRUE.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadFile2 (
+  IN EFI_LOAD_FILE2_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN BOOLEAN                  BootPolicy,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  )
+{
+  PCI_IO_DEVICE                             *PciIoDevice;
+
+  if (BootPolicy) {
+    return EFI_UNSUPPORTED;
+  }
+  PciIoDevice = PCI_IO_DEVICE_FROM_LOAD_FILE2_THIS (This);
+
+  return LocalLoadFile2 (
+           PciIoDevice,
+           FilePath,
+           BufferSize,
+           Buffer
+           );
+}
+
+/**
+  Get Pci device's oprom information.
+
+  @param PciIoDevice    Input Pci device instance.
+                        Output Pci device instance with updated OptionRom size.
+
+  @retval EFI_NOT_FOUND Pci device has not Option Rom.
+  @retval EFI_SUCCESS   Pci device has Option Rom.
+
+**/
+EFI_STATUS
+GetOpRomInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  )
+{
+  UINT8                           RomBarIndex;
+  UINT32                          AllOnes;
+  UINT64                          Address;
+  EFI_STATUS                      Status;
+  UINT8                           Bus;
+  UINT8                           Device;
+  UINT8                           Function;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+
+  Bus             = PciIoDevice->BusNumber;
+  Device          = PciIoDevice->DeviceNumber;
+  Function        = PciIoDevice->FunctionNumber;
+
+  PciRootBridgeIo = PciIoDevice->PciRootBridgeIo;
+
+  //
+  // Offset is 0x30 if is not ppb
+  //
+
+  //
+  // 0x30
+  //
+  RomBarIndex = PCI_EXPANSION_ROM_BASE;
+
+  if (IS_PCI_BRIDGE (&PciIoDevice->Pci)) {
+    //
+    // If is ppb, 0x38
+    //
+    RomBarIndex = PCI_BRIDGE_ROMBAR;
+  }
+  //
+  // The bit0 is 0 to prevent the enabling of the Rom address decoder
+  //
+  AllOnes = 0xfffffffe;
+  Address = EFI_PCI_ADDRESS (Bus, Device, Function, RomBarIndex);
+
+  Status = PciRootBridgeIo->Pci.Write (
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  &AllOnes
+                                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Read back
+  //
+  Status = PciRootBridgeIo->Pci.Read(
+                                  PciRootBridgeIo,
+                                  EfiPciWidthUint32,
+                                  Address,
+                                  1,
+                                  &AllOnes
+                                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Bits [1, 10] are reserved
+  //
+  AllOnes &= 0xFFFFF800;
+  if ((AllOnes == 0) || (AllOnes == 0xFFFFF800)) {
+    return EFI_NOT_FOUND;
+  }
+
+  PciIoDevice->RomSize = (UINT64) ((~AllOnes) + 1);
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if the RomImage contains EFI Images.
+
+  @param  RomImage  The ROM address of Image for check.
+  @param  RomSize   Size of ROM for check.
+
+  @retval TRUE     ROM contain EFI Image.
+  @retval FALSE    ROM not contain EFI Image.
+
+**/
+BOOLEAN
+ContainEfiImage (
+  IN VOID            *RomImage,
+  IN UINT64          RomSize
+  )
+{
+  PCI_EXPANSION_ROM_HEADER  *RomHeader;
+  PCI_DATA_STRUCTURE        *RomPcir;
+  UINT8                     Indicator;
+
+  Indicator = 0;
+  RomHeader = RomImage;
+  if (RomHeader == NULL) {
+    return FALSE;
+  }
+
+  do {
+    if (RomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomHeader = (PCI_EXPANSION_ROM_HEADER *) ((UINT8 *) RomHeader + 512);
+      continue;
+    }
+
+    //
+    // The PCI Data Structure must be DWORD aligned. 
+    //
+    if (RomHeader->PcirOffset == 0 ||
+        (RomHeader->PcirOffset & 3) != 0 ||
+        (UINT8 *) RomHeader + RomHeader->PcirOffset + sizeof (PCI_DATA_STRUCTURE) > (UINT8 *) RomImage + RomSize) {
+      break;
+    }
+
+    RomPcir = (PCI_DATA_STRUCTURE *) ((UINT8 *) RomHeader + RomHeader->PcirOffset);
+    if (RomPcir->Signature != PCI_DATA_STRUCTURE_SIGNATURE) {
+      break;
+    }
+
+    if (RomPcir->CodeType == PCI_CODE_TYPE_EFI_IMAGE) {
+      return TRUE;
+    }
+
+    Indicator = RomPcir->Indicator;
+    RomHeader = (PCI_EXPANSION_ROM_HEADER *) ((UINT8 *) RomHeader + RomPcir->ImageLength * 512);
+  } while (((UINT8 *) RomHeader < (UINT8 *) RomImage + RomSize) && ((Indicator & 0x80) == 0x00));
+
+  return FALSE;
+}
+
+/**
+  Load Option Rom image for specified PCI device.
+
+  @param PciDevice Pci device instance.
+  @param RomBase   Base address of Option Rom.
+
+  @retval EFI_OUT_OF_RESOURCES No enough memory to hold image.
+  @retval EFI_SUCESS           Successfully loaded Option Rom.
+
+**/
+EFI_STATUS
+LoadOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT64          RomBase
+  )
+{
+  UINT8                     RomBarIndex;
+  UINT8                     Indicator;
+  UINT16                    OffsetPcir;
+  UINT32                    RomBarOffset;
+  UINT32                    RomBar;
+  EFI_STATUS                RetStatus;
+  BOOLEAN                   FirstCheck;
+  UINT8                     *Image;
+  PCI_EXPANSION_ROM_HEADER  *RomHeader;
+  PCI_DATA_STRUCTURE        *RomPcir;
+  UINT64                    RomSize;
+  UINT64                    RomImageSize;
+  UINT32                    LegacyImageLength;
+  UINT8                     *RomInMemory;
+  UINT8                     CodeType;
+
+  RomSize       = PciDevice->RomSize;
+
+  Indicator     = 0;
+  RomImageSize  = 0;
+  RomInMemory   = NULL;
+  CodeType      = 0xFF;
+
+  //
+  // Get the RomBarIndex
+  //
+
+  //
+  // 0x30
+  //
+  RomBarIndex = PCI_EXPANSION_ROM_BASE;
+  if (IS_PCI_BRIDGE (&(PciDevice->Pci))) {
+    //
+    // if is ppb
+    //
+
+    //
+    // 0x38
+    //
+    RomBarIndex = PCI_BRIDGE_ROMBAR;
+  }
+  //
+  // Allocate memory for Rom header and PCIR
+  //
+  RomHeader = AllocatePool (sizeof (PCI_EXPANSION_ROM_HEADER));
+  if (RomHeader == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  RomPcir = AllocatePool (sizeof (PCI_DATA_STRUCTURE));
+  if (RomPcir == NULL) {
+    FreePool (RomHeader);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  RomBar = (UINT32) RomBase;
+
+  //
+  // Enable RomBar
+  //
+  RomDecode (PciDevice, RomBarIndex, RomBar, TRUE);
+
+  RomBarOffset  = RomBar;
+  RetStatus     = EFI_NOT_FOUND;
+  FirstCheck    = TRUE;
+  LegacyImageLength = 0;
+
+  do {
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBarOffset,
+                                      sizeof (PCI_EXPANSION_ROM_HEADER),
+                                      (UINT8 *) RomHeader
+                                      );
+
+    if (RomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomBarOffset = RomBarOffset + 512;
+      if (FirstCheck) {
+        break;
+      } else {
+        RomImageSize = RomImageSize + 512;
+        continue;
+      }
+    }
+
+    FirstCheck  = FALSE;
+    OffsetPcir  = RomHeader->PcirOffset;
+    //
+    // If the pointer to the PCI Data Structure is invalid, no further images can be located. 
+    // The PCI Data Structure must be DWORD aligned. 
+    //
+    if (OffsetPcir == 0 ||
+        (OffsetPcir & 3) != 0 ||
+        RomImageSize + OffsetPcir + sizeof (PCI_DATA_STRUCTURE) > RomSize) {
+      break;
+    }
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBarOffset + OffsetPcir,
+                                      sizeof (PCI_DATA_STRUCTURE),
+                                      (UINT8 *) RomPcir
+                                      );
+    //
+    // If a valid signature is not present in the PCI Data Structure, no further images can be located.
+    //
+    if (RomPcir->Signature != PCI_DATA_STRUCTURE_SIGNATURE) {
+      break;
+    }
+    if (RomImageSize + RomPcir->ImageLength * 512 > RomSize) {
+      break;
+    }
+    if (RomPcir->CodeType == PCI_CODE_TYPE_PCAT_IMAGE) {
+      CodeType = PCI_CODE_TYPE_PCAT_IMAGE;
+      LegacyImageLength = ((UINT32)((EFI_LEGACY_EXPANSION_ROM_HEADER *)RomHeader)->Size512) * 512;
+    }
+    Indicator     = RomPcir->Indicator;
+    RomImageSize  = RomImageSize + RomPcir->ImageLength * 512;
+    RomBarOffset  = RomBarOffset + RomPcir->ImageLength * 512;
+  } while (((Indicator & 0x80) == 0x00) && ((RomBarOffset - RomBar) < RomSize));
+
+  //
+  // Some Legacy Cards do not report the correct ImageLength so used the maximum
+  // of the legacy length and the PCIR Image Length
+  //
+  if (CodeType == PCI_CODE_TYPE_PCAT_IMAGE) {
+    RomImageSize = MAX (RomImageSize, LegacyImageLength);
+  }
+
+  if (RomImageSize > 0) {
+    RetStatus = EFI_SUCCESS;
+    Image     = AllocatePool ((UINT32) RomImageSize);
+    if (Image == NULL) {
+      RomDecode (PciDevice, RomBarIndex, RomBar, FALSE);
+      FreePool (RomHeader);
+      FreePool (RomPcir);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Copy Rom image into memory
+    //
+    PciDevice->PciRootBridgeIo->Mem.Read (
+                                      PciDevice->PciRootBridgeIo,
+                                      EfiPciWidthUint8,
+                                      RomBar,
+                                      (UINT32) RomImageSize,
+                                      Image
+                                      );
+    RomInMemory = Image;
+  }
+
+  RomDecode (PciDevice, RomBarIndex, RomBar, FALSE);
+
+  PciDevice->EmbeddedRom    = TRUE;
+  PciDevice->PciIo.RomSize  = RomImageSize;
+  PciDevice->PciIo.RomImage = RomInMemory;
+
+  //
+  // For OpROM read from PCI device:
+  //   Add the Rom Image to internal database for later PCI light enumeration
+  //
+  PciRomAddImageMapping (
+    NULL,
+    PciDevice->PciRootBridgeIo->SegmentNumber,
+    PciDevice->BusNumber,
+    PciDevice->DeviceNumber,
+    PciDevice->FunctionNumber,
+    (UINT64) (UINTN) PciDevice->PciIo.RomImage,
+    PciDevice->PciIo.RomSize
+    );
+
+  //
+  // Free allocated memory
+  //
+  FreePool (RomHeader);
+  FreePool (RomPcir);
+
+  return RetStatus;
+}
+
+/**
+  Enable/Disable Option Rom decode.
+
+  @param PciDevice    Pci device instance.
+  @param RomBarIndex  The BAR index of the standard PCI Configuration header to use as the
+                      base address for resource range. The legal range for this field is 0..5.
+  @param RomBar       Base address of Option Rom.
+  @param Enable       Flag for enable/disable decode.
+
+**/
+VOID
+RomDecode (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT8           RomBarIndex,
+  IN UINT32          RomBar,
+  IN BOOLEAN         Enable
+  )
+{
+  UINT32              Value32;
+  UINT32              Offset;
+  UINT32              OffsetMax;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+
+  PciIo = &PciDevice->PciIo;
+  if (Enable) {
+    //
+    // Clear all bars
+    //
+    OffsetMax = 0x24;
+    if (IS_PCI_BRIDGE(&PciDevice->Pci)) {
+      OffsetMax = 0x14;
+    }
+
+    for (Offset = 0x10; Offset <= OffsetMax; Offset += sizeof (UINT32)) {
+      PciIo->Pci.Write (PciIo, EfiPciIoWidthUint32, Offset, 1, &gAllZero);
+    }
+
+    //
+    // set the Rom base address: now is hardcode
+    // enable its decoder
+    //
+    Value32 = RomBar | 0x1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 (EFI_PCI_IO_PROTOCOL_WIDTH) EfiPciWidthUint32,
+                 RomBarIndex,
+                 1,
+                 &Value32
+                 );
+
+    //
+    // Programe all upstream bridge
+    //
+    ProgrameUpstreamBridgeForRom(PciDevice, RomBar, TRUE);
+
+    //
+    // Setting the memory space bit in the function's command register
+    //
+    PCI_ENABLE_COMMAND_REGISTER(PciDevice, EFI_PCI_COMMAND_MEMORY_SPACE);
+
+  } else {
+
+    //
+    // disable command register decode to memory
+    //
+    PCI_DISABLE_COMMAND_REGISTER(PciDevice, EFI_PCI_COMMAND_MEMORY_SPACE);
+
+    //
+    // Destroy the programmed bar in all the upstream bridge.
+    //
+    ProgrameUpstreamBridgeForRom(PciDevice, RomBar, FALSE);
+
+    //
+    // disable rom decode
+    //
+    Value32 = 0xFFFFFFFE;
+    PciIo->Pci.Write (
+                 PciIo,
+                 (EFI_PCI_IO_PROTOCOL_WIDTH) EfiPciWidthUint32,
+                 RomBarIndex,
+                 1,
+                 &Value32
+                 );
+
+  }
+}
+
+/**
+  Load and start the Option Rom image.
+
+  @param PciDevice       Pci device instance.
+
+  @retval EFI_SUCCESS    Successfully loaded and started PCI Option Rom image.
+  @retval EFI_NOT_FOUND  Failed to process PCI Option Rom image.
+
+**/
+EFI_STATUS
+ProcessOpRomImage (
+  IN  PCI_IO_DEVICE   *PciDevice
+  )
+{
+  UINT8                                    Indicator;
+  UINT32                                   ImageSize;
+  VOID                                     *RomBar;
+  UINT8                                    *RomBarOffset;
+  EFI_HANDLE                               ImageHandle;
+  EFI_STATUS                               Status;
+  EFI_STATUS                               RetStatus;
+  EFI_PCI_EXPANSION_ROM_HEADER             *EfiRomHeader;
+  PCI_DATA_STRUCTURE                       *Pcir;
+  EFI_DEVICE_PATH_PROTOCOL                 *PciOptionRomImageDevicePath;
+  MEDIA_RELATIVE_OFFSET_RANGE_DEVICE_PATH  EfiOpRomImageNode;
+  VOID                                     *Buffer;
+  UINTN                                    BufferSize;
+
+  Indicator = 0;
+
+  //
+  // Get the Address of the Option Rom image
+  //
+  RomBar        = PciDevice->PciIo.RomImage;
+  RomBarOffset  = (UINT8 *) RomBar;
+  RetStatus     = EFI_NOT_FOUND;
+
+  if (RomBar == NULL) {
+    return RetStatus;
+  }
+  ASSERT (((EFI_PCI_EXPANSION_ROM_HEADER *) RomBarOffset)->Signature == PCI_EXPANSION_ROM_HEADER_SIGNATURE);
+
+  do {
+    EfiRomHeader = (EFI_PCI_EXPANSION_ROM_HEADER *) RomBarOffset;
+    if (EfiRomHeader->Signature != PCI_EXPANSION_ROM_HEADER_SIGNATURE) {
+      RomBarOffset += 512;
+      continue;
+    }
+
+    Pcir        = (PCI_DATA_STRUCTURE *) (RomBarOffset + EfiRomHeader->PcirOffset);
+    ASSERT (Pcir->Signature == PCI_DATA_STRUCTURE_SIGNATURE);
+    ImageSize   = (UINT32) (Pcir->ImageLength * 512);
+    Indicator   = Pcir->Indicator;
+
+    //
+    // Skip the image if it is not an EFI PCI Option ROM image
+    //
+    if (Pcir->CodeType != PCI_CODE_TYPE_EFI_IMAGE) {
+      goto NextImage;
+    }
+
+    //
+    // Skip the EFI PCI Option ROM image if its machine type is not supported
+    //
+    if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (EfiRomHeader->EfiMachineType)) {
+      goto NextImage;
+    }
+
+    //
+    // Ignore the EFI PCI Option ROM image if it is an EFI application
+    //
+    if (EfiRomHeader->EfiSubsystem == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION) {
+      goto NextImage;
+    }
+
+    //
+    // Create Pci Option Rom Image device path header
+    //
+    EfiOpRomImageNode.Header.Type     = MEDIA_DEVICE_PATH;
+    EfiOpRomImageNode.Header.SubType  = MEDIA_RELATIVE_OFFSET_RANGE_DP;
+    SetDevicePathNodeLength (&EfiOpRomImageNode.Header, sizeof (EfiOpRomImageNode));
+    EfiOpRomImageNode.StartingOffset  = (UINTN) RomBarOffset - (UINTN) RomBar;
+    EfiOpRomImageNode.EndingOffset    = (UINTN) RomBarOffset + ImageSize - 1 - (UINTN) RomBar;
+
+    PciOptionRomImageDevicePath = AppendDevicePathNode (PciDevice->DevicePath, &EfiOpRomImageNode.Header);
+    ASSERT (PciOptionRomImageDevicePath != NULL);
+
+    //
+    // load image and start image
+    //
+    BufferSize  = 0;
+    Buffer      = NULL;
+    ImageHandle = NULL;
+
+    Status = gBS->LoadImage (
+                    FALSE,
+                    gPciBusDriverBinding.DriverBindingHandle,
+                    PciOptionRomImageDevicePath,
+                    Buffer,
+                    BufferSize,
+                    &ImageHandle
+                    );
+
+    FreePool (PciOptionRomImageDevicePath);
+
+    if (!EFI_ERROR (Status)) {
+      Status = gBS->StartImage (ImageHandle, NULL, NULL);
+      if (!EFI_ERROR (Status)) {
+        AddDriver (PciDevice, ImageHandle);
+        PciRomAddImageMapping (
+          ImageHandle,
+          PciDevice->PciRootBridgeIo->SegmentNumber,
+          PciDevice->BusNumber,
+          PciDevice->DeviceNumber,
+          PciDevice->FunctionNumber,
+          (UINT64) (UINTN) PciDevice->PciIo.RomImage,
+          PciDevice->PciIo.RomSize
+          );
+        RetStatus = EFI_SUCCESS;
+      }
+    }
+
+NextImage:
+    RomBarOffset += ImageSize;
+
+  } while (((Indicator & 0x80) == 0x00) && ((UINTN) (RomBarOffset - (UINT8 *) RomBar) < PciDevice->RomSize));
+
+  return RetStatus;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h
new file mode 100644
index 0000000000..25f78a417f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciOptionRomSupport.h
@@ -0,0 +1,142 @@
+/** @file
+  PCI Rom supporting funtions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _EFI_PCI_OPTION_ROM_SUPPORT_H_
+#define _EFI_PCI_OPTION_ROM_SUPPORT_H_
+
+
+/**
+  Initialize a PCI LoadFile2 instance.
+
+  @param PciIoDevice   PCI IO Device.
+
+**/
+VOID
+InitializePciLoadFile2 (
+  IN PCI_IO_DEVICE       *PciIoDevice
+  );
+
+/**
+  Causes the driver to load a specified file.
+
+  @param This        Indicates a pointer to the calling context.
+  @param FilePath    The device specific path of the file to load.
+  @param BootPolicy  Should always be FALSE.
+  @param BufferSize  On input the size of Buffer in bytes. On output with a return
+                     code of EFI_SUCCESS, the amount of data transferred to Buffer.
+                     On output with a return code of EFI_BUFFER_TOO_SMALL,
+                     the size of Buffer required to retrieve the requested file.
+  @param Buffer      The memory buffer to transfer the file to. If Buffer is NULL,
+                     then no the size of the requested file is returned in BufferSize.
+
+  @retval EFI_SUCCESS           The file was loaded.
+  @retval EFI_UNSUPPORTED       BootPolicy is TRUE.
+  @retval EFI_INVALID_PARAMETER FilePath is not a valid device path, or
+                                BufferSize is NULL.
+  @retval EFI_NOT_FOUND         Not found PCI Option Rom on PCI device.
+  @retval EFI_DEVICE_ERROR      Failed to decompress PCI Option Rom image.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize is too small to read the current directory entry.
+                                BufferSize has been updated with the size needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadFile2 (
+  IN EFI_LOAD_FILE2_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL *FilePath,
+  IN BOOLEAN                  BootPolicy,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer      OPTIONAL
+  );
+
+/**
+  Check if the RomImage contains EFI Images.
+
+  @param  RomImage  The ROM address of Image for check.
+  @param  RomSize   Size of ROM for check.
+
+  @retval TRUE     ROM contain EFI Image.
+  @retval FALSE    ROM not contain EFI Image.
+
+**/
+BOOLEAN
+ContainEfiImage (
+  IN VOID            *RomImage,
+  IN UINT64          RomSize
+  );
+
+/**
+  Get Pci device's oprom information.
+
+  @param PciIoDevice    Input Pci device instance.
+                        Output Pci device instance with updated OptionRom size.
+
+  @retval EFI_NOT_FOUND Pci device has not Option Rom.
+  @retval EFI_SUCCESS   Pci device has Option Rom.
+
+**/
+EFI_STATUS
+GetOpRomInfo (
+  IN OUT PCI_IO_DEVICE    *PciIoDevice
+  );
+
+/**
+  Load Option Rom image for specified PCI device.
+
+  @param PciDevice Pci device instance.
+  @param RomBase   Base address of Option Rom.
+
+  @retval EFI_OUT_OF_RESOURCES No enough memory to hold image.
+  @retval EFI_SUCESS           Successfully loaded Option Rom.
+
+**/
+EFI_STATUS
+LoadOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT64          RomBase
+  );
+
+/**
+  Enable/Disable Option Rom decode.
+
+  @param PciDevice    Pci device instance.
+  @param RomBarIndex  The BAR index of the standard PCI Configuration header to use as the
+                      base address for resource range. The legal range for this field is 0..5.
+  @param RomBar       Base address of Option Rom.
+  @param Enable       Flag for enable/disable decode.
+
+**/
+VOID
+RomDecode (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT8           RomBarIndex,
+  IN UINT32          RomBar,
+  IN BOOLEAN         Enable
+  );
+
+/**
+  Load and start the Option Rom image.
+
+  @param PciDevice       Pci device instance.
+
+  @retval EFI_SUCCESS    Successfully loaded and started PCI Option Rom image.
+  @retval EFI_NOT_FOUND  Failed to process PCI Option Rom image.
+
+**/
+EFI_STATUS
+ProcessOpRomImage (
+  IN PCI_IO_DEVICE   *PciDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c
new file mode 100644
index 0000000000..ab655e7657
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.c
@@ -0,0 +1,88 @@
+/** @file
+  Power management support fucntions implementation for PCI Bus module.
+
+Copyright (c) 2006 - 2012, 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 "PciBus.h"
+
+/**
+  This function is intended to turn off PWE assertion and
+  put the device to D0 state if the device supports
+  PCI Power Management.
+
+  @param PciIoDevice      PCI device instance.
+
+  @retval EFI_UNSUPPORTED PCI Device does not support power management.
+  @retval EFI_SUCCESS     Turned off PWE successfully.
+
+**/
+EFI_STATUS
+ResetPowerManagementFeature (
+  IN PCI_IO_DEVICE *PciIoDevice
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       PowerManagementRegBlock;
+  UINT16      PowerManagementCSR;
+
+  PowerManagementRegBlock = 0;
+
+  Status = LocateCapabilityRegBlock (
+            PciIoDevice,
+            EFI_PCI_CAPABILITY_ID_PMI,
+            &PowerManagementRegBlock,
+            NULL
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Turn off the PWE assertion and put the device into D0 State
+  //
+
+  //
+  // Read PMCSR
+  //
+  Status = PciIoDevice->PciIo.Pci.Read (
+                                    &PciIoDevice->PciIo,
+                                    EfiPciIoWidthUint16,
+                                    PowerManagementRegBlock + 4,
+                                    1,
+                                    &PowerManagementCSR
+                                    );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Clear PME_Status bit
+    //
+    PowerManagementCSR |= BIT15;
+    //
+    // Clear PME_En bit. PowerState = D0.
+    //
+    PowerManagementCSR &= ~(BIT8 | BIT1 | BIT0);
+
+    //
+    // Write PMCSR
+    //
+    Status = PciIoDevice->PciIo.Pci.Write (
+                                      &PciIoDevice->PciIo,
+                                      EfiPciIoWidthUint16,
+                                      PowerManagementRegBlock + 4,
+                                      1,
+                                      &PowerManagementCSR
+                                      );
+  }
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h
new file mode 100644
index 0000000000..45ba59f286
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciPowerManagement.h
@@ -0,0 +1,34 @@
+/** @file
+  Power management support fucntions delaration for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef _EFI_PCI_POWER_MANAGEMENT_H_
+#define _EFI_PCI_POWER_MANAGEMENT_H_
+
+/**
+  This function is intended to turn off PWE assertion and
+  put the device to D0 state if the device supports
+  PCI Power Management.
+
+  @param PciIoDevice      PCI device instance.
+
+  @retval EFI_UNSUPPORTED PCI Device does not support power management.
+  @retval EFI_SUCCESS     Turned off PWE successfully.
+
+**/
+EFI_STATUS
+ResetPowerManagementFeature (
+  IN PCI_IO_DEVICE *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c
new file mode 100644
index 0000000000..e12d59f1d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.c
@@ -0,0 +1,2291 @@
+/** @file
+  PCI resouces support functions implemntation for PCI Bus module.
+
+Copyright (c) 2006 - 2015, 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 "PciBus.h"
+
+//
+// The default policy for the PCI bus driver is NOT to reserve I/O ranges for both ISA aliases and VGA aliases.
+//
+BOOLEAN mReserveIsaAliases = FALSE;
+BOOLEAN mReserveVgaAliases = FALSE;
+BOOLEAN mPolicyDetermined  = FALSE;
+
+/**
+  The function is used to skip VGA range.
+
+  @param Start    Returned start address including VGA range.
+  @param Length   The length of VGA range.
+
+**/
+VOID
+SkipVGAAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  )
+{
+  UINT64  Original;
+  UINT64  Mask;
+  UINT64  StartOffset;
+  UINT64  LimitOffset;
+
+  ASSERT (Start != NULL);
+  //
+  // For legacy VGA, bit 10 to bit 15 is not decoded
+  //
+  Mask        = 0x3FF;
+
+  Original    = *Start;
+  StartOffset = Original & Mask;
+  LimitOffset = ((*Start) + Length - 1) & Mask;
+  if (LimitOffset >= VGABASE1) {
+    *Start = *Start - StartOffset + VGALIMIT2 + 1;
+  }
+}
+
+/**
+  This function is used to skip ISA aliasing aperture.
+
+  @param Start    Returned start address including ISA aliasing aperture.
+  @param Length   The length of ISA aliasing aperture.
+
+**/
+VOID
+SkipIsaAliasAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  )
+{
+
+  UINT64  Original;
+  UINT64  Mask;
+  UINT64  StartOffset;
+  UINT64  LimitOffset;
+
+  ASSERT (Start != NULL);
+
+  //
+  // For legacy ISA, bit 10 to bit 15 is not decoded
+  //
+  Mask        = 0x3FF;
+
+  Original    = *Start;
+  StartOffset = Original & Mask;
+  LimitOffset = ((*Start) + Length - 1) & Mask;
+
+  if (LimitOffset >= ISABASE) {
+    *Start = *Start - StartOffset + ISALIMIT + 1;
+  }
+}
+
+/**
+  This function inserts a resource node into the resource list.
+  The resource list is sorted in descend order.
+
+  @param Bridge  PCI resource node for bridge.
+  @param ResNode Resource node want to be inserted.
+
+**/
+VOID
+InsertResourceNode (
+  IN OUT PCI_RESOURCE_NODE   *Bridge,
+  IN     PCI_RESOURCE_NODE   *ResNode
+  )
+{
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Temp;
+  UINT64            ResNodeAlignRest;
+  UINT64            TempAlignRest;
+
+  ASSERT (Bridge  != NULL);
+  ASSERT (ResNode != NULL);
+
+  InsertHeadList (&Bridge->ChildList, &ResNode->Link);
+
+  CurrentLink = Bridge->ChildList.ForwardLink->ForwardLink;
+  while (CurrentLink != &Bridge->ChildList) {
+    Temp = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (ResNode->Alignment > Temp->Alignment) {
+      break;
+    } else if (ResNode->Alignment == Temp->Alignment) {
+      ResNodeAlignRest  = ResNode->Length & ResNode->Alignment;
+      TempAlignRest     = Temp->Length & Temp->Alignment;
+      if ((ResNodeAlignRest == 0) || (ResNodeAlignRest >= TempAlignRest)) {
+        break;
+      }
+    }
+
+    SwapListEntries (&ResNode->Link, CurrentLink);
+
+    CurrentLink = ResNode->Link.ForwardLink;
+  }
+}
+
+/**
+  This routine is used to merge two different resource trees in need of
+  resoure degradation.
+
+  For example, if an upstream PPB doesn't support,
+  prefetchable memory decoding, the PCI bus driver will choose to call this function
+  to merge prefectchable memory resource list into normal memory list.
+
+  If the TypeMerge is TRUE, Res resource type is changed to the type of destination resource
+  type.
+  If Dst is NULL or Res is NULL, ASSERT ().
+
+  @param Dst        Point to destination resource tree.
+  @param Res        Point to source resource tree.
+  @param TypeMerge  If the TypeMerge is TRUE, Res resource type is changed to the type of
+                    destination resource type.
+
+**/
+VOID
+MergeResourceTree (
+  IN PCI_RESOURCE_NODE   *Dst,
+  IN PCI_RESOURCE_NODE   *Res,
+  IN BOOLEAN             TypeMerge
+  )
+{
+
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Temp;
+
+  ASSERT (Dst != NULL);
+  ASSERT (Res != NULL);
+
+  while (!IsListEmpty (&Res->ChildList)) {
+    CurrentLink = Res->ChildList.ForwardLink;
+
+    Temp        = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (TypeMerge) {
+      Temp->ResType = Dst->ResType;
+    }
+
+    RemoveEntryList (CurrentLink);
+    InsertResourceNode (Dst, Temp);
+  }
+}
+
+/**
+  This function is used to calculate the IO16 aperture
+  for a bridge.
+
+  @param Bridge    PCI resource node for bridge.
+
+**/
+VOID
+CalculateApertureIo16 (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  EFI_STATUS              Status;
+  UINT64                  Aperture;
+  LIST_ENTRY              *CurrentLink;
+  PCI_RESOURCE_NODE       *Node;
+  UINT64                  Offset;
+  EFI_PCI_PLATFORM_POLICY PciPolicy;
+  UINT64                  PaddingAperture;
+
+  if (!mPolicyDetermined) {
+    //
+    // Check PciPlatform policy
+    //
+    Status = EFI_NOT_FOUND;
+    PciPolicy = 0;
+    if (gPciPlatformProtocol != NULL) {
+      Status = gPciPlatformProtocol->GetPlatformPolicy (
+                                       gPciPlatformProtocol,
+                                       &PciPolicy
+                                       );
+    }
+
+    if (EFI_ERROR (Status) && gPciOverrideProtocol != NULL) {
+      Status = gPciOverrideProtocol->GetPlatformPolicy (
+                                       gPciOverrideProtocol,
+                                       &PciPolicy
+                                       );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      if ((PciPolicy & EFI_RESERVE_ISA_IO_ALIAS) != 0) {
+        mReserveIsaAliases = TRUE;
+      }
+      if ((PciPolicy & EFI_RESERVE_VGA_IO_ALIAS) != 0) {
+        mReserveVgaAliases = TRUE;
+      }
+    }
+    mPolicyDetermined = TRUE;
+  }
+
+  Aperture        = 0;
+  PaddingAperture = 0;
+
+  if (Bridge == NULL) {
+    return ;
+  }
+
+  //
+  // Assume the bridge is aligned
+  //
+  for ( CurrentLink = GetFirstNode (&Bridge->ChildList)
+      ; !IsNull (&Bridge->ChildList, CurrentLink)
+      ; CurrentLink = GetNextNode (&Bridge->ChildList, CurrentLink)
+      ) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->ResourceUsage == PciResUsagePadding) {
+      ASSERT (PaddingAperture == 0);
+      PaddingAperture = Node->Length;
+      continue;
+    }
+    //
+    // Consider the aperture alignment
+    //
+    Offset = Aperture & (Node->Alignment);
+
+    if (Offset != 0) {
+
+      Aperture = Aperture + (Node->Alignment + 1) - Offset;
+
+    }
+
+    //
+    // IsaEnable and VGAEnable can not be implemented now.
+    // If both of them are enabled, then the IO resource would
+    // become too limited to meet the requirement of most of devices.
+    //
+    if (mReserveIsaAliases || mReserveVgaAliases) {
+      if (!IS_PCI_BRIDGE (&(Node->PciDev->Pci)) && !IS_CARDBUS_BRIDGE (&(Node->PciDev->Pci))) {
+        //
+        // Check if there is need to support ISA/VGA decoding
+        // If so, we need to avoid isa/vga aliasing range
+        //
+        if (mReserveIsaAliases) {
+          SkipIsaAliasAperture (
+            &Aperture,
+            Node->Length
+            );
+          Offset = Aperture & (Node->Alignment);
+          if (Offset != 0) {
+            Aperture = Aperture + (Node->Alignment + 1) - Offset;
+          }
+        } else if (mReserveVgaAliases) {
+          SkipVGAAperture (
+            &Aperture,
+            Node->Length
+            );
+          Offset = Aperture & (Node->Alignment);
+          if (Offset != 0) {
+            Aperture = Aperture + (Node->Alignment + 1) - Offset;
+          }
+        }
+      }
+    }
+
+    Node->Offset = Aperture;
+
+    //
+    // Increment aperture by the length of node
+    //
+    Aperture += Node->Length;
+  }
+
+  //
+  // Adjust the aperture with the bridge's alignment
+  //
+  Offset = Aperture & (Bridge->Alignment);
+
+  if (Offset != 0) {
+    Aperture = Aperture + (Bridge->Alignment + 1) - Offset;
+  }
+
+  Bridge->Length = Aperture;
+  //
+  // At last, adjust the bridge's alignment to the first child's alignment
+  // if the bridge has at least one child
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  if (CurrentLink != &Bridge->ChildList) {
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->Alignment > Bridge->Alignment) {
+      Bridge->Alignment = Node->Alignment;
+    }
+  }
+
+  //
+  // Hotplug controller needs padding resources.
+  // Use the larger one between the padding resource and actual occupied resource.
+  //
+  Bridge->Length = MAX (Bridge->Length, PaddingAperture);
+}
+
+/**
+  This function is used to calculate the resource aperture
+  for a given bridge device.
+
+  @param Bridge      PCI resouce node for given bridge device.
+
+**/
+VOID
+CalculateResourceAperture (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  UINT64            Aperture;
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Node;
+  UINT64            PaddingAperture;
+  UINT64            Offset;
+
+  Aperture        = 0;
+  PaddingAperture = 0;
+
+  if (Bridge == NULL) {
+    return ;
+  }
+
+  if (Bridge->ResType == PciBarTypeIo16) {
+
+    CalculateApertureIo16 (Bridge);
+    return ;
+  }
+
+  //
+  // Assume the bridge is aligned
+  //
+  for ( CurrentLink = GetFirstNode (&Bridge->ChildList)
+      ; !IsNull (&Bridge->ChildList, CurrentLink)
+      ; CurrentLink = GetNextNode (&Bridge->ChildList, CurrentLink)
+      ) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->ResourceUsage == PciResUsagePadding) {
+      ASSERT (PaddingAperture == 0);
+      PaddingAperture = Node->Length;
+      continue;
+    }
+
+    //
+    // Apply padding resource if available
+    //
+    Offset = Aperture & (Node->Alignment);
+
+    if (Offset != 0) {
+
+      Aperture = Aperture + (Node->Alignment + 1) - Offset;
+
+    }
+
+    //
+    // Recode current aperture as a offset
+    // this offset will be used in future real allocation
+    //
+    Node->Offset = Aperture;
+
+    //
+    // Increment aperture by the length of node
+    //
+    Aperture += Node->Length;
+  }
+
+  //
+  // At last, adjust the aperture with the bridge's
+  // alignment
+  //
+  Offset = Aperture & (Bridge->Alignment);
+  if (Offset != 0) {
+    Aperture = Aperture + (Bridge->Alignment + 1) - Offset;
+  }
+
+  //
+  // If the bridge has already padded the resource and the
+  // amount of padded resource is larger, then keep the
+  // padded resource
+  //
+  if (Bridge->Length < Aperture) {
+    Bridge->Length = Aperture;
+  }
+
+  //
+  // Adjust the bridge's alignment to the first child's alignment
+  // if the bridge has at least one child
+  //
+  CurrentLink = Bridge->ChildList.ForwardLink;
+  if (CurrentLink != &Bridge->ChildList) {
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    if (Node->Alignment > Bridge->Alignment) {
+      Bridge->Alignment = Node->Alignment;
+    }
+  }
+
+  //
+  // Hotplug controller needs padding resources.
+  // Use the larger one between the padding resource and actual occupied resource.
+  //
+  Bridge->Length = MAX (Bridge->Length, PaddingAperture);
+}
+
+/**
+  Get IO/Memory resource infor for given PCI device.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO .
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+GetResourceFromDevice (
+  IN     PCI_IO_DEVICE     *PciDev,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+
+  UINT8             Index;
+  PCI_RESOURCE_NODE *Node;
+  BOOLEAN           ResourceRequested;
+
+  Node              = NULL;
+  ResourceRequested = FALSE;
+
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+
+    switch ((PciDev->PciBar)[Index].BarType) {
+
+    case PciBarTypeMem32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem32Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeMem64:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem64Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypePMem64:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem64Node,
+        Node
+        );
+
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypePMem32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem32Node,
+        Node
+        );
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+
+      Node = CreateResourceNode (
+              PciDev,
+              (PciDev->PciBar)[Index].Length,
+              (PciDev->PciBar)[Index].Alignment,
+              Index,
+              PciBarTypeIo16,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        IoNode,
+        Node
+        );
+      ResourceRequested = TRUE;
+      break;
+
+    case PciBarTypeUnknown:
+      break;
+
+    default:
+      break;
+    }
+  }
+
+  //
+  // Add VF resource
+  //
+  for (Index = 0; Index < PCI_MAX_BAR; Index++) {
+
+    switch ((PciDev->VfPciBar)[Index].BarType) {
+
+    case PciBarTypeMem32:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem32Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypeMem64:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypeMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        Mem64Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypePMem64:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem64,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem64Node,
+        Node
+        );
+
+      break;
+
+    case PciBarTypePMem32:
+
+      Node = CreateVfResourceNode (
+              PciDev,
+              (PciDev->VfPciBar)[Index].Length,
+              (PciDev->VfPciBar)[Index].Alignment,
+              Index,
+              PciBarTypePMem32,
+              PciResUsageTypical
+              );
+
+      InsertResourceNode (
+        PMem32Node,
+        Node
+        );
+      break;
+
+    case PciBarTypeIo16:
+    case PciBarTypeIo32:
+      break;
+
+    case PciBarTypeUnknown:
+      break;
+
+    default:
+      break;
+    }
+  }
+  // If there is no resource requested from this device,
+  // then we indicate this device has been allocated naturally.
+  //
+  if (!ResourceRequested) {
+    PciDev->Allocated = TRUE;
+  }
+}
+
+/**
+  This function is used to create a resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node    = NULL;
+
+  Node    = AllocateZeroPool (sizeof (PCI_RESOURCE_NODE));
+  ASSERT (Node != NULL);
+  if (Node == NULL) {
+    return NULL;
+  }
+
+  Node->Signature     = PCI_RESOURCE_SIGNATURE;
+  Node->PciDev        = PciDev;
+  Node->Length        = Length;
+  Node->Alignment     = Alignment;
+  Node->Bar           = Bar;
+  Node->ResType       = ResType;
+  Node->Reserved      = FALSE;
+  Node->ResourceUsage = ResUsage;
+  InitializeListHead (&Node->ChildList);
+
+  return Node;
+}
+
+/**
+  This function is used to create a IOV VF resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given VF PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateVfResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node = CreateResourceNode (PciDev, Length, Alignment, Bar, ResType, ResUsage);
+  if (Node == NULL) {
+    return Node;
+  }
+
+  Node->Virtual = TRUE;
+
+  return Node;
+}
+
+/**
+  This function is used to extract resource request from
+  device node list.
+
+  @param Bridge     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+CreateResourceMap (
+  IN     PCI_IO_DEVICE     *Bridge,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  PCI_IO_DEVICE     *Temp;
+  PCI_RESOURCE_NODE *IoBridge;
+  PCI_RESOURCE_NODE *Mem32Bridge;
+  PCI_RESOURCE_NODE *PMem32Bridge;
+  PCI_RESOURCE_NODE *Mem64Bridge;
+  PCI_RESOURCE_NODE *PMem64Bridge;
+  LIST_ENTRY        *CurrentLink;
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != NULL && CurrentLink != &Bridge->ChildList) {
+
+    Temp = PCI_IO_DEVICE_FROM_LINK (CurrentLink);
+
+    //
+    // Create resource nodes for this device by scanning the
+    // Bar array in the device private data
+    // If the upstream bridge doesn't support this device,
+    // no any resource node will be created for this device
+    //
+    GetResourceFromDevice (
+      Temp,
+      IoNode,
+      Mem32Node,
+      PMem32Node,
+      Mem64Node,
+      PMem64Node
+      );
+
+    if (IS_PCI_BRIDGE (&Temp->Pci)) {
+
+      //
+      // If the device has children, create a bridge resource node for this PPB
+      // Note: For PPB, memory aperture is aligned with 1MB and IO aperture
+      // is aligned with 4KB (smaller alignments may be supported).
+      //
+      IoBridge = CreateResourceNode (
+                   Temp,
+                   0,
+                   Temp->BridgeIoAlignment,
+                   PPB_IO_RANGE,
+                   PciBarTypeIo16,
+                   PciResUsageTypical
+                   );
+
+      Mem32Bridge = CreateResourceNode (
+                      Temp,
+                      0,
+                      0xFFFFF,
+                      PPB_MEM32_RANGE,
+                      PciBarTypeMem32,
+                      PciResUsageTypical
+                      );
+
+      PMem32Bridge = CreateResourceNode (
+                       Temp,
+                       0,
+                       0xFFFFF,
+                       PPB_PMEM32_RANGE,
+                       PciBarTypePMem32,
+                       PciResUsageTypical
+                       );
+
+      Mem64Bridge = CreateResourceNode (
+                      Temp,
+                      0,
+                      0xFFFFF,
+                      PPB_MEM64_RANGE,
+                      PciBarTypeMem64,
+                      PciResUsageTypical
+                      );
+
+      PMem64Bridge = CreateResourceNode (
+                       Temp,
+                       0,
+                       0xFFFFF,
+                       PPB_PMEM64_RANGE,
+                       PciBarTypePMem64,
+                       PciResUsageTypical
+                       );
+
+      //
+      // Recursively create resouce map on this bridge
+      //
+      CreateResourceMap (
+        Temp,
+        IoBridge,
+        Mem32Bridge,
+        PMem32Bridge,
+        Mem64Bridge,
+        PMem64Bridge
+        );
+
+      if (ResourceRequestExisted (IoBridge)) {
+        InsertResourceNode (
+          IoNode,
+          IoBridge
+          );
+      } else {
+        FreePool (IoBridge);
+        IoBridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (Mem32Bridge)) {
+        InsertResourceNode (
+          Mem32Node,
+          Mem32Bridge
+          );
+      } else {
+        FreePool (Mem32Bridge);
+        Mem32Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (PMem32Bridge)) {
+        InsertResourceNode (
+          PMem32Node,
+          PMem32Bridge
+          );
+      } else {
+        FreePool (PMem32Bridge);
+        PMem32Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (Mem64Bridge)) {
+        InsertResourceNode (
+          Mem64Node,
+          Mem64Bridge
+          );
+      } else {
+        FreePool (Mem64Bridge);
+        Mem64Bridge = NULL;
+      }
+
+      //
+      // If there is node under this resource bridge,
+      // then calculate bridge's aperture of this type
+      // and insert it into the respective resource tree.
+      // If no, delete this resource bridge
+      //
+      if (ResourceRequestExisted (PMem64Bridge)) {
+        InsertResourceNode (
+          PMem64Node,
+          PMem64Bridge
+          );
+      } else {
+        FreePool (PMem64Bridge);
+        PMem64Bridge = NULL;
+      }
+
+    }
+
+    //
+    // If it is P2C, apply hard coded resource padding
+    //
+    if (IS_CARDBUS_BRIDGE (&Temp->Pci)) {
+      ResourcePaddingForCardBusBridge (
+        Temp,
+        IoNode,
+        Mem32Node,
+        PMem32Node,
+        Mem64Node,
+        PMem64Node
+        );
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  //
+  // To do some platform specific resource padding ...
+  //
+  ResourcePaddingPolicy (
+    Bridge,
+    IoNode,
+    Mem32Node,
+    PMem32Node,
+    Mem64Node,
+    PMem64Node
+    );
+
+  //
+  // Degrade resource if necessary
+  //
+  DegradeResource (
+    Bridge,
+    Mem32Node,
+    PMem32Node,
+    Mem64Node,
+    PMem64Node
+    );
+
+  //
+  // Calculate resource aperture for this bridge device
+  //
+  CalculateResourceAperture (Mem32Node);
+  CalculateResourceAperture (PMem32Node);
+  CalculateResourceAperture (Mem64Node);
+  CalculateResourceAperture (PMem64Node);
+  CalculateResourceAperture (IoNode);
+}
+
+/**
+  This function is used to do the resource padding for a specific platform.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingPolicy (
+  IN PCI_IO_DEVICE     *PciDev,
+  IN PCI_RESOURCE_NODE *IoNode,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  //
+  // Create padding resource node
+  //
+  if (PciDev->ResourcePaddingDescriptors != NULL) {
+    ApplyResourcePadding (
+      PciDev,
+      IoNode,
+      Mem32Node,
+      PMem32Node,
+      Mem64Node,
+      PMem64Node
+      );
+  }
+}
+
+/**
+  This function is used to degrade resource if the upstream bridge
+  doesn't support certain resource. Degradation path is
+  PMEM64 -> MEM64  -> MEM32
+  PMEM64 -> PMEM32 -> MEM32
+  IO32   -> IO16.
+
+  @param Bridge     Pci device instance.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+DegradeResource (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  )
+{
+  PCI_IO_DEVICE        *Temp;
+  LIST_ENTRY           *ChildDeviceLink;
+  LIST_ENTRY           *ChildNodeLink;
+  LIST_ENTRY           *NextChildNodeLink;
+  PCI_RESOURCE_NODE    *TempNode;
+
+  //
+  // If any child device has both option ROM and 64-bit BAR, degrade its PMEM64/MEM64
+  // requests in case that if a legacy option ROM image can not access 64-bit resources.
+  //
+  ChildDeviceLink = Bridge->ChildList.ForwardLink;
+  while (ChildDeviceLink != NULL && ChildDeviceLink != &Bridge->ChildList) {
+    Temp = PCI_IO_DEVICE_FROM_LINK (ChildDeviceLink);
+    if (Temp->RomSize != 0) {
+      if (!IsListEmpty (&Mem64Node->ChildList)) {      
+        ChildNodeLink = Mem64Node->ChildList.ForwardLink;
+        while (ChildNodeLink != &Mem64Node->ChildList) {
+          TempNode = RESOURCE_NODE_FROM_LINK (ChildNodeLink);
+          NextChildNodeLink = ChildNodeLink->ForwardLink;
+
+          if (TempNode->PciDev == Temp) {
+            RemoveEntryList (ChildNodeLink);
+            InsertResourceNode (Mem32Node, TempNode);
+          }
+          ChildNodeLink = NextChildNodeLink;
+        }        
+      }
+
+      if (!IsListEmpty (&PMem64Node->ChildList)) {      
+        ChildNodeLink = PMem64Node->ChildList.ForwardLink;
+        while (ChildNodeLink != &PMem64Node->ChildList) {
+          TempNode = RESOURCE_NODE_FROM_LINK (ChildNodeLink);
+          NextChildNodeLink = ChildNodeLink->ForwardLink;
+
+          if (TempNode->PciDev == Temp) {
+            RemoveEntryList (ChildNodeLink);
+            InsertResourceNode (PMem32Node, TempNode);
+          }
+          ChildNodeLink = NextChildNodeLink;
+        }        
+      }
+
+    }
+    ChildDeviceLink = ChildDeviceLink->ForwardLink;
+  }
+
+  //
+  // If firmware is in 32-bit mode,
+  // then degrade PMEM64/MEM64 requests
+  //
+  if (sizeof (UINTN) <= 4) {
+    MergeResourceTree (
+      Mem32Node,
+      Mem64Node,
+      TRUE
+      );
+
+    MergeResourceTree (
+      PMem32Node,
+      PMem64Node,
+      TRUE
+      );
+  } else {
+    //
+    // if the bridge does not support MEM64, degrade MEM64 to MEM32
+    //
+    if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_MEM64_DECODE_SUPPORTED)) {
+        MergeResourceTree (
+          Mem32Node,
+          Mem64Node,
+          TRUE
+          );
+    }
+
+    //
+    // if the bridge does not support PMEM64, degrade PMEM64 to PMEM32
+    //
+    if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM64_DECODE_SUPPORTED)) {
+      MergeResourceTree (
+        PMem32Node,
+        PMem64Node,
+        TRUE
+        );
+    } 
+
+    //
+    // if both PMEM64 and PMEM32 requests from child devices, which can not be satisfied
+    // by a P2P bridge simultaneously, keep PMEM64 and degrade PMEM32 to MEM32.
+    //
+    if (!IsListEmpty (&PMem64Node->ChildList) && Bridge->Parent != NULL) {
+      MergeResourceTree (
+        Mem32Node,
+        PMem32Node,
+        TRUE
+        );
+    }
+  }
+
+  //
+  // If bridge doesn't support Pmem32
+  // degrade it to mem32
+  //
+  if (!BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM32_DECODE_SUPPORTED)) {
+    MergeResourceTree (
+      Mem32Node,
+      PMem32Node,
+      TRUE
+      );
+  }
+
+  //
+  // if root bridge supports combined Pmem Mem decoding
+  // merge these two type of resource
+  //
+  if (BridgeSupportResourceDecode (Bridge, EFI_BRIDGE_PMEM_MEM_COMBINE_SUPPORTED)) {
+    MergeResourceTree (
+      Mem32Node,
+      PMem32Node,
+      FALSE
+      );
+
+    //
+    // No need to check if to degrade MEM64 after merge, because
+    // if there are PMEM64 still here, 64-bit decode should be supported
+    // by the root bride.
+    //
+    MergeResourceTree (
+      Mem64Node,
+      PMem64Node,
+      FALSE
+      );
+  }
+}
+
+/**
+  Test whether bridge device support decode resource.
+
+  @param Bridge    Bridge device instance.
+  @param Decode    Decode type according to resource type.
+
+  @return TRUE     The bridge device support decode resource.
+  @return FALSE    The bridge device don't support decode resource.
+
+**/
+BOOLEAN
+BridgeSupportResourceDecode (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT32        Decode
+  )
+{
+  if (((Bridge->Decodes) & Decode) != 0) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  This function is used to program the resource allocated
+  for each resource node under specified bridge.
+
+  @param Base     Base address of resource to be progammed.
+  @param Bridge   PCI resource node for the bridge device.
+
+  @retval EFI_SUCCESS            Successfully to program all resouces
+                                 on given PCI bridge device.
+  @retval EFI_OUT_OF_RESOURCES   Base is all one.
+
+**/
+EFI_STATUS
+ProgramResource (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Bridge
+  )
+{
+  LIST_ENTRY        *CurrentLink;
+  PCI_RESOURCE_NODE *Node;
+  EFI_STATUS        Status;
+
+  if (Base == gAllOne) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CurrentLink = Bridge->ChildList.ForwardLink;
+
+  while (CurrentLink != &Bridge->ChildList) {
+
+    Node = RESOURCE_NODE_FROM_LINK (CurrentLink);
+
+    if (!IS_PCI_BRIDGE (&(Node->PciDev->Pci))) {
+
+      if (IS_CARDBUS_BRIDGE (&(Node->PciDev->Pci))) {
+        //
+        // Program the PCI Card Bus device
+        //
+        ProgramP2C (Base, Node);
+      } else {
+        //
+        // Program the PCI device BAR
+        //
+        ProgramBar (Base, Node);
+      }
+    } else {
+      //
+      // Program the PCI devices under this bridge
+      //
+      Status = ProgramResource (Base + Node->Offset, Node);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      ProgramPpbApperture (Base, Node);
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  ASSERT (Node->Bar < PCI_MAX_BAR);
+
+  //
+  // Check VF BAR
+  //
+  if (Node->Virtual) {
+    ProgramVfBar (Base, Node);
+    return;
+  }
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch ((Node->PciDev->PciBar[Node->Bar]).BarType) {
+
+  case PciBarTypeIo16:
+  case PciBarTypeIo32:
+  case PciBarTypeMem32:
+  case PciBarTypePMem32:
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+
+    break;
+
+  case PciBarTypeMem64:
+  case PciBarTypePMem64:
+
+    Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (UINT8) ((Node->PciDev->PciBar[Node->Bar]).Offset + 4),
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Program IOV VF Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+EFI_STATUS
+ProgramVfBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  ASSERT (Node->Bar < PCI_MAX_BAR);
+  ASSERT (Node->Virtual);
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch ((Node->PciDev->VfPciBar[Node->Bar]).BarType) {
+
+  case PciBarTypeMem32:
+  case PciBarTypePMem32:
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                (Node->PciDev->VfPciBar[Node->Bar]).Offset,
+                1,
+                &Address
+                );
+
+    Node->PciDev->VfPciBar[Node->Bar].BaseAddress = Address;
+    break;
+
+  case PciBarTypeMem64:
+  case PciBarTypePMem64:
+
+    Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                (Node->PciDev->VfPciBar[Node->Bar]).Offset,
+                1,
+                &Address32
+                );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+
+    PciIo->Pci.Write (
+                PciIo,
+                EfiPciIoWidthUint32,
+                ((Node->PciDev->VfPciBar[Node->Bar]).Offset + 4),
+                1,
+                &Address32
+                );
+
+    Node->PciDev->VfPciBar[Node->Bar].BaseAddress = Address;
+    break;
+
+  case PciBarTypeIo16:
+  case PciBarTypeIo32:
+    break;
+
+  default:
+    break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Program PCI-PCI bridge apperture.
+
+  @param Base  Base address for resource.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramPpbApperture (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT32              Address32;
+
+  Address = 0;
+  //
+  // If no device resource of this PPB, return anyway
+  // Apperture is set default in the initialization code
+  //
+  if (Node->Length == 0 || Node->ResourceUsage == PciResUsagePadding) {
+    //
+    // For padding resource node, just ignore when programming
+    //
+    return ;
+  }
+
+  PciIo   = &(Node->PciDev->PciIo);
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate the PPB resource has been allocated
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch (Node->Bar) {
+
+  case PPB_BAR_0:
+  case PPB_BAR_1:
+    switch ((Node->PciDev->PciBar[Node->Bar]).BarType) {
+
+      case PciBarTypeIo16:
+      case PciBarTypeIo32:
+      case PciBarTypeMem32:
+      case PciBarTypePMem32:
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+        Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+        Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+        break;
+
+      case PciBarTypeMem64:
+      case PciBarTypePMem64:
+
+        Address32 = (UINT32) (Address & 0x00000000FFFFFFFF);
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address32
+                 );
+
+        Address32 = (UINT32) RShiftU64 (Address, 32);
+
+        PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (UINT8) ((Node->PciDev->PciBar[Node->Bar]).Offset + 4),
+                 1,
+                 &Address32
+                 );
+
+        Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+        Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+        break;
+
+      default:
+        break;
+    }
+    break;
+
+  case PPB_IO_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint8,
+                 0x1C,
+                 1,
+                 &Address32
+                 );
+
+    Address32 >>= 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x30,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint8,
+                 0x1D,
+                 1,
+                 &Address32
+                 );
+
+    Address32 >>= 8;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x32,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case PPB_MEM32_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x20,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x22,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case PPB_PMEM32_RANGE:
+  case PPB_PMEM64_RANGE:
+
+    Address32 = ((UINT32) (Address)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x24,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) (Address + Node->Length - 1);
+    Address32 = ((UINT32) (Address32)) >> 16;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint16,
+                 0x26,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 (Address, 32);
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 0x28,
+                 1,
+                 &Address32
+                 );
+
+    Address32 = (UINT32) RShiftU64 ((Address + Node->Length - 1), 32);
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 0x2C,
+                 1,
+                 &Address32
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Program parent bridge for Option Rom.
+
+  @param PciDevice      Pci deivce instance.
+  @param OptionRomBase  Base address for Optiona Rom.
+  @param Enable         Enable or disable PCI memory.
+
+**/
+VOID
+ProgrameUpstreamBridgeForRom (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT32          OptionRomBase,
+  IN BOOLEAN         Enable
+  )
+{
+  PCI_IO_DEVICE     *Parent;
+  PCI_RESOURCE_NODE Node;
+  //
+  // For root bridge, just return.
+  //
+  Parent = PciDevice->Parent;
+  ZeroMem (&Node, sizeof (Node));
+  while (Parent != NULL) {
+    if (!IS_PCI_BRIDGE (&Parent->Pci)) {
+      break;
+    }
+
+    Node.PciDev     = Parent;
+    Node.Length     = PciDevice->RomSize;
+    Node.Alignment  = 0;
+    Node.Bar        = PPB_MEM32_RANGE;
+    Node.ResType    = PciBarTypeMem32;
+    Node.Offset     = 0;
+
+    //
+    // Program PPB to only open a single <= 16MB apperture
+    //
+    if (Enable) {
+      ProgramPpbApperture (OptionRomBase, &Node);
+      PCI_ENABLE_COMMAND_REGISTER (Parent, EFI_PCI_COMMAND_MEMORY_SPACE);
+    } else {
+      InitializePpb (Parent);
+      PCI_DISABLE_COMMAND_REGISTER (Parent, EFI_PCI_COMMAND_MEMORY_SPACE);
+    }
+
+    Parent = Parent->Parent;
+  }
+}
+
+/**
+  Test whether resource exists for a bridge.
+
+  @param Bridge  Point to resource node for a bridge.
+
+  @retval TRUE   There is resource on the given bridge.
+  @retval FALSE  There isn't resource on the given bridge.
+
+**/
+BOOLEAN
+ResourceRequestExisted (
+  IN PCI_RESOURCE_NODE    *Bridge
+  )
+{
+  if (Bridge != NULL) {
+    if (!IsListEmpty (&Bridge->ChildList) || Bridge->Length != 0) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Initialize resource pool structure.
+
+  @param ResourcePool Point to resource pool structure. This pool
+                      is reset to all zero when returned.
+  @param ResourceType Type of resource.
+
+**/
+VOID
+InitializeResourcePool (
+  IN OUT PCI_RESOURCE_NODE   *ResourcePool,
+  IN     PCI_BAR_TYPE        ResourceType
+  )
+{
+  ZeroMem (ResourcePool, sizeof (PCI_RESOURCE_NODE));
+  ResourcePool->ResType   = ResourceType;
+  ResourcePool->Signature = PCI_RESOURCE_SIGNATURE;
+  InitializeListHead (&ResourcePool->ChildList);
+}
+
+/**
+  Destory given resource tree.
+
+  @param Bridge  PCI resource root node of resource tree.
+
+**/
+VOID
+DestroyResourceTree (
+  IN PCI_RESOURCE_NODE *Bridge
+  )
+{
+  PCI_RESOURCE_NODE *Temp;
+  LIST_ENTRY        *CurrentLink;
+
+  while (!IsListEmpty (&Bridge->ChildList)) {
+
+    CurrentLink = Bridge->ChildList.ForwardLink;
+
+    Temp        = RESOURCE_NODE_FROM_LINK (CurrentLink);
+    ASSERT (Temp);
+
+    RemoveEntryList (CurrentLink);
+
+    if (IS_PCI_BRIDGE (&(Temp->PciDev->Pci))) {
+      DestroyResourceTree (Temp);
+    }
+
+    FreePool (Temp);
+  }
+}
+
+/**
+  Insert resource padding for P2C.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingForCardBusBridge (
+  IN PCI_IO_DEVICE        *PciDev,
+  IN PCI_RESOURCE_NODE    *IoNode,
+  IN PCI_RESOURCE_NODE    *Mem32Node,
+  IN PCI_RESOURCE_NODE    *PMem32Node,
+  IN PCI_RESOURCE_NODE    *Mem64Node,
+  IN PCI_RESOURCE_NODE    *PMem64Node
+  )
+{
+  PCI_RESOURCE_NODE *Node;
+
+  Node = NULL;
+
+  //
+  // Memory Base/Limit Register 0
+  // Bar 1 denodes memory range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x2000000,
+           0x1ffffff,
+           1,
+           PciBarTypeMem32,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    Mem32Node,
+    Node
+    );
+
+  //
+  // Memory Base/Limit Register 1
+  // Bar 2 denodes memory range1
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x2000000,
+           0x1ffffff,
+           2,
+           PciBarTypePMem32,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    PMem32Node,
+    Node
+    );
+
+  //
+  // Io Base/Limit
+  // Bar 3 denodes io range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x100,
+           0xff,
+           3,
+           PciBarTypeIo16,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    IoNode,
+    Node
+    );
+
+  //
+  // Io Base/Limit
+  // Bar 4 denodes io range 0
+  //
+  Node = CreateResourceNode (
+           PciDev,
+           0x100,
+           0xff,
+           4,
+           PciBarTypeIo16,
+           PciResUsagePadding
+           );
+
+  InsertResourceNode (
+    IoNode,
+    Node
+    );
+}
+
+/**
+  Program PCI Card device register for given resource node.
+
+  @param Base    Base address of PCI Card device to be programmed.
+  @param Node    Given resource node.
+
+**/
+VOID
+ProgramP2C (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  )
+{
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  UINT64              Address;
+  UINT64              TempAddress;
+  UINT16              BridgeControl;
+
+  Address = 0;
+  PciIo   = &(Node->PciDev->PciIo);
+
+  Address = Base + Node->Offset;
+
+  //
+  // Indicate pci bus driver has allocated
+  // resource for this device
+  // It might be a temporary solution here since
+  // pci device could have multiple bar
+  //
+  Node->PciDev->Allocated = TRUE;
+
+  switch (Node->Bar) {
+
+  case P2C_BAR_0:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 (Node->PciDev->PciBar[Node->Bar]).Offset,
+                 1,
+                 &Address
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    break;
+
+  case P2C_MEM_1:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_BASE_0,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_LIMIT_0,
+                 1,
+                 &TempAddress
+                 );
+
+    if (Node->ResType == PciBarTypeMem32) {
+      //
+      // Set non-prefetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl &= (UINT16) ~PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+    } else {
+      //
+      // Set pre-fetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl |= PCI_CARD_PREFETCHABLE_MEMORY_0_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+    }
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+
+    break;
+
+  case P2C_MEM_2:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_BASE_1,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_MEMORY_LIMIT_1,
+                 1,
+                 &TempAddress
+                 );
+
+    if (Node->ResType == PciBarTypeMem32) {
+
+      //
+      // Set non-prefetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl &= (UINT16) ~(PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE);
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+    } else {
+
+      //
+      // Set pre-fetchable bit
+      //
+      PciIo->Pci.Read (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+
+      BridgeControl |= PCI_CARD_PREFETCHABLE_MEMORY_1_ENABLE;
+      PciIo->Pci.Write (
+                   PciIo,
+                   EfiPciIoWidthUint16,
+                   PCI_CARD_BRIDGE_CONTROL,
+                   1,
+                   &BridgeControl
+                   );
+    }
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+    break;
+
+  case P2C_IO_1:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_BASE_0_LOWER,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_LIMIT_0_LOWER,
+                 1,
+                 &TempAddress
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+
+    break;
+
+  case P2C_IO_2:
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_BASE_1_LOWER,
+                 1,
+                 &Address
+                 );
+
+    TempAddress = Address + Node->Length - 1;
+    PciIo->Pci.Write (
+                 PciIo,
+                 EfiPciIoWidthUint32,
+                 PCI_CARD_IO_LIMIT_1_LOWER,
+                 1,
+                 &TempAddress
+                 );
+
+    Node->PciDev->PciBar[Node->Bar].BaseAddress = Address;
+    Node->PciDev->PciBar[Node->Bar].Length      = Node->Length;
+    Node->PciDev->PciBar[Node->Bar].BarType     = Node->ResType;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Create padding resource node.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ApplyResourcePadding (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN PCI_RESOURCE_NODE     *IoNode,
+  IN PCI_RESOURCE_NODE     *Mem32Node,
+  IN PCI_RESOURCE_NODE     *PMem32Node,
+  IN PCI_RESOURCE_NODE     *Mem64Node,
+  IN PCI_RESOURCE_NODE     *PMem64Node
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Ptr;
+  PCI_RESOURCE_NODE                 *Node;
+  UINT8                             DummyBarIndex;
+
+  DummyBarIndex = 0;
+  Ptr           = PciDev->ResourcePaddingDescriptors;
+
+  while (((EFI_ACPI_END_TAG_DESCRIPTOR *) Ptr)->Desc != ACPI_END_TAG_DESCRIPTOR) {
+
+    if (Ptr->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Ptr->ResType == ACPI_ADDRESS_SPACE_TYPE_IO) {
+      if (Ptr->AddrLen != 0) {
+
+        Node = CreateResourceNode (
+                PciDev,
+                Ptr->AddrLen,
+                Ptr->AddrRangeMax,
+                DummyBarIndex,
+                PciBarTypeIo16,
+                PciResUsagePadding
+                );
+        InsertResourceNode (
+          IoNode,
+          Node
+          );
+      }
+
+      Ptr++;
+      continue;
+    }
+
+    if (Ptr->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR && Ptr->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+
+      if (Ptr->AddrSpaceGranularity == 32) {
+
+        //
+        // prefechable
+        //
+        if (Ptr->SpecificFlag == 0x6) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypePMem32,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              PMem32Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Ptr->SpecificFlag == 0) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypeMem32,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              Mem32Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+      }
+
+      if (Ptr->AddrSpaceGranularity == 64) {
+
+        //
+        // prefechable
+        //
+        if (Ptr->SpecificFlag == 0x6) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypePMem64,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              PMem64Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+
+        //
+        // Non-prefechable
+        //
+        if (Ptr->SpecificFlag == 0) {
+          if (Ptr->AddrLen != 0) {
+            Node = CreateResourceNode (
+                    PciDev,
+                    Ptr->AddrLen,
+                    Ptr->AddrRangeMax,
+                    DummyBarIndex,
+                    PciBarTypeMem64,
+                    PciResUsagePadding
+                    );
+            InsertResourceNode (
+              Mem64Node,
+              Node
+              );
+          }
+
+          Ptr++;
+          continue;
+        }
+      }
+    }
+
+    Ptr++;
+  }
+}
+
+/**
+  Get padding resource for PCI-PCI bridge.
+
+  @param  PciIoDevice     PCI-PCI bridge device instance.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determines
+          whether need to pad resource for them.
+**/
+VOID
+GetResourcePaddingPpb (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  )
+{
+  if (gPciHotPlugInit != NULL && FeaturePcdGet (PcdPciBusHotplugDeviceSupport)) {
+    if (PciIoDevice->ResourcePaddingDescriptors == NULL) {
+      GetResourcePaddingForHpb (PciIoDevice);
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h
new file mode 100644
index 0000000000..763ddbc4ed
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciResourceSupport.h
@@ -0,0 +1,463 @@
+/** @file
+  PCI resouces support functions declaration for PCI Bus module.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _EFI_PCI_RESOURCE_SUPPORT_H_
+#define _EFI_PCI_RESOURCE_SUPPORT_H_
+
+typedef enum {
+  PciResUsageTypical,
+  PciResUsagePadding
+} PCI_RESOURCE_USAGE;
+
+#define PCI_RESOURCE_SIGNATURE  SIGNATURE_32 ('p', 'c', 'r', 'c')
+
+typedef struct {
+  UINT32              Signature;
+  LIST_ENTRY          Link;
+  LIST_ENTRY          ChildList;
+  PCI_IO_DEVICE       *PciDev;
+  UINT64              Alignment;
+  UINT64              Offset;
+  UINT8               Bar;
+  PCI_BAR_TYPE        ResType;
+  UINT64              Length;
+  BOOLEAN             Reserved;
+  PCI_RESOURCE_USAGE  ResourceUsage;
+  BOOLEAN             Virtual;
+} PCI_RESOURCE_NODE;
+
+#define RESOURCE_NODE_FROM_LINK(a) \
+  CR (a, PCI_RESOURCE_NODE, Link, PCI_RESOURCE_SIGNATURE)
+
+/**
+  The function is used to skip VGA range.
+
+  @param Start    Returned start address including VGA range.
+  @param Length   The length of VGA range.
+
+**/
+VOID
+SkipVGAAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  );
+
+/**
+  This function is used to skip ISA aliasing aperture.
+
+  @param Start    Returned start address including ISA aliasing aperture.
+  @param Length   The length of ISA aliasing aperture.
+
+**/
+VOID
+SkipIsaAliasAperture (
+  OUT UINT64   *Start,
+  IN  UINT64   Length
+  );
+
+/**
+  This function inserts a resource node into the resource list.
+  The resource list is sorted in descend order.
+
+  @param Bridge  PCI resource node for bridge.
+  @param ResNode Resource node want to be inserted.
+
+**/
+VOID
+InsertResourceNode (
+  IN OUT PCI_RESOURCE_NODE   *Bridge,
+  IN     PCI_RESOURCE_NODE   *ResNode
+  );
+
+/**
+  This routine is used to merge two different resource trees in need of
+  resoure degradation.
+
+  For example, if an upstream PPB doesn't support,
+  prefetchable memory decoding, the PCI bus driver will choose to call this function
+  to merge prefectchable memory resource list into normal memory list.
+
+  If the TypeMerge is TRUE, Res resource type is changed to the type of destination resource
+  type.
+  If Dst is NULL or Res is NULL, ASSERT ().
+
+  @param Dst        Point to destination resource tree.
+  @param Res        Point to source resource tree.
+  @param TypeMerge  If the TypeMerge is TRUE, Res resource type is changed to the type of
+                    destination resource type.
+
+**/
+VOID
+MergeResourceTree (
+  IN PCI_RESOURCE_NODE   *Dst,
+  IN PCI_RESOURCE_NODE   *Res,
+  IN BOOLEAN             TypeMerge
+  );
+
+/**
+  This function is used to calculate the IO16 aperture
+  for a bridge.
+
+  @param Bridge    PCI resource node for bridge.
+
+**/
+VOID
+CalculateApertureIo16 (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  This function is used to calculate the resource aperture
+  for a given bridge device.
+
+  @param Bridge      PCI resouce node for given bridge device.
+
+**/
+VOID
+CalculateResourceAperture (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  Get IO/Memory resource infor for given PCI device.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO .
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+GetResourceFromDevice (
+  IN     PCI_IO_DEVICE     *PciDev,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to create a resource node.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  );
+
+/**
+  This function is used to extract resource request from
+  IOV VF device node list.
+
+  @param PciDev       Pci device instance.
+  @param Length       Length of Io/Memory resource.
+  @param Alignment    Alignment of resource.
+  @param Bar          Bar index.
+  @param ResType      Type of resource: IO/Memory.
+  @param ResUsage     Resource usage.
+
+  @return PCI resource node created for given PCI device.
+          NULL means PCI resource node is not created.
+
+**/
+PCI_RESOURCE_NODE *
+CreateVfResourceNode (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN UINT64                Length,
+  IN UINT64                Alignment,
+  IN UINT8                 Bar,
+  IN PCI_BAR_TYPE          ResType,
+  IN PCI_RESOURCE_USAGE    ResUsage
+  );
+
+/**
+  This function is used to extract resource request from
+  device node list.
+
+  @param Bridge     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+CreateResourceMap (
+  IN     PCI_IO_DEVICE     *Bridge,
+  IN OUT PCI_RESOURCE_NODE *IoNode,
+  IN OUT PCI_RESOURCE_NODE *Mem32Node,
+  IN OUT PCI_RESOURCE_NODE *PMem32Node,
+  IN OUT PCI_RESOURCE_NODE *Mem64Node,
+  IN OUT PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to do the resource padding for a specific platform.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingPolicy (
+  IN PCI_IO_DEVICE     *PciDev,
+  IN PCI_RESOURCE_NODE *IoNode,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  This function is used to degrade resource if the upstream bridge
+  doesn't support certain resource. Degradation path is
+  PMEM64 -> MEM64  -> MEM32
+  PMEM64 -> PMEM32 -> MEM32
+  IO32   -> IO16.
+
+  @param Bridge     Pci device instance.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+DegradeResource (
+  IN PCI_IO_DEVICE     *Bridge,
+  IN PCI_RESOURCE_NODE *Mem32Node,
+  IN PCI_RESOURCE_NODE *PMem32Node,
+  IN PCI_RESOURCE_NODE *Mem64Node,
+  IN PCI_RESOURCE_NODE *PMem64Node
+  );
+
+/**
+  Test whether bridge device support decode resource.
+
+  @param Bridge    Bridge device instance.
+  @param Decode    Decode type according to resource type.
+
+  @return TRUE     The bridge device support decode resource.
+  @return FALSE    The bridge device don't support decode resource.
+
+**/
+BOOLEAN
+BridgeSupportResourceDecode (
+  IN PCI_IO_DEVICE *Bridge,
+  IN UINT32        Decode
+  );
+
+/**
+  This function is used to program the resource allocated
+  for each resource node under specified bridge.
+
+  @param Base     Base address of resource to be progammed.
+  @param Bridge   PCI resource node for the bridge device.
+
+  @retval EFI_SUCCESS            Successfully to program all resouces
+                                 on given PCI bridge device.
+  @retval EFI_OUT_OF_RESOURCES   Base is all one.
+
+**/
+EFI_STATUS
+ProgramResource (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Bridge
+  );
+
+/**
+  Program Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program IOV VF Bar register for PCI device.
+
+  @param Base  Base address for PCI device resource to be progammed.
+  @param Node  Point to resoure node structure.
+
+**/
+EFI_STATUS
+ProgramVfBar (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program PCI-PCI bridge apperture.
+
+  @param Base  Base address for resource.
+  @param Node  Point to resoure node structure.
+
+**/
+VOID
+ProgramPpbApperture (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Program parent bridge for Option Rom.
+
+  @param PciDevice      Pci deivce instance.
+  @param OptionRomBase  Base address for Optiona Rom.
+  @param Enable         Enable or disable PCI memory.
+
+**/
+VOID
+ProgrameUpstreamBridgeForRom (
+  IN PCI_IO_DEVICE   *PciDevice,
+  IN UINT32          OptionRomBase,
+  IN BOOLEAN         Enable
+  );
+
+/**
+  Test whether resource exists for a bridge.
+
+  @param Bridge  Point to resource node for a bridge.
+
+  @retval TRUE   There is resource on the given bridge.
+  @retval FALSE  There isn't resource on the given bridge.
+
+**/
+BOOLEAN
+ResourceRequestExisted (
+  IN PCI_RESOURCE_NODE    *Bridge
+  );
+
+/**
+  Initialize resource pool structure.
+
+  @param ResourcePool Point to resource pool structure. This pool
+                      is reset to all zero when returned.
+  @param ResourceType Type of resource.
+
+**/
+VOID
+InitializeResourcePool (
+  IN OUT PCI_RESOURCE_NODE   *ResourcePool,
+  IN     PCI_BAR_TYPE        ResourceType
+  );
+
+/**
+  Destory given resource tree.
+
+  @param Bridge  PCI resource root node of resource tree.
+
+**/
+VOID
+DestroyResourceTree (
+  IN PCI_RESOURCE_NODE *Bridge
+  );
+
+/**
+  Insert resource padding for P2C.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ResourcePaddingForCardBusBridge (
+  IN PCI_IO_DEVICE        *PciDev,
+  IN PCI_RESOURCE_NODE    *IoNode,
+  IN PCI_RESOURCE_NODE    *Mem32Node,
+  IN PCI_RESOURCE_NODE    *PMem32Node,
+  IN PCI_RESOURCE_NODE    *Mem64Node,
+  IN PCI_RESOURCE_NODE    *PMem64Node
+  );
+
+/**
+  Program PCI Card device register for given resource node.
+
+  @param Base    Base address of PCI Card device to be programmed.
+  @param Node    Given resource node.
+
+**/
+VOID
+ProgramP2C (
+  IN UINT64            Base,
+  IN PCI_RESOURCE_NODE *Node
+  );
+
+/**
+  Create padding resource node.
+
+  @param PciDev     Pci device instance.
+  @param IoNode     Resource info node for IO.
+  @param Mem32Node  Resource info node for 32-bit memory.
+  @param PMem32Node Resource info node for 32-bit Prefetchable Memory.
+  @param Mem64Node  Resource info node for 64-bit memory.
+  @param PMem64Node Resource info node for 64-bit Prefetchable Memory.
+
+**/
+VOID
+ApplyResourcePadding (
+  IN PCI_IO_DEVICE         *PciDev,
+  IN PCI_RESOURCE_NODE     *IoNode,
+  IN PCI_RESOURCE_NODE     *Mem32Node,
+  IN PCI_RESOURCE_NODE     *PMem32Node,
+  IN PCI_RESOURCE_NODE     *Mem64Node,
+  IN PCI_RESOURCE_NODE     *PMem64Node
+  );
+
+/**
+  Get padding resource for PCI-PCI bridge.
+
+  @param  PciIoDevice     PCI-PCI bridge device instance.
+
+  @note   Feature flag PcdPciBusHotplugDeviceSupport determines
+          whether need to pad resource for them.
+**/
+VOID
+GetResourcePaddingPpb (
+  IN  PCI_IO_DEVICE                  *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c
new file mode 100644
index 0000000000..f48c3a0c59
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.c
@@ -0,0 +1,126 @@
+/** @file
+  Set up ROM Table for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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 "PciBus.h"
+
+//
+// PCI ROM image information
+//
+typedef struct {
+  EFI_HANDLE  ImageHandle;
+  UINTN       Seg;
+  UINT8       Bus;
+  UINT8       Dev;
+  UINT8       Func;
+  UINT64      RomAddress;
+  UINT64      RomLength;
+} EFI_PCI_ROM_IMAGE_MAPPING;
+
+UINTN                      mNumberOfPciRomImages     = 0;
+UINTN                      mMaxNumberOfPciRomImages  = 0;
+EFI_PCI_ROM_IMAGE_MAPPING  *mRomImageTable           = NULL;
+
+/**
+  Add the Rom Image to internal database for later PCI light enumeration.
+
+  @param ImageHandle    Option Rom image handle.
+  @param Seg            Segment of PCI space.
+  @param Bus            Bus NO of PCI space.
+  @param Dev            Dev NO of PCI space.
+  @param Func           Func NO of PCI space.
+  @param RomAddress     Base address of OptionRom.
+  @param RomLength      Length of rom image.
+
+**/
+VOID
+PciRomAddImageMapping (
+  IN  EFI_HANDLE  ImageHandle,
+  IN  UINTN       Seg,
+  IN  UINT8       Bus,
+  IN  UINT8       Dev,
+  IN  UINT8       Func,
+  IN  UINT64      RomAddress,
+  IN  UINT64      RomLength
+  )
+{
+  EFI_PCI_ROM_IMAGE_MAPPING *TempMapping;
+
+  if (mNumberOfPciRomImages >= mMaxNumberOfPciRomImages) {
+
+    mMaxNumberOfPciRomImages += 0x20;
+
+    TempMapping = NULL;
+    TempMapping = AllocatePool (mMaxNumberOfPciRomImages * sizeof (EFI_PCI_ROM_IMAGE_MAPPING));
+    if (TempMapping == NULL) {
+      return ;
+    }
+
+    CopyMem (TempMapping, mRomImageTable, mNumberOfPciRomImages * sizeof (EFI_PCI_ROM_IMAGE_MAPPING));
+
+    if (mRomImageTable != NULL) {
+      FreePool (mRomImageTable);
+    }
+
+    mRomImageTable = TempMapping;
+  }
+
+  mRomImageTable[mNumberOfPciRomImages].ImageHandle = ImageHandle;
+  mRomImageTable[mNumberOfPciRomImages].Seg         = Seg;
+  mRomImageTable[mNumberOfPciRomImages].Bus         = Bus;
+  mRomImageTable[mNumberOfPciRomImages].Dev         = Dev;
+  mRomImageTable[mNumberOfPciRomImages].Func        = Func;
+  mRomImageTable[mNumberOfPciRomImages].RomAddress  = RomAddress;
+  mRomImageTable[mNumberOfPciRomImages].RomLength   = RomLength;
+  mNumberOfPciRomImages++;
+}
+
+/**
+  Get Option rom driver's mapping for PCI device.
+
+  @param PciIoDevice Device instance.
+
+  @retval TRUE   Found Image mapping.
+  @retval FALSE  Cannot found image mapping.
+
+**/
+BOOLEAN
+PciRomGetImageMapping (
+  IN  PCI_IO_DEVICE                       *PciIoDevice
+  )
+{
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL *PciRootBridgeIo;
+  UINTN                           Index;
+  BOOLEAN                         Found;
+
+  PciRootBridgeIo = PciIoDevice->PciRootBridgeIo;
+  Found           = FALSE;
+
+  for (Index = 0; Index < mNumberOfPciRomImages; Index++) {
+    if (mRomImageTable[Index].Seg  == PciRootBridgeIo->SegmentNumber &&
+        mRomImageTable[Index].Bus  == PciIoDevice->BusNumber         &&
+        mRomImageTable[Index].Dev  == PciIoDevice->DeviceNumber      &&
+        mRomImageTable[Index].Func == PciIoDevice->FunctionNumber    ) {
+        Found = TRUE;
+
+      if (mRomImageTable[Index].ImageHandle != NULL) {
+        AddDriver (PciIoDevice, mRomImageTable[Index].ImageHandle);
+      } else {
+        PciIoDevice->PciIo.RomImage = (VOID *) (UINTN) mRomImageTable[Index].RomAddress;
+        PciIoDevice->PciIo.RomSize  = (UINTN) mRomImageTable[Index].RomLength;
+      }
+    }
+  }
+
+  return Found;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h
new file mode 100644
index 0000000000..d443f83336
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciBusDxe/PciRomTable.h
@@ -0,0 +1,55 @@
+/** @file
+  Set up ROM Table for PCI Bus module.
+
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef _EFI_PCI_ROM_TABLE_H_
+#define _EFI_PCI_ROM_TABLE_H_
+
+/**
+  Add the Rom Image to internal database for later PCI light enumeration.
+
+  @param ImageHandle    Option Rom image handle.
+  @param Seg            Segment of PCI space.
+  @param Bus            Bus NO of PCI space.
+  @param Dev            Dev NO of PCI space.
+  @param Func           Func NO of PCI space.
+  @param RomAddress     Base address of OptionRom.
+  @param RomLength      Length of rom image.
+
+**/
+VOID
+PciRomAddImageMapping (
+  IN  EFI_HANDLE  ImageHandle,
+  IN  UINTN       Seg,
+  IN  UINT8       Bus,
+  IN  UINT8       Dev,
+  IN  UINT8       Func,
+  IN  UINT64      RomAddress,
+  IN  UINT64      RomLength
+  );
+
+/**
+  Get Option rom driver's mapping for PCI device.
+
+  @param PciIoDevice Device instance.
+
+  @retval TRUE   Found Image mapping.
+  @retval FALSE  Cannot found image mapping.
+
+**/
+BOOLEAN
+PciRomGetImageMapping (
+  IN  PCI_IO_DEVICE                       *PciIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c
new file mode 100644
index 0000000000..c866e88016
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.c
@@ -0,0 +1,1471 @@
+/** @file
+
+  Provides the basic interfaces to abstract a PCI Host Bridge Resource Allocation.
+
+Copyright (c) 1999 - 2016, 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 "PciHostBridge.h"
+#include "PciRootBridge.h"
+#include "PciHostResource.h"
+
+
+EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+EFI_CPU_IO2_PROTOCOL        *mCpuIo;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 *mAcpiAddressSpaceTypeStr[] = {
+  L"Mem", L"I/O", L"Bus"
+};
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 *mPciResourceTypeStr[] = {
+  L"I/O", L"Mem", L"PMem", L"Mem64", L"PMem64", L"Bus"
+};
+
+/**
+  Ensure the compatibility of an IO space descriptor with the IO aperture.
+
+  The IO space descriptor can come from the GCD IO space map, or it can
+  represent a gap between two neighboring IO space descriptors. In the latter
+  case, the GcdIoType field is expected to be EfiGcdIoTypeNonExistent.
+
+  If the IO space descriptor already has type EfiGcdIoTypeIo, then no action is
+  taken -- it is by definition compatible with the aperture.
+
+  Otherwise, the intersection of the IO space descriptor is calculated with the
+  aperture. If the intersection is the empty set (no overlap), no action is
+  taken; the IO space descriptor is compatible with the aperture.
+
+  Otherwise, the type of the descriptor is investigated again. If the type is
+  EfiGcdIoTypeNonExistent (representing a gap, or a genuine descriptor with
+  such a type), then an attempt is made to add the intersection as IO space to
+  the GCD IO space map. This ensures continuity for the aperture, and the
+  descriptor is deemed compatible with the aperture.
+
+  Otherwise, the IO space descriptor is incompatible with the IO aperture.
+
+  @param[in] Base        Base address of the aperture.
+  @param[in] Length      Length of the aperture.
+  @param[in] Descriptor  The descriptor to ensure compatibility with the
+                         aperture for.
+
+  @retval EFI_SUCCESS            The descriptor is compatible. The GCD IO space
+                                 map may have been updated, for continuity
+                                 within the aperture.
+  @retval EFI_INVALID_PARAMETER  The descriptor is incompatible.
+  @return                        Error codes from gDS->AddIoSpace().
+**/
+EFI_STATUS
+IntersectIoDescriptor (
+  IN  UINT64                            Base,
+  IN  UINT64                            Length,
+  IN  CONST EFI_GCD_IO_SPACE_DESCRIPTOR *Descriptor
+  )
+{
+  UINT64                                IntersectionBase;
+  UINT64                                IntersectionEnd;
+  EFI_STATUS                            Status;
+
+  if (Descriptor->GcdIoType == EfiGcdIoTypeIo) {
+    return EFI_SUCCESS;
+  }
+
+  IntersectionBase = MAX (Base, Descriptor->BaseAddress);
+  IntersectionEnd = MIN (Base + Length,
+                      Descriptor->BaseAddress + Descriptor->Length);
+  if (IntersectionBase >= IntersectionEnd) {
+    //
+    // The descriptor and the aperture don't overlap.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if (Descriptor->GcdIoType == EfiGcdIoTypeNonExistent) {
+    Status = gDS->AddIoSpace (EfiGcdIoTypeIo, IntersectionBase,
+                    IntersectionEnd - IntersectionBase);
+
+    DEBUG ((EFI_ERROR (Status) ? EFI_D_ERROR : EFI_D_VERBOSE,
+      "%a: %a: add [%Lx, %Lx): %r\n", gEfiCallerBaseName, __FUNCTION__,
+      IntersectionBase, IntersectionEnd, Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_ERROR, "%a: %a: desc [%Lx, %Lx) type %u conflicts with "
+    "aperture [%Lx, %Lx)\n", gEfiCallerBaseName, __FUNCTION__,
+    Descriptor->BaseAddress, Descriptor->BaseAddress + Descriptor->Length,
+    (UINT32)Descriptor->GcdIoType, Base, Base + Length));
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add IO space to GCD.
+  The routine checks the GCD database and only adds those which are
+  not added in the specified range to GCD.
+
+  @param Base   Base address of the IO space.
+  @param Length Length of the IO space.
+
+  @retval EFI_SUCCES The IO space was added successfully.
+**/
+EFI_STATUS
+AddIoSpace (
+  IN  UINT64                        Base,
+  IN  UINT64                        Length
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+  UINTN                             NumberOfDescriptors;
+  EFI_GCD_IO_SPACE_DESCRIPTOR       *IoSpaceMap;
+
+  Status = gDS->GetIoSpaceMap (&NumberOfDescriptors, &IoSpaceMap);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a: %a: GetIoSpaceMap(): %r\n",
+      gEfiCallerBaseName, __FUNCTION__, Status));
+    return Status;
+  }
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    Status = IntersectIoDescriptor (Base, Length, &IoSpaceMap[Index]);
+    if (EFI_ERROR (Status)) {
+      goto FreeIoSpaceMap;
+    }
+  }
+
+  DEBUG_CODE (
+    //
+    // Make sure there are adjacent descriptors covering [Base, Base + Length).
+    // It is possible that they have not been merged; merging can be prevented
+    // by allocation.
+    //
+    UINT64                      CheckBase;
+    EFI_STATUS                  CheckStatus;
+    EFI_GCD_IO_SPACE_DESCRIPTOR Descriptor;
+
+    for (CheckBase = Base;
+         CheckBase < Base + Length;
+         CheckBase = Descriptor.BaseAddress + Descriptor.Length) {
+      CheckStatus = gDS->GetIoSpaceDescriptor (CheckBase, &Descriptor);
+      ASSERT_EFI_ERROR (CheckStatus);
+      ASSERT (Descriptor.GcdIoType == EfiGcdIoTypeIo);
+    }
+    );
+
+FreeIoSpaceMap:
+  FreePool (IoSpaceMap);
+
+  return Status;
+}
+
+/**
+  Ensure the compatibility of a memory space descriptor with the MMIO aperture.
+
+  The memory space descriptor can come from the GCD memory space map, or it can
+  represent a gap between two neighboring memory space descriptors. In the
+  latter case, the GcdMemoryType field is expected to be
+  EfiGcdMemoryTypeNonExistent.
+
+  If the memory space descriptor already has type
+  EfiGcdMemoryTypeMemoryMappedIo, and its capabilities are a superset of the
+  required capabilities, then no action is taken -- it is by definition
+  compatible with the aperture.
+
+  Otherwise, the intersection of the memory space descriptor is calculated with
+  the aperture. If the intersection is the empty set (no overlap), no action is
+  taken; the memory space descriptor is compatible with the aperture.
+
+  Otherwise, the type of the descriptor is investigated again. If the type is
+  EfiGcdMemoryTypeNonExistent (representing a gap, or a genuine descriptor with
+  such a type), then an attempt is made to add the intersection as MMIO space
+  to the GCD memory space map, with the specified capabilities. This ensures
+  continuity for the aperture, and the descriptor is deemed compatible with the
+  aperture.
+
+  Otherwise, the memory space descriptor is incompatible with the MMIO
+  aperture.
+
+  @param[in] Base         Base address of the aperture.
+  @param[in] Length       Length of the aperture.
+  @param[in] Capabilities Capabilities required by the aperture.
+  @param[in] Descriptor   The descriptor to ensure compatibility with the
+                          aperture for.
+
+  @retval EFI_SUCCESS            The descriptor is compatible. The GCD memory
+                                 space map may have been updated, for
+                                 continuity within the aperture.
+  @retval EFI_INVALID_PARAMETER  The descriptor is incompatible.
+  @return                        Error codes from gDS->AddMemorySpace().
+**/
+EFI_STATUS
+IntersectMemoryDescriptor (
+  IN  UINT64                                Base,
+  IN  UINT64                                Length,
+  IN  UINT64                                Capabilities,
+  IN  CONST EFI_GCD_MEMORY_SPACE_DESCRIPTOR *Descriptor
+  )
+{
+  UINT64                                    IntersectionBase;
+  UINT64                                    IntersectionEnd;
+  EFI_STATUS                                Status;
+
+  if (Descriptor->GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo &&
+      (Descriptor->Capabilities & Capabilities) == Capabilities) {
+    return EFI_SUCCESS;
+  }
+
+  IntersectionBase = MAX (Base, Descriptor->BaseAddress);
+  IntersectionEnd = MIN (Base + Length,
+                      Descriptor->BaseAddress + Descriptor->Length);
+  if (IntersectionBase >= IntersectionEnd) {
+    //
+    // The descriptor and the aperture don't overlap.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if (Descriptor->GcdMemoryType == EfiGcdMemoryTypeNonExistent) {
+    Status = gDS->AddMemorySpace (EfiGcdMemoryTypeMemoryMappedIo,
+                    IntersectionBase, IntersectionEnd - IntersectionBase,
+                    Capabilities);
+
+    DEBUG ((EFI_ERROR (Status) ? EFI_D_ERROR : EFI_D_VERBOSE,
+      "%a: %a: add [%Lx, %Lx): %r\n", gEfiCallerBaseName, __FUNCTION__,
+      IntersectionBase, IntersectionEnd, Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_ERROR, "%a: %a: desc [%Lx, %Lx) type %u cap %Lx conflicts "
+    "with aperture [%Lx, %Lx) cap %Lx\n", gEfiCallerBaseName, __FUNCTION__,
+    Descriptor->BaseAddress, Descriptor->BaseAddress + Descriptor->Length,
+    (UINT32)Descriptor->GcdMemoryType, Descriptor->Capabilities,
+    Base, Base + Length, Capabilities));
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Add MMIO space to GCD.
+  The routine checks the GCD database and only adds those which are
+  not added in the specified range to GCD.
+
+  @param Base         Base address of the MMIO space.
+  @param Length       Length of the MMIO space.
+  @param Capabilities Capabilities of the MMIO space.
+
+  @retval EFI_SUCCES The MMIO space was added successfully.
+**/
+EFI_STATUS
+AddMemoryMappedIoSpace (
+  IN  UINT64                            Base,
+  IN  UINT64                            Length,
+  IN  UINT64                            Capabilities
+  )
+{
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  UINTN                                 NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR       *MemorySpaceMap;
+
+  Status = gDS->GetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a: %a: GetMemorySpaceMap(): %r\n",
+      gEfiCallerBaseName, __FUNCTION__, Status));
+    return Status;
+  }
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    Status = IntersectMemoryDescriptor (Base, Length, Capabilities,
+               &MemorySpaceMap[Index]);
+    if (EFI_ERROR (Status)) {
+      goto FreeMemorySpaceMap;
+    }
+  }
+
+  DEBUG_CODE (
+    //
+    // Make sure there are adjacent descriptors covering [Base, Base + Length).
+    // It is possible that they have not been merged; merging can be prevented
+    // by allocation and different capabilities.
+    //
+    UINT64                          CheckBase;
+    EFI_STATUS                      CheckStatus;
+    EFI_GCD_MEMORY_SPACE_DESCRIPTOR Descriptor;
+
+    for (CheckBase = Base;
+         CheckBase < Base + Length;
+         CheckBase = Descriptor.BaseAddress + Descriptor.Length) {
+      CheckStatus = gDS->GetMemorySpaceDescriptor (CheckBase, &Descriptor);
+      ASSERT_EFI_ERROR (CheckStatus);
+      ASSERT (Descriptor.GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo);
+      ASSERT ((Descriptor.Capabilities & Capabilities) == Capabilities);
+    }
+    );
+
+FreeMemorySpaceMap:
+  FreePool (MemorySpaceMap);
+
+  return Status;
+}
+
+/**
+
+  Entry point of this driver.
+
+  @param ImageHandle  Image handle of this driver.
+  @param SystemTable  Pointer to standard EFI system table.
+
+  @retval EFI_SUCCESS       Succeed.
+  @retval EFI_DEVICE_ERROR  Fail to install PCI_ROOT_BRIDGE_IO protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciHostBridge (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  PCI_HOST_BRIDGE_INSTANCE    *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE    *RootBridge;
+  PCI_ROOT_BRIDGE             *RootBridges;
+  UINTN                       RootBridgeCount;
+  UINTN                       Index;
+  PCI_ROOT_BRIDGE_APERTURE    *MemApertures[4];
+  UINTN                       MemApertureIndex;
+  BOOLEAN                     ResourceAssigned;
+  LIST_ENTRY                  *Link;
+
+  RootBridges = PciHostBridgeGetRootBridges (&RootBridgeCount);
+  if ((RootBridges == NULL) || (RootBridgeCount == 0)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiMetronomeArchProtocolGuid, NULL, (VOID **) &mMetronome);
+  ASSERT_EFI_ERROR (Status);
+  Status = gBS->LocateProtocol (&gEfiCpuIo2ProtocolGuid, NULL, (VOID **) &mCpuIo);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Most systems in the world including complex servers have only one Host Bridge.
+  //
+  HostBridge = AllocateZeroPool (sizeof (PCI_HOST_BRIDGE_INSTANCE));
+  ASSERT (HostBridge != NULL);
+
+  HostBridge->Signature        = PCI_HOST_BRIDGE_SIGNATURE;
+  HostBridge->CanRestarted     = TRUE;
+  InitializeListHead (&HostBridge->RootBridges);
+  ResourceAssigned             = FALSE;
+
+  //
+  // Create Root Bridge Device Handle in this Host Bridge
+  //
+  for (Index = 0; Index < RootBridgeCount; Index++) {
+    //
+    // Create Root Bridge Handle Instance
+    //
+    RootBridge = CreateRootBridge (&RootBridges[Index]);
+    ASSERT (RootBridge != NULL);
+    if (RootBridge == NULL) {
+      continue;
+    }
+
+    //
+    // Make sure all root bridges share the same ResourceAssigned value.
+    //
+    if (Index == 0) {
+      ResourceAssigned = RootBridges[Index].ResourceAssigned;
+    } else {
+      ASSERT (ResourceAssigned == RootBridges[Index].ResourceAssigned);
+    }
+
+    if (RootBridges[Index].Io.Base <= RootBridges[Index].Io.Limit) {
+      Status = AddIoSpace (
+                 RootBridges[Index].Io.Base,
+                 RootBridges[Index].Io.Limit - RootBridges[Index].Io.Base + 1
+                 );
+      ASSERT_EFI_ERROR (Status);
+      if (ResourceAssigned) {
+        Status = gDS->AllocateIoSpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdIoTypeIo,
+                        0,
+                        RootBridges[Index].Io.Limit - RootBridges[Index].Io.Base + 1,
+                        &RootBridges[Index].Io.Base,
+                        gImageHandle,
+                        NULL
+                        );
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+
+    //
+    // Add all the Mem/PMem aperture to GCD
+    // Mem/PMem shouldn't overlap with each other
+    // Root bridge which needs to combine MEM and PMEM should only report
+    // the MEM aperture in Mem
+    //
+    MemApertures[0] = &RootBridges[Index].Mem;
+    MemApertures[1] = &RootBridges[Index].MemAbove4G;
+    MemApertures[2] = &RootBridges[Index].PMem;
+    MemApertures[3] = &RootBridges[Index].PMemAbove4G;
+
+    for (MemApertureIndex = 0; MemApertureIndex < sizeof (MemApertures) / sizeof (MemApertures[0]); MemApertureIndex++) {
+      if (MemApertures[MemApertureIndex]->Base <= MemApertures[MemApertureIndex]->Limit) {
+        Status = AddMemoryMappedIoSpace (
+                   MemApertures[MemApertureIndex]->Base,
+                   MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                   EFI_MEMORY_UC
+                   );
+        ASSERT_EFI_ERROR (Status);
+        Status = gDS->SetMemorySpaceAttributes (
+                        MemApertures[MemApertureIndex]->Base,
+                        MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                        EFI_MEMORY_UC
+                        );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((DEBUG_WARN, "PciHostBridge driver failed to set EFI_MEMORY_UC to MMIO aperture - %r.\n", Status));
+        }
+        if (ResourceAssigned) {
+          Status = gDS->AllocateMemorySpace (
+                          EfiGcdAllocateAddress,
+                          EfiGcdMemoryTypeMemoryMappedIo,
+                          0,
+                          MemApertures[MemApertureIndex]->Limit - MemApertures[MemApertureIndex]->Base + 1,
+                          &MemApertures[MemApertureIndex]->Base,
+                          gImageHandle,
+                          NULL
+                          );
+          ASSERT_EFI_ERROR (Status);
+        }
+      }
+    }
+    //
+    // Insert Root Bridge Handle Instance
+    //
+    InsertTailList (&HostBridge->RootBridges, &RootBridge->Link);
+  }
+
+  //
+  // When resources were assigned, it's not needed to expose
+  // PciHostBridgeResourceAllocation protocol.
+  //
+  if (!ResourceAssigned) {
+    HostBridge->ResAlloc.NotifyPhase = NotifyPhase;
+    HostBridge->ResAlloc.GetNextRootBridge = GetNextRootBridge;
+    HostBridge->ResAlloc.GetAllocAttributes = GetAttributes;
+    HostBridge->ResAlloc.StartBusEnumeration = StartBusEnumeration;
+    HostBridge->ResAlloc.SetBusNumbers = SetBusNumbers;
+    HostBridge->ResAlloc.SubmitResources = SubmitResources;
+    HostBridge->ResAlloc.GetProposedResources = GetProposedResources;
+    HostBridge->ResAlloc.PreprocessController = PreprocessController;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &HostBridge->Handle,
+                    &gEfiPciHostBridgeResourceAllocationProtocolGuid, &HostBridge->ResAlloc,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    RootBridge->RootBridgeIo.ParentHandle = HostBridge->Handle;
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &RootBridge->Handle,
+                    &gEfiDevicePathProtocolGuid, RootBridge->DevicePath,
+                    &gEfiPciRootBridgeIoProtocolGuid, &RootBridge->RootBridgeIo,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+  PciHostBridgeFreeRootBridges (RootBridges, RootBridgeCount);
+  return Status;
+}
+
+/**
+  This routine constructs the resource descriptors for all root bridges and call PciHostBridgeResourceConflict().
+
+  @param HostBridge The Host Bridge Instance where the resource adjustment happens.
+**/
+VOID
+ResourceConflict (
+  IN  PCI_HOST_BRIDGE_INSTANCE *HostBridge
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Resources;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  LIST_ENTRY                        *Link;
+  UINTN                             RootBridgeCount;
+  PCI_RESOURCE_TYPE                 Index;
+  PCI_RES_NODE                      *ResAllocNode;
+
+  RootBridgeCount = 0;
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridgeCount++;
+  }
+
+  Resources = AllocatePool (
+                RootBridgeCount * (TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)) +
+                sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+                );
+  ASSERT (Resources != NULL);
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges), Descriptor = Resources
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    for (Index = TypeIo; Index < TypeMax; Index++) {
+      ResAllocNode = &RootBridge->ResAllocNode[Index];
+
+      Descriptor->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Descriptor->Len = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+      Descriptor->AddrRangeMin = ResAllocNode->Base;
+      Descriptor->AddrRangeMax = ResAllocNode->Alignment;
+      Descriptor->AddrLen = ResAllocNode->Length;
+      switch (ResAllocNode->Type) {
+
+      case TypeIo:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_IO;
+        break;
+
+      case TypePMem32:
+        Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+      case TypeMem32:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+        Descriptor->AddrSpaceGranularity = 32;
+        break;
+
+      case TypePMem64:
+        Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+      case TypeMem64:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_MEM;
+        Descriptor->AddrSpaceGranularity = 64;
+        break;
+
+      case TypeBus:
+        Descriptor->ResType = ACPI_ADDRESS_SPACE_TYPE_BUS;
+        break;
+
+      default:
+        break;
+      }
+
+      Descriptor++;
+    }
+    //
+    // Terminate the root bridge resources.
+    //
+    End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+    End->Desc = ACPI_END_TAG_DESCRIPTOR;
+    End->Checksum = 0x0;
+
+    Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) (End + 1);
+  }
+  //
+  // Terminate the host bridge resources.
+  //
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+  End->Desc = ACPI_END_TAG_DESCRIPTOR;
+  End->Checksum = 0x0;
+
+  DEBUG ((DEBUG_ERROR, "Call PciHostBridgeResourceConflict().\n"));
+  PciHostBridgeResourceConflict (HostBridge->Handle, Resources);
+  FreePool (Resources);
+}
+
+/**
+  Allocate Length of MMIO or IO resource with alignment BitsOfAlignment
+  from GCD range [BaseAddress, Limit).
+
+  @param Mmio            TRUE for MMIO and FALSE for IO.
+  @param Length          Length of the resource to allocate.
+  @param BitsOfAlignment Alignment of the resource to allocate.
+  @param BaseAddress     The starting address the allocation is from.
+  @param Limit           The ending address the allocation is to.
+
+  @retval  The base address of the allocated resource or MAX_UINT64 if allocation
+           fails.
+**/
+UINT64
+AllocateResource (
+  BOOLEAN Mmio,
+  UINT64  Length,
+  UINTN   BitsOfAlignment,
+  UINT64  BaseAddress,
+  UINT64  Limit
+  )
+{
+  EFI_STATUS Status;
+
+  if (BaseAddress < Limit) {
+    //
+    // Have to make sure Aligment is handled since we are doing direct address allocation
+    //
+    BaseAddress = ALIGN_VALUE (BaseAddress, LShiftU64 (1, BitsOfAlignment));
+
+    while (BaseAddress + Length <= Limit + 1) {
+      if (Mmio) {
+        Status = gDS->AllocateMemorySpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdMemoryTypeMemoryMappedIo,
+                        BitsOfAlignment,
+                        Length,
+                        &BaseAddress,
+                        gImageHandle,
+                        NULL
+                        );
+      } else {
+        Status = gDS->AllocateIoSpace (
+                        EfiGcdAllocateAddress,
+                        EfiGcdIoTypeIo,
+                        BitsOfAlignment,
+                        Length,
+                        &BaseAddress,
+                        gImageHandle,
+                        NULL
+                        );
+      }
+
+      if (!EFI_ERROR (Status)) {
+        return BaseAddress;
+      }
+      BaseAddress += LShiftU64 (1, BitsOfAlignment);
+    }
+  }
+  return MAX_UINT64;
+}
+
+/**
+
+  Enter a certain phase of the PCI enumeration process.
+
+  @param This   The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Phase  The phase during enumeration.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong phase parameter passed in.
+  @retval EFI_NOT_READY          Resources have not been submitted yet.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE    Phase
+  )
+{
+  PCI_HOST_BRIDGE_INSTANCE              *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE              *RootBridge;
+  LIST_ENTRY                            *Link;
+  EFI_PHYSICAL_ADDRESS                  BaseAddress;
+  UINTN                                 BitsOfAlignment;
+  UINT64                                Alignment;
+  EFI_STATUS                            Status;
+  EFI_STATUS                            ReturnStatus;
+  PCI_RESOURCE_TYPE                     Index;
+  PCI_RESOURCE_TYPE                     Index1;
+  PCI_RESOURCE_TYPE                     Index2;
+  BOOLEAN                               ResNodeHandled[TypeMax];
+  UINT64                                MaxAlignment;
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+
+  switch (Phase) {
+  case EfiPciHostBridgeBeginEnumeration:
+    if (!HostBridge->CanRestarted) {
+      return EFI_NOT_READY;
+    }
+    //
+    // Reset Root Bridge
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+          ; !IsNull (&HostBridge->RootBridges, Link)
+          ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+          ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeMax; Index++) {
+        RootBridge->ResAllocNode[Index].Type   = Index;
+        RootBridge->ResAllocNode[Index].Base   = 0;
+        RootBridge->ResAllocNode[Index].Length = 0;
+        RootBridge->ResAllocNode[Index].Status = ResNone;
+
+        RootBridge->ResourceSubmitted = FALSE;
+      }
+    }
+
+    HostBridge->CanRestarted = TRUE;
+    break;
+
+  case EfiPciHostBridgeBeginBusAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    HostBridge->CanRestarted = FALSE;
+    break;
+
+  case EfiPciHostBridgeEndBusAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    break;
+
+  case EfiPciHostBridgeBeginResourceAllocation:
+    //
+    // No specific action is required here, can perform any chipset specific programing
+    //
+    break;
+
+  case EfiPciHostBridgeAllocateResources:
+    ReturnStatus = EFI_SUCCESS;
+
+    //
+    // Make sure the resource for all root bridges has been submitted.
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      if (!RootBridge->ResourceSubmitted) {
+        return EFI_NOT_READY;
+      }
+    }
+
+    DEBUG ((EFI_D_INFO, "PciHostBridge: NotifyPhase (AllocateResources)\n"));
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        ResNodeHandled[Index] = FALSE;
+      }
+
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      DEBUG ((EFI_D_INFO, " RootBridge: %s\n", RootBridge->DevicePathStr));
+
+      for (Index1 = TypeIo; Index1 < TypeBus; Index1++) {
+        if (RootBridge->ResAllocNode[Index1].Status == ResNone) {
+          ResNodeHandled[Index1] = TRUE;
+        } else {
+          //
+          // Allocate the resource node with max alignment at first
+          //
+          MaxAlignment = 0;
+          Index = TypeMax;
+          for (Index2 = TypeIo; Index2 < TypeBus; Index2++) {
+            if (ResNodeHandled[Index2]) {
+              continue;
+            }
+            if (MaxAlignment <= RootBridge->ResAllocNode[Index2].Alignment) {
+              MaxAlignment = RootBridge->ResAllocNode[Index2].Alignment;
+              Index = Index2;
+            }
+          }
+
+          ASSERT (Index < TypeMax);
+          ResNodeHandled[Index] = TRUE;
+          Alignment = RootBridge->ResAllocNode[Index].Alignment;
+          BitsOfAlignment = LowBitSet64 (Alignment + 1);
+          BaseAddress = MAX_UINT64;
+
+          switch (Index) {
+          case TypeIo:
+            BaseAddress = AllocateResource (
+                            FALSE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (15, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->Io.Base, Alignment + 1),
+                            RootBridge->Io.Limit
+                            );
+            break;
+
+          case TypeMem64:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (63, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->MemAbove4G.Base, Alignment + 1),
+                            RootBridge->MemAbove4G.Limit
+                            );
+            if (BaseAddress != MAX_UINT64) {
+              break;
+            }
+            //
+            // If memory above 4GB is not available, try memory below 4GB
+            //
+
+          case TypeMem32:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (31, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->Mem.Base, Alignment + 1),
+                            RootBridge->Mem.Limit
+                            );
+            break;
+
+          case TypePMem64:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (63, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->PMemAbove4G.Base, Alignment + 1),
+                            RootBridge->PMemAbove4G.Limit
+                            );
+            if (BaseAddress != MAX_UINT64) {
+              break;
+            }
+            //
+            // If memory above 4GB is not available, try memory below 4GB
+            //
+          case TypePMem32:
+            BaseAddress = AllocateResource (
+                            TRUE,
+                            RootBridge->ResAllocNode[Index].Length,
+                            MIN (31, BitsOfAlignment),
+                            ALIGN_VALUE (RootBridge->PMem.Base, Alignment + 1),
+                            RootBridge->PMem.Limit
+                            );
+            break;
+
+          default:
+            ASSERT (FALSE);
+            break;
+          }
+
+          DEBUG ((DEBUG_INFO, "  %s: Base/Length/Alignment = %lx/%lx/%lx - ",
+                  mPciResourceTypeStr[Index], BaseAddress, RootBridge->ResAllocNode[Index].Length, Alignment));
+          if (BaseAddress != MAX_UINT64) {
+            RootBridge->ResAllocNode[Index].Base = BaseAddress;
+            RootBridge->ResAllocNode[Index].Status = ResAllocated;
+            DEBUG ((DEBUG_INFO, "Success\n"));
+          } else {
+            ReturnStatus = EFI_OUT_OF_RESOURCES;
+            DEBUG ((DEBUG_ERROR, "Out Of Resource!\n"));
+          }
+        }
+      }
+    }
+
+    if (ReturnStatus == EFI_OUT_OF_RESOURCES) {
+      ResourceConflict (HostBridge);
+    }
+
+    //
+    // Set resource to zero for nodes where allocation fails
+    //
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+          ; !IsNull (&HostBridge->RootBridges, Link)
+          ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+          ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status != ResAllocated) {
+          RootBridge->ResAllocNode[Index].Length = 0;
+        }
+      }
+    }
+    return ReturnStatus;
+
+  case EfiPciHostBridgeSetResources:
+    //
+    // HostBridgeInstance->CanRestarted = FALSE;
+    //
+    break;
+
+  case EfiPciHostBridgeFreeResources:
+    //
+    // HostBridgeInstance->CanRestarted = FALSE;
+    //
+    ReturnStatus = EFI_SUCCESS;
+    for (Link = GetFirstNode (&HostBridge->RootBridges)
+         ; !IsNull (&HostBridge->RootBridges, Link)
+         ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+         ) {
+      RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+      for (Index = TypeIo; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status == ResAllocated) {
+          switch (Index) {
+          case TypeIo:
+            Status = gDS->FreeIoSpace (RootBridge->ResAllocNode[Index].Base, RootBridge->ResAllocNode[Index].Length);
+            if (EFI_ERROR (Status)) {
+              ReturnStatus = Status;
+            }
+            break;
+
+          case TypeMem32:
+          case TypePMem32:
+          case TypeMem64:
+          case TypePMem64:
+            Status = gDS->FreeMemorySpace (RootBridge->ResAllocNode[Index].Base, RootBridge->ResAllocNode[Index].Length);
+            if (EFI_ERROR (Status)) {
+              ReturnStatus = Status;
+            }
+            break;
+
+          default:
+            ASSERT (FALSE);
+            break;
+          }
+
+          RootBridge->ResAllocNode[Index].Type = Index;
+          RootBridge->ResAllocNode[Index].Base = 0;
+          RootBridge->ResAllocNode[Index].Length = 0;
+          RootBridge->ResAllocNode[Index].Status = ResNone;
+        }
+      }
+
+      RootBridge->ResourceSubmitted = FALSE;
+    }
+
+    HostBridge->CanRestarted = TRUE;
+    return ReturnStatus;
+
+  case EfiPciHostBridgeEndResourceAllocation:
+    //
+    // The resource allocation phase is completed.  No specific action is required
+    // here. This notification can be used to perform any chipset specific programming.
+    //
+    break;
+
+  case EfiPciHostBridgeEndEnumeration:
+    //
+    // The Host Bridge Enumeration is completed. No specific action is required here.
+    // This notification can be used to perform any chipset specific programming.
+    //
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Return the device handle of the next PCI root bridge that is associated with
+  this Host Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  Returns the device handle of the next PCI Root Bridge.
+                           On input, it holds the RootBridgeHandle returned by the most
+                           recent call to GetNextRootBridge().The handle for the first
+                           PCI Root Bridge is returned if RootBridgeHandle is NULL on input.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_NOT_FOUND          Next PCI root bridge not found.
+  @retval EFI_INVALID_PARAMETER  Wrong parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNextRootBridge (
+  IN     EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN OUT EFI_HANDLE                                       *RootBridgeHandle
+  )
+{
+  BOOLEAN                   ReturnNext;
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if (RootBridgeHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  ReturnNext = (BOOLEAN) (*RootBridgeHandle == NULL);
+
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (ReturnNext) {
+      *RootBridgeHandle = RootBridge->Handle;
+      return EFI_SUCCESS;
+    }
+
+    ReturnNext = (BOOLEAN) (*RootBridgeHandle == RootBridge->Handle);
+  }
+
+  if (ReturnNext) {
+    ASSERT (IsNull (&HostBridge->RootBridges, Link));
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+}
+
+/**
+
+  Returns the attributes of a PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The device handle of the PCI Root Bridge
+                           that the caller is interested in.
+  @param Attributes        The pointer to attributes of the PCI Root Bridge.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Attributes parameter passed in is NULL or
+                                 RootBridgeHandle is not an EFI_HANDLE
+                                 that was returned on a previous call to
+                                 GetNextRootBridge().
+
+**/
+EFI_STATUS
+EFIAPI
+GetAttributes (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT UINT64                                           *Attributes
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if (Attributes == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      *Attributes = RootBridge->AllocationAttributes;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This is the request from the PCI enumerator to set up
+  the specified PCI Root Bridge for bus enumeration process.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge to be set up.
+  @param Configuration     Pointer to the pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_OUT_OF_RESOURCES   Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+StartBusEnumeration (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      *Configuration = AllocatePool (sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (*Configuration == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) *Configuration;
+      Descriptor->Desc                  = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+      Descriptor->Len                   = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_BUS;
+      Descriptor->GenFlag               = 0;
+      Descriptor->SpecificFlag          = 0;
+      Descriptor->AddrSpaceGranularity  = 0;
+      Descriptor->AddrRangeMin          = RootBridge->Bus.Base;
+      Descriptor->AddrRangeMax          = 0;
+      Descriptor->AddrTranslationOffset = 0;
+      Descriptor->AddrLen               = RootBridge->Bus.Limit - RootBridge->Bus.Base + 1;
+
+      End = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Descriptor + 1);
+      End->Desc = ACPI_END_TAG_DESCRIPTOR;
+      End->Checksum = 0x0;
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function programs the PCI Root Bridge hardware so that
+  it decodes the specified PCI bus range.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge whose bus range is to be programmed.
+  @param Configuration     The pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBusNumbers (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) (Descriptor + 1);
+
+  //
+  // Check the Configuration is valid
+  //
+  if ((Descriptor->Desc != ACPI_ADDRESS_SPACE_DESCRIPTOR) ||
+      (Descriptor->ResType != ACPI_ADDRESS_SPACE_TYPE_BUS) ||
+      (End->Desc != ACPI_END_TAG_DESCRIPTOR)
+     ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+
+      if (Descriptor->AddrLen == 0) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      if ((Descriptor->AddrRangeMin < RootBridge->Bus.Base) ||
+          (Descriptor->AddrRangeMin + Descriptor->AddrLen - 1 > RootBridge->Bus.Limit)
+         ) {
+        return EFI_INVALID_PARAMETER;
+      }
+      //
+      // Update the Bus Range
+      //
+      RootBridge->ResAllocNode[TypeBus].Base    = Descriptor->AddrRangeMin;
+      RootBridge->ResAllocNode[TypeBus].Length  = Descriptor->AddrLen;
+      RootBridge->ResAllocNode[TypeBus].Status  = ResAllocated;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  Submits the I/O and memory resource requirements for the specified PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge whose I/O and memory resource requirements.
+                           are being submitted.
+  @param Configuration     The pointer to the PCI I/O and PCI memory resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong parameters passed in.
+**/
+EFI_STATUS
+EFIAPI
+SubmitResources (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  )
+{
+  LIST_ENTRY                        *Link;
+  PCI_HOST_BRIDGE_INSTANCE          *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  PCI_RESOURCE_TYPE                 Type;
+
+  //
+  // Check the input parameter: Configuration
+  //
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      DEBUG ((EFI_D_INFO, "PciHostBridge: SubmitResources for %s\n", RootBridge->DevicePathStr));
+      //
+      // Check the resource descriptors.
+      // If the Configuration includes one or more invalid resource descriptors, all the resource
+      // descriptors are ignored and the function returns EFI_INVALID_PARAMETER.
+      //
+      for (Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration; Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR; Descriptor++) {
+        if (Descriptor->ResType > ACPI_ADDRESS_SPACE_TYPE_BUS) {
+          return EFI_INVALID_PARAMETER;
+        }
+
+        DEBUG ((EFI_D_INFO, " %s: Granularity/SpecificFlag = %ld / %02x%s\n",
+                mAcpiAddressSpaceTypeStr[Descriptor->ResType], Descriptor->AddrSpaceGranularity, Descriptor->SpecificFlag,
+                (Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0 ? L" (Prefetchable)" : L""
+                ));
+        DEBUG ((EFI_D_INFO, "      Length/Alignment = 0x%lx / 0x%lx\n", Descriptor->AddrLen, Descriptor->AddrRangeMax));
+        switch (Descriptor->ResType) {
+        case ACPI_ADDRESS_SPACE_TYPE_MEM:
+          if (Descriptor->AddrSpaceGranularity != 32 && Descriptor->AddrSpaceGranularity != 64) {
+            return EFI_INVALID_PARAMETER;
+          }
+          if (Descriptor->AddrSpaceGranularity == 32 && Descriptor->AddrLen >= SIZE_4GB) {
+            return EFI_INVALID_PARAMETER;
+          }
+          //
+          // If the PCI root bridge does not support separate windows for nonprefetchable and
+          // prefetchable memory, then the PCI bus driver needs to include requests for
+          // prefetchable memory in the nonprefetchable memory pool.
+          //
+          if (((RootBridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) &&
+              ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0)
+             ) {
+            return EFI_INVALID_PARAMETER;
+          }
+        case ACPI_ADDRESS_SPACE_TYPE_IO:
+          //
+          // Check aligment, it should be of the form 2^n-1
+          //
+          if (GetPowerOfTwo64 (Descriptor->AddrRangeMax + 1) != (Descriptor->AddrRangeMax + 1)) {
+            return EFI_INVALID_PARAMETER;
+          }
+          break;
+        default:
+          ASSERT (FALSE);
+          break;
+        }
+      }
+      if (Descriptor->Desc != ACPI_END_TAG_DESCRIPTOR) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      for (Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration; Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR; Descriptor++) {
+        if (Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+          if (Descriptor->AddrSpaceGranularity == 32) {
+            if ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0) {
+              Type = TypePMem32;
+            } else {
+              Type = TypeMem32;
+            }
+          } else {
+            ASSERT (Descriptor->AddrSpaceGranularity == 64);
+            if ((Descriptor->SpecificFlag & EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE) != 0) {
+              Type = TypePMem64;
+            } else {
+              Type = TypeMem64;
+            }
+          }
+        } else {
+          ASSERT (Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_IO);
+          Type = TypeIo;
+        }
+        RootBridge->ResAllocNode[Type].Length    = Descriptor->AddrLen;
+        RootBridge->ResAllocNode[Type].Alignment = Descriptor->AddrRangeMax;
+        RootBridge->ResAllocNode[Type].Status    = ResSubmitted;
+      }
+      RootBridge->ResourceSubmitted = TRUE;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function returns the proposed resource settings for the specified
+  PCI Root Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge handle.
+  @param Configuration     The pointer to the pointer to the PCI I/O
+                           and memory resource descriptor.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_OUT_OF_RESOURCES   Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GetProposedResources (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  )
+{
+  LIST_ENTRY                        *Link;
+  PCI_HOST_BRIDGE_INSTANCE          *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  UINTN                             Index;
+  UINTN                             Number;
+  VOID                              *Buffer;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+  UINT64                            ResStatus;
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+      ; !IsNull (&HostBridge->RootBridges, Link)
+      ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+      ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      for (Index = 0, Number = 0; Index < TypeBus; Index++) {
+        if (RootBridge->ResAllocNode[Index].Status != ResNone) {
+          Number++;
+        }
+      }
+
+      Buffer = AllocateZeroPool (Number * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR));
+      if (Buffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Buffer;
+      for (Index = 0; Index < TypeBus; Index++) {
+        ResStatus = RootBridge->ResAllocNode[Index].Status;
+        if (ResStatus != ResNone) {
+          Descriptor->Desc                  = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+          Descriptor->Len                   = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;;
+          Descriptor->GenFlag               = 0;
+          Descriptor->AddrRangeMin          = RootBridge->ResAllocNode[Index].Base;
+          Descriptor->AddrRangeMax          = 0;
+          Descriptor->AddrTranslationOffset = (ResStatus == ResAllocated) ? EFI_RESOURCE_SATISFIED : PCI_RESOURCE_LESS;
+          Descriptor->AddrLen               = RootBridge->ResAllocNode[Index].Length;
+
+          switch (Index) {
+
+          case TypeIo:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_IO;
+            break;
+
+          case TypePMem32:
+            Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+          case TypeMem32:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_MEM;
+            Descriptor->AddrSpaceGranularity = 32;
+            break;
+
+          case TypePMem64:
+            Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+          case TypeMem64:
+            Descriptor->ResType              = ACPI_ADDRESS_SPACE_TYPE_MEM;
+            Descriptor->AddrSpaceGranularity = 64;
+            break;
+          }
+
+          Descriptor++;
+        }
+      }
+      End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+      End->Desc      = ACPI_END_TAG_DESCRIPTOR;
+      End->Checksum  = 0;
+
+      *Configuration = Buffer;
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+
+  This function is called for all the PCI controllers that the PCI
+  bus driver finds. Can be used to Preprogram the controller.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  The PCI Root Bridge handle.
+  @param PciAddress        Address of the controller on the PCI bus.
+  @param Phase             The Phase during resource allocation.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+PreprocessController (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL          *This,
+  IN  EFI_HANDLE                                                RootBridgeHandle,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS               PciAddress,
+  IN  EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE              Phase
+  )
+{
+  LIST_ENTRY                *Link;
+  PCI_HOST_BRIDGE_INSTANCE  *HostBridge;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+
+  if ((UINT32) Phase > EfiPciBeforeResourceCollection) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  HostBridge = PCI_HOST_BRIDGE_FROM_THIS (This);
+  for (Link = GetFirstNode (&HostBridge->RootBridges)
+       ; !IsNull (&HostBridge->RootBridges, Link)
+       ; Link = GetNextNode (&HostBridge->RootBridges, Link)
+       ) {
+    RootBridge = ROOT_BRIDGE_FROM_LINK (Link);
+    if (RootBridgeHandle == RootBridge->Handle) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h
new file mode 100644
index 0000000000..9a8ca21f48
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridge.h
@@ -0,0 +1,252 @@
+/** @file
+
+  The Header file of the Pci Host Bridge Driver.
+
+Copyright (c) 1999 - 2016, 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.
+
+**/
+
+#ifndef _PCI_HOST_BRIDGE_H_
+#define _PCI_HOST_BRIDGE_H_
+
+
+#include <PiDxe.h>
+#include <IndustryStandard/Acpi.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PciHostBridgeLib.h>
+#include <Protocol/PciHostBridgeResourceAllocation.h>
+
+#include "PciRootBridge.h"
+
+#define PCI_HOST_BRIDGE_SIGNATURE SIGNATURE_32 ('p', 'h', 'b', 'g')
+typedef struct {
+  UINTN                                             Signature;
+  EFI_HANDLE                                        Handle;
+  LIST_ENTRY                                        RootBridges;
+  BOOLEAN                                           CanRestarted;
+  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL  ResAlloc;
+} PCI_HOST_BRIDGE_INSTANCE;
+
+#define PCI_HOST_BRIDGE_FROM_THIS(a) CR (a, PCI_HOST_BRIDGE_INSTANCE, ResAlloc, PCI_HOST_BRIDGE_SIGNATURE)
+
+//
+// Driver Entry Point
+//
+/**
+
+  Entry point of this driver.
+
+  @param ImageHandle  -  Image handle of this driver.
+  @param SystemTable  -  Pointer to standard EFI system table.
+
+  @retval EFI_SUCCESS       -  Succeed.
+  @retval EFI_DEVICE_ERROR  -  Fail to install PCI_ROOT_BRIDGE_IO protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciHostBridge (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+//
+//  HostBridge Resource Allocation interface
+//
+/**
+
+  Enter a certain phase of the PCI enumeration process.
+
+  @param This   The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL instance.
+  @param Phase  The phase during enumeration.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_INVALID_PARAMETER  Wrong phase parameter passed in.
+  @retval EFI_NOT_READY          Resources have not been submitted yet.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyPhase (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL   *This,
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PHASE      Phase
+  );
+
+/**
+
+  Return the device handle of the next PCI root bridge that is associated with
+  this Host Bridge.
+
+  @param This              The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  Returns the device handle of the next PCI Root Bridge.
+                           On input, it holds the RootBridgeHandle returned by the most
+                           recent call to GetNextRootBridge().The handle for the first
+                           PCI Root Bridge is returned if RootBridgeHandle is NULL on input.
+
+  @retval EFI_SUCCESS            Succeed.
+  @retval EFI_NOT_FOUND          Next PCI root bridge not found.
+  @retval EFI_INVALID_PARAMETER  Wrong parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNextRootBridge (
+  IN     EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN OUT EFI_HANDLE                                       *RootBridgeHandle
+  );
+
+/**
+
+  Returns the attributes of a PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance
+  @param RootBridgeHandle  -  The device handle of the PCI Root Bridge
+                              that the caller is interested in
+  @param Attributes        -  The pointer to attributes of the PCI Root Bridge
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Attributes parameter passed in is NULL or
+                            @retval RootBridgeHandle is not an EFI_HANDLE
+                            @retval that was returned on a previous call to
+                            @retval GetNextRootBridge().
+
+**/
+EFI_STATUS
+EFIAPI
+GetAttributes (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT UINT64                                           *Attributes
+  );
+
+/**
+
+  This is the request from the PCI enumerator to set up
+  the specified PCI Root Bridge for bus enumeration process.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge to be set up.
+  @param Configuration     -  Pointer to the pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+StartBusEnumeration (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  );
+
+/**
+
+  This function programs the PCI Root Bridge hardware so that
+  it decodes the specified PCI bus range.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge whose bus range is to be programmed.
+  @param Configuration     -  The pointer to the PCI bus resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBusNumbers (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  );
+
+/**
+
+  Submits the I/O and memory resource requirements for the specified PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance
+  @param RootBridgeHandle  -  The PCI Root Bridge whose I/O and memory resource requirements
+                              are being submitted
+  @param Configuration     -  The pointer to the PCI I/O and PCI memory resource descriptor
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  Wrong parameters passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+SubmitResources (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN EFI_HANDLE                                       RootBridgeHandle,
+  IN VOID                                             *Configuration
+  );
+
+/**
+
+  This function returns the proposed resource settings for the specified
+  PCI Root Bridge.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge handle.
+  @param Configuration     -  The pointer to the pointer to the PCI I/O
+                              and memory resource descriptor.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough pool to be allocated.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GetProposedResources (
+  IN  EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL *This,
+  IN  EFI_HANDLE                                       RootBridgeHandle,
+  OUT VOID                                             **Configuration
+  );
+
+/**
+
+  This function is called for all the PCI controllers that the PCI
+  bus driver finds. Can be used to Preprogram the controller.
+
+  @param This              -  The EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_ PROTOCOL instance.
+  @param RootBridgeHandle  -  The PCI Root Bridge handle.
+  @param PciAddress        -  Address of the controller on the PCI bus.
+  @param Phase             -  The Phase during resource allocation.
+
+  @retval EFI_SUCCESS            -  Succeed.
+  @retval EFI_INVALID_PARAMETER  -  RootBridgeHandle is not a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+PreprocessController (
+  IN EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL          *This,
+  IN EFI_HANDLE                                                RootBridgeHandle,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS               PciAddress,
+  IN EFI_PCI_CONTROLLER_RESOURCE_ALLOCATION_PHASE              Phase
+  );
+
+/**
+  This routine constructs the resource descriptors for all root bridges and call PciHostBridgeResourceConflict().
+
+  @param HostBridge The Host Bridge Instance where the resource adjustment happens.
+**/
+VOID
+ResourceConflict (
+  IN  PCI_HOST_BRIDGE_INSTANCE *HostBridge
+  );
+
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern EFI_CPU_IO2_PROTOCOL        *mCpuIo;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf
new file mode 100644
index 0000000000..d8b0439699
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#   Generic PCI Host Bridge driver.
+#
+#  Copyright (c) 2009 - 2016, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciHostBridgeDxe
+  FILE_GUID                      = 128FB770-5E79-4176-9E51-9BB268A17DD1
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePciHostBridge
+
+[Sources]
+  PciHostBridge.h
+  PciRootBridge.h
+  PciHostBridge.c
+  PciRootBridgeIo.c
+  PciHostResource.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  DxeServicesTableLib
+  DevicePathLib
+  BaseMemoryLib
+  BaseLib
+  PciSegmentLib
+  PciHostBridgeLib
+
+[Protocols]
+  gEfiMetronomeArchProtocolGuid                   ## CONSUMES
+  gEfiCpuIo2ProtocolGuid                          ## CONSUMES
+  gEfiDevicePathProtocolGuid                      ## BY_START
+  gEfiPciRootBridgeIoProtocolGuid                 ## BY_START
+  gEfiPciHostBridgeResourceAllocationProtocolGuid ## BY_START
+
+[Depex]
+  gEfiCpuIo2ProtocolGuid AND
+  gEfiMetronomeArchProtocolGuid AND
+  gEfiCpuArchProtocolGuid
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h
new file mode 100644
index 0000000000..f35222b634
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostResource.h
@@ -0,0 +1,47 @@
+/** @file
+
+  The Header file of the Pci Host Bridge Driver.
+
+Copyright (c) 1999 - 2016, 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.
+
+**/
+#ifndef _PCI_HOST_RESOURCE_H_
+#define _PCI_HOST_RESOURCE_H_
+
+#include <PiDxe.h>
+
+#define PCI_RESOURCE_LESS         0xFFFFFFFFFFFFFFFE
+
+typedef enum {
+  TypeIo    = 0,
+  TypeMem32,
+  TypePMem32,
+  TypeMem64,
+  TypePMem64,
+  TypeBus,
+  TypeMax
+} PCI_RESOURCE_TYPE;
+
+typedef enum {
+  ResNone,
+  ResSubmitted,
+  ResAllocated,
+  ResStatusMax
+} RES_STATUS;
+
+typedef struct {
+  PCI_RESOURCE_TYPE Type;
+  UINT64            Base;
+  UINT64            Length;
+  UINT64            Alignment;
+  RES_STATUS        Status;
+} PCI_RES_NODE;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h
new file mode 100644
index 0000000000..13185b41ac
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridge.h
@@ -0,0 +1,578 @@
+/** @file
+
+  The PCI Root Bridge header file.
+
+Copyright (c) 1999 - 2016, 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.
+
+**/
+
+#ifndef _PCI_ROOT_BRIDGE_H_
+#define _PCI_ROOT_BRIDGE_H_
+
+#include <PiDxe.h>
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/Pci.h>
+
+//
+// Driver Consumed Protocol Prototypes
+//
+#include <Protocol/Metronome.h>
+#include <Protocol/CpuIo2.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciRootBridgeIo.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PciSegmentLib.h>
+#include "PciHostResource.h"
+
+
+typedef enum {
+  IoOperation,
+  MemOperation,
+  PciOperation
+} OPERATION_TYPE;
+
+#define MAP_INFO_SIGNATURE  SIGNATURE_32 ('_', 'm', 'a', 'p')
+typedef struct {
+  UINT32                                    Signature;
+  LIST_ENTRY                                Link;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION Operation;
+  UINTN                                     NumberOfBytes;
+  UINTN                                     NumberOfPages;
+  EFI_PHYSICAL_ADDRESS                      HostAddress;
+  EFI_PHYSICAL_ADDRESS                      MappedHostAddress;
+} MAP_INFO;
+#define MAP_INFO_FROM_LINK(a) CR (a, MAP_INFO, Link, MAP_INFO_SIGNATURE)
+
+#define PCI_ROOT_BRIDGE_SIGNATURE SIGNATURE_32 ('_', 'p', 'r', 'b')
+
+typedef struct {
+  UINT32                            Signature;
+  LIST_ENTRY                        Link;
+  EFI_HANDLE                        Handle;
+  UINT64                            AllocationAttributes;
+  UINT64                            Attributes;
+  UINT64                            Supports;
+  PCI_RES_NODE                      ResAllocNode[TypeMax];
+  PCI_ROOT_BRIDGE_APERTURE          Bus;
+  PCI_ROOT_BRIDGE_APERTURE          Io;
+  PCI_ROOT_BRIDGE_APERTURE          Mem;
+  PCI_ROOT_BRIDGE_APERTURE          PMem;
+  PCI_ROOT_BRIDGE_APERTURE          MemAbove4G;
+  PCI_ROOT_BRIDGE_APERTURE          PMemAbove4G;
+  BOOLEAN                           DmaAbove4G;
+  BOOLEAN                           NoExtendedConfigSpace;
+  VOID                              *ConfigBuffer;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  CHAR16                            *DevicePathStr;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL   RootBridgeIo;
+
+  BOOLEAN                           ResourceSubmitted;
+  LIST_ENTRY                        Maps;
+} PCI_ROOT_BRIDGE_INSTANCE;
+
+#define ROOT_BRIDGE_FROM_THIS(a) CR (a, PCI_ROOT_BRIDGE_INSTANCE, RootBridgeIo, PCI_ROOT_BRIDGE_SIGNATURE)
+
+#define ROOT_BRIDGE_FROM_LINK(a) CR (a, PCI_ROOT_BRIDGE_INSTANCE, Link, PCI_ROOT_BRIDGE_SIGNATURE)
+
+/**
+  Construct the Pci Root Bridge instance.
+
+  @param Bridge            The root bridge instance.
+
+  @return The pointer to PCI_ROOT_BRIDGE_INSTANCE just created
+          or NULL if creation fails.
+**/
+PCI_ROOT_BRIDGE_INSTANCE *
+CreateRootBridge (
+  IN PCI_ROOT_BRIDGE       *Bridge
+  );
+
+//
+// Protocol Member Function Prototypes
+//
+/**
+
+  Poll an address in memory mapped space until an exit condition is met
+  or a timeout occurs.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  Width of the memory operation.
+  @param Address  -  The base address of the memory operation.
+  @param Mask     -  Mask used for polling criteria.
+  @param Value    -  Comparison value used for polling exit criteria.
+  @param Delay    -  Number of 100ns units to poll.
+  @param Result   -  Pointer to the last value read from memory location.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_TIMEOUT            -  Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollMem (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+;
+
+/**
+
+  Poll an address in I/O space until an exit condition is met
+  or a timeout occurs.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  Width of I/O operation.
+  @param Address  -  The base address of the I/O operation.
+  @param Mask     -  Mask used for polling criteria.
+  @param Value    -  Comparison value used for polling exit criteria.
+  @param Delay    -  Number of 100ns units to poll.
+  @param Result   -  Pointer to the last value read from memory location.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_TIMEOUT            -  Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollIo (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+;
+
+/**
+
+  Allow read from memory mapped I/O space.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  The width of memory operation.
+  @param Address  -  Base address of the memory operation.
+  @param Count    -  Number of memory opeartion to perform.
+  @param Buffer   -  The destination buffer to store data.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Allow write to memory mapped I/O space.
+
+  @param This     -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width    -  The width of memory operation.
+  @param Address  -  Base address of the memory operation.
+  @param Count    -  Number of memory opeartion to perform.
+  @param Buffer   -  The source buffer to write data from.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Fail due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Enable a PCI driver to read PCI controller registers in the
+  PCI root bridge I/O space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width        -  Signifies the width of the memory operation.
+  @param UserAddress  -  The base address of the I/O operation.
+  @param Count        -  The number of I/O operations to perform.
+  @param UserBuffer   -  The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            -  The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 UserAddress,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *UserBuffer
+  )
+;
+
+/**
+
+  Enable a PCI driver to write to PCI controller registers in the
+  PCI root bridge I/O space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width        -  Signifies the width of the memory operation.
+  @param UserAddress  -  The base address of the I/O operation.
+  @param Count        -  The number of I/O operations to perform.
+  @param UserBuffer   -  The source buffer to write data from.
+
+  @retval EFI_SUCCESS            -  The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 UserAddress,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *UserBuffer
+  )
+;
+
+/**
+
+  Copy one region of PCI root bridge memory space to be copied to
+  another region of PCI root bridge memory space.
+
+  @param This         -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Width        -  Signifies the width of the memory operation.
+  @param DestAddress  -  Destination address of the memory operation.
+  @param SrcAddress   -  Source address of the memory operation.
+  @param Count        -  Number of memory operations to perform.
+
+  @retval EFI_SUCCESS            -  The data was copied successfully.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoCopyMem (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL          *This,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH    Width,
+  IN UINT64                                   DestAddress,
+  IN UINT64                                   SrcAddress,
+  IN UINTN                                    Count
+  )
+;
+
+/**
+
+  Allows read from PCI configuration space.
+
+  @param This     -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    -  Signifies the width of the memory operation.
+  @param Address  -  The address within the PCI configuration space
+                     for the PCI controller.
+  @param Count    -  The number of PCI configuration operations
+                     to perform.
+  @param Buffer   -  The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            -  The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+
+  Allows write to PCI configuration space.
+
+  @param This     -  A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    -  Signifies the width of the memory operation.
+  @param Address  -  The address within the PCI configuration space
+                     for the PCI controller.
+  @param Count    -  The number of PCI configuration operations
+                     to perform.
+  @param Buffer   -  The source buffer to get the results.
+
+  @retval EFI_SUCCESS            -  The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameters found.
+  @retval EFI_OUT_OF_RESOURCES   -  The request could not be completed due to a lack of
+                            @retval resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN OUT VOID                                   *Buffer
+  )
+;
+
+/**
+  Provides the PCI controller-specific address needed to access
+  system memory for DMA.
+
+  @param This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Operation      Indicate if the bus master is going to read or write
+                        to system memory.
+  @param HostAddress    The system memory address to map on the PCI controller.
+  @param NumberOfBytes  On input the number of bytes to map.
+                        On output the number of bytes that were mapped.
+  @param DeviceAddress  The resulting map address for the bus master PCI
+                        controller to use to access the system memory's HostAddress.
+  @param Mapping        The value to pass to Unmap() when the bus master DMA
+                        operation is complete.
+
+  @retval EFI_SUCCESS            Success.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+  @retval EFI_UNSUPPORTED        The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_DEVICE_ERROR       The System hardware could not map the requested address.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMap (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL            *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                                       *HostAddress,
+  IN OUT UINTN                                      *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS                       *DeviceAddress,
+  OUT    VOID                                       **Mapping
+  )
+;
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  The Unmap() function completes the Map() operation and releases any
+  corresponding resources.
+  If the operation was an EfiPciOperationBusMasterWrite or
+  EfiPciOperationBusMasterWrite64, the data is committed to the target system
+  memory.
+  Any resources used for the mapping are freed.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Mapping   The mapping value returned from Map().
+
+  @retval EFI_SUCCESS            The range was unmapped.
+  @retval EFI_INVALID_PARAMETER  Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR       The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoUnmap (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  VOID                             *Mapping
+  )
+;
+
+/**
+  Allocates pages that are suitable for an EfiPciOperationBusMasterCommonBuffer
+  or EfiPciOperationBusMasterCommonBuffer64 mapping.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Type        This parameter is not used and must be ignored.
+  @param MemoryType  The type of memory to allocate, EfiBootServicesData or
+                     EfiRuntimeServicesData.
+  @param Pages       The number of pages to allocate.
+  @param HostAddress A pointer to store the base system memory address of the
+                     allocated range.
+  @param Attributes  The requested bit mask of attributes for the allocated
+                     range. Only the attributes
+                     EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE,
+                     EFI_PCI_ATTRIBUTE_MEMORY_CACHED, and
+                     EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE may be used with this
+                     function.
+
+  @retval EFI_SUCCESS            The requested memory pages were allocated.
+  @retval EFI_INVALID_PARAMETER  MemoryType is invalid.
+  @retval EFI_INVALID_PARAMETER  HostAddress is NULL.
+  @retval EFI_UNSUPPORTED        Attributes is unsupported. The only legal
+                                 attribute bits are MEMORY_WRITE_COMBINE,
+                                 MEMORY_CACHED, and DUAL_ADDRESS_CYCLE.
+  @retval EFI_OUT_OF_RESOURCES   The memory pages could not be allocated.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoAllocateBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  EFI_ALLOCATE_TYPE                Type,
+  IN  EFI_MEMORY_TYPE                  MemoryType,
+  IN  UINTN                            Pages,
+  OUT VOID                             **HostAddress,
+  IN  UINT64                           Attributes
+  )
+;
+
+/**
+
+  Free memory allocated in AllocateBuffer.
+
+  @param This         -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+                         instance.
+  @param Pages        -  Number of pages to free.
+  @param HostAddress  -  The base system memory address of the
+                         allocated range.
+
+  @retval EFI_SUCCESS            -  Requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFreeBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  UINTN                            Pages,
+  OUT VOID                             *HostAddress
+  )
+;
+
+/**
+
+  Flushes all PCI posted write transactions from a PCI host
+  bridge to system memory.
+
+  @param This  - Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+
+  @retval EFI_SUCCESS       -  PCI posted write transactions were flushed
+                       @retval from PCI host bridge to system memory.
+  @retval EFI_DEVICE_ERROR  -  Fail due to hardware error.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFlush (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This
+  )
+;
+
+/**
+  Gets the attributes that a PCI root bridge supports setting with
+  SetAttributes(), and the attributes that a PCI root bridge is currently
+  using.
+
+  The GetAttributes() function returns the mask of attributes that this PCI
+  root bridge supports and the mask of attributes that the PCI root bridge is
+  currently using.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Supported   A pointer to the mask of attributes that this PCI root
+                     bridge supports setting with SetAttributes().
+  @param Attributes  A pointer to the mask of attributes that this PCI root
+                     bridge is currently using.
+
+  @retval  EFI_SUCCESS           If Supports is not NULL, then the attributes
+                                 that the PCI root bridge supports is returned
+                                 in Supports. If Attributes is not NULL, then
+                                 the attributes that the PCI root bridge is
+                                 currently using is returned in Attributes.
+  @retval  EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoGetAttributes (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT UINT64                           *Supported,
+  OUT UINT64                           *Attributes
+  )
+;
+
+/**
+
+  Sets the attributes for a resource range on a PCI root bridge.
+
+  @param This            -  Pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Attributes      -  The mask of attributes to set.
+  @param ResourceBase    -  Pointer to the base address of the resource range
+                            to be modified by the attributes specified by Attributes.
+  @param ResourceLength  -  Pointer to the length of the resource range to be modified.
+
+  @retval EFI_SUCCESS            -  Success.
+  @retval EFI_INVALID_PARAMETER  -  Invalid parameter found.
+  @retval EFI_OUT_OF_RESOURCES   -  Not enough resources to set the attributes upon.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoSetAttributes (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN     UINT64                           Attributes,
+  IN OUT UINT64                           *ResourceBase,
+  IN OUT UINT64                           *ResourceLength
+  )
+;
+
+/**
+
+  Retrieves the current resource settings of this PCI root bridge
+  in the form of a set of ACPI 2.0 resource descriptor.
+
+  @param This       -  Pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL instance.
+  @param Resources  -  Pointer to the ACPI 2.0 resource descriptor that
+                       describe the current configuration of this PCI root
+                       bridge.
+
+  @retval EFI_SUCCESS      -  Success.
+  @retval EFI_UNSUPPORTED  -  Current configuration of the PCI root bridge
+                      @retval could not be retrieved.
+
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoConfiguration (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT VOID                             **Resources
+  )
+;
+
+
+extern EFI_METRONOME_ARCH_PROTOCOL *mMetronome;
+extern EFI_CPU_IO2_PROTOCOL         *mCpuIo;
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c
new file mode 100644
index 0000000000..b2d76d67af
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciRootBridgeIo.c
@@ -0,0 +1,1594 @@
+/** @file
+
+  PCI Root Bridge Io Protocol code.
+
+Copyright (c) 1999 - 2016, 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 "PciHostBridge.h"
+#include "PciRootBridge.h"
+#include "PciHostResource.h"
+
+#define NO_MAPPING  (VOID *) (UINTN) -1
+
+//
+// Lookup table for increment values based on transfer widths
+//
+UINT8 mInStride[] = {
+  1, // EfiPciWidthUint8
+  2, // EfiPciWidthUint16
+  4, // EfiPciWidthUint32
+  8, // EfiPciWidthUint64
+  0, // EfiPciWidthFifoUint8
+  0, // EfiPciWidthFifoUint16
+  0, // EfiPciWidthFifoUint32
+  0, // EfiPciWidthFifoUint64
+  1, // EfiPciWidthFillUint8
+  2, // EfiPciWidthFillUint16
+  4, // EfiPciWidthFillUint32
+  8  // EfiPciWidthFillUint64
+};
+
+//
+// Lookup table for increment values based on transfer widths
+//
+UINT8 mOutStride[] = {
+  1, // EfiPciWidthUint8
+  2, // EfiPciWidthUint16
+  4, // EfiPciWidthUint32
+  8, // EfiPciWidthUint64
+  1, // EfiPciWidthFifoUint8
+  2, // EfiPciWidthFifoUint16
+  4, // EfiPciWidthFifoUint32
+  8, // EfiPciWidthFifoUint64
+  0, // EfiPciWidthFillUint8
+  0, // EfiPciWidthFillUint16
+  0, // EfiPciWidthFillUint32
+  0  // EfiPciWidthFillUint64
+};
+
+/**
+  Construct the Pci Root Bridge instance.
+
+  @param Bridge            The root bridge instance.
+
+  @return The pointer to PCI_ROOT_BRIDGE_INSTANCE just created
+          or NULL if creation fails.
+**/
+PCI_ROOT_BRIDGE_INSTANCE *
+CreateRootBridge (
+  IN PCI_ROOT_BRIDGE       *Bridge
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+  PCI_RESOURCE_TYPE        Index;
+  CHAR16                   *DevicePathStr;
+  PCI_ROOT_BRIDGE_APERTURE *Aperture;
+
+  DevicePathStr = NULL;
+
+  DEBUG ((EFI_D_INFO, "RootBridge: "));
+  DEBUG ((EFI_D_INFO, "%s\n", DevicePathStr = ConvertDevicePathToText (Bridge->DevicePath, FALSE, FALSE)));
+  DEBUG ((EFI_D_INFO, "  Support/Attr: %lx / %lx\n", Bridge->Supports, Bridge->Attributes));
+  DEBUG ((EFI_D_INFO, "    DmaAbove4G: %s\n", Bridge->DmaAbove4G ? L"Yes" : L"No"));
+  DEBUG ((EFI_D_INFO, "NoExtConfSpace: %s\n", Bridge->NoExtendedConfigSpace ? L"Yes" : L"No"));
+  DEBUG ((EFI_D_INFO, "     AllocAttr: %lx (%s%s)\n", Bridge->AllocationAttributes,
+          (Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0 ? L"CombineMemPMem " : L"",
+          (Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) != 0 ? L"Mem64Decode" : L""
+          ));
+  DEBUG ((EFI_D_INFO, "           Bus: %lx - %lx\n", Bridge->Bus.Base, Bridge->Bus.Limit));
+  DEBUG ((EFI_D_INFO, "            Io: %lx - %lx\n", Bridge->Io.Base, Bridge->Io.Limit));
+  DEBUG ((EFI_D_INFO, "           Mem: %lx - %lx\n", Bridge->Mem.Base, Bridge->Mem.Limit));
+  DEBUG ((EFI_D_INFO, "    MemAbove4G: %lx - %lx\n", Bridge->MemAbove4G.Base, Bridge->MemAbove4G.Limit));
+  DEBUG ((EFI_D_INFO, "          PMem: %lx - %lx\n", Bridge->PMem.Base, Bridge->PMem.Limit));
+  DEBUG ((EFI_D_INFO, "   PMemAbove4G: %lx - %lx\n", Bridge->PMemAbove4G.Base, Bridge->PMemAbove4G.Limit));
+
+  //
+  // Make sure Mem and MemAbove4G apertures are valid
+  //
+  if (Bridge->Mem.Base <= Bridge->Mem.Limit) {
+    ASSERT (Bridge->Mem.Limit < SIZE_4GB);
+    if (Bridge->Mem.Limit >= SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->MemAbove4G.Base <= Bridge->MemAbove4G.Limit) {
+    ASSERT (Bridge->MemAbove4G.Base >= SIZE_4GB);
+    if (Bridge->MemAbove4G.Base < SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->PMem.Base <= Bridge->PMem.Limit) {
+    ASSERT (Bridge->PMem.Limit < SIZE_4GB);
+    if (Bridge->PMem.Limit >= SIZE_4GB) {
+      return NULL;
+    }
+  }
+  if (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit) {
+    ASSERT (Bridge->PMemAbove4G.Base >= SIZE_4GB);
+    if (Bridge->PMemAbove4G.Base < SIZE_4GB) {
+      return NULL;
+    }
+  }
+
+  //
+  // Ignore AllocationAttributes when resources were already assigned.
+  //
+  if (!Bridge->ResourceAssigned) {
+    if ((Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM) != 0) {
+      //
+      // If this bit is set, then the PCI Root Bridge does not
+      // support separate windows for Non-prefetchable and Prefetchable
+      // memory.
+      //
+      ASSERT (Bridge->PMem.Base > Bridge->PMem.Limit);
+      ASSERT (Bridge->PMemAbove4G.Base > Bridge->PMemAbove4G.Limit);
+      if ((Bridge->PMem.Base <= Bridge->PMem.Limit) ||
+          (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit)
+          ) {
+        return NULL;
+      }
+    }
+
+    if ((Bridge->AllocationAttributes & EFI_PCI_HOST_BRIDGE_MEM64_DECODE) == 0) {
+      //
+      // If this bit is not set, then the PCI Root Bridge does not support
+      // 64 bit memory windows.
+      //
+      ASSERT (Bridge->MemAbove4G.Base > Bridge->MemAbove4G.Limit);
+      ASSERT (Bridge->PMemAbove4G.Base > Bridge->PMemAbove4G.Limit);
+      if ((Bridge->MemAbove4G.Base <= Bridge->MemAbove4G.Limit) ||
+          (Bridge->PMemAbove4G.Base <= Bridge->PMemAbove4G.Limit)
+          ) {
+        return NULL;
+      }
+    }
+  }
+
+  RootBridge = AllocateZeroPool (sizeof (PCI_ROOT_BRIDGE_INSTANCE));
+  ASSERT (RootBridge != NULL);
+
+  RootBridge->Signature = PCI_ROOT_BRIDGE_SIGNATURE;
+  RootBridge->Supports = Bridge->Supports;
+  RootBridge->Attributes = Bridge->Attributes;
+  RootBridge->DmaAbove4G = Bridge->DmaAbove4G;
+  RootBridge->NoExtendedConfigSpace = Bridge->NoExtendedConfigSpace;
+  RootBridge->AllocationAttributes = Bridge->AllocationAttributes;
+  RootBridge->DevicePath = DuplicateDevicePath (Bridge->DevicePath);
+  RootBridge->DevicePathStr = DevicePathStr;
+  RootBridge->ConfigBuffer = AllocatePool (
+    TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+    );
+  ASSERT (RootBridge->ConfigBuffer != NULL);
+  InitializeListHead (&RootBridge->Maps);
+
+  CopyMem (&RootBridge->Bus, &Bridge->Bus, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->Io, &Bridge->Io, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->Mem, &Bridge->Mem, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->MemAbove4G, &Bridge->MemAbove4G, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->PMem, &Bridge->PMem, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+  CopyMem (&RootBridge->PMemAbove4G, &Bridge->PMemAbove4G, sizeof (PCI_ROOT_BRIDGE_APERTURE));
+
+  for (Index = TypeIo; Index < TypeMax; Index++) {
+    switch (Index) {
+    case TypeBus:
+      Aperture = &RootBridge->Bus;
+      break;
+    case TypeIo:
+      Aperture = &RootBridge->Io;
+      break;
+    case TypeMem32:
+      Aperture = &RootBridge->Mem;
+      break;
+    case TypeMem64:
+      Aperture = &RootBridge->MemAbove4G;
+      break;
+    case TypePMem32:
+      Aperture = &RootBridge->PMem;
+      break;
+    case TypePMem64:
+      Aperture = &RootBridge->PMemAbove4G;
+      break;
+    default:
+      ASSERT (FALSE);
+      Aperture = NULL;
+      break;
+    }
+    RootBridge->ResAllocNode[Index].Type     = Index;
+    if (Bridge->ResourceAssigned && (Aperture->Limit >= Aperture->Base)) {
+      RootBridge->ResAllocNode[Index].Base   = Aperture->Base;
+      RootBridge->ResAllocNode[Index].Length = Aperture->Limit - Aperture->Base + 1;
+      RootBridge->ResAllocNode[Index].Status = ResAllocated;
+    } else {
+      RootBridge->ResAllocNode[Index].Base   = 0;
+      RootBridge->ResAllocNode[Index].Length = 0;
+      RootBridge->ResAllocNode[Index].Status = ResNone;
+    }
+  }
+
+  RootBridge->RootBridgeIo.SegmentNumber  = Bridge->Segment;
+  RootBridge->RootBridgeIo.PollMem        = RootBridgeIoPollMem;
+  RootBridge->RootBridgeIo.PollIo         = RootBridgeIoPollIo;
+  RootBridge->RootBridgeIo.Mem.Read       = RootBridgeIoMemRead;
+  RootBridge->RootBridgeIo.Mem.Write      = RootBridgeIoMemWrite;
+  RootBridge->RootBridgeIo.Io.Read        = RootBridgeIoIoRead;
+  RootBridge->RootBridgeIo.Io.Write       = RootBridgeIoIoWrite;
+  RootBridge->RootBridgeIo.CopyMem        = RootBridgeIoCopyMem;
+  RootBridge->RootBridgeIo.Pci.Read       = RootBridgeIoPciRead;
+  RootBridge->RootBridgeIo.Pci.Write      = RootBridgeIoPciWrite;
+  RootBridge->RootBridgeIo.Map            = RootBridgeIoMap;
+  RootBridge->RootBridgeIo.Unmap          = RootBridgeIoUnmap;
+  RootBridge->RootBridgeIo.AllocateBuffer = RootBridgeIoAllocateBuffer;
+  RootBridge->RootBridgeIo.FreeBuffer     = RootBridgeIoFreeBuffer;
+  RootBridge->RootBridgeIo.Flush          = RootBridgeIoFlush;
+  RootBridge->RootBridgeIo.GetAttributes  = RootBridgeIoGetAttributes;
+  RootBridge->RootBridgeIo.SetAttributes  = RootBridgeIoSetAttributes;
+  RootBridge->RootBridgeIo.Configuration  = RootBridgeIoConfiguration;
+
+  return RootBridge;
+}
+
+/**
+  Check parameters for IO,MMIO,PCI read/write services of PCI Root Bridge IO.
+
+  The I/O operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and I/O width restrictions that a PI
+  System on a platform might require. For example on some platforms, width
+  requests of EfiCpuIoWidthUint64 do not work. Misaligned buffers, on the other
+  hand, will be handled by the driver.
+
+  @param[in] This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+
+  @param[in] OperationType  I/O operation type: IO/MMIO/PCI.
+
+  @param[in] Width          Signifies the width of the I/O or Memory operation.
+
+  @param[in] Address        The base address of the I/O operation.
+
+  @param[in] Count          The number of I/O operations to perform. The number
+                            of bytes moved is Width size * Count, starting at
+                            Address.
+
+  @param[in] Buffer         For read operations, the destination buffer to
+                            store the results. For write operations, the source
+                            buffer from which to write data.
+
+  @retval EFI_SUCCESS            The parameters for this request pass the
+                                 checks.
+
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PI system.
+
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+
+  @retval EFI_UNSUPPORTED        The Buffer is not aligned for the given Width.
+
+  @retval EFI_UNSUPPORTED        The address range specified by Address, Width,
+                                 and Count is not valid for this PI system.
+**/
+EFI_STATUS
+RootBridgeIoCheckParameter (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN OPERATION_TYPE                         OperationType,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN UINT64                                 Address,
+  IN UINTN                                  Count,
+  IN VOID                                   *Buffer
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE                     *RootBridge;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS  *PciRbAddr;
+  UINT64                                       Base;
+  UINT64                                       Limit;
+  UINT32                                       Size;
+
+  //
+  // Check to see if Buffer is NULL
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check to see if Width is in the valid range
+  //
+  if ((UINT32) Width >= EfiPciWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For FIFO type, the target address won't increase during the access,
+  // so treat Count as 1
+  //
+  if (Width >= EfiPciWidthFifoUint8 && Width <= EfiPciWidthFifoUint64) {
+    Count = 1;
+  }
+
+  Width = (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH) (Width & 0x03);
+  Size  = 1 << Width;
+
+  //
+  // Check to see if Address is aligned
+  //
+  if ((Address & (Size - 1)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  //
+  // Check to see if any address associated with this transfer exceeds the
+  // maximum allowed address.  The maximum address implied by the parameters
+  // passed in is Address + Size * Count.  If the following condition is met,
+  // then the transfer is not supported.
+  //
+  //    Address + Size * Count > Limit + 1
+  //
+  // Since Limit can be the maximum integer value supported by the CPU and
+  // Count can also be the maximum integer value supported by the CPU, this
+  // range check must be adjusted to avoid all oveflow conditions.
+  //
+  if (OperationType == IoOperation) {
+    //
+    // Allow Legacy IO access
+    //
+    if (Address + MultU64x32 (Count, Size) <= 0x1000) {
+      if ((RootBridge->Attributes & (
+           EFI_PCI_ATTRIBUTE_ISA_IO | EFI_PCI_ATTRIBUTE_VGA_PALETTE_IO | EFI_PCI_ATTRIBUTE_VGA_IO |
+           EFI_PCI_ATTRIBUTE_IDE_PRIMARY_IO | EFI_PCI_ATTRIBUTE_IDE_SECONDARY_IO |
+           EFI_PCI_ATTRIBUTE_ISA_IO_16 | EFI_PCI_ATTRIBUTE_VGA_PALETTE_IO_16 | EFI_PCI_ATTRIBUTE_VGA_IO_16)) != 0) {
+        return EFI_SUCCESS;
+      }
+    }
+    Base = RootBridge->Io.Base;
+    Limit = RootBridge->Io.Limit;
+  } else if (OperationType == MemOperation) {
+    //
+    // Allow Legacy MMIO access
+    //
+    if ((Address >= 0xA0000) && (Address + MultU64x32 (Count, Size)) <= 0xC0000) {
+      if ((RootBridge->Attributes & EFI_PCI_ATTRIBUTE_VGA_MEMORY) != 0) {
+        return EFI_SUCCESS;
+      }
+    }
+    //
+    // By comparing the Address against Limit we know which range to be used
+    // for checking
+    //
+    if (Address + MultU64x32 (Count, Size) <= RootBridge->Mem.Limit + 1) {
+      Base = RootBridge->Mem.Base;
+      Limit = RootBridge->Mem.Limit;
+    } else {
+      Base = RootBridge->MemAbove4G.Base;
+      Limit = RootBridge->MemAbove4G.Limit;
+    }
+  } else {
+    PciRbAddr = (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS*) &Address;
+    if (PciRbAddr->Bus < RootBridge->Bus.Base ||
+        PciRbAddr->Bus > RootBridge->Bus.Limit) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (PciRbAddr->Device > PCI_MAX_DEVICE ||
+        PciRbAddr->Function > PCI_MAX_FUNC) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (PciRbAddr->ExtendedRegister != 0) {
+      Address = PciRbAddr->ExtendedRegister;
+    } else {
+      Address = PciRbAddr->Register;
+    }
+    Base = 0;
+    Limit = RootBridge->NoExtendedConfigSpace ? 0xFF : 0xFFF;
+  }
+
+  if (Address < Base) {
+      return EFI_INVALID_PARAMETER;
+  }
+
+  if (Address + MultU64x32 (Count, Size) > Limit + 1) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Polls an address in memory mapped I/O space until an exit condition is met,
+  or a timeout occurs.
+
+  This function provides a standard way to poll a PCI memory location. A PCI
+  memory read operation is performed at the PCI memory address specified by
+  Address for the width specified by Width. The result of this PCI memory read
+  operation is stored in Result. This PCI memory read operation is repeated
+  until either a timeout of Delay 100 ns units has expired, or (Result & Mask)
+  is equal to Value.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operations.
+  @param[in]   Address   The base address of the memory operations. The caller
+                         is responsible for aligning Address if required.
+  @param[in]   Mask      Mask used for the polling criteria. Bytes above Width
+                         in Mask are ignored. The bits in the bytes below Width
+                         which are zero in Mask are ignored when polling the
+                         memory address.
+  @param[in]   Value     The comparison value used for the polling exit
+                         criteria.
+  @param[in]   Delay     The number of 100 ns units to poll. Note that timer
+                         available may be of poorer granularity.
+  @param[out]  Result    Pointer to the last value read from the memory
+                         location.
+
+  @retval EFI_SUCCESS            The last data returned from the access matched
+                                 the poll exit criteria.
+  @retval EFI_INVALID_PARAMETER  Width is invalid.
+  @retval EFI_INVALID_PARAMETER  Result is NULL.
+  @retval EFI_TIMEOUT            Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollMem (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      NumberOfTicks;
+  UINT32      Remainder;
+
+  if (Result == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32)Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // No matter what, always do a single poll.
+  //
+  Status = This->Mem.Read (This, Width, Address, 1, Result);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((*Result & Mask) == Value) {
+    return EFI_SUCCESS;
+  }
+
+  if (Delay == 0) {
+    return EFI_SUCCESS;
+
+  } else {
+
+    //
+    // Determine the proper # of metronome ticks to wait for polling the
+    // location.  The nuber of ticks is Roundup (Delay /
+    // mMetronome->TickPeriod)+1
+    // The "+1" to account for the possibility of the first tick being short
+    // because we started in the middle of a tick.
+    //
+    // BugBug: overriding mMetronome->TickPeriod with UINT32 until Metronome
+    // protocol definition is updated.
+    //
+    NumberOfTicks = DivU64x32Remainder (Delay, (UINT32) mMetronome->TickPeriod,
+                      &Remainder);
+    if (Remainder != 0) {
+      NumberOfTicks += 1;
+    }
+    NumberOfTicks += 1;
+
+    while (NumberOfTicks != 0) {
+
+      mMetronome->WaitForTick (mMetronome, 1);
+
+      Status = This->Mem.Read (This, Width, Address, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if ((*Result & Mask) == Value) {
+        return EFI_SUCCESS;
+      }
+
+      NumberOfTicks -= 1;
+    }
+  }
+  return EFI_TIMEOUT;
+}
+
+/**
+  Reads from the I/O space of a PCI Root Bridge. Returns when either the
+  polling exit criteria is satisfied or after a defined duration.
+
+  This function provides a standard way to poll a PCI I/O location. A PCI I/O
+  read operation is performed at the PCI I/O address specified by Address for
+  the width specified by Width.
+  The result of this PCI I/O read operation is stored in Result. This PCI I/O
+  read operation is repeated until either a timeout of Delay 100 ns units has
+  expired, or (Result & Mask) is equal to Value.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Width     Signifies the width of the I/O operations.
+  @param[in] Address   The base address of the I/O operations. The caller is
+                       responsible for aligning Address if required.
+  @param[in] Mask      Mask used for the polling criteria. Bytes above Width in
+                       Mask are ignored. The bits in the bytes below Width
+                       which are zero in Mask are ignored when polling the I/O
+                       address.
+  @param[in] Value     The comparison value used for the polling exit criteria.
+  @param[in] Delay     The number of 100 ns units to poll. Note that timer
+                       available may be of poorer granularity.
+  @param[out] Result   Pointer to the last value read from the memory location.
+
+  @retval EFI_SUCCESS            The last data returned from the access matched
+                                 the poll exit criteria.
+  @retval EFI_INVALID_PARAMETER  Width is invalid.
+  @retval EFI_INVALID_PARAMETER  Result is NULL.
+  @retval EFI_TIMEOUT            Delay expired before a match occurred.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPollIo (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN  UINT64                                 Address,
+  IN  UINT64                                 Mask,
+  IN  UINT64                                 Value,
+  IN  UINT64                                 Delay,
+  OUT UINT64                                 *Result
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      NumberOfTicks;
+  UINT32      Remainder;
+
+  //
+  // No matter what, always do a single poll.
+  //
+
+  if (Result == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32)Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = This->Io.Read (This, Width, Address, 1, Result);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  if ((*Result & Mask) == Value) {
+    return EFI_SUCCESS;
+  }
+
+  if (Delay == 0) {
+    return EFI_SUCCESS;
+
+  } else {
+
+    //
+    // Determine the proper # of metronome ticks to wait for polling the
+    // location.  The number of ticks is Roundup (Delay /
+    // mMetronome->TickPeriod)+1
+    // The "+1" to account for the possibility of the first tick being short
+    // because we started in the middle of a tick.
+    //
+    NumberOfTicks = DivU64x32Remainder (Delay, (UINT32)mMetronome->TickPeriod,
+                      &Remainder);
+    if (Remainder != 0) {
+      NumberOfTicks += 1;
+    }
+    NumberOfTicks += 1;
+
+    while (NumberOfTicks != 0) {
+
+      mMetronome->WaitForTick (mMetronome, 1);
+
+      Status = This->Io.Read (This, Width, Address, 1, Result);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if ((*Result & Mask) == Value) {
+        return EFI_SUCCESS;
+      }
+
+      NumberOfTicks -= 1;
+    }
+  }
+  return EFI_TIMEOUT;
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge memory space.
+
+  The Mem.Read(), and Mem.Write() functions enable a driver to access PCI
+  controller registers in the PCI root bridge memory space.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI Root Bridge on a platform might require.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operation.
+  @param[in]   Address   The base address of the memory operation. The caller
+                         is responsible for aligning the Address if required.
+  @param[in]   Count     The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at Address.
+  @param[out]  Buffer    For read operations, the destination buffer to store
+                         the results. For write operations, the source buffer
+                         to write data from.
+
+  @retval EFI_SUCCESS            The data was read from or written to the PCI
+                                 root bridge.
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  OUT    VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                             Status;
+
+  Status = RootBridgeIoCheckParameter (This, MemOperation, Width, Address,
+                                       Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Mem.Read (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge memory space.
+
+  The Mem.Read(), and Mem.Write() functions enable a driver to access PCI
+  controller registers in the PCI root bridge memory space.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI Root Bridge on a platform might require.
+
+  @param[in]   This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width     Signifies the width of the memory operation.
+  @param[in]   Address   The base address of the memory operation. The caller
+                         is responsible for aligning the Address if required.
+  @param[in]   Count     The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at Address.
+  @param[in]   Buffer    For read operations, the destination buffer to store
+                         the results. For write operations, the source buffer
+                         to write data from.
+
+  @retval EFI_SUCCESS            The data was read from or written to the PCI
+                                 root bridge.
+  @retval EFI_INVALID_PARAMETER  Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to a
+                                 lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMemWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  IN     VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                             Status;
+
+  Status = RootBridgeIoCheckParameter (This, MemOperation, Width, Address,
+                                       Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Mem.Write (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge I/O space.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width       Signifies the width of the memory operations.
+  @param[in]   Address     The base address of the I/O operation. The caller is
+                           responsible for aligning the Address if required.
+  @param[in]   Count       The number of I/O operations to perform. Bytes moved
+                           is Width size * Count, starting at Address.
+  @param[out]  Buffer      For read operations, the destination buffer to store
+                           the results. For write operations, the source buffer
+                           to write data from.
+
+  @retval EFI_SUCCESS              The data was read from or written to the PCI
+                                   root bridge.
+  @retval EFI_INVALID_PARAMETER    Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER    Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL        *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                 Address,
+  IN     UINTN                                  Count,
+  OUT    VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                                    Status;
+  Status = RootBridgeIoCheckParameter (
+             This, IoOperation, Width,
+             Address, Count, Buffer
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Io.Read (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to access PCI controller registers in the PCI root
+  bridge I/O space.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in]   Width       Signifies the width of the memory operations.
+  @param[in]   Address     The base address of the I/O operation. The caller is
+                           responsible for aligning the Address if required.
+  @param[in]   Count       The number of I/O operations to perform. Bytes moved
+                           is Width size * Count, starting at Address.
+  @param[in]   Buffer      For read operations, the destination buffer to store
+                           the results. For write operations, the source buffer
+                           to write data from.
+
+  @retval EFI_SUCCESS              The data was read from or written to the PCI
+                                   root bridge.
+  @retval EFI_INVALID_PARAMETER    Width is invalid for this PCI root bridge.
+  @retval EFI_INVALID_PARAMETER    Buffer is NULL.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoIoWrite (
+  IN       EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL         *This,
+  IN       EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH   Width,
+  IN       UINT64                                  Address,
+  IN       UINTN                                   Count,
+  IN       VOID                                    *Buffer
+  )
+{
+  EFI_STATUS                                    Status;
+  Status = RootBridgeIoCheckParameter (
+             This, IoOperation, Width,
+             Address, Count, Buffer
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return mCpuIo->Io.Write (mCpuIo, (EFI_CPU_IO_PROTOCOL_WIDTH) Width, Address, Count, Buffer);
+}
+
+/**
+  Enables a PCI driver to copy one region of PCI root bridge memory space to
+  another region of PCI root bridge memory space.
+
+  The CopyMem() function enables a PCI driver to copy one region of PCI root
+  bridge memory space to another region of PCI root bridge memory space. This
+  is especially useful for video scroll operation on a memory mapped video
+  buffer.
+  The memory operations are carried out exactly as requested. The caller is
+  responsible for satisfying any alignment and memory width restrictions that a
+  PCI root bridge on a platform might require.
+
+  @param[in] This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+                         instance.
+  @param[in] Width       Signifies the width of the memory operations.
+  @param[in] DestAddress The destination address of the memory operation. The
+                         caller is responsible for aligning the DestAddress if
+                         required.
+  @param[in] SrcAddress  The source address of the memory operation. The caller
+                         is responsible for aligning the SrcAddress if
+                         required.
+  @param[in] Count       The number of memory operations to perform. Bytes
+                         moved is Width size * Count, starting at DestAddress
+                         and SrcAddress.
+
+  @retval  EFI_SUCCESS             The data was copied from one memory region
+                                   to another memory region.
+  @retval  EFI_INVALID_PARAMETER   Width is invalid for this PCI root bridge.
+  @retval  EFI_OUT_OF_RESOURCES    The request could not be completed due to a
+                                   lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoCopyMem (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL              *This,
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH        Width,
+  IN UINT64                                       DestAddress,
+  IN UINT64                                       SrcAddress,
+  IN UINTN                                        Count
+  )
+{
+  EFI_STATUS  Status;
+  BOOLEAN     Forward;
+  UINTN       Stride;
+  UINTN       Index;
+  UINT64      Result;
+
+  if ((UINT32) Width > EfiPciWidthUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DestAddress == SrcAddress) {
+    return EFI_SUCCESS;
+  }
+
+  Stride = (UINTN) (1 << Width);
+
+  Forward = TRUE;
+  if ((DestAddress > SrcAddress) &&
+      (DestAddress < (SrcAddress + Count * Stride))) {
+    Forward = FALSE;
+    SrcAddress = SrcAddress + (Count - 1) * Stride;
+    DestAddress = DestAddress + (Count - 1) * Stride;
+  }
+
+  for (Index = 0; Index < Count; Index++) {
+    Status = RootBridgeIoMemRead (
+               This,
+               Width,
+               SrcAddress,
+               1,
+               &Result
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = RootBridgeIoMemWrite (
+               This,
+               Width,
+               DestAddress,
+               1,
+               &Result
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if (Forward) {
+      SrcAddress += Stride;
+      DestAddress += Stride;
+    } else {
+      SrcAddress -= Stride;
+      DestAddress -= Stride;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  PCI configuration space access.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Read     TRUE indicating it's a read operation.
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The destination buffer to store the results.
+
+  @retval EFI_SUCCESS            The data was read/written from/to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciAccess (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     BOOLEAN                               Read,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  EFI_STATUS                                   Status;
+  PCI_ROOT_BRIDGE_INSTANCE                     *RootBridge;
+  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_PCI_ADDRESS  PciAddress;
+  UINT8                                        *Uint8Buffer;
+  UINT8                                        InStride;
+  UINT8                                        OutStride;
+  UINTN                                        Size;
+
+  Status = RootBridgeIoCheckParameter (This, PciOperation, Width, Address, Count, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Read Pci configuration space
+  //
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  CopyMem (&PciAddress, &Address, sizeof (PciAddress));
+
+  if (PciAddress.ExtendedRegister == 0) {
+    PciAddress.ExtendedRegister = PciAddress.Register;
+  }
+
+  Address = PCI_SEGMENT_LIB_ADDRESS (
+              RootBridge->RootBridgeIo.SegmentNumber,
+              PciAddress.Bus,
+              PciAddress.Device,
+              PciAddress.Function,
+              PciAddress.ExtendedRegister
+              );
+
+  //
+  // Select loop based on the width of the transfer
+  //
+  InStride  = mInStride[Width];
+  OutStride = mOutStride[Width];
+  Size      = (UINTN) (1 << (Width & 0x03));
+  for (Uint8Buffer = Buffer; Count > 0; Address += InStride, Uint8Buffer += OutStride, Count--) {
+    if (Read) {
+      PciSegmentReadBuffer (Address, Size, Uint8Buffer);
+    } else {
+      PciSegmentWriteBuffer (Address, Size, Uint8Buffer);
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Allows read from PCI configuration space.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The destination buffer to store the results.
+
+  @retval EFI_SUCCESS           The data was read from the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciRead (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  return RootBridgeIoPciAccess (This, TRUE, Width, Address, Count, Buffer);
+}
+
+/**
+  Allows write to PCI configuration space.
+
+  @param This     A pointer to EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL
+  @param Width    Signifies the width of the memory operation.
+  @param Address  The address within the PCI configuration space
+                  for the PCI controller.
+  @param Count    The number of PCI configuration operations
+                  to perform.
+  @param Buffer   The source buffer to get the results.
+
+  @retval EFI_SUCCESS            The data was written to the PCI root bridge.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoPciWrite (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL       *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_WIDTH Width,
+  IN     UINT64                                Address,
+  IN     UINTN                                 Count,
+  IN OUT VOID                                  *Buffer
+  )
+{
+  return RootBridgeIoPciAccess (This, FALSE, Width, Address, Count, Buffer);
+}
+
+/**
+  Provides the PCI controller-specific address needed to access
+  system memory for DMA.
+
+  @param This           A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Operation      Indicate if the bus master is going to read or write
+                        to system memory.
+  @param HostAddress    The system memory address to map on the PCI controller.
+  @param NumberOfBytes  On input the number of bytes to map.
+                        On output the number of bytes that were mapped.
+  @param DeviceAddress  The resulting map address for the bus master PCI
+                        controller to use to access the system memory's HostAddress.
+  @param Mapping        The value to pass to Unmap() when the bus master DMA
+                        operation is complete.
+
+  @retval EFI_SUCCESS            Success.
+  @retval EFI_INVALID_PARAMETER  Invalid parameters found.
+  @retval EFI_UNSUPPORTED        The HostAddress cannot be mapped as a common buffer.
+  @retval EFI_DEVICE_ERROR       The System hardware could not map the requested address.
+  @retval EFI_OUT_OF_RESOURCES   The request could not be completed due to lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoMap (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL            *This,
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL_OPERATION  Operation,
+  IN     VOID                                       *HostAddress,
+  IN OUT UINTN                                      *NumberOfBytes,
+  OUT    EFI_PHYSICAL_ADDRESS                       *DeviceAddress,
+  OUT    VOID                                       **Mapping
+  )
+{
+  EFI_STATUS                                        Status;
+  PCI_ROOT_BRIDGE_INSTANCE                          *RootBridge;
+  EFI_PHYSICAL_ADDRESS                              PhysicalAddress;
+  MAP_INFO                                          *MapInfo;
+
+  if (HostAddress == NULL || NumberOfBytes == NULL || DeviceAddress == NULL ||
+      Mapping == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Make sure that Operation is valid
+  //
+  if ((UINT32) Operation >= EfiPciOperationMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  PhysicalAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress;
+  if (!RootBridge->DmaAbove4G && ((PhysicalAddress + *NumberOfBytes) > SIZE_4GB)) {
+    //
+    // If the root bridge can not handle performing DMA above 4GB but
+    // any part of the DMA transfer being mapped is above 4GB, then
+    // map the DMA transfer to a buffer below 4GB.
+    //
+
+    if (Operation == EfiPciOperationBusMasterCommonBuffer ||
+        Operation == EfiPciOperationBusMasterCommonBuffer64) {
+      //
+      // Common Buffer operations can not be remapped.  If the common buffer
+      // if above 4GB, then it is not possible to generate a mapping, so return
+      // an error.
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Allocate a MAP_INFO structure to remember the mapping when Unmap() is
+    // called later.
+    //
+    MapInfo = AllocatePool (sizeof (MAP_INFO));
+    if (MapInfo == NULL) {
+      *NumberOfBytes = 0;
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Initialize the MAP_INFO structure
+    //
+    MapInfo->Signature         = MAP_INFO_SIGNATURE;
+    MapInfo->Operation         = Operation;
+    MapInfo->NumberOfBytes     = *NumberOfBytes;
+    MapInfo->NumberOfPages     = EFI_SIZE_TO_PAGES (MapInfo->NumberOfBytes);
+    MapInfo->HostAddress       = PhysicalAddress;
+    MapInfo->MappedHostAddress = SIZE_4GB - 1;
+
+    //
+    // Allocate a buffer below 4GB to map the transfer to.
+    //
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiBootServicesData,
+                    MapInfo->NumberOfPages,
+                    &MapInfo->MappedHostAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (MapInfo);
+      *NumberOfBytes = 0;
+      return Status;
+    }
+
+    //
+    // If this is a read operation from the Bus Master's point of view,
+    // then copy the contents of the real buffer into the mapped buffer
+    // so the Bus Master can read the contents of the real buffer.
+    //
+    if (Operation == EfiPciOperationBusMasterRead ||
+        Operation == EfiPciOperationBusMasterRead64) {
+      CopyMem (
+        (VOID *) (UINTN) MapInfo->MappedHostAddress,
+        (VOID *) (UINTN) MapInfo->HostAddress,
+        MapInfo->NumberOfBytes
+        );
+    }
+
+    InsertTailList (&RootBridge->Maps, &MapInfo->Link);
+
+    //
+    // The DeviceAddress is the address of the maped buffer below 4GB
+    //
+    *DeviceAddress = MapInfo->MappedHostAddress;
+    //
+    // Return a pointer to the MAP_INFO structure in Mapping
+    //
+    *Mapping       = MapInfo;
+  } else {
+    //
+    // If the root bridge CAN handle performing DMA above 4GB or
+    // the transfer is below 4GB, so the DeviceAddress is simply the
+    // HostAddress
+    //
+    *DeviceAddress = PhysicalAddress;
+    *Mapping       = NO_MAPPING;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Completes the Map() operation and releases any corresponding resources.
+
+  The Unmap() function completes the Map() operation and releases any
+  corresponding resources.
+  If the operation was an EfiPciOperationBusMasterWrite or
+  EfiPciOperationBusMasterWrite64, the data is committed to the target system
+  memory.
+  Any resources used for the mapping are freed.
+
+  @param[in] This      A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[in] Mapping   The mapping value returned from Map().
+
+  @retval EFI_SUCCESS            The range was unmapped.
+  @retval EFI_INVALID_PARAMETER  Mapping is not a value that was returned by Map().
+  @retval EFI_DEVICE_ERROR       The data was not committed to the target system memory.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoUnmap (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN VOID                             *Mapping
+  )
+{
+  MAP_INFO                 *MapInfo;
+  LIST_ENTRY               *Link;
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  //
+  // See if the Map() operation associated with this Unmap() required a mapping
+  // buffer. If a mapping buffer was not required, then this function simply
+  // returns EFI_SUCCESS.
+  //
+  if (Mapping == NO_MAPPING) {
+    return EFI_SUCCESS;
+  }
+
+  MapInfo = NO_MAPPING;
+  for (Link = GetFirstNode (&RootBridge->Maps)
+       ; !IsNull (&RootBridge->Maps, Link)
+       ; Link = GetNextNode (&RootBridge->Maps, Link)
+       ) {
+    MapInfo = MAP_INFO_FROM_LINK (Link);
+    if (MapInfo == Mapping) {
+      break;
+    }
+  }
+  //
+  // Mapping is not a valid value returned by Map()
+  //
+  if (MapInfo != Mapping) {
+    return EFI_INVALID_PARAMETER;
+  }
+  RemoveEntryList (&MapInfo->Link);
+
+  //
+  // If this is a write operation from the Bus Master's point of view,
+  // then copy the contents of the mapped buffer into the real buffer
+  // so the processor can read the contents of the real buffer.
+  //
+  if (MapInfo->Operation == EfiPciOperationBusMasterWrite ||
+      MapInfo->Operation == EfiPciOperationBusMasterWrite64) {
+    CopyMem (
+      (VOID *) (UINTN) MapInfo->HostAddress,
+      (VOID *) (UINTN) MapInfo->MappedHostAddress,
+      MapInfo->NumberOfBytes
+      );
+  }
+
+  //
+  // Free the mapped buffer and the MAP_INFO structure.
+  //
+  gBS->FreePages (MapInfo->MappedHostAddress, MapInfo->NumberOfPages);
+  FreePool (Mapping);
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates pages that are suitable for an EfiPciOperationBusMasterCommonBuffer
+  or EfiPciOperationBusMasterCommonBuffer64 mapping.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Type        This parameter is not used and must be ignored.
+  @param MemoryType  The type of memory to allocate, EfiBootServicesData or
+                     EfiRuntimeServicesData.
+  @param Pages       The number of pages to allocate.
+  @param HostAddress A pointer to store the base system memory address of the
+                     allocated range.
+  @param Attributes  The requested bit mask of attributes for the allocated
+                     range. Only the attributes
+                     EFI_PCI_ATTRIBUTE_MEMORY_WRITE_COMBINE,
+                     EFI_PCI_ATTRIBUTE_MEMORY_CACHED, and
+                     EFI_PCI_ATTRIBUTE_DUAL_ADDRESS_CYCLE may be used with this
+                     function.
+
+  @retval EFI_SUCCESS            The requested memory pages were allocated.
+  @retval EFI_INVALID_PARAMETER  MemoryType is invalid.
+  @retval EFI_INVALID_PARAMETER  HostAddress is NULL.
+  @retval EFI_UNSUPPORTED        Attributes is unsupported. The only legal
+                                 attribute bits are MEMORY_WRITE_COMBINE,
+                                 MEMORY_CACHED, and DUAL_ADDRESS_CYCLE.
+  @retval EFI_OUT_OF_RESOURCES   The memory pages could not be allocated.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoAllocateBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  EFI_ALLOCATE_TYPE                Type,
+  IN  EFI_MEMORY_TYPE                  MemoryType,
+  IN  UINTN                            Pages,
+  OUT VOID                             **HostAddress,
+  IN  UINT64                           Attributes
+  )
+{
+  EFI_STATUS                Status;
+  EFI_PHYSICAL_ADDRESS      PhysicalAddress;
+  PCI_ROOT_BRIDGE_INSTANCE  *RootBridge;
+  EFI_ALLOCATE_TYPE         AllocateType;
+
+  //
+  // Validate Attributes
+  //
+  if ((Attributes & EFI_PCI_ATTRIBUTE_INVALID_FOR_ALLOCATE_BUFFER) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check for invalid inputs
+  //
+  if (HostAddress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The only valid memory types are EfiBootServicesData and
+  // EfiRuntimeServicesData
+  //
+  if (MemoryType != EfiBootServicesData &&
+      MemoryType != EfiRuntimeServicesData) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  AllocateType = AllocateAnyPages;
+  if (!RootBridge->DmaAbove4G) {
+    //
+    // Limit allocations to memory below 4GB
+    //
+    AllocateType    = AllocateMaxAddress;
+    PhysicalAddress = (EFI_PHYSICAL_ADDRESS) (SIZE_4GB - 1);
+  }
+  Status = gBS->AllocatePages (
+                  AllocateType,
+                  MemoryType,
+                  Pages,
+                  &PhysicalAddress
+                  );
+  if (!EFI_ERROR (Status)) {
+    *HostAddress = (VOID *) (UINTN) PhysicalAddress;
+  }
+
+  return Status;
+}
+
+/**
+  Frees memory that was allocated with AllocateBuffer().
+
+  The FreeBuffer() function frees memory that was allocated with
+  AllocateBuffer().
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Pages       The number of pages to free.
+  @param HostAddress The base system memory address of the allocated range.
+
+  @retval EFI_SUCCESS            The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER  The memory range specified by HostAddress and
+                                 Pages was not allocated with AllocateBuffer().
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFreeBuffer (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN  UINTN                            Pages,
+  OUT VOID                             *HostAddress
+  )
+{
+  return gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) HostAddress, Pages);
+}
+
+/**
+  Flushes all PCI posted write transactions from a PCI host bridge to system
+  memory.
+
+  The Flush() function flushes any PCI posted write transactions from a PCI
+  host bridge to system memory. Posted write transactions are generated by PCI
+  bus masters when they perform write transactions to target addresses in
+  system memory.
+  This function does not flush posted write transactions from any PCI bridges.
+  A PCI controller specific action must be taken to guarantee that the posted
+  write transactions have been flushed from the PCI controller and from all the
+  PCI bridges into the PCI host bridge. This is typically done with a PCI read
+  transaction from the PCI controller prior to calling Flush().
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+
+  @retval EFI_SUCCESS        The PCI posted write transactions were flushed
+                             from the PCI host bridge to system memory.
+  @retval EFI_DEVICE_ERROR   The PCI posted write transactions were not flushed
+                             from the PCI host bridge due to a hardware error.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoFlush (
+  IN EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL           *This
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the attributes that a PCI root bridge supports setting with
+  SetAttributes(), and the attributes that a PCI root bridge is currently
+  using.
+
+  The GetAttributes() function returns the mask of attributes that this PCI
+  root bridge supports and the mask of attributes that the PCI root bridge is
+  currently using.
+
+  @param This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Supported   A pointer to the mask of attributes that this PCI root
+                     bridge supports setting with SetAttributes().
+  @param Attributes  A pointer to the mask of attributes that this PCI root
+                     bridge is currently using.
+
+  @retval  EFI_SUCCESS           If Supports is not NULL, then the attributes
+                                 that the PCI root bridge supports is returned
+                                 in Supports. If Attributes is not NULL, then
+                                 the attributes that the PCI root bridge is
+                                 currently using is returned in Attributes.
+  @retval  EFI_INVALID_PARAMETER Both Supports and Attributes are NULL.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoGetAttributes (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  OUT UINT64                           *Supported,
+  OUT UINT64                           *Attributes
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE *RootBridge;
+
+  if (Attributes == NULL && Supported == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  //
+  // Set the return value for Supported and Attributes
+  //
+  if (Supported != NULL) {
+    *Supported  = RootBridge->Supports;
+  }
+
+  if (Attributes != NULL) {
+    *Attributes = RootBridge->Attributes;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets attributes for a resource range on a PCI root bridge.
+
+  The SetAttributes() function sets the attributes specified in Attributes for
+  the PCI root bridge on the resource range specified by ResourceBase and
+  ResourceLength. Since the granularity of setting these attributes may vary
+  from resource type to resource type, and from platform to platform, the
+  actual resource range and the one passed in by the caller may differ. As a
+  result, this function may set the attributes specified by Attributes on a
+  larger resource range than the caller requested. The actual range is returned
+  in ResourceBase and ResourceLength. The caller is responsible for verifying
+  that the actual range for which the attributes were set is acceptable.
+
+  @param This            A pointer to the
+                         EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param Attributes      The mask of attributes to set. If the
+                         attribute bit MEMORY_WRITE_COMBINE,
+                         MEMORY_CACHED, or MEMORY_DISABLE is set,
+                         then the resource range is specified by
+                         ResourceBase and ResourceLength. If
+                         MEMORY_WRITE_COMBINE, MEMORY_CACHED, and
+                         MEMORY_DISABLE are not set, then
+                         ResourceBase and ResourceLength are ignored,
+                         and may be NULL.
+  @param ResourceBase    A pointer to the base address of the
+                         resource range to be modified by the
+                         attributes specified by Attributes.
+  @param ResourceLength  A pointer to the length of the resource
+                                   range to be modified by the attributes
+                                   specified by Attributes.
+
+  @retval  EFI_SUCCESS           The current configuration of this PCI root bridge
+                                 was returned in Resources.
+  @retval  EFI_UNSUPPORTED       The current configuration of this PCI root bridge
+                                 could not be retrieved.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoSetAttributes (
+  IN     EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL  *This,
+  IN     UINT64                           Attributes,
+  IN OUT UINT64                           *ResourceBase,
+  IN OUT UINT64                           *ResourceLength
+  )
+{
+  PCI_ROOT_BRIDGE_INSTANCE            *RootBridge;
+
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+
+  if ((Attributes & (~RootBridge->Supports)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  RootBridge->Attributes = Attributes;
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current resource settings of this PCI root bridge in the form
+  of a set of ACPI 2.0 resource descriptors.
+
+  There are only two resource descriptor types from the ACPI Specification that
+  may be used to describe the current resources allocated to a PCI root bridge.
+  These are the QWORD Address Space Descriptor (ACPI 2.0 Section 6.4.3.5.1),
+  and the End Tag (ACPI 2.0 Section 6.4.2.8). The QWORD Address Space
+  Descriptor can describe memory, I/O, and bus number ranges for dynamic or
+  fixed resources. The configuration of a PCI root bridge is described with one
+  or more QWORD Address Space Descriptors followed by an End Tag.
+
+  @param[in]   This        A pointer to the EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  @param[out]  Resources   A pointer to the ACPI 2.0 resource descriptors that
+                           describe the current configuration of this PCI root
+                           bridge. The storage for the ACPI 2.0 resource
+                           descriptors is allocated by this function. The
+                           caller must treat the return buffer as read-only
+                           data, and the buffer must not be freed by the
+                           caller.
+
+  @retval  EFI_SUCCESS     The current configuration of this PCI root bridge
+                           was returned in Resources.
+  @retval  EFI_UNSUPPORTED The current configuration of this PCI root bridge
+                           could not be retrieved.
+**/
+EFI_STATUS
+EFIAPI
+RootBridgeIoConfiguration (
+  IN  EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL     *This,
+  OUT VOID                                **Resources
+  )
+{
+  PCI_RESOURCE_TYPE                 Index;
+  PCI_ROOT_BRIDGE_INSTANCE          *RootBridge;
+  PCI_RES_NODE                      *ResAllocNode;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  EFI_ACPI_END_TAG_DESCRIPTOR       *End;
+
+  //
+  // Get this instance of the Root Bridge.
+  //
+  RootBridge = ROOT_BRIDGE_FROM_THIS (This);
+  ZeroMem (
+    RootBridge->ConfigBuffer,
+    TypeMax * sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) + sizeof (EFI_ACPI_END_TAG_DESCRIPTOR)
+    );
+  Descriptor = RootBridge->ConfigBuffer;
+  for (Index = TypeIo; Index < TypeMax; Index++) {
+
+    ResAllocNode = &RootBridge->ResAllocNode[Index];
+
+    if (ResAllocNode->Status != ResAllocated) {
+      continue;
+    }
+
+    Descriptor->Desc = ACPI_ADDRESS_SPACE_DESCRIPTOR;
+    Descriptor->Len  = sizeof (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR) - 3;
+    Descriptor->AddrRangeMin  = ResAllocNode->Base;
+    Descriptor->AddrRangeMax  = ResAllocNode->Base + ResAllocNode->Length - 1;
+    Descriptor->AddrLen       = ResAllocNode->Length;
+    switch (ResAllocNode->Type) {
+
+    case TypeIo:
+      Descriptor->ResType       = ACPI_ADDRESS_SPACE_TYPE_IO;
+      break;
+
+    case TypePMem32:
+      Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+    case TypeMem32:
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      Descriptor->AddrSpaceGranularity  = 32;
+      break;
+
+    case TypePMem64:
+      Descriptor->SpecificFlag = EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE;
+    case TypeMem64:
+      Descriptor->ResType               = ACPI_ADDRESS_SPACE_TYPE_MEM;
+      Descriptor->AddrSpaceGranularity  = 64;
+      break;
+
+    case TypeBus:
+      Descriptor->ResType       = ACPI_ADDRESS_SPACE_TYPE_BUS;
+      break;
+
+    default:
+      break;
+    }
+
+    Descriptor++;
+  }
+  //
+  // Terminate the entries.
+  //
+  End = (EFI_ACPI_END_TAG_DESCRIPTOR *) Descriptor;
+  End->Desc     = ACPI_END_TAG_DESCRIPTOR;
+  End->Checksum = 0x0;
+
+  *Resources = RootBridge->ConfigBuffer;
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c
new file mode 100644
index 0000000000..994dc847c9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/ComponentName.c
@@ -0,0 +1,288 @@
+/** @file
+  UEFI Component Name and Name2 protocol for Isa serial driver.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gPciSioSerialComponentName = {
+  SerialComponentNameGetDriverName,
+  SerialComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gPciSioSerialComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) SerialComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) SerialComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mSerialDriverNameTable[] = {
+  {
+    "eng;en",
+    L"PCI SIO Serial Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 mSioSerialPortName[] = L"SIO Serial Port #%d";
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 mPciSerialPortName[] = L"PCI Serial Port #%d";
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mSerialDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gPciSioSerialComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                Status;
+  EFI_SERIAL_IO_PROTOCOL    *SerialIo;
+  SERIAL_DEV                *SerialDevice;
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+  EFI_GUID                  *IoProtocolGuid;
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  IoProtocolGuid = &gEfiSioProtocolGuid;
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gSerialControllerDriver.DriverBindingHandle,
+             IoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    IoProtocolGuid = &gEfiPciIoProtocolGuid;
+    Status = EfiTestManagedDevice (
+               ControllerHandle,
+               gSerialControllerDriver.DriverBindingHandle,
+               IoProtocolGuid
+               );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ControllerNameTable = NULL;
+  if (ChildHandle != NULL) {
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               IoProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the Serial I/O Protocol from the child handle
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandle,
+                    &gEfiSerialIoProtocolGuid,
+                    (VOID **) &SerialIo,
+                    gSerialControllerDriver.DriverBindingHandle,
+                    ChildHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the Serial Controller's Device structure
+    //
+    SerialDevice = SERIAL_DEV_FROM_THIS (SerialIo);
+    ControllerNameTable = SerialDevice->ControllerNameTable;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gPciSioSerialComponentName)
+           );
+}
+
+/**
+  Add the ISO639-2 and RFC4646 component name both for the Serial IO device
+
+  @param SerialDevice     A pointer to the SERIAL_DEV instance.
+  @param Instance         Instance ID for the serial device.
+**/
+VOID
+AddName (
+  IN  SERIAL_DEV                               *SerialDevice,
+  IN  UINT32                                   Instance
+  )
+{
+  CHAR16                                       SerialPortName[SERIAL_PORT_NAME_LEN];
+  UnicodeSPrint (
+    SerialPortName,
+    sizeof (SerialPortName),
+    (SerialDevice->PciDeviceInfo != NULL) ? PCI_SERIAL_PORT_NAME : SIO_SERIAL_PORT_NAME,
+    Instance
+    );
+  AddUnicodeString2 (
+    "eng",
+    gPciSioSerialComponentName.SupportedLanguages,
+    &SerialDevice->ControllerNameTable,
+    SerialPortName,
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gPciSioSerialComponentName2.SupportedLanguages,
+    &SerialDevice->ControllerNameTable,
+    SerialPortName,
+    FALSE
+    );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf
new file mode 100644
index 0000000000..03fddfe75e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf
@@ -0,0 +1,81 @@
+## @file
+# Serial driver for standard UARTS on a SIO chip or PCI/PCIE card.
+#
+# Produces the Serial I/O protocol for standard UARTS using Super I/O or PCI I/O.
+#
+# Copyright (c) 2007 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciSioSerialDxe
+  MODULE_UNI_FILE                = PciSioSerialDxe.uni
+  FILE_GUID                      = E2775B47-D453-4EE3-ADA7-391A1B05AC17
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePciSioSerial
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gSerialControllerDriver
+#  COMPONENT_NAME                =  gPciSioSerialComponentName
+#  COMPONENT_NAME2               =  gPciSioSerialComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  SerialIo.c
+  Serial.h
+  Serial.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  ReportStatusCodeLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  DevicePathLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  IoLib
+
+[Guids]
+  gEfiUartDevicePathGuid                        ## SOMETIMES_CONSUMES   ## GUID
+
+[Protocols]
+  gEfiSioProtocolGuid                           ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiSerialIoProtocolGuid                      ## BY_START
+  gEfiDevicePathProtocolGuid                    ## BY_START
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseHalfHandshake|FALSE   ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultBaudRate|115200    ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultDataBits|8         ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultParity|1           ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultStopBits|1         ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialClockRate|1843200 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciSerialParameters     ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PciSioSerialDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni
new file mode 100644
index 0000000000..617f583b35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni
new file mode 100644
index 0000000000..935bdba93e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c
new file mode 100644
index 0000000000..aeafee247c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.c
@@ -0,0 +1,1248 @@
+/** @file
+  Serial driver for PCI or SIO UARTS.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+//
+// ISA Serial Driver Global Variables
+//
+
+EFI_DRIVER_BINDING_PROTOCOL gSerialControllerDriver = {
+  SerialControllerDriverSupported,
+  SerialControllerDriverStart,
+  SerialControllerDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+CONTROLLER_DEVICE_PATH mControllerDevicePathTemplate = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_CONTROLLER_DP,
+    {
+      (UINT8) (sizeof (CONTROLLER_DEVICE_PATH)),
+      (UINT8) ((sizeof (CONTROLLER_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+SERIAL_DEV  gSerialDevTemplate = {
+  SERIAL_DEV_SIGNATURE,
+  NULL,
+  {
+    SERIAL_IO_INTERFACE_REVISION,
+    SerialReset,
+    SerialSetAttributes,
+    SerialSetControl,
+    SerialGetControl,
+    SerialWrite,
+    SerialRead,
+    NULL
+  },                                       // SerialIo
+  {
+    SERIAL_PORT_SUPPORT_CONTROL_MASK,
+    SERIAL_PORT_DEFAULT_TIMEOUT,
+    0,
+    16,
+    0,
+    0,
+    0
+  },                                       // SerialMode
+  NULL,                                    // DevicePath
+  NULL,                                    // ParentDevicePath
+  {
+    {
+      MESSAGING_DEVICE_PATH,
+      MSG_UART_DP,
+      {
+        (UINT8) (sizeof (UART_DEVICE_PATH)),
+        (UINT8) ((sizeof (UART_DEVICE_PATH)) >> 8)
+      }
+    },
+    0, 0, 0, 0, 0
+  },                                       // UartDevicePath
+  0,                                       // BaseAddress
+  FALSE,                                   // MmioAccess
+  1,                                       // RegisterStride
+  0,                                       // ClockRate
+  16,                                      // ReceiveFifoDepth
+  { 0, 0 },                                // Receive;
+  16,                                      // TransmitFifoDepth
+  { 0, 0 },                                // Transmit;
+  FALSE,                                   // SoftwareLoopbackEnable;
+  FALSE,                                   // HardwareFlowControl;
+  NULL,                                    // *ControllerNameTable;
+  FALSE,                                   // ContainsControllerNode;
+  0,                                       // Instance;
+  NULL                                     // *PciDeviceInfo;
+};
+
+/**
+  Check the device path node whether it's the Flow Control node or not.
+
+  @param[in] FlowControl    The device path node to be checked.
+  
+  @retval TRUE              It's the Flow Control node.
+  @retval FALSE             It's not.
+
+**/
+BOOLEAN
+IsUartFlowControlDevicePathNode (
+  IN UART_FLOW_CONTROL_DEVICE_PATH *FlowControl
+  )
+{
+  return (BOOLEAN) (
+           (DevicePathType (FlowControl) == MESSAGING_DEVICE_PATH) &&
+           (DevicePathSubType (FlowControl) == MSG_VENDOR_DP) &&
+           (CompareGuid (&FlowControl->Guid, &gEfiUartDevicePathGuid))
+           );
+}
+
+/**
+  The user Entry Point for module PciSioSerial. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePciSioSerial (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gSerialControllerDriver,
+             ImageHandle,
+             &gPciSioSerialComponentName,
+             &gPciSioSerialComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Initialize UART default setting in gSerialDevTempate
+  //
+  gSerialDevTemplate.SerialMode.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  gSerialDevTemplate.SerialMode.DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  gSerialDevTemplate.SerialMode.Parity   = PcdGet8 (PcdUartDefaultParity);
+  gSerialDevTemplate.SerialMode.StopBits = PcdGet8 (PcdUartDefaultStopBits);
+  gSerialDevTemplate.UartDevicePath.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  gSerialDevTemplate.UartDevicePath.DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  gSerialDevTemplate.UartDevicePath.Parity   = PcdGet8 (PcdUartDefaultParity);
+  gSerialDevTemplate.UartDevicePath.StopBits = PcdGet8 (PcdUartDefaultStopBits);
+  gSerialDevTemplate.ClockRate = PcdGet32 (PcdSerialClockRate);
+
+  return Status;
+}
+
+/**
+  Return whether the controller is a SIO serial controller.
+
+  @param  Controller   The controller handle.
+
+  @retval EFI_SUCCESS  The controller is a SIO serial controller.
+  @retval others       The controller is not a SIO serial controller.
+**/
+EFI_STATUS
+IsSioSerialController (
+  EFI_HANDLE               Controller
+  )
+{
+  EFI_STATUS               Status;
+  EFI_SIO_PROTOCOL         *Sio;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+  ACPI_HID_DEVICE_PATH     *Acpi;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSioProtocolGuid,
+                  (VOID **) &Sio,
+                  gSerialControllerDriver.DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiSioProtocolGuid,
+           gSerialControllerDriver.DriverBindingHandle,
+           Controller
+           );
+
+    Status = gBS->OpenProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      (VOID **) &DevicePath,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller,
+      EFI_OPEN_PROTOCOL_BY_DRIVER
+      );
+    ASSERT (Status != EFI_ALREADY_STARTED);
+
+    if (!EFI_ERROR (Status)) {
+      do {
+        Acpi = (ACPI_HID_DEVICE_PATH *) DevicePath;
+        DevicePath = NextDevicePathNode (DevicePath);
+      } while (!IsDevicePathEnd (DevicePath));
+
+      if (DevicePathType (Acpi) != ACPI_DEVICE_PATH ||
+          (DevicePathSubType (Acpi) != ACPI_DP && DevicePathSubType (Acpi) != ACPI_EXTENDED_DP) ||
+          Acpi->HID != EISA_PNP_ID (0x501)
+          ) {
+        Status = EFI_UNSUPPORTED;
+      }
+    }
+
+    //
+    // Close protocol, don't use device path protocol in the Support() function
+    //
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiDevicePathProtocolGuid,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller
+      );
+  }
+  return Status;
+}
+
+/**
+  Return whether the controller is a PCI serial controller.
+
+  @param  Controller   The controller handle.
+
+  @retval EFI_SUCCESS  The controller is a PCI serial controller.
+  @retval others       The controller is not a PCI serial controller.
+**/
+EFI_STATUS
+IsPciSerialController (
+  EFI_HANDLE               Controller
+  )
+{
+  EFI_STATUS               Status;
+  EFI_PCI_IO_PROTOCOL      *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+  PCI_TYPE00               Pci;
+  PCI_SERIAL_PARAMETER     *PciSerialParameter;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+    Controller,
+    &gEfiPciIoProtocolGuid,
+    (VOID **) &PciIo,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller,
+    EFI_OPEN_PROTOCOL_BY_DRIVER
+    );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    Status = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0, sizeof (Pci), &Pci);
+    if (!EFI_ERROR (Status)) {
+      if (!IS_PCI_16550_SERIAL (&Pci)) {
+        for (PciSerialParameter = (PCI_SERIAL_PARAMETER *) PcdGetPtr (PcdPciSerialParameters)
+             ; PciSerialParameter->VendorId != 0xFFFF
+             ; PciSerialParameter++
+             ) {
+          if ((Pci.Hdr.VendorId == PciSerialParameter->VendorId) &&
+              (Pci.Hdr.DeviceId == PciSerialParameter->DeviceId)
+              ) {
+            break;
+          }
+        }
+        if (PciSerialParameter->VendorId == 0xFFFF) {
+          Status = EFI_UNSUPPORTED;
+        } else {
+          Status = EFI_SUCCESS;
+        }
+      }
+    }
+
+    //
+    // Close the I/O Abstraction(s) used to perform the supported test
+    //
+    gBS->CloseProtocol (
+      Controller,
+      &gEfiPciIoProtocolGuid,
+      gSerialControllerDriver.DriverBindingHandle,
+      Controller
+      );
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+    Controller,
+    &gEfiDevicePathProtocolGuid,
+    (VOID **) &DevicePath,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller,
+    EFI_OPEN_PROTOCOL_BY_DRIVER
+    );
+  ASSERT (Status != EFI_ALREADY_STARTED);
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+    Controller,
+    &gEfiDevicePathProtocolGuid,
+    gSerialControllerDriver.DriverBindingHandle,
+    Controller
+    );
+
+  return Status;
+}
+
+/**
+  Check to see if this driver supports the given controller
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          This driver can support the given controller
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+
+{
+  EFI_STATUS                                Status;
+  UART_DEVICE_PATH                          *Uart;
+  UART_FLOW_CONTROL_DEVICE_PATH             *FlowControl;
+
+  //
+  // Test RemainingDevicePath
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = EFI_UNSUPPORTED;
+
+    Uart = SkipControllerDevicePathNode (RemainingDevicePath, NULL, NULL);
+    if (DevicePathType (Uart) != MESSAGING_DEVICE_PATH ||
+        DevicePathSubType (Uart) != MSG_UART_DP ||
+        DevicePathNodeLength (Uart) != sizeof (UART_DEVICE_PATH)
+        ) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Do a rough check because Clock Rate is unknown until DriverBindingStart()
+    //
+    if (!VerifyUartParameters (0, Uart->BaudRate, Uart->DataBits, Uart->Parity, Uart->StopBits, NULL, NULL)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+    if (IsUartFlowControlDevicePathNode (FlowControl)) {
+      //
+      // If the second node is Flow Control Node,
+      //   return error when it request other than hardware flow control.
+      //
+      if ((ReadUnaligned32 (&FlowControl->FlowControlMap) & ~UART_FLOW_CONTROL_HARDWARE) != 0) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  Status = IsSioSerialController (Controller);
+  if (EFI_ERROR (Status)) {
+    Status = IsPciSerialController (Controller);
+  }
+  return Status;  
+}
+
+/**
+  Create the child serial device instance.
+
+  @param Controller           The parent controller handle.
+  @param Uart                 Pointer to the UART device path node in RemainingDevicePath,
+                              or NULL if RemainingDevicePath is NULL.
+  @param ParentDevicePath     Pointer to the parent device path.
+  @param CreateControllerNode TRUE to create the controller node.
+  @param Instance             Instance number of the serial device.
+                              The value will be set to the controller node
+                              if CreateControllerNode is TRUE.
+  @param ParentIo             A union type pointer to either Sio or PciIo.
+  @param PciSerialParameter   The PCI serial parameter to be used by current serial device.
+                              NULL for SIO serial device.
+  @param PciDeviceInfo        The PCI device info for the current serial device.
+                              NULL for SIO serial device.
+
+  @retval EFI_SUCCESS         The serial device was created successfully.
+  @retval others              The serial device wasn't created.
+**/
+EFI_STATUS
+CreateSerialDevice (
+  IN EFI_HANDLE                     Controller,
+  IN UART_DEVICE_PATH               *Uart,
+  IN EFI_DEVICE_PATH_PROTOCOL       *ParentDevicePath,
+  IN BOOLEAN                        CreateControllerNode,
+  IN UINT32                         Instance,
+  IN PARENT_IO_PROTOCOL_PTR         ParentIo,
+  IN PCI_SERIAL_PARAMETER           *PciSerialParameter, OPTIONAL
+  IN PCI_DEVICE_INFO                *PciDeviceInfo       OPTIONAL
+  )
+{
+  EFI_STATUS                                 Status;
+  SERIAL_DEV                                 *SerialDevice;
+  UINT8                                      BarIndex;
+  UINT64                                     Offset;
+  UART_FLOW_CONTROL_DEVICE_PATH              *FlowControl;
+  UINT32                                     FlowControlMap;
+  ACPI_RESOURCE_HEADER_PTR                   Resources;
+  EFI_ACPI_IO_PORT_DESCRIPTOR                *Io;
+  EFI_ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR *FixedIo;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR          *AddressSpace;
+  EFI_DEVICE_PATH_PROTOCOL                   *TempDevicePath;
+
+  BarIndex = 0;
+  Offset = 0;
+  FlowControl = NULL;
+  FlowControlMap = 0;
+
+  //
+  // Initialize the serial device instance
+  //
+  SerialDevice = AllocateCopyPool (sizeof (SERIAL_DEV), &gSerialDevTemplate);
+  ASSERT (SerialDevice != NULL);
+
+  SerialDevice->SerialIo.Mode    = &(SerialDevice->SerialMode);
+  SerialDevice->ParentDevicePath = ParentDevicePath;
+  SerialDevice->PciDeviceInfo    = PciDeviceInfo;
+  SerialDevice->Instance         = Instance;
+
+  if (Uart != NULL) {
+    CopyMem (&SerialDevice->UartDevicePath, Uart, sizeof (UART_DEVICE_PATH));
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+    if (IsUartFlowControlDevicePathNode (FlowControl)) {
+      FlowControlMap = ReadUnaligned32 (&FlowControl->FlowControlMap);
+    } else {
+      FlowControl = NULL;
+    }
+  }
+
+  //
+  // For PCI serial device, use the information from PCD
+  //
+  if (PciSerialParameter != NULL) {
+    BarIndex = (PciSerialParameter->BarIndex == PCI_BAR_ALL) ? 0 : PciSerialParameter->BarIndex;
+    Offset = PciSerialParameter->Offset;
+    if (PciSerialParameter->RegisterStride != 0) {
+      SerialDevice->RegisterStride = PciSerialParameter->RegisterStride;
+    }
+    if (PciSerialParameter->ClockRate != 0) {
+      SerialDevice->ClockRate = PciSerialParameter->ClockRate;
+    }
+    if (PciSerialParameter->ReceiveFifoDepth != 0) {
+      SerialDevice->ReceiveFifoDepth = PciSerialParameter->ReceiveFifoDepth;
+    }
+    if (PciSerialParameter->TransmitFifoDepth != 0) {
+      SerialDevice->TransmitFifoDepth = PciSerialParameter->TransmitFifoDepth;
+    }
+  }
+
+  //
+  // Pass NULL ActualBaudRate to VerifyUartParameters to disallow baudrate degrade.
+  // DriverBindingStart() shouldn't create a handle with different UART device path.
+  //
+  if (!VerifyUartParameters (SerialDevice->ClockRate, SerialDevice->UartDevicePath.BaudRate, SerialDevice->UartDevicePath.DataBits,
+                            SerialDevice->UartDevicePath.Parity, SerialDevice->UartDevicePath.StopBits, NULL, NULL
+                            )) {
+    Status = EFI_INVALID_PARAMETER;
+    goto CreateError;
+  }
+
+  if (PciSerialParameter == NULL) {
+    Status = ParentIo.Sio->GetResources (ParentIo.Sio, &Resources);
+  } else {
+    Status = ParentIo.PciIo->GetBarAttributes (ParentIo.PciIo, BarIndex, NULL, (VOID **) &Resources);
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Get the base address information from ACPI resource descriptor.
+    // ACPI_IO_PORT_DESCRIPTOR and ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR are returned from Sio;
+    // ACPI_ADDRESS_SPACE_DESCRIPTOR is returned from PciIo.
+    //
+    while ((Resources.SmallHeader->Byte != ACPI_END_TAG_DESCRIPTOR) && (SerialDevice->BaseAddress == 0)) {
+      switch (Resources.SmallHeader->Byte) {
+      case ACPI_IO_PORT_DESCRIPTOR:
+        Io = (EFI_ACPI_IO_PORT_DESCRIPTOR *) Resources.SmallHeader;
+        if (Io->Length != 0) {
+          SerialDevice->BaseAddress = Io->BaseAddressMin;
+        }
+        break;
+
+      case ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR:
+        FixedIo = (EFI_ACPI_FIXED_LOCATION_IO_PORT_DESCRIPTOR *) Resources.SmallHeader;
+        if (FixedIo->Length != 0) {
+          SerialDevice->BaseAddress = FixedIo->BaseAddress;
+        }
+        break;
+
+      case ACPI_ADDRESS_SPACE_DESCRIPTOR:
+        AddressSpace = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Resources.SmallHeader;
+        if (AddressSpace->AddrLen != 0) {
+          if (AddressSpace->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+            SerialDevice->MmioAccess = TRUE;
+          }
+          SerialDevice->BaseAddress = AddressSpace->AddrRangeMin + Offset;
+        }
+        break;
+      }
+
+      if (Resources.SmallHeader->Bits.Type == 0) {
+        Resources.SmallHeader = (ACPI_SMALL_RESOURCE_HEADER *) ((UINT8 *) Resources.SmallHeader
+                                                                + Resources.SmallHeader->Bits.Length
+                                                                + sizeof (*Resources.SmallHeader));
+      } else {
+        Resources.LargeHeader = (ACPI_LARGE_RESOURCE_HEADER *) ((UINT8 *) Resources.LargeHeader
+                                                                + Resources.LargeHeader->Length
+                                                                + sizeof (*Resources.LargeHeader));
+      }
+    }
+  }
+
+  if (SerialDevice->BaseAddress == 0) {
+    Status = EFI_INVALID_PARAMETER;
+    goto CreateError;
+  }
+
+  SerialDevice->HardwareFlowControl = (BOOLEAN) (FlowControlMap == UART_FLOW_CONTROL_HARDWARE);
+
+  //
+  // Report status code the serial present
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_PRESENCE_DETECT | EFI_PERIPHERAL_SERIAL_PORT,
+    SerialDevice->ParentDevicePath
+    );
+
+  if (!SerialPresent (SerialDevice)) {
+    Status = EFI_DEVICE_ERROR;
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_NOT_DETECTED | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->ParentDevicePath
+      );
+    goto CreateError;
+  }
+
+  //
+  // 1. Append Controller device path node.
+  //
+  if (CreateControllerNode) {
+    mControllerDevicePathTemplate.ControllerNumber = SerialDevice->Instance;
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 SerialDevice->ParentDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &mControllerDevicePathTemplate
+                                 );
+    SerialDevice->ContainsControllerNode = TRUE;
+  }
+
+  //
+  // 2. Append UART device path node.
+  //    The Uart setings are zero here.
+  //    SetAttribute() will update them to match the default setings.
+  //
+  TempDevicePath = SerialDevice->DevicePath;
+  if (TempDevicePath != NULL) {
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 TempDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &SerialDevice->UartDevicePath
+                                 );
+    FreePool (TempDevicePath);
+  } else {
+    SerialDevice->DevicePath = AppendDevicePathNode (
+                                 SerialDevice->ParentDevicePath,
+                                 (EFI_DEVICE_PATH_PROTOCOL *) &SerialDevice->UartDevicePath
+                                 );
+  }
+  //
+  // 3. Append the Flow Control device path node.
+  //    Only produce the Flow Control node when remaining device path has it
+  //
+  if (FlowControl != NULL) {
+    TempDevicePath = SerialDevice->DevicePath;
+    if (TempDevicePath != NULL) {
+      SerialDevice->DevicePath = AppendDevicePathNode (
+                                   TempDevicePath,
+                                   (EFI_DEVICE_PATH_PROTOCOL *) FlowControl
+                                   );
+      FreePool (TempDevicePath);
+    }
+  }
+  ASSERT (SerialDevice->DevicePath != NULL);
+
+  //
+  // Fill in Serial I/O Mode structure based on either the RemainingDevicePath or defaults.
+  //
+  SerialDevice->SerialMode.BaudRate = SerialDevice->UartDevicePath.BaudRate;
+  SerialDevice->SerialMode.DataBits = SerialDevice->UartDevicePath.DataBits;
+  SerialDevice->SerialMode.Parity   = SerialDevice->UartDevicePath.Parity;
+  SerialDevice->SerialMode.StopBits = SerialDevice->UartDevicePath.StopBits;
+
+  //
+  // Issue a reset to initialize the COM port
+  //
+  Status = SerialDevice->SerialIo.Reset (&SerialDevice->SerialIo);
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_CONTROLLER_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+    goto CreateError;
+  }
+
+  AddName (SerialDevice, Instance);
+  //
+  // Install protocol interfaces for the serial device.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &SerialDevice->Handle,
+                  &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+                  &gEfiSerialIoProtocolGuid, &SerialDevice->SerialIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto CreateError;
+  }
+  //
+  // Open For Child Device
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  PciSerialParameter != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+                  (VOID **) &ParentIo,
+                  gSerialControllerDriver.DriverBindingHandle,
+                  SerialDevice->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           &SerialDevice->Handle,
+           &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+           &gEfiSerialIoProtocolGuid, &SerialDevice->SerialIo,
+           NULL
+           );
+  }
+
+CreateError:
+  if (EFI_ERROR (Status)) {
+    if (SerialDevice->DevicePath != NULL) {
+      FreePool (SerialDevice->DevicePath);
+    }
+    if (SerialDevice->ControllerNameTable != NULL) {
+      FreeUnicodeStringTable (SerialDevice->ControllerNameTable);
+    }
+    FreePool (SerialDevice);
+  }
+  return Status;
+}
+
+/**
+  Returns an array of pointers containing all the child serial device pointers.
+
+  @param Controller      The parent controller handle.
+  @param IoProtocolGuid  The protocol GUID, either equals to gEfiSioProtocolGuid
+                         or equals to gEfiPciIoProtocolGuid.
+  @param Count           Count of the serial devices.
+
+  @return  An array of pointers containing all the child serial device pointers.
+**/
+SERIAL_DEV **
+GetChildSerialDevices (
+  IN EFI_HANDLE                       Controller,
+  IN EFI_GUID                         *IoProtocolGuid,
+  OUT UINTN                           *Count
+  )
+{
+  EFI_STATUS                                 Status;
+  UINTN                                      Index;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY        *OpenInfoBuffer;
+  UINTN                                      EntryCount;
+  SERIAL_DEV                                 **SerialDevices;
+  EFI_SERIAL_IO_PROTOCOL                     *SerialIo;
+  BOOLEAN                                    OpenByDriver;
+
+  *Count = 0;
+  //
+  // If the SerialIo instance specified by RemainingDevicePath is already created,
+  // update the attributes/control.
+  //
+  Status = gBS->OpenProtocolInformation (
+    Controller,
+    IoProtocolGuid,
+    &OpenInfoBuffer,
+    &EntryCount
+    );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  SerialDevices = AllocatePool (EntryCount * sizeof (SERIAL_DEV *));
+  ASSERT (SerialDevices != NULL);
+
+  *Count = 0;
+  OpenByDriver = FALSE;
+  for (Index = 0; Index < EntryCount; Index++) {
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+        OpenInfoBuffer[Index].ControllerHandle,
+        &gEfiSerialIoProtocolGuid,
+        (VOID **) &SerialIo,
+        gSerialControllerDriver.DriverBindingHandle,
+        Controller,
+        EFI_OPEN_PROTOCOL_GET_PROTOCOL
+        );
+      if (!EFI_ERROR (Status)) {
+        SerialDevices[(*Count)++] = SERIAL_DEV_FROM_THIS (SerialIo);
+      }
+    }
+
+
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+      ASSERT (OpenInfoBuffer[Index].AgentHandle == gSerialControllerDriver.DriverBindingHandle);
+      OpenByDriver = TRUE;
+    }
+  }
+  if (OpenInfoBuffer != NULL) {
+    FreePool (OpenInfoBuffer);
+  }
+
+  ASSERT ((*Count == 0) || (OpenByDriver));
+
+  return SerialDevices;
+}
+
+/**
+  Start to management the controller passed in
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS   Driver is started successfully
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                                 Status;
+  UINTN                                      Index;
+  EFI_DEVICE_PATH_PROTOCOL                   *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL                   *Node;
+  EFI_SERIAL_IO_PROTOCOL                     *SerialIo;
+  UINT32                                     ControllerNumber;
+  UART_DEVICE_PATH                           *Uart;
+  UART_FLOW_CONTROL_DEVICE_PATH              *FlowControl;
+  UINT32                                     Control;
+  PARENT_IO_PROTOCOL_PTR                     ParentIo;
+  ACPI_HID_DEVICE_PATH                       *Acpi;
+  EFI_GUID                                   *IoProtocolGuid;
+  PCI_SERIAL_PARAMETER                       *PciSerialParameter;
+  PCI_SERIAL_PARAMETER                       DefaultPciSerialParameter;
+  PCI_TYPE00                                 Pci;
+  UINT32                                     PciSerialCount;
+  SERIAL_DEV                                 **SerialDevices;
+  UINTN                                      SerialDeviceCount;
+  PCI_DEVICE_INFO                            *PciDeviceInfo;
+  UINT64                                     Supports;
+  BOOLEAN                                    ContainsControllerNode;
+
+  //
+  // Get the Parent Device Path
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+  //
+  // Report status code enable the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_ENABLE | EFI_PERIPHERAL_SERIAL_PORT,
+    ParentDevicePath
+    );
+
+  //
+  // Grab the IO abstraction we need to get any work done
+  //
+  IoProtocolGuid = &gEfiSioProtocolGuid;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  IoProtocolGuid,
+                  (VOID **) &ParentIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    IoProtocolGuid = &gEfiPciIoProtocolGuid;
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    IoProtocolGuid,
+                    (VOID **) &ParentIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  }
+  ASSERT (!EFI_ERROR (Status) || Status == EFI_ALREADY_STARTED);
+
+  //
+  // Do nothing for END device path node
+  //
+  if ((RemainingDevicePath != NULL) && IsDevicePathEnd (RemainingDevicePath)) {
+    return EFI_SUCCESS;
+  }
+
+  ControllerNumber = 0;
+  ContainsControllerNode = FALSE;
+  SerialDevices = GetChildSerialDevices (Controller, IoProtocolGuid, &SerialDeviceCount);
+  //
+  // If the SerialIo instance specified by RemainingDevicePath is already created,
+  // update the attributes/control.
+  //
+  if ((SerialDeviceCount != 0) && (RemainingDevicePath != NULL)) {
+    Uart = (UART_DEVICE_PATH *) SkipControllerDevicePathNode (RemainingDevicePath, &ContainsControllerNode, &ControllerNumber);
+    for (Index = 0; Index < SerialDeviceCount; Index++) {
+      ASSERT ((SerialDevices != NULL) && (SerialDevices[Index] != NULL));
+      if ((!SerialDevices[Index]->ContainsControllerNode && !ContainsControllerNode) ||
+          (SerialDevices[Index]->ContainsControllerNode && ContainsControllerNode && SerialDevices[Index]->Instance == ControllerNumber)
+          ) {
+        SerialIo = &SerialDevices[Index]->SerialIo;
+        Status = EFI_INVALID_PARAMETER;
+        //
+        // Pass NULL ActualBaudRate to VerifyUartParameters to disallow baudrate degrade.
+        // DriverBindingStart() shouldn't create a handle with different UART device path.
+        //
+        if (VerifyUartParameters (SerialDevices[Index]->ClockRate, Uart->BaudRate, Uart->DataBits,
+                                  (EFI_PARITY_TYPE) Uart->Parity, (EFI_STOP_BITS_TYPE) Uart->StopBits, NULL, NULL)) {
+          Status = SerialIo->SetAttributes (
+                               SerialIo,
+                               Uart->BaudRate,
+                               SerialIo->Mode->ReceiveFifoDepth,
+                               SerialIo->Mode->Timeout,
+                               (EFI_PARITY_TYPE) Uart->Parity,
+                               Uart->DataBits,
+                               (EFI_STOP_BITS_TYPE) Uart->StopBits
+                               );
+        }
+        FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) NextDevicePathNode (Uart);
+        if (!EFI_ERROR (Status) && IsUartFlowControlDevicePathNode (FlowControl)) {
+          Status = SerialIo->GetControl (SerialIo, &Control);
+          if (!EFI_ERROR (Status)) {
+            if (ReadUnaligned32 (&FlowControl->FlowControlMap) == UART_FLOW_CONTROL_HARDWARE) {
+              Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+            } else {
+              Control &= ~EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+            }
+            //
+            // Clear the bits that are not allowed to pass to SetControl
+            //
+            Control &= (EFI_SERIAL_REQUEST_TO_SEND | EFI_SERIAL_DATA_TERMINAL_READY |
+                        EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE | EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE |
+                        EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE);
+            Status = SerialIo->SetControl (SerialIo, Control);
+          }
+        }
+        break;
+      }
+    }
+    if (Index != SerialDeviceCount) {
+      //
+      // Directly return if the SerialIo instance specified by RemainingDevicePath is found and updated.
+      // Otherwise continue to create the instance specified by RemainingDevicePath.
+      //
+      if (SerialDevices != NULL) {
+        FreePool (SerialDevices);
+      }
+      return Status;
+    }
+  }
+
+  if (RemainingDevicePath != NULL) {
+    Uart = (UART_DEVICE_PATH *) SkipControllerDevicePathNode (RemainingDevicePath, &ContainsControllerNode, &ControllerNumber);
+  } else {
+    Uart = NULL;
+  }
+
+  PciDeviceInfo = NULL;
+  if (IoProtocolGuid == &gEfiSioProtocolGuid) {
+    Status = EFI_NOT_FOUND;
+    if (RemainingDevicePath == NULL || !ContainsControllerNode) {
+      Node = ParentDevicePath;
+      do {
+        Acpi = (ACPI_HID_DEVICE_PATH *) Node;
+        Node = NextDevicePathNode (Node);
+      } while (!IsDevicePathEnd (Node));
+      Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, FALSE, Acpi->UID, ParentIo, NULL, NULL);
+      DEBUG ((EFI_D_INFO, "PciSioSerial: Create SIO child serial device - %r\n", Status));
+    }
+  } else {
+    Status = ParentIo.PciIo->Pci.Read (ParentIo.PciIo, EfiPciIoWidthUint8, 0, sizeof (Pci), &Pci);
+    if (!EFI_ERROR (Status)) {
+      //
+      // PcdPciSerialParameters takes the higher priority.
+      //
+      PciSerialCount = 0;
+      for (PciSerialParameter = PcdGetPtr (PcdPciSerialParameters); PciSerialParameter->VendorId != 0xFFFF; PciSerialParameter++) {
+        if ((PciSerialParameter->VendorId == Pci.Hdr.VendorId) &&
+            (PciSerialParameter->DeviceId == Pci.Hdr.DeviceId)
+            ) {
+          PciSerialCount++;
+        }
+      }
+
+      if (SerialDeviceCount == 0) {
+        //
+        // Enable the IO & MEM decoding when creating the first child.
+        // Restore the PCI attributes when all children is destroyed (PciDeviceInfo->ChildCount == 0).
+        //
+        PciDeviceInfo = AllocatePool (sizeof (PCI_DEVICE_INFO));
+        ASSERT (PciDeviceInfo != NULL);
+        PciDeviceInfo->ChildCount = 0;
+        PciDeviceInfo->PciIo = ParentIo.PciIo;
+        Status = ParentIo.PciIo->Attributes (
+          ParentIo.PciIo,
+          EfiPciIoAttributeOperationGet,
+          0,
+          &PciDeviceInfo->PciAttributes
+          );
+
+        if (!EFI_ERROR (Status)) {
+          Status = ParentIo.PciIo->Attributes (
+            ParentIo.PciIo,
+            EfiPciIoAttributeOperationSupported,
+            0,
+            &Supports
+            );
+          if (!EFI_ERROR (Status)) {
+            Supports &= (UINT64)(EFI_PCI_IO_ATTRIBUTE_IO | EFI_PCI_IO_ATTRIBUTE_MEMORY);
+            Status = ParentIo.PciIo->Attributes (
+              ParentIo.PciIo,
+              EfiPciIoAttributeOperationEnable,
+              Supports,
+              NULL
+              );
+          }
+        }
+      } else {
+        //
+        // Re-use the PciDeviceInfo stored in existing children.
+        //
+        ASSERT ((SerialDevices != NULL) && (SerialDevices[0] != NULL));
+        PciDeviceInfo = SerialDevices[0]->PciDeviceInfo;
+        ASSERT (PciDeviceInfo != NULL);
+      }
+
+      Status = EFI_NOT_FOUND;
+      if (PciSerialCount <= 1) {
+        //
+        // PCI serial device contains only one UART
+        //
+        if (RemainingDevicePath == NULL || !ContainsControllerNode) {
+          //
+          // This PCI serial device is matched by class code in Supported()
+          //
+          if (PciSerialCount == 0) {
+            DefaultPciSerialParameter.VendorId = Pci.Hdr.VendorId;
+            DefaultPciSerialParameter.DeviceId = Pci.Hdr.DeviceId;
+            DefaultPciSerialParameter.BarIndex = 0;
+            DefaultPciSerialParameter.Offset = 0;
+            DefaultPciSerialParameter.RegisterStride = 0;
+            DefaultPciSerialParameter.ClockRate = 0;
+            PciSerialParameter = &DefaultPciSerialParameter;
+          } else if (PciSerialCount == 1) {
+            PciSerialParameter = PcdGetPtr (PcdPciSerialParameters);
+          }
+
+          Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, FALSE, 0, ParentIo, PciSerialParameter, PciDeviceInfo);
+          DEBUG ((EFI_D_INFO, "PciSioSerial: Create PCI child serial device (single) - %r\n", Status));
+          if (!EFI_ERROR (Status)) {
+            PciDeviceInfo->ChildCount++;
+          }
+        }
+      } else {
+        //
+        // PCI serial device contains multiple UARTs
+        //
+        if (RemainingDevicePath == NULL || ContainsControllerNode) {
+          PciSerialCount = 0;
+          for (PciSerialParameter = PcdGetPtr (PcdPciSerialParameters); PciSerialParameter->VendorId != 0xFFFF; PciSerialParameter++) {
+            if ((PciSerialParameter->VendorId == Pci.Hdr.VendorId) &&
+                (PciSerialParameter->DeviceId == Pci.Hdr.DeviceId) &&
+                ((RemainingDevicePath == NULL) || (ControllerNumber == PciSerialCount))
+                ) {
+              //
+              // Create controller node when PCI serial device contains multiple UARTs
+              //
+              Status = CreateSerialDevice (Controller, Uart, ParentDevicePath, TRUE, PciSerialCount, ParentIo, PciSerialParameter, PciDeviceInfo);
+              PciSerialCount++;
+              DEBUG ((EFI_D_INFO, "PciSioSerial: Create PCI child serial device (multiple) - %r\n", Status));
+              if (!EFI_ERROR (Status)) {
+                PciDeviceInfo->ChildCount++;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  if (SerialDevices != NULL) {
+    FreePool (SerialDevices);
+  }
+
+  //
+  // For multiple PCI serial devices, set Status to SUCCESS if one child is created successfully
+  //
+  if ((PciDeviceInfo != NULL) && (PciDeviceInfo->ChildCount != 0)) {
+    Status = EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status) && (SerialDeviceCount == 0)) {
+    if (PciDeviceInfo != NULL) {
+      Status = ParentIo.PciIo->Attributes (
+        ParentIo.PciIo,
+        EfiPciIoAttributeOperationSet,
+        PciDeviceInfo->PciAttributes,
+        NULL
+        );
+      ASSERT_EFI_ERROR (Status);
+      FreePool (PciDeviceInfo);
+    }
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    gBS->CloseProtocol (
+           Controller,
+           IoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  return Status;
+}
+
+/**
+  Disconnect this driver with the controller, uninstall related protocol instance
+
+  @param  This                  A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The handle of the controller to test.
+  @param  NumberOfChildren      Number of child device.
+  @param  ChildHandleBuffer     A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS           Operation successfully
+  @retval EFI_DEVICE_ERROR      Cannot stop the driver successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+
+{
+  EFI_STATUS                          Status;
+  UINTN                               Index;
+  BOOLEAN                             AllChildrenStopped;
+  EFI_SERIAL_IO_PROTOCOL              *SerialIo;
+  SERIAL_DEV                          *SerialDevice;
+  VOID                                *IoProtocol;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  PCI_DEVICE_INFO                     *PciDeviceInfo;
+
+  PciDeviceInfo = NULL;
+
+  Status = gBS->HandleProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath
+                  );
+
+  //
+  // Report the status code disable the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_DISABLE | EFI_PERIPHERAL_SERIAL_PORT,
+    DevicePath
+    );
+
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiPciIoProtocolGuid,
+                    &IoProtocol,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                    );
+    gBS->CloseProtocol (
+           Controller,
+           !EFI_ERROR (Status) ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiSerialIoProtocolGuid,
+                    (VOID **) &SerialIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+
+      SerialDevice = SERIAL_DEV_FROM_THIS (SerialIo);
+      ASSERT ((PciDeviceInfo == NULL) || (PciDeviceInfo == SerialDevice->PciDeviceInfo));
+      PciDeviceInfo = SerialDevice->PciDeviceInfo;
+
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      PciDeviceInfo != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ChildHandleBuffer[Index],
+                      &gEfiDevicePathProtocolGuid, SerialDevice->DevicePath,
+                      &gEfiSerialIoProtocolGuid,   &SerialDevice->SerialIo,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        gBS->OpenProtocol (
+               Controller,
+               PciDeviceInfo != NULL ? &gEfiPciIoProtocolGuid : &gEfiSioProtocolGuid,
+               &IoProtocol,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      } else {
+        FreePool (SerialDevice->DevicePath);
+        FreeUnicodeStringTable (SerialDevice->ControllerNameTable);
+        FreePool (SerialDevice);
+
+        if (PciDeviceInfo != NULL) {
+          ASSERT (PciDeviceInfo->ChildCount != 0);
+          PciDeviceInfo->ChildCount--;
+        }
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  } else {
+    //
+    // If all children are destroyed, restore the PCI attributes.
+    //
+    if ((PciDeviceInfo != NULL) && (PciDeviceInfo->ChildCount == 0)) {
+      ASSERT (PciDeviceInfo->PciIo != NULL);
+      Status = PciDeviceInfo->PciIo->Attributes (
+        PciDeviceInfo->PciIo,
+        EfiPciIoAttributeOperationSet,
+        PciDeviceInfo->PciAttributes,
+        NULL
+        );
+      ASSERT_EFI_ERROR (Status);
+      FreePool (PciDeviceInfo);
+    }
+    return EFI_SUCCESS;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h
new file mode 100644
index 0000000000..f147e69044
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/Serial.h
@@ -0,0 +1,789 @@
+/** @file
+  Header file for PciSioSerial Driver
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _SERIAL_H_
+#define _SERIAL_H_
+
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Protocol/SuperIo.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/SerialIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PcdLib.h>
+#include <Library/IoLib.h>
+#include <Library/PrintLib.h>
+
+//
+// Driver Binding Externs
+//
+extern EFI_DRIVER_BINDING_PROTOCOL  gSerialControllerDriver;
+extern EFI_COMPONENT_NAME_PROTOCOL  gPciSioSerialComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gPciSioSerialComponentName2;
+
+#define SIO_SERIAL_PORT_NAME L"SIO Serial Port #%d"
+#define PCI_SERIAL_PORT_NAME L"PCI Serial Port #%d"
+#define SERIAL_PORT_NAME_LEN (sizeof (SIO_SERIAL_PORT_NAME) / sizeof (CHAR16) + MAXIMUM_VALUE_CHARACTERS)
+
+//
+// Internal Data Structures
+//
+#define TIMEOUT_STALL_INTERVAL  10
+
+#pragma pack(1)
+///
+/// PcdPciSerialParameters contains zero or more instances of the below structure.
+/// If a PCI device contains multiple UARTs, PcdPciSerialParameters needs to contain
+/// two instances of the below structure, with the VendorId and DeviceId equals to the
+/// device ID and vendor ID of the device. If the PCI device uses the first two BARs
+/// to support multiple UARTs, BarIndex of first instance equals to 0 and BarIndex of
+/// second one equals to 1; if the PCI device uses the first BAR to support multiple
+/// UARTs, BarIndex of both instance equals to 0 and Offset of first instance equals
+/// to 0 while Offset of second one equals to some value bigger or equal to 8.
+/// For certain UART whose register needs to be accessed in DWORD aligned address,
+/// RegisterStride equals to 4.
+///
+typedef struct {
+  UINT16  VendorId;          ///< Vendor ID to match the PCI device.  The value 0xFFFF terminates the list of entries.
+  UINT16  DeviceId;          ///< Device ID to match the PCI device
+  UINT32  ClockRate;         ///< UART clock rate.  Set to 0 for default clock rate of 1843200 Hz
+  UINT64  Offset;            ///< The byte offset into to the BAR
+  UINT8   BarIndex;          ///< Which BAR to get the UART base address
+  UINT8   RegisterStride;    ///< UART register stride in bytes.  Set to 0 for default register stride of 1 byte.
+  UINT16  ReceiveFifoDepth;  ///< UART receive FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.
+  UINT16  TransmitFifoDepth; ///< UART transmit FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.
+  UINT8   Reserved[2];
+} PCI_SERIAL_PARAMETER;
+#pragma pack()
+
+#define SERIAL_MAX_FIFO_SIZE 17       ///< Actual FIFO size is 16. FIFO based on circular wastes one unit.
+typedef struct {
+  UINT16  Head;                       ///< Head pointer of the FIFO. Empty when (Head == Tail).
+  UINT16  Tail;                       ///< Tail pointer of the FIFO. Full when ((Tail + 1) % SERIAL_MAX_FIFO_SIZE == Head).
+  UINT8   Data[SERIAL_MAX_FIFO_SIZE]; ///< Store the FIFO data.
+} SERIAL_DEV_FIFO;
+
+typedef union {
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_SIO_PROTOCOL          *Sio;
+} PARENT_IO_PROTOCOL_PTR;
+
+typedef struct {
+  EFI_PCI_IO_PROTOCOL      *PciIo;        // Pointer to parent PciIo instance.
+  UINTN                    ChildCount;    // Count of child SerialIo instance.
+  UINT64                   PciAttributes; // Original PCI attributes.
+} PCI_DEVICE_INFO;
+
+typedef struct {
+  UINT32                   Signature;
+  EFI_HANDLE               Handle;
+  EFI_SERIAL_IO_PROTOCOL   SerialIo;
+  EFI_SERIAL_IO_MODE       SerialMode;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+
+  EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath;
+  UART_DEVICE_PATH         UartDevicePath;
+
+  EFI_PHYSICAL_ADDRESS     BaseAddress;       ///< UART base address
+  BOOLEAN                  MmioAccess;        ///< TRUE for MMIO, FALSE for IO
+  UINT8                    RegisterStride;    ///< UART Register Stride
+  UINT32                   ClockRate;         ///< UART clock rate
+
+  UINT16                   ReceiveFifoDepth;  ///< UART receive FIFO depth in bytes.
+  SERIAL_DEV_FIFO          Receive;           ///< The FIFO used to store received data
+
+  UINT16                   TransmitFifoDepth; ///< UART transmit FIFO depth in bytes.
+  SERIAL_DEV_FIFO          Transmit;          ///< The FIFO used to store to-transmit data
+
+  BOOLEAN                  SoftwareLoopbackEnable;
+  BOOLEAN                  HardwareFlowControl;
+  EFI_UNICODE_STRING_TABLE *ControllerNameTable;
+  BOOLEAN                  ContainsControllerNode; ///< TRUE if the device produced contains Controller node
+  UINT32                   Instance;
+  PCI_DEVICE_INFO          *PciDeviceInfo;
+} SERIAL_DEV;
+
+#define SERIAL_DEV_SIGNATURE    SIGNATURE_32 ('s', 'e', 'r', 'd')
+#define SERIAL_DEV_FROM_THIS(a) CR (a, SERIAL_DEV, SerialIo, SERIAL_DEV_SIGNATURE)
+
+//
+// Serial Driver Defaults
+//
+#define SERIAL_PORT_DEFAULT_TIMEOUT             1000000
+#define SERIAL_PORT_SUPPORT_CONTROL_MASK        (EFI_SERIAL_CLEAR_TO_SEND                | \
+                                                 EFI_SERIAL_DATA_SET_READY               | \
+                                                 EFI_SERIAL_RING_INDICATE                | \
+                                                 EFI_SERIAL_CARRIER_DETECT               | \
+                                                 EFI_SERIAL_REQUEST_TO_SEND              | \
+                                                 EFI_SERIAL_DATA_TERMINAL_READY          | \
+                                                 EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE     | \
+                                                 EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE     | \
+                                                 EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE | \
+                                                 EFI_SERIAL_OUTPUT_BUFFER_EMPTY          | \
+                                                 EFI_SERIAL_INPUT_BUFFER_EMPTY)
+
+#define SERIAL_PORT_MIN_TIMEOUT             1         // 1 uS
+#define SERIAL_PORT_MAX_TIMEOUT             100000000 // 100 seconds
+//
+// UART Registers
+//
+#define SERIAL_REGISTER_THR 0  ///< WO   Transmit Holding Register
+#define SERIAL_REGISTER_RBR 0  ///< RO   Receive Buffer Register
+#define SERIAL_REGISTER_DLL 0  ///< R/W  Divisor Latch LSB
+#define SERIAL_REGISTER_DLM 1  ///< R/W  Divisor Latch MSB
+#define SERIAL_REGISTER_IER 1  ///< R/W  Interrupt Enable Register
+#define SERIAL_REGISTER_IIR 2  ///< RO   Interrupt Identification Register
+#define SERIAL_REGISTER_FCR 2  ///< WO   FIFO Cotrol Register
+#define SERIAL_REGISTER_LCR 3  ///< R/W  Line Control Register
+#define SERIAL_REGISTER_MCR 4  ///< R/W  Modem Control Register
+#define SERIAL_REGISTER_LSR 5  ///< R/W  Line Status Register
+#define SERIAL_REGISTER_MSR 6  ///< R/W  Modem Status Register
+#define SERIAL_REGISTER_SCR 7  ///< R/W  Scratch Pad Register
+#pragma pack(1)
+
+///
+/// Interrupt Enable Register
+///
+typedef union {
+  struct {
+    UINT8 Ravie : 1;   ///< Receiver Data Available Interrupt Enable
+    UINT8 Theie : 1;   ///< Transmistter Holding Register Empty Interrupt Enable
+    UINT8 Rie : 1;     ///< Receiver Interrupt Enable
+    UINT8 Mie : 1;     ///< Modem Interrupt Enable
+    UINT8 Reserved : 4;
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_IER;
+
+///
+/// FIFO Control Register
+///
+typedef union {
+  struct {
+    UINT8 TrFIFOE : 1;   ///< Transmit and Receive FIFO Enable
+    UINT8 ResetRF : 1;   ///< Reset Reciever FIFO
+    UINT8 ResetTF : 1;   ///< Reset Transmistter FIFO
+    UINT8 Dms : 1;       ///< DMA Mode Select
+    UINT8 Reserved : 1;
+    UINT8 TrFIFO64 : 1;  ///< Enable 64 byte FIFO
+    UINT8 Rtb : 2;       ///< Receive Trigger Bits
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_FCR;
+
+///
+/// Line Control Register
+///
+typedef union {
+  struct {
+    UINT8 SerialDB : 2;   ///< Number of Serial Data Bits
+    UINT8 StopB : 1;      ///< Number of Stop Bits
+    UINT8 ParEn : 1;      ///< Parity Enable
+    UINT8 EvenPar : 1;    ///< Even Parity Select
+    UINT8 SticPar : 1;    ///< Sticky Parity
+    UINT8 BrCon : 1;      ///< Break Control
+    UINT8 DLab : 1;       ///< Divisor Latch Access Bit
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_LCR;
+
+///
+/// Modem Control Register
+///
+typedef union {
+  struct {
+    UINT8 DtrC : 1;  ///< Data Terminal Ready Control
+    UINT8 Rts : 1;   ///< Request To Send Control
+    UINT8 Out1 : 1;  ///< Output1
+    UINT8 Out2 : 1;  ///< Output2, used to disable interrupt
+    UINT8 Lme : 1;   ///< Loopback Mode Enable
+    UINT8 Reserved : 3;
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_MCR;
+
+///
+/// Line Status Register
+///
+typedef union {
+  struct {
+    UINT8 Dr : 1;     ///< Receiver Data Ready Status
+    UINT8 Oe : 1;     ///< Overrun Error Status
+    UINT8 Pe : 1;     ///< Parity Error Status
+    UINT8 Fe : 1;     ///< Framing Error Status
+    UINT8 Bi : 1;     ///< Break Interrupt Status
+    UINT8 Thre : 1;   ///< Transmistter Holding Register Status
+    UINT8 Temt : 1;   ///< Transmitter Empty Status
+    UINT8 FIFOe : 1;  ///< FIFO Error Status
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_LSR;
+
+///
+/// Modem Status Register
+///
+typedef union {
+  struct {
+    UINT8 DeltaCTS : 1;         ///< Delta Clear To Send Status
+    UINT8 DeltaDSR : 1;         ///< Delta Data Set Ready Status
+    UINT8 TrailingEdgeRI : 1;   ///< Trailing Edge of Ring Indicator Status
+    UINT8 DeltaDCD : 1;         ///< Delta Data Carrier Detect Status
+    UINT8 Cts : 1;              ///< Clear To Send Status
+    UINT8 Dsr : 1;              ///< Data Set Ready Status
+    UINT8 Ri : 1;               ///< Ring Indicator Status
+    UINT8 Dcd : 1;              ///< Data Carrier Detect Status
+  } Bits;
+  UINT8 Data;
+} SERIAL_PORT_MSR;
+
+#pragma pack()
+//
+// Define serial register I/O macros
+//
+#define READ_RBR(S)     SerialReadRegister (S, SERIAL_REGISTER_RBR)
+#define READ_DLL(S)     SerialReadRegister (S, SERIAL_REGISTER_DLL)
+#define READ_DLM(S)     SerialReadRegister (S, SERIAL_REGISTER_DLM)
+#define READ_IER(S)     SerialReadRegister (S, SERIAL_REGISTER_IER)
+#define READ_IIR(S)     SerialReadRegister (S, SERIAL_REGISTER_IIR)
+#define READ_LCR(S)     SerialReadRegister (S, SERIAL_REGISTER_LCR)
+#define READ_MCR(S)     SerialReadRegister (S, SERIAL_REGISTER_MCR)
+#define READ_LSR(S)     SerialReadRegister (S, SERIAL_REGISTER_LSR)
+#define READ_MSR(S)     SerialReadRegister (S, SERIAL_REGISTER_MSR)
+#define READ_SCR(S)     SerialReadRegister (S, SERIAL_REGISTER_SCR)
+
+#define WRITE_THR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_THR, D)
+#define WRITE_DLL(S, D) SerialWriteRegister (S, SERIAL_REGISTER_DLL, D)
+#define WRITE_DLM(S, D) SerialWriteRegister (S, SERIAL_REGISTER_DLM, D)
+#define WRITE_IER(S, D) SerialWriteRegister (S, SERIAL_REGISTER_IER, D)
+#define WRITE_FCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_FCR, D)
+#define WRITE_LCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_LCR, D)
+#define WRITE_MCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_MCR, D)
+#define WRITE_LSR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_LSR, D)
+#define WRITE_MSR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_MSR, D)
+#define WRITE_SCR(S, D) SerialWriteRegister (S, SERIAL_REGISTER_SCR, D)
+
+//
+// Prototypes
+// Driver model protocol interface
+//
+/**
+  Check to see if this driver supports the given controller
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          This driver can support the given controller
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start to management the controller passed in
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller           The handle of the controller to test.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path.
+
+  @return EFI_SUCCESS          Driver is started successfully
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Disconnect this driver with the controller, uninstall related protocol instance
+
+  @param  This                  A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  Controller            The handle of the controller to test.
+  @param  NumberOfChildren      Number of child device.
+  @param  ChildHandleBuffer     A pointer to the remaining portion of a device path.
+
+  @retval EFI_SUCCESS           Operation successfully
+  @retval EFI_DEVICE_ERROR      Cannot stop the driver successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// Serial I/O Protocol Interface
+//
+/**
+  Reset serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+
+  @retval EFI_SUCCESS        Reset successfully
+  @retval EFI_DEVICE_ERROR   Failed to reset
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL         *This
+  );
+
+/**
+  Set new attributes to a serial device.
+
+  @param This                     Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BaudRate                 The baudrate of the serial device
+  @param  ReceiveFifoDepth         The depth of receive FIFO buffer
+  @param  Timeout                  The request timeout for a single char
+  @param  Parity                   The type of parity used in serial device
+  @param  DataBits                 Number of databits used in serial device
+  @param  StopBits                 Number of stopbits used in serial device
+
+  @retval  EFI_SUCCESS              The new attributes were set
+  @retval  EFI_INVALID_PARAMETERS   One or more attributes have an unsupported value
+  @retval  EFI_UNSUPPORTED          Data Bits can not set to 5 or 6
+  @retval  EFI_DEVICE_ERROR         The serial device is not functioning correctly (no return)
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN UINT64                         BaudRate,
+  IN UINT32                         ReceiveFifoDepth,
+  IN UINT32                         Timeout,
+  IN EFI_PARITY_TYPE                Parity,
+  IN UINT8                          DataBits,
+  IN EFI_STOP_BITS_TYPE             StopBits
+  );
+
+/**
+  Set Control Bits.
+
+  @param This              Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control           Control bits that can be settable
+
+  @retval EFI_SUCCESS       New Control bits were set successfully
+  @retval EFI_UNSUPPORTED   The Control bits wanted to set are not supported
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN UINT32                         Control
+  );
+
+/**
+  Get ControlBits.
+
+  @param This          Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control       Control signals of the serial device
+
+  @retval EFI_SUCCESS   Get Control signals successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  OUT UINT32                        *Control
+  );
+
+/**
+  Write the specified number of bytes to serial device.
+
+  @param This                Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BufferSize         On input the size of Buffer, on output the amount of
+                             data actually written
+  @param  Buffer             The buffer of data to write
+
+  @retval EFI_SUCCESS        The data were written successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The write operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN OUT UINTN                      *BufferSize,
+  IN VOID                           *Buffer
+  );
+
+/**
+  Read the specified number of bytes from serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param BufferSize         On input the size of Buffer, on output the amount of
+                            data returned in buffer
+  @param Buffer             The buffer to return the data into
+
+  @retval EFI_SUCCESS        The data were read successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The read operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL         *This,
+  IN OUT UINTN                      *BufferSize,
+  OUT VOID                          *Buffer
+  );
+
+//
+// Internal Functions
+//
+/**
+  Use scratchpad register to test if this serial port is present.
+
+  @param SerialDevice   Pointer to serial device structure
+
+  @return if this serial port is present
+**/
+BOOLEAN
+SerialPresent (
+  IN SERIAL_DEV                     *SerialDevice
+  );
+
+/**
+  Detect whether specific FIFO is full or not.
+
+  @param Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is full or not
+
+**/
+BOOLEAN
+SerialFifoFull (
+  IN SERIAL_DEV_FIFO                *Fifo
+  );
+
+/**
+  Detect whether specific FIFO is empty or not.
+ 
+  @param  Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is empty or not
+
+**/
+BOOLEAN
+SerialFifoEmpty (
+  IN SERIAL_DEV_FIFO                *Fifo
+  );
+
+/**
+  Add data to specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data added to FIFO
+
+  @retval EFI_SUCCESS           Add data to specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to add data because FIFO is already full
+
+**/
+EFI_STATUS
+SerialFifoAdd (
+  IN SERIAL_DEV_FIFO                *Fifo,
+  IN UINT8                          Data
+  );
+
+/**
+  Remove data from specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data removed from FIFO
+
+  @retval EFI_SUCCESS           Remove data from specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to remove data because FIFO is empty
+
+**/
+EFI_STATUS
+SerialFifoRemove (
+  IN  SERIAL_DEV_FIFO               *Fifo,
+  OUT UINT8                         *Data
+  );
+
+/**
+  Reads and writes all avaliable data.
+
+  @param SerialDevice           The device to flush
+
+  @retval EFI_SUCCESS           Data was read/written successfully.
+  @retval EFI_OUT_OF_RESOURCE   Failed because software receive FIFO is full.  Note, when
+                                this happens, pending writes are not done.
+
+**/
+EFI_STATUS
+SerialReceiveTransmit (
+  IN SERIAL_DEV                     *SerialDevice
+  );
+
+/**
+  Read serial port.
+
+  @param SerialDev     Pointer to serial device
+  @param Offset        Offset in register group
+
+  @return Data read from serial port
+**/
+UINT8
+SerialReadRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset
+  );
+
+/**
+  Write serial port.
+
+  @param  SerialDev     Pointer to serial device
+  @param  Offset        Offset in register group
+  @param  Data          data which is to be written to some serial port register
+**/
+VOID
+SerialWriteRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset,
+  IN UINT8                                 Data
+  );
+
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Add the component name for the serial io device
+
+  @param SerialDevice     A pointer to the SERIAL_DEV instance.
+  @param Uid              Unique ID for the serial device.
+**/
+VOID
+AddName (
+  IN  SERIAL_DEV                               *SerialDevice,
+  IN  UINT32                                   Uid
+  );
+
+/**
+  Checks whether the UART parameters are valid and computes the Divisor.
+
+  @param  ClockRate      The clock rate of the serial device used to verify
+                         the BaudRate. Do not verify the BaudRate if it's 0.
+  @param  BaudRate       The requested baudrate of the serial device.
+  @param  DataBits       Number of databits used in serial device.
+  @param  Parity         The type of parity used in serial device.
+  @param  StopBits       Number of stopbits used in serial device.
+  @param  Divisor        Return the divisor if ClockRate is not 0.
+  @param  ActualBaudRate Return the actual supported baudrate without
+                         exceeding BaudRate. NULL means baudrate degradation
+                         is not allowed.
+                         If the requested BaudRate is not supported, the routine
+                         returns TRUE and the Actual Baud Rate when ActualBaudRate
+                         is not NULL, returns FALSE when ActualBaudRate is NULL.
+
+  @retval TRUE   The UART parameters are valid.
+  @retval FALSE  The UART parameters are not valid.
+**/
+BOOLEAN
+VerifyUartParameters (
+  IN     UINT32                  ClockRate,
+  IN     UINT64                  BaudRate,
+  IN     UINT8                   DataBits,
+  IN     EFI_PARITY_TYPE         Parity,
+  IN     EFI_STOP_BITS_TYPE      StopBits,
+     OUT UINT64                  *Divisor,
+     OUT UINT64                  *ActualBaudRate
+  );
+
+/**
+  Skip the optional Controller device path node and return the
+  pointer to the next device path node.
+
+  @param DevicePath             Pointer to the device path.
+  @param ContainsControllerNode Returns TRUE if the Controller device path exists.
+  @param ControllerNumber       Returns the Controller Number if Controller device path exists.
+
+  @return     Pointer to the next device path node.
+**/
+UART_DEVICE_PATH *
+SkipControllerDevicePathNode (
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath,
+  BOOLEAN                           *ContainsControllerNode,
+  UINT32                            *ControllerNumber
+  );
+
+/**
+  Check the device path node whether it's the Flow Control node or not.
+
+  @param[in] FlowControl    The device path node to be checked.
+  
+  @retval TRUE              It's the Flow Control node.
+  @retval FALSE             It's not.
+
+**/
+BOOLEAN
+IsUartFlowControlDevicePathNode (
+  IN UART_FLOW_CONTROL_DEVICE_PATH *FlowControl
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c
new file mode 100644
index 0000000000..f1870f3a1b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/PciSioSerialDxe/SerialIo.c
@@ -0,0 +1,1320 @@
+/** @file
+  SerialIo implementation for PCI or SIO UARTs.
+
+Copyright (c) 2006 - 2015, 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 "Serial.h"
+
+/**
+  Skip the optional Controller device path node and return the
+  pointer to the next device path node.
+
+  @param DevicePath             Pointer to the device path.
+  @param ContainsControllerNode Returns TRUE if the Controller device path exists.
+  @param ControllerNumber       Returns the Controller Number if Controller device path exists.
+
+  @return     Pointer to the next device path node.
+**/
+UART_DEVICE_PATH *
+SkipControllerDevicePathNode (
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath,
+  BOOLEAN                           *ContainsControllerNode,
+  UINT32                            *ControllerNumber
+  )
+{
+  if ((DevicePathType (DevicePath) == HARDWARE_DEVICE_PATH) &&
+      (DevicePathSubType (DevicePath) == HW_CONTROLLER_DP)
+      ) {
+    if (ContainsControllerNode != NULL) {
+      *ContainsControllerNode = TRUE;
+    }
+    if (ControllerNumber != NULL) {
+      *ControllerNumber = ((CONTROLLER_DEVICE_PATH *) DevicePath)->ControllerNumber;
+    }
+    DevicePath = NextDevicePathNode (DevicePath);
+  } else {
+    if (ContainsControllerNode != NULL) {
+      *ContainsControllerNode = FALSE;
+    }
+  }
+  return (UART_DEVICE_PATH *) DevicePath;
+}
+
+/**
+  Checks whether the UART parameters are valid and computes the Divisor.
+
+  @param  ClockRate      The clock rate of the serial device used to verify
+                         the BaudRate. Do not verify the BaudRate if it's 0.
+  @param  BaudRate       The requested baudrate of the serial device.
+  @param  DataBits       Number of databits used in serial device.
+  @param  Parity         The type of parity used in serial device.
+  @param  StopBits       Number of stopbits used in serial device.
+  @param  Divisor        Return the divisor if ClockRate is not 0.
+  @param  ActualBaudRate Return the actual supported baudrate without
+                         exceeding BaudRate. NULL means baudrate degradation
+                         is not allowed.
+                         If the requested BaudRate is not supported, the routine
+                         returns TRUE and the Actual Baud Rate when ActualBaudRate
+                         is not NULL, returns FALSE when ActualBaudRate is NULL.
+
+  @retval TRUE   The UART parameters are valid.
+  @retval FALSE  The UART parameters are not valid.
+**/
+BOOLEAN
+VerifyUartParameters (
+  IN     UINT32                  ClockRate,
+  IN     UINT64                  BaudRate,
+  IN     UINT8                   DataBits,
+  IN     EFI_PARITY_TYPE         Parity,
+  IN     EFI_STOP_BITS_TYPE      StopBits,
+     OUT UINT64                  *Divisor,
+     OUT UINT64                  *ActualBaudRate
+  )
+{
+  UINT64                     Remainder;
+  UINT32                     ComputedBaudRate;
+  UINT64                     ComputedDivisor;
+  UINT64                     Percent;
+
+  if ((DataBits < 5) || (DataBits > 8) ||
+      (Parity < NoParity) || (Parity > SpaceParity) ||
+      (StopBits < OneStopBit) || (StopBits > TwoStopBits) ||
+      ((DataBits == 5) && (StopBits == TwoStopBits)) ||
+      ((DataBits >= 6) && (DataBits <= 8) && (StopBits == OneFiveStopBits))
+      ) {
+    return FALSE;
+  } 
+
+  //
+  // Do not verify the baud rate if clock rate is unknown (0).
+  //
+  if (ClockRate == 0) {
+    return TRUE;
+  }
+
+  //
+  // Compute divisor use to program the baud rate using a round determination
+  // Divisor = ClockRate / 16 / BaudRate = ClockRate / (16 * BaudRate)
+  //         = ClockRate / (BaudRate << 4)
+  //
+  ComputedDivisor = DivU64x64Remainder (ClockRate, LShiftU64 (BaudRate, 4), &Remainder);
+  //
+  // Round Divisor up by 1 if the Remainder is more than half (16 * BaudRate)
+  // BaudRate * 16 / 2 = BaudRate * 8 = (BaudRate << 3)
+  //
+  if (Remainder >= LShiftU64 (BaudRate, 3)) {
+    ComputedDivisor++;
+  }
+  //
+  // If the computed divisor is larger than the maximum value that can be programmed
+  // into the UART, then the requested baud rate can not be supported.
+  //
+  if (ComputedDivisor > MAX_UINT16) {
+    return FALSE;
+  }
+
+  //
+  // If the computed divisor is 0, then use a computed divisor of 1, which will select
+  // the maximum supported baud rate.
+  //
+  if (ComputedDivisor == 0) {
+    ComputedDivisor = 1;
+  }
+
+  //
+  // Actual baud rate that the serial port will be programmed for
+  // should be with in 4% of requested one.
+  //
+  ComputedBaudRate = ClockRate / ((UINT16) ComputedDivisor << 4);
+  if (ComputedBaudRate == 0) {
+    return FALSE;
+  }
+
+  Percent = DivU64x32 (MultU64x32 (BaudRate, 100), ComputedBaudRate);
+  DEBUG ((EFI_D_INFO, "ClockRate = %d\n",  ClockRate));
+  DEBUG ((EFI_D_INFO, "Divisor   = %ld\n", ComputedDivisor));
+  DEBUG ((EFI_D_INFO, "BaudRate/Actual (%ld/%d) = %d%%\n", BaudRate, ComputedBaudRate, Percent));
+
+  //
+  // If the requested BaudRate is not supported:
+  //  Returns TRUE and the Actual Baud Rate when ActualBaudRate is not NULL;
+  //  Returns FALSE when ActualBaudRate is NULL.
+  //
+  if ((Percent >= 96) && (Percent <= 104)) {
+    if (ActualBaudRate != NULL) {
+      *ActualBaudRate = BaudRate;
+    }
+    if (Divisor != NULL) {
+      *Divisor = ComputedDivisor;
+    }
+    return TRUE;
+  }
+  if (ComputedBaudRate < BaudRate) {
+    if (ActualBaudRate != NULL) {
+      *ActualBaudRate = ComputedBaudRate;
+    }
+    if (Divisor != NULL) {
+      *Divisor = ComputedDivisor;
+    }
+    return TRUE;
+  }
+
+  //
+  // ActualBaudRate is higher than requested baud rate and more than 4% 
+  // higher than the requested value.  Increment Divisor if it is less 
+  // than MAX_UINT16 and computed baud rate with new divisor.
+  //
+  if (ComputedDivisor == MAX_UINT16) {
+    return FALSE;
+  }
+  ComputedDivisor++;
+  ComputedBaudRate = ClockRate / ((UINT16) ComputedDivisor << 4);
+  if (ComputedBaudRate == 0) {
+    return FALSE;
+  }
+
+  DEBUG ((EFI_D_INFO, "ClockRate = %d\n",  ClockRate));
+  DEBUG ((EFI_D_INFO, "Divisor   = %ld\n", ComputedDivisor));
+  DEBUG ((EFI_D_INFO, "BaudRate/Actual (%ld/%d) = %d%%\n", BaudRate, ComputedBaudRate, Percent));
+
+  if (ActualBaudRate != NULL) {
+    *ActualBaudRate = ComputedBaudRate;
+  }
+  if (Divisor != NULL) {
+    *Divisor = ComputedDivisor;
+  }
+  return TRUE;
+}
+
+/**
+  Detect whether specific FIFO is full or not.
+
+  @param Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is full or not
+**/
+BOOLEAN
+SerialFifoFull (
+  IN SERIAL_DEV_FIFO *Fifo
+  )
+{
+  return (BOOLEAN) (((Fifo->Tail + 1) % SERIAL_MAX_FIFO_SIZE) == Fifo->Head);
+}
+
+/**
+  Detect whether specific FIFO is empty or not.
+ 
+  @param  Fifo    A pointer to the Data Structure SERIAL_DEV_FIFO
+
+  @return whether specific FIFO is empty or not
+**/
+BOOLEAN
+SerialFifoEmpty (
+  IN SERIAL_DEV_FIFO *Fifo
+  )
+
+{
+  return (BOOLEAN) (Fifo->Head == Fifo->Tail);
+}
+
+/**
+  Add data to specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data added to FIFO
+
+  @retval EFI_SUCCESS           Add data to specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to add data because FIFO is already full
+**/
+EFI_STATUS
+SerialFifoAdd (
+  IN OUT SERIAL_DEV_FIFO *Fifo,
+  IN     UINT8           Data
+  )
+{
+  //
+  // if FIFO full can not add data
+  //
+  if (SerialFifoFull (Fifo)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // FIFO is not full can add data
+  //
+  Fifo->Data[Fifo->Tail] = Data;
+  Fifo->Tail = (Fifo->Tail + 1) % SERIAL_MAX_FIFO_SIZE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove data from specific FIFO.
+
+  @param Fifo                  A pointer to the Data Structure SERIAL_DEV_FIFO
+  @param Data                  the data removed from FIFO
+
+  @retval EFI_SUCCESS           Remove data from specific FIFO successfully
+  @retval EFI_OUT_OF_RESOURCE   Failed to remove data because FIFO is empty
+
+**/
+EFI_STATUS
+SerialFifoRemove (
+  IN OUT SERIAL_DEV_FIFO *Fifo,
+  OUT    UINT8           *Data
+  )
+{
+  //
+  // if FIFO is empty, no data can remove
+  //
+  if (SerialFifoEmpty (Fifo)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // FIFO is not empty, can remove data
+  //
+  *Data = Fifo->Data[Fifo->Head];
+  Fifo->Head = (Fifo->Head + 1) % SERIAL_MAX_FIFO_SIZE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads and writes all avaliable data.
+
+  @param SerialDevice           The device to transmit.
+
+  @retval EFI_SUCCESS           Data was read/written successfully.
+  @retval EFI_OUT_OF_RESOURCE   Failed because software receive FIFO is full.  Note, when
+                                this happens, pending writes are not done.
+
+**/
+EFI_STATUS
+SerialReceiveTransmit (
+  IN SERIAL_DEV *SerialDevice
+  )
+
+{
+  SERIAL_PORT_LSR Lsr;
+  UINT8           Data;
+  BOOLEAN         ReceiveFifoFull;
+  SERIAL_PORT_MSR Msr;
+  SERIAL_PORT_MCR Mcr;
+  UINTN           TimeOut;
+
+  Data = 0;
+
+  //
+  // Begin the read or write
+  //
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    do {
+      ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+      if (!SerialFifoEmpty (&SerialDevice->Transmit)) {
+        SerialFifoRemove (&SerialDevice->Transmit, &Data);
+        if (ReceiveFifoFull) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        SerialFifoAdd (&SerialDevice->Receive, Data);
+      }
+    } while (!SerialFifoEmpty (&SerialDevice->Transmit));
+  } else {
+    ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+    //
+    // For full handshake flow control, tell the peer to send data
+    // if receive buffer is available.
+    //
+    if (SerialDevice->HardwareFlowControl &&
+        !FeaturePcdGet(PcdSerialUseHalfHandshake)&&
+        !ReceiveFifoFull
+        ) {
+      Mcr.Data     = READ_MCR (SerialDevice);
+      Mcr.Bits.Rts = 1;
+      WRITE_MCR (SerialDevice, Mcr.Data);
+    }
+    do {
+      Lsr.Data = READ_LSR (SerialDevice);
+
+      //
+      // Flush incomming data to prevent a an overrun during a long write
+      //
+      if ((Lsr.Bits.Dr == 1) && !ReceiveFifoFull) {
+        ReceiveFifoFull = SerialFifoFull (&SerialDevice->Receive);
+        if (!ReceiveFifoFull) {
+          if (Lsr.Bits.FIFOe == 1 || Lsr.Bits.Oe == 1 || Lsr.Bits.Pe == 1 || Lsr.Bits.Fe == 1 || Lsr.Bits.Bi == 1) {
+            REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+              EFI_ERROR_CODE,
+              EFI_P_EC_INPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+              SerialDevice->DevicePath
+              );
+            if (Lsr.Bits.FIFOe == 1 || Lsr.Bits.Pe == 1|| Lsr.Bits.Fe == 1 || Lsr.Bits.Bi == 1) {
+              Data = READ_RBR (SerialDevice);
+              continue;
+            }
+          }
+
+          Data = READ_RBR (SerialDevice);
+
+          SerialFifoAdd (&SerialDevice->Receive, Data);
+          
+          //
+          // For full handshake flow control, if receive buffer full
+          // tell the peer to stop sending data.
+          //
+          if (SerialDevice->HardwareFlowControl &&
+              !FeaturePcdGet(PcdSerialUseHalfHandshake)   &&
+              SerialFifoFull (&SerialDevice->Receive)
+              ) {
+            Mcr.Data     = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts = 0;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+
+
+          continue;
+        } else {
+          REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+            EFI_PROGRESS_CODE,
+            EFI_P_SERIAL_PORT_PC_CLEAR_BUFFER | EFI_PERIPHERAL_SERIAL_PORT,
+            SerialDevice->DevicePath
+            );
+        }
+      }
+      //
+      // Do the write
+      //
+      if (Lsr.Bits.Thre == 1 && !SerialFifoEmpty (&SerialDevice->Transmit)) {
+        //
+        // Make sure the transmit data will not be missed
+        //
+        if (SerialDevice->HardwareFlowControl) {
+          //
+          // For half handshake flow control assert RTS before sending.
+          //
+          if (FeaturePcdGet(PcdSerialUseHalfHandshake)) {
+            Mcr.Data     = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts= 0;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+          //
+          // Wait for CTS
+          //
+          TimeOut   = 0;
+          Msr.Data  = READ_MSR (SerialDevice);
+          while ((Msr.Bits.Dcd == 1) && ((Msr.Bits.Cts == 0) ^ FeaturePcdGet(PcdSerialUseHalfHandshake))) {
+            gBS->Stall (TIMEOUT_STALL_INTERVAL);
+            TimeOut++;
+            if (TimeOut > 5) {
+              break;
+            }
+
+            Msr.Data = READ_MSR (SerialDevice);
+          }
+
+          if ((Msr.Bits.Dcd == 0) || ((Msr.Bits.Cts == 1) ^ FeaturePcdGet(PcdSerialUseHalfHandshake))) {
+            SerialFifoRemove (&SerialDevice->Transmit, &Data);
+            WRITE_THR (SerialDevice, Data);
+          }
+
+          //
+          // For half handshake flow control, tell DCE we are done.
+          //
+          if (FeaturePcdGet(PcdSerialUseHalfHandshake)) {
+            Mcr.Data = READ_MCR (SerialDevice);
+            Mcr.Bits.Rts = 1;
+            WRITE_MCR (SerialDevice, Mcr.Data);
+          }
+        } else {
+          SerialFifoRemove (&SerialDevice->Transmit, &Data);
+          WRITE_THR (SerialDevice, Data);
+        }
+      }
+    } while (Lsr.Bits.Thre == 1 && !SerialFifoEmpty (&SerialDevice->Transmit));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Flush the serial hardware transmit FIFO and shift register.
+
+  @param SerialDevice  The device to flush.
+**/
+VOID
+SerialFlushTransmitFifo (
+  SERIAL_DEV  *SerialDevice
+  )
+{
+  SERIAL_PORT_LSR  Lsr;
+
+  //
+  // Wait for the serial port to be ready, to make sure both the transmit FIFO
+  // and shift register empty.
+  //
+  do {
+    Lsr.Data = READ_LSR (SerialDevice);
+  } while (Lsr.Bits.Temt == 0);
+}
+
+//
+// Interface Functions
+//
+/**
+  Reset serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+
+  @retval EFI_SUCCESS        Reset successfully
+  @retval EFI_DEVICE_ERROR   Failed to reset
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL  *This
+  )
+{
+  EFI_STATUS      Status;
+  SERIAL_DEV      *SerialDevice;
+  SERIAL_PORT_LCR Lcr;
+  SERIAL_PORT_IER Ier;
+  SERIAL_PORT_MCR Mcr;
+  SERIAL_PORT_FCR Fcr;
+  EFI_TPL         Tpl;
+  UINT32          Control;
+
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // Report the status code reset the serial
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    EFI_P_PC_RESET | EFI_PERIPHERAL_SERIAL_PORT,
+    SerialDevice->DevicePath
+    );
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialFlushTransmitFifo (SerialDevice);
+
+  //
+  // Make sure DLAB is 0.
+  //
+  Lcr.Data      = READ_LCR (SerialDevice);
+  Lcr.Bits.DLab = 0;
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Turn off all interrupts
+  //
+  Ier.Data        = READ_IER (SerialDevice);
+  Ier.Bits.Ravie  = 0;
+  Ier.Bits.Theie  = 0;
+  Ier.Bits.Rie    = 0;
+  Ier.Bits.Mie    = 0;
+  WRITE_IER (SerialDevice, Ier.Data);
+
+  //
+  // Reset the FIFO
+  //
+  Fcr.Data = 0;
+  Fcr.Bits.TrFIFOE = 0;
+  WRITE_FCR (SerialDevice, Fcr.Data);
+
+  //
+  // Turn off loopback and disable device interrupt.
+  //
+  Mcr.Data      = READ_MCR (SerialDevice);
+  Mcr.Bits.Out1 = 0;
+  Mcr.Bits.Out2 = 0;
+  Mcr.Bits.Lme  = 0;
+  WRITE_MCR (SerialDevice, Mcr.Data);
+
+  //
+  // Clear the scratch pad register
+  //
+  WRITE_SCR (SerialDevice, 0);
+
+  //
+  // Enable FIFO
+  //
+  Fcr.Bits.TrFIFOE  = 1;
+  if (SerialDevice->ReceiveFifoDepth > 16) {
+    Fcr.Bits.TrFIFO64 = 1;
+  }
+  Fcr.Bits.ResetRF  = 1;
+  Fcr.Bits.ResetTF  = 1;
+  WRITE_FCR (SerialDevice, Fcr.Data);
+
+  //
+  // Go set the current attributes
+  //
+  Status = This->SetAttributes (
+                   This,
+                   This->Mode->BaudRate,
+                   This->Mode->ReceiveFifoDepth,
+                   This->Mode->Timeout,
+                   (EFI_PARITY_TYPE) This->Mode->Parity,
+                   (UINT8) This->Mode->DataBits,
+                   (EFI_STOP_BITS_TYPE) This->Mode->StopBits
+                   );
+
+  if (EFI_ERROR (Status)) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Go set the current control bits
+  //
+  Control = 0;
+  if (SerialDevice->HardwareFlowControl) {
+    Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+  }
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    Control |= EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE;
+  }
+  Status = This->SetControl (
+                   This,
+                   Control
+                   );
+
+  if (EFI_ERROR (Status)) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Reset the software FIFO
+  //
+  SerialDevice->Receive.Head = SerialDevice->Receive.Tail = 0;
+  SerialDevice->Transmit.Head = SerialDevice->Transmit.Tail = 0;
+  gBS->RestoreTPL (Tpl);
+
+  //
+  // Device reset is complete
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Set new attributes to a serial device.
+
+  @param This                     Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BaudRate                 The baudrate of the serial device
+  @param  ReceiveFifoDepth         The depth of receive FIFO buffer
+  @param  Timeout                  The request timeout for a single char
+  @param  Parity                   The type of parity used in serial device
+  @param  DataBits                 Number of databits used in serial device
+  @param  StopBits                 Number of stopbits used in serial device
+
+  @retval  EFI_SUCCESS              The new attributes were set
+  @retval  EFI_INVALID_PARAMETERS   One or more attributes have an unsupported value
+  @retval  EFI_UNSUPPORTED          Data Bits can not set to 5 or 6
+  @retval  EFI_DEVICE_ERROR         The serial device is not functioning correctly (no return)
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN UINT64                  BaudRate,
+  IN UINT32                  ReceiveFifoDepth,
+  IN UINT32                  Timeout,
+  IN EFI_PARITY_TYPE         Parity,
+  IN UINT8                   DataBits,
+  IN EFI_STOP_BITS_TYPE      StopBits
+  )
+{
+  EFI_STATUS                Status;
+  SERIAL_DEV                *SerialDevice;
+  UINT64                    Divisor;
+  SERIAL_PORT_LCR           Lcr;
+  UART_DEVICE_PATH          *Uart;
+  EFI_TPL                   Tpl;
+
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // Check for default settings and fill in actual values.
+  //
+  if (BaudRate == 0) {
+    BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  }
+
+  if (ReceiveFifoDepth == 0) {
+    ReceiveFifoDepth = SerialDevice->ReceiveFifoDepth;
+  }
+
+  if (Timeout == 0) {
+    Timeout = SERIAL_PORT_DEFAULT_TIMEOUT;
+  }
+
+  if (Parity == DefaultParity) {
+    Parity = (EFI_PARITY_TYPE) PcdGet8 (PcdUartDefaultParity);
+  }
+
+  if (DataBits == 0) {
+    DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  }
+
+  if (StopBits == DefaultStopBits) {
+    StopBits = (EFI_STOP_BITS_TYPE) PcdGet8 (PcdUartDefaultStopBits);
+  }
+
+  if (!VerifyUartParameters (SerialDevice->ClockRate, BaudRate, DataBits, Parity, StopBits, &Divisor, &BaudRate)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((ReceiveFifoDepth == 0) || (ReceiveFifoDepth > SerialDevice->ReceiveFifoDepth)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Timeout < SERIAL_PORT_MIN_TIMEOUT) || (Timeout > SERIAL_PORT_MAX_TIMEOUT)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialFlushTransmitFifo (SerialDevice);
+
+  //
+  // Put serial port on Divisor Latch Mode
+  //
+  Lcr.Data      = READ_LCR (SerialDevice);
+  Lcr.Bits.DLab = 1;
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Write the divisor to the serial port
+  //
+  WRITE_DLL (SerialDevice, (UINT8) Divisor);
+  WRITE_DLM (SerialDevice, (UINT8) ((UINT16) Divisor >> 8));
+
+  //
+  // Put serial port back in normal mode and set remaining attributes.
+  //
+  Lcr.Bits.DLab = 0;
+
+  switch (Parity) {
+  case NoParity:
+    Lcr.Bits.ParEn    = 0;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case EvenParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 1;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case OddParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 0;
+    break;
+
+  case SpaceParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 1;
+    Lcr.Bits.SticPar  = 1;
+    break;
+
+  case MarkParity:
+    Lcr.Bits.ParEn    = 1;
+    Lcr.Bits.EvenPar  = 0;
+    Lcr.Bits.SticPar  = 1;
+    break;
+
+  default:
+    break;
+  }
+
+  switch (StopBits) {
+  case OneStopBit:
+    Lcr.Bits.StopB = 0;
+    break;
+
+  case OneFiveStopBits:
+  case TwoStopBits:
+    Lcr.Bits.StopB = 1;
+    break;
+
+  default:
+    break;
+  }
+  //
+  // DataBits
+  //
+  Lcr.Bits.SerialDB = (UINT8) ((DataBits - 5) & 0x03);
+  WRITE_LCR (SerialDevice, Lcr.Data);
+
+  //
+  // Set the Serial I/O mode
+  //
+  This->Mode->BaudRate          = BaudRate;
+  This->Mode->ReceiveFifoDepth  = ReceiveFifoDepth;
+  This->Mode->Timeout           = Timeout;
+  This->Mode->Parity            = Parity;
+  This->Mode->DataBits          = DataBits;
+  This->Mode->StopBits          = StopBits;
+
+  //
+  // See if Device Path Node has actually changed
+  //
+  if (SerialDevice->UartDevicePath.BaudRate == BaudRate &&
+      SerialDevice->UartDevicePath.DataBits == DataBits &&
+      SerialDevice->UartDevicePath.Parity == Parity &&
+      SerialDevice->UartDevicePath.StopBits == StopBits
+      ) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_SUCCESS;
+  }
+  //
+  // Update the device path
+  //
+  SerialDevice->UartDevicePath.BaudRate = BaudRate;
+  SerialDevice->UartDevicePath.DataBits = DataBits;
+  SerialDevice->UartDevicePath.Parity   = (UINT8) Parity;
+  SerialDevice->UartDevicePath.StopBits = (UINT8) StopBits;
+
+  Status = EFI_SUCCESS;
+  if (SerialDevice->Handle != NULL) {
+
+    //
+    // Skip the optional Controller device path node
+    //
+    Uart = SkipControllerDevicePathNode (
+             (EFI_DEVICE_PATH_PROTOCOL *) (
+               (UINT8 *) SerialDevice->DevicePath + GetDevicePathSize (SerialDevice->ParentDevicePath) - END_DEVICE_PATH_LENGTH
+               ),
+             NULL,
+             NULL
+             );
+    CopyMem (Uart, &SerialDevice->UartDevicePath, sizeof (UART_DEVICE_PATH));
+    Status = gBS->ReinstallProtocolInterface (
+                    SerialDevice->Handle,
+                    &gEfiDevicePathProtocolGuid,
+                    SerialDevice->DevicePath,
+                    SerialDevice->DevicePath
+                    );
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Set Control Bits.
+
+  @param This              Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control           Control bits that can be settable
+
+  @retval EFI_SUCCESS       New Control bits were set successfully
+  @retval EFI_UNSUPPORTED   The Control bits wanted to set are not supported
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN UINT32                  Control
+  )
+{
+  SERIAL_DEV                    *SerialDevice;
+  SERIAL_PORT_MCR               Mcr;
+  EFI_TPL                       Tpl;
+  UART_FLOW_CONTROL_DEVICE_PATH *FlowControl;
+  EFI_STATUS                    Status;
+
+  //
+  // The control bits that can be set are :
+  //     EFI_SERIAL_DATA_TERMINAL_READY: 0x0001  // WO
+  //     EFI_SERIAL_REQUEST_TO_SEND: 0x0002  // WO
+  //     EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE: 0x1000  // RW
+  //     EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE: 0x2000  // RW
+  //     EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE: 0x4000 // RW
+  //
+  SerialDevice = SERIAL_DEV_FROM_THIS (This);
+
+  //
+  // first determine the parameter is invalid
+  //
+  if ((Control & (~(EFI_SERIAL_REQUEST_TO_SEND | EFI_SERIAL_DATA_TERMINAL_READY |
+                    EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE | EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE |
+                    EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE))) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Mcr.Data = READ_MCR (SerialDevice);
+  Mcr.Bits.DtrC = 0;
+  Mcr.Bits.Rts = 0;
+  Mcr.Bits.Lme = 0;
+  SerialDevice->SoftwareLoopbackEnable = FALSE;
+  SerialDevice->HardwareFlowControl = FALSE;
+
+  if ((Control & EFI_SERIAL_DATA_TERMINAL_READY) == EFI_SERIAL_DATA_TERMINAL_READY) {
+    Mcr.Bits.DtrC = 1;
+  }
+
+  if ((Control & EFI_SERIAL_REQUEST_TO_SEND) == EFI_SERIAL_REQUEST_TO_SEND) {
+    Mcr.Bits.Rts = 1;
+  }
+
+  if ((Control & EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE) == EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE) {
+    Mcr.Bits.Lme = 1;
+  }
+
+  if ((Control & EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE) == EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE) {
+    SerialDevice->HardwareFlowControl = TRUE;
+  }
+
+  WRITE_MCR (SerialDevice, Mcr.Data);
+
+  if ((Control & EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE) == EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE) {
+    SerialDevice->SoftwareLoopbackEnable = TRUE;
+  }
+
+  Status = EFI_SUCCESS;
+  if (SerialDevice->Handle != NULL) {
+    FlowControl = (UART_FLOW_CONTROL_DEVICE_PATH *) (
+                    (UINTN) SerialDevice->DevicePath
+                    + GetDevicePathSize (SerialDevice->ParentDevicePath)
+                    - END_DEVICE_PATH_LENGTH
+                    + sizeof (UART_DEVICE_PATH)
+                    );
+    if (IsUartFlowControlDevicePathNode (FlowControl) &&
+        ((BOOLEAN) (ReadUnaligned32 (&FlowControl->FlowControlMap) == UART_FLOW_CONTROL_HARDWARE) != SerialDevice->HardwareFlowControl)) {
+      //
+      // Flow Control setting is changed, need to reinstall device path protocol
+      //
+      WriteUnaligned32 (&FlowControl->FlowControlMap, SerialDevice->HardwareFlowControl ? UART_FLOW_CONTROL_HARDWARE : 0);
+      Status = gBS->ReinstallProtocolInterface (
+                      SerialDevice->Handle,
+                      &gEfiDevicePathProtocolGuid,
+                      SerialDevice->DevicePath,
+                      SerialDevice->DevicePath
+                      );
+    }
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Get ControlBits.
+
+  @param This          Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param Control       Control signals of the serial device
+
+  @retval EFI_SUCCESS   Get Control signals successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  OUT UINT32                 *Control
+  )
+{
+  SERIAL_DEV      *SerialDevice;
+  SERIAL_PORT_MSR Msr;
+  SERIAL_PORT_MCR Mcr;
+  EFI_TPL         Tpl;
+
+  Tpl           = gBS->RaiseTPL (TPL_NOTIFY);
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+
+  *Control      = 0;
+
+  //
+  // Read the Modem Status Register
+  //
+  Msr.Data = READ_MSR (SerialDevice);
+
+  if (Msr.Bits.Cts == 1) {
+    *Control |= EFI_SERIAL_CLEAR_TO_SEND;
+  }
+
+  if (Msr.Bits.Dsr == 1) {
+    *Control |= EFI_SERIAL_DATA_SET_READY;
+  }
+
+  if (Msr.Bits.Ri == 1) {
+    *Control |= EFI_SERIAL_RING_INDICATE;
+  }
+
+  if (Msr.Bits.Dcd == 1) {
+    *Control |= EFI_SERIAL_CARRIER_DETECT;
+  }
+  //
+  // Read the Modem Control Register
+  //
+  Mcr.Data = READ_MCR (SerialDevice);
+
+  if (Mcr.Bits.DtrC == 1) {
+    *Control |= EFI_SERIAL_DATA_TERMINAL_READY;
+  }
+
+  if (Mcr.Bits.Rts == 1) {
+    *Control |= EFI_SERIAL_REQUEST_TO_SEND;
+  }
+
+  if (Mcr.Bits.Lme == 1) {
+    *Control |= EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE;
+  }
+
+  if (SerialDevice->HardwareFlowControl) {
+    *Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+  }
+  //
+  // Update FIFO status
+  //
+  SerialReceiveTransmit (SerialDevice);
+
+  //
+  // See if the Transmit FIFO is empty
+  //
+  if (SerialFifoEmpty (&SerialDevice->Transmit)) {
+    *Control |= EFI_SERIAL_OUTPUT_BUFFER_EMPTY;
+  }
+
+  //
+  // See if the Receive FIFO is empty.
+  //
+  if (SerialFifoEmpty (&SerialDevice->Receive)) {
+    *Control |= EFI_SERIAL_INPUT_BUFFER_EMPTY;
+  }
+
+  if (SerialDevice->SoftwareLoopbackEnable) {
+    *Control |= EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE;
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write the specified number of bytes to serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param  BufferSize         On input the size of Buffer, on output the amount of
+                       data actually written
+  @param  Buffer             The buffer of data to write
+
+  @retval EFI_SUCCESS        The data were written successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The write operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN OUT UINTN               *BufferSize,
+  IN VOID                    *Buffer
+  )
+{
+  SERIAL_DEV  *SerialDevice;
+  UINT8       *CharBuffer;
+  UINT32      Index;
+  UINTN       Elapsed;
+  UINTN       ActualWrite;
+  EFI_TPL     Tpl;
+  UINTN       Timeout;
+  UINTN       BitsPerCharacter;
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+  Elapsed       = 0;
+  ActualWrite   = 0;
+
+  if (*BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (Buffer == NULL) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_OUTPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  Tpl         = gBS->RaiseTPL (TPL_NOTIFY);
+
+  CharBuffer  = (UINT8 *) Buffer;
+
+  //
+  // Compute the number of bits in a single character.  This is a start bit,
+  // followed by the number of data bits, followed by the number of stop bits.
+  // The number of stop bits is specified by an enumeration that includes
+  // support for 1.5 stop bits.  Treat 1.5 stop bits as 2 stop bits.
+  //
+  BitsPerCharacter =
+    1 +
+    This->Mode->DataBits +
+    ((This->Mode->StopBits == TwoStopBits) ? 2 : This->Mode->StopBits);
+
+  //
+  // Compute the timeout in microseconds to wait for a single byte to be
+  // transmitted.  The Mode structure contans a Timeout field that is the
+  // maximum time to transmit or receive a character.  However, many UARTs
+  // have a FIFO for transmits, so the time required to add one new character
+  // to the transmit FIFO may be the time required to flush a full FIFO.  If
+  // the Timeout in the Mode structure is smaller than the time required to
+  // flush a full FIFO at the current baud rate, then use a timeout value that
+  // is required to flush a full transmit FIFO.
+  //
+  Timeout = MAX (
+              This->Mode->Timeout,
+              (UINTN)DivU64x64Remainder (
+                BitsPerCharacter * (SerialDevice->TransmitFifoDepth + 1) * 1000000,
+                This->Mode->BaudRate,
+                NULL
+                )
+              );
+  
+  for (Index = 0; Index < *BufferSize; Index++) {
+    SerialFifoAdd (&SerialDevice->Transmit, CharBuffer[Index]);
+
+    while (SerialReceiveTransmit (SerialDevice) != EFI_SUCCESS || !SerialFifoEmpty (&SerialDevice->Transmit)) {
+      //
+      //  Unsuccessful write so check if timeout has expired, if not,
+      //  stall for a bit, increment time elapsed, and try again
+      //
+      if (Elapsed >= Timeout) {
+        *BufferSize = ActualWrite;
+        gBS->RestoreTPL (Tpl);
+        return EFI_TIMEOUT;
+      }
+
+      gBS->Stall (TIMEOUT_STALL_INTERVAL);
+
+      Elapsed += TIMEOUT_STALL_INTERVAL;
+    }
+
+    ActualWrite++;
+    //
+    //  Successful write so reset timeout
+    //
+    Elapsed = 0;
+  }
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read the specified number of bytes from serial device.
+
+  @param This               Pointer to EFI_SERIAL_IO_PROTOCOL
+  @param BufferSize         On input the size of Buffer, on output the amount of
+                            data returned in buffer
+  @param Buffer             The buffer to return the data into
+
+  @retval EFI_SUCCESS        The data were read successfully
+  @retval EFI_DEVICE_ERROR   The device reported an error
+  @retval EFI_TIMEOUT        The read operation was stopped due to timeout
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL  *This,
+  IN OUT UINTN               *BufferSize,
+  OUT VOID                   *Buffer
+  )
+{
+  SERIAL_DEV  *SerialDevice;
+  UINT32      Index;
+  UINT8       *CharBuffer;
+  UINTN       Elapsed;
+  EFI_STATUS  Status;
+  EFI_TPL     Tpl;
+
+  SerialDevice  = SERIAL_DEV_FROM_THIS (This);
+  Elapsed       = 0;
+
+  if (*BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Tpl     = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Status  = SerialReceiveTransmit (SerialDevice);
+
+  if (EFI_ERROR (Status)) {
+    *BufferSize = 0;
+
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE,
+      EFI_P_EC_INPUT_ERROR | EFI_PERIPHERAL_SERIAL_PORT,
+      SerialDevice->DevicePath
+      );
+
+    gBS->RestoreTPL (Tpl);
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  CharBuffer = (UINT8 *) Buffer;
+  for (Index = 0; Index < *BufferSize; Index++) {
+    while (SerialFifoRemove (&SerialDevice->Receive, &(CharBuffer[Index])) != EFI_SUCCESS) {
+      //
+      //  Unsuccessful read so check if timeout has expired, if not,
+      //  stall for a bit, increment time elapsed, and try again
+      //  Need this time out to get conspliter to work.
+      //
+      if (Elapsed >= This->Mode->Timeout) {
+        *BufferSize = Index;
+        gBS->RestoreTPL (Tpl);
+        return EFI_TIMEOUT;
+      }
+
+      gBS->Stall (TIMEOUT_STALL_INTERVAL);
+      Elapsed += TIMEOUT_STALL_INTERVAL;
+
+      Status = SerialReceiveTransmit (SerialDevice);
+      if (Status == EFI_DEVICE_ERROR) {
+        *BufferSize = Index;
+        gBS->RestoreTPL (Tpl);
+        return EFI_DEVICE_ERROR;
+      }
+    }
+    //
+    //  Successful read so reset timeout
+    //
+    Elapsed = 0;
+  }
+
+  SerialReceiveTransmit (SerialDevice);
+
+  gBS->RestoreTPL (Tpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Use scratchpad register to test if this serial port is present.
+
+  @param SerialDevice   Pointer to serial device structure
+
+  @return if this serial port is present
+**/
+BOOLEAN
+SerialPresent (
+  IN SERIAL_DEV *SerialDevice
+  )
+
+{
+  UINT8   Temp;
+  BOOLEAN Status;
+
+  Status = TRUE;
+
+  //
+  // Save SCR reg
+  //
+  Temp = READ_SCR (SerialDevice);
+  WRITE_SCR (SerialDevice, 0xAA);
+
+  if (READ_SCR (SerialDevice) != 0xAA) {
+    Status = FALSE;
+  }
+
+  WRITE_SCR (SerialDevice, 0x55);
+
+  if (READ_SCR (SerialDevice) != 0x55) {
+    Status = FALSE;
+  }
+  //
+  // Restore SCR
+  //
+  WRITE_SCR (SerialDevice, Temp);
+  return Status;
+}
+
+/**
+  Read serial port.
+
+  @param SerialDev     Pointer to serial device
+  @param Offset        Offset in register group
+
+  @return Data read from serial port
+
+**/
+UINT8
+SerialReadRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset
+  )
+{
+  UINT8                                    Data;
+  EFI_STATUS                               Status;
+
+  if (SerialDev->PciDeviceInfo == NULL) {
+    return IoRead8 ((UINTN) SerialDev->BaseAddress + Offset * SerialDev->RegisterStride);
+  } else {
+    if (SerialDev->MmioAccess) {
+      Status = SerialDev->PciDeviceInfo->PciIo->Mem.Read (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                          SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    } else {
+      Status = SerialDev->PciDeviceInfo->PciIo->Io.Read (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                         SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    }
+    ASSERT_EFI_ERROR (Status);
+    return Data;
+  }
+}
+
+/**
+  Write serial port.
+
+  @param  SerialDev     Pointer to serial device
+  @param  Offset        Offset in register group
+  @param  Data          data which is to be written to some serial port register
+**/
+VOID
+SerialWriteRegister (
+  IN SERIAL_DEV                            *SerialDev,
+  IN UINT32                                Offset,
+  IN UINT8                                 Data
+  )
+{
+  EFI_STATUS                               Status;
+
+  if (SerialDev->PciDeviceInfo == NULL) {
+    IoWrite8 ((UINTN) SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, Data);
+  } else {
+    if (SerialDev->MmioAccess) {
+      Status = SerialDev->PciDeviceInfo->PciIo->Mem.Write (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                           SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    } else {
+      Status = SerialDev->PciDeviceInfo->PciIo->Io.Write (SerialDev->PciDeviceInfo->PciIo, EfiPciIoWidthUint8, EFI_PCI_IO_PASS_THROUGH_BAR,
+                                                          SerialDev->BaseAddress + Offset * SerialDev->RegisterStride, 1, &Data);
+    }
+    ASSERT_EFI_ERROR (Status);
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c
new file mode 100644
index 0000000000..1e2db1a876
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/ComponentName.c
@@ -0,0 +1,225 @@
+/** @file
+  UfsHcDxe driver produces EFI_UFS_HOST_CONTROLLER_PROTOCOL. The upper layer module
+  uses it to query the MMIO base address of the UFS host controller.
+
+  Copyright (c) 2014, 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 "UfsPciHcDxe.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gUfsHcComponentName = {
+  UfsHcComponentNameGetDriverName,
+  UfsHcComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUfsHcComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UfsHcComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UfsHcComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsHcDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pci Host Controller Driver"
+  },
+  { 
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsHcControllerNameTable[] = {
+  {
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pci Host Controller"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL       *This,
+  IN  CHAR8                             *Language,
+  OUT CHAR16                            **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsHcDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUfsHcComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUfsHcDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsHcControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUfsHcComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c
new file mode 100644
index 0000000000..23ebbf108f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.c
@@ -0,0 +1,818 @@
+/** @file
+  UfsHcDxe driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014 - 2015, 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 "UfsPciHcDxe.h"
+
+//
+// NVM Express Driver Binding Protocol Instance
+//
+EFI_DRIVER_BINDING_PROTOCOL gUfsHcDriverBinding = {
+  UfsHcDriverBindingSupported,
+  UfsHcDriverBindingStart,
+  UfsHcDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+//
+// Template for Ufs host controller private data.
+//
+UFS_HOST_CONTROLLER_PRIVATE_DATA gUfsHcTemplate = {
+  UFS_HC_PRIVATE_DATA_SIGNATURE,  // Signature
+  NULL,                           // Handle
+  {                               // UfsHcProtocol
+    UfsHcGetMmioBar,
+    UfsHcAllocateBuffer,
+    UfsHcFreeBuffer,
+    UfsHcMap,
+    UfsHcUnmap,
+    UfsHcFlush,
+    UfsHcMmioRead,
+    UfsHcMmioWrite
+  },
+  NULL,                           // PciIo
+  0,                              // BarIndex
+  0                               // PciAttributes
+};
+
+/**
+  Get the MMIO base of the UFS host controller.
+
+  @param[in]   This             A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param[out]  MmioBar          The MMIO base address of UFS host controller.
+
+  @retval EFI_SUCCESS           The operation succeeds.
+  @retval others                The operation fails.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcGetMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+     OUT UINTN                              *MmioBar
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *BarDesc;
+
+  if ((This == NULL) || (MmioBar == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BarDesc  = NULL;
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status = PciIo->GetBarAttributes (
+                    PciIo,
+                    BarIndex,
+                    NULL,
+                    (VOID**) &BarDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  *MmioBar = (UINTN)BarDesc->AddrRangeMin;
+
+  FreePool (BarDesc);
+
+  return Status;
+}
+
+/**                                                                 
+  Provides the UFS controller-specific addresses needed to access system memory.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the UFS controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.                                                 
+  @param  DeviceAddress         The resulting map address for the bus master UFS controller to use to
+                                access the hosts HostAddress.                                        
+  @param  Mapping               A resulting value to pass to Unmap().
+                                  
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.                                
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMap (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_OPERATION  Operation,
+  IN     VOID                                 *HostAddress,
+  IN OUT UINTN                                *NumberOfBytes,
+     OUT EFI_PHYSICAL_ADDRESS                 *DeviceAddress,
+     OUT VOID                                 **Mapping
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL) || (NumberOfBytes == NULL) || (DeviceAddress == NULL) || (Mapping == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Map (PciIo, (EFI_PCI_IO_PROTOCOL_OPERATION)Operation, HostAddress, NumberOfBytes, DeviceAddress, Mapping);
+  return Status;
+}
+
+/**                                                                 
+  Completes the Map() operation and releases any corresponding resources.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.                                      
+  @param  Mapping               The mapping value returned from Map().
+                                  
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcUnmap (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN  VOID                               *Mapping
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (Mapping == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Unmap (PciIo, Mapping);
+  return Status;
+}
+
+/**                                                                 
+  Allocates pages that are suitable for an EfiUfsHcOperationBusMasterCommonBuffer
+  mapping.                                                                       
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.                               
+  @param  Pages                 The number of pages to allocate.                                
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.                                        
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.                     
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.  
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcAllocateBuffer (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN     EFI_ALLOCATE_TYPE                  Type,
+  IN     EFI_MEMORY_TYPE                    MemoryType,
+  IN     UINTN                              Pages,
+     OUT VOID                               **HostAddress,
+  IN     UINT64                             Attributes
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->AllocateBuffer (PciIo, Type, MemoryType, Pages, HostAddress, Attributes);
+  return Status;
+}
+
+/**                                                                 
+  Frees memory that was allocated with AllocateBuffer().
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+  @param  Pages                 The number of pages to free.                                
+  @param  HostAddress           The base system memory address of the allocated range.                                    
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFreeBuffer (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN  UINTN                              Pages,
+  IN  VOID                               *HostAddress
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  if ((This == NULL) || (HostAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->FreeBuffer (PciIo, Pages, HostAddress);
+  return Status;
+}
+
+/**                                                                 
+  Flushes all posted write transactions from the UFS bus to attached UFS device.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+                                  
+  @retval EFI_SUCCESS           The posted write transactions were flushed from the UFS bus
+                                to attached UFS device.                                      
+  @retval EFI_DEVICE_ERROR      The posted write transactions were not flushed from the UFS
+                                bus to attached UFS device due to a hardware error.                           
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFlush (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo   = Private->PciIo;
+
+  Status  = PciIo->Flush (PciIo);
+  return Status;
+}
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioRead (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status   = PciIo->Mem.Read (PciIo, (EFI_PCI_IO_PROTOCOL_WIDTH)Width, BarIndex, Offset, Count, Buffer);
+
+  return Status;
+}
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioWrite (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  )
+{
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  EFI_STATUS                        Status;
+  UINT8                             BarIndex;
+
+  Private  = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (This);
+  PciIo    = Private->PciIo;
+  BarIndex = Private->BarIndex;
+
+  Status   = PciIo->Mem.Write (PciIo, (EFI_PCI_IO_PROTOCOL_WIDTH)Width, BarIndex, Offset, Count, Buffer);
+
+  return Status;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  BOOLEAN                   UfsHcFound;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  PCI_TYPE00                PciData;
+
+  PciIo            = NULL;
+  ParentDevicePath = NULL;
+  UfsHcFound       = FALSE;
+
+  //
+  // UfsHcDxe is a device driver, and should ingore the
+  // "RemainingDevicePath" according to EFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Now test the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Now further check the PCI header: Base class (offset 0x0B) and
+  // Sub Class (offset 0x0A). This controller should be an UFS controller
+  //
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        0,
+                        sizeof (PciData),
+                        &PciData
+                        );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Since we already got the PciData, we can close protocol to avoid to carry it on for multiple exit points.
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Examine UFS Host Controller PCI Configuration table fields
+  //
+  if (PciData.Hdr.ClassCode[2] == PCI_CLASS_MASS_STORAGE) {
+    if (PciData.Hdr.ClassCode[1] == 0x09 ) { //UFS Controller Subclass
+      UfsHcFound = TRUE;
+    }
+  }
+
+  if (!UfsHcFound) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return Status;
+}
+
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_PCI_IO_PROTOCOL               *PciIo;
+  UFS_HOST_CONTROLLER_PRIVATE_DATA  *Private;
+  UINT64                            Supports;
+  UINT8                             BarIndex;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *BarDesc;
+
+  PciIo    = NULL;
+  Private  = NULL;
+  Supports = 0;
+  BarDesc  = NULL;
+
+  //
+  // Now test and open the EfiPciIoProtocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  //
+  // Status == 0 - A normal execution flow, SUCCESS and the program proceeds.
+  // Status == ALREADY_STARTED - A non-zero Status code returned. It indicates
+  //           that the protocol has been opened and should be treated as a
+  //           normal condition and the program proceeds. The Protocol will not
+  //           opened 'again' by this call.
+  // Status != ALREADY_STARTED - Error status, terminate program execution
+  //
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  Private = AllocateCopyPool (sizeof (UFS_HOST_CONTROLLER_PRIVATE_DATA), &gUfsHcTemplate);
+  if (Private == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  Private->PciIo = PciIo;
+
+  for (BarIndex = 0; BarIndex < PCI_MAX_BAR; BarIndex++) {
+    Status = PciIo->GetBarAttributes (
+                      PciIo,
+                      BarIndex,
+                      NULL,
+                      (VOID**) &BarDesc
+                      );
+    if (Status == EFI_UNSUPPORTED) {
+      continue;
+    } else if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    if (BarDesc->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+      Private->BarIndex = BarIndex;
+      FreePool (BarDesc);
+      break;
+    }
+
+    FreePool (BarDesc);
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &Private->PciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status    = PciIo->Attributes (
+                         PciIo,
+                         EfiPciIoAttributeOperationEnable,
+                         Supports,
+                         NULL
+                         );
+  } else {
+    goto Done;
+  }
+
+  ///
+  /// Install UFS_HOST_CONTROLLER protocol
+  ///
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  (VOID*)&(Private->UfsHc)
+                  );
+
+Done:
+  if (EFI_ERROR (Status)) {
+    if ((Private != NULL) && (Private->PciAttributes != 0)) {
+      //
+      // Restore original PCI attributes
+      //
+      Status = PciIo->Attributes (
+                        PciIo,
+                        EfiPciIoAttributeOperationSet,
+                        Private->PciAttributes,
+                        NULL
+                        );
+      ASSERT_EFI_ERROR (Status);
+    }
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiPciIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    if (Private != NULL) {
+      FreePool (Private);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                          Status;
+  UFS_HOST_CONTROLLER_PRIVATE_DATA    *Private;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+
+  ///
+  /// Get private data
+  ///
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  (VOID **) &UfsHc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Private = UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC (UfsHc);
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  &(Private->UfsHc)
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Restore original PCI attributes
+    //
+    Status = Private->PciIo->Attributes (
+                               Private->PciIo,
+                               EfiPciIoAttributeOperationSet,
+                               Private->PciAttributes,
+                               NULL
+                               );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Close protocols opened by UFS host controller driver
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiPciIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+/**
+  The entry point for UFS host controller driver, used to install this driver on the ImageHandle.
+
+  @param[in]  ImageHandle   The firmware allocated handle for this driver image.
+  @param[in]  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS           Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUfsHcDriverBinding,
+             ImageHandle,
+             &gUfsHcComponentName,
+             &gUfsHcComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h
new file mode 100644
index 0000000000..89f19feb7a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.h
@@ -0,0 +1,511 @@
+/** @file
+  UfsHcDxe driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014 - 2015, 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.
+
+**/
+
+#ifndef _EFI_UFS_HOST_CONTROLLER_H_
+#define _EFI_UFS_HOST_CONTROLLER_H_
+
+#include <Uefi.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Acpi.h>
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/UfsHostController.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+extern EFI_DRIVER_BINDING_PROTOCOL                gUfsHcDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL                gUfsHcComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL               gUfsHcComponentName2;
+
+//
+// Unique signature for private data structure.
+//
+#define UFS_HC_PRIVATE_DATA_SIGNATURE             SIGNATURE_32 ('U','F','S','H')
+
+typedef struct _UFS_HOST_CONTROLLER_PRIVATE_DATA  UFS_HOST_CONTROLLER_PRIVATE_DATA;
+
+//
+// Nvme private data structure.
+//
+struct _UFS_HOST_CONTROLLER_PRIVATE_DATA {
+  UINT32                             Signature;
+  EFI_HANDLE                         Handle;
+
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL UfsHc;
+  EFI_PCI_IO_PROTOCOL                *PciIo;  
+  UINT8                              BarIndex;
+  UINT64                             PciAttributes;
+};
+
+#define UFS_HOST_CONTROLLER_PRIVATE_DATA_FROM_UFSHC(a) \
+  CR (a, \
+      UFS_HOST_CONTROLLER_PRIVATE_DATA, \
+      UfsHc, \
+      UFS_HC_PRIVATE_DATA_SIGNATURE \
+      )
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Get the MMIO base of the UFS host controller.
+
+  @param[in]   This             A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param[out]  MmioBar          The MMIO base address of UFS host controller.
+
+  @retval EFI_SUCCESS           The operation succeeds.
+  @retval others                The operation fails.
+**/
+EFI_STATUS
+EFIAPI
+UfsHcGetMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+     OUT UINTN                              *MmioBar
+  );
+
+/**                                                                 
+  Provides the UFS controller-specific addresses needed to access system memory.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the UFS controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.                                                 
+  @param  DeviceAddress         The resulting map address for the bus master UFS controller to use to
+                                access the hosts HostAddress.                                        
+  @param  Mapping               A resulting value to pass to Unmap().
+                                  
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.                                
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMap (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_OPERATION  Operation,
+  IN     VOID                                 *HostAddress,
+  IN OUT UINTN                                *NumberOfBytes,
+     OUT EFI_PHYSICAL_ADDRESS                 *DeviceAddress,
+     OUT VOID                                 **Mapping
+  );
+
+/**                                                                 
+  Completes the Map() operation and releases any corresponding resources.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.                                      
+  @param  Mapping               The mapping value returned from Map().
+                                  
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcUnmap (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN  VOID                                 *Mapping
+  );
+
+/**                                                                 
+  Allocates pages that are suitable for an EfiUfsHcOperationBusMasterCommonBuffer
+  mapping.                                                                       
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.                               
+  @param  Pages                 The number of pages to allocate.                                
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.                                        
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.                     
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.  
+                                   
+**/
+EFI_STATUS
+EFIAPI
+UfsHcAllocateBuffer (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL *This,
+  IN     EFI_ALLOCATE_TYPE                  Type,
+  IN     EFI_MEMORY_TYPE                    MemoryType,
+  IN     UINTN                              Pages,
+     OUT VOID                               **HostAddress,
+  IN     UINT64                             Attributes
+  );
+
+/**                                                                 
+  Frees memory that was allocated with AllocateBuffer().
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+  @param  Pages                 The number of pages to free.                                
+  @param  HostAddress           The base system memory address of the allocated range.                                    
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFreeBuffer (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *This,
+  IN  UINTN                                 Pages,
+  IN  VOID                                  *HostAddress
+  );
+
+/**                                                                 
+  Flushes all posted write transactions from the UFS bus to attached UFS device.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+                                  
+  @retval EFI_SUCCESS           The posted write transactions were flushed from the UFS bus
+                                to attached UFS device.                                      
+  @retval EFI_DEVICE_ERROR      The posted write transactions were not flushed from the UFS
+                                bus to attached UFS device due to a hardware error.                           
+                                     
+**/
+EFI_STATUS
+EFIAPI
+UfsHcFlush (
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This
+  );
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioRead (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  );
+
+/**                                                                 
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsHcMmioWrite (
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf
new file mode 100644
index 0000000000..c1ce9ea851
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#  Component Description File For Universal Flash Storage Pci Host Controller Module.
+#
+#  Copyright (c) 2014 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPciHcDxe
+  MODULE_UNI_FILE                = UfsPciHcDxe.uni
+  FILE_GUID                      = AF43E178-C2E9-4712-A7CD-08BFDAC7482C
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 0.9
+  ENTRY_POINT                    = UfsHcDriverEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUfsHcDriverBinding
+#  COMPONENT_NAME                =  gUfsHcComponentName
+#  COMPONENT_NAME2               =  gUfsHcComponentName2
+
+[Sources]
+  ComponentName.c
+  UfsPciHcDxe.c
+  UfsPciHcDxe.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  DevicePathLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiLib
+
+[Protocols]
+  gEfiPciIoProtocolGuid                       ## TO_START
+  gEfiDevicePathProtocolGuid                  ## TO_START
+  gEdkiiUfsHostControllerProtocolGuid         ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPciHcDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni
new file mode 100644
index 0000000000..5b3a137d80
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni
new file mode 100644
index 0000000000..fb359fd615
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c
new file mode 100644
index 0000000000..36325641e7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.c
@@ -0,0 +1,152 @@
+/** @file
+  UfsPciHcPei driver is used to provide platform-dependent info, mainly UFS host controller
+  MMIO base, to upper layer UFS drivers.
+
+  Copyright (c) 2014, 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 "UfsPciHcPei.h"
+
+EDKII_UFS_HOST_CONTROLLER_PPI  mUfsHostControllerPpi = { GetUfsHcMmioBar };
+
+EFI_PEI_PPI_DESCRIPTOR   mPpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEdkiiPeiUfsHostControllerPpiGuid,
+  &mUfsHostControllerPpi
+};
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param[in]  This               The protocol instance pointer.
+  @param[in]  ControllerId       The ID of the UFS host controller.
+  @param[out] MmioBar            Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+GetUfsHcMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PPI *This,
+  IN     UINT8                         ControllerId,
+     OUT UINTN                         *MmioBar
+  )
+{
+  UFS_HC_PEI_PRIVATE_DATA  *Private;
+
+  if ((This == NULL) || (MmioBar == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = UFS_HC_PEI_PRIVATE_DATA_FROM_THIS (This);
+
+  if (ControllerId >= Private->TotalUfsHcs) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  *MmioBar = (UINTN)Private->UfsHcPciAddr[ControllerId];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user code starts with this function.
+  
+  @param  FileHandle             Handle of the file being invoked.
+  @param  PeiServices            Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            The driver is successfully initialized.
+  @retval Others                 Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsHcPeim (
+  IN EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_BOOT_MODE            BootMode;
+  EFI_STATUS               Status;
+  UINT16                   Bus;
+  UINT16                   Device;
+  UINT16                   Function;
+  UINT32                   Size;
+  UINT8                    SubClass;
+  UINT8                    BaseClass;
+  UFS_HC_PEI_PRIVATE_DATA  *Private;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesGetBootMode (&BootMode);
+  ///
+  /// We do not export this in S3 boot path, because it is only for recovery.
+  ///
+  if (BootMode == BOOT_ON_S3_RESUME) {
+    return EFI_SUCCESS;
+  }
+
+  Private = (UFS_HC_PEI_PRIVATE_DATA *) AllocateZeroPool (sizeof (UFS_HC_PEI_PRIVATE_DATA));
+  if (Private == NULL) {
+    DEBUG ((EFI_D_ERROR, "Failed to allocate memory for UFS_HC_PEI_PRIVATE_DATA! \n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Signature            = UFS_HC_PEI_SIGNATURE;
+  Private->UfsHostControllerPpi = mUfsHostControllerPpi;
+  Private->PpiList              = mPpiList;
+  Private->PpiList.Ppi          = &Private->UfsHostControllerPpi;
+
+  for (Bus = 0; Bus < 256; Bus++) {
+    for (Device = 0; Device < 32; Device++) {
+      for (Function = 0; Function < 8; Function++) {
+        SubClass  = PciRead8 (PCI_LIB_ADDRESS (Bus, Device, Function, 0x0A));
+        BaseClass = PciRead8 (PCI_LIB_ADDRESS (Bus, Device, Function, 0x0B));
+
+        if ((SubClass == 0x09) && (BaseClass == PCI_CLASS_MASS_STORAGE)) {
+          //
+          // Get the Ufs Pci host controller's MMIO region size.
+          //
+          PciAnd16 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_COMMAND_OFFSET), (UINT16)~(EFI_PCI_COMMAND_BUS_MASTER | EFI_PCI_COMMAND_MEMORY_SPACE));
+          PciWrite32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET), 0xFFFFFFFF);
+          Size = PciRead32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET));
+          //
+          // Assign resource to the Ufs Pci host controller's MMIO BAR.
+          // Enable the Ufs Pci host controller by setting BME and MSE bits of PCI_CMD register.
+          //
+          PciWrite32 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_BASE_ADDRESSREG_OFFSET), (UINT32)(PcdGet32 (PcdUfsPciHostControllerMmioBase) + Size * Private->TotalUfsHcs));
+          PciOr16 (PCI_LIB_ADDRESS (Bus, Device, Function, PCI_COMMAND_OFFSET), (EFI_PCI_COMMAND_BUS_MASTER | EFI_PCI_COMMAND_MEMORY_SPACE));
+          //
+          // Record the allocated Mmio base address.
+          //
+          Private->UfsHcPciAddr[Private->TotalUfsHcs] = PcdGet32 (PcdUfsPciHostControllerMmioBase) + Size * Private->TotalUfsHcs;
+          Private->TotalUfsHcs++;
+          ASSERT (Private->TotalUfsHcs < MAX_UFS_HCS);
+        }
+      }
+    }
+  }
+
+  ///
+  /// Install Ufs Host Controller PPI
+  ///
+  Status = PeiServicesInstallPpi (&Private->PpiList);
+
+  ASSERT_EFI_ERROR (Status);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h
new file mode 100644
index 0000000000..2405dd76d8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.h
@@ -0,0 +1,62 @@
+/** @file
+  Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _UFS_PCI_HOST_CONTROLLER_PEI_H_
+#define _UFS_PCI_HOST_CONTROLLER_PEI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/MasterBootMode.h>
+#include <Ppi/UfsHostController.h>
+
+#include <IndustryStandard/Pci.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PciLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#define UFS_HC_PEI_SIGNATURE    SIGNATURE_32 ('U', 'F', 'S', 'P')
+#define MAX_UFS_HCS             8
+
+typedef struct {
+  UINTN                         Signature;
+  EDKII_UFS_HOST_CONTROLLER_PPI UfsHostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR        PpiList;
+  UINTN                         TotalUfsHcs;
+  UINTN                         UfsHcPciAddr[MAX_UFS_HCS];
+} UFS_HC_PEI_PRIVATE_DATA;
+
+#define UFS_HC_PEI_PRIVATE_DATA_FROM_THIS(a)  CR (a, UFS_HC_PEI_PRIVATE_DATA, UfsHostControllerPpi, UFS_HC_PEI_SIGNATURE)
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param[in]  This               The protocol instance pointer.
+  @param[in]  ControllerId       The ID of the UFS host controller.
+  @param[out] MmioBar            Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+GetUfsHcMmioBar (
+  IN     EDKII_UFS_HOST_CONTROLLER_PPI *This,
+  IN     UINT8                         ControllerId,
+     OUT UINTN                         *MmioBar
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf
new file mode 100644
index 0000000000..b16935628c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf
@@ -0,0 +1,56 @@
+## @file
+#  Component Description File For Universal Flash Storage Pci Host Controller Pei Module.
+#
+#  Copyright (c) 2014 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPciHcPei
+  MODULE_UNI_FILE                = UfsPciHcPei.uni
+  FILE_GUID                      = 905DC1AD-C44D-4965-98AC-B6B4444BFD65
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 0.9
+
+  ENTRY_POINT                    = InitializeUfsHcPeim
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UfsPciHcPei.c
+  UfsPciHcPei.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PciLib
+  DebugLib
+  PeiServicesLib
+  MemoryAllocationLib
+  PeimEntryPoint
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUfsPciHostControllerMmioBase   ## CONSUMES
+
+[Ppis]
+  gEdkiiPeiUfsHostControllerPpiGuid         ## PRODUCES
+
+[Depex]
+  gEfiPeiMasterBootModePpiGuid AND gEfiPeiMemoryDiscoveredPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPciHcPeiExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni
new file mode 100644
index 0000000000..7b7ea3af7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni
new file mode 100644
index 0000000000..4487eb968f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c
new file mode 100644
index 0000000000..ad2ba42b55
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.c
@@ -0,0 +1,231 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for UHCI driver.
+
+Copyright (c) 2004 - 2011, 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 "Uhci.h"
+
+
+//
+// EFI Component Name Protocol
+//
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUhciComponentName = {
+  UhciComponentNameGetDriverName,
+  UhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UhciComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUhciDriverNameTable[] = {
+  { "eng;en", L"Usb Uhci Driver" },
+  { NULL, NULL }
+};
+
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  USB_HC_DEV           *UhciDev;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gUhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhciDev = UHC_FROM_USB2_HC_PROTO (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UhciDev->CtrlNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h
new file mode 100644
index 0000000000..fd19a21ec6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/ComponentName.h
@@ -0,0 +1,145 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2008 - 2011, 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.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c
new file mode 100644
index 0000000000..179e36c3a2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.c
@@ -0,0 +1,1889 @@
+/** @file
+
+  The UHCI driver model and HC protocol routines.
+
+Copyright (c) 2004 - 2015, 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 "Uhci.h"
+
+
+EFI_DRIVER_BINDING_PROTOCOL gUhciDriverBinding = {
+  UhciDriverBindingSupported,
+  UhciDriverBindingStart,
+  UhciDriverBindingStop,
+  0x20,
+  NULL,
+  NULL
+};
+
+/**
+  Provides software reset for the USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes             A bit mask of the reset operation to perform.  See
+                                 below for a list of the supported bit mask values.
+
+  @return EFI_SUCCESS            The reset operation succeeded.
+  @return EFI_INVALID_PARAMETER  Attributes is not valid.
+  @return EFI_UNSUPPORTED        This type of reset is not currently supported.
+  @return EFI_DEVICE_ERROR       Other errors.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2Reset (
+  IN EFI_USB2_HC_PROTOCOL   *This,
+  IN UINT16                 Attributes
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+
+  if ((Attributes == EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG) ||
+      (Attributes == EFI_USB_HC_RESET_HOST_WITH_DEBUG)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (Uhc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Uhc->DevicePath
+      );
+  }
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+    //
+    // Stop schedule and set the Global Reset bit in the command register
+    //
+    UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_GRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RESET_STALL);
+
+    //
+    // Clear the Global Reset bit to zero.
+    //
+    UhciClearRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_GRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+    break;
+
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    //
+    // Stop schedule and set Host Controller Reset bit to 1
+    //
+    UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_HCRESET);
+
+    gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+    break;
+
+  default:
+    goto ON_INVAILD_PARAMETER;
+  }
+
+  //
+  // Delete all old transactions on the USB bus, then
+  // reinitialize the frame list
+  //
+  UhciFreeAllAsyncReq (Uhc);
+  UhciDestoryFrameList (Uhc);
+  UhciInitFrameList (Uhc);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+ON_INVAILD_PARAMETER:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_INVALID_PARAMETER;
+}
+
+
+/**
+  Retrieves current state of the USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to receive current device state.
+
+  @return EFI_SUCCESS            The state is returned.
+  @return EFI_INVALID_PARAMETER  State is not valid.
+  @return EFI_DEVICE_ERROR       Other errors.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetState (
+  IN   EFI_USB2_HC_PROTOCOL   *This,
+  OUT  EFI_USB_HC_STATE       *State
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT16              UsbSts;
+  UINT16              UsbCmd;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+
+  UsbCmd  = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+  UsbSts  = UhciReadReg (Uhc->PciIo, USBSTS_OFFSET);
+
+  if ((UsbCmd & USBCMD_EGSM) !=0 ) {
+    *State = EfiUsbHcStateSuspend;
+
+  } else if ((UsbSts & USBSTS_HCH) != 0) {
+    *State = EfiUsbHcStateHalt;
+
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets the USB host controller to a specific state according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Indicates the state of the host controller that will
+                                 be set.
+
+  @return EFI_SUCCESS            Host controller was successfully placed in the state.
+  @return EFI_INVALID_PARAMETER  State is invalid.
+  @return EFI_DEVICE_ERROR       Failed to set the state.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SetState (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN EFI_USB_HC_STATE        State
+  )
+{
+  EFI_USB_HC_STATE    CurState;
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  EFI_STATUS          Status;
+  UINT16              UsbCmd;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  Status  = Uhci2GetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  Status  = EFI_SUCCESS;
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    UsbCmd = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+
+    if (CurState == EfiUsbHcStateHalt) {
+      //
+      // Set Run/Stop bit to 1, also set the bandwidht reclamation
+      // point to 64 bytes
+      //
+      UsbCmd |= USBCMD_RS | USBCMD_MAXP;
+      UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+
+    } else if (CurState == EfiUsbHcStateSuspend) {
+      //
+      // If FGR(Force Global Resume) bit is 0, set it
+      //
+      if ((UsbCmd & USBCMD_FGR) == 0) {
+        UsbCmd |= USBCMD_FGR;
+        UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+      }
+
+      //
+      // wait 20ms to let resume complete (20ms is specified by UHCI spec)
+      //
+      gBS->Stall (UHC_FORCE_GLOBAL_RESUME_STALL);
+
+      //
+      // Write FGR bit to 0 and EGSM(Enter Global Suspend Mode) bit to 0
+      //
+      UsbCmd &= ~USBCMD_FGR;
+      UsbCmd &= ~USBCMD_EGSM;
+      UsbCmd |= USBCMD_RS;
+      UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+    }
+
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = Uhci2SetState (This, EfiUsbHcStateHalt);
+
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto ON_EXIT;
+    }
+
+    //
+    // Set Enter Global Suspend Mode bit to 1.
+    //
+    UsbCmd = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+    UsbCmd |= USBCMD_EGSM;
+    UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, UsbCmd);
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Retrieves capabilities of USB host controller according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed               A pointer to the max speed USB host controller
+                                 supports.
+  @param  PortNumber             A pointer to the number of root hub ports.
+  @param  Is64BitCapable         A pointer to an integer to show whether USB host
+                                 controller supports 64-bit memory addressing.
+
+  @return EFI_SUCCESS            capabilities were retrieved successfully.
+  @return EFI_INVALID_PARAMETER  MaxSpeed or PortNumber or Is64BitCapable is NULL.
+  @return EFI_DEVICE_ERROR       An error was encountered.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT32              Offset;
+  UINT16              PortSC;
+  UINT32              Index;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if ((NULL == MaxSpeed) || (NULL == PortNumber) || (NULL == Is64BitCapable)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *MaxSpeed       = EFI_USB_SPEED_FULL;
+  *Is64BitCapable = (UINT8) FALSE;
+
+  *PortNumber = 0;
+
+  for (Index = 0; Index < USB_MAX_ROOTHUB_PORT; Index++) {
+    Offset  = USBPORTSC_OFFSET + Index * 2;
+    PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+    //
+    // Port status's bit 7 is reserved and always returns 1 if
+    // the port number is valid. Intel's UHCI (in EHCI controller)
+    // returns 0 in this bit if port number is invalid. Also, if
+    // PciIo IoRead returns error, 0xFFFF is returned to caller.
+    //
+    if (((PortSC & 0x80) == 0) || (PortSC == 0xFFFF)) {
+      break;
+    }
+    (*PortNumber)++;
+  }
+
+  Uhc->RootPorts = *PortNumber;
+
+  DEBUG ((EFI_D_INFO, "Uhci2GetCapability: %d ports\n", (UINT32)Uhc->RootPorts));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the current status of a USB root hub port according to UEFI 2.0 spec.
+
+  @param  This                    A pointer to the EFI_USB2_HC_PROTOCOL.
+  @param  PortNumber              The port to get status.
+  @param  PortStatus              A pointer to the current port status bits and  port
+                                  status change bits.
+
+  @return EFI_SUCCESS             status of the USB root hub port was returned in PortStatus.
+  @return EFI_INVALID_PARAMETER   PortNumber is invalid.
+  @return EFI_DEVICE_ERROR        Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2GetRootHubPortStatus (
+  IN   EFI_USB2_HC_PROTOCOL   *This,
+  IN   UINT8                  PortNumber,
+  OUT  EFI_USB_PORT_STATUS    *PortStatus
+  )
+{
+  USB_HC_DEV          *Uhc;
+  UINT32              Offset;
+  UINT16              PortSC;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset                        = USBPORTSC_OFFSET + PortNumber * 2;
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  PortSC                        = UhciReadReg (Uhc->PciIo, Offset);
+
+  if ((PortSC & USBPORTSC_CCS) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_CONNECTION;
+  }
+
+  if ((PortSC & USBPORTSC_PED) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_ENABLE;
+  }
+
+  if ((PortSC & USBPORTSC_SUSP) != 0) {
+    DEBUG ((EFI_D_INFO, "Uhci2GetRootHubPortStatus: port %d is suspended\n", PortNumber));
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  if ((PortSC & USBPORTSC_PR) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_RESET;
+  }
+
+  if ((PortSC & USBPORTSC_LSDA) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+  }
+
+  //
+  // CHC will always return one in port owner bit
+  //
+  PortStatus->PortStatus |= USB_PORT_STAT_OWNER;
+
+  if ((PortSC & USBPORTSC_CSC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_CONNECTION;
+  }
+
+  if ((PortSC & USBPORTSC_PEDC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_ENABLE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets a feature for the specified root hub port according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL.
+  @param  PortNumber             Specifies the root hub port whose feature  is
+                                 requested to be set.
+  @param  PortFeature            Indicates the feature selector associated  with the
+                                 feature set request.
+
+  @return EFI_SUCCESS            PortFeature was set for the root port.
+  @return EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN UINT8                   PortNumber,
+  IN EFI_USB_PORT_FEATURE    PortFeature
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  UINT32              Offset;
+  UINT16              PortSC;
+  UINT16              Command;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset  = USBPORTSC_OFFSET + PortNumber * 2;
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+  PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+  switch (PortFeature) {
+  case EfiUsbPortSuspend:
+    Command = UhciReadReg (Uhc->PciIo, USBCMD_OFFSET);
+    if ((Command & USBCMD_EGSM) == 0) {
+      //
+      // if global suspend is not active, can set port suspend
+      //
+      PortSC &= 0xfff5;
+      PortSC |= USBPORTSC_SUSP;
+    }
+    break;
+
+  case EfiUsbPortReset:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PR;
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // No action
+    //
+    break;
+
+  case EfiUsbPortEnable:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PED;
+    break;
+
+  default:
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhciWriteReg (Uhc->PciIo, Offset, PortSC);
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Clears a feature for the specified root hub port according to Uefi 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber             Specifies the root hub port whose feature  is
+                                 requested to be cleared.
+  @param  PortFeature            Indicates the feature selector associated with the
+                                 feature clear request.
+
+  @return EFI_SUCCESS            PortFeature was cleared for the USB root hub port.
+  @return EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2ClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL    *This,
+  IN UINT8                   PortNumber,
+  IN EFI_USB_PORT_FEATURE    PortFeature
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_TPL             OldTpl;
+  UINT32              Offset;
+  UINT16              PortSC;
+
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+  if (PortNumber >= Uhc->RootPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset  = USBPORTSC_OFFSET + PortNumber * 2;
+
+  OldTpl  = gBS->RaiseTPL (UHCI_TPL);
+  PortSC  = UhciReadReg (Uhc->PciIo, Offset);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_PED;
+    break;
+
+  case EfiUsbPortSuspend:
+    //
+    // Cause a resume on the specified port if in suspend mode.
+    //
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_SUSP;
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // No action
+    //
+    break;
+
+  case EfiUsbPortReset:
+    PortSC &= 0xfff5;
+    PortSC &= ~USBPORTSC_PR;
+    break;
+
+  case EfiUsbPortConnectChange:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_CSC;
+    break;
+
+  case EfiUsbPortEnableChange:
+    PortSC &= 0xfff5;
+    PortSC |= USBPORTSC_PEDC;
+    break;
+
+  case EfiUsbPortSuspendChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  case EfiUsbPortResetChange:
+    //
+    // Root hub does not support this
+    //
+    break;
+
+  default:
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhciWriteReg (Uhc->PciIo, Offset, PortSC);
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Submits control transfer to a target USB device accroding to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Data direction of the Data stage in control transfer.
+  @param  Data                   Data to transmit/receive in data stage.
+  @param  DataLength             Length of the data.
+  @param  TimeOut                Maximum time, in microseconds, for transfer to complete.
+  @param  Translator             Transaction translator to be used by this device.
+  @param  TransferResult         Variable to receive the transfer result.
+
+  @return EFI_SUCCESS            The control transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2ControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                 *This,
+  IN     UINT8                                DeviceAddress,
+  IN     UINT8                                DeviceSpeed,
+  IN     UINTN                                MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST               *Request,
+  IN     EFI_USB_DATA_DIRECTION               TransferDirection,
+  IN OUT VOID                                 *Data,
+  IN OUT UINTN                                *DataLength,
+  IN     UINTN                                TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR   *Translator,
+  OUT    UINT32                               *TransferResult
+  )
+{
+  USB_HC_DEV              *Uhc;
+  UHCI_TD_SW              *TDs;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UHCI_QH_RESULT          QhResult;
+  UINT8                   PktId;
+  UINT8                   *RequestPhy;
+  VOID                    *RequestMap;
+  UINT8                   *DataPhy;
+  VOID                    *DataMap;
+  BOOLEAN                 IsSlowDevice;
+  UINTN                   TransferDataLength;
+
+  Uhc         = UHC_FROM_USB2_HC_PROTO (This);
+  TDs         = NULL;
+  DataPhy     = NULL;
+  DataMap     = NULL;
+  RequestPhy  = NULL;
+  RequestMap  = NULL;
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  //
+  // Parameters Checking
+  //
+  if (Request == NULL || TransferResult == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsSlowDevice && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8) &&  (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) && (Data == NULL || DataLength == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TransferDirection == EfiUsbNoData) {
+    TransferDataLength = 0;
+  } else {
+    TransferDataLength = *DataLength;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  //
+  // If errors exist that cause host controller halt,
+  // clear status then return EFI_DEVICE_ERROR.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the Request and data for bus master access,
+  // then create a list of TD for this transfer
+  //
+  Status = UhciMapUserRequest (Uhc, Request, &RequestPhy, &RequestMap);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = UhciMapUserData (Uhc, TransferDirection, Data, DataLength, &PktId, &DataPhy, &DataMap);
+
+  if (EFI_ERROR (Status)) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, RequestMap);
+    goto ON_EXIT;
+  }
+
+  TDs = UhciCreateCtrlTds (
+          Uhc,
+          DeviceAddress,
+          PktId,
+          (UINT8*)Request,
+          RequestPhy,
+          (UINT8*)Data,
+          DataPhy,
+          TransferDataLength,
+          (UINT8) MaximumPacketLength,
+          IsSlowDevice
+          );
+
+  if (TDs == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto UNMAP_DATA;
+  }
+
+  //
+  // According to the speed of the end point, link
+  // the TD to corrosponding queue head, then check
+  // the execution result
+  //
+  UhciLinkTdToQh (Uhc, Uhc->CtrlQh, TDs);
+  Status = UhciExecuteTransfer (Uhc, Uhc->CtrlQh, TDs, TimeOut, IsSlowDevice, &QhResult);
+  UhciUnlinkTdFromQh (Uhc->CtrlQh, TDs);
+
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+
+  if (DataLength != NULL) {
+    *DataLength = QhResult.Complete;
+  }
+
+  UhciDestoryTds (Uhc, TDs);
+
+UNMAP_DATA:
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+  Uhc->PciIo->Unmap (Uhc->PciIo, RequestMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber      Number of data buffers prepared for the transfer.
+  @param  Data                   Array of pointers to the buffers of data.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  TimeOut                Maximum time out, in microseconds.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  TransferResult         Variable to receive transfer result.
+
+  @return EFI_SUCCESS            The bulk transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2BulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     UINT8                              DataBuffersNumber,
+  IN OUT VOID                               *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                              *DataLength,
+  IN OUT UINT8                              *DataToggle,
+  IN     UINTN                              TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  OUT    UINT32                             *TransferResult
+  )
+{
+  EFI_USB_DATA_DIRECTION  Direction;
+  EFI_TPL                 OldTpl;
+  USB_HC_DEV              *Uhc;
+  UHCI_TD_SW              *TDs;
+  UHCI_QH_SW              *BulkQh;
+  UHCI_QH_RESULT          QhResult;
+  EFI_STATUS              Status;
+  UINT8                   PktId;
+  UINT8                   *DataPhy;
+  VOID                    *DataMap;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  DataPhy = NULL;
+  DataMap = NULL;
+
+  if (DeviceSpeed == EFI_USB_SPEED_LOW) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DataLength == NULL) || (*DataLength == 0) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8) && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_OUT_OF_RESOURCES;
+
+  //
+  // If has errors that cause host controller halt,
+  // then return EFI_DEVICE_ERROR directly.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the source data buffer for bus master access,
+  // then create a list of TDs
+  //
+  if ((EndPointAddress & 0x80) != 0) {
+    Direction = EfiUsbDataIn;
+  } else {
+    Direction = EfiUsbDataOut;
+  }
+
+  Status = UhciMapUserData (Uhc, Direction, *Data, DataLength, &PktId, &DataPhy, &DataMap);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = EFI_OUT_OF_RESOURCES;
+  TDs    = UhciCreateBulkOrIntTds (
+             Uhc,
+             DeviceAddress,
+             EndPointAddress,
+             PktId,
+             (UINT8 *)*Data,
+             DataPhy,
+             *DataLength,
+             DataToggle,
+             (UINT8) MaximumPacketLength,
+             FALSE
+             );
+
+  if (TDs == NULL) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+    goto ON_EXIT;
+  }
+
+
+  //
+  // Link the TDs to bulk queue head. According to the platfore
+  // defintion of UHCI_NO_BW_RECLAMATION, BulkQh is either configured
+  // to do full speed bandwidth reclamation or not.
+  //
+  BulkQh = Uhc->BulkQh;
+
+  UhciLinkTdToQh (Uhc, BulkQh, TDs);
+  Status = UhciExecuteTransfer (Uhc, BulkQh, TDs, TimeOut, FALSE, &QhResult);
+  UhciUnlinkTdFromQh (BulkQh, TDs);
+
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+  *DataToggle     = QhResult.NextToggle;
+  *DataLength     = QhResult.Complete;
+
+  UhciDestoryTds (Uhc, TDs);
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device according to UEFI 2.0 spec.
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  IsNewTransfer          If TRUE, submit a new transfer, if FALSE cancel old transfer.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  PollingInterval        Interrupt poll rate in milliseconds.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  CallBackFunction       Function to call periodically.
+  @param  Context                User context.
+
+  @return EFI_SUCCESS            Transfer was submitted.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_OUT_OF_RESOURCES   Failed due to a lack of resources.
+  @return EFI_DEVICE_ERROR       Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2AsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     BOOLEAN                            IsNewTransfer,
+  IN OUT UINT8                              *DataToggle,
+  IN     UINTN                              PollingInterval,
+  IN     UINTN                              DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK    CallBackFunction,
+  IN     VOID                               *Context
+  )
+{
+  USB_HC_DEV          *Uhc;
+  BOOLEAN             IsSlowDevice;
+  UHCI_QH_SW          *Qh;
+  UHCI_TD_SW          *IntTds;
+  EFI_TPL             OldTpl;
+  EFI_STATUS          Status;
+  UINT8               *DataPtr;
+  UINT8               *DataPhy;
+  UINT8               PktId;
+
+  Uhc       = UHC_FROM_USB2_HC_PROTO (This);
+  Qh        = NULL;
+  IntTds    = NULL;
+  DataPtr   = NULL;
+  DataPhy   = NULL;
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  if ((EndPointAddress & 0x80) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Delete Async interrupt transfer request
+  //
+  if (!IsNewTransfer) {
+    OldTpl = gBS->RaiseTPL (UHCI_TPL);
+    Status = UhciRemoveAsyncReq (Uhc, DeviceAddress, EndPointAddress, DataToggle);
+
+    gBS->RestoreTPL (OldTpl);
+    return Status;
+  }
+
+  if (PollingInterval < 1 || PollingInterval > 255) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataLength == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If has errors that cause host controller halt,
+  // then return EFI_DEVICE_ERROR directly.
+  //
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((EndPointAddress & 0x80) == 0) {
+    PktId = OUTPUT_PACKET_ID;
+  } else {
+    PktId = INPUT_PACKET_ID;
+  }
+
+  //
+  // Allocate and map source data buffer for bus master access.
+  //
+  DataPtr = UsbHcAllocateMem (Uhc->MemPool, DataLength);
+
+  if (DataPtr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DataPhy = (UINT8 *) (UINTN) UsbHcGetPciAddressForHostMem (Uhc->MemPool, DataPtr, DataLength);
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  Qh = UhciCreateQh (Uhc, PollingInterval);
+
+  if (Qh == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_DATA;
+  }
+
+  IntTds = UhciCreateBulkOrIntTds (
+             Uhc,
+             DeviceAddress,
+             EndPointAddress,
+             PktId,
+             DataPtr,
+             DataPhy,
+             DataLength,
+             DataToggle,
+             (UINT8) MaximumPacketLength,
+             IsSlowDevice
+             );
+
+  if (IntTds == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto DESTORY_QH;
+  }
+
+  UhciLinkTdToQh (Uhc, Qh, IntTds);
+
+  //
+  // Save QH-TD structures to async Interrupt transfer list,
+  // for monitor interrupt transfer execution routine use.
+  //
+  Status = UhciCreateAsyncReq (
+             Uhc,
+             Qh,
+             IntTds,
+             DeviceAddress,
+             EndPointAddress,
+             DataLength,
+             PollingInterval,
+             DataPtr,
+             CallBackFunction,
+             Context,
+             IsSlowDevice
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto DESTORY_QH;
+  }
+
+  UhciLinkQhToFrameList (Uhc, Qh);
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+DESTORY_QH:
+  UsbHcFreeMem (Uhc->MemPool, Qh, sizeof (UHCI_QH_SW));
+
+FREE_DATA:
+  UsbHcFreeMem (Uhc->MemPool, DataPtr, DataLength);
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device according to UEFI 2.0 spec.
+
+
+  @param  This                   A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress          Target device address.
+  @param  EndPointAddress        Endpoint number and direction.
+  @param  DeviceSpeed            Device speed.
+  @param  MaximumPacketLength    Maximum packet size of the target endpoint.
+  @param  Data                   Array of pointers to the buffers of data.
+  @param  DataLength             On input, size of the data buffer, On output,
+                                 actually transferred data size.
+  @param  DataToggle             On input, data toggle to use; On output, next data toggle.
+  @param  TimeOut                Maximum time out, in microseconds.
+  @param  Translator             A pointr to the transaction translator data.
+  @param  TransferResult         Variable to receive transfer result.
+
+  @return EFI_SUCCESS            The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES   Failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @return EFI_TIMEOUT            Failed due to timeout.
+  @return EFI_DEVICE_ERROR       Failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2SyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                      *This,
+  IN     UINT8                                     DeviceAddress,
+  IN     UINT8                                     EndPointAddress,
+  IN     UINT8                                     DeviceSpeed,
+  IN     UINTN                                     MaximumPacketLength,
+  IN OUT VOID                                      *Data,
+  IN OUT UINTN                                     *DataLength,
+  IN OUT UINT8                                     *DataToggle,
+  IN     UINTN                                     TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR        *Translator,
+  OUT    UINT32                                    *TransferResult
+  )
+{
+  EFI_STATUS          Status;
+  USB_HC_DEV          *Uhc;
+  UHCI_TD_SW          *TDs;
+  UHCI_QH_RESULT      QhResult;
+  EFI_TPL             OldTpl;
+  UINT8               *DataPhy;
+  VOID                *DataMap;
+  UINT8               PktId;
+  BOOLEAN             IsSlowDevice;
+
+  Uhc     = UHC_FROM_USB2_HC_PROTO (This);
+  DataPhy = NULL;
+  DataMap = NULL;
+  TDs     = NULL;
+
+  if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IsSlowDevice  = (BOOLEAN) ((EFI_USB_SPEED_LOW == DeviceSpeed) ? TRUE : FALSE);
+
+  if ((DataLength == NULL) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataLength == 0) || (MaximumPacketLength > 64)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsSlowDevice && (MaximumPacketLength > 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+
+  UhciAckAllInterrupt (Uhc);
+
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (UHCI_TPL);
+
+  //
+  // Map the source data buffer for bus master access.
+  // Create Tds list, then link it to the UHC's interrupt list
+  //
+  Status = UhciMapUserData (
+             Uhc,
+             EfiUsbDataIn,
+             Data,
+             DataLength,
+             &PktId,
+             &DataPhy,
+             &DataMap
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  TDs = UhciCreateBulkOrIntTds (
+          Uhc,
+          DeviceAddress,
+          EndPointAddress,
+          PktId,
+          (UINT8 *)Data,
+          DataPhy,
+          *DataLength,
+          DataToggle,
+          (UINT8) MaximumPacketLength,
+          IsSlowDevice
+          );
+
+  if (TDs == NULL) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+
+  UhciLinkTdToQh (Uhc, Uhc->SyncIntQh, TDs);
+
+  Status = UhciExecuteTransfer (Uhc, Uhc->SyncIntQh, TDs, TimeOut, IsSlowDevice, &QhResult);
+
+  UhciUnlinkTdFromQh (Uhc->SyncIntQh, TDs);
+  Uhc->PciIo->Flush (Uhc->PciIo);
+
+  *TransferResult = QhResult.Result;
+  *DataToggle     = QhResult.NextToggle;
+  *DataLength     = QhResult.Complete;
+
+  UhciDestoryTds (Uhc, TDs);
+  Uhc->PciIo->Unmap (Uhc->PciIo, DataMap);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device according to UEFI 2.0 spec.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and direction.
+  @param  DeviceSpeed           Device speed.
+  @param  MaximumPacketLength   Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data.
+  @param  DataLength            On input, size of the data buffer, On output,
+                                actually transferred data size.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        Variable to receive transfer result.
+
+  @return EFI_UNSUPPORTED
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2IsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL               *This,
+  IN     UINT8                              DeviceAddress,
+  IN     UINT8                              EndPointAddress,
+  IN     UINT8                              DeviceSpeed,
+  IN     UINTN                              MaximumPacketLength,
+  IN     UINT8                              DataBuffersNumber,
+  IN OUT VOID                               *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                              DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR *Translator,
+  OUT    UINT32                             *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device according to UEFI 2.0 spec.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and direction.
+  @param  DeviceSpeed           Device speed.
+  @param  MaximumPacketLength   Maximum packet size of the target endpoint.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data.
+  @param  DataLength            On input, size of the data buffer, On output,
+                                actually transferred data size.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  IsochronousCallBack   Function to call when the transfer complete.
+  @param  Context               Pass to the call back function as parameter.
+
+  @return EFI_UNSUPPORTED
+
+**/
+EFI_STATUS
+EFIAPI
+Uhci2AsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle      EFI_HANDLE.
+  @param  SystemTable      EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS      Driver is successfully loaded.
+  @return Others           Failed.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gUhciDriverBinding,
+           ImageHandle,
+           &gUhciComponentName,
+           &gUhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has UsbHcProtocol installed will be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS            OpenStatus;
+  EFI_STATUS            Status;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB_CLASSC            UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  OpenStatus = gBS->OpenProtocol (
+                      Controller,
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_BY_DRIVER
+                      );
+
+  if (EFI_ERROR (OpenStatus)) {
+    return OpenStatus;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to UHCI type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) ||
+      (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB) ||
+      (UsbClassCReg.ProgInterface != PCI_IF_UHCI)
+      ) {
+
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+
+}
+
+
+/**
+  Allocate and initialize the empty UHCI device.
+
+  @param  PciIo                  The PCIIO to use.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  The original PCI attributes.
+
+  @return Allocated UHCI device. If err, return NULL.
+
+**/
+USB_HC_DEV *
+UhciAllocateDev (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB_HC_DEV  *Uhc;
+  EFI_STATUS  Status;
+
+  Uhc = AllocateZeroPool (sizeof (USB_HC_DEV));
+
+  if (Uhc == NULL) {
+    return NULL;
+  }
+
+  //
+  // This driver supports both USB_HC_PROTOCOL and USB2_HC_PROTOCOL.
+  // USB_HC_PROTOCOL is for EFI 1.1 backward compability.
+  //
+  Uhc->Signature                        = USB_HC_DEV_SIGNATURE;
+  Uhc->Usb2Hc.GetCapability             = Uhci2GetCapability;
+  Uhc->Usb2Hc.Reset                     = Uhci2Reset;
+  Uhc->Usb2Hc.GetState                  = Uhci2GetState;
+  Uhc->Usb2Hc.SetState                  = Uhci2SetState;
+  Uhc->Usb2Hc.ControlTransfer           = Uhci2ControlTransfer;
+  Uhc->Usb2Hc.BulkTransfer              = Uhci2BulkTransfer;
+  Uhc->Usb2Hc.AsyncInterruptTransfer    = Uhci2AsyncInterruptTransfer;
+  Uhc->Usb2Hc.SyncInterruptTransfer     = Uhci2SyncInterruptTransfer;
+  Uhc->Usb2Hc.IsochronousTransfer       = Uhci2IsochronousTransfer;
+  Uhc->Usb2Hc.AsyncIsochronousTransfer  = Uhci2AsyncIsochronousTransfer;
+  Uhc->Usb2Hc.GetRootHubPortStatus      = Uhci2GetRootHubPortStatus;
+  Uhc->Usb2Hc.SetRootHubPortFeature     = Uhci2SetRootHubPortFeature;
+  Uhc->Usb2Hc.ClearRootHubPortFeature   = Uhci2ClearRootHubPortFeature;
+  Uhc->Usb2Hc.MajorRevision             = 0x1;
+  Uhc->Usb2Hc.MinorRevision             = 0x1;
+
+  Uhc->PciIo                 = PciIo;
+  Uhc->DevicePath            = DevicePath;
+  Uhc->OriginalPciAttributes = OriginalPciAttributes;
+  Uhc->MemPool               = UsbHcInitMemPool (PciIo, TRUE, 0);
+
+  if (Uhc->MemPool == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  InitializeListHead (&Uhc->AsyncIntList);
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  UhciMonitorAsyncReqList,
+                  Uhc,
+                  &Uhc->AsyncIntMonitor
+                  );
+
+  if (EFI_ERROR (Status)) {
+    UsbHcFreeMemPool (Uhc->MemPool);
+    goto ON_ERROR;
+  }
+
+  return Uhc;
+
+ON_ERROR:
+  FreePool (Uhc);
+  return NULL;
+}
+
+
+/**
+  Free the UHCI device and release its associated resources.
+
+  @param  Uhc     The UHCI device to release.
+
+**/
+VOID
+UhciFreeDev (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  if (Uhc->AsyncIntMonitor != NULL) {
+    gBS->CloseEvent (Uhc->AsyncIntMonitor);
+  }
+
+  if (Uhc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Uhc->ExitBootServiceEvent);
+  }
+  
+  if (Uhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Uhc->MemPool);
+  }
+
+  if (Uhc->CtrlNameTable != NULL) {
+    FreeUnicodeStringTable (Uhc->CtrlNameTable);
+  }
+
+  FreePool (Uhc);
+}
+
+
+/**
+  Uninstall all Uhci Interface.
+
+  @param  Controller           Controller handle.
+  @param  This                 Protocol instance pointer.
+
+**/
+VOID
+UhciCleanDevUp (
+  IN  EFI_HANDLE           Controller,
+  IN  EFI_USB2_HC_PROTOCOL *This
+  )
+{
+  USB_HC_DEV          *Uhc;
+  EFI_STATUS          Status;
+
+  //
+  // Uninstall the USB_HC and USB_HC2 protocol, then disable the controller
+  //
+  Uhc = UHC_FROM_USB2_HC_PROTO (This);
+
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  &Uhc->Usb2Hc
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+  UhciFreeAllAsyncReq (Uhc);
+  UhciDestoryFrameList (Uhc);
+
+  //
+  // Restore original PCI attributes
+  //
+  Uhc->PciIo->Attributes (
+                  Uhc->PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Uhc->OriginalPciAttributes,
+                  NULL
+                  );
+
+  UhciFreeDev (Uhc);
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+UhcExitBootService (
+  EFI_EVENT                      Event,
+  VOID                           *Context
+  )
+{
+  USB_HC_DEV   *Uhc;
+
+  Uhc = (USB_HC_DEV *) Context;
+
+  //
+  // Stop the Host Controller
+  //
+  UhciStopHc (Uhc, UHC_GENERIC_TIMEOUT);
+
+  //
+  // Reset the Host Controller
+  //
+  UhciSetRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_HCRESET);
+  gBS->Stall (UHC_ROOT_PORT_RECOVERY_STALL);
+}
+
+/**
+  Starting the Usb UHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      This driver does not support this device.
+  @retval EFI_DEVICE_ERROR     This driver cannot be started due to device Error.
+                               EFI_OUT_OF_RESOURCES- Failed due to resource shortage.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_PCI_IO_PROTOCOL *PciIo;
+  USB_HC_DEV          *Uhc;
+  UINT64              Supports;
+  UINT64              OriginalPciAttributes;
+  BOOLEAN             PciAttributesSaved;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open PCIIO, then enable the EHC device and turn off emulation
+  //
+  Uhc = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  //
+  // Robustnesss improvement such as for UoL
+  // Default is not required.
+  //
+  if (FeaturePcdGet (PcdTurnOffUsbLegacySupport)) {
+    UhciTurnOffUsbEmulation (PciIo);
+  }
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+
+  Uhc = UhciAllocateDev (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Uhc == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Allocate and Init Host Controller's Frame List Entry
+  //
+  Status = UhciInitFrameList (Uhc);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_UHC;
+  }
+
+  Status = gBS->SetTimer (
+                  Uhc->AsyncIntMonitor,
+                  TimerPeriodic,
+                  UHC_ASYNC_POLL_INTERVAL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_UHC;
+  }
+
+  //
+  // Install USB2_HC_PROTOCOL
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  &Uhc->Usb2Hc,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_UHC;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  UhcExitBootService,
+                  Uhc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Uhc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto UNINSTALL_USBHC;
+  }
+
+  //
+  // Install the component name protocol
+  //
+  Uhc->CtrlNameTable = NULL;
+
+  AddUnicodeString2 (
+    "eng",
+    gUhciComponentName.SupportedLanguages,
+    &Uhc->CtrlNameTable,
+    L"Usb Universal Host Controller",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUhciComponentName2.SupportedLanguages,
+    &Uhc->CtrlNameTable,
+    L"Usb Universal Host Controller",
+    FALSE
+    );
+
+
+  //
+  // Start the UHCI hardware, also set its reclamation point to 64 bytes
+  //
+  UhciWriteReg (Uhc->PciIo, USBCMD_OFFSET, USBCMD_RS | USBCMD_MAXP);
+
+  return EFI_SUCCESS;
+  
+UNINSTALL_USBHC:
+  gBS->UninstallMultipleProtocolInterfaces (
+         Controller,
+         &gEfiUsb2HcProtocolGuid,
+         &Uhc->Usb2Hc,
+         NULL
+         );
+
+FREE_UHC:
+  UhciFreeDev (Uhc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS
+  @return others
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_STATUS            Status;
+
+   Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhciCleanDevUp (Controller, Usb2Hc);
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiPciIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h
new file mode 100644
index 0000000000..8635d8231c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/Uhci.h
@@ -0,0 +1,221 @@
+/** @file
+
+  The definition for UHCI driver model and HC protocol routines.
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _EFI_UHCI_H_
+#define _EFI_UHCI_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/UsbHostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB_HC_DEV  USB_HC_DEV;
+
+#include "UsbHcMem.h"
+#include "UhciQueue.h"
+#include "UhciReg.h"
+#include "UhciSched.h"
+#include "UhciDebug.h"
+#include "ComponentName.h"
+
+//
+// UHC timeout experience values
+//
+
+#define UHC_1_MICROSECOND             1
+#define UHC_1_MILLISECOND             (1000 * UHC_1_MICROSECOND)
+#define UHC_1_SECOND                  (1000 * UHC_1_MILLISECOND)
+
+//
+// UHCI register operation timeout, set by experience
+//
+#define UHC_GENERIC_TIMEOUT           UHC_1_SECOND
+
+//
+// Wait for force global resume(FGR) complete, refers to
+// specification[UHCI11-2.1.1]
+//
+#define UHC_FORCE_GLOBAL_RESUME_STALL (20 * UHC_1_MILLISECOND)
+
+//
+// Wait for roothub port reset and recovery, reset stall
+// is set by experience, and recovery stall refers to
+// specification[UHCI11-2.1.1]
+//
+#define UHC_ROOT_PORT_RESET_STALL     (50 * UHC_1_MILLISECOND)
+#define UHC_ROOT_PORT_RECOVERY_STALL  (10 * UHC_1_MILLISECOND)
+
+//
+// Sync and Async transfer polling interval, set by experience,
+// and the unit of Async is 100us.
+//
+#define UHC_SYNC_POLL_INTERVAL        (1 * UHC_1_MILLISECOND)
+#define UHC_ASYNC_POLL_INTERVAL       EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// UHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  UHCI_TPL                     TPL_NOTIFY
+
+#define  USB_HC_DEV_SIGNATURE         SIGNATURE_32 ('u', 'h', 'c', 'i')
+
+#pragma pack(1)
+typedef struct {
+  UINT8               ProgInterface;
+  UINT8               SubClassCode;
+  UINT8               BaseCode;
+} USB_CLASSC;
+#pragma pack()
+
+#define UHC_FROM_USB2_HC_PROTO(This)  CR(This, USB_HC_DEV, Usb2Hc, USB_HC_DEV_SIGNATURE)
+
+//
+// USB_HC_DEV support the UHCI hardware controller. It schedules
+// the asynchronous interrupt transfer with the same method as
+// EHCI: a reversed tree structure. For synchronous interrupt,
+// control and bulk transfer, it uses three static queue head to
+// schedule them. SyncIntQh is for interrupt transfer. LsCtrlQh is
+// for LOW speed control transfer, and FsCtrlBulkQh is for FULL
+// speed control or bulk transfer. This is because FULL speed contrl
+// or bulk transfer can reclaim the unused bandwidth. Some USB
+// device requires this bandwidth reclamation capability.
+//
+struct _USB_HC_DEV {
+  UINT32                    Signature;
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINT64                    OriginalPciAttributes;
+
+  //
+  // Schedule data structures
+  //
+  UINT32                    *FrameBase; // the buffer pointed by this pointer is used to store pci bus address of the QH descriptor.
+  UINT32                    *FrameBaseHostAddr; // the buffer pointed by this pointer is used to store host memory address of the QH descriptor.
+  UHCI_QH_SW                *SyncIntQh;
+  UHCI_QH_SW                *CtrlQh;
+  UHCI_QH_SW                *BulkQh;
+
+  //
+  // Structures to maintain asynchronus interrupt transfers.
+  // When asynchronous interrutp transfer is unlinked from
+  // the frame list, the hardware may still hold a pointer
+  // to it. To synchronize with hardware, its resoureces are
+  // released in two steps using Recycle and RecycleWait.
+  // Check the asynchronous interrupt management routines.
+  //
+  LIST_ENTRY                AsyncIntList;
+  EFI_EVENT                 AsyncIntMonitor;
+  UHCI_ASYNC_REQUEST        *Recycle;
+  UHCI_ASYNC_REQUEST        *RecycleWait;
+
+
+  UINTN                     RootPorts;
+  USBHC_MEM_POOL            *MemPool;
+  EFI_UNICODE_STRING_TABLE  *CtrlNameTable;
+  VOID                      *FrameMapping;
+
+  //
+  // ExitBootServicesEvent is used to stop the EHC DMA operation 
+  // after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+};
+
+extern EFI_DRIVER_BINDING_PROTOCOL   gUhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has UsbHcProtocol installed will be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb UHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      This driver does not support this device.
+  @retval EFI_DEVICE_ERROR     This driver cannot be started due to device Error.
+                               EFI_OUT_OF_RESOURCES- Failed due to resource shortage.
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS
+  @return others
+
+**/
+EFI_STATUS
+EFIAPI
+UhciDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      Controller,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c
new file mode 100644
index 0000000000..9bf53fd411
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.c
@@ -0,0 +1,77 @@
+/** @file
+
+  This file provides the information dump support for Uhci when in debug mode.
+
+Copyright (c) 2007, 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 "Uhci.h"
+
+/**
+  Dump the content of QH structure.
+
+  @param  QhSw    Pointer to software QH structure.
+
+**/
+VOID
+UhciDumpQh (
+  IN UHCI_QH_SW    *QhSw
+  )
+{
+  DEBUG ((EFI_D_VERBOSE, "&QhSw @ 0x%p\n", QhSw));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.NextQh    - 0x%p\n", QhSw->NextQh));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.TDs       - 0x%p\n", QhSw->TDs));
+  DEBUG ((EFI_D_VERBOSE, "QhSw.QhHw:\n"));
+  DEBUG ((EFI_D_VERBOSE, " Horizon  Link - %x\n", QhSw->QhHw.HorizonLink));
+  DEBUG ((EFI_D_VERBOSE, " Vertical Link - %x\n\n", QhSw->QhHw.VerticalLink));
+}
+
+
+/**
+  Dump the content of TD structure.
+
+  @param  TdSw    Pointer to software TD structure.
+
+**/
+VOID
+UhciDumpTds (
+  IN UHCI_TD_SW           *TdSw
+  )
+{
+  UHCI_TD_SW              *CurTdSw;
+
+  CurTdSw = TdSw;
+
+  while (CurTdSw != NULL) {
+    DEBUG ((EFI_D_VERBOSE, "TdSw @ 0x%p\n",           CurTdSw));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.NextTd   - 0x%p\n",  CurTdSw->NextTd));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.DataLen  - %d\n",    CurTdSw->DataLen));
+    DEBUG ((EFI_D_VERBOSE, "TdSw.Data     - 0x%p\n",  CurTdSw->Data));
+    DEBUG ((EFI_D_VERBOSE, "TdHw:\n"));
+    DEBUG ((EFI_D_VERBOSE, " NextLink     - 0x%x\n",  CurTdSw->TdHw.NextLink));
+    DEBUG ((EFI_D_VERBOSE, " ActualLen    - %d\n",    CurTdSw->TdHw.ActualLen));
+    DEBUG ((EFI_D_VERBOSE, " Status       - 0x%x\n",  CurTdSw->TdHw.Status));
+    DEBUG ((EFI_D_VERBOSE, " IOC          - %d\n",    CurTdSw->TdHw.IntOnCpl));
+    DEBUG ((EFI_D_VERBOSE, " IsIsoCh      - %d\n",    CurTdSw->TdHw.IsIsoch));
+    DEBUG ((EFI_D_VERBOSE, " LowSpeed     - %d\n",    CurTdSw->TdHw.LowSpeed));
+    DEBUG ((EFI_D_VERBOSE, " ErrorCount   - %d\n",    CurTdSw->TdHw.ErrorCount));
+    DEBUG ((EFI_D_VERBOSE, " ShortPacket  - %d\n",    CurTdSw->TdHw.ShortPacket));
+    DEBUG ((EFI_D_VERBOSE, " PidCode      - 0x%x\n",  CurTdSw->TdHw.PidCode));
+    DEBUG ((EFI_D_VERBOSE, " DevAddr      - %d\n",    CurTdSw->TdHw.DeviceAddr));
+    DEBUG ((EFI_D_VERBOSE, " EndPoint     - %d\n",    CurTdSw->TdHw.EndPoint));
+    DEBUG ((EFI_D_VERBOSE, " DataToggle   - %d\n",    CurTdSw->TdHw.DataToggle));
+    DEBUG ((EFI_D_VERBOSE, " MaxPacketLen - %d\n",    CurTdSw->TdHw.MaxPacketLen));
+    DEBUG ((EFI_D_VERBOSE, " DataBuffer   - 0x%x\n\n",CurTdSw->TdHw.DataBuffer));
+
+    CurTdSw = CurTdSw->NextTd;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h
new file mode 100644
index 0000000000..abdb64fa36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDebug.h
@@ -0,0 +1,47 @@
+/** @file
+
+  This file contains the definination for host controller debug support routines
+
+Copyright (c) 2007, 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.
+
+**/
+
+#ifndef _EFI_UHCI_DEBUG_H_
+#define _EFI_UHCI_DEBUG_H_
+
+
+/**
+  Dump the content of QH structure.
+
+  @param  QhSw    Pointer to software QH structure.
+
+  @return None.
+
+**/
+VOID
+UhciDumpQh (
+  IN UHCI_QH_SW         *QhSw
+  );
+
+
+/**
+  Dump the content of TD structure.
+
+  @param  TdSw    Pointer to software TD structure.
+
+  @return None.
+
+**/
+VOID
+UhciDumpTds (
+  IN UHCI_TD_SW           *TdSw
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf
new file mode 100644
index 0000000000..9c5ff7b6fc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf
@@ -0,0 +1,86 @@
+## @file
+#  The UhciDxe driver is responsible for managing the behavior of UHCI controller. 
+#  It implements the interfaces of monitoring the status of all ports and transferring
+#  Control, Bulk, Interrupt and Isochronous requests to Usb1.x device
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UhciDxe
+  MODULE_UNI_FILE                = UhciDxe.uni
+  FILE_GUID                      = 2FB92EFA-2EE0-4bae-9EB6-7464125E1EF7
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  
+  ENTRY_POINT                    = UhciDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gUhciDriverBinding                        
+#  COMPONENT_NAME                =  gUhciComponentName
+#  COMPONENT_NAME2               =  gUhciComponentName2
+#
+
+[Sources]
+  UhciSched.c
+  UhciDebug.c
+  UsbHcMem.h
+  UhciDebug.h
+  UhciQueue.c
+  UhciReg.c
+  UsbHcMem.c
+  UhciQueue.h
+  Uhci.c
+  Uhci.h
+  UhciReg.h
+  UhciSched.h
+  ComponentName.c
+  ComponentName.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTurnOffUsbLegacySupport  ## CONSUMES
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  PcdLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni
new file mode 100644
index 0000000000..d011d39f1c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni
new file mode 100644
index 0000000000..71d9f6cf77
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c
new file mode 100644
index 0000000000..1a0aa6e636
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.c
@@ -0,0 +1,707 @@
+/** @file
+
+  The UHCI register operation routines.
+
+Copyright (c) 2007 - 2010, 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 "Uhci.h"
+
+
+/**
+  Map address of request structure buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Request            The user request buffer.
+  @param  MappedAddr         Mapped address of request.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user request.
+
+**/
+EFI_STATUS
+UhciMapUserRequest (
+  IN  USB_HC_DEV          *Uhc,
+  IN  OUT VOID            *Request,
+  OUT UINT8               **MappedAddr,
+  OUT VOID                **Map
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Len;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Len    = sizeof (EFI_USB_DEVICE_REQUEST);
+  Status = Uhc->PciIo->Map (
+                         Uhc->PciIo,
+                         EfiPciIoOperationBusMasterRead,
+                         Request,
+                         &Len,
+                         &PhyAddr,
+                         Map
+                         );
+
+  if (!EFI_ERROR (Status)) {
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+  }
+
+  return Status;
+}
+
+
+/**
+  Map address of user data buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Direction          Direction of the data transfer.
+  @param  Data               The user data buffer.
+  @param  Len                Length of the user data.
+  @param  PktId              Packet identificaion.
+  @param  MappedAddr         Mapped address to return.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user data.
+
+**/
+EFI_STATUS
+UhciMapUserData (
+  IN  USB_HC_DEV              *Uhc,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  VOID                    *Data,
+  IN  OUT UINTN               *Len,
+  OUT UINT8                   *PktId,
+  OUT UINT8                   **MappedAddr,
+  OUT VOID                    **Map
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Status = EFI_SUCCESS;
+
+  switch (Direction) {
+  case EfiUsbDataIn:
+    //
+    // BusMasterWrite means cpu read
+    //
+    *PktId = INPUT_PACKET_ID;
+    Status = Uhc->PciIo->Map (
+                           Uhc->PciIo,
+                           EfiPciIoOperationBusMasterWrite,
+                           Data,
+                           Len,
+                           &PhyAddr,
+                           Map
+                           );
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+    break;
+
+  case EfiUsbDataOut:
+    *PktId = OUTPUT_PACKET_ID;
+    Status = Uhc->PciIo->Map (
+                           Uhc->PciIo,
+                           EfiPciIoOperationBusMasterRead,
+                           Data,
+                           Len,
+                           &PhyAddr,
+                           Map
+                           );
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+
+    *MappedAddr = (UINT8 *) (UINTN) PhyAddr;
+    break;
+
+  case EfiUsbNoData:
+    if ((Len != NULL) && (*Len != 0)) {
+      Status    = EFI_INVALID_PARAMETER;
+      goto EXIT;
+    }
+
+    *PktId      = OUTPUT_PACKET_ID;
+    *MappedAddr = NULL;
+    *Map        = NULL;
+    break;
+
+  default:
+    Status      = EFI_INVALID_PARAMETER;
+  }
+
+EXIT:
+  return Status;
+}
+
+
+/**
+  Link the TD To QH.
+
+  @param  Uhc         The UHCI device.
+  @param  Qh          The queue head for the TD to link to.
+  @param  Td          The TD to link.
+
+**/
+VOID
+UhciLinkTdToQh (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Td, sizeof (UHCI_TD_HW));
+
+  ASSERT ((Qh != NULL) && (Td != NULL));
+
+  Qh->QhHw.VerticalLink = QH_VLINK (PhyAddr, FALSE);
+  Qh->TDs               = (VOID *) Td;
+}
+
+
+/**
+  Unlink TD from the QH.
+
+  @param  Qh          The queue head to unlink from.
+  @param  Td          The TD to unlink.
+
+**/
+VOID
+UhciUnlinkTdFromQh (
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  )
+{
+  ASSERT ((Qh != NULL) && (Td != NULL));
+
+  Qh->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+  Qh->TDs               = NULL;
+}
+
+
+/**
+  Append a new TD To the previous TD.
+
+  @param  Uhc         The UHCI device.
+  @param  PrevTd      Previous UHCI_TD_SW to be linked to.
+  @param  ThisTd      TD to link.
+
+**/
+VOID
+UhciAppendTd (
+  IN USB_HC_DEV     *Uhc,
+  IN UHCI_TD_SW     *PrevTd,
+  IN UHCI_TD_SW     *ThisTd
+  )
+{
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, ThisTd, sizeof (UHCI_TD_HW));
+
+  ASSERT ((PrevTd != NULL) && (ThisTd != NULL));
+
+  PrevTd->TdHw.NextLink = TD_LINK (PhyAddr, TRUE, FALSE);
+  PrevTd->NextTd        = (VOID *) ThisTd;
+}
+
+
+/**
+  Delete a list of TDs.
+
+  @param  Uhc         The UHCI device.
+  @param  FirstTd     TD link list head.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_TD_SW           *FirstTd
+  )
+{
+  UHCI_TD_SW            *NextTd;
+  UHCI_TD_SW            *ThisTd;
+
+  NextTd = FirstTd;
+
+  while (NextTd != NULL) {
+    ThisTd  = NextTd;
+    NextTd  = ThisTd->NextTd;
+    UsbHcFreeMem (Uhc->MemPool, ThisTd, sizeof (UHCI_TD_SW));
+  }
+}
+
+
+/**
+  Create an initialize a new queue head.
+
+  @param  Uhc         The UHCI device.
+  @param  Interval    The polling interval for the queue.
+
+  @return The newly created queue header.
+
+**/
+UHCI_QH_SW *
+UhciCreateQh (
+  IN  USB_HC_DEV        *Uhc,
+  IN  UINTN             Interval
+  )
+{
+  UHCI_QH_SW            *Qh;
+
+  Qh = UsbHcAllocateMem (Uhc->MemPool, sizeof (UHCI_QH_SW));
+
+  if (Qh == NULL) {
+    return NULL;
+  }
+
+  Qh->QhHw.HorizonLink  = QH_HLINK (NULL, TRUE);
+  Qh->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+  Qh->Interval          = UhciConvertPollRate(Interval);
+  Qh->TDs               = NULL;
+  Qh->NextQh            = NULL;
+
+  return Qh;
+}
+
+
+/**
+  Create and intialize a TD.
+
+  @param  Uhc         The UHCI device.
+
+  @return The newly allocated and initialized TD.
+
+**/
+UHCI_TD_SW *
+UhciCreateTd (
+  IN  USB_HC_DEV          *Uhc
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td     = UsbHcAllocateMem (Uhc->MemPool, sizeof (UHCI_TD_SW));
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink = TD_LINK (NULL, FALSE, TRUE);
+  Td->NextTd        = NULL;
+  Td->Data          = NULL;
+  Td->DataLen       = 0;
+
+  return Td;
+}
+
+
+/**
+  Create and initialize a TD for Setup Stage of a control transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  Request     A pointer to cpu memory address of Device request.
+  @param  RequestPhy  A pointer to pci memory address of Device request.
+  @param  IsLow       Full speed or low speed.
+
+  @return The created setup Td Pointer.
+
+**/
+UHCI_TD_SW *
+UhciCreateSetupTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               *Request,
+  IN  UINT8               *RequestPhy,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status      |= USBTD_ACTIVE;
+  Td->TdHw.DataToggle   = 0;
+  Td->TdHw.EndPoint     = 0;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.MaxPacketLen = (UINT32) (sizeof (EFI_USB_DEVICE_REQUEST) - 1);
+  Td->TdHw.PidCode      = SETUP_PACKET_ID;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) RequestPhy;
+
+  Td->Data              = Request;
+  Td->DataLen           = (UINT16) sizeof (EFI_USB_DEVICE_REQUEST);
+
+  return Td;
+}
+
+
+/**
+  Create a TD for data.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  Endpoint    Endpoint number.
+  @param  DataPtr     A pointer to cpu memory address of Data buffer.
+  @param  DataPhyPtr  A pointer to pci memory address of Data buffer.
+  @param  Len         Data length.
+  @param  PktId       Packet ID.
+  @param  Toggle      Data toggle value.
+  @param  IsLow       Full speed or low speed.
+
+  @return Data Td pointer if success, otherwise NULL.
+
+**/
+UHCI_TD_SW *
+UhciCreateDataTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               Endpoint,
+  IN  UINT8               *DataPtr,
+  IN  UINT8               *DataPhyPtr,
+  IN  UINTN               Len,
+  IN  UINT8               PktId,
+  IN  UINT8               Toggle,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW  *Td;
+
+  //
+  // Code as length - 1, and the max valid length is 0x500
+  //
+  ASSERT (Len <= 0x500);
+
+  Td  = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status       = USBTD_ACTIVE;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DataToggle   = Toggle & 0x01;
+  Td->TdHw.EndPoint     = Endpoint & 0x0F;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.MaxPacketLen = (UINT32) (Len - 1);
+  Td->TdHw.PidCode      = (UINT8) PktId;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) DataPhyPtr;
+
+  Td->Data              = DataPtr;
+  Td->DataLen           = (UINT16) Len;
+
+  return Td;
+}
+
+
+/**
+  Create TD for the Status Stage of control transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DevAddr     Device address.
+  @param  PktId       Packet ID.
+  @param  IsLow       Full speed or low speed.
+
+  @return Status Td Pointer.
+
+**/
+UHCI_TD_SW *
+UhciCreateStatusTd (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               PktId,
+  IN  BOOLEAN             IsLow
+  )
+{
+  UHCI_TD_SW              *Td;
+
+  Td = UhciCreateTd (Uhc);
+
+  if (Td == NULL) {
+    return NULL;
+  }
+
+  Td->TdHw.NextLink     = TD_LINK (NULL, TRUE, TRUE);
+  Td->TdHw.ShortPacket  = FALSE;
+  Td->TdHw.IsIsoch      = FALSE;
+  Td->TdHw.IntOnCpl     = FALSE;
+  Td->TdHw.ErrorCount   = 0x03;
+  Td->TdHw.Status      |= USBTD_ACTIVE;
+  Td->TdHw.MaxPacketLen = 0x7FF;      //0x7FF: there is no data (refer to UHCI spec)
+  Td->TdHw.DataToggle   = 1;
+  Td->TdHw.EndPoint     = 0;
+  Td->TdHw.LowSpeed     = IsLow ? 1 : 0;
+  Td->TdHw.DeviceAddr   = DevAddr & 0x7F;
+  Td->TdHw.PidCode      = (UINT8) PktId;
+  Td->TdHw.DataBuffer   = (UINT32) (UINTN) NULL;
+
+  Td->Data              = NULL;
+  Td->DataLen           = 0;
+
+  return Td;
+}
+
+
+/**
+  Create Tds list for Control Transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DeviceAddr  The device address.
+  @param  DataPktId   Packet Identification of Data Tds.
+  @param  Request     A pointer to cpu memory address of request structure buffer to transfer.
+  @param  RequestPhy  A pointer to pci memory address of request structure buffer to transfer.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  MaxPacket   Maximum packet size for control transfer.
+  @param  IsLow       Full speed or low speed.
+
+  @return The Td list head for the control transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateCtrlTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DeviceAddr,
+  IN UINT8                DataPktId,
+  IN UINT8                *Request,
+  IN UINT8                *RequestPhy,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  )
+{
+  UHCI_TD_SW                *SetupTd;
+  UHCI_TD_SW                *FirstDataTd;
+  UHCI_TD_SW                *DataTd;
+  UHCI_TD_SW                *PrevDataTd;
+  UHCI_TD_SW                *StatusTd;
+  UINT8                     DataToggle;
+  UINT8                     StatusPktId;
+  UINTN                     ThisTdLen;
+
+
+  DataTd      = NULL;
+  SetupTd     = NULL;
+  FirstDataTd = NULL;
+  PrevDataTd  = NULL;
+  StatusTd    = NULL;
+
+  //
+  // Create setup packets for the transfer
+  //
+  SetupTd = UhciCreateSetupTd (Uhc, DeviceAddr, Request, RequestPhy, IsLow);
+
+  if (SetupTd == NULL) {
+    return NULL;
+  }
+
+  //
+  // Create data packets for the transfer
+  //
+  DataToggle = 1;
+
+  while (DataLen > 0) {
+    //
+    // PktSize is the data load size in each Td.
+    //
+    ThisTdLen = (DataLen > MaxPacket ? MaxPacket : DataLen);
+
+    DataTd = UhciCreateDataTd (
+               Uhc,
+               DeviceAddr,
+               0,
+               Data,  //cpu memory address
+               DataPhy, //Pci memory address
+               ThisTdLen,
+               DataPktId,
+               DataToggle,
+               IsLow
+               );
+
+    if (DataTd == NULL) {
+      goto FREE_TD;
+    }
+
+    if (FirstDataTd == NULL) {
+      FirstDataTd         = DataTd;
+      FirstDataTd->NextTd = NULL;
+    } else {
+      UhciAppendTd (Uhc, PrevDataTd, DataTd);
+    }
+
+    DataToggle ^= 1;
+    PrevDataTd = DataTd;
+    Data += ThisTdLen;
+    DataPhy += ThisTdLen;
+    DataLen -= ThisTdLen;
+  }
+
+  //
+  // Status packet is on the opposite direction to data packets
+  //
+  if (OUTPUT_PACKET_ID == DataPktId) {
+    StatusPktId = INPUT_PACKET_ID;
+  } else {
+    StatusPktId = OUTPUT_PACKET_ID;
+  }
+
+  StatusTd = UhciCreateStatusTd (Uhc, DeviceAddr, StatusPktId, IsLow);
+
+  if (StatusTd == NULL) {
+    goto FREE_TD;
+  }
+
+  //
+  // Link setup Td -> data Tds -> status Td together
+  //
+  if (FirstDataTd != NULL) {
+    UhciAppendTd (Uhc, SetupTd, FirstDataTd);
+    UhciAppendTd (Uhc, PrevDataTd, StatusTd);
+  } else {
+    UhciAppendTd (Uhc, SetupTd, StatusTd);
+  }
+
+  return SetupTd;
+
+FREE_TD:
+  if (SetupTd != NULL) {
+    UhciDestoryTds (Uhc, SetupTd);
+  }
+
+  if (FirstDataTd != NULL) {
+    UhciDestoryTds (Uhc, FirstDataTd);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Create Tds list for Bulk/Interrupt Transfer.
+
+  @param  Uhc         USB_HC_DEV.
+  @param  DevAddr     Address of Device.
+  @param  EndPoint    Endpoint Number.
+  @param  PktId       Packet Identification of Data Tds.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  DataToggle  Data Toggle Pointer.
+  @param  MaxPacket   Maximum packet size for Bulk/Interrupt transfer.
+  @param  IsLow       Is Low Speed Device.
+
+  @return The Tds list head for the bulk transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateBulkOrIntTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DevAddr,
+  IN UINT8                EndPoint,
+  IN UINT8                PktId,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN OUT UINT8            *DataToggle,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  )
+{
+  UHCI_TD_SW              *DataTd;
+  UHCI_TD_SW              *FirstDataTd;
+  UHCI_TD_SW              *PrevDataTd;
+  UINTN                   ThisTdLen;
+
+  DataTd      = NULL;
+  FirstDataTd = NULL;
+  PrevDataTd  = NULL;
+
+  //
+  // Create data packets for the transfer
+  //
+  while (DataLen > 0) {
+    //
+    // PktSize is the data load size that each Td.
+    //
+    ThisTdLen = DataLen;
+
+    if (DataLen > MaxPacket) {
+      ThisTdLen = MaxPacket;
+    }
+
+    DataTd = UhciCreateDataTd (
+               Uhc,
+               DevAddr,
+               EndPoint,
+               Data,
+               DataPhy,
+               ThisTdLen,
+               PktId,
+               *DataToggle,
+               IsLow
+               );
+
+    if (DataTd == NULL) {
+      goto FREE_TD;
+    }
+
+    if (PktId == INPUT_PACKET_ID) {
+      DataTd->TdHw.ShortPacket = TRUE;
+    }
+
+    if (FirstDataTd == NULL) {
+      FirstDataTd         = DataTd;
+      FirstDataTd->NextTd = NULL;
+    } else {
+      UhciAppendTd (Uhc, PrevDataTd, DataTd);
+    }
+
+    *DataToggle ^= 1;
+    PrevDataTd   = DataTd;
+    Data        += ThisTdLen;
+    DataPhy     += ThisTdLen;
+    DataLen     -= ThisTdLen;
+  }
+
+  return FirstDataTd;
+
+FREE_TD:
+  if (FirstDataTd != NULL) {
+    UhciDestoryTds (Uhc, FirstDataTd);
+  }
+
+  return NULL;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h
new file mode 100644
index 0000000000..9ddca38302
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciQueue.h
@@ -0,0 +1,272 @@
+/** @file
+
+  The definition for UHCI register operation routines.
+
+Copyright (c) 2007, 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.
+
+**/
+
+#ifndef _EFI_UHCI_QUEUE_H_
+#define _EFI_UHCI_QUEUE_H_
+
+//
+// Macroes used to set various links in UHCI's driver.
+// In this UHCI driver, QH's horizontal link always pointers to other QH,
+// and its vertical link always pointers to TD. TD's next pointer always
+// pointers to other sibling TD. Frame link always pointers to QH because
+// ISO transfer isn't supported.
+//
+// We should use UINT32 to access these pointers to void race conditions
+// with hardware.
+//
+#define QH_HLINK(Pointer, Terminate)  \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | 0x02 | ((Terminate) ? 0x01 : 0))
+
+#define QH_VLINK(Pointer, Terminate)  \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | ((Terminate) ? 0x01 : 0))
+
+#define TD_LINK(Pointer, VertFirst, Terminate) \
+        (((UINT32) ((UINTN) (Pointer)) & 0xFFFFFFF0) | \
+         ((VertFirst) ? 0x04 : 0) | ((Terminate) ? 0x01 : 0))
+
+#define LINK_TERMINATED(Link) (((Link) & 0x01) != 0)
+
+#define UHCI_ADDR(QhOrTd)     ((VOID *) (UINTN) ((QhOrTd) & 0xFFFFFFF0))
+
+#pragma pack(1)
+//
+// Both links in QH has this internal structure:
+//   Next pointer: 28, Reserved: 2, NextIsQh: 1, Terminate: 1
+// This is the same as frame list entry.
+//
+typedef struct {
+  UINT32              HorizonLink;
+  UINT32              VerticalLink;
+} UHCI_QH_HW;
+
+//
+// Next link in TD has this internal structure:
+//   Next pointer: 28, Reserved: 1, Vertical First: 1, NextIsQh: 1, Terminate: 1
+//
+typedef struct {
+  UINT32              NextLink;
+  UINT32              ActualLen   : 11;
+  UINT32              Reserved1   : 5;
+  UINT32              Status      : 8;
+  UINT32              IntOnCpl    : 1;
+  UINT32              IsIsoch     : 1;
+  UINT32              LowSpeed    : 1;
+  UINT32              ErrorCount  : 2;
+  UINT32              ShortPacket : 1;
+  UINT32              Reserved2   : 2;
+  UINT32              PidCode     : 8;
+  UINT32              DeviceAddr  : 7;
+  UINT32              EndPoint    : 4;
+  UINT32              DataToggle  : 1;
+  UINT32              Reserved3   : 1;
+  UINT32              MaxPacketLen: 11;
+  UINT32              DataBuffer;
+} UHCI_TD_HW;
+#pragma pack()
+
+typedef struct _UHCI_TD_SW  UHCI_TD_SW;
+typedef struct _UHCI_QH_SW  UHCI_QH_SW;
+
+struct _UHCI_QH_SW {
+  UHCI_QH_HW        QhHw;
+  UHCI_QH_SW        *NextQh;
+  UHCI_TD_SW        *TDs;
+  UINTN             Interval;
+};
+
+struct _UHCI_TD_SW {
+  UHCI_TD_HW        TdHw;
+  UHCI_TD_SW        *NextTd;
+  UINT8             *Data;
+  UINT16            DataLen;
+};
+
+
+/**
+  Link the TD To QH.
+
+  @param  Uhc         The UHCI device.
+  @param  Qh          The queue head for the TD to link to.
+  @param  Td          The TD to link.
+
+**/
+VOID
+UhciLinkTdToQh (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  );
+
+
+/**
+  Unlink TD from the QH.
+
+  @param  Qh          The queue head to unlink from.
+  @param  Td          The TD to unlink.
+
+  @return None.
+
+**/
+VOID
+UhciUnlinkTdFromQh (
+  IN UHCI_QH_SW           *Qh,
+  IN UHCI_TD_SW           *Td
+  );
+
+
+/**
+  Map address of request structure buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Request            The user request buffer.
+  @param  MappedAddr         Mapped address of request.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user request.
+
+**/
+EFI_STATUS
+UhciMapUserRequest (
+  IN  USB_HC_DEV          *Uhc,
+  IN  OUT VOID            *Request,
+  OUT UINT8               **MappedAddr,
+  OUT VOID                **Map
+  );
+
+
+/**
+  Map address of user data buffer.
+
+  @param  Uhc                The UHCI device.
+  @param  Direction          Direction of the data transfer.
+  @param  Data               The user data buffer.
+  @param  Len                Length of the user data.
+  @param  PktId              Packet identificaion.
+  @param  MappedAddr         Mapped address to return.
+  @param  Map                Identificaion of this mapping to return.
+
+  @return EFI_SUCCESS        Success.
+  @return EFI_DEVICE_ERROR   Fail to map the user data.
+
+**/
+EFI_STATUS
+UhciMapUserData (
+  IN  USB_HC_DEV              *Uhc,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  VOID                    *Data,
+  IN  OUT UINTN               *Len,
+  OUT UINT8                   *PktId,
+  OUT UINT8                   **MappedAddr,
+  OUT VOID                    **Map
+  );
+
+
+/**
+  Delete a list of TDs.
+
+  @param  Uhc         The UHCI device.
+  @param  FirstTd     TD link list head.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_TD_SW           *FirstTd
+  );
+
+
+/**
+  Create an initialize a new queue head.
+
+  @param  Uhc         The UHCI device.
+  @param  Interval    The polling interval for the queue.
+
+  @return The newly created queue header.
+
+**/
+UHCI_QH_SW *
+UhciCreateQh (
+  IN  USB_HC_DEV        *Uhc,
+  IN  UINTN             Interval
+  );
+
+
+/**
+  Create Tds list for Control Transfer.
+
+  @param  Uhc         The UHCI device.
+  @param  DeviceAddr  The device address.
+  @param  DataPktId   Packet Identification of Data Tds.
+  @param  Request     A pointer to cpu memory address of request structure buffer to transfer.
+  @param  RequestPhy  A pointer to pci memory address of request structure buffer to transfer.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  MaxPacket   Maximum packet size for control transfer.
+  @param  IsLow       Full speed or low speed.
+
+  @return The Td list head for the control transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateCtrlTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DeviceAddr,
+  IN UINT8                DataPktId,
+  IN UINT8                *Request,
+  IN UINT8                *RequestPhy,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  );
+
+
+/**
+  Create Tds list for Bulk/Interrupt Transfer.
+
+  @param  Uhc         USB_HC_DEV.
+  @param  DevAddr     Address of Device.
+  @param  EndPoint    Endpoint Number.
+  @param  PktId       Packet Identification of Data Tds.
+  @param  Data        A pointer to cpu memory address of user data buffer to transfer.
+  @param  DataPhy     A pointer to pci memory address of user data buffer to transfer.
+  @param  DataLen     Length of user data to transfer.
+  @param  DataToggle  Data Toggle Pointer.
+  @param  MaxPacket   Maximum packet size for Bulk/Interrupt transfer.
+  @param  IsLow       Is Low Speed Device.
+
+  @return The Tds list head for the bulk transfer.
+
+**/
+UHCI_TD_SW *
+UhciCreateBulkOrIntTds (
+  IN USB_HC_DEV           *Uhc,
+  IN UINT8                DevAddr,
+  IN UINT8                EndPoint,
+  IN UINT8                PktId,
+  IN UINT8                *Data,
+  IN UINT8                *DataPhy,
+  IN UINTN                DataLen,
+  IN OUT UINT8            *DataToggle,
+  IN UINT8                MaxPacket,
+  IN BOOLEAN              IsLow
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c
new file mode 100644
index 0000000000..0fd16284b3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.c
@@ -0,0 +1,281 @@
+/** @file
+
+  The UHCI register operation routines.
+
+Copyright (c) 2007, 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 "Uhci.h"
+
+
+/**
+  Read a UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+
+  @return Content of register.
+
+**/
+UINT16
+UhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset
+  )
+{
+  UINT16      Data;
+  EFI_STATUS  Status;
+
+  Status = PciIo->Io.Read (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      USB_BAR_INDEX,
+                      Offset,
+                      1,
+                      &Data
+                      );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciReadReg: PciIo Io.Read error: %r at offset %d\n", Status, Offset));
+
+    Data = 0xFFFF;
+  }
+
+  return Data;
+}
+
+
+/**
+  Write data to UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Data         Data to write.
+
+**/
+VOID
+UhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Data
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = PciIo->Io.Write (
+                      PciIo,
+                      EfiPciIoWidthUint16,
+                      USB_BAR_INDEX,
+                      Offset,
+                      1,
+                      &Data
+                      );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciWriteReg: PciIo Io.Write error: %r at offset %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set a bit of the UHCI Register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to set.
+
+**/
+VOID
+UhciSetRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  )
+{
+  UINT16  Data;
+
+  Data = UhciReadReg (PciIo, Offset);
+  Data = (UINT16) (Data |Bit);
+  UhciWriteReg (PciIo, Offset, Data);
+}
+
+
+/**
+  Clear a bit of the UHCI Register.
+
+  @param  PciIo        The PCI_IO protocol to access the PCI.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to clear.
+
+**/
+VOID
+UhciClearRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  )
+{
+  UINT16  Data;
+
+  Data = UhciReadReg (PciIo, Offset);
+  Data = (UINT16) (Data & ~Bit);
+  UhciWriteReg (PciIo, Offset, Data);
+}
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Uhc          The UHCI device.
+
+**/
+VOID
+UhciAckAllInterrupt (
+  IN  USB_HC_DEV          *Uhc
+  )
+{
+  UhciWriteReg (Uhc->PciIo, USBSTS_OFFSET, 0x3F);
+
+  //
+  // If current HC is halted, re-enable it. Host Controller Process Error
+  // is a temporary error status.
+  //
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    DEBUG ((EFI_D_ERROR, "UhciAckAllInterrupt: re-enable the UHCI from system error\n"));
+    Uhc->Usb2Hc.SetState (&Uhc->Usb2Hc, EfiUsbHcStateOperational);
+  }
+}
+
+
+/**
+  Stop the host controller.
+
+  @param  Uhc          The UHCI device.
+  @param  Timeout      Max time allowed.
+
+  @retval EFI_SUCCESS  The host controller is stopped.
+  @retval EFI_TIMEOUT  Failed to stop the host controller.
+
+**/
+EFI_STATUS
+UhciStopHc (
+  IN USB_HC_DEV        *Uhc,
+  IN UINTN             Timeout
+  )
+{
+  UINT16                UsbSts;
+  UINTN                 Index;
+
+  UhciClearRegBit (Uhc->PciIo, USBCMD_OFFSET, USBCMD_RS);
+
+  //
+  // ensure the HC is in halt status after send the stop command
+  // Timeout is in us unit.
+  //
+  for (Index = 0; Index < (Timeout / 50) + 1; Index++) {
+    UsbSts = UhciReadReg (Uhc->PciIo, USBSTS_OFFSET);
+
+    if ((UsbSts & USBSTS_HCH) == USBSTS_HCH) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (50);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Check whether the host controller operates well.
+
+  @param  PciIo        The PCI_IO protocol to use.
+
+  @retval TRUE         Host controller is working.
+  @retval FALSE        Host controller is halted or system error.
+
+**/
+BOOLEAN
+UhciIsHcWorking (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  )
+{
+  UINT16                UsbSts;
+
+  UsbSts = UhciReadReg (PciIo, USBSTS_OFFSET);
+
+  if ((UsbSts & (USBSTS_HCPE | USBSTS_HSE | USBSTS_HCH)) != 0) {
+    DEBUG ((EFI_D_ERROR, "UhciIsHcWorking: current USB state is %x\n", UsbSts));
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Set the UHCI frame list base address. It can't use
+  UhciWriteReg which access memory in UINT16.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Addr         Address to set.
+
+**/
+VOID
+UhciSetFrameListBaseAddr (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN VOID                    *Addr
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  Data;
+
+  Data = (UINT32) ((UINTN) Addr & 0xFFFFF000);
+
+  Status = PciIo->Io.Write (
+                       PciIo,
+                       EfiPciIoWidthUint32,
+                       USB_BAR_INDEX,
+                       (UINT64) USB_FRAME_BASE_OFFSET,
+                       1,
+                       &Data
+                       );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UhciSetFrameListBaseAddr: PciIo Io.Write error: %r\n", Status));
+  }
+}
+
+
+/**
+  Disable USB Emulation.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL protocol to use.
+
+**/
+VOID
+UhciTurnOffUsbEmulation (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  )
+{
+  UINT16            Command;
+
+  Command = 0;
+
+  PciIo->Pci.Write (
+               PciIo,
+               EfiPciIoWidthUint16,
+               USB_EMULATION_OFFSET,
+               1,
+               &Command
+               );
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h
new file mode 100644
index 0000000000..7212ad0dfa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciReg.h
@@ -0,0 +1,248 @@
+/** @file
+
+  The definition for UHCI register operation routines.
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _EFI_UHCI_REG_H_
+#define _EFI_UHCI_REG_H_
+
+//
+// UHCI register offset
+//
+
+#define UHCI_FRAME_NUM        1024
+
+//
+// Register offset and PCI related staff
+//
+#define USB_BAR_INDEX         4
+
+#define USBCMD_OFFSET         0
+#define USBSTS_OFFSET         2
+#define USBINTR_OFFSET        4
+#define USBPORTSC_OFFSET      0x10
+#define USB_FRAME_NO_OFFSET   6
+#define USB_FRAME_BASE_OFFSET 8
+#define USB_EMULATION_OFFSET  0xC0
+
+//
+// Packet IDs
+//
+#define SETUP_PACKET_ID       0x2D
+#define INPUT_PACKET_ID       0x69
+#define OUTPUT_PACKET_ID      0xE1
+#define ERROR_PACKET_ID       0x55
+
+//
+// USB port status and control bit definition.
+//
+#define USBPORTSC_CCS         BIT0  // Current Connect Status
+#define USBPORTSC_CSC         BIT1  // Connect Status Change
+#define USBPORTSC_PED         BIT2  // Port Enable / Disable
+#define USBPORTSC_PEDC        BIT3  // Port Enable / Disable Change
+#define USBPORTSC_LSL         BIT4  // Line Status Low BIT
+#define USBPORTSC_LSH         BIT5  // Line Status High BIT
+#define USBPORTSC_RD          BIT6  // Resume Detect
+#define USBPORTSC_LSDA        BIT8  // Low Speed Device Attached
+#define USBPORTSC_PR          BIT9  // Port Reset
+#define USBPORTSC_SUSP        BIT12 // Suspend
+
+//
+// UHCI Spec said it must implement 2 ports each host at least,
+// and if more, check whether the bit7 of PORTSC is always 1.
+// So here assume the max of port number each host is 16.
+//
+#define USB_MAX_ROOTHUB_PORT  0x0F
+
+//
+// Command register bit definitions
+//
+#define USBCMD_RS             BIT0  // Run/Stop
+#define USBCMD_HCRESET        BIT1  // Host reset
+#define USBCMD_GRESET         BIT2  // Global reset
+#define USBCMD_EGSM           BIT3  // Global Suspend Mode
+#define USBCMD_FGR            BIT4  // Force Global Resume
+#define USBCMD_SWDBG          BIT5  // SW Debug mode
+#define USBCMD_CF             BIT6  // Config Flag (sw only)
+#define USBCMD_MAXP           BIT7  // Max Packet (0 = 32, 1 = 64)
+
+//
+// USB Status register bit definitions
+//
+#define USBSTS_USBINT         BIT0  // Interrupt due to IOC
+#define USBSTS_ERROR          BIT1  // Interrupt due to error
+#define USBSTS_RD             BIT2  // Resume Detect
+#define USBSTS_HSE            BIT3  // Host System Error
+#define USBSTS_HCPE           BIT4  // Host Controller Process Error
+#define USBSTS_HCH            BIT5  // HC Halted
+
+#define USBTD_ACTIVE          BIT7  // TD is still active
+#define USBTD_STALLED         BIT6  // TD is stalled
+#define USBTD_BUFFERR         BIT5  // Buffer underflow or overflow
+#define USBTD_BABBLE          BIT4  // Babble condition
+#define USBTD_NAK             BIT3  // NAK is received
+#define USBTD_CRC             BIT2  // CRC/Time out error
+#define USBTD_BITSTUFF        BIT1  // Bit stuff error
+
+
+/**
+  Read a UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+
+  @return Content of register.
+
+**/
+UINT16
+UhciReadReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset
+  );
+
+
+
+/**
+  Write data to UHCI register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Data         Data to write.
+
+  @return None.
+
+**/
+VOID
+UhciWriteReg (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Data
+  );
+
+
+
+/**
+  Set a bit of the UHCI Register.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to set.
+
+  @return None.
+
+**/
+VOID
+UhciSetRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  );
+
+
+
+/**
+  Clear a bit of the UHCI Register.
+
+  @param  PciIo        The PCI_IO protocol to access the PCI.
+  @param  Offset       Register offset to USB_BAR_INDEX.
+  @param  Bit          The bit to clear.
+
+  @return None.
+
+**/
+VOID
+UhciClearRegBit (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN UINT32                  Offset,
+  IN UINT16                  Bit
+  );
+
+
+/**
+  Clear all the interrutp status bits, these bits
+  are Write-Clean.
+
+  @param  Uhc          The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciAckAllInterrupt (
+  IN  USB_HC_DEV          *Uhc
+  );
+
+
+/**
+  Stop the host controller.
+
+  @param  Uhc          The UHCI device.
+  @param  Timeout      Max time allowed.
+
+  @retval EFI_SUCCESS  The host controller is stopped.
+  @retval EFI_TIMEOUT  Failed to stop the host controller.
+
+**/
+EFI_STATUS
+UhciStopHc (
+  IN USB_HC_DEV         *Uhc,
+  IN UINTN              Timeout
+  );
+
+
+
+/**
+  Check whether the host controller operates well.
+
+  @param  PciIo        The PCI_IO protocol to use.
+
+  @retval TRUE         Host controller is working.
+  @retval FALSE        Host controller is halted or system error.
+
+**/
+BOOLEAN
+UhciIsHcWorking (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  );
+
+
+/**
+  Set the UHCI frame list base address. It can't use
+  UhciWriteReg which access memory in UINT16.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL to use.
+  @param  Addr         Address to set.
+
+  @return None.
+
+**/
+VOID
+UhciSetFrameListBaseAddr (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo,
+  IN VOID                    *Addr
+  );
+
+
+/**
+  Disable USB Emulation.
+
+  @param  PciIo        The EFI_PCI_IO_PROTOCOL protocol to use.
+
+  @return None.
+
+**/
+VOID
+UhciTurnOffUsbEmulation (
+  IN EFI_PCI_IO_PROTOCOL     *PciIo
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c
new file mode 100644
index 0000000000..90f010c998
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.c
@@ -0,0 +1,1045 @@
+/** @file
+
+  The EHCI register operation routines.
+
+Copyright (c) 2007 - 2013, 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 "Uhci.h"
+
+
+/**
+  Create Frame List Structure.
+
+  @param  Uhc                    UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_UNSUPPORTED        Map memory fail.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhciInitFrameList (
+  IN USB_HC_DEV         *Uhc
+  )
+{
+  EFI_PHYSICAL_ADDRESS  MappedAddr;
+  EFI_STATUS            Status;
+  VOID                  *Buffer;
+  VOID                  *Mapping;
+  UINTN                 Pages;
+  UINTN                 Bytes;
+  UINTN                 Index;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  //
+  // The Frame List is a common buffer that will be
+  // accessed by both the cpu and the usb bus master
+  // at the same time. The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Bytes = 4096;
+  Pages = EFI_SIZE_TO_PAGES (Bytes);
+
+  Status = Uhc->PciIo->AllocateBuffer (
+                         Uhc->PciIo,
+                         AllocateAnyPages,
+                         EfiBootServicesData,
+                         Pages,
+                         &Buffer,
+                         0
+                         );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = Uhc->PciIo->Map (
+                         Uhc->PciIo,
+                         EfiPciIoOperationBusMasterCommonBuffer,
+                         Buffer,
+                         &Bytes,
+                         &MappedAddr,
+                         &Mapping
+                         );
+
+  if (EFI_ERROR (Status) || (Bytes != 4096)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  Uhc->FrameBase           = (UINT32 *) (UINTN) Buffer;
+  Uhc->FrameMapping        = Mapping;
+
+  //
+  // Tell the Host Controller where the Frame List lies,
+  // by set the Frame List Base Address Register.
+  //
+  UhciSetFrameListBaseAddr (Uhc->PciIo, (VOID *) (UINTN) MappedAddr);
+
+  //
+  // Allocate the QH used by sync interrupt/control/bulk transfer.
+  // FS ctrl/bulk queue head is set to loopback so additional BW
+  // can be reclaimed. Notice, LS don't support bulk transfer and
+  // also doesn't support BW reclamation.
+  //
+  Uhc->SyncIntQh  = UhciCreateQh (Uhc, 1);
+  Uhc->CtrlQh     = UhciCreateQh (Uhc, 1);
+  Uhc->BulkQh     = UhciCreateQh (Uhc, 1);
+
+  if ((Uhc->SyncIntQh == NULL) || (Uhc->CtrlQh == NULL) || (Uhc->BulkQh == NULL)) {
+    Uhc->PciIo->Unmap (Uhc->PciIo, Mapping);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  //
+  //                                                +-------------+
+  //                                                |             |
+  // Link the three together: SyncIntQh->CtrlQh->BulkQh <---------+
+  // Each frame entry is linked to this sequence of QH. These QH
+  // will remain on the schedul, never got removed
+  //
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_HW));
+  Uhc->SyncIntQh->QhHw.HorizonLink  = QH_HLINK (PhyAddr, FALSE);
+  Uhc->SyncIntQh->NextQh            = Uhc->CtrlQh;
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_HW));
+  Uhc->CtrlQh->QhHw.HorizonLink     = QH_HLINK (PhyAddr, FALSE);
+  Uhc->CtrlQh->NextQh               = Uhc->BulkQh;
+
+  //
+  // Some old platform such as Intel's Tiger 4 has a difficult time
+  // in supporting the full speed bandwidth reclamation in the previous
+  // mentioned form. Most new platforms don't suffer it.
+  //
+  Uhc->BulkQh->QhHw.HorizonLink     = QH_HLINK (PhyAddr, FALSE);
+
+  Uhc->BulkQh->NextQh               = NULL;
+
+  Uhc->FrameBaseHostAddr = AllocateZeroPool (4096);
+  if (Uhc->FrameBaseHostAddr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_HW));
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index++) {
+    Uhc->FrameBase[Index] = QH_HLINK (PhyAddr, FALSE);
+    Uhc->FrameBaseHostAddr[Index] = (UINT32)(UINTN)Uhc->SyncIntQh;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  if (Uhc->SyncIntQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->CtrlQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->BulkQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_SW));
+  }
+
+  Uhc->PciIo->FreeBuffer (Uhc->PciIo, Pages, Buffer);
+  return Status;
+}
+
+
+/**
+  Destory FrameList buffer.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciDestoryFrameList (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  //
+  // Unmap the common buffer for framelist entry,
+  // and free the common buffer.
+  // Uhci's frame list occupy 4k memory.
+  //
+  Uhc->PciIo->Unmap (Uhc->PciIo, Uhc->FrameMapping);
+
+  Uhc->PciIo->FreeBuffer (
+                Uhc->PciIo,
+                EFI_SIZE_TO_PAGES (4096),
+                (VOID *) Uhc->FrameBase
+                );
+
+  if (Uhc->FrameBaseHostAddr != NULL) {
+    FreePool (Uhc->FrameBaseHostAddr);
+  }
+
+  if (Uhc->SyncIntQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->SyncIntQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->CtrlQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->CtrlQh, sizeof (UHCI_QH_SW));
+  }
+
+  if (Uhc->BulkQh != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, Uhc->BulkQh, sizeof (UHCI_QH_SW));
+  }
+
+  Uhc->FrameBase           = NULL;
+  Uhc->FrameBaseHostAddr   = NULL;
+  Uhc->SyncIntQh           = NULL;
+  Uhc->CtrlQh              = NULL;
+  Uhc->BulkQh              = NULL;
+}
+
+
+/**
+  Convert the poll rate to the maxium 2^n that is smaller
+  than Interval.
+
+  @param  Interval               The poll rate to convert.
+
+  @return The converted poll rate.
+
+**/
+UINTN
+UhciConvertPollRate (
+  IN  UINTN               Interval
+  )
+{
+  UINTN                   BitCount;
+
+  ASSERT (Interval != 0);
+
+  //
+  // Find the index (1 based) of the highest non-zero bit
+  //
+  BitCount = 0;
+
+  while (Interval != 0) {
+    Interval >>= 1;
+    BitCount++;
+  }
+
+  return (UINTN)1 << (BitCount - 1);
+}
+
+
+/**
+  Link a queue head (for asynchronous interrupt transfer) to
+  the frame list.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to link into.
+
+**/
+VOID
+UhciLinkQhToFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  )
+{
+  UINTN                   Index;
+  UHCI_QH_SW              *Prev;
+  UHCI_QH_SW              *Next;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  EFI_PHYSICAL_ADDRESS    QhPciAddr;
+
+  ASSERT ((Uhc->FrameBase != NULL) && (Qh != NULL));
+
+  QhPciAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Qh, sizeof (UHCI_QH_HW));
+
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index += Qh->Interval) {
+    //
+    // First QH can't be NULL because we always keep static queue
+    // heads on the frame list
+    //
+    ASSERT (!LINK_TERMINATED (Uhc->FrameBase[Index]));
+    Next  = (UHCI_QH_SW*)(UINTN)Uhc->FrameBaseHostAddr[Index];
+    Prev  = NULL;
+
+    //
+    // Now, insert the queue head (Qh) into this frame:
+    // 1. Find a queue head with the same poll interval, just insert
+    //    Qh after this queue head, then we are done.
+    //
+    // 2. Find the position to insert the queue head into:
+    //      Previous head's interval is bigger than Qh's
+    //      Next head's interval is less than Qh's
+    //    Then, insert the Qh between then
+    //
+    // This method is very much the same as that used by EHCI.
+    // Because each QH's interval is round down to 2^n, poll
+    // rate is correct.
+    //
+    while (Next->Interval > Qh->Interval) {
+      Prev  = Next;
+      Next  = Next->NextQh;
+      ASSERT (Next != NULL);
+    }
+
+    //
+    // The entry may have been linked into the frame by early insertation.
+    // For example: if insert a Qh with Qh.Interval == 4, and there is a Qh
+    // with Qh.Interval == 8 on the frame. If so, we are done with this frame.
+    // It isn't necessary to compare all the QH with the same interval to
+    // Qh. This is because if there is other QH with the same interval, Qh
+    // should has been inserted after that at FrameBase[0] and at FrameBase[0] it is
+    // impossible (Next == Qh)
+    //
+    if (Next == Qh) {
+      continue;
+    }
+
+    if (Next->Interval == Qh->Interval) {
+      //
+      // If there is a QH with the same interval, it locates at
+      // FrameBase[0], and we can simply insert it after this QH. We
+      // are all done.
+      //
+      ASSERT ((Index == 0) && (Qh->NextQh == NULL));
+
+      Prev                    = Next;
+      Next                    = Next->NextQh;
+
+      Qh->NextQh              = Next;
+      Prev->NextQh            = Qh;
+
+      Qh->QhHw.HorizonLink    = Prev->QhHw.HorizonLink;
+
+      Prev->QhHw.HorizonLink  = QH_HLINK (QhPciAddr, FALSE);
+      break;
+    }
+
+    //
+    // OK, find the right position, insert it in. If Qh's next
+    // link has already been set, it is in position. This is
+    // guarranted by 2^n polling interval.
+    //
+    if (Qh->NextQh == NULL) {
+      Qh->NextQh            = Next;
+      PhyAddr = UsbHcGetPciAddressForHostMem (Uhc->MemPool, Next, sizeof (UHCI_QH_HW));
+      Qh->QhHw.HorizonLink  = QH_HLINK (PhyAddr, FALSE);
+    }
+
+    if (Prev == NULL) {
+      Uhc->FrameBase[Index]           = QH_HLINK (QhPciAddr, FALSE);
+      Uhc->FrameBaseHostAddr[Index]   = (UINT32)(UINTN)Qh;
+    } else {
+      Prev->NextQh            = Qh;
+      Prev->QhHw.HorizonLink  = QH_HLINK (QhPciAddr, FALSE);
+    }
+  }
+}
+
+
+/**
+  Unlink QH from the frame list is easier: find all
+  the precedence node, and pointer there next to QhSw's
+  next.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to unlink.
+
+**/
+VOID
+UhciUnlinkQhFromFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  )
+{
+  UINTN                   Index;
+  UHCI_QH_SW              *Prev;
+  UHCI_QH_SW              *This;
+
+  ASSERT ((Uhc->FrameBase != NULL) && (Qh != NULL));
+
+  for (Index = 0; Index < UHCI_FRAME_NUM; Index += Qh->Interval) {
+    //
+    // Frame link can't be NULL because we always keep static
+    // queue heads on the frame list
+    //
+    ASSERT (!LINK_TERMINATED (Uhc->FrameBase[Index]));
+    This  = (UHCI_QH_SW*)(UINTN)Uhc->FrameBaseHostAddr[Index];
+    Prev  = NULL;
+
+    //
+    // Walk through the frame's QH list to find the
+    // queue head to remove
+    //
+    while ((This != NULL) && (This != Qh)) {
+      Prev  = This;
+      This  = This->NextQh;
+    }
+
+    //
+    // Qh may have already been unlinked from this frame
+    // by early action.
+    //
+    if (This == NULL) {
+      continue;
+    }
+
+    if (Prev == NULL) {
+      //
+      // Qh is the first entry in the frame
+      //
+      Uhc->FrameBase[Index]           = Qh->QhHw.HorizonLink;
+      Uhc->FrameBaseHostAddr[Index]   = (UINT32)(UINTN)Qh->NextQh;
+    } else {
+      Prev->NextQh            = Qh->NextQh;
+      Prev->QhHw.HorizonLink  = Qh->QhHw.HorizonLink;
+    }
+  }
+}
+
+
+/**
+  Check TDs Results.
+
+  @param  Uhc                    This UHCI device.
+  @param  Td                     UHCI_TD_SW to check.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               Return the result of this TD list.
+
+  @return Whether the TD's result is finialized.
+
+**/
+BOOLEAN
+UhciCheckTdStatus (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_TD_SW          *Td,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  )
+{
+  UINTN                   Len;
+  UINT8                   State;
+  UHCI_TD_HW              *TdHw;
+  BOOLEAN                 Finished;
+
+  Finished             = TRUE;
+
+  //
+  // Initialize the data toggle to that of the first
+  // TD. The next toggle to use is either:
+  // 1. first TD's toggle if no TD is executed OK
+  // 2. the next toggle of last executed-OK TD
+  //
+  QhResult->Result     = EFI_USB_NOERROR;
+  QhResult->NextToggle = (UINT8)Td->TdHw.DataToggle;
+  QhResult->Complete   = 0;
+
+  while (Td != NULL) {
+    TdHw  = &Td->TdHw;
+    State = (UINT8)TdHw->Status;
+
+    //
+    // UHCI will set STALLED bit when it abort the execution
+    // of TD list. There are several reasons:
+    //   1. BABBLE error happened
+    //   2. Received a STALL response
+    //   3. Error count decreased to zero.
+    //
+    // It also set CRC/Timeout/NAK/Buffer Error/BitStuff Error
+    // bits when corresponding conditions happen. But these
+    // conditions are not deadly, that is a TD can successfully
+    // completes even these bits are set. But it is likely that
+    // upper layer won't distinguish these condtions. So, only
+    // set these bits when TD is actually halted.
+    //
+    if ((State & USBTD_STALLED) != 0) {
+      if ((State & USBTD_BABBLE) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BABBLE;
+
+      } else if (TdHw->ErrorCount != 0) {
+        QhResult->Result |= EFI_USB_ERR_STALL;
+      }
+
+      if ((State & USBTD_CRC) != 0) {
+        QhResult->Result |= EFI_USB_ERR_CRC;
+      }
+
+      if ((State & USBTD_BUFFERR) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BUFFER;
+      }
+
+      if ((Td->TdHw.Status & USBTD_BITSTUFF) != 0) {
+        QhResult->Result |= EFI_USB_ERR_BITSTUFF;
+      }
+
+      if (TdHw->ErrorCount == 0) {
+        QhResult->Result |= EFI_USB_ERR_TIMEOUT;
+      }
+
+      Finished = TRUE;
+      goto ON_EXIT;
+
+    } else if ((State & USBTD_ACTIVE) != 0) {
+      //
+      // The TD is still active, no need to check further.
+      //
+      QhResult->Result |= EFI_USB_ERR_NOTEXECUTE;
+
+      Finished = FALSE;
+      goto ON_EXIT;
+
+    } else {
+      //
+      // Update the next data toggle, it is always the
+      // next to the last known-good TD's data toggle if
+      // any TD is executed OK
+      //
+      QhResult->NextToggle = (UINT8) (1 - (UINT8)TdHw->DataToggle);
+
+      //
+      // This TD is finished OK or met short packet read. Update the
+      // transfer length if it isn't a SETUP.
+      //
+      Len = (TdHw->ActualLen + 1) & 0x7FF;
+
+      if (TdHw->PidCode != SETUP_PACKET_ID) {
+        QhResult->Complete += Len;
+      }
+
+      //
+      // Short packet condition for full speed input TD, also
+      // terminate the transfer
+      //
+      if (!IsLow && (TdHw->ShortPacket == 1) && (Len < Td->DataLen)) {
+        DEBUG ((EFI_D_VERBOSE, "UhciCheckTdStatus: short packet read occured\n"));
+
+        Finished = TRUE;
+        goto ON_EXIT;
+      }
+    }
+
+    Td = Td->NextTd;
+  }
+
+ON_EXIT:
+  //
+  // Check whether HC is halted. Don't move this up. It must be
+  // called after data toggle is successfully updated.
+  //
+  if (!UhciIsHcWorking (Uhc->PciIo)) {
+    QhResult->Result |= EFI_USB_ERR_SYSTEM;
+    Finished  = TRUE;
+  }
+
+  if (Finished) {
+    Uhc->PciIo->Flush (Uhc->PciIo);
+  }
+
+  UhciAckAllInterrupt (Uhc);
+  return Finished;
+}
+
+
+/**
+  Check the result of the transfer.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  Td                     The first TDs of the transfer.
+  @param  TimeOut                TimeOut value in milliseconds.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               The variable to return result.
+
+  @retval EFI_SUCCESS            The transfer finished with success.
+  @retval EFI_DEVICE_ERROR       Transfer failed.
+
+**/
+EFI_STATUS
+UhciExecuteTransfer (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_QH_SW          *Qh,
+  IN  UHCI_TD_SW          *Td,
+  IN  UINTN               TimeOut,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  )
+{
+  UINTN                   Index;
+  UINTN                   Delay;
+  BOOLEAN                 Finished;
+  EFI_STATUS              Status;
+  BOOLEAN                 InfiniteLoop;
+
+  Finished     = FALSE;
+  Status       = EFI_SUCCESS;
+  Delay        = TimeOut * UHC_1_MILLISECOND;
+  InfiniteLoop = FALSE;
+
+  //
+  // According to UEFI spec section 16.2.4, If Timeout is 0, then the caller
+  // must wait for the function to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR
+  // is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  for (Index = 0; InfiniteLoop || (Index < Delay); Index++) {
+    Finished = UhciCheckTdStatus (Uhc, Td, IsLow, QhResult);
+
+    //
+    // Transfer is OK or some error occured (TD inactive)
+    //
+    if (Finished) {
+      break;
+    }
+
+    gBS->Stall (UHC_1_MICROSECOND);
+  }
+
+  if (!Finished) {
+    DEBUG ((EFI_D_ERROR, "UhciExecuteTransfer: execution not finished for %dms\n", (UINT32)TimeOut));
+    UhciDumpQh (Qh);
+    UhciDumpTds (Td);
+
+    Status = EFI_TIMEOUT;
+
+  } else if (QhResult->Result != EFI_USB_NOERROR) {
+    DEBUG ((EFI_D_ERROR, "UhciExecuteTransfer: execution failed with result %x\n", QhResult->Result));
+    UhciDumpQh (Qh);
+    UhciDumpTds (Td);
+
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+
+/**
+  Update Async Request, QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The UHCI asynchronous transfer to update.
+  @param  Result                 Transfer reslut.
+  @param  NextToggle             The toggle of next data.
+
+**/
+VOID
+UhciUpdateAsyncReq (
+  IN USB_HC_DEV          *Uhc,
+  IN UHCI_ASYNC_REQUEST  *AsyncReq,
+  IN UINT32              Result,
+  IN UINT32              NextToggle
+  )
+{
+  UHCI_QH_SW              *Qh;
+  UHCI_TD_SW              *FirstTd;
+  UHCI_TD_SW              *Td;
+
+  Qh          = AsyncReq->QhSw;
+  FirstTd     = AsyncReq->FirstTd;
+
+  if (Result == EFI_USB_NOERROR) {
+    //
+    // The last transfer succeeds. Then we need to update
+    // the Qh and Td for next round of transfer.
+    // 1. Update the TD's data toggle
+    // 2. Activate all the TDs
+    // 3. Link the TD to the queue head again since during
+    //    execution, queue head's TD pointer is changed by
+    //    hardware.
+    //
+    for (Td = FirstTd; Td != NULL; Td = Td->NextTd) {
+      Td->TdHw.DataToggle = NextToggle;
+      NextToggle         ^= 1;
+      Td->TdHw.Status    |= USBTD_ACTIVE;
+    }
+
+    UhciLinkTdToQh (Uhc, Qh, FirstTd);
+    return ;
+  }
+}
+
+
+/**
+  Create Async Request node, and Link to List.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  FirstTd                First TD of the transfer.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  DataLen                Data length.
+  @param  Interval               Polling Interval when inserted to frame list.
+  @param  Data                   Data buffer, unmapped.
+  @param  Callback               Callback after interrupt transfeer.
+  @param  Context                Callback Context passed as function parameter.
+  @param  IsLow                  Is Low Speed.
+
+  @retval EFI_SUCCESS            An asynchronous transfer is created.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_OUT_OF_RESOURCES   Failed because of resource shortage.
+
+**/
+EFI_STATUS
+UhciCreateAsyncReq (
+  IN USB_HC_DEV                       *Uhc,
+  IN UHCI_QH_SW                       *Qh,
+  IN UHCI_TD_SW                       *FirstTd,
+  IN UINT8                            DevAddr,
+  IN UINT8                            EndPoint,
+  IN UINTN                            DataLen,
+  IN UINTN                            Interval,
+  IN UINT8                            *Data,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,
+  IN VOID                             *Context,
+  IN BOOLEAN                          IsLow
+  )
+{
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+
+  AsyncReq = AllocatePool (sizeof (UHCI_ASYNC_REQUEST));
+
+  if (AsyncReq == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Fill Request field. Data is allocated host memory, not mapped
+  //
+  AsyncReq->Signature   = UHCI_ASYNC_INT_SIGNATURE;
+  AsyncReq->DevAddr     = DevAddr;
+  AsyncReq->EndPoint    = EndPoint;
+  AsyncReq->DataLen     = DataLen;
+  AsyncReq->Interval    = UhciConvertPollRate(Interval);
+  AsyncReq->Data        = Data;
+  AsyncReq->Callback    = Callback;
+  AsyncReq->Context     = Context;
+  AsyncReq->QhSw        = Qh;
+  AsyncReq->FirstTd     = FirstTd;
+  AsyncReq->IsLow       = IsLow;
+
+  //
+  // Insert the new interrupt transfer to the head of the list.
+  // The interrupt transfer's monitor function scans the whole
+  // list from head to tail. The new interrupt transfer MUST be
+  // added to the head of the list.
+  //
+  InsertHeadList (&(Uhc->AsyncIntList), &(AsyncReq->Link));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Free an asynchronous request's resource such as memory.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The asynchronous request to free.
+
+**/
+VOID
+UhciFreeAsyncReq (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_ASYNC_REQUEST   *AsyncReq
+  )
+{
+  ASSERT ((Uhc != NULL) && (AsyncReq != NULL));
+
+  UhciDestoryTds (Uhc, AsyncReq->FirstTd);
+  UsbHcFreeMem (Uhc->MemPool, AsyncReq->QhSw, sizeof (UHCI_QH_SW));
+
+  if (AsyncReq->Data != NULL) {
+    UsbHcFreeMem (Uhc->MemPool, AsyncReq->Data, AsyncReq->DataLen);
+  }
+
+  gBS->FreePool (AsyncReq);
+}
+
+
+/**
+  Unlink an asynchronous request's from UHC's asynchronus list.
+  also remove the queue head from the frame list. If FreeNow,
+  release its resource also. Otherwise, add the request to the
+  UHC's recycle list to wait for a while before release the memory.
+  Until then, hardware won't hold point to the request.
+
+  @param  Uhc                    The UHCI device.
+  @param  AsyncReq               The asynchronous request to free.
+  @param  FreeNow                If TRUE, free the resource immediately, otherwise
+                                 add the request to recycle wait list.
+
+**/
+VOID
+UhciUnlinkAsyncReq (
+  IN USB_HC_DEV           *Uhc,
+  IN UHCI_ASYNC_REQUEST   *AsyncReq,
+  IN BOOLEAN              FreeNow
+  )
+{
+  ASSERT ((Uhc != NULL) && (AsyncReq != NULL));
+
+  RemoveEntryList (&(AsyncReq->Link));
+  UhciUnlinkQhFromFrameList (Uhc, AsyncReq->QhSw);
+
+  if (FreeNow) {
+    UhciFreeAsyncReq (Uhc, AsyncReq);
+  } else {
+    //
+    // To sychronize with hardware, mark the queue head as inactive
+    // then add AsyncReq to UHC's recycle list
+    //
+    AsyncReq->QhSw->QhHw.VerticalLink = QH_VLINK (NULL, TRUE);
+    AsyncReq->Recycle = Uhc->RecycleWait;
+    Uhc->RecycleWait  = AsyncReq;
+  }
+}
+
+
+/**
+  Delete Async Interrupt QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  Toggle                 The next data toggle to use.
+
+  @retval EFI_SUCCESS            The request is deleted.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_NOT_FOUND          The asynchronous isn't found.
+
+**/
+EFI_STATUS
+UhciRemoveAsyncReq (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EndPoint,
+  OUT UINT8               *Toggle
+  )
+{
+  EFI_STATUS          Status;
+  UHCI_ASYNC_REQUEST  *AsyncReq;
+  UHCI_QH_RESULT      QhResult;
+  LIST_ENTRY          *Link;
+  BOOLEAN             Found;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // If no asynchronous interrupt transaction exists
+  //
+  if (IsListEmpty (&(Uhc->AsyncIntList))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Find the asynchronous transfer to this device/endpoint pair
+  //
+  Found = FALSE;
+  Link  = Uhc->AsyncIntList.ForwardLink;
+
+  do {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Link);
+    Link      = Link->ForwardLink;
+
+    if ((AsyncReq->DevAddr == DevAddr) && (AsyncReq->EndPoint == EndPoint)) {
+      Found = TRUE;
+      break;
+    }
+
+  } while (Link != &(Uhc->AsyncIntList));
+
+  if (!Found) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check the result of the async transfer then update it
+  // to get the next data toggle to use.
+  //
+  UhciCheckTdStatus (Uhc, AsyncReq->FirstTd, AsyncReq->IsLow, &QhResult);
+  *Toggle = QhResult.NextToggle;
+
+  //
+  // Don't release the request now, keep it to synchronize with hardware.
+  //
+  UhciUnlinkAsyncReq (Uhc, AsyncReq, FALSE);
+  return Status;
+}
+
+
+/**
+  Recycle the asynchronouse request. When a queue head
+  is unlinked from frame list, host controller hardware
+  may still hold a cached pointer to it. To synchronize
+  with hardware, the request is released in two steps:
+  first it is linked to the UHC's RecycleWait list. At
+  the next time UhciMonitorAsyncReqList is fired, it is
+  moved to UHC's Recylelist. Then, at another timer
+  activation, all the requests on Recycle list is freed.
+  This guarrantes that each unlink queue head keeps
+  existing for at least 50ms, far enough for the hardware
+  to clear its cache.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciRecycleAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  UHCI_ASYNC_REQUEST      *Req;
+  UHCI_ASYNC_REQUEST      *Next;
+
+  Req = Uhc->Recycle;
+
+  while (Req != NULL) {
+    Next = Req->Recycle;
+    UhciFreeAsyncReq (Uhc, Req);
+    Req  = Next;
+  }
+
+  Uhc->Recycle     = Uhc->RecycleWait;
+  Uhc->RecycleWait = NULL;
+}
+
+
+
+/**
+  Release all the asynchronous transfers on the lsit.
+
+  @param  Uhc                    The UHCI device.
+
+**/
+VOID
+UhciFreeAllAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  )
+{
+  LIST_ENTRY              *Head;
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+
+  //
+  // Call UhciRecycleAsyncReq twice. The requests on Recycle
+  // will be released at the first call; The requests on
+  // RecycleWait will be released at the second call.
+  //
+  UhciRecycleAsyncReq (Uhc);
+  UhciRecycleAsyncReq (Uhc);
+
+  Head = &(Uhc->AsyncIntList);
+
+  if (IsListEmpty (Head)) {
+    return;
+  }
+
+  while (!IsListEmpty (Head)) {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Head->ForwardLink);
+    UhciUnlinkAsyncReq (Uhc, AsyncReq, TRUE);
+  }
+}
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                  The event of the time.
+  @param  Context                Context of the event, pointer to USB_HC_DEV.
+
+**/
+VOID
+EFIAPI
+UhciMonitorAsyncReqList (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  UHCI_ASYNC_REQUEST      *AsyncReq;
+  LIST_ENTRY              *Link;
+  USB_HC_DEV              *Uhc;
+  VOID                    *Data;
+  BOOLEAN                 Finished;
+  UHCI_QH_RESULT          QhResult;
+
+  Uhc = (USB_HC_DEV *) Context;
+
+  //
+  // Recycle the asynchronous requests expired, and promote
+  // requests waiting to be recycled the next time when this
+  // timer expires
+  //
+  UhciRecycleAsyncReq (Uhc);
+
+  if (IsListEmpty (&(Uhc->AsyncIntList))) {
+    return ;
+  }
+
+  //
+  // This loop must be delete safe
+  //
+  Link = Uhc->AsyncIntList.ForwardLink;
+
+  do {
+    AsyncReq  = UHCI_ASYNC_INT_FROM_LINK (Link);
+    Link      = Link->ForwardLink;
+
+    Finished = UhciCheckTdStatus (Uhc, AsyncReq->FirstTd, AsyncReq->IsLow, &QhResult);
+
+    if (!Finished) {
+      continue;
+    }
+
+    //
+    // Copy the data to temporary buffer if there are some
+    // data transferred. We may have zero-length packet
+    //
+    Data = NULL;
+
+    if (QhResult.Complete != 0) {
+      Data = AllocatePool (QhResult.Complete);
+
+      if (Data == NULL) {
+        return ;
+      }
+
+      CopyMem (Data, AsyncReq->FirstTd->Data, QhResult.Complete);
+    }
+
+    UhciUpdateAsyncReq (Uhc, AsyncReq, QhResult.Result, QhResult.NextToggle);
+
+    //
+    // Now, either transfer is SUCCESS or met errors since
+    // we have skipped to next transfer earlier if current
+    // transfer is still active.
+    //
+    if (QhResult.Result == EFI_USB_NOERROR) {
+      AsyncReq->Callback (Data, QhResult.Complete, AsyncReq->Context, QhResult.Result);
+    } else {
+      //
+      // Leave error recovery to its related device driver.
+      // A common case of the error recovery is to re-submit
+      // the interrupt transfer. When an interrupt transfer
+      // is re-submitted, its position in the linked list is
+      // changed. It is inserted to the head of the linked
+      // list, while this function scans the whole list from
+      // head to tail. Thus, the re-submitted interrupt transfer's
+      // callback function will not be called again in this round.
+      //
+      AsyncReq->Callback (NULL, 0, AsyncReq->Context, QhResult.Result);
+    }
+
+    if (Data != NULL) {
+      gBS->FreePool (Data);
+    }
+  } while (Link != &(Uhc->AsyncIntList));
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h
new file mode 100644
index 0000000000..f6d4fc40a7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UhciSched.h
@@ -0,0 +1,271 @@
+/** @file
+
+  The definition for EHCI register operation routines.
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _EFI_UHCI_SCHED_H_
+#define _EFI_UHCI_SCHED_H_
+
+
+#define UHCI_ASYNC_INT_SIGNATURE  SIGNATURE_32 ('u', 'h', 'c', 'a')
+//
+// The failure mask for USB transfer return status. If any of
+// these bit is set, the transfer failed. EFI_USB_ERR_NOEXECUTE
+// and EFI_USB_ERR_NAK are not considered as error condition:
+// the transfer is still going on.
+//
+#define USB_ERR_FAIL_MASK  (EFI_USB_ERR_STALL   | EFI_USB_ERR_BUFFER | \
+                            EFI_USB_ERR_BABBLE  | EFI_USB_ERR_CRC    | \
+                            EFI_USB_ERR_TIMEOUT | EFI_USB_ERR_BITSTUFF | \
+                            EFI_USB_ERR_SYSTEM)
+
+
+//
+// Structure to return the result of UHCI QH execution.
+// Result is the final result of the QH's QTD. NextToggle
+// is the next data toggle to use. Complete is the actual
+// length of data transferred.
+//
+typedef struct {
+  UINT32                  Result;
+  UINT8                   NextToggle;
+  UINTN                   Complete;
+} UHCI_QH_RESULT;
+
+typedef struct _UHCI_ASYNC_REQUEST  UHCI_ASYNC_REQUEST;
+
+//
+// Structure used to manager the asynchronous interrupt transfers.
+//
+struct _UHCI_ASYNC_REQUEST{
+  UINTN                           Signature;
+  LIST_ENTRY                      Link;
+  UHCI_ASYNC_REQUEST              *Recycle;
+
+  //
+  // Endpoint attributes
+  //
+  UINT8                           DevAddr;
+  UINT8                           EndPoint;
+  BOOLEAN                         IsLow;
+  UINTN                           Interval;
+
+  //
+  // Data and UHC structures
+  //
+  UHCI_QH_SW                      *QhSw;
+  UHCI_TD_SW                      *FirstTd;
+  UINT8                           *Data;      // Allocated host memory, not mapped memory
+  UINTN                           DataLen;
+  VOID                            *Mapping;
+
+  //
+  // User callback and its context
+  //
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+};
+
+#define UHCI_ASYNC_INT_FROM_LINK(a) \
+          CR (a, UHCI_ASYNC_REQUEST, Link, UHCI_ASYNC_INT_SIGNATURE)
+
+
+/**
+  Create Frame List Structure.
+
+  @param  Uhc                    The UHCI device.
+
+  @return EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @return EFI_UNSUPPORTED        Map memory fail.
+  @return EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhciInitFrameList (
+  IN USB_HC_DEV         *Uhc
+  );
+
+/**
+  Destory FrameList buffer.
+
+  @param  Uhc                    The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciDestoryFrameList (
+  IN USB_HC_DEV           *Uhc
+  );
+
+
+/**
+  Convert the poll rate to the maxium 2^n that is smaller
+  than Interval.
+
+  @param  Interval               The poll rate to convert.
+
+  @return The converted poll rate.
+
+**/
+UINTN
+UhciConvertPollRate (
+  IN  UINTN               Interval
+  );
+
+
+/**
+  Link a queue head (for asynchronous interrupt transfer) to
+  the frame list.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to link into.
+
+**/
+VOID
+UhciLinkQhToFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  );
+
+
+/**
+  Unlink QH from the frame list is easier: find all
+  the precedence node, and pointer there next to QhSw's
+  next.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head to unlink.
+
+**/
+VOID
+UhciUnlinkQhFromFrameList (
+  USB_HC_DEV              *Uhc,
+  UHCI_QH_SW              *Qh
+  );
+
+
+/**
+  Check the result of the transfer.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  Td                     The first TDs of the transfer.
+  @param  TimeOut                TimeOut value in milliseconds.
+  @param  IsLow                  Is Low Speed Device.
+  @param  QhResult               The variable to return result.
+
+  @retval EFI_SUCCESS            The transfer finished with success.
+  @retval EFI_DEVICE_ERROR       Transfer failed.
+
+**/
+EFI_STATUS
+UhciExecuteTransfer (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UHCI_QH_SW          *Qh,
+  IN  UHCI_TD_SW          *Td,
+  IN  UINTN               TimeOut,
+  IN  BOOLEAN             IsLow,
+  OUT UHCI_QH_RESULT      *QhResult
+  );
+
+
+/**
+  Create Async Request node, and Link to List.
+
+  @param  Uhc                    The UHCI device.
+  @param  Qh                     The queue head of the transfer.
+  @param  FirstTd                First TD of the transfer.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  DataLen                Data length.
+  @param  Interval               Polling Interval when inserted to frame list.
+  @param  Data                   Data buffer, unmapped.
+  @param  Callback               Callback after interrupt transfeer.
+  @param  Context                Callback Context passed as function parameter.
+  @param  IsLow                  Is Low Speed.
+
+  @retval EFI_SUCCESS            An asynchronous transfer is created.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_OUT_OF_RESOURCES   Failed because of resource shortage.
+
+**/
+EFI_STATUS
+UhciCreateAsyncReq (
+  IN USB_HC_DEV                       *Uhc,
+  IN UHCI_QH_SW                       *Qh,
+  IN UHCI_TD_SW                       *FirstTd,
+  IN UINT8                            DevAddr,
+  IN UINT8                            EndPoint,
+  IN UINTN                            DataLen,
+  IN UINTN                            Interval,
+  IN UINT8                            *Data,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,
+  IN VOID                             *Context,
+  IN BOOLEAN                          IsLow
+  );
+
+
+/**
+  Delete Async Interrupt QH and TDs.
+
+  @param  Uhc                    The UHCI device.
+  @param  DevAddr                Device Address.
+  @param  EndPoint               EndPoint Address.
+  @param  Toggle                 The next data toggle to use.
+
+  @retval EFI_SUCCESS            The request is deleted.
+  @retval EFI_INVALID_PARAMETER  Paremeter is error.
+  @retval EFI_NOT_FOUND          The asynchronous isn't found.
+
+**/
+EFI_STATUS
+UhciRemoveAsyncReq (
+  IN  USB_HC_DEV          *Uhc,
+  IN  UINT8               DevAddr,
+  IN  UINT8               EndPoint,
+  OUT UINT8               *Toggle
+  );
+
+
+/**
+  Release all the asynchronous transfers on the lsit.
+
+  @param  Uhc                    The UHCI device.
+
+  @return None.
+
+**/
+VOID
+UhciFreeAllAsyncReq (
+  IN USB_HC_DEV           *Uhc
+  );
+
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                  The event of the time.
+  @param  Context                Context of the event, pointer to USB_HC_DEV.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UhciMonitorAsyncReqList (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..e42d482885
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.c
@@ -0,0 +1,564 @@
+/** @file
+
+  The routine procedure for uhci memory allocate/free.
+
+Copyright (c) 2007 - 2010, 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 "Uhci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  //
+  // Check whether the data structure used by the host controller
+  // should be restricted into the same 4G
+  //
+  if (Pool->Check4G && (Pool->Which4G != USB_HC_HIGH_32BIT (MappedAddr))) {
+    PciIo->Unmap (PciIo, Mapping);
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block           The memory block to allocate memory from.
+  @param  Units           Number of memory units to allocate.
+
+  @return EFI_SUCCESS     The needed memory is allocated.
+  @return EFI_NOT_FOUND   Can't find the free memory.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  UINTN                   Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->BufHost <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->BufHost;
+  PhyAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->Buf + Offset);
+  return PhyAddr;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block     The memory block to check.
+
+  @return TRUE      The memory block is empty.
+  @return FALSE     The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Check4G               Whether the host controller requires allocated memory
+                                from one 4G address space.
+  @param  Which4G               The 4G memory area each memory allocated should be from.
+
+  @return EFI_SUCCESS           The memory pool is initialized.
+  @return EFI_OUT_OF_RESOURCE   Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Check4G = Check4G;
+  Pool->Which4G = Which4G;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool               The USB memory pool to free.
+
+  @return EFI_SUCCESS        The memory pool is freed.
+  @return EFI_DEVICE_ERROR   Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_INFO, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..c53d0b78f2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciDxe/UsbHcMem.h
@@ -0,0 +1,161 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _EFI_EHCI_MEM_H_
+#define _EFI_EHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+#define USB_HC_HIGH_32BIT(Addr64)    \
+          ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. EHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+  @param  Check4G             Whether the host controller requires allocated memory
+                              from one 4G address space.
+  @param  Which4G             The 4G memory area each memory allocated should be from.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN BOOLEAN              Check4G,
+  IN UINT32               Which4G
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool               The USB memory pool to free.
+
+  @return EFI_SUCCESS        The memory pool is freed.
+  @return EFI_DEVICE_ERROR   Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+  @return None.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return the pci memory address
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddressForHostMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c
new file mode 100644
index 0000000000..368b75d753
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.c
@@ -0,0 +1,3219 @@
+/** @file
+PEIM to produce gPeiUsbHostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2006 - 2014, 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 "UhcPeim.h"
+
+/**
+  Initializes Usb Host Controller.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            PPI successfully installed.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *ChipSetUsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  USB_UHC_DEV                 *UhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &ChipSetUsbControllerPpi
+             );
+  //
+  // If failed to locate, it is a bug in dispather as depex has gPeiUsbControllerPpiGuid.
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  Index = 0;
+  while (TRUE) {
+    Status = ChipSetUsbControllerPpi->GetUsbController (
+                                        (EFI_PEI_SERVICES **) PeiServices,
+                                        ChipSetUsbControllerPpi,
+                                        Index,
+                                        &ControllerType,
+                                        &BaseAddress
+                                        );
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    //
+    // This PEIM is for UHC type controller.
+    //
+    if (ControllerType != PEI_UHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = sizeof (USB_UHC_DEV) / EFI_PAGE_SIZE + 1;
+
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    UhcDev = (USB_UHC_DEV *) ((UINTN) TempPtr);
+    UhcDev->Signature   = USB_UHC_DEV_SIGNATURE;
+    UhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+
+    //
+    // Init local memory management service
+    //
+    Status = InitializeMemoryManagement (UhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Initialize Uhc's hardware
+    //
+    Status = InitializeUsbHC (UhcDev);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    UhcDev->UsbHostControllerPpi.ControlTransfer          = UhcControlTransfer;
+    UhcDev->UsbHostControllerPpi.BulkTransfer             = UhcBulkTransfer;
+    UhcDev->UsbHostControllerPpi.GetRootHubPortNumber     = UhcGetRootHubPortNumber;
+    UhcDev->UsbHostControllerPpi.GetRootHubPortStatus     = UhcGetRootHubPortStatus;
+    UhcDev->UsbHostControllerPpi.SetRootHubPortFeature    = UhcSetRootHubPortFeature;
+    UhcDev->UsbHostControllerPpi.ClearRootHubPortFeature  = UhcClearRootHubPortFeature;
+
+    UhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    UhcDev->PpiDescriptor.Guid  = &gPeiUsbHostControllerPpiGuid;
+    UhcDev->PpiDescriptor.Ppi   = &UhcDev->UsbHostControllerPpi;
+
+    Status = PeiServicesInstallPpi (&UhcDev->PpiDescriptor);
+    if (EFI_ERROR (Status)) {
+      Index++;
+      continue;
+    }
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+                                 If Timeout is 0, then the caller must wait for the function
+                                 to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcControlTransfer (
+  IN     EFI_PEI_SERVICES               **PeiServices,
+  IN     PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN     UINT8                          DeviceAddress,
+  IN     UINT8                          DeviceSpeed,
+  IN     UINT8                          MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST         *Request,
+  IN     EFI_USB_DATA_DIRECTION         TransferDirection,
+  IN OUT VOID                           *Data                 OPTIONAL,
+  IN OUT UINTN                          *DataLength           OPTIONAL,
+  IN     UINTN                          TimeOut,
+  OUT    UINT32                         *TransferResult
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      StatusReg;
+  UINT8       PktID;
+  QH_STRUCT   *PtrQH;
+  TD_STRUCT   *PtrTD;
+  TD_STRUCT   *PtrPreTD;
+  TD_STRUCT   *PtrSetupTD;
+  TD_STRUCT   *PtrStatusTD;
+  EFI_STATUS  Status;
+  UINT32      DataLen;
+  UINT8       *PtrDataSource;
+  UINT8       *Ptr;
+  UINT8       DataToggle;
+
+  UhcDev      = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  StatusReg   = UhcDev->UsbHostControllerBaseAddress + USBSTS;
+
+  PktID       = INPUT_PACKET_ID;
+
+  if (Request == NULL || TransferResult == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // if errors exist that cause host controller halt,
+  // then return EFI_DEVICE_ERROR.
+  //
+
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult = EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  //
+  // generate Setup Stage TD
+  //
+
+  PtrQH = UhcDev->ConfigQH;
+
+  GenSetupStageTD (
+    UhcDev,
+    DeviceAddress,
+    0,
+    DeviceSpeed,
+    (UINT8 *) Request,
+    (UINT8) sizeof (EFI_USB_DEVICE_REQUEST),
+    &PtrSetupTD
+    );
+
+  //
+  // link setup TD structures to QH structure
+  //
+  LinkTDToQH (PtrQH, PtrSetupTD);
+
+  PtrPreTD = PtrSetupTD;
+
+  //
+  //  Data Stage of Control Transfer
+  //
+  switch (TransferDirection) {
+
+  case EfiUsbDataIn:
+    PktID         = INPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  case EfiUsbDataOut:
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  //
+  // no data stage
+  //
+  case EfiUsbNoData:
+    if (*DataLength != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = NULL;
+    DataLen       = 0;
+    Ptr           = NULL;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataToggle  = 1;
+
+  PtrTD       = PtrSetupTD;
+  while (DataLen > 0) {
+    //
+    // create TD structures and link together
+    //
+    UINT8 PacketSize;
+
+    //
+    // PacketSize is the data load size of each TD carries.
+    //
+    PacketSize = (UINT8) DataLen;
+    if (DataLen > MaximumPacketLength) {
+      PacketSize = MaximumPacketLength;
+    }
+
+    GenDataTD (
+      UhcDev,
+      DeviceAddress,
+      0,
+      Ptr,
+      PacketSize,
+      PktID,
+      DataToggle,
+      DeviceSpeed,
+      &PtrTD
+      );
+
+    //
+    // Link two TDs in vertical depth
+    //
+    LinkTDToTD (PtrPreTD, PtrTD);
+    PtrPreTD = PtrTD;
+
+    DataToggle ^= 1;
+    Ptr += PacketSize;
+    DataLen -= PacketSize;
+  }
+
+  //
+  // PtrPreTD points to the last TD before the Setup-Stage TD.
+  //
+  PtrPreTD = PtrTD;
+
+  //
+  // Status Stage of Control Transfer
+  //
+  if (PktID == OUTPUT_PACKET_ID) {
+    PktID = INPUT_PACKET_ID;
+  } else {
+    PktID = OUTPUT_PACKET_ID;
+  }
+  //
+  // create Status Stage TD structure
+  //
+  CreateStatusTD (
+    UhcDev,
+    DeviceAddress,
+    0,
+    PktID,
+    DeviceSpeed,
+    &PtrStatusTD
+    );
+
+  LinkTDToTD (PtrPreTD, PtrStatusTD);
+
+  //
+  // Poll QH-TDs execution and get result.
+  // detail status is returned
+  //
+  Status = ExecuteControlTransfer (
+            UhcDev,
+            PtrSetupTD,
+            DataLength,
+            TimeOut,
+            TransferResult
+            );
+
+  //
+  // TRUE means must search other framelistindex
+  //
+  SetQHVerticalValidorInvalid(PtrQH, FALSE);
+  DeleteQueuedTDs (UhcDev, PtrSetupTD);
+
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult |= EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcBulkTransfer (
+  IN     EFI_PEI_SERVICES               **PeiServices,
+  IN     PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN     UINT8                          DeviceAddress,
+  IN     UINT8                          EndPointAddress,
+  IN     UINT8                          MaximumPacketLength,
+  IN OUT VOID                           *Data,
+  IN OUT UINTN                          *DataLength,
+  IN OUT UINT8                          *DataToggle,
+  IN     UINTN                          TimeOut,
+  OUT    UINT32                         *TransferResult
+  )
+{
+  USB_UHC_DEV             *UhcDev;
+  UINT32                  StatusReg;
+
+  UINT32                  DataLen;
+
+  QH_STRUCT               *PtrQH;
+  TD_STRUCT               *PtrFirstTD;
+  TD_STRUCT               *PtrTD;
+  TD_STRUCT               *PtrPreTD;
+
+  UINT8                   PktID;
+  UINT8                   *PtrDataSource;
+  UINT8                   *Ptr;
+
+  BOOLEAN                 IsFirstTD;
+
+  EFI_STATUS              Status;
+
+  EFI_USB_DATA_DIRECTION  TransferDirection;
+
+  BOOLEAN                 ShortPacketEnable;
+
+  UINT16                  CommandContent;
+
+  UhcDev = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  //
+  // Enable the maximum packet size (64bytes)
+  // that can be used for full speed bandwidth reclamation
+  // at the end of a frame.
+  //
+  CommandContent = USBReadPortW (UhcDev, UhcDev->UsbHostControllerBaseAddress + USBCMD);
+  if ((CommandContent & USBCMD_MAXP) != USBCMD_MAXP) {
+    CommandContent |= USBCMD_MAXP;
+    USBWritePortW (UhcDev, UhcDev->UsbHostControllerBaseAddress + USBCMD, CommandContent);
+  }
+
+  StatusReg   = UhcDev->UsbHostControllerBaseAddress + USBSTS;
+
+  //
+  // these code lines are added here per complier's strict demand
+  //
+  PktID             = INPUT_PACKET_ID;
+  PtrTD             = NULL;
+  PtrFirstTD        = NULL;
+  PtrPreTD          = NULL;
+  DataLen           = 0;
+  Ptr               = NULL;
+
+  ShortPacketEnable = FALSE;
+
+  if ((DataLength == 0) || (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MaximumPacketLength != 8 && MaximumPacketLength != 16
+      && MaximumPacketLength != 32 && MaximumPacketLength != 64) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult = EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  if ((EndPointAddress & 0x80) != 0) {
+    TransferDirection = EfiUsbDataIn;
+  } else {
+    TransferDirection = EfiUsbDataOut;
+  }
+
+  switch (TransferDirection) {
+
+  case EfiUsbDataIn:
+    ShortPacketEnable = TRUE;
+    PktID             = INPUT_PACKET_ID;
+    PtrDataSource     = Data;
+    DataLen           = (UINT32) *DataLength;
+    Ptr               = PtrDataSource;
+    break;
+
+  case EfiUsbDataOut:
+    PktID         = OUTPUT_PACKET_ID;
+    PtrDataSource = Data;
+    DataLen       = (UINT32) *DataLength;
+    Ptr           = PtrDataSource;
+    break;
+
+  default:
+    break;
+  }
+
+  PtrQH = UhcDev->BulkQH;
+
+  IsFirstTD = TRUE;
+  while (DataLen > 0) {
+    //
+    // create TD structures and link together
+    //
+    UINT8 PacketSize;
+
+    PacketSize = (UINT8) DataLen;
+    if (DataLen > MaximumPacketLength) {
+      PacketSize = MaximumPacketLength;
+    }
+
+    GenDataTD (
+      UhcDev,
+      DeviceAddress,
+      EndPointAddress,
+      Ptr,
+      PacketSize,
+      PktID,
+      *DataToggle,
+      USB_FULL_SPEED_DEVICE,
+      &PtrTD
+      );
+
+    //
+    // Enable short packet detection.
+    // (default action is disabling short packet detection)
+    //
+    if (ShortPacketEnable) {
+      EnableorDisableTDShortPacket (PtrTD, TRUE);
+    }
+
+    if (IsFirstTD) {
+      PtrFirstTD            = PtrTD;
+      PtrFirstTD->PtrNextTD = NULL;
+      IsFirstTD             = FALSE;
+    } else {
+      //
+      // Link two TDs in vertical depth
+      //
+      LinkTDToTD (PtrPreTD, PtrTD);
+    }
+
+    PtrPreTD = PtrTD;
+
+    *DataToggle ^= 1;
+    Ptr += PacketSize;
+    DataLen -= PacketSize;
+  }
+  //
+  // link TD structures to QH structure
+  //
+  LinkTDToQH (PtrQH, PtrFirstTD);
+
+  //
+  // Execute QH-TD and get result
+  //
+  //
+  // detail status is put into the Result field in the pIRP
+  // the Data Toggle value is also re-updated to the value
+  // of the last successful TD
+  //
+  Status = ExecBulkTransfer (
+            UhcDev,
+            PtrFirstTD,
+            DataLength,
+            DataToggle,
+            TimeOut,
+            TransferResult
+            );
+
+  //
+  // Delete Bulk transfer TD structure
+  //
+  DeleteQueuedTDs (UhcDev, PtrFirstTD);
+
+  //
+  // if has errors that cause host controller halt, then return EFI_DEVICE_ERROR directly.
+  //
+  if (!IsStatusOK (UhcDev, StatusReg)) {
+
+    ClearStatusReg (UhcDev, StatusReg);
+    *TransferResult |= EFI_USB_ERR_SYSTEM;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ClearStatusReg (UhcDev, StatusReg);
+
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  OUT UINT8                         *PortNumber
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortControl;
+  UINT32      Index;
+
+  UhcDev = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *PortNumber = 0;
+
+  for (Index = 0; Index < 2; Index++) {
+    PSAddr = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + Index * 2;
+    RHPortControl = USBReadPortW (UhcDev, PSAddr);
+    //
+    // Port Register content is valid
+    //
+    if (RHPortControl != 0xff) {
+      (*PortNumber)++;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortStatus;
+  UINT8       TotalPortNumber;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev                        = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr                        = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  RHPortStatus = USBReadPortW (UhcDev, PSAddr);
+
+  //
+  // Current Connect Status
+  //
+  if ((RHPortStatus & USBPORTSC_CCS) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_CONNECTION;
+  }
+  //
+  // Port Enabled/Disabled
+  //
+  if ((RHPortStatus & USBPORTSC_PED) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_ENABLE;
+  }
+  //
+  // Port Suspend
+  //
+  if ((RHPortStatus & USBPORTSC_SUSP) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+  //
+  // Port Reset
+  //
+  if ((RHPortStatus & USBPORTSC_PR) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_RESET;
+  }
+  //
+  // Low Speed Device Attached
+  //
+  if ((RHPortStatus & USBPORTSC_LSDA) != 0) {
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+  }
+  //
+  //   Fill Port Status Change bits
+  //
+  //
+  // Connect Status Change
+  //
+  if ((RHPortStatus & USBPORTSC_CSC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_CONNECTION;
+  }
+  //
+  // Port Enabled/Disabled Change
+  //
+  if ((RHPortStatus & USBPORTSC_PEDC) != 0) {
+    PortStatus->PortChangeStatus |= USB_PORT_STAT_C_ENABLE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT32      CommandRegAddr;
+  UINT16      RHPortControl;
+  UINT8       TotalPortNumber;
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev          = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr          = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+  CommandRegAddr  = UhcDev->UsbHostControllerBaseAddress + USBCMD;
+
+  RHPortControl = USBReadPortW (UhcDev, PSAddr);
+
+  switch (PortFeature) {
+
+  case EfiUsbPortSuspend:
+    if ((USBReadPortW (UhcDev, CommandRegAddr) & USBCMD_EGSM) == 0) {
+      //
+      // if global suspend is not active, can set port suspend
+      //
+      RHPortControl &= 0xfff5;
+      RHPortControl |= USBPORTSC_SUSP;
+    }
+    break;
+
+  case EfiUsbPortReset:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PR;
+    //
+    // Set the reset bit
+    //
+    break;
+
+  case EfiUsbPortPower:
+    break;
+
+  case EfiUsbPortEnable:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PED;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  USBWritePortW (UhcDev, PSAddr, RHPortControl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  USB_UHC_DEV *UhcDev;
+  UINT32      PSAddr;
+  UINT16      RHPortControl;
+  UINT8       TotalPortNumber;
+
+  UhcGetRootHubPortNumber (PeiServices, This, &TotalPortNumber);
+
+  if (PortNumber > TotalPortNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UhcDev  = PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS (This);
+  PSAddr  = UhcDev->UsbHostControllerBaseAddress + USBPORTSC1 + PortNumber * 2;
+
+  RHPortControl = USBReadPortW (UhcDev, PSAddr);
+
+  switch (PortFeature) {
+  //
+  // clear PORT_ENABLE feature means disable port.
+  //
+  case EfiUsbPortEnable:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_PED;
+    break;
+
+  //
+  // clear PORT_SUSPEND feature means resume the port.
+  // (cause a resume on the specified port if in suspend mode)
+  //
+  case EfiUsbPortSuspend:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_SUSP;
+    break;
+
+  //
+  // no operation
+  //
+  case EfiUsbPortPower:
+    break;
+
+  //
+  // clear PORT_RESET means clear the reset signal.
+  //
+  case EfiUsbPortReset:
+    RHPortControl &= 0xfff5;
+    RHPortControl &= ~USBPORTSC_PR;
+    break;
+
+  //
+  // clear connect status change
+  //
+  case EfiUsbPortConnectChange:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_CSC;
+    break;
+
+  //
+  // clear enable/disable status change
+  //
+  case EfiUsbPortEnableChange:
+    RHPortControl &= 0xfff5;
+    RHPortControl |= USBPORTSC_PEDC;
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortSuspendChange:
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortOverCurrentChange:
+    break;
+
+  //
+  // root hub does not support this request
+  //
+  case EfiUsbPortResetChange:
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  USBWritePortW (UhcDev, PSAddr, RHPortControl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI.
+
+  @param  UhcDev                 UHCI Device.
+
+  @retval EFI_SUCCESS            UHCI successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Resource can not be allocated.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN USB_UHC_DEV          *UhcDev
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      FrameListBaseAddrReg;
+  UINT32      CommandReg;
+  UINT16      Command;
+
+  //
+  // Create and Initialize Frame List For the Host Controller.
+  //
+  Status = CreateFrameList (UhcDev);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FrameListBaseAddrReg  = UhcDev->UsbHostControllerBaseAddress + USBFLBASEADD;
+  CommandReg            = UhcDev->UsbHostControllerBaseAddress + USBCMD;
+
+  //
+  // Set Frame List Base Address to the specific register to inform the hardware.
+  //
+  SetFrameListBaseAddress (UhcDev, FrameListBaseAddrReg, (UINT32) (UINTN) (UhcDev->FrameListEntry));
+
+  Command = USBReadPortW (UhcDev, CommandReg);
+  Command |= USBCMD_GRESET;
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  MicroSecondDelay (50 * 1000);
+
+
+  Command &= ~USBCMD_GRESET;
+
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  //
+  //UHCI spec page120 reset recovery time
+  //
+  MicroSecondDelay (20 * 1000);
+
+  //
+  // Set Run/Stop bit to 1.
+  //
+  Command = USBReadPortW (UhcDev, CommandReg);
+  Command |= USBCMD_RS | USBCMD_MAXP;
+  USBWritePortW (UhcDev, CommandReg, Command);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create Frame List Structure.
+
+  @param  UhcDev                 UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateFrameList (
+  USB_UHC_DEV             *UhcDev
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  FrameListBaseAddr;
+  FRAMELIST_ENTRY       *FrameListPtr;
+  UINTN                 Index;
+
+  //
+  // The Frame List ocupies 4K bytes,
+  // and must be aligned on 4-Kbyte boundaries.
+  //
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             1,
+             &FrameListBaseAddr
+             );
+
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  //Create Control QH and Bulk QH and link them into Framelist Entry
+  //
+  Status = CreateQH(UhcDev, &UhcDev->ConfigQH);
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ASSERT (UhcDev->ConfigQH != NULL);
+
+  Status = CreateQH(UhcDev, &UhcDev->BulkQH);
+  if (Status != EFI_SUCCESS) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ASSERT (UhcDev->BulkQH != NULL);
+
+  //
+  //Set the corresponding QH pointer 
+  //
+  SetQHHorizontalLinkPtr(UhcDev->ConfigQH, UhcDev->BulkQH);
+  SetQHHorizontalQHorTDSelect (UhcDev->ConfigQH, TRUE);
+  SetQHHorizontalValidorInvalid (UhcDev->ConfigQH, TRUE);
+
+  UhcDev->FrameListEntry = (FRAMELIST_ENTRY *) ((UINTN) FrameListBaseAddr);
+
+  FrameListPtr = UhcDev->FrameListEntry;
+
+  for (Index = 0; Index < 1024; Index++) {
+    FrameListPtr->FrameListPtrTerminate = 0;
+    FrameListPtr->FrameListPtr          = (UINT32)(UINTN)UhcDev->ConfigQH >> 4;
+    FrameListPtr->FrameListPtrQSelect   = 1;
+    FrameListPtr->FrameListRsvd         = 0;
+    FrameListPtr ++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read a 16bit width data from Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+
+  @retval the register content read.
+
+**/
+UINT16
+USBReadPortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port
+  )
+{
+  return IoRead16 (Port);
+}
+
+/**
+  Write a 16bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT16        Data
+  )
+{
+  IoWrite16 (Port, Data);
+}
+
+/**
+  Write a 32bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortDW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT32        Data
+  )
+{
+  IoWrite32 (Port, Data);
+}
+
+/**
+  Clear the content of UHCI's Status Register.
+  
+  @param  UhcDev       The UHCI device.
+  @param  StatusAddr   The IO space address of the register.
+
+**/
+VOID
+ClearStatusReg (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        StatusAddr
+  )
+{
+  //
+  // Clear the content of UHCI's Status Register
+  //
+  USBWritePortW (UhcDev, StatusAddr, 0x003F);
+}
+
+/**
+  Check whether the host controller operates well.
+
+  @param  UhcDev        The UHCI device.
+  @param  StatusRegAddr The io address of status register.
+
+  @retval TRUE          Host controller is working.
+  @retval FALSE         Host controller is halted or system error.
+
+**/
+BOOLEAN
+IsStatusOK (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT32          StatusRegAddr
+  )
+{
+  UINT16  StatusValue;
+
+  StatusValue = USBReadPortW (UhcDev, StatusRegAddr);
+
+  if ((StatusValue & (USBSTS_HCPE | USBSTS_HSE | USBSTS_HCH)) != 0) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Get Current Frame Number.
+
+  @param  UhcDev          The UHCI device.
+  @param  FrameNumberAddr The address of frame list register.
+
+  @retval The content of the frame list register.
+
+**/
+UINT16
+GetCurrentFrameNumber (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameNumberAddr
+  )
+{
+  //
+  // Gets value in the USB frame number register.
+  //
+  return (UINT16) (USBReadPortW (UhcDev, FrameNumberAddr) & 0x03FF);
+}
+
+/**
+  Set Frame List Base Address.
+
+  @param  UhcDev           The UHCI device.
+  @param  FrameListRegAddr The address of frame list register.
+  @param  Addr             The address of frame list table.
+
+**/
+VOID
+SetFrameListBaseAddress (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameListRegAddr,
+  IN UINT32        Addr
+  )
+{
+  //
+  // Sets value in the USB Frame List Base Address register.
+  //
+  USBWritePortDW (UhcDev, FrameListRegAddr, (UINT32) (Addr & 0xFFFFF000));
+}
+
+/**
+  Create QH and initialize.
+
+  @param  UhcDev               The UHCI device.
+  @param  PtrQH                Place to store QH_STRUCT pointer.
+
+  @retval EFI_OUT_OF_RESOURCES Can't allocate memory resources.
+  @retval EFI_SUCCESS        Success.
+
+**/
+EFI_STATUS
+CreateQH (
+  IN  USB_UHC_DEV   *UhcDev,
+  OUT QH_STRUCT     **PtrQH
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // allocate align memory for QH_STRUCT
+  //
+  Status = AllocateTDorQHStruct (UhcDev, sizeof(QH_STRUCT), (void **)PtrQH);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // init each field of the QH_STRUCT
+  //
+  SetQHHorizontalValidorInvalid (*PtrQH, FALSE);
+  SetQHVerticalValidorInvalid (*PtrQH, FALSE);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the horizontal link pointer in QH.
+
+  @param  PtrQH               Place to store QH_STRUCT pointer.
+  @param  PtrNext             Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  )
+{
+  //
+  // Since the QH_STRUCT is aligned on 16-byte boundaries,
+  // Only the highest 28bit of the address is valid
+  // (take 32bit address as an example).
+  //
+  PtrQH->QueueHead.QHHorizontalPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Get the horizontal link pointer in QH.
+
+  @param  PtrQH     Place to store QH_STRUCT pointer.
+
+  @retval The horizontal link pointer in QH.
+
+**/
+VOID *
+GetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH
+  )
+{
+  //
+  // Restore the 28bit address to 32bit address
+  // (take 32bit address as an example)
+  //
+  return (VOID *) (UINTN) ((PtrQH->QueueHead.QHHorizontalPtr) << 4);
+}
+
+/**
+  Set a QH or TD horizontally to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHHorizontalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  )
+{
+  //
+  // if QH is connected, the specified bit is set,
+  // if TD is connected, the specified bit is cleared.
+  //
+  PtrQH->QueueHead.QHHorizontalQSelect = IsQH ? 1 : 0;
+}
+
+/**
+  Set the horizontal validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the horizontal linker is valid or not.
+
+**/
+VOID
+SetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  )
+{
+  //
+  // Valid means the horizontal link pointer is valid,
+  // else, it's invalid.
+  //
+  PtrQH->QueueHead.QHHorizontalTerminate = IsValid ? 0 : 1;
+}
+
+/**
+  Set the vertical link pointer in QH.
+
+  @param  PtrQH       Place to store QH_STRUCT pointer.
+  @param  PtrNext     Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHVerticalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  )
+{
+  //
+  // Since the QH_STRUCT is aligned on 16-byte boundaries,
+  // Only the highest 28bit of the address is valid
+  // (take 32bit address as an example).
+  //
+  PtrQH->QueueHead.QHVerticalPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Set a QH or TD vertically to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHVerticalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  )
+{
+  //
+  // Set the specified bit if the Vertical Link Pointer pointing to a QH,
+  // Clear the specified bit if the Vertical Link Pointer pointing to a TD.
+  //
+  PtrQH->QueueHead.QHVerticalQSelect = IsQH ? 1 : 0;
+}
+
+/**
+  Set the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the vertical linker is valid or not.
+
+**/
+VOID
+SetQHVerticalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  )
+{
+  //
+  // If TRUE, meaning the Vertical Link Pointer field is valid,
+  // else, the field is invalid.
+  //
+  PtrQH->QueueHead.QHVerticalTerminate = IsValid ? 0 : 1;
+}
+
+/**
+  Get the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+
+  @retval The vertical linker is valid or not.
+
+**/
+BOOLEAN
+GetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH
+  )
+{
+  //
+  // If TRUE, meaning the Horizontal Link Pointer field is valid,
+  // else, the field is invalid.
+  //
+  return (BOOLEAN) (!(PtrQH->QueueHead.QHHorizontalTerminate));
+}
+
+/**
+  Allocate TD or QH Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Size                   The size of allocation.
+  @param  PtrStruct              Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+AllocateTDorQHStruct (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT32          Size,
+  OUT VOID            **PtrStruct
+  )
+{
+  EFI_STATUS  Status;
+
+  Status      = EFI_SUCCESS;
+  *PtrStruct  = NULL;
+
+  Status = UhcAllocatePool (
+            UhcDev,
+            (UINT8 **) PtrStruct,
+            Size
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ZeroMem (*PtrStruct, Size);
+
+  return Status;
+}
+
+/**
+  Create a TD Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  PtrTD                  Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  EFI_STATUS  Status;
+  //
+  // create memory for TD_STRUCT, and align the memory.
+  //
+  Status = AllocateTDorQHStruct (UhcDev, sizeof(TD_STRUCT), (void **)PtrTD);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Make TD ready.
+  //
+  SetTDLinkPtrValidorInvalid (*PtrTD, FALSE);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Setup Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  DeviceSpeed  Device Speed.
+  @param  DevRequest   Device reuquest.
+  @param  RequestLen   Request length.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate setup stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenSetupStageTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           DeviceSpeed,
+  IN  UINT8           *DevRequest,
+  IN  UINT8           RequestLen,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *TdStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &TdStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (TdStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (TdStruct, TRUE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (TdStruct, FALSE);
+
+  //
+  // Disable Short Packet Detection by default
+  //
+  EnableorDisableTDShortPacket (TdStruct, FALSE);
+
+  //
+  // Max error counter is 3, retry 3 times when error encountered.
+  //
+  SetTDControlErrorCounter (TdStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (TdStruct, FALSE);
+
+  //
+  // Interrupt On Complete bit be set to zero,
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (TdStruct, FALSE);
+
+  //
+  // Set TD Active bit
+  //
+  SetTDStatusActiveorInactive (TdStruct, TRUE);
+
+  SetTDTokenMaxLength (TdStruct, RequestLen);
+
+  SetTDTokenDataToggle0 (TdStruct);
+
+  SetTDTokenEndPoint (TdStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (TdStruct, DevAddr);
+
+  SetTDTokenPacketID (TdStruct, SETUP_PACKET_ID);
+
+  TdStruct->PtrTDBuffer      = (UINT8 *) DevRequest;
+  TdStruct->TDBufferLength = RequestLen;
+  SetTDDataBuffer (TdStruct);
+
+  *PtrTD = TdStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Data Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PtrData      Data buffer.
+  @param  Len          Data length.
+  @param  PktID        PacketID.
+  @param  Toggle       Data toggle value.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate data stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenDataTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           *PtrData,
+  IN  UINT8           Len,
+  IN  UINT8           PktID,
+  IN  UINT8           Toggle,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *TdStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &TdStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (TdStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (TdStruct, TRUE);
+
+  //
+  // Link pointer pointing to TD struct
+  //
+  SetTDLinkPtrQHorTDSelect (TdStruct, FALSE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (TdStruct, FALSE);
+
+  //
+  // Disable short packet detect
+  //
+  EnableorDisableTDShortPacket (TdStruct, FALSE);
+  //
+  // Max error counter is 3
+  //
+  SetTDControlErrorCounter (TdStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (TdStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (TdStruct, FALSE);
+
+  //
+  // Disable Interrupt On Complete
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (TdStruct, FALSE);
+
+  //
+  // Set Active bit
+  //
+  SetTDStatusActiveorInactive (TdStruct, TRUE);
+
+  SetTDTokenMaxLength (TdStruct, Len);
+
+  if (Toggle != 0) {
+    SetTDTokenDataToggle1 (TdStruct);
+  } else {
+    SetTDTokenDataToggle0 (TdStruct);
+  }
+
+  SetTDTokenEndPoint (TdStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (TdStruct, DevAddr);
+
+  SetTDTokenPacketID (TdStruct, PktID);
+
+  TdStruct->PtrTDBuffer      = (UINT8 *) PtrData;
+  TdStruct->TDBufferLength = Len;
+  SetTDDataBuffer (TdStruct);
+
+  *PtrTD = TdStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate Status Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PktID        PacketID.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate status stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateStatusTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           PktID,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  )
+{
+  TD_STRUCT   *PtrTDStruct;
+  EFI_STATUS  Status;
+
+  Status = CreateTD (UhcDev, &PtrTDStruct);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SetTDLinkPtr (PtrTDStruct, NULL);
+
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (PtrTDStruct, TRUE);
+
+  //
+  // initialize as the last TD in the QH context,
+  // this field will be updated in the TD linkage process.
+  //
+  SetTDLinkPtrValidorInvalid (PtrTDStruct, FALSE);
+
+  //
+  // Disable short packet detect
+  //
+  EnableorDisableTDShortPacket (PtrTDStruct, FALSE);
+
+  //
+  // Max error counter is 3
+  //
+  SetTDControlErrorCounter (PtrTDStruct, 3);
+
+  //
+  // set device speed attribute
+  // (TRUE - Slow Device; FALSE - Full Speed Device)
+  //
+  switch (DeviceSpeed) {
+  case USB_SLOW_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (PtrTDStruct, TRUE);
+    break;
+
+  case USB_FULL_SPEED_DEVICE:
+    SetTDLoworFullSpeedDevice (PtrTDStruct, FALSE);
+    break;
+  }
+  //
+  // Non isochronous transfer TD
+  //
+  SetTDControlIsochronousorNot (PtrTDStruct, FALSE);
+
+  //
+  // Disable Interrupt On Complete
+  // Disable IOC interrupt.
+  //
+  SetorClearTDControlIOC (PtrTDStruct, FALSE);
+
+  //
+  // Set TD Active bit
+  //
+  SetTDStatusActiveorInactive (PtrTDStruct, TRUE);
+
+  SetTDTokenMaxLength (PtrTDStruct, 0);
+
+  SetTDTokenDataToggle1 (PtrTDStruct);
+
+  SetTDTokenEndPoint (PtrTDStruct, Endpoint);
+
+  SetTDTokenDeviceAddress (PtrTDStruct, DevAddr);
+
+  SetTDTokenPacketID (PtrTDStruct, PktID);
+
+  PtrTDStruct->PtrTDBuffer      = NULL;
+  PtrTDStruct->TDBufferLength = 0;
+  SetTDDataBuffer (PtrTDStruct);
+
+  *PtrTD = PtrTDStruct;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the link pointer validor bit in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsValid      Specify the linker pointer is valid or not.
+
+**/
+VOID
+SetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsValid
+  )
+{
+  //
+  // Valid means the link pointer is valid,
+  // else, it's invalid.
+  //
+  PtrTDStruct->TDData.TDLinkPtrTerminate = (IsValid ? 0 : 1);
+}
+
+/**
+  Set the Link Pointer pointing to a QH or TD.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsQH          Specify QH or TD is connected.
+
+**/
+VOID
+SetTDLinkPtrQHorTDSelect (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsQH
+  )
+{
+  //
+  // Indicate whether the Link Pointer pointing to a QH or TD
+  //
+  PtrTDStruct->TDData.TDLinkPtrQSelect = (IsQH ? 1 : 0);
+}
+
+/**
+  Set the traverse is depth-first or breadth-first.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsDepth       Specify the traverse is depth-first or breadth-first.
+
+**/
+VOID
+SetTDLinkPtrDepthorBreadth (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsDepth
+  )
+{
+  //
+  // If TRUE, indicating the host controller should process in depth first fashion,
+  // else, the host controller should process in breadth first fashion
+  //
+  PtrTDStruct->TDData.TDLinkPtrDepthSelect = (IsDepth ? 1 : 0);
+}
+
+/**
+  Set TD Link Pointer in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  PtrNext      Place to the next TD_STRUCT.
+
+**/
+VOID
+SetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  VOID      *PtrNext
+  )
+{
+  //
+  // Set TD Link Pointer. Since QH,TD align on 16-byte boundaries,
+  // only the highest 28 bits are valid. (if take 32bit address as an example)
+  //
+  PtrTDStruct->TDData.TDLinkPtr = (UINT32) (UINTN) PtrNext >> 4;
+}
+
+/**
+  Get TD Link Pointer.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval Get TD Link Pointer in TD.
+
+**/
+VOID *
+GetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Get TD Link Pointer. Restore it back to 32bit
+  // (if take 32bit address as an example)
+  //
+  return (VOID *) (UINTN) ((PtrTDStruct->TDData.TDLinkPtr) << 4);
+}
+
+/**
+  Get the information about whether the Link Pointer field pointing to
+  a QH or a TD.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval whether the Link Pointer field pointing to a QH or a TD.
+
+**/
+BOOLEAN
+IsTDLinkPtrQHOrTD (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Get the information about whether the Link Pointer field pointing to
+  // a QH or a TD.
+  //
+  return (BOOLEAN) (PtrTDStruct->TDData.TDLinkPtrQSelect);
+}
+
+/**
+  Enable/Disable short packet detection mechanism.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsEnable     Enable or disable short packet detection mechanism.
+
+**/
+VOID
+EnableorDisableTDShortPacket (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsEnable
+  )
+{
+  //
+  // TRUE means enable short packet detection mechanism.
+  //
+  PtrTDStruct->TDData.TDStatusSPD = (IsEnable ? 1 : 0);
+}
+
+/**
+  Set the max error counter in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  MaxErrors    The number of allowable error.
+
+**/
+VOID
+SetTDControlErrorCounter (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     MaxErrors
+  )
+{
+  //
+  // valid value of MaxErrors is 0,1,2,3
+  //
+  if (MaxErrors > 3) {
+    MaxErrors = 3;
+  }
+
+  PtrTDStruct->TDData.TDStatusErr = MaxErrors;
+}
+
+/**
+  Set the TD is targeting a low-speed device or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsLowSpeedDevice  Whether The device is low-speed.
+
+**/
+VOID
+SetTDLoworFullSpeedDevice (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsLowSpeedDevice
+  )
+{
+  //
+  // TRUE means the TD is targeting at a Low-speed device
+  //
+  PtrTDStruct->TDData.TDStatusLS = (IsLowSpeedDevice ? 1 : 0);
+}
+
+/**
+  Set the TD is isochronous transfer type or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsIsochronous     Whether the transaction isochronous transfer type.
+
+**/
+VOID
+SetTDControlIsochronousorNot (
+  IN  TD_STRUCT   *PtrTDStruct,
+  IN  BOOLEAN     IsIsochronous
+  )
+{
+  //
+  // TRUE means the TD belongs to Isochronous transfer type.
+  //
+  PtrTDStruct->TDData.TDStatusIOS = (IsIsochronous ? 1 : 0);
+}
+
+/**
+  Set if UCHI should issue an interrupt on completion of the frame
+  in which this TD is executed
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsSet             Whether HC should issue an interrupt on completion.
+
+**/
+VOID
+SetorClearTDControlIOC (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsSet
+  )
+{
+  //
+  // If this bit is set, it indicates that the host controller should issue
+  // an interrupt on completion of the frame in which this TD is executed.
+  //
+  PtrTDStruct->TDData.TDStatusIOC = IsSet ? 1 : 0;
+}
+
+/**
+  Set if the TD is active and can be executed.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsActive          Whether the TD is active and can be executed.
+
+**/
+VOID
+SetTDStatusActiveorInactive (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsActive
+  )
+{
+  //
+  // If this bit is set, it indicates that the TD is active and can be
+  // executed.
+  //
+  if (IsActive) {
+    PtrTDStruct->TDData.TDStatus |= 0x80;
+  } else {
+    PtrTDStruct->TDData.TDStatus &= 0x7F;
+  }
+}
+
+/**
+  Specifies the maximum number of data bytes allowed for the transfer.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  MaxLen        The maximum number of data bytes allowed.
+
+  @retval The allowed maximum number of data.
+**/
+UINT16
+SetTDTokenMaxLength (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT16    MaxLen
+  )
+{
+  //
+  // Specifies the maximum number of data bytes allowed for the transfer.
+  // the legal value extent is 0 ~ 0x500.
+  //
+  if (MaxLen > 0x500) {
+    MaxLen = 0x500;
+  }
+
+  PtrTDStruct->TDData.TDTokenMaxLen = MaxLen - 1;
+
+  return MaxLen;
+}
+
+/**
+  Set the data toggle bit to DATA1.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle1 (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the data toggle bit to DATA1
+  //
+  PtrTDStruct->TDData.TDTokenDataToggle = 1;
+}
+
+/**
+  Set the data toggle bit to DATA0.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle0 (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the data toggle bit to DATA0
+  //
+  PtrTDStruct->TDData.TDTokenDataToggle = 0;
+}
+
+/**
+  Set EndPoint Number the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  EndPoint      The Endport number of the target.
+
+**/
+VOID
+SetTDTokenEndPoint (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     EndPoint
+  )
+{
+  //
+  // Set EndPoint Number the TD is targeting at.
+  //
+  PtrTDStruct->TDData.TDTokenEndPt = (UINT8) EndPoint;
+}
+
+/**
+  Set Device Address the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  DevAddr       The Device Address of the target.
+
+**/
+VOID
+SetTDTokenDeviceAddress (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     DevAddr
+  )
+{
+  //
+  // Set Device Address the TD is targeting at.
+  //
+  PtrTDStruct->TDData.TDTokenDevAddr = (UINT8) DevAddr;
+}
+
+/**
+  Set Packet Identification the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  PacketID      The Packet Identification of the target.
+
+**/
+VOID
+SetTDTokenPacketID (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     PacketID
+  )
+{
+  //
+  // Set the Packet Identification to be used for this transaction.
+  //
+  PtrTDStruct->TDData.TDTokenPID = PacketID;
+}
+
+/**
+  Set the beginning address of the data buffer that will be used
+  during the transaction.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDDataBuffer (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Set the beginning address of the data buffer that will be used
+  // during the transaction.
+  //
+  PtrTDStruct->TDData.TDBufferPtr = (UINT32) (UINTN) (PtrTDStruct->PtrTDBuffer);
+}
+
+/**
+  Detect whether the TD is active.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is active or not.
+
+**/
+BOOLEAN
+IsTDStatusActive (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether the TD is active.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x80);
+}
+
+/**
+  Detect whether the TD is stalled.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is stalled or not.
+
+**/
+BOOLEAN
+IsTDStatusStalled (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether the device/endpoint addressed by this TD is stalled.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x40);
+}
+
+/**
+  Detect whether Data Buffer Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Data Buffer Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBufferError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Data Buffer Error is happened.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x20);
+}
+
+/**
+  Detect whether Babble Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Babble Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBabbleError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Babble Error is happened.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x10);
+}
+
+/**
+  Detect whether NAK is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The NAK is received or not.
+
+**/
+BOOLEAN
+IsTDStatusNAKReceived (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether NAK is received.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x08);
+}
+
+/**
+  Detect whether CRC/Time Out Error is encountered.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The CRC/Time Out Error is encountered or not.
+
+**/
+BOOLEAN
+IsTDStatusCRCTimeOutError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether CRC/Time Out Error is encountered.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x04);
+}
+
+/**
+  Detect whether Bitstuff Error is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Bitstuff Error is received or not.
+
+**/
+BOOLEAN
+IsTDStatusBitStuffError (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  UINT8 TDStatus;
+
+  //
+  // Detect whether Bitstuff Error is received.
+  //
+  TDStatus = (UINT8) (PtrTDStruct->TDData.TDStatus);
+  return (BOOLEAN) (TDStatus & 0x02);
+}
+
+/**
+  Retrieve the actual number of bytes that were tansferred.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The actual number of bytes that were tansferred.
+
+**/
+UINT16
+GetTDStatusActualLength (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Retrieve the actual number of bytes that were tansferred.
+  // the value is encoded as n-1. so return the decoded value.
+  //
+  return (UINT16) ((PtrTDStruct->TDData.TDStatusActualLength) + 1);
+}
+
+/**
+  Retrieve the information of whether the Link Pointer field is valid or not.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The linker pointer field is valid or not.
+
+**/
+BOOLEAN
+GetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct
+  )
+{
+  //
+  // Retrieve the information of whether the Link Pointer field
+  // is valid or not.
+  //
+  if ((PtrTDStruct->TDData.TDLinkPtrTerminate & BIT0) != 0) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+
+}
+
+/**
+  Count TD Number from PtrFirstTD.
+
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+  @retval The queued TDs number.
+
+**/
+UINTN
+CountTDsNumber (
+  IN  TD_STRUCT *PtrFirstTD
+  )
+{
+  UINTN     Number;
+  TD_STRUCT *Ptr;
+
+  //
+  // Count the queued TDs number.
+  //
+  Number  = 0;
+  Ptr     = PtrFirstTD;
+  while (Ptr != 0) {
+    Ptr = (TD_STRUCT *) Ptr->PtrNextTD;
+    Number++;
+  }
+
+  return Number;
+}
+
+/**
+  Link TD To QH.
+
+  @param  PtrQH   Place to store QH_STRUCT pointer.
+  @param  PtrTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToQH (
+  IN  QH_STRUCT *PtrQH,
+  IN  TD_STRUCT *PtrTD
+  )
+{
+  if (PtrQH == NULL || PtrTD == NULL) {
+    return ;
+  }
+  //
+  //  Validate QH Vertical Ptr field
+  //
+  SetQHVerticalValidorInvalid (PtrQH, TRUE);
+
+  //
+  //  Vertical Ptr pointing to TD structure
+  //
+  SetQHVerticalQHorTDSelect (PtrQH, FALSE);
+
+  SetQHVerticalLinkPtr (PtrQH, (VOID *) PtrTD);
+
+  PtrQH->PtrDown = (VOID *) PtrTD;
+}
+
+/**
+  Link TD To TD.
+
+  @param  PtrPreTD  Place to store TD_STRUCT pointer.
+  @param  PtrTD     Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToTD (
+  IN  TD_STRUCT *PtrPreTD,
+  IN  TD_STRUCT *PtrTD
+  )
+{
+  if (PtrPreTD == NULL || PtrTD == NULL) {
+    return ;
+  }
+  //
+  // Depth first fashion
+  //
+  SetTDLinkPtrDepthorBreadth (PtrPreTD, TRUE);
+
+  //
+  // Link pointer pointing to TD struct
+  //
+  SetTDLinkPtrQHorTDSelect (PtrPreTD, FALSE);
+
+  //
+  // Validate the link pointer valid bit
+  //
+  SetTDLinkPtrValidorInvalid (PtrPreTD, TRUE);
+
+  SetTDLinkPtr (PtrPreTD, PtrTD);
+
+  PtrPreTD->PtrNextTD = (VOID *) PtrTD;
+
+  PtrTD->PtrNextTD    = NULL;
+}
+
+/**
+  Execute Control Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecuteControlTransfer (
+  IN  USB_UHC_DEV *UhcDev,
+  IN  TD_STRUCT   *PtrTD,
+  OUT UINTN       *ActualLen,
+  IN  UINTN       TimeOut,
+  OUT UINT32      *TransferResult
+  )
+{
+  UINTN   ErrTDPos;
+  UINTN   Delay;
+  BOOLEAN InfiniteLoop;
+
+  ErrTDPos          = 0;
+  *TransferResult   = EFI_USB_NOERROR;
+  *ActualLen        = 0;
+  InfiniteLoop      = FALSE;
+
+  Delay = TimeOut * STALL_1_MILLI_SECOND;
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  do {
+
+    CheckTDsResults (PtrTD, TransferResult, &ErrTDPos, ActualLen);
+
+    //
+    // TD is inactive, means the control transfer is end.
+    //
+    if ((*TransferResult & EFI_USB_ERR_NOTEXECUTE) != EFI_USB_ERR_NOTEXECUTE) {
+      break;
+    }
+    MicroSecondDelay (STALL_1_MICRO_SECOND);
+    Delay--;
+
+  } while (InfiniteLoop || (Delay != 0));
+
+  if (*TransferResult != EFI_USB_NOERROR) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute Bulk Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  DataToggle        DataToggle value.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecBulkTransfer (
+  IN     USB_UHC_DEV *UhcDev,
+  IN     TD_STRUCT   *PtrTD,
+  IN OUT UINTN       *ActualLen,
+  IN     UINT8       *DataToggle,
+  IN     UINTN       TimeOut,
+  OUT    UINT32      *TransferResult
+  )
+{
+  UINTN   ErrTDPos;
+  UINTN   ScrollNum;
+  UINTN   Delay;
+  BOOLEAN InfiniteLoop;
+
+  ErrTDPos          = 0;
+  *TransferResult   = EFI_USB_NOERROR;
+  *ActualLen        = 0;
+  InfiniteLoop      = FALSE;
+
+  Delay = TimeOut * STALL_1_MILLI_SECOND;
+  //
+  // If Timeout is 0, then the caller must wait for the function to be completed
+  // until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  //
+  if (TimeOut == 0) {
+    InfiniteLoop = TRUE;
+  }
+
+  do {
+
+    CheckTDsResults (PtrTD, TransferResult, &ErrTDPos, ActualLen);
+    //
+    // TD is inactive, thus meaning bulk transfer's end.
+    //
+    if ((*TransferResult & EFI_USB_ERR_NOTEXECUTE) != EFI_USB_ERR_NOTEXECUTE) {
+      break;
+    }
+    MicroSecondDelay (STALL_1_MICRO_SECOND);
+    Delay--;
+
+  } while (InfiniteLoop || (Delay != 0));
+
+  //
+  // has error
+  //
+  if (*TransferResult != EFI_USB_NOERROR) {
+    //
+    // scroll the Data Toggle back to the last success TD
+    //
+    ScrollNum = CountTDsNumber (PtrTD) - ErrTDPos;
+    if ((ScrollNum % 2) != 0) {
+      *DataToggle ^= 1;
+    }
+
+  //
+  // If error, wait 100ms to retry by upper layer
+  //
+    MicroSecondDelay (100 * 1000);
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Delete Queued TDs.
+
+  @param  UhcDev       The UCHI device.
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+DeleteQueuedTDs (
+  IN USB_UHC_DEV     *UhcDev,
+  IN TD_STRUCT       *PtrFirstTD
+  )
+{
+  TD_STRUCT *Tptr1;
+
+  TD_STRUCT *Tptr2;
+
+  Tptr1 = PtrFirstTD;
+  //
+  // Delete all the TDs in a queue.
+  //
+  while (Tptr1 != NULL) {
+
+    Tptr2 = Tptr1;
+
+    if (!GetTDLinkPtrValidorInvalid (Tptr2)) {
+      Tptr1 = NULL;
+    } else {
+      //
+      // has more than one TD in the queue.
+      //
+      Tptr1 = GetTDLinkPtr (Tptr2);
+    }
+
+    UhcFreePool (UhcDev, (UINT8 *) Tptr2, sizeof (TD_STRUCT));
+  }
+
+  return ;
+}
+
+/**
+  Check TDs Results.
+
+  @param  PtrTD               A pointer to TD_STRUCT data.
+  @param  Result              The result to return.
+  @param  ErrTDPos            The Error TD position.
+  @param  ActualTransferSize  Actual transfer size.
+
+  @retval The TD is executed successfully or not.
+
+**/
+BOOLEAN
+CheckTDsResults (
+  IN  TD_STRUCT               *PtrTD,
+  OUT UINT32                  *Result,
+  OUT UINTN                   *ErrTDPos,
+  OUT UINTN                   *ActualTransferSize
+  )
+{
+  UINTN Len;
+
+  *Result   = EFI_USB_NOERROR;
+  *ErrTDPos = 0;
+
+  //
+  // Init to zero.
+  //
+  *ActualTransferSize = 0;
+
+  while (PtrTD != NULL) {
+
+    if (IsTDStatusActive (PtrTD)) {
+      *Result |= EFI_USB_ERR_NOTEXECUTE;
+    }
+
+    if (IsTDStatusStalled (PtrTD)) {
+      *Result |= EFI_USB_ERR_STALL;
+    }
+
+    if (IsTDStatusBufferError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BUFFER;
+    }
+
+    if (IsTDStatusBabbleError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BABBLE;
+    }
+
+    if (IsTDStatusNAKReceived (PtrTD)) {
+      *Result |= EFI_USB_ERR_NAK;
+    }
+
+    if (IsTDStatusCRCTimeOutError (PtrTD)) {
+      *Result |= EFI_USB_ERR_TIMEOUT;
+    }
+
+    if (IsTDStatusBitStuffError (PtrTD)) {
+      *Result |= EFI_USB_ERR_BITSTUFF;
+    }
+    //
+    // Accumulate actual transferred data length in each TD.
+    //
+    Len = GetTDStatusActualLength (PtrTD) & 0x7FF;
+    *ActualTransferSize += Len;
+
+    //
+    // if any error encountered, stop processing the left TDs.
+    //
+    if ((*Result) != 0) {
+      return FALSE;
+    }
+
+    PtrTD = (TD_STRUCT *) (PtrTD->PtrNextTD);
+    //
+    // Record the first Error TD's position in the queue,
+    // this value is zero-based.
+    //
+    (*ErrTDPos)++;
+  }
+
+  return TRUE;
+}
+
+/**
+  Create Memory Block.
+
+  @param  UhcDev                   The UCHI device.
+  @param  MemoryHeader             The Pointer to allocated memory block.
+  @param  MemoryBlockSizeInPages   The page size of memory block to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateMemoryBlock (
+  IN  USB_UHC_DEV           *UhcDev,
+  OUT MEMORY_MANAGE_HEADER  **MemoryHeader,
+  IN  UINTN                 MemoryBlockSizeInPages
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+  UINTN                 MemPages;
+  UINT8                 *Ptr;
+
+  //
+  // Memory Block uses MemoryBlockSizeInPages pages,
+  // memory management header and bit array use 1 page
+  //
+  MemPages = MemoryBlockSizeInPages + 1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             MemPages,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ptr = (UINT8 *) ((UINTN) TempPtr);
+
+  ZeroMem (Ptr, MemPages * EFI_PAGE_SIZE);
+
+  *MemoryHeader = (MEMORY_MANAGE_HEADER *) Ptr;
+  //
+  // adjust Ptr pointer to the next empty memory
+  //
+  Ptr += sizeof (MEMORY_MANAGE_HEADER);
+  //
+  // Set Bit Array initial address
+  //
+  (*MemoryHeader)->BitArrayPtr  = Ptr;
+
+  (*MemoryHeader)->Next         = NULL;
+
+  //
+  // Memory block initial address
+  //
+  Ptr = (UINT8 *) ((UINTN) TempPtr);
+  Ptr += EFI_PAGE_SIZE;
+  (*MemoryHeader)->MemoryBlockPtr = Ptr;
+  //
+  // set Memory block size
+  //
+  (*MemoryHeader)->MemoryBlockSizeInBytes = MemoryBlockSizeInPages * EFI_PAGE_SIZE;
+  //
+  // each bit in Bit Array will manage 32byte memory in memory block
+  //
+  (*MemoryHeader)->BitArraySizeInBytes = ((*MemoryHeader)->MemoryBlockSizeInBytes / 32) / 8;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev                 The UCHI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+InitializeMemoryManagement (
+  IN USB_UHC_DEV           *UhcDev
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  EFI_STATUS            Status;
+  UINTN                 MemPages;
+
+  MemPages  = NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES;
+  Status    = CreateMemoryBlock (UhcDev, &MemoryHeader, MemPages);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UhcDev->Header1 = MemoryHeader;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev           The UCHI device.
+  @param  Pool             Buffer pointer to store the buffer pointer.
+  @param  AllocSize        The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhcAllocatePool (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT UINT8           **Pool,
+  IN  UINTN           AllocSize
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+  MEMORY_MANAGE_HEADER  *NewMemoryHeader;
+  UINTN                 RealAllocSize;
+  UINTN                 MemoryBlockSizeInPages;
+  EFI_STATUS            Status;
+
+  *Pool = NULL;
+
+  MemoryHeader = UhcDev->Header1;
+
+  //
+  // allocate unit is 32 byte (align on 32 byte)
+  //
+  if ((AllocSize & 0x1F) != 0) {
+    RealAllocSize = (AllocSize / 32 + 1) * 32;
+  } else {
+    RealAllocSize = AllocSize;
+  }
+
+  Status = EFI_NOT_FOUND;
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+
+    Status = AllocMemInMemoryBlock (
+              TempHeaderPtr,
+              (VOID **) Pool,
+              RealAllocSize / 32
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+  }
+  //
+  // There is no enough memory,
+  // Create a new Memory Block
+  //
+  //
+  // if pool size is larger than NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES,
+  // just allocate a large enough memory block.
+  //
+  if (RealAllocSize > (NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES * EFI_PAGE_SIZE)) {
+    MemoryBlockSizeInPages = RealAllocSize / EFI_PAGE_SIZE + 1;
+  } else {
+    MemoryBlockSizeInPages = NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES;
+  }
+
+  Status = CreateMemoryBlock (UhcDev, &NewMemoryHeader, MemoryBlockSizeInPages);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Link the new Memory Block to the Memory Header list
+  //
+  InsertMemoryHeaderToList (MemoryHeader, NewMemoryHeader);
+
+  Status = AllocMemInMemoryBlock (
+            NewMemoryHeader,
+            (VOID **) Pool,
+            RealAllocSize / 32
+            );
+  return Status;
+}
+
+/**
+  Alloc Memory In MemoryBlock.
+
+  @param  MemoryHeader           The pointer to memory manage header.
+  @param  Pool                   Buffer pointer to store the buffer pointer.
+  @param  NumberOfMemoryUnit     The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+AllocMemInMemoryBlock (
+  IN  MEMORY_MANAGE_HEADER  *MemoryHeader,
+  OUT VOID                  **Pool,
+  IN  UINTN                 NumberOfMemoryUnit
+  )
+{
+  UINTN TempBytePos;
+  UINTN FoundBytePos;
+  UINT8 Index;
+  UINT8 FoundBitPos;
+  UINT8 ByteValue;
+  UINT8 BitValue;
+  UINTN NumberOfZeros;
+  UINTN Count;
+
+  FoundBytePos  = 0;
+  FoundBitPos   = 0;
+
+  ByteValue     = MemoryHeader->BitArrayPtr[0];
+  NumberOfZeros = 0;
+  Index             = 0;
+  for (TempBytePos = 0; TempBytePos < MemoryHeader->BitArraySizeInBytes;) {
+    //
+    // Pop out BitValue from a byte in TempBytePos.
+    //
+    BitValue = (UINT8)(ByteValue & 0x1);
+
+    if (BitValue == 0) {
+      //
+      // Found a free bit, the NumberOfZeros only record the number of those consecutive zeros
+      //
+      NumberOfZeros++;
+      //
+      // Found enough consecutive free space, break the loop
+      //
+      if (NumberOfZeros >= NumberOfMemoryUnit) {
+        break;
+      }
+    } else {
+      //
+      // Encountering a '1', meant the bit is ocupied.
+      //
+      if (NumberOfZeros >= NumberOfMemoryUnit) {
+        //
+        // Found enough consecutive free space,break the loop
+        //
+        break;
+      } else {
+        //
+        // the NumberOfZeros only record the number of those consecutive zeros,
+        // so reset the NumberOfZeros to 0 when encountering '1' before finding
+        // enough consecutive '0's
+        //
+        NumberOfZeros = 0;
+        //
+        // reset the (FoundBytePos,FoundBitPos) to the position of '1'
+        //
+        FoundBytePos  = TempBytePos;
+        FoundBitPos   = Index;
+      }
+    }
+    //
+    // right shift the byte
+    //
+    ByteValue /= 2;
+
+    //
+    // step forward a bit
+    //
+    Index++;
+    if (Index == 8) {
+      //
+      // step forward a byte, getting the byte value,
+      // and reset the bit pos.
+      //
+      TempBytePos += 1;
+      ByteValue = MemoryHeader->BitArrayPtr[TempBytePos];
+      Index     = 0;
+    }
+  }
+
+  if (NumberOfZeros < NumberOfMemoryUnit) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Found enough free space.
+  //
+  //
+  // The values recorded in (FoundBytePos,FoundBitPos) have two conditions:
+  //  1)(FoundBytePos,FoundBitPos) record the position
+  //    of the last '1' before the consecutive '0's, it must
+  //    be adjusted to the start position of the consecutive '0's.
+  //  2)the start address of the consecutive '0's is just the start of
+  //    the bitarray. so no need to adjust the values of (FoundBytePos,FoundBitPos).
+  //
+  if ((MemoryHeader->BitArrayPtr[0] & BIT0) != 0) {
+    FoundBitPos += 1;
+  }
+  //
+  // Have the (FoundBytePos,FoundBitPos) make sense.
+  //
+  if (FoundBitPos > 7) {
+    FoundBytePos += 1;
+    FoundBitPos -= 8;
+  }
+  //
+  // Set the memory as allocated
+  //
+  for (TempBytePos = FoundBytePos, Index = FoundBitPos, Count = 0; Count < NumberOfMemoryUnit; Count++) {
+
+    MemoryHeader->BitArrayPtr[TempBytePos] = (UINT8) (MemoryHeader->BitArrayPtr[TempBytePos] | (1 << Index));
+    Index++;
+    if (Index == 8) {
+      TempBytePos += 1;
+      Index = 0;
+    }
+  }
+
+  *Pool = MemoryHeader->MemoryBlockPtr + (FoundBytePos * 8 + FoundBitPos) * 32;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Uhci Free Pool.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Pool                   A pointer to store the buffer address.
+  @param  AllocSize              The size of the pool to be freed.
+
+**/
+VOID
+UhcFreePool (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT8           *Pool,
+  IN UINTN           AllocSize
+  )
+{
+  MEMORY_MANAGE_HEADER  *MemoryHeader;
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+  UINTN                 StartBytePos;
+  UINTN                 Index;
+  UINT8                 StartBitPos;
+  UINT8                 Index2;
+  UINTN                 Count;
+  UINTN                 RealAllocSize;
+
+  MemoryHeader = UhcDev->Header1;
+
+  //
+  // allocate unit is 32 byte (align on 32 byte)
+  //
+  if ((AllocSize & 0x1F) != 0) {
+    RealAllocSize = (AllocSize / 32 + 1) * 32;
+  } else {
+    RealAllocSize = AllocSize;
+  }
+
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL;
+       TempHeaderPtr = TempHeaderPtr->Next) {
+
+    if ((Pool >= TempHeaderPtr->MemoryBlockPtr) &&
+        ((Pool + RealAllocSize) <= (TempHeaderPtr->MemoryBlockPtr +
+                                    TempHeaderPtr->MemoryBlockSizeInBytes))) {
+
+      //
+      // Pool is in the Memory Block area,
+      // find the start byte and bit in the bit array
+      //
+      StartBytePos  = ((Pool - TempHeaderPtr->MemoryBlockPtr) / 32) / 8;
+      StartBitPos   = (UINT8) (((Pool - TempHeaderPtr->MemoryBlockPtr) / 32) % 8);
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Index = StartBytePos, Index2 = StartBitPos, Count = 0; Count < (RealAllocSize / 32); Count++) {
+
+        TempHeaderPtr->BitArrayPtr[Index] = (UINT8) (TempHeaderPtr->BitArrayPtr[Index] ^ (1 << Index2));
+        Index2++;
+        if (Index2 == 8) {
+          Index += 1;
+          Index2 = 0;
+        }
+      }
+      //
+      // break the loop
+      //
+      break;
+    }
+  }
+
+}
+
+/**
+  Insert a new memory header into list.
+
+  @param  MemoryHeader         A pointer to the memory header list.
+  @param  NewMemoryHeader      A new memory header to be inserted into the list.
+
+**/
+VOID
+InsertMemoryHeaderToList (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeader,
+  IN MEMORY_MANAGE_HEADER  *NewMemoryHeader
+  )
+{
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+
+  for (TempHeaderPtr = MemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+    if (TempHeaderPtr->Next == NULL) {
+      TempHeaderPtr->Next = NewMemoryHeader;
+      break;
+    }
+  }
+}
+
+/**
+  Judge the memory block in the memory header is empty or not.
+
+  @param  MemoryHeaderPtr   A pointer to the memory header list.
+
+  @retval Whether the memory block in the memory header is empty or not.
+
+**/
+BOOLEAN
+IsMemoryBlockEmptied (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeaderPtr
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < MemoryHeaderPtr->BitArraySizeInBytes; Index++) {
+    if (MemoryHeaderPtr->BitArrayPtr[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  remove a memory header from list.
+
+  @param  FirstMemoryHeader   A pointer to the memory header list.
+  @param  FreeMemoryHeader    A memory header to be removed into the list.
+
+**/
+VOID
+DelinkMemoryBlock (
+  IN MEMORY_MANAGE_HEADER    *FirstMemoryHeader,
+  IN MEMORY_MANAGE_HEADER    *FreeMemoryHeader
+  )
+{
+  MEMORY_MANAGE_HEADER  *TempHeaderPtr;
+
+  if ((FirstMemoryHeader == NULL) || (FreeMemoryHeader == NULL)) {
+    return ;
+  }
+
+  for (TempHeaderPtr = FirstMemoryHeader; TempHeaderPtr != NULL; TempHeaderPtr = TempHeaderPtr->Next) {
+
+    if (TempHeaderPtr->Next == FreeMemoryHeader) {
+      //
+      // Link the before and after
+      //
+      TempHeaderPtr->Next = FreeMemoryHeader->Next;
+      break;
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h
new file mode 100644
index 0000000000..460db7eab9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhcPeim.h
@@ -0,0 +1,1333 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef _RECOVERY_UHC_H_
+#define _RECOVERY_UHC_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/UsbHostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+#include <Library/PeiServicesLib.h>
+
+#define USB_SLOW_SPEED_DEVICE 0x01
+#define USB_FULL_SPEED_DEVICE 0x02
+
+//
+// One memory block uses 16 page
+//
+#define NORMAL_MEMORY_BLOCK_UNIT_IN_PAGES 16
+
+#define USBCMD                            0       /* Command Register Offset 00-01h */
+#define USBCMD_RS                         BIT0    /* Run/Stop */
+#define USBCMD_HCRESET                    BIT1    /* Host reset */
+#define USBCMD_GRESET                     BIT2    /* Global reset */
+#define USBCMD_EGSM                       BIT3    /* Global Suspend Mode */
+#define USBCMD_FGR                        BIT4    /* Force Global Resume */
+#define USBCMD_SWDBG                      BIT5    /* SW Debug mode */
+#define USBCMD_CF                         BIT6    /* Config Flag (sw only) */
+#define USBCMD_MAXP                       BIT7    /* Max Packet (0 = 32, 1 = 64) */
+
+/* Status register */
+#define USBSTS        2       /* Status Register Offset 02-03h */
+#define USBSTS_USBINT BIT0    /* Interrupt due to IOC */
+#define USBSTS_ERROR  BIT1    /* Interrupt due to error */
+#define USBSTS_RD     BIT2    /* Resume Detect */
+#define USBSTS_HSE    BIT3    /* Host System Error - basically PCI problems */
+#define USBSTS_HCPE   BIT4    /* Host Controller Process Error - the scripts were buggy */
+#define USBSTS_HCH    BIT5    /* HC Halted */
+
+/* Interrupt enable register */
+#define USBINTR         4       /* Interrupt Enable Register 04-05h */
+#define USBINTR_TIMEOUT BIT0    /* Timeout/CRC error enable */
+#define USBINTR_RESUME  BIT1    /* Resume interrupt enable */
+#define USBINTR_IOC     BIT2    /* Interrupt On Complete enable */
+#define USBINTR_SP      BIT3    /* Short packet interrupt enable */
+
+/* Frame Number Register Offset 06-08h */
+#define USBFRNUM  6
+
+/* Frame List Base Address Register Offset 08-0Bh */
+#define USBFLBASEADD  8
+
+/* Start of Frame Modify Register Offset 0Ch */
+#define USBSOF  0x0c
+
+/* USB port status and control registers */
+#define USBPORTSC1            0x10      /*Port 1 offset 10-11h */
+#define USBPORTSC2            0x12      /*Port 2 offset 12-13h */
+
+#define USBPORTSC_CCS         BIT0      /* Current Connect Status ("device present") */
+#define USBPORTSC_CSC         BIT1      /* Connect Status Change */
+#define USBPORTSC_PED         BIT2      /* Port Enable / Disable */
+#define USBPORTSC_PEDC        BIT3      /* Port Enable / Disable Change */
+#define USBPORTSC_LSL         BIT4      /* Line Status Low bit*/
+#define USBPORTSC_LSH         BIT5      /* Line Status High bit*/
+#define USBPORTSC_RD          BIT6      /* Resume Detect */
+#define USBPORTSC_LSDA        BIT8      /* Low Speed Device Attached */
+#define USBPORTSC_PR          BIT9      /* Port Reset */
+#define USBPORTSC_SUSP        BIT12     /* Suspend */
+
+#define SETUP_PACKET_ID       0x2D
+#define INPUT_PACKET_ID       0x69
+#define OUTPUT_PACKET_ID      0xE1
+#define ERROR_PACKET_ID       0x55
+
+#define STALL_1_MICRO_SECOND  1
+#define STALL_1_MILLI_SECOND  1000
+
+
+#pragma pack(1)
+
+typedef struct {
+  UINT32  FrameListPtrTerminate : 1;
+  UINT32  FrameListPtrQSelect : 1;
+  UINT32  FrameListRsvd : 2;
+  UINT32  FrameListPtr : 28;
+} FRAMELIST_ENTRY;
+
+typedef struct {
+  UINT32  QHHorizontalTerminate : 1;
+  UINT32  QHHorizontalQSelect : 1;
+  UINT32  QHHorizontalRsvd : 2;
+  UINT32  QHHorizontalPtr : 28;
+  UINT32  QHVerticalTerminate : 1;
+  UINT32  QHVerticalQSelect : 1;
+  UINT32  QHVerticalRsvd : 2;
+  UINT32  QHVerticalPtr : 28;
+} QUEUE_HEAD;
+
+typedef struct {
+  QUEUE_HEAD  QueueHead;
+  UINT32      Reserved1;
+  UINT32      Reserved2;
+  VOID        *PtrNext;
+  VOID        *PtrDown;
+  VOID        *Reserved3;
+  UINT32      Reserved4;
+} QH_STRUCT;
+
+typedef struct {
+  UINT32  TDLinkPtrTerminate : 1;
+  UINT32  TDLinkPtrQSelect : 1;
+  UINT32  TDLinkPtrDepthSelect : 1;
+  UINT32  TDLinkPtrRsvd : 1;
+  UINT32  TDLinkPtr : 28;
+  UINT32  TDStatusActualLength : 11;
+  UINT32  TDStatusRsvd : 5;
+  UINT32  TDStatus : 8;
+  UINT32  TDStatusIOC : 1;
+  UINT32  TDStatusIOS : 1;
+  UINT32  TDStatusLS : 1;
+  UINT32  TDStatusErr : 2;
+  UINT32  TDStatusSPD : 1;
+  UINT32  TDStatusRsvd2 : 2;
+  UINT32  TDTokenPID : 8;
+  UINT32  TDTokenDevAddr : 7;
+  UINT32  TDTokenEndPt : 4;
+  UINT32  TDTokenDataToggle : 1;
+  UINT32  TDTokenRsvd : 1;
+  UINT32  TDTokenMaxLen : 11;
+  UINT32  TDBufferPtr;
+} TD;
+
+typedef struct {
+  TD      TDData;
+  UINT8   *PtrTDBuffer;
+  VOID    *PtrNextTD;
+  VOID    *PtrNextQH;
+  UINT16  TDBufferLength;
+  UINT16  Reserved;
+} TD_STRUCT;
+
+#pragma pack()
+
+typedef struct _MEMORY_MANAGE_HEADER MEMORY_MANAGE_HEADER;
+
+struct _MEMORY_MANAGE_HEADER {
+  UINT8                         *BitArrayPtr;
+  UINTN                         BitArraySizeInBytes;
+  UINT8                         *MemoryBlockPtr;
+  UINTN                         MemoryBlockSizeInBytes;
+  MEMORY_MANAGE_HEADER          *Next;
+};
+
+#define USB_UHC_DEV_SIGNATURE SIGNATURE_32 ('p', 'u', 'h', 'c')
+typedef struct {
+  UINTN                       Signature;
+  PEI_USB_HOST_CONTROLLER_PPI UsbHostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR      PpiDescriptor;
+
+  UINT32                      UsbHostControllerBaseAddress;
+  FRAMELIST_ENTRY             *FrameListEntry;
+  QH_STRUCT                   *ConfigQH;
+  QH_STRUCT                   *BulkQH;
+  //
+  // Header1 used for QH,TD memory blocks management
+  //
+  MEMORY_MANAGE_HEADER        *Header1;
+
+} USB_UHC_DEV;
+
+#define PEI_RECOVERY_USB_UHC_DEV_FROM_UHCI_THIS(a)  CR (a, USB_UHC_DEV, UsbHostControllerPpi, USB_UHC_DEV_SIGNATURE)
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress          The target device address.
+  @param  DeviceSpeed            Target device speed.
+  @param  MaximumPacketLength    Maximum packet size the default control transfer 
+                                 endpoint is capable of sending or receiving.
+  @param  Request                USB device request to send.
+  @param  TransferDirection      Specifies the data direction for the data stage.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+  @param  TimeOut                Indicates the maximum timeout, in millisecond.
+  @param  TransferResult         Return the result of this control transfer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcControlTransfer (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    * This,
+  IN     UINT8                      DeviceAddress,
+  IN     UINT8                      DeviceSpeed,
+  IN     UINT8                      MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST     * Request,
+  IN     EFI_USB_DATA_DIRECTION     TransferDirection,
+  IN OUT VOID                       *Data OPTIONAL,
+  IN OUT UINTN                      *DataLength OPTIONAL,
+  IN     UINTN                      TimeOut,
+  OUT    UINT32                     *TransferResult
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of 
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of 
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcBulkTransfer (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         DeviceAddress,
+  IN  UINT8                         EndPointAddress,
+  IN  UINT8                         MaximumPacketLength,
+  IN OUT VOID                       *Data,
+  IN OUT UINTN                      *DataLength,
+  IN OUT UINT8                      *DataToggle,
+  IN  UINTN                         TimeOut,
+  OUT UINT32                        *TransferResult
+  );
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  OUT UINT8                         *PortNumber
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber             The root hub port to retrieve the state from.  
+  @param  PortStatus             Variable to receive the port state.
+
+  @retval EFI_SUCCESS            The status of the USB root hub port specified.
+                                 by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN  UINT8                         PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT            The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_HOST_CONTROLLER_PPI.
+  @param  PortNumber            Specifies the root hub port whose feature
+                                is requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS            The feature specified by PortFeature was cleared 
+                                 for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER  PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UhcClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI    *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+/**
+  Initialize UHCI.
+
+  @param  UhcDev                 UHCI Device.
+
+  @retval EFI_SUCCESS            UHCI successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Resource can not be allocated.
+
+**/
+EFI_STATUS
+InitializeUsbHC (
+  IN USB_UHC_DEV          *UhcDev
+  );
+
+/**
+  Create Frame List Structure.
+
+  @param  UhcDev                 UHCI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateFrameList (
+  USB_UHC_DEV             *UhcDev
+  );
+
+/**
+  Read a 16bit width data from Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+
+  @retval the register content read.
+
+**/
+UINT16
+USBReadPortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port
+  );
+
+/**
+  Write a 16bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT16        Data
+  );
+
+/**
+  Write a 32bit width data into Uhc HC IO space register.
+  
+  @param  UhcDev  The UHCI device.
+  @param  Port    The IO space address of the register.
+  @param  Data    The data written into the register.
+
+**/
+VOID
+USBWritePortDW (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Port,
+  IN  UINT32        Data
+  );
+
+/**
+  Clear the content of UHCI's Status Register.
+  
+  @param  UhcDev       The UHCI device.
+  @param  StatusAddr   The IO space address of the register.
+
+**/
+VOID
+ClearStatusReg (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        StatusAddr
+  );
+
+/**
+  Check whether the host controller operates well.
+
+  @param  UhcDev        The UHCI device.
+  @param  StatusRegAddr The io address of status register.
+
+  @retval TRUE          Host controller is working.
+  @retval FALSE         Host controller is halted or system error.
+
+**/
+BOOLEAN
+IsStatusOK (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT32          StatusRegAddr
+  );
+
+/**
+  Get Current Frame Number.
+
+  @param  UhcDev          The UHCI device.
+  @param  FrameNumberAddr The address of frame list register.
+
+  @retval The content of the frame list register.
+
+**/
+UINT16
+GetCurrentFrameNumber (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameNumberAddr
+  );
+
+/**
+  Set Frame List Base Address.
+
+  @param  UhcDev           The UHCI device.
+  @param  FrameListRegAddr The address of frame list register.
+  @param  Addr             The address of frame list table.
+
+**/
+VOID
+SetFrameListBaseAddress (
+  IN USB_UHC_DEV   *UhcDev,
+  IN UINT32        FrameListRegAddr,
+  IN UINT32        Addr
+  );
+
+/**
+  Create QH and initialize.
+
+  @param  UhcDev               The UHCI device.
+  @param  PtrQH                Place to store QH_STRUCT pointer.
+
+  @retval EFI_OUT_OF_RESOURCES Can't allocate memory resources.
+  @retval EFI_SUCCESS        Success.
+
+**/
+EFI_STATUS
+CreateQH (
+  IN  USB_UHC_DEV  *UhcDev,
+  OUT QH_STRUCT    **PtrQH
+  );
+
+/**
+  Set the horizontal link pointer in QH.
+
+  @param  PtrQH               Place to store QH_STRUCT pointer.
+  @param  PtrNext             Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  );
+
+/**
+  Get the horizontal link pointer in QH.
+
+  @param  PtrQH     Place to store QH_STRUCT pointer.
+
+  @retval The horizontal link pointer in QH.
+
+**/
+VOID  *
+GetQHHorizontalLinkPtr (
+  IN QH_STRUCT  *PtrQH
+  );
+
+/**
+  Set a QH or TD horizontally to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHHorizontalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  );
+
+/**
+  Set the horizontal validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the horizontal linker is valid or not.
+
+**/
+VOID
+SetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  );
+
+/**
+  Set the vertical link pointer in QH.
+
+  @param  PtrQH       Place to store QH_STRUCT pointer.
+  @param  PtrNext     Place to the next QH_STRUCT.
+
+**/
+VOID
+SetQHVerticalLinkPtr (
+  IN QH_STRUCT  *PtrQH,
+  IN VOID       *PtrNext
+  );
+
+/**
+  Set a QH or TD vertically to be connected with a specific QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsQH       Specify QH or TD is connected.
+
+**/
+VOID
+SetQHVerticalQHorTDSelect (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsQH
+  );
+
+/**
+  Set the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+  @param  IsValid    Specify the vertical linker is valid or not.
+
+**/
+VOID
+SetQHVerticalValidorInvalid (
+  IN QH_STRUCT  *PtrQH,
+  IN BOOLEAN    IsValid
+  );
+
+/**
+  Get the vertical validor bit in QH.
+
+  @param  PtrQH      Place to store QH_STRUCT pointer.
+
+  @retval The vertical linker is valid or not.
+
+**/
+BOOLEAN
+GetQHHorizontalValidorInvalid (
+  IN QH_STRUCT  *PtrQH
+  );
+
+/**
+  Allocate TD or QH Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Size                   The size of allocation.
+  @param  PtrStruct              Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+AllocateTDorQHStruct (
+  IN  USB_UHC_DEV   *UhcDev,
+  IN  UINT32        Size,
+  OUT VOID          **PtrStruct
+  );
+
+/**
+  Create a TD Struct.
+
+  @param  UhcDev                 The UHCI device.
+  @param  PtrTD                  Place to store TD_STRUCT pointer.
+
+  @return EFI_SUCCESS            Allocate successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Setup Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  DeviceSpeed  Device Speed.
+  @param  DevRequest   Device reuquest.
+  @param  RequestLen   Request length.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate setup stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenSetupStageTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           DeviceSpeed,
+  IN  UINT8           *DevRequest,
+  IN  UINT8           RequestLen,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Data Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PtrData      Data buffer.
+  @param  Len          Data length.
+  @param  PktID        PacketID.
+  @param  Toggle       Data toggle value.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate data stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+GenDataTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           *PtrData,
+  IN  UINT8           Len,
+  IN  UINT8           PktID,
+  IN  UINT8           Toggle,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Generate Status Stage TD.
+
+  @param  UhcDev       The UHCI device.
+  @param  DevAddr      Device address.
+  @param  Endpoint     Endpoint number.
+  @param  PktID        PacketID.
+  @param  DeviceSpeed  Device Speed.
+  @param  PtrTD        TD_STRUCT generated.
+
+  @return EFI_SUCCESS            Generate status stage TD successfully.
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resource.
+
+**/
+EFI_STATUS
+CreateStatusTD (
+  IN  USB_UHC_DEV     *UhcDev,
+  IN  UINT8           DevAddr,
+  IN  UINT8           Endpoint,
+  IN  UINT8           PktID,
+  IN  UINT8           DeviceSpeed,
+  OUT TD_STRUCT       **PtrTD
+  );
+
+/**
+  Set the link pointer validor bit in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsValid      Specify the linker pointer is valid or not.
+
+**/
+VOID
+SetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsValid
+  );
+
+/**
+  Set the Link Pointer pointing to a QH or TD.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsQH          Specify QH or TD is connected.
+
+**/
+VOID
+SetTDLinkPtrQHorTDSelect (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsQH
+  );
+
+/**
+  Set the traverse is depth-first or breadth-first.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  IsDepth       Specify the traverse is depth-first or breadth-first.
+
+**/
+VOID
+SetTDLinkPtrDepthorBreadth (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsDepth
+  );
+
+/**
+  Set TD Link Pointer in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  PtrNext      Place to the next TD_STRUCT.
+
+**/
+VOID
+SetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  VOID      *PtrNext
+  );
+
+/**
+  Get TD Link Pointer.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval Get TD Link Pointer in TD.
+
+**/
+VOID*
+GetTDLinkPtr (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Get the information about whether the Link Pointer field pointing to
+  a QH or a TD.
+
+  @param  PtrTDStruct     Place to store TD_STRUCT pointer.
+
+  @retval whether the Link Pointer field pointing to a QH or a TD.
+
+**/
+BOOLEAN
+IsTDLinkPtrQHOrTD (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Enable/Disable short packet detection mechanism.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  IsEnable     Enable or disable short packet detection mechanism.
+
+**/
+VOID
+EnableorDisableTDShortPacket (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsEnable
+  );
+
+/**
+  Set the max error counter in TD.
+
+  @param  PtrTDStruct  Place to store TD_STRUCT pointer.
+  @param  MaxErrors    The number of allowable error.
+
+**/
+VOID
+SetTDControlErrorCounter (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     MaxErrors
+  );
+
+/**
+  Set the TD is targeting a low-speed device or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsLowSpeedDevice  Whether The device is low-speed.
+
+**/
+VOID
+SetTDLoworFullSpeedDevice (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsLowSpeedDevice
+  );
+
+/**
+  Set the TD is isochronous transfer type or not.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsIsochronous     Whether the transaction isochronous transfer type.
+
+**/
+VOID
+SetTDControlIsochronousorNot (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsIsochronous
+  );
+
+/**
+  Set if UCHI should issue an interrupt on completion of the frame
+  in which this TD is executed
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsSet             Whether HC should issue an interrupt on completion.
+
+**/
+VOID
+SetorClearTDControlIOC (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsSet
+  );
+
+/**
+  Set if the TD is active and can be executed.
+
+  @param  PtrTDStruct       Place to store TD_STRUCT pointer.
+  @param  IsActive          Whether the TD is active and can be executed.
+
+**/
+VOID
+SetTDStatusActiveorInactive (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  BOOLEAN   IsActive
+  );
+
+/**
+  Specifies the maximum number of data bytes allowed for the transfer.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  MaxLen        The maximum number of data bytes allowed.
+
+  @retval The allowed maximum number of data.
+**/
+UINT16
+SetTDTokenMaxLength (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT16    MaxLen
+  );
+
+/**
+  Set the data toggle bit to DATA1.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle1 (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Set the data toggle bit to DATA0.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDTokenDataToggle0 (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Set EndPoint Number the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  EndPoint      The Endport number of the target.
+
+**/
+VOID
+SetTDTokenEndPoint (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     EndPoint
+  );
+
+/**
+  Set Device Address the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  DevAddr       The Device Address of the target.
+
+**/
+VOID
+SetTDTokenDeviceAddress (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINTN     DevAddr
+  );
+
+/**
+  Set Packet Identification the TD is targeting at.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+  @param  PacketID      The Packet Identification of the target.
+
+**/
+VOID
+SetTDTokenPacketID (
+  IN  TD_STRUCT *PtrTDStruct,
+  IN  UINT8     PacketID
+  );
+
+/**
+  Set the beginning address of the data buffer that will be used
+  during the transaction.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+SetTDDataBuffer (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether the TD is active.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is active or not.
+
+**/
+BOOLEAN
+IsTDStatusActive (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether the TD is stalled.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The TD is stalled or not.
+
+**/
+BOOLEAN
+IsTDStatusStalled (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Data Buffer Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Data Buffer Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBufferError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Babble Error is happened.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Babble Error is happened or not.
+
+**/
+BOOLEAN
+IsTDStatusBabbleError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether NAK is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The NAK is received or not.
+
+**/
+BOOLEAN
+IsTDStatusNAKReceived (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether CRC/Time Out Error is encountered.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The CRC/Time Out Error is encountered or not.
+
+**/
+BOOLEAN
+IsTDStatusCRCTimeOutError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Detect whether Bitstuff Error is received.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The Bitstuff Error is received or not.
+
+**/
+BOOLEAN
+IsTDStatusBitStuffError (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Retrieve the actual number of bytes that were tansferred.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The actual number of bytes that were tansferred.
+
+**/
+UINT16
+GetTDStatusActualLength (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Retrieve the information of whether the Link Pointer field is valid or not.
+
+  @param  PtrTDStruct   Place to store TD_STRUCT pointer.
+
+  @retval The linker pointer field is valid or not.
+
+**/
+BOOLEAN
+GetTDLinkPtrValidorInvalid (
+  IN  TD_STRUCT *PtrTDStruct
+  );
+
+/**
+  Count TD Number from PtrFirstTD.
+
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+  @retval The queued TDs number.
+
+**/
+UINTN
+CountTDsNumber (
+  IN  TD_STRUCT *PtrFirstTD
+  );
+
+/**
+  Link TD To QH.
+
+  @param  PtrQH   Place to store QH_STRUCT pointer.
+  @param  PtrTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToQH (
+  IN  QH_STRUCT *PtrQH,
+  IN  TD_STRUCT *PtrTD
+  );
+
+/**
+  Link TD To TD.
+
+  @param  PtrPreTD  Place to store TD_STRUCT pointer.
+  @param  PtrTD     Place to store TD_STRUCT pointer.
+
+**/
+VOID
+LinkTDToTD (
+  IN  TD_STRUCT *PtrPreTD,
+  IN  TD_STRUCT *PtrTD
+  );
+
+/**
+  Execute Control Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecuteControlTransfer (
+  IN  USB_UHC_DEV *UhcDev,
+  IN  TD_STRUCT   *PtrTD,
+  OUT UINTN       *ActualLen,
+  IN  UINTN       TimeOut,
+  OUT UINT32      *TransferResult
+  );
+
+/**
+  Execute Bulk Transfer.
+
+  @param  UhcDev            The UCHI device.
+  @param  PtrTD             A pointer to TD_STRUCT data.
+  @param  ActualLen         Actual transfer Length.
+  @param  DataToggle        DataToggle value.
+  @param  TimeOut           TimeOut value.
+  @param  TransferResult    Transfer Result.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+ExecBulkTransfer (
+  IN     USB_UHC_DEV *UhcDev,
+  IN     TD_STRUCT   *PtrTD,
+  IN OUT UINTN     *ActualLen,
+  IN     UINT8     *DataToggle,
+  IN     UINTN     TimeOut,
+  OUT    UINT32    *TransferResult
+  );
+
+/**
+  Delete Queued TDs.
+
+  @param  UhcDev       The UCHI device.
+  @param  PtrFirstTD   Place to store TD_STRUCT pointer.
+
+**/
+VOID
+DeleteQueuedTDs (
+  IN USB_UHC_DEV     *UhcDev,
+  IN TD_STRUCT       *PtrFirstTD
+  );
+
+/**
+  Check TDs Results.
+
+  @param  PtrTD               A pointer to TD_STRUCT data.
+  @param  Result              The result to return.
+  @param  ErrTDPos            The Error TD position.
+  @param  ActualTransferSize  Actual transfer size.
+
+  @retval The TD is executed successfully or not.
+
+**/
+BOOLEAN
+CheckTDsResults (
+  IN  TD_STRUCT               *PtrTD,
+  OUT UINT32                  *Result,
+  OUT UINTN                   *ErrTDPos,
+  OUT UINTN                   *ActualTransferSize
+  );
+
+/**
+  Create Memory Block.
+
+  @param  UhcDev                   The UCHI device.
+  @param  MemoryHeader             The Pointer to allocated memory block.
+  @param  MemoryBlockSizeInPages   The page size of memory block to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+CreateMemoryBlock (
+  IN  USB_UHC_DEV           *UhcDev,
+  OUT MEMORY_MANAGE_HEADER  **MemoryHeader,
+  IN  UINTN                 MemoryBlockSizeInPages
+  );
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev                 The UCHI device.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+InitializeMemoryManagement (
+  IN USB_UHC_DEV           *UhcDev
+  );
+
+/**
+  Initialize UHCI memory management.
+
+  @param  UhcDev           The UCHI device.
+  @param  Pool             Buffer pointer to store the buffer pointer.
+  @param  AllocSize        The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+UhcAllocatePool (
+  IN  USB_UHC_DEV     *UhcDev,
+  OUT UINT8           **Pool,
+  IN  UINTN           AllocSize
+  );
+
+/**
+  Alloc Memory In MemoryBlock.
+
+  @param  MemoryHeader           The pointer to memory manage header.
+  @param  Pool                   Buffer pointer to store the buffer pointer.
+  @param  NumberOfMemoryUnit     The size of the pool to be allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Can't allocate memory resources.
+  @retval EFI_SUCCESS            Success.
+
+**/
+EFI_STATUS
+AllocMemInMemoryBlock (
+  IN  MEMORY_MANAGE_HEADER  *MemoryHeader,
+  OUT VOID                  **Pool,
+  IN  UINTN                 NumberOfMemoryUnit
+  );
+
+/**
+  Uhci Free Pool.
+
+  @param  UhcDev                 The UHCI device.
+  @param  Pool                   A pointer to store the buffer address.
+  @param  AllocSize              The size of the pool to be freed.
+
+**/
+VOID
+UhcFreePool (
+  IN USB_UHC_DEV     *UhcDev,
+  IN UINT8           *Pool,
+  IN UINTN           AllocSize
+  );
+
+/**
+  Insert a new memory header into list.
+
+  @param  MemoryHeader         A pointer to the memory header list.
+  @param  NewMemoryHeader      A new memory header to be inserted into the list.
+
+**/
+VOID
+InsertMemoryHeaderToList (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeader,
+  IN MEMORY_MANAGE_HEADER  *NewMemoryHeader
+  );
+
+/**
+  Judge the memory block in the memory header is empty or not.
+
+  @param  MemoryHeaderPtr   A pointer to the memory header list.
+
+  @retval Whether the memory block in the memory header is empty or not.
+
+**/
+BOOLEAN
+IsMemoryBlockEmptied (
+  IN MEMORY_MANAGE_HEADER  *MemoryHeaderPtr
+  );
+
+/**
+  remove a memory header from list.
+
+  @param  FirstMemoryHeader   A pointer to the memory header list.
+  @param  FreeMemoryHeader    A memory header to be removed into the list.
+
+**/
+VOID
+DelinkMemoryBlock (
+  IN MEMORY_MANAGE_HEADER    *FirstMemoryHeader,
+  IN MEMORY_MANAGE_HEADER    *FreeMemoryHeader
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf
new file mode 100644
index 0000000000..7baa07612d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf
@@ -0,0 +1,64 @@
+## @file
+# The UhcPeim driver is responsible for managing the behavior of UHCI controller at PEI phase.
+#
+# It produces gPeiUsbHostControllerPpiGuid based on gPeiUsbControllerPpiGuid which is used
+# to enable recovery function from USB Drivers.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UhciPei
+  MODULE_UNI_FILE                = UhciPei.uni
+  FILE_GUID                      = C463CEAC-FC57-4f36-88B7-356C750C3BCA
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = UhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UhcPeim.c
+  UhcPeim.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+
+
+[Ppis]
+  gPeiUsbHostControllerPpiGuid                  ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni
new file mode 100644
index 0000000000..b3cfb23139
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni
new file mode 100644
index 0000000000..5cde35591a
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/UhciPei/UhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c
new file mode 100644
index 0000000000..706aa292c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for XHCI driver.
+
+Copyright (c) 2011, 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 "Xhci.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gXhciComponentName = {
+  XhciComponentNameGetDriverName,
+  XhciComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gXhciComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) XhciComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) XhciComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mXhciDriverNameTable[] = {
+  { "eng;en", L"Usb Xhci Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mXhciDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gXhciComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  )
+{
+  EFI_STATUS           Status;
+  EFI_USB2_HC_PROTOCOL *Usb2Hc;
+  USB_XHCI_INSTANCE    *XhciDev;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gXhciDriverBinding.DriverBindingHandle,
+             &gEfiPciIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  gXhciDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  XhciDev = XHC_FROM_THIS (Usb2Hc);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           XhciDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gXhciComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h
new file mode 100644
index 0000000000..c3c44ccdfb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/ComponentName.h
@@ -0,0 +1,146 @@
+/** @file
+
+  This file contains the delarations for componet name routines.
+
+Copyright (c) 2011, 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.
+
+**/
+
+#ifndef _EFI_COMPONENT_NAME_H_
+#define _EFI_COMPONENT_NAME_H_
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+XhciComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c
new file mode 100644
index 0000000000..54c69d78ed
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.c
@@ -0,0 +1,758 @@
+/** @file
+
+  Routine procedures for memory allocate/free.
+
+Copyright (c) 2013 - 2015, 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 "Xhci.h"
+
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pool           The buffer pool to allocate memory for.
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Pages
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  VOID                    *BufHost;
+  VOID                    *Mapping;
+  EFI_PHYSICAL_ADDRESS    MappedAddr;
+  UINTN                   Bytes;
+  EFI_STATUS              Status;
+
+  PciIo = Pool->PciIo;
+
+  Block = AllocateZeroPool (sizeof (USBHC_MEM_BLOCK));
+  if (Block == NULL) {
+    return NULL;
+  }
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (USBHC_MEM_UNIT * 8);
+  Block->Bits     = AllocateZeroPool (Block->BitsLen);
+
+  if (Block->Bits == NULL) {
+    gBS->FreePool (Block);
+    return NULL;
+  }
+
+  //
+  // Allocate the number of Pages of memory, then map it for
+  // bus master read and write.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    Pages,
+                    &BufHost,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_BITARRAY;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    BufHost,
+                    &Bytes,
+                    &MappedAddr,
+                    &Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    goto FREE_BUFFER;
+  }
+
+  Block->BufHost  = BufHost;
+  Block->Buf      = (UINT8 *) ((UINTN) MappedAddr);
+  Block->Mapping  = Mapping;
+
+  return Block;
+
+FREE_BUFFER:
+  PciIo->FreeBuffer (PciIo, Pages, BufHost);
+
+FREE_BITARRAY:
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+  return NULL;
+}
+
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL       *Pool,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+
+  ASSERT ((Pool != NULL) && (Block != NULL));
+
+  PciIo = Pool->PciIo;
+
+  //
+  // Unmap the common buffer then free the structures
+  //
+  PciIo->Unmap (PciIo, Block->Mapping);
+  PciIo->FreeBuffer (PciIo, EFI_SIZE_TO_PAGES (Block->BufLen), Block->BufHost);
+
+  gBS->FreePool (Block->Bits);
+  gBS->FreePool (Block);
+}
+
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN  USBHC_MEM_BLOCK     *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return                The pci memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+  UINTN                   Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->BufHost <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->BufHost;
+  PhyAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->Buf + Offset);
+  return PhyAddr;
+}
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to pci memory.
+  @param  Size           The size of the memory region.
+
+  @return                The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINTN                   AllocSize;
+  EFI_PHYSICAL_ADDRESS    HostAddr;
+  UINTN                   Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->Buf <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *)Mem - Block->Buf;
+  HostAddr = (EFI_PHYSICAL_ADDRESS)(UINTN) (Block->BufHost + Offset);
+  return HostAddr;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK      *Head,
+  IN USBHC_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo                The PciIo that can be used to access the host controller.
+
+  @retval EFI_SUCCESS          The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE  Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo
+  )
+{
+  USBHC_MEM_POOL          *Pool;
+
+  Pool = AllocatePool (sizeof (USBHC_MEM_POOL));
+
+  if (Pool == NULL) {
+    return Pool;
+  }
+
+  Pool->PciIo   = PciIo;
+  Pool->Head    = UsbHcAllocMemBlock (Pool, USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    gBS->FreePool (Pool);
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+
+/**
+  Release the memory management pool.
+
+  @param  Pool              The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  )
+{
+  USBHC_MEM_BLOCK *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+  gBS->FreePool (Pool);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool           The host controller's memory pool.
+  @param  Size           Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  USBHC_MEM_BLOCK         *NewBlock;
+  VOID                    *Mem;
+  UINTN                   AllocSize;
+  UINTN                   Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UsbHcAllocMemBlock (Pool, Pages);
+
+  if (NewBlock == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbHcAllocateMem: failed to allocate block\n"));
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  USBHC_MEM_BLOCK         *Head;
+  USBHC_MEM_BLOCK         *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
+
+/**  
+  Allocates pages at a specified alignment that are suitable for an EfiPciIoOperationBusMasterCommonBuffer mapping.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to 
+                                use to access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+  
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN UINTN                  Pages,
+  IN UINTN                  Alignment,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  )
+{
+  EFI_STATUS            Status;
+  VOID                  *Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 UnalignedPages;
+  UINTN                 RealPages;
+  UINTN                 Bytes;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+  
+  if ((Alignment & (Alignment - 1)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+ 
+  if (Pages == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+ 
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      Pages,
+                      &Memory,
+                      0
+                      );    
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory  = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+    UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN) Memory);
+    if (UnalignedPages > 0) {
+      //
+      // Free first unaligned page(s).
+      //
+      Status = PciIo->FreeBuffer (PciIo, UnalignedPages, Memory);
+      ASSERT_EFI_ERROR (Status);
+    }
+    Memory         = (VOID *)(UINTN)(AlignedMemory + EFI_PAGES_TO_SIZE (Pages));
+    UnalignedPages = RealPages - Pages - UnalignedPages;
+    if (UnalignedPages > 0) {
+      //
+      // Free last unaligned page(s).
+      //
+      Status = PciIo->FreeBuffer (PciIo, UnalignedPages, Memory);
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = PciIo->AllocateBuffer (
+                      PciIo,
+                      AllocateAnyPages,
+                      EfiBootServicesData,
+                      Pages,
+                      &Memory,
+                      0
+                      );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory  = (UINTN) Memory;
+  }
+
+  Bytes = EFI_PAGES_TO_SIZE (Pages);
+  Status = PciIo->Map (
+                    PciIo,
+                    EfiPciIoOperationBusMasterCommonBuffer,
+                    (VOID *) AlignedMemory,
+                    &Bytes,
+                    DeviceAddress,
+                    Mapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (Pages))) {
+    Status = PciIo->FreeBuffer (PciIo, Pages, (VOID *) AlignedMemory);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  *HostAddress = (VOID *) AlignedMemory;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+  
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of 4 KB pages to free.
+  @param  Mapping               The mapping value returned from Map().
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN VOID                   *HostAddress,
+  IN UINTN                  Pages,
+  VOID                      *Mapping
+  )
+{
+  EFI_STATUS      Status;
+  
+  ASSERT (Pages != 0);
+  
+  Status = PciIo->Unmap (PciIo, Mapping);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = PciIo->FreeBuffer (
+                    PciIo,
+                    Pages,
+                    HostAddress
+                    );     
+  ASSERT_EFI_ERROR (Status);
+}
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h
new file mode 100644
index 0000000000..1907685ddd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/UsbHcMem.h
@@ -0,0 +1,213 @@
+/** @file
+
+  This file contains the definination for host controller memory management routines.
+
+Copyright (c) 2013, 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.
+
+**/
+
+#ifndef _EFI_XHCI_MEM_H_
+#define _EFI_XHCI_MEM_H_
+
+#define USB_HC_BIT(a)                  ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+struct _USBHC_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen;
+  UINT8                   *Buf;
+  UINT8                   *BufHost;
+  UINTN                   BufLen;   // Memory size in bytes
+  VOID                    *Mapping;
+  USBHC_MEM_BLOCK         *Next;
+};
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. XHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  BOOLEAN                 Check4G;
+  UINT32                  Which4G;
+  USBHC_MEM_BLOCK         *Head;
+} USBHC_MEM_POOL;
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT           64
+
+#define USBHC_MEM_UNIT_MASK      (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_DEFAULT_PAGES  16
+
+#define USBHC_MEM_ROUND(Len)  (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @param  PciIo               The PciIo that can be used to access the host controller.
+
+  @retval EFI_SUCCESS         The memory pool is initialized.
+  @retval EFI_OUT_OF_RESOURCE Fail to init the memory pool.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo
+  );
+
+
+/**
+  Release the memory management pool.
+
+  @param   Pool               The USB memory pool to free.
+
+  @retval EFI_SUCCESS       The memory pool is freed.
+  @retval EFI_DEVICE_ERROR  Failed to free the memory pool.
+
+**/
+EFI_STATUS
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL       *Pool
+  );
+
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool  The host controller's memory pool.
+  @param  Size  Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN  USBHC_MEM_POOL      *Pool,
+  IN  UINTN               Size
+  );
+
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool  The memory pool of the host controller.
+  @param  Mem   The memory to free.
+  @param  Size  The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to host memory.
+  @param  Size           The size of the memory region.
+
+  @return                The pci memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The pointer to pci memory.
+  @param  Size           The size of the memory region.
+
+  @return                The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL       *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+/**  
+  Allocates pages at a specified alignment that are suitable for an EfiPciIoOperationBusMasterCommonBuffer mapping.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to 
+                                use to access the hosts HostAddress.
+  @param  Mapping               A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+  
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN UINTN                  Pages,
+  IN UINTN                  Alignment,
+  OUT VOID                  **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS  *DeviceAddress,
+  OUT VOID                  **Mapping
+  );
+  
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+  
+  @param  PciIo                 The PciIo that can be used to access the host controller.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+  @param  Mapping               The mapping value returned from Map().
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN EFI_PCI_IO_PROTOCOL    *PciIo,
+  IN VOID                   *HostAddress,
+  IN UINTN                  Pages,
+  VOID                      *Mapping
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c
new file mode 100644
index 0000000000..39c28ab7d8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c
@@ -0,0 +1,2250 @@
+/** @file
+  The XHCI controller driver.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+//
+// Two arrays used to translate the XHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {XHC_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {XHC_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbClearPortChangeMap[] = {
+  {XHC_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_PORTSC_PRC, EfiUsbPortResetChange}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortStateMap[] = {
+  {XHC_HUB_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_HUB_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_HUB_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_HUB_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_HUB_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_HUB_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_HUB_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbHubClearPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_HUB_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_HUB_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_HUB_PORTSC_PRC, EfiUsbPortResetChange},
+  {XHC_HUB_PORTSC_BHRC, Usb3PortBHPortResetChange}
+};
+
+EFI_DRIVER_BINDING_PROTOCOL  gXhciDriverBinding = {
+  XhcDriverBindingSupported,
+  XhcDriverBindingStart,
+  XhcDriverBindingStop,
+  0x30,
+  NULL,
+  NULL
+};
+
+//
+// Template for Xhci's Usb2 Host Controller Protocol Instance.
+//
+EFI_USB2_HC_PROTOCOL gXhciUsb2HcTemplate = {
+  XhcGetCapability,
+  XhcReset,
+  XhcGetState,
+  XhcSetState,
+  XhcControlTransfer,
+  XhcBulkTransfer,
+  XhcAsyncInterruptTransfer,
+  XhcSyncInterruptTransfer,
+  XhcIsochronousTransfer,
+  XhcAsyncIsochronousTransfer,
+  XhcGetRootHubPortStatus,
+  XhcSetRootHubPortFeature,
+  XhcClearRootHubPortFeature,
+  0x3,
+  0x0
+};
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  The EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_TPL            OldTpl;
+
+  if ((MaxSpeed == NULL) || (PortNumber == NULL) || (Is64BitCapable == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl          = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+  *MaxSpeed       = EFI_USB_SPEED_SUPER;
+  *PortNumber     = (UINT8) (Xhc->HcSParams1.Data.MaxPorts);
+  *Is64BitCapable = (UINT8) (Xhc->HcCParams.Data.Ac64);
+  DEBUG ((EFI_D_INFO, "XhcGetCapability: %d ports, 64 bit %d\n", *PortNumber, *Is64BitCapable));
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcReset (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT16                Attributes
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_STATUS         Status;
+  EFI_TPL            OldTpl;
+
+  Xhc = XHC_FROM_THIS (This);
+  
+  if (Xhc->DevicePath != NULL) {
+    //
+    // Report Status Code to indicate reset happens
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_IO_BUS_USB | EFI_IOB_PC_RESET),
+      Xhc->DevicePath
+      );
+  }  
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  switch (Attributes) {
+  case EFI_USB_HC_RESET_GLOBAL:
+  //
+  // Flow through, same behavior as Host Controller Reset
+  //
+  case EFI_USB_HC_RESET_HOST_CONTROLLER:
+    if ((Xhc->DebugCapSupOffset != 0xFFFFFFFF) && ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset) & 0xFF) == XHC_CAP_USB_DEBUG) &&
+        ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset + XHC_DC_DCCTRL) & BIT0) != 0)) {
+      Status = EFI_SUCCESS;
+      goto ON_EXIT;
+    }
+    //
+    // Host Controller must be Halt when Reset it
+    //
+    if (!XhcIsHalt (Xhc)) {
+      Status = XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        goto ON_EXIT;
+      }
+    }
+
+    Status = XhcResetHC (Xhc, XHC_RESET_TIMEOUT);
+    ASSERT (!(XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_CNR)));
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+    //
+    // Clean up the asynchronous transfers, currently only
+    // interrupt supports asynchronous operation.
+    //
+    XhciDelAllAsyncIntTransfers (Xhc);
+    XhcFreeSched (Xhc);
+
+    XhcInitSched (Xhc);
+    break;
+
+  case EFI_USB_HC_RESET_GLOBAL_WITH_DEBUG:
+  case EFI_USB_HC_RESET_HOST_WITH_DEBUG:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcReset: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetState (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT EFI_USB_HC_STATE      *State
+  )
+{
+  USB_XHCI_INSTANCE  *Xhc;
+  EFI_TPL            OldTpl;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  if (XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+    *State = EfiUsbHcStateHalt;
+  } else {
+    *State = EfiUsbHcStateOperational;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcGetState: current state %d\n", *State));
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetState (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN EFI_USB_HC_STATE      State
+  )
+{
+  USB_XHCI_INSTANCE   *Xhc;
+  EFI_STATUS          Status;
+  EFI_USB_HC_STATE    CurState;
+  EFI_TPL             OldTpl;
+
+  Status = XhcGetState (This, &CurState);
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (CurState == State) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  switch (State) {
+  case EfiUsbHcStateHalt:
+    Status = XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateOperational:
+    if (XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    //
+    // Software must not write a one to this field unless the host controller
+    // is in the Halted state. Doing so will yield undefined results.
+    // refers to Spec[XHCI1.0-2.3.1]
+    //
+    if (!XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    Status = XhcRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbHcStateSuspend:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcSetState: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetRootHubPortStatus (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  OUT EFI_USB_PORT_STATUS   *PortStatus
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  UINTN                   Index;
+  UINTN                   MapSize;
+  EFI_STATUS              Status;
+  USB_DEV_ROUTE           ParentRouteChart;
+  EFI_TPL                 OldTpl;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc       = XHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = Xhc->HcSParams1.Data.MaxPorts;
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset                       = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  PortStatus->PortStatus       = 0;
+  PortStatus->PortChangeStatus = 0;
+
+  State = XhcReadOpReg (Xhc, Offset);
+
+  //
+  // According to XHCI 1.0 spec, bit 10~13 of the root port status register identifies the speed of the attached device.
+  //
+  switch ((State & XHC_PORTSC_PS) >> 10) {
+  case 2:
+    PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+    break;
+
+  case 3:
+    PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+    break;
+
+  case 4:
+    PortStatus->PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+    break;
+
+  default:
+    break;
+  }
+
+  //
+  // Convert the XHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+  //
+  // Bit5~8 reflects its current link state.
+  //
+  if ((State & XHC_PORTSC_PLS) >> 5 == 3) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbClearPortChangeMap[Index].HwState)) {
+      XhcClearRootHubPortFeature (This, PortNumber, (EFI_USB_PORT_FEATURE)mUsbClearPortChangeMap[Index].Selector);
+    }
+  }
+
+  //
+  // Poll the root port status register to enable/disable corresponding device slot if there is a device attached/detached.
+  // For those devices behind hub, we get its attach/detach event by hooking Get_Port_Status request at control transfer for those hub.
+  //
+  ParentRouteChart.Dword = 0;
+  XhcPollPortStatusChange (Xhc, ParentRouteChart, PortNumber, PortStatus);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  TotalPort = (Xhc->HcSParams1.Data.MaxPorts);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State  = XhcReadOpReg (Xhc, Offset);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+    // A port may be disabled by software writing a '1' to this flag.
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= XHC_PORTSC_LWS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    State &= ~XHC_PORTSC_PLS;
+    State |= (3 << 5) ;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    DEBUG ((EFI_D_INFO, "XhcUsbPortReset!\n"));
+    //
+    // Make sure Host Controller not halt before reset it
+    //
+    if (XhcIsHalt (Xhc)) {
+      Status = XhcRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "XhcSetRootHubPortFeature :failed to start HC - %r\n", Status));
+        break;
+      }
+    }
+
+    //
+    // 4.3.1 Resetting a Root Hub Port
+    // 1) Write the PORTSC register with the Port Reset (PR) bit set to '1'.
+    //
+    State |= XHC_PORTSC_RESET;
+    XhcWriteOpReg (Xhc, Offset, State);
+    XhcWaitOpRegBit(Xhc, Offset, XHC_PORTSC_PRC, TRUE, XHC_GENERIC_TIMEOUT);
+    break;
+
+  case EfiUsbPortPower:
+    //
+    // Not supported, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // XHCI root hub port don't has the owner bit, ignore the operation
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcSetRootHubPortFeature: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  UINT32                  Offset;
+  UINT32                  State;
+  UINT32                  TotalPort;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc       = XHC_FROM_THIS (This);
+  Status    = EFI_SUCCESS;
+
+  TotalPort = (Xhc->HcSParams1.Data.MaxPorts);
+
+  if (PortNumber >= TotalPort) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = XHC_PORTSC_OFFSET + (0x10 * PortNumber);
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bit
+  //
+  State  = XhcReadOpReg (Xhc, Offset);
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+  case EfiUsbPortEnable:
+    //
+    // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+    // A port may be disabled by software writing a '1' to this flag.
+    //
+    State |= XHC_PORTSC_PED;
+    State &= ~XHC_PORTSC_RESET;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortSuspend:
+    State |= XHC_PORTSC_LWS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    State &= ~XHC_PORTSC_PLS;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortReset:
+    //
+    // PORTSC_RESET BIT(4) bit is RW1S attribute, which means Write-1-to-set status:
+    // Register bits indicate status when read, a clear bit may be set by
+    // writing a '1'. Writing a '0' to RW1S bits has no effect.
+    //
+    break;
+
+  case EfiUsbPortOwner:
+    //
+    // XHCI root hub port don't has the owner bit, ignore the operation
+    //
+    break;
+
+  case EfiUsbPortConnectChange:
+    //
+    // Clear connect status change
+    //
+    State |= XHC_PORTSC_CSC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortEnableChange:
+    //
+    // Clear enable status change
+    //
+    State |= XHC_PORTSC_PEC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortOverCurrentChange:
+    //
+    // Clear PortOverCurrent change
+    //
+    State |= XHC_PORTSC_OCC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortResetChange:
+    //
+    // Clear Port Reset change
+    //
+    State |= XHC_PORTSC_PRC;
+    XhcWriteOpReg (Xhc, Offset, State);
+    break;
+
+  case EfiUsbPortPower:
+  case EfiUsbPortSuspendChange:
+    //
+    // Not supported or not related operation
+    //
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcClearRootHubPortFeature: status %r\n", Status));
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  Timeout               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST              *Request,
+  IN     EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   Endpoint;
+  UINT8                   Index;
+  UINT8                   DescriptorType;
+  UINT8                   SlotId;
+  UINT8                   TTT;
+  UINT8                   MTT;
+  UINT32                  MaxPacket0;
+  EFI_USB_HUB_DESCRIPTOR  *HubDesc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  UINTN                   MapSize;
+  EFI_USB_PORT_STATUS     PortStatus;
+  UINT32                  State;
+  EFI_USB_DEVICE_REQUEST  ClearPortRequest;
+  UINTN                   Len;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64) &&
+      (MaximumPacketLength != 512)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength != 512)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Len             = 0;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: HC halted at entrance\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Hook the Set_Address request from UsbBus.
+  // According to XHCI 1.0 spec, the Set_Address request is replaced by XHCI's Address_Device cmd.
+  //
+  if ((Request->Request     == USB_REQ_SET_ADDRESS) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Reset the BusDevAddr field of all disabled entries in UsbDevContext array firstly.
+    // This way is used to clean the history to avoid using wrong device address by XhcAsyncInterruptTransfer().
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (!Xhc->UsbDevContext[Index + 1].Enabled &&
+          (Xhc->UsbDevContext[Index + 1].SlotId == 0) &&
+          (Xhc->UsbDevContext[Index + 1].BusDevAddr == (UINT8)Request->Value)) {
+        Xhc->UsbDevContext[Index + 1].BusDevAddr = 0;
+      }
+    }
+
+    if (Xhc->UsbDevContext[SlotId].XhciDevAddr == 0) {
+      Status = EFI_DEVICE_ERROR;
+      goto ON_EXIT;
+    }
+    //
+    // The actual device address has been assigned by XHCI during initializing the device slot.
+    // So we just need establish the mapping relationship between the device address requested from UsbBus
+    // and the actual device address assigned by XHCI. The the following invocations through EFI_USB2_HC_PROTOCOL interface
+    // can find out the actual device address by it.
+    //
+    Xhc->UsbDevContext[SlotId].BusDevAddr = (UINT8)Request->Value;
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  // Note that we encode the direction in address although default control
+  // endpoint is bidirectional. XhcCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: failed to create URB"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  //
+  // Get the status from URB. The result is updated in XhcCheckUrbResult
+  // which is called by XhcExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcControlTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+    goto FREE_URB;
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcControlTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  
+  if (Urb->DataMap != NULL) {
+    Status = Xhc->PciIo->Unmap (Xhc->PciIo, Urb->DataMap);
+    ASSERT_EFI_ERROR (Status);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    }  
+  }
+
+  //
+  // Hook Get_Descriptor request from UsbBus as we need evaluate context and configure endpoint.
+  // Hook Get_Status request form UsbBus as we need trace device attach/detach event happened at hub.
+  // Hook Set_Config request from UsbBus as we need configure device endpoint.
+  //
+  if ((Request->Request     == USB_REQ_GET_DESCRIPTOR) &&
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE)) || 
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_DEVICE))))) {
+    DescriptorType = (UINT8)(Request->Value >> 8);
+    if ((DescriptorType == USB_DESC_TYPE_DEVICE) && ((*DataLength == sizeof (EFI_USB_DEVICE_DESCRIPTOR)) || ((DeviceSpeed == EFI_USB_SPEED_FULL) && (*DataLength == 8)))) {
+        ASSERT (Data != NULL);
+        //
+        // Store a copy of device scriptor as hub device need this info to configure endpoint.
+        //
+        CopyMem (&Xhc->UsbDevContext[SlotId].DevDesc, Data, *DataLength);
+        if (Xhc->UsbDevContext[SlotId].DevDesc.BcdUSB == 0x0300) {
+          //
+          // If it's a usb3.0 device, then its max packet size is a 2^n.
+          //
+          MaxPacket0 = 1 << Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+        } else {
+          MaxPacket0 = Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+        }
+        Xhc->UsbDevContext[SlotId].ConfDesc = AllocateZeroPool (Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations * sizeof (EFI_USB_CONFIG_DESCRIPTOR *));
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcEvaluateContext (Xhc, SlotId, MaxPacket0);
+        } else {
+          Status = XhcEvaluateContext64 (Xhc, SlotId, MaxPacket0);
+        }
+    } else if (DescriptorType == USB_DESC_TYPE_CONFIG) {
+      ASSERT (Data != NULL);
+      if (*DataLength == ((UINT16 *)Data)[1]) {
+        //
+        // Get configuration value from request, Store the configuration descriptor for Configure_Endpoint cmd.
+        //
+        Index = (UINT8)Request->Value;
+        ASSERT (Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations);
+        Xhc->UsbDevContext[SlotId].ConfDesc[Index] = AllocateZeroPool(*DataLength);
+        CopyMem (Xhc->UsbDevContext[SlotId].ConfDesc[Index], Data, *DataLength);
+        //
+        // Default to use AlternateSetting 0 for all interfaces.
+        //
+        Xhc->UsbDevContext[SlotId].ActiveAlternateSetting = AllocateZeroPool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->NumInterfaces * sizeof (UINT8));
+      }
+    } else if (((DescriptorType == USB_DESC_TYPE_HUB) ||
+               (DescriptorType == USB_DESC_TYPE_HUB_SUPER_SPEED)) && (*DataLength > 2)) {
+      ASSERT (Data != NULL);
+      HubDesc = (EFI_USB_HUB_DESCRIPTOR *)Data;
+      ASSERT (HubDesc->NumPorts <= 15);
+      //
+      // The bit 5,6 of HubCharacter field of Hub Descriptor is TTT.
+      //
+      TTT = (UINT8)((HubDesc->HubCharacter & (BIT5 | BIT6)) >> 5);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.DeviceProtocol == 2) {
+        //
+        // Don't support multi-TT feature for super speed hub now.
+        //
+        MTT = 0;
+        DEBUG ((EFI_D_ERROR, "XHCI: Don't support multi-TT feature for Hub now. (force to disable MTT)\n"));
+      } else {
+        MTT = 0;
+      }
+
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcConfigHubContext (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      } else {
+        Status = XhcConfigHubContext64 (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_CONFIG) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Hook Set_Config request from UsbBus as we need configure device endpoint.
+    //
+    for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+      if (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->ConfigurationValue == (UINT8)Request->Value) {
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcSetConfigCmd (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        } else {
+          Status = XhcSetConfigCmd64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        }
+        break;
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_INTERFACE) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_INTERFACE))) {
+    //
+    // Hook Set_Interface request from UsbBus as we need configure interface setting.
+    // Request->Value indicates AlterlateSetting to set
+    // Request->Index indicates Interface to set
+    //
+    if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] != (UINT8) Request->Value) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcSetInterface (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Xhc->UsbDevContext[SlotId].ActiveConfiguration - 1], Request);
+      } else {
+        Status = XhcSetInterface64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Xhc->UsbDevContext[SlotId].ActiveConfiguration - 1], Request);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_GET_STATUS) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER))) {
+    ASSERT (Data != NULL);
+    //
+    // Hook Get_Status request from UsbBus to keep track of the port status change.
+    //
+    State                       = *(UINT32 *)Data;
+    PortStatus.PortStatus       = 0;
+    PortStatus.PortChangeStatus = 0;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // For super speed hub, its bit10~12 presents the attached device speed.
+      //
+      if ((State & XHC_PORTSC_PS) >> 10 == 0) {
+        PortStatus.PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      }
+    } else {
+      //
+      // For high or full/low speed hub, its bit9~10 presents the attached device speed.
+      //
+      if (XHC_BIT_IS_SET (State, BIT9)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      } else if (XHC_BIT_IS_SET (State, BIT10)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      }
+    }
+
+    //
+    // Convert the XHCI port/port change state to UEFI status
+    //
+    MapSize = sizeof (mUsbHubPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortStateMap[Index].HwState)) {
+        PortStatus.PortStatus = (UINT16) (PortStatus.PortStatus | mUsbHubPortStateMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortChangeMap[Index].HwState)) {
+        PortStatus.PortChangeStatus = (UINT16) (PortStatus.PortChangeStatus | mUsbHubPortChangeMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubClearPortChangeMap[Index].HwState)) {
+        ZeroMem (&ClearPortRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+        ClearPortRequest.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER);
+        ClearPortRequest.Request      = (UINT8) USB_REQ_CLEAR_FEATURE;
+        ClearPortRequest.Value        = mUsbHubClearPortChangeMap[Index].Selector;
+        ClearPortRequest.Index        = Request->Index;
+        ClearPortRequest.Length       = 0;
+
+        XhcControlTransfer (
+          This, 
+          DeviceAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          &ClearPortRequest,
+          EfiUsbNoData,
+          NULL,
+          &Len,
+          Timeout,
+          Translator,
+          TransferResult
+          );
+      }
+    }
+
+    XhcPollPortStatusChange (Xhc, Xhc->UsbDevContext[SlotId].RouteString, (UINT8)Request->Index, &PortStatus);
+
+    *(UINT32 *)Data = *(UINT32*)&PortStatus;
+  }
+
+FREE_URB:
+  FreePool (Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcBulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((EFI_USB_SPEED_HIGH == DeviceSpeed) && (MaximumPacketLength > 512)) ||
+      ((EFI_USB_SPEED_SUPER == DeviceSpeed) && (MaximumPacketLength > 1024))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc             = XHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: HC is halted\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcBulkTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               PollingInterval,
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     CallBackFunction,
+  IN     VOID                                *Context OPTIONAL
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  EFI_STATUS              Status;
+  UINT8                   SlotId;
+  UINT8                   Index;
+  UINT8                   *Data;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validate parameters
+  //
+  if (!XHCI_IS_DATAIN (EndPointAddress)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsNewTransfer) {
+    if (DataLength == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((*DataToggle != 1) && (*DataToggle != 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((PollingInterval > 255) || (PollingInterval < 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = XHC_FROM_THIS (This);
+
+  //
+  // Delete Async interrupt transfer request.
+  //
+  if (!IsNewTransfer) {
+    //
+    // The delete request may happen after device is detached.
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (Xhc->UsbDevContext[Index + 1].BusDevAddr == DeviceAddress) {
+        break;
+      }
+    }
+
+    if (Index == 255) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    Status = XhciDelAsyncIntTransfer (Xhc, DeviceAddress, EndPointAddress);
+    DEBUG ((EFI_D_INFO, "XhcAsyncInterruptTransfer: remove old transfer for addr %d, Status = %r\n", DeviceAddress, Status));
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: HC is halt\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  Data = AllocateZeroPool (DataLength);
+
+  if (Data == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: failed to allocate buffer\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_INT_TRANSFER_ASYNC,
+          NULL,
+          Data,
+          DataLength,
+          CallBackFunction,
+          Context
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcAsyncInterruptTransfer: failed to create URB\n"));
+    FreePool (Data);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  InsertHeadList (&Xhc->AsyncIntTransfers, &Urb->UrbList);
+  //
+  // Ring the doorbell
+  //
+  Status = RingIntTransferDoorBell (Xhc, Urb);
+
+ON_EXIT:
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  Timeout               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_STATUS              RecoveryStatus;
+  EFI_TPL                 OldTpl;
+
+  //
+  // Validates parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 1) && (*DataToggle != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8))  ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 3072))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc     = XHC_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "EhcSyncInterruptTransfer: HC is halt\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  Urb = XhcCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_INT_TRANSFER_SYNC,
+          NULL,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcExecTransfer (Xhc, FALSE, Urb, Timeout);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  Xhc->PciIo->Flush (Xhc->PciIo);
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Entry point for EFI drivers.
+
+  @param  ImageHandle       EFI_HANDLE.
+  @param  SystemTable       EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS       Success.
+  @retval Others            Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gXhciDriverBinding,
+           ImageHandle,
+           &gXhciComponentName,
+           &gXhciComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  USB_CLASSC              UsbClassCReg;
+
+  //
+  // Test whether there is PCI IO Protocol attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = PciIo->Pci.Read (
+                        PciIo,
+                        EfiPciIoWidthUint8,
+                        PCI_CLASSCODE_OFFSET,
+                        sizeof (USB_CLASSC) / sizeof (UINT8),
+                        &UsbClassCReg
+                        );
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Test whether the controller belongs to Xhci type
+  //
+  if ((UsbClassCReg.BaseCode != PCI_CLASS_SERIAL) ||
+      (UsbClassCReg.SubClassCode != PCI_CLASS_SERIAL_USB) ||
+      (UsbClassCReg.ProgInterface != PCI_IF_XHCI)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Create and initialize a USB_XHCI_INSTANCE structure.
+
+  @param  PciIo                  The PciIo on this device.
+  @param  DevicePath             The device path of host controller.
+  @param  OriginalPciAttributes  Original PCI attributes.
+
+  @return The allocated and initialized USB_XHCI_INSTANCE structure if created,
+          otherwise NULL.
+
+**/
+USB_XHCI_INSTANCE*
+XhcCreateUsbHc (
+  IN EFI_PCI_IO_PROTOCOL       *PciIo,
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINT64                    OriginalPciAttributes
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  EFI_STATUS              Status;
+  UINT32                  PageSize;
+  UINT16                  ExtCapReg;
+
+  Xhc = AllocateZeroPool (sizeof (USB_XHCI_INSTANCE));
+
+  if (Xhc == NULL) {
+    return NULL;
+  }
+
+  //
+  // Initialize private data structure
+  //
+  Xhc->Signature             = XHCI_INSTANCE_SIG;
+  Xhc->PciIo                 = PciIo;
+  Xhc->DevicePath            = DevicePath;
+  Xhc->OriginalPciAttributes = OriginalPciAttributes;
+  CopyMem (&Xhc->Usb2Hc, &gXhciUsb2HcTemplate, sizeof (EFI_USB2_HC_PROTOCOL));
+
+  InitializeListHead (&Xhc->AsyncIntTransfers);
+
+  //
+  // Be caution that the Offset passed to XhcReadCapReg() should be Dword align
+  //
+  Xhc->CapLength        = XhcReadCapReg8 (Xhc, XHC_CAPLENGTH_OFFSET);
+  Xhc->HcSParams1.Dword = XhcReadCapReg (Xhc, XHC_HCSPARAMS1_OFFSET);
+  Xhc->HcSParams2.Dword = XhcReadCapReg (Xhc, XHC_HCSPARAMS2_OFFSET);
+  Xhc->HcCParams.Dword  = XhcReadCapReg (Xhc, XHC_HCCPARAMS_OFFSET);
+  Xhc->DBOff            = XhcReadCapReg (Xhc, XHC_DBOFF_OFFSET);
+  Xhc->RTSOff           = XhcReadCapReg (Xhc, XHC_RTSOFF_OFFSET);
+
+  //
+  // This PageSize field defines the page size supported by the xHC implementation.
+  // This xHC supports a page size of 2^(n+12) if bit n is Set. For example,
+  // if bit 0 is Set, the xHC supports 4k byte page sizes.
+  //
+  PageSize      = XhcReadOpReg(Xhc, XHC_PAGESIZE_OFFSET) & XHC_PAGESIZE_MASK;
+  Xhc->PageSize = 1 << (HighBitSet32(PageSize) + 12);
+
+  ExtCapReg            = (UINT16) (Xhc->HcCParams.Data.ExtCapReg);
+  Xhc->ExtCapRegBase   = ExtCapReg << 2;
+  Xhc->UsbLegSupOffset = XhcGetCapabilityAddr (Xhc, XHC_CAP_USB_LEGACY);
+  Xhc->DebugCapSupOffset = XhcGetCapabilityAddr (Xhc, XHC_CAP_USB_DEBUG);
+
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: Capability length 0x%x\n", Xhc->CapLength));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcSParams1 0x%x\n", Xhc->HcSParams1));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcSParams2 0x%x\n", Xhc->HcSParams2));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: HcCParams 0x%x\n", Xhc->HcCParams));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: DBOff 0x%x\n", Xhc->DBOff));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: RTSOff 0x%x\n", Xhc->RTSOff));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: UsbLegSupOffset 0x%x\n", Xhc->UsbLegSupOffset));
+  DEBUG ((EFI_D_INFO, "XhcCreateUsb3Hc: DebugCapSupOffset 0x%x\n", Xhc->DebugCapSupOffset));
+
+  //
+  // Create AsyncRequest Polling Timer
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  XhcMonitorAsyncRequests,
+                  Xhc,
+                  &Xhc->PollTimer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Xhc;
+
+ON_ERROR:
+  FreePool (Xhc);
+  return NULL;
+}
+
+/**
+  One notified function to stop the Host Controller when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+XhcExitBootService (
+  EFI_EVENT  Event,
+  VOID       *Context
+  )
+
+{
+  USB_XHCI_INSTANCE    *Xhc;
+  EFI_PCI_IO_PROTOCOL  *PciIo;
+
+  Xhc = (USB_XHCI_INSTANCE*) Context;
+  PciIo = Xhc->PciIo;
+
+  //
+  // Stop AsyncRequest Polling timer then stop the XHCI driver
+  // and uninstall the XHCI protocl.
+  //
+  gBS->SetTimer (Xhc->PollTimer, TimerCancel, 0);
+  XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+
+  if (Xhc->PollTimer != NULL) {
+    gBS->CloseEvent (Xhc->PollTimer);
+  }
+
+  XhcClearBiosOwnership (Xhc);
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+                  PciIo,
+                  EfiPciIoAttributeOperationSet,
+                  Xhc->OriginalPciAttributes,
+                  NULL
+                  );
+}
+
+/**
+  Starting the Usb XHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PCI_IO_PROTOCOL     *PciIo;
+  UINT64                  Supports;
+  UINT64                  OriginalPciAttributes;
+  BOOLEAN                 PciAttributesSaved;
+  USB_XHCI_INSTANCE       *Xhc;
+  EFI_DEVICE_PATH_PROTOCOL  *HcDevicePath;
+
+  //
+  // Open the PciIo Protocol, then enable the USB host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open Device Path Protocol for on USB host controller
+  //
+  HcDevicePath = NULL;
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &HcDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  PciAttributesSaved = FALSE;
+  //
+  // Save original PCI attributes
+  //
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationGet,
+                    0,
+                    &OriginalPciAttributes
+                    );
+
+  if (EFI_ERROR (Status)) {
+    goto CLOSE_PCIIO;
+  }
+  PciAttributesSaved = TRUE;
+
+  Status = PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSupported,
+                    0,
+                    &Supports
+                    );
+  if (!EFI_ERROR (Status)) {
+    Supports &= (UINT64)EFI_PCI_DEVICE_ENABLE;
+    Status = PciIo->Attributes (
+                      PciIo,
+                      EfiPciIoAttributeOperationEnable,
+                      Supports,
+                      NULL
+                      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to enable controller\n"));
+    goto CLOSE_PCIIO;
+  }
+
+  //
+  // Create then install USB2_HC_PROTOCOL
+  //
+  Xhc = XhcCreateUsbHc (PciIo, HcDevicePath, OriginalPciAttributes);
+
+  if (Xhc == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to create USB2_HC\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  XhcSetBiosOwnership (Xhc);
+
+  XhcResetHC (Xhc, XHC_RESET_TIMEOUT);
+  ASSERT (XhcIsHalt (Xhc));
+
+  //
+  // After Chip Hardware Reset wait until the Controller Not Ready (CNR) flag
+  // in the USBSTS is '0' before writing any xHC Operational or Runtime registers.
+  //
+  ASSERT (!(XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_CNR)));
+
+  //
+  // Initialize the schedule
+  //
+  XhcInitSched (Xhc);
+
+  //
+  // Start the Host Controller
+  //
+  XhcRunHC(Xhc, XHC_GENERIC_TIMEOUT);
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->SetTimer (Xhc->PollTimer, TimerPeriodic, XHC_ASYNC_TIMER_INTERVAL);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to start async interrupt monitor\n"));
+    XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+    goto FREE_POOL;
+  }
+
+  //
+  // Create event to stop the HC when exit boot service.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  XhcExitBootService,
+                  Xhc,
+                  &gEfiEventExitBootServicesGuid,
+                  &Xhc->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto FREE_POOL;
+  }
+
+  //
+  // Install the component name protocol, don't fail the start
+  // because of something for display.
+  //
+  AddUnicodeString2 (
+    "eng",
+    gXhciComponentName.SupportedLanguages,
+    &Xhc->ControllerNameTable,
+    L"eXtensible Host Controller (USB 3.0)",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gXhciComponentName2.SupportedLanguages,
+    &Xhc->ControllerNameTable,
+    L"eXtensible Host Controller (USB 3.0)",
+    FALSE
+    );
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Xhc->Usb2Hc
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDriverBindingStart: failed to install USB2_HC Protocol\n"));
+    goto FREE_POOL;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcDriverBindingStart: XHCI started for controller @ %x\n", Controller));
+  return EFI_SUCCESS;
+
+FREE_POOL:
+  gBS->CloseEvent (Xhc->PollTimer);
+  XhcFreeSched (Xhc);
+  FreePool (Xhc);
+
+CLOSE_PCIIO:
+  if (PciAttributesSaved) {
+    //
+    // Restore original PCI attributes
+    //
+    PciIo->Attributes (
+                    PciIo,
+                    EfiPciIoAttributeOperationSet,
+                    OriginalPciAttributes,
+                    NULL
+                    );
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  EFI_USB2_HC_PROTOCOL  *Usb2Hc;
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  USB_XHCI_INSTANCE     *Xhc;
+  UINT8                 Index;
+
+  //
+  // Test whether the Controller handler passed in is a valid
+  // Usb controller handle that should be supported, if not,
+  // return the error status directly
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  Usb2Hc
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Xhc   = XHC_FROM_THIS (Usb2Hc);
+  PciIo = Xhc->PciIo;
+
+  //
+  // Stop AsyncRequest Polling timer then stop the XHCI driver
+  // and uninstall the XHCI protocl.
+  //
+  gBS->SetTimer (Xhc->PollTimer, TimerCancel, 0);
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0)) {
+      continue;
+    }
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      XhcDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+    } else {
+      XhcDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+    }
+  }
+
+  if (Xhc->PollTimer != NULL) {
+    gBS->CloseEvent (Xhc->PollTimer);
+  }
+
+  if (Xhc->ExitBootServiceEvent != NULL) {
+    gBS->CloseEvent (Xhc->ExitBootServiceEvent);
+  }
+
+  XhcHaltHC (Xhc, XHC_GENERIC_TIMEOUT);
+  XhcClearBiosOwnership (Xhc);
+  XhciDelAllAsyncIntTransfers (Xhc);
+  XhcFreeSched (Xhc);
+
+  if (Xhc->ControllerNameTable) {
+    FreeUnicodeStringTable (Xhc->ControllerNameTable);
+  }
+
+  //
+  // Restore original PCI attributes
+  //
+  PciIo->Attributes (
+           PciIo,
+           EfiPciIoAttributeOperationSet,
+           Xhc->OriginalPciAttributes,
+           NULL
+           );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiPciIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  FreePool (Xhc);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h
new file mode 100644
index 0000000000..7999151b3f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/Xhci.h
@@ -0,0 +1,727 @@
+/** @file
+
+  Provides some data structure definitions used by the XHCI host controller driver.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+#ifndef _EFI_XHCI_H_
+#define _EFI_XHCI_H_
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/PciIo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Pci.h>
+
+typedef struct _USB_XHCI_INSTANCE    USB_XHCI_INSTANCE;
+typedef struct _USB_DEV_CONTEXT      USB_DEV_CONTEXT;
+
+#include "XhciReg.h"
+#include "XhciSched.h"
+#include "ComponentName.h"
+#include "UsbHcMem.h"
+
+//
+// The unit is microsecond, setting it as 1us.
+//
+#define XHC_1_MICROSECOND            (1)
+//
+// The unit is microsecond, setting it as 1ms.
+//
+#define XHC_1_MILLISECOND            (1000)
+//
+// XHC generic timeout experience values.
+// The unit is millisecond, setting it as 10s.
+//
+#define XHC_GENERIC_TIMEOUT          (10 * 1000)
+//
+// XHC reset timeout experience values.
+// The unit is millisecond, setting it as 1s.
+//
+#define XHC_RESET_TIMEOUT            (1000)
+//
+// XHC async transfer timer interval, set by experience.
+// The unit is 100us, takes 1ms as interval.
+//
+#define XHC_ASYNC_TIMER_INTERVAL     EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+//
+// XHC raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define XHC_TPL                      TPL_NOTIFY
+
+#define CMD_RING_TRB_NUMBER          0x100
+#define TR_RING_TRB_NUMBER           0x100
+#define ERST_NUMBER                  0x01
+#define EVENT_RING_TRB_NUMBER        0x200
+
+#define CMD_INTER                    0
+#define CTRL_INTER                   1
+#define BULK_INTER                   2
+#define INT_INTER                    3
+#define INT_INTER_ASYNC              4
+
+//
+// Iterate through the doule linked list. This is delete-safe.
+// Don't touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead) \
+  for (Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+#define EFI_LIST_CONTAINER(Entry, Type, Field) BASE_CR(Entry, Type, Field)
+
+#define XHC_LOW_32BIT(Addr64)          ((UINT32)(((UINTN)(Addr64)) & 0xFFFFFFFF))
+#define XHC_HIGH_32BIT(Addr64)         ((UINT32)(RShiftU64((UINT64)(UINTN)(Addr64), 32) & 0xFFFFFFFF))
+#define XHC_BIT_IS_SET(Data, Bit)      ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define XHC_REG_BIT_IS_SET(Xhc, Offset, Bit) \
+          (XHC_BIT_IS_SET(XhcReadOpReg ((Xhc), (Offset)), (Bit)))
+
+#define XHCI_IS_DATAIN(EndpointAddr)   XHC_BIT_IS_SET((EndpointAddr), 0x80)
+
+#define XHCI_INSTANCE_SIG              SIGNATURE_32 ('x', 'h', 'c', 'i')
+#define XHC_FROM_THIS(a)               CR(a, USB_XHCI_INSTANCE, Usb2Hc, XHCI_INSTANCE_SIG)
+
+#define USB_DESC_TYPE_HUB              0x29
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+//
+// Xhci Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                     ProgInterface;
+  UINT8                     SubClassCode;
+  UINT8                     BaseCode;
+} USB_CLASSC;
+
+typedef struct {
+  UINT8                     Length;
+  UINT8                     DescType;
+  UINT8                     NumPorts;
+  UINT16                    HubCharacter;
+  UINT8                     PwrOn2PwrGood;
+  UINT8                     HubContrCurrent;
+  UINT8                     Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+#pragma pack()
+
+struct _USB_DEV_CONTEXT {
+  //
+  // Whether this entry in UsbDevContext array is used or not.
+  //
+  BOOLEAN                   Enabled;
+  //
+  // The slot id assigned to the new device through XHCI's Enable_Slot cmd.
+  //
+  UINT8                     SlotId;
+  //
+  // The route string presented an attached usb device.
+  //
+  USB_DEV_ROUTE             RouteString;
+  //
+  // The route string of parent device if it exists. Otherwise it's zero.
+  //
+  USB_DEV_ROUTE             ParentRouteString;
+  //
+  // The actual device address assigned by XHCI through Address_Device command.
+  //
+  UINT8                     XhciDevAddr;
+  //
+  // The requested device address from UsbBus driver through Set_Address standard usb request.
+  // As XHCI spec replaces this request with Address_Device command, we have to record the
+  // requested device address and establish a mapping relationship with the actual device address.
+  // Then UsbBus driver just need to be aware of the requested device address to access usb device
+  // through EFI_USB2_HC_PROTOCOL. Xhci driver would be responsible for translating it to actual
+  // device address and access the actual device.
+  //
+  UINT8                     BusDevAddr;
+  //
+  // The pointer to the input device context.
+  //
+  VOID                      *InputContext;
+  //
+  // The pointer to the output device context.
+  //
+  VOID                      *OutputContext;
+  //
+  // The transfer queue for every endpoint.
+  //
+  VOID                      *EndpointTransferRing[31];
+  //
+  // The device descriptor which is stored to support XHCI's Evaluate_Context cmd.
+  //
+  EFI_USB_DEVICE_DESCRIPTOR DevDesc;
+  //
+  // As a usb device may include multiple configuration descriptors, we dynamically allocate an array
+  // to store them.
+  // Note that every configuration descriptor stored here includes those lower level descriptors,
+  // such as Interface descriptor, Endpoint descriptor, and so on.
+  // These information is used to support XHCI's Config_Endpoint cmd.
+  //
+  EFI_USB_CONFIG_DESCRIPTOR **ConfDesc;
+  //
+  // A device has an active Configuration.
+  //
+  UINT8                     ActiveConfiguration;
+  //
+  // Every interface has an active AlternateSetting.
+  //
+  UINT8                     *ActiveAlternateSetting;
+};
+
+struct _USB_XHCI_INSTANCE {
+  UINT32                    Signature;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINT64                    OriginalPciAttributes;
+  USBHC_MEM_POOL            *MemPool;
+
+  EFI_USB2_HC_PROTOCOL      Usb2Hc;
+
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  //
+  // ExitBootServicesEvent is used to set OS semaphore and
+  // stop the XHC DMA operation after exit boot service.
+  //
+  EFI_EVENT                 ExitBootServiceEvent;
+  EFI_EVENT                 PollTimer;
+  LIST_ENTRY                AsyncIntTransfers;
+
+  UINT8                     CapLength;    ///< Capability Register Length
+  XHC_HCSPARAMS1            HcSParams1;   ///< Structural Parameters 1
+  XHC_HCSPARAMS2            HcSParams2;   ///< Structural Parameters 2
+  XHC_HCCPARAMS             HcCParams;    ///< Capability Parameters
+  UINT32                    DBOff;        ///< Doorbell Offset
+  UINT32                    RTSOff;       ///< Runtime Register Space Offset
+  UINT16                    MaxInterrupt;
+  UINT32                    PageSize;
+  UINT64                    *ScratchBuf;
+  VOID                      *ScratchMap;
+  UINT32                    MaxScratchpadBufs;
+  UINT64                    *ScratchEntry;
+  UINTN                     *ScratchEntryMap;
+  UINT32                    ExtCapRegBase;
+  UINT32                    UsbLegSupOffset;
+  UINT32                    DebugCapSupOffset;
+  UINT64                    *DCBAA;
+  VOID                      *DCBAAMap;
+  UINT32                    MaxSlotsEn;
+  //
+  // Cmd Transfer Ring
+  //
+  TRANSFER_RING             CmdRing;
+  //
+  // EventRing
+  //
+  EVENT_RING                EventRing;
+  //
+  // Misc
+  //
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  //
+  // Store device contexts managed by XHCI instance
+  // The array supports up to 255 devices, entry 0 is reserved and should not be used.
+  //
+  USB_DEV_CONTEXT           UsbDevContext[256];
+};
+
+
+extern EFI_DRIVER_BINDING_PROTOCOL      gXhciDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL      gXhciComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL     gXhciComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle. Any
+  ControllerHandle that has Usb2HcProtocol installed will
+  be supported.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          This driver supports this device.
+  @return EFI_UNSUPPORTED      This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Starting the Usb XHCI Driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to test.
+  @param  RemainingDevicePath  Not used.
+
+  @return EFI_SUCCESS          supports this device.
+  @return EFI_UNSUPPORTED      do not support this device.
+  @return EFI_DEVICE_ERROR     cannot be started due to device Error.
+  @return EFI_OUT_OF_RESOURCES cannot allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL    *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stoping any child handles
+  created by this driver.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to stop driver on.
+  @param  NumberOfChildren     Number of Children in the ChildHandleBuffer.
+  @param  ChildHandleBuffer    List of handles for the children we need to stop.
+
+  @return EFI_SUCCESS          Success.
+  @return EFI_DEVICE_ERROR     Fail.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+/**
+  Retrieves the capability of root hub ports.
+
+  @param  This                  The EFI_USB2_HC_PROTOCOL instance.
+  @param  MaxSpeed              Max speed supported by the controller.
+  @param  PortNumber            Number of the root hub ports.
+  @param  Is64BitCapable        Whether the controller supports 64-bit memory
+                                addressing.
+
+  @retval EFI_SUCCESS           Host controller capability were retrieved successfully.
+  @retval EFI_INVALID_PARAMETER Either of the three capability pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetCapability (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT UINT8                 *MaxSpeed,
+  OUT UINT8                 *PortNumber,
+  OUT UINT8                 *Is64BitCapable
+  );
+
+/**
+  Provides software reset for the USB host controller.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  Attributes            A bit mask of the reset operation to perform.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcReset (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT16                Attributes
+  );
+
+/**
+  Retrieve the current state of the USB host controller.
+
+  @param  This                   This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                  Variable to return the current host controller
+                                 state.
+
+  @retval EFI_SUCCESS            Host controller state was returned in State.
+  @retval EFI_INVALID_PARAMETER  State is NULL.
+  @retval EFI_DEVICE_ERROR       An error was encountered while attempting to
+                                 retrieve the host controller's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetState (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  OUT EFI_USB_HC_STATE      *State
+  );
+
+/**
+  Sets the USB host controller to a specific state.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  State                 The state of the host controller that will be set.
+
+  @retval EFI_SUCCESS           The USB host controller was successfully placed
+                                in the state specified by State.
+  @retval EFI_INVALID_PARAMETER State is invalid.
+  @retval EFI_DEVICE_ERROR      Failed to set the state due to device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetState (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN EFI_USB_HC_STATE      State
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            The root hub port to retrieve the state from.
+                                This value is zero-based.
+  @param  PortStatus            Variable to receive the port state.
+
+  @retval EFI_SUCCESS           The status of the USB root hub port specified.
+                                by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcGetRootHubPortStatus (
+  IN  EFI_USB2_HC_PROTOCOL  *This,
+  IN  UINT8                 PortNumber,
+  OUT EFI_USB_PORT_STATUS   *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Root hub port to set.
+  @param  PortFeature           Feature to set.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  This                  A pointer to the EFI_USB2_HC_PROTOCOL instance.
+  @param  PortNumber            Specifies the root hub port whose feature is
+                                requested to be cleared.
+  @param  PortFeature           Indicates the feature selector associated with the
+                                feature clear request.
+
+  @retval EFI_SUCCESS           The feature specified by PortFeature was cleared
+                                for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_DEVICE_ERROR      Can't read register.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcClearRootHubPortFeature (
+  IN EFI_USB2_HC_PROTOCOL  *This,
+  IN UINT8                 PortNumber,
+  IN EFI_USB_PORT_FEATURE  PortFeature
+  );
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         The target device address.
+  @param  DeviceSpeed           Target device speed.
+  @param  MaximumPacketLength   Maximum packet size the default control transfer
+                                endpoint is capable of sending or receiving.
+  @param  Request               USB device request to send.
+  @param  TransferDirection     Specifies the data direction for the data stage
+  @param  Data                  Data buffer to be transmitted or received from USB
+                                device.
+  @param  DataLength            The size (in bytes) of the data buffer.
+  @param  Timeout               Indicates the maximum timeout, in millisecond.
+  @param  Translator            Transaction translator to be used by this device.
+  @param  TransferResult        Return the result of this control transfer.
+
+  @retval EFI_SUCCESS           Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcControlTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     EFI_USB_DEVICE_REQUEST              *Request,
+  IN     EFI_USB_DATA_DIRECTION              TransferDirection,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support bulk
+                                transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  DataBuffersNumber     Number of data buffers prepared for the transfer.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to
+                                use of the subsequent bulk transfer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which
+                                the transfer is allowed to complete.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcBulkTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits an asynchronous interrupt transfer to an
+  interrupt endpoint of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+  @param  IsNewTransfer         If TRUE, to submit an new asynchronous interrupt
+                                transfer If FALSE, to remove the specified
+                                asynchronous interrupt.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  PollingInterval       The he interval, in milliseconds, that the transfer
+                                is polled.
+  @param  DataLength            The length of data to receive at the rate specified
+                                by  PollingInterval.
+  @param  Translator            Transaction translator to use.
+  @param  CallBackFunction      Function to call at the rate specified by
+                                PollingInterval.
+  @param  Context               Context to CallBackFunction.
+
+  @retval EFI_SUCCESS           The request has been successfully submitted or canceled.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request failed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               PollingInterval,
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     CallBackFunction,
+  IN     VOID                                *Context OPTIONAL
+  );
+
+/**
+  Submits synchronous interrupt transfer to an interrupt endpoint
+  of a USB device.
+
+  @param  This                  This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction encoded in bit 7
+  @param  DeviceSpeed           Indicates device speed.
+  @param  MaximumPacketLength   Maximum packet size the target endpoint is capable
+                                of sending or receiving.
+  @param  Data                  Buffer of data that will be transmitted to  USB
+                                device or received from USB device.
+  @param  DataLength            On input, the size, in bytes, of the data buffer; On
+                                output, the number of bytes transferred.
+  @param  DataToggle            On input, the initial data toggle to use; on output,
+                                it is updated to indicate the next data toggle.
+  @param  Timeout               Maximum time, in second, to complete.
+  @param  Translator            Transaction translator to use.
+  @param  TransferResult        Variable to receive the transfer result.
+
+  @return EFI_SUCCESS           The transfer was completed successfully.
+  @return EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @return EFI_INVALID_PARAMETER Some parameters are invalid.
+  @return EFI_TIMEOUT           The transfer failed due to timeout.
+  @return EFI_DEVICE_ERROR      The failed due to host controller or device error
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncInterruptTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN OUT VOID                                *Data,
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               Timeout,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  TransferResult       Variable to receive the transfer result.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer is unsupported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Submits Async isochronous transfer to a target USB device.
+
+  @param  This                 This EFI_USB2_HC_PROTOCOL instance.
+  @param  DeviceAddress        Target device address.
+  @param  EndPointAddress      End point address with its direction.
+  @param  DeviceSpeed          Device speed, Low speed device doesn't support this
+                               type.
+  @param  MaximumPacketLength  Maximum packet size that the endpoint is capable of
+                               sending or receiving.
+  @param  DataBuffersNumber    Number of data buffers prepared for the transfer.
+  @param  Data                 Array of pointers to the buffers of data that will
+                               be transmitted to USB device or received from USB
+                               device.
+  @param  DataLength           The size, in bytes, of the data buffer.
+  @param  Translator           Transaction translator to use.
+  @param  IsochronousCallBack  Function to be called when the transfer complete.
+  @param  Context              Context passed to the call back function as
+                               parameter.
+
+  @return EFI_UNSUPPORTED      Isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcAsyncIsochronousTransfer (
+  IN     EFI_USB2_HC_PROTOCOL                *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,
+  IN     UINTN                               MaximumPacketLength,
+  IN     UINT8                               DataBuffersNumber,
+  IN OUT VOID                                *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN     UINTN                               DataLength,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN     EFI_ASYNC_USB_TRANSFER_CALLBACK     IsochronousCallBack,
+  IN     VOID                                *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf
new file mode 100644
index 0000000000..614938ac89
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf
@@ -0,0 +1,76 @@
+## @file
+#  The XhciDxe driver is responsible for managing the behavior of XHCI controller.
+#  It implements the interfaces of monitoring the status of all ports and transferring
+#  Control, Bulk, Interrupt and Isochronous requests to those attached usb LS/FS/HS/SS devices.
+#
+#  Copyright (c) 2011 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = XhciDxe
+  MODULE_UNI_FILE                = XhciDxe.uni
+  FILE_GUID                      = B7F50E91-A759-412c-ADE4-DCD03E7F7C28
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = XhcDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gXhciDriverBinding
+#  COMPONENT_NAME                =  gXhciComponentName
+#  COMPONENT_NAME2               =  gXhciComponentName2
+#
+
+[Sources]
+  Xhci.c
+  XhciReg.c
+  XhciSched.c
+  UsbHcMem.c
+  UsbHcMem.h  
+  ComponentName.c
+  ComponentName.h
+  Xhci.h
+  XhciReg.h
+  XhciSched.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  ReportStatusCodeLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiPciIoProtocolGuid                         ## TO_START
+  gEfiUsb2HcProtocolGuid                        ## BY_START
+
+# [Event]
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  XhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni
new file mode 100644
index 0000000000..eea95dfa2b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni
new file mode 100644
index 0000000000..9c598b1868
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
new file mode 100644
index 0000000000..d0f22050ad
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.c
@@ -0,0 +1,743 @@
+/** @file
+
+  The XHCI register operation routines.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+/**
+  Read 1-byte width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 1-byte width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFF.
+
+**/
+UINT8
+XhcReadCapReg8 (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT8                   Data;
+  EFI_STATUS              Status;
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint8,
+                             XHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadCapReg: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read 4-bytes width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) Offset,
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadCapReg: Pci Io read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Read 4-bytes width XHCI Operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width operational register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadOpReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadOpReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the 4-bytes width XHCI operational register.
+
+  @param  Xhc      The XHCI Instance.
+  @param  Offset   The offset of the 4-bytes width operational register.
+  @param  Data     The data to write.
+
+**/
+VOID
+XhcWriteOpReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Write the data to the 2-bytes width XHCI operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 2-bytes width operational register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteOpReg16 (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT16               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint16,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->CapLength + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg16: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadDoorBellReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->DBOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadDoorBellReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteDoorBellReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->DBOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteOpReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->RTSOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadRuntimeReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->RTSOff + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteRuntimeReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+/**
+  Read XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadExtCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  )
+{
+  UINT32                  Data;
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->ExtCapRegBase != 0);
+
+  Status = Xhc->PciIo->Mem.Read (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->ExtCapRegBase + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcReadExtCapReg: Pci Io Read error - %r at %d\n", Status, Offset));
+    Data = 0xFFFFFFFF;
+  }
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteExtCapReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  EFI_STATUS              Status;
+
+  ASSERT (Xhc->ExtCapRegBase != 0);
+
+  Status = Xhc->PciIo->Mem.Write (
+                             Xhc->PciIo,
+                             EfiPciIoWidthUint32,
+                             XHC_BAR_INDEX,
+                             (UINT64) (Xhc->ExtCapRegBase + Offset),
+                             1,
+                             &Data
+                             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcWriteExtCapReg: Pci Io Write error: %r at %d\n", Status, Offset));
+  }
+}
+
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadRuntimeReg (Xhc, Offset);
+  Data |= Bit;
+  XhcWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcClearRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadRuntimeReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadOpReg (Xhc, Offset);
+  Data |= Bit;
+  XhcWriteOpReg (Xhc, Offset, Data);
+}
+
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+XhcClearOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  )
+{
+  UINT32                  Data;
+
+  Data  = XhcReadOpReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operation register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+XhcWaitOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  )
+{
+  UINT32                  Index;
+  UINT64                  Loop;
+
+  Loop   = Timeout * XHC_1_MILLISECOND;
+
+  for (Index = 0; Index < Loop; Index++) {
+    if (XHC_REG_BIT_IS_SET (Xhc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (XHC_1_MICROSECOND);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Set Bios Ownership
+
+  @param  Xhc          The XHCI Instance.
+
+**/
+VOID
+XhcSetBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                    Buffer;
+
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcSetBiosOwnership: called to set BIOS ownership\n"));
+
+  Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
+  Buffer = ((Buffer & (~USBLEGSP_OS_SEMAPHORE)) | USBLEGSP_BIOS_SEMAPHORE);
+  XhcWriteExtCapReg (Xhc, Xhc->UsbLegSupOffset, Buffer);
+}
+
+/**
+  Clear Bios Ownership
+
+  @param  Xhc       The XHCI Instance.
+
+**/
+VOID
+XhcClearBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                    Buffer;
+
+  if (Xhc->UsbLegSupOffset == 0xFFFFFFFF) {
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcClearBiosOwnership: called to clear BIOS ownership\n"));
+
+  Buffer = XhcReadExtCapReg (Xhc, Xhc->UsbLegSupOffset);
+  Buffer = ((Buffer & (~USBLEGSP_BIOS_SEMAPHORE)) | USBLEGSP_OS_SEMAPHORE);
+  XhcWriteExtCapReg (Xhc, Xhc->UsbLegSupOffset, Buffer);
+}
+
+/**
+  Calculate the offset of the XHCI capability.
+
+  @param  Xhc     The XHCI Instance.
+  @param  CapId   The XHCI Capability ID.
+
+  @return The offset of XHCI legacy support capability register.
+
+**/
+UINT32
+XhcGetCapabilityAddr (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                CapId
+  )
+{
+  UINT32 ExtCapOffset;
+  UINT8  NextExtCapReg;
+  UINT32 Data;
+
+  ExtCapOffset = 0;
+
+  do {
+    //
+    // Check if the extended capability register's capability id is USB Legacy Support.
+    //
+    Data = XhcReadExtCapReg (Xhc, ExtCapOffset);
+    if ((Data & 0xFF) == CapId) {
+      return ExtCapOffset;
+    }
+    //
+    // If not, then traverse all of the ext capability registers till finding out it.
+    //
+    NextExtCapReg = (UINT8)((Data >> 8) & 0xFF);
+    ExtCapOffset += (NextExtCapReg << 2);
+  } while (NextExtCapReg != 0);
+
+  return 0xFFFFFFFF;
+}
+
+/**
+  Whether the XHCI host controller is halted.
+
+  @param  Xhc     The XHCI Instance.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+XhcIsHalt (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT);
+}
+
+
+/**
+  Whether system error occurred.
+
+  @param  Xhc      The XHCI Instance.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+XhcIsSysError (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE);
+}
+
+/**
+  Reset the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS  The XHCI host controller is reset.
+  @return Others       Failed to reset the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcResetHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EFI_SUCCESS;
+
+  DEBUG ((EFI_D_INFO, "XhcResetHC!\n"));
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT)) {
+    Status = XhcHaltHC (Xhc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if ((Xhc->DebugCapSupOffset == 0xFFFFFFFF) || ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset) & 0xFF) != XHC_CAP_USB_DEBUG) ||
+      ((XhcReadExtCapReg (Xhc, Xhc->DebugCapSupOffset + XHC_DC_DCCTRL) & BIT0) == 0)) {
+    XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
+    Status = XhcWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+  }
+
+  return Status;
+}
+
+
+/**
+  Halt the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @return EFI_SUCCESS  The XHCI host controller is halt.
+  @return EFI_TIMEOUT  Failed to halt the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcHaltHC (
+  IN USB_XHCI_INSTANCE   *Xhc,
+  IN UINT32              Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  XhcClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, TRUE, Timeout);
+  return Status;
+}
+
+
+/**
+  Set the XHCI host controller to run.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in millisecond, ms).
+
+  @return EFI_SUCCESS  The XHCI host controller is running.
+  @return EFI_TIMEOUT  Failed to set the XHCI to run before Timeout.
+
+**/
+EFI_STATUS
+XhcRunHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  )
+{
+  EFI_STATUS              Status;
+
+  XhcSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, FALSE, Timeout);
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h
new file mode 100644
index 0000000000..b748c8d397
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciReg.h
@@ -0,0 +1,583 @@
+/** @file
+
+  This file contains the register definition of XHCI host controller.
+
+Copyright (c) 2011 - 2013, 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.
+
+**/
+
+#ifndef _EFI_XHCI_REG_H_
+#define _EFI_XHCI_REG_H_
+
+#define PCI_IF_XHCI                 0x30
+
+//
+// PCI Configuration Registers
+//
+#define XHC_BAR_INDEX               0x00
+
+#define XHC_PCI_BAR_OFFSET          0x10       // Memory Bar Register Offset
+#define XHC_PCI_BAR_MASK            0xFFFF     // Memory Base Address Mask
+
+#define USB_HUB_CLASS_CODE          0x09
+#define USB_HUB_SUBCLASS_CODE       0x00
+
+#define XHC_CAP_USB_LEGACY          0x01
+#define XHC_CAP_USB_DEBUG           0x0A
+
+//============================================//
+//           XHCI register offset             //
+//============================================//
+
+//
+// Capability registers offset
+//
+#define XHC_CAPLENGTH_OFFSET               0x00 // Capability register length offset
+#define XHC_HCIVERSION_OFFSET              0x02 // Interface Version Number 02-03h
+#define XHC_HCSPARAMS1_OFFSET              0x04 // Structural Parameters 1
+#define XHC_HCSPARAMS2_OFFSET              0x08 // Structural Parameters 2
+#define XHC_HCSPARAMS3_OFFSET              0x0c // Structural Parameters 3
+#define XHC_HCCPARAMS_OFFSET               0x10 // Capability Parameters
+#define XHC_DBOFF_OFFSET                   0x14 // Doorbell Offset
+#define XHC_RTSOFF_OFFSET                  0x18 // Runtime Register Space Offset
+
+//
+// Operational registers offset
+//
+#define XHC_USBCMD_OFFSET                  0x0000 // USB Command Register Offset
+#define XHC_USBSTS_OFFSET                  0x0004 // USB Status Register Offset
+#define XHC_PAGESIZE_OFFSET                0x0008 // USB Page Size Register Offset
+#define XHC_DNCTRL_OFFSET                  0x0014 // Device Notification Control Register Offset
+#define XHC_CRCR_OFFSET                    0x0018 // Command Ring Control Register Offset
+#define XHC_DCBAAP_OFFSET                  0x0030 // Device Context Base Address Array Pointer Register Offset
+#define XHC_CONFIG_OFFSET                  0x0038 // Configure Register Offset
+#define XHC_PORTSC_OFFSET                  0x0400 // Port Status and Control Register Offset
+
+//
+// Runtime registers offset
+//
+#define XHC_MFINDEX_OFFSET                 0x00 // Microframe Index Register Offset
+#define XHC_IMAN_OFFSET                    0x20 // Interrupter X Management Register Offset
+#define XHC_IMOD_OFFSET                    0x24 // Interrupter X Moderation Register Offset
+#define XHC_ERSTSZ_OFFSET                  0x28 // Event Ring Segment Table Size Register Offset
+#define XHC_ERSTBA_OFFSET                  0x30 // Event Ring Segment Table Base Address Register Offset
+#define XHC_ERDP_OFFSET                    0x38 // Event Ring Dequeue Pointer Register Offset
+
+//
+// Debug registers offset
+//
+#define XHC_DC_DCCTRL                      0x20
+
+#define USBLEGSP_BIOS_SEMAPHORE            BIT16 // HC BIOS Owned Semaphore
+#define USBLEGSP_OS_SEMAPHORE              BIT24 // HC OS Owned Semaphore
+
+#pragma pack (1)
+typedef struct {
+  UINT8                   MaxSlots;       // Number of Device Slots
+  UINT16                  MaxIntrs:11;    // Number of Interrupters
+  UINT16                  Rsvd:5;
+  UINT8                   MaxPorts;       // Number of Ports
+} HCSPARAMS1;
+
+//
+// Structural Parameters 1 Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCSPARAMS1              Data;
+} XHC_HCSPARAMS1;
+
+typedef struct {
+  UINT32                  Ist:4;          // Isochronous Scheduling Threshold
+  UINT32                  Erst:4;         // Event Ring Segment Table Max
+  UINT32                  Rsvd:13;
+  UINT32                  ScratchBufHi:5; // Max Scratchpad Buffers Hi
+  UINT32                  Spr:1;          // Scratchpad Restore
+  UINT32                  ScratchBufLo:5; // Max Scratchpad Buffers Lo
+} HCSPARAMS2;
+
+//
+// Structural Parameters 2 Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCSPARAMS2              Data;
+} XHC_HCSPARAMS2;
+
+typedef struct {
+  UINT16                  Ac64:1;        // 64-bit Addressing Capability
+  UINT16                  Bnc:1;         // BW Negotiation Capability
+  UINT16                  Csz:1;         // Context Size
+  UINT16                  Ppc:1;         // Port Power Control
+  UINT16                  Pind:1;        // Port Indicators
+  UINT16                  Lhrc:1;        // Light HC Reset Capability
+  UINT16                  Ltc:1;         // Latency Tolerance Messaging Capability
+  UINT16                  Nss:1;         // No Secondary SID Support
+  UINT16                  Pae:1;         // Parse All Event Data
+  UINT16                  Rsvd:3;
+  UINT16                  MaxPsaSize:4;  // Maximum Primary Stream Array Size
+  UINT16                  ExtCapReg;     // xHCI Extended Capabilities Pointer
+} HCCPARAMS;
+
+//
+// Capability Parameters Register Bitmap Definition
+//
+typedef union {
+  UINT32                  Dword;
+  HCCPARAMS               Data;
+} XHC_HCCPARAMS;
+
+#pragma pack ()
+
+//
+// Register Bit Definition
+//
+#define XHC_USBCMD_RUN                     BIT0  // Run/Stop
+#define XHC_USBCMD_RESET                   BIT1  // Host Controller Reset
+#define XHC_USBCMD_INTE                    BIT2  // Interrupter Enable
+#define XHC_USBCMD_HSEE                    BIT3  // Host System Error Enable
+
+#define XHC_USBSTS_HALT                    BIT0  // Host Controller Halted
+#define XHC_USBSTS_HSE                     BIT2  // Host System Error
+#define XHC_USBSTS_EINT                    BIT3  // Event Interrupt
+#define XHC_USBSTS_PCD                     BIT4  // Port Change Detect
+#define XHC_USBSTS_SSS                     BIT8  // Save State Status
+#define XHC_USBSTS_RSS                     BIT9  // Restore State Status
+#define XHC_USBSTS_SRE                     BIT10 // Save/Restore Error
+#define XHC_USBSTS_CNR                     BIT11 // Host Controller Not Ready
+#define XHC_USBSTS_HCE                     BIT12 // Host Controller Error
+
+#define XHC_PAGESIZE_MASK                  0xFFFF // Page Size
+
+#define XHC_CRCR_RCS                       BIT0  // Ring Cycle State
+#define XHC_CRCR_CS                        BIT1  // Command Stop
+#define XHC_CRCR_CA                        BIT2  // Command Abort
+#define XHC_CRCR_CRR                       BIT3  // Command Ring Running
+
+#define XHC_CONFIG_MASK                    0xFF  // Command Ring Running
+
+#define XHC_PORTSC_CCS                     BIT0  // Current Connect Status
+#define XHC_PORTSC_PED                     BIT1  // Port Enabled/Disabled
+#define XHC_PORTSC_OCA                     BIT3  // Over-current Active
+#define XHC_PORTSC_RESET                   BIT4  // Port Reset
+#define XHC_PORTSC_PLS                     (BIT5|BIT6|BIT7|BIT8)     // Port Link State
+#define XHC_PORTSC_PP                      BIT9  // Port Power
+#define XHC_PORTSC_PS                      (BIT10|BIT11|BIT12)       // Port Speed
+#define XHC_PORTSC_LWS                     BIT16 // Port Link State Write Strobe
+#define XHC_PORTSC_CSC                     BIT17 // Connect Status Change
+#define XHC_PORTSC_PEC                     BIT18 // Port Enabled/Disabled Change
+#define XHC_PORTSC_WRC                     BIT19 // Warm Port Reset Change
+#define XHC_PORTSC_OCC                     BIT20 // Over-Current Change
+#define XHC_PORTSC_PRC                     BIT21 // Port Reset Change
+#define XHC_PORTSC_PLC                     BIT22 // Port Link State Change
+#define XHC_PORTSC_CEC                     BIT23 // Port Config Error Change
+#define XHC_PORTSC_CAS                     BIT24 // Cold Attach Status
+
+#define XHC_HUB_PORTSC_CCS                 BIT0  // Hub's Current Connect Status
+#define XHC_HUB_PORTSC_PED                 BIT1  // Hub's Port Enabled/Disabled
+#define XHC_HUB_PORTSC_OCA                 BIT3  // Hub's Over-current Active
+#define XHC_HUB_PORTSC_RESET               BIT4  // Hub's Port Reset
+#define XHC_HUB_PORTSC_PP                  BIT9  // Hub's Port Power
+#define XHC_HUB_PORTSC_CSC                 BIT16 // Hub's Connect Status Change
+#define XHC_HUB_PORTSC_PEC                 BIT17 // Hub's Port Enabled/Disabled Change
+#define XHC_HUB_PORTSC_OCC                 BIT19 // Hub's Over-Current Change
+#define XHC_HUB_PORTSC_PRC                 BIT20 // Hub's Port Reset Change
+#define XHC_HUB_PORTSC_BHRC                BIT21 // Hub's Port Warm Reset Change
+#define XHC_IMAN_IP                        BIT0  // Interrupt Pending
+#define XHC_IMAN_IE                        BIT1  // Interrupt Enable
+
+#define XHC_IMODI_MASK                     0x0000FFFF  // Interrupt Moderation Interval
+#define XHC_IMODC_MASK                     0xFFFF0000  // Interrupt Moderation Counter
+
+//
+//  Hub Class Feature Selector for Clear Port Feature Request
+//  It's the extension of hub class feature selector of USB 2.0 in USB 3.0 Spec.
+//  For more details, Please refer to USB 3.0 Spec Table 10-7.
+//
+typedef enum {
+  Usb3PortBHPortReset          = 28,
+  Usb3PortBHPortResetChange    = 29
+} XHC_PORT_FEATURE;
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Structure to map the hardware port states to feature selector for clear port feature request.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  Selector;
+} USB_CLEAR_PORT_MAP;
+
+/**
+  Read 1-byte width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 1-byte width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFF.
+
+**/
+UINT8
+XhcReadCapReg8 (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read 4-bytes width XHCI capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width capability register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Read 4-bytes width XHCI Operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 4-bytes width operational register.
+
+  @return The register content read.
+  @retval If err, return 0xFFFFFFFF.
+
+**/
+UINT32
+XhcReadOpReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the 4-bytes width XHCI operational register.
+
+  @param  Xhc      The XHCI Instance.
+  @param  Offset   The offset of the 4-bytes width operational register.
+  @param  Data     The data to write.
+
+**/
+VOID
+XhcWriteOpReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Write the data to the 2-bytes width XHCI operational register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the 2-bytes width operational register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteOpReg16 (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT16               Data
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadDoorBellReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the door bell register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteDoorBellReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit mask of the register to clear.
+
+**/
+VOID
+XhcClearOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to be set (or clear).
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the operational register.
+  @param  Bit          The bit of the register to wait for.
+  @param  WaitToSet    Wait the bit to set or clear.
+  @param  Timeout      The time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS  The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT  The time out occurred.
+
+**/
+EFI_STATUS
+XhcWaitOpRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit,
+  IN BOOLEAN              WaitToSet,
+  IN UINT32               Timeout
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadRuntimeReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Data         The data to write.
+
+**/
+VOID
+XhcWriteRuntimeReg (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  );
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcSetRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcClearRuntimeRegBit (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Bit
+  );
+
+/**
+  Read XHCI extended capability register.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Offset       The offset of the extended capability register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcReadExtCapReg (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT32              Offset
+  );
+
+/**
+  Whether the XHCI host controller is halted.
+
+  @param  Xhc     The XHCI Instance.
+
+  @retval TRUE    The controller is halted.
+  @retval FALSE   It isn't halted.
+
+**/
+BOOLEAN
+XhcIsHalt (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Whether system error occurred.
+
+  @param  Xhc      The XHCI Instance.
+
+  @retval TRUE     System error happened.
+  @retval FALSE    No system error.
+
+**/
+BOOLEAN
+XhcIsSysError (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Reset the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS  The XHCI host controller is reset.
+  @return Others       Failed to reset the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcResetHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  );
+
+/**
+  Halt the XHCI host controller.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @return EFI_SUCCESS  The XHCI host controller is halt.
+  @return EFI_TIMEOUT  Failed to halt the XHCI before Timeout.
+
+**/
+EFI_STATUS
+XhcHaltHC (
+  IN USB_XHCI_INSTANCE   *Xhc,
+  IN UINT32              Timeout
+  );
+
+/**
+  Set the XHCI host controller to run.
+
+  @param  Xhc          The XHCI Instance.
+  @param  Timeout      Time to wait before abort (in microsecond, us).
+
+  @return EFI_SUCCESS  The XHCI host controller is running.
+  @return EFI_TIMEOUT  Failed to set the XHCI to run before Timeout.
+
+**/
+EFI_STATUS
+XhcRunHC (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT32               Timeout
+  );
+
+/**
+  Calculate the offset of the XHCI capability.
+
+  @param  Xhc     The XHCI Instance.
+  @param  CapId   The XHCI Capability ID.
+
+  @return The offset of XHCI legacy support capability register.
+
+**/
+UINT32
+XhcGetCapabilityAddr (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                CapId
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c
new file mode 100644
index 0000000000..c25342dc1f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c
@@ -0,0 +1,3838 @@
+/** @file
+
+  XHCI transfer scheduling routines.
+
+Copyright (c) 2011 - 2015, 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 "Xhci.h"
+
+/**
+  Create a command transfer TRB to support XHCI command interfaces.
+
+  @param  Xhc       The XHCI Instance.
+  @param  CmdTrb    The cmd TRB to be executed.
+
+  @return Created URB or NULL.
+
+**/
+URB*
+XhcCreateCmdTrb (
+  IN USB_XHCI_INSTANCE  *Xhc,
+  IN TRB_TEMPLATE       *CmdTrb
+  )
+{
+  URB    *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = XHC_URB_SIG;
+
+  Urb->Ring       = &Xhc->CmdRing;
+  XhcSyncTrsRing (Xhc, Urb->Ring);
+  Urb->TrbNum     = 1;
+  Urb->TrbStart   = Urb->Ring->RingEnqueue;
+  CopyMem (Urb->TrbStart, CmdTrb, sizeof (TRB_TEMPLATE));
+  Urb->TrbStart->CycleBit = Urb->Ring->RingPCS & BIT0;
+  Urb->TrbEnd             = Urb->TrbStart;
+
+  return Urb;
+}
+
+/**
+  Execute a XHCI cmd TRB pointed by CmdTrb.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  CmdTrb                The cmd TRB to be executed.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  EvtTrb                The event TRB corresponding to the cmd TRB.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCmdTransfer (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  TRB_TEMPLATE          *CmdTrb,
+  IN  UINTN                 Timeout,
+  OUT TRB_TEMPLATE          **EvtTrb
+  )
+{
+  EFI_STATUS      Status;
+  URB             *Urb;
+
+  //
+  // Validate the parameters
+  //
+  if ((Xhc == NULL) || (CmdTrb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_DEVICE_ERROR;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcCmdTransfer: HC is halted\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, then poll the execution status.
+  //
+  Urb = XhcCreateCmdTrb (Xhc, CmdTrb);
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcCmdTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status  = XhcExecTransfer (Xhc, TRUE, Urb, Timeout);
+  *EvtTrb = Urb->EvtTrb;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  XhcFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI Instance
+  @param  BusAddr   The logical device address assigned by UsbBus driver
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcCreateUrb (
+  IN USB_XHCI_INSTANCE                  *Xhc,
+  IN UINT8                              BusAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  )
+{
+  USB_ENDPOINT                  *Ep;
+  EFI_STATUS                    Status;
+  URB                           *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature = XHC_URB_SIG;
+  InitializeListHead (&Urb->UrbList);
+
+  Ep            = &Urb->Ep;
+  Ep->BusAddr   = BusAddr;
+  Ep->EpAddr    = (UINT8)(EpAddr & 0x0F);
+  Ep->Direction = ((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  Ep->DevSpeed  = DevSpeed;
+  Ep->MaxPacket = MaxPacket;
+  Ep->Type      = Type;
+
+  Urb->Request  = Request;
+  Urb->Data     = Data;
+  Urb->DataLen  = DataLen;
+  Urb->Callback = Callback;
+  Urb->Context  = Context;
+
+  Status = XhcCreateTransferTrb (Xhc, Urb);
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcCreateUrb: XhcCreateTransferTrb Failed, Status = %r\n", Status));
+    FreePool (Urb);
+    Urb = NULL;
+  }
+
+  return Urb;
+}
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+XhcFreeUrb (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN URB                  *Urb
+  )
+{
+  if ((Xhc == NULL) || (Urb == NULL)) {
+    return;
+  }
+  
+  if (Urb->DataMap != NULL) {
+    Xhc->PciIo->Unmap (Xhc->PciIo, Urb->DataMap);
+  }
+
+  FreePool (Urb);
+}
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc     The XHCI Instance
+  @param  Urb     The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcCreateTransferTrb (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN URB                        *Urb
+  )
+{
+  VOID                          *OutputContext;
+  TRANSFER_RING                 *EPRing;
+  UINT8                         EPType;
+  UINT8                         SlotId;
+  UINT8                         Dci;
+  TRB                           *TrbStart;
+  UINTN                         TotalLen;
+  UINTN                         Len;
+  UINTN                         TrbNum;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  VOID                          *Map;
+  EFI_STATUS                    Status;
+
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Urb->Finished  = FALSE;
+  Urb->StartDone = FALSE;
+  Urb->EndDone   = FALSE;
+  Urb->Completed = 0;
+  Urb->Result    = EFI_USB_NOERROR;
+
+  Dci       = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  EPRing    = (TRANSFER_RING *)(UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1];
+  Urb->Ring = EPRing;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  if (Xhc->HcCParams.Data.Csz == 0) {
+    EPType  = (UINT8) ((DEVICE_CONTEXT *)OutputContext)->EP[Dci-1].EPType;
+  } else {
+    EPType  = (UINT8) ((DEVICE_CONTEXT_64 *)OutputContext)->EP[Dci-1].EPType;
+  }
+  
+  if (Urb->Data != NULL) {
+    if (((UINT8) (Urb->Ep.Direction)) == EfiUsbDataIn) {
+      MapOp = EfiPciIoOperationBusMasterWrite;
+    } else {
+      MapOp = EfiPciIoOperationBusMasterRead;
+    }
+    
+    Len = Urb->DataLen;
+    Status  = Xhc->PciIo->Map (Xhc->PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+    
+    if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+      DEBUG ((EFI_D_ERROR, "XhcCreateTransferTrb: Fail to map Urb->Data.\n"));
+      return EFI_OUT_OF_RESOURCES;
+    }
+    
+    Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+    Urb->DataMap  = Map;
+  }
+
+  //
+  // Construct the TRB
+  //
+  XhcSyncTrsRing (Xhc, EPRing);
+  Urb->TrbStart = EPRing->RingEnqueue;
+  switch (EPType) {
+    case ED_CONTROL_BIDIR:
+      //
+      // For control transfer, create SETUP_STAGE_TRB first.
+      //
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      TrbStart->TrbCtrSetup.bmRequestType = Urb->Request->RequestType;
+      TrbStart->TrbCtrSetup.bRequest      = Urb->Request->Request;
+      TrbStart->TrbCtrSetup.wValue        = Urb->Request->Value;
+      TrbStart->TrbCtrSetup.wIndex        = Urb->Request->Index;
+      TrbStart->TrbCtrSetup.wLength       = Urb->Request->Length;
+      TrbStart->TrbCtrSetup.Length        = 8;
+      TrbStart->TrbCtrSetup.IntTarget     = 0;
+      TrbStart->TrbCtrSetup.IOC           = 1;
+      TrbStart->TrbCtrSetup.IDT           = 1;
+      TrbStart->TrbCtrSetup.Type          = TRB_TYPE_SETUP_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrSetup.TRT = 3;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrSetup.TRT = 2;
+      } else {
+        TrbStart->TrbCtrSetup.TRT = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrSetup.CycleBit = EPRing->RingPCS & BIT0;
+      Urb->TrbNum++;
+
+      //
+      // For control transfer, create DATA_STAGE_TRB.
+      //
+      if (Urb->DataLen > 0) {
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbCtrData.TRBPtrLo  = XHC_LOW_32BIT(Urb->DataPhy);
+        TrbStart->TrbCtrData.TRBPtrHi  = XHC_HIGH_32BIT(Urb->DataPhy);
+        TrbStart->TrbCtrData.Length    = (UINT32) Urb->DataLen;
+        TrbStart->TrbCtrData.TDSize    = 0;
+        TrbStart->TrbCtrData.IntTarget = 0;
+        TrbStart->TrbCtrData.ISP       = 1;
+        TrbStart->TrbCtrData.IOC       = 1;
+        TrbStart->TrbCtrData.IDT       = 0;
+        TrbStart->TrbCtrData.CH        = 0;
+        TrbStart->TrbCtrData.Type      = TRB_TYPE_DATA_STAGE;
+        if (Urb->Ep.Direction == EfiUsbDataIn) {
+          TrbStart->TrbCtrData.DIR = 1;
+        } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+          TrbStart->TrbCtrData.DIR = 0;
+        } else {
+          TrbStart->TrbCtrData.DIR = 0;
+        }
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbCtrData.CycleBit = EPRing->RingPCS & BIT0;
+        Urb->TrbNum++;
+      }
+      //
+      // For control transfer, create STATUS_STAGE_TRB.
+      // Get the pointer to next TRB for status stage use
+      //
+      XhcSyncTrsRing (Xhc, EPRing);
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      TrbStart->TrbCtrStatus.IntTarget = 0;
+      TrbStart->TrbCtrStatus.IOC       = 1;
+      TrbStart->TrbCtrStatus.CH        = 0;
+      TrbStart->TrbCtrStatus.Type      = TRB_TYPE_STATUS_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrStatus.DIR = 1;
+      } else {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrStatus.CycleBit = EPRing->RingPCS & BIT0;
+      //
+      // Update the enqueue pointer
+      //
+      XhcSyncTrsRing (Xhc, EPRing);
+      Urb->TrbNum++;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+
+      break;
+
+    case ED_BULK_OUT:
+    case ED_BULK_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    case ED_INTERRUPT_OUT:
+    case ED_INTERRUPT_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    default:
+      DEBUG ((EFI_D_INFO, "Not supported EPType 0x%x!\n",EPType));
+      ASSERT (FALSE);
+      break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc        The XHCI Instance to be initialized.
+
+**/
+VOID
+XhcInitSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  VOID                  *Dcbaa;
+  EFI_PHYSICAL_ADDRESS  DcbaaPhy;
+  UINT64                CmdRing;
+  EFI_PHYSICAL_ADDRESS  CmdRingPhy; 
+  UINTN                 Entries;
+  UINT32                MaxScratchpadBufs;
+  UINT64                *ScratchBuf;
+  EFI_PHYSICAL_ADDRESS  ScratchPhy;
+  UINT64                *ScratchEntry;
+  EFI_PHYSICAL_ADDRESS  ScratchEntryPhy;
+  UINT32                Index;
+  UINTN                 *ScratchEntryMap;
+  EFI_STATUS            Status;
+
+  //
+  // Initialize memory management.
+  //
+  Xhc->MemPool = UsbHcInitMemPool (Xhc->PciIo);
+  ASSERT (Xhc->MemPool != NULL);
+
+  //
+  // Program the Max Device Slots Enabled (MaxSlotsEn) field in the CONFIG register (5.4.7)
+  // to enable the device slots that system software is going to use.
+  //
+  Xhc->MaxSlotsEn = Xhc->HcSParams1.Data.MaxSlots;
+  ASSERT (Xhc->MaxSlotsEn >= 1 && Xhc->MaxSlotsEn <= 255);
+  XhcWriteOpReg (Xhc, XHC_CONFIG_OFFSET, Xhc->MaxSlotsEn);
+
+  //
+  // The Device Context Base Address Array entry associated with each allocated Device Slot
+  // shall contain a 64-bit pointer to the base of the associated Device Context.
+  // The Device Context Base Address Array shall contain MaxSlotsEn + 1 entries.
+  // Software shall set Device Context Base Address Array entries for unallocated Device Slots to '0'.
+  //
+  Entries = (Xhc->MaxSlotsEn + 1) * sizeof(UINT64);
+  Dcbaa = UsbHcAllocateMem (Xhc->MemPool, Entries);
+  ASSERT (Dcbaa != NULL);
+  ZeroMem (Dcbaa, Entries);
+
+  //
+  // A Scratchpad Buffer is a PAGESIZE block of system memory located on a PAGESIZE boundary.
+  // System software shall allocate the Scratchpad Buffer(s) before placing the xHC in to Run
+  // mode (Run/Stop(R/S) ='1').
+  //
+  MaxScratchpadBufs      = ((Xhc->HcSParams2.Data.ScratchBufHi) << 5) | (Xhc->HcSParams2.Data.ScratchBufLo);
+  Xhc->MaxScratchpadBufs = MaxScratchpadBufs;
+  ASSERT (MaxScratchpadBufs <= 1023);
+  if (MaxScratchpadBufs != 0) {
+    //
+    // Allocate the buffer to record the Mapping for each scratch buffer in order to Unmap them
+    //
+    ScratchEntryMap = AllocateZeroPool (sizeof (UINTN) * MaxScratchpadBufs);
+    ASSERT (ScratchEntryMap != NULL);
+    Xhc->ScratchEntryMap = ScratchEntryMap;
+    
+    //
+    // Allocate the buffer to record the host address for each entry
+    //
+    ScratchEntry = AllocateZeroPool (sizeof (UINT64) * MaxScratchpadBufs);
+    ASSERT (ScratchEntry != NULL);
+    Xhc->ScratchEntry = ScratchEntry;
+
+    ScratchPhy = 0;
+    Status = UsbHcAllocateAlignedPages (
+               Xhc->PciIo,
+               EFI_SIZE_TO_PAGES (MaxScratchpadBufs * sizeof (UINT64)),
+               Xhc->PageSize,
+               (VOID **) &ScratchBuf, 
+               &ScratchPhy,
+               &Xhc->ScratchMap
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    ZeroMem (ScratchBuf, MaxScratchpadBufs * sizeof (UINT64));
+    Xhc->ScratchBuf = ScratchBuf;
+
+    //
+    // Allocate each scratch buffer
+    //
+    for (Index = 0; Index < MaxScratchpadBufs; Index++) {
+      ScratchEntryPhy = 0;
+      Status = UsbHcAllocateAlignedPages (
+                 Xhc->PciIo,
+                 EFI_SIZE_TO_PAGES (Xhc->PageSize),
+                 Xhc->PageSize,
+                 (VOID **) &ScratchEntry[Index],
+                 &ScratchEntryPhy,
+                 (VOID **) &ScratchEntryMap[Index]
+                 );
+      ASSERT_EFI_ERROR (Status);
+      ZeroMem ((VOID *)(UINTN)ScratchEntry[Index], Xhc->PageSize);
+      //
+      // Fill with the PCI device address
+      //
+      *ScratchBuf++ = ScratchEntryPhy;
+    }
+    //
+    // The Scratchpad Buffer Array contains pointers to the Scratchpad Buffers. Entry 0 of the
+    // Device Context Base Address Array points to the Scratchpad Buffer Array.
+    //
+    *(UINT64 *)Dcbaa = (UINT64)(UINTN) ScratchPhy;
+  }
+
+  //
+  // Program the Device Context Base Address Array Pointer (DCBAAP) register (5.4.6) with
+  // a 64-bit address pointing to where the Device Context Base Address Array is located.
+  //
+  Xhc->DCBAA = (UINT64 *)(UINTN)Dcbaa;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  DcbaaPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Dcbaa, Entries);
+  XhcWriteOpReg (Xhc, XHC_DCBAAP_OFFSET, XHC_LOW_32BIT(DcbaaPhy));
+  XhcWriteOpReg (Xhc, XHC_DCBAAP_OFFSET + 4, XHC_HIGH_32BIT (DcbaaPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcInitSched:DCBAA=0x%x\n", (UINT64)(UINTN)Xhc->DCBAA));
+
+  //
+  // Define the Command Ring Dequeue Pointer by programming the Command Ring Control Register
+  // (5.4.5) with a 64-bit address pointing to the starting address of the first TRB of the Command Ring.
+  // Note: The Command Ring is 64 byte aligned, so the low order 6 bits of the Command Ring Pointer shall
+  // always be '0'.
+  //
+  CreateTransferRing (Xhc, CMD_RING_TRB_NUMBER, &Xhc->CmdRing);
+  //
+  // The xHC uses the Enqueue Pointer to determine when a Transfer Ring is empty. As it fetches TRBs from a
+  // Transfer Ring it checks for a Cycle bit transition. If a transition detected, the ring is empty.
+  // So we set RCS as inverted PCS init value to let Command Ring empty
+  //
+  CmdRing  = (UINT64)(UINTN)Xhc->CmdRing.RingSeg0;
+  CmdRingPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, (VOID *)(UINTN) CmdRing, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+  ASSERT ((CmdRingPhy & 0x3F) == 0);
+  CmdRingPhy |= XHC_CRCR_RCS;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteOpReg (Xhc, XHC_CRCR_OFFSET, XHC_LOW_32BIT(CmdRingPhy));
+  XhcWriteOpReg (Xhc, XHC_CRCR_OFFSET + 4, XHC_HIGH_32BIT (CmdRingPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcInitSched:XHC_CRCR=0x%x\n", Xhc->CmdRing.RingSeg0));
+
+  //
+  // Disable the 'interrupter enable' bit in USB_CMD
+  // and clear IE & IP bit in all Interrupter X Management Registers.
+  //
+  XhcClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_INTE);
+  for (Index = 0; Index < (UINT16)(Xhc->HcSParams1.Data.MaxIntrs); Index++) {
+    XhcClearRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IE);
+    XhcSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IP);
+  }
+
+  //
+  // Allocate EventRing for Cmd, Ctrl, Bulk, Interrupt, AsynInterrupt transfer
+  //
+  CreateEventRing (Xhc, &Xhc->EventRing);
+  DEBUG ((EFI_D_INFO, "XhcInitSched:XHC_EVENTRING=0x%x\n", Xhc->EventRing.EventRingSeg0));
+}
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS           The recovery is successful.
+  @retval Others                Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRecoverHaltedEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  
+  DEBUG ((EFI_D_INFO, "Recovery Halted Slot = %x,Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Reset endpoint command to transit from halt to stop state
+  //
+  Status = XhcResetEndpoint(Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2)Set dequeue pointer
+  //
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3)Ring the doorbell to transit from stop to active
+  //
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  
+  DEBUG ((EFI_D_INFO, "Stop Slot = %x,Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop xHC from executing of the TDs on the endpoint
+  //
+  Status = XhcStopEndpoint(Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2)Set dequeue pointer
+  //
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3)Ring the doorbell to transit from stop to active
+  //
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The created event ring.
+
+**/
+VOID
+CreateEventRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  OUT EVENT_RING            *EventRing
+  )
+{
+  VOID                        *Buf;
+  EVENT_RING_SEG_TABLE_ENTRY  *ERSTBase;
+  UINTN                       Size;
+  EFI_PHYSICAL_ADDRESS        ERSTPhy;
+  EFI_PHYSICAL_ADDRESS        DequeuePhy;
+
+  ASSERT (EventRing != NULL);
+
+  Size = sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  EventRing->EventRingSeg0    = Buf;
+  EventRing->TrbNumber        = EVENT_RING_TRB_NUMBER;
+  EventRing->EventRingDequeue = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  EventRing->EventRingEnqueue = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  
+  DequeuePhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+  
+  //
+  // Software maintains an Event Ring Consumer Cycle State (CCS) bit, initializing it to '1'
+  // and toggling it every time the Event Ring Dequeue Pointer wraps back to the beginning of the Event Ring.
+  //
+  EventRing->EventRingCCS = 1;
+
+  Size = sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  ERSTBase              = (EVENT_RING_SEG_TABLE_ENTRY *) Buf;
+  EventRing->ERSTBase   = ERSTBase;
+  ERSTBase->PtrLo       = XHC_LOW_32BIT (DequeuePhy);
+  ERSTBase->PtrHi       = XHC_HIGH_32BIT (DequeuePhy);
+  ERSTBase->RingTrbSize = EVENT_RING_TRB_NUMBER;
+
+  ERSTPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, ERSTBase, Size);
+
+  //
+  // Program the Interrupter Event Ring Segment Table Size (ERSTSZ) register (5.5.2.3.1)
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTSZ_OFFSET,
+    ERST_NUMBER
+    );
+  //
+  // Program the Interrupter Event Ring Dequeue Pointer (ERDP) register (5.5.2.3.3)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET,
+    XHC_LOW_32BIT((UINT64)(UINTN)DequeuePhy)
+    );
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET + 4,
+    XHC_HIGH_32BIT((UINT64)(UINTN)DequeuePhy)
+    );
+  //
+  // Program the Interrupter Event Ring Segment Table Base Address (ERSTBA) register(5.5.2.3.2)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET,
+    XHC_LOW_32BIT((UINT64)(UINTN)ERSTPhy)
+    );
+  XhcWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET + 4,
+    XHC_HIGH_32BIT((UINT64)(UINTN)ERSTPhy)
+    );
+  //
+  // Need set IMAN IE bit to enble the ring interrupt
+  //
+  XhcSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET, XHC_IMAN_IE);
+}
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Instance.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing           The created transfer ring.
+
+**/
+VOID
+CreateTransferRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  UINTN                 TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  )
+{
+  VOID                  *Buf;
+  LINK_TRB              *EndTrb;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Buf = UsbHcAllocateMem (Xhc->MemPool, sizeof (TRB_TEMPLATE) * TrbNum);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+
+  TransferRing->RingSeg0     = Buf;
+  TransferRing->TrbNumber    = TrbNum;
+  TransferRing->RingEnqueue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingDequeue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingPCS      = 1;
+  //
+  // 4.9.2 Transfer Ring Management
+  // To form a ring (or circular queue) a Link TRB may be inserted at the end of a ring to
+  // point to the first TRB in the ring.
+  //
+  EndTrb        = (LINK_TRB *) ((UINTN)Buf + sizeof (TRB_TEMPLATE) * (TrbNum - 1));
+  EndTrb->Type  = TRB_TYPE_LINK;
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+  EndTrb->PtrLo = XHC_LOW_32BIT (PhyAddr);
+  EndTrb->PtrHi = XHC_HIGH_32BIT (PhyAddr);
+  //
+  // Toggle Cycle (TC). When set to '1', the xHC shall toggle its interpretation of the Cycle bit.
+  //
+  EndTrb->TC    = 1;
+  //
+  // Set Cycle bit as other TRB PCS init value
+  //
+  EndTrb->CycleBit = 0;
+}
+
+/**
+  Free XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The event ring to be freed.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcFreeEventRing (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  EVENT_RING          *EventRing
+)
+{
+  if(EventRing->EventRingSeg0 == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Free EventRing Segment 0
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->EventRingSeg0, sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER);
+
+  //
+  // Free ESRT table
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->ERSTBase, sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER);
+  return EFI_SUCCESS;
+}
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc        The XHCI Instance.
+
+**/
+VOID
+XhcFreeSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  UINT32                  Index;
+  UINT64                  *ScratchEntry;
+  
+  if (Xhc->ScratchBuf != NULL) {
+    ScratchEntry = Xhc->ScratchEntry;
+    for (Index = 0; Index < Xhc->MaxScratchpadBufs; Index++) {
+      //
+      // Free Scratchpad Buffers
+      //
+      UsbHcFreeAlignedPages (Xhc->PciIo, (VOID*)(UINTN)ScratchEntry[Index], EFI_SIZE_TO_PAGES (Xhc->PageSize), (VOID *) Xhc->ScratchEntryMap[Index]);
+    }
+    //
+    // Free Scratchpad Buffer Array
+    //
+    UsbHcFreeAlignedPages (Xhc->PciIo, Xhc->ScratchBuf, EFI_SIZE_TO_PAGES (Xhc->MaxScratchpadBufs * sizeof (UINT64)), Xhc->ScratchMap);
+    FreePool (Xhc->ScratchEntryMap);
+    FreePool (Xhc->ScratchEntry);
+  }
+
+  if (Xhc->CmdRing.RingSeg0 != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+    Xhc->CmdRing.RingSeg0 = NULL;
+  }
+  
+  XhcFreeEventRing (Xhc,&Xhc->EventRing);
+
+  if (Xhc->DCBAA != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->DCBAA, (Xhc->MaxSlotsEn + 1) * sizeof(UINT64));
+    Xhc->DCBAA = NULL;
+  }
+  
+  //
+  // Free memory pool at last
+  //
+  if (Xhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Xhc->MemPool);
+    Xhc->MemPool = NULL;
+  }
+}
+
+/**
+  Check if the Trb is a transaction of the URBs in XHCI's asynchronous transfer list.
+
+  @param Xhc    The XHCI Instance.
+  @param Trb    The TRB to be checked.
+  @param Urb    The pointer to the matched Urb.
+
+  @retval TRUE  The Trb is matched with a transaction of the URBs in the async list.
+  @retval FALSE The Trb is not matched with any URBs in the async list.
+
+**/
+BOOLEAN
+IsAsyncIntTrb (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  TRB_TEMPLATE        *Trb,
+  OUT URB                 **Urb
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  TRB_TEMPLATE            *CheckedTrb;
+  URB                     *CheckedUrb;
+  UINTN                   Index;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    CheckedUrb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    CheckedTrb = CheckedUrb->TrbStart;
+    for (Index = 0; Index < CheckedUrb->TrbNum; Index++) {
+      if (Trb == CheckedTrb) {
+        *Urb = CheckedUrb;
+        return TRUE;
+      }
+      CheckedTrb++;
+      if ((UINTN)CheckedTrb >= ((UINTN) CheckedUrb->Ring->RingSeg0 + sizeof (TRB_TEMPLATE) * CheckedUrb->Ring->TrbNumber)) {
+        CheckedTrb = (TRB_TEMPLATE*) CheckedUrb->Ring->RingSeg0;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check if the Trb is a transaction of the URB.
+
+  @param Trb    The TRB to be checked
+  @param Urb    The transfer ring to be checked.
+
+  @retval TRUE  It is a transaction of the URB.
+  @retval FALSE It is not any transaction of the URB.
+
+**/
+BOOLEAN
+IsTransferRingTrb (
+  IN  TRB_TEMPLATE        *Trb,
+  IN  URB                 *Urb
+  )
+{
+  TRB_TEMPLATE  *CheckedTrb;
+  UINTN         Index;
+
+  CheckedTrb = Urb->Ring->RingSeg0;
+
+  ASSERT (Urb->Ring->TrbNumber == CMD_RING_TRB_NUMBER || Urb->Ring->TrbNumber == TR_RING_TRB_NUMBER);
+
+  for (Index = 0; Index < Urb->Ring->TrbNumber; Index++) {
+    if (Trb == CheckedTrb) {
+      return TRUE;
+    }
+    CheckedTrb++;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Xhc             The XHCI Instance.
+  @param  Urb             The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+XhcCheckUrbResult (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EVT_TRB_TRANSFER        *EvtTrb;
+  TRB_TEMPLATE            *TRBPtr;
+  UINTN                   Index;
+  UINT8                   TRBType;
+  EFI_STATUS              Status;
+  URB                     *AsyncUrb;
+  URB                     *CheckedUrb;
+  UINT64                  XhcDequeue;
+  UINT32                  High;
+  UINT32                  Low;
+  EFI_PHYSICAL_ADDRESS    PhyAddr;
+
+  ASSERT ((Xhc != NULL) && (Urb != NULL));
+
+  Status   = EFI_SUCCESS;
+  AsyncUrb = NULL;
+
+  if (Urb->Finished) {
+    goto EXIT;
+  }
+
+  EvtTrb = NULL;
+
+  if (XhcIsHalt (Xhc) || XhcIsSysError (Xhc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto EXIT;
+  }
+
+  //
+  // Traverse the event ring to find out all new events from the previous check.
+  //
+  XhcSyncEventRing (Xhc, &Xhc->EventRing);
+  for (Index = 0; Index < Xhc->EventRing.TrbNumber; Index++) {
+    Status = XhcCheckNewEvent (Xhc, &Xhc->EventRing, ((TRB_TEMPLATE **)&EvtTrb));
+    if (Status == EFI_NOT_READY) {
+      //
+      // All new events are handled, return directly.
+      //
+      goto EXIT;
+    }
+
+    //
+    // Only handle COMMAND_COMPLETETION_EVENT and TRANSFER_EVENT.
+    //
+    if ((EvtTrb->Type != TRB_TYPE_COMMAND_COMPLT_EVENT) && (EvtTrb->Type != TRB_TYPE_TRANS_EVENT)) {
+      continue;
+    }
+    
+    //
+    // Need convert pci device address to host address
+    //
+    PhyAddr = (EFI_PHYSICAL_ADDRESS)(EvtTrb->TRBPtrLo | LShiftU64 ((UINT64) EvtTrb->TRBPtrHi, 32));
+    TRBPtr = (TRB_TEMPLATE *)(UINTN) UsbHcGetHostAddrForPciAddr (Xhc->MemPool, (VOID *)(UINTN) PhyAddr, sizeof (TRB_TEMPLATE));
+
+    //
+    // Update the status of Urb according to the finished event regardless of whether
+    // the urb is current checked one or in the XHCI's async transfer list.
+    // This way is used to avoid that those completed async transfer events don't get
+    // handled in time and are flushed by newer coming events.
+    //
+    if (IsTransferRingTrb (TRBPtr, Urb)) {
+      CheckedUrb = Urb;
+    } else if (IsAsyncIntTrb (Xhc, TRBPtr, &AsyncUrb)) {    
+      CheckedUrb = AsyncUrb;
+    } else {
+      continue;
+    }
+  
+    switch (EvtTrb->Completecode) {
+      case TRB_COMPLETION_STALL_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_STALL;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: STALL_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_BABBLE_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BABBLE;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: BABBLE_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_DATA_BUFFER_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BUFFER;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: ERR_BUFFER! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_USB_TRANSACTION_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: TRANSACTION_ERROR! Completecode = %x\n",EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_SHORT_PACKET:
+      case TRB_COMPLETION_SUCCESS:
+        if (EvtTrb->Completecode == TRB_COMPLETION_SHORT_PACKET) {
+          DEBUG ((EFI_D_ERROR, "XhcCheckUrbResult: short packet happens!\n"));
+        }
+
+        TRBType = (UINT8) (TRBPtr->Type);
+        if ((TRBType == TRB_TYPE_DATA_STAGE) ||
+            (TRBType == TRB_TYPE_NORMAL) ||
+            (TRBType == TRB_TYPE_ISOCH)) {
+          CheckedUrb->Completed += (((TRANSFER_TRB_NORMAL*)TRBPtr)->Length - EvtTrb->Length);
+        }
+
+        break;
+
+      default:
+        DEBUG ((EFI_D_ERROR, "Transfer Default Error Occur! Completecode = 0x%x!\n",EvtTrb->Completecode));
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        goto EXIT;
+    }
+
+    //
+    // Only check first and end Trb event address
+    //
+    if (TRBPtr == CheckedUrb->TrbStart) {
+      CheckedUrb->StartDone = TRUE;
+    }
+
+    if (TRBPtr == CheckedUrb->TrbEnd) {
+      CheckedUrb->EndDone = TRUE;
+    }
+
+    if (CheckedUrb->StartDone && CheckedUrb->EndDone) {
+      CheckedUrb->Finished = TRUE;
+      CheckedUrb->EvtTrb   = (TRB_TEMPLATE *)EvtTrb;
+    }
+  }
+
+EXIT:
+
+  //
+  // Advance event ring to last available entry
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  Low  = XhcReadRuntimeReg (Xhc, XHC_ERDP_OFFSET);
+  High = XhcReadRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4);
+  XhcDequeue = (UINT64)(LShiftU64((UINT64)High, 32) | Low);
+
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->EventRing.EventRingDequeue, sizeof (TRB_TEMPLATE));
+
+  if ((XhcDequeue & (~0x0F)) != (PhyAddr & (~0x0F))) {
+    //
+    // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+    // So divide it to two 32-bytes width register access.
+    //
+    XhcWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET, XHC_LOW_32BIT (PhyAddr) | BIT3);
+    XhcWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4, XHC_HIGH_32BIT (PhyAddr));
+  }
+
+  return Urb->Finished;
+}
+
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI Instance.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcExecTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  BOOLEAN             CmdTransfer,
+  IN  URB                 *Urb,
+  IN  UINTN               Timeout
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINT64                  Loop;
+  UINT8                   SlotId;
+  UINT8                   Dci;
+  BOOLEAN                 Finished;
+
+  if (CmdTransfer) {
+    SlotId = 0;
+    Dci    = 0;
+  } else {
+    SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      return EFI_DEVICE_ERROR;
+    }
+    Dci  = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+    ASSERT (Dci < 32);
+  }
+
+  Status = EFI_SUCCESS;
+  Loop   = Timeout * XHC_1_MILLISECOND;
+  if (Timeout == 0) {
+    Loop = 0xFFFFFFFF;
+  }
+
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+  for (Index = 0; Index < Loop; Index++) {
+    Finished = XhcCheckUrbResult (Xhc, Urb);
+    if (Finished) {
+      break;
+    }
+    gBS->Stall (XHC_1_MICROSECOND);
+  }
+
+  if (Index == Loop) {
+    Urb->Result = EFI_USB_ERR_TIMEOUT;
+    Status      = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status      = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  BusAddr               The logical device address assigned by UsbBus driver.
+  @param  EpNum                 The endpoint of the target.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+XhciDelAsyncIntTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT8               BusAddr,
+  IN  UINT8               EpNum
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+  EFI_USB_DATA_DIRECTION  Direction;
+
+  Direction = ((EpNum & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  EpNum    &= 0x0F;
+
+  Urb = NULL;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    if ((Urb->Ep.BusAddr == BusAddr) &&
+        (Urb->Ep.EpAddr == EpNum) &&
+        (Urb->Ep.Direction == Direction)) {
+      RemoveEntryList (&Urb->UrbList);
+      FreePool (Urb->Data);
+      XhcFreeUrb (Xhc, Urb);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Remove all the asynchronous interrutp transfers.
+
+  @param  Xhc    The XHCI Instance.
+
+**/
+VOID
+XhciDelAllAsyncIntTransfers (
+  IN USB_XHCI_INSTANCE    *Xhc
+  )
+{
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  URB                     *Urb;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+    RemoveEntryList (&Urb->UrbList);
+    FreePool (Urb->Data);
+    XhcFreeUrb (Xhc, Urb);
+  }
+}
+
+/**
+  Update the queue head for next round of asynchronous transfer
+
+  @param  Xhc     The XHCI Instance.
+  @param  Urb     The URB to update
+
+**/
+VOID
+XhcUpdateAsyncRequest (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN URB                      *Urb
+  )
+{
+  EFI_STATUS    Status;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = XhcCreateTransferTrb (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+    Status = RingIntTransferDoorBell (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+  }
+}
+
+/**
+  Flush data from PCI controller specific address to mapped system
+  memory address.
+
+  @param  Xhc                The XHCI device.
+  @param  Urb                The URB to unmap.
+
+  @retval EFI_SUCCESS        Success to flush data to mapped system memory.
+  @retval EFI_DEVICE_ERROR   Fail to flush data to mapped system memory.
+
+**/
+EFI_STATUS
+XhcFlushAsyncIntMap (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  EFI_PCI_IO_PROTOCOL_OPERATION MapOp;
+  EFI_PCI_IO_PROTOCOL           *PciIo;
+  UINTN                         Len;
+  VOID                          *Map;
+
+  PciIo = Xhc->PciIo;
+  Len   = Urb->DataLen;
+
+  if (Urb->Ep.Direction == EfiUsbDataIn) {
+    MapOp = EfiPciIoOperationBusMasterWrite;
+  } else {
+    MapOp = EfiPciIoOperationBusMasterRead;
+  }
+
+  if (Urb->DataMap != NULL) {
+    Status = PciIo->Unmap (PciIo, Urb->DataMap);
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+  }
+
+  Urb->DataMap = NULL;
+
+  Status = PciIo->Map (PciIo, MapOp, Urb->Data, &Len, &PhyAddr, &Map);
+  if (EFI_ERROR (Status) || (Len != Urb->DataLen)) {
+    goto ON_ERROR;
+  }
+
+  Urb->DataPhy  = (VOID *) ((UINTN) PhyAddr);
+  Urb->DataMap  = Map;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB_XHCI_INSTANCE.
+
+**/
+VOID
+EFIAPI
+XhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_XHCI_INSTANCE       *Xhc;
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *Next;
+  UINT8                   *ProcBuf;
+  URB                     *Urb;
+  UINT8                   SlotId;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (XHC_TPL);
+
+  Xhc    = (USB_XHCI_INSTANCE*) Context;
+
+  EFI_LIST_FOR_EACH_SAFE (Entry, Next, &Xhc->AsyncIntTransfers) {
+    Urb = EFI_LIST_CONTAINER (Entry, URB, UrbList);
+
+    //
+    // Make sure that the device is available before every check.
+    //
+    SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      continue;
+    }
+
+    //
+    // Check the result of URB execution. If it is still
+    // active, check the next one.
+    //
+    XhcCheckUrbResult (Xhc, Urb);
+
+    if (!Urb->Finished) {
+      continue;
+    }
+
+    //
+    // Flush any PCI posted write transactions from a PCI host
+    // bridge to system memory.
+    //
+    Status = XhcFlushAsyncIntMap (Xhc, Urb);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcMonitorAsyncRequests: Fail to Flush AsyncInt Mapped Memeory\n"));
+    }
+
+    //
+    // Allocate a buffer then copy the transferred data for user.
+    // If failed to allocate the buffer, update the URB for next
+    // round of transfer. Ignore the data of this round.
+    //
+    ProcBuf = NULL;
+    if (Urb->Result == EFI_USB_NOERROR) {
+      ASSERT (Urb->Completed <= Urb->DataLen);
+
+      ProcBuf = AllocateZeroPool (Urb->Completed);
+
+      if (ProcBuf == NULL) {
+        XhcUpdateAsyncRequest (Xhc, Urb);
+        continue;
+      }
+
+      CopyMem (ProcBuf, Urb->Data, Urb->Completed);
+    }
+
+    //
+    // Leave error recovery to its related device driver. A
+    // common case of the error recovery is to re-submit the
+    // interrupt transfer which is linked to the head of the
+    // list. This function scans from head to tail. So the
+    // re-submitted interrupt transfer's callback function
+    // will not be called again in this round. Don't touch this
+    // URB after the callback, it may have been removed by the
+    // callback.
+    //
+    if (Urb->Callback != NULL) {
+      //
+      // Restore the old TPL, USB bus maybe connect device in
+      // his callback. Some drivers may has a lower TPL restriction.
+      //
+      gBS->RestoreTPL (OldTpl);
+      (Urb->Callback) (ProcBuf, Urb->Completed, Urb->Context, Urb->Result);
+      OldTpl = gBS->RaiseTPL (XHC_TPL);
+    }
+
+    if (ProcBuf != NULL) {
+      gBS->FreePool (ProcBuf);
+    }
+
+    XhcUpdateAsyncRequest (Xhc, Urb);
+  }
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  ParentRouteChart      The route string pointed to the parent device if it exists.
+  @param  Port                  The port to be polled.
+  @param  PortState             The port state.
+
+  @retval EFI_SUCCESS           Successfully enable/disable device slot according to port state.
+  @retval Others                Should not appear.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPollPortStatusChange (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  USB_DEV_ROUTE         ParentRouteChart,
+  IN  UINT8                 Port,
+  IN  EFI_USB_PORT_STATUS   *PortState
+  )
+{
+  EFI_STATUS        Status;
+  UINT8             Speed;
+  UINT8             SlotId;
+  USB_DEV_ROUTE     RouteChart;
+
+  Status = EFI_SUCCESS;
+
+  if ((PortState->PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (ParentRouteChart.Dword == 0) {
+    RouteChart.Route.RouteString = 0;
+    RouteChart.Route.RootPortNum = Port + 1;
+    RouteChart.Route.TierNum     = 1;
+  } else {
+    if(Port < 14) {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (Port << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    } else {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (15 << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    }
+    RouteChart.Route.RootPortNum   = ParentRouteChart.Route.RootPortNum;
+    RouteChart.Route.TierNum       = ParentRouteChart.Route.TierNum + 1;
+  }
+
+  SlotId = XhcRouteStringToSlotId (Xhc, RouteChart);
+  if (SlotId != 0) {
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      Status = XhcDisableSlotCmd (Xhc, SlotId);
+    } else {
+      Status = XhcDisableSlotCmd64 (Xhc, SlotId);
+    }
+  }
+
+  if (((PortState->PortStatus & USB_PORT_STAT_ENABLE) != 0) &&
+      ((PortState->PortStatus & USB_PORT_STAT_CONNECTION) != 0)) {
+    //
+    // Has a device attached, Identify device speed after port is enabled.
+    //
+    Speed = EFI_USB_SPEED_FULL;
+    if ((PortState->PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_LOW;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_HIGH;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_SUPER;
+    }
+    //
+    // Execute Enable_Slot cmd for attached device, initialize device context and assign device address.
+    //
+    SlotId = XhcRouteStringToSlotId (Xhc, RouteChart);
+    if ((SlotId == 0) && ((PortState->PortChangeStatus & USB_PORT_STAT_C_RESET) != 0)) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcInitializeDeviceSlot (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      } else {
+        Status = XhcInitializeDeviceSlot64 (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      }
+    }
+  } 
+
+  return Status;
+}
+
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr              The target endpoint number.
+  @param  Direction           The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcEndpointToDci (
+  IN  UINT8                   EpAddr,
+  IN  UINT8                   Direction
+  )
+{
+  UINT8 Index;
+
+  if (EpAddr == 0) {
+    return 1;
+  } else {
+    Index = (UINT8) (2 * EpAddr);
+    if (Direction == EfiUsbDataIn) {
+      Index += 1;
+    }
+    return Index;
+  }
+}
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc             The XHCI Instance.
+  @param  BusDevAddr      The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+EFIAPI
+XhcBusDevAddrToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  UINT8              BusDevAddr
+  )
+{
+  UINT8  Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].BusDevAddr == BusDevAddr)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc             The XHCI Instance.
+  @param  RouteString     The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+EFIAPI
+XhcRouteStringToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  USB_DEV_ROUTE      RouteString
+  )
+{
+  UINT8  Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].RouteString.Dword == RouteString.Dword)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  EvtRing     The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncEventRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN EVENT_RING           *EvtRing
+  )
+{
+  UINTN               Index;
+  TRB_TEMPLATE        *EvtTrb1;
+
+  ASSERT (EvtRing != NULL);
+
+  //
+  // Calculate the EventRingEnqueue and EventRingCCS.
+  // Note: only support single Segment
+  //
+  EvtTrb1 = EvtRing->EventRingDequeue;
+
+  for (Index = 0; Index < EvtRing->TrbNumber; Index++) {
+    if (EvtTrb1->CycleBit != EvtRing->EventRingCCS) {
+      break;
+    }
+
+    EvtTrb1++;
+
+    if ((UINTN)EvtTrb1 >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+      EvtTrb1 = EvtRing->EventRingSeg0;
+      EvtRing->EventRingCCS = (EvtRing->EventRingCCS) ? 0 : 1;
+    }
+  }
+
+  if (Index < EvtRing->TrbNumber) {
+    EvtRing->EventRingEnqueue = EvtTrb1;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  TrsRing     The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncTrsRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN TRANSFER_RING        *TrsRing
+  )
+{
+  UINTN               Index;
+  TRB_TEMPLATE        *TrsTrb;
+
+  ASSERT (TrsRing != NULL);
+  //
+  // Calculate the latest RingEnqueue and RingPCS
+  //
+  TrsTrb = TrsRing->RingEnqueue;
+  ASSERT (TrsTrb != NULL);
+
+  for (Index = 0; Index < TrsRing->TrbNumber; Index++) {
+    if (TrsTrb->CycleBit != (TrsRing->RingPCS & BIT0)) {
+      break;
+    }
+    TrsTrb++;
+    if ((UINT8) TrsTrb->Type == TRB_TYPE_LINK) {
+      ASSERT (((LINK_TRB*)TrsTrb)->TC != 0);
+      //
+      // set cycle bit in Link TRB as normal
+      //
+      ((LINK_TRB*)TrsTrb)->CycleBit = TrsRing->RingPCS & BIT0;
+      //
+      // Toggle PCS maintained by software
+      //
+      TrsRing->RingPCS = (TrsRing->RingPCS & BIT0) ? 0 : 1;
+      TrsTrb = (TRB_TEMPLATE *) TrsRing->RingSeg0;  // Use host address
+    }
+  }
+
+  ASSERT (Index != TrsRing->TrbNumber);
+
+  if (TrsTrb != TrsRing->RingEnqueue) {
+    TrsRing->RingEnqueue = TrsTrb;
+  }
+
+  //
+  // Clear the Trb context for enqueue, but reserve the PCS bit
+  //
+  TrsTrb->Parameter1 = 0;
+  TrsTrb->Parameter2 = 0;
+  TrsTrb->Status     = 0;
+  TrsTrb->RsvdZ1     = 0;
+  TrsTrb->Type       = 0;
+  TrsTrb->Control    = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI Instance.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCheckNewEvent (
+  IN  USB_XHCI_INSTANCE       *Xhc,
+  IN  EVENT_RING              *EvtRing,
+  OUT TRB_TEMPLATE            **NewEvtTrb
+  )
+{
+  ASSERT (EvtRing != NULL);
+
+  *NewEvtTrb = EvtRing->EventRingDequeue;
+
+  if (EvtRing->EventRingDequeue == EvtRing->EventRingEnqueue) {
+    return EFI_NOT_READY;
+  }
+
+  EvtRing->EventRingDequeue++;
+  //
+  // If the dequeue pointer is beyond the ring, then roll-back it to the begining of the ring.
+  //
+  if ((UINTN)EvtRing->EventRingDequeue >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+    EvtRing->EventRingDequeue = EvtRing->EventRingSeg0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRingDoorBell (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                SlotId,
+  IN UINT8                Dci
+  )
+{
+  if (SlotId == 0) {
+    XhcWriteDoorBellReg (Xhc, 0, 0);
+  } else {
+    XhcWriteDoorBellReg (Xhc, SlotId * sizeof (UINT32), Dci);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed through URB.
+
+  @param  Xhc           The XHCI Instance.
+  @param  Urb           The URB to be rung.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+RingIntTransferDoorBell (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  UINT8                SlotId;
+  UINT8                Dci;
+
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  Dci    = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+  return EFI_SUCCESS;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  TRANSFER_RING               *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE      CmdTrbAddr;
+  UINT8                       DeviceAddress;
+  CMD_TRB_ENABLE_SLOT         CmdTrb;
+  UINT8                       SlotId;
+  UINT8                       ParentSlotId;
+  DEVICE_CONTEXT              *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcInitializeDeviceSlot: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcRouteStringToSlotId(Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT *)Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) ((DEVICE_CONTEXT *) OutputContext)->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "    Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  return Status;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot64 (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  TRANSFER_RING               *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE      CmdTrbAddr;
+  UINT8                       DeviceAddress;
+  CMD_TRB_ENABLE_SLOT         CmdTrb;
+  UINT8                       SlotId;
+  UINT8                       ParentSlotId;
+  DEVICE_CONTEXT_64           *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcInitializeDeviceSlot64: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT_64));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcRouteStringToSlotId(Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT_64 *)Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT_64));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT_64));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) ((DEVICE_CONTEXT_64 *) OutputContext)->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "    Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+  return Status;
+}
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd (
+  IN USB_XHCI_INSTANCE         *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting != NULL) {
+    FreePool (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting);
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  return Status;
+}
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd64 (
+  IN USB_XHCI_INSTANCE         *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting != NULL) {
+    FreePool (Xhc->UsbDevContext[SlotId].ActiveAlternateSetting);
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+     UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT_64));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  return Status;
+}
+
+/**
+  Initialize endpoint context in input context.
+
+  @param Xhc            The XHCI Instance.
+  @param SlotId         The slot id to be configured.
+  @param DeviceSpeed    The device's speed.
+  @param InputContext   The pointer to the input context.
+  @param IfDesc         The pointer to the usb device interface descriptor.
+
+  @return The maximum device context index of endpoint.
+
+**/
+UINT8
+EFIAPI
+XhcInitializeEndpointContext (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN INPUT_CONTEXT              *InputContext,
+  IN USB_INTERFACE_DESCRIPTOR   *IfDesc
+  )
+{
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  UINT8                         Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+  TRANSFER_RING                 *EndpointTransferRing;
+
+  MaxDci = 0;
+
+  NumEp = IfDesc->NumEndpoints;
+
+  EpDesc = (USB_ENDPOINT_DESCRIPTOR *)(IfDesc + 1);
+  for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+    while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    EpAddr    = (UINT8)(EpDesc->EndpointAddress & 0x0F);
+    Direction = (UINT8)((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+    Dci = XhcEndpointToDci (EpAddr, Direction);
+    ASSERT (Dci < 32);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+    InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+      //
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    } else {
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    }
+
+    switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+      case USB_ENDPOINT_BULK:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+        }
+
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+
+        break;
+      case USB_ENDPOINT_ISO:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+        }
+        //
+        // Do not support isochronous transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+      case USB_ENDPOINT_INTERRUPT:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+        }
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+        //
+        // Get the bInterval from descriptor and init the the interval field of endpoint context
+        //
+        if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+          Interval = EpDesc->Interval;
+          //
+          // Calculate through the bInterval field of Endpoint descriptor.
+          //
+          ASSERT (Interval != 0);
+          InputContext->EP[Dci-1].Interval = (UINT32)HighBitSet32((UINT32)Interval) + 3;
+        } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+          Interval = EpDesc->Interval;
+          ASSERT (Interval >= 1 && Interval <= 16);
+          //
+          // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+          //
+          InputContext->EP[Dci-1].Interval         = Interval - 1;
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = 0x0002;
+          InputContext->EP[Dci-1].MaxBurstSize     = 0x0;
+          InputContext->EP[Dci-1].CErr             = 3;
+        }
+
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+        break;
+
+      case USB_ENDPOINT_CONTROL:
+        //
+        // Do not support control transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+      default:
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext: Unknown EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+    }
+
+    PhyAddr = UsbHcGetPciAddrForHostAddr (
+                Xhc->MemPool,
+                ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                );
+    PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+    PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+    InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+    InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+  }
+
+  return MaxDci;
+}
+
+/**
+  Initialize endpoint context in input context.
+
+  @param Xhc            The XHCI Instance.
+  @param SlotId         The slot id to be configured.
+  @param DeviceSpeed    The device's speed.
+  @param InputContext   The pointer to the input context.
+  @param IfDesc         The pointer to the usb device interface descriptor.
+
+  @return The maximum device context index of endpoint.
+
+**/
+UINT8
+EFIAPI
+XhcInitializeEndpointContext64 (
+  IN USB_XHCI_INSTANCE          *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN INPUT_CONTEXT_64           *InputContext,
+  IN USB_INTERFACE_DESCRIPTOR   *IfDesc
+  )
+{
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  UINT8                         Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+  TRANSFER_RING                 *EndpointTransferRing;
+
+  MaxDci = 0;
+
+  NumEp = IfDesc->NumEndpoints;
+
+  EpDesc = (USB_ENDPOINT_DESCRIPTOR *)(IfDesc + 1);
+  for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+    while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    EpAddr    = (UINT8)(EpDesc->EndpointAddress & 0x0F);
+    Direction = (UINT8)((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+    Dci = XhcEndpointToDci (EpAddr, Direction);
+    ASSERT (Dci < 32);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+    InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+      //
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    } else {
+      InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+    }
+
+    switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+      case USB_ENDPOINT_BULK:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+        }
+
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+
+        break;
+      case USB_ENDPOINT_ISO:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 0;
+          InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+        }
+        //
+        // Do not support isochronous transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+      case USB_ENDPOINT_INTERRUPT:
+        if (Direction == EfiUsbDataIn) {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+        } else {
+          InputContext->EP[Dci-1].CErr   = 3;
+          InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+        }
+        InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+        InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+        //
+        // Get the bInterval from descriptor and init the the interval field of endpoint context
+        //
+        if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+          Interval = EpDesc->Interval;
+          //
+          // Calculate through the bInterval field of Endpoint descriptor.
+          //
+          ASSERT (Interval != 0);
+          InputContext->EP[Dci-1].Interval = (UINT32)HighBitSet32((UINT32)Interval) + 3;
+        } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+          Interval = EpDesc->Interval;
+          ASSERT (Interval >= 1 && Interval <= 16);
+          //
+          // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+          //
+          InputContext->EP[Dci-1].Interval         = Interval - 1;
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = 0x0002;
+          InputContext->EP[Dci-1].MaxBurstSize     = 0x0;
+          InputContext->EP[Dci-1].CErr             = 3;
+        }
+
+        if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+          EndpointTransferRing = AllocateZeroPool(sizeof (TRANSFER_RING));
+          Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+          CreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+        }
+        break;
+
+      case USB_ENDPOINT_CONTROL:
+        //
+        // Do not support control transfer now.
+        //
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+      default:
+        DEBUG ((EFI_D_INFO, "XhcInitializeEndpointContext64: Unknown EP found, Transfer ring is not allocated.\n"));
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+        continue;
+    }
+
+    PhyAddr = UsbHcGetPciAddrForHostAddr (
+                Xhc->MemPool,
+                ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                );
+    PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+    PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+    InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+    InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+  }
+
+  return MaxDci;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  UINT8                       Index;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+    }
+
+    Dci = XhcInitializeEndpointContext (Xhc, SlotId, DeviceSpeed, InputContext, IfDesc);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Endpoint\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd: Config Endpoint Failed, Status = %r\n", Status));
+  } else {
+    Xhc->UsbDevContext[SlotId].ActiveConfiguration = ConfigDesc->ConfigurationValue;
+  }
+
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  UINT8                       Index;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+    }
+
+    Dci = XhcInitializeEndpointContext64 (Xhc, SlotId, DeviceSpeed, InputContext, IfDesc);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+ 
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr  = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Endpoint\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd64: Config Endpoint Failed, Status = %r\n", Status));
+  } else {
+    Xhc->UsbDevContext[SlotId].ActiveConfiguration = ConfigDesc->ConfigurationValue;
+  }
+
+  return Status;
+}
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Stop endpoint successfully.
+  @retval Others                Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcStopEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  CMD_TRB_STOP_ENDPOINT         CmdTrbStopED;
+
+  DEBUG ((EFI_D_INFO, "XhcStopEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbStopED, sizeof (CmdTrbStopED));
+  CmdTrbStopED.CycleBit = 1;
+  CmdTrbStopED.Type     = TRB_TYPE_STOP_ENDPOINT;
+  CmdTrbStopED.EDID     = Dci;
+  CmdTrbStopED.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbStopED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcStopEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDescActive;
+  USB_INTERFACE_DESCRIPTOR    *IfDescSet;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR     *EpDesc;
+  UINTN                       NumEp;
+  UINTN                       EpIndex;
+  UINT8                       EpAddr;
+  UINT8                       Direction;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+  VOID                        *RingSeg;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+
+  Status = EFI_SUCCESS;
+
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 1) If an endpoint is not modified by the Alternate Interface setting, then software shall set the Drop
+  // Context and Add Context flags to '0'.
+  //
+  // Except the interface indicated by Reqeust->Index, no impact to other interfaces.
+  // So the default Drop Context and Add Context flags can be '0' to cover 1).
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDescActive = NULL;
+  IfDescSet = NULL;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  while ((UINTN) IfDesc < ((UINTN) ConfigDesc + ConfigDesc->TotalLength)) {
+    if (IfDesc->DescriptorType == USB_DESC_TYPE_INTERFACE) {
+      if (IfDesc->InterfaceNumber == (UINT8) Request->Index) {
+        if (IfDesc->AlternateSetting == Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[IfDesc->InterfaceNumber]) {
+          //
+          // Find out the active interface descriptor.
+          //
+          IfDescActive = IfDesc;
+        } else if (IfDesc->AlternateSetting == (UINT8) Request->Value) {
+          //
+          // Find out the interface descriptor to set.
+          //
+          IfDescSet = IfDesc;
+        }
+      }
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 2) If an endpoint previously disabled, is enabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '0' and Add Context flag to '1', and initialize the Input Endpoint Context.
+  // 3) If an endpoint previously enabled, is disabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '1' and Add Context flag to '0'.
+  // 4) If a parameter of an enabled endpoint is modified by an Alternate Interface setting, the Drop Context
+  // and Add Context flags shall be set to '1'.
+  //
+  // Below codes are to cover 2), 3) and 4).
+  //
+
+  if ((IfDescActive != NULL) && (IfDescSet != NULL)) {
+    NumEp = IfDescActive->NumEndpoints;
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDescActive + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 1) Stop any Running Transfer Rings affected by the Alternate Interface setting.
+      //
+      Status = XhcStopEndpoint (Xhc, SlotId, Dci);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 2) Free Transfer Rings of all endpoints that will be affected by the Alternate Interface setting.
+      //
+      if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] != NULL) {
+        RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1])->RingSeg0;
+        if (RingSeg != NULL) {
+          UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+        }
+        FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1]);
+        Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] = NULL;
+      }
+
+      //
+      // Set the Drop Context flag to '1'.
+      //
+      InputContext->InputControlContext.Dword1 |= (BIT0 << Dci);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 3) Clear all the Endpoint Context fields of each endpoint that will be disabled by the Alternate
+    // Interface setting, to '0'.
+    //
+    // The step 3) has been covered by the ZeroMem () to InputContext at the start of the function.
+    //
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 4) For each endpoint enabled by the Configure Endpoint Command:
+    //   a. Allocate a Transfer Ring.
+    //   b. Initialize the Transfer Ring Segment(s) by clearing all fields of all TRBs to '0'.
+    //   c. Initialize the Endpoint Context data structure.
+    //
+    Dci = XhcInitializeEndpointContext (Xhc, SlotId, DeviceSpeed, InputContext, IfDescSet);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= BIT0;
+    InputContext->Slot.ContextEntries         = MaxDci;
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 5) Issue and successfully complete a Configure Endpoint Command.
+    //
+    ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+    PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+    CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+    CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+    CmdTrbCfgEP.CycleBit = 1;
+    CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+    CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+    DEBUG ((EFI_D_INFO, "SetInterface: Configure Endpoint\n"));
+    Status = XhcCmdTransfer (
+               Xhc,
+               (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+               XHC_GENERIC_TIMEOUT,
+               (TRB_TEMPLATE **) (UINTN) &EvtTrb
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetInterface: Config Endpoint Failed, Status = %r\n", Status));
+    } else {
+      //
+      // Update the active AlternateSetting.
+      //
+      Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] = (UINT8) Request->Value;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  )
+{
+  EFI_STATUS                  Status;
+  USB_INTERFACE_DESCRIPTOR    *IfDescActive;
+  USB_INTERFACE_DESCRIPTOR    *IfDescSet;
+  USB_INTERFACE_DESCRIPTOR    *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR     *EpDesc;
+  UINTN                       NumEp;
+  UINTN                       EpIndex;
+  UINT8                       EpAddr;
+  UINT8                       Direction;
+  UINT8                       Dci;
+  UINT8                       MaxDci;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+  VOID                        *RingSeg;
+
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+
+  Status = EFI_SUCCESS;
+
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 1) If an endpoint is not modified by the Alternate Interface setting, then software shall set the Drop
+  // Context and Add Context flags to '0'.
+  //
+  // Except the interface indicated by Reqeust->Index, no impact to other interfaces.
+  // So the default Drop Context and Add Context flags can be '0' to cover 1).
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDescActive = NULL;
+  IfDescSet = NULL;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *)(ConfigDesc + 1);
+  while ((UINTN) IfDesc < ((UINTN) ConfigDesc + ConfigDesc->TotalLength)) {
+    if (IfDesc->DescriptorType == USB_DESC_TYPE_INTERFACE) {
+      if (IfDesc->InterfaceNumber == (UINT8) Request->Index) {
+        if (IfDesc->AlternateSetting == Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[IfDesc->InterfaceNumber]) {
+          //
+          // Find out the active interface descriptor.
+          //
+          IfDescActive = IfDesc;
+        } else if (IfDesc->AlternateSetting == (UINT8) Request->Value) {
+          //
+          // Find out the interface descriptor to set.
+          //
+          IfDescSet = IfDesc;
+        }
+      }
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *)((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  //
+  // XHCI 4.6.6 Configure Endpoint
+  // When this command is used to "Set an Alternate Interface on a device", software shall set the Drop
+  // Context and Add Context flags as follows:
+  // 2) If an endpoint previously disabled, is enabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '0' and Add Context flag to '1', and initialize the Input Endpoint Context.
+  // 3) If an endpoint previously enabled, is disabled by the Alternate Interface setting, then software shall set
+  // the Drop Context flag to '1' and Add Context flag to '0'.
+  // 4) If a parameter of an enabled endpoint is modified by an Alternate Interface setting, the Drop Context
+  // and Add Context flags shall be set to '1'.
+  //
+  // Below codes are to cover 2), 3) and 4).
+  //
+
+  if ((IfDescActive != NULL) && (IfDescSet != NULL)) {
+    NumEp = IfDescActive->NumEndpoints;
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDescActive + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 1) Stop any Running Transfer Rings affected by the Alternate Interface setting.
+      //
+      Status = XhcStopEndpoint (Xhc, SlotId, Dci);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      //
+      // XHCI 4.3.6 - Setting Alternate Interfaces
+      // 2) Free Transfer Rings of all endpoints that will be affected by the Alternate Interface setting.
+      //
+      if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] != NULL) {
+        RingSeg = ((TRANSFER_RING *)(UINTN)Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1])->RingSeg0;
+        if (RingSeg != NULL) {
+          UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+        }
+        FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1]);
+        Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci - 1] = NULL;
+      }
+
+      //
+      // Set the Drop Context flag to '1'.
+      //
+      InputContext->InputControlContext.Dword1 |= (BIT0 << Dci);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+    }
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 3) Clear all the Endpoint Context fields of each endpoint that will be disabled by the Alternate
+    // Interface setting, to '0'.
+    //
+    // The step 3) has been covered by the ZeroMem () to InputContext at the start of the function.
+    //
+
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 4) For each endpoint enabled by the Configure Endpoint Command:
+    //   a. Allocate a Transfer Ring.
+    //   b. Initialize the Transfer Ring Segment(s) by clearing all fields of all TRBs to '0'.
+    //   c. Initialize the Endpoint Context data structure.
+    //
+    Dci = XhcInitializeEndpointContext64 (Xhc, SlotId, DeviceSpeed, InputContext, IfDescSet);
+    if (Dci > MaxDci) {
+      MaxDci = Dci;
+    }
+
+    InputContext->InputControlContext.Dword2 |= BIT0;
+    InputContext->Slot.ContextEntries         = MaxDci;
+    //
+    // XHCI 4.3.6 - Setting Alternate Interfaces
+    // 5) Issue and successfully complete a Configure Endpoint Command.
+    //
+    ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+    PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+    CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+    CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+    CmdTrbCfgEP.CycleBit = 1;
+    CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+    CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+    DEBUG ((EFI_D_INFO, "SetInterface64: Configure Endpoint\n"));
+    Status = XhcCmdTransfer (
+               Xhc,
+               (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+               XHC_GENERIC_TIMEOUT,
+               (TRB_TEMPLATE **) (UINTN) &EvtTrb
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SetInterface64: Config Endpoint Failed, Status = %r\n", Status));
+    } else {
+      //
+      // Update the active AlternateSetting.
+      //
+      Xhc->UsbDevContext[SlotId].ActiveAlternateSetting[(UINT8) Request->Index] = (UINT8) Request->Value;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  )
+{
+  EFI_STATUS                  Status;
+  CMD_TRB_EVALUATE_CONTEXT    CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Evaluate context\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  )
+{
+  EFI_STATUS                  Status;
+  CMD_TRB_EVALUATE_CONTEXT    CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Evaluate context\n"));
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext64: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT               *InputContext;
+  DEVICE_CONTEXT              *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  INPUT_CONTEXT_64            *InputContext;
+  DEVICE_CONTEXT_64           *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT     CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT_64));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcCmdTransfer (
+              Xhc,
+              (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+              XHC_GENERIC_TIMEOUT,
+              (TRB_TEMPLATE **) (UINTN) &EvtTrb
+              );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h
new file mode 100644
index 0000000000..931c7efa0c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h
@@ -0,0 +1,1461 @@
+/** @file
+
+  This file contains the definition for XHCI host controller schedule routines.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+#ifndef _EFI_XHCI_SCHED_H_
+#define _EFI_XHCI_SCHED_H_
+
+#define XHC_URB_SIG      SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define XHC_CTRL_TRANSFER                     0x01
+#define XHC_BULK_TRANSFER                     0x02
+#define XHC_INT_TRANSFER_SYNC                 0x04
+#define XHC_INT_TRANSFER_ASYNC                0x08
+#define XHC_INT_ONLY_TRANSFER_ASYNC           0x10
+
+//
+// 6.4.6 TRB Types
+//
+#define TRB_TYPE_NORMAL                       1
+#define TRB_TYPE_SETUP_STAGE                  2
+#define TRB_TYPE_DATA_STAGE                   3
+#define TRB_TYPE_STATUS_STAGE                 4
+#define TRB_TYPE_ISOCH                        5
+#define TRB_TYPE_LINK                         6
+#define TRB_TYPE_EVENT_DATA                   7
+#define TRB_TYPE_NO_OP                        8
+#define TRB_TYPE_EN_SLOT                      9
+#define TRB_TYPE_DIS_SLOT                     10
+#define TRB_TYPE_ADDRESS_DEV                  11
+#define TRB_TYPE_CON_ENDPOINT                 12
+#define TRB_TYPE_EVALU_CONTXT                 13
+#define TRB_TYPE_RESET_ENDPOINT               14
+#define TRB_TYPE_STOP_ENDPOINT                15
+#define TRB_TYPE_SET_TR_DEQUE                 16
+#define TRB_TYPE_RESET_DEV                    17
+#define TRB_TYPE_GET_PORT_BANW                21
+#define TRB_TYPE_FORCE_HEADER                 22
+#define TRB_TYPE_NO_OP_COMMAND                23
+#define TRB_TYPE_TRANS_EVENT                  32
+#define TRB_TYPE_COMMAND_COMPLT_EVENT         33
+#define TRB_TYPE_PORT_STATUS_CHANGE_EVENT     34
+#define TRB_TYPE_HOST_CONTROLLER_EVENT        37
+#define TRB_TYPE_DEVICE_NOTIFI_EVENT          38
+#define TRB_TYPE_MFINDEX_WRAP_EVENT           39
+
+//
+// Endpoint Type (EP Type).
+//
+#define ED_NOT_VALID                          0
+#define ED_ISOCH_OUT                          1
+#define ED_BULK_OUT                           2
+#define ED_INTERRUPT_OUT                      3
+#define ED_CONTROL_BIDIR                      4
+#define ED_ISOCH_IN                           5
+#define ED_BULK_IN                            6
+#define ED_INTERRUPT_IN                       7
+
+//
+// 6.4.5 TRB Completion Codes
+//
+#define TRB_COMPLETION_INVALID                0
+#define TRB_COMPLETION_SUCCESS                1
+#define TRB_COMPLETION_DATA_BUFFER_ERROR      2
+#define TRB_COMPLETION_BABBLE_ERROR           3
+#define TRB_COMPLETION_USB_TRANSACTION_ERROR  4
+#define TRB_COMPLETION_TRB_ERROR              5
+#define TRB_COMPLETION_STALL_ERROR            6
+#define TRB_COMPLETION_SHORT_PACKET           13
+
+//
+// The topology string used to present usb device location
+//
+typedef struct _USB_DEV_TOPOLOGY {
+  //
+  // The tier concatenation of down stream port.
+  //
+  UINT32 RouteString:20;
+  //
+  // The root port number of the chain.
+  //
+  UINT32 RootPortNum:8;
+  //
+  // The Tier the device reside.
+  //
+  UINT32 TierNum:4;
+} USB_DEV_TOPOLOGY;
+
+//
+// USB Device's RouteChart
+//
+typedef union _USB_DEV_ROUTE {
+  UINT32              Dword;
+  USB_DEV_TOPOLOGY    Route;
+} USB_DEV_ROUTE;
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  //
+  // Store logical device address assigned by UsbBus
+  // It's because some XHCI host controllers may assign the same physcial device
+  // address for those devices inserted at different root port.
+  //
+  UINT8                     BusAddr;
+  UINT8                     DevAddr;
+  UINT8                     EpAddr;
+  EFI_USB_DATA_DIRECTION    Direction;
+  UINT8                     DevSpeed;
+  UINTN                     MaxPacket;
+  UINTN                     Type;
+} USB_ENDPOINT;
+
+//
+// TRB Template
+//
+typedef struct _TRB_TEMPLATE {
+  UINT32                    Parameter1;
+
+  UINT32                    Parameter2;
+
+  UINT32                    Status;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:9;
+  UINT32                    Type:6;
+  UINT32                    Control:16;
+} TRB_TEMPLATE;
+
+typedef struct _TRANSFER_RING {
+  VOID                      *RingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *RingEnqueue;
+  TRB_TEMPLATE              *RingDequeue;
+  UINT32                    RingPCS;
+} TRANSFER_RING;
+
+typedef struct _EVENT_RING {
+  VOID                      *ERSTBase;
+  VOID                      *EventRingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *EventRingEnqueue;
+  TRB_TEMPLATE              *EventRingDequeue;
+  UINT32                    EventRingCCS;
+} EVENT_RING;
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+typedef struct _URB {
+  UINT32                          Signature;
+  LIST_ENTRY                      UrbList;
+  //
+  // Usb Device URB related information
+  //
+  USB_ENDPOINT                    Ep;
+  EFI_USB_DEVICE_REQUEST          *Request;
+  VOID                            *Data;
+  UINTN                           DataLen;
+  VOID                            *DataPhy;
+  VOID                            *DataMap;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK Callback;
+  VOID                            *Context;
+  //
+  // Execute result
+  //
+  UINT32                          Result;
+  //
+  // completed data length
+  //
+  UINTN                           Completed;
+  //
+  // Command/Tranfer Ring info
+  //
+  TRANSFER_RING                   *Ring;
+  TRB_TEMPLATE                    *TrbStart;
+  TRB_TEMPLATE                    *TrbEnd;
+  UINTN                           TrbNum;
+  BOOLEAN                         StartDone;
+  BOOLEAN                         EndDone;
+  BOOLEAN                         Finished;
+
+  TRB_TEMPLATE                    *EvtTrb;
+} URB;
+
+//
+// 6.5 Event Ring Segment Table
+// The Event Ring Segment Table is used to define multi-segment Event Rings and to enable runtime
+// expansion and shrinking of the Event Ring. The location of the Event Ring Segment Table is defined by the
+// Event Ring Segment Table Base Address Register (5.5.2.3.2). The size of the Event Ring Segment Table
+// is defined by the Event Ring Segment Table Base Size Register (5.5.2.3.1).
+//
+typedef struct _EVENT_RING_SEG_TABLE_ENTRY {
+  UINT32                  PtrLo;
+  UINT32                  PtrHi;
+  UINT32                  RingTrbSize:16;
+  UINT32                  RsvdZ1:16;
+  UINT32                  RsvdZ2;
+} EVENT_RING_SEG_TABLE_ENTRY;
+
+//
+// 6.4.1.1 Normal TRB
+// A Normal TRB is used in several ways; exclusively on Bulk and Interrupt Transfer Rings for normal and
+// Scatter/Gather operations, to define additional data buffers for Scatter/Gather operations on Isoch Transfer
+// Rings, and to define the Data stage information for Control Transfer Rings.
+//
+typedef struct _TRANSFER_TRB_NORMAL {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:17;
+  UINT32                  TDSize:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  ISP:1;
+  UINT32                  NS:1;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ1:2;
+  UINT32                  BEI:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ2:16;
+} TRANSFER_TRB_NORMAL;
+
+//
+// 6.4.1.2.1 Setup Stage TRB
+// A Setup Stage TRB is created by system software to initiate a USB Setup packet on a control endpoint.
+//
+typedef struct _TRANSFER_TRB_CONTROL_SETUP {
+  UINT32                  bmRequestType:8;
+  UINT32                  bRequest:8;
+  UINT32                  wValue:16;
+
+  UINT32                  wIndex:16;
+  UINT32                  wLength:16;
+
+  UINT32                  Length:17;
+  UINT32                  RsvdZ1:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:4;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ3:3;
+  UINT32                  Type:6;
+  UINT32                  TRT:2;
+  UINT32                  RsvdZ4:14;
+} TRANSFER_TRB_CONTROL_SETUP;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_DATA {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:17;
+  UINT32                  TDSize:5;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  ISP:1;
+  UINT32                  NS:1;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  IDT:1;
+  UINT32                  RsvdZ1:3;
+  UINT32                  Type:6;
+  UINT32                  DIR:1;
+  UINT32                  RsvdZ2:15;
+} TRANSFER_TRB_CONTROL_DATA;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_STATUS {
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  RsvdZ3:22;
+  UINT32                  IntTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  ENT:1;
+  UINT32                  RsvdZ4:2;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  RsvdZ5:4;
+  UINT32                  Type:6;
+  UINT32                  DIR:1;
+  UINT32                  RsvdZ6:15;
+} TRANSFER_TRB_CONTROL_STATUS;
+
+//
+// 6.4.2.1 Transfer Event TRB
+// A Transfer Event provides the completion status associated with a Transfer TRB. Refer to section 4.11.3.1
+// for more information on the use and operation of Transfer Events.
+//
+typedef struct _EVT_TRB_TRANSFER {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  Length:24;
+  UINT32                  Completecode:8;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ1:1;
+  UINT32                  ED:1;
+  UINT32                  RsvdZ2:7;
+  UINT32                  Type:6;
+  UINT32                  EndpointId:5;
+  UINT32                  RsvdZ3:3;
+  UINT32                  SlotId:8;
+} EVT_TRB_TRANSFER;
+
+//
+// 6.4.2.2 Command Completion Event TRB
+// A Command Completion Event TRB shall be generated by the xHC when a command completes on the
+// Command Ring. Refer to section 4.11.4 for more information on the use of Command Completion Events.
+//
+typedef struct _EVT_TRB_COMMAND_COMPLETION {
+  UINT32                  TRBPtrLo;
+
+  UINT32                  TRBPtrHi;
+
+  UINT32                  RsvdZ2:24;
+  UINT32                  Completecode:8;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  VFID:8;
+  UINT32                  SlotId:8;
+} EVT_TRB_COMMAND_COMPLETION;
+
+typedef union _TRB {
+  TRB_TEMPLATE                TrbTemplate;
+  TRANSFER_TRB_NORMAL         TrbNormal;
+  TRANSFER_TRB_CONTROL_SETUP  TrbCtrSetup;
+  TRANSFER_TRB_CONTROL_DATA   TrbCtrData;
+  TRANSFER_TRB_CONTROL_STATUS TrbCtrStatus;
+} TRB;
+
+//
+// 6.4.3.1 No Op Command TRB
+// The No Op Command TRB provides a simple means for verifying the operation of the Command Ring
+// mechanisms offered by the xHCI.
+//
+typedef struct _CMD_TRB_NO_OP {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} CMD_TRB_NO_OP;
+
+//
+// 6.4.3.2 Enable Slot Command TRB
+// The Enable Slot Command TRB causes the xHC to select an available Device Slot and return the ID of the
+// selected slot to the host in a Command Completion Event.
+//
+typedef struct _CMD_TRB_ENABLE_SLOT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} CMD_TRB_ENABLE_SLOT;
+
+//
+// 6.4.3.3 Disable Slot Command TRB
+// The Disable Slot Command TRB releases any bandwidth assigned to the disabled slot and frees any
+// internal xHC resources assigned to the slot.
+//
+typedef struct _CMD_TRB_DISABLE_SLOT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_DISABLE_SLOT;
+
+//
+// 6.4.3.4 Address Device Command TRB
+// The Address Device Command TRB transitions the selected Device Context from the Default to the
+// Addressed state and causes the xHC to select an address for the USB device in the Default State and
+// issue a SET_ADDRESS request to the USB device.
+//
+typedef struct _CMD_TRB_ADDRESS_DEVICE {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:8;
+  UINT32                  BSR:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_ADDRESS_DEVICE;
+
+//
+// 6.4.3.5 Configure Endpoint Command TRB
+// The Configure Endpoint Command TRB evaluates the bandwidth and resource requirements of the
+// endpoints selected by the command.
+//
+typedef struct _CMD_TRB_CONFIG_ENDPOINT {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:8;
+  UINT32                  DC:1;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_CONFIG_ENDPOINT;
+
+//
+// 6.4.3.6 Evaluate Context Command TRB
+// The Evaluate Context Command TRB is used by system software to inform the xHC that the selected
+// Context data structures in the Device Context have been modified by system software and that the xHC
+// shall evaluate any changes
+//
+typedef struct _CMD_TRB_EVALUATE_CONTEXT {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:9;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ3:8;
+  UINT32                  SlotId:8;
+} CMD_TRB_EVALUATE_CONTEXT;
+
+//
+// 6.4.3.7 Reset Endpoint Command TRB
+// The Reset Endpoint Command TRB is used by system software to reset a specified Transfer Ring
+//
+typedef struct _CMD_TRB_RESET_ENDPOINT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:8;
+  UINT32                  TSP:1;
+  UINT32                  Type:6;
+  UINT32                  EDID:5;
+  UINT32                  RsvdZ4:3;
+  UINT32                  SlotId:8;
+} CMD_TRB_RESET_ENDPOINT;
+
+//
+// 6.4.3.8 Stop Endpoint Command TRB
+// The Stop Endpoint Command TRB command allows software to stop the xHC execution of the TDs on a
+// Transfer Ring and temporarily take ownership of TDs that had previously been passed to the xHC.
+//
+typedef struct _CMD_TRB_STOP_ENDPOINT {
+  UINT32                  RsvdZ0;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ3:9;
+  UINT32                  Type:6;
+  UINT32                  EDID:5;
+  UINT32                  RsvdZ4:2;
+  UINT32                  SP:1;
+  UINT32                  SlotId:8;
+} CMD_TRB_STOP_ENDPOINT;
+
+//
+// 6.4.3.9 Set TR Dequeue Pointer Command TRB
+// The Set TR Dequeue Pointer Command TRB is used by system software to modify the TR Dequeue
+// Pointer and DCS fields of an Endpoint or Stream Context.
+//
+typedef struct _CMD_SET_TR_DEQ_POINTER {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1:16;
+  UINT32                  StreamID:16;
+
+  UINT32                  CycleBit:1;
+  UINT32                  RsvdZ2:9;
+  UINT32                  Type:6;
+  UINT32                  Endpoint:5;
+  UINT32                  RsvdZ3:3;
+  UINT32                  SlotId:8;
+} CMD_SET_TR_DEQ_POINTER;
+
+//
+// 6.4.4.1 Link TRB
+// A Link TRB provides support for non-contiguous TRB Rings.
+//
+typedef struct _LINK_TRB {
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  RsvdZ1:22;
+  UINT32                  InterTarget:10;
+
+  UINT32                  CycleBit:1;
+  UINT32                  TC:1;
+  UINT32                  RsvdZ2:2;
+  UINT32                  CH:1;
+  UINT32                  IOC:1;
+  UINT32                  RsvdZ3:4;
+  UINT32                  Type:6;
+  UINT32                  RsvdZ4:16;
+} LINK_TRB;
+
+//
+// 6.2.2 Slot Context
+//
+typedef struct _SLOT_CONTEXT {
+  UINT32                  RouteString:20;
+  UINT32                  Speed:4;
+  UINT32                  RsvdZ1:1;
+  UINT32                  MTT:1;
+  UINT32                  Hub:1;
+  UINT32                  ContextEntries:5;
+
+  UINT32                  MaxExitLatency:16;
+  UINT32                  RootHubPortNum:8;
+  UINT32                  PortNum:8;
+
+  UINT32                  TTHubSlotId:8;
+  UINT32                  TTPortNum:8;
+  UINT32                  TTT:2;
+  UINT32                  RsvdZ2:4;
+  UINT32                  InterTarget:10;
+
+  UINT32                  DeviceAddress:8;
+  UINT32                  RsvdZ3:19;
+  UINT32                  SlotState:5;
+
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+} SLOT_CONTEXT;
+
+typedef struct _SLOT_CONTEXT_64 {
+  UINT32                  RouteString:20;
+  UINT32                  Speed:4;
+  UINT32                  RsvdZ1:1;
+  UINT32                  MTT:1;
+  UINT32                  Hub:1;
+  UINT32                  ContextEntries:5;
+
+  UINT32                  MaxExitLatency:16;
+  UINT32                  RootHubPortNum:8;
+  UINT32                  PortNum:8;
+
+  UINT32                  TTHubSlotId:8;
+  UINT32                  TTPortNum:8;
+  UINT32                  TTT:2;
+  UINT32                  RsvdZ2:4;
+  UINT32                  InterTarget:10;
+
+  UINT32                  DeviceAddress:8;
+  UINT32                  RsvdZ3:19;
+  UINT32                  SlotState:5;
+
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+  UINT32                  RsvdZ15;
+
+} SLOT_CONTEXT_64;
+
+
+//
+// 6.2.3 Endpoint Context
+//
+typedef struct _ENDPOINT_CONTEXT {
+  UINT32                  EPState:3;
+  UINT32                  RsvdZ1:5;
+  UINT32                  Mult:2;
+  UINT32                  MaxPStreams:5;
+  UINT32                  LSA:1;
+  UINT32                  Interval:8;
+  UINT32                  RsvdZ2:8;
+
+  UINT32                  RsvdZ3:1;
+  UINT32                  CErr:2;
+  UINT32                  EPType:3;
+  UINT32                  RsvdZ4:1;
+  UINT32                  HID:1;
+  UINT32                  MaxBurstSize:8;
+  UINT32                  MaxPacketSize:16;
+
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  AverageTRBLength:16;
+  UINT32                  MaxESITPayload:16;
+
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+} ENDPOINT_CONTEXT;
+
+typedef struct _ENDPOINT_CONTEXT_64 {
+  UINT32                  EPState:3;
+  UINT32                  RsvdZ1:5;
+  UINT32                  Mult:2;
+  UINT32                  MaxPStreams:5;
+  UINT32                  LSA:1;
+  UINT32                  Interval:8;
+  UINT32                  RsvdZ2:8;
+
+  UINT32                  RsvdZ3:1;
+  UINT32                  CErr:2;
+  UINT32                  EPType:3;
+  UINT32                  RsvdZ4:1;
+  UINT32                  HID:1;
+  UINT32                  MaxBurstSize:8;
+  UINT32                  MaxPacketSize:16;
+
+  UINT32                  PtrLo;
+
+  UINT32                  PtrHi;
+
+  UINT32                  AverageTRBLength:16;
+  UINT32                  MaxESITPayload:16;
+
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+  
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+  UINT32                  RsvdZ15;
+
+} ENDPOINT_CONTEXT_64;
+
+
+//
+// 6.2.5.1 Input Control Context
+//
+typedef struct _INPUT_CONTRL_CONTEXT {
+  UINT32                  Dword1;
+  UINT32                  Dword2;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+  UINT32                  RsvdZ3;
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+} INPUT_CONTRL_CONTEXT;
+
+typedef struct _INPUT_CONTRL_CONTEXT_64 {
+  UINT32                  Dword1;
+  UINT32                  Dword2;
+  UINT32                  RsvdZ1;
+  UINT32                  RsvdZ2;
+  UINT32                  RsvdZ3;
+  UINT32                  RsvdZ4;
+  UINT32                  RsvdZ5;
+  UINT32                  RsvdZ6;
+  UINT32                  RsvdZ7;
+  UINT32                  RsvdZ8;
+  UINT32                  RsvdZ9;
+  UINT32                  RsvdZ10;
+  UINT32                  RsvdZ11;
+  UINT32                  RsvdZ12;
+  UINT32                  RsvdZ13;
+  UINT32                  RsvdZ14;
+} INPUT_CONTRL_CONTEXT_64;
+
+//
+// 6.2.1 Device Context
+//
+typedef struct _DEVICE_CONTEXT {
+  SLOT_CONTEXT            Slot;
+  ENDPOINT_CONTEXT        EP[31];
+} DEVICE_CONTEXT;
+
+typedef struct _DEVICE_CONTEXT_64 {
+  SLOT_CONTEXT_64         Slot;
+  ENDPOINT_CONTEXT_64     EP[31];
+} DEVICE_CONTEXT_64;
+
+//
+// 6.2.5 Input Context
+//
+typedef struct _INPUT_CONTEXT {
+  INPUT_CONTRL_CONTEXT    InputControlContext;
+  SLOT_CONTEXT            Slot;
+  ENDPOINT_CONTEXT        EP[31];
+} INPUT_CONTEXT;
+
+typedef struct _INPUT_CONTEXT_64 {
+  INPUT_CONTRL_CONTEXT_64 InputControlContext;
+  SLOT_CONTEXT_64         Slot;
+  ENDPOINT_CONTEXT_64     EP[31];
+} INPUT_CONTEXT_64;
+
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc        The XHCI Instance to be initialized.
+
+**/
+VOID
+XhcInitSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc        The XHCI Instance.
+
+**/
+VOID
+XhcFreeSched (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed through URB.
+
+  @param  Xhc           The XHCI Instance.
+  @param  Urb           The URB to be rung.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+RingIntTransferDoorBell (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI Instance.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcExecTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  BOOLEAN             CmdTransfer,
+  IN  URB                 *Urb,
+  IN  UINTN               Timeout
+  );
+
+/**
+  Delete a single asynchronous interrupt transfer for
+  the device and endpoint.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  BusAddr               The logical device address assigned by UsbBus driver.
+  @param  EpNum                 The endpoint of the target.
+
+  @retval EFI_SUCCESS           An asynchronous transfer is removed.
+  @retval EFI_NOT_FOUND         No transfer for the device is found.
+
+**/
+EFI_STATUS
+XhciDelAsyncIntTransfer (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  UINT8               BusAddr,
+  IN  UINT8               EpNum
+  );
+
+/**
+  Remove all the asynchronous interrupt transfers.
+
+  @param  Xhc                   The XHCI Instance.
+
+**/
+VOID
+XhciDelAllAsyncIntTransfers (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Set Bios Ownership
+
+  @param  Xhc          The XHCI Instance.
+
+**/
+VOID
+XhcSetBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Clear Bios Ownership
+
+  @param  Xhc       The XHCI Instance.
+
+**/
+VOID
+XhcClearBiosOwnership (
+  IN USB_XHCI_INSTANCE    *Xhc
+  );
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc             The XHCI Instance.
+  @param  RouteString     The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+EFIAPI
+XhcRouteStringToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  USB_DEV_ROUTE      RouteString
+  );
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr              The target endpoint number.
+  @param  Direction           The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcEndpointToDci (
+  IN  UINT8                   EpAddr,
+  IN  UINT8                   Direction
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Successfully ring the door bell.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRingDoorBell (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN UINT8                SlotId,
+  IN UINT8                Dci
+  );
+
+/**
+  Interrupt transfer periodic check handler.
+
+  @param  Event                 Interrupt event.
+  @param  Context               Pointer to USB_XHCI_INSTANCE.
+
+**/
+VOID
+EFIAPI
+XhcMonitorAsyncRequests (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  ParentRouteChart      The route string pointed to the parent device if it exists.
+  @param  Port                  The port to be polled.
+  @param  PortState             The port state.
+
+  @retval EFI_SUCCESS           Successfully enable/disable device slot according to port state.
+  @retval Others                Should not appear.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPollPortStatusChange (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  USB_DEV_ROUTE         ParentRouteChart,
+  IN  UINT8                 Port,
+  IN  EFI_USB_PORT_STATUS   *PortState
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  );
+
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcConfigHubContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    PortNum,
+  IN UINT8                    TTT,
+  IN UINT8                    MTT
+  );
+
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  );
+
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetConfigCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc
+  );
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  );
+
+/**
+  Set interface through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+  @param  Request       USB device request to send.
+
+  @retval EFI_SUCCESS   Successfully set interface.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetInterface64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT8                    DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR    *ConfigDesc,
+  IN EFI_USB_DEVICE_REQUEST   *Request
+  );
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc             The XHCI Instance.
+  @param  BusDevAddr      The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+EFIAPI
+XhcBusDevAddrToSlotId (
+  IN  USB_XHCI_INSTANCE  *Xhc,
+  IN  UINT8              BusDevAddr
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  ParentRouteChart    The route string pointed to the parent device.
+  @param  ParentPort          The port at which the device is located.
+  @param  RouteChart          The route string pointed to the device.
+  @param  DeviceSpeed         The device speed.
+
+  @retval EFI_SUCCESS   Successfully assign a slot to the device and assign an address to it.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcInitializeDeviceSlot64 (
+  IN  USB_XHCI_INSTANCE         *Xhc,
+  IN  USB_DEV_ROUTE             ParentRouteChart,
+  IN  UINT16                    ParentPort,
+  IN  USB_DEV_ROUTE             RouteChart,
+  IN  UINT8                     DeviceSpeed
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  );
+
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcEvaluateContext64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId,
+  IN UINT32                   MaxPacketSize
+  );
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId
+  );
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI Instance.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDisableSlotCmd64 (
+  IN USB_XHCI_INSTANCE        *Xhc,
+  IN UINT8                    SlotId
+  );
+
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  TrsRing     The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncTrsRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  TRANSFER_RING           *TrsRing
+  );
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc         The XHCI Instance.
+  @param  EvtRing     The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSyncEventRing (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  EVENT_RING              *EvtRing
+  );
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI Instance.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcCheckNewEvent (
+  IN  USB_XHCI_INSTANCE       *Xhc,
+  IN  EVENT_RING              *EvtRing,
+  OUT TRB_TEMPLATE            **NewEvtTrb
+  );
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Instance.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing           The created transfer ring.
+
+**/
+VOID
+CreateTransferRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  IN  UINTN                 TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  );
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc                 The XHCI Instance.
+  @param  EventRing           The created event ring.
+
+**/
+VOID
+CreateEventRing (
+  IN  USB_XHCI_INSTANCE     *Xhc,
+  OUT EVENT_RING            *EventRing
+  );
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS           The recovery is successful.
+  @retval Others                Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcRecoverHaltedEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Stop endpoint successfully.
+  @retval Others                Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcStopEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  );
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI Instance
+  @param  DevAddr   The device address
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcCreateUrb (
+  IN USB_XHCI_INSTANCE                  *Xhc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  );
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The URB to free.
+
+**/
+VOID
+XhcFreeUrb (
+  IN USB_XHCI_INSTANCE    *Xhc,
+  IN URB                  *Urb
+  );
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc     The XHCI Instance
+  @param  Urb     The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcCreateTransferTrb (
+  IN USB_XHCI_INSTANCE            *Xhc,
+  IN URB                          *Urb
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
new file mode 100644
index 0000000000..663ba39d32
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
@@ -0,0 +1,662 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pages         How many pages to allocate.
+
+  @return Pointer to the allocated memory block or NULL if failed.
+
+**/
+USBHC_MEM_BLOCK *
+UsbHcAllocMemBlock (
+  IN UINTN              Pages
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+  EFI_STATUS            Status;
+  UINTN                 PageNumber;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+
+  PageNumber = EFI_SIZE_TO_PAGES (sizeof (USBHC_MEM_BLOCK));
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  //
+  // each bit in the bit array represents USBHC_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (USBHC_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+
+  Block = (USBHC_MEM_BLOCK *) (UINTN) TempPtr;
+  Block->BufLen = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen = Block->BufLen / (USBHC_MEM_UNIT * 8);
+
+  PageNumber = EFI_SIZE_TO_PAGES (Block->BitsLen);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  Block->Bits = (UINT8 *) (UINTN) TempPtr;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             Pages,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (Pages));
+
+  Block->BufHost = (UINT8 *) (UINTN) TempPtr;;
+  Block->Buf = (UINT8 *) (UINTN) TempPtr;
+  Block->Next = NULL;
+
+  return Block;
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool          The memory pool to free the block from.
+  @param  Block         The memory block to free.
+
+**/
+VOID
+UsbHcFreeMemBlock (
+  IN USBHC_MEM_POOL     *Pool,
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+  //
+  // No free memory in PEI.
+  //
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block         The memory block to allocate memory from.
+  @param  Units         Number of memory units to allocate.
+
+  @return The pointer to the allocated memory.
+          If couldn't allocate the needed memory, the return value is NULL.
+
+**/
+VOID *
+UsbHcAllocMemFromBlock (
+  IN USBHC_MEM_BLOCK    *Block,
+  IN UINTN              Units
+  )
+{
+  UINTN                 Byte;
+  UINT8                 Bit;
+  UINTN                 StartByte;
+  UINT8                 StartBit;
+  UINTN                 Available;
+  UINTN                 Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      NEXT_BIT (Byte, Bit);
+    } else {
+      NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) USB_HC_BIT (Bit));
+    NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->BufHost + (StartByte * 8 + StartBit) * USBHC_MEM_UNIT;
+}
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to host memory.
+  @param  Size          The size of the memory region.
+
+  @return               The pci memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  UINTN                 AllocSize;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+  UINTN                 Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->BufHost <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the pci memory address for host memory address.
+  //
+  Offset = (UINT8 *) Mem - Block->BufHost;
+  PhyAddr = (EFI_PHYSICAL_ADDRESS) (UINTN) (Block->Buf + Offset);
+  return PhyAddr;
+}
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to pci memory.
+  @param  Size          The size of the memory region.
+
+  @return               The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  UINTN                 AllocSize;
+  EFI_PHYSICAL_ADDRESS  HostAddr;
+  UINTN                 Offset;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+
+  if (Mem == NULL) {
+    return 0;
+  }
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the allocated memory.
+    //
+    if ((Block->Buf <= (UINT8 *) Mem) && (((UINT8 *) Mem + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      break;
+    }
+  }
+
+  ASSERT ((Block != NULL));
+  //
+  // calculate the host memory address for pci memory address.
+  //
+  Offset = (UINT8 *) Mem - Block->Buf;
+  HostAddr = (EFI_PHYSICAL_ADDRESS) (UINTN) (Block->BufHost + Offset);
+  return HostAddr;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head          The head of the memory pool's block list.
+  @param  Block         The memory block to insert.
+
+**/
+VOID
+UsbHcInsertMemBlockToPool (
+  IN USBHC_MEM_BLOCK    *Head,
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block         The memory block to check.
+
+  @retval TRUE          The memory block is empty.
+  @retval FALSE         The memory block isn't empty.
+
+**/
+BOOLEAN
+UsbHcIsMemBlockEmpty (
+  IN USBHC_MEM_BLOCK    *Block
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head          The block list head of the memory's pool.
+  @param  BlockToUnlink The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK    *Head,
+  IN USBHC_MEM_BLOCK    *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @return Pointer to the allocated memory pool or NULL if failed.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  VOID
+  )
+{
+  USBHC_MEM_POOL        *Pool;
+  UINTN                 PageNumber;
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  TempPtr;
+
+  PageNumber = EFI_SIZE_TO_PAGES (sizeof (USBHC_MEM_POOL));
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesData,
+             PageNumber,
+             &TempPtr
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (PageNumber));
+
+  Pool = (USBHC_MEM_POOL *) ((UINTN) TempPtr);
+  Pool->Head = UsbHcAllocMemBlock (USBHC_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    //
+    // No free memory in PEI.
+    //
+    Pool = NULL;
+  }
+
+  return Pool;
+}
+
+/**
+  Release the memory management pool.
+
+  @param  Pool          The USB memory pool to free.
+
+**/
+VOID
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL     *Pool
+  )
+{
+  USBHC_MEM_BLOCK       *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UsbHcUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    //UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+
+  UsbHcFreeMemBlock (Pool, Pool->Head);
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool          The host controller's memory pool.
+  @param  Size          Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  USBHC_MEM_BLOCK       *NewBlock;
+  VOID                  *Mem;
+  UINTN                 AllocSize;
+  UINTN                 Pages;
+
+  Mem       = NULL;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UsbHcAllocMemFromBlock (Block, AllocSize / USBHC_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (USBHC_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize);
+  } else {
+    Pages = USBHC_MEM_DEFAULT_PAGES;
+  }
+  NewBlock = UsbHcAllocMemBlock (Pages);
+
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UsbHcInsertMemBlockToPool (Head, NewBlock);
+  Mem = UsbHcAllocMemFromBlock (NewBlock, AllocSize / USBHC_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The memory to free.
+  @param  Size          The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+{
+  USBHC_MEM_BLOCK       *Head;
+  USBHC_MEM_BLOCK       *Block;
+  UINT8                 *ToFree;
+  UINTN                 AllocSize;
+  UINTN                 Byte;
+  UINTN                 Bit;
+  UINTN                 Count;
+
+  Head      = Pool->Head;
+  AllocSize = USBHC_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->BufHost <= ToFree) && ((ToFree + AllocSize) <= (Block->BufHost + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->BufHost) / USBHC_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / USBHC_MEM_UNIT); Count++) {
+        ASSERT (USB_HC_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ USB_HC_BIT (Bit));
+        NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory pointer
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
+    //UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcFreeMemBlock (Pool, Block);
+  }
+}
+
+/**
+  Allocates pages at a specified alignment.
+
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to
+                                use to access the hosts HostAddress.
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN UINTN                      Pages,
+  IN UINTN                      Alignment,
+  OUT VOID                      **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS      *DeviceAddress
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 RealPages;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+
+  if ((Alignment & (Alignment - 1)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Pages == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               Pages,
+               &Memory
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               Pages,
+               &Memory
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    AlignedMemory = (UINTN) Memory;
+  }
+
+  *HostAddress = (VOID *) AlignedMemory;
+  *DeviceAddress = (EFI_PHYSICAL_ADDRESS) AlignedMemory;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN VOID               *HostAddress,
+  IN UINTN              Pages
+  )
+{
+  ASSERT (Pages != 0);
+  //
+  // No free memory in PEI.
+  //
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h
new file mode 100644
index 0000000000..c314e92004
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.h
@@ -0,0 +1,142 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_MEM_H_
+#define _EFI_PEI_XHCI_MEM_H_
+
+#include <Uefi.h>
+
+#define USBHC_MEM_DEFAULT_PAGES 16
+
+typedef struct _USBHC_MEM_BLOCK USBHC_MEM_BLOCK;
+
+struct _USBHC_MEM_BLOCK {
+  UINT8                 *Bits;  // Bit array to record which unit is allocated
+  UINTN                 BitsLen;
+  UINT8                 *Buf;
+  UINT8                 *BufHost;
+  UINTN                 BufLen; // Memory size in bytes
+  USBHC_MEM_BLOCK       *Next;
+};
+
+//
+// Memory allocation unit, must be 2^n, n>4
+//
+#define USBHC_MEM_UNIT          64
+
+#define USBHC_MEM_UNIT_MASK     (USBHC_MEM_UNIT - 1)
+#define USBHC_MEM_ROUND(Len)    (((Len) + USBHC_MEM_UNIT_MASK) & (~USBHC_MEM_UNIT_MASK))
+
+#define USB_HC_BIT(a)           ((UINTN)(1 << (a)))
+
+#define USB_HC_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & USB_HC_BIT(Bit)) == USB_HC_BIT(Bit)))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define NEXT_BIT(Byte, Bit)  \
+          do {               \
+            (Bit)++;         \
+            if ((Bit) > 7) { \
+              (Byte)++;      \
+              (Bit) = 0;     \
+            }                \
+          } while (0)
+
+//
+// USBHC_MEM_POOL is used to manage the memory used by USB
+// host controller. XHCI requires the control memory and transfer
+// data to be on the same 4G memory.
+//
+typedef struct _USBHC_MEM_POOL {
+  BOOLEAN               Check4G;
+  UINT32                Which4G;
+  USBHC_MEM_BLOCK       *Head;
+} USBHC_MEM_POOL;
+
+/**
+  Calculate the corresponding pci bus address according to the Mem parameter.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to host memory.
+  @param  Size          The size of the memory region.
+
+  @return               The pci memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetPciAddrForHostAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  );
+
+/**
+  Calculate the corresponding host address according to the pci address.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The pointer to pci memory.
+  @param  Size          The size of the memory region.
+
+  @return               The host memory address
+
+**/
+EFI_PHYSICAL_ADDRESS
+UsbHcGetHostAddrForPciAddr (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  );
+
+/**
+  Allocates pages at a specified alignment.
+
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+
+  @param  Pages                 The number of pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  DeviceAddress         The resulting map address for the bus master PCI controller to
+                                use to access the hosts HostAddress.
+
+  @retval EFI_SUCCESS           Success to allocate aligned pages.
+  @retval EFI_INVALID_PARAMETER Pages or Alignment is not valid.
+  @retval EFI_OUT_OF_RESOURCES  Do not have enough resources to allocate memory.
+
+**/
+EFI_STATUS
+UsbHcAllocateAlignedPages (
+  IN UINTN                      Pages,
+  IN UINTN                      Alignment,
+  OUT VOID                      **HostAddress,
+  OUT EFI_PHYSICAL_ADDRESS      *DeviceAddress
+  );
+
+/**
+  Frees memory that was allocated with UsbHcAllocateAlignedPages().
+
+  @param  HostAddress           The system memory address to map to the PCI controller.
+  @param  Pages                 The number of pages to free.
+
+**/
+VOID
+UsbHcFreeAlignedPages (
+  IN VOID               *HostAddress,
+  IN UINTN              Pages
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c
new file mode 100644
index 0000000000..eaea38d94d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c
@@ -0,0 +1,1534 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+//
+// Two arrays used to translate the XHCI port state (change)
+// to the UEFI protocol's port state (change).
+//
+USB_PORT_STATE_MAP  mUsbPortStateMap[] = {
+  {XHC_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_PORTSC_PP,    USB_PORT_STAT_POWER},
+  {XHC_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbPortChangeMap[] = {
+  {XHC_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbClearPortChangeMap[] = {
+  {XHC_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_PORTSC_PRC, EfiUsbPortResetChange}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortStateMap[] = {
+  {XHC_HUB_PORTSC_CCS,   USB_PORT_STAT_CONNECTION},
+  {XHC_HUB_PORTSC_PED,   USB_PORT_STAT_ENABLE},
+  {XHC_HUB_PORTSC_OCA,   USB_PORT_STAT_OVERCURRENT},
+  {XHC_HUB_PORTSC_PP,    USB_PORT_STAT_POWER},
+  {XHC_HUB_PORTSC_RESET, USB_PORT_STAT_RESET}
+};
+
+USB_PORT_STATE_MAP  mUsbHubPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, USB_PORT_STAT_C_CONNECTION},
+  {XHC_HUB_PORTSC_PEC, USB_PORT_STAT_C_ENABLE},
+  {XHC_HUB_PORTSC_OCC, USB_PORT_STAT_C_OVERCURRENT},
+  {XHC_HUB_PORTSC_PRC, USB_PORT_STAT_C_RESET}
+};
+
+USB_CLEAR_PORT_MAP mUsbHubClearPortChangeMap[] = {
+  {XHC_HUB_PORTSC_CSC, EfiUsbPortConnectChange},
+  {XHC_HUB_PORTSC_PEC, EfiUsbPortEnableChange},
+  {XHC_HUB_PORTSC_OCC, EfiUsbPortOverCurrentChange},
+  {XHC_HUB_PORTSC_PRC, EfiUsbPortResetChange},
+  {XHC_HUB_PORTSC_BHRC, Usb3PortBHPortResetChange}
+};
+
+/**
+  Read XHCI Operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset
+  )
+{
+  UINT32                Data;
+
+  ASSERT (Xhc->CapLength != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->CapLength + Offset);
+  return Data;
+}
+
+/**
+  Write the data to the XHCI operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+  @param Data           The data to write.
+
+**/
+VOID
+XhcPeiWriteOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  )
+{
+  ASSERT (Xhc->CapLength != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->CapLength + Offset, Data);
+}
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadOpReg (Xhc, Offset);
+  Data |= Bit;
+  XhcPeiWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to clear.
+
+**/
+VOID
+XhcPeiClearOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadOpReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcPeiWriteOpReg (Xhc, Offset, Data);
+}
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to become set (or clear).
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in millisecond, ms).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+XhcPeiWaitOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit,
+  IN BOOLEAN            WaitToSet,
+  IN UINT32             Timeout
+  )
+{
+  UINT64                Index;
+
+  for (Index = 0; Index < Timeout * XHC_1_MILLISECOND; Index++) {
+    if (XHC_REG_BIT_IS_SET (Xhc, Offset, Bit) == WaitToSet) {
+      return EFI_SUCCESS;
+    }
+
+    MicroSecondDelay (XHC_1_MICROSECOND);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Read XHCI capability register.
+
+  @param Xhc        The XHCI device.
+  @param Offset     Capability register address.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadCapRegister (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset
+  )
+{
+  UINT32                Data;
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Offset);
+
+  return Data;
+}
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc       The XHCI device.
+  @param  Offset    The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadDoorBellReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  )
+{
+  UINT32                  Data;
+
+  ASSERT (Xhc->DBOff != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->DBOff + Offset);
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteDoorBellReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  )
+{
+  ASSERT (Xhc->DBOff != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->DBOff + Offset, Data);
+}
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadRuntimeReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  )
+{
+  UINT32                Data;
+
+  ASSERT (Xhc->RTSOff != 0);
+
+  Data = MmioRead32 (Xhc->UsbHostControllerBaseAddress + Xhc->RTSOff + Offset);
+
+  return Data;
+}
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc       The XHCI device.
+  @param  Offset    The offset of the runtime register.
+  @param  Data      The data to write.
+
+**/
+VOID
+XhcPeiWriteRuntimeReg (
+  IN PEI_XHC_DEV          *Xhc,
+  IN UINT32               Offset,
+  IN UINT32               Data
+  )
+{
+  ASSERT (Xhc->RTSOff != 0);
+
+  MmioWrite32 (Xhc->UsbHostControllerBaseAddress + Xhc->RTSOff + Offset, Data);
+}
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI device.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadRuntimeReg (Xhc, Offset);
+  Data |= Bit;
+  XhcPeiWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc          The XHCI device.
+  @param  Offset       The offset of the runtime register.
+  @param  Bit          The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiClearRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  )
+{
+  UINT32                Data;
+
+  Data  = XhcPeiReadRuntimeReg (Xhc, Offset);
+  Data &= ~Bit;
+  XhcPeiWriteRuntimeReg (Xhc, Offset, Data);
+}
+
+/**
+  Check whether Xhc is halted.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          The controller is halted.
+  @retval FALSE         The controller isn't halted.
+
+**/
+BOOLEAN
+XhcPeiIsHalt (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT);
+}
+
+/**
+  Check whether system error occurred.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          System error happened.
+  @retval FALSE         No system error.
+
+**/
+BOOLEAN
+XhcPeiIsSysError (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  return XHC_REG_BIT_IS_SET (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HSE);
+}
+
+/**
+  Reset the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT   The transfer failed due to time out.
+  @retval Others        Failed to reset the host.
+
+**/
+EFI_STATUS
+XhcPeiResetHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  //
+  // Host can only be reset when it is halt. If not so, halt it
+  //
+  if (!XhcPeiIsHalt (Xhc)) {
+    Status = XhcPeiHaltHC (Xhc, Timeout);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  XhcPeiSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RESET, FALSE, Timeout);
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiResetHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Halt the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_TIMEOUT   Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS   The XHCI is halt.
+
+**/
+EFI_STATUS
+XhcPeiHaltHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  XhcPeiClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, TRUE, Timeout);
+  DEBUG ((EFI_D_INFO, "XhcPeiHaltHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Set the XHCI to run.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_SUCCESS   The XHCI is running.
+  @retval Others        Failed to set the XHCI to run.
+
+**/
+EFI_STATUS
+XhcPeiRunHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  )
+{
+  EFI_STATUS            Status;
+
+  XhcPeiSetOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_RUN);
+  Status = XhcPeiWaitOpRegBit (Xhc, XHC_USBSTS_OFFSET, XHC_USBSTS_HALT, FALSE, Timeout);
+  DEBUG ((EFI_D_INFO, "XhcPeiRunHC: %r\n", Status));
+  return Status;
+}
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress             The target device address.
+  @param  DeviceSpeed               Target device speed.
+  @param  MaximumPacketLength       Maximum packet size the default control transfer
+                                    endpoint is capable of sending or receiving.
+  @param  Request                   USB device request to send.
+  @param  TransferDirection         Specifies the data direction for the data stage.
+  @param  Data                      Data buffer to be transmitted or received from USB device.
+  @param  DataLength                The size (in bytes) of the data buffer.
+  @param  TimeOut                   Indicates the maximum timeout, in millisecond.
+                                    If Timeout is 0, then the caller must wait for the function
+                                    to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator                Transaction translator to be used by this device.
+  @param  TransferResult            Return the result of this control transfer.
+
+  @retval EFI_SUCCESS               Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES      The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER     Some parameters are invalid.
+  @retval EFI_TIMEOUT               Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR          Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiControlTransfer (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  DeviceAddress,
+  IN UINT8                                  DeviceSpeed,
+  IN UINTN                                  MaximumPacketLength,
+  IN EFI_USB_DEVICE_REQUEST                 *Request,
+  IN EFI_USB_DATA_DIRECTION                 TransferDirection,
+  IN OUT VOID                               *Data,
+  IN OUT UINTN                              *DataLength,
+  IN UINTN                                  TimeOut,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR     *Translator,
+  OUT UINT32                                *TransferResult
+  )
+{
+  PEI_XHC_DEV                   *Xhc;
+  URB                           *Urb;
+  UINT8                         Endpoint;
+  UINT8                         Index;
+  UINT8                         DescriptorType;
+  UINT8                         SlotId;
+  UINT8                         TTT;
+  UINT8                         MTT;
+  UINT32                        MaxPacket0;
+  EFI_USB_HUB_DESCRIPTOR        *HubDesc;
+  EFI_STATUS                    Status;
+  EFI_STATUS                    RecoveryStatus;
+  UINTN                         MapSize;
+  EFI_USB_PORT_STATUS           PortStatus;
+  UINT32                        State;
+  EFI_USB_DEVICE_REQUEST        ClearPortRequest;
+  UINTN                         Len;
+
+  //
+  // Validate parameters
+  //
+  if ((Request == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbDataIn) &&
+      (TransferDirection != EfiUsbDataOut) &&
+      (TransferDirection != EfiUsbNoData)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection == EfiUsbNoData) &&
+      ((Data != NULL) || (*DataLength != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((TransferDirection != EfiUsbNoData) &&
+     ((Data == NULL) || (*DataLength == 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MaximumPacketLength != 8)  && (MaximumPacketLength != 16) &&
+      (MaximumPacketLength != 32) && (MaximumPacketLength != 64) &&
+      (MaximumPacketLength != 512)
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) && (MaximumPacketLength != 8)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength != 512)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc             = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  Status          = EFI_DEVICE_ERROR;
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Len             = 0;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Hook the Set_Address request from UsbBus.
+  // According to XHCI 1.0 spec, the Set_Address request is replaced by XHCI's Address_Device cmd.
+  //
+  if ((Request->Request     == USB_REQ_SET_ADDRESS) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Reset the BusDevAddr field of all disabled entries in UsbDevContext array firstly.
+    // This way is used to clean the history to avoid using wrong device address afterwards.
+    //
+    for (Index = 0; Index < 255; Index++) {
+      if (!Xhc->UsbDevContext[Index + 1].Enabled &&
+          (Xhc->UsbDevContext[Index + 1].SlotId == 0) &&
+          (Xhc->UsbDevContext[Index + 1].BusDevAddr == (UINT8) Request->Value)) {
+        Xhc->UsbDevContext[Index + 1].BusDevAddr = 0;
+      }
+    }
+
+    if (Xhc->UsbDevContext[SlotId].XhciDevAddr == 0) {
+      goto ON_EXIT;
+    }
+    //
+    // The actual device address has been assigned by XHCI during initializing the device slot.
+    // So we just need establish the mapping relationship between the device address requested from UsbBus
+    // and the actual device address assigned by XHCI. The following invocations through EFI_USB2_HC_PROTOCOL interface
+    // can find out the actual device address by it.
+    //
+    Xhc->UsbDevContext[SlotId].BusDevAddr = (UINT8) Request->Value;
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  // Note that we encode the direction in address although default control
+  // endpoint is bidirectional. XhcPeiCreateUrb expects this
+  // combination of Ep addr and its direction.
+  //
+  Endpoint = (UINT8) (0 | ((TransferDirection == EfiUsbDataIn) ? 0x80 : 0));
+  Urb = XhcPeiCreateUrb (
+          Xhc,
+          DeviceAddress,
+          Endpoint,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_CTRL_TRANSFER,
+          Request,
+          Data,
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: failed to create URB"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcPeiExecTransfer (Xhc, FALSE, Urb, TimeOut);
+
+  //
+  // Get the status from URB. The result is updated in XhcPeiCheckUrbResult
+  // which is called by XhcPeiExecTransfer
+  //
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
+    }
+    goto FREE_URB;
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
+  }
+
+  //
+  // Hook Get_Descriptor request from UsbBus as we need evaluate context and configure endpoint.
+  // Hook Get_Status request form UsbBus as we need trace device attach/detach event happened at hub.
+  // Hook Set_Config request from UsbBus as we need configure device endpoint.
+  //
+  if ((Request->Request     == USB_REQ_GET_DESCRIPTOR) &&
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE)) ||
+      ((Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_DEVICE))))) {
+    DescriptorType = (UINT8) (Request->Value >> 8);
+    if ((DescriptorType == USB_DESC_TYPE_DEVICE) && ((*DataLength == sizeof (EFI_USB_DEVICE_DESCRIPTOR)) || ((DeviceSpeed == EFI_USB_SPEED_FULL) && (*DataLength == 8)))) {
+      ASSERT (Data != NULL);
+      //
+      // Store a copy of device scriptor as hub device need this info to configure endpoint.
+      //
+      CopyMem (&Xhc->UsbDevContext[SlotId].DevDesc, Data, *DataLength);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.BcdUSB == 0x0300) {
+        //
+        // If it's a usb3.0 device, then its max packet size is a 2^n.
+        //
+        MaxPacket0 = 1 << Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+      } else {
+        MaxPacket0 = Xhc->UsbDevContext[SlotId].DevDesc.MaxPacketSize0;
+      }
+      Xhc->UsbDevContext[SlotId].ConfDesc = AllocateZeroPool (Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations * sizeof (EFI_USB_CONFIG_DESCRIPTOR *));
+      if (Xhc->UsbDevContext[SlotId].ConfDesc == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto FREE_URB;
+      }
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiEvaluateContext (Xhc, SlotId, MaxPacket0);
+      } else {
+        Status = XhcPeiEvaluateContext64 (Xhc, SlotId, MaxPacket0);
+      }
+    } else if (DescriptorType == USB_DESC_TYPE_CONFIG) {
+      ASSERT (Data != NULL);
+      if (*DataLength == ((UINT16 *) Data)[1]) {
+        //
+        // Get configuration value from request, store the configuration descriptor for Configure_Endpoint cmd.
+        //
+        Index = (UINT8) Request->Value;
+        ASSERT (Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations);
+        Xhc->UsbDevContext[SlotId].ConfDesc[Index] = AllocateZeroPool (*DataLength);
+        if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto FREE_URB;
+        }
+        CopyMem (Xhc->UsbDevContext[SlotId].ConfDesc[Index], Data, *DataLength);
+      }
+    } else if (((DescriptorType == USB_DESC_TYPE_HUB) ||
+               (DescriptorType == USB_DESC_TYPE_HUB_SUPER_SPEED)) && (*DataLength > 2)) {
+      ASSERT (Data != NULL);
+      HubDesc = (EFI_USB_HUB_DESCRIPTOR *) Data;
+      ASSERT (HubDesc->NumPorts <= 15);
+      //
+      // The bit 5,6 of HubCharacter field of Hub Descriptor is TTT.
+      //
+      TTT = (UINT8) ((HubDesc->HubCharacter & (BIT5 | BIT6)) >> 5);
+      if (Xhc->UsbDevContext[SlotId].DevDesc.DeviceProtocol == 2) {
+        //
+        // Don't support multi-TT feature for super speed hub now.
+        //
+        MTT = 0;
+        DEBUG ((EFI_D_ERROR, "XHCI: Don't support multi-TT feature for Hub now. (force to disable MTT)\n"));
+      } else {
+        MTT = 0;
+      }
+
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiConfigHubContext (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      } else {
+        Status = XhcPeiConfigHubContext64 (Xhc, SlotId, HubDesc->NumPorts, TTT, MTT);
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_SET_CONFIG) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, USB_TARGET_DEVICE))) {
+    //
+    // Hook Set_Config request from UsbBus as we need configure device endpoint.
+    //
+    for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+      if (Xhc->UsbDevContext[SlotId].ConfDesc[Index]->ConfigurationValue == (UINT8)Request->Value) {
+        if (Xhc->HcCParams.Data.Csz == 0) {
+          Status = XhcPeiSetConfigCmd (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        } else {
+          Status = XhcPeiSetConfigCmd64 (Xhc, SlotId, DeviceSpeed, Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+        }
+        break;
+      }
+    }
+  } else if ((Request->Request     == USB_REQ_GET_STATUS) &&
+             (Request->RequestType == USB_REQUEST_TYPE (EfiUsbDataIn, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER))) {
+    ASSERT (Data != NULL);
+    //
+    // Hook Get_Status request from UsbBus to keep track of the port status change.
+    //
+    State                       = *(UINT32 *) Data;
+    PortStatus.PortStatus       = 0;
+    PortStatus.PortChangeStatus = 0;
+
+    if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+      //
+      // For super speed hub, its bit10~12 presents the attached device speed.
+      //
+      if ((State & XHC_PORTSC_PS) >> 10 == 0) {
+        PortStatus.PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      }
+    } else {
+      //
+      // For high or full/low speed hub, its bit9~10 presents the attached device speed.
+      //
+      if (XHC_BIT_IS_SET (State, BIT9)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      } else if (XHC_BIT_IS_SET (State, BIT10)) {
+        PortStatus.PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      }
+    }
+
+    //
+    // Convert the XHCI port/port change state to UEFI status
+    //
+    MapSize = sizeof (mUsbHubPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortStateMap[Index].HwState)) {
+        PortStatus.PortStatus = (UINT16) (PortStatus.PortStatus | mUsbHubPortStateMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubPortChangeMap[Index].HwState)) {
+        PortStatus.PortChangeStatus = (UINT16) (PortStatus.PortChangeStatus | mUsbHubPortChangeMap[Index].UefiState);
+      }
+    }
+
+    MapSize = sizeof (mUsbHubClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+    for (Index = 0; Index < MapSize; Index++) {
+      if (XHC_BIT_IS_SET (State, mUsbHubClearPortChangeMap[Index].HwState)) {
+        ZeroMem (&ClearPortRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+        ClearPortRequest.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_CLASS, USB_TARGET_OTHER);
+        ClearPortRequest.Request      = (UINT8) USB_REQ_CLEAR_FEATURE;
+        ClearPortRequest.Value        = mUsbHubClearPortChangeMap[Index].Selector;
+        ClearPortRequest.Index        = Request->Index;
+        ClearPortRequest.Length       = 0;
+
+        XhcPeiControlTransfer (
+          PeiServices,
+          This,
+          DeviceAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          &ClearPortRequest,
+          EfiUsbNoData,
+          NULL,
+          &Len,
+          TimeOut,
+          Translator,
+          TransferResult
+          );
+      }
+    }
+
+    XhcPeiPollPortStatusChange (Xhc, Xhc->UsbDevContext[SlotId].RouteString, (UINT8)Request->Index, &PortStatus);
+
+    *(UINT32 *) Data = *(UINT32 *) &PortStatus;
+  }
+
+FREE_URB:
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  DeviceAddress         Target device address.
+  @param  EndPointAddress       Endpoint number and its direction in bit 7.
+  @param  DeviceSpeed           Device speed, Low speed device doesn't support
+                                bulk transfer.
+  @param  MaximumPacketLength   Maximum packet size the endpoint is capable of
+                                sending or receiving.
+  @param  Data                  Array of pointers to the buffers of data to transmit
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  DataToggle            On input, the initial data toggle for the transfer;
+                                On output, it is updated to to next data toggle to use of
+                                the subsequent bulk transfer.
+  @param  TimeOut               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+                                If Timeout is 0, then the caller must wait for the function
+                                to be completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Translator            A pointr to the transaction translator data.
+  @param  TransferResult        A pointer to the detailed result information of the
+                                bulk transfer.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiBulkTransfer (
+  IN EFI_PEI_SERVICES                       **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI           *This,
+  IN UINT8                                  DeviceAddress,
+  IN UINT8                                  EndPointAddress,
+  IN UINT8                                  DeviceSpeed,
+  IN UINTN                                  MaximumPacketLength,
+  IN OUT VOID                               *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                              *DataLength,
+  IN OUT UINT8                              *DataToggle,
+  IN UINTN                                  TimeOut,
+  IN EFI_USB2_HC_TRANSACTION_TRANSLATOR     *Translator,
+  OUT UINT32                                *TransferResult
+  )
+{
+  PEI_XHC_DEV                   *Xhc;
+  URB                           *Urb;
+  UINT8                         SlotId;
+  EFI_STATUS                    Status;
+  EFI_STATUS                    RecoveryStatus;
+
+  //
+  // Validate the parameters
+  //
+  if ((DataLength == NULL) || (*DataLength == 0) ||
+      (Data == NULL) || (Data[0] == NULL) || (TransferResult == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*DataToggle != 0) && (*DataToggle != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DeviceSpeed == EFI_USB_SPEED_LOW) ||
+      ((DeviceSpeed == EFI_USB_SPEED_FULL) && (MaximumPacketLength > 64)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_HIGH) && (MaximumPacketLength > 512)) ||
+      ((DeviceSpeed == EFI_USB_SPEED_SUPER) && (MaximumPacketLength > 1024))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc             = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  *TransferResult = EFI_USB_ERR_SYSTEM;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Check if the device is still enabled before every transaction.
+  //
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, DeviceAddress);
+  if (SlotId == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, insert it into the asynchronous
+  // schedule list, then poll the execution status.
+  //
+  Urb = XhcPeiCreateUrb (
+          Xhc,
+          DeviceAddress,
+          EndPointAddress,
+          DeviceSpeed,
+          MaximumPacketLength,
+          XHC_BULK_TRANSFER,
+          NULL,
+          Data[0],
+          *DataLength,
+          NULL,
+          NULL
+          );
+
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status = XhcPeiExecTransfer (Xhc, FALSE, Urb, TimeOut);
+
+  *TransferResult = Urb->Result;
+  *DataLength     = Urb->Completed;
+
+  if (Status == EFI_TIMEOUT) {
+    //
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
+    //
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: error - %r, transfer - %x\n", Status, *TransferResult));
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices           The pointer to the PEI Services Table.
+  @param[in]  This                  The pointer to this instance of the
+                                    PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber            The pointer to the number of the root hub ports.
+
+  @retval EFI_SUCCESS               The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER     PortNumber is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiGetRootHubPortNumber (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  OUT UINT8                         *PortNumber
+  )
+{
+  PEI_XHC_DEV           *XhcDev;
+  XhcDev = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  if (PortNumber == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *PortNumber = XhcDev->HcSParams1.Data.MaxPorts;
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortNumber: PortNumber = %x\n", *PortNumber));
+  return EFI_SUCCESS;
+}
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber                Specifies the root hub port whose feature
+                                    is requested to be cleared.
+  @param  PortFeature               Indicates the feature selector associated with the
+                                    feature clear request.
+
+  @retval EFI_SUCCESS               The feature specified by PortFeature was cleared
+                                    for the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid or PortFeature is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiClearRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  PEI_XHC_DEV           *Xhc;
+  UINT32                Offset;
+  UINT32                State;
+  EFI_STATUS            Status;
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiClearRootHubPortFeature: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bits
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+    case EfiUsbPortEnable:
+      //
+      // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+      // A port may be disabled by software writing a '1' to this flag.
+      //
+      State |= XHC_PORTSC_PED;
+      State &= ~XHC_PORTSC_RESET;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortSuspend:
+      State |= XHC_PORTSC_LWS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      State &= ~XHC_PORTSC_PLS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortReset:
+      //
+      // PORTSC_RESET BIT(4) bit is RW1S attribute, which means Write-1-to-set status:
+      // Register bits indicate status when read, a clear bit may be set by
+      // writing a '1'. Writing a '0' to RW1S bits has no effect.
+      //
+      break;
+
+    case EfiUsbPortPower:
+      if (Xhc->HcCParams.Data.Ppc) {
+        //
+        // Port Power Control supported
+        //
+        State &= ~XHC_PORTSC_PP;
+        XhcPeiWriteOpReg (Xhc, Offset, State);
+      }
+      break;
+
+    case EfiUsbPortOwner:
+      //
+      // XHCI root hub port don't has the owner bit, ignore the operation
+      //
+      break;
+
+    case EfiUsbPortConnectChange:
+      //
+      // Clear connect status change
+      //
+      State |= XHC_PORTSC_CSC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortEnableChange:
+      //
+      // Clear enable status change
+      //
+      State |= XHC_PORTSC_PEC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortOverCurrentChange:
+      //
+      // Clear PortOverCurrent change
+      //
+      State |= XHC_PORTSC_OCC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortResetChange:
+      //
+      // Clear Port Reset change
+      //
+      State |= XHC_PORTSC_PRC;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortSuspendChange:
+      //
+      // Not supported or not related operation
+      //
+      break;
+
+    default:
+      Status = EFI_INVALID_PARAMETER;
+      break;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiClearRootHubPortFeature: PortFeature: %x Status = %r\n", PortFeature, Status));
+  return Status;
+}
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI
+  @param  PortNumber                Root hub port to set.
+  @param  PortFeature               Feature to set.
+
+  @retval EFI_SUCCESS               The feature specified by PortFeature was set.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid or PortFeature is invalid.
+  @retval EFI_TIMEOUT               The time out occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetRootHubPortFeature (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  )
+{
+  PEI_XHC_DEV           *Xhc;
+  UINT32                Offset;
+  UINT32                State;
+  EFI_STATUS            Status;
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+  Status = EFI_SUCCESS;
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Offset = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiSetRootHubPortFeature: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // Mask off the port status change bits, these bits are
+  // write clean bits
+  //
+  State &= ~ (BIT1 | BIT17 | BIT18 | BIT19 | BIT20 | BIT21 | BIT22 | BIT23);
+
+  switch (PortFeature) {
+    case EfiUsbPortEnable:
+      //
+      // Ports may only be enabled by the xHC. Software cannot enable a port by writing a '1' to this flag.
+      // A port may be disabled by software writing a '1' to this flag.
+      //
+      break;
+
+    case EfiUsbPortSuspend:
+      State |= XHC_PORTSC_LWS;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      State &= ~XHC_PORTSC_PLS;
+      State |= (3 << 5) ;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      break;
+
+    case EfiUsbPortReset:
+      //
+      // Make sure Host Controller not halt before reset it
+      //
+      if (XhcPeiIsHalt (Xhc)) {
+        Status = XhcPeiRunHC (Xhc, XHC_GENERIC_TIMEOUT);
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+      }
+
+      //
+      // 4.3.1 Resetting a Root Hub Port
+      // 1) Write the PORTSC register with the Port Reset (PR) bit set to '1'.
+      // 2) Wait for a successful Port Status Change Event for the port, where the Port Reset Change (PRC)
+      //    bit in the PORTSC field is set to '1'.
+      //
+      State |= XHC_PORTSC_RESET;
+      XhcPeiWriteOpReg (Xhc, Offset, State);
+      XhcPeiWaitOpRegBit(Xhc, Offset, XHC_PORTSC_PRC, TRUE, XHC_GENERIC_TIMEOUT);
+      break;
+
+    case EfiUsbPortPower:
+      if (Xhc->HcCParams.Data.Ppc) {
+        //
+        // Port Power Control supported
+        //
+        State |= XHC_PORTSC_PP;
+        XhcPeiWriteOpReg (Xhc, Offset, State);
+      }
+      break;
+
+    case EfiUsbPortOwner:
+      //
+      // XHCI root hub port don't has the owner bit, ignore the operation
+      //
+      break;
+
+    default:
+      Status = EFI_INVALID_PARAMETER;
+  }
+
+ON_EXIT:
+  DEBUG ((EFI_D_INFO, "XhcPeiSetRootHubPortFeature: PortFeature: %x Status = %r\n", PortFeature, Status));
+  return Status;
+}
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param  PeiServices               The pointer of EFI_PEI_SERVICES.
+  @param  This                      The pointer of PEI_USB2_HOST_CONTROLLER_PPI.
+  @param  PortNumber                The root hub port to retrieve the state from.
+  @param  PortStatus                Variable to receive the port state.
+
+  @retval EFI_SUCCESS               The status of the USB root hub port specified.
+                                    by PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER     PortNumber is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiGetRootHubPortStatus (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  )
+{
+  PEI_XHC_DEV               *Xhc;
+  UINT32                    Offset;
+  UINT32                    State;
+  UINTN                     Index;
+  UINTN                     MapSize;
+  USB_DEV_ROUTE             ParentRouteChart;
+
+  if (PortStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Xhc = PEI_RECOVERY_USB_XHC_DEV_FROM_THIS (This);
+
+  if (PortNumber >= Xhc->HcSParams1.Data.MaxPorts) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Clear port status.
+  //
+  PortStatus->PortStatus        = 0;
+  PortStatus->PortChangeStatus  = 0;
+
+  Offset                        = (UINT32) (XHC_PORTSC_OFFSET + (0x10 * PortNumber));
+  State                         = XhcPeiReadOpReg (Xhc, Offset);
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortStatus: Port: %x State: %x\n", PortNumber, State));
+
+  //
+  // According to XHCI 1.0 spec, bit 10~13 of the root port status register identifies the speed of the attached device.
+  //
+  switch ((State & XHC_PORTSC_PS) >> 10) {
+    case 2:
+      PortStatus->PortStatus |= USB_PORT_STAT_LOW_SPEED;
+      break;
+
+    case 3:
+      PortStatus->PortStatus |= USB_PORT_STAT_HIGH_SPEED;
+      break;
+
+    case 4:
+      PortStatus->PortStatus |= USB_PORT_STAT_SUPER_SPEED;
+      break;
+
+    default:
+      break;
+  }
+
+  //
+  // Convert the XHCI port/port change state to UEFI status
+  //
+  MapSize = sizeof (mUsbPortStateMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortStateMap[Index].HwState)) {
+      PortStatus->PortStatus = (UINT16) (PortStatus->PortStatus | mUsbPortStateMap[Index].UefiState);
+    }
+  }
+  //
+  // Bit5~8 reflects its current link state.
+  //
+  if ((State & XHC_PORTSC_PLS) >> 5 == 3) {
+    PortStatus->PortStatus |= USB_PORT_STAT_SUSPEND;
+  }
+
+  MapSize = sizeof (mUsbPortChangeMap) / sizeof (USB_PORT_STATE_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbPortChangeMap[Index].HwState)) {
+      PortStatus->PortChangeStatus = (UINT16) (PortStatus->PortChangeStatus | mUsbPortChangeMap[Index].UefiState);
+    }
+  }
+
+  MapSize = sizeof (mUsbClearPortChangeMap) / sizeof (USB_CLEAR_PORT_MAP);
+
+  for (Index = 0; Index < MapSize; Index++) {
+    if (XHC_BIT_IS_SET (State, mUsbClearPortChangeMap[Index].HwState)) {
+      XhcPeiClearRootHubPortFeature (PeiServices, This, PortNumber, (EFI_USB_PORT_FEATURE)mUsbClearPortChangeMap[Index].Selector);
+    }
+  }
+
+  //
+  // Poll the root port status register to enable/disable corresponding device slot if there is a device attached/detached.
+  // For those devices behind hub, we get its attach/detach event by hooking Get_Port_Status request at control transfer for those hub.
+  //
+  ParentRouteChart.Dword = 0;
+  XhcPeiPollPortStatusChange (Xhc, ParentRouteChart, PortNumber, PortStatus);
+
+  DEBUG ((EFI_D_INFO, "XhcPeiGetRootHubPortStatus: PortChangeStatus: %x PortStatus: %x\n", PortStatus->PortChangeStatus, PortStatus->PortStatus));
+  return EFI_SUCCESS;
+}
+
+/**
+  @param FileHandle     Handle of the file being invoked.
+  @param PeiServices    Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS   PPI successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeimEntry (
+  IN EFI_PEI_FILE_HANDLE    FileHandle,
+  IN CONST EFI_PEI_SERVICES **PeiServices
+  )
+{
+  PEI_USB_CONTROLLER_PPI      *UsbControllerPpi;
+  EFI_STATUS                  Status;
+  UINT8                       Index;
+  UINTN                       ControllerType;
+  UINTN                       BaseAddress;
+  UINTN                       MemPages;
+  PEI_XHC_DEV                 *XhcDev;
+  EFI_PHYSICAL_ADDRESS        TempPtr;
+  UINT32                      PageSize;
+
+  //
+  // Shadow this PEIM to run from memory.
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  Status = PeiServicesLocatePpi (
+             &gPeiUsbControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &UsbControllerPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Index = 0;
+  while (TRUE) {
+    Status = UsbControllerPpi->GetUsbController (
+                                 (EFI_PEI_SERVICES **) PeiServices,
+                                 UsbControllerPpi,
+                                 Index,
+                                 &ControllerType,
+                                 &BaseAddress
+                                 );
+    //
+    // When status is error, it means no controller is found.
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    //
+    // This PEIM is for XHC type controller.
+    //
+    if (ControllerType != PEI_XHCI_CONTROLLER) {
+      Index++;
+      continue;
+    }
+
+    MemPages = EFI_SIZE_TO_PAGES (sizeof (PEI_XHC_DEV));
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               MemPages,
+               &TempPtr
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    ZeroMem ((VOID *) (UINTN) TempPtr, EFI_PAGES_TO_SIZE (MemPages));
+    XhcDev = (PEI_XHC_DEV *) ((UINTN) TempPtr);
+
+    XhcDev->Signature = USB_XHC_DEV_SIGNATURE;
+    XhcDev->UsbHostControllerBaseAddress = (UINT32) BaseAddress;
+    XhcDev->CapLength           = (UINT8) (XhcPeiReadCapRegister (XhcDev, XHC_CAPLENGTH_OFFSET) & 0x0FF);
+    XhcDev->HcSParams1.Dword    = XhcPeiReadCapRegister (XhcDev, XHC_HCSPARAMS1_OFFSET);
+    XhcDev->HcSParams2.Dword    = XhcPeiReadCapRegister (XhcDev, XHC_HCSPARAMS2_OFFSET);
+    XhcDev->HcCParams.Dword     = XhcPeiReadCapRegister (XhcDev, XHC_HCCPARAMS_OFFSET);
+    XhcDev->DBOff               = XhcPeiReadCapRegister (XhcDev, XHC_DBOFF_OFFSET);
+    XhcDev->RTSOff              = XhcPeiReadCapRegister (XhcDev, XHC_RTSOFF_OFFSET);
+
+    //
+    // This PageSize field defines the page size supported by the xHC implementation.
+    // This xHC supports a page size of 2^(n+12) if bit n is Set. For example,
+    // if bit 0 is Set, the xHC supports 4k byte page sizes.
+    //
+    PageSize         = XhcPeiReadOpReg (XhcDev, XHC_PAGESIZE_OFFSET) & XHC_PAGESIZE_MASK;
+    XhcDev->PageSize = 1 << (HighBitSet32 (PageSize) + 12);
+
+    DEBUG ((EFI_D_INFO, "XhciPei: UsbHostControllerBaseAddress: %x\n", XhcDev->UsbHostControllerBaseAddress));
+    DEBUG ((EFI_D_INFO, "XhciPei: CapLength:                    %x\n", XhcDev->CapLength));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcSParams1:                   %x\n", XhcDev->HcSParams1.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcSParams2:                   %x\n", XhcDev->HcSParams2.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: HcCParams:                    %x\n", XhcDev->HcCParams.Dword));
+    DEBUG ((EFI_D_INFO, "XhciPei: DBOff:                        %x\n", XhcDev->DBOff));
+    DEBUG ((EFI_D_INFO, "XhciPei: RTSOff:                       %x\n", XhcDev->RTSOff));
+    DEBUG ((EFI_D_INFO, "XhciPei: PageSize:                     %x\n", XhcDev->PageSize));
+
+    XhcPeiResetHC (XhcDev, XHC_RESET_TIMEOUT);
+    ASSERT (XhcPeiIsHalt (XhcDev));
+
+    //
+    // Initialize the schedule
+    //
+    XhcPeiInitSched (XhcDev);
+
+    //
+    // Start the Host Controller
+    //
+    XhcPeiRunHC (XhcDev, XHC_GENERIC_TIMEOUT);
+
+    //
+    // Wait for root port state stable
+    //
+    MicroSecondDelay (XHC_ROOT_PORT_STATE_STABLE);
+
+    XhcDev->Usb2HostControllerPpi.ControlTransfer           = XhcPeiControlTransfer;
+    XhcDev->Usb2HostControllerPpi.BulkTransfer              = XhcPeiBulkTransfer;
+    XhcDev->Usb2HostControllerPpi.GetRootHubPortNumber      = XhcPeiGetRootHubPortNumber;
+    XhcDev->Usb2HostControllerPpi.GetRootHubPortStatus      = XhcPeiGetRootHubPortStatus;
+    XhcDev->Usb2HostControllerPpi.SetRootHubPortFeature     = XhcPeiSetRootHubPortFeature;
+    XhcDev->Usb2HostControllerPpi.ClearRootHubPortFeature   = XhcPeiClearRootHubPortFeature;
+
+    XhcDev->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+    XhcDev->PpiDescriptor.Guid = &gPeiUsb2HostControllerPpiGuid;
+    XhcDev->PpiDescriptor.Ppi = &XhcDev->Usb2HostControllerPpi;
+
+    PeiServicesInstallPpi (&XhcDev->PpiDescriptor);
+
+    Index++;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h
new file mode 100644
index 0000000000..ccf4dc26e5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.h
@@ -0,0 +1,239 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014 - 2015, 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.
+
+**/
+
+#ifndef _RECOVERY_XHC_H_
+#define _RECOVERY_XHC_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbController.h>
+#include <Ppi/Usb2HostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/IoLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+typedef struct _PEI_XHC_DEV PEI_XHC_DEV;
+typedef struct _USB_DEV_CONTEXT USB_DEV_CONTEXT;
+
+#include "UsbHcMem.h"
+#include "XhciReg.h"
+#include "XhciSched.h"
+
+#define CMD_RING_TRB_NUMBER         0x100
+#define TR_RING_TRB_NUMBER          0x100
+#define ERST_NUMBER                 0x01
+#define EVENT_RING_TRB_NUMBER       0x200
+
+#define XHC_1_MICROSECOND           1
+#define XHC_1_MILLISECOND           (1000 * XHC_1_MICROSECOND)
+#define XHC_1_SECOND                (1000 * XHC_1_MILLISECOND)
+
+//
+// XHC reset timeout experience values.
+// The unit is millisecond, setting it as 1s.
+//
+#define XHC_RESET_TIMEOUT           (1000)
+
+//
+// Wait for root port state stable.
+//
+#define XHC_ROOT_PORT_STATE_STABLE  (200 * XHC_1_MILLISECOND)
+
+//
+// XHC generic timeout experience values.
+// The unit is millisecond, setting it as 10s.
+//
+#define XHC_GENERIC_TIMEOUT         (10 * 1000)
+
+#define XHC_LOW_32BIT(Addr64)       ((UINT32)(((UINTN)(Addr64)) & 0XFFFFFFFF))
+#define XHC_HIGH_32BIT(Addr64)      ((UINT32)(RShiftU64((UINTN)(Addr64), 32) & 0XFFFFFFFF))
+#define XHC_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define XHC_REG_BIT_IS_SET(XHC, Offset, Bit) \
+          (XHC_BIT_IS_SET(XhcPeiReadOpReg ((XHC), (Offset)), (Bit)))
+
+#define USB_DESC_TYPE_HUB              0x29
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+struct _USB_DEV_CONTEXT {
+  //
+  // Whether this entry in UsbDevContext array is used or not.
+  //
+  BOOLEAN                           Enabled;
+  //
+  // The slot id assigned to the new device through XHCI's Enable_Slot cmd.
+  //
+  UINT8                             SlotId;
+  //
+  // The route string presented an attached usb device.
+  //
+  USB_DEV_ROUTE                     RouteString;
+  //
+  // The route string of parent device if it exists. Otherwise it's zero.
+  //
+  USB_DEV_ROUTE                     ParentRouteString;
+  //
+  // The actual device address assigned by XHCI through Address_Device command.
+  //
+  UINT8                             XhciDevAddr;
+  //
+  // The requested device address from UsbBus driver through Set_Address standard usb request.
+  // As XHCI spec replaces this request with Address_Device command, we have to record the
+  // requested device address and establish a mapping relationship with the actual device address.
+  // Then UsbBus driver just need to be aware of the requested device address to access usb device
+  // through EFI_USB2_HC_PROTOCOL. Xhci driver would be responsible for translating it to actual
+  // device address and access the actual device.
+  //
+  UINT8                             BusDevAddr;
+  //
+  // The pointer to the input device context.
+  //
+  VOID                              *InputContext;
+  //
+  // The pointer to the output device context.
+  //
+  VOID                              *OutputContext;
+  //
+  // The transfer queue for every endpoint.
+  //
+  VOID                              *EndpointTransferRing[31];
+  //
+  // The device descriptor which is stored to support XHCI's Evaluate_Context cmd.
+  //
+  EFI_USB_DEVICE_DESCRIPTOR         DevDesc;
+  //
+  // As a usb device may include multiple configuration descriptors, we dynamically allocate an array
+  // to store them.
+  // Note that every configuration descriptor stored here includes those lower level descriptors,
+  // such as Interface descriptor, Endpoint descriptor, and so on.
+  // These information is used to support XHCI's Config_Endpoint cmd.
+  //
+  EFI_USB_CONFIG_DESCRIPTOR         **ConfDesc;
+};
+
+#define USB_XHC_DEV_SIGNATURE       SIGNATURE_32 ('x', 'h', 'c', 'i')
+
+struct _PEI_XHC_DEV {
+  UINTN                             Signature;
+  PEI_USB2_HOST_CONTROLLER_PPI      Usb2HostControllerPpi;
+  EFI_PEI_PPI_DESCRIPTOR            PpiDescriptor;
+  UINT32                            UsbHostControllerBaseAddress;
+  USBHC_MEM_POOL                    *MemPool;
+
+  //
+  // XHCI configuration data
+  //
+  UINT8                             CapLength;    ///< Capability Register Length
+  XHC_HCSPARAMS1                    HcSParams1;   ///< Structural Parameters 1
+  XHC_HCSPARAMS2                    HcSParams2;   ///< Structural Parameters 2
+  XHC_HCCPARAMS                     HcCParams;    ///< Capability Parameters
+  UINT32                            DBOff;        ///< Doorbell Offset
+  UINT32                            RTSOff;       ///< Runtime Register Space Offset
+  UINT32                            PageSize;
+  UINT32                            MaxScratchpadBufs;
+  UINT64                            *ScratchBuf;
+  UINT64                            *ScratchEntry;
+  UINT64                            *DCBAA;
+  UINT32                            MaxSlotsEn;
+  //
+  // Cmd Transfer Ring
+  //
+  TRANSFER_RING                     CmdRing;
+  //
+  // EventRing
+  //
+  EVENT_RING                        EventRing;
+
+  //
+  // Store device contexts managed by XHCI device
+  // The array supports up to 255 devices, entry 0 is reserved and should not be used.
+  //
+  USB_DEV_CONTEXT                   UsbDevContext[256];
+};
+
+#define PEI_RECOVERY_USB_XHC_DEV_FROM_THIS(a) CR (a, PEI_XHC_DEV, Usb2HostControllerPpi, USB_XHC_DEV_SIGNATURE)
+
+/**
+  Initialize the memory management pool for the host controller.
+
+  @return Pointer to the allocated memory pool or NULL if failed.
+
+**/
+USBHC_MEM_POOL *
+UsbHcInitMemPool (
+  VOID
+  )
+;
+
+/**
+  Release the memory management pool.
+
+  @param  Pool          The USB memory pool to free.
+
+**/
+VOID
+UsbHcFreeMemPool (
+  IN USBHC_MEM_POOL     *Pool
+  )
+;
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+
+  @param  Pool          The host controller's memory pool.
+  @param  Size          Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UsbHcAllocateMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN UINTN              Size
+  )
+;
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool          The memory pool of the host controller.
+  @param  Mem           The memory to free.
+  @param  Size          The size of the memory to free.
+
+**/
+VOID
+UsbHcFreeMem (
+  IN USBHC_MEM_POOL     *Pool,
+  IN VOID               *Mem,
+  IN UINTN              Size
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf
new file mode 100644
index 0000000000..dc65f283bc
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf
@@ -0,0 +1,64 @@
+## @file
+# The XhcPeim driver is responsible for managing the behavior of XHCI controller at PEI phase.
+#
+# It produces gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+# which is used to enable recovery function from USB Drivers.
+#
+# Copyright (c) 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = XhciPei
+  MODULE_UNI_FILE                = XhciPei.uni
+  FILE_GUID                      = 65E5746E-9C14-467d-B5B3-932A66D59F79
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = XhcPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  XhcPeim.c
+  XhcPeim.h
+  XhciSched.c
+  UsbHcMem.c
+  XhciReg.h
+  XhciSched.h
+  UsbHcMem.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+  MemoryAllocationLib
+
+[Ppis]
+  gPeiUsb2HostControllerPpiGuid                 ## PRODUCES
+  gPeiUsbControllerPpiGuid                      ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbControllerPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  XhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni
new file mode 100644
index 0000000000..451007efb6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPei.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni
new file mode 100644
index 0000000000..d977f2f8c2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h
new file mode 100644
index 0000000000..1a62560665
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciReg.h
@@ -0,0 +1,471 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_REG_H_
+#define _EFI_PEI_XHCI_REG_H_
+
+//
+// Capability registers offset
+//
+#define XHC_CAPLENGTH_OFFSET            0x00    // Capability register length offset
+#define XHC_HCIVERSION_OFFSET           0x02    // Interface Version Number 02-03h
+#define XHC_HCSPARAMS1_OFFSET           0x04    // Structural Parameters 1
+#define XHC_HCSPARAMS2_OFFSET           0x08    // Structural Parameters 2
+#define XHC_HCSPARAMS3_OFFSET           0x0c    // Structural Parameters 3
+#define XHC_HCCPARAMS_OFFSET            0x10    // Capability Parameters
+#define XHC_DBOFF_OFFSET                0x14    // Doorbell Offset
+#define XHC_RTSOFF_OFFSET               0x18    // Runtime Register Space Offset
+
+//
+// Operational registers offset
+//
+#define XHC_USBCMD_OFFSET               0x0000  // USB Command Register Offset
+#define XHC_USBSTS_OFFSET               0x0004  // USB Status Register Offset
+#define XHC_PAGESIZE_OFFSET             0x0008  // USB Page Size Register Offset
+#define XHC_DNCTRL_OFFSET               0x0014  // Device Notification Control Register Offset
+#define XHC_CRCR_OFFSET                 0x0018  // Command Ring Control Register Offset
+#define XHC_DCBAAP_OFFSET               0x0030  // Device Context Base Address Array Pointer Register Offset
+#define XHC_CONFIG_OFFSET               0x0038  // Configure Register Offset
+#define XHC_PORTSC_OFFSET               0x0400  // Port Status and Control Register Offset
+
+//
+// Runtime registers offset
+//
+#define XHC_MFINDEX_OFFSET              0x00    // Microframe Index Register Offset
+#define XHC_IMAN_OFFSET                 0x20    // Interrupter X Management Register Offset
+#define XHC_IMOD_OFFSET                 0x24    // Interrupter X Moderation Register Offset
+#define XHC_ERSTSZ_OFFSET               0x28    // Event Ring Segment Table Size Register Offset
+#define XHC_ERSTBA_OFFSET               0x30    // Event Ring Segment Table Base Address Register Offset
+#define XHC_ERDP_OFFSET                 0x38    // Event Ring Dequeue Pointer Register Offset
+
+//
+// Register Bit Definition
+//
+#define XHC_USBCMD_RUN                  BIT0    // Run/Stop
+#define XHC_USBCMD_RESET                BIT1    // Host Controller Reset
+#define XHC_USBCMD_INTE                 BIT2    // Interrupter Enable
+#define XHC_USBCMD_HSEE                 BIT3    // Host System Error Enable
+
+#define XHC_USBSTS_HALT                 BIT0    // Host Controller Halted
+#define XHC_USBSTS_HSE                  BIT2    // Host System Error
+#define XHC_USBSTS_EINT                 BIT3    // Event Interrupt
+#define XHC_USBSTS_PCD                  BIT4    // Port Change Detect
+#define XHC_USBSTS_SSS                  BIT8    // Save State Status
+#define XHC_USBSTS_RSS                  BIT9    // Restore State Status
+#define XHC_USBSTS_SRE                  BIT10   // Save/Restore Error
+#define XHC_USBSTS_CNR                  BIT11   // Host Controller Not Ready
+#define XHC_USBSTS_HCE                  BIT12   // Host Controller Error
+
+#define XHC_PAGESIZE_MASK               0xFFFF  // Page Size
+
+#define XHC_CRCR_RCS                    BIT0    // Ring Cycle State
+#define XHC_CRCR_CS                     BIT1    // Command Stop
+#define XHC_CRCR_CA                     BIT2    // Command Abort
+#define XHC_CRCR_CRR                    BIT3    // Command Ring Running
+
+#define XHC_CONFIG_MASK                 0xFF    // Max Device Slots Enabled
+
+#define XHC_PORTSC_CCS                  BIT0    // Current Connect Status
+#define XHC_PORTSC_PED                  BIT1    // Port Enabled/Disabled
+#define XHC_PORTSC_OCA                  BIT3    // Over-current Active
+#define XHC_PORTSC_RESET                BIT4    // Port Reset
+#define XHC_PORTSC_PLS                  (BIT5|BIT6|BIT7|BIT8)   // Port Link State
+#define XHC_PORTSC_PP                   BIT9    // Port Power
+#define XHC_PORTSC_PS                   (BIT10|BIT11|BIT12)     // Port Speed
+#define XHC_PORTSC_LWS                  BIT16   // Port Link State Write Strobe
+#define XHC_PORTSC_CSC                  BIT17   // Connect Status Change
+#define XHC_PORTSC_PEC                  BIT18   // Port Enabled/Disabled Change
+#define XHC_PORTSC_WRC                  BIT19   // Warm Port Reset Change
+#define XHC_PORTSC_OCC                  BIT20   // Over-Current Change
+#define XHC_PORTSC_PRC                  BIT21   // Port Reset Change
+#define XHC_PORTSC_PLC                  BIT22   // Port Link State Change
+#define XHC_PORTSC_CEC                  BIT23   // Port Config Error Change
+#define XHC_PORTSC_CAS                  BIT24   // Cold Attach Status
+
+#define XHC_HUB_PORTSC_CCS              BIT0    // Hub's Current Connect Status
+#define XHC_HUB_PORTSC_PED              BIT1    // Hub's Port Enabled/Disabled
+#define XHC_HUB_PORTSC_OCA              BIT3    // Hub's Over-current Active
+#define XHC_HUB_PORTSC_RESET            BIT4    // Hub's Port Reset
+#define XHC_HUB_PORTSC_PP               BIT9    // Hub's Port Power
+#define XHC_HUB_PORTSC_CSC              BIT16   // Hub's Connect Status Change
+#define XHC_HUB_PORTSC_PEC              BIT17   // Hub's Port Enabled/Disabled Change
+#define XHC_HUB_PORTSC_OCC              BIT19   // Hub's Over-Current Change
+#define XHC_HUB_PORTSC_PRC              BIT20   // Hub's Port Reset Change
+#define XHC_HUB_PORTSC_BHRC             BIT21   // Hub's Port Warm Reset Change
+
+#define XHC_IMAN_IP                     BIT0    // Interrupt Pending
+#define XHC_IMAN_IE                     BIT1    // Interrupt Enable
+
+#define XHC_IMODI_MASK                  0x0000FFFF  // Interrupt Moderation Interval
+#define XHC_IMODC_MASK                  0xFFFF0000  // Interrupt Moderation Counter
+
+
+#pragma pack (1)
+typedef struct {
+  UINT8                 MaxSlots;       // Number of Device Slots
+  UINT16                MaxIntrs:11;    // Number of Interrupters
+  UINT16                Rsvd:5;
+  UINT8                 MaxPorts;       // Number of Ports
+} HCSPARAMS1;
+
+//
+// Structural Parameters 1 Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCSPARAMS1            Data;
+} XHC_HCSPARAMS1;
+
+typedef struct {
+  UINT32                Ist:4;          // Isochronous Scheduling Threshold
+  UINT32                Erst:4;         // Event Ring Segment Table Max
+  UINT32                Rsvd:13;
+  UINT32                ScratchBufHi:5; // Max Scratchpad Buffers Hi
+  UINT32                Spr:1;          // Scratchpad Restore
+  UINT32                ScratchBufLo:5; // Max Scratchpad Buffers Lo
+} HCSPARAMS2;
+
+//
+// Structural Parameters 2 Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCSPARAMS2            Data;
+} XHC_HCSPARAMS2;
+
+typedef struct {
+  UINT16                Ac64:1;        // 64-bit Addressing Capability
+  UINT16                Bnc:1;         // BW Negotiation Capability
+  UINT16                Csz:1;         // Context Size
+  UINT16                Ppc:1;         // Port Power Control
+  UINT16                Pind:1;        // Port Indicators
+  UINT16                Lhrc:1;        // Light HC Reset Capability
+  UINT16                Ltc:1;         // Latency Tolerance Messaging Capability
+  UINT16                Nss:1;         // No Secondary SID Support
+  UINT16                Pae:1;         // Parse All Event Data
+  UINT16                Rsvd:3;
+  UINT16                MaxPsaSize:4;  // Maximum Primary Stream Array Size
+  UINT16                ExtCapReg;     // xHCI Extended Capabilities Pointer
+} HCCPARAMS;
+
+//
+// Capability Parameters Register Bitmap Definition
+//
+typedef union {
+  UINT32                Dword;
+  HCCPARAMS             Data;
+} XHC_HCCPARAMS;
+
+#pragma pack ()
+
+//
+// XHCi Data and Ctrl Structures
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Pi;
+  UINT8                   SubClassCode;
+  UINT8                   BaseCode;
+} USB_CLASSC;
+
+typedef struct {
+  UINT8                     Length;
+  UINT8                     DescType;
+  UINT8                     NumPorts;
+  UINT16                    HubCharacter;
+  UINT8                     PwrOn2PwrGood;
+  UINT8                     HubContrCurrent;
+  UINT8                     Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+#pragma pack()
+
+//
+//  Hub Class Feature Selector for Clear Port Feature Request
+//  It's the extension of hub class feature selector of USB 2.0 in USB 3.0 Spec.
+//  For more details, Please refer to USB 3.0 Spec Table 10-7.
+//
+typedef enum {
+  Usb3PortBHPortReset          = 28,
+  Usb3PortBHPortResetChange    = 29
+} XHC_PORT_FEATURE;
+
+//
+// Structure to map the hardware port states to the
+// UEFI's port states.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  UefiState;
+} USB_PORT_STATE_MAP;
+
+//
+// Structure to map the hardware port states to feature selector for clear port feature request.
+//
+typedef struct {
+  UINT32                  HwState;
+  UINT16                  Selector;
+} USB_CLEAR_PORT_MAP;
+
+/**
+  Read XHCI Operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+
+  @retval the register content read.
+
+**/
+UINT32
+XhcPeiReadOpReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI operation register.
+
+  @param Xhc            The XHCI device.
+  @param Offset         The operation register offset.
+  @param Data           The data to write.
+
+**/
+VOID
+XhcPeiWriteOpReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Set one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Clear one bit of the operational register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit mask of the register to clear.
+
+**/
+VOID
+XhcPeiClearOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Wait the operation register's bit as specified by Bit
+  to be set (or clear).
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the operational register.
+  @param  Bit           The bit of the register to wait for.
+  @param  WaitToSet     Wait the bit to set or clear.
+  @param  Timeout       The time to wait before abort (in microsecond, us).
+
+  @retval EFI_SUCCESS   The bit successfully changed by host controller.
+  @retval EFI_TIMEOUT   The time out occurred.
+
+**/
+EFI_STATUS
+XhcPeiWaitOpRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit,
+  IN BOOLEAN            WaitToSet,
+  IN UINT32             Timeout
+  );
+
+/**
+  Read XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadDoorBellReg (
+  IN  PEI_XHC_DEV       *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI door bell register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the door bell register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteDoorBellReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Read XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+
+  @return The register content read
+
+**/
+UINT32
+XhcPeiReadRuntimeReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN  UINT32            Offset
+  );
+
+/**
+  Write the data to the XHCI runtime register.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Data          The data to write.
+
+**/
+VOID
+XhcPeiWriteRuntimeReg (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Data
+  );
+
+/**
+  Set one bit of the runtime register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiSetRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Clear one bit of the runtime register while keeping other bits.
+
+  @param  Xhc           The XHCI device.
+  @param  Offset        The offset of the runtime register.
+  @param  Bit           The bit mask of the register to set.
+
+**/
+VOID
+XhcPeiClearRuntimeRegBit (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Offset,
+  IN UINT32             Bit
+  );
+
+/**
+  Check whether Xhc is halted.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          The controller is halted.
+  @retval FALSE         The controller isn't halted.
+
+**/
+BOOLEAN
+XhcPeiIsHalt (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Check whether system error occurred.
+
+  @param  Xhc           The XHCI device.
+
+  @retval TRUE          System error happened.
+  @retval FALSE         No system error.
+
+**/
+BOOLEAN
+XhcPeiIsSysError (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Reset the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort (in millisecond, ms).
+
+  @retval EFI_TIMEOUT   The transfer failed due to time out.
+  @retval Others        Failed to reset the host.
+
+**/
+EFI_STATUS
+XhcPeiResetHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+/**
+  Halt the host controller.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_TIMEOUT   Failed to halt the controller before Timeout.
+  @retval EFI_SUCCESS   The XHCI is halt.
+
+**/
+EFI_STATUS
+XhcPeiHaltHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+/**
+  Set the XHCI to run.
+
+  @param  Xhc           The XHCI device.
+  @param  Timeout       Time to wait before abort.
+
+  @retval EFI_SUCCESS   The XHCI is running.
+  @retval Others        Failed to set the XHCI to run.
+
+**/
+EFI_STATUS
+XhcPeiRunHC (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT32             Timeout
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c
new file mode 100644
index 0000000000..3632e8a769
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c
@@ -0,0 +1,2963 @@
+/** @file
+PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
+which is used to enable recovery function from USB Drivers.
+
+Copyright (c) 2014 - 2015, 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 "XhcPeim.h"
+
+/**
+  Create a command transfer TRB to support XHCI command interfaces.
+
+  @param  Xhc       The XHCI device.
+  @param  CmdTrb    The cmd TRB to be executed.
+
+  @return Created URB or NULL.
+
+**/
+URB*
+XhcPeiCreateCmdTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRB_TEMPLATE   *CmdTrb
+  )
+{
+  URB   *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature  = XHC_URB_SIG;
+
+  Urb->Ring       = &Xhc->CmdRing;
+  XhcPeiSyncTrsRing (Xhc, Urb->Ring);
+  Urb->TrbNum     = 1;
+  Urb->TrbStart   = Urb->Ring->RingEnqueue;
+  CopyMem (Urb->TrbStart, CmdTrb, sizeof (TRB_TEMPLATE));
+  Urb->TrbStart->CycleBit = Urb->Ring->RingPCS & BIT0;
+  Urb->TrbEnd             = Urb->TrbStart;
+
+  return Urb;
+}
+
+/**
+  Execute a XHCI cmd TRB pointed by CmdTrb.
+
+  @param  Xhc                   The XHCI device.
+  @param  CmdTrb                The cmd TRB to be executed.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete.
+  @param  EvtTrb                The event TRB corresponding to the cmd TRB.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+XhcPeiCmdTransfer (
+  IN PEI_XHC_DEV                *Xhc,
+  IN TRB_TEMPLATE               *CmdTrb,
+  IN UINTN                      Timeout,
+  OUT TRB_TEMPLATE              **EvtTrb
+  )
+{
+  EFI_STATUS    Status;
+  URB           *Urb;
+
+  //
+  // Validate the parameters
+  //
+  if ((Xhc == NULL) || (CmdTrb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_DEVICE_ERROR;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCmdTransfer: HC is halted or has system error\n"));
+    goto ON_EXIT;
+  }
+
+  //
+  // Create a new URB, then poll the execution status.
+  //
+  Urb = XhcPeiCreateCmdTrb (Xhc, CmdTrb);
+  if (Urb == NULL) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCmdTransfer: failed to create URB\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Status  = XhcPeiExecTransfer (Xhc, TRUE, Urb, Timeout);
+  *EvtTrb = Urb->EvtTrb;
+
+  if (Urb->Result == EFI_USB_NOERROR) {
+    Status = EFI_SUCCESS;
+  }
+
+  XhcPeiFreeUrb (Xhc, Urb);
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI device
+  @param  BusAddr   The logical device address assigned by UsbBus driver
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcPeiCreateUrb (
+  IN PEI_XHC_DEV                        *Xhc,
+  IN UINT8                              BusAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  )
+{
+  USB_ENDPOINT      *Ep;
+  EFI_STATUS        Status;
+  URB               *Urb;
+
+  Urb = AllocateZeroPool (sizeof (URB));
+  if (Urb == NULL) {
+    return NULL;
+  }
+
+  Urb->Signature = XHC_URB_SIG;
+
+  Ep            = &Urb->Ep;
+  Ep->BusAddr   = BusAddr;
+  Ep->EpAddr    = (UINT8) (EpAddr & 0x0F);
+  Ep->Direction = ((EpAddr & 0x80) != 0) ? EfiUsbDataIn : EfiUsbDataOut;
+  Ep->DevSpeed  = DevSpeed;
+  Ep->MaxPacket = MaxPacket;
+  Ep->Type      = Type;
+
+  Urb->Request  = Request;
+  Urb->Data     = Data;
+  Urb->DataLen  = DataLen;
+  Urb->Callback = Callback;
+  Urb->Context  = Context;
+
+  Status = XhcPeiCreateTransferTrb (Xhc, Urb);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiCreateUrb: XhcPeiCreateTransferTrb Failed, Status = %r\n", Status));
+    FreePool (Urb);
+    Urb = NULL;
+  }
+
+  return Urb;
+}
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc       The XHCI device.
+  @param  Urb       The URB to free.
+
+**/
+VOID
+XhcPeiFreeUrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  )
+{
+  if ((Xhc == NULL) || (Urb == NULL)) {
+    return;
+  }
+
+  FreePool (Urb);
+}
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc       The XHCI device
+  @param  Urb       The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcPeiCreateTransferTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  )
+{
+  VOID                          *OutputContext;
+  TRANSFER_RING                 *EPRing;
+  UINT8                         EPType;
+  UINT8                         SlotId;
+  UINT8                         Dci;
+  TRB                           *TrbStart;
+  UINTN                         TotalLen;
+  UINTN                         Len;
+  UINTN                         TrbNum;
+
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Urb->Finished  = FALSE;
+  Urb->StartDone = FALSE;
+  Urb->EndDone   = FALSE;
+  Urb->Completed = 0;
+  Urb->Result    = EFI_USB_NOERROR;
+
+  Dci       = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  EPRing    = (TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1];
+  Urb->Ring = EPRing;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  if (Xhc->HcCParams.Data.Csz == 0) {
+    EPType  = (UINT8) ((DEVICE_CONTEXT *)OutputContext)->EP[Dci-1].EPType;
+  } else {
+    EPType  = (UINT8) ((DEVICE_CONTEXT_64 *)OutputContext)->EP[Dci-1].EPType;
+  }
+
+  Urb->DataPhy = Urb->Data;
+
+  //
+  // Construct the TRB
+  //
+  XhcPeiSyncTrsRing (Xhc, EPRing);
+  Urb->TrbStart = EPRing->RingEnqueue;
+  switch (EPType) {
+    case ED_CONTROL_BIDIR:
+      //
+      // For control transfer, create SETUP_STAGE_TRB first.
+      //
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      TrbStart->TrbCtrSetup.bmRequestType = Urb->Request->RequestType;
+      TrbStart->TrbCtrSetup.bRequest      = Urb->Request->Request;
+      TrbStart->TrbCtrSetup.wValue        = Urb->Request->Value;
+      TrbStart->TrbCtrSetup.wIndex        = Urb->Request->Index;
+      TrbStart->TrbCtrSetup.wLength       = Urb->Request->Length;
+      TrbStart->TrbCtrSetup.Length        = 8;
+      TrbStart->TrbCtrSetup.IntTarget     = 0;
+      TrbStart->TrbCtrSetup.IOC           = 1;
+      TrbStart->TrbCtrSetup.IDT           = 1;
+      TrbStart->TrbCtrSetup.Type          = TRB_TYPE_SETUP_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrSetup.TRT = 3;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrSetup.TRT = 2;
+      } else {
+        TrbStart->TrbCtrSetup.TRT = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrSetup.CycleBit = EPRing->RingPCS & BIT0;
+      Urb->TrbNum++;
+
+      //
+      // For control transfer, create DATA_STAGE_TRB.
+      //
+      if (Urb->DataLen > 0) {
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+        TrbStart->TrbCtrData.TRBPtrLo  = XHC_LOW_32BIT (Urb->DataPhy);
+        TrbStart->TrbCtrData.TRBPtrHi  = XHC_HIGH_32BIT (Urb->DataPhy);
+        TrbStart->TrbCtrData.Length    = (UINT32) Urb->DataLen;
+        TrbStart->TrbCtrData.TDSize    = 0;
+        TrbStart->TrbCtrData.IntTarget = 0;
+        TrbStart->TrbCtrData.ISP       = 1;
+        TrbStart->TrbCtrData.IOC       = 1;
+        TrbStart->TrbCtrData.IDT       = 0;
+        TrbStart->TrbCtrData.CH        = 0;
+        TrbStart->TrbCtrData.Type      = TRB_TYPE_DATA_STAGE;
+        if (Urb->Ep.Direction == EfiUsbDataIn) {
+          TrbStart->TrbCtrData.DIR = 1;
+        } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+          TrbStart->TrbCtrData.DIR = 0;
+        } else {
+          TrbStart->TrbCtrData.DIR = 0;
+        }
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbCtrData.CycleBit = EPRing->RingPCS & BIT0;
+        Urb->TrbNum++;
+      }
+      //
+      // For control transfer, create STATUS_STAGE_TRB.
+      // Get the pointer to next TRB for status stage use
+      //
+      XhcPeiSyncTrsRing (Xhc, EPRing);
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      TrbStart->TrbCtrStatus.IntTarget = 0;
+      TrbStart->TrbCtrStatus.IOC       = 1;
+      TrbStart->TrbCtrStatus.CH        = 0;
+      TrbStart->TrbCtrStatus.Type      = TRB_TYPE_STATUS_STAGE;
+      if (Urb->Ep.Direction == EfiUsbDataIn) {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      } else if (Urb->Ep.Direction == EfiUsbDataOut) {
+        TrbStart->TrbCtrStatus.DIR = 1;
+      } else {
+        TrbStart->TrbCtrStatus.DIR = 0;
+      }
+      //
+      // Update the cycle bit
+      //
+      TrbStart->TrbCtrStatus.CycleBit = EPRing->RingPCS & BIT0;
+      //
+      // Update the enqueue pointer
+      //
+      XhcPeiSyncTrsRing (Xhc, EPRing);
+      Urb->TrbNum++;
+      Urb->TrbEnd = (TRB_TEMPLATE *) (UINTN) TrbStart;
+
+      break;
+
+    case ED_BULK_OUT:
+    case ED_BULK_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    case ED_INTERRUPT_OUT:
+    case ED_INTERRUPT_IN:
+      TotalLen = 0;
+      Len      = 0;
+      TrbNum   = 0;
+      TrbStart = (TRB *) (UINTN) EPRing->RingEnqueue;
+      while (TotalLen < Urb->DataLen) {
+        if ((TotalLen + 0x10000) >= Urb->DataLen) {
+          Len = Urb->DataLen - TotalLen;
+        } else {
+          Len = 0x10000;
+        }
+        TrbStart = (TRB *)(UINTN)EPRing->RingEnqueue;
+        TrbStart->TrbNormal.TRBPtrLo  = XHC_LOW_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.TRBPtrHi  = XHC_HIGH_32BIT((UINT8 *) Urb->DataPhy + TotalLen);
+        TrbStart->TrbNormal.Length    = (UINT32) Len;
+        TrbStart->TrbNormal.TDSize    = 0;
+        TrbStart->TrbNormal.IntTarget = 0;
+        TrbStart->TrbNormal.ISP       = 1;
+        TrbStart->TrbNormal.IOC       = 1;
+        TrbStart->TrbNormal.Type      = TRB_TYPE_NORMAL;
+        //
+        // Update the cycle bit
+        //
+        TrbStart->TrbNormal.CycleBit = EPRing->RingPCS & BIT0;
+
+        XhcPeiSyncTrsRing (Xhc, EPRing);
+        TrbNum++;
+        TotalLen += Len;
+      }
+
+      Urb->TrbNum = TrbNum;
+      Urb->TrbEnd = (TRB_TEMPLATE *)(UINTN)TrbStart;
+      break;
+
+    default:
+      DEBUG ((EFI_D_INFO, "Not supported EPType 0x%x!\n",EPType));
+      ASSERT (FALSE);
+      break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc           The XHCI device.
+  @param  Urb           The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS   The recovery is successful.
+  @retval Others        Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+XhcPeiRecoverHaltedEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8) (Urb->Ep.Direction));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiRecoverHaltedEndpoint: Recovery Halted Slot = %x, Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Reset endpoint command to transit from halt to stop state
+  //
+  Status = XhcPeiResetEndpoint (Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2) Set dequeue pointer
+  //
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3) Ring the doorbell to transit from stop to active
+  //
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8) (Urb->Ep.Direction));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDequeueTrbFromEndpoint: Stop Slot = %x, Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop endpoint.
+  //
+  Status = XhcPeiStopEndpoint (Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2) Set dequeue pointer
+  //
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3) Ring the doorbell to transit from stop to active
+  //
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  Check if the Trb is a transaction of the URB.
+
+  @param Trb        The TRB to be checked
+  @param Urb        The transfer ring to be checked.
+
+  @retval TRUE      It is a transaction of the URB.
+  @retval FALSE     It is not any transaction of the URB.
+
+**/
+BOOLEAN
+XhcPeiIsTransferRingTrb (
+  IN TRB_TEMPLATE   *Trb,
+  IN URB            *Urb
+  )
+{
+  TRB_TEMPLATE  *CheckedTrb;
+  UINTN         Index;
+
+  CheckedTrb = Urb->Ring->RingSeg0;
+
+  ASSERT (Urb->Ring->TrbNumber == CMD_RING_TRB_NUMBER || Urb->Ring->TrbNumber == TR_RING_TRB_NUMBER);
+
+  for (Index = 0; Index < Urb->Ring->TrbNumber; Index++) {
+    if (Trb == CheckedTrb) {
+      return TRUE;
+    }
+    CheckedTrb++;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check the URB's execution result and update the URB's
+  result accordingly.
+
+  @param  Xhc               The XHCI device.
+  @param  Urb               The URB to check result.
+
+  @return Whether the result of URB transfer is finialized.
+
+**/
+BOOLEAN
+XhcPeiCheckUrbResult (
+  IN PEI_XHC_DEV            *Xhc,
+  IN URB                    *Urb
+  )
+{
+  EVT_TRB_TRANSFER          *EvtTrb;
+  TRB_TEMPLATE              *TRBPtr;
+  UINTN                     Index;
+  UINT8                     TRBType;
+  EFI_STATUS                Status;
+  URB                       *CheckedUrb;
+  UINT64                    XhcDequeue;
+  UINT32                    High;
+  UINT32                    Low;
+  EFI_PHYSICAL_ADDRESS      PhyAddr;
+
+  ASSERT ((Xhc != NULL) && (Urb != NULL));
+
+  Status = EFI_SUCCESS;
+
+  if (Urb->Finished) {
+    goto EXIT;
+  }
+
+  EvtTrb = NULL;
+
+  if (XhcPeiIsHalt (Xhc) || XhcPeiIsSysError (Xhc)) {
+    Urb->Result |= EFI_USB_ERR_SYSTEM;
+    goto EXIT;
+  }
+
+  //
+  // Traverse the event ring to find out all new events from the previous check.
+  //
+  XhcPeiSyncEventRing (Xhc, &Xhc->EventRing);
+  for (Index = 0; Index < Xhc->EventRing.TrbNumber; Index++) {
+    Status = XhcPeiCheckNewEvent (Xhc, &Xhc->EventRing, ((TRB_TEMPLATE **) &EvtTrb));
+    if (Status == EFI_NOT_READY) {
+      //
+      // All new events are handled, return directly.
+      //
+      goto EXIT;
+    }
+
+    //
+    // Only handle COMMAND_COMPLETETION_EVENT and TRANSFER_EVENT.
+    //
+    if ((EvtTrb->Type != TRB_TYPE_COMMAND_COMPLT_EVENT) && (EvtTrb->Type != TRB_TYPE_TRANS_EVENT)) {
+      continue;
+    }
+
+    //
+    // Need convert pci device address to host address
+    //
+    PhyAddr = (EFI_PHYSICAL_ADDRESS) (EvtTrb->TRBPtrLo | LShiftU64 ((UINT64) EvtTrb->TRBPtrHi, 32));
+    TRBPtr = (TRB_TEMPLATE *) (UINTN) UsbHcGetHostAddrForPciAddr (Xhc->MemPool, (VOID *) (UINTN) PhyAddr, sizeof (TRB_TEMPLATE));
+
+    //
+    // Update the status of Urb according to the finished event regardless of whether
+    // the urb is current checked one or in the XHCI's async transfer list.
+    // This way is used to avoid that those completed async transfer events don't get
+    // handled in time and are flushed by newer coming events.
+    //
+    if (XhcPeiIsTransferRingTrb (TRBPtr, Urb)) {
+      CheckedUrb = Urb;
+    } else {
+      continue;
+    }
+
+    switch (EvtTrb->Completecode) {
+      case TRB_COMPLETION_STALL_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_STALL;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: STALL_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_BABBLE_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BABBLE;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: BABBLE_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_DATA_BUFFER_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_BUFFER;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: ERR_BUFFER! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_USB_TRANSACTION_ERROR:
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: TRANSACTION_ERROR! Completecode = %x\n", EvtTrb->Completecode));
+        goto EXIT;
+
+      case TRB_COMPLETION_SHORT_PACKET:
+      case TRB_COMPLETION_SUCCESS:
+        if (EvtTrb->Completecode == TRB_COMPLETION_SHORT_PACKET) {
+          DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: short packet happens!\n"));
+        }
+
+        TRBType = (UINT8) (TRBPtr->Type);
+        if ((TRBType == TRB_TYPE_DATA_STAGE) ||
+            (TRBType == TRB_TYPE_NORMAL) ||
+            (TRBType == TRB_TYPE_ISOCH)) {
+          CheckedUrb->Completed += (((TRANSFER_TRB_NORMAL*)TRBPtr)->Length - EvtTrb->Length);
+        }
+
+        break;
+
+      default:
+        DEBUG ((EFI_D_ERROR, "XhcPeiCheckUrbResult: Transfer Default Error Occur! Completecode = 0x%x!\n", EvtTrb->Completecode));
+        CheckedUrb->Result  |= EFI_USB_ERR_TIMEOUT;
+        CheckedUrb->Finished = TRUE;
+        goto EXIT;
+    }
+
+    //
+    // Only check first and end Trb event address
+    //
+    if (TRBPtr == CheckedUrb->TrbStart) {
+      CheckedUrb->StartDone = TRUE;
+    }
+
+    if (TRBPtr == CheckedUrb->TrbEnd) {
+      CheckedUrb->EndDone = TRUE;
+    }
+
+    if (CheckedUrb->StartDone && CheckedUrb->EndDone) {
+      CheckedUrb->Finished = TRUE;
+      CheckedUrb->EvtTrb   = (TRB_TEMPLATE *) EvtTrb;
+    }
+  }
+
+EXIT:
+
+  //
+  // Advance event ring to last available entry
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  Low  = XhcPeiReadRuntimeReg (Xhc, XHC_ERDP_OFFSET);
+  High = XhcPeiReadRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4);
+  XhcDequeue = (UINT64) (LShiftU64((UINT64) High, 32) | Low);
+
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->EventRing.EventRingDequeue, sizeof (TRB_TEMPLATE));
+
+  if ((XhcDequeue & (~0x0F)) != (PhyAddr & (~0x0F))) {
+    //
+    // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+    // So divide it to two 32-bytes width register access.
+    //
+    XhcPeiWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET, XHC_LOW_32BIT (PhyAddr) | BIT3);
+    XhcPeiWriteRuntimeReg (Xhc, XHC_ERDP_OFFSET + 4, XHC_HIGH_32BIT (PhyAddr));
+  }
+
+  return Urb->Finished;
+}
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI device.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcPeiExecTransfer (
+  IN PEI_XHC_DEV            *Xhc,
+  IN BOOLEAN                CmdTransfer,
+  IN URB                    *Urb,
+  IN UINTN                  Timeout
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         Index;
+  UINT64        Loop;
+  UINT8         SlotId;
+  UINT8         Dci;
+  BOOLEAN       Finished;
+
+  if (CmdTransfer) {
+    SlotId = 0;
+    Dci    = 0;
+  } else {
+    SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+    if (SlotId == 0) {
+      return EFI_DEVICE_ERROR;
+    }
+    Dci  = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  }
+
+  Status = EFI_SUCCESS;
+  Loop   = Timeout * XHC_1_MILLISECOND;
+  if (Timeout == 0) {
+    Loop = 0xFFFFFFFF;
+  }
+
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+  for (Index = 0; Index < Loop; Index++) {
+    Finished = XhcPeiCheckUrbResult (Xhc, Urb);
+    if (Finished) {
+      break;
+    }
+    MicroSecondDelay (XHC_1_MICROSECOND);
+  }
+
+  if (Index == Loop) {
+    Urb->Result = EFI_USB_ERR_TIMEOUT;
+    Status      = EFI_TIMEOUT;
+  } else if (Urb->Result != EFI_USB_NOERROR) {
+    Status      = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc               The XHCI device.
+  @param  ParentRouteChart  The route string pointed to the parent device if it exists.
+  @param  Port              The port to be polled.
+  @param  PortState         The port state.
+
+  @retval EFI_SUCCESS       Successfully enable/disable device slot according to port state.
+  @retval Others            Should not appear.
+
+**/
+EFI_STATUS
+XhcPeiPollPortStatusChange (
+  IN PEI_XHC_DEV            *Xhc,
+  IN USB_DEV_ROUTE          ParentRouteChart,
+  IN UINT8                  Port,
+  IN EFI_USB_PORT_STATUS    *PortState
+  )
+{
+  EFI_STATUS        Status;
+  UINT8             Speed;
+  UINT8             SlotId;
+  USB_DEV_ROUTE     RouteChart;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiPollPortStatusChange: PortChangeStatus: %x PortStatus: %x\n", PortState->PortChangeStatus, PortState->PortStatus));
+
+  Status = EFI_SUCCESS;
+
+  if ((PortState->PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (ParentRouteChart.Dword == 0) {
+    RouteChart.Route.RouteString = 0;
+    RouteChart.Route.RootPortNum = Port + 1;
+    RouteChart.Route.TierNum     = 1;
+  } else {
+    if(Port < 14) {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (Port << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    } else {
+      RouteChart.Route.RouteString = ParentRouteChart.Route.RouteString | (15 << (4 * (ParentRouteChart.Route.TierNum - 1)));
+    }
+    RouteChart.Route.RootPortNum   = ParentRouteChart.Route.RootPortNum;
+    RouteChart.Route.TierNum       = ParentRouteChart.Route.TierNum + 1;
+  }
+
+  SlotId = XhcPeiRouteStringToSlotId (Xhc, RouteChart);
+  if (SlotId != 0) {
+    if (Xhc->HcCParams.Data.Csz == 0) {
+      Status = XhcPeiDisableSlotCmd (Xhc, SlotId);
+    } else {
+      Status = XhcPeiDisableSlotCmd64 (Xhc, SlotId);
+    }
+  }
+
+  if (((PortState->PortStatus & USB_PORT_STAT_ENABLE) != 0) &&
+      ((PortState->PortStatus & USB_PORT_STAT_CONNECTION) != 0)) {
+    //
+    // Has a device attached, Identify device speed after port is enabled.
+    //
+    Speed = EFI_USB_SPEED_FULL;
+    if ((PortState->PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_LOW;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_HIGH;
+    } else if ((PortState->PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+      Speed = EFI_USB_SPEED_SUPER;
+    }
+    //
+    // Execute Enable_Slot cmd for attached device, initialize device context and assign device address.
+    //
+    SlotId = XhcPeiRouteStringToSlotId (Xhc, RouteChart);
+    if ((SlotId == 0) && ((PortState->PortChangeStatus & USB_PORT_STAT_C_RESET) != 0)) {
+      if (Xhc->HcCParams.Data.Csz == 0) {
+        Status = XhcPeiInitializeDeviceSlot (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      } else {
+        Status = XhcPeiInitializeDeviceSlot64 (Xhc, ParentRouteChart, Port, RouteChart, Speed);
+      }
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr        The target endpoint number.
+  @param  Direction     The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcPeiEndpointToDci (
+  IN UINT8                      EpAddr,
+  IN EFI_USB_DATA_DIRECTION     Direction
+  )
+{
+  UINT8 Index;
+
+  ASSERT (EpAddr <= 15);
+
+  if (EpAddr == 0) {
+    return 1;
+  } else {
+    Index = (UINT8) (2 * EpAddr);
+    if (Direction == EfiUsbDataIn) {
+      Index += 1;
+    }
+    return Index;
+  }
+}
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc           The XHCI device.
+  @param  BusDevAddr    The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+XhcPeiBusDevAddrToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              BusDevAddr
+  )
+{
+  UINT8   Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].BusDevAddr == BusDevAddr)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc           The XHCI device.
+  @param  RouteString   The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+XhcPeiRouteStringToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN USB_DEV_ROUTE      RouteString
+  )
+{
+  UINT8   Index;
+
+  for (Index = 0; Index < 255; Index++) {
+    if (Xhc->UsbDevContext[Index + 1].Enabled &&
+        (Xhc->UsbDevContext[Index + 1].SlotId != 0) &&
+        (Xhc->UsbDevContext[Index + 1].RouteString.Dword == RouteString.Dword)) {
+      break;
+    }
+  }
+
+  if (Index == 255) {
+    return 0;
+  }
+
+  return Xhc->UsbDevContext[Index + 1].SlotId;
+}
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+**/
+VOID
+XhcPeiRingDoorBell (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  if (SlotId == 0) {
+    XhcPeiWriteDoorBellReg (Xhc, 0, 0);
+  } else {
+    XhcPeiWriteDoorBellReg (Xhc, SlotId * sizeof (UINT32), Dci);
+  }
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE        CmdTrbAddr;
+  UINT8                         DeviceAddress;
+  CMD_TRB_ENABLE_SLOT           CmdTrb;
+  UINT8                         SlotId;
+  UINT8                         ParentSlotId;
+  DEVICE_CONTEXT                *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiInitializeDeviceSlot: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8) EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString != 0) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcPeiRouteStringToSlotId (Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    // If the Full/Low device attached to a High Speed Hub, init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT *) Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) OutputContext->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot: Enable Slot, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_ADDRESS_DEVICE        CmdTrbAddr;
+  UINT8                         DeviceAddress;
+  CMD_TRB_ENABLE_SLOT           CmdTrb;
+  UINT8                         SlotId;
+  UINT8                         ParentSlotId;
+  DEVICE_CONTEXT_64             *ParentDeviceContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ZeroMem (&CmdTrb, sizeof (CMD_TRB_ENABLE_SLOT));
+  CmdTrb.CycleBit = 1;
+  CmdTrb.Type     = TRB_TYPE_EN_SLOT;
+
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrb,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiInitializeDeviceSlot64: Enable Slot Failed, Status = %r\n", Status));
+    return Status;
+  }
+  ASSERT (EvtTrb->SlotId <= Xhc->MaxSlotsEn);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Enable Slot Successfully, The Slot ID = 0x%x\n", EvtTrb->SlotId));
+  SlotId = (UINT8)EvtTrb->SlotId;
+  ASSERT (SlotId != 0);
+
+  ZeroMem (&Xhc->UsbDevContext[SlotId], sizeof (USB_DEV_CONTEXT));
+  Xhc->UsbDevContext[SlotId].Enabled                 = TRUE;
+  Xhc->UsbDevContext[SlotId].SlotId                  = SlotId;
+  Xhc->UsbDevContext[SlotId].RouteString.Dword       = RouteChart.Dword;
+  Xhc->UsbDevContext[SlotId].ParentRouteString.Dword = ParentRouteChart.Dword;
+
+  //
+  // 4.3.3 Device Slot Initialization
+  // 1) Allocate an Input Context data structure (6.2.5) and initialize all fields to '0'.
+  //
+  InputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (INPUT_CONTEXT_64));
+  ASSERT (InputContext != NULL);
+  ASSERT (((UINTN) InputContext & 0x3F) == 0);
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].InputContext = (VOID *) InputContext;
+
+  //
+  // 2) Initialize the Input Control Context (6.2.5.1) of the Input Context by setting the A0 and A1
+  //    flags to '1'. These flags indicate that the Slot Context and the Endpoint 0 Context of the Input
+  //    Context are affected by the command.
+  //
+  InputContext->InputControlContext.Dword2 |= (BIT0 | BIT1);
+
+  //
+  // 3) Initialize the Input Slot Context data structure
+  //
+  InputContext->Slot.RouteString    = RouteChart.Route.RouteString;
+  InputContext->Slot.Speed          = DeviceSpeed + 1;
+  InputContext->Slot.ContextEntries = 1;
+  InputContext->Slot.RootHubPortNum = RouteChart.Route.RootPortNum;
+
+  if (RouteChart.Route.RouteString != 0) {
+    //
+    // The device is behind of hub device.
+    //
+    ParentSlotId = XhcPeiRouteStringToSlotId (Xhc, ParentRouteChart);
+    ASSERT (ParentSlotId != 0);
+    //
+    //if the Full/Low device attached to a High Speed Hub, Init the TTPortNum and TTHubSlotId field of slot context
+    //
+    ParentDeviceContext = (DEVICE_CONTEXT_64 *) Xhc->UsbDevContext[ParentSlotId].OutputContext;
+    if ((ParentDeviceContext->Slot.TTPortNum == 0) &&
+        (ParentDeviceContext->Slot.TTHubSlotId == 0)) {
+      if ((ParentDeviceContext->Slot.Speed == (EFI_USB_SPEED_HIGH + 1)) && (DeviceSpeed < EFI_USB_SPEED_HIGH)) {
+        //
+        // Full/Low device attached to High speed hub port that isolates the high speed signaling
+        // environment from Full/Low speed signaling environment for a device
+        //
+        InputContext->Slot.TTPortNum   = ParentPort;
+        InputContext->Slot.TTHubSlotId = ParentSlotId;
+      }
+    } else {
+      //
+      // Inherit the TT parameters from parent device.
+      //
+      InputContext->Slot.TTPortNum   = ParentDeviceContext->Slot.TTPortNum;
+      InputContext->Slot.TTHubSlotId = ParentDeviceContext->Slot.TTHubSlotId;
+      //
+      // If the device is a High speed device then down the speed to be the same as its parent Hub
+      //
+      if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+        InputContext->Slot.Speed = ParentDeviceContext->Slot.Speed;
+      }
+    }
+  }
+
+  //
+  // 4) Allocate and initialize the Transfer Ring for the Default Control Endpoint.
+  //
+  EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+  Xhc->UsbDevContext[SlotId].EndpointTransferRing[0] = EndpointTransferRing;
+  XhcPeiCreateTransferRing(Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0]);
+  //
+  // 5) Initialize the Input default control Endpoint 0 Context (6.2.3).
+  //
+  InputContext->EP[0].EPType = ED_CONTROL_BIDIR;
+
+  if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    InputContext->EP[0].MaxPacketSize = 512;
+  } else if (DeviceSpeed == EFI_USB_SPEED_HIGH) {
+    InputContext->EP[0].MaxPacketSize = 64;
+  } else {
+    InputContext->EP[0].MaxPacketSize = 8;
+  }
+  //
+  // Initial value of Average TRB Length for Control endpoints would be 8B, Interrupt endpoints
+  // 1KB, and Bulk and Isoch endpoints 3KB.
+  //
+  InputContext->EP[0].AverageTRBLength = 8;
+  InputContext->EP[0].MaxBurstSize     = 0;
+  InputContext->EP[0].Interval         = 0;
+  InputContext->EP[0].MaxPStreams      = 0;
+  InputContext->EP[0].Mult             = 0;
+  InputContext->EP[0].CErr             = 3;
+
+  //
+  // Init the DCS(dequeue cycle state) as the transfer ring's CCS
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (
+              Xhc->MemPool,
+              ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[0])->RingSeg0,
+              sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+              );
+  InputContext->EP[0].PtrLo = XHC_LOW_32BIT (PhyAddr) | BIT0;
+  InputContext->EP[0].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+  //
+  // 6) Allocate the Output Device Context data structure (6.2.1) and initialize it to '0'.
+  //
+  OutputContext = UsbHcAllocateMem (Xhc->MemPool, sizeof (DEVICE_CONTEXT_64));
+  ASSERT (OutputContext != NULL);
+  ASSERT (((UINTN) OutputContext & 0x3F) == 0);
+  ZeroMem (OutputContext, sizeof (DEVICE_CONTEXT_64));
+
+  Xhc->UsbDevContext[SlotId].OutputContext = OutputContext;
+  //
+  // 7) Load the appropriate (Device Slot ID) entry in the Device Context Base Address Array (5.4.6) with
+  //    a pointer to the Output Device Context data structure (6.2.1).
+  //
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, OutputContext, sizeof (DEVICE_CONTEXT_64));
+  //
+  // Fill DCBAA with PCI device address
+  //
+  Xhc->DCBAA[SlotId] = (UINT64) (UINTN) PhyAddr;
+
+  //
+  // 8) Issue an Address Device Command for the Device Slot, where the command points to the Input
+  //    Context data structure described above.
+  //
+  ZeroMem (&CmdTrbAddr, sizeof (CmdTrbAddr));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbAddr.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbAddr.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbAddr.CycleBit = 1;
+  CmdTrbAddr.Type     = TRB_TYPE_ADDRESS_DEV;
+  CmdTrbAddr.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbAddr,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (!EFI_ERROR (Status)) {
+    DeviceAddress = (UINT8) OutputContext->Slot.DeviceAddress;
+    DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Address %d assigned successfully\n", DeviceAddress));
+    Xhc->UsbDevContext[SlotId].XhciDevAddr = DeviceAddress;
+  }
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitializeDeviceSlot64: Enable Slot, Status = %r\n", Status));
+  return Status;
+}
+
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd (
+  IN PEI_XHC_DEV               *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcPeiDisableSlotCmd (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd: Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd: Disable Slot Command, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd64 (
+  IN PEI_XHC_DEV               *Xhc,
+  IN UINT8                     SlotId
+  )
+{
+  EFI_STATUS            Status;
+  TRB_TEMPLATE          *EvtTrb;
+  CMD_TRB_DISABLE_SLOT  CmdTrbDisSlot;
+  UINT8                 Index;
+  VOID                  *RingSeg;
+
+  //
+  // Disable the device slots occupied by these devices on its downstream ports.
+  // Entry 0 is reserved.
+  //
+  for (Index = 0; Index < 255; Index++) {
+    if (!Xhc->UsbDevContext[Index + 1].Enabled ||
+        (Xhc->UsbDevContext[Index + 1].SlotId == 0) ||
+        (Xhc->UsbDevContext[Index + 1].ParentRouteString.Dword != Xhc->UsbDevContext[SlotId].RouteString.Dword)) {
+      continue;
+    }
+
+    Status = XhcPeiDisableSlotCmd64 (Xhc, Xhc->UsbDevContext[Index + 1].SlotId);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd64: failed to disable child, ignore error\n"));
+      Xhc->UsbDevContext[Index + 1].SlotId = 0;
+    }
+  }
+
+  //
+  // Construct the disable slot command
+  //
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd64: Disable device slot %d!\n", SlotId));
+
+  ZeroMem (&CmdTrbDisSlot, sizeof (CmdTrbDisSlot));
+  CmdTrbDisSlot.CycleBit = 1;
+  CmdTrbDisSlot.Type     = TRB_TYPE_DIS_SLOT;
+  CmdTrbDisSlot.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbDisSlot,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDisableSlotCmd64: Disable Slot Command Failed, Status = %r\n", Status));
+    return Status;
+  }
+  //
+  // Free the slot's device context entry
+  //
+  Xhc->DCBAA[SlotId] = 0;
+
+  //
+  // Free the slot related data structure
+  //
+  for (Index = 0; Index < 31; Index++) {
+    if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] != NULL) {
+      RingSeg = ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index])->RingSeg0;
+      if (RingSeg != NULL) {
+        UsbHcFreeMem (Xhc->MemPool, RingSeg, sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER);
+      }
+      FreePool (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index]);
+      Xhc->UsbDevContext[SlotId].EndpointTransferRing[Index] = NULL;
+    }
+  }
+
+  for (Index = 0; Index < Xhc->UsbDevContext[SlotId].DevDesc.NumConfigurations; Index++) {
+    if (Xhc->UsbDevContext[SlotId].ConfDesc[Index] != NULL) {
+      FreePool (Xhc->UsbDevContext[SlotId].ConfDesc[Index]);
+    }
+  }
+
+  if (Xhc->UsbDevContext[SlotId].InputContext != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].InputContext, sizeof (INPUT_CONTEXT_64));
+  }
+
+  if (Xhc->UsbDevContext[SlotId].OutputContext != NULL) {
+     UsbHcFreeMem (Xhc->MemPool, Xhc->UsbDevContext[SlotId].OutputContext, sizeof (DEVICE_CONTEXT_64));
+  }
+  //
+  // Doesn't zero the entry because XhcAsyncInterruptTransfer() may be invoked to remove the established
+  // asynchronous interrupt pipe after the device is disabled. It needs the device address mapping info to
+  // remove urb from XHCI's asynchronous transfer list.
+  //
+  Xhc->UsbDevContext[SlotId].Enabled = FALSE;
+  Xhc->UsbDevContext[SlotId].SlotId  = 0;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDisableSlotCmd64: Disable Slot Command, Status = %r\n", Status));
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  )
+{
+  EFI_STATUS                    Status;
+  USB_INTERFACE_DESCRIPTOR      *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINT8                         Index;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  EFI_USB_DATA_DIRECTION        Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *) (ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+    }
+
+    NumEp = IfDesc->NumEndpoints;
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDesc + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcPeiEndpointToDci (EpAddr, Direction);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+
+      InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+      InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+      if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+        //
+        // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+        //
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      } else {
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      }
+
+      switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+        case USB_ENDPOINT_BULK:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+          }
+
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+
+          break;
+        case USB_ENDPOINT_ISO:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+          }
+          //
+          // Do not support isochronous transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+        case USB_ENDPOINT_INTERRUPT:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+          }
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+          //
+          // Get the bInterval from descriptor and init the interval field of endpoint context
+          //
+          if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+            Interval = EpDesc->Interval;
+            //
+            // Calculate through the bInterval field of Endpoint descriptor.
+            //
+            ASSERT (Interval != 0);
+            InputContext->EP[Dci-1].Interval = (UINT32) HighBitSet32 ((UINT32) Interval) + 3;
+          } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+            Interval = EpDesc->Interval;
+            ASSERT (Interval >= 1 && Interval <= 16);
+            //
+            // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+            //
+            InputContext->EP[Dci-1].Interval = Interval - 1;
+          }
+
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+          break;
+
+        case USB_ENDPOINT_CONTROL:
+          //
+          // Do not support control transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+        default:
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd: Unknown EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+      }
+
+      PhyAddr = UsbHcGetPciAddrForHostAddr (
+                  Xhc->MemPool,
+                  ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                  sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                  );
+      PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+      PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+      InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+      InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcSetConfigCmd: Configure Endpoint\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  )
+{
+  EFI_STATUS                    Status;
+  USB_INTERFACE_DESCRIPTOR      *IfDesc;
+  USB_ENDPOINT_DESCRIPTOR       *EpDesc;
+  UINT8                         Index;
+  UINTN                         NumEp;
+  UINTN                         EpIndex;
+  UINT8                         EpAddr;
+  EFI_USB_DATA_DIRECTION        Direction;
+  UINT8                         Dci;
+  UINT8                         MaxDci;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+  UINT8                         Interval;
+
+  TRANSFER_RING                 *EndpointTransferRing;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  //
+  // 4.6.6 Configure Endpoint
+  //
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+  CopyMem (&InputContext->Slot, &OutputContext->Slot, sizeof (SLOT_CONTEXT_64));
+
+  ASSERT (ConfigDesc != NULL);
+
+  MaxDci = 0;
+
+  IfDesc = (USB_INTERFACE_DESCRIPTOR *) (ConfigDesc + 1);
+  for (Index = 0; Index < ConfigDesc->NumInterfaces; Index++) {
+    while ((IfDesc->DescriptorType != USB_DESC_TYPE_INTERFACE) || (IfDesc->AlternateSetting != 0)) {
+      IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN) IfDesc + IfDesc->Length);
+    }
+
+    NumEp = IfDesc->NumEndpoints;
+
+    EpDesc = (USB_ENDPOINT_DESCRIPTOR *) (IfDesc + 1);
+    for (EpIndex = 0; EpIndex < NumEp; EpIndex++) {
+      while (EpDesc->DescriptorType != USB_DESC_TYPE_ENDPOINT) {
+        EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN) EpDesc + EpDesc->Length);
+      }
+
+      EpAddr    = (UINT8) (EpDesc->EndpointAddress & 0x0F);
+      Direction = (UINT8) ((EpDesc->EndpointAddress & 0x80) ? EfiUsbDataIn : EfiUsbDataOut);
+
+      Dci = XhcPeiEndpointToDci (EpAddr, Direction);
+      ASSERT (Dci < 32);
+      if (Dci > MaxDci) {
+        MaxDci = Dci;
+      }
+
+      InputContext->InputControlContext.Dword2 |= (BIT0 << Dci);
+      InputContext->EP[Dci-1].MaxPacketSize     = EpDesc->MaxPacketSize;
+
+      if (DeviceSpeed == EFI_USB_SPEED_SUPER) {
+        //
+        // 6.2.3.4, shall be set to the value defined in the bMaxBurst field of the SuperSpeed Endpoint Companion Descriptor.
+        //
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      } else {
+        InputContext->EP[Dci-1].MaxBurstSize = 0x0;
+      }
+
+      switch (EpDesc->Attributes & USB_ENDPOINT_TYPE_MASK) {
+        case USB_ENDPOINT_BULK:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_BULK_OUT;
+          }
+
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+
+          break;
+        case USB_ENDPOINT_ISO:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 0;
+            InputContext->EP[Dci-1].EPType = ED_ISOCH_OUT;
+          }
+          //
+          // Do not support isochronous transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unsupport ISO EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+        case USB_ENDPOINT_INTERRUPT:
+          if (Direction == EfiUsbDataIn) {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_IN;
+          } else {
+            InputContext->EP[Dci-1].CErr   = 3;
+            InputContext->EP[Dci-1].EPType = ED_INTERRUPT_OUT;
+          }
+          InputContext->EP[Dci-1].AverageTRBLength = 0x1000;
+          InputContext->EP[Dci-1].MaxESITPayload   = EpDesc->MaxPacketSize;
+          //
+          // Get the bInterval from descriptor and init the the interval field of endpoint context
+          //
+          if ((DeviceSpeed == EFI_USB_SPEED_FULL) || (DeviceSpeed == EFI_USB_SPEED_LOW)) {
+            Interval = EpDesc->Interval;
+            //
+            // Calculate through the bInterval field of Endpoint descriptor.
+            //
+            ASSERT (Interval != 0);
+            InputContext->EP[Dci-1].Interval = (UINT32) HighBitSet32( (UINT32) Interval) + 3;
+          } else if ((DeviceSpeed == EFI_USB_SPEED_HIGH) || (DeviceSpeed == EFI_USB_SPEED_SUPER)) {
+            Interval = EpDesc->Interval;
+            ASSERT (Interval >= 1 && Interval <= 16);
+            //
+            // Refer to XHCI 1.0 spec section 6.2.3.6, table 61
+            //
+            InputContext->EP[Dci-1].Interval = Interval - 1;
+          }
+
+          if (Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] == NULL) {
+            EndpointTransferRing = AllocateZeroPool (sizeof (TRANSFER_RING));
+            Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1] = (VOID *) EndpointTransferRing;
+            XhcPeiCreateTransferRing (Xhc, TR_RING_TRB_NUMBER, (TRANSFER_RING *) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1]);
+          }
+          break;
+
+        case USB_ENDPOINT_CONTROL:
+          //
+          // Do not support control transfer now.
+          //
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unsupport Control EP found, Transfer ring is not allocated.\n"));
+        default:
+          DEBUG ((EFI_D_INFO, "XhcPeiSetConfigCmd64: Unknown EP found, Transfer ring is not allocated.\n"));
+          EpDesc = (USB_ENDPOINT_DESCRIPTOR *)((UINTN)EpDesc + EpDesc->Length);
+          continue;
+      }
+
+      PhyAddr = UsbHcGetPciAddrForHostAddr (
+                  Xhc->MemPool,
+                  ((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingSeg0,
+                  sizeof (TRB_TEMPLATE) * TR_RING_TRB_NUMBER
+                  );
+
+      PhyAddr &= ~((EFI_PHYSICAL_ADDRESS)0x0F);
+      PhyAddr |= (EFI_PHYSICAL_ADDRESS)((TRANSFER_RING *) (UINTN) Xhc->UsbDevContext[SlotId].EndpointTransferRing[Dci-1])->RingPCS;
+
+      InputContext->EP[Dci-1].PtrLo = XHC_LOW_32BIT (PhyAddr);
+      InputContext->EP[Dci-1].PtrHi = XHC_HIGH_32BIT (PhyAddr);
+
+      EpDesc = (USB_ENDPOINT_DESCRIPTOR *) ((UINTN)EpDesc + EpDesc->Length);
+    }
+    IfDesc = (USB_INTERFACE_DESCRIPTOR *) ((UINTN)IfDesc + IfDesc->Length);
+  }
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+  InputContext->Slot.ContextEntries         = MaxDci;
+  //
+  // configure endpoint
+  //
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr  = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcSetConfigCmd64: Configure Endpoint\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetConfigCmd64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  )
+{
+  EFI_STATUS                    Status;
+  CMD_TRB_EVALUATE_CONTEXT      CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcEvaluateContext: Evaluate context\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  )
+{
+  EFI_STATUS                    Status;
+  CMD_TRB_EVALUATE_CONTEXT      CmdTrbEvalu;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  InputContext = Xhc->UsbDevContext[SlotId].InputContext;
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT1;
+  InputContext->EP[0].MaxPacketSize         = MaxPacketSize;
+
+  ZeroMem (&CmdTrbEvalu, sizeof (CmdTrbEvalu));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbEvalu.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbEvalu.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbEvalu.CycleBit = 1;
+  CmdTrbEvalu.Type     = TRB_TYPE_EVALU_CONTXT;
+  CmdTrbEvalu.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "XhcEvaluateContext64: Evaluate context 64\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbEvalu,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcEvaluateContext64: Evaluate Context Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT                 *InputContext;
+  DEVICE_CONTEXT                *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  INPUT_CONTEXT_64              *InputContext;
+  DEVICE_CONTEXT_64             *OutputContext;
+  CMD_TRB_CONFIG_ENDPOINT       CmdTrbCfgEP;
+  EFI_PHYSICAL_ADDRESS          PhyAddr;
+
+  ASSERT (Xhc->UsbDevContext[SlotId].SlotId != 0);
+  InputContext  = Xhc->UsbDevContext[SlotId].InputContext;
+  OutputContext = Xhc->UsbDevContext[SlotId].OutputContext;
+
+  //
+  // 4.6.7 Evaluate Context
+  //
+  ZeroMem (InputContext, sizeof (INPUT_CONTEXT_64));
+
+  InputContext->InputControlContext.Dword2 |= BIT0;
+
+  //
+  // Copy the slot context from OutputContext to Input context
+  //
+  CopyMem(&(InputContext->Slot), &(OutputContext->Slot), sizeof (SLOT_CONTEXT_64));
+  InputContext->Slot.Hub     = 1;
+  InputContext->Slot.PortNum = PortNum;
+  InputContext->Slot.TTT     = TTT;
+  InputContext->Slot.MTT     = MTT;
+
+  ZeroMem (&CmdTrbCfgEP, sizeof (CmdTrbCfgEP));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, InputContext, sizeof (INPUT_CONTEXT_64));
+  CmdTrbCfgEP.PtrLo    = XHC_LOW_32BIT (PhyAddr);
+  CmdTrbCfgEP.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdTrbCfgEP.CycleBit = 1;
+  CmdTrbCfgEP.Type     = TRB_TYPE_CON_ENDPOINT;
+  CmdTrbCfgEP.SlotId   = Xhc->UsbDevContext[SlotId].SlotId;
+  DEBUG ((EFI_D_INFO, "Configure Hub Slot Context 64\n"));
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbCfgEP,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcConfigHubContext64: Config Endpoint Failed, Status = %r\n", Status));
+  }
+  return Status;
+}
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  CMD_TRB_STOP_ENDPOINT         CmdTrbStopED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiStopEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbStopED, sizeof (CmdTrbStopED));
+  CmdTrbStopED.CycleBit = 1;
+  CmdTrbStopED.Type     = TRB_TYPE_STOP_ENDPOINT;
+  CmdTrbStopED.EDID     = Dci;
+  CmdTrbStopED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbStopED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiStopEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI device.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+XhcPeiCheckNewEvent (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EvtRing,
+  OUT TRB_TEMPLATE      **NewEvtTrb
+  )
+{
+  ASSERT (EvtRing != NULL);
+
+  *NewEvtTrb = EvtRing->EventRingDequeue;
+
+  if (EvtRing->EventRingDequeue == EvtRing->EventRingEnqueue) {
+    return EFI_NOT_READY;
+  }
+
+  EvtRing->EventRingDequeue++;
+  //
+  // If the dequeue pointer is beyond the ring, then roll-back it to the begining of the ring.
+  //
+  if ((UINTN) EvtRing->EventRingDequeue >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+    EvtRing->EventRingDequeue = EvtRing->EventRingSeg0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  EvtRing   The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncEventRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN EVENT_RING     *EvtRing
+  )
+{
+  UINTN             Index;
+  TRB_TEMPLATE      *EvtTrb;
+
+  ASSERT (EvtRing != NULL);
+
+  //
+  // Calculate the EventRingEnqueue and EventRingCCS.
+  // Note: only support single Segment
+  //
+  EvtTrb = EvtRing->EventRingDequeue;
+
+  for (Index = 0; Index < EvtRing->TrbNumber; Index++) {
+    if (EvtTrb->CycleBit != EvtRing->EventRingCCS) {
+      break;
+    }
+
+    EvtTrb++;
+
+    if ((UINTN) EvtTrb >= ((UINTN) EvtRing->EventRingSeg0 + sizeof (TRB_TEMPLATE) * EvtRing->TrbNumber)) {
+      EvtTrb = EvtRing->EventRingSeg0;
+      EvtRing->EventRingCCS = (EvtRing->EventRingCCS) ? 0 : 1;
+    }
+  }
+
+  if (Index < EvtRing->TrbNumber) {
+    EvtRing->EventRingEnqueue = EvtTrb;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Free XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The event ring to be freed.
+
+**/
+VOID
+XhcPeiFreeEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EventRing
+  )
+{
+  if(EventRing->EventRingSeg0 == NULL) {
+    return;
+  }
+
+  //
+  // Free EventRing Segment 0
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->EventRingSeg0, sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER);
+
+  //
+  // Free ERST table
+  //
+  UsbHcFreeMem (Xhc->MemPool, EventRing->ERSTBase, sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER);
+}
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The created event ring.
+
+**/
+VOID
+XhcPeiCreateEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  OUT EVENT_RING        *EventRing
+  )
+{
+  VOID                          *Buf;
+  EVENT_RING_SEG_TABLE_ENTRY    *ERSTBase;
+  UINTN                         Size;
+  EFI_PHYSICAL_ADDRESS          ERSTPhy;
+  EFI_PHYSICAL_ADDRESS          DequeuePhy;
+
+  ASSERT (EventRing != NULL);
+
+  Size = sizeof (TRB_TEMPLATE) * EVENT_RING_TRB_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  DequeuePhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+
+  EventRing->EventRingSeg0      = Buf;
+  EventRing->TrbNumber          = EVENT_RING_TRB_NUMBER;
+  EventRing->EventRingDequeue   = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+  EventRing->EventRingEnqueue   = (TRB_TEMPLATE *) EventRing->EventRingSeg0;
+
+  //
+  // Software maintains an Event Ring Consumer Cycle State (CCS) bit, initializing it to '1'
+  // and toggling it every time the Event Ring Dequeue Pointer wraps back to the beginning of the Event Ring.
+  //
+  EventRing->EventRingCCS = 1;
+
+  Size = sizeof (EVENT_RING_SEG_TABLE_ENTRY) * ERST_NUMBER;
+  Buf = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, Size);
+
+  ERSTBase              = (EVENT_RING_SEG_TABLE_ENTRY *) Buf;
+  EventRing->ERSTBase   = ERSTBase;
+  ERSTBase->PtrLo       = XHC_LOW_32BIT (DequeuePhy);
+  ERSTBase->PtrHi       = XHC_HIGH_32BIT (DequeuePhy);
+  ERSTBase->RingTrbSize = EVENT_RING_TRB_NUMBER;
+
+  ERSTPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, Size);
+
+  //
+  // Program the Interrupter Event Ring Segment Table Size (ERSTSZ) register (5.5.2.3.1)
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTSZ_OFFSET,
+    ERST_NUMBER
+    );
+  //
+  // Program the Interrupter Event Ring Dequeue Pointer (ERDP) register (5.5.2.3.3)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET,
+    XHC_LOW_32BIT ((UINT64) (UINTN) DequeuePhy)
+    );
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERDP_OFFSET + 4,
+    XHC_HIGH_32BIT ((UINT64) (UINTN) DequeuePhy)
+    );
+  //
+  // Program the Interrupter Event Ring Segment Table Base Address (ERSTBA) register (5.5.2.3.2)
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET,
+    XHC_LOW_32BIT ((UINT64) (UINTN) ERSTPhy)
+    );
+  XhcPeiWriteRuntimeReg (
+    Xhc,
+    XHC_ERSTBA_OFFSET + 4,
+    XHC_HIGH_32BIT ((UINT64) (UINTN) ERSTPhy)
+    );
+  //
+  // Need set IMAN IE bit to enable the ring interrupt
+  //
+  XhcPeiSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET, XHC_IMAN_IE);
+}
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  TrsRing   The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncTrsRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRANSFER_RING  *TrsRing
+  )
+{
+  UINTN             Index;
+  TRB_TEMPLATE      *TrsTrb;
+
+  ASSERT (TrsRing != NULL);
+  //
+  // Calculate the latest RingEnqueue and RingPCS
+  //
+  TrsTrb = TrsRing->RingEnqueue;
+  ASSERT (TrsTrb != NULL);
+
+  for (Index = 0; Index < TrsRing->TrbNumber; Index++) {
+    if (TrsTrb->CycleBit != (TrsRing->RingPCS & BIT0)) {
+      break;
+    }
+    TrsTrb++;
+    if ((UINT8) TrsTrb->Type == TRB_TYPE_LINK) {
+      ASSERT (((LINK_TRB *) TrsTrb)->TC != 0);
+      //
+      // set cycle bit in Link TRB as normal
+      //
+      ((LINK_TRB*)TrsTrb)->CycleBit = TrsRing->RingPCS & BIT0;
+      //
+      // Toggle PCS maintained by software
+      //
+      TrsRing->RingPCS = (TrsRing->RingPCS & BIT0) ? 0 : 1;
+      TrsTrb = (TRB_TEMPLATE *) TrsRing->RingSeg0;  // Use host address
+    }
+  }
+
+  ASSERT (Index != TrsRing->TrbNumber);
+
+  if (TrsTrb != TrsRing->RingEnqueue) {
+    TrsRing->RingEnqueue = TrsTrb;
+  }
+
+  //
+  // Clear the Trb context for enqueue, but reserve the PCS bit
+  //
+  TrsTrb->Parameter1 = 0;
+  TrsTrb->Parameter2 = 0;
+  TrsTrb->Status     = 0;
+  TrsTrb->RsvdZ1     = 0;
+  TrsTrb->Type       = 0;
+  TrsTrb->Control    = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Device.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing      The created transfer ring.
+
+**/
+VOID
+XhcPeiCreateTransferRing (
+  IN PEI_XHC_DEV            *Xhc,
+  IN UINTN                  TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  )
+{
+  VOID                  *Buf;
+  LINK_TRB              *EndTrb;
+  EFI_PHYSICAL_ADDRESS  PhyAddr;
+
+  Buf = UsbHcAllocateMem (Xhc->MemPool, sizeof (TRB_TEMPLATE) * TrbNum);
+  ASSERT (Buf != NULL);
+  ASSERT (((UINTN) Buf & 0x3F) == 0);
+  ZeroMem (Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+
+  TransferRing->RingSeg0     = Buf;
+  TransferRing->TrbNumber    = TrbNum;
+  TransferRing->RingEnqueue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingDequeue  = (TRB_TEMPLATE *) TransferRing->RingSeg0;
+  TransferRing->RingPCS      = 1;
+  //
+  // 4.9.2 Transfer Ring Management
+  // To form a ring (or circular queue) a Link TRB may be inserted at the end of a ring to
+  // point to the first TRB in the ring.
+  //
+  EndTrb        = (LINK_TRB *) ((UINTN) Buf + sizeof (TRB_TEMPLATE) * (TrbNum - 1));
+  EndTrb->Type  = TRB_TYPE_LINK;
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Buf, sizeof (TRB_TEMPLATE) * TrbNum);
+  EndTrb->PtrLo = XHC_LOW_32BIT (PhyAddr);
+  EndTrb->PtrHi = XHC_HIGH_32BIT (PhyAddr);
+  //
+  // Toggle Cycle (TC). When set to '1', the xHC shall toggle its interpretation of the Cycle bit.
+  //
+  EndTrb->TC    = 1;
+  //
+  // Set Cycle bit as other TRB PCS init value
+  //
+  EndTrb->CycleBit = 0;
+}
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc       The XHCI device to be initialized.
+
+**/
+VOID
+XhcPeiInitSched (
+  IN PEI_XHC_DEV        *Xhc
+  )
+{
+  VOID                  *Dcbaa;
+  EFI_PHYSICAL_ADDRESS  DcbaaPhy;
+  UINTN                 Size;
+  EFI_PHYSICAL_ADDRESS  CmdRingPhy;
+  UINT32                MaxScratchpadBufs;
+  UINT64                *ScratchBuf;
+  EFI_PHYSICAL_ADDRESS  ScratchPhy;
+  UINT64                *ScratchEntry;
+  EFI_PHYSICAL_ADDRESS  ScratchEntryPhy;
+  UINT32                Index;
+  EFI_STATUS            Status;
+
+  //
+  // Initialize memory management.
+  //
+  Xhc->MemPool = UsbHcInitMemPool ();
+  ASSERT (Xhc->MemPool != NULL);
+
+  //
+  // Program the Max Device Slots Enabled (MaxSlotsEn) field in the CONFIG register (5.4.7)
+  // to enable the device slots that system software is going to use.
+  //
+  Xhc->MaxSlotsEn = Xhc->HcSParams1.Data.MaxSlots;
+  ASSERT (Xhc->MaxSlotsEn >= 1 && Xhc->MaxSlotsEn <= 255);
+  XhcPeiWriteOpReg (Xhc, XHC_CONFIG_OFFSET, (XhcPeiReadOpReg (Xhc, XHC_CONFIG_OFFSET) & ~XHC_CONFIG_MASK) | Xhc->MaxSlotsEn);
+
+  //
+  // The Device Context Base Address Array entry associated with each allocated Device Slot
+  // shall contain a 64-bit pointer to the base of the associated Device Context.
+  // The Device Context Base Address Array shall contain MaxSlotsEn + 1 entries.
+  // Software shall set Device Context Base Address Array entries for unallocated Device Slots to '0'.
+  //
+  Size = (Xhc->MaxSlotsEn + 1) * sizeof (UINT64);
+  Dcbaa = UsbHcAllocateMem (Xhc->MemPool, Size);
+  ASSERT (Dcbaa != NULL);
+
+  //
+  // A Scratchpad Buffer is a PAGESIZE block of system memory located on a PAGESIZE boundary.
+  // System software shall allocate the Scratchpad Buffer(s) before placing the xHC in to Run
+  // mode (Run/Stop(R/S) ='1').
+  //
+  MaxScratchpadBufs      = ((Xhc->HcSParams2.Data.ScratchBufHi) << 5) | (Xhc->HcSParams2.Data.ScratchBufLo);
+  Xhc->MaxScratchpadBufs = MaxScratchpadBufs;
+  ASSERT (MaxScratchpadBufs <= 1023);
+  if (MaxScratchpadBufs != 0) {
+    //
+    // Allocate the buffer to record the host address for each entry
+    //
+    ScratchEntry = AllocateZeroPool (sizeof (UINT64) * MaxScratchpadBufs);
+    ASSERT (ScratchEntry != NULL);
+    Xhc->ScratchEntry = ScratchEntry;
+
+    ScratchPhy = 0;
+    Status = UsbHcAllocateAlignedPages (
+               EFI_SIZE_TO_PAGES (MaxScratchpadBufs * sizeof (UINT64)),
+               Xhc->PageSize,
+               (VOID **) &ScratchBuf,
+               &ScratchPhy
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    ZeroMem (ScratchBuf, MaxScratchpadBufs * sizeof (UINT64));
+    Xhc->ScratchBuf = ScratchBuf;
+
+    //
+    // Allocate each scratch buffer
+    //
+    for (Index = 0; Index < MaxScratchpadBufs; Index++) {
+      ScratchEntryPhy = 0;
+      Status = UsbHcAllocateAlignedPages (
+                 EFI_SIZE_TO_PAGES (Xhc->PageSize),
+                 Xhc->PageSize,
+                 (VOID **) &ScratchEntry[Index],
+                 &ScratchEntryPhy
+                 );
+      ASSERT_EFI_ERROR (Status);
+      ZeroMem ((VOID *) (UINTN) ScratchEntry[Index], Xhc->PageSize);
+      //
+      // Fill with the PCI device address
+      //
+      *ScratchBuf++ = ScratchEntryPhy;
+    }
+    //
+    // The Scratchpad Buffer Array contains pointers to the Scratchpad Buffers. Entry 0 of the
+    // Device Context Base Address Array points to the Scratchpad Buffer Array.
+    //
+    *(UINT64 *) Dcbaa = (UINT64) (UINTN) ScratchPhy;
+  }
+
+  //
+  // Program the Device Context Base Address Array Pointer (DCBAAP) register (5.4.6) with
+  // a 64-bit address pointing to where the Device Context Base Address Array is located.
+  //
+  Xhc->DCBAA = (UINT64 *) (UINTN) Dcbaa;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  DcbaaPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Dcbaa, Size);
+  XhcPeiWriteOpReg (Xhc, XHC_DCBAAP_OFFSET, XHC_LOW_32BIT (DcbaaPhy));
+  XhcPeiWriteOpReg (Xhc, XHC_DCBAAP_OFFSET + 4, XHC_HIGH_32BIT (DcbaaPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:DCBAA=0x%x\n", Xhc->DCBAA));
+
+  //
+  // Define the Command Ring Dequeue Pointer by programming the Command Ring Control Register
+  // (5.4.5) with a 64-bit address pointing to the starting address of the first TRB of the Command Ring.
+  // Note: The Command Ring is 64 byte aligned, so the low order 6 bits of the Command Ring Pointer shall
+  // always be '0'.
+  //
+  XhcPeiCreateTransferRing (Xhc, CMD_RING_TRB_NUMBER, &Xhc->CmdRing);
+  //
+  // The xHC uses the Enqueue Pointer to determine when a Transfer Ring is empty. As it fetches TRBs from a
+  // Transfer Ring it checks for a Cycle bit transition. If a transition detected, the ring is empty.
+  // So we set RCS as inverted PCS init value to let Command Ring empty
+  //
+  CmdRingPhy = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+  ASSERT ((CmdRingPhy & 0x3F) == 0);
+  CmdRingPhy |= XHC_CRCR_RCS;
+  //
+  // Some 3rd party XHCI external cards don't support single 64-bytes width register access,
+  // So divide it to two 32-bytes width register access.
+  //
+  XhcPeiWriteOpReg (Xhc, XHC_CRCR_OFFSET, XHC_LOW_32BIT (CmdRingPhy));
+  XhcPeiWriteOpReg (Xhc, XHC_CRCR_OFFSET + 4, XHC_HIGH_32BIT (CmdRingPhy));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:XHC_CRCR=0x%x\n", Xhc->CmdRing.RingSeg0));
+
+  //
+  // Disable the 'interrupter enable' bit in USB_CMD
+  // and clear IE & IP bit in all Interrupter X Management Registers.
+  //
+  XhcPeiClearOpRegBit (Xhc, XHC_USBCMD_OFFSET, XHC_USBCMD_INTE);
+  for (Index = 0; Index < (UINT16)(Xhc->HcSParams1.Data.MaxIntrs); Index++) {
+    XhcPeiClearRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IE);
+    XhcPeiSetRuntimeRegBit (Xhc, XHC_IMAN_OFFSET + (Index * 32), XHC_IMAN_IP);
+  }
+
+  //
+  // Allocate EventRing for Cmd, Ctrl, Bulk, Interrupt, AsynInterrupt transfer
+  //
+  XhcPeiCreateEventRing (Xhc, &Xhc->EventRing);
+  DEBUG ((EFI_D_INFO, "XhcPeiInitSched:XHC_EVENTRING=0x%x\n", Xhc->EventRing.EventRingSeg0));
+}
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc       The XHCI device.
+
+**/
+VOID
+XhcPeiFreeSched (
+  IN PEI_XHC_DEV    *Xhc
+  )
+{
+  UINT32                  Index;
+  UINT64                  *ScratchEntry;
+
+  if (Xhc->ScratchBuf != NULL) {
+    ScratchEntry = Xhc->ScratchEntry;
+    for (Index = 0; Index < Xhc->MaxScratchpadBufs; Index++) {
+      //
+      // Free Scratchpad Buffers
+      //
+      UsbHcFreeAlignedPages ((VOID*) (UINTN) ScratchEntry[Index], EFI_SIZE_TO_PAGES (Xhc->PageSize));
+    }
+    //
+    // Free Scratchpad Buffer Array
+    //
+    UsbHcFreeAlignedPages (Xhc->ScratchBuf, EFI_SIZE_TO_PAGES (Xhc->MaxScratchpadBufs * sizeof (UINT64)));
+    FreePool (Xhc->ScratchEntry);
+  }
+
+  if (Xhc->CmdRing.RingSeg0 != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->CmdRing.RingSeg0, sizeof (TRB_TEMPLATE) * CMD_RING_TRB_NUMBER);
+    Xhc->CmdRing.RingSeg0 = NULL;
+  }
+
+  XhcPeiFreeEventRing (Xhc,&Xhc->EventRing);
+
+  if (Xhc->DCBAA != NULL) {
+    UsbHcFreeMem (Xhc->MemPool, Xhc->DCBAA, (Xhc->MaxSlotsEn + 1) * sizeof (UINT64));
+    Xhc->DCBAA = NULL;
+  }
+
+  //
+  // Free memory pool at last
+  //
+  if (Xhc->MemPool != NULL) {
+    UsbHcFreeMemPool (Xhc->MemPool);
+    Xhc->MemPool = NULL;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h
new file mode 100644
index 0000000000..b3d4c45614
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h
@@ -0,0 +1,1307 @@
+/** @file
+Private Header file for Usb Host Controller PEIM
+
+Copyright (c) 2014 - 2015, 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.
+
+**/
+
+#ifndef _EFI_PEI_XHCI_SCHED_H_
+#define _EFI_PEI_XHCI_SCHED_H_
+
+//
+// Transfer types, used in URB to identify the transfer type
+//
+#define XHC_CTRL_TRANSFER                       0x01
+#define XHC_BULK_TRANSFER                       0x02
+
+//
+// 6.4.6 TRB Types
+//
+#define TRB_TYPE_NORMAL                         1
+#define TRB_TYPE_SETUP_STAGE                    2
+#define TRB_TYPE_DATA_STAGE                     3
+#define TRB_TYPE_STATUS_STAGE                   4
+#define TRB_TYPE_ISOCH                          5
+#define TRB_TYPE_LINK                           6
+#define TRB_TYPE_EVENT_DATA                     7
+#define TRB_TYPE_NO_OP                          8
+#define TRB_TYPE_EN_SLOT                        9
+#define TRB_TYPE_DIS_SLOT                       10
+#define TRB_TYPE_ADDRESS_DEV                    11
+#define TRB_TYPE_CON_ENDPOINT                   12
+#define TRB_TYPE_EVALU_CONTXT                   13
+#define TRB_TYPE_RESET_ENDPOINT                 14
+#define TRB_TYPE_STOP_ENDPOINT                  15
+#define TRB_TYPE_SET_TR_DEQUE                   16
+#define TRB_TYPE_RESET_DEV                      17
+#define TRB_TYPE_GET_PORT_BANW                  21
+#define TRB_TYPE_FORCE_HEADER                   22
+#define TRB_TYPE_NO_OP_COMMAND                  23
+#define TRB_TYPE_TRANS_EVENT                    32
+#define TRB_TYPE_COMMAND_COMPLT_EVENT           33
+#define TRB_TYPE_PORT_STATUS_CHANGE_EVENT       34
+#define TRB_TYPE_HOST_CONTROLLER_EVENT          37
+#define TRB_TYPE_DEVICE_NOTIFI_EVENT            38
+#define TRB_TYPE_MFINDEX_WRAP_EVENT             39
+
+//
+// Endpoint Type (EP Type).
+//
+#define ED_NOT_VALID                            0
+#define ED_ISOCH_OUT                            1
+#define ED_BULK_OUT                             2
+#define ED_INTERRUPT_OUT                        3
+#define ED_CONTROL_BIDIR                        4
+#define ED_ISOCH_IN                             5
+#define ED_BULK_IN                              6
+#define ED_INTERRUPT_IN                         7
+
+//
+// 6.4.5 TRB Completion Codes
+//
+#define TRB_COMPLETION_INVALID                  0
+#define TRB_COMPLETION_SUCCESS                  1
+#define TRB_COMPLETION_DATA_BUFFER_ERROR        2
+#define TRB_COMPLETION_BABBLE_ERROR             3
+#define TRB_COMPLETION_USB_TRANSACTION_ERROR    4
+#define TRB_COMPLETION_TRB_ERROR                5
+#define TRB_COMPLETION_STALL_ERROR              6
+#define TRB_COMPLETION_SHORT_PACKET             13
+
+//
+// The topology string used to present usb device location
+//
+typedef struct _USB_DEV_TOPOLOGY {
+  //
+  // The tier concatenation of down stream port.
+  //
+  UINT32 RouteString:20;
+  //
+  // The root port number of the chain.
+  //
+  UINT32 RootPortNum:8;
+  //
+  // The Tier the device reside.
+  //
+  UINT32 TierNum:4;
+} USB_DEV_TOPOLOGY;
+
+//
+// USB Device's RouteChart
+//
+typedef union _USB_DEV_ROUTE {
+  UINT32                    Dword;
+  USB_DEV_TOPOLOGY          Route;
+} USB_DEV_ROUTE;
+
+//
+// Endpoint address and its capabilities
+//
+typedef struct _USB_ENDPOINT {
+  //
+  // Store logical device address assigned by UsbBus
+  // It's because some XHCI host controllers may assign the same physcial device
+  // address for those devices inserted at different root port.
+  //
+  UINT8                     BusAddr;
+  UINT8                     DevAddr;
+  UINT8                     EpAddr;
+  EFI_USB_DATA_DIRECTION    Direction;
+  UINT8                     DevSpeed;
+  UINTN                     MaxPacket;
+  UINTN                     Type;
+} USB_ENDPOINT;
+
+//
+// TRB Template
+//
+typedef struct _TRB_TEMPLATE {
+  UINT32                    Parameter1;
+
+  UINT32                    Parameter2;
+
+  UINT32                    Status;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:9;
+  UINT32                    Type:6;
+  UINT32                    Control:16;
+} TRB_TEMPLATE;
+
+typedef struct _TRANSFER_RING {
+  VOID                      *RingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *RingEnqueue;
+  TRB_TEMPLATE              *RingDequeue;
+  UINT32                    RingPCS;
+} TRANSFER_RING;
+
+typedef struct _EVENT_RING {
+  VOID                      *ERSTBase;
+  VOID                      *EventRingSeg0;
+  UINTN                     TrbNumber;
+  TRB_TEMPLATE              *EventRingEnqueue;
+  TRB_TEMPLATE              *EventRingDequeue;
+  UINT32                    EventRingCCS;
+} EVENT_RING;
+
+#define XHC_URB_SIG                 SIGNATURE_32 ('U', 'S', 'B', 'R')
+
+//
+// URB (Usb Request Block) contains information for all kinds of
+// usb requests.
+//
+typedef struct _URB {
+  UINT32                            Signature;
+  //
+  // Usb Device URB related information
+  //
+  USB_ENDPOINT                      Ep;
+  EFI_USB_DEVICE_REQUEST            *Request;
+  VOID                              *Data;
+  UINTN                             DataLen;
+  VOID                              *DataPhy;
+  EFI_ASYNC_USB_TRANSFER_CALLBACK   Callback;
+  VOID                              *Context;
+  //
+  // Execute result
+  //
+  UINT32                            Result;
+  //
+  // completed data length
+  //
+  UINTN                             Completed;
+  //
+  // Command/Tranfer Ring info
+  //
+  TRANSFER_RING                     *Ring;
+  TRB_TEMPLATE                      *TrbStart;
+  TRB_TEMPLATE                      *TrbEnd;
+  UINTN                             TrbNum;
+  BOOLEAN                           StartDone;
+  BOOLEAN                           EndDone;
+  BOOLEAN                           Finished;
+
+  TRB_TEMPLATE                      *EvtTrb;
+} URB;
+
+//
+// 6.5 Event Ring Segment Table
+// The Event Ring Segment Table is used to define multi-segment Event Rings and to enable runtime
+// expansion and shrinking of the Event Ring. The location of the Event Ring Segment Table is defined by the
+// Event Ring Segment Table Base Address Register (5.5.2.3.2). The size of the Event Ring Segment Table
+// is defined by the Event Ring Segment Table Base Size Register (5.5.2.3.1).
+//
+typedef struct _EVENT_RING_SEG_TABLE_ENTRY {
+  UINT32                    PtrLo;
+  UINT32                    PtrHi;
+  UINT32                    RingTrbSize:16;
+  UINT32                    RsvdZ1:16;
+  UINT32                    RsvdZ2;
+} EVENT_RING_SEG_TABLE_ENTRY;
+
+//
+// 6.4.1.1 Normal TRB
+// A Normal TRB is used in several ways; exclusively on Bulk and Interrupt Transfer Rings for normal and
+// Scatter/Gather operations, to define additional data buffers for Scatter/Gather operations on Isoch Transfer
+// Rings, and to define the Data stage information for Control Transfer Rings.
+//
+typedef struct _TRANSFER_TRB_NORMAL {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:17;
+  UINT32                    TDSize:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    ISP:1;
+  UINT32                    NS:1;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ1:2;
+  UINT32                    BEI:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ2:16;
+} TRANSFER_TRB_NORMAL;
+
+//
+// 6.4.1.2.1 Setup Stage TRB
+// A Setup Stage TRB is created by system software to initiate a USB Setup packet on a control endpoint.
+//
+typedef struct _TRANSFER_TRB_CONTROL_SETUP {
+  UINT32                    bmRequestType:8;
+  UINT32                    bRequest:8;
+  UINT32                    wValue:16;
+
+  UINT32                    wIndex:16;
+  UINT32                    wLength:16;
+
+  UINT32                    Length:17;
+  UINT32                    RsvdZ1:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:4;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ3:3;
+  UINT32                    Type:6;
+  UINT32                    TRT:2;
+  UINT32                    RsvdZ4:14;
+} TRANSFER_TRB_CONTROL_SETUP;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_DATA {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:17;
+  UINT32                    TDSize:5;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    ISP:1;
+  UINT32                    NS:1;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    IDT:1;
+  UINT32                    RsvdZ1:3;
+  UINT32                    Type:6;
+  UINT32                    DIR:1;
+  UINT32                    RsvdZ2:15;
+} TRANSFER_TRB_CONTROL_DATA;
+
+//
+// 6.4.1.2.2 Data Stage TRB
+// A Data Stage TRB is used generate the Data stage transaction of a USB Control transfer.
+//
+typedef struct _TRANSFER_TRB_CONTROL_STATUS {
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    RsvdZ3:22;
+  UINT32                    IntTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    ENT:1;
+  UINT32                    RsvdZ4:2;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    RsvdZ5:4;
+  UINT32                    Type:6;
+  UINT32                    DIR:1;
+  UINT32                    RsvdZ6:15;
+} TRANSFER_TRB_CONTROL_STATUS;
+
+//
+// 6.4.2.1 Transfer Event TRB
+// A Transfer Event provides the completion status associated with a Transfer TRB. Refer to section 4.11.3.1
+// for more information on the use and operation of Transfer Events.
+//
+typedef struct _EVT_TRB_TRANSFER {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    Length:24;
+  UINT32                    Completecode:8;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ1:1;
+  UINT32                    ED:1;
+  UINT32                    RsvdZ2:7;
+  UINT32                    Type:6;
+  UINT32                    EndpointId:5;
+  UINT32                    RsvdZ3:3;
+  UINT32                    SlotId:8;
+} EVT_TRB_TRANSFER;
+
+//
+// 6.4.2.2 Command Completion Event TRB
+// A Command Completion Event TRB shall be generated by the xHC when a command completes on the
+// Command Ring. Refer to section 4.11.4 for more information on the use of Command Completion Events.
+//
+typedef struct _EVT_TRB_COMMAND_COMPLETION {
+  UINT32                    TRBPtrLo;
+
+  UINT32                    TRBPtrHi;
+
+  UINT32                    RsvdZ2:24;
+  UINT32                    Completecode:8;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    VFID:8;
+  UINT32                    SlotId:8;
+} EVT_TRB_COMMAND_COMPLETION;
+
+typedef union _TRB {
+  TRB_TEMPLATE                  TrbTemplate;
+  TRANSFER_TRB_NORMAL           TrbNormal;
+  TRANSFER_TRB_CONTROL_SETUP    TrbCtrSetup;
+  TRANSFER_TRB_CONTROL_DATA     TrbCtrData;
+  TRANSFER_TRB_CONTROL_STATUS   TrbCtrStatus;
+} TRB;
+
+//
+// 6.4.3.1 No Op Command TRB
+// The No Op Command TRB provides a simple means for verifying the operation of the Command Ring
+// mechanisms offered by the xHCI.
+//
+typedef struct _CMD_TRB_NO_OP {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} CMD_TRB_NO_OP;
+
+//
+// 6.4.3.2 Enable Slot Command TRB
+// The Enable Slot Command TRB causes the xHC to select an available Device Slot and return the ID of the
+// selected slot to the host in a Command Completion Event.
+//
+typedef struct _CMD_TRB_ENABLE_SLOT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} CMD_TRB_ENABLE_SLOT;
+
+//
+// 6.4.3.3 Disable Slot Command TRB
+// The Disable Slot Command TRB releases any bandwidth assigned to the disabled slot and frees any
+// internal xHC resources assigned to the slot.
+//
+typedef struct _CMD_TRB_DISABLE_SLOT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_DISABLE_SLOT;
+
+//
+// 6.4.3.4 Address Device Command TRB
+// The Address Device Command TRB transitions the selected Device Context from the Default to the
+// Addressed state and causes the xHC to select an address for the USB device in the Default State and
+// issue a SET_ADDRESS request to the USB device.
+//
+typedef struct _CMD_TRB_ADDRESS_DEVICE {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:8;
+  UINT32                    BSR:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_ADDRESS_DEVICE;
+
+//
+// 6.4.3.5 Configure Endpoint Command TRB
+// The Configure Endpoint Command TRB evaluates the bandwidth and resource requirements of the
+// endpoints selected by the command.
+//
+typedef struct _CMD_TRB_CONFIG_ENDPOINT {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:8;
+  UINT32                    DC:1;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_CONFIG_ENDPOINT;
+
+//
+// 6.4.3.6 Evaluate Context Command TRB
+// The Evaluate Context Command TRB is used by system software to inform the xHC that the selected
+// Context data structures in the Device Context have been modified by system software and that the xHC
+// shall evaluate any changes
+//
+typedef struct _CMD_TRB_EVALUATE_CONTEXT {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:9;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ3:8;
+  UINT32                    SlotId:8;
+} CMD_TRB_EVALUATE_CONTEXT;
+
+//
+// 6.4.3.7 Reset Endpoint Command TRB
+// The Reset Endpoint Command TRB is used by system software to reset a specified Transfer Ring
+//
+typedef struct _CMD_TRB_RESET_ENDPOINT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:8;
+  UINT32                    TSP:1;
+  UINT32                    Type:6;
+  UINT32                    EDID:5;
+  UINT32                    RsvdZ4:3;
+  UINT32                    SlotId:8;
+} CMD_TRB_RESET_ENDPOINT;
+
+//
+// 6.4.3.8 Stop Endpoint Command TRB
+// The Stop Endpoint Command TRB command allows software to stop the xHC execution of the TDs on a
+// Transfer Ring and temporarily take ownership of TDs that had previously been passed to the xHC.
+//
+typedef struct _CMD_TRB_STOP_ENDPOINT {
+  UINT32                    RsvdZ0;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ3:9;
+  UINT32                    Type:6;
+  UINT32                    EDID:5;
+  UINT32                    RsvdZ4:2;
+  UINT32                    SP:1;
+  UINT32                    SlotId:8;
+} CMD_TRB_STOP_ENDPOINT;
+
+//
+// 6.4.3.9 Set TR Dequeue Pointer Command TRB
+// The Set TR Dequeue Pointer Command TRB is used by system software to modify the TR Dequeue
+// Pointer and DCS fields of an Endpoint or Stream Context.
+//
+typedef struct _CMD_SET_TR_DEQ_POINTER {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1:16;
+  UINT32                    StreamID:16;
+
+  UINT32                    CycleBit:1;
+  UINT32                    RsvdZ2:9;
+  UINT32                    Type:6;
+  UINT32                    Endpoint:5;
+  UINT32                    RsvdZ3:3;
+  UINT32                    SlotId:8;
+} CMD_SET_TR_DEQ_POINTER;
+
+//
+// 6.4.4.1 Link TRB
+// A Link TRB provides support for non-contiguous TRB Rings.
+//
+typedef struct _LINK_TRB {
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    RsvdZ1:22;
+  UINT32                    InterTarget:10;
+
+  UINT32                    CycleBit:1;
+  UINT32                    TC:1;
+  UINT32                    RsvdZ2:2;
+  UINT32                    CH:1;
+  UINT32                    IOC:1;
+  UINT32                    RsvdZ3:4;
+  UINT32                    Type:6;
+  UINT32                    RsvdZ4:16;
+} LINK_TRB;
+
+//
+// 6.2.2 Slot Context
+//
+typedef struct _SLOT_CONTEXT {
+  UINT32                    RouteString:20;
+  UINT32                    Speed:4;
+  UINT32                    RsvdZ1:1;
+  UINT32                    MTT:1;
+  UINT32                    Hub:1;
+  UINT32                    ContextEntries:5;
+
+  UINT32                    MaxExitLatency:16;
+  UINT32                    RootHubPortNum:8;
+  UINT32                    PortNum:8;
+
+  UINT32                    TTHubSlotId:8;
+  UINT32                    TTPortNum:8;
+  UINT32                    TTT:2;
+  UINT32                    RsvdZ2:4;
+  UINT32                    InterTarget:10;
+
+  UINT32                    DeviceAddress:8;
+  UINT32                    RsvdZ3:19;
+  UINT32                    SlotState:5;
+
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+} SLOT_CONTEXT;
+
+typedef struct _SLOT_CONTEXT_64 {
+  UINT32                    RouteString:20;
+  UINT32                    Speed:4;
+  UINT32                    RsvdZ1:1;
+  UINT32                    MTT:1;
+  UINT32                    Hub:1;
+  UINT32                    ContextEntries:5;
+
+  UINT32                    MaxExitLatency:16;
+  UINT32                    RootHubPortNum:8;
+  UINT32                    PortNum:8;
+
+  UINT32                    TTHubSlotId:8;
+  UINT32                    TTPortNum:8;
+  UINT32                    TTT:2;
+  UINT32                    RsvdZ2:4;
+  UINT32                    InterTarget:10;
+
+  UINT32                    DeviceAddress:8;
+  UINT32                    RsvdZ3:19;
+  UINT32                    SlotState:5;
+
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+  UINT32                    RsvdZ15;
+
+} SLOT_CONTEXT_64;
+
+
+//
+// 6.2.3 Endpoint Context
+//
+typedef struct _ENDPOINT_CONTEXT {
+  UINT32                    EPState:3;
+  UINT32                    RsvdZ1:5;
+  UINT32                    Mult:2;
+  UINT32                    MaxPStreams:5;
+  UINT32                    LSA:1;
+  UINT32                    Interval:8;
+  UINT32                    RsvdZ2:8;
+
+  UINT32                    RsvdZ3:1;
+  UINT32                    CErr:2;
+  UINT32                    EPType:3;
+  UINT32                    RsvdZ4:1;
+  UINT32                    HID:1;
+  UINT32                    MaxBurstSize:8;
+  UINT32                    MaxPacketSize:16;
+
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    AverageTRBLength:16;
+  UINT32                    MaxESITPayload:16;
+
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+} ENDPOINT_CONTEXT;
+
+typedef struct _ENDPOINT_CONTEXT_64 {
+  UINT32                    EPState:3;
+  UINT32                    RsvdZ1:5;
+  UINT32                    Mult:2;
+  UINT32                    MaxPStreams:5;
+  UINT32                    LSA:1;
+  UINT32                    Interval:8;
+  UINT32                    RsvdZ2:8;
+
+  UINT32                    RsvdZ3:1;
+  UINT32                    CErr:2;
+  UINT32                    EPType:3;
+  UINT32                    RsvdZ4:1;
+  UINT32                    HID:1;
+  UINT32                    MaxBurstSize:8;
+  UINT32                    MaxPacketSize:16;
+
+  UINT32                    PtrLo;
+
+  UINT32                    PtrHi;
+
+  UINT32                    AverageTRBLength:16;
+  UINT32                    MaxESITPayload:16;
+
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+  UINT32                    RsvdZ15;
+
+} ENDPOINT_CONTEXT_64;
+
+
+//
+// 6.2.5.1 Input Control Context
+//
+typedef struct _INPUT_CONTRL_CONTEXT {
+  UINT32                    Dword1;
+  UINT32                    Dword2;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+  UINT32                    RsvdZ3;
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+} INPUT_CONTRL_CONTEXT;
+
+typedef struct _INPUT_CONTRL_CONTEXT_64 {
+  UINT32                    Dword1;
+  UINT32                    Dword2;
+  UINT32                    RsvdZ1;
+  UINT32                    RsvdZ2;
+  UINT32                    RsvdZ3;
+  UINT32                    RsvdZ4;
+  UINT32                    RsvdZ5;
+  UINT32                    RsvdZ6;
+  UINT32                    RsvdZ7;
+  UINT32                    RsvdZ8;
+  UINT32                    RsvdZ9;
+  UINT32                    RsvdZ10;
+  UINT32                    RsvdZ11;
+  UINT32                    RsvdZ12;
+  UINT32                    RsvdZ13;
+  UINT32                    RsvdZ14;
+} INPUT_CONTRL_CONTEXT_64;
+
+//
+// 6.2.1 Device Context
+//
+typedef struct _DEVICE_CONTEXT {
+  SLOT_CONTEXT              Slot;
+  ENDPOINT_CONTEXT          EP[31];
+} DEVICE_CONTEXT;
+
+typedef struct _DEVICE_CONTEXT_64 {
+  SLOT_CONTEXT_64           Slot;
+  ENDPOINT_CONTEXT_64       EP[31];
+} DEVICE_CONTEXT_64;
+
+//
+// 6.2.5 Input Context
+//
+typedef struct _INPUT_CONTEXT {
+  INPUT_CONTRL_CONTEXT      InputControlContext;
+  SLOT_CONTEXT              Slot;
+  ENDPOINT_CONTEXT          EP[31];
+} INPUT_CONTEXT;
+
+typedef struct _INPUT_CONTEXT_64 {
+  INPUT_CONTRL_CONTEXT_64   InputControlContext;
+  SLOT_CONTEXT_64           Slot;
+  ENDPOINT_CONTEXT_64       EP[31];
+} INPUT_CONTEXT_64;
+
+/**
+  Execute the transfer by polling the URB. This is a synchronous operation.
+
+  @param  Xhc               The XHCI device.
+  @param  CmdTransfer       The executed URB is for cmd transfer or not.
+  @param  Urb               The URB to execute.
+  @param  Timeout           The time to wait before abort, in millisecond.
+
+  @return EFI_DEVICE_ERROR  The transfer failed due to transfer error.
+  @return EFI_TIMEOUT       The transfer failed due to time out.
+  @return EFI_SUCCESS       The transfer finished OK.
+
+**/
+EFI_STATUS
+XhcPeiExecTransfer (
+  IN PEI_XHC_DEV            *Xhc,
+  IN BOOLEAN                CmdTransfer,
+  IN URB                    *Urb,
+  IN UINTN                  Timeout
+  );
+
+/**
+  Find out the actual device address according to the requested device address from UsbBus.
+
+  @param  Xhc           The XHCI device.
+  @param  BusDevAddr    The requested device address by UsbBus upper driver.
+
+  @return The actual device address assigned to the device.
+
+**/
+UINT8
+XhcPeiBusDevAddrToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              BusDevAddr
+  );
+
+/**
+  Find out the slot id according to the device's route string.
+
+  @param  Xhc           The XHCI device.
+  @param  RouteString   The route string described the device location.
+
+  @return The slot id used by the device.
+
+**/
+UINT8
+XhcPeiRouteStringToSlotId (
+  IN PEI_XHC_DEV        *Xhc,
+  IN USB_DEV_ROUTE      RouteString
+  );
+
+/**
+  Calculate the device context index by endpoint address and direction.
+
+  @param  EpAddr        The target endpoint number.
+  @param  Direction     The direction of the target endpoint.
+
+  @return The device context index of endpoint.
+
+**/
+UINT8
+XhcPeiEndpointToDci (
+  IN UINT8                      EpAddr,
+  IN EFI_USB_DATA_DIRECTION     Direction
+  );
+
+/**
+  Ring the door bell to notify XHCI there is a transaction to be executed.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+**/
+VOID
+XhcPeiRingDoorBell (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Monitor the port status change. Enable/Disable device slot if there is a device attached/detached.
+
+  @param  Xhc               The XHCI device.
+  @param  ParentRouteChart  The route string pointed to the parent device if it exists.
+  @param  Port              The port to be polled.
+  @param  PortState         The port state.
+
+  @retval EFI_SUCCESS       Successfully enable/disable device slot according to port state.
+  @retval Others            Should not appear.
+
+**/
+EFI_STATUS
+XhcPeiPollPortStatusChange (
+  IN PEI_XHC_DEV            *Xhc,
+  IN USB_DEV_ROUTE          ParentRouteChart,
+  IN UINT8                  Port,
+  IN EFI_USB_PORT_STATUS    *PortState
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  );
+
+/**
+  Evaluate the slot context for hub device through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  PortNum       The total number of downstream port supported by the hub.
+  @param  TTT           The TT think time of the hub device.
+  @param  MTT           The multi-TT of the hub device.
+
+  @retval EFI_SUCCESS   Successfully configure the hub device's slot context.
+
+**/
+EFI_STATUS
+XhcPeiConfigHubContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      PortNum,
+  IN UINT8                      TTT,
+  IN UINT8                      MTT
+  );
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  );
+
+/**
+  Configure all the device endpoints through XHCI's Configure_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be configured.
+  @param  DeviceSpeed   The device's speed.
+  @param  ConfigDesc    The pointer to the usb device configuration descriptor.
+
+  @retval EFI_SUCCESS   Successfully configure all the device endpoints.
+
+**/
+EFI_STATUS
+XhcPeiSetConfigCmd64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT8                      DeviceSpeed,
+  IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
+  );
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  );
+
+/**
+  Assign and initialize the device slot for a new device.
+
+  @param  Xhc                   The XHCI device.
+  @param  ParentRouteChart      The route string pointed to the parent device.
+  @param  ParentPort            The port at which the device is located.
+  @param  RouteChart            The route string pointed to the device.
+  @param  DeviceSpeed           The device speed.
+
+  @retval EFI_SUCCESS           Successfully assign a slot to the device and assign an address to it.
+  @retval Others                Fail to initialize device slot.
+
+**/
+EFI_STATUS
+XhcPeiInitializeDeviceSlot64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN USB_DEV_ROUTE              ParentRouteChart,
+  IN UINT16                     ParentPort,
+  IN USB_DEV_ROUTE              RouteChart,
+  IN UINT8                      DeviceSpeed
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  );
+
+/**
+  Evaluate the endpoint 0 context through XHCI's Evaluate_Context cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be evaluated.
+  @param  MaxPacketSize The max packet size supported by the device control transfer.
+
+  @retval EFI_SUCCESS   Successfully evaluate the device endpoint 0.
+
+**/
+EFI_STATUS
+XhcPeiEvaluateContext64 (
+  IN PEI_XHC_DEV                *Xhc,
+  IN UINT8                      SlotId,
+  IN UINT32                     MaxPacketSize
+  );
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd (
+  IN PEI_XHC_DEV              *Xhc,
+  IN UINT8                    SlotId
+  );
+
+/**
+  Disable the specified device slot.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id to be disabled.
+
+  @retval EFI_SUCCESS   Successfully disable the device slot.
+
+**/
+EFI_STATUS
+XhcPeiDisableSlotCmd64 (
+  IN PEI_XHC_DEV              *Xhc,
+  IN UINT8                    SlotId
+  );
+
+/**
+  System software shall use a Reset Endpoint Command (section 4.11.4.7) to remove the Halted
+  condition in the xHC. After the successful completion of the Reset Endpoint Command, the Endpoint
+  Context is transitioned from the Halted to the Stopped state and the Transfer Ring of the endpoint is
+  reenabled. The next write to the Doorbell of the Endpoint will transition the Endpoint Context from the
+  Stopped to the Running state.
+
+  @param  Xhc           The XHCI device.
+  @param  Urb           The urb which makes the endpoint halted.
+
+  @retval EFI_SUCCESS   The recovery is successful.
+  @retval Others        Failed to recovery halted endpoint.
+
+**/
+EFI_STATUS
+XhcPeiRecoverHaltedEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  );
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  );
+
+/**
+  Create a new URB for a new transaction.
+
+  @param  Xhc       The XHCI device
+  @param  DevAddr   The device address
+  @param  EpAddr    Endpoint addrress
+  @param  DevSpeed  The device speed
+  @param  MaxPacket The max packet length of the endpoint
+  @param  Type      The transaction type
+  @param  Request   The standard USB request for control transfer
+  @param  Data      The user data to transfer
+  @param  DataLen   The length of data buffer
+  @param  Callback  The function to call when data is transferred
+  @param  Context   The context to the callback
+
+  @return Created URB or NULL
+
+**/
+URB*
+XhcPeiCreateUrb (
+  IN PEI_XHC_DEV                        *Xhc,
+  IN UINT8                              DevAddr,
+  IN UINT8                              EpAddr,
+  IN UINT8                              DevSpeed,
+  IN UINTN                              MaxPacket,
+  IN UINTN                              Type,
+  IN EFI_USB_DEVICE_REQUEST             *Request,
+  IN VOID                               *Data,
+  IN UINTN                              DataLen,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK    Callback,
+  IN VOID                               *Context
+  );
+
+/**
+  Free an allocated URB.
+
+  @param  Xhc       The XHCI device.
+  @param  Urb       The URB to free.
+
+**/
+VOID
+XhcPeiFreeUrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  );
+
+/**
+  Create a transfer TRB.
+
+  @param  Xhc       The XHCI device
+  @param  Urb       The urb used to construct the transfer TRB.
+
+  @return Created TRB or NULL
+
+**/
+EFI_STATUS
+XhcPeiCreateTransferTrb (
+  IN PEI_XHC_DEV    *Xhc,
+  IN URB            *Urb
+  );
+
+/**
+  Synchronize the specified transfer ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  TrsRing   The transfer ring to sync.
+
+  @retval EFI_SUCCESS The transfer ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncTrsRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN TRANSFER_RING  *TrsRing
+  );
+
+/**
+  Create XHCI transfer ring.
+
+  @param  Xhc               The XHCI Device.
+  @param  TrbNum            The number of TRB in the ring.
+  @param  TransferRing      The created transfer ring.
+
+**/
+VOID
+XhcPeiCreateTransferRing (
+  IN PEI_XHC_DEV            *Xhc,
+  IN UINTN                  TrbNum,
+  OUT TRANSFER_RING         *TransferRing
+  );
+
+/**
+  Check if there is a new generated event.
+
+  @param  Xhc           The XHCI device.
+  @param  EvtRing       The event ring to check.
+  @param  NewEvtTrb     The new event TRB found.
+
+  @retval EFI_SUCCESS   Found a new event TRB at the event ring.
+  @retval EFI_NOT_READY The event ring has no new event.
+
+**/
+EFI_STATUS
+XhcPeiCheckNewEvent (
+  IN PEI_XHC_DEV        *Xhc,
+  IN EVENT_RING         *EvtRing,
+  OUT TRB_TEMPLATE      **NewEvtTrb
+  );
+
+/**
+  Synchronize the specified event ring to update the enqueue and dequeue pointer.
+
+  @param  Xhc       The XHCI device.
+  @param  EvtRing   The event ring to sync.
+
+  @retval EFI_SUCCESS The event ring is synchronized successfully.
+
+**/
+EFI_STATUS
+XhcPeiSyncEventRing (
+  IN PEI_XHC_DEV    *Xhc,
+  IN EVENT_RING     *EvtRing
+  );
+
+/**
+  Create XHCI event ring.
+
+  @param  Xhc           The XHCI device.
+  @param  EventRing     The created event ring.
+
+**/
+VOID
+XhcPeiCreateEventRing (
+  IN PEI_XHC_DEV        *Xhc,
+  OUT EVENT_RING        *EventRing
+  );
+
+/**
+  Initialize the XHCI host controller for schedule.
+
+  @param  Xhc       The XHCI device to be initialized.
+
+**/
+VOID
+XhcPeiInitSched (
+  IN PEI_XHC_DEV        *Xhc
+  );
+
+/**
+  Free the resouce allocated at initializing schedule.
+
+  @param  Xhc       The XHCI device.
+
+**/
+VOID
+XhcPeiFreeSched (
+  IN PEI_XHC_DEV    *Xhc
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c
new file mode 100644
index 0000000000..e8ba85a9e7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ComponentName.c
@@ -0,0 +1,177 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for SCSI bus driver.
+
+Copyright (c) 2006 - 2011, 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 "ScsiBus.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gScsiBusComponentName = {
+  ScsiBusComponentNameGetDriverName,
+  ScsiBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gScsiBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ScsiBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ScsiBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mScsiBusDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"SCSI Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mScsiBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gScsiBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c
new file mode 100644
index 0000000000..0802b61726
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.c
@@ -0,0 +1,1512 @@
+/** @file
+  SCSI Bus driver that layers on every SCSI Pass Thru and
+  Extended SCSI Pass Thru protocol in the system.
+
+Copyright (c) 2006 - 2015, 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 "ScsiBus.h"
+
+
+EFI_DRIVER_BINDING_PROTOCOL gSCSIBusDriverBinding = {
+  SCSIBusDriverBindingSupported,
+  SCSIBusDriverBindingStart,
+  SCSIBusDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+VOID  *mWorkingBuffer;
+
+/**
+  Convert EFI_SCSI_IO_SCSI_REQUEST_PACKET packet to EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet.
+
+  @param  Packet         The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+  @param  CommandPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiioToPassThruPacket (
+  IN      EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet,
+  OUT     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *CommandPacket
+  );
+
+/**
+  Convert EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet to EFI_SCSI_IO_SCSI_REQUEST_PACKET packet.
+
+  @param  ScsiPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+  @param  Packet      The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+PassThruToScsiioPacket (
+  IN     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ScsiPacket,
+  OUT    EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet
+  );
+
+/**
+  Notify Function in which convert EFI1.0 PassThru Packet back to UEF2.0
+  SCSI IO Packet.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+NotifyFunction (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  );
+
+/**
+  Allocates an aligned buffer for SCSI device.
+
+  This function allocates an aligned buffer for the SCSI device to perform
+  SCSI pass through operations. The alignment requirement is from SCSI pass
+  through interface.
+
+  @param  ScsiIoDevice      The SCSI child device involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN SCSI_IO_DEV              *ScsiIoDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), ScsiIoDevice->ScsiIo.IoAlign);
+}
+
+/**
+  Frees an aligned buffer for SCSI device.
+
+  This function frees an aligned buffer for the SCSI device to perform
+  SCSI pass through operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+/**
+  The user Entry Point for module ScsiBus. The user code starts with this function.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeScsiBus(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gSCSIBusDriverBinding,
+             ImageHandle,
+             &gScsiBusComponentName,
+             &gScsiBusComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Supported() it must also follow these calling
+  restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SCSI_PASS_THRU_PROTOCOL     *PassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL *ExtPassThru;
+  UINT64                          Lun;
+  UINT8                           *TargetId;
+  SCSI_TARGET_ID                  ScsiTargetId;
+
+  TargetId = &ScsiTargetId.ScsiId.ExtScsi[0];
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+
+  //
+  // To keep backward compatibility, UEFI ExtPassThru Protocol is supported as well as 
+  // EFI PassThru Protocol. From priority perspective, ExtPassThru Protocol is firstly
+  // tried to open on host controller handle. If fails, then PassThru Protocol is tried instead.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **)&ExtPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  } else if (!EFI_ERROR(Status)) {
+    //
+    // Check if RemainingDevicePath is NULL or the End of Device Path Node,
+    // if yes, return EFI_SUCCESS.
+    //
+    if ((RemainingDevicePath == NULL) || IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // Close protocol regardless of RemainingDevicePath validation
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );      
+      return EFI_SUCCESS;
+    } else {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node, check its validation
+      //
+      Status = ExtPassThru->GetTargetLun (ExtPassThru, RemainingDevicePath, &TargetId, &Lun);
+      //
+      // Close protocol regardless of RemainingDevicePath validation
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );      
+      if (!EFI_ERROR(Status)) {
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Come here in 2 condition: 
+  // 1. ExtPassThru doesn't exist.
+  // 2. ExtPassThru exists but RemainingDevicePath is invalid.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiPassThruProtocolGuid,
+                  (VOID **)&PassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+  
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Test RemainingDevicePath is valid or not.
+  //
+  if ((RemainingDevicePath != NULL) && !IsDevicePathEnd (RemainingDevicePath)) {
+    Status = PassThru->GetTargetLun (PassThru, RemainingDevicePath, &ScsiTargetId.ScsiId.Scsi, &Lun);
+  }
+  
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiPassThruProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  UINT64                                Lun;
+  UINT8                                 *TargetId;
+  BOOLEAN                               ScanOtherPuns;
+  BOOLEAN                               FromFirstTarget;
+  BOOLEAN                               ExtScsiSupport;
+  EFI_STATUS                            Status;
+  EFI_STATUS                            DevicePathStatus;
+  EFI_STATUS                            PassThruStatus;
+  SCSI_BUS_DEVICE                       *ScsiBusDev;
+  SCSI_TARGET_ID                        ScsiTargetId;
+  EFI_DEVICE_PATH_PROTOCOL              *ParentDevicePath;
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiInterface;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiInterface;
+  EFI_SCSI_BUS_PROTOCOL                 *BusIdentify;
+
+  TargetId        = NULL;
+  ScanOtherPuns   = TRUE;
+  FromFirstTarget = FALSE;
+  ExtScsiSupport  = FALSE;
+  PassThruStatus  = EFI_SUCCESS;
+  
+  TargetId = &ScsiTargetId.ScsiId.ExtScsi[0];
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+  
+  DevicePathStatus = gBS->OpenProtocol (
+                            Controller,
+                            &gEfiDevicePathProtocolGuid,
+                            (VOID **) &ParentDevicePath,
+                            This->DriverBindingHandle,
+                            Controller,
+                            EFI_OPEN_PROTOCOL_BY_DRIVER
+                            );
+  if (EFI_ERROR (DevicePathStatus) && (DevicePathStatus != EFI_ALREADY_STARTED)) {
+    return DevicePathStatus;
+  }
+
+  //
+  // Report Status Code to indicate SCSI bus starts
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );  
+
+  //
+  // To keep backward compatibility, UEFI ExtPassThru Protocol is supported as well as 
+  // EFI PassThru Protocol. From priority perspective, ExtPassThru Protocol is firstly
+  // tried to open on host controller handle. If fails, then PassThru Protocol is tried instead.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **) &ExtScsiInterface,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  //
+  // Fail to open UEFI ExtendPassThru Protocol, then try to open EFI PassThru Protocol instead.
+  //
+  if (EFI_ERROR(Status) && (Status != EFI_ALREADY_STARTED)) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiScsiPassThruProtocolGuid,
+                    (VOID **) &ScsiInterface,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+    //
+    // Fail to open EFI PassThru Protocol, Close the DevicePathProtocol if it is opened by this time.
+    //
+    if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+      if (!EFI_ERROR(DevicePathStatus)) {
+        gBS->CloseProtocol (
+               Controller,
+               &gEfiDevicePathProtocolGuid,
+               This->DriverBindingHandle,
+               Controller
+               );
+      } 
+      return Status;
+    } 
+  } else {
+    //
+    // Succeed to open ExtPassThru Protocol, and meanwhile open PassThru Protocol 
+    // with BY_DRIVER if it is also present on the handle. The intent is to prevent 
+    // another SCSI Bus Driver to work on the same host handle.
+    //
+    ExtScsiSupport = TRUE;
+    PassThruStatus = gBS->OpenProtocol (
+                            Controller,
+                            &gEfiScsiPassThruProtocolGuid,
+                            (VOID **) &ScsiInterface,
+                            This->DriverBindingHandle,
+                            Controller,
+                            EFI_OPEN_PROTOCOL_BY_DRIVER
+                            );
+  }
+    
+  if (Status != EFI_ALREADY_STARTED) {
+    //
+    // Go through here means either ExtPassThru or PassThru Protocol is successfully opened
+    // on this handle for this time. Then construct Host controller private data.
+    //
+    ScsiBusDev = NULL;
+    ScsiBusDev = AllocateZeroPool(sizeof(SCSI_BUS_DEVICE));
+    if (ScsiBusDev == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ErrorExit;
+    }
+    ScsiBusDev->Signature        = SCSI_BUS_DEVICE_SIGNATURE;
+    ScsiBusDev->ExtScsiSupport   = ExtScsiSupport;
+    ScsiBusDev->DevicePath       = ParentDevicePath;
+    if (ScsiBusDev->ExtScsiSupport) {
+      ScsiBusDev->ExtScsiInterface = ExtScsiInterface;
+    } else {
+      ScsiBusDev->ScsiInterface    = ScsiInterface;    
+    }
+
+    //
+    // Install EFI_SCSI_BUS_PROTOCOL to the controller handle, So ScsiBusDev could be
+    // retrieved on this controller handle. With ScsiBusDev, we can know which PassThru
+    // Protocol is present on the handle, UEFI ExtPassThru Protocol or EFI PassThru Protocol.
+    // 
+    Status = gBS->InstallProtocolInterface (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &ScsiBusDev->BusIdentify
+                    );
+    if (EFI_ERROR (Status)) {
+      goto ErrorExit;
+    }
+  } else {
+    //
+    // Go through here means Start() is re-invoked again, nothing special is required to do except
+    // picking up Host controller private information.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &BusIdentify,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    ScsiBusDev = SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS (BusIdentify);
+  }
+
+  //
+  // Report Status Code to indicate detecting devices on bus
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_DETECT),
+    ParentDevicePath
+    );
+
+  Lun  = 0;
+  if (RemainingDevicePath == NULL) {
+    //
+    // If RemainingDevicePath is NULL, 
+    // must enumerate all SCSI devices anyway
+    //
+    FromFirstTarget = TRUE;
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath isn't the End of Device Path Node, 
+    // only scan the specified device by RemainingDevicePath
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      Status = ScsiBusDev->ExtScsiInterface->GetTargetLun (ScsiBusDev->ExtScsiInterface, RemainingDevicePath, &TargetId, &Lun);  
+    } else {
+      Status = ScsiBusDev->ScsiInterface->GetTargetLun (ScsiBusDev->ScsiInterface, RemainingDevicePath, &ScsiTargetId.ScsiId.Scsi, &Lun);
+    }
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  } else {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // skip enumerate any device and return EFI_SUCESSS
+    // 
+    ScanOtherPuns = FALSE;
+  }
+
+  while(ScanOtherPuns) {
+    if (FromFirstTarget) {
+      //
+      // Remaining Device Path is NULL, scan all the possible Puns in the
+      // SCSI Channel.
+      //
+      if (ScsiBusDev->ExtScsiSupport) {
+        Status = ScsiBusDev->ExtScsiInterface->GetNextTargetLun (ScsiBusDev->ExtScsiInterface, &TargetId, &Lun);
+      } else {
+        Status = ScsiBusDev->ScsiInterface->GetNextDevice (ScsiBusDev->ScsiInterface, &ScsiTargetId.ScsiId.Scsi, &Lun);
+      }
+      if (EFI_ERROR (Status)) {
+        //
+        // no legal Pun and Lun found any more
+        //
+        break;
+      }
+    } else {
+      ScanOtherPuns = FALSE;
+    }
+    //
+    // Avoid creating handle for the host adapter.
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      if ((ScsiTargetId.ScsiId.Scsi) == ScsiBusDev->ExtScsiInterface->Mode->AdapterId) {
+        continue;
+      }
+    } else {
+      if ((ScsiTargetId.ScsiId.Scsi) == ScsiBusDev->ScsiInterface->Mode->AdapterId) {
+        continue;
+      }
+    }
+    //
+    // Scan for the scsi device, if it attaches to the scsi bus,
+    // then create handle and install scsi i/o protocol.
+    //
+    Status = ScsiScanCreateDevice (This, Controller, &ScsiTargetId, Lun, ScsiBusDev);
+  }
+  return EFI_SUCCESS;
+
+ErrorExit:
+  
+  if (ScsiBusDev != NULL) {
+    FreePool (ScsiBusDev);
+  }
+  
+  if (ExtScsiSupport) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiExtScsiPassThruProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    if (!EFI_ERROR (PassThruStatus)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    }
+  } else {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiScsiPassThruProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must also
+  follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  BOOLEAN                     AllChildrenStopped;
+  UINTN                       Index;
+  EFI_SCSI_IO_PROTOCOL        *ScsiIo;
+  SCSI_IO_DEV                 *ScsiIoDevice;
+  VOID                        *ScsiPassThru;
+  EFI_SCSI_BUS_PROTOCOL       *Scsidentifier;
+  SCSI_BUS_DEVICE             *ScsiBusDev;
+
+  if (NumberOfChildren == 0) {
+    //
+    // Get the SCSI_BUS_DEVICE
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &Scsidentifier,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    ScsiBusDev = SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS (Scsidentifier);
+
+    //
+    // Uninstall SCSI Bus Protocol
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiCallerIdGuid,
+           &ScsiBusDev->BusIdentify
+           );
+
+    //
+    // Close the bus driver
+    //
+    if (ScsiBusDev->ExtScsiSupport) {
+      //
+      // Close ExtPassThru Protocol from this controller handle
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      //
+      // When Start() succeeds to open ExtPassThru, it always tries to open PassThru BY_DRIVER.
+      // Its intent is to prevent another SCSI Bus Driver from woking on the same host handle. 
+      // So Stop() needs to try to close PassThru if present here.
+      //
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    } else {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    }
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiDevicePathProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+    FreePool (ScsiBusDev);
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiScsiIoProtocolGuid,
+                    (VOID **) &ScsiIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      continue;
+    }
+
+    ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (ScsiIo);
+    //
+    // Close the child handle
+    //
+    if (ScsiIoDevice->ExtScsiSupport) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiExtScsiPassThruProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+
+    } else {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiScsiPassThruProtocolGuid,
+                      This->DriverBindingHandle,
+                      ChildHandleBuffer[Index]
+                      );
+    }
+
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandleBuffer[Index],
+                    &gEfiDevicePathProtocolGuid,
+                    ScsiIoDevice->DevicePath,
+                    &gEfiScsiIoProtocolGuid,
+                    &ScsiIoDevice->ScsiIo,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      if (ScsiIoDevice->ExtScsiSupport) {
+        gBS->OpenProtocol (
+               Controller,
+               &gEfiExtScsiPassThruProtocolGuid,
+               &ScsiPassThru,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      } else {
+        gBS->OpenProtocol (
+               Controller,
+               &gEfiScsiPassThruProtocolGuid,
+               &ScsiPassThru,
+               This->DriverBindingHandle,
+               ChildHandleBuffer[Index],
+               EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+               );
+      }
+    } else {
+      FreePool (ScsiIoDevice);
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the device type information of the SCSI Controller.
+
+  @param  This          Protocol instance pointer.
+  @param  DeviceType    A pointer to the device type information retrieved from
+                        the SCSI Controller. 
+
+  @retval EFI_SUCCESS             Retrieves the device type information successfully.
+  @retval EFI_INVALID_PARAMETER   The DeviceType is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceType (
+  IN  EFI_SCSI_IO_PROTOCOL     *This,
+  OUT UINT8                    *DeviceType
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  if (DeviceType == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice  = SCSI_IO_DEV_FROM_THIS (This);
+  *DeviceType   = ScsiIoDevice->ScsiDeviceType;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves the device location in the SCSI channel.
+
+  @param  This   Protocol instance pointer.
+  @param  Target A pointer to the Target ID of a SCSI device
+                 on the SCSI channel.
+  @param  Lun    A pointer to the LUN of the SCSI device on
+                 the SCSI channel.
+
+  @retval EFI_SUCCESS           Retrieves the device location successfully.
+  @retval EFI_INVALID_PARAMETER The Target or Lun is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceLocation (
+  IN  EFI_SCSI_IO_PROTOCOL    *This,
+  IN OUT UINT8                **Target,
+  OUT UINT64                  *Lun
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  CopyMem (*Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+  *Lun         = ScsiIoDevice->Lun;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets the SCSI Bus that the SCSI Controller is attached to.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       The SCSI bus is reset successfully.
+  @retval  EFI_DEVICE_ERROR  Errors encountered when resetting the SCSI bus.
+  @retval  EFI_UNSUPPORTED   The bus reset operation is not supported by the
+                             SCSI Host Controller.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset 
+                             the SCSI bus.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetBus (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  )
+{
+  SCSI_IO_DEV *ScsiIoDevice;
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    ScsiIoDevice->ScsiBusDeviceData->DevicePath
+    );
+
+  if (ScsiIoDevice->ExtScsiSupport){
+    return ScsiIoDevice->ExtScsiPassThru->ResetChannel (ScsiIoDevice->ExtScsiPassThru);
+  } else {
+    return ScsiIoDevice->ScsiPassThru->ResetChannel (ScsiIoDevice->ScsiPassThru);
+  }
+}
+
+
+/**
+  Resets the SCSI Controller that the device handle specifies.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       Reset the SCSI controller successfully.
+  @retval  EFI_DEVICE_ERROR  Errors are encountered when resetting the SCSI Controller.
+  @retval  EFI_UNSUPPORTED   The SCSI bus does not support a device reset operation.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset the
+                             SCSI Controller.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetDevice (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  )
+{
+  SCSI_IO_DEV  *ScsiIoDevice;
+  UINT8        Target[TARGET_MAX_BYTES];
+
+  ScsiIoDevice = SCSI_IO_DEV_FROM_THIS (This);
+
+  //
+  // Report Status Code to indicate reset happens
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_ATA_ATAPI | EFI_IOB_PC_RESET),
+    ScsiIoDevice->ScsiBusDeviceData->DevicePath
+    );
+  
+  CopyMem (Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+
+  if (ScsiIoDevice->ExtScsiSupport) {
+    return ScsiIoDevice->ExtScsiPassThru->ResetTargetLun (
+                                        ScsiIoDevice->ExtScsiPassThru,
+                                        Target,
+                                        ScsiIoDevice->Lun
+                                          );
+  } else {
+    return ScsiIoDevice->ScsiPassThru->ResetTarget (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun
+                                            );
+  }
+}
+
+
+/**
+  Sends a SCSI Request Packet to the SCSI Controller for execution.
+
+  @param  This            Protocol instance pointer.
+  @param  CommandPacket   The SCSI request packet to send to the SCSI 
+                          Controller specified by the device handle.
+  @param  Event           If the SCSI bus where the SCSI device is attached
+                          does not support non-blocking I/O, then Event is 
+                          ignored, and blocking I/O is performed.  
+                          If Event is NULL, then blocking I/O is performed.
+                          If Event is not NULL and non-blocking I/O is 
+                          supported, then non-blocking I/O is performed,
+                          and Event will be signaled when the SCSI Request
+                          Packet completes.
+
+  @retval EFI_SUCCESS         The SCSI Request Packet was sent by the host 
+                              successfully, and TransferLength bytes were 
+                              transferred to/from DataBuffer.See 
+                              HostAdapterStatus, TargetStatus, 
+                              SenseDataLength, and SenseData in that order
+                              for additional status information.
+  @retval EFI_BAD_BUFFER_SIZE The SCSI Request Packet was executed, 
+                              but the entire DataBuffer could not be transferred.
+                              The actual number of bytes transferred is returned
+                              in TransferLength. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_NOT_READY       The SCSI Request Packet could not be sent because 
+                              there are too many SCSI Command Packets already 
+                              queued.The caller may retry again later.
+  @retval EFI_DEVICE_ERROR    A device error occurred while attempting to send 
+                              the SCSI Request Packet. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_INVALID_PARAMETER  The contents of CommandPacket are invalid.  
+                                 The SCSI Request Packet was not sent, so no 
+                                 additional status information is available.
+  @retval EFI_UNSUPPORTED     The command described by the SCSI Request Packet
+                              is not supported by the SCSI initiator(i.e., SCSI 
+                              Host Controller). The SCSI Request Packet was not
+                              sent, so no additional status information is 
+                              available.
+  @retval EFI_TIMEOUT         A timeout occurred while waiting for the SCSI 
+                              Request Packet to execute. See HostAdapterStatus,
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+**/
+EFI_STATUS
+EFIAPI
+ScsiExecuteSCSICommand (
+  IN     EFI_SCSI_IO_PROTOCOL                     *This,
+  IN OUT EFI_SCSI_IO_SCSI_REQUEST_PACKET          *Packet,
+  IN     EFI_EVENT                                Event  OPTIONAL
+  )
+{
+  SCSI_IO_DEV                                 *ScsiIoDevice;
+  EFI_STATUS                                  Status;
+  UINT8                                       Target[TARGET_MAX_BYTES];
+  EFI_EVENT                                   PacketEvent;
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ExtRequestPacket;
+  SCSI_EVENT_DATA                             EventData;                                     
+
+  PacketEvent = NULL;
+  
+  if (Packet == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ScsiIoDevice  = SCSI_IO_DEV_FROM_THIS (This);
+  CopyMem (Target,&ScsiIoDevice->Pun, TARGET_MAX_BYTES);
+
+  if (ScsiIoDevice->ExtScsiSupport) {
+    ExtRequestPacket = (EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET *) Packet;
+
+    if (((ScsiIoDevice->ExtScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO) != 0) && (Event !=  NULL)) {
+      Status = ScsiIoDevice->ExtScsiPassThru->PassThru (
+                                                ScsiIoDevice->ExtScsiPassThru,
+                                                Target,
+                                                ScsiIoDevice->Lun,
+                                                ExtRequestPacket,
+                                                Event
+                                                );
+    } else {
+      //
+      // If there's no event or the SCSI Device doesn't support NON-BLOCKING,
+      // let the 'Event' parameter for PassThru() be NULL.
+      //
+      Status = ScsiIoDevice->ExtScsiPassThru->PassThru (
+                                                ScsiIoDevice->ExtScsiPassThru,
+                                                Target,
+                                                ScsiIoDevice->Lun,
+                                                ExtRequestPacket,
+                                                NULL
+                                                );
+      if ((!EFI_ERROR(Status)) && (Event != NULL)) {
+        //
+        // Signal Event to tell caller to pick up the SCSI IO packet if the
+        // PassThru() succeeds.
+        //
+        gBS->SignalEvent (Event);
+      }
+    }
+  } else {
+
+    mWorkingBuffer = AllocatePool (sizeof(EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+
+    if (mWorkingBuffer == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // Convert package into EFI1.0, EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET.
+    //
+    Status = ScsiioToPassThruPacket(Packet, (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer);
+    if (EFI_ERROR(Status)) {
+      FreePool(mWorkingBuffer);
+      return Status;
+    }
+
+    if (((ScsiIoDevice->ScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO) != 0) && (Event !=  NULL)) {
+      EventData.Data1 = (VOID*)Packet;
+      EventData.Data2 = Event;
+      //
+      // Create Event
+      //
+      Status = gBS->CreateEvent (
+                       EVT_NOTIFY_SIGNAL,
+                       TPL_NOTIFY,
+                       NotifyFunction,
+                       &EventData,
+                       &PacketEvent
+                       );
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        return Status;
+      }
+
+      Status = ScsiIoDevice->ScsiPassThru->PassThru (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun,
+                                          mWorkingBuffer,
+                                          PacketEvent
+                                          );
+
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        gBS->CloseEvent(PacketEvent);
+        return Status;
+      }
+
+    } else {
+      //
+      // If there's no event or SCSI Device doesn't support NON-BLOCKING, just convert
+      // EFI1.0 PassThru packet back to UEFI2.0 SCSI IO Packet.
+      //
+      Status = ScsiIoDevice->ScsiPassThru->PassThru (
+                                          ScsiIoDevice->ScsiPassThru,
+                                          ScsiIoDevice->Pun.ScsiId.Scsi,
+                                          ScsiIoDevice->Lun,
+                                          mWorkingBuffer,
+                                          NULL
+                                          );
+      if (EFI_ERROR(Status)) {
+        FreePool(mWorkingBuffer);
+        return Status;
+      }
+
+      PassThruToScsiioPacket((EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer,Packet);
+      //
+      // After converting EFI1.0 PassThru Packet back to UEFI2.0 SCSI IO Packet,
+      // free mWorkingBuffer.
+      //
+      FreePool(mWorkingBuffer);
+
+      //
+      // Signal Event to tell caller to pick up the SCSI IO Packet.
+      //
+      if (Event != NULL) {
+        gBS->SignalEvent (Event);
+      }
+    }
+  }
+  return Status;
+}
+
+
+/**
+  Scan SCSI Bus to discover the device, and attach ScsiIoProtocol to it.
+
+  @param  This           Protocol instance pointer
+  @param  Controller     Controller handle
+  @param  TargetId       Tartget to be scanned
+  @param  Lun            The Lun of the SCSI device on the SCSI channel.
+  @param  ScsiBusDev     The pointer of SCSI_BUS_DEVICE
+
+  @retval EFI_SUCCESS           Successfully to discover the device and attach
+                                ScsiIoProtocol to it.
+  @retval EFI_OUT_OF_RESOURCES  Fail to discover the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiScanCreateDevice (
+  IN     EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN     EFI_HANDLE                    Controller,
+  IN     SCSI_TARGET_ID                *TargetId,
+  IN     UINT64                        Lun,
+  IN OUT SCSI_BUS_DEVICE               *ScsiBusDev
+  )
+{
+  EFI_STATUS                Status;
+  SCSI_IO_DEV               *ScsiIoDevice;
+  EFI_DEVICE_PATH_PROTOCOL  *ScsiDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath;
+  EFI_HANDLE                 DeviceHandle;
+
+  DevicePath          = NULL;
+  RemainingDevicePath = NULL;
+  ScsiDevicePath      = NULL;
+  ScsiIoDevice        = NULL;
+
+  //
+  // Build Device Path
+  //
+  if (ScsiBusDev->ExtScsiSupport){
+    Status = ScsiBusDev->ExtScsiInterface->BuildDevicePath (
+                                             ScsiBusDev->ExtScsiInterface,
+                                             &TargetId->ScsiId.ExtScsi[0],
+                                             Lun,
+                                             &ScsiDevicePath
+                                             );
+  } else {
+    Status = ScsiBusDev->ScsiInterface->BuildDevicePath (
+                                          ScsiBusDev->ScsiInterface,
+                                          TargetId->ScsiId.Scsi,
+                                          Lun,
+                                          &ScsiDevicePath
+                                          );
+  }
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DevicePath = AppendDevicePathNode (
+                 ScsiBusDev->DevicePath,
+                 ScsiDevicePath
+                 );
+
+  if (DevicePath == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  DeviceHandle = NULL;
+  RemainingDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &DeviceHandle);
+  if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(RemainingDevicePath)) {
+    //
+    // The device has been started, directly return to fast boot.
+    //
+    Status = EFI_ALREADY_STARTED;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice = AllocateZeroPool (sizeof (SCSI_IO_DEV));
+  if (ScsiIoDevice == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice->Signature                 = SCSI_IO_DEV_SIGNATURE;
+  ScsiIoDevice->ScsiBusDeviceData         = ScsiBusDev;
+  CopyMem(&ScsiIoDevice->Pun, TargetId, TARGET_MAX_BYTES);
+  ScsiIoDevice->Lun                       = Lun;
+
+  if (ScsiBusDev->ExtScsiSupport) {
+    ScsiIoDevice->ExtScsiPassThru         = ScsiBusDev->ExtScsiInterface;
+    ScsiIoDevice->ExtScsiSupport          = TRUE;
+    ScsiIoDevice->ScsiIo.IoAlign          = ScsiIoDevice->ExtScsiPassThru->Mode->IoAlign;
+
+  } else {
+    ScsiIoDevice->ScsiPassThru            = ScsiBusDev->ScsiInterface;
+    ScsiIoDevice->ExtScsiSupport          = FALSE;
+    ScsiIoDevice->ScsiIo.IoAlign          = ScsiIoDevice->ScsiPassThru->Mode->IoAlign;
+  }
+
+  ScsiIoDevice->ScsiIo.GetDeviceType      = ScsiGetDeviceType;
+  ScsiIoDevice->ScsiIo.GetDeviceLocation  = ScsiGetDeviceLocation;
+  ScsiIoDevice->ScsiIo.ResetBus           = ScsiResetBus;
+  ScsiIoDevice->ScsiIo.ResetDevice        = ScsiResetDevice;
+  ScsiIoDevice->ScsiIo.ExecuteScsiCommand = ScsiExecuteSCSICommand;
+
+  //
+  // Report Status Code here since the new SCSI device will be discovered
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_SCSI | EFI_IOB_PC_ENABLE),
+    ScsiBusDev->DevicePath
+    );
+
+  if (!DiscoverScsiDevice (ScsiIoDevice)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  ScsiIoDevice->DevicePath = DevicePath;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ScsiIoDevice->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  ScsiIoDevice->DevicePath,
+                  &gEfiScsiIoProtocolGuid,
+                  &ScsiIoDevice->ScsiIo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  } else {
+    if (ScsiBusDev->ExtScsiSupport) {
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiExtScsiPassThruProtocolGuid,
+             (VOID **) &(ScsiBusDev->ExtScsiInterface),
+             This->DriverBindingHandle,
+             ScsiIoDevice->Handle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+     } else {
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiScsiPassThruProtocolGuid,
+             (VOID **) &(ScsiBusDev->ScsiInterface),
+             This->DriverBindingHandle,
+             ScsiIoDevice->Handle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+     }
+  }
+  return EFI_SUCCESS;
+
+ErrorExit:
+  
+  //
+  // The memory space for ScsiDevicePath is allocated in
+  // ScsiPassThru->BuildDevicePath() function; It is no longer used
+  // after AppendDevicePathNode,so free the memory it occupies.
+  //
+  FreePool (ScsiDevicePath);
+
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+
+  if (ScsiIoDevice != NULL) {
+    FreePool (ScsiIoDevice);
+  }
+
+  return Status;
+}
+
+
+/**
+  Discovery SCSI Device
+
+  @param  ScsiIoDevice    The pointer of SCSI_IO_DEV
+
+  @retval  TRUE   Find SCSI Device and verify it.
+  @retval  FALSE  Unable to find SCSI Device.
+
+**/
+BOOLEAN
+DiscoverScsiDevice (
+  IN OUT  SCSI_IO_DEV   *ScsiIoDevice
+  )
+{
+  EFI_STATUS            Status;
+  UINT32                InquiryDataLength;
+  UINT8                 SenseDataLength;
+  UINT8                 HostAdapterStatus;
+  UINT8                 TargetStatus;
+  EFI_SCSI_INQUIRY_DATA *InquiryData;
+  UINT8                 MaxRetry;
+  UINT8                 Index;
+  BOOLEAN               ScsiDeviceFound;
+
+  HostAdapterStatus = 0;
+  TargetStatus      = 0;
+
+  InquiryData = AllocateAlignedBuffer (ScsiIoDevice, sizeof (EFI_SCSI_INQUIRY_DATA));
+  if (InquiryData == NULL) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  //
+  // Using Inquiry command to scan for the device
+  //
+  InquiryDataLength = sizeof (EFI_SCSI_INQUIRY_DATA);
+  SenseDataLength   = 0;
+  ZeroMem (InquiryData, InquiryDataLength);
+
+  MaxRetry = 2;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiInquiryCommand (
+              &ScsiIoDevice->ScsiIo,
+              SCSI_BUS_TIMEOUT,
+              NULL,
+              &SenseDataLength,
+              &HostAdapterStatus,
+              &TargetStatus,
+              (VOID *) InquiryData,
+              &InquiryDataLength,
+              FALSE
+              );
+    if (!EFI_ERROR (Status)) {
+      break;
+    } else if ((Status == EFI_BAD_BUFFER_SIZE) || 
+               (Status == EFI_INVALID_PARAMETER) ||
+               (Status == EFI_UNSUPPORTED)) {
+      ScsiDeviceFound = FALSE;
+      goto Done;
+    }
+  }
+
+  if (Index == MaxRetry) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+  
+  //
+  // Retrieved inquiry data successfully
+  //
+  if ((InquiryData->Peripheral_Qualifier != 0) &&
+      (InquiryData->Peripheral_Qualifier != 3)) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  if (InquiryData->Peripheral_Qualifier == 3) {
+    if (InquiryData->Peripheral_Type != 0x1f) {
+      ScsiDeviceFound = FALSE;
+      goto Done;
+    }
+  }
+
+  if (0x1e >= InquiryData->Peripheral_Type && InquiryData->Peripheral_Type >= 0xa) {
+    ScsiDeviceFound = FALSE;
+    goto Done;
+  }
+
+  //
+  // valid device type and peripheral qualifier combination.
+  //
+  ScsiIoDevice->ScsiDeviceType  = InquiryData->Peripheral_Type;
+  ScsiIoDevice->RemovableDevice = InquiryData->Rmb;
+  if (InquiryData->Version == 0) {
+    ScsiIoDevice->ScsiVersion = 0;
+  } else {
+    //
+    // ANSI-approved version
+    //
+    ScsiIoDevice->ScsiVersion = (UINT8) (InquiryData->Version & 0x07);
+  }
+
+  ScsiDeviceFound = TRUE;
+
+Done:
+  FreeAlignedBuffer (InquiryData, sizeof (EFI_SCSI_INQUIRY_DATA));
+
+  return ScsiDeviceFound;
+}
+
+
+/**
+  Convert EFI_SCSI_IO_SCSI_REQUEST_PACKET packet to EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet.
+
+  @param  Packet         The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+  @param  CommandPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiioToPassThruPacket (
+  IN      EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet,
+  OUT     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *CommandPacket
+  )
+{
+  //
+  //EFI 1.10 doesn't support Bi-Direction Command.
+  //
+  if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_BIDIRECTIONAL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  ZeroMem (CommandPacket, sizeof (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET));
+
+  CommandPacket->Timeout           = Packet->Timeout;
+  CommandPacket->Cdb               = Packet->Cdb;
+  CommandPacket->CdbLength         = Packet->CdbLength;
+  CommandPacket->DataDirection     = Packet->DataDirection;
+  CommandPacket->HostAdapterStatus = Packet->HostAdapterStatus;
+  CommandPacket->TargetStatus      = Packet->TargetStatus;
+  CommandPacket->SenseData         = Packet->SenseData;
+  CommandPacket->SenseDataLength   = Packet->SenseDataLength;
+
+  if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_READ) {
+    CommandPacket->DataBuffer = Packet->InDataBuffer;
+    CommandPacket->TransferLength = Packet->InTransferLength;
+  } else if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_WRITE) {
+    CommandPacket->DataBuffer = Packet->OutDataBuffer;
+    CommandPacket->TransferLength = Packet->OutTransferLength;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Convert EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET packet to EFI_SCSI_IO_SCSI_REQUEST_PACKET packet.
+
+  @param  ScsiPacket  The pointer of EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET
+  @param  Packet      The pointer of EFI_SCSI_IO_SCSI_REQUEST_PACKET
+
+**/
+EFI_STATUS
+EFIAPI
+PassThruToScsiioPacket (
+  IN     EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *ScsiPacket,
+  OUT    EFI_SCSI_IO_SCSI_REQUEST_PACKET         *Packet
+  )
+{
+  Packet->Timeout           = ScsiPacket->Timeout;
+  Packet->Cdb               = ScsiPacket->Cdb;
+  Packet->CdbLength         = ScsiPacket->CdbLength;
+  Packet->DataDirection     = ScsiPacket->DataDirection;
+  Packet->HostAdapterStatus = ScsiPacket->HostAdapterStatus;
+  Packet->TargetStatus      = ScsiPacket->TargetStatus;
+  Packet->SenseData         = ScsiPacket->SenseData;
+  Packet->SenseDataLength   = ScsiPacket->SenseDataLength;
+
+  if (ScsiPacket->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_READ) {
+    Packet->InDataBuffer = ScsiPacket->DataBuffer;
+    Packet->InTransferLength = ScsiPacket->TransferLength;
+  } else if (Packet->DataDirection == EFI_SCSI_IO_DATA_DIRECTION_WRITE) {
+    Packet->OutDataBuffer = ScsiPacket->DataBuffer;
+    Packet->OutTransferLength = ScsiPacket->TransferLength;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Notify Function in which convert EFI1.0 PassThru Packet back to UEF2.0
+  SCSI IO Packet.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+NotifyFunction (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET          *Packet;
+  EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET   *ScsiPacket;
+  EFI_EVENT                                CallerEvent;
+  SCSI_EVENT_DATA                          *PassData;
+
+  PassData = (SCSI_EVENT_DATA*)Context;
+  Packet  = (EFI_SCSI_IO_SCSI_REQUEST_PACKET *)PassData->Data1;
+  ScsiPacket =  (EFI_SCSI_PASS_THRU_SCSI_REQUEST_PACKET*)mWorkingBuffer;
+
+  //
+  // Convert EFI1.0 PassThru packet to UEFI2.0 SCSI IO Packet.
+  //
+  PassThruToScsiioPacket(ScsiPacket, Packet);
+
+  //
+  // After converting EFI1.0 PassThru Packet back to UEFI2.0 SCSI IO Packet,
+  // free mWorkingBuffer.
+  //
+  gBS->FreePool(mWorkingBuffer);
+
+  //
+  // Signal Event to tell caller to pick up UEFI2.0 SCSI IO Packet.
+  //
+  CallerEvent = PassData->Data2;
+  gBS->CloseEvent(Event);
+  gBS->SignalEvent(CallerEvent);
+}
+
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h
new file mode 100644
index 0000000000..babf86f2c3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.h
@@ -0,0 +1,492 @@
+/** @file
+  Header file for SCSI Bus Driver.
+
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef _SCSI_BUS_H_
+#define _SCSI_BUS_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/ScsiPassThru.h>
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/ScsiIo.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiScsiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+#include <IndustryStandard/Scsi.h>
+
+#define SCSI_IO_DEV_SIGNATURE SIGNATURE_32 ('s', 'c', 'i', 'o')
+
+typedef union {
+  UINT32  Scsi;
+  UINT8   ExtScsi[4];   
+} SCSI_ID;
+
+typedef struct _SCSI_TARGET_ID {
+  SCSI_ID ScsiId;
+  UINT8   ExtScsiId[12];
+}SCSI_TARGET_ID;
+
+
+typedef struct {
+   VOID   *Data1;
+   VOID   *Data2;
+} SCSI_EVENT_DATA;
+
+//
+// SCSI Bus Controller device strcuture
+//
+#define SCSI_BUS_DEVICE_SIGNATURE  SIGNATURE_32 ('s', 'c', 's', 'i')
+
+//
+// SCSI Bus Timeout Experience Value
+//
+#define SCSI_BUS_TIMEOUT           EFI_TIMER_PERIOD_SECONDS (3)
+
+//
+// The ScsiBusProtocol is just used to locate ScsiBusDev
+// structure in the SCSIBusDriverBindingStop(). Then we can
+// Close all opened protocols and release this structure.
+// ScsiBusProtocol is the private protocol.
+// gEfiCallerIdGuid will be used as its protocol guid.
+//
+typedef struct _EFI_SCSI_BUS_PROTOCOL {
+  UINT64  Reserved;
+} EFI_SCSI_BUS_PROTOCOL;
+
+typedef struct _SCSI_BUS_DEVICE {
+  UINTN                                 Signature;
+  EFI_SCSI_BUS_PROTOCOL                 BusIdentify;
+  BOOLEAN                               ExtScsiSupport; 
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiInterface;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiInterface;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+} SCSI_BUS_DEVICE;
+
+#define SCSI_BUS_CONTROLLER_DEVICE_FROM_THIS(a)  CR (a, SCSI_BUS_DEVICE, BusIdentify, SCSI_BUS_DEVICE_SIGNATURE)
+
+typedef struct {
+  UINT32                             Signature;
+  EFI_HANDLE                         Handle;
+  EFI_SCSI_IO_PROTOCOL               ScsiIo;
+  EFI_DEVICE_PATH_PROTOCOL           *DevicePath;
+  BOOLEAN                            ExtScsiSupport; 
+  EFI_SCSI_PASS_THRU_PROTOCOL        *ScsiPassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *ExtScsiPassThru;
+  SCSI_BUS_DEVICE                    *ScsiBusDeviceData;
+  SCSI_TARGET_ID                     Pun;
+  UINT64                             Lun;
+  UINT8                              ScsiDeviceType;
+  UINT8                              ScsiVersion;
+  BOOLEAN                            RemovableDevice;
+} SCSI_IO_DEV;
+
+#define SCSI_IO_DEV_FROM_THIS(a)  CR (a, SCSI_IO_DEV, ScsiIo, SCSI_IO_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gScsiBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gScsiBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gScsiBusComponentName2;
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Supported() it must also follow these calling
+  restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must also
+  follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+SCSIBusDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Retrieves the device type information of the SCSI Controller.
+
+  @param  This          Protocol instance pointer.
+  @param  DeviceType    A pointer to the device type information retrieved from
+                        the SCSI Controller. 
+
+  @retval EFI_SUCCESS             Retrieves the device type information successfully.
+  @retval EFI_INVALID_PARAMETER   The DeviceType is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceType (
+  IN  EFI_SCSI_IO_PROTOCOL     *This,
+  OUT UINT8                    *DeviceType
+  );
+
+/**
+  Retrieves the device location in the SCSI channel.
+
+  @param  This   Protocol instance pointer.
+  @param  Target A pointer to the Target ID of a SCSI device
+                 on the SCSI channel.
+  @param  Lun    A pointer to the LUN of the SCSI device on
+                 the SCSI channel.
+
+  @retval EFI_SUCCESS           Retrieves the device location successfully.
+  @retval EFI_INVALID_PARAMETER The Target or Lun is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiGetDeviceLocation (
+  IN  EFI_SCSI_IO_PROTOCOL    *This,
+  IN OUT UINT8                **Target,
+  OUT UINT64                  *Lun
+  );
+
+/**
+  Resets the SCSI Bus that the SCSI Controller is attached to.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       The SCSI bus is reset successfully.
+  @retval  EFI_DEVICE_ERROR  Errors encountered when resetting the SCSI bus.
+  @retval  EFI_UNSUPPORTED   The bus reset operation is not supported by the
+                             SCSI Host Controller.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset 
+                             the SCSI bus.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetBus (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  );
+
+/**
+  Resets the SCSI Controller that the device handle specifies.
+
+  @param  This  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS       Reset the SCSI controller successfully.
+  @retval  EFI_DEVICE_ERROR  Errors are encountered when resetting the SCSI Controller.
+  @retval  EFI_UNSUPPORTED   The SCSI bus does not support a device reset operation.
+  @retval  EFI_TIMEOUT       A timeout occurred while attempting to reset the
+                             SCSI Controller.
+**/
+EFI_STATUS
+EFIAPI
+ScsiResetDevice (
+  IN  EFI_SCSI_IO_PROTOCOL     *This
+  );
+
+/**
+  Sends a SCSI Request Packet to the SCSI Controller for execution.
+
+  @param  This            Protocol instance pointer.
+  @param  CommandPacket   The SCSI request packet to send to the SCSI 
+                          Controller specified by the device handle.
+  @param  Event           If the SCSI bus where the SCSI device is attached
+                          does not support non-blocking I/O, then Event is 
+                          ignored, and blocking I/O is performed.  
+                          If Event is NULL, then blocking I/O is performed.
+                          If Event is not NULL and non-blocking I/O is 
+                          supported, then non-blocking I/O is performed,
+                          and Event will be signaled when the SCSI Request
+                          Packet completes.
+
+  @retval EFI_SUCCESS         The SCSI Request Packet was sent by the host 
+                              successfully, and TransferLength bytes were 
+                              transferred to/from DataBuffer.See 
+                              HostAdapterStatus, TargetStatus, 
+                              SenseDataLength, and SenseData in that order
+                              for additional status information.
+  @retval EFI_BAD_BUFFER_SIZE The SCSI Request Packet was executed, 
+                              but the entire DataBuffer could not be transferred.
+                              The actual number of bytes transferred is returned
+                              in TransferLength. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_NOT_READY       The SCSI Request Packet could not be sent because 
+                              there are too many SCSI Command Packets already 
+                              queued.The caller may retry again later.
+  @retval EFI_DEVICE_ERROR    A device error occurred while attempting to send 
+                              the SCSI Request Packet. See HostAdapterStatus, 
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+  @retval EFI_INVALID_PARAMETER  The contents of CommandPacket are invalid.  
+                                 The SCSI Request Packet was not sent, so no 
+                                 additional status information is available.
+  @retval EFI_UNSUPPORTED     The command described by the SCSI Request Packet
+                              is not supported by the SCSI initiator(i.e., SCSI 
+                              Host Controller). The SCSI Request Packet was not
+                              sent, so no additional status information is 
+                              available.
+  @retval EFI_TIMEOUT         A timeout occurred while waiting for the SCSI 
+                              Request Packet to execute. See HostAdapterStatus,
+                              TargetStatus, SenseDataLength, and SenseData in 
+                              that order for additional status information.
+**/
+EFI_STATUS
+EFIAPI
+ScsiExecuteSCSICommand (
+  IN  EFI_SCSI_IO_PROTOCOL                 *This,
+  IN OUT  EFI_SCSI_IO_SCSI_REQUEST_PACKET  *CommandPacket,
+  IN  EFI_EVENT                            Event  OPTIONAL
+  );
+
+/**
+  Scan SCSI Bus to discover the device, and attach ScsiIoProtocol to it.
+
+  @param  This           Protocol instance pointer
+  @param  Controller     Controller handle
+  @param  TargetId       Tartget to be scanned
+  @param  Lun            The Lun of the SCSI device on the SCSI channel.
+  @param  ScsiBusDev     The pointer of SCSI_BUS_DEVICE
+
+  @retval EFI_SUCCESS           Successfully to discover the device and attach
+                                ScsiIoProtocol to it.
+  @retval EFI_OUT_OF_RESOURCES  Fail to discover the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiScanCreateDevice (
+  IN     EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN     EFI_HANDLE                    Controller,
+  IN     SCSI_TARGET_ID                *TargetId,
+  IN     UINT64                        Lun,
+  IN OUT SCSI_BUS_DEVICE               *ScsiBusDev
+  );
+
+/**
+  Discovery SCSI Device
+
+  @param  ScsiIoDevice    The pointer of SCSI_IO_DEV
+
+  @retval  TRUE   Find SCSI Device and verify it.
+  @retval  FALSE  Unable to find SCSI Device.
+
+**/
+BOOLEAN
+DiscoverScsiDevice (
+  IN  OUT  SCSI_IO_DEV   *ScsiIoDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni
new file mode 100644
index 0000000000..cce1cfdbe5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBus.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf
new file mode 100644
index 0000000000..18ffd88d89
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf
@@ -0,0 +1,70 @@
+## @file
+#  The SCSI bus driver scans all SCSI devices and creates a device handle for each of them.
+#  Note that the driver will install the Device Path Protocol and SCSI I/O Protocol on
+#  these handles.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ScsiBus
+  MODULE_UNI_FILE                = ScsiBus.uni
+  FILE_GUID                      = 0167CCC4-D0F7-4f21-A3EF-9E64B7CDCE8B
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeScsiBus
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES              =  IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gSCSIBusDriverBinding                        
+#  COMPONENT_NAME                =  gScsiBusComponentName
+#  COMPONENT_NAME2               =  gScsiBusComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  ScsiBus.c
+  ScsiBus.h
+
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  UefiScsiLib
+  BaseMemoryLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  MemoryAllocationLib
+  ReportStatusCodeLib
+
+
+[Protocols]
+  gEfiScsiIoProtocolGuid                        ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid
+  gEfiScsiPassThruProtocolGuid                  ## TO_START
+  gEfiExtScsiPassThruProtocolGuid               ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ScsiBusExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni
new file mode 100644
index 0000000000..420965e83d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c
new file mode 100644
index 0000000000..08b71d08f3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for SCSI disk driver.
+
+Copyright (c) 2004 - 2015, 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 "ScsiDisk.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gScsiDiskComponentName = {
+  ScsiDiskComponentNameGetDriverName,
+  ScsiDiskComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gScsiDiskComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ScsiDiskComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ScsiDiskComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mScsiDiskDriverNameTable[] = {
+  { "eng;en", (CHAR16 *) L"Scsi Disk Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mScsiDiskDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gScsiDiskComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS            Status;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // Make sure this driver is currently managing ControllerHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gScsiDiskDriverBinding.DriverBindingHandle,
+             &gEfiScsiIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  gScsiDiskDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (BlockIo);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           ScsiDiskDevice->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gScsiDiskComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c
new file mode 100644
index 0000000000..6cd9a18701
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.c
@@ -0,0 +1,4994 @@
+/** @file
+  SCSI disk driver that layers on every SCSI IO protocol in the system.
+
+Copyright (c) 2006 - 2016, 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 "ScsiDisk.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gScsiDiskDriverBinding = {
+  ScsiDiskDriverBindingSupported,
+  ScsiDiskDriverBindingStart,
+  ScsiDiskDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_DISK_INFO_PROTOCOL gScsiDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_SCSI_INTERFACE_GUID,
+  ScsiDiskInfoInquiry,
+  ScsiDiskInfoIdentify,
+  ScsiDiskInfoSenseData,
+  ScsiDiskInfoWhichIde
+};
+
+/**
+  Allocates an aligned buffer for SCSI disk.
+
+  This function allocates an aligned buffer for the SCSI disk to perform
+  SCSI IO operations. The alignment requirement is from SCSI IO interface.
+
+  @param  ScsiDiskDevice    The SCSI disk involved for the operation.
+  @param  BufferSize        The request buffer size.
+
+  @return A pointer to the aligned buffer or NULL if the allocation fails.
+
+**/
+VOID *
+AllocateAlignedBuffer (
+  IN SCSI_DISK_DEV            *ScsiDiskDevice,
+  IN UINTN                    BufferSize
+  )
+{
+  return AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), ScsiDiskDevice->ScsiIo->IoAlign);
+}
+
+/**
+  Frees an aligned buffer for SCSI disk.
+
+  This function frees an aligned buffer for the SCSI disk to perform
+  SCSI IO operations.
+
+  @param  Buffer            The aligned buffer to be freed.
+  @param  BufferSize        The request buffer size.
+
+**/
+VOID
+FreeAlignedBuffer (
+  IN VOID                     *Buffer,
+  IN UINTN                    BufferSize
+  )
+{
+  if (Buffer != NULL) {
+    FreeAlignedPages (Buffer, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
+
+/**
+  The user Entry Point for module ScsiDisk.
+
+  The user code starts with this function.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.  
+  @param  SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeScsiDisk(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gScsiDiskDriverBinding,
+             ImageHandle,
+             &gScsiDiskComponentName,
+             &gScsiDiskComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+
+  return Status;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions.
+  If any other agent wishes to call Supported() it must also follow these
+  calling restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_SCSI_IO_PROTOCOL  *ScsiIo;
+  UINT8                 DeviceType;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiIoProtocolGuid,
+                  (VOID **) &ScsiIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = ScsiIo->GetDeviceType (ScsiIo, &DeviceType);
+  if (!EFI_ERROR (Status)) {
+    if ((DeviceType == EFI_SCSI_TYPE_DISK) || (DeviceType == EFI_SCSI_TYPE_CDROM)) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_UNSUPPORTED;
+    }
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_SCSI_IO_PROTOCOL  *ScsiIo;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  BOOLEAN               Temp;
+  UINT8                 Index;
+  UINT8                 MaxRetry;
+  BOOLEAN               NeedRetry;
+  BOOLEAN               MustReadCapacity;
+
+  MustReadCapacity = TRUE;
+
+  ScsiDiskDevice = (SCSI_DISK_DEV *) AllocateZeroPool (sizeof (SCSI_DISK_DEV));
+  if (ScsiDiskDevice == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiScsiIoProtocolGuid,
+                  (VOID **) &ScsiIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (ScsiDiskDevice);
+    return Status;
+  }
+
+  ScsiDiskDevice->Signature            = SCSI_DISK_DEV_SIGNATURE;
+  ScsiDiskDevice->ScsiIo               = ScsiIo;
+  ScsiDiskDevice->BlkIo.Revision       = EFI_BLOCK_IO_PROTOCOL_REVISION3;
+  ScsiDiskDevice->BlkIo.Media          = &ScsiDiskDevice->BlkIoMedia;
+  ScsiDiskDevice->BlkIo.Media->IoAlign = ScsiIo->IoAlign;
+  ScsiDiskDevice->BlkIo.Reset          = ScsiDiskReset;
+  ScsiDiskDevice->BlkIo.ReadBlocks     = ScsiDiskReadBlocks;
+  ScsiDiskDevice->BlkIo.WriteBlocks    = ScsiDiskWriteBlocks;
+  ScsiDiskDevice->BlkIo.FlushBlocks    = ScsiDiskFlushBlocks;
+  ScsiDiskDevice->BlkIo2.Media         = &ScsiDiskDevice->BlkIoMedia;
+  ScsiDiskDevice->BlkIo2.Reset         = ScsiDiskResetEx;
+  ScsiDiskDevice->BlkIo2.ReadBlocksEx  = ScsiDiskReadBlocksEx;
+  ScsiDiskDevice->BlkIo2.WriteBlocksEx = ScsiDiskWriteBlocksEx;
+  ScsiDiskDevice->BlkIo2.FlushBlocksEx = ScsiDiskFlushBlocksEx;
+  ScsiDiskDevice->Handle               = Controller;
+  InitializeListHead (&ScsiDiskDevice->BlkIo2Queue);
+
+  ScsiIo->GetDeviceType (ScsiIo, &(ScsiDiskDevice->DeviceType));
+  switch (ScsiDiskDevice->DeviceType) {
+  case EFI_SCSI_TYPE_DISK:
+    ScsiDiskDevice->BlkIo.Media->BlockSize = 0x200;
+    MustReadCapacity = TRUE;
+    break;
+
+  case EFI_SCSI_TYPE_CDROM:
+    ScsiDiskDevice->BlkIo.Media->BlockSize = 0x800;
+    ScsiDiskDevice->BlkIo.Media->ReadOnly  = TRUE;
+    MustReadCapacity = FALSE;
+    break;
+  }
+  //
+  // The Sense Data Array's initial size is 6
+  //
+  ScsiDiskDevice->SenseDataNumber = 6;
+  ScsiDiskDevice->SenseData = (EFI_SCSI_SENSE_DATA *) AllocateZeroPool (
+                                 sizeof (EFI_SCSI_SENSE_DATA) * ScsiDiskDevice->SenseDataNumber
+                                 );
+  if (ScsiDiskDevice->SenseData == NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiScsiIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    FreePool (ScsiDiskDevice);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Retrieve device information
+  //
+  MaxRetry = 2;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskInquiryDevice (ScsiDiskDevice, &NeedRetry);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (!NeedRetry) {
+      FreePool (ScsiDiskDevice->SenseData);
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiScsiIoProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      FreePool (ScsiDiskDevice);
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // The second parameter "TRUE" means must
+  // retrieve media capacity
+  //
+  Status = ScsiDiskDetectMedia (ScsiDiskDevice, MustReadCapacity, &Temp);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Determine if Block IO & Block IO2 should be produced on this controller
+    // handle
+    //
+    if (DetermineInstallBlockIo(Controller)) {
+      InitializeInstallDiskInfo(ScsiDiskDevice, Controller);
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Controller,
+                      &gEfiBlockIoProtocolGuid,
+                      &ScsiDiskDevice->BlkIo,
+                      &gEfiBlockIo2ProtocolGuid,
+                      &ScsiDiskDevice->BlkIo2,
+                      &gEfiDiskInfoProtocolGuid,
+                      &ScsiDiskDevice->DiskInfo,
+                      NULL
+                      );
+      if (!EFI_ERROR(Status)) {
+        ScsiDiskDevice->ControllerNameTable = NULL;
+        AddUnicodeString2 (
+          "eng",
+          gScsiDiskComponentName.SupportedLanguages,
+          &ScsiDiskDevice->ControllerNameTable,
+          L"SCSI Disk Device",
+          TRUE
+          );
+        AddUnicodeString2 (
+          "en",
+          gScsiDiskComponentName2.SupportedLanguages,
+          &ScsiDiskDevice->ControllerNameTable,
+          L"SCSI Disk Device",
+          FALSE
+          );
+        return EFI_SUCCESS;
+      }
+    } 
+  }
+
+  gBS->FreePool (ScsiDiskDevice->SenseData);
+  gBS->FreePool (ScsiDiskDevice);
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiScsiIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;
+  
+}
+
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must
+  also follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer   OPTIONAL
+  )
+{
+  EFI_BLOCK_IO_PROTOCOL *BlkIo;
+  SCSI_DISK_DEV         *ScsiDiskDevice;
+  EFI_STATUS            Status;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlkIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (BlkIo);
+
+  //
+  // Wait for the BlockIo2 requests queue to become empty
+  //
+  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  &ScsiDiskDevice->BlkIo,
+                  &gEfiBlockIo2ProtocolGuid,
+                  &ScsiDiskDevice->BlkIo2,
+                  &gEfiDiskInfoProtocolGuid,
+                  &ScsiDiskDevice->DiskInfo,
+                  NULL
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiScsiIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+
+    ReleaseScsiDiskDeviceResources (ScsiDiskDevice);
+
+    return EFI_SUCCESS;
+  }
+  //
+  // errors met
+  //
+  return Status;
+}
+
+/**
+  Reset SCSI Disk.
+
+
+  @param  This                 The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  ExtendedVerification The flag about if extend verificate
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL       OldTpl;
+  SCSI_DISK_DEV *ScsiDiskDevice;
+  EFI_STATUS    Status;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_UNSUPPORTED) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+  }
+
+  if (!ExtendedVerification) {
+    goto Done;
+  }
+
+  Status = ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Done;
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHANGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // If all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  Status = ScsiDiskReadSectors (ScsiDiskDevice, Buffer, Lba, NumberOfBlocks);
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHNAGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+  //
+  // if all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  Status = ScsiDiskWriteSectors (ScsiDiskDevice, Buffer, Lba, NumberOfBlocks);
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flush Block to Disk.
+
+  EFI_SUCCESS is returned directly.
+
+  @param  This              The pointer of EFI_BLOCK_IO_PROTOCOL
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  )
+{
+  //
+  // return directly
+  //
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset SCSI Disk.
+
+  @param  This                 The pointer of EFI_BLOCK_IO2_PROTOCOL.
+  @param  ExtendedVerification The flag about if extend verificate.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  EFI_TPL       OldTpl;
+  SCSI_DISK_DEV *ScsiDiskDevice;
+  EFI_STATUS    Status;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  Status          = ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_UNSUPPORTED) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+  }
+
+  if (!ExtendedVerification) {
+    goto Done;
+  }
+
+  Status = ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Done;
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The read request was queued if Token-> Event is
+                                not NULL. The data was read correctly from the
+                                device if theToken-> Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the read operation.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL   *This,
+  IN     UINT32                   MediaId,
+  IN     EFI_LBA                  Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN      *Token,
+  IN     UINTN                    BufferSize,
+  OUT    VOID                     *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo2.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // If all the parameters are valid, then perform read sectors command
+  // to transfer data from device to host.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    Status = ScsiDiskAsyncReadSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks,
+               Token
+               );
+  } else {
+    Status = ScsiDiskReadSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks
+               );
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  IN     VOID                   *Buffer
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  UINTN               BlockSize;
+  UINTN               NumberOfBlocks;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+  //
+  // Get the intrinsic block size
+  //
+  Media           = ScsiDiskDevice->BlkIo2.Media;
+  BlockSize       = Media->BlockSize;
+
+  NumberOfBlocks  = BufferSize / BlockSize;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  if (BufferSize == 0) {
+    if ((Token != NULL) && (Token->Event != NULL)) {
+      Token->TransactionStatus = EFI_SUCCESS;
+      gBS->SignalEvent (Token->Event);
+    }
+
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto Done;
+  }
+
+  if (Lba > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Lba + NumberOfBlocks - 1) > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if ((Media->IoAlign > 1) && (((UINTN) Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // if all the parameters are valid, then perform write sectors command
+  // to transfer data from device to host.
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    Status = ScsiDiskAsyncWriteSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks,
+               Token
+               );
+  } else {
+    Status = ScsiDiskWriteSectors (
+               ScsiDiskDevice,
+               Buffer,
+               Lba,
+               NumberOfBlocks
+               );
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flush the Block Device.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS         All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR    The device reported an error while attempting to
+                              write data.
+  @retval EFI_WRITE_PROTECTED The device cannot be written to.
+  @retval EFI_NO_MEDIA        There is no media in the device.
+  @retval EFI_MEDIA_CHANGED   The MediaId is not for the current media.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token
+  )
+{
+  SCSI_DISK_DEV       *ScsiDiskDevice;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  BOOLEAN             MediaChange;
+  EFI_TPL             OldTpl;
+
+  MediaChange    = FALSE;
+  OldTpl         = gBS->RaiseTPL (TPL_CALLBACK);
+  ScsiDiskDevice = SCSI_DISK_DEV_FROM_BLKIO2 (This);
+
+  if (!IS_DEVICE_FIXED(ScsiDiskDevice)) {
+
+    Status = ScsiDiskDetectMedia (ScsiDiskDevice, FALSE, &MediaChange);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto Done;
+    }
+
+    if (MediaChange) {
+      gBS->ReinstallProtocolInterface (
+            ScsiDiskDevice->Handle,
+            &gEfiBlockIoProtocolGuid,
+            &ScsiDiskDevice->BlkIo,
+            &ScsiDiskDevice->BlkIo
+            );
+      gBS->ReinstallProtocolInterface (
+             ScsiDiskDevice->Handle,
+             &gEfiBlockIo2ProtocolGuid,
+             &ScsiDiskDevice->BlkIo2,
+             &ScsiDiskDevice->BlkIo2
+             );
+      Status = EFI_MEDIA_CHANGED;
+      goto Done;
+    }
+  }
+
+  Media = ScsiDiskDevice->BlkIo2.Media;
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto Done;
+  }
+
+  if (Media->ReadOnly) {
+    Status = EFI_WRITE_PROTECTED;
+    goto Done;
+  }
+
+  //
+  // Wait for the BlockIo2 requests queue to become empty
+  //
+  while (!IsListEmpty (&ScsiDiskDevice->BlkIo2Queue));
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Signal caller event
+  //
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Token->TransactionStatus = EFI_SUCCESS;
+    gBS->SignalEvent (Token->Event);
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Detect Device and read out capacity ,if error occurs, parse the sense key.
+
+  @param  ScsiDiskDevice    The pointer of SCSI_DISK_DEV
+  @param  MustReadCapacity  The flag about reading device capacity
+  @param  MediaChange       The pointer of flag indicates if media has changed 
+
+  @retval EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskDetectMedia (
+  IN   SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN   BOOLEAN         MustReadCapacity,
+  OUT  BOOLEAN         *MediaChange
+  )
+{
+  EFI_STATUS          Status;
+  EFI_SCSI_SENSE_DATA *SenseData;
+  UINTN               NumberOfSenseKeys;
+  BOOLEAN             NeedRetry;
+  BOOLEAN             NeedReadCapacity;
+  UINT8               Retry;
+  UINT8               MaxRetry;
+  EFI_BLOCK_IO_MEDIA  OldMedia;
+  UINTN               Action;
+  EFI_EVENT           TimeoutEvt;
+
+  Status              = EFI_SUCCESS;
+  SenseData           = NULL;
+  NumberOfSenseKeys   = 0;
+  Retry               = 0;
+  MaxRetry            = 3;
+  Action              = ACTION_NO_ACTION;
+  NeedReadCapacity    = FALSE;
+  *MediaChange        = FALSE;
+  TimeoutEvt          = NULL;
+
+  CopyMem (&OldMedia, ScsiDiskDevice->BlkIo.Media, sizeof (OldMedia));
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimeoutEvt
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (TimeoutEvt, TimerRelative, EFI_TIMER_PERIOD_SECONDS(120));
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Sending Test_Unit cmd to poll device status.
+  // If the sense data shows the drive is not ready or reset before, we need poll the device status again.
+  // We limit the upper boundary to 120 seconds.
+  //
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvt))) {
+    Status = ScsiDiskTestUnitReady (
+              ScsiDiskDevice,
+              &NeedRetry,
+              &SenseData,
+              &NumberOfSenseKeys
+              );
+    if (!EFI_ERROR (Status)) {
+      Status = DetectMediaParsingSenseKeys (
+                 ScsiDiskDevice,
+                 SenseData,
+                 NumberOfSenseKeys,
+                 &Action
+                 );
+      if (EFI_ERROR (Status)) {
+        goto EXIT;
+      } else if (Action == ACTION_RETRY_COMMAND_LATER) {
+        continue;
+      } else {
+        break;
+      }
+    } else {
+      Retry++;
+      if (!NeedRetry || (Retry >= MaxRetry)) {
+        goto EXIT;
+      }
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // ACTION_NO_ACTION: need not read capacity
+  // other action code: need read capacity
+  //
+  if (Action == ACTION_READ_CAPACITY) {
+    NeedReadCapacity = TRUE;
+  }
+
+  //
+  // either NeedReadCapacity is TRUE, or MustReadCapacity is TRUE,
+  // retrieve capacity via Read Capacity command
+  //
+  if (NeedReadCapacity || MustReadCapacity) {
+    //
+    // retrieve media information
+    //
+    for (Retry = 0; Retry < MaxRetry; Retry++) {
+      Status = ScsiDiskReadCapacity (
+                 ScsiDiskDevice,
+                 &NeedRetry,
+                 &SenseData,
+                 &NumberOfSenseKeys
+                 );
+      if (!EFI_ERROR (Status)) {
+        //
+        // analyze sense key to action
+        //
+        Status = DetectMediaParsingSenseKeys (
+                   ScsiDiskDevice,
+                   SenseData,
+                   NumberOfSenseKeys,
+                   &Action
+                   );
+        if (EFI_ERROR (Status)) {
+          //
+          // if Status is error, it may indicate crisis error,
+          // so return without retry.
+          //
+          goto EXIT;
+        } else if (Action == ACTION_RETRY_COMMAND_LATER) {
+          Retry = 0;
+          continue;
+        } else {
+          break;
+        }
+      } else {   
+        Retry++;
+        if (!NeedRetry || (Retry >= MaxRetry)) {
+          goto EXIT;
+        }
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->MediaId != OldMedia.MediaId) {
+    //
+    // Media change information got from the device
+    //
+    *MediaChange = TRUE;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->ReadOnly != OldMedia.ReadOnly) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->BlockSize != OldMedia.BlockSize) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->LastBlock != OldMedia.LastBlock) {
+    *MediaChange = TRUE;
+    ScsiDiskDevice->BlkIo.Media->MediaId += 1;
+  }
+
+  if (ScsiDiskDevice->BlkIo.Media->MediaPresent != OldMedia.MediaPresent) {
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent) {
+      //
+      // when change from no media to media present, reset the MediaId to 1.
+      //
+      ScsiDiskDevice->BlkIo.Media->MediaId = 1;
+    } else {
+      //
+      // when no media, reset the MediaId to zero.
+      //
+      ScsiDiskDevice->BlkIo.Media->MediaId = 0;
+    }
+
+    *MediaChange = TRUE;
+  }
+
+EXIT:
+  if (TimeoutEvt != NULL) {
+    gBS->CloseEvent (TimeoutEvt);
+  }
+  return Status;
+}
+
+
+/**
+  Send out Inquiry command to Device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  NeedRetry       Indicates if needs try again when error happens
+
+  @retval  EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval  EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskInquiryDevice (
+  IN OUT  SCSI_DISK_DEV   *ScsiDiskDevice,
+     OUT  BOOLEAN         *NeedRetry
+  )
+{
+  UINT32                                InquiryDataLength;
+  UINT8                                 SenseDataLength;
+  UINT8                                 HostAdapterStatus;
+  UINT8                                 TargetStatus;
+  EFI_SCSI_SENSE_DATA                   *SenseDataArray;
+  UINTN                                 NumberOfSenseKeys;
+  EFI_STATUS                            Status;
+  UINT8                                 MaxRetry;
+  UINT8                                 Index;
+  EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE *SupportedVpdPages;
+  EFI_SCSI_BLOCK_LIMITS_VPD_PAGE        *BlockLimits;
+  UINTN                                 PageLength;
+
+  InquiryDataLength = sizeof (EFI_SCSI_INQUIRY_DATA);
+  SenseDataLength   = 0;
+
+  Status = ScsiInquiryCommand (
+            ScsiDiskDevice->ScsiIo,
+            SCSI_DISK_TIMEOUT,
+            NULL,
+            &SenseDataLength,
+            &HostAdapterStatus,
+            &TargetStatus,
+            (VOID *) &(ScsiDiskDevice->InquiryData),
+            &InquiryDataLength,
+            FALSE
+            );
+    //
+    // no need to check HostAdapterStatus and TargetStatus
+    //
+  if ((Status == EFI_SUCCESS) || (Status == EFI_WARN_BUFFER_TOO_SMALL)) {
+    ParseInquiryData (ScsiDiskDevice);
+
+    if (ScsiDiskDevice->DeviceType == EFI_SCSI_TYPE_DISK) {
+      //
+      // Check whether the device supports Block Limits VPD page (0xB0)
+      //
+      SupportedVpdPages = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+      if (SupportedVpdPages == NULL) {
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      ZeroMem (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+      InquiryDataLength = sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE);
+      SenseDataLength   = 0;
+      Status = ScsiInquiryCommandEx (
+                 ScsiDiskDevice->ScsiIo,
+                 SCSI_DISK_TIMEOUT,
+                 NULL,
+                 &SenseDataLength,
+                 &HostAdapterStatus,
+                 &TargetStatus,
+                 (VOID *) SupportedVpdPages,
+                 &InquiryDataLength,
+                 TRUE,
+                 EFI_SCSI_PAGE_CODE_SUPPORTED_VPD
+                 );
+      if (!EFI_ERROR (Status)) {
+        PageLength = (SupportedVpdPages->PageLength2 << 8)
+                   |  SupportedVpdPages->PageLength1;
+        for (Index = 0; Index < PageLength; Index++) {
+          if (SupportedVpdPages->SupportedVpdPageList[Index] == EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD) {
+            break;
+          }
+        }
+
+        //
+        // Query the Block Limits VPD page
+        //
+        if (Index < PageLength) {
+          BlockLimits = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+          if (BlockLimits == NULL) {
+            FreeAlignedBuffer (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+            *NeedRetry = FALSE;
+            return EFI_DEVICE_ERROR;
+          }
+          ZeroMem (BlockLimits, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+          InquiryDataLength = sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE);
+          SenseDataLength   = 0;
+          Status = ScsiInquiryCommandEx (
+                     ScsiDiskDevice->ScsiIo,
+                     SCSI_DISK_TIMEOUT,
+                     NULL,
+                     &SenseDataLength,
+                     &HostAdapterStatus,
+                     &TargetStatus,
+                     (VOID *) BlockLimits,
+                     &InquiryDataLength,
+                     TRUE,
+                     EFI_SCSI_PAGE_CODE_BLOCK_LIMITS_VPD
+                     );
+          if (!EFI_ERROR (Status)) {
+            ScsiDiskDevice->BlkIo.Media->OptimalTransferLengthGranularity = 
+              (BlockLimits->OptimalTransferLengthGranularity2 << 8) |
+               BlockLimits->OptimalTransferLengthGranularity1;
+          }
+
+          FreeAlignedBuffer (BlockLimits, sizeof (EFI_SCSI_BLOCK_LIMITS_VPD_PAGE));
+        }
+      }
+
+      FreeAlignedBuffer (SupportedVpdPages, sizeof (EFI_SCSI_SUPPORTED_VPD_PAGES_VPD_PAGE));
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+
+  } else if (Status == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+ 
+  } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR)
+  //
+
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+      //
+      // reset the scsi channel
+      //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  
+  //
+  // if goes here, meant ScsiInquiryCommand() failed.
+  // if ScsiDiskRequestSenseKeys() succeeds at last,
+  // better retry ScsiInquiryCommand(). (by setting *NeedRetry = TRUE)
+  //
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              &SenseDataArray,
+              &NumberOfSenseKeys,
+              TRUE
+              );
+    if (!EFI_ERROR (Status)) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  To test device.
+
+  When Test Unit Ready command succeeds, retrieve Sense Keys via Request Sense;
+  When Test Unit Ready command encounters any error caused by host adapter or
+  target, return error without retrieving Sense Keys.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates try again
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The pointer of the number of sense data array
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to test unit
+
+**/
+EFI_STATUS
+ScsiDiskTestUnitReady (
+  IN  SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT BOOLEAN               *NeedRetry,
+  OUT EFI_SCSI_SENSE_DATA   **SenseDataArray,
+  OUT UINTN                 *NumberOfSenseKeys
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINT8       Index;
+  UINT8       MaxRetry;
+
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  *NumberOfSenseKeys  = 0;
+
+  //
+  // Parameter 3 and 4: do not require sense data, retrieve it when needed.
+  //
+  Status = ScsiTestUnitReadyCommand (
+            ScsiDiskDevice->ScsiIo,
+            SCSI_DISK_TIMEOUT,
+            ScsiDiskDevice->SenseData,
+            &SenseDataLength,
+            &HostAdapterStatus,
+            &TargetStatus
+            );
+  //
+  // no need to check HostAdapterStatus and TargetStatus
+  //
+  if (Status == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus(in case of EFI_DEVICE_ERROR)
+  //
+
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (SenseDataLength != 0) {
+    *NumberOfSenseKeys = SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA);
+    *SenseDataArray    = ScsiDiskDevice->SenseData;
+    return EFI_SUCCESS;
+  }
+
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              SenseDataArray,
+              NumberOfSenseKeys,
+              FALSE
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Parsing Sense Keys which got from request sense command.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NumberOfSenseKeys  The number of sense key  
+  @param  Action             The pointer of action which indicates what is need to do next
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+DetectMediaParsingSenseKeys (
+  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+  IN   EFI_SCSI_SENSE_DATA     *SenseData,
+  IN   UINTN                   NumberOfSenseKeys,
+  OUT  UINTN                   *Action
+  )
+{
+  BOOLEAN RetryLater;
+
+  //
+  // Default is to read capacity, unless..
+  //
+  *Action = ACTION_READ_CAPACITY;
+
+  if (NumberOfSenseKeys == 0) {
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent == TRUE) {
+      *Action = ACTION_NO_ACTION;
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (!ScsiDiskHaveSenseKey (SenseData, NumberOfSenseKeys)) {
+    //
+    // No Sense Key returned from last submitted command
+    //
+    if (ScsiDiskDevice->BlkIo.Media->MediaPresent == TRUE) {
+      *Action = ACTION_NO_ACTION;
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsNoMedia (SenseData, NumberOfSenseKeys)) {
+    ScsiDiskDevice->BlkIo.Media->MediaPresent = FALSE;
+    ScsiDiskDevice->BlkIo.Media->LastBlock    = 0;
+    *Action = ACTION_NO_ACTION;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsNoMedia\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsMediaChange (SenseData, NumberOfSenseKeys)) {
+    ScsiDiskDevice->BlkIo.Media->MediaId++;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsMediaChange!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsResetBefore (SenseData, NumberOfSenseKeys)) {
+    *Action = ACTION_RETRY_COMMAND_LATER;
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsResetBefore!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (ScsiDiskIsMediaError (SenseData, NumberOfSenseKeys)) {
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsMediaError\n"));
+    *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (ScsiDiskIsHardwareError (SenseData, NumberOfSenseKeys)) {
+    DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskIsHardwareError\n"));
+    *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (!ScsiDiskIsDriveReady (SenseData, NumberOfSenseKeys, &RetryLater)) {
+    if (RetryLater) {
+      *Action = ACTION_RETRY_COMMAND_LATER;
+      DEBUG ((EFI_D_VERBOSE, "ScsiDisk: ScsiDiskDriveNotReady!\n"));
+      return EFI_SUCCESS;
+    }
+    *Action = ACTION_NO_ACTION;
+    return EFI_DEVICE_ERROR;
+  }
+
+  *Action = ACTION_RETRY_WITH_BACKOFF_ALGO;
+  DEBUG ((EFI_D_VERBOSE, "ScsiDisk: Sense Key = 0x%x ASC = 0x%x!\n", SenseData->Sense_Key, SenseData->Addnl_Sense_Code));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Send read capacity command to device and get the device parameter.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to read capacity or sense data is received.
+
+**/
+EFI_STATUS
+ScsiDiskReadCapacity (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys
+  )
+{
+  UINT8                         HostAdapterStatus;
+  UINT8                         TargetStatus;
+  EFI_STATUS                    CommandStatus;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+  UINT8                         MaxRetry;
+  UINT8                         SenseDataLength;
+  UINT32                        DataLength10;
+  UINT32                        DataLength16;
+  EFI_SCSI_DISK_CAPACITY_DATA   *CapacityData10;
+  EFI_SCSI_DISK_CAPACITY_DATA16 *CapacityData16;
+
+  CapacityData10 = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+  if (CapacityData10 == NULL) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  CapacityData16 = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+  if (CapacityData16 == NULL) {
+    FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  SenseDataLength       = 0;
+  DataLength10          = sizeof (EFI_SCSI_DISK_CAPACITY_DATA);
+  DataLength16          = sizeof (EFI_SCSI_DISK_CAPACITY_DATA16);
+  ZeroMem (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+  ZeroMem (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+
+  *NumberOfSenseKeys  = 0;
+  *NeedRetry          = FALSE;
+
+  //
+  // submit Read Capacity(10) Command. If it returns capacity of FFFFFFFFh, 
+  // 16 byte command should be used to access large hard disk >2TB
+  //
+  CommandStatus = ScsiReadCapacityCommand (
+                    ScsiDiskDevice->ScsiIo,
+                    SCSI_DISK_TIMEOUT,
+                    NULL,
+                    &SenseDataLength,
+                    &HostAdapterStatus,
+                    &TargetStatus,
+                    (VOID *) CapacityData10,
+                    &DataLength10,
+                    FALSE
+                    );
+
+  ScsiDiskDevice->Cdb16Byte = FALSE;
+  if ((!EFI_ERROR (CommandStatus)) && (CapacityData10->LastLba3 == 0xff) && (CapacityData10->LastLba2 == 0xff) &&
+      (CapacityData10->LastLba1 == 0xff) && (CapacityData10->LastLba0 == 0xff)) {
+    //
+    // use Read Capacity (16), Read (16) and Write (16) next when hard disk size > 2TB
+    //
+    ScsiDiskDevice->Cdb16Byte = TRUE;
+    //
+    // submit Read Capacity(16) Command to get parameter LogicalBlocksPerPhysicalBlock
+    // and LowestAlignedLba
+    //
+    CommandStatus = ScsiReadCapacity16Command (
+                      ScsiDiskDevice->ScsiIo,
+                      SCSI_DISK_TIMEOUT,
+                      NULL,
+                      &SenseDataLength,
+                      &HostAdapterStatus,
+                      &TargetStatus,
+                      (VOID *) CapacityData16,
+                      &DataLength16,
+                      FALSE
+                      );
+  }
+
+    //
+    // no need to check HostAdapterStatus and TargetStatus
+    //
+   if (CommandStatus == EFI_SUCCESS) {
+     GetMediaInfo (ScsiDiskDevice, CapacityData10, CapacityData16);
+     FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+     FreeAlignedBuffer (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+     return EFI_SUCCESS;
+   }
+
+   FreeAlignedBuffer (CapacityData10, sizeof (EFI_SCSI_DISK_CAPACITY_DATA));
+   FreeAlignedBuffer (CapacityData16, sizeof (EFI_SCSI_DISK_CAPACITY_DATA16));
+
+   if (CommandStatus == EFI_NOT_READY) {
+     *NeedRetry = TRUE;
+     return EFI_DEVICE_ERROR;
+   } else if ((CommandStatus == EFI_INVALID_PARAMETER) || (CommandStatus == EFI_UNSUPPORTED)) {
+     *NeedRetry = FALSE;
+     return EFI_DEVICE_ERROR;
+   }
+
+   //
+   // go ahead to check HostAdapterStatus and TargetStatus
+   // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+   //
+ 
+   Status = CheckHostAdapterStatus (HostAdapterStatus);
+   if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+     *NeedRetry = TRUE;
+     return EFI_DEVICE_ERROR;
+ 
+   } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+  
+  //
+  // if goes here, meant ScsiReadCapacityCommand() failed.
+  // if ScsiDiskRequestSenseKeys() succeeds at last,
+  // better retry ScsiReadCapacityCommand(). (by setting *NeedRetry = TRUE)
+  //
+  MaxRetry = 3;
+  for (Index = 0; Index < MaxRetry; Index++) {
+
+    Status = ScsiDiskRequestSenseKeys (
+              ScsiDiskDevice,
+              NeedRetry,
+              SenseDataArray,
+              NumberOfSenseKeys,
+              TRUE
+              );
+    if (!EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+
+    if (!*NeedRetry) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  //
+  // ScsiDiskRequestSenseKeys() failed after several rounds of retry.
+  // set *NeedRetry = FALSE to avoid the outside caller try again.
+  //
+  *NeedRetry = FALSE;
+  return EFI_DEVICE_ERROR;
+}
+
+/**
+  Check the HostAdapter status and re-interpret it in EFI_STATUS.
+
+  @param  HostAdapterStatus  Host Adapter status
+
+  @retval  EFI_SUCCESS       Host adapter is OK.
+  @retval  EFI_TIMEOUT       Timeout.
+  @retval  EFI_NOT_READY     Adapter NOT ready.
+  @retval  EFI_DEVICE_ERROR  Adapter device error.
+
+**/
+EFI_STATUS
+CheckHostAdapterStatus (
+  IN UINT8   HostAdapterStatus
+  )
+{
+  switch (HostAdapterStatus) {
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_OK:
+    return EFI_SUCCESS;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_SELECTION_TIMEOUT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT_COMMAND:
+    return EFI_TIMEOUT;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_MESSAGE_REJECT:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PARITY_ERROR:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_REQUEST_SENSE_FAILED:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_DATA_OVERRUN_UNDERRUN:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_BUS_RESET:
+    return EFI_NOT_READY;
+
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_BUS_FREE:
+  case EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR:
+    return EFI_DEVICE_ERROR;
+
+  default:
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Check the target status and re-interpret it in EFI_STATUS.
+
+  @param  TargetStatus  Target status
+
+  @retval EFI_NOT_READY       Device is NOT ready.
+  @retval EFI_DEVICE_ERROR 
+  @retval EFI_SUCCESS
+
+**/
+EFI_STATUS
+CheckTargetStatus (
+  IN  UINT8   TargetStatus
+  )
+{
+  switch (TargetStatus) {
+  case EFI_EXT_SCSI_STATUS_TARGET_GOOD:
+  case EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION:
+  case EFI_EXT_SCSI_STATUS_TARGET_CONDITION_MET:
+    return EFI_SUCCESS;
+
+  case EFI_EXT_SCSI_STATUS_TARGET_INTERMEDIATE:
+  case EFI_EXT_SCSI_STATUS_TARGET_INTERMEDIATE_CONDITION_MET:
+  case EFI_EXT_SCSI_STATUS_TARGET_BUSY:
+  case EFI_EXT_SCSI_STATUS_TARGET_TASK_SET_FULL:
+    return EFI_NOT_READY;
+
+  case EFI_EXT_SCSI_STATUS_TARGET_RESERVATION_CONFLICT:
+    return EFI_DEVICE_ERROR;
+
+  default:
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Retrieve all sense keys from the device.
+
+  When encountering error during the process, if retrieve sense keys before
+  error encountered, it returns the sense keys with return status set to EFI_SUCCESS,
+  and NeedRetry set to FALSE; otherwize, return the proper return status.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+  @param  AskResetIfError    The flag indicates if need reset when error occurs
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to request sense key
+
+**/
+EFI_STATUS
+ScsiDiskRequestSenseKeys (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys,
+  IN       BOOLEAN                 AskResetIfError
+  )
+{
+  EFI_SCSI_SENSE_DATA *PtrSenseData;
+  UINT8               SenseDataLength;
+  BOOLEAN             SenseReq;
+  EFI_STATUS          Status;
+  EFI_STATUS          FallStatus;
+  UINT8               HostAdapterStatus;
+  UINT8               TargetStatus;
+
+  FallStatus      = EFI_SUCCESS;
+  SenseDataLength = (UINT8) sizeof (EFI_SCSI_SENSE_DATA);
+
+  ZeroMem (
+    ScsiDiskDevice->SenseData,
+    sizeof (EFI_SCSI_SENSE_DATA) * (ScsiDiskDevice->SenseDataNumber)
+    );
+
+  *NumberOfSenseKeys  = 0;
+  *SenseDataArray     = ScsiDiskDevice->SenseData;
+  Status              = EFI_SUCCESS;
+  PtrSenseData        = AllocateAlignedBuffer (ScsiDiskDevice, sizeof (EFI_SCSI_SENSE_DATA));
+  if (PtrSenseData == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  for (SenseReq = TRUE; SenseReq;) {
+    ZeroMem (PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+    Status = ScsiRequestSenseCommand (
+              ScsiDiskDevice->ScsiIo,
+              SCSI_DISK_TIMEOUT,
+              PtrSenseData,
+              &SenseDataLength,
+              &HostAdapterStatus,
+              &TargetStatus
+              );
+     if ((Status == EFI_SUCCESS) || (Status == EFI_WARN_BUFFER_TOO_SMALL)) {
+        FallStatus = EFI_SUCCESS;
+  
+     } else if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+       *NeedRetry  = TRUE;
+       FallStatus  = EFI_DEVICE_ERROR;
+ 
+     } else if ((Status == EFI_INVALID_PARAMETER) || (Status == EFI_UNSUPPORTED)) {
+       *NeedRetry  = FALSE;
+       FallStatus  = EFI_DEVICE_ERROR;
+ 
+     } else if (Status == EFI_DEVICE_ERROR) {
+        if (AskResetIfError) {
+          ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+        }
+  
+        FallStatus = EFI_DEVICE_ERROR;
+    }
+
+    if (EFI_ERROR (FallStatus)) {
+      if (*NumberOfSenseKeys != 0) {
+        *NeedRetry = FALSE;
+        Status = EFI_SUCCESS;
+        goto EXIT;
+      } else {
+        Status = EFI_DEVICE_ERROR;
+        goto EXIT;
+      }
+    }
+
+    CopyMem (ScsiDiskDevice->SenseData + *NumberOfSenseKeys, PtrSenseData, SenseDataLength);
+    (*NumberOfSenseKeys) += 1;
+
+    //
+    // no more sense key or number of sense keys exceeds predefined,
+    // skip the loop.
+    //
+    if ((PtrSenseData->Sense_Key == EFI_SCSI_SK_NO_SENSE) || 
+        (*NumberOfSenseKeys == ScsiDiskDevice->SenseDataNumber)) {
+      SenseReq = FALSE;
+    }
+  }
+
+EXIT:
+  FreeAlignedBuffer (PtrSenseData, sizeof (EFI_SCSI_SENSE_DATA));
+  return Status;
+}
+
+
+/**
+  Get information from media read capacity command.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Capacity10      The pointer of EFI_SCSI_DISK_CAPACITY_DATA
+  @param  Capacity16      The pointer of EFI_SCSI_DISK_CAPACITY_DATA16
+
+**/
+VOID
+GetMediaInfo (
+  IN OUT SCSI_DISK_DEV                  *ScsiDiskDevice,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA    *Capacity10,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA16  *Capacity16
+  )
+{
+  UINT8       *Ptr;
+
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    ScsiDiskDevice->BlkIo.Media->LastBlock =  (Capacity10->LastLba3 << 24) |
+                                              (Capacity10->LastLba2 << 16) |
+                                              (Capacity10->LastLba1 << 8)  |
+                                               Capacity10->LastLba0;
+  
+    ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity10->BlockSize3 << 24) |
+                                             (Capacity10->BlockSize2 << 16) | 
+                                             (Capacity10->BlockSize1 << 8)  |
+                                              Capacity10->BlockSize0;
+    ScsiDiskDevice->BlkIo.Media->LowestAlignedLba               = 0;
+    ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = 0;
+  } else {
+    Ptr = (UINT8*)&ScsiDiskDevice->BlkIo.Media->LastBlock;
+    *Ptr++ = Capacity16->LastLba0;
+    *Ptr++ = Capacity16->LastLba1;
+    *Ptr++ = Capacity16->LastLba2;
+    *Ptr++ = Capacity16->LastLba3;
+    *Ptr++ = Capacity16->LastLba4;
+    *Ptr++ = Capacity16->LastLba5;
+    *Ptr++ = Capacity16->LastLba6;
+    *Ptr   = Capacity16->LastLba7;
+
+    ScsiDiskDevice->BlkIo.Media->BlockSize = (Capacity16->BlockSize3 << 24) |
+                                             (Capacity16->BlockSize2 << 16) | 
+                                             (Capacity16->BlockSize1 << 8)  |
+                                              Capacity16->BlockSize0;
+
+    ScsiDiskDevice->BlkIo.Media->LowestAlignedLba = (Capacity16->LowestAlignLogic2 << 8) |
+                                                     Capacity16->LowestAlignLogic1;
+    ScsiDiskDevice->BlkIo.Media->LogicalBlocksPerPhysicalBlock  = (1 << Capacity16->LogicPerPhysical);
+  }
+
+  ScsiDiskDevice->BlkIo.Media->MediaPresent = TRUE;
+}
+
+/**
+  Parse Inquiry data.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ParseInquiryData (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  ScsiDiskDevice->FixedDevice               = (BOOLEAN) ((ScsiDiskDevice->InquiryData.Rmb == 1) ? 0 : 1);
+  ScsiDiskDevice->BlkIoMedia.RemovableMedia = (BOOLEAN) (!ScsiDiskDevice->FixedDevice);
+}
+
+/**
+  Read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer to fill in the read out data
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskReadSectors (
+  IN   SCSI_DISK_DEV     *ScsiDiskDevice,
+  OUT  VOID              *Buffer,
+  IN   EFI_LBA           Lba,
+  IN   UINTN             NumberOfBlocks
+  )
+{
+  UINTN               BlocksRemaining;
+  UINT8               *PtrBuffer;
+  UINT32              BlockSize;
+  UINT32              ByteCount;
+  UINT32              MaxBlock;
+  UINT32              SectorCount;
+  UINT64              Timeout;
+  EFI_STATUS          Status;
+  UINT8               Index;
+  UINT8               MaxRetry;
+  BOOLEAN             NeedRetry;
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+  
+  //
+  // limit the data bytes that can be transferred by one Read(10) or Read(16) Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices, we have to use
+    // the lowest transfer rate to calculate the possible maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional 30s is added
+    // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+    // commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    MaxRetry  = 2;
+    for (Index = 0; Index < MaxRetry; Index++) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskRead10 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  (UINT32) Lba,
+                  SectorCount
+                  );
+      } else {
+        Status = ScsiDiskRead16 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  Lba,
+                  SectorCount
+                  );
+      }
+      if (!EFI_ERROR (Status)) {
+        break;
+      }
+
+      if (!NeedRetry) {
+        return EFI_DEVICE_ERROR;
+      }
+
+    }
+
+    if ((Index == MaxRetry) && (Status != EFI_SUCCESS)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // actual transferred sectors
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer of data to be written into SCSI Disk
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskWriteSectors (
+  IN  SCSI_DISK_DEV     *ScsiDiskDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_LBA           Lba,
+  IN  UINTN             NumberOfBlocks
+  )
+{
+  UINTN               BlocksRemaining;
+  UINT8               *PtrBuffer;
+  UINT32              BlockSize;
+  UINT32              ByteCount;
+  UINT32              MaxBlock;
+  UINT32              SectorCount;
+  UINT64              Timeout;
+  EFI_STATUS          Status;
+  UINT8               Index;
+  UINT8               MaxRetry;
+  BOOLEAN             NeedRetry;
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // limit the data bytes that can be transferred by one Read(10) or Read(16) Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices, we have to use
+    // the lowest transfer rate to calculate the possible maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional 30s is added
+    // to follow ATA spec in which it mentioned that the device may take up to 30s to respond
+    // commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+    MaxRetry  = 2;
+    for (Index = 0; Index < MaxRetry; Index++) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskWrite10 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  (UINT32) Lba,
+                  SectorCount
+                  );
+      } else {
+        Status = ScsiDiskWrite16 (
+                  ScsiDiskDevice,
+                  &NeedRetry,
+                  Timeout,
+                  PtrBuffer,
+                  &ByteCount,
+                  Lba,
+                  SectorCount
+                  );         
+        }
+      if (!EFI_ERROR (Status)) {
+        break;
+      }
+
+      if (!NeedRetry) {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    if ((Index == MaxRetry) && (Status != EFI_SUCCESS)) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // actual transferred sectors
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Asynchronously read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer to fill in the read out data.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL.
+  @retval EFI_DEVICE_ERROR       Indicates a device error.
+  @retval EFI_SUCCESS            Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncReadSectors (
+  IN   SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT  VOID                  *Buffer,
+  IN   EFI_LBA               Lba,
+  IN   UINTN                 NumberOfBlocks,
+  IN   EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  UINTN                 BlocksRemaining;
+  UINT8                 *PtrBuffer;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  UINT32                MaxBlock;
+  UINT32                SectorCount;
+  UINT64                Timeout;
+  SCSI_BLKIO2_REQUEST   *BlkIo2Req;
+  EFI_STATUS            Status;
+  EFI_TPL               OldTpl;
+
+  if ((Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BlkIo2Req = AllocateZeroPool (sizeof (SCSI_BLKIO2_REQUEST));
+  if (BlkIo2Req == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlkIo2Req->Token  = Token;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  InitializeListHead (&BlkIo2Req->ScsiRWQueue);
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // Limit the data bytes that can be transferred by one Read(10) or Read(16)
+  // Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices,
+    // we have to use the lowest transfer rate to calculate the possible
+    // maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional
+    // 30s is added to follow ATA spec in which it mentioned that the device
+    // may take up to 30s to respond commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncRead10 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 (UINT32) Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncRead16 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data
+      // length of a SCSI I/O command is too large.
+      // In this case, we retry sending the SCSI command with a data length
+      // half of its previous value.
+      //
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {
+        if ((MaxBlock > 1) && (SectorCount > 1)) {
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;
+          continue;
+        }
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+        //
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other
+        // SCSI sub-task running. Otherwise, it will be freed in the callback
+        // function ScsiDiskNotify().
+        //
+        RemoveEntryList (&BlkIo2Req->Link);
+        FreePool (BlkIo2Req);
+        BlkIo2Req = NULL;
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // It is safe to return error status to the caller, since there is no
+        // previous SCSI sub-task executing.
+        //
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      } else {
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // There are previous SCSI commands still running, EFI_SUCCESS should
+        // be returned to make sure that the caller does not free resources
+        // still using by these SCSI commands.
+        //
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+    }
+
+    //
+    // Sectors submitted for transfer
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  if (BlkIo2Req != NULL) {
+    BlkIo2Req->LastScsiRW = TRUE;
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+      RemoveEntryList (&BlkIo2Req->Link);
+      FreePool (BlkIo2Req);
+      BlkIo2Req = NULL;
+
+      gBS->SignalEvent (Token->Event);
+    }
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  return Status;
+}
+
+/**
+  Asynchronously write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer of data to be written into SCSI Disk.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWriteSectors (
+  IN  SCSI_DISK_DEV          *ScsiDiskDevice,
+  IN  VOID                   *Buffer,
+  IN  EFI_LBA                Lba,
+  IN  UINTN                  NumberOfBlocks,
+  IN  EFI_BLOCK_IO2_TOKEN    *Token
+  )
+{
+  UINTN                 BlocksRemaining;
+  UINT8                 *PtrBuffer;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  UINT32                MaxBlock;
+  UINT32                SectorCount;
+  UINT64                Timeout;
+  SCSI_BLKIO2_REQUEST   *BlkIo2Req;
+  EFI_STATUS            Status;
+  EFI_TPL               OldTpl;
+
+  if ((Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BlkIo2Req = AllocateZeroPool (sizeof (SCSI_BLKIO2_REQUEST));
+  if (BlkIo2Req == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlkIo2Req->Token  = Token;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&ScsiDiskDevice->BlkIo2Queue, &BlkIo2Req->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  InitializeListHead (&BlkIo2Req->ScsiRWQueue);
+
+  Status            = EFI_SUCCESS;
+
+  BlocksRemaining   = NumberOfBlocks;
+  BlockSize         = ScsiDiskDevice->BlkIo.Media->BlockSize;
+
+  //
+  // Limit the data bytes that can be transferred by one Read(10) or Read(16)
+  // Command
+  //
+  if (!ScsiDiskDevice->Cdb16Byte) {
+    MaxBlock         = 0xFFFF;
+  } else {
+    MaxBlock         = 0xFFFFFFFF;
+  }
+
+  PtrBuffer = Buffer;
+
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        SectorCount = (UINT16) BlocksRemaining;
+      } else {
+        SectorCount = (UINT32) BlocksRemaining;
+      }
+    } else {
+      SectorCount = MaxBlock;
+    }
+
+    ByteCount = SectorCount * BlockSize;
+    //
+    // |------------------------|-----------------|------------------|-----------------|
+    // |   ATA Transfer Mode    |  Transfer Rate  |  SCSI Interface  |  Transfer Rate  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 0       |  3.3Mbytes/sec  |     SCSI-1       |    5Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 1       |  5.2Mbytes/sec  |    Fast SCSI     |   10Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 2       |  8.3Mbytes/sec  |  Fast-Wide SCSI  |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 3       | 11.1Mbytes/sec  |    Ultra SCSI    |   20Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // |       PIO Mode 4       | 16.6Mbytes/sec  |  Ultra Wide SCSI |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 0 |  2.1Mbytes/sec  |    Ultra2 SCSI   |   40Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 1 |  4.2Mbytes/sec  | Ultra2 Wide SCSI |   80Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Single-word DMA Mode 2 |  8.4Mbytes/sec  |    Ultra3 SCSI   |  160Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 0  |  4.2Mbytes/sec  |  Ultra-320 SCSI  |  320Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    // | Multi-word DMA Mode 1  | 13.3Mbytes/sec  |  Ultra-640 SCSI  |  640Mbytes/sec  |
+    // |------------------------|-----------------|------------------|-----------------|
+    //
+    // As ScsiDisk and ScsiBus driver are used to manage SCSI or ATAPI devices,
+    // we have to use the lowest transfer rate to calculate the possible
+    // maximum timeout value for each operation.
+    // From the above table, we could know 2.1Mbytes per second is lowest one.
+    // The timout value is rounded up to nearest integar and here an additional
+    // 30s is added to follow ATA spec in which it mentioned that the device
+    // may take up to 30s to respond commands in the Standby/Idle mode.
+    //
+    Timeout   = EFI_TIMER_PERIOD_SECONDS (ByteCount / 2100000 + 31);
+
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncWrite10 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 (UINT32) Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncWrite16 (
+                 ScsiDiskDevice,
+                 Timeout,
+                 0,
+                 PtrBuffer,
+                 ByteCount,
+                 Lba,
+                 SectorCount,
+                 BlkIo2Req,
+                 Token
+                 );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Some devices will return EFI_DEVICE_ERROR or EFI_TIMEOUT when the data
+      // length of a SCSI I/O command is too large.
+      // In this case, we retry sending the SCSI command with a data length
+      // half of its previous value.
+      //
+      if ((Status == EFI_DEVICE_ERROR) || (Status == EFI_TIMEOUT)) {
+        if ((MaxBlock > 1) && (SectorCount > 1)) {
+          MaxBlock = MIN (MaxBlock, SectorCount) >> 1;
+          continue;
+        }
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+      if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+        //
+        // Free the SCSI_BLKIO2_REQUEST structure only when there is no other
+        // SCSI sub-task running. Otherwise, it will be freed in the callback
+        // function ScsiDiskNotify().
+        //
+        RemoveEntryList (&BlkIo2Req->Link);
+        FreePool (BlkIo2Req);
+        BlkIo2Req = NULL;
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // It is safe to return error status to the caller, since there is no
+        // previous SCSI sub-task executing.
+        //
+        Status = EFI_DEVICE_ERROR;
+        goto Done;
+      } else {
+        gBS->RestoreTPL (OldTpl);
+
+        //
+        // There are previous SCSI commands still running, EFI_SUCCESS should
+        // be returned to make sure that the caller does not free resources
+        // still using by these SCSI commands.
+        //
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+    }
+
+    //
+    // Sectors submitted for transfer
+    //
+    SectorCount = ByteCount / BlockSize;
+
+    Lba += SectorCount;
+    PtrBuffer = PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining -= SectorCount;
+  }
+
+  Status = EFI_SUCCESS;
+
+Done:
+  if (BlkIo2Req != NULL) {
+    BlkIo2Req->LastScsiRW = TRUE;
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    if (IsListEmpty (&BlkIo2Req->ScsiRWQueue)) {
+      RemoveEntryList (&BlkIo2Req->Link);
+      FreePool (BlkIo2Req);
+      BlkIo2Req = NULL;
+
+      gBS->SignalEvent (Token->Event);
+    }
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead10Command().
+**/
+EFI_STATUS
+ScsiDiskRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  UINT8       SenseDataLength;
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiRead10Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskRead10: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Write(10) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite10Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiWrite10Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskWrite10: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead16Command().
+**/
+EFI_STATUS
+ScsiDiskRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  UINT8       SenseDataLength;
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiRead16Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskRead16: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Submit Write(16) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite16Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  ReturnStatus;
+  UINT8       SenseDataLength;
+  UINT8       HostAdapterStatus;
+  UINT8       TargetStatus;
+  UINTN       Action;
+
+  //
+  // Implement a backoff algorithem to resolve some compatibility issues that
+  // some SCSI targets or ATAPI devices couldn't correctly response reading/writing
+  // big data in a single operation.
+  // This algorithem will at first try to execute original request. If the request fails
+  // with media error sense data or else, it will reduce the transfer length to half and
+  // try again till the operation succeeds or fails with one sector transfer length.
+  //
+BackOff:
+  *NeedRetry          = FALSE;
+  Action              = ACTION_NO_ACTION;
+  SenseDataLength     = (UINT8) (ScsiDiskDevice->SenseDataNumber * sizeof (EFI_SCSI_SENSE_DATA));
+  ReturnStatus = ScsiWrite16Command (
+                   ScsiDiskDevice->ScsiIo,
+                   Timeout,
+                   ScsiDiskDevice->SenseData,
+                   &SenseDataLength,
+                   &HostAdapterStatus,
+                   &TargetStatus,
+                   DataBuffer,
+                   DataLength,
+                   StartLba,
+                   SectorCount
+                   );
+  if (ReturnStatus == EFI_NOT_READY) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if ((ReturnStatus == EFI_INVALID_PARAMETER) || (ReturnStatus == EFI_UNSUPPORTED)) {
+    *NeedRetry = FALSE;
+    return ReturnStatus;
+  }
+
+  //
+  // go ahead to check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = CheckTargetStatus (TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    *NeedRetry = TRUE;
+    return EFI_DEVICE_ERROR;
+  } else if (Status == EFI_DEVICE_ERROR) {
+    *NeedRetry = FALSE;
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) || (EFI_ERROR (ReturnStatus))) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskWrite16: Check Condition happened!\n"));
+    Status = DetectMediaParsingSenseKeys (ScsiDiskDevice, ScsiDiskDevice->SenseData, SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA), &Action);
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      *NeedRetry = TRUE;
+      return EFI_DEVICE_ERROR;
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer length.
+        //
+        *NeedRetry = FALSE;
+        return EFI_DEVICE_ERROR;
+      }
+      //
+      // Try again with half length if the sense data shows we need to retry.
+      //
+      SectorCount >>= 1;
+      *DataLength = SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      goto BackOff;
+    } else {
+      *NeedRetry = FALSE;
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Internal helper notify function in which determine whether retry of a SCSI
+  Read/Write command is needed and signal the event passed from Block I/O(2) if
+  the SCSI I/O operation completes.
+
+  @param  Event    The instance of EFI_EVENT.
+  @param  Context  The parameter passed in.
+
+**/
+VOID
+EFIAPI
+ScsiDiskNotify (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_STATUS                       Status;
+  SCSI_ASYNC_RW_REQUEST            *Request;
+  SCSI_DISK_DEV                    *ScsiDiskDevice;
+  EFI_BLOCK_IO2_TOKEN              *Token;
+  UINTN                            Action;
+  UINT32                           OldDataLength;
+  UINT32                           OldSectorCount;
+  UINT8                            MaxRetry;
+
+  gBS->CloseEvent (Event);
+
+  Request         = (SCSI_ASYNC_RW_REQUEST *) Context;
+  ScsiDiskDevice  = Request->ScsiDiskDevice;
+  Token           = Request->BlkIo2Req->Token;
+  OldDataLength   = Request->DataLength;
+  OldSectorCount  = Request->SectorCount;
+  MaxRetry        = 2;
+
+  //
+  // If previous sub-tasks already fails, no need to process this sub-task.
+  //
+  if (Token->TransactionStatus != EFI_SUCCESS) {
+    goto Exit;
+  }
+
+  //
+  // Check HostAdapterStatus and TargetStatus
+  // (EFI_TIMEOUT, EFI_DEVICE_ERROR, EFI_WARN_BUFFER_TOO_SMALL)
+  //
+  Status = CheckHostAdapterStatus (Request->HostAdapterStatus);
+  if ((Status == EFI_TIMEOUT) || (Status == EFI_NOT_READY)) {
+    if (++Request->TimesRetry > MaxRetry) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else {
+      goto Retry;
+    }
+  } else if (Status == EFI_DEVICE_ERROR) {
+    //
+    // reset the scsi channel
+    //
+    ScsiDiskDevice->ScsiIo->ResetBus (ScsiDiskDevice->ScsiIo);
+    Token->TransactionStatus = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  Status = CheckTargetStatus (Request->TargetStatus);
+  if (Status == EFI_NOT_READY) {
+    //
+    // reset the scsi device
+    //
+    ScsiDiskDevice->ScsiIo->ResetDevice (ScsiDiskDevice->ScsiIo);
+    if (++Request->TimesRetry > MaxRetry) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else {
+      goto Retry;
+    }
+  } else if (Status == EFI_DEVICE_ERROR) {
+    Token->TransactionStatus = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Request->TargetStatus == EFI_EXT_SCSI_STATUS_TARGET_CHECK_CONDITION) {
+    DEBUG ((EFI_D_ERROR, "ScsiDiskNotify: Check Condition happened!\n"));
+
+    Status = DetectMediaParsingSenseKeys (
+               ScsiDiskDevice,
+               Request->SenseData,
+               Request->SenseDataLength / sizeof (EFI_SCSI_SENSE_DATA),
+               &Action
+               );
+    if (Action == ACTION_RETRY_COMMAND_LATER) {
+      if (++Request->TimesRetry > MaxRetry) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      } else {
+        goto Retry;
+      }
+    } else if (Action == ACTION_RETRY_WITH_BACKOFF_ALGO) {
+      if (Request->SectorCount <= 1) {
+        //
+        // Jump out if the operation still fails with one sector transfer
+        // length.
+        //
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+      //
+      // Try again with two half length request if the sense data shows we need
+      // to retry.
+      //
+      Request->SectorCount >>= 1;
+      Request->DataLength = Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize;
+      Request->TimesRetry  = 0;
+
+      goto Retry;
+    } else {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+  }
+
+  //
+  // This sub-task succeeds, no need to retry.
+  //
+  goto Exit;
+
+Retry:
+  if (Request->InBuffer != NULL) {
+    //
+    // SCSI read command
+    //
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncRead10 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->InBuffer,
+                 Request->DataLength,
+                 (UINT32) Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncRead16 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->InBuffer,
+                 Request->DataLength,
+                 Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    }
+
+    if (EFI_ERROR (Status)) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else if (OldSectorCount != Request->SectorCount) {
+      //
+      // Original sub-task will be split into two new sub-tasks with smaller
+      // DataLength
+      //
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskAsyncRead10 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   (UINT32) Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      } else {
+        Status = ScsiDiskAsyncRead16 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->InBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      }
+      if (EFI_ERROR (Status)) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+    }
+  } else {
+    //
+    // SCSI write command
+    //
+    if (!ScsiDiskDevice->Cdb16Byte) {
+      Status = ScsiDiskAsyncWrite10 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->OutBuffer,
+                 Request->DataLength,
+                 (UINT32) Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    } else {
+      Status = ScsiDiskAsyncWrite16 (
+                 ScsiDiskDevice,
+                 Request->Timeout,
+                 Request->TimesRetry,
+                 Request->OutBuffer,
+                 Request->DataLength,
+                 Request->StartLba,
+                 Request->SectorCount,
+                 Request->BlkIo2Req,
+                 Token
+                 );
+    }
+
+    if (EFI_ERROR (Status)) {
+      Token->TransactionStatus = EFI_DEVICE_ERROR;
+      goto Exit;
+    } else if (OldSectorCount != Request->SectorCount) {
+      //
+      // Original sub-task will be split into two new sub-tasks with smaller
+      // DataLength
+      //
+      if (!ScsiDiskDevice->Cdb16Byte) {
+        Status = ScsiDiskAsyncWrite10 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   (UINT32) Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      } else {
+        Status = ScsiDiskAsyncWrite16 (
+                   ScsiDiskDevice,
+                   Request->Timeout,
+                   0,
+                   Request->OutBuffer + Request->SectorCount * ScsiDiskDevice->BlkIo.Media->BlockSize,
+                   OldDataLength - Request->DataLength,
+                   Request->StartLba + Request->SectorCount,
+                   OldSectorCount - Request->SectorCount,
+                   Request->BlkIo2Req,
+                   Token
+                   );
+      }
+      if (EFI_ERROR (Status)) {
+        Token->TransactionStatus = EFI_DEVICE_ERROR;
+        goto Exit;
+      }
+    }
+  }
+
+Exit:
+  RemoveEntryList (&Request->Link);
+  if ((IsListEmpty (&Request->BlkIo2Req->ScsiRWQueue)) &&
+      (Request->BlkIo2Req->LastScsiRW)) {
+    //
+    // The last SCSI R/W command of a BlockIo2 request completes
+    //
+    RemoveEntryList (&Request->BlkIo2Req->Link);
+    FreePool (Request->BlkIo2Req);  // Should be freed only once
+    gBS->SignalEvent (Token->Event);
+  }
+
+  FreePool (Request->SenseData);
+  FreePool (Request);
+}
+
+
+/**
+  Submit Async Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->InBuffer        = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiRead10CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->InBuffer,
+             &Request->DataLength,
+             (UINT32) Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Write(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->OutBuffer       = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiWrite10CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->OutBuffer,
+             &Request->DataLength,
+             (UINT32) Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->InBuffer        = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiRead16CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->InBuffer,
+             &Request->DataLength,
+             Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Submit Async Write(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  )
+{
+  EFI_STATUS                   Status;
+  SCSI_ASYNC_RW_REQUEST        *Request;
+  EFI_EVENT                    AsyncIoEvent;
+  EFI_TPL                      OldTpl;
+
+  AsyncIoEvent = NULL;
+
+  Request = AllocateZeroPool (sizeof (SCSI_ASYNC_RW_REQUEST));
+  if (Request == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  InsertTailList (&BlkIo2Req->ScsiRWQueue, &Request->Link);
+  gBS->RestoreTPL (OldTpl);
+
+  Request->SenseDataLength = (UINT8) (6 * sizeof (EFI_SCSI_SENSE_DATA));
+  Request->SenseData       = AllocateZeroPool (Request->SenseDataLength);
+  if (Request->SenseData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  Request->ScsiDiskDevice  = ScsiDiskDevice;
+  Request->Timeout         = Timeout;
+  Request->TimesRetry      = TimesRetry;
+  Request->OutBuffer       = DataBuffer;
+  Request->DataLength      = DataLength;
+  Request->StartLba        = StartLba;
+  Request->SectorCount     = SectorCount;
+  Request->BlkIo2Req       = BlkIo2Req;
+
+  //
+  // Create Event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ScsiDiskNotify,
+                  Request,
+                  &AsyncIoEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  Status = ScsiWrite16CommandEx (
+             ScsiDiskDevice->ScsiIo,
+             Request->Timeout,
+             Request->SenseData,
+             &Request->SenseDataLength,
+             &Request->HostAdapterStatus,
+             &Request->TargetStatus,
+             Request->OutBuffer,
+             &Request->DataLength,
+             Request->StartLba,
+             Request->SectorCount,
+             AsyncIoEvent
+             );
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  if (AsyncIoEvent != NULL) {
+    gBS->CloseEvent (AsyncIoEvent);
+  }
+
+  if (Request != NULL) {
+    if (Request->SenseData != NULL) {
+      FreePool (Request->SenseData);
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    RemoveEntryList (&Request->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    FreePool (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  Check sense key to find if media presents.
+
+  @param  SenseData   The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts The number of sense key
+
+  @retval TRUE    NOT any media
+  @retval FALSE   Media presents
+**/
+BOOLEAN
+ScsiDiskIsNoMedia (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsNoMedia;
+
+  IsNoMedia = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // Sense Key is EFI_SCSI_SK_NOT_READY (0x2),
+    // Additional Sense Code is ASC_NO_MEDIA (0x3A)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_NO_MEDIA)) {
+      IsNoMedia = TRUE;
+    }
+    SensePtr++;
+  }
+
+  return IsNoMedia;
+}
+
+
+/**
+  Parse sense key.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Error
+  @retval FALSE  NOT error
+
+**/
+BOOLEAN
+ScsiDiskIsMediaError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsError;
+
+  IsError   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->Sense_Key) {
+
+    case EFI_SCSI_SK_MEDIUM_ERROR:
+      //
+      // Sense Key is EFI_SCSI_SK_MEDIUM_ERROR (0x3)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR1:
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR2:
+
+      //
+      // fall through
+      //
+      case EFI_SCSI_ASC_MEDIA_ERR3:
+      case EFI_SCSI_ASC_MEDIA_ERR4:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case EFI_SCSI_SK_NOT_READY:
+      //
+      // Sense Key is EFI_SCSI_SK_NOT_READY (0x2)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+      //
+      // Additional Sense Code is ASC_MEDIA_UPSIDE_DOWN (0x6)
+      //
+      case EFI_SCSI_ASC_MEDIA_UPSIDE_DOWN:
+        IsError = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+
+/**
+  Check sense key to find if hardware error happens.
+
+  @param  SenseData     The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts   The number of sense key
+
+  @retval TRUE  Hardware error exits.
+  @retval FALSE NO error.
+
+**/
+BOOLEAN
+ScsiDiskIsHardwareError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsError;
+
+  IsError   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is EFI_SCSI_SK_HARDWARE_ERROR (0x4)
+    //
+    if (SensePtr->Sense_Key == EFI_SCSI_SK_HARDWARE_ERROR) {
+      IsError = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsError;
+}
+
+
+/**
+  Check sense key to find if media has changed.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Media is changed.
+  @retval FALSE  Media is NOT changed.
+**/
+BOOLEAN
+ScsiDiskIsMediaChange (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsMediaChanged;
+
+  IsMediaChanged  = FALSE;
+  SensePtr        = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // Sense Key is EFI_SCSI_SK_UNIT_ATTENTION (0x6),
+    // Additional sense code is EFI_SCSI_ASC_MEDIA_CHANGE (0x28)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_CHANGE)) {
+      IsMediaChanged = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsMediaChanged;
+}
+
+/**
+  Check sense key to find if reset happens.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE  It is reset before.
+  @retval FALSE It is NOT reset before.
+
+**/
+BOOLEAN
+ScsiDiskIsResetBefore (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsResetBefore;
+
+  IsResetBefore = FALSE;
+  SensePtr      = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is EFI_SCSI_SK_UNIT_ATTENTION (0x6)
+    // Additional Sense Code is EFI_SCSI_ASC_RESET (0x29)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+        (SensePtr->Addnl_Sense_Code == EFI_SCSI_ASC_RESET)) {
+      IsResetBefore = TRUE;
+    }
+
+    SensePtr++;
+  }
+
+  return IsResetBefore;
+}
+
+/**
+  Check sense key to find if the drive is ready.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+  @param  RetryLater   The flag means if need a retry 
+
+  @retval TRUE  Drive is ready.
+  @retval FALSE Drive is NOT ready.
+
+**/
+BOOLEAN
+ScsiDiskIsDriveReady (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *RetryLater
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             IsReady;
+
+  IsReady     = TRUE;
+  *RetryLater = FALSE;
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->Sense_Key) {
+
+    case EFI_SCSI_SK_NOT_READY:
+      //
+      // Sense Key is EFI_SCSI_SK_NOT_READY (0x2)
+      //
+      switch (SensePtr->Addnl_Sense_Code) {
+      case EFI_SCSI_ASC_NOT_READY:
+        //
+        // Additional Sense Code is EFI_SCSI_ASC_NOT_READY (0x4)
+        //
+        switch (SensePtr->Addnl_Sense_Code_Qualifier) {
+        case EFI_SCSI_ASCQ_IN_PROGRESS:
+          //
+          // Additional Sense Code Qualifier is
+          // EFI_SCSI_ASCQ_IN_PROGRESS (0x1)
+          //
+          IsReady     = FALSE;
+          *RetryLater = TRUE;
+          break;
+
+        default:
+          IsReady     = FALSE;
+          *RetryLater = FALSE;
+          break;
+        }
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return IsReady;
+}
+
+/**
+  Check sense key to find if it has sense key.
+
+  @param  SenseData   - The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts - The number of sense key
+
+  @retval TRUE  It has sense key.
+  @retval FALSE It has NOT any sense key.
+
+**/
+BOOLEAN
+ScsiDiskHaveSenseKey (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  )
+{
+  EFI_SCSI_SENSE_DATA *SensePtr;
+  UINTN               Index;
+  BOOLEAN             HaveSenseKey;
+
+  if (SenseCounts == 0) {
+    HaveSenseKey = FALSE;
+  } else {
+    HaveSenseKey = TRUE;
+  }
+
+  SensePtr = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    
+    //
+    // Sense Key is SK_NO_SENSE (0x0)
+    //
+    if ((SensePtr->Sense_Key == EFI_SCSI_SK_NO_SENSE) &&
+        (Index == 0)) {
+      HaveSenseKey = FALSE;
+    }
+
+    SensePtr++;
+  }
+
+  return HaveSenseKey;
+}
+
+/**
+  Release resource about disk device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ReleaseScsiDiskDeviceResources (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  if (ScsiDiskDevice == NULL) {
+    return ;
+  }
+
+  if (ScsiDiskDevice->SenseData != NULL) {
+    FreePool (ScsiDiskDevice->SenseData);
+    ScsiDiskDevice->SenseData = NULL;
+  }
+
+  if (ScsiDiskDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (ScsiDiskDevice->ControllerNameTable);
+    ScsiDiskDevice->ControllerNameTable = NULL;
+  }
+
+  FreePool (ScsiDiskDevice);
+
+  ScsiDiskDevice = NULL;
+}
+
+/**
+  Determine if Block Io & Block Io2 should be produced.
+  
+
+  @param  ChildHandle  Child Handle to retrieve Parent information.
+  
+  @retval  TRUE    Should produce Block Io & Block Io2.
+  @retval  FALSE   Should not produce Block Io & Block Io2.
+
+**/  
+BOOLEAN
+DetermineInstallBlockIo (
+  IN  EFI_HANDLE      ChildHandle
+  )  
+{
+  EFI_SCSI_PASS_THRU_PROTOCOL           *ScsiPassThru;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiPassThru;
+
+  //
+  // Firstly, check if ExtScsiPassThru Protocol parent handle exists. If existence,
+  // check its attribute, logic or physical.
+  //
+  ExtScsiPassThru = (EFI_EXT_SCSI_PASS_THRU_PROTOCOL *)GetParentProtocol (&gEfiExtScsiPassThruProtocolGuid, ChildHandle);
+  if (ExtScsiPassThru != NULL) {
+    if ((ExtScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL) != 0) {
+      return TRUE;
+    }
+  }
+
+  //
+  // Secondly, check if ScsiPassThru Protocol parent handle exists. If existence,
+  // check its attribute, logic or physical.
+  //
+  ScsiPassThru = (EFI_SCSI_PASS_THRU_PROTOCOL *)GetParentProtocol (&gEfiScsiPassThruProtocolGuid, ChildHandle);
+  if (ScsiPassThru != NULL) {
+    if ((ScsiPassThru->Mode->Attributes & EFI_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL) != 0) {
+      return TRUE;
+    }
+  }
+  
+  return FALSE;
+}
+
+/**
+  Search protocol database and check to see if the protocol
+  specified by ProtocolGuid is present on a ControllerHandle and opened by
+  ChildHandle with an attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  If the ControllerHandle is found, then the protocol specified by ProtocolGuid
+  will be opened on it.  
+  
+
+  @param  ProtocolGuid   ProtocolGuid pointer.
+  @param  ChildHandle    Child Handle to retrieve Parent information.
+  
+**/ 
+VOID *
+EFIAPI
+GetParentProtocol (
+  IN  EFI_GUID                          *ProtocolGuid,
+  IN  EFI_HANDLE                        ChildHandle
+  ) 
+{
+  UINTN                                 Index;
+  UINTN                                 HandleCount;
+  VOID                                  *Interface;  
+  EFI_STATUS                            Status;
+  EFI_HANDLE                            *HandleBuffer;
+
+  //
+  // Retrieve the list of all handles from the handle database
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  ProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Iterate to find who is parent handle that is opened with ProtocolGuid by ChildHandle 
+  //
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = EfiTestChildHandle (HandleBuffer[Index], ChildHandle, ProtocolGuid);
+    if (!EFI_ERROR (Status)) {
+      Status = gBS->HandleProtocol (HandleBuffer[Index], ProtocolGuid, (VOID **)&Interface);
+      if (!EFI_ERROR (Status)) {
+        gBS->FreePool (HandleBuffer);
+        return Interface;
+      }
+    }
+  }
+
+  gBS->FreePool (HandleBuffer);
+  return NULL;
+} 
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  EFI_STATUS      Status;
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*InquiryDataSize >= sizeof (ScsiDiskDevice->InquiryData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (InquiryData, &ScsiDiskDevice->InquiryData, sizeof (ScsiDiskDevice->InquiryData));
+  }
+  *InquiryDataSize = sizeof (ScsiDiskDevice->InquiryData);
+  return Status;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  EFI_STATUS      Status;
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  if (CompareGuid (&This->Interface, &gEfiDiskInfoScsiInterfaceGuid) || CompareGuid (&This->Interface, &gEfiDiskInfoUfsInterfaceGuid)) {
+    //
+    // Physical SCSI bus does not support this data class. 
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*IdentifyDataSize >= sizeof (ScsiDiskDevice->IdentifyData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (IdentifyData, &ScsiDiskDevice->IdentifyData, sizeof (ScsiDiskDevice->IdentifyData));
+  }
+  *IdentifyDataSize = sizeof (ScsiDiskDevice->IdentifyData);
+  return Status;
+}
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used by the IDE bus driver to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  SCSI_DISK_DEV   *ScsiDiskDevice;
+
+  if (CompareGuid (&This->Interface, &gEfiDiskInfoScsiInterfaceGuid) || CompareGuid (&This->Interface, &gEfiDiskInfoUfsInterfaceGuid)) {
+    //
+    // This is not an IDE physical device.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  ScsiDiskDevice  = SCSI_DISK_DEV_FROM_DISKINFO (This);
+  *IdeChannel     = ScsiDiskDevice->Channel;
+  *IdeDevice      = ScsiDiskDevice->Device;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Issues ATA IDENTIFY DEVICE command to identify ATAPI device.
+
+  This function tries to fill 512-byte ATAPI_IDENTIFY_DATA for ATAPI device to
+  implement Identify() interface for DiskInfo protocol. The ATA command is sent
+  via SCSI Request Packet.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  
+  @retval EFI_SUCCESS     The ATAPI device identify data were retrieved successfully.
+  @retval others          Some error occurred during the identification that ATAPI device.
+
+**/  
+EFI_STATUS
+AtapiIdentifyDevice (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  )
+{
+  EFI_SCSI_IO_SCSI_REQUEST_PACKET CommandPacket;
+  UINT8                           Cdb[6];
+
+  //
+  // Initialize SCSI REQUEST_PACKET and 6-byte Cdb
+  //
+  ZeroMem (&CommandPacket, sizeof (CommandPacket));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = ATA_CMD_IDENTIFY_DEVICE;
+  CommandPacket.Timeout = SCSI_DISK_TIMEOUT;
+  CommandPacket.Cdb = Cdb;
+  CommandPacket.CdbLength = (UINT8) sizeof (Cdb);
+  CommandPacket.InDataBuffer = &ScsiDiskDevice->IdentifyData;
+  CommandPacket.InTransferLength = sizeof (ScsiDiskDevice->IdentifyData);
+
+  return ScsiDiskDevice->ScsiIo->ExecuteScsiCommand (ScsiDiskDevice->ScsiIo, &CommandPacket, NULL);
+}
+
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with SCSI interface GUID. If it further
+  detects that the physical device is an ATAPI/AHCI device, it then updates interface GUID
+  to be IDE/AHCI interface GUID.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  ChildHandle     Child handle to install DiskInfo protocol.
+  
+**/  
+VOID
+InitializeInstallDiskInfo (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN  EFI_HANDLE      ChildHandle
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ChildDevicePathNode;
+  ATAPI_DEVICE_PATH         *AtapiDevicePath;
+  SATA_DEVICE_PATH          *SataDevicePath;
+  UINTN                     IdentifyRetry;
+
+  Status = gBS->HandleProtocol (ChildHandle, &gEfiDevicePathProtocolGuid, (VOID **) &DevicePathNode);
+  //
+  // Device Path protocol must be installed on the device handle. 
+  //
+  ASSERT_EFI_ERROR (Status);
+  //
+  // Copy the DiskInfo protocol template.
+  //
+  CopyMem (&ScsiDiskDevice->DiskInfo, &gScsiDiskInfoProtocolTemplate, sizeof (gScsiDiskInfoProtocolTemplate));
+
+  while (!IsDevicePathEnd (DevicePathNode)) {
+    ChildDevicePathNode = NextDevicePathNode (DevicePathNode);
+    if ((DevicePathType (DevicePathNode) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType (DevicePathNode) == HW_PCI_DP) &&
+        (DevicePathType (ChildDevicePathNode) == MESSAGING_DEVICE_PATH) &&
+       ((DevicePathSubType (ChildDevicePathNode) == MSG_ATAPI_DP) ||
+        (DevicePathSubType (ChildDevicePathNode) == MSG_SATA_DP))) {
+
+      IdentifyRetry = 3;
+      do {
+        //
+        // Issue ATA Identify Device Command via SCSI command, which is required to publish DiskInfo protocol
+        // with IDE/AHCI interface GUID.
+        //
+        Status = AtapiIdentifyDevice (ScsiDiskDevice);
+        if (!EFI_ERROR (Status)) {
+          if (DevicePathSubType(ChildDevicePathNode) == MSG_ATAPI_DP) {
+            //
+            // We find the valid ATAPI device path
+            //
+            AtapiDevicePath = (ATAPI_DEVICE_PATH *) ChildDevicePathNode;
+            ScsiDiskDevice->Channel = AtapiDevicePath->PrimarySecondary;
+            ScsiDiskDevice->Device = AtapiDevicePath->SlaveMaster;
+            //
+            // Update the DiskInfo.Interface to IDE interface GUID for the physical ATAPI device. 
+            //
+            CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoIdeInterfaceGuid);
+          } else {
+            //
+            // We find the valid SATA device path
+            //
+            SataDevicePath = (SATA_DEVICE_PATH *) ChildDevicePathNode;
+            ScsiDiskDevice->Channel = SataDevicePath->HBAPortNumber;
+            ScsiDiskDevice->Device = SataDevicePath->PortMultiplierPortNumber;
+            //
+            // Update the DiskInfo.Interface to AHCI interface GUID for the physical AHCI device. 
+            //
+            CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoAhciInterfaceGuid);
+          }
+          return;
+        }
+      } while (--IdentifyRetry > 0);
+    } else if ((DevicePathType (ChildDevicePathNode) == MESSAGING_DEVICE_PATH) &&
+       (DevicePathSubType (ChildDevicePathNode) == MSG_UFS_DP)) {
+      CopyGuid (&ScsiDiskDevice->DiskInfo.Interface, &gEfiDiskInfoUfsInterfaceGuid);
+      break;
+    }
+    DevicePathNode = ChildDevicePathNode;
+  }
+
+  return;
+}
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h
new file mode 100644
index 0000000000..2406df51b8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.h
@@ -0,0 +1,1354 @@
+/** @file
+  Header file for SCSI Disk Driver.
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _SCSI_DISK_H_
+#define _SCSI_DISK_H_
+
+
+#include <Uefi.h>
+
+
+#include <Protocol/ScsiIo.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/ScsiPassThru.h>
+#include <Protocol/DiskInfo.h>
+
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiScsiLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <IndustryStandard/Scsi.h>
+#include <IndustryStandard/Atapi.h>
+
+#define IS_DEVICE_FIXED(a)        (a)->FixedDevice ? 1 : 0
+
+#define SCSI_DISK_DEV_SIGNATURE SIGNATURE_32 ('s', 'c', 'd', 'k')
+
+typedef struct {
+  UINT32                    Signature;
+
+  EFI_HANDLE                Handle;
+
+  EFI_BLOCK_IO_PROTOCOL     BlkIo;
+  EFI_BLOCK_IO2_PROTOCOL    BlkIo2;
+  EFI_BLOCK_IO_MEDIA        BlkIoMedia;
+  EFI_SCSI_IO_PROTOCOL      *ScsiIo;
+  UINT8                     DeviceType;
+  BOOLEAN                   FixedDevice;
+  UINT16                    Reserved;
+
+  EFI_SCSI_SENSE_DATA       *SenseData;
+  UINTN                     SenseDataNumber;
+  EFI_SCSI_INQUIRY_DATA     InquiryData;
+
+  EFI_UNICODE_STRING_TABLE  *ControllerNameTable;
+
+  EFI_DISK_INFO_PROTOCOL    DiskInfo;
+
+  //
+  // The following fields are only valid for ATAPI/SATA device
+  //
+  UINT32                    Channel;
+  UINT32                    Device;
+  ATAPI_IDENTIFY_DATA       IdentifyData;
+  
+  //
+  // The flag indicates if 16-byte command can be used
+  //
+  BOOLEAN                   Cdb16Byte;
+
+  //
+  // The queue for BlockIo2 requests
+  //
+  LIST_ENTRY                BlkIo2Queue;
+} SCSI_DISK_DEV;
+
+#define SCSI_DISK_DEV_FROM_BLKIO(a)  CR (a, SCSI_DISK_DEV, BlkIo, SCSI_DISK_DEV_SIGNATURE)
+#define SCSI_DISK_DEV_FROM_BLKIO2(a)  CR (a, SCSI_DISK_DEV, BlkIo2, SCSI_DISK_DEV_SIGNATURE)
+
+#define SCSI_DISK_DEV_FROM_DISKINFO(a) CR (a, SCSI_DISK_DEV, DiskInfo, SCSI_DISK_DEV_SIGNATURE)
+
+//
+// Asynchronous I/O request
+//
+//
+// Private data structure for a BlockIo2 request
+//
+typedef struct {
+  EFI_BLOCK_IO2_TOKEN                  *Token;
+  //
+  // The flag indicates if the last Scsi Read/Write sub-task for a BlockIo2
+  // request is sent to device
+  //
+  BOOLEAN                              LastScsiRW;
+
+  //
+  // The queue for Scsi Read/Write sub-tasks of a BlockIo2 request
+  //
+  LIST_ENTRY                           ScsiRWQueue;
+
+  LIST_ENTRY                           Link;
+} SCSI_BLKIO2_REQUEST;
+
+//
+// Private data structure for a SCSI Read/Write request
+//
+typedef struct {
+  SCSI_DISK_DEV                        *ScsiDiskDevice;
+  UINT64                               Timeout;
+  EFI_SCSI_SENSE_DATA                  *SenseData;
+  UINT8                                SenseDataLength;
+  UINT8                                HostAdapterStatus;
+  UINT8                                TargetStatus;
+  UINT8                                *InBuffer;
+  UINT8                                *OutBuffer;
+  UINT32                               DataLength;
+  UINT64                               StartLba;
+  UINT32                               SectorCount;
+  UINT8                                TimesRetry;
+
+  //
+  // The BlockIo2 request this SCSI command belongs to
+  //
+  SCSI_BLKIO2_REQUEST                  *BlkIo2Req;
+
+  LIST_ENTRY                           Link;
+} SCSI_ASYNC_RW_REQUEST;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gScsiDiskDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gScsiDiskComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gScsiDiskComponentName2;
+//
+// action code used in detect media process
+//
+#define ACTION_NO_ACTION               0x00
+#define ACTION_READ_CAPACITY           0x01
+#define ACTION_RETRY_COMMAND_LATER     0x02
+#define ACTION_RETRY_WITH_BACKOFF_ALGO 0x03
+
+#define SCSI_COMMAND_VERSION_1      0x01
+#define SCSI_COMMAND_VERSION_2      0x02
+#define SCSI_COMMAND_VERSION_3      0x03
+
+//
+// SCSI Disk Timeout Experience Value
+//
+#define SCSI_DISK_TIMEOUT           EFI_TIMER_PERIOD_SECONDS (3)
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions.
+  If any other agent wishes to call Supported() it must also follow these
+  calling restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle.
+
+  This service is called by the EFI boot service ConnectController(). In order
+  to make drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these calling restrictions. If
+  any other agent wishes to call Start() it must also follow these calling
+  restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath   OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+
+  This service is called by the EFI boot service DisconnectController().
+  In order to make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController() must follow these
+  calling restrictions. If any other agent wishes to call Stop() it must
+  also follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer   OPTIONAL
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Reset SCSI Disk.
+
+
+  @param  This                 The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  ExtendedVerification The flag about if extend verificate
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is retured from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReset (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHANGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL
+  @param  MediaId    The Id of Media detected
+  @param  Lba        The logic block address
+  @param  BufferSize The size of Buffer
+  @param  Buffer     The buffer to fill the read out data
+
+  @retval EFI_SUCCESS           Successfully to read out block.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      Fail to detect media.
+  @retval EFI_NO_MEDIA          Media is not present.
+  @retval EFI_MEDIA_CHNAGED     Media has changed.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER Invalid parameter passed in.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSize,
+  IN  VOID                    *Buffer
+  );
+
+
+/**
+  Flush Block to Disk.
+
+  EFI_SUCCESS is returned directly.
+
+  @param  This              The pointer of EFI_BLOCK_IO_PROTOCOL
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This
+  );
+
+
+/**
+  Reset SCSI Disk.
+
+  @param  This                 The pointer of EFI_BLOCK_IO2_PROTOCOL.
+  @param  ExtendedVerification The flag about if extend verificate.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+  @return EFI_STATUS is returned from EFI_SCSI_IO_PROTOCOL.ResetDevice().
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskResetEx (
+  IN  EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  The function is to Read Block from SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The read request was queued if Token-> Event is
+                                not NULL. The data was read correctly from the
+                                device if theToken-> Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the read operation.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskReadBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL   *This,
+  IN     UINT32                   MediaId,
+  IN     EFI_LBA                  Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN      *Token,
+  IN     UINTN                    BufferSize,
+  OUT    VOID                     *Buffer
+  );
+
+/**
+  The function is to Write Block to SCSI Disk.
+
+  @param  This       The pointer of EFI_BLOCK_IO_PROTOCOL.
+  @param  MediaId    The Id of Media detected.
+  @param  Lba        The logic block address.
+  @param  Token      A pointer to the token associated with the transaction.
+  @param  BufferSize The size of Buffer.
+  @param  Buffer     The buffer to fill the read out data.
+
+  @retval EFI_SUCCESS           The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR      The device reported an error while attempting
+                                to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of
+                                the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not
+                                valid, or the buffer is not on proper
+                                alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskWriteBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  IN     VOID                   *Buffer
+  );
+
+/**
+  Flush the Block Device.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  Token      A pointer to the token associated with the transaction.
+
+  @retval EFI_SUCCESS         All outstanding data was written to the device.
+  @retval EFI_DEVICE_ERROR    The device reported an error while attempting to
+                              write data.
+  @retval EFI_WRITE_PROTECTED The device cannot be written to.
+  @retval EFI_NO_MEDIA        There is no media in the device.
+  @retval EFI_MEDIA_CHANGED   The MediaId is not for the current media.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskFlushBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL  *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN     *Token
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used by the IDE bus driver to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used by the IDE bus driver to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]     This               Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used by the IDE bus driver to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+/**
+  This function is used by the IDE bus driver to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+ScsiDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+
+/**
+  Detect Device and read out capacity ,if error occurs, parse the sense key.
+
+  @param  ScsiDiskDevice    The pointer of SCSI_DISK_DEV
+  @param  MustReadCapacity  The flag about reading device capacity
+  @param  MediaChange       The pointer of flag indicates if media has changed 
+
+  @retval EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskDetectMedia (
+  IN   SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN   BOOLEAN         MustReadCapacity,
+  OUT  BOOLEAN         *MediaChange
+  );
+
+/**
+  To test device.
+
+  When Test Unit Ready command succeeds, retrieve Sense Keys via Request Sense;
+  When Test Unit Ready command encounters any error caused by host adapter or
+  target, return error without retrieving Sense Keys.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates try again
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The pointer of the number of sense data array
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to test unit
+
+**/
+EFI_STATUS
+ScsiDiskTestUnitReady (
+  IN  SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT BOOLEAN               *NeedRetry,
+  OUT EFI_SCSI_SENSE_DATA   **SenseDataArray,
+  OUT UINTN                 *NumberOfSenseKeys
+  );
+
+
+/**
+  Parsing Sense Keys which got from request sense command.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NumberOfSenseKeys  The number of sense key  
+  @param  Action             The pointer of action which indicates what is need to do next
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+DetectMediaParsingSenseKeys (
+  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+  IN   EFI_SCSI_SENSE_DATA     *SenseData,
+  IN   UINTN                   NumberOfSenseKeys,
+  OUT  UINTN                   *Action
+  );
+
+
+/**
+  Send read capacity command to device and get the device parameter.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to read capacity
+
+**/
+EFI_STATUS
+ScsiDiskReadCapacity (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys
+  );
+
+/**
+  Check the HostAdapter status and re-interpret it in EFI_STATUS.
+
+  @param  HostAdapterStatus  Host Adapter status
+
+  @retval  EFI_SUCCESS       Host adapter is OK.
+  @retval  EFI_TIMEOUT       Timeout.
+  @retval  EFI_NOT_READY     Adapter NOT ready.
+  @retval  EFI_DEVICE_ERROR  Adapter device error.
+
+**/
+EFI_STATUS
+CheckHostAdapterStatus (
+  IN UINT8   HostAdapterStatus
+  );
+
+
+/**
+  Check the target status and re-interpret it in EFI_STATUS.
+
+  @param  TargetStatus  Target status
+
+  @retval EFI_NOT_READY       Device is NOT ready.
+  @retval EFI_DEVICE_ERROR 
+  @retval EFI_SUCCESS
+
+**/
+EFI_STATUS
+CheckTargetStatus (
+  IN  UINT8   TargetStatus
+  );
+
+/**
+  Retrieve all sense keys from the device.
+
+  When encountering error during the process, if retrieve sense keys before
+  error encountered, it returns the sense keys with return status set to EFI_SUCCESS,
+  and NeedRetry set to FALSE; otherwize, return the proper return status.
+
+  @param  ScsiDiskDevice     The pointer of SCSI_DISK_DEV
+  @param  NeedRetry          The pointer of flag indicates if need a retry
+  @param  SenseDataArray     The pointer of an array of sense data
+  @param  NumberOfSenseKeys  The number of sense key
+  @param  AskResetIfError    The flag indicates if need reset when error occurs
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to request sense key
+
+**/
+EFI_STATUS
+ScsiDiskRequestSenseKeys (
+  IN  OUT  SCSI_DISK_DEV           *ScsiDiskDevice,
+      OUT  BOOLEAN                 *NeedRetry,
+      OUT  EFI_SCSI_SENSE_DATA     **SenseDataArray,
+      OUT  UINTN                   *NumberOfSenseKeys,
+  IN       BOOLEAN                 AskResetIfError
+  );
+
+/**
+  Send out Inquiry command to Device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  NeedRetry       Indicates if needs try again when error happens
+
+  @retval  EFI_DEVICE_ERROR  Indicates that error occurs
+  @retval  EFI_SUCCESS       Successfully to detect media
+
+**/
+EFI_STATUS
+ScsiDiskInquiryDevice (
+  IN OUT  SCSI_DISK_DEV   *ScsiDiskDevice,
+     OUT  BOOLEAN         *NeedRetry
+  );
+
+/**
+  Parse Inquiry data.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ParseInquiryData (
+  IN OUT SCSI_DISK_DEV   *ScsiDiskDevice
+  );
+
+/**
+  Read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer to fill in the read out data
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskReadSectors (
+  IN   SCSI_DISK_DEV     *ScsiDiskDevice,
+  OUT  VOID              *Buffer,
+  IN   EFI_LBA           Lba,
+  IN   UINTN             NumberOfBlocks
+  );
+
+/**
+  Write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Buffer          The buffer of data to be written into SCSI Disk
+  @param  Lba             Logic block address
+  @param  NumberOfBlocks  The number of blocks to read
+
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskWriteSectors (
+  IN  SCSI_DISK_DEV     *ScsiDiskDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_LBA           Lba,
+  IN  UINTN             NumberOfBlocks
+  );
+
+/**
+  Asynchronously read sector from SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer to fill in the read out data.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL.
+  @retval EFI_DEVICE_ERROR       Indicates a device error.
+  @retval EFI_SUCCESS            Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncReadSectors (
+  IN   SCSI_DISK_DEV         *ScsiDiskDevice,
+  OUT  VOID                  *Buffer,
+  IN   EFI_LBA               Lba,
+  IN   UINTN                 NumberOfBlocks,
+  IN   EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Asynchronously write sector to SCSI Disk.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  Buffer          The buffer of data to be written into SCSI Disk.
+  @param  Lba             Logic block address.
+  @param  NumberOfBlocks  The number of blocks to read.
+  @param  Token           A pointer to the token associated with the
+                          non-blocking read request.
+
+  @retval EFI_INVALID_PARAMETER  Token is NULL or Token->Event is NULL
+  @retval EFI_DEVICE_ERROR  Indicates a device error.
+  @retval EFI_SUCCESS       Operation is successful.
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWriteSectors (
+  IN  SCSI_DISK_DEV          *ScsiDiskDevice,
+  IN  VOID                   *Buffer,
+  IN  EFI_LBA                Lba,
+  IN  UINTN                  NumberOfBlocks,
+  IN  EFI_BLOCK_IO2_TOKEN    *Token
+  );
+
+/**
+  Submit Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead10Command().
+**/
+EFI_STATUS
+ScsiDiskRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  );
+
+/**
+  Submit Write(10) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite10Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount
+  );
+
+/**
+  Submit Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to read
+
+  @return  EFI_STATUS is returned by calling ScsiRead16Command().
+**/
+EFI_STATUS
+ScsiDiskRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+     OUT UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  );
+  
+/**
+  Submit Write(16) Command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice
+  @param  NeedRetry          The pointer of flag indicates if needs retry if error happens
+  @param  Timeout            The time to complete the command
+  @param  DataBuffer         The buffer to fill with the read out data
+  @param  DataLength         The length of buffer
+  @param  StartLba           The start logic block address
+  @param  SectorCount        The number of blocks to write
+
+  @return  EFI_STATUS is returned by calling ScsiWrite16Command().
+
+**/
+EFI_STATUS
+ScsiDiskWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+     OUT BOOLEAN               *NeedRetry,
+  IN     UINT64                Timeout,
+  IN     UINT8                 *DataBuffer,
+  IN OUT UINT32                *DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount
+  );  
+
+/**
+  Submit Async Read(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Write(10) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite10CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite10 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT32                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Read(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer to fill with the read out data.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to read.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiRead16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncRead16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+     OUT UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Submit Async Write(16) command.
+
+  @param  ScsiDiskDevice     The pointer of ScsiDiskDevice.
+  @param  Timeout            The time to complete the command.
+  @param  TimesRetry         The number of times the command has been retried.
+  @param  DataBuffer         The buffer contains the data to write.
+  @param  DataLength         The length of buffer.
+  @param  StartLba           The start logic block address.
+  @param  SectorCount        The number of blocks to write.
+  @param  BlkIo2Req          The upstream BlockIo2 request.
+  @param  Token              The pointer to the token associated with the
+                             non-blocking read request.
+
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a
+                                lack of resources.
+  @return others                Status returned by calling
+                                ScsiWrite16CommandEx().
+
+**/
+EFI_STATUS
+ScsiDiskAsyncWrite16 (
+  IN     SCSI_DISK_DEV         *ScsiDiskDevice,
+  IN     UINT64                Timeout,
+  IN     UINT8                 TimesRetry,
+  IN     UINT8                 *DataBuffer,
+  IN     UINT32                DataLength,
+  IN     UINT64                StartLba,
+  IN     UINT32                SectorCount,
+  IN OUT SCSI_BLKIO2_REQUEST   *BlkIo2Req,
+  IN     EFI_BLOCK_IO2_TOKEN   *Token
+  );
+
+/**
+  Get information from media read capacity command.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+  @param  Capacity10      The pointer of EFI_SCSI_DISK_CAPACITY_DATA
+  @param  Capacity16      The pointer of EFI_SCSI_DISK_CAPACITY_DATA16
+**/
+VOID
+GetMediaInfo (
+  IN OUT SCSI_DISK_DEV                  *ScsiDiskDevice,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA    *Capacity10,
+  IN     EFI_SCSI_DISK_CAPACITY_DATA16  *Capacity16
+  );
+
+/**
+  Check sense key to find if media presents.
+
+  @param  SenseData   The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts The number of sense key
+
+  @retval TRUE    NOT any media
+  @retval FALSE   Media presents
+**/
+BOOLEAN
+ScsiDiskIsNoMedia (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Parse sense key.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Error
+  @retval FALSE  NOT error
+
+**/
+BOOLEAN
+ScsiDiskIsMediaError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if hardware error happens.
+
+  @param  SenseData     The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts   The number of sense key
+
+  @retval TRUE  Hardware error exits.
+  @retval FALSE NO error.
+
+**/
+BOOLEAN
+ScsiDiskIsHardwareError (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if media has changed.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE   Media is changed.
+  @retval FALSE  Medit is NOT changed.
+**/
+BOOLEAN
+ScsiDiskIsMediaChange (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if reset happens.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+
+  @retval TRUE  It is reset before.
+  @retval FALSE It is NOT reset before.
+
+**/
+BOOLEAN
+ScsiDiskIsResetBefore (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Check sense key to find if the drive is ready.
+
+  @param  SenseData    The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts  The number of sense key
+  @param  RetryLater   The flag means if need a retry 
+
+  @retval TRUE  Drive is ready.
+  @retval FALSE Drive is NOT ready.
+
+**/
+BOOLEAN
+ScsiDiskIsDriveReady (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts,
+  OUT BOOLEAN               *RetryLater
+  );
+
+/**
+  Check sense key to find if it has sense key.
+
+  @param  SenseData   - The pointer of EFI_SCSI_SENSE_DATA
+  @param  SenseCounts - The number of sense key
+
+  @retval TRUE  It has sense key.
+  @retval FALSE It has NOT any sense key.
+
+**/
+BOOLEAN
+ScsiDiskHaveSenseKey (
+  IN  EFI_SCSI_SENSE_DATA   *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**
+  Release resource about disk device.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV
+
+**/
+VOID
+ReleaseScsiDiskDeviceResources (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice
+  );
+
+/**
+  Determine if Block Io should be produced.
+  
+
+  @param  ChildHandle  Child Handle to retrieve Parent information.
+  
+  @retval  TRUE    Should produce Block Io.
+  @retval  FALSE   Should not produce Block Io.
+
+**/  
+BOOLEAN
+DetermineInstallBlockIo (
+  IN  EFI_HANDLE      ChildHandle
+  );
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with SCSI interface GUID. If it further
+  detects that the physical device is an ATAPI/AHCI device, it then updates interface GUID
+  to be IDE/AHCI interface GUID.
+
+  @param  ScsiDiskDevice  The pointer of SCSI_DISK_DEV.
+  @param  ChildHandle     Child handle to install DiskInfo protocol.
+  
+**/  
+VOID
+InitializeInstallDiskInfo (
+  IN  SCSI_DISK_DEV   *ScsiDiskDevice,
+  IN  EFI_HANDLE      ChildHandle
+  ); 
+
+/**
+  Search protocol database and check to see if the protocol
+  specified by ProtocolGuid is present on a ControllerHandle and opened by
+  ChildHandle with an attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  If the ControllerHandle is found, then the protocol specified by ProtocolGuid
+  will be opened on it.  
+  
+
+  @param  ProtocolGuid   ProtocolGuid pointer.
+  @param  ChildHandle    Child Handle to retrieve Parent information.
+  
+**/ 
+VOID *
+EFIAPI
+GetParentProtocol (
+  IN  EFI_GUID                          *ProtocolGuid,
+  IN  EFI_HANDLE                        ChildHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni
new file mode 100644
index 0000000000..1a3d294a05
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDisk.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf
new file mode 100644
index 0000000000..47539e55c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf
@@ -0,0 +1,75 @@
+## @file
+#  The Scsi Disk driver is used to retrieve the media info in the attached SCSI disk.
+#  It detects the SCSI disk media and installs Block I/O and Block I/O2 Protocol on
+#  the device handle.
+#  
+#  Copyright (c) 2006 - 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ScsiDisk
+  MODULE_UNI_FILE                = ScsiDisk.uni
+  FILE_GUID                      = 0A66E322-3740-4cce-AD62-BD172CECCA35
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeScsiDisk
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gScsiDiskDriverBinding
+#  COMPONENT_NAME                =  gScsiDiskComponentName
+#  COMPONENT_NAME2               =  gScsiDiskComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  ScsiDisk.c
+  ScsiDisk.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiScsiLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  DevicePathLib
+
+[Protocols]
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiBlockIo2ProtocolGuid                      ## BY_START
+  gEfiScsiIoProtocolGuid                        ## TO_START
+  gEfiScsiPassThruProtocolGuid                  ## TO_START
+  gEfiExtScsiPassThruProtocolGuid               ## TO_START
+
+[Guids]
+  gEfiDiskInfoScsiInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoIdeInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoAhciInterfaceGuid                 ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiDiskInfoUfsInterfaceGuid                  ## SOMETIMES_PRODUCES ## UNDEFINED
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ScsiDiskExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni
new file mode 100644
index 0000000000..0cb5610ce5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c
new file mode 100644
index 0000000000..0a76e26aa2
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.c
@@ -0,0 +1,1193 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+//
+// Template for UFS HC Peim Private Data.
+//
+UFS_PEIM_HC_PRIVATE_DATA   gUfsHcPeimTemplate = {
+  UFS_PEIM_HC_SIG,                // Signature
+  NULL,                           // Controller
+  NULL,                           // Pool
+  {                               // BlkIoPpi
+    UfsBlockIoPeimGetDeviceNo,
+    UfsBlockIoPeimGetMediaInfo,
+    UfsBlockIoPeimReadBlocks
+  },
+  {                               // BlkIo2Ppi
+    EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION,
+    UfsBlockIoPeimGetDeviceNo2,
+    UfsBlockIoPeimGetMediaInfo2,
+    UfsBlockIoPeimReadBlocks2
+  },
+  {                               // BlkIoPpiList
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gEfiPeiVirtualBlockIoPpiGuid,
+    NULL
+  },
+  {                               // BlkIo2PpiList
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiVirtualBlockIo2PpiGuid,
+    NULL
+  },
+  {                               // Media
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    },
+    {
+      MSG_UFS_DP,
+      FALSE,
+      TRUE,
+      FALSE,
+      0x1000,
+      0
+    }
+  },
+  0,                              // UfsHcBase
+  0,                              // Capabilities
+  0,                              // TaskTag
+  0,                              // UtpTrlBase
+  0,                              // Nutrs
+  0,                              // UtpTmrlBase
+  0,                              // Nutmrs
+  {                               // Luns
+    {
+      UFS_LUN_0,                      // Ufs Common Lun 0
+      UFS_LUN_1,                      // Ufs Common Lun 1
+      UFS_LUN_2,                      // Ufs Common Lun 2
+      UFS_LUN_3,                      // Ufs Common Lun 3
+      UFS_LUN_4,                      // Ufs Common Lun 4
+      UFS_LUN_5,                      // Ufs Common Lun 5
+      UFS_LUN_6,                      // Ufs Common Lun 6
+      UFS_LUN_7,                      // Ufs Common Lun 7
+    },
+    0x0000,                           // By default exposing all Luns.
+    0x0
+  }
+};
+
+/**
+  Execute Request Sense SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to output sense data.
+  @param[out] DataBufferLength     The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRequestSense (
+  IN     UFS_PEIM_HC_PRIVATE_DATA        *Private,
+  IN     UINTN                           Lun,  
+     OUT VOID                            *DataBuffer,
+     OUT UINT32                          *DataBufferLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET                Packet;
+  UINT8                                  Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                             Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_REQUEST_SENSE;
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.DataDirection    = UfsDataIn;
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataBufferLength;
+  Packet.SenseData        = NULL;
+  Packet.SenseDataLength  = 0;
+
+  Status = UfsExecScsiCmds (Private,(UINT8)Lun, &Packet);
+
+  if (!EFI_ERROR (Status)) {
+    *DataBufferLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute TEST UNITY READY SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimTestUnitReady (
+  IN     UFS_PEIM_HC_PRIVATE_DATA        *Private,
+  IN     UINTN                           Lun,  
+     OUT VOID                            *SenseData,  OPTIONAL
+     OUT UINT8                           *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET                Packet;
+  UINT8                                  Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                             Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_TEST_UNIT_READY;
+
+  Packet.Timeout         = UFS_TIMEOUT;
+  Packet.Cdb             = Cdb;
+  Packet.CdbLength       = sizeof (Cdb);
+  Packet.DataDirection   = UfsNoData;
+  Packet.SenseData       = SenseData;
+  Packet.SenseDataLength = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private,(UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute INQUIRY SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] Inquiry              A pointer to Inquiry data buffer.
+  @param[out] InquiryLengths       The length of output Inquiry data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimInquiry (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *Inquiry,
+     OUT UINT32                       *InquiryLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIX];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_INQUIRY;
+  Cdb[4]  = sizeof (EFI_SCSI_INQUIRY_DATA);
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = Inquiry;
+  Packet.InTransferLength = *InquiryLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *InquiryLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY(10) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to READ_CAPACITY data buffer.
+  @param[out] DataLength           The length of output READ_CAPACITY data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimReadCapacity (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_TEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ_CAPACITY;
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY(16) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[out] DataBuffer           A pointer to READ_CAPACITY data buffer.
+  @param[out] DataLength           The length of output READ_CAPACITY data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimReadCapacity16 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIXTEEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0]  = EFI_SCSI_OP_READ_CAPACITY16;
+  Cdb[1]  = 0x10;          // Service Action should be 0x10 for UFS device.
+  Cdb[13] = 0x20;          // The maximum number of bytes for returned data.
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ (10) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[in]  StartLba             The start LBA.
+  @param[in]  SectorNum            The sector number to be read.
+  @param[out] DataBuffer           A pointer to data buffer.
+  @param[out] DataLength           The length of output data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRead10 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+  IN     UINTN                        StartLba,
+  IN     UINT32                       SectorNum,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_TEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ10;
+  WriteUnaligned32 ((UINT32 *)&Cdb[2], SwapBytes32 ((UINT32) StartLba));
+  WriteUnaligned16 ((UINT16 *)&Cdb[7], SwapBytes16 ((UINT16) SectorNum));
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Execute READ (16) SCSI command on a specific UFS device.
+
+  @param[in]  Private              A pointer to UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun                  The lun on which the SCSI cmd executed.
+  @param[in]  StartLba             The start LBA.
+  @param[in]  SectorNum            The sector number to be read.
+  @param[out] DataBuffer           A pointer to data buffer.
+  @param[out] DataLength           The length of output data.
+  @param[out] SenseData            A pointer to output sense data.
+  @param[out] SenseDataLength      The length of output sense data.
+
+  @retval EFI_SUCCESS              The command executed successfully.
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to send SCSI Request Packet.
+  @retval EFI_TIMEOUT              A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsPeimRead16 (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINTN                        Lun,
+  IN     UINTN                        StartLba,
+  IN     UINT32                       SectorNum,
+     OUT VOID                         *DataBuffer,
+     OUT UINT32                       *DataLength,
+     OUT VOID                         *SenseData,  OPTIONAL
+     OUT UINT8                        *SenseDataLength
+  )  
+{
+  UFS_SCSI_REQUEST_PACKET             Packet;
+  UINT8                               Cdb[UFS_SCSI_OP_LENGTH_SIXTEEN];
+  EFI_STATUS                          Status;
+
+  ZeroMem (&Packet, sizeof (UFS_SCSI_REQUEST_PACKET));
+  ZeroMem (Cdb, sizeof (Cdb));
+
+  Cdb[0] = EFI_SCSI_OP_READ16;
+  WriteUnaligned64 ((UINT64 *)&Cdb[2], SwapBytes64 (StartLba));
+  WriteUnaligned32 ((UINT32 *)&Cdb[10], SwapBytes32 (SectorNum));
+
+  Packet.Timeout          = UFS_TIMEOUT;
+  Packet.Cdb              = Cdb;
+  Packet.CdbLength        = sizeof (Cdb);
+  Packet.InDataBuffer     = DataBuffer;
+  Packet.InTransferLength = *DataLength;
+  Packet.DataDirection    = UfsDataIn;
+  Packet.SenseData        = SenseData;
+  Packet.SenseDataLength  = *SenseDataLength;
+
+  Status = UfsExecScsiCmds (Private, (UINT8)Lun, &Packet);
+
+  if (*SenseDataLength != 0) {
+    *SenseDataLength = Packet.SenseDataLength;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    *DataLength = Packet.InTransferLength;
+  }
+
+  return Status;
+}
+
+/**
+  Parsing Sense Keys from sense data.
+
+  @param  Media              The pointer of EFI_PEI_BLOCK_IO_MEDIA
+  @param  SenseData          The pointer of EFI_SCSI_SENSE_DATA
+  @param  NeedRetry          The pointer of action which indicates what is need to retry
+
+  @retval EFI_DEVICE_ERROR   Indicates that error occurs
+  @retval EFI_SUCCESS        Successfully to complete the parsing
+
+**/
+EFI_STATUS
+UfsPeimParsingSenseKeys (
+  IN     EFI_PEI_BLOCK_IO2_MEDIA   *Media,
+  IN     EFI_SCSI_SENSE_DATA       *SenseData,
+     OUT BOOLEAN                   *NeedRetry
+  )
+{
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_NO_MEDIA)) {
+    Media->MediaPresent = FALSE;
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Is No Media\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_CHANGE)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Is Media Change\n"));
+    return EFI_SUCCESS;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_UNIT_ATTENTION) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_RESET)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Was Reset Before\n"));
+    return EFI_SUCCESS;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_MEDIUM_ERROR) ||
+      ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_MEDIA_UPSIDE_DOWN))) {
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Media Error\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (SenseData->Sense_Key == EFI_SCSI_SK_HARDWARE_ERROR) {
+    *NeedRetry = FALSE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Hardware Error\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((SenseData->Sense_Key == EFI_SCSI_SK_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code == EFI_SCSI_ASC_NOT_READY) &&
+      (SenseData->Addnl_Sense_Code_Qualifier == EFI_SCSI_ASCQ_IN_PROGRESS)) {
+    *NeedRetry = TRUE;
+    DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Was Reset Before\n"));
+    return EFI_SUCCESS;
+  }
+
+  *NeedRetry = FALSE;
+  DEBUG ((EFI_D_VERBOSE, "UfsBlockIoPei: Sense Key = 0x%x ASC = 0x%x!\n", SenseData->Sense_Key, SenseData->Addnl_Sense_Code));
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  OUT UINTN                          *NumberBlockDevices
+  )
+{
+  //
+  // For Ufs device, it has up to 8 normal Luns plus some well-known Luns.
+  // At PEI phase, we will only expose normal Luns to user.
+  // For those disabled Lun, when user try to access it, the operation would fail.
+  //
+  *NumberBlockDevices = UFS_PEIM_MAX_LUNS;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA         *MediaInfo
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_SCSI_SENSE_DATA                SenseData;
+  UINT8                              SenseDataLength;
+  EFI_SCSI_DISK_CAPACITY_DATA        Capacity;
+  EFI_SCSI_DISK_CAPACITY_DATA16      Capacity16;
+  UINTN                              DataLength;
+  BOOLEAN                            NeedRetry;  
+
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS (This);
+  NeedRetry = TRUE;
+
+  if (DeviceIndex >= UFS_PEIM_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Private->Luns.BitMask & (BIT0 << DeviceIndex)) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  ZeroMem (&SenseData, sizeof (SenseData));
+  ZeroMem (&Capacity, sizeof (Capacity));
+  ZeroMem (&Capacity16, sizeof (Capacity16));
+  SenseDataLength = sizeof (SenseData);
+  //
+  // First test unit ready
+  //
+  do {
+    Status = UfsPeimTestUnitReady (
+               Private,
+               DeviceIndex,
+               &SenseData,
+               &SenseDataLength
+               );
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  
+    if (SenseDataLength == 0) {
+      continue;
+    }
+
+    Status = UfsPeimParsingSenseKeys (&(Private->Media[DeviceIndex]), &SenseData, &NeedRetry);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } while (NeedRetry);
+
+  DataLength      = sizeof (EFI_SCSI_DISK_CAPACITY_DATA);
+  SenseDataLength = 0;
+  Status = UfsPeimReadCapacity (Private, DeviceIndex, &Capacity, (UINT32 *)&DataLength, NULL, &SenseDataLength);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((Capacity.LastLba3 == 0xff) && (Capacity.LastLba2 == 0xff) &&
+      (Capacity.LastLba1 == 0xff) && (Capacity.LastLba0 == 0xff)) {
+    DataLength      = sizeof (EFI_SCSI_DISK_CAPACITY_DATA16);
+    SenseDataLength = 0;
+    Status = UfsPeimReadCapacity16 (Private, DeviceIndex, &Capacity16, (UINT32 *)&DataLength, NULL, &SenseDataLength);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    Private->Media[DeviceIndex].LastBlock  = (Capacity16.LastLba3 << 24) | (Capacity16.LastLba2 << 16) | (Capacity16.LastLba1 << 8) | Capacity16.LastLba0;
+    Private->Media[DeviceIndex].LastBlock |= ((UINT64)Capacity16.LastLba7 << 56) | ((UINT64)Capacity16.LastLba6 << 48) | ((UINT64)Capacity16.LastLba5 << 40) | ((UINT64)Capacity16.LastLba4 << 32);
+    Private->Media[DeviceIndex].BlockSize  = (Capacity16.BlockSize3 << 24) | (Capacity16.BlockSize2 << 16) | (Capacity16.BlockSize1 << 8) | Capacity16.BlockSize0;
+  } else {
+    Private->Media[DeviceIndex].LastBlock  = (Capacity.LastLba3 << 24) | (Capacity.LastLba2 << 16) | (Capacity.LastLba1 << 8) | Capacity.LastLba0;
+    Private->Media[DeviceIndex].BlockSize  = (Capacity.BlockSize3 << 24) | (Capacity.BlockSize2 << 16) | (Capacity.BlockSize1 << 8) | Capacity.BlockSize0;
+  }
+
+  MediaInfo->DeviceType   = UfsDevice;
+  MediaInfo->MediaPresent = Private->Media[DeviceIndex].MediaPresent;
+  MediaInfo->LastBlock    = (UINTN)Private->Media[DeviceIndex].LastBlock;
+  MediaInfo->BlockSize    = Private->Media[DeviceIndex].BlockSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  )
+{
+  EFI_STATUS                         Status;
+  UINTN                              BlockSize;
+  UINTN                              NumberOfBlocks;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_SCSI_SENSE_DATA                SenseData;
+  UINT8                              SenseDataLength;
+  BOOLEAN                            NeedRetry;  
+
+  Status    = EFI_SUCCESS;
+  NeedRetry = TRUE;
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS (This);
+
+  ZeroMem (&SenseData, sizeof (SenseData));
+  SenseDataLength = sizeof (SenseData);
+
+  //
+  // Check parameters
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (DeviceIndex >= UFS_PEIM_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Private->Luns.BitMask & (BIT0 << DeviceIndex)) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  BlockSize = Private->Media[DeviceIndex].BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (StartLBA > Private->Media[DeviceIndex].LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  NumberOfBlocks = BufferSize / BlockSize;
+
+  do {
+    Status = UfsPeimTestUnitReady (
+               Private,
+               DeviceIndex,
+               &SenseData,
+               &SenseDataLength
+               );
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  
+    if (SenseDataLength == 0) {
+      continue;
+    }
+
+    Status = UfsPeimParsingSenseKeys (&(Private->Media[DeviceIndex]), &SenseData, &NeedRetry);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } while (NeedRetry);
+
+  SenseDataLength = 0;
+  if (Private->Media[DeviceIndex].LastBlock < 0xfffffffful) {
+    Status = UfsPeimRead10 (
+               Private,
+               DeviceIndex,
+               (UINT32)StartLBA,
+               (UINT32)NumberOfBlocks,
+               Buffer,
+               (UINT32 *)&BufferSize,
+               NULL,
+               &SenseDataLength
+               );
+  } else {
+    Status = UfsPeimRead16 (
+               Private,
+               DeviceIndex,
+               (UINT32)StartLBA,
+               (UINT32)NumberOfBlocks,
+               Buffer,
+               (UINT32 *)&BufferSize,
+               NULL,
+               &SenseDataLength
+               );
+  }
+  return Status;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  OUT UINTN                          *NumberBlockDevices
+  )
+{
+  //
+  // For Ufs device, it has up to 8 normal Luns plus some well-known Luns.
+  // At PEI phase, we will only expose normal Luns to user.
+  // For those disabled Lun, when user try to access it, the operation would fail.
+  //
+  *NumberBlockDevices = UFS_PEIM_MAX_LUNS;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA        *MediaInfo
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private;
+  EFI_PEI_BLOCK_IO_MEDIA             Media;
+
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2 (This);
+  
+  Status    = UfsBlockIoPeimGetMediaInfo (
+                PeiServices,
+                &Private->BlkIoPpi,
+                DeviceIndex,
+                &Media
+                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (MediaInfo, &(Private->Media[DeviceIndex]), sizeof (EFI_PEI_BLOCK_IO2_MEDIA));
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  )
+{
+  EFI_STATUS                         Status;
+  UFS_PEIM_HC_PRIVATE_DATA           *Private; 
+
+  Status    = EFI_SUCCESS;
+  Private   = GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2 (This);
+
+  Status  = UfsBlockIoPeimReadBlocks (
+              PeiServices,
+              &Private->BlkIoPpi,
+              DeviceIndex,
+              StartLBA,
+              BufferSize,
+              Buffer
+              );
+  return Status;
+}
+
+/**
+  The user code starts with this function.
+  
+  @param  FileHandle             Handle of the file being invoked.
+  @param  PeiServices            Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            The driver is successfully initialized.
+  @retval Others                 Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsBlockIoPeim (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                    Status;
+  UFS_PEIM_HC_PRIVATE_DATA      *Private;
+  EDKII_UFS_HOST_CONTROLLER_PPI *UfsHcPpi;
+  UINT32                        Index;
+  UFS_CONFIG_DESC               Config;
+  UINTN                         MmioBase;
+  UINT8                         Controller;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // locate ufs host controller PPI
+  //
+  Status = PeiServicesLocatePpi (
+             &gEdkiiPeiUfsHostControllerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &UfsHcPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Controller = 0;
+  MmioBase   = 0;
+  while (TRUE) {
+    Status = UfsHcPpi->GetUfsHcMmioBar (UfsHcPpi, Controller, &MmioBase);
+    //
+    // When status is error, meant no controller is found
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    Private = AllocateCopyPool (sizeof (UFS_PEIM_HC_PRIVATE_DATA), &gUfsHcPeimTemplate);
+    if (Private == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+
+    Private->BlkIoPpiList.Ppi  = &Private->BlkIoPpi;
+    Private->BlkIo2PpiList.Ppi = &Private->BlkIo2Ppi;
+    Private->UfsHcBase         = MmioBase;
+
+    //
+    // Initialize the memory pool which will be used in all transactions.
+    //
+    Status = UfsPeimInitMemPool (Private);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+
+    //
+    // Initialize UFS Host Controller H/W.
+    //
+    Status = UfsControllerInit (Private);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UfsDevicePei: Host Controller Initialization Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // UFS 2.0 spec Section 13.1.3.3:
+    // At the end of the UFS Interconnect Layer initialization on both host and device side, 
+    // the host shall send a NOP OUT UPIU to verify that the device UTP Layer is ready. 
+    //
+    Status = UfsExecNopCmds (Private);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Sending NOP IN command Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // The host enables the device initialization completion by setting fDeviceInit flag.
+    //
+    Status = UfsSetFlag (Private, UfsFlagDevInit);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Set fDeviceInit Flag Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    //
+    // Get Ufs Device's Lun Info by reading Configuration Descriptor.
+    //
+    Status = UfsRwDeviceDesc (Private, TRUE, UfsConfigDesc, 0, 0, &Config, sizeof (UFS_CONFIG_DESC));
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ufs Get Configuration Descriptor Error, Status = %r\n", Status));
+      Controller++;
+      continue;
+    }
+
+    for (Index = 0; Index < UFS_PEIM_MAX_LUNS; Index++) {
+      if (Config.UnitDescConfParams[Index].LunEn != 0) {
+        Private->Luns.BitMask |= (BIT0 << Index);
+        DEBUG ((EFI_D_INFO, "Ufs %d Lun %d is enabled\n", Controller, Index));
+      }
+    }
+    
+    Status = PeiServicesInstallPpi (&Private->BlkIoPpiList);
+    Controller++;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h
new file mode 100644
index 0000000000..46e9bfe03f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.h
@@ -0,0 +1,560 @@
+/** @file
+
+  Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _UFS_BLOCK_IO_PEI_H_
+#define _UFS_BLOCK_IO_PEI_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UfsHostController.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PeiServicesLib.h>
+
+#include <IndustryStandard/Scsi.h>
+
+#include "UfsHci.h"
+#include "UfsHcMem.h"
+
+#define UFS_PEIM_HC_SIG             SIGNATURE_32 ('U', 'F', 'S', 'H')
+
+#define UFS_PEIM_MAX_LUNS           8
+
+typedef struct {
+  UINT8    Lun[UFS_PEIM_MAX_LUNS];
+  UINT16   BitMask:12;              // Bit 0~7 is for common luns. Bit 8~11 is reserved for those well known luns
+  UINT16   Rsvd:4;
+} UFS_PEIM_EXPOSED_LUNS;
+
+typedef struct {
+  ///
+  /// The timeout, in 100 ns units, to use for the execution of this SCSI
+  /// Request Packet. A Timeout value of 0 means that this function
+  /// will wait indefinitely for the SCSI Request Packet to execute. If
+  /// Timeout is greater than zero, then this function will return
+  /// EFI_TIMEOUT if the time required to execute the SCSI
+  /// Request Packet is greater than Timeout.
+  ///
+  UINT64 Timeout;
+  ///
+  /// A pointer to the data buffer to transfer between the SCSI
+  /// controller and the SCSI device for read and bidirectional commands.
+  ///
+  VOID   *InDataBuffer;
+  ///
+  /// A pointer to the data buffer to transfer between the SCSI
+  /// controller and the SCSI device for write or bidirectional commands.
+  ///
+  VOID   *OutDataBuffer;
+  ///
+  /// A pointer to the sense data that was generated by the execution of
+  /// the SCSI Request Packet.
+  ///
+  VOID   *SenseData;
+  ///
+  /// A pointer to buffer that contains the Command Data Block to
+  /// send to the SCSI device specified by Target and Lun.
+  ///
+  VOID   *Cdb;
+  ///
+  /// On Input, the size, in bytes, of InDataBuffer. On output, the
+  /// number of bytes transferred between the SCSI controller and the SCSI device.
+  ///
+  UINT32 InTransferLength;
+  ///
+  /// On Input, the size, in bytes of OutDataBuffer. On Output, the
+  /// Number of bytes transferred between SCSI Controller and the SCSI device.
+  ///
+  UINT32 OutTransferLength;
+  ///
+  /// The length, in bytes, of the buffer Cdb. The standard values are 6,
+  /// 10, 12, and 16, but other values are possible if a variable length CDB is used.
+  ///
+  UINT8  CdbLength;
+  ///
+  /// The direction of the data transfer. 0 for reads, 1 for writes. A
+  /// value of 2 is Reserved for Bi-Directional SCSI commands.
+  ///
+  UINT8  DataDirection;
+  ///
+  /// On input, the length in bytes of the SenseData buffer. On
+  /// output, the number of bytes written to the SenseData buffer.
+  ///
+  UINT8  SenseDataLength;
+} UFS_SCSI_REQUEST_PACKET;
+
+typedef struct _UFS_PEIM_HC_PRIVATE_DATA {  
+  UINT32                            Signature;
+  EFI_HANDLE                        Controller;
+
+  UFS_PEIM_MEM_POOL                 *Pool;
+
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI     BlkIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI    BlkIo2Ppi;
+  EFI_PEI_PPI_DESCRIPTOR            BlkIoPpiList;
+  EFI_PEI_PPI_DESCRIPTOR            BlkIo2PpiList;
+  EFI_PEI_BLOCK_IO2_MEDIA           Media[UFS_PEIM_MAX_LUNS];
+
+  UINTN                             UfsHcBase;
+  UINT32                            Capabilities;
+
+  UINT8                             TaskTag;
+
+  VOID                              *UtpTrlBase;
+  UINT8                             Nutrs;
+  VOID                              *UtpTmrlBase;
+  UINT8                             Nutmrs;
+
+  UFS_PEIM_EXPOSED_LUNS             Luns;
+} UFS_PEIM_HC_PRIVATE_DATA;
+
+#define UFS_TIMEOUT                 MultU64x32((UINT64)(3), 10000000)
+
+#define ROUNDUP8(x) (((x) % 8 == 0) ? (x) : ((x) / 8 + 1) * 8)
+
+#define IS_ALIGNED(addr, size)      (((UINTN) (addr) & (size - 1)) == 0)
+
+#define GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS(a) CR (a, UFS_PEIM_HC_PRIVATE_DATA, BlkIoPpi, UFS_PEIM_HC_SIG)
+#define GET_UFS_PEIM_HC_PRIVATE_DATA_FROM_THIS2(a) CR (a, UFS_PEIM_HC_PRIVATE_DATA, BlkIo2Ppi, UFS_PEIM_HC_SIG)
+
+#define UFS_SCSI_OP_LENGTH_SIX      0x6
+#define UFS_SCSI_OP_LENGTH_TEN      0xa
+#define UFS_SCSI_OP_LENGTH_SIXTEEN  0x10
+
+typedef struct _UFS_DEVICE_MANAGEMENT_REQUEST_PACKET {
+  UINT64                            Timeout;
+  VOID                              *InDataBuffer;
+  VOID                              *OutDataBuffer;
+  UINT8                             Opcode;
+  UINT8                             DescId;
+  UINT8                             Index;
+  UINT8                             Selector;
+  UINT32                            InTransferLength;
+  UINT32                            OutTransferLength;
+  UINT8                             DataDirection;
+  UINT8                             Ocs;
+} UFS_DEVICE_MANAGEMENT_REQUEST_PACKET;
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN     UINT8                         Lun,
+  IN OUT UFS_SCSI_REQUEST_PACKET       *Packet
+  );
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  );
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  );
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  OUT UINTN                          *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA         *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI  *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          The operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetDeviceNo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  OUT UINTN                          *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @par Note: 
+      The MediaInfo structure describes an enumeration of possible block device 
+      types.  This enumeration exists because no device paths are actually passed 
+      across interfaces that describe the type or class of hardware that is publishing 
+      the block I/O interface. This enumeration will allow for policy decisions
+      in the Recovery PEIM, such as "Try to recover from legacy floppy first, 
+      LS-120 second, CD-ROM third." If there are multiple partitions abstracted 
+      by a given device type, they should be reported in ascending order; this 
+      order also applies to nested partitions, such as legacy MBR, where the 
+      outermost partitions would have precedence in the reporting order. The 
+      same logic applies to systems such as IDE that have precedence relationships 
+      like "Master/Slave" or "Primary/Secondary". The master device should be 
+      reported first, the slave second.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA        *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsBlockIoPeimReadBlocks2 (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI *This,
+  IN  UINTN                          DeviceIndex,
+  IN  EFI_PEI_LBA                    StartLBA,
+  IN  UINTN                          BufferSize,
+  OUT VOID                           *Buffer
+  );
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Private               The Ufs Peim driver private data.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval Others                Fail to init the memory pool.
+
+**/
+EFI_STATUS
+UfsPeimInitMemPool (
+  IN  UFS_PEIM_HC_PRIVATE_DATA      *Private
+  );
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UfsPeimAllocateMem (
+  IN  UFS_PEIM_MEM_POOL        *Pool,
+  IN  UINTN                    Size
+  );
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UfsPeimFreeMem (
+  IN UFS_PEIM_MEM_POOL    *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf
new file mode 100644
index 0000000000..80fe0392b9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf
@@ -0,0 +1,62 @@
+## @file
+# Description file for the Universal Flash Storage (UFS) Peim driver.
+#
+# Copyright (c) 2014 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsBlockIoPei
+  MODULE_UNI_FILE                = UfsBlockIoPei.uni
+  FILE_GUID                      = BE189D38-C963-41CF-B695-D90E9E545A13
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 0.9
+
+  ENTRY_POINT                    = InitializeUfsBlockIoPeim
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UfsBlockIoPei.c
+  UfsBlockIoPei.h
+  UfsHci.c
+  UfsHci.h
+  UfsHcMem.c
+  UfsHcMem.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  IoLib
+  TimerLib
+  BaseMemoryLib
+  PeimEntryPoint
+  PeiServicesLib
+  DebugLib
+
+[Ppis]
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+  gEdkiiPeiUfsHostControllerPpiGuid             ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEdkiiPeiUfsHostControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsBlockIoPeiExtra.uni
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni
new file mode 100644
index 0000000000..23b6e633b9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni
new file mode 100644
index 0000000000..706fcdc8f4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c
new file mode 100644
index 0000000000..cc6c3d4e3e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.c
@@ -0,0 +1,455 @@
+/** @file
+
+Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+/**
+  Allocate a block of memory to be used by the buffer pool.
+
+  @param  Pages          How many pages to allocate.
+
+  @return The allocated memory block or NULL if failed.
+
+**/
+UFS_PEIM_MEM_BLOCK *
+UfsPeimAllocMemBlock (
+  IN  UINTN                    Pages
+  )
+{
+  UFS_PEIM_MEM_BLOCK           *Block;
+  EFI_STATUS                   Status;
+  VOID                         *TempPtr;
+  EFI_PHYSICAL_ADDRESS         Address;
+
+  TempPtr = NULL;
+  Block   = NULL;
+
+  Status = PeiServicesAllocatePool (sizeof(UFS_PEIM_MEM_BLOCK), &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, sizeof(UFS_PEIM_MEM_BLOCK));
+  
+  //
+  // each bit in the bit array represents UFS_PEIM_MEM_UNIT
+  // bytes of memory in the memory block.
+  //
+  ASSERT (UFS_PEIM_MEM_UNIT * 8 <= EFI_PAGE_SIZE);
+  
+  Block = (UFS_PEIM_MEM_BLOCK*)(UINTN)TempPtr;
+  Block->BufLen   = EFI_PAGES_TO_SIZE (Pages);
+  Block->BitsLen  = Block->BufLen / (UFS_PEIM_MEM_UNIT * 8);
+  
+  Status = PeiServicesAllocatePool (Block->BitsLen, &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, Block->BitsLen);
+
+  Block->Bits = (UINT8*)(UINTN)TempPtr;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             Pages,
+             &Address
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Address, EFI_PAGES_TO_SIZE (Pages));
+
+  Block->Buf  = (UINT8*)((UINTN)Address);
+  Block->Next = NULL;
+
+  return Block;
+}
+
+/**
+  Free the memory block from the memory pool.
+
+  @param  Pool           The memory pool to free the block from.
+  @param  Block          The memory block to free.
+
+**/
+VOID
+UfsPeimFreeMemBlock (
+  IN UFS_PEIM_MEM_POOL       *Pool,
+  IN UFS_PEIM_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Pool != NULL) && (Block != NULL));
+}
+
+/**
+  Alloc some memory from the block.
+
+  @param  Block          The memory block to allocate memory from.
+  @param  Units          Number of memory units to allocate.
+
+  @return The pointer to the allocated memory. If couldn't allocate the needed memory,
+          the return value is NULL.
+
+**/
+VOID *
+UfsPeimAllocMemFromBlock (
+  IN  UFS_PEIM_MEM_BLOCK  *Block,
+  IN  UINTN               Units
+  )
+{
+  UINTN                   Byte;
+  UINT8                   Bit;
+  UINTN                   StartByte;
+  UINT8                   StartBit;
+  UINTN                   Available;
+  UINTN                   Count;
+
+  ASSERT ((Block != 0) && (Units != 0));
+
+  StartByte  = 0;
+  StartBit   = 0;
+  Available  = 0;
+
+  for (Byte = 0, Bit = 0; Byte < Block->BitsLen;) {
+    //
+    // If current bit is zero, the corresponding memory unit is
+    // available, otherwise we need to restart our searching.
+    // Available counts the consective number of zero bit.
+    //
+    if (!UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit)) {
+      Available++;
+
+      if (Available >= Units) {
+        break;
+      }
+
+      UFS_PEIM_NEXT_BIT (Byte, Bit);
+
+    } else {
+      UFS_PEIM_NEXT_BIT (Byte, Bit);
+
+      Available  = 0;
+      StartByte  = Byte;
+      StartBit   = Bit;
+    }
+  }
+
+  if (Available < Units) {
+    return NULL;
+  }
+  
+  //
+  // Mark the memory as allocated
+  //
+  Byte  = StartByte;
+  Bit   = StartBit;
+
+  for (Count = 0; Count < Units; Count++) {
+    ASSERT (!UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+    Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] | (UINT8) UFS_PEIM_MEM_BIT (Bit));
+    UFS_PEIM_NEXT_BIT (Byte, Bit);
+  }
+
+  return Block->Buf + (StartByte * 8 + StartBit) * UFS_PEIM_MEM_UNIT;
+}
+
+/**
+  Insert the memory block to the pool's list of the blocks.
+
+  @param  Head           The head of the memory pool's block list.
+  @param  Block          The memory block to insert.
+
+**/
+VOID
+UfsPeimInsertMemBlockToPool (
+  IN UFS_PEIM_MEM_BLOCK      *Head,
+  IN UFS_PEIM_MEM_BLOCK      *Block
+  )
+{
+  ASSERT ((Head != NULL) && (Block != NULL));
+  Block->Next = Head->Next;
+  Head->Next  = Block;
+}
+
+/**
+  Is the memory block empty?
+
+  @param  Block   The memory block to check.
+
+  @retval TRUE    The memory block is empty.
+  @retval FALSE   The memory block isn't empty.
+
+**/
+BOOLEAN
+UfsPeimIsMemBlockEmpty (
+  IN UFS_PEIM_MEM_BLOCK     *Block
+  )
+{
+  UINTN                   Index;
+
+  
+  for (Index = 0; Index < Block->BitsLen; Index++) {
+    if (Block->Bits[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UfsPeimUnlinkMemBlock (
+  IN UFS_PEIM_MEM_BLOCK      *Head,
+  IN UFS_PEIM_MEM_BLOCK      *BlockToUnlink
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
+  Initialize the memory management pool for the host controller.
+  
+  @param  Private               The Ufs Peim driver private data.
+
+  @retval EFI_SUCCESS           The memory pool is initialized.
+  @retval Others                Fail to init the memory pool.
+
+**/
+EFI_STATUS
+UfsPeimInitMemPool (
+  IN  UFS_PEIM_HC_PRIVATE_DATA  *Private
+  )
+{
+  UFS_PEIM_MEM_POOL          *Pool;
+  EFI_STATUS                 Status;
+  VOID                       *TempPtr;
+
+  TempPtr = NULL;
+  Pool    = NULL;
+  
+  Status = PeiServicesAllocatePool (sizeof (UFS_PEIM_MEM_POOL), &TempPtr);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ZeroMem ((VOID*)(UINTN)TempPtr, sizeof (UFS_PEIM_MEM_POOL));
+
+  Pool = (UFS_PEIM_MEM_POOL *)((UINTN)TempPtr);
+
+  Pool->Head = UfsPeimAllocMemBlock (UFS_PEIM_MEM_DEFAULT_PAGES);
+
+  if (Pool->Head == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Pool = Pool;
+  return EFI_SUCCESS;
+}
+
+/**
+  Release the memory management pool.
+  
+  @param  Pool                  The memory pool to free.
+
+  @retval EFI_DEVICE_ERROR      Fail to free the memory pool.
+  @retval EFI_SUCCESS           The memory pool is freed.
+
+**/
+EFI_STATUS
+UfsPeimFreeMemPool (
+  IN UFS_PEIM_MEM_POOL       *Pool
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Block;
+
+  ASSERT (Pool->Head != NULL);
+
+  //
+  // Unlink all the memory blocks from the pool, then free them.
+  // UfsPeimUnlinkMemBlock can't be used to unlink and free the
+  // first block.
+  //
+  for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
+    UfsPeimFreeMemBlock (Pool, Block);
+  }
+
+  UfsPeimFreeMemBlock (Pool, Pool->Head);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate some memory from the host controller's memory pool
+  which can be used to communicate with host controller.
+  
+  @param  Pool      The host controller's memory pool.
+  @param  Size      Size of the memory to allocate.
+
+  @return The allocated memory or NULL.
+
+**/
+VOID *
+UfsPeimAllocateMem (
+  IN  UFS_PEIM_MEM_POOL        *Pool,
+  IN  UINTN                    Size
+  )
+{
+  UFS_PEIM_MEM_BLOCK         *Head;
+  UFS_PEIM_MEM_BLOCK         *Block;
+  UFS_PEIM_MEM_BLOCK         *NewBlock;
+  VOID                       *Mem;
+  UINTN                      AllocSize;
+  UINTN                      Pages;
+
+  Mem       = NULL;
+  AllocSize = UFS_PEIM_MEM_ROUND (Size);
+  Head      = Pool->Head;
+  ASSERT (Head != NULL);
+
+  //
+  // First check whether current memory blocks can satisfy the allocation.
+  //
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    Mem = UfsPeimAllocMemFromBlock (Block, AllocSize / UFS_PEIM_MEM_UNIT);
+
+    if (Mem != NULL) {
+      ZeroMem (Mem, Size);
+      break;
+    }
+  }
+
+  if (Mem != NULL) {
+    return Mem;
+  }
+
+  //
+  // Create a new memory block if there is not enough memory
+  // in the pool. If the allocation size is larger than the
+  // default page number, just allocate a large enough memory
+  // block. Otherwise allocate default pages.
+  //
+  if (AllocSize > EFI_PAGES_TO_SIZE (UFS_PEIM_MEM_DEFAULT_PAGES)) {
+    Pages = EFI_SIZE_TO_PAGES (AllocSize) + 1;
+  } else {
+    Pages = UFS_PEIM_MEM_DEFAULT_PAGES;
+  }
+
+  NewBlock = UfsPeimAllocMemBlock (Pages);
+  if (NewBlock == NULL) {
+    return NULL;
+  }
+  
+  //
+  // Add the new memory block to the pool, then allocate memory from it
+  //
+  UfsPeimInsertMemBlockToPool (Head, NewBlock);
+  Mem = UfsPeimAllocMemFromBlock (NewBlock, AllocSize / UFS_PEIM_MEM_UNIT);
+
+  if (Mem != NULL) {
+    ZeroMem (Mem, Size);
+  }
+
+  return Mem;
+}
+
+/**
+  Free the allocated memory back to the memory pool.
+
+  @param  Pool           The memory pool of the host controller.
+  @param  Mem            The memory to free.
+  @param  Size           The size of the memory to free.
+
+**/
+VOID
+UfsPeimFreeMem (
+  IN UFS_PEIM_MEM_POOL    *Pool,
+  IN VOID                 *Mem,
+  IN UINTN                Size
+  )
+{
+  UFS_PEIM_MEM_BLOCK      *Head;
+  UFS_PEIM_MEM_BLOCK      *Block;
+  UINT8                   *ToFree;
+  UINTN                   AllocSize;
+  UINTN                   Byte;
+  UINTN                   Bit;
+  UINTN                   Count;
+
+  Head      = Pool->Head;
+  AllocSize = UFS_PEIM_MEM_ROUND (Size);
+  ToFree    = (UINT8 *) Mem;
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    //
+    // scan the memory block list for the memory block that
+    // completely contains the memory to free.
+    //
+    if ((Block->Buf <= ToFree) && ((ToFree + AllocSize) <= (Block->Buf + Block->BufLen))) {
+      //
+      // compute the start byte and bit in the bit array
+      //
+      Byte  = ((ToFree - Block->Buf) / UFS_PEIM_MEM_UNIT) / 8;
+      Bit   = ((ToFree - Block->Buf) / UFS_PEIM_MEM_UNIT) % 8;
+
+      //
+      // reset associated bits in bit arry
+      //
+      for (Count = 0; Count < (AllocSize / UFS_PEIM_MEM_UNIT); Count++) {
+        ASSERT (UFS_PEIM_MEM_BIT_IS_SET (Block->Bits[Byte], Bit));
+
+        Block->Bits[Byte] = (UINT8) (Block->Bits[Byte] ^ UFS_PEIM_MEM_BIT (Bit));
+        UFS_PEIM_NEXT_BIT (Byte, Bit);
+      }
+
+      break;
+    }
+  }
+
+  //
+  // If Block == NULL, it means that the current memory isn't
+  // in the host controller's pool. This is critical because
+  // the caller has passed in a wrong memory point
+  //
+  ASSERT (Block != NULL);
+
+  //
+  // Release the current memory block if it is empty and not the head
+  //
+  if ((Block != Head) && UfsPeimIsMemBlockEmpty (Block)) {
+    UfsPeimFreeMemBlock (Pool, Block);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h
new file mode 100644
index 0000000000..3c4b2407c8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHcMem.h
@@ -0,0 +1,61 @@
+/** @file
+
+Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _UFS_PEIM_MEM_H_
+#define _UFS_PEIM_MEM_H_
+
+#define UFS_PEIM_MEM_BIT(a)          ((UINTN)(1 << (a)))
+
+#define UFS_PEIM_MEM_BIT_IS_SET(Data, Bit)   \
+          ((BOOLEAN)(((Data) & UFS_PEIM_MEM_BIT(Bit)) == UFS_PEIM_MEM_BIT(Bit)))
+
+typedef struct _UFS_PEIM_MEM_BLOCK UFS_PEIM_MEM_BLOCK;
+
+struct _UFS_PEIM_MEM_BLOCK {
+  UINT8                   *Bits;    // Bit array to record which unit is allocated
+  UINTN                   BitsLen; 
+  UINT8                   *Buf;
+  UINTN                   BufLen;   // Memory size in bytes
+  UFS_PEIM_MEM_BLOCK      *Next;
+};
+
+typedef struct _UFS_PEIM_MEM_POOL {
+  UFS_PEIM_MEM_BLOCK         *Head;
+} UFS_PEIM_MEM_POOL;
+
+//
+// Memory allocation unit, note that the value must meet UFS spec alignment requirement.
+//
+#define UFS_PEIM_MEM_UNIT           128
+
+#define UFS_PEIM_MEM_UNIT_MASK      (UFS_PEIM_MEM_UNIT - 1)
+#define UFS_PEIM_MEM_DEFAULT_PAGES  16
+
+#define UFS_PEIM_MEM_ROUND(Len)  (((Len) + UFS_PEIM_MEM_UNIT_MASK) & (~UFS_PEIM_MEM_UNIT_MASK))
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define UFS_PEIM_NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c
new file mode 100644
index 0000000000..68800570cb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.c
@@ -0,0 +1,1787 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsBlockIoPei.h"
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param  Address           The system memory address to test.
+  @param  MaskValue         The mask value of memory.
+  @param  TestValue         The test value of memory.
+  @param  Timeout           The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsWaitMemSet (
+  IN  UINTN                     Address,
+  IN  UINT32                    MaskValue,
+  IN  UINT32                    TestValue,
+  IN  UINT64                    Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 10) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Value = MmioRead32 (Address) & MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 1 microseconds.
+    //
+    MicroSecondDelay (1);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Dump UIC command execution result for debugging.
+
+  @param[in]   UicOpcode  The executed UIC opcode.
+  @param[in]   Result     The result to be parsed.
+
+**/
+VOID
+DumpUicCmdExecResult (
+  IN  UINT8     UicOpcode,
+  IN  UINT8     Result
+  )
+{
+  if (UicOpcode <= UfsUicDmePeerSet) {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x02:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE_VALUE\n"));
+        break;
+      case 0x03:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - READ_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x04:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - WRITE_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x05:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_INDEX\n"));
+        break;
+      case 0x06:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - LOCKED_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x07:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_TEST_FEATURE_INDEX\n"));
+        break;
+      case 0x08:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - PEER_COMMUNICATION_FAILURE\n"));
+        break; 
+      case 0x09:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BUSY\n"));
+        break;
+      case 0x0A:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - DME_FAILURE\n"));
+        break;        
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  } else {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC control command fails - FAILURE\n"));
+        break;     
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  }
+}
+
+/**
+  Dump QUERY RESPONSE UPIU result for debugging.
+
+  @param[in]   Result  The result to be parsed.
+
+**/
+VOID
+DumpQueryResponseResult (
+  IN  UINT8     Result
+  )
+{
+  switch (Result) {
+    case 0xF6:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Readable\n"));
+      break;
+    case 0xF7:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Writeable\n"));
+      break;
+    case 0xF8:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Already Written\n"));
+      break;
+    case 0xF9:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Length\n"));
+      break;
+    case 0xFA:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Value\n"));
+      break;
+    case 0xFB:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Selector\n"));
+      break;
+    case 0xFC:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Index\n"));
+      break;
+    case 0xFD:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Idn\n"));
+      break;
+    case 0xFE:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Opcode\n"));
+      break; 
+    case 0xFF:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with General Failure\n"));
+      break;
+    default :
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Swap little endian to big endian.
+
+  @param[in, out] Buffer      The data buffer. In input, it contains little endian data.
+                              In output, it will become big endian.
+  @param[in]      BufferSize  The length of converted data.
+
+**/
+VOID
+SwapLittleEndianToBigEndian (
+  IN OUT UINT8         *Buffer,
+  IN     UINT32        BufferSize
+  )
+{
+  UINT32 Index;
+  UINT8  Temp;
+  UINT32 SwapCount;
+
+  SwapCount = BufferSize / 2;
+  for (Index = 0; Index < SwapCount; Index++) {
+    Temp = Buffer[Index];
+    Buffer[Index] = Buffer[BufferSize - 1 - Index];
+    Buffer[BufferSize - 1 - Index] = Temp;
+  }
+}
+
+/**
+  Fill TSF field of QUERY REQUEST UPIU.
+
+  @param[in, out] TsfBase      The base address of TSF field of QUERY REQUEST UPIU.
+  @param[in]      Opcode       The opcode of request.
+  @param[in]      DescId       The descriptor ID of request.
+  @param[in]      Index        The index of request.
+  @param[in]      Selector     The selector of request.
+  @param[in]      Length       The length of transferred data. The maximum is 4.
+  @param[in]      Value        The value of transferred data.
+
+**/
+VOID
+UfsFillTsfOfQueryReqUpiu (
+  IN OUT UTP_UPIU_TSF        *TsfBase,
+  IN     UINT8               Opcode,
+  IN     UINT8               DescId    OPTIONAL,
+  IN     UINT8               Index     OPTIONAL,
+  IN     UINT8               Selector  OPTIONAL,
+  IN     UINT16              Length    OPTIONAL,
+  IN     UINT32              Value     OPTIONAL
+  )
+{
+  ASSERT (TsfBase != NULL);
+  ASSERT (Opcode <= UtpQueryFuncOpcodeTogFlag);
+
+  TsfBase->Opcode   = Opcode;
+  if (Opcode != UtpQueryFuncOpcodeNop) {
+    TsfBase->DescId   = DescId;
+    TsfBase->Index    = Index;
+    TsfBase->Selector = Selector;
+
+    if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Length, sizeof (Length));
+      TsfBase->Length = Length;
+    }
+  
+    if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Value, sizeof (Value));
+      TsfBase->Value  = Value;
+    }
+  }
+}
+
+/**
+  Initialize COMMAND UPIU.
+
+  @param[in, out] Command         The base address of COMMAND UPIU.
+  @param[in]      Lun             The Lun on which the SCSI command is executed.
+  @param[in]      TaskTag         The task tag of request.
+  @param[in]      Cdb             The cdb buffer containing SCSI command.
+  @param[in]      CdbLength       The cdb length.
+  @param[in]      DataDirection   The direction of data transfer.
+  @param[in]      ExpDataTranLen  The expected transfer data length.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitCommandUpiu (
+  IN OUT UTP_COMMAND_UPIU              *Command,
+  IN     UINT8                         Lun,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         *Cdb,
+  IN     UINT8                         CdbLength,
+  IN     UFS_DATA_DIRECTION            DataDirection,
+  IN     UINT32                        ExpDataTranLen
+  )
+{
+  UINT8                   Flags;
+
+  ASSERT ((Command != NULL) && (Cdb != NULL));
+
+  //
+  // Task attribute is hard-coded to Ordered.
+  //
+  if (DataDirection == UfsDataIn) {
+    Flags = BIT0 | BIT6;
+  } else if (DataDirection == UfsDataOut) {
+    Flags = BIT0 | BIT5;
+  } else {
+    Flags = BIT0;
+  }
+
+  //
+  // Fill UTP COMMAND UPIU associated fields.
+  //
+  Command->TransCode = 0x01;
+  Command->Flags     = Flags;
+  Command->Lun       = Lun;
+  Command->TaskTag   = TaskTag;
+  Command->CmdSet    = 0x00;
+  SwapLittleEndianToBigEndian ((UINT8*)&ExpDataTranLen, sizeof (ExpDataTranLen));
+  Command->ExpDataTranLen = ExpDataTranLen;
+
+  CopyMem (Command->Cdb, Cdb, CdbLength);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UTP PRDT for data transfer.
+
+  @param[in] Prdt         The base address of PRDT.
+  @param[in] Buffer       The buffer to be read or written.
+  @param[in] BufferSize   The data size to be read or written.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitUtpPrdt (
+  IN  UTP_TR_PRD                       *Prdt,
+  IN  VOID                             *Buffer,
+  IN  UINT32                           BufferSize
+  )
+{
+  UINT32     PrdtIndex;
+  UINT32     RemainingLen;
+  UINT8      *Remaining;
+  UINTN      PrdtNumber;
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (((UINTN)Buffer & (BIT0 | BIT1)) == 0);
+
+  RemainingLen = BufferSize;
+  Remaining    = Buffer;
+  PrdtNumber   = (UINTN)DivU64x32 ((UINT64)BufferSize + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainingLen < UFS_MAX_DATA_LEN_PER_PRD) {
+      Prdt[PrdtIndex].DbCount = (UINT32)RemainingLen - 1;
+    } else {
+      Prdt[PrdtIndex].DbCount = UFS_MAX_DATA_LEN_PER_PRD - 1;
+    }
+
+    Prdt[PrdtIndex].DbAddr  = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 2);
+    Prdt[PrdtIndex].DbAddrU = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 32);
+    RemainingLen -= UFS_MAX_DATA_LEN_PER_PRD;
+    Remaining    += UFS_MAX_DATA_LEN_PER_PRD;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize QUERY REQUEST UPIU.
+
+  @param[in, out] QueryReq      The base address of QUERY REQUEST UPIU.
+  @param[in]      TaskTag       The task tag of request.
+  @param[in]      Opcode        The opcode of request.
+  @param[in]      DescId        The descriptor ID of request.
+  @param[in]      Index         The index of request.
+  @param[in]      Selector      The selector of request.
+  @param[in]      DataSize      The data size to be read or written.
+  @param[in]      Data          The buffer to be read or written.
+
+  @retval EFI_SUCCESS           The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitQueryRequestUpiu (
+  IN OUT UTP_QUERY_REQ_UPIU            *QueryReq,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         Opcode,
+  IN     UINT8                         DescId,
+  IN     UINT8                         Index,
+  IN     UINT8                         Selector,
+  IN     UINTN                         DataSize   OPTIONAL,
+  IN     UINT8                         *Data      OPTIONAL
+  )
+{
+  ASSERT (QueryReq != NULL);
+
+  QueryReq->TransCode = 0x16;
+  QueryReq->TaskTag   = TaskTag;
+  if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeRdFlag) || (Opcode == UtpQueryFuncOpcodeRdAttr)) {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_READ_REQ;
+  } else {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_WRITE_REQ;
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, *(UINT32*)Data);
+  } else if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, (UINT16)DataSize, 0);
+  } else {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, 0);
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    CopyMem (QueryReq + 1, Data, DataSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate COMMAND/RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Lun               The Lun on which the SCSI command is executed.
+  @param[in]  Packet            The pointer to the UFS_SCSI_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateScsiCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA            *Private,
+  IN  UINT8                               Lun,
+  IN  UFS_SCSI_REQUEST_PACKET             *Packet,
+  IN  UTP_TRD                             *Trd
+  )
+{
+  UINT8                    *CommandDesc;
+  UINTN                    TotalLen;
+  UINTN                    PrdtNumber;
+  VOID                     *Buffer;
+  UINT32                   Length;
+  UTP_COMMAND_UPIU         *CommandUpiu;
+  UTP_TR_PRD               *PrdtBase;
+  UFS_DATA_DIRECTION       DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  if (Packet->DataDirection == UfsDataIn) {
+    Buffer = Packet->InDataBuffer;
+    Length = Packet->InTransferLength;
+    DataDirection = UfsDataIn;
+  } else {
+    Buffer = Packet->OutDataBuffer;
+    Length = Packet->OutTransferLength;
+    DataDirection = UfsDataOut;
+  }
+
+  if (Length == 0) {
+    DataDirection = UfsNoData;
+  }
+
+  PrdtNumber = (UINTN)DivU64x32 ((UINT64)Length + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  TotalLen    = ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)) + PrdtNumber * sizeof (UTP_TR_PRD);
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CommandUpiu  = (UTP_COMMAND_UPIU*)CommandDesc;
+  PrdtBase     = (UTP_TR_PRD*)(CommandDesc + ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)));
+
+  UfsInitCommandUpiu (CommandUpiu, Lun, Private->TaskTag++, Packet->Cdb, Packet->CdbLength, DataDirection, Length);
+  UfsInitUtpPrdt (PrdtBase, Buffer, Length);
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Response UPIU and the Physical Region Description Table
+  // *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)CommandUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)CommandUpiu, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)), sizeof (UINT32));
+  Trd->PrdtL  = (UINT16)PrdtNumber;
+  Trd->PrdtO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU))), sizeof (UINT32));
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate QUERY REQUEST/QUERY RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Packet            The pointer to the UFS_DEVICE_MANAGEMENT_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+  @retval EFI_INVALID_PARAMETER The parameter passed in is invalid.
+
+**/
+EFI_STATUS
+UfsCreateDMCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA              *Private,
+  IN  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET  *Packet,
+  IN  UTP_TRD                               *Trd
+  )
+{
+  UINT8                         *CommandDesc;
+  UINTN                         TotalLen;
+  UTP_QUERY_REQ_UPIU            *QueryReqUpiu;
+  UINT8                         Opcode;
+  UINT32                        DataSize;
+  UINT8                         *Data;
+  UINT8                         DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  Opcode = Packet->Opcode;
+  if ((Opcode > UtpQueryFuncOpcodeTogFlag) || (Opcode == UtpQueryFuncOpcodeNop)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataDirection = Packet->DataDirection;
+  if (DataDirection == UfsDataIn) {
+    DataSize = Packet->InTransferLength;
+    Data     = Packet->InDataBuffer;
+  } else if (DataDirection == UfsDataOut) {
+    DataSize = Packet->OutTransferLength;
+    Data     = Packet->OutDataBuffer;
+  } else {
+    DataSize = 0;
+    Data     = NULL;
+  }
+
+  if (((Opcode != UtpQueryFuncOpcodeSetFlag) && (Opcode != UtpQueryFuncOpcodeClrFlag) && (Opcode != UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize == 0) || (Data == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((Opcode == UtpQueryFuncOpcodeSetFlag) || (Opcode == UtpQueryFuncOpcodeClrFlag) || (Opcode == UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize != 0) || (Data != NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrAttr) && (DataSize != sizeof (UINT32))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrDesc) || (Opcode == UtpQueryFuncOpcodeRdDesc)) {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize);
+  } else {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU));
+  }
+
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize UTP QUERY REQUEST UPIU
+  //
+  QueryReqUpiu = (UTP_QUERY_REQ_UPIU*)CommandDesc;
+  UfsInitQueryRequestUpiu (
+    QueryReqUpiu,
+    Private->TaskTag++,
+    Opcode,
+    Packet->DescId,
+    Packet->Index,
+    Packet->Selector,
+    DataSize,
+    Data
+    );
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Query Response UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->Ocs    = 0x0F;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)QueryReqUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)QueryReqUpiu, 32);
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+  } else {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)), sizeof (UINT32));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate NOP IN and NOP OUT UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateNopCommandDesc (
+  IN  UFS_PEIM_HC_PRIVATE_DATA                    *Private,
+  IN  UTP_TRD                                     *Trd
+  )
+{
+  UINT8                    *CommandDesc;
+  UINTN                    TotalLen;
+  UTP_NOP_OUT_UPIU         *NopOutUpiu;
+
+  ASSERT ((Private != NULL) && (Trd != NULL));
+
+  TotalLen    = ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)) + ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU));
+  CommandDesc = UfsPeimAllocateMem (Private->Pool, TotalLen);
+  if (CommandDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NopOutUpiu = (UTP_NOP_OUT_UPIU*)CommandDesc;
+
+  NopOutUpiu->TaskTag = Private->TaskTag++;
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Nop Out UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = 0x00;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)(UINTN)NopOutUpiu, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)(UINTN)NopOutUpiu, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)), sizeof (UINT32));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTrl (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in task management transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTmrl (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be started.
+
+**/
+VOID
+UfsStartExecCmd (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINTN         UfsHcBase;
+  UINTN         Address;
+  UINT32        Data;
+
+  UfsHcBase = Private->UfsHcBase;
+
+  Address = UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_UTRLRSR) != UFS_HC_UTRLRSR) {
+    MmioWrite32 (Address, UFS_HC_UTRLRSR);
+  }
+
+  Address = UfsHcBase + UFS_HC_UTRLDBR_OFFSET;
+  MmioWrite32 (Address, BIT0 << Slot);
+}
+
+/**
+  Stop specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be stop.
+
+**/
+VOID
+UfsStopExecCmd (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINTN         UfsHcBase;
+  UINTN         Address;
+  UINT32        Data;
+
+  UfsHcBase = Private->UfsHcBase;
+
+  Address = UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & (BIT0 << Slot)) != 0) {
+    Address = UfsHcBase + UFS_HC_UTRLCLR_OFFSET;  
+    Data    = MmioRead32 (Address);
+    MmioWrite32 (Address, (Data & ~(BIT0 << Slot)));
+  }
+}
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINT16                               ReturnDataSize;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.InDataBuffer      = Descriptor;
+    Packet.InTransferLength  = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdDesc;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.OutDataBuffer     = Descriptor;
+    Packet.OutTransferLength = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrDesc;
+  }
+  Packet.DescId              = DescId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnDataSize = QueryResp->Tsf.Length;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnDataSize, sizeof (UINT16));
+
+    if (Read) {
+      CopyMem (Packet.InDataBuffer, (QueryResp + 1), ReturnDataSize);
+      Packet.InTransferLength = ReturnDataSize;
+    } else {
+      Packet.OutTransferLength = ReturnDataSize;
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Read or write specified attribute of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      AttrId        The ID of Attribute.
+  @param[in]      Index         The Index of Attribute.
+  @param[in]      Selector      The Selector of Attribute.
+  @param[in, out] Attributes    The value of Attribute to be read or written.
+
+  @retval EFI_SUCCESS           The Attribute was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the Attribute.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the Attribute.
+
+**/
+EFI_STATUS
+UfsRwAttributes (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        AttrId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT UINT32                       *Attributes
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINT32                               ReturnData;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdAttr;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrAttr;
+  }
+  Packet.DescId              = AttrId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnData = QueryResp->Tsf.Value;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnData, sizeof (UINT32));
+    *Attributes = ReturnData;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Read or write specified flag of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      FlagId        The ID of flag to be read or written.
+  @param[in, out] Value         The value to set or clear flag.
+
+  @retval EFI_SUCCESS           The flag was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the flag.
+
+**/
+EFI_STATUS
+UfsRwFlags (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        FlagId,
+  IN OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+
+  if (Value == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    ASSERT (Value != NULL);
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdFlag;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    if (*Value == 1) {
+      Packet.Opcode          = UtpQueryFuncOpcodeSetFlag;
+    } else if (*Value == 0) {
+      Packet.Opcode          = UtpQueryFuncOpcodeClrFlag;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  Packet.DescId              = FlagId;
+  Packet.Index               = 0;
+  Packet.Selector            = 0;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    *Value = (UINT8)QueryResp->Tsf.Value;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 1;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Clear specified flag to 0 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be cleared.
+
+  @retval EFI_SUCCESS           The flag was cleared successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to clear the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of clearing the flag.
+
+**/
+EFI_STATUS
+UfsClearFlag (
+  IN  UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 0;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Read specified flag from a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be read.
+  @param[out] Value             The flag's value.
+
+  @retval EFI_SUCCESS           The flag was read successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to read the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of reading the flag.
+
+**/
+EFI_STATUS
+UfsReadFlag (
+  IN     UFS_PEIM_HC_PRIVATE_DATA     *Private,
+  IN     UINT8                        FlagId,
+     OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+
+  Status = UfsRwFlags (Private, TRUE, FlagId, Value);
+
+  return Status;
+}
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PEIM_HC_PRIVATE_DATA         *Private
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_NOP_IN_UPIU                      *NopInUpiu;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UINTN                                Address;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateNopCommandDesc (Private, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  CmdDescBase = (UINT8 *)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  NopInUpiu   = (UTP_NOP_IN_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (NopInUpiu->Resp != 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN     UINT8                         Lun,
+  IN OUT UFS_SCSI_REQUEST_PACKET       *Packet
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UINTN                                Address;
+  UINT8                                *CmdDescBase;
+  UINT32                               CmdDescSize;
+  UTP_RESPONSE_UPIU                    *Response;
+  UINT16                               SenseDataLen;
+  UINT32                               ResTranCount;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateScsiCommandDesc (Private, Lun, Packet, Trd);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CmdDescBase = (UINT8*)(UINTN)(LShiftU64 ((UINT64)Trd->UcdBaU, 32) | LShiftU64 ((UINT64)Trd->UcdBa, 7));
+  CmdDescSize = Trd->PrdtO * sizeof (UINT32) + Trd->PrdtL * sizeof (UTP_TR_PRD);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Address = Private->UfsHcBase + UFS_HC_UTRLDBR_OFFSET;  
+  Status = UfsWaitMemSet (Address, BIT0, 0, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get sense data if exists
+  //
+  Response     = (UTP_RESPONSE_UPIU*)(CmdDescBase + Trd->RuO * sizeof (UINT32));
+  SenseDataLen = Response->SenseDataLen;
+  SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+  
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+    CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+    Packet->SenseDataLength = (UINT8)SenseDataLen;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  if (Response->Response != 0) {
+    DEBUG ((EFI_D_ERROR, "UfsExecScsiCmds() fails with Target Failure\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    if (Packet->DataDirection == UfsDataIn) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->InTransferLength -= ResTranCount;
+      }
+    } else if (Packet->DataDirection == UfsDataOut) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->OutTransferLength -= ResTranCount;
+      }
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsStopExecCmd (Private, Slot);
+  UfsPeimFreeMem (Private->Pool, CmdDescBase, CmdDescSize);
+
+  return Status;
+}
+
+
+/**
+  Sent UIC DME_LINKSTARTUP command to start the link startup procedure.
+
+  @param[in] Private          The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+  @param[in] UicOpcode        The opcode of the UIC command.
+  @param[in] Arg1             The value for 1st argument of the UIC command.
+  @param[in] Arg2             The value for 2nd argument of the UIC command.
+  @param[in] Arg3             The value for 3rd argument of the UIC command.
+
+  @return EFI_SUCCESS      Successfully execute this UIC command and detect attached UFS device.
+  @return EFI_DEVICE_ERROR Fail to execute this UIC command and detect attached UFS device.
+  @return EFI_NOT_FOUND    The presence of the UFS device isn't detected.
+
+**/
+EFI_STATUS
+UfsExecUicCommands (
+  IN  UFS_PEIM_HC_PRIVATE_DATA      *Private,
+  IN  UINT8                         UicOpcode,
+  IN  UINT32                        Arg1,
+  IN  UINT32                        Arg2,
+  IN  UINT32                        Arg3
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Address;
+  UINT32      Data;
+  UINTN       UfsHcBase;
+
+  UfsHcBase = Private->UfsHcBase;
+  Address   = UfsHcBase + UFS_HC_IS_OFFSET;
+  Data      = MmioRead32 (Address);
+  if ((Data & UFS_HC_IS_UCCS) == UFS_HC_IS_UCCS) {
+    //
+    // Clear IS.BIT10 UIC Command Completion Status (UCCS) at first.
+    //
+    MmioWrite32 (Address, Data);
+  }
+
+  //
+  // When programming UIC command registers, host software shall set the register UICCMD
+  // only after all the UIC command argument registers (UICCMDARG1, UICCMDARG2 and UICCMDARG3)
+  // are set.
+  //
+  Address = UfsHcBase + UFS_HC_UCMD_ARG1_OFFSET;
+  MmioWrite32 (Address, Arg1);
+
+  Address = UfsHcBase + UFS_HC_UCMD_ARG2_OFFSET;
+  MmioWrite32 (Address, Arg2);
+
+  Address = UfsHcBase + UFS_HC_UCMD_ARG3_OFFSET;
+  MmioWrite32 (Address, Arg3);
+
+  //
+  // Host software shall only set the UICCMD if HCS.UCRDY is set to 1.
+  //
+  Address = Private->UfsHcBase + UFS_HC_STATUS_OFFSET;
+  Status = UfsWaitMemSet (Address, UFS_HC_HCS_UCRDY, UFS_HC_HCS_UCRDY, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Address = UfsHcBase + UFS_HC_UIC_CMD_OFFSET;
+  MmioWrite32 (Address, (UINT32)UicOpcode);
+
+  //
+  // UFS 2.0 spec section 5.3.1 Offset:0x20 IS.Bit10 UIC Command Completion Status (UCCS)
+  // This bit is set to '1' by the host controller upon completion of a UIC command. 
+  //
+  Address = UfsHcBase + UFS_HC_IS_OFFSET;
+  Data    = MmioRead32 (Address);
+  Status  = UfsWaitMemSet (Address, UFS_HC_IS_UCCS, UFS_HC_IS_UCCS, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (UicOpcode != UfsUicDmeReset) {
+    Address = UfsHcBase + UFS_HC_UCMD_ARG2_OFFSET;
+    Data    = MmioRead32 (Address);
+    if ((Data & 0xFF) != 0) {
+      DEBUG_CODE_BEGIN();
+        DumpUicCmdExecResult (UicOpcode, (UINT8)(Data & 0xFF));
+      DEBUG_CODE_END();
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Check value of HCS.DP and make sure that there is a device attached to the Link.
+  //
+  Address = UfsHcBase + UFS_HC_STATUS_OFFSET;  
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_HCS_DP) == 0) {
+    Address = UfsHcBase + UFS_HC_IS_OFFSET;
+    Status  = UfsWaitMemSet (Address, UFS_HC_IS_ULSS, UFS_HC_IS_ULSS, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    return EFI_NOT_FOUND;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsblockioPei: found a attached UFS device\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable the UFS host controller for accessing.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS host controller enabling was executed successfully.
+  @retval EFI_DEVICE_ERROR           A device error occurred while enabling the UFS host controller.
+
+**/
+EFI_STATUS
+UfsEnableHostController (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  Address;
+  UINT32                 Data;
+
+  //
+  // UFS 2.0 spec section 7.1.1 - Host Controller Initialization
+  //
+  // Reinitialize the UFS host controller if HCE bit of HC register is set.
+  //
+  Address = Private->UfsHcBase + UFS_HC_ENABLE_OFFSET;
+  Data    = MmioRead32 (Address);
+  if ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN) {
+    //
+    // Write a 0 to the HCE register at first to disable the host controller.
+    //
+    MmioWrite32 (Address, 0);
+    //
+    // Wait until HCE is read as '0' before continuing.
+    //
+    Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Write a 1 to the HCE register to enable the UFS host controller.
+  //
+  MmioWrite32 (Address, UFS_HC_HCE_EN);
+  //
+  // Wait until HCE is read as '1' before continuing.
+  //
+  Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, UFS_HC_HCE_EN, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect if a UFS device attached.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS device detection was executed successfully.
+  @retval EFI_NOT_FOUND              Not found a UFS device attached.
+  @retval EFI_DEVICE_ERROR           A device error occurred while detecting the UFS device.
+
+**/
+EFI_STATUS
+UfsDeviceDetection (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Retry;
+  EFI_STATUS             Status;
+
+  //
+  // Start UFS device detection.
+  // Try up to 3 times for establishing data link with device.
+  //
+  for (Retry = 0; Retry < 3; Retry++) {
+    Status = UfsExecUicCommands (Private, UfsUicDmeLinkStartup, 0, 0, 0);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      continue;
+    }
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Retry == 3) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS task management request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS task management list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTaskManagementRequestList (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Address;
+  UINT32                 Data;
+  UINT8                  Nutmrs;
+  EFI_PHYSICAL_ADDRESS   Buffer;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  Address = Private->UfsHcBase + UFS_HC_CAP_OFFSET;  
+  Data    = MmioRead32 (Address);
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Task Management Request List.
+  //
+  Nutmrs = (UINT8) (RShiftU64 ((Private->Capabilities & UFS_HC_CAP_NUTMRS), 16) + 1);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             EFI_SIZE_TO_PAGES (Nutmrs * sizeof (UTP_TMRD)),
+             &Buffer
+             );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Buffer, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Nutmrs * sizeof (UTP_TMRD))));
+
+  //
+  // Program the UTP Task Management Request List Base Address and UTP Task Management
+  // Request List Base Address with a 64-bit address allocated at step 6.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLBA_OFFSET;  
+  MmioWrite32 (Address, (UINT32)(UINTN)Buffer);
+  Address = Private->UfsHcBase + UFS_HC_UTMRLBAU_OFFSET;  
+  MmioWrite32 (Address, (UINT32)RShiftU64 ((UINT64)Buffer, 32));
+  Private->UtpTmrlBase = (VOID*)(UINTN)Buffer;
+  Private->Nutmrs      = Nutmrs;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLRSR_OFFSET;  
+  MmioWrite32 (Address, UFS_HC_UTMRLRSR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS transfer request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS transfer list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTransferRequestList (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  UINTN                  Address;
+  UINT32                 Data;
+  UINT8                  Nutrs;
+  EFI_PHYSICAL_ADDRESS   Buffer;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  Address = Private->UfsHcBase + UFS_HC_CAP_OFFSET;  
+  Data    = MmioRead32 (Address);
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Transfer Request List.
+  //
+  Nutrs  = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             EFI_SIZE_TO_PAGES (Nutrs * sizeof (UTP_TRD)),
+             &Buffer
+             );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem ((VOID*)(UINTN)Buffer, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Nutrs * sizeof (UTP_TRD))));
+
+  //
+  // Program the UTP Transfer Request List Base Address and UTP Transfer Request List
+  // Base Address with a 64-bit address allocated at step 8.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLBA_OFFSET;  
+  MmioWrite32 (Address, (UINT32)(UINTN)Buffer);
+  Address = Private->UfsHcBase + UFS_HC_UTRLBAU_OFFSET;  
+  MmioWrite32 (Address, (UINT32)RShiftU64 ((UINT64)Buffer, 32));
+  Private->UtpTrlBase = (VOID*)(UINTN)Buffer;
+  Private->Nutrs      = Nutrs;
+  
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  MmioWrite32 (Address, UFS_HC_UTRLRSR);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+
+  Status = UfsEnableHostController (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Enable Host Controller Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsDeviceDetection (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Device Detection Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTaskManagementRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Task management list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTransferRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsDevicePei: Transfer list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsDevicePei Finished\n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PEIM_HC_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PEIM_HC_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINTN                  Address;
+  UINT32                 Data;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTMRLRSR_OFFSET;  
+  MmioWrite32 (Address, 0);
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Address = Private->UfsHcBase + UFS_HC_UTRLRSR_OFFSET;  
+  MmioWrite32 (Address, 0);
+
+  //
+  // Write a 0 to the HCE register in order to disable the host controller.
+  //
+  Address = Private->UfsHcBase + UFS_HC_ENABLE_OFFSET;
+  Data    = MmioRead32 (Address);
+  ASSERT ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN);
+  MmioWrite32 (Address, 0);
+
+  //
+  // Wait until HCE is read as '0' before continuing.
+  //
+  Status = UfsWaitMemSet (Address, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsDevicePei: Stop the UFS Host Controller\n"));
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h
new file mode 100644
index 0000000000..a423a921de
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsHci.h
@@ -0,0 +1,1339 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _UFS_PASS_THRU_HCI_H_
+#define _UFS_PASS_THRU_HCI_H_
+
+//
+// Host Capabilities Register Offsets
+//
+#define UFS_HC_CAP_OFFSET          0x0000  // Controller Capabilities
+#define UFS_HC_VER_OFFSET          0x0008  // Version
+#define UFS_HC_DDID_OFFSET         0x0010  // Device ID and Device Class
+#define UFS_HC_PMID_OFFSET         0x0014  // Product ID and Manufacturer ID
+#define UFS_HC_AHIT_OFFSET         0x0018  // Auto-Hibernate Idle Timer
+//
+// Operation and Runtime Register Offsets
+//
+#define UFS_HC_IS_OFFSET           0x0020  // Interrupt Status
+#define UFS_HC_IE_OFFSET           0x0024  // Interrupt Enable
+#define UFS_HC_STATUS_OFFSET       0x0030  // Host Controller Status
+#define UFS_HC_ENABLE_OFFSET       0x0034  // Host Controller Enable
+#define UFS_HC_UECPA_OFFSET        0x0038  // Host UIC Error Code PHY Adapter Layer
+#define UFS_HC_UECDL_OFFSET        0x003c  // Host UIC Error Code Data Link Layer
+#define UFS_HC_UECN_OFFSET         0x0040  // Host UIC Error Code Network Layer
+#define UFS_HC_UECT_OFFSET         0x0044  // Host UIC Error Code Transport Layer
+#define UFS_HC_UECDME_OFFSET       0x0048  // Host UIC Error Code DME
+#define UFS_HC_UTRIACR_OFFSET      0x004c  // UTP Transfer Request Interrupt Aggregation Control Register
+//
+// UTP Transfer Register Offsets
+//
+#define UFS_HC_UTRLBA_OFFSET       0x0050  // UTP Transfer Request List Base Address
+#define UFS_HC_UTRLBAU_OFFSET      0x0054  // UTP Transfer Request List Base Address Upper 32-Bits
+#define UFS_HC_UTRLDBR_OFFSET      0x0058  // UTP Transfer Request List Door Bell Register
+#define UFS_HC_UTRLCLR_OFFSET      0x005c  // UTP Transfer Request List CLear Register
+#define UFS_HC_UTRLRSR_OFFSET      0x0060  // UTP Transfer Request Run-Stop Register
+//
+// UTP Task Management Register Offsets
+//
+#define UFS_HC_UTMRLBA_OFFSET      0x0070  // UTP Task Management Request List Base Address
+#define UFS_HC_UTMRLBAU_OFFSET     0x0074  // UTP Task Management Request List Base Address Upper 32-Bits
+#define UFS_HC_UTMRLDBR_OFFSET     0x0078  // UTP Task Management Request List Door Bell Register
+#define UFS_HC_UTMRLCLR_OFFSET     0x007c  // UTP Task Management Request List CLear Register
+#define UFS_HC_UTMRLRSR_OFFSET     0x0080  // UTP Task Management Run-Stop Register
+//
+// UIC Command Register Offsets
+//
+#define UFS_HC_UIC_CMD_OFFSET      0x0090  // UIC Command Register
+#define UFS_HC_UCMD_ARG1_OFFSET    0x0094  // UIC Command Argument 1
+#define UFS_HC_UCMD_ARG2_OFFSET    0x0098  // UIC Command Argument 2
+#define UFS_HC_UCMD_ARG3_OFFSET    0x009c  // UIC Command Argument 3
+//
+// UMA Register Offsets
+//
+#define UFS_HC_UMA_OFFSET          0x00b0  // Reserved for Unified Memory Extension
+
+#define UFS_HC_HCE_EN              BIT0
+#define UFS_HC_HCS_DP              BIT0
+#define UFS_HC_HCS_UCRDY           BIT3
+#define UFS_HC_IS_ULSS             BIT8
+#define UFS_HC_IS_UCCS             BIT10
+#define UFS_HC_CAP_64ADDR          BIT24
+#define UFS_HC_CAP_NUTMRS          (BIT16 | BIT17 | BIT18)
+#define UFS_HC_CAP_NUTRS           (BIT0 | BIT1 | BIT2 | BIT3 | BIT4)
+#define UFS_HC_UTMRLRSR            BIT0
+#define UFS_HC_UTRLRSR             BIT0
+
+//
+// A maximum of length of 256KB is supported by PRDT entry
+//
+#define UFS_MAX_DATA_LEN_PER_PRD   0x40000
+
+#define UFS_STORAGE_COMMAND_TYPE   0x01
+
+#define UFS_REGULAR_COMMAND        0x00
+#define UFS_INTERRUPT_COMMAND      0x01
+
+#define UFS_LUN_0                  0x00
+#define UFS_LUN_1                  0x01
+#define UFS_LUN_2                  0x02
+#define UFS_LUN_3                  0x03
+#define UFS_LUN_4                  0x04
+#define UFS_LUN_5                  0x05
+#define UFS_LUN_6                  0x06
+#define UFS_LUN_7                  0x07
+#define UFS_WLUN_REPORT_LUNS       0x81
+#define UFS_WLUN_UFS_DEV           0xD0
+#define UFS_WLUN_BOOT              0xB0
+#define UFS_WLUN_RPMB              0xC4
+
+#pragma pack(1)
+
+//
+// UFSHCI 2.0 Spec Section 5.2.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT8  Nutrs:4;      // Number of UTP Transfer Request Slots
+  UINT8  Rsvd1:4;
+
+  UINT8  NoRtt;        // Number of outstanding READY TO TRANSFER (RTT) requests supported
+
+  UINT8  Nutmrs:3;     // Number of UTP Task Management Request Slots
+  UINT8  Rsvd2:4;
+  UINT8  AutoHs:1;     // Auto-Hibernation Support
+
+  UINT8  As64:1;       // 64-bit addressing supported
+  UINT8  Oodds:1;      // Out of order data delivery supported
+  UINT8  UicDmetms:1;  // UIC DME_TEST_MODE command supported
+  UINT8  Ume:1;        // Reserved for Unified Memory Extension
+  UINT8  Rsvd4:4;
+} UFS_HC_CAP;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.2 Offset 08h: VER - UFS Version
+//
+typedef struct {
+  UINT8  Vs:4;         // Version Suffix
+  UINT8  Mnr:4;        // Minor version number
+
+  UINT8  Mjr;          // Major version number
+
+  UINT16 Rsvd1;
+} UFS_HC_VER;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.3 Offset 10h: HCPID - Host Controller Product ID
+//
+#define UFS_HC_PID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.4 Offset 14h: HCMID - Host Controller Manufacturer ID
+//
+#define UFS_HC_MID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.5 Offset 18h: AHIT - Auto-Hibernate Idle Timer
+//
+typedef struct {
+  UINT32 Ahitv:10;     // Auto-Hibernate Idle Timer Value 
+  UINT32 Ts:3;         // Timer scale
+  UINT32 Rsvd1:19;
+} UFS_HC_AHIT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.1 Offset 20h: IS - Interrupt Status
+//
+typedef struct {
+  UINT16 Utrcs:1;      // UTP Transfer Request Completion Status
+  UINT16 Udepri:1;     // UIC DME_ENDPOINT_RESET Indication
+  UINT16 Ue:1;         // UIC Error
+  UINT16 Utms:1;       // UIC Test Mode Status 
+
+  UINT16 Upms:1;       // UIC Power Mode Status 
+  UINT16 Uhxs:1;       // UIC Hibernate Exit Status 
+  UINT16 Uhes:1;       // UIC Hibernate Enter Status 
+  UINT16 Ulls:1;       // UIC Link Lost Status 
+
+  UINT16 Ulss:1;       // UIC Link Startup Status 
+  UINT16 Utmrcs:1;     // UTP Task  Management Request Completion Status 
+  UINT16 Uccs:1;       // UIC Command Completion Status 
+  UINT16 Dfes:1;       // Device Fatal Error Status  
+
+  UINT16 Utpes:1;      // UTP Error Status  
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfes:1;      // Host Controller Fatal Error Status
+  UINT16 Sbfes:1;      // System Bus Fatal Error Status
+  UINT16 Rsvd2:14;
+} UFS_HC_IS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.2 Offset 24h: IE - Interrupt Enable
+//
+typedef struct {
+  UINT16 Utrce:1;      // UTP Transfer Request Completion Enable
+  UINT16 Udeprie:1;    // UIC DME_ENDPOINT_RESET Enable
+  UINT16 Uee:1;        // UIC Error Enable
+  UINT16 Utmse:1;      // UIC Test Mode Status Enable
+
+  UINT16 Upmse:1;      // UIC Power Mode Status Enable 
+  UINT16 Uhxse:1;      // UIC Hibernate Exit Status Enable
+  UINT16 Uhese:1;      // UIC Hibernate Enter Status Enable 
+  UINT16 Ullse:1;      // UIC Link Lost Status Enable
+
+  UINT16 Ulsse:1;      // UIC Link Startup Status Enable
+  UINT16 Utmrce:1;     // UTP Task  Management Request Completion Enable
+  UINT16 Ucce:1;       // UIC Command Completion Enable
+  UINT16 Dfee:1;       // Device Fatal Error Enable
+
+  UINT16 Utpee:1;      // UTP Error Enable
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfee:1;      // Host Controller Fatal Error Enable
+  UINT16 Sbfee:1;      // System Bus Fatal Error Enable
+  UINT16 Rsvd2:14;
+} UFS_HC_IE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.3 Offset 30h: HCS - Host Controller Status
+//
+typedef struct {
+  UINT8  Dp:1;         // Device Present
+  UINT8  UtrlRdy:1;    // UTP Transfer Request List Ready
+  UINT8  UtmrlRdy:1;   // UTP Task Management Request List Ready
+  UINT8  UcRdy:1;      // UIC COMMAND Ready
+  UINT8  Rsvd1:4;
+
+  UINT8  Upmcrs:3;     // UIC Power Mode Change Request Status
+  UINT8  Rsvd2:1;      // UIC Hibernate Exit Status Enable
+  UINT8  Utpec:4;      // UTP Error Code
+
+  UINT8  TtagUtpE;     // Task Tag of UTP error
+  UINT8  TlunUtpE;     // Target LUN of UTP error
+} UFS_HC_STATUS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.4 Offset 34h: HCE - Host Controller Enable
+//
+typedef struct {
+  UINT32 Hce:1;        // Host Controller Enable
+  UINT32 Rsvd1:31;
+} UFS_HC_ENABLE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.5 Offset 38h: UECPA - Host UIC Error Code PHY Adapter Layer
+//
+typedef struct {
+  UINT32 Ec:5;         // UIC PHY Adapter Layer Error Code
+  UINT32 Rsvd1:26;
+  UINT32 Err:1;        // UIC PHY Adapter Layer Error
+} UFS_HC_UECPA;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.6 Offset 3ch: UECDL - Host UIC Error Code Data Link Layer
+//
+typedef struct {
+  UINT32 Ec:15;        // UIC Data Link Layer Error Code
+  UINT32 Rsvd1:16;
+  UINT32 Err:1;        // UIC Data Link Layer Error
+} UFS_HC_UECDL;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.7 Offset 40h: UECN - Host UIC Error Code Network Layer
+//
+typedef struct {
+  UINT32 Ec:3;         // UIC Network Layer Error Code
+  UINT32 Rsvd1:28;
+  UINT32 Err:1;        // UIC Network Layer Error
+} UFS_HC_UECN;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.8 Offset 44h: UECT - Host UIC Error Code Transport Layer
+//
+typedef struct {
+  UINT32 Ec:7;         // UIC Transport Layer Error Code
+  UINT32 Rsvd1:24;
+  UINT32 Err:1;        // UIC Transport Layer Error
+} UFS_HC_UECT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.9 Offset 48h: UECDME - Host UIC Error Code
+//
+typedef struct {
+  UINT32 Ec:1;         // UIC DME Error Code
+  UINT32 Rsvd1:30;
+  UINT32 Err:1;        // UIC DME Error
+} UFS_HC_UECDME;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.10 Offset 4Ch: UTRIACR - UTP Transfer Request Interrupt Aggregation Control Register
+//
+typedef struct {
+  UINT8  IaToVal;      // Interrupt aggregation timeout value
+
+  UINT8  IacTh:5;      // Interrupt aggregation counter threshold
+  UINT8  Rsvd1:3;
+
+  UINT8  Ctr:1;        // Counter and Timer Reset
+  UINT8  Rsvd2:3;
+  UINT8  Iasb:1;       // Interrupt aggregation status bit
+  UINT8  Rsvd3:3;
+
+  UINT8  IapwEn:1;     // Interrupt aggregation parameter write enable
+  UINT8  Rsvd4:6;
+  UINT8  IaEn:1;       // Interrupt Aggregation Enable/Disable
+} UFS_HC_UTRIACR;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.1 Offset 50h: UTRLBA - UTP Transfer Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtrlBa:22;    // UTP Transfer Request List Base Address
+} UFS_HC_UTRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.2 Offset 54h: UTRLBAU - UTP Transfer Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.3 Offset 58h: UTRLDBR - UTP Transfer Request List Door Bell Register
+//
+#define UFS_HC_UTRLDBR UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.4 Offset 5Ch: UTRLCLR - UTP Transfer Request List CLear Register
+//
+#define UFS_HC_UTRLCLR UINT32
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.4.5 Offset 60h: UTRLRSR - UTP Transfer Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtrlRsr:1;    // UTP Transfer Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.5.1 Offset 70h: UTMRLBA - UTP Task Management Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtmrlBa:22;   // UTP Task Management Request List Base Address
+} UFS_HC_UTMRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.2 Offset 74h: UTMRLBAU - UTP Task Management Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTMRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.5.3 Offset 78h: UTMRLDBR - UTP Task Management Request List Door Bell Register
+//
+typedef struct {
+  UINT32 UtmrlDbr:8;   // UTP Task Management Request List Door bell Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLDBR;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.4 Offset 7Ch: UTMRLCLR - UTP Task Management Request List CLear Register
+//
+typedef struct {
+  UINT32 UtmrlClr:8;   // UTP Task Management List Clear Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLCLR;
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.5.5 Offset 80h: UTMRLRSR - UTP Task Management Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtmrlRsr:1;   // UTP Task Management Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTMRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.6.1 Offset 90h: UICCMD - UIC Command
+//
+typedef struct {
+  UINT32 CmdOp:8;      // Command Opcode
+  UINT32 Rsvd1:24;
+} UFS_HC_UICCMD;
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 94h: UICCMDARG1 - UIC Command Argument 1
+//
+#define UFS_HC_UICCMD_ARG1 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 98h: UICCMDARG2 - UIC Command Argument 2
+//
+#define UFS_HC_UICCMD_ARG2 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 9ch: UICCMDARG3 - UIC Command Argument 3
+//
+#define UFS_HC_UICCMD_ARG3 UINT32
+
+//
+// UIC command opcodes
+//
+typedef enum {
+  UfsUicDmeGet            = 0x01,
+  UfsUicDmeSet            = 0x02,
+  UfsUicDmePeerGet        = 0x03,
+  UfsUicDmePeerSet        = 0x04,
+  UfsUicDmePwrOn          = 0x10,
+  UfsUicDmePwrOff         = 0x11,
+  UfsUicDmeEnable         = 0x12,
+  UfsUicDmeReset          = 0x14,
+  UfsUicDmeEndpointReset  = 0x15,
+  UfsUicDmeLinkStartup    = 0x16,
+  UfsUicDmeHibernateEnter = 0x17,
+  UfsUicDmeHibernateExit  = 0x18,
+  UfsUicDmeTestMode       = 0x1A
+} UFS_UIC_OPCODE;
+
+//
+// UTP Transfer Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Dd:2;                /* Data Direction */
+  UINT32 Rsvd2:1;
+  UINT32 Ct:4;                /* Command Type */
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4
+  //
+  UINT32 Rsvd6:7;
+  UINT32 UcdBa:25;            /* UTP Command Descriptor Base Address */
+  
+  //
+  // DW5
+  //
+  UINT32 UcdBaU;              /* UTP Command Descriptor Base Address Upper 32-bits */
+ 
+  //
+  // DW6
+  //
+  UINT16 RuL;                 /* Response UPIU Length */  
+  UINT16 RuO;                 /* Response UPIU Offset */
+
+  //
+  // DW7
+  //
+  UINT16 PrdtL;               /* PRDT Length */  
+  UINT16 PrdtO;               /* PRDT Offset */
+} UTP_TRD;
+
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:2;
+  UINT32 DbAddr:30;           /* Data Base Address */
+  
+  //
+  // DW1
+  //
+  UINT32 DbAddrU;             /* Data Base Address Upper 32-bits */
+ 
+  //
+  // DW2
+  //
+  UINT32 Rsvd2;
+
+  //
+  // DW3
+  //
+  UINT32 DbCount:18;          /* Data Byte Count */
+  UINT32 Rsvd3:14;
+} UTP_TR_PRD;
+
+//
+// UFS 2.0 Spec Section 10.5.3 - UTP Command UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x01*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Rsvd3;
+  UINT8  Rsvd4;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd5;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 ExpDataTranLen;      /* Expected Data Transfer Length - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Cdb[16];
+} UTP_COMMAND_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.4 - UTP Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x21*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Response;            /* Response */
+  UINT8  Status;              /* Status */
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 ResTranCount;        /* Residual Transfer Count - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd3[16];
+
+  //
+  // Data Segment - Sense Data
+  //
+  UINT16 SenseDataLen;        /* Sense Data Length - Big Endian */
+  UINT8  SenseData[18];       /* Sense Data */
+} UTP_RESPONSE_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.5 - UTP Data-Out UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x02*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be sent out
+  //
+  //UINT8  Data[];            /* Data to be sent out, maximum is 65535 bytes */
+} UTP_DATA_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.6 - UTP Data-In UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x22*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_DATA_IN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.7 - UTP Ready-To-Transfer UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x31*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_RDY_TO_TRAN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.8 - UTP Task Management Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x04*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1;
+  UINT8  TskManFunc;          /* Task Management Function */
+  UINT8  Rsvd2[2];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 InputParam1;         /* Input Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 InputParam2;         /* Input Parameter 2 - Big Endian */
+
+  //
+  // DW5
+  //
+  UINT32 InputParam3;         /* Input Parameter 3 - Big Endian */
+
+  //
+  // DW6 - DW7
+  //
+  UINT8  Rsvd4[8];
+} UTP_TM_REQ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.9 - UTP Task Management Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x24*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Resp;                /* Response */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 OutputParam1;        /* Output Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 OutputParam2;        /* Output Parameter 2 - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd4[12];
+} UTP_TM_RESP_UPIU;
+
+//
+// UTP Task Management Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Rsvd2:7;
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4 - DW11
+  //
+  UTP_TM_REQ_UPIU TmReq;      /* Task Management Request UPIU */
+  
+  //
+  // DW12 - DW19
+  //
+  UTP_TM_RESP_UPIU TmResp;    /* Task Management Response UPIU */
+} UTP_TMRD;
+
+
+typedef struct {
+  UINT8  Opcode;
+  UINT8  DescId;
+  UINT8  Index;
+  UINT8  Selector;
+  UINT16 Rsvd1;
+  UINT16 Length;
+  UINT32 Value;
+  UINT32 Rsvd2;
+} UTP_UPIU_TSF;
+
+//
+// UFS 2.0 Spec Section 10.5.10 - UTP Query Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x16*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        Rsvd3[2];
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        Rsvd4;
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd5[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8  Data[];            /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_REQ_UPIU;
+
+#define QUERY_FUNC_STD_READ_REQ     0x01
+#define QUERY_FUNC_STD_WRITE_REQ    0x81
+
+typedef enum {
+  UtpQueryFuncOpcodeNop      = 0x00,
+  UtpQueryFuncOpcodeRdDesc   = 0x01,
+  UtpQueryFuncOpcodeWrDesc   = 0x02,
+  UtpQueryFuncOpcodeRdAttr   = 0x03,
+  UtpQueryFuncOpcodeWrAttr   = 0x04,
+  UtpQueryFuncOpcodeRdFlag   = 0x05,
+  UtpQueryFuncOpcodeSetFlag  = 0x06,
+  UtpQueryFuncOpcodeClrFlag  = 0x07,
+  UtpQueryFuncOpcodeTogFlag  = 0x08
+} UTP_QUERY_FUNC_OPCODE;
+
+//
+// UFS 2.0 Spec Section 10.5.11 - UTP Query Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x36*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        QueryResp;     /* Query Response */
+  UINT8        Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        DevInfo;       /* Device Information */
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd4[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8      Data[];        /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_RESP_UPIU;
+
+typedef enum {
+  UfsUtpQueryResponseSuccess             = 0x00,
+  UfsUtpQueryResponseParamNotReadable    = 0xF6,
+  UfsUtpQueryResponseParamNotWriteable   = 0xF7,  
+  UfsUtpQueryResponseParamAlreadyWritten = 0xF8,
+  UfsUtpQueryResponseInvalidLen          = 0xF9,
+  UfsUtpQueryResponseInvalidVal          = 0xFA,
+  UfsUtpQueryResponseInvalidSelector     = 0xFB,
+  UfsUtpQueryResponseInvalidIndex        = 0xFC,
+  UfsUtpQueryResponseInvalidIdn          = 0xFD,
+  UfsUtpQueryResponseInvalidOpc          = 0xFE,
+  UfsUtpQueryResponseGeneralFailure      = 0xFF
+} UTP_QUERY_RESP_CODE;
+
+//
+// UFS 2.0 Spec Section 10.5.12 - UTP Reject UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x3F*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Response;            /* Response - 0x01 */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT8  HdrSts;              /* Basic Header Status */
+  UINT8  Rsvd3;
+  UINT8  E2ESts;              /* End-to-End Status */
+  UINT8  Rsvd4;
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd5[16];
+} UTP_REJ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.13 - UTP NOP OUT UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x00*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.14 - UTP NOP IN UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x20*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[2];
+  UINT8  Resp;                /* Response - 0x00 */
+  UINT8  Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_IN_UPIU;
+
+//
+// UFS Descriptors
+//
+typedef enum {
+  UfsDeviceDesc     = 0x00,
+  UfsConfigDesc     = 0x01,
+  UfsUnitDesc       = 0x02,
+  UfsInterConnDesc  = 0x04,
+  UfsStringDesc     = 0x05,
+  UfsGeometryDesc   = 0x07,
+  UfsPowerDesc      = 0x08
+} UFS_DESC_IDN;
+
+//
+// UFS 2.0 Spec Section 14.1.6.2 - Device Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Device;
+  UINT8  DevClass;
+  UINT8  DevSubClass;
+  UINT8  Protocol;
+  UINT8  NumLun;
+  UINT8  NumWLun;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  SecurityLun;
+  UINT8  BgOpsTermLat;
+  UINT8  InitActiveIccLevel;
+  UINT16 SpecVersion;
+  UINT16 ManufactureDate;
+  UINT8  ManufacturerName;
+  UINT8  ProductName;
+  UINT8  SerialName;
+  UINT8  OemId;
+  UINT16 ManufacturerId;
+  UINT8  Ud0BaseOffset;
+  UINT8  Ud0ConfParamLen;
+  UINT8  DevRttCap;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd1[17];
+  UINT8  Rsvd2[16];
+} UFS_DEV_DESC;
+
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Rsvd1;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  InitActiveIccLevel;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd2[5];
+} UFS_CONFIG_DESC_GEN_HEADER;
+
+typedef struct {
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  MemType;
+  UINT32 NumAllocUnits;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT8  ProvisionType;
+  UINT16 CtxCap;
+  UINT8  Rsvd1[3];
+} UFS_UNIT_DESC_CONFIG_PARAMS;
+
+//
+// UFS 2.0 Spec Section 14.1.6.3 - Configuration Descriptor
+//
+typedef struct {
+  UFS_CONFIG_DESC_GEN_HEADER  Header;
+  UFS_UNIT_DESC_CONFIG_PARAMS UnitDescConfParams[8];
+} UFS_CONFIG_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.4 - Geometry Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  MediaTech;
+  UINT8  Rsvd1;
+  UINT64 TotalRawDevCapacity;
+  UINT8  Rsvd2;
+  UINT32 SegSize;
+  UINT8  AllocUnitSize;
+  UINT8  MinAddrBlkSize;
+  UINT8  OptReadBlkSize;
+  UINT8  OptWriteBlkSize;
+  UINT8  MaxInBufSize;
+  UINT8  MaxOutBufSize;
+  UINT8  RpmbRwSize;
+  UINT8  Rsvd3;
+  UINT8  DataOrder;
+  UINT8  MaxCtxIdNum;
+  UINT8  SysDataTagUnitSize;
+  UINT8  SysDataResUnitSize;
+  UINT8  SupSecRemovalTypes;
+  UINT16 SupMemTypes;
+  UINT32 SysCodeMaxNumAllocUnits;
+  UINT16 SupCodeCapAdjFac;
+  UINT32 NonPersMaxNumAllocUnits;
+  UINT16 NonPersCapAdjFac;
+  UINT32 Enhance1MaxNumAllocUnits;
+  UINT16 Enhance1CapAdjFac;
+  UINT32 Enhance2MaxNumAllocUnits;
+  UINT16 Enhance2CapAdjFac;
+  UINT32 Enhance3MaxNumAllocUnits;
+  UINT16 Enhance3CapAdjFac;
+  UINT32 Enhance4MaxNumAllocUnits;
+  UINT16 Enhance4CapAdjFac;
+} UFS_GEOMETRY_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.5 - Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT16 CtxCap;
+  UINT8  LargeUnitGranularity;
+} UFS_UNIT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.6 - RPMB Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  Rsvd2;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT8  Rsvd3[3];
+} UFS_RPMB_UNIT_DESC;
+
+typedef struct {
+  UINT16 Value:10;
+  UINT16 Rsvd1:4;
+  UINT16 Unit:2;
+} UFS_POWER_PARAM_ELEMENT;
+
+//
+// UFS 2.0 Spec Section 14.1.6.7 - Power Parameter Descriptor
+//
+typedef struct {
+  UINT8                    Length;
+  UINT8                    DescType;
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVcc[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ2[16];
+} UFS_POWER_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.8 - InterConnect Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT16 UniProVer;
+  UINT16 MphyVer;
+} UFS_INTER_CONNECT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.9 - 14.1.6.12 - String Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  CHAR16 Unicode[126];
+} UFS_STRING_DESC;
+
+//
+// UFS 2.0 Spec Section 14.2 - Flags
+//
+typedef enum {
+  UfsFlagDevInit         = 0x01,
+  UfsFlagPermWpEn        = 0x02,
+  UfsFlagPowerOnWpEn     = 0x03,
+  UfsFlagBgOpsEn         = 0x04,
+  UfsFlagPurgeEn         = 0x06,
+  UfsFlagPhyResRemoval   = 0x08,
+  UfsFlagBusyRtc         = 0x09,
+  UfsFlagPermDisFwUpdate = 0x0B    
+} UFS_FLAGS_IDN;
+
+//
+// UFS 2.0 Spec Section 14.2 - Attributes
+//
+typedef enum {
+  UfsAttrBootLunEn         = 0x00,
+  UfsAttrCurPowerMode      = 0x02,
+  UfsAttrActiveIccLevel    = 0x03,
+  UfsAttrOutOfOrderDataEn  = 0x04,
+  UfsAttrBgOpStatus        = 0x05,
+  UfsAttrPurgeStatus       = 0x06,
+  UfsAttrMaxDataInSize     = 0x07,
+  UfsAttrMaxDataOutSize    = 0x08,
+  UfsAttrDynCapNeeded      = 0x09,
+  UfsAttrRefClkFreq        = 0x0a,
+  UfsAttrConfigDescLock    = 0x0b,
+  UfsAttrMaxNumOfRtt       = 0x0c,
+  UfsAttrExceptionEvtCtrl  = 0x0d,
+  UfsAttrExceptionEvtSts   = 0x0e,
+  UfsAttrSecondsPassed     = 0x0f,
+  UfsAttrContextConf       = 0x10,
+  UfsAttrCorrPrgBlkNum     = 0x11
+} UFS_ATTR_IDN;
+
+typedef enum {
+  UfsNoData                = 0,
+  UfsDataOut               = 1,
+  UfsDataIn                = 2,
+  UfsDdReserved
+} UFS_DATA_DIRECTION;
+
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c
new file mode 100644
index 0000000000..7da21110e3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/ComponentName.c
@@ -0,0 +1,222 @@
+/** @file
+
+  Copyright (c) 2014, 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 "UfsPassThru.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUfsPassThruComponentName = {
+  UfsPassThruComponentNameGetDriverName,
+  UfsPassThruComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUfsPassThruComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UfsPassThruComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UfsPassThruComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsPassThruDriverNameTable[] = {
+  { 
+    "eng;en",
+    L"Universal Flash Storage (UFS) Pass Thru Driver"
+  },
+  { 
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUfsPassThruControllerNameTable[] = {
+  { 
+    "eng;en",
+    L"Universal Flash Storage (UFS) Host Controller"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsPassThruDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUfsPassThruComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+
+  if (Language == NULL || ControllerName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing Controller Handle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gUfsPassThruDriverBinding.DriverBindingHandle,
+             &gEdkiiUfsHostControllerProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUfsPassThruControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUfsPassThruComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c
new file mode 100644
index 0000000000..7c831e92d0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.c
@@ -0,0 +1,1108 @@
+/** @file
+
+  Copyright (c) 2014 - 2015, 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 "UfsPassThru.h"
+
+//
+// Template for Ufs Pass Thru private data.
+//
+UFS_PASS_THRU_PRIVATE_DATA gUfsPassThruTemplate = {
+  UFS_PASS_THRU_SIG,              // Signature
+  NULL,                           // Handle  
+  {                               // ExtScsiPassThruMode
+    0xFFFFFFFF,
+    EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_NONBLOCKIO,
+    sizeof (UINTN)
+  },
+  {                               // ExtScsiPassThru
+    NULL,
+    UfsPassThruPassThru,
+    UfsPassThruGetNextTargetLun,
+    UfsPassThruBuildDevicePath,
+    UfsPassThruGetTargetLun,
+    UfsPassThruResetChannel,
+    UfsPassThruResetTargetLun,
+    UfsPassThruGetNextTarget
+  },
+  0,                              // UfsHostController
+  0,                              // UfsHcBase
+  0,                              // Capabilities
+  0,                              // TaskTag
+  0,                              // UtpTrlBase
+  0,                              // Nutrs
+  0,                              // TrlMapping
+  0,                              // UtpTmrlBase
+  0,                              // Nutmrs
+  0,                              // TmrlMapping
+  {                               // Luns
+    {
+      UFS_LUN_0,                      // Ufs Common Lun 0
+      UFS_LUN_1,                      // Ufs Common Lun 1
+      UFS_LUN_2,                      // Ufs Common Lun 2
+      UFS_LUN_3,                      // Ufs Common Lun 3
+      UFS_LUN_4,                      // Ufs Common Lun 4
+      UFS_LUN_5,                      // Ufs Common Lun 5
+      UFS_LUN_6,                      // Ufs Common Lun 6
+      UFS_LUN_7,                      // Ufs Common Lun 7
+      UFS_WLUN_REPORT_LUNS,           // Ufs Reports Luns Well Known Lun
+      UFS_WLUN_UFS_DEV,               // Ufs Device Well Known Lun
+      UFS_WLUN_BOOT,                  // Ufs Boot Well Known Lun
+      UFS_WLUN_RPMB                   // RPMB Well Known Lun
+    },
+    0x0000,                           // By default don't expose any Luns.
+    0x0
+  },
+  NULL,                           // TimerEvent
+  {                               // Queue
+    NULL,
+    NULL
+  }
+};
+
+EFI_DRIVER_BINDING_PROTOCOL gUfsPassThruDriverBinding = {
+  UfsPassThruDriverBindingSupported,
+  UfsPassThruDriverBindingStart,
+  UfsPassThruDriverBindingStop,
+  0x10,
+  NULL,
+  NULL
+};
+
+UFS_DEVICE_PATH    mUfsDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_UFS_DP,
+    {
+      (UINT8) (sizeof (UFS_DEVICE_PATH)),
+      (UINT8) ((sizeof (UFS_DEVICE_PATH)) >> 8)
+    }
+  },
+  0,
+  0
+};
+
+UINT8 mUfsTargetId[TARGET_MAX_BYTES];
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((Packet == NULL) || (Packet->Cdb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Don't support variable length CDB
+  //
+  if ((Packet->CdbLength != 6) && (Packet->CdbLength != 10) &&
+      (Packet->CdbLength != 12) && (Packet->CdbLength != 16)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->InDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->OutDataBuffer, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((This->Mode->IoAlign > 1) && !IS_ALIGNED(Packet->SenseData, This->Mode->IoAlign)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // For UFS 2.0 compatible device, 0 is always used to represent the location of the UFS device.
+  //
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if ((Target == NULL) || (CompareMem(Target, mUfsTargetId, TARGET_MAX_BYTES) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // UFS 2.0 spec Section 10.6.7 - Translation of 8-bit UFS LUN to 64-bit SCSI LUN Address
+  // 0xC1 in the first 8 bits of the 64-bit address indicates a well known LUN address in the SAM SCSI format.
+  // The second 8 bits of the 64-bit address saves the corresponding 8-bit UFS LUN.
+  //
+  if ((UINT8)Lun == UFS_WLUN_PREFIX) {
+    UfsLun = BIT7 | (((UINT8*)&Lun)[1] & 0xFF);
+  } else if ((UINT8)Lun == 0) {
+    UfsLun = ((UINT8*)&Lun)[1] & 0xFF;
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = UfsExecScsiCmds (Private, UfsLun, Packet, Event);
+
+  return Status;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+  UINT16                          Next;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  if (Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UfsLun = 0;
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0xFF);
+  if (CompareMem (*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    //
+    // If the array is all 0xFF's, return the first exposed Lun to caller.
+    //
+    SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+    for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+      if ((Private->Luns.BitMask & (BIT0 << Index)) != 0) {
+        UfsLun = Private->Luns.Lun[Index];
+        break;
+      }
+    }
+    if (Index != UFS_MAX_LUNS) {
+      *Lun = 0;
+      if ((UfsLun & BIT7) == BIT7) {
+        ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+        ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+      } else {
+        ((UINT8*)Lun)[1] = UfsLun;
+      }
+      return EFI_SUCCESS;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if (CompareMem (*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    if (((UINT8*)Lun)[0] == UFS_WLUN_PREFIX) {
+      UfsLun = BIT7 | (((UINT8*)Lun)[1] & 0xFF);
+    } else if (((UINT8*)Lun)[0] == 0) {
+      UfsLun = ((UINT8*)Lun)[1] & 0xFF;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+
+    for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+      if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+        continue;
+      }
+
+      if (Private->Luns.Lun[Index] != UfsLun) {
+        continue;
+      }
+
+      for (Next = Index + 1; Next < UFS_MAX_LUNS; Next++) {
+        if ((Private->Luns.BitMask & (BIT0 << Next)) != 0) {
+          UfsLun = Private->Luns.Lun[Next];
+          break;
+        }
+      }
+
+      if (Next == UFS_MAX_LUNS) {
+        return EFI_NOT_FOUND;
+      } else {
+        break;
+      }
+    }
+
+    if (Index != UFS_MAX_LUNS) {
+      *Lun = 0;
+      if ((UfsLun & BIT7) == BIT7) {
+        ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+        ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+      } else {
+        ((UINT8*)Lun)[1] = UfsLun;
+      }
+      return EFI_SUCCESS;
+    } else {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  EFI_DEV_PATH                    *DevicePathNode;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0x00);
+  if (CompareMem (Target, mUfsTargetId, TARGET_MAX_BYTES) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT8)Lun == UFS_WLUN_PREFIX) {
+    UfsLun = BIT7 | (((UINT8*)&Lun)[1] & 0xFF);
+  } else if ((UINT8)Lun == 0) {
+    UfsLun = ((UINT8*)&Lun)[1] & 0xFF;
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_NOT_FOUND;
+  }
+
+  DevicePathNode = AllocateCopyPool (sizeof (UFS_DEVICE_PATH), &mUfsDevicePathTemplate);
+  if (DevicePathNode == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DevicePathNode->Ufs.Pun = 0;
+  DevicePathNode->Ufs.Lun = UfsLun;
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathNode;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA      *Private;
+  EFI_DEV_PATH                    *DevicePathNode;
+  UINT8                           Pun;
+  UINT8                           UfsLun;
+  UINT16                          Index;
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Validate parameters passed in.
+  //
+  if (DevicePath == NULL || Target == NULL || Lun == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the DevicePath belongs to SCSI_DEVICE_PATH
+  //
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) || (DevicePath->SubType != MSG_UFS_DP) ||
+      (DevicePathNodeLength(DevicePath) != sizeof(SCSI_DEVICE_PATH))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DevicePathNode = (EFI_DEV_PATH *) DevicePath;
+
+  Pun    = (UINT8) DevicePathNode->Ufs.Pun;
+  UfsLun = (UINT8) DevicePathNode->Ufs.Lun;
+
+  if (Pun != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  for (Index = 0; Index < UFS_MAX_LUNS; Index++) {
+    if ((Private->Luns.BitMask & (BIT0 << Index)) == 0) {
+      continue;
+    }
+  
+    if (Private->Luns.Lun[Index] == UfsLun) {
+      break;
+    }
+  }
+
+  if (Index == UFS_MAX_LUNS) {
+    return EFI_NOT_FOUND;
+  }
+
+  SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+  *Lun = 0;
+  if ((UfsLun & BIT7) == BIT7) {
+    ((UINT8*)Lun)[0] = UFS_WLUN_PREFIX;
+    ((UINT8*)Lun)[1] = UfsLun & ~BIT7;
+  } else {
+    ((UINT8*)Lun)[1] = UfsLun;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset channel operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  )
+{
+  //
+  // Return success directly then upper layer driver could think reset target LUN operation is done.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  )
+{
+  if (Target == NULL || *Target == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  SetMem (mUfsTargetId, TARGET_MAX_BYTES, 0xFF);
+  if (CompareMem(*Target, mUfsTargetId, TARGET_MAX_BYTES) == 0) {
+    SetMem (*Target, TARGET_MAX_BYTES, 0x00);
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN EFI_HANDLE                        Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_DEVICE_PATH_PROTOCOL             *ParentDevicePath;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHostController;
+
+  //
+  // Ufs Pass Thru driver is a device driver, and should ingore the
+  // "RemainingDevicePath" according to UEFI spec
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID *) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+  //
+  // Close the protocol because we don't use it here
+  //
+  gBS->CloseProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEdkiiUfsHostControllerProtocolGuid,
+                  (VOID **) &UfsHostController,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // EFI_ALREADY_STARTED is also an error
+    //
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEdkiiUfsHostControllerProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+        
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL        *This,
+  IN EFI_HANDLE                         Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL           *RemainingDevicePath
+  )
+{
+  EFI_STATUS                            Status;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *UfsHc;
+  UFS_PASS_THRU_PRIVATE_DATA            *Private;
+  UINTN                                 UfsHcBase;
+  UINT32                                Index;
+  UFS_CONFIG_DESC                       Config;
+
+  Status    = EFI_SUCCESS;
+  UfsHc     = NULL;
+  Private   = NULL;
+  UfsHcBase = 0;
+
+  DEBUG ((EFI_D_INFO, "==UfsPassThru Start== Controller = %x\n", Controller));
+
+  Status  = gBS->OpenProtocol (
+                   Controller,
+                   &gEdkiiUfsHostControllerProtocolGuid,
+                   (VOID **) &UfsHc,
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Open Ufs Host Controller Protocol Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Get the UFS Host Controller MMIO Bar Base Address.
+  //
+  Status = UfsHc->GetUfsHcMmioBar (UfsHc, &UfsHcBase);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Get Ufs Host Controller Mmio Bar Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Initialize Ufs Pass Thru private data for managed UFS Host Controller.
+  //
+  Private = AllocateCopyPool (sizeof (UFS_PASS_THRU_PRIVATE_DATA), &gUfsPassThruTemplate);
+  if (Private == NULL) {
+    DEBUG ((EFI_D_ERROR, "Unable to allocate Ufs Pass Thru private data\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  Private->ExtScsiPassThru.Mode = &Private->ExtScsiPassThruMode;
+  Private->UfsHostController    = UfsHc;
+  Private->UfsHcBase            = UfsHcBase;
+  InitializeListHead (&Private->Queue);
+
+  //
+  // Initialize UFS Host Controller H/W.
+  //
+  Status = UfsControllerInit (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Host Controller Initialization Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // UFS 2.0 spec Section 13.1.3.3:
+  // At the end of the UFS Interconnect Layer initialization on both host and device side, 
+  // the host shall send a NOP OUT UPIU to verify that the device UTP Layer is ready. 
+  //
+  Status = UfsExecNopCmds (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Sending NOP IN command Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // The host enables the device initialization completion by setting fDeviceInit flag.
+  //
+  Status = UfsSetFlag (Private, UfsFlagDevInit);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Set fDeviceInit Flag Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Get Ufs Device's Lun Info by reading Configuration Descriptor.
+  //
+  Status = UfsRwDeviceDesc (Private, TRUE, UfsConfigDesc, 0, 0, &Config, sizeof (UFS_CONFIG_DESC));
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Get Configuration Descriptor Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  //
+  // Check if 8 common luns are active and set corresponding bit mask.
+  // TODO: Parse device descriptor to decide if exposing RPMB LUN to upper layer for authentication access.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    if (Config.UnitDescConfParams[Index].LunEn != 0) {
+      Private->Luns.BitMask |= (BIT0 << Index);
+      DEBUG ((EFI_D_INFO, "Ufs Lun %d is enabled\n", Index));
+    }
+  }
+
+  //
+  // Start the asynchronous interrupt monitor
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ProcessAsyncTaskList,
+                  Private,
+                  &Private->TimerEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Create Async Tasks Event Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->SetTimer (
+                  Private->TimerEvent,
+                  TimerPeriodic,
+                  UFS_HC_ASYNC_TIMER
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ufs Set Periodic Timer Error, Status = %r\n", Status));
+    goto Error;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &(Private->ExtScsiPassThru)
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+
+Error:
+  if (Private != NULL) {
+    if (Private->TmrlMapping != NULL) {
+      UfsHc->Unmap (UfsHc, Private->TmrlMapping);  
+    }
+    if (Private->UtpTmrlBase != NULL) {
+      UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutmrs * sizeof (UTP_TMRD)), Private->UtpTmrlBase);
+    }
+
+    if (Private->TrlMapping != NULL) {
+      UfsHc->Unmap (UfsHc, Private->TrlMapping);
+    }
+    if (Private->UtpTrlBase != NULL) {
+      UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutrs * sizeof (UTP_TMRD)), Private->UtpTrlBase);
+    }
+
+    if (Private->TimerEvent != NULL) {
+      gBS->CloseEvent (Private->TimerEvent);
+    }
+
+    FreePool (Private);
+  }
+
+  if (UfsHc != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEdkiiUfsHostControllerProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+
+  return Status;
+}
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *This,
+  IN  EFI_HANDLE                        Controller,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                            Status;
+  UFS_PASS_THRU_PRIVATE_DATA            *Private;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL       *ExtScsiPassThru;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL    *UfsHc;
+  UFS_PASS_THRU_TRANS_REQ               *TransReq;
+  LIST_ENTRY                            *Entry;
+  LIST_ENTRY                            *NextEntry;
+
+  DEBUG ((EFI_D_INFO, "==UfsPassThru Stop== Controller Controller = %x\n", Controller));
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  (VOID **) &ExtScsiPassThru,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Private = UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS (ExtScsiPassThru);
+  UfsHc   = Private->UfsHostController;
+
+  //
+  // Cleanup the resources of I/O requests in the async I/O queue
+  //
+  if (!IsListEmpty(&Private->Queue)) {
+    EFI_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Private->Queue) {
+      TransReq  = UFS_PASS_THRU_TRANS_REQ_FROM_THIS (Entry);
+
+      //
+      // TODO: Should find/add a proper host adapter return status for this
+      // case.
+      //
+      TransReq->Packet->HostAdapterStatus =
+        EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR;
+
+      SignalCallerEvent (Private, TransReq);
+    }
+  }
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  &(Private->ExtScsiPassThru)
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Stop Ufs Host Controller
+  //
+  Status = UfsControllerStop (Private);
+  ASSERT_EFI_ERROR (Status);
+
+  if (Private->TmrlMapping != NULL) {
+    UfsHc->Unmap (UfsHc, Private->TmrlMapping);
+  }
+  if (Private->UtpTmrlBase != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutmrs * sizeof (UTP_TMRD)), Private->UtpTmrlBase);
+  }
+
+  if (Private->TrlMapping != NULL) {
+    UfsHc->Unmap (UfsHc, Private->TrlMapping);
+  }
+  if (Private->UtpTrlBase != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (Private->Nutrs * sizeof (UTP_TMRD)), Private->UtpTrlBase);
+  }
+
+  if (Private->TimerEvent != NULL) {
+    gBS->CloseEvent (Private->TimerEvent);
+  }
+
+  FreePool (Private);
+
+  //
+  // Close protocols opened by UfsPassThru controller driver
+  //
+  gBS->CloseProtocol (
+         Controller,
+         &gEdkiiUfsHostControllerProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+
+/**
+  The user Entry Point for module UfsPassThru. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUfsPassThru (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUfsPassThruDriverBinding,
+             ImageHandle,
+             &gUfsPassThruComponentName,
+             &gUfsPassThruComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h
new file mode 100644
index 0000000000..4f7087f44f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.h
@@ -0,0 +1,799 @@
+/** @file
+
+  Copyright (c) 2014 - 2015, 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.
+
+**/
+
+#ifndef _UFS_PASS_THRU_H_
+#define _UFS_PASS_THRU_H_
+
+#include <Uefi.h>
+
+#include <Protocol/ScsiPassThruExt.h>
+#include <Protocol/UfsHostController.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/TimerLib.h>
+
+#include "UfsPassThruHci.h"
+
+#define UFS_PASS_THRU_SIG           SIGNATURE_32 ('U', 'F', 'S', 'P')
+
+//
+// Lun 0~7 is for 8 common luns. 
+// Lun 8~11 is for those 4 well known luns (Refer to UFS 2.0 spec Table 10.58 for details):
+//  Lun 8:  REPORT LUNS
+//  Lun 9:  UFS DEVICE
+//  Lun 10: BOOT
+//  Lun 11: RPMB
+//
+#define UFS_MAX_LUNS                12
+#define UFS_WLUN_PREFIX             0xC1
+
+typedef struct {
+  UINT8    Lun[UFS_MAX_LUNS];
+  UINT16   BitMask:12;              // Bit 0~7 is 1/1 mapping to common luns. Bit 8~11 is 1/1 mapping to well-known luns.
+  UINT16   Rsvd:4;
+} UFS_EXPOSED_LUNS;
+
+//
+// Iterate through the doule linked list. This is delete-safe.
+// Do not touch NextEntry
+//
+#define EFI_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead)            \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink;\
+      Entry != (ListHead); Entry = NextEntry, NextEntry = Entry->ForwardLink)
+
+typedef struct _UFS_PASS_THRU_PRIVATE_DATA {  
+  UINT32                              Signature;
+  EFI_HANDLE                          Handle;
+  EFI_EXT_SCSI_PASS_THRU_MODE         ExtScsiPassThruMode;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL     ExtScsiPassThru;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHostController;
+  UINTN                               UfsHcBase;
+  UINT32                              Capabilities;
+
+  UINT8                               TaskTag;
+
+  VOID                                *UtpTrlBase;
+  UINT8                               Nutrs;
+  VOID                                *TrlMapping;
+  VOID                                *UtpTmrlBase;
+  UINT8                               Nutmrs;
+  VOID                                *TmrlMapping;
+
+  UFS_EXPOSED_LUNS                    Luns;
+
+  //
+  // For Non-blocking operation.
+  //
+  EFI_EVENT                           TimerEvent;
+  LIST_ENTRY                          Queue;
+} UFS_PASS_THRU_PRIVATE_DATA;
+
+#define UFS_PASS_THRU_TRANS_REQ_SIG   SIGNATURE_32 ('U', 'F', 'S', 'T')
+
+typedef struct {
+  UINT32                                        Signature;
+  LIST_ENTRY                                    TransferList;
+
+  UINT8                                         Slot;
+  UTP_TRD                                       *Trd;
+  UINT32                                        CmdDescSize;
+  VOID                                          *CmdDescHost;
+  VOID                                          *CmdDescMapping;
+  VOID                                          *DataBufMapping;
+
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet;
+  UINT64                                        TimeoutRemain;
+  EFI_EVENT                                     CallerEvent;
+} UFS_PASS_THRU_TRANS_REQ;
+
+#define UFS_PASS_THRU_TRANS_REQ_FROM_THIS(a) \
+    CR(a, UFS_PASS_THRU_TRANS_REQ, TransferList, UFS_PASS_THRU_TRANS_REQ_SIG)
+
+#define UFS_TIMEOUT                   EFI_TIMER_PERIOD_SECONDS(3)
+#define UFS_HC_ASYNC_TIMER            EFI_TIMER_PERIOD_MILLISECONDS(1)
+
+#define ROUNDUP8(x) (((x) % 8 == 0) ? (x) : ((x) / 8 + 1) * 8)
+
+#define IS_ALIGNED(addr, size)        (((UINTN) (addr) & (size - 1)) == 0)
+
+#define UFS_PASS_THRU_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      UFS_PASS_THRU_PRIVATE_DATA, \
+      ExtScsiPassThru, \
+      UFS_PASS_THRU_SIG \
+      )
+
+typedef struct _UFS_DEVICE_MANAGEMENT_REQUEST_PACKET {
+  UINT64           Timeout;
+  VOID             *InDataBuffer;
+  VOID             *OutDataBuffer;
+  UINT8            Opcode;
+  UINT8            DescId;
+  UINT8            Index;
+  UINT8            Selector;
+  UINT32           InTransferLength;
+  UINT32           OutTransferLength;
+  UINT8            DataDirection;
+  UINT8            Ocs;
+} UFS_DEVICE_MANAGEMENT_REQUEST_PACKET;
+
+//
+// function prototype
+//
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided,
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  This function checks to see if the driver specified by This supports the device specified by
+  ControllerHandle. Drivers will typically use the device path attached to
+  ControllerHandle and/or the services from the bus I/O abstraction attached to
+  ControllerHandle to determine if the driver supports ControllerHandle. This function
+  may be called many times during platform initialization. In order to reduce boot times, the tests
+  performed by this function must be very small, and take as little time as possible to execute. This
+  function must not change the state of any hardware devices, and this function must be aware that the
+  device specified by ControllerHandle may already be managed by the same driver or a
+  different driver. This function must match its calls to AllocatePages() with FreePages(),
+  AllocatePool() with FreePool(), and OpenProtocol() with CloseProtocol().
+  Since ControllerHandle may have been previously started by the same driver, if a protocol is
+  already in the opened state, then it must not be closed with CloseProtocol(). This is required
+  to guarantee the state of ControllerHandle is not modified by this function.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For bus drivers, if this parameter is not NULL, then
+                                   the bus driver must determine if the bus controller specified
+                                   by ControllerHandle and the child controller specified
+                                   by RemainingDevicePath are both supported by this
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive access.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts a device controller or a bus controller.
+
+  The Start() function is designed to be invoked from the EFI boot service ConnectController().
+  As a result, much of the error checking on the parameters to Start() has been moved into this
+  common boot service. It is legal to call Start() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle
+                                   must support a protocol interface that supplies
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This
+                                   parameter is ignored by device drivers, and is optional for bus
+                                   drivers. For a bus driver, if this parameter is NULL, then handles
+                                   for all the children of Controller are created by this driver.
+                                   If this parameter is not NULL and the first Device Path Node is
+                                   not the End of Device Path Node, then only the handle for the
+                                   child device specified by the first Device Path Node of
+                                   RemainingDevicePath is created by this driver.
+                                   If the first Device Path Node of RemainingDevicePath is
+                                   the End of Device Path Node, no child handle is created by this
+                                   driver.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stops a device controller or a bus controller.
+
+  The Stop() function is designed to be invoked from the EFI boot service DisconnectController().
+  As a result, much of the error checking on the parameters to Stop() has been moved
+  into this common boot service. It is legal to call Stop() from other locations,
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must
+                                support a bus specific I/O protocol for the driver
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL
+                                if NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      Controller,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.
+
+  @param  This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                  the id of the SCSI device to send the SCSI Request Packet. Each
+                  transport driver may choose to utilize a subset of this size to suit the needs
+                  of transport target representation. For example, a Fibre Channel driver
+                  may use only 8 bytes (WWN) to represent an FC target.
+  @param  Lun     The LUN of the SCSI device to send the SCSI Request Packet.
+  @param  Packet  A pointer to the SCSI Request Packet to send to the SCSI device
+                  specified by Target and Lun.
+  @param  Event   If nonblocking I/O is not supported then Event is ignored, and blocking
+                  I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                  Event is not NULL and non blocking I/O is supported, then
+                  nonblocking I/O is performed, and Event will be signaled when the
+                  SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer.
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruPassThru (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                    *This,
+  IN UINT8                                              *Target,
+  IN UINT64                                             Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET     *Packet,
+  IN EFI_EVENT                                          Event OPTIONAL
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI
+  channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target On input, a pointer to the Target ID (an array of size
+                 TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+  @param  Lun    On input, a pointer to the LUN of a SCSI device present on the SCSI
+                 channel. On output, a pointer to the LUN of the next SCSI device present
+                 on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target,
+  IN OUT UINT64                          *Lun
+  );
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target     The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                     Target ID of the SCSI device for which a device path node is to be
+                     allocated and built. Transport drivers may chose to utilize a subset of
+                     this size to suit the representation of targets. For example, a Fibre
+                     Channel driver may use only 8 bytes (WWN) in the array to represent a
+                     FC target.
+  @param  Lun        The LUN of the SCSI device for which a device path node is to be
+                     allocated and built.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     specified by Target and Lun. This function is responsible for
+                     allocating the buffer DevicePath with the boot service
+                     AllocatePool(). It is the caller's responsibility to free
+                     DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruBuildDevicePath (
+  IN     EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN     UINT8                              *Target,
+  IN     UINT64                             Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL           **DevicePath
+  );
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  DevicePath A pointer to a single device path node that describes the SCSI device
+                     on the SCSI channel.
+  @param  Target     A pointer to the Target Array which represents the ID of a SCSI device
+                     on the SCSI channel.
+  @param  Lun        A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                 DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetTargetLun (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN  EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  OUT UINT8                              **Target,
+  OUT UINT64                             *Lun
+  );
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+
+  @param  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetChannel (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL   *This
+  );
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                 target port ID of the SCSI device containing the SCSI logical unit to
+                 reset. Transport drivers may chose to utilize a subset of this array to suit
+                 the representation of their targets.
+  @param  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                 specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN UINT8                              *Target,
+  IN UINT64                             Lun
+  );
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to
+  see if a SCSI device is actually present at that location on the SCSI channel.
+
+  @param  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param  Target (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                 On output, a pointer to the Target ID (an array of
+                 TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                 channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                 Target array retrieves the Target ID of the first SCSI device present on a
+                 SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target was not
+                                returned on a previous call to GetNextTarget().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+
+**/
+EFI_STATUS
+EFIAPI
+UfsPassThruGetNextTarget (
+  IN  EFI_EXT_SCSI_PASS_THRU_PROTOCOL    *This,
+  IN OUT UINT8                           **Target
+  );
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+  @param[in]      Event         If nonblocking I/O is not supported then Event is ignored, and blocking
+                                I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                Event is not NULL and non blocking I/O is supported, then
+                                nonblocking I/O is performed, and Event will be signaled when the
+                                SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     EFI_EVENT                                   Event    OPTIONAL
+  );
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The NVM Express Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  );
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  );
+
+/**
+  Allocate common buffer for host and UFS bus master access simultaneously.
+
+  @param[in]  Private                The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Size                   The length of buffer to be allocated.
+  @param[out] CmdDescHost            A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescPhyAddr         The resulting map address for the UFS bus master to use to access the hosts CmdDescHost.
+  @param[out] CmdDescMapping         A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS                The common buffer was allocated successfully.
+  @retval EFI_DEVICE_ERROR           The allocation fails.
+  @retval EFI_OUT_OF_RESOURCES       The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsAllocateAlignCommonBuffer (
+  IN     UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN     UINTN                         Size,
+     OUT VOID                          **CmdDescHost,
+     OUT EFI_PHYSICAL_ADDRESS          *CmdDescPhyAddr,
+     OUT VOID                          **CmdDescMapping
+  );
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  );
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  );
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PASS_THRU_PRIVATE_DATA       *Private
+  );
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+
+**/
+VOID
+EFIAPI
+ProcessAsyncTaskList (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  );
+
+/**
+  Internal helper function which will signal the caller event and clean up
+  resources.
+
+  @param[in] Private   The pointer to the UFS_PASS_THRU_PRIVATE_DATA data
+                       structure.
+  @param[in] TransReq  The pointer to the UFS_PASS_THRU_TRANS_REQ data
+                       structure.
+
+**/
+VOID
+EFIAPI
+SignalCallerEvent (
+  IN UFS_PASS_THRU_PRIVATE_DATA      *Private,
+  IN UFS_PASS_THRU_TRANS_REQ         *TransReq
+  );
+
+extern EFI_COMPONENT_NAME_PROTOCOL  gUfsPassThruComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gUfsPassThruComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gUfsPassThruDriverBinding;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni
new file mode 100644
index 0000000000..1e627bd9cd
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThru.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf
new file mode 100644
index 0000000000..c90c72f915
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf
@@ -0,0 +1,62 @@
+## @file
+# Description file for the Universal Flash Storage (UFS) Pass Thru driver.
+#
+# Copyright (c) 2014 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UfsPassThruDxe
+  MODULE_UNI_FILE                = UfsPassThru.uni
+  FILE_GUID                      = E7F1DFF9-DAB6-498A-9ADF-57F344EDDF57
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeUfsPassThru
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUfsPassThruDriverBinding              
+#  COMPONENT_NAME                =  gUfsPassThruComponentName              
+#
+
+[Sources]
+  ComponentName.c
+  UfsPassThru.c
+  UfsPassThru.h
+  UfsPassThruHci.c
+  UfsPassThruHci.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+  DevicePathLib
+  TimerLib
+
+[Protocols]
+  gEfiExtScsiPassThruProtocolGuid               ## BY_START
+  gEdkiiUfsHostControllerProtocolGuid           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UfsPassThruExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni
new file mode 100644
index 0000000000..771b7ba3b5
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c
new file mode 100644
index 0000000000..26986cbf26
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.c
@@ -0,0 +1,2343 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014 - 2015, 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 "UfsPassThru.h"
+
+/**
+  Read 32bits data from specified UFS MMIO register.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Offset        The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[out] Value         The data buffer to store.
+
+  @retval EFI_TIMEOUT       The operation is time out.
+  @retval EFI_SUCCESS       The operation succeeds.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsMmioRead32 (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     UINTN                        Offset,
+     OUT UINT32                       *Value
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+  EFI_STATUS                          Status;
+
+  UfsHc = Private->UfsHostController;
+
+  Status = UfsHc->Read (UfsHc, EfiUfsHcWidthUint32, Offset, 1, Value);
+
+  return Status;
+}
+
+/**
+  Write 32bits data to specified UFS MMIO register.
+
+  @param[in] Private        The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in] Offset         The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[in] Value          The data to write.
+
+  @retval EFI_TIMEOUT       The operation is time out.
+  @retval EFI_SUCCESS       The operation succeeds.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsMmioWrite32 (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINTN                        Offset,
+  IN  UINT32                       Value
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL  *UfsHc;
+  EFI_STATUS                          Status;
+
+  UfsHc = Private->UfsHostController;
+
+  Status = UfsHc->Write (UfsHc, EfiUfsHcWidthUint32, Offset, 1, &Value);
+
+  return Status;
+}
+
+/**
+  Wait for the value of the specified system memory set to the test value.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Offset        The offset within the UFS Host Controller MMIO space to start
+                            the memory operation.
+  @param[in]  MaskValue     The mask value of memory.
+  @param[in]  TestValue     The test value of memory.
+  @param[in]  Timeout       The time out value for wait memory set, uses 100ns as a unit.
+
+  @retval EFI_TIMEOUT       The system memory setting is time out.
+  @retval EFI_SUCCESS       The system memory is correct set.
+  @retval Others            The operation fails.
+
+**/
+EFI_STATUS
+UfsWaitMemSet (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINTN                        Offset,
+  IN  UINT32                       MaskValue,
+  IN  UINT32                       TestValue,
+  IN  UINT64                       Timeout
+  )
+{
+  UINT32     Value;
+  UINT64     Delay;
+  BOOLEAN    InfiniteWait;
+  EFI_STATUS Status;
+
+  if (Timeout == 0) {
+    InfiniteWait = TRUE;
+  } else {
+    InfiniteWait = FALSE;
+  }
+
+  Delay = DivU64x32 (Timeout, 10) + 1;
+
+  do {
+    //
+    // Access PCI MMIO space to see if the value is the tested one.
+    //
+    Status = UfsMmioRead32 (Private, Offset, &Value);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Value &= MaskValue;
+
+    if (Value == TestValue) {
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Stall for 1 microseconds.
+    //
+    MicroSecondDelay (1);
+
+    Delay--;
+
+  } while (InfiniteWait || (Delay > 0));
+
+  return EFI_TIMEOUT;
+}
+
+/**
+  Dump UIC command execution result for debugging.
+
+  @param[in]   UicOpcode  The executed UIC opcode.
+  @param[in]   Result     The result to be parsed.
+
+**/
+VOID
+DumpUicCmdExecResult (
+  IN  UINT8     UicOpcode,
+  IN  UINT8     Result
+  )
+{
+  if (UicOpcode <= UfsUicDmePeerSet) {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x02:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - INVALID_MIB_ATTRIBUTE_VALUE\n"));
+        break;
+      case 0x03:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - READ_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x04:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - WRITE_ONLY_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x05:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_INDEX\n"));
+        break;
+      case 0x06:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - LOCKED_MIB_ATTRIBUTE\n"));
+        break;
+      case 0x07:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BAD_TEST_FEATURE_INDEX\n"));
+        break;
+      case 0x08:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - PEER_COMMUNICATION_FAILURE\n"));
+        break; 
+      case 0x09:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - BUSY\n"));
+        break;
+      case 0x0A:
+        DEBUG ((EFI_D_VERBOSE, "UIC configuration command fails - DME_FAILURE\n"));
+        break;        
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  } else {
+    switch (Result) {
+      case 0x00:
+        break;
+      case 0x01:
+        DEBUG ((EFI_D_VERBOSE, "UIC control command fails - FAILURE\n"));
+        break;     
+      default :
+        ASSERT (FALSE);
+        break;
+    }
+  }
+}
+
+/**
+  Dump QUERY RESPONSE UPIU result for debugging.
+
+  @param[in]   Result  The result to be parsed.
+
+**/
+VOID
+DumpQueryResponseResult (
+  IN  UINT8     Result
+  )
+{
+  switch (Result) {
+    case 0xF6:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Readable\n"));
+      break;
+    case 0xF7:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Not Writeable\n"));
+      break;
+    case 0xF8:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Parameter Already Written\n"));
+      break;
+    case 0xF9:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Length\n"));
+      break;
+    case 0xFA:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Value\n"));
+      break;
+    case 0xFB:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Selector\n"));
+      break;
+    case 0xFC:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Index\n"));
+      break;
+    case 0xFD:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Idn\n"));
+      break;
+    case 0xFE:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with Invalid Opcode\n"));
+      break; 
+    case 0xFF:
+      DEBUG ((EFI_D_VERBOSE, "Query Response with General Failure\n"));
+      break;
+    default :
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Swap little endian to big endian.
+
+  @param[in, out] Buffer      The data buffer. In input, it contains little endian data.
+                              In output, it will become big endian.
+  @param[in]      BufferSize  The length of converted data.
+
+**/
+VOID
+SwapLittleEndianToBigEndian (
+  IN OUT UINT8         *Buffer,
+  IN     UINT32        BufferSize
+  )
+{
+  UINT32 Index;
+  UINT8  Temp;
+  UINT32 SwapCount;
+
+  SwapCount = BufferSize / 2;
+  for (Index = 0; Index < SwapCount; Index++) {
+    Temp = Buffer[Index];
+    Buffer[Index] = Buffer[BufferSize - 1 - Index];
+    Buffer[BufferSize - 1 - Index] = Temp;
+  }
+}
+
+/**
+  Fill TSF field of QUERY REQUEST UPIU.
+
+  @param[in, out] TsfBase      The base address of TSF field of QUERY REQUEST UPIU.
+  @param[in]      Opcode       The opcode of request.
+  @param[in]      DescId       The descriptor ID of request.
+  @param[in]      Index        The index of request.
+  @param[in]      Selector     The selector of request.
+  @param[in]      Length       The length of transferred data. The maximum is 4.
+  @param[in]      Value        The value of transferred data.
+
+**/
+VOID
+UfsFillTsfOfQueryReqUpiu (
+  IN OUT UTP_UPIU_TSF        *TsfBase,
+  IN     UINT8               Opcode,
+  IN     UINT8               DescId    OPTIONAL,
+  IN     UINT8               Index     OPTIONAL,
+  IN     UINT8               Selector  OPTIONAL,
+  IN     UINT16              Length    OPTIONAL,
+  IN     UINT32              Value     OPTIONAL
+  )
+{
+  ASSERT (TsfBase != NULL);
+  ASSERT (Opcode <= UtpQueryFuncOpcodeTogFlag);
+
+  TsfBase->Opcode   = Opcode;
+  if (Opcode != UtpQueryFuncOpcodeNop) {
+    TsfBase->DescId   = DescId;
+    TsfBase->Index    = Index;
+    TsfBase->Selector = Selector;
+
+    if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Length, sizeof (Length));
+      TsfBase->Length = Length;
+    }
+  
+    if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+      SwapLittleEndianToBigEndian ((UINT8*)&Value, sizeof (Value));
+      TsfBase->Value  = Value;
+    }
+  }
+}
+
+/**
+  Initialize COMMAND UPIU.
+
+  @param[in, out] Command         The base address of COMMAND UPIU.
+  @param[in]      Lun             The Lun on which the SCSI command is executed.
+  @param[in]      TaskTag         The task tag of request.
+  @param[in]      Cdb             The cdb buffer containing SCSI command.
+  @param[in]      CdbLength       The cdb length.
+  @param[in]      DataDirection   The direction of data transfer.
+  @param[in]      ExpDataTranLen  The expected transfer data length.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitCommandUpiu (
+  IN OUT UTP_COMMAND_UPIU              *Command,
+  IN     UINT8                         Lun,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         *Cdb,
+  IN     UINT8                         CdbLength,
+  IN     UFS_DATA_DIRECTION            DataDirection,
+  IN     UINT32                        ExpDataTranLen
+  )
+{
+  UINT8                   Flags;
+
+  ASSERT ((Command != NULL) && (Cdb != NULL));
+
+  //
+  // Task attribute is hard-coded to Ordered.
+  //
+  if (DataDirection == UfsDataIn) {
+    Flags = BIT0 | BIT6;
+  } else if (DataDirection == UfsDataOut) {
+    Flags = BIT0 | BIT5;
+  } else {
+    Flags = BIT0;
+  }
+
+  //
+  // Fill UTP COMMAND UPIU associated fields.
+  //
+  Command->TransCode = 0x01;
+  Command->Flags     = Flags;
+  Command->Lun       = Lun;
+  Command->TaskTag   = TaskTag;
+  Command->CmdSet    = 0x00;
+  SwapLittleEndianToBigEndian ((UINT8*)&ExpDataTranLen, sizeof (ExpDataTranLen));
+  Command->ExpDataTranLen = ExpDataTranLen;
+
+  CopyMem (Command->Cdb, Cdb, CdbLength);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UTP PRDT for data transfer.
+
+  @param[in] Prdt         The base address of PRDT.
+  @param[in] Buffer       The buffer to be read or written.
+  @param[in] BufferSize   The data size to be read or written.
+
+  @retval EFI_SUCCESS     The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitUtpPrdt (
+  IN  UTP_TR_PRD                       *Prdt,
+  IN  VOID                             *Buffer,
+  IN  UINT32                           BufferSize
+  )
+{
+  UINT32     PrdtIndex;
+  UINT32     RemainingLen;
+  UINT8      *Remaining;
+  UINTN      PrdtNumber;
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (((UINTN)Buffer & (BIT0 | BIT1)) == 0);
+
+  RemainingLen = BufferSize;
+  Remaining    = Buffer;
+  PrdtNumber   = (UINTN)DivU64x32 ((UINT64)BufferSize + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
+    if (RemainingLen < UFS_MAX_DATA_LEN_PER_PRD) {
+      Prdt[PrdtIndex].DbCount = (UINT32)RemainingLen - 1;
+    } else {
+      Prdt[PrdtIndex].DbCount = UFS_MAX_DATA_LEN_PER_PRD - 1;
+    }
+
+    Prdt[PrdtIndex].DbAddr  = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 2);
+    Prdt[PrdtIndex].DbAddrU = (UINT32)RShiftU64 ((UINT64)(UINTN)Remaining, 32);
+    RemainingLen -= UFS_MAX_DATA_LEN_PER_PRD;
+    Remaining    += UFS_MAX_DATA_LEN_PER_PRD;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize QUERY REQUEST UPIU.
+
+  @param[in, out] QueryReq      The base address of QUERY REQUEST UPIU.
+  @param[in]      TaskTag       The task tag of request.
+  @param[in]      Opcode        The opcode of request.
+  @param[in]      DescId        The descriptor ID of request.
+  @param[in]      Index         The index of request.
+  @param[in]      Selector      The selector of request.
+  @param[in]      DataSize      The data size to be read or written.
+  @param[in]      Data          The buffer to be read or written.
+
+  @retval EFI_SUCCESS           The initialization succeed.
+
+**/
+EFI_STATUS
+UfsInitQueryRequestUpiu (
+  IN OUT UTP_QUERY_REQ_UPIU            *QueryReq,
+  IN     UINT8                         TaskTag,
+  IN     UINT8                         Opcode,
+  IN     UINT8                         DescId,
+  IN     UINT8                         Index,
+  IN     UINT8                         Selector,
+  IN     UINTN                         DataSize   OPTIONAL,
+  IN     UINT8                         *Data      OPTIONAL
+  )
+{
+  ASSERT (QueryReq != NULL);
+
+  QueryReq->TransCode = 0x16;
+  QueryReq->TaskTag   = TaskTag;
+  if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeRdFlag) || (Opcode == UtpQueryFuncOpcodeRdAttr)) {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_READ_REQ;
+  } else {
+    QueryReq->QueryFunc = QUERY_FUNC_STD_WRITE_REQ;
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrAttr) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, *(UINT32*)Data);
+  } else if ((Opcode == UtpQueryFuncOpcodeRdDesc) || (Opcode == UtpQueryFuncOpcodeWrDesc)) {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, (UINT16)DataSize, 0);
+  } else {
+    UfsFillTsfOfQueryReqUpiu (&QueryReq->Tsf, Opcode, DescId, Index, Selector, 0, 0);
+  }
+
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    CopyMem (QueryReq + 1, Data, DataSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate COMMAND/RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Lun               The Lun on which the SCSI command is executed.
+  @param[in]  Packet            The pointer to the EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateScsiCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN     EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     UTP_TRD                                     *Trd,
+     OUT VOID                                        **CmdDescHost,
+     OUT VOID                                        **CmdDescMapping
+  )
+{
+  UINTN                             TotalLen;
+  UINTN                             PrdtNumber;
+  UTP_COMMAND_UPIU                  *CommandUpiu;
+  EFI_PHYSICAL_ADDRESS              CmdDescPhyAddr;
+  EFI_STATUS                        Status;
+  UINT32                            DataLen;
+  UFS_DATA_DIRECTION                DataDirection;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    DataLen       = Packet->InTransferLength;
+    DataDirection = UfsDataIn;
+  } else {
+    DataLen       = Packet->OutTransferLength;
+    DataDirection = UfsDataOut;
+  }
+
+  if (DataLen == 0) {
+    DataDirection = UfsNoData;
+  }
+
+  PrdtNumber = (UINTN)DivU64x32 ((UINT64)DataLen + UFS_MAX_DATA_LEN_PER_PRD - 1, UFS_MAX_DATA_LEN_PER_PRD);
+
+  TotalLen   = ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)) + PrdtNumber * sizeof (UTP_TR_PRD);
+
+  Status = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CommandUpiu = (UTP_COMMAND_UPIU*)*CmdDescHost;
+
+  UfsInitCommandUpiu (CommandUpiu, Lun, Private->TaskTag++, Packet->Cdb, Packet->CdbLength, DataDirection, DataLen);
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Response UPIU and the Physical Region Description Table
+  // *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)), sizeof (UINT32));
+  Trd->PrdtL  = (UINT16)PrdtNumber;
+  Trd->PrdtO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU))), sizeof (UINT32));
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate QUERY REQUEST/QUERY RESPONSE UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Packet            The pointer to the UFS_DEVICE_MANAGEMENT_REQUEST_PACKET data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+  @retval EFI_INVALID_PARAMETER The parameter passed in is invalid.
+
+**/
+EFI_STATUS
+UfsCreateDMCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA            *Private,
+  IN     UFS_DEVICE_MANAGEMENT_REQUEST_PACKET  *Packet,
+  IN     UTP_TRD                               *Trd,
+     OUT VOID                                  **CmdDescHost,
+     OUT VOID                                  **CmdDescMapping
+  )
+{
+  UINTN                         TotalLen;
+  UTP_QUERY_REQ_UPIU            *QueryReqUpiu;
+  UINT8                         Opcode;
+  UINT32                        DataSize;
+  UINT8                         *Data;
+  UINT8                         DataDirection;
+  EFI_PHYSICAL_ADDRESS          CmdDescPhyAddr;
+  EFI_STATUS                    Status;
+
+  ASSERT ((Private != NULL) && (Packet != NULL) && (Trd != NULL));
+
+  Opcode = Packet->Opcode;
+  if ((Opcode > UtpQueryFuncOpcodeTogFlag) || (Opcode == UtpQueryFuncOpcodeNop)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DataDirection = Packet->DataDirection;
+  if (DataDirection == UfsDataIn) {
+    DataSize = Packet->InTransferLength;
+    Data     = Packet->InDataBuffer;
+  } else if (DataDirection == UfsDataOut) {
+    DataSize = Packet->OutTransferLength;
+    Data     = Packet->OutDataBuffer;
+  } else {
+    DataSize = 0;
+    Data     = NULL;
+  }
+
+  if (((Opcode != UtpQueryFuncOpcodeSetFlag) && (Opcode != UtpQueryFuncOpcodeClrFlag) && (Opcode != UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize == 0) || (Data == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((Opcode == UtpQueryFuncOpcodeSetFlag) || (Opcode == UtpQueryFuncOpcodeClrFlag) || (Opcode == UtpQueryFuncOpcodeTogFlag))
+    && ((DataSize != 0) || (Data != NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrAttr) && (DataSize != sizeof (UINT32))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Opcode == UtpQueryFuncOpcodeWrDesc) || (Opcode == UtpQueryFuncOpcodeRdDesc)) {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize);
+  } else {
+    TotalLen = ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU));
+  }
+
+  Status = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Initialize UTP QUERY REQUEST UPIU
+  //
+  QueryReqUpiu = (UTP_QUERY_REQ_UPIU*)*CmdDescHost;
+  ASSERT (QueryReqUpiu != NULL);
+  UfsInitQueryRequestUpiu (
+    QueryReqUpiu,
+    Private->TaskTag++,
+    Opcode,
+    Packet->DescId,
+    Packet->Index,
+    Packet->Selector,
+    DataSize,
+    Data
+    );
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Query Response UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = DataDirection;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->Ocs    = 0x0F;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  if (Opcode == UtpQueryFuncOpcodeWrDesc) {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+  } else {
+    Trd->RuL  = (UINT16)DivU64x32 ((UINT64)(ROUNDUP8 (sizeof (UTP_QUERY_RESP_UPIU)) + ROUNDUP8 (DataSize)), sizeof (UINT32));
+    Trd->RuO  = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_QUERY_REQ_UPIU)), sizeof (UINT32));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate NOP IN and NOP OUT UPIU for filling UTP TRD's command descriptor field.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Trd               The pointer to the UTP Transfer Request Descriptor.
+  @param[out] CmdDescHost       A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescMapping    A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS           The creation succeed.
+  @retval EFI_DEVICE_ERROR      The creation failed.
+  @retval EFI_OUT_OF_RESOURCES  The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsCreateNopCommandDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA        *Private,
+  IN     UTP_TRD                           *Trd,
+     OUT VOID                              **CmdDescHost,
+     OUT VOID                              **CmdDescMapping
+  )
+{
+  UINTN                    TotalLen;
+  UTP_NOP_OUT_UPIU         *NopOutUpiu;
+  EFI_STATUS               Status;
+  EFI_PHYSICAL_ADDRESS     CmdDescPhyAddr;
+
+  ASSERT ((Private != NULL) && (Trd != NULL));
+
+  TotalLen = ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)) + ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU));
+  Status   = UfsAllocateAlignCommonBuffer (Private, TotalLen, CmdDescHost, &CmdDescPhyAddr, CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  NopOutUpiu = (UTP_NOP_OUT_UPIU*)*CmdDescHost;
+  ASSERT (NopOutUpiu != NULL);
+  NopOutUpiu->TaskTag = Private->TaskTag++;
+
+  //
+  // Fill UTP_TRD associated fields
+  // NOTE: Some UFS host controllers request the Nop Out UPIU *MUST* be located at a 64-bit aligned boundary.
+  //
+  Trd->Int    = UFS_INTERRUPT_COMMAND;
+  Trd->Dd     = 0x00;
+  Trd->Ct     = UFS_STORAGE_COMMAND_TYPE;
+  Trd->UcdBa  = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 7);
+  Trd->UcdBaU = (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32);
+  Trd->RuL    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_IN_UPIU)), sizeof (UINT32));
+  Trd->RuO    = (UINT16)DivU64x32 ((UINT64)ROUNDUP8 (sizeof (UTP_NOP_OUT_UPIU)), sizeof (UINT32));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find out available slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+  @retval EFI_NOT_READY     No slot is available at this moment.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTrl (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  UINT8            Nutrs;
+  UINT8            Index;
+  UINT32           Data;
+  EFI_STATUS       Status;
+
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  Status  = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Nutrs   = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+
+  for (Index = 0; Index < Nutrs; Index++) {
+    if ((Data & (BIT0 << Index)) == 0) {
+      *Slot = Index;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_READY;
+}
+
+/**
+  Find out available slot in task management transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[out] Slot          The available slot.
+
+  @retval EFI_SUCCESS       The available slot was found successfully.
+
+**/
+EFI_STATUS
+UfsFindAvailableSlotInTmrl (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+     OUT UINT8                        *Slot
+  )
+{
+  ASSERT ((Private != NULL) && (Slot != NULL));
+
+  //
+  // The simplest algo to always use slot 0.
+  // TODO: enhance it to support async transfer with multiple slot.
+  //
+  *Slot = 0;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be started.
+
+**/
+EFI_STATUS
+UfsStartExecCmd (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINT32        Data;
+  EFI_STATUS    Status;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_UTRLRSR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_UTRLRSR) != UFS_HC_UTRLRSR) {
+    Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, UFS_HC_UTRLRSR);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLDBR_OFFSET, BIT0 << Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop specified slot in transfer list of a UFS device.
+
+  @param[in]  Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Slot          The slot to be stop.
+
+**/
+EFI_STATUS
+UfsStopExecCmd (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        Slot
+  ) 
+{
+  UINT32        Data;
+  EFI_STATUS    Status;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & (BIT0 << Slot)) != 0) {
+    Status = UfsMmioRead32 (Private, UFS_HC_UTRLCLR_OFFSET, &Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = UfsMmioWrite32 (Private, UFS_HC_UTRLCLR_OFFSET, Data & ~(BIT0 << Slot));
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read or write specified device descriptor of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      DescId        The ID of device descriptor.
+  @param[in]      Index         The Index of device descriptor.
+  @param[in]      Selector      The Selector of device descriptor.
+  @param[in, out] Descriptor    The buffer of device descriptor to be read or written.
+  @param[in]      DescSize      The size of device descriptor buffer.
+
+  @retval EFI_SUCCESS           The device descriptor was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the device descriptor.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the device descriptor.
+
+**/
+EFI_STATUS
+UfsRwDeviceDesc (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        DescId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT VOID                         *Descriptor,
+  IN     UINT32                       DescSize
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  UINT16                               ReturnDataSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.InDataBuffer      = Descriptor;
+    Packet.InTransferLength  = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdDesc;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.OutDataBuffer     = Descriptor;
+    Packet.OutTransferLength = DescSize;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrDesc;
+  }
+  Packet.DescId              = DescId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnDataSize = QueryResp->Tsf.Length;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnDataSize, sizeof (UINT16));
+
+    if (Read) {
+      CopyMem (Packet.InDataBuffer, (QueryResp + 1), ReturnDataSize);
+      Packet.InTransferLength = ReturnDataSize;
+    } else {
+      Packet.OutTransferLength = ReturnDataSize;
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Read or write specified attribute of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      AttrId        The ID of Attribute.
+  @param[in]      Index         The Index of Attribute.
+  @param[in]      Selector      The Selector of Attribute.
+  @param[in, out] Attributes    The value of Attribute to be read or written.
+
+  @retval EFI_SUCCESS           The Attribute was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the Attribute.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the Attribute.
+
+**/
+EFI_STATUS
+UfsRwAttributes (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        AttrId,
+  IN     UINT8                        Index,
+  IN     UINT8                        Selector,
+  IN OUT UINT32                       *Attributes
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  UINT32                               ReturnData;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdAttr;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    Packet.Opcode            = UtpQueryFuncOpcodeWrAttr;
+  }
+  Packet.DescId              = AttrId;
+  Packet.Index               = Index;
+  Packet.Selector            = Selector;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Trd = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    ReturnData = QueryResp->Tsf.Value;
+    SwapLittleEndianToBigEndian ((UINT8*)&ReturnData, sizeof (UINT32));
+    *Attributes = ReturnData;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Read or write specified flag of a UFS device.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Read          The boolean variable to show r/w direction.
+  @param[in]      FlagId        The ID of flag to be read or written.
+  @param[in, out] Value         The value to set or clear flag.
+
+  @retval EFI_SUCCESS           The flag was read/written successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to r/w the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of r/w the flag.
+
+**/
+EFI_STATUS
+UfsRwFlags (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     BOOLEAN                      Read,
+  IN     UINT8                        FlagId,
+  IN OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+  UFS_DEVICE_MANAGEMENT_REQUEST_PACKET Packet;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_QUERY_RESP_UPIU                  *QueryResp;
+  UINT32                               CmdDescSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  if (Value == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&Packet, sizeof (UFS_DEVICE_MANAGEMENT_REQUEST_PACKET));
+
+  if (Read) {
+    ASSERT (Value != NULL);
+    Packet.DataDirection     = UfsDataIn;
+    Packet.Opcode            = UtpQueryFuncOpcodeRdFlag;
+  } else {
+    Packet.DataDirection     = UfsDataOut;
+    if (*Value == 1) {
+      Packet.Opcode          = UtpQueryFuncOpcodeSetFlag;
+    } else if (*Value == 0) {
+      Packet.Opcode          = UtpQueryFuncOpcodeClrFlag;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  Packet.DescId              = FlagId;
+  Packet.Index               = 0;
+  Packet.Selector            = 0;
+  Packet.Timeout             = UFS_TIMEOUT;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateDMCommandDesc (Private, &Packet, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  QueryResp   = (UTP_QUERY_RESP_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (QueryResp != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet.Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (QueryResp->QueryResp != 0) {
+    DumpQueryResponseResult (QueryResp->QueryResp);
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (Trd->Ocs == 0) {
+    *Value = (UINT8)QueryResp->Tsf.Value;
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Set specified flag to 1 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be set.
+
+  @retval EFI_SUCCESS           The flag was set successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to set the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of setting the flag.
+
+**/
+EFI_STATUS
+UfsSetFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 1;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Clear specified flag to 0 on a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be cleared.
+
+  @retval EFI_SUCCESS           The flag was cleared successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to clear the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of clearing the flag.
+
+**/
+EFI_STATUS
+UfsClearFlag (
+  IN  UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN  UINT8                        FlagId
+  )
+{
+  EFI_STATUS             Status;
+  UINT8                  Value;
+
+  Value  = 0;
+  Status = UfsRwFlags (Private, FALSE, FlagId, &Value);
+
+  return Status;
+}
+
+/**
+  Read specified flag from a UFS device.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  FlagId            The ID of flag to be read.
+  @param[out] Value             The flag's value.
+
+  @retval EFI_SUCCESS           The flag was read successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to read the flag.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the completion of reading the flag.
+
+**/
+EFI_STATUS
+UfsReadFlag (
+  IN     UFS_PASS_THRU_PRIVATE_DATA   *Private,
+  IN     UINT8                        FlagId,
+     OUT UINT8                        *Value
+  )
+{
+  EFI_STATUS                           Status;
+
+  Status = UfsRwFlags (Private, TRUE, FlagId, Value);
+
+  return Status;
+}
+
+/**
+  Sends NOP IN cmd to a UFS device for initialization process request.
+  For more details, please refer to UFS 2.0 spec Figure 13.3.
+
+  @param[in]  Private           The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS           The NOP IN command was sent by the host. The NOP OUT response was
+                                received successfully.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to execute NOP IN command.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the NOP IN command to execute.
+
+**/
+EFI_STATUS
+UfsExecNopCmds (
+  IN  UFS_PASS_THRU_PRIVATE_DATA       *Private
+  )
+{
+  EFI_STATUS                           Status;
+  UINT8                                Slot;
+  UTP_TRD                              *Trd;
+  UTP_NOP_IN_UPIU                      *NopInUpiu;
+  UINT32                               CmdDescSize;
+  VOID                                 *CmdDescHost;
+  VOID                                 *CmdDescMapping;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Trd    = ((UTP_TRD*)Private->UtpTrlBase) + Slot;
+  Status = UfsCreateNopCommandDesc (Private, Trd, &CmdDescHost, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  UfsHc       = Private->UfsHostController;
+  NopInUpiu   = (UTP_NOP_IN_UPIU*)((UINT8*)CmdDescHost + Trd->RuO * sizeof (UINT32));
+  ASSERT (NopInUpiu != NULL);
+  CmdDescSize = Trd->RuO * sizeof (UINT32) + Trd->RuL * sizeof (UINT32);
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, Slot);
+
+  //
+  // Wait for the completion of the transfer request.
+  //  
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (NopInUpiu->Resp != 0) {
+    Status = EFI_DEVICE_ERROR;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, Slot);
+
+  if (CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, CmdDescMapping);
+  }
+  if (CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (CmdDescSize), CmdDescHost);
+  }
+
+  return Status;
+}
+
+/**
+  Sends a UFS-supported SCSI Request Packet to a UFS device that is attached to the UFS host controller.
+
+  @param[in]      Private       The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]      Lun           The LUN of the UFS device to send the SCSI Request Packet.
+  @param[in, out] Packet        A pointer to the SCSI Request Packet to send to a specified Lun of the
+                                UFS device.
+  @param[in]      Event         If nonblocking I/O is not supported then Event is ignored, and blocking
+                                I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                                Event is not NULL and non blocking I/O is supported, then
+                                nonblocking I/O is performed, and Event will be signaled when the
+                                SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet.
+  @retval EFI_OUT_OF_RESOURCES  The resource for transfer is not available.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.
+
+**/
+EFI_STATUS
+UfsExecScsiCmds (
+  IN     UFS_PASS_THRU_PRIVATE_DATA                  *Private,
+  IN     UINT8                                       Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet,
+  IN     EFI_EVENT                                   Event    OPTIONAL
+  )
+{
+  EFI_STATUS                           Status;
+  UTP_RESPONSE_UPIU                    *Response;
+  UINT16                               SenseDataLen;
+  UINT32                               ResTranCount;
+  VOID                                 *DataBuf;
+  EFI_PHYSICAL_ADDRESS                 DataBufPhyAddr;
+  UINT32                               DataLen;
+  UINTN                                MapLength;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+  EDKII_UFS_HOST_CONTROLLER_OPERATION  Flag;
+  UTP_TR_PRD                           *PrdtBase;
+  EFI_TPL                              OldTpl;
+  UFS_PASS_THRU_TRANS_REQ              *TransReq;
+
+  TransReq       = AllocateZeroPool (sizeof (UFS_PASS_THRU_TRANS_REQ));
+  if (TransReq == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  TransReq->Signature     = UFS_PASS_THRU_TRANS_REQ_SIG;
+  TransReq->TimeoutRemain = Packet->Timeout;
+  DataBufPhyAddr = 0;
+  UfsHc          = Private->UfsHostController;
+  //
+  // Find out which slot of transfer request list is available.
+  //
+  Status = UfsFindAvailableSlotInTrl (Private, &TransReq->Slot);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  TransReq->Trd = ((UTP_TRD*)Private->UtpTrlBase) + TransReq->Slot;
+
+  //
+  // Fill transfer request descriptor to this slot.
+  //
+  Status = UfsCreateScsiCommandDesc (
+             Private,
+             Lun,
+             Packet,
+             TransReq->Trd,
+             &TransReq->CmdDescHost,
+             &TransReq->CmdDescMapping
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  TransReq->CmdDescSize = TransReq->Trd->PrdtO * sizeof (UINT32) + TransReq->Trd->PrdtL * sizeof (UTP_TR_PRD);
+
+  if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+    DataBuf       = Packet->InDataBuffer;
+    DataLen       = Packet->InTransferLength;
+    Flag          = EdkiiUfsHcOperationBusMasterWrite;
+  } else {
+    DataBuf       = Packet->OutDataBuffer;
+    DataLen       = Packet->OutTransferLength;
+    Flag          = EdkiiUfsHcOperationBusMasterRead;
+  }
+
+  if (DataLen != 0) {
+    MapLength = DataLen;
+    Status    = UfsHc->Map (
+                         UfsHc,
+                         Flag,
+                         DataBuf,
+                         &MapLength,
+                         &DataBufPhyAddr,
+                         &TransReq->DataBufMapping
+                         );
+
+    if (EFI_ERROR (Status) || (DataLen != MapLength)) {
+      goto Exit1;
+    }
+  }
+  //
+  // Fill PRDT table of Command UPIU for executed SCSI cmd.
+  //
+  PrdtBase = (UTP_TR_PRD*)((UINT8*)TransReq->CmdDescHost + ROUNDUP8 (sizeof (UTP_COMMAND_UPIU)) + ROUNDUP8 (sizeof (UTP_RESPONSE_UPIU)));
+  ASSERT (PrdtBase != NULL);
+  UfsInitUtpPrdt (PrdtBase, (VOID*)(UINTN)DataBufPhyAddr, DataLen);
+
+  //
+  // Insert the async SCSI cmd to the Async I/O list
+  //
+  if (Event != NULL) {
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    TransReq->Packet      = Packet;
+    TransReq->CallerEvent = Event;
+    InsertTailList (&Private->Queue, &TransReq->TransferList);
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  //
+  // Start to execute the transfer request.
+  //
+  UfsStartExecCmd (Private, TransReq->Slot);
+
+  //
+  // Immediately return for async I/O.
+  //
+  if (Event != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Wait for the completion of the transfer request.
+  // 
+  Status = UfsWaitMemSet (Private, UFS_HC_UTRLDBR_OFFSET, BIT0, 0, Packet->Timeout);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get sense data if exists
+  //
+  Response     = (UTP_RESPONSE_UPIU*)((UINT8*)TransReq->CmdDescHost + TransReq->Trd->RuO * sizeof (UINT32));
+  ASSERT (Response != NULL);
+  SenseDataLen = Response->SenseDataLen;
+  SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+  
+  if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+    CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+    Packet->SenseDataLength = (UINT8)SenseDataLen;
+  }
+
+  //
+  // Check the transfer request result.
+  //
+  Packet->TargetStatus = Response->Status;
+  if (Response->Response != 0) {
+    DEBUG ((EFI_D_ERROR, "UfsExecScsiCmds() fails with Target Failure\n"));
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  if (TransReq->Trd->Ocs == 0) {
+    if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->InTransferLength -= ResTranCount;
+      }
+    } else {
+      if ((Response->Flags & BIT5) == BIT5) {
+        ResTranCount = Response->ResTranCount;
+        SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+        Packet->OutTransferLength -= ResTranCount;
+      }
+    }
+  } else {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+Exit:
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, TransReq->Slot);
+
+  if (TransReq->DataBufMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->DataBufMapping);
+  }
+
+Exit1:
+  if (TransReq->CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->CmdDescMapping);
+  }
+  if (TransReq->CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (UfsHc, EFI_SIZE_TO_PAGES (TransReq->CmdDescSize), TransReq->CmdDescHost);
+  }
+  if (TransReq != NULL) {
+    FreePool (TransReq);
+  }
+  return Status;
+}
+
+
+/**
+  Sent UIC DME_LINKSTARTUP command to start the link startup procedure.
+
+  @param[in] Private          The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in] UicOpcode        The opcode of the UIC command.
+  @param[in] Arg1             The value for 1st argument of the UIC command.
+  @param[in] Arg2             The value for 2nd argument of the UIC command.
+  @param[in] Arg3             The value for 3rd argument of the UIC command.
+
+  @return EFI_SUCCESS      Successfully execute this UIC command and detect attached UFS device.
+  @return EFI_DEVICE_ERROR Fail to execute this UIC command and detect attached UFS device.
+  @return EFI_NOT_FOUND    The presence of the UFS device isn't detected.
+
+**/
+EFI_STATUS
+UfsExecUicCommands (
+  IN  UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN  UINT8                         UicOpcode,
+  IN  UINT32                        Arg1,
+  IN  UINT32                        Arg2,
+  IN  UINT32                        Arg3
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      Data;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_IS_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_IS_UCCS) == UFS_HC_IS_UCCS) {
+    //
+    // Clear IS.BIT10 UIC Command Completion Status (UCCS) at first.
+    //
+    Status = UfsMmioWrite32 (Private, UFS_HC_IS_OFFSET, Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // When programming UIC command registers, host software shall set the register UICCMD
+  // only after all the UIC command argument registers (UICCMDARG1, UICCMDARG2 and UICCMDARG3)
+  // are set.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG1_OFFSET, Arg1);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG2_OFFSET, Arg2);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UCMD_ARG3_OFFSET, Arg3);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Host software shall only set the UICCMD if HCS.UCRDY is set to 1.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_STATUS_OFFSET, UFS_HC_HCS_UCRDY, UFS_HC_HCS_UCRDY, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UIC_CMD_OFFSET, (UINT32)UicOpcode);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // UFS 2.0 spec section 5.3.1 Offset:0x20 IS.Bit10 UIC Command Completion Status (UCCS)
+  // This bit is set to '1' by the host controller upon completion of a UIC command. 
+  //
+  Status  = UfsWaitMemSet (Private, UFS_HC_IS_OFFSET, UFS_HC_IS_UCCS, UFS_HC_IS_UCCS, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (UicOpcode != UfsUicDmeReset) {
+    Status = UfsMmioRead32 (Private, UFS_HC_UCMD_ARG2_OFFSET, &Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if ((Data & 0xFF) != 0) {
+      DEBUG_CODE_BEGIN();
+        DumpUicCmdExecResult (UicOpcode, (UINT8)(Data & 0xFF));
+      DEBUG_CODE_END();
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Check value of HCS.DP and make sure that there is a device attached to the Link.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_STATUS_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_HCS_DP) == 0) {
+    Status  = UfsWaitMemSet (Private, UFS_HC_IS_OFFSET, UFS_HC_IS_ULSS, UFS_HC_IS_ULSS, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+    return EFI_NOT_FOUND;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsPassThruDxe: found a attached UFS device\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate common buffer for host and UFS bus master access simultaneously.
+
+  @param[in]  Private                The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+  @param[in]  Size                   The length of buffer to be allocated.
+  @param[out] CmdDescHost            A pointer to store the base system memory address of the allocated range.
+  @param[out] CmdDescPhyAddr         The resulting map address for the UFS bus master to use to access the hosts CmdDescHost.
+  @param[out] CmdDescMapping         A resulting value to pass to Unmap().
+
+  @retval EFI_SUCCESS                The common buffer was allocated successfully.
+  @retval EFI_DEVICE_ERROR           The allocation fails.
+  @retval EFI_OUT_OF_RESOURCES       The memory resource is insufficient.
+
+**/
+EFI_STATUS
+UfsAllocateAlignCommonBuffer (
+  IN     UFS_PASS_THRU_PRIVATE_DATA    *Private,
+  IN     UINTN                         Size,
+     OUT VOID                          **CmdDescHost,
+     OUT EFI_PHYSICAL_ADDRESS          *CmdDescPhyAddr,
+     OUT VOID                          **CmdDescMapping
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                Bytes;
+  BOOLEAN                              Is32BitAddr;
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *UfsHc;
+
+  if ((Private->Capabilities & UFS_HC_CAP_64ADDR) == UFS_HC_CAP_64ADDR) {
+    Is32BitAddr = TRUE;
+  } else {
+    Is32BitAddr = FALSE;
+  }
+
+  UfsHc  = Private->UfsHostController;
+  Status = UfsHc->AllocateBuffer (
+                    UfsHc,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES (Size),
+                    CmdDescHost,
+                    0
+                    );
+  if (EFI_ERROR (Status)) {
+    *CmdDescMapping = NULL;
+    *CmdDescHost    = NULL;
+    *CmdDescPhyAddr = 0;
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Bytes  = EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size));
+  Status = UfsHc->Map (
+                    UfsHc,
+                    EdkiiUfsHcOperationBusMasterCommonBuffer,
+                    *CmdDescHost,
+                    &Bytes,
+                    CmdDescPhyAddr,
+                    CmdDescMapping
+                    );
+
+  if (EFI_ERROR (Status) || (Bytes != EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)))) {
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)),
+             *CmdDescHost
+             );
+    *CmdDescHost = NULL;
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (Is32BitAddr && ((*CmdDescPhyAddr) > 0x100000000ULL)) {
+    //
+    // The UFS host controller doesn't support 64bit addressing, so should not get a >4G UFS bus master address.
+    //
+    UfsHc->Unmap (
+             UfsHc,
+             *CmdDescMapping
+             );
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)),
+             *CmdDescHost
+             );
+    *CmdDescMapping = NULL;
+    *CmdDescHost    = NULL;
+    return EFI_DEVICE_ERROR;
+  }
+
+  ZeroMem (*CmdDescHost, EFI_PAGES_TO_SIZE (EFI_SIZE_TO_PAGES (Size)));
+  return EFI_SUCCESS;
+}
+
+/**
+  Enable the UFS host controller for accessing.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS host controller enabling was executed successfully.
+  @retval EFI_DEVICE_ERROR           A device error occurred while enabling the UFS host controller.
+
+**/
+EFI_STATUS
+UfsEnableHostController (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINT32                 Data;
+
+  //
+  // UFS 2.0 spec section 7.1.1 - Host Controller Initialization
+  //
+  // Reinitialize the UFS host controller if HCE bit of HC register is set.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_ENABLE_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN) {
+    //
+    // Write a 0 to the HCE register at first to disable the host controller.
+    //
+    Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, 0);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Wait until HCE is read as '0' before continuing.
+    //
+    Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Write a 1 to the HCE register to enable the UFS host controller.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Wait until HCE is read as '1' before continuing.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, UFS_HC_HCE_EN, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect if a UFS device attached.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS device detection was executed successfully.
+  @retval EFI_NOT_FOUND              Not found a UFS device attached.
+  @retval EFI_DEVICE_ERROR           A device error occurred while detecting the UFS device.
+
+**/
+EFI_STATUS
+UfsDeviceDetection (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINTN                  Retry;
+  EFI_STATUS             Status;
+
+  //
+  // Start UFS device detection.
+  // Try up to 3 times for establishing data link with device.
+  //
+  for (Retry = 0; Retry < 3; Retry++) {
+    Status = UfsExecUicCommands (Private, UfsUicDmeLinkStartup, 0, 0, 0);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      continue;
+    }
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (Retry == 3) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS task management request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS task management list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTaskManagementRequestList (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINT32                 Data;
+  UINT8                  Nutmrs;
+  VOID                   *CmdDescHost;
+  EFI_PHYSICAL_ADDRESS   CmdDescPhyAddr;
+  VOID                   *CmdDescMapping;
+  EFI_STATUS             Status;
+  
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  CmdDescHost    = NULL;
+  CmdDescMapping = NULL;
+  CmdDescPhyAddr = 0;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_CAP_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Task Management Request List.
+  //
+  Nutmrs = (UINT8) (RShiftU64 ((Private->Capabilities & UFS_HC_CAP_NUTMRS), 16) + 1);
+  Status = UfsAllocateAlignCommonBuffer (Private, Nutmrs * sizeof (UTP_TMRD), &CmdDescHost, &CmdDescPhyAddr, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program the UTP Task Management Request List Base Address and UTP Task Management
+  // Request List Base Address with a 64-bit address allocated at step 6.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLBA_OFFSET, (UINT32)(UINTN)CmdDescPhyAddr);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLBAU_OFFSET, (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Private->UtpTmrlBase = CmdDescHost;
+  Private->Nutmrs      = Nutmrs;
+  Private->TmrlMapping = CmdDescMapping;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLRSR_OFFSET, UFS_HC_UTMRLRSR);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize UFS transfer request list related h/w context.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The UFS transfer list was initialzed successfully.
+  @retval EFI_DEVICE_ERROR           The initialization fails.
+
+**/
+EFI_STATUS
+UfsInitTransferRequestList (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  UINT32                 Data;
+  UINT8                  Nutrs;
+  VOID                   *CmdDescHost;
+  EFI_PHYSICAL_ADDRESS   CmdDescPhyAddr;
+  VOID                   *CmdDescMapping;  
+  EFI_STATUS             Status;
+
+  //
+  // Initial h/w and s/w context for future operations.
+  //
+  CmdDescHost    = NULL;
+  CmdDescMapping = NULL;
+  CmdDescPhyAddr = 0;
+
+  Status = UfsMmioRead32 (Private, UFS_HC_CAP_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->Capabilities = Data;
+
+  //
+  // Allocate and initialize UTP Transfer Request List.
+  //
+  Nutrs  = (UINT8)((Private->Capabilities & UFS_HC_CAP_NUTRS) + 1);
+  Status = UfsAllocateAlignCommonBuffer (Private, Nutrs * sizeof (UTP_TRD), &CmdDescHost, &CmdDescPhyAddr, &CmdDescMapping);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Program the UTP Transfer Request List Base Address and UTP Transfer Request List
+  // Base Address with a 64-bit address allocated at step 8.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLBA_OFFSET, (UINT32)(UINTN)CmdDescPhyAddr);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLBAU_OFFSET, (UINT32)RShiftU64 ((UINT64)CmdDescPhyAddr, 32));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Private->UtpTrlBase = CmdDescHost;
+  Private->Nutrs      = Nutrs;  
+  Private->TrlMapping = CmdDescMapping;
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, UFS_HC_UTRLRSR);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is initialized successfully.
+  @retval Others                     A device error occurred while initializing the controller.
+
+**/
+EFI_STATUS
+UfsControllerInit (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+
+  Status = UfsEnableHostController (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Enable Host Controller Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsDeviceDetection (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Device Detection Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTaskManagementRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Task management list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  Status = UfsInitTransferRequestList (Private);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UfsControllerInit: Transfer list initialization Fails, Status = %r\n", Status));
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsControllerInit Finished\n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop the UFS host controller.
+
+  @param[in] Private                 The pointer to the UFS_PASS_THRU_PRIVATE_DATA data structure.
+
+  @retval EFI_SUCCESS                The Ufs Host Controller is stopped successfully.
+  @retval Others                     A device error occurred while stopping the controller.
+
+**/
+EFI_STATUS
+UfsControllerStop (
+  IN  UFS_PASS_THRU_PRIVATE_DATA     *Private
+  )
+{
+  EFI_STATUS             Status;
+  UINT32                 Data;
+
+  //
+  // Enable the UTP Task Management Request List by setting the UTP Task Management
+  // Request List RunStop Register (UTMRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTMRLRSR_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Enable the UTP Transfer Request List by setting the UTP Transfer Request List
+  // RunStop Register (UTRLRSR) to '1'.
+  //
+  Status = UfsMmioWrite32 (Private, UFS_HC_UTRLRSR_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Write a 0 to the HCE register in order to disable the host controller.
+  //
+  Status = UfsMmioRead32 (Private, UFS_HC_ENABLE_OFFSET, &Data);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT ((Data & UFS_HC_HCE_EN) == UFS_HC_HCE_EN);
+
+  Status = UfsMmioWrite32 (Private, UFS_HC_ENABLE_OFFSET, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Wait until HCE is read as '0' before continuing.
+  //
+  Status = UfsWaitMemSet (Private, UFS_HC_ENABLE_OFFSET, UFS_HC_HCE_EN, 0, UFS_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DEBUG ((EFI_D_INFO, "UfsController is stopped\n"));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Internal helper function which will signal the caller event and clean up
+  resources.
+
+  @param[in] Private   The pointer to the UFS_PASS_THRU_PRIVATE_DATA data
+                       structure.
+  @param[in] TransReq  The pointer to the UFS_PASS_THRU_TRANS_REQ data
+                       structure.
+
+**/
+VOID
+EFIAPI
+SignalCallerEvent (
+  IN UFS_PASS_THRU_PRIVATE_DATA      *Private,
+  IN UFS_PASS_THRU_TRANS_REQ         *TransReq
+  )
+{
+  EDKII_UFS_HOST_CONTROLLER_PROTOCOL *UfsHc;
+  EFI_EVENT                          CallerEvent;
+
+  ASSERT ((Private != NULL) && (TransReq != NULL));
+
+  UfsHc        = Private->UfsHostController;
+  CallerEvent  = TransReq->CallerEvent;
+
+  RemoveEntryList (&TransReq->TransferList);
+
+  UfsHc->Flush (UfsHc);
+
+  UfsStopExecCmd (Private, TransReq->Slot);
+
+  if (TransReq->DataBufMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->DataBufMapping);
+  }
+
+  if (TransReq->CmdDescMapping != NULL) {
+    UfsHc->Unmap (UfsHc, TransReq->CmdDescMapping);
+  }
+  if (TransReq->CmdDescHost != NULL) {
+    UfsHc->FreeBuffer (
+             UfsHc,
+             EFI_SIZE_TO_PAGES (TransReq->CmdDescSize),
+             TransReq->CmdDescHost
+             );
+  }
+
+  FreePool (TransReq);
+
+  gBS->SignalEvent (CallerEvent);
+  return;
+}
+
+/**
+  Call back function when the timer event is signaled.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+
+**/
+VOID
+EFIAPI
+ProcessAsyncTaskList (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  )
+{
+  UFS_PASS_THRU_PRIVATE_DATA                    *Private;
+  LIST_ENTRY                                    *Entry;
+  LIST_ENTRY                                    *NextEntry;
+  UFS_PASS_THRU_TRANS_REQ                       *TransReq;
+  EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET    *Packet;
+  UTP_RESPONSE_UPIU                             *Response;
+  UINT16                                        SenseDataLen;
+  UINT32                                        ResTranCount;
+  UINT32                                        SlotsMap;
+  UINT32                                        Value;
+  EFI_STATUS                                    Status;
+
+  Private   = (UFS_PASS_THRU_PRIVATE_DATA*) Context;
+  SlotsMap  = 0;
+
+  //
+  // Check the entries in the async I/O queue are done or not.
+  //
+  if (!IsListEmpty(&Private->Queue)) {
+    EFI_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Private->Queue) {
+      TransReq  = UFS_PASS_THRU_TRANS_REQ_FROM_THIS (Entry);
+      Packet    = TransReq->Packet;
+
+      if ((SlotsMap & (BIT0 << TransReq->Slot)) != 0) {
+        return;
+      }
+      SlotsMap |= BIT0 << TransReq->Slot;
+
+      Status = UfsMmioRead32 (Private, UFS_HC_UTRLDBR_OFFSET, &Value);
+      if (EFI_ERROR (Status)) {
+        //
+        // TODO: Should find/add a proper host adapter return status for this
+        // case.
+        //
+        Packet->HostAdapterStatus = EFI_EXT_SCSI_STATUS_HOST_ADAPTER_PHASE_ERROR;
+        DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p UfsMmioRead32() Error.\n", TransReq->CallerEvent));
+        SignalCallerEvent (Private, TransReq);
+        continue;
+      }
+
+      if ((Value & (BIT0 << TransReq->Slot)) != 0) {
+        //
+        // Scsi cmd not finished yet.
+        //
+        if (TransReq->TimeoutRemain > UFS_HC_ASYNC_TIMER) {
+          TransReq->TimeoutRemain -= UFS_HC_ASYNC_TIMER;
+          continue;
+        } else {
+          //
+          // Timeout occurs.
+          //
+          Packet->HostAdapterStatus = EFI_EXT_SCSI_STATUS_HOST_ADAPTER_TIMEOUT_COMMAND;
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p EFI_TIMEOUT.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+      } else {
+        //
+        // Scsi cmd finished.
+        //
+        // Get sense data if exists
+        //
+        Response = (UTP_RESPONSE_UPIU*)((UINT8*)TransReq->CmdDescHost + TransReq->Trd->RuO * sizeof (UINT32));
+        ASSERT (Response != NULL);
+        SenseDataLen = Response->SenseDataLen;
+        SwapLittleEndianToBigEndian ((UINT8*)&SenseDataLen, sizeof (UINT16));
+
+        if ((Packet->SenseDataLength != 0) && (Packet->SenseData != NULL)) {
+          CopyMem (Packet->SenseData, Response->SenseData, SenseDataLen);
+          Packet->SenseDataLength = (UINT8)SenseDataLen;
+        }
+
+        //
+        // Check the transfer request result.
+        //
+        Packet->TargetStatus = Response->Status;
+        if (Response->Response != 0) {
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Target Failure.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+
+        if (TransReq->Trd->Ocs == 0) {
+          if (Packet->DataDirection == EFI_EXT_SCSI_DATA_DIRECTION_READ) {
+            if ((Response->Flags & BIT5) == BIT5) {
+              ResTranCount = Response->ResTranCount;
+              SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+              Packet->InTransferLength -= ResTranCount;
+            }
+          } else {
+            if ((Response->Flags & BIT5) == BIT5) {
+              ResTranCount = Response->ResTranCount;
+              SwapLittleEndianToBigEndian ((UINT8*)&ResTranCount, sizeof (UINT32));
+              Packet->OutTransferLength -= ResTranCount;
+            }
+          }
+        } else {
+          DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Target Device Error.\n", TransReq->CallerEvent));
+          SignalCallerEvent (Private, TransReq);
+          continue;
+        }
+
+        DEBUG ((EFI_D_VERBOSE, "ProcessAsyncTaskList(): Signal Event %p Success.\n", TransReq->CallerEvent));
+        SignalCallerEvent (Private, TransReq);
+      }
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h
new file mode 100644
index 0000000000..a423a921de
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruHci.h
@@ -0,0 +1,1339 @@
+/** @file
+  UfsPassThruDxe driver is used to produce EFI_EXT_SCSI_PASS_THRU protocol interface
+  for upper layer application to execute UFS-supported SCSI cmds.
+
+  Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _UFS_PASS_THRU_HCI_H_
+#define _UFS_PASS_THRU_HCI_H_
+
+//
+// Host Capabilities Register Offsets
+//
+#define UFS_HC_CAP_OFFSET          0x0000  // Controller Capabilities
+#define UFS_HC_VER_OFFSET          0x0008  // Version
+#define UFS_HC_DDID_OFFSET         0x0010  // Device ID and Device Class
+#define UFS_HC_PMID_OFFSET         0x0014  // Product ID and Manufacturer ID
+#define UFS_HC_AHIT_OFFSET         0x0018  // Auto-Hibernate Idle Timer
+//
+// Operation and Runtime Register Offsets
+//
+#define UFS_HC_IS_OFFSET           0x0020  // Interrupt Status
+#define UFS_HC_IE_OFFSET           0x0024  // Interrupt Enable
+#define UFS_HC_STATUS_OFFSET       0x0030  // Host Controller Status
+#define UFS_HC_ENABLE_OFFSET       0x0034  // Host Controller Enable
+#define UFS_HC_UECPA_OFFSET        0x0038  // Host UIC Error Code PHY Adapter Layer
+#define UFS_HC_UECDL_OFFSET        0x003c  // Host UIC Error Code Data Link Layer
+#define UFS_HC_UECN_OFFSET         0x0040  // Host UIC Error Code Network Layer
+#define UFS_HC_UECT_OFFSET         0x0044  // Host UIC Error Code Transport Layer
+#define UFS_HC_UECDME_OFFSET       0x0048  // Host UIC Error Code DME
+#define UFS_HC_UTRIACR_OFFSET      0x004c  // UTP Transfer Request Interrupt Aggregation Control Register
+//
+// UTP Transfer Register Offsets
+//
+#define UFS_HC_UTRLBA_OFFSET       0x0050  // UTP Transfer Request List Base Address
+#define UFS_HC_UTRLBAU_OFFSET      0x0054  // UTP Transfer Request List Base Address Upper 32-Bits
+#define UFS_HC_UTRLDBR_OFFSET      0x0058  // UTP Transfer Request List Door Bell Register
+#define UFS_HC_UTRLCLR_OFFSET      0x005c  // UTP Transfer Request List CLear Register
+#define UFS_HC_UTRLRSR_OFFSET      0x0060  // UTP Transfer Request Run-Stop Register
+//
+// UTP Task Management Register Offsets
+//
+#define UFS_HC_UTMRLBA_OFFSET      0x0070  // UTP Task Management Request List Base Address
+#define UFS_HC_UTMRLBAU_OFFSET     0x0074  // UTP Task Management Request List Base Address Upper 32-Bits
+#define UFS_HC_UTMRLDBR_OFFSET     0x0078  // UTP Task Management Request List Door Bell Register
+#define UFS_HC_UTMRLCLR_OFFSET     0x007c  // UTP Task Management Request List CLear Register
+#define UFS_HC_UTMRLRSR_OFFSET     0x0080  // UTP Task Management Run-Stop Register
+//
+// UIC Command Register Offsets
+//
+#define UFS_HC_UIC_CMD_OFFSET      0x0090  // UIC Command Register
+#define UFS_HC_UCMD_ARG1_OFFSET    0x0094  // UIC Command Argument 1
+#define UFS_HC_UCMD_ARG2_OFFSET    0x0098  // UIC Command Argument 2
+#define UFS_HC_UCMD_ARG3_OFFSET    0x009c  // UIC Command Argument 3
+//
+// UMA Register Offsets
+//
+#define UFS_HC_UMA_OFFSET          0x00b0  // Reserved for Unified Memory Extension
+
+#define UFS_HC_HCE_EN              BIT0
+#define UFS_HC_HCS_DP              BIT0
+#define UFS_HC_HCS_UCRDY           BIT3
+#define UFS_HC_IS_ULSS             BIT8
+#define UFS_HC_IS_UCCS             BIT10
+#define UFS_HC_CAP_64ADDR          BIT24
+#define UFS_HC_CAP_NUTMRS          (BIT16 | BIT17 | BIT18)
+#define UFS_HC_CAP_NUTRS           (BIT0 | BIT1 | BIT2 | BIT3 | BIT4)
+#define UFS_HC_UTMRLRSR            BIT0
+#define UFS_HC_UTRLRSR             BIT0
+
+//
+// A maximum of length of 256KB is supported by PRDT entry
+//
+#define UFS_MAX_DATA_LEN_PER_PRD   0x40000
+
+#define UFS_STORAGE_COMMAND_TYPE   0x01
+
+#define UFS_REGULAR_COMMAND        0x00
+#define UFS_INTERRUPT_COMMAND      0x01
+
+#define UFS_LUN_0                  0x00
+#define UFS_LUN_1                  0x01
+#define UFS_LUN_2                  0x02
+#define UFS_LUN_3                  0x03
+#define UFS_LUN_4                  0x04
+#define UFS_LUN_5                  0x05
+#define UFS_LUN_6                  0x06
+#define UFS_LUN_7                  0x07
+#define UFS_WLUN_REPORT_LUNS       0x81
+#define UFS_WLUN_UFS_DEV           0xD0
+#define UFS_WLUN_BOOT              0xB0
+#define UFS_WLUN_RPMB              0xC4
+
+#pragma pack(1)
+
+//
+// UFSHCI 2.0 Spec Section 5.2.1 Offset 00h: CAP - Controller Capabilities
+//
+typedef struct {
+  UINT8  Nutrs:4;      // Number of UTP Transfer Request Slots
+  UINT8  Rsvd1:4;
+
+  UINT8  NoRtt;        // Number of outstanding READY TO TRANSFER (RTT) requests supported
+
+  UINT8  Nutmrs:3;     // Number of UTP Task Management Request Slots
+  UINT8  Rsvd2:4;
+  UINT8  AutoHs:1;     // Auto-Hibernation Support
+
+  UINT8  As64:1;       // 64-bit addressing supported
+  UINT8  Oodds:1;      // Out of order data delivery supported
+  UINT8  UicDmetms:1;  // UIC DME_TEST_MODE command supported
+  UINT8  Ume:1;        // Reserved for Unified Memory Extension
+  UINT8  Rsvd4:4;
+} UFS_HC_CAP;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.2 Offset 08h: VER - UFS Version
+//
+typedef struct {
+  UINT8  Vs:4;         // Version Suffix
+  UINT8  Mnr:4;        // Minor version number
+
+  UINT8  Mjr;          // Major version number
+
+  UINT16 Rsvd1;
+} UFS_HC_VER;
+
+//
+// UFSHCI 2.0 Spec Section 5.2.3 Offset 10h: HCPID - Host Controller Product ID
+//
+#define UFS_HC_PID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.4 Offset 14h: HCMID - Host Controller Manufacturer ID
+//
+#define UFS_HC_MID     UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.2.5 Offset 18h: AHIT - Auto-Hibernate Idle Timer
+//
+typedef struct {
+  UINT32 Ahitv:10;     // Auto-Hibernate Idle Timer Value 
+  UINT32 Ts:3;         // Timer scale
+  UINT32 Rsvd1:19;
+} UFS_HC_AHIT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.1 Offset 20h: IS - Interrupt Status
+//
+typedef struct {
+  UINT16 Utrcs:1;      // UTP Transfer Request Completion Status
+  UINT16 Udepri:1;     // UIC DME_ENDPOINT_RESET Indication
+  UINT16 Ue:1;         // UIC Error
+  UINT16 Utms:1;       // UIC Test Mode Status 
+
+  UINT16 Upms:1;       // UIC Power Mode Status 
+  UINT16 Uhxs:1;       // UIC Hibernate Exit Status 
+  UINT16 Uhes:1;       // UIC Hibernate Enter Status 
+  UINT16 Ulls:1;       // UIC Link Lost Status 
+
+  UINT16 Ulss:1;       // UIC Link Startup Status 
+  UINT16 Utmrcs:1;     // UTP Task  Management Request Completion Status 
+  UINT16 Uccs:1;       // UIC Command Completion Status 
+  UINT16 Dfes:1;       // Device Fatal Error Status  
+
+  UINT16 Utpes:1;      // UTP Error Status  
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfes:1;      // Host Controller Fatal Error Status
+  UINT16 Sbfes:1;      // System Bus Fatal Error Status
+  UINT16 Rsvd2:14;
+} UFS_HC_IS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.2 Offset 24h: IE - Interrupt Enable
+//
+typedef struct {
+  UINT16 Utrce:1;      // UTP Transfer Request Completion Enable
+  UINT16 Udeprie:1;    // UIC DME_ENDPOINT_RESET Enable
+  UINT16 Uee:1;        // UIC Error Enable
+  UINT16 Utmse:1;      // UIC Test Mode Status Enable
+
+  UINT16 Upmse:1;      // UIC Power Mode Status Enable 
+  UINT16 Uhxse:1;      // UIC Hibernate Exit Status Enable
+  UINT16 Uhese:1;      // UIC Hibernate Enter Status Enable 
+  UINT16 Ullse:1;      // UIC Link Lost Status Enable
+
+  UINT16 Ulsse:1;      // UIC Link Startup Status Enable
+  UINT16 Utmrce:1;     // UTP Task  Management Request Completion Enable
+  UINT16 Ucce:1;       // UIC Command Completion Enable
+  UINT16 Dfee:1;       // Device Fatal Error Enable
+
+  UINT16 Utpee:1;      // UTP Error Enable
+  UINT16 Rsvd1:3;
+
+  UINT16 Hcfee:1;      // Host Controller Fatal Error Enable
+  UINT16 Sbfee:1;      // System Bus Fatal Error Enable
+  UINT16 Rsvd2:14;
+} UFS_HC_IE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.3 Offset 30h: HCS - Host Controller Status
+//
+typedef struct {
+  UINT8  Dp:1;         // Device Present
+  UINT8  UtrlRdy:1;    // UTP Transfer Request List Ready
+  UINT8  UtmrlRdy:1;   // UTP Task Management Request List Ready
+  UINT8  UcRdy:1;      // UIC COMMAND Ready
+  UINT8  Rsvd1:4;
+
+  UINT8  Upmcrs:3;     // UIC Power Mode Change Request Status
+  UINT8  Rsvd2:1;      // UIC Hibernate Exit Status Enable
+  UINT8  Utpec:4;      // UTP Error Code
+
+  UINT8  TtagUtpE;     // Task Tag of UTP error
+  UINT8  TlunUtpE;     // Target LUN of UTP error
+} UFS_HC_STATUS;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.4 Offset 34h: HCE - Host Controller Enable
+//
+typedef struct {
+  UINT32 Hce:1;        // Host Controller Enable
+  UINT32 Rsvd1:31;
+} UFS_HC_ENABLE;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.5 Offset 38h: UECPA - Host UIC Error Code PHY Adapter Layer
+//
+typedef struct {
+  UINT32 Ec:5;         // UIC PHY Adapter Layer Error Code
+  UINT32 Rsvd1:26;
+  UINT32 Err:1;        // UIC PHY Adapter Layer Error
+} UFS_HC_UECPA;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.6 Offset 3ch: UECDL - Host UIC Error Code Data Link Layer
+//
+typedef struct {
+  UINT32 Ec:15;        // UIC Data Link Layer Error Code
+  UINT32 Rsvd1:16;
+  UINT32 Err:1;        // UIC Data Link Layer Error
+} UFS_HC_UECDL;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.7 Offset 40h: UECN - Host UIC Error Code Network Layer
+//
+typedef struct {
+  UINT32 Ec:3;         // UIC Network Layer Error Code
+  UINT32 Rsvd1:28;
+  UINT32 Err:1;        // UIC Network Layer Error
+} UFS_HC_UECN;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.8 Offset 44h: UECT - Host UIC Error Code Transport Layer
+//
+typedef struct {
+  UINT32 Ec:7;         // UIC Transport Layer Error Code
+  UINT32 Rsvd1:24;
+  UINT32 Err:1;        // UIC Transport Layer Error
+} UFS_HC_UECT;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.9 Offset 48h: UECDME - Host UIC Error Code
+//
+typedef struct {
+  UINT32 Ec:1;         // UIC DME Error Code
+  UINT32 Rsvd1:30;
+  UINT32 Err:1;        // UIC DME Error
+} UFS_HC_UECDME;
+
+//
+// UFSHCI 2.0 Spec Section 5.3.10 Offset 4Ch: UTRIACR - UTP Transfer Request Interrupt Aggregation Control Register
+//
+typedef struct {
+  UINT8  IaToVal;      // Interrupt aggregation timeout value
+
+  UINT8  IacTh:5;      // Interrupt aggregation counter threshold
+  UINT8  Rsvd1:3;
+
+  UINT8  Ctr:1;        // Counter and Timer Reset
+  UINT8  Rsvd2:3;
+  UINT8  Iasb:1;       // Interrupt aggregation status bit
+  UINT8  Rsvd3:3;
+
+  UINT8  IapwEn:1;     // Interrupt aggregation parameter write enable
+  UINT8  Rsvd4:6;
+  UINT8  IaEn:1;       // Interrupt Aggregation Enable/Disable
+} UFS_HC_UTRIACR;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.1 Offset 50h: UTRLBA - UTP Transfer Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtrlBa:22;    // UTP Transfer Request List Base Address
+} UFS_HC_UTRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.4.2 Offset 54h: UTRLBAU - UTP Transfer Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.3 Offset 58h: UTRLDBR - UTP Transfer Request List Door Bell Register
+//
+#define UFS_HC_UTRLDBR UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.4.4 Offset 5Ch: UTRLCLR - UTP Transfer Request List CLear Register
+//
+#define UFS_HC_UTRLCLR UINT32
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.4.5 Offset 60h: UTRLRSR - UTP Transfer Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtrlRsr:1;    // UTP Transfer Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.5.1 Offset 70h: UTMRLBA - UTP Task Management Request List Base Address
+//
+typedef struct {
+  UINT32 Rsvd1:10;
+  UINT32 UtmrlBa:22;   // UTP Task Management Request List Base Address
+} UFS_HC_UTMRLBA;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.2 Offset 74h: UTMRLBAU - UTP Task Management Request List Base Address Upper 32-bits
+//
+#define UFS_HC_UTMRLBAU UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.5.3 Offset 78h: UTMRLDBR - UTP Task Management Request List Door Bell Register
+//
+typedef struct {
+  UINT32 UtmrlDbr:8;   // UTP Task Management Request List Door bell Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLDBR;
+
+//
+// UFSHCI 2.0 Spec Section 5.5.4 Offset 7Ch: UTMRLCLR - UTP Task Management Request List CLear Register
+//
+typedef struct {
+  UINT32 UtmrlClr:8;   // UTP Task Management List Clear Register
+  UINT32 Rsvd1:24;
+} UFS_HC_UTMRLCLR;
+
+#if 0
+//
+// UFSHCI 2.0 Spec Section 5.5.5 Offset 80h: UTMRLRSR - UTP Task Management Request List Run Stop Register
+//
+typedef struct {
+  UINT32 UtmrlRsr:1;   // UTP Task Management Request List Run-Stop Register
+  UINT32 Rsvd1:31;
+} UFS_HC_UTMRLRSR;
+#endif
+
+//
+// UFSHCI 2.0 Spec Section 5.6.1 Offset 90h: UICCMD - UIC Command
+//
+typedef struct {
+  UINT32 CmdOp:8;      // Command Opcode
+  UINT32 Rsvd1:24;
+} UFS_HC_UICCMD;
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 94h: UICCMDARG1 - UIC Command Argument 1
+//
+#define UFS_HC_UICCMD_ARG1 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 98h: UICCMDARG2 - UIC Command Argument 2
+//
+#define UFS_HC_UICCMD_ARG2 UINT32
+
+//
+// UFSHCI 2.0 Spec Section 5.6.2 Offset 9ch: UICCMDARG3 - UIC Command Argument 3
+//
+#define UFS_HC_UICCMD_ARG3 UINT32
+
+//
+// UIC command opcodes
+//
+typedef enum {
+  UfsUicDmeGet            = 0x01,
+  UfsUicDmeSet            = 0x02,
+  UfsUicDmePeerGet        = 0x03,
+  UfsUicDmePeerSet        = 0x04,
+  UfsUicDmePwrOn          = 0x10,
+  UfsUicDmePwrOff         = 0x11,
+  UfsUicDmeEnable         = 0x12,
+  UfsUicDmeReset          = 0x14,
+  UfsUicDmeEndpointReset  = 0x15,
+  UfsUicDmeLinkStartup    = 0x16,
+  UfsUicDmeHibernateEnter = 0x17,
+  UfsUicDmeHibernateExit  = 0x18,
+  UfsUicDmeTestMode       = 0x1A
+} UFS_UIC_OPCODE;
+
+//
+// UTP Transfer Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Dd:2;                /* Data Direction */
+  UINT32 Rsvd2:1;
+  UINT32 Ct:4;                /* Command Type */
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4
+  //
+  UINT32 Rsvd6:7;
+  UINT32 UcdBa:25;            /* UTP Command Descriptor Base Address */
+  
+  //
+  // DW5
+  //
+  UINT32 UcdBaU;              /* UTP Command Descriptor Base Address Upper 32-bits */
+ 
+  //
+  // DW6
+  //
+  UINT16 RuL;                 /* Response UPIU Length */  
+  UINT16 RuO;                 /* Response UPIU Offset */
+
+  //
+  // DW7
+  //
+  UINT16 PrdtL;               /* PRDT Length */  
+  UINT16 PrdtO;               /* PRDT Offset */
+} UTP_TRD;
+
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:2;
+  UINT32 DbAddr:30;           /* Data Base Address */
+  
+  //
+  // DW1
+  //
+  UINT32 DbAddrU;             /* Data Base Address Upper 32-bits */
+ 
+  //
+  // DW2
+  //
+  UINT32 Rsvd2;
+
+  //
+  // DW3
+  //
+  UINT32 DbCount:18;          /* Data Byte Count */
+  UINT32 Rsvd3:14;
+} UTP_TR_PRD;
+
+//
+// UFS 2.0 Spec Section 10.5.3 - UTP Command UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x01*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Rsvd3;
+  UINT8  Rsvd4;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd5;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 ExpDataTranLen;      /* Expected Data Transfer Length - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Cdb[16];
+} UTP_COMMAND_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.4 - UTP Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x21*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  CmdSet:4;            /* Command Set Type */
+  UINT8  Rsvd1:4;
+  UINT8  Rsvd2;
+  UINT8  Response;            /* Response */
+  UINT8  Status;              /* Status */
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 ResTranCount;        /* Residual Transfer Count - Big Endian */
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd3[16];
+
+  //
+  // Data Segment - Sense Data
+  //
+  UINT16 SenseDataLen;        /* Sense Data Length - Big Endian */
+  UINT8  SenseData[18];       /* Sense Data */
+} UTP_RESPONSE_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.5 - UTP Data-Out UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x02*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be sent out
+  //
+  //UINT8  Data[];            /* Data to be sent out, maximum is 65535 bytes */
+} UTP_DATA_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.6 - UTP Data-In UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x22*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_DATA_IN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.7 - UTP Ready-To-Transfer UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x31*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd2;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 DataBufOffset;       /* Data Buffer Offset - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 DataTranCount;       /* Data Transfer Count - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd3[12];
+
+  //
+  // Data Segment - Data to be read
+  //
+  //UINT8  Data[];            /* Data to be read, maximum is 65535 bytes */
+} UTP_RDY_TO_TRAN_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.8 - UTP Task Management Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x04*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1;
+  UINT8  TskManFunc;          /* Task Management Function */
+  UINT8  Rsvd2[2];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 InputParam1;         /* Input Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 InputParam2;         /* Input Parameter 2 - Big Endian */
+
+  //
+  // DW5
+  //
+  UINT32 InputParam3;         /* Input Parameter 3 - Big Endian */
+
+  //
+  // DW6 - DW7
+  //
+  UINT8  Rsvd4[8];
+} UTP_TM_REQ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.9 - UTP Task Management Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x24*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Resp;                /* Response */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT32 OutputParam1;        /* Output Parameter 1 - Big Endian */
+
+  //
+  // DW4
+  //
+  UINT32 OutputParam2;        /* Output Parameter 2 - Big Endian */
+
+  //
+  // DW5 - DW7
+  //
+  UINT8  Rsvd4[12];
+} UTP_TM_RESP_UPIU;
+
+//
+// UTP Task Management Request Descriptor
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT32 Rsvd1:24;
+  UINT32 Int:1;               /* Interrupt */
+  UINT32 Rsvd2:7;
+
+  //
+  // DW1
+  //
+  UINT32 Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT32 Ocs:8;               /* Overall Command Status */
+  UINT32 Rsvd4:24;
+
+  //
+  // DW3
+  //
+  UINT32 Rsvd5;
+
+  //
+  // DW4 - DW11
+  //
+  UTP_TM_REQ_UPIU TmReq;      /* Task Management Request UPIU */
+  
+  //
+  // DW12 - DW19
+  //
+  UTP_TM_RESP_UPIU TmResp;    /* Task Management Response UPIU */
+} UTP_TMRD;
+
+
+typedef struct {
+  UINT8  Opcode;
+  UINT8  DescId;
+  UINT8  Index;
+  UINT8  Selector;
+  UINT16 Rsvd1;
+  UINT16 Length;
+  UINT32 Value;
+  UINT32 Rsvd2;
+} UTP_UPIU_TSF;
+
+//
+// UFS 2.0 Spec Section 10.5.10 - UTP Query Request UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x16*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        Rsvd3[2];
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        Rsvd4;
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd5[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8  Data[];            /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_REQ_UPIU;
+
+#define QUERY_FUNC_STD_READ_REQ     0x01
+#define QUERY_FUNC_STD_WRITE_REQ    0x81
+
+typedef enum {
+  UtpQueryFuncOpcodeNop      = 0x00,
+  UtpQueryFuncOpcodeRdDesc   = 0x01,
+  UtpQueryFuncOpcodeWrDesc   = 0x02,
+  UtpQueryFuncOpcodeRdAttr   = 0x03,
+  UtpQueryFuncOpcodeWrAttr   = 0x04,
+  UtpQueryFuncOpcodeRdFlag   = 0x05,
+  UtpQueryFuncOpcodeSetFlag  = 0x06,
+  UtpQueryFuncOpcodeClrFlag  = 0x07,
+  UtpQueryFuncOpcodeTogFlag  = 0x08
+} UTP_QUERY_FUNC_OPCODE;
+
+//
+// UFS 2.0 Spec Section 10.5.11 - UTP Query Response UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8        TransCode:6;   /* Transaction Type - 0x36*/
+  UINT8        Dd:1;
+  UINT8        Hd:1;
+  UINT8        Flags;
+  UINT8        Rsvd1;
+  UINT8        TaskTag;       /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8        Rsvd2;
+  UINT8        QueryFunc;     /* Query Function */
+  UINT8        QueryResp;     /* Query Response */
+  UINT8        Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8        EhsLen;        /* Total EHS Length - 0x00 */
+  UINT8        DevInfo;       /* Device Information */
+  UINT16       DataSegLen;    /* Data Segment Length - Big Endian */
+
+  //
+  // DW3 - 6
+  //
+  UTP_UPIU_TSF Tsf;           /* Transaction Specific Fields */
+
+  //
+  // DW7
+  //
+  UINT8        Rsvd4[4];
+
+  //
+  // Data Segment - Data to be transferred
+  //
+  //UINT8      Data[];        /* Data to be transferred, maximum is 65535 bytes */
+} UTP_QUERY_RESP_UPIU;
+
+typedef enum {
+  UfsUtpQueryResponseSuccess             = 0x00,
+  UfsUtpQueryResponseParamNotReadable    = 0xF6,
+  UfsUtpQueryResponseParamNotWriteable   = 0xF7,  
+  UfsUtpQueryResponseParamAlreadyWritten = 0xF8,
+  UfsUtpQueryResponseInvalidLen          = 0xF9,
+  UfsUtpQueryResponseInvalidVal          = 0xFA,
+  UfsUtpQueryResponseInvalidSelector     = 0xFB,
+  UfsUtpQueryResponseInvalidIndex        = 0xFC,
+  UfsUtpQueryResponseInvalidIdn          = 0xFD,
+  UfsUtpQueryResponseInvalidOpc          = 0xFE,
+  UfsUtpQueryResponseGeneralFailure      = 0xFF
+} UTP_QUERY_RESP_CODE;
+
+//
+// UFS 2.0 Spec Section 10.5.12 - UTP Reject UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x3F*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Lun;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd1[2];
+  UINT8  Response;            /* Response - 0x01 */
+  UINT8  Rsvd2;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3
+  //
+  UINT8  HdrSts;              /* Basic Header Status */
+  UINT8  Rsvd3;
+  UINT8  E2ESts;              /* End-to-End Status */
+  UINT8  Rsvd4;
+
+  //
+  // DW4 - DW7
+  //
+  UINT8  Rsvd5[16];
+} UTP_REJ_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.13 - UTP NOP OUT UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x00*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[4];
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  Rsvd3;
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_OUT_UPIU;
+
+//
+// UFS 2.0 Spec Section 10.5.14 - UTP NOP IN UPIU
+//
+typedef struct {
+  //
+  // DW0
+  //
+  UINT8  TransCode:6;         /* Transaction Type - 0x20*/
+  UINT8  Dd:1;
+  UINT8  Hd:1;
+  UINT8  Flags;
+  UINT8  Rsvd1;
+  UINT8  TaskTag;             /* Task Tag */
+
+  //
+  // DW1
+  //
+  UINT8  Rsvd2[2];
+  UINT8  Resp;                /* Response - 0x00 */
+  UINT8  Rsvd3;
+
+  //
+  // DW2
+  //
+  UINT8  EhsLen;              /* Total EHS Length - 0x00 */
+  UINT8  DevInfo;             /* Device Information - 0x00 */
+  UINT16 DataSegLen;          /* Data Segment Length - Big Endian - 0x0000 */
+
+  //
+  // DW3 - DW7
+  //
+  UINT8  Rsvd4[20];
+} UTP_NOP_IN_UPIU;
+
+//
+// UFS Descriptors
+//
+typedef enum {
+  UfsDeviceDesc     = 0x00,
+  UfsConfigDesc     = 0x01,
+  UfsUnitDesc       = 0x02,
+  UfsInterConnDesc  = 0x04,
+  UfsStringDesc     = 0x05,
+  UfsGeometryDesc   = 0x07,
+  UfsPowerDesc      = 0x08
+} UFS_DESC_IDN;
+
+//
+// UFS 2.0 Spec Section 14.1.6.2 - Device Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Device;
+  UINT8  DevClass;
+  UINT8  DevSubClass;
+  UINT8  Protocol;
+  UINT8  NumLun;
+  UINT8  NumWLun;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  SecurityLun;
+  UINT8  BgOpsTermLat;
+  UINT8  InitActiveIccLevel;
+  UINT16 SpecVersion;
+  UINT16 ManufactureDate;
+  UINT8  ManufacturerName;
+  UINT8  ProductName;
+  UINT8  SerialName;
+  UINT8  OemId;
+  UINT16 ManufacturerId;
+  UINT8  Ud0BaseOffset;
+  UINT8  Ud0ConfParamLen;
+  UINT8  DevRttCap;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd1[17];
+  UINT8  Rsvd2[16];
+} UFS_DEV_DESC;
+
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  Rsvd1;
+  UINT8  BootEn;
+  UINT8  DescAccessEn;
+  UINT8  InitPowerMode;
+  UINT8  HighPriorityLun;
+  UINT8  SecureRemovalType;
+  UINT8  InitActiveIccLevel;
+  UINT16 PeriodicRtcUpdate;
+  UINT8  Rsvd2[5];
+} UFS_CONFIG_DESC_GEN_HEADER;
+
+typedef struct {
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  MemType;
+  UINT32 NumAllocUnits;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT8  ProvisionType;
+  UINT16 CtxCap;
+  UINT8  Rsvd1[3];
+} UFS_UNIT_DESC_CONFIG_PARAMS;
+
+//
+// UFS 2.0 Spec Section 14.1.6.3 - Configuration Descriptor
+//
+typedef struct {
+  UFS_CONFIG_DESC_GEN_HEADER  Header;
+  UFS_UNIT_DESC_CONFIG_PARAMS UnitDescConfParams[8];
+} UFS_CONFIG_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.4 - Geometry Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  MediaTech;
+  UINT8  Rsvd1;
+  UINT64 TotalRawDevCapacity;
+  UINT8  Rsvd2;
+  UINT32 SegSize;
+  UINT8  AllocUnitSize;
+  UINT8  MinAddrBlkSize;
+  UINT8  OptReadBlkSize;
+  UINT8  OptWriteBlkSize;
+  UINT8  MaxInBufSize;
+  UINT8  MaxOutBufSize;
+  UINT8  RpmbRwSize;
+  UINT8  Rsvd3;
+  UINT8  DataOrder;
+  UINT8  MaxCtxIdNum;
+  UINT8  SysDataTagUnitSize;
+  UINT8  SysDataResUnitSize;
+  UINT8  SupSecRemovalTypes;
+  UINT16 SupMemTypes;
+  UINT32 SysCodeMaxNumAllocUnits;
+  UINT16 SupCodeCapAdjFac;
+  UINT32 NonPersMaxNumAllocUnits;
+  UINT16 NonPersCapAdjFac;
+  UINT32 Enhance1MaxNumAllocUnits;
+  UINT16 Enhance1CapAdjFac;
+  UINT32 Enhance2MaxNumAllocUnits;
+  UINT16 Enhance2CapAdjFac;
+  UINT32 Enhance3MaxNumAllocUnits;
+  UINT16 Enhance3CapAdjFac;
+  UINT32 Enhance4MaxNumAllocUnits;
+  UINT16 Enhance4CapAdjFac;
+} UFS_GEOMETRY_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.5 - Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  DataReliability;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT16 CtxCap;
+  UINT8  LargeUnitGranularity;
+} UFS_UNIT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.6 - RPMB Unit Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT8  UnitIdx;
+  UINT8  LunEn;
+  UINT8  BootLunId;
+  UINT8  LunWriteProt;
+  UINT8  LunQueueDep;
+  UINT8  Rsvd1;
+  UINT8  MemType;
+  UINT8  Rsvd2;
+  UINT8  LogicBlkSize;
+  UINT64 LogicBlkCount;
+  UINT32 EraseBlkSize;
+  UINT8  ProvisionType;
+  UINT64 PhyMemResCount;
+  UINT8  Rsvd3[3];
+} UFS_RPMB_UNIT_DESC;
+
+typedef struct {
+  UINT16 Value:10;
+  UINT16 Rsvd1:4;
+  UINT16 Unit:2;
+} UFS_POWER_PARAM_ELEMENT;
+
+//
+// UFS 2.0 Spec Section 14.1.6.7 - Power Parameter Descriptor
+//
+typedef struct {
+  UINT8                    Length;
+  UINT8                    DescType;
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVcc[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ[16];
+  UFS_POWER_PARAM_ELEMENT  ActiveIccLevelVccQ2[16];
+} UFS_POWER_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.8 - InterConnect Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  UINT16 UniProVer;
+  UINT16 MphyVer;
+} UFS_INTER_CONNECT_DESC;
+
+//
+// UFS 2.0 Spec Section 14.1.6.9 - 14.1.6.12 - String Descriptor
+//
+typedef struct {
+  UINT8  Length;
+  UINT8  DescType;
+  CHAR16 Unicode[126];
+} UFS_STRING_DESC;
+
+//
+// UFS 2.0 Spec Section 14.2 - Flags
+//
+typedef enum {
+  UfsFlagDevInit         = 0x01,
+  UfsFlagPermWpEn        = 0x02,
+  UfsFlagPowerOnWpEn     = 0x03,
+  UfsFlagBgOpsEn         = 0x04,
+  UfsFlagPurgeEn         = 0x06,
+  UfsFlagPhyResRemoval   = 0x08,
+  UfsFlagBusyRtc         = 0x09,
+  UfsFlagPermDisFwUpdate = 0x0B    
+} UFS_FLAGS_IDN;
+
+//
+// UFS 2.0 Spec Section 14.2 - Attributes
+//
+typedef enum {
+  UfsAttrBootLunEn         = 0x00,
+  UfsAttrCurPowerMode      = 0x02,
+  UfsAttrActiveIccLevel    = 0x03,
+  UfsAttrOutOfOrderDataEn  = 0x04,
+  UfsAttrBgOpStatus        = 0x05,
+  UfsAttrPurgeStatus       = 0x06,
+  UfsAttrMaxDataInSize     = 0x07,
+  UfsAttrMaxDataOutSize    = 0x08,
+  UfsAttrDynCapNeeded      = 0x09,
+  UfsAttrRefClkFreq        = 0x0a,
+  UfsAttrConfigDescLock    = 0x0b,
+  UfsAttrMaxNumOfRtt       = 0x0c,
+  UfsAttrExceptionEvtCtrl  = 0x0d,
+  UfsAttrExceptionEvtSts   = 0x0e,
+  UfsAttrSecondsPassed     = 0x0f,
+  UfsAttrContextConf       = 0x10,
+  UfsAttrCorrPrgBlkNum     = 0x11
+} UFS_ATTR_IDN;
+
+typedef enum {
+  UfsNoData                = 0,
+  UfsDataOut               = 1,
+  UfsDataIn                = 2,
+  UfsDdReserved
+} UFS_DATA_DIRECTION;
+
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c
new file mode 100644
index 0000000000..55c4a537fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.c
@@ -0,0 +1,401 @@
+/** @file
+BOT Transportation implementation.
+
+Copyright (c) 2006, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+#include "PeiUsbLib.h"
+
+/**
+  Reset the given usb device.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+
+  @retval EFI_INVALID_PARAMETER  Can not get usb io ppi.
+  @retval EFI_SUCCESS            Failed to reset the given usb device.
+
+**/
+EFI_STATUS
+BotRecoveryReset (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  UINT32                  Timeout;
+  PEI_USB_IO_PPI          *UsbIoPpi;
+  UINT8                   EndpointAddr;
+  EFI_STATUS              Status;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  if (UsbIoPpi == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = 0x21;
+  DevReq.Request      = 0xFF;
+  DevReq.Value        = 0;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  Timeout             = 3000;
+
+  Status = UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbNoData,
+                      Timeout,
+                      NULL,
+                      0
+                      );
+
+  //
+  // clear bulk in endpoint stall feature
+  //
+  EndpointAddr = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+  PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+
+  //
+  // clear bulk out endpoint stall feature
+  //
+  EndpointAddr = (PeiBotDev->BulkOutEndpoint)->EndpointAddress;
+  PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+
+  return Status;
+}
+
+/**
+  Send the command to the device using Bulk-Out endpoint.
+
+  This function sends the command to the device using Bulk-Out endpoint.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Command phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to transfer to device.
+  @param  CommandSize            The length of the command.
+  @param  DataTransferLength     The expected length of the data.
+  @param  Direction              The direction of the data.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to send the command to device.
+  @retval EFI_SUCCESS            Failed to send the command to device.
+
+**/
+EFI_STATUS
+BotCommandPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  IN  VOID                      *Command,
+  IN  UINT8                     CommandSize,
+  IN  UINT32                    DataTransferLength,
+  IN  EFI_USB_DATA_DIRECTION    Direction,
+  IN  UINT16                    Timeout
+  )
+{
+  CBW             Cbw;
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINTN           DataSize;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  ZeroMem (&Cbw, sizeof (CBW));
+
+  //
+  // Fill the command block, detailed see BOT spec
+  //
+  Cbw.Signature           = CBWSIG;
+  Cbw.Tag                 = 0x01;
+  Cbw.DataTransferLength  = DataTransferLength;
+  Cbw.Flags               = (UINT8) ((Direction == EfiUsbDataIn) ? 0x80 : 0);
+  Cbw.Lun                 = 0;
+  Cbw.CmdLen              = CommandSize;
+
+  CopyMem (Cbw.CmdBlock, Command, CommandSize);
+
+  DataSize = sizeof (CBW);
+
+  Status = UsbIoPpi->UsbBulkTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      (PeiBotDev->BulkOutEndpoint)->EndpointAddress,
+                      (UINT8 *) &Cbw,
+                      &DataSize,
+                      Timeout
+                      );
+  if (EFI_ERROR (Status)) {
+    //
+    // Command phase fail, we need to recovery reset this device
+    //
+    BotRecoveryReset (PeiServices, PeiBotDev);
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Transfer the data between the device and host.
+
+  This function transfers the data between the device and host.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Data phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  DataSize               The length of the data.
+  @param  DataBuffer             The pointer to the data.
+  @param  Direction              The direction of the data.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to send the data to device.
+  @retval EFI_SUCCESS            Failed to send the data to device.
+
+**/
+EFI_STATUS
+BotDataPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  IN  UINT32                    *DataSize,
+  IN  OUT VOID                  *DataBuffer,
+  IN  EFI_USB_DATA_DIRECTION    Direction,
+  IN  UINT16                    Timeout
+  )
+{
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINT8           EndpointAddr;
+  UINTN           Remain;
+  UINTN           Increment;
+  UINT32          MaxPacketLen;
+  UINT8           *BufferPtr;
+  UINTN           TransferredSize;
+
+  UsbIoPpi        = PeiBotDev->UsbIoPpi;
+
+  Remain          = *DataSize;
+  BufferPtr       = (UINT8 *) DataBuffer;
+  TransferredSize = 0;
+
+  //
+  // retrieve the the max packet length of the given endpoint
+  //
+  if (Direction == EfiUsbDataIn) {
+    MaxPacketLen  = (PeiBotDev->BulkInEndpoint)->MaxPacketSize;
+    EndpointAddr  = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+  } else {
+    MaxPacketLen  = (PeiBotDev->BulkOutEndpoint)->MaxPacketSize;
+    EndpointAddr  = (PeiBotDev->BulkOutEndpoint)->EndpointAddress;
+  }
+
+  while (Remain > 0) {
+    //
+    // Using 15 packets to avoid Bitstuff error
+    //
+    if (Remain > 16 * MaxPacketLen) {
+      Increment = 16 * MaxPacketLen;
+    } else {
+      Increment = Remain;
+    }
+
+    Status = UsbIoPpi->UsbBulkTransfer (
+                        PeiServices,
+                        UsbIoPpi,
+                        EndpointAddr,
+                        BufferPtr,
+                        &Increment,
+                        Timeout
+                        );
+
+    TransferredSize += Increment;
+
+    if (EFI_ERROR (Status)) {
+      PeiUsbClearEndpointHalt (PeiServices, UsbIoPpi, EndpointAddr);
+      return Status;
+    }
+
+    BufferPtr += Increment;
+    Remain -= Increment;
+  }
+
+  *DataSize = (UINT32) TransferredSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the command execution status from device.
+
+  This function gets the command execution status from device.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Status phase.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  TransferStatus         The status of the transaction.
+  @param  Timeout                Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+BotStatusPhase (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_BOT_DEVICE            *PeiBotDev,
+  OUT UINT8                     *TransferStatus,
+  IN  UINT16                    Timeout
+  )
+{
+  CSW             Csw;
+  EFI_STATUS      Status;
+  PEI_USB_IO_PPI  *UsbIoPpi;
+  UINT8           EndpointAddr;
+  UINTN           DataSize;
+
+  UsbIoPpi = PeiBotDev->UsbIoPpi;
+
+  ZeroMem (&Csw, sizeof (CSW));
+
+  EndpointAddr  = (PeiBotDev->BulkInEndpoint)->EndpointAddress;
+
+  DataSize      = sizeof (CSW);
+
+  //
+  // Get the status field from bulk transfer
+  //
+  Status = UsbIoPpi->UsbBulkTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      EndpointAddr,
+                      &Csw,
+                      &DataSize,
+                      Timeout
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Csw.Signature == CSWSIG) {
+    *TransferStatus = Csw.Status;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send ATAPI command using BOT protocol.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to be sent to ATAPI device.
+  @param  CommandSize            The length of the data to be sent.
+  @param  DataBuffer             The pointer to the data.
+  @param  BufferLength           The length of the data.
+  @param  Direction              The direction of the data.
+  @param  TimeOutInMilliSeconds  Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+PeiAtapiCommand (
+  IN  EFI_PEI_SERVICES            **PeiServices,
+  IN  PEI_BOT_DEVICE              *PeiBotDev,
+  IN  VOID                        *Command,
+  IN  UINT8                       CommandSize,
+  IN  VOID                        *DataBuffer,
+  IN  UINT32                      BufferLength,
+  IN  EFI_USB_DATA_DIRECTION      Direction,
+  IN  UINT16                      TimeOutInMilliSeconds
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STATUS  BotDataStatus;
+  UINT8       TransferStatus;
+  UINT32      BufferSize;
+
+  BotDataStatus = EFI_SUCCESS;
+  //
+  // First send ATAPI command through Bot
+  //
+  Status = BotCommandPhase (
+            PeiServices,
+            PeiBotDev,
+            Command,
+            CommandSize,
+            BufferLength,
+            Direction,
+            TimeOutInMilliSeconds
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Send/Get Data if there is a Data Stage
+  //
+  switch (Direction) {
+  case EfiUsbDataIn:
+  case EfiUsbDataOut:
+    BufferSize = BufferLength;
+
+    BotDataStatus = BotDataPhase (
+                      PeiServices,
+                      PeiBotDev,
+                      &BufferSize,
+                      DataBuffer,
+                      Direction,
+                      TimeOutInMilliSeconds
+                      );
+    break;
+
+  case EfiUsbNoData:
+    break;
+  }
+  //
+  // Status Phase
+  //
+  Status = BotStatusPhase (
+            PeiServices,
+            PeiBotDev,
+            &TransferStatus,
+            TimeOutInMilliSeconds
+            );
+  if (EFI_ERROR (Status)) {
+    BotRecoveryReset (PeiServices, PeiBotDev);
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (TransferStatus == 0x01) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return BotDataStatus;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h
new file mode 100644
index 0000000000..07235c397b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/BotPeim.h
@@ -0,0 +1,224 @@
+/** @file
+BOT Transportation implementation.
+
+Copyright (c) 2006, 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.
+
+**/
+
+#ifndef _PEI_BOT_PEIM_H_
+#define _PEI_BOT_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/UsbHostController.h>
+#include <Ppi/BlockIo.h>
+
+//#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+
+#include <IndustryStandard/Atapi.h>
+
+#pragma pack(1)
+//
+// Bulk Only device protocol
+//
+typedef struct {
+  UINT32  Signature;
+  UINT32  Tag;
+  UINT32  DataTransferLength;
+  UINT8   Flags;
+  UINT8   Lun;
+  UINT8   CmdLen;
+  UINT8   CmdBlock[16];
+} CBW;
+
+typedef struct {
+  UINT32  Signature;
+  UINT32  Tag;
+  UINT32  DataResidue;
+  UINT8   Status;
+} CSW;
+
+#pragma pack()
+//
+// Status code, see Usb Bot device spec
+//
+#define CSWSIG  0x53425355
+#define CBWSIG  0x43425355
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device. This command will
+  return INQUIRY data of the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       Inquiry command completes successfully.
+  @retval EFI_DEVICE_ERROR  Inquiry command failed.
+
+**/
+EFI_STATUS
+PeiUsbInquiry (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+PeiUsbTestUnitReady (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+  @param SenseCounts    Length of sense buffer.
+  @param SenseKeyBuffer Pointer to sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRequestSense (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  OUT UINTN             *SenseCounts,
+  IN  UINT8             *SenseKeyBuffer
+  );
+
+/**
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Sends out ATAPI Read Format Capacity Data Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadFormattedCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  );
+
+/**
+  Execute Read(10) ATAPI command on a specific SCSI target.
+
+  Executes the ATAPI Read(10) command on the ATAPI target specified by PeiBotDevice.
+
+  @param PeiServices       The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice      The pointer to PEI_BOT_DEVICE instance.
+  @param Buffer            The pointer to data buffer.
+  @param Lba               The start logic block address of reading.
+  @param NumberOfBlocks    The block number of reading.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRead10 (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_PEI_LBA       Lba,
+  IN  UINTN             NumberOfBlocks
+  );
+
+/**  
+  Check if there is media according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+/**  
+  Check if media is changed according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    There is media change event.
+  @retval FALSE   media is NOT changed.
+
+**/
+BOOLEAN
+IsMediaChange (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                 SenseCounts
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c
new file mode 100644
index 0000000000..eaad626d61
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiAtapi.c
@@ -0,0 +1,654 @@
+/** @file
+Pei USB ATATPI command implementations.
+
+Copyright (c) 1999 - 2015, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+
+#define MAXSENSEKEY 5
+
+/**
+  Sends out ATAPI Inquiry Packet Command to the specified device. This command will
+  return INQUIRY data of the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       Inquiry command completes successfully.
+  @retval EFI_DEVICE_ERROR  Inquiry command failed.
+
+**/
+EFI_STATUS
+PeiUsbInquiry (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+  ATAPI_INQUIRY_DATA          Idata;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  ZeroMem (&Idata, sizeof (ATAPI_INQUIRY_DATA));
+
+  Packet.Inquiry.opcode             = ATA_CMD_INQUIRY;
+  Packet.Inquiry.page_code          = 0;
+  Packet.Inquiry.allocation_length  = 36;
+
+  //
+  // Send scsi INQUIRY command packet.
+  // According to SCSI Primary Commands-2 spec, host only needs to
+  // retrieve the first 36 bytes for standard INQUIRY data.
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            &Idata,
+            36,
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if ((Idata.peripheral_type & 0x1f) == 0x05) {
+    PeiBotDevice->DeviceType      = USBCDROM;
+    PeiBotDevice->Media.BlockSize = 0x800;
+    PeiBotDevice->Media2.ReadOnly       = TRUE;
+    PeiBotDevice->Media2.RemovableMedia = TRUE;
+    PeiBotDevice->Media2.BlockSize      = 0x800;
+  } else {
+    PeiBotDevice->DeviceType      = USBFLOPPY;
+    PeiBotDevice->Media.BlockSize = 0x200;
+    PeiBotDevice->Media2.ReadOnly       = FALSE;
+    PeiBotDevice->Media2.RemovableMedia = TRUE;
+    PeiBotDevice->Media2.BlockSize      = 0x200;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Test Unit Ready Packet Command to the specified device
+  to find out whether device is accessible.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS        TestUnit command executed successfully.
+  @retval EFI_DEVICE_ERROR   Device cannot be executed TestUnit command successfully.
+
+**/
+EFI_STATUS
+PeiUsbTestUnitReady (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  EFI_STATUS            Status;
+
+  //
+  // fill command packet
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.TestUnitReady.opcode = ATA_CMD_TEST_UNIT_READY;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            NULL,
+            0,
+            EfiUsbNoData,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Request Sense Packet Command to the specified device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+  @param SenseCounts    Length of sense buffer.
+  @param SenseKeyBuffer Pointer to sense buffer.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRequestSense (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  OUT UINTN             *SenseCounts,
+  IN  UINT8             *SenseKeyBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  ATAPI_PACKET_COMMAND        Packet;
+  UINT8                       *Ptr;
+  BOOLEAN                     SenseReq;
+  ATAPI_REQUEST_SENSE_DATA    *Sense;
+
+  *SenseCounts = 0;
+
+  //
+  // fill command packet for Request Sense Packet Command
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Packet.RequestSence.opcode            = ATA_CMD_REQUEST_SENSE;
+  Packet.RequestSence.allocation_length = (UINT8) sizeof (ATAPI_REQUEST_SENSE_DATA);
+
+  Ptr = SenseKeyBuffer;
+
+  SenseReq = TRUE;
+
+  //
+  //  request sense data from device continuously
+  //  until no sense data exists in the device.
+  //
+  while (SenseReq) {
+    Sense = (ATAPI_REQUEST_SENSE_DATA *) Ptr;
+
+    //
+    // send out Request Sense Packet Command and get one Sense
+    // data form device.
+    //
+    Status = PeiAtapiCommand (
+              PeiServices,
+              PeiBotDevice,
+              &Packet,
+              (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+              (VOID *) Ptr,
+              sizeof (ATAPI_REQUEST_SENSE_DATA),
+              EfiUsbDataIn,
+              2000
+              );
+
+    //
+    // failed to get Sense data
+    //
+    if (EFI_ERROR (Status)) {
+      if (*SenseCounts == 0) {
+        return EFI_DEVICE_ERROR;
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+
+    if (Sense->sense_key != ATA_SK_NO_SENSE) {
+
+      Ptr += sizeof (ATAPI_REQUEST_SENSE_DATA);
+      //
+      // Ptr is byte based pointer
+      //
+      (*SenseCounts)++;
+
+      if (*SenseCounts == MAXSENSEKEY) {
+        break;
+      }
+
+    } else {
+      //
+      // when no sense key, skip out the loop
+      //
+      SenseReq = FALSE;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Read Capacity Packet Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  EFI_STATUS                  Status;
+  ATAPI_PACKET_COMMAND        Packet;
+  ATAPI_READ_CAPACITY_DATA    Data;
+  UINT32                      LastBlock;
+
+  ZeroMem (&Data, sizeof (ATAPI_READ_CAPACITY_DATA));
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+
+  Packet.Inquiry.opcode = ATA_CMD_READ_CAPACITY;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            (VOID *) &Data,
+            sizeof (ATAPI_READ_CAPACITY_DATA),
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  LastBlock = (Data.LastLba3 << 24) | (Data.LastLba2 << 16) | (Data.LastLba1 << 8) | Data.LastLba0;
+
+  if (LastBlock == 0xFFFFFFFF) {
+    DEBUG ((EFI_D_INFO, "The usb device LBA count is larger than 0xFFFFFFFF!\n"));
+  }
+
+  PeiBotDevice->Media.LastBlock    = LastBlock;
+  PeiBotDevice->Media.MediaPresent = TRUE;
+
+  PeiBotDevice->Media2.LastBlock    = LastBlock;
+  PeiBotDevice->Media2.MediaPresent = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sends out ATAPI Read Format Capacity Data Command to the specified device.
+  This command will return the information regarding the capacity of the
+  media in the device.
+
+  @param PeiServices    The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice   The pointer to PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbReadFormattedCapacity (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice
+  )
+{
+  EFI_STATUS                      Status;
+  ATAPI_PACKET_COMMAND            Packet;
+  ATAPI_READ_FORMAT_CAPACITY_DATA FormatData;
+  UINT32                          LastBlock;
+
+  ZeroMem (&FormatData, sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA));
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+
+  Packet.ReadFormatCapacity.opcode                = ATA_CMD_READ_FORMAT_CAPACITY;
+  Packet.ReadFormatCapacity.allocation_length_lo  = 12;
+
+  //
+  // send command packet
+  //
+  Status = PeiAtapiCommand (
+            PeiServices,
+            PeiBotDevice,
+            &Packet,
+            (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+            (VOID *) &FormatData,
+            sizeof (ATAPI_READ_FORMAT_CAPACITY_DATA),
+            EfiUsbDataIn,
+            2000
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (FormatData.DesCode == 3) {
+    //
+    // Media is not present
+    //
+    PeiBotDevice->Media.MediaPresent  = FALSE;
+    PeiBotDevice->Media.LastBlock     = 0;
+    PeiBotDevice->Media2.MediaPresent  = FALSE;
+    PeiBotDevice->Media2.LastBlock     = 0;
+
+  } else {
+    LastBlock = (FormatData.LastLba3 << 24) | (FormatData.LastLba2 << 16) | (FormatData.LastLba1 << 8) | FormatData.LastLba0;
+    if (LastBlock == 0xFFFFFFFF) {
+      DEBUG ((EFI_D_INFO, "The usb device LBA count is larger than 0xFFFFFFFF!\n"));
+    }
+
+    PeiBotDevice->Media.LastBlock = LastBlock;
+
+    PeiBotDevice->Media.LastBlock--;
+
+    PeiBotDevice->Media.MediaPresent = TRUE;
+
+    PeiBotDevice->Media2.MediaPresent = TRUE;
+    PeiBotDevice->Media2.LastBlock    = PeiBotDevice->Media.LastBlock;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute Read(10) ATAPI command on a specific SCSI target.
+
+  Executes the ATAPI Read(10) command on the ATAPI target specified by PeiBotDevice.
+
+  @param PeiServices       The pointer of EFI_PEI_SERVICES.
+  @param PeiBotDevice      The pointer to PEI_BOT_DEVICE instance.
+  @param Buffer            The pointer to data buffer.
+  @param Lba               The start logic block address of reading.
+  @param NumberOfBlocks    The block number of reading.
+
+  @retval EFI_SUCCESS           Command executed successfully.
+  @retval EFI_DEVICE_ERROR      Some device errors happen.
+
+**/
+EFI_STATUS
+PeiUsbRead10 (
+  IN  EFI_PEI_SERVICES  **PeiServices,
+  IN  PEI_BOT_DEVICE    *PeiBotDevice,
+  IN  VOID              *Buffer,
+  IN  EFI_PEI_LBA       Lba,
+  IN  UINTN             NumberOfBlocks
+  )
+{
+  ATAPI_PACKET_COMMAND  Packet;
+  ATAPI_READ10_CMD      *Read10Packet;
+  UINT16                MaxBlock;
+  UINT16                BlocksRemaining;
+  UINT16                SectorCount;
+  UINT32                Lba32;
+  UINT32                BlockSize;
+  UINT32                ByteCount;
+  VOID                  *PtrBuffer;
+  EFI_STATUS            Status;
+  UINT16                TimeOut;
+
+  //
+  // prepare command packet for the Inquiry Packet Command.
+  //
+  ZeroMem (&Packet, sizeof (ATAPI_PACKET_COMMAND));
+  Read10Packet    = &Packet.Read10;
+  Lba32           = (UINT32) Lba;
+  PtrBuffer       = Buffer;
+
+  BlockSize       = (UINT32) PeiBotDevice->Media.BlockSize;
+
+  MaxBlock        = (UINT16) (65535 / BlockSize);
+  BlocksRemaining = (UINT16) NumberOfBlocks;
+
+  Status          = EFI_SUCCESS;
+  while (BlocksRemaining > 0) {
+
+    if (BlocksRemaining <= MaxBlock) {
+
+      SectorCount = BlocksRemaining;
+
+    } else {
+
+      SectorCount = MaxBlock;
+    }
+    //
+    // fill the Packet data structure
+    //
+    Read10Packet->opcode = ATA_CMD_READ_10;
+
+    //
+    // Lba0 ~ Lba3 specify the start logical block address of the data transfer.
+    // Lba0 is MSB, Lba3 is LSB
+    //
+    Read10Packet->Lba3  = (UINT8) (Lba32 & 0xff);
+    Read10Packet->Lba2  = (UINT8) (Lba32 >> 8);
+    Read10Packet->Lba1  = (UINT8) (Lba32 >> 16);
+    Read10Packet->Lba0  = (UINT8) (Lba32 >> 24);
+
+    //
+    // TranLen0 ~ TranLen1 specify the transfer length in block unit.
+    // TranLen0 is MSB, TranLen is LSB
+    //
+    Read10Packet->TranLen1  = (UINT8) (SectorCount & 0xff);
+    Read10Packet->TranLen0  = (UINT8) (SectorCount >> 8);
+
+    ByteCount               = SectorCount * BlockSize;
+
+    TimeOut                 = (UINT16) (SectorCount * 2000);
+
+    //
+    // send command packet
+    //
+    Status = PeiAtapiCommand (
+              PeiServices,
+              PeiBotDevice,
+              &Packet,
+              (UINT8) sizeof (ATAPI_PACKET_COMMAND),
+              (VOID *) PtrBuffer,
+              ByteCount,
+              EfiUsbDataIn,
+              TimeOut
+              );
+
+    if (Status != EFI_SUCCESS) {
+      return Status;
+    }
+
+    Lba32 += SectorCount;
+    PtrBuffer       = (UINT8 *) PtrBuffer + SectorCount * BlockSize;
+    BlocksRemaining = (UINT16) (BlocksRemaining - SectorCount);
+  }
+
+  return Status;
+}
+
+/**  
+  Check if there is media according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    No media
+  @retval FALSE   Media exists
+
+**/
+BOOLEAN
+IsNoMedia (
+  IN  ATAPI_REQUEST_SENSE_DATA *SenseData,
+  IN  UINTN                    SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   NoMedia;
+
+  NoMedia   = FALSE;
+  SensePtr  = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      //
+      // if no media, fill IdeDev parameter with specific info.
+      //
+      case ATA_ASC_NO_MEDIA:
+        NoMedia = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return NoMedia;
+}
+
+/**  
+  Check if there is media error according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    Media error
+  @retval FALSE   No media error
+
+**/
+BOOLEAN
+IsMediaError (
+  IN  ATAPI_REQUEST_SENSE_DATA    *SenseData,
+  IN  UINTN                       SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   Error;
+
+  SensePtr  = SenseData;
+  Error     = FALSE;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+
+    switch (SensePtr->sense_key) {
+    //
+    // Medium error case
+    //
+    case ATA_SK_MEDIUM_ERROR:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_ERR1:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR2:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR3:
+        //
+        // fall through
+        //
+      case ATA_ASC_MEDIA_ERR4:
+        Error = TRUE;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    //
+    // Medium upside-down case
+    //
+    case ATA_SK_NOT_READY:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_UPSIDE_DOWN:
+        Error = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return Error;
+}
+
+/**  
+  Check if media is changed according to sense data.
+
+  @param  SenseData   Pointer to sense data.
+  @param  SenseCounts Count of sense data.
+
+  @retval TRUE    There is media change event.
+  @retval FALSE   media is NOT changed.
+
+**/
+BOOLEAN
+IsMediaChange (
+  IN  ATAPI_REQUEST_SENSE_DATA *SenseData,
+  IN  UINTN                    SenseCounts
+  )
+{
+  ATAPI_REQUEST_SENSE_DATA  *SensePtr;
+  UINTN                     Index;
+  BOOLEAN                   MediaChange;
+
+  MediaChange = FALSE;
+
+  SensePtr    = SenseData;
+
+  for (Index = 0; Index < SenseCounts; Index++) {
+    //
+    // catch media change sense key and addition sense data
+    //
+    switch (SensePtr->sense_key) {
+    case ATA_SK_UNIT_ATTENTION:
+      switch (SensePtr->addnl_sense_code) {
+      case ATA_ASC_MEDIA_CHANGE:
+        MediaChange = TRUE;
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    SensePtr++;
+  }
+
+  return MediaChange;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c
new file mode 100644
index 0000000000..46c1a06444
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.c
@@ -0,0 +1,331 @@
+/** @file
+Common Libarary  for PEI USB.
+
+Copyright (c) 2006 - 2014, 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 "PeiUsbLib.h"
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS        Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the usb descriptor due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_GET_DESCRIPTOR_REQ_TYPE;
+  DevReq.Request      = USB_DEV_GET_DESCRIPTOR;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = DescriptorLength;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     Descriptor,
+                     DescriptorLength
+                     );
+}
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_SET_ADDRESS_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_ADDRESS;
+  DevReq.Value        = AddressValue;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  switch (Recipient) {
+  case EfiUsbDevice:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_D;
+    break;
+
+  case EfiUsbInterface:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_I;
+    break;
+
+  case EfiUsbEndpoint:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_E;
+    break;
+  }
+
+  DevReq.Request      = USB_DEV_CLEAR_FEATURE;
+  DevReq.Value        = Value;
+  DevReq.Index        = Target;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb device due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = USB_DEV_SET_CONFIGURATION_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_CONFIGURATION;
+  DevReq.Value        = 1;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear Endpoint Halt.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointAddress   The endpoint address.
+
+  @retval EFI_SUCCESS       Endpoint halt is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the endpoint halt status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearEndpointHalt (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT8                    EndpointAddress
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDescriptor;
+  UINT8                         EndpointIndex;
+
+
+  //
+  // Check its interface
+  //
+  Status = UsbIoPpi->UsbGetInterfaceDescriptor (
+                      PeiServices,
+                      UsbIoPpi,
+                      &InterfaceDesc
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  for (EndpointIndex = 0; EndpointIndex < InterfaceDesc->NumEndpoints; EndpointIndex++) {
+    Status = UsbIoPpi->UsbGetEndpointDescriptor (PeiServices, UsbIoPpi, EndpointIndex, &EndpointDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (EndpointDescriptor->EndpointAddress == EndpointAddress) {
+      break;
+    }
+  }
+
+  if (EndpointIndex == InterfaceDesc->NumEndpoints) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PeiUsbClearDeviceFeature (
+            PeiServices,
+            UsbIoPpi,
+            EfiUsbEndpoint,
+            EfiUsbEndpointHalt,
+            EndpointAddress
+            );
+
+  return Status;
+}
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 0 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Judge if the port is connected with a low-speed usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A low-speed usb device is connected with the port.
+  @retval FALSE       No low-speed usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortLowSpeedDeviceAttached (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 9 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  )
+{
+  //
+  // return the bit 0 value of PortChangeStatus
+  //
+  if ((PortChangeStatus & USB_PORT_STAT_C_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h
new file mode 100644
index 0000000000..eafccfdbf8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/PeiUsbLib.h
@@ -0,0 +1,248 @@
+/** @file
+Common Libarary  for PEI USB.
+
+Copyright (c) 1999 - 2010, 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.
+
+**/
+
+#ifndef _PEI_USB_LIB_H_
+#define _PEI_USB_LIB_H_
+//
+// Standard device request and request type
+// By [Spec-USB20/Chapter-9.4]
+//
+#define USB_DEV_GET_STATUS                  0x00
+#define USB_DEV_GET_STATUS_REQ_TYPE_D       0x80 // Receiver : Device
+#define USB_DEV_GET_STATUS_REQ_TYPE_I       0x81 // Receiver : Interface
+#define USB_DEV_GET_STATUS_REQ_TYPE_E       0x82 // Receiver : Endpoint
+
+#define USB_DEV_CLEAR_FEATURE               0x01
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_D    0x00 // Receiver : Device
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_I    0x01 // Receiver : Interface
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_E    0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_FEATURE                 0x03
+#define USB_DEV_SET_FEATURE_REQ_TYPE_D      0x00 // Receiver : Device
+#define USB_DEV_SET_FEATURE_REQ_TYPE_I      0x01 // Receiver : Interface
+#define USB_DEV_SET_FEATURE_REQ_TYPE_E      0x02 // Receiver : Endpoint
+ 
+#define USB_DEV_SET_ADDRESS                 0x05
+#define USB_DEV_SET_ADDRESS_REQ_TYPE        0x00
+
+#define USB_DEV_GET_DESCRIPTOR              0x06
+#define USB_DEV_GET_DESCRIPTOR_REQ_TYPE     0x80
+
+#define USB_DEV_SET_DESCRIPTOR              0x07
+#define USB_DEV_SET_DESCRIPTOR_REQ_TYPE     0x00
+
+#define USB_DEV_GET_CONFIGURATION           0x08
+#define USB_DEV_GET_CONFIGURATION_REQ_TYPE  0x80
+
+#define USB_DEV_SET_CONFIGURATION           0x09
+#define USB_DEV_SET_CONFIGURATION_REQ_TYPE  0x00
+
+#define USB_DEV_GET_INTERFACE               0x0A
+#define USB_DEV_GET_INTERFACE_REQ_TYPE      0x81
+
+#define USB_DEV_SET_INTERFACE               0x0B
+#define USB_DEV_SET_INTERFACE_REQ_TYPE      0x01
+
+#define USB_DEV_SYNCH_FRAME                 0x0C
+#define USB_DEV_SYNCH_FRAME_REQ_TYPE        0x82
+
+//
+// USB Descriptor types
+//
+#define USB_DT_DEVICE     0x01
+#define USB_DT_CONFIG     0x02
+#define USB_DT_STRING     0x03
+#define USB_DT_INTERFACE  0x04
+#define USB_DT_ENDPOINT   0x05
+#define USB_DT_HUB        0x29
+#define USB_DT_HID        0x21
+
+//
+// USB request type
+//
+#define USB_TYPE_STANDARD (0x00 << 5)
+#define USB_TYPE_CLASS    (0x01 << 5)
+#define USB_TYPE_VENDOR   (0x02 << 5)
+#define USB_TYPE_RESERVED (0x03 << 5)
+
+//
+// USB request targer device
+//
+#define USB_RECIP_DEVICE    0x00
+#define USB_RECIP_INTERFACE 0x01
+#define USB_RECIP_ENDPOINT  0x02
+#define USB_RECIP_OTHER     0x03
+
+typedef enum {
+  EfiUsbEndpointHalt,
+  EfiUsbDeviceRemoteWakeup
+} EFI_USB_STANDARD_FEATURE_SELECTOR;
+
+//
+// Usb Data recipient type
+//
+typedef enum {
+  EfiUsbDevice,
+  EfiUsbInterface,
+  EfiUsbEndpoint
+} EFI_USB_RECIPIENT;
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  );
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  );
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  );
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS        Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb device due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  );
+
+/**
+  Clear Endpoint Halt.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointAddress   The endpoint address.
+
+  @retval EFI_SUCCESS       Endpoint halt is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the endpoint halt status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearEndpointHalt (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT8                    EndpointAddress
+  );
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Judge if the port is connected with a low-speed usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A low-speed usb device is connected with the port.
+  @retval FALSE       No low-speed usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortLowSpeedDeviceAttached (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf
new file mode 100644
index 0000000000..977bef5c8b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf
@@ -0,0 +1,69 @@
+## @file
+# The Usb mass storage device Peim driver is used to support recovery from USB device.
+#
+# Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBotPei
+  MODULE_UNI_FILE                = UsbBotPei.uni
+  FILE_GUID                      = 8401A046-6F70-4505-8471-7015B40355E3
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = PeimInitializeUsbBot
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PeiUsbLib.c
+  PeiAtapi.c
+  BotPeim.c
+  UsbBotPeim.c
+  UsbPeim.h
+  UsbBotPeim.h
+  PeiUsbLib.h
+  BotPeim.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  PcdLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUsbTransferTimeoutValue  ## CONSUMES
+
+[Ppis]
+  gEfiPeiVirtualBlockIoPpiGuid                  ## PRODUCES
+  gEfiPeiVirtualBlockIo2PpiGuid                 ## PRODUCES
+  ## CONSUMES
+  ## NOTIFY
+  gPeiUsbIoPpiGuid
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gPeiUsbIoPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBotPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni
new file mode 100644
index 0000000000..3f7125ab57
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni
new file mode 100644
index 0000000000..2e2cdf5a7b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c
new file mode 100644
index 0000000000..5e18306652
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.c
@@ -0,0 +1,922 @@
+/** @file
+
+Copyright (c) 2006 - 2015, 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 "UsbBotPeim.h"
+#include "BotPeim.h"
+
+//
+// Global function
+//
+EFI_PEI_NOTIFY_DESCRIPTOR        mNotifyList = {
+  EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+  &gPeiUsbIoPpiGuid,
+  NotifyOnUsbIoPpi
+};
+
+EFI_PEI_RECOVERY_BLOCK_IO_PPI    mRecoveryBlkIoPpi = {
+  BotGetNumberOfBlockDevices,
+  BotGetMediaInfo,
+  BotReadBlocks
+};
+
+EFI_PEI_RECOVERY_BLOCK_IO2_PPI   mRecoveryBlkIo2Ppi = {
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI_REVISION,
+  BotGetNumberOfBlockDevices2,
+  BotGetMediaInfo2,
+  BotReadBlocks2
+};
+
+EFI_PEI_PPI_DESCRIPTOR           mPpiList[2] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gEfiPeiVirtualBlockIoPpiGuid,
+    NULL
+  },
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiVirtualBlockIo2PpiGuid,
+    NULL
+  }
+};
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  PeiBotDev     Indicates the PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       The media status is successfully checked.
+  @retval Other             Failed to detect media.
+
+**/
+EFI_STATUS
+PeiBotDetectMedia (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_BOT_DEVICE                            *PeiBotDev
+  );
+
+/**
+  Initializes the Usb Bot. 
+  
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS            Usb bot driver is successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Can't initialize the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeUsbBot (
+  IN EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       UsbIoPpiInstance;
+  EFI_PEI_PPI_DESCRIPTOR      *TempPpiDescriptor;
+  PEI_USB_IO_PPI              *UsbIoPpi;
+
+  //
+  // Shadow this PEIM to run from memory
+  //
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // locate all usb io PPIs
+  //
+  for (UsbIoPpiInstance = 0; UsbIoPpiInstance < PEI_FAT_MAX_USB_IO_PPI; UsbIoPpiInstance++) {
+
+    Status = PeiServicesLocatePpi (
+                              &gPeiUsbIoPpiGuid,
+                              UsbIoPpiInstance,
+                              &TempPpiDescriptor,
+                              (VOID **) &UsbIoPpi
+                              );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+  //
+  // Register a notify function
+  //
+  return PeiServicesNotifyPpi (&mNotifyList);
+}
+
+/**
+  UsbIo installation notification function. 
+  
+  This function finds out all the current USB IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices      Indirect reference to the PEI Services Table.
+  @param  NotifyDesc       Address of the notification descriptor data structure.
+  @param  InvokePpi        Address of the PPI that was invoked.
+
+  @retval EFI_SUCCESS      The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyOnUsbIoPpi (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_NOTIFY_DESCRIPTOR                     *NotifyDesc,
+  IN  VOID                                          *InvokePpi
+  )
+{
+  PEI_USB_IO_PPI  *UsbIoPpi;
+
+  UsbIoPpi = (PEI_USB_IO_PPI *) InvokePpi;
+
+  InitUsbBot (PeiServices, UsbIoPpi);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the usb bot device.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       The usb bot device is initialized successfully.
+  @retval Other             Failed to initialize media.
+
+**/
+EFI_STATUS
+InitUsbBot (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_USB_IO_PPI                            *UsbIoPpi
+  )
+{
+  PEI_BOT_DEVICE                *PeiBotDevice;
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  UINTN                         MemPages;
+  EFI_PHYSICAL_ADDRESS          AllocateAddress;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDesc;
+  UINT8                         Index;
+
+  //
+  // Check its interface
+  //
+  Status = UsbIoPpi->UsbGetInterfaceDescriptor (
+                      PeiServices,
+                      UsbIoPpi,
+                      &InterfaceDesc
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Check if it is the BOT device we support
+  //
+  if ((InterfaceDesc->InterfaceClass != 0x08) || (InterfaceDesc->InterfaceProtocol != 0x50)) {
+
+    return EFI_NOT_FOUND;
+  }
+
+  MemPages = sizeof (PEI_BOT_DEVICE) / EFI_PAGE_SIZE + 1;
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             MemPages,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiBotDevice                  = (PEI_BOT_DEVICE *) ((UINTN) AllocateAddress);
+
+  PeiBotDevice->Signature       = PEI_BOT_DEVICE_SIGNATURE;
+  PeiBotDevice->UsbIoPpi        = UsbIoPpi;
+  PeiBotDevice->AllocateAddress = (UINTN) AllocateAddress;
+  PeiBotDevice->BotInterface    = InterfaceDesc;
+
+  //
+  // Default value
+  //
+  PeiBotDevice->Media.DeviceType  = UsbMassStorage;
+  PeiBotDevice->Media.BlockSize   = 0x200;
+  PeiBotDevice->Media2.InterfaceType = MSG_USB_DP;
+  PeiBotDevice->Media2.BlockSize     = 0x200;
+  PeiBotDevice->Media2.RemovableMedia = FALSE;
+  PeiBotDevice->Media2.ReadOnly       = FALSE;
+
+  //
+  // Check its Bulk-in/Bulk-out endpoint
+  //
+  for (Index = 0; Index < 2; Index++) {
+    Status = UsbIoPpi->UsbGetEndpointDescriptor (
+                        PeiServices,
+                        UsbIoPpi,
+                        Index,
+                        &EndpointDesc
+                        );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if ((EndpointDesc->EndpointAddress & 0x80) != 0) {
+      PeiBotDevice->BulkInEndpoint = EndpointDesc;
+    } else {
+      PeiBotDevice->BulkOutEndpoint = EndpointDesc;
+    }
+  }
+
+  CopyMem (
+    &(PeiBotDevice->BlkIoPpi),
+    &mRecoveryBlkIoPpi,
+    sizeof (EFI_PEI_RECOVERY_BLOCK_IO_PPI)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIo2Ppi),
+    &mRecoveryBlkIo2Ppi,
+    sizeof (EFI_PEI_RECOVERY_BLOCK_IO2_PPI)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIoPpiList),
+    &mPpiList[0],
+    sizeof (EFI_PEI_PPI_DESCRIPTOR)
+    );
+  CopyMem (
+    &(PeiBotDevice->BlkIo2PpiList),
+    &mPpiList[1],
+    sizeof (EFI_PEI_PPI_DESCRIPTOR)
+    );
+  PeiBotDevice->BlkIoPpiList.Ppi  = &PeiBotDevice->BlkIoPpi;
+  PeiBotDevice->BlkIo2PpiList.Ppi = &PeiBotDevice->BlkIo2Ppi;
+
+  Status                          = PeiUsbInquiry (PeiServices, PeiBotDevice);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiBotDevice->SensePtr = (ATAPI_REQUEST_SENSE_DATA *) ((UINTN) AllocateAddress);
+
+  Status = PeiServicesInstallPpi (&PeiBotDevice->BlkIoPpiList);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI            *This,
+  OUT UINTN                                    *NumberBlockDevices
+  )
+{
+  //
+  // For Usb devices, this value should be always 1
+  //
+  *NumberBlockDevices = 1;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+  
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA                    *MediaInfo
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  PeiBotDev = PEI_BOT_DEVICE_FROM_THIS (This);
+
+  //
+  // First test unit ready
+  //
+  PeiUsbTestUnitReady (
+    PeiServices,
+    PeiBotDev
+    );
+
+  Status = PeiBotDetectMedia (
+            PeiServices,
+            PeiBotDev
+            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  CopyMem (
+    MediaInfo,
+    &(PeiBotDev->Media),
+    sizeof (EFI_PEI_BLOCK_IO_MEDIA)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+                         
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+  UINTN           BlockSize;
+  UINTN           NumberOfBlocks;
+
+  Status    = EFI_SUCCESS;
+  PeiBotDev = PEI_BOT_DEVICE_FROM_THIS (This);
+
+  //
+  // Check parameters
+  //
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (!PeiBotDev->Media.MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  BlockSize = PeiBotDev->Media.BlockSize;
+
+  if (BufferSize % BlockSize != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (StartLBA > PeiBotDev->Media2.LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  NumberOfBlocks = BufferSize / (PeiBotDev->Media.BlockSize);
+
+  if (Status == EFI_SUCCESS) {
+
+    Status = PeiUsbTestUnitReady (
+              PeiServices,
+              PeiBotDev
+              );
+    if (Status == EFI_SUCCESS) {
+      Status = PeiUsbRead10 (
+                PeiServices,
+                PeiBotDev,
+                Buffer,
+                StartLBA,
+                1
+                );
+    }
+  } else {
+    //
+    // To generate sense data for DetectMedia use.
+    //
+    PeiUsbTestUnitReady (
+      PeiServices,
+      PeiBotDev
+      );
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // if any error encountered, detect what happened to the media and
+    // update the media info accordingly.
+    //
+    Status = PeiBotDetectMedia (
+              PeiServices,
+              PeiBotDev
+              );
+    if (Status != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    NumberOfBlocks = BufferSize / PeiBotDev->Media.BlockSize;
+
+    if (!(PeiBotDev->Media.MediaPresent)) {
+      return EFI_NO_MEDIA;
+    }
+
+    if (BufferSize % (PeiBotDev->Media.BlockSize) != 0) {
+      return EFI_BAD_BUFFER_SIZE;
+    }
+
+    if (StartLBA > PeiBotDev->Media2.LastBlock) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((StartLBA + NumberOfBlocks - 1) > PeiBotDev->Media2.LastBlock) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Status = PeiUsbRead10 (
+              PeiServices,
+              PeiBotDev,
+              Buffer,
+              StartLBA,
+              NumberOfBlocks
+              );
+
+    switch (Status) {
+
+    case EFI_SUCCESS:
+      return EFI_SUCCESS;
+
+    default:
+      return EFI_DEVICE_ERROR;
+    }
+  } else {
+    StartLBA += 1;
+    NumberOfBlocks -= 1;
+    Buffer = (UINT8 *) Buffer + PeiBotDev->Media.BlockSize;
+
+    if (NumberOfBlocks == 0) {
+      return EFI_SUCCESS;
+    }
+
+    Status = PeiUsbRead10 (
+              PeiServices,
+              PeiBotDev,
+              Buffer,
+              StartLBA,
+              NumberOfBlocks
+              );
+    switch (Status) {
+
+    case EFI_SUCCESS:
+      return EFI_SUCCESS;
+
+    default:
+      return EFI_DEVICE_ERROR;
+
+    }
+  }
+}
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices2 (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI           *This,
+  OUT UINTN                                    *NumberBlockDevices
+  )
+{
+  //
+  // For Usb devices, this value should be always 1
+  //
+  *NumberBlockDevices = 1;
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA                   *MediaInfo
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  PeiBotDev = PEI_BOT_DEVICE2_FROM_THIS (This);
+
+  Status = BotGetMediaInfo (
+             PeiServices,
+             &PeiBotDev->BlkIoPpi,
+             DeviceIndex,
+             &PeiBotDev->Media
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (
+    MediaInfo,
+    &(PeiBotDev->Media2),
+    sizeof (EFI_PEI_BLOCK_IO2_MEDIA)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  )
+{
+  PEI_BOT_DEVICE  *PeiBotDev;
+  EFI_STATUS      Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status    = EFI_SUCCESS;
+  PeiBotDev = PEI_BOT_DEVICE2_FROM_THIS (This);
+
+  Status = BotReadBlocks (
+             PeiServices,
+             &PeiBotDev->BlkIoPpi,
+             DeviceIndex,
+             StartLBA,
+             BufferSize,
+             Buffer
+             );
+
+  return Status;
+}
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  PeiBotDev     Indicates the PEI_BOT_DEVICE instance.
+
+  @retval EFI_SUCCESS       The media status is successfully checked.
+  @retval Other             Failed to detect media.
+
+**/
+EFI_STATUS
+PeiBotDetectMedia (
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  PEI_BOT_DEVICE          *PeiBotDev
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_STATUS                  FloppyStatus;
+  UINTN                       SenseCounts;
+  BOOLEAN                     NeedReadCapacity;
+  EFI_PHYSICAL_ADDRESS        AllocateAddress;
+  ATAPI_REQUEST_SENSE_DATA    *SensePtr;
+  UINTN                       Retry;
+
+  //
+  // if there is no media present,or media not changed,
+  // the request sense command will detect faster than read capacity command.
+  // read capacity command can be bypassed, thus improve performance.
+  //
+  SenseCounts       = 0;
+  NeedReadCapacity  = TRUE;
+
+  Status = PeiServicesAllocatePages (
+             EfiBootServicesCode,
+             1,
+             &AllocateAddress
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SensePtr = PeiBotDev->SensePtr;
+  ZeroMem (SensePtr, EFI_PAGE_SIZE);
+
+  Status = PeiUsbRequestSense (
+            PeiServices,
+            PeiBotDev,
+            &SenseCounts,
+            (UINT8 *) SensePtr
+            );
+
+  if (Status == EFI_SUCCESS) {
+    //
+    // No Media
+    //
+    if (IsNoMedia (SensePtr, SenseCounts)) {
+      NeedReadCapacity              = FALSE;
+      PeiBotDev->Media.MediaPresent = FALSE;
+      PeiBotDev->Media.LastBlock    = 0;
+      PeiBotDev->Media2.MediaPresent = FALSE;
+      PeiBotDev->Media2.LastBlock    = 0;
+    } else {
+      //
+      // Media Changed
+      //
+      if (IsMediaChange (SensePtr, SenseCounts)) {
+        PeiBotDev->Media.MediaPresent  = TRUE;
+        PeiBotDev->Media2.MediaPresent = TRUE;
+      }
+      //
+      // Media Error
+      //
+      if (IsMediaError (SensePtr, SenseCounts)) {
+        //
+        // if media error encountered, make it look like no media present.
+        //
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+      }
+
+    }
+
+  }
+
+  if (NeedReadCapacity) {
+    //
+    // Retry at most 4 times to detect media info
+    //
+    for (Retry = 0; Retry < 4; Retry++) {
+      switch (PeiBotDev->DeviceType) {
+      case USBCDROM:
+        Status = PeiUsbReadCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+        break;
+
+      case USBFLOPPY2:
+        Status = PeiUsbReadFormattedCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+          if (EFI_ERROR(Status)||
+              !PeiBotDev->Media.MediaPresent) {
+          //
+          // retry the ReadCapacity command
+          //
+          PeiBotDev->DeviceType = USBFLOPPY;
+          Status = EFI_DEVICE_ERROR;
+        }
+        break;
+
+      case USBFLOPPY:
+        Status = PeiUsbReadCapacity (
+                  PeiServices,
+                  PeiBotDev
+                  );
+        if (EFI_ERROR (Status)) {
+          //
+          // retry the ReadFormatCapacity command
+          //
+          PeiBotDev->DeviceType = USBFLOPPY2;
+        }
+        break;
+
+      default:
+        return EFI_INVALID_PARAMETER;
+      }
+
+      SenseCounts = 0;
+      ZeroMem (SensePtr, EFI_PAGE_SIZE);
+
+      if (Status == EFI_SUCCESS) {
+        break;
+      }
+
+      FloppyStatus = PeiUsbRequestSense (
+                      PeiServices,
+                      PeiBotDev,
+                      &SenseCounts,
+                      (UINT8 *) SensePtr
+                      );
+
+      //
+      // If Request Sense data failed,retry.
+      //
+      if (EFI_ERROR (FloppyStatus)) {
+        continue;
+      }
+      //
+      // No Media
+      //
+      if (IsNoMedia (SensePtr, SenseCounts)) {
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+        break;
+      }
+
+      if (IsMediaError (SensePtr, SenseCounts)) {
+        //
+        // if media error encountered, make it look like no media present.
+        //
+        PeiBotDev->Media.MediaPresent = FALSE;
+        PeiBotDev->Media.LastBlock    = 0;
+        PeiBotDev->Media2.MediaPresent = FALSE;
+        PeiBotDev->Media2.LastBlock    = 0;
+        break;
+      }
+    }
+    //
+    // ENDFOR
+    //
+    // tell whether the readcapacity process is successful or not
+    // ("Status" variable record the latest status returned
+    // by ReadCapacity )
+    //
+    if (Status != EFI_SUCCESS) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h
new file mode 100644
index 0000000000..3ae8c67b35
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPeim.h
@@ -0,0 +1,346 @@
+/** @file
+Usb BOT Peim definition.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_USB_BOT_PEIM_H_
+#define _PEI_USB_BOT_PEIM_H_
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/UsbHostController.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+#include <IndustryStandard/Atapi.h>
+
+#define PEI_FAT_MAX_USB_IO_PPI  127
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one 
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process. 
+  To the PEI legacy floppy driver, it returns the number of all the legacy 
+  devices it finds during its enumeration process. If no device is detected, 
+  then the function will return zero.  
+  
+  @param[in]  PeiServices          General-purpose services that are available 
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI 
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI            *This,
+  OUT UINTN                                    *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media 
+  information. If the media changes, calling this function will update the media 
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the 
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to 
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.  
+                            The caller is responsible for the ownership of this 
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device 
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware 
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO_MEDIA                    *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the 
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to 
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants 
+                            to talk. Because the driver that implements Block I/O 
+                            PPIs will manage multiple block devices, the PPIs that 
+                            want to talk to a single device must specify the device 
+                            index that was assigned during the enumeration process. 
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the 
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting 
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not 
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO_PPI             *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  );
+
+/**
+  Gets the count of block I/O devices that one specific block driver detects.
+
+  This function is used for getting the count of block I/O devices that one
+  specific block driver detects.  To the PEI ATAPI driver, it returns the number
+  of all the detected ATAPI devices it detects during the enumeration process.
+  To the PEI legacy floppy driver, it returns the number of all the legacy
+  devices it finds during its enumeration process. If no device is detected,
+  then the function will return zero.
+
+  @param[in]  PeiServices          General-purpose services that are available
+                                   to every PEIM.
+  @param[in]  This                 Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI
+                                   instance.
+  @param[out] NumberBlockDevices   The number of block I/O devices discovered.
+
+  @retval     EFI_SUCCESS          Operation performed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetNumberOfBlockDevices2 (
+  IN  EFI_PEI_SERVICES                         **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI           *This,
+  OUT UINTN                                    *NumberBlockDevices
+  );
+
+/**
+  Gets a block device's media information.
+
+  This function will provide the caller with the specified block device's media
+  information. If the media changes, calling this function will update the media
+  information accordingly.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the
+                            device index that was assigned during the enumeration
+                            process. This index is a number from one to
+                            NumberBlockDevices.
+  @param[out] MediaInfo     The media information of the specified block media.
+                            The caller is responsible for the ownership of this
+                            data structure.
+
+  @retval EFI_SUCCESS        Media information about the specified block device
+                             was obtained successfully.
+  @retval EFI_DEVICE_ERROR   Cannot get the media information due to a hardware
+                             error.
+
+**/
+EFI_STATUS
+EFIAPI
+BotGetMediaInfo2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  OUT EFI_PEI_BLOCK_IO2_MEDIA                   *MediaInfo
+  );
+
+/**
+  Reads the requested number of blocks from the specified block device.
+
+  The function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned. If there is no media in the device,
+  the function returns EFI_NO_MEDIA.
+
+  @param[in]  PeiServices   General-purpose services that are available to
+                            every PEIM.
+  @param[in]  This          Indicates the EFI_PEI_RECOVERY_BLOCK_IO2_PPI instance.
+  @param[in]  DeviceIndex   Specifies the block device to which the function wants
+                            to talk. Because the driver that implements Block I/O
+                            PPIs will manage multiple block devices, the PPIs that
+                            want to talk to a single device must specify the device
+                            index that was assigned during the enumeration process.
+                            This index is a number from one to NumberBlockDevices.
+  @param[in]  StartLBA      The starting logical block address (LBA) to read from
+                            on the device
+  @param[in]  BufferSize    The size of the Buffer in bytes. This number must be
+                            a multiple of the intrinsic block size of the device.
+  @param[out] Buffer        A pointer to the destination buffer for the data.
+                            The caller is responsible for the ownership of the
+                            buffer.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting
+                                  to perform the read operation.
+  @retval EFI_INVALID_PARAMETER   The read request contains LBAs that are not
+                                  valid, or the buffer is not properly aligned.
+  @retval EFI_NO_MEDIA            There is no media in the device.
+  @retval EFI_BAD_BUFFER_SIZE     The BufferSize parameter is not a multiple of
+                                  the intrinsic block size of the device.
+
+**/
+EFI_STATUS
+EFIAPI
+BotReadBlocks2 (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  EFI_PEI_RECOVERY_BLOCK_IO2_PPI            *This,
+  IN  UINTN                                     DeviceIndex,
+  IN  EFI_PEI_LBA                               StartLBA,
+  IN  UINTN                                     BufferSize,
+  OUT VOID                                      *Buffer
+  );
+
+/**
+  UsbIo installation notification function.
+
+  This function finds out all the current USB IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices      Indirect reference to the PEI Services Table.
+  @param  NotifyDesc       Address of the notification descriptor data structure.
+  @param  InvokePpi        Address of the PPI that was invoked.
+
+  @retval EFI_SUCCESS      The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NotifyOnUsbIoPpi (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_NOTIFY_DESCRIPTOR                     *NotifyDesc,
+  IN  VOID                                          *InvokePpi
+  );
+
+/**
+  Initialize the usb bot device.
+
+  @param[in]  PeiServices   General-purpose services that are available to every
+                            PEIM.
+  @param[in]  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       The usb bot device is initialized successfully.
+  @retval Other             Failed to initialize media.
+
+**/
+EFI_STATUS
+InitUsbBot (
+  IN  EFI_PEI_SERVICES                          **PeiServices,
+  IN  PEI_USB_IO_PPI                            *UsbIoPpi
+  );
+
+#define USBCDROM    1 // let the device type value equal to USBCDROM, which is defined by PI spec.
+                      // Therefore the CdExpressPei module can do recovery on UsbCdrom.
+#define USBFLOPPY   2 // for those that use ReadCapacity(0x25) command to retrieve media capacity
+#define USBFLOPPY2  3 // for those that use ReadFormatCapacity(0x23) command to retrieve media capacity
+
+//
+// Bot device structure
+//
+#define PEI_BOT_DEVICE_SIGNATURE  SIGNATURE_32 ('U', 'B', 'O', 'T')
+typedef struct {
+  UINTN                           Signature;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   BlkIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  BlkIo2Ppi;
+  EFI_PEI_PPI_DESCRIPTOR          BlkIoPpiList;
+  EFI_PEI_PPI_DESCRIPTOR          BlkIo2PpiList;
+  EFI_PEI_BLOCK_IO_MEDIA          Media;
+  EFI_PEI_BLOCK_IO2_MEDIA         Media2;
+  PEI_USB_IO_PPI                  *UsbIoPpi;
+  EFI_USB_INTERFACE_DESCRIPTOR    *BotInterface;
+  EFI_USB_ENDPOINT_DESCRIPTOR     *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR     *BulkOutEndpoint;
+  UINTN                           AllocateAddress;
+  UINTN                           DeviceType;
+  ATAPI_REQUEST_SENSE_DATA        *SensePtr;
+} PEI_BOT_DEVICE;
+
+#define PEI_BOT_DEVICE_FROM_THIS(a) CR (a, PEI_BOT_DEVICE, BlkIoPpi, PEI_BOT_DEVICE_SIGNATURE)
+#define PEI_BOT_DEVICE2_FROM_THIS(a) CR (a, PEI_BOT_DEVICE, BlkIo2Ppi, PEI_BOT_DEVICE_SIGNATURE)
+
+/**
+  Send ATAPI command using BOT protocol.
+
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  PeiBotDev              The instance to PEI_BOT_DEVICE.
+  @param  Command                The command to be sent to ATAPI device.
+  @param  CommandSize            The length of the data to be sent.
+  @param  DataBuffer             The pointer to the data.
+  @param  BufferLength           The length of the data.
+  @param  Direction              The direction of the data.
+  @param  TimeOutInMilliSeconds  Indicates the maximum time, in millisecond, which the
+                                 transfer is allowed to complete.
+
+  @retval EFI_DEVICE_ERROR       Successful to get the status of device.
+  @retval EFI_SUCCESS            Failed to get the status of device.
+
+**/
+EFI_STATUS
+PeiAtapiCommand (
+  IN  EFI_PEI_SERVICES            **PeiServices,
+  IN  PEI_BOT_DEVICE              *PeiBotDev,
+  IN  VOID                        *Command,
+  IN  UINT8                       CommandSize,
+  IN  VOID                        *DataBuffer,
+  IN  UINT32                      BufferLength,
+  IN  EFI_USB_DATA_DIRECTION      Direction,
+  IN  UINT16                      TimeOutInMilliSeconds
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h
new file mode 100644
index 0000000000..c62b99d4a1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBotPei/UsbPeim.h
@@ -0,0 +1,33 @@
+/** @file
+Usb Peim definition.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_USB_PEIM_H_
+#define _PEI_USB_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbIo.h>
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c
new file mode 100644
index 0000000000..dc00208632
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/ComponentName.c
@@ -0,0 +1,309 @@
+/** @file
+
+  UEFI Component Name(2) protocol implementation for Usb Bus driver.
+
+Copyright (c) 2004 - 2011, 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 <Uefi.h>
+
+
+#include <Library/UefiLib.h>
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  mUsbBusComponentName = {
+  UsbBusComponentNameGetDriverName,
+  UsbBusComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL mUsbBusComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbBusComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbBusComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbBusDriverNameTable[] = {
+  { "eng;en", L"Usb Bus Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbBusDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &mUsbBusComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c
new file mode 100644
index 0000000000..ef002f53bb
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.c
@@ -0,0 +1,1532 @@
+/** @file
+
+    Usb Bus Driver Binding and Bus IO Protocol.
+
+Copyright (c) 2004 - 2013, 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 "UsbBus.h"
+
+EFI_USB_IO_PROTOCOL mUsbIoProtocol = {
+  UsbIoControlTransfer,
+  UsbIoBulkTransfer,
+  UsbIoAsyncInterruptTransfer,
+  UsbIoSyncInterruptTransfer,
+  UsbIoIsochronousTransfer,
+  UsbIoAsyncIsochronousTransfer,
+  UsbIoGetDeviceDescriptor,
+  UsbIoGetActiveConfigDescriptor,
+  UsbIoGetInterfaceDescriptor,
+  UsbIoGetEndpointDescriptor,
+  UsbIoGetStringDescriptor,
+  UsbIoGetSupportedLanguages,
+  UsbIoPortReset
+};
+
+EFI_DRIVER_BINDING_PROTOCOL mUsbBusDriverBinding = {
+  UsbBusControllerDriverSupported,
+  UsbBusControllerDriverStart,
+  UsbBusControllerDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  USB_IO function to execute a control transfer. This
+  function will execute the USB transfer. If transfer
+  successes, it will sync the internal state of USB bus
+  with device state.
+
+  @param  This                   The USB_IO instance
+  @param  Request                The control transfer request
+  @param  Direction              Direction for data stage
+  @param  Timeout                The time to wait before timeout
+  @param  Data                   The buffer holding the data
+  @param  DataLength             Then length of the data
+  @param  UsbStatus              USB result
+
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid
+  @retval EFI_SUCCESS            The control transfer succeeded.
+  @retval Others                 Failed to execute the transfer
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoControlTransfer (
+  IN  EFI_USB_IO_PROTOCOL     *This,
+  IN  EFI_USB_DEVICE_REQUEST  *Request,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  UINT32                  Timeout,
+  IN  OUT VOID                *Data,      OPTIONAL
+  IN  UINTN                   DataLength, OPTIONAL
+  OUT UINT32                  *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  if (UsbStatus == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  Status = UsbHcControlTransfer (
+             Dev->Bus,
+             Dev->Address,
+             Dev->Speed,
+             Dev->MaxPacket0,
+             Request,
+             Direction,
+             Data,
+             &DataLength,
+             (UINTN) Timeout,
+             &Dev->Translator,
+             UsbStatus
+             );
+
+  if (EFI_ERROR (Status) || (*UsbStatus != EFI_USB_NOERROR)) {
+    //
+    // Clear TT buffer when CTRL/BULK split transaction failes
+    // Clear the TRANSLATOR TT buffer, not parent's buffer
+    //
+    ASSERT (Dev->Translator.TranslatorHubAddress < Dev->Bus->MaxDevices);
+    if (Dev->Translator.TranslatorHubAddress != 0) {
+      UsbHubCtrlClearTTBuffer (
+        Dev->Bus->Devices[Dev->Translator.TranslatorHubAddress],
+        Dev->Translator.TranslatorPortNumber,
+        Dev->Address,
+        0,
+        USB_ENDPOINT_CONTROL
+        );
+    }
+
+    goto ON_EXIT;
+  }
+
+  //
+  // Some control transfer will change the device's internal
+  // status, such as Set_Configuration and Set_Interface.
+  // We must synchronize the bus driver's status with that in
+  // device. We ignore the Set_Descriptor request because it's
+  // hardly used by any device, especially in pre-boot environment
+  //
+
+  //
+  // Reset the endpoint toggle when endpoint stall is cleared
+  //
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_ENDPOINT)) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+
+    EpDesc = UsbGetEndpointDesc (UsbIf, (UINT8) Request->Index);
+
+    if (EpDesc != NULL) {
+      EpDesc->Toggle = 0;
+    }
+  }
+
+  //
+  // Select a new configuration. This is a dangerous action. Upper driver
+  // should stop use its current UsbIo after calling this driver. The old
+  // UsbIo will be uninstalled and new UsbIo be installed. We can't use
+  // ReinstallProtocol since interfaces in different configuration may be
+  // completely irrelevant.
+  //
+  if ((Request->Request == USB_REQ_SET_CONFIG) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_DEVICE))) {
+    //
+    // Don't re-create the USB interfaces if configuration isn't changed.
+    //
+    if ((Dev->ActiveConfig != NULL) &&
+        (Request->Value == Dev->ActiveConfig->Desc.ConfigurationValue)) {
+
+      goto ON_EXIT;
+    }
+    DEBUG ((EFI_D_INFO, "UsbIoControlTransfer: configure changed!!! Do NOT use old UsbIo!!!\n"));
+
+    if (Dev->ActiveConfig != NULL) {
+      UsbRemoveConfig (Dev);
+    }
+
+    if (Request->Value != 0) {
+      Status = UsbSelectConfig (Dev, (UINT8) Request->Value);
+    }
+
+    //
+    // Exit now, Old USB_IO is invalid now
+    //
+    goto ON_EXIT;
+  }
+
+  //
+  // A new alternative setting is selected for the interface.
+  // No need to reinstall UsbIo in this case because only
+  // underlying communication endpoints are changed. Functionality
+  // should remains the same.
+  //
+  if ((Request->Request     == USB_REQ_SET_INTERFACE) &&
+      (Request->RequestType == USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD,
+                                                 USB_TARGET_INTERFACE)) &&
+      (Request->Index       == UsbIf->IfSetting->Desc.InterfaceNumber)) {
+
+    Status = UsbSelectSetting (UsbIf->IfDesc, (UINT8) Request->Value);
+
+    if (!EFI_ERROR (Status)) {
+      ASSERT (UsbIf->IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+      UsbIf->IfSetting = UsbIf->IfDesc->Settings[UsbIf->IfDesc->ActiveIndex];
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a bulk transfer to the device endpoint.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The bulk transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoBulkTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  UINT8                   BufNum;
+  UINT8                   Toggle;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR(Endpoint) > 15) ||
+      (UsbStatus == NULL)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_BULK)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  BufNum  = 1;
+  Toggle  = EpDesc->Toggle;
+  Status  = UsbHcBulkTransfer (
+              Dev->Bus,
+              Dev->Address,
+              Endpoint,
+              Dev->Speed,
+              EpDesc->Desc.MaxPacketSize,
+              BufNum,
+              &Data,
+              DataLength,
+              &Toggle,
+              Timeout,
+              &Dev->Translator,
+              UsbStatus
+              );
+
+  EpDesc->Toggle = Toggle;
+
+  if (EFI_ERROR (Status) || (*UsbStatus != EFI_USB_NOERROR)) {
+    //
+    // Clear TT buffer when CTRL/BULK split transaction failes.
+    // Clear the TRANSLATOR TT buffer, not parent's buffer
+    //
+    ASSERT (Dev->Translator.TranslatorHubAddress < Dev->Bus->MaxDevices);
+    if (Dev->Translator.TranslatorHubAddress != 0) {
+      UsbHubCtrlClearTTBuffer (
+        Dev->Bus->Devices[Dev->Translator.TranslatorHubAddress],
+        Dev->Translator.TranslatorPortNumber,
+        Dev->Address,
+        0,
+        USB_ENDPOINT_BULK
+        );
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a synchronous interrupt transfer.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The synchronous interrupt transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoSyncInterruptTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  UINT8                   Toggle;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR(Endpoint) > 15) ||
+      (UsbStatus == NULL)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_INTERRUPT)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Toggle = EpDesc->Toggle;
+  Status = UsbHcSyncInterruptTransfer (
+             Dev->Bus,
+             Dev->Address,
+             Endpoint,
+             Dev->Speed,
+             EpDesc->Desc.MaxPacketSize,
+             Data,
+             DataLength,
+             &Toggle,
+             Timeout,
+             &Dev->Translator,
+             UsbStatus
+             );
+
+  EpDesc->Toggle = Toggle;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Queue a new asynchronous interrupt transfer, or remove the old
+  request if (IsNewTransfer == FALSE).
+
+  @param  This                   The USB_IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  IsNewTransfer          Whether this is a new request, if it's old, remove
+                                 the request.
+  @param  PollInterval           The interval to poll the transfer result, (in ms).
+  @param  DataLength             The length of perodic data transfer.
+  @param  Callback               The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_SUCCESS            New transfer is queued or old request is removed.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to queue the new request or remove the old
+                                 request.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncInterruptTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            Endpoint,
+  IN BOOLEAN                          IsNewTransfer,
+  IN UINTN                            PollInterval,       OPTIONAL
+  IN UINTN                            DataLength,         OPTIONAL
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,           OPTIONAL
+  IN VOID                             *Context            OPTIONAL
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  USB_ENDPOINT_DESC       *EpDesc;
+  EFI_TPL                 OldTpl;
+  UINT8                   Toggle;
+  EFI_STATUS              Status;
+
+  if ((USB_ENDPOINT_ADDR (Endpoint) == 0) || (USB_ENDPOINT_ADDR (Endpoint) > 15)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (USB_BUS_TPL);
+  UsbIf   = USB_INTERFACE_FROM_USBIO (This);
+  Dev     = UsbIf->Device;
+
+  EpDesc  = UsbGetEndpointDesc (UsbIf, Endpoint);
+
+  if ((EpDesc == NULL) || (USB_ENDPOINT_TYPE (&EpDesc->Desc) != USB_ENDPOINT_INTERRUPT)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Toggle  = EpDesc->Toggle;
+  Status  = UsbHcAsyncInterruptTransfer (
+              Dev->Bus,
+              Dev->Address,
+              Endpoint,
+              Dev->Speed,
+              EpDesc->Desc.MaxPacketSize,
+              IsNewTransfer,
+              &Toggle,
+              PollInterval,
+              DataLength,
+              &Dev->Translator,
+              Callback,
+              Context
+              );
+
+  EpDesc->Toggle = Toggle;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Execute a synchronous isochronous transfer.
+
+  @param  This                   The USB IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoIsochronousTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               DeviceEndpoint,
+  IN  OUT VOID            *Data,
+  IN  UINTN               DataLength,
+  OUT UINT32              *Status
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  This                   The USB_IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of perodic data transfer.
+  @param  IsochronousCallBack    The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncIsochronousTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            DeviceEndpoint,
+  IN OUT VOID                         *Data,
+  IN UINTN                            DataLength,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  IsochronousCallBack,
+  IN VOID                             *Context              OPTIONAL
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Retrieve the device descriptor of the device.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive the device descriptor.
+
+  @retval EFI_SUCCESS            The device descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetDeviceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_DEVICE_DESCRIPTOR *Descriptor
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  CopyMem (Descriptor, &Dev->DevDesc->Desc, sizeof (EFI_USB_DEVICE_DESCRIPTOR));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Return the configuration descriptor of the current active configuration.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The USB configuration descriptor.
+
+  @retval EFI_SUCCESS            The active configuration descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+  @retval EFI_NOT_FOUND          Currently no active configuration is selected.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetActiveConfigDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_CONFIG_DESCRIPTOR *Descriptor
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_SUCCESS;
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  if (Dev->ActiveConfig == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto ON_EXIT;
+  }
+
+  CopyMem (Descriptor, &(Dev->ActiveConfig->Desc), sizeof (EFI_USB_CONFIG_DESCRIPTOR));
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Retrieve the active interface setting descriptor for this USB IO instance.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive active interface setting.
+
+  @retval EFI_SUCCESS            The active interface setting is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetInterfaceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL           *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR  *Descriptor
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  CopyMem (Descriptor, &(UsbIf->IfSetting->Desc), sizeof (EFI_USB_INTERFACE_DESCRIPTOR));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve the endpoint descriptor from this interface setting.
+
+  @param  This                   The USB IO instance.
+  @param  Index                  The index (start from zero) of the endpoint to
+                                 retrieve.
+  @param  Descriptor             The variable to receive the descriptor.
+
+  @retval EFI_SUCCESS            The endpoint descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetEndpointDescriptor (
+  IN  EFI_USB_IO_PROTOCOL         *This,
+  IN  UINT8                       Index,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR *Descriptor
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+
+  if ((Descriptor == NULL) || (Index > 15)) {
+    gBS->RestoreTPL (OldTpl);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Index >= UsbIf->IfSetting->Desc.NumEndpoints) {
+    gBS->RestoreTPL (OldTpl);
+    return EFI_NOT_FOUND;
+  }
+
+  CopyMem (
+    Descriptor,
+    &(UsbIf->IfSetting->Endpoints[Index]->Desc),
+    sizeof (EFI_USB_ENDPOINT_DESCRIPTOR)
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve the supported language ID table from the device.
+
+  @param  This                   The USB IO instance.
+  @param  LangIDTable            The table to return the language IDs.
+  @param  TableSize              The size, in bytes, of the table LangIDTable.
+
+  @retval EFI_SUCCESS            The language ID is return.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetSupportedLanguages (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  OUT UINT16              **LangIDTable,
+  OUT UINT16              *TableSize
+  )
+{
+  USB_DEVICE              *Dev;
+  USB_INTERFACE           *UsbIf;
+  EFI_TPL                 OldTpl;
+
+  OldTpl        = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf         = USB_INTERFACE_FROM_USBIO (This);
+  Dev           = UsbIf->Device;
+
+  *LangIDTable  = Dev->LangId;
+  *TableSize    = (UINT16) (Dev->TotalLangId * sizeof (UINT16));
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve an indexed string in the language of LangID.
+
+  @param  This                   The USB IO instance.
+  @param  LangID                 The language ID of the string to retrieve.
+  @param  StringIndex            The index of the string.
+  @param  String                 The variable to receive the string.
+
+  @retval EFI_SUCCESS            The string is returned.
+  @retval EFI_NOT_FOUND          No such string existed.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetStringDescriptor (
+  IN  EFI_USB_IO_PROTOCOL   *This,
+  IN  UINT16                LangID,
+  IN  UINT8                 StringIndex,
+  OUT CHAR16                **String
+  )
+{
+  USB_DEVICE                *Dev;
+  USB_INTERFACE             *UsbIf;
+  EFI_USB_STRING_DESCRIPTOR *StrDesc;
+  EFI_TPL                   OldTpl;
+  UINT8                     *Buf;
+  UINT8                     Index;
+  EFI_STATUS                Status;
+
+  if ((StringIndex == 0) || (LangID == 0)) {
+    return EFI_NOT_FOUND;
+  }
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  //
+  // Check whether language ID is supported
+  //
+  Status = EFI_NOT_FOUND;
+
+  for (Index = 0; Index < Dev->TotalLangId; Index++) {
+    ASSERT (Index < USB_MAX_LANG_ID);
+    if (Dev->LangId[Index] == LangID) {
+      break;
+    }
+  }
+
+  if (Index == Dev->TotalLangId) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Retrieve the string descriptor then allocate a buffer
+  // to hold the string itself.
+  //
+  StrDesc = UsbGetOneString (Dev, StringIndex, LangID);
+
+  if (StrDesc == NULL) {
+    goto ON_EXIT;
+  }
+
+  if (StrDesc->Length <= 2) {
+    goto FREE_STR;
+  }
+
+  Buf = AllocateZeroPool (StrDesc->Length);
+
+  if (Buf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_STR;
+  }
+
+  CopyMem (Buf, StrDesc->String, StrDesc->Length - 2);
+  *String = (CHAR16 *) Buf;
+  Status  = EFI_SUCCESS;
+
+FREE_STR:
+  gBS->FreePool (StrDesc);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Reset the device, then if that succeeds, reconfigure the
+  device with its address and current active configuration.
+
+  @param  This                   The USB IO instance.
+
+  @retval EFI_SUCCESS            The device is reset and configured.
+  @retval Others                 Failed to reset the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoPortReset (
+  IN EFI_USB_IO_PROTOCOL  *This
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  USB_INTERFACE           *HubIf;
+  USB_DEVICE              *Dev;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+  UINT8                   DevAddress;
+
+  OldTpl = gBS->RaiseTPL (USB_BUS_TPL);
+
+  UsbIf  = USB_INTERFACE_FROM_USBIO (This);
+  Dev    = UsbIf->Device;
+
+  if (UsbIf->IsHub) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  HubIf  = Dev->ParentIf;
+  Status = HubIf->HubApi->ResetPort (HubIf, Dev->ParentPort);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to reset hub port %d@hub  %d, %r \n",
+                Dev->ParentPort, Dev->ParentAddr, Status));
+
+    goto ON_EXIT;
+  }
+
+  HubIf->HubApi->ClearPortChange (HubIf, Dev->ParentPort);
+
+  //
+  // Reset the device to its current address. The device now has an address
+  // of ZERO after port reset, so need to set Dev->Address to the device again for
+  // host to communicate with it.
+  //
+  DevAddress   = Dev->Address;
+  Dev->Address = 0;
+  Status  = UsbSetAddress (Dev, DevAddress);
+  Dev->Address = DevAddress;
+
+  gBS->Stall (USB_SET_DEVICE_ADDRESS_STALL);
+  
+  if (EFI_ERROR (Status)) {
+    //
+    // It may fail due to device disconnection or other reasons.
+    //
+    DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to set address for device %d - %r\n",
+                Dev->Address, Status));
+
+    goto ON_EXIT;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbIoPortReset: device is now ADDRESSED at %d\n", Dev->Address));
+
+  //
+  // Reset the current active configure, after this device
+  // is in CONFIGURED state.
+  //
+  if (Dev->ActiveConfig != NULL) {
+    Status = UsbSetConfig (Dev, Dev->ActiveConfig->Desc.ConfigurationValue);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbIoPortReset: failed to set configure for device %d - %r\n",
+                  Dev->Address, Status));
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Install Usb Bus Protocol on host controller, and start the Usb bus.
+
+  @param This                    The USB bus driver binding instance.
+  @param Controller              The controller to check.
+  @param RemainingDevicePath     The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusBuildProtocol (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_BUS                 *UsbBus;
+  USB_DEVICE              *RootHub;
+  USB_INTERFACE           *RootIf;
+  EFI_STATUS              Status;
+  EFI_STATUS              Status2;
+
+  UsbBus = AllocateZeroPool (sizeof (USB_BUS));
+
+  if (UsbBus == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UsbBus->Signature  = USB_BUS_SIGNATURE;
+  UsbBus->HostHandle = Controller;
+  UsbBus->MaxDevices = USB_MAX_DEVICES;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbBus->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to open device path %r\n", Status));
+
+    FreePool (UsbBus);
+    return Status;
+  }
+
+  //
+  // Get USB_HC2/USB_HC host controller protocol (EHCI/UHCI).
+  // This is for backward compatibility with EFI 1.x. In UEFI
+  // 2.x, USB_HC2 replaces USB_HC. We will open both USB_HC2
+  // and USB_HC because EHCI driver will install both protocols
+  // (for the same reason). If we don't consume both of them,
+  // the unconsumed one may be opened by others.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &(UsbBus->Usb2Hc),
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  Status2 = gBS->OpenProtocol (
+                   Controller,
+                   &gEfiUsbHcProtocolGuid,
+                   (VOID **) &(UsbBus->UsbHc),
+                   This->DriverBindingHandle,
+                   Controller,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+
+  if (EFI_ERROR (Status) && EFI_ERROR (Status2)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to open USB_HC/USB2_HC %r\n", Status));
+
+    Status = EFI_DEVICE_ERROR;
+    goto CLOSE_HC;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // The EFI_USB2_HC_PROTOCOL is produced for XHCI support.
+    // Then its max supported devices are 256. Otherwise it's 128.
+    //
+    ASSERT (UsbBus->Usb2Hc != NULL);
+    if (UsbBus->Usb2Hc->MajorRevision == 0x3) {
+      UsbBus->MaxDevices = 256;
+    }
+  }
+
+  UsbHcReset (UsbBus, EFI_USB_HC_RESET_GLOBAL);
+  UsbHcSetState (UsbBus, EfiUsbHcStateOperational);
+
+  //
+  // Install an EFI_USB_BUS_PROTOCOL to host controller to identify it.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiCallerIdGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbBus->BusId
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to install bus protocol %r\n", Status));
+    goto CLOSE_HC;
+  }
+
+  //
+  // Initial the wanted child device path list, and add first RemainingDevicePath
+  //
+  InitializeListHead (&UsbBus->WantedUsbIoDPList);
+  Status = UsbBusAddWantedUsbIoDP (&UsbBus->BusId, RemainingDevicePath);
+  ASSERT (!EFI_ERROR (Status));
+  //
+  // Create a fake usb device for root hub
+  //
+  RootHub = AllocateZeroPool (sizeof (USB_DEVICE));
+
+  if (RootHub == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto UNINSTALL_USBBUS;
+  }
+
+  RootIf = AllocateZeroPool (sizeof (USB_INTERFACE));
+
+  if (RootIf == NULL) {
+    FreePool (RootHub);
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FREE_ROOTHUB;
+  }
+
+  RootHub->Bus            = UsbBus;
+  RootHub->NumOfInterface = 1;
+  RootHub->Interfaces[0]  = RootIf;
+  RootHub->Tier           = 0;
+  RootIf->Signature       = USB_INTERFACE_SIGNATURE;
+  RootIf->Device          = RootHub;
+  RootIf->DevicePath      = UsbBus->DevicePath;
+  
+  //
+  // Report Status Code here since we will enumerate the USB devices
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_DETECT),
+    UsbBus->DevicePath
+    );
+  
+  Status                  = mUsbRootHubApi.Init (RootIf);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to init root hub %r\n", Status));
+    goto FREE_ROOTHUB;
+  }
+
+  UsbBus->Devices[0] = RootHub;
+
+  DEBUG ((EFI_D_INFO, "UsbBusStart: usb bus started on %p, root hub %p\n", Controller, RootIf));
+  return EFI_SUCCESS;
+
+FREE_ROOTHUB:
+  if (RootIf != NULL) {
+    FreePool (RootIf);
+  }
+  if (RootHub != NULL) {
+    FreePool (RootHub);
+  }
+
+UNINSTALL_USBBUS:
+  gBS->UninstallProtocolInterface (Controller, &gEfiCallerIdGuid, &UsbBus->BusId);
+
+CLOSE_HC:
+  if (UsbBus->Usb2Hc != NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsb2HcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+  if (UsbBus->UsbHc != NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbHcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  FreePool (UsbBus);
+
+  DEBUG ((EFI_D_ERROR, "UsbBusStart: Failed to start bus driver %r\n", Status));
+  return Status;
+}
+
+
+/**
+  The USB bus driver entry pointer.
+
+  @param ImageHandle       The driver image handle.
+  @param SystemTable       The system table.
+
+  @return EFI_SUCCESS      The component name protocol is installed.
+  @return Others           Failed to init the usb driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &mUsbBusDriverBinding,
+           ImageHandle,
+           &mUsbBusComponentName,
+           &mUsbBusComponentName2
+           );
+}
+
+
+/**
+  Check whether USB bus driver support this device.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The bus supports this controller.
+  @retval EFI_UNSUPPORTED        This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_DEV_PATH_PTR          DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_USB2_HC_PROTOCOL      *Usb2Hc;
+  EFI_USB_HC_PROTOCOL       *UsbHc;
+  EFI_STATUS                Status;
+
+  //
+  // Check whether device path is valid
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      DevicePathNode.DevPath = RemainingDevicePath;
+      
+      if ((DevicePathNode.DevPath->Type    != MESSAGING_DEVICE_PATH) ||
+          (DevicePathNode.DevPath->SubType != MSG_USB_DP &&
+           DevicePathNode.DevPath->SubType != MSG_USB_CLASS_DP
+           && DevicePathNode.DevPath->SubType != MSG_USB_WWID_DP
+           )) {
+      
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Check whether USB_HC2 protocol is installed
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsb2HcProtocolGuid,
+                  (VOID **) &Usb2Hc,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If failed to open USB_HC2, fall back to USB_HC
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbHcProtocolGuid,
+                    (VOID **) &UsbHc,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+    if (Status == EFI_ALREADY_STARTED) {
+      return EFI_SUCCESS;
+    }
+  
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Close the USB_HC used to perform the supported test
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbHcProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+  } else {
+
+    //
+    // Close the USB_HC2 used to perform the supported test
+    //
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsb2HcProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+ 
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Close protocol, don't use device path protocol in the Support() function
+    //
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    return EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+
+/**
+  Start to process the controller.
+
+  @param  This                   The USB bus driver binding instance.
+  @param  Controller             The controller to check.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb
+                                 bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_USB_BUS_PROTOCOL          *UsbBusId;
+  EFI_STATUS                    Status;
+  EFI_DEVICE_PATH_PROTOCOL      *ParentDevicePath;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report Status Code here since we will initialize the host controller
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_INIT),
+    ParentDevicePath
+    );
+
+  //
+  // Locate the USB bus protocol, if it is found, USB bus
+  // is already started on this controller.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &UsbBusId,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If first start, build the bus execute environment and install bus protocol
+    //
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_IO_BUS_USB | EFI_P_PC_ENABLE));
+    Status = UsbBusBuildProtocol (This, Controller, RemainingDevicePath);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Try get the Usb Bus protocol interface again
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &UsbBusId,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  } else {
+    //
+    // USB Bus driver need to control the recursive connect policy of the bus, only those wanted
+    // usb child device will be recursively connected.
+    // The RemainingDevicePath indicate the child usb device which user want to fully recursively connecte this time.
+    // All wanted usb child devices will be remembered by the usb bus driver itself.
+    // If RemainingDevicePath == NULL, all the usb child devices in the usb bus are wanted devices.
+    //
+    // Save the passed in RemainingDevicePath this time
+    //
+    if (RemainingDevicePath != NULL) {
+      if (IsDevicePathEnd (RemainingDevicePath)) {
+        //
+        // If RemainingDevicePath is the End of Device Path Node,
+        // skip enumerate any device and return EFI_SUCESSS
+        // 
+        return EFI_SUCCESS;
+      }
+    }
+
+    Status = UsbBusAddWantedUsbIoDP (UsbBusId, RemainingDevicePath);
+    ASSERT (!EFI_ERROR (Status));
+    //
+    // Ensure all wanted child usb devices are fully recursively connected
+    //
+    Status = UsbBusRecursivelyConnectWantedUsbIo (UsbBusId);
+    ASSERT (!EFI_ERROR (Status));
+  }
+
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Stop handle the controller by this USB bus driver.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The child of USB bus that opened controller
+                                 BY_CHILD.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The controller or children are stopped.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  USB_BUS               *Bus;
+  USB_DEVICE            *RootHub;
+  USB_DEVICE            *UsbDev;
+  USB_INTERFACE         *RootIf;
+  USB_INTERFACE         *UsbIf;
+  EFI_USB_BUS_PROTOCOL  *BusId;
+  EFI_USB_IO_PROTOCOL   *UsbIo;
+  EFI_TPL               OldTpl;
+  UINTN                 Index;
+  EFI_STATUS            Status;
+  EFI_STATUS            ReturnStatus;
+
+  Status  = EFI_SUCCESS;
+
+  if (NumberOfChildren > 0) {
+    //
+    // BugBug: Raise TPL to callback level instead of USB_BUS_TPL to avoid TPL conflict
+    //
+    OldTpl   = gBS->RaiseTPL (TPL_CALLBACK);
+
+    ReturnStatus = EFI_SUCCESS;
+    for (Index = 0; Index < NumberOfChildren; Index++) {
+      Status = gBS->OpenProtocol (
+                      ChildHandleBuffer[Index],
+                      &gEfiUsbIoProtocolGuid,
+                      (VOID **) &UsbIo,
+                      This->DriverBindingHandle,
+                      Controller,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+
+      if (EFI_ERROR (Status)) {
+        //
+        // It is possible that the child has already been released:
+        // 1. For combo device, free one device will release others.
+        // 2. If a hub is released, all devices on its down facing
+        //    ports are released also.
+        //
+        continue;
+      }
+
+      UsbIf   = USB_INTERFACE_FROM_USBIO (UsbIo);
+      UsbDev  = UsbIf->Device;
+
+      ReturnStatus = UsbRemoveDevice (UsbDev);
+    }
+
+    gBS->RestoreTPL (OldTpl);
+    return ReturnStatus;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbBusStop: usb bus stopped on %p\n", Controller));
+
+  //
+  // Locate USB_BUS for the current host controller
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &BusId,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Bus = USB_BUS_FROM_THIS (BusId);
+
+  //
+  // Stop the root hub, then free all the devices
+  //
+  // BugBug: Raise TPL to callback level instead of USB_BUS_TPL to avoid TPL conflict
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  RootHub = Bus->Devices[0];
+  RootIf  = RootHub->Interfaces[0];
+
+  ASSERT (Bus->MaxDevices <= 256);
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    if (Bus->Devices[Index] != NULL) {
+      Status = UsbRemoveDevice (Bus->Devices[Index]);
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      }
+    }
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  if (!EFI_ERROR (ReturnStatus)) {
+    mUsbRootHubApi.Release (RootIf);
+    gBS->FreePool   (RootIf);
+    gBS->FreePool   (RootHub);
+
+    Status = UsbBusFreeUsbDPList (&Bus->WantedUsbIoDPList);
+    ASSERT (!EFI_ERROR (Status));
+
+    //
+    // Uninstall the bus identifier and close USB_HC/USB2_HC protocols
+    //
+    gBS->UninstallProtocolInterface (Controller, &gEfiCallerIdGuid, &Bus->BusId);
+
+    if (Bus->Usb2Hc != NULL) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiUsb2HcProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );
+    }
+
+    if (Bus->UsbHc != NULL) {
+      Status = gBS->CloseProtocol (
+                      Controller,
+                      &gEfiUsbHcProtocolGuid,
+                      This->DriverBindingHandle,
+                      Controller
+                      );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+
+      gBS->FreePool (Bus);
+    }
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h
new file mode 100644
index 0000000000..9ede83ab7e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBus.h
@@ -0,0 +1,770 @@
+/** @file
+
+    Usb Bus Driver Binding and Bus IO Protocol.
+
+Copyright (c) 2004 - 2012, 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.
+
+**/
+
+#ifndef _EFI_USB_BUS_H_
+#define _EFI_USB_BUS_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Usb2HostController.h>
+#include <Protocol/UsbHostController.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+
+#include <IndustryStandard/Usb.h>
+
+typedef struct _USB_DEVICE     USB_DEVICE;
+typedef struct _USB_INTERFACE  USB_INTERFACE;
+typedef struct _USB_BUS        USB_BUS;
+typedef struct _USB_HUB_API    USB_HUB_API;
+
+
+#include "UsbUtility.h"
+#include "UsbDesc.h"
+#include "UsbHub.h"
+#include "UsbEnumer.h"
+
+//
+// USB bus timeout experience values
+//
+
+#define USB_MAX_LANG_ID           16
+#define USB_MAX_INTERFACE         16
+#define USB_MAX_DEVICES           128
+
+#define USB_BUS_1_MILLISECOND     1000
+
+//
+// Roothub and hub's polling interval, set by experience,
+// The unit of roothub is 100us, means 100ms as interval, and
+// the unit of hub is 1ms, means 64ms as interval.
+//
+#define USB_ROOTHUB_POLL_INTERVAL (100 * 10000U)
+#define USB_HUB_POLL_INTERVAL     64
+
+//
+// Wait for port stable to work, refers to specification
+// [USB20-9.1.2]
+//
+#define USB_WAIT_PORT_STABLE_STALL  (100 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port statue reg change, set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_STALL (100)
+
+//
+// Wait for set device address, refers to specification
+// [USB20-9.2.6.3, it says 2ms]
+//
+#define USB_SET_DEVICE_ADDRESS_STALL   (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for retry max packet size, set by experience
+//
+#define USB_RETRY_MAX_PACK_SIZE_STALL  (100 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for hub port power-on, refers to specification
+// [USB20-11.23.2]
+//
+#define USB_SET_PORT_POWER_STALL       (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port reset, refers to specification
+// [USB20-7.1.7.5, it says 10ms for hub and 50ms for
+// root hub]
+//
+// According to USB2.0, Chapter 11.5.1.5 Resetting,
+// the worst case for TDRST is 20ms
+//
+#define USB_SET_PORT_RESET_STALL       (20 * USB_BUS_1_MILLISECOND)
+#define USB_SET_ROOT_PORT_RESET_STALL  (50 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port recovery to accept SetAddress, refers to specification
+// [USB20-7.1.7.5, it says 10 ms for TRSTRCY]
+//
+#define USB_SET_PORT_RECOVERY_STALL    (10 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for clear roothub port reset, set by experience
+//
+#define USB_CLR_ROOT_PORT_RESET_STALL  (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for set roothub port enable, set by experience
+//
+#define USB_SET_ROOT_PORT_ENABLE_STALL (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Send general device request timeout.
+// 
+// The USB Specification 2.0, section 11.24.1 recommends a value of
+// 50 milliseconds.  We use a value of 500 milliseconds to work
+// around slower hubs and devices.
+//
+#define USB_GENERAL_DEVICE_REQUEST_TIMEOUT 500
+
+//
+// Send clear feature request timeout, set by experience
+//
+#define USB_CLEAR_FEATURE_REQUEST_TIMEOUT  10
+
+//
+// Bus raises TPL to TPL_NOTIFY to serialize all its operations
+// to protect shared data structures.
+//
+#define  USB_BUS_TPL               TPL_NOTIFY
+
+#define  USB_INTERFACE_SIGNATURE   SIGNATURE_32 ('U', 'S', 'B', 'I')
+#define  USB_BUS_SIGNATURE         SIGNATURE_32 ('U', 'S', 'B', 'B')
+
+#define USB_BIT(a)                  ((UINTN)(1 << (a)))
+#define USB_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define USB_INTERFACE_FROM_USBIO(a) \
+          CR(a, USB_INTERFACE, UsbIo, USB_INTERFACE_SIGNATURE)
+
+#define USB_BUS_FROM_THIS(a) \
+          CR(a, USB_BUS, BusId, USB_BUS_SIGNATURE)
+
+//
+// Used to locate USB_BUS
+// UsbBusProtocol is the private protocol.
+// gEfiCallerIdGuid will be used as its protocol guid.
+//
+typedef struct _EFI_USB_BUS_PROTOCOL {
+  UINT64                    Reserved;
+} EFI_USB_BUS_PROTOCOL;
+
+
+//
+// Stands for the real USB device. Each device may
+// has several seperately working interfaces.
+//
+struct _USB_DEVICE {
+  USB_BUS                   *Bus;
+
+  //
+  // Configuration information
+  //
+  UINT8                     Speed;
+  UINT8                     Address;
+  UINT32                    MaxPacket0;
+
+  //
+  // The device's descriptors and its configuration
+  //
+  USB_DEVICE_DESC           *DevDesc;
+  USB_CONFIG_DESC           *ActiveConfig;
+
+  UINT16                    LangId [USB_MAX_LANG_ID];
+  UINT16                    TotalLangId;
+
+  UINT8                     NumOfInterface;
+  USB_INTERFACE             *Interfaces [USB_MAX_INTERFACE];
+
+  //
+  // Parent child relationship
+  //
+  EFI_USB2_HC_TRANSACTION_TRANSLATOR Translator;
+
+  UINT8                     ParentAddr;
+  USB_INTERFACE             *ParentIf;
+  UINT8                     ParentPort;       // Start at 0
+  UINT8                     Tier;
+  BOOLEAN                   DisconnectFail;
+};
+
+//
+// Stands for different functions of USB device
+//
+struct _USB_INTERFACE {
+  UINTN                     Signature;
+  USB_DEVICE                *Device;
+  USB_INTERFACE_DESC        *IfDesc;
+  USB_INTERFACE_SETTING     *IfSetting;
+
+  //
+  // Handles and protocols
+  //
+  EFI_HANDLE                Handle;
+  EFI_USB_IO_PROTOCOL       UsbIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  BOOLEAN                   IsManaged;
+
+  //
+  // Hub device special data
+  //
+  BOOLEAN                   IsHub;
+  USB_HUB_API               *HubApi;
+  UINT8                     NumOfPort;
+  EFI_EVENT                 HubNotify;
+
+  //
+  // Data used only by normal hub devices
+  //
+  USB_ENDPOINT_DESC         *HubEp;
+  UINT8                     *ChangeMap;
+
+  //
+  // Data used only by root hub to hand over device to
+  // companion UHCI driver if low/full speed devices are
+  // connected to EHCI.
+  //
+  UINT8                     MaxSpeed;
+};
+
+//
+// Stands for the current USB Bus
+//
+struct _USB_BUS {
+  UINTN                     Signature;
+  EFI_USB_BUS_PROTOCOL      BusId;
+
+  //
+  // Managed USB host controller
+  //
+  EFI_HANDLE                HostHandle;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_USB2_HC_PROTOCOL      *Usb2Hc;
+  EFI_USB_HC_PROTOCOL       *UsbHc;
+
+  //
+  // Recorded the max supported usb devices.
+  // XHCI can support up to 255 devices.
+  // EHCI/UHCI/OHCI supports up to 127 devices.
+  //
+  UINT32                    MaxDevices;
+  //
+  // An array of device that is on the bus. Devices[0] is
+  // for root hub. Device with address i is at Devices[i].
+  //
+  USB_DEVICE                *Devices[256];
+
+  //
+  // USB Bus driver need to control the recursive connect policy of the bus, only those wanted
+  // usb child device will be recursively connected.
+  //
+  // WantedUsbIoDPList tracks the Usb child devices which user want to recursivly fully connecte,
+  // every wanted child device is stored in a item of the WantedUsbIoDPList, whose structrure is
+  // DEVICE_PATH_LIST_ITEM
+  //
+  LIST_ENTRY                WantedUsbIoDPList;
+
+};
+
+//
+// USB Hub Api
+//
+struct _USB_HUB_API{
+  USB_HUB_INIT                Init;
+  USB_HUB_GET_PORT_STATUS     GetPortStatus;
+  USB_HUB_CLEAR_PORT_CHANGE   ClearPortChange;
+  USB_HUB_SET_PORT_FEATURE    SetPortFeature;
+  USB_HUB_CLEAR_PORT_FEATURE  ClearPortFeature;
+  USB_HUB_RESET_PORT          ResetPort;
+  USB_HUB_RELEASE             Release;
+};
+
+#define USB_US_LAND_ID   0x0409
+
+#define DEVICE_PATH_LIST_ITEM_SIGNATURE     SIGNATURE_32('d','p','l','i')
+typedef struct _DEVICE_PATH_LIST_ITEM{
+  UINTN                                 Signature;
+  LIST_ENTRY                            Link;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+} DEVICE_PATH_LIST_ITEM;
+
+typedef struct {
+  USB_CLASS_DEVICE_PATH           UsbClass;
+  EFI_DEVICE_PATH_PROTOCOL        End;
+} USB_CLASS_FORMAT_DEVICE_PATH;
+
+/**
+  Free a DEVICE_PATH_LIST_ITEM list.
+
+  @param  UsbIoDPList            a DEVICE_PATH_LIST_ITEM list pointer.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If free operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusFreeUsbDPList (
+  IN     LIST_ENTRY                                 *UsbIoDPList
+  );
+
+/**
+  Store a wanted usb child device info (its Usb part of device path) which is indicated by
+  RemainingDevicePath in a Usb bus which  is indicated by UsbBusId.
+
+  @param  UsbBusId               Point to EFI_USB_BUS_PROTOCOL interface.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            Add operation is successful.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusAddWantedUsbIoDP (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Check whether a usb child device is the wanted device in a bus.
+
+  @param  Bus     The Usb bus's private data pointer.
+  @param  UsbIf   The usb child device inferface.
+
+  @retval True    If a usb child device is the wanted device in a bus.
+  @retval False   If a usb child device is *NOT* the wanted device in a bus.
+
+**/
+BOOLEAN
+EFIAPI
+UsbBusIsWantedUsbIO (
+  IN USB_BUS                 *Bus,
+  IN USB_INTERFACE           *UsbIf
+  );
+
+/**
+  Recursively connnect every wanted usb child device to ensure they all fully connected.
+  Check all the child Usb IO handles in this bus, recursively connecte if it is wanted usb child device.
+
+  @param  UsbBusId                  Point to EFI_USB_BUS_PROTOCOL interface.
+
+  @retval EFI_SUCCESS               Connect is done successfully.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusRecursivelyConnectWantedUsbIo (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId
+  );
+
+/**
+  USB_IO function to execute a control transfer. This
+  function will execute the USB transfer. If transfer
+  successes, it will sync the internal state of USB bus
+  with device state.
+
+  @param  This                   The USB_IO instance
+  @param  Request                The control transfer request
+  @param  Direction              Direction for data stage
+  @param  Timeout                The time to wait before timeout
+  @param  Data                   The buffer holding the data
+  @param  DataLength             Then length of the data
+  @param  UsbStatus              USB result
+
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid
+  @retval EFI_SUCCESS            The control transfer succeded.
+  @retval Others                 Failed to execute the transfer
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoControlTransfer (
+  IN  EFI_USB_IO_PROTOCOL     *This,
+  IN  EFI_USB_DEVICE_REQUEST  *Request,
+  IN  EFI_USB_DATA_DIRECTION  Direction,
+  IN  UINT32                  Timeout,
+  IN  OUT VOID                *Data,      OPTIONAL
+  IN  UINTN                   DataLength, OPTIONAL
+  OUT UINT32                  *UsbStatus
+  );
+
+/**
+  Execute a bulk transfer to the device endpoint.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The bulk transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoBulkTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  );
+
+/**
+  Execute a synchronous interrupt transfer.
+
+  @param  This                   The USB IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  Timeout                Time to wait before timeout.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_SUCCESS            The synchronous interrupt transfer is OK.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to execute transfer, reason returned in
+                                 UsbStatus.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoSyncInterruptTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               Endpoint,
+  IN  OUT VOID            *Data,
+  IN  OUT UINTN           *DataLength,
+  IN  UINTN               Timeout,
+  OUT UINT32              *UsbStatus
+  );
+
+/**
+  Queue a new asynchronous interrupt transfer, or remove the old
+  request if (IsNewTransfer == FALSE).
+
+  @param  This                   The USB_IO instance.
+  @param  Endpoint               The device endpoint.
+  @param  IsNewTransfer          Whether this is a new request, if it's old, remove
+                                 the request.
+  @param  PollInterval           The interval to poll the transfer result, (in ms).
+  @param  DataLength             The length of perodic data transfer.
+  @param  Callback               The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_SUCCESS            New transfer is queued or old request is removed.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval Others                 Failed to queue the new request or remove the old
+                                 request.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncInterruptTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            Endpoint,
+  IN BOOLEAN                          IsNewTransfer,
+  IN UINTN                            PollInterval,       OPTIONAL
+  IN UINTN                            DataLength,         OPTIONAL
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  Callback,           OPTIONAL
+  IN VOID                             *Context            OPTIONAL
+  );
+
+/**
+  Execute a synchronous isochronous transfer.
+
+  @param  This                   The USB IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of the data to transfer.
+  @param  UsbStatus              The result of USB transfer.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoIsochronousTransfer (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  IN  UINT8               DeviceEndpoint,
+  IN  OUT VOID            *Data,
+  IN  UINTN               DataLength,
+  OUT UINT32              *Status
+  );
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  This                   The USB_IO instance.
+  @param  DeviceEndpoint         The device endpoint.
+  @param  Data                   The data to transfer.
+  @param  DataLength             The length of perodic data transfer.
+  @param  IsochronousCallBack    The function to call periodicaly when transfer is
+                                 ready.
+  @param  Context                The context to the callback.
+
+  @retval EFI_UNSUPPORTED        Currently isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoAsyncIsochronousTransfer (
+  IN EFI_USB_IO_PROTOCOL              *This,
+  IN UINT8                            DeviceEndpoint,
+  IN OUT VOID                         *Data,
+  IN UINTN                            DataLength,
+  IN EFI_ASYNC_USB_TRANSFER_CALLBACK  IsochronousCallBack,
+  IN VOID                             *Context              OPTIONAL
+  );
+
+/**
+  Retrieve the device descriptor of the device.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive the device descriptor.
+
+  @retval EFI_SUCCESS            The device descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetDeviceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_DEVICE_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Return the configuration descriptor of the current active configuration.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The USB configuration descriptor.
+
+  @retval EFI_SUCCESS            The active configuration descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+  @retval EFI_NOT_FOUND          Currently no active configuration is selected.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetActiveConfigDescriptor (
+  IN  EFI_USB_IO_PROTOCOL       *This,
+  OUT EFI_USB_CONFIG_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Retrieve the active interface setting descriptor for this USB IO instance.
+
+  @param  This                   The USB IO instance.
+  @param  Descriptor             The variable to receive active interface setting.
+
+  @retval EFI_SUCCESS            The active interface setting is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetInterfaceDescriptor (
+  IN  EFI_USB_IO_PROTOCOL           *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR  *Descriptor
+  );
+
+/**
+  Retrieve the endpoint descriptor from this interface setting.
+
+  @param  This                   The USB IO instance.
+  @param  Index                  The index (start from zero) of the endpoint to
+                                 retrieve.
+  @param  Descriptor             The variable to receive the descriptor.
+
+  @retval EFI_SUCCESS            The endpoint descriptor is returned.
+  @retval EFI_INVALID_PARAMETER  Some parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetEndpointDescriptor (
+  IN  EFI_USB_IO_PROTOCOL         *This,
+  IN  UINT8                       Index,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR *Descriptor
+  );
+
+/**
+  Retrieve the supported language ID table from the device.
+
+  @param  This                   The USB IO instance.
+  @param  LangIDTable            The table to return the language IDs.
+  @param  TableSize              The size, in bytes, of the table LangIDTable.
+
+  @retval EFI_SUCCESS            The language ID is return.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetSupportedLanguages (
+  IN  EFI_USB_IO_PROTOCOL *This,
+  OUT UINT16              **LangIDTable,
+  OUT UINT16              *TableSize
+  );
+
+/**
+  Retrieve an indexed string in the language of LangID.
+
+  @param  This                   The USB IO instance.
+  @param  LangID                 The language ID of the string to retrieve.
+  @param  StringIndex            The index of the string.
+  @param  String                 The variable to receive the string.
+
+  @retval EFI_SUCCESS            The string is returned.
+  @retval EFI_NOT_FOUND          No such string existed.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoGetStringDescriptor (
+  IN  EFI_USB_IO_PROTOCOL   *This,
+  IN  UINT16                LangID,
+  IN  UINT8                 StringIndex,
+  OUT CHAR16                **String
+  );
+
+/**
+  Reset the device, then if that succeeds, reconfigure the
+  device with its address and current active configuration.
+
+  @param  This                   The USB IO instance.
+
+  @retval EFI_SUCCESS            The device is reset and configured.
+  @retval Others                 Failed to reset the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbIoPortReset (
+  IN EFI_USB_IO_PROTOCOL  *This
+  );
+
+/**
+  Install Usb Bus Protocol on host controller, and start the Usb bus.
+
+  @param This                    The USB bus driver binding instance.
+  @param Controller              The controller to check.
+  @param RemainingDevicePath     The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusBuildProtocol (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  The USB bus driver entry pointer.
+
+  @param ImageHandle       The driver image handle.
+  @param SystemTable       The system table.
+
+  @return EFI_SUCCESS      The component name protocol is installed.
+  @return Others           Failed to init the usb driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+  Check whether USB bus driver support this device.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The bus supports this controller.
+  @retval EFI_UNSUPPORTED        This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Start to process the controller.
+
+  @param  This                   The USB bus driver binding instance.
+  @param  Controller             The controller to check.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb bus.
+  @retval EFI_ALREADY_STARTED    The controller is already controlled by the usb
+                                 bus.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop handle the controller by this USB bus driver.
+
+  @param  This                   The USB bus driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The child of USB bus that opened controller
+                                 BY_CHILD.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The controller or children are stopped.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusControllerDriverStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+extern EFI_USB_IO_PROTOCOL            mUsbIoProtocol;
+extern EFI_DRIVER_BINDING_PROTOCOL    mUsbBusDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL    mUsbBusComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   mUsbBusComponentName2;
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf
new file mode 100644
index 0000000000..b7b30cc937
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf
@@ -0,0 +1,79 @@
+## @file
+#  The Usb Bus Dxe driver is used to enumerate and manage all attached usb devices.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBusDxe
+  MODULE_UNI_FILE                = UsbBusDxe.uni
+  FILE_GUID                      = 240612B7-A063-11d4-9A3A-0090273FC14D
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = UsbBusDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  mUsbBusDriverBinding                        
+#  COMPONENT_NAME                =  mUsbBusComponentName
+#  COMPONENT_NAME2               =  mUsbBusComponentName2
+#
+
+[Sources]
+  UsbDesc.c
+  UsbEnumer.c
+  UsbEnumer.h
+  UsbBus.c
+  UsbHub.c
+  ComponentName.c
+  UsbUtility.h
+  UsbHub.h
+  UsbUtility.c
+  UsbDesc.h
+  UsbBus.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  MemoryAllocationLib
+  DevicePathLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  ReportStatusCodeLib
+
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## BY_START
+  ## TO_START
+  ## BY_START
+  gEfiDevicePathProtocolGuid                    
+  gEfiUsb2HcProtocolGuid                        ## TO_START
+  gEfiUsbHcProtocolGuid                         ## TO_START
+
+# [Event]
+#
+# EVENT_TYPE_PERIODIC_TIMER       ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni
new file mode 100644
index 0000000000..70f9e62436
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni
new file mode 100644
index 0000000000..305ab09c3e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c
new file mode 100644
index 0000000000..9687eb0bca
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.c
@@ -0,0 +1,978 @@
+/** @file
+
+    Manage Usb Descriptor List
+
+Copyright (c) 2007 - 2013, 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 "UsbBus.h"
+
+
+/**
+  Free the interface setting descriptor.
+
+  @param  Setting               The descriptor to free.
+
+**/
+VOID
+UsbFreeInterfaceDesc (
+  IN USB_INTERFACE_SETTING  *Setting
+  )
+{
+  USB_ENDPOINT_DESC       *Ep;
+  UINTN                   Index;
+
+  if (Setting->Endpoints != NULL) {
+    //
+    // Each interface setting may have several endpoints, free them first.
+    //
+    for (Index = 0; Index < Setting->Desc.NumEndpoints; Index++) {
+      Ep = Setting->Endpoints[Index];
+
+      if (Ep != NULL) {
+        FreePool (Ep);
+      }
+    }
+
+    //
+    // Only call FreePool() if NumEndpoints > 0.
+    //
+    if (Setting->Desc.NumEndpoints > 0) {
+      FreePool (Setting->Endpoints);
+    }
+  }
+
+  FreePool (Setting);
+}
+
+
+/**
+  Free a configuration descriptor with its interface
+  descriptors. It may be initialized partially.
+
+  @param  Config                The configuration descriptor to free.
+
+**/
+VOID
+UsbFreeConfigDesc (
+  IN USB_CONFIG_DESC      *Config
+  )
+{
+  USB_INTERFACE_DESC      *Interface;
+  UINTN                   Index;
+  UINTN                   SetIndex;
+
+  if (Config->Interfaces != NULL) {
+    //
+    // A configuration may have several interfaces, free the interface
+    //
+    for (Index = 0; Index < Config->Desc.NumInterfaces; Index++) {
+      Interface = Config->Interfaces[Index];
+
+      if (Interface == NULL) {
+        continue;
+      }
+
+      //
+      // Each interface may have several settings, free the settings
+      //
+      for (SetIndex = 0; SetIndex < Interface->NumOfSetting; SetIndex++) {
+        if (Interface->Settings[SetIndex] != NULL) {
+          UsbFreeInterfaceDesc (Interface->Settings[SetIndex]);
+        }
+      }
+
+      FreePool (Interface);
+    }
+
+    FreePool (Config->Interfaces);
+  }
+
+  FreePool (Config);
+
+}
+
+
+/**
+  Free a device descriptor with its configurations.
+
+  @param  DevDesc               The device descriptor.
+
+**/
+VOID
+UsbFreeDevDesc (
+  IN USB_DEVICE_DESC      *DevDesc
+  )
+{
+  UINTN                   Index;
+
+  if (DevDesc->Configs != NULL) {
+    for (Index = 0; Index < DevDesc->Desc.NumConfigurations; Index++) {
+      if (DevDesc->Configs[Index] != NULL) {
+        UsbFreeConfigDesc (DevDesc->Configs[Index]);
+      }
+    }
+
+    FreePool (DevDesc->Configs);
+  }
+
+  FreePool (DevDesc);
+}
+
+
+/**
+  Create a descriptor.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+  @param  Type                  The type of descriptor to create.
+  @param  Consumed              Number of bytes consumed.
+
+  @return Created descriptor or NULL.
+
+**/
+VOID *
+UsbCreateDesc (
+  IN  UINT8               *DescBuf,
+  IN  UINTN               Len,
+  IN  UINT8               Type,
+  OUT UINTN               *Consumed
+  )
+{
+  USB_DESC_HEAD           *Head;
+  UINTN                   DescLen;
+  UINTN                   CtrlLen;
+  UINTN                   Offset;
+  VOID                    *Desc;
+
+  DescLen   = 0;
+  CtrlLen   = 0;
+  *Consumed = 0;
+
+  switch (Type) {
+  case USB_DESC_TYPE_DEVICE:
+    DescLen = sizeof (EFI_USB_DEVICE_DESCRIPTOR);
+    CtrlLen = sizeof (USB_DEVICE_DESC);
+    break;
+
+  case USB_DESC_TYPE_CONFIG:
+    DescLen = sizeof (EFI_USB_CONFIG_DESCRIPTOR);
+    CtrlLen = sizeof (USB_CONFIG_DESC);
+    break;
+
+  case USB_DESC_TYPE_INTERFACE:
+    DescLen = sizeof (EFI_USB_INTERFACE_DESCRIPTOR);
+    CtrlLen = sizeof (USB_INTERFACE_SETTING);
+    break;
+
+  case USB_DESC_TYPE_ENDPOINT:
+    DescLen = sizeof (EFI_USB_ENDPOINT_DESCRIPTOR);
+    CtrlLen = sizeof (USB_ENDPOINT_DESC);
+    break;
+  }
+
+  //
+  // 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*)DescBuf;
+
+  while ((Offset < Len) && (Head->Type != Type)) {
+    Offset += Head->Len;
+    if (Len <= Offset) {
+      DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor, Beyond boundary!\n"));
+      return NULL;
+    }
+    Head    = (USB_DESC_HEAD*)(DescBuf + Offset);
+    if (Head->Len == 0) {
+      DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor, Head->Len = 0!\n"));
+      return NULL;
+    }
+  }
+
+  if ((Len <= Offset)      || (Len < Offset + DescLen) ||
+      (Head->Type != Type) || (Head->Len != DescLen)) {
+    DEBUG (( EFI_D_ERROR, "UsbCreateDesc: met mal-format descriptor\n"));
+    return NULL;
+  }
+
+  Desc = AllocateZeroPool ((UINTN) CtrlLen);
+  if (Desc == NULL) {
+    return NULL;
+  }
+
+  CopyMem (Desc, Head, (UINTN) DescLen);
+
+  *Consumed = Offset + Head->Len;
+
+  return Desc;
+}
+
+
+/**
+  Parse an interface descriptor and its endpoints.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+  @param  Consumed              The number of raw descriptor consumed.
+
+  @return The create interface setting or NULL if failed.
+
+**/
+USB_INTERFACE_SETTING *
+UsbParseInterfaceDesc (
+  IN  UINT8               *DescBuf,
+  IN  UINTN               Len,
+  OUT UINTN               *Consumed
+  )
+{
+  USB_INTERFACE_SETTING   *Setting;
+  USB_ENDPOINT_DESC       *Ep;
+  UINTN                   Index;
+  UINTN                   NumEp;
+  UINTN                   Used;
+  UINTN                   Offset;
+
+  *Consumed = 0;
+  Setting   = UsbCreateDesc (DescBuf, Len, USB_DESC_TYPE_INTERFACE, &Used);
+
+  if (Setting == NULL) {
+    DEBUG (( EFI_D_ERROR, "UsbParseInterfaceDesc: failed to create interface descriptor\n"));
+    return NULL;
+  }
+
+  Offset = Used;
+
+  //
+  // Create an array to hold the interface's endpoints
+  //
+  NumEp  = Setting->Desc.NumEndpoints;
+
+  DEBUG (( EFI_D_INFO, "UsbParseInterfaceDesc: interface %d(setting %d) has %d endpoints\n",
+              Setting->Desc.InterfaceNumber, Setting->Desc.AlternateSetting, (UINT32)NumEp));
+
+  if (NumEp == 0) {
+    goto ON_EXIT;
+  }
+
+  Setting->Endpoints  = AllocateZeroPool (sizeof (USB_ENDPOINT_DESC *) * NumEp);
+
+  if (Setting->Endpoints == NULL) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Create the endpoints for this interface
+  //
+  for (Index = 0; (Index < NumEp) && (Offset < Len); Index++) {
+    Ep = UsbCreateDesc (DescBuf + Offset, Len - Offset, USB_DESC_TYPE_ENDPOINT, &Used);
+
+    if (Ep == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbParseInterfaceDesc: failed to create endpoint(index %d)\n", (UINT32)Index));
+      goto ON_ERROR;
+    }
+
+    Setting->Endpoints[Index]  = Ep;
+    Offset                    += Used;
+  }
+
+
+ON_EXIT:
+  *Consumed = Offset;
+  return Setting;
+
+ON_ERROR:
+  UsbFreeInterfaceDesc (Setting);
+  return NULL;
+}
+
+
+/**
+  Parse the configuration descriptor and its interfaces.
+
+  @param  DescBuf               The buffer of raw descriptor.
+  @param  Len                   The length of the raw descriptor buffer.
+
+  @return The created configuration descriptor.
+
+**/
+USB_CONFIG_DESC *
+UsbParseConfigDesc (
+  IN UINT8                *DescBuf,
+  IN UINTN                Len
+  )
+{
+  USB_CONFIG_DESC         *Config;
+  USB_INTERFACE_SETTING   *Setting;
+  USB_INTERFACE_DESC      *Interface;
+  UINTN                   Index;
+  UINTN                   NumIf;
+  UINTN                   Consumed;
+
+  ASSERT (DescBuf != NULL);
+
+  Config = UsbCreateDesc (DescBuf, Len, USB_DESC_TYPE_CONFIG, &Consumed);
+
+  if (Config == NULL) {
+    return NULL;
+  }
+
+  //
+  // Initialize an array of setting for the configuration's interfaces.
+  //
+  NumIf               = Config->Desc.NumInterfaces;
+  Config->Interfaces  = AllocateZeroPool (sizeof (USB_INTERFACE_DESC *) * NumIf);
+
+  if (Config->Interfaces == NULL) {
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbParseConfigDesc: config %d has %d interfaces\n",
+                Config->Desc.ConfigurationValue, (UINT32)NumIf));
+
+  for (Index = 0; Index < NumIf; Index++) {
+    Interface = AllocateZeroPool (sizeof (USB_INTERFACE_DESC));
+
+    if (Interface == NULL) {
+      goto ON_ERROR;
+    }
+
+    Config->Interfaces[Index] = Interface;
+  }
+
+  //
+  // If a configuration has several interfaces, these interfaces are
+  // numbered from zero to n. If a interface has several settings,
+  // these settings are also number from zero to m. The interface
+  // setting must be organized as |interface 0, setting 0|interface 0
+  // setting 1|interface 1, setting 0|interface 2, setting 0|. Check
+  // USB2.0 spec, page 267.
+  //
+  DescBuf += Consumed;
+  Len     -= Consumed;
+
+  //
+  // Make allowances for devices that return extra data at the 
+  // end of their config descriptors
+  //
+  while (Len >= sizeof (EFI_USB_INTERFACE_DESCRIPTOR)) {
+    Setting = UsbParseInterfaceDesc (DescBuf, Len, &Consumed);
+
+    if (Setting == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbParseConfigDesc: warning: failed to get interface setting, stop parsing now.\n"));
+      break;
+
+    } else if (Setting->Desc.InterfaceNumber >= NumIf) {
+      DEBUG (( EFI_D_ERROR, "UsbParseConfigDesc: mal-formated interface descriptor\n"));
+
+      UsbFreeInterfaceDesc (Setting);
+      goto ON_ERROR;
+    }
+
+    //
+    // Insert the descriptor to the corresponding set.
+    //
+    Interface = Config->Interfaces[Setting->Desc.InterfaceNumber];
+
+    if (Interface->NumOfSetting >= USB_MAX_INTERFACE_SETTING) {
+      goto ON_ERROR;
+    }
+
+    Interface->Settings[Interface->NumOfSetting] = Setting;
+    Interface->NumOfSetting++;
+
+    DescBuf += Consumed;
+    Len     -= Consumed;
+  }
+
+  return Config;
+
+ON_ERROR:
+  UsbFreeConfigDesc (Config);
+  return NULL;
+}
+
+
+/**
+  USB standard control transfer support routine. This
+  function is used by USB device. It is possible that
+  the device's interfaces are still waiting to be
+  enumerated.
+
+  @param  UsbDev                The usb device.
+  @param  Direction             The direction of data transfer.
+  @param  Type                  Standard / class specific / vendor specific.
+  @param  Target                The receiving target.
+  @param  Request               Which request.
+  @param  Value                 The wValue parameter of the request.
+  @param  Index                 The wIndex parameter of the request.
+  @param  Buf                   The buffer to receive data into / transmit from.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The control request is executed.
+  @retval EFI_DEVICE_ERROR      Failed to execute the control transfer.
+
+**/
+EFI_STATUS
+UsbCtrlRequest (
+  IN USB_DEVICE             *UsbDev,
+  IN EFI_USB_DATA_DIRECTION Direction,
+  IN UINTN                  Type,
+  IN UINTN                  Target,
+  IN UINTN                  Request,
+  IN UINT16                 Value,
+  IN UINT16                 Index,
+  IN OUT VOID               *Buf,
+  IN UINTN                  Length
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINTN                   Len;
+
+  ASSERT ((UsbDev != NULL) && (UsbDev->Bus != NULL));
+
+  DevReq.RequestType  = USB_REQUEST_TYPE (Direction, Type, Target);
+  DevReq.Request      = (UINT8) Request;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = (UINT16) Length;
+
+  Len                 = Length;
+  Status = UsbHcControlTransfer (
+             UsbDev->Bus,
+             UsbDev->Address,
+             UsbDev->Speed,
+             UsbDev->MaxPacket0,
+             &DevReq,
+             Direction,
+             Buf,
+             &Len,
+             USB_GENERAL_DEVICE_REQUEST_TIMEOUT,
+             &UsbDev->Translator,
+             &Result
+             );
+
+  return Status;
+}
+
+
+/**
+  Get the standard descriptors.
+
+  @param  UsbDev                The USB device to read descriptor from.
+  @param  DescType              The type of descriptor to read.
+  @param  DescIndex             The index of descriptor to read.
+  @param  LangId                Language ID, only used to get string, otherwise set
+                                it to 0.
+  @param  Buf                   The buffer to hold the descriptor read.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The descriptor is read OK.
+  @retval Others                Failed to retrieve the descriptor.
+
+**/
+EFI_STATUS
+UsbCtrlGetDesc (
+  IN  USB_DEVICE          *UsbDev,
+  IN  UINTN               DescType,
+  IN  UINTN               DescIndex,
+  IN  UINT16              LangId,
+  OUT VOID                *Buf,
+  IN  UINTN               Length
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_GET_DESCRIPTOR,
+             (UINT16) ((DescType << 8) | DescIndex),
+             LangId,
+             Buf,
+             Length
+             );
+
+  return Status;
+}
+
+
+/**
+  Return the max packet size for endpoint zero. This function
+  is the first function called to get descriptors during bus
+  enumeration.
+
+  @param  UsbDev                The usb device.
+
+  @retval EFI_SUCCESS           The max packet size of endpoint zero is retrieved.
+  @retval EFI_DEVICE_ERROR      Failed to retrieve it.
+
+**/
+EFI_STATUS
+UsbGetMaxPacketSize0 (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_DEVICE_DESCRIPTOR DevDesc;
+  EFI_STATUS                Status;
+  UINTN                     Index;
+
+
+  //
+  // Get the first 8 bytes of the device descriptor which contains
+  // max packet size for endpoint 0, which is at least 8.
+  //
+  for (Index = 0; Index < 3; Index++) {
+    Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_DEVICE, 0, 0, &DevDesc, 8);
+
+    if (!EFI_ERROR (Status)) {
+      if ((DevDesc.BcdUSB == 0x0300) && (DevDesc.MaxPacketSize0 == 9)) {
+        UsbDev->MaxPacket0 = 1 << 9;
+        return EFI_SUCCESS;
+      }
+      UsbDev->MaxPacket0 = DevDesc.MaxPacketSize0;
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (USB_RETRY_MAX_PACK_SIZE_STALL);
+  }
+
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Get the device descriptor for the device.
+
+  @param  UsbDev                The Usb device to retrieve descriptor from.
+
+  @retval EFI_SUCCESS           The device descriptor is returned.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory.
+
+**/
+EFI_STATUS
+UsbGetDevDesc (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  USB_DEVICE_DESC         *DevDesc;
+  EFI_STATUS              Status;
+
+  DevDesc = AllocateZeroPool (sizeof (USB_DEVICE_DESC));
+
+  if (DevDesc == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status  = UsbCtrlGetDesc (
+              UsbDev,
+              USB_DESC_TYPE_DEVICE,
+              0,
+              0,
+              DevDesc,
+              sizeof (EFI_USB_DEVICE_DESCRIPTOR)
+              );
+
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (DevDesc);
+  } else {
+    UsbDev->DevDesc = DevDesc;
+  }
+
+  return Status;
+}
+
+
+/**
+  Retrieve the indexed string for the language. It requires two
+  steps to get a string, first to get the string's length. Then
+  the string itself.
+
+  @param  UsbDev                The usb device.
+  @param  Index                 The index the string to retrieve.
+  @param  LangId                Language ID.
+
+  @return The created string descriptor or NULL.
+
+**/
+EFI_USB_STRING_DESCRIPTOR *
+UsbGetOneString (
+  IN     USB_DEVICE       *UsbDev,
+  IN     UINT8            Index,
+  IN     UINT16           LangId
+  )
+{
+  EFI_USB_STRING_DESCRIPTOR Desc;
+  EFI_STATUS                Status;
+  UINT8                     *Buf;
+
+  //
+  // First get two bytes which contains the string length.
+  //
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_STRING, Index, LangId, &Desc, 2);
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Buf = AllocateZeroPool (Desc.Length);
+
+  if (Buf == NULL) {
+    return NULL;
+  }
+
+  Status = UsbCtrlGetDesc (
+             UsbDev,
+             USB_DESC_TYPE_STRING,
+             Index,
+             LangId,
+             Buf,
+             Desc.Length
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return NULL;
+  }
+
+  return (EFI_USB_STRING_DESCRIPTOR *) Buf;
+}
+
+
+/**
+  Build the language ID table for string descriptors.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_UNSUPPORTED       This device doesn't support string table.
+
+**/
+EFI_STATUS
+UsbBuildLangTable (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_STRING_DESCRIPTOR *Desc;
+  EFI_STATUS                Status;
+  UINTN                     Index;
+  UINTN                     Max;
+  UINT16                    *Point;
+
+  //
+  // The string of language ID zero returns the supported languages
+  //
+  Desc = UsbGetOneString (UsbDev, 0, 0);
+
+  if (Desc == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Desc->Length < 4) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+
+  Status = EFI_SUCCESS;
+
+  Max   = (Desc->Length - 2) / 2;
+  Max   = MIN(Max, USB_MAX_LANG_ID);
+  
+  Point = Desc->String;
+  for (Index = 0; Index < Max; Index++) {
+    UsbDev->LangId[Index] = *Point;
+    Point++;
+  }
+
+  UsbDev->TotalLangId = (UINT16)Max;
+
+ON_EXIT:
+  gBS->FreePool (Desc);
+  return Status;
+}
+
+
+/**
+  Retrieve the indexed configure for the device. USB device
+  returns the configuration together with the interfaces for
+  this configuration. Configuration descriptor is also of
+  variable length.
+
+  @param  UsbDev                The Usb interface.
+  @param  Index                 The index of the configuration.
+
+  @return The created configuration descriptor.
+
+**/
+EFI_USB_CONFIG_DESCRIPTOR *
+UsbGetOneConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Index
+  )
+{
+  EFI_USB_CONFIG_DESCRIPTOR Desc;
+  EFI_STATUS                Status;
+  VOID                      *Buf;
+
+  //
+  // First get four bytes which contains the total length
+  // for this configuration.
+  //
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_CONFIG, Index, 0, &Desc, 8);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbGetOneConfig: failed to get descript length(%d) %r\n",
+                Desc.TotalLength, Status));
+
+    return NULL;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbGetOneConfig: total length is %d\n", Desc.TotalLength));
+
+  Buf = AllocateZeroPool (Desc.TotalLength);
+
+  if (Buf == NULL) {
+    return NULL;
+  }
+
+  Status = UsbCtrlGetDesc (UsbDev, USB_DESC_TYPE_CONFIG, Index, 0, Buf, Desc.TotalLength);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbGetOneConfig: failed to get full descript %r\n", Status));
+
+    FreePool (Buf);
+    return NULL;
+  }
+
+  return Buf;
+}
+
+
+/**
+  Build the whole array of descriptors. This function must
+  be called after UsbGetMaxPacketSize0 returns the max packet
+  size correctly for endpoint 0.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The descriptor table is build.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the descriptor.
+
+**/
+EFI_STATUS
+UsbBuildDescTable (
+  IN USB_DEVICE           *UsbDev
+  )
+{
+  EFI_USB_CONFIG_DESCRIPTOR *Config;
+  USB_DEVICE_DESC           *DevDesc;
+  USB_CONFIG_DESC           *ConfigDesc;
+  UINT8                     NumConfig;
+  EFI_STATUS                Status;
+  UINT8                     Index;
+
+  //
+  // Get the device descriptor, then allocate the configure
+  // descriptor pointer array to hold configurations.
+  //
+  Status = UsbGetDevDesc (UsbDev);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to get device descriptor - %r\n", Status));
+    return Status;
+  }
+
+  DevDesc   = UsbDev->DevDesc;
+  NumConfig = DevDesc->Desc.NumConfigurations;
+  if (NumConfig == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DevDesc->Configs = AllocateZeroPool (NumConfig * sizeof (USB_CONFIG_DESC *));
+  if (DevDesc->Configs == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbBuildDescTable: device has %d configures\n", NumConfig));
+
+  //
+  // Read each configurations, then parse them
+  //
+  for (Index = 0; Index < NumConfig; Index++) {
+    Config = UsbGetOneConfig (UsbDev, Index);
+
+    if (Config == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to get configure (index %d)\n", Index));
+
+      //
+      // If we can get the default descriptor, it is likely that the
+      // device is still operational.
+      //
+      if (Index == 0) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      break;
+    }
+
+    ConfigDesc = UsbParseConfigDesc ((UINT8 *) Config, Config->TotalLength);
+
+    FreePool (Config);
+
+    if (ConfigDesc == NULL) {
+      DEBUG (( EFI_D_ERROR, "UsbBuildDescTable: failed to parse configure (index %d)\n", Index));
+
+      //
+      // If we can get the default descriptor, it is likely that the
+      // device is still operational.
+      //
+      if (Index == 0) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      break;
+    }
+
+    DevDesc->Configs[Index] = ConfigDesc;
+  }
+
+  //
+  // Don't return error even this function failed because
+  // it is possible for the device to not support strings.
+  //
+  Status = UsbBuildLangTable (UsbDev);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_INFO, "UsbBuildDescTable: get language ID table %r\n", Status));
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Set the device's address.
+
+  @param  UsbDev                The device to set address to.
+  @param  Address               The address to set.
+
+  @retval EFI_SUCCESS           The device is set to the address.
+  @retval Others                Failed to set the device address.
+
+**/
+EFI_STATUS
+UsbSetAddress (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Address
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_SET_ADDRESS,
+             Address,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Set the device's configuration. This function changes
+  the device's internal state. UsbSelectConfig changes
+  the Usb bus's internal state.
+
+  @param  UsbDev                The USB device to set configure to.
+  @param  ConfigIndex           The configure index to set.
+
+  @retval EFI_SUCCESS           The device is configured now.
+  @retval Others                Failed to set the device configure.
+
+**/
+EFI_STATUS
+UsbSetConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                ConfigIndex
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             UsbDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_STANDARD,
+             USB_TARGET_DEVICE,
+             USB_REQ_SET_CONFIG,
+             ConfigIndex,
+             0,
+             NULL,
+             0
+            );
+
+  return Status;
+}
+
+
+/**
+  Usb UsbIo interface to clear the feature. This is should
+  only be used by HUB which is considered a device driver
+  on top of the UsbIo interface.
+
+  @param  UsbIo                 The UsbIo interface.
+  @param  Target                The target of the transfer: endpoint/device.
+  @param  Feature               The feature to clear.
+  @param  Index                 The wIndex parameter.
+
+  @retval EFI_SUCCESS           The device feature is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbIoClearFeature (
+  IN  EFI_USB_IO_PROTOCOL *UsbIo,
+  IN  UINTN               Target,
+  IN  UINT16              Feature,
+  IN  UINT16              Index
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  UINT32                  UsbResult;
+  EFI_STATUS              Status;
+
+  DevReq.RequestType  = USB_REQUEST_TYPE (EfiUsbNoData, USB_REQ_TYPE_STANDARD, Target);
+  DevReq.Request      = USB_REQ_CLEAR_FEATURE;
+  DevReq.Value        = Feature;
+  DevReq.Index        = Index;
+  DevReq.Length       = 0;
+
+  Status = UsbIo->UsbControlTransfer (
+                    UsbIo,
+                    &DevReq,
+                    EfiUsbNoData,
+                    USB_CLEAR_FEATURE_REQUEST_TIMEOUT,
+                    NULL,
+                    0,
+                    &UsbResult
+                    );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h
new file mode 100644
index 0000000000..482a71f338
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbDesc.h
@@ -0,0 +1,233 @@
+/** @file
+
+    Manage Usb Descriptor List
+
+Copyright (c) 2007 - 2014, 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.
+
+**/
+
+#ifndef _USB_DESCRIPTOR_H_
+#define _USB_DESCRIPTOR_H_
+
+#define USB_MAX_INTERFACE_SETTING  256
+
+//
+// The RequestType in EFI_USB_DEVICE_REQUEST is composed of
+// three fields: One bit direction, 2 bit type, and 5 bit
+// target.
+//
+#define USB_REQUEST_TYPE(Dir, Type, Target) \
+          ((UINT8)((((Dir) == EfiUsbDataIn ? 0x01 : 0) << 7) | (Type) | (Target)))
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+
+//
+// Each USB device has a device descriptor. Each device may
+// have several configures. Each configure contains several
+// interfaces. Each interface may have several settings. Each
+// setting has several endpoints.
+//
+// EFI_USB_..._DESCRIPTOR must be the first member of the
+// structure.
+//
+typedef struct {
+  EFI_USB_ENDPOINT_DESCRIPTOR   Desc;
+  UINT8                         Toggle;
+} USB_ENDPOINT_DESC;
+
+typedef struct {
+  EFI_USB_INTERFACE_DESCRIPTOR  Desc;
+  USB_ENDPOINT_DESC             **Endpoints;
+} USB_INTERFACE_SETTING;
+
+//
+// An interface may have several settings. Use a
+// fixed max number of settings to simplify code.
+// It should sufice in most environments.
+//
+typedef struct {
+  USB_INTERFACE_SETTING*        Settings[USB_MAX_INTERFACE_SETTING];
+  UINTN                         NumOfSetting;
+  UINTN                         ActiveIndex;  // Index of active setting
+} USB_INTERFACE_DESC;
+
+typedef struct {
+  EFI_USB_CONFIG_DESCRIPTOR     Desc;
+  USB_INTERFACE_DESC            **Interfaces;
+} USB_CONFIG_DESC;
+
+typedef struct {
+  EFI_USB_DEVICE_DESCRIPTOR     Desc;
+  USB_CONFIG_DESC               **Configs;
+} USB_DEVICE_DESC;
+
+/**
+  USB standard control transfer support routine. This
+  function is used by USB device. It is possible that
+  the device's interfaces are still waiting to be
+  enumerated.
+
+  @param  UsbDev                The usb device.
+  @param  Direction             The direction of data transfer.
+  @param  Type                  Standard / class specific / vendor specific.
+  @param  Target                The receiving target.
+  @param  Request               Which request.
+  @param  Value                 The wValue parameter of the request.
+  @param  Index                 The wIndex parameter of the request.
+  @param  Buf                   The buffer to receive data into / transmit from.
+  @param  Length                The length of the buffer.
+
+  @retval EFI_SUCCESS           The control request is executed.
+  @retval EFI_DEVICE_ERROR      Failed to execute the control transfer.
+
+**/
+EFI_STATUS
+UsbCtrlRequest (
+  IN USB_DEVICE             *UsbDev,
+  IN EFI_USB_DATA_DIRECTION Direction,
+  IN UINTN                  Type,
+  IN UINTN                  Target,
+  IN UINTN                  Request,
+  IN UINT16                 Value,
+  IN UINT16                 Index,
+  IN OUT VOID               *Buf,
+  IN UINTN                  Length
+  );
+
+/**
+  Return the max packet size for endpoint zero. This function
+  is the first function called to get descriptors during bus
+  enumeration.
+
+  @param  UsbDev                The usb device.
+
+  @retval EFI_SUCCESS           The max packet size of endpoint zero is retrieved.
+  @retval EFI_DEVICE_ERROR      Failed to retrieve it.
+
+**/
+EFI_STATUS
+UsbGetMaxPacketSize0 (
+  IN USB_DEVICE           *UsbDev
+  );
+
+/**
+  Free a device descriptor with its configurations.
+
+  @param  DevDesc               The device descriptor.
+
+  @return None.
+
+**/
+VOID
+UsbFreeDevDesc (
+  IN USB_DEVICE_DESC      *DevDesc
+  );
+
+/**
+  Retrieve the indexed string for the language. It requires two
+  steps to get a string, first to get the string's length. Then
+  the string itself.
+
+  @param  UsbDev                The usb device.
+  @param  StringIndex           The index of the string to retrieve.
+  @param  LangId                Language ID.
+
+  @return The created string descriptor or NULL.
+
+**/
+EFI_USB_STRING_DESCRIPTOR*
+UsbGetOneString (
+  IN     USB_DEVICE       *UsbDev,
+  IN     UINT8            StringIndex,
+  IN     UINT16           LangId
+  );
+
+/**
+  Build the whole array of descriptors. This function must
+  be called after UsbGetMaxPacketSize0 returns the max packet
+  size correctly for endpoint 0.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The descriptor table is build.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the descriptor.
+
+**/
+EFI_STATUS
+UsbBuildDescTable (
+  IN USB_DEVICE           *UsbDev
+  );
+
+/**
+  Set the device's address.
+
+  @param  UsbDev                The device to set address to.
+  @param  Address               The address to set.
+
+  @retval EFI_SUCCESS           The device is set to the address.
+  @retval Others                Failed to set the device address.
+
+**/
+EFI_STATUS
+UsbSetAddress (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Address
+  );
+
+/**
+  Set the device's configuration. This function changes
+  the device's internal state. UsbSelectConfig changes
+  the Usb bus's internal state.
+
+  @param  UsbDev                The USB device to set configure to.
+  @param  ConfigIndex           The configure index to set.
+
+  @retval EFI_SUCCESS           The device is configured now.
+  @retval Others                Failed to set the device configure.
+
+**/
+EFI_STATUS
+UsbSetConfig (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                ConfigIndex
+  );
+
+/**
+  Usb UsbIo interface to clear the feature. This is should
+  only be used by HUB which is considered a device driver
+  on top of the UsbIo interface.
+
+  @param  UsbIo                 The UsbIo interface.
+  @param  Target                The target of the transfer: endpoint/device.
+  @param  Feature               The feature to clear.
+  @param  Index                 The wIndex parameter.
+
+  @retval EFI_SUCCESS           The device feature is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbIoClearFeature (
+  IN  EFI_USB_IO_PROTOCOL *UsbIo,
+  IN  UINTN               Target,
+  IN  UINT16              Feature,
+  IN  UINT16              Index
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c
new file mode 100644
index 0000000000..79453fed26
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.c
@@ -0,0 +1,1065 @@
+/** @file
+
+    Usb bus enumeration support.
+
+Copyright (c) 2007 - 2014, 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 "UsbBus.h"
+
+/**
+  Return the endpoint descriptor in this interface.
+
+  @param  UsbIf                 The interface to search in.
+  @param  EpAddr                The address of the endpoint to return.
+
+  @return The endpoint descriptor or NULL.
+
+**/
+USB_ENDPOINT_DESC *
+UsbGetEndpointDesc (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                EpAddr
+  )
+{
+  USB_ENDPOINT_DESC       *EpDesc;
+  UINT8                   Index;
+  UINT8                   NumEndpoints;
+  
+  NumEndpoints = UsbIf->IfSetting->Desc.NumEndpoints;
+  
+  for (Index = 0; Index < NumEndpoints; Index++) {
+    EpDesc = UsbIf->IfSetting->Endpoints[Index];
+
+    if (EpDesc->Desc.EndpointAddress == EpAddr) {
+      return EpDesc;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Free the resource used by USB interface.
+
+  @param  UsbIf                 The USB interface to free.
+
+**/
+VOID
+UsbFreeInterface (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  UsbCloseHostProtoByChild (UsbIf->Device->Bus, UsbIf->Handle);
+
+  gBS->UninstallMultipleProtocolInterfaces (
+         UsbIf->Handle,
+         &gEfiDevicePathProtocolGuid,
+         UsbIf->DevicePath,
+         &gEfiUsbIoProtocolGuid,
+         &UsbIf->UsbIo,
+         NULL
+         );
+
+  if (UsbIf->DevicePath != NULL) {
+    FreePool (UsbIf->DevicePath);
+  }
+
+  FreePool (UsbIf);
+}
+
+
+/**
+  Create an interface for the descriptor IfDesc. Each
+  device's configuration can have several interfaces.
+
+  @param  Device                The device has the interface descriptor.
+  @param  IfDesc                The interface descriptor.
+
+  @return The created USB interface for the descriptor, or NULL.
+
+**/
+USB_INTERFACE *
+UsbCreateInterface (
+  IN USB_DEVICE           *Device,
+  IN USB_INTERFACE_DESC   *IfDesc
+  )
+{
+  USB_DEVICE_PATH         UsbNode;
+  USB_INTERFACE           *UsbIf;
+  USB_INTERFACE           *HubIf;
+  EFI_STATUS              Status;
+
+  UsbIf = AllocateZeroPool (sizeof (USB_INTERFACE));
+
+  if (UsbIf == NULL) {
+    return NULL;
+  }
+
+  UsbIf->Signature  = USB_INTERFACE_SIGNATURE;
+  UsbIf->Device     = Device;
+  UsbIf->IfDesc     = IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  UsbIf->IfSetting  = IfDesc->Settings[IfDesc->ActiveIndex];
+
+  CopyMem (
+    &(UsbIf->UsbIo),
+    &mUsbIoProtocol,
+    sizeof (EFI_USB_IO_PROTOCOL)
+    );
+
+  //
+  // Install protocols for USBIO and device path
+  //
+  UsbNode.Header.Type       = MESSAGING_DEVICE_PATH;
+  UsbNode.Header.SubType    = MSG_USB_DP;
+  UsbNode.ParentPortNumber  = Device->ParentPort;
+  UsbNode.InterfaceNumber   = UsbIf->IfSetting->Desc.InterfaceNumber;
+
+  SetDevicePathNodeLength (&UsbNode.Header, sizeof (UsbNode));
+
+  HubIf = Device->ParentIf;
+  ASSERT (HubIf != NULL);
+
+  UsbIf->DevicePath = AppendDevicePathNode (HubIf->DevicePath, &UsbNode.Header);
+
+  if (UsbIf->DevicePath == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to create device path\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &UsbIf->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  UsbIf->DevicePath,
+                  &gEfiUsbIoProtocolGuid,
+                  &UsbIf->UsbIo,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to install UsbIo - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Open USB Host Controller Protocol by Child
+  //
+  Status = UsbOpenHostProtoByChild (Device->Bus, UsbIf->Handle);
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           &UsbIf->Handle,
+           &gEfiDevicePathProtocolGuid,
+           UsbIf->DevicePath,
+           &gEfiUsbIoProtocolGuid,
+           &UsbIf->UsbIo,
+           NULL
+           );
+
+    DEBUG ((EFI_D_ERROR, "UsbCreateInterface: failed to open host for child - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  return UsbIf;
+
+ON_ERROR:
+  if (UsbIf->DevicePath != NULL) {
+    FreePool (UsbIf->DevicePath);
+  }
+
+  FreePool (UsbIf);
+  return NULL;
+}
+
+
+/**
+  Free the resource used by this USB device.
+
+  @param  Device                The USB device to free.
+
+**/
+VOID
+UsbFreeDevice (
+  IN USB_DEVICE           *Device
+  )
+{
+  if (Device->DevDesc != NULL) {
+    UsbFreeDevDesc (Device->DevDesc);
+  }
+
+  gBS->FreePool (Device);
+}
+
+
+/**
+  Create a device which is on the parent's ParentPort port.
+
+  @param  ParentIf              The parent HUB interface.
+  @param  ParentPort            The port on the HUB this device is connected to.
+
+  @return Created USB device, Or NULL.
+
+**/
+USB_DEVICE *
+UsbCreateDevice (
+  IN USB_INTERFACE        *ParentIf,
+  IN UINT8                ParentPort
+  )
+{
+  USB_DEVICE              *Device;
+
+  ASSERT (ParentIf != NULL);
+
+  Device = AllocateZeroPool (sizeof (USB_DEVICE));
+
+  if (Device == NULL) {
+    return NULL;
+  }
+
+  Device->Bus         = ParentIf->Device->Bus;
+  Device->MaxPacket0  = 8;
+  Device->ParentAddr  = ParentIf->Device->Address;
+  Device->ParentIf    = ParentIf;
+  Device->ParentPort  = ParentPort;
+  Device->Tier        = (UINT8)(ParentIf->Device->Tier + 1);
+  return Device;
+}
+
+
+/**
+  Connect the USB interface with its driver. EFI USB bus will
+  create a USB interface for each separate interface descriptor.
+
+  @param  UsbIf             The interface to connect driver to.
+
+  @return EFI_SUCCESS       Interface is managed by some driver.
+  @return Others            Failed to locate a driver for this interface.
+
+**/
+EFI_STATUS
+UsbConnectDriver (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_STATUS              Status;
+  EFI_TPL                 OldTpl;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Hub is maintained by the USB bus driver. Otherwise try to
+  // connect drivers with this interface
+  //
+  if (UsbIsHubInterface (UsbIf)) {
+    DEBUG ((EFI_D_INFO, "UsbConnectDriver: found a hub device\n"));
+    Status = mUsbHubApi.Init (UsbIf);
+
+  } else {
+    //
+    // This function is called in both UsbIoControlTransfer and
+    // the timer callback in hub enumeration. So, at least it is
+    // called at TPL_CALLBACK. Some driver sitting on USB has
+    // twisted TPL used. It should be no problem for us to connect
+    // or disconnect at CALLBACK.
+    //
+    
+    //
+    // Only recursively wanted usb child device
+    //
+    if (UsbBusIsWantedUsbIO (UsbIf->Device->Bus, UsbIf)) {
+      OldTpl            = UsbGetCurrentTpl ();
+      DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL before connect is %d, %p\n", (UINT32)OldTpl, UsbIf->Handle));
+
+      gBS->RestoreTPL (TPL_CALLBACK);
+
+      Status            = gBS->ConnectController (UsbIf->Handle, NULL, NULL, TRUE);
+      UsbIf->IsManaged  = (BOOLEAN)!EFI_ERROR (Status);
+
+      DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL after connect is %d\n", (UINT32)UsbGetCurrentTpl()));
+      ASSERT (UsbGetCurrentTpl () == TPL_CALLBACK);
+
+      gBS->RaiseTPL (OldTpl);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Select an alternate setting for the interface.
+  Each interface can have several mutually exclusive
+  settings. Only one setting is active. It will
+  also reset its endpoints' toggle to zero.
+
+  @param  IfDesc                The interface descriptor to set.
+  @param  Alternate             The alternate setting number to locate.
+
+  @retval EFI_NOT_FOUND         There is no setting with this alternate index.
+  @retval EFI_SUCCESS           The interface is set to Alternate setting.
+
+**/
+EFI_STATUS
+UsbSelectSetting (
+  IN USB_INTERFACE_DESC   *IfDesc,
+  IN UINT8                Alternate
+  )
+{
+  USB_INTERFACE_SETTING   *Setting;
+  UINTN                   Index;
+
+  //
+  // Locate the active alternate setting
+  //
+  Setting = NULL;
+
+  for (Index = 0; Index < IfDesc->NumOfSetting; Index++) {
+    ASSERT (Index < USB_MAX_INTERFACE_SETTING);
+    Setting = IfDesc->Settings[Index];
+
+    if (Setting->Desc.AlternateSetting == Alternate) {
+      break;
+    }
+  }
+
+  if (Index == IfDesc->NumOfSetting) {
+    return EFI_NOT_FOUND;
+  }
+
+  IfDesc->ActiveIndex = Index;
+
+  ASSERT (Setting != NULL);
+  DEBUG ((EFI_D_INFO, "UsbSelectSetting: setting %d selected for interface %d\n",
+              Alternate, Setting->Desc.InterfaceNumber));
+
+  //
+  // Reset the endpoint toggle to zero
+  //
+  for (Index = 0; Index < Setting->Desc.NumEndpoints; Index++) {
+    Setting->Endpoints[Index]->Toggle = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Select a new configuration for the device. Each
+  device may support several configurations.
+
+  @param  Device                The device to select configuration.
+  @param  ConfigValue           The index of the configuration ( != 0).
+
+  @retval EFI_NOT_FOUND         There is no configuration with the index.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+  @retval EFI_SUCCESS           The configuration is selected.
+
+**/
+EFI_STATUS
+UsbSelectConfig (
+  IN USB_DEVICE           *Device,
+  IN UINT8                ConfigValue
+  )
+{
+  USB_DEVICE_DESC         *DevDesc;
+  USB_CONFIG_DESC         *ConfigDesc;
+  USB_INTERFACE_DESC      *IfDesc;
+  USB_INTERFACE           *UsbIf;
+  EFI_STATUS              Status;
+  UINT8                   Index;
+
+  //
+  // Locate the active config, then set the device's pointer
+  //
+  DevDesc     = Device->DevDesc;
+  ConfigDesc  = NULL;
+
+  for (Index = 0; Index < DevDesc->Desc.NumConfigurations; Index++) {
+    ConfigDesc = DevDesc->Configs[Index];
+
+    if (ConfigDesc->Desc.ConfigurationValue == ConfigValue) {
+      break;
+    }
+  }
+
+  if (Index == DevDesc->Desc.NumConfigurations) {
+    return EFI_NOT_FOUND;
+  }
+
+  Device->ActiveConfig = ConfigDesc;
+
+  DEBUG ((EFI_D_INFO, "UsbSelectConfig: config %d selected for device %d\n",
+              ConfigValue, Device->Address));
+
+  //
+  // Create interfaces for each USB interface descriptor.
+  //
+  for (Index = 0; Index < ConfigDesc->Desc.NumInterfaces; Index++) {
+    //
+    // First select the default interface setting, and reset
+    // the endpoint toggles to zero for its endpoints.
+    //
+    IfDesc = ConfigDesc->Interfaces[Index];
+    UsbSelectSetting (IfDesc, IfDesc->Settings[0]->Desc.AlternateSetting);
+
+    //
+    // Create a USB_INTERFACE and install USB_IO and other protocols
+    //
+    UsbIf = UsbCreateInterface (Device, ConfigDesc->Interfaces[Index]);
+
+    if (UsbIf == NULL) {
+      Device->NumOfInterface = Index;
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ASSERT (Index < USB_MAX_INTERFACE);
+    Device->Interfaces[Index] = UsbIf;
+
+    //
+    // Connect the device to drivers, if it failed, ignore
+    // the error. Don't let the unsupported interfaces to block
+    // the supported interfaces.
+    //
+    Status = UsbConnectDriver (UsbIf);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbSelectConfig: failed to connect driver %r, ignored\n", Status));
+    }
+  }
+
+  Device->NumOfInterface = Index;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Disconnect the USB interface with its driver.
+
+  @param  UsbIf                 The interface to disconnect driver from.
+
+**/
+EFI_STATUS
+UsbDisconnectDriver (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  //
+  // Release the hub if it's a hub controller, otherwise
+  // disconnect the driver if it is managed by other drivers.
+  //
+  Status = EFI_SUCCESS;
+  if (UsbIf->IsHub) {
+    Status = UsbIf->HubApi->Release (UsbIf);
+
+  } else if (UsbIf->IsManaged) {
+    //
+    // This function is called in both UsbIoControlTransfer and
+    // the timer callback in hub enumeration. So, at least it is
+    // called at TPL_CALLBACK. Some driver sitting on USB has
+    // twisted TPL used. It should be no problem for us to connect
+    // or disconnect at CALLBACK.
+    //
+    OldTpl           = UsbGetCurrentTpl ();
+    DEBUG ((EFI_D_INFO, "UsbDisconnectDriver: old TPL is %d, %p\n", (UINT32)OldTpl, UsbIf->Handle));
+
+    gBS->RestoreTPL (TPL_CALLBACK);
+
+    Status = gBS->DisconnectController (UsbIf->Handle, NULL, NULL);
+    if (!EFI_ERROR (Status)) {
+      UsbIf->IsManaged = FALSE;
+    }
+    
+    DEBUG (( EFI_D_INFO, "UsbDisconnectDriver: TPL after disconnect is %d, %d\n", (UINT32)UsbGetCurrentTpl(), Status));
+    ASSERT (UsbGetCurrentTpl () == TPL_CALLBACK);
+
+    gBS->RaiseTPL (OldTpl);
+  }
+  
+  return Status;
+}
+
+
+/**
+  Remove the current device configuration.
+
+  @param  Device                The USB device to remove configuration from.
+
+**/
+EFI_STATUS
+UsbRemoveConfig (
+  IN USB_DEVICE           *Device
+  )
+{
+  USB_INTERFACE           *UsbIf;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+  EFI_STATUS              ReturnStatus;
+
+  //
+  // Remove each interface of the device
+  //
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 0; Index < Device->NumOfInterface; Index++) {    
+    ASSERT (Index < USB_MAX_INTERFACE);
+    UsbIf = Device->Interfaces[Index];
+
+    if (UsbIf == NULL) {
+      continue;
+    }
+
+    Status = UsbDisconnectDriver (UsbIf);
+    if (!EFI_ERROR (Status)) {
+      UsbFreeInterface (UsbIf);
+      Device->Interfaces[Index] = NULL;
+    } else {
+      ReturnStatus = Status;
+    }
+  }
+
+  Device->ActiveConfig    = NULL;
+  return ReturnStatus;
+}
+
+
+/**
+  Remove the device and all its children from the bus.
+
+  @param  Device                The device to remove.
+
+  @retval EFI_SUCCESS           The device is removed.
+
+**/
+EFI_STATUS
+UsbRemoveDevice (
+  IN USB_DEVICE           *Device
+  )
+{
+  USB_BUS                 *Bus;
+  USB_DEVICE              *Child;
+  EFI_STATUS              Status;
+  EFI_STATUS              ReturnStatus;
+  UINTN                   Index;
+
+  Bus = Device->Bus;
+
+  //
+  // Remove all the devices on its downstream ports. Search from devices[1].
+  // Devices[0] is the root hub.
+  //
+  ReturnStatus = EFI_SUCCESS;
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    Child = Bus->Devices[Index];
+
+    if ((Child == NULL) || (Child->ParentAddr != Device->Address)) {
+      continue;
+    }
+
+    Status = UsbRemoveDevice (Child);
+
+    if (!EFI_ERROR (Status)) {
+      Bus->Devices[Index] = NULL;
+    } else {
+      Bus->Devices[Index]->DisconnectFail = TRUE;
+      ReturnStatus = Status;
+      DEBUG ((EFI_D_INFO, "UsbRemoveDevice: failed to remove child %p at parent %p\n", Child, Device));
+    }
+  }
+
+  if (EFI_ERROR (ReturnStatus)) {
+    return ReturnStatus;
+  }
+
+  Status = UsbRemoveConfig (Device);
+
+  if (!EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_INFO, "UsbRemoveDevice: device %d removed\n", Device->Address));
+
+    ASSERT (Device->Address < Bus->MaxDevices);
+    Bus->Devices[Device->Address] = NULL;
+    UsbFreeDevice (Device);
+  } else {
+    Bus->Devices[Device->Address]->DisconnectFail = TRUE;
+  }
+  return Status;
+}
+
+
+/**
+  Find the child device on the hub's port.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub this child is connected to.
+
+  @return The device on the hub's port, or NULL if there is none.
+
+**/
+USB_DEVICE *
+UsbFindChild (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_DEVICE              *Device;
+  USB_BUS                 *Bus;
+  UINTN                   Index;
+
+  Bus = HubIf->Device->Bus;
+
+  //
+  // Start checking from device 1, device 0 is the root hub
+  //
+  for (Index = 1; Index < Bus->MaxDevices; Index++) {
+    Device = Bus->Devices[Index];
+
+    if ((Device != NULL) && (Device->ParentAddr == HubIf->Device->Address) &&
+        (Device->ParentPort == Port)) {
+
+      return Device;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Enumerate and configure the new device on the port of this HUB interface.
+
+  @param  HubIf                 The HUB that has the device connected.
+  @param  Port                  The port index of the hub (started with zero).
+
+  @retval EFI_SUCCESS           The device is enumerated (added or removed).
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the device.
+  @retval Others                Failed to enumerate the device.
+
+**/
+EFI_STATUS
+UsbEnumerateNewDev (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_BUS                 *Bus;
+  USB_HUB_API             *HubApi;
+  USB_DEVICE              *Child;
+  USB_DEVICE              *Parent;
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Address;
+  UINT8                   Config;
+  EFI_STATUS              Status;
+
+  Parent  = HubIf->Device;
+  Bus     = Parent->Bus;
+  HubApi  = HubIf->HubApi;  
+  Address = Bus->MaxDevices;
+
+  gBS->Stall (USB_WAIT_PORT_STABLE_STALL);
+  
+  //
+  // Hub resets the device for at least 10 milliseconds.
+  // Host learns device speed. If device is of low/full speed
+  // and the hub is a EHCI root hub, ResetPort will release
+  // the device to its companion UHCI and return an error.
+  //
+  Status = HubApi->ResetPort (HubIf, Port);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to reset port %d - %r\n", Port, Status));
+
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: hub port %d is reset\n", Port));
+
+  Child = UsbCreateDevice (HubIf, Port);
+
+  if (Child == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // OK, now identify the device speed. After reset, hub
+  // fully knows the actual device speed.
+  //
+  Status = HubApi->GetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to get speed of port %d\n", Port));
+    goto ON_ERROR;
+  }
+
+  if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_CONNECTION)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: No device present at port %d\n", Port));
+    goto ON_ERROR;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_SUPER_SPEED)){
+    Child->Speed      = EFI_USB_SPEED_SUPER;
+    Child->MaxPacket0 = 512;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_HIGH_SPEED)) {
+    Child->Speed      = EFI_USB_SPEED_HIGH;
+    Child->MaxPacket0 = 64;
+  } else if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_LOW_SPEED)) {
+    Child->Speed      = EFI_USB_SPEED_LOW;
+    Child->MaxPacket0 = 8;
+  } else {
+    Child->Speed      = EFI_USB_SPEED_FULL;
+    Child->MaxPacket0 = 8;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device is of %d speed\n", Child->Speed));
+
+  if (((Child->Speed == EFI_USB_SPEED_LOW) || (Child->Speed == EFI_USB_SPEED_FULL)) &&
+      (Parent->Speed == EFI_USB_SPEED_HIGH)) {
+    //
+    // If the child is a low or full speed device, it is necessary to
+    // set the transaction translator. Port TT is 1-based.
+    // This is quite simple:
+    //  1. if parent is of high speed, then parent is our translator
+    //  2. otherwise use parent's translator.
+    //
+    Child->Translator.TranslatorHubAddress  = Parent->Address;
+    Child->Translator.TranslatorPortNumber  = (UINT8) (Port + 1);
+  } else {
+    Child->Translator = Parent->Translator;
+  }
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device uses translator (%d, %d)\n",
+           Child->Translator.TranslatorHubAddress,
+           Child->Translator.TranslatorPortNumber));
+
+  //
+  // After port is reset, hub establishes a signal path between
+  // the device and host (DEFALUT state). Device's registers are
+  // reset, use default address 0 (host enumerates one device at
+  // a time) , and ready to respond to control transfer at EP 0.
+  //
+
+  //
+  // Host assigns an address to the device. Device completes the
+  // status stage with default address, then switches to new address.
+  // ADDRESS state. Address zero is reserved for root hub.
+  //
+  ASSERT (Bus->MaxDevices <= 256);
+  for (Address = 1; Address < Bus->MaxDevices; Address++) {
+    if (Bus->Devices[Address] == NULL) {
+      break;
+    }
+  }
+
+  if (Address >= Bus->MaxDevices) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: address pool is full for port %d\n", Port));
+
+    Status = EFI_ACCESS_DENIED;
+    goto ON_ERROR;
+  }
+
+  Status                = UsbSetAddress (Child, (UINT8)Address);
+  Child->Address        = (UINT8)Address;
+  Bus->Devices[Address] = Child;
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to set device address - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  gBS->Stall (USB_SET_DEVICE_ADDRESS_STALL);
+
+  DEBUG ((EFI_D_INFO, "UsbEnumerateNewDev: device is now ADDRESSED at %d\n", Address));
+
+  //
+  // Host sends a Get_Descriptor request to learn the max packet
+  // size of default pipe (only part of the device's descriptor).
+  //
+  Status = UsbGetMaxPacketSize0 (Child);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to get max packet for EP 0 - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: max packet size for EP 0 is %d\n", Child->MaxPacket0));
+
+  //
+  // Host learns about the device's abilities by requesting device's
+  // entire descriptions.
+  //
+  Status = UsbBuildDescTable (Child);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to build descriptor table - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Select a default configuration: UEFI must set the configuration
+  // before the driver can connect to the device.
+  //
+  Config = Child->DevDesc->Configs[0]->Desc.ConfigurationValue;
+  Status = UsbSetConfig (Child, Config);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to set configure %d - %r\n", Config, Status));
+    goto ON_ERROR;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumerateNewDev: device %d is now in CONFIGED state\n", Address));
+
+  //
+  // Host assigns and loads a device driver.
+  //
+  Status = UsbSelectConfig (Child, Config);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumerateNewDev: failed to create interfaces - %r\n", Status));
+    goto ON_ERROR;
+  }
+
+  //
+  // Report Status Code to indicate USB device has been detected by hotplug
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_IO_BUS_USB | EFI_IOB_PC_HOTPLUG),
+    Bus->DevicePath
+    );
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  //
+  // If reach here, it means the enumeration process on a given port is interrupted due to error.
+  // The s/w resources, including the assigned address(Address) and the allocated usb device data
+  // structure(Bus->Devices[Address]), will NOT be freed here. These resources will be freed when
+  // the device is unplugged from the port or DriverBindingStop() is invoked.
+  //
+  // This way is used to co-work with the lower layer EDKII UHCI/EHCI/XHCI host controller driver.
+  // It's mainly because to keep UEFI spec unchanged EDKII XHCI driver have to maintain a state machine
+  // to keep track of the mapping between actual address and request address. If the request address
+  // (Address) is freed here, the Address value will be used by next enumerated device. Then EDKII XHCI
+  // host controller driver will have wrong information, which will cause further transaction error.
+  //
+  // EDKII UHCI/EHCI doesn't get impacted as it's make sense to reserve s/w resource till it gets unplugged.
+  //
+  return Status;
+}
+
+
+/**
+  Process the events on the port.
+
+  @param  HubIf                 The HUB that has the device connected.
+  @param  Port                  The port index of the hub (started with zero).
+
+  @retval EFI_SUCCESS           The device is enumerated (added or removed).
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the device.
+  @retval Others                Failed to enumerate the device.
+
+**/
+EFI_STATUS
+UsbEnumeratePort (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  USB_HUB_API             *HubApi;
+  USB_DEVICE              *Child;
+  EFI_USB_PORT_STATUS     PortState;
+  EFI_STATUS              Status;
+
+  Child   = NULL;
+  HubApi  = HubIf->HubApi;
+
+  //
+  // Host learns of the new device by polling the hub for port changes.
+  //
+  Status = HubApi->GetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbEnumeratePort: failed to get state of port %d\n", Port));
+    return Status;
+  }
+
+  //
+  // Only handle connection/enable/overcurrent/reset change.
+  // Usb super speed hub may report other changes, such as warm reset change. Ignore them.
+  //
+  if ((PortState.PortChangeStatus & (USB_PORT_STAT_C_CONNECTION | USB_PORT_STAT_C_ENABLE | USB_PORT_STAT_C_OVERCURRENT | USB_PORT_STAT_C_RESET)) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbEnumeratePort: port %d state - %02x, change - %02x on %p\n",
+              Port, PortState.PortStatus, PortState.PortChangeStatus, HubIf));
+
+  //
+  // This driver only process two kinds of events now: over current and
+  // connect/disconnect. Other three events are: ENABLE, SUSPEND, RESET.
+  // ENABLE/RESET is used to reset port. SUSPEND isn't supported.
+  //
+  
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_OVERCURRENT)) {     
+
+    if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_OVERCURRENT)) {
+      //
+      // Case1:
+      //   Both OverCurrent and OverCurrentChange set, means over current occurs, 
+      //   which probably is caused by short circuit. It has to wait system hardware
+      //   to perform recovery.
+      //
+      DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: Critical Over Current\n", Port));
+      return EFI_DEVICE_ERROR;
+      
+    } 
+    //
+    // Case2:
+    //   Only OverCurrentChange set, means system has been recoveried from 
+    //   over current. As a result, all ports are nearly power-off, so
+    //   it's necessary to detach and enumerate all ports again. 
+    //
+    DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: 2.0 device Recovery Over Current\n", Port)); 
+  }
+
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_ENABLE)) {  
+    //
+    // Case3:
+    //   1.1 roothub port reg doesn't reflect over-current state, while its counterpart
+    //   on 2.0 roothub does. When over-current has influence on 1.1 device, the port 
+    //   would be disabled, so it's also necessary to detach and enumerate again.
+    //
+    DEBUG (( EFI_D_ERROR, "UsbEnumeratePort: 1.1 device Recovery Over Current\n", Port));
+  }
+  
+  if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_CONNECTION)) {
+    //
+    // Case4:
+    //   Device connected or disconnected normally. 
+    //
+    DEBUG ((EFI_D_INFO, "UsbEnumeratePort: Device Connect/Disconnect Normally\n", Port));
+  }
+
+  // 
+  // Following as the above cases, it's safety to remove and create again.
+  //
+  Child = UsbFindChild (HubIf, Port);
+  
+  if (Child != NULL) {
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: device at port %d removed from root hub %p\n", Port, HubIf));
+    UsbRemoveDevice (Child);
+  }
+  
+  if (USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_CONNECTION)) {
+    //
+    // Now, new device connected, enumerate and configure the device 
+    //
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: new device connected at port %d\n", Port));
+    Status = UsbEnumerateNewDev (HubIf, Port);
+  
+  } else {
+    DEBUG (( EFI_D_INFO, "UsbEnumeratePort: device disconnected event on port %d\n", Port));
+  }
+  
+  HubApi->ClearPortChange (HubIf, Port);
+  return Status;
+}
+
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+**/
+VOID
+EFIAPI
+UsbHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_INTERFACE           *HubIf;
+  UINT8                   Byte;
+  UINT8                   Bit;
+  UINT8                   Index;
+  USB_DEVICE              *Child;
+  
+  ASSERT (Context != NULL);
+
+  HubIf = (USB_INTERFACE *) Context;
+
+  for (Index = 0; Index < HubIf->NumOfPort; Index++) {
+    Child = UsbFindChild (HubIf, Index);
+    if ((Child != NULL) && (Child->DisconnectFail == TRUE)) {
+      DEBUG (( EFI_D_INFO, "UsbEnumeratePort: The device disconnect fails at port %d from hub %p, try again\n", Index, HubIf));
+      UsbRemoveDevice (Child);
+    }
+  }
+
+  if (HubIf->ChangeMap == NULL) {
+    return ;
+  }
+
+  //
+  // HUB starts its port index with 1.
+  //
+  Byte  = 0;
+  Bit   = 1;
+
+  for (Index = 0; Index < HubIf->NumOfPort; Index++) {
+    if (USB_BIT_IS_SET (HubIf->ChangeMap[Byte], USB_BIT (Bit))) {
+      UsbEnumeratePort (HubIf, Index);
+    }
+
+    USB_NEXT_BIT (Byte, Bit);
+  }
+
+  UsbHubAckHubStatus (HubIf->Device);
+
+  gBS->FreePool (HubIf->ChangeMap);
+  HubIf->ChangeMap = NULL;
+  return ;
+}
+
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+**/
+VOID
+EFIAPI
+UsbRootHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  USB_INTERFACE           *RootHub;
+  UINT8                   Index;
+  USB_DEVICE              *Child;
+
+  RootHub = (USB_INTERFACE *) Context;
+
+  for (Index = 0; Index < RootHub->NumOfPort; Index++) {
+    Child = UsbFindChild (RootHub, Index);
+    if ((Child != NULL) && (Child->DisconnectFail == TRUE)) {
+      DEBUG (( EFI_D_INFO, "UsbEnumeratePort: The device disconnect fails at port %d from root hub %p, try again\n", Index, RootHub));
+      UsbRemoveDevice (Child);
+    }
+    
+    UsbEnumeratePort (RootHub, Index);
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h
new file mode 100644
index 0000000000..ef7d440917
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbEnumer.h
@@ -0,0 +1,203 @@
+/** @file
+
+    USB bus enumeration interface.
+
+Copyright (c) 2007, 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.
+
+**/
+
+#ifndef _USB_ENUMERATION_H_
+#define _USB_ENUMERATION_H_
+
+//
+// Advance the byte and bit to the next bit, adjust byte accordingly.
+//
+#define USB_NEXT_BIT(Byte, Bit)   \
+          do {                \
+            (Bit)++;          \
+            if ((Bit) > 7) {  \
+              (Byte)++;       \
+              (Bit) = 0;      \
+            }                 \
+          } while (0)
+
+
+//
+// Common interface used by usb bus enumeration process.
+// This interface is defined to mask the difference between
+// the root hub and normal hub. So, bus enumeration code
+// can be shared by both root hub and normal hub
+//
+typedef
+EFI_STATUS
+(*USB_HUB_INIT) (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+//
+// Get the port status. This function is required to
+// ACK the port change bits although it will return
+// the port changes in PortState. Bus enumeration code
+// doesn't need to ACK the port change bits.
+//
+typedef
+EFI_STATUS
+(*USB_HUB_GET_PORT_STATUS) (
+  IN  USB_INTERFACE       *UsbIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  );
+
+typedef
+VOID
+(*USB_HUB_CLEAR_PORT_CHANGE) (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_SET_PORT_FEATURE) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_CLEAR_PORT_FEATURE) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_RESET_PORT) (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                Port
+  );
+
+typedef
+EFI_STATUS
+(*USB_HUB_RELEASE) (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+/**
+  Return the endpoint descriptor in this interface.
+
+  @param  UsbIf                 The interface to search in.
+  @param  EpAddr                The address of the endpoint to return.
+
+  @return The endpoint descriptor or NULL.
+
+**/
+USB_ENDPOINT_DESC*
+UsbGetEndpointDesc (
+  IN USB_INTERFACE        *UsbIf,
+  IN UINT8                EpAddr
+  );
+
+/**
+  Select an alternate setting for the interface.
+  Each interface can have several mutually exclusive
+  settings. Only one setting is active. It will
+  also reset its endpoints' toggle to zero.
+
+  @param  IfDesc                The interface descriptor to set.
+  @param  Alternate             The alternate setting number to locate.
+
+  @retval EFI_NOT_FOUND         There is no setting with this alternate index.
+  @retval EFI_SUCCESS           The interface is set to Alternate setting.
+
+**/
+EFI_STATUS
+UsbSelectSetting (
+  IN USB_INTERFACE_DESC   *IfDesc,
+  IN UINT8                Alternate
+  );
+
+/**
+  Select a new configuration for the device. Each
+  device may support several configurations.
+
+  @param  Device                The device to select configuration.
+  @param  ConfigIndex           The index of the configuration ( != 0).
+
+  @retval EFI_NOT_FOUND         There is no configuration with the index.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+  @retval EFI_SUCCESS           The configuration is selected.
+
+**/
+EFI_STATUS
+UsbSelectConfig (
+  IN USB_DEVICE           *Device,
+  IN UINT8                ConfigIndex
+  );
+
+/**
+  Remove the current device configuration.
+
+  @param  Device                The USB device to remove configuration from.
+
+  @return None.
+
+**/
+EFI_STATUS
+UsbRemoveConfig (
+  IN USB_DEVICE           *Device
+  );
+
+/**
+  Remove the device and all its children from the bus.
+
+  @param  Device                The device to remove.
+
+  @retval EFI_SUCCESS           The device is removed.
+
+**/
+EFI_STATUS
+UsbRemoveDevice (
+  IN USB_DEVICE           *Device
+  );
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UsbHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+
+/**
+  Enumerate all the changed hub ports.
+
+  @param  Event                 The event that is triggered.
+  @param  Context               The context to the event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+UsbRootHubEnumeration (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c
new file mode 100644
index 0000000000..e3752d1f83
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.c
@@ -0,0 +1,1411 @@
+/** @file
+
+    Unified interface for RootHub and Hub.
+
+Copyright (c) 2007 - 2012, 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 "UsbBus.h"
+
+//
+// Array that maps the change bit to feature value which is
+// used to clear these change bit. USB HUB API will clear
+// these change bit automatically. For non-root hub, these
+// bits determine whether hub will report the port in changed
+// bit maps.
+//
+USB_CHANGE_FEATURE_MAP  mHubFeatureMap[] = {
+  {USB_PORT_STAT_C_CONNECTION,  EfiUsbPortConnectChange},
+  {USB_PORT_STAT_C_ENABLE,      EfiUsbPortEnableChange},
+  {USB_PORT_STAT_C_SUSPEND,     EfiUsbPortSuspendChange},
+  {USB_PORT_STAT_C_OVERCURRENT, EfiUsbPortOverCurrentChange},
+  {USB_PORT_STAT_C_RESET,       EfiUsbPortResetChange}
+};
+
+USB_CHANGE_FEATURE_MAP  mRootHubFeatureMap[] = {
+  {USB_PORT_STAT_C_CONNECTION,  EfiUsbPortConnectChange},
+  {USB_PORT_STAT_C_ENABLE,      EfiUsbPortEnableChange},
+  {USB_PORT_STAT_C_SUSPEND,     EfiUsbPortSuspendChange},
+  {USB_PORT_STAT_C_OVERCURRENT, EfiUsbPortOverCurrentChange},
+  {USB_PORT_STAT_C_RESET,       EfiUsbPortResetChange},
+};
+
+//
+// USB hub class specific requests. Although USB hub
+// is related to an interface, these requests are sent
+// to the control endpoint of the device.
+//
+/**
+  USB hub control transfer to set the hub depth.
+
+  @param  HubDev                The device of the hub.
+  @param  Depth                 The depth to set.
+
+  @retval EFI_SUCCESS           Depth of the hub is set.
+  @retval Others                Failed to set the depth.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetHubDepth (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT16              Depth
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_SET_DEPTH,
+             Depth,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+/**
+  USB hub control transfer to clear the hub feature.
+
+  @param  HubDev                The device of the hub.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           Feature of the hub is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearHubFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT16               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_CLEAR_FEATURE,
+             Feature,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Clear the feature of the device's port.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port to clear feature.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The feature of the port is cleared.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearPortFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT16               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_CLEAR_FEATURE,
+             Feature,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Clear the transaction translate buffer if full/low
+  speed control/bulk transfer failed and the transfer
+  uses this hub as translator.Remember to clear the TT
+  buffer of transaction translator, not that of the
+  parent.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+  @param  DevAddr               Address of the failed transaction.
+  @param  EpNum                 The endpoint number of the failed transaction.
+  @param  EpType                The type of failed transaction.
+
+  @retval EFI_SUCCESS           The TT buffer is cleared.
+  @retval Others                Failed to clear the TT buffer.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearTTBuffer (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT16               DevAddr,
+  IN UINT16               EpNum,
+  IN UINT16               EpType
+  )
+{
+  EFI_STATUS              Status;
+  UINT16                  Value;
+
+  //
+  // Check USB2.0 spec page 424 for wValue's encoding
+  //
+  Value = (UINT16) ((EpNum & 0x0F) | (DevAddr << 4) |
+          ((EpType & 0x03) << 11) | ((EpNum & 0x80) << 15));
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_CLEAR_TT,
+             Value,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+/**
+  Usb hub control transfer to get the super speed hub descriptor.
+
+  @param  HubDev                The hub device.
+  @param  Buf                   The buffer to hold the descriptor.
+
+  @retval EFI_SUCCESS           The hub descriptor is retrieved.
+  @retval Others                Failed to retrieve the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetSuperSpeedHubDesc (
+  IN  USB_DEVICE          *HubDev,
+  OUT VOID                *Buf
+  )
+{
+  EFI_STATUS              Status;
+  
+  Status = EFI_INVALID_PARAMETER;
+  
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_DESC,
+             (UINT16) (USB_DESC_TYPE_HUB_SUPER_SPEED << 8),
+             0,
+             Buf,
+             32
+             );
+
+  return Status;
+}
+
+/**
+  Usb hub control transfer to get the hub descriptor.
+
+  @param  HubDev                The hub device.
+  @param  Buf                   The buffer to hold the descriptor.
+  @param  Len                   The length to retrieve.
+
+  @retval EFI_SUCCESS           The hub descriptor is retrieved.
+  @retval Others                Failed to retrieve the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetHubDesc (
+  IN  USB_DEVICE          *HubDev,
+  OUT VOID                *Buf,
+  IN  UINTN               Len
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_DESC,
+             (UINT16) (USB_DESC_TYPE_HUB << 8),
+             0,
+             Buf,
+             Len
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to get the hub status.
+
+  @param  HubDev                The hub device.
+  @param  State                 The variable to return the status.
+
+  @retval EFI_SUCCESS           The hub status is returned in State.
+  @retval Others                Failed to get the hub status.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetHubStatus (
+  IN  USB_DEVICE          *HubDev,
+  OUT UINT32              *State
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_GET_STATUS,
+             0,
+             0,
+             State,
+             4
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to get the port status.
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+  @param  State                 Variable to return the hub port state.
+
+  @retval EFI_SUCCESS           The port state is returned in State.
+  @retval Others                Failed to retrieve the port state.
+
+**/
+EFI_STATUS
+UsbHubCtrlGetPortStatus (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Port,
+  OUT VOID                *State
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  // No need to convert the hub bit to UEFI definition, they
+  // are the same
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbDataIn,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_GET_STATUS,
+             0,
+             (UINT16) (Port + 1),
+             State,
+             4
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to reset the TT (Transaction Transaltor).
+
+  @param  HubDev                The hub device.
+  @param  Port                  The port of the hub.
+
+  @retval EFI_SUCCESS           The TT of the hub is reset.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbHubCtrlResetTT (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Port
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_RESET_TT,
+             0,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to set the hub feature.
+
+  @param  HubDev                The hub device.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCESS            The feature is set for the hub.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetHubFeature (
+  IN  USB_DEVICE          *HubDev,
+  IN  UINT8               Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_HUB,
+             USB_HUB_REQ_SET_FEATURE,
+             Feature,
+             0,
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Usb hub control transfer to set the port feature.
+
+  @param  HubDev                The Usb hub device.
+  @param  Port                  The Usb port to set feature for.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCCESS           The feature is set for the port.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbHubCtrlSetPortFeature (
+  IN USB_DEVICE           *HubDev,
+  IN UINT8                Port,
+  IN UINT8                Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // In USB bus, all the port index starts from 0. But HUB
+  // indexes its port from 1. So, port number is added one.
+  //
+  Status = UsbCtrlRequest (
+             HubDev,
+             EfiUsbNoData,
+             USB_REQ_TYPE_CLASS,
+             USB_HUB_TARGET_PORT,
+             USB_HUB_REQ_SET_FEATURE,
+             Feature,
+             (UINT16) (Port + 1),
+             NULL,
+             0
+             );
+
+  return Status;
+}
+
+
+/**
+  Read the whole usb hub descriptor. It is necessary
+  to do it in two steps because hub descriptor is of
+  variable length.
+
+  @param  HubDev                The hub device.
+  @param  HubDesc               The variable to return the descriptor.
+
+  @retval EFI_SUCCESS           The hub descriptor is read.
+  @retval Others                Failed to read the hub descriptor.
+
+**/
+EFI_STATUS
+UsbHubReadDesc (
+  IN  USB_DEVICE              *HubDev,
+  OUT EFI_USB_HUB_DESCRIPTOR  *HubDesc
+  )
+{
+  EFI_STATUS              Status;
+
+  if (HubDev->Speed == EFI_USB_SPEED_SUPER) {
+    //
+    // Get the super speed hub descriptor
+    //
+    Status = UsbHubCtrlGetSuperSpeedHubDesc (HubDev, HubDesc);
+  } else {
+
+    //
+    // First get the hub descriptor length
+    //
+    Status = UsbHubCtrlGetHubDesc (HubDev, HubDesc, 2);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the whole hub descriptor
+    //
+    Status = UsbHubCtrlGetHubDesc (HubDev, HubDesc, HubDesc->Length);
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Ack the hub change bits. If these bits are not ACKed, Hub will
+  always return changed bit map from its interrupt endpoint.
+
+  @param  HubDev                The hub device.
+
+  @retval EFI_SUCCESS           The hub change status is ACKed.
+  @retval Others                Failed to ACK the hub status.
+
+**/
+EFI_STATUS
+UsbHubAckHubStatus (
+  IN  USB_DEVICE         *HubDev
+  )
+{
+  EFI_USB_PORT_STATUS     HubState;
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlGetHubStatus (HubDev, (UINT32 *) &HubState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (USB_BIT_IS_SET (HubState.PortChangeStatus, USB_HUB_STAT_C_LOCAL_POWER)) {
+    UsbHubCtrlClearHubFeature (HubDev, USB_HUB_C_HUB_LOCAL_POWER);
+  }
+
+  if (USB_BIT_IS_SET (HubState.PortChangeStatus, USB_HUB_STAT_C_OVER_CURRENT)) {
+    UsbHubCtrlClearHubFeature (HubDev, USB_HUB_C_HUB_OVER_CURRENT);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Test whether the interface is a hub interface.
+
+  @param  UsbIf                 The interface to test.
+
+  @retval TRUE                  The interface is a hub interface.
+  @retval FALSE                 The interface isn't a hub interface.
+
+**/
+BOOLEAN
+UsbIsHubInterface (
+  IN USB_INTERFACE        *UsbIf
+  )
+{
+  EFI_USB_INTERFACE_DESCRIPTOR  *Setting;
+
+  //
+  // If the hub is a high-speed hub with multiple TT,
+  // the hub will has a default setting of single TT.
+  //
+  Setting = &UsbIf->IfSetting->Desc;
+
+  if ((Setting->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (Setting->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  The callback function to the USB hub status change
+  interrupt endpoint. It is called periodically by
+  the underlying host controller.
+
+  @param  Data                  The data read.
+  @param  DataLength            The length of the data read.
+  @param  Context               The context.
+  @param  Result                The result of the last interrupt transfer.
+
+  @retval EFI_SUCCESS           The process is OK.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbOnHubInterrupt (
+  IN  VOID                *Data,
+  IN  UINTN               DataLength,
+  IN  VOID                *Context,
+  IN  UINT32              Result
+  )
+{
+  USB_INTERFACE               *HubIf;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EpDesc;
+  EFI_STATUS                  Status;
+
+  HubIf   = (USB_INTERFACE *) Context;
+  UsbIo   = &(HubIf->UsbIo);
+  EpDesc  = &(HubIf->HubEp->Desc);
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // If endpoint is stalled, clear the stall. Use UsbIo to access
+    // the control transfer so internal status are maintained.
+    //
+    if (USB_BIT_IS_SET (Result, EFI_USB_ERR_STALL)) {
+      UsbIoClearFeature (
+        UsbIo,
+        USB_TARGET_ENDPOINT,
+        USB_FEATURE_ENDPOINT_HALT,
+        EpDesc->EndpointAddress
+        );
+    }
+
+    //
+    // Delete and submit a new async interrupt
+    //
+    Status = UsbIo->UsbAsyncInterruptTransfer (
+                      UsbIo,
+                      EpDesc->EndpointAddress,
+                      FALSE,
+                      0,
+                      0,
+                      NULL,
+                      NULL
+                      );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbOnHubInterrupt: failed to remove async transfer - %r\n", Status));
+      return Status;
+    }
+
+    Status = UsbIo->UsbAsyncInterruptTransfer (
+                      UsbIo,
+                      EpDesc->EndpointAddress,
+                      TRUE,
+                      USB_HUB_POLL_INTERVAL,
+                      HubIf->NumOfPort / 8 + 1,
+                      UsbOnHubInterrupt,
+                      HubIf
+                      );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG (( EFI_D_ERROR, "UsbOnHubInterrupt: failed to submit new async transfer - %r\n", Status));
+    }
+
+    return Status;
+  }
+
+  if ((DataLength == 0) || (Data == NULL)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // OK, actually something is changed, save the change map
+  // then signal the HUB to do enumeration. This is a good
+  // practise since UsbOnHubInterrupt is called in the context
+  // of host contrller's AsyncInterrupt monitor.
+  //
+  HubIf->ChangeMap = AllocateZeroPool (DataLength);
+
+  if (HubIf->ChangeMap == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CopyMem (HubIf->ChangeMap, Data, DataLength);
+  gBS->SignalEvent (HubIf->HubNotify);
+
+  return EFI_SUCCESS;
+}
+
+
+
+
+/**
+  Initialize the device for a non-root hub.
+
+  @param  HubIf                 The USB hub interface.
+
+  @retval EFI_SUCCESS           The hub is initialized.
+  @retval EFI_DEVICE_ERROR      Failed to initialize the hub.
+
+**/
+EFI_STATUS
+UsbHubInit (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_USB_HUB_DESCRIPTOR  HubDesc;
+  USB_ENDPOINT_DESC       *EpDesc;
+  USB_INTERFACE_SETTING   *Setting;
+  EFI_USB_IO_PROTOCOL     *UsbIo;
+  USB_DEVICE              *HubDev;
+  EFI_STATUS              Status;
+  UINT8                   Index;
+  UINT8                   NumEndpoints;
+  UINT16                  Depth;
+
+  //
+  // Locate the interrupt endpoint for port change map
+  //
+  HubIf->IsHub  = FALSE;
+  Setting       = HubIf->IfSetting;
+  HubDev        = HubIf->Device;
+  EpDesc        = NULL;
+  NumEndpoints  = Setting->Desc.NumEndpoints;
+
+  for (Index = 0; Index < NumEndpoints; Index++) {
+    ASSERT ((Setting->Endpoints != NULL) && (Setting->Endpoints[Index] != NULL));
+
+    EpDesc = Setting->Endpoints[Index];
+
+    if (USB_BIT_IS_SET (EpDesc->Desc.EndpointAddress, USB_ENDPOINT_DIR_IN) &&
+       (USB_ENDPOINT_TYPE (&EpDesc->Desc) == USB_ENDPOINT_INTERRUPT)) {
+      break;
+    }
+  }
+
+  if (Index == NumEndpoints) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: no interrupt endpoint found for hub %d\n", HubDev->Address));
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = UsbHubReadDesc (HubDev, &HubDesc);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to read HUB descriptor %r\n", Status));
+    return Status;
+  }
+
+  HubIf->NumOfPort = HubDesc.NumPorts;
+
+  DEBUG (( EFI_D_INFO, "UsbHubInit: hub %d has %d ports\n", HubDev->Address,HubIf->NumOfPort));
+
+  //
+  // OK, set IsHub to TRUE. Now usb bus can handle this device
+  // as a working HUB. If failed eariler, bus driver will not
+  // recognize it as a hub. Other parts of the bus should be able
+  // to work.
+  //
+  HubIf->IsHub  = TRUE;
+  HubIf->HubApi = &mUsbHubApi;
+  HubIf->HubEp  = EpDesc;
+
+  if (HubIf->Device->Speed == EFI_USB_SPEED_SUPER) {
+    Depth = (UINT16)(HubIf->Device->Tier - 1);
+    DEBUG ((EFI_D_INFO, "UsbHubInit: Set Hub Depth as 0x%x\n", Depth));
+    UsbHubCtrlSetHubDepth (HubIf->Device, Depth);
+    
+    for (Index = 0; Index < HubDesc.NumPorts; Index++) {
+      UsbHubCtrlSetPortFeature (HubIf->Device, Index, USB_HUB_PORT_REMOTE_WAKE_MASK);
+    }    
+  } else {
+    //
+    // Feed power to all the hub ports. It should be ok
+    // for both gang/individual powered hubs.
+    //
+    for (Index = 0; Index < HubDesc.NumPorts; Index++) {
+      UsbHubCtrlSetPortFeature (HubIf->Device, Index, (EFI_USB_PORT_FEATURE) USB_HUB_PORT_POWER);
+    }
+
+    //
+    // Update for the usb hub has no power on delay requirement
+    //
+    if (HubDesc.PwrOn2PwrGood > 0) {
+      gBS->Stall (HubDesc.PwrOn2PwrGood * USB_SET_PORT_POWER_STALL);
+    }
+    UsbHubAckHubStatus (HubIf->Device);
+  }
+
+  //
+  // Create an event to enumerate the hub's port. On
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  UsbHubEnumeration,
+                  HubIf,
+                  &HubIf->HubNotify
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to create signal for hub %d - %r\n",
+                HubDev->Address, Status));
+
+    return Status;
+  }
+
+  //
+  // Create AsyncInterrupt to query hub port change endpoint
+  // periodically. If the hub ports are changed, hub will return
+  // changed port map from the interrupt endpoint. The port map
+  // must be able to hold (HubIf->NumOfPort + 1) bits (one bit for
+  // host change status).
+  //
+  UsbIo  = &HubIf->UsbIo;
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EpDesc->Desc.EndpointAddress,
+                    TRUE,
+                    USB_HUB_POLL_INTERVAL,
+                    HubIf->NumOfPort / 8 + 1,
+                    UsbOnHubInterrupt,
+                    HubIf
+                    );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbHubInit: failed to queue interrupt transfer for hub %d - %r\n",
+                HubDev->Address, Status));
+
+    gBS->CloseEvent (HubIf->HubNotify);
+    HubIf->HubNotify = NULL;
+
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbHubInit: hub %d initialized\n", HubDev->Address));
+  return Status;
+}
+
+
+
+/**
+  Get the port status. This function is required to
+  ACK the port change bits although it will return
+  the port changes in PortState. Bus enumeration code
+  doesn't need to ACK the port change bits.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub to get state.
+  @param  PortState             Variable to return the port state.
+
+  @retval EFI_SUCCESS           The port status is successfully returned.
+  @retval Others                Failed to return the status.
+
+**/
+EFI_STATUS
+UsbHubGetPortStatus (
+  IN  USB_INTERFACE       *HubIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHubCtrlGetPortStatus (HubIf->Device, Port, PortState);
+
+  return Status;
+}
+
+
+
+/**
+  Clear the port change status.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The hub port.
+
+**/
+VOID
+UsbHubClearPortChange (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  USB_CHANGE_FEATURE_MAP  *Map;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // OK, get the usb port status, now ACK the change bits.
+  // Don't return error when failed to clear the change bits.
+  // It may lead to extra port state report. USB bus should
+  // be able to handle this.
+  //
+  for (Index = 0; Index < sizeof (mHubFeatureMap) / sizeof (mHubFeatureMap[0]); Index++) {
+    Map = &mHubFeatureMap[Index];
+
+    if (USB_BIT_IS_SET (PortState.PortChangeStatus, Map->ChangedBit)) {
+      UsbHubCtrlClearPortFeature (HubIf->Device, Port, (UINT16) Map->Feature);
+    }
+  }
+}
+
+
+
+/**
+  Function to set the port feature for non-root hub.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub.
+  @param  Feature               The feature of the port to set.
+
+  @retval EFI_SUCCESS           The hub port feature is set.
+  @retval Others                Failed to set the port feature.
+
+**/
+EFI_STATUS
+UsbHubSetPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlSetPortFeature (HubIf->Device, Port, (UINT8) Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to clear the port feature for non-root hub.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port of the hub to clear feature for.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The port feature is cleared.
+  @retval Others                Failed to clear the port feature.
+
+**/
+EFI_STATUS
+UsbHubClearPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = UsbHubCtrlClearPortFeature (HubIf->Device, Port, (UINT8) Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to reset the port.
+
+  @param  HubIf                 The hub interface.
+  @param  Port                  The port to reset.
+
+  @retval EFI_SUCCESS           The hub port is reset.
+  @retval EFI_TIMEOUT           Failed to reset the port in time.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbHubResetPort (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } else if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+    DEBUG (( EFI_D_INFO, "UsbHubResetPort: skip reset on hub %p port %d\n", HubIf, Port));
+    return EFI_SUCCESS;
+  }
+
+  Status  = UsbHubSetPortFeature (HubIf, Port, (EFI_USB_PORT_FEATURE) USB_HUB_PORT_RESET);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Drive the reset signal for worst 20ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  gBS->Stall (USB_SET_PORT_RESET_STALL);
+
+  //
+  // Check USB_PORT_STAT_C_RESET bit to see if the resetting state is done.
+  //
+  ZeroMem (&PortState, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = UsbHubGetPortStatus (HubIf, Port, &PortState);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (!EFI_ERROR (Status) &&
+        USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+      gBS->Stall (USB_SET_PORT_RECOVERY_STALL);
+      return EFI_SUCCESS;
+    }
+
+    gBS->Stall (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  return EFI_TIMEOUT;
+}
+
+
+/**
+  Release the hub's control of the interface.
+
+  @param  HubIf                 The hub interface.
+
+  @retval EFI_SUCCESS           The interface is release of hub control.
+
+**/
+EFI_STATUS
+UsbHubRelease (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_USB_IO_PROTOCOL     *UsbIo;
+  EFI_STATUS              Status;
+
+  UsbIo  = &HubIf->UsbIo;
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    HubIf->HubEp->Desc.EndpointAddress,
+                    FALSE,
+                    USB_HUB_POLL_INTERVAL,
+                    0,
+                    NULL,
+                    0
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseEvent (HubIf->HubNotify);
+
+  HubIf->IsHub      = FALSE;
+  HubIf->HubApi     = NULL;
+  HubIf->HubEp      = NULL;
+  HubIf->HubNotify  = NULL;
+
+  DEBUG (( EFI_D_INFO, "UsbHubRelease: hub device %d released\n", HubIf->Device->Address));
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Initialize the interface for root hub.
+
+  @param  HubIf                 The root hub interface.
+
+  @retval EFI_SUCCESS           The interface is initialized for root hub.
+  @retval Others                Failed to initialize the hub.
+
+**/
+EFI_STATUS
+UsbRootHubInit (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  EFI_STATUS              Status;
+  UINT8                   MaxSpeed;
+  UINT8                   NumOfPort;
+  UINT8                   Support64;
+
+  Status = UsbHcGetCapability (HubIf->Device->Bus, &MaxSpeed, &NumOfPort, &Support64);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG (( EFI_D_INFO, "UsbRootHubInit: root hub %p - max speed %d, %d ports\n",
+              HubIf, MaxSpeed, NumOfPort));
+
+  HubIf->IsHub      = TRUE;
+  HubIf->HubApi     = &mUsbRootHubApi;
+  HubIf->HubEp      = NULL;
+  HubIf->MaxSpeed   = MaxSpeed;
+  HubIf->NumOfPort  = NumOfPort;
+  HubIf->HubNotify  = NULL;
+
+  //
+  // Create a timer to poll root hub ports periodically
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  UsbRootHubEnumeration,
+                  HubIf,
+                  &HubIf->HubNotify
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // It should signal the event immediately here, or device detection
+  // by bus enumeration might be delayed by the timer interval.
+  //
+  gBS->SignalEvent (HubIf->HubNotify);
+
+  Status = gBS->SetTimer (
+                  HubIf->HubNotify,
+                  TimerPeriodic,
+                  USB_ROOTHUB_POLL_INTERVAL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->CloseEvent (HubIf->HubNotify);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the port status. This function is required to
+  ACK the port change bits although it will return
+  the port changes in PortState. Bus enumeration code
+  doesn't need to ACK the port change bits.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port to get the state.
+  @param  PortState             Variable to return the port state.
+
+  @retval EFI_SUCCESS           The port state is returned.
+  @retval Others                Failed to retrieve the port state.
+
+**/
+EFI_STATUS
+UsbRootHubGetPortStatus (
+  IN  USB_INTERFACE       *HubIf,
+  IN  UINT8               Port,
+  OUT EFI_USB_PORT_STATUS *PortState
+  )
+{
+  USB_BUS                 *Bus;
+  EFI_STATUS              Status;
+
+  Bus     = HubIf->Device->Bus;
+  Status  = UsbHcGetRootHubPortStatus (Bus, Port, PortState);
+
+  return Status;
+}
+
+
+/**
+  Clear the port change status.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port.
+
+**/
+VOID
+UsbRootHubClearPortChange (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port
+  )
+{
+  EFI_USB_PORT_STATUS     PortState;
+  USB_CHANGE_FEATURE_MAP  *Map;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  Status = UsbRootHubGetPortStatus (HubIf, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // OK, get the usb port status, now ACK the change bits.
+  // Don't return error when failed to clear the change bits.
+  // It may lead to extra port state report. USB bus should
+  // be able to handle this.
+  //
+  for (Index = 0; Index < sizeof (mRootHubFeatureMap) / sizeof (mRootHubFeatureMap[0]); Index++) {
+    Map = &mRootHubFeatureMap[Index];
+
+    if (USB_BIT_IS_SET (PortState.PortChangeStatus, Map->ChangedBit)) {
+      UsbHcClearRootHubPortFeature (HubIf->Device->Bus, Port, (EFI_USB_PORT_FEATURE) Map->Feature);
+    }
+  }
+}
+
+
+/**
+  Set the root hub port feature.
+
+  @param  HubIf                 The Usb hub interface.
+  @param  Port                  The hub port.
+  @param  Feature               The feature to set.
+
+  @retval EFI_SUCCESS           The root hub port is set with the feature.
+  @retval Others                Failed to set the feature.
+
+**/
+EFI_STATUS
+UsbRootHubSetPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHcSetRootHubPortFeature (HubIf->Device->Bus, Port, Feature);
+
+  return Status;
+}
+
+
+/**
+  Clear the root hub port feature.
+
+  @param  HubIf                 The root hub interface.
+  @param  Port                  The root hub port.
+  @param  Feature               The feature to clear.
+
+  @retval EFI_SUCCESS           The root hub port is cleared of the feature.
+  @retval Others                Failed to clear the feature.
+
+**/
+EFI_STATUS
+UsbRootHubClearPortFeature (
+  IN USB_INTERFACE        *HubIf,
+  IN UINT8                Port,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  Status  = UsbHcClearRootHubPortFeature (HubIf->Device->Bus, Port, Feature);
+
+  return Status;
+}
+
+
+/**
+  Interface function to reset the root hub port.
+
+  @param  RootIf                The root hub interface.
+  @param  Port                  The port to reset.
+
+  @retval EFI_SUCCESS           The hub port is reset.
+  @retval EFI_TIMEOUT           Failed to reset the port in time.
+  @retval EFI_NOT_FOUND         The low/full speed device connected to high  speed.
+                                root hub is released to the companion UHCI.
+  @retval Others                Failed to reset the port.
+
+**/
+EFI_STATUS
+UsbRootHubResetPort (
+  IN USB_INTERFACE        *RootIf,
+  IN UINT8                Port
+  )
+{
+  USB_BUS                 *Bus;
+  EFI_STATUS              Status;
+  EFI_USB_PORT_STATUS     PortState;
+  UINTN                   Index;
+
+  //
+  // Notice: although EHCI requires that ENABLED bit be cleared
+  // when reset the port, we don't need to care that here. It
+  // should be handled in the EHCI driver.
+  //
+  Bus     = RootIf->Device->Bus;
+
+  Status = UsbHcGetRootHubPortStatus (Bus, Port, &PortState);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } else if (USB_BIT_IS_SET (PortState.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+    DEBUG (( EFI_D_INFO, "UsbRootHubResetPort: skip reset on root port %d\n", Port));
+    return EFI_SUCCESS;
+  }
+
+  Status  = UsbHcSetRootHubPortFeature (Bus, Port, EfiUsbPortReset);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to start reset on port %d\n", Port));
+    return Status;
+  }
+
+  //
+  // Drive the reset signal for at least 50ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  gBS->Stall (USB_SET_ROOT_PORT_RESET_STALL);
+
+  Status = UsbHcClearRootHubPortFeature (Bus, Port, EfiUsbPortReset);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to clear reset on port %d\n", Port));
+    return Status;
+  }
+
+  gBS->Stall (USB_CLR_ROOT_PORT_RESET_STALL);
+
+  //
+  // USB host controller won't clear the RESET bit until
+  // reset is actually finished.
+  //
+  ZeroMem (&PortState, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = UsbHcGetRootHubPortStatus (Bus, Port, &PortState);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_RESET)) {
+      break;
+    }
+
+    gBS->Stall (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+    DEBUG ((EFI_D_ERROR, "UsbRootHubResetPort: reset not finished in time on port %d\n", Port));
+    return EFI_TIMEOUT;
+  }
+
+  if (!USB_BIT_IS_SET (PortState.PortStatus, USB_PORT_STAT_ENABLE)) {
+    //
+    // OK, the port is reset. If root hub is of high speed and
+    // the device is of low/full speed, release the ownership to
+    // companion UHCI. If root hub is of full speed, it won't
+    // automatically enable the port, we need to enable it manually.
+    //
+    if (RootIf->MaxSpeed == EFI_USB_SPEED_HIGH) {
+      DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: release low/full speed device (%d) to UHCI\n", Port));
+
+      UsbRootHubSetPortFeature (RootIf, Port, EfiUsbPortOwner);
+      return EFI_NOT_FOUND;
+
+    } else {
+
+      Status = UsbRootHubSetPortFeature (RootIf, Port, EfiUsbPortEnable);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG (( EFI_D_ERROR, "UsbRootHubResetPort: failed to enable port %d for UHCI\n", Port));
+        return Status;
+      }
+
+      gBS->Stall (USB_SET_ROOT_PORT_ENABLE_STALL);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Release the root hub's control of the interface.
+
+  @param  HubIf                 The root hub interface.
+
+  @retval EFI_SUCCESS           The root hub's control of the interface is
+                                released.
+
+**/
+EFI_STATUS
+UsbRootHubRelease (
+  IN USB_INTERFACE        *HubIf
+  )
+{
+  DEBUG (( EFI_D_INFO, "UsbRootHubRelease: root hub released for hub %p\n", HubIf));
+
+  gBS->SetTimer (HubIf->HubNotify, TimerCancel, USB_ROOTHUB_POLL_INTERVAL);
+  gBS->CloseEvent (HubIf->HubNotify);
+
+  return EFI_SUCCESS;
+}
+
+USB_HUB_API mUsbHubApi = {
+  UsbHubInit,
+  UsbHubGetPortStatus,
+  UsbHubClearPortChange,
+  UsbHubSetPortFeature,
+  UsbHubClearPortFeature,
+  UsbHubResetPort,
+  UsbHubRelease
+};
+
+USB_HUB_API mUsbRootHubApi = {
+  UsbRootHubInit,
+  UsbRootHubGetPortStatus,
+  UsbRootHubClearPortChange,
+  UsbRootHubSetPortFeature,
+  UsbRootHubClearPortFeature,
+  UsbRootHubResetPort,
+  UsbRootHubRelease
+};
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h
new file mode 100644
index 0000000000..4e5fcd85e0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbHub.h
@@ -0,0 +1,199 @@
+/** @file
+
+    The definition for USB hub.
+
+Copyright (c) 2007 - 2010, 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.
+
+**/
+
+#ifndef _USB_HUB_H_
+#define _USB_HUB_H_
+
+#include <IndustryStandard/Usb.h>
+
+#define USB_ENDPOINT_ADDR(EpAddr) ((EpAddr) & 0x7F)
+#define USB_ENDPOINT_TYPE(Desc)   ((Desc)->Attributes & USB_ENDPOINT_TYPE_MASK)
+
+
+#define USB_DESC_TYPE_HUB     0x29
+
+#define USB_DESC_TYPE_HUB_SUPER_SPEED  0x2a
+
+//
+// Hub class control transfer target
+//
+#define USB_HUB_TARGET_HUB    0
+#define USB_HUB_TARGET_PORT   3
+//
+// HUB class specific contrl transfer request type
+//
+#define USB_HUB_REQ_GET_STATUS      0
+#define USB_HUB_REQ_CLEAR_FEATURE   1
+#define USB_HUB_REQ_SET_FEATURE     3
+#define USB_HUB_REQ_GET_DESC        6
+#define USB_HUB_REQ_SET_DESC        7
+#define USB_HUB_REQ_CLEAR_TT        8
+#define USB_HUB_REQ_RESET_TT        9
+#define USB_HUB_REQ_GET_TT_STATE    10
+#define USB_HUB_REQ_STOP_TT         11
+
+#define USB_HUB_REQ_SET_DEPTH       12
+
+//
+// USB hub class feature selector
+//
+#define USB_HUB_C_HUB_LOCAL_POWER   0
+#define USB_HUB_C_HUB_OVER_CURRENT  1
+#define USB_HUB_PORT_CONNECTION     0
+#define USB_HUB_PORT_ENABLE         1
+#define USB_HUB_PORT_SUSPEND        2
+#define USB_HUB_PORT_OVER_CURRENT   3
+#define USB_HUB_PORT_RESET          4
+
+#define USB_HUB_PORT_LINK_STATE     5
+
+#define USB_HUB_PORT_POWER          8
+#define USB_HUB_PORT_LOW_SPEED      9
+#define USB_HUB_C_PORT_CONNECT      16
+#define USB_HUB_C_PORT_ENABLE       17
+#define USB_HUB_C_PORT_SUSPEND      18
+#define USB_HUB_C_PORT_OVER_CURRENT 19
+#define USB_HUB_C_PORT_RESET        20
+#define USB_HUB_PORT_TEST           21
+#define USB_HUB_PORT_INDICATOR      22
+
+#define USB_HUB_C_PORT_LINK_STATE     25
+#define USB_HUB_PORT_REMOTE_WAKE_MASK 27
+#define USB_HUB_BH_PORT_RESET         28
+#define USB_HUB_C_BH_PORT_RESET       29
+
+//
+// Constant value for Port Status & Port Change Status of SuperSpeed port
+//
+#define USB_SS_PORT_STAT_C_BH_RESET         0x0020
+#define USB_SS_PORT_STAT_C_PORT_LINK_STATE  0x0040
+//
+// USB hub power control method. In gang power control
+//
+#define USB_HUB_GANG_POWER_CTRL     0
+#define USB_HUB_PORT_POWER_CTRL     0x01
+//
+// USB hub status bits
+//
+#define USB_HUB_STAT_LOCAL_POWER    0x01
+#define USB_HUB_STAT_OVER_CURRENT   0x02
+#define USB_HUB_STAT_C_LOCAL_POWER  0x01
+#define USB_HUB_STAT_C_OVER_CURRENT 0x02
+
+#define USB_HUB_CLASS_CODE          0x09
+#define USB_HUB_SUBCLASS_CODE       0x00
+
+//
+// Host software return timeout if port status doesn't change 
+// after 500ms(LOOP * STALL = 5000 * 0.1ms), set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_LOOP  5000
+
+#pragma pack(1)
+//
+// Hub descriptor, the last two fields are of variable lenght.
+//
+typedef struct {
+  UINT8           Length;
+  UINT8           DescType;
+  UINT8           NumPorts;
+  UINT16          HubCharacter;
+  UINT8           PwrOn2PwrGood;
+  UINT8           HubContrCurrent;
+  UINT8           Filler[16];
+} EFI_USB_HUB_DESCRIPTOR;
+
+typedef struct {
+  UINT8           Length;
+  UINT8           DescType;
+  UINT8           NumPorts;
+  UINT16          HubCharacter;
+  UINT8           PwrOn2PwrGood;
+  UINT8           HubContrCurrent;
+  UINT8           HubHdrDecLat;
+  UINT8           HubDelay;
+  UINT8           DeviceRemovable;
+} EFI_USB_SUPER_SPEED_HUB_DESCRIPTOR;
+
+#pragma pack()
+
+
+typedef struct {
+  UINT16                ChangedBit;
+  EFI_USB_PORT_FEATURE  Feature;
+} USB_CHANGE_FEATURE_MAP;
+
+
+/**
+  Clear the transaction translate buffer if full/low
+  speed control/bulk transfer failed and the transfer
+  uses this hub as translator.Remember to clear the TT
+  buffer of transaction translator, not that of the
+  parent.
+
+  @param  UsbDev                The Usb device.
+  @param  Port                  The port of the hub.
+  @param  DevAddr               Address of the failed transaction.
+  @param  EpNum                 The endpoint number of the failed transaction.
+  @param  EpType                The type of failed transaction.
+
+  @retval EFI_SUCCESS           The TT buffer is cleared.
+  @retval Others                Failed to clear the TT buffer.
+
+**/
+EFI_STATUS
+UsbHubCtrlClearTTBuffer (
+  IN USB_DEVICE           *UsbDev,
+  IN UINT8                Port,
+  IN UINT16               DevAddr,
+  IN UINT16               EpNum,
+  IN UINT16               EpType
+  );
+
+
+/**
+  Test whether the interface is a hub interface.
+
+  @param  UsbIf                 The interface to test.
+
+  @retval TRUE                  The interface is a hub interface.
+  @retval FALSE                 The interface isn't a hub interface.
+
+**/
+BOOLEAN
+UsbIsHubInterface (
+  IN USB_INTERFACE        *UsbIf
+  );
+
+
+/**
+  Ack the hub change bits. If these bits are not ACKed, Hub will
+  always return changed bit map from its interrupt endpoint.
+
+  @param  UsbDev                The Usb device.
+
+  @retval EFI_SUCCESS           The hub change status is ACKed.
+  @retval Others                Failed to ACK the hub status.
+
+**/
+EFI_STATUS
+UsbHubAckHubStatus (
+  IN  USB_DEVICE         *UsbDev
+  );
+
+extern USB_HUB_API        mUsbHubApi;
+extern USB_HUB_API        mUsbRootHubApi;
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c
new file mode 100644
index 0000000000..e67a88541b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.c
@@ -0,0 +1,1377 @@
+/** @file
+
+    Wrapper function for usb host controller interface.
+
+Copyright (c) 2007 - 2010, 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 "UsbBus.h"
+
+//
+// if RemainingDevicePath== NULL, then all Usb child devices in this bus are wanted.
+// Use a shor form Usb class Device Path, which could match any usb device, in WantedUsbIoDPList to indicate all Usb devices
+// are wanted Usb devices
+//
+USB_CLASS_FORMAT_DEVICE_PATH mAllUsbClassDevicePath = {
+  {
+    {
+      MESSAGING_DEVICE_PATH,
+      MSG_USB_CLASS_DP,
+      {
+        (UINT8) (sizeof (USB_CLASS_DEVICE_PATH)),
+        (UINT8) ((sizeof (USB_CLASS_DEVICE_PATH)) >> 8)
+      }
+    },
+    0xffff, // VendorId
+    0xffff, // ProductId
+    0xff,   // DeviceClass
+    0xff,   // DeviceSubClass
+    0xff    // DeviceProtocol
+  },
+
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      END_DEVICE_PATH_LENGTH,
+      0
+    }
+  }
+};
+
+
+/**
+  Get the capability of the host controller.
+
+  @param  UsbBus           The usb driver.
+  @param  MaxSpeed         The maximum speed this host controller supports.
+  @param  NumOfPort        The number of the root hub port.
+  @param  Is64BitCapable   Whether this controller support 64 bit addressing.
+
+  @retval EFI_SUCCESS      The host controller capability is returned.
+  @retval Others           Failed to retrieve the host controller capability.
+
+**/
+EFI_STATUS
+UsbHcGetCapability (
+  IN  USB_BUS             *UsbBus,
+  OUT UINT8               *MaxSpeed,
+  OUT UINT8               *NumOfPort,
+  OUT UINT8               *Is64BitCapable
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetCapability (
+                              UsbBus->Usb2Hc,
+                              MaxSpeed,
+                              NumOfPort,
+                              Is64BitCapable
+                              );
+
+  } else {
+    Status = UsbBus->UsbHc->GetRootHubPortNumber (UsbBus->UsbHc, NumOfPort);
+
+    *MaxSpeed       = EFI_USB_SPEED_FULL;
+    *Is64BitCapable = (UINT8) FALSE;
+  }
+
+  return Status;
+}
+
+
+/**
+  Reset the host controller.
+
+  @param  UsbBus                The usb bus driver.
+  @param  Attributes            The reset type, only global reset is used by this driver.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+**/
+EFI_STATUS
+UsbHcReset (
+  IN USB_BUS              *UsbBus,
+  IN UINT16               Attributes
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->Reset (UsbBus->Usb2Hc, Attributes);
+  } else {
+    Status = UsbBus->UsbHc->Reset (UsbBus->UsbHc, Attributes);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the current operation state of the host controller.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The host controller operation state.
+
+  @retval EFI_SUCCESS      The operation state is returned in State.
+  @retval Others           Failed to get the host controller state.
+
+**/
+EFI_STATUS
+UsbHcGetState (
+  IN  USB_BUS             *UsbBus,
+  OUT EFI_USB_HC_STATE    *State
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetState (UsbBus->Usb2Hc, State);
+  } else {
+    Status = UsbBus->UsbHc->GetState (UsbBus->UsbHc, State);
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the host controller operation state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The state to set.
+
+  @retval EFI_SUCCESS      The host controller is now working at State.
+  @retval Others           Failed to set operation state.
+
+**/
+EFI_STATUS
+UsbHcSetState (
+  IN USB_BUS              *UsbBus,
+  IN EFI_USB_HC_STATE     State
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SetState (UsbBus->Usb2Hc, State);
+  } else {
+    Status = UsbBus->UsbHc->SetState (UsbBus->UsbHc, State);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the root hub port state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The index of port.
+  @param  PortStatus       The variable to save port state.
+
+  @retval EFI_SUCCESS      The root port state is returned in.
+  @retval Others           Failed to get the root hub port state.
+
+**/
+EFI_STATUS
+UsbHcGetRootHubPortStatus (
+  IN  USB_BUS             *UsbBus,
+  IN  UINT8               PortIndex,
+  OUT EFI_USB_PORT_STATUS *PortStatus
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->GetRootHubPortStatus (UsbBus->Usb2Hc, PortIndex, PortStatus);
+  } else {
+    Status = UsbBus->UsbHc->GetRootHubPortStatus (UsbBus->UsbHc, PortIndex, PortStatus);
+  }
+
+  return Status;
+}
+
+
+/**
+  Set the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to set.
+
+  @retval EFI_SUCCESS      The port feature is set.
+  @retval Others           Failed to set port feature.
+
+**/
+EFI_STATUS
+UsbHcSetRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SetRootHubPortFeature (UsbBus->Usb2Hc, PortIndex, Feature);
+  } else {
+    Status = UsbBus->UsbHc->SetRootHubPortFeature (UsbBus->UsbHc, PortIndex, Feature);
+  }
+
+  return Status;
+}
+
+
+/**
+  Clear the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to clear.
+
+  @retval EFI_SUCCESS      The port feature is clear.
+  @retval Others           Failed to clear port feature.
+
+**/
+EFI_STATUS
+UsbHcClearRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->ClearRootHubPortFeature (UsbBus->Usb2Hc, PortIndex, Feature);
+  } else {
+    Status = UsbBus->UsbHc->ClearRootHubPortFeature (UsbBus->UsbHc, PortIndex, Feature);
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a control transfer to the device.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The device address.
+  @param  DevSpeed         The device speed.
+  @param  MaxPacket        Maximum packet size of endpoint 0.
+  @param  Request          The control transfer request.
+  @param  Direction        The direction of data stage.
+  @param  Data             The buffer holding data.
+  @param  DataLength       The length of the data.
+  @param  TimeOut          Timeout (in ms) to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of transfer.
+
+  @retval EFI_SUCCESS      The control transfer finished without error.
+  @retval Others           The control transfer failed, reason returned in UsbReslt.
+
+**/
+EFI_STATUS
+UsbHcControlTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              Direction,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->ControlTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               Request,
+                               Direction,
+                               Data,
+                               DataLength,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+
+  } else {
+    IsSlowDevice = (BOOLEAN)(EFI_USB_SPEED_LOW == DevSpeed);
+    Status = UsbBus->UsbHc->ControlTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              Request,
+                              Direction,
+                              Data,
+                              DataLength,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a bulk transfer to the device's endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The bulk transfer is finished without error.
+  @retval Others           Failed to execute bulk transfer, result in UsbResult.
+
+**/
+EFI_STATUS
+UsbHcBulkTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->BulkTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               BufferNum,
+                               Data,
+                               DataLength,
+                               DataToggle,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+  } else {
+    Status = UsbBus->UsbHc->BulkTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              (UINT8) MaxPacket,
+                              *Data,
+                              DataLength,
+                              DataToggle,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Queue or cancel an asynchronous interrupt transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  IsNewTransfer    Whether this is a new request. If not, cancel the old
+                           request.
+  @param  DataToggle       Data toggle to use on input, next toggle on output.
+  @param  PollingInterval  The interval to poll the interrupt transfer (in ms).
+  @param  DataLength       The length of periodical data receive.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         Function to call when data is received.
+  @param  Context          The context to the callback.
+
+  @retval EFI_SUCCESS      The asynchronous transfer is queued.
+  @retval Others           Failed to queue the transfer.
+
+**/
+EFI_STATUS
+UsbHcAsyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               PollingInterval,
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->AsyncInterruptTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               IsNewTransfer,
+                               DataToggle,
+                               PollingInterval,
+                               DataLength,
+                               Translator,
+                               Callback,
+                               Context
+                               );
+  } else {
+    IsSlowDevice = (BOOLEAN)(EFI_USB_SPEED_LOW == DevSpeed);
+
+    Status = UsbBus->UsbHc->AsyncInterruptTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              IsNewTransfer,
+                              DataToggle,
+                              PollingInterval,
+                              DataLength,
+                              Callback,
+                              Context
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a synchronous interrupt transfer to the target endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  Data             Pointer to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK.
+  @retval Others           Failed to execute the synchronous interrupt transfer.
+
+**/
+EFI_STATUS
+UsbHcSyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN OUT VOID                             *Data,
+  IN OUT UINTN                            *DataLength,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  EFI_STATUS              Status;
+  BOOLEAN                 IsSlowDevice;
+
+  if (UsbBus->Usb2Hc != NULL) {
+    Status = UsbBus->Usb2Hc->SyncInterruptTransfer (
+                               UsbBus->Usb2Hc,
+                               DevAddr,
+                               EpAddr,
+                               DevSpeed,
+                               MaxPacket,
+                               Data,
+                               DataLength,
+                               DataToggle,
+                               TimeOut,
+                               Translator,
+                               UsbResult
+                               );
+  } else {
+    IsSlowDevice = (BOOLEAN) ((EFI_USB_SPEED_LOW == DevSpeed) ? TRUE : FALSE);
+    Status = UsbBus->UsbHc->SyncInterruptTransfer (
+                              UsbBus->UsbHc,
+                              DevAddr,
+                              EpAddr,
+                              IsSlowDevice,
+                              (UINT8) MaxPacket,
+                              Data,
+                              DataLength,
+                              DataToggle,
+                              TimeOut,
+                              UsbResult
+                              );
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute a synchronous Isochronous USB transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now.
+
+**/
+EFI_STATUS
+UsbHcIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         The function to call when data is transferred.
+  @param  Context          The context to the callback function.
+
+  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+UsbHcAsyncIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN OUT VOID                             *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Open the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return The open protocol return.
+
+**/
+EFI_STATUS
+UsbOpenHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  )
+{
+  EFI_USB_HC_PROTOCOL     *UsbHc;
+  EFI_USB2_HC_PROTOCOL    *Usb2Hc;
+  EFI_STATUS              Status;
+
+  if (Bus->Usb2Hc != NULL) {
+    Status = gBS->OpenProtocol (
+                    Bus->HostHandle,
+                    &gEfiUsb2HcProtocolGuid,
+                    (VOID **) &Usb2Hc,
+                    mUsbBusDriverBinding.DriverBindingHandle,
+                    Child,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+
+  } else {
+    Status = gBS->OpenProtocol (
+                    Bus->HostHandle,
+                    &gEfiUsbHcProtocolGuid,
+                    (VOID **) &UsbHc,
+                    mUsbBusDriverBinding.DriverBindingHandle,
+                    Child,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+  }
+
+  return Status;
+}
+
+
+/**
+  Close the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+**/
+VOID
+UsbCloseHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  )
+{
+  if (Bus->Usb2Hc != NULL) {
+    gBS->CloseProtocol (
+           Bus->HostHandle,
+           &gEfiUsb2HcProtocolGuid,
+           mUsbBusDriverBinding.DriverBindingHandle,
+           Child
+           );
+
+  } else {
+    gBS->CloseProtocol (
+           Bus->HostHandle,
+           &gEfiUsbHcProtocolGuid,
+           mUsbBusDriverBinding.DriverBindingHandle,
+           Child
+           );
+  }
+}
+
+
+/**
+  return the current TPL, copied from the EDKII glue lib.
+
+  @param  VOID.
+
+  @return Current TPL.
+
+**/
+EFI_TPL
+UsbGetCurrentTpl (
+  VOID
+  )
+{
+  EFI_TPL                 Tpl;
+
+  Tpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  gBS->RestoreTPL (Tpl);
+
+  return Tpl;
+}
+
+/**
+  Create a new device path which only contain the first Usb part of the DevicePath.
+
+  @param DevicePath  A full device path which contain the usb nodes.
+
+  @return            A new device path which only contain the Usb part of the DevicePath.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+EFIAPI
+GetUsbDPFromFullDP (
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathPtr;
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathBeginPtr;
+  EFI_DEVICE_PATH_PROTOCOL    *UsbDevicePathEndPtr;
+  UINTN                       Size;
+
+  //
+  // Get the Usb part first Begin node in full device path
+  //
+  UsbDevicePathBeginPtr = DevicePath;
+  while ( (!IsDevicePathEnd (UsbDevicePathBeginPtr))&&
+         ((UsbDevicePathBeginPtr->Type != MESSAGING_DEVICE_PATH) ||
+         (UsbDevicePathBeginPtr->SubType != MSG_USB_DP &&
+          UsbDevicePathBeginPtr->SubType != MSG_USB_CLASS_DP
+          && UsbDevicePathBeginPtr->SubType != MSG_USB_WWID_DP
+          ))) {
+
+    UsbDevicePathBeginPtr = NextDevicePathNode(UsbDevicePathBeginPtr);
+  }
+
+  //
+  // Get the Usb part first End node in full device path
+  //
+  UsbDevicePathEndPtr = UsbDevicePathBeginPtr;
+  while ((!IsDevicePathEnd (UsbDevicePathEndPtr))&&
+         (UsbDevicePathEndPtr->Type == MESSAGING_DEVICE_PATH) &&
+         (UsbDevicePathEndPtr->SubType == MSG_USB_DP ||
+          UsbDevicePathEndPtr->SubType == MSG_USB_CLASS_DP
+          || UsbDevicePathEndPtr->SubType == MSG_USB_WWID_DP
+          )) {
+
+    UsbDevicePathEndPtr = NextDevicePathNode(UsbDevicePathEndPtr);
+  }
+
+  Size  = GetDevicePathSize (UsbDevicePathBeginPtr);
+  Size -= GetDevicePathSize (UsbDevicePathEndPtr);
+  if (Size ==0){
+    //
+    // The passed in DevicePath does not contain the usb nodes
+    //
+    return NULL;
+  }
+
+  //
+  // Create a new device path which only contain the above Usb part
+  //
+  UsbDevicePathPtr = AllocateZeroPool (Size + sizeof (EFI_DEVICE_PATH_PROTOCOL));
+  ASSERT (UsbDevicePathPtr != NULL);
+  CopyMem (UsbDevicePathPtr, UsbDevicePathBeginPtr, Size);
+  //
+  // Append end device path node
+  //
+  UsbDevicePathEndPtr = (EFI_DEVICE_PATH_PROTOCOL *) ((UINTN) UsbDevicePathPtr + Size);
+  SetDevicePathEndNode (UsbDevicePathEndPtr);
+  return UsbDevicePathPtr;
+}
+
+/**
+  Check whether a usb device path is in a DEVICE_PATH_LIST_ITEM list.
+
+  @param UsbDP       a usb device path of DEVICE_PATH_LIST_ITEM.
+  @param UsbIoDPList a DEVICE_PATH_LIST_ITEM list.
+
+  @retval TRUE       there is a DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.
+  @retval FALSE      there is no DEVICE_PATH_LIST_ITEM in UsbIoDPList which contains the passed in UsbDP.
+
+**/
+BOOLEAN
+EFIAPI
+SearchUsbDPInList (
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,
+  IN LIST_ENTRY                   *UsbIoDPList
+  )
+{
+  LIST_ENTRY                  *ListIndex;
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+  BOOLEAN                     Found;
+  UINTN                       UsbDpDevicePathSize;
+
+  //
+  // Check that UsbDP and UsbIoDPList are valid
+  //
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {
+    return FALSE;
+  }
+
+  Found = FALSE;
+  ListIndex = UsbIoDPList->ForwardLink;
+  while (ListIndex != UsbIoDPList){
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    //
+    // Compare DEVICE_PATH_LIST_ITEM.DevicePath[]
+    //
+    ASSERT (ListItem->DevicePath != NULL);
+
+    UsbDpDevicePathSize  = GetDevicePathSize (UsbDP);
+    if (UsbDpDevicePathSize == GetDevicePathSize (ListItem->DevicePath)) {
+      if ((CompareMem (UsbDP, ListItem->DevicePath, UsbDpDevicePathSize)) == 0) {
+        Found = TRUE;
+        break;
+      }
+    }
+    ListIndex =  ListIndex->ForwardLink;
+  }
+
+  return Found;
+}
+
+/**
+  Add a usb device path into the DEVICE_PATH_LIST_ITEM list.
+
+  @param UsbDP                   a usb device path of DEVICE_PATH_LIST_ITEM.
+  @param UsbIoDPList             a DEVICE_PATH_LIST_ITEM list.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If Add operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+AddUsbDPToList (
+  IN EFI_DEVICE_PATH_PROTOCOL     *UsbDP,
+  IN LIST_ENTRY                   *UsbIoDPList
+  )
+{
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+
+  //
+  // Check that UsbDP and UsbIoDPList are valid
+  //
+  if ((UsbIoDPList == NULL) || (UsbDP == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (SearchUsbDPInList (UsbDP, UsbIoDPList)){
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Prepare the usbio device path DEVICE_PATH_LIST_ITEM structure.
+  //
+  ListItem = AllocateZeroPool (sizeof (DEVICE_PATH_LIST_ITEM));
+  ASSERT (ListItem != NULL);
+  ListItem->Signature = DEVICE_PATH_LIST_ITEM_SIGNATURE;
+  ListItem->DevicePath = DuplicateDevicePath (UsbDP);
+
+  InsertTailList (UsbIoDPList, &ListItem->Link);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether usb device, whose interface is UsbIf, matches the usb class which indicated by
+  UsbClassDevicePathPtr whose is a short form usb class device path.
+
+  @param UsbClassDevicePathPtr    a short form usb class device path.
+  @param UsbIf                    a usb device interface.
+
+  @retval TRUE                    the usb device match the usb class.
+  @retval FALSE                   the usb device does not match the usb class.
+
+**/
+BOOLEAN
+EFIAPI
+MatchUsbClass (
+  IN USB_CLASS_DEVICE_PATH      *UsbClassDevicePathPtr,
+  IN USB_INTERFACE              *UsbIf
+  )
+{
+  USB_INTERFACE_DESC            *IfDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;
+
+
+  if ((UsbClassDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||
+      (UsbClassDevicePathPtr->Header.SubType != MSG_USB_CLASS_DP)){
+    ASSERT (0);
+    return FALSE;
+  }
+
+  IfDesc       = UsbIf->IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);
+
+  //
+  // If connect class policy, determine whether to create device handle by the five fields
+  // in class device path node.
+  //
+  // In addtion, hub interface is always matched for this policy.
+  //
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+    return TRUE;
+  }
+
+  //
+  // If vendor id or product id is 0xffff, they will be ignored.
+  //
+  if ((UsbClassDevicePathPtr->VendorId == 0xffff || UsbClassDevicePathPtr->VendorId == DevDesc->IdVendor) &&
+      (UsbClassDevicePathPtr->ProductId == 0xffff || UsbClassDevicePathPtr->ProductId == DevDesc->IdProduct)) {
+
+    //
+    // If Class in Device Descriptor is set to 0, the counterparts in interface should be checked.
+    //
+    if (DevDesc->DeviceClass == 0) {
+      if ((UsbClassDevicePathPtr->DeviceClass == ActIfDesc->InterfaceClass ||
+                                          UsbClassDevicePathPtr->DeviceClass == 0xff) &&
+          (UsbClassDevicePathPtr->DeviceSubClass == ActIfDesc->InterfaceSubClass ||
+                                       UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&
+          (UsbClassDevicePathPtr->DeviceProtocol == ActIfDesc->InterfaceProtocol ||
+                                       UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {
+        return TRUE;
+      }
+
+    } else if ((UsbClassDevicePathPtr->DeviceClass == DevDesc->DeviceClass ||
+                                         UsbClassDevicePathPtr->DeviceClass == 0xff) &&
+               (UsbClassDevicePathPtr->DeviceSubClass == DevDesc->DeviceSubClass ||
+                                      UsbClassDevicePathPtr->DeviceSubClass == 0xff) &&
+               (UsbClassDevicePathPtr->DeviceProtocol == DevDesc->DeviceProtocol ||
+                                      UsbClassDevicePathPtr->DeviceProtocol == 0xff)) {
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether usb device, whose interface is UsbIf, matches the usb WWID requirement which indicated by
+  UsbWWIDDevicePathPtr whose is a short form usb WWID device path.
+
+  @param UsbWWIDDevicePathPtr    a short form usb WWID device path.
+  @param UsbIf                   a usb device interface.
+
+  @retval TRUE                   the usb device match the usb WWID requirement.
+  @retval FALSE                  the usb device does not match the usb WWID requirement.
+
+**/
+BOOLEAN
+MatchUsbWwid (
+  IN USB_WWID_DEVICE_PATH       *UsbWWIDDevicePathPtr,
+  IN USB_INTERFACE              *UsbIf
+  )
+{
+  USB_INTERFACE_DESC            *IfDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *ActIfDesc;
+  EFI_USB_DEVICE_DESCRIPTOR     *DevDesc;
+  EFI_USB_STRING_DESCRIPTOR     *StrDesc;
+  UINT16                        Index;
+  CHAR16                        *CompareStr;
+  UINTN                         CompareLen;
+  UINTN                         Length;
+
+  if ((UsbWWIDDevicePathPtr->Header.Type != MESSAGING_DEVICE_PATH) ||
+     (UsbWWIDDevicePathPtr->Header.SubType != MSG_USB_WWID_DP )){
+    ASSERT (0);
+    return FALSE;
+  }
+
+  IfDesc       = UsbIf->IfDesc;
+  ASSERT (IfDesc->ActiveIndex < USB_MAX_INTERFACE_SETTING);
+  ActIfDesc    = &(IfDesc->Settings[IfDesc->ActiveIndex]->Desc);
+  DevDesc      = &(UsbIf->Device->DevDesc->Desc);
+
+  //
+  // In addition, Hub interface is always matched for this policy.
+  //
+  if ((ActIfDesc->InterfaceClass == USB_HUB_CLASS_CODE) &&
+      (ActIfDesc->InterfaceSubClass == USB_HUB_SUBCLASS_CODE)) {
+    return TRUE;
+  }
+
+  //
+  // Check Vendor Id, Product Id and Interface Number.
+  //
+  if ((DevDesc->IdVendor != UsbWWIDDevicePathPtr->VendorId) ||
+      (DevDesc->IdProduct != UsbWWIDDevicePathPtr->ProductId) ||
+      (ActIfDesc->InterfaceNumber != UsbWWIDDevicePathPtr->InterfaceNumber)) {
+    return FALSE;
+  }
+
+  //
+  // Check SerialNumber.
+  //
+  if (DevDesc->StrSerialNumber == 0) {
+    return FALSE;
+  }
+
+  //
+  // Serial number in USB WWID device path is the last 64-or-less UTF-16 characters.
+  //
+  CompareStr = (CHAR16 *) (UINTN) (UsbWWIDDevicePathPtr + 1);
+  CompareLen = (DevicePathNodeLength (UsbWWIDDevicePathPtr) - sizeof (USB_WWID_DEVICE_PATH)) / sizeof (CHAR16);
+  if (CompareStr[CompareLen - 1] == L'\0') {
+    CompareLen--;
+  }
+
+  //
+  // Compare serial number in each supported language.
+  //
+  for (Index = 0; Index < UsbIf->Device->TotalLangId; Index++) {
+    StrDesc = UsbGetOneString (UsbIf->Device, DevDesc->StrSerialNumber, UsbIf->Device->LangId[Index]);
+    if (StrDesc == NULL) {
+      continue;
+    }
+
+    Length = (StrDesc->Length - 2) / sizeof (CHAR16);
+    if ((Length >= CompareLen) &&
+        (CompareMem (StrDesc->String + Length - CompareLen, CompareStr, CompareLen * sizeof (CHAR16)) == 0)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Free a DEVICE_PATH_LIST_ITEM list.
+
+  @param  UsbIoDPList            a DEVICE_PATH_LIST_ITEM list pointer.
+
+  @retval EFI_INVALID_PARAMETER  If parameters are invalid, return this value.
+  @retval EFI_SUCCESS            If free operation is successful, return this value.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusFreeUsbDPList (
+  IN LIST_ENTRY          *UsbIoDPList
+  )
+{
+  LIST_ENTRY                  *ListIndex;
+  DEVICE_PATH_LIST_ITEM       *ListItem;
+
+  //
+  // Check that ControllerHandle is a valid handle
+  //
+  if (UsbIoDPList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ListIndex = UsbIoDPList->ForwardLink;
+  while (ListIndex != UsbIoDPList){
+    ListItem = CR(ListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    //
+    // Free DEVICE_PATH_LIST_ITEM.DevicePath[]
+    //
+    if (ListItem->DevicePath != NULL){
+      FreePool(ListItem->DevicePath);
+    }
+    //
+    // Free DEVICE_PATH_LIST_ITEM itself
+    //
+    ListIndex =  ListIndex->ForwardLink;
+    RemoveEntryList (&ListItem->Link);
+    FreePool (ListItem);
+  }
+
+  InitializeListHead (UsbIoDPList);
+  return EFI_SUCCESS;
+}
+
+/**
+  Store a wanted usb child device info (its Usb part of device path) which is indicated by
+  RemainingDevicePath in a Usb bus which  is indicated by UsbBusId.
+
+  @param  UsbBusId               Point to EFI_USB_BUS_PROTOCOL interface.
+  @param  RemainingDevicePath    The remaining device patch.
+
+  @retval EFI_SUCCESS            Add operation is successful.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusAddWantedUsbIoDP (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_BUS                       *Bus;
+  EFI_STATUS                    Status;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;
+
+  //
+  // Check whether remaining device path is valid
+  //
+  if (RemainingDevicePath != NULL && !IsDevicePathEnd (RemainingDevicePath)) {
+    if ((RemainingDevicePath->Type    != MESSAGING_DEVICE_PATH) ||
+        (RemainingDevicePath->SubType != MSG_USB_DP &&
+         RemainingDevicePath->SubType != MSG_USB_CLASS_DP
+         && RemainingDevicePath->SubType != MSG_USB_WWID_DP
+         )) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  if (UsbBusId == NULL){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Bus = USB_BUS_FROM_THIS (UsbBusId);
+
+  if (RemainingDevicePath == NULL) {
+    //
+    // RemainingDevicePath == NULL means all Usb devices in this bus are wanted.
+    // Here use a Usb class Device Path in WantedUsbIoDPList to indicate all Usb devices
+    // are wanted Usb devices
+    //
+    Status = UsbBusFreeUsbDPList (&Bus->WantedUsbIoDPList);
+    ASSERT (!EFI_ERROR (Status));
+    DevicePathPtr = DuplicateDevicePath ((EFI_DEVICE_PATH_PROTOCOL *) &mAllUsbClassDevicePath);
+  } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+    //
+    // If RemainingDevicePath isn't the End of Device Path Node, 
+    // Create new Usb device path according to the usb part in remaining device path
+    //
+    DevicePathPtr = GetUsbDPFromFullDP (RemainingDevicePath);
+  } else {
+    //
+    // If RemainingDevicePath is the End of Device Path Node,
+    // skip enumerate any device and return EFI_SUCESSS
+    // 
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (DevicePathPtr != NULL);
+  Status = AddUsbDPToList (DevicePathPtr, &Bus->WantedUsbIoDPList);
+  ASSERT (!EFI_ERROR (Status));
+  FreePool (DevicePathPtr);
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether a usb child device is the wanted device in a bus.
+
+  @param  Bus     The Usb bus's private data pointer.
+  @param  UsbIf   The usb child device inferface.
+
+  @retval True    If a usb child device is the wanted device in a bus.
+  @retval False   If a usb child device is *NOT* the wanted device in a bus.
+
+**/
+BOOLEAN
+EFIAPI
+UsbBusIsWantedUsbIO (
+  IN USB_BUS                 *Bus,
+  IN USB_INTERFACE           *UsbIf
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePathPtr;
+  LIST_ENTRY                    *WantedUsbIoDPListPtr;
+  LIST_ENTRY                    *WantedListIndex;
+  DEVICE_PATH_LIST_ITEM         *WantedListItem;
+  BOOLEAN                       DoConvert;
+  UINTN                         FirstDevicePathSize;
+
+  //
+  // Check whether passed in parameters are valid
+  //
+  if ((UsbIf == NULL) || (Bus == NULL)) {
+    return FALSE;
+  }
+  //
+  // Check whether UsbIf is Hub
+  //
+  if (UsbIf->IsHub) {
+    return TRUE;
+  }
+
+  //
+  // Check whether all Usb devices in this bus are wanted
+  //
+  if (SearchUsbDPInList ((EFI_DEVICE_PATH_PROTOCOL *)&mAllUsbClassDevicePath, &Bus->WantedUsbIoDPList)){
+    return TRUE;
+  }
+
+  //
+  // Check whether the Usb device match any item in WantedUsbIoDPList
+  //
+  WantedUsbIoDPListPtr = &Bus->WantedUsbIoDPList;
+  //
+  // Create new Usb device path according to the usb part in UsbIo full device path
+  //
+  DevicePathPtr = GetUsbDPFromFullDP (UsbIf->DevicePath);
+  ASSERT (DevicePathPtr != NULL);
+
+  DoConvert = FALSE;
+  WantedListIndex = WantedUsbIoDPListPtr->ForwardLink;
+  while (WantedListIndex != WantedUsbIoDPListPtr){
+    WantedListItem = CR(WantedListIndex, DEVICE_PATH_LIST_ITEM, Link, DEVICE_PATH_LIST_ITEM_SIGNATURE);
+    ASSERT (WantedListItem->DevicePath->Type == MESSAGING_DEVICE_PATH);
+    switch (WantedListItem->DevicePath->SubType) {
+    case MSG_USB_DP:
+      FirstDevicePathSize = GetDevicePathSize (WantedListItem->DevicePath);
+      if (FirstDevicePathSize == GetDevicePathSize (DevicePathPtr)) {
+        if (CompareMem (
+              WantedListItem->DevicePath,
+              DevicePathPtr,
+              GetDevicePathSize (DevicePathPtr)) == 0
+            ) {
+          DoConvert = TRUE;
+        }
+      }
+      break;
+    case MSG_USB_CLASS_DP:
+      if (MatchUsbClass((USB_CLASS_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {
+        DoConvert = TRUE;
+      }
+      break;
+   case MSG_USB_WWID_DP:
+      if (MatchUsbWwid((USB_WWID_DEVICE_PATH *)WantedListItem->DevicePath, UsbIf)) {
+        DoConvert = TRUE;
+      }
+      break;
+    default:
+      ASSERT (0);
+      break;
+    }
+
+    if (DoConvert) {
+      break;
+    }
+
+    WantedListIndex =  WantedListIndex->ForwardLink;
+  }
+  gBS->FreePool (DevicePathPtr);
+
+  //
+  // Check whether the new Usb device path is wanted
+  //
+  if (DoConvert){
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Recursively connnect every wanted usb child device to ensure they all fully connected.
+  Check all the child Usb IO handles in this bus, recursively connecte if it is wanted usb child device.
+
+  @param  UsbBusId                  Point to EFI_USB_BUS_PROTOCOL interface.
+
+  @retval EFI_SUCCESS               Connect is done successfully.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbBusRecursivelyConnectWantedUsbIo (
+  IN EFI_USB_BUS_PROTOCOL         *UsbBusId
+  )
+{
+  USB_BUS                       *Bus;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  USB_INTERFACE                 *UsbIf;
+  UINTN                         UsbIoHandleCount;
+  EFI_HANDLE                    *UsbIoBuffer;
+  EFI_DEVICE_PATH_PROTOCOL      *UsbIoDevicePath;
+
+  if (UsbBusId == NULL){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Bus = USB_BUS_FROM_THIS (UsbBusId);
+
+  //
+  // Get all Usb IO handles in system
+  //
+  UsbIoHandleCount = 0;
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiUsbIoProtocolGuid, NULL, &UsbIoHandleCount, &UsbIoBuffer);
+  if (Status == EFI_NOT_FOUND || UsbIoHandleCount == 0) {
+    return EFI_SUCCESS;
+  }
+  ASSERT (!EFI_ERROR (Status));
+
+  for (Index = 0; Index < UsbIoHandleCount; Index++) {
+    //
+    // Check whether the USB IO handle is a child of this bus
+    // Note: The usb child handle maybe invalid because of hot plugged out during the loop
+    //
+    UsbIoDevicePath = NULL;
+    Status = gBS->HandleProtocol (UsbIoBuffer[Index], &gEfiDevicePathProtocolGuid, (VOID *) &UsbIoDevicePath);
+    if (EFI_ERROR (Status) || UsbIoDevicePath == NULL) {
+      continue;
+    }
+    if (CompareMem (
+            UsbIoDevicePath,
+            Bus->DevicePath,
+            (GetDevicePathSize (Bus->DevicePath) - sizeof (EFI_DEVICE_PATH_PROTOCOL))
+            ) != 0) {
+      continue;
+    }
+
+    //
+    // Get the child Usb IO interface
+    //
+    Status = gBS->HandleProtocol(
+                     UsbIoBuffer[Index],
+                     &gEfiUsbIoProtocolGuid,
+                     (VOID **) &UsbIo
+                     );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    UsbIf   = USB_INTERFACE_FROM_USBIO (UsbIo);
+
+    if (UsbBusIsWantedUsbIO (Bus, UsbIf)) {
+      if (!UsbIf->IsManaged) {
+        //
+        // Recursively connect the wanted Usb Io handle
+        //
+        DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL before connect is %d\n", (UINT32)UsbGetCurrentTpl ()));
+        Status            = gBS->ConnectController (UsbIf->Handle, NULL, NULL, TRUE);
+        UsbIf->IsManaged  = (BOOLEAN)!EFI_ERROR (Status);
+        DEBUG ((EFI_D_INFO, "UsbConnectDriver: TPL after connect is %d\n", (UINT32)UsbGetCurrentTpl()));
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h
new file mode 100644
index 0000000000..26709caa36
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusDxe/UsbUtility.h
@@ -0,0 +1,398 @@
+/** @file
+
+    Manage Usb Port/Hc/Etc.
+
+Copyright (c) 2007, 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.
+
+**/
+
+#ifndef _EFI_USB_UTILITY_H_
+#define _EFI_USB_UTILITY_H_
+
+/**
+  Get the capability of the host controller.
+
+  @param  UsbBus           The usb driver.
+  @param  MaxSpeed         The maximum speed this host controller supports.
+  @param  NumOfPort        The number of the root hub port.
+  @param  Is64BitCapable   Whether this controller support 64 bit addressing.
+
+  @retval EFI_SUCCESS      The host controller capability is returned.
+  @retval Others           Failed to retrieve the host controller capability.
+
+**/
+EFI_STATUS
+UsbHcGetCapability (
+  IN  USB_BUS             *UsbBus,
+  OUT UINT8               *MaxSpeed,
+  OUT UINT8               *NumOfPort,
+  OUT UINT8               *Is64BitCapable
+  );
+
+/**
+  Reset the host controller.
+
+  @param  UsbBus                The usb bus driver.
+  @param  Attributes            The reset type, only global reset is used by this driver.
+
+  @retval EFI_SUCCESS           The reset operation succeeded.
+  @retval EFI_INVALID_PARAMETER Attributes is not valid.
+  @retval EFI_UNSUPPOURTED      The type of reset specified by Attributes is
+                                not currently supported by the host controller.
+  @retval EFI_DEVICE_ERROR      Host controller isn't halted to reset.
+**/
+EFI_STATUS
+UsbHcReset (
+  IN USB_BUS              *UsbBus,
+  IN UINT16               Attributes
+  );
+
+/**
+  Get the current operation state of the host controller.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The host controller operation state.
+
+  @retval EFI_SUCCESS      The operation state is returned in State.
+  @retval Others           Failed to get the host controller state.
+
+**/
+EFI_STATUS
+UsbHcGetState (
+  IN  USB_BUS             *UsbBus,
+  OUT EFI_USB_HC_STATE    *State
+  );
+
+/**
+  Set the host controller operation state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  State            The state to set.
+
+  @retval EFI_SUCCESS      The host controller is now working at State.
+  @retval Others           Failed to set operation state.
+
+**/
+EFI_STATUS
+UsbHcSetState (
+  IN  USB_BUS             *UsbBus,
+  IN EFI_USB_HC_STATE     State
+  );
+
+/**
+  Get the root hub port state.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The index of port.
+  @param  PortStatus       The variable to save port state.
+
+  @retval EFI_SUCCESS      The root port state is returned in.
+  @retval Others           Failed to get the root hub port state.
+
+**/
+EFI_STATUS
+UsbHcGetRootHubPortStatus (
+  IN  USB_BUS             *UsbBus,
+  IN  UINT8               PortIndex,
+  OUT EFI_USB_PORT_STATUS *PortStatus
+  );
+
+/**
+  Set the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to set.
+
+  @retval EFI_SUCCESS      The port feature is set.
+  @retval Others           Failed to set port feature.
+
+**/
+EFI_STATUS
+UsbHcSetRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+/**
+  Clear the root hub port feature.
+
+  @param  UsbBus           The USB bus driver.
+  @param  PortIndex        The port index.
+  @param  Feature          The port feature to clear.
+
+  @retval EFI_SUCCESS      The port feature is clear.
+  @retval Others           Failed to clear port feature.
+
+**/
+EFI_STATUS
+UsbHcClearRootHubPortFeature (
+  IN USB_BUS              *UsbBus,
+  IN UINT8                PortIndex,
+  IN EFI_USB_PORT_FEATURE Feature
+  );
+
+/**
+  Execute a control transfer to the device.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The device address.
+  @param  DevSpeed         The device speed.
+  @param  MaxPacket        Maximum packet size of endpoint 0.
+  @param  Request          The control transfer request.
+  @param  Direction        The direction of data stage.
+  @param  Data             The buffer holding data.
+  @param  DataLength       The length of the data.
+  @param  TimeOut          Timeout (in ms) to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of transfer.
+
+  @retval EFI_SUCCESS      The control transfer finished without error.
+  @retval Others           The control transfer failed, reason returned in UsbReslt.
+
+**/
+EFI_STATUS
+UsbHcControlTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  EFI_USB_DEVICE_REQUEST              *Request,
+  IN  EFI_USB_DATA_DIRECTION              Direction,
+  IN  OUT VOID                            *Data,
+  IN  OUT UINTN                           *DataLength,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Execute a bulk transfer to the device's endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The bulk transfer is finished without error.
+  @retval Others           Failed to execute bulk transfer, result in UsbResult.
+
+**/
+EFI_STATUS
+UsbHcBulkTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN  OUT UINTN                           *DataLength,
+  IN  OUT UINT8                           *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Queue or cancel an asynchronous interrupt transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  IsNewTransfer    Whether this is a new request. If not, cancel the old
+                           request.
+  @param  DataToggle       Data toggle to use on input, next toggle on output.
+  @param  PollingInterval  The interval to poll the interrupt transfer (in ms).
+  @param  DataLength       The length of periodical data receive.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         Function to call when data is received.
+  @param  Context          The context to the callback.
+
+  @retval EFI_SUCCESS      The asynchronous transfer is queued.
+  @retval Others           Failed to queue the transfer.
+
+**/
+EFI_STATUS
+UsbHcAsyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  BOOLEAN                             IsNewTransfer,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               PollingInterval,
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context OPTIONAL
+  );
+
+/**
+  Execute a synchronous interrupt transfer to the target endpoint.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  Data             Pointer to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  DataToggle       On input, the initial data toggle to use, also  return
+                           the next toggle on output.
+  @param  TimeOut          The time to wait until timeout.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_SUCCESS      The synchronous interrupt transfer is OK.
+  @retval Others           Failed to execute the synchronous interrupt transfer.
+
+**/
+EFI_STATUS
+UsbHcSyncInterruptTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN OUT VOID                             *Data,
+  IN OUT UINTN                            *DataLength,
+  IN OUT UINT8                            *DataToggle,
+  IN  UINTN                               TimeOut,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Execute a synchronous Isochronous USB transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  UsbResult        The result of USB execution.
+
+  @retval EFI_UNSUPPORTED  The isochronous transfer isn't supported now.
+
+**/
+EFI_STATUS
+UsbHcIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN  OUT VOID                            *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT UINT32                              *UsbResult
+  );
+
+/**
+  Queue an asynchronous isochronous transfer.
+
+  @param  UsbBus           The USB bus driver.
+  @param  DevAddr          The target device address.
+  @param  EpAddr           The target endpoint address, with direction encoded in
+                           bit 7.
+  @param  DevSpeed         The device's speed.
+  @param  MaxPacket        The endpoint's max packet size.
+  @param  BufferNum        The number of data buffer.
+  @param  Data             Array of pointers to data buffer.
+  @param  DataLength       The length of data buffer.
+  @param  Translator       The transaction translator for low/full speed device.
+  @param  Callback         The function to call when data is transferred.
+  @param  Context          The context to the callback function.
+
+  @retval EFI_UNSUPPORTED  The asynchronous isochronous transfer isn't supported.
+
+**/
+EFI_STATUS
+UsbHcAsyncIsochronousTransfer (
+  IN  USB_BUS                             *UsbBus,
+  IN  UINT8                               DevAddr,
+  IN  UINT8                               EpAddr,
+  IN  UINT8                               DevSpeed,
+  IN  UINTN                               MaxPacket,
+  IN  UINT8                               BufferNum,
+  IN OUT VOID                             *Data[EFI_USB_MAX_ISO_BUFFER_NUM],
+  IN  UINTN                               DataLength,
+  IN  EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  IN  EFI_ASYNC_USB_TRANSFER_CALLBACK     Callback,
+  IN  VOID                                *Context
+  );
+
+/**
+  Open the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return The open protocol return.
+
+**/
+EFI_STATUS
+UsbOpenHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  );
+
+/**
+  Close the USB host controller protocol BY_CHILD.
+
+  @param  Bus              The USB bus driver.
+  @param  Child            The child handle.
+
+  @return None.
+
+**/
+VOID
+UsbCloseHostProtoByChild (
+  IN USB_BUS              *Bus,
+  IN EFI_HANDLE           Child
+  );
+
+/**
+  return the current TPL, copied from the EDKII glue lib.
+
+  @param  VOID.
+
+  @return Current TPL.
+
+**/
+EFI_TPL
+UsbGetCurrentTpl (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c
new file mode 100644
index 0000000000..16a7b589c1
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.c
@@ -0,0 +1,670 @@
+/** @file
+Usb Hub Request Support In PEI Phase
+
+Copyright (c) 2006 - 2014, 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"
+
+/**
+  Get a given hub port status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  PortStatus    Current Hub port status and change status.
+
+  @retval EFI_SUCCESS       Port status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the port status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetPortStatus (
+  IN  EFI_PEI_SERVICES    **PeiServices,
+  IN  PEI_USB_IO_PPI      *UsbIoPpi,
+  IN  UINT8               Port,
+  OUT UINT32              *PortStatus
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_GET_PORT_STATUS_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_GET_PORT_STATUS;
+  DeviceRequest.Index       = Port;
+  DeviceRequest.Length      = (UINT16) sizeof (UINT32);
+
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     PortStatus,
+                     sizeof (UINT32)
+                     );
+
+}
+
+/**
+  Set specified feature to a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetPortFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Port,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_SET_PORT_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_SET_PORT_FEATURE;
+  DeviceRequest.Value       = Value;
+  DeviceRequest.Index       = Port;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear specified feature on a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Port feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearPortFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Port,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_CLEAR_FEATURE_PORT_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_CLEAR_FEATURE_PORT;
+  DeviceRequest.Value       = Value;
+  DeviceRequest.Index       = Port;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Get a given hub status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  HubStatus     Current Hub status and change status.
+
+  @retval EFI_SUCCESS       Hub status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetHubStatus (
+  IN  EFI_PEI_SERVICES    **PeiServices,
+  IN  PEI_USB_IO_PPI      *UsbIoPpi,
+  OUT UINT32              *HubStatus
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_GET_HUB_STATUS_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_GET_HUB_STATUS;
+  DeviceRequest.Length      = (UINT16) sizeof (UINT32);
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     HubStatus,
+                     sizeof (UINT32)
+                     );
+}
+
+/**
+  Set specified feature to a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetHubFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_SET_HUB_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_SET_HUB_FEATURE;
+  DeviceRequest.Value       = Value;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DeviceRequest,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear specified feature on a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Hub feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the hub feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearHubFeature (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               Value
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DeviceRequest;
+
+  ZeroMem (&DeviceRequest, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DeviceRequest.RequestType = USB_HUB_CLEAR_FEATURE_REQ_TYPE;
+  DeviceRequest.Request     = USB_HUB_CLEAR_FEATURE;
+  DeviceRequest.Value       = Value;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DeviceRequest,
+                      EfiUsbNoData,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      NULL,
+                      0
+                      );
+}
+
+/**
+  Get a given hub descriptor.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  DescriptorSize The length of Hub Descriptor buffer.
+  @param  HubDescriptor  Caller allocated buffer to store the hub descriptor if
+                         successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetHubDescriptor (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_USB_IO_PPI            *UsbIoPpi,
+  IN  UINTN                     DescriptorSize,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST      DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB | 0x80;
+  DevReq.Request     = USB_HUB_GET_DESCRIPTOR;
+  DevReq.Value       = USB_DT_HUB << 8;
+  DevReq.Length      = (UINT16)DescriptorSize;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbDataIn,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      HubDescriptor,
+                      (UINT16)DescriptorSize
+                      );
+}
+
+/**
+  Get a given SuperSpeed hub descriptor.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  HubDescriptor     Caller allocated buffer to store the hub descriptor if
+                            successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetSuperSpeedHubDesc (
+  IN  EFI_PEI_SERVICES          **PeiServices,
+  IN  PEI_USB_IO_PPI            *UsbIoPpi,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST        DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB | 0x80;
+  DevReq.Request     = USB_HUB_GET_DESCRIPTOR;
+  DevReq.Value       = USB_DT_SUPERSPEED_HUB << 8;
+  DevReq.Length      = 12;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbDataIn,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      HubDescriptor,
+                      12
+                      );
+}
+
+/**
+  Read the whole usb hub descriptor. It is necessary
+  to do it in two steps because hub descriptor is of
+  variable length.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice      Indicates the hub controller device.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  HubDescriptor     Caller allocated buffer to store the hub descriptor if
+                            successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbHubReadDesc (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN PEI_USB_DEVICE             *PeiUsbDevice,
+  IN PEI_USB_IO_PPI             *UsbIoPpi,
+  OUT EFI_USB_HUB_DESCRIPTOR    *HubDescriptor
+  )
+{
+  EFI_STATUS Status;
+
+  if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    //
+    // Get the super speed hub descriptor
+    //
+    Status = PeiGetSuperSpeedHubDesc (PeiServices, UsbIoPpi, HubDescriptor);
+  } else {
+
+    //
+    // First get the hub descriptor length
+    //
+    Status = PeiGetHubDescriptor (PeiServices, UsbIoPpi, 2, HubDescriptor);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Get the whole hub descriptor
+    //
+    Status = PeiGetHubDescriptor (PeiServices, UsbIoPpi, HubDescriptor->Length, HubDescriptor);
+  }
+
+  return Status;
+}
+
+/**
+  USB hub control transfer to set the hub depth.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice      Indicates the hub controller device.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Depth of the hub is set.
+  @retval Others            Failed to set the depth.
+
+**/
+EFI_STATUS
+PeiUsbHubCtrlSetHubDepth (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN PEI_USB_DEVICE             *PeiUsbDevice,
+  IN PEI_USB_IO_PPI             *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST        DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  //
+  // Fill Device request packet
+  //
+  DevReq.RequestType = USB_RT_HUB;
+  DevReq.Request     = USB_HUB_REQ_SET_DEPTH;
+  DevReq.Value       = PeiUsbDevice->Tier;
+  DevReq.Length      = 0;
+
+  return  UsbIoPpi->UsbControlTransfer (
+                      PeiServices,
+                      UsbIoPpi,
+                      &DevReq,
+                      EfiUsbNoData,
+                      PcdGet32 (PcdUsbTransferTimeoutValue),
+                      NULL,
+                      0
+                      );
+}
+
+/**
+  Configure a given hub.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice   Indicating the hub controller device that will be configured
+
+  @retval EFI_SUCCESS       Hub configuration is done successfully.
+  @retval EFI_DEVICE_ERROR  Cannot configure the hub due to a hardware error.
+
+**/
+EFI_STATUS
+PeiDoHubConfig (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_DEVICE      *PeiUsbDevice
+  )
+{
+  EFI_USB_HUB_DESCRIPTOR  HubDescriptor;
+  EFI_STATUS              Status;
+  EFI_USB_HUB_STATUS      HubStatus;
+  UINTN                   Index;
+  PEI_USB_IO_PPI          *UsbIoPpi;
+
+  ZeroMem (&HubDescriptor, sizeof (HubDescriptor));
+  UsbIoPpi = &PeiUsbDevice->UsbIoPpi;
+
+  //
+  // Get the hub descriptor 
+  //
+  Status = PeiUsbHubReadDesc (
+            PeiServices,
+            PeiUsbDevice,
+            UsbIoPpi,
+            &HubDescriptor
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  PeiUsbDevice->DownStreamPortNo = HubDescriptor.NbrPorts;
+
+  if (PeiUsbDevice->DeviceSpeed == EFI_USB_SPEED_SUPER) {
+    DEBUG ((EFI_D_INFO, "PeiDoHubConfig: Set Hub Depth as 0x%x\n", PeiUsbDevice->Tier));
+    PeiUsbHubCtrlSetHubDepth (
+      PeiServices,
+      PeiUsbDevice,
+      UsbIoPpi
+      );
+  } else {
+    //
+    //  Power all the hub ports
+    //
+    for (Index = 0; Index < PeiUsbDevice->DownStreamPortNo; Index++) {
+      Status = PeiHubSetPortFeature (
+                PeiServices,
+                UsbIoPpi,
+                (UINT8) (Index + 1),
+                EfiUsbPortPower
+                );
+      if (EFI_ERROR (Status)) {
+        DEBUG (( EFI_D_ERROR, "PeiDoHubConfig: PeiHubSetPortFeature EfiUsbPortPower failed %x\n", Index));
+        continue;
+      }
+    }
+
+    DEBUG (( EFI_D_INFO, "PeiDoHubConfig: HubDescriptor.PwrOn2PwrGood: 0x%x\n", HubDescriptor.PwrOn2PwrGood));
+    if (HubDescriptor.PwrOn2PwrGood > 0) {
+      MicroSecondDelay (HubDescriptor.PwrOn2PwrGood * USB_SET_PORT_POWER_STALL);
+    }
+
+    //
+    // Clear Hub Status Change
+    //
+    Status = PeiHubGetHubStatus (
+              PeiServices,
+              UsbIoPpi,
+              (UINT32 *) &HubStatus
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    } else {
+      //
+      // Hub power supply change happens
+      //
+      if ((HubStatus.HubChangeStatus & HUB_CHANGE_LOCAL_POWER) != 0) {
+        PeiHubClearHubFeature (
+          PeiServices,
+          UsbIoPpi,
+          C_HUB_LOCAL_POWER
+          );
+      }
+      //
+      // Hub change overcurrent happens
+      //
+      if ((HubStatus.HubChangeStatus & HUB_CHANGE_OVERCURRENT) != 0) {
+        PeiHubClearHubFeature (
+          PeiServices,
+          UsbIoPpi,
+          C_HUB_OVER_CURRENT
+          );
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Send reset signal over the given root hub port.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  PortNum        Usb hub port number (starting from 1).
+
+**/
+VOID
+PeiResetHubPort (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               PortNum
+  )
+{
+  EFI_STATUS          Status;
+  UINTN               Index;
+  EFI_USB_PORT_STATUS HubPortStatus;
+
+  MicroSecondDelay (100 * 1000);
+
+  //
+  // reset root port
+  //
+  PeiHubSetPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortReset
+    );
+
+  //
+  // Drive the reset signal for worst 20ms. Check USB 2.0 Spec
+  // section 7.1.7.5 for timing requirements.
+  //
+  MicroSecondDelay (USB_SET_PORT_RESET_STALL);
+
+  //
+  // Check USB_PORT_STAT_C_RESET bit to see if the resetting state is done.
+  //
+  ZeroMem (&HubPortStatus, sizeof (EFI_USB_PORT_STATUS));
+
+  for (Index = 0; Index < USB_WAIT_PORT_STS_CHANGE_LOOP; Index++) {
+    Status = PeiHubGetPortStatus (
+               PeiServices,
+               UsbIoPpi,
+               PortNum,
+               (UINT32 *) &HubPortStatus
+               );
+
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+
+    if (USB_BIT_IS_SET (HubPortStatus.PortChangeStatus, USB_PORT_STAT_C_RESET)) {
+      break;
+    }
+
+    MicroSecondDelay (USB_WAIT_PORT_STS_CHANGE_STALL);
+  }
+
+  if (Index == USB_WAIT_PORT_STS_CHANGE_LOOP) {
+    DEBUG ((EFI_D_ERROR, "PeiResetHubPort: reset not finished in time on port %d\n", PortNum));
+    return;
+  }
+
+  //
+  // clear reset change root port
+  //
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortResetChange
+    );
+
+  MicroSecondDelay (1 * 1000);
+
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortConnectChange
+    );
+
+  //
+  // Set port enable
+  //
+  PeiHubSetPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortEnable
+    );
+
+  //
+  // Clear any change status
+  //
+
+  PeiHubClearPortFeature (
+    PeiServices,
+    UsbIoPpi,
+    PortNum,
+    EfiUsbPortEnableChange
+    );
+
+  MicroSecondDelay (10 * 1000);
+
+  return;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h
new file mode 100644
index 0000000000..f50bc63501
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/HubPeim.h
@@ -0,0 +1,281 @@
+/** @file
+Constants definitions for Usb Hub Peim
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _PEI_HUB_PEIM_H_
+#define _PEI_HUB_PEIM_H_
+
+
+//
+// Hub feature numbers
+//
+#define C_HUB_LOCAL_POWER   0
+#define C_HUB_OVER_CURRENT  1
+
+//
+// Hub class code & sub class code
+//
+#define CLASS_CODE_HUB      0x09
+#define SUB_CLASS_CODE_HUB  0
+
+//
+// Hub Status & Hub Change bit masks
+//
+#define HUB_STATUS_LOCAL_POWER  0x0001
+#define HUB_STATUS_OVERCURRENT  0x0002
+
+#define HUB_CHANGE_LOCAL_POWER  0x0001
+#define HUB_CHANGE_OVERCURRENT  0x0002
+
+//
+// Hub Characteristics
+//
+#define HUB_CHAR_LPSM     0x0003
+#define HUB_CHAR_COMPOUND 0x0004
+#define HUB_CHAR_OCPM     0x0018
+
+//
+// Standard hub request and request type
+// By [Spec-USB20/Chapter-11.24]
+//
+#define USB_HUB_CLEAR_FEATURE               0x01
+#define USB_HUB_CLEAR_FEATURE_REQ_TYPE      0x20
+
+#define USB_HUB_CLEAR_FEATURE_PORT          0x01
+#define USB_HUB_CLEAR_FEATURE_PORT_REQ_TYPE 0x23
+
+#define USB_HUB_GET_BUS_STATE               0x02
+#define USB_HUB_GET_BUS_STATE_REQ_TYPE      0xA3
+
+#define USB_HUB_GET_DESCRIPTOR              0x06
+#define USB_HUB_GET_DESCRIPTOR_REQ_TYPE     0xA0
+
+#define USB_HUB_GET_HUB_STATUS              0x00
+#define USB_HUB_GET_HUB_STATUS_REQ_TYPE     0xA0
+
+#define USB_HUB_GET_PORT_STATUS             0x00
+#define USB_HUB_GET_PORT_STATUS_REQ_TYPE    0xA3
+
+#define USB_HUB_SET_DESCRIPTOR              0x07
+#define USB_HUB_SET_DESCRIPTOR_REQ_TYPE     0x20
+
+#define USB_HUB_SET_HUB_FEATURE             0x03
+#define USB_HUB_SET_HUB_FEATURE_REQ_TYPE    0x20
+
+#define USB_HUB_SET_PORT_FEATURE            0x03
+#define USB_HUB_SET_PORT_FEATURE_REQ_TYPE   0x23
+
+#define USB_RT_HUB        (USB_TYPE_CLASS | USB_RECIP_DEVICE)
+#define USB_RT_PORT       (USB_TYPE_CLASS | USB_RECIP_OTHER)
+
+#define USB_HUB_REQ_SET_DEPTH               12
+
+#define MAXBYTES  8
+#pragma pack(1)
+//
+// Hub descriptor, the last two fields are of variable lenght.
+//
+typedef struct {
+  UINT8 Length;
+  UINT8 DescriptorType;
+  UINT8 NbrPorts;
+  UINT8 HubCharacteristics[2];
+  UINT8 PwrOn2PwrGood;
+  UINT8 HubContrCurrent;
+  UINT8 Filler[MAXBYTES];
+} EFI_USB_HUB_DESCRIPTOR;
+
+typedef struct {
+  UINT16  HubStatus;
+  UINT16  HubChangeStatus;
+} EFI_USB_HUB_STATUS;
+
+#pragma pack()
+/**
+  Get a given hub port status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  PortStatus    Current Hub port status and change status.
+
+  @retval EFI_SUCCESS       Port status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the port status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetPortStatus (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  OUT UINT32                *PortStatus
+  );
+
+/**
+  Set specified feature to a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetPortFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  IN  UINT8                 Value
+  );
+
+/**
+  Set specified feature to a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         New feature value.
+
+  @retval EFI_SUCCESS       Port feature is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubSetHubFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Value
+  );
+
+/**
+  Get a given hub status.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  HubStatus     Current Hub status and change status.
+
+  @retval EFI_SUCCESS       Hub status is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub status due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubGetHubStatus (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  OUT UINT32                *HubStatus
+  );
+
+/**
+  Clear specified feature on a given hub port.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Port          Usb hub port number (starting from 1).
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Port feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the port feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearPortFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Port,
+  IN  UINT8                 Value
+  );
+
+/**
+  Clear specified feature on a given hub.
+
+  @param  PeiServices   General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi      Indicates the PEI_USB_IO_PPI instance.
+  @param  Value         Feature value that will be cleared from the hub port.
+
+  @retval EFI_SUCCESS       Hub feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the hub feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiHubClearHubFeature (
+  IN EFI_PEI_SERVICES       **PeiServices,
+  IN PEI_USB_IO_PPI         *UsbIoPpi,
+  IN  UINT8                 Value
+  );
+
+/**
+  Get a given hub descriptor.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  DescriptorSize The length of Hub Descriptor buffer.
+  @param  HubDescriptor  Caller allocated buffer to store the hub descriptor if
+                         successfully returned.
+
+  @retval EFI_SUCCESS       Hub descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the hub descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiGetHubDescriptor (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINTN                    DescriptorSize,
+  OUT EFI_USB_HUB_DESCRIPTOR  *HubDescriptor
+  );
+
+/**
+  Configure a given hub.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  PeiUsbDevice   Indicating the hub controller device that will be configured
+
+  @retval EFI_SUCCESS       Hub configuration is done successfully.
+  @retval EFI_DEVICE_ERROR  Cannot configure the hub due to a hardware error.
+
+**/
+EFI_STATUS
+PeiDoHubConfig (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_DEVICE           *PeiUsbDevice
+  );
+
+/**
+  Send reset signal over the given root hub port.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi       Indicates the PEI_USB_IO_PPI instance.
+  @param  PortNum        Usb hub port number (starting from 1).
+
+**/
+VOID
+PeiResetHubPort (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *UsbIoPpi,
+  IN UINT8               PortNum
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c
new file mode 100644
index 0000000000..42be13ac3b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.c
@@ -0,0 +1,269 @@
+/** @file
+Common Libarary for PEI USB
+
+Copyright (c) 2006 - 2014, 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 "PeiUsbLib.h"
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_GET_DESCRIPTOR_REQ_TYPE;
+  DevReq.Request      = USB_DEV_GET_DESCRIPTOR;
+  DevReq.Value        = Value;
+  DevReq.Index        = Index;
+  DevReq.Length       = DescriptorLength;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbDataIn,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     Descriptor,
+                     DescriptorLength
+                     );
+}
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS       Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb address due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  DevReq.RequestType  = USB_DEV_SET_ADDRESS_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_ADDRESS;
+  DevReq.Value        = AddressValue;
+  DevReq.Index        = 0;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+
+  ASSERT (UsbIoPpi != NULL);
+
+  switch (Recipient) {
+  case EfiUsbDevice:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_D;
+    break;
+
+  case EfiUsbInterface:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_I;
+    break;
+
+  case EfiUsbEndpoint:
+    DevReq.RequestType = USB_DEV_CLEAR_FEATURE_REQ_TYPE_E;
+    break;
+  }
+
+  DevReq.Request      = USB_DEV_CLEAR_FEATURE;
+  DevReq.Value        = Value;
+  DevReq.Index        = Target;
+  DevReq.Length       = 0;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS       Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR  Cannot set the usb device due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  )
+{
+  EFI_USB_DEVICE_REQUEST  DevReq;
+  ZeroMem (&DevReq, sizeof (EFI_USB_DEVICE_REQUEST));
+
+  DevReq.RequestType  = USB_DEV_SET_CONFIGURATION_REQ_TYPE;
+  DevReq.Request      = USB_DEV_SET_CONFIGURATION;
+  DevReq.Value        = 1;
+
+  return UsbIoPpi->UsbControlTransfer (
+                     PeiServices,
+                     UsbIoPpi,
+                     &DevReq,
+                     EfiUsbNoData,
+                     PcdGet32 (PcdUsbTransferTimeoutValue),
+                     NULL,
+                     0
+                     );
+}
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  )
+{
+  //
+  // return the bit 0 value of PortStatus
+  //
+  if ((PortStatus & USB_PORT_STAT_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Get device speed according to port status.
+
+  @param    PortStatus  The usb port status gotten.
+
+  @return   Device speed value.
+
+**/
+UINTN
+PeiUsbGetDeviceSpeed (
+  IN UINT16 PortStatus
+  )
+{
+  if ((PortStatus & USB_PORT_STAT_LOW_SPEED) != 0) {
+    return EFI_USB_SPEED_LOW;
+  } else if ((PortStatus & USB_PORT_STAT_HIGH_SPEED) != 0){
+    return EFI_USB_SPEED_HIGH;
+  } else if ((PortStatus & USB_PORT_STAT_SUPER_SPEED) != 0) {
+    return EFI_USB_SPEED_SUPER;
+  } else {
+    return EFI_USB_SPEED_FULL;
+  }
+}
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  )
+{
+  //
+  // return the bit 0 value of PortChangeStatus
+  //
+  if ((PortChangeStatus & USB_PORT_STAT_C_CONNECTION) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h
new file mode 100644
index 0000000000..1ace89fbc3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/PeiUsbLib.h
@@ -0,0 +1,231 @@
+/** @file
+Common Libarary for PEI USB
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _PEI_USB_LIB_H_
+#define _PEI_USB_LIB_H_
+
+
+//
+// Standard device request and request type
+// By [Spec-USB20/Chapter-9.4]
+//
+#define USB_DEV_GET_STATUS                  0x00
+#define USB_DEV_GET_STATUS_REQ_TYPE_D       0x80 // Receiver : Device
+#define USB_DEV_GET_STATUS_REQ_TYPE_I       0x81 // Receiver : Interface
+#define USB_DEV_GET_STATUS_REQ_TYPE_E       0x82 // Receiver : Endpoint
+
+#define USB_DEV_CLEAR_FEATURE               0x01
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_D    0x00 // Receiver : Device
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_I    0x01 // Receiver : Interface
+#define USB_DEV_CLEAR_FEATURE_REQ_TYPE_E    0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_FEATURE                 0x03
+#define USB_DEV_SET_FEATURE_REQ_TYPE_D      0x00 // Receiver : Device
+#define USB_DEV_SET_FEATURE_REQ_TYPE_I      0x01 // Receiver : Interface
+#define USB_DEV_SET_FEATURE_REQ_TYPE_E      0x02 // Receiver : Endpoint
+
+#define USB_DEV_SET_ADDRESS                 0x05
+#define USB_DEV_SET_ADDRESS_REQ_TYPE        0x00
+
+#define USB_DEV_GET_DESCRIPTOR              0x06
+#define USB_DEV_GET_DESCRIPTOR_REQ_TYPE     0x80
+
+#define USB_DEV_SET_DESCRIPTOR              0x07
+#define USB_DEV_SET_DESCRIPTOR_REQ_TYPE     0x00
+
+#define USB_DEV_GET_CONFIGURATION           0x08
+#define USB_DEV_GET_CONFIGURATION_REQ_TYPE  0x80
+
+#define USB_DEV_SET_CONFIGURATION           0x09
+#define USB_DEV_SET_CONFIGURATION_REQ_TYPE  0x00
+
+#define USB_DEV_GET_INTERFACE               0x0A
+#define USB_DEV_GET_INTERFACE_REQ_TYPE      0x81
+
+#define USB_DEV_SET_INTERFACE               0x0B
+#define USB_DEV_SET_INTERFACE_REQ_TYPE      0x01
+
+#define USB_DEV_SYNCH_FRAME                 0x0C
+#define USB_DEV_SYNCH_FRAME_REQ_TYPE        0x82
+
+//
+// USB Descriptor types
+//
+#define USB_DT_DEVICE     0x01
+#define USB_DT_CONFIG     0x02
+#define USB_DT_STRING     0x03
+#define USB_DT_INTERFACE  0x04
+#define USB_DT_ENDPOINT   0x05
+#define USB_DT_HUB        0x29
+#define USB_DT_SUPERSPEED_HUB 0x2A
+#define USB_DT_HID        0x21
+
+//
+// USB request type
+//
+#define USB_TYPE_STANDARD (0x00 << 5)
+#define USB_TYPE_CLASS    (0x01 << 5)
+#define USB_TYPE_VENDOR   (0x02 << 5)
+#define USB_TYPE_RESERVED (0x03 << 5)
+
+//
+// USB request targer device
+//
+#define USB_RECIP_DEVICE    0x00
+#define USB_RECIP_INTERFACE 0x01
+#define USB_RECIP_ENDPOINT  0x02
+#define USB_RECIP_OTHER     0x03
+
+typedef enum {
+  EfiUsbEndpointHalt,
+  EfiUsbDeviceRemoteWakeup
+} EFI_USB_STANDARD_FEATURE_SELECTOR;
+
+//
+// Usb Data recipient type
+//
+typedef enum {
+  EfiUsbDevice,
+  EfiUsbInterface,
+  EfiUsbEndpoint
+} EFI_USB_RECIPIENT;
+
+/**
+  Get a given usb descriptor.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  Value              Request Value.
+  @param  Index              Request Index.
+  @param  DescriptorLength   Request descriptor Length.
+  @param  Descriptor         Request descriptor.
+
+
+  @retval EFI_SUCCESS       Usb descriptor is obtained successfully.
+  @retval EFI_DEVICE_ERROR  Cannot get the usb descriptor due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbGetDescriptor (
+  IN  EFI_PEI_SERVICES         **PeiServices,
+  IN  PEI_USB_IO_PPI           *UsbIoPpi,
+  IN  UINT16                   Value,
+  IN  UINT16                   Index,
+  IN  UINT16                   DescriptorLength,
+  OUT VOID                     *Descriptor
+  );
+
+/**
+  Set a usb device with a specified address.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+  @param  AddressValue       The address to assign.
+
+  @retval EFI_SUCCESS        Usb device address is set successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb address due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetDeviceAddress (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN UINT16                   AddressValue
+  );
+
+/**
+  Clear a given usb feature.
+
+  @param  PeiServices       General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi          Indicates the PEI_USB_IO_PPI instance.
+  @param  Recipient         The recipient of ClearFeature Request, should be one of Device/Interface/Endpoint.
+  @param  Value             Request Value.
+  @param  Target            Request Index.
+
+  @retval EFI_SUCCESS       Usb feature is cleared successfully.
+  @retval EFI_DEVICE_ERROR  Cannot clear the usb feature due to a hardware error.
+  @retval Others            Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbClearDeviceFeature (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi,
+  IN EFI_USB_RECIPIENT        Recipient,
+  IN UINT16                   Value,
+  IN UINT16                   Target
+  );
+
+/**
+  Configure a usb device to Configuration 1.
+
+  @param  PeiServices        General-purpose services that are available to every PEIM.
+  @param  UsbIoPpi           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS        Usb device is set to use Configuration 1 successfully.
+  @retval EFI_DEVICE_ERROR   Cannot set the usb device due to a hardware error.
+  @retval Others             Other failure occurs.
+
+**/
+EFI_STATUS
+PeiUsbSetConfiguration (
+  IN EFI_PEI_SERVICES         **PeiServices,
+  IN PEI_USB_IO_PPI           *UsbIoPpi
+  );
+
+/**
+  Judge if the port is connected with a usb device or not.
+
+  @param  PortStatus  The usb port status gotten.
+
+  @retval TRUE        A usb device is connected with the port.
+  @retval FALSE       No usb device is connected with the port.
+
+**/
+BOOLEAN
+IsPortConnect (
+  IN UINT16  PortStatus
+  );
+
+/**
+  Get device speed according to port status.
+
+  @param    PortStatus  The usb port status gotten.
+
+  @return   Device speed value.
+
+**/
+UINTN
+PeiUsbGetDeviceSpeed (
+  IN UINT16 PortStatus
+  );
+
+/**
+  Judge if the port is in "connection change" status or not.
+
+  @param  PortChangeStatus  The usb port change status gotten.
+
+  @retval TRUE              The port is in "connection change" status.
+  @retval FALSE             The port is NOT in "connection change" status.
+
+**/
+BOOLEAN
+IsPortConnectChange (
+  IN UINT16  PortChangeStatus
+  );
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf
new file mode 100644
index 0000000000..734619a2fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf
@@ -0,0 +1,67 @@
+## @file
+# The Usb Bus Peim driver is used to support recovery from usb device.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbBusPei
+  MODULE_UNI_FILE                = UsbBusPei.uni
+  FILE_GUID                      = 8401A045-6F70-4505-8471-7015B40355E3
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = PeimInitializeUsb
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PeiUsbLib.c
+  HubPeim.c
+  UsbIoPeim.c
+  UsbPeim.c
+  UsbPeim.h
+  PeiUsbLib.h
+  HubPeim.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  TimerLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  PcdLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUsbTransferTimeoutValue  ## CONSUMES
+  
+[Ppis]
+  gPeiUsbIoPpiGuid                              ## PRODUCES
+  gPeiUsbHostControllerPpiGuid                  ## SOMETIMES_CONSUMES
+  gPeiUsb2HostControllerPpiGuid                 ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid AND gPeiUsb2HostControllerPpiGuid OR gPeiUsbHostControllerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni
new file mode 100644
index 0000000000..66a8c01eda
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni
new file mode 100644
index 0000000000..3520fdba45
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPeiExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c
new file mode 100644
index 0000000000..d13a7ee0a3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbIoPeim.c
@@ -0,0 +1,372 @@
+/** @file
+The module is used to implement Usb Io PPI interfaces.
+
+Copyright (c) 2006 - 2014, 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 "PeiUsbLib.h"
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_IO_PPI.
+  @param  Request                USB device request to send.
+  @param  Direction              Specifies the data direction for the data stage.
+  @param  Timeout                Indicates the maximum timeout, in millisecond. If Timeout
+                                 is 0, then the caller must wait for the function to be
+                                 completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbControlTransfer (
+  IN     EFI_PEI_SERVICES          **PeiServices,
+  IN     PEI_USB_IO_PPI            *This,
+  IN     EFI_USB_DEVICE_REQUEST    *Request,
+  IN     EFI_USB_DATA_DIRECTION    Direction,
+  IN     UINT32                    Timeout,
+  IN OUT VOID                      *Data,      OPTIONAL
+  IN     UINTN                     DataLength  OPTIONAL
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_USB_DEVICE              *PeiUsbDev;
+  UINT32                      TransferResult;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EndpointDescriptor;
+  UINT8                       EndpointIndex;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  EndpointDescriptor = NULL;
+  EndpointIndex = 0;
+
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_DEV_CLEAR_FEATURE_REQ_TYPE_E) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+    //
+    // Request->Index is the Endpoint Address, use it to get the Endpoint Index.
+    //
+    while (EndpointIndex < MAX_ENDPOINT) {
+      Status = PeiUsbGetEndpointDescriptor (PeiServices, This, EndpointIndex, &EndpointDescriptor);
+      if (EFI_ERROR (Status)) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      if (EndpointDescriptor->EndpointAddress == Request->Index) {
+        break;
+      }
+
+      EndpointIndex++;
+    }
+
+    if (EndpointIndex == MAX_ENDPOINT) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  if (PeiUsbDev->Usb2HcPpi != NULL) {
+    Status = PeiUsbDev->Usb2HcPpi->ControlTransfer (
+                        PeiServices,
+                        PeiUsbDev->Usb2HcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        PeiUsbDev->DeviceSpeed,
+                        PeiUsbDev->MaxPacketSize0,
+                        Request,
+                        Direction,
+                        Data,
+                        &DataLength,
+                        Timeout,
+                        &(PeiUsbDev->Translator),
+                        &TransferResult
+                        );
+  } else {
+    Status = PeiUsbDev->UsbHcPpi->ControlTransfer (
+                        PeiServices,
+                        PeiUsbDev->UsbHcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        PeiUsbDev->DeviceSpeed,
+                        (UINT8) PeiUsbDev->MaxPacketSize0,
+                        Request,
+                        Direction,
+                        Data,
+                        &DataLength,
+                        Timeout,
+                        &TransferResult
+                        );
+  }
+
+  //
+  // Reset the endpoint toggle when endpoint stall is cleared
+  //
+  if ((Request->Request     == USB_REQ_CLEAR_FEATURE) &&
+      (Request->RequestType == USB_DEV_CLEAR_FEATURE_REQ_TYPE_E) &&
+      (Request->Value       == USB_FEATURE_ENDPOINT_HALT)) {
+    if ((PeiUsbDev->DataToggle & (1 << EndpointIndex)) != 0) {
+      PeiUsbDev->DataToggle = (UINT16) (PeiUsbDev->DataToggle ^ (1 << EndpointIndex));
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "PeiUsbControlTransfer: %r\n", Status));
+  return Status;
+}
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_IO_PPI.
+  @param  DeviceEndpoint        Endpoint number and its direction in bit 7.
+  @param  Data                  A pointer to the buffer of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete. If Timeout is 0, then
+                                the caller must wait for the function to be completed
+                                until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbBulkTransfer (
+  IN     EFI_PEI_SERVICES    **PeiServices,
+  IN     PEI_USB_IO_PPI      *This,
+  IN     UINT8               DeviceEndpoint,
+  IN OUT VOID                *Data,
+  IN OUT UINTN               *DataLength,
+  IN     UINTN               Timeout
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_USB_DEVICE              *PeiUsbDev;
+  UINT32                      TransferResult;
+  UINTN                       MaxPacketLength;
+  UINT8                       DataToggle;
+  UINT8                       OldToggle;
+  EFI_USB_ENDPOINT_DESCRIPTOR *EndpointDescriptor;
+  UINT8                       EndpointIndex;
+  VOID                        *Data2[EFI_USB_MAX_BULK_BUFFER_NUM];
+
+  PeiUsbDev     = PEI_USB_DEVICE_FROM_THIS (This);
+
+  EndpointDescriptor = NULL;
+  EndpointIndex = 0;
+  Data2[0] = Data;
+  Data2[1] = NULL;
+
+  while (EndpointIndex < MAX_ENDPOINT) {
+    Status = PeiUsbGetEndpointDescriptor (PeiServices, This, EndpointIndex, &EndpointDescriptor);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (EndpointDescriptor->EndpointAddress == DeviceEndpoint) {
+      break;
+    }
+
+    EndpointIndex++;
+  }
+
+  if (EndpointIndex == MAX_ENDPOINT) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MaxPacketLength = PeiUsbDev->EndpointDesc[EndpointIndex]->MaxPacketSize;
+  if ((PeiUsbDev->DataToggle & (1 << EndpointIndex)) != 0) {
+    DataToggle = 1;
+  } else {
+    DataToggle = 0;
+  }
+
+  OldToggle = DataToggle;
+
+  if (PeiUsbDev->Usb2HcPpi != NULL) {
+    Status = PeiUsbDev->Usb2HcPpi->BulkTransfer (
+                        PeiServices,
+                        PeiUsbDev->Usb2HcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        DeviceEndpoint,
+                        PeiUsbDev->DeviceSpeed,
+                        MaxPacketLength,
+                        Data2,
+                        DataLength,
+                        &DataToggle,
+                        Timeout,
+                        &(PeiUsbDev->Translator),
+                        &TransferResult
+                        );
+  } else {
+    Status = PeiUsbDev->UsbHcPpi->BulkTransfer (
+                        PeiServices,
+                        PeiUsbDev->UsbHcPpi,
+                        PeiUsbDev->DeviceAddress,
+                        DeviceEndpoint,
+                        (UINT8) MaxPacketLength,
+                        Data,
+                        DataLength,
+                        &DataToggle,
+                        Timeout,
+                        &TransferResult
+                        );
+  }
+
+  if (OldToggle != DataToggle) {
+    PeiUsbDev->DataToggle = (UINT16) (PeiUsbDev->DataToggle ^ (1 << EndpointIndex));
+  }
+
+  DEBUG ((EFI_D_INFO, "PeiUsbBulkTransfer: %r\n", Status));
+  return Status;
+}
+
+/**
+  Get the usb interface descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  InterfaceDescriptor  Request interface descriptor.
+
+
+  @retval EFI_SUCCESS          Usb interface descriptor is obtained successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetInterfaceDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR   **InterfaceDescriptor
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+  PeiUsbDev             = PEI_USB_DEVICE_FROM_THIS (This);
+  *InterfaceDescriptor  = PeiUsbDev->InterfaceDesc;
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the usb endpoint descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointIndex        The valid index of the specified endpoint.
+  @param  EndpointDescriptor   Request endpoint descriptor.
+
+  @retval EFI_SUCCESS       Usb endpoint descriptor is obtained successfully.
+  @retval EFI_NOT_FOUND     Usb endpoint descriptor is NOT found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetEndpointDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  IN  UINT8                          EndpointIndex,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR    **EndpointDescriptor
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  ASSERT (EndpointDescriptor != NULL);
+
+  //
+  // The valid range of EndpointIndex is 0..15
+  // If EndpointIndex is lesser than 15 but larger than the number of interfaces,
+  // a EFI_NOT_FOUND should be returned
+  //
+  ASSERT (EndpointIndex <= 15);
+
+  if (EndpointIndex >= PeiUsbDev->InterfaceDesc->NumEndpoints) {
+    return EFI_NOT_FOUND;
+  }
+
+  *EndpointDescriptor = PeiUsbDev->EndpointDesc[EndpointIndex];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the port and re-configure the usb device.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  This           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS    Usb device is reset and configured successfully.
+  @retval Others         Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbPortReset (
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_IO_PPI                 *This
+  )
+{
+  PEI_USB_DEVICE  *PeiUsbDev;
+  EFI_STATUS      Status;
+  UINT8           Address;
+
+  PeiUsbDev = PEI_USB_DEVICE_FROM_THIS (This);
+
+  ResetRootPort (
+    PeiServices,
+    PeiUsbDev->UsbHcPpi,
+    PeiUsbDev->Usb2HcPpi,
+    PeiUsbDev->DeviceAddress,
+    0
+    );
+
+  //
+  // Set address
+  //
+  Address                   = PeiUsbDev->DeviceAddress;
+  PeiUsbDev->DeviceAddress  = 0;
+
+  Status = PeiUsbSetDeviceAddress (
+            PeiServices,
+            This,
+            Address
+            );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PeiUsbDev->DeviceAddress = Address;
+
+  //
+  // Set default configuration
+  //
+  Status = PeiUsbSetConfiguration (
+            PeiServices,
+            This
+            );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
new file mode 100644
index 0000000000..f350021862
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.c
@@ -0,0 +1,1231 @@
+/** @file
+The module to produce Usb Bus PPI.
+
+Copyright (c) 2006 - 2014, 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;
+
+  //
+  // 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
+  )
+{
+  UINT16  DescriptorHeader;
+  UINT8   Len;
+  UINT8   *Ptr;
+  UINTN   Parsed;
+
+  Parsed  = 0;
+  Ptr     = Buffer;
+
+  while (TRUE) {
+    //
+    // Buffer length should not less than Desc length
+    //
+    if (Length < DescLength) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    DescriptorHeader  = (UINT16) (*Ptr + ((*(Ptr + 1)) << 8));
+
+    Len               = Buffer[0];
+
+    //
+    // Check to see if it is a start of expected descriptor
+    //
+    if (DescriptorHeader == ((DescType << 8) | DescLength)) {
+      break;
+    }
+
+    if ((UINT8) (DescriptorHeader >> 8) == DescType) {
+      if (Len > DescLength) {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+    //
+    // Descriptor length should be at least 2
+    // and should not exceed the buffer length
+    //
+    if (Len < 2) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (Len > Length) {
+      return EFI_DEVICE_ERROR;
+    }
+    //
+    // Skip this mismatch descriptor
+    //
+    Length -= Len;
+    Ptr += Len;
+    Parsed += Len;
+  }
+
+  *ParsedBytes = Parsed;
+
+  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;
+}
+
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h
new file mode 100644
index 0000000000..dff8eeb202
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbBusPei/UsbPeim.h
@@ -0,0 +1,253 @@
+/** @file
+Usb Peim definition.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _PEI_USB_PEIM_H_
+#define _PEI_USB_PEIM_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/UsbHostController.h>
+#include <Ppi/Usb2HostController.h>
+#include <Ppi/UsbIo.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PcdLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define MAX_INTERFACE             8
+#define MAX_ENDPOINT              16
+
+#define PEI_USB_DEVICE_SIGNATURE  SIGNATURE_32 ('U', 's', 'b', 'D')
+typedef struct {
+  UINTN                         Signature;
+  PEI_USB_IO_PPI                UsbIoPpi;
+  EFI_PEI_PPI_DESCRIPTOR        UsbIoPpiList;
+  UINT16                        MaxPacketSize0;
+  UINT16                        DataToggle;
+  UINT8                         DeviceAddress;
+  UINT8                         DeviceSpeed;
+  UINT8                         IsHub;
+  UINT8                         DownStreamPortNo;
+  UINTN                         AllocateAddress;
+  PEI_USB_HOST_CONTROLLER_PPI   *UsbHcPpi;
+  PEI_USB2_HOST_CONTROLLER_PPI  *Usb2HcPpi;
+  UINT8                         ConfigurationData[1024];
+  EFI_USB_CONFIG_DESCRIPTOR     *ConfigDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  *InterfaceDescList[MAX_INTERFACE];
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDesc[MAX_ENDPOINT];
+  EFI_USB_ENDPOINT_DESCRIPTOR   *EndpointDescList[MAX_INTERFACE][MAX_ENDPOINT];
+  EFI_USB2_HC_TRANSACTION_TRANSLATOR Translator;
+  UINT8                          Tier;
+} PEI_USB_DEVICE;
+
+#define PEI_USB_DEVICE_FROM_THIS(a) CR (a, PEI_USB_DEVICE, UsbIoPpi, PEI_USB_DEVICE_SIGNATURE)
+
+#define USB_BIT_IS_SET(Data, Bit)   ((BOOLEAN)(((Data) & (Bit)) == (Bit)))
+
+#define USB_BUS_1_MILLISECOND       1000
+
+//
+// Wait for port reset, refers to specification
+// [USB20-7.1.7.5, it says 10ms for hub and 50ms for
+// root hub]
+//
+// According to USB2.0, Chapter 11.5.1.5 Resetting,
+// the worst case for TDRST is 20ms
+//
+#define USB_SET_PORT_RESET_STALL        (20 * USB_BUS_1_MILLISECOND)
+#define USB_SET_ROOT_PORT_RESET_STALL   (50 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for clear roothub port reset, set by experience
+//
+#define USB_CLR_ROOT_PORT_RESET_STALL   (20 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for port statue reg change, set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_STALL  (100)
+
+//
+// Host software return timeout if port status doesn't change 
+// after 500ms(LOOP * STALL = 5000 * 0.1ms), set by experience
+//
+#define USB_WAIT_PORT_STS_CHANGE_LOOP   5000
+
+//
+// Wait for hub port power-on, refers to specification
+// [USB20-11.23.2]
+//
+#define USB_SET_PORT_POWER_STALL        (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for set device address, refers to specification
+// [USB20-9.2.6.3, it says 2ms]
+//
+#define USB_SET_DEVICE_ADDRESS_STALL    (2 * USB_BUS_1_MILLISECOND)
+
+//
+// Wait for get configuration descriptor, set by experience
+//
+#define USB_GET_CONFIG_DESCRIPTOR_STALL (1 * USB_BUS_1_MILLISECOND)
+
+/**
+  Submits control transfer to a target USB device.
+  
+  @param  PeiServices            The pointer of EFI_PEI_SERVICES.
+  @param  This                   The pointer of PEI_USB_IO_PPI.
+  @param  Request                USB device request to send.
+  @param  Direction              Specifies the data direction for the data stage.
+  @param  Timeout                Indicates the maximum timeout, in millisecond. If Timeout
+                                 is 0, then the caller must wait for the function to be
+                                 completed until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+  @param  Data                   Data buffer to be transmitted or received from USB device.
+  @param  DataLength             The size (in bytes) of the data buffer.
+
+  @retval EFI_SUCCESS            Transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   The transfer failed due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_TIMEOUT            Transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR       Transfer failed due to host controller or device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbControlTransfer (
+  IN     EFI_PEI_SERVICES          **PeiServices,
+  IN     PEI_USB_IO_PPI            *This,
+  IN     EFI_USB_DEVICE_REQUEST    *Request,
+  IN     EFI_USB_DATA_DIRECTION    Direction,
+  IN     UINT32                    Timeout,
+  IN OUT VOID                      *Data,      OPTIONAL
+  IN     UINTN                     DataLength  OPTIONAL
+  );
+
+/**
+  Submits bulk transfer to a bulk endpoint of a USB device.
+  
+  @param  PeiServices           The pointer of EFI_PEI_SERVICES.
+  @param  This                  The pointer of PEI_USB_IO_PPI.
+  @param  DeviceEndpoint        Endpoint number and its direction in bit 7.
+  @param  Data                  A pointer to the buffer of data to transmit 
+                                from or receive into.
+  @param  DataLength            The lenght of the data buffer.
+  @param  Timeout               Indicates the maximum time, in millisecond, which the
+                                transfer is allowed to complete. If Timeout is 0, then
+                                the caller must wait for the function to be completed
+                                until EFI_SUCCESS or EFI_DEVICE_ERROR is returned.
+
+  @retval EFI_SUCCESS           The transfer was completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The transfer failed due to lack of resource.
+  @retval EFI_INVALID_PARAMETER Parameters are invalid.
+  @retval EFI_TIMEOUT           The transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR      The transfer failed due to host controller error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbBulkTransfer (
+  IN     EFI_PEI_SERVICES    **PeiServices,
+  IN     PEI_USB_IO_PPI      *This,
+  IN     UINT8               DeviceEndpoint,
+  IN OUT VOID                *Data,
+  IN OUT UINTN               *DataLength,
+  IN     UINTN               Timeout
+  );
+
+/**
+  Get the usb interface descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  InterfaceDescriptor  Request interface descriptor.
+
+
+  @retval EFI_SUCCESS          Usb interface descriptor is obtained successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetInterfaceDescriptor (
+  IN  EFI_PEI_SERVICES                **PeiServices,
+  IN  PEI_USB_IO_PPI                  *This,
+  OUT EFI_USB_INTERFACE_DESCRIPTOR    **InterfaceDescriptor
+  );
+
+/**
+  Get the usb endpoint descriptor.
+
+  @param  PeiServices          General-purpose services that are available to every PEIM.
+  @param  This                 Indicates the PEI_USB_IO_PPI instance.
+  @param  EndpointIndex        The valid index of the specified endpoint.
+  @param  EndpointDescriptor   Request endpoint descriptor.
+
+  @retval EFI_SUCCESS       Usb endpoint descriptor is obtained successfully.
+  @retval EFI_NOT_FOUND     Usb endpoint descriptor is NOT found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbGetEndpointDescriptor (
+  IN  EFI_PEI_SERVICES               **PeiServices,
+  IN  PEI_USB_IO_PPI                 *This,
+  IN  UINT8                          EndpointIndex,
+  OUT EFI_USB_ENDPOINT_DESCRIPTOR    **EndpointDescriptor
+  );
+
+/**
+  Reset the port and re-configure the usb device.
+
+  @param  PeiServices    General-purpose services that are available to every PEIM.
+  @param  This           Indicates the PEI_USB_IO_PPI instance.
+
+  @retval EFI_SUCCESS    Usb device is reset and configured successfully.
+  @retval Others         Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiUsbPortReset (
+  IN EFI_PEI_SERVICES    **PeiServices,
+  IN PEI_USB_IO_PPI      *This
+  );
+
+/**
+  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
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c
new file mode 100644
index 0000000000..186bdbe950
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/ComponentName.c
@@ -0,0 +1,223 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Keyboard driver.
+
+Copyright (c) 2004 - 2011, 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 "KeyBoard.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbKeyboardComponentName = {
+  UsbKeyboardComponentNameGetDriverName,
+  UsbKeyboardComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbKeyboardComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbKeyboardComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbKeyboardComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbKeyboardDriverNameTable[] = {
+  { "eng;en", L"Usb Keyboard Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbKeyboardDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbKeyboardComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                  Status;
+  USB_KB_DEV                  *UsbKbDev;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL *SimpleTxtIn;
+  EFI_USB_IO_PROTOCOL         *UsbIoProtocol;
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbKeyboardDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbKeyboardDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextInProtocolGuid,
+                  (VOID **) &SimpleTxtIn,
+                  gUsbKeyboardDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbKbDev = USB_KB_DEV_FROM_THIS (SimpleTxtIn);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbKbDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbKeyboardComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c
new file mode 100644
index 0000000000..fb7558b730
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.c
@@ -0,0 +1,1172 @@
+/** @file
+  USB Keyboard Driver that manages USB keyboard and produces Simple Text Input
+  Protocol and Simple Text Input Ex Protocol.
+
+Copyright (c) 2004 - 2012, 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 "EfiKey.h"
+#include "KeyBoard.h"
+
+//
+// USB Keyboard Driver Global Variables
+//
+EFI_DRIVER_BINDING_PROTOCOL gUsbKeyboardDriverBinding = {
+  USBKeyboardDriverBindingSupported,
+  USBKeyboardDriverBindingStart,
+  USBKeyboardDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Keyboard Driver.
+
+  This function is the entrypoint of USB Keyboard Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbKeyboardDriverBinding,
+             ImageHandle,
+             &gUsbKeyboardComponentName,
+             &gUsbKeyboardComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether USB keyboard driver supports this device.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  //
+  // Check if USB I/O Protocol is attached on the controller handle.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a keyboard device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+
+  if (!IsUSBKeyboard (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Starts the keyboard device with this driver.
+
+  This function produces Simple Text Input Protocol and Simple Text Input Ex Protocol,
+  initializes the keyboard device, and submit Asynchronous Interrupt Transfer to manage
+  this keyboard device.
+
+  @param  This                   The USB keyboard driver binding instance.
+  @param  Controller             Handle of device to bind driver to.
+  @param  RemainingDevicePath    Optional parameter use to pick a specific child
+                                 device to start.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb keyboard driver.
+  @retval EFI_UNSUPPORTED        No interrupt endpoint can be found.
+  @retval Other                  This controller cannot be started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  USB_KB_DEV                    *UsbKeyboardDevice;
+  UINT8                         EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndpointDescriptor;
+  UINT8                         Index;
+  UINT8                         EndpointAddr;
+  UINT8                         PollingInterval;
+  UINT8                         PacketSize;
+  BOOLEAN                       Found;
+  EFI_TPL                       OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+
+  UsbKeyboardDevice = AllocateZeroPool (sizeof (USB_KB_DEV));
+  ASSERT (UsbKeyboardDevice != NULL);
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbKeyboardDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+  //
+  // Report that the USB keyboard is being enabled
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_ENABLE),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // This is pretty close to keyboard detection, so log progress
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_PRESENCE_DETECT),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  UsbKeyboardDevice->UsbIo = UsbIo;
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbKeyboardDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbKeyboardDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem(&UsbKeyboardDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB keyboard
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_DETECTED),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  UsbKeyboardDevice->Signature                  = USB_KB_DEV_SIGNATURE;
+  UsbKeyboardDevice->SimpleInput.Reset          = USBKeyboardReset;
+  UsbKeyboardDevice->SimpleInput.ReadKeyStroke  = USBKeyboardReadKeyStroke;
+
+  UsbKeyboardDevice->SimpleInputEx.Reset               = USBKeyboardResetEx;
+  UsbKeyboardDevice->SimpleInputEx.ReadKeyStrokeEx     = USBKeyboardReadKeyStrokeEx;
+  UsbKeyboardDevice->SimpleInputEx.SetState            = USBKeyboardSetState;
+  UsbKeyboardDevice->SimpleInputEx.RegisterKeyNotify   = USBKeyboardRegisterKeyNotify;
+  UsbKeyboardDevice->SimpleInputEx.UnregisterKeyNotify = USBKeyboardUnregisterKeyNotify;
+
+  InitializeListHead (&UsbKeyboardDevice->NotifyList);
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  USBKeyboardTimerHandler,
+                  UsbKeyboardDevice,
+                  &UsbKeyboardDevice->TimerEvent
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->SetTimer (UsbKeyboardDevice->TimerEvent, TimerPeriodic, KEYBOARD_TIMER_INTERVAL);
+  }
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  USBKeyboardWaitForKey,
+                  UsbKeyboardDevice,
+                  &(UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx)
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  USBKeyboardWaitForKey,
+                  UsbKeyboardDevice,
+                  &(UsbKeyboardDevice->SimpleInput.WaitForKey)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Install Simple Text Input Protocol and Simple Text Input Ex Protocol
+  // for the USB keyboard device.
+  // USB keyboard is a hot plug device, and expected to work immediately
+  // when plugging into system, other conventional console devices could
+  // distinguish it by its device path.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInput,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInputEx,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  UsbKeyboardDevice->ControllerHandle = Controller;
+  Status = InitKeyboardLayout (UsbKeyboardDevice);
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+      Controller,
+      &gEfiSimpleTextInProtocolGuid,
+      &UsbKeyboardDevice->SimpleInput,
+      &gEfiSimpleTextInputExProtocolGuid,
+      &UsbKeyboardDevice->SimpleInputEx,
+      NULL
+      );
+    goto ErrorExit;
+  }
+
+
+  //
+  // Reset USB Keyboard Device exhaustively.
+  //
+  Status = UsbKeyboardDevice->SimpleInputEx.Reset (
+                                            &UsbKeyboardDevice->SimpleInputEx,
+                                            TRUE
+                                            );
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiSimpleTextInProtocolGuid,
+           &UsbKeyboardDevice->SimpleInput,
+           &gEfiSimpleTextInputExProtocolGuid,
+           &UsbKeyboardDevice->SimpleInputEx,
+           NULL
+           );
+    goto ErrorExit;
+  }
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbKeyboardDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbKeyboardDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    KeyboardHandler,
+                    UsbKeyboardDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Controller,
+           &gEfiSimpleTextInProtocolGuid,
+           &UsbKeyboardDevice->SimpleInput,
+           &gEfiSimpleTextInputExProtocolGuid,
+           &UsbKeyboardDevice->SimpleInputEx,
+           NULL
+           );
+    goto ErrorExit;
+  }
+
+  UsbKeyboardDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbKeyboardComponentName.SupportedLanguages,
+    &UsbKeyboardDevice->ControllerNameTable,
+    L"Generic Usb Keyboard",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUsbKeyboardComponentName2.SupportedLanguages,
+    &UsbKeyboardDevice->ControllerNameTable,
+    L"Generic Usb Keyboard",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (UsbKeyboardDevice != NULL) {
+    if (UsbKeyboardDevice->TimerEvent != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->TimerEvent);
+    }
+    if (UsbKeyboardDevice->SimpleInput.WaitForKey != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->SimpleInput.WaitForKey);
+    }
+    if (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx != NULL) {
+      gBS->CloseEvent (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx);
+    }
+    if (UsbKeyboardDevice->KeyboardLayoutEvent != NULL) {
+      ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+      gBS->CloseEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+    }
+    FreePool (UsbKeyboardDevice);
+    UsbKeyboardDevice = NULL;
+  }
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+
+}
+
+
+/**
+  Stop the USB keyboard device handled by this driver.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Text In Protocol or Simple Text In Ex Protocol
+                                 is not installed on Controller.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL *SimpleInput;
+  USB_KB_DEV                     *UsbKeyboardDevice;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  (VOID **) &SimpleInput,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (SimpleInput);
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_DISABLE),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbKeyboardDevice->UsbIo->UsbAsyncInterruptTransfer (
+                              UsbKeyboardDevice->UsbIo,
+                              UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+                              FALSE,
+                              UsbKeyboardDevice->IntEndpointDescriptor.Interval,
+                              0,
+                              NULL,
+                              NULL
+                              );
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  Controller,
+                  &gEfiSimpleTextInProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInput,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  &UsbKeyboardDevice->SimpleInputEx,
+                  NULL
+                  );
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbKeyboardDevice->TimerEvent);
+  gBS->CloseEvent (UsbKeyboardDevice->RepeatTimer);
+  gBS->CloseEvent (UsbKeyboardDevice->DelayedRecoveryEvent);
+  gBS->CloseEvent (UsbKeyboardDevice->SimpleInput.WaitForKey);
+  gBS->CloseEvent (UsbKeyboardDevice->SimpleInputEx.WaitForKeyEx);
+  KbdFreeNotifyList (&UsbKeyboardDevice->NotifyList);
+
+  ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+  gBS->CloseEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+
+  if (UsbKeyboardDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbKeyboardDevice->ControllerNameTable);
+  }
+
+  DestroyQueue (&UsbKeyboardDevice->UsbKeyQueue);
+  DestroyQueue (&UsbKeyboardDevice->EfiKeyQueue);
+
+  FreePool (UsbKeyboardDevice);
+
+  return Status;
+}
+
+/**
+  Internal function to read the next keystroke from the keyboard buffer.
+
+  @param  UsbKeyboardDevice       USB keyboard's private structure.
+  @param  KeyData                 A pointer to buffer to hold the keystroke
+                                  data for the key that was pressed.
+
+  @retval EFI_SUCCESS             The keystroke information was returned.
+  @retval EFI_NOT_READY           There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR        The keystroke information was not returned due to
+                                  hardware errors.
+  @retval EFI_INVALID_PARAMETER   KeyData is NULL.
+  @retval Others                  Fail to translate keycode into EFI_INPUT_KEY
+
+**/
+EFI_STATUS
+USBKeyboardReadKeyStrokeWorker (
+  IN OUT USB_KB_DEV                 *UsbKeyboardDevice,
+     OUT EFI_KEY_DATA               *KeyData
+  )
+{
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsQueueEmpty (&UsbKeyboardDevice->EfiKeyQueue)) {
+    return EFI_NOT_READY;
+  }
+
+  Dequeue (&UsbKeyboardDevice->EfiKeyQueue, KeyData, sizeof (*KeyData));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reset the input device and optionally run diagnostics
+
+  There are 2 types of reset for USB keyboard.
+  For non-exhaustive reset, only keyboard buffer is cleared.
+  For exhaustive reset, in addition to clearance of keyboard buffer, the hardware status
+  is also re-initialized.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  IN  BOOLEAN                          ExtendedVerification
+  )
+{
+  EFI_STATUS          Status;
+  USB_KB_DEV          *UsbKeyboardDevice;
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_PC_RESET),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  //
+  // Non-exhaustive reset:
+  // only reset private data structures.
+  //
+  if (!ExtendedVerification) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_KEYBOARD_PC_CLEAR_BUFFER),
+      UsbKeyboardDevice->DevicePath
+      );
+    //
+    // Clear the key buffer of this USB keyboard
+    //
+    InitQueue (&UsbKeyboardDevice->UsbKeyQueue, sizeof (USB_KEY));
+    InitQueue (&UsbKeyboardDevice->EfiKeyQueue, sizeof (EFI_KEY_DATA));
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Exhaustive reset
+  //
+  Status = InitUSBKeyboard (UsbKeyboardDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                 The EFI_SIMPLE_TEXT_INPUT_PROTOCOL instance.
+  @param  Key                  A pointer to a buffer that is filled in with the keystroke
+                               information for the key that was pressed.
+
+  @retval EFI_SUCCESS          The keystroke information was returned.
+  @retval EFI_NOT_READY        There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR     The keystroke information was not returned due to
+                               hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  OUT EFI_INPUT_KEY                    *Key
+  )
+{
+  USB_KB_DEV   *UsbKeyboardDevice;
+  EFI_STATUS   Status;
+  EFI_KEY_DATA KeyData;
+
+  UsbKeyboardDevice = USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Considering if the partial keystroke is enabled, there maybe a partial
+  // keystroke in the queue, so here skip the partial keystroke and get the
+  // next key from the queue
+  //
+  while (1) {
+    Status = USBKeyboardReadKeyStrokeWorker (UsbKeyboardDevice, &KeyData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // SimpleTextIn Protocol doesn't support partial keystroke;
+    //
+    if (KeyData.Key.ScanCode == CHAR_NULL && KeyData.Key.UnicodeChar == SCAN_NULL) {
+      continue;
+    }
+    //
+    // Translate the CTRL-Alpha characters to their corresponding control value
+    // (ctrl-a = 0x0001 through ctrl-Z = 0x001A)
+    //
+    if ((KeyData.KeyState.KeyShiftState & (EFI_LEFT_CONTROL_PRESSED | EFI_RIGHT_CONTROL_PRESSED)) != 0) {
+      if (KeyData.Key.UnicodeChar >= L'a' && KeyData.Key.UnicodeChar <= L'z') {
+        KeyData.Key.UnicodeChar = (CHAR16) (KeyData.Key.UnicodeChar - L'a' + 1);
+      } else if (KeyData.Key.UnicodeChar >= L'A' && KeyData.Key.UnicodeChar <= L'Z') {
+        KeyData.Key.UnicodeChar = (CHAR16) (KeyData.Key.UnicodeChar - L'A' + 1);
+      }
+    }
+
+    CopyMem (Key, &KeyData.Key, sizeof (EFI_INPUT_KEY));
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Event notification function registered for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx
+  and EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey.
+
+  @param  Event        Event to be signaled when a key is pressed.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardWaitForKey (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_KB_DEV  *UsbKeyboardDevice;
+  EFI_KEY_DATA KeyData;
+  EFI_TPL      OldTpl;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Enter critical section
+  //  
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  
+  //
+  // WaitforKey doesn't suppor the partial key.
+  // Considering if the partial keystroke is enabled, there maybe a partial
+  // keystroke in the queue, so here skip the partial keystroke and get the
+  // next key from the queue
+  //
+  while (!IsQueueEmpty (&UsbKeyboardDevice->EfiKeyQueue)) {
+    //
+    // If there is pending key, signal the event.
+    //
+    CopyMem (
+      &KeyData,
+      UsbKeyboardDevice->EfiKeyQueue.Buffer[UsbKeyboardDevice->EfiKeyQueue.Head],
+      sizeof (EFI_KEY_DATA)
+      );
+    if (KeyData.Key.ScanCode == SCAN_NULL && KeyData.Key.UnicodeChar == CHAR_NULL) {
+      Dequeue (&UsbKeyboardDevice->EfiKeyQueue, &KeyData, sizeof (EFI_KEY_DATA));
+      continue;
+    }
+    gBS->SignalEvent (Event);
+    break;
+  }
+  //
+  // Leave critical section and return
+  //
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Timer handler to convert the key from USB.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+USBKeyboardTimerHandler (
+  IN  EFI_EVENT                 Event,
+  IN  VOID                      *Context
+  )
+{
+  EFI_STATUS                    Status;
+  USB_KB_DEV                    *UsbKeyboardDevice;
+  UINT8                         KeyCode;
+  EFI_KEY_DATA                  KeyData;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Fetch raw data from the USB keyboard buffer,
+  // and translate it into USB keycode.
+  //
+  Status = USBParseKey (UsbKeyboardDevice, &KeyCode);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Translate saved USB keycode into EFI_INPUT_KEY
+  //
+  Status = UsbKeyCodeToEfiInputKey (UsbKeyboardDevice, KeyCode, &KeyData);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Insert to the EFI Key queue
+  //
+  Enqueue (&UsbKeyboardDevice->EfiKeyQueue, &KeyData, sizeof (KeyData));
+}
+
+/**
+  Free keyboard notify list.
+
+  @param  NotifyList              The keyboard notify list to free.
+
+  @retval EFI_SUCCESS             Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER   NotifyList is NULL.
+
+**/
+EFI_STATUS
+KbdFreeNotifyList (
+  IN OUT LIST_ENTRY           *NotifyList
+  )
+{
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY *NotifyNode;
+  LIST_ENTRY                    *Link;
+
+  if (NotifyList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  while (!IsListEmpty (NotifyList)) {
+    Link = GetFirstNode (NotifyList);
+    NotifyNode = CR (Link, KEYBOARD_CONSOLE_IN_EX_NOTIFY, NotifyEntry, USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE);
+    RemoveEntryList (Link);
+    FreePool (NotifyNode);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether the pressed key matches a registered key or not.
+
+  @param  RegsiteredData    A pointer to keystroke data for the key that was registered.
+  @param  InputData         A pointer to keystroke data for the key that was pressed.
+
+  @retval TRUE              Key pressed matches a registered key.
+  @retval FLASE             Key pressed does not matches a registered key.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  )
+{
+  ASSERT (RegsiteredData != NULL && InputData != NULL);
+
+  if ((RegsiteredData->Key.ScanCode    != InputData->Key.ScanCode) ||
+      (RegsiteredData->Key.UnicodeChar != InputData->Key.UnicodeChar)) {
+    return FALSE;
+  }
+
+  //
+  // Assume KeyShiftState/KeyToggleState = 0 in Registered key data means these state could be ignored.
+  //
+  if (RegsiteredData->KeyState.KeyShiftState != 0 &&
+      RegsiteredData->KeyState.KeyShiftState != InputData->KeyState.KeyShiftState) {
+    return FALSE;
+  }
+  if (RegsiteredData->KeyState.KeyToggleState != 0 &&
+      RegsiteredData->KeyState.KeyToggleState != InputData->KeyState.KeyToggleState) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+//
+// Simple Text Input Ex protocol functions
+//
+/**
+  Resets the input device hardware.
+
+  The Reset() function resets the input device hardware. As part
+  of initialization process, the firmware/device will make a quick
+  but reasonable attempt to verify that the device is functioning.
+  If the ExtendedVerification flag is TRUE the firmware may take
+  an extended amount of time to verify the device is operating on
+  reset. Otherwise the reset operation is to occur as quickly as
+  possible. The hardware verification process is not defined by
+  this specification and is left up to the platform firmware or
+  driver to implement.
+
+  @param This                 A pointer to the EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL instance.
+
+  @param ExtendedVerification Indicates that the driver may perform a more exhaustive
+                              verification operation of the device during reset.
+
+  @retval EFI_SUCCESS         The device was reset.
+  @retval EFI_DEVICE_ERROR    The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  )
+{
+  EFI_STATUS                Status;
+  USB_KB_DEV                *UsbKeyboardDevice;
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  Status = UsbKeyboardDevice->SimpleInput.Reset (&UsbKeyboardDevice->SimpleInput, ExtendedVerification);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  UsbKeyboardDevice->KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID;
+  UsbKeyboardDevice->KeyState.KeyToggleState = EFI_TOGGLE_STATE_VALID;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                   Protocol instance pointer.
+  @param  KeyData                A pointer to a buffer that is filled in with the keystroke
+                                 state data for the key that was pressed.
+
+  @retval EFI_SUCCESS            The keystroke information was returned.
+  @retval EFI_NOT_READY          There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR       The keystroke information was not returned due to
+                                 hardware errors.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  return USBKeyboardReadKeyStrokeWorker (UsbKeyboardDevice, KeyData);
+
+}
+
+/**
+  Set certain state for the input device.
+
+  @param  This                    Protocol instance pointer.
+  @param  KeyToggleState          A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                  state for the input device.
+
+  @retval EFI_SUCCESS             The device state was set appropriately.
+  @retval EFI_DEVICE_ERROR        The device is not functioning correctly and could
+                                  not have the setting adjusted.
+  @retval EFI_UNSUPPORTED         The device does not support the ability to have its state set.
+  @retval EFI_INVALID_PARAMETER   KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+
+  if (KeyToggleState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  if (((UsbKeyboardDevice->KeyState.KeyToggleState & EFI_TOGGLE_STATE_VALID) != EFI_TOGGLE_STATE_VALID) ||
+      ((*KeyToggleState & EFI_TOGGLE_STATE_VALID) != EFI_TOGGLE_STATE_VALID)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Update the status light
+  //
+
+  UsbKeyboardDevice->ScrollOn   = FALSE;
+  UsbKeyboardDevice->NumLockOn  = FALSE;
+  UsbKeyboardDevice->CapsOn     = FALSE;
+  UsbKeyboardDevice->IsSupportPartialKey = FALSE;
+
+  if ((*KeyToggleState & EFI_SCROLL_LOCK_ACTIVE) == EFI_SCROLL_LOCK_ACTIVE) {
+    UsbKeyboardDevice->ScrollOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_NUM_LOCK_ACTIVE) == EFI_NUM_LOCK_ACTIVE) {
+    UsbKeyboardDevice->NumLockOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_CAPS_LOCK_ACTIVE) == EFI_CAPS_LOCK_ACTIVE) {
+    UsbKeyboardDevice->CapsOn = TRUE;
+  }
+  if ((*KeyToggleState & EFI_KEY_STATE_EXPOSED) == EFI_KEY_STATE_EXPOSED) {
+    UsbKeyboardDevice->IsSupportPartialKey = TRUE;
+  }
+
+  SetKeyLED (UsbKeyboardDevice);
+
+  UsbKeyboardDevice->KeyState.KeyToggleState = *KeyToggleState;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                        Protocol instance pointer.
+  @param  KeyData                     A pointer to a buffer that is filled in with the keystroke
+                                      information data for the key that was pressed.
+  @param  KeyNotificationFunction     Points to the function to be called when the key
+                                      sequence is typed specified by KeyData.
+  @param  NotifyHandle                Points to the unique handle assigned to the registered notification.
+
+  @retval EFI_SUCCESS                 The notification function was registered successfully.
+  @retval EFI_OUT_OF_RESOURCES        Unable to allocate resources for necessary data structures.
+  @retval EFI_INVALID_PARAMETER       KeyData or NotifyHandle or KeyNotificationFunction is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardRegisterKeyNotify (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN  EFI_KEY_DATA                       *KeyData,
+  IN  EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                               **NotifyHandle
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *NewNotify;
+  LIST_ENTRY                        *Link;
+  LIST_ENTRY                        *NotifyList;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *CurrentNotify;
+
+  if (KeyData == NULL || NotifyHandle == NULL || KeyNotificationFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Return EFI_SUCCESS if the (KeyData, NotificationFunction) is already registered.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+
+  for (Link = GetFirstNode (NotifyList);
+       !IsNull (NotifyList, Link);
+       Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      KEYBOARD_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      if (CurrentNotify->KeyNotificationFn == KeyNotificationFunction) {
+        *NotifyHandle = CurrentNotify;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Allocate resource to save the notification function
+  //
+  NewNotify = (KEYBOARD_CONSOLE_IN_EX_NOTIFY *) AllocateZeroPool (sizeof (KEYBOARD_CONSOLE_IN_EX_NOTIFY));
+  if (NewNotify == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewNotify->Signature         = USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE;
+  NewNotify->KeyNotificationFn = KeyNotificationFunction;
+  CopyMem (&NewNotify->KeyData, KeyData, sizeof (EFI_KEY_DATA));
+  InsertTailList (&UsbKeyboardDevice->NotifyList, &NewNotify->NotifyEntry);
+
+
+  *NotifyHandle = NewNotify;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                      Protocol instance pointer.
+  @param  NotificationHandle        The handle of the notification function being unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  )
+{
+  USB_KB_DEV                        *UsbKeyboardDevice;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY     *CurrentNotify;
+  LIST_ENTRY                        *Link;
+  LIST_ENTRY                        *NotifyList;
+
+  if (NotificationHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  UsbKeyboardDevice = TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS (This);
+
+  //
+  // Traverse notify list of USB keyboard and remove the entry of NotificationHandle.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList);
+       !IsNull (NotifyList, Link);
+       Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      KEYBOARD_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (CurrentNotify == NotificationHandle) {
+      //
+      // Remove the notification function from NotifyList and free resources
+      //
+      RemoveEntryList (&CurrentNotify->NotifyEntry);
+
+      FreePool (CurrentNotify);
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Cannot find the matching entry in database.
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h
new file mode 100644
index 0000000000..58edb3f65f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/EfiKey.h
@@ -0,0 +1,600 @@
+/** @file
+  Header file for USB Keyboard Driver's Data Structures.
+
+Copyright (c) 2004 - 2012, 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.
+
+**/
+#ifndef _EFI_USB_KB_H_
+#define _EFI_USB_KB_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/SimpleTextInEx.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Guid/HiiKeyBoardLayout.h>
+#include <Guid/UsbKeyBoardLayout.h>
+
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/HiiLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define KEYBOARD_TIMER_INTERVAL         200000  // 0.02s
+
+#define MAX_KEY_ALLOWED     32
+
+#define HZ                  1000 * 1000 * 10
+#define USBKBD_REPEAT_DELAY ((HZ) / 2)
+#define USBKBD_REPEAT_RATE  ((HZ) / 50)
+
+#define CLASS_HID           3
+#define SUBCLASS_BOOT       1
+#define PROTOCOL_KEYBOARD   1
+
+#define BOOT_PROTOCOL       0
+#define REPORT_PROTOCOL     1
+
+typedef struct {
+  BOOLEAN  Down;
+  UINT8    KeyCode;
+} USB_KEY;
+
+typedef struct {
+  VOID          *Buffer[MAX_KEY_ALLOWED + 1];
+  UINTN         Head;
+  UINTN         Tail;
+  UINTN         ItemSize;
+} USB_SIMPLE_QUEUE;
+
+#define USB_KB_DEV_SIGNATURE  SIGNATURE_32 ('u', 'k', 'b', 'd')
+#define USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE SIGNATURE_32 ('u', 'k', 'b', 'x')
+
+typedef struct _KEYBOARD_CONSOLE_IN_EX_NOTIFY {
+  UINTN                                 Signature;
+  EFI_KEY_DATA                          KeyData;
+  EFI_KEY_NOTIFY_FUNCTION               KeyNotificationFn;
+  LIST_ENTRY                            NotifyEntry;
+} KEYBOARD_CONSOLE_IN_EX_NOTIFY;
+
+#define USB_NS_KEY_SIGNATURE  SIGNATURE_32 ('u', 'n', 's', 'k')
+
+typedef struct {
+  UINTN                         Signature;
+  LIST_ENTRY                    Link;
+
+  //
+  // The number of EFI_NS_KEY_MODIFIER children definitions
+  //
+  UINTN                         KeyCount;
+
+  //
+  // NsKey[0] : Non-spacing key
+  // NsKey[1] ~ NsKey[KeyCount] : Physical keys
+  //
+  EFI_KEY_DESCRIPTOR            *NsKey;
+} USB_NS_KEY;
+
+#define USB_NS_KEY_FORM_FROM_LINK(a)  CR (a, USB_NS_KEY, Link, USB_NS_KEY_SIGNATURE)
+
+///
+/// Structure to describe USB keyboard device
+///
+typedef struct {
+  UINTN                             Signature;
+  EFI_HANDLE                        ControllerHandle;
+  EFI_DEVICE_PATH_PROTOCOL          *DevicePath;
+  EFI_EVENT                         DelayedRecoveryEvent;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL    SimpleInput;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL SimpleInputEx;
+  EFI_USB_IO_PROTOCOL               *UsbIo;
+
+  EFI_USB_INTERFACE_DESCRIPTOR      InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR       IntEndpointDescriptor;
+
+  USB_SIMPLE_QUEUE                  UsbKeyQueue;
+  USB_SIMPLE_QUEUE                  EfiKeyQueue;
+  BOOLEAN                           CtrlOn;
+  BOOLEAN                           AltOn;
+  BOOLEAN                           ShiftOn;
+  BOOLEAN                           NumLockOn;
+  BOOLEAN                           CapsOn;
+  BOOLEAN                           ScrollOn;
+  UINT8                             LastKeyCodeArray[8];
+  UINT8                             CurKeyCode;
+
+  EFI_EVENT                         TimerEvent;
+
+  UINT8                             RepeatKey;
+  EFI_EVENT                         RepeatTimer;
+
+  EFI_UNICODE_STRING_TABLE          *ControllerNameTable;
+
+  BOOLEAN                           LeftCtrlOn;
+  BOOLEAN                           LeftAltOn;
+  BOOLEAN                           LeftShiftOn;
+  BOOLEAN                           LeftLogoOn;
+  BOOLEAN                           RightCtrlOn;
+  BOOLEAN                           RightAltOn;
+  BOOLEAN                           RightShiftOn;
+  BOOLEAN                           RightLogoOn;
+  BOOLEAN                           MenuKeyOn;
+  BOOLEAN                           SysReqOn;
+  BOOLEAN                           AltGrOn;
+
+  BOOLEAN                         IsSupportPartialKey;
+
+  EFI_KEY_STATE                     KeyState;
+  //
+  // Notification function list
+  //
+  LIST_ENTRY                        NotifyList;
+
+  //
+  // Non-spacing key list
+  //
+  LIST_ENTRY                        NsKeyList;
+  USB_NS_KEY                        *CurrentNsKey;
+  EFI_KEY_DESCRIPTOR                *KeyConvertionTable;
+  EFI_EVENT                         KeyboardLayoutEvent;
+} USB_KB_DEV;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbKeyboardDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbKeyboardComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbKeyboardComponentName2;
+
+#define USB_KB_DEV_FROM_THIS(a) \
+    CR(a, USB_KB_DEV, SimpleInput, USB_KB_DEV_SIGNATURE)
+#define TEXT_INPUT_EX_USB_KB_DEV_FROM_THIS(a) \
+    CR(a, USB_KB_DEV, SimpleInputEx, USB_KB_DEV_SIGNATURE)
+
+//
+// According to Universal Serial Bus HID Usage Tables document ver 1.12,
+// a Boot Keyboard should support the keycode range from 0x0 to 0x65 and 0xE0 to 0xE7.
+// 0xE0 to 0xE7 are for modifier keys, and 0x0 to 0x3 are reserved for typical
+// keyboard status or keyboard errors.
+// So the number of valid non-modifier USB keycodes is 0x62, and the number of
+// valid keycodes is 0x6A.
+//
+#define NUMBER_OF_VALID_NON_MODIFIER_USB_KEYCODE      0x62
+#define NUMBER_OF_VALID_USB_KEYCODE                   0x6A
+//
+// 0x0 to 0x3 are reserved for typical keyboard status or keyboard errors.
+//
+#define USBKBD_VALID_KEYCODE(Key) ((UINT8) (Key) > 3)
+
+typedef struct {
+  UINT8 NumLock : 1;
+  UINT8 CapsLock : 1;
+  UINT8 ScrollLock : 1;
+  UINT8 Resrvd : 5;
+} LED_MAP;
+
+//
+// Functions of Driver Binding Protocol
+//
+/**
+  Check whether USB keyboard driver supports this device.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the keyboard device with this driver.
+
+  This function produces Simple Text Input Protocol and Simple Text Input Ex Protocol,
+  initializes the keyboard device, and submit Asynchronous Interrupt Transfer to manage
+  this keyboard device.
+
+  @param  This                   The USB keyboard driver binding instance.
+  @param  Controller             Handle of device to bind driver to.
+  @param  RemainingDevicePath    Optional parameter use to pick a specific child
+                                 device to start.
+
+  @retval EFI_SUCCESS            The controller is controlled by the usb keyboard driver.
+  @retval EFI_UNSUPPORTED        No interrupt endpoint can be found.
+  @retval Other                  This controller cannot be started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB keyboard device handled by this driver.
+
+  @param  This                   The USB keyboard driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Text In Protocol or Simple Text In Ex Protocol
+                                 is not installed on Controller.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbKeyboardComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of Simple Text Input Protocol
+//
+/**
+  Reset the input device and optionaly run diagnostics
+
+  There are 2 types of reset for USB keyboard.
+  For non-exhaustive reset, only keyboard buffer is cleared.
+  For exhaustive reset, in addition to clearance of keyboard buffer, the hardware status
+  is also re-initialized.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  IN  BOOLEAN                          ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                 The EFI_SIMPLE_TEXT_INPUT_PROTOCOL instance.
+  @param  Key                  A pointer to a buffer that is filled in with the keystroke
+                               information for the key that was pressed.
+
+  @retval EFI_SUCCESS          The keystroke information was returned.
+  @retval EFI_NOT_READY        There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR     The keydtroke information was not returned due to
+                               hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL   *This,
+  OUT EFI_INPUT_KEY                    *Key
+  );
+
+//
+// Simple Text Input Ex protocol functions
+//
+/**
+  Resets the input device hardware.
+
+  The Reset() function resets the input device hardware. As part
+  of initialization process, the firmware/device will make a quick
+  but reasonable attempt to verify that the device is functioning.
+  If the ExtendedVerification flag is TRUE the firmware may take
+  an extended amount of time to verify the device is operating on
+  reset. Otherwise the reset operation is to occur as quickly as
+  possible. The hardware verification process is not defined by
+  this specification and is left up to the platform firmware or
+  driver to implement.
+
+  @param This                 A pointer to the EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL instance.
+
+  @param ExtendedVerification Indicates that the driver may perform a more exhaustive
+                              verification operation of the device during reset.
+
+  @retval EFI_SUCCESS         The device was reset.
+  @retval EFI_DEVICE_ERROR    The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device.
+
+  @param  This                   Protocol instance pointer.
+  @param  KeyData                A pointer to a buffer that is filled in with the keystroke
+                                 state data for the key that was pressed.
+
+  @retval EFI_SUCCESS            The keystroke information was returned.
+  @retval EFI_NOT_READY          There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR       The keystroke information was not returned due to
+                                 hardware errors.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  );
+
+/**
+  Set certain state for the input device.
+
+  @param  This                    Protocol instance pointer.
+  @param  KeyToggleState          A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                  state for the input device.
+
+  @retval EFI_SUCCESS             The device state was set appropriately.
+  @retval EFI_DEVICE_ERROR        The device is not functioning correctly and could
+                                  not have the setting adjusted.
+  @retval EFI_UNSUPPORTED         The device does not support the ability to have its state set.
+  @retval EFI_INVALID_PARAMETER   KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  );
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                        Protocol instance pointer.
+  @param  KeyData                     A pointer to a buffer that is filled in with the keystroke
+                                      information data for the key that was pressed.
+  @param  KeyNotificationFunction     Points to the function to be called when the key
+                                      sequence is typed specified by KeyData.
+  @param  NotifyHandle                Points to the unique handle assigned to the registered notification.
+
+  @retval EFI_SUCCESS                 The notification function was registered successfully.
+  @retval EFI_OUT_OF_RESOURCES        Unable to allocate resources for necesssary data structures.
+  @retval EFI_INVALID_PARAMETER       KeyData or NotifyHandle or KeyNotificationFunction is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardRegisterKeyNotify (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN  EFI_KEY_DATA                       *KeyData,
+  IN  EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                               **NotifyHandle
+  );
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                      Protocol instance pointer.
+  @param  NotificationHandle        The handle of the notification function being unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid
+  @retval EFI_NOT_FOUND            Cannot find the matching entry in database.
+
+**/
+EFI_STATUS
+EFIAPI
+USBKeyboardUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  );
+
+/**
+  Event notification function registered for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx
+  and EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey.
+
+  @param  Event        Event to be signaled when a key is pressed.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardWaitForKey (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+/**
+  Free keyboard notify list.
+
+  @param  NotifyList              The keyboard notify list to free.
+
+  @retval EFI_SUCCESS             Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER   NotifyList is NULL.
+
+**/
+EFI_STATUS
+KbdFreeNotifyList (
+  IN OUT LIST_ENTRY           *NotifyList
+  );
+
+/**
+  Check whether the pressed key matches a registered key or not.
+
+  @param  RegsiteredData    A pointer to keystroke data for the key that was registered.
+  @param  InputData         A pointer to keystroke data for the key that was pressed.
+
+  @retval TRUE              Key pressed matches a registered key.
+  @retval FLASE             Key pressed does not matche a registered key.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  );
+
+/**
+  Timer handler to convert the key from USB.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+USBKeyboardTimerHandler (
+  IN  EFI_EVENT                 Event,
+  IN  VOID                      *Context
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c
new file mode 100644
index 0000000000..86dd99d254
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.c
@@ -0,0 +1,1956 @@
+/** @file
+  Helper functions for USB Keyboard Driver.
+
+Copyright (c) 2004 - 2013, 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 "KeyBoard.h"
+
+USB_KEYBOARD_LAYOUT_PACK_BIN  mUsbKeyboardLayoutBin = {
+  sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN),   // Binary size
+
+  //
+  // EFI_HII_PACKAGE_HEADER
+  //
+  {
+    sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN) - sizeof (UINT32),
+    EFI_HII_PACKAGE_KEYBOARD_LAYOUT
+  },
+  1,  // LayoutCount
+  sizeof (USB_KEYBOARD_LAYOUT_PACK_BIN) - sizeof (UINT32) - sizeof (EFI_HII_PACKAGE_HEADER) - sizeof (UINT16), // LayoutLength
+  USB_KEYBOARD_LAYOUT_KEY_GUID,  // KeyGuid
+  sizeof (UINT16) + sizeof (EFI_GUID) + sizeof (UINT32) + sizeof (UINT8) + (USB_KEYBOARD_KEY_COUNT * sizeof (EFI_KEY_DESCRIPTOR)), // LayoutDescriptorStringOffset
+  USB_KEYBOARD_KEY_COUNT, // DescriptorCount
+  {
+    //
+    // EFI_KEY_DESCRIPTOR (total number is USB_KEYBOARD_KEY_COUNT)
+    //
+    {EfiKeyC1,         'a',      'A',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB5,         'b',      'B',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB3,         'c',      'C',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC3,         'd',      'D',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD3,         'e',      'E',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC4,         'f',      'F',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC5,         'g',      'G',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC6,         'h',      'H',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD8,         'i',      'I',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC7,         'j',      'J',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC8,         'k',      'K',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC9,         'l',      'L',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB7,         'm',      'M',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB6,         'n',      'N',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD9,         'o',      'O',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD10,        'p',      'P',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD1,         'q',      'Q',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD4,         'r',      'R',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyC2,         's',      'S',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD5,         't',      'T',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD7,         'u',      'U',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB4,         'v',      'V',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD2,         'w',      'W',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB2,         'x',      'X',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyD6,         'y',      'Y',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyB1,         'z',      'Z',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_CAPS_LOCK},
+    {EfiKeyE1,         '1',      '!',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE2,         '2',      '@',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE3,         '3',      '#',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE4,         '4',      '$',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE5,         '5',      '%',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE6,         '6',      '^',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE7,         '7',      '&',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE8,         '8',      '*',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE9,         '9',      '(',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE10,        '0',      ')',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyEnter,      0x0d,     0x0d, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyEsc,        0x1b,     0x1b, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyBackSpace,  0x08,     0x08, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyTab,        0x09,     0x09, 0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeySpaceBar,   ' ',      ' ',  0, 0,  EFI_NULL_MODIFIER,   0},
+    {EfiKeyE11,        '-',      '_',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE12,        '=',      '+',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD11,        '[',      '{',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD12,        ']',      '}',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyD13,        '\\',     '|',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC12,        '\\',     '|',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC10,        ';',      ':',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyC11,        '\'',     '"',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyE0,         '`',      '~',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB8,         ',',      '<',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB9,         '.',      '>',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyB10,        '/',      '?',  0, 0,  EFI_NULL_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT},
+    {EfiKeyCapsLock,   0x00,     0x00, 0, 0,  EFI_CAPS_LOCK_MODIFIER,            0},
+    {EfiKeyF1,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_ONE_MODIFIER,     0},
+    {EfiKeyF2,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TWO_MODIFIER,     0},
+    {EfiKeyF3,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_THREE_MODIFIER,   0},
+    {EfiKeyF4,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_FOUR_MODIFIER,    0},
+    {EfiKeyF5,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_FIVE_MODIFIER,    0},
+    {EfiKeyF6,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_SIX_MODIFIER,     0},
+    {EfiKeyF7,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_SEVEN_MODIFIER,   0},
+    {EfiKeyF8,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_EIGHT_MODIFIER,   0},
+    {EfiKeyF9,         0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_NINE_MODIFIER,    0},
+    {EfiKeyF10,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TEN_MODIFIER,     0},
+    {EfiKeyF11,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_ELEVEN_MODIFIER,  0},
+    {EfiKeyF12,        0x00,     0x00, 0, 0,  EFI_FUNCTION_KEY_TWELVE_MODIFIER,  0},
+    {EfiKeyPrint,      0x00,     0x00, 0, 0,  EFI_PRINT_MODIFIER,                0},
+    {EfiKeySLck,       0x00,     0x00, 0, 0,  EFI_SCROLL_LOCK_MODIFIER,          0},
+    {EfiKeyPause,      0x00,     0x00, 0, 0,  EFI_PAUSE_MODIFIER,                0},
+    {EfiKeyIns,        0x00,     0x00, 0, 0,  EFI_INSERT_MODIFIER,               0},
+    {EfiKeyHome,       0x00,     0x00, 0, 0,  EFI_HOME_MODIFIER,                 0},
+    {EfiKeyPgUp,       0x00,     0x00, 0, 0,  EFI_PAGE_UP_MODIFIER,              0},
+    {EfiKeyDel,        0x00,     0x00, 0, 0,  EFI_DELETE_MODIFIER,               0},
+    {EfiKeyEnd,        0x00,     0x00, 0, 0,  EFI_END_MODIFIER,                  0},
+    {EfiKeyPgDn,       0x00,     0x00, 0, 0,  EFI_PAGE_DOWN_MODIFIER,            0},
+    {EfiKeyRightArrow, 0x00,     0x00, 0, 0,  EFI_RIGHT_ARROW_MODIFIER,          0},
+    {EfiKeyLeftArrow,  0x00,     0x00, 0, 0,  EFI_LEFT_ARROW_MODIFIER,           0},
+    {EfiKeyDownArrow,  0x00,     0x00, 0, 0,  EFI_DOWN_ARROW_MODIFIER,           0},
+    {EfiKeyUpArrow,    0x00,     0x00, 0, 0,  EFI_UP_ARROW_MODIFIER,             0},
+    {EfiKeyNLck,       0x00,     0x00, 0, 0,  EFI_NUM_LOCK_MODIFIER,             0},
+    {EfiKeySlash,      '/',      '/',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyAsterisk,   '*',      '*',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyMinus,      '-',      '-',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyPlus,       '+',      '+',  0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyEnter,      0x0d,     0x0d, 0, 0,  EFI_NULL_MODIFIER,                 0},
+    {EfiKeyOne,        '1',      '1',  0, 0,  EFI_END_MODIFIER,         EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyTwo,        '2',      '2',  0, 0,  EFI_DOWN_ARROW_MODIFIER,  EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyThree,      '3',      '3',  0, 0,  EFI_PAGE_DOWN_MODIFIER,   EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyFour,       '4',      '4',  0, 0,  EFI_LEFT_ARROW_MODIFIER,  EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyFive,       '5',      '5',  0, 0,  EFI_NULL_MODIFIER,        EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeySix,        '6',      '6',  0, 0,  EFI_RIGHT_ARROW_MODIFIER, EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeySeven,      '7',      '7',  0, 0,  EFI_HOME_MODIFIER,        EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyEight,      '8',      '8',  0, 0,  EFI_UP_ARROW_MODIFIER,    EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyNine,       '9',      '9',  0, 0,  EFI_PAGE_UP_MODIFIER,     EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyZero,       '0',      '0',  0, 0,  EFI_INSERT_MODIFIER,      EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyPeriod,     '.',      '.',  0, 0,  EFI_DELETE_MODIFIER,      EFI_AFFECTED_BY_STANDARD_SHIFT | EFI_AFFECTED_BY_NUM_LOCK},
+    {EfiKeyA4,         0x00,     0x00, 0, 0,  EFI_MENU_MODIFIER,            0},
+    {EfiKeyLCtrl,      0,        0,    0, 0,  EFI_LEFT_CONTROL_MODIFIER,    0},
+    {EfiKeyLShift,     0,        0,    0, 0,  EFI_LEFT_SHIFT_MODIFIER,      0},
+    {EfiKeyLAlt,       0,        0,    0, 0,  EFI_LEFT_ALT_MODIFIER,        0},
+    {EfiKeyA0,         0,        0,    0, 0,  EFI_LEFT_LOGO_MODIFIER,       0},
+    {EfiKeyRCtrl,      0,        0,    0, 0,  EFI_RIGHT_CONTROL_MODIFIER,   0},
+    {EfiKeyRShift,     0,        0,    0, 0,  EFI_RIGHT_SHIFT_MODIFIER,     0},
+    {EfiKeyA2,         0,        0,    0, 0,  EFI_RIGHT_ALT_MODIFIER,       0},
+    {EfiKeyA3,         0,        0,    0, 0,  EFI_RIGHT_LOGO_MODIFIER,      0},
+  },
+  1,                          // DescriptionCount
+  {'e', 'n', '-', 'U', 'S'},  // RFC4646 language code
+  ' ',                        // Space
+  {'E', 'n', 'g', 'l', 'i', 's', 'h', ' ', 'K', 'e', 'y', 'b', 'o', 'a', 'r', 'd', '\0'}, // DescriptionString[]
+};
+
+//
+// EFI_KEY to USB Keycode conversion table
+// EFI_KEY is defined in UEFI spec.
+// USB Keycode is defined in USB HID Firmware spec.
+//
+UINT8 EfiKeyToUsbKeyCodeConvertionTable[] = {
+  0xe0,  //  EfiKeyLCtrl
+  0xe3,  //  EfiKeyA0
+  0xe2,  //  EfiKeyLAlt
+  0x2c,  //  EfiKeySpaceBar
+  0xe6,  //  EfiKeyA2
+  0xe7,  //  EfiKeyA3
+  0x65,  //  EfiKeyA4
+  0xe4,  //  EfiKeyRCtrl
+  0x50,  //  EfiKeyLeftArrow
+  0x51,  //  EfiKeyDownArrow
+  0x4F,  //  EfiKeyRightArrow
+  0x62,  //  EfiKeyZero
+  0x63,  //  EfiKeyPeriod
+  0x28,  //  EfiKeyEnter
+  0xe1,  //  EfiKeyLShift
+  0x64,  //  EfiKeyB0
+  0x1D,  //  EfiKeyB1
+  0x1B,  //  EfiKeyB2
+  0x06,  //  EfiKeyB3
+  0x19,  //  EfiKeyB4
+  0x05,  //  EfiKeyB5
+  0x11,  //  EfiKeyB6
+  0x10,  //  EfiKeyB7
+  0x36,  //  EfiKeyB8
+  0x37,  //  EfiKeyB9
+  0x38,  //  EfiKeyB10
+  0xe5,  //  EfiKeyRShift
+  0x52,  //  EfiKeyUpArrow
+  0x59,  //  EfiKeyOne
+  0x5A,  //  EfiKeyTwo
+  0x5B,  //  EfiKeyThree
+  0x39,  //  EfiKeyCapsLock
+  0x04,  //  EfiKeyC1
+  0x16,  //  EfiKeyC2
+  0x07,  //  EfiKeyC3
+  0x09,  //  EfiKeyC4
+  0x0A,  //  EfiKeyC5
+  0x0B,  //  EfiKeyC6
+  0x0D,  //  EfiKeyC7
+  0x0E,  //  EfiKeyC8
+  0x0F,  //  EfiKeyC9
+  0x33,  //  EfiKeyC10
+  0x34,  //  EfiKeyC11
+  0x32,  //  EfiKeyC12
+  0x5C,  //  EfiKeyFour
+  0x5D,  //  EfiKeyFive
+  0x5E,  //  EfiKeySix
+  0x57,  //  EfiKeyPlus
+  0x2B,  //  EfiKeyTab
+  0x14,  //  EfiKeyD1
+  0x1A,  //  EfiKeyD2
+  0x08,  //  EfiKeyD3
+  0x15,  //  EfiKeyD4
+  0x17,  //  EfiKeyD5
+  0x1C,  //  EfiKeyD6
+  0x18,  //  EfiKeyD7
+  0x0C,  //  EfiKeyD8
+  0x12,  //  EfiKeyD9
+  0x13,  //  EfiKeyD10
+  0x2F,  //  EfiKeyD11
+  0x30,  //  EfiKeyD12
+  0x31,  //  EfiKeyD13
+  0x4C,  //  EfiKeyDel
+  0x4D,  //  EfiKeyEnd
+  0x4E,  //  EfiKeyPgDn
+  0x5F,  //  EfiKeySeven
+  0x60,  //  EfiKeyEight
+  0x61,  //  EfiKeyNine
+  0x35,  //  EfiKeyE0
+  0x1E,  //  EfiKeyE1
+  0x1F,  //  EfiKeyE2
+  0x20,  //  EfiKeyE3
+  0x21,  //  EfiKeyE4
+  0x22,  //  EfiKeyE5
+  0x23,  //  EfiKeyE6
+  0x24,  //  EfiKeyE7
+  0x25,  //  EfiKeyE8
+  0x26,  //  EfiKeyE9
+  0x27,  //  EfiKeyE10
+  0x2D,  //  EfiKeyE11
+  0x2E,  //  EfiKeyE12
+  0x2A,  //  EfiKeyBackSpace
+  0x49,  //  EfiKeyIns
+  0x4A,  //  EfiKeyHome
+  0x4B,  //  EfiKeyPgUp
+  0x53,  //  EfiKeyNLck
+  0x54,  //  EfiKeySlash
+  0x55,  //  EfiKeyAsterisk
+  0x56,  //  EfiKeyMinus
+  0x29,  //  EfiKeyEsc
+  0x3A,  //  EfiKeyF1
+  0x3B,  //  EfiKeyF2
+  0x3C,  //  EfiKeyF3
+  0x3D,  //  EfiKeyF4
+  0x3E,  //  EfiKeyF5
+  0x3F,  //  EfiKeyF6
+  0x40,  //  EfiKeyF7
+  0x41,  //  EfiKeyF8
+  0x42,  //  EfiKeyF9
+  0x43,  //  EfiKeyF10
+  0x44,  //  EfiKeyF11
+  0x45,  //  EfiKeyF12
+  0x46,  //  EfiKeyPrint
+  0x47,  //  EfiKeySLck
+  0x48   //  EfiKeyPause
+};
+
+//
+// Keyboard modifier value to EFI Scan Code convertion table
+// EFI Scan Code and the modifier values are defined in UEFI spec.
+//
+UINT8 ModifierValueToEfiScanCodeConvertionTable[] = {
+  SCAN_NULL,       // EFI_NULL_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_CONTROL_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_CONTROL_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_ALT_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_ALT_MODIFIER
+  SCAN_NULL,       // EFI_ALT_GR_MODIFIER
+  SCAN_INSERT,     // EFI_INSERT_MODIFIER
+  SCAN_DELETE,     // EFI_DELETE_MODIFIER
+  SCAN_PAGE_DOWN,  // EFI_PAGE_DOWN_MODIFIER
+  SCAN_PAGE_UP,    // EFI_PAGE_UP_MODIFIER
+  SCAN_HOME,       // EFI_HOME_MODIFIER
+  SCAN_END,        // EFI_END_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_SHIFT_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_SHIFT_MODIFIER
+  SCAN_NULL,       // EFI_CAPS_LOCK_MODIFIER
+  SCAN_NULL,       // EFI_NUM_LOCK_MODIFIER
+  SCAN_LEFT,       // EFI_LEFT_ARROW_MODIFIER
+  SCAN_RIGHT,      // EFI_RIGHT_ARROW_MODIFIER
+  SCAN_DOWN,       // EFI_DOWN_ARROW_MODIFIER
+  SCAN_UP,         // EFI_UP_ARROW_MODIFIER
+  SCAN_NULL,       // EFI_NS_KEY_MODIFIER
+  SCAN_NULL,       // EFI_NS_KEY_DEPENDENCY_MODIFIER
+  SCAN_F1,         // EFI_FUNCTION_KEY_ONE_MODIFIER
+  SCAN_F2,         // EFI_FUNCTION_KEY_TWO_MODIFIER
+  SCAN_F3,         // EFI_FUNCTION_KEY_THREE_MODIFIER
+  SCAN_F4,         // EFI_FUNCTION_KEY_FOUR_MODIFIER
+  SCAN_F5,         // EFI_FUNCTION_KEY_FIVE_MODIFIER
+  SCAN_F6,         // EFI_FUNCTION_KEY_SIX_MODIFIER
+  SCAN_F7,         // EFI_FUNCTION_KEY_SEVEN_MODIFIER
+  SCAN_F8,         // EFI_FUNCTION_KEY_EIGHT_MODIFIER
+  SCAN_F9,         // EFI_FUNCTION_KEY_NINE_MODIFIER
+  SCAN_F10,        // EFI_FUNCTION_KEY_TEN_MODIFIER
+  SCAN_F11,        // EFI_FUNCTION_KEY_ELEVEN_MODIFIER
+  SCAN_F12,        // EFI_FUNCTION_KEY_TWELVE_MODIFIER
+  //
+  // For Partial Keystroke support
+  //
+  SCAN_NULL,       // EFI_PRINT_MODIFIER
+  SCAN_NULL,       // EFI_SYS_REQUEST_MODIFIER
+  SCAN_NULL,       // EFI_SCROLL_LOCK_MODIFIER
+  SCAN_PAUSE,      // EFI_PAUSE_MODIFIER
+  SCAN_NULL,       // EFI_BREAK_MODIFIER
+  SCAN_NULL,       // EFI_LEFT_LOGO_MODIFIER
+  SCAN_NULL,       // EFI_RIGHT_LOGO_MODIFER
+  SCAN_NULL,       // EFI_MENU_MODIFER
+};
+
+/**
+  Initialize Key Convention Table by using default keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS          The default keyboard layout was installed successfully
+  @retval Others               Failure to install default keyboard layout.
+**/
+EFI_STATUS
+InstallDefaultKeyboardLayout (
+   IN OUT USB_KB_DEV           *UsbKeyboardDevice
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_DATABASE_PROTOCOL    *HiiDatabase;
+  EFI_HII_HANDLE               HiiHandle;
+
+  //
+  // Locate Hii database protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &HiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Install Keyboard Layout package to HII database
+  //
+  HiiHandle = HiiAddPackages (
+                &gUsbKeyboardLayoutPackageGuid,
+                UsbKeyboardDevice->ControllerHandle,
+                &mUsbKeyboardLayoutBin,
+                NULL
+                );
+  if (HiiHandle == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Set current keyboard layout
+  //
+  Status = HiiDatabase->SetKeyboardLayout (HiiDatabase, &gUsbKeyboardLayoutKeyGuid);
+
+  return Status;
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB keyboard device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB keyboard device.
+  @retval FALSE    Device is a not USB keyboard device.
+
+**/
+BOOLEAN
+IsUSBKeyboard (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if (InterfaceDescriptor.InterfaceClass == CLASS_HID &&
+      InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT &&
+      InterfaceDescriptor.InterfaceProtocol == PROTOCOL_KEYBOARD
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get current keyboard layout from HII database.
+
+  @return Pointer to HII Keyboard Layout.
+          NULL means failure occurred while trying to get keyboard layout.
+
+**/
+EFI_HII_KEYBOARD_LAYOUT *
+GetCurrentKeyboardLayout (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HII_DATABASE_PROTOCOL *HiiDatabase;
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  UINT16                    Length;
+
+  //
+  // Locate HII Database Protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &HiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Get current keyboard layout from HII database
+  //
+  Length = 0;
+  KeyboardLayout = NULL;
+  Status = HiiDatabase->GetKeyboardLayout (
+                          HiiDatabase,
+                          NULL,
+                          &Length,
+                          KeyboardLayout
+                          );
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    KeyboardLayout = AllocatePool (Length);
+    ASSERT (KeyboardLayout != NULL);
+
+    Status = HiiDatabase->GetKeyboardLayout (
+                            HiiDatabase,
+                            NULL,
+                            &Length,
+                            KeyboardLayout
+                            );
+    if (EFI_ERROR (Status)) {
+      FreePool (KeyboardLayout);
+      KeyboardLayout = NULL;
+    }
+  }
+
+  return KeyboardLayout;
+}
+
+/**
+  Find Key Descriptor in Key Convertion Table given its USB keycode.
+
+  @param  UsbKeyboardDevice   The USB_KB_DEV instance.
+  @param  KeyCode             USB Keycode.
+
+  @return The Key Descriptor in Key Convertion Table.
+          NULL means not found.
+
+**/
+EFI_KEY_DESCRIPTOR *
+GetKeyDescriptor (
+  IN USB_KB_DEV        *UsbKeyboardDevice,
+  IN UINT8             KeyCode
+  )
+{
+  UINT8  Index;
+
+  //
+  // Make sure KeyCode is in the range of [0x4, 0x65] or [0xe0, 0xe7]
+  //
+  if ((!USBKBD_VALID_KEYCODE (KeyCode)) || ((KeyCode > 0x65) && (KeyCode < 0xe0)) || (KeyCode > 0xe7)) {
+    return NULL;
+  }
+
+  //
+  // Calculate the index of Key Descriptor in Key Convertion Table
+  //
+  if (KeyCode <= 0x65) {
+    Index = (UINT8) (KeyCode - 4);
+  } else {
+    Index = (UINT8) (KeyCode - 0xe0 + NUMBER_OF_VALID_NON_MODIFIER_USB_KEYCODE);
+  }
+
+  return &UsbKeyboardDevice->KeyConvertionTable[Index];
+}
+
+/**
+  Find Non-Spacing key for given Key descriptor.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyDescriptor        Key descriptor.
+
+  @return The Non-Spacing key corresponding to KeyDescriptor
+          NULL means not found.
+
+**/
+USB_NS_KEY *
+FindUsbNsKey (
+  IN USB_KB_DEV          *UsbKeyboardDevice,
+  IN EFI_KEY_DESCRIPTOR  *KeyDescriptor
+  )
+{
+  LIST_ENTRY      *Link;
+  LIST_ENTRY      *NsKeyList;
+  USB_NS_KEY      *UsbNsKey;
+
+  NsKeyList = &UsbKeyboardDevice->NsKeyList;
+  Link = GetFirstNode (NsKeyList);
+  while (!IsNull (NsKeyList, Link)) {
+    UsbNsKey = USB_NS_KEY_FORM_FROM_LINK (Link);
+
+    if (UsbNsKey->NsKey[0].Key == KeyDescriptor->Key) {
+      return UsbNsKey;
+    }
+
+    Link = GetNextNode (NsKeyList, Link);
+  }
+
+  return NULL;
+}
+
+/**
+  Find physical key definition for a given key descriptor.
+
+  For a specified non-spacing key, there are a list of physical
+  keys following it. This function traverses the list of
+  physical keys and tries to find the physical key matching
+  the KeyDescriptor.
+
+  @param  UsbNsKey          The non-spacing key information.
+  @param  KeyDescriptor     The key descriptor.
+
+  @return The physical key definition.
+          If no physical key is found, parameter KeyDescriptor is returned.
+
+**/
+EFI_KEY_DESCRIPTOR *
+FindPhysicalKey (
+  IN USB_NS_KEY          *UsbNsKey,
+  IN EFI_KEY_DESCRIPTOR  *KeyDescriptor
+  )
+{
+  UINTN               Index;
+  EFI_KEY_DESCRIPTOR  *PhysicalKey;
+
+  PhysicalKey = &UsbNsKey->NsKey[1];
+  for (Index = 0; Index < UsbNsKey->KeyCount; Index++) {
+    if (KeyDescriptor->Key == PhysicalKey->Key) {
+      return PhysicalKey;
+    }
+
+    PhysicalKey++;
+  }
+
+  //
+  // No children definition matched, return original key
+  //
+  return KeyDescriptor;
+}
+
+/**
+  The notification function for EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID.
+
+  This function is registered to event of EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID
+  group type, which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  It tries to get curent keyboard layout from HII database.
+
+  @param  Event        Event being signaled.
+  @param  Context      Points to USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+SetKeyboardLayoutEvent (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  USB_KB_DEV                *UsbKeyboardDevice;
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  EFI_KEY_DESCRIPTOR        TempKey;
+  EFI_KEY_DESCRIPTOR        *KeyDescriptor;
+  EFI_KEY_DESCRIPTOR        *TableEntry;
+  EFI_KEY_DESCRIPTOR        *NsKey;
+  USB_NS_KEY                *UsbNsKey;
+  UINTN                     Index;
+  UINTN                     Index2;
+  UINTN                     KeyCount;
+  UINT8                     KeyCode;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+  if (UsbKeyboardDevice->Signature != USB_KB_DEV_SIGNATURE) {
+    return;
+  }
+
+  //
+  // Try to get current keyboard layout from HII database
+  //
+  KeyboardLayout = GetCurrentKeyboardLayout ();
+  if (KeyboardLayout == NULL) {
+    return;
+  }
+
+  //
+  // Re-allocate resource for KeyConvertionTable
+  //
+  ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+  UsbKeyboardDevice->KeyConvertionTable = AllocateZeroPool ((NUMBER_OF_VALID_USB_KEYCODE) * sizeof (EFI_KEY_DESCRIPTOR));
+  ASSERT (UsbKeyboardDevice->KeyConvertionTable != NULL);
+
+  //
+  // Traverse the list of key descriptors following the header of EFI_HII_KEYBOARD_LAYOUT
+  //
+  KeyDescriptor = (EFI_KEY_DESCRIPTOR *) (((UINT8 *) KeyboardLayout) + sizeof (EFI_HII_KEYBOARD_LAYOUT));
+  for (Index = 0; Index < KeyboardLayout->DescriptorCount; Index++) {
+    //
+    // Copy from HII keyboard layout package binary for alignment
+    //
+    CopyMem (&TempKey, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+    //
+    // Fill the key into KeyConvertionTable, whose index is calculated from USB keycode.
+    //
+    KeyCode = EfiKeyToUsbKeyCodeConvertionTable [(UINT8) (TempKey.Key)];
+    TableEntry = GetKeyDescriptor (UsbKeyboardDevice, KeyCode);
+    if (TableEntry == NULL) {
+      ReleaseKeyboardLayoutResources (UsbKeyboardDevice);
+      FreePool (KeyboardLayout);
+      return;
+    }
+    CopyMem (TableEntry, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+    //
+    // For non-spacing key, create the list with a non-spacing key followed by physical keys.
+    //
+    if (TempKey.Modifier == EFI_NS_KEY_MODIFIER) {
+      UsbNsKey = AllocateZeroPool (sizeof (USB_NS_KEY));
+      ASSERT (UsbNsKey != NULL);
+
+      //
+      // Search for sequential children physical key definitions
+      //
+      KeyCount = 0;
+      NsKey = KeyDescriptor + 1;
+      for (Index2 = (UINT8) Index + 1; Index2 < KeyboardLayout->DescriptorCount; Index2++) {
+        CopyMem (&TempKey, NsKey, sizeof (EFI_KEY_DESCRIPTOR));
+        if (TempKey.Modifier == EFI_NS_KEY_DEPENDENCY_MODIFIER) {
+          KeyCount++;
+        } else {
+          break;
+        }
+        NsKey++;
+      }
+
+      UsbNsKey->Signature = USB_NS_KEY_SIGNATURE;
+      UsbNsKey->KeyCount = KeyCount;
+      UsbNsKey->NsKey = AllocateCopyPool (
+                          (KeyCount + 1) * sizeof (EFI_KEY_DESCRIPTOR),
+                          KeyDescriptor
+                          );
+      InsertTailList (&UsbKeyboardDevice->NsKeyList, &UsbNsKey->Link);
+
+      //
+      // Skip over the child physical keys
+      //
+      Index += KeyCount;
+      KeyDescriptor += KeyCount;
+    }
+
+    KeyDescriptor++;
+  }
+
+  //
+  // There are two EfiKeyEnter, duplicate its key descriptor
+  //
+  TableEntry = GetKeyDescriptor (UsbKeyboardDevice, 0x58);
+  KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, 0x28);
+  CopyMem (TableEntry, KeyDescriptor, sizeof (EFI_KEY_DESCRIPTOR));
+
+  FreePool (KeyboardLayout);
+}
+
+/**
+  Destroy resources for keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+**/
+VOID
+ReleaseKeyboardLayoutResources (
+  IN OUT USB_KB_DEV              *UsbKeyboardDevice
+  )
+{
+  USB_NS_KEY      *UsbNsKey;
+  LIST_ENTRY      *Link;
+
+  if (UsbKeyboardDevice->KeyConvertionTable != NULL) {
+    FreePool (UsbKeyboardDevice->KeyConvertionTable);
+  }
+  UsbKeyboardDevice->KeyConvertionTable = NULL;
+
+  while (!IsListEmpty (&UsbKeyboardDevice->NsKeyList)) {
+    Link = GetFirstNode (&UsbKeyboardDevice->NsKeyList);
+    UsbNsKey = USB_NS_KEY_FORM_FROM_LINK (Link);
+    RemoveEntryList (&UsbNsKey->Link);
+
+    FreePool (UsbNsKey->NsKey);
+    FreePool (UsbNsKey);
+  }
+}
+
+/**
+  Initialize USB keyboard layout.
+
+  This function initializes Key Convertion Table for the USB keyboard device.
+  It first tries to retrieve layout from HII database. If failed and default
+  layout is enabled, then it just uses the default layout.
+
+  @param  UsbKeyboardDevice      The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS            Initialization succeeded.
+  @retval EFI_NOT_READY          Keyboard layout cannot be retrieve from HII
+                                 database, and default layout is disabled.
+  @retval Other                  Fail to register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group.
+
+**/
+EFI_STATUS
+InitKeyboardLayout (
+  OUT USB_KB_DEV   *UsbKeyboardDevice
+  )
+{
+  EFI_HII_KEYBOARD_LAYOUT   *KeyboardLayout;
+  EFI_STATUS                Status;
+
+  UsbKeyboardDevice->KeyConvertionTable = AllocateZeroPool ((NUMBER_OF_VALID_USB_KEYCODE) * sizeof (EFI_KEY_DESCRIPTOR));
+  ASSERT (UsbKeyboardDevice->KeyConvertionTable != NULL);
+
+  InitializeListHead (&UsbKeyboardDevice->NsKeyList);
+  UsbKeyboardDevice->CurrentNsKey = NULL;
+  UsbKeyboardDevice->KeyboardLayoutEvent = NULL;
+
+  //
+  // Register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group,
+  // which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  SetKeyboardLayoutEvent,
+                  UsbKeyboardDevice,
+                  &gEfiHiiKeyBoardLayoutGuid,
+                  &UsbKeyboardDevice->KeyboardLayoutEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  KeyboardLayout = GetCurrentKeyboardLayout ();
+  if (KeyboardLayout != NULL) {
+    //
+    // If current keyboard layout is successfully retrieved from HII database,
+    // force to initialize the keyboard layout.
+    //
+    gBS->SignalEvent (UsbKeyboardDevice->KeyboardLayoutEvent);
+  } else {
+    if (FeaturePcdGet (PcdDisableDefaultKeyboardLayoutInUsbKbDriver)) {
+      //
+      // If no keyboard layout can be retrieved from HII database, and default layout
+      // is disabled, then return EFI_NOT_READY.
+      //
+      return EFI_NOT_READY;
+    }
+    //
+    // If no keyboard layout can be retrieved from HII database, and default layout
+    // is enabled, then load the default keyboard layout.
+    //
+    InstallDefaultKeyboardLayout (UsbKeyboardDevice);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize USB keyboard device and all private data structures.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS        Initialization is successful.
+  @retval EFI_DEVICE_ERROR   Keyboard initialization failed.
+
+**/
+EFI_STATUS
+InitUSBKeyboard (
+  IN OUT USB_KB_DEV   *UsbKeyboardDevice
+  )
+{
+  UINT16              ConfigValue;
+  UINT8               Protocol;
+  EFI_STATUS          Status;
+  UINT32              TransferResult;
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_KEYBOARD | EFI_P_KEYBOARD_PC_SELF_TEST),
+    UsbKeyboardDevice->DevicePath
+    );
+
+  InitQueue (&UsbKeyboardDevice->UsbKeyQueue, sizeof (USB_KEY));
+  InitQueue (&UsbKeyboardDevice->EfiKeyQueue, sizeof (EFI_KEY_DATA));
+
+  //
+  // Use the config out of the descriptor
+  // Assumed the first config is the correct one and this is not always the case
+  //
+  Status = UsbGetConfiguration (
+             UsbKeyboardDevice->UsbIo,
+             &ConfigValue,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    ConfigValue = 0x01;
+    //
+    // Uses default configuration to configure the USB Keyboard device.
+    //
+    Status = UsbSetConfiguration (
+               UsbKeyboardDevice->UsbIo,
+               ConfigValue,
+               &TransferResult
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // If configuration could not be set here, it means
+      // the keyboard interface has some errors and could
+      // not be initialized
+      //
+      REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+        EFI_ERROR_CODE | EFI_ERROR_MINOR,
+        (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_INTERFACE_ERROR),
+        UsbKeyboardDevice->DevicePath
+        );
+
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  UsbGetProtocolRequest (
+    UsbKeyboardDevice->UsbIo,
+    UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  //
+  // Set boot protocol for the USB Keyboard.
+  // This driver only supports boot protocol.
+  //
+  if (Protocol != BOOT_PROTOCOL) {
+    UsbSetProtocolRequest (
+      UsbKeyboardDevice->UsbIo,
+      UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+      BOOT_PROTOCOL
+      );
+  }
+
+  UsbKeyboardDevice->CtrlOn     = FALSE;
+  UsbKeyboardDevice->AltOn      = FALSE;
+  UsbKeyboardDevice->ShiftOn    = FALSE;
+  UsbKeyboardDevice->NumLockOn  = FALSE;
+  UsbKeyboardDevice->CapsOn     = FALSE;
+  UsbKeyboardDevice->ScrollOn   = FALSE;
+
+  UsbKeyboardDevice->LeftCtrlOn   = FALSE;
+  UsbKeyboardDevice->LeftAltOn    = FALSE;
+  UsbKeyboardDevice->LeftShiftOn  = FALSE;
+  UsbKeyboardDevice->LeftLogoOn   = FALSE;
+  UsbKeyboardDevice->RightCtrlOn  = FALSE;
+  UsbKeyboardDevice->RightAltOn   = FALSE;
+  UsbKeyboardDevice->RightShiftOn = FALSE;
+  UsbKeyboardDevice->RightLogoOn  = FALSE;
+  UsbKeyboardDevice->MenuKeyOn    = FALSE;
+  UsbKeyboardDevice->SysReqOn     = FALSE;
+
+  UsbKeyboardDevice->AltGrOn      = FALSE;
+
+  UsbKeyboardDevice->CurrentNsKey = NULL;
+
+  //
+  // Sync the initial state of lights on keyboard.
+  //
+  SetKeyLED (UsbKeyboardDevice);
+
+  ZeroMem (UsbKeyboardDevice->LastKeyCodeArray, sizeof (UINT8) * 8);
+
+  //
+  // Create event for repeat keys' generation.
+  //
+  if (UsbKeyboardDevice->RepeatTimer != NULL) {
+    gBS->CloseEvent (UsbKeyboardDevice->RepeatTimer);
+    UsbKeyboardDevice->RepeatTimer = NULL;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_CALLBACK,
+         USBKeyboardRepeatHandler,
+         UsbKeyboardDevice,
+         &UsbKeyboardDevice->RepeatTimer
+         );
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbKeyboardDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbKeyboardDevice->DelayedRecoveryEvent);
+    UsbKeyboardDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBKeyboardRecoveryHandler,
+         UsbKeyboardDevice,
+         &UsbKeyboardDevice->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB keyboard's asynchronous interrupt transfer.
+
+  This function is the handler function for USB keyboard's asynchronous interrupt transfer
+  to manage the keyboard. It parses the USB keyboard input report, and inserts data to
+  keyboard buffer according to state of modifer keys and normal keys. Timer for repeat key
+  is also set accordingly.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+KeyboardHandler (
+  IN  VOID          *Data,
+  IN  UINTN         DataLength,
+  IN  VOID          *Context,
+  IN  UINT32        Result
+  )
+{
+  USB_KB_DEV          *UsbKeyboardDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               *CurKeyCodeBuffer;
+  UINT8               *OldKeyCodeBuffer;
+  UINT8               CurModifierMap;
+  UINT8               OldModifierMap;
+  UINT8               Mask;
+  UINTN               Index;
+  UINT8               Index2;
+  BOOLEAN             KeyRelease;
+  BOOLEAN             KeyPress;
+  USB_KEY             UsbKey;
+  UINT8               NewRepeatKey;
+  UINT32              UsbStatus;
+  EFI_KEY_DESCRIPTOR  *KeyDescriptor;
+
+  ASSERT (Context != NULL);
+
+  NewRepeatKey      = 0;
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+  UsbIo             = UsbKeyboardDevice->UsbIo;
+
+  //
+  // Analyzes Result and performs corresponding action.
+  //
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_KEYBOARD | EFI_P_EC_INPUT_ERROR),
+      UsbKeyboardDevice->DevicePath
+      );
+
+    //
+    // Stop the repeat key generation if any
+    //
+    UsbKeyboardDevice->RepeatKey = 0;
+
+    gBS->SetTimer (
+          UsbKeyboardDevice->RepeatTimer,
+          TimerCancel,
+          USBKBD_REPEAT_RATE
+          );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      UsbClearEndpointHalt (
+        UsbIo,
+        UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+        &UsbStatus
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt.
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Following code checks current keyboard input report against old key code buffer.
+  // According to USB HID Firmware Specification, the report consists of 8 bytes.
+  // Byte 0 is map of Modifier keys.
+  // Byte 1 is reserved.
+  // Bytes 2 to 7 are keycodes.
+  //
+  CurKeyCodeBuffer  = (UINT8 *) Data;
+  OldKeyCodeBuffer  = UsbKeyboardDevice->LastKeyCodeArray;
+
+  //
+  // Checks for new key stroke.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    if (OldKeyCodeBuffer[Index] != CurKeyCodeBuffer[Index]) {
+      break;
+    }
+  }
+
+  //
+  // If no new key, return EFI_SUCCESS immediately.
+  //
+  if (Index == 8) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Parse the modifier key, which is the first byte of keyboard input report.
+  //
+  CurModifierMap  = CurKeyCodeBuffer[0];
+  OldModifierMap  = OldKeyCodeBuffer[0];
+
+  //
+  // Handle modifier key's pressing or releasing situation.
+  // According to USB HID Firmware spec, Byte 0 uses folloing map of Modifier keys:
+  // Bit0: Left Control,  Keycode: 0xe0
+  // Bit1: Left Shift,    Keycode: 0xe1
+  // Bit2: Left Alt,      Keycode: 0xe2
+  // Bit3: Left GUI,      Keycode: 0xe3
+  // Bit4: Right Control, Keycode: 0xe4
+  // Bit5: Right Shift,   Keycode: 0xe5
+  // Bit6: Right Alt,     Keycode: 0xe6
+  // Bit7: Right GUI,     Keycode: 0xe7
+  //
+  for (Index = 0; Index < 8; Index++) {
+    Mask = (UINT8) (1 << Index);
+    if ((CurModifierMap & Mask) != (OldModifierMap & Mask)) {
+      //
+      // If current modifier key is up, then CurModifierMap & Mask = 0;
+      // otherwise it is a non-zero value.
+      // Insert the changed modifier key into key buffer.
+      //
+      UsbKey.KeyCode = (UINT8) (0xe0 + Index);
+      UsbKey.Down    = (BOOLEAN) ((CurModifierMap & Mask) != 0);
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+    }
+  }
+
+  //
+  // Handle normal key's releasing situation
+  // Bytes 2 to 7 are for normal keycodes
+  //
+  KeyRelease = FALSE;
+  for (Index = 2; Index < 8; Index++) {
+
+    if (!USBKBD_VALID_KEYCODE (OldKeyCodeBuffer[Index])) {
+      continue;
+    }
+    //
+    // For any key in old keycode buffer, if it is not in current keycode buffer,
+    // then it is released. Otherwise, it is not released.
+    //
+    KeyRelease = TRUE;
+    for (Index2 = 2; Index2 < 8; Index2++) {
+
+      if (!USBKBD_VALID_KEYCODE (CurKeyCodeBuffer[Index2])) {
+        continue;
+      }
+
+      if (OldKeyCodeBuffer[Index] == CurKeyCodeBuffer[Index2]) {
+        KeyRelease = FALSE;
+        break;
+      }
+    }
+
+    if (KeyRelease) {
+      UsbKey.KeyCode = OldKeyCodeBuffer[Index];
+      UsbKey.Down    = FALSE;
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+      //
+      // The original repeat key is released.
+      //
+      if (OldKeyCodeBuffer[Index] == UsbKeyboardDevice->RepeatKey) {
+        UsbKeyboardDevice->RepeatKey = 0;
+      }
+    }
+  }
+
+  //
+  // If original repeat key is released, cancel the repeat timer
+  //
+  if (UsbKeyboardDevice->RepeatKey == 0) {
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerCancel,
+           USBKBD_REPEAT_RATE
+           );
+  }
+
+  //
+  // Handle normal key's pressing situation
+  //
+  KeyPress = FALSE;
+  for (Index = 2; Index < 8; Index++) {
+
+    if (!USBKBD_VALID_KEYCODE (CurKeyCodeBuffer[Index])) {
+      continue;
+    }
+    //
+    // For any key in current keycode buffer, if it is not in old keycode buffer,
+    // then it is pressed. Otherwise, it is not pressed.
+    //
+    KeyPress = TRUE;
+    for (Index2 = 2; Index2 < 8; Index2++) {
+
+      if (!USBKBD_VALID_KEYCODE (OldKeyCodeBuffer[Index2])) {
+        continue;
+      }
+
+      if (CurKeyCodeBuffer[Index] == OldKeyCodeBuffer[Index2]) {
+        KeyPress = FALSE;
+        break;
+      }
+    }
+
+    if (KeyPress) {
+      UsbKey.KeyCode = CurKeyCodeBuffer[Index];
+      UsbKey.Down    = TRUE;
+      Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+      //
+      // Handle repeat key
+      //
+      KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, CurKeyCodeBuffer[Index]);
+      ASSERT (KeyDescriptor != NULL);
+
+      if (KeyDescriptor->Modifier == EFI_NUM_LOCK_MODIFIER || KeyDescriptor->Modifier == EFI_CAPS_LOCK_MODIFIER) {
+        //
+        // For NumLock or CapsLock pressed, there is no need to handle repeat key for them.
+        //
+        UsbKeyboardDevice->RepeatKey = 0;
+      } else {
+        //
+        // Prepare new repeat key, and clear the original one.
+        //
+        NewRepeatKey = CurKeyCodeBuffer[Index];
+        UsbKeyboardDevice->RepeatKey = 0;
+      }
+    }
+  }
+
+  //
+  // Update LastKeycodeArray buffer in the UsbKeyboardDevice data structure.
+  //
+  for (Index = 0; Index < 8; Index++) {
+    UsbKeyboardDevice->LastKeyCodeArray[Index] = CurKeyCodeBuffer[Index];
+  }
+
+  //
+  // If there is new key pressed, update the RepeatKey value, and set the
+  // timer to repeate delay timer
+  //
+  if (NewRepeatKey != 0) {
+    //
+    // Sets trigger time to "Repeat Delay Time",
+    // to trigger the repeat timer when the key is hold long
+    // enough time.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerRelative,
+           USBKBD_REPEAT_DELAY
+           );
+    UsbKeyboardDevice->RepeatKey = NewRepeatKey;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieves a USB keycode after parsing the raw data in keyboard buffer.
+
+  This function parses keyboard buffer. It updates state of modifier key for
+  USB_KB_DEV instancem, and returns keycode for output.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyCode              Pointer to the USB keycode for output.
+
+  @retval EFI_SUCCESS          Keycode successfully parsed.
+  @retval EFI_NOT_READY        Keyboard buffer is not ready for a valid keycode
+
+**/
+EFI_STATUS
+USBParseKey (
+  IN OUT  USB_KB_DEV  *UsbKeyboardDevice,
+     OUT  UINT8       *KeyCode
+  )
+{
+  USB_KEY             UsbKey;
+  EFI_KEY_DESCRIPTOR  *KeyDescriptor;
+
+  *KeyCode = 0;
+
+  while (!IsQueueEmpty (&UsbKeyboardDevice->UsbKeyQueue)) {
+    //
+    // Pops one raw data off.
+    //
+    Dequeue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+    KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, UsbKey.KeyCode);
+    ASSERT (KeyDescriptor != NULL);
+
+    if (!UsbKey.Down) {
+      //
+      // Key is released.
+      //
+      switch (KeyDescriptor->Modifier) {
+
+      //
+      // Ctrl release
+      //
+      case EFI_LEFT_CONTROL_MODIFIER:
+        UsbKeyboardDevice->LeftCtrlOn = FALSE;
+        UsbKeyboardDevice->CtrlOn = FALSE;
+        break;
+      case EFI_RIGHT_CONTROL_MODIFIER:
+        UsbKeyboardDevice->RightCtrlOn = FALSE;
+        UsbKeyboardDevice->CtrlOn = FALSE;
+        break;
+
+      //
+      // Shift release
+      //
+      case EFI_LEFT_SHIFT_MODIFIER:
+        UsbKeyboardDevice->LeftShiftOn = FALSE;
+        UsbKeyboardDevice->ShiftOn = FALSE;
+        break;
+      case EFI_RIGHT_SHIFT_MODIFIER:
+        UsbKeyboardDevice->RightShiftOn = FALSE;
+        UsbKeyboardDevice->ShiftOn = FALSE;
+        break;
+
+      //
+      // Alt release
+      //
+      case EFI_LEFT_ALT_MODIFIER:
+        UsbKeyboardDevice->LeftAltOn = FALSE;
+        UsbKeyboardDevice->AltOn = FALSE;
+        break;
+      case EFI_RIGHT_ALT_MODIFIER:
+        UsbKeyboardDevice->RightAltOn = FALSE;
+        UsbKeyboardDevice->AltOn = FALSE;
+        break;
+
+      //
+      // Left Logo release
+      //
+      case EFI_LEFT_LOGO_MODIFIER:
+        UsbKeyboardDevice->LeftLogoOn = FALSE;
+        break;
+
+      //
+      // Right Logo release
+      //
+      case EFI_RIGHT_LOGO_MODIFIER:
+        UsbKeyboardDevice->RightLogoOn = FALSE;
+        break;
+
+      //
+      // Menu key release
+      //
+      case EFI_MENU_MODIFIER:
+        UsbKeyboardDevice->MenuKeyOn = FALSE;
+        break;
+
+      //
+      // SysReq release
+      //
+      case EFI_PRINT_MODIFIER:
+      case EFI_SYS_REQUEST_MODIFIER:
+        UsbKeyboardDevice->SysReqOn = FALSE;
+        break;
+
+      //
+      // AltGr release
+      //
+      case EFI_ALT_GR_MODIFIER:
+        UsbKeyboardDevice->AltGrOn = FALSE;
+        break;
+
+      default:
+        break;
+      }
+
+      continue;
+    }
+
+    //
+    // Analyzes key pressing situation
+    //
+    switch (KeyDescriptor->Modifier) {
+
+    //
+    // Ctrl press
+    //
+    case EFI_LEFT_CONTROL_MODIFIER:
+      UsbKeyboardDevice->LeftCtrlOn = TRUE;
+      UsbKeyboardDevice->CtrlOn = TRUE;
+      break;
+    case EFI_RIGHT_CONTROL_MODIFIER:
+      UsbKeyboardDevice->RightCtrlOn = TRUE;
+      UsbKeyboardDevice->CtrlOn = TRUE;
+      break;
+
+    //
+    // Shift press
+    //
+    case EFI_LEFT_SHIFT_MODIFIER:
+      UsbKeyboardDevice->LeftShiftOn = TRUE;
+      UsbKeyboardDevice->ShiftOn = TRUE;
+      break;
+    case EFI_RIGHT_SHIFT_MODIFIER:
+      UsbKeyboardDevice->RightShiftOn = TRUE;
+      UsbKeyboardDevice->ShiftOn = TRUE;
+      break;
+
+    //
+    // Alt press
+    //
+    case EFI_LEFT_ALT_MODIFIER:
+      UsbKeyboardDevice->LeftAltOn = TRUE;
+      UsbKeyboardDevice->AltOn = TRUE;
+      break;
+    case EFI_RIGHT_ALT_MODIFIER:
+      UsbKeyboardDevice->RightAltOn = TRUE;
+      UsbKeyboardDevice->AltOn = TRUE;
+      break;
+
+    //
+    // Left Logo press
+    //
+    case EFI_LEFT_LOGO_MODIFIER:
+      UsbKeyboardDevice->LeftLogoOn = TRUE;
+      break;
+
+    //
+    // Right Logo press
+    //
+    case EFI_RIGHT_LOGO_MODIFIER:
+      UsbKeyboardDevice->RightLogoOn = TRUE;
+      break;
+
+    //
+    // Menu key press
+    //
+    case EFI_MENU_MODIFIER:
+      UsbKeyboardDevice->MenuKeyOn = TRUE;
+      break;
+
+    //
+    // SysReq press
+    //
+    case EFI_PRINT_MODIFIER:
+    case EFI_SYS_REQUEST_MODIFIER:
+      UsbKeyboardDevice->SysReqOn = TRUE;
+      break;
+
+    //
+    // AltGr press
+    //
+    case EFI_ALT_GR_MODIFIER:
+      UsbKeyboardDevice->AltGrOn = TRUE;
+      break;
+
+    case EFI_NUM_LOCK_MODIFIER:
+      //
+      // Toggle NumLock
+      //
+      UsbKeyboardDevice->NumLockOn = (BOOLEAN) (!(UsbKeyboardDevice->NumLockOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    case EFI_CAPS_LOCK_MODIFIER:
+      //
+      // Toggle CapsLock
+      //
+      UsbKeyboardDevice->CapsOn = (BOOLEAN) (!(UsbKeyboardDevice->CapsOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    case EFI_SCROLL_LOCK_MODIFIER:
+      //
+      // Toggle ScrollLock
+      //
+      UsbKeyboardDevice->ScrollOn = (BOOLEAN) (!(UsbKeyboardDevice->ScrollOn));
+      SetKeyLED (UsbKeyboardDevice);
+      break;
+
+    default:
+      break;
+    }
+
+    //
+    // When encountering Ctrl + Alt + Del, then warm reset.
+    //
+    if (KeyDescriptor->Modifier == EFI_DELETE_MODIFIER) {
+      if ((UsbKeyboardDevice->CtrlOn) && (UsbKeyboardDevice->AltOn)) {
+        gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+      }
+    }
+
+    *KeyCode = UsbKey.KeyCode;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_READY;
+}
+
+
+/**
+  Converts USB Keycode ranging from 0x4 to 0x65 to EFI_INPUT_KEY.
+
+  @param  UsbKeyboardDevice     The USB_KB_DEV instance.
+  @param  KeyCode               Indicates the key code that will be interpreted.
+  @param  KeyData               A pointer to a buffer that is filled in with
+                                the keystroke information for the key that
+                                was pressed.
+
+  @retval EFI_SUCCESS           Success.
+  @retval EFI_INVALID_PARAMETER KeyCode is not in the range of 0x4 to 0x65.
+  @retval EFI_INVALID_PARAMETER Translated EFI_INPUT_KEY has zero for both ScanCode and UnicodeChar.
+  @retval EFI_NOT_READY         KeyCode represents a dead key with EFI_NS_KEY_MODIFIER
+  @retval EFI_DEVICE_ERROR      Keyboard layout is invalid.
+
+**/
+EFI_STATUS
+UsbKeyCodeToEfiInputKey (
+  IN  USB_KB_DEV                *UsbKeyboardDevice,
+  IN  UINT8                     KeyCode,
+  OUT EFI_KEY_DATA              *KeyData
+  )
+{
+  EFI_KEY_DESCRIPTOR            *KeyDescriptor;
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *NotifyList;
+  KEYBOARD_CONSOLE_IN_EX_NOTIFY *CurrentNotify;
+
+  //
+  // KeyCode must in the range of  [0x4, 0x65] or [0xe0, 0xe7].
+  //
+  KeyDescriptor = GetKeyDescriptor (UsbKeyboardDevice, KeyCode);
+  ASSERT (KeyDescriptor != NULL);
+
+  if (KeyDescriptor->Modifier == EFI_NS_KEY_MODIFIER) {
+    //
+    // If this is a dead key with EFI_NS_KEY_MODIFIER, then record it and return.
+    //
+    UsbKeyboardDevice->CurrentNsKey = FindUsbNsKey (UsbKeyboardDevice, KeyDescriptor);
+    return EFI_NOT_READY;
+  }
+
+  if (UsbKeyboardDevice->CurrentNsKey != NULL) {
+    //
+    // If this keystroke follows a non-spacing key, then find the descriptor for corresponding
+    // physical key.
+    //
+    KeyDescriptor = FindPhysicalKey (UsbKeyboardDevice->CurrentNsKey, KeyDescriptor);
+    UsbKeyboardDevice->CurrentNsKey = NULL;
+  }
+
+  //
+  // Make sure modifier of Key Descriptor is in the valid range according to UEFI spec.
+  //
+  if (KeyDescriptor->Modifier >= (sizeof (ModifierValueToEfiScanCodeConvertionTable) / sizeof (UINT8))) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  KeyData->Key.ScanCode    = ModifierValueToEfiScanCodeConvertionTable[KeyDescriptor->Modifier];
+  KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_STANDARD_SHIFT)!= 0) {
+    if (UsbKeyboardDevice->ShiftOn) {
+      KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedUnicode;
+
+      //
+      // Need not return associated shift state if a class of printable characters that
+      // are normally adjusted by shift modifiers. e.g. Shift Key + 'f' key = 'F'
+      //
+      if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_CAPS_LOCK) != 0) {
+        UsbKeyboardDevice->LeftShiftOn = FALSE;
+        UsbKeyboardDevice->RightShiftOn = FALSE;
+      }
+
+      if (UsbKeyboardDevice->AltGrOn) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedAltGrUnicode;
+      }
+    } else {
+      //
+      // Shift off
+      //
+      KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+
+      if (UsbKeyboardDevice->AltGrOn) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->AltGrUnicode;
+      }
+    }
+  }
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_CAPS_LOCK) != 0) {
+    if (UsbKeyboardDevice->CapsOn) {
+      if (KeyData->Key.UnicodeChar == KeyDescriptor->Unicode) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->ShiftedUnicode;
+      } else if (KeyData->Key.UnicodeChar == KeyDescriptor->ShiftedUnicode) {
+        KeyData->Key.UnicodeChar = KeyDescriptor->Unicode;
+      }
+    }
+  }
+
+  if ((KeyDescriptor->AffectedAttribute & EFI_AFFECTED_BY_NUM_LOCK) != 0) {
+    //
+    // For key affected by NumLock, if NumLock is on and Shift is not pressed, then it means
+    // normal key, instead of original control key. So the ScanCode should be cleaned.
+    // Otherwise, it means control key, so preserve the EFI Scan Code and clear the unicode keycode.
+    //
+    if ((UsbKeyboardDevice->NumLockOn) && (!(UsbKeyboardDevice->ShiftOn))) {
+      KeyData->Key.ScanCode = SCAN_NULL;
+    } else {
+      KeyData->Key.UnicodeChar = CHAR_NULL;
+    }
+  }
+
+  //
+  // Translate Unicode 0x1B (ESC) to EFI Scan Code
+  //
+  if (KeyData->Key.UnicodeChar == 0x1B && KeyData->Key.ScanCode == SCAN_NULL) {
+    KeyData->Key.ScanCode = SCAN_ESC;
+    KeyData->Key.UnicodeChar = CHAR_NULL;
+  }
+
+  //
+  // Not valid for key without both unicode key code and EFI Scan Code.
+  //
+  if (KeyData->Key.UnicodeChar == 0 && KeyData->Key.ScanCode == SCAN_NULL) {
+    if (!UsbKeyboardDevice->IsSupportPartialKey) {
+    return EFI_NOT_READY;
+    }
+  }
+
+  //
+  // Save Shift/Toggle state
+  //
+  KeyData->KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID;
+  KeyData->KeyState.KeyToggleState = EFI_TOGGLE_STATE_VALID;
+
+  if (UsbKeyboardDevice->LeftCtrlOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_CONTROL_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightCtrlOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_CONTROL_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftAltOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_ALT_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightAltOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_ALT_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftShiftOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_SHIFT_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightShiftOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_SHIFT_PRESSED;
+  }
+  if (UsbKeyboardDevice->LeftLogoOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_LEFT_LOGO_PRESSED;
+  }
+  if (UsbKeyboardDevice->RightLogoOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_RIGHT_LOGO_PRESSED;
+  }
+  if (UsbKeyboardDevice->MenuKeyOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_MENU_KEY_PRESSED;
+  }
+  if (UsbKeyboardDevice->SysReqOn) {
+    KeyData->KeyState.KeyShiftState |= EFI_SYS_REQ_PRESSED;
+  }
+
+  if (UsbKeyboardDevice->ScrollOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_SCROLL_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->NumLockOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_NUM_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->CapsOn) {
+    KeyData->KeyState.KeyToggleState |= EFI_CAPS_LOCK_ACTIVE;
+  }
+  if (UsbKeyboardDevice->IsSupportPartialKey) {
+    KeyData->KeyState.KeyToggleState |= EFI_KEY_STATE_EXPOSED;
+  }
+  //
+  // Invoke notification functions if the key is registered.
+  //
+  NotifyList = &UsbKeyboardDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList); !IsNull (NotifyList, Link); Link = GetNextNode (NotifyList, Link)) {
+    CurrentNotify = CR (Link, KEYBOARD_CONSOLE_IN_EX_NOTIFY, NotifyEntry, USB_KB_CONSOLE_IN_EX_NOTIFY_SIGNATURE);
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      CurrentNotify->KeyNotificationFn (KeyData);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create the queue.
+
+  @param  Queue     Points to the queue.
+  @param  ItemSize  Size of the single item.
+
+**/
+VOID
+InitQueue (
+  IN OUT  USB_SIMPLE_QUEUE   *Queue,
+  IN      UINTN              ItemSize
+  )
+{
+  UINTN                      Index;
+
+  Queue->ItemSize  = ItemSize;
+  Queue->Head      = 0;
+  Queue->Tail      = 0;
+
+  if (Queue->Buffer[0] != NULL) {
+    FreePool (Queue->Buffer[0]);
+  }
+
+  Queue->Buffer[0] = AllocatePool (sizeof (Queue->Buffer) / sizeof (Queue->Buffer[0]) * ItemSize);
+  ASSERT (Queue->Buffer[0] != NULL);
+
+  for (Index = 1; Index < sizeof (Queue->Buffer) / sizeof (Queue->Buffer[0]); Index++) {
+    Queue->Buffer[Index] = ((UINT8 *) Queue->Buffer[Index - 1]) + ItemSize;
+  }
+}
+
+/**
+  Destroy the queue
+
+  @param Queue    Points to the queue.
+**/
+VOID
+DestroyQueue (
+  IN OUT USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  FreePool (Queue->Buffer[0]);
+}
+
+
+/**
+  Check whether the queue is empty.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is empty.
+  @retval FALSE     Queue is not empty.
+
+**/
+BOOLEAN
+IsQueueEmpty (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  //
+  // Meet FIFO empty condition
+  //
+  return (BOOLEAN) (Queue->Head == Queue->Tail);
+}
+
+
+/**
+  Check whether the queue is full.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is full.
+  @retval FALSE     Queue is not full.
+
+**/
+BOOLEAN
+IsQueueFull (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  )
+{
+  return (BOOLEAN) (((Queue->Tail + 1) % (MAX_KEY_ALLOWED + 1)) == Queue->Head);
+}
+
+
+/**
+  Enqueue the item to the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Points to the item to be enqueued.
+  @param  ItemSize  Size of the item.
+**/
+VOID
+Enqueue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+  IN      VOID             *Item,
+  IN      UINTN            ItemSize
+  )
+{
+  ASSERT (ItemSize == Queue->ItemSize);
+  //
+  // If keyboard buffer is full, throw the
+  // first key out of the keyboard buffer.
+  //
+  if (IsQueueFull (Queue)) {
+    Queue->Head = (Queue->Head + 1) % (MAX_KEY_ALLOWED + 1);
+  }
+
+  CopyMem (Queue->Buffer[Queue->Tail], Item, ItemSize);
+
+  //
+  // Adjust the tail pointer of the FIFO keyboard buffer.
+  //
+  Queue->Tail = (Queue->Tail + 1) % (MAX_KEY_ALLOWED + 1);
+}
+
+
+/**
+  Dequeue a item from the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Receives the item.
+  @param  ItemSize  Size of the item.
+
+  @retval EFI_SUCCESS        Item was successfully dequeued.
+  @retval EFI_DEVICE_ERROR   The queue is empty.
+
+**/
+EFI_STATUS
+Dequeue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+     OUT  VOID             *Item,
+  IN      UINTN            ItemSize
+  )
+{
+  ASSERT (Queue->ItemSize == ItemSize);
+
+  if (IsQueueEmpty (Queue)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  CopyMem (Item, Queue->Buffer[Queue->Head], ItemSize);
+
+  //
+  // Adjust the head pointer of the FIFO keyboard buffer.
+  //
+  Queue->Head = (Queue->Head + 1) % (MAX_KEY_ALLOWED + 1);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets USB keyboard LED state.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+**/
+VOID
+SetKeyLED (
+  IN  USB_KB_DEV    *UsbKeyboardDevice
+  )
+{
+  LED_MAP Led;
+  UINT8   ReportId;
+
+  //
+  // Set each field in Led map.
+  //
+  Led.NumLock    = (UINT8) ((UsbKeyboardDevice->NumLockOn) ? 1 : 0);
+  Led.CapsLock   = (UINT8) ((UsbKeyboardDevice->CapsOn) ? 1 : 0);
+  Led.ScrollLock = (UINT8) ((UsbKeyboardDevice->ScrollOn) ? 1 : 0);
+  Led.Resrvd     = 0;
+
+  ReportId       = 0;
+  //
+  // Call Set_Report Request to lighten the LED.
+  //
+  UsbSetReportRequest (
+    UsbKeyboardDevice->UsbIo,
+    UsbKeyboardDevice->InterfaceDescriptor.InterfaceNumber,
+    ReportId,
+    HID_OUTPUT_REPORT,
+    1,
+    (UINT8 *) &Led
+    );
+}
+
+
+/**
+  Handler for Repeat Key event.
+
+  This function is the handler for Repeat Key event triggered
+  by timer.
+  After a repeatable key is pressed, the event would be triggered
+  with interval of USBKBD_REPEAT_DELAY. Once the event is triggered,
+  following trigger will come with interval of USBKBD_REPEAT_RATE.
+
+  @param  Event              The Repeat Key event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRepeatHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_KB_DEV  *UsbKeyboardDevice;
+  USB_KEY     UsbKey;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  //
+  // Do nothing when there is no repeat key.
+  //
+  if (UsbKeyboardDevice->RepeatKey != 0) {
+    //
+    // Inserts the repeat key into keyboard buffer,
+    //
+    UsbKey.KeyCode = UsbKeyboardDevice->RepeatKey;
+    UsbKey.Down    = TRUE;
+    Enqueue (&UsbKeyboardDevice->UsbKeyQueue, &UsbKey, sizeof (UsbKey));
+
+    //
+    // Set repeat rate for next repeat key generation.
+    //
+    gBS->SetTimer (
+           UsbKeyboardDevice->RepeatTimer,
+           TimerRelative,
+           USBKBD_REPEAT_RATE
+           );
+  }
+}
+
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+
+  USB_KB_DEV          *UsbKeyboardDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               PacketSize;
+
+  UsbKeyboardDevice = (USB_KB_DEV *) Context;
+
+  UsbIo             = UsbKeyboardDevice->UsbIo;
+
+  PacketSize        = (UINT8) (UsbKeyboardDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbKeyboardDevice->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbKeyboardDevice->IntEndpointDescriptor.Interval,
+           PacketSize,
+           KeyboardHandler,
+           UsbKeyboardDevice
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h
new file mode 100644
index 0000000000..b3eb3e462f
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/KeyBoard.h
@@ -0,0 +1,320 @@
+/** @file
+  Function prototype for USB Keyboard Driver.
+
+Copyright (c) 2004 - 2013, 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.
+
+**/
+
+#ifndef _EFI_KEYBOARD_H_
+#define _EFI_KEYBOARD_H_
+
+
+#include "EfiKey.h"
+
+#define USB_KEYBOARD_KEY_COUNT            105
+
+#define USB_KEYBOARD_LANGUAGE_STR_LEN     5         // RFC4646 Language Code: "en-US"
+#define USB_KEYBOARD_DESCRIPTION_STR_LEN  (16 + 1)  // Description: "English Keyboard"
+
+#pragma pack (1)
+typedef struct {
+  //
+  // This 4-bytes total array length is required by PreparePackageList()
+  //
+  UINT32                 Length;
+
+  //
+  // Keyboard Layout package definition
+  //
+  EFI_HII_PACKAGE_HEADER PackageHeader;
+  UINT16                 LayoutCount;
+
+  //
+  // EFI_HII_KEYBOARD_LAYOUT
+  //
+  UINT16                 LayoutLength;
+  EFI_GUID               Guid;
+  UINT32                 LayoutDescriptorStringOffset;
+  UINT8                  DescriptorCount;
+  EFI_KEY_DESCRIPTOR     KeyDescriptor[USB_KEYBOARD_KEY_COUNT];
+  UINT16                 DescriptionCount;
+  CHAR16                 Language[USB_KEYBOARD_LANGUAGE_STR_LEN];
+  CHAR16                 Space;
+  CHAR16                 DescriptionString[USB_KEYBOARD_DESCRIPTION_STR_LEN];
+} USB_KEYBOARD_LAYOUT_PACK_BIN;
+#pragma pack()
+/**
+  Uses USB I/O to check whether the device is a USB keyboard device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB keyboard device.
+  @retval FALSE    Device is a not USB keyboard device.
+
+**/
+BOOLEAN
+IsUSBKeyboard (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo
+  );
+
+/**
+  Initialize USB keyboard device and all private data structures.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS        Initialization is successful.
+  @retval EFI_DEVICE_ERROR   Keyboard initialization failed.
+
+**/
+EFI_STATUS
+InitUSBKeyboard (
+  IN OUT USB_KB_DEV   *UsbKeyboardDevice
+  );
+
+/**
+  Initialize USB keyboard layout.
+
+  This function initializes Key Convertion Table for the USB keyboard device.
+  It first tries to retrieve layout from HII database. If failed and default
+  layout is enabled, then it just uses the default layout.
+
+  @param  UsbKeyboardDevice      The USB_KB_DEV instance.
+
+  @retval EFI_SUCCESS            Initialization succeeded.
+  @retval EFI_NOT_READY          Keyboard layout cannot be retrieve from HII
+                                 database, and default layout is disabled.
+  @retval Other                  Fail to register event to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group.
+
+**/
+EFI_STATUS
+InitKeyboardLayout (
+  OUT USB_KB_DEV   *UsbKeyboardDevice
+  );
+
+/**
+  Destroy resources for keyboard layout.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+
+**/
+VOID
+ReleaseKeyboardLayoutResources (
+  IN OUT USB_KB_DEV              *UsbKeyboardDevice
+  );
+
+/**
+  Handler function for USB keyboard's asynchronous interrupt transfer.
+
+  This function is the handler function for USB keyboard's asynchronous interrupt transfer
+  to manage the keyboard. It parses the USB keyboard input report, and inserts data to
+  keyboard buffer according to state of modifer keys and normal keys. Timer for repeat key
+  is also set accordingly.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+KeyboardHandler (
+  IN  VOID          *Data,
+  IN  UINTN         DataLength,
+  IN  VOID          *Context,
+  IN  UINT32        Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Retrieves a USB keycode after parsing the raw data in keyboard buffer.
+
+  This function parses keyboard buffer. It updates state of modifier key for
+  USB_KB_DEV instancem, and returns keycode for output.
+
+  @param  UsbKeyboardDevice    The USB_KB_DEV instance.
+  @param  KeyCode              Pointer to the USB keycode for output.
+
+  @retval EFI_SUCCESS          Keycode successfully parsed.
+  @retval EFI_NOT_READY        Keyboard buffer is not ready for a valid keycode
+
+**/
+EFI_STATUS
+USBParseKey (
+  IN OUT  USB_KB_DEV  *UsbKeyboardDevice,
+  OUT     UINT8       *KeyCode
+  );
+
+/**
+  Converts USB Keycode ranging from 0x4 to 0x65 to EFI_INPUT_KEY.
+
+  @param  UsbKeyboardDevice     The USB_KB_DEV instance.
+  @param  KeyCode               Indicates the key code that will be interpreted.
+  @param  KeyData               A pointer to a buffer that is filled in with
+                                the keystroke information for the key that
+                                was pressed.
+
+  @retval EFI_SUCCESS           Success.
+  @retval EFI_INVALID_PARAMETER KeyCode is not in the range of 0x4 to 0x65.
+  @retval EFI_INVALID_PARAMETER Translated EFI_INPUT_KEY has zero for both ScanCode and UnicodeChar.
+  @retval EFI_NOT_READY         KeyCode represents a dead key with EFI_NS_KEY_MODIFIER
+  @retval EFI_DEVICE_ERROR      Keyboard layout is invalid.
+
+**/
+EFI_STATUS
+UsbKeyCodeToEfiInputKey (
+  IN  USB_KB_DEV      *UsbKeyboardDevice,
+  IN  UINT8           KeyCode,
+  OUT EFI_KEY_DATA    *KeyData
+  );
+
+
+/**
+  Create the queue.
+
+  @param  Queue     Points to the queue.
+  @param  ItemSize  Size of the single item.
+
+**/
+VOID
+InitQueue (
+  IN OUT  USB_SIMPLE_QUEUE   *Queue,
+  IN      UINTN              ItemSize
+  );
+
+/**
+  Destroy the queue
+
+  @param Queue    Points to the queue.
+**/
+VOID
+DestroyQueue (
+  IN OUT USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Check whether the queue is empty.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is empty.
+  @retval FALSE     Queue is not empty.
+
+**/
+BOOLEAN
+IsQueueEmpty (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Check whether the queue is full.
+
+  @param  Queue     Points to the queue.
+
+  @retval TRUE      Queue is full.
+  @retval FALSE     Queue is not full.
+
+**/
+BOOLEAN
+IsQueueFull (
+  IN  USB_SIMPLE_QUEUE   *Queue
+  );
+
+
+/**
+  Enqueue the item to the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Points to the item to be enqueued.
+  @param  ItemSize  Size of the item.
+**/
+VOID
+Enqueue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+  IN      VOID             *Item,
+  IN      UINTN            ItemSize
+  );
+
+
+/**
+  Dequeue a item from the queue.
+
+  @param  Queue     Points to the queue.
+  @param  Item      Receives the item.
+  @param  ItemSize  Size of the item.
+
+  @retval EFI_SUCCESS        Item was successfully dequeued.
+  @retval EFI_DEVICE_ERROR   The queue is empty.
+
+**/
+EFI_STATUS
+Dequeue (
+  IN OUT  USB_SIMPLE_QUEUE *Queue,
+     OUT  VOID             *Item,
+  IN      UINTN            ItemSize
+  );
+
+/**
+  Handler for Repeat Key event.
+
+  This function is the handler for Repeat Key event triggered
+  by timer.
+  After a repeatable key is pressed, the event would be triggered
+  with interval of USBKBD_REPEAT_DELAY. Once the event is triggered,
+  following trigger will come with interval of USBKBD_REPEAT_RATE.
+
+  @param  Event              The Repeat Key event.
+  @param  Context            Points to the USB_KB_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBKeyboardRepeatHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Sets USB keyboard LED state.
+
+  @param  UsbKeyboardDevice  The USB_KB_DEV instance.
+
+**/
+VOID
+SetKeyLED (
+  IN  USB_KB_DEV    *UsbKeyboardDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
new file mode 100644
index 0000000000..7939a1857d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
@@ -0,0 +1,99 @@
+## @file
+# USB Keyboard Driver that manages USB keyboard and produces Simple Text Input(Ex) Protocol.
+#
+# USB Keyboard Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Simple Text Input Protocol and Simple Text Input Ex Protocol on USB keyboard devices.
+# It initializes the keyboard layout according to info retrieved from HII database.
+# If HII cannot provide the info, this module uses its carried default one if PCD allows.
+# It manages the USB keyboard device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID documents.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. Universal Serial Bus HID Usage Tables, ver 1.12
+# 3. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbKbDxe
+  MODULE_UNI_FILE                = UsbKbDxe.uni
+  FILE_GUID                      = 2D2E62CF-9ECF-43b7-8219-94E7FC713DFE
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBKeyboardDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+#  DRIVER_BINDING                =  gUsbKeyboardDriverBinding                        
+#  COMPONENT_NAME                =  gUsbKeyboardComponentName
+#  COMPONENT_NAME2               =  gUsbKeyboardComponentName2
+#
+
+[Sources]
+  EfiKey.c
+  EfiKey.h
+  KeyBoard.c
+  ComponentName.c
+  KeyBoard.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  ReportStatusCodeLib
+  DebugLib
+  PcdLib
+  UefiUsbLib
+  HiiLib
+
+[Guids]
+  #
+  # Event registered to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group,
+  # which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  #
+  gEfiHiiKeyBoardLayoutGuid                     ## SOMETIMES_CONSUMES ## Event
+  gUsbKeyboardLayoutPackageGuid                 ## SOMETIMES_CONSUMES ## HII
+  gUsbKeyboardLayoutKeyGuid                     ## SOMETIMES_PRODUCES ## UNDEFINED
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimpleTextInProtocolGuid                  ## BY_START
+  gEfiSimpleTextInputExProtocolGuid             ## BY_START
+  #
+  # If HII Database Protocol exists, then keyboard layout from HII database is used.
+  # Otherwise, USB keyboard module tries to use its carried default layout.
+  #
+  gEfiHiiDatabaseProtocolGuid                   ## SOMETIMES_CONSUMES
+  
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDisableDefaultKeyboardLayoutInUsbKbDriver ## CONSUMES
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbKbDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni
new file mode 100644
index 0000000000..77586520c4
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni
new file mode 100644
index 0000000000..b337726abf
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c
new file mode 100644
index 0000000000..ad879afe59
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/ComponentName.c
@@ -0,0 +1,162 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mass Storage Driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMass.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMassStorageComponentName = {
+  UsbMassStorageGetDriverName,
+  UsbMassStorageGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMassStorageComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMassStorageGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMassStorageGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE
+mUsbMassStorageDriverNameTable[] = {
+  {"eng;en", L"Usb Mass Storage Driver"},
+  {NULL,  NULL}
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMassStorageDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMassStorageComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h
new file mode 100644
index 0000000000..ebfb254de6
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMass.h
@@ -0,0 +1,193 @@
+/** @file
+  Definition of USB Mass Storage Class and its value, USB Mass Transport Protocol, 
+  and other common definitions.
+
+Copyright (c) 2007 - 2014, 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.
+
+**/
+
+#ifndef _EFI_USBMASS_H_
+#define _EFI_USBMASS_H_
+
+
+#include <Uefi.h>
+#include <IndustryStandard/Scsi.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/DiskInfo.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>
+
+typedef struct _USB_MASS_TRANSPORT USB_MASS_TRANSPORT;
+typedef struct _USB_MASS_DEVICE    USB_MASS_DEVICE;
+
+#include "UsbMassBot.h"
+#include "UsbMassCbi.h"
+#include "UsbMassBoot.h"
+#include "UsbMassDiskInfo.h"
+#include "UsbMassImpl.h"
+
+#define USB_IS_IN_ENDPOINT(EndPointAddr)      (((EndPointAddr) & BIT7) == BIT7)
+#define USB_IS_OUT_ENDPOINT(EndPointAddr)     (((EndPointAddr) & BIT7) == 0)
+#define USB_IS_BULK_ENDPOINT(Attribute)       (((Attribute) & (BIT0 | BIT1)) == USB_ENDPOINT_BULK)
+#define USB_IS_INTERRUPT_ENDPOINT(Attribute)  (((Attribute) & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT)
+#define USB_IS_ERROR(Result, Error)           (((Result) & (Error)) != 0)
+
+#define USB_MASS_1_MILLISECOND  1000
+#define USB_MASS_1_SECOND       (1000 * USB_MASS_1_MILLISECOND)
+
+#define USB_MASS_CMD_SUCCESS    0
+#define USB_MASS_CMD_FAIL       1
+#define USB_MASS_CMD_PERSISTENT 2
+
+/**
+  Initializes USB transport protocol.
+
+  This function initializes the USB mass storage class transport protocol.
+  It will save its context in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB transport initialization fails.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_INIT_TRANSPORT) (
+  IN  EFI_USB_IO_PROTOCOL     *Usb,
+  OUT VOID                    **Context    OPTIONAL
+  );
+
+/**
+  Execute USB mass storage command through the transport protocol.
+
+  @param  Context               The USB Transport Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_EXEC_COMMAND) (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by Transport protocol.
+
+  @param  Context               The USB Transport Protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_RESET) (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the transport protocol.
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_GET_MAX_LUN) (
+  IN  VOID                    *Context,
+  IN  UINT8                   *MaxLun
+  );
+
+/**
+  Clean up the transport protocol's resource.
+
+  @param  Context               The instance of transport protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+typedef
+EFI_STATUS
+(*USB_MASS_CLEAN_UP) (
+  IN  VOID                    *Context
+  );
+
+///
+/// This structure contains information necessary to select the
+/// proper transport protocol. The mass storage class defines
+/// two transport protocols. One is the CBI, and the other is BOT.
+/// CBI is being obseleted. The design is made modular by this
+/// structure so that the CBI protocol can be easily removed when
+/// it is no longer necessary.
+///
+struct _USB_MASS_TRANSPORT {
+  UINT8                   Protocol;
+  USB_MASS_INIT_TRANSPORT Init;        ///< Initialize the mass storage transport protocol
+  USB_MASS_EXEC_COMMAND   ExecCommand; ///< Transport command to the device then get result
+  USB_MASS_RESET          Reset;       ///< Reset the device
+  USB_MASS_GET_MAX_LUN    GetMaxLun;   ///< Get max lun, only for bot
+  USB_MASS_CLEAN_UP       CleanUp;     ///< Clean up the resources.
+};
+
+struct _USB_MASS_DEVICE {
+  UINT32                    Signature;
+  EFI_HANDLE                Controller;
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_BLOCK_IO_PROTOCOL     BlockIo;
+  EFI_BLOCK_IO_MEDIA        BlockIoMedia;
+  BOOLEAN                   OpticalStorage;
+  UINT8                     Lun;          ///< Logical Unit Number
+  UINT8                     Pdt;          ///< Peripheral Device Type
+  USB_MASS_TRANSPORT        *Transport;   ///< USB mass storage transport protocol
+  VOID                      *Context;
+  EFI_DISK_INFO_PROTOCOL    DiskInfo;
+  USB_BOOT_INQUIRY_DATA     InquiryData;
+  BOOLEAN                   Cdb16Byte;
+};
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c
new file mode 100644
index 0000000000..9f99650703
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.c
@@ -0,0 +1,1106 @@
+/** @file
+  Implementation of the command set of USB Mass Storage Specification
+  for Bootability, Revision 1.0.
+
+Copyright (c) 2007 - 2014, 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 "UsbMass.h"
+
+/**
+  Execute REQUEST SENSE Command to retrieve sense data from device.
+
+  @param  UsbMass                The device whose sense data is requested.
+
+  @retval EFI_SUCCESS            The command is excuted successfully.
+  @retval EFI_DEVICE_ERROR       Failed to request sense.
+  @retval EFI_NO_RESPONSE        The device media doesn't response this request.
+  @retval EFI_INVALID_PARAMETER  The command has some invalid parameters.
+  @retval EFI_WRITE_PROTECTED    The device is write protected.
+  @retval EFI_MEDIA_CHANGED      The device media has been changed.
+
+**/
+EFI_STATUS
+UsbBootRequestSense (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  USB_BOOT_REQUEST_SENSE_CMD  SenseCmd;
+  USB_BOOT_REQUEST_SENSE_DATA SenseData;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  USB_MASS_TRANSPORT          *Transport;
+  EFI_STATUS                  Status;
+  UINT32                      CmdResult;
+
+  Transport = UsbMass->Transport;
+
+  //
+  // Request the sense data from the device
+  //
+  ZeroMem (&SenseCmd, sizeof (USB_BOOT_REQUEST_SENSE_CMD));
+  ZeroMem (&SenseData, sizeof (USB_BOOT_REQUEST_SENSE_DATA));
+
+  SenseCmd.OpCode   = USB_BOOT_REQUEST_SENSE_OPCODE;
+  SenseCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+  SenseCmd.AllocLen = (UINT8) sizeof (USB_BOOT_REQUEST_SENSE_DATA);
+
+  Status = Transport->ExecCommand (
+                        UsbMass->Context,
+                        &SenseCmd,
+                        sizeof (USB_BOOT_REQUEST_SENSE_CMD),
+                        EfiUsbDataIn,
+                        &SenseData,
+                        sizeof (USB_BOOT_REQUEST_SENSE_DATA),
+                        UsbMass->Lun,
+                        USB_BOOT_GENERAL_CMD_TIMEOUT,
+                        &CmdResult
+                        );
+  if (EFI_ERROR (Status) || CmdResult != USB_MASS_CMD_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "UsbBootRequestSense: (%r) CmdResult=0x%x\n", Status, CmdResult));
+    if (!EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+    }
+    return Status;
+  }
+
+  //
+  // If sense data is retrieved successfully, interpret the sense data
+  // and update the media status if necessary.
+  //
+  Media = &UsbMass->BlockIoMedia;
+
+  switch (USB_BOOT_SENSE_KEY (SenseData.SenseKey)) {
+
+  case USB_BOOT_SENSE_NO_SENSE:
+    Status = EFI_NO_RESPONSE;
+    break;
+
+  case USB_BOOT_SENSE_RECOVERED:
+    //
+    // Suppose hardware can handle this case, and recover later by itself
+    //
+    Status = EFI_NOT_READY;
+    break;
+
+  case USB_BOOT_SENSE_NOT_READY:
+    Status = EFI_DEVICE_ERROR;
+    if (SenseData.Asc == USB_BOOT_ASC_NO_MEDIA) {
+      Media->MediaPresent = FALSE;
+      Status = EFI_NO_MEDIA;
+    } else if (SenseData.Asc == USB_BOOT_ASC_NOT_READY) {
+      Status = EFI_NOT_READY;
+    }
+    break;
+
+  case USB_BOOT_SENSE_ILLEGAL_REQUEST:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+
+  case USB_BOOT_SENSE_UNIT_ATTENTION:
+    Status = EFI_DEVICE_ERROR;
+    if (SenseData.Asc == USB_BOOT_ASC_MEDIA_CHANGE) {
+      //
+      // If MediaChange, reset ReadOnly and new MediaId
+      //
+      Status = EFI_MEDIA_CHANGED;
+      Media->ReadOnly = FALSE;
+      Media->MediaId++;
+    }
+    break;
+
+  case USB_BOOT_SENSE_DATA_PROTECT:
+    Status = EFI_WRITE_PROTECTED;
+    Media->ReadOnly = TRUE;
+    break;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    break;
+  }
+
+  DEBUG ((EFI_D_INFO, "UsbBootRequestSense: (%r) with sense key %x/%x/%x\n",
+          Status,
+          USB_BOOT_SENSE_KEY (SenseData.SenseKey),
+          SenseData.Asc,
+          SenseData.Ascq
+          ));
+
+  return Status;
+}
+
+
+/**
+  Execute the USB mass storage bootability commands.
+
+  This function executes the USB mass storage bootability commands.
+  If execution failed, retrieve the error by REQUEST_SENSE, then
+  update the device's status, such as ReadyOnly.
+
+  @param  UsbMass                The device to issue commands to
+  @param  Cmd                    The command to execute
+  @param  CmdLen                 The length of the command
+  @param  DataDir                The direction of data transfer
+  @param  Data                   The buffer to hold the data
+  @param  DataLen                The length of expected data
+  @param  Timeout                The timeout used to transfer
+
+  @retval EFI_SUCCESS            Command is excuted successfully
+  @retval Others                 Command execution failed.
+
+**/
+EFI_STATUS
+UsbBootExecCmd (
+  IN USB_MASS_DEVICE            *UsbMass,
+  IN VOID                       *Cmd,
+  IN UINT8                      CmdLen,
+  IN EFI_USB_DATA_DIRECTION     DataDir,
+  IN VOID                       *Data,
+  IN UINT32                     DataLen,
+  IN UINT32                     Timeout
+  )
+{
+  USB_MASS_TRANSPORT          *Transport;
+  EFI_STATUS                  Status;
+  UINT32                      CmdResult;
+
+  Transport = UsbMass->Transport;
+  Status    = Transport->ExecCommand (
+                           UsbMass->Context,
+                           Cmd,
+                           CmdLen,
+                           DataDir,
+                           Data,
+                           DataLen,
+                           UsbMass->Lun,
+                           Timeout,
+                           &CmdResult
+                           );
+
+  if (Status == EFI_TIMEOUT) {
+    DEBUG ((EFI_D_ERROR, "UsbBootExecCmd: Timeout to Exec 0x%x Cmd\n", *(UINT8 *)Cmd));
+    return EFI_TIMEOUT;
+  }
+
+  //
+  // If ExecCommand() returns no error and CmdResult is success,
+  // then the commnad transfer is successful.
+  //
+  if ((CmdResult == USB_MASS_CMD_SUCCESS) && !EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If command execution failed, then retrieve error info via sense request.
+  //
+  return UsbBootRequestSense (UsbMass);
+}
+
+
+/**
+  Execute the USB mass storage bootability commands with retrial.
+
+  This function executes USB mass storage bootability commands.
+  If the device isn't ready, wait for it. If the device is ready
+  and error occurs, retry the command again until it exceeds the
+  limit of retrial times.
+  
+  @param  UsbMass                The device to issue commands to
+  @param  Cmd                    The command to execute
+  @param  CmdLen                 The length of the command
+  @param  DataDir                The direction of data transfer
+  @param  Data                   The buffer to hold the data
+  @param  DataLen                The length of expected data
+  @param  Timeout                The timeout used to transfer
+
+  @retval EFI_SUCCESS            The command is executed successfully.
+  @retval EFI_MEDIA_CHANGED      The device media has been changed.
+  @retval Others                 Command execution failed after retrial.
+
+**/
+EFI_STATUS
+UsbBootExecCmdWithRetry (
+  IN USB_MASS_DEVICE          *UsbMass,
+  IN VOID                     *Cmd,
+  IN UINT8                    CmdLen,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN VOID                     *Data,
+  IN UINT32                   DataLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       Retry;
+  EFI_EVENT                   TimeoutEvt;
+
+  Retry  = 0;
+  Status = EFI_SUCCESS;
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimeoutEvt
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (TimeoutEvt, TimerRelative, EFI_TIMER_PERIOD_SECONDS(60));
+  if (EFI_ERROR (Status)) {
+    goto EXIT;
+  }
+
+  //
+  // Execute the cmd and retry if it fails.
+  //
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvt))) {
+    Status = UsbBootExecCmd (
+               UsbMass,
+               Cmd,
+               CmdLen,
+               DataDir,
+               Data,
+               DataLen,
+               Timeout
+               );
+    if (Status == EFI_SUCCESS || Status == EFI_MEDIA_CHANGED || Status == EFI_NO_MEDIA) {
+      break;
+    }
+    //
+    // If the sense data shows the drive is not ready, we need execute the cmd again.
+    // We limit the upper boundary to 60 seconds.
+    //
+    if (Status == EFI_NOT_READY) {
+      continue;
+    }
+    //
+    // If the status is other error, then just retry 5 times.
+    //
+    if (Retry++ >= USB_BOOT_COMMAND_RETRY) {
+      break;
+    }
+  }
+
+EXIT:
+  if (TimeoutEvt != NULL) {
+    gBS->CloseEvent (TimeoutEvt);
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute TEST UNIT READY command to check if the device is ready.
+
+  @param  UsbMass                The device to test
+
+  @retval EFI_SUCCESS            The device is ready.
+  @retval Others                 Device not ready.
+
+**/
+EFI_STATUS
+UsbBootIsUnitReady (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  USB_BOOT_TEST_UNIT_READY_CMD  TestCmd;
+
+  ZeroMem (&TestCmd, sizeof (USB_BOOT_TEST_UNIT_READY_CMD));
+
+  TestCmd.OpCode  = USB_BOOT_TEST_UNIT_READY_OPCODE;
+  TestCmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+
+  return UsbBootExecCmdWithRetry (
+           UsbMass,
+           &TestCmd,
+           (UINT8) sizeof (USB_BOOT_TEST_UNIT_READY_CMD),
+           EfiUsbNoData,
+           NULL,
+           0,
+           USB_BOOT_GENERAL_CMD_TIMEOUT
+           );
+}
+
+
+/**
+  Execute INQUIRY Command to request information regarding parameters of
+  the device be sent to the host computer.
+
+  @param  UsbMass                The device to inquire.
+
+  @retval EFI_SUCCESS            INQUIRY Command is executed successfully.
+  @retval Others                 INQUIRY Command is not executed successfully.
+
+**/
+EFI_STATUS
+UsbBootInquiry (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  USB_BOOT_INQUIRY_CMD        InquiryCmd;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media = &(UsbMass->BlockIoMedia);
+
+  ZeroMem (&InquiryCmd, sizeof (USB_BOOT_INQUIRY_CMD));
+  ZeroMem (&UsbMass->InquiryData, sizeof (USB_BOOT_INQUIRY_DATA));
+
+  InquiryCmd.OpCode   = USB_BOOT_INQUIRY_OPCODE;
+  InquiryCmd.Lun      = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+  InquiryCmd.AllocLen = (UINT8) sizeof (USB_BOOT_INQUIRY_DATA);
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &InquiryCmd,
+             (UINT8) sizeof (USB_BOOT_INQUIRY_CMD),
+             EfiUsbDataIn,
+             &UsbMass->InquiryData,
+             sizeof (USB_BOOT_INQUIRY_DATA),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get information from PDT (Peripheral Device Type) field and Removable Medium Bit
+  // from the inquiry data.
+  //
+  UsbMass->Pdt          = (UINT8) (USB_BOOT_PDT (UsbMass->InquiryData.Pdt));
+  Media->RemovableMedia = (BOOLEAN) (USB_BOOT_REMOVABLE (UsbMass->InquiryData.Removable));
+  //
+  // Set block size to the default value of 512 Bytes, in case no media is present at first time.
+  //
+  Media->BlockSize      = 0x0200;
+
+  return Status;
+}
+
+/**
+  Execute READ CAPACITY 16 bytes command to request information regarding
+  the capacity of the installed medium of the device.
+
+  This function executes READ CAPACITY 16 bytes command to get the capacity
+  of the USB mass storage media, including the presence, block size,
+  and last block number.
+
+  @param  UsbMass                The device to retireve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.
+  @retval EFI_NOT_READY          The returned block size is zero.
+  @retval Other                  READ CAPACITY 16 bytes command execution failed.
+ 
+**/
+EFI_STATUS
+UsbBootReadCapacity16 (
+  IN USB_MASS_DEVICE            *UsbMass
+  )
+{
+  UINT8                         CapacityCmd[16];
+  EFI_SCSI_DISK_CAPACITY_DATA16 CapacityData;
+  EFI_BLOCK_IO_MEDIA            *Media;
+  EFI_STATUS                    Status;
+  UINT32                        BlockSize;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  Media->MediaPresent = FALSE;
+  Media->LastBlock    = 0;
+  Media->BlockSize    = 0;
+
+  ZeroMem (CapacityCmd, sizeof (CapacityCmd));
+  ZeroMem (&CapacityData, sizeof (CapacityData));
+
+  CapacityCmd[0]  = EFI_SCSI_OP_READ_CAPACITY16;
+  CapacityCmd[1]  = 0x10;
+  //
+  // Partial medium indicator, set the bytes 2 ~ 9 of the Cdb as ZERO.
+  //
+  ZeroMem ((CapacityCmd + 2), 8);
+
+  CapacityCmd[13] = sizeof (CapacityData);
+  
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             CapacityCmd,
+             (UINT8) sizeof (CapacityCmd),
+             EfiUsbDataIn,
+             &CapacityData,
+             sizeof (CapacityData),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the information on media presence, block size, and last block number
+  // from READ CAPACITY data.
+  //
+  Media->MediaPresent = TRUE;
+  Media->LastBlock    = SwapBytes64 (ReadUnaligned64 ((CONST UINT64 *) &(CapacityData.LastLba7)));
+
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) &(CapacityData.BlockSize3)));
+  
+  Media->LowestAlignedLba = (CapacityData.LowestAlignLogic2 << 8) |
+                             CapacityData.LowestAlignLogic1;
+  Media->LogicalBlocksPerPhysicalBlock  = (1 << CapacityData.LogicPerPhysical);
+  if (BlockSize == 0) {
+    //
+    //  Get sense data  
+    //
+    return UsbBootRequestSense (UsbMass);
+  } else {
+    Media->BlockSize = BlockSize;
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute READ CAPACITY command to request information regarding
+  the capacity of the installed medium of the device.
+
+  This function executes READ CAPACITY command to get the capacity
+  of the USB mass storage media, including the presence, block size,
+  and last block number.
+
+  @param  UsbMass                The device to retireve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk geometry is successfully retrieved.
+  @retval EFI_NOT_READY          The returned block size is zero.
+  @retval Other                  READ CAPACITY command execution failed.
+ 
+**/
+EFI_STATUS
+UsbBootReadCapacity (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  USB_BOOT_READ_CAPACITY_CMD  CapacityCmd;
+  USB_BOOT_READ_CAPACITY_DATA CapacityData;
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+  UINT32                      BlockSize;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  ZeroMem (&CapacityCmd, sizeof (USB_BOOT_READ_CAPACITY_CMD));
+  ZeroMem (&CapacityData, sizeof (USB_BOOT_READ_CAPACITY_DATA));
+
+  CapacityCmd.OpCode = USB_BOOT_READ_CAPACITY_OPCODE;
+  CapacityCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &CapacityCmd,
+             (UINT8) sizeof (USB_BOOT_READ_CAPACITY_CMD),
+             EfiUsbDataIn,
+             &CapacityData,
+             sizeof (USB_BOOT_READ_CAPACITY_DATA),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the information on media presence, block size, and last block number
+  // from READ CAPACITY data.
+  //
+  Media->MediaPresent = TRUE;
+  Media->LastBlock    = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.LastLba));
+
+  BlockSize           = SwapBytes32 (ReadUnaligned32 ((CONST UINT32 *) CapacityData.BlockLen));
+  if (BlockSize == 0) {
+    //
+    //  Get sense data  
+    //
+    return UsbBootRequestSense (UsbMass);
+  } else {
+    Media->BlockSize = BlockSize;
+  }
+
+  if (Media->LastBlock == 0xFFFFFFFF) {
+    Status = UsbBootReadCapacity16 (UsbMass);
+    if (!EFI_ERROR (Status)) {
+      UsbMass->Cdb16Byte = TRUE;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Retrieves SCSI mode sense information via MODE SENSE(6) command.
+
+  @param  UsbMass                The device whose sense data is requested.
+
+  @retval EFI_SUCCESS            SCSI mode sense information retrieved successfully.
+  @retval Other                  Command execution failed.
+
+**/
+EFI_STATUS
+UsbScsiModeSense (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_STATUS                       Status;
+  USB_SCSI_MODE_SENSE6_CMD         ModeSenseCmd;
+  USB_SCSI_MODE_SENSE6_PARA_HEADER ModeParaHeader;
+  EFI_BLOCK_IO_MEDIA               *Media;
+
+  Media   = &UsbMass->BlockIoMedia;
+
+  ZeroMem (&ModeSenseCmd, sizeof (USB_SCSI_MODE_SENSE6_CMD));
+  ZeroMem (&ModeParaHeader, sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER));
+
+  //
+  // MODE SENSE(6) command is defined in Section 8.2.10 of SCSI-2 Spec
+  //
+  ModeSenseCmd.OpCode         = USB_SCSI_MODE_SENSE6_OPCODE;
+  ModeSenseCmd.Lun            = (UINT8) USB_BOOT_LUN (UsbMass->Lun);
+  ModeSenseCmd.PageCode       = 0x3F;
+  ModeSenseCmd.AllocateLen    = (UINT8) sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER);
+
+  Status = UsbBootExecCmdWithRetry (
+             UsbMass,
+             &ModeSenseCmd,
+             (UINT8) sizeof (USB_SCSI_MODE_SENSE6_CMD),
+             EfiUsbDataIn,
+             &ModeParaHeader,
+             sizeof (USB_SCSI_MODE_SENSE6_PARA_HEADER),
+             USB_BOOT_GENERAL_CMD_TIMEOUT
+             );
+
+  //
+  // Format of device-specific parameter byte of the mode parameter header is defined in
+  // Section 8.2.10 of SCSI-2 Spec.
+  // BIT7 of this byte is indicates whether the medium is write protected.
+  //
+  if (!EFI_ERROR (Status)) {
+    Media->ReadOnly = (BOOLEAN) ((ModeParaHeader.DevicePara & BIT7) != 0);
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the parameters for the USB mass storage media.
+
+  This function get the parameters for the USB mass storage media,
+  It is used both to initialize the media during the Start() phase
+  of Driver Binding Protocol and to re-initialize it when the media is
+  changed. Althought the RemoveableMedia is unlikely to change,
+  it is also included here.
+
+  @param  UsbMass                The device to retrieve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.
+  @retval Other                  Failed to get the parameters.
+
+**/
+EFI_STATUS
+UsbBootGetParams (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media  = &(UsbMass->BlockIoMedia);
+
+  Status = UsbBootInquiry (UsbMass);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: UsbBootInquiry (%r)\n", Status));
+    return Status;
+  }
+
+  //
+  // According to USB Mass Storage Specification for Bootability, only following
+  // 4 Peripheral Device Types are in spec.
+  //
+  if ((UsbMass->Pdt != USB_PDT_DIRECT_ACCESS) &&
+       (UsbMass->Pdt != USB_PDT_CDROM) &&
+       (UsbMass->Pdt != USB_PDT_OPTICAL) &&
+       (UsbMass->Pdt != USB_PDT_SIMPLE_DIRECT)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootGetParams: Found an unsupported peripheral type[%d]\n", UsbMass->Pdt));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Don't use the Removable bit in inquiry data to test whether the media
+  // is removable because many flash disks wrongly set this bit.
+  //
+  if ((UsbMass->Pdt == USB_PDT_CDROM) || (UsbMass->Pdt == USB_PDT_OPTICAL)) {
+    //
+    // CD-Rom device and Non-CD optical device
+    //
+    UsbMass->OpticalStorage = TRUE;
+    //
+    // Default value 2048 Bytes, in case no media present at first time
+    //
+    Media->BlockSize        = 0x0800;
+  }
+
+  Status = UsbBootDetectMedia (UsbMass);
+
+  return Status;
+}
+
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param  UsbMass                The device to check.
+
+  @retval EFI_SUCCESS            The media status is successfully checked.
+  @retval Other                  Failed to detect media.
+
+**/
+EFI_STATUS
+UsbBootDetectMedia (
+  IN  USB_MASS_DEVICE       *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA        OldMedia;
+  EFI_BLOCK_IO_MEDIA        *Media;
+  UINT8                     CmdSet;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+
+  Media    = &UsbMass->BlockIoMedia;
+
+  CopyMem (&OldMedia, &(UsbMass->BlockIoMedia), sizeof (EFI_BLOCK_IO_MEDIA));
+
+  CmdSet = ((EFI_USB_INTERFACE_DESCRIPTOR *) (UsbMass->Context))->InterfaceSubClass;
+
+  Status = UsbBootIsUnitReady (UsbMass);
+  if (EFI_ERROR (Status) && (Status != EFI_MEDIA_CHANGED)) {
+    goto ON_ERROR;
+  }
+
+  if ((UsbMass->Pdt != USB_PDT_CDROM) && (CmdSet == USB_MASS_STORE_SCSI)) {
+    //
+    // MODE SENSE is required for the device with PDT of 0x00/0x07/0x0E,
+    // according to Section 4 of USB Mass Storage Specification for Bootability.
+    // MODE SENSE(10) is useless here, while MODE SENSE(6) defined in SCSI
+    // could get the information of Write Protected.
+    // Since not all device support this command, skip if fail.
+    //
+    UsbScsiModeSense (UsbMass);
+  }
+
+  Status = UsbBootReadCapacity (UsbMass);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBootDetectMedia: UsbBootReadCapacity (%r)\n", Status));
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  //
+  // Detect whether it is necessary to reinstall the Block I/O Protocol.
+  //
+  // MediaId may change in RequestSense for MediaChanged
+  // MediaPresent may change in RequestSense for NoMedia
+  // MediaReadOnly may change in RequestSense for WriteProtected or MediaChanged
+  // MediaPresent/BlockSize/LastBlock may change in ReadCapacity
+  //
+  if ((Media->MediaId != OldMedia.MediaId) ||
+      (Media->MediaPresent != OldMedia.MediaPresent) ||
+      (Media->ReadOnly != OldMedia.ReadOnly) ||
+      (Media->BlockSize != OldMedia.BlockSize) ||
+      (Media->LastBlock != OldMedia.LastBlock)) {
+
+    //
+    // This function is called by Block I/O Protocol APIs, which run at TPL_NOTIFY.
+    // Here we temporarily restore TPL to TPL_CALLBACK to invoke ReinstallProtocolInterface().
+    //
+    OldTpl = EfiGetCurrentTpl ();
+    gBS->RestoreTPL (TPL_CALLBACK);
+
+    gBS->ReinstallProtocolInterface (
+           UsbMass->Controller,
+           &gEfiBlockIoProtocolGuid,
+           &UsbMass->BlockIo,
+           &UsbMass->BlockIo
+           );
+
+    ASSERT (EfiGetCurrentTpl () == TPL_CALLBACK);
+    gBS->RaiseTPL (OldTpl);
+
+    //
+    // Update MediaId after reinstalling Block I/O Protocol.
+    //
+    if (Media->MediaPresent != OldMedia.MediaPresent) {
+      if (Media->MediaPresent) {
+        Media->MediaId = 1;
+      } else {
+        Media->MediaId = 0;
+      }
+    }
+
+    if ((Media->ReadOnly != OldMedia.ReadOnly) ||
+        (Media->BlockSize != OldMedia.BlockSize) ||
+        (Media->LastBlock != OldMedia.LastBlock)) {
+      Media->MediaId++;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Read some blocks from the device.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT32                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  )
+{
+  USB_BOOT_READ10_CMD       ReadCmd;
+  EFI_STATUS                Status;
+  UINT16                    Count;
+  UINT32                    BlockSize;
+  UINT32                    ByteSize;
+  UINT32                    Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces to ease the pressure
+    // on the device. We must split the total block because the READ10
+    // command only has 16 bit transfer length (in the unit of block).
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the command then execute
+    //
+    ZeroMem (&ReadCmd, sizeof (USB_BOOT_READ10_CMD));
+
+    ReadCmd.OpCode  = USB_BOOT_READ10_OPCODE;
+    ReadCmd.Lun     = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+    WriteUnaligned32 ((UINT32 *) ReadCmd.Lba, SwapBytes32 (Lba));
+    WriteUnaligned16 ((UINT16 *) ReadCmd.TransferLen, SwapBytes16 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               &ReadCmd,
+               (UINT8) sizeof (USB_BOOT_READ10_CMD),
+               EfiUsbDataIn,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+
+/**
+  Write some blocks to the device.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  )
+{
+  USB_BOOT_WRITE10_CMD  WriteCmd;
+  EFI_STATUS            Status;
+  UINT16                Count;
+  UINT32                BlockSize;
+  UINT32                ByteSize;
+  UINT32                Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces to ease the pressure
+    // on the device. We must split the total block because the WRITE10
+    // command only has 16 bit transfer length (in the unit of block).
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the write10 command block
+    //
+    ZeroMem (&WriteCmd, sizeof (USB_BOOT_WRITE10_CMD));
+
+    WriteCmd.OpCode = USB_BOOT_WRITE10_OPCODE;
+    WriteCmd.Lun    = (UINT8) (USB_BOOT_LUN (UsbMass->Lun));
+    WriteUnaligned32 ((UINT32 *) WriteCmd.Lba, SwapBytes32 (Lba));
+    WriteUnaligned16 ((UINT16 *) WriteCmd.TransferLen, SwapBytes16 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               &WriteCmd,
+               (UINT8) sizeof (USB_BOOT_WRITE10_CMD),
+               EfiUsbDataOut,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%x), Blk (0x%x)\n", Lba, Count));
+
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+/**
+  Read some blocks from the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks16 (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT64                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  )
+{
+  UINT8                     ReadCmd[16];
+  EFI_STATUS                Status;
+  UINT16                    Count;
+  UINT32                    BlockSize;
+  UINT32                    ByteSize;
+  UINT32                    Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces.
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the command then execute
+    //
+    ZeroMem (ReadCmd, sizeof (ReadCmd));
+
+    ReadCmd[0]  = EFI_SCSI_OP_READ16;
+    ReadCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));
+    WriteUnaligned64 ((UINT64 *) &ReadCmd[2], SwapBytes64 (Lba));
+    WriteUnaligned32 ((UINT32 *) &ReadCmd[10], SwapBytes32 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               ReadCmd,
+               (UINT8) sizeof (ReadCmd),
+               EfiUsbDataIn,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootReadBlocks16: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+
+/**
+  Write some blocks to the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks16 (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT64                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  )
+{
+  UINT8                 WriteCmd[16];
+  EFI_STATUS            Status;
+  UINT16                Count;
+  UINT32                BlockSize;
+  UINT32                ByteSize;
+  UINT32                Timeout;
+
+  BlockSize = UsbMass->BlockIoMedia.BlockSize;
+  Status    = EFI_SUCCESS;
+
+  while (TotalBlock > 0) {
+    //
+    // Split the total blocks into smaller pieces.
+    //
+    Count     = (UINT16)((TotalBlock < USB_BOOT_IO_BLOCKS) ? TotalBlock : USB_BOOT_IO_BLOCKS);
+    ByteSize  = (UINT32)Count * BlockSize;
+
+    //
+    // USB command's upper limit timeout is 5s. [USB2.0-9.2.6.1]
+    //
+    Timeout = (UINT32) USB_BOOT_GENERAL_CMD_TIMEOUT;
+
+    //
+    // Fill in the write16 command block
+    //
+    ZeroMem (WriteCmd, sizeof (WriteCmd));
+
+    WriteCmd[0]  = EFI_SCSI_OP_WRITE16;
+    WriteCmd[1]  = (UINT8) ((USB_BOOT_LUN (UsbMass->Lun) & 0xE0));
+    WriteUnaligned64 ((UINT64 *) &WriteCmd[2], SwapBytes64 (Lba));
+    WriteUnaligned32 ((UINT32 *) &WriteCmd[10], SwapBytes32 (Count));
+
+    Status = UsbBootExecCmdWithRetry (
+               UsbMass,
+               WriteCmd,
+               (UINT8) sizeof (WriteCmd),
+               EfiUsbDataOut,
+               Buffer,
+               ByteSize,
+               Timeout
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DEBUG ((EFI_D_BLKIO, "UsbBootWriteBlocks: LBA (0x%lx), Blk (0x%x)\n", Lba, Count));
+    Lba        += Count;
+    Buffer     += Count * BlockSize;
+    TotalBlock -= Count;
+  }
+
+  return Status;
+}
+
+/**
+  Use the USB clear feature control transfer to clear the endpoint stall condition.
+
+  @param  UsbIo                  The USB I/O Protocol instance
+  @param  EndpointAddr           The endpoint to clear stall for
+
+  @retval EFI_SUCCESS            The endpoint stall condition is cleared.
+  @retval Others                 Failed to clear the endpoint stall condition.
+
+**/
+EFI_STATUS
+UsbClearEndpointStall (
+  IN EFI_USB_IO_PROTOCOL    *UsbIo,
+  IN UINT8                  EndpointAddr
+  )
+{
+  EFI_USB_DEVICE_REQUEST    Request;
+  EFI_STATUS                Status;
+  UINT32                    CmdResult;
+  UINT32                    Timeout;
+
+  Request.RequestType = 0x02;
+  Request.Request     = USB_REQ_CLEAR_FEATURE;
+  Request.Value       = USB_FEATURE_ENDPOINT_HALT;
+  Request.Index       = EndpointAddr;
+  Request.Length      = 0;
+  Timeout             = USB_BOOT_GENERAL_CMD_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbIo->UsbControlTransfer (
+                    UsbIo,
+                    &Request,
+                    EfiUsbNoData,
+                    Timeout,
+                    NULL,
+                    0,
+                    &CmdResult
+                    );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h
new file mode 100644
index 0000000000..c4082558fa
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBoot.h
@@ -0,0 +1,371 @@
+/** @file
+  Definition of the command set of USB Mass Storage Specification
+  for Bootability, Revision 1.0.
+
+Copyright (c) 2007 - 2012, 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.
+
+**/
+
+#ifndef _EFI_USB_MASS_BOOT_H_
+#define _EFI_USB_MASS_BOOT_H_
+
+//
+// The opcodes of various USB boot commands:
+// INQUIRY/REQUEST_SENSE are "No Timeout Commands" as specified
+// by Multi-Media Commands (MMC) set.
+// Others are "Group 1 Timeout Commands". That is,
+// they should be retried if driver is ready. 
+//
+#define USB_BOOT_INQUIRY_OPCODE         0x12
+#define USB_BOOT_REQUEST_SENSE_OPCODE   0x03
+#define USB_BOOT_MODE_SENSE10_OPCODE    0x5A
+#define USB_BOOT_READ_CAPACITY_OPCODE   0x25
+#define USB_BOOT_TEST_UNIT_READY_OPCODE 0x00
+#define USB_BOOT_READ10_OPCODE          0x28
+#define USB_BOOT_WRITE10_OPCODE         0x2A
+
+#define USB_SCSI_MODE_SENSE6_OPCODE     0x1A
+
+//
+// The Sense Key part of the sense data. Sense data has three levels:
+// Sense key, Additional Sense Code and Additional Sense Code Qualifier
+//
+#define USB_BOOT_SENSE_NO_SENSE         0x00 ///< No sense key
+#define USB_BOOT_SENSE_RECOVERED        0x01 ///< Last command succeed with recovery actions
+#define USB_BOOT_SENSE_NOT_READY        0x02 ///< Device not ready
+#define USB_BOOT_SNESE_MEDIUM_ERROR     0X03 ///< Failed probably because flaw in the media
+#define USB_BOOT_SENSE_HARDWARE_ERROR   0X04 ///< Non-recoverable hardware failure
+#define USB_BOOT_SENSE_ILLEGAL_REQUEST  0X05 ///< Illegal parameters in the request
+#define USB_BOOT_SENSE_UNIT_ATTENTION   0X06 ///< Removable medium may have been changed
+#define USB_BOOT_SENSE_DATA_PROTECT     0X07 ///< Write protected
+#define USB_BOOT_SENSE_BLANK_CHECK      0X08 ///< Blank/non-blank medium while reading/writing
+#define USB_BOOT_SENSE_VENDOR           0X09 ///< Vendor specific sense key
+#define USB_BOOT_SENSE_ABORTED          0X0B ///< Command aborted by the device
+#define USB_BOOT_SENSE_VOLUME_OVERFLOW  0x0D ///< Partition overflow
+#define USB_BOOT_SENSE_MISCOMPARE       0x0E ///< Source data mis-match while verfying.
+
+#define USB_BOOT_ASC_NOT_READY          0x04
+#define USB_BOOT_ASC_NO_MEDIA           0x3A
+#define USB_BOOT_ASC_MEDIA_CHANGE       0x28
+
+//
+// Supported PDT codes, or Peripheral Device Type
+//
+#define USB_PDT_DIRECT_ACCESS           0x00       ///< Direct access device
+#define USB_PDT_CDROM                   0x05       ///< CDROM
+#define USB_PDT_OPTICAL                 0x07       ///< Non-CD optical disks
+#define USB_PDT_SIMPLE_DIRECT           0x0E       ///< Simplified direct access device
+
+//
+// Other parameters, Max carried size is 512B * 128 = 64KB
+//
+#define USB_BOOT_IO_BLOCKS              128
+
+//
+// Retry mass command times, set by experience
+//
+#define USB_BOOT_COMMAND_RETRY          5
+
+//
+// Wait for unit ready command, set by experience
+//
+#define USB_BOOT_RETRY_UNIT_READY_STALL (500 * USB_MASS_1_MILLISECOND)
+
+//
+// Mass command timeout, refers to specification[USB20-9.2.6.1]
+//
+// USB2.0 Spec define the up-limit timeout 5s for all command. USB floppy, 
+// USB CD-Rom and iPod devices are much slower than USB key when reponse 
+// most of commands, So we set 5s as timeout here.
+// 
+#define USB_BOOT_GENERAL_CMD_TIMEOUT    (5 * USB_MASS_1_SECOND)
+
+//
+// The required commands are INQUIRY, READ CAPACITY, TEST UNIT READY,
+// READ10, WRITE10, and REQUEST SENSE. The BLOCK_IO protocol uses LBA
+// so it isn't necessary to issue MODE SENSE / READ FORMAT CAPACITY
+// command to retrieve the disk gemotrics. 
+//
+#pragma pack(1)
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (high 3 bits)
+  UINT8             Reserved0[2];
+  UINT8             AllocLen;
+  UINT8             Reserved1;
+  UINT8             Pad[6];
+} USB_BOOT_INQUIRY_CMD;
+
+typedef struct {
+  UINT8             Pdt;            ///< Peripheral Device Type (low 5 bits)
+  UINT8             Removable;      ///< Removable Media (highest bit)
+  UINT8             Reserved0[2];
+  UINT8             AddLen;         ///< Additional length
+  UINT8             Reserved1[3];
+  UINT8             VendorID[8];
+  UINT8             ProductID[16];
+  UINT8             ProductRevision[4];
+} USB_BOOT_INQUIRY_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Reserved0[8];
+  UINT8             Pad[2];
+} USB_BOOT_READ_CAPACITY_CMD;
+
+typedef struct {
+  UINT8             LastLba[4];
+  UINT8             BlockLen[4];
+} USB_BOOT_READ_CAPACITY_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Reserved[4];
+  UINT8             Pad[6];
+} USB_BOOT_TEST_UNIT_READY_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             PageCode;
+  UINT8             Reserved0[4];
+  UINT8             ParaListLenMsb;
+  UINT8             ParaListLenLsb;
+  UINT8             Reserved1;
+  UINT8             Pad[2];
+} USB_BOOT_MODE_SENSE10_CMD;
+
+typedef struct {
+  UINT8             ModeDataLenMsb;
+  UINT8             ModeDataLenLsb;
+  UINT8             Reserved0[4];
+  UINT8             BlkDesLenMsb;
+  UINT8             BlkDesLenLsb;
+} USB_BOOT_MODE_SENSE10_PARA_HEADER;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (High 3 bits)
+  UINT8             Lba[4];         ///< Logical block address
+  UINT8             Reserved0;
+  UINT8             TransferLen[2]; ///< Transfer length
+  UINT8             Reserverd1;
+  UINT8             Pad[2];
+} USB_BOOT_READ10_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             Lba[4];
+  UINT8             Reserved0;
+  UINT8             TransferLen[2];
+  UINT8             Reserverd1;
+  UINT8             Pad[2];
+} USB_BOOT_WRITE10_CMD;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;            ///< Lun (High 3 bits)
+  UINT8             Reserved0[2];
+  UINT8             AllocLen;       ///< Allocation length
+  UINT8             Reserved1;
+  UINT8             Pad[6];
+} USB_BOOT_REQUEST_SENSE_CMD;
+
+typedef struct {
+  UINT8             ErrorCode;
+  UINT8             Reserved0;
+  UINT8             SenseKey;       ///< Sense key (low 4 bits)
+  UINT8             Infor[4];
+  UINT8             AddLen;         ///< Additional Sense length, 10
+  UINT8             Reserved1[4];
+  UINT8             Asc;            ///< Additional Sense Code
+  UINT8             Ascq;           ///< Additional Sense Code Qualifier
+  UINT8             Reserverd2[4];
+} USB_BOOT_REQUEST_SENSE_DATA;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Lun;
+  UINT8             PageCode;
+  UINT8             Reserved0;
+  UINT8             AllocateLen;
+  UINT8             Control;
+} USB_SCSI_MODE_SENSE6_CMD;
+
+typedef struct {
+  UINT8             ModeDataLen;
+  UINT8             MediumType;
+  UINT8             DevicePara;
+  UINT8             BlkDesLen;
+} USB_SCSI_MODE_SENSE6_PARA_HEADER;
+#pragma pack()
+
+//
+// Convert a LUN number to that in the command
+//
+#define USB_BOOT_LUN(Lun) ((Lun) << 5)
+
+//
+// Get the removable, PDT, and sense key bits from the command data
+//
+#define USB_BOOT_REMOVABLE(RmbByte) (((RmbByte) & BIT7) != 0)
+#define USB_BOOT_PDT(Pdt)           ((Pdt) & 0x1f)
+#define USB_BOOT_SENSE_KEY(Key)     ((Key) & 0x0f)
+
+/**
+  Get the parameters for the USB mass storage media.
+
+  This function get the parameters for the USB mass storage media,
+  It is used both to initialize the media during the Start() phase
+  of Driver Binding Protocol and to re-initialize it when the media is
+  changed. Althought the RemoveableMedia is unlikely to change,
+  it is also included here.
+
+  @param  UsbMass                The device to retrieve disk gemotric.
+
+  @retval EFI_SUCCESS            The disk gemotric is successfully retrieved.
+  @retval Other                  Failed to get the parameters.
+
+**/
+EFI_STATUS
+UsbBootGetParams (
+  IN USB_MASS_DEVICE          *UsbMass
+  );
+
+/**
+  Execute TEST UNIT READY command to check if the device is ready.
+
+  @param  UsbMass                The device to test
+
+  @retval EFI_SUCCESS            The device is ready.
+  @retval Others                 Device not ready.
+
+**/
+EFI_STATUS
+UsbBootIsUnitReady (
+  IN USB_MASS_DEVICE          *UsbMass
+  );
+
+/**
+  Detect whether the removable media is present and whether it has changed.
+
+  @param  UsbMass                The device to check.
+
+  @retval EFI_SUCCESS            The media status is successfully checked.
+  @retval Other                  Failed to detect media.
+
+**/
+EFI_STATUS
+UsbBootDetectMedia (
+  IN  USB_MASS_DEVICE       *UsbMass
+  );
+
+/**
+  Read some blocks from the device.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  OUT UINT8                   *Buffer
+  );
+
+/**
+  Write some blocks to the device.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT32                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  );
+
+/**
+  Read some blocks from the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to read from
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to read
+  @param  Buffer                 The buffer to read to
+
+  @retval EFI_SUCCESS            Data are read into the buffer
+  @retval Others                 Failed to read all the data
+
+**/
+EFI_STATUS
+UsbBootReadBlocks16 (
+  IN  USB_MASS_DEVICE       *UsbMass,
+  IN  UINT64                Lba,
+  IN  UINTN                 TotalBlock,
+  OUT UINT8                 *Buffer
+  );
+
+/**
+  Write some blocks to the device by SCSI 16 byte cmd.
+
+  @param  UsbMass                The USB mass storage device to write to
+  @param  Lba                    The start block number
+  @param  TotalBlock             Total block number to write
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            Data are written into the buffer
+  @retval Others                 Failed to write all the data
+
+**/
+EFI_STATUS
+UsbBootWriteBlocks16 (
+  IN  USB_MASS_DEVICE         *UsbMass,
+  IN  UINT64                  Lba,
+  IN  UINTN                   TotalBlock,
+  IN  UINT8                   *Buffer
+  );
+
+
+/**
+  Use the USB clear feature control transfer to clear the endpoint stall condition.
+
+  @param  UsbIo                  The USB I/O Protocol instance
+  @param  EndpointAddr           The endpoint to clear stall for
+
+  @retval EFI_SUCCESS            The endpoint stall condition is cleared.
+  @retval Others                 Failed to clear the endpoint stall condition.
+
+**/
+EFI_STATUS
+UsbClearEndpointStall (
+  IN EFI_USB_IO_PROTOCOL    *UsbIo,
+  IN UINT8                  EndpointAddr
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c
new file mode 100644
index 0000000000..dd83540285
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.c
@@ -0,0 +1,599 @@
+/** @file
+  Implementation of the USB mass storage Bulk-Only Transport protocol,
+  according to USB Mass Storage Class Bulk-Only Transport, Revision 1.0.
+
+Copyright (c) 2007 - 2011, 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 "UsbMass.h"
+
+//
+// Definition of USB BOT Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbBotTransport = {
+  USB_MASS_STORE_BOT,
+  UsbBotInit,
+  UsbBotExecCommand,
+  UsbBotResetDevice,
+  UsbBotGetMaxLun,
+  UsbBotCleanUp
+};
+
+/**
+  Initializes USB BOT protocol.
+
+  This function initializes the USB mass storage class BOT protocol.
+  It will save its context which is a USB_BOT_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB BOT initialization fails.
+
+**/
+EFI_STATUS
+UsbBotInit (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo,
+  OUT VOID                      **Context OPTIONAL
+  )
+{
+  USB_BOT_PROTOCOL              *UsbBot;
+  EFI_USB_INTERFACE_DESCRIPTOR  *Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndPoint;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+
+  //
+  // Allocate the BOT context for USB_BOT_PROTOCOL and two endpoint descriptors.
+  //
+  UsbBot = AllocateZeroPool (sizeof (USB_BOT_PROTOCOL) + 2 * sizeof (EFI_USB_ENDPOINT_DESCRIPTOR));
+  ASSERT (UsbBot != NULL);
+
+  UsbBot->UsbIo = UsbIo;
+
+  //
+  // Get the interface descriptor and validate that it
+  // is a USB Mass Storage BOT interface.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &UsbBot->Interface);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Interface = &UsbBot->Interface;
+
+  if (Interface->InterfaceProtocol != USB_MASS_STORE_BOT) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // Locate and save the first bulk-in and bulk-out endpoint
+  //
+  for (Index = 0; Index < Interface->NumEndpoints; Index++) {
+    Status = UsbIo->UsbGetEndpointDescriptor (UsbIo, Index, &EndPoint);
+
+    if (EFI_ERROR (Status) || !USB_IS_BULK_ENDPOINT (EndPoint.Attributes)) {
+      continue;
+    }
+
+    if (USB_IS_IN_ENDPOINT (EndPoint.EndpointAddress) &&
+       (UsbBot->BulkInEndpoint == NULL)) {
+
+      UsbBot->BulkInEndpoint  = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbBot + 1);
+      CopyMem(UsbBot->BulkInEndpoint, &EndPoint, sizeof (EndPoint));
+    }
+
+    if (USB_IS_OUT_ENDPOINT (EndPoint.EndpointAddress) &&
+       (UsbBot->BulkOutEndpoint == NULL)) {
+
+      UsbBot->BulkOutEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbBot + 1) + 1;
+      CopyMem (UsbBot->BulkOutEndpoint, &EndPoint, sizeof(EndPoint));
+    }
+  }
+
+  //
+  // If bulk-in or bulk-out endpoint is not found, report error.
+  //
+  if ((UsbBot->BulkInEndpoint == NULL) || (UsbBot->BulkOutEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // The USB BOT protocol uses CBWTag to match the CBW and CSW.
+  //
+  UsbBot->CbwTag = 0x01;
+
+  if (Context != NULL) {
+    *Context = UsbBot;
+  } else {
+    FreePool (UsbBot);
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (UsbBot);
+  return Status;
+}
+
+/**
+  Send the command to the device using Bulk-Out endpoint.
+
+  This function sends the command to the device using Bulk-Out endpoint.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Command phase.
+
+  @param  UsbBot                The USB BOT device
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of the data
+  @param  TransLen              The expected length of the data
+  @param  Lun                   The number of logic unit
+
+  @retval EFI_SUCCESS           The command is sent to the device.
+  @retval EFI_NOT_READY         The device return NAK to the transfer
+  @retval Others                Failed to send the command to device
+
+**/
+EFI_STATUS
+UsbBotSendCommand (
+  IN USB_BOT_PROTOCOL         *UsbBot,
+  IN UINT8                    *Cmd,
+  IN UINT8                    CmdLen,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN UINT32                   TransLen,
+  IN UINT8                    Lun
+  )
+{
+  USB_BOT_CBW               Cbw;
+  EFI_STATUS                Status;
+  UINT32                    Result;
+  UINTN                     DataLen;
+  UINTN                     Timeout;
+
+  ASSERT ((CmdLen > 0) && (CmdLen <= USB_BOT_MAX_CMDLEN));
+
+  //
+  // Fill in the Command Block Wrapper.
+  //
+  Cbw.Signature = USB_BOT_CBW_SIGNATURE;
+  Cbw.Tag       = UsbBot->CbwTag;
+  Cbw.DataLen   = TransLen;
+  Cbw.Flag      = (UINT8) ((DataDir == EfiUsbDataIn) ? BIT7 : 0);
+  Cbw.Lun       = Lun;
+  Cbw.CmdLen    = CmdLen;
+
+  ZeroMem (Cbw.CmdBlock, USB_BOT_MAX_CMDLEN);
+  CopyMem (Cbw.CmdBlock, Cmd, CmdLen);
+
+  Result  = 0;
+  DataLen = sizeof (USB_BOT_CBW);
+  Timeout = USB_BOT_SEND_CBW_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  //
+  // Use USB I/O Protocol to send the Command Block Wrapper to the device.
+  //
+  Status = UsbBot->UsbIo->UsbBulkTransfer (
+                            UsbBot->UsbIo,
+                            UsbBot->BulkOutEndpoint->EndpointAddress,
+                            &Cbw,
+                            &DataLen,
+                            Timeout,
+                            &Result
+                            );
+  if (EFI_ERROR (Status)) {
+    if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL) && DataDir == EfiUsbDataOut) {
+      //
+      // Respond to Bulk-Out endpoint stall with a Reset Recovery,
+      // according to section 5.3.1 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+      //
+      UsbBotResetDevice (UsbBot, FALSE);
+    } else if (USB_IS_ERROR (Result, EFI_USB_ERR_NAK)) {
+      Status = EFI_NOT_READY;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Transfer the data between the device and host.
+
+  This function transfers the data between the device and host.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Data phase.
+
+  @param  UsbBot                The USB BOT device
+  @param  DataDir               The direction of the data
+  @param  Data                  The buffer to hold data
+  @param  TransLen              The expected length of the data
+  @param  Timeout               The time to wait the command to complete
+
+  @retval EFI_SUCCESS           The data is transferred
+  @retval EFI_SUCCESS           No data to transfer
+  @retval EFI_NOT_READY         The device return NAK to the transfer
+  @retval Others                Failed to transfer data
+
+**/
+EFI_STATUS
+UsbBotDataTransfer (
+  IN USB_BOT_PROTOCOL         *UsbBot,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN OUT UINT8                *Data,
+  IN OUT UINTN                *TransLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_USB_ENDPOINT_DESCRIPTOR *Endpoint;
+  EFI_STATUS                  Status;
+  UINT32                      Result;
+
+  //
+  // If no data to transfer, just return EFI_SUCCESS.
+  //
+  if ((DataDir == EfiUsbNoData) || (*TransLen == 0)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Select the endpoint then issue the transfer
+  //
+  if (DataDir == EfiUsbDataIn) {
+    Endpoint = UsbBot->BulkInEndpoint;
+  } else {
+    Endpoint = UsbBot->BulkOutEndpoint;
+  }
+
+  Result  = 0;
+  Timeout = Timeout / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbBulkTransfer (
+                            UsbBot->UsbIo,
+                            Endpoint->EndpointAddress,
+                            Data,
+                            TransLen,
+                            Timeout,
+                            &Result
+                            );
+  if (EFI_ERROR (Status)) {
+    if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL)) {
+      DEBUG ((EFI_D_INFO, "UsbBotDataTransfer: (%r)\n", Status));      
+      DEBUG ((EFI_D_INFO, "UsbBotDataTransfer: DataIn Stall\n"));
+      UsbClearEndpointStall (UsbBot->UsbIo, Endpoint->EndpointAddress);
+    } else if (USB_IS_ERROR (Result, EFI_USB_ERR_NAK)) {
+      Status = EFI_NOT_READY;
+    } else {
+      DEBUG ((EFI_D_ERROR, "UsbBotDataTransfer: (%r)\n", Status));
+    }
+    if(Status == EFI_TIMEOUT){
+      UsbBotResetDevice(UsbBot, FALSE);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the command execution status from device.
+
+  This function gets the command execution status from device.
+  BOT transfer is composed of three phases: Command, Data, and Status.
+  This is the Status phase.
+
+  This function returns the transfer status of the BOT's CSW status,
+  and returns the high level command execution result in Result. So
+  even if EFI_SUCCESS is returned, the command may still have failed.
+
+  @param  UsbBot         The USB BOT device.
+  @param  TransLen       The expected length of the data.
+  @param  CmdStatus      The result of the command execution.
+
+  @retval EFI_SUCCESS    Command execute result is retrieved and in the Result.
+  @retval Other          Error occurred when trying to get status.
+
+**/
+EFI_STATUS
+UsbBotGetStatus (
+  IN  USB_BOT_PROTOCOL      *UsbBot,
+  IN  UINT32                TransLen,
+  OUT UINT8                 *CmdStatus
+  )
+{
+  USB_BOT_CSW               Csw;
+  UINTN                     Len;
+  UINT8                     Endpoint;
+  EFI_STATUS                Status;
+  UINT32                    Result;
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT32                    Index;
+  UINTN                     Timeout;
+
+  *CmdStatus = USB_BOT_COMMAND_ERROR;
+  Status     = EFI_DEVICE_ERROR;
+  Endpoint   = UsbBot->BulkInEndpoint->EndpointAddress;
+  UsbIo      = UsbBot->UsbIo;
+  Timeout    = USB_BOT_RECV_CSW_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  for (Index = 0; Index < USB_BOT_RECV_CSW_RETRY; Index++) {
+    //
+    // Attemp to the read Command Status Wrapper from bulk in endpoint
+    //
+    ZeroMem (&Csw, sizeof (USB_BOT_CSW));
+    Result = 0;
+    Len    = sizeof (USB_BOT_CSW);
+    Status = UsbIo->UsbBulkTransfer (
+                      UsbIo,
+                      Endpoint,
+                      &Csw,
+                      &Len,
+                      Timeout,
+                      &Result
+                      );
+    if (EFI_ERROR(Status)) {
+      if (USB_IS_ERROR (Result, EFI_USB_ERR_STALL)) {
+        UsbClearEndpointStall (UsbIo, Endpoint);
+      }
+      continue;
+    }
+
+    if (Csw.Signature != USB_BOT_CSW_SIGNATURE) {
+      //
+      // CSW is invalid, so perform reset recovery
+      //
+      Status = UsbBotResetDevice (UsbBot, FALSE);
+    } else if (Csw.CmdStatus == USB_BOT_COMMAND_ERROR) {
+      //
+      // Respond phase error also needs reset recovery
+      //
+      Status = UsbBotResetDevice (UsbBot, FALSE);
+    } else {
+      *CmdStatus = Csw.CmdStatus;
+      break;
+    }
+  }
+  //
+  //The tag is increased even if there is an error.
+  //
+  UsbBot->CbwTag++;
+
+  return Status;
+}
+
+
+/**
+  Call the USB Mass Storage Class BOT protocol to issue
+  the command/data/status circle to execute the commands.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL
+  @param  Cmd                   The high level command
+  @param  CmdLen                The command length
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold data
+  @param  DataLen               The length of the data
+  @param  Lun                   The number of logic unit
+  @param  Timeout               The time to wait command
+  @param  CmdStatus             The result of high level command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to excute command
+
+**/
+EFI_STATUS
+UsbBotExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  )
+{
+  USB_BOT_PROTOCOL          *UsbBot;
+  EFI_STATUS                Status;
+  UINTN                     TransLen;
+  UINT8                     Result;
+
+  *CmdStatus  = USB_MASS_CMD_FAIL;
+  UsbBot      = (USB_BOT_PROTOCOL *) Context;
+
+  //
+  // Send the command to the device. Return immediately if device
+  // rejects the command.
+  //
+  Status = UsbBotSendCommand (UsbBot, Cmd, CmdLen, DataDir, DataLen, Lun);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBotExecCommand: UsbBotSendCommand (%r)\n", Status));
+    return Status;
+  }
+
+  //
+  // Transfer the data. Don't return immediately even data transfer
+  // failed. The host should attempt to receive the CSW no matter
+  // whether it succeeds or fails.
+  //
+  TransLen = (UINTN) DataLen;
+  UsbBotDataTransfer (UsbBot, DataDir, Data, &TransLen, Timeout);
+
+  //
+  // Get the status, if that succeeds, interpret the result
+  //
+  Status = UsbBotGetStatus (UsbBot, DataLen, &Result);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbBotExecCommand: UsbBotGetStatus (%r)\n", Status));
+    return Status;
+  }
+
+  if (Result == 0) {
+    *CmdStatus = USB_MASS_CMD_SUCCESS;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the USB mass storage device by BOT protocol.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL.
+  @param  ExtendedVerification  If FALSE, just issue Bulk-Only Mass Storage Reset request.
+                                If TRUE, additionally reset parent hub port.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device..
+
+**/
+EFI_STATUS
+UsbBotResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  )
+{
+  USB_BOT_PROTOCOL        *UsbBot;
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINT32                  Timeout;
+
+  UsbBot = (USB_BOT_PROTOCOL *) Context;
+
+  if (ExtendedVerification) {
+    //
+    // If we need to do strictly reset, reset its parent hub port
+    //
+    Status = UsbBot->UsbIo->UsbPortReset (UsbBot->UsbIo);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Issue a class specific Bulk-Only Mass Storage Reset request,
+  // according to section 3.1 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+  //
+  Request.RequestType = 0x21;
+  Request.Request     = USB_BOT_RESET_REQUEST;
+  Request.Value       = 0;
+  Request.Index       = UsbBot->Interface.InterfaceNumber;
+  Request.Length      = 0;
+  Timeout             = USB_BOT_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbControlTransfer (
+                            UsbBot->UsbIo,
+                            &Request,
+                            EfiUsbNoData,
+                            Timeout,
+                            NULL,
+                            0,
+                            &Result
+                            );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // The device shall NAK the host's request until the reset is
+  // complete. We can use this to sync the device and host. For
+  // now just stall 100ms to wait for the device.
+  //
+  gBS->Stall (USB_BOT_RESET_DEVICE_STALL);
+
+  //
+  // Clear the Bulk-In and Bulk-Out stall condition.
+  //
+  UsbClearEndpointStall (UsbBot->UsbIo, UsbBot->BulkInEndpoint->EndpointAddress);
+  UsbClearEndpointStall (UsbBot->UsbIo, UsbBot->BulkOutEndpoint->EndpointAddress);
+
+  return Status;
+}
+
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the BOT protocol, that is, USB_BOT_PROTOCOL
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+EFI_STATUS
+UsbBotGetMaxLun (
+  IN  VOID                    *Context,
+  OUT UINT8                   *MaxLun
+  )
+{
+  USB_BOT_PROTOCOL        *UsbBot;
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  Result;
+  UINT32                  Timeout;
+
+  ASSERT (Context);
+  
+  UsbBot = (USB_BOT_PROTOCOL *) Context;
+
+  //
+  // Issue a class specific Bulk-Only Mass Storage get max lun reqest.
+  // according to section 3.2 of USB Mass Storage Class Bulk-Only Transport Spec, v1.0.
+  //
+  Request.RequestType = 0xA1;
+  Request.Request     = USB_BOT_GETLUN_REQUEST;
+  Request.Value       = 0;
+  Request.Index       = UsbBot->Interface.InterfaceNumber;
+  Request.Length      = 1;
+  Timeout             = USB_BOT_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  Status = UsbBot->UsbIo->UsbControlTransfer (
+                            UsbBot->UsbIo,
+                            &Request,
+                            EfiUsbDataIn,
+                            Timeout,
+                            (VOID *) MaxLun,
+                            1,
+                            &Result
+                            );
+
+  return Status;
+}
+
+/**
+  Clean up the resource used by this BOT protocol.
+
+  @param  Context         The context of the BOT protocol, that is, USB_BOT_PROTOCOL.
+
+  @retval EFI_SUCCESS     The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbBotCleanUp (
+  IN  VOID                    *Context
+  )
+{
+  FreePool (Context);
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h
new file mode 100644
index 0000000000..bdbddf559b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassBot.h
@@ -0,0 +1,193 @@
+/** @file
+  Definition for the USB mass storage Bulk-Only Transport protocol,
+  based on the "Universal Serial Bus Mass Storage Class Bulk-Only
+  Transport" Revision 1.0, September 31, 1999.
+
+Copyright (c) 2007 - 2011, 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.
+
+**/
+
+#ifndef _EFI_USBMASS_BOT_H_
+#define _EFI_USBMASS_BOT_H_
+
+extern USB_MASS_TRANSPORT mUsbBotTransport;
+
+//
+// Usb Bulk-Only class specfic request
+//
+#define USB_BOT_RESET_REQUEST    0xFF       ///< Bulk-Only Mass Storage Reset
+#define USB_BOT_GETLUN_REQUEST   0xFE       ///< Get Max Lun
+#define USB_BOT_CBW_SIGNATURE    0x43425355 ///< dCBWSignature, tag the packet as CBW
+#define USB_BOT_CSW_SIGNATURE    0x53425355 ///< dCSWSignature, tag the packet as CSW
+#define USB_BOT_MAX_LUN          0x0F       ///< Lun number is from 0 to 15
+#define USB_BOT_MAX_CMDLEN       16         ///< Maxium number of command from command set
+
+//
+// Usb BOT command block status values
+//
+#define USB_BOT_COMMAND_OK       0x00 ///< Command passed, good status
+#define USB_BOT_COMMAND_FAILED   0x01 ///< Command failed
+#define USB_BOT_COMMAND_ERROR    0x02 ///< Phase error, need to reset the device
+
+//
+// Usb Bot retry to get CSW, refers to specification[BOT10-5.3, it says 2 times]
+//
+#define USB_BOT_RECV_CSW_RETRY   3
+
+//
+// Usb Bot wait device reset complete, set by experience
+//  
+#define USB_BOT_RESET_DEVICE_STALL  (100 * USB_MASS_1_MILLISECOND)
+
+//
+// Usb Bot transport timeout, set by experience
+//
+#define USB_BOT_SEND_CBW_TIMEOUT     (3 * USB_MASS_1_SECOND)
+#define USB_BOT_RECV_CSW_TIMEOUT     (3 * USB_MASS_1_SECOND)
+#define USB_BOT_RESET_DEVICE_TIMEOUT (3 * USB_MASS_1_SECOND)
+
+#pragma pack(1)
+///
+/// The CBW (Command Block Wrapper) structures used by the USB BOT protocol.
+///
+typedef struct {
+  UINT32              Signature;
+  UINT32              Tag;
+  UINT32              DataLen;  ///< Length of data between CBW and CSW
+  UINT8               Flag;     ///< Bit 7, 0 ~ Data-Out, 1 ~ Data-In
+  UINT8               Lun;      ///< Lun number. Bits 0~3 are used
+  UINT8               CmdLen;   ///< Length of the command. Bits 0~4 are used
+  UINT8               CmdBlock[USB_BOT_MAX_CMDLEN];
+} USB_BOT_CBW;
+
+///
+/// The and CSW (Command Status Wrapper) structures used by the USB BOT protocol.
+///
+typedef struct {
+  UINT32              Signature;
+  UINT32              Tag;
+  UINT32              DataResidue;
+  UINT8               CmdStatus;
+} USB_BOT_CSW;
+#pragma pack()
+
+typedef struct {
+  //
+  // Put Interface at the first field to make it easy to distinguish BOT/CBI Protocol instance
+  //
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkOutEndpoint;
+  UINT32                        CbwTag;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+} USB_BOT_PROTOCOL;
+
+/**
+  Initializes USB BOT protocol.
+
+  This function initializes the USB mass storage class BOT protocol.
+  It will save its context which is a USB_BOT_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB BOT initialization fails.
+
+**/
+EFI_STATUS
+UsbBotInit (
+  IN  EFI_USB_IO_PROTOCOL       *UsbIo,
+  OUT VOID                      **Context OPTIONAL
+  );
+
+/**
+  Call the USB Mass Storage Class BOT protocol to issue
+  the command/data/status circle to execute the commands.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL
+  @param  Cmd                   The high level command
+  @param  CmdLen                The command length
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold data
+  @param  DataLen               The length of the data
+  @param  Lun                   The number of logic unit
+  @param  Timeout               The time to wait command
+  @param  CmdStatus             The result of high level command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to excute command
+
+**/
+EFI_STATUS
+UsbBotExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by BOT protocol.
+
+  @param  Context               The context of the BOT protocol, that is,
+                                USB_BOT_PROTOCOL.
+  @param  ExtendedVerification  If FALSE, just issue Bulk-Only Mass Storage Reset request.
+                                If TRUE, additionally reset parent hub port.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device..
+
+**/
+EFI_STATUS
+UsbBotResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                 ExtendedVerification
+  );
+
+/**
+  Get the max LUN (Logical Unit Number) of USB mass storage device.
+
+  @param  Context          The context of the BOT protocol, that is, USB_BOT_PROTOCOL
+  @param  MaxLun           Return pointer to the max number of LUN. (e.g. MaxLun=1 means LUN0 and
+                           LUN1 in all.)
+
+  @retval EFI_SUCCESS      Max LUN is got successfully.
+  @retval Others           Fail to execute this request.
+
+**/
+EFI_STATUS
+UsbBotGetMaxLun (
+  IN  VOID                    *Context,
+  OUT UINT8                   *MaxLun
+  );
+
+/**
+  Clean up the resource used by this BOT protocol.
+
+  @param  Context         The context of the BOT protocol, that is, USB_BOT_PROTOCOL.
+
+  @retval EFI_SUCCESS     The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbBotCleanUp (
+  IN  VOID                    *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c
new file mode 100644
index 0000000000..3f6201d053
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.c
@@ -0,0 +1,612 @@
+/** @file
+  Implementation of the USB mass storage Control/Bulk/Interrupt transport,
+  according to USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1.
+  Notice: it is being obsoleted by the standard body in favor of the BOT
+  (Bulk-Only Transport).
+
+Copyright (c) 2007 - 2011, 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 "UsbMass.h"
+
+//
+// Definition of USB CBI0 Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbCbi0Transport = {
+  USB_MASS_STORE_CBI0,
+  UsbCbiInit,
+  UsbCbiExecCommand,
+  UsbCbiResetDevice,
+  NULL,
+  UsbCbiCleanUp
+};
+
+//
+// Definition of USB CBI1 Transport Protocol
+//
+USB_MASS_TRANSPORT mUsbCbi1Transport = {
+  USB_MASS_STORE_CBI1,
+  UsbCbiInit,
+  UsbCbiExecCommand,
+  UsbCbiResetDevice,
+  NULL,
+  UsbCbiCleanUp
+};
+
+/**
+  Initializes USB CBI protocol.
+
+  This function initializes the USB mass storage class CBI protocol.
+  It will save its context which is a USB_CBI_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB CBI initialization fails.
+
+**/
+EFI_STATUS
+UsbCbiInit (
+  IN  EFI_USB_IO_PROTOCOL   *UsbIo,
+  OUT VOID                  **Context       OPTIONAL
+  )
+{
+  USB_CBI_PROTOCOL              *UsbCbi;
+  EFI_USB_INTERFACE_DESCRIPTOR  *Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   EndPoint;
+  EFI_STATUS                    Status;
+  UINT8                         Index;
+
+  //
+  // Allocate the CBI context for USB_CBI_PROTOCOL and 3 endpoint descriptors.
+  //
+  UsbCbi = AllocateZeroPool (
+             sizeof (USB_CBI_PROTOCOL) + 3 * sizeof (EFI_USB_ENDPOINT_DESCRIPTOR)
+             );
+  ASSERT (UsbCbi != NULL);
+
+  UsbCbi->UsbIo = UsbIo;
+
+  //
+  // Get the interface descriptor and validate that it
+  // is a USB Mass Storage CBI interface.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &UsbCbi->Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Interface = &UsbCbi->Interface;
+  if ((Interface->InterfaceProtocol != USB_MASS_STORE_CBI0)
+      && (Interface->InterfaceProtocol != USB_MASS_STORE_CBI1)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  //
+  // Locate and save the bulk-in, bulk-out, and interrupt endpoint
+  //
+  for (Index = 0; Index < Interface->NumEndpoints; Index++) {
+    Status = UsbIo->UsbGetEndpointDescriptor (UsbIo, Index, &EndPoint);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (USB_IS_BULK_ENDPOINT (EndPoint.Attributes)) {
+      //
+      // Use the first Bulk-In and Bulk-Out endpoints
+      //
+      if (USB_IS_IN_ENDPOINT (EndPoint.EndpointAddress) &&
+         (UsbCbi->BulkInEndpoint == NULL)) {
+
+        UsbCbi->BulkInEndpoint  = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1);
+        CopyMem(UsbCbi->BulkInEndpoint, &EndPoint, sizeof (EndPoint));;
+      }
+
+      if (USB_IS_OUT_ENDPOINT (EndPoint.EndpointAddress) &&
+         (UsbCbi->BulkOutEndpoint == NULL)) {
+
+        UsbCbi->BulkOutEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1) + 1;
+        CopyMem(UsbCbi->BulkOutEndpoint, &EndPoint, sizeof (EndPoint));
+      }
+    } else if (USB_IS_INTERRUPT_ENDPOINT (EndPoint.Attributes)) {
+      //
+      // Use the first interrupt endpoint if it is CBI0
+      //
+      if ((Interface->InterfaceProtocol == USB_MASS_STORE_CBI0) &&
+          (UsbCbi->InterruptEndpoint == NULL)) {
+
+        UsbCbi->InterruptEndpoint   = (EFI_USB_ENDPOINT_DESCRIPTOR *) (UsbCbi + 1) + 2;
+        CopyMem(UsbCbi->InterruptEndpoint, &EndPoint, sizeof (EndPoint));
+      }
+    }
+  }
+
+  if ((UsbCbi->BulkInEndpoint == NULL) || (UsbCbi->BulkOutEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+  if ((Interface->InterfaceProtocol == USB_MASS_STORE_CBI0) && (UsbCbi->InterruptEndpoint == NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_ERROR;
+  }
+
+  if (Context != NULL) {
+    *Context = UsbCbi;
+  } else {
+    FreePool (UsbCbi);
+  }
+ 
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (UsbCbi);
+  return Status;
+}
+
+/**
+  Send the command to the device using class specific control transfer.
+
+  This function sends command to the device using class specific control transfer.
+  The CBI contains three phases: Command, Data, and Status. This is Command phase.
+
+  @param  UsbCbi                The USB CBI protocol
+  @param  Cmd                   The high level command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  Timeout               The time to wait the command to finish
+
+  @retval EFI_SUCCESS           The command is sent to the device.
+  @retval Others                The command failed to transfer to device
+
+**/
+EFI_STATUS
+UsbCbiSendCommand (
+  IN USB_CBI_PROTOCOL       *UsbCbi,
+  IN UINT8                  *Cmd,
+  IN UINT8                  CmdLen,
+  IN UINT32                 Timeout
+  )
+{
+  EFI_USB_DEVICE_REQUEST  Request;
+  EFI_STATUS              Status;
+  UINT32                  TransStatus;
+  UINTN                   DataLen;
+  INTN                    Retry;
+
+  //
+  // Fill in the device request, CBI use the "Accept Device-Specific
+  // Cmd" (ADSC) class specific request to send commands.
+  //
+  Request.RequestType = 0x21;
+  Request.Request     = 0;
+  Request.Value       = 0;
+  Request.Index       = UsbCbi->Interface.InterfaceNumber;
+  Request.Length      = CmdLen;
+
+  Status              = EFI_SUCCESS;
+  Timeout             = Timeout / USB_MASS_1_MILLISECOND;
+
+  for (Retry = 0; Retry < USB_CBI_MAX_RETRY; Retry++) {
+    //
+    // Use USB I/O Protocol to send the command to the device
+    //
+    TransStatus = 0;
+    DataLen     = CmdLen;
+
+    Status = UsbCbi->UsbIo->UsbControlTransfer (
+                              UsbCbi->UsbIo,
+                              &Request,
+                              EfiUsbDataOut,
+                              Timeout,
+                              Cmd,
+                              DataLen,
+                              &TransStatus
+                              );
+    //
+    // The device can fail the command by STALL the control endpoint.
+    // It can delay the command by NAK the data or status stage, this
+    // is a "class-specific exemption to the USB specification". Retry
+    // if the command is NAKed.
+    //
+    if (EFI_ERROR (Status) && (TransStatus == EFI_USB_ERR_NAK)) {
+      continue;
+    }
+
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Transfer data between the device and host.
+
+  This function transfers data between the device and host.
+  The CBI contains three phases: Command, Data, and Status. This is Data phase.
+
+  @param  UsbCbi                The USB CBI device
+  @param  DataDir               The direction of the data transfer
+  @param  Data                  The buffer to hold the data for input or output.
+  @param  TransLen              On input, the expected transfer length.
+                                On output, the length of data actually transferred.
+  @param  Timeout               The time to wait for the command to execute
+
+  @retval EFI_SUCCESS           The data transferred successfully.
+  @retval EFI_SUCCESS           No data to transfer
+  @retval Others                Failed to transfer all the data
+
+**/
+EFI_STATUS
+UsbCbiDataTransfer (
+  IN USB_CBI_PROTOCOL         *UsbCbi,
+  IN EFI_USB_DATA_DIRECTION   DataDir,
+  IN OUT UINT8                *Data,
+  IN OUT UINTN                *TransLen,
+  IN UINT32                   Timeout
+  )
+{
+  EFI_USB_ENDPOINT_DESCRIPTOR *Endpoint;
+  EFI_STATUS                  Status;
+  UINT32                      TransStatus;
+  UINTN                       Remain;
+  UINTN                       Increment;
+  UINT8                       *Next;
+  UINTN                       Retry;
+
+  //
+  // If no data to transfer, just return EFI_SUCCESS.
+  //
+  if ((DataDir == EfiUsbNoData) || (*TransLen == 0)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Select the endpoint then issue the transfer
+  //
+  if (DataDir == EfiUsbDataIn) {
+    Endpoint = UsbCbi->BulkInEndpoint;
+  } else {
+    Endpoint = UsbCbi->BulkOutEndpoint;
+  }
+
+  Next    = Data;
+  Remain  = *TransLen;
+  Retry   = 0;
+  Status  = EFI_SUCCESS;
+  Timeout = Timeout / USB_MASS_1_MILLISECOND;
+
+  //
+  // Transfer the data with a loop. The length of data transferred once is restricted.
+  //
+  while (Remain > 0) {
+    TransStatus = 0;
+
+    if (Remain > (UINTN) USB_CBI_MAX_PACKET_NUM * Endpoint->MaxPacketSize) {
+      Increment = USB_CBI_MAX_PACKET_NUM * Endpoint->MaxPacketSize;
+    } else {
+      Increment = Remain;
+    }
+
+    Status = UsbCbi->UsbIo->UsbBulkTransfer (
+                              UsbCbi->UsbIo,
+                              Endpoint->EndpointAddress,
+                              Next,
+                              &Increment,
+                              Timeout,
+                              &TransStatus
+                              );
+    if (EFI_ERROR (Status)) {
+      if (TransStatus == EFI_USB_ERR_NAK) {
+        //
+        // The device can NAK the host if either the data/buffer isn't
+        // aviable or the command is in-progress.
+        // If data are partially transferred, we just ignore NAK and continue.
+        // If all data have been transferred and status is NAK, then we retry for several times.
+        // If retry exceeds the USB_CBI_MAX_RETRY, then return error status.
+        //
+        if (Increment == 0) {
+          if (++Retry > USB_CBI_MAX_RETRY) {
+            goto ON_EXIT;
+          }
+        } else {
+          Next   += Increment;
+          Remain -= Increment;
+          Retry   = 0;
+        }
+
+        continue;
+      }
+
+      //
+      // The device can fail the command by STALL the bulk endpoint.
+      // Clear the stall if that is the case.
+      //
+      if (TransStatus == EFI_USB_ERR_STALL) {
+        UsbClearEndpointStall (UsbCbi->UsbIo, Endpoint->EndpointAddress);
+      }
+
+      goto ON_EXIT;
+    }
+
+    Next += Increment;
+    Remain -= Increment;
+  }
+
+ON_EXIT:
+  *TransLen -= Remain;
+  return Status;
+}
+
+
+/**
+  Gets the result of high level command execution from interrupt endpoint.
+
+  This function returns the USB transfer status, and put the high level
+  command execution result in Result.
+  The CBI contains three phases: Command, Data, and Status. This is Status phase.
+
+  @param  UsbCbi                The USB CBI protocol
+  @param  Timeout               The time to wait for the command to execute
+  @param  Result                The result of the command execution.
+
+  @retval EFI_SUCCESS           The high level command execution result is
+                                retrieved in Result.
+  @retval Others                Failed to retrieve the result.
+
+**/
+EFI_STATUS
+UsbCbiGetStatus (
+  IN  USB_CBI_PROTOCOL        *UsbCbi,
+  IN  UINT32                  Timeout,
+  OUT USB_CBI_STATUS          *Result
+  )
+{
+  UINTN                     Len;
+  UINT8                     Endpoint;
+  EFI_STATUS                Status;
+  UINT32                    TransStatus;
+  INTN                      Retry;
+
+  Endpoint  = UsbCbi->InterruptEndpoint->EndpointAddress;
+  Status    = EFI_SUCCESS;
+  Timeout   = Timeout / USB_MASS_1_MILLISECOND;
+
+  //
+  // Attemp to the read the result from interrupt endpoint
+  //
+  for (Retry = 0; Retry < USB_CBI_MAX_RETRY; Retry++) {
+    TransStatus = 0;
+    Len         = sizeof (USB_CBI_STATUS);
+
+    Status = UsbCbi->UsbIo->UsbSyncInterruptTransfer (
+                              UsbCbi->UsbIo,
+                              Endpoint,
+                              Result,
+                              &Len,
+                              Timeout,
+                              &TransStatus
+                              );
+    //
+    // The CBI can NAK the interrupt endpoint if the command is in-progress.
+    //
+    if (EFI_ERROR (Status) && (TransStatus == EFI_USB_ERR_NAK)) {
+      continue;
+    }
+
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute USB mass storage command through the CBI0/CBI1 transport protocol.
+
+  @param  Context               The USB CBI Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+EFI_STATUS
+UsbCbiExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  )
+{
+  USB_CBI_PROTOCOL          *UsbCbi;
+  USB_CBI_STATUS            Result;
+  EFI_STATUS                Status;
+  UINTN                     TransLen;
+
+  *CmdStatus  = USB_MASS_CMD_SUCCESS;
+  UsbCbi      = (USB_CBI_PROTOCOL *) Context;
+
+  //
+  // Send the command to the device. Return immediately if device
+  // rejects the command.
+  //
+  Status = UsbCbiSendCommand (UsbCbi, Cmd, CmdLen, Timeout);
+  if (EFI_ERROR (Status)) {
+    gBS->Stall(10 * USB_MASS_1_MILLISECOND);
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiSendCommand (%r)\n",Status));
+    return Status;
+  }
+
+  //
+  // Transfer the data. Return this status if no interrupt endpoint
+  // is used to report the transfer status.
+  //
+  TransLen = (UINTN) DataLen;
+
+  Status   = UsbCbiDataTransfer (UsbCbi, DataDir, Data, &TransLen, Timeout);
+  if (UsbCbi->InterruptEndpoint == NULL) {
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiDataTransfer (%r)\n",Status));
+    return Status;
+  }
+
+  //
+  // Get the status. If it succeeds, interpret the result.
+  //
+  Status = UsbCbiGetStatus (UsbCbi, Timeout, &Result);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbCbiExecCommand: UsbCbiGetStatus (%r)\n",Status));
+    return Status;
+  }
+
+  if (UsbCbi->Interface.InterfaceSubClass == USB_MASS_STORE_UFI) {
+    //
+    // For UFI device, ASC and ASCQ are returned.
+    //
+    // Do not set the USB_MASS_CMD_FAIL for a request sense command
+    // as a bad result type doesn't mean a cmd failure
+    //
+    if (Result.Type != 0 && *(UINT8*)Cmd != 0x03) {
+      *CmdStatus = USB_MASS_CMD_FAIL;
+    }
+  } else {
+    //
+    // Check page 27, CBI spec 1.1 for vaious reture status.
+    //
+    switch (Result.Value & 0x03) {
+    case 0x00:
+      //
+      // Pass
+      //
+      *CmdStatus = USB_MASS_CMD_SUCCESS;
+      break;
+
+    case 0x02:
+      //
+      // Phase Error, response with reset.
+      // No break here to fall through to "Fail".
+      //
+      UsbCbiResetDevice (UsbCbi, FALSE);
+
+    case 0x01:
+      //
+      // Fail
+      //
+      *CmdStatus = USB_MASS_CMD_FAIL;
+      break;
+
+    case 0x03:
+      //
+      // Persistent Fail. Need to send REQUEST SENSE.
+      //
+      *CmdStatus = USB_MASS_CMD_PERSISTENT;
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the USB mass storage device by CBI protocol.
+
+  This function resets the USB mass storage device by CBI protocol.
+  The reset is defined as a non-data command. Don't use UsbCbiExecCommand
+  to send the command to device because that may introduce recursive loop.
+
+  @param  Context               The USB CBI protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+EFI_STATUS
+UsbCbiResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                  ExtendedVerification
+  )
+{
+  UINT8                     ResetCmd[USB_CBI_RESET_CMD_LEN];
+  USB_CBI_PROTOCOL          *UsbCbi;
+  USB_CBI_STATUS            Result;
+  EFI_STATUS                Status;
+  UINT32                    Timeout;
+
+  UsbCbi = (USB_CBI_PROTOCOL *) Context;
+
+  //
+  // Fill in the reset command.
+  //
+  SetMem (ResetCmd, USB_CBI_RESET_CMD_LEN, 0xFF);
+
+  ResetCmd[0] = 0x1D;
+  ResetCmd[1] = 0x04;
+  Timeout     = USB_CBI_RESET_DEVICE_TIMEOUT / USB_MASS_1_MILLISECOND;
+
+  //
+  // Send the command to the device. Don't use UsbCbiExecCommand here.
+  //
+  Status = UsbCbiSendCommand (UsbCbi, ResetCmd, USB_CBI_RESET_CMD_LEN, Timeout);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Just retrieve the status and ignore that. Then stall
+  // 50ms to wait for it to complete.
+  //
+  UsbCbiGetStatus (UsbCbi, Timeout, &Result);
+  gBS->Stall (USB_CBI_RESET_DEVICE_STALL);
+
+  //
+  // Clear the Bulk-In and Bulk-Out stall condition and init data toggle.
+  //
+  UsbClearEndpointStall (UsbCbi->UsbIo, UsbCbi->BulkInEndpoint->EndpointAddress);
+  UsbClearEndpointStall (UsbCbi->UsbIo, UsbCbi->BulkOutEndpoint->EndpointAddress);
+
+  return Status;
+}
+
+
+/**
+  Clean up the CBI protocol's resource.
+
+  @param  Context               The instance of CBI protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbCbiCleanUp (
+  IN  VOID                   *Context
+  )
+{
+  FreePool (Context);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h
new file mode 100644
index 0000000000..b414a46fde
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassCbi.h
@@ -0,0 +1,140 @@
+/** @file
+  Defination for the USB mass storage Control/Bulk/Interrupt (CBI) transport,
+  according to USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1.
+
+Copyright (c) 2007 - 2011, 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.
+
+**/
+
+#ifndef _EFI_USBMASS_CBI_H_
+#define _EFI_USBMASS_CBI_H_
+
+extern USB_MASS_TRANSPORT mUsbCbi0Transport;
+extern USB_MASS_TRANSPORT mUsbCbi1Transport;
+
+#define USB_CBI_MAX_PACKET_NUM        16
+#define USB_CBI_RESET_CMD_LEN         12
+//
+// USB CBI retry C/B/I transport times, set by experience
+//
+#define USB_CBI_MAX_RETRY             3
+//
+// Time to wait for USB CBI reset to complete, set by experience
+//  
+#define USB_CBI_RESET_DEVICE_STALL    (50 * USB_MASS_1_MILLISECOND)
+//
+// USB CBI transport timeout, set by experience
+//
+#define USB_CBI_RESET_DEVICE_TIMEOUT  (1 * USB_MASS_1_SECOND)
+
+typedef struct {
+  //
+  // Put Interface at the first field to make it easy to distinguish BOT/CBI Protocol instance
+  //
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkInEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *BulkOutEndpoint;
+  EFI_USB_ENDPOINT_DESCRIPTOR   *InterruptEndpoint;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+} USB_CBI_PROTOCOL;
+
+#pragma pack(1)
+typedef struct {
+  UINT8               Type;
+  UINT8               Value;
+} USB_CBI_STATUS;
+#pragma pack()
+
+/**
+  Initializes USB CBI protocol.
+
+  This function initializes the USB mass storage class CBI protocol.
+  It will save its context which is a USB_CBI_PROTOCOL structure
+  in the Context if Context isn't NULL.
+
+  @param  UsbIo                 The USB I/O Protocol instance
+  @param  Context               The buffer to save the context to
+
+  @retval EFI_SUCCESS           The device is successfully initialized.
+  @retval EFI_UNSUPPORTED       The transport protocol doesn't support the device.
+  @retval Other                 The USB CBI initialization fails.
+
+**/
+EFI_STATUS
+UsbCbiInit (
+  IN  EFI_USB_IO_PROTOCOL   *UsbIo,
+  OUT VOID                  **Context       OPTIONAL
+  );
+
+/**
+  Execute USB mass storage command through the CBI0/CBI1 transport protocol.
+
+  @param  Context               The USB CBI Protocol.
+  @param  Cmd                   The command to transfer to device
+  @param  CmdLen                The length of the command
+  @param  DataDir               The direction of data transfer
+  @param  Data                  The buffer to hold the data
+  @param  DataLen               The length of the buffer
+  @param  Lun                   Should be 0, this field for bot only
+  @param  Timeout               The time to wait
+  @param  CmdStatus             The result of the command execution
+
+  @retval EFI_SUCCESS           The command is executed successfully.
+  @retval Other                 Failed to execute the command
+
+**/
+EFI_STATUS
+UsbCbiExecCommand (
+  IN  VOID                    *Context,
+  IN  VOID                    *Cmd,
+  IN  UINT8                   CmdLen,
+  IN  EFI_USB_DATA_DIRECTION  DataDir,
+  IN  VOID                    *Data,
+  IN  UINT32                  DataLen,
+  IN  UINT8                   Lun,
+  IN  UINT32                  Timeout,
+  OUT UINT32                  *CmdStatus
+  );
+
+/**
+  Reset the USB mass storage device by CBI protocol.
+
+  This function resets the USB mass storage device by CBI protocol.
+  The reset is defined as a non-data command. Don't use UsbCbiExecCommand
+  to send the command to device because that may introduce recursive loop.
+
+  @param  Context               The USB CBI protocol
+  @param  ExtendedVerification  The flag controlling the rule of reset.
+                                Not used here.
+
+  @retval EFI_SUCCESS           The device is reset.
+  @retval Others                Failed to reset the device.
+
+**/
+EFI_STATUS
+UsbCbiResetDevice (
+  IN  VOID                    *Context,
+  IN  BOOLEAN                  ExtendedVerification
+  );
+
+/**
+  Clean up the CBI protocol's resource.
+
+  @param  Context               The instance of CBI protocol.
+
+  @retval EFI_SUCCESS           The resource is cleaned up.
+
+**/
+EFI_STATUS
+UsbCbiCleanUp (
+  IN  VOID                   *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c
new file mode 100644
index 0000000000..1fa8543e2b
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.c
@@ -0,0 +1,162 @@
+/** @file
+  This file is used to implement the EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2011, 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 "UsbMass.h"
+
+EFI_DISK_INFO_PROTOCOL gUsbDiskInfoProtocolTemplate = {
+  EFI_DISK_INFO_USB_INTERFACE_GUID,
+  UsbDiskInfoInquiry,
+  UsbDiskInfoIdentify,
+  UsbDiskInfoSenseData,
+  UsbDiskInfoWhichIde
+};
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with USB interface GUID. 
+
+  @param[in]  UsbMass  The pointer of USB_MASS_DEVICE.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  USB_MASS_DEVICE   *UsbMass
+  )
+{
+  CopyMem (&UsbMass->DiskInfo, &gUsbDiskInfoProtocolTemplate, sizeof (gUsbDiskInfoProtocolTemplate));
+}
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  )
+{
+  EFI_STATUS        Status;
+  USB_MASS_DEVICE   *UsbMass;
+
+  UsbMass  = USB_MASS_DEVICE_FROM_DISK_INFO (This);
+
+  Status = EFI_BUFFER_TOO_SMALL;
+  if (*InquiryDataSize >= sizeof (UsbMass->InquiryData)) {
+    Status = EFI_SUCCESS;
+    CopyMem (InquiryData, &UsbMass->InquiryData, sizeof (UsbMass->InquiryData));
+  }
+  *InquiryDataSize = sizeof (UsbMass->InquiryData);
+  return Status;
+}
+
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  )
+{
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h
new file mode 100644
index 0000000000..e0a32ca8ee
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassDiskInfo.h
@@ -0,0 +1,129 @@
+/** @file
+  Header file for EFI_DISK_INFO_PROTOCOL interface.
+
+Copyright (c) 2011, 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.
+
+**/
+
+#ifndef _EFI_USBMASS_DISKINFO_H_
+#define _EFI_USBMASS_DISKINFO_H_
+
+/**
+  Initialize the installation of DiskInfo protocol.
+
+  This function prepares for the installation of DiskInfo protocol on the child handle.
+  By default, it installs DiskInfo protocol with USB interface GUID. 
+
+  @param  UsbMass  The pointer of USB_MASS_DEVICE.
+
+**/
+VOID
+InitializeDiskInfo (
+  IN  USB_MASS_DEVICE   *UsbMass
+  );
+
+
+/**
+  Provides inquiry information for the controller type.
+  
+  This function is used to get inquiry data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] InquiryData       Pointer to a buffer for the inquiry data.
+  @param[in, out] InquiryDataSize   Pointer to the value for the inquiry data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading InquiryData from device 
+  @retval EFI_BUFFER_TOO_SMALL   InquiryDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoInquiry (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *InquiryData,
+  IN OUT UINT32                   *InquiryDataSize
+  );
+
+/**
+  Provides identify information for the controller type.
+
+  This function is used to get identify data.  Data format
+  of Identify data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL 
+                                    instance.
+  @param[in, out] IdentifyData      Pointer to a buffer for the identify data.
+  @param[in, out] IdentifyDataSize  Pointer to the value for the identify data
+                                    size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class 
+  @retval EFI_DEVICE_ERROR       Error reading IdentifyData from device 
+  @retval EFI_BUFFER_TOO_SMALL   IdentifyDataSize not big enough 
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoIdentify (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *IdentifyData,
+  IN OUT UINT32                   *IdentifyDataSize
+  );
+
+/**
+  Provides sense data information for the controller type.
+  
+  This function is used to get sense data. 
+  Data format of Sense data is defined by the Interface GUID.
+
+  @param[in]      This              Pointer to the EFI_DISK_INFO_PROTOCOL instance.
+  @param[in, out] SenseData         Pointer to the SenseData.
+  @param[in, out] SenseDataSize     Size of SenseData in bytes.
+  @param[out]     SenseDataNumber   Pointer to the value for the sense data size.
+
+  @retval EFI_SUCCESS            The command was accepted without any errors.
+  @retval EFI_NOT_FOUND          Device does not support this data class.
+  @retval EFI_DEVICE_ERROR       Error reading SenseData from device.
+  @retval EFI_BUFFER_TOO_SMALL   SenseDataSize not big enough.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoSenseData (
+  IN     EFI_DISK_INFO_PROTOCOL   *This,
+  IN OUT VOID                     *SenseData,
+  IN OUT UINT32                   *SenseDataSize,
+  OUT    UINT8                    *SenseDataNumber
+  );
+
+
+/**
+  This function is used to get controller information.
+
+  @param[in]  This         Pointer to the EFI_DISK_INFO_PROTOCOL instance. 
+  @param[out] IdeChannel   Pointer to the Ide Channel number.  Primary or secondary.
+  @param[out] IdeDevice    Pointer to the Ide Device number.  Master or slave.
+
+  @retval EFI_SUCCESS       IdeChannel and IdeDevice are valid.
+  @retval EFI_UNSUPPORTED   This is not an IDE device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbDiskInfoWhichIde (
+  IN  EFI_DISK_INFO_PROTOCOL   *This,
+  OUT UINT32                   *IdeChannel,
+  OUT UINT32                   *IdeDevice
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c
new file mode 100644
index 0000000000..9d1bb25fb3
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.c
@@ -0,0 +1,1106 @@
+/** @file
+  USB Mass Storage Driver that manages USB Mass Storage Device and produces Block I/O Protocol.
+
+Copyright (c) 2007 - 2015, 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 "UsbMass.h"
+
+#define USB_MASS_TRANSPORT_COUNT    3
+//
+// Array of USB transport interfaces. 
+//
+USB_MASS_TRANSPORT *mUsbMassTransport[USB_MASS_TRANSPORT_COUNT] = {
+  &mUsbCbi0Transport,
+  &mUsbCbi1Transport,
+  &mUsbBotTransport,
+};
+
+EFI_DRIVER_BINDING_PROTOCOL gUSBMassDriverBinding = {
+  USBMassDriverBindingSupported,
+  USBMassDriverBindingStart,
+  USBMassDriverBindingStop,
+  0x11,
+  NULL,
+  NULL
+};
+
+/**
+  Reset the block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.Reset(). 
+  It resets the block device hardware.
+  ExtendedVerification is ignored in this implementation.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  ExtendedVerification   Indicates that the driver may perform a more exhaustive
+                                 verification operation of the device during reset.
+
+  @retval EFI_SUCCESS            The block device was reset.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReset (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN BOOLEAN                  ExtendedVerification
+  )
+{
+  USB_MASS_DEVICE *UsbMass;
+  EFI_TPL         OldTpl;
+  EFI_STATUS      Status;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Status  = UsbMass->Transport->Reset (UsbMass->Context, ExtendedVerification);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Reads the requested number of blocks from the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.ReadBlocks(). 
+  It reads the requested number of blocks from the device.
+  All the blocks are read, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the read request is for.
+  @param  Lba                    The starting logical block address to read from on the device.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 A pointer to the destination buffer for the data. The caller is
+                                 responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS            The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the read operation.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER  The read request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReadBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  USB_MASS_DEVICE     *UsbMass;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  EFI_TPL             OldTpl;
+  UINTN               TotalBlock;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Media   = &UsbMass->BlockIoMedia;
+
+  //
+  // If it is a removable media, such as CD-Rom or Usb-Floppy,
+  // need to detect the media before each read/write. While some of
+  // Usb-Flash is marked as removable media.
+  //
+  if (Media->RemovableMedia) {
+    Status = UsbBootDetectMedia (UsbMass);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto ON_EXIT;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto ON_EXIT;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // BufferSize must be a multiple of the intrinsic block size of the device.
+  //
+  if ((BufferSize % Media->BlockSize) != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto ON_EXIT;
+  }
+
+  TotalBlock = BufferSize / Media->BlockSize;
+
+  //
+  // Make sure the range to read is valid.
+  //
+  if (Lba + TotalBlock - 1 > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (UsbMass->Cdb16Byte) {
+    Status = UsbBootReadBlocks16 (UsbMass, Lba, TotalBlock, Buffer);
+  } else {
+    Status = UsbBootReadBlocks (UsbMass, (UINT32) Lba, TotalBlock, Buffer);
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassReadBlocks: UsbBootReadBlocks (%r) -> Reset\n", Status));
+    UsbMassReset (This, TRUE);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Writes a specified number of blocks to the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.WriteBlocks(). 
+  It writes a specified number of blocks to the device.
+  All blocks are written, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the write request is for.
+  @param  Lba                    The starting logical block address to be written.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED    The device cannot be written to.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic
+                                 block size of the device.
+  @retval EFI_INVALID_PARAMETER  The write request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassWriteBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  )
+{
+  USB_MASS_DEVICE     *UsbMass;
+  EFI_BLOCK_IO_MEDIA  *Media;
+  EFI_STATUS          Status;
+  EFI_TPL             OldTpl;
+  UINTN               TotalBlock;
+
+  //
+  // Raise TPL to TPL_NOTIFY to serialize all its operations
+  // to protect shared data structures.
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (This);
+  Media   = &UsbMass->BlockIoMedia;
+
+  //
+  // If it is a removable media, such as CD-Rom or Usb-Floppy,
+  // need to detect the media before each read/write. Some of
+  // USB Flash is marked as removable media.
+  //
+  if (Media->RemovableMedia) {
+    Status = UsbBootDetectMedia (UsbMass);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  if (!(Media->MediaPresent)) {
+    Status = EFI_NO_MEDIA;
+    goto ON_EXIT;
+  }
+
+  if (MediaId != Media->MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+    goto ON_EXIT;
+  }
+
+  if (BufferSize == 0) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // BufferSize must be a multiple of the intrinsic block size of the device.
+  //
+  if ((BufferSize % Media->BlockSize) != 0) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto ON_EXIT;
+  }
+
+  TotalBlock = BufferSize / Media->BlockSize;
+
+  //
+  // Make sure the range to write is valid.
+  //
+  if (Lba + TotalBlock - 1 > Media->LastBlock) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // Try to write the data even the device is marked as ReadOnly,
+  // and clear the status should the write succeed.
+  //
+  if (UsbMass->Cdb16Byte) {
+    Status = UsbBootWriteBlocks16 (UsbMass, Lba, TotalBlock, Buffer);
+  } else {
+    Status = UsbBootWriteBlocks (UsbMass, (UINT32) Lba, TotalBlock, Buffer);
+  }  
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassWriteBlocks: UsbBootWriteBlocks (%r) -> Reset\n", Status));
+    UsbMassReset (This, TRUE);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Flushes all modified data to a physical block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.FlushBlocks().
+  USB mass storage device doesn't support write cache,
+  so return EFI_SUCCESS directly.
+
+  @param  This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS            All outstanding data were written correctly to the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to write data.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+
+  @param  UsbMass                The USB mass storage device
+
+  @retval EFI_SUCCESS            The media parameters are updated successfully.
+  @retval Others                 Failed to get the media parameters.
+
+**/
+EFI_STATUS
+UsbMassInitMedia (
+  IN USB_MASS_DEVICE          *UsbMass
+  )
+{
+  EFI_BLOCK_IO_MEDIA          *Media;
+  EFI_STATUS                  Status;
+
+  Media = &UsbMass->BlockIoMedia;
+
+  //
+  // Fields of EFI_BLOCK_IO_MEDIA are defined in UEFI 2.0 spec,
+  // section for Block I/O Protocol.
+  //
+  Media->MediaPresent     = FALSE;
+  Media->LogicalPartition = FALSE;
+  Media->ReadOnly         = FALSE;
+  Media->WriteCaching     = FALSE;
+  Media->IoAlign          = 0;
+  Media->MediaId          = 1;
+
+  Status = UsbBootGetParams (UsbMass);
+  return Status;
+}
+
+/**
+  Initilize the USB Mass Storage transport.
+
+  This function tries to find the matching USB Mass Storage transport
+  protocol for USB device. If found, initializes the matching transport.
+
+  @param  This            The USB mass driver's driver binding.
+  @param  Controller      The device to test.
+  @param  Transport       The pointer to pointer to USB_MASS_TRANSPORT.
+  @param  Context         The parameter for USB_MASS_DEVICE.Context.
+  @param  MaxLun          Get the MaxLun if is BOT dev.
+
+  @retval EFI_SUCCESS     The initialization is successful.
+  @retval EFI_UNSUPPORTED No matching transport protocol is found.
+  @retval Others          Failed to initialize dev.
+
+**/
+EFI_STATUS
+UsbMassInitTransport (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Controller,
+  OUT USB_MASS_TRANSPORT           **Transport,
+  OUT VOID                         **Context,
+  OUT UINT8                        *MaxLun
+  )
+{
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  UINT8                         Index;
+  EFI_STATUS                    Status;
+ 
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  
+  Status = EFI_UNSUPPORTED;
+
+  //
+  // Traverse the USB_MASS_TRANSPORT arrary and try to find the
+  // matching transport protocol.
+  // If not found, return EFI_UNSUPPORTED.
+  // If found, execute USB_MASS_TRANSPORT.Init() to initialize the transport context.
+  //
+  for (Index = 0; Index < USB_MASS_TRANSPORT_COUNT; Index++) {
+    *Transport = mUsbMassTransport[Index];
+
+    if (Interface.InterfaceProtocol == (*Transport)->Protocol) {
+      Status  = (*Transport)->Init (UsbIo, Context);
+      break;
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // For BOT device, try to get its max LUN. 
+  // If max LUN is 0, then it is a non-lun device.
+  // Otherwise, it is a multi-lun device.
+  //
+  if ((*Transport)->Protocol == USB_MASS_STORE_BOT) {
+    (*Transport)->GetMaxLun (*Context, MaxLun);
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+  return Status;  
+}
+
+/**
+  Initialize data for device that supports multiple LUNSs.
+
+  @param  This                 The Driver Binding Protocol instance.
+  @param  Controller           The device to initialize.
+  @param  Transport            Pointer to USB_MASS_TRANSPORT.
+  @param  Context              Parameter for USB_MASS_DEVICE.Context.
+  @param  DevicePath           The remaining device path.
+  @param  MaxLun               The max LUN number.
+
+  @retval EFI_SUCCESS          At least one LUN is initialized successfully.
+  @retval EFI_NOT_FOUND        Fail to initialize any of multiple LUNs.
+
+**/
+EFI_STATUS
+UsbMassInitMultiLun (
+  IN EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN EFI_HANDLE                    Controller,
+  IN USB_MASS_TRANSPORT            *Transport,
+  IN VOID                          *Context,
+  IN EFI_DEVICE_PATH_PROTOCOL      *DevicePath,
+  IN UINT8                         MaxLun
+  )
+{
+  USB_MASS_DEVICE                  *UsbMass;
+  EFI_USB_IO_PROTOCOL              *UsbIo;
+  DEVICE_LOGICAL_UNIT_DEVICE_PATH  LunNode;
+  UINT8                            Index;
+  EFI_STATUS                       Status;
+  EFI_STATUS                       ReturnStatus;
+
+  ASSERT (MaxLun > 0);
+  ReturnStatus = EFI_NOT_FOUND;
+
+  for (Index = 0; Index <= MaxLun; Index++) { 
+
+    DEBUG ((EFI_D_INFO, "UsbMassInitMultiLun: Start to initialize No.%d logic unit\n", Index));
+    
+    UsbIo   = NULL;
+    UsbMass = AllocateZeroPool (sizeof (USB_MASS_DEVICE));
+    ASSERT (UsbMass != NULL);
+      
+    UsbMass->Signature            = USB_MASS_SIGNATURE;
+    UsbMass->UsbIo                = UsbIo;
+    UsbMass->BlockIo.Media        = &UsbMass->BlockIoMedia;
+    UsbMass->BlockIo.Reset        = UsbMassReset;
+    UsbMass->BlockIo.ReadBlocks   = UsbMassReadBlocks;
+    UsbMass->BlockIo.WriteBlocks  = UsbMassWriteBlocks;
+    UsbMass->BlockIo.FlushBlocks  = UsbMassFlushBlocks;
+    UsbMass->OpticalStorage       = FALSE;
+    UsbMass->Transport            = Transport;
+    UsbMass->Context              = Context;
+    UsbMass->Lun                  = Index;
+    
+    //
+    // Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+    //
+    Status = UsbMassInitMedia (UsbMass);
+    if ((EFI_ERROR (Status)) && (Status != EFI_NO_MEDIA)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: UsbMassInitMedia (%r)\n", Status));
+      FreePool (UsbMass);
+      continue;
+    }
+
+    //
+    // Create a device path node for device logic unit, and append it.
+    //
+    LunNode.Header.Type    = MESSAGING_DEVICE_PATH;
+    LunNode.Header.SubType = MSG_DEVICE_LOGICAL_UNIT_DP;
+    LunNode.Lun            = UsbMass->Lun;
+  
+    SetDevicePathNodeLength (&LunNode.Header, sizeof (LunNode));
+  
+    UsbMass->DevicePath = AppendDevicePathNode (DevicePath, &LunNode.Header);
+  
+    if (UsbMass->DevicePath == NULL) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: failed to create device logic unit device path\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      FreePool (UsbMass);
+      continue;
+    }
+
+    InitializeDiskInfo (UsbMass);
+
+    //
+    // Create a new handle for each LUN, and install Block I/O Protocol and Device Path Protocol.
+    //
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &UsbMass->Controller,
+                    &gEfiDevicePathProtocolGuid,
+                    UsbMass->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: InstallMultipleProtocolInterfaces (%r)\n", Status));
+      FreePool (UsbMass->DevicePath);
+      FreePool (UsbMass);
+      continue;
+    }
+
+    //
+    // Open USB I/O Protocol by child to setup a parent-child relationship.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbIoProtocolGuid,
+                    (VOID **) &UsbIo,
+                    This->DriverBindingHandle,
+                    UsbMass->Controller,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "UsbMassInitMultiLun: OpenUsbIoProtocol By Child (%r)\n", Status));
+      gBS->UninstallMultipleProtocolInterfaces (
+             &UsbMass->Controller,
+             &gEfiDevicePathProtocolGuid,
+             UsbMass->DevicePath,
+             &gEfiBlockIoProtocolGuid,
+             &UsbMass->BlockIo,
+             &gEfiDiskInfoProtocolGuid,
+             &UsbMass->DiskInfo,
+             NULL
+             );
+      FreePool (UsbMass->DevicePath);
+      FreePool (UsbMass);
+      continue;
+    }
+    ReturnStatus = EFI_SUCCESS;
+    DEBUG ((EFI_D_INFO, "UsbMassInitMultiLun: Success to initialize No.%d logic unit\n", Index));
+  }
+  
+  return ReturnStatus;
+}
+
+/**
+  Initialize data for device that does not support multiple LUNSs.
+
+  @param  This            The Driver Binding Protocol instance.
+  @param  Controller      The device to initialize.
+  @param  Transport       Pointer to USB_MASS_TRANSPORT.
+  @param  Context         Parameter for USB_MASS_DEVICE.Context.
+
+  @retval EFI_SUCCESS     Initialization succeeds.
+  @retval Other           Initialization fails.
+
+**/
+EFI_STATUS
+UsbMassInitNonLun (
+  IN EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN EFI_HANDLE                    Controller,
+  IN USB_MASS_TRANSPORT            *Transport,
+  IN VOID                          *Context
+  )
+{
+  USB_MASS_DEVICE             *UsbMass;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  EFI_STATUS                  Status;
+
+  UsbIo   = NULL;
+  UsbMass = AllocateZeroPool (sizeof (USB_MASS_DEVICE));
+  ASSERT (UsbMass != NULL);
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassInitNonLun: OpenUsbIoProtocol By Driver (%r)\n", Status));
+    goto ON_ERROR;
+  }
+  
+  UsbMass->Signature            = USB_MASS_SIGNATURE;
+  UsbMass->Controller           = Controller;
+  UsbMass->UsbIo                = UsbIo;
+  UsbMass->BlockIo.Media        = &UsbMass->BlockIoMedia;
+  UsbMass->BlockIo.Reset        = UsbMassReset;
+  UsbMass->BlockIo.ReadBlocks   = UsbMassReadBlocks;
+  UsbMass->BlockIo.WriteBlocks  = UsbMassWriteBlocks;
+  UsbMass->BlockIo.FlushBlocks  = UsbMassFlushBlocks;
+  UsbMass->OpticalStorage       = FALSE;
+  UsbMass->Transport            = Transport;
+  UsbMass->Context              = Context;
+  
+  //
+  // Initialize the media parameter data for EFI_BLOCK_IO_MEDIA of Block I/O Protocol.
+  //
+  Status = UsbMassInitMedia (UsbMass);
+  if ((EFI_ERROR (Status)) && (Status != EFI_NO_MEDIA)) {
+    DEBUG ((EFI_D_ERROR, "UsbMassInitNonLun: UsbMassInitMedia (%r)\n", Status));
+    goto ON_ERROR;
+  }
+    
+  InitializeDiskInfo (UsbMass);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiBlockIoProtocolGuid,
+                  &UsbMass->BlockIo,
+                  &gEfiDiskInfoProtocolGuid,
+                  &UsbMass->DiskInfo,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  if (UsbMass != NULL) {
+    FreePool (UsbMass);
+  }
+  if (UsbIo != NULL) {
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsbIoProtocolGuid,
+           This->DriverBindingHandle,
+           Controller
+           );
+  }
+  return Status;  
+}
+
+
+/**
+  Check whether the controller is a supported USB mass storage.
+
+  @param  This                   The USB mass storage driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  Interface;
+  USB_MASS_TRANSPORT            *Transport;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the interface descriptor to check the USB class and find a transport
+  // protocol handler.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &Interface);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = EFI_UNSUPPORTED;
+
+  if (Interface.InterfaceClass != USB_MASS_STORE_CLASS) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Traverse the USB_MASS_TRANSPORT arrary and try to find the
+  // matching transport method.
+  // If not found, return EFI_UNSUPPORTED.
+  // If found, execute USB_MASS_TRANSPORT.Init() to initialize the transport context.
+  //
+  for (Index = 0; Index < USB_MASS_TRANSPORT_COUNT; Index++) {
+    Transport = mUsbMassTransport[Index];
+    if (Interface.InterfaceProtocol == Transport->Protocol) {
+      Status = Transport->Init (UsbIo, NULL);
+      break;
+    }
+  }
+
+ON_EXIT:
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  return Status;
+}
+
+/**
+  Starts the USB mass storage device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mass storage device,
+  installs Block I/O Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mass storage device.
+
+  @param  This                  The USB mass storage driver binding protocol.
+  @param  Controller            The USB mass storage device to start on
+  @param  RemainingDevicePath   The remaining device path.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  USB_MASS_TRANSPORT            *Transport;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  VOID                          *Context;
+  UINT8                         MaxLun;
+  EFI_STATUS                    Status;
+  EFI_USB_IO_PROTOCOL           *UsbIo; 
+  EFI_TPL                       OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Transport = NULL;
+  Context   = NULL;
+  MaxLun    = 0;
+
+  Status = UsbMassInitTransport (This, Controller, &Transport, &Context, &MaxLun);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitTransport (%r)\n", Status));
+    goto Exit;
+  }
+  if (MaxLun == 0) {
+    //
+    // Initialize data for device that does not support multiple LUNSs.
+    //
+    Status = UsbMassInitNonLun (This, Controller, Transport, Context);
+    if (EFI_ERROR (Status)) { 
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitNonLun (%r)\n", Status));
+    }
+  } else {
+    //
+    // Open device path to prepare for appending Device Logic Unit node.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiDevicePathProtocolGuid,
+                    (VOID **) &DevicePath,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: OpenDevicePathProtocol By Driver (%r)\n", Status));
+      goto Exit;
+    }
+
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUsbIoProtocolGuid,
+                    (VOID **) &UsbIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: OpenUsbIoProtocol By Driver (%r)\n", Status));
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiDevicePathProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+      goto Exit;
+    }
+
+    //
+    // Initialize data for device that supports multiple LUNs.
+    // EFI_SUCCESS is returned if at least 1 LUN is initialized successfully.
+    //
+    Status = UsbMassInitMultiLun (This, Controller, Transport, Context, DevicePath, MaxLun);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+              Controller,
+              &gEfiDevicePathProtocolGuid,
+              This->DriverBindingHandle,
+              Controller
+              );
+      gBS->CloseProtocol (
+              Controller,
+              &gEfiUsbIoProtocolGuid,
+              This->DriverBindingHandle,
+              Controller
+              );
+      DEBUG ((EFI_D_ERROR, "USBMassDriverBindingStart: UsbMassInitMultiLun (%r) with Maxlun=%d\n", Status, MaxLun));
+    }
+  }
+Exit:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Stop controlling the device.
+
+  @param  This                   The USB mass storage driver binding
+  @param  Controller             The device controller controlled by the driver.
+  @param  NumberOfChildren       The number of children of this device
+  @param  ChildHandleBuffer      The buffer of children handle.
+
+  @retval EFI_SUCCESS            The driver stopped from controlling the device.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval EFI_UNSUPPORTED        Block I/O Protocol is not installed on Controller.
+  @retval Others                 Failed to stop the driver
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN  EFI_HANDLE                  Controller,
+  IN  UINTN                       NumberOfChildren,
+  IN  EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  USB_MASS_DEVICE       *UsbMass;
+  EFI_USB_IO_PROTOCOL   *UsbIo;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+  UINTN                 Index;
+  BOOLEAN               AllChildrenStopped;
+
+  //
+  // This is a bus driver stop function since multi-lun is supported.
+  // There are three kinds of device handles that might be passed:
+  // 1st is a handle with USB I/O & Block I/O installed (non-multi-lun)
+  // 2nd is a handle with Device Path & USB I/O installed (multi-lun root)
+  // 3rd is a handle with Device Path & USB I/O & Block I/O installed (multi-lun).
+  //
+  if (NumberOfChildren == 0) {
+    //
+    // A handle without any children, might be 1st and 2nd type.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+  
+    if (EFI_ERROR(Status)) {
+      //
+      // This is a 2nd type handle(multi-lun root), it needs to close devicepath
+      // and usbio protocol.
+      //
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiDevicePathProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiUsbIoProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+      DEBUG ((EFI_D_INFO, "Success to stop multi-lun root handle\n"));
+      return EFI_SUCCESS;
+    }
+    
+    //
+    // This is a 1st type handle(non-multi-lun), which only needs to uninstall
+    // Block I/O Protocol, close USB I/O Protocol and free mass device.
+    //
+    UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (BlockIo);
+  
+    //
+    // Uninstall Block I/O protocol from the device handle,
+    // then call the transport protocol to stop itself.
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    Controller,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  
+    UsbMass->Transport->CleanUp (UsbMass->Context);
+    FreePool (UsbMass);
+    
+    DEBUG ((EFI_D_INFO, "Success to stop non-multi-lun root handle\n"));
+    return EFI_SUCCESS;
+  } 
+
+  //
+  // This is a 3rd type handle(multi-lun), which needs uninstall
+  // Block I/O Protocol and Device Path Protocol, close USB I/O Protocol and 
+  // free mass device for all children.
+  //
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlockIo,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      DEBUG ((EFI_D_ERROR, "Fail to stop No.%d multi-lun child handle when opening blockio\n", (UINT32)Index));
+      continue;
+    }
+
+    UsbMass = USB_MASS_DEVICE_FROM_BLOCK_IO (BlockIo);
+
+    gBS->CloseProtocol (
+           Controller,
+           &gEfiUsbIoProtocolGuid,
+           This->DriverBindingHandle,
+           ChildHandleBuffer[Index]
+           );
+  
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ChildHandleBuffer[Index],
+                    &gEfiDevicePathProtocolGuid,
+                    UsbMass->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &UsbMass->BlockIo,
+                    &gEfiDiskInfoProtocolGuid,
+                    &UsbMass->DiskInfo,
+                    NULL
+                    );
+    
+    if (EFI_ERROR (Status)) {
+      //
+      // Fail to uninstall Block I/O Protocol and Device Path Protocol, so re-open USB I/O Protocol by child.
+      //
+      AllChildrenStopped = FALSE;
+      DEBUG ((EFI_D_ERROR, "Fail to stop No.%d multi-lun child handle when uninstalling blockio and devicepath\n", (UINT32)Index));
+      
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiUsbIoProtocolGuid,
+             (VOID **) &UsbIo,
+             This->DriverBindingHandle,
+             ChildHandleBuffer[Index],
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+    } else {
+      //
+      // Succeed to stop this multi-lun handle, so go on with next child.
+      //
+      if (((Index + 1) == NumberOfChildren) && AllChildrenStopped) {
+        UsbMass->Transport->CleanUp (UsbMass->Context);
+      }
+      FreePool (UsbMass);
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+  
+  DEBUG ((EFI_D_INFO, "Success to stop all %d multi-lun children handles\n", (UINT32) NumberOfChildren));
+  return EFI_SUCCESS;
+}
+
+/**
+  Entrypoint of USB Mass Storage Driver.
+
+  This function is the entrypoint of USB Mass Storage Driver. It installs Driver Binding
+  Protocol together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassStorageEntryPoint (
+  IN EFI_HANDLE               ImageHandle,
+  IN EFI_SYSTEM_TABLE         *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Install driver binding protocol
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUSBMassDriverBinding,
+             ImageHandle,
+             &gUsbMassStorageComponentName,
+             &gUsbMassStorageComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h
new file mode 100644
index 0000000000..0f013c1ad7
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassImpl.h
@@ -0,0 +1,333 @@
+/** @file
+  Definitions of functions for Driver Binding Protocol and Block I/O Protocol,
+  and other internal definitions.
+
+Copyright (c) 2007 - 2011, 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.
+
+**/
+
+#ifndef _EFI_USBMASS_IMPL_H_
+#define _EFI_USBMASS_IMPL_H_
+
+#define  USB_MASS_SIGNATURE    SIGNATURE_32 ('U', 's', 'b', 'M')
+
+#define USB_MASS_DEVICE_FROM_BLOCK_IO(a) \
+        CR (a, USB_MASS_DEVICE, BlockIo, USB_MASS_SIGNATURE)
+
+#define USB_MASS_DEVICE_FROM_DISK_INFO(a) \
+        CR (a, USB_MASS_DEVICE, DiskInfo, USB_MASS_SIGNATURE)
+
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMassStorageComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMassStorageComponentName2;
+
+//
+// Functions for Driver Binding Protocol
+//
+
+/**
+  Check whether the controller is a supported USB mass storage.
+
+  @param  This                   The USB mass storage driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Starts the USB mass storage device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mass storage device,
+  installs Block I/O Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mass storage device.
+
+  @param  This                  The USB mass storage driver binding protocol.
+  @param  Controller            The USB mass storage device to start on
+  @param  RemainingDevicePath   The remaining device path.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop controlling the device.
+
+  @param  This                   The USB mass storage driver binding
+  @param  Controller             The device controller controlled by the driver.
+  @param  NumberOfChildren       The number of children of this device
+  @param  ChildHandleBuffer      The buffer of children handle.
+
+  @retval EFI_SUCCESS            The driver stopped from controlling the device.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+  @retval EFI_UNSUPPORTED        Block I/O Protocol is not installed on Controller.
+  @retval Others                 Failed to stop the driver
+
+**/
+EFI_STATUS
+EFIAPI
+USBMassDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN  EFI_HANDLE                  Controller,
+  IN  UINTN                       NumberOfChildren,
+  IN  EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+//
+// Functions for Block I/O Protocol
+//
+
+/**
+  Reset the block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.Reset(). 
+  It resets the block device hardware.
+  ExtendedVerification is ignored in this implementation.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  ExtendedVerification   Indicates that the driver may perform a more exhaustive
+                                 verification operation of the device during reset.
+
+  @retval EFI_SUCCESS            The block device was reset.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReset (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN BOOLEAN                  ExtendedVerification
+  );
+
+/**
+  Reads the requested number of blocks from the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.ReadBlocks(). 
+  It reads the requested number of blocks from the device.
+  All the blocks are read, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the read request is for.
+  @param  Lba                    The starting logical block address to read from on the device.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 A pointer to the destination buffer for the data. The caller is
+                                 responsible for either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS            The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the read operation.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER  The read request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassReadBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  OUT VOID                    *Buffer
+  );
+
+/**
+  Writes a specified number of blocks to the device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.WriteBlocks(). 
+  It writes a specified number of blocks to the device.
+  All blocks are written, or an error is returned.
+
+  @param  This                   Indicates a pointer to the calling context.
+  @param  MediaId                The media ID that the write request is for.
+  @param  Lba                    The starting logical block address to be written.
+  @param  BufferSize             The size of the Buffer in bytes.
+                                 This must be a multiple of the intrinsic block size of the device.
+  @param  Buffer                 Pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS            The data were written correctly to the device.
+  @retval EFI_WRITE_PROTECTED    The device cannot be written to.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+  @retval EFI_MEDIA_CHANGED      The MediaId is not for the current media.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to perform the write operation.
+  @retval EFI_BAD_BUFFER_SIZE    The BufferSize parameter is not a multiple of the intrinsic
+                                 block size of the device.
+  @retval EFI_INVALID_PARAMETER  The write request contains LBAs that are not valid,
+                                 or the buffer is not on proper alignment.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassWriteBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN UINT32                   MediaId,
+  IN EFI_LBA                  Lba,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  );
+
+/**
+  Flushes all modified data to a physical block device.
+
+  This function implements EFI_BLOCK_IO_PROTOCOL.FlushBlocks().
+  USB mass storage device doesn't support write cache,
+  so return EFI_SUCCESS directly.
+
+  @param  This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS            All outstanding data were written correctly to the device.
+  @retval EFI_DEVICE_ERROR       The device reported an error while attempting to write data.
+  @retval EFI_NO_MEDIA           There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMassStorageGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
new file mode 100644
index 0000000000..26d15c7679
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
@@ -0,0 +1,87 @@
+## @file
+# USB Mass Storage Driver that manages USB mass storage devices and produces Block I/O Protocol.
+#
+# The USB mass storage class is specified in two layers: the bottom layer
+# is the transportation protocol. The top layer is the command set.
+# The transportation layer provides the transportation of the command, data and result.
+# The command set defines the command, data and result.
+# The Bulk-Only-Transport and Control/Bulk/Interrupt transport are two transportation protocol.
+# USB mass storage class adopts various industrial standard as its command set.
+# This module refers to following specifications:
+# 1. USB Mass Storage Specification for Bootability, Revision 1.0
+# 2. USB Mass Storage Class Control/Bulk/Interrupt (CBI) Transport, Revision 1.1
+# 3. USB Mass Storage Class Bulk-Only Transport, Revision 1.0.
+# 4. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMassStorageDxe
+  MODULE_UNI_FILE                = UsbMassStorageDxe.uni
+  FILE_GUID                      = 9FB4B4A7-42C0-4bcd-8540-9BCC6711F83E
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMassStorageEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUSBMassDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMassStorageComponentName
+#  COMPONENT_NAME2               =  gUsbMassStorageComponentName2
+#
+
+[Sources]
+  UsbMassBoot.h
+  UsbMassImpl.h
+  UsbMassBot.h
+  UsbMassBot.c
+  ComponentName.c
+  UsbMassImpl.c
+  UsbMassBoot.c
+  UsbMassCbi.h
+  UsbMass.h
+  UsbMassCbi.c
+  UsbMassDiskInfo.h
+  UsbMassDiskInfo.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiBlockIoProtocolGuid                       ## BY_START
+  gEfiDiskInfoProtocolGuid                      ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMassStorageDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni
new file mode 100644
index 0000000000..77cf18cd97
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni
new file mode 100644
index 0000000000..7603d47e39
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c
new file mode 100644
index 0000000000..4b79a0f07e
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mouse Absolute Pointer Driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMouseAbsolutePointer.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMouseAbsolutePointerComponentName = {
+  UsbMouseAbsolutePointerComponentNameGetDriverName,
+  UsbMouseAbsolutePointerComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMouseAbsolutePointerComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMouseAbsolutePointerComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMouseAbsolutePointerComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbMouseAbsolutePointerDriverNameTable[] = {
+  { "eng;en", L"Usb Mouse Absolute Pointer Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMouseAbsolutePointerDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMouseAbsolutePointerComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                       Status;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointerDev;
+  EFI_ABSOLUTE_POINTER_PROTOCOL    *AbsolutePointerProtocol;
+  EFI_USB_IO_PROTOCOL              *UsbIoProtocol;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                 &gEfiAbsolutePointerProtocolGuid,
+                  (VOID **) &AbsolutePointerProtocol,
+                  gUsbMouseAbsolutePointerDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbMouseAbsolutePointerDev = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (AbsolutePointerProtocol);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbMouseAbsolutePointerDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbMouseAbsolutePointerComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c
new file mode 100644
index 0000000000..92ce18f425
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/MouseHid.c
@@ -0,0 +1,281 @@
+/** @file
+  Helper functions to parse HID report descriptor and items.
+
+Copyright (c) 2004 - 2010, 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 "UsbMouseAbsolutePointer.h"
+
+
+/**
+  Get next HID item from report descriptor.
+
+  This function retrieves next HID item from report descriptor, according to
+  the start position.
+  According to USB HID Specification, An item is piece of information
+  about the device. All items have a one-byte prefix that contains
+  the item tag, item type, and item size.
+  There are two basic types of items: short items and long items.
+  If the item is a short item, its optional data size may be 0, 1, 2, or 4 bytes.
+  Only short item is supported here.
+
+  @param  StartPos          Start position of the HID item to get.
+  @param  EndPos            End position of the range to get the the next HID item.
+  @param  HidItem           Buffer for the HID Item to return.
+
+  @return Pointer to end of the HID item returned.
+          NULL if no HID item retrieved.
+
+**/
+UINT8 *
+GetNextHidItem (
+  IN  UINT8    *StartPos,
+  IN  UINT8    *EndPos,
+  OUT HID_ITEM *HidItem
+  )
+{
+  UINT8 Temp;
+
+  if (EndPos <= StartPos) {
+    return NULL;
+  }
+
+  Temp = *StartPos;
+  StartPos++;
+
+  //
+  // Bit format of prefix byte:
+  // Bits 0-1: Size
+  // Bits 2-3: Type
+  // Bits 4-7: Tag
+  //
+  HidItem->Type = BitFieldRead8 (Temp, 2, 3);
+  HidItem->Tag  = BitFieldRead8 (Temp, 4, 7);
+
+  if (HidItem->Tag == HID_ITEM_TAG_LONG) {
+    //
+    // Long Items are not supported, although we try to parse it.
+    //
+    HidItem->Format = HID_ITEM_FORMAT_LONG;
+
+    if ((EndPos - StartPos) >= 2) {
+      HidItem->Size = *StartPos++;
+      HidItem->Tag  = *StartPos++;
+
+      if ((EndPos - StartPos) >= HidItem->Size) {
+        HidItem->Data.LongData = StartPos;
+        StartPos += HidItem->Size;
+        return StartPos;
+      }
+    }
+  } else {
+    HidItem->Format = HID_ITEM_FORMAT_SHORT;
+    HidItem->Size   = BitFieldRead8 (Temp, 0, 1);
+
+    switch (HidItem->Size) {
+    case 0:
+      //
+      // No data
+      //
+      return StartPos;
+
+    case 1:
+      //
+      // 1-byte data
+      //
+      if ((EndPos - StartPos) >= 1) {
+        HidItem->Data.Uint8 = *StartPos++;
+        return StartPos;
+      }
+
+    case 2:
+      //
+      // 2-byte data
+      //
+      if ((EndPos - StartPos) >= 2) {
+        CopyMem (&HidItem->Data.Uint16, StartPos, sizeof (UINT16));
+        StartPos += 2;
+        return StartPos;
+      }
+
+    case 3:
+      //
+      // 4-byte data, adjust size
+      //
+      HidItem->Size = 4;
+      if ((EndPos - StartPos) >= 4) {
+        CopyMem (&HidItem->Data.Uint32, StartPos, sizeof (UINT32));
+        StartPos += 4;
+        return StartPos;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get data from HID item.
+
+  This function retrieves data from HID item.
+  It only supports short items, which has 4 types of data:
+  0, 1, 2, or 4 bytes.
+
+  @param  HidItem       Pointer to the HID item.
+
+  @return The data of HID item.
+
+**/
+UINT32
+GetItemData (
+  IN  HID_ITEM *HidItem
+  )
+{
+  //
+  // Get data from HID item.
+  //
+  switch (HidItem->Size) {
+  case 1:
+    return HidItem->Data.Uint8;
+  case 2:
+    return HidItem->Data.Uint16;
+  case 4:
+    return HidItem->Data.Uint32;
+  }
+  return 0;
+}
+
+/**
+  Parse HID item from report descriptor.
+
+  There are three item types: Main, Global, and Local.
+  This function parses these types of HID items according
+  to tag info.
+
+  @param  UsbMouse          The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  HidItem           The HID item to parse
+
+**/
+VOID
+ParseHidItem (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouse,
+  IN  HID_ITEM                         *HidItem
+  )
+{
+  UINT8  Data;
+
+  switch (HidItem->Type) {
+
+  case HID_ITEM_TYPE_MAIN:
+    //
+    // we don't care any main items, just skip
+    //
+    return ;
+
+  case HID_ITEM_TYPE_GLOBAL:
+    //
+    // For global items, we only care Usage Page tag for Button Page here
+    //
+    if (HidItem->Tag == HID_GLOBAL_ITEM_TAG_USAGE_PAGE) {
+      Data = (UINT8) GetItemData (HidItem);
+      if (Data == 0x09) {
+        //
+        // Button Page
+        //
+        UsbMouse->PrivateData.ButtonDetected = TRUE;
+      }
+  }
+    return;
+
+  case HID_ITEM_TYPE_LOCAL:
+    if (HidItem->Size == 0) {
+    //
+      // No expected data for local item
+    //
+    return ;
+    }
+
+    Data = (UINT8) GetItemData (HidItem);
+
+    switch (HidItem->Tag) {
+    case HID_LOCAL_ITEM_TAG_USAGE_MINIMUM:
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMinIndex = Data;
+    }
+    return ;
+
+    case HID_LOCAL_ITEM_TAG_USAGE_MAXIMUM:
+    {
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMaxIndex = Data;
+      }
+      return ;
+    }
+
+    default:
+        return ;
+      }
+  }
+}
+
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_ABSOLUTE_POINTER_DEV.
+
+  @param  UsbMouseAbsolutePointer  The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  ReportDescriptor         Report descriptor to parse
+  @param  ReportSize               Report descriptor size
+
+  @retval EFI_SUCCESS              Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED          Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointer,
+  IN  UINT8                            *ReportDescriptor,
+  IN  UINTN                            ReportSize
+  )
+{
+  UINT8     *DescriptorEnd;
+  UINT8     *Ptr;
+  HID_ITEM  HidItem;
+
+  DescriptorEnd = ReportDescriptor + ReportSize;
+
+  Ptr = GetNextHidItem (ReportDescriptor, DescriptorEnd, &HidItem);
+  while (Ptr != NULL) {
+    if (HidItem.Format != HID_ITEM_FORMAT_SHORT) {
+      //
+      // Long Item is not supported at current HID revision
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    ParseHidItem (UsbMouseAbsolutePointer, &HidItem);
+
+    Ptr = GetNextHidItem (Ptr, DescriptorEnd, &HidItem);
+  }
+
+  UsbMouseAbsolutePointer->NumberOfButtons = (UINT8) (UsbMouseAbsolutePointer->PrivateData.ButtonMaxIndex - UsbMouseAbsolutePointer->PrivateData.ButtonMinIndex + 1);
+  UsbMouseAbsolutePointer->XLogicMax = 1023;
+  UsbMouseAbsolutePointer->YLogicMax = 1023;
+  UsbMouseAbsolutePointer->XLogicMin = -1023;
+  UsbMouseAbsolutePointer->YLogicMin = -1023;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c
new file mode 100644
index 0000000000..42e696fcc9
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.c
@@ -0,0 +1,995 @@
+/** @file
+  USB Mouse Driver that manages USB mouse and produces Absolute Pointer Protocol.
+
+Copyright (c) 2004 - 2012, 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 "UsbMouseAbsolutePointer.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gUsbMouseAbsolutePointerDriverBinding = {
+  USBMouseAbsolutePointerDriverBindingSupported,
+  USBMouseAbsolutePointerDriverBindingStart,
+  USBMouseAbsolutePointerDriverBindingStop,
+  0x1,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Mouse Absolute Pointer Driver.
+
+  This function is the entrypoint of USB Mouse Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbMouseAbsolutePointerDriverBinding,
+             ImageHandle,
+             &gUsbMouseAbsolutePointerComponentName,
+             &gUsbMouseAbsolutePointerComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether USB Mouse Absolute Pointer Driver supports this device.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a mouse device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+  if (!IsUsbMouse (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+  
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiUsbIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+        
+  return Status;
+}
+
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Absolute Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_USB_IO_PROTOCOL            *UsbIo;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *UsbMouseAbsolutePointerDevice;
+  UINT8                          EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR    EndpointDescriptor;
+  UINT8                          Index;
+  UINT8                          EndpointAddr;
+  UINT8                          PollingInterval;
+  UINT8                          PacketSize;
+  BOOLEAN                        Found;
+  EFI_TPL                        OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER                  
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+  
+  UsbMouseAbsolutePointerDevice = AllocateZeroPool (sizeof (USB_MOUSE_ABSOLUTE_POINTER_DEV));
+  ASSERT (UsbMouseAbsolutePointerDevice != NULL);
+
+  UsbMouseAbsolutePointerDevice->UsbIo     = UsbIo;
+  UsbMouseAbsolutePointerDevice->Signature = USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE;
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbMouseAbsolutePointerDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse will be detected next.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_PRESENCE_DETECT),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbMouseAbsolutePointerDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbMouseAbsolutePointerDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem (&UsbMouseAbsolutePointerDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB mouse
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse has be detected.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DETECTED),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  Status = InitializeUsbMouseDevice (UsbMouseAbsolutePointerDevice);
+  if (EFI_ERROR (Status)) {
+    //
+    // Fail to initialize USB mouse device.
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INTERFACE_ERROR),
+      UsbMouseAbsolutePointerDevice->DevicePath
+      );
+
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize and install EFI Absolute Pointer Protocol.
+  //
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.GetState = GetMouseAbsolutePointerState;
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.Reset	  = UsbMouseAbsolutePointerReset;
+  UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.Mode	  = &UsbMouseAbsolutePointerDevice->Mode;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  UsbMouseAbsolutePointerWaitForInput,
+                  UsbMouseAbsolutePointerDevice,
+                  &((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // The next step would be submitting Asynchronous Interrupt Transfer on this mouse device.
+  // After that we will be able to get key data from it. Thus this is deemed as
+  // the enable action of the mouse, so report status code accordingly.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_ENABLE),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    OnMouseInterruptComplete,
+                    UsbMouseAbsolutePointerDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If submit error, uninstall that interface
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiAbsolutePointerProtocolGuid,
+           &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+           );
+    goto ErrorExit;
+  }
+
+  UsbMouseAbsolutePointerDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbMouseAbsolutePointerComponentName.SupportedLanguages,
+    &UsbMouseAbsolutePointerDevice->ControllerNameTable,
+    L"Generic Usb Mouse Absolute Pointer",
+      TRUE
+      );
+  AddUnicodeString2 (
+    "en",
+    gUsbMouseAbsolutePointerComponentName2.SupportedLanguages,
+    &UsbMouseAbsolutePointerDevice->ControllerNameTable,
+    L"Generic Usb Mouse Absolute Pointer",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    if (UsbMouseAbsolutePointerDevice != NULL) {
+      if ((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput != NULL) {
+        gBS->CloseEvent ((UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol).WaitForInput);
+      }
+
+      FreePool (UsbMouseAbsolutePointerDevice);
+      UsbMouseAbsolutePointerDevice = NULL;
+    }
+  }
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Absolute Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                      Status;
+  USB_MOUSE_ABSOLUTE_POINTER_DEV  *UsbMouseAbsolutePointerDevice;
+  EFI_ABSOLUTE_POINTER_PROTOCOL   *AbsolutePointerProtocol;
+  EFI_USB_IO_PROTOCOL             *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  (VOID **) &AbsolutePointerProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbMouseAbsolutePointerDevice = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (AbsolutePointerProtocol);
+
+  UsbIo = UsbMouseAbsolutePointerDevice->UsbIo;
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DISABLE),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress,
+           FALSE,
+           UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.Interval,
+           0,
+           NULL,
+           NULL
+           );
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiAbsolutePointerProtocolGuid,
+                  &UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbMouseAbsolutePointerDevice->AbsolutePointerProtocol.WaitForInput);
+  
+  if (UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent);
+    UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  if (UsbMouseAbsolutePointerDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbMouseAbsolutePointerDevice->ControllerNameTable);
+  }
+
+  FreePool (UsbMouseAbsolutePointerDevice);
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if ((InterfaceDescriptor.InterfaceClass == CLASS_HID) &&
+      (InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT) &&
+      (InterfaceDescriptor.InterfaceProtocol == PROTOCOL_MOUSE)
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_ABSOLUTE_POINTER_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseAbsolutePointerDev   Device instance to be initialized.
+
+  @retval EFI_SUCCESS                  USB mouse device successfully initialized.
+  @retval EFI_UNSUPPORTED              HID descriptor type is not report descriptor.
+  @retval Other                        USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV           *UsbMouseAbsolutePointerDev
+  )
+{
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT8                     Protocol;
+  EFI_STATUS                Status;
+  EFI_USB_HID_DESCRIPTOR    *MouseHidDesc;
+  UINT8                     *ReportDesc;
+  EFI_USB_CONFIG_DESCRIPTOR ConfigDesc;
+  VOID                      *Buf;
+  UINT32                    TransferResult;
+  UINT16                    Total;
+  USB_DESC_HEAD             *Head;
+  BOOLEAN                   Start;
+
+  UsbIo = UsbMouseAbsolutePointerDev->UsbIo;
+
+  //
+  // Get the current configuration descriptor. Note that it doesn't include other descriptors.
+  //
+  Status = UsbIo->UsbGetConfigDescriptor (
+                    UsbIo,
+                    &ConfigDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // By issuing Get_Descriptor(Configuration) request with total length, we get the Configuration descriptor,
+  // all Interface descriptors, all Endpoint descriptors, and the HID descriptor for each interface.
+  //
+  Buf = AllocateZeroPool (ConfigDesc.TotalLength);
+  if (Buf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = UsbGetDescriptor (
+             UsbIo,
+             (UINT16)((USB_DESC_TYPE_CONFIG << 8) | (ConfigDesc.ConfigurationValue - 1)),
+             0,
+             ConfigDesc.TotalLength,
+             Buf,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return Status;
+  }
+
+  Total = 0;
+  Start = FALSE;
+  Head  = (USB_DESC_HEAD *)Buf;  
+  MouseHidDesc = NULL;
+
+  //
+  // Get HID descriptor from the receipt of Get_Descriptor(Configuration) request.
+  // This algorithm is based on the fact that the HID descriptor shall be interleaved
+  // between the interface and endpoint descriptors for HID interfaces.
+  //
+  while (Total < ConfigDesc.TotalLength) {
+    if (Head->Type == USB_DESC_TYPE_INTERFACE) {
+      if ((((USB_INTERFACE_DESCRIPTOR *)Head)->InterfaceNumber == UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber) &&
+        (((USB_INTERFACE_DESCRIPTOR *)Head)->AlternateSetting == UsbMouseAbsolutePointerDev->InterfaceDescriptor.AlternateSetting)) {
+        Start = TRUE;
+      }
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_ENDPOINT)) {
+      break;
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_HID)) {
+      MouseHidDesc = (EFI_USB_HID_DESCRIPTOR *)Head;
+      break;
+    }
+    Total = Total + (UINT16)Head->Len;
+    Head  = (USB_DESC_HEAD*)((UINT8 *)Buf + Total);
+  }
+
+  if (MouseHidDesc == NULL) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get report descriptor
+  //
+  if (MouseHidDesc->HidClassDesc[0].DescriptorType != USB_DESC_TYPE_REPORT) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  ReportDesc = AllocateZeroPool (MouseHidDesc->HidClassDesc[0].DescriptorLength);
+  ASSERT (ReportDesc != NULL);
+
+  Status = UsbGetReportDescriptor (
+             UsbIo,
+             UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength,
+             ReportDesc
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Parse report descriptor
+  //
+  Status = ParseMouseReportDescriptor (
+             UsbMouseAbsolutePointerDev,
+             ReportDesc,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxX = 1024;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxY = 1024;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMaxZ = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinX = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinY = 0;
+  UsbMouseAbsolutePointerDev->Mode.AbsoluteMinZ = 0;
+  UsbMouseAbsolutePointerDev->Mode.Attributes   = 0x3;
+  
+  //
+  // Set boot protocol for the USB mouse.
+  // This driver only supports boot protocol.
+  //
+  UsbGetProtocolRequest (
+    UsbIo,
+    UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  if (Protocol != BOOT_PROTOCOL) {
+    Status = UsbSetProtocolRequest (
+               UsbIo,
+               UsbMouseAbsolutePointerDev->InterfaceDescriptor.InterfaceNumber,
+               BOOT_PROTOCOL
+               );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (Buf);
+      FreePool (ReportDesc);
+      return Status;
+    }
+  }
+
+  FreePool (Buf);
+  FreePool (ReportDesc);
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbMouseAbsolutePointerDev->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseAbsolutePointerDev->DelayedRecoveryEvent);
+    UsbMouseAbsolutePointerDev->DelayedRecoveryEvent = 0;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBMouseRecoveryHandler,
+         UsbMouseAbsolutePointerDev,
+         &UsbMouseAbsolutePointerDev->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointerDevice;
+  EFI_USB_IO_PROTOCOL              *UsbIo;
+  UINT8                            EndpointAddr;
+  UINT32                           UsbResult;
+
+  UsbMouseAbsolutePointerDevice  = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+  UsbIo                          = UsbMouseAbsolutePointerDevice->UsbIo;
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INPUT_ERROR),
+      UsbMouseAbsolutePointerDevice->DevicePath
+      );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      EndpointAddr = UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress;
+
+      UsbClearEndpointHalt (
+        UsbIo,
+        EndpointAddr,
+        &UsbResult
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt. 
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbMouseAbsolutePointerDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbMouseAbsolutePointerDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  UsbMouseAbsolutePointerDevice->StateChanged = TRUE;
+
+  //
+  // Check mouse Data
+  // USB HID Specification specifies following data format:
+  // Byte    Bits    Description
+  // 0       0       Button 1
+  //         1       Button 2
+  //         2       Button 3
+  //         4 to 7  Device-specific
+  // 1       0 to 7  X displacement
+  // 2       0 to 7  Y displacement
+  // 3 to n  0 to 7  Device specific (optional)
+  //
+  UsbMouseAbsolutePointerDevice->State.CurrentX += *((INT8 *) Data + 1);
+  UsbMouseAbsolutePointerDevice->State.CurrentY += *((INT8 *) Data + 2);
+  
+  if (DataLength > 3) {
+    UsbMouseAbsolutePointerDevice->State.CurrentZ += *((INT8 *) Data + 3);
+  }
+  UsbMouseAbsolutePointerDevice->State.ActiveButtons = *(UINT8 *) Data & (BIT0 | BIT1);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER State is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseAbsolutePointerState (
+  IN   EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  OUT  EFI_ABSOLUTE_POINTER_STATE     *State
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *MouseAbsolutePointerDev;
+
+  if (State == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MouseAbsolutePointerDev = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  if (!MouseAbsolutePointerDev->StateChanged) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Retrieve mouse state from USB_MOUSE_ABSOLUTE_POINTER_DEV,
+  // which was filled by OnMouseInterruptComplete()
+  //
+  CopyMem (
+    State,
+    &MouseAbsolutePointerDev->State,
+    sizeof (EFI_ABSOLUTE_POINTER_STATE)
+    );
+
+  //
+  // Clear previous move state
+  //
+  MouseAbsolutePointerDev->State.CurrentX      = 0;
+  MouseAbsolutePointerDev->State.CurrentY      = 0;
+  MouseAbsolutePointerDev->State.CurrentZ      = 0;
+  MouseAbsolutePointerDev->State.ActiveButtons = 0;
+
+  MouseAbsolutePointerDev->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  IN BOOLEAN                        ExtendedVerification
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV       *UsbMouseAbsolutePointerDevice;
+
+  UsbMouseAbsolutePointerDevice  = USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_RESET),
+    UsbMouseAbsolutePointerDevice->DevicePath
+    );
+
+  //
+  // Clear mouse state.
+  //
+  ZeroMem (
+    &UsbMouseAbsolutePointerDevice->State,
+    sizeof (EFI_ABSOLUTE_POINTER_STATE)
+    );
+  UsbMouseAbsolutePointerDevice->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Event notification function for EFI_ABSOLUTE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+UsbMouseAbsolutePointerWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV *UsbMouseAbsolutePointerDev;
+
+  UsbMouseAbsolutePointerDev = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+
+  //
+  // If there's input from mouse, signal the event.
+  //
+  if (UsbMouseAbsolutePointerDev->StateChanged) {
+    gBS->SignalEvent (Event);
+  }
+}
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event                 The Delayed Recovery event.
+  @param  Context               Points to the USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_MOUSE_ABSOLUTE_POINTER_DEV       *UsbMouseAbsolutePointerDev;
+  EFI_USB_IO_PROTOCOL                  *UsbIo;
+
+  UsbMouseAbsolutePointerDev = (USB_MOUSE_ABSOLUTE_POINTER_DEV *) Context;
+
+  UsbIo       = UsbMouseAbsolutePointerDev->UsbIo;
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.Interval,
+           UsbMouseAbsolutePointerDev->IntEndpointDescriptor.MaxPacketSize,
+           OnMouseInterruptComplete,
+           UsbMouseAbsolutePointerDev
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h
new file mode 100644
index 0000000000..08b8d56dd8
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointer.h
@@ -0,0 +1,471 @@
+/** @file
+  Helper routine and corresponding data struct used by USB Mouse Absolute Pointer Driver.
+
+Copyright (c) 2004 - 2012, 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.
+
+**/
+
+#ifndef _USB_MOUSE_ABSOLUTE_POINTER_H_
+#define _USB_MOUSE_ABSOLUTE_POINTER_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/AbsolutePointer.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define CLASS_HID               3
+#define SUBCLASS_BOOT           1
+#define PROTOCOL_MOUSE          2
+
+#define BOOT_PROTOCOL           0
+#define REPORT_PROTOCOL         1
+
+#define USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE SIGNATURE_32 ('u', 'm', 's', 't')
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+///
+/// Button range and status
+///
+typedef struct {
+  BOOLEAN ButtonDetected;
+  UINT8   ButtonMinIndex;
+  UINT8   ButtonMaxIndex;
+  UINT8   Reserved;
+} USB_MOUSE_BUTTON_DATA;
+
+///
+/// Device instance of USB mouse.
+///
+typedef struct {
+  UINTN                         Signature;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  EFI_EVENT                     DelayedRecoveryEvent;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR   IntEndpointDescriptor;
+  UINT8                         NumberOfButtons;
+  INT32                         XLogicMax;
+  INT32                         XLogicMin;
+  INT32                         YLogicMax;
+  INT32                         YLogicMin;
+  EFI_ABSOLUTE_POINTER_PROTOCOL AbsolutePointerProtocol;
+  EFI_ABSOLUTE_POINTER_STATE    State;
+  EFI_ABSOLUTE_POINTER_MODE     Mode;
+  BOOLEAN                       StateChanged;
+  USB_MOUSE_BUTTON_DATA         PrivateData;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+} USB_MOUSE_ABSOLUTE_POINTER_DEV;
+
+///
+/// General HID Item structure
+///
+
+typedef union {
+  UINT8   Uint8;
+  UINT16  Uint16;
+  UINT32  Uint32;
+  INT8    Int8;
+  INT16   Int16;
+  INT32   Int32;
+  UINT8   *LongData;
+} HID_DATA;
+
+typedef struct {
+  UINT16    Format;
+  UINT8     Size;
+  UINT8     Type;
+  UINT8     Tag;
+  HID_DATA  Data;
+} HID_ITEM;
+
+#define USB_MOUSE_ABSOLUTE_POINTER_DEV_FROM_MOUSE_PROTOCOL(a) \
+    CR(a, USB_MOUSE_ABSOLUTE_POINTER_DEV, AbsolutePointerProtocol, USB_MOUSE_ABSOLUTE_POINTER_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbMouseAbsolutePointerDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMouseAbsolutePointerComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMouseAbsolutePointerComponentName2;
+
+//
+// Functions of Driver Binding Protocol
+//
+
+/**
+  Check whether USB Mouse Absolute Pointer Driver supports this device.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Absolute Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Absolute Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of EFI_ABSOLUTE_POINTER_PROTOCOL
+//
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER State is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseAbsolutePointerState (
+  IN   EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  OUT  EFI_ABSOLUTE_POINTER_STATE     *State
+  );
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                  A pointer to the EFI_ABSOLUTE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL  *This,
+  IN BOOLEAN                        ExtendedVerification
+  );
+
+/**
+  Event notification function for EFI_ABSOLUTE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+UsbMouseAbsolutePointerWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+//
+// Internal worker functions
+//
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  );
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_ABSOLUTE_POINTER_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseAbsolutePointerDev   Device instance to be initialized.
+
+  @retval EFI_SUCCESS                  USB mouse device successfully initialized.
+  @retval EFI_UNSUPPORTED              HID descriptor type is not report descriptor.
+  @retval Other                        USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN  USB_MOUSE_ABSOLUTE_POINTER_DEV           *UsbMouseAbsolutePointerDev
+  );
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event                 The Delayed Recovery event.
+  @param  Context               Points to the USB_MOUSE_ABSOLUTE_POINTER_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_ABSOLUTE_POINTER_DEV.
+
+  @param  UsbMouseAbsolutePointer  The instance of USB_MOUSE_ABSOLUTE_POINTER_DEV
+  @param  ReportDescriptor         Report descriptor to parse
+  @param  ReportSize               Report descriptor size
+
+  @retval EFI_SUCCESS              Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED          Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_ABSOLUTE_POINTER_DEV   *UsbMouseAbsolutePointer,
+  IN  UINT8                            *ReportDescriptor,
+  IN  UINTN                            ReportSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf
new file mode 100644
index 0000000000..af10382d52
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf
@@ -0,0 +1,72 @@
+## @file
+# USB Mouse Driver that manages USB mouse and produces Absolute Pointer Protocol.
+#
+# USB Mouse Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Absolute Pointer Protocol on USB mouse devices.
+# It manages the USB mouse device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID Specification.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMouseAbsolutePointerDxe
+  MODULE_UNI_FILE                = UsbMouseAbsolutePointerDxe.uni
+  FILE_GUID                      = 4EA43463-747C-46eb-97FB-B0E5C5F05306
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMouseAbsolutePointerDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUsbMouseAbsolutePointerDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMouseAbsolutePointerComponentName
+#  COMPONENT_NAME2               =  gUsbMouseAbsolutePointerComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  MouseHid.c
+  UsbMouseAbsolutePointer.c
+  UsbMouseAbsolutePointer.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  ReportStatusCodeLib
+  UefiUsbLib
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiAbsolutePointerProtocolGuid               ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMouseAbsolutePointerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni
new file mode 100644
index 0000000000..628d6933c0
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni
new file mode 100644
index 0000000000..763dcaed03
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c
new file mode 100644
index 0000000000..a7df52221c
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/ComponentName.c
@@ -0,0 +1,224 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for USB Mouse driver.
+
+Copyright (c) 2004 - 2011, 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 "UsbMouse.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUsbMouseComponentName = {
+  UsbMouseComponentNameGetDriverName,
+  UsbMouseComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUsbMouseComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UsbMouseComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UsbMouseComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUsbMouseDriverNameTable[] = {
+  { "eng;en", L"Usb Mouse Driver" },
+  { NULL , NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUsbMouseDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUsbMouseComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                  Status;
+  USB_MOUSE_DEV               *UsbMouseDev;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointerProtocol;
+  EFI_USB_IO_PROTOCOL         *UsbIoProtocol;
+
+  //
+  // This is a device driver, so ChildHandle must be NULL.
+  //
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIoProtocol,
+                  gUsbMouseDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiUsbIoProtocolGuid,
+           gUsbMouseDriverBinding.DriverBindingHandle,
+           ControllerHandle
+           );
+
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Get the device context
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                 &gEfiSimplePointerProtocolGuid,
+                  (VOID **) &SimplePointerProtocol,
+                  gUsbMouseDriverBinding.DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UsbMouseDev = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (SimplePointerProtocol);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           UsbMouseDev->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUsbMouseComponentName)
+           );
+
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c
new file mode 100644
index 0000000000..ec6a420c14
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/MouseHid.c
@@ -0,0 +1,281 @@
+/** @file
+  Helper functions to parse HID report descriptor and items.
+
+Copyright (c) 2004 - 2010, 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 "UsbMouse.h"
+
+
+/**
+  Get next HID item from report descriptor.
+
+  This function retrieves next HID item from report descriptor, according to
+  the start position.
+  According to USB HID Specification, An item is piece of information
+  about the device. All items have a one-byte prefix that contains
+  the item tag, item type, and item size.
+  There are two basic types of items: short items and long items.
+  If the item is a short item, its optional data size may be 0, 1, 2, or 4 bytes.
+  Only short item is supported here.
+
+  @param  StartPos          Start position of the HID item to get.
+  @param  EndPos            End position of the range to get the the next HID item.
+  @param  HidItem           Buffer for the HID Item to return.
+
+  @return Pointer to end of the HID item returned.
+          NULL if no HID item retrieved.
+
+**/
+UINT8 *
+GetNextHidItem (
+  IN  UINT8    *StartPos,
+  IN  UINT8    *EndPos,
+  OUT HID_ITEM *HidItem
+  )
+{
+  UINT8 Temp;
+
+  if (EndPos <= StartPos) {
+    return NULL;
+  }
+
+  Temp = *StartPos;
+  StartPos++;
+
+  //
+  // Bit format of prefix byte:
+  // Bits 0-1: Size
+  // Bits 2-3: Type
+  // Bits 4-7: Tag
+  //
+  HidItem->Type = BitFieldRead8 (Temp, 2, 3);
+  HidItem->Tag  = BitFieldRead8 (Temp, 4, 7);
+
+  if (HidItem->Tag == HID_ITEM_TAG_LONG) {
+    //
+    // Long Items are not supported, although we try to parse it.
+    //
+    HidItem->Format = HID_ITEM_FORMAT_LONG;
+
+    if ((EndPos - StartPos) >= 2) {
+      HidItem->Size = *StartPos++;
+      HidItem->Tag  = *StartPos++;
+
+      if ((EndPos - StartPos) >= HidItem->Size) {
+        HidItem->Data.LongData = StartPos;
+        StartPos += HidItem->Size;
+        return StartPos;
+      }
+    }
+  } else {
+    HidItem->Format = HID_ITEM_FORMAT_SHORT;
+    HidItem->Size   = BitFieldRead8 (Temp, 0, 1);
+
+    switch (HidItem->Size) {
+    case 0:
+      //
+      // No data
+      //
+      return StartPos;
+
+    case 1:
+      //
+      // 1-byte data
+      //
+      if ((EndPos - StartPos) >= 1) {
+        HidItem->Data.Uint8 = *StartPos++;
+        return StartPos;
+      }
+
+    case 2:
+      //
+      // 2-byte data
+      //
+      if ((EndPos - StartPos) >= 2) {
+        CopyMem (&HidItem->Data.Uint16, StartPos, sizeof (UINT16));
+        StartPos += 2;
+        return StartPos;
+      }
+
+    case 3:
+      //
+      // 4-byte data, adjust size
+      //
+      HidItem->Size = 4;
+      if ((EndPos - StartPos) >= 4) {
+        CopyMem (&HidItem->Data.Uint32, StartPos, sizeof (UINT32));
+        StartPos += 4;
+        return StartPos;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get data from HID item.
+
+  This function retrieves data from HID item.
+  It only supports short items, which has 4 types of data:
+  0, 1, 2, or 4 bytes.
+
+  @param  HidItem       Pointer to the HID item.
+
+  @return The data of HID item.
+
+**/
+UINT32
+GetItemData (
+  IN  HID_ITEM *HidItem
+  )
+{
+  //
+  // Get data from HID item.
+  //
+  switch (HidItem->Size) {
+  case 1:
+    return HidItem->Data.Uint8;
+  case 2:
+    return HidItem->Data.Uint16;
+  case 4:
+    return HidItem->Data.Uint32;
+  }
+  return 0;
+}
+
+/**
+  Parse HID item from report descriptor.
+
+  There are three item types: Main, Global, and Local.
+  This function parses these types of HID items according
+  to tag info.
+  
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  HidItem           The HID item to parse
+
+**/
+VOID
+ParseHidItem (
+  IN  USB_MOUSE_DEV   *UsbMouse,
+  IN  HID_ITEM        *HidItem
+  )
+{
+  UINT8  Data;
+
+  switch (HidItem->Type) {
+
+  case HID_ITEM_TYPE_MAIN:
+    //
+    // we don't care any main items, just skip
+    //
+    return;
+
+  case HID_ITEM_TYPE_GLOBAL:
+    //
+    // For global items, we only care Usage Page tag for Button Page here
+    //
+    if (HidItem->Tag == HID_GLOBAL_ITEM_TAG_USAGE_PAGE) {
+      Data = (UINT8) GetItemData (HidItem);
+      if (Data == 0x09) {
+        //
+        // Button Page
+        //
+        UsbMouse->PrivateData.ButtonDetected = TRUE;
+      }
+    }
+    return;
+
+  case HID_ITEM_TYPE_LOCAL:
+    if (HidItem->Size == 0) {
+      //
+      // No expected data for local item
+      //
+      return ;
+    }
+
+    Data = (UINT8) GetItemData (HidItem);
+
+    switch (HidItem->Tag) {
+    case HID_LOCAL_ITEM_TAG_USAGE_MINIMUM:
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMinIndex = Data;
+      }
+      return ;
+
+    case HID_LOCAL_ITEM_TAG_USAGE_MAXIMUM:
+    {
+      if (UsbMouse->PrivateData.ButtonDetected) {
+        UsbMouse->PrivateData.ButtonMaxIndex = Data;
+      }
+      return ;
+    }
+
+    default:
+      return;
+    }
+  }
+}
+
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_DEV.
+
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  ReportDescriptor  Report descriptor to parse
+  @param  ReportSize        Report descriptor size
+
+  @retval EFI_SUCCESS       Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED   Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_DEV   *UsbMouse,
+  IN  UINT8           *ReportDescriptor,
+  IN  UINTN           ReportSize
+  )
+{
+  UINT8     *DescriptorEnd;
+  UINT8     *Ptr;
+  HID_ITEM  HidItem;
+
+  DescriptorEnd = ReportDescriptor + ReportSize;
+
+  Ptr = GetNextHidItem (ReportDescriptor, DescriptorEnd, &HidItem);
+  while (Ptr != NULL) {
+    if (HidItem.Format != HID_ITEM_FORMAT_SHORT) {
+      //
+      // Long Item is not supported at current HID revision
+      //
+      return EFI_UNSUPPORTED;
+    }
+
+    ParseHidItem (UsbMouse, &HidItem);
+
+    Ptr = GetNextHidItem (Ptr, DescriptorEnd, &HidItem);
+  }
+
+  UsbMouse->NumberOfButtons = (UINT8) (UsbMouse->PrivateData.ButtonMaxIndex - UsbMouse->PrivateData.ButtonMinIndex + 1);
+  UsbMouse->XLogicMax = 127;
+  UsbMouse->YLogicMax = 127;
+  UsbMouse->XLogicMin = -127;
+  UsbMouse->YLogicMin = -127;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c
new file mode 100644
index 0000000000..0ad5616d96
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.c
@@ -0,0 +1,1000 @@
+/** @file
+  USB Mouse Driver that manages USB mouse and produces Simple Pointer Protocol.
+
+Copyright (c) 2004 - 2012, 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 "UsbMouse.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gUsbMouseDriverBinding = {
+  USBMouseDriverBindingSupported,
+  USBMouseDriverBindingStart,
+  USBMouseDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of USB Mouse Driver.
+
+  This function is the entrypoint of USB Mouse Driver. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUsbMouseDriverBinding,
+             ImageHandle,
+             &gUsbMouseComponentName,
+             &gUsbMouseComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether USB mouse driver supports this device.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS          Status;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Use the USB I/O Protocol interface to check whether Controller is
+  // a mouse device that can be managed by this driver.
+  //
+  Status = EFI_SUCCESS;
+  if (!IsUsbMouse (UsbIo)) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiUsbIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Simple Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The USB mouse driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+  USB_MOUSE_DEV               *UsbMouseDevice;
+  UINT8                       EndpointNumber;
+  EFI_USB_ENDPOINT_DESCRIPTOR EndpointDescriptor;
+  UINT8                       Index;
+  UINT8                       EndpointAddr;
+  UINT8                       PollingInterval;
+  UINT8                       PacketSize;
+  BOOLEAN                     Found;
+  EFI_TPL                     OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  //
+  // Open USB I/O Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+
+  UsbMouseDevice = AllocateZeroPool (sizeof (USB_MOUSE_DEV));
+  ASSERT (UsbMouseDevice != NULL);
+
+  UsbMouseDevice->UsbIo     = UsbIo;
+  UsbMouseDevice->Signature = USB_MOUSE_DEV_SIGNATURE;
+
+  //
+  // Get the Device Path Protocol on Controller's handle
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &UsbMouseDevice->DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse will be detected next.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_PRESENCE_DETECT),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Get interface & endpoint descriptor
+  //
+  UsbIo->UsbGetInterfaceDescriptor (
+           UsbIo,
+           &UsbMouseDevice->InterfaceDescriptor
+           );
+
+  EndpointNumber = UsbMouseDevice->InterfaceDescriptor.NumEndpoints;
+
+  //
+  // Traverse endpoints to find interrupt endpoint
+  //
+  Found = FALSE;
+  for (Index = 0; Index < EndpointNumber; Index++) {
+    UsbIo->UsbGetEndpointDescriptor (
+             UsbIo,
+             Index,
+             &EndpointDescriptor
+             );
+
+    if ((EndpointDescriptor.Attributes & (BIT0 | BIT1)) == USB_ENDPOINT_INTERRUPT) {
+      //
+      // We only care interrupt endpoint here
+      //
+      CopyMem(&UsbMouseDevice->IntEndpointDescriptor, &EndpointDescriptor, sizeof(EndpointDescriptor));
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (!Found) {
+    //
+    // Report Status Code to indicate that there is no USB mouse
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_NOT_DETECTED)
+      );
+    //
+    // No interrupt endpoint found, then return unsupported.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ErrorExit;
+  }
+
+  //
+  // Report Status Code here since USB mouse has be detected.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DETECTED),
+    UsbMouseDevice->DevicePath
+    );
+
+  Status = InitializeUsbMouseDevice (UsbMouseDevice);
+  if (EFI_ERROR (Status)) {
+    //
+    // Fail to initialize USB mouse device.
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INTERFACE_ERROR),
+      UsbMouseDevice->DevicePath
+      );
+
+    goto ErrorExit;
+  }
+
+  //
+  // Initialize and install EFI Simple Pointer Protocol.
+  //
+  UsbMouseDevice->SimplePointerProtocol.GetState  = GetMouseState;
+  UsbMouseDevice->SimplePointerProtocol.Reset     = UsbMouseReset;
+  UsbMouseDevice->SimplePointerProtocol.Mode      = &UsbMouseDevice->Mode;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  UsbMouseWaitForInput,
+                  UsbMouseDevice,
+                  &((UsbMouseDevice->SimplePointerProtocol).WaitForInput)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &UsbMouseDevice->SimplePointerProtocol
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // The next step would be submitting Asynchronous Interrupt Transfer on this mouse device.
+  // After that we will be able to get key data from it. Thus this is deemed as
+  // the enable action of the mouse, so report status code accordingly.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_ENABLE),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Submit Asynchronous Interrupt Transfer to manage this device.
+  //
+  EndpointAddr    = UsbMouseDevice->IntEndpointDescriptor.EndpointAddress;
+  PollingInterval = UsbMouseDevice->IntEndpointDescriptor.Interval;
+  PacketSize      = (UINT8) (UsbMouseDevice->IntEndpointDescriptor.MaxPacketSize);
+
+  Status = UsbIo->UsbAsyncInterruptTransfer (
+                    UsbIo,
+                    EndpointAddr,
+                    TRUE,
+                    PollingInterval,
+                    PacketSize,
+                    OnMouseInterruptComplete,
+                    UsbMouseDevice
+                    );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If submit error, uninstall that interface
+    //
+    gBS->UninstallProtocolInterface (
+           Controller,
+           &gEfiSimplePointerProtocolGuid,
+           &UsbMouseDevice->SimplePointerProtocol
+           );
+    goto ErrorExit;
+  }
+  
+  UsbMouseDevice->ControllerNameTable = NULL;
+  AddUnicodeString2 (
+    "eng",
+    gUsbMouseComponentName.SupportedLanguages,
+    &UsbMouseDevice->ControllerNameTable,
+    L"Generic Usb Mouse",
+    TRUE
+    );
+  AddUnicodeString2 (
+    "en",
+    gUsbMouseComponentName2.SupportedLanguages,
+    &UsbMouseDevice->ControllerNameTable,
+    L"Generic Usb Mouse",
+    FALSE
+    );
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+//
+// Error handler
+//
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUsbIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    if (UsbMouseDevice != NULL) {
+      if ((UsbMouseDevice->SimplePointerProtocol).WaitForInput != NULL) {
+        gBS->CloseEvent ((UsbMouseDevice->SimplePointerProtocol).WaitForInput);
+      }
+
+      FreePool (UsbMouseDevice);
+      UsbMouseDevice = NULL;
+    }
+  }
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  USB_MOUSE_DEV               *UsbMouseDevice;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointerProtocol;
+  EFI_USB_IO_PROTOCOL         *UsbIo;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  (VOID **) &SimplePointerProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  UsbMouseDevice = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (SimplePointerProtocol);
+
+  UsbIo = UsbMouseDevice->UsbIo;
+
+  //
+  // The key data input from this device will be disabled.
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_DISABLE),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Delete the Asynchronous Interrupt Transfer from this device
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseDevice->IntEndpointDescriptor.EndpointAddress,
+           FALSE,
+           UsbMouseDevice->IntEndpointDescriptor.Interval,
+           0,
+           NULL,
+           NULL
+           );
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiSimplePointerProtocolGuid,
+                  &UsbMouseDevice->SimplePointerProtocol
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+         Controller,
+         &gEfiUsbIoProtocolGuid,
+         This->DriverBindingHandle,
+         Controller
+         );
+
+  //
+  // Free all resources.
+  //
+  gBS->CloseEvent (UsbMouseDevice->SimplePointerProtocol.WaitForInput);
+
+  if (UsbMouseDevice->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseDevice->DelayedRecoveryEvent);
+    UsbMouseDevice->DelayedRecoveryEvent = NULL;
+  }
+
+  if (UsbMouseDevice->ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (UsbMouseDevice->ControllerNameTable);
+  }
+
+  FreePool (UsbMouseDevice);
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+
+  //
+  // Get the default interface descriptor
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (
+                    UsbIo,
+                    &InterfaceDescriptor
+                    );
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if ((InterfaceDescriptor.InterfaceClass == CLASS_HID) &&
+      (InterfaceDescriptor.InterfaceSubClass == SUBCLASS_BOOT) &&
+      (InterfaceDescriptor.InterfaceProtocol == PROTOCOL_MOUSE)
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseDev           Device instance to be initialized.
+
+  @retval EFI_SUCCESS           USB mouse device successfully initialized..
+  @retval EFI_UNSUPPORTED       HID descriptor type is not report descriptor.
+  @retval Other                 USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN OUT USB_MOUSE_DEV           *UsbMouseDev
+  )
+{
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINT8                     Protocol;
+  EFI_STATUS                Status;
+  EFI_USB_HID_DESCRIPTOR    *MouseHidDesc;
+  UINT8                     *ReportDesc;
+  EFI_USB_CONFIG_DESCRIPTOR ConfigDesc;
+  VOID                      *Buf;
+  UINT32                    TransferResult;
+  UINT16                    Total;
+  USB_DESC_HEAD             *Head;
+  BOOLEAN                   Start;
+
+  UsbIo = UsbMouseDev->UsbIo;
+
+  //
+  // Get the current configuration descriptor. Note that it doesn't include other descriptors.
+  //
+  Status = UsbIo->UsbGetConfigDescriptor (
+                    UsbIo,
+                    &ConfigDesc
+                    );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // By issuing Get_Descriptor(Configuration) request with total length, we get the Configuration descriptor,
+  // all Interface descriptors, all Endpoint descriptors, and the HID descriptor for each interface.
+  //
+  Buf = AllocateZeroPool (ConfigDesc.TotalLength);
+  if (Buf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = UsbGetDescriptor (
+             UsbIo,
+             (UINT16)((USB_DESC_TYPE_CONFIG << 8) | (ConfigDesc.ConfigurationValue - 1)),
+             0,
+             ConfigDesc.TotalLength,
+             Buf,
+             &TransferResult
+             );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    return Status;
+  }
+
+  Total = 0;
+  Start = FALSE;
+  Head  = (USB_DESC_HEAD *)Buf;  
+  MouseHidDesc = NULL;
+
+  //
+  // Get HID descriptor from the receipt of Get_Descriptor(Configuration) request.
+  // This algorithm is based on the fact that the HID descriptor shall be interleaved
+  // between the interface and endpoint descriptors for HID interfaces.
+  //
+  while (Total < ConfigDesc.TotalLength) {
+    if (Head->Type == USB_DESC_TYPE_INTERFACE) {
+      if ((((USB_INTERFACE_DESCRIPTOR *)Head)->InterfaceNumber == UsbMouseDev->InterfaceDescriptor.InterfaceNumber) &&
+        (((USB_INTERFACE_DESCRIPTOR *)Head)->AlternateSetting == UsbMouseDev->InterfaceDescriptor.AlternateSetting)) {
+        Start = TRUE;
+      }
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_ENDPOINT)) {
+      break;
+    }
+    if (Start && (Head->Type == USB_DESC_TYPE_HID)) {
+      MouseHidDesc = (EFI_USB_HID_DESCRIPTOR *)Head;
+      break;
+    }
+    Total = Total + (UINT16)Head->Len;
+    Head  = (USB_DESC_HEAD*)((UINT8 *)Buf + Total);
+  }
+
+  if (MouseHidDesc == NULL) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get report descriptor
+  //
+  if (MouseHidDesc->HidClassDesc[0].DescriptorType != USB_DESC_TYPE_REPORT) {
+    FreePool (Buf);
+    return EFI_UNSUPPORTED;
+  }
+
+  ReportDesc = AllocateZeroPool (MouseHidDesc->HidClassDesc[0].DescriptorLength);
+  ASSERT (ReportDesc != NULL);
+
+  Status = UsbGetReportDescriptor (
+             UsbIo,
+             UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength,
+             ReportDesc
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Parse report descriptor
+  //
+  Status = ParseMouseReportDescriptor (
+             UsbMouseDev,
+             ReportDesc,
+             MouseHidDesc->HidClassDesc[0].DescriptorLength
+             );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Buf);
+    FreePool (ReportDesc);
+    return Status;
+  }
+
+  //
+  // Check the presence of left and right buttons,
+  // and initialize fields of EFI_SIMPLE_POINTER_MODE.
+  //
+  if (UsbMouseDev->NumberOfButtons >= 1) {
+    UsbMouseDev->Mode.LeftButton = TRUE;
+  }
+  if (UsbMouseDev->NumberOfButtons > 1) {
+    UsbMouseDev->Mode.RightButton = TRUE;
+  }
+  UsbMouseDev->Mode.ResolutionX = 8;
+  UsbMouseDev->Mode.ResolutionY = 8;
+  UsbMouseDev->Mode.ResolutionZ = 0;
+
+  //
+  // Set boot protocol for the USB mouse.
+  // This driver only supports boot protocol.
+  //
+  UsbGetProtocolRequest (
+    UsbIo,
+    UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+    &Protocol
+    );
+  if (Protocol != BOOT_PROTOCOL) {
+    Status = UsbSetProtocolRequest (
+               UsbIo,
+               UsbMouseDev->InterfaceDescriptor.InterfaceNumber,
+               BOOT_PROTOCOL
+               );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (Buf);
+      FreePool (ReportDesc);
+      return Status;
+    }
+  }
+
+  FreePool (Buf);
+  FreePool (ReportDesc);
+
+  //
+  // Create event for delayed recovery, which deals with device error.
+  //
+  if (UsbMouseDev->DelayedRecoveryEvent != NULL) {
+    gBS->CloseEvent (UsbMouseDev->DelayedRecoveryEvent);
+    UsbMouseDev->DelayedRecoveryEvent = 0;
+  }
+
+  gBS->CreateEvent (
+         EVT_TIMER | EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         USBMouseRecoveryHandler,
+         UsbMouseDev,
+         &UsbMouseDev->DelayedRecoveryEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDevice;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+  UINT8               EndpointAddr;
+  UINT32              UsbResult;
+
+  UsbMouseDevice  = (USB_MOUSE_DEV *) Context;
+  UsbIo           = UsbMouseDevice->UsbIo;
+
+  if (Result != EFI_USB_NOERROR) {
+    //
+    // Some errors happen during the process
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_MOUSE | EFI_P_EC_INPUT_ERROR),
+      UsbMouseDevice->DevicePath
+      );
+
+    if ((Result & EFI_USB_ERR_STALL) == EFI_USB_ERR_STALL) {
+      EndpointAddr = UsbMouseDevice->IntEndpointDescriptor.EndpointAddress;
+
+      UsbClearEndpointHalt (
+        UsbIo,
+        EndpointAddr,
+        &UsbResult
+        );
+    }
+
+    //
+    // Delete & Submit this interrupt again
+    // Handler of DelayedRecoveryEvent triggered by timer will re-submit the interrupt. 
+    //
+    UsbIo->UsbAsyncInterruptTransfer (
+             UsbIo,
+             UsbMouseDevice->IntEndpointDescriptor.EndpointAddress,
+             FALSE,
+             0,
+             0,
+             NULL,
+             NULL
+             );
+    //
+    // EFI_USB_INTERRUPT_DELAY is defined in USB standard for error handling.
+    //
+    gBS->SetTimer (
+           UsbMouseDevice->DelayedRecoveryEvent,
+           TimerRelative,
+           EFI_USB_INTERRUPT_DELAY
+           );
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // If no error and no data, just return EFI_SUCCESS.
+  //
+  if (DataLength == 0 || Data == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  UsbMouseDevice->StateChanged = TRUE;
+
+  //
+  // Check mouse Data
+  // USB HID Specification specifies following data format:
+  // Byte    Bits    Description
+  // 0       0       Button 1
+  //         1       Button 2
+  //         2       Button 3
+  //         4 to 7  Device-specific
+  // 1       0 to 7  X displacement
+  // 2       0 to 7  Y displacement
+  // 3 to n  0 to 7  Device specific (optional)
+  //
+  UsbMouseDevice->State.LeftButton  = (BOOLEAN) ((*(UINT8 *) Data & BIT0) != 0);
+  UsbMouseDevice->State.RightButton = (BOOLEAN) ((*(UINT8 *) Data & BIT1) != 0);
+  UsbMouseDevice->State.RelativeMovementX += *((INT8 *) Data + 1);
+  UsbMouseDevice->State.RelativeMovementY += *((INT8 *) Data + 2);
+
+  if (DataLength > 3) {
+    UsbMouseDevice->State.RelativeMovementZ += *((INT8 *) Data + 3);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the current state of a pointer device.
+    
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+                                
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER MouseState is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseState (
+  IN   EFI_SIMPLE_POINTER_PROTOCOL  *This,
+  OUT  EFI_SIMPLE_POINTER_STATE     *MouseState
+  )
+{
+  USB_MOUSE_DEV *MouseDev;
+
+  if (MouseState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MouseDev = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  if (!MouseDev->StateChanged) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Retrieve mouse state from USB_MOUSE_DEV, which was filled by OnMouseInterruptComplete()
+  //
+  CopyMem (
+    MouseState,
+    &MouseDev->State,
+    sizeof (EFI_SIMPLE_POINTER_STATE)
+    );
+
+  //
+  // Clear previous move state
+  //
+  MouseDev->State.RelativeMovementX = 0;
+  MouseDev->State.RelativeMovementY = 0;
+  MouseDev->State.RelativeMovementZ = 0;
+
+  MouseDev->StateChanged            = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+
+/**                                                                 
+  Resets the pointer device hardware.
+  
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+                                
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseReset (
+  IN EFI_SIMPLE_POINTER_PROTOCOL    *This,
+  IN BOOLEAN                        ExtendedVerification
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDevice;
+
+  UsbMouseDevice  = USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL (This);
+
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_MOUSE | EFI_P_PC_RESET),
+    UsbMouseDevice->DevicePath
+    );
+
+  //
+  // Clear mouse state.
+  //
+  ZeroMem (
+    &UsbMouseDevice->State,
+    sizeof (EFI_SIMPLE_POINTER_STATE)
+    );
+  UsbMouseDevice->StateChanged = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Event notification function for EFI_SIMPLE_POINTER_PROTOCOL.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_DEV instance.
+ 
+**/
+VOID
+EFIAPI
+UsbMouseWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  )
+{
+  USB_MOUSE_DEV *UsbMouseDev;
+
+  UsbMouseDev = (USB_MOUSE_DEV *) Context;
+
+  //
+  // If there's input from mouse, signal the event.
+  //
+  if (UsbMouseDev->StateChanged) {
+    gBS->SignalEvent (Event);
+  }
+}
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_MOUSE_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  )
+{
+  USB_MOUSE_DEV       *UsbMouseDev;
+  EFI_USB_IO_PROTOCOL *UsbIo;
+
+  UsbMouseDev = (USB_MOUSE_DEV *) Context;
+
+  UsbIo       = UsbMouseDev->UsbIo;
+
+  //
+  // Re-submit Asynchronous Interrupt Transfer for recovery.
+  //
+  UsbIo->UsbAsyncInterruptTransfer (
+           UsbIo,
+           UsbMouseDev->IntEndpointDescriptor.EndpointAddress,
+           TRUE,
+           UsbMouseDev->IntEndpointDescriptor.Interval,
+           UsbMouseDev->IntEndpointDescriptor.MaxPacketSize,
+           OnMouseInterruptComplete,
+           UsbMouseDev
+           );
+}
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h
new file mode 100644
index 0000000000..0dab9de11d
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouse.h
@@ -0,0 +1,471 @@
+/** @file
+  Helper routine and corresponding data struct used by USB Mouse Driver.
+
+Copyright (c) 2004 - 2012, 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.
+
+**/
+
+#ifndef _EFI_USB_MOUSE_H_
+#define _EFI_USB_MOUSE_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/SimplePointer.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiUsbLib.h>
+#include <Library/DebugLib.h>
+
+#include <IndustryStandard/Usb.h>
+
+#define CLASS_HID               3
+#define SUBCLASS_BOOT           1
+#define PROTOCOL_MOUSE          2
+
+#define BOOT_PROTOCOL           0
+#define REPORT_PROTOCOL         1
+
+#define USB_MOUSE_DEV_SIGNATURE SIGNATURE_32 ('u', 'm', 'o', 'u')
+
+//
+// A common header for usb standard descriptor.
+// Each stand descriptor has a length and type.
+//
+#pragma pack(1)
+typedef struct {
+  UINT8                   Len;
+  UINT8                   Type;
+} USB_DESC_HEAD;
+#pragma pack()
+
+///
+/// Button range and status
+///
+typedef struct {
+  BOOLEAN ButtonDetected;
+  UINT8   ButtonMinIndex;
+  UINT8   ButtonMaxIndex;
+  UINT8   Reserved;
+} USB_MOUSE_BUTTON_DATA;
+
+///
+/// Device instance of USB mouse.
+///
+typedef struct {
+  UINTN                         Signature;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+  EFI_EVENT                     DelayedRecoveryEvent;
+  EFI_USB_IO_PROTOCOL           *UsbIo;
+  EFI_USB_INTERFACE_DESCRIPTOR  InterfaceDescriptor;
+  EFI_USB_ENDPOINT_DESCRIPTOR   IntEndpointDescriptor;
+  UINT8                         NumberOfButtons;
+  INT32                         XLogicMax;
+  INT32                         XLogicMin;
+  INT32                         YLogicMax;
+  INT32                         YLogicMin;
+  EFI_SIMPLE_POINTER_PROTOCOL   SimplePointerProtocol;
+  EFI_SIMPLE_POINTER_STATE      State;
+  EFI_SIMPLE_POINTER_MODE       Mode;
+  BOOLEAN                       StateChanged;
+  USB_MOUSE_BUTTON_DATA         PrivateData;
+  EFI_UNICODE_STRING_TABLE      *ControllerNameTable;
+} USB_MOUSE_DEV;
+
+///
+/// General HID Item structure
+///
+
+typedef union {
+  UINT8   Uint8;
+  UINT16  Uint16;
+  UINT32  Uint32;
+  INT8    Int8;
+  INT16   Int16;
+  INT32   Int32;
+  UINT8   *LongData;
+} HID_DATA;
+
+typedef struct {
+  UINT16    Format;
+  UINT8     Size;
+  UINT8     Type;
+  UINT8     Tag;
+  HID_DATA  Data;
+} HID_ITEM;
+
+#define USB_MOUSE_DEV_FROM_MOUSE_PROTOCOL(a) \
+    CR(a, USB_MOUSE_DEV, SimplePointerProtocol, USB_MOUSE_DEV_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gUsbMouseDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gUsbMouseComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gUsbMouseComponentName2;
+
+//
+// Functions of Driver Binding Protocol
+//
+
+/**
+  Check whether USB mouse driver supports this device.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller handle to check.
+  @param  RemainingDevicePath    The remaining device path.
+
+  @retval EFI_SUCCESS            The driver supports this controller.
+  @retval other                  This device isn't supported.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Starts the mouse device with this driver.
+
+  This function consumes USB I/O Portocol, intializes USB mouse device,
+  installs Simple Pointer Protocol, and submits Asynchronous Interrupt
+  Transfer to manage the USB mouse device.
+
+  @param  This                  The USB mouse driver binding instance.
+  @param  Controller            Handle of device to bind driver to.
+  @param  RemainingDevicePath   Optional parameter use to pick a specific child
+                                device to start.
+
+  @retval EFI_SUCCESS           This driver supports this device.
+  @retval EFI_UNSUPPORTED       This driver does not support this device.
+  @retval EFI_DEVICE_ERROR      This driver cannot be started due to device Error.
+  @retval EFI_OUT_OF_RESOURCES  Can't allocate memory resources.
+  @retval EFI_ALREADY_STARTED   This driver has been started.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop the USB mouse device handled by this driver.
+
+  @param  This                   The USB mouse driver binding protocol.
+  @param  Controller             The controller to release.
+  @param  NumberOfChildren       The number of handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer      The array of child handle.
+
+  @retval EFI_SUCCESS            The device was stopped.
+  @retval EFI_UNSUPPORTED        Simple Pointer Protocol is not installed on Controller.
+  @retval Others                 Fail to uninstall protocols attached on the device.
+
+**/
+EFI_STATUS
+EFIAPI
+USBMouseDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName            A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This                  A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle      The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle           The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language              A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName        A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+//
+// Functions of EFI_SIMPLE_POINTER_PROTOCOL
+//
+
+/**
+  Retrieves the current state of a pointer device.
+    
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.                                   
+  @param  MouseState            A pointer to the state information on the pointer device.
+                                
+  @retval EFI_SUCCESS           The state of the pointer device was returned in State.
+  @retval EFI_NOT_READY         The state of the pointer device has not changed since the last call to
+                                GetState().                                                           
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to retrieve the pointer device's
+                                current state.                                                           
+  @retval EFI_INVALID_PARAMETER MouseState is NULL.                                                           
+
+**/
+EFI_STATUS
+EFIAPI
+GetMouseState (
+  IN   EFI_SIMPLE_POINTER_PROTOCOL  *This,
+  OUT  EFI_SIMPLE_POINTER_STATE     *MouseState
+  );
+
+/**                                                                 
+  Resets the pointer device hardware.
+  
+  @param  This                  A pointer to the EFI_SIMPLE_POINTER_PROTOCOL instance.
+  @param  ExtendedVerification  Indicates that the driver may perform a more exhaustive
+                                verification operation of the device during reset.
+                                
+  @retval EFI_SUCCESS           The device was reset.
+  @retval EFI_DEVICE_ERROR      The device is not functioning correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+UsbMouseReset (
+  IN EFI_SIMPLE_POINTER_PROTOCOL    *This,
+  IN BOOLEAN                        ExtendedVerification
+  );
+
+/**
+  Event notification function for SIMPLE_POINTER.WaitForInput event.
+
+  @param  Event        Event to be signaled when there's input from mouse.
+  @param  Context      Points to USB_MOUSE_DEV instance.
+ 
+**/
+VOID
+EFIAPI
+UsbMouseWaitForInput (
+  IN  EFI_EVENT               Event,
+  IN  VOID                    *Context
+  );
+
+//
+// Internal worker functions
+//
+
+/**
+  Uses USB I/O to check whether the device is a USB mouse device.
+
+  @param  UsbIo    Pointer to a USB I/O protocol instance.
+
+  @retval TRUE     Device is a USB mouse device.
+  @retval FALSE    Device is a not USB mouse device.
+
+**/
+BOOLEAN
+IsUsbMouse (
+  IN  EFI_USB_IO_PROTOCOL     *UsbIo
+  );
+
+/**
+  Initialize the USB mouse device.
+
+  This function retrieves and parses HID report descriptor, and
+  initializes state of USB_MOUSE_DEV. Then it sets indefinite idle
+  rate for the device. Finally it creates event for delayed recovery,
+  which deals with device error.
+
+  @param  UsbMouseDev           Device instance to be initialized.
+
+  @retval EFI_SUCCESS           USB mouse device successfully initialized..
+  @retval EFI_UNSUPPORTED       HID descriptor type is not report descriptor.
+  @retval Other                 USB mouse device was not initialized successfully.
+
+**/
+EFI_STATUS
+InitializeUsbMouseDevice (
+  IN OUT USB_MOUSE_DEV           *UsbMouseDev
+  );
+
+/**
+  Handler function for USB mouse's asynchronous interrupt transfer.
+
+  This function is the handler function for USB mouse's asynchronous interrupt transfer
+  to manage the mouse. It parses data returned from asynchronous interrupt transfer, and
+  get button and movement state.
+
+  @param  Data             A pointer to a buffer that is filled with key data which is
+                           retrieved via asynchronous interrupt transfer.
+  @param  DataLength       Indicates the size of the data buffer.
+  @param  Context          Pointing to USB_KB_DEV instance.
+  @param  Result           Indicates the result of the asynchronous interrupt transfer.
+
+  @retval EFI_SUCCESS      Asynchronous interrupt transfer is handled successfully.
+  @retval EFI_DEVICE_ERROR Hardware error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+OnMouseInterruptComplete (
+  IN  VOID        *Data,
+  IN  UINTN       DataLength,
+  IN  VOID        *Context,
+  IN  UINT32      Result
+  );
+
+/**
+  Handler for Delayed Recovery event.
+
+  This function is the handler for Delayed Recovery event triggered
+  by timer.
+  After a device error occurs, the event would be triggered
+  with interval of EFI_USB_INTERRUPT_DELAY. EFI_USB_INTERRUPT_DELAY
+  is defined in USB standard for error handling.
+
+  @param  Event              The Delayed Recovery event.
+  @param  Context            Points to the USB_MOUSE_DEV instance.
+
+**/
+VOID
+EFIAPI
+USBMouseRecoveryHandler (
+  IN    EFI_EVENT    Event,
+  IN    VOID         *Context
+  );
+
+/**
+  Parse Mouse Report Descriptor.
+
+  According to USB HID Specification, report descriptors are
+  composed of pieces of information. Each piece of information
+  is called an Item. This function retrieves each item from
+  the report descriptor and updates USB_MOUSE_DEV.
+
+  @param  UsbMouse          The instance of USB_MOUSE_DEV
+  @param  ReportDescriptor  Report descriptor to parse
+  @param  ReportSize        Report descriptor size
+
+  @retval EFI_SUCCESS       Report descriptor successfully parsed.
+  @retval EFI_UNSUPPORTED   Report descriptor contains long item.
+
+**/
+EFI_STATUS
+ParseMouseReportDescriptor (
+  OUT USB_MOUSE_DEV   *UsbMouse,
+  IN  UINT8           *ReportDescriptor,
+  IN  UINTN           ReportSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf
new file mode 100644
index 0000000000..13d8859f98
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf
@@ -0,0 +1,72 @@
+## @file
+# USB Mouse Driver that manages USB mouse and produces Simple Pointer Protocol.
+#
+# USB Mouse Driver consumes USB I/O Protocol and Device Path Protocol, and produces
+# Simple Pointer Protocol on USB mouse devices.
+# It manages the USB mouse device via Asynchronous Interrupt Transfer of USB I/O Protocol,
+# and parses the data according to USB HID Specification.
+# This module refers to following specifications:
+# 1. Universal Serial Bus HID Firmware Specification, ver 1.11
+# 2. UEFI Specification, v2.1
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UsbMouseDxe
+  MODULE_UNI_FILE                = UsbMouseDxe.uni
+  FILE_GUID                      = 2D2E62AA-9ECF-43b7-8219-94E7FC713DFE
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = USBMouseDriverBindingEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUsbMouseDriverBinding                        
+#  COMPONENT_NAME                =  gUsbMouseComponentName
+#  COMPONENT_NAME2               =  gUsbMouseComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  MouseHid.c
+  UsbMouse.c
+  UsbMouse.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  ReportStatusCodeLib
+  UefiUsbLib
+
+[Protocols]
+  gEfiUsbIoProtocolGuid                         ## TO_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimplePointerProtocolGuid                 ## BY_START
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER        ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UsbMouseDxeExtra.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni
new file mode 100644
index 0000000000..6da63cbd48
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.uni
diff --git a/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni
new file mode 100644
index 0000000000..6aab5aaa10
--- /dev/null
+++ b/Core/MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxeExtra.uni
diff --git a/Core/MdeModulePkg/Contributions.txt b/Core/MdeModulePkg/Contributions.txt
new file mode 100644
index 0000000000..f87cbd73c6
--- /dev/null
+++ b/Core/MdeModulePkg/Contributions.txt
@@ -0,0 +1,218 @@
+
+======================
+= Code Contributions =
+======================
+
+To make a contribution to a TianoCore project, follow these steps.
+1. Create a change description in the format specified below to
+   use in the source control commit log.
+2. Your commit message must include your "Signed-off-by" signature,
+   and "Contributed-under" message.
+3. Your "Contributed-under" message explicitly states that the
+   contribution is made under the terms of the specified
+   contribution agreement.  Your "Contributed-under" message
+   must include the name of contribution agreement and version.
+   For example: Contributed-under: TianoCore Contribution Agreement 1.0
+   The "TianoCore Contribution Agreement" is included below in
+   this document.
+4. Submit your code to the TianoCore project using the process
+   that the project documents on its web page.  If the process is
+   not documented, then submit the code on development email list
+   for the project.
+5. It is preferred that contributions are submitted using the same
+   copyright license as the base project. When that is not possible,
+   then contributions using the following licenses can be accepted:
+   * BSD (2-clause): http://opensource.org/licenses/BSD-2-Clause
+   * BSD (3-clause): http://opensource.org/licenses/BSD-3-Clause
+   * MIT: http://opensource.org/licenses/MIT
+   * Python-2.0: http://opensource.org/licenses/Python-2.0
+   * Zlib: http://opensource.org/licenses/Zlib
+
+   Contributions of code put into the public domain can also be
+   accepted.
+
+   Contributions using other licenses might be accepted, but further
+   review will be required.
+
+=====================================================
+= Change Description / Commit Message / Patch Email =
+=====================================================
+
+Your change description should use the standard format for a
+commit message, and must include your "Signed-off-by" signature
+and the "Contributed-under" message.
+
+== Sample Change Description / Commit Message =
+
+=== Start of sample patch email message ===
+
+From: Contributor Name <contributor@example.com>
+Subject: [PATCH] CodeModule: Brief-single-line-summary
+
+Full-commit-message
+
+Contributed-under: TianoCore Contribution Agreement 1.0
+Signed-off-by: Contributor Name <contributor@example.com>
+---
+
+An extra message for the patch email which will not be considered part
+of the commit message can be added here.
+
+Patch content inline or attached
+
+=== End of sample patch email message ===
+
+=== Notes for sample patch email ===
+
+* The first line of commit message is taken from the email's subject
+  line following [PATCH]. The remaining portion of the commit message
+  is the email's content until the '---' line.
+* git format-patch is one way to create this format
+
+=== Definitions for sample patch email ===
+
+* "CodeModule" is a short idenfier for the affected code.  For
+  example MdePkg, or MdeModulePkg UsbBusDxe.
+* "Brief-single-line-summary" is a short summary of the change.
+* The entire first line should be less than ~70 characters.
+* "Full-commit-message" a verbose multiple line comment describing
+  the change.  Each line should be less than ~70 characters.
+* "Contributed-under" explicitely states that the contribution is
+  made under the terms of the contribtion agreement.  This
+  agreement is included below in this document.
+* "Signed-off-by" is the contributor's signature identifying them
+  by their real/legal name and their email address.
+
+========================================
+= TianoCore Contribution Agreement 1.0 =
+========================================
+
+INTEL CORPORATION ("INTEL") MAKES AVAILABLE SOFTWARE, DOCUMENTATION,
+INFORMATION AND/OR OTHER MATERIALS FOR USE IN THE TIANOCORE OPEN SOURCE
+PROJECT (COLLECTIVELY "CONTENT"). USE OF THE CONTENT IS GOVERNED BY THE
+TERMS AND CONDITIONS OF THIS AGREEMENT BETWEEN YOU AND INTEL AND/OR THE
+TERMS AND CONDITIONS OF LICENSE AGREEMENTS OR NOTICES INDICATED OR
+REFERENCED BELOW. BY USING THE CONTENT, YOU AGREE THAT YOUR USE OF THE
+CONTENT IS GOVERNED BY THIS AGREEMENT AND/OR THE TERMS AND CONDITIONS
+OF ANY APPLICABLE LICENSE AGREEMENTS OR NOTICES INDICATED OR REFERENCED
+BELOW. IF YOU DO NOT AGREE TO THE TERMS AND CONDITIONS OF THIS
+AGREEMENT AND THE TERMS AND CONDITIONS OF ANY APPLICABLE LICENSE
+AGREEMENTS OR NOTICES INDICATED OR REFERENCED BELOW, THEN YOU MAY NOT
+USE THE CONTENT.
+
+Unless otherwise indicated, all Content made available on the TianoCore
+site is provided to you under the terms and conditions of the BSD
+License ("BSD"). A copy of the BSD License is available at
+http://opensource.org/licenses/bsd-license.php
+or when applicable, in the associated License.txt file.
+
+Certain other content may be made available under other licenses as
+indicated in or with such Content. (For example, in a License.txt file.)
+
+You accept and agree to the following terms and conditions for Your
+present and future Contributions submitted to TianoCore site. Except
+for the license granted to Intel hereunder, You reserve all right,
+title, and interest in and to Your Contributions.
+
+== SECTION 1: Definitions ==
+* "You" or "Contributor" shall mean the copyright owner or legal
+  entity authorized by the copyright owner that is making a
+  Contribution hereunder. All other entities that control, are
+  controlled by, or are under common control with that entity are
+  considered to be a single Contributor. For the purposes of this
+  definition, "control" means (i) the power, direct or indirect, to
+  cause the direction or management of such entity, whether by
+  contract or otherwise, or (ii) ownership of fifty percent (50%)
+  or more of the outstanding shares, or (iii) beneficial ownership
+  of such entity.
+* "Contribution" shall mean any original work of authorship,
+  including any modifications or additions to an existing work,
+  that is intentionally submitted by You to the TinaoCore site for
+  inclusion in, or documentation of, any of the Content. For the
+  purposes of this definition, "submitted" means any form of
+  electronic, verbal, or written communication sent to the
+  TianoCore site or its representatives, including but not limited
+  to communication on electronic mailing lists, source code
+  control systems, and issue tracking systems that are managed by,
+  or on behalf of, the TianoCore site for the purpose of
+  discussing and improving the Content, but excluding
+  communication that is conspicuously marked or otherwise
+  designated in writing by You as "Not a Contribution."
+
+== SECTION 2: License for Contributions ==
+* Contributor hereby agrees that redistribution and use of the
+  Contribution in source and binary forms, with or without
+  modification, are permitted provided that the following
+  conditions are met:
+** Redistributions of source code must retain the Contributor's
+   copyright notice, this list of conditions and the following
+   disclaimer.
+** Redistributions in binary form must reproduce the Contributor's
+   copyright notice, this list of conditions and the following
+   disclaimer in the documentation and/or other materials provided
+   with the distribution.
+* Disclaimer. None of the names of Contributor, Intel, or the names
+  of their respective contributors may be used to endorse or
+  promote products derived from this software without specific
+  prior written permission.
+* Contributor grants a license (with the right to sublicense) under
+  claims of Contributor's patents that Contributor can license that
+  are infringed by the Contribution (as delivered by Contributor) to
+  make, use, distribute, sell, offer for sale, and import the
+  Contribution and derivative works thereof solely to the minimum
+  extent necessary for licensee to exercise the granted copyright
+  license; this patent license applies solely to those portions of
+  the Contribution that are unmodified. No hardware per se is
+  licensed.
+* EXCEPT AS EXPRESSLY SET FORTH IN SECTION 3 BELOW, THE
+  CONTRIBUTION IS PROVIDED BY THE CONTRIBUTOR "AS IS" AND ANY
+  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+  CONTRIBUTOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+  NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THE
+  CONTRIBUTION, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+  DAMAGE.
+
+== SECTION 3: Representations ==
+* You represent that You are legally entitled to grant the above
+  license. If your employer(s) has rights to intellectual property
+  that You create that includes Your Contributions, You represent
+  that You have received permission to make Contributions on behalf
+  of that employer, that Your employer has waived such rights for
+  Your Contributions.
+* You represent that each of Your Contributions is Your original
+  creation (see Section 4 for submissions on behalf of others).
+  You represent that Your Contribution submissions include complete
+  details of any third-party license or other restriction
+  (including, but not limited to, related patents and trademarks)
+  of which You are personally aware and which are associated with
+  any part of Your Contributions.
+
+== SECTION 4: Third Party Contributions ==
+* Should You wish to submit work that is not Your original creation,
+  You may submit it to TianoCore site separately from any
+  Contribution, identifying the complete details of its source
+  and of any license or other restriction (including, but not
+  limited to, related patents, trademarks, and license agreements)
+  of which You are personally aware, and conspicuously marking the
+  work as "Submitted on behalf of a third-party: [named here]".
+
+== SECTION 5: Miscellaneous ==
+* Applicable Laws. Any claims arising under or relating to this
+  Agreement shall be governed by the internal substantive laws of
+  the State of Delaware or federal courts located in Delaware,
+  without regard to principles of conflict of laws.
+* Language. This Agreement is in the English language only, which
+  language shall be controlling in all respects, and all versions
+  of this Agreement in any other language shall be for accommodation
+  only and shall not be binding. All communications and notices made
+  or given pursuant to this Agreement, and all documentation and
+  support to be provided, unless otherwise noted, shall be in the
+  English language.
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dependency.c b/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dependency.c
new file mode 100644
index 0000000000..1d8a57b879
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dependency.c
@@ -0,0 +1,442 @@
+/** @file
+  DXE Dispatcher Dependency Evaluator.
+
+  This routine evaluates a dependency expression (DEPENDENCY_EXPRESSION) to determine
+  if a driver can be scheduled for execution.  The criteria for
+  schedulability is that the dependency expression is satisfied.
+
+Copyright (c) 2006 - 2010, 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 "DxeMain.h"
+
+//
+// Global stack used to evaluate dependency expressions
+//
+BOOLEAN *mDepexEvaluationStack        = NULL;
+BOOLEAN *mDepexEvaluationStackEnd     = NULL;
+BOOLEAN *mDepexEvaluationStackPointer = NULL;
+
+//
+// Worker functions
+//
+
+
+/**
+  Grow size of the Depex stack
+
+  @retval EFI_SUCCESS           Stack successfully growed.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+GrowDepexStack (
+  VOID
+  )
+{
+  BOOLEAN     *NewStack;
+  UINTN       Size;
+
+  Size = DEPEX_STACK_SIZE_INCREMENT;
+  if (mDepexEvaluationStack != NULL) {
+    Size = Size + (mDepexEvaluationStackEnd - mDepexEvaluationStack);
+  }
+
+  NewStack = AllocatePool (Size * sizeof (BOOLEAN));
+  if (NewStack == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (mDepexEvaluationStack != NULL) {
+    //
+    // Copy to Old Stack to the New Stack
+    //
+    CopyMem (
+      NewStack,
+      mDepexEvaluationStack,
+      (mDepexEvaluationStackEnd - mDepexEvaluationStack) * sizeof (BOOLEAN)
+      );
+
+    //
+    // Free The Old Stack
+    //
+    FreePool (mDepexEvaluationStack);
+  }
+
+  //
+  // Make the Stack pointer point to the old data in the new stack
+  //
+  mDepexEvaluationStackPointer = NewStack + (mDepexEvaluationStackPointer - mDepexEvaluationStack);
+  mDepexEvaluationStack        = NewStack;
+  mDepexEvaluationStackEnd     = NewStack + Size;
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Push an element onto the Boolean Stack.
+
+  @param  Value                 BOOLEAN to push.
+
+  @retval EFI_SUCCESS           The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushBool (
+  IN BOOLEAN  Value
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check for a stack overflow condition
+  //
+  if (mDepexEvaluationStackPointer == mDepexEvaluationStackEnd) {
+    //
+    // Grow the stack
+    //
+    Status = GrowDepexStack ();
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Push the item onto the stack
+  //
+  *mDepexEvaluationStackPointer = Value;
+  mDepexEvaluationStackPointer++;
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Pop an element from the Boolean stack.
+
+  @param  Value                 BOOLEAN to pop.
+
+  @retval EFI_SUCCESS           The value was popped onto the stack.
+  @retval EFI_ACCESS_DENIED     The pop operation underflowed the stack.
+
+**/
+EFI_STATUS
+PopBool (
+  OUT BOOLEAN  *Value
+  )
+{
+  //
+  // Check for a stack underflow condition
+  //
+  if (mDepexEvaluationStackPointer == mDepexEvaluationStack) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Pop the item off the stack
+  //
+  mDepexEvaluationStackPointer--;
+  *Value = *mDepexEvaluationStackPointer;
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Preprocess dependency expression and update DriverEntry to reflect the
+  state of  Before, After, and SOR dependencies. If DriverEntry->Before
+  or DriverEntry->After is set it will never be cleared. If SOR is set
+  it will be cleared by CoreSchedule(), and then the driver can be
+  dispatched.
+
+  @param  DriverEntry           DriverEntry element to update .
+
+  @retval EFI_SUCCESS           It always works.
+
+**/
+EFI_STATUS
+CorePreProcessDepex (
+  IN  EFI_CORE_DRIVER_ENTRY   *DriverEntry
+  )
+{
+  UINT8  *Iterator;
+
+  Iterator = DriverEntry->Depex;
+  if (*Iterator == EFI_DEP_SOR) {
+    DriverEntry->Unrequested = TRUE;
+  } else {
+    DriverEntry->Dependent = TRUE;
+  }
+
+  if (*Iterator == EFI_DEP_BEFORE) {
+    DriverEntry->Before = TRUE;
+  } else if (*Iterator == EFI_DEP_AFTER) {
+    DriverEntry->After = TRUE;
+  }
+
+  if (DriverEntry->Before || DriverEntry->After) {
+    CopyMem (&DriverEntry->BeforeAfterGuid, Iterator + 1, sizeof (EFI_GUID));
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  This is the POSTFIX version of the dependency evaluator.  This code does
+  not need to handle Before or After, as it is not valid to call this
+  routine in this case. The SOR is just ignored and is a nop in the grammer.
+  POSTFIX means all the math is done on top of the stack.
+
+  @param  DriverEntry           DriverEntry element to update.
+
+  @retval TRUE                  If driver is ready to run.
+  @retval FALSE                 If driver is not ready to run or some fatal error
+                                was found.
+
+**/
+BOOLEAN
+CoreIsSchedulable (
+  IN  EFI_CORE_DRIVER_ENTRY   *DriverEntry
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       *Iterator;
+  BOOLEAN     Operator;
+  BOOLEAN     Operator2;
+  EFI_GUID    DriverGuid;
+  VOID        *Interface;
+
+  Operator = FALSE;
+  Operator2 = FALSE;
+
+  if (DriverEntry->After || DriverEntry->Before) {
+    //
+    // If Before or After Depex skip as CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter ()
+    // processes them.
+    //
+    return FALSE;
+  }
+
+  DEBUG ((DEBUG_DISPATCH, "Evaluate DXE DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+
+  if (DriverEntry->Depex == NULL) {
+    //
+    // A NULL Depex means treat the driver like an UEFI 2.0 thing.
+    //
+    Status = CoreAllEfiServicesAvailable ();
+    DEBUG ((DEBUG_DISPATCH, "  All UEFI Services Available                     = "));
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_DISPATCH, "FALSE\n  RESULT = FALSE\n"));
+      return FALSE;
+    }
+    DEBUG ((DEBUG_DISPATCH, "TRUE\n  RESULT = TRUE\n"));
+    return TRUE;
+  }
+
+  //
+  // Clean out memory leaks in Depex Boolean stack. Leaks are only caused by
+  // incorrectly formed DEPEX expressions
+  //
+  mDepexEvaluationStackPointer = mDepexEvaluationStack;
+
+
+  Iterator = DriverEntry->Depex;
+
+  while (TRUE) {
+    //
+    // Check to see if we are attempting to fetch dependency expression instructions
+    // past the end of the dependency expression.
+    //
+    if (((UINTN)Iterator - (UINTN)DriverEntry->Depex) >= DriverEntry->DepexSize) {
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Attempt to fetch past end of depex)\n"));
+      return FALSE;
+    }
+
+    //
+    // Look at the opcode of the dependency expression instruction.
+    //
+    switch (*Iterator) {
+    case EFI_DEP_BEFORE:
+    case EFI_DEP_AFTER:
+      //
+      // For a well-formed Dependency Expression, the code should never get here.
+      // The BEFORE and AFTER are processed prior to this routine's invocation.
+      // If the code flow arrives at this point, there was a BEFORE or AFTER
+      // that were not the first opcodes.
+      //
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected BEFORE or AFTER opcode)\n"));
+      ASSERT (FALSE);
+    case EFI_DEP_SOR:
+      //
+      // These opcodes can only appear once as the first opcode.  If it is found
+      // at any other location, then the dependency expression evaluates to FALSE
+      //
+      if (Iterator != DriverEntry->Depex) {
+        DEBUG ((DEBUG_DISPATCH, "  SOR\n"));
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected SOR opcode)\n"));
+        return FALSE;
+      }
+      DEBUG ((DEBUG_DISPATCH, "  SOR                                             = Requested\n"));
+      //
+      // Otherwise, it is the first opcode and should be treated as a NOP.
+      //
+      break;
+
+    case EFI_DEP_PUSH:
+      //
+      // Push operator is followed by a GUID. Test to see if the GUID protocol
+      // is installed and push the boolean result on the stack.
+      //
+      CopyMem (&DriverGuid, Iterator + 1, sizeof (EFI_GUID));
+
+      Status = CoreLocateProtocol (&DriverGuid, NULL, &Interface);
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = FALSE\n", &DriverGuid));
+        Status = PushBool (FALSE);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = TRUE\n", &DriverGuid));
+        *Iterator = EFI_DEP_REPLACE_TRUE;
+        Status = PushBool (TRUE);
+      }
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Iterator += sizeof (EFI_GUID);
+      break;
+
+    case EFI_DEP_AND:
+      DEBUG ((DEBUG_DISPATCH, "  AND\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PopBool (&Operator2);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(Operator && Operator2));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_OR:
+      DEBUG ((DEBUG_DISPATCH, "  OR\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PopBool (&Operator2);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(Operator || Operator2));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_NOT:
+      DEBUG ((DEBUG_DISPATCH, "  NOT\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(!Operator));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_TRUE:
+      DEBUG ((DEBUG_DISPATCH, "  TRUE\n"));
+      Status = PushBool (TRUE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_FALSE:
+      DEBUG ((DEBUG_DISPATCH, "  FALSE\n"));
+      Status = PushBool (FALSE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_END:
+      DEBUG ((DEBUG_DISPATCH, "  END\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = %a\n", Operator ? "TRUE" : "FALSE"));
+      return Operator;
+
+    case EFI_DEP_REPLACE_TRUE:
+      CopyMem (&DriverGuid, Iterator + 1, sizeof (EFI_GUID));
+      DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = TRUE\n", &DriverGuid));
+      
+      Status = PushBool (TRUE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Iterator += sizeof (EFI_GUID);
+      break;
+
+    default:
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unknown opcode)\n"));
+      goto Done;
+    }
+
+    //
+    // Skip over the Dependency Op Code we just processed in the switch.
+    // The math is done out of order, but it should not matter. That is
+    // we may add in the sizeof (EFI_GUID) before we account for the OP Code.
+    // This is not an issue, since we just need the correct end result. You
+    // need to be careful using Iterator in the loop as it's intermediate value
+    // may be strange.
+    //
+    Iterator++;
+  }
+
+Done:
+  return FALSE;
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dispatcher.c b/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dispatcher.c
new file mode 100644
index 0000000000..0776cd6d24
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Dispatcher/Dispatcher.c
@@ -0,0 +1,1426 @@
+/** @file
+  DXE Dispatcher.
+
+  Step #1 - When a FV protocol is added to the system every driver in the FV
+            is added to the mDiscoveredList. The SOR, Before, and After Depex are
+            pre-processed as drivers are added to the mDiscoveredList. If an Apriori
+            file exists in the FV those drivers are addeded to the
+            mScheduledQueue. The mFvHandleList is used to make sure a
+            FV is only processed once.
+
+  Step #2 - Dispatch. Remove driver from the mScheduledQueue and load and
+            start it. After mScheduledQueue is drained check the
+            mDiscoveredList to see if any item has a Depex that is ready to
+            be placed on the mScheduledQueue.
+
+  Step #3 - Adding to the mScheduledQueue requires that you process Before
+            and After dependencies. This is done recursively as the call to add
+            to the mScheduledQueue checks for Before and recursively adds
+            all Befores. It then addes the item that was passed in and then
+            processess the After dependecies by recursively calling the routine.
+
+  Dispatcher Rules:
+  The rules for the dispatcher are in chapter 10 of the DXE CIS. Figure 10-3
+  is the state diagram for the DXE dispatcher
+
+  Depex - Dependency Expresion.
+  SOR   - Schedule On Request - Don't schedule if this bit is set.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+
+//
+// The Driver List contains one copy of every driver that has been discovered.
+// Items are never removed from the driver list. List of EFI_CORE_DRIVER_ENTRY
+//
+LIST_ENTRY  mDiscoveredList = INITIALIZE_LIST_HEAD_VARIABLE (mDiscoveredList);
+
+//
+// Queue of drivers that are ready to dispatch. This queue is a subset of the
+// mDiscoveredList.list of EFI_CORE_DRIVER_ENTRY.
+//
+LIST_ENTRY  mScheduledQueue = INITIALIZE_LIST_HEAD_VARIABLE (mScheduledQueue);
+
+//
+// List of handles who's Fv's have been parsed and added to the mFwDriverList.
+//
+LIST_ENTRY  mFvHandleList = INITIALIZE_LIST_HEAD_VARIABLE (mFvHandleList);           // list of KNOWN_HANDLE
+
+//
+// Lock for mDiscoveredList, mScheduledQueue, gDispatcherRunning.
+//
+EFI_LOCK  mDispatcherLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_HIGH_LEVEL);
+
+
+//
+// Flag for the DXE Dispacher.  TRUE if dispatcher is execuing.
+//
+BOOLEAN  gDispatcherRunning = FALSE;
+
+//
+// Module globals to manage the FwVol registration notification event
+//
+EFI_EVENT       mFwVolEvent;
+VOID            *mFwVolEventRegistration;
+
+//
+// List of file types supported by dispatcher
+//
+EFI_FV_FILETYPE mDxeFileTypes[] = {
+  EFI_FV_FILETYPE_DRIVER,
+  EFI_FV_FILETYPE_COMBINED_SMM_DXE,
+  EFI_FV_FILETYPE_COMBINED_PEIM_DRIVER,
+  EFI_FV_FILETYPE_DXE_CORE,
+  EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE
+};
+
+typedef struct {
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH   File;
+  EFI_DEVICE_PATH_PROTOCOL            End;
+} FV_FILEPATH_DEVICE_PATH;
+
+FV_FILEPATH_DEVICE_PATH mFvDevicePath;
+
+//
+// Function Prototypes
+//
+/**
+  Insert InsertedDriverEntry onto the mScheduledQueue. To do this you
+  must add any driver with a before dependency on InsertedDriverEntry first.
+  You do this by recursively calling this routine. After all the Befores are
+  processed you can add InsertedDriverEntry to the mScheduledQueue.
+  Then you can add any driver with an After dependency on InsertedDriverEntry
+  by recursively calling this routine.
+
+  @param  InsertedDriverEntry   The driver to insert on the ScheduledLink Queue
+
+**/
+VOID
+CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter (
+  IN  EFI_CORE_DRIVER_ENTRY   *InsertedDriverEntry
+  );
+
+/**
+  Event notification that is fired every time a FV dispatch protocol is added.
+  More than one protocol may have been added when this event is fired, so you
+  must loop on CoreLocateHandle () to see how many protocols were added and
+  do the following to each FV:
+  If the Fv has already been processed, skip it. If the Fv has not been
+  processed then mark it as being processed, as we are about to process it.
+  Read the Fv and add any driver in the Fv to the mDiscoveredList.The
+  mDiscoveredList is never free'ed and contains variables that define
+  the other states the DXE driver transitions to..
+  While you are at it read the A Priori file into memory.
+  Place drivers in the A Priori list onto the mScheduledQueue.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+CoreFwVolEventProtocolNotify (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  );
+
+/**
+  Convert FvHandle and DriverName into an EFI device path
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_CORE_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+
+  @return Pointer to device path constructed from FvHandle and DriverName
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CoreFvToDevicePath (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName
+  );
+
+/**
+  Add an entry to the mDiscoveredList. Allocate memory to store the DriverEntry,
+  and initilize any state variables. Read the Depex from the FV and store it
+  in DriverEntry. Pre-process the Depex to set the SOR, Before and After state.
+  The Discovered list is never free'ed and contains booleans that represent the
+  other possible DXE driver states.
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_CORE_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+  @param  Type                  Fv File Type of file to add to mDiscoveredList.
+
+  @retval EFI_SUCCESS           If driver was added to the mDiscoveredList.
+  @retval EFI_ALREADY_STARTED   The driver has already been started. Only one
+                                DriverName may be active in the system at any one
+                                time.
+
+**/
+EFI_STATUS
+CoreAddToDriverList (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName,
+  IN  EFI_FV_FILETYPE                 Type
+  );
+
+/**
+  Get the driver from the FV through driver name, and produce a FVB protocol on FvHandle.
+
+  @param  Fv                    The FIRMWARE_VOLUME protocol installed on the FV.
+  @param  FvHandle              The handle which FVB protocol installed on.
+  @param  DriverName            The driver guid specified.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough memory or other resource.
+  @retval EFI_VOLUME_CORRUPTED  Corrupted volume.
+  @retval EFI_SUCCESS           Function successfully returned.
+
+**/
+EFI_STATUS
+CoreProcessFvImageFile (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName
+  );
+
+
+/**
+  Enter critical section by gaining lock on mDispatcherLock.
+
+**/
+VOID
+CoreAcquireDispatcherLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&mDispatcherLock);
+}
+
+
+/**
+  Exit critical section by releasing lock on mDispatcherLock.
+
+**/
+VOID
+CoreReleaseDispatcherLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&mDispatcherLock);
+}
+
+
+/**
+  Read Depex and pre-process the Depex for Before and After. If Section Extraction
+  protocol returns an error via ReadSection defer the reading of the Depex.
+
+  @param  DriverEntry           Driver to work on.
+
+  @retval EFI_SUCCESS           Depex read and preprossesed
+  @retval EFI_PROTOCOL_ERROR    The section extraction protocol returned an error
+                                and  Depex reading needs to be retried.
+  @retval Error                 DEPEX not found.
+
+**/
+EFI_STATUS
+CoreGetDepexSectionAndPreProccess (
+  IN  EFI_CORE_DRIVER_ENTRY   *DriverEntry
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SECTION_TYPE              SectionType;
+  UINT32                        AuthenticationStatus;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *Fv;
+
+
+  Fv = DriverEntry->Fv;
+
+  //
+  // Grab Depex info, it will never be free'ed.
+  //
+  SectionType         = EFI_SECTION_DXE_DEPEX;
+  Status = Fv->ReadSection (
+                DriverEntry->Fv,
+                &DriverEntry->FileName,
+                SectionType,
+                0,
+                &DriverEntry->Depex,
+                (UINTN *)&DriverEntry->DepexSize,
+                &AuthenticationStatus
+                );
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_PROTOCOL_ERROR) {
+      //
+      // The section extraction protocol failed so set protocol error flag
+      //
+      DriverEntry->DepexProtocolError = TRUE;
+    } else {
+      //
+      // If no Depex assume UEFI 2.0 driver model
+      //
+      DriverEntry->Depex = NULL;
+      DriverEntry->Dependent = TRUE;
+      DriverEntry->DepexProtocolError = FALSE;
+    }
+  } else {
+    //
+    // Set Before, After, and Unrequested state information based on Depex
+    // Driver will be put in Dependent or Unrequested state
+    //
+    CorePreProcessDepex (DriverEntry);
+    DriverEntry->DepexProtocolError = FALSE;
+  }
+
+  return Status;
+}
+
+
+/**
+  Check every driver and locate a matching one. If the driver is found, the Unrequested
+  state flag is cleared.
+
+  @param  FirmwareVolumeHandle  The handle of the Firmware Volume that contains
+                                the firmware  file specified by DriverName.
+  @param  DriverName            The Driver name to put in the Dependent state.
+
+  @retval EFI_SUCCESS           The DriverName was found and it's SOR bit was
+                                cleared
+  @retval EFI_NOT_FOUND         The DriverName does not exist or it's SOR bit was
+                                not set.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSchedule (
+  IN  EFI_HANDLE  FirmwareVolumeHandle,
+  IN  EFI_GUID    *DriverName
+  )
+{
+  LIST_ENTRY            *Link;
+  EFI_CORE_DRIVER_ENTRY *DriverEntry;
+
+  //
+  // Check every driver
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->FvHandle == FirmwareVolumeHandle &&
+        DriverEntry->Unrequested &&
+        CompareGuid (DriverName, &DriverEntry->FileName)) {
+      //
+      // Move the driver from the Unrequested to the Dependent state
+      //
+      CoreAcquireDispatcherLock ();
+      DriverEntry->Unrequested  = FALSE;
+      DriverEntry->Dependent    = TRUE;
+      CoreReleaseDispatcherLock ();
+
+      DEBUG ((DEBUG_DISPATCH, "Schedule FFS(%g) - EFI_SUCCESS\n", DriverName));
+      
+      return EFI_SUCCESS;
+    }
+  }
+  
+  DEBUG ((DEBUG_DISPATCH, "Schedule FFS(%g) - EFI_NOT_FOUND\n", DriverName));
+  
+  return EFI_NOT_FOUND;
+}
+
+
+
+/**
+  Convert a driver from the Untrused back to the Scheduled state.
+
+  @param  FirmwareVolumeHandle  The handle of the Firmware Volume that contains
+                                the firmware  file specified by DriverName.
+  @param  DriverName            The Driver name to put in the Scheduled state
+
+  @retval EFI_SUCCESS           The file was found in the untrusted state, and it
+                                was promoted  to the trusted state.
+  @retval EFI_NOT_FOUND         The file was not found in the untrusted state.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreTrust (
+  IN  EFI_HANDLE  FirmwareVolumeHandle,
+  IN  EFI_GUID    *DriverName
+  )
+{
+  LIST_ENTRY            *Link;
+  EFI_CORE_DRIVER_ENTRY *DriverEntry;
+
+  //
+  // Check every driver
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->FvHandle == FirmwareVolumeHandle &&
+        DriverEntry->Untrusted &&
+        CompareGuid (DriverName, &DriverEntry->FileName)) {
+      //
+      // Transition driver from Untrusted to Scheduled state.
+      //
+      CoreAcquireDispatcherLock ();
+      DriverEntry->Untrusted = FALSE;
+      DriverEntry->Scheduled = TRUE;
+      InsertTailList (&mScheduledQueue, &DriverEntry->ScheduledLink);
+      CoreReleaseDispatcherLock ();
+
+      return EFI_SUCCESS;
+    }
+  }
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  An empty function to pass error checking of CreateEventEx ().
+
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               Pointer to the notification function's context,
+                                which is implementation-dependent.
+
+**/
+VOID
+EFIAPI
+CoreEmptyCallbackFunction (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  return;
+}
+
+/**
+  This is the main Dispatcher for DXE and it exits when there are no more
+  drivers to run. Drain the mScheduledQueue and load and start a PE
+  image for each driver. Search the mDiscoveredList to see if any driver can
+  be placed on the mScheduledQueue. If no drivers are placed on the
+  mScheduledQueue exit the function. On exit it is assumed the Bds()
+  will be called, and when the Bds() exits the Dispatcher will be called
+  again.
+
+  @retval EFI_ALREADY_STARTED   The DXE Dispatcher is already running
+  @retval EFI_NOT_FOUND         No DXE Drivers were dispatched
+  @retval EFI_SUCCESS           One or more DXE Drivers were dispatched
+
+**/
+EFI_STATUS
+EFIAPI
+CoreDispatcher (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_STATUS                      ReturnStatus;
+  LIST_ENTRY                      *Link;
+  EFI_CORE_DRIVER_ENTRY           *DriverEntry;
+  BOOLEAN                         ReadyToRun;
+  EFI_EVENT                       DxeDispatchEvent;
+  
+
+  if (gDispatcherRunning) {
+    //
+    // If the dispatcher is running don't let it be restarted.
+    //
+    return EFI_ALREADY_STARTED;
+  }
+
+  gDispatcherRunning = TRUE;
+
+  Status = CoreCreateEventEx (
+             EVT_NOTIFY_SIGNAL,
+             TPL_NOTIFY,
+             CoreEmptyCallbackFunction,
+             NULL,
+             &gEfiEventDxeDispatchGuid,
+             &DxeDispatchEvent
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ReturnStatus = EFI_NOT_FOUND;
+  do {
+    //
+    // Drain the Scheduled Queue
+    //
+    while (!IsListEmpty (&mScheduledQueue)) {
+      DriverEntry = CR (
+                      mScheduledQueue.ForwardLink,
+                      EFI_CORE_DRIVER_ENTRY,
+                      ScheduledLink,
+                      EFI_CORE_DRIVER_ENTRY_SIGNATURE
+                      );
+
+      //
+      // Load the DXE Driver image into memory. If the Driver was transitioned from
+      // Untrused to Scheduled it would have already been loaded so we may need to
+      // skip the LoadImage
+      //
+      if (DriverEntry->ImageHandle == NULL && !DriverEntry->IsFvImage) {
+        DEBUG ((DEBUG_INFO, "Loading driver %g\n", &DriverEntry->FileName));
+        Status = CoreLoadImage (
+                        FALSE,
+                        gDxeCoreImageHandle,
+                        DriverEntry->FvFileDevicePath,
+                        NULL,
+                        0,
+                        &DriverEntry->ImageHandle
+                        );
+
+        //
+        // Update the driver state to reflect that it's been loaded
+        //
+        if (EFI_ERROR (Status)) {
+          CoreAcquireDispatcherLock ();
+
+          if (Status == EFI_SECURITY_VIOLATION) {
+            //
+            // Take driver from Scheduled to Untrused state
+            //
+            DriverEntry->Untrusted = TRUE;
+          } else {
+            //
+            // The DXE Driver could not be loaded, and do not attempt to load or start it again.
+            // Take driver from Scheduled to Initialized.
+            //
+            // This case include the Never Trusted state if EFI_ACCESS_DENIED is returned
+            //
+            DriverEntry->Initialized  = TRUE;
+          }
+
+          DriverEntry->Scheduled = FALSE;
+          RemoveEntryList (&DriverEntry->ScheduledLink);
+
+          CoreReleaseDispatcherLock ();
+
+          //
+          // If it's an error don't try the StartImage
+          //
+          continue;
+        }
+      }
+
+      CoreAcquireDispatcherLock ();
+
+      DriverEntry->Scheduled    = FALSE;
+      DriverEntry->Initialized  = TRUE;
+      RemoveEntryList (&DriverEntry->ScheduledLink);
+
+      CoreReleaseDispatcherLock ();
+
+ 
+      if (DriverEntry->IsFvImage) {
+        //
+        // Produce a firmware volume block protocol for FvImage so it gets dispatched from. 
+        //
+        Status = CoreProcessFvImageFile (DriverEntry->Fv, DriverEntry->FvHandle, &DriverEntry->FileName);
+      } else {
+        REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+          EFI_PROGRESS_CODE,
+          (EFI_SOFTWARE_DXE_CORE | EFI_SW_PC_INIT_BEGIN),
+          &DriverEntry->ImageHandle,
+          sizeof (DriverEntry->ImageHandle)
+          );
+        ASSERT (DriverEntry->ImageHandle != NULL);
+  
+        Status = CoreStartImage (DriverEntry->ImageHandle, NULL, NULL);
+  
+        REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+          EFI_PROGRESS_CODE,
+          (EFI_SOFTWARE_DXE_CORE | EFI_SW_PC_INIT_END),
+          &DriverEntry->ImageHandle,
+          sizeof (DriverEntry->ImageHandle)
+          );
+      }
+
+      ReturnStatus = EFI_SUCCESS;
+    }
+
+    //
+    // Now DXE Dispatcher finished one round of dispatch, signal an event group
+    // so that SMM Dispatcher get chance to dispatch SMM Drivers which depend
+    // on UEFI protocols
+    //
+    if (!EFI_ERROR (ReturnStatus)) {
+      CoreSignalEvent (DxeDispatchEvent);
+    }
+
+    //
+    // Search DriverList for items to place on Scheduled Queue
+    //
+    ReadyToRun = FALSE;
+    for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+      DriverEntry = CR (Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+
+      if (DriverEntry->DepexProtocolError){
+        //
+        // If Section Extraction Protocol did not let the Depex be read before retry the read
+        //
+        Status = CoreGetDepexSectionAndPreProccess (DriverEntry);
+      }
+
+      if (DriverEntry->Dependent) {
+        if (CoreIsSchedulable (DriverEntry)) {
+          CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+          ReadyToRun = TRUE;
+        }
+      } else {
+        if (DriverEntry->Unrequested) {
+          DEBUG ((DEBUG_DISPATCH, "Evaluate DXE DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+          DEBUG ((DEBUG_DISPATCH, "  SOR                                             = Not Requested\n"));
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE\n"));
+        }
+      }
+    }
+  } while (ReadyToRun);
+
+  //
+  // Close DXE dispatch Event
+  //
+  CoreCloseEvent (DxeDispatchEvent);
+
+  gDispatcherRunning = FALSE;
+
+  return ReturnStatus;
+}
+
+
+/**
+  Insert InsertedDriverEntry onto the mScheduledQueue. To do this you
+  must add any driver with a before dependency on InsertedDriverEntry first.
+  You do this by recursively calling this routine. After all the Befores are
+  processed you can add InsertedDriverEntry to the mScheduledQueue.
+  Then you can add any driver with an After dependency on InsertedDriverEntry
+  by recursively calling this routine.
+
+  @param  InsertedDriverEntry   The driver to insert on the ScheduledLink Queue
+
+**/
+VOID
+CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter (
+  IN  EFI_CORE_DRIVER_ENTRY   *InsertedDriverEntry
+  )
+{
+  LIST_ENTRY            *Link;
+  EFI_CORE_DRIVER_ENTRY *DriverEntry;
+
+  //
+  // Process Before Dependency
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->Before && DriverEntry->Dependent && DriverEntry != InsertedDriverEntry) {
+      DEBUG ((DEBUG_DISPATCH, "Evaluate DXE DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+      DEBUG ((DEBUG_DISPATCH, "  BEFORE FFS(%g) = ", &DriverEntry->BeforeAfterGuid));
+      if (CompareGuid (&InsertedDriverEntry->FileName, &DriverEntry->BeforeAfterGuid)) {
+        //
+        // Recursively process BEFORE
+        //
+        DEBUG ((DEBUG_DISPATCH, "TRUE\n  END\n  RESULT = TRUE\n"));
+        CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "FALSE\n  END\n  RESULT = FALSE\n"));
+      }
+    }
+  }
+
+  //
+  // Convert driver from Dependent to Scheduled state
+  //
+  CoreAcquireDispatcherLock ();
+
+  InsertedDriverEntry->Dependent = FALSE;
+  InsertedDriverEntry->Scheduled = TRUE;
+  InsertTailList (&mScheduledQueue, &InsertedDriverEntry->ScheduledLink);
+
+  CoreReleaseDispatcherLock ();
+
+  //
+  // Process After Dependency
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->After && DriverEntry->Dependent && DriverEntry != InsertedDriverEntry) {
+      DEBUG ((DEBUG_DISPATCH, "Evaluate DXE DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+      DEBUG ((DEBUG_DISPATCH, "  AFTER FFS(%g) = ", &DriverEntry->BeforeAfterGuid));
+      if (CompareGuid (&InsertedDriverEntry->FileName, &DriverEntry->BeforeAfterGuid)) {
+        //
+        // Recursively process AFTER
+        //
+        DEBUG ((DEBUG_DISPATCH, "TRUE\n  END\n  RESULT = TRUE\n"));
+        CoreInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "FALSE\n  END\n  RESULT = FALSE\n"));
+      }
+    }
+  }
+}
+
+
+/**
+  Return TRUE if the Fv has been processed, FALSE if not.
+
+  @param  FvHandle              The handle of a FV that's being tested
+
+  @retval TRUE                  Fv protocol on FvHandle has been processed
+  @retval FALSE                 Fv protocol on FvHandle has not yet been processed
+
+**/
+BOOLEAN
+FvHasBeenProcessed (
+  IN  EFI_HANDLE      FvHandle
+  )
+{
+  LIST_ENTRY      *Link;
+  KNOWN_HANDLE    *KnownHandle;
+
+  for (Link = mFvHandleList.ForwardLink; Link != &mFvHandleList; Link = Link->ForwardLink) {
+    KnownHandle = CR(Link, KNOWN_HANDLE, Link, KNOWN_HANDLE_SIGNATURE);
+    if (KnownHandle->Handle == FvHandle) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+
+/**
+  Remember that Fv protocol on FvHandle has had it's drivers placed on the
+  mDiscoveredList. This fucntion adds entries on the mFvHandleList if new 
+  entry is different from one in mFvHandleList by checking FvImage Guid.
+  Items are never removed/freed from the mFvHandleList.
+
+  @param  FvHandle              The handle of a FV that has been processed
+
+  @return A point to new added FvHandle entry. If FvHandle with the same FvImage guid
+          has been added, NULL will return. 
+
+**/
+KNOWN_HANDLE * 
+FvIsBeingProcesssed (
+  IN  EFI_HANDLE    FvHandle
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_GUID                              FvNameGuid;
+  BOOLEAN                               FvNameGuidIsFound;
+  UINT32                                ExtHeaderOffset;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *Fvb;
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+  EFI_FV_BLOCK_MAP_ENTRY                *BlockMap;
+  UINTN                                 LbaOffset;
+  UINTN                                 Index;
+  EFI_LBA                               LbaIndex;
+  LIST_ENTRY                            *Link;
+  KNOWN_HANDLE                          *KnownHandle;
+
+  FwVolHeader = NULL;
+
+  //
+  // Get the FirmwareVolumeBlock protocol on that handle
+  //
+  FvNameGuidIsFound = FALSE;
+  Status = CoreHandleProtocol (FvHandle, &gEfiFirmwareVolumeBlockProtocolGuid, (VOID **)&Fvb);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Get the full FV header based on FVB protocol.
+    //
+    ASSERT (Fvb != NULL);
+    Status = GetFwVolHeader (Fvb, &FwVolHeader);
+    if (!EFI_ERROR (Status)) {
+      ASSERT (FwVolHeader != NULL);
+      if (VerifyFvHeaderChecksum (FwVolHeader) && FwVolHeader->ExtHeaderOffset != 0) {
+        ExtHeaderOffset = (UINT32) FwVolHeader->ExtHeaderOffset;
+        BlockMap  = FwVolHeader->BlockMap;
+        LbaIndex  = 0;
+        LbaOffset = 0;
+        //
+        // Find LbaIndex and LbaOffset for FV extension header based on BlockMap.
+        //
+        while ((BlockMap->NumBlocks != 0) || (BlockMap->Length != 0)) {
+          for (Index = 0; Index < BlockMap->NumBlocks && ExtHeaderOffset >= BlockMap->Length; Index ++) {
+            ExtHeaderOffset -= BlockMap->Length;
+            LbaIndex ++;
+          }
+          //
+          // Check whether FvExtHeader is crossing the multi block range.
+          //
+          if (Index < BlockMap->NumBlocks) {
+            LbaOffset = ExtHeaderOffset;
+            break;
+          }
+          BlockMap++;
+        }
+        //
+        // Read FvNameGuid from FV extension header.
+        //
+        Status = ReadFvbData (Fvb, &LbaIndex, &LbaOffset, sizeof (FvNameGuid), (UINT8 *) &FvNameGuid);
+        if (!EFI_ERROR (Status)) {
+          FvNameGuidIsFound = TRUE;
+        }
+      }
+      CoreFreePool (FwVolHeader);
+    }
+  }
+
+  if (FvNameGuidIsFound) {
+    //
+    // Check whether the FV image with the found FvNameGuid has been processed.
+    //
+    for (Link = mFvHandleList.ForwardLink; Link != &mFvHandleList; Link = Link->ForwardLink) {
+      KnownHandle = CR(Link, KNOWN_HANDLE, Link, KNOWN_HANDLE_SIGNATURE);
+      if (CompareGuid (&FvNameGuid, &KnownHandle->FvNameGuid)) {
+        DEBUG ((EFI_D_ERROR, "FvImage on FvHandle %p and %p has the same FvNameGuid %g.\n", FvHandle, KnownHandle->Handle, FvNameGuid));
+        return NULL;
+      }
+    }
+  }
+
+  KnownHandle = AllocateZeroPool (sizeof (KNOWN_HANDLE));
+  ASSERT (KnownHandle != NULL);
+
+  KnownHandle->Signature = KNOWN_HANDLE_SIGNATURE;
+  KnownHandle->Handle = FvHandle;
+  if (FvNameGuidIsFound) {
+    CopyGuid (&KnownHandle->FvNameGuid, &FvNameGuid);
+  }
+  InsertTailList (&mFvHandleList, &KnownHandle->Link);
+  return KnownHandle;
+}
+
+
+
+
+/**
+  Convert FvHandle and DriverName into an EFI device path
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_CORE_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+
+  @return Pointer to device path constructed from FvHandle and DriverName
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+CoreFvToDevicePath (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_DEVICE_PATH_PROTOCOL            *FvDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *FileNameDevicePath;
+
+  //
+  // Remember the device path of the FV
+  //
+  Status = CoreHandleProtocol (FvHandle, &gEfiDevicePathProtocolGuid, (VOID **)&FvDevicePath);
+  if (EFI_ERROR (Status)) {
+    FileNameDevicePath = NULL;
+  } else {
+    //
+    // Build a device path to the file in the FV to pass into gBS->LoadImage
+    //
+    EfiInitializeFwVolDevicepathNode (&mFvDevicePath.File, DriverName);
+    SetDevicePathEndNode (&mFvDevicePath.End);
+
+    FileNameDevicePath = AppendDevicePath (
+                            FvDevicePath,
+                            (EFI_DEVICE_PATH_PROTOCOL *)&mFvDevicePath
+                            );
+  }
+
+  return FileNameDevicePath;
+}
+
+
+
+/**
+  Add an entry to the mDiscoveredList. Allocate memory to store the DriverEntry,
+  and initilize any state variables. Read the Depex from the FV and store it
+  in DriverEntry. Pre-process the Depex to set the SOR, Before and After state.
+  The Discovered list is never free'ed and contains booleans that represent the
+  other possible DXE driver states.
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_CORE_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+  @param  Type                  Fv File Type of file to add to mDiscoveredList.
+
+  @retval EFI_SUCCESS           If driver was added to the mDiscoveredList.
+  @retval EFI_ALREADY_STARTED   The driver has already been started. Only one
+                                DriverName may be active in the system at any one
+                                time.
+
+**/
+EFI_STATUS
+CoreAddToDriverList (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName,
+  IN  EFI_FV_FILETYPE                 Type
+  )
+{
+  EFI_CORE_DRIVER_ENTRY               *DriverEntry;
+
+
+  //
+  // Create the Driver Entry for the list. ZeroPool initializes lots of variables to
+  // NULL or FALSE.
+  //
+  DriverEntry = AllocateZeroPool (sizeof (EFI_CORE_DRIVER_ENTRY));
+  ASSERT (DriverEntry != NULL);
+  if (Type == EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE) {
+    DriverEntry->IsFvImage = TRUE;
+  }
+
+  DriverEntry->Signature        = EFI_CORE_DRIVER_ENTRY_SIGNATURE;
+  CopyGuid (&DriverEntry->FileName, DriverName);
+  DriverEntry->FvHandle         = FvHandle;
+  DriverEntry->Fv               = Fv;
+  DriverEntry->FvFileDevicePath = CoreFvToDevicePath (Fv, FvHandle, DriverName);
+
+  CoreGetDepexSectionAndPreProccess (DriverEntry);
+
+  CoreAcquireDispatcherLock ();
+
+  InsertTailList (&mDiscoveredList, &DriverEntry->Link);
+
+  CoreReleaseDispatcherLock ();
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check if a FV Image type file (EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE) is
+  described by a EFI_HOB_FIRMWARE_VOLUME2 Hob.
+
+  @param  FvNameGuid            The FV image guid specified.
+  @param  DriverName            The driver guid specified.
+
+  @retval TRUE                  This file is found in a EFI_HOB_FIRMWARE_VOLUME2
+                                Hob.
+  @retval FALSE                 Not found.
+
+**/
+BOOLEAN
+FvFoundInHobFv2 (
+  IN  CONST EFI_GUID                  *FvNameGuid,
+  IN  CONST EFI_GUID                  *DriverName
+  )
+{
+  EFI_PEI_HOB_POINTERS                HobFv2;
+
+  HobFv2.Raw = GetHobList ();
+
+  while ((HobFv2.Raw = GetNextHob (EFI_HOB_TYPE_FV2, HobFv2.Raw)) != NULL) {
+    //
+    // Compare parent FvNameGuid and FileGuid both.
+    //
+    if (CompareGuid (DriverName, &HobFv2.FirmwareVolume2->FileName) &&
+        CompareGuid (FvNameGuid, &HobFv2.FirmwareVolume2->FvName)) {
+      return TRUE;
+    }
+    HobFv2.Raw = GET_NEXT_HOB (HobFv2);
+  }
+
+  return FALSE;
+}
+
+
+
+/**
+  Get the driver from the FV through driver name, and produce a FVB protocol on FvHandle.
+
+  @param  Fv                    The FIRMWARE_VOLUME protocol installed on the FV.
+  @param  FvHandle              The handle which FVB protocol installed on.
+  @param  DriverName            The driver guid specified.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough memory or other resource.
+  @retval EFI_VOLUME_CORRUPTED  Corrupted volume.
+  @retval EFI_SUCCESS           Function successfully returned.
+
+**/
+EFI_STATUS
+CoreProcessFvImageFile (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_SECTION_TYPE                    SectionType;
+  UINT32                              AuthenticationStatus;
+  VOID                                *Buffer;
+  VOID                                *AlignedBuffer;
+  UINTN                               BufferSize;
+  EFI_FIRMWARE_VOLUME_HEADER          *FvHeader;
+  UINT32                              FvAlignment;
+  EFI_DEVICE_PATH_PROTOCOL            *FvFileDevicePath;
+
+  //
+  // Read the first (and only the first) firmware volume section
+  //
+  SectionType   = EFI_SECTION_FIRMWARE_VOLUME_IMAGE;
+  FvHeader      = NULL;
+  FvAlignment   = 0;
+  Buffer        = NULL;
+  BufferSize    = 0;
+  AlignedBuffer = NULL;
+  Status = Fv->ReadSection (
+                 Fv,
+                 DriverName,
+                 SectionType,
+                 0,
+                 &Buffer,
+                 &BufferSize,
+                 &AuthenticationStatus
+                 );
+  if (!EFI_ERROR (Status)) {
+     //
+    // Evaluate the authentication status of the Firmware Volume through
+    // Security Architectural Protocol
+    //
+    if (gSecurity != NULL) {
+      FvFileDevicePath = CoreFvToDevicePath (Fv, FvHandle, DriverName);
+      Status = gSecurity->FileAuthenticationState (
+                            gSecurity,
+                            AuthenticationStatus,
+                            FvFileDevicePath
+                            );
+      if (FvFileDevicePath != NULL) {
+        FreePool (FvFileDevicePath);
+      }
+
+      if (Status != EFI_SUCCESS) {
+        //
+        // Security check failed. The firmware volume should not be used for any purpose.
+        //
+        if (Buffer != NULL) {
+          FreePool (Buffer);
+        }
+        return Status;
+      }
+    }
+
+    //
+    // FvImage should be at its required alignment.
+    //
+    FvHeader = (EFI_FIRMWARE_VOLUME_HEADER *) Buffer;
+    //
+    // If EFI_FVB2_WEAK_ALIGNMENT is set in the volume header then the first byte of the volume
+    // can be aligned on any power-of-two boundary. A weakly aligned volume can not be moved from
+    // its initial linked location and maintain its alignment.
+    //
+    if ((FvHeader->Attributes & EFI_FVB2_WEAK_ALIGNMENT) != EFI_FVB2_WEAK_ALIGNMENT) {
+      //
+      // Get FvHeader alignment
+      //
+      FvAlignment = 1 << ((FvHeader->Attributes & EFI_FVB2_ALIGNMENT) >> 16);
+      //
+      // FvAlignment must be greater than or equal to 8 bytes of the minimum FFS alignment value.
+      //
+      if (FvAlignment < 8) {
+        FvAlignment = 8;
+      }
+      //
+      // Allocate the aligned buffer for the FvImage.
+      //
+      AlignedBuffer = AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), (UINTN) FvAlignment);
+      if (AlignedBuffer == NULL) {
+        FreePool (Buffer);
+        return EFI_OUT_OF_RESOURCES;
+      } else {
+        //
+        // Move FvImage into the aligned buffer and release the original buffer.
+        //
+        CopyMem (AlignedBuffer, Buffer, BufferSize);
+        FvHeader = (EFI_FIRMWARE_VOLUME_HEADER *) AlignedBuffer;
+        CoreFreePool (Buffer);
+        Buffer = NULL;
+      }
+    }
+    //
+    // Produce a FVB protocol for the file
+    //
+    Status = ProduceFVBProtocolOnBuffer (
+              (EFI_PHYSICAL_ADDRESS) (UINTN) FvHeader,
+              (UINT64)BufferSize,
+              FvHandle,
+              AuthenticationStatus,
+              NULL
+              );
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // ReadSection or Produce FVB failed, Free data buffer
+    //
+    if (Buffer != NULL) {
+      FreePool (Buffer);
+    }
+
+    if (AlignedBuffer != NULL) {
+      FreeAlignedPages (AlignedBuffer, EFI_SIZE_TO_PAGES (BufferSize));
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Event notification that is fired every time a FV dispatch protocol is added.
+  More than one protocol may have been added when this event is fired, so you
+  must loop on CoreLocateHandle () to see how many protocols were added and
+  do the following to each FV:
+  If the Fv has already been processed, skip it. If the Fv has not been
+  processed then mark it as being processed, as we are about to process it.
+  Read the Fv and add any driver in the Fv to the mDiscoveredList.The
+  mDiscoveredList is never free'ed and contains variables that define
+  the other states the DXE driver transitions to..
+  While you are at it read the A Priori file into memory.
+  Place drivers in the A Priori list onto the mScheduledQueue.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+CoreFwVolEventProtocolNotify (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    GetNextFileStatus;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *Fv;
+  EFI_DEVICE_PATH_PROTOCOL      *FvDevicePath;
+  EFI_HANDLE                    FvHandle;
+  UINTN                         BufferSize;
+  EFI_GUID                      NameGuid;
+  UINTN                         Key;
+  EFI_FV_FILETYPE               Type;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  UINTN                         Size;
+  EFI_CORE_DRIVER_ENTRY         *DriverEntry;
+  EFI_GUID                      *AprioriFile;
+  UINTN                         AprioriEntryCount;
+  UINTN                         Index;
+  LIST_ENTRY                    *Link;
+  UINT32                        AuthenticationStatus;
+  UINTN                         SizeOfBuffer;
+  VOID                          *DepexBuffer;
+  KNOWN_HANDLE                  *KnownHandle;
+
+  FvHandle = NULL;
+
+  while (TRUE) {
+    BufferSize = sizeof (EFI_HANDLE);
+    Status = CoreLocateHandle (
+               ByRegisterNotify,
+               NULL,
+               mFwVolEventRegistration,
+               &BufferSize,
+               &FvHandle
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // If no more notification events exit
+      //
+      return;
+    }
+
+    if (FvHasBeenProcessed (FvHandle)) {
+      //
+      // This Fv has already been processed so lets skip it!
+      //
+      continue;
+    }
+
+    //
+    // Since we are about to process this Fv mark it as processed.
+    //
+    KnownHandle = FvIsBeingProcesssed (FvHandle);
+    if (KnownHandle == NULL) {
+      //
+      // The FV with the same FV name guid has already been processed. 
+      // So lets skip it!
+      //
+      continue;
+    }
+
+    Status = CoreHandleProtocol (FvHandle, &gEfiFirmwareVolume2ProtocolGuid, (VOID **)&Fv);
+    if (EFI_ERROR (Status) || Fv == NULL) {
+      //
+      // FvHandle must have Firmware Volume2 protocol thus we should never get here.
+      //
+      ASSERT (FALSE);
+      continue;
+    }
+
+    Status = CoreHandleProtocol (FvHandle, &gEfiDevicePathProtocolGuid, (VOID **)&FvDevicePath);
+    if (EFI_ERROR (Status)) {
+      //
+      // The Firmware volume doesn't have device path, can't be dispatched.
+      //
+      continue;
+    }
+
+    //
+    // Discover Drivers in FV and add them to the Discovered Driver List.
+    // Process EFI_FV_FILETYPE_DRIVER type and then EFI_FV_FILETYPE_COMBINED_PEIM_DRIVER
+    //  EFI_FV_FILETYPE_DXE_CORE is processed to produce a Loaded Image protocol for the core
+    //  EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE is processed to create a Fvb
+    //
+    for (Index = 0; Index < sizeof (mDxeFileTypes) / sizeof (EFI_FV_FILETYPE); Index++) {
+      //
+      // Initialize the search key
+      //
+      Key = 0;
+      do {
+        Type = mDxeFileTypes[Index];
+        GetNextFileStatus = Fv->GetNextFile (
+                                  Fv,
+                                  &Key,
+                                  &Type,
+                                  &NameGuid,
+                                  &Attributes,
+                                  &Size
+                                  );
+        if (!EFI_ERROR (GetNextFileStatus)) {
+          if (Type == EFI_FV_FILETYPE_DXE_CORE) {
+            //
+            // If this is the DXE core fill in it's DevicePath & DeviceHandle
+            //
+            if (gDxeCoreLoadedImage->FilePath == NULL) {
+              if (CompareGuid (&NameGuid, gDxeCoreFileName)) {
+                //
+                // Maybe One specail Fv cantains only one DXE_CORE module, so its device path must
+                // be initialized completely.
+                //
+                EfiInitializeFwVolDevicepathNode (&mFvDevicePath.File, &NameGuid);
+                SetDevicePathEndNode (&mFvDevicePath.End);
+
+                gDxeCoreLoadedImage->FilePath = DuplicateDevicePath (
+                                                  (EFI_DEVICE_PATH_PROTOCOL *)&mFvDevicePath
+                                                  );
+                gDxeCoreLoadedImage->DeviceHandle = FvHandle;
+              }
+            }
+          } else if (Type == EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE) {
+            //
+            // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has already
+            // been extracted.
+            //
+            if (FvFoundInHobFv2 (&KnownHandle->FvNameGuid, &NameGuid)) {
+              continue;
+            }
+
+            //
+            // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has PEI depex section.
+            //
+            DepexBuffer  = NULL;
+            SizeOfBuffer = 0;
+            Status = Fv->ReadSection (
+                           Fv,
+                           &NameGuid,
+                           EFI_SECTION_PEI_DEPEX,
+                           0,
+                           &DepexBuffer,
+                           &SizeOfBuffer,
+                           &AuthenticationStatus
+                           );
+            if (!EFI_ERROR (Status)) {
+              //
+              // If PEI depex section is found, this FV image will be ignored in DXE phase.
+              // Now, DxeCore doesn't support FV image with more one type DEPEX section.
+              //
+              FreePool (DepexBuffer);
+              continue;
+            }
+
+            //
+            // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has SMM depex section.
+            //
+            DepexBuffer  = NULL;
+            SizeOfBuffer = 0;
+            Status = Fv->ReadSection (
+                           Fv,
+                           &NameGuid,
+                           EFI_SECTION_SMM_DEPEX,
+                           0,
+                           &DepexBuffer,
+                           &SizeOfBuffer,
+                           &AuthenticationStatus
+                           );
+            if (!EFI_ERROR (Status)) {
+              //
+              // If SMM depex section is found, this FV image will be ignored in DXE phase.
+              // Now, DxeCore doesn't support FV image with more one type DEPEX section.
+              //
+              FreePool (DepexBuffer);
+              continue;
+            }
+
+            //
+            // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has DXE depex section.
+            //
+            DepexBuffer  = NULL;
+            SizeOfBuffer = 0;
+            Status = Fv->ReadSection (
+                           Fv,
+                           &NameGuid,
+                           EFI_SECTION_DXE_DEPEX,
+                           0,
+                           &DepexBuffer,
+                           &SizeOfBuffer,
+                           &AuthenticationStatus
+                           );
+            if (EFI_ERROR (Status)) {
+              //
+              // If no depex section, produce a firmware volume block protocol for it so it gets dispatched from. 
+              //
+              CoreProcessFvImageFile (Fv, FvHandle, &NameGuid);
+            } else {
+              //
+              // If depex section is found, this FV image will be dispatched until its depex is evaluated to TRUE.
+              //
+              FreePool (DepexBuffer);
+              CoreAddToDriverList (Fv, FvHandle, &NameGuid, Type);
+            }
+          } else {
+            //
+            // Transition driver from Undiscovered to Discovered state
+            //
+            CoreAddToDriverList (Fv, FvHandle, &NameGuid, Type);
+          }
+        }
+      } while (!EFI_ERROR (GetNextFileStatus));
+    }
+
+    //
+    // Read the array of GUIDs from the Apriori file if it is present in the firmware volume
+    //
+    AprioriFile = NULL;
+    Status = Fv->ReadSection (
+                  Fv,
+                  &gAprioriGuid,
+                  EFI_SECTION_RAW,
+                  0,
+                  (VOID **)&AprioriFile,
+                  &SizeOfBuffer,
+                  &AuthenticationStatus
+                  );
+    if (!EFI_ERROR (Status)) {
+      AprioriEntryCount = SizeOfBuffer / sizeof (EFI_GUID);
+    } else {
+      AprioriEntryCount = 0;
+    }
+
+    //
+    // Put drivers on Apriori List on the Scheduled queue. The Discovered List includes
+    // drivers not in the current FV and these must be skipped since the a priori list
+    // is only valid for the FV that it resided in.
+    //
+
+    for (Index = 0; Index < AprioriEntryCount; Index++) {
+      for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+        DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+        if (CompareGuid (&DriverEntry->FileName, &AprioriFile[Index]) &&
+            (FvHandle == DriverEntry->FvHandle)) {
+          CoreAcquireDispatcherLock ();
+          DriverEntry->Dependent = FALSE;
+          DriverEntry->Scheduled = TRUE;
+          InsertTailList (&mScheduledQueue, &DriverEntry->ScheduledLink);
+          CoreReleaseDispatcherLock ();
+          DEBUG ((DEBUG_DISPATCH, "Evaluate DXE DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = TRUE (Apriori)\n"));
+          break;
+        }
+      }
+    }
+
+    //
+    // Free data allocated by Fv->ReadSection ()
+    //
+    CoreFreePool (AprioriFile);
+  }
+}
+
+
+
+/**
+  Initialize the dispatcher. Initialize the notification function that runs when
+  an FV2 protocol is added to the system.
+
+**/
+VOID
+CoreInitializeDispatcher (
+  VOID
+  )
+{
+  mFwVolEvent = EfiCreateProtocolNotifyEvent (
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  TPL_CALLBACK,
+                  CoreFwVolEventProtocolNotify,
+                  NULL,
+                  &mFwVolEventRegistration
+                  );
+}
+
+//
+// Function only used in debug builds
+//
+
+/**
+  Traverse the discovered list for any drivers that were discovered but not loaded
+  because the dependency experessions evaluated to false.
+
+**/
+VOID
+CoreDisplayDiscoveredNotDispatched (
+  VOID
+  )
+{
+  LIST_ENTRY                    *Link;
+  EFI_CORE_DRIVER_ENTRY         *DriverEntry;
+
+  for (Link = mDiscoveredList.ForwardLink;Link !=&mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_CORE_DRIVER_ENTRY, Link, EFI_CORE_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->Dependent) {
+      DEBUG ((DEBUG_LOAD, "Driver %g was discovered but not loaded!!\n", &DriverEntry->FileName));
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeCore.uni b/Core/MdeModulePkg/Core/Dxe/DxeCore.uni
new file mode 100644
index 0000000000..9dd7d394d2
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeCore.uni
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeCoreExtra.uni b/Core/MdeModulePkg/Core/Dxe/DxeCoreExtra.uni
new file mode 100644
index 0000000000..094ad27f07
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeMain.h b/Core/MdeModulePkg/Core/Dxe/DxeMain.h
new file mode 100644
index 0000000000..fefe5bec19
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeMain.h
@@ -0,0 +1,2924 @@
+/** @file
+  The internal header file includes the common header files, defines
+  internal structure and functions used by DxeCore module.
+
+Copyright (c) 2006 - 2016, 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.
+
+**/
+
+#ifndef _DXE_MAIN_H_
+#define _DXE_MAIN_H_
+
+
+
+#include <PiDxe.h>
+
+#include <Protocol/LoadedImage.h>
+#include <Protocol/GuidedSectionExtraction.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/Runtime.h>
+#include <Protocol/LoadFile.h>
+#include <Protocol/LoadFile2.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/VariableWrite.h>
+#include <Protocol/PlatformDriverOverride.h>
+#include <Protocol/Variable.h>
+#include <Protocol/Timer.h>
+#include <Protocol/SimpleFileSystem.h>
+#include <Protocol/Bds.h>
+#include <Protocol/RealTimeClock.h>
+#include <Protocol/WatchdogTimer.h>
+#include <Protocol/FirmwareVolume2.h>
+#include <Protocol/MonotonicCounter.h>
+#include <Protocol/StatusCode.h>
+#include <Protocol/Decompress.h>
+#include <Protocol/LoadPe32Image.h>
+#include <Protocol/Security.h>
+#include <Protocol/Security2.h>
+#include <Protocol/Ebc.h>
+#include <Protocol/Reset.h>
+#include <Protocol/Cpu.h>
+#include <Protocol/Metronome.h>
+#include <Protocol/FirmwareVolumeBlock.h>
+#include <Protocol/Capsule.h>
+#include <Protocol/BusSpecificDriverOverride.h>
+#include <Protocol/DriverFamilyOverride.h>
+#include <Protocol/TcgService.h>
+#include <Protocol/HiiPackageList.h>
+#include <Protocol/SmmBase2.h>
+#include <Guid/MemoryTypeInformation.h>
+#include <Guid/FirmwareFileSystem2.h>
+#include <Guid/FirmwareFileSystem3.h>
+#include <Guid/HobList.h>
+#include <Guid/DebugImageInfoTable.h>
+#include <Guid/FileInfo.h>
+#include <Guid/Apriori.h>
+#include <Guid/DxeServices.h>
+#include <Guid/MemoryAllocationHob.h>
+#include <Guid/EventLegacyBios.h>
+#include <Guid/EventGroup.h>
+#include <Guid/EventExitBootServiceFailed.h>
+#include <Guid/LoadModuleAtFixedAddress.h>
+#include <Guid/IdleLoopEvent.h>
+#include <Guid/VectorHandoffTable.h>
+#include <Ppi/VectorHandoffInfo.h>
+#include <Guid/ZeroGuid.h>
+#include <Guid/MemoryProfile.h>
+
+#include <Library/DxeCoreEntryPoint.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/HobLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/UefiDecompressLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/PeCoffExtraActionLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/TimerLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/DebugAgentLib.h>
+#include <Library/CpuExceptionHandlerLib.h>
+
+
+//
+// attributes for reserved memory before it is promoted to system memory
+//
+#define EFI_MEMORY_PRESENT      0x0100000000000000ULL
+#define EFI_MEMORY_INITIALIZED  0x0200000000000000ULL
+#define EFI_MEMORY_TESTED       0x0400000000000000ULL
+
+//
+// range for memory mapped port I/O on IPF
+//
+#define EFI_MEMORY_PORT_IO  0x4000000000000000ULL
+
+
+///
+/// EFI_DEP_REPLACE_TRUE - Used to dynamically patch the dependecy expression
+///                        to save time.  A EFI_DEP_PUSH is evauated one an
+///                        replaced with EFI_DEP_REPLACE_TRUE. If PI spec's Vol 2
+///                        Driver Execution Environment Core Interface use 0xff
+///                        as new DEPEX opcode. EFI_DEP_REPLACE_TRUE should be
+///                        defined to a new value that is not conflicting with PI spec.
+///
+#define EFI_DEP_REPLACE_TRUE  0xff
+
+///
+/// Define the initial size of the dependency expression evaluation stack
+///
+#define DEPEX_STACK_SIZE_INCREMENT  0x1000
+
+#if defined (MDE_CPU_IPF)
+///
+/// For Itanium machines make the default allocations 8K aligned
+///
+#define EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT  (EFI_PAGE_SIZE * 2)
+#define DEFAULT_PAGE_ALLOCATION                     (EFI_PAGE_SIZE * 2)
+
+#elif defined (MDE_CPU_AARCH64)
+///
+/// 64-bit ARM systems allow the OS to execute with 64 KB page size,
+/// so for improved interoperability with the firmware, align the
+/// runtime regions to 64 KB as well
+///
+#define EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT  (SIZE_64KB)
+#define DEFAULT_PAGE_ALLOCATION                     (EFI_PAGE_SIZE)
+
+#else
+///
+/// For genric EFI machines make the default allocations 4K aligned
+///
+#define EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT  (EFI_PAGE_SIZE)
+#define DEFAULT_PAGE_ALLOCATION                     (EFI_PAGE_SIZE)
+
+#endif
+
+typedef struct {
+  EFI_GUID                    *ProtocolGuid;
+  VOID                        **Protocol;
+  EFI_EVENT                   Event;
+  VOID                        *Registration;
+  BOOLEAN                     Present;
+} EFI_CORE_PROTOCOL_NOTIFY_ENTRY;
+
+//
+// DXE Dispatcher Data structures
+//
+
+#define KNOWN_HANDLE_SIGNATURE  SIGNATURE_32('k','n','o','w')
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Link;         // mFvHandleList
+  EFI_HANDLE      Handle;
+  EFI_GUID        FvNameGuid;
+} KNOWN_HANDLE;
+
+
+#define EFI_CORE_DRIVER_ENTRY_SIGNATURE SIGNATURE_32('d','r','v','r')
+typedef struct {
+  UINTN                           Signature;
+  LIST_ENTRY                      Link;             // mDriverList
+
+  LIST_ENTRY                      ScheduledLink;    // mScheduledQueue
+
+  EFI_HANDLE                      FvHandle;
+  EFI_GUID                        FileName;
+  EFI_DEVICE_PATH_PROTOCOL        *FvFileDevicePath;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv;
+
+  VOID                            *Depex;
+  UINTN                           DepexSize;
+
+  BOOLEAN                         Before;
+  BOOLEAN                         After;
+  EFI_GUID                        BeforeAfterGuid;
+
+  BOOLEAN                         Dependent;
+  BOOLEAN                         Unrequested;
+  BOOLEAN                         Scheduled;
+  BOOLEAN                         Untrusted;
+  BOOLEAN                         Initialized;
+  BOOLEAN                         DepexProtocolError;
+
+  EFI_HANDLE                      ImageHandle;
+  BOOLEAN                         IsFvImage;
+
+} EFI_CORE_DRIVER_ENTRY;
+
+//
+//The data structure of GCD memory map entry
+//
+#define EFI_GCD_MAP_SIGNATURE  SIGNATURE_32('g','c','d','m')
+typedef struct {
+  UINTN                 Signature;
+  LIST_ENTRY            Link;
+  EFI_PHYSICAL_ADDRESS  BaseAddress;
+  UINT64                EndAddress;
+  UINT64                Capabilities;
+  UINT64                Attributes;
+  EFI_GCD_MEMORY_TYPE   GcdMemoryType;
+  EFI_GCD_IO_TYPE       GcdIoType;
+  EFI_HANDLE            ImageHandle;
+  EFI_HANDLE            DeviceHandle;
+} EFI_GCD_MAP_ENTRY;
+
+
+#define LOADED_IMAGE_PRIVATE_DATA_SIGNATURE   SIGNATURE_32('l','d','r','i')
+
+typedef struct {
+  UINTN                       Signature;
+  /// Image handle
+  EFI_HANDLE                  Handle;   
+  /// Image type
+  UINTN                       Type;           
+  /// If entrypoint has been called
+  BOOLEAN                     Started;        
+  /// The image's entry point
+  EFI_IMAGE_ENTRY_POINT       EntryPoint;     
+  /// loaded image protocol
+  EFI_LOADED_IMAGE_PROTOCOL   Info;           
+  /// Location in memory
+  EFI_PHYSICAL_ADDRESS        ImageBasePage;  
+  /// Number of pages
+  UINTN                       NumberOfPages;  
+  /// Original fixup data
+  CHAR8                       *FixupData;     
+  /// Tpl of started image
+  EFI_TPL                     Tpl;            
+  /// Status returned by started image
+  EFI_STATUS                  Status;         
+  /// Size of ExitData from started image
+  UINTN                       ExitDataSize;   
+  /// Pointer to exit data from started image
+  VOID                        *ExitData;      
+  /// Pointer to pool allocation for context save/retore
+  VOID                        *JumpBuffer;    
+  /// Pointer to buffer for context save/retore
+  BASE_LIBRARY_JUMP_BUFFER    *JumpContext;  
+  /// Machine type from PE image
+  UINT16                      Machine;        
+  /// EBC Protocol pointer
+  EFI_EBC_PROTOCOL            *Ebc;           
+  /// Runtime image list
+  EFI_RUNTIME_IMAGE_ENTRY     *RuntimeData;   
+  /// Pointer to Loaded Image Device Path Protocl
+  EFI_DEVICE_PATH_PROTOCOL    *LoadedImageDevicePath;  
+  /// PeCoffLoader ImageContext
+  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext; 
+  /// Status returned by LoadImage() service.
+  EFI_STATUS                  LoadImageStatus;
+} LOADED_IMAGE_PRIVATE_DATA;
+
+#define LOADED_IMAGE_PRIVATE_DATA_FROM_THIS(a) \
+          CR(a, LOADED_IMAGE_PRIVATE_DATA, Info, LOADED_IMAGE_PRIVATE_DATA_SIGNATURE)
+
+//
+// DXE Core Global Variables
+//
+extern EFI_SYSTEM_TABLE                         *gDxeCoreST;
+extern EFI_RUNTIME_SERVICES                     *gDxeCoreRT;
+extern EFI_DXE_SERVICES                         *gDxeCoreDS;
+extern EFI_HANDLE                               gDxeCoreImageHandle;
+
+extern EFI_DECOMPRESS_PROTOCOL                  gEfiDecompress;
+
+extern EFI_RUNTIME_ARCH_PROTOCOL                *gRuntime;
+extern EFI_CPU_ARCH_PROTOCOL                    *gCpu;
+extern EFI_WATCHDOG_TIMER_ARCH_PROTOCOL         *gWatchdogTimer;
+extern EFI_METRONOME_ARCH_PROTOCOL              *gMetronome;
+extern EFI_TIMER_ARCH_PROTOCOL                  *gTimer;
+extern EFI_SECURITY_ARCH_PROTOCOL               *gSecurity;
+extern EFI_SECURITY2_ARCH_PROTOCOL              *gSecurity2;
+extern EFI_BDS_ARCH_PROTOCOL                    *gBds;
+extern EFI_SMM_BASE2_PROTOCOL                   *gSmmBase2;
+
+extern EFI_TPL                                  gEfiCurrentTpl;
+
+extern EFI_GUID                                 *gDxeCoreFileName;
+extern EFI_LOADED_IMAGE_PROTOCOL                *gDxeCoreLoadedImage;
+
+extern EFI_MEMORY_TYPE_INFORMATION              gMemoryTypeInformation[EfiMaxMemoryType + 1];
+
+extern BOOLEAN                                  gDispatcherRunning;
+extern EFI_RUNTIME_ARCH_PROTOCOL                gRuntimeTemplate;
+
+extern EFI_LOAD_FIXED_ADDRESS_CONFIGURATION_TABLE    gLoadModuleAtFixAddressConfigurationTable;
+extern BOOLEAN                                       gLoadFixedAddressCodeMemoryReady;
+//
+// Service Initialization Functions
+//
+
+
+
+/**
+  Called to initialize the pool.
+
+**/
+VOID
+CoreInitializePool (
+  VOID
+  );
+
+
+/**
+  Called to initialize the memory map and add descriptors to
+  the current descriptor list.
+  The first descriptor that is added must be general usable
+  memory as the addition allocates heap.
+
+  @param  Type                   The type of memory to add
+  @param  Start                  The starting address in the memory range Must be
+                                 page aligned
+  @param  NumberOfPages          The number of pages in the range
+  @param  Attribute              Attributes of the memory to add
+
+  @return None.  The range is added to the memory map
+
+**/
+VOID
+CoreAddMemoryDescriptor (
+  IN EFI_MEMORY_TYPE       Type,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                NumberOfPages,
+  IN UINT64                Attribute
+  );
+
+
+/**
+  Release memory lock on mGcdMemorySpaceLock.
+
+**/
+VOID
+CoreReleaseGcdMemoryLock (
+  VOID
+  );
+
+
+/**
+  Acquire memory lock on mGcdMemorySpaceLock.
+
+**/
+VOID
+CoreAcquireGcdMemoryLock (
+  VOID
+  );
+
+
+/**
+  External function. Initializes memory services based on the memory
+  descriptor HOBs.  This function is responsible for priming the memory
+  map, so memory allocations and resource allocations can be made.
+  The first part of this function can not depend on any memory services
+  until at least one memory descriptor is provided to the memory services.
+
+  @param  HobStart               The start address of the HOB.
+  @param  MemoryBaseAddress      Start address of memory region found to init DXE
+                                 core.
+  @param  MemoryLength           Length of memory region found to init DXE core.
+
+  @retval EFI_SUCCESS            Memory services successfully initialized.
+
+**/
+EFI_STATUS
+CoreInitializeMemoryServices (
+  IN  VOID                  **HobStart,
+  OUT EFI_PHYSICAL_ADDRESS  *MemoryBaseAddress,
+  OUT UINT64                *MemoryLength
+  );
+
+
+
+/**
+  External function. Initializes the GCD and memory services based on the memory
+  descriptor HOBs.  This function is responsible for priming the GCD map and the
+  memory map, so memory allocations and resource allocations can be made. The
+  HobStart will be relocated to a pool buffer.
+
+  @param  HobStart               The start address of the HOB
+  @param  MemoryBaseAddress      Start address of memory region found to init DXE
+                                 core.
+  @param  MemoryLength           Length of memory region found to init DXE core.
+
+  @retval EFI_SUCCESS            GCD services successfully initialized.
+
+**/
+EFI_STATUS
+CoreInitializeGcdServices (
+  IN OUT VOID              **HobStart,
+  IN EFI_PHYSICAL_ADDRESS  MemoryBaseAddress,
+  IN UINT64                MemoryLength
+  );
+
+
+/**
+  Initializes "event" support.
+
+  @retval EFI_SUCCESS            Always return success
+
+**/
+EFI_STATUS
+CoreInitializeEventServices (
+  VOID
+  );
+
+
+/**
+  Add the Image Services to EFI Boot Services Table and install the protocol
+  interfaces for this image.
+
+  @param  HobStart                The HOB to initialize
+
+  @return Status code.
+
+**/
+EFI_STATUS
+CoreInitializeImageServices (
+  IN  VOID *HobStart
+  );
+
+
+/**
+  Creates an event that is fired everytime a Protocol of a specific type is installed.
+
+**/
+VOID
+CoreNotifyOnProtocolInstallation (
+  VOID
+  );
+
+
+/**
+  Return TRUE if all AP services are availible.
+
+  @retval EFI_SUCCESS    All AP services are available
+  @retval EFI_NOT_FOUND  At least one AP service is not available
+
+**/
+EFI_STATUS
+CoreAllEfiServicesAvailable (
+  VOID
+  );
+
+
+/**
+  Calcualte the 32-bit CRC in a EFI table using the service provided by the
+  gRuntime service.
+
+  @param  Hdr                    Pointer to an EFI standard header
+
+**/
+VOID
+CalculateEfiHdrCrc (
+  IN  OUT EFI_TABLE_HEADER    *Hdr
+  );
+
+
+/**
+  Called by the platform code to process a tick.
+
+  @param  Duration               The number of 100ns elasped since the last call
+                                 to TimerTick
+
+**/
+VOID
+EFIAPI
+CoreTimerTick (
+  IN UINT64   Duration
+  );
+
+
+/**
+  Initialize the dispatcher. Initialize the notification function that runs when
+  an FV2 protocol is added to the system.
+
+**/
+VOID
+CoreInitializeDispatcher (
+  VOID
+  );
+
+
+/**
+  This is the POSTFIX version of the dependency evaluator.  This code does
+  not need to handle Before or After, as it is not valid to call this
+  routine in this case. The SOR is just ignored and is a nop in the grammer.
+  POSTFIX means all the math is done on top of the stack.
+
+  @param  DriverEntry           DriverEntry element to update.
+
+  @retval TRUE                  If driver is ready to run.
+  @retval FALSE                 If driver is not ready to run or some fatal error
+                                was found.
+
+**/
+BOOLEAN
+CoreIsSchedulable (
+  IN  EFI_CORE_DRIVER_ENTRY   *DriverEntry
+  );
+
+
+/**
+  Preprocess dependency expression and update DriverEntry to reflect the
+  state of  Before, After, and SOR dependencies. If DriverEntry->Before
+  or DriverEntry->After is set it will never be cleared. If SOR is set
+  it will be cleared by CoreSchedule(), and then the driver can be
+  dispatched.
+
+  @param  DriverEntry           DriverEntry element to update .
+
+  @retval EFI_SUCCESS           It always works.
+
+**/
+EFI_STATUS
+CorePreProcessDepex (
+  IN  EFI_CORE_DRIVER_ENTRY   *DriverEntry
+  );
+
+
+
+/**
+  Terminates all boot services.
+
+  @param  ImageHandle            Handle that identifies the exiting image.
+  @param  MapKey                 Key to the latest memory map.
+
+  @retval EFI_SUCCESS            Boot Services terminated
+  @retval EFI_INVALID_PARAMETER  MapKey is incorrect.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreExitBootServices (
+  IN EFI_HANDLE   ImageHandle,
+  IN UINTN        MapKey
+  );
+
+
+/**
+  Make sure the memory map is following all the construction rules,
+  it is the last time to check memory map error before exit boot services.
+
+  @param  MapKey                 Memory map key
+
+  @retval EFI_INVALID_PARAMETER  Memory map not consistent with construction
+                                 rules.
+  @retval EFI_SUCCESS            Valid memory map.
+
+**/
+EFI_STATUS
+CoreTerminateMemoryMap (
+  IN UINTN          MapKey
+  );
+
+
+/**
+  Signals all events in the EventGroup.
+
+  @param  EventGroup             The list to signal
+
+**/
+VOID
+CoreNotifySignalList (
+  IN EFI_GUID     *EventGroup
+  );
+
+
+
+/**
+  Boot Service called to add, modify, or remove a system configuration table from
+  the EFI System Table.
+
+  @param  Guid           Pointer to the GUID for the entry to add, update, or
+                         remove
+  @param  Table          Pointer to the configuration table for the entry to add,
+                         update, or remove, may be NULL.
+
+  @return EFI_SUCCESS               Guid, Table pair added, updated, or removed.
+  @return EFI_INVALID_PARAMETER     Input GUID not valid.
+  @return EFI_NOT_FOUND             Attempted to delete non-existant entry
+  @return EFI_OUT_OF_RESOURCES      Not enough memory available
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallConfigurationTable (
+  IN EFI_GUID *Guid,
+  IN VOID     *Table
+  );
+
+
+
+/**
+  Raise the task priority level to the new level.
+  High level is implemented by disabling processor interrupts.
+
+  @param  NewTpl  New task priority level
+
+  @return The previous task priority level
+
+**/
+EFI_TPL
+EFIAPI
+CoreRaiseTpl (
+  IN EFI_TPL      NewTpl
+  );
+
+
+
+/**
+  Lowers the task priority to the previous value.   If the new
+  priority unmasks events at a higher priority, they are dispatched.
+
+  @param  NewTpl  New, lower, task priority
+
+**/
+VOID
+EFIAPI
+CoreRestoreTpl (
+  IN EFI_TPL NewTpl
+  );
+
+
+
+/**
+  Introduces a fine-grained stall.
+
+  @param  Microseconds           The number of microseconds to stall execution.
+
+  @retval EFI_SUCCESS            Execution was stalled for at least the requested
+                                 amount of microseconds.
+  @retval EFI_NOT_AVAILABLE_YET  gMetronome is not available yet
+
+**/
+EFI_STATUS
+EFIAPI
+CoreStall (
+  IN UINTN            Microseconds
+  );
+
+
+
+/**
+  Sets the system's watchdog timer.
+
+  @param  Timeout         The number of seconds to set the watchdog timer to.
+                          A value of zero disables the timer.
+  @param  WatchdogCode    The numeric code to log on a watchdog timer timeout
+                          event. The firmware reserves codes 0x0000 to 0xFFFF.
+                          Loaders and operating systems may use other timeout
+                          codes.
+  @param  DataSize        The size, in bytes, of WatchdogData.
+  @param  WatchdogData    A data buffer that includes a Null-terminated Unicode
+                          string, optionally followed by additional binary data.
+                          The string is a description that the call may use to
+                          further indicate the reason to be logged with a
+                          watchdog event.
+
+  @return EFI_SUCCESS               Timeout has been set
+  @return EFI_NOT_AVAILABLE_YET     WatchdogTimer is not available yet
+  @return EFI_UNSUPPORTED           System does not have a timer (currently not used)
+  @return EFI_DEVICE_ERROR          Could not complete due to hardware error
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetWatchdogTimer (
+  IN UINTN    Timeout,
+  IN UINT64   WatchdogCode,
+  IN UINTN    DataSize,
+  IN CHAR16   *WatchdogData OPTIONAL
+  );
+
+
+
+/**
+  Wrapper function to CoreInstallProtocolInterfaceNotify.  This is the public API which
+  Calls the private one which contains a BOOLEAN parameter for notifications
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallProtocolInterface (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface
+  );
+
+
+/**
+  Installs a protocol interface into the boot services environment.
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+  @param  Notify                 indicates whether notify the notification list
+                                 for this protocol
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Protocol interface successfully installed
+
+**/
+EFI_STATUS
+CoreInstallProtocolInterfaceNotify (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface,
+  IN BOOLEAN            Notify
+  );
+
+
+
+/**
+  Installs a list of protocol interface into the boot services environment.
+  This function calls InstallProtocolInterface() in a loop. If any error
+  occures all the protocols added by this function are removed. This is
+  basically a lib function to save space.
+
+  @param  Handle                 The handle to install the protocol handlers on,
+                                 or NULL if a new handle is to be allocated
+  @param  ...                    EFI_GUID followed by protocol instance. A NULL
+                                 terminates the  list. The pairs are the
+                                 arguments to InstallProtocolInterface(). All the
+                                 protocols are added to Handle.
+
+  @retval EFI_SUCCESS            All the protocol interface was installed.
+  @retval EFI_OUT_OF_RESOURCES   There was not enough memory in pool to install all the protocols.
+  @retval EFI_ALREADY_STARTED    A Device Path Protocol instance was passed in that is already present in
+                                 the handle database.
+  @retval EFI_INVALID_PARAMETER  Handle is NULL.
+  @retval EFI_INVALID_PARAMETER  Protocol is already installed on the handle specified by Handle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallMultipleProtocolInterfaces (
+  IN OUT EFI_HANDLE           *Handle,
+  ...
+  );
+
+
+
+/**
+  Uninstalls a list of protocol interface in the boot services environment.
+  This function calls UnisatllProtocolInterface() in a loop. This is
+  basically a lib function to save space.
+
+  @param  Handle                 The handle to uninstall the protocol
+  @param  ...                    EFI_GUID followed by protocol instance. A NULL
+                                 terminates the  list. The pairs are the
+                                 arguments to UninstallProtocolInterface(). All
+                                 the protocols are added to Handle.
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUninstallMultipleProtocolInterfaces (
+  IN EFI_HANDLE           Handle,
+  ...
+  );
+
+
+
+/**
+  Reinstall a protocol interface on a device handle.  The OldInterface for Protocol is replaced by the NewInterface.
+
+  @param  UserHandle             Handle on which the interface is to be
+                                 reinstalled
+  @param  Protocol               The numeric ID of the interface
+  @param  OldInterface           A pointer to the old interface
+  @param  NewInterface           A pointer to the new interface
+
+  @retval EFI_SUCCESS            The protocol interface was installed
+  @retval EFI_NOT_FOUND          The OldInterface on the handle was not found
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+
+**/
+EFI_STATUS
+EFIAPI
+CoreReinstallProtocolInterface (
+  IN EFI_HANDLE     UserHandle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *OldInterface,
+  IN VOID           *NewInterface
+  );
+
+
+
+/**
+  Uninstalls all instances of a protocol:interfacer from a handle.
+  If the last protocol interface is remove from the handle, the
+  handle is freed.
+
+  @param  UserHandle             The handle to remove the protocol handler from
+  @param  Protocol               The protocol, of protocol:interface, to remove
+  @param  Interface              The interface, of protocol:interface, to remove
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Protocol interface successfully uninstalled.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUninstallProtocolInterface (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  IN VOID             *Interface
+  );
+
+
+
+/**
+  Queries a handle to determine if it supports a specified protocol.
+
+  @param  UserHandle             The handle being queried.
+  @param  Protocol               The published unique identifier of the protocol.
+  @param  Interface              Supplies the address where a pointer to the
+                                 corresponding Protocol Interface is returned.
+
+  @return The requested protocol interface for the handle
+
+**/
+EFI_STATUS
+EFIAPI
+CoreHandleProtocol (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  OUT VOID            **Interface
+  );
+
+
+
+/**
+  Locates the installed protocol handler for the handle, and
+  invokes it to obtain the protocol interface. Usage information
+  is registered in the protocol data base.
+
+  @param  UserHandle             The handle to obtain the protocol interface on
+  @param  Protocol               The ID of the protocol
+  @param  Interface              The location to return the protocol interface
+  @param  ImageHandle            The handle of the Image that is opening the
+                                 protocol interface specified by Protocol and
+                                 Interface.
+  @param  ControllerHandle       The controller handle that is requiring this
+                                 interface.
+  @param  Attributes             The open mode of the protocol interface
+                                 specified by Handle and Protocol.
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Get the protocol interface.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreOpenProtocol (
+  IN  EFI_HANDLE                UserHandle,
+  IN  EFI_GUID                  *Protocol,
+  OUT VOID                      **Interface OPTIONAL,
+  IN  EFI_HANDLE                ImageHandle,
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  UINT32                    Attributes
+  );
+
+
+
+/**
+  Return information about Opened protocols in the system
+
+  @param  UserHandle             The handle to close the protocol interface on
+  @param  Protocol               The ID of the protocol
+  @param  EntryBuffer            A pointer to a buffer of open protocol
+                                 information in the form of
+                                 EFI_OPEN_PROTOCOL_INFORMATION_ENTRY structures.
+  @param  EntryCount             Number of EntryBuffer entries
+
+**/
+EFI_STATUS
+EFIAPI
+CoreOpenProtocolInformation (
+  IN  EFI_HANDLE                          UserHandle,
+  IN  EFI_GUID                            *Protocol,
+  OUT EFI_OPEN_PROTOCOL_INFORMATION_ENTRY **EntryBuffer,
+  OUT UINTN                               *EntryCount
+  );
+
+
+
+/**
+  Closes a protocol on a handle that was opened using OpenProtocol().
+
+  @param  UserHandle             The handle for the protocol interface that was
+                                 previously opened with OpenProtocol(), and is
+                                 now being closed.
+  @param  Protocol               The published unique identifier of the protocol.
+                                 It is the caller's responsibility to pass in a
+                                 valid GUID.
+  @param  AgentHandle            The handle of the agent that is closing the
+                                 protocol interface.
+  @param  ControllerHandle       If the agent that opened a protocol is a driver
+                                 that follows the EFI Driver Model, then this
+                                 parameter is the controller handle that required
+                                 the protocol interface. If the agent does not
+                                 follow the EFI Driver Model, then this parameter
+                                 is optional and may be NULL.
+
+  @retval EFI_SUCCESS            The protocol instance was closed.
+  @retval EFI_INVALID_PARAMETER  Handle, AgentHandle or ControllerHandle is not a
+                                 valid EFI_HANDLE.
+  @retval EFI_NOT_FOUND          Can not find the specified protocol or
+                                 AgentHandle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCloseProtocol (
+  IN  EFI_HANDLE                UserHandle,
+  IN  EFI_GUID                  *Protocol,
+  IN  EFI_HANDLE                AgentHandle,
+  IN  EFI_HANDLE                ControllerHandle
+  );
+
+
+
+/**
+  Retrieves the list of protocol interface GUIDs that are installed on a handle in a buffer allocated
+  from pool.
+
+  @param  UserHandle             The handle from which to retrieve the list of
+                                 protocol interface GUIDs.
+  @param  ProtocolBuffer         A pointer to the list of protocol interface GUID
+                                 pointers that are installed on Handle.
+  @param  ProtocolBufferCount    A pointer to the number of GUID pointers present
+                                 in ProtocolBuffer.
+
+  @retval EFI_SUCCESS            The list of protocol interface GUIDs installed
+                                 on Handle was returned in ProtocolBuffer. The
+                                 number of protocol interface GUIDs was returned
+                                 in ProtocolBufferCount.
+  @retval EFI_INVALID_PARAMETER  Handle is NULL.
+  @retval EFI_INVALID_PARAMETER  Handle is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER  ProtocolBuffer is NULL.
+  @retval EFI_INVALID_PARAMETER  ProtocolBufferCount is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the
+                                 results.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreProtocolsPerHandle (
+  IN EFI_HANDLE       UserHandle,
+  OUT EFI_GUID        ***ProtocolBuffer,
+  OUT UINTN           *ProtocolBufferCount
+  );
+
+
+
+/**
+  Add a new protocol notification record for the request protocol.
+
+  @param  Protocol               The requested protocol to add the notify
+                                 registration
+  @param  Event                  The event to signal
+  @param  Registration           Returns the registration record
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully returned the registration record
+                                 that has been added
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRegisterProtocolNotify (
+  IN EFI_GUID       *Protocol,
+  IN EFI_EVENT      Event,
+  OUT  VOID         **Registration
+  );
+
+
+/**
+  Removes all the events in the protocol database that match Event.
+
+  @param  Event                  The event to search for in the protocol
+                                 database.
+
+  @return EFI_SUCCESS when done searching the entire database.
+
+**/
+EFI_STATUS
+CoreUnregisterProtocolNotify (
+  IN EFI_EVENT      Event
+  );
+
+
+/**
+  Locates the requested handle(s) and returns them in Buffer.
+
+  @param  SearchType             The type of search to perform to locate the
+                                 handles
+  @param  Protocol               The protocol to search for
+  @param  SearchKey              Dependant on SearchType
+  @param  BufferSize             On input the size of Buffer.  On output the
+                                 size of data returned.
+  @param  Buffer                 The buffer to return the results in
+
+  @retval EFI_BUFFER_TOO_SMALL   Buffer too small, required buffer size is
+                                 returned in BufferSize.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully found the requested handle(s) and
+                                 returns them in Buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateHandle (
+  IN EFI_LOCATE_SEARCH_TYPE   SearchType,
+  IN EFI_GUID                 *Protocol   OPTIONAL,
+  IN VOID                     *SearchKey  OPTIONAL,
+  IN OUT UINTN                *BufferSize,
+  OUT EFI_HANDLE              *Buffer
+  );
+
+
+
+/**
+  Locates the handle to a device on the device path that best matches the specified protocol.
+
+  @param  Protocol               The protocol to search for.
+  @param  DevicePath             On input, a pointer to a pointer to the device
+                                 path. On output, the device path pointer is
+                                 modified to point to the remaining part of the
+                                 devicepath.
+  @param  Device                 A pointer to the returned device handle.
+
+  @retval EFI_SUCCESS            The resulting handle was returned.
+  @retval EFI_NOT_FOUND          No handles matched the search.
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateDevicePath (
+  IN EFI_GUID                       *Protocol,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath,
+  OUT EFI_HANDLE                    *Device
+  );
+
+
+
+/**
+  Function returns an array of handles that support the requested protocol
+  in a buffer allocated from pool. This is a version of CoreLocateHandle()
+  that allocates a buffer for the caller.
+
+  @param  SearchType             Specifies which handle(s) are to be returned.
+  @param  Protocol               Provides the protocol to search by.    This
+                                 parameter is only valid for SearchType
+                                 ByProtocol.
+  @param  SearchKey              Supplies the search key depending on the
+                                 SearchType.
+  @param  NumberHandles          The number of handles returned in Buffer.
+  @param  Buffer                 A pointer to the buffer to return the requested
+                                 array of  handles that support Protocol.
+
+  @retval EFI_SUCCESS            The result array of handles was returned.
+  @retval EFI_NOT_FOUND          No handles match the search.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the
+                                 matching results.
+  @retval EFI_INVALID_PARAMETER  One or more paramters are not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateHandleBuffer (
+  IN EFI_LOCATE_SEARCH_TYPE       SearchType,
+  IN EFI_GUID                     *Protocol OPTIONAL,
+  IN VOID                         *SearchKey OPTIONAL,
+  IN OUT UINTN                    *NumberHandles,
+  OUT EFI_HANDLE                  **Buffer
+  );
+
+
+
+/**
+  Return the first Protocol Interface that matches the Protocol GUID. If
+  Registration is passed in, return a Protocol Instance that was just add
+  to the system. If Registration is NULL return the first Protocol Interface
+  you find.
+
+  @param  Protocol               The protocol to search for
+  @param  Registration           Optional Registration Key returned from
+                                 RegisterProtocolNotify()
+  @param  Interface              Return the Protocol interface (instance).
+
+  @retval EFI_SUCCESS            If a valid Interface is returned
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Protocol interface not found
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateProtocol (
+  IN  EFI_GUID  *Protocol,
+  IN  VOID      *Registration OPTIONAL,
+  OUT VOID      **Interface
+  );
+
+
+/**
+  return handle database key.
+
+
+  @return Handle database key.
+
+**/
+UINT64
+CoreGetHandleDatabaseKey (
+  VOID
+  );
+
+
+/**
+  Go connect any handles that were created or modified while a image executed.
+
+  @param  Key                    The Key to show that the handle has been
+                                 created/modified
+
+**/
+VOID
+CoreConnectHandlesByKey (
+  UINT64  Key
+  );
+
+
+
+/**
+  Connects one or more drivers to a controller.
+
+  @param  ControllerHandle      The handle of the controller to which driver(s) are to be connected.
+  @param  DriverImageHandle     A pointer to an ordered list handles that support the
+                                EFI_DRIVER_BINDING_PROTOCOL.
+  @param  RemainingDevicePath   A pointer to the device path that specifies a child of the
+                                controller specified by ControllerHandle.
+  @param  Recursive             If TRUE, then ConnectController() is called recursively
+                                until the entire tree of controllers below the controller specified
+                                by ControllerHandle have been created. If FALSE, then
+                                the tree of controllers is only expanded one level.
+
+  @retval EFI_SUCCESS           1) One or more drivers were connected to ControllerHandle.
+                                2) No drivers were connected to ControllerHandle, but
+                                RemainingDevicePath is not NULL, and it is an End Device
+                                Path Node.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_NOT_FOUND         1) There are no EFI_DRIVER_BINDING_PROTOCOL instances
+                                present in the system.
+                                2) No drivers were connected to ControllerHandle.
+  @retval EFI_SECURITY_VIOLATION 
+                                The user has no permission to start UEFI device drivers on the device path 
+                                associated with the ControllerHandle or specified by the RemainingDevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreConnectController (
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  EFI_HANDLE                *DriverImageHandle    OPTIONAL,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath  OPTIONAL,
+  IN  BOOLEAN                   Recursive
+  );
+
+
+
+/**
+  Disonnects a controller from a driver
+
+  @param  ControllerHandle                      ControllerHandle The handle of
+                                                the controller from which
+                                                driver(s)  are to be
+                                                disconnected.
+  @param  DriverImageHandle                     DriverImageHandle The driver to
+                                                disconnect from ControllerHandle.
+  @param  ChildHandle                           ChildHandle The handle of the
+                                                child to destroy.
+
+  @retval EFI_SUCCESS                           One or more drivers were
+                                                disconnected from the controller.
+  @retval EFI_SUCCESS                           On entry, no drivers are managing
+                                                ControllerHandle.
+  @retval EFI_SUCCESS                           DriverImageHandle is not NULL,
+                                                and on entry DriverImageHandle is
+                                                not managing ControllerHandle.
+  @retval EFI_INVALID_PARAMETER                 ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER                 DriverImageHandle is not NULL,
+                                                and it is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER                 ChildHandle is not NULL, and it
+                                                is not a valid EFI_HANDLE.
+  @retval EFI_OUT_OF_RESOURCES                  There are not enough resources
+                                                available to disconnect any
+                                                drivers from ControllerHandle.
+  @retval EFI_DEVICE_ERROR                      The controller could not be
+                                                disconnected because of a device
+                                                error.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreDisconnectController (
+  IN  EFI_HANDLE  ControllerHandle,
+  IN  EFI_HANDLE  DriverImageHandle  OPTIONAL,
+  IN  EFI_HANDLE  ChildHandle        OPTIONAL
+  );
+
+
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @return Status. On success, Memory is filled in with the base address allocated
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in
+                                 spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePages (
+  IN EFI_ALLOCATE_TYPE      Type,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  NumberOfPages,
+  IN OUT EFI_PHYSICAL_ADDRESS  *Memory
+  );
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @return Status. On success, Memory is filled in with the base address allocated
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in
+                                 spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalAllocatePages (
+  IN EFI_ALLOCATE_TYPE      Type,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  NumberOfPages,
+  IN OUT EFI_PHYSICAL_ADDRESS  *Memory
+  );
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS         -Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  );
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS         -Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalFreePages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  );
+
+/**
+  This function returns a copy of the current memory map. The map is an array of
+  memory descriptors, each of which describes a contiguous block of memory.
+
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 the buffer allocated by the caller.  On output,
+                                 it is the size of the buffer returned by the
+                                 firmware  if the buffer was large enough, or the
+                                 size of the buffer needed  to contain the map if
+                                 the buffer was too small.
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MapKey                 A pointer to the location in which firmware
+                                 returns the key for the current memory map.
+  @param  DescriptorSize         A pointer to the location in which firmware
+                                 returns the size, in bytes, of an individual
+                                 EFI_MEMORY_DESCRIPTOR.
+  @param  DescriptorVersion      A pointer to the location in which firmware
+                                 returns the version number associated with the
+                                 EFI_MEMORY_DESCRIPTOR.
+
+  @retval EFI_SUCCESS            The memory map was returned in the MemoryMap
+                                 buffer.
+  @retval EFI_BUFFER_TOO_SMALL   The MemoryMap buffer was too small. The current
+                                 buffer size needed to hold the memory map is
+                                 returned in MemoryMapSize.
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemoryMap (
+  IN OUT UINTN                  *MemoryMapSize,
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  OUT UINTN                     *MapKey,
+  OUT UINTN                     *DescriptorSize,
+  OUT UINT32                    *DescriptorVersion
+  );
+
+
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid or Buffer is NULL
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size,
+  OUT VOID            **Buffer
+  );
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid or Buffer is NULL
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalAllocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size,
+  OUT VOID            **Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePool (
+  IN VOID        *Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalFreePool (
+  IN VOID        *Buffer
+  );
+
+/**
+  Loads an EFI image into memory and returns a handle to the image.
+
+  @param  BootPolicy              If TRUE, indicates that the request originates
+                                  from the boot manager, and that the boot
+                                  manager is attempting to load FilePath as a
+                                  boot selection.
+  @param  ParentImageHandle       The caller's image handle.
+  @param  FilePath                The specific file path from which the image is
+                                  loaded.
+  @param  SourceBuffer            If not NULL, a pointer to the memory location
+                                  containing a copy of the image to be loaded.
+  @param  SourceSize              The size in bytes of SourceBuffer.
+  @param  ImageHandle             Pointer to the returned image handle that is
+                                  created when the image is successfully loaded.
+
+  @retval EFI_SUCCESS             The image was loaded into memory.
+  @retval EFI_NOT_FOUND           The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_UNSUPPORTED         The image type is not supported, or the device
+                                  path cannot be parsed to locate the proper
+                                  protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES    Image was not loaded due to insufficient
+                                  resources.
+  @retval EFI_LOAD_ERROR          Image was not loaded because the image format was corrupt or not
+                                  understood.
+  @retval EFI_DEVICE_ERROR        Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED       Image was not loaded because the platform policy prohibits the 
+                                  image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION  Image was loaded and an ImageHandle was created with a 
+                                  valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                  platform policy specifies that the image should not be started.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLoadImage (
+  IN BOOLEAN                    BootPolicy,
+  IN EFI_HANDLE                 ParentImageHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL   *FilePath,
+  IN VOID                       *SourceBuffer   OPTIONAL,
+  IN UINTN                      SourceSize,
+  OUT EFI_HANDLE                *ImageHandle
+  );
+
+
+
+/**
+  Unloads an image.
+
+  @param  ImageHandle             Handle that identifies the image to be
+                                  unloaded.
+
+  @retval EFI_SUCCESS             The image has been unloaded.
+  @retval EFI_UNSUPPORTED         The image has been sarted, and does not support
+                                  unload.
+  @retval EFI_INVALID_PARAMPETER  ImageHandle is not a valid image handle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUnloadImage (
+  IN EFI_HANDLE  ImageHandle
+  );
+
+
+
+/**
+  Transfer control to a loaded image's entry point.
+
+  @param  ImageHandle             Handle of image to be started.
+  @param  ExitDataSize            Pointer of the size to ExitData
+  @param  ExitData                Pointer to a pointer to a data buffer that
+                                  includes a Null-terminated string,
+                                  optionally followed by additional binary data.
+                                  The string is a description that the caller may
+                                  use to further indicate the reason for the
+                                  image's exit.
+
+  @retval EFI_INVALID_PARAMETER   Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+  @retval EFI_SECURITY_VIOLATION  The current platform policy specifies that the image should not be started.
+  @retval EFI_SUCCESS             Successfully transfer control to the image's
+                                  entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreStartImage (
+  IN EFI_HANDLE  ImageHandle,
+  OUT UINTN      *ExitDataSize,
+  OUT CHAR16     **ExitData  OPTIONAL
+  );
+
+
+
+/**
+  Terminates the currently loaded EFI image and returns control to boot services.
+
+  @param  ImageHandle             Handle that identifies the image. This
+                                  parameter is passed to the image on entry.
+  @param  Status                  The image's exit code.
+  @param  ExitDataSize            The size, in bytes, of ExitData. Ignored if
+                                  ExitStatus is EFI_SUCCESS.
+  @param  ExitData                Pointer to a data buffer that includes a
+                                  Null-terminated Unicode string, optionally
+                                  followed by additional binary data. The string
+                                  is a description that the caller may use to
+                                  further indicate the reason for the image's
+                                  exit.
+
+  @retval EFI_INVALID_PARAMETER   Image handle is NULL or it is not current
+                                  image.
+  @retval EFI_SUCCESS             Successfully terminates the currently loaded
+                                  EFI image.
+  @retval EFI_ACCESS_DENIED       Should never reach there.
+  @retval EFI_OUT_OF_RESOURCES    Could not allocate pool
+
+**/
+EFI_STATUS
+EFIAPI
+CoreExit (
+  IN EFI_HANDLE  ImageHandle,
+  IN EFI_STATUS  Status,
+  IN UINTN       ExitDataSize,
+  IN CHAR16      *ExitData  OPTIONAL
+  );
+
+
+
+/**
+  Creates an event.
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEvent (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN VOID                     *NotifyContext, OPTIONAL
+  OUT EFI_EVENT               *Event
+  );
+
+
+
+/**
+  Creates an event in a group.
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  EventGroup             GUID for EventGroup if NULL act the same as
+                                 gBS->CreateEvent().
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEventEx (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN CONST VOID               *NotifyContext, OPTIONAL
+  IN CONST EFI_GUID           *EventGroup,    OPTIONAL
+  OUT EFI_EVENT               *Event
+  );
+
+/**
+  Creates a general-purpose event structure
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  EventGroup             GUID for EventGroup if NULL act the same as
+                                 gBS->CreateEvent().
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEventInternal (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN CONST VOID               *NotifyContext, OPTIONAL
+  IN CONST EFI_GUID           *EventGroup,    OPTIONAL
+  OUT EFI_EVENT               *Event
+  );
+
+/**
+  Sets the type of timer and the trigger time for a timer event.
+
+  @param  UserEvent              The timer event that is to be signaled at the
+                                 specified time
+  @param  Type                   The type of time that is specified in
+                                 TriggerTime
+  @param  TriggerTime            The number of 100ns units until the timer
+                                 expires
+
+  @retval EFI_SUCCESS            The event has been set to be signaled at the
+                                 requested time
+  @retval EFI_INVALID_PARAMETER  Event or Type is not valid
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetTimer (
+  IN EFI_EVENT            UserEvent,
+  IN EFI_TIMER_DELAY      Type,
+  IN UINT64               TriggerTime
+  );
+
+
+
+/**
+  Signals the event.  Queues the event to be notified if needed.
+
+  @param  UserEvent              The event to signal .
+
+  @retval EFI_INVALID_PARAMETER  Parameters are not valid.
+  @retval EFI_SUCCESS            The event was signaled.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSignalEvent (
+  IN EFI_EVENT    UserEvent
+  );
+
+
+
+/**
+  Stops execution until an event is signaled.
+
+  @param  NumberOfEvents         The number of events in the UserEvents array
+  @param  UserEvents             An array of EFI_EVENT
+  @param  UserIndex              Pointer to the index of the event which
+                                 satisfied the wait condition
+
+  @retval EFI_SUCCESS            The event indicated by Index was signaled.
+  @retval EFI_INVALID_PARAMETER  The event indicated by Index has a notification
+                                 function or Event was not a valid type
+  @retval EFI_UNSUPPORTED        The current TPL is not TPL_APPLICATION
+
+**/
+EFI_STATUS
+EFIAPI
+CoreWaitForEvent (
+  IN UINTN        NumberOfEvents,
+  IN EFI_EVENT    *UserEvents,
+  OUT UINTN       *UserIndex
+  );
+
+
+
+/**
+  Closes an event and frees the event structure.
+
+  @param  UserEvent              Event to close
+
+  @retval EFI_INVALID_PARAMETER  Parameters are not valid.
+  @retval EFI_SUCCESS            The event has been closed
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCloseEvent (
+  IN EFI_EVENT    UserEvent
+  );
+
+
+
+/**
+  Check the status of an event.
+
+  @param  UserEvent              The event to check
+
+  @retval EFI_SUCCESS            The event is in the signaled state
+  @retval EFI_NOT_READY          The event is not in the signaled state
+  @retval EFI_INVALID_PARAMETER  Event is of type EVT_NOTIFY_SIGNAL
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCheckEvent (
+  IN EFI_EVENT        UserEvent
+  );
+
+
+/**
+  Adds reserved memory, system memory, or memory-mapped I/O resources to the
+  global coherency domain of the processor.
+
+  @param  GcdMemoryType          Memory type of the memory space.
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+  @param  Capabilities           alterable attributes of the memory space.
+
+  @retval EFI_SUCCESS            Merged this memory space into GCD map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAddMemorySpace (
+  IN EFI_GCD_MEMORY_TYPE   GcdMemoryType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities
+  );
+
+
+/**
+  Allocates nonexistent memory, reserved memory, system memory, or memorymapped
+  I/O resources from the global coherency domain of the processor.
+
+  @param  GcdAllocateType        The type of allocate operation
+  @param  GcdMemoryType          The desired memory type
+  @param  Alignment              Align with 2^Alignment
+  @param  Length                 Length to allocate
+  @param  BaseAddress            Base address to allocate
+  @param  ImageHandle            The image handle consume the allocated space.
+  @param  DeviceHandle           The device handle consume the allocated space.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_NOT_FOUND          No descriptor contains the desired space.
+  @retval EFI_SUCCESS            Memory space successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocateMemorySpace (
+  IN     EFI_GCD_ALLOCATE_TYPE  GcdAllocateType,
+  IN     EFI_GCD_MEMORY_TYPE    GcdMemoryType,
+  IN     UINTN                  Alignment,
+  IN     UINT64                 Length,
+  IN OUT EFI_PHYSICAL_ADDRESS   *BaseAddress,
+  IN     EFI_HANDLE             ImageHandle,
+  IN     EFI_HANDLE             DeviceHandle OPTIONAL
+  );
+
+
+/**
+  Frees nonexistent memory, reserved memory, system memory, or memory-mapped
+  I/O resources from the global coherency domain of the processor.
+
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+
+  @retval EFI_SUCCESS            Space successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreeMemorySpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  );
+
+
+/**
+  Removes reserved memory, system memory, or memory-mapped I/O resources from
+  the global coherency domain of the processor.
+
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+
+  @retval EFI_SUCCESS            Successfully remove a segment of memory space.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRemoveMemorySpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  );
+
+
+/**
+  Retrieves the descriptor for a memory region containing a specified address.
+
+  @param  BaseAddress            Specified start address
+  @param  Descriptor             Specified length
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully get memory space descriptor.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemorySpaceDescriptor (
+  IN  EFI_PHYSICAL_ADDRESS             BaseAddress,
+  OUT EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *Descriptor
+  );
+
+
+/**
+  Modifies the attributes for a memory region in the global coherency domain of the
+  processor.
+
+  @param  BaseAddress            Specified start address
+  @param  Length                 Specified length
+  @param  Attributes             Specified attributes
+
+  @retval EFI_SUCCESS           The attributes were set for the memory region.
+  @retval EFI_INVALID_PARAMETER Length is zero. 
+  @retval EFI_UNSUPPORTED       The processor does not support one or more bytes of the memory
+                                resource range specified by BaseAddress and Length.
+  @retval EFI_UNSUPPORTED       The bit mask of attributes is not support for the memory resource
+                                range specified by BaseAddress and Length.
+  @retval EFI_ACCESS_DENIED     The attributes for the memory resource range specified by
+                                BaseAddress and Length cannot be modified.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough system resources to modify the attributes of
+                                the memory resource range.
+  @retval EFI_NOT_AVAILABLE_YET The attributes cannot be set because CPU architectural protocol is
+                                not available yet.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetMemorySpaceAttributes (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  );
+
+
+/**
+  Modifies the capabilities for a memory region in the global coherency domain of the
+  processor.
+
+  @param  BaseAddress      The physical address that is the start address of a memory region.
+  @param  Length           The size in bytes of the memory region.
+  @param  Capabilities     The bit mask of capabilities that the memory region supports.
+
+  @retval EFI_SUCCESS           The capabilities were set for the memory region.
+  @retval EFI_INVALID_PARAMETER Length is zero.
+  @retval EFI_UNSUPPORTED       The capabilities specified by Capabilities do not include the
+                                memory region attributes currently in use.
+  @retval EFI_ACCESS_DENIED     The capabilities for the memory resource range specified by
+                                BaseAddress and Length cannot be modified.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough system resources to modify the capabilities
+                                of the memory resource range.
+**/
+EFI_STATUS
+EFIAPI
+CoreSetMemorySpaceCapabilities (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities
+  );
+
+
+/**
+  Returns a map of the memory resources in the global coherency domain of the
+  processor.
+
+  @param  NumberOfDescriptors    Number of descriptors.
+  @param  MemorySpaceMap         Descriptor array
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Successfully get memory space map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemorySpaceMap (
+  OUT UINTN                            *NumberOfDescriptors,
+  OUT EFI_GCD_MEMORY_SPACE_DESCRIPTOR  **MemorySpaceMap
+  );
+
+
+/**
+  Adds reserved I/O or I/O resources to the global coherency domain of the processor.
+
+  @param  GcdIoType              IO type of the segment.
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Merged this segment into GCD map.
+  @retval EFI_INVALID_PARAMETER  Parameter not valid
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAddIoSpace (
+  IN EFI_GCD_IO_TYPE       GcdIoType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  );
+
+
+/**
+  Allocates nonexistent I/O, reserved I/O, or I/O resources from the global coherency
+  domain of the processor.
+
+  @param  GcdAllocateType        The type of allocate operation
+  @param  GcdIoType              The desired IO type
+  @param  Alignment              Align with 2^Alignment
+  @param  Length                 Length to allocate
+  @param  BaseAddress            Base address to allocate
+  @param  ImageHandle            The image handle consume the allocated space.
+  @param  DeviceHandle           The device handle consume the allocated space.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_NOT_FOUND          No descriptor contains the desired space.
+  @retval EFI_SUCCESS            IO space successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocateIoSpace (
+  IN     EFI_GCD_ALLOCATE_TYPE  GcdAllocateType,
+  IN     EFI_GCD_IO_TYPE        GcdIoType,
+  IN     UINTN                  Alignment,
+  IN     UINT64                 Length,
+  IN OUT EFI_PHYSICAL_ADDRESS   *BaseAddress,
+  IN     EFI_HANDLE             ImageHandle,
+  IN     EFI_HANDLE             DeviceHandle OPTIONAL
+  );
+
+
+/**
+  Frees nonexistent I/O, reserved I/O, or I/O resources from the global coherency
+  domain of the processor.
+
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Space successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreeIoSpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  );
+
+
+/**
+  Removes reserved I/O or I/O resources from the global coherency domain of the
+  processor.
+
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Successfully removed a segment of IO space.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRemoveIoSpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  );
+
+
+/**
+  Retrieves the descriptor for an I/O region containing a specified address.
+
+  @param  BaseAddress            Specified start address
+  @param  Descriptor             Specified length
+
+  @retval EFI_INVALID_PARAMETER  Descriptor is NULL.
+  @retval EFI_SUCCESS            Successfully get the IO space descriptor.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetIoSpaceDescriptor (
+  IN  EFI_PHYSICAL_ADDRESS         BaseAddress,
+  OUT EFI_GCD_IO_SPACE_DESCRIPTOR  *Descriptor
+  );
+
+
+/**
+  Returns a map of the I/O resources in the global coherency domain of the processor.
+
+  @param  NumberOfDescriptors    Number of descriptors.
+  @param  IoSpaceMap             Descriptor array
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Successfully get IO space map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetIoSpaceMap (
+  OUT UINTN                        *NumberOfDescriptors,
+  OUT EFI_GCD_IO_SPACE_DESCRIPTOR  **IoSpaceMap
+  );
+
+
+/**
+  This is the main Dispatcher for DXE and it exits when there are no more
+  drivers to run. Drain the mScheduledQueue and load and start a PE
+  image for each driver. Search the mDiscoveredList to see if any driver can
+  be placed on the mScheduledQueue. If no drivers are placed on the
+  mScheduledQueue exit the function. On exit it is assumed the Bds()
+  will be called, and when the Bds() exits the Dispatcher will be called
+  again.
+
+  @retval EFI_ALREADY_STARTED   The DXE Dispatcher is already running
+  @retval EFI_NOT_FOUND         No DXE Drivers were dispatched
+  @retval EFI_SUCCESS           One or more DXE Drivers were dispatched
+
+**/
+EFI_STATUS
+EFIAPI
+CoreDispatcher (
+  VOID
+  );
+
+/**
+  Check every driver and locate a matching one. If the driver is found, the Unrequested
+  state flag is cleared.
+
+  @param  FirmwareVolumeHandle  The handle of the Firmware Volume that contains
+                                the firmware  file specified by DriverName.
+  @param  DriverName            The Driver name to put in the Dependent state.
+
+  @retval EFI_SUCCESS           The DriverName was found and it's SOR bit was
+                                cleared
+  @retval EFI_NOT_FOUND         The DriverName does not exist or it's SOR bit was
+                                not set.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSchedule (
+  IN  EFI_HANDLE  FirmwareVolumeHandle,
+  IN  EFI_GUID    *DriverName
+  );
+
+
+/**
+  Convert a driver from the Untrused back to the Scheduled state.
+
+  @param  FirmwareVolumeHandle  The handle of the Firmware Volume that contains
+                                the firmware  file specified by DriverName.
+  @param  DriverName            The Driver name to put in the Scheduled state
+
+  @retval EFI_SUCCESS           The file was found in the untrusted state, and it
+                                was promoted  to the trusted state.
+  @retval EFI_NOT_FOUND         The file was not found in the untrusted state.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreTrust (
+  IN  EFI_HANDLE  FirmwareVolumeHandle,
+  IN  EFI_GUID    *DriverName
+  );
+
+
+/**
+  This routine is the driver initialization entry point.  It initializes the
+  libraries, and registers two notification functions.  These notification
+  functions are responsible for building the FV stack dynamically.
+
+  @param  ImageHandle           The image handle.
+  @param  SystemTable           The system table.
+
+  @retval EFI_SUCCESS           Function successfully returned.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolDriverInit (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  );
+
+
+/**
+  Entry point of the section extraction code. Initializes an instance of the
+  section extraction interface and installs it on a new handle.
+
+  @param  ImageHandle   A handle for the image that is initializing this driver
+  @param  SystemTable   A pointer to the EFI system table
+
+  @retval EFI_SUCCESS           Driver initialized successfully
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate needed resources
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeSectionExtraction (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  );
+
+
+/**
+  This DXE service routine is used to process a firmware volume. In
+  particular, it can be called by BDS to process a single firmware
+  volume found in a capsule.
+
+  @param  FvHeader               pointer to a firmware volume header
+  @param  Size                   the size of the buffer pointed to by FvHeader
+  @param  FVProtocolHandle       the handle on which a firmware volume protocol
+                                 was produced for the firmware volume passed in.
+
+  @retval EFI_OUT_OF_RESOURCES   if an FVB could not be produced due to lack of
+                                 system resources
+  @retval EFI_VOLUME_CORRUPTED   if the volume was corrupted
+  @retval EFI_SUCCESS            a firmware volume protocol was produced for the
+                                 firmware volume
+
+**/
+EFI_STATUS
+EFIAPI
+CoreProcessFirmwareVolume (
+  IN VOID                             *FvHeader,
+  IN UINTN                            Size,
+  OUT EFI_HANDLE                      *FVProtocolHandle
+  );
+
+//
+//Functions used during debug buils
+//
+
+/**
+  Displays Architectural protocols that were not loaded and are required for DXE
+  core to function.  Only used in Debug Builds.
+
+**/
+VOID
+CoreDisplayMissingArchProtocols (
+  VOID
+  );
+
+
+/**
+  Traverse the discovered list for any drivers that were discovered but not loaded
+  because the dependency experessions evaluated to false.
+
+**/
+VOID
+CoreDisplayDiscoveredNotDispatched (
+  VOID
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are
+  available.
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg0 (
+  VOID
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are
+  available.
+
+  @param  Arg1                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg1 (
+  UINTN Arg1
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg2 (
+  UINTN Arg1,
+  UINTN Arg2
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg3 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg4 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4
+  );
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+  @param  Arg5                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg5 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4,
+  UINTN Arg5
+  );
+
+
+/**
+  Given a compressed source buffer, this function retrieves the size of the
+  uncompressed buffer and the size of the scratch buffer required to decompress
+  the compressed source buffer.
+
+  The GetInfo() function retrieves the size of the uncompressed buffer and the
+  temporary scratch buffer required to decompress the buffer specified by Source
+  and SourceSize. If the size of the uncompressed buffer or the size of the
+  scratch buffer cannot be determined from the compressed data specified by
+  Source and SourceData, then EFI_INVALID_PARAMETER is returned. Otherwise, the
+  size of the uncompressed buffer is returned in DestinationSize, the size of
+  the scratch buffer is returned in ScratchSize, and EFI_SUCCESS is returned.
+  The GetInfo() function does not have scratch buffer available to perform a
+  thorough checking of the validity of the source data. It just retrieves the
+  "Original Size" field from the beginning bytes of the source data and output
+  it as DestinationSize. And ScratchSize is specific to the decompression
+  implementation.
+
+  @param  This               A pointer to the EFI_DECOMPRESS_PROTOCOL instance.
+  @param  Source             The source buffer containing the compressed data.
+  @param  SourceSize         The size, in bytes, of the source buffer.
+  @param  DestinationSize    A pointer to the size, in bytes, of the
+                             uncompressed buffer that will be generated when the
+                             compressed buffer specified by Source and
+                             SourceSize is decompressed.
+  @param  ScratchSize        A pointer to the size, in bytes, of the scratch
+                             buffer that is required to decompress the
+                             compressed buffer specified by Source and
+                             SourceSize.
+
+  @retval EFI_SUCCESS        The size of the uncompressed data was returned in
+                             DestinationSize and the size of the scratch buffer
+                             was returned in ScratchSize.
+  @retval EFI_INVALID_PARAMETER The size of the uncompressed data or the size of
+                                the scratch buffer cannot be determined from the
+                                compressed data specified by Source and
+                                SourceSize.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeMainUefiDecompressGetInfo (
+  IN EFI_DECOMPRESS_PROTOCOL            *This,
+  IN   VOID                             *Source,
+  IN   UINT32                           SourceSize,
+  OUT  UINT32                           *DestinationSize,
+  OUT  UINT32                           *ScratchSize
+  );
+
+
+/**
+  Decompresses a compressed source buffer.
+
+  The Decompress() function extracts decompressed data to its original form.
+  This protocol is designed so that the decompression algorithm can be
+  implemented without using any memory services. As a result, the Decompress()
+  Function is not allowed to call AllocatePool() or AllocatePages() in its
+  implementation. It is the caller's responsibility to allocate and free the
+  Destination and Scratch buffers.
+  If the compressed source data specified by Source and SourceSize is
+  sucessfully decompressed into Destination, then EFI_SUCCESS is returned. If
+  the compressed source data specified by Source and SourceSize is not in a
+  valid compressed data format, then EFI_INVALID_PARAMETER is returned.
+
+  @param  This                A pointer to the EFI_DECOMPRESS_PROTOCOL instance.
+  @param  Source              The source buffer containing the compressed data.
+  @param  SourceSize          SourceSizeThe size of source data.
+  @param  Destination         On output, the destination buffer that contains
+                              the uncompressed data.
+  @param  DestinationSize     The size of the destination buffer.  The size of
+                              the destination buffer needed is obtained from
+                              EFI_DECOMPRESS_PROTOCOL.GetInfo().
+  @param  Scratch             A temporary scratch buffer that is used to perform
+                              the decompression.
+  @param  ScratchSize         The size of scratch buffer. The size of the
+                              scratch buffer needed is obtained from GetInfo().
+
+  @retval EFI_SUCCESS         Decompression completed successfully, and the
+                              uncompressed buffer is returned in Destination.
+  @retval EFI_INVALID_PARAMETER  The source buffer specified by Source and
+                                 SourceSize is corrupted (not in a valid
+                                 compressed format).
+
+**/
+EFI_STATUS
+EFIAPI
+DxeMainUefiDecompress (
+  IN     EFI_DECOMPRESS_PROTOCOL          *This,
+  IN     VOID                             *Source,
+  IN     UINT32                           SourceSize,
+  IN OUT VOID                             *Destination,
+  IN     UINT32                           DestinationSize,
+  IN OUT VOID                             *Scratch,
+  IN     UINT32                           ScratchSize
+  );
+
+/**
+  SEP member function.  This function creates and returns a new section stream
+  handle to represent the new section stream.
+
+  @param  SectionStreamLength    Size in bytes of the section stream.
+  @param  SectionStream          Buffer containing the new section stream.
+  @param  SectionStreamHandle    A pointer to a caller allocated UINTN that on
+                                 output contains the new section stream handle.
+
+  @retval EFI_SUCCESS            The section stream is created successfully.
+  @retval EFI_OUT_OF_RESOURCES   memory allocation failed.
+  @retval EFI_INVALID_PARAMETER  Section stream does not end concident with end
+                                 of last section.
+
+**/
+EFI_STATUS
+EFIAPI
+OpenSectionStream (
+  IN     UINTN                                     SectionStreamLength,
+  IN     VOID                                      *SectionStream,
+     OUT UINTN                                     *SectionStreamHandle
+  );
+
+
+
+/**
+  SEP member function.  Retrieves requested section from section stream.
+
+  @param  SectionStreamHandle   The section stream from which to extract the
+                                requested section.
+  @param  SectionType           A pointer to the type of section to search for.
+  @param  SectionDefinitionGuid If the section type is EFI_SECTION_GUID_DEFINED,
+                                then SectionDefinitionGuid indicates which of
+                                these types of sections to search for.
+  @param  SectionInstance       Indicates which instance of the requested
+                                section to return.
+  @param  Buffer                Double indirection to buffer.  If *Buffer is
+                                non-null on input, then the buffer is caller
+                                allocated.  If Buffer is NULL, then the buffer
+                                is callee allocated.  In either case, the
+                                requried buffer size is returned in *BufferSize.
+  @param  BufferSize            On input, indicates the size of *Buffer if
+                                *Buffer is non-null on input.  On output,
+                                indicates the required size (allocated size if
+                                callee allocated) of *Buffer.
+  @param  AuthenticationStatus  A pointer to a caller-allocated UINT32 that
+                                indicates the authentication status of the
+                                output buffer. If the input section's
+                                GuidedSectionHeader.Attributes field
+                                has the EFI_GUIDED_SECTION_AUTH_STATUS_VALID
+                                bit as clear, AuthenticationStatus must return
+                                zero. Both local bits (19:16) and aggregate
+                                bits (3:0) in AuthenticationStatus are returned
+                                by ExtractSection(). These bits reflect the
+                                status of the extraction operation. The bit
+                                pattern in both regions must be the same, as
+                                the local and aggregate authentication statuses
+                                have equivalent meaning at this level. If the
+                                function returns anything other than
+                                EFI_SUCCESS, the value of *AuthenticationStatus
+                                is undefined.
+  @param  IsFfs3Fv              Indicates the FV format.
+
+  @retval EFI_SUCCESS           Section was retrieved successfully
+  @retval EFI_PROTOCOL_ERROR    A GUID defined section was encountered in the
+                                section stream with its
+                                EFI_GUIDED_SECTION_PROCESSING_REQUIRED bit set,
+                                but there was no corresponding GUIDed Section
+                                Extraction Protocol in the handle database.
+                                *Buffer is unmodified.
+  @retval EFI_NOT_FOUND         An error was encountered when parsing the
+                                SectionStream.  This indicates the SectionStream
+                                is not correctly formatted.
+  @retval EFI_NOT_FOUND         The requested section does not exist.
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient resources to process
+                                the request.
+  @retval EFI_INVALID_PARAMETER The SectionStreamHandle does not exist.
+  @retval EFI_WARN_TOO_SMALL    The size of the caller allocated input buffer is
+                                insufficient to contain the requested section.
+                                The input buffer is filled and section contents
+                                are truncated.
+
+**/
+EFI_STATUS
+EFIAPI
+GetSection (
+  IN UINTN                                              SectionStreamHandle,
+  IN EFI_SECTION_TYPE                                   *SectionType,
+  IN EFI_GUID                                           *SectionDefinitionGuid,
+  IN UINTN                                              SectionInstance,
+  IN VOID                                               **Buffer,
+  IN OUT UINTN                                          *BufferSize,
+  OUT UINT32                                            *AuthenticationStatus,
+  IN BOOLEAN                                            IsFfs3Fv
+  );
+
+
+/**
+  SEP member function.  Deletes an existing section stream
+
+  @param  StreamHandleToClose    Indicates the stream to close
+  @param  FreeStreamBuffer       TRUE - Need to free stream buffer;
+                                 FALSE - No need to free stream buffer.
+
+  @retval EFI_SUCCESS            The section stream is closed sucessfully.
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_INVALID_PARAMETER  Section stream does not end concident with end
+                                 of last section.
+
+**/
+EFI_STATUS
+EFIAPI
+CloseSectionStream (
+  IN  UINTN                                     StreamHandleToClose,
+  IN  BOOLEAN                                   FreeStreamBuffer
+  );
+
+/**
+  Creates and initializes the DebugImageInfo Table.  Also creates the configuration
+  table and registers it into the system table.
+
+  Note:
+    This function allocates memory, frees it, and then allocates memory at an
+    address within the initial allocation. Since this function is called early
+    in DXE core initialization (before drivers are dispatched), this should not
+    be a problem.
+
+**/
+VOID
+CoreInitializeDebugImageInfoTable (
+  VOID
+  );
+
+
+/**
+  Update the CRC32 in the Debug Table.
+  Since the CRC32 service is made available by the Runtime driver, we have to
+  wait for the Runtime Driver to be installed before the CRC32 can be computed.
+  This function is called elsewhere by the core when the runtime architectural
+  protocol is produced.
+
+**/
+VOID
+CoreUpdateDebugTableCrc32 (
+  VOID
+  );
+
+
+/**
+  Adds a new DebugImageInfo structure to the DebugImageInfo Table.  Re-Allocates
+  the table if it's not large enough to accomidate another entry.
+
+  @param  ImageInfoType  type of debug image information
+  @param  LoadedImage    pointer to the loaded image protocol for the image being
+                         loaded
+  @param  ImageHandle    image handle for the image being loaded
+
+**/
+VOID
+CoreNewDebugImageInfoEntry (
+  IN  UINT32                      ImageInfoType,
+  IN  EFI_LOADED_IMAGE_PROTOCOL   *LoadedImage,
+  IN  EFI_HANDLE                  ImageHandle
+  );
+
+
+/**
+  Removes and frees an entry from the DebugImageInfo Table.
+
+  @param  ImageHandle    image handle for the image being unloaded
+
+**/
+VOID
+CoreRemoveDebugImageInfoEntry (
+  EFI_HANDLE ImageHandle
+  );
+
+
+/**
+  This routine consumes FV hobs and produces instances of FW_VOL_BLOCK_PROTOCOL as appropriate.
+
+  @param  ImageHandle            The image handle.
+  @param  SystemTable            The system table.
+
+  @retval EFI_SUCCESS            Successfully initialized firmware volume block
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockDriverInit (
+  IN EFI_HANDLE                 ImageHandle,
+  IN EFI_SYSTEM_TABLE           *SystemTable
+  );
+
+/**
+
+  Get FVB authentication status
+
+  @param FvbProtocol    FVB protocol.
+
+  @return Authentication status.
+
+**/
+UINT32
+GetFvbAuthenticationStatus (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *FvbProtocol
+  );
+
+/**
+  This routine produces a firmware volume block protocol on a given
+  buffer.
+
+  @param  BaseAddress            base address of the firmware volume image
+  @param  Length                 length of the firmware volume image
+  @param  ParentHandle           handle of parent firmware volume, if this image
+                                 came from an FV image file and section in another firmware
+                                 volume (ala capsules)
+  @param  AuthenticationStatus   Authentication status inherited, if this image
+                                 came from an FV image file and section in another firmware volume.
+  @param  FvProtocol             Firmware volume block protocol produced.
+
+  @retval EFI_VOLUME_CORRUPTED   Volume corrupted.
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to be allocated.
+  @retval EFI_SUCCESS            Successfully produced a FVB protocol on given
+                                 buffer.
+
+**/
+EFI_STATUS
+ProduceFVBProtocolOnBuffer (
+  IN EFI_PHYSICAL_ADDRESS   BaseAddress,
+  IN UINT64                 Length,
+  IN EFI_HANDLE             ParentHandle,
+  IN UINT32                 AuthenticationStatus,
+  OUT EFI_HANDLE            *FvProtocol  OPTIONAL
+  );
+
+
+/**
+  Raising to the task priority level of the mutual exclusion
+  lock, and then acquires ownership of the lock.
+
+  @param  Lock               The lock to acquire
+
+  @return Lock owned
+
+**/
+VOID
+CoreAcquireLock (
+  IN EFI_LOCK  *Lock
+  );
+
+
+/**
+  Initialize a basic mutual exclusion lock.   Each lock
+  provides mutual exclusion access at it's task priority
+  level.  Since there is no-premption (at any TPL) or
+  multiprocessor support, acquiring the lock only consists
+  of raising to the locks TPL.
+
+  @param  Lock               The EFI_LOCK structure to initialize
+
+  @retval EFI_SUCCESS        Lock Owned.
+  @retval EFI_ACCESS_DENIED  Reentrant Lock Acquisition, Lock not Owned.
+
+**/
+EFI_STATUS
+CoreAcquireLockOrFail (
+  IN EFI_LOCK  *Lock
+  );
+
+
+/**
+  Releases ownership of the mutual exclusion lock, and
+  restores the previous task priority level.
+
+  @param  Lock               The lock to release
+
+  @return Lock unowned
+
+**/
+VOID
+CoreReleaseLock (
+  IN EFI_LOCK  *Lock
+  );
+
+
+/**
+  An empty function to pass error checking of CreateEventEx ().
+
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               Pointer to the notification function's context,
+                                which is implementation-dependent.
+
+**/
+VOID
+EFIAPI
+CoreEmptyCallbackFunction (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  );
+
+/**
+  Read data from Firmware Block by FVB protocol Read. 
+  The data may cross the multi block ranges.
+
+  @param  Fvb                   The FW_VOL_BLOCK_PROTOCOL instance from which to read data.
+  @param  StartLba              Pointer to StartLba.
+                                On input, the start logical block index from which to read.
+                                On output,the end logical block index after reading.
+  @param  Offset                Pointer to Offset
+                                On input, offset into the block at which to begin reading.
+                                On output, offset into the end block after reading.
+  @param  DataSize              Size of data to be read.
+  @param  Data                  Pointer to Buffer that the data will be read into.
+
+  @retval EFI_SUCCESS           Successfully read data from firmware block.
+  @retval others
+**/
+EFI_STATUS
+ReadFvbData (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *Fvb,
+  IN OUT EFI_LBA                                *StartLba,
+  IN OUT UINTN                                  *Offset,
+  IN     UINTN                                  DataSize,
+  OUT    UINT8                                  *Data
+  );
+
+/**
+  Given the supplied FW_VOL_BLOCK_PROTOCOL, allocate a buffer for output and
+  copy the real length volume header into it.
+
+  @param  Fvb                   The FW_VOL_BLOCK_PROTOCOL instance from which to
+                                read the volume header
+  @param  FwVolHeader           Pointer to pointer to allocated buffer in which
+                                the volume header is returned.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough buffer could be allocated.
+  @retval EFI_SUCCESS           Successfully read volume header to the allocated
+                                buffer.
+  @retval EFI_INVALID_PARAMETER The FV Header signature is not as expected or
+                                the file system could not be understood.
+
+**/
+EFI_STATUS
+GetFwVolHeader (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *Fvb,
+  OUT    EFI_FIRMWARE_VOLUME_HEADER             **FwVolHeader
+  );
+
+/**
+  Verify checksum of the firmware volume header.
+
+  @param  FvHeader       Points to the firmware volume header to be checked
+
+  @retval TRUE           Checksum verification passed
+  @retval FALSE          Checksum verification failed
+
+**/
+BOOLEAN
+VerifyFvHeaderChecksum (
+  IN EFI_FIRMWARE_VOLUME_HEADER *FvHeader
+  );
+
+/**
+  Initialize memory profile.
+
+  @param HobStart   The start address of the HOB.
+
+**/
+VOID
+MemoryProfileInit (
+  IN VOID   *HobStart
+  );
+
+/**
+  Install memory profile protocol.
+
+**/
+VOID
+MemoryProfileInstallProtocol (
+  VOID
+  );
+
+/**
+  Register image to memory profile.
+
+  @param DriverEntry    Image info.
+  @param FileType       Image file type.
+
+  @retval TRUE          Register success.
+  @retval FALSE         Register fail.
+
+**/
+BOOLEAN
+RegisterMemoryProfileImage (
+  IN LOADED_IMAGE_PRIVATE_DATA  *DriverEntry,
+  IN EFI_FV_FILETYPE            FileType
+  );
+
+/**
+  Unregister image from memory profile.
+
+  @param DriverEntry    Image info.
+
+  @retval TRUE          Unregister success.
+  @retval FALSE         Unregister fail.
+
+**/
+BOOLEAN
+UnregisterMemoryProfileImage (
+  IN LOADED_IMAGE_PRIVATE_DATA  *DriverEntry
+  );
+
+/**
+  Update memory profile information.
+
+  @param CallerAddress  Address of caller who call Allocate or Free.
+  @param Action         This Allocate or Free action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+CoreUpdateProfile (
+  IN EFI_PHYSICAL_ADDRESS   CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN EFI_MEMORY_TYPE        MemoryType, // Valid for AllocatePages/AllocatePool
+  IN UINTN                  Size,       // Valid for AllocatePages/FreePages/AllocatePool
+  IN VOID                   *Buffer
+  );
+
+/**
+  Internal function.  Converts a memory range to use new attributes.
+
+  @param  Start                  The first address of the range Must be page
+                                 aligned
+  @param  NumberOfPages          The number of pages to convert
+  @param  NewAttributes          The new attributes value for the range.
+
+**/
+VOID
+CoreUpdateMemoryAttributes (
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                NumberOfPages,
+  IN UINT64                NewAttributes
+  );
+
+/**
+  Initialize PropertiesTable support.
+**/
+VOID
+EFIAPI
+CoreInitializePropertiesTable (
+  VOID
+  );
+
+/**
+  Initialize MemoryAttrubutesTable support.
+**/
+VOID
+EFIAPI
+CoreInitializeMemoryAttributesTable (
+  VOID
+  );
+
+/**
+  Insert image record.
+
+  @param  RuntimeImage    Runtime image information
+**/
+VOID
+InsertImageRecord (
+  IN EFI_RUNTIME_IMAGE_ENTRY  *RuntimeImage
+  );
+
+/**
+  Remove Image record.
+
+  @param  RuntimeImage    Runtime image information
+**/
+VOID
+RemoveImageRecord (
+  IN EFI_RUNTIME_IMAGE_ENTRY  *RuntimeImage
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeMain.inf b/Core/MdeModulePkg/Core/Dxe/DxeMain.inf
new file mode 100644
index 0000000000..e3e4d036db
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeMain.inf
@@ -0,0 +1,204 @@
+## @file
+#  This is core module in DXE phase.
+#
+#  It provides an implementation of DXE Core that is compliant with DXE CIS.
+#  
+#  Copyright (c) 2006 - 2016, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeCore
+  MODULE_UNI_FILE                = DxeCore.uni
+  FILE_GUID                      = D6A2CB7F-6A18-4e2f-B43B-9920A733700A
+  MODULE_TYPE                    = DXE_CORE
+  VERSION_STRING                 = 1.0
+
+
+  ENTRY_POINT                    = DxeMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  DxeMain.h
+  SectionExtraction/CoreSectionExtraction.c
+  Image/Image.c
+  Image/Image.h
+  Misc/DebugImageInfo.c
+  Misc/Stall.c
+  Misc/SetWatchdogTimer.c
+  Misc/InstallConfigurationTable.c
+  Misc/PropertiesTable.c
+  Misc/MemoryAttributesTable.c
+  Library/Library.c
+  Hand/DriverSupport.c
+  Hand/Notify.c
+  Hand/Locate.c
+  Hand/Handle.c
+  Hand/Handle.h
+  Gcd/Gcd.c
+  Gcd/Gcd.h
+  Mem/Pool.c
+  Mem/Page.c
+  Mem/MemData.c
+  Mem/Imem.h
+  Mem/MemoryProfileRecord.c
+  FwVolBlock/FwVolBlock.c
+  FwVolBlock/FwVolBlock.h
+  FwVol/FwVolWrite.c
+  FwVol/FwVolRead.c
+  FwVol/FwVolAttrib.c
+  FwVol/Ffs.c
+  FwVol/FwVol.c
+  FwVol/FwVolDriver.h
+  Event/Tpl.c
+  Event/Timer.c
+  Event/Event.c
+  Event/Event.h
+  Dispatcher/Dependency.c
+  Dispatcher/Dispatcher.c
+  DxeMain/DxeProtocolNotify.c
+  DxeMain/DxeMain.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  CacheMaintenanceLib
+  UefiDecompressLib
+  PerformanceLib
+  HobLib
+  BaseLib
+  UefiLib
+  DebugLib
+  DxeCoreEntryPoint
+  PeCoffLib
+  PeCoffGetEntryPointLib
+  PeCoffExtraActionLib
+  ExtractGuidedSectionLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  DevicePathLib
+  ReportStatusCodeLib
+  TimerLib
+  DxeServicesLib
+  DebugAgentLib
+  CpuExceptionHandlerLib
+  PcdLib
+
+[Guids]
+  gEfiEventMemoryMapChangeGuid                  ## PRODUCES             ## Event
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES             ## Event
+  ## CONSUMES   ## Event
+  ## PRODUCES   ## Event
+  gEfiEventExitBootServicesGuid
+  gEfiHobMemoryAllocModuleGuid                  ## CONSUMES             ## HOB
+  gEfiFirmwareFileSystem2Guid                   ## CONSUMES             ## GUID # Used to compare with FV's file system guid and get the FV's file system format
+  gEfiFirmwareFileSystem3Guid                   ## CONSUMES             ## GUID # Used to compare with FV's file system guid and get the FV's file system format
+  gAprioriGuid                                  ## SOMETIMES_CONSUMES   ## File
+  gEfiDebugImageInfoTableGuid                   ## PRODUCES             ## SystemTable
+  gEfiHobListGuid                               ## PRODUCES             ## SystemTable
+  gEfiDxeServicesTableGuid                      ## PRODUCES             ## SystemTable
+  ## PRODUCES               ## SystemTable
+  ## SOMETIMES_CONSUMES     ## HOB
+  gEfiMemoryTypeInformationGuid
+  gEfiEventDxeDispatchGuid                      ## PRODUCES             ## Event
+  gLoadFixedAddressConfigurationTableGuid       ## SOMETIMES_PRODUCES   ## SystemTable
+  ## PRODUCES   ## Event
+  ## CONSUMES   ## Event
+  gIdleLoopEventGuid
+  gEventExitBootServicesFailedGuid              ## SOMETIMES_PRODUCES   ## Event
+  gEfiVectorHandoffTableGuid                    ## SOMETIMES_PRODUCES   ## SystemTable
+  gEdkiiMemoryProfileGuid                       ## SOMETIMES_PRODUCES   ## GUID # Install protocol
+  gZeroGuid                                     ## SOMETIMES_CONSUMES   ## GUID
+  gEfiPropertiesTableGuid                       ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiMemoryAttributesTableGuid                 ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiEndOfDxeEventGroupGuid                    ## SOMETIMES_CONSUMES   ## Event
+
+[Ppis]
+  gEfiVectorHandoffInfoPpiGuid                  ## UNDEFINED # HOB
+
+[Protocols]
+  ## PRODUCES
+  ## SOMETIMES_CONSUMES
+  gEfiDecompressProtocolGuid
+  gEfiLoadPeImageProtocolGuid                   ## SOMETIMES_PRODUCES   # Produces when PcdFrameworkCompatibilitySupport is set
+  gEfiSimpleFileSystemProtocolGuid              ## SOMETIMES_CONSUMES
+  gEfiLoadFileProtocolGuid                      ## SOMETIMES_CONSUMES
+  gEfiLoadFile2ProtocolGuid                     ## SOMETIMES_CONSUMES
+  gEfiBusSpecificDriverOverrideProtocolGuid     ## SOMETIMES_CONSUMES
+  gEfiDriverFamilyOverrideProtocolGuid          ## SOMETIMES_CONSUMES
+  gEfiPlatformDriverOverrideProtocolGuid        ## SOMETIMES_CONSUMES
+  gEfiDriverBindingProtocolGuid                 ## SOMETIMES_CONSUMES
+  ## PRODUCES
+  ## CONSUMES
+  ## NOTIFY
+  gEfiFirmwareVolumeBlockProtocolGuid
+  ## PRODUCES
+  ## CONSUMES
+  ## NOTIFY
+  gEfiFirmwareVolume2ProtocolGuid
+  ## PRODUCES
+  ## CONSUMES
+  gEfiDevicePathProtocolGuid
+  gEfiLoadedImageProtocolGuid                   ## PRODUCES
+  gEfiLoadedImageDevicePathProtocolGuid         ## PRODUCES
+  gEfiHiiPackageListProtocolGuid                ## SOMETIMES_PRODUCES
+  gEfiEbcProtocolGuid                           ## SOMETIMES_CONSUMES
+  gEfiSmmBase2ProtocolGuid                      ## SOMETIMES_CONSUMES
+
+  # Arch Protocols
+  gEfiBdsArchProtocolGuid                       ## CONSUMES
+  gEfiCpuArchProtocolGuid                       ## CONSUMES
+  gEfiMetronomeArchProtocolGuid                 ## CONSUMES
+  gEfiMonotonicCounterArchProtocolGuid          ## CONSUMES
+  gEfiRealTimeClockArchProtocolGuid             ## CONSUMES
+  gEfiResetArchProtocolGuid                     ## CONSUMES
+  gEfiRuntimeArchProtocolGuid                   ## CONSUMES
+  gEfiSecurityArchProtocolGuid                  ## CONSUMES
+  gEfiSecurity2ArchProtocolGuid                 ## SOMETIMES_CONSUMES
+  gEfiTimerArchProtocolGuid                     ## CONSUMES
+  gEfiVariableWriteArchProtocolGuid             ## CONSUMES
+  gEfiVariableArchProtocolGuid                  ## CONSUMES
+  gEfiCapsuleArchProtocolGuid                   ## CONSUMES
+  gEfiWatchdogTimerArchProtocolGuid             ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrameworkCompatibilitySupport	   ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressBootTimeCodePageNumber    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressRuntimeCodePageNumber     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxEfiSystemTablePointerAddress         ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfileMemoryType                 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfilePropertyMask               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPropertiesTableEnable                   ## CONSUMES
+
+# [Hob]
+# RESOURCE_DESCRIPTOR   ## CONSUMES
+# MEMORY_ALLOCATION     ## CONSUMES
+# FIRMWARE_VOLUME       ## CONSUMES
+# UNDEFINED             ## CONSUMES # CPU
+#
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER ## PRODUCES  # DxeCore signals timer event.
+# EVENT_TYPE_PERIODIC_TIMER ## PRODUCES  # DxeCore signals timer event.
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DxeCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeMain.c b/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeMain.c
new file mode 100644
index 0000000000..0a34711b22
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeMain.c
@@ -0,0 +1,922 @@
+/** @file
+  DXE Core Main Entry Point
+
+Copyright (c) 2006 - 2016, 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 "DxeMain.h"
+
+//
+// DXE Core Global Variables for Protocols from PEI
+//
+EFI_HANDLE                                mDecompressHandle = NULL;
+
+//
+// DXE Core globals for Architecture Protocols
+//
+EFI_SECURITY_ARCH_PROTOCOL        *gSecurity      = NULL;
+EFI_SECURITY2_ARCH_PROTOCOL       *gSecurity2     = NULL;
+EFI_CPU_ARCH_PROTOCOL             *gCpu           = NULL;
+EFI_METRONOME_ARCH_PROTOCOL       *gMetronome     = NULL;
+EFI_TIMER_ARCH_PROTOCOL           *gTimer         = NULL;
+EFI_BDS_ARCH_PROTOCOL             *gBds           = NULL;
+EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *gWatchdogTimer = NULL;
+
+//
+// DXE Core globals for optional protocol dependencies
+//
+EFI_SMM_BASE2_PROTOCOL            *gSmmBase2      = NULL;
+
+//
+// DXE Core Global used to update core loaded image protocol handle
+//
+EFI_GUID                           *gDxeCoreFileName;
+EFI_LOADED_IMAGE_PROTOCOL          *gDxeCoreLoadedImage;
+
+//
+// DXE Core Module Variables
+//
+EFI_BOOT_SERVICES mBootServices = {
+  {
+    EFI_BOOT_SERVICES_SIGNATURE,                                                          // Signature
+    EFI_BOOT_SERVICES_REVISION,                                                           // Revision
+    sizeof (EFI_BOOT_SERVICES),                                                           // HeaderSize
+    0,                                                                                    // CRC32
+    0                                                                                     // Reserved
+  },
+  (EFI_RAISE_TPL)                               CoreRaiseTpl,                             // RaiseTPL
+  (EFI_RESTORE_TPL)                             CoreRestoreTpl,                           // RestoreTPL
+  (EFI_ALLOCATE_PAGES)                          CoreAllocatePages,                        // AllocatePages
+  (EFI_FREE_PAGES)                              CoreFreePages,                            // FreePages
+  (EFI_GET_MEMORY_MAP)                          CoreGetMemoryMap,                         // GetMemoryMap
+  (EFI_ALLOCATE_POOL)                           CoreAllocatePool,                         // AllocatePool
+  (EFI_FREE_POOL)                               CoreFreePool,                             // FreePool
+  (EFI_CREATE_EVENT)                            CoreCreateEvent,                          // CreateEvent
+  (EFI_SET_TIMER)                               CoreSetTimer,                             // SetTimer
+  (EFI_WAIT_FOR_EVENT)                          CoreWaitForEvent,                         // WaitForEvent
+  (EFI_SIGNAL_EVENT)                            CoreSignalEvent,                          // SignalEvent
+  (EFI_CLOSE_EVENT)                             CoreCloseEvent,                           // CloseEvent
+  (EFI_CHECK_EVENT)                             CoreCheckEvent,                           // CheckEvent
+  (EFI_INSTALL_PROTOCOL_INTERFACE)              CoreInstallProtocolInterface,             // InstallProtocolInterface
+  (EFI_REINSTALL_PROTOCOL_INTERFACE)            CoreReinstallProtocolInterface,           // ReinstallProtocolInterface
+  (EFI_UNINSTALL_PROTOCOL_INTERFACE)            CoreUninstallProtocolInterface,           // UninstallProtocolInterface
+  (EFI_HANDLE_PROTOCOL)                         CoreHandleProtocol,                       // HandleProtocol
+  (VOID *)                                      NULL,                                     // Reserved
+  (EFI_REGISTER_PROTOCOL_NOTIFY)                CoreRegisterProtocolNotify,               // RegisterProtocolNotify
+  (EFI_LOCATE_HANDLE)                           CoreLocateHandle,                         // LocateHandle
+  (EFI_LOCATE_DEVICE_PATH)                      CoreLocateDevicePath,                     // LocateDevicePath
+  (EFI_INSTALL_CONFIGURATION_TABLE)             CoreInstallConfigurationTable,            // InstallConfigurationTable
+  (EFI_IMAGE_LOAD)                              CoreLoadImage,                            // LoadImage
+  (EFI_IMAGE_START)                             CoreStartImage,                           // StartImage
+  (EFI_EXIT)                                    CoreExit,                                 // Exit
+  (EFI_IMAGE_UNLOAD)                            CoreUnloadImage,                          // UnloadImage
+  (EFI_EXIT_BOOT_SERVICES)                      CoreExitBootServices,                     // ExitBootServices
+  (EFI_GET_NEXT_MONOTONIC_COUNT)                CoreEfiNotAvailableYetArg1,               // GetNextMonotonicCount
+  (EFI_STALL)                                   CoreStall,                                // Stall
+  (EFI_SET_WATCHDOG_TIMER)                      CoreSetWatchdogTimer,                     // SetWatchdogTimer
+  (EFI_CONNECT_CONTROLLER)                      CoreConnectController,                    // ConnectController
+  (EFI_DISCONNECT_CONTROLLER)                   CoreDisconnectController,                 // DisconnectController
+  (EFI_OPEN_PROTOCOL)                           CoreOpenProtocol,                         // OpenProtocol
+  (EFI_CLOSE_PROTOCOL)                          CoreCloseProtocol,                        // CloseProtocol
+  (EFI_OPEN_PROTOCOL_INFORMATION)               CoreOpenProtocolInformation,              // OpenProtocolInformation
+  (EFI_PROTOCOLS_PER_HANDLE)                    CoreProtocolsPerHandle,                   // ProtocolsPerHandle
+  (EFI_LOCATE_HANDLE_BUFFER)                    CoreLocateHandleBuffer,                   // LocateHandleBuffer
+  (EFI_LOCATE_PROTOCOL)                         CoreLocateProtocol,                       // LocateProtocol
+  (EFI_INSTALL_MULTIPLE_PROTOCOL_INTERFACES)    CoreInstallMultipleProtocolInterfaces,    // InstallMultipleProtocolInterfaces
+  (EFI_UNINSTALL_MULTIPLE_PROTOCOL_INTERFACES)  CoreUninstallMultipleProtocolInterfaces,  // UninstallMultipleProtocolInterfaces
+  (EFI_CALCULATE_CRC32)                         CoreEfiNotAvailableYetArg3,               // CalculateCrc32
+  (EFI_COPY_MEM)                                CopyMem,                                  // CopyMem
+  (EFI_SET_MEM)                                 SetMem,                                   // SetMem
+  (EFI_CREATE_EVENT_EX)                         CoreCreateEventEx                         // CreateEventEx
+};
+
+EFI_DXE_SERVICES mDxeServices = {
+  {
+    DXE_SERVICES_SIGNATURE,                                           // Signature
+    DXE_SERVICES_REVISION,                                            // Revision
+    sizeof (DXE_SERVICES),                                            // HeaderSize
+    0,                                                                    // CRC32
+    0                                                                     // Reserved
+  },
+  (EFI_ADD_MEMORY_SPACE)             CoreAddMemorySpace,                  // AddMemorySpace
+  (EFI_ALLOCATE_MEMORY_SPACE)        CoreAllocateMemorySpace,             // AllocateMemorySpace
+  (EFI_FREE_MEMORY_SPACE)            CoreFreeMemorySpace,                 // FreeMemorySpace
+  (EFI_REMOVE_MEMORY_SPACE)          CoreRemoveMemorySpace,               // RemoveMemorySpace
+  (EFI_GET_MEMORY_SPACE_DESCRIPTOR)  CoreGetMemorySpaceDescriptor,        // GetMemorySpaceDescriptor
+  (EFI_SET_MEMORY_SPACE_ATTRIBUTES)  CoreSetMemorySpaceAttributes,        // SetMemorySpaceAttributes
+  (EFI_GET_MEMORY_SPACE_MAP)         CoreGetMemorySpaceMap,               // GetMemorySpaceMap
+  (EFI_ADD_IO_SPACE)                 CoreAddIoSpace,                      // AddIoSpace
+  (EFI_ALLOCATE_IO_SPACE)            CoreAllocateIoSpace,                 // AllocateIoSpace
+  (EFI_FREE_IO_SPACE)                CoreFreeIoSpace,                     // FreeIoSpace
+  (EFI_REMOVE_IO_SPACE)              CoreRemoveIoSpace,                   // RemoveIoSpace
+  (EFI_GET_IO_SPACE_DESCRIPTOR)      CoreGetIoSpaceDescriptor,            // GetIoSpaceDescriptor
+  (EFI_GET_IO_SPACE_MAP)             CoreGetIoSpaceMap,                   // GetIoSpaceMap
+  (EFI_DISPATCH)                     CoreDispatcher,                      // Dispatch
+  (EFI_SCHEDULE)                     CoreSchedule,                        // Schedule
+  (EFI_TRUST)                        CoreTrust,                           // Trust
+  (EFI_PROCESS_FIRMWARE_VOLUME)      CoreProcessFirmwareVolume,           // ProcessFirmwareVolume
+  (EFI_SET_MEMORY_SPACE_CAPABILITIES)CoreSetMemorySpaceCapabilities,      // SetMemorySpaceCapabilities
+};
+
+EFI_SYSTEM_TABLE mEfiSystemTableTemplate = {
+  {
+    EFI_SYSTEM_TABLE_SIGNATURE,                                           // Signature
+    EFI_SYSTEM_TABLE_REVISION,                                            // Revision
+    sizeof (EFI_SYSTEM_TABLE),                                            // HeaderSize
+    0,                                                                    // CRC32
+    0                                                                     // Reserved
+  },
+  NULL,                                                                   // FirmwareVendor
+  0,                                                                      // FirmwareRevision
+  NULL,                                                                   // ConsoleInHandle
+  NULL,                                                                   // ConIn
+  NULL,                                                                   // ConsoleOutHandle
+  NULL,                                                                   // ConOut
+  NULL,                                                                   // StandardErrorHandle
+  NULL,                                                                   // StdErr
+  NULL,                                                                   // RuntimeServices
+  &mBootServices,                                                         // BootServices
+  0,                                                                      // NumberOfConfigurationTableEntries
+  NULL                                                                    // ConfigurationTable
+};
+
+EFI_RUNTIME_SERVICES mEfiRuntimeServicesTableTemplate = {
+  {
+    EFI_RUNTIME_SERVICES_SIGNATURE,                               // Signature
+    EFI_RUNTIME_SERVICES_REVISION,                                // Revision
+    sizeof (EFI_RUNTIME_SERVICES),                                // HeaderSize
+    0,                                                            // CRC32
+    0                                                             // Reserved
+  },
+  (EFI_GET_TIME)                    CoreEfiNotAvailableYetArg2,   // GetTime
+  (EFI_SET_TIME)                    CoreEfiNotAvailableYetArg1,   // SetTime
+  (EFI_GET_WAKEUP_TIME)             CoreEfiNotAvailableYetArg3,   // GetWakeupTime
+  (EFI_SET_WAKEUP_TIME)             CoreEfiNotAvailableYetArg2,   // SetWakeupTime
+  (EFI_SET_VIRTUAL_ADDRESS_MAP)     CoreEfiNotAvailableYetArg4,   // SetVirtualAddressMap
+  (EFI_CONVERT_POINTER)             CoreEfiNotAvailableYetArg2,   // ConvertPointer
+  (EFI_GET_VARIABLE)                CoreEfiNotAvailableYetArg5,   // GetVariable
+  (EFI_GET_NEXT_VARIABLE_NAME)      CoreEfiNotAvailableYetArg3,   // GetNextVariableName
+  (EFI_SET_VARIABLE)                CoreEfiNotAvailableYetArg5,   // SetVariable
+  (EFI_GET_NEXT_HIGH_MONO_COUNT)    CoreEfiNotAvailableYetArg1,   // GetNextHighMonotonicCount
+  (EFI_RESET_SYSTEM)                CoreEfiNotAvailableYetArg4,   // ResetSystem
+  (EFI_UPDATE_CAPSULE)              CoreEfiNotAvailableYetArg3,   // UpdateCapsule
+  (EFI_QUERY_CAPSULE_CAPABILITIES)  CoreEfiNotAvailableYetArg4,   // QueryCapsuleCapabilities
+  (EFI_QUERY_VARIABLE_INFO)         CoreEfiNotAvailableYetArg4    // QueryVariableInfo
+};
+
+EFI_RUNTIME_ARCH_PROTOCOL gRuntimeTemplate = {
+  INITIALIZE_LIST_HEAD_VARIABLE (gRuntimeTemplate.ImageHead),
+  INITIALIZE_LIST_HEAD_VARIABLE (gRuntimeTemplate.EventHead),
+
+  //
+  // Make sure Size != sizeof (EFI_MEMORY_DESCRIPTOR). This will
+  // prevent people from having pointer math bugs in their code.
+  // now you have to use *DescriptorSize to make things work.
+  //
+  sizeof (EFI_MEMORY_DESCRIPTOR) + sizeof (UINT64) - (sizeof (EFI_MEMORY_DESCRIPTOR) % sizeof (UINT64)),
+  EFI_MEMORY_DESCRIPTOR_VERSION,
+  0,
+  NULL,
+  NULL,
+  FALSE,
+  FALSE
+};
+
+EFI_RUNTIME_ARCH_PROTOCOL *gRuntime = &gRuntimeTemplate;
+
+//
+// DXE Core Global Variables for the EFI System Table, Boot Services Table,
+// DXE Services Table, and Runtime Services Table
+//
+EFI_DXE_SERVICES      *gDxeCoreDS = &mDxeServices;
+EFI_SYSTEM_TABLE      *gDxeCoreST = NULL;
+
+//
+// For debug initialize gDxeCoreRT to template. gDxeCoreRT must be allocated from RT memory
+//  but gDxeCoreRT is used for ASSERT () and DEBUG () type macros so lets give it
+//  a value that will not cause debug infrastructure to crash early on.
+//
+EFI_RUNTIME_SERVICES  *gDxeCoreRT = &mEfiRuntimeServicesTableTemplate;
+EFI_HANDLE            gDxeCoreImageHandle = NULL;
+
+
+//
+// EFI Decompress Protocol
+//
+EFI_DECOMPRESS_PROTOCOL  gEfiDecompress = {
+  DxeMainUefiDecompressGetInfo,
+  DxeMainUefiDecompress
+};
+
+//
+// For Loading modules at fixed address feature, the configuration table is to cache the top address below which to load
+// Runtime code&boot time code
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_LOAD_FIXED_ADDRESS_CONFIGURATION_TABLE    gLoadModuleAtFixAddressConfigurationTable = {0, 0};
+
+// Main entry point to the DXE Core
+//
+
+/**
+  Main entry point to DXE Core.
+
+  @param  HobStart               Pointer to the beginning of the HOB List from PEI.
+
+  @return This function should never return.
+
+**/
+VOID
+EFIAPI
+DxeMain (
+  IN  VOID *HobStart
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          MemoryBaseAddress;
+  UINT64                        MemoryLength;
+  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext;
+  UINTN                         Index;
+  EFI_HOB_GUID_TYPE             *GuidHob;
+  EFI_VECTOR_HANDOFF_INFO       *VectorInfoList;
+  EFI_VECTOR_HANDOFF_INFO       *VectorInfo;
+
+  //
+  // Setup the default exception handlers
+  //
+  VectorInfoList = NULL;
+  GuidHob = GetNextGuidHob (&gEfiVectorHandoffInfoPpiGuid, HobStart);
+  if (GuidHob != NULL) {
+    VectorInfoList = (EFI_VECTOR_HANDOFF_INFO *) (GET_GUID_HOB_DATA(GuidHob));
+  }
+  Status = InitializeCpuExceptionHandlers (VectorInfoList);
+  ASSERT_EFI_ERROR (Status);
+  
+  //
+  // Initialize Debug Agent to support source level debug in DXE phase
+  //
+  InitializeDebugAgent (DEBUG_AGENT_INIT_DXE_CORE, HobStart, NULL);
+
+  //
+  // Initialize Memory Services
+  //
+  CoreInitializeMemoryServices (&HobStart, &MemoryBaseAddress, &MemoryLength);
+
+  MemoryProfileInit (HobStart);
+
+  //
+  // Allocate the EFI System Table and EFI Runtime Service Table from EfiRuntimeServicesData
+  // Use the templates to initialize the contents of the EFI System Table and EFI Runtime Services Table
+  //
+  gDxeCoreST = AllocateRuntimeCopyPool (sizeof (EFI_SYSTEM_TABLE), &mEfiSystemTableTemplate);
+  ASSERT (gDxeCoreST != NULL);
+
+  gDxeCoreRT = AllocateRuntimeCopyPool (sizeof (EFI_RUNTIME_SERVICES), &mEfiRuntimeServicesTableTemplate);
+  ASSERT (gDxeCoreRT != NULL);
+
+  gDxeCoreST->RuntimeServices = gDxeCoreRT;
+
+  //
+  // Start the Image Services.
+  //
+  Status = CoreInitializeImageServices (HobStart);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report DXE Core image information to the PE/COFF Extra Action Library
+  //
+  ZeroMem (&ImageContext, sizeof (ImageContext));
+  ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)gDxeCoreLoadedImage->ImageBase;
+  ImageContext.PdbPointer   = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageContext.ImageAddress);
+  PeCoffLoaderRelocateImageExtraAction (&ImageContext);
+
+  //
+  // Initialize the Global Coherency Domain Services
+  //
+  Status = CoreInitializeGcdServices (&HobStart, MemoryBaseAddress, MemoryLength);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Call constructor for all libraries
+  //
+  ProcessLibraryConstructorList (gDxeCoreImageHandle, gDxeCoreST);
+  PERF_END   (NULL,"PEI", NULL, 0) ;
+  PERF_START (NULL,"DXE", NULL, 0) ;
+
+  //
+  // Install the DXE Services Table into the EFI System Tables's Configuration Table
+  //
+  Status = CoreInstallConfigurationTable (&gEfiDxeServicesTableGuid, gDxeCoreDS);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install the HOB List into the EFI System Tables's Configuration Table
+  //
+  Status = CoreInstallConfigurationTable (&gEfiHobListGuid, HobStart);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install Memory Type Information Table into the EFI System Tables's Configuration Table
+  //
+  Status = CoreInstallConfigurationTable (&gEfiMemoryTypeInformationGuid, &gMemoryTypeInformation);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // If Loading modules At fixed address feature is enabled, install Load moduels at fixed address
+  // Configuration Table so that user could easily to retrieve the top address to load Dxe and PEI
+  // Code and Tseg base to load SMM driver.
+  //
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+    Status = CoreInstallConfigurationTable (&gLoadFixedAddressConfigurationTableGuid, &gLoadModuleAtFixAddressConfigurationTable);
+    ASSERT_EFI_ERROR (Status);
+  }
+  //
+  // Report Status Code here for DXE_ENTRY_POINT once it is available
+  //
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_SOFTWARE_DXE_CORE | EFI_SW_DXE_CORE_PC_ENTRY_POINT)
+    );
+
+  //
+  // Create the aligned system table pointer structure that is used by external
+  // debuggers to locate the system table...  Also, install debug image info
+  // configuration table.
+  //
+  CoreInitializeDebugImageInfoTable ();
+  CoreNewDebugImageInfoEntry (
+    EFI_DEBUG_IMAGE_INFO_TYPE_NORMAL,
+    gDxeCoreLoadedImage,
+    gDxeCoreImageHandle
+    );
+
+  DEBUG ((DEBUG_INFO | DEBUG_LOAD, "HOBLIST address in DXE = 0x%p\n", HobStart));
+
+  DEBUG_CODE_BEGIN ();
+    EFI_PEI_HOB_POINTERS               Hob;
+
+    for (Hob.Raw = HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+      if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
+        DEBUG ((DEBUG_INFO | DEBUG_LOAD, "Memory Allocation 0x%08x 0x%0lx - 0x%0lx\n", \
+          Hob.MemoryAllocation->AllocDescriptor.MemoryType,                      \
+          Hob.MemoryAllocation->AllocDescriptor.MemoryBaseAddress,               \
+          Hob.MemoryAllocation->AllocDescriptor.MemoryBaseAddress + Hob.MemoryAllocation->AllocDescriptor.MemoryLength - 1));
+      }
+    }
+    for (Hob.Raw = HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+      if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_FV2) {
+        DEBUG ((DEBUG_INFO | DEBUG_LOAD, "FV2 Hob           0x%0lx - 0x%0lx\n", Hob.FirmwareVolume2->BaseAddress, Hob.FirmwareVolume2->BaseAddress + Hob.FirmwareVolume2->Length - 1));
+      } else if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_FV) {
+        DEBUG ((DEBUG_INFO | DEBUG_LOAD, "FV Hob            0x%0lx - 0x%0lx\n", Hob.FirmwareVolume->BaseAddress, Hob.FirmwareVolume->BaseAddress + Hob.FirmwareVolume->Length - 1));
+      }
+    }
+  DEBUG_CODE_END ();
+
+  //
+  // Initialize the Event Services
+  //
+  Status = CoreInitializeEventServices ();
+  ASSERT_EFI_ERROR (Status);
+
+  MemoryProfileInstallProtocol ();
+
+  CoreInitializePropertiesTable ();
+  CoreInitializeMemoryAttributesTable ();
+
+  //
+  // Get persisted vector hand-off info from GUIDeed HOB again due to HobStart may be updated,
+  // and install configuration table
+  //
+  GuidHob = GetNextGuidHob (&gEfiVectorHandoffInfoPpiGuid, HobStart);
+  if (GuidHob != NULL) {
+    VectorInfoList = (EFI_VECTOR_HANDOFF_INFO *) (GET_GUID_HOB_DATA(GuidHob));
+    VectorInfo = VectorInfoList;
+    Index = 1;
+    while (VectorInfo->Attribute != EFI_VECTOR_HANDOFF_LAST_ENTRY) {
+      VectorInfo ++;
+      Index ++;
+    }
+    VectorInfo = AllocateCopyPool (sizeof (EFI_VECTOR_HANDOFF_INFO) * Index, (VOID *) VectorInfoList);
+    ASSERT (VectorInfo != NULL);
+    Status = CoreInstallConfigurationTable (&gEfiVectorHandoffTableGuid, (VOID *) VectorInfo);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Get the Protocols that were passed in from PEI to DXE through GUIDed HOBs
+  //
+  // These Protocols are not architectural. This implementation is sharing code between
+  // PEI and DXE in order to save FLASH space. These Protocols could also be implemented
+  // as part of the DXE Core. However, that would also require the DXE Core to be ported
+  // each time a different CPU is used, a different Decompression algorithm is used, or a
+  // different Image type is used. By placing these Protocols in PEI, the DXE Core remains
+  // generic, and only PEI and the Arch Protocols need to be ported from Platform to Platform,
+  // and from CPU to CPU.
+  //
+
+  //
+  // Publish the EFI, Tiano, and Custom Decompress protocols for use by other DXE components
+  //
+  Status = CoreInstallMultipleProtocolInterfaces (
+             &mDecompressHandle,
+             &gEfiDecompressProtocolGuid,           &gEfiDecompress,
+             NULL
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register for the GUIDs of the Architectural Protocols, so the rest of the
+  // EFI Boot Services and EFI Runtime Services tables can be filled in.
+  // Also register for the GUIDs of optional protocols.
+  //
+  CoreNotifyOnProtocolInstallation ();
+
+  //
+  // Produce Firmware Volume Protocols, one for each FV in the HOB list.
+  //
+  Status = FwVolBlockDriverInit (gDxeCoreImageHandle, gDxeCoreST);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = FwVolDriverInit (gDxeCoreImageHandle, gDxeCoreST);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Produce the Section Extraction Protocol
+  //
+  Status = InitializeSectionExtraction (gDxeCoreImageHandle, gDxeCoreST);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Initialize the DXE Dispatcher
+  //
+  PERF_START (NULL,"CoreInitializeDispatcher", "DxeMain", 0) ;
+  CoreInitializeDispatcher ();
+  PERF_END (NULL,"CoreInitializeDispatcher", "DxeMain", 0) ;
+
+  //
+  // Invoke the DXE Dispatcher
+  //
+  PERF_START (NULL, "CoreDispatcher", "DxeMain", 0);
+  CoreDispatcher ();
+  PERF_END (NULL, "CoreDispatcher", "DxeMain", 0);
+
+  //
+  // Display Architectural protocols that were not loaded if this is DEBUG build
+  //
+  DEBUG_CODE_BEGIN ();
+    CoreDisplayMissingArchProtocols ();
+  DEBUG_CODE_END ();
+
+  //
+  // Display any drivers that were not dispatched because dependency expression
+  // evaluated to false if this is a debug build
+  //
+  DEBUG_CODE_BEGIN ();
+    CoreDisplayDiscoveredNotDispatched ();
+  DEBUG_CODE_END ();
+
+  //
+  // Assert if the Architectural Protocols are not present.
+  //
+  Status = CoreAllEfiServicesAvailable ();
+  if (EFI_ERROR(Status)) {
+    //
+    // Report Status code that some Architectural Protocols are not present.
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+      (EFI_SOFTWARE_DXE_CORE | EFI_SW_DXE_CORE_EC_NO_ARCH)
+      );    
+  }
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Report Status code before transfer control to BDS
+  //
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_SOFTWARE_DXE_CORE | EFI_SW_DXE_CORE_PC_HANDOFF_TO_NEXT)
+    );
+
+  //
+  // Transfer control to the BDS Architectural Protocol
+  //
+  gBds->Entry (gBds);
+
+  //
+  // BDS should never return
+  //
+  ASSERT (FALSE);
+  CpuDeadLoop ();
+}
+
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are
+  available.
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg0 (
+  VOID
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are
+  available.
+
+  @param  Arg1                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg1 (
+  UINTN Arg1
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg2 (
+  UINTN Arg1,
+  UINTN Arg2
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg3 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg4 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Place holder function until all the Boot Services and Runtime Services are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+  @param  Arg5                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+CoreEfiNotAvailableYetArg5 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4,
+  UINTN Arg5
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.  The CpuBreakpoint () is commented out for now until the
+  // DXE Core and all the Architectural Protocols are complete.
+  //
+
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+
+/**
+  Calcualte the 32-bit CRC in a EFI table using the service provided by the
+  gRuntime service.
+
+  @param  Hdr                    Pointer to an EFI standard header
+
+**/
+VOID
+CalculateEfiHdrCrc (
+  IN  OUT EFI_TABLE_HEADER    *Hdr
+  )
+{
+  UINT32 Crc;
+
+  Hdr->CRC32 = 0;
+
+  //
+  // If gBS->CalculateCrce32 () == CoreEfiNotAvailableYet () then
+  //  Crc will come back as zero if we set it to zero here
+  //
+  Crc = 0;
+  gBS->CalculateCrc32 ((UINT8 *)Hdr, Hdr->HeaderSize, &Crc);
+  Hdr->CRC32 = Crc;
+}
+
+
+/**
+  Terminates all boot services.
+
+  @param  ImageHandle            Handle that identifies the exiting image.
+  @param  MapKey                 Key to the latest memory map.
+
+  @retval EFI_SUCCESS            Boot Services terminated
+  @retval EFI_INVALID_PARAMETER  MapKey is incorrect.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreExitBootServices (
+  IN EFI_HANDLE   ImageHandle,
+  IN UINTN        MapKey
+  )
+{
+  EFI_STATUS                Status;
+
+  //
+  // Disable Timer
+  //
+  gTimer->SetTimerPeriod (gTimer, 0);
+
+  //
+  // Terminate memory services if the MapKey matches
+  //
+  Status = CoreTerminateMemoryMap (MapKey);
+  if (EFI_ERROR (Status)) {
+    //
+    // Notify other drivers that ExitBootServices fail 
+    //
+    CoreNotifySignalList (&gEventExitBootServicesFailedGuid);
+    return Status;
+  }
+
+  //
+  // Notify other drivers that we are exiting boot services.
+  //
+  CoreNotifySignalList (&gEfiEventExitBootServicesGuid);
+
+  //
+  // Report that ExitBootServices() has been called
+  //
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_SOFTWARE_EFI_BOOT_SERVICE | EFI_SW_BS_PC_EXIT_BOOT_SERVICES)
+    );
+
+  //
+  // Disable interrupt of Debug timer.
+  //
+  SaveAndSetDebugTimerInterrupt (FALSE);
+
+  //
+  // Disable CPU Interrupts
+  //
+  gCpu->DisableInterrupt (gCpu);
+
+  //
+  // Clear the non-runtime values of the EFI System Table
+  //
+  gDxeCoreST->BootServices        = NULL;
+  gDxeCoreST->ConIn               = NULL;
+  gDxeCoreST->ConsoleInHandle     = NULL;
+  gDxeCoreST->ConOut              = NULL;
+  gDxeCoreST->ConsoleOutHandle    = NULL;
+  gDxeCoreST->StdErr              = NULL;
+  gDxeCoreST->StandardErrorHandle = NULL;
+
+  //
+  // Recompute the 32-bit CRC of the EFI System Table
+  //
+  CalculateEfiHdrCrc (&gDxeCoreST->Hdr);
+
+  //
+  // Zero out the Boot Service Table
+  //
+  ZeroMem (gBS, sizeof (EFI_BOOT_SERVICES));
+  gBS = NULL;
+
+  //
+  // Update the AtRuntime field in Runtiem AP.
+  //
+  gRuntime->AtRuntime = TRUE;
+
+  return Status;
+}
+
+
+/**
+  Given a compressed source buffer, this function retrieves the size of the
+  uncompressed buffer and the size of the scratch buffer required to decompress
+  the compressed source buffer.
+
+  The GetInfo() function retrieves the size of the uncompressed buffer and the
+  temporary scratch buffer required to decompress the buffer specified by Source
+  and SourceSize. If the size of the uncompressed buffer or the size of the
+  scratch buffer cannot be determined from the compressed data specified by
+  Source and SourceData, then EFI_INVALID_PARAMETER is returned. Otherwise, the
+  size of the uncompressed buffer is returned in DestinationSize, the size of
+  the scratch buffer is returned in ScratchSize, and EFI_SUCCESS is returned.
+  The GetInfo() function does not have scratch buffer available to perform a
+  thorough checking of the validity of the source data. It just retrieves the
+  "Original Size" field from the beginning bytes of the source data and output
+  it as DestinationSize. And ScratchSize is specific to the decompression
+  implementation.
+
+  @param  This               A pointer to the EFI_DECOMPRESS_PROTOCOL instance.
+  @param  Source             The source buffer containing the compressed data.
+  @param  SourceSize         The size, in bytes, of the source buffer.
+  @param  DestinationSize    A pointer to the size, in bytes, of the
+                             uncompressed buffer that will be generated when the
+                             compressed buffer specified by Source and
+                             SourceSize is decompressed.
+  @param  ScratchSize        A pointer to the size, in bytes, of the scratch
+                             buffer that is required to decompress the
+                             compressed buffer specified by Source and
+                             SourceSize.
+
+  @retval EFI_SUCCESS        The size of the uncompressed data was returned in
+                             DestinationSize and the size of the scratch buffer
+                             was returned in ScratchSize.
+  @retval EFI_INVALID_PARAMETER The size of the uncompressed data or the size of
+                                the scratch buffer cannot be determined from the
+                                compressed data specified by Source and
+                                SourceSize.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeMainUefiDecompressGetInfo (
+  IN EFI_DECOMPRESS_PROTOCOL            *This,
+  IN   VOID                             *Source,
+  IN   UINT32                           SourceSize,
+  OUT  UINT32                           *DestinationSize,
+  OUT  UINT32                           *ScratchSize
+  )
+{
+  if (Source == NULL || DestinationSize == NULL || ScratchSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return UefiDecompressGetInfo (Source, SourceSize, DestinationSize, ScratchSize);
+}
+
+
+/**
+  Decompresses a compressed source buffer.
+
+  The Decompress() function extracts decompressed data to its original form.
+  This protocol is designed so that the decompression algorithm can be
+  implemented without using any memory services. As a result, the Decompress()
+  Function is not allowed to call AllocatePool() or AllocatePages() in its
+  implementation. It is the caller's responsibility to allocate and free the
+  Destination and Scratch buffers.
+  If the compressed source data specified by Source and SourceSize is
+  successfully decompressed into Destination, then EFI_SUCCESS is returned. If
+  the compressed source data specified by Source and SourceSize is not in a
+  valid compressed data format, then EFI_INVALID_PARAMETER is returned.
+
+  @param  This                A pointer to the EFI_DECOMPRESS_PROTOCOL instance.
+  @param  Source              The source buffer containing the compressed data.
+  @param  SourceSize          SourceSizeThe size of source data.
+  @param  Destination         On output, the destination buffer that contains
+                              the uncompressed data.
+  @param  DestinationSize     The size of the destination buffer.  The size of
+                              the destination buffer needed is obtained from
+                              EFI_DECOMPRESS_PROTOCOL.GetInfo().
+  @param  Scratch             A temporary scratch buffer that is used to perform
+                              the decompression.
+  @param  ScratchSize         The size of scratch buffer. The size of the
+                              scratch buffer needed is obtained from GetInfo().
+
+  @retval EFI_SUCCESS         Decompression completed successfully, and the
+                              uncompressed buffer is returned in Destination.
+  @retval EFI_INVALID_PARAMETER  The source buffer specified by Source and
+                                 SourceSize is corrupted (not in a valid
+                                 compressed format).
+
+**/
+EFI_STATUS
+EFIAPI
+DxeMainUefiDecompress (
+  IN     EFI_DECOMPRESS_PROTOCOL          *This,
+  IN     VOID                             *Source,
+  IN     UINT32                           SourceSize,
+  IN OUT VOID                             *Destination,
+  IN     UINT32                           DestinationSize,
+  IN OUT VOID                             *Scratch,
+  IN     UINT32                           ScratchSize
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      TestDestinationSize;
+  UINT32      TestScratchSize;
+
+  if (Source == NULL || Destination== NULL || Scratch == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = UefiDecompressGetInfo (Source, SourceSize, &TestDestinationSize, &TestScratchSize);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (ScratchSize < TestScratchSize || DestinationSize < TestDestinationSize) {
+    return RETURN_INVALID_PARAMETER;
+  }
+
+  return UefiDecompress (Source, Destination, Scratch);
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeProtocolNotify.c b/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeProtocolNotify.c
new file mode 100644
index 0000000000..034520c5c6
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/DxeMain/DxeProtocolNotify.c
@@ -0,0 +1,285 @@
+/** @file
+  This file deals with Architecture Protocol (AP) registration in
+  the Dxe Core. The mArchProtocols[] array represents a list of
+  events that represent the Architectural Protocols.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+
+//
+// DXE Core Global Variables for all of the Architectural Protocols.
+// If a protocol is installed mArchProtocols[].Present will be TRUE.
+//
+// CoreNotifyOnArchProtocolInstallation () fills in mArchProtocols[].Event
+// and mArchProtocols[].Registration as it creates events for every array
+// entry.
+//
+EFI_CORE_PROTOCOL_NOTIFY_ENTRY  mArchProtocols[] = {
+  { &gEfiSecurityArchProtocolGuid,         (VOID **)&gSecurity,      NULL, NULL, FALSE },
+  { &gEfiCpuArchProtocolGuid,              (VOID **)&gCpu,           NULL, NULL, FALSE },
+  { &gEfiMetronomeArchProtocolGuid,        (VOID **)&gMetronome,     NULL, NULL, FALSE },
+  { &gEfiTimerArchProtocolGuid,            (VOID **)&gTimer,         NULL, NULL, FALSE },
+  { &gEfiBdsArchProtocolGuid,              (VOID **)&gBds,           NULL, NULL, FALSE },
+  { &gEfiWatchdogTimerArchProtocolGuid,    (VOID **)&gWatchdogTimer, NULL, NULL, FALSE },
+  { &gEfiRuntimeArchProtocolGuid,          (VOID **)&gRuntime,       NULL, NULL, FALSE },
+  { &gEfiVariableArchProtocolGuid,         (VOID **)NULL,            NULL, NULL, FALSE },
+  { &gEfiVariableWriteArchProtocolGuid,    (VOID **)NULL,            NULL, NULL, FALSE },
+  { &gEfiCapsuleArchProtocolGuid,          (VOID **)NULL,            NULL, NULL, FALSE },
+  { &gEfiMonotonicCounterArchProtocolGuid, (VOID **)NULL,            NULL, NULL, FALSE },
+  { &gEfiResetArchProtocolGuid,            (VOID **)NULL,            NULL, NULL, FALSE },
+  { &gEfiRealTimeClockArchProtocolGuid,    (VOID **)NULL,            NULL, NULL, FALSE },
+  { NULL,                                  (VOID **)NULL,            NULL, NULL, FALSE }
+};
+
+//
+// Optional protocols that the DXE Core will use if they are present
+//
+EFI_CORE_PROTOCOL_NOTIFY_ENTRY  mOptionalProtocols[] = {
+  { &gEfiSecurity2ArchProtocolGuid,        (VOID **)&gSecurity2,     NULL, NULL, FALSE },
+  { &gEfiSmmBase2ProtocolGuid,             (VOID **)&gSmmBase2,      NULL, NULL, FALSE },
+  { NULL,                                  (VOID **)NULL,            NULL, NULL, FALSE }
+};
+
+//
+// Following is needed to display missing architectural protocols in debug builds
+//
+typedef struct {
+  EFI_GUID  *ProtocolGuid;
+  CHAR8     *GuidString;
+} GUID_TO_STRING_PROTOCOL_ENTRY;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST GUID_TO_STRING_PROTOCOL_ENTRY mMissingProtocols[] = {
+  { &gEfiSecurityArchProtocolGuid,         "Security"           },
+  { &gEfiCpuArchProtocolGuid,              "CPU"                },
+  { &gEfiMetronomeArchProtocolGuid,        "Metronome"          },
+  { &gEfiTimerArchProtocolGuid,            "Timer"              },
+  { &gEfiBdsArchProtocolGuid,              "Bds"                },
+  { &gEfiWatchdogTimerArchProtocolGuid,    "Watchdog Timer"     },
+  { &gEfiRuntimeArchProtocolGuid,          "Runtime"            },
+  { &gEfiVariableArchProtocolGuid,         "Variable"           },
+  { &gEfiVariableWriteArchProtocolGuid,    "Variable Write"     },
+  { &gEfiCapsuleArchProtocolGuid,          "Capsule"            },
+  { &gEfiMonotonicCounterArchProtocolGuid, "Monotonic Counter"  },
+  { &gEfiResetArchProtocolGuid,            "Reset"              },
+  { &gEfiRealTimeClockArchProtocolGuid,    "Real Time Clock"    },
+  { NULL,                                  ""                   }
+};
+
+/**
+  Return TRUE if all AP services are availible.
+
+  @retval EFI_SUCCESS    All AP services are available
+  @retval EFI_NOT_FOUND  At least one AP service is not available
+
+**/
+EFI_STATUS
+CoreAllEfiServicesAvailable (
+  VOID
+  )
+{
+  EFI_CORE_PROTOCOL_NOTIFY_ENTRY  *Entry;
+
+  for (Entry = mArchProtocols; Entry->ProtocolGuid != NULL; Entry++) {
+    if (!Entry->Present) {
+      return EFI_NOT_FOUND;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Notification event handler registered by CoreNotifyOnArchProtocolInstallation ().
+  This notify function is registered for every architectural protocol. This handler
+  updates mArchProtocol[] array entry with protocol instance data and sets it's
+  present flag to TRUE. If any constructor is required it is executed. The EFI
+  System Table headers are updated.
+
+  @param  Event          The Event that is being processed, not used.
+  @param  Context        Event Context, not used.
+
+**/
+VOID
+EFIAPI
+GenericProtocolNotify (
+  IN  EFI_EVENT  Event,
+  IN  VOID       *Context
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_CORE_PROTOCOL_NOTIFY_ENTRY  *Entry;
+  VOID                            *Protocol;
+  LIST_ENTRY                      *Link;
+  LIST_ENTRY                      TempLinkNode;
+
+  Protocol = NULL;
+
+  //
+  // Get Entry from Context
+  //
+  Entry = (EFI_CORE_PROTOCOL_NOTIFY_ENTRY *)Context;
+
+  //
+  // See if the expected protocol is present in the handle database
+  //
+  Status = CoreLocateProtocol (Entry->ProtocolGuid, Entry->Registration, &Protocol);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Mark the protocol as present
+  //
+  Entry->Present = TRUE;
+
+  //
+  // Update protocol global variable if one exists. Entry->Protocol points to a global variable
+  // if one exists in the DXE core for this Architectural Protocol
+  //
+  if (Entry->Protocol != NULL) {
+    *(Entry->Protocol) = Protocol;
+  }
+
+  //
+  // Do special operations for Architectural Protocols
+  //
+
+  if (CompareGuid (Entry->ProtocolGuid, &gEfiTimerArchProtocolGuid)) {
+    //
+    // Register the Core timer tick handler with the Timer AP
+    //
+    gTimer->RegisterHandler (gTimer, CoreTimerTick);
+  }
+
+  if (CompareGuid (Entry->ProtocolGuid, &gEfiRuntimeArchProtocolGuid)) {
+    //
+    // When runtime architectural protocol is available, updates CRC32 in the Debug Table
+    //
+    CoreUpdateDebugTableCrc32 ();
+
+    //
+    // Update the Runtime Architectural protocol with the template that the core was
+    // using so there would not need to be a dependency on the Runtime AP
+    //
+
+    //
+    // Copy all the registered Image to new gRuntime protocol
+    //
+    for (Link = gRuntimeTemplate.ImageHead.ForwardLink; Link != &gRuntimeTemplate.ImageHead; Link = TempLinkNode.ForwardLink) {
+      CopyMem (&TempLinkNode, Link, sizeof(LIST_ENTRY));
+      InsertTailList (&gRuntime->ImageHead, Link);
+    }
+    //
+    // Copy all the registered Event to new gRuntime protocol
+    //
+    for (Link = gRuntimeTemplate.EventHead.ForwardLink; Link != &gRuntimeTemplate.EventHead; Link = TempLinkNode.ForwardLink) {
+      CopyMem (&TempLinkNode, Link, sizeof(LIST_ENTRY));
+      InsertTailList (&gRuntime->EventHead, Link);
+    }
+
+    //
+    // Clean up gRuntimeTemplate
+    //
+    gRuntimeTemplate.ImageHead.ForwardLink = &gRuntimeTemplate.ImageHead;
+    gRuntimeTemplate.ImageHead.BackLink    = &gRuntimeTemplate.ImageHead;
+    gRuntimeTemplate.EventHead.ForwardLink = &gRuntimeTemplate.EventHead;
+    gRuntimeTemplate.EventHead.BackLink    = &gRuntimeTemplate.EventHead;
+  }
+
+  //
+  // It's over kill to do them all every time, but it saves a lot of code.
+  //
+  CalculateEfiHdrCrc (&gDxeCoreRT->Hdr);
+  CalculateEfiHdrCrc (&gBS->Hdr);
+  CalculateEfiHdrCrc (&gDxeCoreST->Hdr);
+  CalculateEfiHdrCrc (&gDxeCoreDS->Hdr);
+}
+
+/**
+  Creates an event for each entry in a table that is fired everytime a Protocol 
+  of a specific type is installed.
+
+  @param Entry  Pointer to EFI_CORE_PROTOCOL_NOTIFY_ENTRY.
+
+**/
+VOID
+CoreNotifyOnProtocolEntryTable (
+  EFI_CORE_PROTOCOL_NOTIFY_ENTRY  *Entry
+  )
+{
+  EFI_STATUS  Status;
+
+  for (; Entry->ProtocolGuid != NULL; Entry++) {
+    //
+    // Create the event
+    //
+    Status = CoreCreateEvent (
+              EVT_NOTIFY_SIGNAL,
+              TPL_CALLBACK,
+              GenericProtocolNotify,
+              Entry,
+              &Entry->Event
+              );
+    ASSERT_EFI_ERROR(Status);
+
+    //
+    // Register for protocol notifactions on this event
+    //
+    Status = CoreRegisterProtocolNotify (
+              Entry->ProtocolGuid,
+              Entry->Event,
+              &Entry->Registration
+              );
+    ASSERT_EFI_ERROR(Status);
+  }
+}
+
+/**
+  Creates an events for the Architectural Protocols and the optional protocols 
+  that are fired everytime a Protocol of a specific type is installed.
+
+**/
+VOID
+CoreNotifyOnProtocolInstallation (
+  VOID
+  )
+{
+  CoreNotifyOnProtocolEntryTable (mArchProtocols);
+  CoreNotifyOnProtocolEntryTable (mOptionalProtocols);
+}
+
+
+/**
+  Displays Architectural protocols that were not loaded and are required for DXE
+  core to function.  Only used in Debug Builds.
+
+**/
+VOID
+CoreDisplayMissingArchProtocols (
+  VOID
+  )
+{
+  EFI_CORE_PROTOCOL_NOTIFY_ENTRY       *Entry;
+  CONST GUID_TO_STRING_PROTOCOL_ENTRY  *MissingEntry;
+
+  for (Entry = mArchProtocols; Entry->ProtocolGuid != NULL; Entry++) {
+    if (!Entry->Present) {
+      for (MissingEntry = mMissingProtocols; MissingEntry->ProtocolGuid != NULL; MissingEntry++) {
+        if (CompareGuid (Entry->ProtocolGuid, MissingEntry->ProtocolGuid)) {
+          DEBUG ((DEBUG_ERROR, "\n%a Arch Protocol not present!!\n", MissingEntry->GuidString));
+          break;
+        }
+      }
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Event/Event.c b/Core/MdeModulePkg/Core/Dxe/Event/Event.c
new file mode 100644
index 0000000000..a8c4c34d83
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Event/Event.c
@@ -0,0 +1,774 @@
+/** @file
+  UEFI Event support functions implemented in this file.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "Event.h"
+
+///
+/// gEfiCurrentTpl - Current Task priority level
+///
+EFI_TPL  gEfiCurrentTpl = TPL_APPLICATION;
+
+///
+/// gEventQueueLock - Protects the event queues
+///
+EFI_LOCK gEventQueueLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_HIGH_LEVEL);
+
+///
+/// gEventQueue - A list of event's to notify for each priority level
+///
+LIST_ENTRY      gEventQueue[TPL_HIGH_LEVEL + 1];
+
+///
+/// gEventPending - A bitmask of the EventQueues that are pending
+///
+UINTN           gEventPending = 0;
+
+///
+/// gEventSignalQueue - A list of events to signal based on EventGroup type
+///
+LIST_ENTRY      gEventSignalQueue = INITIALIZE_LIST_HEAD_VARIABLE (gEventSignalQueue);
+
+///
+/// Enumerate the valid types
+///
+UINT32 mEventTable[] = {
+  ///
+  /// 0x80000200       Timer event with a notification function that is
+  /// queue when the event is signaled with SignalEvent()
+  ///
+  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+  ///
+  /// 0x80000000       Timer event without a notification function. It can be
+  /// signaled with SignalEvent() and checked with CheckEvent() or WaitForEvent().
+  ///
+  EVT_TIMER,
+  ///
+  /// 0x00000100       Generic event with a notification function that
+  /// can be waited on with CheckEvent() or WaitForEvent()
+  ///
+  EVT_NOTIFY_WAIT,
+  ///
+  /// 0x00000200       Generic event with a notification function that
+  /// is queue when the event is signaled with SignalEvent()
+  ///
+  EVT_NOTIFY_SIGNAL,
+  ///
+  /// 0x00000201       ExitBootServicesEvent.
+  ///
+  EVT_SIGNAL_EXIT_BOOT_SERVICES,
+  ///
+  /// 0x60000202       SetVirtualAddressMapEvent.
+  ///
+  EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE,
+
+  ///
+  /// 0x00000000       Generic event without a notification function.
+  /// It can be signaled with SignalEvent() and checked with CheckEvent()
+  /// or WaitForEvent().
+  ///
+  0x00000000,
+  ///
+  /// 0x80000100       Timer event with a notification function that can be
+  /// waited on with CheckEvent() or WaitForEvent()
+  ///
+  EVT_TIMER | EVT_NOTIFY_WAIT,
+};
+
+///
+/// gIdleLoopEvent - Event which is signalled when the core is idle
+///
+EFI_EVENT       gIdleLoopEvent = NULL;
+
+
+/**
+  Enter critical section by acquiring the lock on gEventQueueLock.
+
+**/
+VOID
+CoreAcquireEventLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&gEventQueueLock);
+}
+
+
+/**
+  Exit critical section by releasing the lock on gEventQueueLock.
+
+**/
+VOID
+CoreReleaseEventLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&gEventQueueLock);
+}
+
+
+
+/**
+  Initializes "event" support.
+
+  @retval EFI_SUCCESS            Always return success
+
+**/
+EFI_STATUS
+CoreInitializeEventServices (
+  VOID
+  )
+{
+  UINTN        Index;
+
+  for (Index=0; Index <= TPL_HIGH_LEVEL; Index++) {
+    InitializeListHead (&gEventQueue[Index]);
+  }
+
+  CoreInitializeTimer ();
+
+  CoreCreateEventEx (
+    EVT_NOTIFY_SIGNAL,
+    TPL_NOTIFY,
+    CoreEmptyCallbackFunction,
+    NULL,
+    &gIdleLoopEventGuid,
+    &gIdleLoopEvent
+    );
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Dispatches all pending events.
+
+  @param  Priority               The task priority level of event notifications
+                                 to dispatch
+
+**/
+VOID
+CoreDispatchEventNotifies (
+  IN EFI_TPL      Priority
+  )
+{
+  IEVENT          *Event;
+  LIST_ENTRY      *Head;
+
+  CoreAcquireEventLock ();
+  ASSERT (gEventQueueLock.OwnerTpl == Priority);
+  Head = &gEventQueue[Priority];
+
+  //
+  // Dispatch all the pending notifications
+  //
+  while (!IsListEmpty (Head)) {
+
+    Event = CR (Head->ForwardLink, IEVENT, NotifyLink, EVENT_SIGNATURE);
+    RemoveEntryList (&Event->NotifyLink);
+
+    Event->NotifyLink.ForwardLink = NULL;
+
+    //
+    // Only clear the SIGNAL status if it is a SIGNAL type event.
+    // WAIT type events are only cleared in CheckEvent()
+    //
+    if ((Event->Type & EVT_NOTIFY_SIGNAL) != 0) {
+      Event->SignalCount = 0;
+    }
+
+    CoreReleaseEventLock ();
+
+    //
+    // Notify this event
+    //
+    ASSERT (Event->NotifyFunction != NULL);
+    Event->NotifyFunction (Event, Event->NotifyContext);
+
+    //
+    // Check for next pending event
+    //
+    CoreAcquireEventLock ();
+  }
+
+  gEventPending &= ~(UINTN)(1 << Priority);
+  CoreReleaseEventLock ();
+}
+
+
+
+/**
+  Queues the event's notification function to fire.
+
+  @param  Event                  The Event to notify
+
+**/
+VOID
+CoreNotifyEvent (
+  IN  IEVENT      *Event
+  )
+{
+
+  //
+  // Event database must be locked
+  //
+  ASSERT_LOCKED (&gEventQueueLock);
+
+  //
+  // If the event is queued somewhere, remove it
+  //
+
+  if (Event->NotifyLink.ForwardLink != NULL) {
+    RemoveEntryList (&Event->NotifyLink);
+    Event->NotifyLink.ForwardLink = NULL;
+  }
+
+  //
+  // Queue the event to the pending notification list
+  //
+
+  InsertTailList (&gEventQueue[Event->NotifyTpl], &Event->NotifyLink);
+  gEventPending |= (UINTN)(1 << Event->NotifyTpl);
+}
+
+
+
+
+/**
+  Signals all events in the EventGroup.
+
+  @param  EventGroup             The list to signal
+
+**/
+VOID
+CoreNotifySignalList (
+  IN EFI_GUID     *EventGroup
+  )
+{
+  LIST_ENTRY              *Link;
+  LIST_ENTRY              *Head;
+  IEVENT                  *Event;
+
+  CoreAcquireEventLock ();
+
+  Head = &gEventSignalQueue;
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    Event = CR (Link, IEVENT, SignalLink, EVENT_SIGNATURE);
+    if (CompareGuid (&Event->EventGroup, EventGroup)) {
+      CoreNotifyEvent (Event);
+    }
+  }
+
+  CoreReleaseEventLock ();
+}
+
+
+/**
+  Creates an event.
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEvent (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN VOID                     *NotifyContext, OPTIONAL
+  OUT EFI_EVENT               *Event
+  )
+{
+  return CoreCreateEventEx (Type, NotifyTpl, NotifyFunction, NotifyContext, NULL, Event);
+}
+
+
+
+/**
+  Creates an event in a group.
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  EventGroup             GUID for EventGroup if NULL act the same as
+                                 gBS->CreateEvent().
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEventEx (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN CONST VOID               *NotifyContext, OPTIONAL
+  IN CONST EFI_GUID           *EventGroup,    OPTIONAL
+  OUT EFI_EVENT               *Event
+  )
+{
+  //
+  // If it's a notify type of event, check for invalid NotifyTpl
+  //
+  if ((Type & (EVT_NOTIFY_WAIT | EVT_NOTIFY_SIGNAL)) != 0) {
+    if (NotifyTpl != TPL_APPLICATION &&
+        NotifyTpl != TPL_CALLBACK &&
+        NotifyTpl != TPL_NOTIFY) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return CoreCreateEventInternal (Type, NotifyTpl, NotifyFunction, NotifyContext, EventGroup, Event);
+}
+
+/**
+  Creates a general-purpose event structure
+
+  @param  Type                   The type of event to create and its mode and
+                                 attributes
+  @param  NotifyTpl              The task priority level of event notifications
+  @param  NotifyFunction         Pointer to the events notification function
+  @param  NotifyContext          Pointer to the notification functions context;
+                                 corresponds to parameter "Context" in the
+                                 notification function
+  @param  EventGroup             GUID for EventGroup if NULL act the same as
+                                 gBS->CreateEvent().
+  @param  Event                  Pointer to the newly created event if the call
+                                 succeeds; undefined otherwise
+
+  @retval EFI_SUCCESS            The event structure was created
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+  @retval EFI_OUT_OF_RESOURCES   The event could not be allocated
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCreateEventInternal (
+  IN UINT32                   Type,
+  IN EFI_TPL                  NotifyTpl,
+  IN EFI_EVENT_NOTIFY         NotifyFunction, OPTIONAL
+  IN CONST VOID               *NotifyContext, OPTIONAL
+  IN CONST EFI_GUID           *EventGroup,    OPTIONAL
+  OUT EFI_EVENT               *Event
+  )
+{
+  EFI_STATUS      Status;
+  IEVENT          *IEvent;
+  INTN            Index;
+
+
+  if (Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check to make sure no reserved flags are set
+  //
+  Status = EFI_INVALID_PARAMETER;
+  for (Index = 0; Index < (sizeof (mEventTable) / sizeof (UINT32)); Index++) {
+     if (Type == mEventTable[Index]) {
+       Status = EFI_SUCCESS;
+       break;
+     }
+  }
+  if(EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Convert Event type for pre-defined Event groups
+  //
+  if (EventGroup != NULL) {
+    //
+    // For event group, type EVT_SIGNAL_EXIT_BOOT_SERVICES and EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE
+    // are not valid
+    //
+    if ((Type == EVT_SIGNAL_EXIT_BOOT_SERVICES) || (Type == EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if (CompareGuid (EventGroup, &gEfiEventExitBootServicesGuid)) {
+      Type = EVT_SIGNAL_EXIT_BOOT_SERVICES;
+    } else if (CompareGuid (EventGroup, &gEfiEventVirtualAddressChangeGuid)) {
+      Type = EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE;
+    }
+  } else {
+    //
+    // Convert EFI 1.10 Events to their UEFI 2.0 CreateEventEx mapping
+    //
+    if (Type == EVT_SIGNAL_EXIT_BOOT_SERVICES) {
+      EventGroup = &gEfiEventExitBootServicesGuid;
+    } else if (Type == EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE) {
+      EventGroup = &gEfiEventVirtualAddressChangeGuid;
+    }
+  }
+
+  //
+  // If it's a notify type of event, check its parameters
+  //
+  if ((Type & (EVT_NOTIFY_WAIT | EVT_NOTIFY_SIGNAL)) != 0) {
+    //
+    // Check for an invalid NotifyFunction or NotifyTpl
+    //
+    if ((NotifyFunction == NULL) ||
+        (NotifyTpl <= TPL_APPLICATION) ||
+       (NotifyTpl >= TPL_HIGH_LEVEL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+  } else {
+    //
+    // No notification needed, zero ignored values
+    //
+    NotifyTpl = 0;
+    NotifyFunction = NULL;
+    NotifyContext = NULL;
+  }
+
+  //
+  // Allocate and initialize a new event structure.
+  //
+  if ((Type & EVT_RUNTIME) != 0) {
+    IEvent = AllocateRuntimeZeroPool (sizeof (IEVENT));
+  } else {
+    IEvent = AllocateZeroPool (sizeof (IEVENT));
+  }
+  if (IEvent == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  IEvent->Signature = EVENT_SIGNATURE;
+  IEvent->Type = Type;
+
+  IEvent->NotifyTpl      = NotifyTpl;
+  IEvent->NotifyFunction = NotifyFunction;
+  IEvent->NotifyContext  = (VOID *)NotifyContext;
+  if (EventGroup != NULL) {
+    CopyGuid (&IEvent->EventGroup, EventGroup);
+    IEvent->ExFlag = TRUE;
+  }
+
+  *Event = IEvent;
+
+  if ((Type & EVT_RUNTIME) != 0) {
+    //
+    // Keep a list of all RT events so we can tell the RT AP.
+    //
+    IEvent->RuntimeData.Type           = Type;
+    IEvent->RuntimeData.NotifyTpl      = NotifyTpl;
+    IEvent->RuntimeData.NotifyFunction = NotifyFunction;
+    IEvent->RuntimeData.NotifyContext  = (VOID *) NotifyContext;
+    IEvent->RuntimeData.Event          = (EFI_EVENT *) IEvent;
+    InsertTailList (&gRuntime->EventHead, &IEvent->RuntimeData.Link);
+  }
+
+  CoreAcquireEventLock ();
+
+  if ((Type & EVT_NOTIFY_SIGNAL) != 0x00000000) {
+    //
+    // The Event's NotifyFunction must be queued whenever the event is signaled
+    //
+    InsertHeadList (&gEventSignalQueue, &IEvent->SignalLink);
+  }
+
+  CoreReleaseEventLock ();
+
+  //
+  // Done
+  //
+  return EFI_SUCCESS;
+}
+
+
+
+
+/**
+  Signals the event.  Queues the event to be notified if needed.
+
+  @param  UserEvent              The event to signal .
+
+  @retval EFI_INVALID_PARAMETER  Parameters are not valid.
+  @retval EFI_SUCCESS            The event was signaled.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSignalEvent (
+  IN EFI_EVENT    UserEvent
+  )
+{
+  IEVENT          *Event;
+
+  Event = UserEvent;
+
+  if (Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Event->Signature != EVENT_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireEventLock ();
+
+  //
+  // If the event is not already signalled, do so
+  //
+
+  if (Event->SignalCount == 0x00000000) {
+    Event->SignalCount++;
+
+    //
+    // If signalling type is a notify function, queue it
+    //
+    if ((Event->Type & EVT_NOTIFY_SIGNAL) != 0) {
+      if (Event->ExFlag) {
+        //
+        // The CreateEventEx() style requires all members of the Event Group
+        //  to be signaled.
+        //
+        CoreReleaseEventLock ();
+        CoreNotifySignalList (&Event->EventGroup);
+        CoreAcquireEventLock ();
+       } else {
+        CoreNotifyEvent (Event);
+      }
+    }
+  }
+
+  CoreReleaseEventLock ();
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Check the status of an event.
+
+  @param  UserEvent              The event to check
+
+  @retval EFI_SUCCESS            The event is in the signaled state
+  @retval EFI_NOT_READY          The event is not in the signaled state
+  @retval EFI_INVALID_PARAMETER  Event is of type EVT_NOTIFY_SIGNAL
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCheckEvent (
+  IN EFI_EVENT        UserEvent
+  )
+{
+  IEVENT      *Event;
+  EFI_STATUS  Status;
+
+  Event = UserEvent;
+
+  if (Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Event->Signature != EVENT_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Event->Type & EVT_NOTIFY_SIGNAL) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_NOT_READY;
+
+  if ((Event->SignalCount == 0) && ((Event->Type & EVT_NOTIFY_WAIT) != 0)) {
+
+    //
+    // Queue the wait notify function
+    //
+    CoreAcquireEventLock ();
+    if (Event->SignalCount == 0) {
+      CoreNotifyEvent (Event);
+    }
+    CoreReleaseEventLock ();
+  }
+
+  //
+  // If the even looks signalled, get the lock and clear it
+  //
+
+  if (Event->SignalCount != 0) {
+    CoreAcquireEventLock ();
+
+    if (Event->SignalCount != 0) {
+      Event->SignalCount = 0;
+      Status = EFI_SUCCESS;
+    }
+
+    CoreReleaseEventLock ();
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Stops execution until an event is signaled.
+
+  @param  NumberOfEvents         The number of events in the UserEvents array
+  @param  UserEvents             An array of EFI_EVENT
+  @param  UserIndex              Pointer to the index of the event which
+                                 satisfied the wait condition
+
+  @retval EFI_SUCCESS            The event indicated by Index was signaled.
+  @retval EFI_INVALID_PARAMETER  The event indicated by Index has a notification
+                                 function or Event was not a valid type
+  @retval EFI_UNSUPPORTED        The current TPL is not TPL_APPLICATION
+
+**/
+EFI_STATUS
+EFIAPI
+CoreWaitForEvent (
+  IN UINTN        NumberOfEvents,
+  IN EFI_EVENT    *UserEvents,
+  OUT UINTN       *UserIndex
+  )
+{
+  EFI_STATUS      Status;
+  UINTN           Index;
+
+  //
+  // Can only WaitForEvent at TPL_APPLICATION
+  //
+  if (gEfiCurrentTpl != TPL_APPLICATION) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (NumberOfEvents == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (UserEvents == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for(;;) {
+
+    for(Index = 0; Index < NumberOfEvents; Index++) {
+
+      Status = CoreCheckEvent (UserEvents[Index]);
+
+      //
+      // provide index of event that caused problem
+      //
+      if (Status != EFI_NOT_READY) {
+        if (UserIndex != NULL) {
+          *UserIndex = Index;
+        }
+        return Status;
+      }
+    }
+
+    //
+    // Signal the Idle event
+    //
+    CoreSignalEvent (gIdleLoopEvent);
+  }
+}
+
+
+/**
+  Closes an event and frees the event structure.
+
+  @param  UserEvent              Event to close
+
+  @retval EFI_INVALID_PARAMETER  Parameters are not valid.
+  @retval EFI_SUCCESS            The event has been closed
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCloseEvent (
+  IN EFI_EVENT    UserEvent
+  )
+{
+  EFI_STATUS  Status;
+  IEVENT      *Event;
+
+  Event = UserEvent;
+
+  if (Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Event->Signature != EVENT_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If it's a timer event, make sure it's not pending
+  //
+  if ((Event->Type & EVT_TIMER) != 0) {
+    CoreSetTimer (Event, TimerCancel, 0);
+  }
+
+  CoreAcquireEventLock ();
+
+  //
+  // If the event is queued somewhere, remove it
+  //
+
+  if (Event->RuntimeData.Link.ForwardLink != NULL) {
+    RemoveEntryList (&Event->RuntimeData.Link);
+  }
+
+  if (Event->NotifyLink.ForwardLink != NULL) {
+    RemoveEntryList (&Event->NotifyLink);
+  }
+
+  if (Event->SignalLink.ForwardLink != NULL) {
+    RemoveEntryList (&Event->SignalLink);
+  }
+
+  CoreReleaseEventLock ();
+
+  //
+  // If the event is registered on a protocol notify, then remove it from the protocol database
+  //
+  CoreUnregisterProtocolNotify (Event);
+
+  Status = CoreFreePool (Event);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Event/Event.h b/Core/MdeModulePkg/Core/Dxe/Event/Event.h
new file mode 100644
index 0000000000..c110f4d072
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Event/Event.h
@@ -0,0 +1,88 @@
+/** @file
+  UEFI Event support functions and structure.
+
+Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef __EVENT_H__
+#define __EVENT_H__
+
+
+#define VALID_TPL(a)            ((a) <= TPL_HIGH_LEVEL)
+extern  UINTN                   gEventPending;
+
+
+//
+// EFI_EVENT
+//
+
+///
+/// Timer event information
+///
+typedef struct {
+  LIST_ENTRY      Link;
+  UINT64          TriggerTime;
+  UINT64          Period;
+} TIMER_EVENT_INFO;
+
+#define EVENT_SIGNATURE         SIGNATURE_32('e','v','n','t')
+typedef struct {
+  UINTN                   Signature;
+  UINT32                  Type;
+  UINT32                  SignalCount;
+  ///
+  /// Entry if the event is registered to be signalled
+  ///
+  LIST_ENTRY              SignalLink;
+  ///
+  /// Notification information for this event
+  ///
+  EFI_TPL                 NotifyTpl;
+  EFI_EVENT_NOTIFY        NotifyFunction;
+  VOID                    *NotifyContext;
+  EFI_GUID                EventGroup;
+  LIST_ENTRY              NotifyLink;
+  BOOLEAN                 ExFlag;
+  ///
+  /// A list of all runtime events
+  ///
+  EFI_RUNTIME_EVENT_ENTRY RuntimeData;
+  TIMER_EVENT_INFO        Timer;
+} IEVENT;
+
+//
+// Internal prototypes
+//
+
+
+/**
+  Dispatches all pending events.
+
+  @param  Priority               The task priority level of event notifications
+                                 to dispatch
+
+**/
+VOID
+CoreDispatchEventNotifies (
+  IN EFI_TPL      Priority
+  );
+
+
+/**
+  Initializes timer support.
+
+**/
+VOID
+CoreInitializeTimer (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Event/Timer.c b/Core/MdeModulePkg/Core/Dxe/Event/Timer.c
new file mode 100644
index 0000000000..087e55e649
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Event/Timer.c
@@ -0,0 +1,301 @@
+/** @file
+  Core Timer Services
+
+Copyright (c) 2006 - 2013, 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 "DxeMain.h"
+#include "Event.h"
+
+//
+// Internal data
+//
+
+LIST_ENTRY       mEfiTimerList = INITIALIZE_LIST_HEAD_VARIABLE (mEfiTimerList);
+EFI_LOCK         mEfiTimerLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_HIGH_LEVEL - 1);
+EFI_EVENT        mEfiCheckTimerEvent = NULL;
+
+EFI_LOCK         mEfiSystemTimeLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_HIGH_LEVEL);
+UINT64           mEfiSystemTime = 0;
+
+//
+// Timer functions
+//
+/**
+  Inserts the timer event.
+
+  @param  Event                  Points to the internal structure of timer event
+                                 to be installed
+
+**/
+VOID
+CoreInsertEventTimer (
+  IN IEVENT   *Event
+  )
+{
+  UINT64          TriggerTime;
+  LIST_ENTRY      *Link;
+  IEVENT          *Event2;
+
+  ASSERT_LOCKED (&mEfiTimerLock);
+
+  //
+  // Get the timer's trigger time
+  //
+  TriggerTime = Event->Timer.TriggerTime;
+
+  //
+  // Insert the timer into the timer database in assending sorted order
+  //
+  for (Link = mEfiTimerList.ForwardLink; Link != &mEfiTimerList; Link = Link->ForwardLink) {
+    Event2 = CR (Link, IEVENT, Timer.Link, EVENT_SIGNATURE);
+
+    if (Event2->Timer.TriggerTime > TriggerTime) {
+      break;
+    }
+  }
+
+  InsertTailList (Link, &Event->Timer.Link);
+}
+
+/**
+  Returns the current system time.
+
+  @return The current system time
+
+**/
+UINT64
+CoreCurrentSystemTime (
+  VOID
+  )
+{
+  UINT64          SystemTime;
+
+  CoreAcquireLock (&mEfiSystemTimeLock);
+  SystemTime = mEfiSystemTime;
+  CoreReleaseLock (&mEfiSystemTimeLock);
+
+  return SystemTime;
+}
+
+/**
+  Checks the sorted timer list against the current system time.
+  Signals any expired event timer.
+
+  @param  CheckEvent             Not used
+  @param  Context                Not used
+
+**/
+VOID
+EFIAPI
+CoreCheckTimers (
+  IN EFI_EVENT            CheckEvent,
+  IN VOID                 *Context
+  )
+{
+  UINT64                  SystemTime;
+  IEVENT                  *Event;
+
+  //
+  // Check the timer database for expired timers
+  //
+  CoreAcquireLock (&mEfiTimerLock);
+  SystemTime = CoreCurrentSystemTime ();
+
+  while (!IsListEmpty (&mEfiTimerList)) {
+    Event = CR (mEfiTimerList.ForwardLink, IEVENT, Timer.Link, EVENT_SIGNATURE);
+
+    //
+    // If this timer is not expired, then we're done
+    //
+    if (Event->Timer.TriggerTime > SystemTime) {
+      break;
+    }
+
+    //
+    // Remove this timer from the timer queue
+    //
+
+    RemoveEntryList (&Event->Timer.Link);
+    Event->Timer.Link.ForwardLink = NULL;
+
+    //
+    // Signal it
+    //
+    CoreSignalEvent (Event);
+
+    //
+    // If this is a periodic timer, set it
+    //
+    if (Event->Timer.Period != 0) {
+      //
+      // Compute the timers new trigger time
+      //
+      Event->Timer.TriggerTime = Event->Timer.TriggerTime + Event->Timer.Period;
+
+      //
+      // If that's before now, then reset the timer to start from now
+      //
+      if (Event->Timer.TriggerTime <= SystemTime) {
+        Event->Timer.TriggerTime = SystemTime;
+        CoreSignalEvent (mEfiCheckTimerEvent);
+      }
+
+      //
+      // Add the timer
+      //
+      CoreInsertEventTimer (Event);
+    }
+  }
+
+  CoreReleaseLock (&mEfiTimerLock);
+}
+
+
+/**
+  Initializes timer support.
+
+**/
+VOID
+CoreInitializeTimer (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = CoreCreateEventInternal (
+             EVT_NOTIFY_SIGNAL,
+             TPL_HIGH_LEVEL - 1,
+             CoreCheckTimers,
+             NULL,
+             NULL,
+             &mEfiCheckTimerEvent
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+
+/**
+  Called by the platform code to process a tick.
+
+  @param  Duration               The number of 100ns elasped since the last call
+                                 to TimerTick
+
+**/
+VOID
+EFIAPI
+CoreTimerTick (
+  IN UINT64   Duration
+  )
+{
+  IEVENT          *Event;
+
+  //
+  // Check runtiem flag in case there are ticks while exiting boot services
+  //
+  CoreAcquireLock (&mEfiSystemTimeLock);
+
+  //
+  // Update the system time
+  //
+  mEfiSystemTime += Duration;
+
+  //
+  // If the head of the list is expired, fire the timer event
+  // to process it
+  //
+  if (!IsListEmpty (&mEfiTimerList)) {
+    Event = CR (mEfiTimerList.ForwardLink, IEVENT, Timer.Link, EVENT_SIGNATURE);
+
+    if (Event->Timer.TriggerTime <= mEfiSystemTime) {
+      CoreSignalEvent (mEfiCheckTimerEvent);
+    }
+  }
+
+  CoreReleaseLock (&mEfiSystemTimeLock);
+}
+
+
+
+/**
+  Sets the type of timer and the trigger time for a timer event.
+
+  @param  UserEvent              The timer event that is to be signaled at the
+                                 specified time
+  @param  Type                   The type of time that is specified in
+                                 TriggerTime
+  @param  TriggerTime            The number of 100ns units until the timer
+                                 expires
+
+  @retval EFI_SUCCESS            The event has been set to be signaled at the
+                                 requested time
+  @retval EFI_INVALID_PARAMETER  Event or Type is not valid
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetTimer (
+  IN EFI_EVENT            UserEvent,
+  IN EFI_TIMER_DELAY      Type,
+  IN UINT64               TriggerTime
+  )
+{
+  IEVENT      *Event;
+
+  Event = UserEvent;
+
+  if (Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Event->Signature != EVENT_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32)Type > TimerRelative  || (Event->Type & EVT_TIMER) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireLock (&mEfiTimerLock);
+
+  //
+  // If the timer is queued to the timer database, remove it
+  //
+  if (Event->Timer.Link.ForwardLink != NULL) {
+    RemoveEntryList (&Event->Timer.Link);
+    Event->Timer.Link.ForwardLink = NULL;
+  }
+
+  Event->Timer.TriggerTime = 0;
+  Event->Timer.Period = 0;
+
+  if (Type != TimerCancel) {
+
+    if (Type == TimerPeriodic) {
+      if (TriggerTime == 0) {
+        gTimer->GetTimerPeriod (gTimer, &TriggerTime);
+      }
+      Event->Timer.Period = TriggerTime;
+    }
+
+    Event->Timer.TriggerTime = CoreCurrentSystemTime () + TriggerTime;
+    CoreInsertEventTimer (Event);
+
+    if (TriggerTime == 0) {
+      CoreSignalEvent (mEfiCheckTimerEvent);
+    }
+  }
+
+  CoreReleaseLock (&mEfiTimerLock);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Event/Tpl.c b/Core/MdeModulePkg/Core/Dxe/Event/Tpl.c
new file mode 100644
index 0000000000..8ad0a33701
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Event/Tpl.c
@@ -0,0 +1,148 @@
+/** @file
+  Task priority (TPL) functions.
+
+Copyright (c) 2006 - 2015, 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 "DxeMain.h"
+#include "Event.h"
+
+/**
+  Set Interrupt State.
+
+  @param  Enable  The state of enable or disable interrupt
+
+**/
+VOID
+CoreSetInterruptState (
+  IN BOOLEAN      Enable
+  )
+{
+  EFI_STATUS  Status;
+  BOOLEAN     InSmm;
+  
+  if (gCpu == NULL) {
+    return;
+  }
+  if (!Enable) {
+    gCpu->DisableInterrupt (gCpu);
+    return;
+  }
+  if (gSmmBase2 == NULL) {
+    gCpu->EnableInterrupt (gCpu);
+    return;
+  }
+  Status = gSmmBase2->InSmm (gSmmBase2, &InSmm);
+  if (!EFI_ERROR (Status) && !InSmm) {
+    gCpu->EnableInterrupt(gCpu);
+  }
+}
+
+
+/**
+  Raise the task priority level to the new level.
+  High level is implemented by disabling processor interrupts.
+
+  @param  NewTpl  New task priority level
+
+  @return The previous task priority level
+
+**/
+EFI_TPL
+EFIAPI
+CoreRaiseTpl (
+  IN EFI_TPL      NewTpl
+  )
+{
+  EFI_TPL     OldTpl;
+
+  OldTpl = gEfiCurrentTpl;
+  if (OldTpl > NewTpl) {
+    DEBUG ((EFI_D_ERROR, "FATAL ERROR - RaiseTpl with OldTpl(0x%x) > NewTpl(0x%x)\n", OldTpl, NewTpl));
+    ASSERT (FALSE);
+  }
+  ASSERT (VALID_TPL (NewTpl));
+
+  //
+  // If raising to high level, disable interrupts
+  //
+  if (NewTpl >= TPL_HIGH_LEVEL  &&  OldTpl < TPL_HIGH_LEVEL) {
+    CoreSetInterruptState (FALSE);
+  }
+
+  //
+  // Set the new value
+  //
+  gEfiCurrentTpl = NewTpl;
+
+  return OldTpl;
+}
+
+
+
+
+/**
+  Lowers the task priority to the previous value.   If the new
+  priority unmasks events at a higher priority, they are dispatched.
+
+  @param  NewTpl  New, lower, task priority
+
+**/
+VOID
+EFIAPI
+CoreRestoreTpl (
+  IN EFI_TPL NewTpl
+  )
+{
+  EFI_TPL     OldTpl;
+
+  OldTpl = gEfiCurrentTpl;
+  if (NewTpl > OldTpl) {
+    DEBUG ((EFI_D_ERROR, "FATAL ERROR - RestoreTpl with NewTpl(0x%x) > OldTpl(0x%x)\n", NewTpl, OldTpl));
+    ASSERT (FALSE);
+  }
+  ASSERT (VALID_TPL (NewTpl));
+
+  //
+  // If lowering below HIGH_LEVEL, make sure
+  // interrupts are enabled
+  //
+
+  if (OldTpl >= TPL_HIGH_LEVEL  &&  NewTpl < TPL_HIGH_LEVEL) {
+    gEfiCurrentTpl = TPL_HIGH_LEVEL;
+  }
+
+  //
+  // Dispatch any pending events
+  //
+  while (((-2 << NewTpl) & gEventPending) != 0) {
+    gEfiCurrentTpl = (UINTN) HighBitSet64 (gEventPending);
+    if (gEfiCurrentTpl < TPL_HIGH_LEVEL) {
+      CoreSetInterruptState (TRUE);
+    }
+    CoreDispatchEventNotifies (gEfiCurrentTpl);
+  }
+
+  //
+  // Set the new value
+  //
+
+  gEfiCurrentTpl = NewTpl;
+
+  //
+  // If lowering below HIGH_LEVEL, make sure
+  // interrupts are enabled
+  //
+  if (gEfiCurrentTpl < TPL_HIGH_LEVEL) {
+    CoreSetInterruptState (TRUE);
+  }
+
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/Ffs.c b/Core/MdeModulePkg/Core/Dxe/FwVol/Ffs.c
new file mode 100644
index 0000000000..20dade18a2
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/Ffs.c
@@ -0,0 +1,233 @@
+/** @file
+  FFS file access utilities.
+
+Copyright (c) 2006 - 2011, 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 "DxeMain.h"
+#include "FwVolDriver.h"
+
+
+/**
+  Get the FFS file state by checking the highest bit set in the header's state field.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file header
+
+  @return FFS File state
+
+**/
+EFI_FFS_FILE_STATE
+GetFileState (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  )
+{
+  EFI_FFS_FILE_STATE      FileState;
+  UINT8                   HighestBit;
+
+  FileState = FfsHeader->State;
+
+  if (ErasePolarity != 0) {
+    FileState = (EFI_FFS_FILE_STATE)~FileState;
+  }
+
+  HighestBit = 0x80;
+  while (HighestBit != 0 && ((HighestBit & FileState) == 0)) {
+    HighestBit >>= 1;
+  }
+
+  return (EFI_FFS_FILE_STATE) HighestBit;
+}
+
+
+
+/**
+  Check if a block of buffer is erased.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  InBuffer       The buffer to be checked
+  @param  BufferSize     Size of the buffer in bytes
+
+  @retval TRUE           The block of buffer is erased
+  @retval FALSE          The block of buffer is not erased
+
+**/
+BOOLEAN
+IsBufferErased (
+  IN UINT8    ErasePolarity,
+  IN VOID     *InBuffer,
+  IN UINTN    BufferSize
+  )
+{
+  UINTN   Count;
+  UINT8   EraseByte;
+  UINT8   *Buffer;
+
+  if(ErasePolarity == 1) {
+    EraseByte = 0xFF;
+  } else {
+    EraseByte = 0;
+  }
+
+  Buffer = InBuffer;
+  for (Count = 0; Count < BufferSize; Count++) {
+    if (Buffer[Count] != EraseByte) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+
+/**
+  Verify checksum of the firmware volume header.
+
+  @param  FvHeader       Points to the firmware volume header to be checked
+
+  @retval TRUE           Checksum verification passed
+  @retval FALSE          Checksum verification failed
+
+**/
+BOOLEAN
+VerifyFvHeaderChecksum (
+  IN EFI_FIRMWARE_VOLUME_HEADER *FvHeader
+  )
+{
+  UINT16  Checksum;
+
+  Checksum = CalculateSum16 ((UINT16 *) FvHeader, FvHeader->HeaderLength);
+
+  if (Checksum == 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+
+/**
+  Verify checksum of the FFS file header.
+
+  @param  FfsHeader      Points to the FFS file header to be checked
+
+  @retval TRUE           Checksum verification passed
+  @retval FALSE          Checksum verification failed
+
+**/
+BOOLEAN
+VerifyHeaderChecksum (
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  )
+{
+  UINT8 HeaderChecksum;
+
+  if (IS_FFS_FILE2 (FfsHeader)) {
+    HeaderChecksum = CalculateSum8 ((UINT8 *) FfsHeader, sizeof (EFI_FFS_FILE_HEADER2));
+  } else {
+    HeaderChecksum = CalculateSum8 ((UINT8 *) FfsHeader, sizeof (EFI_FFS_FILE_HEADER));
+  }
+  HeaderChecksum = (UINT8) (HeaderChecksum - FfsHeader->State - FfsHeader->IntegrityCheck.Checksum.File);
+
+  if (HeaderChecksum == 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+
+
+/**
+  Check if it's a valid FFS file header.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file header to be checked
+  @param  FileState      FFS file state to be returned
+
+  @retval TRUE           Valid FFS file header
+  @retval FALSE          Invalid FFS file header
+
+**/
+BOOLEAN
+IsValidFfsHeader (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader,
+  OUT EFI_FFS_FILE_STATE  *FileState
+  )
+{
+  *FileState = GetFileState (ErasePolarity, FfsHeader);
+
+  switch (*FileState) {
+  case EFI_FILE_HEADER_VALID:
+  case EFI_FILE_DATA_VALID:
+  case EFI_FILE_MARKED_FOR_UPDATE:
+  case EFI_FILE_DELETED:
+    //
+    // Here we need to verify header checksum
+    //
+    return VerifyHeaderChecksum (FfsHeader);
+
+  case EFI_FILE_HEADER_CONSTRUCTION:
+  case EFI_FILE_HEADER_INVALID:
+  default:
+    return FALSE;
+  }
+}
+
+
+/**
+  Check if it's a valid FFS file.
+  Here we are sure that it has a valid FFS file header since we must call IsValidFfsHeader() first.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file to be checked
+
+  @retval TRUE           Valid FFS file
+  @retval FALSE          Invalid FFS file
+
+**/
+BOOLEAN
+IsValidFfsFile (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  )
+{
+  EFI_FFS_FILE_STATE  FileState;
+  UINT8               DataCheckSum;
+
+  FileState = GetFileState (ErasePolarity, FfsHeader);
+  switch (FileState) {
+
+  case EFI_FILE_DELETED:
+  case EFI_FILE_DATA_VALID:
+  case EFI_FILE_MARKED_FOR_UPDATE:
+    DataCheckSum = FFS_FIXED_CHECKSUM;
+    if ((FfsHeader->Attributes & FFS_ATTRIB_CHECKSUM) == FFS_ATTRIB_CHECKSUM) {
+      if (IS_FFS_FILE2 (FfsHeader)) {
+        DataCheckSum = CalculateCheckSum8 ((CONST UINT8 *) FfsHeader + sizeof (EFI_FFS_FILE_HEADER2), FFS_FILE2_SIZE (FfsHeader) - sizeof(EFI_FFS_FILE_HEADER2));
+      } else {
+        DataCheckSum = CalculateCheckSum8 ((CONST UINT8 *) FfsHeader + sizeof (EFI_FFS_FILE_HEADER), FFS_FILE_SIZE (FfsHeader) - sizeof(EFI_FFS_FILE_HEADER));
+      }
+    }
+    if (FfsHeader->IntegrityCheck.Checksum.File == DataCheckSum) {
+      return TRUE;
+    }
+
+  default:
+    return FALSE;
+  }
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/FwVol.c b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVol.c
new file mode 100644
index 0000000000..0ca765d691
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVol.c
@@ -0,0 +1,775 @@
+/** @file
+  Firmware File System driver that produce Firmware Volume protocol.
+  Layers on top of Firmware Block protocol to produce a file abstraction
+  of FV based files.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "FwVolDriver.h"
+
+
+//
+// Protocol notify related globals
+//
+VOID          *gEfiFwVolBlockNotifyReg;
+EFI_EVENT     gEfiFwVolBlockEvent;
+
+FV_DEVICE mFvDevice = {
+  FV2_DEVICE_SIGNATURE,
+  NULL,
+  NULL,
+  {
+    FvGetVolumeAttributes,
+    FvSetVolumeAttributes,
+    FvReadFile,
+    FvReadFileSection,
+    FvWriteFile,
+    FvGetNextFile,   
+	sizeof (UINTN),
+    NULL,
+    FvGetVolumeInfo,
+    FvSetVolumeInfo
+  },
+  NULL,
+  NULL,
+  NULL,
+  NULL,
+  { NULL, NULL },
+  0,
+  0,
+  FALSE,
+  FALSE
+};
+
+
+//
+// FFS helper functions
+//
+/**
+  Read data from Firmware Block by FVB protocol Read. 
+  The data may cross the multi block ranges.
+
+  @param  Fvb                   The FW_VOL_BLOCK_PROTOCOL instance from which to read data.
+  @param  StartLba              Pointer to StartLba.
+                                On input, the start logical block index from which to read.
+                                On output,the end logical block index after reading.
+  @param  Offset                Pointer to Offset
+                                On input, offset into the block at which to begin reading.
+                                On output, offset into the end block after reading.
+  @param  DataSize              Size of data to be read.
+  @param  Data                  Pointer to Buffer that the data will be read into.
+
+  @retval EFI_SUCCESS           Successfully read data from firmware block.
+  @retval others
+**/
+EFI_STATUS
+ReadFvbData (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *Fvb,
+  IN OUT EFI_LBA                                *StartLba,
+  IN OUT UINTN                                  *Offset,
+  IN     UINTN                                  DataSize,
+  OUT    UINT8                                  *Data
+  )
+{
+  UINTN                       BlockSize;
+  UINTN                       NumberOfBlocks;
+  UINTN                       BlockIndex;
+  UINTN                       ReadDataSize;
+  EFI_STATUS                  Status;
+  
+  //
+  // Try read data in current block
+  //
+  BlockIndex   = 0;
+  ReadDataSize = DataSize;
+  Status = Fvb->Read (Fvb, *StartLba, *Offset, &ReadDataSize, Data);
+  if (Status == EFI_SUCCESS) {
+    *Offset  += DataSize;
+    return EFI_SUCCESS;
+  } else if (Status != EFI_BAD_BUFFER_SIZE) {
+    //
+    // other error will direct return
+    //
+    return Status;
+  }
+  
+  //
+  // Data crosses the blocks, read data from next block
+  //
+  DataSize -= ReadDataSize;
+  Data     += ReadDataSize;
+  *StartLba = *StartLba + 1;
+  while (DataSize > 0) {
+    Status = Fvb->GetBlockSize (Fvb, *StartLba, &BlockSize, &NumberOfBlocks);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Read data from the crossing blocks
+    //
+    BlockIndex = 0; 
+    while (BlockIndex < NumberOfBlocks && DataSize >= BlockSize) {
+      Status = Fvb->Read (Fvb, *StartLba + BlockIndex, 0, &BlockSize, Data);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Data += BlockSize;
+      DataSize -= BlockSize;
+      BlockIndex ++;
+    }
+    
+    //
+    // Data doesn't exceed the current block range.
+    //
+    if (DataSize < BlockSize) {
+      break;
+    }
+    
+    //
+    // Data must be got from the next block range.
+    //
+    *StartLba += NumberOfBlocks;
+  }
+  
+  //
+  // read the remaining data
+  //
+  if (DataSize > 0) {
+    Status = Fvb->Read (Fvb, *StartLba + BlockIndex, 0, &DataSize, Data);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  
+  //
+  // Update Lba and Offset used by the following read.
+  //
+  *StartLba += BlockIndex;
+  *Offset   = DataSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Given the supplied FW_VOL_BLOCK_PROTOCOL, allocate a buffer for output and
+  copy the real length volume header into it.
+
+  @param  Fvb                   The FW_VOL_BLOCK_PROTOCOL instance from which to
+                                read the volume header
+  @param  FwVolHeader           Pointer to pointer to allocated buffer in which
+                                the volume header is returned.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough buffer could be allocated.
+  @retval EFI_SUCCESS           Successfully read volume header to the allocated
+                                buffer.
+  @retval EFI_INVALID_PARAMETER The FV Header signature is not as expected or
+                                the file system could not be understood.
+
+**/
+EFI_STATUS
+GetFwVolHeader (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *Fvb,
+  OUT    EFI_FIRMWARE_VOLUME_HEADER             **FwVolHeader
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_FIRMWARE_VOLUME_HEADER  TempFvh;
+  UINTN                       FvhLength;
+  EFI_LBA                     StartLba;
+  UINTN                       Offset;
+  UINT8                       *Buffer;
+  
+  //
+  // Read the standard FV header
+  //
+  StartLba = 0;
+  Offset   = 0;
+  FvhLength = sizeof (EFI_FIRMWARE_VOLUME_HEADER);
+  Status = ReadFvbData (Fvb, &StartLba, &Offset, FvhLength, (UINT8 *)&TempFvh);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Validate FV Header signature, if not as expected, continue.
+  //
+  if (TempFvh.Signature != EFI_FVH_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check to see that the file system is indeed formatted in a way we can
+  // understand it...
+  //
+  if ((!CompareGuid (&TempFvh.FileSystemGuid, &gEfiFirmwareFileSystem2Guid)) &&
+      (!CompareGuid (&TempFvh.FileSystemGuid, &gEfiFirmwareFileSystem3Guid))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Allocate a buffer for the caller
+  //
+  *FwVolHeader = AllocatePool (TempFvh.HeaderLength);
+  if (*FwVolHeader == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Copy the standard header into the buffer
+  //
+  CopyMem (*FwVolHeader, &TempFvh, sizeof (EFI_FIRMWARE_VOLUME_HEADER));
+
+  //
+  // Read the rest of the header
+  //
+  FvhLength = TempFvh.HeaderLength - sizeof (EFI_FIRMWARE_VOLUME_HEADER);
+  Buffer = (UINT8 *)*FwVolHeader + sizeof (EFI_FIRMWARE_VOLUME_HEADER);
+  Status = ReadFvbData (Fvb, &StartLba, &Offset, FvhLength, Buffer);
+  if (EFI_ERROR (Status)) {
+    //
+    // Read failed so free buffer
+    //
+    CoreFreePool (*FwVolHeader);
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Free FvDevice resource when error happens
+
+  @param  FvDevice              pointer to the FvDevice to be freed.
+
+**/
+VOID
+FreeFvDeviceResource (
+  IN FV_DEVICE  *FvDevice
+  )
+{
+  FFS_FILE_LIST_ENTRY         *FfsFileEntry;
+  LIST_ENTRY                  *NextEntry;
+
+  //
+  // Free File List Entry
+  //
+  FfsFileEntry = (FFS_FILE_LIST_ENTRY *)FvDevice->FfsFileListHeader.ForwardLink;
+  while (&FfsFileEntry->Link != &FvDevice->FfsFileListHeader) {
+    NextEntry = (&FfsFileEntry->Link)->ForwardLink;
+
+    if (FfsFileEntry->StreamHandle != 0) {
+      //
+      // Close stream and free resources from SEP
+      //
+      CloseSectionStream (FfsFileEntry->StreamHandle, FALSE);
+    }
+
+    if (FfsFileEntry->FileCached) {
+      //
+      // Free the cached file buffer.
+      //
+      CoreFreePool (FfsFileEntry->FfsHeader);
+    }
+
+    CoreFreePool (FfsFileEntry);
+
+    FfsFileEntry = (FFS_FILE_LIST_ENTRY *) NextEntry;
+  }
+
+  if (!FvDevice->IsMemoryMapped) {
+    //
+    // Free the cached FV buffer.
+    //
+    CoreFreePool (FvDevice->CachedFv);
+  }
+
+  //
+  // Free Volume Header
+  //
+  CoreFreePool (FvDevice->FwVolHeader);
+
+  return;
+}
+
+
+
+/**
+  Check if an FV is consistent and allocate cache for it.
+
+  @param  FvDevice              A pointer to the FvDevice to be checked.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough buffer could be allocated.
+  @retval EFI_SUCCESS           FV is consistent and cache is allocated.
+  @retval EFI_VOLUME_CORRUPTED  File system is corrupted.
+
+**/
+EFI_STATUS
+FvCheck (
+  IN OUT FV_DEVICE  *FvDevice
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *Fvb;
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+  EFI_FIRMWARE_VOLUME_EXT_HEADER        *FwVolExtHeader;
+  EFI_FVB_ATTRIBUTES_2                  FvbAttributes;
+  EFI_FV_BLOCK_MAP_ENTRY                *BlockMap;
+  FFS_FILE_LIST_ENTRY                   *FfsFileEntry;
+  EFI_FFS_FILE_HEADER                   *FfsHeader;
+  UINT8                                 *CacheLocation;
+  UINTN                                 LbaOffset;
+  UINTN                                 HeaderSize;
+  UINTN                                 Index;
+  EFI_LBA                               LbaIndex;
+  UINTN                                 Size;
+  EFI_FFS_FILE_STATE                    FileState;
+  UINT8                                 *TopFvAddress;
+  UINTN                                 TestLength;
+  EFI_PHYSICAL_ADDRESS                  PhysicalAddress;
+  BOOLEAN                               FileCached;
+  UINTN                                 WholeFileSize;
+  EFI_FFS_FILE_HEADER                   *CacheFfsHeader;
+
+  FileCached = FALSE;
+  CacheFfsHeader = NULL;
+
+  Fvb = FvDevice->Fvb;
+  FwVolHeader = FvDevice->FwVolHeader;
+
+  Status = Fvb->GetAttributes (Fvb, &FvbAttributes);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Size is the size of the FV minus the head. We have already allocated
+  // the header to check to make sure the volume is valid
+  //
+  Size = (UINTN)(FwVolHeader->FvLength - FwVolHeader->HeaderLength);
+  if ((FvbAttributes & EFI_FVB2_MEMORY_MAPPED) != 0) {
+    FvDevice->IsMemoryMapped = TRUE;
+
+    Status = Fvb->GetPhysicalAddress (Fvb, &PhysicalAddress);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Don't cache memory mapped FV really.
+    //
+    FvDevice->CachedFv = (UINT8 *) (UINTN) (PhysicalAddress + FwVolHeader->HeaderLength);
+  } else {
+    FvDevice->IsMemoryMapped = FALSE;
+    FvDevice->CachedFv = AllocatePool (Size);
+
+    if (FvDevice->CachedFv == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  //
+  // Remember a pointer to the end fo the CachedFv
+  //
+  FvDevice->EndOfCachedFv = FvDevice->CachedFv + Size;
+
+  if (!FvDevice->IsMemoryMapped) {
+    //
+    // Copy FV minus header into memory using the block map we have all ready
+    // read into memory.
+    //
+    BlockMap = FwVolHeader->BlockMap;
+    CacheLocation = FvDevice->CachedFv;
+    LbaIndex = 0;
+    LbaOffset = 0;
+    HeaderSize = FwVolHeader->HeaderLength;
+    while ((BlockMap->NumBlocks != 0) || (BlockMap->Length != 0)) {
+      Index = 0;
+      Size  = BlockMap->Length;
+      if (HeaderSize > 0) {
+        //
+        // Skip header size
+        //
+        for (; Index < BlockMap->NumBlocks && HeaderSize >= BlockMap->Length; Index ++) {
+          HeaderSize -= BlockMap->Length;
+          LbaIndex ++;
+        }
+
+        //
+        // Check whether FvHeader is crossing the multi block range.
+        //
+        if (Index >= BlockMap->NumBlocks) {
+          BlockMap++;
+          continue;
+        } else if (HeaderSize > 0) {
+          LbaOffset = HeaderSize;
+          Size = BlockMap->Length - HeaderSize;
+          HeaderSize = 0;
+        }
+      }
+    
+      //
+      // read the FV data  
+      //
+      for (; Index < BlockMap->NumBlocks; Index ++) {
+        Status = Fvb->Read (Fvb,
+                        LbaIndex,
+                        LbaOffset,
+                        &Size,
+                        CacheLocation
+                        );
+
+        //
+        // Not check EFI_BAD_BUFFER_SIZE, for Size = BlockMap->Length
+        //
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+
+        LbaIndex++;
+        CacheLocation += Size;
+
+        //
+        // After we skip Fv Header always read from start of block
+        //
+        LbaOffset = 0;
+        Size  = BlockMap->Length;
+      }
+
+      BlockMap++;
+    }
+  }
+
+  //
+  // Scan to check the free space & File list
+  //
+  if ((FvbAttributes & EFI_FVB2_ERASE_POLARITY) != 0) {
+    FvDevice->ErasePolarity = 1;
+  } else {
+    FvDevice->ErasePolarity = 0;
+  }
+
+
+  //
+  // go through the whole FV cache, check the consistence of the FV.
+  // Make a linked list of all the Ffs file headers
+  //
+  Status = EFI_SUCCESS;
+  InitializeListHead (&FvDevice->FfsFileListHeader);
+
+  //
+  // Build FFS list
+  //
+  if (FwVolHeader->ExtHeaderOffset != 0) {
+    //
+    // Searching for files starts on an 8 byte aligned boundary after the end of the Extended Header if it exists.
+    //
+    FwVolExtHeader = (EFI_FIRMWARE_VOLUME_EXT_HEADER *) (FvDevice->CachedFv + (FwVolHeader->ExtHeaderOffset - FwVolHeader->HeaderLength));
+    FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FwVolExtHeader + FwVolExtHeader->ExtHeaderSize);
+    FfsHeader = (EFI_FFS_FILE_HEADER *) ALIGN_POINTER (FfsHeader, 8);
+  } else {
+    FfsHeader = (EFI_FFS_FILE_HEADER *) (FvDevice->CachedFv);
+  }
+  TopFvAddress = FvDevice->EndOfCachedFv;
+  while (((UINTN) FfsHeader >= (UINTN) FvDevice->CachedFv) && ((UINTN) FfsHeader <= (UINTN) ((UINTN) TopFvAddress - sizeof (EFI_FFS_FILE_HEADER)))) {
+
+    if (FileCached) {
+      CoreFreePool (CacheFfsHeader);
+      FileCached = FALSE;
+    }
+
+    TestLength = TopFvAddress - ((UINT8 *) FfsHeader);
+    if (TestLength > sizeof (EFI_FFS_FILE_HEADER)) {
+      TestLength = sizeof (EFI_FFS_FILE_HEADER);
+    }
+
+    if (IsBufferErased (FvDevice->ErasePolarity, FfsHeader, TestLength)) {
+      //
+      // We have found the free space so we are done!
+      //
+      goto Done;
+    }
+
+    if (!IsValidFfsHeader (FvDevice->ErasePolarity, FfsHeader, &FileState)) {
+      if ((FileState == EFI_FILE_HEADER_INVALID) ||
+          (FileState == EFI_FILE_HEADER_CONSTRUCTION)) {
+        if (IS_FFS_FILE2 (FfsHeader)) {
+          if (!FvDevice->IsFfs3Fv) {
+            DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FfsHeader->Name));
+          }
+          FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsHeader + sizeof (EFI_FFS_FILE_HEADER2));
+        } else {
+          FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsHeader + sizeof (EFI_FFS_FILE_HEADER));
+        }
+        continue;
+      } else {
+        //
+        // File system is corrputed
+        //
+        Status = EFI_VOLUME_CORRUPTED;
+        goto Done;
+      }
+    }
+
+    CacheFfsHeader = FfsHeader;
+    if ((CacheFfsHeader->Attributes & FFS_ATTRIB_CHECKSUM) == FFS_ATTRIB_CHECKSUM) {
+      if (FvDevice->IsMemoryMapped) {
+        //
+        // Memory mapped FV has not been cached.
+        // Here is to cache FFS file to memory buffer for following checksum calculating.
+        // And then, the cached file buffer can be also used for FvReadFile.
+        //
+        WholeFileSize = IS_FFS_FILE2 (CacheFfsHeader) ? FFS_FILE2_SIZE (CacheFfsHeader): FFS_FILE_SIZE (CacheFfsHeader);
+        CacheFfsHeader = AllocateCopyPool (WholeFileSize, CacheFfsHeader);
+        if (CacheFfsHeader == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto Done;
+        }
+        FileCached = TRUE;
+      }
+    }
+
+    if (!IsValidFfsFile (FvDevice->ErasePolarity, CacheFfsHeader)) {
+      //
+      // File system is corrupted
+      //
+      Status = EFI_VOLUME_CORRUPTED;
+      goto Done;
+    }
+
+    if (IS_FFS_FILE2 (CacheFfsHeader)) {
+      ASSERT (FFS_FILE2_SIZE (CacheFfsHeader) > 0x00FFFFFF);
+      if (!FvDevice->IsFfs3Fv) {
+        DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &CacheFfsHeader->Name));
+        FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsHeader + FFS_FILE2_SIZE (CacheFfsHeader));
+        //
+        // Adjust pointer to the next 8-byte aligned boundry.
+        //
+        FfsHeader = (EFI_FFS_FILE_HEADER *) (((UINTN) FfsHeader + 7) & ~0x07);
+        continue;
+      }
+    }
+
+    FileState = GetFileState (FvDevice->ErasePolarity, CacheFfsHeader);
+
+    //
+    // check for non-deleted file
+    //
+    if (FileState != EFI_FILE_DELETED) {
+      //
+      // Create a FFS list entry for each non-deleted file
+      //
+      FfsFileEntry = AllocateZeroPool (sizeof (FFS_FILE_LIST_ENTRY));
+      if (FfsFileEntry == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+
+      FfsFileEntry->FfsHeader = CacheFfsHeader;
+      FfsFileEntry->FileCached = FileCached;
+      FileCached = FALSE;
+      InsertTailList (&FvDevice->FfsFileListHeader, &FfsFileEntry->Link);
+    }
+
+    if (IS_FFS_FILE2 (CacheFfsHeader)) {
+      FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsHeader + FFS_FILE2_SIZE (CacheFfsHeader));
+    } else {
+      FfsHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsHeader + FFS_FILE_SIZE (CacheFfsHeader));
+    }
+
+    //
+    // Adjust pointer to the next 8-byte aligned boundry.
+    //
+    FfsHeader = (EFI_FFS_FILE_HEADER *)(((UINTN)FfsHeader + 7) & ~0x07);
+
+  }
+
+Done:
+  if (EFI_ERROR (Status)) {
+    if (FileCached) {
+      CoreFreePool (CacheFfsHeader);
+      FileCached = FALSE;
+    }
+    FreeFvDeviceResource (FvDevice);
+  }
+
+  return Status;
+}
+
+
+
+/**
+  This notification function is invoked when an instance of the
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL is produced.  It layers an instance of the
+  EFI_FIRMWARE_VOLUME2_PROTOCOL on the same handle.  This is the function where
+  the actual initialization of the EFI_FIRMWARE_VOLUME2_PROTOCOL is done.
+
+  @param  Event                 The event that occured
+  @param  Context               For EFI compatiblity.  Not used.
+
+**/
+VOID
+EFIAPI
+NotifyFwVolBlock (
+  IN  EFI_EVENT Event,
+  IN  VOID      *Context
+  )
+{
+  EFI_HANDLE                            Handle;
+  EFI_STATUS                            Status;
+  UINTN                                 BufferSize;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *Fvb;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL         *Fv;
+  FV_DEVICE                             *FvDevice;
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+  //
+  // Examine all new handles
+  //
+  for (;;) {
+    //
+    // Get the next handle
+    //
+    BufferSize = sizeof (Handle);
+    Status = CoreLocateHandle (
+              ByRegisterNotify,
+              NULL,
+              gEfiFwVolBlockNotifyReg,
+              &BufferSize,
+              &Handle
+              );
+
+    //
+    // If not found, we're done
+    //
+    if (EFI_NOT_FOUND == Status) {
+      break;
+    }
+
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Get the FirmwareVolumeBlock protocol on that handle
+    //
+    Status = CoreHandleProtocol (Handle, &gEfiFirmwareVolumeBlockProtocolGuid, (VOID **)&Fvb);
+    ASSERT_EFI_ERROR (Status);
+    ASSERT (Fvb != NULL);
+
+    //
+    // Make sure the Fv Header is O.K.
+    //
+    Status = GetFwVolHeader (Fvb, &FwVolHeader);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    ASSERT (FwVolHeader != NULL);
+
+    if (!VerifyFvHeaderChecksum (FwVolHeader)) {
+      CoreFreePool (FwVolHeader);
+      continue;
+    }
+
+    //
+    // Check if there is an FV protocol already installed in that handle
+    //
+    Status = CoreHandleProtocol (Handle, &gEfiFirmwareVolume2ProtocolGuid, (VOID **)&Fv);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update Fv to use a new Fvb
+      //
+      FvDevice = BASE_CR (Fv, FV_DEVICE, Fv);
+      if (FvDevice->Signature == FV2_DEVICE_SIGNATURE) {
+        //
+        // Only write into our device structure if it's our device structure
+        //
+        FvDevice->Fvb = Fvb;
+      }
+
+    } else {
+      //
+      // No FwVol protocol on the handle so create a new one
+      //
+      FvDevice = AllocateCopyPool (sizeof (FV_DEVICE), &mFvDevice);
+      if (FvDevice == NULL) {
+        return;
+      }
+
+      FvDevice->Fvb             = Fvb;
+      FvDevice->Handle          = Handle;
+      FvDevice->FwVolHeader     = FwVolHeader;
+      FvDevice->IsFfs3Fv        = CompareGuid (&FwVolHeader->FileSystemGuid, &gEfiFirmwareFileSystem3Guid);
+      FvDevice->Fv.ParentHandle = Fvb->ParentHandle;
+
+      if (Fvb->ParentHandle != NULL) {
+        //
+        // Inherit the authentication status from FVB.
+        //
+        FvDevice->AuthenticationStatus = GetFvbAuthenticationStatus (Fvb);
+      }
+      
+      if (!EFI_ERROR (FvCheck (FvDevice))) {
+        //
+        // Install an New FV protocol on the existing handle
+        //
+        Status = CoreInstallProtocolInterface (
+                    &Handle,
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &FvDevice->Fv
+                    );
+        ASSERT_EFI_ERROR (Status);
+      } else {
+        //
+        // Free FvDevice Buffer for the corrupt FV image.
+        //
+        CoreFreePool (FvDevice);
+      }
+    }
+  }
+
+  return;
+}
+
+
+
+/**
+  This routine is the driver initialization entry point.  It registers
+  a notification function.  This notification function are responsible
+  for building the FV stack dynamically.
+
+  @param  ImageHandle           The image handle.
+  @param  SystemTable           The system table.
+
+  @retval EFI_SUCCESS           Function successfully returned.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolDriverInit (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  gEfiFwVolBlockEvent = EfiCreateProtocolNotifyEvent (
+                          &gEfiFirmwareVolumeBlockProtocolGuid,
+                          TPL_CALLBACK,
+                          NotifyFwVolBlock,
+                          NULL,
+                          &gEfiFwVolBlockNotifyReg
+                          );
+  return EFI_SUCCESS;
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolAttrib.c b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolAttrib.c
new file mode 100644
index 0000000000..0a89e70761
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolAttrib.c
@@ -0,0 +1,135 @@
+/** @file
+  Implements get/set firmware volume attributes
+
+Copyright (c) 2006 - 2012, 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 "DxeMain.h"
+#include "FwVolDriver.h"
+
+
+/**
+  Retrieves attributes, insures positive polarity of attribute bits, returns
+  resulting attributes in output parameter.
+
+  @param  This             Calling context
+  @param  Attributes       output buffer which contains attributes
+
+  @retval EFI_SUCCESS      Successfully got volume attributes
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetVolumeAttributes (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  OUT       EFI_FV_ATTRIBUTES             *Attributes
+  )
+{
+  EFI_STATUS                                Status;
+  FV_DEVICE                                 *FvDevice;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL        *Fvb;
+  EFI_FVB_ATTRIBUTES_2                      FvbAttributes;
+
+  FvDevice = FV_DEVICE_FROM_THIS (This);
+  Fvb = FvDevice->Fvb;
+
+  //
+  // First get the Firmware Volume Block Attributes
+  //
+  Status = Fvb->GetAttributes (Fvb, &FvbAttributes);
+
+  //
+  // Mask out Fvb bits that are not defined in FV
+  //
+  FvbAttributes &= 0xfffff0ff;
+
+  *Attributes = (EFI_FV_ATTRIBUTES)FvbAttributes;
+
+  return Status;
+}
+
+
+
+/**
+  Sets current attributes for volume
+
+  @param  This             Calling context
+  @param  Attributes       At input, contains attributes to be set.  At output
+                           contains new value of FV
+
+  @retval EFI_UNSUPPORTED  Could not be set.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSetVolumeAttributes (
+  IN     CONST EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN OUT       EFI_FV_ATTRIBUTES              *Attributes
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Return information of type InformationType for the requested firmware
+  volume.
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  InformationType  InformationType for requested.
+  @param  BufferSize       On input, size of Buffer.On output, the amount of data
+                           returned in Buffer.
+  @param  Buffer           A poniter to the data buffer to return.
+
+  @retval EFI_SUCCESS      Successfully got volume Information.
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetVolumeInfo (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL       *This,
+  IN  CONST EFI_GUID                            *InformationType,
+  IN OUT UINTN                                  *BufferSize,
+  OUT VOID                                      *Buffer
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+
+/**
+  Set information of type InformationType for the requested firmware
+  volume.
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  InformationType  InformationType for requested.
+  @param  BufferSize       On input, size of Buffer.On output, the amount of data
+                           returned in Buffer.
+  @param  Buffer           A poniter to the data buffer to return.
+
+  @retval EFI_SUCCESS      Successfully set volume Information.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSetVolumeInfo (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL       *This,
+  IN  CONST EFI_GUID                            *InformationType,
+  IN  UINTN                                     BufferSize,
+  IN CONST  VOID                                *Buffer
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolDriver.h b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolDriver.h
new file mode 100644
index 0000000000..96cbde37d5
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolDriver.h
@@ -0,0 +1,408 @@
+/** @file
+  Firmware File System protocol. Layers on top of Firmware
+  Block protocol to produce a file abstraction of FV based files.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef __FW_VOL_DRIVER_H_
+#define __FW_VOL_DRIVER_H_
+
+
+#define FV2_DEVICE_SIGNATURE SIGNATURE_32 ('_', 'F', 'V', '2')
+
+//
+// Used to track all non-deleted files
+//
+typedef struct {
+  LIST_ENTRY                      Link;
+  EFI_FFS_FILE_HEADER             *FfsHeader;
+  UINTN                           StreamHandle;
+  BOOLEAN                         FileCached;
+} FFS_FILE_LIST_ENTRY;
+
+typedef struct {
+  UINTN                                   Signature;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL      *Fvb;
+  EFI_HANDLE                              Handle;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL           Fv;
+
+  EFI_FIRMWARE_VOLUME_HEADER              *FwVolHeader;
+  UINT8                                   *CachedFv;
+  UINT8                                   *EndOfCachedFv;
+
+  FFS_FILE_LIST_ENTRY                     *LastKey;
+
+  LIST_ENTRY                              FfsFileListHeader;
+
+  UINT32                                  AuthenticationStatus;
+  UINT8                                   ErasePolarity;
+  BOOLEAN                                 IsFfs3Fv;
+  BOOLEAN                                 IsMemoryMapped;
+} FV_DEVICE;
+
+#define FV_DEVICE_FROM_THIS(a) CR(a, FV_DEVICE, Fv, FV2_DEVICE_SIGNATURE)
+
+/**
+  Retrieves attributes, insures positive polarity of attribute bits, returns
+  resulting attributes in output parameter.
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  Attributes       output buffer which contains attributes.
+
+  @retval EFI_SUCCESS      Successfully got volume attributes.
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetVolumeAttributes (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  OUT       EFI_FV_ATTRIBUTES             *Attributes
+  );
+
+
+/**
+  Sets current attributes for volume
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  Attributes       At input, contains attributes to be set.  At output
+                           contains new value of FV.
+
+  @retval EFI_UNSUPPORTED  Could not be set.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSetVolumeAttributes (
+  IN     CONST EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN OUT       EFI_FV_ATTRIBUTES              *Attributes
+  );
+
+
+/**
+  Given the input key, search for the next matching file in the volume.
+
+  @param  This                       Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  Key                        Key is a pointer to a caller allocated
+                                     buffer that contains implementation specific
+                                     data that is used to track where to begin
+                                     the search for the next file. The size of
+                                     the buffer must be at least This->KeySize
+                                     bytes long. To reinitialize the search and
+                                     begin from the beginning of the firmware
+                                     volume, the entire buffer must be cleared to
+                                     zero. Other than clearing the buffer to
+                                     initiate a new search, the caller must not
+                                     modify the data in the buffer between calls
+                                     to GetNextFile().
+  @param  FileType                   FileType is a pointer to a caller allocated
+                                     EFI_FV_FILETYPE. The GetNextFile() API can
+                                     filter it's search for files based on the
+                                     value of *FileType input. A *FileType input
+                                     of 0 causes GetNextFile() to search for
+                                     files of all types.  If a file is found, the
+                                     file's type is returned in *FileType.
+                                     *FileType is not modified if no file is
+                                     found.
+  @param  NameGuid                   NameGuid is a pointer to a caller allocated
+                                     EFI_GUID. If a file is found, the file's
+                                     name is returned in *NameGuid.  *NameGuid is
+                                     not modified if no file is found.
+  @param  Attributes                 Attributes is a pointer to a caller
+                                     allocated EFI_FV_FILE_ATTRIBUTES.  If a file
+                                     is found, the file's attributes are returned
+                                     in *Attributes. *Attributes is not modified
+                                     if no file is found.
+  @param  Size                       Size is a pointer to a caller allocated
+                                     UINTN. If a file is found, the file's size
+                                     is returned in *Size. *Size is not modified
+                                     if no file is found.
+
+  @retval EFI_SUCCESS                Successfully find the file.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Fv could not read.
+  @retval EFI_NOT_FOUND              No matching file found.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetNextFile (
+  IN CONST   EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  IN OUT     VOID                          *Key,
+  IN OUT     EFI_FV_FILETYPE               *FileType,
+  OUT        EFI_GUID                      *NameGuid,
+  OUT        EFI_FV_FILE_ATTRIBUTES        *Attributes,
+  OUT        UINTN                         *Size
+  );
+
+
+
+/**
+  Locates a file in the firmware volume and
+  copies it to the supplied buffer.
+
+  @param  This                       Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  NameGuid                   Pointer to an EFI_GUID, which is the
+                                     filename.
+  @param  Buffer                     Buffer is a pointer to pointer to a buffer
+                                     in which the file or section contents or are
+                                     returned.
+  @param  BufferSize                 BufferSize is a pointer to caller allocated
+                                     UINTN. On input *BufferSize indicates the
+                                     size in bytes of the memory region pointed
+                                     to by Buffer. On output, *BufferSize
+                                     contains the number of bytes required to
+                                     read the file.
+  @param  FoundType                  FoundType is a pointer to a caller allocated
+                                     EFI_FV_FILETYPE that on successful return
+                                     from Read() contains the type of file read.
+                                     This output reflects the file type
+                                     irrespective of the value of the SectionType
+                                     input.
+  @param  FileAttributes             FileAttributes is a pointer to a caller
+                                     allocated EFI_FV_FILE_ATTRIBUTES.  On
+                                     successful return from Read(),
+                                     *FileAttributes contains the attributes of
+                                     the file read.
+  @param  AuthenticationStatus       AuthenticationStatus is a pointer to a
+                                     caller allocated UINTN in which the
+                                     authentication status is returned.
+
+  @retval EFI_SUCCESS                Successfully read to memory buffer.
+  @retval EFI_WARN_BUFFER_TOO_SMALL  Buffer too small.
+  @retval EFI_NOT_FOUND              Not found.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Could not read.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+  @retval EFI_OUT_OF_RESOURCES       Not enough buffer to be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+FvReadFile (
+  IN CONST EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  IN CONST EFI_GUID                      *NameGuid,
+  IN OUT   VOID                          **Buffer,
+  IN OUT   UINTN                         *BufferSize,
+  OUT      EFI_FV_FILETYPE               *FoundType,
+  OUT      EFI_FV_FILE_ATTRIBUTES        *FileAttributes,
+  OUT      UINT32                        *AuthenticationStatus
+  );
+
+
+/**
+  Locates a section in a given FFS File and
+  copies it to the supplied buffer (not including section header).
+
+  @param  This                       Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  NameGuid                   Pointer to an EFI_GUID, which is the
+                                     filename.
+  @param  SectionType                Indicates the section type to return.
+  @param  SectionInstance            Indicates which instance of sections with a
+                                     type of SectionType to return.
+  @param  Buffer                     Buffer is a pointer to pointer to a buffer
+                                     in which the file or section contents or are
+                                     returned.
+  @param  BufferSize                 BufferSize is a pointer to caller allocated
+                                     UINTN.
+  @param  AuthenticationStatus       AuthenticationStatus is a pointer to a
+                                     caller allocated UINT32 in which the
+                                     authentication status is returned.
+
+  @retval EFI_SUCCESS                Successfully read the file section into
+                                     buffer.
+  @retval EFI_WARN_BUFFER_TOO_SMALL  Buffer too small.
+  @retval EFI_NOT_FOUND              Section not found.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Could not read.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+
+**/
+EFI_STATUS
+EFIAPI
+FvReadFileSection (
+  IN CONST  EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN CONST  EFI_GUID                       *NameGuid,
+  IN        EFI_SECTION_TYPE               SectionType,
+  IN        UINTN                          SectionInstance,
+  IN OUT    VOID                           **Buffer,
+  IN OUT    UINTN                          *BufferSize,
+  OUT       UINT32                         *AuthenticationStatus
+  );
+
+
+/**
+  Writes one or more files to the firmware volume.
+
+  @param  This                   Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  NumberOfFiles          Number of files.
+  @param  WritePolicy            WritePolicy indicates the level of reliability
+                                 for the write in the event of a power failure or
+                                 other system failure during the write operation.
+  @param  FileData               FileData is an pointer to an array of
+                                 EFI_FV_WRITE_DATA. Each element of array
+                                 FileData represents a file to be written.
+
+  @retval EFI_SUCCESS            Files successfully written to firmware volume
+  @retval EFI_OUT_OF_RESOURCES   Not enough buffer to be allocated.
+  @retval EFI_DEVICE_ERROR       Device error.
+  @retval EFI_WRITE_PROTECTED    Write protected.
+  @retval EFI_NOT_FOUND          Not found.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_UNSUPPORTED        This function not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FvWriteFile (
+  IN CONST EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN       UINT32                         NumberOfFiles,
+  IN       EFI_FV_WRITE_POLICY            WritePolicy,
+  IN       EFI_FV_WRITE_FILE_DATA         *FileData
+  );
+
+
+/**
+  Return information of type InformationType for the requested firmware
+  volume.
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  InformationType  InformationType for requested.
+  @param  BufferSize       On input, size of Buffer.On output, the amount of data
+                           returned in Buffer.
+  @param  Buffer           A poniter to the data buffer to return.
+
+  @retval EFI_SUCCESS      Successfully got volume Information.
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetVolumeInfo (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL       *This,
+  IN  CONST EFI_GUID                            *InformationType,
+  IN OUT UINTN                                  *BufferSize,
+  OUT VOID                                      *Buffer
+  );
+
+
+
+/**
+  Set information of type InformationType for the requested firmware
+  volume.
+
+  @param  This             Pointer to EFI_FIRMWARE_VOLUME2_PROTOCOL.
+  @param  InformationType  InformationType for requested.
+  @param  BufferSize       On input, size of Buffer.On output, the amount of data
+                           returned in Buffer.
+  @param  Buffer           A poniter to the data buffer to return.
+
+  @retval EFI_SUCCESS      Successfully set volume Information.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSetVolumeInfo (
+  IN  CONST EFI_FIRMWARE_VOLUME2_PROTOCOL       *This,
+  IN  CONST EFI_GUID                            *InformationType,
+  IN  UINTN                                     BufferSize,
+  IN CONST  VOID                                *Buffer
+  );
+
+
+
+/**
+  Check if a block of buffer is erased.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  InBuffer       The buffer to be checked
+  @param  BufferSize     Size of the buffer in bytes
+
+  @retval TRUE           The block of buffer is erased
+  @retval FALSE          The block of buffer is not erased
+
+**/
+BOOLEAN
+IsBufferErased (
+  IN UINT8    ErasePolarity,
+  IN VOID     *InBuffer,
+  IN UINTN    BufferSize
+  );
+
+
+/**
+  Get the FFS file state by checking the highest bit set in the header's state field.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file header
+
+  @return FFS File state
+
+**/
+EFI_FFS_FILE_STATE
+GetFileState (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  );
+
+
+/**
+  Set the FFS file state.
+
+  @param  State                      The state to be set.
+  @param  FfsHeader                  Points to the FFS file header
+
+  @return None.
+
+**/
+VOID
+SetFileState (
+  IN UINT8                State,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  );
+
+/**
+  Check if it's a valid FFS file header.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file header to be checked
+  @param  FileState      FFS file state to be returned
+
+  @retval TRUE           Valid FFS file header
+  @retval FALSE          Invalid FFS file header
+
+**/
+BOOLEAN
+IsValidFfsHeader (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader,
+  OUT EFI_FFS_FILE_STATE  *FileState
+  );
+
+
+/**
+  Check if it's a valid FFS file.
+  Here we are sure that it has a valid FFS file header since we must call IsValidFfsHeader() first.
+
+  @param  ErasePolarity  Erase polarity attribute of the firmware volume
+  @param  FfsHeader      Points to the FFS file to be checked
+
+  @retval TRUE           Valid FFS file
+  @retval FALSE          Invalid FFS file
+
+**/
+BOOLEAN
+IsValidFfsFile (
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolRead.c b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolRead.c
new file mode 100644
index 0000000000..1acac5d8fd
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolRead.c
@@ -0,0 +1,526 @@
+/** @file
+  Implements functions to read firmware file
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "FwVolDriver.h"
+
+/**
+Required Alignment             Alignment Value in FFS         Alignment Value in
+(bytes)                        Attributes Field               Firmware Volume Interfaces
+1                                    0                                     0
+16                                   1                                     4
+128                                  2                                     7
+512                                  3                                     9
+1 KB                                 4                                     10
+4 KB                                 5                                     12
+32 KB                                6                                     15
+64 KB                                7                                     16
+**/
+UINT8 mFvAttributes[] = {0, 4, 7, 9, 10, 12, 15, 16};
+
+/**
+  Convert the FFS File Attributes to FV File Attributes
+
+  @param  FfsAttributes              The attributes of UINT8 type.
+
+  @return The attributes of EFI_FV_FILE_ATTRIBUTES
+
+**/
+EFI_FV_FILE_ATTRIBUTES
+FfsAttributes2FvFileAttributes (
+  IN EFI_FFS_FILE_ATTRIBUTES FfsAttributes
+  )
+{
+  UINT8                     DataAlignment;
+  EFI_FV_FILE_ATTRIBUTES    FileAttribute;
+
+  DataAlignment = (UINT8) ((FfsAttributes & FFS_ATTRIB_DATA_ALIGNMENT) >> 3);
+  ASSERT (DataAlignment < 8);
+
+  FileAttribute = (EFI_FV_FILE_ATTRIBUTES) mFvAttributes[DataAlignment];
+
+  if ((FfsAttributes & FFS_ATTRIB_FIXED) == FFS_ATTRIB_FIXED) {
+    FileAttribute |= EFI_FV_FILE_ATTRIB_FIXED;
+  }
+
+  return FileAttribute;
+}
+
+/**
+  Given the input key, search for the next matching file in the volume.
+
+  @param  This                       Indicates the calling context.
+  @param  Key                        Key is a pointer to a caller allocated
+                                     buffer that contains implementation specific
+                                     data that is used to track where to begin
+                                     the search for the next file. The size of
+                                     the buffer must be at least This->KeySize
+                                     bytes long. To reinitialize the search and
+                                     begin from the beginning of the firmware
+                                     volume, the entire buffer must be cleared to
+                                     zero. Other than clearing the buffer to
+                                     initiate a new search, the caller must not
+                                     modify the data in the buffer between calls
+                                     to GetNextFile().
+  @param  FileType                   FileType is a pointer to a caller allocated
+                                     EFI_FV_FILETYPE. The GetNextFile() API can
+                                     filter it's search for files based on the
+                                     value of *FileType input. A *FileType input
+                                     of 0 causes GetNextFile() to search for
+                                     files of all types.  If a file is found, the
+                                     file's type is returned in *FileType.
+                                     *FileType is not modified if no file is
+                                     found.
+  @param  NameGuid                   NameGuid is a pointer to a caller allocated
+                                     EFI_GUID. If a file is found, the file's
+                                     name is returned in *NameGuid.  *NameGuid is
+                                     not modified if no file is found.
+  @param  Attributes                 Attributes is a pointer to a caller
+                                     allocated EFI_FV_FILE_ATTRIBUTES.  If a file
+                                     is found, the file's attributes are returned
+                                     in *Attributes. *Attributes is not modified
+                                     if no file is found.
+  @param  Size                       Size is a pointer to a caller allocated
+                                     UINTN. If a file is found, the file's size
+                                     is returned in *Size. *Size is not modified
+                                     if no file is found.
+
+  @retval EFI_SUCCESS                Successfully find the file.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Fv could not read.
+  @retval EFI_NOT_FOUND              No matching file found.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter
+
+**/
+EFI_STATUS
+EFIAPI
+FvGetNextFile (
+  IN CONST   EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  IN OUT     VOID                          *Key,
+  IN OUT     EFI_FV_FILETYPE               *FileType,
+  OUT        EFI_GUID                      *NameGuid,
+  OUT        EFI_FV_FILE_ATTRIBUTES        *Attributes,
+  OUT        UINTN                         *Size
+  )
+{
+  EFI_STATUS                                  Status;
+  FV_DEVICE                                   *FvDevice;
+  EFI_FV_ATTRIBUTES                           FvAttributes;
+  EFI_FFS_FILE_HEADER                         *FfsFileHeader;
+  UINTN                                       *KeyValue;
+  LIST_ENTRY                                  *Link;
+  FFS_FILE_LIST_ENTRY                         *FfsFileEntry;
+
+  FvDevice = FV_DEVICE_FROM_THIS (This);
+
+  Status = FvGetVolumeAttributes (This, &FvAttributes);
+  if (EFI_ERROR (Status)){
+    return Status;
+  }
+
+  //
+  // Check if read operation is enabled
+  //
+  if ((FvAttributes & EFI_FV2_READ_STATUS) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (*FileType > EFI_FV_FILETYPE_SMM_CORE) {
+    //
+    // File type needs to be in 0 - 0x0D
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  KeyValue = (UINTN *)Key;
+  for (;;) {
+    if (*KeyValue == 0) {
+      //
+      // Search for 1st matching file
+      //
+      Link = &FvDevice->FfsFileListHeader;
+    } else {
+      //
+      // Key is pointer to FFsFileEntry, so get next one
+      //
+      Link = (LIST_ENTRY *)(*KeyValue);
+    }
+
+    if (Link->ForwardLink == &FvDevice->FfsFileListHeader) {
+      //
+      // Next is end of list so we did not find data
+      //
+      return EFI_NOT_FOUND;
+    }
+
+    FfsFileEntry = (FFS_FILE_LIST_ENTRY *)Link->ForwardLink;
+    FfsFileHeader = (EFI_FFS_FILE_HEADER *)FfsFileEntry->FfsHeader;
+
+    //
+    // remember the key
+    //
+    *KeyValue = (UINTN)FfsFileEntry;
+
+    if (FfsFileHeader->Type == EFI_FV_FILETYPE_FFS_PAD) {
+      //
+      // we ignore pad files
+      //
+      continue;
+    }
+
+    if (*FileType == EFI_FV_FILETYPE_ALL) {
+      //
+      // Process all file types so we have a match
+      //
+      break;
+    }
+
+    if (*FileType == FfsFileHeader->Type) {
+      //
+      // Found a matching file type
+      //
+      break;
+    }
+
+  }
+
+  //
+  // Return FileType, NameGuid, and Attributes
+  //
+  *FileType = FfsFileHeader->Type;
+  CopyGuid (NameGuid, &FfsFileHeader->Name);
+  *Attributes = FfsAttributes2FvFileAttributes (FfsFileHeader->Attributes);
+  if ((FvDevice->FwVolHeader->Attributes & EFI_FVB2_MEMORY_MAPPED) == EFI_FVB2_MEMORY_MAPPED) {
+    *Attributes |= EFI_FV_FILE_ATTRIB_MEMORY_MAPPED;
+  }
+
+  //
+  // we need to substract the header size
+  //
+  if (IS_FFS_FILE2 (FfsFileHeader)) {
+    *Size = FFS_FILE2_SIZE (FfsFileHeader) - sizeof (EFI_FFS_FILE_HEADER2);
+  } else {
+    *Size = FFS_FILE_SIZE (FfsFileHeader) - sizeof (EFI_FFS_FILE_HEADER);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Locates a file in the firmware volume and
+  copies it to the supplied buffer.
+
+  @param  This                       Indicates the calling context.
+  @param  NameGuid                   Pointer to an EFI_GUID, which is the
+                                     filename.
+  @param  Buffer                     Buffer is a pointer to pointer to a buffer
+                                     in which the file or section contents or are
+                                     returned.
+  @param  BufferSize                 BufferSize is a pointer to caller allocated
+                                     UINTN. On input *BufferSize indicates the
+                                     size in bytes of the memory region pointed
+                                     to by Buffer. On output, *BufferSize
+                                     contains the number of bytes required to
+                                     read the file.
+  @param  FoundType                  FoundType is a pointer to a caller allocated
+                                     EFI_FV_FILETYPE that on successful return
+                                     from Read() contains the type of file read.
+                                     This output reflects the file type
+                                     irrespective of the value of the SectionType
+                                     input.
+  @param  FileAttributes             FileAttributes is a pointer to a caller
+                                     allocated EFI_FV_FILE_ATTRIBUTES.  On
+                                     successful return from Read(),
+                                     *FileAttributes contains the attributes of
+                                     the file read.
+  @param  AuthenticationStatus       AuthenticationStatus is a pointer to a
+                                     caller allocated UINTN in which the
+                                     authentication status is returned.
+
+  @retval EFI_SUCCESS                Successfully read to memory buffer.
+  @retval EFI_WARN_BUFFER_TOO_SMALL  Buffer too small.
+  @retval EFI_NOT_FOUND              Not found.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Could not read.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+  @retval EFI_OUT_OF_RESOURCES       Not enough buffer to be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+FvReadFile (
+  IN CONST EFI_FIRMWARE_VOLUME2_PROTOCOL *This,
+  IN CONST EFI_GUID                      *NameGuid,
+  IN OUT   VOID                          **Buffer,
+  IN OUT   UINTN                         *BufferSize,
+  OUT      EFI_FV_FILETYPE               *FoundType,
+  OUT      EFI_FV_FILE_ATTRIBUTES        *FileAttributes,
+  OUT      UINT32                        *AuthenticationStatus
+  )
+{
+  EFI_STATUS                        Status;
+  FV_DEVICE                         *FvDevice;
+  EFI_GUID                          SearchNameGuid;
+  EFI_FV_FILETYPE                   LocalFoundType;
+  EFI_FV_FILE_ATTRIBUTES            LocalAttributes;
+  UINTN                             FileSize;
+  UINT8                             *SrcPtr;
+  EFI_FFS_FILE_HEADER               *FfsHeader;
+  UINTN                             InputBufferSize;
+  UINTN                             WholeFileSize;
+
+  if (NameGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FvDevice = FV_DEVICE_FROM_THIS (This);
+
+
+  //
+  // Keep looking until we find the matching NameGuid.
+  // The Key is really a FfsFileEntry
+  //
+  FvDevice->LastKey = 0;
+  do {
+    LocalFoundType = 0;
+    Status = FvGetNextFile (
+              This,
+              &FvDevice->LastKey,
+              &LocalFoundType,
+              &SearchNameGuid,
+              &LocalAttributes,
+              &FileSize
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_NOT_FOUND;
+    }
+  } while (!CompareGuid (&SearchNameGuid, NameGuid));
+
+  //
+  // Get a pointer to the header
+  //
+  FfsHeader = FvDevice->LastKey->FfsHeader;
+  if (FvDevice->IsMemoryMapped) {
+    //
+    // Memory mapped FV has not been cached, so here is to cache by file.
+    //
+    if (!FvDevice->LastKey->FileCached) {
+      //
+      // Cache FFS file to memory buffer.
+      //
+      WholeFileSize = IS_FFS_FILE2 (FfsHeader) ? FFS_FILE2_SIZE (FfsHeader): FFS_FILE_SIZE (FfsHeader);
+      FfsHeader = AllocateCopyPool (WholeFileSize, FfsHeader);
+      if (FfsHeader == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Let FfsHeader in FfsFileEntry point to the cached file buffer.
+      //
+      FvDevice->LastKey->FfsHeader = FfsHeader;
+      FvDevice->LastKey->FileCached = TRUE;
+    }
+  }
+
+  //
+  // Remember callers buffer size
+  //
+  InputBufferSize = *BufferSize;
+
+  //
+  // Calculate return values
+  //
+  *FoundType = FfsHeader->Type;
+  *FileAttributes = FfsAttributes2FvFileAttributes (FfsHeader->Attributes);
+   if ((FvDevice->FwVolHeader->Attributes & EFI_FVB2_MEMORY_MAPPED) == EFI_FVB2_MEMORY_MAPPED) {
+     *FileAttributes |= EFI_FV_FILE_ATTRIB_MEMORY_MAPPED;
+   }
+  *AuthenticationStatus = 0;
+  *BufferSize = FileSize;
+
+  if (Buffer == NULL) {
+    //
+    // If Buffer is NULL, we only want to get the information collected so far
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip over file header
+  //
+  if (IS_FFS_FILE2 (FfsHeader)) {
+    SrcPtr = ((UINT8 *) FfsHeader) + sizeof (EFI_FFS_FILE_HEADER2);
+  } else {
+    SrcPtr = ((UINT8 *) FfsHeader) + sizeof (EFI_FFS_FILE_HEADER);
+  }
+
+  Status = EFI_SUCCESS;
+  if (*Buffer == NULL) {
+    //
+    // Caller passed in a pointer so allocate buffer for them
+    //
+    *Buffer = AllocatePool (FileSize);
+    if (*Buffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else if (FileSize > InputBufferSize) {
+    //
+    // Callers buffer was not big enough
+    //
+    Status = EFI_WARN_BUFFER_TOO_SMALL;
+    FileSize = InputBufferSize;
+  }
+
+  //
+  // Copy data into callers buffer
+  //
+  CopyMem (*Buffer, SrcPtr, FileSize);
+
+  return Status;
+}
+
+
+
+/**
+  Locates a section in a given FFS File and
+  copies it to the supplied buffer (not including section header).
+
+  @param  This                       Indicates the calling context.
+  @param  NameGuid                   Pointer to an EFI_GUID, which is the
+                                     filename.
+  @param  SectionType                Indicates the section type to return.
+  @param  SectionInstance            Indicates which instance of sections with a
+                                     type of SectionType to return.
+  @param  Buffer                     Buffer is a pointer to pointer to a buffer
+                                     in which the file or section contents or are
+                                     returned.
+  @param  BufferSize                 BufferSize is a pointer to caller allocated
+                                     UINTN.
+  @param  AuthenticationStatus       AuthenticationStatus is a pointer to a
+                                     caller allocated UINT32 in which the
+                                     authentication status is returned.
+
+  @retval EFI_SUCCESS                Successfully read the file section into
+                                     buffer.
+  @retval EFI_WARN_BUFFER_TOO_SMALL  Buffer too small.
+  @retval EFI_NOT_FOUND              Section not found.
+  @retval EFI_DEVICE_ERROR           Device error.
+  @retval EFI_ACCESS_DENIED          Could not read.
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+
+**/
+EFI_STATUS
+EFIAPI
+FvReadFileSection (
+  IN CONST  EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN CONST  EFI_GUID                       *NameGuid,
+  IN        EFI_SECTION_TYPE               SectionType,
+  IN        UINTN                          SectionInstance,
+  IN OUT    VOID                           **Buffer,
+  IN OUT    UINTN                          *BufferSize,
+  OUT       UINT32                         *AuthenticationStatus
+  )
+{
+  EFI_STATUS                        Status;
+  FV_DEVICE                         *FvDevice;
+  EFI_FV_FILETYPE                   FileType;
+  EFI_FV_FILE_ATTRIBUTES            FileAttributes;
+  UINTN                             FileSize;
+  UINT8                             *FileBuffer;
+  FFS_FILE_LIST_ENTRY               *FfsEntry;
+
+  if (NameGuid == NULL || Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FvDevice = FV_DEVICE_FROM_THIS (This);
+
+  //
+  // Read the file
+  //
+  Status = FvReadFile (
+            This,
+            NameGuid,
+            NULL,
+            &FileSize,
+            &FileType,
+            &FileAttributes,
+            AuthenticationStatus
+            );
+  //
+  // Get the last key used by our call to FvReadFile as it is the FfsEntry for this file.
+  //
+  FfsEntry = (FFS_FILE_LIST_ENTRY *) FvDevice->LastKey;
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  if (IS_FFS_FILE2 (FfsEntry->FfsHeader)) {
+    FileBuffer = ((UINT8 *) FfsEntry->FfsHeader) + sizeof (EFI_FFS_FILE_HEADER2);
+  } else {
+    FileBuffer = ((UINT8 *) FfsEntry->FfsHeader) + sizeof (EFI_FFS_FILE_HEADER);
+  }
+  //
+  // Check to see that the file actually HAS sections before we go any further.
+  //
+  if (FileType == EFI_FV_FILETYPE_RAW) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  //
+  // Use FfsEntry to cache Section Extraction Protocol Information
+  //
+  if (FfsEntry->StreamHandle == 0) {
+    Status = OpenSectionStream (
+               FileSize,
+               FileBuffer,
+               &FfsEntry->StreamHandle
+               );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+  }
+
+  //
+  // If SectionType == 0 We need the whole section stream
+  //
+  Status = GetSection (
+             FfsEntry->StreamHandle,
+             (SectionType == 0) ? NULL : &SectionType,
+             NULL,
+             (SectionType == 0) ? 0 : SectionInstance,
+             Buffer,
+             BufferSize,
+             AuthenticationStatus,
+             FvDevice->IsFfs3Fv
+             );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Inherit the authentication status.
+    //
+    *AuthenticationStatus |= FvDevice->AuthenticationStatus;
+  }
+
+  //
+  // Close of stream defered to close of FfsHeader list to allow SEP to cache data
+  //
+
+Done:
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolWrite.c b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolWrite.c
new file mode 100644
index 0000000000..1ec563c865
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVol/FwVolWrite.c
@@ -0,0 +1,52 @@
+/** @file
+  Implements functions to write firmware file
+
+Copyright (c) 2006 - 2008, 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 "DxeMain.h"
+#include "FwVolDriver.h"
+
+
+/**
+  Writes one or more files to the firmware volume.
+
+  @param  This                   Indicates the calling context.
+  @param  NumberOfFiles          Number of files.
+  @param  WritePolicy            WritePolicy indicates the level of reliability
+                                 for the write in the event of a power failure or
+                                 other system failure during the write operation.
+  @param  FileData               FileData is an pointer to an array of
+                                 EFI_FV_WRITE_DATA. Each element of array
+                                 FileData represents a file to be written.
+
+  @retval EFI_SUCCESS            Files successfully written to firmware volume
+  @retval EFI_OUT_OF_RESOURCES   Not enough buffer to be allocated.
+  @retval EFI_DEVICE_ERROR       Device error.
+  @retval EFI_WRITE_PROTECTED    Write protected.
+  @retval EFI_NOT_FOUND          Not found.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_UNSUPPORTED        This function not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FvWriteFile (
+  IN CONST EFI_FIRMWARE_VOLUME2_PROTOCOL  *This,
+  IN       UINT32                         NumberOfFiles,
+  IN       EFI_FV_WRITE_POLICY            WritePolicy,
+  IN       EFI_FV_WRITE_FILE_DATA         *FileData
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.c b/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.c
new file mode 100644
index 0000000000..fffd821b51
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.c
@@ -0,0 +1,705 @@
+/** @file
+  Implementations for Firmware Volume Block protocol.
+
+  It consumes FV HOBs and creates read-only Firmare Volume Block protocol
+  instances for each of them.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "FwVolBlock.h"
+
+FV_MEMMAP_DEVICE_PATH mFvMemmapDevicePathTemplate = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_MEMMAP_DP,
+      {
+        (UINT8)(sizeof (MEMMAP_DEVICE_PATH)),
+        (UINT8)(sizeof (MEMMAP_DEVICE_PATH) >> 8)
+      }
+    },
+    EfiMemoryMappedIO,
+    (EFI_PHYSICAL_ADDRESS) 0,
+    (EFI_PHYSICAL_ADDRESS) 0,
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      END_DEVICE_PATH_LENGTH,
+      0
+    }
+  }
+};
+
+FV_PIWG_DEVICE_PATH mFvPIWGDevicePathTemplate = {
+  {
+    {
+      MEDIA_DEVICE_PATH,
+      MEDIA_PIWG_FW_VOL_DP,
+      {
+        (UINT8)(sizeof (MEDIA_FW_VOL_DEVICE_PATH)),
+        (UINT8)(sizeof (MEDIA_FW_VOL_DEVICE_PATH) >> 8)
+      }
+    },
+    { 0 }
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      END_DEVICE_PATH_LENGTH,
+      0
+    }
+  }
+};
+
+EFI_FW_VOL_BLOCK_DEVICE  mFwVolBlock = {
+  FVB_DEVICE_SIGNATURE,
+  NULL,
+  NULL,
+  {
+    FwVolBlockGetAttributes,
+    (EFI_FVB_SET_ATTRIBUTES)FwVolBlockSetAttributes,
+    FwVolBlockGetPhysicalAddress,
+    FwVolBlockGetBlockSize,
+    FwVolBlockReadBlock,
+    (EFI_FVB_WRITE)FwVolBlockWriteBlock,
+    (EFI_FVB_ERASE_BLOCKS)FwVolBlockEraseBlock,
+    NULL
+  },
+  0,
+  NULL,
+  0,
+  0,
+  0
+};
+
+
+
+/**
+  Retrieves Volume attributes.  No polarity translations are done.
+
+  @param  This                   Calling context
+  @param  Attributes             output buffer which contains attributes
+
+  @retval EFI_SUCCESS            The firmware volume attributes were returned.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetAttributes (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  OUT       EFI_FVB_ATTRIBUTES_2                *Attributes
+  )
+{
+  EFI_FW_VOL_BLOCK_DEVICE               *FvbDevice;
+
+  FvbDevice = FVB_DEVICE_FROM_THIS (This);
+
+  //
+  // Since we are read only, it's safe to get attributes data from our in-memory copy.
+  //
+  *Attributes = FvbDevice->FvbAttributes & ~EFI_FVB2_WRITE_STATUS;
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Modifies the current settings of the firmware volume according to the input parameter.
+
+  @param  This                   Calling context
+  @param  Attributes             input buffer which contains attributes
+
+  @retval EFI_SUCCESS            The firmware volume attributes were returned.
+  @retval EFI_INVALID_PARAMETER  The attributes requested are in conflict with
+                                 the capabilities as declared in the firmware
+                                 volume header.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockSetAttributes (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  IN CONST  EFI_FVB_ATTRIBUTES_2                *Attributes
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+
+/**
+  The EraseBlock() function erases one or more blocks as denoted by the
+  variable argument list. The entire parameter list of blocks must be verified
+  prior to erasing any blocks.  If a block is requested that does not exist
+  within the associated firmware volume (it has a larger index than the last
+  block of the firmware volume), the EraseBlock() function must return
+  EFI_INVALID_PARAMETER without modifying the contents of the firmware volume.
+
+  @param  This                   Calling context
+  @param  ...                    Starting LBA followed by Number of Lba to erase.
+                                 a -1 to terminate the list.
+
+  @retval EFI_SUCCESS            The erase request was successfully completed.
+  @retval EFI_ACCESS_DENIED      The firmware volume is in the WriteDisabled
+                                 state.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly
+                                 and could not be written. The firmware device
+                                 may have been partially erased.
+  @retval EFI_INVALID_PARAMETER  One or more of the LBAs listed in the variable
+                                 argument list do
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockEraseBlock (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *This,
+  ...
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+
+/**
+  Read the specified number of bytes from the block to the input buffer.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    The starting logical block index to read.
+  @param  Offset                 Offset into the block at which to begin reading.
+  @param  NumBytes               Pointer to a UINT32. At entry, *NumBytes
+                                 contains the total size of the buffer. At exit,
+                                 *NumBytes contains the total number of bytes
+                                 actually read.
+  @param  Buffer                 Pinter to a caller-allocated buffer that
+                                 contains the destine for the read.
+
+  @retval EFI_SUCCESS            The firmware volume was read successfully.
+  @retval EFI_BAD_BUFFER_SIZE    The read was attempted across an LBA boundary.
+  @retval EFI_ACCESS_DENIED      Access denied.
+  @retval EFI_DEVICE_ERROR       The block device is malfunctioning and could not
+                                 be read.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockReadBlock (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL   *This,
+  IN CONST  EFI_LBA                              Lba,
+  IN CONST  UINTN                                Offset,
+  IN OUT    UINTN                                *NumBytes,
+  IN OUT    UINT8                                *Buffer
+  )
+{
+  EFI_FW_VOL_BLOCK_DEVICE               *FvbDevice;
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+  UINT8                                 *LbaOffset;
+  UINTN                                 LbaStart;
+  UINTN                                 NumOfBytesRead;
+  UINTN                                 LbaIndex;
+
+  FvbDevice = FVB_DEVICE_FROM_THIS (This);
+
+  //
+  // Check if This FW can be read
+  //
+  if ((FvbDevice->FvbAttributes & EFI_FVB2_READ_STATUS) == 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  LbaIndex = (UINTN) Lba;
+  if (LbaIndex >= FvbDevice->NumBlocks) {
+    //
+    // Invalid Lba, read nothing.
+    //
+    *NumBytes = 0;
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (Offset > FvbDevice->LbaCache[LbaIndex].Length) {
+    //
+    // all exceed boundry, read nothing.
+    //
+    *NumBytes = 0;
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NumOfBytesRead = *NumBytes;
+  if (Offset + NumOfBytesRead > FvbDevice->LbaCache[LbaIndex].Length) {
+    //
+    // partial exceed boundry, read data from current postion to end.
+    //
+    NumOfBytesRead = FvbDevice->LbaCache[LbaIndex].Length - Offset;
+  }
+
+  LbaStart = FvbDevice->LbaCache[LbaIndex].Base;
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *)((UINTN) FvbDevice->BaseAddress);
+  LbaOffset = (UINT8 *) FwVolHeader + LbaStart + Offset;
+
+  //
+  // Perform read operation
+  //
+  CopyMem (Buffer, LbaOffset, NumOfBytesRead);
+
+  if (NumOfBytesRead == *NumBytes) {
+    return EFI_SUCCESS;
+  }
+
+  *NumBytes = NumOfBytesRead;
+  return EFI_BAD_BUFFER_SIZE;
+}
+
+
+
+/**
+  Writes the specified number of bytes from the input buffer to the block.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    The starting logical block index to write to.
+  @param  Offset                 Offset into the block at which to begin writing.
+  @param  NumBytes               Pointer to a UINT32. At entry, *NumBytes
+                                 contains the total size of the buffer. At exit,
+                                 *NumBytes contains the total number of bytes
+                                 actually written.
+  @param  Buffer                 Pinter to a caller-allocated buffer that
+                                 contains the source for the write.
+
+  @retval EFI_SUCCESS            The firmware volume was written successfully.
+  @retval EFI_BAD_BUFFER_SIZE    The write was attempted across an LBA boundary.
+                                 On output, NumBytes contains the total number of
+                                 bytes actually written.
+  @retval EFI_ACCESS_DENIED      The firmware volume is in the WriteDisabled
+                                 state.
+  @retval EFI_DEVICE_ERROR       The block device is malfunctioning and could not
+                                 be written.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockWriteBlock (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL   *This,
+  IN     EFI_LBA                              Lba,
+  IN     UINTN                                Offset,
+  IN OUT UINTN                                *NumBytes,
+  IN     UINT8                                *Buffer
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+
+/**
+  Get Fvb's base address.
+
+  @param  This                   Indicates the calling context.
+  @param  Address                Fvb device base address.
+
+  @retval EFI_SUCCESS            Successfully got Fvb's base address.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetPhysicalAddress (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  OUT       EFI_PHYSICAL_ADDRESS                *Address
+  )
+{
+  EFI_FW_VOL_BLOCK_DEVICE               *FvbDevice;
+
+  FvbDevice = FVB_DEVICE_FROM_THIS (This);
+
+  if ((FvbDevice->FvbAttributes & EFI_FVB2_MEMORY_MAPPED) != 0) {
+    *Address = FvbDevice->BaseAddress;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+
+
+/**
+  Retrieves the size in bytes of a specific block within a firmware volume.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    Indicates the block for which to return the
+                                 size.
+  @param  BlockSize              Pointer to a caller-allocated UINTN in which the
+                                 size of the block is returned.
+  @param  NumberOfBlocks         Pointer to a caller-allocated UINTN in which the
+                                 number of consecutive blocks starting with Lba
+                                 is returned. All blocks in this range have a
+                                 size of BlockSize.
+
+  @retval EFI_SUCCESS            The firmware volume base address is returned.
+  @retval EFI_INVALID_PARAMETER  The requested LBA is out of range.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetBlockSize (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  IN CONST  EFI_LBA                             Lba,
+  IN OUT    UINTN                               *BlockSize,
+  IN OUT    UINTN                               *NumberOfBlocks
+  )
+{
+  UINTN                                 TotalBlocks;
+  EFI_FW_VOL_BLOCK_DEVICE               *FvbDevice;
+  EFI_FV_BLOCK_MAP_ENTRY                *PtrBlockMapEntry;
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+
+  FvbDevice = FVB_DEVICE_FROM_THIS (This);
+
+  //
+  // Do parameter checking
+  //
+  if (Lba >= FvbDevice->NumBlocks) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *)((UINTN)FvbDevice->BaseAddress);
+
+  PtrBlockMapEntry = FwVolHeader->BlockMap;
+
+  //
+  // Search the block map for the given block
+  //
+  TotalBlocks = 0;
+  while ((PtrBlockMapEntry->NumBlocks != 0) || (PtrBlockMapEntry->Length !=0 )) {
+    TotalBlocks += PtrBlockMapEntry->NumBlocks;
+    if (Lba < TotalBlocks) {
+      //
+      // We find the range
+      //
+      break;
+    }
+
+    PtrBlockMapEntry++;
+  }
+
+  *BlockSize = PtrBlockMapEntry->Length;
+  *NumberOfBlocks = TotalBlocks - (UINTN)Lba;
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Get FVB authentication status
+
+  @param FvbProtocol    FVB protocol.
+
+  @return Authentication status.
+
+**/
+UINT32
+GetFvbAuthenticationStatus (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL     *FvbProtocol
+  )
+{
+  EFI_FW_VOL_BLOCK_DEVICE   *FvbDevice;
+  UINT32                    AuthenticationStatus;
+
+  AuthenticationStatus = 0;
+  FvbDevice = BASE_CR (FvbProtocol, EFI_FW_VOL_BLOCK_DEVICE, FwVolBlockInstance);
+  if (FvbDevice->Signature == FVB_DEVICE_SIGNATURE) {
+    AuthenticationStatus = FvbDevice->AuthenticationStatus;
+  }
+
+  return AuthenticationStatus;
+}
+
+/**
+  This routine produces a firmware volume block protocol on a given
+  buffer.
+
+  @param  BaseAddress            base address of the firmware volume image
+  @param  Length                 length of the firmware volume image
+  @param  ParentHandle           handle of parent firmware volume, if this image
+                                 came from an FV image file and section in another firmware
+                                 volume (ala capsules)
+  @param  AuthenticationStatus   Authentication status inherited, if this image
+                                 came from an FV image file and section in another firmware volume.
+  @param  FvProtocol             Firmware volume block protocol produced.
+
+  @retval EFI_VOLUME_CORRUPTED   Volume corrupted.
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to be allocated.
+  @retval EFI_SUCCESS            Successfully produced a FVB protocol on given
+                                 buffer.
+
+**/
+EFI_STATUS
+ProduceFVBProtocolOnBuffer (
+  IN EFI_PHYSICAL_ADDRESS   BaseAddress,
+  IN UINT64                 Length,
+  IN EFI_HANDLE             ParentHandle,
+  IN UINT32                 AuthenticationStatus,
+  OUT EFI_HANDLE            *FvProtocol  OPTIONAL
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_FW_VOL_BLOCK_DEVICE       *FvbDev;
+  EFI_FIRMWARE_VOLUME_HEADER    *FwVolHeader;
+  UINTN                         BlockIndex;
+  UINTN                         BlockIndex2;
+  UINTN                         LinearOffset;
+  UINT32                        FvAlignment;
+  EFI_FV_BLOCK_MAP_ENTRY        *PtrBlockMapEntry;
+
+  FvAlignment = 0;
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *)(UINTN) BaseAddress;
+  //
+  // Validate FV Header, if not as expected, return
+  //
+  if (FwVolHeader->Signature != EFI_FVH_SIGNATURE) {
+    return EFI_VOLUME_CORRUPTED;
+  }
+
+  //
+  // If EFI_FVB2_WEAK_ALIGNMENT is set in the volume header then the first byte of the volume
+  // can be aligned on any power-of-two boundary. A weakly aligned volume can not be moved from
+  // its initial linked location and maintain its alignment.
+  //
+  if ((FwVolHeader->Attributes & EFI_FVB2_WEAK_ALIGNMENT) != EFI_FVB2_WEAK_ALIGNMENT) {
+    //
+    // Get FvHeader alignment
+    //
+    FvAlignment = 1 << ((FwVolHeader->Attributes & EFI_FVB2_ALIGNMENT) >> 16);
+    //
+    // FvAlignment must be greater than or equal to 8 bytes of the minimum FFS alignment value.
+    //
+    if (FvAlignment < 8) {
+      FvAlignment = 8;
+    }
+    if ((UINTN)BaseAddress % FvAlignment != 0) {
+      //
+      // FvImage buffer is not at its required alignment.
+      //
+      return EFI_VOLUME_CORRUPTED;
+    }
+  }
+
+  //
+  // Allocate EFI_FW_VOL_BLOCK_DEVICE
+  //
+  FvbDev = AllocateCopyPool (sizeof (EFI_FW_VOL_BLOCK_DEVICE), &mFwVolBlock);
+  if (FvbDev == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  FvbDev->BaseAddress   = BaseAddress;
+  FvbDev->FvbAttributes = FwVolHeader->Attributes;
+  FvbDev->FwVolBlockInstance.ParentHandle = ParentHandle;
+  if (ParentHandle != NULL) {
+    FvbDev->AuthenticationStatus = AuthenticationStatus;
+  }
+
+  //
+  // Init the block caching fields of the device
+  // First, count the number of blocks
+  //
+  FvbDev->NumBlocks = 0;
+  for (PtrBlockMapEntry = FwVolHeader->BlockMap;
+       PtrBlockMapEntry->NumBlocks != 0;
+       PtrBlockMapEntry++) {
+    FvbDev->NumBlocks += PtrBlockMapEntry->NumBlocks;
+  }
+
+  //
+  // Second, allocate the cache
+  //
+  if (FvbDev->NumBlocks >= (MAX_ADDRESS / sizeof (LBA_CACHE))) {
+    CoreFreePool (FvbDev);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  FvbDev->LbaCache = AllocatePool (FvbDev->NumBlocks * sizeof (LBA_CACHE));
+  if (FvbDev->LbaCache == NULL) {
+    CoreFreePool (FvbDev);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Last, fill in the cache with the linear address of the blocks
+  //
+  BlockIndex = 0;
+  LinearOffset = 0;
+  for (PtrBlockMapEntry = FwVolHeader->BlockMap;
+       PtrBlockMapEntry->NumBlocks != 0; PtrBlockMapEntry++) {
+    for (BlockIndex2 = 0; BlockIndex2 < PtrBlockMapEntry->NumBlocks; BlockIndex2++) {
+      FvbDev->LbaCache[BlockIndex].Base = LinearOffset;
+      FvbDev->LbaCache[BlockIndex].Length = PtrBlockMapEntry->Length;
+      LinearOffset += PtrBlockMapEntry->Length;
+      BlockIndex++;
+    }
+  }
+
+  //
+  // Judget whether FV name guid is produced in Fv extension header
+  //
+  if (FwVolHeader->ExtHeaderOffset == 0) {
+    //
+    // FV does not contains extension header, then produce MEMMAP_DEVICE_PATH
+    //
+    FvbDev->DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) AllocateCopyPool (sizeof (FV_MEMMAP_DEVICE_PATH), &mFvMemmapDevicePathTemplate);
+    if (FvbDev->DevicePath == NULL) {
+      FreePool (FvbDev);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    ((FV_MEMMAP_DEVICE_PATH *) FvbDev->DevicePath)->MemMapDevPath.StartingAddress = BaseAddress;
+    ((FV_MEMMAP_DEVICE_PATH *) FvbDev->DevicePath)->MemMapDevPath.EndingAddress   = BaseAddress + FwVolHeader->FvLength - 1;
+  } else {
+    //
+    // FV contains extension header, then produce MEDIA_FW_VOL_DEVICE_PATH
+    //
+    FvbDev->DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) AllocateCopyPool (sizeof (FV_PIWG_DEVICE_PATH), &mFvPIWGDevicePathTemplate);
+    if (FvbDev->DevicePath == NULL) {
+      FreePool (FvbDev);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyGuid (
+      &((FV_PIWG_DEVICE_PATH *)FvbDev->DevicePath)->FvDevPath.FvName, 
+      (GUID *)(UINTN)(BaseAddress + FwVolHeader->ExtHeaderOffset)
+      );
+  }
+  
+  //
+  //
+  // Attach FvVolBlock Protocol to new handle
+  //
+  Status = CoreInstallMultipleProtocolInterfaces (
+             &FvbDev->Handle,
+             &gEfiFirmwareVolumeBlockProtocolGuid,     &FvbDev->FwVolBlockInstance,
+             &gEfiDevicePathProtocolGuid,              FvbDev->DevicePath,
+             NULL
+             );
+
+  //
+  // If they want the handle back, set it.
+  //
+  if (FvProtocol != NULL) {
+    *FvProtocol = FvbDev->Handle;
+  }
+
+  return Status;
+}
+
+
+
+/**
+  This routine consumes FV hobs and produces instances of FW_VOL_BLOCK_PROTOCOL as appropriate.
+
+  @param  ImageHandle            The image handle.
+  @param  SystemTable            The system table.
+
+  @retval EFI_SUCCESS            Successfully initialized firmware volume block
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockDriverInit (
+  IN EFI_HANDLE                 ImageHandle,
+  IN EFI_SYSTEM_TABLE           *SystemTable
+  )
+{
+  EFI_PEI_HOB_POINTERS          FvHob;
+
+  //
+  // Core Needs Firmware Volumes to function
+  //
+  FvHob.Raw = GetHobList ();
+  while ((FvHob.Raw = GetNextHob (EFI_HOB_TYPE_FV, FvHob.Raw)) != NULL) {
+    //
+    // Produce an FVB protocol for it
+    //
+    ProduceFVBProtocolOnBuffer (FvHob.FirmwareVolume->BaseAddress, FvHob.FirmwareVolume->Length, NULL, 0, NULL);
+    FvHob.Raw = GET_NEXT_HOB (FvHob);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  This DXE service routine is used to process a firmware volume. In
+  particular, it can be called by BDS to process a single firmware
+  volume found in a capsule.
+
+  Caution: The caller need validate the input firmware volume to follow
+  PI specification.
+  DxeCore will trust the input data and process firmware volume directly.
+
+  @param  FvHeader               pointer to a firmware volume header
+  @param  Size                   the size of the buffer pointed to by FvHeader
+  @param  FVProtocolHandle       the handle on which a firmware volume protocol
+                                 was produced for the firmware volume passed in.
+
+  @retval EFI_OUT_OF_RESOURCES   if an FVB could not be produced due to lack of
+                                 system resources
+  @retval EFI_VOLUME_CORRUPTED   if the volume was corrupted
+  @retval EFI_SUCCESS            a firmware volume protocol was produced for the
+                                 firmware volume
+
+**/
+EFI_STATUS
+EFIAPI
+CoreProcessFirmwareVolume (
+  IN VOID                             *FvHeader,
+  IN UINTN                            Size,
+  OUT EFI_HANDLE                      *FVProtocolHandle
+  )
+{
+  VOID        *Ptr;
+  EFI_STATUS  Status;
+
+  *FVProtocolHandle = NULL;
+  Status = ProduceFVBProtocolOnBuffer (
+            (EFI_PHYSICAL_ADDRESS) (UINTN) FvHeader,
+            (UINT64)Size,
+            NULL,
+            0,
+            FVProtocolHandle
+            );
+  //
+  // Since in our implementation we use register-protocol-notify to put a
+  // FV protocol on the FVB protocol handle, we can't directly verify that
+  // the FV protocol was produced. Therefore here we will check the handle
+  // and make sure an FV protocol is on it. This indicates that all went
+  // well. Otherwise we have to assume that the volume was corrupted
+  // somehow.
+  //
+  if (!EFI_ERROR(Status)) {
+    ASSERT (*FVProtocolHandle != NULL);
+    Ptr = NULL;
+    Status = CoreHandleProtocol (*FVProtocolHandle, &gEfiFirmwareVolume2ProtocolGuid, (VOID **) &Ptr);
+    if (EFI_ERROR(Status) || (Ptr == NULL)) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+    return EFI_SUCCESS;
+  }
+  return Status;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.h b/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.h
new file mode 100644
index 0000000000..7ad4e35ecb
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/FwVolBlock/FwVolBlock.h
@@ -0,0 +1,244 @@
+/** @file
+  Firmware Volume Block protocol functions.
+  Consumes FV hobs and creates appropriate block protocols.
+
+Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef _FWVOL_BLOCK_H_
+#define _FWVOL_BLOCK_H_
+
+
+#define FVB_DEVICE_SIGNATURE       SIGNATURE_32('_','F','V','B')
+
+
+typedef struct {
+  UINTN                       Base;
+  UINTN                       Length;
+} LBA_CACHE;
+
+typedef struct {
+  MEMMAP_DEVICE_PATH          MemMapDevPath;
+  EFI_DEVICE_PATH_PROTOCOL    EndDevPath;
+} FV_MEMMAP_DEVICE_PATH;
+
+//
+// UEFI Specification define FV device path format if FV provide name guid in extension header
+//
+typedef struct {
+  MEDIA_FW_VOL_DEVICE_PATH    FvDevPath;
+  EFI_DEVICE_PATH_PROTOCOL    EndDevPath;
+} FV_PIWG_DEVICE_PATH;
+
+typedef struct {
+  UINTN                                 Signature;
+  EFI_HANDLE                            Handle;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    FwVolBlockInstance;
+  UINTN                                 NumBlocks;
+  LBA_CACHE                             *LbaCache;
+  UINT32                                FvbAttributes;
+  EFI_PHYSICAL_ADDRESS                  BaseAddress;
+  UINT32                                AuthenticationStatus;
+} EFI_FW_VOL_BLOCK_DEVICE;
+
+
+#define FVB_DEVICE_FROM_THIS(a) \
+  CR(a, EFI_FW_VOL_BLOCK_DEVICE, FwVolBlockInstance, FVB_DEVICE_SIGNATURE)
+
+
+/**
+  Retrieves Volume attributes.  No polarity translations are done.
+
+  @param  This                   Calling context
+  @param  Attributes             output buffer which contains attributes
+
+  @retval EFI_SUCCESS            The firmware volume attributes were returned.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetAttributes (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  OUT       EFI_FVB_ATTRIBUTES_2                *Attributes
+  );
+
+
+
+/**
+  Modifies the current settings of the firmware volume according to the input parameter.
+
+  @param  This                   Calling context
+  @param  Attributes             input buffer which contains attributes
+
+  @retval EFI_SUCCESS            The firmware volume attributes were returned.
+  @retval EFI_INVALID_PARAMETER  The attributes requested are in conflict with
+                                 the capabilities as declared in the firmware
+                                 volume header.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockSetAttributes (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  IN CONST  EFI_FVB_ATTRIBUTES_2                *Attributes
+  );
+
+
+
+/**
+  The EraseBlock() function erases one or more blocks as denoted by the
+  variable argument list. The entire parameter list of blocks must be verified
+  prior to erasing any blocks.  If a block is requested that does not exist
+  within the associated firmware volume (it has a larger index than the last
+  block of the firmware volume), the EraseBlock() function must return
+  EFI_INVALID_PARAMETER without modifying the contents of the firmware volume.
+
+  @param  This                   Calling context
+  @param  ...                    Starting LBA followed by Number of Lba to erase.
+                                 a -1 to terminate the list.
+
+  @retval EFI_SUCCESS            The erase request was successfully completed.
+  @retval EFI_ACCESS_DENIED      The firmware volume is in the WriteDisabled
+                                 state.
+  @retval EFI_DEVICE_ERROR       The block device is not functioning correctly
+                                 and could not be written. The firmware device
+                                 may have been partially erased.
+  @retval EFI_INVALID_PARAMETER  One or more of the LBAs listed in the variable
+                                 argument list do
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockEraseBlock (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *This,
+  ...
+  );
+
+
+
+/**
+  Read the specified number of bytes from the block to the input buffer.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    The starting logical block index to read.
+  @param  Offset                 Offset into the block at which to begin reading.
+  @param  NumBytes               Pointer to a UINT32. At entry, *NumBytes
+                                 contains the total size of the buffer. At exit,
+                                 *NumBytes contains the total number of bytes
+                                 actually read.
+  @param  Buffer                 Pinter to a caller-allocated buffer that
+                                 contains the destine for the read.
+
+  @retval EFI_SUCCESS            The firmware volume was read successfully.
+  @retval EFI_BAD_BUFFER_SIZE    The read was attempted across an LBA boundary.
+  @retval EFI_ACCESS_DENIED      Access denied.
+  @retval EFI_DEVICE_ERROR       The block device is malfunctioning and could not
+                                 be read.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockReadBlock (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL   *This,
+  IN CONST  EFI_LBA                              Lba,
+  IN CONST  UINTN                                Offset,
+  IN OUT    UINTN                                *NumBytes,
+  IN OUT    UINT8                                *Buffer
+  );
+
+
+
+/**
+  Writes the specified number of bytes from the input buffer to the block.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    The starting logical block index to write to.
+  @param  Offset                 Offset into the block at which to begin writing.
+  @param  NumBytes               Pointer to a UINT32. At entry, *NumBytes
+                                 contains the total size of the buffer. At exit,
+                                 *NumBytes contains the total number of bytes
+                                 actually written.
+  @param  Buffer                 Pinter to a caller-allocated buffer that
+                                 contains the source for the write.
+
+  @retval EFI_SUCCESS            The firmware volume was written successfully.
+  @retval EFI_BAD_BUFFER_SIZE    The write was attempted across an LBA boundary.
+                                 On output, NumBytes contains the total number of
+                                 bytes actually written.
+  @retval EFI_ACCESS_DENIED      The firmware volume is in the WriteDisabled
+                                 state.
+  @retval EFI_DEVICE_ERROR       The block device is malfunctioning and could not
+                                 be written.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockWriteBlock (
+  IN     EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL   *This,
+  IN     EFI_LBA                              Lba,
+  IN     UINTN                                Offset,
+  IN OUT UINTN                                *NumBytes,
+  IN     UINT8                                *Buffer
+  );
+
+
+
+/**
+  Get Fvb's base address.
+
+  @param  This                   Indicates the calling context.
+  @param  Address                Fvb device base address.
+
+  @retval EFI_SUCCESS            Successfully got Fvb's base address.
+  @retval EFI_UNSUPPORTED        Not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetPhysicalAddress (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  OUT       EFI_PHYSICAL_ADDRESS                *Address
+  );
+
+
+
+/**
+  Retrieves the size in bytes of a specific block within a firmware volume.
+
+  @param  This                   Indicates the calling context.
+  @param  Lba                    Indicates the block for which to return the
+                                 size.
+  @param  BlockSize              Pointer to a caller-allocated UINTN in which the
+                                 size of the block is returned.
+  @param  NumberOfBlocks         Pointer to a caller-allocated UINTN in which the
+                                 number of consecutive blocks starting with Lba
+                                 is returned. All blocks in this range have a
+                                 size of BlockSize.
+
+  @retval EFI_SUCCESS            The firmware volume base address is returned.
+  @retval EFI_INVALID_PARAMETER  The requested LBA is out of range.
+
+**/
+EFI_STATUS
+EFIAPI
+FwVolBlockGetBlockSize (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *This,
+  IN CONST  EFI_LBA                             Lba,
+  IN OUT    UINTN                               *BlockSize,
+  IN OUT    UINTN                               *NumberOfBlocks
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.c b/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.c
new file mode 100644
index 0000000000..bd7c6c6493
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.c
@@ -0,0 +1,2606 @@
+/** @file
+  The file contains the GCD related services in the EFI Boot Services Table.
+  The GCD services are used to manage the memory and I/O regions that
+  are accessible to the CPU that is executing the DXE core.
+
+Copyright (c) 2006 - 2015, 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 "DxeMain.h"
+#include "Gcd.h"
+
+#define MINIMUM_INITIAL_MEMORY_SIZE 0x10000
+
+#define MEMORY_ATTRIBUTE_MASK         (EFI_RESOURCE_ATTRIBUTE_PRESENT             | \
+                                       EFI_RESOURCE_ATTRIBUTE_INITIALIZED         | \
+                                       EFI_RESOURCE_ATTRIBUTE_TESTED              | \
+                                       EFI_RESOURCE_ATTRIBUTE_READ_PROTECTED      | \
+                                       EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTED     | \
+                                       EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTED | \
+                                       EFI_RESOURCE_ATTRIBUTE_READ_ONLY_PROTECTED | \
+                                       EFI_RESOURCE_ATTRIBUTE_16_BIT_IO           | \
+                                       EFI_RESOURCE_ATTRIBUTE_32_BIT_IO           | \
+                                       EFI_RESOURCE_ATTRIBUTE_64_BIT_IO           | \
+                                       EFI_RESOURCE_ATTRIBUTE_PERSISTENT          )
+
+#define TESTED_MEMORY_ATTRIBUTES      (EFI_RESOURCE_ATTRIBUTE_PRESENT     | \
+                                       EFI_RESOURCE_ATTRIBUTE_INITIALIZED | \
+                                       EFI_RESOURCE_ATTRIBUTE_TESTED      )
+
+#define INITIALIZED_MEMORY_ATTRIBUTES (EFI_RESOURCE_ATTRIBUTE_PRESENT     | \
+                                       EFI_RESOURCE_ATTRIBUTE_INITIALIZED )
+
+#define PRESENT_MEMORY_ATTRIBUTES     (EFI_RESOURCE_ATTRIBUTE_PRESENT)
+
+#define INVALID_CPU_ARCH_ATTRIBUTES   0xffffffff
+
+//
+// Module Variables
+//
+EFI_LOCK           mGcdMemorySpaceLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+EFI_LOCK           mGcdIoSpaceLock     = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+LIST_ENTRY         mGcdMemorySpaceMap  = INITIALIZE_LIST_HEAD_VARIABLE (mGcdMemorySpaceMap);
+LIST_ENTRY         mGcdIoSpaceMap      = INITIALIZE_LIST_HEAD_VARIABLE (mGcdIoSpaceMap);
+
+EFI_GCD_MAP_ENTRY mGcdMemorySpaceMapEntryTemplate = {
+  EFI_GCD_MAP_SIGNATURE,
+  {
+    NULL,
+    NULL
+  },
+  0,
+  0,
+  0,
+  0,
+  EfiGcdMemoryTypeNonExistent,
+  (EFI_GCD_IO_TYPE) 0,
+  NULL,
+  NULL
+};
+
+EFI_GCD_MAP_ENTRY mGcdIoSpaceMapEntryTemplate = {
+  EFI_GCD_MAP_SIGNATURE,
+  {
+    NULL,
+    NULL
+  },
+  0,
+  0,
+  0,
+  0,
+  (EFI_GCD_MEMORY_TYPE) 0,
+  EfiGcdIoTypeNonExistent,
+  NULL,
+  NULL
+};
+
+GCD_ATTRIBUTE_CONVERSION_ENTRY mAttributeConversionTable[] = {
+  { EFI_RESOURCE_ATTRIBUTE_UNCACHEABLE,             EFI_MEMORY_UC,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_UNCACHED_EXPORTED,       EFI_MEMORY_UCE,             TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_WRITE_COMBINEABLE,       EFI_MEMORY_WC,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_WRITE_THROUGH_CACHEABLE, EFI_MEMORY_WT,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_WRITE_BACK_CACHEABLE,    EFI_MEMORY_WB,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_READ_PROTECTABLE,        EFI_MEMORY_RP,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTABLE,       EFI_MEMORY_WP,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTABLE,   EFI_MEMORY_XP,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_READ_ONLY_PROTECTABLE,   EFI_MEMORY_RO,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_PRESENT,                 EFI_MEMORY_PRESENT,         FALSE },
+  { EFI_RESOURCE_ATTRIBUTE_INITIALIZED,             EFI_MEMORY_INITIALIZED,     FALSE },
+  { EFI_RESOURCE_ATTRIBUTE_TESTED,                  EFI_MEMORY_TESTED,          FALSE },
+  { EFI_RESOURCE_ATTRIBUTE_PERSISTABLE,             EFI_MEMORY_NV,              TRUE  },
+  { EFI_RESOURCE_ATTRIBUTE_MORE_RELIABLE,           EFI_MEMORY_MORE_RELIABLE,   TRUE  },
+  { 0,                                              0,                          FALSE }
+};
+
+///
+/// Lookup table used to print GCD Memory Space Map
+///
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 *mGcdMemoryTypeNames[] = {
+  "NonExist ",  // EfiGcdMemoryTypeNonExistent
+  "Reserved ",  // EfiGcdMemoryTypeReserved
+  "SystemMem",  // EfiGcdMemoryTypeSystemMemory
+  "MMIO     ",  // EfiGcdMemoryTypeMemoryMappedIo
+  "PersisMem",  // EfiGcdMemoryTypePersistentMemory
+  "MoreRelia",  // EfiGcdMemoryTypeMoreReliable
+  "Unknown  "   // EfiGcdMemoryTypeMaximum
+};
+
+///
+/// Lookup table used to print GCD I/O Space Map
+///
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 *mGcdIoTypeNames[] = {
+  "NonExist",  // EfiGcdIoTypeNonExistent
+  "Reserved",  // EfiGcdIoTypeReserved
+  "I/O     ",  // EfiGcdIoTypeIo
+  "Unknown "   // EfiGcdIoTypeMaximum 
+};
+
+///
+/// Lookup table used to print GCD Allocation Types
+///
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 *mGcdAllocationTypeNames[] = {
+  "AnySearchBottomUp        ",  // EfiGcdAllocateAnySearchBottomUp
+  "MaxAddressSearchBottomUp ",  // EfiGcdAllocateMaxAddressSearchBottomUp
+  "AtAddress                ",  // EfiGcdAllocateAddress
+  "AnySearchTopDown         ",  // EfiGcdAllocateAnySearchTopDown
+  "MaxAddressSearchTopDown  ",  // EfiGcdAllocateMaxAddressSearchTopDown
+  "Unknown                  "   // EfiGcdMaxAllocateType
+};
+
+/**
+  Dump the entire contents if the GCD Memory Space Map using DEBUG() macros when
+  PcdDebugPrintErrorLevel has the DEBUG_GCD bit set.
+
+  @param  InitialMap  TRUE if the initial GCD Memory Map is being dumped.  Otherwise, FALSE.
+  
+**/
+VOID
+EFIAPI
+CoreDumpGcdMemorySpaceMap (
+  BOOLEAN  InitialMap
+  )
+{
+  DEBUG_CODE (
+    EFI_STATUS                       Status;
+    UINTN                            NumberOfDescriptors;
+    EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *MemorySpaceMap;
+    UINTN                            Index;
+   
+    Status = CoreGetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+    ASSERT (Status == EFI_SUCCESS && MemorySpaceMap != NULL);
+
+    if (InitialMap) {
+      DEBUG ((DEBUG_GCD, "GCD:Initial GCD Memory Space Map\n"));
+    }
+    DEBUG ((DEBUG_GCD, "GCDMemType Range                             Capabilities     Attributes      \n"));
+    DEBUG ((DEBUG_GCD, "========== ================================= ================ ================\n"));
+    for (Index = 0; Index < NumberOfDescriptors; Index++) {
+      DEBUG ((DEBUG_GCD, "%a  %016lx-%016lx %016lx %016lx%c\n", 
+        mGcdMemoryTypeNames[MIN (MemorySpaceMap[Index].GcdMemoryType, EfiGcdMemoryTypeMaximum)],
+        MemorySpaceMap[Index].BaseAddress, 
+        MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length - 1,
+        MemorySpaceMap[Index].Capabilities, 
+        MemorySpaceMap[Index].Attributes,
+        MemorySpaceMap[Index].ImageHandle == NULL ? ' ' : '*'
+        ));
+    }
+    DEBUG ((DEBUG_GCD, "\n"));
+    FreePool (MemorySpaceMap);
+  );
+}
+
+/**
+  Dump the entire contents if the GCD I/O Space Map using DEBUG() macros when 
+  PcdDebugPrintErrorLevel has the DEBUG_GCD bit set.
+
+  @param  InitialMap  TRUE if the initial GCD I/O Map is being dumped.  Otherwise, FALSE.
+  
+**/
+VOID
+EFIAPI
+CoreDumpGcdIoSpaceMap (
+  BOOLEAN  InitialMap
+  )
+{
+  DEBUG_CODE (
+    EFI_STATUS                   Status;
+    UINTN                        NumberOfDescriptors;
+    EFI_GCD_IO_SPACE_DESCRIPTOR  *IoSpaceMap;
+    UINTN                        Index;
+    
+    Status = CoreGetIoSpaceMap (&NumberOfDescriptors, &IoSpaceMap);
+    ASSERT (Status == EFI_SUCCESS && IoSpaceMap != NULL);
+    
+    if (InitialMap) {
+      DEBUG ((DEBUG_GCD, "GCD:Initial GCD I/O Space Map\n"));
+    }  
+    
+    DEBUG ((DEBUG_GCD, "GCDIoType  Range                            \n"));
+    DEBUG ((DEBUG_GCD, "========== =================================\n"));
+    for (Index = 0; Index < NumberOfDescriptors; Index++) {
+      DEBUG ((DEBUG_GCD, "%a   %016lx-%016lx%c\n", 
+        mGcdIoTypeNames[MIN (IoSpaceMap[Index].GcdIoType, EfiGcdIoTypeMaximum)],
+        IoSpaceMap[Index].BaseAddress, 
+        IoSpaceMap[Index].BaseAddress + IoSpaceMap[Index].Length - 1,
+        IoSpaceMap[Index].ImageHandle == NULL ? ' ' : '*'
+        ));
+    }
+    DEBUG ((DEBUG_GCD, "\n"));
+    FreePool (IoSpaceMap);
+  );
+}
+  
+/**
+  Validate resource descriptor HOB's attributes.
+
+  If Attributes includes some memory resource's settings, it should include 
+  the corresponding capabilites also.
+
+  @param  Attributes  Resource descriptor HOB attributes.
+
+**/
+VOID
+CoreValidateResourceDescriptorHobAttributes (
+  IN UINT64  Attributes
+  )
+{
+  ASSERT (((Attributes & EFI_RESOURCE_ATTRIBUTE_READ_PROTECTED) == 0) ||
+          ((Attributes & EFI_RESOURCE_ATTRIBUTE_READ_PROTECTABLE) != 0));
+  ASSERT (((Attributes & EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTED) == 0) ||
+          ((Attributes & EFI_RESOURCE_ATTRIBUTE_WRITE_PROTECTABLE) != 0));
+  ASSERT (((Attributes & EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTED) == 0) ||
+          ((Attributes & EFI_RESOURCE_ATTRIBUTE_EXECUTION_PROTECTABLE) != 0));
+  ASSERT (((Attributes & EFI_RESOURCE_ATTRIBUTE_READ_ONLY_PROTECTED) == 0) ||
+          ((Attributes & EFI_RESOURCE_ATTRIBUTE_READ_ONLY_PROTECTABLE) != 0));
+  ASSERT (((Attributes & EFI_RESOURCE_ATTRIBUTE_PERSISTENT) == 0) ||
+          ((Attributes & EFI_RESOURCE_ATTRIBUTE_PERSISTABLE) != 0));
+}
+
+/**
+  Acquire memory lock on mGcdMemorySpaceLock.
+
+**/
+VOID
+CoreAcquireGcdMemoryLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&mGcdMemorySpaceLock);
+}
+
+
+
+/**
+  Release memory lock on mGcdMemorySpaceLock.
+
+**/
+VOID
+CoreReleaseGcdMemoryLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&mGcdMemorySpaceLock);
+}
+
+
+
+/**
+  Acquire memory lock on mGcdIoSpaceLock.
+
+**/
+VOID
+CoreAcquireGcdIoLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&mGcdIoSpaceLock);
+}
+
+
+/**
+  Release memory lock on mGcdIoSpaceLock.
+
+**/
+VOID
+CoreReleaseGcdIoLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&mGcdIoSpaceLock);
+}
+
+
+
+//
+// GCD Initialization Worker Functions
+//
+/**
+  Aligns a value to the specified boundary.
+
+  @param  Value                  64 bit value to align
+  @param  Alignment              Log base 2 of the boundary to align Value to
+  @param  RoundUp                TRUE if Value is to be rounded up to the nearest
+                                 aligned boundary.  FALSE is Value is to be
+                                 rounded down to the nearest aligned boundary.
+
+  @return A 64 bit value is the aligned to the value nearest Value with an alignment by Alignment.
+
+**/
+UINT64
+AlignValue (
+  IN UINT64   Value,
+  IN UINTN    Alignment,
+  IN BOOLEAN  RoundUp
+  )
+{
+  UINT64  AlignmentMask;
+
+  AlignmentMask = LShiftU64 (1, Alignment) - 1;
+  if (RoundUp) {
+    Value += AlignmentMask;
+  }
+  return Value & (~AlignmentMask);
+}
+
+
+/**
+  Aligns address to the page boundary.
+
+  @param  Value                  64 bit address to align
+
+  @return A 64 bit value is the aligned to the value nearest Value with an alignment by Alignment.
+
+**/
+UINT64
+PageAlignAddress (
+  IN UINT64 Value
+  )
+{
+  return AlignValue (Value, EFI_PAGE_SHIFT, TRUE);
+}
+
+
+/**
+  Aligns length to the page boundary.
+
+  @param  Value                  64 bit length to align
+
+  @return A 64 bit value is the aligned to the value nearest Value with an alignment by Alignment.
+
+**/
+UINT64
+PageAlignLength (
+  IN UINT64 Value
+  )
+{
+  return AlignValue (Value, EFI_PAGE_SHIFT, FALSE);
+}
+
+//
+// GCD Memory Space Worker Functions
+//
+
+/**
+  Allocate pool for two entries.
+
+  @param  TopEntry               An entry of GCD map
+  @param  BottomEntry            An entry of GCD map
+
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to be allocated.
+  @retval EFI_SUCCESS            Both entries successfully allocated.
+
+**/
+EFI_STATUS
+CoreAllocateGcdMapEntry (
+  IN OUT EFI_GCD_MAP_ENTRY  **TopEntry,
+  IN OUT EFI_GCD_MAP_ENTRY  **BottomEntry
+  )
+{
+  *TopEntry = AllocateZeroPool (sizeof (EFI_GCD_MAP_ENTRY));
+  if (*TopEntry == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  *BottomEntry = AllocateZeroPool (sizeof (EFI_GCD_MAP_ENTRY));
+  if (*BottomEntry == NULL) {
+    CoreFreePool (*TopEntry);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Internal function.  Inserts a new descriptor into a sorted list
+
+  @param  Link                   The linked list to insert the range BaseAddress
+                                 and Length into
+  @param  Entry                  A pointer to the entry that is inserted
+  @param  BaseAddress            The base address of the new range
+  @param  Length                 The length of the new range in bytes
+  @param  TopEntry               Top pad entry to insert if needed.
+  @param  BottomEntry            Bottom pad entry to insert if needed.
+
+  @retval EFI_SUCCESS            The new range was inserted into the linked list
+
+**/
+EFI_STATUS
+CoreInsertGcdMapEntry (
+  IN LIST_ENTRY           *Link,
+  IN EFI_GCD_MAP_ENTRY     *Entry,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN EFI_GCD_MAP_ENTRY     *TopEntry,
+  IN EFI_GCD_MAP_ENTRY     *BottomEntry
+  )
+{
+  ASSERT (Length != 0);
+
+  if (BaseAddress > Entry->BaseAddress) {
+    ASSERT (BottomEntry->Signature == 0);
+
+    CopyMem (BottomEntry, Entry, sizeof (EFI_GCD_MAP_ENTRY));
+    Entry->BaseAddress      = BaseAddress;
+    BottomEntry->EndAddress = BaseAddress - 1;
+    InsertTailList (Link, &BottomEntry->Link);
+  }
+
+  if ((BaseAddress + Length - 1) < Entry->EndAddress) {
+    ASSERT (TopEntry->Signature == 0);
+
+    CopyMem (TopEntry, Entry, sizeof (EFI_GCD_MAP_ENTRY));
+    TopEntry->BaseAddress = BaseAddress + Length;
+    Entry->EndAddress     = BaseAddress + Length - 1;
+    InsertHeadList (Link, &TopEntry->Link);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Merge the Gcd region specified by Link and its adjacent entry.
+
+  @param  Link                   Specify the entry to be merged (with its
+                                 adjacent entry).
+  @param  Forward                Direction (forward or backward).
+  @param  Map                    Boundary.
+
+  @retval EFI_SUCCESS            Successfully returned.
+  @retval EFI_UNSUPPORTED        These adjacent regions could not merge.
+
+**/
+EFI_STATUS
+CoreMergeGcdMapEntry (
+  IN LIST_ENTRY      *Link,
+  IN BOOLEAN         Forward,
+  IN LIST_ENTRY      *Map
+  )
+{
+  LIST_ENTRY         *AdjacentLink;
+  EFI_GCD_MAP_ENTRY  *Entry;
+  EFI_GCD_MAP_ENTRY  *AdjacentEntry;
+
+  //
+  // Get adjacent entry
+  //
+  if (Forward) {
+    AdjacentLink = Link->ForwardLink;
+  } else {
+    AdjacentLink = Link->BackLink;
+  }
+
+  //
+  // If AdjacentLink is the head of the list, then no merge can be performed
+  //
+  if (AdjacentLink == Map) {
+    return EFI_SUCCESS;
+  }
+
+  Entry         = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+  AdjacentEntry = CR (AdjacentLink, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+
+  if (Entry->Capabilities != AdjacentEntry->Capabilities) {
+    return EFI_UNSUPPORTED;
+  }
+  if (Entry->Attributes != AdjacentEntry->Attributes) {
+    return EFI_UNSUPPORTED;
+  }
+  if (Entry->GcdMemoryType != AdjacentEntry->GcdMemoryType) {
+    return EFI_UNSUPPORTED;
+  }
+  if (Entry->GcdIoType != AdjacentEntry->GcdIoType) {
+    return EFI_UNSUPPORTED;
+  }
+  if (Entry->ImageHandle != AdjacentEntry->ImageHandle) {
+    return EFI_UNSUPPORTED;
+  }
+  if (Entry->DeviceHandle != AdjacentEntry->DeviceHandle) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Forward) {
+    Entry->EndAddress  = AdjacentEntry->EndAddress;
+  } else {
+    Entry->BaseAddress = AdjacentEntry->BaseAddress;
+  }
+  RemoveEntryList (AdjacentLink);
+  CoreFreePool (AdjacentEntry);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Merge adjacent entries on total chain.
+
+  @param  TopEntry               Top entry of GCD map.
+  @param  BottomEntry            Bottom entry of GCD map.
+  @param  StartLink              Start link of the list for this loop.
+  @param  EndLink                End link of the list for this loop.
+  @param  Map                    Boundary.
+
+  @retval EFI_SUCCESS            GCD map successfully cleaned up.
+
+**/
+EFI_STATUS
+CoreCleanupGcdMapEntry (
+  IN EFI_GCD_MAP_ENTRY  *TopEntry,
+  IN EFI_GCD_MAP_ENTRY  *BottomEntry,
+  IN LIST_ENTRY         *StartLink,
+  IN LIST_ENTRY         *EndLink,
+  IN LIST_ENTRY         *Map
+  )
+{
+  LIST_ENTRY  *Link;
+
+  if (TopEntry->Signature == 0) {
+    CoreFreePool (TopEntry);
+  }
+  if (BottomEntry->Signature == 0) {
+    CoreFreePool (BottomEntry);
+  }
+
+  Link = StartLink;
+  while (Link != EndLink->ForwardLink) {
+    CoreMergeGcdMapEntry (Link, FALSE, Map);
+    Link = Link->ForwardLink;
+  }
+  CoreMergeGcdMapEntry (EndLink, TRUE, Map);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Search a segment of memory space in GCD map. The result is a range of GCD entry list.
+
+  @param  BaseAddress            The start address of the segment.
+  @param  Length                 The length of the segment.
+  @param  StartLink              The first GCD entry involves this segment of
+                                 memory space.
+  @param  EndLink                The first GCD entry involves this segment of
+                                 memory space.
+  @param  Map                    Points to the start entry to search.
+
+  @retval EFI_SUCCESS            Successfully found the entry.
+  @retval EFI_NOT_FOUND          Not found.
+
+**/
+EFI_STATUS
+CoreSearchGcdMapEntry (
+  IN  EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN  UINT64                Length,
+  OUT LIST_ENTRY            **StartLink,
+  OUT LIST_ENTRY            **EndLink,
+  IN  LIST_ENTRY            *Map
+  )
+{
+  LIST_ENTRY         *Link;
+  EFI_GCD_MAP_ENTRY  *Entry;
+
+  ASSERT (Length != 0);
+
+  *StartLink = NULL;
+  *EndLink   = NULL;
+
+  Link = Map->ForwardLink;
+  while (Link != Map) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    if (BaseAddress >= Entry->BaseAddress && BaseAddress <= Entry->EndAddress) {
+      *StartLink = Link;
+    }
+    if (*StartLink != NULL) {
+      if ((BaseAddress + Length - 1) >= Entry->BaseAddress &&
+          (BaseAddress + Length - 1) <= Entry->EndAddress     ) {
+        *EndLink = Link;
+        return EFI_SUCCESS;
+      }
+    }
+    Link = Link->ForwardLink;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Count the amount of GCD map entries.
+
+  @param  Map                    Points to the start entry to do the count loop.
+
+  @return The count.
+
+**/
+UINTN
+CoreCountGcdMapEntry (
+  IN LIST_ENTRY  *Map
+  )
+{
+  UINTN           Count;
+  LIST_ENTRY      *Link;
+
+  Count = 0;
+  Link = Map->ForwardLink;
+  while (Link != Map) {
+    Count++;
+    Link = Link->ForwardLink;
+  }
+
+  return Count;
+}
+
+
+
+/**
+  Return the memory attribute specified by Attributes
+
+  @param  Attributes             A num with some attribute bits on.
+
+  @return The enum value of memory attribute.
+
+**/
+UINT64
+ConverToCpuArchAttributes (
+  UINT64 Attributes
+  )
+{
+  if ( (Attributes & EFI_MEMORY_UC) == EFI_MEMORY_UC) {
+    return EFI_MEMORY_UC;
+  }
+
+  if ( (Attributes & EFI_MEMORY_WC ) == EFI_MEMORY_WC) {
+    return EFI_MEMORY_WC;
+  }
+
+  if ( (Attributes & EFI_MEMORY_WT ) == EFI_MEMORY_WT) {
+    return EFI_MEMORY_WT;
+  }
+
+  if ( (Attributes & EFI_MEMORY_WB) == EFI_MEMORY_WB) {
+    return EFI_MEMORY_WB;
+  }
+
+  if ( (Attributes & EFI_MEMORY_WP) == EFI_MEMORY_WP) {
+    return EFI_MEMORY_WP;
+  }
+
+  return INVALID_CPU_ARCH_ATTRIBUTES;
+
+}
+
+
+/**
+  Do operation on a segment of memory space specified (add, free, remove, change attribute ...).
+
+  @param  Operation              The type of the operation
+  @param  GcdMemoryType          Additional information for the operation
+  @param  GcdIoType              Additional information for the operation
+  @param  BaseAddress            Start address of the segment
+  @param  Length                 length of the segment
+  @param  Capabilities           The alterable attributes of a newly added entry
+  @param  Attributes             The attributes needs to be set
+
+  @retval EFI_INVALID_PARAMETER  Length is 0 or address (length) not aligned when
+                                 setting attribute.
+  @retval EFI_SUCCESS            Action successfully done.
+  @retval EFI_UNSUPPORTED        Could not find the proper descriptor on this
+                                 segment or  set an upsupported attribute.
+  @retval EFI_ACCESS_DENIED      Operate on an space non-exist or is used for an
+                                 image.
+  @retval EFI_NOT_FOUND          Free a non-using space or remove a non-exist
+                                 space, and so on.
+  @retval EFI_OUT_OF_RESOURCES   No buffer could be allocated.
+  @retval EFI_NOT_AVAILABLE_YET  The attributes cannot be set because CPU architectural protocol
+                                 is not available yet.
+**/
+EFI_STATUS
+CoreConvertSpace (
+  IN UINTN                 Operation,
+  IN EFI_GCD_MEMORY_TYPE   GcdMemoryType,
+  IN EFI_GCD_IO_TYPE       GcdIoType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities,
+  IN UINT64                Attributes
+  )
+{
+  EFI_STATUS         Status;
+  LIST_ENTRY         *Map;
+  LIST_ENTRY         *Link;
+  EFI_GCD_MAP_ENTRY  *Entry;
+  EFI_GCD_MAP_ENTRY  *TopEntry;
+  EFI_GCD_MAP_ENTRY  *BottomEntry;
+  LIST_ENTRY         *StartLink;
+  LIST_ENTRY         *EndLink;
+  UINT64             CpuArchAttributes;
+
+  if (Length == 0) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Map = NULL;
+  if ((Operation & GCD_MEMORY_SPACE_OPERATION) != 0) {
+    CoreAcquireGcdMemoryLock ();
+    Map = &mGcdMemorySpaceMap;
+  } else if ((Operation & GCD_IO_SPACE_OPERATION) != 0) {
+    CoreAcquireGcdIoLock ();
+    Map = &mGcdIoSpaceMap;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  //
+  // Search for the list of descriptors that cover the range BaseAddress to BaseAddress+Length
+  //
+  Status = CoreSearchGcdMapEntry (BaseAddress, Length, &StartLink, &EndLink, Map);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+
+    goto Done;
+  }
+  ASSERT (StartLink != NULL && EndLink != NULL);
+
+  //
+  // Verify that the list of descriptors are unallocated non-existent memory.
+  //
+  Link = StartLink;
+  while (Link != EndLink->ForwardLink) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    switch (Operation) {
+    //
+    // Add operations
+    //
+    case GCD_ADD_MEMORY_OPERATION:
+      if (Entry->GcdMemoryType != EfiGcdMemoryTypeNonExistent ||
+          Entry->ImageHandle   != NULL                           ) {
+        Status = EFI_ACCESS_DENIED;
+        goto Done;
+      }
+      break;
+    case GCD_ADD_IO_OPERATION:
+      if (Entry->GcdIoType   != EfiGcdIoTypeNonExistent ||
+          Entry->ImageHandle != NULL                       ) {
+        Status = EFI_ACCESS_DENIED;
+        goto Done;
+      }
+      break;
+    //
+    // Free operations
+    //
+    case GCD_FREE_MEMORY_OPERATION:
+    case GCD_FREE_IO_OPERATION:
+      if (Entry->ImageHandle == NULL) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+      break;
+    //
+    // Remove operations
+    //
+    case GCD_REMOVE_MEMORY_OPERATION:
+      if (Entry->GcdMemoryType == EfiGcdMemoryTypeNonExistent) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+      if (Entry->ImageHandle != NULL) {
+        Status = EFI_ACCESS_DENIED;
+        goto Done;
+      }
+      break;
+    case GCD_REMOVE_IO_OPERATION:
+      if (Entry->GcdIoType == EfiGcdIoTypeNonExistent) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+      if (Entry->ImageHandle != NULL) {
+        Status = EFI_ACCESS_DENIED;
+        goto Done;
+      }
+      break;
+    //
+    // Set attributes operation
+    //
+    case GCD_SET_ATTRIBUTES_MEMORY_OPERATION:
+      if ((Attributes & EFI_MEMORY_RUNTIME) != 0) {
+        if ((BaseAddress & EFI_PAGE_MASK) != 0 || (Length & EFI_PAGE_MASK) != 0) {
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+      }
+      if ((Entry->Capabilities & Attributes) != Attributes) {
+        Status = EFI_UNSUPPORTED;
+        goto Done;
+      }
+      break;
+    //
+    // Set capabilities operation
+    //
+    case GCD_SET_CAPABILITIES_MEMORY_OPERATION:
+      if ((BaseAddress & EFI_PAGE_MASK) != 0 || (Length & EFI_PAGE_MASK) != 0) {
+        Status = EFI_INVALID_PARAMETER;
+
+        goto Done;
+      }
+      //
+      // Current attributes must still be supported with new capabilities
+      //
+      if ((Capabilities & Entry->Attributes) != Entry->Attributes) {
+        Status = EFI_UNSUPPORTED;
+        goto Done;
+      }
+      break;
+    }
+    Link = Link->ForwardLink;
+  }
+
+  //
+  // Allocate work space to perform this operation
+  //
+  Status = CoreAllocateGcdMapEntry (&TopEntry, &BottomEntry);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  ASSERT (TopEntry != NULL && BottomEntry != NULL);
+
+  if (Operation == GCD_SET_ATTRIBUTES_MEMORY_OPERATION) {
+    //
+    // Call CPU Arch Protocol to attempt to set attributes on the range
+    //
+    CpuArchAttributes = ConverToCpuArchAttributes (Attributes);
+    if (CpuArchAttributes != INVALID_CPU_ARCH_ATTRIBUTES) {
+      if (gCpu == NULL) {
+        Status = EFI_NOT_AVAILABLE_YET;
+      } else {
+        Status = gCpu->SetMemoryAttributes (
+                         gCpu,
+                         BaseAddress,
+                         Length,
+                         CpuArchAttributes
+                         );
+      }
+      if (EFI_ERROR (Status)) {
+        CoreFreePool (TopEntry);
+        CoreFreePool (BottomEntry);
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Convert/Insert the list of descriptors from StartLink to EndLink
+  //
+  Link = StartLink;
+  while (Link != EndLink->ForwardLink) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    CoreInsertGcdMapEntry (Link, Entry, BaseAddress, Length, TopEntry, BottomEntry);
+    switch (Operation) {
+    //
+    // Add operations
+    //
+    case GCD_ADD_MEMORY_OPERATION:
+      Entry->GcdMemoryType = GcdMemoryType;
+      if (GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) {
+        Entry->Capabilities  = Capabilities | EFI_MEMORY_RUNTIME | EFI_MEMORY_PORT_IO;
+      } else {
+        Entry->Capabilities  = Capabilities | EFI_MEMORY_RUNTIME;
+      }
+      break;
+    case GCD_ADD_IO_OPERATION:
+      Entry->GcdIoType = GcdIoType;
+      break;
+    //
+    // Free operations
+    //
+    case GCD_FREE_MEMORY_OPERATION:
+    case GCD_FREE_IO_OPERATION:
+      Entry->ImageHandle  = NULL;
+      Entry->DeviceHandle = NULL;
+      break;
+    //
+    // Remove operations
+    //
+    case GCD_REMOVE_MEMORY_OPERATION:
+      Entry->GcdMemoryType = EfiGcdMemoryTypeNonExistent;
+      Entry->Capabilities  = 0;
+      break;
+    case GCD_REMOVE_IO_OPERATION:
+      Entry->GcdIoType = EfiGcdIoTypeNonExistent;
+      break;
+    //
+    // Set attributes operation
+    //
+    case GCD_SET_ATTRIBUTES_MEMORY_OPERATION:
+      Entry->Attributes = Attributes;
+      break;
+    //
+    // Set capabilities operation
+    //
+    case GCD_SET_CAPABILITIES_MEMORY_OPERATION:
+      Entry->Capabilities = Capabilities;
+      break;
+    }
+    Link = Link->ForwardLink;
+  }
+
+  //
+  // Cleanup
+  //
+  Status = CoreCleanupGcdMapEntry (TopEntry, BottomEntry, StartLink, EndLink, Map);
+
+Done:
+  DEBUG ((DEBUG_GCD, "  Status = %r\n", Status));
+
+  if ((Operation & GCD_MEMORY_SPACE_OPERATION) != 0) {
+    CoreReleaseGcdMemoryLock ();
+    CoreDumpGcdMemorySpaceMap (FALSE);
+  }
+  if ((Operation & GCD_IO_SPACE_OPERATION) != 0) {
+    CoreReleaseGcdIoLock ();
+    CoreDumpGcdIoSpaceMap (FALSE);
+  }
+
+  return Status;
+}
+
+
+/**
+  Check whether an entry could be used to allocate space.
+
+  @param  Operation              Allocate memory or IO
+  @param  Entry                  The entry to be tested
+  @param  GcdMemoryType          The desired memory type
+  @param  GcdIoType              The desired IO type
+
+  @retval EFI_NOT_FOUND          The memory type does not match or there's an
+                                 image handle on the entry.
+  @retval EFI_UNSUPPORTED        The operation unsupported.
+  @retval EFI_SUCCESS            It's ok for this entry to be used to allocate
+                                 space.
+
+**/
+EFI_STATUS
+CoreAllocateSpaceCheckEntry (
+  IN UINTN                Operation,
+  IN EFI_GCD_MAP_ENTRY    *Entry,
+  IN EFI_GCD_MEMORY_TYPE  GcdMemoryType,
+  IN EFI_GCD_IO_TYPE      GcdIoType
+  )
+{
+  if (Entry->ImageHandle != NULL) {
+    return EFI_NOT_FOUND;
+  }
+  switch (Operation) {
+  case GCD_ALLOCATE_MEMORY_OPERATION:
+    if (Entry->GcdMemoryType != GcdMemoryType) {
+      return EFI_NOT_FOUND;
+    }
+    break;
+  case GCD_ALLOCATE_IO_OPERATION:
+    if (Entry->GcdIoType != GcdIoType) {
+      return EFI_NOT_FOUND;
+    }
+    break;
+  default:
+    return EFI_UNSUPPORTED;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate space on specified address and length.
+
+  @param  Operation              The type of operation (memory or IO)
+  @param  GcdAllocateType        The type of allocate operation
+  @param  GcdMemoryType          The desired memory type
+  @param  GcdIoType              The desired IO type
+  @param  Alignment              Align with 2^Alignment
+  @param  Length                 Length to allocate
+  @param  BaseAddress            Base address to allocate
+  @param  ImageHandle            The image handle consume the allocated space.
+  @param  DeviceHandle           The device handle consume the allocated space.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_NOT_FOUND          No descriptor for the desired space exists.
+  @retval EFI_SUCCESS            Space successfully allocated.
+
+**/
+EFI_STATUS
+CoreAllocateSpace (
+  IN     UINTN                  Operation,
+  IN     EFI_GCD_ALLOCATE_TYPE  GcdAllocateType,
+  IN     EFI_GCD_MEMORY_TYPE    GcdMemoryType,
+  IN     EFI_GCD_IO_TYPE        GcdIoType,
+  IN     UINTN                  Alignment,
+  IN     UINT64                 Length,
+  IN OUT EFI_PHYSICAL_ADDRESS   *BaseAddress,
+  IN     EFI_HANDLE             ImageHandle,
+  IN     EFI_HANDLE             DeviceHandle OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  AlignmentMask;
+  EFI_PHYSICAL_ADDRESS  MaxAddress;
+  LIST_ENTRY            *Map;
+  LIST_ENTRY            *Link;
+  LIST_ENTRY            *SubLink;
+  EFI_GCD_MAP_ENTRY     *Entry;
+  EFI_GCD_MAP_ENTRY     *TopEntry;
+  EFI_GCD_MAP_ENTRY     *BottomEntry;
+  LIST_ENTRY            *StartLink;
+  LIST_ENTRY            *EndLink;
+  BOOLEAN               Found;
+
+  //
+  // Make sure parameters are valid
+  //
+  if ((UINT32)GcdAllocateType >= EfiGcdMaxAllocateType) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((UINT32)GcdMemoryType >= EfiGcdMemoryTypeMaximum) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((UINT32)GcdIoType >= EfiGcdIoTypeMaximum) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  if (BaseAddress == NULL) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  if (ImageHandle == NULL) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Alignment >= 64) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_NOT_FOUND));
+    return EFI_NOT_FOUND;
+  }
+  if (Length == 0) {
+    DEBUG ((DEBUG_GCD, "  Status = %r\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Map = NULL;
+  if ((Operation & GCD_MEMORY_SPACE_OPERATION) != 0) {
+    CoreAcquireGcdMemoryLock ();
+    Map = &mGcdMemorySpaceMap;
+  } else if ((Operation & GCD_IO_SPACE_OPERATION) != 0) {
+    CoreAcquireGcdIoLock ();
+    Map = &mGcdIoSpaceMap;
+  } else {
+    ASSERT (FALSE);
+  }
+
+  Found     = FALSE;
+  StartLink = NULL;
+  EndLink   = NULL;
+  //
+  // Compute alignment bit mask
+  //
+  AlignmentMask = LShiftU64 (1, Alignment) - 1;
+
+  if (GcdAllocateType == EfiGcdAllocateAddress) {
+    //
+    // Verify that the BaseAddress passed in is aligned correctly
+    //
+    if ((*BaseAddress & AlignmentMask) != 0) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+
+    //
+    // Search for the list of descriptors that cover the range BaseAddress to BaseAddress+Length
+    //
+    Status = CoreSearchGcdMapEntry (*BaseAddress, Length, &StartLink, &EndLink, Map);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+    ASSERT (StartLink != NULL && EndLink != NULL);
+
+    //
+    // Verify that the list of descriptors are unallocated memory matching GcdMemoryType.
+    //
+    Link = StartLink;
+    while (Link != EndLink->ForwardLink) {
+      Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+      Link = Link->ForwardLink;
+      Status = CoreAllocateSpaceCheckEntry (Operation, Entry, GcdMemoryType, GcdIoType);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+    Found = TRUE;
+  } else {
+
+    Entry = CR (Map->BackLink, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+
+    //
+    // Compute the maximum address to use in the search algorithm
+    //
+    if (GcdAllocateType == EfiGcdAllocateMaxAddressSearchBottomUp ||
+        GcdAllocateType == EfiGcdAllocateMaxAddressSearchTopDown     ) {
+      MaxAddress = *BaseAddress;
+    } else {
+      MaxAddress = Entry->EndAddress;
+    }
+
+    //
+    // Verify that the list of descriptors are unallocated memory matching GcdMemoryType.
+    //
+    if (GcdAllocateType == EfiGcdAllocateMaxAddressSearchTopDown ||
+        GcdAllocateType == EfiGcdAllocateAnySearchTopDown ) {
+      Link = Map->BackLink;
+    } else {
+      Link = Map->ForwardLink;
+    }
+    while (Link != Map) {
+      Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+
+      if (GcdAllocateType == EfiGcdAllocateMaxAddressSearchTopDown ||
+          GcdAllocateType == EfiGcdAllocateAnySearchTopDown           ) {
+        Link = Link->BackLink;
+      } else {
+        Link = Link->ForwardLink;
+      }
+
+      Status = CoreAllocateSpaceCheckEntry (Operation, Entry, GcdMemoryType, GcdIoType);
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      if (GcdAllocateType == EfiGcdAllocateMaxAddressSearchTopDown ||
+          GcdAllocateType == EfiGcdAllocateAnySearchTopDown) {
+        if ((Entry->BaseAddress + Length) > MaxAddress) {
+          continue;
+        }
+        if (Length > (Entry->EndAddress + 1)) {
+          Status = EFI_NOT_FOUND;
+          goto Done;
+        }
+        if (Entry->EndAddress > MaxAddress) {
+          *BaseAddress = MaxAddress;
+        } else {
+          *BaseAddress = Entry->EndAddress;
+        }
+        *BaseAddress = (*BaseAddress + 1 - Length) & (~AlignmentMask);
+      } else {
+        *BaseAddress = (Entry->BaseAddress + AlignmentMask) & (~AlignmentMask);
+        if ((*BaseAddress + Length - 1) > MaxAddress) {
+          Status = EFI_NOT_FOUND;
+          goto Done;
+        }
+      }
+
+      //
+      // Search for the list of descriptors that cover the range BaseAddress to BaseAddress+Length
+      //
+      Status = CoreSearchGcdMapEntry (*BaseAddress, Length, &StartLink, &EndLink, Map);
+      if (EFI_ERROR (Status)) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+      ASSERT (StartLink != NULL && EndLink != NULL);
+
+      Link = StartLink;
+      //
+      // Verify that the list of descriptors are unallocated memory matching GcdMemoryType.
+      //
+      Found = TRUE;
+      SubLink = StartLink;
+      while (SubLink != EndLink->ForwardLink) {
+        Entry = CR (SubLink, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+        Status = CoreAllocateSpaceCheckEntry (Operation, Entry, GcdMemoryType, GcdIoType);
+        if (EFI_ERROR (Status)) {
+          Link = SubLink;
+          Found = FALSE;
+          break;
+        }
+        SubLink = SubLink->ForwardLink;
+      }
+      if (Found) {
+        break;
+      }
+    }
+  }
+  if (!Found) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  //
+  // Allocate work space to perform this operation
+  //
+  Status = CoreAllocateGcdMapEntry (&TopEntry, &BottomEntry);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  ASSERT (TopEntry != NULL && BottomEntry != NULL);
+
+  //
+  // Convert/Insert the list of descriptors from StartLink to EndLink
+  //
+  Link = StartLink;
+  while (Link != EndLink->ForwardLink) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    CoreInsertGcdMapEntry (Link, Entry, *BaseAddress, Length, TopEntry, BottomEntry);
+    Entry->ImageHandle  = ImageHandle;
+    Entry->DeviceHandle = DeviceHandle;
+    Link = Link->ForwardLink;
+  }
+
+  //
+  // Cleanup
+  //
+  Status = CoreCleanupGcdMapEntry (TopEntry, BottomEntry, StartLink, EndLink, Map);
+
+Done:
+  DEBUG ((DEBUG_GCD, "  Status = %r", Status));
+  if (!EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_GCD, "  (BaseAddress = %016lx)", *BaseAddress));
+  }
+  DEBUG ((DEBUG_GCD, "\n"));
+  
+  if ((Operation & GCD_MEMORY_SPACE_OPERATION) != 0) {
+    CoreReleaseGcdMemoryLock ();
+    CoreDumpGcdMemorySpaceMap (FALSE);
+  }
+  if ((Operation & GCD_IO_SPACE_OPERATION) !=0) {
+    CoreReleaseGcdIoLock ();
+    CoreDumpGcdIoSpaceMap (FALSE);
+  }
+
+  return Status;
+}
+
+
+/**
+  Add a segment of memory to GCD map.
+
+  @param  GcdMemoryType          Memory type of the segment.
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+  @param  Capabilities           alterable attributes of the segment.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameters.
+  @retval EFI_SUCCESS            Successfully add a segment of memory space.
+
+**/
+EFI_STATUS
+CoreInternalAddMemorySpace (
+  IN EFI_GCD_MEMORY_TYPE   GcdMemoryType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:AddMemorySpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  GcdMemoryType   = %a\n", mGcdMemoryTypeNames[MIN (GcdMemoryType, EfiGcdMemoryTypeMaximum)]));
+  DEBUG ((DEBUG_GCD, "  Capabilities    = %016lx\n", Capabilities));
+
+  //
+  // Make sure parameters are valid
+  //
+  if (GcdMemoryType <= EfiGcdMemoryTypeNonExistent || GcdMemoryType >= EfiGcdMemoryTypeMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return CoreConvertSpace (GCD_ADD_MEMORY_OPERATION, GcdMemoryType, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, Capabilities, 0);
+}
+
+//
+// GCD Core Services
+//
+
+/**
+  Allocates nonexistent memory, reserved memory, system memory, or memorymapped
+  I/O resources from the global coherency domain of the processor.
+
+  @param  GcdAllocateType        The type of allocate operation
+  @param  GcdMemoryType          The desired memory type
+  @param  Alignment              Align with 2^Alignment
+  @param  Length                 Length to allocate
+  @param  BaseAddress            Base address to allocate
+  @param  ImageHandle            The image handle consume the allocated space.
+  @param  DeviceHandle           The device handle consume the allocated space.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_NOT_FOUND          No descriptor contains the desired space.
+  @retval EFI_SUCCESS            Memory space successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocateMemorySpace (
+  IN     EFI_GCD_ALLOCATE_TYPE  GcdAllocateType,
+  IN     EFI_GCD_MEMORY_TYPE    GcdMemoryType,
+  IN     UINTN                  Alignment,
+  IN     UINT64                 Length,
+  IN OUT EFI_PHYSICAL_ADDRESS   *BaseAddress,
+  IN     EFI_HANDLE             ImageHandle,
+  IN     EFI_HANDLE             DeviceHandle OPTIONAL
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:AllocateMemorySpace(Base=%016lx,Length=%016lx)\n", *BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  GcdAllocateType = %a\n", mGcdAllocationTypeNames[MIN (GcdAllocateType, EfiGcdMaxAllocateType)]));
+  DEBUG ((DEBUG_GCD, "  GcdMemoryType   = %a\n", mGcdMemoryTypeNames[MIN (GcdMemoryType, EfiGcdMemoryTypeMaximum)]));
+  DEBUG ((DEBUG_GCD, "  Alignment       = %016lx\n", LShiftU64 (1, Alignment)));
+  DEBUG ((DEBUG_GCD, "  ImageHandle     = %p\n", ImageHandle));
+  DEBUG ((DEBUG_GCD, "  DeviceHandle    = %p\n", DeviceHandle));
+  
+  return CoreAllocateSpace (
+           GCD_ALLOCATE_MEMORY_OPERATION,
+           GcdAllocateType,
+           GcdMemoryType,
+           (EFI_GCD_IO_TYPE) 0,
+           Alignment,
+           Length,
+           BaseAddress,
+           ImageHandle,
+           DeviceHandle
+           );
+}
+
+
+/**
+  Adds reserved memory, system memory, or memory-mapped I/O resources to the
+  global coherency domain of the processor.
+
+  @param  GcdMemoryType          Memory type of the memory space.
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+  @param  Capabilities           alterable attributes of the memory space.
+
+  @retval EFI_SUCCESS            Merged this memory space into GCD map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAddMemorySpace (
+  IN EFI_GCD_MEMORY_TYPE   GcdMemoryType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  PageBaseAddress;
+  UINT64                PageLength;
+
+  Status = CoreInternalAddMemorySpace (GcdMemoryType, BaseAddress, Length, Capabilities);
+
+  if (!EFI_ERROR (Status) && ((GcdMemoryType == EfiGcdMemoryTypeSystemMemory) || (GcdMemoryType == EfiGcdMemoryTypeMoreReliable))) {
+
+    PageBaseAddress = PageAlignAddress (BaseAddress);
+    PageLength      = PageAlignLength (BaseAddress + Length - PageBaseAddress);
+
+    Status = CoreAllocateMemorySpace (
+               EfiGcdAllocateAddress,
+               GcdMemoryType,
+               EFI_PAGE_SHIFT,
+               PageLength,
+               &PageBaseAddress,
+               gDxeCoreImageHandle,
+               NULL
+               );
+
+    if (!EFI_ERROR (Status)) {
+      CoreAddMemoryDescriptor (
+        EfiConventionalMemory,
+        PageBaseAddress,
+        RShiftU64 (PageLength, EFI_PAGE_SHIFT),
+        Capabilities
+        );
+    } else {
+      for (; PageLength != 0; PageLength -= EFI_PAGE_SIZE, PageBaseAddress += EFI_PAGE_SIZE) {
+        Status = CoreAllocateMemorySpace (
+                   EfiGcdAllocateAddress,
+                   GcdMemoryType,
+                   EFI_PAGE_SHIFT,
+                   EFI_PAGE_SIZE,
+                   &PageBaseAddress,
+                   gDxeCoreImageHandle,
+                   NULL
+                   );
+
+        if (!EFI_ERROR (Status)) {
+          CoreAddMemoryDescriptor (
+            EfiConventionalMemory,
+            PageBaseAddress,
+            1,
+            Capabilities
+            );
+        }
+      }
+    }
+  }
+  return Status;
+}
+
+
+/**
+  Frees nonexistent memory, reserved memory, system memory, or memory-mapped
+  I/O resources from the global coherency domain of the processor.
+
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+
+  @retval EFI_SUCCESS            Space successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreeMemorySpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:FreeMemorySpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+
+  return CoreConvertSpace (GCD_FREE_MEMORY_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, 0, 0);
+}
+
+
+/**
+  Removes reserved memory, system memory, or memory-mapped I/O resources from
+  the global coherency domain of the processor.
+
+  @param  BaseAddress            Base address of the memory space.
+  @param  Length                 Length of the memory space.
+
+  @retval EFI_SUCCESS            Successfully remove a segment of memory space.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRemoveMemorySpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:RemoveMemorySpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  
+  return CoreConvertSpace (GCD_REMOVE_MEMORY_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, 0, 0);
+}
+
+
+/**
+  Build a memory descriptor according to an entry.
+
+  @param  Descriptor             The descriptor to be built
+  @param  Entry                  According to this entry
+
+**/
+VOID
+BuildMemoryDescriptor (
+  IN OUT EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *Descriptor,
+  IN EFI_GCD_MAP_ENTRY                *Entry
+  )
+{
+  Descriptor->BaseAddress   = Entry->BaseAddress;
+  Descriptor->Length        = Entry->EndAddress - Entry->BaseAddress + 1;
+  Descriptor->Capabilities  = Entry->Capabilities;
+  Descriptor->Attributes    = Entry->Attributes;
+  Descriptor->GcdMemoryType = Entry->GcdMemoryType;
+  Descriptor->ImageHandle   = Entry->ImageHandle;
+  Descriptor->DeviceHandle  = Entry->DeviceHandle;
+}
+
+
+/**
+  Retrieves the descriptor for a memory region containing a specified address.
+
+  @param  BaseAddress            Specified start address
+  @param  Descriptor             Specified length
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully get memory space descriptor.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemorySpaceDescriptor (
+  IN  EFI_PHYSICAL_ADDRESS             BaseAddress,
+  OUT EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *Descriptor
+  )
+{
+  EFI_STATUS         Status;
+  LIST_ENTRY         *StartLink;
+  LIST_ENTRY         *EndLink;
+  EFI_GCD_MAP_ENTRY  *Entry;
+
+  //
+  // Make sure parameters are valid
+  //
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireGcdMemoryLock ();
+
+  //
+  // Search for the list of descriptors that contain BaseAddress
+  //
+  Status = CoreSearchGcdMapEntry (BaseAddress, 1, &StartLink, &EndLink, &mGcdMemorySpaceMap);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_NOT_FOUND;
+  } else {
+    ASSERT (StartLink != NULL && EndLink != NULL);
+    //
+    // Copy the contents of the found descriptor into Descriptor
+    //
+    Entry = CR (StartLink, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    BuildMemoryDescriptor (Descriptor, Entry);
+  }
+
+  CoreReleaseGcdMemoryLock ();
+
+  return Status;
+}
+
+
+/**
+  Modifies the attributes for a memory region in the global coherency domain of the
+  processor.
+
+  @param  BaseAddress            Specified start address
+  @param  Length                 Specified length
+  @param  Attributes             Specified attributes
+
+  @retval EFI_SUCCESS           The attributes were set for the memory region.
+  @retval EFI_INVALID_PARAMETER Length is zero. 
+  @retval EFI_UNSUPPORTED       The processor does not support one or more bytes of the memory
+                                resource range specified by BaseAddress and Length.
+  @retval EFI_UNSUPPORTED       The bit mask of attributes is not support for the memory resource
+                                range specified by BaseAddress and Length.
+  @retval EFI_ACCESS_DEFINED    The attributes for the memory resource range specified by
+                                BaseAddress and Length cannot be modified.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough system resources to modify the attributes of
+                                the memory resource range.
+  @retval EFI_NOT_AVAILABLE_YET The attributes cannot be set because CPU architectural protocol is
+                                not available yet.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetMemorySpaceAttributes (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Attributes
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:SetMemorySpaceAttributes(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  Attributes  = %016lx\n", Attributes));
+
+  return CoreConvertSpace (GCD_SET_ATTRIBUTES_MEMORY_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, 0, Attributes);
+}
+
+
+/**
+  Modifies the capabilities for a memory region in the global coherency domain of the
+  processor.
+
+  @param  BaseAddress      The physical address that is the start address of a memory region.
+  @param  Length           The size in bytes of the memory region.
+  @param  Capabilities     The bit mask of capabilities that the memory region supports.
+
+  @retval EFI_SUCCESS           The capabilities were set for the memory region.
+  @retval EFI_INVALID_PARAMETER Length is zero.
+  @retval EFI_UNSUPPORTED       The capabilities specified by Capabilities do not include the
+                                memory region attributes currently in use.
+  @retval EFI_ACCESS_DENIED     The capabilities for the memory resource range specified by
+                                BaseAddress and Length cannot be modified.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough system resources to modify the capabilities
+                                of the memory resource range.
+**/
+EFI_STATUS
+EFIAPI
+CoreSetMemorySpaceCapabilities (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length,
+  IN UINT64                Capabilities
+  )
+{
+  EFI_STATUS    Status;
+
+  DEBUG ((DEBUG_GCD, "GCD:CoreSetMemorySpaceCapabilities(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  Capabilities  = %016lx\n", Capabilities));
+
+  Status = CoreConvertSpace (GCD_SET_CAPABILITIES_MEMORY_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, Capabilities, 0);
+  if (!EFI_ERROR(Status)) {
+    CoreUpdateMemoryAttributes(BaseAddress, RShiftU64(Length, EFI_PAGE_SHIFT), Capabilities);
+  }
+
+  return Status;
+}
+
+
+/**
+  Returns a map of the memory resources in the global coherency domain of the
+  processor.
+
+  @param  NumberOfDescriptors    Number of descriptors.
+  @param  MemorySpaceMap         Descriptor array
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Successfully get memory space map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemorySpaceMap (
+  OUT UINTN                            *NumberOfDescriptors,
+  OUT EFI_GCD_MEMORY_SPACE_DESCRIPTOR  **MemorySpaceMap
+  )
+{
+  EFI_STATUS                       Status;
+  LIST_ENTRY                       *Link;
+  EFI_GCD_MAP_ENTRY                *Entry;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR  *Descriptor;
+
+  //
+  // Make sure parameters are valid
+  //
+  if (NumberOfDescriptors == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (MemorySpaceMap == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireGcdMemoryLock ();
+
+  //
+  // Count the number of descriptors
+  //
+  *NumberOfDescriptors = CoreCountGcdMapEntry (&mGcdMemorySpaceMap);
+
+  //
+  // Allocate the MemorySpaceMap
+  //
+  *MemorySpaceMap = AllocatePool (*NumberOfDescriptors * sizeof (EFI_GCD_MEMORY_SPACE_DESCRIPTOR));
+  if (*MemorySpaceMap == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Fill in the MemorySpaceMap
+  //
+  Descriptor = *MemorySpaceMap;
+  Link = mGcdMemorySpaceMap.ForwardLink;
+  while (Link != &mGcdMemorySpaceMap) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    BuildMemoryDescriptor (Descriptor, Entry);
+    Descriptor++;
+    Link = Link->ForwardLink;
+  }
+  Status = EFI_SUCCESS;
+
+Done:
+  CoreReleaseGcdMemoryLock ();
+  return Status;
+}
+
+
+/**
+  Adds reserved I/O or I/O resources to the global coherency domain of the processor.
+
+  @param  GcdIoType              IO type of the segment.
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Merged this segment into GCD map.
+  @retval EFI_INVALID_PARAMETER  Parameter not valid
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAddIoSpace (
+  IN EFI_GCD_IO_TYPE       GcdIoType,
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:AddIoSpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  GcdIoType    = %a\n", mGcdIoTypeNames[MIN (GcdIoType, EfiGcdIoTypeMaximum)]));
+  
+  //
+  // Make sure parameters are valid
+  //
+  if (GcdIoType <= EfiGcdIoTypeNonExistent || GcdIoType >= EfiGcdIoTypeMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return CoreConvertSpace (GCD_ADD_IO_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, GcdIoType, BaseAddress, Length, 0, 0);
+}
+
+
+/**
+  Allocates nonexistent I/O, reserved I/O, or I/O resources from the global coherency
+  domain of the processor.
+
+  @param  GcdAllocateType        The type of allocate operation
+  @param  GcdIoType              The desired IO type
+  @param  Alignment              Align with 2^Alignment
+  @param  Length                 Length to allocate
+  @param  BaseAddress            Base address to allocate
+  @param  ImageHandle            The image handle consume the allocated space.
+  @param  DeviceHandle           The device handle consume the allocated space.
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter.
+  @retval EFI_NOT_FOUND          No descriptor contains the desired space.
+  @retval EFI_SUCCESS            IO space successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocateIoSpace (
+  IN     EFI_GCD_ALLOCATE_TYPE  GcdAllocateType,
+  IN     EFI_GCD_IO_TYPE        GcdIoType,
+  IN     UINTN                  Alignment,
+  IN     UINT64                 Length,
+  IN OUT EFI_PHYSICAL_ADDRESS   *BaseAddress,
+  IN     EFI_HANDLE             ImageHandle,
+  IN     EFI_HANDLE             DeviceHandle OPTIONAL
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:AllocateIoSpace(Base=%016lx,Length=%016lx)\n", *BaseAddress, Length));
+  DEBUG ((DEBUG_GCD, "  GcdAllocateType = %a\n", mGcdAllocationTypeNames[MIN (GcdAllocateType, EfiGcdMaxAllocateType)]));
+  DEBUG ((DEBUG_GCD, "  GcdIoType       = %a\n", mGcdIoTypeNames[MIN (GcdIoType, EfiGcdIoTypeMaximum)]));
+  DEBUG ((DEBUG_GCD, "  Alignment       = %016lx\n", LShiftU64 (1, Alignment)));
+  DEBUG ((DEBUG_GCD, "  ImageHandle     = %p\n", ImageHandle));
+  DEBUG ((DEBUG_GCD, "  DeviceHandle    = %p\n", DeviceHandle));
+  
+  return CoreAllocateSpace (
+           GCD_ALLOCATE_IO_OPERATION,
+           GcdAllocateType,
+           (EFI_GCD_MEMORY_TYPE) 0,
+           GcdIoType,
+           Alignment,
+           Length,
+           BaseAddress,
+           ImageHandle,
+           DeviceHandle
+           );
+}
+
+
+/**
+  Frees nonexistent I/O, reserved I/O, or I/O resources from the global coherency
+  domain of the processor.
+
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Space successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreeIoSpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:FreeIoSpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+
+  return CoreConvertSpace (GCD_FREE_IO_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, 0, 0);
+}
+
+
+/**
+  Removes reserved I/O or I/O resources from the global coherency domain of the
+  processor.
+
+  @param  BaseAddress            Base address of the segment.
+  @param  Length                 Length of the segment.
+
+  @retval EFI_SUCCESS            Successfully removed a segment of IO space.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRemoveIoSpace (
+  IN EFI_PHYSICAL_ADDRESS  BaseAddress,
+  IN UINT64                Length
+  )
+{
+  DEBUG ((DEBUG_GCD, "GCD:RemoveIoSpace(Base=%016lx,Length=%016lx)\n", BaseAddress, Length));
+  
+  return CoreConvertSpace (GCD_REMOVE_IO_OPERATION, (EFI_GCD_MEMORY_TYPE) 0, (EFI_GCD_IO_TYPE) 0, BaseAddress, Length, 0, 0);
+}
+
+
+/**
+  Build a IO descriptor according to an entry.
+
+  @param  Descriptor             The descriptor to be built
+  @param  Entry                  According to this entry
+
+**/
+VOID
+BuildIoDescriptor (
+  IN EFI_GCD_IO_SPACE_DESCRIPTOR  *Descriptor,
+  IN EFI_GCD_MAP_ENTRY            *Entry
+  )
+{
+  Descriptor->BaseAddress  = Entry->BaseAddress;
+  Descriptor->Length       = Entry->EndAddress - Entry->BaseAddress + 1;
+  Descriptor->GcdIoType    = Entry->GcdIoType;
+  Descriptor->ImageHandle  = Entry->ImageHandle;
+  Descriptor->DeviceHandle = Entry->DeviceHandle;
+}
+
+
+/**
+  Retrieves the descriptor for an I/O region containing a specified address.
+
+  @param  BaseAddress            Specified start address
+  @param  Descriptor             Specified length
+
+  @retval EFI_INVALID_PARAMETER  Descriptor is NULL.
+  @retval EFI_SUCCESS            Successfully get the IO space descriptor.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetIoSpaceDescriptor (
+  IN  EFI_PHYSICAL_ADDRESS         BaseAddress,
+  OUT EFI_GCD_IO_SPACE_DESCRIPTOR  *Descriptor
+  )
+{
+  EFI_STATUS         Status;
+  LIST_ENTRY         *StartLink;
+  LIST_ENTRY         *EndLink;
+  EFI_GCD_MAP_ENTRY  *Entry;
+
+  //
+  // Make sure parameters are valid
+  //
+  if (Descriptor == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireGcdIoLock ();
+
+  //
+  // Search for the list of descriptors that contain BaseAddress
+  //
+  Status = CoreSearchGcdMapEntry (BaseAddress, 1, &StartLink, &EndLink, &mGcdIoSpaceMap);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_NOT_FOUND;
+  } else {
+    ASSERT (StartLink != NULL && EndLink != NULL);
+    //
+    // Copy the contents of the found descriptor into Descriptor
+    //
+    Entry = CR (StartLink, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    BuildIoDescriptor (Descriptor, Entry);
+  }
+
+  CoreReleaseGcdIoLock ();
+
+  return Status;
+}
+
+
+/**
+  Returns a map of the I/O resources in the global coherency domain of the processor.
+
+  @param  NumberOfDescriptors    Number of descriptors.
+  @param  IoSpaceMap             Descriptor array
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Successfully get IO space map.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetIoSpaceMap (
+  OUT UINTN                        *NumberOfDescriptors,
+  OUT EFI_GCD_IO_SPACE_DESCRIPTOR  **IoSpaceMap
+  )
+{
+  EFI_STATUS                   Status;
+  LIST_ENTRY                   *Link;
+  EFI_GCD_MAP_ENTRY            *Entry;
+  EFI_GCD_IO_SPACE_DESCRIPTOR  *Descriptor;
+
+  //
+  // Make sure parameters are valid
+  //
+  if (NumberOfDescriptors == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (IoSpaceMap == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireGcdIoLock ();
+
+  //
+  // Count the number of descriptors
+  //
+  *NumberOfDescriptors = CoreCountGcdMapEntry (&mGcdIoSpaceMap);
+
+  //
+  // Allocate the IoSpaceMap
+  //
+  *IoSpaceMap = AllocatePool (*NumberOfDescriptors * sizeof (EFI_GCD_IO_SPACE_DESCRIPTOR));
+  if (*IoSpaceMap == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Fill in the IoSpaceMap
+  //
+  Descriptor = *IoSpaceMap;
+  Link = mGcdIoSpaceMap.ForwardLink;
+  while (Link != &mGcdIoSpaceMap) {
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    BuildIoDescriptor (Descriptor, Entry);
+    Descriptor++;
+    Link = Link->ForwardLink;
+  }
+  Status = EFI_SUCCESS;
+
+Done:
+  CoreReleaseGcdIoLock ();
+  return Status;
+}
+
+
+/**
+  Converts a Resource Descriptor HOB attributes mask to an EFI Memory Descriptor
+  capabilities mask
+
+  @param  GcdMemoryType          Type of resource in the GCD memory map.
+  @param  Attributes             The attribute mask in the Resource Descriptor
+                                 HOB.
+
+  @return The capabilities mask for an EFI Memory Descriptor.
+
+**/
+UINT64
+CoreConvertResourceDescriptorHobAttributesToCapabilities (
+  EFI_GCD_MEMORY_TYPE  GcdMemoryType,
+  UINT64               Attributes
+  )
+{
+  UINT64                          Capabilities;
+  GCD_ATTRIBUTE_CONVERSION_ENTRY  *Conversion;
+
+  //
+  // Convert the Resource HOB Attributes to an EFI Memory Capabilities mask
+  //
+  for (Capabilities = 0, Conversion = mAttributeConversionTable; Conversion->Attribute != 0; Conversion++) {
+    if (Conversion->Memory || ((GcdMemoryType != EfiGcdMemoryTypeSystemMemory) && (GcdMemoryType != EfiGcdMemoryTypeMoreReliable))) {
+      if (Attributes & Conversion->Attribute) {
+        Capabilities |= Conversion->Capability;
+      }
+    }
+  }
+
+  return Capabilities;
+}
+
+/**
+  Calculate total memory bin size neeeded.
+
+  @return The total memory bin size neeeded.
+
+**/
+UINT64
+CalculateTotalMemoryBinSizeNeeded (
+  VOID
+  )
+{
+  UINTN     Index;
+  UINT64    TotalSize;
+
+  //
+  // Loop through each memory type in the order specified by the gMemoryTypeInformation[] array
+  //
+  TotalSize = 0;
+  for (Index = 0; gMemoryTypeInformation[Index].Type != EfiMaxMemoryType; Index++) {
+    TotalSize += LShiftU64 (gMemoryTypeInformation[Index].NumberOfPages, EFI_PAGE_SHIFT);
+  }
+
+  return TotalSize;
+}
+
+/**
+  External function. Initializes memory services based on the memory
+  descriptor HOBs.  This function is responsible for priming the memory
+  map, so memory allocations and resource allocations can be made.
+  The first part of this function can not depend on any memory services
+  until at least one memory descriptor is provided to the memory services.
+
+  @param  HobStart               The start address of the HOB.
+  @param  MemoryBaseAddress      Start address of memory region found to init DXE
+                                 core.
+  @param  MemoryLength           Length of memory region found to init DXE core.
+
+  @retval EFI_SUCCESS            Memory services successfully initialized.
+
+**/
+EFI_STATUS
+CoreInitializeMemoryServices (
+  IN  VOID                  **HobStart,
+  OUT EFI_PHYSICAL_ADDRESS  *MemoryBaseAddress,
+  OUT UINT64                *MemoryLength
+  )
+{
+  EFI_PEI_HOB_POINTERS               Hob;
+  EFI_MEMORY_TYPE_INFORMATION        *EfiMemoryTypeInformation;
+  UINTN                              DataSize;
+  BOOLEAN                            Found;
+  EFI_HOB_HANDOFF_INFO_TABLE         *PhitHob;
+  EFI_HOB_RESOURCE_DESCRIPTOR        *ResourceHob;
+  EFI_HOB_RESOURCE_DESCRIPTOR        *PhitResourceHob;
+  EFI_PHYSICAL_ADDRESS               BaseAddress;
+  UINT64                             Length;
+  UINT64                             Attributes;
+  UINT64                             Capabilities;
+  EFI_PHYSICAL_ADDRESS               TestedMemoryBaseAddress;
+  UINT64                             TestedMemoryLength;
+  EFI_PHYSICAL_ADDRESS               HighAddress;
+  EFI_HOB_GUID_TYPE                  *GuidHob;
+  UINT32                             ReservedCodePageNumber;
+  UINT64                             MinimalMemorySizeNeeded;
+
+  //
+  // Point at the first HOB.  This must be the PHIT HOB.
+  //
+  Hob.Raw = *HobStart;
+  ASSERT (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_HANDOFF);
+
+  //
+  // Initialize the spin locks and maps in the memory services.
+  // Also fill in the memory services into the EFI Boot Services Table
+  //
+  CoreInitializePool ();
+
+  //
+  // Initialize Local Variables
+  //
+  PhitResourceHob       = NULL;
+  ResourceHob           = NULL;
+  BaseAddress           = 0;
+  Length                = 0;
+  Attributes            = 0;
+
+  //
+  // Cache the PHIT HOB for later use
+  //
+  PhitHob = Hob.HandoffInformationTable;
+  
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+  	ReservedCodePageNumber = PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber);
+  	ReservedCodePageNumber += PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber);
+   
+  	//
+  	// cache the Top address for loading modules at Fixed Address 
+  	//
+    gLoadModuleAtFixAddressConfigurationTable.DxeCodeTopAddress = PhitHob->EfiMemoryTop 
+                                                                   + EFI_PAGES_TO_SIZE(ReservedCodePageNumber);
+  }
+  //
+  // See if a Memory Type Information HOB is available
+  //
+  GuidHob = GetFirstGuidHob (&gEfiMemoryTypeInformationGuid);
+  if (GuidHob != NULL) {
+    EfiMemoryTypeInformation = GET_GUID_HOB_DATA (GuidHob);
+    DataSize                 = GET_GUID_HOB_DATA_SIZE (GuidHob);
+    if (EfiMemoryTypeInformation != NULL && DataSize > 0 && DataSize <= (EfiMaxMemoryType + 1) * sizeof (EFI_MEMORY_TYPE_INFORMATION)) {
+      CopyMem (&gMemoryTypeInformation, EfiMemoryTypeInformation, DataSize);
+    }
+  }
+
+  //
+  // Include the total memory bin size needed to make sure memory bin could be allocated successfully.
+  //
+  MinimalMemorySizeNeeded = MINIMUM_INITIAL_MEMORY_SIZE + CalculateTotalMemoryBinSizeNeeded ();
+
+  //
+  // Find the Resource Descriptor HOB that contains PHIT range EfiFreeMemoryBottom..EfiFreeMemoryTop
+  //
+  Found  = FALSE;
+  for (Hob.Raw = *HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+    //
+    // Skip all HOBs except Resource Descriptor HOBs
+    //
+    if (GET_HOB_TYPE (Hob) != EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+      continue;
+    }
+
+    //
+    // Skip Resource Descriptor HOBs that do not describe tested system memory
+    //
+    ResourceHob = Hob.ResourceDescriptor;
+    if (ResourceHob->ResourceType != EFI_RESOURCE_SYSTEM_MEMORY) {
+      continue;
+    }
+    if ((ResourceHob->ResourceAttribute & MEMORY_ATTRIBUTE_MASK) != TESTED_MEMORY_ATTRIBUTES) {
+      continue;
+    }
+
+    //
+    // Skip Resource Descriptor HOBs that do not contain the PHIT range EfiFreeMemoryBottom..EfiFreeMemoryTop
+    //
+    if (PhitHob->EfiFreeMemoryBottom < ResourceHob->PhysicalStart) {
+      continue;
+    }
+    if (PhitHob->EfiFreeMemoryTop > (ResourceHob->PhysicalStart + ResourceHob->ResourceLength)) {
+      continue;
+    }
+
+    //
+    // Cache the resource descriptor HOB for the memory region described by the PHIT HOB
+    //
+    PhitResourceHob = ResourceHob;
+    Found = TRUE;
+
+    //
+    // Compute range between PHIT EfiMemoryTop and the end of the Resource Descriptor HOB
+    //
+    Attributes  = PhitResourceHob->ResourceAttribute;
+    BaseAddress = PageAlignAddress (PhitHob->EfiMemoryTop);
+    Length      = PageAlignLength  (ResourceHob->PhysicalStart + ResourceHob->ResourceLength - BaseAddress);
+    if (Length < MinimalMemorySizeNeeded) {
+      //
+      // If that range is not large enough to intialize the DXE Core, then 
+      // Compute range between PHIT EfiFreeMemoryBottom and PHIT EfiFreeMemoryTop
+      //
+      BaseAddress = PageAlignAddress (PhitHob->EfiFreeMemoryBottom);
+      Length      = PageAlignLength  (PhitHob->EfiFreeMemoryTop - BaseAddress);
+      if (Length < MinimalMemorySizeNeeded) {
+        //
+        // If that range is not large enough to intialize the DXE Core, then 
+        // Compute range between the start of the Resource Descriptor HOB and the start of the HOB List
+        //
+        BaseAddress = PageAlignAddress (ResourceHob->PhysicalStart);
+        Length      = PageAlignLength  ((UINT64)((UINTN)*HobStart - BaseAddress));
+      }
+    }
+    break;
+  }
+
+  //
+  // Assert if a resource descriptor HOB for the memory region described by the PHIT was not found
+  //
+  ASSERT (Found);
+
+  //
+  // Take the range in the resource descriptor HOB for the memory region described
+  // by the PHIT as higher priority if it is big enough. It can make the memory bin
+  // allocated to be at the same memory region with PHIT that has more better compatibility
+  // to avoid memory fragmentation for some code practices assume and allocate <4G ACPI memory.
+  //
+  if (Length < MinimalMemorySizeNeeded) {
+    //
+    // Search all the resource descriptor HOBs from the highest possible addresses down for a memory
+    // region that is big enough to initialize the DXE core.  Always skip the PHIT Resource HOB.
+    // The max address must be within the physically addressible range for the processor.
+    //
+    HighAddress = MAX_ADDRESS;
+    for (Hob.Raw = *HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+      //
+      // Skip the Resource Descriptor HOB that contains the PHIT
+      //
+      if (Hob.ResourceDescriptor == PhitResourceHob) {
+        continue;
+      }
+      //
+      // Skip all HOBs except Resource Descriptor HOBs
+      //
+      if (GET_HOB_TYPE (Hob) != EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+        continue;
+      }
+
+      //
+      // Skip Resource Descriptor HOBs that do not describe tested system memory below MAX_ADDRESS
+      //
+      ResourceHob = Hob.ResourceDescriptor;
+      if (ResourceHob->ResourceType != EFI_RESOURCE_SYSTEM_MEMORY) {
+        continue;
+      }
+      if ((ResourceHob->ResourceAttribute & MEMORY_ATTRIBUTE_MASK) != TESTED_MEMORY_ATTRIBUTES) {
+        continue;
+      }
+      if ((ResourceHob->PhysicalStart + ResourceHob->ResourceLength) > (EFI_PHYSICAL_ADDRESS)MAX_ADDRESS) {
+        continue;
+      }
+
+      //
+      // Skip Resource Descriptor HOBs that are below a previously found Resource Descriptor HOB
+      //
+      if (HighAddress != (EFI_PHYSICAL_ADDRESS)MAX_ADDRESS && ResourceHob->PhysicalStart <= HighAddress) {
+        continue;
+      }
+
+      //
+      // Skip Resource Descriptor HOBs that are not large enough to initilize the DXE Core
+      //
+      TestedMemoryBaseAddress = PageAlignAddress (ResourceHob->PhysicalStart);
+      TestedMemoryLength      = PageAlignLength  (ResourceHob->PhysicalStart + ResourceHob->ResourceLength - TestedMemoryBaseAddress);
+      if (TestedMemoryLength < MinimalMemorySizeNeeded) {
+        continue;
+      }
+
+      //
+      // Save the range described by the Resource Descriptor that is large enough to initilize the DXE Core
+      //
+      BaseAddress = TestedMemoryBaseAddress;
+      Length      = TestedMemoryLength;
+      Attributes  = ResourceHob->ResourceAttribute; 
+      HighAddress = ResourceHob->PhysicalStart;
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "CoreInitializeMemoryServices:\n"));
+  DEBUG ((EFI_D_INFO, "  BaseAddress - 0x%lx Length - 0x%lx MinimalMemorySizeNeeded - 0x%lx\n", BaseAddress, Length, MinimalMemorySizeNeeded));
+
+  //
+  // If no memory regions are found that are big enough to initialize the DXE core, then ASSERT().
+  //
+  ASSERT (Length >= MinimalMemorySizeNeeded);
+
+  //
+  // Convert the Resource HOB Attributes to an EFI Memory Capabilities mask
+  //
+  if ((Attributes & EFI_RESOURCE_ATTRIBUTE_MORE_RELIABLE) == EFI_RESOURCE_ATTRIBUTE_MORE_RELIABLE) {
+    Capabilities = CoreConvertResourceDescriptorHobAttributesToCapabilities (EfiGcdMemoryTypeMoreReliable, Attributes);
+  } else {
+    Capabilities = CoreConvertResourceDescriptorHobAttributesToCapabilities (EfiGcdMemoryTypeSystemMemory, Attributes);
+  }
+
+  //
+  // Declare the very first memory region, so the EFI Memory Services are available.
+  //
+  CoreAddMemoryDescriptor (
+    EfiConventionalMemory,
+    BaseAddress,
+    RShiftU64 (Length, EFI_PAGE_SHIFT),
+    Capabilities
+    );
+
+  *MemoryBaseAddress = BaseAddress;
+  *MemoryLength      = Length;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  External function. Initializes the GCD and memory services based on the memory
+  descriptor HOBs.  This function is responsible for priming the GCD map and the
+  memory map, so memory allocations and resource allocations can be made. The
+  HobStart will be relocated to a pool buffer.
+
+  @param  HobStart               The start address of the HOB
+  @param  MemoryBaseAddress      Start address of memory region found to init DXE
+                                 core.
+  @param  MemoryLength           Length of memory region found to init DXE core.
+
+  @retval EFI_SUCCESS            GCD services successfully initialized.
+
+**/
+EFI_STATUS
+CoreInitializeGcdServices (
+  IN OUT VOID              **HobStart,
+  IN EFI_PHYSICAL_ADDRESS  MemoryBaseAddress,
+  IN UINT64                MemoryLength
+  )
+{
+  EFI_PEI_HOB_POINTERS               Hob;
+  VOID                               *NewHobList;
+  EFI_HOB_HANDOFF_INFO_TABLE         *PhitHob;
+  UINT8                              SizeOfMemorySpace;
+  UINT8                              SizeOfIoSpace;
+  EFI_HOB_RESOURCE_DESCRIPTOR        *ResourceHob;
+  EFI_PHYSICAL_ADDRESS               BaseAddress;
+  UINT64                             Length;
+  EFI_STATUS                         Status;
+  EFI_GCD_MAP_ENTRY                  *Entry;
+  EFI_GCD_MEMORY_TYPE                GcdMemoryType;
+  EFI_GCD_IO_TYPE                    GcdIoType;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR    Descriptor;
+  EFI_HOB_MEMORY_ALLOCATION          *MemoryHob;
+  EFI_HOB_FIRMWARE_VOLUME            *FirmwareVolumeHob;
+  UINTN                              NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR    *MemorySpaceMap;
+  UINTN                              Index;
+  UINT64                             Capabilities;
+  EFI_HOB_CPU *                      CpuHob;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR    *MemorySpaceMapHobList;
+
+  //
+  // Cache the PHIT HOB for later use
+  //
+  PhitHob = (EFI_HOB_HANDOFF_INFO_TABLE *)(*HobStart);
+
+  //
+  // Get the number of address lines in the I/O and Memory space for the CPU
+  //
+  CpuHob = GetFirstHob (EFI_HOB_TYPE_CPU);
+  ASSERT (CpuHob != NULL);
+  SizeOfMemorySpace = CpuHob->SizeOfMemorySpace;
+  SizeOfIoSpace     = CpuHob->SizeOfIoSpace;
+
+  //
+  // Initialize the GCD Memory Space Map
+  //
+  Entry = AllocateCopyPool (sizeof (EFI_GCD_MAP_ENTRY), &mGcdMemorySpaceMapEntryTemplate);
+  ASSERT (Entry != NULL);
+
+  Entry->EndAddress = LShiftU64 (1, SizeOfMemorySpace) - 1;
+
+  InsertHeadList (&mGcdMemorySpaceMap, &Entry->Link);
+
+  CoreDumpGcdMemorySpaceMap (TRUE);
+  
+  //
+  // Initialize the GCD I/O Space Map
+  //
+  Entry = AllocateCopyPool (sizeof (EFI_GCD_MAP_ENTRY), &mGcdIoSpaceMapEntryTemplate);
+  ASSERT (Entry != NULL);
+
+  Entry->EndAddress = LShiftU64 (1, SizeOfIoSpace) - 1;
+
+  InsertHeadList (&mGcdIoSpaceMap, &Entry->Link);
+
+  CoreDumpGcdIoSpaceMap (TRUE);
+  
+  //
+  // Walk the HOB list and add all resource descriptors to the GCD
+  //
+  for (Hob.Raw = *HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+
+    GcdMemoryType = EfiGcdMemoryTypeNonExistent;
+    GcdIoType     = EfiGcdIoTypeNonExistent;
+
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+
+      ResourceHob = Hob.ResourceDescriptor;
+
+      switch (ResourceHob->ResourceType) {
+      case EFI_RESOURCE_SYSTEM_MEMORY:
+        if ((ResourceHob->ResourceAttribute & MEMORY_ATTRIBUTE_MASK) == TESTED_MEMORY_ATTRIBUTES) {
+          if ((ResourceHob->ResourceAttribute & EFI_RESOURCE_ATTRIBUTE_MORE_RELIABLE) == EFI_RESOURCE_ATTRIBUTE_MORE_RELIABLE) {
+            GcdMemoryType = EfiGcdMemoryTypeMoreReliable;
+          } else {
+            GcdMemoryType = EfiGcdMemoryTypeSystemMemory;
+          }
+        }
+        if ((ResourceHob->ResourceAttribute & MEMORY_ATTRIBUTE_MASK) == INITIALIZED_MEMORY_ATTRIBUTES) {
+          GcdMemoryType = EfiGcdMemoryTypeReserved;
+        }
+        if ((ResourceHob->ResourceAttribute & MEMORY_ATTRIBUTE_MASK) == PRESENT_MEMORY_ATTRIBUTES) {
+          GcdMemoryType = EfiGcdMemoryTypeReserved;
+        }
+        if ((ResourceHob->ResourceAttribute & EFI_RESOURCE_ATTRIBUTE_PERSISTENT) == EFI_RESOURCE_ATTRIBUTE_PERSISTENT) {
+          GcdMemoryType = EfiGcdMemoryTypePersistentMemory;
+        }
+        break;
+      case EFI_RESOURCE_MEMORY_MAPPED_IO:
+      case EFI_RESOURCE_FIRMWARE_DEVICE:
+        GcdMemoryType = EfiGcdMemoryTypeMemoryMappedIo;
+        break;
+      case EFI_RESOURCE_MEMORY_MAPPED_IO_PORT:
+      case EFI_RESOURCE_MEMORY_RESERVED:
+        GcdMemoryType = EfiGcdMemoryTypeReserved;
+        break;
+      case EFI_RESOURCE_IO:
+        GcdIoType = EfiGcdIoTypeIo;
+        break;
+      case EFI_RESOURCE_IO_RESERVED:
+        GcdIoType = EfiGcdIoTypeReserved;
+        break;
+      }
+
+      if (GcdMemoryType != EfiGcdMemoryTypeNonExistent) {
+        //
+        // Validate the Resource HOB Attributes
+        //
+        CoreValidateResourceDescriptorHobAttributes (ResourceHob->ResourceAttribute);
+
+        //
+        // Convert the Resource HOB Attributes to an EFI Memory Capabilities mask
+        //
+        Capabilities = CoreConvertResourceDescriptorHobAttributesToCapabilities (
+                         GcdMemoryType,
+                         ResourceHob->ResourceAttribute
+                         );
+
+        Status = CoreInternalAddMemorySpace (
+                   GcdMemoryType,
+                   ResourceHob->PhysicalStart,
+                   ResourceHob->ResourceLength,
+                   Capabilities
+                   );
+      }
+
+      if (GcdIoType != EfiGcdIoTypeNonExistent) {
+        Status = CoreAddIoSpace (
+                   GcdIoType,
+                   ResourceHob->PhysicalStart,
+                   ResourceHob->ResourceLength
+                   );
+      }
+    }
+  }
+
+  //
+  // Allocate first memory region from the GCD by the DXE core
+  //
+  Status = CoreGetMemorySpaceDescriptor (MemoryBaseAddress, &Descriptor);
+  if (!EFI_ERROR (Status)) {
+    ASSERT ((Descriptor.GcdMemoryType == EfiGcdMemoryTypeSystemMemory) ||
+            (Descriptor.GcdMemoryType == EfiGcdMemoryTypeMoreReliable));
+    Status = CoreAllocateMemorySpace (
+               EfiGcdAllocateAddress,
+               Descriptor.GcdMemoryType,
+               0,
+               MemoryLength,
+               &MemoryBaseAddress,
+               gDxeCoreImageHandle,
+               NULL
+               );
+  }
+
+  //
+  // Walk the HOB list and allocate all memory space that is consumed by memory allocation HOBs,
+  // and Firmware Volume HOBs.  Also update the EFI Memory Map with the memory allocation HOBs.
+  //
+  for (Hob.Raw = *HobStart; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
+      MemoryHob = Hob.MemoryAllocation;
+      BaseAddress = MemoryHob->AllocDescriptor.MemoryBaseAddress;
+      Status = CoreGetMemorySpaceDescriptor  (BaseAddress, &Descriptor);
+      if (!EFI_ERROR (Status)) {
+        Status = CoreAllocateMemorySpace (
+                   EfiGcdAllocateAddress,
+                   Descriptor.GcdMemoryType,
+                   0,
+                   MemoryHob->AllocDescriptor.MemoryLength,
+                   &BaseAddress,
+                   gDxeCoreImageHandle,
+                   NULL
+                   );
+        if (!EFI_ERROR (Status) &&
+            ((Descriptor.GcdMemoryType == EfiGcdMemoryTypeSystemMemory) ||
+             (Descriptor.GcdMemoryType == EfiGcdMemoryTypeMoreReliable))) {
+          CoreAddMemoryDescriptor (
+            MemoryHob->AllocDescriptor.MemoryType,
+            MemoryHob->AllocDescriptor.MemoryBaseAddress,
+            RShiftU64 (MemoryHob->AllocDescriptor.MemoryLength, EFI_PAGE_SHIFT),
+            Descriptor.Capabilities & (~EFI_MEMORY_RUNTIME)
+            );
+        }
+      }
+    }
+
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_FV) {
+      FirmwareVolumeHob = Hob.FirmwareVolume;
+      BaseAddress = FirmwareVolumeHob->BaseAddress;
+      Status = CoreAllocateMemorySpace (
+                 EfiGcdAllocateAddress,
+                 EfiGcdMemoryTypeMemoryMappedIo,
+                 0,
+                 FirmwareVolumeHob->Length,
+                 &BaseAddress,
+                 gDxeCoreImageHandle,
+                 NULL
+                 );
+    }
+  }
+
+  //
+  // Add and allocate the remaining unallocated system memory to the memory services.
+  //
+  Status = CoreGetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+  ASSERT (Status == EFI_SUCCESS);
+
+  MemorySpaceMapHobList = NULL;
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if ((MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeSystemMemory) ||
+        (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeMoreReliable)) {
+      if (MemorySpaceMap[Index].ImageHandle == NULL) {
+        BaseAddress  = PageAlignAddress (MemorySpaceMap[Index].BaseAddress);
+        Length       = PageAlignLength  (MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length - BaseAddress);
+        if (Length == 0 || MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length < BaseAddress) {
+          continue;
+        }
+        if (((UINTN) MemorySpaceMap[Index].BaseAddress <= (UINTN) (*HobStart)) &&
+            ((UINTN) (MemorySpaceMap[Index].BaseAddress + MemorySpaceMap[Index].Length) >= (UINTN) PhitHob->EfiFreeMemoryBottom)) {
+          //
+          // Skip the memory space that covers HOB List, it should be processed
+          // after HOB List relocation to avoid the resources allocated by others
+          // to corrupt HOB List before its relocation.
+          //
+          MemorySpaceMapHobList = &MemorySpaceMap[Index];
+          continue;
+        }
+        CoreAddMemoryDescriptor (
+          EfiConventionalMemory,
+          BaseAddress,
+          RShiftU64 (Length, EFI_PAGE_SHIFT),
+          MemorySpaceMap[Index].Capabilities & (~EFI_MEMORY_RUNTIME)
+          );
+        Status = CoreAllocateMemorySpace (
+                   EfiGcdAllocateAddress,
+                   MemorySpaceMap[Index].GcdMemoryType,
+                   0,
+                   Length,
+                   &BaseAddress,
+                   gDxeCoreImageHandle,
+                   NULL
+                   );
+      }
+    }
+  }
+
+  //
+  // Relocate HOB List to an allocated pool buffer.
+  // The relocation should be at after all the tested memory resources added
+  // (except the memory space that covers HOB List) to the memory services,
+  // because the memory resource found in CoreInitializeMemoryServices()
+  // may have not enough remaining resource for HOB List.
+  //
+  NewHobList = AllocateCopyPool (
+                 (UINTN) PhitHob->EfiFreeMemoryBottom - (UINTN) (*HobStart),
+                 *HobStart
+                 );
+  ASSERT (NewHobList != NULL);
+
+  *HobStart = NewHobList;
+  gHobList  = NewHobList;
+
+  if (MemorySpaceMapHobList != NULL) {
+    //
+    // Add and allocate the memory space that covers HOB List to the memory services
+    // after HOB List relocation.
+    //
+    BaseAddress = PageAlignAddress (MemorySpaceMapHobList->BaseAddress);
+    Length      = PageAlignLength  (MemorySpaceMapHobList->BaseAddress + MemorySpaceMapHobList->Length - BaseAddress);
+    CoreAddMemoryDescriptor (
+      EfiConventionalMemory,
+      BaseAddress,
+      RShiftU64 (Length, EFI_PAGE_SHIFT),
+      MemorySpaceMapHobList->Capabilities & (~EFI_MEMORY_RUNTIME)
+      );
+    Status = CoreAllocateMemorySpace (
+               EfiGcdAllocateAddress,
+               MemorySpaceMapHobList->GcdMemoryType,
+               0,
+               Length,
+               &BaseAddress,
+               gDxeCoreImageHandle,
+               NULL
+               );
+  }
+
+  CoreFreePool (MemorySpaceMap);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.h b/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.h
new file mode 100644
index 0000000000..1d5fb61092
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Gcd/Gcd.h
@@ -0,0 +1,46 @@
+/** @file
+  GCD Operations and data structure used to
+  convert from GCD attributes to EFI Memory Map attributes.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _GCD_H_
+#define _GCD_H_
+
+//
+// GCD Operations
+//
+#define GCD_MEMORY_SPACE_OPERATION 0x20
+#define GCD_IO_SPACE_OPERATION     0x40
+
+#define GCD_ADD_MEMORY_OPERATION               (GCD_MEMORY_SPACE_OPERATION | 0)
+#define GCD_ALLOCATE_MEMORY_OPERATION          (GCD_MEMORY_SPACE_OPERATION | 1)
+#define GCD_FREE_MEMORY_OPERATION              (GCD_MEMORY_SPACE_OPERATION | 2)
+#define GCD_REMOVE_MEMORY_OPERATION            (GCD_MEMORY_SPACE_OPERATION | 3)
+#define GCD_SET_ATTRIBUTES_MEMORY_OPERATION    (GCD_MEMORY_SPACE_OPERATION | 4)
+#define GCD_SET_CAPABILITIES_MEMORY_OPERATION  (GCD_MEMORY_SPACE_OPERATION | 5)
+
+#define GCD_ADD_IO_OPERATION                   (GCD_IO_SPACE_OPERATION | 0)
+#define GCD_ALLOCATE_IO_OPERATION              (GCD_IO_SPACE_OPERATION | 1)
+#define GCD_FREE_IO_OPERATION                  (GCD_IO_SPACE_OPERATION | 2)
+#define GCD_REMOVE_IO_OPERATION                (GCD_IO_SPACE_OPERATION | 3)
+
+//
+// The data structure used to convert from GCD attributes to EFI Memory Map attributes
+//
+typedef struct {
+  UINT64   Attribute;
+  UINT64   Capability;
+  BOOLEAN  Memory;
+} GCD_ATTRIBUTE_CONVERSION_ENTRY;
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Hand/DriverSupport.c b/Core/MdeModulePkg/Core/Dxe/Hand/DriverSupport.c
new file mode 100644
index 0000000000..33dd0bd0b7
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Hand/DriverSupport.c
@@ -0,0 +1,964 @@
+/** @file
+  Support functions to connect/disconnect UEFI Driver model Protocol
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "Handle.h"
+
+
+//
+// Driver Support Functions
+//
+/**
+  Connects one or more drivers to a controller.
+
+  @param  ControllerHandle      The handle of the controller to which driver(s) are to be connected.
+  @param  DriverImageHandle     A pointer to an ordered list handles that support the
+                                EFI_DRIVER_BINDING_PROTOCOL.
+  @param  RemainingDevicePath   A pointer to the device path that specifies a child of the
+                                controller specified by ControllerHandle.
+  @param  Recursive             If TRUE, then ConnectController() is called recursively
+                                until the entire tree of controllers below the controller specified
+                                by ControllerHandle have been created. If FALSE, then
+                                the tree of controllers is only expanded one level.
+
+  @retval EFI_SUCCESS           1) One or more drivers were connected to ControllerHandle.
+                                2) No drivers were connected to ControllerHandle, but
+                                RemainingDevicePath is not NULL, and it is an End Device
+                                Path Node.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_NOT_FOUND         1) There are no EFI_DRIVER_BINDING_PROTOCOL instances
+                                present in the system.
+                                2) No drivers were connected to ControllerHandle.
+  @retval EFI_SECURITY_VIOLATION 
+                                The user has no permission to start UEFI device drivers on the device path 
+                                associated with the ControllerHandle or specified by the RemainingDevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreConnectController (
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  EFI_HANDLE                *DriverImageHandle    OPTIONAL,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath  OPTIONAL,
+  IN  BOOLEAN                   Recursive
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_STATUS                           ReturnStatus;
+  IHANDLE                              *Handle;
+  PROTOCOL_INTERFACE                   *Prot;
+  LIST_ENTRY                           *Link;
+  LIST_ENTRY                           *ProtLink;
+  OPEN_PROTOCOL_DATA                   *OpenData;
+  EFI_DEVICE_PATH_PROTOCOL             *AlignedRemainingDevicePath;
+  EFI_HANDLE                           *ChildHandleBuffer;
+  UINTN                                ChildHandleCount;
+  UINTN                                Index;
+  UINTN                                HandleFilePathSize;
+  UINTN                                RemainingDevicePathSize;
+  EFI_DEVICE_PATH_PROTOCOL             *HandleFilePath;
+  EFI_DEVICE_PATH_PROTOCOL             *FilePath;
+  EFI_DEVICE_PATH_PROTOCOL             *TempFilePath;
+
+  //
+  // Make sure ControllerHandle is valid
+  //
+  Status = CoreValidateHandle (ControllerHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (gSecurity2 != NULL) {
+    //
+    // Check whether the user has permission to start UEFI device drivers.
+    //
+    Status = CoreHandleProtocol (ControllerHandle, &gEfiDevicePathProtocolGuid, (VOID **)&HandleFilePath);
+    if (!EFI_ERROR (Status)) {
+      ASSERT (HandleFilePath != NULL);
+      FilePath     = HandleFilePath;
+      TempFilePath = NULL;
+      if (RemainingDevicePath != NULL && !Recursive) {
+        HandleFilePathSize      = GetDevicePathSize (HandleFilePath) - sizeof (EFI_DEVICE_PATH_PROTOCOL);
+        RemainingDevicePathSize = GetDevicePathSize (RemainingDevicePath);
+        TempFilePath = AllocateZeroPool (HandleFilePathSize + RemainingDevicePathSize);
+        ASSERT (TempFilePath != NULL);
+        CopyMem (TempFilePath, HandleFilePath, HandleFilePathSize);
+        CopyMem ((UINT8 *) TempFilePath + HandleFilePathSize, RemainingDevicePath, RemainingDevicePathSize);
+        FilePath = TempFilePath;
+      }
+      Status = gSecurity2->FileAuthentication (
+                            gSecurity2,
+                            FilePath,
+                            NULL,
+                            0,
+                            FALSE
+                            );
+      if (TempFilePath != NULL) {
+        FreePool (TempFilePath);
+      }
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+  
+  Handle = ControllerHandle;
+
+  //
+  // Make a copy of RemainingDevicePath to guanatee it is aligned
+  //
+  AlignedRemainingDevicePath = NULL;
+  if (RemainingDevicePath != NULL) {
+    AlignedRemainingDevicePath = DuplicateDevicePath (RemainingDevicePath);
+
+    if (AlignedRemainingDevicePath == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  //
+  // Connect all drivers to ControllerHandle
+  // If CoreConnectSingleController returns EFI_NOT_READY, then the number of
+  // Driver Binding Protocols in the handle database has increased during the call
+  // so the connect operation must be restarted
+  //
+  do {
+    ReturnStatus = CoreConnectSingleController (
+                     ControllerHandle,
+                     DriverImageHandle,
+                     AlignedRemainingDevicePath
+                     );
+  } while (ReturnStatus == EFI_NOT_READY);
+
+  //
+  // Free the aligned copy of RemainingDevicePath
+  //
+  if (AlignedRemainingDevicePath != NULL) {
+    CoreFreePool (AlignedRemainingDevicePath);
+  }
+
+  //
+  // If recursive, then connect all drivers to all of ControllerHandle's children
+  //
+  if (Recursive) {
+    //
+    // Acquire the protocol lock on the handle database so the child handles can be collected
+    //
+    CoreAcquireProtocolLock ();
+
+    //
+    // Make sure the DriverBindingHandle is valid
+    //
+    Status = CoreValidateHandle (ControllerHandle);
+    if (EFI_ERROR (Status)) {
+      //
+      // Release the protocol lock on the handle database
+      //
+      CoreReleaseProtocolLock ();
+
+      return ReturnStatus;
+    }
+
+
+    //
+    // Count ControllerHandle's children
+    //
+    for (Link = Handle->Protocols.ForwardLink, ChildHandleCount = 0; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      for (ProtLink = Prot->OpenList.ForwardLink;
+          ProtLink != &Prot->OpenList;
+          ProtLink = ProtLink->ForwardLink) {
+        OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+          ChildHandleCount++;
+        }
+      }
+    }
+
+    //
+    // Allocate a handle buffer for ControllerHandle's children
+    //
+    ChildHandleBuffer = AllocatePool (ChildHandleCount * sizeof(EFI_HANDLE));
+    if (ChildHandleBuffer == NULL) {
+      CoreReleaseProtocolLock ();
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Fill in a handle buffer with ControllerHandle's children
+    //
+    for (Link = Handle->Protocols.ForwardLink, ChildHandleCount = 0; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      for (ProtLink = Prot->OpenList.ForwardLink;
+          ProtLink != &Prot->OpenList;
+          ProtLink = ProtLink->ForwardLink) {
+        OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+          ChildHandleBuffer[ChildHandleCount] = OpenData->ControllerHandle;
+          ChildHandleCount++;
+        }
+      }
+    }
+
+    //
+    // Release the protocol lock on the handle database
+    //
+    CoreReleaseProtocolLock ();
+
+    //
+    // Recursively connect each child handle
+    //
+    for (Index = 0; Index < ChildHandleCount; Index++) {
+      CoreConnectController (
+        ChildHandleBuffer[Index],
+        NULL,
+        NULL,
+        TRUE
+        );
+    }
+
+    //
+    // Free the handle buffer of ControllerHandle's children
+    //
+    CoreFreePool (ChildHandleBuffer);
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Add Driver Binding Protocols from Context Driver Image Handles to sorted
+  Driver Binding Protocol list.
+
+  @param  DriverBindingHandle                   Handle of the driver binding
+                                                protocol.
+  @param  NumberOfSortedDriverBindingProtocols  Number Of sorted driver binding
+                                                protocols
+  @param  SortedDriverBindingProtocols          The sorted protocol list.
+  @param  DriverBindingHandleCount              Driver Binding Handle Count.
+  @param  DriverBindingHandleBuffer             The buffer of driver binding
+                                                protocol to be modified.
+  @param  IsImageHandle                         Indicate whether
+                                                DriverBindingHandle is an image
+                                                handle
+
+  @return None.
+
+**/
+VOID
+AddSortedDriverBindingProtocol (
+  IN      EFI_HANDLE                   DriverBindingHandle,
+  IN OUT  UINTN                        *NumberOfSortedDriverBindingProtocols,
+  IN OUT  EFI_DRIVER_BINDING_PROTOCOL  **SortedDriverBindingProtocols,
+  IN      UINTN                        DriverBindingHandleCount,
+  IN OUT  EFI_HANDLE                   *DriverBindingHandleBuffer,
+  IN      BOOLEAN                      IsImageHandle
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_DRIVER_BINDING_PROTOCOL  *DriverBinding;
+  UINTN                        Index;
+
+  //
+  // Make sure the DriverBindingHandle is valid
+  //
+  Status = CoreValidateHandle (DriverBindingHandle);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // If IsImageHandle is TRUE, then DriverBindingHandle is an image handle
+  // Find all the DriverBindingHandles associated with that image handle and add them to the sorted list
+  //
+  if (IsImageHandle) {
+    //
+    // Loop through all the Driver Binding Handles
+    //
+    for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+      //
+      // Retrieve the Driver Binding Protocol associated with each Driver Binding Handle
+      //
+      Status = CoreHandleProtocol (
+                DriverBindingHandleBuffer[Index],
+                &gEfiDriverBindingProtocolGuid,
+                (VOID **) &DriverBinding
+                );
+      if (EFI_ERROR (Status) || DriverBinding == NULL) {
+        continue;
+      }
+
+      //
+      // If the ImageHandle associated with DriverBinding matches DriverBindingHandle,
+      // then add the DriverBindingProtocol[Index] to the sorted list
+      //
+      if (DriverBinding->ImageHandle == DriverBindingHandle) {
+        AddSortedDriverBindingProtocol (
+          DriverBindingHandleBuffer[Index],
+          NumberOfSortedDriverBindingProtocols,
+          SortedDriverBindingProtocols,
+          DriverBindingHandleCount,
+          DriverBindingHandleBuffer,
+          FALSE
+          );
+      }
+    }
+    return;
+  }
+
+  //
+  // Retrieve the Driver Binding Protocol from DriverBindingHandle
+  //
+  Status = CoreHandleProtocol(
+             DriverBindingHandle,
+             &gEfiDriverBindingProtocolGuid,
+             (VOID **) &DriverBinding
+             );
+  //
+  // If DriverBindingHandle does not support the Driver Binding Protocol then return
+  //
+  if (EFI_ERROR (Status) || DriverBinding == NULL) {
+    return;
+  }
+
+  //
+  // See if DriverBinding is already in the sorted list
+  //
+  for (Index = 0; Index < *NumberOfSortedDriverBindingProtocols && Index < DriverBindingHandleCount; Index++) {
+    if (DriverBinding == SortedDriverBindingProtocols[Index]) {
+      return;
+    }
+  }
+
+  //
+  // Add DriverBinding to the end of the list
+  //
+  if (*NumberOfSortedDriverBindingProtocols < DriverBindingHandleCount) {
+    SortedDriverBindingProtocols[*NumberOfSortedDriverBindingProtocols] = DriverBinding;
+  }
+  *NumberOfSortedDriverBindingProtocols = *NumberOfSortedDriverBindingProtocols + 1;
+
+  //
+  // Mark the cooresponding handle in DriverBindingHandleBuffer as used
+  //
+  for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+    if (DriverBindingHandleBuffer[Index] == DriverBindingHandle) {
+      DriverBindingHandleBuffer[Index] = NULL;
+    }
+  }
+}
+
+
+/**
+  Connects a controller to a driver.
+
+  @param  ControllerHandle                      Handle of the controller to be
+                                                connected.
+  @param  ContextDriverImageHandles             DriverImageHandle A pointer to an
+                                                ordered list of driver image
+                                                handles.
+  @param  RemainingDevicePath                   RemainingDevicePath A pointer to
+                                                the device path that specifies a
+                                                child  of the controller
+                                                specified by ControllerHandle.
+
+  @retval EFI_SUCCESS                           One or more drivers were
+                                                connected to ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES                  No enough system resources to
+                                                complete the request.
+  @retval EFI_NOT_FOUND                         No drivers were connected to
+                                                ControllerHandle.
+
+**/
+EFI_STATUS
+CoreConnectSingleController (
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  EFI_HANDLE                *ContextDriverImageHandles OPTIONAL,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath       OPTIONAL
+  )
+{
+  EFI_STATUS                                 Status;
+  UINTN                                      Index;
+  EFI_HANDLE                                 DriverImageHandle;
+  EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL      *PlatformDriverOverride;
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL  *BusSpecificDriverOverride;
+  UINTN                                      DriverBindingHandleCount;
+  EFI_HANDLE                                 *DriverBindingHandleBuffer;
+  UINTN                                      NewDriverBindingHandleCount;
+  EFI_HANDLE                                 *NewDriverBindingHandleBuffer;
+  EFI_DRIVER_BINDING_PROTOCOL                *DriverBinding;
+  EFI_DRIVER_FAMILY_OVERRIDE_PROTOCOL        *DriverFamilyOverride;
+  UINTN                                      NumberOfSortedDriverBindingProtocols;
+  EFI_DRIVER_BINDING_PROTOCOL                **SortedDriverBindingProtocols;
+  UINT32                                     DriverFamilyOverrideVersion;
+  UINT32                                     HighestVersion;
+  UINTN                                      HighestIndex;
+  UINTN                                      SortIndex;
+  BOOLEAN                                    OneStarted;
+  BOOLEAN                                    DriverFound;
+
+  //
+  // Initialize local variables
+  //
+  DriverBindingHandleCount              = 0;
+  DriverBindingHandleBuffer             = NULL;
+  NumberOfSortedDriverBindingProtocols  = 0;
+  SortedDriverBindingProtocols          = NULL;
+  PlatformDriverOverride                = NULL;
+  NewDriverBindingHandleBuffer          = NULL;
+
+  //
+  // Get list of all Driver Binding Protocol Instances
+  //
+  Status = CoreLocateHandleBuffer (
+             ByProtocol,
+             &gEfiDriverBindingProtocolGuid,
+             NULL,
+             &DriverBindingHandleCount,
+             &DriverBindingHandleBuffer
+             );
+  if (EFI_ERROR (Status) || (DriverBindingHandleCount == 0)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Allocate a duplicate array for the sorted Driver Binding Protocol Instances
+  //
+  SortedDriverBindingProtocols = AllocatePool (sizeof (VOID *) * DriverBindingHandleCount);
+  if (SortedDriverBindingProtocols == NULL) {
+    CoreFreePool (DriverBindingHandleBuffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Add Driver Binding Protocols from Context Driver Image Handles first
+  //
+  if (ContextDriverImageHandles != NULL) {
+    for (Index = 0; ContextDriverImageHandles[Index] != NULL; Index++) {
+      AddSortedDriverBindingProtocol (
+        ContextDriverImageHandles[Index],
+        &NumberOfSortedDriverBindingProtocols,
+        SortedDriverBindingProtocols,
+        DriverBindingHandleCount,
+        DriverBindingHandleBuffer,
+        FALSE
+        );
+    }
+  }
+
+  //
+  // Add the Platform Driver Override Protocol drivers for ControllerHandle next
+  //
+  Status = CoreLocateProtocol (
+             &gEfiPlatformDriverOverrideProtocolGuid,
+             NULL,
+             (VOID **) &PlatformDriverOverride
+             );
+  if (!EFI_ERROR (Status) && (PlatformDriverOverride != NULL)) {
+    DriverImageHandle = NULL;
+    do {
+      Status = PlatformDriverOverride->GetDriver (
+                                         PlatformDriverOverride,
+                                         ControllerHandle,
+                                         &DriverImageHandle
+                                         );
+      if (!EFI_ERROR (Status)) {
+        AddSortedDriverBindingProtocol (
+          DriverImageHandle,
+          &NumberOfSortedDriverBindingProtocols,
+          SortedDriverBindingProtocols,
+          DriverBindingHandleCount,
+          DriverBindingHandleBuffer,
+          TRUE
+          );
+      }
+    } while (!EFI_ERROR (Status));
+  }
+
+  //
+  // Add the Driver Family Override Protocol drivers for ControllerHandle
+  //
+  while (TRUE) {
+    HighestIndex   = DriverBindingHandleCount;
+    HighestVersion = 0;
+    for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+      Status = CoreHandleProtocol (
+                 DriverBindingHandleBuffer[Index],
+                 &gEfiDriverFamilyOverrideProtocolGuid,
+                 (VOID **) &DriverFamilyOverride
+                 );
+      if (!EFI_ERROR (Status) && (DriverFamilyOverride != NULL)) {
+        DriverFamilyOverrideVersion = DriverFamilyOverride->GetVersion (DriverFamilyOverride);
+        if ((HighestIndex == DriverBindingHandleCount) || (DriverFamilyOverrideVersion > HighestVersion)) {
+          HighestVersion = DriverFamilyOverrideVersion;
+          HighestIndex   = Index;
+        }
+      }
+    }
+
+    if (HighestIndex == DriverBindingHandleCount) {
+      break;
+    }
+
+    AddSortedDriverBindingProtocol (
+      DriverBindingHandleBuffer[HighestIndex],
+      &NumberOfSortedDriverBindingProtocols,
+      SortedDriverBindingProtocols,
+      DriverBindingHandleCount,
+      DriverBindingHandleBuffer,
+      FALSE
+      );
+  }
+
+  //
+  // Get the Bus Specific Driver Override Protocol instance on the Controller Handle
+  //
+  Status = CoreHandleProtocol (
+             ControllerHandle,
+             &gEfiBusSpecificDriverOverrideProtocolGuid,
+             (VOID **) &BusSpecificDriverOverride
+             );
+  if (!EFI_ERROR (Status) && (BusSpecificDriverOverride != NULL)) {
+    DriverImageHandle = NULL;
+    do {
+      Status = BusSpecificDriverOverride->GetDriver (
+                                            BusSpecificDriverOverride,
+                                            &DriverImageHandle
+                                            );
+      if (!EFI_ERROR (Status)) {
+        AddSortedDriverBindingProtocol (
+          DriverImageHandle,
+          &NumberOfSortedDriverBindingProtocols,
+          SortedDriverBindingProtocols,
+          DriverBindingHandleCount,
+          DriverBindingHandleBuffer,
+          TRUE
+          );
+      }
+    } while (!EFI_ERROR (Status));
+  }
+
+  //
+  // Then add all the remaining Driver Binding Protocols
+  //
+  SortIndex = NumberOfSortedDriverBindingProtocols;
+  for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+    AddSortedDriverBindingProtocol (
+      DriverBindingHandleBuffer[Index],
+      &NumberOfSortedDriverBindingProtocols,
+      SortedDriverBindingProtocols,
+      DriverBindingHandleCount,
+      DriverBindingHandleBuffer,
+      FALSE
+      );
+  }
+
+  //
+  // Free the Driver Binding Handle Buffer
+  //
+  CoreFreePool (DriverBindingHandleBuffer);
+
+  //
+  // If the number of Driver Binding Protocols has increased since this function started, then return
+  // EFI_NOT_READY, so it will be restarted
+  //
+  Status = CoreLocateHandleBuffer (
+             ByProtocol,
+             &gEfiDriverBindingProtocolGuid,
+             NULL,
+             &NewDriverBindingHandleCount,
+             &NewDriverBindingHandleBuffer
+             );
+  CoreFreePool (NewDriverBindingHandleBuffer);
+  if (NewDriverBindingHandleCount > DriverBindingHandleCount) {
+    //
+    // Free any buffers that were allocated with AllocatePool()
+    //
+    CoreFreePool (SortedDriverBindingProtocols);
+
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Sort the remaining DriverBinding Protocol based on their Version field from
+  // highest to lowest.
+  //
+  for ( ; SortIndex < NumberOfSortedDriverBindingProtocols; SortIndex++) {
+    HighestVersion = SortedDriverBindingProtocols[SortIndex]->Version;
+    HighestIndex   = SortIndex;
+    for (Index = SortIndex + 1; Index < NumberOfSortedDriverBindingProtocols; Index++) {
+      if (SortedDriverBindingProtocols[Index]->Version > HighestVersion) {
+        HighestVersion = SortedDriverBindingProtocols[Index]->Version;
+        HighestIndex   = Index;
+      }
+    }
+    if (SortIndex != HighestIndex) {
+      DriverBinding = SortedDriverBindingProtocols[SortIndex];
+      SortedDriverBindingProtocols[SortIndex] = SortedDriverBindingProtocols[HighestIndex];
+      SortedDriverBindingProtocols[HighestIndex] = DriverBinding;
+    }
+  }
+
+  //
+  // Loop until no more drivers can be started on ControllerHandle
+  //
+  OneStarted = FALSE;
+  do {
+
+    //
+    // Loop through the sorted Driver Binding Protocol Instances in order, and see if
+    // any of the Driver Binding Protocols support the controller specified by
+    // ControllerHandle.
+    //
+    DriverBinding = NULL;
+    DriverFound = FALSE;
+    for (Index = 0; (Index < NumberOfSortedDriverBindingProtocols) && !DriverFound; Index++) {
+      if (SortedDriverBindingProtocols[Index] != NULL) {
+        DriverBinding = SortedDriverBindingProtocols[Index];
+        PERF_START (DriverBinding->DriverBindingHandle, "DB:Support:", NULL, 0);
+        Status = DriverBinding->Supported(
+                                  DriverBinding,
+                                  ControllerHandle,
+                                  RemainingDevicePath
+                                  );
+        PERF_END (DriverBinding->DriverBindingHandle, "DB:Support:", NULL, 0);
+        if (!EFI_ERROR (Status)) {
+          SortedDriverBindingProtocols[Index] = NULL;
+          DriverFound = TRUE;
+
+          //
+          // A driver was found that supports ControllerHandle, so attempt to start the driver
+          // on ControllerHandle.
+          //
+          PERF_START (DriverBinding->DriverBindingHandle, "DB:Start:", NULL, 0);
+          Status = DriverBinding->Start (
+                                    DriverBinding,
+                                    ControllerHandle,
+                                    RemainingDevicePath
+                                    );
+          PERF_END (DriverBinding->DriverBindingHandle, "DB:Start:", NULL, 0);
+
+          if (!EFI_ERROR (Status)) {
+            //
+            // The driver was successfully started on ControllerHandle, so set a flag
+            //
+            OneStarted = TRUE;
+          }
+        }
+      }
+    }
+  } while (DriverFound);
+
+  //
+  // Free any buffers that were allocated with AllocatePool()
+  //
+  CoreFreePool (SortedDriverBindingProtocols);
+
+  //
+  // If at least one driver was started on ControllerHandle, then return EFI_SUCCESS.
+  //
+  if (OneStarted) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If no drivers started and RemainingDevicePath is an End Device Path Node, then return EFI_SUCCESS
+  //
+  if (RemainingDevicePath != NULL) {
+    if (IsDevicePathEnd (RemainingDevicePath)) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Otherwise, no drivers were started on ControllerHandle, so return EFI_NOT_FOUND
+  //
+  return EFI_NOT_FOUND;
+}
+
+
+
+/**
+  Disonnects a controller from a driver
+
+  @param  ControllerHandle                      ControllerHandle The handle of
+                                                the controller from which
+                                                driver(s)  are to be
+                                                disconnected.
+  @param  DriverImageHandle                     DriverImageHandle The driver to
+                                                disconnect from ControllerHandle.
+  @param  ChildHandle                           ChildHandle The handle of the
+                                                child to destroy.
+
+  @retval EFI_SUCCESS                           One or more drivers were
+                                                disconnected from the controller.
+  @retval EFI_SUCCESS                           On entry, no drivers are managing
+                                                ControllerHandle.
+  @retval EFI_SUCCESS                           DriverImageHandle is not NULL,
+                                                and on entry DriverImageHandle is
+                                                not managing ControllerHandle.
+  @retval EFI_INVALID_PARAMETER                 ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER                 DriverImageHandle is not NULL,
+                                                and it is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER                 ChildHandle is not NULL, and it
+                                                is not a valid EFI_HANDLE.
+  @retval EFI_OUT_OF_RESOURCES                  There are not enough resources
+                                                available to disconnect any
+                                                drivers from ControllerHandle.
+  @retval EFI_DEVICE_ERROR                      The controller could not be
+                                                disconnected because of a device
+                                                error.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreDisconnectController (
+  IN  EFI_HANDLE  ControllerHandle,
+  IN  EFI_HANDLE  DriverImageHandle  OPTIONAL,
+  IN  EFI_HANDLE  ChildHandle        OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  IHANDLE                             *Handle;
+  EFI_HANDLE                          *DriverImageHandleBuffer;
+  EFI_HANDLE                          *ChildBuffer;
+  UINTN                               Index;
+  UINTN                               HandleIndex;
+  UINTN                               DriverImageHandleCount;
+  UINTN                               ChildrenToStop;
+  UINTN                               ChildBufferCount;
+  UINTN                               StopCount;
+  BOOLEAN                             Duplicate;
+  BOOLEAN                             ChildHandleValid;
+  BOOLEAN                             DriverImageHandleValid;
+  LIST_ENTRY                          *Link;
+  LIST_ENTRY                          *ProtLink;
+  OPEN_PROTOCOL_DATA                  *OpenData;
+  PROTOCOL_INTERFACE                  *Prot;
+  EFI_DRIVER_BINDING_PROTOCOL         *DriverBinding;
+
+  //
+  // Make sure ControllerHandle is valid
+  //
+  Status = CoreValidateHandle (ControllerHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Make sure ChildHandle is valid if it is not NULL
+  //
+  if (ChildHandle != NULL) {
+    Status = CoreValidateHandle (ChildHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  Handle = ControllerHandle;
+
+  //
+  // Get list of drivers that are currently managing ControllerHandle
+  //
+  DriverImageHandleBuffer = NULL;
+  DriverImageHandleCount  = 1;
+
+  if (DriverImageHandle == NULL) {
+    //
+    // Look at each protocol interface for a match
+    //
+    DriverImageHandleCount = 0;
+
+    CoreAcquireProtocolLock ();
+    for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      for (ProtLink = Prot->OpenList.ForwardLink;
+           ProtLink != &Prot->OpenList;
+           ProtLink = ProtLink->ForwardLink) {
+        OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+          DriverImageHandleCount++;
+        }
+      }
+    }
+    CoreReleaseProtocolLock ();
+
+    //
+    // If there are no drivers managing this controller, then return EFI_SUCCESS
+    //
+    if (DriverImageHandleCount == 0) {
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    DriverImageHandleBuffer = AllocatePool (sizeof (EFI_HANDLE) * DriverImageHandleCount);
+    if (DriverImageHandleBuffer == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    DriverImageHandleCount = 0;
+
+    CoreAcquireProtocolLock ();
+    for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      for (ProtLink = Prot->OpenList.ForwardLink;
+           ProtLink != &Prot->OpenList;
+           ProtLink = ProtLink->ForwardLink) {
+        OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+          Duplicate = FALSE;
+          for (Index = 0; Index< DriverImageHandleCount; Index++) {
+            if (DriverImageHandleBuffer[Index] == OpenData->AgentHandle) {
+              Duplicate = TRUE;
+              break;
+            }
+          }
+          if (!Duplicate) {
+            DriverImageHandleBuffer[DriverImageHandleCount] = OpenData->AgentHandle;
+            DriverImageHandleCount++;
+          }
+        }
+      }
+    }
+    CoreReleaseProtocolLock ();
+  }
+
+  StopCount = 0;
+  for (HandleIndex = 0; HandleIndex < DriverImageHandleCount; HandleIndex++) {
+
+    if (DriverImageHandleBuffer != NULL) {
+      DriverImageHandle = DriverImageHandleBuffer[HandleIndex];
+    }
+
+    //
+    // Get the Driver Binding Protocol of the driver that is managing this controller
+    //
+    Status = CoreHandleProtocol (
+               DriverImageHandle,
+               &gEfiDriverBindingProtocolGuid,
+               (VOID **)&DriverBinding
+               );
+    if (EFI_ERROR (Status) || DriverBinding == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    //
+    // Look at each protocol interface for a match
+    //
+    DriverImageHandleValid = FALSE;
+    ChildBufferCount = 0;
+
+    CoreAcquireProtocolLock ();
+    for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      for (ProtLink = Prot->OpenList.ForwardLink;
+           ProtLink != &Prot->OpenList;
+           ProtLink = ProtLink->ForwardLink) {
+        OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if (OpenData->AgentHandle == DriverImageHandle) {
+          if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+            ChildBufferCount++;
+          }
+          if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+            DriverImageHandleValid = TRUE;
+          }
+        }
+      }
+    }
+    CoreReleaseProtocolLock ();
+
+    if (DriverImageHandleValid) {
+      ChildHandleValid = FALSE;
+      ChildBuffer = NULL;
+      if (ChildBufferCount != 0) {
+        ChildBuffer = AllocatePool (sizeof (EFI_HANDLE) * ChildBufferCount);
+        if (ChildBuffer == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto Done;
+        }
+
+        ChildBufferCount = 0;
+
+        CoreAcquireProtocolLock ();
+        for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+          Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+          for (ProtLink = Prot->OpenList.ForwardLink;
+               ProtLink != &Prot->OpenList;
+               ProtLink = ProtLink->ForwardLink) {
+            OpenData = CR (ProtLink, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+            if ((OpenData->AgentHandle == DriverImageHandle) &&
+                ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0)) {
+              Duplicate = FALSE;
+              for (Index = 0; Index < ChildBufferCount; Index++) {
+                if (ChildBuffer[Index] == OpenData->ControllerHandle) {
+                  Duplicate = TRUE;
+                  break;
+                }
+              }
+              if (!Duplicate) {
+                ChildBuffer[ChildBufferCount] = OpenData->ControllerHandle;
+                if (ChildHandle == ChildBuffer[ChildBufferCount]) {
+                  ChildHandleValid = TRUE;
+                }
+                ChildBufferCount++;
+              }
+            }
+          }
+        }
+        CoreReleaseProtocolLock ();
+      }
+
+      if (ChildHandle == NULL || ChildHandleValid) {
+        ChildrenToStop = 0;
+        Status = EFI_SUCCESS;
+        if (ChildBufferCount > 0) {
+          if (ChildHandle != NULL) {
+            ChildrenToStop = 1;
+            Status = DriverBinding->Stop (DriverBinding, ControllerHandle, ChildrenToStop, &ChildHandle);
+          } else {
+            ChildrenToStop = ChildBufferCount;
+            Status = DriverBinding->Stop (DriverBinding, ControllerHandle, ChildrenToStop, ChildBuffer);
+          }
+        }
+        if (!EFI_ERROR (Status) && ((ChildHandle == NULL) || (ChildBufferCount == ChildrenToStop))) {
+          Status = DriverBinding->Stop (DriverBinding, ControllerHandle, 0, NULL);
+        }
+        if (!EFI_ERROR (Status)) {
+          StopCount++;
+        }
+      }
+
+      if (ChildBuffer != NULL) {
+        CoreFreePool (ChildBuffer);
+      }
+    }
+  }
+
+  if (StopCount > 0) {
+    Status = EFI_SUCCESS;
+  } else {
+    Status = EFI_NOT_FOUND;
+  }
+
+Done:
+
+  if (DriverImageHandleBuffer != NULL) {
+    CoreFreePool (DriverImageHandleBuffer);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Hand/Handle.c b/Core/MdeModulePkg/Core/Dxe/Hand/Handle.c
new file mode 100644
index 0000000000..1c25521672
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Hand/Handle.c
@@ -0,0 +1,1553 @@
+/** @file
+  UEFI handle & protocol handling.
+
+Copyright (c) 2006 - 2013, 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 "DxeMain.h"
+#include "Handle.h"
+
+
+//
+// mProtocolDatabase     - A list of all protocols in the system.  (simple list for now)
+// gHandleList           - A list of all the handles in the system
+// gProtocolDatabaseLock - Lock to protect the mProtocolDatabase
+// gHandleDatabaseKey    -  The Key to show that the handle has been created/modified
+//
+LIST_ENTRY      mProtocolDatabase     = INITIALIZE_LIST_HEAD_VARIABLE (mProtocolDatabase);
+LIST_ENTRY      gHandleList           = INITIALIZE_LIST_HEAD_VARIABLE (gHandleList);
+EFI_LOCK        gProtocolDatabaseLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+UINT64          gHandleDatabaseKey    = 0;
+
+
+
+/**
+  Acquire lock on gProtocolDatabaseLock.
+
+**/
+VOID
+CoreAcquireProtocolLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&gProtocolDatabaseLock);
+}
+
+
+
+/**
+  Release lock on gProtocolDatabaseLock.
+
+**/
+VOID
+CoreReleaseProtocolLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&gProtocolDatabaseLock);
+}
+
+
+
+/**
+  Check whether a handle is a valid EFI_HANDLE
+
+  @param  UserHandle             The handle to check
+
+  @retval EFI_INVALID_PARAMETER  The handle is NULL or not a valid EFI_HANDLE.
+  @retval EFI_SUCCESS            The handle is valid EFI_HANDLE.
+
+**/
+EFI_STATUS
+CoreValidateHandle (
+  IN  EFI_HANDLE                UserHandle
+  )
+{
+  IHANDLE             *Handle;
+
+  Handle = (IHANDLE *)UserHandle;
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Handle->Signature != EFI_HANDLE_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Finds the protocol entry for the requested protocol.
+  The gProtocolDatabaseLock must be owned
+
+  @param  Protocol               The ID of the protocol
+  @param  Create                 Create a new entry if not found
+
+  @return Protocol entry
+
+**/
+PROTOCOL_ENTRY  *
+CoreFindProtocolEntry (
+  IN EFI_GUID   *Protocol,
+  IN BOOLEAN    Create
+  )
+{
+  LIST_ENTRY          *Link;
+  PROTOCOL_ENTRY      *Item;
+  PROTOCOL_ENTRY      *ProtEntry;
+
+  ASSERT_LOCKED(&gProtocolDatabaseLock);
+
+  //
+  // Search the database for the matching GUID
+  //
+
+  ProtEntry = NULL;
+  for (Link = mProtocolDatabase.ForwardLink;
+       Link != &mProtocolDatabase;
+       Link = Link->ForwardLink) {
+
+    Item = CR(Link, PROTOCOL_ENTRY, AllEntries, PROTOCOL_ENTRY_SIGNATURE);
+    if (CompareGuid (&Item->ProtocolID, Protocol)) {
+
+      //
+      // This is the protocol entry
+      //
+
+      ProtEntry = Item;
+      break;
+    }
+  }
+
+  //
+  // If the protocol entry was not found and Create is TRUE, then
+  // allocate a new entry
+  //
+  if ((ProtEntry == NULL) && Create) {
+    ProtEntry = AllocatePool (sizeof(PROTOCOL_ENTRY));
+
+    if (ProtEntry != NULL) {
+      //
+      // Initialize new protocol entry structure
+      //
+      ProtEntry->Signature = PROTOCOL_ENTRY_SIGNATURE;
+      CopyGuid ((VOID *)&ProtEntry->ProtocolID, Protocol);
+      InitializeListHead (&ProtEntry->Protocols);
+      InitializeListHead (&ProtEntry->Notify);
+
+      //
+      // Add it to protocol database
+      //
+      InsertTailList (&mProtocolDatabase, &ProtEntry->AllEntries);
+    }
+  }
+
+  return ProtEntry;
+}
+
+
+
+/**
+  Finds the protocol instance for the requested handle and protocol.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  Handle                 The handle to search the protocol on
+  @param  Protocol               GUID of the protocol
+  @param  Interface              The interface for the protocol being searched
+
+  @return Protocol instance (NULL: Not found)
+
+**/
+PROTOCOL_INTERFACE *
+CoreFindProtocolInterface (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_ENTRY      *ProtEntry;
+  LIST_ENTRY          *Link;
+
+  ASSERT_LOCKED(&gProtocolDatabaseLock);
+  Prot = NULL;
+
+  //
+  // Lookup the protocol entry for this protocol ID
+  //
+
+  ProtEntry = CoreFindProtocolEntry (Protocol, FALSE);
+  if (ProtEntry != NULL) {
+
+    //
+    // Look at each protocol interface for any matches
+    //
+    for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link=Link->ForwardLink) {
+
+      //
+      // If this protocol interface matches, remove it
+      //
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      if (Prot->Interface == Interface && Prot->Protocol == ProtEntry) {
+        break;
+      }
+
+      Prot = NULL;
+    }
+  }
+
+  return Prot;
+}
+
+
+/**
+  Removes an event from a register protocol notify list on a protocol.
+
+  @param  Event                  The event to search for in the protocol
+                                 database.
+
+  @return EFI_SUCCESS   if the event was found and removed.
+  @return EFI_NOT_FOUND if the event was not found in the protocl database.
+
+**/
+EFI_STATUS
+CoreUnregisterProtocolNotifyEvent (
+  IN EFI_EVENT      Event
+  )
+{
+  LIST_ENTRY         *Link;
+  PROTOCOL_ENTRY     *ProtEntry;
+  LIST_ENTRY         *NotifyLink;
+  PROTOCOL_NOTIFY    *ProtNotify;
+
+  CoreAcquireProtocolLock ();
+
+  for ( Link =  mProtocolDatabase.ForwardLink;
+        Link != &mProtocolDatabase;
+        Link =  Link->ForwardLink) {
+
+    ProtEntry = CR(Link, PROTOCOL_ENTRY, AllEntries, PROTOCOL_ENTRY_SIGNATURE);
+
+    for ( NotifyLink =  ProtEntry->Notify.ForwardLink;
+          NotifyLink != &ProtEntry->Notify;
+          NotifyLink =  NotifyLink->ForwardLink) {
+
+      ProtNotify = CR(NotifyLink, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+
+      if (ProtNotify->Event == Event) {
+        RemoveEntryList(&ProtNotify->Link);
+        CoreFreePool(ProtNotify);
+        CoreReleaseProtocolLock ();
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  CoreReleaseProtocolLock ();
+  return EFI_NOT_FOUND;
+}
+
+
+
+/**
+  Removes all the events in the protocol database that match Event.
+
+  @param  Event                  The event to search for in the protocol
+                                 database.
+
+  @return EFI_SUCCESS when done searching the entire database.
+
+**/
+EFI_STATUS
+CoreUnregisterProtocolNotify (
+  IN EFI_EVENT      Event
+  )
+{
+  EFI_STATUS       Status;
+
+  do {
+    Status = CoreUnregisterProtocolNotifyEvent (Event);
+  } while (!EFI_ERROR (Status));
+
+  return EFI_SUCCESS;
+}
+
+
+
+
+/**
+  Wrapper function to CoreInstallProtocolInterfaceNotify.  This is the public API which
+  Calls the private one which contains a BOOLEAN parameter for notifications
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallProtocolInterface (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface
+  )
+{
+  return CoreInstallProtocolInterfaceNotify (
+            UserHandle,
+            Protocol,
+            InterfaceType,
+            Interface,
+            TRUE
+            );
+}
+
+
+/**
+  Installs a protocol interface into the boot services environment.
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+  @param  Notify                 indicates whether notify the notification list
+                                 for this protocol
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Protocol interface successfully installed
+
+**/
+EFI_STATUS
+CoreInstallProtocolInterfaceNotify (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface,
+  IN BOOLEAN            Notify
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_ENTRY      *ProtEntry;
+  IHANDLE             *Handle;
+  EFI_STATUS          Status;
+  VOID                *ExistingInterface;
+
+  //
+  // returns EFI_INVALID_PARAMETER if InterfaceType is invalid.
+  // Also added check for invalid UserHandle and Protocol pointers.
+  //
+  if (UserHandle == NULL || Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (InterfaceType != EFI_NATIVE_INTERFACE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Print debug message
+  //
+  DEBUG((DEBUG_INFO, "InstallProtocolInterface: %g %p\n", Protocol, Interface));
+
+  Status = EFI_OUT_OF_RESOURCES;
+  Prot = NULL;
+  Handle = NULL;
+
+  if (*UserHandle != NULL) {
+    Status = CoreHandleProtocol (*UserHandle, Protocol, (VOID **)&ExistingInterface);
+    if (!EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Lookup the Protocol Entry for the requested protocol
+  //
+  ProtEntry = CoreFindProtocolEntry (Protocol, TRUE);
+  if (ProtEntry == NULL) {
+    goto Done;
+  }
+
+  //
+  // Allocate a new protocol interface structure
+  //
+  Prot = AllocateZeroPool (sizeof(PROTOCOL_INTERFACE));
+  if (Prot == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // If caller didn't supply a handle, allocate a new one
+  //
+  Handle = (IHANDLE *)*UserHandle;
+  if (Handle == NULL) {
+    Handle = AllocateZeroPool (sizeof(IHANDLE));
+    if (Handle == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    //
+    // Initialize new handler structure
+    //
+    Handle->Signature = EFI_HANDLE_SIGNATURE;
+    InitializeListHead (&Handle->Protocols);
+
+    //
+    // Initialize the Key to show that the handle has been created/modified
+    //
+    gHandleDatabaseKey++;
+    Handle->Key = gHandleDatabaseKey;
+
+    //
+    // Add this handle to the list global list of all handles
+    // in the system
+    //
+    InsertTailList (&gHandleList, &Handle->AllHandles);
+  }
+
+  Status = CoreValidateHandle (Handle);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Each interface that is added must be unique
+  //
+  ASSERT (CoreFindProtocolInterface (Handle, Protocol, Interface) == NULL);
+
+  //
+  // Initialize the protocol interface structure
+  //
+  Prot->Signature = PROTOCOL_INTERFACE_SIGNATURE;
+  Prot->Handle = Handle;
+  Prot->Protocol = ProtEntry;
+  Prot->Interface = Interface;
+
+  //
+  // Initalize OpenProtocol Data base
+  //
+  InitializeListHead (&Prot->OpenList);
+  Prot->OpenListCount = 0;
+
+  //
+  // Add this protocol interface to the head of the supported
+  // protocol list for this handle
+  //
+  InsertHeadList (&Handle->Protocols, &Prot->Link);
+
+  //
+  // Add this protocol interface to the tail of the
+  // protocol entry
+  //
+  InsertTailList (&ProtEntry->Protocols, &Prot->ByProtocol);
+
+  //
+  // Notify the notification list for this protocol
+  //
+  if (Notify) {
+    CoreNotifyProtocolEntry (ProtEntry);
+  }
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Done, unlock the database and return
+  //
+  CoreReleaseProtocolLock ();
+  if (!EFI_ERROR (Status)) {
+    //
+    // Return the new handle back to the caller
+    //
+    *UserHandle = Handle;
+  } else {
+    //
+    // There was an error, clean up
+    //
+    if (Prot != NULL) {
+      CoreFreePool (Prot);
+    }
+  }
+
+  return Status;
+}
+
+
+
+
+/**
+  Installs a list of protocol interface into the boot services environment.
+  This function calls InstallProtocolInterface() in a loop. If any error
+  occures all the protocols added by this function are removed. This is
+  basically a lib function to save space.
+
+  @param  Handle                 The pointer to a handle to install the new
+                                 protocol interfaces on, or a pointer to NULL
+                                 if a new handle is to be allocated.
+  @param  ...                    EFI_GUID followed by protocol instance. A NULL
+                                 terminates the  list. The pairs are the
+                                 arguments to InstallProtocolInterface(). All the
+                                 protocols are added to Handle.
+
+  @retval EFI_SUCCESS            All the protocol interface was installed.
+  @retval EFI_OUT_OF_RESOURCES   There was not enough memory in pool to install all the protocols.
+  @retval EFI_ALREADY_STARTED    A Device Path Protocol instance was passed in that is already present in
+                                 the handle database.
+  @retval EFI_INVALID_PARAMETER  Handle is NULL.
+  @retval EFI_INVALID_PARAMETER  Protocol is already installed on the handle specified by Handle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallMultipleProtocolInterfaces (
+  IN OUT EFI_HANDLE           *Handle,
+  ...
+  )
+{
+  VA_LIST                   Args;
+  EFI_STATUS                Status;
+  EFI_GUID                  *Protocol;
+  VOID                      *Interface;
+  EFI_TPL                   OldTpl;
+  UINTN                     Index;
+  EFI_HANDLE                OldHandle;
+  EFI_HANDLE                DeviceHandle;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Syncronize with notifcations.
+  //
+  OldTpl = CoreRaiseTpl (TPL_NOTIFY);
+  OldHandle = *Handle;
+
+  //
+  // Check for duplicate device path and install the protocol interfaces
+  //
+  VA_START (Args, Handle);
+  for (Index = 0, Status = EFI_SUCCESS; !EFI_ERROR (Status); Index++) {
+    //
+    // If protocol is NULL, then it's the end of the list
+    //
+    Protocol = VA_ARG (Args, EFI_GUID *);
+    if (Protocol == NULL) {
+      break;
+    }
+
+    Interface = VA_ARG (Args, VOID *);
+
+    //
+    // Make sure you are installing on top a device path that has already been added.
+    //
+    if (CompareGuid (Protocol, &gEfiDevicePathProtocolGuid)) {
+      DeviceHandle = NULL;
+      DevicePath   = Interface;
+      Status = CoreLocateDevicePath (&gEfiDevicePathProtocolGuid, &DevicePath, &DeviceHandle);
+      if (!EFI_ERROR (Status) && (DeviceHandle != NULL) && IsDevicePathEnd(DevicePath)) {
+        Status = EFI_ALREADY_STARTED;
+        continue;
+      }
+    }
+
+    //
+    // Install it
+    //
+    Status = CoreInstallProtocolInterface (Handle, Protocol, EFI_NATIVE_INTERFACE, Interface);
+  }
+  VA_END (Args);
+
+  //
+  // If there was an error, remove all the interfaces that were installed without any errors
+  //
+  if (EFI_ERROR (Status)) {
+    //
+    // Reset the va_arg back to the first argument.
+    //
+    VA_START (Args, Handle);
+    for (; Index > 1; Index--) {
+      Protocol = VA_ARG (Args, EFI_GUID *);
+      Interface = VA_ARG (Args, VOID *);
+      CoreUninstallProtocolInterface (*Handle, Protocol, Interface);
+    }
+    VA_END (Args);
+    
+    *Handle = OldHandle;
+  }
+
+  //
+  // Done
+  //
+  CoreRestoreTpl (OldTpl);
+  return Status;
+}
+
+
+/**
+  Attempts to disconnect all drivers that are using the protocol interface being queried.
+  If failed, reconnect all drivers disconnected.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  UserHandle             The handle on which the protocol is installed
+  @param  Prot                   The protocol to disconnect drivers from
+
+  @retval EFI_SUCCESS            Drivers using the protocol interface are all
+                                 disconnected
+  @retval EFI_ACCESS_DENIED      Failed to disconnect one or all of the drivers
+
+**/
+EFI_STATUS
+CoreDisconnectControllersUsingProtocolInterface (
+  IN EFI_HANDLE           UserHandle,
+  IN PROTOCOL_INTERFACE   *Prot
+  )
+{
+  EFI_STATUS            Status;
+  BOOLEAN               ItemFound;
+  LIST_ENTRY            *Link;
+  OPEN_PROTOCOL_DATA    *OpenData;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Attempt to disconnect all drivers from this protocol interface
+  //
+  do {
+    ItemFound = FALSE;
+    for ( Link = Prot->OpenList.ForwardLink;
+          (Link != &Prot->OpenList) && !ItemFound;
+          Link = Link->ForwardLink ) {
+      OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+      if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+        ItemFound = TRUE;
+        CoreReleaseProtocolLock ();
+        Status = CoreDisconnectController (UserHandle, OpenData->AgentHandle, NULL);
+        CoreAcquireProtocolLock ();
+        if (EFI_ERROR (Status)) {
+           ItemFound = FALSE;
+           break;
+        }
+      }
+    }
+  } while (ItemFound);
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Attempt to remove BY_HANDLE_PROTOOCL and GET_PROTOCOL and TEST_PROTOCOL Open List items
+    //
+    do {
+      ItemFound = FALSE;
+      for ( Link = Prot->OpenList.ForwardLink;
+            (Link != &Prot->OpenList) && !ItemFound;
+            Link = Link->ForwardLink ) {
+        OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+        if ((OpenData->Attributes &
+            (EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL | EFI_OPEN_PROTOCOL_GET_PROTOCOL | EFI_OPEN_PROTOCOL_TEST_PROTOCOL)) != 0) {
+          ItemFound = TRUE;
+          RemoveEntryList (&OpenData->Link);
+          Prot->OpenListCount--;
+          CoreFreePool (OpenData);
+        }
+      }
+    } while (ItemFound);
+  }
+
+  //
+  // If there are errors or still has open items in the list, then reconnect all the drivers and return an error
+  //
+  if (EFI_ERROR (Status) || (Prot->OpenListCount > 0)) {
+    CoreReleaseProtocolLock ();
+    CoreConnectController (UserHandle, NULL, NULL, TRUE);
+    CoreAcquireProtocolLock ();
+    Status = EFI_ACCESS_DENIED;
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Uninstalls all instances of a protocol:interfacer from a handle.
+  If the last protocol interface is remove from the handle, the
+  handle is freed.
+
+  @param  UserHandle             The handle to remove the protocol handler from
+  @param  Protocol               The protocol, of protocol:interface, to remove
+  @param  Interface              The interface, of protocol:interface, to remove
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Protocol interface successfully uninstalled.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUninstallProtocolInterface (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  IN VOID             *Interface
+  )
+{
+  EFI_STATUS            Status;
+  IHANDLE               *Handle;
+  PROTOCOL_INTERFACE    *Prot;
+
+  //
+  // Check that Protocol is valid
+  //
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check that UserHandle is a valid handle
+  //
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Check that Protocol exists on UserHandle, and Interface matches the interface in the database
+  //
+  Prot = CoreFindProtocolInterface (UserHandle, Protocol, Interface);
+  if (Prot == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  //
+  // Attempt to disconnect all drivers that are using the protocol interface that is about to be removed
+  //
+  Status = CoreDisconnectControllersUsingProtocolInterface (
+             UserHandle,
+             Prot
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // One or more drivers refused to release, so return the error
+    //
+    goto Done;
+  }
+
+  //
+  // Remove the protocol interface from the protocol
+  //
+  Status = EFI_NOT_FOUND;
+  Handle = (IHANDLE *)UserHandle;
+  Prot   = CoreRemoveInterfaceFromProtocol (Handle, Protocol, Interface);
+
+  if (Prot != NULL) {
+    //
+    // Update the Key to show that the handle has been created/modified
+    //
+    gHandleDatabaseKey++;
+    Handle->Key = gHandleDatabaseKey;
+
+    //
+    // Remove the protocol interface from the handle
+    //
+    RemoveEntryList (&Prot->Link);
+
+    //
+    // Free the memory
+    //
+    Prot->Signature = 0;
+    CoreFreePool (Prot);
+    Status = EFI_SUCCESS;
+  }
+
+  //
+  // If there are no more handlers for the handle, free the handle
+  //
+  if (IsListEmpty (&Handle->Protocols)) {
+    Handle->Signature = 0;
+    RemoveEntryList (&Handle->AllHandles);
+    CoreFreePool (Handle);
+  }
+
+Done:
+  //
+  // Done, unlock the database and return
+  //
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+
+/**
+  Uninstalls a list of protocol interface in the boot services environment.
+  This function calls UnisatllProtocolInterface() in a loop. This is
+  basically a lib function to save space.
+
+  @param  Handle                 The handle to uninstall the protocol
+  @param  ...                    EFI_GUID followed by protocol instance. A NULL
+                                 terminates the  list. The pairs are the
+                                 arguments to UninstallProtocolInterface(). All
+                                 the protocols are added to Handle.
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUninstallMultipleProtocolInterfaces (
+  IN EFI_HANDLE           Handle,
+  ...
+  )
+{
+  EFI_STATUS      Status;
+  VA_LIST         Args;
+  EFI_GUID        *Protocol;
+  VOID            *Interface;
+  UINTN           Index;
+
+  VA_START (Args, Handle);
+  for (Index = 0, Status = EFI_SUCCESS; !EFI_ERROR (Status); Index++) {
+    //
+    // If protocol is NULL, then it's the end of the list
+    //
+    Protocol = VA_ARG (Args, EFI_GUID *);
+    if (Protocol == NULL) {
+      break;
+    }
+
+    Interface = VA_ARG (Args, VOID *);
+
+    //
+    // Uninstall it
+    //
+    Status = CoreUninstallProtocolInterface (Handle, Protocol, Interface);
+  }
+  VA_END (Args);
+
+  //
+  // If there was an error, add all the interfaces that were
+  // uninstalled without any errors
+  //
+  if (EFI_ERROR (Status)) {
+    //
+    // Reset the va_arg back to the first argument.
+    //
+    VA_START (Args, Handle);
+    for (; Index > 1; Index--) {
+      Protocol = VA_ARG(Args, EFI_GUID *);
+      Interface = VA_ARG(Args, VOID *);
+      CoreInstallProtocolInterface (&Handle, Protocol, EFI_NATIVE_INTERFACE, Interface);
+    }
+    VA_END (Args);
+  }
+
+  return Status;
+}
+
+
+/**
+  Locate a certain GUID protocol interface in a Handle's protocols.
+
+  @param  UserHandle             The handle to obtain the protocol interface on
+  @param  Protocol               The GUID of the protocol
+
+  @return The requested protocol interface for the handle
+
+**/
+PROTOCOL_INTERFACE  *
+CoreGetProtocolInterface (
+  IN  EFI_HANDLE                UserHandle,
+  IN  EFI_GUID                  *Protocol
+  )
+{
+  EFI_STATUS          Status;
+  PROTOCOL_ENTRY      *ProtEntry;
+  PROTOCOL_INTERFACE  *Prot;
+  IHANDLE             *Handle;
+  LIST_ENTRY          *Link;
+
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Handle = (IHANDLE *)UserHandle;
+
+  //
+  // Look at each protocol interface for a match
+  //
+  for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+    Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+    ProtEntry = Prot->Protocol;
+    if (CompareGuid (&ProtEntry->ProtocolID, Protocol)) {
+      return Prot;
+    }
+  }
+  return NULL;
+}
+
+
+
+/**
+  Queries a handle to determine if it supports a specified protocol.
+
+  @param  UserHandle             The handle being queried.
+  @param  Protocol               The published unique identifier of the protocol.
+  @param  Interface              Supplies the address where a pointer to the
+                                 corresponding Protocol Interface is returned.
+
+  @retval EFI_SUCCESS            The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED        The device does not support the specified protocol.
+  @retval EFI_INVALID_PARAMETER  Handle is NULL..
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_INVALID_PARAMETER  Interface is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreHandleProtocol (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  OUT VOID            **Interface
+  )
+{
+  return CoreOpenProtocol (
+          UserHandle,
+          Protocol,
+          Interface,
+          gDxeCoreImageHandle,
+          NULL,
+          EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL
+          );
+}
+
+
+
+/**
+  Locates the installed protocol handler for the handle, and
+  invokes it to obtain the protocol interface. Usage information
+  is registered in the protocol data base.
+
+  @param  UserHandle             The handle to obtain the protocol interface on
+  @param  Protocol               The ID of the protocol
+  @param  Interface              The location to return the protocol interface
+  @param  ImageHandle            The handle of the Image that is opening the
+                                 protocol interface specified by Protocol and
+                                 Interface.
+  @param  ControllerHandle       The controller handle that is requiring this
+                                 interface.
+  @param  Attributes             The open mode of the protocol interface
+                                 specified by Handle and Protocol.
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Get the protocol interface.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreOpenProtocol (
+  IN  EFI_HANDLE                UserHandle,
+  IN  EFI_GUID                  *Protocol,
+  OUT VOID                      **Interface OPTIONAL,
+  IN  EFI_HANDLE                ImageHandle,
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  UINT32                    Attributes
+  )
+{
+  EFI_STATUS          Status;
+  PROTOCOL_INTERFACE  *Prot;
+  LIST_ENTRY          *Link;
+  OPEN_PROTOCOL_DATA  *OpenData;
+  BOOLEAN             ByDriver;
+  BOOLEAN             Exclusive;
+  BOOLEAN             Disconnect;
+  BOOLEAN             ExactMatch;
+
+  //
+  // Check for invalid Protocol
+  //
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check for invalid Interface
+  //
+  if (Attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL) {
+    if (Interface == NULL) {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      *Interface = NULL;
+    }
+  }
+
+  //
+  // Check for invalid UserHandle
+  //
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check for invalid Attributes
+  //
+  switch (Attributes) {
+  case EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER :
+    Status = CoreValidateHandle (ImageHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = CoreValidateHandle (ControllerHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if (UserHandle == ControllerHandle) {
+      return EFI_INVALID_PARAMETER;
+    }
+    break;
+  case EFI_OPEN_PROTOCOL_BY_DRIVER :
+  case EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE :
+    Status = CoreValidateHandle (ImageHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = CoreValidateHandle (ControllerHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    break;
+  case EFI_OPEN_PROTOCOL_EXCLUSIVE :
+    Status = CoreValidateHandle (ImageHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    break;
+  case EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL :
+  case EFI_OPEN_PROTOCOL_GET_PROTOCOL :
+  case EFI_OPEN_PROTOCOL_TEST_PROTOCOL :
+    break;
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Look at each protocol interface for a match
+  //
+  Prot = CoreGetProtocolInterface (UserHandle, Protocol);
+  if (Prot == NULL) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  //
+  // This is the protocol interface entry for this protocol
+  //
+  if (Attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL) {
+    *Interface = Prot->Interface;
+  }
+  Status = EFI_SUCCESS;
+
+  ByDriver        = FALSE;
+  Exclusive       = FALSE;
+  for ( Link = Prot->OpenList.ForwardLink; Link != &Prot->OpenList; Link = Link->ForwardLink) {
+    OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+    ExactMatch =  (BOOLEAN)((OpenData->AgentHandle == ImageHandle) &&
+                            (OpenData->Attributes == Attributes)  &&
+                            (OpenData->ControllerHandle == ControllerHandle));
+    if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+      ByDriver = TRUE;
+      if (ExactMatch) {
+        Status = EFI_ALREADY_STARTED;
+        goto Done;
+      }
+    }
+    if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE) != 0) {
+      Exclusive = TRUE;
+    } else if (ExactMatch) {
+      OpenData->OpenCount++;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+  }
+
+  //
+  // ByDriver  TRUE  -> A driver is managing (UserHandle, Protocol)
+  // ByDriver  FALSE -> There are no drivers managing (UserHandle, Protocol)
+  // Exclusive TRUE  -> Something has exclusive access to (UserHandle, Protocol)
+  // Exclusive FALSE -> Nothing has exclusive access to (UserHandle, Protocol)
+  //
+
+  switch (Attributes) {
+  case EFI_OPEN_PROTOCOL_BY_DRIVER :
+    if (Exclusive || ByDriver) {
+      Status = EFI_ACCESS_DENIED;
+      goto Done;
+    }
+    break;
+  case EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE :
+  case EFI_OPEN_PROTOCOL_EXCLUSIVE :
+    if (Exclusive) {
+      Status = EFI_ACCESS_DENIED;
+      goto Done;
+    }
+    if (ByDriver) {
+      do {
+        Disconnect = FALSE;
+        for ( Link = Prot->OpenList.ForwardLink; (Link != &Prot->OpenList) && (!Disconnect); Link = Link->ForwardLink) {
+          OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+          if ((OpenData->Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+            Disconnect = TRUE;
+            CoreReleaseProtocolLock ();
+            Status = CoreDisconnectController (UserHandle, OpenData->AgentHandle, NULL);
+            CoreAcquireProtocolLock ();
+            if (EFI_ERROR (Status)) {
+              Status = EFI_ACCESS_DENIED;
+              goto Done;
+            }
+          }
+        }
+      } while (Disconnect);
+    }
+    break;
+  case EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER :
+  case EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL :
+  case EFI_OPEN_PROTOCOL_GET_PROTOCOL :
+  case EFI_OPEN_PROTOCOL_TEST_PROTOCOL :
+    break;
+  }
+
+  if (ImageHandle == NULL) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+  //
+  // Create new entry
+  //
+  OpenData = AllocatePool (sizeof(OPEN_PROTOCOL_DATA));
+  if (OpenData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+  } else {
+    OpenData->Signature         = OPEN_PROTOCOL_DATA_SIGNATURE;
+    OpenData->AgentHandle       = ImageHandle;
+    OpenData->ControllerHandle  = ControllerHandle;
+    OpenData->Attributes        = Attributes;
+    OpenData->OpenCount         = 1;
+    InsertTailList (&Prot->OpenList, &OpenData->Link);
+    Prot->OpenListCount++;
+    Status = EFI_SUCCESS;
+  }
+
+Done:
+  //
+  // Done. Release the database lock are return
+  //
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+/**
+  Closes a protocol on a handle that was opened using OpenProtocol().
+
+  @param  UserHandle             The handle for the protocol interface that was
+                                 previously opened with OpenProtocol(), and is
+                                 now being closed.
+  @param  Protocol               The published unique identifier of the protocol.
+                                 It is the caller's responsibility to pass in a
+                                 valid GUID.
+  @param  AgentHandle            The handle of the agent that is closing the
+                                 protocol interface.
+  @param  ControllerHandle       If the agent that opened a protocol is a driver
+                                 that follows the EFI Driver Model, then this
+                                 parameter is the controller handle that required
+                                 the protocol interface. If the agent does not
+                                 follow the EFI Driver Model, then this parameter
+                                 is optional and may be NULL.
+
+  @retval EFI_SUCCESS            The protocol instance was closed.
+  @retval EFI_INVALID_PARAMETER  Handle, AgentHandle or ControllerHandle is not a
+                                 valid EFI_HANDLE.
+  @retval EFI_NOT_FOUND          Can not find the specified protocol or
+                                 AgentHandle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreCloseProtocol (
+  IN  EFI_HANDLE                UserHandle,
+  IN  EFI_GUID                  *Protocol,
+  IN  EFI_HANDLE                AgentHandle,
+  IN  EFI_HANDLE                ControllerHandle
+  )
+{
+  EFI_STATUS          Status;
+  PROTOCOL_INTERFACE  *ProtocolInterface;
+  LIST_ENTRY          *Link;
+  OPEN_PROTOCOL_DATA  *OpenData;
+
+  //
+  // Check for invalid parameters
+  //
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = CoreValidateHandle (AgentHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  if (ControllerHandle != NULL) {
+    Status = CoreValidateHandle (ControllerHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Look at each protocol interface for a match
+  //
+  Status = EFI_NOT_FOUND;
+  ProtocolInterface = CoreGetProtocolInterface (UserHandle, Protocol);
+  if (ProtocolInterface == NULL) {
+    goto Done;
+  }
+
+  //
+  // Walk the Open data base looking for AgentHandle
+  //
+  Link = ProtocolInterface->OpenList.ForwardLink;
+  while (Link != &ProtocolInterface->OpenList) {
+    OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+    Link = Link->ForwardLink;
+    if ((OpenData->AgentHandle == AgentHandle) && (OpenData->ControllerHandle == ControllerHandle)) {
+        RemoveEntryList (&OpenData->Link);
+        ProtocolInterface->OpenListCount--;
+        CoreFreePool (OpenData);
+        Status = EFI_SUCCESS;
+    }
+  }
+
+Done:
+  //
+  // Done. Release the database lock and return.
+  //
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+
+/**
+  Return information about Opened protocols in the system
+
+  @param  UserHandle             The handle to close the protocol interface on
+  @param  Protocol               The ID of the protocol
+  @param  EntryBuffer            A pointer to a buffer of open protocol information in the
+                                 form of EFI_OPEN_PROTOCOL_INFORMATION_ENTRY structures.
+  @param  EntryCount             Number of EntryBuffer entries
+
+  @retval EFI_SUCCESS            The open protocol information was returned in EntryBuffer, 
+                                 and the number of entries was returned EntryCount.
+  @retval EFI_NOT_FOUND          Handle does not support the protocol specified by Protocol.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to allocate EntryBuffer.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreOpenProtocolInformation (
+  IN  EFI_HANDLE                          UserHandle,
+  IN  EFI_GUID                            *Protocol,
+  OUT EFI_OPEN_PROTOCOL_INFORMATION_ENTRY **EntryBuffer,
+  OUT UINTN                               *EntryCount
+  )
+{
+  EFI_STATUS                          Status;
+  PROTOCOL_INTERFACE                  *ProtocolInterface;
+  LIST_ENTRY                          *Link;
+  OPEN_PROTOCOL_DATA                  *OpenData;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY *Buffer;
+  UINTN                               Count;
+  UINTN                               Size;
+
+  *EntryBuffer = NULL;
+  *EntryCount = 0;
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Look at each protocol interface for a match
+  //
+  Status = EFI_NOT_FOUND;
+  ProtocolInterface = CoreGetProtocolInterface (UserHandle, Protocol);
+  if (ProtocolInterface == NULL) {
+    goto Done;
+  }
+
+  //
+  // Count the number of Open Entries
+  //
+  for ( Link = ProtocolInterface->OpenList.ForwardLink, Count = 0;
+        (Link != &ProtocolInterface->OpenList) ;
+        Link = Link->ForwardLink  ) {
+    Count++;
+  }
+
+  ASSERT (Count == ProtocolInterface->OpenListCount);
+
+  if (Count == 0) {
+    Size = sizeof(EFI_OPEN_PROTOCOL_INFORMATION_ENTRY);
+  } else {
+    Size = Count * sizeof(EFI_OPEN_PROTOCOL_INFORMATION_ENTRY);
+  }
+
+  Buffer = AllocatePool (Size);
+  if (Buffer == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  Status = EFI_SUCCESS;
+  for ( Link = ProtocolInterface->OpenList.ForwardLink, Count = 0;
+        (Link != &ProtocolInterface->OpenList);
+        Link = Link->ForwardLink, Count++  ) {
+    OpenData = CR (Link, OPEN_PROTOCOL_DATA, Link, OPEN_PROTOCOL_DATA_SIGNATURE);
+
+    Buffer[Count].AgentHandle      = OpenData->AgentHandle;
+    Buffer[Count].ControllerHandle = OpenData->ControllerHandle;
+    Buffer[Count].Attributes       = OpenData->Attributes;
+    Buffer[Count].OpenCount        = OpenData->OpenCount;
+  }
+
+  *EntryBuffer = Buffer;
+  *EntryCount = Count;
+
+Done:
+  //
+  // Done. Release the database lock.
+  //
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+
+/**
+  Retrieves the list of protocol interface GUIDs that are installed on a handle in a buffer allocated
+  from pool.
+
+  @param  UserHandle             The handle from which to retrieve the list of
+                                 protocol interface GUIDs.
+  @param  ProtocolBuffer         A pointer to the list of protocol interface GUID
+                                 pointers that are installed on Handle.
+  @param  ProtocolBufferCount    A pointer to the number of GUID pointers present
+                                 in ProtocolBuffer.
+
+  @retval EFI_SUCCESS            The list of protocol interface GUIDs installed
+                                 on Handle was returned in ProtocolBuffer. The
+                                 number of protocol interface GUIDs was returned
+                                 in ProtocolBufferCount.
+  @retval EFI_INVALID_PARAMETER  Handle is NULL.
+  @retval EFI_INVALID_PARAMETER  Handle is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER  ProtocolBuffer is NULL.
+  @retval EFI_INVALID_PARAMETER  ProtocolBufferCount is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the
+                                 results.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreProtocolsPerHandle (
+  IN EFI_HANDLE       UserHandle,
+  OUT EFI_GUID        ***ProtocolBuffer,
+  OUT UINTN           *ProtocolBufferCount
+  )
+{
+  EFI_STATUS                          Status;
+  IHANDLE                             *Handle;
+  PROTOCOL_INTERFACE                  *Prot;
+  LIST_ENTRY                          *Link;
+  UINTN                               ProtocolCount;
+  EFI_GUID                            **Buffer;
+
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Handle = (IHANDLE *)UserHandle;
+
+  if (ProtocolBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (ProtocolBufferCount == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *ProtocolBufferCount = 0;
+
+  ProtocolCount = 0;
+
+  CoreAcquireProtocolLock ();
+
+  for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+    ProtocolCount++;
+  }
+
+  //
+  // If there are no protocol interfaces installed on Handle, then Handle is not a valid EFI_HANDLE
+  //
+  if (ProtocolCount == 0) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  Buffer = AllocatePool (sizeof (EFI_GUID *) * ProtocolCount);
+  if (Buffer == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  *ProtocolBuffer = Buffer;
+  *ProtocolBufferCount = ProtocolCount;
+
+  for ( Link = Handle->Protocols.ForwardLink, ProtocolCount = 0;
+        Link != &Handle->Protocols;
+        Link = Link->ForwardLink, ProtocolCount++) {
+    Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+    Buffer[ProtocolCount] = &(Prot->Protocol->ProtocolID);
+  }
+  Status = EFI_SUCCESS;
+
+Done:
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+/**
+  return handle database key.
+
+
+  @return Handle database key.
+
+**/
+UINT64
+CoreGetHandleDatabaseKey (
+  VOID
+  )
+{
+  return gHandleDatabaseKey;
+}
+
+
+
+/**
+  Go connect any handles that were created or modified while a image executed.
+
+  @param  Key                    The Key to show that the handle has been
+                                 created/modified
+
+**/
+VOID
+CoreConnectHandlesByKey (
+  UINT64  Key
+  )
+{
+  UINTN           Count;
+  LIST_ENTRY      *Link;
+  EFI_HANDLE      *HandleBuffer;
+  IHANDLE         *Handle;
+  UINTN           Index;
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  for (Link = gHandleList.ForwardLink, Count = 0; Link != &gHandleList; Link = Link->ForwardLink) {
+    Handle = CR (Link, IHANDLE, AllHandles, EFI_HANDLE_SIGNATURE);
+    if (Handle->Key > Key) {
+      Count++;
+    }
+  }
+
+  HandleBuffer = AllocatePool (Count * sizeof (EFI_HANDLE));
+  if (HandleBuffer == NULL) {
+    CoreReleaseProtocolLock ();
+    return;
+  }
+
+  for (Link = gHandleList.ForwardLink, Count = 0; Link != &gHandleList; Link = Link->ForwardLink) {
+    Handle = CR (Link, IHANDLE, AllHandles, EFI_HANDLE_SIGNATURE);
+    if (Handle->Key > Key) {
+      HandleBuffer[Count++] = Handle;
+    }
+  }
+
+  //
+  // Unlock the protocol database
+  //
+  CoreReleaseProtocolLock ();
+
+  //
+  // Connect all handles whose Key value is greater than Key
+  //
+  for (Index = 0; Index < Count; Index++) {
+    CoreConnectController (HandleBuffer[Index], NULL, NULL, TRUE);
+  }
+
+  CoreFreePool(HandleBuffer);
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Hand/Handle.h b/Core/MdeModulePkg/Core/Dxe/Hand/Handle.h
new file mode 100644
index 0000000000..28f762265a
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Hand/Handle.h
@@ -0,0 +1,270 @@
+/** @file
+  Support functions for managing protocol.
+
+Copyright (c) 2006 - 2008, 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.
+
+**/
+
+#ifndef  _HAND_H_
+#define  _HAND_H_
+
+
+#define EFI_HANDLE_SIGNATURE            SIGNATURE_32('h','n','d','l')
+
+///
+/// IHANDLE - contains a list of protocol handles
+///
+typedef struct {
+  UINTN               Signature;
+  /// All handles list of IHANDLE
+  LIST_ENTRY          AllHandles;
+  /// List of PROTOCOL_INTERFACE's for this handle
+  LIST_ENTRY          Protocols;      
+  UINTN               LocateRequest;
+  /// The Handle Database Key value when this handle was last created or modified
+  UINT64              Key;
+} IHANDLE;
+
+#define ASSERT_IS_HANDLE(a)  ASSERT((a)->Signature == EFI_HANDLE_SIGNATURE)
+
+#define PROTOCOL_ENTRY_SIGNATURE        SIGNATURE_32('p','r','t','e')
+
+///
+/// PROTOCOL_ENTRY - each different protocol has 1 entry in the protocol
+/// database.  Each handler that supports this protocol is listed, along
+/// with a list of registered notifies.
+///
+typedef struct {
+  UINTN               Signature;
+  /// Link Entry inserted to mProtocolDatabase
+  LIST_ENTRY          AllEntries;  
+  /// ID of the protocol
+  EFI_GUID            ProtocolID;  
+  /// All protocol interfaces
+  LIST_ENTRY          Protocols;     
+  /// Registerd notification handlers
+  LIST_ENTRY          Notify;                 
+} PROTOCOL_ENTRY;
+
+
+#define PROTOCOL_INTERFACE_SIGNATURE  SIGNATURE_32('p','i','f','c')
+
+///
+/// PROTOCOL_INTERFACE - each protocol installed on a handle is tracked
+/// with a protocol interface structure
+///
+typedef struct {
+  UINTN                       Signature;
+  /// Link on IHANDLE.Protocols
+  LIST_ENTRY                  Link;   
+  /// Back pointer
+  IHANDLE                     *Handle;  
+  /// Link on PROTOCOL_ENTRY.Protocols
+  LIST_ENTRY                  ByProtocol; 
+  /// The protocol ID
+  PROTOCOL_ENTRY              *Protocol;  
+  /// The interface value
+  VOID                        *Interface; 
+  /// OPEN_PROTOCOL_DATA list
+  LIST_ENTRY                  OpenList;       
+  UINTN                       OpenListCount;
+
+} PROTOCOL_INTERFACE;
+
+#define OPEN_PROTOCOL_DATA_SIGNATURE  SIGNATURE_32('p','o','d','l')
+
+typedef struct {
+  UINTN                       Signature;
+  ///Link on PROTOCOL_INTERFACE.OpenList
+  LIST_ENTRY                  Link;      
+
+  EFI_HANDLE                  AgentHandle;
+  EFI_HANDLE                  ControllerHandle;
+  UINT32                      Attributes;
+  UINT32                      OpenCount;
+} OPEN_PROTOCOL_DATA;
+
+
+#define PROTOCOL_NOTIFY_SIGNATURE       SIGNATURE_32('p','r','t','n')
+
+///
+/// PROTOCOL_NOTIFY - used for each register notification for a protocol
+///
+typedef struct {
+  UINTN               Signature;
+  PROTOCOL_ENTRY      *Protocol;
+  /// All notifications for this protocol
+  LIST_ENTRY          Link;                   
+  /// Event to notify
+  EFI_EVENT           Event;    
+  /// Last position notified
+  LIST_ENTRY          *Position;              
+} PROTOCOL_NOTIFY;
+
+
+
+/**
+  Finds the protocol entry for the requested protocol.
+  The gProtocolDatabaseLock must be owned
+
+  @param  Protocol               The ID of the protocol
+  @param  Create                 Create a new entry if not found
+
+  @return Protocol entry
+
+**/
+PROTOCOL_ENTRY  *
+CoreFindProtocolEntry (
+  IN EFI_GUID   *Protocol,
+  IN BOOLEAN    Create
+  );
+
+
+/**
+  Signal event for every protocol in protocol entry.
+
+  @param  ProtEntry              Protocol entry
+
+**/
+VOID
+CoreNotifyProtocolEntry (
+  IN PROTOCOL_ENTRY   *ProtEntry
+  );
+
+
+/**
+  Finds the protocol instance for the requested handle and protocol.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  Handle                 The handle to search the protocol on
+  @param  Protocol               GUID of the protocol
+  @param  Interface              The interface for the protocol being searched
+
+  @return Protocol instance (NULL: Not found)
+
+**/
+PROTOCOL_INTERFACE *
+CoreFindProtocolInterface (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  );
+
+
+/**
+  Removes Protocol from the protocol list (but not the handle list).
+
+  @param  Handle                 The handle to remove protocol on.
+  @param  Protocol               GUID of the protocol to be moved
+  @param  Interface              The interface of the protocol
+
+  @return Protocol Entry
+
+**/
+PROTOCOL_INTERFACE *
+CoreRemoveInterfaceFromProtocol (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  );
+
+
+/**
+  Connects a controller to a driver.
+
+  @param  ControllerHandle                      Handle of the controller to be
+                                                connected.
+  @param  ContextDriverImageHandles             DriverImageHandle A pointer to an
+                                                ordered list of driver image
+                                                handles.
+  @param  RemainingDevicePath                   RemainingDevicePath A pointer to
+                                                the device path that specifies a
+                                                child  of the controller
+                                                specified by ControllerHandle.
+
+  @retval EFI_SUCCESS                           One or more drivers were
+                                                connected to ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES                  No enough system resources to
+                                                complete the request.
+  @retval EFI_NOT_FOUND                         No drivers were connected to
+                                                ControllerHandle.
+
+**/
+EFI_STATUS
+CoreConnectSingleController (
+  IN  EFI_HANDLE                ControllerHandle,
+  IN  EFI_HANDLE                *ContextDriverImageHandles OPTIONAL,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath       OPTIONAL
+  );
+
+/**
+  Attempts to disconnect all drivers that are using the protocol interface being queried.
+  If failed, reconnect all drivers disconnected.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  UserHandle             The handle on which the protocol is installed
+  @param  Prot                   The protocol to disconnect drivers from
+
+  @retval EFI_SUCCESS            Drivers using the protocol interface are all
+                                 disconnected
+  @retval EFI_ACCESS_DENIED      Failed to disconnect one or all of the drivers
+
+**/
+EFI_STATUS
+CoreDisconnectControllersUsingProtocolInterface (
+  IN EFI_HANDLE           UserHandle,
+  IN PROTOCOL_INTERFACE   *Prot
+  );
+
+
+/**
+  Acquire lock on gProtocolDatabaseLock.
+
+**/
+VOID
+CoreAcquireProtocolLock (
+  VOID
+  );
+
+
+/**
+  Release lock on gProtocolDatabaseLock.
+
+**/
+VOID
+CoreReleaseProtocolLock (
+  VOID
+  );
+
+
+/**
+  Check whether a handle is a valid EFI_HANDLE
+
+  @param  UserHandle             The handle to check
+
+  @retval EFI_INVALID_PARAMETER  The handle is NULL or not a valid EFI_HANDLE.
+  @retval EFI_SUCCESS            The handle is valid EFI_HANDLE.
+
+**/
+EFI_STATUS
+CoreValidateHandle (
+  IN  EFI_HANDLE                UserHandle
+  );
+
+//
+// Externs
+//
+extern EFI_LOCK         gProtocolDatabaseLock;
+extern LIST_ENTRY       gHandleList;
+extern UINT64           gHandleDatabaseKey;
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Hand/Locate.c b/Core/MdeModulePkg/Core/Dxe/Hand/Locate.c
new file mode 100644
index 0000000000..d98b55ab93
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Hand/Locate.c
@@ -0,0 +1,709 @@
+/** @file
+  Locate handle functions
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+#include "Handle.h"
+
+//
+// ProtocolRequest - Last LocateHandle request ID
+//
+UINTN mEfiLocateHandleRequest = 0;
+
+//
+// Internal prototypes
+//
+
+typedef struct {
+  EFI_GUID        *Protocol;
+  VOID            *SearchKey;
+  LIST_ENTRY      *Position;
+  PROTOCOL_ENTRY  *ProtEntry;
+} LOCATE_POSITION;
+
+typedef
+IHANDLE *
+(* CORE_GET_NEXT) (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  );
+
+/**
+  Routine to get the next Handle, when you are searching for all handles.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateAllHandles (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  );
+
+/**
+  Routine to get the next Handle, when you are searching for register protocol
+  notifies.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateByRegisterNotify (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  );
+
+/**
+  Routine to get the next Handle, when you are searching for a given protocol.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateByProtocol (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  );
+
+
+/**
+  Locates the requested handle(s) and returns them in Buffer.
+
+  @param  SearchType             The type of search to perform to locate the
+                                 handles
+  @param  Protocol               The protocol to search for
+  @param  SearchKey              Dependant on SearchType
+  @param  BufferSize             On input the size of Buffer.  On output the
+                                 size of data returned.
+  @param  Buffer                 The buffer to return the results in
+
+  @retval EFI_BUFFER_TOO_SMALL   Buffer too small, required buffer size is
+                                 returned in BufferSize.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully found the requested handle(s) and
+                                 returns them in Buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateHandle (
+  IN EFI_LOCATE_SEARCH_TYPE   SearchType,
+  IN EFI_GUID                 *Protocol   OPTIONAL,
+  IN VOID                     *SearchKey  OPTIONAL,
+  IN OUT UINTN                *BufferSize,
+  OUT EFI_HANDLE              *Buffer
+  )
+{
+  EFI_STATUS          Status;
+  LOCATE_POSITION     Position;
+  PROTOCOL_NOTIFY     *ProtNotify;
+  CORE_GET_NEXT       GetNext;
+  UINTN               ResultSize;
+  IHANDLE             *Handle;
+  IHANDLE             **ResultBuffer;
+  VOID                *Interface;
+
+  if (BufferSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*BufferSize > 0) && (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  GetNext = NULL;
+
+  //
+  // Set initial position
+  //
+  Position.Protocol  = Protocol;
+  Position.SearchKey = SearchKey;
+  Position.Position  = &gHandleList;
+
+  ResultSize = 0;
+  ResultBuffer = (IHANDLE **) Buffer;
+  Status = EFI_SUCCESS;
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Get the search function based on type
+  //
+  switch (SearchType) {
+  case AllHandles:
+    GetNext = CoreGetNextLocateAllHandles;
+    break;
+
+  case ByRegisterNotify:
+    //
+    // Must have SearchKey for locate ByRegisterNotify
+    //
+    if (SearchKey == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      break;
+    }
+    GetNext = CoreGetNextLocateByRegisterNotify;
+    break;
+
+  case ByProtocol:
+    GetNext = CoreGetNextLocateByProtocol;
+    if (Protocol == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      break;
+    }
+    //
+    // Look up the protocol entry and set the head pointer
+    //
+    Position.ProtEntry = CoreFindProtocolEntry (Protocol, FALSE);
+    if (Position.ProtEntry == NULL) {
+      Status = EFI_NOT_FOUND;
+      break;
+    }
+    Position.Position = &Position.ProtEntry->Protocols;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  if (EFI_ERROR(Status)) {
+    CoreReleaseProtocolLock ();
+    return Status;
+  }
+
+  ASSERT (GetNext != NULL);
+  //
+  // Enumerate out the matching handles
+  //
+  mEfiLocateHandleRequest += 1;
+  for (; ;) {
+    //
+    // Get the next handle.  If no more handles, stop
+    //
+    Handle = GetNext (&Position, &Interface);
+    if (NULL == Handle) {
+      break;
+    }
+
+    //
+    // Increase the resulting buffer size, and if this handle
+    // fits return it
+    //
+    ResultSize += sizeof(Handle);
+    if (ResultSize <= *BufferSize) {
+        *ResultBuffer = Handle;
+        ResultBuffer += 1;
+    }
+  }
+
+  //
+  // If the result is a zero length buffer, then there were no
+  // matching handles
+  //
+  if (ResultSize == 0) {
+    Status = EFI_NOT_FOUND;
+  } else {
+    //
+    // Return the resulting buffer size.  If it's larger than what
+    // was passed, then set the error code
+    //
+    if (ResultSize > *BufferSize) {
+      Status = EFI_BUFFER_TOO_SMALL;
+    }
+
+    *BufferSize = ResultSize;
+
+    if (SearchType == ByRegisterNotify && !EFI_ERROR(Status)) {
+      //
+      // If this is a search by register notify and a handle was
+      // returned, update the register notification position
+      //
+      ASSERT (SearchKey != NULL);
+      ProtNotify = SearchKey;
+      ProtNotify->Position = ProtNotify->Position->ForwardLink;
+    }
+  }
+
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+
+/**
+  Routine to get the next Handle, when you are searching for all handles.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateAllHandles (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  )
+{
+  IHANDLE     *Handle;
+
+  //
+  // Next handle
+  //
+  Position->Position = Position->Position->ForwardLink;
+
+  //
+  // If not at the end of the list, get the handle
+  //
+  Handle      = NULL;
+  *Interface  = NULL;
+  if (Position->Position != &gHandleList) {
+    Handle = CR (Position->Position, IHANDLE, AllHandles, EFI_HANDLE_SIGNATURE);
+  }
+
+  return Handle;
+}
+
+
+
+/**
+  Routine to get the next Handle, when you are searching for register protocol
+  notifies.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateByRegisterNotify (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  )
+{
+  IHANDLE             *Handle;
+  PROTOCOL_NOTIFY     *ProtNotify;
+  PROTOCOL_INTERFACE  *Prot;
+  LIST_ENTRY          *Link;
+
+  Handle      = NULL;
+  *Interface  = NULL;
+  ProtNotify = Position->SearchKey;
+
+  //
+  // If this is the first request, get the next handle
+  //
+  if (ProtNotify != NULL) {
+    ASSERT(ProtNotify->Signature == PROTOCOL_NOTIFY_SIGNATURE);
+    Position->SearchKey = NULL;
+
+    //
+    // If not at the end of the list, get the next handle
+    //
+    Link = ProtNotify->Position->ForwardLink;
+    if (Link != &ProtNotify->Protocol->Protocols) {
+      Prot = CR (Link, PROTOCOL_INTERFACE, ByProtocol, PROTOCOL_INTERFACE_SIGNATURE);
+      Handle = Prot->Handle;
+      *Interface = Prot->Interface;
+    }
+  }
+
+  return Handle;
+}
+
+
+/**
+  Routine to get the next Handle, when you are searching for a given protocol.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+CoreGetNextLocateByProtocol (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  )
+{
+  IHANDLE             *Handle;
+  LIST_ENTRY          *Link;
+  PROTOCOL_INTERFACE  *Prot;
+
+  Handle      = NULL;
+  *Interface  = NULL;
+  for (; ;) {
+    //
+    // Next entry
+    //
+    Link = Position->Position->ForwardLink;
+    Position->Position = Link;
+
+    //
+    // If not at the end, return the handle
+    //
+    if (Link == &Position->ProtEntry->Protocols) {
+      Handle = NULL;
+      break;
+    }
+
+    //
+    // Get the handle
+    //
+    Prot = CR(Link, PROTOCOL_INTERFACE, ByProtocol, PROTOCOL_INTERFACE_SIGNATURE);
+    Handle = Prot->Handle;
+    *Interface = Prot->Interface;
+
+    //
+    // If this handle has not been returned this request, then
+    // return it now
+    //
+    if (Handle->LocateRequest != mEfiLocateHandleRequest) {
+      Handle->LocateRequest = mEfiLocateHandleRequest;
+      break;
+    }
+  }
+
+  return Handle;
+}
+
+
+/**
+  Locates the handle to a device on the device path that supports the specified protocol.
+
+  @param  Protocol              Specifies the protocol to search for.
+  @param  DevicePath            On input, a pointer to a pointer to the device path. On output, the device
+                                path pointer is modified to point to the remaining part of the device
+                                path.
+  @param  Device                A pointer to the returned device handle.
+
+  @retval EFI_SUCCESS           The resulting handle was returned.
+  @retval EFI_NOT_FOUND         No handles match the search.
+  @retval EFI_INVALID_PARAMETER Protocol is NULL.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_INVALID_PARAMETER A handle matched the search and Device is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateDevicePath (
+  IN EFI_GUID                       *Protocol,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL   **DevicePath,
+  OUT EFI_HANDLE                    *Device
+  )
+{
+  INTN                        SourceSize;
+  INTN                        Size;
+  INTN                        BestMatch;
+  UINTN                       HandleCount;
+  UINTN                       Index;
+  EFI_STATUS                  Status;
+  EFI_HANDLE                  *Handles;
+  EFI_HANDLE                  Handle;
+  EFI_HANDLE                  BestDevice;
+  EFI_DEVICE_PATH_PROTOCOL    *SourcePath;
+  EFI_DEVICE_PATH_PROTOCOL    *TmpDevicePath;
+
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DevicePath == NULL) || (*DevicePath == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Handles = NULL;
+  BestDevice = NULL;
+  SourcePath = *DevicePath;
+  TmpDevicePath = SourcePath;
+  while (!IsDevicePathEnd (TmpDevicePath)) {
+    if (IsDevicePathEndInstance (TmpDevicePath)) {
+      //
+      // If DevicePath is a multi-instance device path,
+      // the function will operate on the first instance 
+      //
+      break;
+    }
+    TmpDevicePath = NextDevicePathNode (TmpDevicePath);
+  }
+
+  SourceSize = (UINTN) TmpDevicePath - (UINTN) SourcePath;
+
+  //
+  // Get a list of all handles that support the requested protocol
+  //
+  Status = CoreLocateHandleBuffer (ByProtocol, Protocol, NULL, &HandleCount, &Handles);
+  if (EFI_ERROR (Status) || HandleCount == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  BestMatch = -1;
+  for(Index = 0; Index < HandleCount; Index += 1) {
+    Handle = Handles[Index];
+    Status = CoreHandleProtocol (Handle, &gEfiDevicePathProtocolGuid, (VOID **)&TmpDevicePath);
+    if (EFI_ERROR (Status)) {
+      //
+      // If this handle doesn't support device path, then skip it
+      //
+      continue;
+    }
+
+    //
+    // Check if DevicePath is first part of SourcePath
+    //
+    Size = GetDevicePathSize (TmpDevicePath) - sizeof(EFI_DEVICE_PATH_PROTOCOL);
+    ASSERT (Size >= 0);
+    if ((Size <= SourceSize) && CompareMem (SourcePath, TmpDevicePath, (UINTN) Size) == 0) {
+      //
+      // If the size is equal to the best match, then we
+      // have a duplicate device path for 2 different device
+      // handles
+      //
+      ASSERT (Size != BestMatch);
+
+      //
+      // We've got a match, see if it's the best match so far
+      //
+      if (Size > BestMatch) {
+        BestMatch = Size;
+        BestDevice = Handle;
+      }
+    }
+  }
+
+  CoreFreePool (Handles);
+
+  //
+  // If there wasn't any match, then no parts of the device path was found.
+  // Which is strange since there is likely a "root level" device path in the system.
+  //
+  if (BestMatch == -1) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Device == NULL) {
+    return  EFI_INVALID_PARAMETER;
+  }
+  *Device = BestDevice;
+  
+  //
+  // Return the remaining part of the device path
+  //
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) (((UINT8 *) SourcePath) + BestMatch);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Return the first Protocol Interface that matches the Protocol GUID. If
+  Registration is passed in, return a Protocol Instance that was just add
+  to the system. If Registration is NULL return the first Protocol Interface
+  you find.
+
+  @param  Protocol               The protocol to search for
+  @param  Registration           Optional Registration Key returned from
+                                 RegisterProtocolNotify()
+  @param  Interface              Return the Protocol interface (instance).
+
+  @retval EFI_SUCCESS            If a valid Interface is returned
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Protocol interface not found
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateProtocol (
+  IN  EFI_GUID  *Protocol,
+  IN  VOID      *Registration OPTIONAL,
+  OUT VOID      **Interface
+  )
+{
+  EFI_STATUS              Status;
+  LOCATE_POSITION         Position;
+  PROTOCOL_NOTIFY         *ProtNotify;
+  IHANDLE                 *Handle;
+
+  if (Interface == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Protocol == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Interface = NULL;
+  Status = EFI_SUCCESS;
+
+  //
+  // Set initial position
+  //
+  Position.Protocol  = Protocol;
+  Position.SearchKey = Registration;
+  Position.Position  = &gHandleList;
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  mEfiLocateHandleRequest += 1;
+
+  if (Registration == NULL) {
+    //
+    // Look up the protocol entry and set the head pointer
+    //
+    Position.ProtEntry = CoreFindProtocolEntry (Protocol, FALSE);
+    if (Position.ProtEntry == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+    Position.Position = &Position.ProtEntry->Protocols;
+
+    Handle = CoreGetNextLocateByProtocol (&Position, Interface);
+  } else {
+    Handle = CoreGetNextLocateByRegisterNotify (&Position, Interface);
+  }
+
+  if (Handle == NULL) {
+    Status = EFI_NOT_FOUND;
+  } else if (Registration != NULL) {
+    //
+    // If this is a search by register notify and a handle was
+    // returned, update the register notification position
+    //
+    ProtNotify = Registration;
+    ProtNotify->Position = ProtNotify->Position->ForwardLink;
+  }
+
+Done:
+  CoreReleaseProtocolLock ();
+  return Status;
+}
+
+
+/**
+  Function returns an array of handles that support the requested protocol
+  in a buffer allocated from pool. This is a version of CoreLocateHandle()
+  that allocates a buffer for the caller.
+
+  @param  SearchType             Specifies which handle(s) are to be returned.
+  @param  Protocol               Provides the protocol to search by.    This
+                                 parameter is only valid for SearchType
+                                 ByProtocol.
+  @param  SearchKey              Supplies the search key depending on the
+                                 SearchType.
+  @param  NumberHandles          The number of handles returned in Buffer.
+  @param  Buffer                 A pointer to the buffer to return the requested
+                                 array of  handles that support Protocol.
+
+  @retval EFI_SUCCESS            The result array of handles was returned.
+  @retval EFI_NOT_FOUND          No handles match the search.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the
+                                 matching results.
+  @retval EFI_INVALID_PARAMETER  One or more paramters are not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLocateHandleBuffer (
+  IN EFI_LOCATE_SEARCH_TYPE       SearchType,
+  IN EFI_GUID                     *Protocol OPTIONAL,
+  IN VOID                         *SearchKey OPTIONAL,
+  IN OUT UINTN                    *NumberHandles,
+  OUT EFI_HANDLE                  **Buffer
+  )
+{
+  EFI_STATUS          Status;
+  UINTN               BufferSize;
+
+  if (NumberHandles == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BufferSize = 0;
+  *NumberHandles = 0;
+  *Buffer = NULL;
+  Status = CoreLocateHandle (
+             SearchType,
+             Protocol,
+             SearchKey,
+             &BufferSize,
+             *Buffer
+             );
+  //
+  // LocateHandleBuffer() returns incorrect status code if SearchType is
+  // invalid.
+  //
+  // Add code to correctly handle expected errors from CoreLocateHandle().
+  //
+  if (EFI_ERROR(Status) && Status != EFI_BUFFER_TOO_SMALL) {
+    if (Status != EFI_INVALID_PARAMETER) {
+      Status = EFI_NOT_FOUND;
+    }
+    return Status;
+  }
+
+  *Buffer = AllocatePool (BufferSize);
+  if (*Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = CoreLocateHandle (
+             SearchType,
+             Protocol,
+             SearchKey,
+             &BufferSize,
+             *Buffer
+             );
+
+  *NumberHandles = BufferSize / sizeof(EFI_HANDLE);
+  if (EFI_ERROR(Status)) {
+    *NumberHandles = 0;
+  }
+
+  return Status;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Hand/Notify.c b/Core/MdeModulePkg/Core/Dxe/Hand/Notify.c
new file mode 100644
index 0000000000..53780f8e6b
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Hand/Notify.c
@@ -0,0 +1,290 @@
+/** @file
+  Support functions for UEFI protocol notification infrastructure.
+
+Copyright (c) 2006 - 2008, 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 "DxeMain.h"
+#include "Handle.h"
+
+
+/**
+  Signal event for every protocol in protocol entry.
+
+  @param  ProtEntry              Protocol entry
+
+**/
+VOID
+CoreNotifyProtocolEntry (
+  IN PROTOCOL_ENTRY   *ProtEntry
+  )
+{
+  PROTOCOL_NOTIFY     *ProtNotify;
+  LIST_ENTRY          *Link;
+
+  ASSERT_LOCKED (&gProtocolDatabaseLock);
+
+  for (Link=ProtEntry->Notify.ForwardLink; Link != &ProtEntry->Notify; Link=Link->ForwardLink) {
+    ProtNotify = CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+    CoreSignalEvent (ProtNotify->Event);
+  }
+}
+
+
+
+/**
+  Removes Protocol from the protocol list (but not the handle list).
+
+  @param  Handle                 The handle to remove protocol on.
+  @param  Protocol               GUID of the protocol to be moved
+  @param  Interface              The interface of the protocol
+
+  @return Protocol Entry
+
+**/
+PROTOCOL_INTERFACE *
+CoreRemoveInterfaceFromProtocol (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_NOTIFY     *ProtNotify;
+  PROTOCOL_ENTRY      *ProtEntry;
+  LIST_ENTRY          *Link;
+
+  ASSERT_LOCKED (&gProtocolDatabaseLock);
+
+  Prot = CoreFindProtocolInterface (Handle, Protocol, Interface);
+  if (Prot != NULL) {
+
+    ProtEntry = Prot->Protocol;
+
+    //
+    // If there's a protocol notify location pointing to this entry, back it up one
+    //
+    for(Link = ProtEntry->Notify.ForwardLink; Link != &ProtEntry->Notify; Link=Link->ForwardLink) {
+      ProtNotify = CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+
+      if (ProtNotify->Position == &Prot->ByProtocol) {
+        ProtNotify->Position = Prot->ByProtocol.BackLink;
+      }
+    }
+
+    //
+    // Remove the protocol interface entry
+    //
+    RemoveEntryList (&Prot->ByProtocol);
+  }
+
+  return Prot;
+}
+
+
+/**
+  Add a new protocol notification record for the request protocol.
+
+  @param  Protocol               The requested protocol to add the notify
+                                 registration
+  @param  Event                  The event to signal
+  @param  Registration           Returns the registration record
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully returned the registration record
+                                 that has been added
+
+**/
+EFI_STATUS
+EFIAPI
+CoreRegisterProtocolNotify (
+  IN EFI_GUID       *Protocol,
+  IN EFI_EVENT      Event,
+  OUT  VOID         **Registration
+  )
+{
+  PROTOCOL_ENTRY    *ProtEntry;
+  PROTOCOL_NOTIFY   *ProtNotify;
+  EFI_STATUS        Status;
+
+  if ((Protocol == NULL) || (Event == NULL) || (Registration == NULL))  {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireProtocolLock ();
+
+  ProtNotify = NULL;
+
+  //
+  // Get the protocol entry to add the notification too
+  //
+
+  ProtEntry = CoreFindProtocolEntry (Protocol, TRUE);
+  if (ProtEntry != NULL) {
+
+    //
+    // Allocate a new notification record
+    //
+    ProtNotify = AllocatePool (sizeof(PROTOCOL_NOTIFY));
+    if (ProtNotify != NULL) {
+
+      ProtNotify->Signature = PROTOCOL_NOTIFY_SIGNATURE;
+      ProtNotify->Protocol = ProtEntry;
+      ProtNotify->Event = Event;
+      //
+      // start at the begining
+      //
+      ProtNotify->Position = &ProtEntry->Protocols;
+
+      InsertTailList (&ProtEntry->Notify, &ProtNotify->Link);
+    }
+  }
+
+  CoreReleaseProtocolLock ();
+
+  //
+  // Done.  If we have a protocol notify entry, then return it.
+  // Otherwise, we must have run out of resources trying to add one
+  //
+
+  Status = EFI_OUT_OF_RESOURCES;
+  if (ProtNotify != NULL) {
+    *Registration = ProtNotify;
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+
+/**
+  Reinstall a protocol interface on a device handle.  The OldInterface for Protocol is replaced by the NewInterface.
+
+  @param  UserHandle             Handle on which the interface is to be
+                                 reinstalled
+  @param  Protocol               The numeric ID of the interface
+  @param  OldInterface           A pointer to the old interface
+  @param  NewInterface           A pointer to the new interface
+
+  @retval EFI_SUCCESS            The protocol interface was installed
+  @retval EFI_NOT_FOUND          The OldInterface on the handle was not found
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value
+
+**/
+EFI_STATUS
+EFIAPI
+CoreReinstallProtocolInterface (
+  IN EFI_HANDLE     UserHandle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *OldInterface,
+  IN VOID           *NewInterface
+  )
+{
+  EFI_STATUS                Status;
+  IHANDLE                   *Handle;
+  PROTOCOL_INTERFACE        *Prot;
+  PROTOCOL_ENTRY            *ProtEntry;
+
+  Status = CoreValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Handle = (IHANDLE *) UserHandle;
+
+  //
+  // Lock the protocol database
+  //
+  CoreAcquireProtocolLock ();
+
+  //
+  // Check that Protocol exists on UserHandle, and Interface matches the interface in the database
+  //
+  Prot = CoreFindProtocolInterface (UserHandle, Protocol, OldInterface);
+  if (Prot == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  //
+  // Attempt to disconnect all drivers that are using the protocol interface that is about to be reinstalled
+  //
+  Status = CoreDisconnectControllersUsingProtocolInterface (
+             UserHandle,
+             Prot
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // One or more drivers refused to release, so return the error
+    //
+    goto Done;
+  }
+
+  //
+  // Remove the protocol interface from the protocol
+  //
+  Prot = CoreRemoveInterfaceFromProtocol (Handle, Protocol, OldInterface);
+
+  if (Prot == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  ProtEntry = Prot->Protocol;
+
+  //
+  // Update the interface on the protocol
+  //
+  Prot->Interface = NewInterface;
+
+  //
+  // Add this protocol interface to the tail of the
+  // protocol entry
+  //
+  InsertTailList (&ProtEntry->Protocols, &Prot->ByProtocol);
+
+  //
+  // Update the Key to show that the handle has been created/modified
+  //
+  gHandleDatabaseKey++;
+  Handle->Key = gHandleDatabaseKey;
+
+  //
+  // Release the lock and connect all drivers to UserHandle
+  //
+  CoreReleaseProtocolLock ();
+  //
+  // Return code is ignored on purpose.
+  //
+  CoreConnectController (
+    UserHandle,
+    NULL,
+    NULL,
+    TRUE
+    );
+  CoreAcquireProtocolLock ();
+
+  //
+  // Notify the notification list for this protocol
+  //
+  CoreNotifyProtocolEntry (ProtEntry);
+
+  Status = EFI_SUCCESS;
+
+Done:
+  CoreReleaseProtocolLock ();
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Image/Image.c b/Core/MdeModulePkg/Core/Dxe/Image/Image.c
new file mode 100644
index 0000000000..5355940b58
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Image/Image.c
@@ -0,0 +1,1932 @@
+/** @file
+  Core image handling services to load and unload PeImage.
+
+Copyright (c) 2006 - 2015, 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 "DxeMain.h"
+#include "Image.h"
+
+//
+// Module Globals
+//
+LOADED_IMAGE_PRIVATE_DATA  *mCurrentImage = NULL;
+
+LOAD_PE32_IMAGE_PRIVATE_DATA  mLoadPe32PrivateData = {
+  LOAD_PE32_IMAGE_PRIVATE_DATA_SIGNATURE,
+  NULL,
+  {
+    CoreLoadImageEx,
+    CoreUnloadImageEx
+  }
+};
+
+
+//
+// This code is needed to build the Image handle for the DXE Core
+//
+LOADED_IMAGE_PRIVATE_DATA mCorePrivateImage  = {
+  LOADED_IMAGE_PRIVATE_DATA_SIGNATURE,            // Signature
+  NULL,                                           // Image handle
+  EFI_IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER,    // Image type
+  TRUE,                                           // If entrypoint has been called
+  NULL, // EntryPoint
+  {
+    EFI_LOADED_IMAGE_INFORMATION_REVISION,        // Revision
+    NULL,                                         // Parent handle
+    NULL,                                         // System handle
+
+    NULL,                                         // Device handle
+    NULL,                                         // File path
+    NULL,                                         // Reserved
+
+    0,                                            // LoadOptionsSize
+    NULL,                                         // LoadOptions
+
+    NULL,                                         // ImageBase
+    0,                                            // ImageSize
+    EfiBootServicesCode,                          // ImageCodeType
+    EfiBootServicesData                           // ImageDataType
+  },
+  (EFI_PHYSICAL_ADDRESS)0,    // ImageBasePage
+  0,                          // NumberOfPages
+  NULL,                       // FixupData
+  0,                          // Tpl
+  EFI_SUCCESS,                // Status
+  0,                          // ExitDataSize
+  NULL,                       // ExitData
+  NULL,                       // JumpBuffer
+  NULL,                       // JumpContext
+  0,                          // Machine
+  NULL,                       // Ebc
+  NULL,                       // RuntimeData
+  NULL                        // LoadedImageDevicePath
+};
+//
+// The field is define for Loading modules at fixed address feature to tracker the PEI code
+// memory range usage. It is a bit mapped array in which every bit indicates the correspoding memory page
+// available or not. 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED    UINT64                *mDxeCodeMemoryRangeUsageBitMap=NULL;
+
+typedef struct {
+  UINT16  MachineType;
+  CHAR16  *MachineTypeName;
+} MACHINE_TYPE_INFO;
+
+//
+// EBC machine is not listed in this table, because EBC is in the default supported scopes of other machine type.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED MACHINE_TYPE_INFO  mMachineTypeInfo[] = {
+  {EFI_IMAGE_MACHINE_IA32,           L"IA32"},
+  {EFI_IMAGE_MACHINE_IA64,           L"IA64"},
+  {EFI_IMAGE_MACHINE_X64,            L"X64"},
+  {EFI_IMAGE_MACHINE_ARMTHUMB_MIXED, L"ARM"}
+};
+
+UINT16 mDxeCoreImageMachineType = 0;
+
+/**
+ Return machine type name.
+
+ @param MachineType The machine type
+
+ @return machine type name
+**/
+CHAR16 *
+GetMachineTypeName (
+  UINT16 MachineType
+  )
+{
+  UINTN  Index;
+  
+  for (Index = 0; Index < sizeof(mMachineTypeInfo)/sizeof(mMachineTypeInfo[0]); Index++) {
+    if (mMachineTypeInfo[Index].MachineType == MachineType) {
+      return mMachineTypeInfo[Index].MachineTypeName;
+    }
+  }
+
+  return L"<Unknown>";
+}
+
+/**
+  Add the Image Services to EFI Boot Services Table and install the protocol
+  interfaces for this image.
+
+  @param  HobStart                The HOB to initialize
+
+  @return Status code.
+
+**/
+EFI_STATUS
+CoreInitializeImageServices (
+  IN  VOID *HobStart
+  )
+{
+  EFI_STATUS                        Status;
+  LOADED_IMAGE_PRIVATE_DATA         *Image;
+  EFI_PHYSICAL_ADDRESS              DxeCoreImageBaseAddress;
+  UINT64                            DxeCoreImageLength;
+  VOID                              *DxeCoreEntryPoint;
+  EFI_PEI_HOB_POINTERS              DxeCoreHob;
+ 
+  //
+  // Searching for image hob
+  //
+  DxeCoreHob.Raw          = HobStart;
+  while ((DxeCoreHob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, DxeCoreHob.Raw)) != NULL) {
+    if (CompareGuid (&DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.Name, &gEfiHobMemoryAllocModuleGuid)) {
+      //
+      // Find Dxe Core HOB
+      //
+      break;
+    }
+    DxeCoreHob.Raw = GET_NEXT_HOB (DxeCoreHob);
+  }
+  ASSERT (DxeCoreHob.Raw != NULL);
+
+  DxeCoreImageBaseAddress = DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.MemoryBaseAddress;
+  DxeCoreImageLength      = DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.MemoryLength;
+  DxeCoreEntryPoint       = (VOID *) (UINTN) DxeCoreHob.MemoryAllocationModule->EntryPoint;
+  gDxeCoreFileName        = &DxeCoreHob.MemoryAllocationModule->ModuleName;
+  
+  //
+  // Initialize the fields for an internal driver
+  //
+  Image = &mCorePrivateImage;
+
+  Image->EntryPoint         = (EFI_IMAGE_ENTRY_POINT)(UINTN)DxeCoreEntryPoint;
+  Image->ImageBasePage      = DxeCoreImageBaseAddress;
+  Image->NumberOfPages      = (UINTN)(EFI_SIZE_TO_PAGES((UINTN)(DxeCoreImageLength)));
+  Image->Tpl                = gEfiCurrentTpl;
+  Image->Info.SystemTable   = gDxeCoreST;
+  Image->Info.ImageBase     = (VOID *)(UINTN)DxeCoreImageBaseAddress;
+  Image->Info.ImageSize     = DxeCoreImageLength;
+
+  //
+  // Install the protocol interfaces for this image
+  //
+  Status = CoreInstallProtocolInterface (
+             &Image->Handle,
+             &gEfiLoadedImageProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             &Image->Info
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  mCurrentImage = Image;
+
+  //
+  // Fill in DXE globals
+  //
+  mDxeCoreImageMachineType = PeCoffLoaderGetMachineType (Image->Info.ImageBase);
+  gDxeCoreImageHandle = Image->Handle;
+  gDxeCoreLoadedImage = &Image->Info;
+
+  if (FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    //
+    // Export DXE Core PE Loader functionality for backward compatibility.
+    //
+    Status = CoreInstallProtocolInterface (
+               &mLoadPe32PrivateData.Handle,
+               &gEfiLoadPeImageProtocolGuid,
+               EFI_NATIVE_INTERFACE,
+               &mLoadPe32PrivateData.Pe32Image
+               );
+  }
+
+  return Status;
+}
+
+/**
+  Read image file (specified by UserHandle) into user specified buffer with specified offset
+  and length.
+
+  @param  UserHandle             Image file handle
+  @param  Offset                 Offset to the source file
+  @param  ReadSize               For input, pointer of size to read; For output,
+                                 pointer of size actually read.
+  @param  Buffer                 Buffer to write into
+
+  @retval EFI_SUCCESS            Successfully read the specified part of file
+                                 into buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreReadImageFile (
+  IN     VOID    *UserHandle,
+  IN     UINTN   Offset,
+  IN OUT UINTN   *ReadSize,
+  OUT    VOID    *Buffer
+  )
+{
+  UINTN               EndPosition;
+  IMAGE_FILE_HANDLE  *FHand;
+
+  if (UserHandle == NULL || ReadSize == NULL || Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MAX_ADDRESS - Offset < *ReadSize) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FHand = (IMAGE_FILE_HANDLE  *)UserHandle;
+  ASSERT (FHand->Signature == IMAGE_FILE_HANDLE_SIGNATURE);
+
+  //
+  // Move data from our local copy of the file
+  //
+  EndPosition = Offset + *ReadSize;
+  if (EndPosition > FHand->SourceSize) {
+    *ReadSize = (UINT32)(FHand->SourceSize - Offset);
+  }
+  if (Offset >= FHand->SourceSize) {
+      *ReadSize = 0;
+  }
+
+  CopyMem (Buffer, (CHAR8 *)FHand->Source + Offset, *ReadSize);
+  return EFI_SUCCESS;
+}
+/**
+  To check memory usage bit map arry to figure out if the memory range the image will be loaded in is available or not. If 
+  memory range is avaliable, the function will mark the correponding bits to 1 which indicates the memory range is used.
+  The function is only invoked when load modules at fixed address feature is enabled. 
+  
+  @param  ImageBase                The base addres the image will be loaded at.
+  @param  ImageSize                The size of the image
+  
+  @retval EFI_SUCCESS              The memory range the image will be loaded in is available
+  @retval EFI_NOT_FOUND            The memory range the image will be loaded in is not available
+**/
+EFI_STATUS
+CheckAndMarkFixLoadingMemoryUsageBitMap (
+  IN  EFI_PHYSICAL_ADDRESS          ImageBase,
+  IN  UINTN                         ImageSize
+  )
+{
+   UINT32                             DxeCodePageNumber;
+   UINT64                             DxeCodeSize; 
+   EFI_PHYSICAL_ADDRESS               DxeCodeBase;
+   UINTN                              BaseOffsetPageNumber;
+   UINTN                              TopOffsetPageNumber;
+   UINTN                              Index;
+   //
+   // The DXE code range includes RuntimeCodePage range and Boot time code range.
+   //  
+   DxeCodePageNumber = PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber);
+   DxeCodePageNumber += PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber);
+   DxeCodeSize       = EFI_PAGES_TO_SIZE(DxeCodePageNumber);
+   DxeCodeBase       =  gLoadModuleAtFixAddressConfigurationTable.DxeCodeTopAddress - DxeCodeSize;
+   
+   //
+   // If the memory usage bit map is not initialized,  do it. Every bit in the array 
+   // indicate the status of the corresponding memory page, available or not
+   // 
+   if (mDxeCodeMemoryRangeUsageBitMap == NULL) {
+     mDxeCodeMemoryRangeUsageBitMap = AllocateZeroPool(((DxeCodePageNumber/64) + 1)*sizeof(UINT64));
+   }
+   //
+   // If the Dxe code memory range is not allocated or the bit map array allocation failed, return EFI_NOT_FOUND
+   //
+   if (!gLoadFixedAddressCodeMemoryReady || mDxeCodeMemoryRangeUsageBitMap == NULL) {
+     return EFI_NOT_FOUND;
+   }
+   //
+   // Test the memory range for loading the image in the DXE code range.
+   //
+   if (gLoadModuleAtFixAddressConfigurationTable.DxeCodeTopAddress <  ImageBase + ImageSize ||
+       DxeCodeBase >  ImageBase) {
+     return EFI_NOT_FOUND;   
+   }   
+   //
+   // Test if the memory is avalaible or not.
+   // 
+   BaseOffsetPageNumber = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase - DxeCodeBase));
+   TopOffsetPageNumber  = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase + ImageSize - DxeCodeBase));
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     if ((mDxeCodeMemoryRangeUsageBitMap[Index / 64] & LShiftU64(1, (Index % 64))) != 0) {
+       //
+       // This page is already used.
+       //
+       return EFI_NOT_FOUND;  
+     }
+   }
+   
+   //
+   // Being here means the memory range is available.  So mark the bits for the memory range
+   // 
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     mDxeCodeMemoryRangeUsageBitMap[Index / 64] |= LShiftU64(1, (Index % 64));
+   }
+   return  EFI_SUCCESS;   
+}
+/**
+
+  Get the fixed loadding address from image header assigned by build tool. This function only be called
+  when Loading module at Fixed address feature enabled.
+
+  @param  ImageContext              Pointer to the image context structure that describes the PE/COFF
+                                    image that needs to be examined by this function.
+  @retval EFI_SUCCESS               An fixed loading address is assigned to this image by build tools .
+  @retval EFI_NOT_FOUND             The image has no assigned fixed loadding address.
+
+**/
+EFI_STATUS
+GetPeCoffImageFixLoadingAssignedAddress(
+  IN OUT PE_COFF_LOADER_IMAGE_CONTEXT  *ImageContext
+  )
+{
+   UINTN                              SectionHeaderOffset;
+   EFI_STATUS                         Status;
+   EFI_IMAGE_SECTION_HEADER           SectionHeader;
+   EFI_IMAGE_OPTIONAL_HEADER_UNION    *ImgHdr;
+   UINT16                             Index;
+   UINTN                              Size;
+   UINT16                             NumberOfSections;
+   IMAGE_FILE_HANDLE                  *Handle;
+   UINT64                             ValueInSectionHeader;
+                             
+
+   Status = EFI_NOT_FOUND;
+ 
+   //
+   // Get PeHeader pointer
+   //
+   Handle = (IMAGE_FILE_HANDLE*)ImageContext->Handle;
+   ImgHdr = (EFI_IMAGE_OPTIONAL_HEADER_UNION *)((CHAR8* )Handle->Source + ImageContext->PeCoffHeaderOffset);
+   SectionHeaderOffset = (UINTN)(
+                                 ImageContext->PeCoffHeaderOffset +
+                                 sizeof (UINT32) +
+                                 sizeof (EFI_IMAGE_FILE_HEADER) +
+                                 ImgHdr->Pe32.FileHeader.SizeOfOptionalHeader
+                                 );
+   NumberOfSections = ImgHdr->Pe32.FileHeader.NumberOfSections;
+
+   //
+   // Get base address from the first section header that doesn't point to code section.
+   //
+   for (Index = 0; Index < NumberOfSections; Index++) {
+     //
+     // Read section header from file
+     //
+     Size = sizeof (EFI_IMAGE_SECTION_HEADER);
+     Status = ImageContext->ImageRead (
+                              ImageContext->Handle,
+                              SectionHeaderOffset,
+                              &Size,
+                              &SectionHeader
+                              );
+     if (EFI_ERROR (Status)) {
+       return Status;
+     }
+     if (Size != sizeof (EFI_IMAGE_SECTION_HEADER)) {
+       return EFI_NOT_FOUND;
+     }
+
+     Status = EFI_NOT_FOUND;
+     
+     if ((SectionHeader.Characteristics & EFI_IMAGE_SCN_CNT_CODE) == 0) {
+       //
+       // Build tool will save the address in PointerToRelocations & PointerToLineNumbers fields in the first section header
+       // that doesn't point to code section in image header, as well as ImageBase field of image header. And there is an 
+       // assumption that when the feature is enabled, if a module is assigned a loading address by tools, PointerToRelocations  
+       // & PointerToLineNumbers fields should NOT be Zero, or else, these 2 fileds should be set to Zero
+       //
+       ValueInSectionHeader = ReadUnaligned64((UINT64*)&SectionHeader.PointerToRelocations);
+       if (ValueInSectionHeader != 0) {
+         //
+         // When the feature is configured as load module at fixed absolute address, the ImageAddress field of ImageContext 
+         // hold the spcified address. If the feature is configured as load module at fixed offset, ImageAddress hold an offset
+         // relative to top address
+         //
+         if ((INT64)PcdGet64(PcdLoadModuleAtFixAddressEnable) < 0) {
+         	 ImageContext->ImageAddress = gLoadModuleAtFixAddressConfigurationTable.DxeCodeTopAddress + (INT64)(INTN)ImageContext->ImageAddress;
+         }
+         //
+         // Check if the memory range is avaliable.
+         //
+         Status = CheckAndMarkFixLoadingMemoryUsageBitMap (ImageContext->ImageAddress, (UINTN)(ImageContext->ImageSize + ImageContext->SectionAlignment));
+       }
+       break; 
+     }
+     SectionHeaderOffset += sizeof (EFI_IMAGE_SECTION_HEADER);
+   }
+   DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED INFO: Loading module at fixed address 0x%11p. Status = %r \n", (VOID *)(UINTN)(ImageContext->ImageAddress), Status));
+   return Status;
+}
+/**
+  Loads, relocates, and invokes a PE/COFF image
+
+  @param  BootPolicy              If TRUE, indicates that the request originates
+                                  from the boot manager, and that the boot
+                                  manager is attempting to load FilePath as a
+                                  boot selection.
+  @param  Pe32Handle              The handle of PE32 image
+  @param  Image                   PE image to be loaded
+  @param  DstBuffer               The buffer to store the image
+  @param  EntryPoint              A pointer to the entry point
+  @param  Attribute               The bit mask of attributes to set for the load
+                                  PE image
+
+  @retval EFI_SUCCESS             The file was loaded, relocated, and invoked
+  @retval EFI_OUT_OF_RESOURCES    There was not enough memory to load and
+                                  relocate the PE/COFF file
+  @retval EFI_INVALID_PARAMETER   Invalid parameter
+  @retval EFI_BUFFER_TOO_SMALL    Buffer for image is too small
+
+**/
+EFI_STATUS
+CoreLoadPeImage (
+  IN BOOLEAN                     BootPolicy,
+  IN VOID                        *Pe32Handle,
+  IN LOADED_IMAGE_PRIVATE_DATA   *Image,
+  IN EFI_PHYSICAL_ADDRESS        DstBuffer    OPTIONAL,
+  OUT EFI_PHYSICAL_ADDRESS       *EntryPoint  OPTIONAL,
+  IN  UINT32                     Attribute
+  )
+{
+  EFI_STATUS                Status;
+  BOOLEAN                   DstBufAlocated;
+  UINTN                     Size;
+
+  ZeroMem (&Image->ImageContext, sizeof (Image->ImageContext));
+
+  Image->ImageContext.Handle    = Pe32Handle;
+  Image->ImageContext.ImageRead = (PE_COFF_LOADER_READ_FILE)CoreReadImageFile;
+
+  //
+  // Get information about the image being loaded
+  //
+  Status = PeCoffLoaderGetImageInfo (&Image->ImageContext);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (Image->ImageContext.Machine)) {
+    if (!EFI_IMAGE_MACHINE_CROSS_TYPE_SUPPORTED (Image->ImageContext.Machine)) {
+      //
+      // The PE/COFF loader can support loading image types that can be executed.
+      // If we loaded an image type that we can not execute return EFI_UNSUPORTED.
+      //
+      DEBUG ((EFI_D_ERROR, "Image type %s can't be loaded ", GetMachineTypeName(Image->ImageContext.Machine)));
+      DEBUG ((EFI_D_ERROR, "on %s UEFI system.\n", GetMachineTypeName(mDxeCoreImageMachineType)));
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // Set EFI memory type based on ImageType
+  //
+  switch (Image->ImageContext.ImageType) {
+  case EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION:
+    Image->ImageContext.ImageCodeMemoryType = EfiLoaderCode;
+    Image->ImageContext.ImageDataMemoryType = EfiLoaderData;
+    break;
+  case EFI_IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER:
+    Image->ImageContext.ImageCodeMemoryType = EfiBootServicesCode;
+    Image->ImageContext.ImageDataMemoryType = EfiBootServicesData;
+    break;
+  case EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER:
+  case EFI_IMAGE_SUBSYSTEM_SAL_RUNTIME_DRIVER:
+    Image->ImageContext.ImageCodeMemoryType = EfiRuntimeServicesCode;
+    Image->ImageContext.ImageDataMemoryType = EfiRuntimeServicesData;
+    break;
+  default:
+    Image->ImageContext.ImageError = IMAGE_ERROR_INVALID_SUBSYSTEM;
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Allocate memory of the correct memory type aligned on the required image boundry
+  //
+  DstBufAlocated = FALSE;
+  if (DstBuffer == 0) {
+    //
+    // Allocate Destination Buffer as caller did not pass it in
+    //
+
+    if (Image->ImageContext.SectionAlignment > EFI_PAGE_SIZE) {
+      Size = (UINTN)Image->ImageContext.ImageSize + Image->ImageContext.SectionAlignment;
+    } else {
+      Size = (UINTN)Image->ImageContext.ImageSize;
+    }
+
+    Image->NumberOfPages = EFI_SIZE_TO_PAGES (Size);
+
+    //
+    // If the image relocations have not been stripped, then load at any address.
+    // Otherwise load at the address at which it was linked.
+    //
+    // Memory below 1MB should be treated reserved for CSM and there should be
+    // no modules whose preferred load addresses are below 1MB.
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    //
+    // If Loading Module At Fixed Address feature is enabled, the module should be loaded to
+    // a specified address.
+    //
+    if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0 ) {
+      Status = GetPeCoffImageFixLoadingAssignedAddress (&(Image->ImageContext));
+
+      if (EFI_ERROR (Status))  {
+          //
+      	  // If the code memory is not ready, invoke CoreAllocatePage with AllocateAnyPages to load the driver.
+      	  //
+          DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED ERROR: Loading module at fixed address failed since specified memory is not available.\n"));
+        
+          Status = CoreAllocatePages (
+                     AllocateAnyPages,
+                     (EFI_MEMORY_TYPE) (Image->ImageContext.ImageCodeMemoryType),
+                     Image->NumberOfPages,
+                     &Image->ImageContext.ImageAddress
+                     );         
+      } 
+    } else {
+      if (Image->ImageContext.ImageAddress >= 0x100000 || Image->ImageContext.RelocationsStripped) {
+        Status = CoreAllocatePages (
+                   AllocateAddress,
+                   (EFI_MEMORY_TYPE) (Image->ImageContext.ImageCodeMemoryType),
+                   Image->NumberOfPages,
+                   &Image->ImageContext.ImageAddress
+                   );
+      }
+      if (EFI_ERROR (Status) && !Image->ImageContext.RelocationsStripped) {
+        Status = CoreAllocatePages (
+                   AllocateAnyPages,
+                   (EFI_MEMORY_TYPE) (Image->ImageContext.ImageCodeMemoryType),
+                   Image->NumberOfPages,
+                   &Image->ImageContext.ImageAddress
+                   );
+      }
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    DstBufAlocated = TRUE;
+  } else {
+    //
+    // Caller provided the destination buffer
+    //
+
+    if (Image->ImageContext.RelocationsStripped && (Image->ImageContext.ImageAddress != DstBuffer)) {
+      //
+      // If the image relocations were stripped, and the caller provided a
+      // destination buffer address that does not match the address that the
+      // image is linked at, then the image cannot be loaded.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (Image->NumberOfPages != 0 &&
+        Image->NumberOfPages <
+        (EFI_SIZE_TO_PAGES ((UINTN)Image->ImageContext.ImageSize + Image->ImageContext.SectionAlignment))) {
+      Image->NumberOfPages = EFI_SIZE_TO_PAGES ((UINTN)Image->ImageContext.ImageSize + Image->ImageContext.SectionAlignment);
+      return EFI_BUFFER_TOO_SMALL;
+    }
+
+    Image->NumberOfPages = EFI_SIZE_TO_PAGES ((UINTN)Image->ImageContext.ImageSize + Image->ImageContext.SectionAlignment);
+    Image->ImageContext.ImageAddress = DstBuffer;
+  }
+
+  Image->ImageBasePage = Image->ImageContext.ImageAddress;
+  if (!Image->ImageContext.IsTeImage) {
+    Image->ImageContext.ImageAddress =
+        (Image->ImageContext.ImageAddress + Image->ImageContext.SectionAlignment - 1) &
+        ~((UINTN)Image->ImageContext.SectionAlignment - 1);
+  }
+
+  //
+  // Load the image from the file into the allocated memory
+  //
+  Status = PeCoffLoaderLoadImage (&Image->ImageContext);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // If this is a Runtime Driver, then allocate memory for the FixupData that
+  // is used to relocate the image when SetVirtualAddressMap() is called. The
+  // relocation is done by the Runtime AP.
+  //
+  if ((Attribute & EFI_LOAD_PE_IMAGE_ATTRIBUTE_RUNTIME_REGISTRATION) != 0) {
+    if (Image->ImageContext.ImageType == EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER) {
+      Image->ImageContext.FixupData = AllocateRuntimePool ((UINTN)(Image->ImageContext.FixupDataSize));
+      if (Image->ImageContext.FixupData == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+    }
+  }
+
+  //
+  // Relocate the image in memory
+  //
+  Status = PeCoffLoaderRelocateImage (&Image->ImageContext);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Flush the Instruction Cache
+  //
+  InvalidateInstructionCacheRange ((VOID *)(UINTN)Image->ImageContext.ImageAddress, (UINTN)Image->ImageContext.ImageSize);
+
+  //
+  // Copy the machine type from the context to the image private data. This
+  // is needed during image unload to know if we should call an EBC protocol
+  // to unload the image.
+  //
+  Image->Machine = Image->ImageContext.Machine;
+
+  //
+  // Get the image entry point. If it's an EBC image, then call into the
+  // interpreter to create a thunk for the entry point and use the returned
+  // value for the entry point.
+  //
+  Image->EntryPoint   = (EFI_IMAGE_ENTRY_POINT)(UINTN)Image->ImageContext.EntryPoint;
+  if (Image->ImageContext.Machine == EFI_IMAGE_MACHINE_EBC) {
+    //
+    // Locate the EBC interpreter protocol
+    //
+    Status = CoreLocateProtocol (&gEfiEbcProtocolGuid, NULL, (VOID **)&Image->Ebc);
+    if (EFI_ERROR(Status) || Image->Ebc == NULL) {
+      DEBUG ((DEBUG_LOAD | DEBUG_ERROR, "CoreLoadPeImage: There is no EBC interpreter for an EBC image.\n"));
+      goto Done;
+    }
+
+    //
+    // Register a callback for flushing the instruction cache so that created
+    // thunks can be flushed.
+    //
+    Status = Image->Ebc->RegisterICacheFlush (Image->Ebc, (EBC_ICACHE_FLUSH)InvalidateInstructionCacheRange);
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+
+    //
+    // Create a thunk for the image's entry point. This will be the new
+    // entry point for the image.
+    //
+    Status = Image->Ebc->CreateThunk (
+                           Image->Ebc,
+                           Image->Handle,
+                           (VOID *)(UINTN) Image->ImageContext.EntryPoint,
+                           (VOID **) &Image->EntryPoint
+                           );
+    if (EFI_ERROR(Status)) {
+      goto Done;
+    }
+  }
+
+  //
+  // Fill in the image information for the Loaded Image Protocol
+  //
+  Image->Type               = Image->ImageContext.ImageType;
+  Image->Info.ImageBase     = (VOID *)(UINTN)Image->ImageContext.ImageAddress;
+  Image->Info.ImageSize     = Image->ImageContext.ImageSize;
+  Image->Info.ImageCodeType = (EFI_MEMORY_TYPE) (Image->ImageContext.ImageCodeMemoryType);
+  Image->Info.ImageDataType = (EFI_MEMORY_TYPE) (Image->ImageContext.ImageDataMemoryType);
+  if ((Attribute & EFI_LOAD_PE_IMAGE_ATTRIBUTE_RUNTIME_REGISTRATION) != 0) {
+    if (Image->ImageContext.ImageType == EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER) {
+      //
+      // Make a list off all the RT images so we can let the RT AP know about them.
+      //
+      Image->RuntimeData = AllocateRuntimePool (sizeof(EFI_RUNTIME_IMAGE_ENTRY));
+      if (Image->RuntimeData == NULL) {
+        goto Done;
+      }
+      Image->RuntimeData->ImageBase      = Image->Info.ImageBase;
+      Image->RuntimeData->ImageSize      = (UINT64) (Image->Info.ImageSize);
+      Image->RuntimeData->RelocationData = Image->ImageContext.FixupData;
+      Image->RuntimeData->Handle         = Image->Handle;
+      InsertTailList (&gRuntime->ImageHead, &Image->RuntimeData->Link);
+      InsertImageRecord (Image->RuntimeData);
+    }
+  }
+
+  //
+  // Fill in the entry point of the image if it is available
+  //
+  if (EntryPoint != NULL) {
+    *EntryPoint = Image->ImageContext.EntryPoint;
+  }
+
+  //
+  // Print the load address and the PDB file name if it is available
+  //
+
+  DEBUG_CODE_BEGIN ();
+
+    UINTN Index;
+    UINTN StartIndex;
+    CHAR8 EfiFileName[256];
+
+
+    DEBUG ((DEBUG_INFO | DEBUG_LOAD,
+           "Loading driver at 0x%11p EntryPoint=0x%11p ",
+           (VOID *)(UINTN) Image->ImageContext.ImageAddress,
+           FUNCTION_ENTRY_POINT (Image->ImageContext.EntryPoint)));
+
+
+    //
+    // Print Module Name by Pdb file path.
+    // Windows and Unix style file path are all trimmed correctly.
+    //
+    if (Image->ImageContext.PdbPointer != NULL) {
+      StartIndex = 0;
+      for (Index = 0; Image->ImageContext.PdbPointer[Index] != 0; Index++) {
+        if ((Image->ImageContext.PdbPointer[Index] == '\\') || (Image->ImageContext.PdbPointer[Index] == '/')) {
+          StartIndex = Index + 1;
+        }
+      }
+      //
+      // Copy the PDB file name to our temporary string, and replace .pdb with .efi
+      // The PDB file name is limited in the range of 0~255.
+      // If the length is bigger than 255, trim the redudant characters to avoid overflow in array boundary.
+      //
+      for (Index = 0; Index < sizeof (EfiFileName) - 4; Index++) {
+        EfiFileName[Index] = Image->ImageContext.PdbPointer[Index + StartIndex];
+        if (EfiFileName[Index] == 0) {
+          EfiFileName[Index] = '.';
+        }
+        if (EfiFileName[Index] == '.') {
+          EfiFileName[Index + 1] = 'e';
+          EfiFileName[Index + 2] = 'f';
+          EfiFileName[Index + 3] = 'i';
+          EfiFileName[Index + 4] = 0;
+          break;
+        }
+      }
+
+      if (Index == sizeof (EfiFileName) - 4) {
+        EfiFileName[Index] = 0;
+      }
+      DEBUG ((DEBUG_INFO | DEBUG_LOAD, "%a", EfiFileName)); // &Image->ImageContext.PdbPointer[StartIndex]));
+    }
+    DEBUG ((DEBUG_INFO | DEBUG_LOAD, "\n"));
+
+  DEBUG_CODE_END ();
+
+  return EFI_SUCCESS;
+
+Done:
+
+  //
+  // Free memory.
+  //
+
+  if (DstBufAlocated) {
+    CoreFreePages (Image->ImageContext.ImageAddress, Image->NumberOfPages);
+  }
+
+  if (Image->ImageContext.FixupData != NULL) {
+    CoreFreePool (Image->ImageContext.FixupData);
+  }
+
+  return Status;
+}
+
+
+
+/**
+  Get the image's private data from its handle.
+
+  @param  ImageHandle             The image handle
+
+  @return Return the image private data associated with ImageHandle.
+
+**/
+LOADED_IMAGE_PRIVATE_DATA *
+CoreLoadedImageInfo (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_LOADED_IMAGE_PROTOCOL  *LoadedImage;
+  LOADED_IMAGE_PRIVATE_DATA  *Image;
+
+  Status = CoreHandleProtocol (
+             ImageHandle,
+             &gEfiLoadedImageProtocolGuid,
+             (VOID **)&LoadedImage
+             );
+  if (!EFI_ERROR (Status)) {
+    Image = LOADED_IMAGE_PRIVATE_DATA_FROM_THIS (LoadedImage);
+  } else {
+    DEBUG ((DEBUG_LOAD, "CoreLoadedImageInfo: Not an ImageHandle %p\n", ImageHandle));
+    Image = NULL;
+  }
+
+  return Image;
+}
+
+
+/**
+  Unloads EFI image from memory.
+
+  @param  Image                   EFI image
+  @param  FreePage                Free allocated pages
+
+**/
+VOID
+CoreUnloadAndCloseImage (
+  IN LOADED_IMAGE_PRIVATE_DATA  *Image,
+  IN BOOLEAN                    FreePage
+  )
+{
+  EFI_STATUS                          Status;
+  UINTN                               HandleCount;
+  EFI_HANDLE                          *HandleBuffer;
+  UINTN                               HandleIndex;
+  EFI_GUID                            **ProtocolGuidArray;
+  UINTN                               ArrayCount;
+  UINTN                               ProtocolIndex;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY *OpenInfo;
+  UINTN                               OpenInfoCount;
+  UINTN                               OpenInfoIndex;
+
+  HandleBuffer = NULL;
+  ProtocolGuidArray = NULL;
+
+  if (Image->Started) {
+    UnregisterMemoryProfileImage (Image);
+  }
+
+  if (Image->Ebc != NULL) {
+    //
+    // If EBC protocol exists we must perform cleanups for this image.
+    //
+    Image->Ebc->UnloadImage (Image->Ebc, Image->Handle);
+  }
+
+  //
+  // Unload image, free Image->ImageContext->ModHandle
+  //
+  PeCoffLoaderUnloadImage (&Image->ImageContext);
+
+  //
+  // Free our references to the image handle
+  //
+  if (Image->Handle != NULL) {
+
+    Status = CoreLocateHandleBuffer (
+               AllHandles,
+               NULL,
+               NULL,
+               &HandleCount,
+               &HandleBuffer
+               );
+    if (!EFI_ERROR (Status)) {
+      for (HandleIndex = 0; HandleIndex < HandleCount; HandleIndex++) {
+        Status = CoreProtocolsPerHandle (
+                   HandleBuffer[HandleIndex],
+                   &ProtocolGuidArray,
+                   &ArrayCount
+                   );
+        if (!EFI_ERROR (Status)) {
+          for (ProtocolIndex = 0; ProtocolIndex < ArrayCount; ProtocolIndex++) {
+            Status = CoreOpenProtocolInformation (
+                       HandleBuffer[HandleIndex],
+                       ProtocolGuidArray[ProtocolIndex],
+                       &OpenInfo,
+                       &OpenInfoCount
+                       );
+            if (!EFI_ERROR (Status)) {
+              for (OpenInfoIndex = 0; OpenInfoIndex < OpenInfoCount; OpenInfoIndex++) {
+                if (OpenInfo[OpenInfoIndex].AgentHandle == Image->Handle) {
+                  Status = CoreCloseProtocol (
+                             HandleBuffer[HandleIndex],
+                             ProtocolGuidArray[ProtocolIndex],
+                             Image->Handle,
+                             OpenInfo[OpenInfoIndex].ControllerHandle
+                             );
+                }
+              }
+              if (OpenInfo != NULL) {
+                CoreFreePool(OpenInfo);
+              }
+            }
+          }
+          if (ProtocolGuidArray != NULL) {
+            CoreFreePool(ProtocolGuidArray);
+          }
+        }
+      }
+      if (HandleBuffer != NULL) {
+        CoreFreePool (HandleBuffer);
+      }
+    }
+
+    CoreRemoveDebugImageInfoEntry (Image->Handle);
+
+    Status = CoreUninstallProtocolInterface (
+               Image->Handle,
+               &gEfiLoadedImageDevicePathProtocolGuid,
+               Image->LoadedImageDevicePath
+               );
+
+    Status = CoreUninstallProtocolInterface (
+               Image->Handle,
+               &gEfiLoadedImageProtocolGuid,
+               &Image->Info
+               );
+
+    if (Image->ImageContext.HiiResourceData != 0) {
+      Status = CoreUninstallProtocolInterface (
+                 Image->Handle,
+                 &gEfiHiiPackageListProtocolGuid,
+                 (VOID *) (UINTN) Image->ImageContext.HiiResourceData
+                 );
+    }
+
+  }
+
+  if (Image->RuntimeData != NULL) {
+    if (Image->RuntimeData->Link.ForwardLink != NULL) {
+      //
+      // Remove the Image from the Runtime Image list as we are about to Free it!
+      //
+      RemoveEntryList (&Image->RuntimeData->Link);
+      RemoveImageRecord (Image->RuntimeData);
+    }
+    CoreFreePool (Image->RuntimeData);
+  }
+
+  //
+  // Free the Image from memory
+  //
+  if ((Image->ImageBasePage != 0) && FreePage) {
+    CoreFreePages (Image->ImageBasePage, Image->NumberOfPages);
+  }
+
+  //
+  // Done with the Image structure
+  //
+  if (Image->Info.FilePath != NULL) {
+    CoreFreePool (Image->Info.FilePath);
+  }
+
+  if (Image->LoadedImageDevicePath != NULL) {
+    CoreFreePool (Image->LoadedImageDevicePath);
+  }
+
+  if (Image->FixupData != NULL) {
+    CoreFreePool (Image->FixupData);
+  }
+
+  CoreFreePool (Image);
+}
+
+
+/**
+  Loads an EFI image into memory and returns a handle to the image.
+
+  @param  BootPolicy              If TRUE, indicates that the request originates
+                                  from the boot manager, and that the boot
+                                  manager is attempting to load FilePath as a
+                                  boot selection.
+  @param  ParentImageHandle       The caller's image handle.
+  @param  FilePath                The specific file path from which the image is
+                                  loaded.
+  @param  SourceBuffer            If not NULL, a pointer to the memory location
+                                  containing a copy of the image to be loaded.
+  @param  SourceSize              The size in bytes of SourceBuffer.
+  @param  DstBuffer               The buffer to store the image
+  @param  NumberOfPages           If not NULL, it inputs a pointer to the page
+                                  number of DstBuffer and outputs a pointer to
+                                  the page number of the image. If this number is
+                                  not enough,  return EFI_BUFFER_TOO_SMALL and
+                                  this parameter contains the required number.
+  @param  ImageHandle             Pointer to the returned image handle that is
+                                  created when the image is successfully loaded.
+  @param  EntryPoint              A pointer to the entry point
+  @param  Attribute               The bit mask of attributes to set for the load
+                                  PE image
+
+  @retval EFI_SUCCESS             The image was loaded into memory.
+  @retval EFI_NOT_FOUND           The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_BUFFER_TOO_SMALL    The buffer is too small
+  @retval EFI_UNSUPPORTED         The image type is not supported, or the device
+                                  path cannot be parsed to locate the proper
+                                  protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES    Image was not loaded due to insufficient
+                                  resources.
+  @retval EFI_LOAD_ERROR          Image was not loaded because the image format was corrupt or not
+                                  understood.
+  @retval EFI_DEVICE_ERROR        Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED       Image was not loaded because the platform policy prohibits the 
+                                  image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION  Image was loaded and an ImageHandle was created with a 
+                                  valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                  platform policy specifies that the image should not be started.
+
+**/
+EFI_STATUS
+CoreLoadImageCommon (
+  IN  BOOLEAN                          BootPolicy,
+  IN  EFI_HANDLE                       ParentImageHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *FilePath,
+  IN  VOID                             *SourceBuffer       OPTIONAL,
+  IN  UINTN                            SourceSize,
+  IN  EFI_PHYSICAL_ADDRESS             DstBuffer           OPTIONAL,
+  IN OUT UINTN                         *NumberOfPages      OPTIONAL,
+  OUT EFI_HANDLE                       *ImageHandle,
+  OUT EFI_PHYSICAL_ADDRESS             *EntryPoint         OPTIONAL,
+  IN  UINT32                           Attribute
+  )
+{
+  LOADED_IMAGE_PRIVATE_DATA  *Image;
+  LOADED_IMAGE_PRIVATE_DATA  *ParentImage;
+  IMAGE_FILE_HANDLE          FHand;
+  EFI_STATUS                 Status;
+  EFI_STATUS                 SecurityStatus;
+  EFI_HANDLE                 DeviceHandle;
+  UINT32                     AuthenticationStatus;
+  EFI_DEVICE_PATH_PROTOCOL   *OriginalFilePath;
+  EFI_DEVICE_PATH_PROTOCOL   *HandleFilePath;
+  EFI_DEVICE_PATH_PROTOCOL   *InputFilePath;
+  EFI_DEVICE_PATH_PROTOCOL   *Node;
+  UINTN                      FilePathSize;
+  BOOLEAN                    ImageIsFromFv;
+  BOOLEAN                    ImageIsFromLoadFile;
+
+  SecurityStatus = EFI_SUCCESS;
+
+  ASSERT (gEfiCurrentTpl < TPL_NOTIFY);
+  ParentImage = NULL;
+
+  //
+  // The caller must pass in a valid ParentImageHandle
+  //
+  if (ImageHandle == NULL || ParentImageHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ParentImage = CoreLoadedImageInfo (ParentImageHandle);
+  if (ParentImage == NULL) {
+    DEBUG((DEBUG_LOAD|DEBUG_ERROR, "LoadImageEx: Parent handle not an image handle\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&FHand, sizeof (IMAGE_FILE_HANDLE));
+  FHand.Signature  = IMAGE_FILE_HANDLE_SIGNATURE;
+  OriginalFilePath = FilePath;
+  InputFilePath    = FilePath;
+  HandleFilePath   = FilePath;
+  DeviceHandle     = NULL;
+  Status           = EFI_SUCCESS;
+  AuthenticationStatus = 0;
+  ImageIsFromFv        = FALSE;
+  ImageIsFromLoadFile  = FALSE;
+
+  //
+  // If the caller passed a copy of the file, then just use it
+  //
+  if (SourceBuffer != NULL) {
+    FHand.Source     = SourceBuffer;
+    FHand.SourceSize = SourceSize;
+    Status = CoreLocateDevicePath (&gEfiDevicePathProtocolGuid, &HandleFilePath, &DeviceHandle);
+    if (EFI_ERROR (Status)) {
+      DeviceHandle = NULL;
+    }
+    if (SourceSize > 0) {
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_LOAD_ERROR;
+    }
+  } else {
+    if (FilePath == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Try to get the image device handle by checking the match protocol.
+    //
+    Node   = NULL;
+    Status = CoreLocateDevicePath (&gEfiFirmwareVolume2ProtocolGuid, &HandleFilePath, &DeviceHandle);
+    if (!EFI_ERROR (Status)) {
+      ImageIsFromFv = TRUE;
+    } else {
+      HandleFilePath = FilePath;
+      Status = CoreLocateDevicePath (&gEfiSimpleFileSystemProtocolGuid, &HandleFilePath, &DeviceHandle);
+      if (EFI_ERROR (Status)) {
+        if (!BootPolicy) {
+          HandleFilePath = FilePath;
+          Status = CoreLocateDevicePath (&gEfiLoadFile2ProtocolGuid, &HandleFilePath, &DeviceHandle);
+        }
+        if (EFI_ERROR (Status)) {
+          HandleFilePath = FilePath;
+          Status = CoreLocateDevicePath (&gEfiLoadFileProtocolGuid, &HandleFilePath, &DeviceHandle);
+          if (!EFI_ERROR (Status)) {
+            ImageIsFromLoadFile = TRUE;
+            Node = HandleFilePath;
+          }
+        }
+      }
+    }
+
+    //
+    // Get the source file buffer by its device path.
+    //
+    FHand.Source = GetFileBufferByFilePath (
+                      BootPolicy, 
+                      FilePath,
+                      &FHand.SourceSize,
+                      &AuthenticationStatus
+                      );
+    if (FHand.Source == NULL) {
+      Status = EFI_NOT_FOUND;
+    } else {
+      FHand.FreeBuffer = TRUE;
+      if (ImageIsFromLoadFile) {
+        //
+        // LoadFile () may cause the device path of the Handle be updated.
+        //
+        OriginalFilePath = AppendDevicePath (DevicePathFromHandle (DeviceHandle), Node);
+      }
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    Image = NULL;
+    goto Done;
+  }
+
+  if (gSecurity2 != NULL) {
+    //
+    // Verify File Authentication through the Security2 Architectural Protocol
+    //
+    SecurityStatus = gSecurity2->FileAuthentication (
+                                  gSecurity2,
+                                  OriginalFilePath,
+                                  FHand.Source,
+                                  FHand.SourceSize,
+                                  BootPolicy
+                                  );
+    if (!EFI_ERROR (SecurityStatus) && ImageIsFromFv) {
+      //
+      // When Security2 is installed, Security Architectural Protocol must be published.
+      //
+      ASSERT (gSecurity != NULL);
+
+      //
+      // Verify the Authentication Status through the Security Architectural Protocol
+      // Only on images that have been read using Firmware Volume protocol.
+      //
+      SecurityStatus = gSecurity->FileAuthenticationState (
+                                    gSecurity,
+                                    AuthenticationStatus,
+                                    OriginalFilePath
+                                    );
+    }
+  } else if ((gSecurity != NULL) && (OriginalFilePath != NULL)) {
+    //
+    // Verify the Authentication Status through the Security Architectural Protocol
+    //
+    SecurityStatus = gSecurity->FileAuthenticationState (
+                                  gSecurity,
+                                  AuthenticationStatus,
+                                  OriginalFilePath
+                                  );
+  }
+
+  //
+  // Check Security Status.
+  //
+  if (EFI_ERROR (SecurityStatus) && SecurityStatus != EFI_SECURITY_VIOLATION) {
+    if (SecurityStatus == EFI_ACCESS_DENIED) {
+      //
+      // Image was not loaded because the platform policy prohibits the image from being loaded.
+      // It's the only place we could meet EFI_ACCESS_DENIED.
+      //
+      *ImageHandle = NULL;
+    }
+    Status = SecurityStatus;
+    Image = NULL;
+    goto Done;
+  }
+
+  //
+  // Allocate a new image structure
+  //
+  Image = AllocateZeroPool (sizeof(LOADED_IMAGE_PRIVATE_DATA));
+  if (Image == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Pull out just the file portion of the DevicePath for the LoadedImage FilePath
+  //
+  FilePath = OriginalFilePath;
+  if (DeviceHandle != NULL) {
+    Status = CoreHandleProtocol (DeviceHandle, &gEfiDevicePathProtocolGuid, (VOID **)&HandleFilePath);
+    if (!EFI_ERROR (Status)) {
+      FilePathSize = GetDevicePathSize (HandleFilePath) - sizeof(EFI_DEVICE_PATH_PROTOCOL);
+      FilePath = (EFI_DEVICE_PATH_PROTOCOL *) (((UINT8 *)FilePath) + FilePathSize );
+    }
+  }
+  //
+  // Initialize the fields for an internal driver
+  //
+  Image->Signature         = LOADED_IMAGE_PRIVATE_DATA_SIGNATURE;
+  Image->Info.SystemTable  = gDxeCoreST;
+  Image->Info.DeviceHandle = DeviceHandle;
+  Image->Info.Revision     = EFI_LOADED_IMAGE_PROTOCOL_REVISION;
+  Image->Info.FilePath     = DuplicateDevicePath (FilePath);
+  Image->Info.ParentHandle = ParentImageHandle;
+
+
+  if (NumberOfPages != NULL) {
+    Image->NumberOfPages = *NumberOfPages ;
+  } else {
+    Image->NumberOfPages = 0 ;
+  }
+
+  //
+  // Install the protocol interfaces for this image
+  // don't fire notifications yet
+  //
+  Status = CoreInstallProtocolInterfaceNotify (
+             &Image->Handle,
+             &gEfiLoadedImageProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             &Image->Info,
+             FALSE
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Load the image.  If EntryPoint is Null, it will not be set.
+  //
+  Status = CoreLoadPeImage (BootPolicy, &FHand, Image, DstBuffer, EntryPoint, Attribute);
+  if (EFI_ERROR (Status)) {
+    if ((Status == EFI_BUFFER_TOO_SMALL) || (Status == EFI_OUT_OF_RESOURCES)) {
+      if (NumberOfPages != NULL) {
+        *NumberOfPages = Image->NumberOfPages;
+      }
+    }
+    goto Done;
+  }
+
+  if (NumberOfPages != NULL) {
+    *NumberOfPages = Image->NumberOfPages;
+  }
+
+  //
+  // Register the image in the Debug Image Info Table if the attribute is set
+  //
+  if ((Attribute & EFI_LOAD_PE_IMAGE_ATTRIBUTE_DEBUG_IMAGE_INFO_TABLE_REGISTRATION) != 0) {
+    CoreNewDebugImageInfoEntry (EFI_DEBUG_IMAGE_INFO_TYPE_NORMAL, &Image->Info, Image->Handle);
+  }
+
+  //
+  //Reinstall loaded image protocol to fire any notifications
+  //
+  Status = CoreReinstallProtocolInterface (
+             Image->Handle,
+             &gEfiLoadedImageProtocolGuid,
+             &Image->Info,
+             &Image->Info
+             );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // If DevicePath parameter to the LoadImage() is not NULL, then make a copy of DevicePath,
+  // otherwise Loaded Image Device Path Protocol is installed with a NULL interface pointer.
+  //
+  if (OriginalFilePath != NULL) {
+    Image->LoadedImageDevicePath = DuplicateDevicePath (OriginalFilePath);
+  }
+
+  //
+  // Install Loaded Image Device Path Protocol onto the image handle of a PE/COFE image
+  //
+  Status = CoreInstallProtocolInterface (
+            &Image->Handle,
+            &gEfiLoadedImageDevicePathProtocolGuid,
+            EFI_NATIVE_INTERFACE,
+            Image->LoadedImageDevicePath
+            );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Install HII Package List Protocol onto the image handle
+  //
+  if (Image->ImageContext.HiiResourceData != 0) {
+    Status = CoreInstallProtocolInterface (
+               &Image->Handle,
+               &gEfiHiiPackageListProtocolGuid,
+               EFI_NATIVE_INTERFACE,
+               (VOID *) (UINTN) Image->ImageContext.HiiResourceData
+               );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+  }
+
+  //
+  // Success.  Return the image handle
+  //
+  *ImageHandle = Image->Handle;
+
+Done:
+  //
+  // All done accessing the source file
+  // If we allocated the Source buffer, free it
+  //
+  if (FHand.FreeBuffer) {
+    CoreFreePool (FHand.Source);
+  }
+  if (OriginalFilePath != InputFilePath) {
+    CoreFreePool (OriginalFilePath);
+  }
+
+  //
+  // There was an error.  If there's an Image structure, free it
+  //
+  if (EFI_ERROR (Status)) {
+    if (Image != NULL) {
+      CoreUnloadAndCloseImage (Image, (BOOLEAN)(DstBuffer == 0));
+      Image = NULL;
+    }
+  } else if (EFI_ERROR (SecurityStatus)) {
+    Status = SecurityStatus;
+  }
+
+  //
+  // Track the return status from LoadImage.
+  //
+  if (Image != NULL) {
+    Image->LoadImageStatus = Status;
+  }
+
+  return Status;
+}
+
+
+
+
+/**
+  Loads an EFI image into memory and returns a handle to the image.
+
+  @param  BootPolicy              If TRUE, indicates that the request originates
+                                  from the boot manager, and that the boot
+                                  manager is attempting to load FilePath as a
+                                  boot selection.
+  @param  ParentImageHandle       The caller's image handle.
+  @param  FilePath                The specific file path from which the image is
+                                  loaded.
+  @param  SourceBuffer            If not NULL, a pointer to the memory location
+                                  containing a copy of the image to be loaded.
+  @param  SourceSize              The size in bytes of SourceBuffer.
+  @param  ImageHandle             Pointer to the returned image handle that is
+                                  created when the image is successfully loaded.
+
+  @retval EFI_SUCCESS             The image was loaded into memory.
+  @retval EFI_NOT_FOUND           The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_UNSUPPORTED         The image type is not supported, or the device
+                                  path cannot be parsed to locate the proper
+                                  protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES    Image was not loaded due to insufficient
+                                  resources.
+  @retval EFI_LOAD_ERROR          Image was not loaded because the image format was corrupt or not
+                                  understood.
+  @retval EFI_DEVICE_ERROR        Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED       Image was not loaded because the platform policy prohibits the 
+                                  image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION  Image was loaded and an ImageHandle was created with a 
+                                  valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                  platform policy specifies that the image should not be started.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLoadImage (
+  IN BOOLEAN                    BootPolicy,
+  IN EFI_HANDLE                 ParentImageHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL   *FilePath,
+  IN VOID                       *SourceBuffer   OPTIONAL,
+  IN UINTN                      SourceSize,
+  OUT EFI_HANDLE                *ImageHandle
+  )
+{
+  EFI_STATUS    Status;
+  UINT64        Tick;
+  EFI_HANDLE    Handle;
+
+  Tick = 0;
+  PERF_CODE (
+    Tick = GetPerformanceCounter ();
+  );
+
+  Status = CoreLoadImageCommon (
+             BootPolicy,
+             ParentImageHandle,
+             FilePath,
+             SourceBuffer,
+             SourceSize,
+             (EFI_PHYSICAL_ADDRESS) (UINTN) NULL,
+             NULL,
+             ImageHandle,
+             NULL,
+             EFI_LOAD_PE_IMAGE_ATTRIBUTE_RUNTIME_REGISTRATION | EFI_LOAD_PE_IMAGE_ATTRIBUTE_DEBUG_IMAGE_INFO_TABLE_REGISTRATION
+             );
+
+  Handle = NULL; 
+  if (!EFI_ERROR (Status)) {
+    //
+    // ImageHandle will be valid only Status is success. 
+    //
+    Handle = *ImageHandle;
+  }
+
+  PERF_START (Handle, "LoadImage:", NULL, Tick);
+  PERF_END (Handle, "LoadImage:", NULL, 0);
+
+  return Status;
+}
+
+
+
+/**
+  Loads an EFI image into memory and returns a handle to the image with extended parameters.
+
+  @param  This                    Calling context
+  @param  ParentImageHandle       The caller's image handle.
+  @param  FilePath                The specific file path from which the image is
+                                  loaded.
+  @param  SourceBuffer            If not NULL, a pointer to the memory location
+                                  containing a copy of the image to be loaded.
+  @param  SourceSize              The size in bytes of SourceBuffer.
+  @param  DstBuffer               The buffer to store the image.
+  @param  NumberOfPages           For input, specifies the space size of the
+                                  image by caller if not NULL. For output,
+                                  specifies the actual space size needed.
+  @param  ImageHandle             Image handle for output.
+  @param  EntryPoint              Image entry point for output.
+  @param  Attribute               The bit mask of attributes to set for the load
+                                  PE image.
+
+  @retval EFI_SUCCESS             The image was loaded into memory.
+  @retval EFI_NOT_FOUND           The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_UNSUPPORTED         The image type is not supported, or the device
+                                  path cannot be parsed to locate the proper
+                                  protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES    Image was not loaded due to insufficient
+                                  resources.
+  @retval EFI_LOAD_ERROR          Image was not loaded because the image format was corrupt or not
+                                  understood.
+  @retval EFI_DEVICE_ERROR        Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED       Image was not loaded because the platform policy prohibits the 
+                                  image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION  Image was loaded and an ImageHandle was created with a 
+                                  valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                  platform policy specifies that the image should not be started.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLoadImageEx (
+  IN  EFI_PE32_IMAGE_PROTOCOL          *This,
+  IN  EFI_HANDLE                       ParentImageHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *FilePath,
+  IN  VOID                             *SourceBuffer       OPTIONAL,
+  IN  UINTN                            SourceSize,
+  IN  EFI_PHYSICAL_ADDRESS             DstBuffer           OPTIONAL,
+  OUT UINTN                            *NumberOfPages      OPTIONAL,
+  OUT EFI_HANDLE                       *ImageHandle,
+  OUT EFI_PHYSICAL_ADDRESS             *EntryPoint         OPTIONAL,
+  IN  UINT32                           Attribute
+  )
+{
+  EFI_STATUS    Status;
+  UINT64        Tick;
+  EFI_HANDLE    Handle;
+
+  Tick = 0;
+  PERF_CODE (
+    Tick = GetPerformanceCounter ();
+  );
+
+  Status = CoreLoadImageCommon (
+           TRUE,
+           ParentImageHandle,
+           FilePath,
+           SourceBuffer,
+           SourceSize,
+           DstBuffer,
+           NumberOfPages,
+           ImageHandle,
+           EntryPoint,
+           Attribute
+           );
+
+  Handle = NULL; 
+  if (!EFI_ERROR (Status)) {
+    //
+    // ImageHandle will be valid only Status is success. 
+    //
+    Handle = *ImageHandle;
+  }
+
+  PERF_START (Handle, "LoadImage:", NULL, Tick);
+  PERF_END (Handle, "LoadImage:", NULL, 0);
+
+  return Status;
+}
+
+
+/**
+  Transfer control to a loaded image's entry point.
+
+  @param  ImageHandle             Handle of image to be started.
+  @param  ExitDataSize            Pointer of the size to ExitData
+  @param  ExitData                Pointer to a pointer to a data buffer that
+                                  includes a Null-terminated string,
+                                  optionally followed by additional binary data.
+                                  The string is a description that the caller may
+                                  use to further indicate the reason for the
+                                  image's exit.
+
+  @retval EFI_INVALID_PARAMETER   Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES    No enough buffer to allocate
+  @retval EFI_SECURITY_VIOLATION  The current platform policy specifies that the image should not be started.
+  @retval EFI_SUCCESS             Successfully transfer control to the image's
+                                  entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreStartImage (
+  IN EFI_HANDLE  ImageHandle,
+  OUT UINTN      *ExitDataSize,
+  OUT CHAR16     **ExitData  OPTIONAL
+  )
+{
+  EFI_STATUS                    Status;
+  LOADED_IMAGE_PRIVATE_DATA     *Image;
+  LOADED_IMAGE_PRIVATE_DATA     *LastImage;
+  UINT64                        HandleDatabaseKey;
+  UINTN                         SetJumpFlag;
+  UINT64                        Tick;
+  EFI_HANDLE                    Handle;
+
+  Tick = 0;
+  Handle = ImageHandle;
+
+  Image = CoreLoadedImageInfo (ImageHandle);
+  if (Image == NULL  ||  Image->Started) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (EFI_ERROR (Image->LoadImageStatus)) {
+    return Image->LoadImageStatus;
+  }
+
+  //
+  // The image to be started must have the machine type supported by DxeCore.
+  //
+  if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (Image->Machine)) {
+    //
+    // Do not ASSERT here, because image might be loaded via EFI_IMAGE_MACHINE_CROSS_TYPE_SUPPORTED
+    // But it can not be started.
+    //
+    DEBUG ((EFI_D_ERROR, "Image type %s can't be started ", GetMachineTypeName(Image->Machine)));
+    DEBUG ((EFI_D_ERROR, "on %s UEFI system.\n", GetMachineTypeName(mDxeCoreImageMachineType)));
+    return EFI_UNSUPPORTED;
+  }
+
+  PERF_CODE (
+    Tick = GetPerformanceCounter ();
+  );
+
+
+  //
+  // Push the current start image context, and
+  // link the current image to the head.   This is the
+  // only image that can call Exit()
+  //
+  HandleDatabaseKey = CoreGetHandleDatabaseKey ();
+  LastImage         = mCurrentImage;
+  mCurrentImage     = Image;
+  Image->Tpl        = gEfiCurrentTpl;
+
+  //
+  // Set long jump for Exit() support
+  // JumpContext must be aligned on a CPU specific boundary.
+  // Overallocate the buffer and force the required alignment
+  //
+  Image->JumpBuffer = AllocatePool (sizeof (BASE_LIBRARY_JUMP_BUFFER) + BASE_LIBRARY_JUMP_BUFFER_ALIGNMENT);
+  if (Image->JumpBuffer == NULL) {
+    //
+    // Image may be unloaded after return with failure,
+    // then ImageHandle may be invalid, so use NULL handle to record perf log.
+    //
+    PERF_START (NULL, "StartImage:", NULL, Tick);
+    PERF_END (NULL, "StartImage:", NULL, 0);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  Image->JumpContext = ALIGN_POINTER (Image->JumpBuffer, BASE_LIBRARY_JUMP_BUFFER_ALIGNMENT);
+
+  SetJumpFlag = SetJump (Image->JumpContext);
+  //
+  // The initial call to SetJump() must always return 0.
+  // Subsequent calls to LongJump() cause a non-zero value to be returned by SetJump().
+  //
+  if (SetJumpFlag == 0) {
+    RegisterMemoryProfileImage (Image, (Image->ImageContext.ImageType == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION ? EFI_FV_FILETYPE_APPLICATION : EFI_FV_FILETYPE_DRIVER));
+    //
+    // Call the image's entry point
+    //
+    Image->Started = TRUE;
+    Image->Status = Image->EntryPoint (ImageHandle, Image->Info.SystemTable);
+
+    //
+    // Add some debug information if the image returned with error.
+    // This make the user aware and check if the driver image have already released
+    // all the resource in this situation.
+    //
+    DEBUG_CODE_BEGIN ();
+      if (EFI_ERROR (Image->Status)) {
+        DEBUG ((DEBUG_ERROR, "Error: Image at %11p start failed: %r\n", Image->Info.ImageBase, Image->Status));
+      }
+    DEBUG_CODE_END ();
+
+    //
+    // If the image returns, exit it through Exit()
+    //
+    CoreExit (ImageHandle, Image->Status, 0, NULL);
+  }
+
+  //
+  // Image has completed.  Verify the tpl is the same
+  //
+  ASSERT (Image->Tpl == gEfiCurrentTpl);
+  CoreRestoreTpl (Image->Tpl);
+
+  CoreFreePool (Image->JumpBuffer);
+
+  //
+  // Pop the current start image context
+  //
+  mCurrentImage = LastImage;
+
+  //
+  // Go connect any handles that were created or modified while the image executed.
+  //
+  CoreConnectHandlesByKey (HandleDatabaseKey);
+
+  //
+  // Handle the image's returned ExitData
+  //
+  DEBUG_CODE_BEGIN ();
+    if (Image->ExitDataSize != 0 || Image->ExitData != NULL) {
+
+      DEBUG ((DEBUG_LOAD, "StartImage: ExitDataSize %d, ExitData %p", (UINT32)Image->ExitDataSize, Image->ExitData));
+      if (Image->ExitData != NULL) {
+        DEBUG ((DEBUG_LOAD, " (%hs)", Image->ExitData));
+      }
+      DEBUG ((DEBUG_LOAD, "\n"));
+    }
+  DEBUG_CODE_END ();
+
+  //
+  //  Return the exit data to the caller
+  //
+  if (ExitData != NULL && ExitDataSize != NULL) {
+    *ExitDataSize = Image->ExitDataSize;
+    *ExitData     = Image->ExitData;
+  } else {
+    //
+    // Caller doesn't want the exit data, free it
+    //
+    CoreFreePool (Image->ExitData);
+    Image->ExitData = NULL;
+  }
+
+  //
+  // Save the Status because Image will get destroyed if it is unloaded.
+  //
+  Status = Image->Status;
+
+  //
+  // If the image returned an error, or if the image is an application
+  // unload it
+  //
+  if (EFI_ERROR (Image->Status) || Image->Type == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION) {
+    CoreUnloadAndCloseImage (Image, TRUE);
+    //
+    // ImageHandle may be invalid after the image is unloaded, so use NULL handle to record perf log.
+    //
+    Handle = NULL;
+  }
+
+  //
+  // Done
+  //
+  PERF_START (Handle, "StartImage:", NULL, Tick);
+  PERF_END (Handle, "StartImage:", NULL, 0);
+  return Status;
+}
+
+/**
+  Terminates the currently loaded EFI image and returns control to boot services.
+
+  @param  ImageHandle             Handle that identifies the image. This
+                                  parameter is passed to the image on entry.
+  @param  Status                  The image's exit code.
+  @param  ExitDataSize            The size, in bytes, of ExitData. Ignored if
+                                  ExitStatus is EFI_SUCCESS.
+  @param  ExitData                Pointer to a data buffer that includes a
+                                  Null-terminated Unicode string, optionally
+                                  followed by additional binary data. The string
+                                  is a description that the caller may use to
+                                  further indicate the reason for the image's
+                                  exit.
+
+  @retval EFI_INVALID_PARAMETER   Image handle is NULL or it is not current
+                                  image.
+  @retval EFI_SUCCESS             Successfully terminates the currently loaded
+                                  EFI image.
+  @retval EFI_ACCESS_DENIED       Should never reach there.
+  @retval EFI_OUT_OF_RESOURCES    Could not allocate pool
+
+**/
+EFI_STATUS
+EFIAPI
+CoreExit (
+  IN EFI_HANDLE  ImageHandle,
+  IN EFI_STATUS  Status,
+  IN UINTN       ExitDataSize,
+  IN CHAR16      *ExitData  OPTIONAL
+  )
+{
+  LOADED_IMAGE_PRIVATE_DATA  *Image;
+  EFI_TPL                    OldTpl;
+
+  //
+  // Prevent possible reentrance to this function
+  // for the same ImageHandle
+  //
+  OldTpl = CoreRaiseTpl (TPL_NOTIFY);
+
+  Image = CoreLoadedImageInfo (ImageHandle);
+  if (Image == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (!Image->Started) {
+    //
+    // The image has not been started so just free its resources
+    //
+    CoreUnloadAndCloseImage (Image, TRUE);
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  //
+  // Image has been started, verify this image can exit
+  //
+  if (Image != mCurrentImage) {
+    DEBUG ((DEBUG_LOAD|DEBUG_ERROR, "Exit: Image is not exitable image\n"));
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // Set status
+  //
+  Image->Status = Status;
+
+  //
+  // If there's ExitData info, move it
+  //
+  if (ExitData != NULL) {
+    Image->ExitDataSize = ExitDataSize;
+    Image->ExitData = AllocatePool (Image->ExitDataSize);
+    if (Image->ExitData == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    CopyMem (Image->ExitData, ExitData, Image->ExitDataSize);
+  }
+
+  CoreRestoreTpl (OldTpl);
+  //
+  // return to StartImage
+  //
+  LongJump (Image->JumpContext, (UINTN)-1);
+
+  //
+  // If we return from LongJump, then it is an error
+  //
+  ASSERT (FALSE);
+  Status = EFI_ACCESS_DENIED;
+Done:
+  CoreRestoreTpl (OldTpl);
+  return Status;
+}
+
+
+
+
+/**
+  Unloads an image.
+
+  @param  ImageHandle             Handle that identifies the image to be
+                                  unloaded.
+
+  @retval EFI_SUCCESS             The image has been unloaded.
+  @retval EFI_UNSUPPORTED         The image has been sarted, and does not support
+                                  unload.
+  @retval EFI_INVALID_PARAMPETER  ImageHandle is not a valid image handle.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUnloadImage (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  EFI_STATUS                 Status;
+  LOADED_IMAGE_PRIVATE_DATA  *Image;
+
+  Image = CoreLoadedImageInfo (ImageHandle);
+  if (Image == NULL ) {
+    //
+    // The image handle is not valid
+    //
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  if (Image->Started) {
+    //
+    // The image has been started, request it to unload.
+    //
+    Status = EFI_UNSUPPORTED;
+    if (Image->Info.Unload != NULL) {
+      Status = Image->Info.Unload (ImageHandle);
+    }
+
+  } else {
+    //
+    // This Image hasn't been started, thus it can be unloaded
+    //
+    Status = EFI_SUCCESS;
+  }
+
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // if the Image was not started or Unloaded O.K. then clean up
+    //
+    CoreUnloadAndCloseImage (Image, TRUE);
+  }
+
+Done:
+  return Status;
+}
+
+
+
+/**
+  Unload the specified image.
+
+  @param  This                    Indicates the calling context.
+  @param  ImageHandle             The specified image handle.
+
+  @retval EFI_INVALID_PARAMETER   Image handle is NULL.
+  @retval EFI_UNSUPPORTED         Attempt to unload an unsupported image.
+  @retval EFI_SUCCESS             Image successfully unloaded.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUnloadImageEx (
+  IN EFI_PE32_IMAGE_PROTOCOL  *This,
+  IN EFI_HANDLE                         ImageHandle
+  )
+{
+  return CoreUnloadImage (ImageHandle);
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Image/Image.h b/Core/MdeModulePkg/Core/Dxe/Image/Image.h
new file mode 100644
index 0000000000..7fb8c9368e
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Image/Image.h
@@ -0,0 +1,113 @@
+/** @file
+  Data structure and functions to load and unload PeImage.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+
+#ifndef _IMAGE_H_
+#define _IMAGE_H_
+
+#define LOAD_PE32_IMAGE_PRIVATE_DATA_SIGNATURE  SIGNATURE_32('l','p','e','i')
+
+typedef struct {
+  UINTN                       Signature;
+  /// Image handle
+  EFI_HANDLE                  Handle;         
+  EFI_PE32_IMAGE_PROTOCOL     Pe32Image;
+} LOAD_PE32_IMAGE_PRIVATE_DATA;
+
+#define LOAD_PE32_IMAGE_PRIVATE_DATA_FROM_THIS(a) \
+          CR(a, LOAD_PE32_IMAGE_PRIVATE_DATA, Pe32Image, LOAD_PE32_IMAGE_PRIVATE_DATA_SIGNATURE)
+
+
+//
+// Private Data Types
+//
+#define IMAGE_FILE_HANDLE_SIGNATURE       SIGNATURE_32('i','m','g','f')
+typedef struct {
+  UINTN               Signature;
+  BOOLEAN             FreeBuffer;
+  VOID                *Source;
+  UINTN               SourceSize;
+} IMAGE_FILE_HANDLE;
+
+/**
+  Loads an EFI image into memory and returns a handle to the image with extended parameters.
+
+  @param  This                    Calling context
+  @param  ParentImageHandle       The caller's image handle.
+  @param  FilePath                The specific file path from which the image is
+                                  loaded.
+  @param  SourceBuffer            If not NULL, a pointer to the memory location
+                                  containing a copy of the image to be loaded.
+  @param  SourceSize              The size in bytes of SourceBuffer.
+  @param  DstBuffer               The buffer to store the image.
+  @param  NumberOfPages           For input, specifies the space size of the
+                                  image by caller if not NULL. For output,
+                                  specifies the actual space size needed.
+  @param  ImageHandle             Image handle for output.
+  @param  EntryPoint              Image entry point for output.
+  @param  Attribute               The bit mask of attributes to set for the load
+                                  PE image.
+
+  @retval EFI_SUCCESS             The image was loaded into memory.
+  @retval EFI_NOT_FOUND           The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_UNSUPPORTED         The image type is not supported, or the device
+                                  path cannot be parsed to locate the proper
+                                  protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES    Image was not loaded due to insufficient
+                                  resources.
+  @retval EFI_LOAD_ERROR          Image was not loaded because the image format was corrupt or not
+                                  understood.
+  @retval EFI_DEVICE_ERROR        Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED       Image was not loaded because the platform policy prohibits the 
+                                  image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION  Image was loaded and an ImageHandle was created with a 
+                                  valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                  platform policy specifies that the image should not be started.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreLoadImageEx (
+  IN  EFI_PE32_IMAGE_PROTOCOL          *This,
+  IN  EFI_HANDLE                       ParentImageHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *FilePath,
+  IN  VOID                             *SourceBuffer       OPTIONAL,
+  IN  UINTN                            SourceSize,
+  IN  EFI_PHYSICAL_ADDRESS             DstBuffer           OPTIONAL,
+  OUT UINTN                            *NumberOfPages      OPTIONAL,
+  OUT EFI_HANDLE                       *ImageHandle,
+  OUT EFI_PHYSICAL_ADDRESS             *EntryPoint         OPTIONAL,
+  IN  UINT32                           Attribute
+  );
+
+
+/**
+  Unload the specified image.
+
+  @param  This                    Indicates the calling context.
+  @param  ImageHandle             The specified image handle.
+
+  @retval EFI_INVALID_PARAMETER   Image handle is NULL.
+  @retval EFI_UNSUPPORTED         Attempt to unload an unsupported image.
+  @retval EFI_SUCCESS             Image successfully unloaded.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreUnloadImageEx (
+  IN EFI_PE32_IMAGE_PROTOCOL  *This,
+  IN EFI_HANDLE                         ImageHandle
+  );
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Library/Library.c b/Core/MdeModulePkg/Core/Dxe/Library/Library.c
new file mode 100644
index 0000000000..ad46c27fa2
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Library/Library.c
@@ -0,0 +1,106 @@
+/** @file
+  DXE Core library services.
+
+Copyright (c) 2006 - 2008, 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 "DxeMain.h"
+
+//
+// Lock Stuff
+//
+/**
+  Initialize a basic mutual exclusion lock.   Each lock
+  provides mutual exclusion access at it's task priority
+  level.  Since there is no-premption (at any TPL) or
+  multiprocessor support, acquiring the lock only consists
+  of raising to the locks TPL.
+
+  @param  Lock               The EFI_LOCK structure to initialize
+
+  @retval EFI_SUCCESS        Lock Owned.
+  @retval EFI_ACCESS_DENIED  Reentrant Lock Acquisition, Lock not Owned.
+
+**/
+EFI_STATUS
+CoreAcquireLockOrFail (
+  IN EFI_LOCK  *Lock
+  )
+{
+  ASSERT (Lock != NULL);
+  ASSERT (Lock->Lock != EfiLockUninitialized);
+
+  if (Lock->Lock == EfiLockAcquired) {
+    //
+    // Lock is already owned, so bail out
+    //
+    return EFI_ACCESS_DENIED;
+  }
+
+  Lock->OwnerTpl = CoreRaiseTpl (Lock->Tpl);
+
+  Lock->Lock = EfiLockAcquired;
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Raising to the task priority level of the mutual exclusion
+  lock, and then acquires ownership of the lock.
+
+  @param  Lock               The lock to acquire
+
+  @return Lock owned
+
+**/
+VOID
+CoreAcquireLock (
+  IN EFI_LOCK  *Lock
+  )
+{
+  ASSERT (Lock != NULL);
+  ASSERT (Lock->Lock == EfiLockReleased);
+
+  Lock->OwnerTpl = CoreRaiseTpl (Lock->Tpl);
+  Lock->Lock     = EfiLockAcquired;
+}
+
+
+
+/**
+  Releases ownership of the mutual exclusion lock, and
+  restores the previous task priority level.
+
+  @param  Lock               The lock to release
+
+  @return Lock unowned
+
+**/
+VOID
+CoreReleaseLock (
+  IN EFI_LOCK  *Lock
+  )
+{
+  EFI_TPL Tpl;
+
+  ASSERT (Lock != NULL);
+  ASSERT (Lock->Lock == EfiLockAcquired);
+
+  Tpl = Lock->OwnerTpl;
+
+  Lock->Lock = EfiLockReleased;
+
+  CoreRestoreTpl (Tpl);
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Mem/Imem.h b/Core/MdeModulePkg/Core/Dxe/Mem/Imem.h
new file mode 100644
index 0000000000..a76ae2fe67
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Mem/Imem.h
@@ -0,0 +1,154 @@
+/** @file
+  Data structure and functions to allocate and free memory space.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _IMEM_H_
+#define _IMEM_H_
+
+//
+// +---------------------------------------------------+
+// | 0..(EfiMaxMemoryType - 1)    - Normal memory type |
+// +---------------------------------------------------+
+// | EfiMaxMemoryType..0x6FFFFFFF - Ilegal             |
+// +---------------------------------------------------+
+// | 0x70000000..0x7FFFFFFF       - OEM reserved       |
+// +---------------------------------------------------+
+// | 0x80000000..0xFFFFFFFF       - OS reserved        |
+// +---------------------------------------------------+
+//
+#define MEMORY_TYPE_OS_RESERVED_MIN                 0x80000000
+#define MEMORY_TYPE_OS_RESERVED_MAX                 0xFFFFFFFF
+#define MEMORY_TYPE_OEM_RESERVED_MIN                0x70000000
+#define MEMORY_TYPE_OEM_RESERVED_MAX                0x7FFFFFFF
+
+//
+// MEMORY_MAP_ENTRY
+//
+
+#define MEMORY_MAP_SIGNATURE   SIGNATURE_32('m','m','a','p')
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Link;
+  BOOLEAN         FromPages;
+
+  EFI_MEMORY_TYPE Type;
+  UINT64          Start;
+  UINT64          End;
+
+  UINT64          VirtualStart;
+  UINT64          Attribute;
+} MEMORY_MAP;
+
+//
+// Internal prototypes
+//
+
+
+/**
+  Internal function.  Used by the pool functions to allocate pages
+  to back pool allocation requests.
+
+  @param  PoolType               The type of memory for the new pool pages
+  @param  NumberOfPages          No of pages to allocate
+  @param  Alignment              Bits to align.
+
+  @return The allocated memory, or NULL
+
+**/
+VOID *
+CoreAllocatePoolPages (
+  IN EFI_MEMORY_TYPE    PoolType,
+  IN UINTN              NumberOfPages,
+  IN UINTN              Alignment
+  );
+
+
+
+/**
+  Internal function.  Frees pool pages allocated via AllocatePoolPages ()
+
+  @param  Memory                 The base address to free
+  @param  NumberOfPages          The number of pages to free
+
+**/
+VOID
+CoreFreePoolPages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  );
+
+
+
+/**
+  Internal function to allocate pool of a particular type.
+  Caller must have the memory lock held
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+
+  @return The allocate pool, or NULL
+
+**/
+VOID *
+CoreAllocatePoolI (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size
+  );
+
+
+
+/**
+  Internal function to free a pool entry.
+  Caller must have the memory lock held
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer not valid
+  @retval EFI_SUCCESS            Buffer successfully freed.
+
+**/
+EFI_STATUS
+CoreFreePoolI (
+  IN VOID       *Buffer
+  );
+
+
+
+/**
+  Enter critical section by gaining lock on gMemoryLock.
+
+**/
+VOID
+CoreAcquireMemoryLock (
+  VOID
+  );
+
+
+/**
+  Exit critical section by releasing lock on gMemoryLock.
+
+**/
+VOID
+CoreReleaseMemoryLock (
+  VOID
+  );
+
+
+//
+// Internal Global data
+//
+
+extern EFI_LOCK           gMemoryLock;
+extern LIST_ENTRY         gMemoryMap;
+extern LIST_ENTRY         mGcdMemorySpaceMap;
+#endif
diff --git a/Core/MdeModulePkg/Core/Dxe/Mem/MemData.c b/Core/MdeModulePkg/Core/Dxe/Mem/MemData.c
new file mode 100644
index 0000000000..e51b894433
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Mem/MemData.c
@@ -0,0 +1,26 @@
+/** @file
+  Global data used in memory service
+
+Copyright (c) 2006 - 2008, 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 "DxeMain.h"
+
+
+//
+// MemoryLock - synchronizes access to the memory map and pool lists
+//
+EFI_LOCK          gMemoryLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+
+//
+// MemoryMap - the current memory map
+//
+LIST_ENTRY        gMemoryMap  = INITIALIZE_LIST_HEAD_VARIABLE (gMemoryMap);
diff --git a/Core/MdeModulePkg/Core/Dxe/Mem/MemoryProfileRecord.c b/Core/MdeModulePkg/Core/Dxe/Mem/MemoryProfileRecord.c
new file mode 100644
index 0000000000..7141373fd5
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Mem/MemoryProfileRecord.c
@@ -0,0 +1,1390 @@
+/** @file
+  Support routines for UEFI memory profile.
+
+  Copyright (c) 2014 - 2015, 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 "DxeMain.h"
+#include "Imem.h"
+
+#define IS_UEFI_MEMORY_PROFILE_ENABLED ((PcdGet8 (PcdMemoryProfilePropertyMask) & BIT0) != 0)
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_CONTEXT        Context;
+  LIST_ENTRY                    *DriverInfoList;
+} MEMORY_PROFILE_CONTEXT_DATA;
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_DRIVER_INFO    DriverInfo;
+  LIST_ENTRY                    *AllocInfoList;
+  LIST_ENTRY                    Link;
+} MEMORY_PROFILE_DRIVER_INFO_DATA;
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_ALLOC_INFO     AllocInfo;
+  LIST_ENTRY                    Link;
+} MEMORY_PROFILE_ALLOC_INFO_DATA;
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED LIST_ENTRY  mImageQueue = INITIALIZE_LIST_HEAD_VARIABLE (mImageQueue);
+GLOBAL_REMOVE_IF_UNREFERENCED MEMORY_PROFILE_CONTEXT_DATA mMemoryProfileContext = {
+  MEMORY_PROFILE_CONTEXT_SIGNATURE,
+  {
+    {
+      MEMORY_PROFILE_CONTEXT_SIGNATURE,
+      sizeof (MEMORY_PROFILE_CONTEXT),
+      MEMORY_PROFILE_CONTEXT_REVISION
+    },
+    0,
+    0,
+    {0},
+    {0},
+    0,
+    0,
+    0
+  },
+  &mImageQueue,
+};
+GLOBAL_REMOVE_IF_UNREFERENCED MEMORY_PROFILE_CONTEXT_DATA *mMemoryProfileContextPtr = NULL;
+
+BOOLEAN mMemoryProfileRecordingStatus = FALSE;
+
+/**
+  Get memory profile data.
+
+  @param[in]      This              The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in, out] ProfileSize       On entry, points to the size in bytes of the ProfileBuffer.
+                                    On return, points to the size of the data returned in ProfileBuffer.
+  @param[out]     ProfileBuffer     Profile buffer.
+                      
+  @return EFI_SUCCESS               Get the memory profile data successfully.
+  @return EFI_BUFFER_TO_SMALL       The ProfileSize is too small for the resulting data. 
+                                    ProfileSize is updated with the size required.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolGetData (
+  IN     EDKII_MEMORY_PROFILE_PROTOCOL  *This,
+  IN OUT UINT64                         *ProfileSize,
+     OUT VOID                           *ProfileBuffer
+  );
+
+/**
+  Register image to memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+  @param[in] FileType           File type of the image.
+
+  @return EFI_SUCCESS           Register success.
+  @return EFI_OUT_OF_RESOURCE   No enough resource for this register.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolRegisterImage (
+  IN EDKII_MEMORY_PROFILE_PROTOCOL      *This,
+  IN EFI_DEVICE_PATH_PROTOCOL           *FilePath,
+  IN PHYSICAL_ADDRESS                   ImageBase,
+  IN UINT64                             ImageSize,
+  IN EFI_FV_FILETYPE                    FileType
+  );
+
+/**
+  Unregister image from memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+
+  @return EFI_SUCCESS           Unregister success.
+  @return EFI_NOT_FOUND         The image is not found.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolUnregisterImage (
+  IN EDKII_MEMORY_PROFILE_PROTOCOL      *This,
+  IN EFI_DEVICE_PATH_PROTOCOL           *FilePath,
+  IN PHYSICAL_ADDRESS                   ImageBase,
+  IN UINT64                             ImageSize
+  );
+
+EDKII_MEMORY_PROFILE_PROTOCOL mProfileProtocol = {
+  ProfileProtocolGetData,
+  ProfileProtocolRegisterImage,
+  ProfileProtocolUnregisterImage
+};
+
+/**
+  Return memory profile context.
+
+  @return Memory profile context.
+
+**/
+MEMORY_PROFILE_CONTEXT_DATA *
+GetMemoryProfileContext (
+  VOID
+  )
+{
+  return mMemoryProfileContextPtr;
+}
+
+/**
+  Retrieves the magic value from the PE/COFF header.
+
+  @param Hdr    The buffer in which to return the PE32, PE32+, or TE header.
+
+  @return EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC - Image is PE32
+  @return EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC - Image is PE32+
+
+**/
+UINT16
+InternalPeCoffGetPeHeaderMagicValue (
+  IN  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr
+  )
+{
+  //
+  // NOTE: Some versions of Linux ELILO for Itanium have an incorrect magic value
+  //       in the PE/COFF Header.  If the MachineType is Itanium(IA64) and the
+  //       Magic value in the OptionalHeader is  EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC
+  //       then override the returned value to EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC
+  //
+  if (Hdr.Pe32->FileHeader.Machine == IMAGE_FILE_MACHINE_IA64 && Hdr.Pe32->OptionalHeader.Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+    return EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC;
+  }
+  //
+  // Return the magic value from the PC/COFF Optional Header
+  //
+  return Hdr.Pe32->OptionalHeader.Magic;
+}
+
+/**
+  Retrieves and returns the Subsystem of a PE/COFF image that has been loaded into system memory.
+  If Pe32Data is NULL, then ASSERT().
+
+  @param Pe32Data   The pointer to the PE/COFF image that is loaded in system memory.
+
+  @return The Subsystem of the PE/COFF image.
+
+**/
+UINT16
+InternalPeCoffGetSubsystem (
+  IN VOID  *Pe32Data
+  )
+{
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
+  EFI_IMAGE_DOS_HEADER                 *DosHdr;
+  UINT16                               Magic;
+
+  ASSERT (Pe32Data != NULL);
+
+  DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    //
+    // DOS image header is present, so read the PE header after the DOS image header.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) ((UINTN) Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+  } else {
+    //
+    // DOS image header is not present, so PE header is at the image base.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) Pe32Data;
+  }
+
+  if (Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) {
+    return Hdr.Te->Subsystem;
+  } else if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE)  {
+    Magic = InternalPeCoffGetPeHeaderMagicValue (Hdr);
+    if (Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+      return Hdr.Pe32->OptionalHeader.Subsystem;
+    } else if (Magic == EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC) {
+      return Hdr.Pe32Plus->OptionalHeader.Subsystem;
+    }
+  }
+
+  return 0x0000;
+}
+
+/**
+  Retrieves and returns a pointer to the entry point to a PE/COFF image that has been loaded
+  into system memory with the PE/COFF Loader Library functions.
+
+  Retrieves the entry point to the PE/COFF image specified by Pe32Data and returns this entry
+  point in EntryPoint.  If the entry point could not be retrieved from the PE/COFF image, then
+  return RETURN_INVALID_PARAMETER.  Otherwise return RETURN_SUCCESS.
+  If Pe32Data is NULL, then ASSERT().
+  If EntryPoint is NULL, then ASSERT().
+
+  @param  Pe32Data                  The pointer to the PE/COFF image that is loaded in system memory.
+  @param  EntryPoint                The pointer to entry point to the PE/COFF image to return.
+
+  @retval RETURN_SUCCESS            EntryPoint was returned.
+  @retval RETURN_INVALID_PARAMETER  The entry point could not be found in the PE/COFF image.
+
+**/
+RETURN_STATUS
+InternalPeCoffGetEntryPoint (
+  IN  VOID  *Pe32Data,
+  OUT VOID  **EntryPoint
+  )
+{
+  EFI_IMAGE_DOS_HEADER                  *DosHdr;
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION   Hdr;
+
+  ASSERT (Pe32Data   != NULL);
+  ASSERT (EntryPoint != NULL);
+
+  DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    //
+    // DOS image header is present, so read the PE header after the DOS image header.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) ((UINTN) Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+  } else {
+    //
+    // DOS image header is not present, so PE header is at the image base.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) Pe32Data;
+  }
+
+  //
+  // Calculate the entry point relative to the start of the image.
+  // AddressOfEntryPoint is common for PE32 & PE32+
+  //
+  if (Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) {
+    *EntryPoint = (VOID *) ((UINTN) Pe32Data + (UINTN) (Hdr.Te->AddressOfEntryPoint & 0x0ffffffff) + sizeof (EFI_TE_IMAGE_HEADER) - Hdr.Te->StrippedSize);
+    return RETURN_SUCCESS;
+  } else if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE) {
+    *EntryPoint = (VOID *) ((UINTN) Pe32Data + (UINTN) (Hdr.Pe32->OptionalHeader.AddressOfEntryPoint & 0x0ffffffff));
+    return RETURN_SUCCESS;
+  }
+
+  return RETURN_UNSUPPORTED;
+}
+
+/**
+  Build driver info.
+
+  @param ContextData    Memory profile context.
+  @param FileName       File name of the image.
+  @param ImageBase      Image base address.
+  @param ImageSize      Image size.
+  @param EntryPoint     Entry point of the image.
+  @param ImageSubsystem Image subsystem of the image.
+  @param FileType       File type of the image.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+BuildDriverInfo (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize,
+  IN PHYSICAL_ADDRESS               EntryPoint,
+  IN UINT16                         ImageSubsystem,
+  IN EFI_FV_FILETYPE                FileType
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  VOID                              *EntryPointInImage;
+
+  //
+  // Use CoreInternalAllocatePool() that will not update profile for this AllocatePool action.
+  //
+  Status = CoreInternalAllocatePool (
+             EfiBootServicesData,
+             sizeof (*DriverInfoData) + sizeof (LIST_ENTRY),
+             (VOID **) &DriverInfoData
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ASSERT (DriverInfoData != NULL);
+
+  ZeroMem (DriverInfoData, sizeof (*DriverInfoData));
+
+  DriverInfo = &DriverInfoData->DriverInfo;
+  DriverInfoData->Signature = MEMORY_PROFILE_DRIVER_INFO_SIGNATURE;
+  DriverInfo->Header.Signature = MEMORY_PROFILE_DRIVER_INFO_SIGNATURE;
+  DriverInfo->Header.Length = sizeof (MEMORY_PROFILE_DRIVER_INFO);
+  DriverInfo->Header.Revision = MEMORY_PROFILE_DRIVER_INFO_REVISION;
+  if (FileName != NULL) {
+    CopyMem (&DriverInfo->FileName, FileName, sizeof (EFI_GUID));
+  }
+  DriverInfo->ImageBase = ImageBase;
+  DriverInfo->ImageSize = ImageSize;
+  DriverInfo->EntryPoint = EntryPoint;
+  DriverInfo->ImageSubsystem = ImageSubsystem;
+  if ((EntryPoint != 0) && ((EntryPoint < ImageBase) || (EntryPoint >= (ImageBase + ImageSize)))) {
+    //
+    // If the EntryPoint is not in the range of image buffer, it should come from emulation environment.
+    // So patch ImageBuffer here to align the EntryPoint.
+    //
+    Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageBase, &EntryPointInImage);
+    ASSERT_EFI_ERROR (Status);
+    DriverInfo->ImageBase = ImageBase + EntryPoint - (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+  }
+  DriverInfo->FileType = FileType;
+  DriverInfoData->AllocInfoList = (LIST_ENTRY *) (DriverInfoData + 1);
+  InitializeListHead (DriverInfoData->AllocInfoList);
+  DriverInfo->CurrentUsage = 0;
+  DriverInfo->PeakUsage = 0;
+  DriverInfo->AllocRecordCount = 0;
+
+  InsertTailList (ContextData->DriverInfoList, &DriverInfoData->Link);
+  ContextData->Context.ImageCount ++;
+  ContextData->Context.TotalImageSize += DriverInfo->ImageSize;
+
+  return DriverInfoData;
+}
+
+/**
+  Register DXE Core to memory profile.
+
+  @param HobStart       The start address of the HOB.
+  @param ContextData    Memory profile context.
+
+  @retval TRUE      Register success.
+  @retval FALSE     Register fail.
+
+**/
+BOOLEAN
+RegisterDxeCore (
+  IN VOID                           *HobStart,
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData
+  )
+{
+  EFI_PEI_HOB_POINTERS              DxeCoreHob;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  PHYSICAL_ADDRESS                  ImageBase;
+
+  ASSERT (ContextData != NULL);
+
+  //
+  // Searching for image hob
+  //
+  DxeCoreHob.Raw          = HobStart;
+  while ((DxeCoreHob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, DxeCoreHob.Raw)) != NULL) {
+    if (CompareGuid (&DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.Name, &gEfiHobMemoryAllocModuleGuid)) {
+      //
+      // Find Dxe Core HOB
+      //
+      break;
+    }
+    DxeCoreHob.Raw = GET_NEXT_HOB (DxeCoreHob);
+  }
+  ASSERT (DxeCoreHob.Raw != NULL);
+
+  ImageBase = DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.MemoryBaseAddress;
+  DriverInfoData = BuildDriverInfo (
+                     ContextData,
+                     &DxeCoreHob.MemoryAllocationModule->ModuleName,
+                     ImageBase,
+                     DxeCoreHob.MemoryAllocationModule->MemoryAllocationHeader.MemoryLength,
+                     DxeCoreHob.MemoryAllocationModule->EntryPoint,
+                     InternalPeCoffGetSubsystem ((VOID *) (UINTN) ImageBase),
+                     EFI_FV_FILETYPE_DXE_CORE
+                     );
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Initialize memory profile.
+
+  @param HobStart   The start address of the HOB.
+
+**/
+VOID
+MemoryProfileInit (
+  IN VOID   *HobStart
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+
+  if (!IS_UEFI_MEMORY_PROFILE_ENABLED) {
+    return;
+  }
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData != NULL) {
+    return;
+  }
+
+  mMemoryProfileRecordingStatus = TRUE;
+  mMemoryProfileContextPtr = &mMemoryProfileContext;
+
+  RegisterDxeCore (HobStart, &mMemoryProfileContext);
+
+  DEBUG ((EFI_D_INFO, "MemoryProfileInit MemoryProfileContext - 0x%x\n", &mMemoryProfileContext));
+}
+
+/**
+  Install memory profile protocol.
+
+**/
+VOID
+MemoryProfileInstallProtocol (
+  VOID
+  )
+{
+  EFI_HANDLE    Handle;
+  EFI_STATUS    Status;
+
+  if (!IS_UEFI_MEMORY_PROFILE_ENABLED) {
+    return;
+  }
+
+  Handle = NULL;
+  Status = CoreInstallMultipleProtocolInterfaces (
+             &Handle,
+             &gEdkiiMemoryProfileGuid,
+             &mProfileProtocol,
+             NULL
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Get the GUID file name from the file path.
+
+  @param FilePath  File path.
+
+  @return The GUID file name from the file path.
+
+**/
+EFI_GUID *
+GetFileNameFromFilePath (
+  IN EFI_DEVICE_PATH_PROTOCOL   *FilePath
+  )
+{
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH     *ThisFilePath;
+  EFI_GUID                              *FileName;
+
+  FileName = NULL;
+  if (FilePath != NULL) {
+    ThisFilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) FilePath;
+    while (!IsDevicePathEnd (ThisFilePath)) {
+      FileName = EfiGetNameGuidFromFwVolDevicePathNode (ThisFilePath);
+      if (FileName != NULL) {
+        break;
+      }
+      ThisFilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) NextDevicePathNode (ThisFilePath);
+    }
+  }
+
+  return FileName;
+}
+
+/**
+  Register image to memory profile.
+
+  @param DriverEntry    Image info.
+  @param FileType       Image file type.
+
+  @retval TRUE          Register success.
+  @retval FALSE         Register fail.
+
+**/
+BOOLEAN
+RegisterMemoryProfileImage (
+  IN LOADED_IMAGE_PRIVATE_DATA  *DriverEntry,
+  IN EFI_FV_FILETYPE            FileType
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+
+  if (!IS_UEFI_MEMORY_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = BuildDriverInfo (
+                     ContextData,
+                     GetFileNameFromFilePath (DriverEntry->Info.FilePath),
+                     DriverEntry->ImageContext.ImageAddress,
+                     DriverEntry->ImageContext.ImageSize,
+                     DriverEntry->ImageContext.EntryPoint,
+                     DriverEntry->ImageContext.ImageType,
+                     FileType
+                     );
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Search image from memory profile.
+
+  @param ContextData    Memory profile context.
+  @param FileName       Image file name.
+  @param Address        Image Address.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+GetMemoryProfileDriverInfoByFileNameAndAddress (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               Address
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    DriverInfo = &DriverInfoData->DriverInfo;
+    if ((CompareGuid (&DriverInfo->FileName, FileName)) &&
+        (Address >= DriverInfo->ImageBase) &&
+        (Address < (DriverInfo->ImageBase + DriverInfo->ImageSize))) {
+      return DriverInfoData;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Search dummy image from memory profile.
+
+  @param ContextData    Memory profile context.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+FindDummyImage (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                   DriverLink,
+                   MEMORY_PROFILE_DRIVER_INFO_DATA,
+                   Link,
+                   MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                   );
+    if (CompareGuid (&gZeroGuid, &DriverInfoData->DriverInfo.FileName)) {
+      return DriverInfoData;
+    }
+  }
+
+  return BuildDriverInfo (ContextData, &gZeroGuid, 0, 0, 0, 0, 0);
+}
+
+/**
+  Search image from memory profile.
+  It will return image, if (Address >= ImageBuffer) AND (Address < ImageBuffer + ImageSize)
+
+  @param ContextData    Memory profile context.
+  @param Address        Image or Function address.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+GetMemoryProfileDriverInfoFromAddress (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN PHYSICAL_ADDRESS               Address
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    DriverInfo = &DriverInfoData->DriverInfo;
+    if ((Address >= DriverInfo->ImageBase) &&
+        (Address < (DriverInfo->ImageBase + DriverInfo->ImageSize))) {
+      return DriverInfoData;
+    }
+  }
+
+  //
+  // Should never come here.
+  //
+  return FindDummyImage (ContextData);
+}
+
+/**
+  Unregister image from memory profile.
+
+  @param DriverEntry    Image info.
+
+  @retval TRUE          Unregister success.
+  @retval FALSE         Unregister fail.
+
+**/
+BOOLEAN
+UnregisterMemoryProfileImage (
+  IN LOADED_IMAGE_PRIVATE_DATA      *DriverEntry
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  EFI_GUID                          *FileName;
+  PHYSICAL_ADDRESS                  ImageAddress;
+  VOID                              *EntryPointInImage;
+
+  if (!IS_UEFI_MEMORY_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = NULL;
+  FileName = GetFileNameFromFilePath (DriverEntry->Info.FilePath);
+  ImageAddress = DriverEntry->ImageContext.ImageAddress;
+  if ((DriverEntry->ImageContext.EntryPoint < ImageAddress) || (DriverEntry->ImageContext.EntryPoint >= (ImageAddress + DriverEntry->ImageContext.ImageSize))) {
+    //
+    // If the EntryPoint is not in the range of image buffer, it should come from emulation environment.
+    // So patch ImageAddress here to align the EntryPoint.
+    //
+    Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageAddress, &EntryPointInImage);
+    ASSERT_EFI_ERROR (Status);
+    ImageAddress = ImageAddress + (UINTN) DriverEntry->ImageContext.EntryPoint - (UINTN) EntryPointInImage;
+  }
+  if (FileName != NULL) {
+    DriverInfoData = GetMemoryProfileDriverInfoByFileNameAndAddress (ContextData, FileName, ImageAddress);
+  }
+  if (DriverInfoData == NULL) {
+    DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, ImageAddress);
+  }
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  ContextData->Context.TotalImageSize -= DriverInfoData->DriverInfo.ImageSize;
+
+  DriverInfoData->DriverInfo.ImageBase = 0;
+  DriverInfoData->DriverInfo.ImageSize = 0;
+
+  if (DriverInfoData->DriverInfo.PeakUsage == 0) {
+    ContextData->Context.ImageCount --;
+    RemoveEntryList (&DriverInfoData->Link);
+    //
+    // Use CoreInternalFreePool() that will not update profile for this FreePool action.
+    //
+    CoreInternalFreePool (DriverInfoData);
+  }
+
+  return TRUE;
+}
+
+/**
+  Return if this memory type needs to be recorded into memory profile.
+  If BIOS memory type (0 ~ EfiMaxMemoryType - 1), it checks bit (1 << MemoryType).
+  If OS memory type (0x80000000 ~ 0xFFFFFFFF), it checks bit63 - 0x8000000000000000.
+  If OEM memory type (0x70000000 ~ 0x7FFFFFFF), it checks bit62 - 0x4000000000000000.
+
+  @param MemoryType     Memory type.
+
+  @retval TRUE          This memory type need to be recorded.
+  @retval FALSE         This memory type need not to be recorded.
+
+**/
+BOOLEAN
+CoreNeedRecordProfile (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  UINT64 TestBit;
+
+  if ((UINT32) MemoryType >= MEMORY_TYPE_OS_RESERVED_MIN) {
+    TestBit = BIT63;
+  } else if ((UINT32) MemoryType >= MEMORY_TYPE_OEM_RESERVED_MIN) {
+    TestBit = BIT62;
+  } else {
+    TestBit = LShiftU64 (1, MemoryType);
+  }
+
+  if ((PcdGet64 (PcdMemoryProfileMemoryType) & TestBit) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Convert EFI memory type to profile memory index. The rule is:
+  If BIOS memory type (0 ~ EfiMaxMemoryType - 1), ProfileMemoryIndex = MemoryType.
+  If OS memory type (0x80000000 ~ 0xFFFFFFFF), ProfileMemoryIndex = EfiMaxMemoryType.
+  If OEM memory type (0x70000000 ~ 0x7FFFFFFF), ProfileMemoryIndex = EfiMaxMemoryType + 1.
+
+  @param MemoryType     Memory type.
+
+  @return Profile memory index.
+
+**/
+UINTN
+GetProfileMemoryIndex (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  if ((UINT32) MemoryType >= MEMORY_TYPE_OS_RESERVED_MIN) {
+    return EfiMaxMemoryType;
+  } else if ((UINT32) MemoryType >= MEMORY_TYPE_OEM_RESERVED_MIN) {
+    return EfiMaxMemoryType + 1;
+  } else {
+    return MemoryType;
+  }
+}
+
+/**
+  Update memory profile Allocate information.
+
+  @param CallerAddress  Address of caller who call Allocate.
+  @param Action         This Allocate action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+CoreUpdateProfileAllocate (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  Size,
+  IN VOID                   *Buffer
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_CONTEXT           *Context;
+  MEMORY_PROFILE_DRIVER_INFO       *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO        *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+  UINTN                             ProfileMemoryIndex;
+
+  AllocInfoData = NULL;
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, CallerAddress);
+  ASSERT (DriverInfoData != NULL);
+
+  //
+  // Use CoreInternalAllocatePool() that will not update profile for this AllocatePool action.
+  //
+  Status = CoreInternalAllocatePool (
+             EfiBootServicesData,
+             sizeof (*AllocInfoData),
+             (VOID **) &AllocInfoData
+             );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  ASSERT (AllocInfoData != NULL);
+  AllocInfo = &AllocInfoData->AllocInfo;
+  AllocInfoData->Signature      = MEMORY_PROFILE_ALLOC_INFO_SIGNATURE;
+  AllocInfo->Header.Signature   = MEMORY_PROFILE_ALLOC_INFO_SIGNATURE;
+  AllocInfo->Header.Length      = sizeof (MEMORY_PROFILE_ALLOC_INFO);
+  AllocInfo->Header.Revision    = MEMORY_PROFILE_ALLOC_INFO_REVISION;
+  AllocInfo->CallerAddress      = CallerAddress;
+  AllocInfo->SequenceId         = ContextData->Context.SequenceCount;
+  AllocInfo->Action             = Action;
+  AllocInfo->MemoryType         = MemoryType;
+  AllocInfo->Buffer             = (PHYSICAL_ADDRESS) (UINTN) Buffer;
+  AllocInfo->Size               = Size;
+
+  InsertTailList (DriverInfoData->AllocInfoList, &AllocInfoData->Link);
+
+  ProfileMemoryIndex = GetProfileMemoryIndex (MemoryType);
+
+  DriverInfo = &DriverInfoData->DriverInfo;
+  DriverInfo->CurrentUsage += Size;
+  if (DriverInfo->PeakUsage < DriverInfo->CurrentUsage) {
+    DriverInfo->PeakUsage = DriverInfo->CurrentUsage;
+  }
+  DriverInfo->CurrentUsageByType[ProfileMemoryIndex] += Size;
+  if (DriverInfo->PeakUsageByType[ProfileMemoryIndex] < DriverInfo->CurrentUsageByType[ProfileMemoryIndex]) {
+    DriverInfo->PeakUsageByType[ProfileMemoryIndex] = DriverInfo->CurrentUsageByType[ProfileMemoryIndex];
+  }
+  DriverInfo->AllocRecordCount ++;
+
+  Context = &ContextData->Context;
+  Context->CurrentTotalUsage += Size;
+  if (Context->PeakTotalUsage < Context->CurrentTotalUsage) {
+    Context->PeakTotalUsage = Context->CurrentTotalUsage;
+  }
+  Context->CurrentTotalUsageByType[ProfileMemoryIndex] += Size;
+  if (Context->PeakTotalUsageByType[ProfileMemoryIndex] < Context->CurrentTotalUsageByType[ProfileMemoryIndex]) {
+    Context->PeakTotalUsageByType[ProfileMemoryIndex] = Context->CurrentTotalUsageByType[ProfileMemoryIndex];
+  }
+  Context->SequenceCount ++;
+
+  return TRUE;
+}
+
+/**
+  Get memory profile alloc info from memory profile
+
+  @param DriverInfoData     Driver info
+  @param Action             This Free action
+  @param Size               Buffer size
+  @param Buffer             Buffer address
+
+  @return Pointer to memory profile alloc info.
+**/
+MEMORY_PROFILE_ALLOC_INFO_DATA *
+GetMemoryProfileAllocInfoFromAddress (
+  IN MEMORY_PROFILE_DRIVER_INFO_DATA    *DriverInfoData,
+  IN MEMORY_PROFILE_ACTION              Action,
+  IN UINTN                              Size,
+  IN VOID                               *Buffer
+  )
+{
+  LIST_ENTRY                        *AllocInfoList;
+  LIST_ENTRY                        *AllocLink;
+  MEMORY_PROFILE_ALLOC_INFO         *AllocInfo;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+
+  AllocInfoList = DriverInfoData->AllocInfoList;
+
+  for (AllocLink = AllocInfoList->ForwardLink;
+       AllocLink != AllocInfoList;
+       AllocLink = AllocLink->ForwardLink) {
+    AllocInfoData = CR (
+                      AllocLink,
+                      MEMORY_PROFILE_ALLOC_INFO_DATA,
+                      Link,
+                      MEMORY_PROFILE_ALLOC_INFO_SIGNATURE
+                      );
+    AllocInfo = &AllocInfoData->AllocInfo;
+    if (AllocInfo->Action != Action) {
+      continue;
+    }
+    switch (Action) {
+      case MemoryProfileActionAllocatePages:
+        if ((AllocInfo->Buffer <= (PHYSICAL_ADDRESS) (UINTN) Buffer) &&
+            ((AllocInfo->Buffer + AllocInfo->Size) >= ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size))) {
+          return AllocInfoData;
+        }
+        break;
+      case MemoryProfileActionAllocatePool:
+        if (AllocInfo->Buffer == (PHYSICAL_ADDRESS) (UINTN) Buffer) {
+          return AllocInfoData;
+        }
+        break;
+      default:
+        ASSERT (FALSE);
+        break;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Update memory profile Free information.
+
+  @param CallerAddress  Address of caller who call Free.
+  @param Action         This Free action.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+CoreUpdateProfileFree (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN UINTN                  Size,
+  IN VOID                   *Buffer
+  )
+{
+  MEMORY_PROFILE_CONTEXT           *Context;
+  MEMORY_PROFILE_DRIVER_INFO       *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO        *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA      *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *DriverInfoData;
+  LIST_ENTRY                       *DriverLink;
+  LIST_ENTRY                       *DriverInfoList;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *ThisDriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA   *AllocInfoData;
+  UINTN                            ProfileMemoryIndex;
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, CallerAddress);
+  ASSERT (DriverInfoData != NULL);
+
+  switch (Action) {
+    case MemoryProfileActionFreePages:
+      AllocInfoData = GetMemoryProfileAllocInfoFromAddress (DriverInfoData, MemoryProfileActionAllocatePages, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePool:
+      AllocInfoData = GetMemoryProfileAllocInfoFromAddress (DriverInfoData, MemoryProfileActionAllocatePool, 0, Buffer);
+      break;
+    default:
+      ASSERT (FALSE);
+      AllocInfoData = NULL;
+      break;
+  }
+  if (AllocInfoData == NULL) {
+    //
+    // Legal case, because driver A might free memory allocated by driver B, by some protocol.
+    //
+    DriverInfoList = ContextData->DriverInfoList;
+
+    for (DriverLink = DriverInfoList->ForwardLink;
+         DriverLink != DriverInfoList;
+         DriverLink = DriverLink->ForwardLink) {
+      ThisDriverInfoData = CR (
+                             DriverLink,
+                             MEMORY_PROFILE_DRIVER_INFO_DATA,
+                             Link,
+                             MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                             );
+      switch (Action) {
+        case MemoryProfileActionFreePages:
+          AllocInfoData = GetMemoryProfileAllocInfoFromAddress (ThisDriverInfoData, MemoryProfileActionAllocatePages, Size, Buffer);
+          break;
+        case MemoryProfileActionFreePool:
+          AllocInfoData = GetMemoryProfileAllocInfoFromAddress (ThisDriverInfoData, MemoryProfileActionAllocatePool, 0, Buffer);
+          break;
+        default:
+          ASSERT (FALSE);
+          AllocInfoData = NULL;
+          break;
+      }
+      if (AllocInfoData != NULL) {
+        DriverInfoData = ThisDriverInfoData;
+        break;
+      }
+    }
+
+    if (AllocInfoData == NULL) {
+      //
+      // No matched allocate operation is found for this free operation.
+      // It is because the specified memory type allocate operation has been
+      // filtered by CoreNeedRecordProfile(), but free operations have no
+      // memory type information, they can not be filtered by CoreNeedRecordProfile().
+      // Then, they will be filtered here.
+      //
+      return FALSE;
+    }
+  }
+
+  Context = &ContextData->Context;
+  DriverInfo = &DriverInfoData->DriverInfo;
+  AllocInfo = &AllocInfoData->AllocInfo;
+
+  ProfileMemoryIndex = GetProfileMemoryIndex (AllocInfo->MemoryType);
+
+  Context->CurrentTotalUsage -= AllocInfo->Size;
+  Context->CurrentTotalUsageByType[ProfileMemoryIndex] -= AllocInfo->Size;
+
+  DriverInfo->CurrentUsage -= AllocInfo->Size;
+  DriverInfo->CurrentUsageByType[ProfileMemoryIndex] -= AllocInfo->Size;
+  DriverInfo->AllocRecordCount --;
+
+  RemoveEntryList (&AllocInfoData->Link);
+
+  if (Action == MemoryProfileActionFreePages) {
+    if (AllocInfo->Buffer != (PHYSICAL_ADDRESS) (UINTN) Buffer) {
+      CoreUpdateProfileAllocate (
+        AllocInfo->CallerAddress,
+        MemoryProfileActionAllocatePages,
+        AllocInfo->MemoryType,
+        (UINTN) ((PHYSICAL_ADDRESS) (UINTN) Buffer - AllocInfo->Buffer),
+        (VOID *) (UINTN) AllocInfo->Buffer
+        );
+    }
+    if (AllocInfo->Buffer + AllocInfo->Size != ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size)) {
+      CoreUpdateProfileAllocate (
+        AllocInfo->CallerAddress,
+        MemoryProfileActionAllocatePages,
+        AllocInfo->MemoryType,
+        (UINTN) ((AllocInfo->Buffer + AllocInfo->Size) - ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size)),
+        (VOID *) ((UINTN) Buffer + Size)
+        );
+    }
+  }
+
+  //
+  // Use CoreInternalFreePool() that will not update profile for this FreePool action.
+  //
+  CoreInternalFreePool (AllocInfoData);
+
+  return TRUE;
+}
+
+/**
+  Update memory profile information.
+
+  @param CallerAddress  Address of caller who call Allocate or Free.
+  @param Action         This Allocate or Free action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+CoreUpdateProfile (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN EFI_MEMORY_TYPE        MemoryType, // Valid for AllocatePages/AllocatePool
+  IN UINTN                  Size,       // Valid for AllocatePages/FreePages/AllocatePool
+  IN VOID                   *Buffer
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+
+  if (!IS_UEFI_MEMORY_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  if (!mMemoryProfileRecordingStatus) {
+    return FALSE;
+  }
+
+  //
+  // Free operations have no memory type information, so skip the check.
+  //
+  if ((Action == MemoryProfileActionAllocatePages) || (Action == MemoryProfileActionAllocatePool)) {
+    //
+    // Only record limited MemoryType.
+    //
+    if (!CoreNeedRecordProfile (MemoryType)) {
+      return FALSE;
+    }
+  }
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  switch (Action) {
+    case MemoryProfileActionAllocatePages:
+      CoreUpdateProfileAllocate (CallerAddress, Action, MemoryType, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePages:
+      CoreUpdateProfileFree (CallerAddress, Action, Size, Buffer);
+      break;
+    case MemoryProfileActionAllocatePool:
+      CoreUpdateProfileAllocate (CallerAddress, Action, MemoryType, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePool:
+      CoreUpdateProfileFree (CallerAddress, Action, 0, Buffer);
+      break;
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+  return TRUE;
+}
+
+////////////////////
+
+/**
+  Get memory profile data size.
+
+  @return Memory profile data size.
+
+**/
+UINTN
+MemoryProfileGetDataSize (
+  VOID
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverInfoList;
+  LIST_ENTRY                        *DriverLink;
+  UINTN                             TotalSize;
+
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return 0;
+  }
+
+  TotalSize = sizeof (MEMORY_PROFILE_CONTEXT);
+  TotalSize += sizeof (MEMORY_PROFILE_DRIVER_INFO) * (UINTN) ContextData->Context.ImageCount;
+
+  DriverInfoList = ContextData->DriverInfoList;
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    TotalSize += sizeof (MEMORY_PROFILE_ALLOC_INFO) * (UINTN) DriverInfoData->DriverInfo.AllocRecordCount;
+  }
+
+  return TotalSize;
+}
+
+/**
+  Copy memory profile data.
+
+  @param ProfileBuffer  The buffer to hold memory profile data.
+
+**/
+VOID
+MemoryProfileCopyData (
+  IN VOID   *ProfileBuffer
+  )
+{
+  MEMORY_PROFILE_CONTEXT            *Context;
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO         *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+  LIST_ENTRY                        *DriverInfoList;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *AllocInfoList;
+  LIST_ENTRY                        *AllocLink;
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  Context = ProfileBuffer;
+  CopyMem (Context, &ContextData->Context, sizeof (MEMORY_PROFILE_CONTEXT));
+  DriverInfo = (MEMORY_PROFILE_DRIVER_INFO *) (Context + 1);
+
+  DriverInfoList = ContextData->DriverInfoList;
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    CopyMem (DriverInfo, &DriverInfoData->DriverInfo, sizeof (MEMORY_PROFILE_DRIVER_INFO));
+    AllocInfo = (MEMORY_PROFILE_ALLOC_INFO *) (DriverInfo + 1);
+
+    AllocInfoList = DriverInfoData->AllocInfoList;
+    for (AllocLink = AllocInfoList->ForwardLink;
+         AllocLink != AllocInfoList;
+         AllocLink = AllocLink->ForwardLink) {
+      AllocInfoData = CR (
+                        AllocLink,
+                        MEMORY_PROFILE_ALLOC_INFO_DATA,
+                        Link,
+                        MEMORY_PROFILE_ALLOC_INFO_SIGNATURE
+                        );
+      CopyMem (AllocInfo, &AllocInfoData->AllocInfo, sizeof (MEMORY_PROFILE_ALLOC_INFO));
+      AllocInfo += 1;
+    }
+
+    DriverInfo = (MEMORY_PROFILE_DRIVER_INFO *) ((UINTN) (DriverInfo + 1) + sizeof (MEMORY_PROFILE_ALLOC_INFO) * (UINTN) DriverInfo->AllocRecordCount);
+  }
+}
+
+/**
+  Get memory profile data.
+
+  @param[in]      This              The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in, out] ProfileSize       On entry, points to the size in bytes of the ProfileBuffer.
+                                    On return, points to the size of the data returned in ProfileBuffer.
+  @param[out]     ProfileBuffer     Profile buffer.
+                      
+  @return EFI_SUCCESS               Get the memory profile data successfully.
+  @return EFI_BUFFER_TO_SMALL       The ProfileSize is too small for the resulting data. 
+                                    ProfileSize is updated with the size required.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolGetData (
+  IN     EDKII_MEMORY_PROFILE_PROTOCOL  *This,
+  IN OUT UINT64                         *ProfileSize,
+     OUT VOID                           *ProfileBuffer
+  )
+{
+  UINTN                                 Size;
+  MEMORY_PROFILE_CONTEXT_DATA           *ContextData;
+  BOOLEAN                               MemoryProfileRecordingStatus;
+
+  ContextData = GetMemoryProfileContext ();
+  if (ContextData == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  MemoryProfileRecordingStatus = mMemoryProfileRecordingStatus;
+  mMemoryProfileRecordingStatus = FALSE;
+
+  Size = MemoryProfileGetDataSize ();
+
+  if (*ProfileSize < Size) {
+    *ProfileSize = Size;
+    mMemoryProfileRecordingStatus = MemoryProfileRecordingStatus;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *ProfileSize = Size;
+  MemoryProfileCopyData (ProfileBuffer);
+
+  mMemoryProfileRecordingStatus = MemoryProfileRecordingStatus;
+  return EFI_SUCCESS;
+}
+
+/**
+  Register image to memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+  @param[in] FileType           File type of the image.
+
+  @return EFI_SUCCESS           Register success.
+  @return EFI_OUT_OF_RESOURCE   No enough resource for this register.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolRegisterImage (
+  IN EDKII_MEMORY_PROFILE_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL       *FilePath,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize,
+  IN EFI_FV_FILETYPE                FileType
+  )
+{
+  EFI_STATUS                        Status;
+  LOADED_IMAGE_PRIVATE_DATA         DriverEntry;
+  VOID                              *EntryPointInImage;
+
+  ZeroMem (&DriverEntry, sizeof (DriverEntry));
+  DriverEntry.Info.FilePath = FilePath;
+  DriverEntry.ImageContext.ImageAddress = ImageBase;
+  DriverEntry.ImageContext.ImageSize = ImageSize;
+  Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageBase, &EntryPointInImage);
+  ASSERT_EFI_ERROR (Status);
+  DriverEntry.ImageContext.EntryPoint = (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+  DriverEntry.ImageContext.ImageType = InternalPeCoffGetSubsystem ((VOID *) (UINTN) ImageBase);
+
+  return RegisterMemoryProfileImage (&DriverEntry, FileType) ? EFI_SUCCESS: EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Unregister image from memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+
+  @return EFI_SUCCESS           Unregister success.
+  @return EFI_NOT_FOUND         The image is not found.
+
+**/
+EFI_STATUS
+EFIAPI
+ProfileProtocolUnregisterImage (
+  IN EDKII_MEMORY_PROFILE_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL       *FilePath,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize
+  )
+{
+  EFI_STATUS                        Status;
+  LOADED_IMAGE_PRIVATE_DATA         DriverEntry;
+  VOID                              *EntryPointInImage;
+
+  ZeroMem (&DriverEntry, sizeof (DriverEntry));
+  DriverEntry.Info.FilePath = FilePath;
+  DriverEntry.ImageContext.ImageAddress = ImageBase;
+  DriverEntry.ImageContext.ImageSize = ImageSize;
+  Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageBase, &EntryPointInImage);
+  ASSERT_EFI_ERROR (Status);
+  DriverEntry.ImageContext.EntryPoint = (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+
+  return UnregisterMemoryProfileImage (&DriverEntry) ? EFI_SUCCESS: EFI_NOT_FOUND;
+}
+
+////////////////////
diff --git a/Core/MdeModulePkg/Core/Dxe/Mem/Page.c b/Core/MdeModulePkg/Core/Dxe/Mem/Page.c
new file mode 100644
index 0000000000..9dbb85da7c
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Mem/Page.c
@@ -0,0 +1,1910 @@
+/** @file
+  UEFI Memory page management functions.
+
+Copyright (c) 2007 - 2015, 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 "DxeMain.h"
+#include "Imem.h"
+
+#define EFI_DEFAULT_PAGE_ALLOCATION_ALIGNMENT  (EFI_PAGE_SIZE)
+
+//
+// Entry for tracking the memory regions for each memory type to coalesce similar memory types
+//
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  BaseAddress;
+  EFI_PHYSICAL_ADDRESS  MaximumAddress;
+  UINT64                CurrentNumberOfPages;
+  UINT64                NumberOfPages;
+  UINTN                 InformationIndex;
+  BOOLEAN               Special;
+  BOOLEAN               Runtime;
+} EFI_MEMORY_TYPE_STATISTICS;
+
+//
+// MemoryMap - The current memory map
+//
+UINTN     mMemoryMapKey = 0;
+
+#define MAX_MAP_DEPTH 6
+
+///
+/// mMapDepth - depth of new descriptor stack
+///
+UINTN         mMapDepth = 0;
+///
+/// mMapStack - space to use as temp storage to build new map descriptors
+///
+MEMORY_MAP    mMapStack[MAX_MAP_DEPTH];
+UINTN         mFreeMapStack = 0;
+///
+/// This list maintain the free memory map list
+///
+LIST_ENTRY   mFreeMemoryMapEntryList = INITIALIZE_LIST_HEAD_VARIABLE (mFreeMemoryMapEntryList);
+BOOLEAN      mMemoryTypeInformationInitialized = FALSE;
+
+EFI_MEMORY_TYPE_STATISTICS mMemoryTypeStatistics[EfiMaxMemoryType + 1] = {
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  FALSE },  // EfiReservedMemoryType
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiLoaderCode
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiLoaderData
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiBootServicesCode
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiBootServicesData
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  TRUE  },  // EfiRuntimeServicesCode
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  TRUE  },  // EfiRuntimeServicesData
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiConventionalMemory
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiUnusableMemory
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  FALSE },  // EfiACPIReclaimMemory
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  FALSE },  // EfiACPIMemoryNVS
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiMemoryMappedIO
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiMemoryMappedIOPortSpace
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, TRUE,  TRUE  },  // EfiPalCode
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE },  // EfiPersistentMemory
+  { 0, MAX_ADDRESS, 0, 0, EfiMaxMemoryType, FALSE, FALSE }   // EfiMaxMemoryType
+};
+
+EFI_PHYSICAL_ADDRESS mDefaultMaximumAddress = MAX_ADDRESS;
+EFI_PHYSICAL_ADDRESS mDefaultBaseAddress = MAX_ADDRESS;
+
+EFI_MEMORY_TYPE_INFORMATION gMemoryTypeInformation[EfiMaxMemoryType + 1] = {
+  { EfiReservedMemoryType,      0 },
+  { EfiLoaderCode,              0 },
+  { EfiLoaderData,              0 },
+  { EfiBootServicesCode,        0 },
+  { EfiBootServicesData,        0 },
+  { EfiRuntimeServicesCode,     0 },
+  { EfiRuntimeServicesData,     0 },
+  { EfiConventionalMemory,      0 },
+  { EfiUnusableMemory,          0 },
+  { EfiACPIReclaimMemory,       0 },
+  { EfiACPIMemoryNVS,           0 },
+  { EfiMemoryMappedIO,          0 },
+  { EfiMemoryMappedIOPortSpace, 0 },
+  { EfiPalCode,                 0 },
+  { EfiPersistentMemory,        0 },
+  { EfiMaxMemoryType,           0 }
+};
+//
+// Only used when load module at fixed address feature is enabled. True means the memory is alreay successfully allocated
+// and ready to load the module in to specified address.or else, the memory is not ready and module will be loaded at a 
+//  address assigned by DXE core.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED   BOOLEAN       gLoadFixedAddressCodeMemoryReady = FALSE;
+
+/**
+  Enter critical section by gaining lock on gMemoryLock.
+
+**/
+VOID
+CoreAcquireMemoryLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&gMemoryLock);
+}
+
+
+
+/**
+  Exit critical section by releasing lock on gMemoryLock.
+
+**/
+VOID
+CoreReleaseMemoryLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&gMemoryLock);
+}
+
+
+
+
+/**
+  Internal function.  Removes a descriptor entry.
+
+  @param  Entry                  The entry to remove
+
+**/
+VOID
+RemoveMemoryMapEntry (
+  IN OUT MEMORY_MAP      *Entry
+  )
+{
+  RemoveEntryList (&Entry->Link);
+  Entry->Link.ForwardLink = NULL;
+
+  if (Entry->FromPages) {
+    //
+    // Insert the free memory map descriptor to the end of mFreeMemoryMapEntryList
+    //
+    InsertTailList (&mFreeMemoryMapEntryList, &Entry->Link);
+  }
+}
+
+/**
+  Internal function.  Adds a ranges to the memory map.
+  The range must not already exist in the map.
+
+  @param  Type                   The type of memory range to add
+  @param  Start                  The starting address in the memory range Must be
+                                 paged aligned
+  @param  End                    The last address in the range Must be the last
+                                 byte of a page
+  @param  Attribute              The attributes of the memory range to add
+
+**/
+VOID
+CoreAddRange (
+  IN EFI_MEMORY_TYPE          Type,
+  IN EFI_PHYSICAL_ADDRESS     Start,
+  IN EFI_PHYSICAL_ADDRESS     End,
+  IN UINT64                   Attribute
+  )
+{
+  LIST_ENTRY        *Link;
+  MEMORY_MAP        *Entry;
+
+  ASSERT ((Start & EFI_PAGE_MASK) == 0);
+  ASSERT (End > Start) ;
+
+  ASSERT_LOCKED (&gMemoryLock);
+
+  DEBUG ((DEBUG_PAGE, "AddRange: %lx-%lx to %d\n", Start, End, Type));
+  
+  //
+  // If memory of type EfiConventionalMemory is being added that includes the page 
+  // starting at address 0, then zero the page starting at address 0.  This has 
+  // two benifits.  It helps find NULL pointer bugs and it also maximizes 
+  // compatibility with operating systems that may evaluate memory in this page 
+  // for legacy data structures.  If memory of any other type is added starting 
+  // at address 0, then do not zero the page at address 0 because the page is being 
+  // used for other purposes.
+  //  
+  if (Type == EfiConventionalMemory && Start == 0 && (End >= EFI_PAGE_SIZE - 1)) {
+    SetMem ((VOID *)(UINTN)Start, EFI_PAGE_SIZE, 0);
+  }
+  
+  //
+  // Memory map being altered so updated key
+  //
+  mMemoryMapKey += 1;
+
+  //
+  // UEFI 2.0 added an event group for notificaiton on memory map changes.
+  // So we need to signal this Event Group every time the memory map changes.
+  // If we are in EFI 1.10 compatability mode no event groups will be
+  // found and nothing will happen we we call this function. These events
+  // will get signaled but since a lock is held around the call to this
+  // function the notificaiton events will only be called after this funciton
+  // returns and the lock is released.
+  //
+  CoreNotifySignalList (&gEfiEventMemoryMapChangeGuid);
+
+  //
+  // Look for adjoining memory descriptor
+  //
+
+  // Two memory descriptors can only be merged if they have the same Type
+  // and the same Attribute
+  //
+
+  Link = gMemoryMap.ForwardLink;
+  while (Link != &gMemoryMap) {
+    Entry = CR (Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+    Link  = Link->ForwardLink;
+
+    if (Entry->Type != Type) {
+      continue;
+    }
+
+    if (Entry->Attribute != Attribute) {
+      continue;
+    }
+
+    if (Entry->End + 1 == Start) {
+
+      Start = Entry->Start;
+      RemoveMemoryMapEntry (Entry);
+
+    } else if (Entry->Start == End + 1) {
+
+      End = Entry->End;
+      RemoveMemoryMapEntry (Entry);
+    }
+  }
+
+  //
+  // Add descriptor
+  //
+
+  mMapStack[mMapDepth].Signature     = MEMORY_MAP_SIGNATURE;
+  mMapStack[mMapDepth].FromPages      = FALSE;
+  mMapStack[mMapDepth].Type          = Type;
+  mMapStack[mMapDepth].Start         = Start;
+  mMapStack[mMapDepth].End           = End;
+  mMapStack[mMapDepth].VirtualStart  = 0;
+  mMapStack[mMapDepth].Attribute     = Attribute;
+  InsertTailList (&gMemoryMap, &mMapStack[mMapDepth].Link);
+
+  mMapDepth += 1;
+  ASSERT (mMapDepth < MAX_MAP_DEPTH);
+
+  return ;
+}
+
+/**
+  Internal function.  Deque a descriptor entry from the mFreeMemoryMapEntryList.
+  If the list is emtry, then allocate a new page to refuel the list.
+  Please Note this algorithm to allocate the memory map descriptor has a property
+  that the memory allocated for memory entries always grows, and will never really be freed
+  For example, if the current boot uses 2000 memory map entries at the maximum point, but
+  ends up with only 50 at the time the OS is booted, then the memory associated with the 1950
+  memory map entries is still allocated from EfiBootServicesMemory.
+
+
+  @return The Memory map descriptor dequed from the mFreeMemoryMapEntryList
+
+**/
+MEMORY_MAP *
+AllocateMemoryMapEntry (
+  VOID
+  )
+{
+  MEMORY_MAP*            FreeDescriptorEntries;
+  MEMORY_MAP*            Entry;
+  UINTN                  Index;
+
+  if (IsListEmpty (&mFreeMemoryMapEntryList)) {
+    //
+    // The list is empty, to allocate one page to refuel the list
+    //
+    FreeDescriptorEntries = CoreAllocatePoolPages (EfiBootServicesData, EFI_SIZE_TO_PAGES(DEFAULT_PAGE_ALLOCATION), DEFAULT_PAGE_ALLOCATION);
+    if(FreeDescriptorEntries != NULL) {
+      //
+      // Enque the free memmory map entries into the list
+      //
+      for (Index = 0; Index< DEFAULT_PAGE_ALLOCATION / sizeof(MEMORY_MAP); Index++) {
+        FreeDescriptorEntries[Index].Signature = MEMORY_MAP_SIGNATURE;
+        InsertTailList (&mFreeMemoryMapEntryList, &FreeDescriptorEntries[Index].Link);
+      }
+    } else {
+      return NULL;
+    }
+  }
+  //
+  // dequeue the first descriptor from the list
+  //
+  Entry = CR (mFreeMemoryMapEntryList.ForwardLink, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+  RemoveEntryList (&Entry->Link);
+
+  return Entry;
+}
+
+
+/**
+  Internal function.  Moves any memory descriptors that are on the
+  temporary descriptor stack to heap.
+
+**/
+VOID
+CoreFreeMemoryMapStack (
+  VOID
+  )
+{
+  MEMORY_MAP      *Entry;
+  MEMORY_MAP      *Entry2;
+  LIST_ENTRY      *Link2;
+
+  ASSERT_LOCKED (&gMemoryLock);
+
+  //
+  // If already freeing the map stack, then return
+  //
+  if (mFreeMapStack != 0) {
+    return ;
+  }
+
+  //
+  // Move the temporary memory descriptor stack into pool
+  //
+  mFreeMapStack += 1;
+
+  while (mMapDepth != 0) {
+    //
+    // Deque an memory map entry from mFreeMemoryMapEntryList
+    //
+    Entry = AllocateMemoryMapEntry ();
+
+    ASSERT (Entry);
+
+    //
+    // Update to proper entry
+    //
+    mMapDepth -= 1;
+
+    if (mMapStack[mMapDepth].Link.ForwardLink != NULL) {
+
+      //
+      // Move this entry to general memory
+      //
+      RemoveEntryList (&mMapStack[mMapDepth].Link);
+      mMapStack[mMapDepth].Link.ForwardLink = NULL;
+
+      CopyMem (Entry , &mMapStack[mMapDepth], sizeof (MEMORY_MAP));
+      Entry->FromPages = TRUE;
+
+      //
+      // Find insertion location
+      //
+      for (Link2 = gMemoryMap.ForwardLink; Link2 != &gMemoryMap; Link2 = Link2->ForwardLink) {
+        Entry2 = CR (Link2, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+        if (Entry2->FromPages && Entry2->Start > Entry->Start) {
+          break;
+        }
+      }
+
+      InsertTailList (Link2, &Entry->Link);
+
+    } else {
+      //
+      // This item of mMapStack[mMapDepth] has already been dequeued from gMemoryMap list,
+      // so here no need to move it to memory.
+      //
+      InsertTailList (&mFreeMemoryMapEntryList, &Entry->Link);
+    }
+  }
+
+  mFreeMapStack -= 1;
+}
+
+/**
+  Find untested but initialized memory regions in GCD map and convert them to be DXE allocatable.
+
+**/
+BOOLEAN
+PromoteMemoryResource (
+  VOID
+  )
+{
+  LIST_ENTRY         *Link;
+  EFI_GCD_MAP_ENTRY  *Entry;
+  BOOLEAN            Promoted;
+
+  DEBUG ((DEBUG_PAGE, "Promote the memory resource\n"));
+
+  CoreAcquireGcdMemoryLock ();
+
+  Promoted = FALSE;
+  Link = mGcdMemorySpaceMap.ForwardLink;
+  while (Link != &mGcdMemorySpaceMap) {
+
+    Entry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+
+    if (Entry->GcdMemoryType == EfiGcdMemoryTypeReserved &&
+        Entry->EndAddress < MAX_ADDRESS &&
+        (Entry->Capabilities & (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED)) ==
+          (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED)) {
+      //
+      // Update the GCD map
+      //
+      if ((Entry->Capabilities & EFI_MEMORY_MORE_RELIABLE) == EFI_MEMORY_MORE_RELIABLE) {
+        Entry->GcdMemoryType = EfiGcdMemoryTypeMoreReliable;
+      } else {
+        Entry->GcdMemoryType = EfiGcdMemoryTypeSystemMemory;
+      }
+      Entry->Capabilities |= EFI_MEMORY_TESTED;
+      Entry->ImageHandle  = gDxeCoreImageHandle;
+      Entry->DeviceHandle = NULL;
+
+      //
+      // Add to allocable system memory resource
+      //
+
+      CoreAddRange (
+        EfiConventionalMemory,
+        Entry->BaseAddress,
+        Entry->EndAddress,
+        Entry->Capabilities & ~(EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED | EFI_MEMORY_RUNTIME)
+        );
+      CoreFreeMemoryMapStack ();
+
+      Promoted = TRUE;
+    }
+
+    Link = Link->ForwardLink;
+  }
+
+  CoreReleaseGcdMemoryLock ();
+
+  return Promoted;
+}
+/**
+  This function try to allocate Runtime code & Boot time code memory range. If LMFA enabled, 2 patchable PCD 
+  PcdLoadFixAddressRuntimeCodePageNumber & PcdLoadFixAddressBootTimeCodePageNumber which are set by tools will record the 
+  size of boot time and runtime code.
+
+**/
+VOID
+CoreLoadingFixedAddressHook (
+  VOID
+  )
+{
+   UINT32                     RuntimeCodePageNumber;
+   UINT32                     BootTimeCodePageNumber;
+   EFI_PHYSICAL_ADDRESS       RuntimeCodeBase;
+   EFI_PHYSICAL_ADDRESS       BootTimeCodeBase;
+   EFI_STATUS                 Status;
+
+   //
+   // Make sure these 2 areas are not initialzied.
+   //
+   if (!gLoadFixedAddressCodeMemoryReady) {   
+     RuntimeCodePageNumber = PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber);
+     BootTimeCodePageNumber= PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber);
+     RuntimeCodeBase       = (EFI_PHYSICAL_ADDRESS)(gLoadModuleAtFixAddressConfigurationTable.DxeCodeTopAddress - EFI_PAGES_TO_SIZE (RuntimeCodePageNumber));
+     BootTimeCodeBase      = (EFI_PHYSICAL_ADDRESS)(RuntimeCodeBase - EFI_PAGES_TO_SIZE (BootTimeCodePageNumber));
+     //
+     // Try to allocate runtime memory.
+     //
+     Status = CoreAllocatePages (
+                       AllocateAddress,
+                       EfiRuntimeServicesCode,
+                       RuntimeCodePageNumber,
+                       &RuntimeCodeBase
+                       );
+     if (EFI_ERROR(Status)) {
+       //
+       // Runtime memory allocation failed 
+       //
+       return;
+     }
+     //
+     // Try to allocate boot memory.
+     //
+     Status = CoreAllocatePages (
+                       AllocateAddress,
+                       EfiBootServicesCode,
+                       BootTimeCodePageNumber,
+                       &BootTimeCodeBase
+                       );
+     if (EFI_ERROR(Status)) {
+       //
+     	 // boot memory allocation failed. Free Runtime code range and will try the allocation again when 
+     	 // new memory range is installed.
+     	 //
+     	 CoreFreePages (
+              RuntimeCodeBase,
+              RuntimeCodePageNumber
+              );
+       return;
+     }
+     gLoadFixedAddressCodeMemoryReady = TRUE;
+   } 
+   return;
+}  
+
+/**
+  Called to initialize the memory map and add descriptors to
+  the current descriptor list.
+  The first descriptor that is added must be general usable
+  memory as the addition allocates heap.
+
+  @param  Type                   The type of memory to add
+  @param  Start                  The starting address in the memory range Must be
+                                 page aligned
+  @param  NumberOfPages          The number of pages in the range
+  @param  Attribute              Attributes of the memory to add
+
+  @return None.  The range is added to the memory map
+
+**/
+VOID
+CoreAddMemoryDescriptor (
+  IN EFI_MEMORY_TYPE       Type,
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                NumberOfPages,
+  IN UINT64                Attribute
+  )
+{
+  EFI_PHYSICAL_ADDRESS        End;
+  EFI_STATUS                  Status;
+  UINTN                       Index;
+  UINTN                       FreeIndex;
+  
+  if ((Start & EFI_PAGE_MASK) != 0) {
+    return;
+  }
+
+  if (Type >= EfiMaxMemoryType && Type < MEMORY_TYPE_OEM_RESERVED_MIN) {
+    return;
+  }
+  CoreAcquireMemoryLock ();
+  End = Start + LShiftU64 (NumberOfPages, EFI_PAGE_SHIFT) - 1;
+  CoreAddRange (Type, Start, End, Attribute);
+  CoreFreeMemoryMapStack ();
+  CoreReleaseMemoryLock ();
+
+  //
+  // If Loading Module At Fixed Address feature is enabled. try to allocate memory with Runtime code & Boot time code type
+  //
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+    CoreLoadingFixedAddressHook();
+  }
+  
+  //
+  // Check to see if the statistics for the different memory types have already been established
+  //
+  if (mMemoryTypeInformationInitialized) {
+    return;
+  }
+
+  
+  //
+  // Loop through each memory type in the order specified by the gMemoryTypeInformation[] array
+  //
+  for (Index = 0; gMemoryTypeInformation[Index].Type != EfiMaxMemoryType; Index++) {
+    //
+    // Make sure the memory type in the gMemoryTypeInformation[] array is valid
+    //
+    Type = (EFI_MEMORY_TYPE) (gMemoryTypeInformation[Index].Type);
+    if ((UINT32)Type > EfiMaxMemoryType) {
+      continue;
+    }
+    if (gMemoryTypeInformation[Index].NumberOfPages != 0) {
+      //
+      // Allocate pages for the current memory type from the top of available memory
+      //
+      Status = CoreAllocatePages (
+                 AllocateAnyPages,
+                 Type,
+                 gMemoryTypeInformation[Index].NumberOfPages,
+                 &mMemoryTypeStatistics[Type].BaseAddress
+                 );
+      if (EFI_ERROR (Status)) {
+        //
+        // If an error occurs allocating the pages for the current memory type, then
+        // free all the pages allocates for the previous memory types and return.  This
+        // operation with be retied when/if more memory is added to the system
+        //
+        for (FreeIndex = 0; FreeIndex < Index; FreeIndex++) {
+          //
+          // Make sure the memory type in the gMemoryTypeInformation[] array is valid
+          //
+          Type = (EFI_MEMORY_TYPE) (gMemoryTypeInformation[FreeIndex].Type);
+          if ((UINT32)Type > EfiMaxMemoryType) {
+            continue;
+          }
+
+          if (gMemoryTypeInformation[FreeIndex].NumberOfPages != 0) {
+            CoreFreePages (
+              mMemoryTypeStatistics[Type].BaseAddress,
+              gMemoryTypeInformation[FreeIndex].NumberOfPages
+              );
+            mMemoryTypeStatistics[Type].BaseAddress    = 0;
+            mMemoryTypeStatistics[Type].MaximumAddress = MAX_ADDRESS;
+          }
+        }
+        return;
+      }
+
+      //
+      // Compute the address at the top of the current statistics
+      //
+      mMemoryTypeStatistics[Type].MaximumAddress =
+        mMemoryTypeStatistics[Type].BaseAddress +
+        LShiftU64 (gMemoryTypeInformation[Index].NumberOfPages, EFI_PAGE_SHIFT) - 1;
+
+      //
+      // If the current base address is the lowest address so far, then update the default
+      // maximum address
+      //
+      if (mMemoryTypeStatistics[Type].BaseAddress < mDefaultMaximumAddress) {
+        mDefaultMaximumAddress = mMemoryTypeStatistics[Type].BaseAddress - 1;
+      }
+    }
+  }
+
+  //
+  // There was enough system memory for all the the memory types were allocated.  So,
+  // those memory areas can be freed for future allocations, and all future memory
+  // allocations can occur within their respective bins
+  //
+  for (Index = 0; gMemoryTypeInformation[Index].Type != EfiMaxMemoryType; Index++) {
+    //
+    // Make sure the memory type in the gMemoryTypeInformation[] array is valid
+    //
+    Type = (EFI_MEMORY_TYPE) (gMemoryTypeInformation[Index].Type);
+    if ((UINT32)Type > EfiMaxMemoryType) {
+      continue;
+    }
+    if (gMemoryTypeInformation[Index].NumberOfPages != 0) {
+      CoreFreePages (
+        mMemoryTypeStatistics[Type].BaseAddress,
+        gMemoryTypeInformation[Index].NumberOfPages
+        );
+      mMemoryTypeStatistics[Type].NumberOfPages   = gMemoryTypeInformation[Index].NumberOfPages;
+      gMemoryTypeInformation[Index].NumberOfPages = 0;
+    }
+  }
+
+  //
+  // If the number of pages reserved for a memory type is 0, then all allocations for that type
+  // should be in the default range.
+  //
+  for (Type = (EFI_MEMORY_TYPE) 0; Type < EfiMaxMemoryType; Type++) {
+    for (Index = 0; gMemoryTypeInformation[Index].Type != EfiMaxMemoryType; Index++) {
+      if (Type == (EFI_MEMORY_TYPE)gMemoryTypeInformation[Index].Type) {
+        mMemoryTypeStatistics[Type].InformationIndex = Index;
+      }
+    }
+    mMemoryTypeStatistics[Type].CurrentNumberOfPages = 0;
+    if (mMemoryTypeStatistics[Type].MaximumAddress == MAX_ADDRESS) {
+      mMemoryTypeStatistics[Type].MaximumAddress = mDefaultMaximumAddress;
+    }
+  }
+
+  mMemoryTypeInformationInitialized = TRUE;
+}
+
+
+/**
+  Internal function.  Converts a memory range to the specified type or attributes.
+  The range must exist in the memory map.  Either ChangingType or
+  ChangingAttributes must be set, but not both.
+
+  @param  Start                  The first address of the range Must be page
+                                 aligned
+  @param  NumberOfPages          The number of pages to convert
+  @param  ChangingType           Boolean indicating that type value should be changed
+  @param  NewType                The new type for the memory range
+  @param  ChangingAttributes     Boolean indicating that attributes value should be changed
+  @param  NewAttributes          The new attributes for the memory range
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Could not find a descriptor cover the specified
+                                 range  or convertion not allowed.
+  @retval EFI_SUCCESS            Successfully converts the memory range to the
+                                 specified type.
+
+**/
+EFI_STATUS
+CoreConvertPagesEx (
+  IN UINT64           Start,
+  IN UINT64           NumberOfPages,
+  IN BOOLEAN          ChangingType,
+  IN EFI_MEMORY_TYPE  NewType,
+  IN BOOLEAN          ChangingAttributes,
+  IN UINT64           NewAttributes
+  )
+{
+
+  UINT64          NumberOfBytes;
+  UINT64          End;
+  UINT64          RangeEnd;
+  UINT64          Attribute;
+  EFI_MEMORY_TYPE MemType;
+  LIST_ENTRY      *Link;
+  MEMORY_MAP      *Entry;
+
+  Entry = NULL;
+  NumberOfBytes = LShiftU64 (NumberOfPages, EFI_PAGE_SHIFT);
+  End = Start + NumberOfBytes - 1;
+
+  ASSERT (NumberOfPages);
+  ASSERT ((Start & EFI_PAGE_MASK) == 0);
+  ASSERT (End > Start) ;
+  ASSERT_LOCKED (&gMemoryLock);
+  ASSERT ( (ChangingType == FALSE) || (ChangingAttributes == FALSE) );
+
+  if (NumberOfPages == 0 || ((Start & EFI_PAGE_MASK) != 0) || (Start > (Start + NumberOfBytes))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Convert the entire range
+  //
+
+  while (Start < End) {
+
+    //
+    // Find the entry that the covers the range
+    //
+    for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+      Entry = CR (Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+
+      if (Entry->Start <= Start && Entry->End > Start) {
+        break;
+      }
+    }
+
+    if (Link == &gMemoryMap) {
+      DEBUG ((DEBUG_ERROR | DEBUG_PAGE, "ConvertPages: failed to find range %lx - %lx\n", Start, End));
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Convert range to the end, or to the end of the descriptor
+    // if that's all we've got
+    //
+    RangeEnd = End;
+
+    ASSERT (Entry != NULL);
+    if (Entry->End < End) {
+      RangeEnd = Entry->End;
+    }
+
+    if (ChangingType) {
+      DEBUG ((DEBUG_PAGE, "ConvertRange: %lx-%lx to type %d\n", Start, RangeEnd, NewType));
+    }
+    if (ChangingAttributes) {
+      DEBUG ((DEBUG_PAGE, "ConvertRange: %lx-%lx to attr %lx\n", Start, RangeEnd, NewAttributes));
+    }
+
+    if (ChangingType) {
+      //
+      // Debug code - verify conversion is allowed
+      //
+      if (!(NewType == EfiConventionalMemory ? 1 : 0) ^ (Entry->Type == EfiConventionalMemory ? 1 : 0)) {
+        DEBUG ((DEBUG_ERROR | DEBUG_PAGE, "ConvertPages: Incompatible memory types\n"));
+        return EFI_NOT_FOUND;
+      }
+
+      //
+      // Update counters for the number of pages allocated to each memory type
+      //
+      if ((UINT32)Entry->Type < EfiMaxMemoryType) {
+        if ((Start >= mMemoryTypeStatistics[Entry->Type].BaseAddress && Start <= mMemoryTypeStatistics[Entry->Type].MaximumAddress) ||
+            (Start >= mDefaultBaseAddress && Start <= mDefaultMaximumAddress)                                                          ) {
+          if (NumberOfPages > mMemoryTypeStatistics[Entry->Type].CurrentNumberOfPages) {
+            mMemoryTypeStatistics[Entry->Type].CurrentNumberOfPages = 0;
+          } else {
+            mMemoryTypeStatistics[Entry->Type].CurrentNumberOfPages -= NumberOfPages;
+          }
+        }
+      }
+
+      if ((UINT32)NewType < EfiMaxMemoryType) {
+        if ((Start >= mMemoryTypeStatistics[NewType].BaseAddress && Start <= mMemoryTypeStatistics[NewType].MaximumAddress) ||
+            (Start >= mDefaultBaseAddress && Start <= mDefaultMaximumAddress)                                                  ) {
+          mMemoryTypeStatistics[NewType].CurrentNumberOfPages += NumberOfPages;
+          if (mMemoryTypeStatistics[NewType].CurrentNumberOfPages > gMemoryTypeInformation[mMemoryTypeStatistics[NewType].InformationIndex].NumberOfPages) {
+            gMemoryTypeInformation[mMemoryTypeStatistics[NewType].InformationIndex].NumberOfPages = (UINT32)mMemoryTypeStatistics[NewType].CurrentNumberOfPages;
+          }
+        }
+      }
+    }
+
+    //
+    // Pull range out of descriptor
+    //
+    if (Entry->Start == Start) {
+
+      //
+      // Clip start
+      //
+      Entry->Start = RangeEnd + 1;
+
+    } else if (Entry->End == RangeEnd) {
+
+      //
+      // Clip end
+      //
+      Entry->End = Start - 1;
+
+    } else {
+
+      //
+      // Pull it out of the center, clip current
+      //
+
+      //
+      // Add a new one
+      //
+      mMapStack[mMapDepth].Signature = MEMORY_MAP_SIGNATURE;
+      mMapStack[mMapDepth].FromPages  = FALSE;
+      mMapStack[mMapDepth].Type      = Entry->Type;
+      mMapStack[mMapDepth].Start     = RangeEnd+1;
+      mMapStack[mMapDepth].End       = Entry->End;
+
+      //
+      // Inherit Attribute from the Memory Descriptor that is being clipped
+      //
+      mMapStack[mMapDepth].Attribute = Entry->Attribute;
+
+      Entry->End = Start - 1;
+      ASSERT (Entry->Start < Entry->End);
+
+      Entry = &mMapStack[mMapDepth];
+      InsertTailList (&gMemoryMap, &Entry->Link);
+
+      mMapDepth += 1;
+      ASSERT (mMapDepth < MAX_MAP_DEPTH);
+    }
+
+    //
+    // The new range inherits the same Attribute as the Entry
+    // it is being cut out of unless attributes are being changed
+    //
+    if (ChangingType) {
+      Attribute = Entry->Attribute;
+      MemType = NewType;
+    } else {
+      Attribute = NewAttributes;
+      MemType = Entry->Type;
+    }
+
+    //
+    // If the descriptor is empty, then remove it from the map
+    //
+    if (Entry->Start == Entry->End + 1) {
+      RemoveMemoryMapEntry (Entry);
+      Entry = NULL;
+    }
+
+    //
+    // Add our new range in
+    //
+    CoreAddRange (MemType, Start, RangeEnd, Attribute);
+    if (ChangingType && (MemType == EfiConventionalMemory)) {
+      //
+      // Avoid calling DEBUG_CLEAR_MEMORY() for an address of 0 because this
+      // macro will ASSERT() if address is 0.  Instead, CoreAddRange() guarantees
+      // that the page starting at address 0 is always filled with zeros.
+      //
+      if (Start == 0) {
+        if (RangeEnd > EFI_PAGE_SIZE) {
+          DEBUG_CLEAR_MEMORY ((VOID *)(UINTN) EFI_PAGE_SIZE, (UINTN) (RangeEnd - EFI_PAGE_SIZE + 1));
+        }
+      } else {
+        DEBUG_CLEAR_MEMORY ((VOID *)(UINTN) Start, (UINTN) (RangeEnd - Start + 1));
+      }
+    }
+
+    //
+    // Move any map descriptor stack to general pool
+    //
+    CoreFreeMemoryMapStack ();
+
+    //
+    // Bump the starting address, and convert the next range
+    //
+    Start = RangeEnd + 1;
+  }
+
+  //
+  // Converted the whole range, done
+  //
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Internal function.  Converts a memory range to the specified type.
+  The range must exist in the memory map.
+
+  @param  Start                  The first address of the range Must be page
+                                 aligned
+  @param  NumberOfPages          The number of pages to convert
+  @param  NewType                The new type for the memory range
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Could not find a descriptor cover the specified
+                                 range  or convertion not allowed.
+  @retval EFI_SUCCESS            Successfully converts the memory range to the
+                                 specified type.
+
+**/
+EFI_STATUS
+CoreConvertPages (
+  IN UINT64           Start,
+  IN UINT64           NumberOfPages,
+  IN EFI_MEMORY_TYPE  NewType
+  )
+{
+  return CoreConvertPagesEx(Start, NumberOfPages, TRUE, NewType, FALSE, 0);
+}
+
+
+/**
+  Internal function.  Converts a memory range to use new attributes.
+
+  @param  Start                  The first address of the range Must be page
+                                 aligned
+  @param  NumberOfPages          The number of pages to convert
+  @param  NewAttributes          The new attributes value for the range.
+
+**/
+VOID
+CoreUpdateMemoryAttributes (
+  IN EFI_PHYSICAL_ADDRESS  Start,
+  IN UINT64                NumberOfPages,
+  IN UINT64                NewAttributes
+  )
+{
+  CoreAcquireMemoryLock ();
+
+  //
+  // Update the attributes to the new value
+  //
+  CoreConvertPagesEx(Start, NumberOfPages, FALSE, (EFI_MEMORY_TYPE)0, TRUE, NewAttributes);
+
+  CoreReleaseMemoryLock ();
+}
+
+
+/**
+  Internal function. Finds a consecutive free page range below
+  the requested address.
+
+  @param  MaxAddress             The address that the range must be below
+  @param  MinAddress             The address that the range must be above
+  @param  NumberOfPages          Number of pages needed
+  @param  NewType                The type of memory the range is going to be
+                                 turned into
+  @param  Alignment              Bits to align with
+
+  @return The base address of the range, or 0 if the range was not found
+
+**/
+UINT64
+CoreFindFreePagesI (
+  IN UINT64           MaxAddress,
+  IN UINT64           MinAddress,
+  IN UINT64           NumberOfPages,
+  IN EFI_MEMORY_TYPE  NewType,
+  IN UINTN            Alignment
+  )
+{
+  UINT64          NumberOfBytes;
+  UINT64          Target;
+  UINT64          DescStart;
+  UINT64          DescEnd;
+  UINT64          DescNumberOfBytes;
+  LIST_ENTRY      *Link;
+  MEMORY_MAP      *Entry;
+
+  if ((MaxAddress < EFI_PAGE_MASK) ||(NumberOfPages == 0)) {
+    return 0;
+  }
+
+  if ((MaxAddress & EFI_PAGE_MASK) != EFI_PAGE_MASK) {
+
+    //
+    // If MaxAddress is not aligned to the end of a page
+    //
+
+    //
+    // Change MaxAddress to be 1 page lower
+    //
+    MaxAddress -= (EFI_PAGE_MASK + 1);
+
+    //
+    // Set MaxAddress to a page boundary
+    //
+    MaxAddress &= ~(UINT64)EFI_PAGE_MASK;
+
+    //
+    // Set MaxAddress to end of the page
+    //
+    MaxAddress |= EFI_PAGE_MASK;
+  }
+
+  NumberOfBytes = LShiftU64 (NumberOfPages, EFI_PAGE_SHIFT);
+  Target = 0;
+
+  for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+    Entry = CR (Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+
+    //
+    // If it's not a free entry, don't bother with it
+    //
+    if (Entry->Type != EfiConventionalMemory) {
+      continue;
+    }
+
+    DescStart = Entry->Start;
+    DescEnd = Entry->End;
+
+    //
+    // If desc is past max allowed address or below min allowed address, skip it
+    //
+    if ((DescStart >= MaxAddress) || (DescEnd < MinAddress)) {
+      continue;
+    }
+
+    //
+    // If desc ends past max allowed address, clip the end
+    //
+    if (DescEnd >= MaxAddress) {
+      DescEnd = MaxAddress;
+    }
+
+    DescEnd = ((DescEnd + 1) & (~(Alignment - 1))) - 1;
+
+    // Skip if DescEnd is less than DescStart after alignment clipping
+    if (DescEnd < DescStart) {
+      continue;
+    }
+
+    //
+    // Compute the number of bytes we can used from this
+    // descriptor, and see it's enough to satisfy the request
+    //
+    DescNumberOfBytes = DescEnd - DescStart + 1;
+
+    if (DescNumberOfBytes >= NumberOfBytes) {
+      //
+      // If the start of the allocated range is below the min address allowed, skip it
+      //
+      if ((DescEnd - NumberOfBytes + 1) < MinAddress) {
+        continue;
+      }
+
+      //
+      // If this is the best match so far remember it
+      //
+      if (DescEnd > Target) {
+        Target = DescEnd;
+      }
+    }
+  }
+
+  //
+  // If this is a grow down, adjust target to be the allocation base
+  //
+  Target -= NumberOfBytes - 1;
+
+  //
+  // If we didn't find a match, return 0
+  //
+  if ((Target & EFI_PAGE_MASK) != 0) {
+    return 0;
+  }
+
+  return Target;
+}
+
+
+/**
+  Internal function.  Finds a consecutive free page range below
+  the requested address
+
+  @param  MaxAddress             The address that the range must be below
+  @param  NoPages                Number of pages needed
+  @param  NewType                The type of memory the range is going to be
+                                 turned into
+  @param  Alignment              Bits to align with
+
+  @return The base address of the range, or 0 if the range was not found.
+
+**/
+UINT64
+FindFreePages (
+    IN UINT64           MaxAddress,
+    IN UINT64           NoPages,
+    IN EFI_MEMORY_TYPE  NewType,
+    IN UINTN            Alignment
+    )
+{
+  UINT64   Start;
+
+  //
+  // Attempt to find free pages in the preferred bin based on the requested memory type
+  //
+  if ((UINT32)NewType < EfiMaxMemoryType && MaxAddress >= mMemoryTypeStatistics[NewType].MaximumAddress) {
+    Start = CoreFindFreePagesI (
+              mMemoryTypeStatistics[NewType].MaximumAddress, 
+              mMemoryTypeStatistics[NewType].BaseAddress, 
+              NoPages, 
+              NewType, 
+              Alignment
+              );
+    if (Start != 0) {
+      return Start;
+    }
+  }
+
+  //
+  // Attempt to find free pages in the default allocation bin
+  //
+  if (MaxAddress >= mDefaultMaximumAddress) {
+    Start = CoreFindFreePagesI (mDefaultMaximumAddress, 0, NoPages, NewType, Alignment);
+    if (Start != 0) {
+      if (Start < mDefaultBaseAddress) {
+        mDefaultBaseAddress = Start;
+      }
+      return Start;
+    }
+  }
+
+  //
+  // The allocation did not succeed in any of the prefered bins even after 
+  // promoting resources. Attempt to find free pages anywhere is the requested 
+  // address range.  If this allocation fails, then there are not enough 
+  // resources anywhere to satisfy the request.
+  //
+  Start = CoreFindFreePagesI (MaxAddress, 0, NoPages, NewType, Alignment);
+  if (Start != 0) {
+    return Start;
+  }
+
+  //
+  // If allocations from the preferred bins fail, then attempt to promote memory resources.
+  //
+  if (!PromoteMemoryResource ()) {
+    return 0;
+  }
+
+  //
+  // If any memory resources were promoted, then re-attempt the allocation
+  //
+  return FindFreePages (MaxAddress, NoPages, NewType, Alignment);
+}
+
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @return Status. On success, Memory is filled in with the base address allocated
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in
+                                 spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalAllocatePages (
+  IN EFI_ALLOCATE_TYPE      Type,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  NumberOfPages,
+  IN OUT EFI_PHYSICAL_ADDRESS  *Memory
+  )
+{
+  EFI_STATUS      Status;
+  UINT64          Start;
+  UINT64          MaxAddress;
+  UINTN           Alignment;
+
+  if ((UINT32)Type >= MaxAllocateType) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((MemoryType >= EfiMaxMemoryType && MemoryType < MEMORY_TYPE_OEM_RESERVED_MIN) ||
+       (MemoryType == EfiConventionalMemory) || (MemoryType == EfiPersistentMemory)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Memory == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Alignment = EFI_DEFAULT_PAGE_ALLOCATION_ALIGNMENT;
+
+  if  (MemoryType == EfiACPIReclaimMemory   ||
+       MemoryType == EfiACPIMemoryNVS       ||
+       MemoryType == EfiRuntimeServicesCode ||
+       MemoryType == EfiRuntimeServicesData) {
+
+    Alignment = EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT;
+  }
+
+  if (Type == AllocateAddress) {
+    if ((*Memory & (Alignment - 1)) != 0) {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  NumberOfPages += EFI_SIZE_TO_PAGES (Alignment) - 1;
+  NumberOfPages &= ~(EFI_SIZE_TO_PAGES (Alignment) - 1);
+
+  //
+  // If this is for below a particular address, then
+  //
+  Start = *Memory;
+
+  //
+  // The max address is the max natively addressable address for the processor
+  //
+  MaxAddress = MAX_ADDRESS;
+
+  if (Type == AllocateMaxAddress) {
+    MaxAddress = Start;
+  }
+
+  CoreAcquireMemoryLock ();
+
+  //
+  // If not a specific address, then find an address to allocate
+  //
+  if (Type != AllocateAddress) {
+    Start = FindFreePages (MaxAddress, NumberOfPages, MemoryType, Alignment);
+    if (Start == 0) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+  }
+
+  //
+  // Convert pages from FreeMemory to the requested type
+  //
+  Status = CoreConvertPages (Start, NumberOfPages, MemoryType);
+
+Done:
+  CoreReleaseMemoryLock ();
+
+  if (!EFI_ERROR (Status)) {
+    *Memory = Start;
+  }
+
+  return Status;
+}
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @return Status. On success, Memory is filled in with the base address allocated
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in
+                                 spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePages (
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 NumberOfPages,
+  OUT EFI_PHYSICAL_ADDRESS  *Memory
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = CoreInternalAllocatePages (Type, MemoryType, NumberOfPages, Memory);
+  if (!EFI_ERROR (Status)) {
+    CoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionAllocatePages, MemoryType, EFI_PAGES_TO_SIZE (NumberOfPages), (VOID *) (UINTN) *Memory);
+  }
+  return Status;
+}
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS         -Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalFreePages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  )
+{
+  EFI_STATUS      Status;
+  LIST_ENTRY      *Link;
+  MEMORY_MAP      *Entry;
+  UINTN           Alignment;
+
+  //
+  // Free the range
+  //
+  CoreAcquireMemoryLock ();
+
+  //
+  // Find the entry that the covers the range
+  //
+  Entry = NULL;
+  for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+    Entry = CR(Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+    if (Entry->Start <= Memory && Entry->End > Memory) {
+        break;
+    }
+  }
+  if (Link == &gMemoryMap) {
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+
+  Alignment = EFI_DEFAULT_PAGE_ALLOCATION_ALIGNMENT;
+
+  ASSERT (Entry != NULL);
+  if  (Entry->Type == EfiACPIReclaimMemory   ||
+       Entry->Type == EfiACPIMemoryNVS       ||
+       Entry->Type == EfiRuntimeServicesCode ||
+       Entry->Type == EfiRuntimeServicesData) {
+
+    Alignment = EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT;
+
+  }
+
+  if ((Memory & (Alignment - 1)) != 0) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  NumberOfPages += EFI_SIZE_TO_PAGES (Alignment) - 1;
+  NumberOfPages &= ~(EFI_SIZE_TO_PAGES (Alignment) - 1);
+
+  Status = CoreConvertPages (Memory, NumberOfPages, EfiConventionalMemory);
+
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+Done:
+  CoreReleaseMemoryLock ();
+  return Status;
+}
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS         -Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePages (
+  IN EFI_PHYSICAL_ADDRESS  Memory,
+  IN UINTN                 NumberOfPages
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = CoreInternalFreePages (Memory, NumberOfPages);
+  if (!EFI_ERROR (Status)) {
+    CoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionFreePages, (EFI_MEMORY_TYPE) 0, EFI_PAGES_TO_SIZE (NumberOfPages), (VOID *) (UINTN) Memory);
+  }
+  return Status;
+}
+
+/**
+  This function checks to see if the last memory map descriptor in a memory map
+  can be merged with any of the other memory map descriptors in a memorymap.
+  Memory descriptors may be merged if they are adjacent and have the same type
+  and attributes.
+
+  @param  MemoryMap              A pointer to the start of the memory map.
+  @param  MemoryMapDescriptor    A pointer to the last descriptor in MemoryMap.
+  @param  DescriptorSize         The size, in bytes, of an individual
+                                 EFI_MEMORY_DESCRIPTOR.
+
+  @return  A pointer to the next available descriptor in MemoryMap
+
+**/
+EFI_MEMORY_DESCRIPTOR *
+MergeMemoryMapDescriptor (
+  IN EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN EFI_MEMORY_DESCRIPTOR  *MemoryMapDescriptor,
+  IN UINTN                  DescriptorSize
+  )
+{
+  //
+  // Traverse the array of descriptors in MemoryMap
+  //
+  for (; MemoryMap != MemoryMapDescriptor; MemoryMap = NEXT_MEMORY_DESCRIPTOR (MemoryMap, DescriptorSize)) {
+    //
+    // Check to see if the Type fields are identical.
+    //
+    if (MemoryMap->Type != MemoryMapDescriptor->Type) {
+      continue;
+    }
+
+    //
+    // Check to see if the Attribute fields are identical.
+    //
+    if (MemoryMap->Attribute != MemoryMapDescriptor->Attribute) {
+      continue;
+    }
+
+    //
+    // Check to see if MemoryMapDescriptor is immediately above MemoryMap
+    //
+    if (MemoryMap->PhysicalStart + EFI_PAGES_TO_SIZE ((UINTN)MemoryMap->NumberOfPages) == MemoryMapDescriptor->PhysicalStart) { 
+      //
+      // Merge MemoryMapDescriptor into MemoryMap
+      //
+      MemoryMap->NumberOfPages += MemoryMapDescriptor->NumberOfPages;
+
+      //
+      // Return MemoryMapDescriptor as the next available slot int he MemoryMap array
+      //
+      return MemoryMapDescriptor;
+    }
+
+    //
+    // Check to see if MemoryMapDescriptor is immediately below MemoryMap
+    //
+    if (MemoryMap->PhysicalStart - EFI_PAGES_TO_SIZE ((UINTN)MemoryMapDescriptor->NumberOfPages) == MemoryMapDescriptor->PhysicalStart) {
+      //
+      // Merge MemoryMapDescriptor into MemoryMap
+      //
+      MemoryMap->PhysicalStart  = MemoryMapDescriptor->PhysicalStart;
+      MemoryMap->VirtualStart   = MemoryMapDescriptor->VirtualStart;
+      MemoryMap->NumberOfPages += MemoryMapDescriptor->NumberOfPages;
+
+      //
+      // Return MemoryMapDescriptor as the next available slot int he MemoryMap array
+      //
+      return MemoryMapDescriptor;
+    }
+  }
+
+  //
+  // MemoryMapDescrtiptor could not be merged with any descriptors in MemoryMap.
+  //
+  // Return the slot immediately after MemoryMapDescriptor as the next available 
+  // slot in the MemoryMap array
+  //
+  return NEXT_MEMORY_DESCRIPTOR (MemoryMapDescriptor, DescriptorSize);
+}
+
+/**
+  This function returns a copy of the current memory map. The map is an array of
+  memory descriptors, each of which describes a contiguous block of memory.
+
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 the buffer allocated by the caller.  On output,
+                                 it is the size of the buffer returned by the
+                                 firmware  if the buffer was large enough, or the
+                                 size of the buffer needed  to contain the map if
+                                 the buffer was too small.
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MapKey                 A pointer to the location in which firmware
+                                 returns the key for the current memory map.
+  @param  DescriptorSize         A pointer to the location in which firmware
+                                 returns the size, in bytes, of an individual
+                                 EFI_MEMORY_DESCRIPTOR.
+  @param  DescriptorVersion      A pointer to the location in which firmware
+                                 returns the version number associated with the
+                                 EFI_MEMORY_DESCRIPTOR.
+
+  @retval EFI_SUCCESS            The memory map was returned in the MemoryMap
+                                 buffer.
+  @retval EFI_BUFFER_TOO_SMALL   The MemoryMap buffer was too small. The current
+                                 buffer size needed to hold the memory map is
+                                 returned in MemoryMapSize.
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemoryMap (
+  IN OUT UINTN                  *MemoryMapSize,
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  OUT UINTN                     *MapKey,
+  OUT UINTN                     *DescriptorSize,
+  OUT UINT32                    *DescriptorVersion
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Size;
+  UINTN                             BufferSize;
+  UINTN                             NumberOfEntries;
+  LIST_ENTRY                        *Link;
+  MEMORY_MAP                        *Entry;
+  EFI_GCD_MAP_ENTRY                 *GcdMapEntry;
+  EFI_GCD_MAP_ENTRY                 MergeGcdMapEntry;
+  EFI_MEMORY_TYPE                   Type;
+  EFI_MEMORY_DESCRIPTOR             *MemoryMapStart;
+
+  //
+  // Make sure the parameters are valid
+  //
+  if (MemoryMapSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireGcdMemoryLock ();
+
+  //
+  // Count the number of Reserved and runtime MMIO entries
+  // And, count the number of Persistent entries.
+  //
+  NumberOfEntries = 0;
+  for (Link = mGcdMemorySpaceMap.ForwardLink; Link != &mGcdMemorySpaceMap; Link = Link->ForwardLink) {
+    GcdMapEntry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+    if ((GcdMapEntry->GcdMemoryType == EfiGcdMemoryTypePersistentMemory) || 
+        (GcdMapEntry->GcdMemoryType == EfiGcdMemoryTypeReserved) ||
+        ((GcdMapEntry->GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) &&
+        ((GcdMapEntry->Attributes & EFI_MEMORY_RUNTIME) == EFI_MEMORY_RUNTIME))) {
+      NumberOfEntries ++;
+    }
+  }
+
+  Size = sizeof (EFI_MEMORY_DESCRIPTOR);
+
+  //
+  // Make sure Size != sizeof(EFI_MEMORY_DESCRIPTOR). This will
+  // prevent people from having pointer math bugs in their code.
+  // now you have to use *DescriptorSize to make things work.
+  //
+  Size += sizeof(UINT64) - (Size % sizeof (UINT64));
+
+  if (DescriptorSize != NULL) {
+    *DescriptorSize = Size;
+  }
+
+  if (DescriptorVersion != NULL) {
+    *DescriptorVersion = EFI_MEMORY_DESCRIPTOR_VERSION;
+  }
+
+  CoreAcquireMemoryLock ();
+
+  //
+  // Compute the buffer size needed to fit the entire map
+  //
+  BufferSize = Size * NumberOfEntries;
+  for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+    BufferSize += Size;
+  }
+
+  if (*MemoryMapSize < BufferSize) {
+    Status = EFI_BUFFER_TOO_SMALL;
+    goto Done;
+  }
+
+  if (MemoryMap == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto Done;
+  }
+
+  //
+  // Build the map
+  //
+  ZeroMem (MemoryMap, BufferSize);
+  MemoryMapStart = MemoryMap;
+  for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+    Entry = CR (Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+    ASSERT (Entry->VirtualStart == 0);
+
+    //
+    // Convert internal map into an EFI_MEMORY_DESCRIPTOR
+    //
+    MemoryMap->Type           = Entry->Type;
+    MemoryMap->PhysicalStart  = Entry->Start;
+    MemoryMap->VirtualStart   = Entry->VirtualStart;
+    MemoryMap->NumberOfPages  = RShiftU64 (Entry->End - Entry->Start + 1, EFI_PAGE_SHIFT);
+    //
+    // If the memory type is EfiConventionalMemory, then determine if the range is part of a
+    // memory type bin and needs to be converted to the same memory type as the rest of the
+    // memory type bin in order to minimize EFI Memory Map changes across reboots.  This
+    // improves the chances for a successful S4 resume in the presence of minor page allocation
+    // differences across reboots.
+    //
+    if (MemoryMap->Type == EfiConventionalMemory) {
+      for (Type = (EFI_MEMORY_TYPE) 0; Type < EfiMaxMemoryType; Type++) {
+        if (mMemoryTypeStatistics[Type].Special                        &&
+            mMemoryTypeStatistics[Type].NumberOfPages > 0              &&
+            Entry->Start >= mMemoryTypeStatistics[Type].BaseAddress    &&
+            Entry->End   <= mMemoryTypeStatistics[Type].MaximumAddress) {
+          MemoryMap->Type = Type;
+        }
+      }
+    }
+    MemoryMap->Attribute = Entry->Attribute;
+    if (MemoryMap->Type < EfiMaxMemoryType) {
+      if (mMemoryTypeStatistics[MemoryMap->Type].Runtime) {
+        MemoryMap->Attribute |= EFI_MEMORY_RUNTIME;
+      }
+    }
+
+    //
+    // Check to see if the new Memory Map Descriptor can be merged with an 
+    // existing descriptor if they are adjacent and have the same attributes
+    //
+    MemoryMap = MergeMemoryMapDescriptor (MemoryMapStart, MemoryMap, Size);
+  }
+
+ 
+  ZeroMem (&MergeGcdMapEntry, sizeof (MergeGcdMapEntry));
+  GcdMapEntry = NULL;
+  for (Link = mGcdMemorySpaceMap.ForwardLink; ; Link = Link->ForwardLink) {
+    if (Link != &mGcdMemorySpaceMap) {
+      //
+      // Merge adjacent same type and attribute GCD memory range
+      //
+      GcdMapEntry = CR (Link, EFI_GCD_MAP_ENTRY, Link, EFI_GCD_MAP_SIGNATURE);
+  
+      if ((MergeGcdMapEntry.Capabilities == GcdMapEntry->Capabilities) && 
+          (MergeGcdMapEntry.Attributes == GcdMapEntry->Attributes) &&
+          (MergeGcdMapEntry.GcdMemoryType == GcdMapEntry->GcdMemoryType) &&
+          (MergeGcdMapEntry.GcdIoType == GcdMapEntry->GcdIoType)) {
+        MergeGcdMapEntry.EndAddress  = GcdMapEntry->EndAddress;
+        continue;
+      }
+    }
+
+    if ((MergeGcdMapEntry.GcdMemoryType == EfiGcdMemoryTypeReserved) ||
+        ((MergeGcdMapEntry.GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) &&
+        ((MergeGcdMapEntry.Attributes & EFI_MEMORY_RUNTIME) == EFI_MEMORY_RUNTIME))) {
+      //
+      // Page Align GCD range is required. When it is converted to EFI_MEMORY_DESCRIPTOR, 
+      // it will be recorded as page PhysicalStart and NumberOfPages. 
+      //
+      ASSERT ((MergeGcdMapEntry.BaseAddress & EFI_PAGE_MASK) == 0);
+      ASSERT (((MergeGcdMapEntry.EndAddress - MergeGcdMapEntry.BaseAddress + 1) & EFI_PAGE_MASK) == 0);
+      
+      // 
+      // Create EFI_MEMORY_DESCRIPTOR for every Reserved and runtime MMIO GCD entries
+      //
+      MemoryMap->PhysicalStart = MergeGcdMapEntry.BaseAddress;
+      MemoryMap->VirtualStart  = 0;
+      MemoryMap->NumberOfPages = RShiftU64 ((MergeGcdMapEntry.EndAddress - MergeGcdMapEntry.BaseAddress + 1), EFI_PAGE_SHIFT);
+      MemoryMap->Attribute     = (MergeGcdMapEntry.Attributes & ~EFI_MEMORY_PORT_IO) | 
+                                (MergeGcdMapEntry.Capabilities & (EFI_MEMORY_RP | EFI_MEMORY_WP | EFI_MEMORY_XP | EFI_MEMORY_RO |
+                                EFI_MEMORY_UC | EFI_MEMORY_UCE | EFI_MEMORY_WC | EFI_MEMORY_WT | EFI_MEMORY_WB));
+
+      if (MergeGcdMapEntry.GcdMemoryType == EfiGcdMemoryTypeReserved) {
+        MemoryMap->Type = EfiReservedMemoryType;
+      } else if (MergeGcdMapEntry.GcdMemoryType == EfiGcdMemoryTypeMemoryMappedIo) {
+        if ((MergeGcdMapEntry.Attributes & EFI_MEMORY_PORT_IO) == EFI_MEMORY_PORT_IO) {
+          MemoryMap->Type = EfiMemoryMappedIOPortSpace;
+        } else {
+          MemoryMap->Type = EfiMemoryMappedIO;
+        }
+      }
+
+      //
+      // Check to see if the new Memory Map Descriptor can be merged with an 
+      // existing descriptor if they are adjacent and have the same attributes
+      //
+      MemoryMap = MergeMemoryMapDescriptor (MemoryMapStart, MemoryMap, Size);
+    }
+    
+    if (MergeGcdMapEntry.GcdMemoryType == EfiGcdMemoryTypePersistentMemory) {
+      //
+      // Page Align GCD range is required. When it is converted to EFI_MEMORY_DESCRIPTOR, 
+      // it will be recorded as page PhysicalStart and NumberOfPages. 
+      //
+      ASSERT ((MergeGcdMapEntry.BaseAddress & EFI_PAGE_MASK) == 0);
+      ASSERT (((MergeGcdMapEntry.EndAddress - MergeGcdMapEntry.BaseAddress + 1) & EFI_PAGE_MASK) == 0);
+
+      // 
+      // Create EFI_MEMORY_DESCRIPTOR for every Persistent GCD entries
+      //
+      MemoryMap->PhysicalStart = MergeGcdMapEntry.BaseAddress;
+      MemoryMap->VirtualStart  = 0;
+      MemoryMap->NumberOfPages = RShiftU64 ((MergeGcdMapEntry.EndAddress - MergeGcdMapEntry.BaseAddress + 1), EFI_PAGE_SHIFT);
+      MemoryMap->Attribute     = MergeGcdMapEntry.Attributes | EFI_MEMORY_NV | 
+                                (MergeGcdMapEntry.Capabilities & (EFI_MEMORY_RP | EFI_MEMORY_WP | EFI_MEMORY_XP | EFI_MEMORY_RO |
+                                EFI_MEMORY_UC | EFI_MEMORY_UCE | EFI_MEMORY_WC | EFI_MEMORY_WT | EFI_MEMORY_WB));
+      MemoryMap->Type          = EfiPersistentMemory;
+      
+      //
+      // Check to see if the new Memory Map Descriptor can be merged with an 
+      // existing descriptor if they are adjacent and have the same attributes
+      //
+      MemoryMap = MergeMemoryMapDescriptor (MemoryMapStart, MemoryMap, Size);
+    }
+    if (Link == &mGcdMemorySpaceMap) {
+      //
+      // break loop when arrive at head.
+      //
+      break;
+    }
+    if (GcdMapEntry != NULL) {
+      //
+      // Copy new GCD map entry for the following GCD range merge
+      //
+      CopyMem (&MergeGcdMapEntry, GcdMapEntry, sizeof (MergeGcdMapEntry));
+    }
+  }
+
+  //
+  // Compute the size of the buffer actually used after all memory map descriptor merge operations
+  //
+  BufferSize = ((UINT8 *)MemoryMap - (UINT8 *)MemoryMapStart);
+
+  Status = EFI_SUCCESS;
+
+Done:
+  //
+  // Update the map key finally
+  //
+  if (MapKey != NULL) {
+    *MapKey = mMemoryMapKey;
+  }
+
+  CoreReleaseMemoryLock ();
+
+  CoreReleaseGcdMemoryLock ();
+
+  *MemoryMapSize = BufferSize;
+
+  return Status;
+}
+
+
+/**
+  Internal function.  Used by the pool functions to allocate pages
+  to back pool allocation requests.
+
+  @param  PoolType               The type of memory for the new pool pages
+  @param  NumberOfPages          No of pages to allocate
+  @param  Alignment              Bits to align.
+
+  @return The allocated memory, or NULL
+
+**/
+VOID *
+CoreAllocatePoolPages (
+  IN EFI_MEMORY_TYPE    PoolType,
+  IN UINTN              NumberOfPages,
+  IN UINTN              Alignment
+  )
+{
+  UINT64            Start;
+
+  //
+  // Find the pages to convert
+  //
+  Start = FindFreePages (MAX_ADDRESS, NumberOfPages, PoolType, Alignment);
+
+  //
+  // Convert it to boot services data
+  //
+  if (Start == 0) {
+    DEBUG ((DEBUG_ERROR | DEBUG_PAGE, "AllocatePoolPages: failed to allocate %d pages\n", (UINT32)NumberOfPages));
+  } else {
+    CoreConvertPages (Start, NumberOfPages, PoolType);
+  }
+
+  return (VOID *)(UINTN) Start;
+}
+
+
+/**
+  Internal function.  Frees pool pages allocated via AllocatePoolPages ()
+
+  @param  Memory                 The base address to free
+  @param  NumberOfPages          The number of pages to free
+
+**/
+VOID
+CoreFreePoolPages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  )
+{
+  CoreConvertPages (Memory, NumberOfPages, EfiConventionalMemory);
+}
+
+
+
+/**
+  Make sure the memory map is following all the construction rules,
+  it is the last time to check memory map error before exit boot services.
+
+  @param  MapKey                 Memory map key
+
+  @retval EFI_INVALID_PARAMETER  Memory map not consistent with construction
+                                 rules.
+  @retval EFI_SUCCESS            Valid memory map.
+
+**/
+EFI_STATUS
+CoreTerminateMemoryMap (
+  IN UINTN          MapKey
+  )
+{
+  EFI_STATUS        Status;
+  LIST_ENTRY        *Link;
+  MEMORY_MAP        *Entry;
+
+  Status = EFI_SUCCESS;
+
+  CoreAcquireMemoryLock ();
+
+  if (MapKey == mMemoryMapKey) {
+
+    //
+    // Make sure the memory map is following all the construction rules
+    // This is the last chance we will be able to display any messages on
+    // the  console devices.
+    //
+
+    for (Link = gMemoryMap.ForwardLink; Link != &gMemoryMap; Link = Link->ForwardLink) {
+      Entry = CR(Link, MEMORY_MAP, Link, MEMORY_MAP_SIGNATURE);
+      if (Entry->Type < EfiMaxMemoryType) {
+        if (mMemoryTypeStatistics[Entry->Type].Runtime) {
+          ASSERT (Entry->Type != EfiACPIReclaimMemory);
+          ASSERT (Entry->Type != EfiACPIMemoryNVS);
+          if ((Entry->Start & (EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT - 1)) != 0) {
+            DEBUG((DEBUG_ERROR | DEBUG_PAGE, "ExitBootServices: A RUNTIME memory entry is not on a proper alignment.\n"));
+            Status =  EFI_INVALID_PARAMETER;
+            goto Done;
+          }
+          if (((Entry->End + 1) & (EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT - 1)) != 0) {
+            DEBUG((DEBUG_ERROR | DEBUG_PAGE, "ExitBootServices: A RUNTIME memory entry is not on a proper alignment.\n"));
+            Status =  EFI_INVALID_PARAMETER;
+            goto Done;
+          }
+        }
+      }
+    }
+
+    //
+    // The map key they gave us matches what we expect. Fall through and
+    // return success. In an ideal world we would clear out all of
+    // EfiBootServicesCode and EfiBootServicesData. However this function
+    // is not the last one called by ExitBootServices(), so we have to
+    // preserve the memory contents.
+    //
+  } else {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+Done:
+  CoreReleaseMemoryLock ();
+
+  return Status;
+}
+
+
+
+
+
+
+
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Mem/Pool.c b/Core/MdeModulePkg/Core/Dxe/Mem/Pool.c
new file mode 100644
index 0000000000..fc08159d52
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Mem/Pool.c
@@ -0,0 +1,672 @@
+/** @file
+  UEFI Memory pool management functions.
+
+Copyright (c) 2006 - 2015, 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 "DxeMain.h"
+#include "Imem.h"
+
+#define POOL_FREE_SIGNATURE   SIGNATURE_32('p','f','r','0')
+typedef struct {
+  UINT32          Signature;
+  UINT32          Index;
+  LIST_ENTRY      Link;
+} POOL_FREE;
+
+
+#define POOL_HEAD_SIGNATURE   SIGNATURE_32('p','h','d','0')
+typedef struct {
+  UINT32          Signature;
+  UINT32          Reserved;
+  EFI_MEMORY_TYPE Type;
+  UINTN           Size;
+  CHAR8           Data[1];
+} POOL_HEAD;
+
+#define SIZE_OF_POOL_HEAD OFFSET_OF(POOL_HEAD,Data)
+
+#define POOL_TAIL_SIGNATURE   SIGNATURE_32('p','t','a','l')
+typedef struct {
+  UINT32      Signature;
+  UINT32      Reserved;
+  UINTN       Size;
+} POOL_TAIL;
+
+#define POOL_OVERHEAD (SIZE_OF_POOL_HEAD + sizeof(POOL_TAIL))
+
+#define HEAD_TO_TAIL(a)   \
+  ((POOL_TAIL *) (((CHAR8 *) (a)) + (a)->Size - sizeof(POOL_TAIL)));
+
+//
+// Each element is the sum of the 2 previous ones: this allows us to migrate
+// blocks between bins by splitting them up, while not wasting too much memory
+// as we would in a strict power-of-2 sequence
+//
+STATIC CONST UINT16 mPoolSizeTable[] = {
+  128, 256, 384, 640, 1024, 1664, 2688, 4352, 7040, 11392, 18432, 29824
+};
+
+#define SIZE_TO_LIST(a)   (GetPoolIndexFromSize (a))
+#define LIST_TO_SIZE(a)   (mPoolSizeTable [a])
+
+#define MAX_POOL_LIST     (sizeof (mPoolSizeTable) / sizeof (mPoolSizeTable[0]))
+
+#define MAX_POOL_SIZE     (MAX_ADDRESS - POOL_OVERHEAD)
+
+//
+// Globals
+//
+
+#define POOL_SIGNATURE  SIGNATURE_32('p','l','s','t')
+typedef struct {
+    INTN             Signature;
+    UINTN            Used;
+    EFI_MEMORY_TYPE  MemoryType;
+    LIST_ENTRY       FreeList[MAX_POOL_LIST];
+    LIST_ENTRY       Link;
+} POOL;
+
+//
+// Pool header for each memory type.
+//
+POOL            mPoolHead[EfiMaxMemoryType];
+
+//
+// List of pool header to search for the appropriate memory type.
+//
+LIST_ENTRY      mPoolHeadList = INITIALIZE_LIST_HEAD_VARIABLE (mPoolHeadList);
+
+/**
+  Get pool size table index from the specified size.
+
+  @param  Size          The specified size to get index from pool table.
+
+  @return               The index of pool size table.
+
+**/
+STATIC
+UINTN
+GetPoolIndexFromSize (
+  UINTN   Size
+  )
+{
+  UINTN   Index;
+
+  for (Index = 0; Index < MAX_POOL_LIST; Index++) {
+    if (mPoolSizeTable [Index] >= Size) {
+      return Index;
+    }
+  }
+  return MAX_POOL_LIST;
+}
+
+/**
+  Called to initialize the pool.
+
+**/
+VOID
+CoreInitializePool (
+  VOID
+  )
+{
+  UINTN  Type;
+  UINTN  Index;
+
+  for (Type=0; Type < EfiMaxMemoryType; Type++) {
+    mPoolHead[Type].Signature  = 0;
+    mPoolHead[Type].Used       = 0;
+    mPoolHead[Type].MemoryType = (EFI_MEMORY_TYPE) Type;
+    for (Index=0; Index < MAX_POOL_LIST; Index++) {
+      InitializeListHead (&mPoolHead[Type].FreeList[Index]);
+    }
+  }
+}
+
+
+/**
+  Look up pool head for specified memory type.
+
+  @param  MemoryType             Memory type of which pool head is looked for
+
+  @return Pointer of Corresponding pool head.
+
+**/
+POOL *
+LookupPoolHead (
+  IN EFI_MEMORY_TYPE  MemoryType
+  )
+{
+  LIST_ENTRY      *Link;
+  POOL            *Pool;
+  UINTN           Index;
+
+  if ((UINT32)MemoryType < EfiMaxMemoryType) {
+    return &mPoolHead[MemoryType];
+  }
+
+  //
+  // MemoryType values in the range 0x80000000..0xFFFFFFFF are reserved for use by UEFI
+  // OS loaders that are provided by operating system vendors.
+  // MemoryType values in the range 0x70000000..0x7FFFFFFF are reserved for OEM use.
+  //
+  if ((UINT32) MemoryType >= MEMORY_TYPE_OEM_RESERVED_MIN) {
+
+    for (Link = mPoolHeadList.ForwardLink; Link != &mPoolHeadList; Link = Link->ForwardLink) {
+      Pool = CR(Link, POOL, Link, POOL_SIGNATURE);
+      if (Pool->MemoryType == MemoryType) {
+        return Pool;
+      }
+    }
+
+    Pool = CoreAllocatePoolI (EfiBootServicesData, sizeof (POOL));
+    if (Pool == NULL) {
+      return NULL;
+    }
+
+    Pool->Signature = POOL_SIGNATURE;
+    Pool->Used      = 0;
+    Pool->MemoryType = MemoryType;
+    for (Index=0; Index < MAX_POOL_LIST; Index++) {
+      InitializeListHead (&Pool->FreeList[Index]);
+    }
+
+    InsertHeadList (&mPoolHeadList, &Pool->Link);
+
+    return Pool;
+  }
+
+  return NULL;
+}
+
+
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid or Buffer is NULL. 
+                                 PoolType was EfiPersistentMemory.
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalAllocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size,
+  OUT VOID            **Buffer
+  )
+{
+  EFI_STATUS    Status;
+
+  //
+  // If it's not a valid type, fail it
+  //
+  if ((PoolType >= EfiMaxMemoryType && PoolType < MEMORY_TYPE_OEM_RESERVED_MIN) ||
+       (PoolType == EfiConventionalMemory) || (PoolType == EfiPersistentMemory)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Buffer = NULL;
+
+  //
+  // If size is too large, fail it
+  // Base on the EFI spec, return status of EFI_OUT_OF_RESOURCES
+  //
+  if (Size > MAX_POOL_SIZE) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Acquire the memory lock and make the allocation
+  //
+  Status = CoreAcquireLockOrFail (&gMemoryLock);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  *Buffer = CoreAllocatePoolI (PoolType, Size);
+  CoreReleaseMemoryLock ();
+  return (*Buffer != NULL) ? EFI_SUCCESS : EFI_OUT_OF_RESOURCES;
+}
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid or Buffer is NULL. 
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size,
+  OUT VOID            **Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = CoreInternalAllocatePool (PoolType, Size, Buffer);
+  if (!EFI_ERROR (Status)) {
+    CoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionAllocatePool, PoolType, Size, *Buffer);
+  }
+  return Status;
+}
+
+/**
+  Internal function to allocate pool of a particular type.
+  Caller must have the memory lock held
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+
+  @return The allocate pool, or NULL
+
+**/
+VOID *
+CoreAllocatePoolI (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size
+  )
+{
+  POOL        *Pool;
+  POOL_FREE   *Free;
+  POOL_HEAD   *Head;
+  POOL_TAIL   *Tail;
+  CHAR8       *NewPage;
+  VOID        *Buffer;
+  UINTN       Index;
+  UINTN       FSize;
+  UINTN       Offset, MaxOffset;
+  UINTN       NoPages;
+  UINTN       Granularity;
+
+  ASSERT_LOCKED (&gMemoryLock);
+
+  if  (PoolType == EfiACPIReclaimMemory   ||
+       PoolType == EfiACPIMemoryNVS       ||
+       PoolType == EfiRuntimeServicesCode ||
+       PoolType == EfiRuntimeServicesData) {
+
+    Granularity = EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT;
+  } else {
+    Granularity = DEFAULT_PAGE_ALLOCATION;
+  }
+
+  //
+  // Adjust the size by the pool header & tail overhead
+  //
+
+  //
+  // Adjusting the Size to be of proper alignment so that
+  // we don't get an unaligned access fault later when
+  // pool_Tail is being initialized
+  //
+  Size = ALIGN_VARIABLE (Size);
+
+  Size += POOL_OVERHEAD;
+  Index = SIZE_TO_LIST(Size);
+  Pool = LookupPoolHead (PoolType);
+  if (Pool== NULL) {
+    return NULL;
+  }
+  Head = NULL;
+
+  //
+  // If allocation is over max size, just allocate pages for the request
+  // (slow)
+  //
+  if (Index >= SIZE_TO_LIST (Granularity)) {
+    NoPages = EFI_SIZE_TO_PAGES(Size) + EFI_SIZE_TO_PAGES (Granularity) - 1;
+    NoPages &= ~(UINTN)(EFI_SIZE_TO_PAGES (Granularity) - 1);
+    Head = CoreAllocatePoolPages (PoolType, NoPages, Granularity);
+    goto Done;
+  }
+
+  //
+  // If there's no free pool in the proper list size, go get some more pages
+  //
+  if (IsListEmpty (&Pool->FreeList[Index])) {
+
+    Offset = LIST_TO_SIZE (Index);
+    MaxOffset = Granularity;
+
+    //
+    // Check the bins holding larger blocks, and carve one up if needed
+    //
+    while (++Index < SIZE_TO_LIST (Granularity)) {
+      if (!IsListEmpty (&Pool->FreeList[Index])) {
+        Free = CR (Pool->FreeList[Index].ForwardLink, POOL_FREE, Link, POOL_FREE_SIGNATURE);
+        RemoveEntryList (&Free->Link);
+        NewPage = (VOID *) Free;
+        MaxOffset = LIST_TO_SIZE (Index);
+        goto Carve;
+      }
+    }
+
+    //
+    // Get another page
+    //
+    NewPage = CoreAllocatePoolPages(PoolType, EFI_SIZE_TO_PAGES (Granularity), Granularity);
+    if (NewPage == NULL) {
+      goto Done;
+    }
+
+    //
+    // Serve the allocation request from the head of the allocated block
+    //
+Carve:
+    Head = (POOL_HEAD *) NewPage;
+
+    //
+    // Carve up remaining space into free pool blocks
+    //
+    Index--;
+    while (Offset < MaxOffset) {
+      ASSERT (Index < MAX_POOL_LIST);
+      FSize = LIST_TO_SIZE(Index);
+
+      while (Offset + FSize <= MaxOffset) {
+        Free = (POOL_FREE *) &NewPage[Offset];
+        Free->Signature = POOL_FREE_SIGNATURE;
+        Free->Index     = (UINT32)Index;
+        InsertHeadList (&Pool->FreeList[Index], &Free->Link);
+        Offset += FSize;
+      }
+      Index -= 1;
+    }
+
+    ASSERT (Offset == MaxOffset);
+    goto Done;
+  }
+
+  //
+  // Remove entry from free pool list
+  //
+  Free = CR (Pool->FreeList[Index].ForwardLink, POOL_FREE, Link, POOL_FREE_SIGNATURE);
+  RemoveEntryList (&Free->Link);
+
+  Head = (POOL_HEAD *) Free;
+
+Done:
+  Buffer = NULL;
+
+  if (Head != NULL) {
+
+    //
+    // If we have a pool buffer, fill in the header & tail info
+    //
+    Head->Signature = POOL_HEAD_SIGNATURE;
+    Head->Size      = Size;
+    Head->Type      = (EFI_MEMORY_TYPE) PoolType;
+    Tail            = HEAD_TO_TAIL (Head);
+    Tail->Signature = POOL_TAIL_SIGNATURE;
+    Tail->Size      = Size;
+    Buffer          = Head->Data;
+    DEBUG_CLEAR_MEMORY (Buffer, Size - POOL_OVERHEAD);
+
+    DEBUG ((
+      DEBUG_POOL,
+      "AllocatePoolI: Type %x, Addr %p (len %lx) %,ld\n", PoolType,
+      Buffer,
+      (UINT64)(Size - POOL_OVERHEAD),
+      (UINT64) Pool->Used
+      ));
+
+    //
+    // Account the allocation
+    //
+    Pool->Used += Size;
+
+  } else {
+    DEBUG ((DEBUG_ERROR | DEBUG_POOL, "AllocatePool: failed to allocate %ld bytes\n", (UINT64) Size));
+  }
+
+  return Buffer;
+}
+
+
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInternalFreePool (
+  IN VOID        *Buffer
+  )
+{
+  EFI_STATUS Status;
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquireMemoryLock ();
+  Status = CoreFreePoolI (Buffer);
+  CoreReleaseMemoryLock ();
+  return Status;
+}
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePool (
+  IN VOID  *Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = CoreInternalFreePool (Buffer);
+  if (!EFI_ERROR (Status)) {
+    CoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionFreePool, (EFI_MEMORY_TYPE) 0, 0, Buffer);
+  }
+  return Status;
+}
+
+/**
+  Internal function to free a pool entry.
+  Caller must have the memory lock held
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer not valid
+  @retval EFI_SUCCESS            Buffer successfully freed.
+
+**/
+EFI_STATUS
+CoreFreePoolI (
+  IN VOID       *Buffer
+  )
+{
+  POOL        *Pool;
+  POOL_HEAD   *Head;
+  POOL_TAIL   *Tail;
+  POOL_FREE   *Free;
+  UINTN       Index;
+  UINTN       NoPages;
+  UINTN       Size;
+  CHAR8       *NewPage;
+  UINTN       Offset;
+  BOOLEAN     AllFree;
+  UINTN       Granularity;
+
+  ASSERT(Buffer != NULL);
+  //
+  // Get the head & tail of the pool entry
+  //
+  Head = CR (Buffer, POOL_HEAD, Data, POOL_HEAD_SIGNATURE);
+  ASSERT(Head != NULL);
+
+  if (Head->Signature != POOL_HEAD_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Tail = HEAD_TO_TAIL (Head);
+  ASSERT(Tail != NULL);
+
+  //
+  // Debug
+  //
+  ASSERT (Tail->Signature == POOL_TAIL_SIGNATURE);
+  ASSERT (Head->Size == Tail->Size);
+  ASSERT_LOCKED (&gMemoryLock);
+
+  if (Tail->Signature != POOL_TAIL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Head->Size != Tail->Size) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Determine the pool type and account for it
+  //
+  Size = Head->Size;
+  Pool = LookupPoolHead (Head->Type);
+  if (Pool == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  Pool->Used -= Size;
+  DEBUG ((DEBUG_POOL, "FreePool: %p (len %lx) %,ld\n", Head->Data, (UINT64)(Head->Size - POOL_OVERHEAD), (UINT64) Pool->Used));
+
+  if  (Head->Type == EfiACPIReclaimMemory   ||
+       Head->Type == EfiACPIMemoryNVS       ||
+       Head->Type == EfiRuntimeServicesCode ||
+       Head->Type == EfiRuntimeServicesData) {
+
+    Granularity = EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT;
+  } else {
+    Granularity = DEFAULT_PAGE_ALLOCATION;
+  }
+
+  //
+  // Determine the pool list
+  //
+  Index = SIZE_TO_LIST(Size);
+  DEBUG_CLEAR_MEMORY (Head, Size);
+
+  //
+  // If it's not on the list, it must be pool pages
+  //
+  if (Index >= SIZE_TO_LIST (Granularity)) {
+
+    //
+    // Return the memory pages back to free memory
+    //
+    NoPages = EFI_SIZE_TO_PAGES(Size) + EFI_SIZE_TO_PAGES (Granularity) - 1;
+    NoPages &= ~(UINTN)(EFI_SIZE_TO_PAGES (Granularity) - 1);
+    CoreFreePoolPages ((EFI_PHYSICAL_ADDRESS) (UINTN) Head, NoPages);
+
+  } else {
+
+    //
+    // Put the pool entry onto the free pool list
+    //
+    Free = (POOL_FREE *) Head;
+    ASSERT(Free != NULL);
+    Free->Signature = POOL_FREE_SIGNATURE;
+    Free->Index     = (UINT32)Index;
+    InsertHeadList (&Pool->FreeList[Index], &Free->Link);
+
+    //
+    // See if all the pool entries in the same page as Free are freed pool
+    // entries
+    //
+    NewPage = (CHAR8 *)((UINTN)Free & ~(Granularity - 1));
+    Free = (POOL_FREE *) &NewPage[0];
+    ASSERT(Free != NULL);
+
+    if (Free->Signature == POOL_FREE_SIGNATURE) {
+
+      AllFree = TRUE;
+      Offset = 0;
+
+      while ((Offset < Granularity) && (AllFree)) {
+        Free = (POOL_FREE *) &NewPage[Offset];
+        ASSERT(Free != NULL);
+        if (Free->Signature != POOL_FREE_SIGNATURE) {
+          AllFree = FALSE;
+        }
+        Offset += LIST_TO_SIZE(Free->Index);
+      }
+
+      if (AllFree) {
+
+        //
+        // All of the pool entries in the same page as Free are free pool
+        // entries
+        // Remove all of these pool entries from the free loop lists.
+        //
+        Free = (POOL_FREE *) &NewPage[0];
+        ASSERT(Free != NULL);
+        Offset = 0;
+
+        while (Offset < Granularity) {
+          Free = (POOL_FREE *) &NewPage[Offset];
+          ASSERT(Free != NULL);
+          RemoveEntryList (&Free->Link);
+          Offset += LIST_TO_SIZE(Free->Index);
+        }
+
+        //
+        // Free the page
+        //
+        CoreFreePoolPages ((EFI_PHYSICAL_ADDRESS) (UINTN)NewPage, EFI_SIZE_TO_PAGES (Granularity));
+      }
+    }
+  }
+
+  //
+  // If this is an OS specific memory type, then check to see if the last
+  // portion of that memory type has been freed.  If it has, then free the
+  // list entry for that memory type
+  //
+  if ((INT32)Pool->MemoryType < 0 && Pool->Used == 0) {
+    RemoveEntryList (&Pool->Link);
+    CoreFreePoolI (Pool);
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/DebugImageInfo.c b/Core/MdeModulePkg/Core/Dxe/Misc/DebugImageInfo.c
new file mode 100644
index 0000000000..47660729ba
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/DebugImageInfo.c
@@ -0,0 +1,288 @@
+/** @file
+  Support functions for managing debug image info table when loading and unloading
+  images.
+
+Copyright (c) 2006 - 2010, 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 "DxeMain.h"
+
+
+EFI_DEBUG_IMAGE_INFO_TABLE_HEADER  mDebugInfoTableHeader = {
+  0,          // volatile UINT32                 UpdateStatus;
+  0,          // UINT32                          TableSize;
+  NULL        // EFI_DEBUG_IMAGE_INFO            *EfiDebugImageInfoTable;
+};
+
+UINTN mMaxTableEntries = 0;
+
+EFI_SYSTEM_TABLE_POINTER  *mDebugTable = NULL;
+
+#define EFI_DEBUG_TABLE_ENTRY_SIZE       (sizeof (VOID *))
+
+/**
+  Creates and initializes the DebugImageInfo Table.  Also creates the configuration
+  table and registers it into the system table.
+
+**/
+VOID
+CoreInitializeDebugImageInfoTable (
+  VOID
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Pages;
+  EFI_PHYSICAL_ADDRESS  Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 UnalignedPages;
+  UINTN                 RealPages;
+
+  //
+  // Allocate 4M aligned page for the structure and fill in the data.
+  // Ideally we would update the CRC now as well, but the service may not yet be available.
+  // See comments in the CoreUpdateDebugTableCrc32() function below for details.
+  //
+  Pages          = EFI_SIZE_TO_PAGES (sizeof (EFI_SYSTEM_TABLE_POINTER));
+  AlignmentMask  = SIZE_4MB - 1;
+  RealPages      = Pages + EFI_SIZE_TO_PAGES (SIZE_4MB);
+
+  //
+  // Attempt to allocate memory below PcdMaxEfiSystemTablePointerAddress
+  // If PcdMaxEfiSystemTablePointerAddress is 0, then allocate memory below
+  // MAX_ADDRESS
+  //
+  Memory = PcdGet64 (PcdMaxEfiSystemTablePointerAddress);
+  if (Memory == 0) {
+    Memory = MAX_ADDRESS;
+  }
+  Status = CoreAllocatePages (
+             AllocateMaxAddress, 
+             EfiBootServicesData,
+             RealPages, 
+             &Memory
+             );
+  if (EFI_ERROR (Status)) {
+    if (PcdGet64 (PcdMaxEfiSystemTablePointerAddress) != 0) {
+      DEBUG ((EFI_D_INFO, "Allocate memory for EFI_SYSTEM_TABLE_POINTER below PcdMaxEfiSystemTablePointerAddress failed. \
+                          Retry to allocate memroy as close to the top of memory as feasible.\n"));
+    }
+    //
+    // If the initial memory allocation fails, then reattempt allocation
+    // as close to the top of memory as feasible.
+    //
+    Status = CoreAllocatePages (
+               AllocateAnyPages, 
+               EfiBootServicesData,
+               RealPages, 
+               &Memory
+               );
+    ASSERT_EFI_ERROR (Status);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+  }  
+
+  //
+  // Free overallocated pages
+  //
+  AlignedMemory  = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+  UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN)Memory);
+  if (UnalignedPages > 0) {
+    //
+    // Free first unaligned page(s).
+    //
+    Status = CoreFreePages (Memory, UnalignedPages);
+    ASSERT_EFI_ERROR (Status);
+  }
+  Memory         = (EFI_PHYSICAL_ADDRESS)(AlignedMemory + EFI_PAGES_TO_SIZE (Pages));
+  UnalignedPages = RealPages - Pages - UnalignedPages;
+  if (UnalignedPages > 0) {
+    //
+    // Free last unaligned page(s).
+    //
+    Status = CoreFreePages (Memory, UnalignedPages);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Set mDebugTable to the 4MB aligned allocated pages
+  //
+  mDebugTable = (EFI_SYSTEM_TABLE_POINTER  *)(AlignedMemory);
+  ASSERT (mDebugTable != NULL);
+
+  //
+  // Initialize EFI_SYSTEM_TABLE_POINTER structure
+  //  
+  mDebugTable->Signature          = EFI_SYSTEM_TABLE_SIGNATURE;
+  mDebugTable->EfiSystemTableBase = (EFI_PHYSICAL_ADDRESS) (UINTN) gDxeCoreST;
+  mDebugTable->Crc32              = 0;
+  
+  //
+  // Install the EFI_SYSTEM_TABLE_POINTER structure in the EFI System 
+  // Configuration Table
+  //
+  Status = CoreInstallConfigurationTable (&gEfiDebugImageInfoTableGuid, &mDebugInfoTableHeader);
+  ASSERT_EFI_ERROR (Status);
+}
+
+
+/**
+  Update the CRC32 in the Debug Table.
+  Since the CRC32 service is made available by the Runtime driver, we have to
+  wait for the Runtime Driver to be installed before the CRC32 can be computed.
+  This function is called elsewhere by the core when the runtime architectural
+  protocol is produced.
+
+**/
+VOID
+CoreUpdateDebugTableCrc32 (
+  VOID
+  )
+{
+  ASSERT(mDebugTable != NULL);
+  mDebugTable->Crc32 = 0;
+  gBS->CalculateCrc32 ((VOID *)mDebugTable, sizeof (EFI_SYSTEM_TABLE_POINTER), &mDebugTable->Crc32);
+}
+
+
+/**
+  Adds a new DebugImageInfo structure to the DebugImageInfo Table.  Re-Allocates
+  the table if it's not large enough to accomidate another entry.
+
+  @param  ImageInfoType  type of debug image information
+  @param  LoadedImage    pointer to the loaded image protocol for the image being
+                         loaded
+  @param  ImageHandle    image handle for the image being loaded
+
+**/
+VOID
+CoreNewDebugImageInfoEntry (
+  IN  UINT32                      ImageInfoType,
+  IN  EFI_LOADED_IMAGE_PROTOCOL   *LoadedImage,
+  IN  EFI_HANDLE                  ImageHandle
+  )
+{
+  EFI_DEBUG_IMAGE_INFO      *Table;
+  EFI_DEBUG_IMAGE_INFO      *NewTable;
+  UINTN                     Index;
+  UINTN                     TableSize;
+
+  //
+  // Set the flag indicating that we're in the process of updating the table.
+  //
+  mDebugInfoTableHeader.UpdateStatus |= EFI_DEBUG_IMAGE_INFO_UPDATE_IN_PROGRESS;
+
+  Table = mDebugInfoTableHeader.EfiDebugImageInfoTable;
+  
+  if (mDebugInfoTableHeader.TableSize < mMaxTableEntries) {
+    //
+    // We still have empty entires in the Table, find the first empty entry.
+    //
+    Index = 0;
+    while (Table[Index].NormalImage != NULL) {
+      Index++;
+    }
+    //
+    // There must be an empty entry in the in the table.
+    //
+    ASSERT (Index < mMaxTableEntries);
+  } else {
+    //
+    //  Table is full, so re-allocate another page for a larger table...
+    //
+    TableSize = mMaxTableEntries * EFI_DEBUG_TABLE_ENTRY_SIZE;
+    NewTable = AllocateZeroPool (TableSize + EFI_PAGE_SIZE);
+    if (NewTable == NULL) {
+      mDebugInfoTableHeader.UpdateStatus &= ~EFI_DEBUG_IMAGE_INFO_UPDATE_IN_PROGRESS;
+      return;
+    }
+    //
+    // Copy the old table into the new one
+    //
+    CopyMem (NewTable, Table, TableSize);
+    //
+    // Free the old table
+    //
+    CoreFreePool (Table);
+    //
+    // Update the table header
+    //
+    Table = NewTable;
+    mDebugInfoTableHeader.EfiDebugImageInfoTable = NewTable;
+    //
+    // Enlarge the max table entries and set the first empty entry index to
+    // be the original max table entries.
+    //
+    Index             = mMaxTableEntries;
+    mMaxTableEntries += EFI_PAGE_SIZE / EFI_DEBUG_TABLE_ENTRY_SIZE;
+  }
+
+  //
+  // Allocate data for new entry
+  //
+  Table[Index].NormalImage = AllocateZeroPool (sizeof (EFI_DEBUG_IMAGE_INFO_NORMAL));
+  if (Table[Index].NormalImage != NULL) {
+    //
+    // Update the entry
+    //
+    Table[Index].NormalImage->ImageInfoType               = (UINT32) ImageInfoType;
+    Table[Index].NormalImage->LoadedImageProtocolInstance = LoadedImage;
+    Table[Index].NormalImage->ImageHandle                 = ImageHandle;
+    //
+    // Increase the number of EFI_DEBUG_IMAGE_INFO elements and set the mDebugInfoTable in modified status.
+    //
+    mDebugInfoTableHeader.TableSize++;
+    mDebugInfoTableHeader.UpdateStatus |= EFI_DEBUG_IMAGE_INFO_TABLE_MODIFIED;
+  }
+  mDebugInfoTableHeader.UpdateStatus &= ~EFI_DEBUG_IMAGE_INFO_UPDATE_IN_PROGRESS;
+}
+
+
+
+/**
+  Removes and frees an entry from the DebugImageInfo Table.
+
+  @param  ImageHandle    image handle for the image being unloaded
+
+**/
+VOID
+CoreRemoveDebugImageInfoEntry (
+  EFI_HANDLE ImageHandle
+  )
+{
+  EFI_DEBUG_IMAGE_INFO  *Table;
+  UINTN                 Index;
+
+  mDebugInfoTableHeader.UpdateStatus |= EFI_DEBUG_IMAGE_INFO_UPDATE_IN_PROGRESS;
+
+  Table = mDebugInfoTableHeader.EfiDebugImageInfoTable;
+
+  for (Index = 0; Index < mMaxTableEntries; Index++) {
+    if (Table[Index].NormalImage != NULL && Table[Index].NormalImage->ImageHandle == ImageHandle) {
+      //
+      // Found a match. Free up the record, then NULL the pointer to indicate the slot
+      // is free.
+      //
+      CoreFreePool (Table[Index].NormalImage);
+      Table[Index].NormalImage = NULL;
+      //
+      // Decrease the number of EFI_DEBUG_IMAGE_INFO elements and set the mDebugInfoTable in modified status.
+      //
+      mDebugInfoTableHeader.TableSize--;
+      mDebugInfoTableHeader.UpdateStatus |= EFI_DEBUG_IMAGE_INFO_TABLE_MODIFIED;
+      break;
+    }
+  }
+  mDebugInfoTableHeader.UpdateStatus &= ~EFI_DEBUG_IMAGE_INFO_UPDATE_IN_PROGRESS;
+}
+
+
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/InstallConfigurationTable.c b/Core/MdeModulePkg/Core/Dxe/Misc/InstallConfigurationTable.c
new file mode 100644
index 0000000000..e4735db7ba
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/InstallConfigurationTable.c
@@ -0,0 +1,171 @@
+/** @file
+  UEFI Miscellaneous boot Services InstallConfigurationTable service
+
+Copyright (c) 2006 - 2011, 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 "DxeMain.h"
+
+#define CONFIG_TABLE_SIZE_INCREASED 0x10
+
+UINTN mSystemTableAllocateSize = 0;
+
+/**
+  Boot Service called to add, modify, or remove a system configuration table from
+  the EFI System Table.
+
+  @param  Guid           Pointer to the GUID for the entry to add, update, or
+                         remove
+  @param  Table          Pointer to the configuration table for the entry to add,
+                         update, or remove, may be NULL.
+
+  @return EFI_SUCCESS               Guid, Table pair added, updated, or removed.
+  @return EFI_INVALID_PARAMETER     Input GUID is NULL.
+  @return EFI_NOT_FOUND             Attempted to delete non-existant entry
+  @return EFI_OUT_OF_RESOURCES      Not enough memory available
+
+**/
+EFI_STATUS
+EFIAPI
+CoreInstallConfigurationTable (
+  IN EFI_GUID *Guid,
+  IN VOID     *Table
+  )
+{
+  UINTN                   Index;
+  EFI_CONFIGURATION_TABLE *EfiConfigurationTable;
+
+  //
+  // If Guid is NULL, then this operation cannot be performed
+  //
+  if (Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  EfiConfigurationTable = gDxeCoreST->ConfigurationTable;
+
+  //
+  // Search all the table for an entry that matches Guid
+  //
+  for (Index = 0; Index < gDxeCoreST->NumberOfTableEntries; Index++) {
+    if (CompareGuid (Guid, &(gDxeCoreST->ConfigurationTable[Index].VendorGuid))) {
+      break;
+    }
+  }
+
+  if (Index < gDxeCoreST->NumberOfTableEntries) {
+    //
+    // A match was found, so this is either a modify or a delete operation
+    //
+    if (Table != NULL) {
+      //
+      // If Table is not NULL, then this is a modify operation.
+      // Modify the table enty and return.
+      //
+      gDxeCoreST->ConfigurationTable[Index].VendorTable = Table;
+
+      //
+      // Signal Configuration Table change
+      //
+      CoreNotifySignalList (Guid);
+
+      return EFI_SUCCESS;
+    }
+
+    //
+    // A match was found and Table is NULL, so this is a delete operation.
+    //
+    gDxeCoreST->NumberOfTableEntries--;
+
+    //
+    // Copy over deleted entry
+    //
+    CopyMem (
+      &(EfiConfigurationTable[Index]),
+      &(gDxeCoreST->ConfigurationTable[Index + 1]),
+      (gDxeCoreST->NumberOfTableEntries - Index) * sizeof (EFI_CONFIGURATION_TABLE)
+      );
+
+  } else {
+
+    //
+    // No matching GUIDs were found, so this is an add operation.
+    //
+
+    if (Table == NULL) {
+      //
+      // If Table is NULL on an add operation, then return an error.
+      //
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Assume that Index == gDxeCoreST->NumberOfTableEntries
+    //
+    if ((Index * sizeof (EFI_CONFIGURATION_TABLE)) >= mSystemTableAllocateSize) {
+      //
+      // Allocate a table with one additional entry.
+      //
+      mSystemTableAllocateSize += (CONFIG_TABLE_SIZE_INCREASED * sizeof (EFI_CONFIGURATION_TABLE));
+      EfiConfigurationTable = AllocateRuntimePool (mSystemTableAllocateSize);
+      if (EfiConfigurationTable == NULL) {
+        //
+        // If a new table could not be allocated, then return an error.
+        //
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      if (gDxeCoreST->ConfigurationTable != NULL) {
+        //
+        // Copy the old table to the new table.
+        //
+        CopyMem (
+          EfiConfigurationTable,
+          gDxeCoreST->ConfigurationTable,
+          Index * sizeof (EFI_CONFIGURATION_TABLE)
+          );
+
+        //
+        // Free Old Table
+        //
+        CoreFreePool (gDxeCoreST->ConfigurationTable);
+      }
+
+      //
+      // Update System Table
+      //
+      gDxeCoreST->ConfigurationTable = EfiConfigurationTable;
+    }
+
+    //
+    // Fill in the new entry
+    //
+    CopyGuid ((VOID *)&EfiConfigurationTable[Index].VendorGuid, Guid);
+    EfiConfigurationTable[Index].VendorTable  = Table;
+
+    //
+    // This is an add operation, so increment the number of table entries
+    //
+    gDxeCoreST->NumberOfTableEntries++;
+  }
+
+  //
+  // Fix up the CRC-32 in the EFI System Table
+  //
+  CalculateEfiHdrCrc (&gDxeCoreST->Hdr);
+
+  //
+  // Signal Configuration Table change
+  //
+  CoreNotifySignalList (Guid);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/MemoryAttributesTable.c b/Core/MdeModulePkg/Core/Dxe/Misc/MemoryAttributesTable.c
new file mode 100644
index 0000000000..416ed3dca8
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/MemoryAttributesTable.c
@@ -0,0 +1,208 @@
+/** @file
+  UEFI MemoryAttributesTable support
+
+Copyright (c) 2016, 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 <PiDxe.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Guid/MemoryAttributesTable.h>
+#include <Guid/PropertiesTable.h>
+
+#include "DxeMain.h"
+
+/**
+  This function for GetMemoryMap() with properties table capability.
+
+  It calls original GetMemoryMap() to get the original memory map information. Then
+  plus the additional memory map entries for PE Code/Data seperation.
+
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 the buffer allocated by the caller.  On output,
+                                 it is the size of the buffer returned by the
+                                 firmware  if the buffer was large enough, or the
+                                 size of the buffer needed  to contain the map if
+                                 the buffer was too small.
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MapKey                 A pointer to the location in which firmware
+                                 returns the key for the current memory map.
+  @param  DescriptorSize         A pointer to the location in which firmware
+                                 returns the size, in bytes, of an individual
+                                 EFI_MEMORY_DESCRIPTOR.
+  @param  DescriptorVersion      A pointer to the location in which firmware
+                                 returns the version number associated with the
+                                 EFI_MEMORY_DESCRIPTOR.
+
+  @retval EFI_SUCCESS            The memory map was returned in the MemoryMap
+                                 buffer.
+  @retval EFI_BUFFER_TOO_SMALL   The MemoryMap buffer was too small. The current
+                                 buffer size needed to hold the memory map is
+                                 returned in MemoryMapSize.
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemoryMapPropertiesTable (
+  IN OUT UINTN                  *MemoryMapSize,
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  OUT UINTN                     *MapKey,
+  OUT UINTN                     *DescriptorSize,
+  OUT UINT32                    *DescriptorVersion
+  );
+
+extern EFI_PROPERTIES_TABLE  mPropertiesTable;
+
+/**
+  Install MemoryAttributesTable.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+**/
+VOID
+EFIAPI
+InstallMemoryAttributesTable (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  UINTN                          MemoryMapSize;
+  EFI_MEMORY_DESCRIPTOR          *MemoryMap;
+  EFI_MEMORY_DESCRIPTOR          *MemoryMapStart;
+  UINTN                          MapKey;
+  UINTN                          DescriptorSize;
+  UINT32                         DescriptorVersion;
+  UINTN                          Index;
+  EFI_STATUS                     Status;
+  UINT32                         RuntimeEntryCount;
+  EFI_MEMORY_ATTRIBUTES_TABLE    *MemoryAttributesTable;
+  EFI_MEMORY_DESCRIPTOR          *MemoryAttributesEntry;
+
+  if ((mPropertiesTable.MemoryProtectionAttribute & EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA) == 0) {
+    DEBUG ((EFI_D_VERBOSE, "MemoryProtectionAttribute NON_EXECUTABLE_PE_DATA is not set, "));
+    DEBUG ((EFI_D_VERBOSE, "because Runtime Driver Section Alignment is not %dK.\n", EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT >> 10));
+    return ;
+  }
+
+  MemoryMapSize = 0;
+  MemoryMap = NULL;
+  Status = CoreGetMemoryMapPropertiesTable (
+             &MemoryMapSize,
+             MemoryMap,
+             &MapKey,
+             &DescriptorSize,
+             &DescriptorVersion
+             );
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+
+  do {
+    MemoryMap = AllocatePool (MemoryMapSize);
+    ASSERT (MemoryMap != NULL);
+
+    Status = CoreGetMemoryMapPropertiesTable (
+               &MemoryMapSize,
+               MemoryMap,
+               &MapKey,
+               &DescriptorSize,
+               &DescriptorVersion
+               );
+    if (EFI_ERROR (Status)) {
+      FreePool (MemoryMap);
+    }
+  } while (Status == EFI_BUFFER_TOO_SMALL);
+
+  MemoryMapStart = MemoryMap;
+  RuntimeEntryCount = 0;
+  for (Index = 0; Index < MemoryMapSize/DescriptorSize; Index++) {
+    switch (MemoryMap->Type) {
+    case EfiRuntimeServicesCode:
+    case EfiRuntimeServicesData:
+      RuntimeEntryCount ++;
+      break;
+    }
+    MemoryMap = NEXT_MEMORY_DESCRIPTOR(MemoryMap, DescriptorSize);
+  }
+
+  //
+  // Allocate MemoryAttributesTable
+  //
+  MemoryAttributesTable = AllocatePool (sizeof(EFI_MEMORY_ATTRIBUTES_TABLE) + DescriptorSize * RuntimeEntryCount);
+  ASSERT (MemoryAttributesTable != NULL);
+  MemoryAttributesTable->Version         = EFI_MEMORY_ATTRIBUTES_TABLE_VERSION;
+  MemoryAttributesTable->NumberOfEntries = RuntimeEntryCount;
+  MemoryAttributesTable->DescriptorSize  = (UINT32)DescriptorSize;
+  MemoryAttributesTable->Reserved        = 0;
+  DEBUG ((EFI_D_VERBOSE, "MemoryAttributesTable:\n"));
+  DEBUG ((EFI_D_VERBOSE, "  Version              - 0x%08x\n", MemoryAttributesTable->Version));
+  DEBUG ((EFI_D_VERBOSE, "  NumberOfEntries      - 0x%08x\n", MemoryAttributesTable->NumberOfEntries));
+  DEBUG ((EFI_D_VERBOSE, "  DescriptorSize       - 0x%08x\n", MemoryAttributesTable->DescriptorSize));
+  MemoryAttributesEntry = (EFI_MEMORY_DESCRIPTOR *)(MemoryAttributesTable + 1);
+  MemoryMap = MemoryMapStart;
+  for (Index = 0; Index < MemoryMapSize/DescriptorSize; Index++) {
+    switch (MemoryMap->Type) {
+    case EfiRuntimeServicesCode:
+    case EfiRuntimeServicesData:
+      CopyMem (MemoryAttributesEntry, MemoryMap, DescriptorSize);
+      MemoryAttributesEntry->Attribute &= (EFI_MEMORY_RO|EFI_MEMORY_XP|EFI_MEMORY_RUNTIME);
+      DEBUG ((EFI_D_VERBOSE, "Entry (0x%x)\n", MemoryAttributesEntry));
+      DEBUG ((EFI_D_VERBOSE, "  Type              - 0x%x\n", MemoryAttributesEntry->Type));
+      DEBUG ((EFI_D_VERBOSE, "  PhysicalStart     - 0x%016lx\n", MemoryAttributesEntry->PhysicalStart));
+      DEBUG ((EFI_D_VERBOSE, "  VirtualStart      - 0x%016lx\n", MemoryAttributesEntry->VirtualStart));
+      DEBUG ((EFI_D_VERBOSE, "  NumberOfPages     - 0x%016lx\n", MemoryAttributesEntry->NumberOfPages));
+      DEBUG ((EFI_D_VERBOSE, "  Attribute         - 0x%016lx\n", MemoryAttributesEntry->Attribute));
+      MemoryAttributesEntry = NEXT_MEMORY_DESCRIPTOR(MemoryAttributesEntry, DescriptorSize);
+      break;
+    }
+    MemoryMap = NEXT_MEMORY_DESCRIPTOR(MemoryMap, DescriptorSize);
+  }
+
+  Status = gBS->InstallConfigurationTable (&gEfiMemoryAttributesTableGuid, MemoryAttributesTable);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Initialize MemoryAttrubutesTable support.
+**/
+VOID
+EFIAPI
+CoreInitializeMemoryAttributesTable (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   ReadyToBootEvent;
+
+  //
+  // Construct the table at ReadyToBoot, because this should be
+  // last point to allocate RuntimeCode/RuntimeData.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  InstallMemoryAttributesTable,
+                  NULL,
+                  &gEfiEventReadyToBootGuid,
+                  &ReadyToBootEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return ;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/PropertiesTable.c b/Core/MdeModulePkg/Core/Dxe/Misc/PropertiesTable.c
new file mode 100644
index 0000000000..06f3451b4e
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/PropertiesTable.c
@@ -0,0 +1,1401 @@
+/** @file
+  UEFI PropertiesTable support
+
+Copyright (c) 2015 - 2016, 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 <PiDxe.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PcdLib.h>
+
+#include <Guid/EventGroup.h>
+#include <Protocol/DxeSmmReadyToLock.h>
+
+#include <Library/PeCoffLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Protocol/Runtime.h>
+
+#include <Guid/PropertiesTable.h>
+
+#include "DxeMain.h"
+
+#define PREVIOUS_MEMORY_DESCRIPTOR(MemoryDescriptor, Size) \
+  ((EFI_MEMORY_DESCRIPTOR *)((UINT8 *)(MemoryDescriptor) - (Size)))
+
+#define IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE SIGNATURE_32 ('I','P','R','C')
+
+typedef struct {
+  UINT32                 Signature;
+  LIST_ENTRY             Link;
+  EFI_PHYSICAL_ADDRESS   CodeSegmentBase;
+  UINT64                 CodeSegmentSize;
+} IMAGE_PROPERTIES_RECORD_CODE_SECTION;
+
+#define IMAGE_PROPERTIES_RECORD_SIGNATURE SIGNATURE_32 ('I','P','R','D')
+
+typedef struct {
+  UINT32                 Signature;
+  LIST_ENTRY             Link;
+  EFI_PHYSICAL_ADDRESS   ImageBase;
+  UINT64                 ImageSize;
+  UINTN                  CodeSegmentCount;
+  LIST_ENTRY             CodeSegmentList;
+} IMAGE_PROPERTIES_RECORD;
+
+#define IMAGE_PROPERTIES_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('I','P','P','D')
+
+typedef struct {
+  UINT32                 Signature;
+  UINTN                  ImageRecordCount;
+  UINTN                  CodeSegmentCountMax;
+  LIST_ENTRY             ImageRecordList;
+} IMAGE_PROPERTIES_PRIVATE_DATA;
+
+IMAGE_PROPERTIES_PRIVATE_DATA  mImagePropertiesPrivateData = {
+  IMAGE_PROPERTIES_PRIVATE_DATA_SIGNATURE,
+  0,
+  0,
+  INITIALIZE_LIST_HEAD_VARIABLE (mImagePropertiesPrivateData.ImageRecordList)
+};
+
+EFI_PROPERTIES_TABLE  mPropertiesTable = {
+  EFI_PROPERTIES_TABLE_VERSION,
+  sizeof(EFI_PROPERTIES_TABLE),
+  EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA
+};
+
+EFI_LOCK           mPropertiesTableLock = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+
+BOOLEAN            mPropertiesTableEnable;
+
+//
+// Below functions are for MemoryMap
+//
+
+/**
+  Converts a number of EFI_PAGEs to a size in bytes.
+
+  NOTE: Do not use EFI_PAGES_TO_SIZE because it handles UINTN only.
+
+  @param  Pages     The number of EFI_PAGES.
+
+  @return  The number of bytes associated with the number of EFI_PAGEs specified
+           by Pages.
+**/
+STATIC
+UINT64
+EfiPagesToSize (
+  IN UINT64 Pages
+  )
+{
+  return LShiftU64 (Pages, EFI_PAGE_SHIFT);
+}
+
+/**
+  Converts a size, in bytes, to a number of EFI_PAGESs.
+
+  NOTE: Do not use EFI_SIZE_TO_PAGES because it handles UINTN only.
+
+  @param  Size      A size in bytes.
+
+  @return  The number of EFI_PAGESs associated with the number of bytes specified
+           by Size.
+
+**/
+STATIC
+UINT64
+EfiSizeToPages (
+  IN UINT64 Size
+  )
+{
+  return RShiftU64 (Size, EFI_PAGE_SHIFT) + ((((UINTN)Size) & EFI_PAGE_MASK) ? 1 : 0);
+}
+
+/**
+  Acquire memory lock on mPropertiesTableLock.
+**/
+STATIC
+VOID
+CoreAcquirePropertiesTableLock (
+  VOID
+  )
+{
+  CoreAcquireLock (&mPropertiesTableLock);
+}
+
+/**
+  Release memory lock on mPropertiesTableLock.
+**/
+STATIC
+VOID
+CoreReleasePropertiesTableLock (
+  VOID
+  )
+{
+  CoreReleaseLock (&mPropertiesTableLock);
+}
+
+/**
+  Sort memory map entries based upon PhysicalStart, from low to high.
+
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MemoryMapSize          Size, in bytes, of the MemoryMap buffer.
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+STATIC
+VOID
+SortMemoryMap (
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN UINTN                      MemoryMapSize,
+  IN UINTN                      DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *NextMemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEnd;
+  EFI_MEMORY_DESCRIPTOR       TempMemoryMap;
+
+  MemoryMapEntry = MemoryMap;
+  NextMemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+  MemoryMapEnd = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) MemoryMap + MemoryMapSize);
+  while (MemoryMapEntry < MemoryMapEnd) {
+    while (NextMemoryMapEntry < MemoryMapEnd) {
+      if (MemoryMapEntry->PhysicalStart > NextMemoryMapEntry->PhysicalStart) {
+        CopyMem (&TempMemoryMap, MemoryMapEntry, sizeof(EFI_MEMORY_DESCRIPTOR));
+        CopyMem (MemoryMapEntry, NextMemoryMapEntry, sizeof(EFI_MEMORY_DESCRIPTOR));
+        CopyMem (NextMemoryMapEntry, &TempMemoryMap, sizeof(EFI_MEMORY_DESCRIPTOR));
+      }
+
+      NextMemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (NextMemoryMapEntry, DescriptorSize);
+    }
+
+    MemoryMapEntry      = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+    NextMemoryMapEntry  = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+  }
+
+  return ;
+}
+
+/**
+  Merge continous memory map entries whose have same attributes.
+
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 the current memory map.  On output,
+                                 it is the size of new memory map after merge.
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+STATIC
+VOID
+MergeMemoryMap (
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN OUT UINTN                  *MemoryMapSize,
+  IN UINTN                      DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEnd;
+  UINT64                      MemoryBlockLength;
+  EFI_MEMORY_DESCRIPTOR       *NewMemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *NextMemoryMapEntry;
+
+  MemoryMapEntry = MemoryMap;
+  NewMemoryMapEntry = MemoryMap;
+  MemoryMapEnd = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) MemoryMap + *MemoryMapSize);
+  while ((UINTN)MemoryMapEntry < (UINTN)MemoryMapEnd) {
+    CopyMem (NewMemoryMapEntry, MemoryMapEntry, sizeof(EFI_MEMORY_DESCRIPTOR));
+    NextMemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+
+    do {
+      MemoryBlockLength = (UINT64) (EfiPagesToSize (MemoryMapEntry->NumberOfPages));
+      if (((UINTN)NextMemoryMapEntry < (UINTN)MemoryMapEnd) &&
+          (MemoryMapEntry->Type == NextMemoryMapEntry->Type) &&
+          (MemoryMapEntry->Attribute == NextMemoryMapEntry->Attribute) &&
+          ((MemoryMapEntry->PhysicalStart + MemoryBlockLength) == NextMemoryMapEntry->PhysicalStart)) {
+        MemoryMapEntry->NumberOfPages += NextMemoryMapEntry->NumberOfPages;
+        if (NewMemoryMapEntry != MemoryMapEntry) {
+          NewMemoryMapEntry->NumberOfPages += NextMemoryMapEntry->NumberOfPages;
+        }
+
+        NextMemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (NextMemoryMapEntry, DescriptorSize);
+        continue;
+      } else {
+        MemoryMapEntry = PREVIOUS_MEMORY_DESCRIPTOR (NextMemoryMapEntry, DescriptorSize);
+        break;
+      }
+    } while (TRUE);
+
+    MemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+    NewMemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (NewMemoryMapEntry, DescriptorSize);
+  }
+
+  *MemoryMapSize = (UINTN)NewMemoryMapEntry - (UINTN)MemoryMap;
+
+  return ;
+}
+
+/**
+  Enforce memory map attributes.
+  This function will set EfiRuntimeServicesData/EfiMemoryMappedIO/EfiMemoryMappedIOPortSpace to be EFI_MEMORY_XP.
+
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MemoryMapSize          Size, in bytes, of the MemoryMap buffer.
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+STATIC
+VOID
+EnforceMemoryMapAttribute (
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN UINTN                      MemoryMapSize,
+  IN UINTN                      DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEnd;
+
+  MemoryMapEntry = MemoryMap;
+  MemoryMapEnd   = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) MemoryMap + MemoryMapSize);
+  while ((UINTN)MemoryMapEntry < (UINTN)MemoryMapEnd) {
+    switch (MemoryMapEntry->Type) {
+    case EfiRuntimeServicesCode:
+      // do nothing
+      break;
+    case EfiRuntimeServicesData:
+    case EfiMemoryMappedIO:
+    case EfiMemoryMappedIOPortSpace:
+      MemoryMapEntry->Attribute |= EFI_MEMORY_XP;
+      break;
+    case EfiReservedMemoryType:
+    case EfiACPIMemoryNVS:
+      break;
+    }
+
+    MemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+  }
+
+  return ;
+}
+
+/**
+  Return the first image record, whose [ImageBase, ImageSize] covered by [Buffer, Length].
+
+  @param Buffer  Start Address
+  @param Length  Address length
+
+  @return first image record covered by [buffer, length]
+**/
+STATIC
+IMAGE_PROPERTIES_RECORD *
+GetImageRecordByAddress (
+  IN EFI_PHYSICAL_ADDRESS  Buffer,
+  IN UINT64                Length
+  )
+{
+  IMAGE_PROPERTIES_RECORD    *ImageRecord;
+  LIST_ENTRY                 *ImageRecordLink;
+  LIST_ENTRY                 *ImageRecordList;
+
+  ImageRecordList = &mImagePropertiesPrivateData.ImageRecordList;
+
+  for (ImageRecordLink = ImageRecordList->ForwardLink;
+       ImageRecordLink != ImageRecordList;
+       ImageRecordLink = ImageRecordLink->ForwardLink) {
+    ImageRecord = CR (
+                    ImageRecordLink,
+                    IMAGE_PROPERTIES_RECORD,
+                    Link,
+                    IMAGE_PROPERTIES_RECORD_SIGNATURE
+                    );
+
+    if ((Buffer <= ImageRecord->ImageBase) &&
+        (Buffer + Length >= ImageRecord->ImageBase + ImageRecord->ImageSize)) {
+      return ImageRecord;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Set the memory map to new entries, according to one old entry,
+  based upon PE code section and data section in image record
+
+  @param  ImageRecord            An image record whose [ImageBase, ImageSize] covered
+                                 by old memory map entry.
+  @param  NewRecord              A pointer to several new memory map entries.
+                                 The caller gurantee the buffer size be 1 +
+                                 (SplitRecordCount * DescriptorSize) calculated
+                                 below.
+  @param  OldRecord              A pointer to one old memory map entry.
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+STATIC
+UINTN
+SetNewRecord (
+  IN IMAGE_PROPERTIES_RECORD       *ImageRecord,
+  IN OUT EFI_MEMORY_DESCRIPTOR     *NewRecord,
+  IN EFI_MEMORY_DESCRIPTOR         *OldRecord,
+  IN UINTN                         DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR                     TempRecord;
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      *ImageRecordCodeSection;
+  LIST_ENTRY                                *ImageRecordCodeSectionLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionEndLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionList;
+  UINTN                                     NewRecordCount;
+  UINT64                                    PhysicalEnd;
+  UINT64                                    ImageEnd;
+
+  CopyMem (&TempRecord, OldRecord, sizeof(EFI_MEMORY_DESCRIPTOR));
+  PhysicalEnd = TempRecord.PhysicalStart + EfiPagesToSize(TempRecord.NumberOfPages);
+  NewRecordCount = 0;
+
+  ImageRecordCodeSectionList = &ImageRecord->CodeSegmentList;
+
+  ImageRecordCodeSectionLink = ImageRecordCodeSectionList->ForwardLink;
+  ImageRecordCodeSectionEndLink = ImageRecordCodeSectionList;
+  while (ImageRecordCodeSectionLink != ImageRecordCodeSectionEndLink) {
+    ImageRecordCodeSection = CR (
+                               ImageRecordCodeSectionLink,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION,
+                               Link,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE
+                               );
+    ImageRecordCodeSectionLink = ImageRecordCodeSectionLink->ForwardLink;
+
+    if (TempRecord.PhysicalStart <= ImageRecordCodeSection->CodeSegmentBase) {
+      //
+      // DATA
+      //
+      if (!mPropertiesTableEnable) {
+        NewRecord->Type = TempRecord.Type;
+      } else {
+        NewRecord->Type = EfiRuntimeServicesData;
+      }
+      NewRecord->PhysicalStart = TempRecord.PhysicalStart;
+      NewRecord->VirtualStart  = 0;
+      NewRecord->NumberOfPages = EfiSizeToPages(ImageRecordCodeSection->CodeSegmentBase - NewRecord->PhysicalStart);
+      NewRecord->Attribute     = TempRecord.Attribute | EFI_MEMORY_XP;
+      if (NewRecord->NumberOfPages != 0) {
+        NewRecord = NEXT_MEMORY_DESCRIPTOR (NewRecord, DescriptorSize);
+        NewRecordCount ++;
+      }
+
+      //
+      // CODE
+      //
+      if (!mPropertiesTableEnable) {
+        NewRecord->Type = TempRecord.Type;
+      } else {
+        NewRecord->Type = EfiRuntimeServicesCode;
+      }
+      NewRecord->PhysicalStart = ImageRecordCodeSection->CodeSegmentBase;
+      NewRecord->VirtualStart  = 0;
+      NewRecord->NumberOfPages = EfiSizeToPages(ImageRecordCodeSection->CodeSegmentSize);
+      NewRecord->Attribute     = (TempRecord.Attribute & (~EFI_MEMORY_XP)) | EFI_MEMORY_RO;
+      if (NewRecord->NumberOfPages != 0) {
+        NewRecord = NEXT_MEMORY_DESCRIPTOR (NewRecord, DescriptorSize);
+        NewRecordCount ++;
+      }
+
+      TempRecord.PhysicalStart = ImageRecordCodeSection->CodeSegmentBase + EfiPagesToSize (EfiSizeToPages(ImageRecordCodeSection->CodeSegmentSize));
+      TempRecord.NumberOfPages = EfiSizeToPages(PhysicalEnd - TempRecord.PhysicalStart);
+      if (TempRecord.NumberOfPages == 0) {
+        break;
+      }
+    }
+  }
+
+  ImageEnd = ImageRecord->ImageBase + ImageRecord->ImageSize;
+
+  //
+  // Final DATA
+  //
+  if (TempRecord.PhysicalStart < ImageEnd) {
+    if (!mPropertiesTableEnable) {
+      NewRecord->Type = TempRecord.Type;
+    } else {
+      NewRecord->Type = EfiRuntimeServicesData;
+    }
+    NewRecord->PhysicalStart = TempRecord.PhysicalStart;
+    NewRecord->VirtualStart  = 0;
+    NewRecord->NumberOfPages = EfiSizeToPages (ImageEnd - TempRecord.PhysicalStart);
+    NewRecord->Attribute     = TempRecord.Attribute | EFI_MEMORY_XP;
+    NewRecordCount ++;
+  }
+
+  return NewRecordCount;
+}
+
+/**
+  Return the max number of new splitted entries, according to one old entry,
+  based upon PE code section and data section.
+
+  @param  OldRecord              A pointer to one old memory map entry.
+
+  @retval  0 no entry need to be splitted.
+  @return  the max number of new splitted entries
+**/
+STATIC
+UINTN
+GetMaxSplitRecordCount (
+  IN EFI_MEMORY_DESCRIPTOR *OldRecord
+  )
+{
+  IMAGE_PROPERTIES_RECORD *ImageRecord;
+  UINTN                   SplitRecordCount;
+  UINT64                  PhysicalStart;
+  UINT64                  PhysicalEnd;
+
+  SplitRecordCount = 0;
+  PhysicalStart = OldRecord->PhysicalStart;
+  PhysicalEnd = OldRecord->PhysicalStart + EfiPagesToSize(OldRecord->NumberOfPages);
+
+  do {
+    ImageRecord = GetImageRecordByAddress (PhysicalStart, PhysicalEnd - PhysicalStart);
+    if (ImageRecord == NULL) {
+      break;
+    }
+    SplitRecordCount += (2 * ImageRecord->CodeSegmentCount + 1);
+    PhysicalStart = ImageRecord->ImageBase + ImageRecord->ImageSize;
+  } while ((ImageRecord != NULL) && (PhysicalStart < PhysicalEnd));
+
+  if (SplitRecordCount != 0) {
+    SplitRecordCount--;
+  }
+
+  return SplitRecordCount;
+}
+
+/**
+  Split the memory map to new entries, according to one old entry,
+  based upon PE code section and data section.
+
+  @param  OldRecord              A pointer to one old memory map entry.
+  @param  NewRecord              A pointer to several new memory map entries.
+                                 The caller gurantee the buffer size be 1 +
+                                 (SplitRecordCount * DescriptorSize) calculated
+                                 below.
+  @param  MaxSplitRecordCount    The max number of splitted entries
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+
+  @retval  0 no entry is splitted.
+  @return  the real number of splitted record.
+**/
+STATIC
+UINTN
+SplitRecord (
+  IN EFI_MEMORY_DESCRIPTOR     *OldRecord,
+  IN OUT EFI_MEMORY_DESCRIPTOR *NewRecord,
+  IN UINTN                     MaxSplitRecordCount,
+  IN UINTN                     DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR   TempRecord;
+  IMAGE_PROPERTIES_RECORD *ImageRecord;
+  IMAGE_PROPERTIES_RECORD *NewImageRecord;
+  UINT64                  PhysicalStart;
+  UINT64                  PhysicalEnd;
+  UINTN                   NewRecordCount;
+  UINTN                   TotalNewRecordCount;
+  BOOLEAN                 IsLastRecordData;
+
+  if (MaxSplitRecordCount == 0) {
+    CopyMem (NewRecord, OldRecord, DescriptorSize);
+    return 0;
+  }
+
+  TotalNewRecordCount = 0;
+
+  //
+  // Override previous record
+  //
+  CopyMem (&TempRecord, OldRecord, sizeof(EFI_MEMORY_DESCRIPTOR));
+  PhysicalStart = TempRecord.PhysicalStart;
+  PhysicalEnd = TempRecord.PhysicalStart + EfiPagesToSize(TempRecord.NumberOfPages);
+
+  ImageRecord = NULL;
+  do {
+    NewImageRecord = GetImageRecordByAddress (PhysicalStart, PhysicalEnd - PhysicalStart);
+    if (NewImageRecord == NULL) {
+      //
+      // No more image covered by this range, stop
+      //
+      if ((PhysicalEnd > PhysicalStart) && (ImageRecord != NULL)) {
+        //
+        // If this is still address in this record, need record.
+        //
+        NewRecord = PREVIOUS_MEMORY_DESCRIPTOR (NewRecord, DescriptorSize);
+        IsLastRecordData = FALSE;
+        if (!mPropertiesTableEnable) {
+          if ((NewRecord->Attribute & EFI_MEMORY_XP) != 0) {
+            IsLastRecordData = TRUE;
+          }
+        } else {
+          if (NewRecord->Type == EfiRuntimeServicesData) {
+            IsLastRecordData = TRUE;
+          }
+        }
+        if (IsLastRecordData) {
+          //
+          // Last record is DATA, just merge it.
+          //
+          NewRecord->NumberOfPages = EfiSizeToPages(PhysicalEnd - NewRecord->PhysicalStart);
+        } else {
+          //
+          // Last record is CODE, create a new DATA entry.
+          //
+          NewRecord = NEXT_MEMORY_DESCRIPTOR (NewRecord, DescriptorSize);
+          if (!mPropertiesTableEnable) {
+            NewRecord->Type = TempRecord.Type;
+          } else {
+            NewRecord->Type = EfiRuntimeServicesData;
+          }
+          NewRecord->PhysicalStart = TempRecord.PhysicalStart;
+          NewRecord->VirtualStart  = 0;
+          NewRecord->NumberOfPages = TempRecord.NumberOfPages;
+          NewRecord->Attribute     = TempRecord.Attribute | EFI_MEMORY_XP;
+          TotalNewRecordCount ++;
+        }
+      }
+      break;
+    }
+    ImageRecord = NewImageRecord;
+
+    //
+    // Set new record
+    //
+    NewRecordCount = SetNewRecord (ImageRecord, NewRecord, &TempRecord, DescriptorSize);
+    TotalNewRecordCount += NewRecordCount;
+    NewRecord = (EFI_MEMORY_DESCRIPTOR *)((UINT8 *)NewRecord + NewRecordCount * DescriptorSize);
+
+    //
+    // Update PhysicalStart, in order to exclude the image buffer already splitted.
+    //
+    PhysicalStart = ImageRecord->ImageBase + ImageRecord->ImageSize;
+    TempRecord.PhysicalStart = PhysicalStart;
+    TempRecord.NumberOfPages = EfiSizeToPages (PhysicalEnd - PhysicalStart);
+  } while ((ImageRecord != NULL) && (PhysicalStart < PhysicalEnd));
+
+  return TotalNewRecordCount - 1;
+}
+
+/**
+  Split the original memory map, and add more entries to describe PE code section and data section.
+  This function will set EfiRuntimeServicesData to be EFI_MEMORY_XP.
+  This function will merge entries with same attributes finally.
+
+  NOTE: It assumes PE code/data section are page aligned.
+  NOTE: It assumes enough entry is prepared for new memory map.
+
+  Split table:
+   +---------------+
+   | Record X      |
+   +---------------+
+   | Record RtCode |
+   +---------------+
+   | Record Y      |
+   +---------------+
+   ==>
+   +---------------+
+   | Record X      |
+   +---------------+ ----
+   | Record RtData |     |
+   +---------------+     |
+   | Record RtCode |     |-> PE/COFF1
+   +---------------+     |
+   | Record RtData |     |
+   +---------------+ ----
+   | Record RtData |     |
+   +---------------+     |
+   | Record RtCode |     |-> PE/COFF2
+   +---------------+     |
+   | Record RtData |     |
+   +---------------+ ----
+   | Record Y      |
+   +---------------+
+
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 old MemoryMap before split. The actual buffer
+                                 size of MemoryMap is MemoryMapSize +
+                                 (AdditionalRecordCount * DescriptorSize) calculated
+                                 below. On output, it is the size of new MemoryMap
+                                 after split.
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  DescriptorSize         Size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+STATIC
+VOID
+SplitTable (
+  IN OUT UINTN                  *MemoryMapSize,
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN UINTN                      DescriptorSize
+  )
+{
+  INTN        IndexOld;
+  INTN        IndexNew;
+  UINTN       MaxSplitRecordCount;
+  UINTN       RealSplitRecordCount;
+  UINTN       TotalSplitRecordCount;
+  UINTN       AdditionalRecordCount;
+
+  AdditionalRecordCount = (2 * mImagePropertiesPrivateData.CodeSegmentCountMax + 1) * mImagePropertiesPrivateData.ImageRecordCount;
+
+  TotalSplitRecordCount = 0;
+  //
+  // Let old record point to end of valid MemoryMap buffer.
+  //
+  IndexOld = ((*MemoryMapSize) / DescriptorSize) - 1;
+  //
+  // Let new record point to end of full MemoryMap buffer.
+  //
+  IndexNew = ((*MemoryMapSize) / DescriptorSize) - 1 + AdditionalRecordCount;
+  for (; IndexOld >= 0; IndexOld--) {
+    MaxSplitRecordCount = GetMaxSplitRecordCount ((EFI_MEMORY_DESCRIPTOR *)((UINT8 *)MemoryMap + IndexOld * DescriptorSize));
+    //
+    // Split this MemoryMap record
+    //
+    IndexNew -= MaxSplitRecordCount;
+    RealSplitRecordCount = SplitRecord (
+                             (EFI_MEMORY_DESCRIPTOR *)((UINT8 *)MemoryMap + IndexOld * DescriptorSize),
+                             (EFI_MEMORY_DESCRIPTOR *)((UINT8 *)MemoryMap + IndexNew * DescriptorSize),
+                             MaxSplitRecordCount,
+                             DescriptorSize
+                             );
+    //
+    // Adjust IndexNew according to real split.
+    //
+    CopyMem (
+      ((UINT8 *)MemoryMap + (IndexNew + MaxSplitRecordCount - RealSplitRecordCount) * DescriptorSize),
+      ((UINT8 *)MemoryMap + IndexNew * DescriptorSize),
+      RealSplitRecordCount * DescriptorSize
+      );
+    IndexNew = IndexNew + MaxSplitRecordCount - RealSplitRecordCount;
+    TotalSplitRecordCount += RealSplitRecordCount;
+    IndexNew --;
+  }
+  //
+  // Move all records to the beginning.
+  //
+  CopyMem (
+    MemoryMap,
+    (UINT8 *)MemoryMap + (AdditionalRecordCount - TotalSplitRecordCount) * DescriptorSize,
+    (*MemoryMapSize) + TotalSplitRecordCount * DescriptorSize
+    );
+
+  *MemoryMapSize = (*MemoryMapSize) + DescriptorSize * TotalSplitRecordCount;
+
+  //
+  // Sort from low to high (Just in case)
+  //
+  SortMemoryMap (MemoryMap, *MemoryMapSize, DescriptorSize);
+
+  //
+  // Set RuntimeData to XP
+  //
+  EnforceMemoryMapAttribute (MemoryMap, *MemoryMapSize, DescriptorSize);
+
+  //
+  // Merge same type to save entry size
+  //
+  MergeMemoryMap (MemoryMap, MemoryMapSize, DescriptorSize);
+
+  return ;
+}
+
+/**
+  This function for GetMemoryMap() with properties table capability.
+
+  It calls original GetMemoryMap() to get the original memory map information. Then
+  plus the additional memory map entries for PE Code/Data seperation.
+
+  @param  MemoryMapSize          A pointer to the size, in bytes, of the
+                                 MemoryMap buffer. On input, this is the size of
+                                 the buffer allocated by the caller.  On output,
+                                 it is the size of the buffer returned by the
+                                 firmware  if the buffer was large enough, or the
+                                 size of the buffer needed  to contain the map if
+                                 the buffer was too small.
+  @param  MemoryMap              A pointer to the buffer in which firmware places
+                                 the current memory map.
+  @param  MapKey                 A pointer to the location in which firmware
+                                 returns the key for the current memory map.
+  @param  DescriptorSize         A pointer to the location in which firmware
+                                 returns the size, in bytes, of an individual
+                                 EFI_MEMORY_DESCRIPTOR.
+  @param  DescriptorVersion      A pointer to the location in which firmware
+                                 returns the version number associated with the
+                                 EFI_MEMORY_DESCRIPTOR.
+
+  @retval EFI_SUCCESS            The memory map was returned in the MemoryMap
+                                 buffer.
+  @retval EFI_BUFFER_TOO_SMALL   The MemoryMap buffer was too small. The current
+                                 buffer size needed to hold the memory map is
+                                 returned in MemoryMapSize.
+  @retval EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreGetMemoryMapPropertiesTable (
+  IN OUT UINTN                  *MemoryMapSize,
+  IN OUT EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  OUT UINTN                     *MapKey,
+  OUT UINTN                     *DescriptorSize,
+  OUT UINT32                    *DescriptorVersion
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       OldMemoryMapSize;
+  UINTN       AdditionalRecordCount;
+
+  //
+  // If PE code/data is not aligned, just return.
+  //
+  if ((mPropertiesTable.MemoryProtectionAttribute & EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA) == 0) {
+    return CoreGetMemoryMap (MemoryMapSize, MemoryMap, MapKey, DescriptorSize, DescriptorVersion);
+  }
+
+  if (MemoryMapSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CoreAcquirePropertiesTableLock ();
+
+  AdditionalRecordCount = (2 * mImagePropertiesPrivateData.CodeSegmentCountMax + 1) * mImagePropertiesPrivateData.ImageRecordCount;
+
+  OldMemoryMapSize = *MemoryMapSize;
+  Status = CoreGetMemoryMap (MemoryMapSize, MemoryMap, MapKey, DescriptorSize, DescriptorVersion);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    *MemoryMapSize = *MemoryMapSize + (*DescriptorSize) * AdditionalRecordCount;
+  } else if (Status == EFI_SUCCESS) {
+    ASSERT (MemoryMap != NULL);
+    if (OldMemoryMapSize - *MemoryMapSize < (*DescriptorSize) * AdditionalRecordCount) {
+      *MemoryMapSize = *MemoryMapSize + (*DescriptorSize) * AdditionalRecordCount;
+      //
+      // Need update status to buffer too small
+      //
+      Status = EFI_BUFFER_TOO_SMALL;
+    } else {
+      //
+      // Split PE code/data
+      //
+      SplitTable (MemoryMapSize, MemoryMap, *DescriptorSize);
+    }
+  }
+
+  CoreReleasePropertiesTableLock ();
+  return Status;
+}
+
+//
+// Below functions are for ImageRecord
+//
+
+/**
+  Set PropertiesTable accroding to PE/COFF image section alignment.
+
+  @param  SectionAlignment    PE/COFF section alignment
+**/
+STATIC
+VOID
+SetPropertiesTableSectionAlignment (
+  IN UINT32  SectionAlignment
+  )
+{
+  if (((SectionAlignment & (EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT - 1)) != 0) &&
+      ((mPropertiesTable.MemoryProtectionAttribute & EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA) != 0)) {
+    DEBUG ((EFI_D_VERBOSE, "SetPropertiesTableSectionAlignment - Clear\n"));
+    mPropertiesTable.MemoryProtectionAttribute &= ~((UINT64)EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA);
+    gBS->GetMemoryMap = CoreGetMemoryMap;
+    gBS->Hdr.CRC32 = 0;
+    gBS->CalculateCrc32 ((UINT8 *)gBS, gBS->Hdr.HeaderSize, &gBS->Hdr.CRC32);
+  }
+}
+
+/**
+  Swap two code sections in image record.
+
+  @param  FirstImageRecordCodeSection    first code section in image record
+  @param  SecondImageRecordCodeSection   second code section in image record
+**/
+STATIC
+VOID
+SwapImageRecordCodeSection (
+  IN IMAGE_PROPERTIES_RECORD_CODE_SECTION      *FirstImageRecordCodeSection,
+  IN IMAGE_PROPERTIES_RECORD_CODE_SECTION      *SecondImageRecordCodeSection
+  )
+{
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      TempImageRecordCodeSection;
+
+  TempImageRecordCodeSection.CodeSegmentBase = FirstImageRecordCodeSection->CodeSegmentBase;
+  TempImageRecordCodeSection.CodeSegmentSize = FirstImageRecordCodeSection->CodeSegmentSize;
+
+  FirstImageRecordCodeSection->CodeSegmentBase = SecondImageRecordCodeSection->CodeSegmentBase;
+  FirstImageRecordCodeSection->CodeSegmentSize = SecondImageRecordCodeSection->CodeSegmentSize;
+
+  SecondImageRecordCodeSection->CodeSegmentBase = TempImageRecordCodeSection.CodeSegmentBase;
+  SecondImageRecordCodeSection->CodeSegmentSize = TempImageRecordCodeSection.CodeSegmentSize;
+}
+
+/**
+  Sort code section in image record, based upon CodeSegmentBase from low to high.
+
+  @param  ImageRecord    image record to be sorted
+**/
+STATIC
+VOID
+SortImageRecordCodeSection (
+  IN IMAGE_PROPERTIES_RECORD              *ImageRecord
+  )
+{
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      *ImageRecordCodeSection;
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      *NextImageRecordCodeSection;
+  LIST_ENTRY                                *ImageRecordCodeSectionLink;
+  LIST_ENTRY                                *NextImageRecordCodeSectionLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionEndLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionList;
+
+  ImageRecordCodeSectionList = &ImageRecord->CodeSegmentList;
+
+  ImageRecordCodeSectionLink = ImageRecordCodeSectionList->ForwardLink;
+  NextImageRecordCodeSectionLink = ImageRecordCodeSectionLink->ForwardLink;
+  ImageRecordCodeSectionEndLink = ImageRecordCodeSectionList;
+  while (ImageRecordCodeSectionLink != ImageRecordCodeSectionEndLink) {
+    ImageRecordCodeSection = CR (
+                               ImageRecordCodeSectionLink,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION,
+                               Link,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE
+                               );
+    while (NextImageRecordCodeSectionLink != ImageRecordCodeSectionEndLink) {
+      NextImageRecordCodeSection = CR (
+                                     NextImageRecordCodeSectionLink,
+                                     IMAGE_PROPERTIES_RECORD_CODE_SECTION,
+                                     Link,
+                                     IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE
+                                     );
+      if (ImageRecordCodeSection->CodeSegmentBase > NextImageRecordCodeSection->CodeSegmentBase) {
+        SwapImageRecordCodeSection (ImageRecordCodeSection, NextImageRecordCodeSection);
+      }
+      NextImageRecordCodeSectionLink = NextImageRecordCodeSectionLink->ForwardLink;
+    }
+
+    ImageRecordCodeSectionLink = ImageRecordCodeSectionLink->ForwardLink;
+    NextImageRecordCodeSectionLink = ImageRecordCodeSectionLink->ForwardLink;
+  }
+}
+
+/**
+  Check if code section in image record is valid.
+
+  @param  ImageRecord    image record to be checked
+
+  @retval TRUE  image record is valid
+  @retval FALSE image record is invalid
+**/
+STATIC
+BOOLEAN
+IsImageRecordCodeSectionValid (
+  IN IMAGE_PROPERTIES_RECORD              *ImageRecord
+  )
+{
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      *ImageRecordCodeSection;
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION      *LastImageRecordCodeSection;
+  LIST_ENTRY                                *ImageRecordCodeSectionLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionEndLink;
+  LIST_ENTRY                                *ImageRecordCodeSectionList;
+
+  DEBUG ((EFI_D_VERBOSE, "ImageCode SegmentCount - 0x%x\n", ImageRecord->CodeSegmentCount));
+
+  ImageRecordCodeSectionList = &ImageRecord->CodeSegmentList;
+
+  ImageRecordCodeSectionLink = ImageRecordCodeSectionList->ForwardLink;
+  ImageRecordCodeSectionEndLink = ImageRecordCodeSectionList;
+  LastImageRecordCodeSection = NULL;
+  while (ImageRecordCodeSectionLink != ImageRecordCodeSectionEndLink) {
+    ImageRecordCodeSection = CR (
+                               ImageRecordCodeSectionLink,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION,
+                               Link,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE
+                               );
+    if (ImageRecordCodeSection->CodeSegmentSize == 0) {
+      return FALSE;
+    }
+    if (ImageRecordCodeSection->CodeSegmentBase < ImageRecord->ImageBase) {
+      return FALSE;
+    }
+    if (ImageRecordCodeSection->CodeSegmentBase >= MAX_ADDRESS - ImageRecordCodeSection->CodeSegmentSize) {
+      return FALSE;
+    }
+    if ((ImageRecordCodeSection->CodeSegmentBase + ImageRecordCodeSection->CodeSegmentSize) > (ImageRecord->ImageBase + ImageRecord->ImageSize)) {
+      return FALSE;
+    }
+    if (LastImageRecordCodeSection != NULL) {
+      if ((LastImageRecordCodeSection->CodeSegmentBase + LastImageRecordCodeSection->CodeSegmentSize) > ImageRecordCodeSection->CodeSegmentBase) {
+        return FALSE;
+      }
+    }
+
+    LastImageRecordCodeSection = ImageRecordCodeSection;
+    ImageRecordCodeSectionLink = ImageRecordCodeSectionLink->ForwardLink;
+  }
+
+  return TRUE;
+}
+
+/**
+  Swap two image records.
+
+  @param  FirstImageRecord   first image record.
+  @param  SecondImageRecord  second image record.
+**/
+STATIC
+VOID
+SwapImageRecord (
+  IN IMAGE_PROPERTIES_RECORD      *FirstImageRecord,
+  IN IMAGE_PROPERTIES_RECORD      *SecondImageRecord
+  )
+{
+  IMAGE_PROPERTIES_RECORD      TempImageRecord;
+
+  TempImageRecord.ImageBase = FirstImageRecord->ImageBase;
+  TempImageRecord.ImageSize = FirstImageRecord->ImageSize;
+  TempImageRecord.CodeSegmentCount = FirstImageRecord->CodeSegmentCount;
+
+  FirstImageRecord->ImageBase = SecondImageRecord->ImageBase;
+  FirstImageRecord->ImageSize = SecondImageRecord->ImageSize;
+  FirstImageRecord->CodeSegmentCount = SecondImageRecord->CodeSegmentCount;
+
+  SecondImageRecord->ImageBase = TempImageRecord.ImageBase;
+  SecondImageRecord->ImageSize = TempImageRecord.ImageSize;
+  SecondImageRecord->CodeSegmentCount = TempImageRecord.CodeSegmentCount;
+
+  SwapListEntries (&FirstImageRecord->CodeSegmentList, &SecondImageRecord->CodeSegmentList);
+}
+
+/**
+  Sort image record based upon the ImageBase from low to high.
+**/
+STATIC
+VOID
+SortImageRecord (
+  VOID
+  )
+{
+  IMAGE_PROPERTIES_RECORD      *ImageRecord;
+  IMAGE_PROPERTIES_RECORD      *NextImageRecord;
+  LIST_ENTRY                   *ImageRecordLink;
+  LIST_ENTRY                   *NextImageRecordLink;
+  LIST_ENTRY                   *ImageRecordEndLink;
+  LIST_ENTRY                   *ImageRecordList;
+
+  ImageRecordList = &mImagePropertiesPrivateData.ImageRecordList;
+
+  ImageRecordLink = ImageRecordList->ForwardLink;
+  NextImageRecordLink = ImageRecordLink->ForwardLink;
+  ImageRecordEndLink = ImageRecordList;
+  while (ImageRecordLink != ImageRecordEndLink) {
+    ImageRecord = CR (
+                    ImageRecordLink,
+                    IMAGE_PROPERTIES_RECORD,
+                    Link,
+                    IMAGE_PROPERTIES_RECORD_SIGNATURE
+                    );
+    while (NextImageRecordLink != ImageRecordEndLink) {
+      NextImageRecord = CR (
+                          NextImageRecordLink,
+                          IMAGE_PROPERTIES_RECORD,
+                          Link,
+                          IMAGE_PROPERTIES_RECORD_SIGNATURE
+                          );
+      if (ImageRecord->ImageBase > NextImageRecord->ImageBase) {
+        SwapImageRecord (ImageRecord, NextImageRecord);
+      }
+      NextImageRecordLink = NextImageRecordLink->ForwardLink;
+    }
+
+    ImageRecordLink = ImageRecordLink->ForwardLink;
+    NextImageRecordLink = ImageRecordLink->ForwardLink;
+  }
+}
+
+/**
+  Dump image record.
+**/
+STATIC
+VOID
+DumpImageRecord (
+  VOID
+  )
+{
+  IMAGE_PROPERTIES_RECORD      *ImageRecord;
+  LIST_ENTRY                   *ImageRecordLink;
+  LIST_ENTRY                   *ImageRecordList;
+  UINTN                        Index;
+
+  ImageRecordList = &mImagePropertiesPrivateData.ImageRecordList;
+
+  for (ImageRecordLink = ImageRecordList->ForwardLink, Index= 0;
+       ImageRecordLink != ImageRecordList;
+       ImageRecordLink = ImageRecordLink->ForwardLink, Index++) {
+    ImageRecord = CR (
+                    ImageRecordLink,
+                    IMAGE_PROPERTIES_RECORD,
+                    Link,
+                    IMAGE_PROPERTIES_RECORD_SIGNATURE
+                    );
+    DEBUG ((EFI_D_VERBOSE, "  Image[%d]: 0x%016lx - 0x%016lx\n", Index, ImageRecord->ImageBase, ImageRecord->ImageSize));
+  }
+}
+
+/**
+  Insert image record.
+
+  @param  RuntimeImage    Runtime image information
+**/
+VOID
+InsertImageRecord (
+  IN EFI_RUNTIME_IMAGE_ENTRY  *RuntimeImage
+  )
+{
+  VOID                                 *ImageAddress;
+  EFI_IMAGE_DOS_HEADER                 *DosHdr;
+  UINT32                               PeCoffHeaderOffset;
+  UINT32                               SectionAlignment;
+  EFI_IMAGE_SECTION_HEADER             *Section;
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
+  UINT8                                *Name;
+  UINTN                                Index;
+  IMAGE_PROPERTIES_RECORD              *ImageRecord;
+  CHAR8                                *PdbPointer;
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION *ImageRecordCodeSection;
+  UINT16                               Magic;
+
+  DEBUG ((EFI_D_VERBOSE, "InsertImageRecord - 0x%x\n", RuntimeImage));
+  DEBUG ((EFI_D_VERBOSE, "InsertImageRecord - 0x%016lx - 0x%016lx\n", (EFI_PHYSICAL_ADDRESS)(UINTN)RuntimeImage->ImageBase, RuntimeImage->ImageSize));
+
+  ImageRecord = AllocatePool (sizeof(*ImageRecord));
+  if (ImageRecord == NULL) {
+    return ;
+  }
+  ImageRecord->Signature = IMAGE_PROPERTIES_RECORD_SIGNATURE;
+
+  DEBUG ((EFI_D_VERBOSE, "ImageRecordCount - 0x%x\n", mImagePropertiesPrivateData.ImageRecordCount));
+
+  //
+  // Step 1: record whole region
+  //
+  ImageRecord->ImageBase = (EFI_PHYSICAL_ADDRESS)(UINTN)RuntimeImage->ImageBase;
+  ImageRecord->ImageSize = RuntimeImage->ImageSize;
+
+  ImageAddress = RuntimeImage->ImageBase;
+
+  PdbPointer = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageAddress);
+  if (PdbPointer != NULL) {
+    DEBUG ((EFI_D_VERBOSE, "  Image - %a\n", PdbPointer));
+  }
+
+  //
+  // Check PE/COFF image
+  //
+  DosHdr = (EFI_IMAGE_DOS_HEADER *) (UINTN) ImageAddress;
+  PeCoffHeaderOffset = 0;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    PeCoffHeaderOffset = DosHdr->e_lfanew;
+  }
+
+  Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)((UINT8 *) (UINTN) ImageAddress + PeCoffHeaderOffset);
+  if (Hdr.Pe32->Signature != EFI_IMAGE_NT_SIGNATURE) {
+    DEBUG ((EFI_D_VERBOSE, "Hdr.Pe32->Signature invalid - 0x%x\n", Hdr.Pe32->Signature));
+    // It might be image in SMM.
+    goto Finish;
+  }
+
+  //
+  // Get SectionAlignment
+  //
+  if (Hdr.Pe32->FileHeader.Machine == IMAGE_FILE_MACHINE_IA64 && Hdr.Pe32->OptionalHeader.Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+    //
+    // NOTE: Some versions of Linux ELILO for Itanium have an incorrect magic value
+    //       in the PE/COFF Header. If the MachineType is Itanium(IA64) and the
+    //       Magic value in the OptionalHeader is EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC
+    //       then override the magic value to EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC
+    //
+    Magic = EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC;
+  } else {
+    //
+    // Get the magic value from the PE/COFF Optional Header
+    //
+    Magic = Hdr.Pe32->OptionalHeader.Magic;
+  }
+  if (Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+    SectionAlignment  = Hdr.Pe32->OptionalHeader.SectionAlignment;
+  } else {
+    SectionAlignment  = Hdr.Pe32Plus->OptionalHeader.SectionAlignment;
+  }
+
+  SetPropertiesTableSectionAlignment (SectionAlignment);
+  if ((SectionAlignment & (EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT - 1)) != 0) {
+    DEBUG ((EFI_D_WARN, "!!!!!!!!  InsertImageRecord - Section Alignment(0x%x) is not %dK  !!!!!!!!\n",
+      SectionAlignment, EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT >> 10));
+    PdbPointer = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageAddress);
+    if (PdbPointer != NULL) {
+      DEBUG ((EFI_D_WARN, "!!!!!!!!  Image - %a  !!!!!!!!\n", PdbPointer));
+    }
+    goto Finish;
+  }
+
+  Section = (EFI_IMAGE_SECTION_HEADER *) (
+               (UINT8 *) (UINTN) ImageAddress +
+               PeCoffHeaderOffset +
+               sizeof(UINT32) +
+               sizeof(EFI_IMAGE_FILE_HEADER) +
+               Hdr.Pe32->FileHeader.SizeOfOptionalHeader
+               );
+  ImageRecord->CodeSegmentCount = 0;
+  InitializeListHead (&ImageRecord->CodeSegmentList);
+  for (Index = 0; Index < Hdr.Pe32->FileHeader.NumberOfSections; Index++) {
+    Name = Section[Index].Name;
+    DEBUG ((
+      EFI_D_VERBOSE,
+      "  Section - '%c%c%c%c%c%c%c%c'\n",
+      Name[0],
+      Name[1],
+      Name[2],
+      Name[3],
+      Name[4],
+      Name[5],
+      Name[6],
+      Name[7]
+      ));
+
+    if ((Section[Index].Characteristics & EFI_IMAGE_SCN_CNT_CODE) != 0) {
+      DEBUG ((EFI_D_VERBOSE, "  VirtualSize          - 0x%08x\n", Section[Index].Misc.VirtualSize));
+      DEBUG ((EFI_D_VERBOSE, "  VirtualAddress       - 0x%08x\n", Section[Index].VirtualAddress));
+      DEBUG ((EFI_D_VERBOSE, "  SizeOfRawData        - 0x%08x\n", Section[Index].SizeOfRawData));
+      DEBUG ((EFI_D_VERBOSE, "  PointerToRawData     - 0x%08x\n", Section[Index].PointerToRawData));
+      DEBUG ((EFI_D_VERBOSE, "  PointerToRelocations - 0x%08x\n", Section[Index].PointerToRelocations));
+      DEBUG ((EFI_D_VERBOSE, "  PointerToLinenumbers - 0x%08x\n", Section[Index].PointerToLinenumbers));
+      DEBUG ((EFI_D_VERBOSE, "  NumberOfRelocations  - 0x%08x\n", Section[Index].NumberOfRelocations));
+      DEBUG ((EFI_D_VERBOSE, "  NumberOfLinenumbers  - 0x%08x\n", Section[Index].NumberOfLinenumbers));
+      DEBUG ((EFI_D_VERBOSE, "  Characteristics      - 0x%08x\n", Section[Index].Characteristics));
+
+      //
+      // Step 2: record code section
+      //
+      ImageRecordCodeSection = AllocatePool (sizeof(*ImageRecordCodeSection));
+      if (ImageRecordCodeSection == NULL) {
+        return ;
+      }
+      ImageRecordCodeSection->Signature = IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE;
+
+      ImageRecordCodeSection->CodeSegmentBase = (UINTN)ImageAddress + Section[Index].VirtualAddress;
+      ImageRecordCodeSection->CodeSegmentSize = Section[Index].SizeOfRawData;
+
+      DEBUG ((EFI_D_VERBOSE, "ImageCode: 0x%016lx - 0x%016lx\n", ImageRecordCodeSection->CodeSegmentBase, ImageRecordCodeSection->CodeSegmentSize));
+
+      InsertTailList (&ImageRecord->CodeSegmentList, &ImageRecordCodeSection->Link);
+      ImageRecord->CodeSegmentCount++;
+    }
+  }
+
+  if (ImageRecord->CodeSegmentCount == 0) {
+    SetPropertiesTableSectionAlignment (1);
+    DEBUG ((EFI_D_ERROR, "!!!!!!!!  InsertImageRecord - CodeSegmentCount is 0  !!!!!!!!\n"));
+    PdbPointer = PeCoffLoaderGetPdbPointer ((VOID*) (UINTN) ImageAddress);
+    if (PdbPointer != NULL) {
+      DEBUG ((EFI_D_ERROR, "!!!!!!!!  Image - %a  !!!!!!!!\n", PdbPointer));
+    }
+    goto Finish;
+  }
+
+  //
+  // Final
+  //
+  SortImageRecordCodeSection (ImageRecord);
+  //
+  // Check overlap all section in ImageBase/Size
+  //
+  if (!IsImageRecordCodeSectionValid (ImageRecord)) {
+    DEBUG ((EFI_D_ERROR, "IsImageRecordCodeSectionValid - FAIL\n"));
+    goto Finish;
+  }
+
+  InsertTailList (&mImagePropertiesPrivateData.ImageRecordList, &ImageRecord->Link);
+  mImagePropertiesPrivateData.ImageRecordCount++;
+
+  SortImageRecord ();
+
+  if (mImagePropertiesPrivateData.CodeSegmentCountMax < ImageRecord->CodeSegmentCount) {
+    mImagePropertiesPrivateData.CodeSegmentCountMax = ImageRecord->CodeSegmentCount;
+  }
+
+Finish:
+  return ;
+}
+
+/**
+  Find image record accroding to image base and size.
+
+  @param  ImageBase    Base of PE image
+  @param  ImageSize    Size of PE image
+
+  @return image record
+**/
+STATIC
+IMAGE_PROPERTIES_RECORD *
+FindImageRecord (
+  IN EFI_PHYSICAL_ADDRESS  ImageBase,
+  IN UINT64                ImageSize
+  )
+{
+  IMAGE_PROPERTIES_RECORD    *ImageRecord;
+  LIST_ENTRY                 *ImageRecordLink;
+  LIST_ENTRY                 *ImageRecordList;
+
+  ImageRecordList = &mImagePropertiesPrivateData.ImageRecordList;
+
+  for (ImageRecordLink = ImageRecordList->ForwardLink;
+       ImageRecordLink != ImageRecordList;
+       ImageRecordLink = ImageRecordLink->ForwardLink) {
+    ImageRecord = CR (
+                    ImageRecordLink,
+                    IMAGE_PROPERTIES_RECORD,
+                    Link,
+                    IMAGE_PROPERTIES_RECORD_SIGNATURE
+                    );
+
+    if ((ImageBase == ImageRecord->ImageBase) &&
+        (ImageSize == ImageRecord->ImageSize)) {
+      return ImageRecord;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Remove Image record.
+
+  @param  RuntimeImage    Runtime image information
+**/
+VOID
+RemoveImageRecord (
+  IN EFI_RUNTIME_IMAGE_ENTRY  *RuntimeImage
+  )
+{
+  IMAGE_PROPERTIES_RECORD              *ImageRecord;
+  LIST_ENTRY                           *CodeSegmentListHead;
+  IMAGE_PROPERTIES_RECORD_CODE_SECTION *ImageRecordCodeSection;
+
+  DEBUG ((EFI_D_VERBOSE, "RemoveImageRecord - 0x%x\n", RuntimeImage));
+  DEBUG ((EFI_D_VERBOSE, "RemoveImageRecord - 0x%016lx - 0x%016lx\n", (EFI_PHYSICAL_ADDRESS)(UINTN)RuntimeImage->ImageBase, RuntimeImage->ImageSize));
+
+  ImageRecord = FindImageRecord ((EFI_PHYSICAL_ADDRESS)(UINTN)RuntimeImage->ImageBase, RuntimeImage->ImageSize);
+  if (ImageRecord == NULL) {
+    DEBUG ((EFI_D_ERROR, "!!!!!!!! ImageRecord not found !!!!!!!!\n"));
+    return ;
+  }
+
+  CodeSegmentListHead = &ImageRecord->CodeSegmentList;
+  while (!IsListEmpty (CodeSegmentListHead)) {
+    ImageRecordCodeSection = CR (
+                               CodeSegmentListHead->ForwardLink,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION,
+                               Link,
+                               IMAGE_PROPERTIES_RECORD_CODE_SECTION_SIGNATURE
+                               );
+    RemoveEntryList (&ImageRecordCodeSection->Link);
+    FreePool (ImageRecordCodeSection);
+  }
+
+  RemoveEntryList (&ImageRecord->Link);
+  FreePool (ImageRecord);
+  mImagePropertiesPrivateData.ImageRecordCount--;
+}
+
+
+/**
+  Install PropertiesTable.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+**/
+VOID
+EFIAPI
+InstallPropertiesTable (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  if (PcdGetBool (PcdPropertiesTableEnable)) {
+    EFI_STATUS  Status;
+
+    Status = gBS->InstallConfigurationTable (&gEfiPropertiesTableGuid, &mPropertiesTable);
+    ASSERT_EFI_ERROR (Status);
+
+    DEBUG ((EFI_D_INFO, "MemoryProtectionAttribute - 0x%016lx\n", mPropertiesTable.MemoryProtectionAttribute));
+    if ((mPropertiesTable.MemoryProtectionAttribute & EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA) == 0) {
+      DEBUG ((EFI_D_ERROR, "MemoryProtectionAttribute NON_EXECUTABLE_PE_DATA is not set, "));
+      DEBUG ((EFI_D_ERROR, "because Runtime Driver Section Alignment is not %dK.\n", EFI_ACPI_RUNTIME_PAGE_ALLOCATION_ALIGNMENT >> 10));
+      return ;
+    }
+
+    gBS->GetMemoryMap = CoreGetMemoryMapPropertiesTable;
+    gBS->Hdr.CRC32 = 0;
+    gBS->CalculateCrc32 ((UINT8 *)gBS, gBS->Hdr.HeaderSize, &gBS->Hdr.CRC32);
+
+    DEBUG ((EFI_D_VERBOSE, "Total Image Count - 0x%x\n", mImagePropertiesPrivateData.ImageRecordCount));
+    DEBUG ((EFI_D_VERBOSE, "Dump ImageRecord:\n"));
+    DumpImageRecord ();
+
+    mPropertiesTableEnable = TRUE;
+  }
+}
+
+/**
+  Initialize PropertiesTable support.
+**/
+VOID
+EFIAPI
+CoreInitializePropertiesTable (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   EndOfDxeEvent;
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  InstallPropertiesTable,
+                  NULL,
+                  &gEfiEndOfDxeEventGroupGuid,
+                  &EndOfDxeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+  return ;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/SetWatchdogTimer.c b/Core/MdeModulePkg/Core/Dxe/Misc/SetWatchdogTimer.c
new file mode 100644
index 0000000000..f153815e86
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/SetWatchdogTimer.c
@@ -0,0 +1,72 @@
+/** @file
+  UEFI Miscellaneous boot Services SetWatchdogTimer service implementation
+
+Copyright (c) 2006 - 2008, 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 "DxeMain.h"
+
+#define WATCHDOG_TIMER_CALIBRATE_PER_SECOND 10000000
+
+/**
+  Sets the system's watchdog timer.
+
+  @param  Timeout         The number of seconds to set the watchdog timer to.
+                          A value of zero disables the timer.
+  @param  WatchdogCode    The numeric code to log on a watchdog timer timeout
+                          event. The firmware reserves codes 0x0000 to 0xFFFF.
+                          Loaders and operating systems may use other timeout
+                          codes.
+  @param  DataSize        The size, in bytes, of WatchdogData.
+  @param  WatchdogData    A data buffer that includes a Null-terminated Unicode
+                          string, optionally followed by additional binary data.
+                          The string is a description that the call may use to
+                          further indicate the reason to be logged with a
+                          watchdog event.
+
+  @return EFI_SUCCESS               Timeout has been set
+  @return EFI_NOT_AVAILABLE_YET     WatchdogTimer is not available yet
+  @return EFI_UNSUPPORTED           System does not have a timer (currently not used)
+  @return EFI_DEVICE_ERROR          Could not complete due to hardware error
+
+**/
+EFI_STATUS
+EFIAPI
+CoreSetWatchdogTimer (
+  IN UINTN    Timeout,
+  IN UINT64   WatchdogCode,
+  IN UINTN    DataSize,
+  IN CHAR16   *WatchdogData OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check our architectural protocol
+  //
+  if (gWatchdogTimer == NULL) {
+    return EFI_NOT_AVAILABLE_YET;
+  }
+
+  //
+  // Attempt to set the timeout
+  //
+  Status = gWatchdogTimer->SetTimerPeriod (gWatchdogTimer, MultU64x32 (Timeout, WATCHDOG_TIMER_CALIBRATE_PER_SECOND));
+
+  //
+  // Check for errors
+  //
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/Misc/Stall.c b/Core/MdeModulePkg/Core/Dxe/Misc/Stall.c
new file mode 100644
index 0000000000..73e90785e2
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/Misc/Stall.c
@@ -0,0 +1,113 @@
+/** @file
+  UEFI Miscellaneous boot Services Stall service implementation
+
+Copyright (c) 2006 - 2011, 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 statements
+//
+
+#include "DxeMain.h"
+
+/**
+  Internal worker function to call the Metronome Architectural Protocol for 
+  the number of ticks specified by the UINT64 Counter value.  WaitForTick() 
+  service of the Metronome Architectural Protocol uses a UINT32 for the number
+  of ticks to wait, so this function loops when Counter is larger than 0xffffffff.
+
+  @param  Counter           Number of ticks to wait.
+
+**/
+VOID
+CoreInternalWaitForTick (
+  IN UINT64  Counter
+  )
+{
+  while (RShiftU64 (Counter, 32) > 0) {
+    gMetronome->WaitForTick (gMetronome, 0xffffffff);
+    Counter -= 0xffffffff;
+  }
+  gMetronome->WaitForTick (gMetronome, (UINT32)Counter);
+}
+
+/**
+  Introduces a fine-grained stall.
+
+  @param  Microseconds           The number of microseconds to stall execution.
+
+  @retval EFI_SUCCESS            Execution was stalled for at least the requested
+                                 amount of microseconds.
+  @retval EFI_NOT_AVAILABLE_YET  gMetronome is not available yet
+
+**/
+EFI_STATUS
+EFIAPI
+CoreStall (
+  IN UINTN            Microseconds
+  )
+{
+  UINT64  Counter;
+  UINT32  Remainder;
+  UINTN   Index;
+
+  if (gMetronome == NULL) {
+    return EFI_NOT_AVAILABLE_YET;
+  }
+
+  //
+  // Counter = Microseconds * 10 / gMetronome->TickPeriod
+  // 0x1999999999999999 = (2^64 - 1) / 10
+  //
+  if (Microseconds > 0x1999999999999999ULL) {
+    //
+    // Microseconds is too large to multiple by 10 first.  Perform the divide 
+    // operation first and loop 10 times to avoid 64-bit math overflow.
+    //
+    Counter = DivU64x32Remainder (
+                Microseconds,
+                gMetronome->TickPeriod,
+                &Remainder
+                );
+    for (Index = 0; Index < 10; Index++) {
+      CoreInternalWaitForTick (Counter);
+    }      
+
+    if (Remainder != 0) {
+      //
+      // If Remainder was not zero, then normally, Counter would be rounded 
+      // up by 1 tick.  In this case, since a loop for 10 counts was used
+      // to emulate the multiply by 10 operation, Counter needs to be rounded
+      // up by 10 counts.
+      //
+      CoreInternalWaitForTick (10);
+    }
+  } else {
+    //
+    // Calculate the number of ticks by dividing the number of microseconds by
+    // the TickPeriod.  Calculation is based on 100ns unit.
+    //
+    Counter = DivU64x32Remainder (
+                MultU64x32 (Microseconds, 10),
+                gMetronome->TickPeriod,
+                &Remainder
+                );
+    if (Remainder != 0) {
+      //
+      // If Remainder is not zero, then round Counter up by one tick.
+      //
+      Counter++;
+    }
+    CoreInternalWaitForTick (Counter);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Dxe/SectionExtraction/CoreSectionExtraction.c b/Core/MdeModulePkg/Core/Dxe/SectionExtraction/CoreSectionExtraction.c
new file mode 100644
index 0000000000..3c4f3f58db
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Dxe/SectionExtraction/CoreSectionExtraction.c
@@ -0,0 +1,1605 @@
+/** @file
+  Section Extraction Protocol implementation.
+
+  Stream database is implemented as a linked list of section streams,
+  where each stream contains a linked list of children, which may be leaves or
+  encapsulations.
+
+  Children that are encapsulations generate new stream entries
+  when they are created.  Streams can also be created by calls to
+  SEP->OpenSectionStream().
+
+  The database is only created far enough to return the requested data from
+  any given stream, or to determine that the requested data is not found.
+
+  If a GUIDed encapsulation is encountered, there are three possiblilites.
+
+  1) A support protocol is found, in which the stream is simply processed with
+     the support protocol.
+
+  2) A support protocol is not found, but the data is available to be read
+     without processing.  In this case, the database is built up through the
+     recursions to return the data, and a RPN event is set that will enable
+     the stream in question to be refreshed if and when the required section
+     extraction protocol is published.This insures the AuthenticationStatus
+     does not become stale in the cache.
+
+  3) A support protocol is not found, and the data is not available to be read
+     without it.  This results in EFI_PROTOCOL_ERROR.
+
+Copyright (c) 2006 - 2014, 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 "DxeMain.h"
+
+//
+// Local defines and typedefs
+//
+#define CORE_SECTION_CHILD_SIGNATURE  SIGNATURE_32('S','X','C','S')
+#define CHILD_SECTION_NODE_FROM_LINK(Node) \
+  CR (Node, CORE_SECTION_CHILD_NODE, Link, CORE_SECTION_CHILD_SIGNATURE)
+
+typedef struct {
+  UINT32                      Signature;
+  LIST_ENTRY                  Link;
+  UINT32                      Type;
+  UINT32                      Size;
+  //
+  // StreamBase + OffsetInStream == pointer to section header in stream.  The
+  // stream base is always known when walking the sections within.
+  //
+  UINT32                      OffsetInStream;
+  //
+  // Then EncapsulatedStreamHandle below is always 0 if the section is NOT an
+  // encapsulating section.  Otherwise, it contains the stream handle
+  // of the encapsulated stream.  This handle is ALWAYS produced any time an
+  // encapsulating child is encountered, irrespective of whether the
+  // encapsulated stream is processed further.
+  //
+  UINTN                       EncapsulatedStreamHandle;
+  EFI_GUID                    *EncapsulationGuid;
+  //
+  // If the section REQUIRES an extraction protocol, register for RPN 
+  // when the required GUIDed extraction protocol becomes available.
+  //
+  EFI_EVENT                   Event;
+} CORE_SECTION_CHILD_NODE;
+
+#define CORE_SECTION_STREAM_SIGNATURE SIGNATURE_32('S','X','S','S')
+#define STREAM_NODE_FROM_LINK(Node) \
+  CR (Node, CORE_SECTION_STREAM_NODE, Link, CORE_SECTION_STREAM_SIGNATURE)
+
+typedef struct {
+  UINT32                      Signature;
+  LIST_ENTRY                  Link;
+  UINTN                       StreamHandle;
+  UINT8                       *StreamBuffer;
+  UINTN                       StreamLength;
+  LIST_ENTRY                  Children;
+  //
+  // Authentication status is from GUIDed encapsulations.
+  //
+  UINT32                      AuthenticationStatus;
+} CORE_SECTION_STREAM_NODE;
+
+#define NULL_STREAM_HANDLE    0
+
+typedef struct {
+  CORE_SECTION_CHILD_NODE     *ChildNode;
+  CORE_SECTION_STREAM_NODE    *ParentStream;
+  VOID                        *Registration;
+} RPN_EVENT_CONTEXT;
+
+
+/**
+  The ExtractSection() function processes the input section and
+  allocates a buffer from the pool in which it returns the section
+  contents. If the section being extracted contains
+  authentication information (the section's
+  GuidedSectionHeader.Attributes field has the
+  EFI_GUIDED_SECTION_AUTH_STATUS_VALID bit set), the values
+  returned in AuthenticationStatus must reflect the results of
+  the authentication operation. Depending on the algorithm and
+  size of the encapsulated data, the time that is required to do
+  a full authentication may be prohibitively long for some
+  classes of systems. To indicate this, use
+  EFI_SECURITY_POLICY_PROTOCOL_GUID, which may be published by
+  the security policy driver (see the Platform Initialization
+  Driver Execution Environment Core Interface Specification for
+  more details and the GUID definition). If the
+  EFI_SECURITY_POLICY_PROTOCOL_GUID exists in the handle
+  database, then, if possible, full authentication should be
+  skipped and the section contents simply returned in the
+  OutputBuffer. In this case, the
+  EFI_AUTH_STATUS_PLATFORM_OVERRIDE bit AuthenticationStatus
+  must be set on return. ExtractSection() is callable only from
+  TPL_NOTIFY and below. Behavior of ExtractSection() at any
+  EFI_TPL above TPL_NOTIFY is undefined. Type EFI_TPL is
+  defined in RaiseTPL() in the UEFI 2.0 specification.
+
+
+  @param This         Indicates the
+                      EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL instance.
+  @param InputSection Buffer containing the input GUIDed section
+                      to be processed. OutputBuffer OutputBuffer
+                      is allocated from boot services pool
+                      memory and contains the new section
+                      stream. The caller is responsible for
+                      freeing this buffer.
+  @param OutputBuffer *OutputBuffer is allocated from boot services
+                      pool memory and contains the new section stream.
+                      The caller is responsible for freeing this buffer.
+  @param OutputSize   A pointer to a caller-allocated UINTN in
+                      which the size of OutputBuffer allocation
+                      is stored. If the function returns
+                      anything other than EFI_SUCCESS, the value
+                      of OutputSize is undefined.
+
+  @param AuthenticationStatus A pointer to a caller-allocated
+                              UINT32 that indicates the
+                              authentication status of the
+                              output buffer. If the input
+                              section's
+                              GuidedSectionHeader.Attributes
+                              field has the
+                              EFI_GUIDED_SECTION_AUTH_STATUS_VAL
+                              bit as clear, AuthenticationStatus
+                              must return zero. Both local bits
+                              (19:16) and aggregate bits (3:0)
+                              in AuthenticationStatus are
+                              returned by ExtractSection().
+                              These bits reflect the status of
+                              the extraction operation. The bit
+                              pattern in both regions must be
+                              the same, as the local and
+                              aggregate authentication statuses
+                              have equivalent meaning at this
+                              level. If the function returns
+                              anything other than EFI_SUCCESS,
+                              the value of AuthenticationStatus
+                              is undefined.
+
+
+  @retval EFI_SUCCESS          The InputSection was successfully
+                               processed and the section contents were
+                               returned.
+
+  @retval EFI_OUT_OF_RESOURCES The system has insufficient
+                               resources to process the
+                               request.
+
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction
+                                Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL *This,
+  IN CONST  VOID                                   *InputSection,
+  OUT       VOID                                   **OutputBuffer,
+  OUT       UINTN                                  *OutputSize,
+  OUT       UINT32                                 *AuthenticationStatus
+  );
+
+//
+// Module globals
+//
+LIST_ENTRY mStreamRoot = INITIALIZE_LIST_HEAD_VARIABLE (mStreamRoot);
+
+EFI_HANDLE mSectionExtractionHandle = NULL;
+
+EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL mCustomGuidedSectionExtractionProtocol = {
+  CustomGuidedSectionExtract
+};
+
+
+/**
+  Entry point of the section extraction code. Initializes an instance of the
+  section extraction interface and installs it on a new handle.
+
+  @param  ImageHandle   A handle for the image that is initializing this driver
+  @param  SystemTable   A pointer to the EFI system table
+
+  @retval EFI_SUCCESS           Driver initialized successfully
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate needed resources
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeSectionExtraction (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_GUID                           *ExtractHandlerGuidTable;
+  UINTN                              ExtractHandlerNumber;
+
+  //
+  // Get custom extract guided section method guid list
+  //
+  ExtractHandlerNumber = ExtractGuidedSectionGetGuidList (&ExtractHandlerGuidTable);
+
+  Status = EFI_SUCCESS;
+  //
+  // Install custom guided extraction protocol
+  //
+  while (ExtractHandlerNumber-- > 0) {
+    Status = CoreInstallProtocolInterface (
+              &mSectionExtractionHandle,
+              &ExtractHandlerGuidTable [ExtractHandlerNumber],
+              EFI_NATIVE_INTERFACE,
+              &mCustomGuidedSectionExtractionProtocol
+              );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return Status;
+}
+
+
+/**
+  Check if a stream is valid.
+
+  @param  SectionStream          The section stream to be checked
+  @param  SectionStreamLength    The length of section stream
+
+  @return A boolean value indicating the validness of the section stream.
+
+**/
+BOOLEAN
+IsValidSectionStream (
+  IN  VOID              *SectionStream,
+  IN  UINTN             SectionStreamLength
+  )
+{
+  UINTN                       TotalLength;
+  UINTN                       SectionLength;
+  EFI_COMMON_SECTION_HEADER   *SectionHeader;
+  EFI_COMMON_SECTION_HEADER   *NextSectionHeader;
+
+  TotalLength = 0;
+  SectionHeader = (EFI_COMMON_SECTION_HEADER *)SectionStream;
+
+  while (TotalLength < SectionStreamLength) {
+    if (IS_SECTION2 (SectionHeader)) {
+      SectionLength = SECTION2_SIZE (SectionHeader);
+    } else {
+      SectionLength = SECTION_SIZE (SectionHeader);
+    }
+    TotalLength += SectionLength;
+
+    if (TotalLength == SectionStreamLength) {
+      return TRUE;
+    }
+
+    //
+    // Move to the next byte following the section...
+    //
+    SectionHeader = (EFI_COMMON_SECTION_HEADER *) ((UINT8 *) SectionHeader + SectionLength);
+
+    //
+    // Figure out where the next section begins
+    //
+    NextSectionHeader = ALIGN_POINTER(SectionHeader, 4);
+    TotalLength += (UINTN) NextSectionHeader - (UINTN) SectionHeader;
+    SectionHeader = NextSectionHeader;
+  }
+
+  ASSERT (FALSE);
+  return FALSE;
+}
+
+
+/**
+  Worker function.  Constructor for section streams.
+
+  @param  SectionStreamLength    Size in bytes of the section stream.
+  @param  SectionStream          Buffer containing the new section stream.
+  @param  AllocateBuffer         Indicates whether the stream buffer is to be
+                                 copied or the input buffer is to be used in
+                                 place. AuthenticationStatus- Indicates the
+                                 default authentication status for the new
+                                 stream.
+  @param  AuthenticationStatus   A pointer to a caller-allocated UINT32 that
+                                 indicates the authentication status of the
+                                 output buffer. If the input section's
+                                 GuidedSectionHeader.Attributes field
+                                 has the EFI_GUIDED_SECTION_AUTH_STATUS_VALID
+                                 bit as clear, AuthenticationStatus must return
+                                 zero. Both local bits (19:16) and aggregate
+                                 bits (3:0) in AuthenticationStatus are returned
+                                 by ExtractSection(). These bits reflect the
+                                 status of the extraction operation. The bit
+                                 pattern in both regions must be the same, as
+                                 the local and aggregate authentication statuses
+                                 have equivalent meaning at this level. If the
+                                 function returns anything other than
+                                 EFI_SUCCESS, the value of *AuthenticationStatus
+                                 is undefined.
+  @param  SectionStreamHandle    A pointer to a caller allocated section stream
+                                 handle.
+
+  @retval EFI_SUCCESS            Stream was added to stream database.
+  @retval EFI_OUT_OF_RESOURCES   memory allocation failed.
+
+**/
+EFI_STATUS
+OpenSectionStreamEx (
+  IN     UINTN                                     SectionStreamLength,
+  IN     VOID                                      *SectionStream,
+  IN     BOOLEAN                                   AllocateBuffer,
+  IN     UINT32                                    AuthenticationStatus,
+     OUT UINTN                                     *SectionStreamHandle
+  )
+{
+  CORE_SECTION_STREAM_NODE    *NewStream;
+  EFI_TPL                     OldTpl;
+
+  //
+  // Allocate a new stream
+  //
+  NewStream = AllocatePool (sizeof (CORE_SECTION_STREAM_NODE));
+  if (NewStream == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (AllocateBuffer) {
+    //
+    // if we're here, we're double buffering, allocate the buffer and copy the
+    // data in
+    //
+    if (SectionStreamLength > 0) {
+      NewStream->StreamBuffer = AllocatePool (SectionStreamLength);
+      if (NewStream->StreamBuffer == NULL) {
+        CoreFreePool (NewStream);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Copy in stream data
+      //
+      CopyMem (NewStream->StreamBuffer, SectionStream, SectionStreamLength);
+    } else {
+      //
+      // It's possible to have a zero length section stream.
+      //
+      NewStream->StreamBuffer = NULL;
+    }
+  } else {
+    //
+    // If were here, the caller has supplied the buffer (it's an internal call)
+    // so just assign the buffer.  This happens when we open section streams
+    // as a result of expanding an encapsulating section.
+    //
+    NewStream->StreamBuffer = SectionStream;
+  }
+
+  //
+  // Initialize the rest of the section stream
+  //
+  NewStream->Signature = CORE_SECTION_STREAM_SIGNATURE;
+  NewStream->StreamHandle = (UINTN) NewStream;
+  NewStream->StreamLength = SectionStreamLength;
+  InitializeListHead (&NewStream->Children);
+  NewStream->AuthenticationStatus = AuthenticationStatus;
+
+  //
+  // Add new stream to stream list
+  //
+  OldTpl = CoreRaiseTpl (TPL_NOTIFY);
+  InsertTailList (&mStreamRoot, &NewStream->Link);
+  CoreRestoreTpl (OldTpl);
+
+  *SectionStreamHandle = NewStream->StreamHandle;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  SEP member function.  This function creates and returns a new section stream
+  handle to represent the new section stream.
+
+  @param  SectionStreamLength    Size in bytes of the section stream.
+  @param  SectionStream          Buffer containing the new section stream.
+  @param  SectionStreamHandle    A pointer to a caller allocated UINTN that on
+                                 output contains the new section stream handle.
+
+  @retval EFI_SUCCESS            The section stream is created successfully.
+  @retval EFI_OUT_OF_RESOURCES   memory allocation failed.
+  @retval EFI_INVALID_PARAMETER  Section stream does not end concident with end
+                                 of last section.
+
+**/
+EFI_STATUS
+EFIAPI
+OpenSectionStream (
+  IN     UINTN                                     SectionStreamLength,
+  IN     VOID                                      *SectionStream,
+     OUT UINTN                                     *SectionStreamHandle
+  )
+{
+  //
+  // Check to see section stream looks good...
+  //
+  if (!IsValidSectionStream (SectionStream, SectionStreamLength)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return OpenSectionStreamEx (
+           SectionStreamLength,
+           SectionStream,
+           FALSE,
+           0,
+           SectionStreamHandle
+           );
+}
+
+
+
+/**
+  Worker function.  Determine if the input stream:child matches the input type.
+
+  @param  Stream                 Indicates the section stream associated with the
+                                 child
+  @param  Child                  Indicates the child to check
+  @param  SearchType             Indicates the type of section to check against
+                                 for
+  @param  SectionDefinitionGuid  Indicates the GUID to check against if the type
+                                 is EFI_SECTION_GUID_DEFINED
+
+  @retval TRUE                   The child matches
+  @retval FALSE                  The child doesn't match
+
+**/
+BOOLEAN
+ChildIsType (
+  IN CORE_SECTION_STREAM_NODE *Stream,
+  IN CORE_SECTION_CHILD_NODE  *Child,
+  IN EFI_SECTION_TYPE         SearchType,
+  IN EFI_GUID                 *SectionDefinitionGuid
+  )
+{
+  EFI_GUID_DEFINED_SECTION    *GuidedSection;
+
+  if (SearchType == EFI_SECTION_ALL) {
+    return TRUE;
+  }
+  if (Child->Type != SearchType) {
+    return FALSE;
+  }
+  if ((SearchType != EFI_SECTION_GUID_DEFINED) || (SectionDefinitionGuid == NULL)) {
+    return TRUE;
+  }
+  GuidedSection = (EFI_GUID_DEFINED_SECTION * )(Stream->StreamBuffer + Child->OffsetInStream);
+  if (IS_SECTION2 (GuidedSection)) {
+    return CompareGuid (&(((EFI_GUID_DEFINED_SECTION2 *) GuidedSection)->SectionDefinitionGuid), SectionDefinitionGuid);
+  } else {
+    return CompareGuid (&GuidedSection->SectionDefinitionGuid, SectionDefinitionGuid);
+  }
+}
+
+/**
+  Verify the Guided Section GUID by checking if there is the Guided Section GUID configuration table recorded the GUID itself.
+
+  @param GuidedSectionGuid          The Guided Section GUID.
+  @param GuidedSectionExtraction    A pointer to the pointer to the supported Guided Section Extraction Protocol
+                                    for the Guided Section.
+
+  @return TRUE      The GuidedSectionGuid could be identified, and the pointer to
+                    the Guided Section Extraction Protocol will be returned to *GuidedSectionExtraction.
+  @return FALSE     The GuidedSectionGuid could not be identified, or 
+                    the Guided Section Extraction Protocol has not been installed yet.
+
+**/
+BOOLEAN
+VerifyGuidedSectionGuid (
+  IN  EFI_GUID                                  *GuidedSectionGuid,
+  OUT EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL    **GuidedSectionExtraction
+  )
+{
+  EFI_GUID              *GuidRecorded;
+  VOID                  *Interface;
+  EFI_STATUS            Status;
+
+  Interface = NULL;
+
+  //
+  // Check if there is the Guided Section GUID configuration table recorded the GUID itself.
+  //
+  Status = EfiGetSystemConfigurationTable (GuidedSectionGuid, (VOID **) &GuidRecorded);
+  if (Status == EFI_SUCCESS) {
+    if (CompareGuid (GuidRecorded, GuidedSectionGuid)) {
+      //
+      // Found the recorded GuidedSectionGuid.
+      //
+      Status = CoreLocateProtocol (GuidedSectionGuid, NULL, (VOID **) &Interface);
+      if (!EFI_ERROR (Status) && Interface != NULL) {
+        //
+        // Found the supported Guided Section Extraction Porotocol for the Guided Section.
+        //
+        *GuidedSectionExtraction = (EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL *) Interface;
+        return TRUE;
+      }
+      return FALSE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  RPN callback function. Initializes the section stream
+  when GUIDED_SECTION_EXTRACTION_PROTOCOL is installed.
+
+  @param Event               The event that fired
+  @param RpnContext          A pointer to the context that allows us to identify
+                             the relevent encapsulation.
+**/
+VOID
+EFIAPI
+NotifyGuidedExtraction (
+  IN   EFI_EVENT   Event,
+  IN   VOID        *RpnContext
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_GUID_DEFINED_SECTION                *GuidedHeader;
+  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL  *GuidedExtraction;
+  VOID                                    *NewStreamBuffer;
+  UINTN                                   NewStreamBufferSize;
+  UINT32                                  AuthenticationStatus;
+  RPN_EVENT_CONTEXT                       *Context;
+  
+  Context = RpnContext;
+  
+  GuidedHeader = (EFI_GUID_DEFINED_SECTION *) (Context->ParentStream->StreamBuffer + Context->ChildNode->OffsetInStream);
+  ASSERT (GuidedHeader->CommonHeader.Type == EFI_SECTION_GUID_DEFINED);
+
+  if (!VerifyGuidedSectionGuid (Context->ChildNode->EncapsulationGuid, &GuidedExtraction)) {
+    return;
+  }
+
+  Status = GuidedExtraction->ExtractSection (
+                               GuidedExtraction,
+                               GuidedHeader,
+                               &NewStreamBuffer,
+                               &NewStreamBufferSize,
+                               &AuthenticationStatus
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Make sure we initialize the new stream with the correct
+  // authentication status for both aggregate and local status fields.
+  //
+  if ((GuidedHeader->Attributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID) != 0) {
+    //
+    // OR in the parent stream's aggregate status.
+    //
+    AuthenticationStatus |= Context->ParentStream->AuthenticationStatus & EFI_AUTH_STATUS_ALL;
+  } else {
+    //
+    // since there's no authentication data contributed by the section,
+    // just inherit the full value from our immediate parent.
+    //
+    AuthenticationStatus = Context->ParentStream->AuthenticationStatus;
+  }
+
+  Status = OpenSectionStreamEx (
+             NewStreamBufferSize,
+             NewStreamBuffer,
+             FALSE,
+             AuthenticationStatus,
+             &Context->ChildNode->EncapsulatedStreamHandle
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  //  Close the event when done.
+  //
+  gBS->CloseEvent (Event);
+  Context->ChildNode->Event = NULL;
+  FreePool (Context);
+}  
+
+/**
+  Constructor for RPN event when a missing GUIDED_SECTION_EXTRACTION_PROTOCOL appears...
+
+  @param ParentStream        Indicates the parent of the ecnapsulation section (child)
+  @param ChildNode           Indicates the child node that is the encapsulation section.
+
+**/
+VOID
+CreateGuidedExtractionRpnEvent (
+  IN CORE_SECTION_STREAM_NODE      *ParentStream,
+  IN CORE_SECTION_CHILD_NODE       *ChildNode
+  )
+{
+  RPN_EVENT_CONTEXT *Context;
+  
+  //
+  // Allocate new event structure and context
+  //
+  Context = AllocatePool (sizeof (RPN_EVENT_CONTEXT));
+  ASSERT (Context != NULL);
+  
+  Context->ChildNode = ChildNode;
+  Context->ParentStream = ParentStream;
+ 
+  Context->ChildNode->Event = EfiCreateProtocolNotifyEvent (
+                                Context->ChildNode->EncapsulationGuid,
+                                TPL_NOTIFY,
+                                NotifyGuidedExtraction,
+                                Context,
+                                &Context->Registration
+                                );
+}
+
+/**
+  Worker function.  Constructor for new child nodes.
+
+  @param  Stream                 Indicates the section stream in which to add the
+                                 child.
+  @param  ChildOffset            Indicates the offset in Stream that is the
+                                 beginning of the child section.
+  @param  ChildNode              Indicates the Callee allocated and initialized
+                                 child.
+
+  @retval EFI_SUCCESS            Child node was found and returned.
+                                 EFI_OUT_OF_RESOURCES- Memory allocation failed.
+  @retval EFI_PROTOCOL_ERROR     Encapsulation sections produce new stream
+                                 handles when the child node is created.  If the
+                                 section type is GUID defined, and the extraction
+                                 GUID does not exist, and producing the stream
+                                 requires the GUID, then a protocol error is
+                                 generated and no child is produced. Values
+                                 returned by OpenSectionStreamEx.
+
+**/
+EFI_STATUS
+CreateChildNode (
+  IN     CORE_SECTION_STREAM_NODE              *Stream,
+  IN     UINT32                                ChildOffset,
+  OUT    CORE_SECTION_CHILD_NODE               **ChildNode
+  )
+{
+  EFI_STATUS                                   Status;
+  EFI_COMMON_SECTION_HEADER                    *SectionHeader;
+  EFI_COMPRESSION_SECTION                      *CompressionHeader;
+  EFI_GUID_DEFINED_SECTION                     *GuidedHeader;
+  EFI_DECOMPRESS_PROTOCOL                      *Decompress;
+  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL       *GuidedExtraction;
+  VOID                                         *NewStreamBuffer;
+  VOID                                         *ScratchBuffer;
+  UINT32                                       ScratchSize;
+  UINTN                                        NewStreamBufferSize;
+  UINT32                                       AuthenticationStatus;
+  VOID                                         *CompressionSource;
+  UINT32                                       CompressionSourceSize;
+  UINT32                                       UncompressedLength;
+  UINT8                                        CompressionType;
+  UINT16                                       GuidedSectionAttributes;
+
+  CORE_SECTION_CHILD_NODE                      *Node;
+
+  SectionHeader = (EFI_COMMON_SECTION_HEADER *) (Stream->StreamBuffer + ChildOffset);
+
+  //
+  // Allocate a new node
+  //
+  *ChildNode = AllocateZeroPool (sizeof (CORE_SECTION_CHILD_NODE));
+  Node = *ChildNode;
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Now initialize it
+  //
+  Node->Signature = CORE_SECTION_CHILD_SIGNATURE;
+  Node->Type = SectionHeader->Type;
+  if (IS_SECTION2 (SectionHeader)) {
+    Node->Size = SECTION2_SIZE (SectionHeader);
+  } else {
+    Node->Size = SECTION_SIZE (SectionHeader);
+  }
+  Node->OffsetInStream = ChildOffset;
+  Node->EncapsulatedStreamHandle = NULL_STREAM_HANDLE;
+  Node->EncapsulationGuid = NULL;
+
+  //
+  // If it's an encapsulating section, then create the new section stream also
+  //
+  switch (Node->Type) {
+    case EFI_SECTION_COMPRESSION:
+      //
+      // Get the CompressionSectionHeader
+      //
+      if (Node->Size < sizeof (EFI_COMPRESSION_SECTION)) {
+        CoreFreePool (Node);
+        return EFI_NOT_FOUND;
+      }
+
+      CompressionHeader = (EFI_COMPRESSION_SECTION *) SectionHeader;
+
+      if (IS_SECTION2 (CompressionHeader)) {
+        CompressionSource = (VOID *) ((UINT8 *) CompressionHeader + sizeof (EFI_COMPRESSION_SECTION2));
+        CompressionSourceSize = (UINT32) (SECTION2_SIZE (CompressionHeader) - sizeof (EFI_COMPRESSION_SECTION2));
+        UncompressedLength = ((EFI_COMPRESSION_SECTION2 *) CompressionHeader)->UncompressedLength;
+        CompressionType = ((EFI_COMPRESSION_SECTION2 *) CompressionHeader)->CompressionType;
+      } else {
+        CompressionSource = (VOID *) ((UINT8 *) CompressionHeader + sizeof (EFI_COMPRESSION_SECTION));
+        CompressionSourceSize = (UINT32) (SECTION_SIZE (CompressionHeader) - sizeof (EFI_COMPRESSION_SECTION));
+        UncompressedLength = CompressionHeader->UncompressedLength;
+        CompressionType = CompressionHeader->CompressionType;
+      }
+
+      //
+      // Allocate space for the new stream
+      //
+      if (UncompressedLength > 0) {
+        NewStreamBufferSize = UncompressedLength;
+        NewStreamBuffer = AllocatePool (NewStreamBufferSize);
+        if (NewStreamBuffer == NULL) {
+          CoreFreePool (Node);
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        if (CompressionType == EFI_NOT_COMPRESSED) {
+          //
+          // stream is not actually compressed, just encapsulated.  So just copy it.
+          //
+          CopyMem (NewStreamBuffer, CompressionSource, NewStreamBufferSize);
+        } else if (CompressionType == EFI_STANDARD_COMPRESSION) {
+          //
+          // Only support the EFI_SATNDARD_COMPRESSION algorithm.
+          //
+
+          //
+          // Decompress the stream
+          //
+          Status = CoreLocateProtocol (&gEfiDecompressProtocolGuid, NULL, (VOID **)&Decompress);
+          ASSERT_EFI_ERROR (Status);
+          ASSERT (Decompress != NULL);
+
+          Status = Decompress->GetInfo (
+                                 Decompress,
+                                 CompressionSource,
+                                 CompressionSourceSize,
+                                 (UINT32 *)&NewStreamBufferSize,
+                                 &ScratchSize
+                                 );
+          if (EFI_ERROR (Status) || (NewStreamBufferSize != UncompressedLength)) {
+            CoreFreePool (Node);
+            CoreFreePool (NewStreamBuffer);
+            if (!EFI_ERROR (Status)) {
+              Status = EFI_BAD_BUFFER_SIZE;
+            }
+            return Status;
+          }
+
+          ScratchBuffer = AllocatePool (ScratchSize);
+          if (ScratchBuffer == NULL) {
+            CoreFreePool (Node);
+            CoreFreePool (NewStreamBuffer);
+            return EFI_OUT_OF_RESOURCES;
+          }
+
+          Status = Decompress->Decompress (
+                                 Decompress,
+                                 CompressionSource,
+                                 CompressionSourceSize,
+                                 NewStreamBuffer,
+                                 (UINT32)NewStreamBufferSize,
+                                 ScratchBuffer,
+                                 ScratchSize
+                                 );
+          CoreFreePool (ScratchBuffer);
+          if (EFI_ERROR (Status)) {
+            CoreFreePool (Node);
+            CoreFreePool (NewStreamBuffer);
+            return Status;
+          }
+        }
+      } else {
+        NewStreamBuffer = NULL;
+        NewStreamBufferSize = 0;
+      }
+
+      Status = OpenSectionStreamEx (
+                 NewStreamBufferSize,
+                 NewStreamBuffer,
+                 FALSE,
+                 Stream->AuthenticationStatus,
+                 &Node->EncapsulatedStreamHandle
+                 );
+      if (EFI_ERROR (Status)) {
+        CoreFreePool (Node);
+        CoreFreePool (NewStreamBuffer);
+        return Status;
+      }
+      break;
+
+    case EFI_SECTION_GUID_DEFINED:
+      GuidedHeader = (EFI_GUID_DEFINED_SECTION *) SectionHeader;
+      if (IS_SECTION2 (GuidedHeader)) {
+        Node->EncapsulationGuid = &(((EFI_GUID_DEFINED_SECTION2 *) GuidedHeader)->SectionDefinitionGuid);
+        GuidedSectionAttributes = ((EFI_GUID_DEFINED_SECTION2 *) GuidedHeader)->Attributes;
+      } else {
+        Node->EncapsulationGuid = &GuidedHeader->SectionDefinitionGuid;
+        GuidedSectionAttributes = GuidedHeader->Attributes;
+      }
+      if (VerifyGuidedSectionGuid (Node->EncapsulationGuid, &GuidedExtraction)) {
+        //
+        // NewStreamBuffer is always allocated by ExtractSection... No caller
+        // allocation here.
+        //
+        Status = GuidedExtraction->ExtractSection (
+                                     GuidedExtraction,
+                                     GuidedHeader,
+                                     &NewStreamBuffer,
+                                     &NewStreamBufferSize,
+                                     &AuthenticationStatus
+                                     );
+        if (EFI_ERROR (Status)) {
+          CoreFreePool (*ChildNode);
+          return EFI_PROTOCOL_ERROR;
+        }
+
+        //
+        // Make sure we initialize the new stream with the correct
+        // authentication status for both aggregate and local status fields.
+        //
+        if ((GuidedSectionAttributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID) != 0) {
+          //
+          // OR in the parent stream's aggregate status.
+          //
+          AuthenticationStatus |= Stream->AuthenticationStatus & EFI_AUTH_STATUS_ALL;
+        } else {
+          //
+          // since there's no authentication data contributed by the section,
+          // just inherit the full value from our immediate parent.
+          //
+          AuthenticationStatus = Stream->AuthenticationStatus;
+        }
+
+        Status = OpenSectionStreamEx (
+                   NewStreamBufferSize,
+                   NewStreamBuffer,
+                   FALSE,
+                   AuthenticationStatus,
+                   &Node->EncapsulatedStreamHandle
+                   );
+        if (EFI_ERROR (Status)) {
+          CoreFreePool (*ChildNode);
+          CoreFreePool (NewStreamBuffer);
+          return Status;
+        }
+      } else {
+        //
+        // There's no GUIDed section extraction protocol available.
+        //
+        if ((GuidedSectionAttributes & EFI_GUIDED_SECTION_PROCESSING_REQUIRED) != 0) {
+          //
+          // If the section REQUIRES an extraction protocol, register for RPN 
+          // when the required GUIDed extraction protocol becomes available. 
+          //
+          CreateGuidedExtractionRpnEvent (Stream, Node);
+        } else {
+          //
+          // Figure out the proper authentication status
+          //
+          AuthenticationStatus = Stream->AuthenticationStatus;
+
+          if ((GuidedSectionAttributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID) == EFI_GUIDED_SECTION_AUTH_STATUS_VALID) {
+            AuthenticationStatus |= EFI_AUTH_STATUS_IMAGE_SIGNED | EFI_AUTH_STATUS_NOT_TESTED;
+          }
+
+          if (IS_SECTION2 (GuidedHeader)) {
+            Status = OpenSectionStreamEx (
+                       SECTION2_SIZE (GuidedHeader) - ((EFI_GUID_DEFINED_SECTION2 *) GuidedHeader)->DataOffset,
+                       (UINT8 *) GuidedHeader + ((EFI_GUID_DEFINED_SECTION2 *) GuidedHeader)->DataOffset,
+                       TRUE,
+                       AuthenticationStatus,
+                       &Node->EncapsulatedStreamHandle
+                       );
+          } else {
+            Status = OpenSectionStreamEx (
+                       SECTION_SIZE (GuidedHeader) - ((EFI_GUID_DEFINED_SECTION *) GuidedHeader)->DataOffset,
+                       (UINT8 *) GuidedHeader + ((EFI_GUID_DEFINED_SECTION *) GuidedHeader)->DataOffset,
+                       TRUE,
+                       AuthenticationStatus,
+                       &Node->EncapsulatedStreamHandle
+                       );
+          }
+          if (EFI_ERROR (Status)) {
+            CoreFreePool (Node);
+            return Status;
+          }
+        }
+      }
+
+      break;
+
+    default:
+
+      //
+      // Nothing to do if it's a leaf
+      //
+      break;
+  }
+
+  //
+  // Last, add the new child node to the stream
+  //
+  InsertTailList (&Stream->Children, &Node->Link);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Worker function  Recursively searches / builds section stream database
+  looking for requested section.
+
+  @param  SourceStream           Indicates the section stream in which to do the
+                                 search.
+  @param  SearchType             Indicates the type of section to search for.
+  @param  SectionInstance        Indicates which instance of section to find.
+                                 This is an in/out parameter to deal with
+                                 recursions.
+  @param  SectionDefinitionGuid  Guid of section definition
+  @param  FoundChild             Output indicating the child node that is found.
+  @param  FoundStream            Output indicating which section stream the child
+                                 was found in.  If this stream was generated as a
+                                 result of an encapsulation section, the
+                                 streamhandle is visible within the SEP driver
+                                 only.
+  @param  AuthenticationStatus   Indicates the authentication status of the found section.
+
+  @retval EFI_SUCCESS            Child node was found and returned.
+                                 EFI_OUT_OF_RESOURCES- Memory allocation failed.
+  @retval EFI_NOT_FOUND          Requested child node does not exist.
+  @retval EFI_PROTOCOL_ERROR     a required GUIDED section extraction protocol
+                                 does not exist
+
+**/
+EFI_STATUS
+FindChildNode (
+  IN     CORE_SECTION_STREAM_NODE                   *SourceStream,
+  IN     EFI_SECTION_TYPE                           SearchType,
+  IN OUT UINTN                                      *SectionInstance,
+  IN     EFI_GUID                                   *SectionDefinitionGuid,
+  OUT    CORE_SECTION_CHILD_NODE                    **FoundChild,
+  OUT    CORE_SECTION_STREAM_NODE                   **FoundStream,
+  OUT    UINT32                                     *AuthenticationStatus
+  )
+{
+  CORE_SECTION_CHILD_NODE                       *CurrentChildNode;
+  CORE_SECTION_CHILD_NODE                       *RecursedChildNode;
+  CORE_SECTION_STREAM_NODE                      *RecursedFoundStream;
+  UINT32                                        NextChildOffset;
+  EFI_STATUS                                    ErrorStatus;
+  EFI_STATUS                                    Status;
+
+  CurrentChildNode = NULL;
+  ErrorStatus = EFI_NOT_FOUND;
+
+  if (SourceStream->StreamLength == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (IsListEmpty (&SourceStream->Children) &&
+      SourceStream->StreamLength >= sizeof (EFI_COMMON_SECTION_HEADER)) {
+    //
+    // This occurs when a section stream exists, but no child sections
+    // have been parsed out yet.  Therefore, extract the first child and add it
+    // to the list of children so we can get started.
+    // Section stream may contain an array of zero or more bytes.
+    // So, its size should be >= the size of commen section header.
+    //
+    Status = CreateChildNode (SourceStream, 0, &CurrentChildNode);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // At least one child has been parsed out of the section stream.  So, walk
+  // through the sections that have already been parsed out looking for the
+  // requested section, if necessary, continue parsing section stream and
+  // adding children until either the requested section is found, or we run
+  // out of data
+  //
+  CurrentChildNode = CHILD_SECTION_NODE_FROM_LINK (GetFirstNode(&SourceStream->Children));
+
+  for (;;) {
+    ASSERT (CurrentChildNode != NULL);
+    if (ChildIsType (SourceStream, CurrentChildNode, SearchType, SectionDefinitionGuid)) {
+      //
+      // The type matches, so check the instance count to see if it's the one we want
+      //
+      (*SectionInstance)--;
+      if (*SectionInstance == 0) {
+        //
+        // Got it!
+        //
+        *FoundChild = CurrentChildNode;
+        *FoundStream = SourceStream;
+        *AuthenticationStatus = SourceStream->AuthenticationStatus;
+        return EFI_SUCCESS;
+      }
+    }
+
+    if (CurrentChildNode->EncapsulatedStreamHandle != NULL_STREAM_HANDLE) {
+      //
+      // If the current node is an encapsulating node, recurse into it...
+      //
+      Status = FindChildNode (
+                (CORE_SECTION_STREAM_NODE *)CurrentChildNode->EncapsulatedStreamHandle,
+                SearchType,
+                SectionInstance,
+                SectionDefinitionGuid,
+                &RecursedChildNode,
+                &RecursedFoundStream,
+                AuthenticationStatus
+                );
+      //
+      // If the status is not EFI_SUCCESS, just save the error code and continue
+      // to find the request child node in the rest stream.
+      //
+      if (*SectionInstance == 0) {
+        ASSERT_EFI_ERROR (Status);
+        *FoundChild = RecursedChildNode;
+        *FoundStream = RecursedFoundStream;
+        return EFI_SUCCESS;
+      } else {
+        ErrorStatus = Status;
+      }
+    } else if ((CurrentChildNode->Type == EFI_SECTION_GUID_DEFINED) && (SearchType != EFI_SECTION_GUID_DEFINED)) {
+      //
+      // When Node Type is GUIDED section, but Node has no encapsulated data, Node data should not be parsed
+      // because a required GUIDED section extraction protocol does not exist.
+      // If SearchType is not GUIDED section, EFI_PROTOCOL_ERROR should return.
+      //
+      ErrorStatus = EFI_PROTOCOL_ERROR;
+    }
+
+    if (!IsNodeAtEnd (&SourceStream->Children, &CurrentChildNode->Link)) {
+      //
+      // We haven't found the child node we're interested in yet, but there's
+      // still more nodes that have already been parsed so get the next one
+      // and continue searching..
+      //
+      CurrentChildNode = CHILD_SECTION_NODE_FROM_LINK (GetNextNode (&SourceStream->Children, &CurrentChildNode->Link));
+    } else {
+      //
+      // We've exhausted children that have already been parsed, so see if
+      // there's any more data and continue parsing out more children if there
+      // is.
+      //
+      NextChildOffset = CurrentChildNode->OffsetInStream + CurrentChildNode->Size;
+      //
+      // Round up to 4 byte boundary
+      //
+      NextChildOffset += 3;
+      NextChildOffset &= ~(UINTN) 3;
+      if (NextChildOffset <= SourceStream->StreamLength - sizeof (EFI_COMMON_SECTION_HEADER)) {
+        //
+        // There's an unparsed child remaining in the stream, so create a new child node
+        //
+        Status = CreateChildNode (SourceStream, NextChildOffset, &CurrentChildNode);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+      } else {
+        ASSERT (EFI_ERROR (ErrorStatus));
+        return ErrorStatus;
+      }
+    }
+  }
+}
+
+
+/**
+  Worker function.  Search stream database for requested stream handle.
+
+  @param  SearchHandle           Indicates which stream to look for.
+  @param  FoundStream            Output pointer to the found stream.
+
+  @retval EFI_SUCCESS            StreamHandle was found and *FoundStream contains
+                                 the stream node.
+  @retval EFI_NOT_FOUND          SearchHandle was not found in the stream
+                                 database.
+
+**/
+EFI_STATUS
+FindStreamNode (
+  IN  UINTN                                     SearchHandle,
+  OUT CORE_SECTION_STREAM_NODE                  **FoundStream
+  )
+{
+  CORE_SECTION_STREAM_NODE                      *StreamNode;
+
+  if (!IsListEmpty (&mStreamRoot)) {
+    StreamNode = STREAM_NODE_FROM_LINK (GetFirstNode (&mStreamRoot));
+    for (;;) {
+      if (StreamNode->StreamHandle == SearchHandle) {
+        *FoundStream = StreamNode;
+        return EFI_SUCCESS;
+      } else if (IsNodeAtEnd (&mStreamRoot, &StreamNode->Link)) {
+        break;
+      } else {
+        StreamNode = STREAM_NODE_FROM_LINK (GetNextNode (&mStreamRoot, &StreamNode->Link));
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  SEP member function.  Retrieves requested section from section stream.
+
+  @param  SectionStreamHandle   The section stream from which to extract the
+                                requested section.
+  @param  SectionType           A pointer to the type of section to search for.
+  @param  SectionDefinitionGuid If the section type is EFI_SECTION_GUID_DEFINED,
+                                then SectionDefinitionGuid indicates which of
+                                these types of sections to search for.
+  @param  SectionInstance       Indicates which instance of the requested
+                                section to return.
+  @param  Buffer                Double indirection to buffer.  If *Buffer is
+                                non-null on input, then the buffer is caller
+                                allocated.  If Buffer is NULL, then the buffer
+                                is callee allocated.  In either case, the
+                                requried buffer size is returned in *BufferSize.
+  @param  BufferSize            On input, indicates the size of *Buffer if
+                                *Buffer is non-null on input.  On output,
+                                indicates the required size (allocated size if
+                                callee allocated) of *Buffer.
+  @param  AuthenticationStatus  A pointer to a caller-allocated UINT32 that
+                                indicates the authentication status of the
+                                output buffer. If the input section's
+                                GuidedSectionHeader.Attributes field
+                                has the EFI_GUIDED_SECTION_AUTH_STATUS_VALID
+                                bit as clear, AuthenticationStatus must return
+                                zero. Both local bits (19:16) and aggregate
+                                bits (3:0) in AuthenticationStatus are returned
+                                by ExtractSection(). These bits reflect the
+                                status of the extraction operation. The bit
+                                pattern in both regions must be the same, as
+                                the local and aggregate authentication statuses
+                                have equivalent meaning at this level. If the
+                                function returns anything other than
+                                EFI_SUCCESS, the value of *AuthenticationStatus
+                                is undefined.
+  @param  IsFfs3Fv              Indicates the FV format.
+
+  @retval EFI_SUCCESS           Section was retrieved successfully
+  @retval EFI_PROTOCOL_ERROR    A GUID defined section was encountered in the
+                                section stream with its
+                                EFI_GUIDED_SECTION_PROCESSING_REQUIRED bit set,
+                                but there was no corresponding GUIDed Section
+                                Extraction Protocol in the handle database.
+                                *Buffer is unmodified.
+  @retval EFI_NOT_FOUND         An error was encountered when parsing the
+                                SectionStream.  This indicates the SectionStream
+                                is not correctly formatted.
+  @retval EFI_NOT_FOUND         The requested section does not exist.
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient resources to process
+                                the request.
+  @retval EFI_INVALID_PARAMETER The SectionStreamHandle does not exist.
+  @retval EFI_WARN_TOO_SMALL    The size of the caller allocated input buffer is
+                                insufficient to contain the requested section.
+                                The input buffer is filled and section contents
+                                are truncated.
+
+**/
+EFI_STATUS
+EFIAPI
+GetSection (
+  IN UINTN                                              SectionStreamHandle,
+  IN EFI_SECTION_TYPE                                   *SectionType,
+  IN EFI_GUID                                           *SectionDefinitionGuid,
+  IN UINTN                                              SectionInstance,
+  IN VOID                                               **Buffer,
+  IN OUT UINTN                                          *BufferSize,
+  OUT UINT32                                            *AuthenticationStatus,
+  IN BOOLEAN                                            IsFfs3Fv
+  )
+{
+  CORE_SECTION_STREAM_NODE                              *StreamNode;
+  EFI_TPL                                               OldTpl;
+  EFI_STATUS                                            Status;
+  CORE_SECTION_CHILD_NODE                               *ChildNode;
+  CORE_SECTION_STREAM_NODE                              *ChildStreamNode;
+  UINTN                                                 CopySize;
+  UINT32                                                ExtractedAuthenticationStatus;
+  UINTN                                                 Instance;
+  UINT8                                                 *CopyBuffer;
+  UINTN                                                 SectionSize;
+  EFI_COMMON_SECTION_HEADER                             *Section;
+
+
+  ChildStreamNode = NULL;
+  OldTpl = CoreRaiseTpl (TPL_NOTIFY);
+  Instance = SectionInstance + 1;
+
+  //
+  // Locate target stream
+  //
+  Status = FindStreamNode (SectionStreamHandle, &StreamNode);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto GetSection_Done;
+  }
+
+  //
+  // Found the stream, now locate and return the appropriate section
+  //
+  if (SectionType == NULL) {
+    //
+    // SectionType == NULL means return the WHOLE section stream...
+    //
+    CopySize = StreamNode->StreamLength;
+    CopyBuffer = StreamNode->StreamBuffer;
+    *AuthenticationStatus = StreamNode->AuthenticationStatus;
+  } else {
+    //
+    // There's a requested section type, so go find it and return it...
+    //
+    Status = FindChildNode (
+               StreamNode,
+               *SectionType,
+               &Instance,
+               SectionDefinitionGuid,
+               &ChildNode,
+               &ChildStreamNode,
+               &ExtractedAuthenticationStatus
+               );
+    if (EFI_ERROR (Status)) {
+      goto GetSection_Done;
+    }
+
+    Section = (EFI_COMMON_SECTION_HEADER *) (ChildStreamNode->StreamBuffer + ChildNode->OffsetInStream);
+
+    if (IS_SECTION2 (Section)) {
+      ASSERT (SECTION2_SIZE (Section) > 0x00FFFFFF);
+      if (!IsFfs3Fv) {
+        DEBUG ((DEBUG_ERROR, "It is a FFS3 formatted section in a non-FFS3 formatted FV.\n"));
+        Status = EFI_NOT_FOUND;
+        goto GetSection_Done;
+      }
+      CopySize = SECTION2_SIZE (Section) - sizeof (EFI_COMMON_SECTION_HEADER2);
+      CopyBuffer = (UINT8 *) Section + sizeof (EFI_COMMON_SECTION_HEADER2);
+    } else {
+      CopySize = SECTION_SIZE (Section) - sizeof (EFI_COMMON_SECTION_HEADER);
+      CopyBuffer = (UINT8 *) Section + sizeof (EFI_COMMON_SECTION_HEADER);
+    }
+    *AuthenticationStatus = ExtractedAuthenticationStatus;
+  }
+
+  SectionSize = CopySize;
+  if (*Buffer != NULL) {
+    //
+    // Caller allocated buffer.  Fill to size and return required size...
+    //
+    if (*BufferSize < CopySize) {
+      Status = EFI_WARN_BUFFER_TOO_SMALL;
+      CopySize = *BufferSize;
+    }
+  } else {
+    //
+    // Callee allocated buffer.  Allocate buffer and return size.
+    //
+    *Buffer = AllocatePool (CopySize);
+    if (*Buffer == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto GetSection_Done;
+    }
+  }
+  CopyMem (*Buffer, CopyBuffer, CopySize);
+  *BufferSize = SectionSize;
+
+GetSection_Done:
+  CoreRestoreTpl (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Worker function.  Destructor for child nodes.
+
+  @param  ChildNode              Indicates the node to destroy
+
+**/
+VOID
+FreeChildNode (
+  IN  CORE_SECTION_CHILD_NODE                   *ChildNode
+  )
+{
+  ASSERT (ChildNode->Signature == CORE_SECTION_CHILD_SIGNATURE);
+  //
+  // Remove the child from it's list
+  //
+  RemoveEntryList (&ChildNode->Link);
+
+  if (ChildNode->EncapsulatedStreamHandle != NULL_STREAM_HANDLE) {
+    //
+    // If it's an encapsulating section, we close the resulting section stream.
+    // CloseSectionStream will free all memory associated with the stream.
+    //
+    CloseSectionStream (ChildNode->EncapsulatedStreamHandle, TRUE);
+  }
+
+  if (ChildNode->Event != NULL) {
+    gBS->CloseEvent (ChildNode->Event);
+  }
+
+  //
+  // Last, free the child node itself
+  //
+  CoreFreePool (ChildNode);
+}
+
+
+/**
+  SEP member function.  Deletes an existing section stream
+
+  @param  StreamHandleToClose    Indicates the stream to close
+  @param  FreeStreamBuffer       TRUE - Need to free stream buffer;
+                                 FALSE - No need to free stream buffer.
+
+  @retval EFI_SUCCESS            The section stream is closed sucessfully.
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_INVALID_PARAMETER  Section stream does not end concident with end
+                                 of last section.
+
+**/
+EFI_STATUS
+EFIAPI
+CloseSectionStream (
+  IN  UINTN                                     StreamHandleToClose,
+  IN  BOOLEAN                                   FreeStreamBuffer
+  )
+{
+  CORE_SECTION_STREAM_NODE                      *StreamNode;
+  EFI_TPL                                       OldTpl;
+  EFI_STATUS                                    Status;
+  LIST_ENTRY                                    *Link;
+  CORE_SECTION_CHILD_NODE                       *ChildNode;
+
+  OldTpl = CoreRaiseTpl (TPL_NOTIFY);
+
+  //
+  // Locate target stream
+  //
+  Status = FindStreamNode (StreamHandleToClose, &StreamNode);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Found the stream, so close it
+    //
+    RemoveEntryList (&StreamNode->Link);
+    while (!IsListEmpty (&StreamNode->Children)) {
+      Link = GetFirstNode (&StreamNode->Children);
+      ChildNode = CHILD_SECTION_NODE_FROM_LINK (Link);
+      FreeChildNode (ChildNode);
+    }
+    if (FreeStreamBuffer) {
+      CoreFreePool (StreamNode->StreamBuffer);
+    }
+    CoreFreePool (StreamNode);
+    Status = EFI_SUCCESS;
+  } else {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  CoreRestoreTpl (OldTpl);
+  return Status;
+}
+
+
+/**
+  The ExtractSection() function processes the input section and
+  allocates a buffer from the pool in which it returns the section
+  contents. If the section being extracted contains
+  authentication information (the section's
+  GuidedSectionHeader.Attributes field has the
+  EFI_GUIDED_SECTION_AUTH_STATUS_VALID bit set), the values
+  returned in AuthenticationStatus must reflect the results of
+  the authentication operation. Depending on the algorithm and
+  size of the encapsulated data, the time that is required to do
+  a full authentication may be prohibitively long for some
+  classes of systems. To indicate this, use
+  EFI_SECURITY_POLICY_PROTOCOL_GUID, which may be published by
+  the security policy driver (see the Platform Initialization
+  Driver Execution Environment Core Interface Specification for
+  more details and the GUID definition). If the
+  EFI_SECURITY_POLICY_PROTOCOL_GUID exists in the handle
+  database, then, if possible, full authentication should be
+  skipped and the section contents simply returned in the
+  OutputBuffer. In this case, the
+  EFI_AUTH_STATUS_PLATFORM_OVERRIDE bit AuthenticationStatus
+  must be set on return. ExtractSection() is callable only from
+  TPL_NOTIFY and below. Behavior of ExtractSection() at any
+  EFI_TPL above TPL_NOTIFY is undefined. Type EFI_TPL is
+  defined in RaiseTPL() in the UEFI 2.0 specification.
+
+
+  @param This         Indicates the
+                      EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL instance.
+  @param InputSection Buffer containing the input GUIDed section
+                      to be processed. OutputBuffer OutputBuffer
+                      is allocated from boot services pool
+                      memory and contains the new section
+                      stream. The caller is responsible for
+                      freeing this buffer.
+  @param OutputBuffer *OutputBuffer is allocated from boot services
+                      pool memory and contains the new section stream.
+                      The caller is responsible for freeing this buffer.
+  @param OutputSize   A pointer to a caller-allocated UINTN in
+                      which the size of OutputBuffer allocation
+                      is stored. If the function returns
+                      anything other than EFI_SUCCESS, the value
+                      of OutputSize is undefined.
+
+  @param AuthenticationStatus A pointer to a caller-allocated
+                              UINT32 that indicates the
+                              authentication status of the
+                              output buffer. If the input
+                              section's
+                              GuidedSectionHeader.Attributes
+                              field has the
+                              EFI_GUIDED_SECTION_AUTH_STATUS_VAL
+                              bit as clear, AuthenticationStatus
+                              must return zero. Both local bits
+                              (19:16) and aggregate bits (3:0)
+                              in AuthenticationStatus are
+                              returned by ExtractSection().
+                              These bits reflect the status of
+                              the extraction operation. The bit
+                              pattern in both regions must be
+                              the same, as the local and
+                              aggregate authentication statuses
+                              have equivalent meaning at this
+                              level. If the function returns
+                              anything other than EFI_SUCCESS,
+                              the value of AuthenticationStatus
+                              is undefined.
+
+
+  @retval EFI_SUCCESS          The InputSection was successfully
+                               processed and the section contents were
+                               returned.
+
+  @retval EFI_OUT_OF_RESOURCES The system has insufficient
+                               resources to process the
+                               request.
+
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction
+                                Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL *This,
+  IN CONST  VOID                                   *InputSection,
+  OUT       VOID                                   **OutputBuffer,
+  OUT       UINTN                                  *OutputSize,
+  OUT       UINT32                                 *AuthenticationStatus
+  )
+{
+  EFI_STATUS      Status;
+  VOID            *ScratchBuffer;
+  VOID            *AllocatedOutputBuffer;
+  UINT32          OutputBufferSize;
+  UINT32          ScratchBufferSize;
+  UINT16          SectionAttribute;
+
+  //
+  // Init local variable
+  //
+  ScratchBuffer         = NULL;
+  AllocatedOutputBuffer = NULL;
+
+  //
+  // Call GetInfo to get the size and attribute of input guided section data.
+  //
+  Status = ExtractGuidedSectionGetInfo (
+             InputSection,
+             &OutputBufferSize,
+             &ScratchBufferSize,
+             &SectionAttribute
+             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "GetInfo from guided section Failed - %r\n", Status));
+    return Status;
+  }
+
+  if (ScratchBufferSize > 0) {
+    //
+    // Allocate scratch buffer
+    //
+    ScratchBuffer = AllocatePool (ScratchBufferSize);
+    if (ScratchBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  if (OutputBufferSize > 0) {
+    //
+    // Allocate output buffer
+    //
+    AllocatedOutputBuffer = AllocatePool (OutputBufferSize);
+    if (AllocatedOutputBuffer == NULL) {
+      FreePool (ScratchBuffer);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    *OutputBuffer = AllocatedOutputBuffer;
+  }
+
+  //
+  // Call decode function to extract raw data from the guided section.
+  //
+  Status = ExtractGuidedSectionDecode (
+             InputSection,
+             OutputBuffer,
+             ScratchBuffer,
+             AuthenticationStatus
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // Decode failed
+    //
+    if (AllocatedOutputBuffer != NULL) {
+      CoreFreePool (AllocatedOutputBuffer);
+    }
+    if (ScratchBuffer != NULL) {
+      CoreFreePool (ScratchBuffer);
+    }
+    DEBUG ((DEBUG_ERROR, "Extract guided section Failed - %r\n", Status));
+    return Status;
+  }
+
+  if (*OutputBuffer != AllocatedOutputBuffer) {
+    //
+    // OutputBuffer was returned as a different value,
+    // so copy section contents to the allocated memory buffer.
+    //
+    CopyMem (AllocatedOutputBuffer, *OutputBuffer, OutputBufferSize);
+    *OutputBuffer = AllocatedOutputBuffer;
+  }
+
+  //
+  // Set real size of output buffer.
+  //
+  *OutputSize = (UINTN) OutputBufferSize;
+
+  //
+  // Free unused scratch buffer.
+  //
+  if (ScratchBuffer != NULL) {
+    CoreFreePool (ScratchBuffer);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Arm/DxeLoadFunc.c b/Core/MdeModulePkg/Core/DxeIplPeim/Arm/DxeLoadFunc.c
new file mode 100644
index 0000000000..df2dc70b17
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Arm/DxeLoadFunc.c
@@ -0,0 +1,70 @@
+/** @file
+  ARM specifc functionality for DxeLoad. 
+  
+Copyright (c) 2006 - 2008, Intel Corporation. All rights reserved.<BR>
+Portions copyright (c) 2008 - 2009, Apple Inc. 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 "DxeIpl.h"
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  )
+{
+  VOID                *BaseOfStack;
+  VOID                *TopOfStack;
+  EFI_STATUS          Status;
+
+  //
+  // Allocate 128KB for the Stack
+  //
+  BaseOfStack = AllocatePages (EFI_SIZE_TO_PAGES (STACK_SIZE));
+  ASSERT (BaseOfStack != NULL);
+
+  //
+  // Compute the top of the stack we were allocated. Pre-allocate a UINTN
+  // for safety.
+  //
+  TopOfStack = (VOID *) ((UINTN) BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - CPU_STACK_ALIGNMENT);
+  TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);
+
+  //
+  // End of PEI phase singal
+  //
+  Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+  //    
+  UpdateStackHob ((EFI_PHYSICAL_ADDRESS)(UINTN) BaseOfStack, STACK_SIZE);
+
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+    HobList.Raw,
+    NULL,
+    TopOfStack
+    );
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.h b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.h
new file mode 100644
index 0000000000..75372ace8d
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.h
@@ -0,0 +1,225 @@
+/** @file
+  Master header file for DxeIpl PEIM. All source files in this module should
+  include this file for common definitions.
+
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef __PEI_DXEIPL_H__
+#define __PEI_DXEIPL_H__
+
+#include <PiPei.h>
+#include <Ppi/DxeIpl.h>
+#include <Ppi/EndOfPeiPhase.h>
+#include <Ppi/MemoryDiscovered.h>
+#include <Ppi/ReadOnlyVariable2.h>
+#include <Ppi/Decompress.h>
+#include <Ppi/FirmwareVolumeInfo.h>
+#include <Ppi/GuidedSectionExtraction.h>
+#include <Ppi/LoadFile.h>
+#include <Ppi/S3Resume2.h>
+#include <Ppi/RecoveryModule.h>
+#include <Ppi/VectorHandoffInfo.h>
+
+#include <Guid/MemoryTypeInformation.h>
+#include <Guid/MemoryAllocationHob.h>
+#include <Guid/FirmwareFileSystem2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/HobLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/UefiDecompressLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/S3Lib.h>
+#include <Library/RecoveryLib.h>
+#include <Library/DebugAgentLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+
+#define STACK_SIZE      0x20000
+#define BSP_STORE_SIZE  0x4000
+
+
+//
+// This PPI is installed to indicate the end of the PEI usage of memory
+//
+extern CONST EFI_PEI_PPI_DESCRIPTOR gEndOfPeiSignalPpi;
+
+
+/**
+   Searches DxeCore in all firmware Volumes and loads the first
+   instance that contains DxeCore.
+
+   @return FileHandle of DxeCore to load DxeCore.
+
+**/
+EFI_PEI_FILE_HANDLE
+DxeIplFindDxeCore (
+  VOID
+  );
+
+
+/**
+   Main entry point to last PEIM
+
+   @param This          Entry point for DXE IPL PPI
+   @param PeiServices   General purpose services available to every PEIM.
+   @param HobList       Address to the Pei HOB list
+
+   @return EFI_SUCCESS              DXE core was successfully loaded.
+   @return EFI_OUT_OF_RESOURCES     There are not enough resources to load DXE core.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeLoadCore (
+  IN CONST EFI_DXE_IPL_PPI *This,
+  IN EFI_PEI_SERVICES      **PeiServices,
+  IN EFI_PEI_HOB_POINTERS  HobList
+  );
+
+
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  );
+
+
+
+/**
+   Updates the Stack HOB passed to DXE phase.
+
+   This function traverses the whole HOB list and update the stack HOB to
+   reflect the real stack that is used by DXE core.
+
+   @param BaseAddress           The lower address of stack used by DxeCore.
+   @param Length                The length of stack used by DxeCore.
+
+**/
+VOID
+UpdateStackHob (
+  IN EFI_PHYSICAL_ADDRESS        BaseAddress,
+  IN UINT64                      Length
+  );
+
+/**
+  The ExtractSection() function processes the input section and
+  returns a pointer to the section contents. If the section being
+  extracted does not require processing (if the section
+  GuidedSectionHeader.Attributes has the
+  EFI_GUIDED_SECTION_PROCESSING_REQUIRED field cleared), then
+  OutputBuffer is just updated to point to the start of the
+  section's contents. Otherwise, *Buffer must be allocated
+  from PEI permanent memory.
+
+  @param This                   Indicates the
+                                EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI instance.
+                                Buffer containing the input GUIDed section to be
+                                processed. OutputBuffer OutputBuffer is
+                                allocated from PEI permanent memory and contains
+                                the new section stream.
+  @param InputSection           A pointer to the input buffer, which contains
+                                the input section to be processed.
+  @param OutputBuffer           A pointer to a caller-allocated buffer, whose
+                                size is specified by the contents of OutputSize.
+  @param OutputSize             A pointer to a caller-allocated
+                                UINTN in which the size of *OutputBuffer
+                                allocation is stored. If the function
+                                returns anything other than EFI_SUCCESS,
+                                the value of OutputSize is undefined.
+  @param AuthenticationStatus   A pointer to a caller-allocated
+                                UINT32 that indicates the
+                                authentication status of the
+                                output buffer. If the input
+                                section's GuidedSectionHeader.
+                                Attributes field has the
+                                EFI_GUIDED_SECTION_AUTH_STATUS_VALID
+                                bit as clear,
+                                AuthenticationStatus must return
+                                zero. These bits reflect the
+                                status of the extraction
+                                operation. If the function
+                                returns anything other than
+                                EFI_SUCCESS, the value of
+                                AuthenticationStatus is
+                                undefined.
+
+  @retval EFI_SUCCESS           The InputSection was
+                                successfully processed and the
+                                section contents were returned.
+
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient
+                                resources to process the request.
+
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction PPI.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI *This,
+  IN CONST  VOID                                  *InputSection,
+  OUT       VOID                                  **OutputBuffer,
+  OUT       UINTN                                 *OutputSize,
+  OUT       UINT32                                *AuthenticationStatus
+  );
+
+
+/**
+   Decompresses a section to the output buffer.
+
+   This function looks up the compression type field in the input section and
+   applies the appropriate compression algorithm to compress the section to a
+   callee allocated buffer.
+
+   @param  This                  Points to this instance of the
+                                 EFI_PEI_DECOMPRESS_PEI PPI.
+   @param  CompressionSection    Points to the compressed section.
+   @param  OutputBuffer          Holds the returned pointer to the decompressed
+                                 sections.
+   @param  OutputSize            Holds the returned size of the decompress
+                                 section streams.
+
+   @retval EFI_SUCCESS           The section was decompressed successfully.
+                                 OutputBuffer contains the resulting data and
+                                 OutputSize contains the resulting size.
+
+**/
+EFI_STATUS
+EFIAPI
+Decompress (
+  IN CONST  EFI_PEI_DECOMPRESS_PPI  *This,
+  IN CONST  EFI_COMPRESSION_SECTION *CompressionSection,
+  OUT       VOID                    **OutputBuffer,
+  OUT       UINTN                   *OutputSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.inf b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.inf
new file mode 100644
index 0000000000..66c58b1d0f
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.inf
@@ -0,0 +1,128 @@
+## @file
+#  Last PEIM executed in PEI phase to load DXE Core from a Firmware Volume.
+#
+#  This module produces a special PPI named the DXE Initial Program Load (IPL)
+#  PPI to discover and dispatch the DXE Foundation and components that are
+#  needed to run the DXE Foundation.
+#
+#  Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeIpl
+  MODULE_UNI_FILE                = DxeIpl.uni
+  FILE_GUID                      = 86D70125-BAA3-4296-A62F-602BEBBB9081
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = PeimInitializeDxeIpl
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only) AARCH64
+#
+
+[Sources]
+  DxeIpl.h
+  DxeLoad.c
+
+[Sources.Ia32]
+  X64/VirtualMemory.h  ||||gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode
+  X64/VirtualMemory.c  ||||gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode
+  Ia32/DxeLoadFunc.c
+  Ia32/IdtVectorAsm.asm||||gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode
+  Ia32/IdtVectorAsm.S  ||||gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode
+
+[Sources.X64]
+  X64/VirtualMemory.h
+  X64/VirtualMemory.c
+  X64/DxeLoadFunc.c    
+
+[Sources.IPF]
+  Ipf/DxeLoadFunc.c
+
+[Sources.EBC]
+  Ebc/DxeLoadFunc.c
+
+[Sources.ARM, Sources.AARCH64]
+  Arm/DxeLoadFunc.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  ExtractGuidedSectionLib
+  UefiDecompressLib
+  ReportStatusCodeLib
+  PeiServicesLib
+  HobLib
+  BaseLib
+  PeimEntryPoint
+  DebugLib
+  DebugAgentLib
+  PeiServicesTablePointerLib
+
+[Ppis]
+  gEfiDxeIplPpiGuid                 ## PRODUCES
+  gEfiPeiDecompressPpiGuid          ## PRODUCES
+  gEfiEndOfPeiSignalPpiGuid         ## SOMETIMES_PRODUCES # Not produced on S3 boot path
+  gEfiPeiReadOnlyVariable2PpiGuid   ## SOMETIMES_CONSUMES
+  gEfiPeiLoadFilePpiGuid            ## SOMETIMES_CONSUMES
+  gEfiPeiS3Resume2PpiGuid           ## SOMETIMES_CONSUMES # Consumed on S3 boot path
+  gEfiPeiRecoveryModulePpiGuid      ## SOMETIMES_CONSUMES # Consumed on recovery boot path
+  ## SOMETIMES_CONSUMES
+  ## UNDEFINED # HOB
+  gEfiVectorHandoffInfoPpiGuid
+
+[Guids]
+  ## SOMETIMES_CONSUMES ## Variable:L"MemoryTypeInformation"
+  ## SOMETIMES_PRODUCES ## HOB
+  gEfiMemoryTypeInformationGuid
+
+[FeaturePcd.IA32]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode      ## CONSUMES
+
+[FeaturePcd.X64]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplBuildPageTables       ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSupportUefiDecompress ## CONSUMES
+
+[Pcd.IA32,Pcd.X64]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable              ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetNxForStack               ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiLoadFilePpiGuid AND gEfiPeiMasterBootModePpiGuid
+
+#
+# [BootMode]
+#  S3_RESUME                        ## SOMETIMES_CONSUMES
+#  RECOVERY_FULL                    ## SOMETIMES_CONSUMES
+#
+#
+# [Hob]
+# MEMORY_ALLOCATION                 ## SOMETIMES_PRODUCES # MEMORY_ALLOCATION_MODULE for DxeCore
+# MEMORY_ALLOCATION                 ## SOMETIMES_PRODUCES # New Stack HoB   
+# MEMORY_ALLOCATION                 ## SOMETIMES_PRODUCES # Old Stack HOB
+#
+# [Hob.IPF]
+# MEMORY_ALLOCATION                 ## SOMETIMES_PRODUCES # MEMORY_ALLOCATION_BSP_STORE
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DxeIplExtra.uni
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.uni b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.uni
new file mode 100644
index 0000000000..cfd1333c7a
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIpl.uni
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/DxeIplExtra.uni b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIplExtra.uni
new file mode 100644
index 0000000000..ec8cebc428
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/DxeIplExtra.uni
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/DxeLoad.c b/Core/MdeModulePkg/Core/DxeIplPeim/DxeLoad.c
new file mode 100644
index 0000000000..d7d693fdeb
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/DxeLoad.c
@@ -0,0 +1,762 @@
+/** @file
+  Last PEIM.
+  Responsibility of this module is to load the DXE Core from a Firmware Volume.
+
+Copyright (c) 2006 - 2012, 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 "DxeIpl.h"
+
+
+//
+// Module Globals used in the DXE to PEI hand off
+// These must be module globals, so the stack can be switched
+//
+CONST EFI_DXE_IPL_PPI mDxeIplPpi = {
+  DxeLoadCore
+};
+
+CONST EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI mCustomGuidedSectionExtractionPpi = {
+  CustomGuidedSectionExtract
+};
+
+CONST EFI_PEI_DECOMPRESS_PPI mDecompressPpi = {
+  Decompress
+};
+
+CONST EFI_PEI_PPI_DESCRIPTOR mPpiList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gEfiDxeIplPpiGuid,
+    (VOID *) &mDxeIplPpi
+  },
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiPeiDecompressPpiGuid,
+    (VOID *) &mDecompressPpi
+  }
+};
+
+CONST EFI_PEI_PPI_DESCRIPTOR gEndOfPeiSignalPpi = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiEndOfPeiSignalPpiGuid,
+  NULL
+};
+
+/**
+  Entry point of DXE IPL PEIM.
+  
+  This function installs DXE IPL PPI and Decompress PPI.  It also reloads
+  itself to memory on non-S3 resume boot path.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCESS  The entry point of DXE IPL PEIM executes successfully.
+  @retval Others      Some error occurs during the execution of this function. 
+
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeDxeIpl (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_BOOT_MODE                             BootMode;
+  EFI_GUID                                  *ExtractHandlerGuidTable;
+  UINTN                                     ExtractHandlerNumber;
+  EFI_PEI_PPI_DESCRIPTOR                    *GuidPpi;
+  
+  BootMode = GetBootModeHob ();
+
+  if (BootMode != BOOT_ON_S3_RESUME) {
+    Status = PeiServicesRegisterForShadow (FileHandle);
+    if (Status == EFI_SUCCESS) {
+      //
+      // EFI_SUCESS means it is the first time to call register for shadow. 
+      // 
+      return Status;
+    }
+    
+    //
+    // Ensure that DXE IPL is shadowed to permanent memory.
+    //
+    ASSERT (Status == EFI_ALREADY_STARTED);
+  }
+     
+  //
+  // Get custom extract guided section method guid list 
+  //
+  ExtractHandlerNumber = ExtractGuidedSectionGetGuidList (&ExtractHandlerGuidTable);
+  
+  //
+  // Install custom extraction guid PPI
+  //
+  if (ExtractHandlerNumber > 0) {
+    GuidPpi = (EFI_PEI_PPI_DESCRIPTOR *) AllocatePool (ExtractHandlerNumber * sizeof (EFI_PEI_PPI_DESCRIPTOR));
+    ASSERT (GuidPpi != NULL);
+    while (ExtractHandlerNumber-- > 0) {
+      GuidPpi->Flags = EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST;
+      GuidPpi->Ppi   = (VOID *) &mCustomGuidedSectionExtractionPpi;
+      GuidPpi->Guid  = &ExtractHandlerGuidTable[ExtractHandlerNumber];
+      Status = PeiServicesInstallPpi (GuidPpi++);
+      ASSERT_EFI_ERROR(Status);
+    }
+  }
+  
+  //
+  // Install DxeIpl and Decompress PPIs.
+  //
+  Status = PeiServicesInstallPpi (mPpiList);
+  ASSERT_EFI_ERROR(Status);
+
+  return Status;
+}
+
+/**
+   Validate variable data for the MemoryTypeInformation. 
+
+   @param MemoryData       Variable data.
+   @param MemoryDataSize   Variable data length.
+
+   @return TRUE            The variable data is valid.
+   @return FALSE           The variable data is invalid.
+
+**/
+BOOLEAN
+ValidateMemoryTypeInfoVariable (
+  IN EFI_MEMORY_TYPE_INFORMATION      *MemoryData,
+  IN UINTN                            MemoryDataSize
+  )
+{
+  UINTN                       Count;
+  UINTN                       Index;
+
+  // Check the input parameter.
+  if (MemoryData == NULL) {
+    return FALSE;
+  }
+
+  // Get Count
+  Count = MemoryDataSize / sizeof (*MemoryData);
+
+  // Check Size
+  if (Count * sizeof(*MemoryData) != MemoryDataSize) {
+    return FALSE;
+  }
+
+  // Check last entry type filed.
+  if (MemoryData[Count - 1].Type != EfiMaxMemoryType) {
+    return FALSE;
+  }
+
+  // Check the type filed.
+  for (Index = 0; Index < Count - 1; Index++) {
+    if (MemoryData[Index].Type >= EfiMaxMemoryType) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+   Main entry point to last PEIM. 
+
+   This function finds DXE Core in the firmware volume and transfer the control to
+   DXE core.
+    
+   @param This          Entry point for DXE IPL PPI.
+   @param PeiServices   General purpose services available to every PEIM.
+   @param HobList       Address to the Pei HOB list.
+   
+   @return EFI_SUCCESS              DXE core was successfully loaded. 
+   @return EFI_OUT_OF_RESOURCES     There are not enough resources to load DXE core.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeLoadCore (
+  IN CONST EFI_DXE_IPL_PPI *This,
+  IN EFI_PEI_SERVICES      **PeiServices,
+  IN EFI_PEI_HOB_POINTERS  HobList
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_FV_FILE_INFO                          DxeCoreFileInfo;
+  EFI_PHYSICAL_ADDRESS                      DxeCoreAddress;
+  UINT64                                    DxeCoreSize;
+  EFI_PHYSICAL_ADDRESS                      DxeCoreEntryPoint;
+  EFI_BOOT_MODE                             BootMode;
+  EFI_PEI_FILE_HANDLE                       FileHandle;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI           *Variable;
+  EFI_PEI_LOAD_FILE_PPI                     *LoadFile;
+  UINTN                                     Instance;
+  UINT32                                    AuthenticationState;
+  UINTN                                     DataSize;
+  EFI_PEI_S3_RESUME2_PPI                    *S3Resume;
+  EFI_PEI_RECOVERY_MODULE_PPI               *PeiRecovery;
+  EFI_MEMORY_TYPE_INFORMATION               MemoryData[EfiMaxMemoryType + 1];
+
+  //
+  // if in S3 Resume, restore configure
+  //
+  BootMode = GetBootModeHob ();
+
+  if (BootMode == BOOT_ON_S3_RESUME) {
+    Status = PeiServicesLocatePpi (
+               &gEfiPeiS3Resume2PpiGuid,
+               0,
+               NULL,
+               (VOID **) &S3Resume
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // Report Status code that S3Resume PPI can not be found
+      //
+      REPORT_STATUS_CODE (
+        EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+        (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_S3_RESUME_PPI_NOT_FOUND)
+        );
+    }
+    ASSERT_EFI_ERROR (Status);
+    
+    Status = S3Resume->S3RestoreConfig2 (S3Resume);
+    ASSERT_EFI_ERROR (Status);
+  } else if (BootMode == BOOT_IN_RECOVERY_MODE) {
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_RECOVERY_BEGIN));
+    Status = PeiServicesLocatePpi (
+               &gEfiPeiRecoveryModulePpiGuid,
+               0,
+               NULL,
+               (VOID **) &PeiRecovery
+               );
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "Locate Recovery PPI Failed.(Status = %r)\n", Status));
+      //
+      // Report Status code the failure of locating Recovery PPI 
+      //
+      REPORT_STATUS_CODE (
+        EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+        (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_RECOVERY_PPI_NOT_FOUND)
+        );
+      CpuDeadLoop ();
+    }
+
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_CAPSULE_LOAD));
+    Status = PeiRecovery->LoadRecoveryCapsule (PeiServices, PeiRecovery);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "Load Recovery Capsule Failed.(Status = %r)\n", Status));
+      //
+      // Report Status code that recovery image can not be found
+      //
+      REPORT_STATUS_CODE (
+        EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+        (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_NO_RECOVERY_CAPSULE)
+        );
+      CpuDeadLoop ();
+    }
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_CAPSULE_START));
+    //
+    // Now should have a HOB with the DXE core
+    //
+  }
+
+  if (GetFirstGuidHob ((CONST EFI_GUID *)&gEfiMemoryTypeInformationGuid) == NULL) {
+    //
+    // Don't build GuidHob if GuidHob has been installed.
+    //
+    Status = PeiServicesLocatePpi (
+               &gEfiPeiReadOnlyVariable2PpiGuid,
+               0,
+               NULL,
+               (VOID **)&Variable
+               );
+    if (!EFI_ERROR (Status)) {
+      DataSize = sizeof (MemoryData);
+      Status = Variable->GetVariable ( 
+                           Variable, 
+                           EFI_MEMORY_TYPE_INFORMATION_VARIABLE_NAME,
+                           &gEfiMemoryTypeInformationGuid,
+                           NULL,
+                           &DataSize,
+                           &MemoryData
+                           );
+      if (!EFI_ERROR (Status) && ValidateMemoryTypeInfoVariable(MemoryData, DataSize)) {
+        //
+        // Build the GUID'd HOB for DXE
+        //
+        BuildGuidDataHob (
+          &gEfiMemoryTypeInformationGuid,
+          MemoryData,
+          DataSize
+          );
+      }
+    }
+  }
+
+  //
+  // Look in all the FVs present in PEI and find the DXE Core FileHandle
+  //
+  FileHandle = DxeIplFindDxeCore ();
+
+  //
+  // Load the DXE Core from a Firmware Volume.
+  //
+  Instance = 0;
+  do {
+    Status = PeiServicesLocatePpi (&gEfiPeiLoadFilePpiGuid, Instance++, NULL, (VOID **) &LoadFile);
+    //
+    // These must exist an instance of EFI_PEI_LOAD_FILE_PPI to support to load DxeCore file handle successfully.
+    //
+    ASSERT_EFI_ERROR (Status);
+
+    Status = LoadFile->LoadFile (
+                         LoadFile,
+                         FileHandle,
+                         &DxeCoreAddress,
+                         &DxeCoreSize,
+                         &DxeCoreEntryPoint,
+                         &AuthenticationState
+                         );
+  } while (EFI_ERROR (Status));
+
+  //
+  // Get the DxeCore File Info from the FileHandle for the DxeCore GUID file name.
+  //
+  Status = PeiServicesFfsGetFileInfo (FileHandle, &DxeCoreFileInfo);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Add HOB for the DXE Core
+  //
+  BuildModuleHob (
+    &DxeCoreFileInfo.FileName,
+    DxeCoreAddress,
+    ALIGN_VALUE (DxeCoreSize, EFI_PAGE_SIZE),
+    DxeCoreEntryPoint
+    );
+
+  //
+  // Report Status Code EFI_SW_PEI_PC_HANDOFF_TO_NEXT
+  //
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_PEI_CORE | EFI_SW_PEI_CORE_PC_HANDOFF_TO_NEXT));
+
+  DEBUG ((DEBUG_INFO | DEBUG_LOAD, "Loading DXE CORE at 0x%11p EntryPoint=0x%11p\n", (VOID *)(UINTN)DxeCoreAddress, FUNCTION_ENTRY_POINT (DxeCoreEntryPoint)));
+
+  //
+  // Transfer control to the DXE Core
+  // The hand off state is simply a pointer to the HOB list
+  //
+  HandOffToDxeCore (DxeCoreEntryPoint, HobList);
+  //
+  // If we get here, then the DXE Core returned.  This is an error
+  // DxeCore should not return.
+  //
+  ASSERT (FALSE);
+  CpuDeadLoop ();
+
+  return EFI_OUT_OF_RESOURCES;
+}
+
+
+/**
+   Searches DxeCore in all firmware Volumes and loads the first
+   instance that contains DxeCore.
+
+   @return FileHandle of DxeCore to load DxeCore.
+   
+**/
+EFI_PEI_FILE_HANDLE
+DxeIplFindDxeCore (
+  VOID
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 Instance;
+  EFI_PEI_FV_HANDLE     VolumeHandle;
+  EFI_PEI_FILE_HANDLE   FileHandle;
+  
+  Instance    = 0;
+  while (TRUE) {
+    //
+    // Traverse all firmware volume instances
+    //
+    Status = PeiServicesFfsFindNextVolume (Instance, &VolumeHandle);
+    //
+    // If some error occurs here, then we cannot find any firmware
+    // volume that may contain DxeCore.
+    //
+    if (EFI_ERROR (Status)) {
+      REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_CORE_EC_DXE_CORRUPT));
+    }
+    ASSERT_EFI_ERROR (Status);
+    
+    //
+    // Find the DxeCore file type from the beginning in this firmware volume.
+    //
+    FileHandle = NULL;
+    Status = PeiServicesFfsFindNextFile (EFI_FV_FILETYPE_DXE_CORE, VolumeHandle, &FileHandle);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Find DxeCore FileHandle in this volume, then we skip other firmware volume and
+      // return the FileHandle.
+      //
+      return FileHandle;
+    }
+    //
+    // We cannot find DxeCore in this firmware volume, then search the next volume.
+    //
+    Instance++;
+  }
+}
+
+
+
+/**
+  The ExtractSection() function processes the input section and
+  returns a pointer to the section contents. If the section being
+  extracted does not require processing (if the section
+  GuidedSectionHeader.Attributes has the
+  EFI_GUIDED_SECTION_PROCESSING_REQUIRED field cleared), then
+  OutputBuffer is just updated to point to the start of the
+  section's contents. Otherwise, *Buffer must be allocated
+  from PEI permanent memory.
+
+  @param This                   Indicates the
+                                EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI instance.
+                                Buffer containing the input GUIDed section to be
+                                processed. OutputBuffer OutputBuffer is
+                                allocated from PEI permanent memory and contains
+                                the new section stream.
+  @param InputSection           A pointer to the input buffer, which contains
+                                the input section to be processed.
+  @param OutputBuffer           A pointer to a caller-allocated buffer, whose
+                                size is specified by the contents of OutputSize.
+  @param OutputSize             A pointer to a caller-allocated
+                                UINTN in which the size of *OutputBuffer
+                                allocation is stored. If the function
+                                returns anything other than EFI_SUCCESS,
+                                the value of OutputSize is undefined.
+  @param AuthenticationStatus   A pointer to a caller-allocated
+                                UINT32 that indicates the
+                                authentication status of the
+                                output buffer. If the input
+                                section's GuidedSectionHeader.
+                                Attributes field has the
+                                EFI_GUIDED_SECTION_AUTH_STATUS_VALID 
+                                bit as clear,
+                                AuthenticationStatus must return
+                                zero. These bits reflect the
+                                status of the extraction
+                                operation. If the function
+                                returns anything other than
+                                EFI_SUCCESS, the value of
+                                AuthenticationStatus is
+                                undefined.
+  
+  @retval EFI_SUCCESS           The InputSection was
+                                successfully processed and the
+                                section contents were returned.
+  
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient
+                                resources to process the request.
+  
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction PPI.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI *This,
+  IN CONST  VOID                                  *InputSection,
+  OUT       VOID                                  **OutputBuffer,
+  OUT       UINTN                                 *OutputSize,
+  OUT       UINT32                                *AuthenticationStatus
+)
+{
+  EFI_STATUS      Status;
+  UINT8           *ScratchBuffer;
+  UINT32          ScratchBufferSize;
+  UINT32          OutputBufferSize;
+  UINT16          SectionAttribute;
+  
+  //
+  // Init local variable
+  //
+  ScratchBuffer = NULL;
+
+  //
+  // Call GetInfo to get the size and attribute of input guided section data.
+  //
+  Status = ExtractGuidedSectionGetInfo (
+             InputSection,
+             &OutputBufferSize,
+             &ScratchBufferSize,
+             &SectionAttribute
+             );
+  
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "GetInfo from guided section Failed - %r\n", Status));
+    return Status;
+  }
+  
+  if (ScratchBufferSize != 0) {
+    //
+    // Allocate scratch buffer
+    //
+    ScratchBuffer = AllocatePages (EFI_SIZE_TO_PAGES (ScratchBufferSize));
+    if (ScratchBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  if (((SectionAttribute & EFI_GUIDED_SECTION_PROCESSING_REQUIRED) != 0) && OutputBufferSize > 0) {  
+    //
+    // Allocate output buffer
+    //
+    *OutputBuffer = AllocatePages (EFI_SIZE_TO_PAGES (OutputBufferSize) + 1);
+    if (*OutputBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DEBUG ((DEBUG_INFO, "Customized Guided section Memory Size required is 0x%x and address is 0x%p\n", OutputBufferSize, *OutputBuffer));
+    //
+    // *OutputBuffer still is one section. Adjust *OutputBuffer offset, 
+    // skip EFI section header to make section data at page alignment.
+    //
+    *OutputBuffer = (VOID *)((UINT8 *) *OutputBuffer + EFI_PAGE_SIZE - sizeof (EFI_COMMON_SECTION_HEADER));
+  }
+  
+  Status = ExtractGuidedSectionDecode (
+             InputSection, 
+             OutputBuffer,
+             ScratchBuffer,
+             AuthenticationStatus
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // Decode failed
+    //
+    DEBUG ((DEBUG_ERROR, "Extract guided section Failed - %r\n", Status));
+    return Status;
+  }
+  
+  *OutputSize = (UINTN) OutputBufferSize;
+  
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+   Decompresses a section to the output buffer.
+
+   This function looks up the compression type field in the input section and
+   applies the appropriate compression algorithm to compress the section to a
+   callee allocated buffer.
+    
+   @param  This                  Points to this instance of the
+                                 EFI_PEI_DECOMPRESS_PEI PPI.
+   @param  CompressionSection    Points to the compressed section.
+   @param  OutputBuffer          Holds the returned pointer to the decompressed
+                                 sections.
+   @param  OutputSize            Holds the returned size of the decompress
+                                 section streams.
+   
+   @retval EFI_SUCCESS           The section was decompressed successfully.
+                                 OutputBuffer contains the resulting data and
+                                 OutputSize contains the resulting size.
+
+**/
+EFI_STATUS
+EFIAPI 
+Decompress (
+  IN CONST  EFI_PEI_DECOMPRESS_PPI  *This,
+  IN CONST  EFI_COMPRESSION_SECTION *CompressionSection,
+  OUT       VOID                    **OutputBuffer,
+  OUT       UINTN                   *OutputSize
+ )
+{
+  EFI_STATUS                      Status;
+  UINT8                           *DstBuffer;
+  UINT8                           *ScratchBuffer;
+  UINT32                          DstBufferSize;
+  UINT32                          ScratchBufferSize;
+  VOID                            *CompressionSource;
+  UINT32                          CompressionSourceSize;
+  UINT32                          UncompressedLength;
+  UINT8                           CompressionType;
+
+  if (CompressionSection->CommonHeader.Type != EFI_SECTION_COMPRESSION) {
+    ASSERT (FALSE);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IS_SECTION2 (CompressionSection)) {
+    CompressionSource = (VOID *) ((UINT8 *) CompressionSection + sizeof (EFI_COMPRESSION_SECTION2));
+    CompressionSourceSize = (UINT32) (SECTION2_SIZE (CompressionSection) - sizeof (EFI_COMPRESSION_SECTION2));
+    UncompressedLength = ((EFI_COMPRESSION_SECTION2 *) CompressionSection)->UncompressedLength;
+    CompressionType = ((EFI_COMPRESSION_SECTION2 *) CompressionSection)->CompressionType;
+  } else {
+    CompressionSource = (VOID *) ((UINT8 *) CompressionSection + sizeof (EFI_COMPRESSION_SECTION));
+    CompressionSourceSize = (UINT32) (SECTION_SIZE (CompressionSection) - sizeof (EFI_COMPRESSION_SECTION));
+    UncompressedLength = CompressionSection->UncompressedLength;
+    CompressionType = CompressionSection->CompressionType;
+  }
+  
+  //
+  // This is a compression set, expand it
+  //
+  switch (CompressionType) {
+  case EFI_STANDARD_COMPRESSION:
+    if (FeaturePcdGet(PcdDxeIplSupportUefiDecompress)) {
+      //
+      // Load EFI standard compression.
+      // For compressed data, decompress them to destination buffer.
+      //
+      Status = UefiDecompressGetInfo (
+                 CompressionSource,
+                 CompressionSourceSize,
+                 &DstBufferSize,
+                 &ScratchBufferSize
+                 );
+      if (EFI_ERROR (Status)) {
+        //
+        // GetInfo failed
+        //
+        DEBUG ((DEBUG_ERROR, "Decompress GetInfo Failed - %r\n", Status));
+        return EFI_NOT_FOUND;
+      }
+      //
+      // Allocate scratch buffer
+      //
+      ScratchBuffer = AllocatePages (EFI_SIZE_TO_PAGES (ScratchBufferSize));
+      if (ScratchBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Allocate destination buffer, extra one page for adjustment 
+      //
+      DstBuffer = AllocatePages (EFI_SIZE_TO_PAGES (DstBufferSize) + 1);
+      if (DstBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // DstBuffer still is one section. Adjust DstBuffer offset, skip EFI section header
+      // to make section data at page alignment.
+      //
+      DstBuffer = DstBuffer + EFI_PAGE_SIZE - sizeof (EFI_COMMON_SECTION_HEADER);
+      //
+      // Call decompress function
+      //
+      Status = UefiDecompress (
+                  CompressionSource,
+                  DstBuffer,
+                  ScratchBuffer
+                  );
+      if (EFI_ERROR (Status)) {
+        //
+        // Decompress failed
+        //
+        DEBUG ((DEBUG_ERROR, "Decompress Failed - %r\n", Status));
+        return EFI_NOT_FOUND;
+      }
+      break;
+    } else {
+      //
+      // PcdDxeIplSupportUefiDecompress is FALSE
+      // Don't support UEFI decompression algorithm.
+      //
+      ASSERT (FALSE);
+      return EFI_NOT_FOUND;
+    }
+
+  case EFI_NOT_COMPRESSED:
+    //
+    // Allocate destination buffer
+    //
+    DstBufferSize = UncompressedLength;
+    DstBuffer     = AllocatePages (EFI_SIZE_TO_PAGES (DstBufferSize) + 1);
+    if (DstBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Adjust DstBuffer offset, skip EFI section header
+    // to make section data at page alignment.
+    //
+    DstBuffer = DstBuffer + EFI_PAGE_SIZE - sizeof (EFI_COMMON_SECTION_HEADER);
+    //
+    // stream is not actually compressed, just encapsulated.  So just copy it.
+    //
+    CopyMem (DstBuffer, CompressionSource, DstBufferSize);
+    break;
+
+  default:
+    //
+    // Don't support other unknown compression type.
+    //
+    ASSERT (FALSE);
+    return EFI_NOT_FOUND;
+  }
+
+  *OutputSize = DstBufferSize;
+  *OutputBuffer = DstBuffer;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+   Updates the Stack HOB passed to DXE phase.
+
+   This function traverses the whole HOB list and update the stack HOB to
+   reflect the real stack that is used by DXE core.
+
+   @param BaseAddress           The lower address of stack used by DxeCore.
+   @param Length                The length of stack used by DxeCore.
+
+**/
+VOID
+UpdateStackHob (
+  IN EFI_PHYSICAL_ADDRESS        BaseAddress,
+  IN UINT64                      Length
+  )
+{
+  EFI_PEI_HOB_POINTERS           Hob;
+
+  Hob.Raw = GetHobList ();
+  while ((Hob.Raw = GetNextHob (EFI_HOB_TYPE_MEMORY_ALLOCATION, Hob.Raw)) != NULL) {
+    if (CompareGuid (&gEfiHobMemoryAllocStackGuid, &(Hob.MemoryAllocationStack->AllocDescriptor.Name))) {
+      //
+      // Build a new memory allocation HOB with old stack info with EfiBootServicesData type. Need to 
+      // avoid this region be reclaimed by DXE core as the IDT built in SEC might be on stack, and some 
+      // PEIMs may also keep key information on stack
+      //
+      BuildMemoryAllocationHob (
+        Hob.MemoryAllocationStack->AllocDescriptor.MemoryBaseAddress,
+        Hob.MemoryAllocationStack->AllocDescriptor.MemoryLength,
+        EfiBootServicesData
+        );
+      //
+      // Update the BSP Stack Hob to reflect the new stack info.
+      //
+      Hob.MemoryAllocationStack->AllocDescriptor.MemoryBaseAddress = BaseAddress;
+      Hob.MemoryAllocationStack->AllocDescriptor.MemoryLength = Length;
+      break;
+    }
+    Hob.Raw = GET_NEXT_HOB (Hob);
+  }
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Ebc/DxeLoadFunc.c b/Core/MdeModulePkg/Core/DxeIplPeim/Ebc/DxeLoadFunc.c
new file mode 100644
index 0000000000..f06bd75891
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Ebc/DxeLoadFunc.c
@@ -0,0 +1,73 @@
+/** @file
+  EBC-specific functionality for DxeLoad.
+
+Copyright (c) 2006 - 2008, 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 "DxeIpl.h"
+
+
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  )
+{
+  VOID                *BaseOfStack;
+  VOID                *TopOfStack;
+  EFI_STATUS          Status;
+
+  //
+  // Allocate 128KB for the Stack
+  //
+  BaseOfStack = AllocatePages (EFI_SIZE_TO_PAGES (STACK_SIZE));
+  ASSERT (BaseOfStack != NULL);
+
+  //
+  // Compute the top of the stack we were allocated. Pre-allocate a UINTN
+  // for safety.
+  //
+  TopOfStack = (VOID *) ((UINTN) BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - CPU_STACK_ALIGNMENT);
+  TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);
+
+  //
+  // End of PEI phase signal
+  //
+  Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+  //    
+  UpdateStackHob ((EFI_PHYSICAL_ADDRESS)(UINTN) BaseOfStack, STACK_SIZE);
+
+  //
+  // Transfer the control to the entry point of DxeCore.
+  //
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+    HobList.Raw,
+    NULL,
+    TopOfStack
+    );
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/DxeLoadFunc.c b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/DxeLoadFunc.c
new file mode 100644
index 0000000000..8ce72cb316
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/DxeLoadFunc.c
@@ -0,0 +1,410 @@
+/** @file
+  Ia32-specific functionality for DxeLoad.
+
+Copyright (c) 2006 - 2015, 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 "DxeIpl.h"
+#include "VirtualMemory.h"
+
+#define IDT_ENTRY_COUNT       32
+
+typedef struct _X64_IDT_TABLE {
+  //
+  // Reserved 4 bytes preceding PeiService and IdtTable,
+  // since IDT base address should be 8-byte alignment.
+  //
+  UINT32                   Reserved;
+  CONST EFI_PEI_SERVICES   **PeiService;
+  X64_IDT_GATE_DESCRIPTOR  IdtTable[IDT_ENTRY_COUNT];
+} X64_IDT_TABLE;
+
+//
+// Global Descriptor Table (GDT)
+//
+GLOBAL_REMOVE_IF_UNREFERENCED IA32_GDT gGdtEntries[] = {
+/* selector { Global Segment Descriptor                              } */
+/* 0x00 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //null descriptor
+/* 0x08 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear data segment descriptor
+/* 0x10 */  {{0xffff, 0,  0,  0xf,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear code segment descriptor
+/* 0x18 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
+/* 0x20 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system code segment descriptor
+/* 0x28 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
+/* 0x30 */  {{0xffff, 0,  0,  0x2,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
+/* 0x38 */  {{0xffff, 0,  0,  0xa,  1,  0,  1,  0xf,  0,  1, 0,  1,  0}}, //system code segment descriptor
+/* 0x40 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
+};
+
+//
+// IA32 Gdt register
+//
+GLOBAL_REMOVE_IF_UNREFERENCED CONST IA32_DESCRIPTOR gGdt = {
+  sizeof (gGdtEntries) - 1,
+  (UINTN) gGdtEntries
+  };
+
+GLOBAL_REMOVE_IF_UNREFERENCED  IA32_DESCRIPTOR gLidtDescriptor = {
+  sizeof (X64_IDT_GATE_DESCRIPTOR) * IDT_ENTRY_COUNT - 1,
+  0
+};
+
+/**
+  Allocates and fills in the Page Directory and Page Table Entries to
+  establish a 4G page table.
+
+  @param[in] StackBase  Stack base address.
+  @param[in] StackSize  Stack size.
+
+  @return The address of page table.
+
+**/
+UINTN
+Create4GPageTablesIa32Pae (
+  IN EFI_PHYSICAL_ADDRESS   StackBase,
+  IN UINTN                  StackSize
+  )
+{  
+  UINT8                                         PhysicalAddressBits;
+  EFI_PHYSICAL_ADDRESS                          PhysicalAddress;
+  UINTN                                         IndexOfPdpEntries;
+  UINTN                                         IndexOfPageDirectoryEntries;
+  UINT32                                        NumberOfPdpEntriesNeeded;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageMap;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageDirectoryPointerEntry;
+  PAGE_TABLE_ENTRY                              *PageDirectoryEntry;
+  UINTN                                         TotalPagesNum;
+  UINTN                                         PageAddress;
+
+  PhysicalAddressBits = 32;
+
+  //
+  // Calculate the table entries needed.
+  //
+  NumberOfPdpEntriesNeeded = (UINT32) LShiftU64 (1, (PhysicalAddressBits - 30));
+
+  TotalPagesNum = NumberOfPdpEntriesNeeded + 1;
+  PageAddress = (UINTN) AllocatePages (TotalPagesNum);
+  ASSERT (PageAddress != 0);
+
+  PageMap = (VOID *) PageAddress;
+  PageAddress += SIZE_4KB;
+
+  PageDirectoryPointerEntry = PageMap;
+  PhysicalAddress = 0;
+
+  for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+    //
+    // Each Directory Pointer entries points to a page of Page Directory entires.
+    // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.
+    //       
+    PageDirectoryEntry = (VOID *) PageAddress;
+    PageAddress += SIZE_4KB;
+
+    //
+    // Fill in a Page Directory Pointer Entries
+    //
+    PageDirectoryPointerEntry->Uint64 = (UINT64) (UINTN) PageDirectoryEntry;
+    PageDirectoryPointerEntry->Bits.Present = 1;
+
+    for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PhysicalAddress += SIZE_2MB) {
+      if ((PhysicalAddress < StackBase + StackSize) && ((PhysicalAddress + SIZE_2MB) > StackBase)) {
+        //
+        // Need to split this 2M page that covers stack range.
+        //
+        Split2MPageTo4K (PhysicalAddress, (UINT64 *) PageDirectoryEntry, StackBase, StackSize);
+      } else {
+        //
+        // Fill in the Page Directory entries
+        //
+        PageDirectoryEntry->Uint64 = (UINT64) PhysicalAddress;
+        PageDirectoryEntry->Bits.ReadWrite = 1;
+        PageDirectoryEntry->Bits.Present = 1;
+        PageDirectoryEntry->Bits.MustBe1 = 1;
+      }
+    }
+  }
+
+  for (; IndexOfPdpEntries < 512; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+    ZeroMem (
+      PageDirectoryPointerEntry,
+      sizeof (PAGE_MAP_AND_DIRECTORY_POINTER)
+      );
+  }
+
+  return (UINTN) PageMap;
+}
+
+/**
+  The function will check if IA32 PAE is supported.
+
+  @retval TRUE      IA32 PAE is supported.
+  @retval FALSE     IA32 PAE is not supported.
+
+**/
+BOOLEAN
+IsIa32PaeSupport (
+  VOID
+  )
+{
+  UINT32            RegEax;
+  UINT32            RegEdx;
+  BOOLEAN           Ia32PaeSupport;
+
+  Ia32PaeSupport = FALSE;
+  AsmCpuid (0x0, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= 0x1) {
+    AsmCpuid (0x1, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & BIT6) != 0) {
+      Ia32PaeSupport = TRUE;
+    }
+  }
+
+  return Ia32PaeSupport;
+}
+
+/**
+  The function will check if Execute Disable Bit is available.
+
+  @retval TRUE      Execute Disable Bit is available.
+  @retval FALSE     Execute Disable Bit is not available.
+
+**/
+BOOLEAN
+IsExecuteDisableBitAvailable (
+  VOID
+  )
+{
+  UINT32            RegEax;
+  UINT32            RegEdx;
+  BOOLEAN           Available;
+
+  Available = FALSE;
+  AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= 0x80000001) {
+    AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+    if ((RegEdx & BIT20) != 0) {
+      //
+      // Bit 20: Execute Disable Bit available.
+      //
+      Available = TRUE;
+    }
+  }
+
+  return Available;
+}
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  )
+{
+  EFI_STATUS                Status;
+  EFI_PHYSICAL_ADDRESS      BaseOfStack;
+  EFI_PHYSICAL_ADDRESS      TopOfStack;
+  UINTN                     PageTables;
+  X64_IDT_GATE_DESCRIPTOR   *IdtTable;
+  UINTN                     SizeOfTemplate;
+  VOID                      *TemplateBase;
+  EFI_PHYSICAL_ADDRESS      VectorAddress;
+  UINT32                    Index;
+  X64_IDT_TABLE             *IdtTableForX64;
+  EFI_VECTOR_HANDOFF_INFO   *VectorInfo;
+  EFI_PEI_VECTOR_HANDOFF_INFO_PPI *VectorHandoffInfoPpi;
+  BOOLEAN                   BuildPageTablesIa32Pae;
+
+  Status = PeiServicesAllocatePages (EfiBootServicesData, EFI_SIZE_TO_PAGES (STACK_SIZE), &BaseOfStack);
+  ASSERT_EFI_ERROR (Status);
+
+  if (FeaturePcdGet(PcdDxeIplSwitchToLongMode)) {
+    //
+    // Compute the top of the stack we were allocated, which is used to load X64 dxe core.
+    // Pre-allocate a 32 bytes which confroms to x64 calling convention.
+    //
+    // The first four parameters to a function are passed in rcx, rdx, r8 and r9.
+    // Any further parameters are pushed on the stack. Furthermore, space (4 * 8bytes) for the
+    // register parameters is reserved on the stack, in case the called function
+    // wants to spill them; this is important if the function is variadic.
+    //
+    TopOfStack = BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - 32;
+
+    //
+    //  x64 Calling Conventions requires that the stack must be aligned to 16 bytes
+    //
+    TopOfStack = (EFI_PHYSICAL_ADDRESS) (UINTN) ALIGN_POINTER (TopOfStack, 16);
+
+    //
+    // Load the GDT of Go64. Since the GDT of 32-bit Tiano locates in the BS_DATA
+    // memory, it may be corrupted when copying FV to high-end memory
+    //
+    AsmWriteGdtr (&gGdt);
+    //
+    // Create page table and save PageMapLevel4 to CR3
+    //
+    PageTables = CreateIdentityMappingPageTables (BaseOfStack, STACK_SIZE);
+
+    //
+    // End of PEI phase signal
+    //
+    Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+    ASSERT_EFI_ERROR (Status);
+
+    AsmWriteCr3 (PageTables);
+
+    //
+    // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+    //
+    UpdateStackHob (BaseOfStack, STACK_SIZE);
+
+    SizeOfTemplate = AsmGetVectorTemplatInfo (&TemplateBase);
+
+    Status = PeiServicesAllocatePages (
+               EfiBootServicesData,
+               EFI_SIZE_TO_PAGES(sizeof (X64_IDT_TABLE) + SizeOfTemplate * IDT_ENTRY_COUNT),
+               (EFI_PHYSICAL_ADDRESS *) &IdtTableForX64
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Store EFI_PEI_SERVICES** in the 4 bytes immediately preceding IDT to avoid that
+    // it may not be gotten correctly after IDT register is re-written.
+    //
+    IdtTableForX64->PeiService = GetPeiServicesTablePointer ();
+
+    VectorAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) (IdtTableForX64 + 1);
+    IdtTable      = IdtTableForX64->IdtTable;
+    for (Index = 0; Index < IDT_ENTRY_COUNT; Index++) {
+      IdtTable[Index].Ia32IdtEntry.Bits.GateType    =  0x8e;
+      IdtTable[Index].Ia32IdtEntry.Bits.Reserved_0  =  0;
+      IdtTable[Index].Ia32IdtEntry.Bits.Selector    =  SYS_CODE64_SEL;
+
+      IdtTable[Index].Ia32IdtEntry.Bits.OffsetLow   = (UINT16) VectorAddress;
+      IdtTable[Index].Ia32IdtEntry.Bits.OffsetHigh  = (UINT16) (RShiftU64 (VectorAddress, 16));
+      IdtTable[Index].Offset32To63                  = (UINT32) (RShiftU64 (VectorAddress, 32));
+      IdtTable[Index].Reserved                      = 0;
+
+      CopyMem ((VOID *) (UINTN) VectorAddress, TemplateBase, SizeOfTemplate);
+      AsmVectorFixup ((VOID *) (UINTN) VectorAddress, (UINT8) Index);
+
+      VectorAddress += SizeOfTemplate;
+    }
+
+    gLidtDescriptor.Base = (UINTN) IdtTable;
+
+    //
+    // Disable interrupt of Debug timer, since new IDT table cannot handle it.
+    //
+    SaveAndSetDebugTimerInterrupt (FALSE);
+
+    AsmWriteIdtr (&gLidtDescriptor);
+
+    //
+    // Go to Long Mode and transfer control to DxeCore.
+    // Interrupts will not get turned on until the CPU AP is loaded.
+    // Call x64 drivers passing in single argument, a pointer to the HOBs.
+    //
+    AsmEnablePaging64 (
+      SYS_CODE64_SEL,
+      DxeCoreEntryPoint,
+      (EFI_PHYSICAL_ADDRESS)(UINTN)(HobList.Raw),
+      0,
+      TopOfStack
+      );
+  } else {
+    //
+    // Get Vector Hand-off Info PPI and build Guided HOB
+    //
+    Status = PeiServicesLocatePpi (
+               &gEfiVectorHandoffInfoPpiGuid,
+               0,
+               NULL,
+               (VOID **)&VectorHandoffInfoPpi
+               );
+    if (Status == EFI_SUCCESS) {
+      DEBUG ((EFI_D_INFO, "Vector Hand-off Info PPI is gotten, GUIDed HOB is created!\n"));
+      VectorInfo = VectorHandoffInfoPpi->Info;
+      Index = 1;
+      while (VectorInfo->Attribute != EFI_VECTOR_HANDOFF_LAST_ENTRY) {
+        VectorInfo ++;
+        Index ++;
+      }
+      BuildGuidDataHob (
+        &gEfiVectorHandoffInfoPpiGuid,
+        VectorHandoffInfoPpi->Info,
+        sizeof (EFI_VECTOR_HANDOFF_INFO) * Index
+        );
+    }
+
+    //
+    // Compute the top of the stack we were allocated. Pre-allocate a UINTN
+    // for safety.
+    //
+    TopOfStack = BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - CPU_STACK_ALIGNMENT;
+    TopOfStack = (EFI_PHYSICAL_ADDRESS) (UINTN) ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);
+
+    PageTables = 0;
+    BuildPageTablesIa32Pae = (BOOLEAN) (PcdGetBool (PcdSetNxForStack) && IsIa32PaeSupport () && IsExecuteDisableBitAvailable ());
+    if (BuildPageTablesIa32Pae) {
+      PageTables = Create4GPageTablesIa32Pae (BaseOfStack, STACK_SIZE);
+      EnableExecuteDisableBit ();
+    }
+
+    //
+    // End of PEI phase signal
+    //
+    Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+    ASSERT_EFI_ERROR (Status);
+
+    if (BuildPageTablesIa32Pae) {
+      AsmWriteCr3 (PageTables);
+      //
+      // Set Physical Address Extension (bit 5 of CR4).
+      //
+      AsmWriteCr4 (AsmReadCr4 () | BIT5);
+    }
+
+    //
+    // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+    //
+    UpdateStackHob (BaseOfStack, STACK_SIZE);
+
+    //
+    // Transfer the control to the entry point of DxeCore.
+    //
+    if (BuildPageTablesIa32Pae) {
+      AsmEnablePaging32 (
+        (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+        HobList.Raw,
+        NULL,
+        (VOID *) (UINTN) TopOfStack
+        );
+    } else {
+      SwitchStack (
+        (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+        HobList.Raw,
+        NULL,
+        (VOID *) (UINTN) TopOfStack
+        );
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.S b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.S
new file mode 100644
index 0000000000..96a3acbac1
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.S
@@ -0,0 +1,80 @@
+#/** @file
+#  
+#    IDT vector entry.
+#  
+#  Copyright (c) 2007 - 2009, 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.
+#  
+#**/
+
+    .text
+    .code32
+
+
+    .p2align 3
+    ASM_GLOBAL  ASM_PFX(AsmGetVectorTemplatInfo)
+    ASM_GLOBAL 	ASM_PFX(AsmVectorFixup)
+/*
+;
+;-----------------------------------------------------------------------
+;  Template of IDT Vector Handlers. 
+;
+;-----------------------------------------------------------------------
+*/
+VectorTemplateBase:
+        pushl %eax
+        .byte 0x6a    #  push #VectorNum
+VectorNum:
+        .byte 0
+        movl  CommonInterruptEntry, %eax
+        jmp   *%eax
+VectorTemplateEnd:
+
+
+ASM_PFX(AsmGetVectorTemplatInfo):
+        movl  4(%esp), %ecx
+        movl  $VectorTemplateBase, (%ecx)
+        movl  $(VectorTemplateEnd - VectorTemplateBase), %eax
+        ret
+
+ASM_PFX(AsmVectorFixup):
+        movl  8(%esp), %eax
+        movl  4(%esp), %ecx
+        movb  %al, (VectorNum - VectorTemplateBase)(%ecx)
+        ret
+
+/*
+; The follow algorithm is used for the common interrupt routine.
+
+;
+; +---------------------+ <-- 16-byte aligned ensured by processor
+; +    Old SS           +
+; +---------------------+
+; +    Old RSP          +
+; +---------------------+
+; +    RFlags           +
+; +---------------------+
+; +    CS               +
+; +---------------------+
+; +    RIP              +
+; +---------------------+
+; +    Error Code       +
+; +---------------------+
+; +    Vector Number    +
+; +---------------------+
+*/
+
+CommonInterruptEntry: 
+        cli
+1:
+        jmp   1b
+
+
+
+
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.asm b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.asm
new file mode 100644
index 0000000000..69fb9a15e7
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Ia32/IdtVectorAsm.asm
@@ -0,0 +1,88 @@
+;/** @file
+;  
+;    IDT vector entry.
+;  
+;  Copyright (c) 2007 - 2008, 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.
+;  
+;**/
+
+    .686p
+    .model  flat,C
+    .code
+
+;
+;------------------------------------------------------------------------------
+;  Generic IDT Vector Handlers for the Host. 
+;
+;------------------------------------------------------------------------------
+
+ALIGN   8
+PUBLIC  AsmGetVectorTemplatInfo
+PUBLIC  AsmVectorFixup
+
+PUBLIC  AsmVectorFixup
+
+@VectorTemplateBase:
+        push  eax
+        db    6ah       ; push #VectorNumber
+@VectorNum:
+        db    0
+        mov   eax, CommonInterruptEntry
+        jmp   eax
+@VectorTemplateEnd:
+
+
+AsmGetVectorTemplatInfo PROC
+        mov   ecx, [esp + 4]
+        mov   [ecx], @VectorTemplateBase
+        mov   eax, (@VectorTemplateEnd - @VectorTemplateBase)
+        ret
+AsmGetVectorTemplatInfo ENDP
+
+
+AsmVectorFixup PROC
+        mov   eax, dword ptr [esp + 8]
+        mov   ecx, [esp + 4]
+        mov   [ecx + (@VectorNum - @VectorTemplateBase)], al
+        ret
+AsmVectorFixup ENDP
+
+
+;---------------------------------------;
+; CommonInterruptEntry                  ;
+;---------------------------------------;
+; The follow algorithm is used for the common interrupt routine.
+
+;
+; +---------------------+ <-- 16-byte aligned ensured by processor
+; +    Old SS           +
+; +---------------------+
+; +    Old RSP          +
+; +---------------------+
+; +    RFlags           +
+; +---------------------+
+; +    CS               +
+; +---------------------+
+; +    RIP              +
+; +---------------------+
+; +    Error Code       +
+; +---------------------+
+; +    Vector Number    +
+; +---------------------+
+
+CommonInterruptEntry PROC 
+  cli
+ 
+  jmp $
+CommonInterruptEntry ENDP
+
+END
+
+
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/Ipf/DxeLoadFunc.c b/Core/MdeModulePkg/Core/DxeIplPeim/Ipf/DxeLoadFunc.c
new file mode 100644
index 0000000000..7443648017
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/Ipf/DxeLoadFunc.c
@@ -0,0 +1,85 @@
+/** @file
+  Ipf-specific functionality for DxeLoad.
+
+Copyright (c) 2006 - 2008, 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 "DxeIpl.h"
+
+
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  )
+{
+  VOID                *BaseOfStack;
+  VOID                *TopOfStack;
+  VOID                *BspStore;
+  EFI_STATUS          Status;
+
+  //
+  // Allocate 128KB for the Stack
+  //
+  BaseOfStack = AllocatePages (EFI_SIZE_TO_PAGES (STACK_SIZE));
+  ASSERT (BaseOfStack != NULL);
+
+  //
+  // Allocate 16KB for the BspStore
+  //
+  BspStore    = AllocatePages (EFI_SIZE_TO_PAGES (BSP_STORE_SIZE));
+  ASSERT (BspStore != NULL);
+  //
+  // Build BspStoreHob
+  //
+  BuildBspStoreHob ((EFI_PHYSICAL_ADDRESS) (UINTN) BspStore, BSP_STORE_SIZE, EfiBootServicesData);
+
+  //
+  // Compute the top of the stack we were allocated. Pre-allocate a UINTN
+  // for safety.
+  //
+  TopOfStack = (VOID *) ((UINTN) BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - CPU_STACK_ALIGNMENT);
+  TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);
+
+  //
+  // End of PEI phase signal
+  //
+  Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+  //
+  UpdateStackHob ((EFI_PHYSICAL_ADDRESS)(UINTN) BaseOfStack, STACK_SIZE);
+
+  //
+  // Transfer the control to the entry point of DxeCore.
+  //
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+    HobList.Raw,
+    NULL,
+    TopOfStack,
+    BspStore
+    );
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/X64/DxeLoadFunc.c b/Core/MdeModulePkg/Core/DxeIplPeim/X64/DxeLoadFunc.c
new file mode 100644
index 0000000000..6488880eab
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/X64/DxeLoadFunc.c
@@ -0,0 +1,120 @@
+/** @file
+  x64-specifc functionality for DxeLoad.
+
+Copyright (c) 2006 - 2015, 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 "DxeIpl.h"
+#include "X64/VirtualMemory.h"
+
+
+
+/**
+   Transfers control to DxeCore.
+
+   This function performs a CPU architecture specific operations to execute
+   the entry point of DxeCore with the parameters of HobList.
+   It also installs EFI_END_OF_PEI_PPI to signal the end of PEI phase.
+
+   @param DxeCoreEntryPoint         The entry point of DxeCore.
+   @param HobList                   The start of HobList passed to DxeCore.
+
+**/
+VOID
+HandOffToDxeCore (
+  IN EFI_PHYSICAL_ADDRESS   DxeCoreEntryPoint,
+  IN EFI_PEI_HOB_POINTERS   HobList
+  )
+{
+  VOID                            *BaseOfStack;
+  VOID                            *TopOfStack;
+  EFI_STATUS                      Status;
+  UINTN                           PageTables;
+  UINT32                          Index;
+  EFI_VECTOR_HANDOFF_INFO         *VectorInfo;
+  EFI_PEI_VECTOR_HANDOFF_INFO_PPI *VectorHandoffInfoPpi;
+
+  //
+  // Get Vector Hand-off Info PPI and build Guided HOB
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiVectorHandoffInfoPpiGuid,
+             0,
+             NULL,
+             (VOID **)&VectorHandoffInfoPpi
+             );
+  if (Status == EFI_SUCCESS) {
+    DEBUG ((EFI_D_INFO, "Vector Hand-off Info PPI is gotten, GUIDed HOB is created!\n"));
+    VectorInfo = VectorHandoffInfoPpi->Info;
+    Index = 1;
+    while (VectorInfo->Attribute != EFI_VECTOR_HANDOFF_LAST_ENTRY) {
+      VectorInfo ++;
+      Index ++;
+    }
+    BuildGuidDataHob (
+      &gEfiVectorHandoffInfoPpiGuid,
+      VectorHandoffInfoPpi->Info,
+      sizeof (EFI_VECTOR_HANDOFF_INFO) * Index
+      );
+  }
+
+  //
+  // Allocate 128KB for the Stack
+  //
+  BaseOfStack = AllocatePages (EFI_SIZE_TO_PAGES (STACK_SIZE));
+  ASSERT (BaseOfStack != NULL);
+
+  //
+  // Compute the top of the stack we were allocated. Pre-allocate a UINTN
+  // for safety.
+  //
+  TopOfStack = (VOID *) ((UINTN) BaseOfStack + EFI_SIZE_TO_PAGES (STACK_SIZE) * EFI_PAGE_SIZE - CPU_STACK_ALIGNMENT);
+  TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);
+
+  PageTables = 0;
+  if (FeaturePcdGet (PcdDxeIplBuildPageTables)) {
+    //
+    // Create page table and save PageMapLevel4 to CR3
+    //
+    PageTables = CreateIdentityMappingPageTables ((EFI_PHYSICAL_ADDRESS) (UINTN) BaseOfStack, STACK_SIZE);
+  } else {
+    //
+    // Set NX for stack feature also require PcdDxeIplBuildPageTables be TRUE
+    // for the DxeIpl and the DxeCore are both X64.
+    //
+    ASSERT (PcdGetBool (PcdSetNxForStack) == FALSE);
+  }
+  
+  //
+  // End of PEI phase signal
+  //
+  Status = PeiServicesInstallPpi (&gEndOfPeiSignalPpi);
+  ASSERT_EFI_ERROR (Status);
+
+  if (FeaturePcdGet (PcdDxeIplBuildPageTables)) {
+    AsmWriteCr3 (PageTables);
+  }
+
+  //
+  // Update the contents of BSP stack HOB to reflect the real stack info passed to DxeCore.
+  //    
+  UpdateStackHob ((EFI_PHYSICAL_ADDRESS)(UINTN) BaseOfStack, STACK_SIZE);
+
+  //
+  // Transfer the control to the entry point of DxeCore.
+  //
+  SwitchStack (
+    (SWITCH_STACK_ENTRY_POINT)(UINTN)DxeCoreEntryPoint,
+    HobList.Raw,
+    NULL,
+    TopOfStack
+    );
+}
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.c b/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.c
new file mode 100644
index 0000000000..ab1e91462e
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.c
@@ -0,0 +1,328 @@
+/** @file
+  x64 Virtual Memory Management Services in the form of an IA-32 driver.  
+  Used to establish a 1:1 Virtual to Physical Mapping that is required to
+  enter Long Mode (x64 64-bit mode).
+
+  While we make a 1:1 mapping (identity mapping) for all physical pages 
+  we still need to use the MTRR's to ensure that the cachability attributes
+  for all memory regions is correct.
+
+  The basic idea is to use 2MB page table entries where ever possible. If
+  more granularity of cachability is required then 4K page tables are used.
+
+  References:
+    1) IA-32 Intel(R) Architecture Software Developer's Manual Volume 1:Basic Architecture, Intel
+    2) IA-32 Intel(R) Architecture Software Developer's Manual Volume 2:Instruction Set Reference, Intel
+    3) IA-32 Intel(R) Architecture Software Developer's Manual Volume 3:System Programmer's Guide, Intel
+
+Copyright (c) 2006 - 2015, 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 "DxeIpl.h"
+#include "VirtualMemory.h"
+
+/**
+  Enable Execute Disable Bit.
+
+**/
+VOID
+EnableExecuteDisableBit (
+  VOID
+  )
+{
+  UINT64           MsrRegisters;
+
+  MsrRegisters = AsmReadMsr64 (0xC0000080);
+  MsrRegisters |= BIT11;
+  AsmWriteMsr64 (0xC0000080, MsrRegisters);
+}
+
+/**
+  Split 2M page to 4K.
+
+  @param[in]      PhysicalAddress       Start physical address the 2M page covered.
+  @param[in, out] PageEntry2M           Pointer to 2M page entry.
+  @param[in]      StackBase             Stack base address.
+  @param[in]      StackSize             Stack size.
+
+**/
+VOID
+Split2MPageTo4K (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN OUT UINT64                         *PageEntry2M,
+  IN EFI_PHYSICAL_ADDRESS               StackBase,
+  IN UINTN                              StackSize
+  )
+{
+  EFI_PHYSICAL_ADDRESS                  PhysicalAddress4K;
+  UINTN                                 IndexOfPageTableEntries;
+  PAGE_TABLE_4K_ENTRY                   *PageTableEntry;
+
+  PageTableEntry = AllocatePages (1);
+  //
+  // Fill in 2M page entry.
+  //
+  *PageEntry2M = (UINT64) (UINTN) PageTableEntry | IA32_PG_P | IA32_PG_RW;
+
+  PhysicalAddress4K = PhysicalAddress;
+  for (IndexOfPageTableEntries = 0; IndexOfPageTableEntries < 512; IndexOfPageTableEntries++, PageTableEntry++, PhysicalAddress4K += SIZE_4KB) {
+    //
+    // Fill in the Page Table entries
+    //
+    PageTableEntry->Uint64 = (UINT64) PhysicalAddress4K;
+    PageTableEntry->Bits.ReadWrite = 1;
+    PageTableEntry->Bits.Present = 1;
+    if ((PhysicalAddress4K >= StackBase) && (PhysicalAddress4K < StackBase + StackSize)) {
+      //
+      // Set Nx bit for stack.
+      //
+      PageTableEntry->Bits.Nx = 1;
+    }
+  }
+}
+
+/**
+  Split 1G page to 2M.
+
+  @param[in]      PhysicalAddress       Start physical address the 1G page covered.
+  @param[in, out] PageEntry1G           Pointer to 1G page entry.
+  @param[in]      StackBase             Stack base address.
+  @param[in]      StackSize             Stack size.
+
+**/
+VOID
+Split1GPageTo2M (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN OUT UINT64                         *PageEntry1G,
+  IN EFI_PHYSICAL_ADDRESS               StackBase,
+  IN UINTN                              StackSize
+  )
+{
+  EFI_PHYSICAL_ADDRESS                  PhysicalAddress2M;
+  UINTN                                 IndexOfPageDirectoryEntries;
+  PAGE_TABLE_ENTRY                      *PageDirectoryEntry;
+
+  PageDirectoryEntry = AllocatePages (1);
+  //
+  // Fill in 1G page entry.
+  //
+  *PageEntry1G = (UINT64) (UINTN) PageDirectoryEntry | IA32_PG_P | IA32_PG_RW;
+
+  PhysicalAddress2M = PhysicalAddress;
+  for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PhysicalAddress2M += SIZE_2MB) {
+    if ((PhysicalAddress2M < StackBase + StackSize) && ((PhysicalAddress2M + SIZE_2MB) > StackBase)) {
+      //
+      // Need to split this 2M page that covers stack range.
+      //
+      Split2MPageTo4K (PhysicalAddress2M, (UINT64 *) PageDirectoryEntry, StackBase, StackSize);
+    } else {
+      //
+      // Fill in the Page Directory entries
+      //
+      PageDirectoryEntry->Uint64 = (UINT64) PhysicalAddress2M;
+      PageDirectoryEntry->Bits.ReadWrite = 1;
+      PageDirectoryEntry->Bits.Present = 1;
+      PageDirectoryEntry->Bits.MustBe1 = 1;
+    }
+  }
+}
+
+/**
+  Allocates and fills in the Page Directory and Page Table Entries to
+  establish a 1:1 Virtual to Physical mapping.
+
+  @param[in] StackBase  Stack base address.
+  @param[in] StackSize  Stack size.
+
+  @return The address of 4 level page map.
+
+**/
+UINTN
+CreateIdentityMappingPageTables (
+  IN EFI_PHYSICAL_ADDRESS   StackBase,
+  IN UINTN                  StackSize
+  )
+{  
+  UINT32                                        RegEax;
+  UINT32                                        RegEdx;
+  UINT8                                         PhysicalAddressBits;
+  EFI_PHYSICAL_ADDRESS                          PageAddress;
+  UINTN                                         IndexOfPml4Entries;
+  UINTN                                         IndexOfPdpEntries;
+  UINTN                                         IndexOfPageDirectoryEntries;
+  UINT32                                        NumberOfPml4EntriesNeeded;
+  UINT32                                        NumberOfPdpEntriesNeeded;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageMapLevel4Entry;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageMap;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageDirectoryPointerEntry;
+  PAGE_TABLE_ENTRY                              *PageDirectoryEntry;
+  UINTN                                         TotalPagesNum;
+  UINTN                                         BigPageAddress;
+  VOID                                          *Hob;
+  BOOLEAN                                       Page1GSupport;
+  PAGE_TABLE_1G_ENTRY                           *PageDirectory1GEntry;
+
+  Page1GSupport = FALSE;
+  if (PcdGetBool(PcdUse1GPageTable)) {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000001) {
+      AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+      if ((RegEdx & BIT26) != 0) {
+        Page1GSupport = TRUE;
+      }
+    }
+  }
+
+  //
+  // Get physical address bits supported.
+  //
+  Hob = GetFirstHob (EFI_HOB_TYPE_CPU);
+  if (Hob != NULL) {
+    PhysicalAddressBits = ((EFI_HOB_CPU *) Hob)->SizeOfMemorySpace;
+  } else {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000008) {
+      AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
+      PhysicalAddressBits = (UINT8) RegEax;
+    } else {
+      PhysicalAddressBits = 36;
+    }
+  }
+
+  //
+  // IA-32e paging translates 48-bit linear addresses to 52-bit physical addresses.
+  //
+  ASSERT (PhysicalAddressBits <= 52);
+  if (PhysicalAddressBits > 48) {
+    PhysicalAddressBits = 48;
+  }
+
+  //
+  // Calculate the table entries needed.
+  //
+  if (PhysicalAddressBits <= 39 ) {
+    NumberOfPml4EntriesNeeded = 1;
+    NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 30));
+  } else {
+    NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 39));
+    NumberOfPdpEntriesNeeded = 512;
+  }
+
+  //
+  // Pre-allocate big pages to avoid later allocations. 
+  //
+  if (!Page1GSupport) {
+    TotalPagesNum = (NumberOfPdpEntriesNeeded + 1) * NumberOfPml4EntriesNeeded + 1;
+  } else {
+    TotalPagesNum = NumberOfPml4EntriesNeeded + 1;
+  }
+  BigPageAddress = (UINTN) AllocatePages (TotalPagesNum);
+  ASSERT (BigPageAddress != 0);
+
+  //
+  // By architecture only one PageMapLevel4 exists - so lets allocate storage for it.
+  //
+  PageMap         = (VOID *) BigPageAddress;
+  BigPageAddress += SIZE_4KB;
+
+  PageMapLevel4Entry = PageMap;
+  PageAddress        = 0;
+  for (IndexOfPml4Entries = 0; IndexOfPml4Entries < NumberOfPml4EntriesNeeded; IndexOfPml4Entries++, PageMapLevel4Entry++) {
+    //
+    // Each PML4 entry points to a page of Page Directory Pointer entires.
+    // So lets allocate space for them and fill them in in the IndexOfPdpEntries loop.
+    //
+    PageDirectoryPointerEntry = (VOID *) BigPageAddress;
+    BigPageAddress += SIZE_4KB;
+
+    //
+    // Make a PML4 Entry
+    //
+    PageMapLevel4Entry->Uint64 = (UINT64)(UINTN)PageDirectoryPointerEntry;
+    PageMapLevel4Entry->Bits.ReadWrite = 1;
+    PageMapLevel4Entry->Bits.Present = 1;
+
+    if (Page1GSupport) {
+      PageDirectory1GEntry = (VOID *) PageDirectoryPointerEntry;
+    
+      for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectory1GEntry++, PageAddress += SIZE_1GB) {
+        if (PcdGetBool (PcdSetNxForStack) && (PageAddress < StackBase + StackSize) && ((PageAddress + SIZE_1GB) > StackBase)) {
+          Split1GPageTo2M (PageAddress, (UINT64 *) PageDirectory1GEntry, StackBase, StackSize);
+        } else {
+          //
+          // Fill in the Page Directory entries
+          //
+          PageDirectory1GEntry->Uint64 = (UINT64)PageAddress;
+          PageDirectory1GEntry->Bits.ReadWrite = 1;
+          PageDirectory1GEntry->Bits.Present = 1;
+          PageDirectory1GEntry->Bits.MustBe1 = 1;
+        }
+      }
+    } else {
+      for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+        //
+        // Each Directory Pointer entries points to a page of Page Directory entires.
+        // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.
+        //       
+        PageDirectoryEntry = (VOID *) BigPageAddress;
+        BigPageAddress += SIZE_4KB;
+
+        //
+        // Fill in a Page Directory Pointer Entries
+        //
+        PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry;
+        PageDirectoryPointerEntry->Bits.ReadWrite = 1;
+        PageDirectoryPointerEntry->Bits.Present = 1;
+
+        for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PageAddress += SIZE_2MB) {
+          if (PcdGetBool (PcdSetNxForStack) && (PageAddress < StackBase + StackSize) && ((PageAddress + SIZE_2MB) > StackBase)) {
+            //
+            // Need to split this 2M page that covers stack range.
+            //
+            Split2MPageTo4K (PageAddress, (UINT64 *) PageDirectoryEntry, StackBase, StackSize);
+          } else {
+            //
+            // Fill in the Page Directory entries
+            //
+            PageDirectoryEntry->Uint64 = (UINT64)PageAddress;
+            PageDirectoryEntry->Bits.ReadWrite = 1;
+            PageDirectoryEntry->Bits.Present = 1;
+            PageDirectoryEntry->Bits.MustBe1 = 1;
+          }
+        }
+      }
+
+      for (; IndexOfPdpEntries < 512; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+        ZeroMem (
+          PageDirectoryPointerEntry,
+          sizeof(PAGE_MAP_AND_DIRECTORY_POINTER)
+          );
+      }
+    }
+  }
+
+  //
+  // For the PML4 entries we are not using fill in a null entry.
+  //
+  for (; IndexOfPml4Entries < 512; IndexOfPml4Entries++, PageMapLevel4Entry++) {
+    ZeroMem (
+      PageMapLevel4Entry,
+      sizeof (PAGE_MAP_AND_DIRECTORY_POINTER)
+      );
+  }
+
+  if (PcdGetBool (PcdSetNxForStack)) {
+    EnableExecuteDisableBit ();
+  }
+
+  return (UINTN)PageMap;
+}
+
diff --git a/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.h b/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.h
new file mode 100644
index 0000000000..20c31f571d
--- /dev/null
+++ b/Core/MdeModulePkg/Core/DxeIplPeim/X64/VirtualMemory.h
@@ -0,0 +1,226 @@
+/** @file
+  x64 Long Mode Virtual Memory Management Definitions  
+
+  References:
+    1) IA-32 Intel(R) Architecture Software Developer's Manual Volume 1:Basic Architecture, Intel
+    2) IA-32 Intel(R) Architecture Software Developer's Manual Volume 2:Instruction Set Reference, Intel
+    3) IA-32 Intel(R) Architecture Software Developer's Manual Volume 3:System Programmer's Guide, Intel
+    4) AMD64 Architecture Programmer's Manual Volume 2: System Programming
+
+Copyright (c) 2006 - 2015, 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.
+
+**/  
+#ifndef _VIRTUAL_MEMORY_H_
+#define _VIRTUAL_MEMORY_H_
+
+
+#define SYS_CODE64_SEL 0x38
+
+#pragma pack(1)
+
+typedef union {
+  struct {
+    UINT32  LimitLow    : 16;
+    UINT32  BaseLow     : 16;
+    UINT32  BaseMid     : 8;
+    UINT32  Type        : 4;
+    UINT32  System      : 1;
+    UINT32  Dpl         : 2;
+    UINT32  Present     : 1;
+    UINT32  LimitHigh   : 4;
+    UINT32  Software    : 1;
+    UINT32  Reserved    : 1;
+    UINT32  DefaultSize : 1;
+    UINT32  Granularity : 1;
+    UINT32  BaseHigh    : 8;
+  } Bits;
+  UINT64  Uint64;
+} IA32_GDT;
+
+typedef struct {
+  IA32_IDT_GATE_DESCRIPTOR  Ia32IdtEntry;
+  UINT32                    Offset32To63;
+  UINT32                    Reserved;
+} X64_IDT_GATE_DESCRIPTOR;
+
+//
+// Page-Map Level-4 Offset (PML4) and
+// Page-Directory-Pointer Offset (PDPE) entries 4K & 2MB
+//
+
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Reserved:1;               // Reserved
+    UINT64  MustBeZero:2;             // Must Be Zero
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PageTableBaseAddress:40;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // No Execute bit
+  } Bits;
+  UINT64    Uint64;
+} PAGE_MAP_AND_DIRECTORY_POINTER;
+
+//
+// Page Table Entry 4KB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  PAT:1;                    //
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PageTableBaseAddress:40;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_4K_ENTRY;
+
+//
+// Page Table Entry 2MB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1:1;                // Must be 1 
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PAT:1;                    //
+    UINT64  MustBeZero:8;             // Must be zero;
+    UINT64  PageTableBaseAddress:31;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_ENTRY;
+
+//
+// Page Table Entry 1GB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1:1;                // Must be 1 
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PAT:1;                    //
+    UINT64  MustBeZero:17;            // Must be zero;
+    UINT64  PageTableBaseAddress:22;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_1G_ENTRY;
+
+#pragma pack()
+
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+
+/**
+  Enable Execute Disable Bit.
+
+**/
+VOID
+EnableExecuteDisableBit (
+  VOID
+  );
+
+/**
+  Split 2M page to 4K.
+
+  @param[in]      PhysicalAddress       Start physical address the 2M page covered.
+  @param[in, out] PageEntry2M           Pointer to 2M page entry.
+  @param[in]      StackBase             Stack base address.
+  @param[in]      StackSize             Stack size.
+
+**/
+VOID
+Split2MPageTo4K (
+  IN EFI_PHYSICAL_ADDRESS               PhysicalAddress,
+  IN OUT UINT64                         *PageEntry2M,
+  IN EFI_PHYSICAL_ADDRESS               StackBase,
+  IN UINTN                              StackSize
+  );
+
+/**
+  Allocates and fills in the Page Directory and Page Table Entries to
+  establish a 1:1 Virtual to Physical mapping.
+
+  @param[in] StackBase  Stack base address.
+  @param[in] StackSize  Stack size.
+
+  @return The address of 4 level page map.
+
+**/
+UINTN
+CreateIdentityMappingPageTables (
+  IN EFI_PHYSICAL_ADDRESS   StackBase,
+  IN UINTN                  StackSize
+  );
+
+
+/**
+ 
+  Fix up the vector number in the vector code.
+ 
+  @param VectorBase   Base address of the vector handler.
+  @param VectorNum    Index of vector.
+
+**/
+VOID
+EFIAPI
+AsmVectorFixup (
+  VOID    *VectorBase,
+  UINT8   VectorNum
+  );
+
+
+/**
+ 
+  Get the information of vector template.
+  
+  @param TemplateBase   Base address of the template code.
+ 
+  @return               Size of the Template code.
+
+**/
+UINTN
+EFIAPI
+AsmGetVectorTemplatInfo (
+  OUT   VOID  **TemplateBase
+  );
+
+
+#endif 
diff --git a/Core/MdeModulePkg/Core/Pei/BootMode/BootMode.c b/Core/MdeModulePkg/Core/Pei/BootMode/BootMode.c
new file mode 100644
index 0000000000..39afeba838
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/BootMode/BootMode.c
@@ -0,0 +1,86 @@
+/** @file
+  This module provide function for ascertaining and updating the boot mode:
+  GetBootMode()
+  SetBootMode()
+  See PI Specification volume I, chapter 9 Boot Paths for additional information
+  on the boot mode.
+  
+Copyright (c) 2006 - 2008, 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 "PeiMain.h"
+
+/**
+  This service enables PEIMs to ascertain the present value of the boot mode.
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param BootMode               A pointer to contain the value of the boot mode.
+
+  @retval EFI_SUCCESS           The boot mode was returned successfully.
+  @retval EFI_INVALID_PARAMETER BootMode is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetBootMode (
+  IN  CONST EFI_PEI_SERVICES  **PeiServices,
+  IN  OUT   EFI_BOOT_MODE     *BootMode
+  )
+{
+  PEI_CORE_INSTANCE             *PrivateData;    
+  EFI_HOB_HANDOFF_INFO_TABLE    *HandOffHob;
+
+
+  if (BootMode == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+  
+  HandOffHob  = (PrivateData->HobList.HandoffInformationTable);
+  
+  *BootMode   = HandOffHob->BootMode;
+  
+
+  return EFI_SUCCESS;   
+}
+
+
+/**
+  This service enables PEIMs to update the boot mode variable.
+
+
+  @param PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param BootMode         The value of the boot mode to set.
+
+  @return EFI_SUCCESS     The value was successfully updated
+
+**/
+EFI_STATUS
+EFIAPI
+PeiSetBootMode (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN EFI_BOOT_MODE           BootMode
+  )
+{
+  PEI_CORE_INSTANCE                    *PrivateData;    
+  EFI_HOB_HANDOFF_INFO_TABLE    *HandOffHob;
+
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+  
+  HandOffHob  = (PrivateData->HobList.HandoffInformationTable);
+  
+  HandOffHob->BootMode = BootMode;
+
+
+  return EFI_SUCCESS;   
+}
diff --git a/Core/MdeModulePkg/Core/Pei/CpuIo/CpuIo.c b/Core/MdeModulePkg/Core/Pei/CpuIo/CpuIo.c
new file mode 100644
index 0000000000..c82c221540
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/CpuIo/CpuIo.c
@@ -0,0 +1,541 @@
+/** @file
+  The default version of EFI_PEI_CPU_IO_PPI support published by PeiServices in
+  PeiCore initialization phase.
+ 
+  EFI_PEI_CPU_IO_PPI is installed by some platform or chipset-specific PEIM that
+  abstracts the processor-visible I/O operations. When PeiCore is started, the
+  default version of EFI_PEI_CPU_IO_PPI will be assigned to PeiServices table.
+  
+Copyright (c) 2009, 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 "PeiMain.h"
+
+///
+/// This default instance of EFI_PEI_CPU_IO_PPI install assigned to EFI_PEI_SERVICE.CpuIo
+/// when PeiCore's initialization.
+///
+EFI_PEI_CPU_IO_PPI gPeiDefaultCpuIoPpi = {
+  {
+    PeiDefaultMemRead,
+    PeiDefaultMemWrite
+  },
+  {
+    PeiDefaultIoRead,
+    PeiDefaultIoWrite
+  },
+  PeiDefaultIoRead8,
+  PeiDefaultIoRead16,
+  PeiDefaultIoRead32,
+  PeiDefaultIoRead64,
+  PeiDefaultIoWrite8,
+  PeiDefaultIoWrite16,
+  PeiDefaultIoWrite32,
+  PeiDefaultIoWrite64,
+  PeiDefaultMemRead8,
+  PeiDefaultMemRead16,
+  PeiDefaultMemRead32,
+  PeiDefaultMemRead64,
+  PeiDefaultMemWrite8,
+  PeiDefaultMemWrite16,
+  PeiDefaultMemWrite32,
+  PeiDefaultMemWrite64
+};
+
+/**
+  Memory-based read services.
+  
+  This function is to perform the Memory Access Read service based on installed 
+  instance of the EFI_PEI_CPU_IO_PPI. 
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+   
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.     
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultMemRead (
+  IN  CONST EFI_PEI_SERVICES            **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI          *This,
+  IN  EFI_PEI_CPU_IO_PPI_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  OUT VOID                          *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}
+  
+/**
+  Memory-based write services.
+   
+  This function is to perform the Memory Access Write service based on installed 
+  instance of the EFI_PEI_CPU_IO_PPI. 
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+   
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.     
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultMemWrite (
+  IN  CONST EFI_PEI_SERVICES            **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI          *This,
+  IN  EFI_PEI_CPU_IO_PPI_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  OUT VOID                          *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  IO-based read services.
+  
+  This function is to perform the IO-base read service for the EFI_PEI_CPU_IO_PPI.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultIoRead (
+  IN      CONST EFI_PEI_SERVICES          **PeiServices,
+  IN      CONST EFI_PEI_CPU_IO_PPI        *This,
+  IN      EFI_PEI_CPU_IO_PPI_WIDTH        Width,
+  IN      UINT64                          Address,
+  IN      UINTN                           Count,
+  IN OUT  VOID                            *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  IO-based write services.
+  
+  This function is to perform the IO-base write service for the EFI_PEI_CPU_IO_PPI.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultIoWrite (
+  IN      CONST EFI_PEI_SERVICES          **PeiServices,
+  IN      CONST EFI_PEI_CPU_IO_PPI        *This,
+  IN      EFI_PEI_CPU_IO_PPI_WIDTH        Width,
+  IN      UINT64                          Address,
+  IN      UINTN                           Count,
+  IN OUT  VOID                            *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  8-bit I/O read operations.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 8-bit value returned from the I/O space.
+**/
+UINT8
+EFIAPI
+PeiDefaultIoRead8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}
+
+/**
+  Reads an 16-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 16-bit value returned from the I/O space.
+**/
+UINT16
+EFIAPI
+PeiDefaultIoRead16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}
+
+/**
+  Reads an 32-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 32-bit value returned from the I/O space.
+**/
+UINT32
+EFIAPI
+PeiDefaultIoRead32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}
+
+/**
+  Reads an 64-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 64-bit value returned from the I/O space.
+**/
+UINT64
+EFIAPI
+PeiDefaultIoRead64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}
+
+/**
+  8-bit I/O write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT8                       Data
+  )
+{
+}  
+
+/**
+  16-bit I/O write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT16                      Data
+  )
+{
+}
+
+/**
+  32-bit I/O write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT32                      Data
+  )
+{
+}
+
+/**
+  64-bit I/O write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT64                      Data
+  )
+{
+}
+
+/**
+  8-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 8-bit value returned from the memory space.
+
+**/
+UINT8
+EFIAPI
+PeiDefaultMemRead8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}  
+
+/**
+  16-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 16-bit value returned from the memory space.
+
+**/
+UINT16
+EFIAPI
+PeiDefaultMemRead16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+}  
+
+/**
+  32-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 32-bit value returned from the memory space.
+
+**/
+UINT32
+EFIAPI
+PeiDefaultMemRead32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+} 
+
+/**
+  64-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 64-bit value returned from the memory space.
+
+**/
+UINT64
+EFIAPI
+PeiDefaultMemRead64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  )
+{
+  return 0;
+} 
+
+/**
+  8-bit memory write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite8 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT8                         Data
+  )
+{
+}  
+
+/**
+  16-bit memory write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite16 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT16                        Data
+  )
+{
+}  
+
+/**
+  32-bit memory write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite32 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT32                        Data
+  )
+{
+} 
+
+/**
+  64-bit memory write operations.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then do
+  nothing.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite64 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT64                        Data
+  )
+{
+} 
diff --git a/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.c b/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.c
new file mode 100644
index 0000000000..e71566b5a7
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.c
@@ -0,0 +1,253 @@
+/** @file
+  PEI Dispatcher Dependency Evaluator
+
+  This routine evaluates a dependency expression (DEPENDENCY_EXPRESSION) to determine
+  if a driver can be scheduled for execution.  The criteria for
+  schedulability is that the dependency expression is satisfied.
+
+Copyright (c) 2006 - 2014, 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 "PeiMain.h"
+#include "Dependency.h"
+
+/**
+
+  This routine determines if a PPI has been installed.
+  The truth value of a GUID is determined by if the PPI has
+  been published and can be queried from the PPI database.
+
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param Stack           Reference to EVAL_STACK_ENTRY that contains PPI GUID to check
+
+  @retval TRUE  if the PPI is already installed.
+  @retval FALSE if the PPI has yet to be installed.
+
+**/
+BOOLEAN
+IsPpiInstalled (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN EVAL_STACK_ENTRY  *Stack
+  )
+{
+  VOID        *PeiInstance;
+  EFI_STATUS  Status;
+  EFI_GUID    PpiGuid;
+  
+  //
+  // If there is no GUID to evaluate, just return current result on stack.
+  //
+  if (Stack->Operator == NULL) {
+    return Stack->Result;
+  }
+  
+  //
+  // Copy the Guid into a locale variable so that there are no
+  // possibilities of alignment faults for cross-compilation 
+  // environments such as Intel?Itanium(TM).
+  //
+  CopyMem(&PpiGuid, Stack->Operator, sizeof(EFI_GUID));
+
+  //
+  // Check if the PPI is installed.
+  //
+  Status = PeiServicesLocatePpi(
+             &PpiGuid,        // GUID
+             0,               // INSTANCE
+             NULL,            // EFI_PEI_PPI_DESCRIPTOR
+             &PeiInstance     // PPI
+             );
+
+  if (EFI_ERROR(Status)) {
+    return FALSE;
+  }
+   
+  return TRUE;
+}
+
+/**
+
+  This is the POSTFIX version of the dependency evaluator.  When a
+  PUSH [PPI GUID] is encountered, a pointer to the GUID is stored on
+  the evaluation stack.  When that entry is poped from the evaluation
+  stack, the PPI is checked if it is installed.  This method allows
+  some time savings as not all PPIs must be checked for certain
+  operation types (AND, OR).
+
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param DependencyExpression   Pointer to a dependency expression.  The Grammar adheres to
+                                the BNF described above and is stored in postfix notation.
+
+  @retval TRUE      if it is a well-formed Grammar
+  @retval FALSE     if the dependency expression overflows the evaluation stack
+                    if the dependency expression underflows the evaluation stack
+                    if the dependency expression is not a well-formed Grammar.
+
+**/
+BOOLEAN
+PeimDispatchReadiness (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN VOID               *DependencyExpression
+  )
+{
+  DEPENDENCY_EXPRESSION_OPERAND  *Iterator;
+  EVAL_STACK_ENTRY               *StackPtr;
+  EVAL_STACK_ENTRY               EvalStack[MAX_GRAMMAR_SIZE];
+
+  Iterator  = DependencyExpression;
+
+  StackPtr = EvalStack;
+
+  while (TRUE) {
+
+    switch (*(Iterator++)) {
+      
+      //
+      // For performance reason we put the frequently used items in front of 
+      // the rarely used  items
+      //
+      
+      case (EFI_DEP_PUSH):
+        //
+        // Check to make sure the dependency grammar doesn't overflow the
+        // EvalStack on the push
+        //
+        if (StackPtr > &EvalStack[MAX_GRAMMAR_SIZE-1]) {
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Underflow Error)\n"));
+          return FALSE;
+        }
+
+        //
+        // Push the pointer to the PUSH opcode operator (pointer to PPI GUID)
+        // We will evaluate if the PPI is insalled on the POP operation.
+        //
+        StackPtr->Operator = (VOID *) Iterator;
+        Iterator = Iterator + sizeof (EFI_GUID);
+        DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = %a\n", StackPtr->Operator, IsPpiInstalled (PeiServices, StackPtr) ? "TRUE" : "FALSE"));
+        StackPtr++;
+        break;
+
+      case (EFI_DEP_AND):    
+      case (EFI_DEP_OR):     
+        if (*(Iterator - 1) == EFI_DEP_AND) {
+          DEBUG ((DEBUG_DISPATCH, "  AND\n"));
+        } else {
+          DEBUG ((DEBUG_DISPATCH, "  OR\n"));
+        }
+        //
+        // Check to make sure the dependency grammar doesn't underflow the
+        // EvalStack on the two POPs for the AND operation.  Don't need to
+        // check for the overflow on PUSHing the result since we already
+        // did two POPs.
+        //
+        if (StackPtr < &EvalStack[2]) {
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Underflow Error)\n"));
+          return FALSE;
+        }
+
+        //
+        // Evaluate the first POPed operator only. If the operand is
+        // EFI_DEP_AND and the POPed operator evaluates to FALSE, or the
+        // operand is EFI_DEP_OR and the POPed operator evaluates to TRUE,
+        // we don't need to check the second operator, and the result will be
+        // evaluation of the POPed operator. Otherwise, don't POP the second
+        // operator since it will now evaluate to the final result on the
+        // next operand that causes a POP.
+        // 
+        StackPtr--;
+        //
+        // Iterator has increased by 1 after we retrieve the operand, so here we 
+        // should get the value pointed by (Iterator - 1), in order to obtain the 
+        // same operand.
+        //
+        if (*(Iterator - 1) == EFI_DEP_AND) {
+          if (!(IsPpiInstalled (PeiServices, StackPtr))) {
+            (StackPtr-1)->Result = FALSE;
+            (StackPtr-1)->Operator = NULL;
+          }
+        } else {
+          if (IsPpiInstalled (PeiServices, StackPtr)) {
+            (StackPtr-1)->Result = TRUE;
+            (StackPtr-1)->Operator = NULL;
+          }
+        }
+        break;
+        
+      case (EFI_DEP_END):
+        DEBUG ((DEBUG_DISPATCH, "  END\n"));
+        StackPtr--;
+        //
+        // Check to make sure EvalStack is balanced.  If not, then there is
+        // an error in the dependency grammar, so return EFI_INVALID_PARAMETER.
+        //
+        if (StackPtr != &EvalStack[0]) {
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Underflow Error)\n"));
+          return FALSE;
+        }
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = %a\n", IsPpiInstalled (PeiServices, StackPtr) ? "TRUE" : "FALSE"));
+        return IsPpiInstalled (PeiServices, StackPtr);
+
+      case (EFI_DEP_NOT):    
+        DEBUG ((DEBUG_DISPATCH, "  NOT\n"));
+        //
+        // Check to make sure the dependency grammar doesn't underflow the
+        // EvalStack on the POP for the NOT operation.  Don't need to
+        // check for the overflow on PUSHing the result since we already
+        // did a POP.
+        //
+        if (StackPtr < &EvalStack[1]) {
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Underflow Error)\n"));
+          return FALSE;
+        }
+        (StackPtr-1)->Result = (BOOLEAN) !IsPpiInstalled (PeiServices, (StackPtr-1));
+        (StackPtr-1)->Operator = NULL;
+        break;
+
+      case (EFI_DEP_TRUE):
+      case (EFI_DEP_FALSE):
+        if (*(Iterator - 1) == EFI_DEP_TRUE) {
+          DEBUG ((DEBUG_DISPATCH, "  TRUE\n"));
+        } else {
+          DEBUG ((DEBUG_DISPATCH, "  FALSE\n"));
+        }
+        //
+        // Check to make sure the dependency grammar doesn't overflow the
+        // EvalStack on the push
+        //
+        if (StackPtr > &EvalStack[MAX_GRAMMAR_SIZE-1]) {
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Underflow Error)\n"));
+          return FALSE;
+        }
+        //
+        // Iterator has increased by 1 after we retrieve the operand, so here we 
+        // should get the value pointed by (Iterator - 1), in order to obtain the 
+        // same operand.
+        //
+        if (*(Iterator - 1) == EFI_DEP_TRUE) {
+          StackPtr->Result = TRUE;
+        } else {
+          StackPtr->Result = FALSE;
+        }
+        StackPtr->Operator = NULL;
+        StackPtr++;
+        break;
+
+      default:
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Invalid opcode)\n"));
+        //
+        // The grammar should never arrive here
+        //
+        return FALSE;
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.h b/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.h
new file mode 100644
index 0000000000..5021ce056b
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Dependency/Dependency.h
@@ -0,0 +1,32 @@
+/** @file
+  This module contains data specific to dependency expressions
+  and local function prototypes.
+  
+Copyright (c) 2006 - 2008, 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.             
+
+**/
+
+#ifndef _PEI_DEPENDENCY_H_
+#define _PEI_DEPENDENCY_H_
+
+
+#define MAX_GRAMMAR_SIZE  64
+
+//
+// type definitions
+//
+typedef UINT8 DEPENDENCY_EXPRESSION_OPERAND;
+
+typedef struct {
+  BOOLEAN                 Result;
+  VOID                    *Operator;
+} EVAL_STACK_ENTRY;
+
+#endif
diff --git a/Core/MdeModulePkg/Core/Pei/Dispatcher/Dispatcher.c b/Core/MdeModulePkg/Core/Pei/Dispatcher/Dispatcher.c
new file mode 100644
index 0000000000..e7e795d123
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Dispatcher/Dispatcher.c
@@ -0,0 +1,1345 @@
+/** @file
+  EFI PEI Core dispatch services
+  
+Copyright (c) 2006 - 2015, 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 "PeiMain.h"
+
+///
+/// temporary memory is filled with this initial value during SEC phase
+///
+#define INIT_CAR_VALUE 0x5AA55AA5
+
+/**
+
+  Discover all Peims and optional Apriori file in one FV. There is at most one
+  Apriori file in one FV.
+
+
+  @param Private          Pointer to the private data passed in from caller
+  @param CoreFileHandle   The instance of PEI_CORE_FV_HANDLE.
+
+**/
+VOID
+DiscoverPeimsAndOrderWithApriori (
+  IN  PEI_CORE_INSTANCE    *Private,
+  IN  PEI_CORE_FV_HANDLE   *CoreFileHandle
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_PEI_FILE_HANDLE                 FileHandle;
+  EFI_PEI_FILE_HANDLE                 AprioriFileHandle;
+  EFI_GUID                            *Apriori;
+  UINTN                               Index;
+  UINTN                               Index2;
+  UINTN                               PeimIndex;
+  UINTN                               PeimCount;
+  EFI_GUID                            *Guid;
+  EFI_PEI_FILE_HANDLE                 *TempFileHandles;
+  EFI_GUID                            *FileGuid;
+  EFI_PEI_FIRMWARE_VOLUME_PPI         *FvPpi;
+  EFI_FV_FILE_INFO                    FileInfo;
+  
+  FvPpi = CoreFileHandle->FvPpi;
+  
+  //
+  // Walk the FV and find all the PEIMs and the Apriori file.
+  //
+  AprioriFileHandle = NULL;
+  Private->CurrentFvFileHandles[0] = NULL;
+  Guid = NULL;
+  FileHandle = NULL;
+  TempFileHandles = Private->FileHandles;
+  FileGuid        = Private->FileGuid;
+
+  //
+  // If the current Fv has been scanned, directly get its cachable record.
+  //
+  if (Private->Fv[Private->CurrentPeimFvCount].ScanFv) {
+    CopyMem (Private->CurrentFvFileHandles, Private->Fv[Private->CurrentPeimFvCount].FvFileHandles, sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv));
+    return;
+  }
+
+  //
+  // Go ahead to scan this Fv, and cache FileHandles within it.
+  //
+  Status = EFI_NOT_FOUND;
+  for (PeimCount = 0; PeimCount <= PcdGet32 (PcdPeiCoreMaxPeimPerFv); PeimCount++) {
+    Status = FvPpi->FindFileByType (FvPpi, PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE, CoreFileHandle->FvHandle, &FileHandle);
+    if (Status != EFI_SUCCESS || PeimCount == PcdGet32 (PcdPeiCoreMaxPeimPerFv)) {
+      break;
+    }
+
+    Private->CurrentFvFileHandles[PeimCount] = FileHandle;
+  }
+
+  //
+  // Check whether the count of files exceeds the max support files in a FV image
+  // If more files are required in a FV image, PcdPeiCoreMaxPeimPerFv can be set to a larger value in DSC file.
+  //
+  ASSERT ((Status != EFI_SUCCESS) || (PeimCount < PcdGet32 (PcdPeiCoreMaxPeimPerFv)));
+
+  //
+  // Get Apriori File handle
+  //
+  Private->AprioriCount = 0;
+  Status = FvPpi->FindFileByName (FvPpi, &gPeiAprioriFileNameGuid, &CoreFileHandle->FvHandle, &AprioriFileHandle);
+  if (!EFI_ERROR(Status) && AprioriFileHandle != NULL) {
+    //
+    // Read the Apriori file
+    //
+    Status = FvPpi->FindSectionByType (FvPpi, EFI_SECTION_RAW, AprioriFileHandle, (VOID **) &Apriori);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Calculate the number of PEIMs in the A Priori list
+      //
+      Status = FvPpi->GetFileInfo (FvPpi, AprioriFileHandle, &FileInfo);
+      ASSERT_EFI_ERROR (Status);
+      Private->AprioriCount = FileInfo.BufferSize;
+      if (IS_SECTION2 (FileInfo.Buffer)) {
+        Private->AprioriCount -= sizeof (EFI_COMMON_SECTION_HEADER2);
+      } else {
+        Private->AprioriCount -= sizeof (EFI_COMMON_SECTION_HEADER);
+      }
+      Private->AprioriCount /= sizeof (EFI_GUID);
+
+      for (Index = 0; Index < PeimCount; Index++) {
+        //
+        // Make an array of file name guids that matches the FileHandle array so we can convert
+        // quickly from file name to file handle
+        //
+        Status = FvPpi->GetFileInfo (FvPpi, Private->CurrentFvFileHandles[Index], &FileInfo);
+        CopyMem (&FileGuid[Index], &FileInfo.FileName, sizeof(EFI_GUID));
+      }
+
+      //
+      // Walk through FileGuid array to find out who is invalid PEIM guid in Apriori file.
+      // Add available PEIMs in Apriori file into TempFileHandles array at first.
+      //
+      Index2 = 0;
+      for (Index = 0; Index2 < Private->AprioriCount; Index++) {
+        while (Index2 < Private->AprioriCount) {
+          Guid = ScanGuid (FileGuid, PeimCount * sizeof (EFI_GUID), &Apriori[Index2++]);
+          if (Guid != NULL) {
+            break;
+          }
+        }
+        if (Guid == NULL) {
+          break;
+        }
+        PeimIndex = ((UINTN)Guid - (UINTN)&FileGuid[0])/sizeof (EFI_GUID);
+        TempFileHandles[Index] = Private->CurrentFvFileHandles[PeimIndex];
+
+        //
+        // Since we have copied the file handle we can remove it from this list.
+        //
+        Private->CurrentFvFileHandles[PeimIndex] = NULL;
+      }
+
+      //
+      // Update valid Aprioricount
+      //
+      Private->AprioriCount = Index;
+
+      //
+      // Add in any PEIMs not in the Apriori file
+      //
+      for (;Index < PeimCount; Index++) {
+        for (Index2 = 0; Index2 < PeimCount; Index2++) {
+          if (Private->CurrentFvFileHandles[Index2] != NULL) {
+            TempFileHandles[Index] = Private->CurrentFvFileHandles[Index2];
+            Private->CurrentFvFileHandles[Index2] = NULL;
+            break;
+          }
+        }
+      }
+      //
+      //Index the end of array contains re-range Pei moudle.
+      //
+      TempFileHandles[Index] = NULL;
+
+      //
+      // Private->CurrentFvFileHandles is currently in PEIM in the FV order.
+      // We need to update it to start with files in the A Priori list and
+      // then the remaining files in PEIM order.
+      //
+      CopyMem (Private->CurrentFvFileHandles, TempFileHandles, sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv));
+    }
+  }
+  //
+  // Cache the current Fv File Handle. So that we don't have to scan the Fv again.
+  // Instead, we can retrieve the file handles within this Fv from cachable data.
+  //
+  Private->Fv[Private->CurrentPeimFvCount].ScanFv = TRUE;
+  CopyMem (Private->Fv[Private->CurrentPeimFvCount].FvFileHandles, Private->CurrentFvFileHandles, sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv));
+
+}
+
+//
+// This is the minimum memory required by DxeCore initialization. When LMFA feature enabled,
+// This part of memory still need reserved on the very top of memory so that the DXE Core could  
+// use these memory for data initialization. This macro should be sync with the same marco
+// defined in DXE Core.
+//
+#define MINIMUM_INITIAL_MEMORY_SIZE 0x10000
+/**
+  This function is to test if the memory range described in resource HOB is available or not. 
+  
+  This function should only be invoked when Loading Module at Fixed Address(LMFA) feature is enabled. Some platform may allocate the 
+  memory before PeiLoadFixAddressHook in invoked. so this function is to test if the memory range described by the input resource HOB is
+  available or not.
+
+  @param PrivateData         Pointer to the private data passed in from caller
+  @param ResourceHob         Pointer to a resource HOB which described the memory range described by the input resource HOB
+**/
+BOOLEAN
+PeiLoadFixAddressIsMemoryRangeAvailable (
+  IN PEI_CORE_INSTANCE                  *PrivateData,
+  IN EFI_HOB_RESOURCE_DESCRIPTOR        *ResourceHob
+  )
+{
+	EFI_HOB_MEMORY_ALLOCATION          *MemoryHob;
+	BOOLEAN                             IsAvailable;
+	EFI_PEI_HOB_POINTERS                Hob;
+	
+  IsAvailable = TRUE;
+	if (PrivateData == NULL || ResourceHob == NULL) {
+	  return FALSE;
+	}
+	//
+  // test if the memory range describe in the HOB is already allocated.
+  //
+  for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+    //                                                              
+    // See if this is a memory allocation HOB                     
+    //
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_MEMORY_ALLOCATION) { 
+      MemoryHob = Hob.MemoryAllocation;
+      if(MemoryHob->AllocDescriptor.MemoryBaseAddress == ResourceHob->PhysicalStart && 
+         MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength == ResourceHob->PhysicalStart + ResourceHob->ResourceLength) {
+         IsAvailable = FALSE;
+         break;  
+       }
+     }
+  }
+  
+  return IsAvailable;
+       
+}
+/**
+  Hook function for Loading Module at Fixed Address feature
+  
+  This function should only be invoked when Loading Module at Fixed Address(LMFA) feature is enabled. When feature is
+  configured as Load Modules at Fix Absolute Address, this function is to validate the top address assigned by user. When 
+  feature is configured as Load Modules at Fixed Offset, the functino is to find the top address which is TOLM-TSEG in general.  
+  And also the function will re-install PEI memory. 
+
+  @param PrivateData         Pointer to the private data passed in from caller
+
+**/
+VOID
+PeiLoadFixAddressHook(
+  IN PEI_CORE_INSTANCE           *PrivateData
+  )
+{
+  EFI_PHYSICAL_ADDRESS               TopLoadingAddress;
+  UINT64                             PeiMemorySize;
+  UINT64                             TotalReservedMemorySize;
+  UINT64                             MemoryRangeEnd;
+  EFI_PHYSICAL_ADDRESS               HighAddress; 
+  EFI_HOB_RESOURCE_DESCRIPTOR        *ResourceHob;
+  EFI_HOB_RESOURCE_DESCRIPTOR        *NextResourceHob;
+  EFI_HOB_RESOURCE_DESCRIPTOR        *CurrentResourceHob;
+  EFI_PEI_HOB_POINTERS               CurrentHob;
+  EFI_PEI_HOB_POINTERS               Hob;
+  EFI_PEI_HOB_POINTERS               NextHob;
+  EFI_HOB_MEMORY_ALLOCATION          *MemoryHob;
+  //
+  // Initialize Local Variables
+  //
+  CurrentResourceHob    = NULL;
+  ResourceHob           = NULL;
+  NextResourceHob       = NULL;
+  HighAddress           = 0;
+  TopLoadingAddress     = 0;
+  MemoryRangeEnd      = 0;
+  CurrentHob.Raw      = PrivateData->HobList.Raw;
+  PeiMemorySize = PrivateData->PhysicalMemoryLength;
+  //
+  // The top reserved memory include 3 parts: the topest range is for DXE core initialization with the size  MINIMUM_INITIAL_MEMORY_SIZE
+  // then RuntimeCodePage range and Boot time code range.
+  //  
+  TotalReservedMemorySize = MINIMUM_INITIAL_MEMORY_SIZE + EFI_PAGES_TO_SIZE(PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber));
+  TotalReservedMemorySize+= EFI_PAGES_TO_SIZE(PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber)) ;  
+  //
+  // PEI memory range lies below the top reserved memory
+  // 
+  TotalReservedMemorySize += PeiMemorySize;
+  
+  DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: PcdLoadFixAddressRuntimeCodePageNumber= 0x%x.\n", PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber)));
+  DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: PcdLoadFixAddressBootTimeCodePageNumber= 0x%x.\n", PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber)));
+  DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: PcdLoadFixAddressPeiCodePageNumber= 0x%x.\n", PcdGet32(PcdLoadFixAddressPeiCodePageNumber)));   
+  DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: Total Reserved Memory Size = 0x%lx.\n", TotalReservedMemorySize));
+  //
+  // Loop through the system memory typed hob to merge the adjacent memory range 
+  //
+  for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+    //                                                              
+    // See if this is a resource descriptor HOB                     
+    //
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+      
+      ResourceHob = Hob.ResourceDescriptor;  
+      //
+      // If range described in this hob is not system memory or heigher than MAX_ADDRESS, ignored.
+      //
+      if (ResourceHob->ResourceType != EFI_RESOURCE_SYSTEM_MEMORY ||
+          ResourceHob->PhysicalStart + ResourceHob->ResourceLength > MAX_ADDRESS)   {
+        continue;
+      }   
+      
+      for (NextHob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(NextHob); NextHob.Raw = GET_NEXT_HOB(NextHob)) {       
+        if (NextHob.Raw == Hob.Raw){
+          continue;
+        }  
+        //
+        // See if this is a resource descriptor HOB
+        //
+        if (GET_HOB_TYPE (NextHob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+      
+          NextResourceHob = NextHob.ResourceDescriptor;
+          //
+          // test if range described in this NextResourceHob is system memory and have the same attribute.
+          // Note: Here is a assumption that system memory should always be healthy even without test.
+          //    
+          if (NextResourceHob->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY &&
+             (((NextResourceHob->ResourceAttribute^ResourceHob->ResourceAttribute)&(~EFI_RESOURCE_ATTRIBUTE_TESTED)) == 0)){
+              
+              //
+              // See if the memory range described in ResourceHob and NextResourceHob is adjacent
+              //
+              if ((ResourceHob->PhysicalStart <= NextResourceHob->PhysicalStart && 
+                    ResourceHob->PhysicalStart + ResourceHob->ResourceLength >= NextResourceHob->PhysicalStart)|| 
+                  (ResourceHob->PhysicalStart >= NextResourceHob->PhysicalStart&&
+                     ResourceHob->PhysicalStart <= NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength)) {
+             
+                MemoryRangeEnd = ((ResourceHob->PhysicalStart + ResourceHob->ResourceLength)>(NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength)) ?
+                                     (ResourceHob->PhysicalStart + ResourceHob->ResourceLength):(NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength);
+          
+                ResourceHob->PhysicalStart = (ResourceHob->PhysicalStart < NextResourceHob->PhysicalStart) ? 
+                                                    ResourceHob->PhysicalStart : NextResourceHob->PhysicalStart;
+                
+               
+                ResourceHob->ResourceLength = (MemoryRangeEnd - ResourceHob->PhysicalStart);
+                
+                ResourceHob->ResourceAttribute = ResourceHob->ResourceAttribute & (~EFI_RESOURCE_ATTRIBUTE_TESTED);
+                //
+                // Delete the NextResourceHob by marking it as unused.
+                //
+                GET_HOB_TYPE (NextHob) = EFI_HOB_TYPE_UNUSED;
+                
+              }
+           }
+        } 
+      }
+    } 
+  }
+  //
+  // Some platform is already allocated pages before the HOB re-org. Here to build dedicated resource HOB to describe
+  //  the allocated memory range
+  //
+  for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+    //                                                              
+    // See if this is a memory allocation HOB                     
+    //
+    if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_MEMORY_ALLOCATION) {
+      MemoryHob = Hob.MemoryAllocation;
+      for (NextHob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(NextHob); NextHob.Raw = GET_NEXT_HOB(NextHob)) {
+        //
+        // See if this is a resource descriptor HOB
+        //
+        if (GET_HOB_TYPE (NextHob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+        	NextResourceHob = NextHob.ResourceDescriptor;
+          //
+          // If range described in this hob is not system memory or heigher than MAX_ADDRESS, ignored.
+          //
+          if (NextResourceHob->ResourceType != EFI_RESOURCE_SYSTEM_MEMORY || NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength > MAX_ADDRESS) {
+            continue;
+          }
+          //
+          // If the range describe in memory allocation HOB  belongs to the memroy range described by the resource hob
+          //          
+          if (MemoryHob->AllocDescriptor.MemoryBaseAddress >= NextResourceHob->PhysicalStart && 
+              MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength <= NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength) {
+             //
+             // Build seperate resource hob for this allocated range
+             //                     
+             if (MemoryHob->AllocDescriptor.MemoryBaseAddress > NextResourceHob->PhysicalStart) {
+               BuildResourceDescriptorHob (
+                 EFI_RESOURCE_SYSTEM_MEMORY,                       
+                 NextResourceHob->ResourceAttribute,
+                 NextResourceHob->PhysicalStart,                             
+                 (MemoryHob->AllocDescriptor.MemoryBaseAddress - NextResourceHob->PhysicalStart)      
+               );
+             }
+             if (MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength < NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength) {
+               BuildResourceDescriptorHob (
+                 EFI_RESOURCE_SYSTEM_MEMORY,                       
+                 NextResourceHob->ResourceAttribute,
+                 MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength,                            
+                 (NextResourceHob->PhysicalStart + NextResourceHob->ResourceLength -(MemoryHob->AllocDescriptor.MemoryBaseAddress + MemoryHob->AllocDescriptor.MemoryLength))    
+               );
+             }
+             NextResourceHob->PhysicalStart = MemoryHob->AllocDescriptor.MemoryBaseAddress;
+             NextResourceHob->ResourceLength = MemoryHob->AllocDescriptor.MemoryLength;
+             break;
+          }
+        }
+      }
+    }
+  }
+
+  //
+  // Try to find and validate the TOP address.
+  //  
+  if ((INT64)PcdGet64(PcdLoadModuleAtFixAddressEnable) > 0 ) {
+    //
+    // The LMFA feature is enabled as load module at fixed absolute address.
+    //
+    TopLoadingAddress = (EFI_PHYSICAL_ADDRESS)PcdGet64(PcdLoadModuleAtFixAddressEnable);
+    DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: Loading module at fixed absolute address.\n"));
+    //
+    // validate the Address. Loop the resource descriptor HOB to make sure the address is in valid memory range
+    //
+    if ((TopLoadingAddress & EFI_PAGE_MASK) != 0) {
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED ERROR:Top Address 0x%lx is invalid since top address should be page align. \n", TopLoadingAddress)); 
+      ASSERT (FALSE);    
+    }
+    //
+    // Search for a memory region that is below MAX_ADDRESS and in which TopLoadingAddress lies 
+    //
+    for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+      //
+      // See if this is a resource descriptor HOB
+      //
+      if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+
+        ResourceHob = Hob.ResourceDescriptor;
+        //
+        // See if this resource descrior HOB describes tested system memory below MAX_ADDRESS
+        //    
+        if (ResourceHob->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY &&
+            ResourceHob->PhysicalStart + ResourceHob->ResourceLength <= MAX_ADDRESS) {
+            //
+            // See if Top address specified by user is valid.
+            //
+            if (ResourceHob->PhysicalStart + TotalReservedMemorySize < TopLoadingAddress && 
+                (ResourceHob->PhysicalStart + ResourceHob->ResourceLength - MINIMUM_INITIAL_MEMORY_SIZE) >= TopLoadingAddress && 
+                PeiLoadFixAddressIsMemoryRangeAvailable(PrivateData, ResourceHob)) {
+              CurrentResourceHob = ResourceHob; 
+              CurrentHob = Hob;
+              break;
+           }
+        }
+      }  
+    }  
+    if (CurrentResourceHob != NULL) {
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO:Top Address 0x%lx is valid \n",  TopLoadingAddress));
+      TopLoadingAddress += MINIMUM_INITIAL_MEMORY_SIZE; 
+    } else {
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED ERROR:Top Address 0x%lx is invalid \n",  TopLoadingAddress)); 
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED ERROR:The recommended Top Address for the platform is: \n")); 
+      //
+      // Print the recomended Top address range.
+      // 
+      for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+        //
+        // See if this is a resource descriptor HOB
+        //
+        if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+        
+          ResourceHob = Hob.ResourceDescriptor;
+          //
+          // See if this resource descrior HOB describes tested system memory below MAX_ADDRESS
+          //    
+          if (ResourceHob->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY &&
+              ResourceHob->PhysicalStart + ResourceHob->ResourceLength <= MAX_ADDRESS) {
+              //
+              // See if Top address specified by user is valid.
+              //
+              if (ResourceHob->ResourceLength > TotalReservedMemorySize && PeiLoadFixAddressIsMemoryRangeAvailable(PrivateData, ResourceHob)) {
+                 DEBUG ((EFI_D_INFO, "(0x%lx, 0x%lx)\n",  
+                          (ResourceHob->PhysicalStart + TotalReservedMemorySize -MINIMUM_INITIAL_MEMORY_SIZE), 
+                          (ResourceHob->PhysicalStart + ResourceHob->ResourceLength -MINIMUM_INITIAL_MEMORY_SIZE) 
+                        )); 
+              }
+          }
+        }
+      }  
+      //
+      // Assert here 
+      //
+      ASSERT (FALSE);   
+      return;   
+    }     
+  } else {
+    //
+    // The LMFA feature is enabled as load module at fixed offset relative to TOLM
+    // Parse the Hob list to find the topest available memory. Generally it is (TOLM - TSEG)
+    //
+    //
+    // Search for a tested memory region that is below MAX_ADDRESS
+    //
+    for (Hob.Raw = PrivateData->HobList.Raw; !END_OF_HOB_LIST(Hob); Hob.Raw = GET_NEXT_HOB(Hob)) {
+      //
+      // See if this is a resource descriptor HOB 
+      //
+      if (GET_HOB_TYPE (Hob) == EFI_HOB_TYPE_RESOURCE_DESCRIPTOR) {
+        
+        ResourceHob = Hob.ResourceDescriptor;                                                                                                                                                                                                                               
+        //
+        // See if this resource descrior HOB describes tested system memory below MAX_ADDRESS
+        //
+        if (ResourceHob->ResourceType == EFI_RESOURCE_SYSTEM_MEMORY && 
+            ResourceHob->PhysicalStart + ResourceHob->ResourceLength <= MAX_ADDRESS &&
+            ResourceHob->ResourceLength > TotalReservedMemorySize && PeiLoadFixAddressIsMemoryRangeAvailable(PrivateData, ResourceHob)) {
+          //
+          // See if this is the highest largest system memory region below MaxAddress
+          //
+          if (ResourceHob->PhysicalStart > HighAddress) {
+             CurrentResourceHob = ResourceHob;
+             CurrentHob = Hob;
+             HighAddress = CurrentResourceHob->PhysicalStart;
+          }
+        }
+      }  
+    }
+    if (CurrentResourceHob == NULL) {
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED ERROR:The System Memory is too small\n")); 
+      //
+      // Assert here 
+      //
+      ASSERT (FALSE);
+      return;  
+    } else {
+      TopLoadingAddress = CurrentResourceHob->PhysicalStart + CurrentResourceHob->ResourceLength ; 
+    }         
+  }
+  
+  if (CurrentResourceHob != NULL) {
+    //
+    // rebuild resource HOB for PEI memmory and reserved memory
+    //
+    BuildResourceDescriptorHob (
+      EFI_RESOURCE_SYSTEM_MEMORY,                       
+      (
+      EFI_RESOURCE_ATTRIBUTE_PRESENT |
+      EFI_RESOURCE_ATTRIBUTE_INITIALIZED |
+      EFI_RESOURCE_ATTRIBUTE_TESTED |
+      EFI_RESOURCE_ATTRIBUTE_UNCACHEABLE |
+      EFI_RESOURCE_ATTRIBUTE_WRITE_COMBINEABLE |
+      EFI_RESOURCE_ATTRIBUTE_WRITE_THROUGH_CACHEABLE |
+      EFI_RESOURCE_ATTRIBUTE_WRITE_BACK_CACHEABLE
+      ),
+      (TopLoadingAddress - TotalReservedMemorySize),                             
+      TotalReservedMemorySize     
+    );
+    //
+    // rebuild resource for the remain memory if necessary
+    //
+    if (CurrentResourceHob->PhysicalStart < TopLoadingAddress - TotalReservedMemorySize) {
+      BuildResourceDescriptorHob (
+        EFI_RESOURCE_SYSTEM_MEMORY,                       
+        (
+         EFI_RESOURCE_ATTRIBUTE_PRESENT |
+         EFI_RESOURCE_ATTRIBUTE_INITIALIZED |
+         EFI_RESOURCE_ATTRIBUTE_UNCACHEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_COMBINEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_THROUGH_CACHEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_BACK_CACHEABLE
+         ),
+         CurrentResourceHob->PhysicalStart,                             
+         (TopLoadingAddress - TotalReservedMemorySize - CurrentResourceHob->PhysicalStart)      
+       );
+    }
+    if (CurrentResourceHob->PhysicalStart + CurrentResourceHob->ResourceLength  > TopLoadingAddress ) {
+      BuildResourceDescriptorHob (
+        EFI_RESOURCE_SYSTEM_MEMORY,                     
+        (
+         EFI_RESOURCE_ATTRIBUTE_PRESENT |
+         EFI_RESOURCE_ATTRIBUTE_INITIALIZED |
+         EFI_RESOURCE_ATTRIBUTE_UNCACHEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_COMBINEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_THROUGH_CACHEABLE |
+         EFI_RESOURCE_ATTRIBUTE_WRITE_BACK_CACHEABLE
+         ),
+         TopLoadingAddress,                            
+         (CurrentResourceHob->PhysicalStart + CurrentResourceHob->ResourceLength  - TopLoadingAddress)     
+       );
+    }
+    //
+    // Delete CurrentHob by marking it as unused since the the memory range described by is rebuilt.
+    //
+    GET_HOB_TYPE (CurrentHob) = EFI_HOB_TYPE_UNUSED;         
+  }
+
+  //
+  // Cache the top address for Loading Module at Fixed Address feature
+  //
+  PrivateData->LoadModuleAtFixAddressTopAddress = TopLoadingAddress - MINIMUM_INITIAL_MEMORY_SIZE;
+  DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: Top address = 0x%lx\n",  PrivateData->LoadModuleAtFixAddressTopAddress)); 
+  //
+  // reinstall the PEI memory relative to TopLoadingAddress
+  //
+  PrivateData->PhysicalMemoryBegin   = TopLoadingAddress - TotalReservedMemorySize;
+  PrivateData->FreePhysicalMemoryTop = PrivateData->PhysicalMemoryBegin + PeiMemorySize;
+}
+
+/**
+  This routine is invoked in switch stack as PeiCore Entry.
+
+  @param SecCoreData     Points to a data structure containing information about the PEI core's operating
+                         environment, such as the size and location of temporary RAM, the stack location and
+                         the BFV location.
+  @param Private         Pointer to old core data that is used to initialize the
+                         core's data areas.
+**/
+VOID
+EFIAPI
+PeiCoreEntry (
+  IN CONST EFI_SEC_PEI_HAND_OFF    *SecCoreData,
+  IN PEI_CORE_INSTANCE             *Private
+  )
+{
+  //
+  // Entry PEI Phase 2
+  //
+  PeiCore (SecCoreData, NULL, Private);
+}
+
+/**
+  Check SwitchStackSignal and switch stack if SwitchStackSignal is TRUE.
+
+  @param[in] SecCoreData    Points to a data structure containing information about the PEI core's operating
+                            environment, such as the size and location of temporary RAM, the stack location and
+                            the BFV location.
+  @param[in] Private        Pointer to the private data passed in from caller.
+
+**/
+VOID
+PeiCheckAndSwitchStack (
+  IN CONST EFI_SEC_PEI_HAND_OFF         *SecCoreData,
+  IN PEI_CORE_INSTANCE                  *Private
+  )
+{
+  VOID                                  *LoadFixPeiCodeBegin;
+  EFI_STATUS                            Status;
+  CONST EFI_PEI_SERVICES                **PeiServices;
+  UINT64                                NewStackSize;
+  EFI_PHYSICAL_ADDRESS                  TopOfOldStack;
+  EFI_PHYSICAL_ADDRESS                  TopOfNewStack;
+  UINTN                                 StackOffset;
+  BOOLEAN                               StackOffsetPositive;
+  EFI_PHYSICAL_ADDRESS                  TemporaryRamBase;
+  UINTN                                 TemporaryRamSize;
+  UINTN                                 TemporaryStackSize;
+  VOID                                  *TemporaryStackBase;
+  UINTN                                 PeiTemporaryRamSize;
+  VOID                                  *PeiTemporaryRamBase;
+  EFI_PEI_TEMPORARY_RAM_SUPPORT_PPI     *TemporaryRamSupportPpi;
+  EFI_PHYSICAL_ADDRESS                  BaseOfNewHeap;
+  EFI_PHYSICAL_ADDRESS                  HoleMemBase;
+  UINTN                                 HoleMemSize;
+  UINTN                                 HeapTemporaryRamSize;
+  EFI_PHYSICAL_ADDRESS                  TempBase1;
+  UINTN                                 TempSize1;
+  EFI_PHYSICAL_ADDRESS                  TempBase2;
+  UINTN                                 TempSize2;
+  UINTN                                 Index;
+
+  PeiServices = (CONST EFI_PEI_SERVICES **) &Private->Ps;
+
+  if (Private->SwitchStackSignal) {
+    //
+    // Before switch stack from temporary memory to permenent memory, calculate the heap and stack
+    // usage in temporary memory for debuging.
+    //
+    DEBUG_CODE_BEGIN ();
+      UINT32  *StackPointer;
+
+      for (StackPointer = (UINT32*)SecCoreData->StackBase;
+           (StackPointer < (UINT32*)((UINTN)SecCoreData->StackBase + SecCoreData->StackSize)) \
+           && (*StackPointer == INIT_CAR_VALUE);
+           StackPointer ++);
+
+        DEBUG ((EFI_D_INFO, "Temp Stack : BaseAddress=0x%p Length=0x%X\n", SecCoreData->StackBase, (UINT32)SecCoreData->StackSize));
+        DEBUG ((EFI_D_INFO, "Temp Heap  : BaseAddress=0x%p Length=0x%X\n", Private->HobList.Raw, (UINT32)((UINTN) Private->HobList.HandoffInformationTable->EfiFreeMemoryBottom - (UINTN) Private->HobList.Raw)));
+        DEBUG ((EFI_D_INFO, "Total temporary memory:    %d bytes.\n", (UINT32)SecCoreData->TemporaryRamSize));
+        DEBUG ((EFI_D_INFO, "  temporary memory stack ever used: %d bytes.\n",
+               (UINT32)(SecCoreData->StackSize - ((UINTN) StackPointer - (UINTN)SecCoreData->StackBase))
+              ));
+        DEBUG ((EFI_D_INFO, "  temporary memory heap used:       %d bytes.\n",
+               (UINT32)((UINTN)Private->HobList.HandoffInformationTable->EfiFreeMemoryBottom - (UINTN)Private->HobList.Raw)
+              ));
+    DEBUG_CODE_END ();
+
+    if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0 && (Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME)) {
+      //
+      // Loading Module at Fixed Address is enabled
+      //
+      PeiLoadFixAddressHook (Private);
+
+      //
+      // If Loading Module at Fixed Address is enabled, Allocating memory range for Pei code range.
+      //
+      LoadFixPeiCodeBegin = AllocatePages((UINTN)PcdGet32(PcdLoadFixAddressPeiCodePageNumber));
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: PeiCodeBegin = 0x%lX, PeiCodeTop= 0x%lX\n", (UINT64)(UINTN)LoadFixPeiCodeBegin, (UINT64)((UINTN)LoadFixPeiCodeBegin + PcdGet32(PcdLoadFixAddressPeiCodePageNumber) * EFI_PAGE_SIZE)));
+    }
+
+    //
+    // Reserve the size of new stack at bottom of physical memory
+    //
+    // The size of new stack in permenent memory must be the same size 
+    // or larger than the size of old stack in temporary memory.
+    // But if new stack is smaller than the size of old stack, we also reserve
+    // the size of old stack at bottom of permenent memory.
+    //
+    NewStackSize = RShiftU64 (Private->PhysicalMemoryLength, 1);
+    NewStackSize = ALIGN_VALUE (NewStackSize, EFI_PAGE_SIZE);
+    NewStackSize = MIN (PcdGet32(PcdPeiCoreMaxPeiStackSize), NewStackSize);
+    DEBUG ((EFI_D_INFO, "Old Stack size %d, New stack size %d\n", (UINT32)SecCoreData->StackSize, (UINT32)NewStackSize));
+    ASSERT (NewStackSize >= SecCoreData->StackSize);
+
+    //
+    // Calculate stack offset and heap offset between temporary memory and new permement 
+    // memory seperately.
+    //
+    TopOfOldStack = (UINTN)SecCoreData->StackBase + SecCoreData->StackSize;
+    TopOfNewStack = Private->PhysicalMemoryBegin + NewStackSize;
+    if (TopOfNewStack >= TopOfOldStack) {
+      StackOffsetPositive = TRUE;
+      StackOffset = (UINTN)(TopOfNewStack - TopOfOldStack);
+    } else {
+      StackOffsetPositive = FALSE;
+      StackOffset = (UINTN)(TopOfOldStack - TopOfNewStack);
+    }
+    Private->StackOffsetPositive = StackOffsetPositive;
+    Private->StackOffset = StackOffset;
+
+    //
+    // Build Stack HOB that describes the permanent memory stack
+    //
+    DEBUG ((EFI_D_INFO, "Stack Hob: BaseAddress=0x%lX Length=0x%lX\n", TopOfNewStack - NewStackSize, NewStackSize));
+    BuildStackHob (TopOfNewStack - NewStackSize, NewStackSize);
+
+    //
+    // Cache information from SecCoreData into locals before SecCoreData is converted to a permanent memory address
+    //
+    TemporaryRamBase    = (EFI_PHYSICAL_ADDRESS)(UINTN)SecCoreData->TemporaryRamBase;
+    TemporaryRamSize    = SecCoreData->TemporaryRamSize;
+    TemporaryStackSize  = SecCoreData->StackSize;
+    TemporaryStackBase  = SecCoreData->StackBase;
+    PeiTemporaryRamSize = SecCoreData->PeiTemporaryRamSize;
+    PeiTemporaryRamBase = SecCoreData->PeiTemporaryRamBase;
+
+    //
+    // TemporaryRamSupportPpi is produced by platform's SEC
+    //
+    Status = PeiServicesLocatePpi (
+               &gEfiTemporaryRamSupportPpiGuid,
+               0,
+               NULL,
+               (VOID**)&TemporaryRamSupportPpi
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Heap Offset
+      //
+      BaseOfNewHeap = TopOfNewStack;
+      if (BaseOfNewHeap >= (UINTN)SecCoreData->PeiTemporaryRamBase) {
+        Private->HeapOffsetPositive = TRUE;
+        Private->HeapOffset = (UINTN)(BaseOfNewHeap - (UINTN)SecCoreData->PeiTemporaryRamBase);
+      } else {
+        Private->HeapOffsetPositive = FALSE;
+        Private->HeapOffset = (UINTN)((UINTN)SecCoreData->PeiTemporaryRamBase - BaseOfNewHeap);
+      }
+
+      DEBUG ((EFI_D_INFO, "Heap Offset = 0x%lX Stack Offset = 0x%lX\n", (UINT64) Private->HeapOffset, (UINT64) Private->StackOffset));
+
+      //
+      // Calculate new HandOffTable and PrivateData address in permanent memory's stack
+      //
+      if (StackOffsetPositive) {
+        SecCoreData = (CONST EFI_SEC_PEI_HAND_OFF *)((UINTN)(VOID *)SecCoreData + StackOffset);
+        Private = (PEI_CORE_INSTANCE *)((UINTN)(VOID *)Private + StackOffset);
+      } else {
+        SecCoreData = (CONST EFI_SEC_PEI_HAND_OFF *)((UINTN)(VOID *)SecCoreData - StackOffset);
+        Private = (PEI_CORE_INSTANCE *)((UINTN)(VOID *)Private - StackOffset);
+      }
+
+      //
+      // Temporary Ram Support PPI is provided by platform, it will copy 
+      // temporary memory to permenent memory and do stack switching.
+      // After invoking Temporary Ram Support PPI, the following code's 
+      // stack is in permanent memory.
+      //
+      TemporaryRamSupportPpi->TemporaryRamMigration (
+                                PeiServices,
+                                TemporaryRamBase,
+                                (EFI_PHYSICAL_ADDRESS)(UINTN)(TopOfNewStack - TemporaryStackSize),
+                                TemporaryRamSize
+                                );
+
+      //
+      // Entry PEI Phase 2
+      //
+      PeiCore (SecCoreData, NULL, Private);
+    } else {
+      //
+      // Migrate the PEI Services Table pointer from temporary RAM to permanent RAM.
+      //
+      MigratePeiServicesTablePointer ();
+                
+      //
+      // Heap Offset
+      //
+      BaseOfNewHeap = TopOfNewStack;
+      HoleMemBase   = TopOfNewStack;
+      HoleMemSize   = TemporaryRamSize - PeiTemporaryRamSize - TemporaryStackSize;
+      if (HoleMemSize != 0) {
+        //
+        // Make sure HOB List start address is 8 byte alignment.
+        //
+        BaseOfNewHeap = ALIGN_VALUE (BaseOfNewHeap + HoleMemSize, 8);
+      }
+      if (BaseOfNewHeap >= (UINTN)SecCoreData->PeiTemporaryRamBase) {
+        Private->HeapOffsetPositive = TRUE;
+        Private->HeapOffset = (UINTN)(BaseOfNewHeap - (UINTN)SecCoreData->PeiTemporaryRamBase);
+      } else {
+        Private->HeapOffsetPositive = FALSE;
+        Private->HeapOffset = (UINTN)((UINTN)SecCoreData->PeiTemporaryRamBase - BaseOfNewHeap);
+      }
+
+      DEBUG ((EFI_D_INFO, "Heap Offset = 0x%lX Stack Offset = 0x%lX\n", (UINT64) Private->HeapOffset, (UINT64) Private->StackOffset));
+
+      //
+      // Migrate Heap
+      //
+      HeapTemporaryRamSize = (UINTN) (Private->HobList.HandoffInformationTable->EfiFreeMemoryBottom - Private->HobList.HandoffInformationTable->EfiMemoryBottom);
+      ASSERT (BaseOfNewHeap + HeapTemporaryRamSize <= Private->FreePhysicalMemoryTop);
+      CopyMem ((UINT8 *) (UINTN) BaseOfNewHeap, (UINT8 *) PeiTemporaryRamBase, HeapTemporaryRamSize);
+
+      //
+      // Migrate Stack
+      //
+      CopyMem ((UINT8 *) (UINTN) (TopOfNewStack - TemporaryStackSize), TemporaryStackBase, TemporaryStackSize);
+
+      //
+      // Copy Hole Range Data
+      // Convert PPI from Hole. 
+      //
+      if (HoleMemSize != 0) {
+        //
+        // Prepare Hole
+        //
+        if (PeiTemporaryRamBase < TemporaryStackBase) {
+          TempBase1 = (EFI_PHYSICAL_ADDRESS) (UINTN) PeiTemporaryRamBase;
+          TempSize1 = PeiTemporaryRamSize;
+          TempBase2 = (EFI_PHYSICAL_ADDRESS) (UINTN) TemporaryStackBase;
+          TempSize2 = TemporaryStackSize;
+        } else {
+          TempBase1 = (EFI_PHYSICAL_ADDRESS) (UINTN) TemporaryStackBase;
+          TempSize1 = TemporaryStackSize;
+          TempBase2 =(EFI_PHYSICAL_ADDRESS) (UINTN) PeiTemporaryRamBase;
+          TempSize2 = PeiTemporaryRamSize;
+        }
+        if (TemporaryRamBase < TempBase1) {
+          Private->HoleData[0].Base = TemporaryRamBase;
+          Private->HoleData[0].Size = (UINTN) (TempBase1 - TemporaryRamBase);
+        }
+        if (TempBase1 + TempSize1 < TempBase2) {
+          Private->HoleData[1].Base = TempBase1 + TempSize1;
+          Private->HoleData[1].Size = (UINTN) (TempBase2 - TempBase1 - TempSize1);
+        }
+        if (TempBase2 + TempSize2 < TemporaryRamBase + TemporaryRamSize) {
+          Private->HoleData[2].Base = TempBase2 + TempSize2;
+          Private->HoleData[2].Size = (UINTN) (TemporaryRamBase + TemporaryRamSize - TempBase2 - TempSize2);
+        }
+
+        //
+        // Copy Hole Range data.
+        //
+        for (Index = 0; Index < HOLE_MAX_NUMBER; Index ++) {
+          if (Private->HoleData[Index].Size > 0) {
+            if (HoleMemBase > Private->HoleData[Index].Base) {
+              Private->HoleData[Index].OffsetPositive = TRUE;
+              Private->HoleData[Index].Offset = (UINTN) (HoleMemBase - Private->HoleData[Index].Base);
+            } else {
+              Private->HoleData[Index].OffsetPositive = FALSE;
+              Private->HoleData[Index].Offset = (UINTN) (Private->HoleData[Index].Base - HoleMemBase);
+            }
+            CopyMem ((VOID *) (UINTN) HoleMemBase, (VOID *) (UINTN) Private->HoleData[Index].Base, Private->HoleData[Index].Size);
+            HoleMemBase = HoleMemBase + Private->HoleData[Index].Size;
+          }
+        }
+      }
+
+      //
+      // Switch new stack
+      //
+      SwitchStack (
+        (SWITCH_STACK_ENTRY_POINT)(UINTN)PeiCoreEntry,
+        (VOID *) SecCoreData,
+        (VOID *) Private,
+        (VOID *) (UINTN) TopOfNewStack
+        );
+    }
+
+    //
+    // Code should not come here
+    //
+    ASSERT (FALSE);
+  }
+}
+
+/**
+  Conduct PEIM dispatch.
+
+  @param SecCoreData     Points to a data structure containing information about the PEI core's operating
+                         environment, such as the size and location of temporary RAM, the stack location and
+                         the BFV location.
+  @param Private         Pointer to the private data passed in from caller
+
+**/
+VOID
+PeiDispatcher (
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *Private
+  )
+{
+  EFI_STATUS                          Status;
+  UINT32                              Index1;
+  UINT32                              Index2;
+  CONST EFI_PEI_SERVICES              **PeiServices;
+  EFI_PEI_FILE_HANDLE                 PeimFileHandle;
+  UINTN                               FvCount;
+  UINTN                               PeimCount;
+  UINT32                              AuthenticationState;
+  EFI_PHYSICAL_ADDRESS                EntryPoint;
+  EFI_PEIM_ENTRY_POINT2               PeimEntryPoint;
+  UINTN                               SaveCurrentPeimCount;
+  UINTN                               SaveCurrentFvCount;
+  EFI_PEI_FILE_HANDLE                 SaveCurrentFileHandle;
+  EFI_FV_FILE_INFO                    FvFileInfo;
+  PEI_CORE_FV_HANDLE                  *CoreFvHandle;
+  
+  PeiServices = (CONST EFI_PEI_SERVICES **) &Private->Ps;
+  PeimEntryPoint = NULL;
+  PeimFileHandle = NULL;
+  EntryPoint     = 0;
+
+  if ((Private->PeiMemoryInstalled) && (Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME || PcdGetBool (PcdShadowPeimOnS3Boot))) {
+    //
+    // Once real memory is available, shadow the RegisterForShadow modules. And meanwhile
+    // update the modules' status from PEIM_STATE_REGISITER_FOR_SHADOW to PEIM_STATE_DONE.
+    //
+    SaveCurrentPeimCount  = Private->CurrentPeimCount;
+    SaveCurrentFvCount    = Private->CurrentPeimFvCount;
+    SaveCurrentFileHandle =  Private->CurrentFileHandle;
+
+    for (Index1 = 0; Index1 <= SaveCurrentFvCount; Index1++) {
+      for (Index2 = 0; (Index2 < PcdGet32 (PcdPeiCoreMaxPeimPerFv)) && (Private->Fv[Index1].FvFileHandles[Index2] != NULL); Index2++) {
+        if (Private->Fv[Index1].PeimState[Index2] == PEIM_STATE_REGISITER_FOR_SHADOW) {
+          PeimFileHandle = Private->Fv[Index1].FvFileHandles[Index2];
+          Private->CurrentFileHandle   = PeimFileHandle;
+          Private->CurrentPeimFvCount  = Index1;
+          Private->CurrentPeimCount    = Index2;
+          Status = PeiLoadImage (
+                    (CONST EFI_PEI_SERVICES **) &Private->Ps,
+                    PeimFileHandle,
+                    PEIM_STATE_REGISITER_FOR_SHADOW,
+                    &EntryPoint,
+                    &AuthenticationState
+                    );
+          if (Status == EFI_SUCCESS) {
+            //
+            // PEIM_STATE_REGISITER_FOR_SHADOW move to PEIM_STATE_DONE
+            //
+            Private->Fv[Index1].PeimState[Index2]++;
+            //
+            // Call the PEIM entry point
+            //
+            PeimEntryPoint = (EFI_PEIM_ENTRY_POINT2)(UINTN)EntryPoint;
+
+            PERF_START (PeimFileHandle, "PEIM", NULL, 0);
+            PeimEntryPoint(PeimFileHandle, (const EFI_PEI_SERVICES **) &Private->Ps);
+            PERF_END (PeimFileHandle, "PEIM", NULL, 0);
+          }
+
+          //
+          // Process the Notify list and dispatch any notifies for
+          // newly installed PPIs.
+          //
+          ProcessNotifyList (Private);
+        }
+      }
+    }
+    Private->CurrentFileHandle  = SaveCurrentFileHandle;
+    Private->CurrentPeimFvCount = SaveCurrentFvCount;
+    Private->CurrentPeimCount   = SaveCurrentPeimCount;
+  }
+
+  //
+  // This is the main dispatch loop.  It will search known FVs for PEIMs and
+  // attempt to dispatch them.  If any PEIM gets dispatched through a single
+  // pass of the dispatcher, it will start over from the Bfv again to see
+  // if any new PEIMs dependencies got satisfied.  With a well ordered
+  // FV where PEIMs are found in the order their dependencies are also
+  // satisfied, this dipatcher should run only once.
+  //
+  do {
+    //
+    // In case that reenter PeiCore happens, the last pass record is still available.   
+    //
+    if (!Private->PeimDispatcherReenter) {
+      Private->PeimNeedingDispatch      = FALSE;
+      Private->PeimDispatchOnThisPass   = FALSE;
+    } else {
+      Private->PeimDispatcherReenter    = FALSE;
+    }
+    
+    for (FvCount = Private->CurrentPeimFvCount; FvCount < Private->FvCount; FvCount++) {
+      CoreFvHandle = FindNextCoreFvHandle (Private, FvCount);
+      ASSERT (CoreFvHandle != NULL);
+      
+      //
+      // If the FV has corresponding EFI_PEI_FIRMWARE_VOLUME_PPI instance, then dispatch it.
+      //
+      if (CoreFvHandle->FvPpi == NULL) {
+        continue;
+      }
+      
+      Private->CurrentPeimFvCount = FvCount;
+
+      if (Private->CurrentPeimCount == 0) {
+        //
+        // When going through each FV, at first, search Apriori file to
+        // reorder all PEIMs to ensure the PEIMs in Apriori file to get
+        // dispatch at first.
+        //
+        DiscoverPeimsAndOrderWithApriori (Private, CoreFvHandle);
+      }
+
+      //
+      // Start to dispatch all modules within the current Fv.
+      //
+      for (PeimCount = Private->CurrentPeimCount;
+           (PeimCount < PcdGet32 (PcdPeiCoreMaxPeimPerFv)) && (Private->CurrentFvFileHandles[PeimCount] != NULL);
+           PeimCount++) {
+        Private->CurrentPeimCount  = PeimCount;
+        PeimFileHandle = Private->CurrentFileHandle = Private->CurrentFvFileHandles[PeimCount];
+
+        if (Private->Fv[FvCount].PeimState[PeimCount] == PEIM_STATE_NOT_DISPATCHED) {
+          if (!DepexSatisfied (Private, PeimFileHandle, PeimCount)) {
+            Private->PeimNeedingDispatch = TRUE;
+          } else {
+            Status = CoreFvHandle->FvPpi->GetFileInfo (CoreFvHandle->FvPpi, PeimFileHandle, &FvFileInfo);
+            ASSERT_EFI_ERROR (Status);
+            if (FvFileInfo.FileType == EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE) {
+              //
+              // For Fv type file, Produce new FvInfo PPI and FV hob
+              //
+              Status = ProcessFvFile (Private, &Private->Fv[FvCount], PeimFileHandle);
+              if (Status == EFI_SUCCESS) {
+                //
+                // PEIM_STATE_NOT_DISPATCHED move to PEIM_STATE_DISPATCHED
+                //
+                Private->Fv[FvCount].PeimState[PeimCount]++;
+                Private->PeimDispatchOnThisPass = TRUE;
+              } else {
+                //
+                // The related GuidedSectionExtraction/Decompress PPI for the
+                // encapsulated FV image section may be installed in the rest
+                // of this do-while loop, so need to make another pass.
+                //
+                Private->PeimNeedingDispatch = TRUE;
+              }
+            } else {
+              //
+              // For PEIM driver, Load its entry point
+              //
+              Status = PeiLoadImage (
+                         PeiServices,
+                         PeimFileHandle,
+                         PEIM_STATE_NOT_DISPATCHED,
+                         &EntryPoint,
+                         &AuthenticationState
+                         );
+              if (Status == EFI_SUCCESS) {
+                //
+                // The PEIM has its dependencies satisfied, and its entry point
+                // has been found, so invoke it.
+                //
+                PERF_START (PeimFileHandle, "PEIM", NULL, 0);
+
+                REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+                  EFI_PROGRESS_CODE,
+                  (EFI_SOFTWARE_PEI_CORE | EFI_SW_PC_INIT_BEGIN),
+                  (VOID *)(&PeimFileHandle),
+                  sizeof (PeimFileHandle)
+                  );
+
+                Status = VerifyPeim (Private, CoreFvHandle->FvHandle, PeimFileHandle, AuthenticationState);
+                if (Status != EFI_SECURITY_VIOLATION) {
+                  //
+                  // PEIM_STATE_NOT_DISPATCHED move to PEIM_STATE_DISPATCHED
+                  //
+                  Private->Fv[FvCount].PeimState[PeimCount]++;
+                  //
+                  // Call the PEIM entry point for PEIM driver
+                  //
+                  PeimEntryPoint = (EFI_PEIM_ENTRY_POINT2)(UINTN)EntryPoint;
+                  PeimEntryPoint (PeimFileHandle, (const EFI_PEI_SERVICES **) PeiServices);
+                  Private->PeimDispatchOnThisPass = TRUE;
+                }
+
+                REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+                  EFI_PROGRESS_CODE,
+                  (EFI_SOFTWARE_PEI_CORE | EFI_SW_PC_INIT_END),
+                  (VOID *)(&PeimFileHandle),
+                  sizeof (PeimFileHandle)
+                  );
+                PERF_END (PeimFileHandle, "PEIM", NULL, 0);
+
+              }
+            }
+
+            PeiCheckAndSwitchStack (SecCoreData, Private);
+
+            //
+            // Process the Notify list and dispatch any notifies for
+            // newly installed PPIs.
+            //
+            ProcessNotifyList (Private);
+
+            //
+            // Recheck SwitchStackSignal after ProcessNotifyList()
+            // in case PeiInstallPeiMemory() is done in a callback with
+            // EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH.
+            //
+            PeiCheckAndSwitchStack (SecCoreData, Private);
+
+            if ((Private->PeiMemoryInstalled) && (Private->Fv[FvCount].PeimState[PeimCount] == PEIM_STATE_REGISITER_FOR_SHADOW) &&   \
+                (Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME || PcdGetBool (PcdShadowPeimOnS3Boot))) {
+              //
+              // If memory is availble we shadow images by default for performance reasons.
+              // We call the entry point a 2nd time so the module knows it's shadowed.
+              //
+              //PERF_START (PeiServices, L"PEIM", PeimFileHandle, 0);
+              if ((Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME) && !PcdGetBool (PcdShadowPeimOnBoot)) {
+                //
+                // Load PEIM into Memory for Register for shadow PEIM.
+                //
+                Status = PeiLoadImage (
+                           PeiServices,
+                           PeimFileHandle,
+                           PEIM_STATE_REGISITER_FOR_SHADOW,
+                           &EntryPoint,
+                           &AuthenticationState
+                           );
+                if (Status == EFI_SUCCESS) {
+                  PeimEntryPoint = (EFI_PEIM_ENTRY_POINT2)(UINTN)EntryPoint;
+                }
+              }
+              ASSERT (PeimEntryPoint != NULL);
+              PeimEntryPoint (PeimFileHandle, (const EFI_PEI_SERVICES **) PeiServices);
+              //PERF_END (PeiServices, L"PEIM", PeimFileHandle, 0);
+
+              //
+              // PEIM_STATE_REGISITER_FOR_SHADOW move to PEIM_STATE_DONE
+              //
+              Private->Fv[FvCount].PeimState[PeimCount]++;
+
+              //
+              // Process the Notify list and dispatch any notifies for
+              // newly installed PPIs.
+              //
+              ProcessNotifyList (Private);
+            }
+          }
+        }
+      }
+
+      //
+      // We set to NULL here to optimize the 2nd entry to this routine after
+      //  memory is found. This reprevents rescanning of the FV. We set to
+      //  NULL here so we start at the begining of the next FV
+      //
+      Private->CurrentFileHandle = NULL;
+      Private->CurrentPeimCount = 0;
+      //
+      // Before walking through the next FV,Private->CurrentFvFileHandles[]should set to NULL
+      //
+      SetMem (Private->CurrentFvFileHandles, sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv), 0);
+    }
+
+    //
+    // Before making another pass, we should set Private->CurrentPeimFvCount =0 to go
+    // through all the FV.
+    //
+    Private->CurrentPeimFvCount = 0;
+
+    //
+    // PeimNeedingDispatch being TRUE means we found a PEIM/FV that did not get
+    //  dispatched. So we need to make another pass
+    //
+    // PeimDispatchOnThisPass being TRUE means we dispatched a PEIM/FV on this
+    //  pass. If we did not dispatch a PEIM/FV there is no point in trying again
+    //  as it will fail the next time too (nothing has changed).
+    //
+  } while (Private->PeimNeedingDispatch && Private->PeimDispatchOnThisPass);
+
+}
+
+/**
+  Initialize the Dispatcher's data members
+
+  @param PrivateData     PeiCore's private data structure
+  @param OldCoreData     Old data from SecCore
+                         NULL if being run in non-permament memory mode.
+  @param SecCoreData     Points to a data structure containing information about the PEI core's operating
+                         environment, such as the size and location of temporary RAM, the stack location and
+                         the BFV location.
+
+  @return None.
+
+**/
+VOID
+InitializeDispatcherData (
+  IN PEI_CORE_INSTANCE            *PrivateData,
+  IN PEI_CORE_INSTANCE            *OldCoreData,
+  IN CONST EFI_SEC_PEI_HAND_OFF   *SecCoreData
+  )
+{
+  if (OldCoreData == NULL) {
+    PrivateData->PeimDispatcherReenter = FALSE;
+    PeiInitializeFv (PrivateData, SecCoreData);
+  } else {
+    PeiReinitializeFv (PrivateData);
+  }
+
+  return;
+}
+
+/**
+  This routine parses the Dependency Expression, if available, and
+  decides if the module can be executed.
+
+
+  @param Private         PeiCore's private data structure
+  @param FileHandle      PEIM's file handle
+  @param PeimCount       Peim count in all dispatched PEIMs.
+
+  @retval TRUE   Can be dispatched
+  @retval FALSE  Cannot be dispatched
+
+**/
+BOOLEAN
+DepexSatisfied (
+  IN PEI_CORE_INSTANCE          *Private,
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN UINTN                      PeimCount
+  )
+{
+  EFI_STATUS           Status;
+  VOID                 *DepexData;
+  EFI_FV_FILE_INFO     FileInfo;
+
+  Status = PeiServicesFfsGetFileInfo (FileHandle, &FileInfo);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_DISPATCH, "Evaluate PEI DEPEX for FFS(Unknown)\n"));
+  } else {
+    DEBUG ((DEBUG_DISPATCH, "Evaluate PEI DEPEX for FFS(%g)\n", &FileInfo.FileName));
+  }
+  
+  if (PeimCount < Private->AprioriCount) {
+    //
+    // If its in the A priori file then we set Depex to TRUE
+    //
+    DEBUG ((DEBUG_DISPATCH, "  RESULT = TRUE (Apriori)\n"));
+    return TRUE;
+  }
+
+  //
+  // Depex section not in the encapsulated section.
+  //
+  Status = PeiServicesFfsFindSectionData (
+              EFI_SECTION_PEI_DEPEX,
+              FileHandle,
+              (VOID **)&DepexData
+              );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // If there is no DEPEX, assume the module can be executed
+    //
+    DEBUG ((DEBUG_DISPATCH, "  RESULT = TRUE (No DEPEX)\n"));
+    return TRUE;
+  }
+
+  //
+  // Evaluate a given DEPEX
+  //
+  return PeimDispatchReadiness (&Private->Ps, DepexData);
+}
+
+/**
+  This routine enable a PEIM to register itself to shadow when PEI Foundation
+  discovery permanent memory.
+
+  @param FileHandle             File handle of a PEIM.
+
+  @retval EFI_NOT_FOUND         The file handle doesn't point to PEIM itself.
+  @retval EFI_ALREADY_STARTED   Indicate that the PEIM has been registered itself.
+  @retval EFI_SUCCESS           Successfully to register itself.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiRegisterForShadow (
+  IN EFI_PEI_FILE_HANDLE       FileHandle
+  )
+{
+  PEI_CORE_INSTANCE            *Private;
+  Private = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer ());
+
+  if (Private->CurrentFileHandle != FileHandle) {
+    //
+    // The FileHandle must be for the current PEIM
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  if (Private->Fv[Private->CurrentPeimFvCount].PeimState[Private->CurrentPeimCount] >= PEIM_STATE_REGISITER_FOR_SHADOW) {
+    //
+    // If the PEIM has already entered the PEIM_STATE_REGISTER_FOR_SHADOW or PEIM_STATE_DONE then it's already been started
+    //
+    return EFI_ALREADY_STARTED;
+  }
+
+  Private->Fv[Private->CurrentPeimFvCount].PeimState[Private->CurrentPeimCount] = PEIM_STATE_REGISITER_FOR_SHADOW;
+
+  return EFI_SUCCESS;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.c b/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.c
new file mode 100644
index 0000000000..363ff7e694
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.c
@@ -0,0 +1,2306 @@
+/** @file
+  Pei Core Firmware File System service routines.
+  
+Copyright (c) 2006 - 2015, 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 "FwVol.h"
+
+EFI_PEI_NOTIFY_DESCRIPTOR mNotifyOnFvInfoList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
+    &gEfiPeiFirmwareVolumeInfoPpiGuid,
+    FirmwareVolmeInfoPpiNotifyCallback 
+  },
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiPeiFirmwareVolumeInfo2PpiGuid,
+    FirmwareVolmeInfoPpiNotifyCallback 
+  }
+};
+
+PEI_FW_VOL_INSTANCE mPeiFfs2FwVol = {
+  PEI_FW_VOL_SIGNATURE,
+  FALSE,
+  {
+    PeiFfsFvPpiProcessVolume,
+    PeiFfsFvPpiFindFileByType,
+    PeiFfsFvPpiFindFileByName,
+    PeiFfsFvPpiGetFileInfo,
+    PeiFfsFvPpiGetVolumeInfo,
+    PeiFfsFvPpiFindSectionByType,
+    PeiFfsFvPpiGetFileInfo2,
+    PeiFfsFvPpiFindSectionByType2,
+    EFI_PEI_FIRMWARE_VOLUME_PPI_SIGNATURE,
+    EFI_PEI_FIRMWARE_VOLUME_PPI_REVISION
+  }
+};
+
+PEI_FW_VOL_INSTANCE mPeiFfs3FwVol = {
+  PEI_FW_VOL_SIGNATURE,
+  TRUE,
+  {
+    PeiFfsFvPpiProcessVolume,
+    PeiFfsFvPpiFindFileByType,
+    PeiFfsFvPpiFindFileByName,
+    PeiFfsFvPpiGetFileInfo,
+    PeiFfsFvPpiGetVolumeInfo,
+    PeiFfsFvPpiFindSectionByType,
+    PeiFfsFvPpiGetFileInfo2,
+    PeiFfsFvPpiFindSectionByType2,
+    EFI_PEI_FIRMWARE_VOLUME_PPI_SIGNATURE,
+    EFI_PEI_FIRMWARE_VOLUME_PPI_REVISION
+  }
+};
+            
+EFI_PEI_PPI_DESCRIPTOR  mPeiFfs2FvPpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiFirmwareFileSystem2Guid,
+  &mPeiFfs2FwVol.Fv
+};
+
+EFI_PEI_PPI_DESCRIPTOR  mPeiFfs3FvPpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiFirmwareFileSystem3Guid,
+  &mPeiFfs3FwVol.Fv
+};
+
+/**
+Required Alignment             Alignment Value in FFS         Alignment Value in
+(bytes)                        Attributes Field               Firmware Volume Interfaces
+1                                    0                                     0
+16                                   1                                     4
+128                                  2                                     7
+512                                  3                                     9
+1 KB                                 4                                     10
+4 KB                                 5                                     12
+32 KB                                6                                     15
+64 KB                                7                                     16
+**/
+UINT8 mFvAttributes[] = {0, 4, 7, 9, 10, 12, 15, 16};
+
+/**
+  Convert the FFS File Attributes to FV File Attributes
+
+  @param  FfsAttributes              The attributes of UINT8 type.
+
+  @return The attributes of EFI_FV_FILE_ATTRIBUTES
+
+**/
+EFI_FV_FILE_ATTRIBUTES
+FfsAttributes2FvFileAttributes (
+  IN EFI_FFS_FILE_ATTRIBUTES FfsAttributes
+  )
+{
+  UINT8                     DataAlignment;
+  EFI_FV_FILE_ATTRIBUTES    FileAttribute;
+
+  DataAlignment = (UINT8) ((FfsAttributes & FFS_ATTRIB_DATA_ALIGNMENT) >> 3);
+  ASSERT (DataAlignment < 8);
+
+  FileAttribute = (EFI_FV_FILE_ATTRIBUTES) mFvAttributes[DataAlignment];
+
+  if ((FfsAttributes & FFS_ATTRIB_FIXED) == FFS_ATTRIB_FIXED) {
+    FileAttribute |= EFI_FV_FILE_ATTRIB_FIXED;
+  }
+
+  return FileAttribute;
+}
+
+/**
+  Returns the file state set by the highest zero bit in the State field
+
+  @param ErasePolarity   Erase Polarity  as defined by EFI_FVB2_ERASE_POLARITY
+                         in the Attributes field.
+  @param FfsHeader       Pointer to FFS File Header.
+
+  @retval EFI_FFS_FILE_STATE File state is set by the highest none zero bit 
+                             in the header State field.
+**/
+EFI_FFS_FILE_STATE
+GetFileState(
+  IN UINT8                ErasePolarity,
+  IN EFI_FFS_FILE_HEADER  *FfsHeader
+  )
+{
+  EFI_FFS_FILE_STATE  FileState;
+  EFI_FFS_FILE_STATE  HighestBit;
+
+  FileState = FfsHeader->State;
+
+  if (ErasePolarity != 0) {
+    FileState = (EFI_FFS_FILE_STATE)~FileState;
+  }
+  
+  //
+  // Get file state set by its highest none zero bit.
+  //
+  HighestBit = 0x80;
+  while (HighestBit != 0 && (HighestBit & FileState) == 0) {
+    HighestBit >>= 1;
+  }
+
+  return HighestBit;
+} 
+
+/**
+  Calculates the checksum of the header of a file.
+
+  @param FileHeader      Pointer to FFS File Header.
+
+  @return Checksum of the header.
+          Zero means the header is good.
+          Non-zero means the header is bad.
+**/
+UINT8
+CalculateHeaderChecksum (
+  IN EFI_FFS_FILE_HEADER  *FileHeader
+  )
+{
+  EFI_FFS_FILE_HEADER2 TestFileHeader;
+
+  if (IS_FFS_FILE2 (FileHeader)) {
+    CopyMem (&TestFileHeader, FileHeader, sizeof (EFI_FFS_FILE_HEADER2));
+    //
+    // Ingore State and File field in FFS header.
+    //
+    TestFileHeader.State = 0;
+    TestFileHeader.IntegrityCheck.Checksum.File = 0;
+
+    return CalculateSum8 ((CONST UINT8 *) &TestFileHeader, sizeof (EFI_FFS_FILE_HEADER2));
+  } else {
+    CopyMem (&TestFileHeader, FileHeader, sizeof (EFI_FFS_FILE_HEADER));
+    //
+    // Ingore State and File field in FFS header.
+    //
+    TestFileHeader.State = 0;
+    TestFileHeader.IntegrityCheck.Checksum.File = 0;
+
+    return CalculateSum8 ((CONST UINT8 *) &TestFileHeader, sizeof (EFI_FFS_FILE_HEADER));
+  }
+}
+
+/**
+  Find FV handler according to FileHandle in that FV.
+
+  @param FileHandle      Handle of file image
+  
+  @return Pointer to instance of PEI_CORE_FV_HANDLE.
+**/
+PEI_CORE_FV_HANDLE*
+FileHandleToVolume (
+  IN   EFI_PEI_FILE_HANDLE          FileHandle
+  )
+{
+  UINTN                       Index;
+  PEI_CORE_INSTANCE           *PrivateData;
+  EFI_FIRMWARE_VOLUME_HEADER  *FwVolHeader;
+  UINTN                       BestIndex;
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer ());
+  BestIndex   = PrivateData->FvCount;
+  
+  //
+  // Find the best matched FV image that includes this FileHandle.
+  // FV may include the child FV, and they are in the same continuous space. 
+  // If FileHandle is from the child FV, the updated logic can find its matched FV.
+  //
+  for (Index = 0; Index < PrivateData->FvCount; Index++) {
+    FwVolHeader = PrivateData->Fv[Index].FvHeader;
+    if (((UINT64) (UINTN) FileHandle > (UINT64) (UINTN) FwVolHeader ) &&   \
+        ((UINT64) (UINTN) FileHandle <= ((UINT64) (UINTN) FwVolHeader + FwVolHeader->FvLength - 1))) {
+      if (BestIndex == PrivateData->FvCount) {
+        BestIndex = Index;
+      } else {
+        if ((UINT64) (UINTN) PrivateData->Fv[BestIndex].FvHeader < (UINT64) (UINTN) FwVolHeader) {
+          BestIndex = Index;
+        }
+      }
+    }
+  }
+
+  if (BestIndex < PrivateData->FvCount) {
+    return &PrivateData->Fv[BestIndex];
+  }
+
+  return NULL;
+}
+
+/**
+  Given the input file pointer, search for the first matching file in the
+  FFS volume as defined by SearchType. The search starts from FileHeader inside
+  the Firmware Volume defined by FwVolHeader.
+  If SearchType is EFI_FV_FILETYPE_ALL, the first FFS file will return without check its file type.
+  If SearchType is PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE, 
+  the first PEIM, or COMBINED PEIM or FV file type FFS file will return.  
+
+  @param FvHandle        Pointer to the FV header of the volume to search
+  @param FileName        File name
+  @param SearchType      Filter to find only files of this type.
+                         Type EFI_FV_FILETYPE_ALL causes no filtering to be done.
+  @param FileHandle      This parameter must point to a valid FFS volume.
+  @param AprioriFile     Pointer to AprioriFile image in this FV if has
+
+  @return EFI_NOT_FOUND  No files matching the search criteria were found
+  @retval EFI_SUCCESS    Success to search given file
+
+**/
+EFI_STATUS
+FindFileEx (
+  IN  CONST EFI_PEI_FV_HANDLE        FvHandle,
+  IN  CONST EFI_GUID                 *FileName,   OPTIONAL
+  IN        EFI_FV_FILETYPE          SearchType,
+  IN OUT    EFI_PEI_FILE_HANDLE      *FileHandle,
+  IN OUT    EFI_PEI_FILE_HANDLE      *AprioriFile  OPTIONAL
+  )
+{
+  EFI_FIRMWARE_VOLUME_HEADER            *FwVolHeader;
+  EFI_FIRMWARE_VOLUME_EXT_HEADER        *FwVolExtHeader;
+  EFI_FFS_FILE_HEADER                   **FileHeader;
+  EFI_FFS_FILE_HEADER                   *FfsFileHeader;
+  UINT32                                FileLength;
+  UINT32                                FileOccupiedSize;
+  UINT32                                FileOffset;
+  UINT64                                FvLength;
+  UINT8                                 ErasePolarity;
+  UINT8                                 FileState;
+  UINT8                                 DataCheckSum;
+  BOOLEAN                               IsFfs3Fv;
+  
+  //
+  // Convert the handle of FV to FV header for memory-mapped firmware volume
+  //
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *) FvHandle;
+  FileHeader  = (EFI_FFS_FILE_HEADER **)FileHandle;
+
+  IsFfs3Fv = CompareGuid (&FwVolHeader->FileSystemGuid, &gEfiFirmwareFileSystem3Guid);
+
+  FvLength = FwVolHeader->FvLength;
+  if ((FwVolHeader->Attributes & EFI_FVB2_ERASE_POLARITY) != 0) {
+    ErasePolarity = 1;
+  } else {
+    ErasePolarity = 0;
+  }
+  
+  //
+  // If FileHeader is not specified (NULL) or FileName is not NULL,
+  // start with the first file in the firmware volume.  Otherwise,
+  // start from the FileHeader.
+  //
+  if ((*FileHeader == NULL) || (FileName != NULL)) {
+    if (FwVolHeader->ExtHeaderOffset != 0) {
+      //
+      // Searching for files starts on an 8 byte aligned boundary after the end of the Extended Header if it exists.
+      //
+      FwVolExtHeader = (EFI_FIRMWARE_VOLUME_EXT_HEADER *) ((UINT8 *) FwVolHeader + FwVolHeader->ExtHeaderOffset);
+      FfsFileHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FwVolExtHeader + FwVolExtHeader->ExtHeaderSize);
+      FfsFileHeader = (EFI_FFS_FILE_HEADER *) ALIGN_POINTER (FfsFileHeader, 8);
+    } else {
+      FfsFileHeader = (EFI_FFS_FILE_HEADER *)((UINT8 *) FwVolHeader + FwVolHeader->HeaderLength);
+    }
+  } else {
+    if (IS_FFS_FILE2 (*FileHeader)) {
+      if (!IsFfs3Fv) {
+        DEBUG ((EFI_D_ERROR, "It is a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &(*FileHeader)->Name));
+      }
+      FileLength = FFS_FILE2_SIZE (*FileHeader);
+      ASSERT (FileLength > 0x00FFFFFF);
+    } else {
+      FileLength = FFS_FILE_SIZE (*FileHeader);
+    }
+    //
+    // FileLength is adjusted to FileOccupiedSize as it is 8 byte aligned.
+    //
+    FileOccupiedSize = GET_OCCUPIED_SIZE (FileLength, 8);
+    FfsFileHeader = (EFI_FFS_FILE_HEADER *)((UINT8 *)*FileHeader + FileOccupiedSize);
+  }
+  
+  FileOffset = (UINT32) ((UINT8 *)FfsFileHeader - (UINT8 *)FwVolHeader);
+  ASSERT (FileOffset <= 0xFFFFFFFF);
+
+  while (FileOffset < (FvLength - sizeof (EFI_FFS_FILE_HEADER))) {
+    //
+    // Get FileState which is the highest bit of the State 
+    //
+    FileState = GetFileState (ErasePolarity, FfsFileHeader);
+    switch (FileState) {
+
+    case EFI_FILE_HEADER_CONSTRUCTION:
+    case EFI_FILE_HEADER_INVALID:
+      if (IS_FFS_FILE2 (FfsFileHeader)) {
+        if (!IsFfs3Fv) {
+          DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FfsFileHeader->Name));
+        }
+        FileOffset    += sizeof (EFI_FFS_FILE_HEADER2);
+        FfsFileHeader =  (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER2));
+      } else {
+        FileOffset    += sizeof (EFI_FFS_FILE_HEADER);
+        FfsFileHeader =  (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER));
+      }
+      break;
+        
+    case EFI_FILE_DATA_VALID:
+    case EFI_FILE_MARKED_FOR_UPDATE:
+      if (CalculateHeaderChecksum (FfsFileHeader) != 0) {
+        ASSERT (FALSE);
+        *FileHeader = NULL;
+        return EFI_NOT_FOUND;
+      }
+
+      if (IS_FFS_FILE2 (FfsFileHeader)) {
+        FileLength = FFS_FILE2_SIZE (FfsFileHeader);
+        ASSERT (FileLength > 0x00FFFFFF);
+        FileOccupiedSize = GET_OCCUPIED_SIZE (FileLength, 8);
+        if (!IsFfs3Fv) {
+          DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FfsFileHeader->Name));
+          FileOffset += FileOccupiedSize;
+          FfsFileHeader = (EFI_FFS_FILE_HEADER *) ((UINT8 *) FfsFileHeader + FileOccupiedSize);
+          break;
+        }
+      } else {
+        FileLength = FFS_FILE_SIZE (FfsFileHeader);
+        FileOccupiedSize = GET_OCCUPIED_SIZE (FileLength, 8);
+      }
+
+      DataCheckSum = FFS_FIXED_CHECKSUM;
+      if ((FfsFileHeader->Attributes & FFS_ATTRIB_CHECKSUM) == FFS_ATTRIB_CHECKSUM) {
+        if (IS_FFS_FILE2 (FfsFileHeader)) {
+          DataCheckSum = CalculateCheckSum8 ((CONST UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER2), FileLength - sizeof(EFI_FFS_FILE_HEADER2));
+        } else {
+          DataCheckSum = CalculateCheckSum8 ((CONST UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER), FileLength - sizeof(EFI_FFS_FILE_HEADER));
+        }
+      }
+      if (FfsFileHeader->IntegrityCheck.Checksum.File != DataCheckSum) {
+        ASSERT (FALSE);
+        *FileHeader = NULL;
+        return EFI_NOT_FOUND;
+      }
+
+      if (FileName != NULL) {
+        if (CompareGuid (&FfsFileHeader->Name, (EFI_GUID*)FileName)) {
+          *FileHeader = FfsFileHeader;
+          return EFI_SUCCESS;
+        }
+      } else if (SearchType == PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE) {
+        if ((FfsFileHeader->Type == EFI_FV_FILETYPE_PEIM) || 
+            (FfsFileHeader->Type == EFI_FV_FILETYPE_COMBINED_PEIM_DRIVER) ||
+            (FfsFileHeader->Type == EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE)) { 
+          
+          *FileHeader = FfsFileHeader;
+          return EFI_SUCCESS;
+        } else if (AprioriFile != NULL) {
+          if (FfsFileHeader->Type == EFI_FV_FILETYPE_FREEFORM) {
+            if (CompareGuid (&FfsFileHeader->Name, &gPeiAprioriFileNameGuid)) {
+              *AprioriFile = FfsFileHeader;
+            }           
+          } 
+        }
+      } else if (((SearchType == FfsFileHeader->Type) || (SearchType == EFI_FV_FILETYPE_ALL)) && 
+                 (FfsFileHeader->Type != EFI_FV_FILETYPE_FFS_PAD)) { 
+        *FileHeader = FfsFileHeader;
+        return EFI_SUCCESS;
+      }
+
+      FileOffset    += FileOccupiedSize; 
+      FfsFileHeader =  (EFI_FFS_FILE_HEADER *)((UINT8 *)FfsFileHeader + FileOccupiedSize);
+      break;
+    
+    case EFI_FILE_DELETED:
+      if (IS_FFS_FILE2 (FfsFileHeader)) {
+        if (!IsFfs3Fv) {
+          DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FfsFileHeader->Name));
+        }
+        FileLength = FFS_FILE2_SIZE (FfsFileHeader);
+        ASSERT (FileLength > 0x00FFFFFF);
+      } else {
+        FileLength = FFS_FILE_SIZE (FfsFileHeader);
+      }
+      FileOccupiedSize =  GET_OCCUPIED_SIZE(FileLength, 8);
+      FileOffset       += FileOccupiedSize;
+      FfsFileHeader    =  (EFI_FFS_FILE_HEADER *)((UINT8 *)FfsFileHeader + FileOccupiedSize);
+      break;
+
+    default:
+      *FileHeader = NULL;
+      return EFI_NOT_FOUND;
+    } 
+  }
+  
+  *FileHeader = NULL;
+  return EFI_NOT_FOUND;  
+}
+
+/**
+  Initialize PeiCore Fv List.
+
+  @param PrivateData     - Pointer to PEI_CORE_INSTANCE.
+  @param SecCoreData     - Pointer to EFI_SEC_PEI_HAND_OFF.
+**/
+VOID 
+PeiInitializeFv (
+  IN  PEI_CORE_INSTANCE           *PrivateData,
+  IN CONST EFI_SEC_PEI_HAND_OFF   *SecCoreData
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PEI_FIRMWARE_VOLUME_PPI   *FvPpi;
+  EFI_PEI_FV_HANDLE             FvHandle;
+  EFI_FIRMWARE_VOLUME_HEADER    *BfvHeader;
+  
+  //
+  // Install FV_PPI for FFS2 file system.
+  //
+  PeiServicesInstallPpi (&mPeiFfs2FvPpiList);
+
+  //
+  // Install FV_PPI for FFS3 file system.
+  //
+  PeiServicesInstallPpi (&mPeiFfs3FvPpiList);
+
+  BfvHeader = (EFI_FIRMWARE_VOLUME_HEADER *)SecCoreData->BootFirmwareVolumeBase;
+  
+  //
+  // The FV_PPI in BFV's format should be installed.
+  //
+  Status = PeiServicesLocatePpi (
+             &BfvHeader->FileSystemGuid,
+             0,
+             NULL,
+             (VOID**)&FvPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+    
+  //
+  // Get handle of BFV
+  //
+  FvPpi->ProcessVolume (
+           FvPpi, 
+           SecCoreData->BootFirmwareVolumeBase,
+           (UINTN)BfvHeader->FvLength,
+           &FvHandle
+           );
+
+  //
+  // Update internal PEI_CORE_FV array.
+  //
+  PrivateData->Fv[PrivateData->FvCount].FvHeader = BfvHeader;
+  PrivateData->Fv[PrivateData->FvCount].FvPpi    = FvPpi;
+  PrivateData->Fv[PrivateData->FvCount].FvHandle = FvHandle;
+  PrivateData->Fv[PrivateData->FvCount].AuthenticationStatus = 0;
+  DEBUG ((
+    EFI_D_INFO, 
+    "The %dth FV start address is 0x%11p, size is 0x%08x, handle is 0x%p\n", 
+    (UINT32) PrivateData->FvCount, 
+    (VOID *) BfvHeader, 
+    BfvHeader->FvLength,
+    FvHandle
+    ));    
+  PrivateData->FvCount ++;
+                            
+  //
+  // Post a call-back for the FvInfoPPI and FvInfo2PPI services to expose
+  // additional Fvs to PeiCore.
+  //
+  Status = PeiServicesNotifyPpi (mNotifyOnFvInfoList);
+  ASSERT_EFI_ERROR (Status);
+
+}
+  
+/**
+  Process Firmware Volum Information once FvInfoPPI or FvInfo2PPI install.
+  The FV Info will be registered into PeiCore private data structure.
+  And search the inside FV image, if found, the new FV INFO(2) PPI will be installed.
+
+  @param PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param NotifyDescriptor  Address of the notification descriptor data structure.
+  @param Ppi               Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS    The FV Info is registered into PeiCore private data structure.
+  @return if not EFI_SUCESS, fail to verify FV.
+
+**/
+EFI_STATUS
+EFIAPI
+FirmwareVolmeInfoPpiNotifyCallback (
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR     *NotifyDescriptor,
+  IN VOID                          *Ppi
+  )
+{
+  EFI_PEI_FIRMWARE_VOLUME_INFO2_PPI     FvInfo2Ppi;
+  EFI_PEI_FIRMWARE_VOLUME_PPI           *FvPpi;
+  PEI_CORE_INSTANCE                     *PrivateData;
+  EFI_STATUS                            Status;
+  EFI_PEI_FV_HANDLE                     FvHandle;
+  UINTN                                 FvIndex;
+  EFI_PEI_FILE_HANDLE                   FileHandle;
+  VOID                                  *DepexData;
+  BOOLEAN                               IsFvInfo2;
+  
+  Status       = EFI_SUCCESS;
+  PrivateData  = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+
+  if (CompareGuid (NotifyDescriptor->Guid, &gEfiPeiFirmwareVolumeInfo2PpiGuid)) {
+    //
+    // It is FvInfo2PPI.
+    //
+    CopyMem (&FvInfo2Ppi, Ppi, sizeof (EFI_PEI_FIRMWARE_VOLUME_INFO2_PPI));
+    IsFvInfo2 = TRUE;
+  } else {
+    //
+    // It is FvInfoPPI.
+    //
+    CopyMem (&FvInfo2Ppi, Ppi, sizeof (EFI_PEI_FIRMWARE_VOLUME_INFO_PPI));
+    FvInfo2Ppi.AuthenticationStatus = 0;
+    IsFvInfo2 = FALSE;
+  }
+
+  //
+  // Locate the corresponding FV_PPI according to founded FV's format guid
+  //
+  Status = PeiServicesLocatePpi (
+             &FvInfo2Ppi.FvFormat,
+             0,
+             NULL,
+             (VOID**)&FvPpi
+             );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Process new found FV and get FV handle.
+    //
+    Status = FvPpi->ProcessVolume (FvPpi, FvInfo2Ppi.FvInfo, FvInfo2Ppi.FvInfoSize, &FvHandle);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Fail to process new found FV, FV may be corrupted!\n"));
+      return Status;
+    }
+
+    //
+    // Check whether the FV has already been processed.
+    //
+    for (FvIndex = 0; FvIndex < PrivateData->FvCount; FvIndex ++) {
+      if (PrivateData->Fv[FvIndex].FvHandle == FvHandle) {
+        if (IsFvInfo2 && (FvInfo2Ppi.AuthenticationStatus != PrivateData->Fv[FvIndex].AuthenticationStatus)) {
+          PrivateData->Fv[FvIndex].AuthenticationStatus = FvInfo2Ppi.AuthenticationStatus;
+          DEBUG ((EFI_D_INFO, "Update AuthenticationStatus of the %dth FV to 0x%x!\n", FvIndex, FvInfo2Ppi.AuthenticationStatus));          
+        }
+        DEBUG ((EFI_D_INFO, "The Fv %p has already been processed!\n", FvInfo2Ppi.FvInfo));
+        return EFI_SUCCESS;
+      }
+    }
+
+    if (PrivateData->FvCount >= PcdGet32 (PcdPeiCoreMaxFvSupported)) {
+      DEBUG ((EFI_D_ERROR, "The number of Fv Images (%d) exceed the max supported FVs (%d) in Pei", PrivateData->FvCount + 1, PcdGet32 (PcdPeiCoreMaxFvSupported)));
+      DEBUG ((EFI_D_ERROR, "PcdPeiCoreMaxFvSupported value need be reconfigurated in DSC"));
+      ASSERT (FALSE);
+    }
+
+    //
+    // Update internal PEI_CORE_FV array.
+    //
+    PrivateData->Fv[PrivateData->FvCount].FvHeader = (EFI_FIRMWARE_VOLUME_HEADER*) FvInfo2Ppi.FvInfo;
+    PrivateData->Fv[PrivateData->FvCount].FvPpi    = FvPpi;
+    PrivateData->Fv[PrivateData->FvCount].FvHandle = FvHandle;
+    PrivateData->Fv[PrivateData->FvCount].AuthenticationStatus = FvInfo2Ppi.AuthenticationStatus;
+    DEBUG ((
+      EFI_D_INFO, 
+      "The %dth FV start address is 0x%11p, size is 0x%08x, handle is 0x%p\n", 
+      (UINT32) PrivateData->FvCount, 
+      (VOID *) FvInfo2Ppi.FvInfo, 
+      FvInfo2Ppi.FvInfoSize,
+      FvHandle
+      ));    
+    PrivateData->FvCount ++;
+
+    //
+    // Scan and process the new discoveried FV for EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE 
+    //
+    FileHandle = NULL;
+    do {
+      Status = FvPpi->FindFileByType (
+                        FvPpi,
+                        EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE,
+                        FvHandle,
+                        &FileHandle
+                       );
+      if (!EFI_ERROR (Status)) {
+        Status = FvPpi->FindSectionByType (
+                          FvPpi,
+                          EFI_SECTION_PEI_DEPEX,
+                          FileHandle,
+                          (VOID**)&DepexData
+                          );
+        if (!EFI_ERROR (Status)) {
+          if (!PeimDispatchReadiness (PeiServices, DepexData)) {
+            //
+            // Dependency is not satisfied.
+            //
+            continue;
+          }
+        }
+        
+        DEBUG ((EFI_D_INFO, "Found firmware volume Image File %p in FV[%d] %p\n", FileHandle, PrivateData->FvCount - 1, FvHandle));
+        ProcessFvFile (PrivateData, &PrivateData->Fv[PrivateData->FvCount - 1], FileHandle);
+      }
+    } while (FileHandle != NULL);
+  } else {
+    DEBUG ((EFI_D_ERROR, "Fail to process FV %p because no corresponding EFI_FIRMWARE_VOLUME_PPI is found!\n", FvInfo2Ppi.FvInfo));
+    
+    AddUnknownFormatFvInfo (PrivateData, &FvInfo2Ppi);
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Verify the Guided Section GUID by checking if there is the Guided Section GUID HOB recorded the GUID itself.
+
+  @param GuidedSectionGuid          The Guided Section GUID.
+  @param GuidedSectionExtraction    A pointer to the pointer to the supported Guided Section Extraction Ppi
+                                    for the Guided Section.
+
+  @return TRUE      The GuidedSectionGuid could be identified, and the pointer to
+                    the Guided Section Extraction Ppi will be returned to *GuidedSectionExtraction.
+  @return FALSE     The GuidedSectionGuid could not be identified, or 
+                    the Guided Section Extraction Ppi has not been installed yet.
+
+**/
+BOOLEAN
+VerifyGuidedSectionGuid (
+  IN  EFI_GUID                                  *GuidedSectionGuid,
+  OUT EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI     **GuidedSectionExtraction
+  )
+{
+  EFI_PEI_HOB_POINTERS  Hob;
+  EFI_GUID              *GuidRecorded;
+  VOID                  *Interface;
+  EFI_STATUS            Status;
+
+  //
+  // Check if there is the Guided Section GUID HOB recorded the GUID itself.
+  //
+  Hob.Raw = GetFirstGuidHob (GuidedSectionGuid);
+  if (Hob.Raw != NULL) {
+    GuidRecorded = (EFI_GUID *) GET_GUID_HOB_DATA (Hob);
+    if (CompareGuid (GuidRecorded, GuidedSectionGuid)) {
+      //
+      // Found the recorded GuidedSectionGuid.
+      //
+      Status = PeiServicesLocatePpi (GuidedSectionGuid, 0, NULL, (VOID **) &Interface);
+      if (!EFI_ERROR (Status) && Interface != NULL) {
+        //
+        // Found the supported Guided Section Extraction Ppi for the Guided Section.
+        //
+        *GuidedSectionExtraction = (EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI *) Interface;
+        return TRUE;
+      }
+      return FALSE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Go through the file to search SectionType section. 
+  Search within encapsulation sections (compression and GUIDed) recursively, 
+  until the match section is found.
+  
+  @param PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param SectionType       Filter to find only section of this type.
+  @param SectionInstance   Pointer to the filter to find the specific instance of section.
+  @param Section           From where to search.
+  @param SectionSize       The file size to search.
+  @param OutputBuffer      A pointer to the discovered section, if successful.
+                           NULL if section not found
+  @param AuthenticationStatus Updated upon return to point to the authentication status for this section.
+  @param IsFfs3Fv          Indicates the FV format.
+
+  @return EFI_NOT_FOUND    The match section is not found.
+  @return EFI_SUCCESS      The match section is found.
+
+**/
+EFI_STATUS
+ProcessSection (
+  IN CONST EFI_PEI_SERVICES     **PeiServices,
+  IN EFI_SECTION_TYPE           SectionType,
+  IN OUT UINTN                  *SectionInstance,
+  IN EFI_COMMON_SECTION_HEADER  *Section,
+  IN UINTN                      SectionSize,
+  OUT VOID                      **OutputBuffer,
+  OUT UINT32                    *AuthenticationStatus,
+  IN BOOLEAN                    IsFfs3Fv
+  )
+{
+  EFI_STATUS                              Status;
+  UINT32                                  SectionLength;
+  UINT32                                  ParsedLength;
+  EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI   *GuidSectionPpi;
+  EFI_PEI_DECOMPRESS_PPI                  *DecompressPpi;
+  VOID                                    *PpiOutput;
+  UINTN                                   PpiOutputSize;
+  UINTN                                   Index;
+  UINT32                                  Authentication;
+  PEI_CORE_INSTANCE                       *PrivateData;
+  EFI_GUID                                *SectionDefinitionGuid;
+  BOOLEAN                                 SectionCached;
+  VOID                                    *TempOutputBuffer;
+  UINT32                                  TempAuthenticationStatus;
+  UINT16                                  GuidedSectionAttributes;
+
+  PrivateData   = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+  *OutputBuffer = NULL;
+  ParsedLength  = 0;
+  Index         = 0;
+  Status        = EFI_NOT_FOUND;
+  PpiOutput     = NULL;
+  PpiOutputSize = 0;
+  while (ParsedLength < SectionSize) {
+
+    if (IS_SECTION2 (Section)) {
+      ASSERT (SECTION2_SIZE (Section) > 0x00FFFFFF);
+      if (!IsFfs3Fv) {
+        DEBUG ((EFI_D_ERROR, "Found a FFS3 formatted section in a non-FFS3 formatted FV.\n"));
+        SectionLength = SECTION2_SIZE (Section);
+        //
+        // SectionLength is adjusted it is 4 byte aligned.
+        // Go to the next section
+        //
+        SectionLength = GET_OCCUPIED_SIZE (SectionLength, 4);
+        ASSERT (SectionLength != 0);
+        ParsedLength += SectionLength;
+        Section = (EFI_COMMON_SECTION_HEADER *) ((UINT8 *) Section + SectionLength);
+        continue;
+      }
+    }
+
+    if (Section->Type == SectionType) {
+      //
+      // The type matches, so check the instance count to see if it's the one we want.
+      //
+      (*SectionInstance)--;
+      if (*SectionInstance == 0) {
+        //
+        // Got it!
+        //
+        if (IS_SECTION2 (Section)) {
+          *OutputBuffer = (VOID *)((UINT8 *) Section + sizeof (EFI_COMMON_SECTION_HEADER2));
+        } else {
+          *OutputBuffer = (VOID *)((UINT8 *) Section + sizeof (EFI_COMMON_SECTION_HEADER));
+        }
+        return EFI_SUCCESS;
+      } else {
+        if (IS_SECTION2 (Section)) {
+          SectionLength = SECTION2_SIZE (Section);
+        } else {
+          SectionLength = SECTION_SIZE (Section);
+        }
+        //
+        // SectionLength is adjusted it is 4 byte aligned.
+        // Go to the next section
+        //
+        SectionLength = GET_OCCUPIED_SIZE (SectionLength, 4);
+        ASSERT (SectionLength != 0);
+        ParsedLength += SectionLength;
+        Section = (EFI_COMMON_SECTION_HEADER *)((UINT8 *)Section + SectionLength);
+        continue;
+      }
+    } else if ((Section->Type == EFI_SECTION_GUID_DEFINED) || (Section->Type == EFI_SECTION_COMPRESSION)) {
+      //
+      // Check the encapsulated section is extracted into the cache data.
+      //
+      SectionCached = FALSE;
+      for (Index = 0; Index < PrivateData->CacheSection.AllSectionCount; Index ++) {
+        if (Section == PrivateData->CacheSection.Section[Index]) {
+          SectionCached = TRUE;
+          PpiOutput     = PrivateData->CacheSection.SectionData[Index];
+          PpiOutputSize = PrivateData->CacheSection.SectionSize[Index];
+          Authentication = PrivateData->CacheSection.AuthenticationStatus[Index];
+          //
+          // Search section directly from the cache data.
+          //
+          TempAuthenticationStatus = 0;
+          Status = ProcessSection (
+                     PeiServices,
+                     SectionType,
+                     SectionInstance,
+                     PpiOutput, 
+                     PpiOutputSize, 
+                     &TempOutputBuffer,
+                     &TempAuthenticationStatus,
+                     IsFfs3Fv
+                   );
+          if (!EFI_ERROR (Status)) {
+            *OutputBuffer = TempOutputBuffer;
+            *AuthenticationStatus = TempAuthenticationStatus | Authentication;
+            return EFI_SUCCESS;
+          }
+        }
+      }
+
+      //
+      // If SectionCached is TRUE, the section data has been cached and scanned.
+      //
+      if (!SectionCached) {
+        Status = EFI_NOT_FOUND;
+        Authentication = 0;
+        if (Section->Type == EFI_SECTION_GUID_DEFINED) {
+          if (IS_SECTION2 (Section)) {
+            SectionDefinitionGuid   = &((EFI_GUID_DEFINED_SECTION2 *)Section)->SectionDefinitionGuid;
+            GuidedSectionAttributes = ((EFI_GUID_DEFINED_SECTION2 *)Section)->Attributes;
+          } else {
+            SectionDefinitionGuid   = &((EFI_GUID_DEFINED_SECTION *)Section)->SectionDefinitionGuid;
+            GuidedSectionAttributes = ((EFI_GUID_DEFINED_SECTION *)Section)->Attributes;
+          }
+          if (VerifyGuidedSectionGuid (SectionDefinitionGuid, &GuidSectionPpi)) {
+            Status = GuidSectionPpi->ExtractSection (
+                                       GuidSectionPpi,
+                                       Section,
+                                       &PpiOutput,
+                                       &PpiOutputSize,
+                                       &Authentication
+                                       );
+          } else if ((GuidedSectionAttributes & EFI_GUIDED_SECTION_PROCESSING_REQUIRED) == 0) {
+            //
+            // Figure out the proper authentication status for GUIDED section without processing required
+            //
+            Status = EFI_SUCCESS;
+            if ((GuidedSectionAttributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID) == EFI_GUIDED_SECTION_AUTH_STATUS_VALID) {
+              Authentication |= EFI_AUTH_STATUS_IMAGE_SIGNED | EFI_AUTH_STATUS_NOT_TESTED;
+            }
+            if (IS_SECTION2 (Section)) {
+              PpiOutputSize = SECTION2_SIZE (Section) - ((EFI_GUID_DEFINED_SECTION2 *) Section)->DataOffset;
+              PpiOutput     = (UINT8 *) Section + ((EFI_GUID_DEFINED_SECTION2 *) Section)->DataOffset;
+            } else {
+              PpiOutputSize = SECTION_SIZE (Section) - ((EFI_GUID_DEFINED_SECTION *) Section)->DataOffset;
+              PpiOutput     = (UINT8 *) Section + ((EFI_GUID_DEFINED_SECTION *) Section)->DataOffset;
+            }
+          }
+        } else if (Section->Type == EFI_SECTION_COMPRESSION) {
+          Status = PeiServicesLocatePpi (&gEfiPeiDecompressPpiGuid, 0, NULL, (VOID **) &DecompressPpi);
+          if (!EFI_ERROR (Status)) {
+            Status = DecompressPpi->Decompress (
+                                      DecompressPpi,
+                                      (CONST EFI_COMPRESSION_SECTION*) Section,
+                                      &PpiOutput,
+                                      &PpiOutputSize
+                                      );
+          }
+        }
+
+        if (!EFI_ERROR (Status)) {
+          //
+          // Update cache section data.
+          //
+          if (PrivateData->CacheSection.AllSectionCount < CACHE_SETION_MAX_NUMBER) {
+            PrivateData->CacheSection.AllSectionCount ++;
+          }
+          PrivateData->CacheSection.Section [PrivateData->CacheSection.SectionIndex]     = Section;
+          PrivateData->CacheSection.SectionData [PrivateData->CacheSection.SectionIndex] = PpiOutput;
+          PrivateData->CacheSection.SectionSize [PrivateData->CacheSection.SectionIndex] = PpiOutputSize;
+          PrivateData->CacheSection.AuthenticationStatus [PrivateData->CacheSection.SectionIndex] = Authentication;
+          PrivateData->CacheSection.SectionIndex = (PrivateData->CacheSection.SectionIndex + 1)%CACHE_SETION_MAX_NUMBER;
+
+          TempAuthenticationStatus = 0;
+          Status = ProcessSection (
+                     PeiServices,
+                     SectionType,
+                     SectionInstance,
+                     PpiOutput, 
+                     PpiOutputSize, 
+                     &TempOutputBuffer,
+                     &TempAuthenticationStatus,
+                     IsFfs3Fv
+                   );
+          if (!EFI_ERROR (Status)) {
+            *OutputBuffer = TempOutputBuffer;
+            *AuthenticationStatus = TempAuthenticationStatus | Authentication;
+            return EFI_SUCCESS;
+          }
+        }
+      }
+    }
+
+    if (IS_SECTION2 (Section)) {
+      SectionLength = SECTION2_SIZE (Section);
+    } else {
+      SectionLength = SECTION_SIZE (Section);
+    }
+    //
+    // SectionLength is adjusted it is 4 byte aligned.
+    // Go to the next section
+    //
+    SectionLength = GET_OCCUPIED_SIZE (SectionLength, 4);
+    ASSERT (SectionLength != 0);
+    ParsedLength += SectionLength;
+    Section = (EFI_COMMON_SECTION_HEADER *)((UINT8 *)Section + SectionLength);
+  }
+  
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Searches for the next matching section within the specified file.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param SectionType     Filter to find only sections of this type.
+  @param FileHandle      Pointer to the current file to search.
+  @param SectionData     A pointer to the discovered section, if successful.
+                         NULL if section not found
+
+  @retval EFI_NOT_FOUND  The section was not found.
+  @retval EFI_SUCCESS    The section was found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindSectionData (
+  IN CONST EFI_PEI_SERVICES    **PeiServices,
+  IN     EFI_SECTION_TYPE      SectionType,
+  IN     EFI_PEI_FILE_HANDLE   FileHandle,
+  OUT VOID                     **SectionData
+  )
+{
+  PEI_CORE_FV_HANDLE           *CoreFvHandle;
+  
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if ((CoreFvHandle == NULL) || (CoreFvHandle->FvPpi == NULL)) {
+    return EFI_NOT_FOUND;
+  }
+  
+  return CoreFvHandle->FvPpi->FindSectionByType (CoreFvHandle->FvPpi, SectionType, FileHandle, SectionData);
+}
+
+/**
+  Searches for the next matching section within the specified file.
+
+  @param  PeiServices           An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  SectionType           The value of the section type to find.
+  @param  SectionInstance       Section instance to find.
+  @param  FileHandle            Handle of the firmware file to search.
+  @param  SectionData           A pointer to the discovered section, if successful.
+  @param  AuthenticationStatus  A pointer to the authentication status for this section.
+
+  @retval EFI_SUCCESS      The section was found.
+  @retval EFI_NOT_FOUND    The section was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindSectionData3 (
+  IN CONST EFI_PEI_SERVICES    **PeiServices,
+  IN     EFI_SECTION_TYPE      SectionType,
+  IN     UINTN                 SectionInstance,
+  IN     EFI_PEI_FILE_HANDLE   FileHandle,
+  OUT VOID                     **SectionData,
+  OUT UINT32                   *AuthenticationStatus
+  )
+{
+  PEI_CORE_FV_HANDLE           *CoreFvHandle;
+  
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if ((CoreFvHandle == NULL) || (CoreFvHandle->FvPpi == NULL)) {
+    return EFI_NOT_FOUND;
+  }
+
+  if ((CoreFvHandle->FvPpi->Signature == EFI_PEI_FIRMWARE_VOLUME_PPI_SIGNATURE) &&
+      (CoreFvHandle->FvPpi->Revision == EFI_PEI_FIRMWARE_VOLUME_PPI_REVISION)) {
+    return CoreFvHandle->FvPpi->FindSectionByType2 (CoreFvHandle->FvPpi, SectionType, SectionInstance, FileHandle, SectionData, AuthenticationStatus);
+  }
+  //
+  // The old FvPpi doesn't support to find section by section instance
+  // and return authentication status, so return EFI_UNSUPPORTED.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Searches for the next matching file in the firmware volume.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param SearchType      Filter to find only files of this type.
+                         Type EFI_FV_FILETYPE_ALL causes no filtering to be done.
+  @param FvHandle        Handle of firmware volume in which to search.
+  @param FileHandle      On entry, points to the current handle from which to begin searching or NULL to start
+                         at the beginning of the firmware volume. On exit, points the file handle of the next file
+                         in the volume or NULL if there are no more files.
+
+  @retval EFI_NOT_FOUND  The file was not found.
+  @retval EFI_NOT_FOUND  The header checksum was not zero.
+  @retval EFI_SUCCESS    The file was found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindNextFile (
+  IN CONST EFI_PEI_SERVICES      **PeiServices,
+  IN UINT8                       SearchType,
+  IN EFI_PEI_FV_HANDLE           FvHandle,
+  IN OUT EFI_PEI_FILE_HANDLE     *FileHandle
+  )
+{
+  PEI_CORE_FV_HANDLE      *CoreFvHandle;
+  
+  CoreFvHandle = FvHandleToCoreHandle (FvHandle);
+  
+  //
+  // To make backward compatiblity, if can not find corresponding the handle of FV
+  // then treat FV as build-in FFS2/FFS3 format and memory mapped FV that FV handle is pointed
+  // to the address of first byte of FV.
+  //
+  if ((CoreFvHandle == NULL) && FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    return FindFileEx (FvHandle, NULL, SearchType, FileHandle, NULL);
+  } 
+  
+  if ((CoreFvHandle == NULL) || CoreFvHandle->FvPpi == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  
+  return CoreFvHandle->FvPpi->FindFileByType (CoreFvHandle->FvPpi, SearchType, FvHandle, FileHandle);
+}
+
+
+/**
+  Search the firmware volumes by index
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param Instance        This instance of the firmware volume to find. The value 0 is the Boot Firmware
+                         Volume (BFV).
+  @param VolumeHandle    On exit, points to the next volume handle or NULL if it does not exist.
+
+  @retval EFI_INVALID_PARAMETER  VolumeHandle is NULL
+  @retval EFI_NOT_FOUND          The volume was not found.
+  @retval EFI_SUCCESS            The volume was found.
+
+**/
+EFI_STATUS 
+EFIAPI
+PeiFfsFindNextVolume (
+  IN CONST EFI_PEI_SERVICES         **PeiServices,
+  IN     UINTN                      Instance,
+  IN OUT EFI_PEI_FV_HANDLE          *VolumeHandle
+  )
+{
+  PEI_CORE_INSTANCE  *Private;
+  PEI_CORE_FV_HANDLE *CoreFvHandle;
+  
+  if (VolumeHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Private = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+  
+  CoreFvHandle = FindNextCoreFvHandle (Private, Instance);
+  if (CoreFvHandle == NULL) {
+    *VolumeHandle = NULL;
+    return EFI_NOT_FOUND;
+  }
+  
+  *VolumeHandle = CoreFvHandle->FvHandle;
+  
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Find a file within a volume by its name.
+
+  @param FileName        A pointer to the name of the file to find within the firmware volume.
+  @param VolumeHandle    The firmware volume to search
+  @param FileHandle      Upon exit, points to the found file's handle 
+                         or NULL if it could not be found.
+
+  @retval EFI_SUCCESS            File was found.
+  @retval EFI_NOT_FOUND          File was not found.
+  @retval EFI_INVALID_PARAMETER  VolumeHandle or FileHandle or FileName was NULL.
+
+**/
+EFI_STATUS
+EFIAPI 
+PeiFfsFindFileByName (
+  IN  CONST EFI_GUID        *FileName,
+  IN  EFI_PEI_FV_HANDLE     VolumeHandle,
+  OUT EFI_PEI_FILE_HANDLE   *FileHandle
+  )
+{
+  PEI_CORE_FV_HANDLE            *CoreFvHandle;
+  
+  if ((VolumeHandle == NULL) || (FileName == NULL) || (FileHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  CoreFvHandle = FvHandleToCoreHandle (VolumeHandle);
+  if ((CoreFvHandle == NULL) || (CoreFvHandle->FvPpi == NULL)) {
+    return EFI_NOT_FOUND;
+  }
+  
+  return CoreFvHandle->FvPpi->FindFileByName (CoreFvHandle->FvPpi, FileName, &VolumeHandle, FileHandle);
+}
+
+/**
+  Returns information about a specific file.
+
+  @param FileHandle       Handle of the file.
+  @param FileInfo         Upon exit, points to the file's information.
+
+  @retval EFI_INVALID_PARAMETER If FileInfo is NULL.
+  @retval EFI_INVALID_PARAMETER If FileHandle does not represent a valid file.
+  @retval EFI_SUCCESS           File information returned.
+
+**/
+EFI_STATUS
+EFIAPI 
+PeiFfsGetFileInfo (
+  IN EFI_PEI_FILE_HANDLE  FileHandle,
+  OUT EFI_FV_FILE_INFO    *FileInfo
+  )
+{
+  PEI_CORE_FV_HANDLE          *CoreFvHandle;
+  
+  if ((FileHandle == NULL) || (FileInfo == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the FirmwareVolume which the file resides in.
+  //
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if ((CoreFvHandle == NULL) || (CoreFvHandle->FvPpi == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return CoreFvHandle->FvPpi->GetFileInfo (CoreFvHandle->FvPpi, FileHandle, FileInfo);
+}
+
+/**
+  Returns information about a specific file.
+
+  @param FileHandle       Handle of the file.
+  @param FileInfo         Upon exit, points to the file's information.
+
+  @retval EFI_INVALID_PARAMETER If FileInfo is NULL.
+  @retval EFI_INVALID_PARAMETER If FileHandle does not represent a valid file.
+  @retval EFI_SUCCESS           File information returned.
+
+**/
+EFI_STATUS
+EFIAPI 
+PeiFfsGetFileInfo2 (
+  IN EFI_PEI_FILE_HANDLE  FileHandle,
+  OUT EFI_FV_FILE_INFO2   *FileInfo
+  )
+{
+  PEI_CORE_FV_HANDLE          *CoreFvHandle;
+  
+  if ((FileHandle == NULL) || (FileInfo == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the FirmwareVolume which the file resides in.
+  //
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if ((CoreFvHandle == NULL) || (CoreFvHandle->FvPpi == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((CoreFvHandle->FvPpi->Signature == EFI_PEI_FIRMWARE_VOLUME_PPI_SIGNATURE) &&
+      (CoreFvHandle->FvPpi->Revision == EFI_PEI_FIRMWARE_VOLUME_PPI_REVISION)) {
+    return CoreFvHandle->FvPpi->GetFileInfo2 (CoreFvHandle->FvPpi, FileHandle, FileInfo);
+  }
+  //
+  // The old FvPpi doesn't support to return file info with authentication status,
+  // so return EFI_UNSUPPORTED.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Returns information about the specified volume.
+
+  This function returns information about a specific firmware
+  volume, including its name, type, attributes, starting address
+  and size.
+
+  @param VolumeHandle   Handle of the volume.
+  @param VolumeInfo     Upon exit, points to the volume's information.
+
+  @retval EFI_SUCCESS             Volume information returned.
+  @retval EFI_INVALID_PARAMETER   If VolumeHandle does not represent a valid volume.
+  @retval EFI_INVALID_PARAMETER   If VolumeHandle is NULL.
+  @retval EFI_SUCCESS             Information successfully returned.
+  @retval EFI_INVALID_PARAMETER   The volume designated by the VolumeHandle is not available.
+
+**/
+EFI_STATUS
+EFIAPI 
+PeiFfsGetVolumeInfo (
+  IN EFI_PEI_FV_HANDLE  VolumeHandle,
+  OUT EFI_FV_INFO       *VolumeInfo
+  )
+{
+  PEI_CORE_FV_HANDLE                     *CoreHandle;
+  
+  if ((VolumeInfo == NULL) || (VolumeHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  CoreHandle = FvHandleToCoreHandle (VolumeHandle);
+  
+  if ((CoreHandle == NULL) || (CoreHandle->FvPpi == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  return CoreHandle->FvPpi->GetVolumeInfo (CoreHandle->FvPpi, VolumeHandle, VolumeInfo);
+}
+
+/**
+  Get Fv image from the FV type file, then install FV INFO(2) ppi, Build FV hob.
+
+  @param PrivateData          PeiCore's private data structure
+  @param ParentFvCoreHandle   Pointer of EFI_CORE_FV_HANDLE to parent Fv image that contain this Fv image.
+  @param ParentFvFileHandle   File handle of a Fv type file that contain this Fv image.
+
+  @retval EFI_NOT_FOUND         FV image can't be found.
+  @retval EFI_SUCCESS           Successfully to process it.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate page when aligning FV image
+  @retval EFI_SECURITY_VIOLATION Image is illegal
+  @retval Others                Can not find EFI_SECTION_FIRMWARE_VOLUME_IMAGE section
+  
+**/
+EFI_STATUS
+ProcessFvFile (
+  IN  PEI_CORE_INSTANCE           *PrivateData,
+  IN  PEI_CORE_FV_HANDLE          *ParentFvCoreHandle,
+  IN  EFI_PEI_FILE_HANDLE         ParentFvFileHandle
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_FV_INFO                   ParentFvImageInfo;
+  UINT32                        FvAlignment;
+  VOID                          *NewFvBuffer;
+  EFI_PEI_HOB_POINTERS          HobPtr;
+  EFI_PEI_FIRMWARE_VOLUME_PPI   *ParentFvPpi;
+  EFI_PEI_FV_HANDLE             ParentFvHandle;
+  EFI_FIRMWARE_VOLUME_HEADER    *FvHeader;
+  EFI_FV_FILE_INFO              FileInfo;
+  UINT64                        FvLength;
+  UINT32                        AuthenticationStatus;
+  
+  //
+  // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has already
+  // been extracted.
+  //
+  HobPtr.Raw = GetHobList ();
+  while ((HobPtr.Raw = GetNextHob (EFI_HOB_TYPE_FV2, HobPtr.Raw)) != NULL) {
+    if (CompareGuid (&(((EFI_FFS_FILE_HEADER *)ParentFvFileHandle)->Name), &HobPtr.FirmwareVolume2->FileName)) {
+      //
+      // this FILE has been dispatched, it will not be dispatched again.
+      //
+      DEBUG ((EFI_D_INFO, "FV file %p has been dispatched!\r\n", ParentFvFileHandle));
+      return EFI_SUCCESS;
+    }
+    HobPtr.Raw = GET_NEXT_HOB (HobPtr);
+  }
+
+  ParentFvHandle = ParentFvCoreHandle->FvHandle;
+  ParentFvPpi    = ParentFvCoreHandle->FvPpi;
+  
+  //
+  // Find FvImage in FvFile
+  //
+  AuthenticationStatus = 0;
+  if ((ParentFvPpi->Signature == EFI_PEI_FIRMWARE_VOLUME_PPI_SIGNATURE) &&
+      (ParentFvPpi->Revision == EFI_PEI_FIRMWARE_VOLUME_PPI_REVISION)) {
+    Status = ParentFvPpi->FindSectionByType2 (
+                            ParentFvPpi,
+                            EFI_SECTION_FIRMWARE_VOLUME_IMAGE,
+                            0,
+                            ParentFvFileHandle,
+                            (VOID **)&FvHeader,
+                            &AuthenticationStatus
+                            );
+  } else {
+    Status = ParentFvPpi->FindSectionByType (
+                            ParentFvPpi,
+                            EFI_SECTION_FIRMWARE_VOLUME_IMAGE,
+                            ParentFvFileHandle,
+                            (VOID **)&FvHeader
+                            );
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = VerifyPeim (PrivateData, ParentFvHandle, ParentFvFileHandle, AuthenticationStatus);
+  if (Status == EFI_SECURITY_VIOLATION) {
+    return Status;
+  }
+
+  //
+  // If EFI_FVB2_WEAK_ALIGNMENT is set in the volume header then the first byte of the volume
+  // can be aligned on any power-of-two boundary. A weakly aligned volume can not be moved from
+  // its initial linked location and maintain its alignment.
+  //
+  if ((ReadUnaligned32 (&FvHeader->Attributes) & EFI_FVB2_WEAK_ALIGNMENT) != EFI_FVB2_WEAK_ALIGNMENT) {
+    //
+    // FvAlignment must be greater than or equal to 8 bytes of the minimum FFS alignment value.
+    //
+    FvAlignment = 1 << ((ReadUnaligned32 (&FvHeader->Attributes) & EFI_FVB2_ALIGNMENT) >> 16);
+    if (FvAlignment < 8) {
+      FvAlignment = 8;
+    }
+
+    //
+    // Check FvImage
+    //
+    if ((UINTN) FvHeader % FvAlignment != 0) {
+      FvLength    = ReadUnaligned64 (&FvHeader->FvLength);
+      NewFvBuffer = AllocateAlignedPages (EFI_SIZE_TO_PAGES ((UINT32) FvLength), FvAlignment);
+      if (NewFvBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      CopyMem (NewFvBuffer, FvHeader, (UINTN) FvLength);
+      FvHeader = (EFI_FIRMWARE_VOLUME_HEADER*) NewFvBuffer;
+    }
+  }
+
+  Status = ParentFvPpi->GetVolumeInfo (ParentFvPpi, ParentFvHandle, &ParentFvImageInfo);
+  ASSERT_EFI_ERROR (Status);
+  
+  Status = ParentFvPpi->GetFileInfo (ParentFvPpi, ParentFvFileHandle, &FileInfo);
+  ASSERT_EFI_ERROR (Status);
+  
+  //
+  // Install FvInfo(2) Ppi
+  //
+  PeiServicesInstallFvInfoPpi (
+    &FvHeader->FileSystemGuid,
+    (VOID**) FvHeader,
+    (UINT32) FvHeader->FvLength,
+    &ParentFvImageInfo.FvName,
+    &FileInfo.FileName
+    );
+
+  PeiServicesInstallFvInfo2Ppi (
+    &FvHeader->FileSystemGuid,
+    (VOID**) FvHeader,
+    (UINT32) FvHeader->FvLength,
+    &ParentFvImageInfo.FvName,
+    &FileInfo.FileName,
+    AuthenticationStatus
+    );
+
+  //
+  // Inform the extracted FvImage to Fv HOB consumer phase, i.e. DXE phase
+  //
+  BuildFvHob (
+    (EFI_PHYSICAL_ADDRESS) (UINTN) FvHeader,
+    FvHeader->FvLength
+    );
+
+  //
+  // Makes the encapsulated volume show up in DXE phase to skip processing of
+  // encapsulated file again.
+  //
+  BuildFv2Hob (
+    (EFI_PHYSICAL_ADDRESS) (UINTN) FvHeader,
+    FvHeader->FvLength,
+    &ParentFvImageInfo.FvName,
+    &FileInfo.FileName
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Process a firmware volume and create a volume handle.
+
+  Create a volume handle from the information in the buffer. For
+  memory-mapped firmware volumes, Buffer and BufferSize refer to
+  the start of the firmware volume and the firmware volume size.
+  For non memory-mapped firmware volumes, this points to a
+  buffer which contains the necessary information for creating
+  the firmware volume handle. Normally, these values are derived
+  from the EFI_FIRMWARE_VOLUME_INFO_PPI.
+  
+  
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param Buffer                 Points to the start of the buffer.
+  @param BufferSize             Size of the buffer.
+  @param FvHandle               Points to the returned firmware volume
+                                handle. The firmware volume handle must
+                                be unique within the system. 
+
+  @retval EFI_SUCCESS           Firmware volume handle created.
+  @retval EFI_VOLUME_CORRUPTED  Volume was corrupt.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiProcessVolume (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN  VOID                               *Buffer,
+  IN  UINTN                              BufferSize,
+  OUT EFI_PEI_FV_HANDLE                  *FvHandle
+  )
+{
+  EFI_STATUS          Status;
+  
+  ASSERT (FvHandle != NULL);
+  
+  if (Buffer == NULL) {
+    return EFI_VOLUME_CORRUPTED;
+  }
+  
+  //
+  // The build-in EFI_PEI_FIRMWARE_VOLUME_PPI for FFS2/FFS3 support memory-mapped
+  // FV image and the handle is pointed to Fv image's buffer.
+  //
+  *FvHandle = (EFI_PEI_FV_HANDLE) Buffer;
+  
+  //
+  // Do verify for given FV buffer.
+  //
+  Status = VerifyFv ((EFI_FIRMWARE_VOLUME_HEADER*) Buffer);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "Fail to verify FV which address is 0x%11p", Buffer));
+    return EFI_VOLUME_CORRUPTED;
+  }
+
+  return EFI_SUCCESS;
+}  
+
+/**
+  Finds the next file of the specified type.
+
+  This service enables PEI modules to discover additional firmware files. 
+  The FileHandle must be unique within the system.
+
+  @param This           Points to this instance of the
+                        EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType     A filter to find only files of this type. Type
+                        EFI_FV_FILETYPE_ALL causes no filtering to be
+                        done.
+  @param FvHandle       Handle of firmware volume in which to
+                        search.
+  @param FileHandle     Points to the current handle from which to
+                        begin searching or NULL to start at the
+                        beginning of the firmware volume. Updated
+                        upon return to reflect the file found.
+
+  @retval EFI_SUCCESS   The file was found.
+  @retval EFI_NOT_FOUND The file was not found. FileHandle contains NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindFileByType (
+  IN CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN        EFI_FV_FILETYPE             SearchType,
+  IN        EFI_PEI_FV_HANDLE           FvHandle,
+  IN OUT    EFI_PEI_FILE_HANDLE         *FileHandle
+  )
+{ 
+  return FindFileEx (FvHandle, NULL, SearchType, FileHandle, NULL);
+}
+
+/**
+  Find a file within a volume by its name. 
+  
+  This service searches for files with a specific name, within
+  either the specified firmware volume or all firmware volumes.
+
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileName               A pointer to the name of the file to find
+                                within the firmware volume.
+  @param FvHandle               Upon entry, the pointer to the firmware
+                                volume to search or NULL if all firmware
+                                volumes should be searched. Upon exit, the
+                                actual firmware volume in which the file was
+                                found.
+  @param FileHandle             Upon exit, points to the found file's
+                                handle or NULL if it could not be found.
+
+  @retval EFI_SUCCESS           File was found.
+  @retval EFI_NOT_FOUND         File was not found.
+  @retval EFI_INVALID_PARAMETER FvHandle or FileHandle or
+                                FileName was NULL.
+
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindFileByName (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN  CONST  EFI_GUID                    *FileName,
+  IN  EFI_PEI_FV_HANDLE                  *FvHandle,
+  OUT EFI_PEI_FILE_HANDLE                *FileHandle  
+  )
+{
+  EFI_STATUS        Status;
+  PEI_CORE_INSTANCE *PrivateData;
+  UINTN             Index;
+  
+  if ((FvHandle == NULL) || (FileName == NULL) || (FileHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  if (*FvHandle != NULL) {
+    Status = FindFileEx (*FvHandle, FileName, 0, FileHandle, NULL);
+    if (Status == EFI_NOT_FOUND) {
+      *FileHandle = NULL;
+    }
+  } else {   
+    //
+    // If *FvHandle = NULL, so search all FV for given filename
+    //
+    Status = EFI_NOT_FOUND;
+    
+    PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer());
+    for (Index = 0; Index < PrivateData->FvCount; Index ++) {
+      //
+      // Only search the FV which is associated with a EFI_PEI_FIRMWARE_VOLUME_PPI instance.
+      //
+      if (PrivateData->Fv[Index].FvPpi != NULL) {
+        Status = FindFileEx (PrivateData->Fv[Index].FvHandle, FileName, 0, FileHandle, NULL);
+        if (!EFI_ERROR (Status)) {
+          *FvHandle = PrivateData->Fv[Index].FvHandle;
+          break;
+        }
+      }
+    }
+  }
+  
+  return Status;  
+}  
+
+/**
+  Returns information about a specific file.
+
+  This function returns information about a specific
+  file, including its file name, type, attributes, starting
+  address and size. 
+   
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileHandle               Handle of the file.
+  @param FileInfo                 Upon exit, points to the file's
+                                  information.
+
+  @retval EFI_SUCCESS             File information returned.
+  @retval EFI_INVALID_PARAMETER   If FileHandle does not
+                                  represent a valid file.
+  @retval EFI_INVALID_PARAMETER   If FileInfo is NULL.
+  
+**/ 
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetFileInfo (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI   *This, 
+  IN        EFI_PEI_FILE_HANDLE           FileHandle, 
+  OUT       EFI_FV_FILE_INFO              *FileInfo
+  )
+{
+  UINT8                       FileState;
+  UINT8                       ErasePolarity;
+  EFI_FFS_FILE_HEADER         *FileHeader;
+  PEI_CORE_FV_HANDLE          *CoreFvHandle;
+  PEI_FW_VOL_INSTANCE         *FwVolInstance;
+
+  if ((FileHandle == NULL) || (FileInfo == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the FirmwareVolume which the file resides in.
+  //
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if (CoreFvHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FwVolInstance = PEI_FW_VOL_INSTANCE_FROM_FV_THIS (This);
+
+  if ((CoreFvHandle->FvHeader->Attributes & EFI_FVB2_ERASE_POLARITY) != 0) {
+    ErasePolarity = 1;
+  } else {
+    ErasePolarity = 0;
+  }
+
+  //
+  // Get FileState which is the highest bit of the State 
+  //
+  FileState = GetFileState (ErasePolarity, (EFI_FFS_FILE_HEADER*)FileHandle);
+
+  switch (FileState) {
+    case EFI_FILE_DATA_VALID:
+    case EFI_FILE_MARKED_FOR_UPDATE:
+      break;  
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+
+  FileHeader = (EFI_FFS_FILE_HEADER *)FileHandle;
+  if (IS_FFS_FILE2 (FileHeader)) {
+    ASSERT (FFS_FILE2_SIZE (FileHeader) > 0x00FFFFFF);
+    if (!FwVolInstance->IsFfs3Fv) {
+      DEBUG ((EFI_D_ERROR, "It is a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FileHeader->Name));
+      return EFI_INVALID_PARAMETER;
+    }
+    FileInfo->BufferSize = FFS_FILE2_SIZE (FileHeader) - sizeof (EFI_FFS_FILE_HEADER2);
+    FileInfo->Buffer = (UINT8 *) FileHeader + sizeof (EFI_FFS_FILE_HEADER2);
+  } else {
+    FileInfo->BufferSize = FFS_FILE_SIZE (FileHeader) - sizeof (EFI_FFS_FILE_HEADER);
+    FileInfo->Buffer = (UINT8 *) FileHeader + sizeof (EFI_FFS_FILE_HEADER);
+  }
+  CopyMem (&FileInfo->FileName, &FileHeader->Name, sizeof(EFI_GUID));
+  FileInfo->FileType = FileHeader->Type;
+  FileInfo->FileAttributes = FfsAttributes2FvFileAttributes (FileHeader->Attributes);
+  if ((CoreFvHandle->FvHeader->Attributes & EFI_FVB2_MEMORY_MAPPED) == EFI_FVB2_MEMORY_MAPPED) {
+    FileInfo->FileAttributes |= EFI_FV_FILE_ATTRIB_MEMORY_MAPPED;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns information about a specific file.
+
+  This function returns information about a specific
+  file, including its file name, type, attributes, starting
+  address, size and authentication status.
+
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileHandle               Handle of the file.
+  @param FileInfo                 Upon exit, points to the file's
+                                  information.
+
+  @retval EFI_SUCCESS             File information returned.
+  @retval EFI_INVALID_PARAMETER   If FileHandle does not
+                                  represent a valid file.
+  @retval EFI_INVALID_PARAMETER   If FileInfo is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetFileInfo2 (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI   *This, 
+  IN        EFI_PEI_FILE_HANDLE           FileHandle, 
+  OUT       EFI_FV_FILE_INFO2             *FileInfo
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_CORE_FV_HANDLE          *CoreFvHandle;
+
+  if ((FileHandle == NULL) || (FileInfo == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the FirmwareVolume which the file resides in.
+  //
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if (CoreFvHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PeiFfsFvPpiGetFileInfo (This, FileHandle, (EFI_FV_FILE_INFO *) FileInfo);
+  if (!EFI_ERROR (Status)) {
+    FileInfo->AuthenticationStatus = CoreFvHandle->AuthenticationStatus;
+  }
+
+  return Status;
+}
+
+/**
+  This function returns information about the firmware volume.
+  
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FvHandle                 Handle to the firmware handle.
+  @param VolumeInfo               Points to the returned firmware volume
+                                  information.
+
+  @retval EFI_SUCCESS             Information returned successfully.
+  @retval EFI_INVALID_PARAMETER   FvHandle does not indicate a valid
+                                  firmware volume or VolumeInfo is NULL.
+
+**/   
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetVolumeInfo (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI   *This, 
+  IN  EFI_PEI_FV_HANDLE                    FvHandle, 
+  OUT EFI_FV_INFO                          *VolumeInfo
+  )
+{
+  EFI_FIRMWARE_VOLUME_HEADER             FwVolHeader;
+  EFI_FIRMWARE_VOLUME_EXT_HEADER         *FwVolExHeaderInfo;
+
+  if ((VolumeInfo == NULL) || (FvHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // VolumeHandle may not align at 8 byte, 
+  // but FvLength is UINT64 type, which requires FvHeader align at least 8 byte. 
+  // So, Copy FvHeader into the local FvHeader structure.
+  //
+  CopyMem (&FwVolHeader, FvHandle, sizeof (EFI_FIRMWARE_VOLUME_HEADER));
+
+  //
+  // Check Fv Image Signature
+  //
+  if (FwVolHeader.Signature != EFI_FVH_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (VolumeInfo, sizeof (EFI_FV_INFO));
+  VolumeInfo->FvAttributes  = FwVolHeader.Attributes;
+  VolumeInfo->FvStart       = (VOID *) FvHandle;
+  VolumeInfo->FvSize        = FwVolHeader.FvLength;
+  CopyMem (&VolumeInfo->FvFormat, &FwVolHeader.FileSystemGuid, sizeof(EFI_GUID));
+
+  if (FwVolHeader.ExtHeaderOffset != 0) {
+    FwVolExHeaderInfo = (EFI_FIRMWARE_VOLUME_EXT_HEADER*)(((UINT8 *)FvHandle) + FwVolHeader.ExtHeaderOffset);
+    CopyMem (&VolumeInfo->FvName, &FwVolExHeaderInfo->FvName, sizeof(EFI_GUID));
+  }
+  
+  return EFI_SUCCESS;  
+}    
+
+/**
+  Find the next matching section in the firmware file.
+  
+  This service enables PEI modules to discover sections
+  of a given type within a valid file.
+  
+  @param This             Points to this instance of the
+                          EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType       A filter to find only sections of this
+                          type.
+  @param FileHandle       Handle of firmware file in which to
+                          search.
+  @param SectionData      Updated upon return to point to the
+                          section found.
+  
+  @retval EFI_SUCCESS     Section was found.
+  @retval EFI_NOT_FOUND   Section of the specified type was not
+                          found. SectionData contains NULL.
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindSectionByType (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI    *This,
+  IN        EFI_SECTION_TYPE               SearchType,
+  IN        EFI_PEI_FILE_HANDLE            FileHandle,
+  OUT VOID                                 **SectionData
+  )
+{
+  UINT32 AuthenticationStatus;
+  return PeiFfsFvPpiFindSectionByType2 (This, SearchType, 0, FileHandle, SectionData, &AuthenticationStatus);
+}
+
+/**
+  Find the next matching section in the firmware file.
+
+  This service enables PEI modules to discover sections
+  of a given instance and type within a valid file.
+
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType             A filter to find only sections of this
+                                type.
+  @param SearchInstance         A filter to find the specific instance
+                                of sections.
+  @param FileHandle             Handle of firmware file in which to
+                                search.
+  @param SectionData            Updated upon return to point to the
+                                section found.
+  @param AuthenticationStatus   Updated upon return to point to the
+                                authentication status for this section.
+
+  @retval EFI_SUCCESS     Section was found.
+  @retval EFI_NOT_FOUND   Section of the specified type was not
+                          found. SectionData contains NULL.
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindSectionByType2 (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI    *This,
+  IN        EFI_SECTION_TYPE               SearchType,
+  IN        UINTN                          SearchInstance,
+  IN        EFI_PEI_FILE_HANDLE            FileHandle,
+  OUT VOID                                 **SectionData,
+  OUT UINT32                               *AuthenticationStatus
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_FFS_FILE_HEADER                     *FfsFileHeader;
+  UINT32                                  FileSize;
+  EFI_COMMON_SECTION_HEADER               *Section;
+  PEI_FW_VOL_INSTANCE                     *FwVolInstance;
+  PEI_CORE_FV_HANDLE                      *CoreFvHandle;
+  UINTN                                   Instance;
+  UINT32                                  ExtractedAuthenticationStatus;
+
+  if (SectionData == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  FwVolInstance = PEI_FW_VOL_INSTANCE_FROM_FV_THIS (This);
+
+  //
+  // Retrieve the FirmwareVolume which the file resides in.
+  //
+  CoreFvHandle = FileHandleToVolume (FileHandle);
+  if (CoreFvHandle == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  FfsFileHeader = (EFI_FFS_FILE_HEADER *)(FileHandle);
+
+  if (IS_FFS_FILE2 (FfsFileHeader)) {
+    ASSERT (FFS_FILE2_SIZE (FfsFileHeader) > 0x00FFFFFF);
+    if (!FwVolInstance->IsFfs3Fv) {
+      DEBUG ((EFI_D_ERROR, "It is a FFS3 formatted file: %g in a non-FFS3 formatted FV.\n", &FfsFileHeader->Name));
+      return EFI_NOT_FOUND;
+    }
+    Section = (EFI_COMMON_SECTION_HEADER *) ((UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER2));
+    FileSize = FFS_FILE2_SIZE (FfsFileHeader) - sizeof (EFI_FFS_FILE_HEADER2);
+  } else {
+    Section = (EFI_COMMON_SECTION_HEADER *) ((UINT8 *) FfsFileHeader + sizeof (EFI_FFS_FILE_HEADER));
+    FileSize = FFS_FILE_SIZE (FfsFileHeader) - sizeof (EFI_FFS_FILE_HEADER);
+  }
+
+  Instance = SearchInstance + 1;
+  ExtractedAuthenticationStatus = 0;
+  Status = ProcessSection (
+             GetPeiServicesTablePointer (),
+             SearchType,
+             &Instance,
+             Section, 
+             FileSize, 
+             SectionData,
+             &ExtractedAuthenticationStatus,
+             FwVolInstance->IsFfs3Fv
+             );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Inherit the authentication status.
+    //
+    *AuthenticationStatus = ExtractedAuthenticationStatus | CoreFvHandle->AuthenticationStatus;
+  }
+  return Status;
+}
+
+/**
+  Convert the handle of FV to pointer of corresponding PEI_CORE_FV_HANDLE.
+  
+  @param FvHandle   The handle of a FV.
+  
+  @retval NULL if can not find.
+  @return Pointer of corresponding PEI_CORE_FV_HANDLE. 
+**/
+PEI_CORE_FV_HANDLE *
+FvHandleToCoreHandle (
+  IN EFI_PEI_FV_HANDLE  FvHandle
+  )
+{
+  UINTN             Index;
+  PEI_CORE_INSTANCE *PrivateData;
+  
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer());  
+  for (Index = 0; Index < PrivateData->FvCount; Index ++) {
+    if (FvHandle == PrivateData->Fv[Index].FvHandle) {
+      return &PrivateData->Fv[Index];
+    }
+  }
+  
+  return NULL;
+}  
+
+/**
+  Get instance of PEI_CORE_FV_HANDLE for next volume according to given index.
+  
+  This routine also will install FvInfo ppi for FV hob in PI ways.
+  
+  @param Private    Pointer of PEI_CORE_INSTANCE
+  @param Instance   The index of FV want to be searched.
+  
+  @return Instance of PEI_CORE_FV_HANDLE.
+**/
+PEI_CORE_FV_HANDLE *
+FindNextCoreFvHandle (
+  IN PEI_CORE_INSTANCE  *Private,
+  IN UINTN              Instance
+  )
+{
+  UINTN                    Index;
+  BOOLEAN                  Match;
+  EFI_HOB_FIRMWARE_VOLUME  *FvHob;
+  
+  //
+  // Handle Framework FvHob and Install FvInfo Ppi for it.
+  //
+  if (FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    //
+    // Loop to search the wanted FirmwareVolume which supports FFS
+    //
+    FvHob = (EFI_HOB_FIRMWARE_VOLUME *)GetFirstHob (EFI_HOB_TYPE_FV);
+    while (FvHob != NULL) {
+      //
+      // Search whether FvHob has been installed into PeiCore's FV database.
+      // If found, no need install new FvInfoPpi for it.
+      //
+      for (Index = 0, Match = FALSE; Index < Private->FvCount; Index++) {
+        if ((EFI_PEI_FV_HANDLE)(UINTN)FvHob->BaseAddress == Private->Fv[Index].FvHeader) {
+          Match = TRUE;
+          break;
+        }
+      }
+      
+      //
+      // Search whether FvHob has been cached into PeiCore's Unknown FV database.
+      // If found, no need install new FvInfoPpi for it.
+      //
+      if (!Match) {
+        for (Index = 0; Index < Private->UnknownFvInfoCount; Index ++) {
+          if ((UINTN)FvHob->BaseAddress == (UINTN)Private->UnknownFvInfo[Index].FvInfo) {
+            Match = TRUE;
+            break;
+          }
+        }
+      }
+
+      //
+      // If the Fv in FvHob has not been installed into PeiCore's FV database and has
+      // not been cached into PeiCore's Unknown FV database, install a new FvInfoPpi
+      // for it then PeiCore will dispatch it in callback of FvInfoPpi.
+      //
+      if (!Match) {
+        PeiServicesInstallFvInfoPpi (
+          &(((EFI_FIRMWARE_VOLUME_HEADER *)(UINTN)FvHob->BaseAddress)->FileSystemGuid),
+          (VOID *)(UINTN)FvHob->BaseAddress,
+          (UINT32)FvHob->Length,
+          NULL,
+          NULL
+          );
+      }
+      
+      FvHob = (EFI_HOB_FIRMWARE_VOLUME *)GetNextHob (EFI_HOB_TYPE_FV, (VOID *)((UINTN)FvHob + FvHob->Header.HobLength)); 
+    }
+  }
+
+  ASSERT (Private->FvCount <= PcdGet32 (PcdPeiCoreMaxFvSupported));
+  if (Instance >= Private->FvCount) {
+    return NULL;
+  }
+
+  return &Private->Fv[Instance];
+}  
+
+/**
+  After PeiCore image is shadowed into permanent memory, all build-in FvPpi should
+  be re-installed with the instance in permanent memory and all cached FvPpi pointers in 
+  PrivateData->Fv[] array should be fixed up to be pointed to the one in permenant
+  memory.
+  
+  @param PrivateData   Pointer to PEI_CORE_INSTANCE.
+**/  
+VOID
+PeiReinitializeFv (
+  IN  PEI_CORE_INSTANCE           *PrivateData
+  )
+{
+  VOID                    *OldFfsFvPpi;
+  EFI_PEI_PPI_DESCRIPTOR  *OldDescriptor;
+  UINTN                   Index;
+  EFI_STATUS              Status;
+
+  //
+  // Locate old build-in Ffs2 EFI_PEI_FIRMWARE_VOLUME_PPI which
+  // in flash.
+  //
+  Status = PeiServicesLocatePpi (
+            &gEfiFirmwareFileSystem2Guid,
+            0,
+            &OldDescriptor,
+            &OldFfsFvPpi
+            );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Re-install the EFI_PEI_FIRMWARE_VOLUME_PPI for build-in Ffs2
+  // which is shadowed from flash to permanent memory within PeiCore image.
+  //
+  Status = PeiServicesReInstallPpi (OldDescriptor, &mPeiFfs2FvPpiList);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Fixup all FvPpi pointers for the implementation in flash to permanent memory.
+  //
+  for (Index = 0; Index < PcdGet32 (PcdPeiCoreMaxFvSupported); Index ++) {
+    if (PrivateData->Fv[Index].FvPpi == OldFfsFvPpi) {
+      PrivateData->Fv[Index].FvPpi = &mPeiFfs2FwVol.Fv;
+    }
+  }
+
+  //
+  // Locate old build-in Ffs3 EFI_PEI_FIRMWARE_VOLUME_PPI which
+  // in flash.
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiFirmwareFileSystem3Guid,
+             0,
+             &OldDescriptor,
+             &OldFfsFvPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Re-install the EFI_PEI_FIRMWARE_VOLUME_PPI for build-in Ffs3
+  // which is shadowed from flash to permanent memory within PeiCore image.
+  //
+  Status = PeiServicesReInstallPpi (OldDescriptor, &mPeiFfs3FvPpiList);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Fixup all FvPpi pointers for the implementation in flash to permanent memory.
+  //
+  for (Index = 0; Index < PcdGet32 (PcdPeiCoreMaxFvSupported); Index ++) {
+    if (PrivateData->Fv[Index].FvPpi == OldFfsFvPpi) {
+      PrivateData->Fv[Index].FvPpi = &mPeiFfs3FwVol.Fv;
+    }
+  }
+}
+
+/**
+  Report the information for a new discoveried FV in unknown third-party format.
+  
+  If the EFI_PEI_FIRMWARE_VOLUME_PPI has not been installed for third-party FV format, but
+  the FV in this format has been discoveried, then this FV's information will be cached into
+  PEI_CORE_INSTANCE's UnknownFvInfo array.
+  Also a notification would be installed for unknown third-party FV format guid, if EFI_PEI_FIRMWARE_VOLUME_PPI
+  is installed later by platform's PEIM, the original unknown third-party FV will be processed by
+  using new installed EFI_PEI_FIRMWARE_VOLUME_PPI.
+  
+  @param PrivateData  Point to instance of PEI_CORE_INSTANCE
+  @param FvInfo2Ppi   Point to FvInfo2 PPI.
+  
+  @retval EFI_OUT_OF_RESOURCES  The FV info array in PEI_CORE_INSTANCE has no more spaces.
+  @retval EFI_SUCCESS           Success to add the information for unknown FV.
+**/
+EFI_STATUS
+AddUnknownFormatFvInfo (
+  IN PEI_CORE_INSTANCE                  *PrivateData,
+  IN EFI_PEI_FIRMWARE_VOLUME_INFO2_PPI  *FvInfo2Ppi
+  )
+{
+  PEI_CORE_UNKNOW_FORMAT_FV_INFO    *NewUnknownFv;
+  
+  if (PrivateData->UnknownFvInfoCount + 1 >= PcdGet32 (PcdPeiCoreMaxFvSupported)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  NewUnknownFv = &PrivateData->UnknownFvInfo[PrivateData->UnknownFvInfoCount];
+  PrivateData->UnknownFvInfoCount ++;
+  
+  CopyGuid (&NewUnknownFv->FvFormat, &FvInfo2Ppi->FvFormat);
+  NewUnknownFv->FvInfo     = FvInfo2Ppi->FvInfo;
+  NewUnknownFv->FvInfoSize = FvInfo2Ppi->FvInfoSize;
+  NewUnknownFv->AuthenticationStatus = FvInfo2Ppi->AuthenticationStatus;
+  NewUnknownFv->NotifyDescriptor.Flags  = (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+  NewUnknownFv->NotifyDescriptor.Guid   = &NewUnknownFv->FvFormat;
+  NewUnknownFv->NotifyDescriptor.Notify = ThirdPartyFvPpiNotifyCallback;
+  
+  PeiServicesNotifyPpi (&NewUnknownFv->NotifyDescriptor);
+  return EFI_SUCCESS;
+}
+
+/**
+  Find the FV information according to third-party FV format guid.
+  
+  This routine also will remove the FV information found by given FV format guid from
+  PrivateData->UnknownFvInfo[].
+  
+  @param PrivateData      Point to instance of PEI_CORE_INSTANCE
+  @param Format           Point to given FV format guid
+  @param FvInfo           On return, the pointer of FV information buffer
+  @param FvInfoSize       On return, the size of FV information buffer.
+  @param AuthenticationStatus On return, the authentication status of FV information buffer.
+  
+  @retval EFI_NOT_FOUND  The FV is not found for new installed EFI_PEI_FIRMWARE_VOLUME_PPI
+  @retval EFI_SUCCESS    Success to find a FV which could be processed by new installed EFI_PEI_FIRMWARE_VOLUME_PPI.
+**/
+EFI_STATUS
+FindUnknownFormatFvInfo (
+  IN  PEI_CORE_INSTANCE *PrivateData,
+  IN  EFI_GUID          *Format,
+  OUT VOID              **FvInfo,
+  OUT UINT32            *FvInfoSize,
+  OUT UINT32            *AuthenticationStatus
+  )
+{
+  UINTN Index;
+  UINTN Index2;
+
+  Index = 0;
+  for (; Index < PrivateData->UnknownFvInfoCount; Index ++) {
+    if (CompareGuid (Format, &PrivateData->UnknownFvInfo[Index].FvFormat)) {
+      break;
+    }
+  }
+  
+  if (Index == PrivateData->UnknownFvInfoCount) {
+    return EFI_NOT_FOUND;
+  }
+  
+  *FvInfo     = PrivateData->UnknownFvInfo[Index].FvInfo;
+  *FvInfoSize = PrivateData->UnknownFvInfo[Index].FvInfoSize;
+  *AuthenticationStatus = PrivateData->UnknownFvInfo[Index].AuthenticationStatus;
+  
+  //
+  // Remove an entry from UnknownFvInfo array.
+  //
+  Index2 = Index + 1;
+  for (;Index2 < PrivateData->UnknownFvInfoCount; Index2 ++, Index ++) {
+    CopyMem (&PrivateData->UnknownFvInfo[Index], &PrivateData->UnknownFvInfo[Index2], sizeof (PEI_CORE_UNKNOW_FORMAT_FV_INFO));
+  }
+  PrivateData->UnknownFvInfoCount --;
+  return EFI_SUCCESS;
+}  
+
+/**
+  Notification callback function for EFI_PEI_FIRMWARE_VOLUME_PPI.
+  
+  When a EFI_PEI_FIRMWARE_VOLUME_PPI is installed to support new FV format, this 
+  routine is called to process all discoveried FVs in this format.
+  
+  @param PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param NotifyDescriptor  Address of the notification descriptor data structure.
+  @param Ppi               Address of the PPI that was installed.
+  
+  @retval EFI_SUCCESS  The notification callback is processed correctly.
+**/
+EFI_STATUS
+EFIAPI
+ThirdPartyFvPpiNotifyCallback (
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR     *NotifyDescriptor,
+  IN VOID                          *Ppi
+  )
+{
+  PEI_CORE_INSTANCE            *PrivateData;
+  EFI_PEI_FIRMWARE_VOLUME_PPI  *FvPpi;
+  VOID                         *FvInfo;
+  UINT32                       FvInfoSize;
+  UINT32                       AuthenticationStatus;
+  EFI_STATUS                   Status;
+  EFI_PEI_FV_HANDLE            FvHandle;
+  BOOLEAN                      IsProcessed;
+  UINTN                        FvIndex;
+  EFI_PEI_FILE_HANDLE          FileHandle;
+  VOID                         *DepexData;  
+  
+  PrivateData  = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+  FvPpi = (EFI_PEI_FIRMWARE_VOLUME_PPI*) Ppi;
+  
+  do {
+    Status = FindUnknownFormatFvInfo (PrivateData, NotifyDescriptor->Guid, &FvInfo, &FvInfoSize, &AuthenticationStatus);
+    if (EFI_ERROR (Status)) {
+      return EFI_SUCCESS;
+    }
+    
+    //
+    // Process new found FV and get FV handle.
+    //
+    Status = FvPpi->ProcessVolume (FvPpi, FvInfo, FvInfoSize, &FvHandle);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Fail to process the FV 0x%p, FV may be corrupted!\n", FvInfo));
+      continue;
+    }
+
+    //
+    // Check whether the FV has already been processed.
+    //
+    IsProcessed = FALSE;
+    for (FvIndex = 0; FvIndex < PrivateData->FvCount; FvIndex ++) {
+      if (PrivateData->Fv[FvIndex].FvHandle == FvHandle) {
+        DEBUG ((EFI_D_INFO, "The Fv %p has already been processed!\n", FvInfo));
+        IsProcessed = TRUE;
+        break;
+      }
+    }
+  
+    if (IsProcessed) {
+      continue;
+    }
+    
+    if (PrivateData->FvCount >= PcdGet32 (PcdPeiCoreMaxFvSupported)) {
+      DEBUG ((EFI_D_ERROR, "The number of Fv Images (%d) exceed the max supported FVs (%d) in Pei", PrivateData->FvCount + 1, PcdGet32 (PcdPeiCoreMaxFvSupported)));
+      DEBUG ((EFI_D_ERROR, "PcdPeiCoreMaxFvSupported value need be reconfigurated in DSC"));
+      ASSERT (FALSE);
+    }
+        
+    //
+    // Update internal PEI_CORE_FV array.
+    //
+    PrivateData->Fv[PrivateData->FvCount].FvHeader = (EFI_FIRMWARE_VOLUME_HEADER*) FvInfo;
+    PrivateData->Fv[PrivateData->FvCount].FvPpi    = FvPpi;
+    PrivateData->Fv[PrivateData->FvCount].FvHandle = FvHandle;
+    PrivateData->Fv[PrivateData->FvCount].AuthenticationStatus = AuthenticationStatus;
+    DEBUG ((
+      EFI_D_INFO, 
+      "The %dth FV start address is 0x%11p, size is 0x%08x, handle is 0x%p\n", 
+      (UINT32) PrivateData->FvCount, 
+      (VOID *) FvInfo, 
+      FvInfoSize,
+      FvHandle
+      ));    
+    PrivateData->FvCount ++;
+
+    //
+    // Scan and process the new discoveried FV for EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE 
+    //
+    FileHandle = NULL;
+    do {
+      Status = FvPpi->FindFileByType (
+                        FvPpi,
+                        EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE,
+                        FvHandle,
+                        &FileHandle
+                       );
+      if (!EFI_ERROR (Status)) {
+        Status = FvPpi->FindSectionByType (
+                          FvPpi,
+                          EFI_SECTION_PEI_DEPEX,
+                          FileHandle,
+                          (VOID**)&DepexData
+                          );
+        if (!EFI_ERROR (Status)) {
+          if (!PeimDispatchReadiness (PeiServices, DepexData)) {
+            //
+            // Dependency is not satisfied.
+            //
+            continue;
+          }
+        }
+        
+        DEBUG ((EFI_D_INFO, "Found firmware volume Image File %p in FV[%d] %p\n", FileHandle, PrivateData->FvCount - 1, FvHandle));
+        ProcessFvFile (PrivateData, &PrivateData->Fv[PrivateData->FvCount - 1], FileHandle);
+      }
+    } while (FileHandle != NULL);
+  } while (TRUE);
+}
diff --git a/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.h b/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.h
new file mode 100644
index 0000000000..1daeb6d97b
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/FwVol/FwVol.h
@@ -0,0 +1,377 @@
+/** @file
+  The internal header file for firmware volume related definitions.
+  
+Copyright (c) 2009 - 2011, 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.             
+
+**/
+
+#ifndef _FWVOL_H_
+#define _FWVOL_H_
+
+#include "PeiMain.h"
+
+#define GET_OCCUPIED_SIZE(ActualSize, Alignment) \
+  ((ActualSize) + (((Alignment) - ((ActualSize) & ((Alignment) - 1))) & ((Alignment) - 1)))
+
+
+#define PEI_FW_VOL_SIGNATURE  SIGNATURE_32('P','F','W','V')
+
+typedef struct {
+  UINTN                         Signature;
+  BOOLEAN                       IsFfs3Fv;
+  EFI_PEI_FIRMWARE_VOLUME_PPI   Fv;
+} PEI_FW_VOL_INSTANCE;
+
+#define PEI_FW_VOL_INSTANCE_FROM_FV_THIS(a) \
+  CR(a, PEI_FW_VOL_INSTANCE, Fv, PEI_FW_VOL_SIGNATURE)
+
+
+/**
+  Process a firmware volume and create a volume handle.
+
+  Create a volume handle from the information in the buffer. For
+  memory-mapped firmware volumes, Buffer and BufferSize refer to
+  the start of the firmware volume and the firmware volume size.
+  For non memory-mapped firmware volumes, this points to a
+  buffer which contains the necessary information for creating
+  the firmware volume handle. Normally, these values are derived
+  from the EFI_FIRMWARE_VOLUME_INFO_PPI.
+  
+  
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param Buffer                 Points to the start of the buffer.
+  @param BufferSize             Size of the buffer.
+  @param FvHandle               Points to the returned firmware volume
+                                handle. The firmware volume handle must
+                                be unique within the system. 
+
+  @retval EFI_SUCCESS           Firmware volume handle created.
+  @retval EFI_VOLUME_CORRUPTED  Volume was corrupt.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiProcessVolume (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN  VOID                               *Buffer,
+  IN  UINTN                              BufferSize,
+  OUT EFI_PEI_FV_HANDLE                  *FvHandle
+  );
+  
+/**
+  Finds the next file of the specified type.
+
+  This service enables PEI modules to discover additional firmware files. 
+  The FileHandle must be unique within the system.
+
+  @param This           Points to this instance of the
+                        EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType     A filter to find only files of this type. Type
+                        EFI_FV_FILETYPE_ALL causes no filtering to be
+                        done.
+  @param FvHandle       Handle of firmware volume in which to
+                        search.
+  @param FileHandle     Points to the current handle from which to
+                        begin searching or NULL to start at the
+                        beginning of the firmware volume. Updated
+                        upon return to reflect the file found.
+
+  @retval EFI_SUCCESS   The file was found.
+  @retval EFI_NOT_FOUND The file was not found. FileHandle contains NULL.
+
+**/  
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindFileByType (
+  IN CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN        EFI_FV_FILETYPE             SearchType,
+  IN        EFI_PEI_FV_HANDLE           FvHandle,
+  IN OUT    EFI_PEI_FILE_HANDLE         *FileHandle
+  );
+
+/**
+  Find a file within a volume by its name. 
+  
+  This service searches for files with a specific name, within
+  either the specified firmware volume or all firmware volumes.
+
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileName               A pointer to the name of the file to find
+                                within the firmware volume.
+  @param FvHandle               Upon entry, the pointer to the firmware
+                                volume to search or NULL if all firmware
+                                volumes should be searched. Upon exit, the
+                                actual firmware volume in which the file was
+                                found.
+  @param FileHandle             Upon exit, points to the found file's
+                                handle or NULL if it could not be found.
+
+  @retval EFI_SUCCESS           File was found.
+  @retval EFI_NOT_FOUND         File was not found.
+  @retval EFI_INVALID_PARAMETER FvHandle or FileHandle or
+                                FileName was NULL.
+
+
+**/    
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindFileByName (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI *This,
+  IN  CONST  EFI_GUID                    *FileName,
+  IN  EFI_PEI_FV_HANDLE                  *FvHandle,
+  OUT EFI_PEI_FILE_HANDLE                *FileHandle  
+  );
+
+/**
+  Find the next matching section in the firmware file.
+  
+  This service enables PEI modules to discover sections
+  of a given type within a valid file.
+  
+  @param This             Points to this instance of the
+                          EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType       A filter to find only sections of this
+                          type.
+  @param FileHandle       Handle of firmware file in which to
+                          search.
+  @param SectionData      Updated upon  return to point to the
+                          section found.
+  
+  @retval EFI_SUCCESS     Section was found.
+  @retval EFI_NOT_FOUND   Section of the specified type was not
+                          found. SectionData contains NULL.
+**/      
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindSectionByType (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI    *This,
+  IN        EFI_SECTION_TYPE               SearchType,
+  IN        EFI_PEI_FILE_HANDLE            FileHandle,
+  OUT VOID                                 **SectionData
+  );
+
+/**
+  Find the next matching section in the firmware file.
+
+  This service enables PEI modules to discover sections
+  of a given instance and type within a valid file.
+
+  @param This                   Points to this instance of the
+                                EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param SearchType             A filter to find only sections of this
+                                type.
+  @param SearchInstance         A filter to find the specific instance
+                                of sections.
+  @param FileHandle             Handle of firmware file in which to
+                                search.
+  @param SectionData            Updated upon return to point to the
+                                section found.
+  @param AuthenticationStatus   Updated upon return to point to the
+                                authentication status for this section.
+
+  @retval EFI_SUCCESS     Section was found.
+  @retval EFI_NOT_FOUND   Section of the specified type was not
+                          found. SectionData contains NULL.
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiFindSectionByType2 (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI    *This,
+  IN        EFI_SECTION_TYPE               SearchType,
+  IN        UINTN                          SearchInstance,
+  IN        EFI_PEI_FILE_HANDLE            FileHandle,
+  OUT VOID                                 **SectionData,
+  OUT UINT32                               *AuthenticationStatus
+  );
+
+/**
+  Returns information about a specific file.
+
+  This function returns information about a specific
+  file, including its file name, type, attributes, starting
+  address and size. 
+   
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileHandle               Handle of the file.
+  @param FileInfo                 Upon exit, points to the file's
+                                  information.
+
+  @retval EFI_SUCCESS             File information returned.
+  @retval EFI_INVALID_PARAMETER   If FileHandle does not
+                                  represent a valid file.
+  @retval EFI_INVALID_PARAMETER   If FileInfo is NULL.
+  
+**/         
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetFileInfo (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI   *This, 
+  IN        EFI_PEI_FILE_HANDLE           FileHandle, 
+  OUT       EFI_FV_FILE_INFO              *FileInfo
+  );
+
+/**
+  Returns information about a specific file.
+
+  This function returns information about a specific
+  file, including its file name, type, attributes, starting
+  address, size and authentication status.
+
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FileHandle               Handle of the file.
+  @param FileInfo                 Upon exit, points to the file's
+                                  information.
+
+  @retval EFI_SUCCESS             File information returned.
+  @retval EFI_INVALID_PARAMETER   If FileHandle does not
+                                  represent a valid file.
+  @retval EFI_INVALID_PARAMETER   If FileInfo is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetFileInfo2 (
+  IN  CONST EFI_PEI_FIRMWARE_VOLUME_PPI   *This,
+  IN        EFI_PEI_FILE_HANDLE           FileHandle,
+  OUT       EFI_FV_FILE_INFO2             *FileInfo
+  );
+
+/**
+  This function returns information about the firmware volume.
+  
+  @param This                     Points to this instance of the
+                                  EFI_PEI_FIRMWARE_VOLUME_PPI.
+  @param FvHandle                 Handle to the firmware handle.
+  @param VolumeInfo               Points to the returned firmware volume
+                                  information.
+
+  @retval EFI_SUCCESS             Information returned successfully.
+  @retval EFI_INVALID_PARAMETER   FvHandle does not indicate a valid
+                                  firmware volume or VolumeInfo is NULL.
+
+**/            
+EFI_STATUS
+EFIAPI
+PeiFfsFvPpiGetVolumeInfo (
+  IN  CONST  EFI_PEI_FIRMWARE_VOLUME_PPI   *This, 
+  IN  EFI_PEI_FV_HANDLE                    FvHandle, 
+  OUT EFI_FV_INFO                          *VolumeInfo
+  );
+
+/**
+  Convert the handle of FV to pointer of corresponding PEI_CORE_FV_HANDLE.
+  
+  @param FvHandle   The handle of a FV.
+  
+  @retval NULL if can not find.
+  @return Pointer of corresponding PEI_CORE_FV_HANDLE. 
+**/
+PEI_CORE_FV_HANDLE *
+FvHandleToCoreHandle (
+  IN EFI_PEI_FV_HANDLE  FvHandle
+  );
+  
+/**
+  Given the input file pointer, search for the next matching file in the
+  FFS volume as defined by SearchType. The search starts from FileHeader inside
+  the Firmware Volume defined by FwVolHeader.
+
+
+  @param FvHandle        Pointer to the FV header of the volume to search
+  @param FileName        File name
+  @param SearchType      Filter to find only files of this type.
+                         Type EFI_FV_FILETYPE_ALL causes no filtering to be done.
+  @param FileHandle      This parameter must point to a valid FFS volume.
+  @param AprioriFile     Pointer to AprioriFile image in this FV if has
+
+  @return EFI_NOT_FOUND  No files matching the search criteria were found
+  @retval EFI_SUCCESS    Success to search given file
+
+**/
+EFI_STATUS
+FindFileEx (
+  IN  CONST EFI_PEI_FV_HANDLE        FvHandle,
+  IN  CONST EFI_GUID                 *FileName,   OPTIONAL
+  IN        EFI_FV_FILETYPE          SearchType,
+  IN OUT    EFI_PEI_FILE_HANDLE      *FileHandle,
+  IN OUT    EFI_PEI_FV_HANDLE        *AprioriFile  OPTIONAL
+  );
+
+/**
+  Report the information for a new discoveried FV in unknown format.
+  
+  If the EFI_PEI_FIRMWARE_VOLUME_PPI has not been installed for specifical FV format, but
+  the FV in this FV format has been discoveried, then the information of this FV
+  will be cached into PEI_CORE_INSTANCE's UnknownFvInfo array.
+  Also a notification would be installed for unknown FV format guid, if EFI_PEI_FIRMWARE_VOLUME_PPI
+  is installed later by platform's PEIM, the original unknown FV will be processed by
+  using new installed EFI_PEI_FIRMWARE_VOLUME_PPI.
+  
+  @param PrivateData  Point to instance of PEI_CORE_INSTANCE
+  @param FvInfo2Ppi   Point to FvInfo2 PPI.
+  
+  @retval EFI_OUT_OF_RESOURCES  The FV info array in PEI_CORE_INSTANCE has no more spaces.
+  @retval EFI_SUCCESS           Success to add the information for unknown FV.
+**/
+EFI_STATUS
+AddUnknownFormatFvInfo (
+  IN PEI_CORE_INSTANCE                  *PrivateData,
+  IN EFI_PEI_FIRMWARE_VOLUME_INFO2_PPI  *FvInfo2Ppi
+  );
+  
+/**
+  Find the FV information according to FV format guid.
+  
+  This routine also will remove the FV information found by given FV format guid from
+  PrivateData->UnknownFvInfo[].
+  
+  @param PrivateData      Point to instance of PEI_CORE_INSTANCE
+  @param Format           Point to given FV format guid
+  @param FvInfo           On return, the pointer of FV information buffer in given FV format guid
+  @param FvInfoSize       On return, the size of FV information buffer.
+  @param AuthenticationStatus On return, the authentication status of FV information buffer.
+  
+  @retval EFI_NOT_FOUND  The FV is not found for new installed EFI_PEI_FIRMWARE_VOLUME_PPI
+  @retval EFI_SUCCESS    Success to find a FV which could be processed by new installed EFI_PEI_FIRMWARE_VOLUME_PPI.
+**/
+EFI_STATUS
+FindUnknownFormatFvInfo (
+  IN  PEI_CORE_INSTANCE *PrivateData,
+  IN  EFI_GUID          *Format,
+  OUT VOID              **FvInfo,
+  OUT UINT32            *FvInfoSize,
+  OUT UINT32            *AuthenticationStatus
+  );
+  
+/**
+  Notification callback function for EFI_PEI_FIRMWARE_VOLUME_PPI.
+  
+  When a EFI_PEI_FIRMWARE_VOLUME_PPI is installed to support new FV format, this 
+  routine is called to process all discoveried FVs in this format.
+  
+  @param PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param NotifyDescriptor  Address of the notification descriptor data structure.
+  @param Ppi               Address of the PPI that was installed.
+  
+  @retval EFI_SUCCESS  The notification callback is processed correctly.
+**/
+EFI_STATUS
+EFIAPI
+ThirdPartyFvPpiNotifyCallback (
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR     *NotifyDescriptor,
+  IN VOID                          *Ppi
+  );  
+  
+#endif 
diff --git a/Core/MdeModulePkg/Core/Pei/Hob/Hob.c b/Core/MdeModulePkg/Core/Pei/Hob/Hob.c
new file mode 100644
index 0000000000..e0ee8e7f10
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Hob/Hob.c
@@ -0,0 +1,168 @@
+/** @file
+  This module provide Hand-Off Block manupulation.
+  
+Copyright (c) 2006 - 2012, 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 "PeiMain.h"
+
+/**
+
+  Gets the pointer to the HOB List.
+
+  @param PeiServices                   An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param HobList                       Pointer to the HOB List.
+
+  @retval EFI_SUCCESS                  Get the pointer of HOB List
+  @retval EFI_NOT_AVAILABLE_YET        the HOB List is not yet published
+  @retval EFI_INVALID_PARAMETER        HobList is NULL (in debug mode)
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetHobList (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN OUT VOID          **HobList
+  )
+{
+  PEI_CORE_INSTANCE *PrivateData;
+  
+  //
+  // Only check this parameter in debug mode
+  //
+  
+  DEBUG_CODE_BEGIN ();  
+    if (HobList == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  DEBUG_CODE_END ();
+  
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+
+  *HobList    = PrivateData->HobList.Raw;
+
+  return EFI_SUCCESS;   
+}
+
+
+/**
+  Add a new HOB to the HOB List.
+
+  @param PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Type             Type of the new HOB.
+  @param Length           Length of the new HOB to allocate.
+  @param Hob              Pointer to the new HOB.
+
+  @return  EFI_SUCCESS           Success to create hob.
+  @retval  EFI_INVALID_PARAMETER if Hob is NULL
+  @retval  EFI_NOT_AVAILABLE_YET if HobList is still not available.
+  @retval  EFI_OUT_OF_RESOURCES  if there is no more memory to grow the Hoblist.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiCreateHob (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN UINT16            Type,
+  IN UINT16            Length,
+  IN OUT VOID          **Hob
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_HOB_HANDOFF_INFO_TABLE           *HandOffHob;
+  EFI_HOB_GENERIC_HEADER               *HobEnd;
+  EFI_PHYSICAL_ADDRESS                 FreeMemory;
+
+
+  Status = PeiGetHobList (PeiServices, Hob);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  HandOffHob = *Hob;
+
+  //
+  // Check Length to avoid data overflow. 
+  //
+  if (0x10000 - Length <= 0x7) {
+    return EFI_INVALID_PARAMETER;
+  }
+  Length     = (UINT16)((Length + 0x7) & (~0x7));
+
+  FreeMemory = HandOffHob->EfiFreeMemoryTop -
+               HandOffHob->EfiFreeMemoryBottom;
+
+  if (FreeMemory < Length) {
+    DEBUG ((EFI_D_ERROR, "PeiCreateHob fail: Length - 0x%08x\n", (UINTN)Length));
+    DEBUG ((EFI_D_ERROR, "  FreeMemoryTop    - 0x%08x\n", (UINTN)HandOffHob->EfiFreeMemoryTop));
+    DEBUG ((EFI_D_ERROR, "  FreeMemoryBottom - 0x%08x\n", (UINTN)HandOffHob->EfiFreeMemoryBottom));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  *Hob = (VOID*) (UINTN) HandOffHob->EfiEndOfHobList;
+  ((EFI_HOB_GENERIC_HEADER*) *Hob)->HobType   = Type;
+  ((EFI_HOB_GENERIC_HEADER*) *Hob)->HobLength = Length;
+  ((EFI_HOB_GENERIC_HEADER*) *Hob)->Reserved  = 0;
+
+  HobEnd = (EFI_HOB_GENERIC_HEADER*) ((UINTN) *Hob + Length);
+  HandOffHob->EfiEndOfHobList = (EFI_PHYSICAL_ADDRESS) (UINTN) HobEnd;
+ 
+  HobEnd->HobType   = EFI_HOB_TYPE_END_OF_HOB_LIST;
+  HobEnd->HobLength = (UINT16) sizeof (EFI_HOB_GENERIC_HEADER);
+  HobEnd->Reserved  = 0;
+  HobEnd++;
+  HandOffHob->EfiFreeMemoryBottom = (EFI_PHYSICAL_ADDRESS) (UINTN) HobEnd;
+
+  return EFI_SUCCESS;   
+}
+
+/**
+
+  Builds a Handoff Information Table HOB
+
+  @param BootMode        - Current Bootmode
+  @param MemoryBegin     - Start Memory Address.
+  @param MemoryLength    - Length of Memory.
+
+  @return EFI_SUCCESS Always success to initialize HOB.
+
+**/
+EFI_STATUS
+PeiCoreBuildHobHandoffInfoTable (
+  IN EFI_BOOT_MODE         BootMode,
+  IN EFI_PHYSICAL_ADDRESS  MemoryBegin,
+  IN UINT64                MemoryLength
+  )
+{
+  EFI_HOB_HANDOFF_INFO_TABLE   *Hob;
+  EFI_HOB_GENERIC_HEADER       *HobEnd;
+
+  Hob                      = (VOID *)(UINTN)MemoryBegin;
+  HobEnd                   = (EFI_HOB_GENERIC_HEADER*) (Hob+1);
+  Hob->Header.HobType      = EFI_HOB_TYPE_HANDOFF;
+  Hob->Header.HobLength    = (UINT16) sizeof (EFI_HOB_HANDOFF_INFO_TABLE);
+  Hob->Header.Reserved     = 0;
+  
+  HobEnd->HobType          = EFI_HOB_TYPE_END_OF_HOB_LIST;
+  HobEnd->HobLength        = (UINT16) sizeof (EFI_HOB_GENERIC_HEADER);
+  HobEnd->Reserved         = 0;
+
+  Hob->Version             = EFI_HOB_HANDOFF_TABLE_VERSION;
+  Hob->BootMode            = BootMode;
+  
+  Hob->EfiMemoryTop        = MemoryBegin + MemoryLength;
+  Hob->EfiMemoryBottom     = MemoryBegin;
+  Hob->EfiFreeMemoryTop    = MemoryBegin + MemoryLength;
+  Hob->EfiFreeMemoryBottom = (EFI_PHYSICAL_ADDRESS) (UINTN) (HobEnd + 1);
+  Hob->EfiEndOfHobList     = (EFI_PHYSICAL_ADDRESS) (UINTN) HobEnd;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Pei/Image/Image.c b/Core/MdeModulePkg/Core/Pei/Image/Image.c
new file mode 100644
index 0000000000..9c54192d86
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Image/Image.c
@@ -0,0 +1,909 @@
+/** @file
+  Pei Core Load Image Support
+
+Copyright (c) 2006 - 2015, 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 "PeiMain.h"
+
+
+EFI_PEI_LOAD_FILE_PPI   mPeiLoadImagePpi = {
+  PeiLoadImageLoadImageWrapper
+};
+
+
+EFI_PEI_PPI_DESCRIPTOR     gPpiLoadFilePpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiLoadFilePpiGuid,
+  &mPeiLoadImagePpi
+};
+
+/**
+
+  Support routine for the PE/COFF Loader that reads a buffer from a PE/COFF file.
+  The function is used for XIP code to have optimized memory copy.
+
+  @param FileHandle      - The handle to the PE/COFF file
+  @param FileOffset      - The offset, in bytes, into the file to read
+  @param ReadSize        - The number of bytes to read from the file starting at FileOffset
+  @param Buffer          - A pointer to the buffer to read the data into.
+
+  @return EFI_SUCCESS - ReadSize bytes of data were read into Buffer from the PE/COFF file starting at FileOffset
+
+**/
+EFI_STATUS
+EFIAPI
+PeiImageRead (
+  IN     VOID    *FileHandle,
+  IN     UINTN   FileOffset,
+  IN     UINTN   *ReadSize,
+  OUT    VOID    *Buffer
+  )
+{
+  CHAR8 *Destination8;
+  CHAR8 *Source8;
+  
+  Destination8  = Buffer;
+  Source8       = (CHAR8 *) ((UINTN) FileHandle + FileOffset);
+  if (Destination8 != Source8) {
+    CopyMem (Destination8, Source8, *ReadSize);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Support routine for the PE/COFF Loader that reads a buffer from a PE/COFF file.
+  The function is implemented as PIC so as to support shadowing.
+
+  @param FileHandle      - The handle to the PE/COFF file
+  @param FileOffset      - The offset, in bytes, into the file to read
+  @param ReadSize        - The number of bytes to read from the file starting at FileOffset
+  @param Buffer          - A pointer to the buffer to read the data into.
+
+  @return EFI_SUCCESS - ReadSize bytes of data were read into Buffer from the PE/COFF file starting at FileOffset
+
+**/
+EFI_STATUS
+EFIAPI
+PeiImageReadForShadow (
+  IN     VOID    *FileHandle,
+  IN     UINTN   FileOffset,
+  IN     UINTN   *ReadSize,
+  OUT    VOID    *Buffer
+  )
+{
+  volatile CHAR8  *Destination8;
+  CHAR8           *Source8;
+  UINTN           Length;
+
+  Destination8  = Buffer;
+  Source8       = (CHAR8 *) ((UINTN) FileHandle + FileOffset);
+  if (Destination8 != Source8) {
+    Length        = *ReadSize;
+    while ((Length--) > 0) {
+      *(Destination8++) = *(Source8++);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Support routine to get the Image read file function.
+
+  @param ImageContext    - The context of the image being loaded
+
+  @retval EFI_SUCCESS - If Image function location is found
+
+**/
+EFI_STATUS
+GetImageReadFunction (
+  IN      PE_COFF_LOADER_IMAGE_CONTEXT  *ImageContext
+  )
+{
+  PEI_CORE_INSTANCE  *Private;
+  VOID*  MemoryBuffer;
+
+  Private = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer ());
+  
+  if (Private->PeiMemoryInstalled  && (((Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME) && PcdGetBool (PcdShadowPeimOnBoot)) || 
+      ((Private->HobList.HandoffInformationTable->BootMode == BOOT_ON_S3_RESUME) && PcdGetBool (PcdShadowPeimOnS3Boot))) &&
+      (EFI_IMAGE_MACHINE_TYPE_SUPPORTED(EFI_IMAGE_MACHINE_X64) || EFI_IMAGE_MACHINE_TYPE_SUPPORTED(EFI_IMAGE_MACHINE_IA32))) {
+    // 
+    // Shadow algorithm makes lots of non ANSI C assumptions and only works for IA32 and X64 
+    //  compilers that have been tested
+    //
+    if (Private->ShadowedImageRead == NULL) {
+      MemoryBuffer = AllocatePages (0x400 / EFI_PAGE_SIZE + 1);
+      ASSERT (MemoryBuffer != NULL);
+      CopyMem (MemoryBuffer, (CONST VOID *) (UINTN) PeiImageReadForShadow, 0x400);
+      Private->ShadowedImageRead = (PE_COFF_LOADER_READ_FILE) (UINTN) MemoryBuffer;
+    }
+
+    ImageContext->ImageRead = Private->ShadowedImageRead;
+  } else {
+    ImageContext->ImageRead = PeiImageRead;
+  }
+
+  return EFI_SUCCESS;
+}
+/**
+  To check memory usage bit map arry to figure out if the memory range the image will be loaded in is available or not. If 
+  memory range is avaliable, the function will mark the correponding bits to 1 which indicates the memory range is used.
+  The function is only invoked when load modules at fixed address feature is enabled. 
+  
+  @param  Private                  Pointer to the private data passed in from caller
+  @param  ImageBase                The base addres the image will be loaded at.
+  @param  ImageSize                The size of the image
+  
+  @retval EFI_SUCCESS              The memory range the image will be loaded in is available
+  @retval EFI_NOT_FOUND            The memory range the image will be loaded in is not available
+**/
+EFI_STATUS
+CheckAndMarkFixLoadingMemoryUsageBitMap (
+  IN  PEI_CORE_INSTANCE             *Private,
+  IN  EFI_PHYSICAL_ADDRESS          ImageBase,
+  IN  UINT32                        ImageSize
+  )
+{
+   UINT32                             DxeCodePageNumber;
+   UINT64                             ReservedCodeSize;
+   EFI_PHYSICAL_ADDRESS               PeiCodeBase;
+   UINT32                             BaseOffsetPageNumber;
+   UINT32                             TopOffsetPageNumber;
+   UINT32                             Index;
+   UINT64                             *MemoryUsageBitMap;
+   
+
+   //
+   // The reserved code range includes RuntimeCodePage range, Boot time code range and PEI code range.
+   //
+   DxeCodePageNumber = PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber);
+   DxeCodePageNumber += PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber);
+   ReservedCodeSize  = EFI_PAGES_TO_SIZE(DxeCodePageNumber + PcdGet32(PcdLoadFixAddressPeiCodePageNumber));
+   PeiCodeBase       = Private->LoadModuleAtFixAddressTopAddress - ReservedCodeSize;
+   
+   //
+   // Test the memory range for loading the image in the PEI code range.
+   //
+   if ((Private->LoadModuleAtFixAddressTopAddress - EFI_PAGES_TO_SIZE(DxeCodePageNumber)) < (ImageBase + ImageSize) ||
+       (PeiCodeBase > ImageBase)) {         
+     return EFI_NOT_FOUND; 
+   }
+   
+   //
+   // Test if the memory is avalaible or not.
+   //
+   MemoryUsageBitMap    = Private->PeiCodeMemoryRangeUsageBitMap;  
+   BaseOffsetPageNumber = EFI_SIZE_TO_PAGES((UINT32)(ImageBase - PeiCodeBase));
+   TopOffsetPageNumber  = EFI_SIZE_TO_PAGES((UINT32)(ImageBase + ImageSize - PeiCodeBase));
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     if ((MemoryUsageBitMap[Index / 64] & LShiftU64(1, (Index % 64))) != 0) {
+       //
+       // This page is already used.
+       //
+       return EFI_NOT_FOUND;  
+     }
+   }
+   
+   //
+   // Being here means the memory range is available.  So mark the bits for the memory range
+   // 
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     MemoryUsageBitMap[Index / 64] |= LShiftU64(1, (Index % 64));
+   }
+   return  EFI_SUCCESS;   
+}
+/**
+
+  Get the fixed loadding address from image header assigned by build tool. This function only be called
+  when Loading module at Fixed address feature enabled.
+
+  @param ImageContext              Pointer to the image context structure that describes the PE/COFF
+                                    image that needs to be examined by this function.
+  @param Private                    Pointer to the private data passed in from caller
+
+  @retval EFI_SUCCESS               An fixed loading address is assigned to this image by build tools .
+  @retval EFI_NOT_FOUND             The image has no assigned fixed loadding address.
+
+**/
+EFI_STATUS
+GetPeCoffImageFixLoadingAssignedAddress(
+  IN OUT PE_COFF_LOADER_IMAGE_CONTEXT  *ImageContext,
+  IN     PEI_CORE_INSTANCE             *Private
+  )
+{
+   UINTN                              SectionHeaderOffset;
+   EFI_STATUS                         Status;
+   EFI_IMAGE_SECTION_HEADER           SectionHeader;
+   EFI_IMAGE_OPTIONAL_HEADER_UNION    *ImgHdr;
+   EFI_PHYSICAL_ADDRESS               FixLoaddingAddress;
+   UINT16                             Index;
+   UINTN                              Size;
+   UINT16                             NumberOfSections;
+   UINT64                             ValueInSectionHeader;
+ 
+
+   FixLoaddingAddress = 0;
+   Status = EFI_NOT_FOUND;
+
+   //
+   // Get PeHeader pointer
+   //
+   ImgHdr = (EFI_IMAGE_OPTIONAL_HEADER_UNION *)((CHAR8* )ImageContext->Handle + ImageContext->PeCoffHeaderOffset);
+   if (ImageContext->IsTeImage) {
+     //
+     // for TE image, the fix loadding address is saved in first section header that doesn't point
+     // to code section.
+     //
+     SectionHeaderOffset = sizeof (EFI_TE_IMAGE_HEADER);
+     NumberOfSections = ImgHdr->Te.NumberOfSections;
+   } else {
+     SectionHeaderOffset = (UINTN)(
+                                 ImageContext->PeCoffHeaderOffset +
+                                 sizeof (UINT32) +
+                                 sizeof (EFI_IMAGE_FILE_HEADER) +
+                                 ImgHdr->Pe32.FileHeader.SizeOfOptionalHeader
+                                 );
+      NumberOfSections = ImgHdr->Pe32.FileHeader.NumberOfSections;
+   }
+   //
+   // Get base address from the first section header that doesn't point to code section.
+   //
+   for (Index = 0; Index < NumberOfSections; Index++) {
+     //
+     // Read section header from file
+     //
+     Size = sizeof (EFI_IMAGE_SECTION_HEADER);
+     Status = ImageContext->ImageRead (
+                              ImageContext->Handle,
+                              SectionHeaderOffset,
+                              &Size,
+                              &SectionHeader
+                              );
+     if (EFI_ERROR (Status)) {
+       return Status;
+     }
+
+     Status = EFI_NOT_FOUND;
+
+     if ((SectionHeader.Characteristics & EFI_IMAGE_SCN_CNT_CODE) == 0) {
+       //
+       // Build tool will save the address in PointerToRelocations & PointerToLineNumbers fields in the first section header
+       // that doesn't point to code section in image header, as well as ImageBase field of image header. A notable thing is
+       // that for PEIM, the value in ImageBase field may not be equal to the value in PointerToRelocations & PointerToLineNumbers because
+       // for XIP PEIM, ImageBase field holds the image base address running on the Flash. And PointerToRelocations & PointerToLineNumbers
+       // hold the image base address when it is shadow to the memory. And there is an assumption that when the feature is enabled, if a
+       // module is assigned a loading address by tools, PointerToRelocations & PointerToLineNumbers fields should NOT be Zero, or
+       // else, these 2 fileds should be set to Zero
+       //
+       ValueInSectionHeader = ReadUnaligned64((UINT64*)&SectionHeader.PointerToRelocations);
+       if (ValueInSectionHeader != 0) {
+         //
+         // Found first section header that doesn't point to code section.
+         //
+         if ((INT64)PcdGet64(PcdLoadModuleAtFixAddressEnable) > 0) {
+           //
+           // When LMFA feature is configured as Load Module at Fixed Absolute Address mode, PointerToRelocations & PointerToLineNumbers field
+           // hold the absolute address of image base runing in memory
+           //
+           FixLoaddingAddress = ValueInSectionHeader;
+         } else {
+           //
+           // When LMFA feature is configured as Load Module at Fixed offset mode, PointerToRelocations & PointerToLineNumbers field
+           // hold the offset relative to a platform-specific top address.
+           //
+           FixLoaddingAddress = (EFI_PHYSICAL_ADDRESS)(Private->LoadModuleAtFixAddressTopAddress + (INT64)ValueInSectionHeader);
+         }
+         //
+         // Check if the memory range is avaliable.
+         //
+         Status = CheckAndMarkFixLoadingMemoryUsageBitMap (Private, FixLoaddingAddress, (UINT32) ImageContext->ImageSize);
+         if (!EFI_ERROR(Status)) {
+           //
+           // The assigned address is valid. Return the specified loadding address
+           //
+           ImageContext->ImageAddress = FixLoaddingAddress;
+         }
+       }
+       break;
+     }
+     SectionHeaderOffset += sizeof (EFI_IMAGE_SECTION_HEADER);
+   }
+   DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED INFO: Loading module at fixed address 0x%11p. Status= %r \n", (VOID *)(UINTN)FixLoaddingAddress, Status));
+   return Status;
+}
+/**
+
+  Loads and relocates a PE/COFF image into memory.
+  If the image is not relocatable, it will not be loaded into memory and be loaded as XIP image.
+
+  @param FileHandle      - Pointer to the FFS file header of the image.
+  @param Pe32Data        - The base address of the PE/COFF file that is to be loaded and relocated
+  @param ImageAddress    - The base address of the relocated PE/COFF image
+  @param ImageSize       - The size of the relocated PE/COFF image
+  @param EntryPoint      - The entry point of the relocated PE/COFF image
+
+  @retval EFI_SUCCESS           The file was loaded and relocated
+  @retval EFI_OUT_OF_RESOURCES  There was not enough memory to load and relocate the PE/COFF file
+  @retval EFI_WARN_BUFFER_TOO_SMALL 
+                                There is not enough heap to allocate the requested size.
+                                This will not prevent the XIP image from being invoked.
+
+**/
+EFI_STATUS
+LoadAndRelocatePeCoffImage (
+  IN  EFI_PEI_FILE_HANDLE                       FileHandle,
+  IN  VOID                                      *Pe32Data,
+  OUT EFI_PHYSICAL_ADDRESS                      *ImageAddress,
+  OUT UINT64                                    *ImageSize,
+  OUT EFI_PHYSICAL_ADDRESS                      *EntryPoint
+  )
+{
+  EFI_STATUS                            Status;
+  PE_COFF_LOADER_IMAGE_CONTEXT          ImageContext;
+  PEI_CORE_INSTANCE                     *Private;
+  UINT64                                AlignImageSize;
+  BOOLEAN                               IsXipImage;
+  EFI_STATUS                            ReturnStatus;
+  BOOLEAN                               IsS3Boot;
+  BOOLEAN                               IsRegisterForShadow;
+
+  Private = PEI_CORE_INSTANCE_FROM_PS_THIS (GetPeiServicesTablePointer ());
+
+  ReturnStatus = EFI_SUCCESS;
+  IsXipImage   = FALSE;
+  ZeroMem (&ImageContext, sizeof (ImageContext));
+  ImageContext.Handle = Pe32Data;
+  Status              = GetImageReadFunction (&ImageContext);
+
+  ASSERT_EFI_ERROR (Status);
+
+  Status = PeCoffLoaderGetImageInfo (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Initilize local IsS3Boot and IsRegisterForShadow variable
+  //
+  IsS3Boot = FALSE;
+  if (Private->HobList.HandoffInformationTable->BootMode == BOOT_ON_S3_RESUME) {
+    IsS3Boot = TRUE;
+  }
+  IsRegisterForShadow = FALSE;
+  if ((Private->CurrentFileHandle == FileHandle) 
+    && (Private->Fv[Private->CurrentPeimFvCount].PeimState[Private->CurrentPeimCount] == PEIM_STATE_REGISITER_FOR_SHADOW)) {
+    IsRegisterForShadow = TRUE;
+  }
+
+  //
+  // XIP image that ImageAddress is same to Image handle.
+  //
+  if (ImageContext.ImageAddress == (EFI_PHYSICAL_ADDRESS)(UINTN) Pe32Data) {
+    IsXipImage = TRUE;
+  }
+
+  //
+  // When Image has no reloc section, it can't be relocated into memory.
+  //
+  if (ImageContext.RelocationsStripped && (Private->PeiMemoryInstalled) && (
+      (!IsS3Boot && (PcdGetBool (PcdShadowPeimOnBoot) || IsRegisterForShadow)) || (IsS3Boot && PcdGetBool (PcdShadowPeimOnS3Boot)))) {
+    DEBUG ((EFI_D_INFO|EFI_D_LOAD, "The image at 0x%08x without reloc section can't be loaded into memory\n", (UINTN) Pe32Data));
+  }
+
+  //
+  // Set default base address to current image address.
+  //
+  ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Pe32Data;
+
+  //
+  // Allocate Memory for the image when memory is ready, and image is relocatable.
+  // On normal boot, PcdShadowPeimOnBoot decides whether load PEIM or PeiCore into memory.
+  // On S3 boot, PcdShadowPeimOnS3Boot decides whether load PEIM or PeiCore into memory.
+  //
+  if ((!ImageContext.RelocationsStripped) && (Private->PeiMemoryInstalled) && (
+      (!IsS3Boot && (PcdGetBool (PcdShadowPeimOnBoot) || IsRegisterForShadow)) || (IsS3Boot && PcdGetBool (PcdShadowPeimOnS3Boot)))) {
+    //
+    // Allocate more buffer to avoid buffer overflow.
+    //
+    if (ImageContext.IsTeImage) {
+      AlignImageSize = ImageContext.ImageSize + ((EFI_TE_IMAGE_HEADER *) Pe32Data)->StrippedSize - sizeof (EFI_TE_IMAGE_HEADER);
+    } else {
+      AlignImageSize = ImageContext.ImageSize;
+    }
+
+    if (ImageContext.SectionAlignment > EFI_PAGE_SIZE) {
+      AlignImageSize += ImageContext.SectionAlignment;
+    }
+
+    if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0 && (Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME)) {
+      Status = GetPeCoffImageFixLoadingAssignedAddress(&ImageContext, Private);
+      if (EFI_ERROR (Status)){
+        DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED ERROR: Failed to load module at fixed address. \n"));
+        //
+        // The PEIM is not assiged valid address, try to allocate page to load it.
+        //
+        ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) AllocatePages (EFI_SIZE_TO_PAGES ((UINT32) AlignImageSize));
+      }
+    } else {
+      ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) AllocatePages (EFI_SIZE_TO_PAGES ((UINT32) AlignImageSize));
+    }
+    if (ImageContext.ImageAddress != 0) {
+      //
+      // Adjust the Image Address to make sure it is section alignment.
+      //
+      if (ImageContext.SectionAlignment > EFI_PAGE_SIZE) {
+        ImageContext.ImageAddress =
+            (ImageContext.ImageAddress + ImageContext.SectionAlignment - 1) &
+            ~((UINTN)ImageContext.SectionAlignment - 1);
+      }
+      //
+      // Fix alignment requirement when Load IPF TeImage into memory.
+      // Skip the reserved space for the stripped PeHeader when load TeImage into memory.
+      //
+      if (ImageContext.IsTeImage) {
+        ImageContext.ImageAddress = ImageContext.ImageAddress +
+                                    ((EFI_TE_IMAGE_HEADER *) Pe32Data)->StrippedSize -
+                                    sizeof (EFI_TE_IMAGE_HEADER);
+      }
+    } else {
+      //
+      // No enough memory resource.
+      //
+      if (IsXipImage) {
+        //
+        // XIP image can still be invoked.
+        //
+        ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Pe32Data;
+        ReturnStatus = EFI_WARN_BUFFER_TOO_SMALL;
+      } else {
+        //
+        // Non XIP image can't be loaded because no enough memory is allocated.
+        //
+        ASSERT (FALSE);
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+  }
+
+  //
+  // Load the image to our new buffer
+  //
+  Status = PeCoffLoaderLoadImage (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Relocate the image in our new buffer
+  //
+  Status = PeCoffLoaderRelocateImage (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Flush the instruction cache so the image data is written before we execute it
+  //
+  if (ImageContext.ImageAddress != (EFI_PHYSICAL_ADDRESS)(UINTN) Pe32Data) {
+    InvalidateInstructionCacheRange ((VOID *)(UINTN)ImageContext.ImageAddress, (UINTN)ImageContext.ImageSize);
+  }
+
+  *ImageAddress = ImageContext.ImageAddress;
+  *ImageSize    = ImageContext.ImageSize;
+  *EntryPoint   = ImageContext.EntryPoint;
+
+  return ReturnStatus;
+}
+
+/**
+  Loads a PEIM into memory for subsequent execution. If there are compressed
+  images or images that need to be relocated into memory for performance reasons,
+  this service performs that transformation.
+
+  @param PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param FileHandle       Pointer to the FFS file header of the image.
+  @param ImageAddressArg  Pointer to PE/TE image.
+  @param ImageSizeArg     Size of PE/TE image.
+  @param EntryPoint       Pointer to entry point of specified image file for output.
+  @param AuthenticationState - Pointer to attestation authentication state of image.
+
+  @retval EFI_SUCCESS      Image is successfully loaded.
+  @retval EFI_NOT_FOUND    Fail to locate necessary PPI.
+  @retval EFI_UNSUPPORTED  Image Machine Type is not supported.
+  @retval EFI_WARN_BUFFER_TOO_SMALL 
+                           There is not enough heap to allocate the requested size.
+                           This will not prevent the XIP image from being invoked.
+
+**/
+EFI_STATUS
+PeiLoadImageLoadImage (
+  IN     CONST EFI_PEI_SERVICES       **PeiServices,
+  IN     EFI_PEI_FILE_HANDLE          FileHandle,
+  OUT    EFI_PHYSICAL_ADDRESS         *ImageAddressArg,  OPTIONAL
+  OUT    UINT64                       *ImageSizeArg,     OPTIONAL
+  OUT    EFI_PHYSICAL_ADDRESS         *EntryPoint,
+  OUT    UINT32                       *AuthenticationState
+  )
+{
+  EFI_STATUS                  Status;
+  VOID                        *Pe32Data;
+  EFI_PHYSICAL_ADDRESS        ImageAddress;
+  UINT64                      ImageSize;
+  EFI_PHYSICAL_ADDRESS        ImageEntryPoint;
+  UINT16                      Machine;
+  EFI_SECTION_TYPE            SearchType1;
+  EFI_SECTION_TYPE            SearchType2;
+
+  *EntryPoint          = 0;
+  ImageSize            = 0;
+  *AuthenticationState = 0;
+
+  if (FeaturePcdGet (PcdPeiCoreImageLoaderSearchTeSectionFirst)) {
+    SearchType1 = EFI_SECTION_TE;
+    SearchType2 = EFI_SECTION_PE32;
+  } else {
+    SearchType1 = EFI_SECTION_PE32;
+    SearchType2 = EFI_SECTION_TE;
+  }
+
+  //
+  // Try to find a first exe section (if PcdPeiCoreImageLoaderSearchTeSectionFirst
+  // is true, TE will be searched first).
+  //
+  Status = PeiServicesFfsFindSectionData3 (
+             SearchType1,
+             0,
+             FileHandle,
+             &Pe32Data,
+             AuthenticationState
+             );
+  //
+  // If we didn't find a first exe section, try to find the second exe section.
+  //
+  if (EFI_ERROR (Status)) {
+    Status = PeiServicesFfsFindSectionData3 (
+               SearchType2,
+               0,
+               FileHandle,
+               &Pe32Data,
+               AuthenticationState
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // PEI core only carry the loader function for TE and PE32 executables
+      // If this two section does not exist, just return.
+      //
+      return Status;
+    }
+  }
+
+  //
+  // If memory is installed, perform the shadow operations
+  //
+  Status = LoadAndRelocatePeCoffImage (
+    FileHandle,
+    Pe32Data,
+    &ImageAddress,
+    &ImageSize,
+    &ImageEntryPoint
+  );
+
+  ASSERT_EFI_ERROR (Status);
+
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Got the entry point from the loaded Pe32Data
+  //
+  Pe32Data    = (VOID *) ((UINTN) ImageAddress);
+  *EntryPoint = ImageEntryPoint;
+
+  Machine = PeCoffLoaderGetMachineType (Pe32Data);
+
+  if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (Machine)) {
+    if (!EFI_IMAGE_MACHINE_CROSS_TYPE_SUPPORTED (Machine)) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  if (ImageAddressArg != NULL) {
+    *ImageAddressArg = ImageAddress;
+  }
+
+  if (ImageSizeArg != NULL) {
+    *ImageSizeArg = ImageSize;
+  }
+
+  DEBUG_CODE_BEGIN ();
+    CHAR8                              *AsciiString;
+    CHAR8                              EfiFileName[512];
+    INT32                              Index;
+    INT32                              StartIndex;
+
+    //
+    // Print debug message: Loading PEIM at 0x12345678 EntryPoint=0x12345688 Driver.efi
+    //
+    if (Machine != EFI_IMAGE_MACHINE_IA64) {
+      DEBUG ((EFI_D_INFO | EFI_D_LOAD, "Loading PEIM at 0x%11p EntryPoint=0x%11p ", (VOID *)(UINTN)ImageAddress, (VOID *)(UINTN)*EntryPoint));
+    } else {
+      //
+      // For IPF Image, the real entry point should be print.
+      //
+      DEBUG ((EFI_D_INFO | EFI_D_LOAD, "Loading PEIM at 0x%11p EntryPoint=0x%11p ", (VOID *)(UINTN)ImageAddress, (VOID *)(UINTN)(*(UINT64 *)(UINTN)*EntryPoint)));
+    }
+
+    //
+    // Print Module Name by PeImage PDB file name.
+    //
+    AsciiString = PeCoffLoaderGetPdbPointer (Pe32Data);
+
+    if (AsciiString != NULL) {
+      StartIndex = 0;
+      for (Index = 0; AsciiString[Index] != 0; Index++) {
+        if (AsciiString[Index] == '\\' || AsciiString[Index] == '/') {
+          StartIndex = Index + 1;
+        }
+      }
+
+      //
+      // Copy the PDB file name to our temporary string, and replace .pdb with .efi
+      // The PDB file name is limited in the range of 0~511.
+      // If the length is bigger than 511, trim the redudant characters to avoid overflow in array boundary.
+      //
+      for (Index = 0; Index < sizeof (EfiFileName) - 4; Index++) {
+        EfiFileName[Index] = AsciiString[Index + StartIndex];
+        if (EfiFileName[Index] == 0) {
+          EfiFileName[Index] = '.';
+        }
+        if (EfiFileName[Index] == '.') {
+          EfiFileName[Index + 1] = 'e';
+          EfiFileName[Index + 2] = 'f';
+          EfiFileName[Index + 3] = 'i';
+          EfiFileName[Index + 4] = 0;
+          break;
+        }
+      }
+
+      if (Index == sizeof (EfiFileName) - 4) {
+        EfiFileName[Index] = 0;
+      }
+
+      DEBUG ((EFI_D_INFO | EFI_D_LOAD, "%a", EfiFileName));
+    }
+
+  DEBUG_CODE_END ();
+
+  DEBUG ((EFI_D_INFO | EFI_D_LOAD, "\n"));
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  The wrapper function of PeiLoadImageLoadImage().
+
+  @param This            - Pointer to EFI_PEI_LOAD_FILE_PPI.
+  @param FileHandle      - Pointer to the FFS file header of the image.
+  @param ImageAddressArg - Pointer to PE/TE image.
+  @param ImageSizeArg    - Size of PE/TE image.
+  @param EntryPoint      - Pointer to entry point of specified image file for output.
+  @param AuthenticationState - Pointer to attestation authentication state of image.
+
+  @return Status of PeiLoadImageLoadImage().
+
+**/
+EFI_STATUS
+EFIAPI
+PeiLoadImageLoadImageWrapper (
+  IN     CONST EFI_PEI_LOAD_FILE_PPI  *This,
+  IN     EFI_PEI_FILE_HANDLE          FileHandle,
+  OUT    EFI_PHYSICAL_ADDRESS         *ImageAddressArg,  OPTIONAL
+  OUT    UINT64                       *ImageSizeArg,     OPTIONAL
+  OUT    EFI_PHYSICAL_ADDRESS         *EntryPoint,
+  OUT    UINT32                       *AuthenticationState
+  )
+{
+  return PeiLoadImageLoadImage (
+           GetPeiServicesTablePointer (),
+           FileHandle,
+           ImageAddressArg,
+           ImageSizeArg,
+           EntryPoint,
+           AuthenticationState
+           );
+}
+
+/**
+  Check whether the input image has the relocation.
+
+  @param  Pe32Data   Pointer to the PE/COFF or TE image.
+
+  @retval TRUE       Relocation is stripped.
+  @retval FALSE      Relocation is not stripped.
+
+**/
+BOOLEAN
+RelocationIsStrip (
+  IN VOID  *Pe32Data
+  )
+{
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
+  EFI_IMAGE_DOS_HEADER                 *DosHdr;
+
+  ASSERT (Pe32Data != NULL);
+
+  DosHdr = (EFI_IMAGE_DOS_HEADER *)Pe32Data;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    //
+    // DOS image header is present, so read the PE header after the DOS image header.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)((UINTN) Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+  } else {
+    //
+    // DOS image header is not present, so PE header is at the image base.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *)Pe32Data;
+  }
+
+  //
+  // Three cases with regards to relocations:
+  // - Image has base relocs, RELOCS_STRIPPED==0    => image is relocatable
+  // - Image has no base relocs, RELOCS_STRIPPED==1 => Image is not relocatable
+  // - Image has no base relocs, RELOCS_STRIPPED==0 => Image is relocatable but
+  //   has no base relocs to apply
+  // Obviously having base relocations with RELOCS_STRIPPED==1 is invalid.
+  //
+  // Look at the file header to determine if relocations have been stripped, and
+  // save this info in the image context for later use.
+  //
+  if (Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) {
+    if ((Hdr.Te->DataDirectory[0].Size == 0) && (Hdr.Te->DataDirectory[0].VirtualAddress == 0)) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  } else if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE)  {
+    if ((Hdr.Pe32->FileHeader.Characteristics & EFI_IMAGE_FILE_RELOCS_STRIPPED) != 0) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Routine to load image file for subsequent execution by LoadFile Ppi.
+  If any LoadFile Ppi is not found, the build-in support function for the PE32+/TE
+  XIP image format is used.
+
+  @param PeiServices     - An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param FileHandle      - Pointer to the FFS file header of the image.
+  @param PeimState       - The dispatch state of the input PEIM handle.
+  @param EntryPoint      - Pointer to entry point of specified image file for output.
+  @param AuthenticationState - Pointer to attestation authentication state of image.
+
+  @retval EFI_SUCCESS    - Image is successfully loaded.
+  @retval EFI_NOT_FOUND  - Fail to locate necessary PPI
+  @retval Others         - Fail to load file.
+
+**/
+EFI_STATUS
+PeiLoadImage (
+  IN     CONST EFI_PEI_SERVICES       **PeiServices,
+  IN     EFI_PEI_FILE_HANDLE          FileHandle,
+  IN     UINT8                        PeimState,
+  OUT    EFI_PHYSICAL_ADDRESS         *EntryPoint,
+  OUT    UINT32                       *AuthenticationState
+  )
+{
+  EFI_STATUS              PpiStatus;
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  EFI_PEI_LOAD_FILE_PPI   *LoadFile;
+  EFI_PHYSICAL_ADDRESS    ImageAddress;
+  UINT64                  ImageSize;
+  BOOLEAN                 IsStrip;
+
+  IsStrip = FALSE;
+  //
+  // If any instances of PEI_LOAD_FILE_PPI are installed, they are called.
+  // one at a time, until one reports EFI_SUCCESS.
+  //
+  Index = 0;
+  do {
+    PpiStatus = PeiServicesLocatePpi (
+                  &gEfiPeiLoadFilePpiGuid,
+                  Index,
+                  NULL,
+                  (VOID **)&LoadFile
+                  );
+    if (!EFI_ERROR (PpiStatus)) {
+      Status = LoadFile->LoadFile (
+                          LoadFile,
+                          FileHandle,
+                          &ImageAddress,
+                          &ImageSize,
+                          EntryPoint,
+                          AuthenticationState
+                          );
+      if (!EFI_ERROR (Status) || Status == EFI_WARN_BUFFER_TOO_SMALL) {
+        //
+        // The shadowed PEIM must be relocatable.
+        //
+        if (PeimState == PEIM_STATE_REGISITER_FOR_SHADOW) {
+          IsStrip = RelocationIsStrip ((VOID *) (UINTN) ImageAddress);
+          ASSERT (!IsStrip);
+          if (IsStrip) {
+            return EFI_UNSUPPORTED;
+          }
+        }
+
+        //
+        // The image to be started must have the machine type supported by PeiCore.
+        //
+        ASSERT (EFI_IMAGE_MACHINE_TYPE_SUPPORTED (PeCoffLoaderGetMachineType ((VOID *) (UINTN) ImageAddress)));
+        if (!EFI_IMAGE_MACHINE_TYPE_SUPPORTED (PeCoffLoaderGetMachineType ((VOID *) (UINTN) ImageAddress))) {
+          return EFI_UNSUPPORTED;
+        }
+        return EFI_SUCCESS;
+      }
+    }
+    Index++;
+  } while (!EFI_ERROR (PpiStatus));
+
+  return PpiStatus;
+}
+
+
+/**
+
+  Install Pei Load File PPI.
+
+
+  @param PrivateData     - Pointer to PEI_CORE_INSTANCE.
+  @param OldCoreData     - Pointer to PEI_CORE_INSTANCE.
+
+**/
+VOID
+InitializeImageServices (
+  IN  PEI_CORE_INSTANCE   *PrivateData,
+  IN  PEI_CORE_INSTANCE   *OldCoreData
+  )
+{
+  if (OldCoreData == NULL) {
+    //
+    // The first time we are XIP (running from FLASH). We need to remember the
+    // FLASH address so we can reinstall the memory version that runs faster
+    //
+    PrivateData->XipLoadFile = &gPpiLoadFilePpiList;
+    PeiServicesInstallPpi (PrivateData->XipLoadFile);
+  } else {
+    //
+    // 2nd time we are running from memory so replace the XIP version with the
+    // new memory version.
+    //
+    PeiServicesReInstallPpi (PrivateData->XipLoadFile, &gPpiLoadFilePpiList);
+  }
+}
+
+
+
+
diff --git a/Core/MdeModulePkg/Core/Pei/Memory/MemoryServices.c b/Core/MdeModulePkg/Core/Pei/Memory/MemoryServices.c
new file mode 100644
index 0000000000..2f9b9dea31
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Memory/MemoryServices.c
@@ -0,0 +1,272 @@
+/** @file
+  EFI PEI Core memory services
+  
+Copyright (c) 2006 - 2015, 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 "PeiMain.h"
+
+/**
+
+  Initialize the memory services.
+
+  @param PrivateData     Points to PeiCore's private instance data.
+  @param SecCoreData     Points to a data structure containing information about the PEI core's operating
+                         environment, such as the size and location of temporary RAM, the stack location and
+                         the BFV location.
+  @param OldCoreData     Pointer to the PEI Core data.
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializeMemoryServices (
+  IN PEI_CORE_INSTANCE           *PrivateData,
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *OldCoreData
+  )
+{
+  
+  PrivateData->SwitchStackSignal    = FALSE;
+
+  //
+  // First entering PeiCore, following code will initialized some field
+  // in PeiCore's private data according to hand off data from sec core.
+  //
+  if (OldCoreData == NULL) {
+
+    PrivateData->PeiMemoryInstalled = FALSE;
+    PrivateData->HobList.Raw        = SecCoreData->PeiTemporaryRamBase;
+    
+    PeiCoreBuildHobHandoffInfoTable (
+      BOOT_WITH_FULL_CONFIGURATION,
+      (EFI_PHYSICAL_ADDRESS) (UINTN) SecCoreData->PeiTemporaryRamBase,
+      (UINTN) SecCoreData->PeiTemporaryRamSize
+      );
+
+    //
+    // Set Ps to point to ServiceTableShadow in Cache
+    //
+    PrivateData->Ps = &(PrivateData->ServiceTableShadow);
+  }
+  
+  return;
+}
+
+/**
+
+  This function registers the found memory configuration with the PEI Foundation.
+
+  The usage model is that the PEIM that discovers the permanent memory shall invoke this service.
+  This routine will hold discoveried memory information into PeiCore's private data,
+  and set SwitchStackSignal flag. After PEIM who discovery memory is dispatched,
+  PeiDispatcher will migrate temporary memory to permenement memory.
+  
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param MemoryBegin        Start of memory address.
+  @param MemoryLength       Length of memory.
+
+  @return EFI_SUCCESS Always success.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiInstallPeiMemory (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN EFI_PHYSICAL_ADDRESS    MemoryBegin,
+  IN UINT64                  MemoryLength
+  )
+{
+  PEI_CORE_INSTANCE                     *PrivateData;
+
+  DEBUG ((EFI_D_INFO, "PeiInstallPeiMemory MemoryBegin 0x%LX, MemoryLength 0x%LX\n", MemoryBegin, MemoryLength));
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+
+  //
+  // PEI_SERVICE.InstallPeiMemory should only be called one time during whole PEI phase.
+  // If it is invoked more than one time, ASSERT information is given for developer debugging in debug tip and
+  // simply return EFI_SUCESS in release tip to ignore it.
+  // 
+  if (PrivateData->PeiMemoryInstalled) {
+    DEBUG ((EFI_D_ERROR, "ERROR: PeiInstallPeiMemory is called more than once!\n"));
+    ASSERT (FALSE);
+    return EFI_SUCCESS;
+  }
+  
+  PrivateData->PhysicalMemoryBegin   = MemoryBegin;
+  PrivateData->PhysicalMemoryLength  = MemoryLength;
+  PrivateData->FreePhysicalMemoryTop = MemoryBegin + MemoryLength;
+   
+  PrivateData->SwitchStackSignal      = TRUE;
+
+  return EFI_SUCCESS;   
+}
+
+/**
+  The purpose of the service is to publish an interface that allows 
+  PEIMs to allocate memory ranges that are managed by the PEI Foundation.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  MemoryType       The type of memory to allocate.
+  @param  Pages            The number of contiguous 4 KB pages to allocate.
+  @param  Memory           Pointer to a physical address. On output, the address is set to the base 
+                           of the page range that was allocated.
+
+  @retval EFI_SUCCESS           The memory range was successfully allocated.
+  @retval EFI_OUT_OF_RESOURCES  The pages could not be allocated.
+  @retval EFI_INVALID_PARAMETER Type is not equal to EfiLoaderCode, EfiLoaderData, EfiRuntimeServicesCode, 
+                                EfiRuntimeServicesData, EfiBootServicesCode, EfiBootServicesData,
+                                EfiACPIReclaimMemory, EfiReservedMemoryType, or EfiACPIMemoryNVS.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiAllocatePages (
+  IN CONST EFI_PEI_SERVICES     **PeiServices,
+  IN       EFI_MEMORY_TYPE      MemoryType,
+  IN       UINTN                Pages,
+  OUT      EFI_PHYSICAL_ADDRESS *Memory
+  )
+{
+  PEI_CORE_INSTANCE                       *PrivateData;
+  EFI_PEI_HOB_POINTERS                    Hob;
+  EFI_PHYSICAL_ADDRESS                    *FreeMemoryTop;
+  EFI_PHYSICAL_ADDRESS                    *FreeMemoryBottom;
+  UINTN                                   RemainingPages;
+
+  if ((MemoryType != EfiLoaderCode) &&
+      (MemoryType != EfiLoaderData) &&
+      (MemoryType != EfiRuntimeServicesCode) &&
+      (MemoryType != EfiRuntimeServicesData) &&
+      (MemoryType != EfiBootServicesCode) &&
+      (MemoryType != EfiBootServicesData) &&
+      (MemoryType != EfiACPIReclaimMemory) &&
+      (MemoryType != EfiReservedMemoryType) &&
+      (MemoryType != EfiACPIMemoryNVS)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+  Hob.Raw     = PrivateData->HobList.Raw;
+  
+  //
+  // Check if Hob already available
+  //
+  if (!PrivateData->PeiMemoryInstalled) {
+    //
+    // When PeiInstallMemory is called but temporary memory has *not* been moved to temporary memory,
+    // the AllocatePage will depend on the field of PEI_CORE_INSTANCE structure.
+    //
+    if (!PrivateData->SwitchStackSignal) {
+      return EFI_NOT_AVAILABLE_YET;
+    } else {
+      FreeMemoryTop     = &(PrivateData->FreePhysicalMemoryTop);
+      FreeMemoryBottom  = &(PrivateData->PhysicalMemoryBegin);
+    }
+  } else {
+    FreeMemoryTop     = &(Hob.HandoffInformationTable->EfiFreeMemoryTop);
+    FreeMemoryBottom  = &(Hob.HandoffInformationTable->EfiFreeMemoryBottom);
+  }
+
+  //
+  // Check to see if on 4k boundary, If not aligned, make the allocation aligned.
+  //
+  *(FreeMemoryTop) -= *(FreeMemoryTop) & 0xFFF;
+
+  //
+  // Verify that there is sufficient memory to satisfy the allocation.
+  // For page allocation, the overhead sizeof (EFI_HOB_MEMORY_ALLOCATION) needs to be considered.
+  //
+  if ((UINTN) (*FreeMemoryTop - *FreeMemoryBottom) < (UINTN) ALIGN_VALUE (sizeof (EFI_HOB_MEMORY_ALLOCATION), 8)) {
+    DEBUG ((EFI_D_ERROR, "AllocatePages failed: No space to build memory allocation hob.\n"));
+    return  EFI_OUT_OF_RESOURCES;
+  }
+  RemainingPages = (UINTN)(*FreeMemoryTop - *FreeMemoryBottom - ALIGN_VALUE (sizeof (EFI_HOB_MEMORY_ALLOCATION), 8)) >> EFI_PAGE_SHIFT;
+  //
+  // The number of remaining pages needs to be greater than or equal to that of the request pages.
+  //
+  if (RemainingPages < Pages) {
+    DEBUG ((EFI_D_ERROR, "AllocatePages failed: No 0x%lx Pages is available.\n", (UINT64) Pages));
+    DEBUG ((EFI_D_ERROR, "There is only left 0x%lx pages memory resource to be allocated.\n", (UINT64) RemainingPages));
+    return  EFI_OUT_OF_RESOURCES;
+  } else {
+    //
+    // Update the PHIT to reflect the memory usage
+    //
+    *(FreeMemoryTop) -= Pages * EFI_PAGE_SIZE;
+
+    //
+    // Update the value for the caller
+    //
+    *Memory = *(FreeMemoryTop);
+
+    //
+    // Create a memory allocation HOB.
+    //
+    BuildMemoryAllocationHob (
+      *(FreeMemoryTop),
+      Pages * EFI_PAGE_SIZE,
+      MemoryType
+      );
+
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+
+  Pool allocation service. Before permenent memory is discoveried, the pool will 
+  be allocated the heap in the temporary memory. Genenrally, the size of heap in temporary 
+  memory does not exceed to 64K, so the biggest pool size could be allocated is 
+  64K.
+
+  @param PeiServices               An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Size                      Amount of memory required
+  @param Buffer                    Address of pointer to the buffer
+
+  @retval EFI_SUCCESS              The allocation was successful
+  @retval EFI_OUT_OF_RESOURCES     There is not enough heap to satisfy the requirement
+                                   to allocate the requested size.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiAllocatePool (
+  IN CONST EFI_PEI_SERVICES     **PeiServices,
+  IN       UINTN                Size,
+  OUT      VOID                 **Buffer
+  )
+{
+  EFI_STATUS               Status;
+  EFI_HOB_MEMORY_POOL      *Hob;
+
+  //
+  // If some "post-memory" PEIM wishes to allocate larger pool,
+  // it should use AllocatePages service instead.
+  //
+  
+  //
+  // Generally, the size of heap in temporary memory does not exceed to 64K,
+  // HobLength is multiples of 8 bytes, so the maxmium size of pool is 0xFFF8 - sizeof (EFI_HOB_MEMORY_POOL)
+  //
+  if (Size > (0xFFF8 - sizeof (EFI_HOB_MEMORY_POOL))) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  Status = PeiServicesCreateHob (
+             EFI_HOB_TYPE_MEMORY_POOL,
+             (UINT16)(sizeof (EFI_HOB_MEMORY_POOL) + Size),
+             (VOID **)&Hob
+             );
+  ASSERT_EFI_ERROR (Status);
+  *Buffer = Hob+1;  
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/Pei/PciCfg2/PciCfg2.c b/Core/MdeModulePkg/Core/Pei/PciCfg2/PciCfg2.c
new file mode 100644
index 0000000000..a20e7c00c5
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PciCfg2/PciCfg2.c
@@ -0,0 +1,128 @@
+/** @file
+  The default version of EFI_PEI_PCI_CFG2_PPI support published by PeiServices in
+  PeiCore initialization phase.
+ 
+  EFI_PEI_PCI_CFG2_PPI is installed by the PEIM which supports a PCI root bridge. 
+  When PeiCore is started, the default version of EFI_PEI_PCI_CFG2_PPI will be assigned 
+  to PeiServices table.
+  
+Copyright (c) 2009, 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 "PeiMain.h"
+
+///
+/// This default instance of EFI_PEI_PCI_CFG2_PPI install assigned to EFI_PEI_SERVICE.PciCfg 
+/// when PeiCore's initialization.
+///
+EFI_PEI_PCI_CFG2_PPI gPeiDefaultPciCfg2Ppi = {
+  PeiDefaultPciCfg2Read,
+  PeiDefaultPciCfg2Write,
+  PeiDefaultPciCfg2Modify
+};
+
+/**
+  Reads from a given location in the PCI configuration space.
+
+  If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Read (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  Write to a given location in the PCI configuration space.
+
+  If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Write (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}  
+
+/**
+  This function performs a read-modify-write operation on the contents from a given
+  location in the PCI configuration space.
+  If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+
+  @param  PeiServices     An indirect pointer to the PEI Services Table
+                          published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes. Type
+                          EFI_PEI_PCI_CFG_PPI_WIDTH is defined in Read().
+  @param  Address         The physical address of the access.
+  @param  SetBits         Points to value to bitwise-OR with the read configuration value.
+                          The size of the value is determined by Width.
+  @param  ClearBits       Points to the value to negate and bitwise-AND with the read configuration value.
+                          The size of the value is determined by Width.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Modify (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN        VOID                      *SetBits,
+  IN        VOID                      *ClearBits
+  )
+{
+  return EFI_NOT_AVAILABLE_YET;
+}   
diff --git a/Core/MdeModulePkg/Core/Pei/PeiCore.uni b/Core/MdeModulePkg/Core/Pei/PeiCore.uni
new file mode 100644
index 0000000000..abd228e709
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PeiCore.uni
diff --git a/Core/MdeModulePkg/Core/Pei/PeiCoreExtra.uni b/Core/MdeModulePkg/Core/Pei/PeiCoreExtra.uni
new file mode 100644
index 0000000000..f01d0e307a
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PeiCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/Pei/PeiMain.h b/Core/MdeModulePkg/Core/Pei/PeiMain.h
new file mode 100644
index 0000000000..24983201d0
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PeiMain.h
@@ -0,0 +1,1745 @@
+/** @file
+  Definition of Pei Core Structures and Services
+  
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_MAIN_H_
+#define _PEI_MAIN_H_
+
+#include <PiPei.h>
+#include <Ppi/DxeIpl.h>
+#include <Ppi/MemoryDiscovered.h>
+#include <Ppi/StatusCode.h>
+#include <Ppi/Reset.h>
+#include <Ppi/Reset2.h>
+#include <Ppi/FirmwareVolume.h>
+#include <Ppi/FirmwareVolumeInfo.h>
+#include <Ppi/FirmwareVolumeInfo2.h>
+#include <Ppi/Decompress.h>
+#include <Ppi/GuidedSectionExtraction.h>
+#include <Ppi/LoadFile.h>
+#include <Ppi/Security2.h>
+#include <Ppi/TemporaryRamSupport.h>
+#include <Ppi/TemporaryRamDone.h>
+#include <Library/DebugLib.h>
+#include <Library/PeiCoreEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/HobLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/PcdLib.h>
+#include <IndustryStandard/PeImage.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Guid/FirmwareFileSystem2.h>
+#include <Guid/FirmwareFileSystem3.h>
+#include <Guid/AprioriFileName.h>
+
+///
+/// It is an FFS type extension used for PeiFindFileEx. It indicates current
+/// Ffs searching is for all PEIMs can be dispatched by PeiCore.
+///
+#define PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE   0xff
+
+///
+/// Pei Core private data structures
+///
+typedef union {
+  EFI_PEI_PPI_DESCRIPTOR      *Ppi;
+  EFI_PEI_NOTIFY_DESCRIPTOR   *Notify;
+  VOID                        *Raw;
+} PEI_PPI_LIST_POINTERS;
+
+///
+/// PPI database structure which contains two link: PpiList and NotifyList. PpiList
+/// is in head of PpiListPtrs array and notify is in end of PpiListPtrs.
+///
+typedef struct {
+  ///
+  /// index of end of PpiList link list.
+  ///
+  INTN                    PpiListEnd;
+  ///
+  /// index of end of notify link list.
+  ///
+  INTN                    NotifyListEnd;
+  ///
+  /// index of the dispatched notify list.
+  ///
+  INTN                    DispatchListEnd;
+  ///
+  /// index of last installed Ppi description in PpiList link list.
+  ///
+  INTN                    LastDispatchedInstall;
+  ///
+  /// index of last dispatched notify in Notify link list.
+  /// 
+  INTN                    LastDispatchedNotify;
+  ///
+  /// Ppi database has the PcdPeiCoreMaxPpiSupported number of entries.
+  ///
+  PEI_PPI_LIST_POINTERS   *PpiListPtrs;
+} PEI_PPI_DATABASE;
+
+
+//
+// PEI_CORE_FV_HANDE.PeimState
+// Do not change these values as there is code doing math to change states.
+// Look for Private->Fv[FvCount].PeimState[PeimCount]++;
+//
+#define PEIM_STATE_NOT_DISPATCHED         0x00
+#define PEIM_STATE_DISPATCHED             0x01
+#define PEIM_STATE_REGISITER_FOR_SHADOW   0x02
+#define PEIM_STATE_DONE                   0x03
+
+typedef struct {
+  EFI_FIRMWARE_VOLUME_HEADER          *FvHeader;
+  EFI_PEI_FIRMWARE_VOLUME_PPI         *FvPpi;
+  EFI_PEI_FV_HANDLE                   FvHandle;
+  //
+  // Ponter to the buffer with the PcdPeiCoreMaxPeimPerFv number of Entries.
+  //
+  UINT8                               *PeimState;
+  //
+  // Ponter to the buffer with the PcdPeiCoreMaxPeimPerFv number of Entries.
+  //
+  EFI_PEI_FILE_HANDLE                 *FvFileHandles;
+  BOOLEAN                             ScanFv;
+  UINT32                              AuthenticationStatus;
+} PEI_CORE_FV_HANDLE;
+
+typedef struct {
+  EFI_GUID                            FvFormat;
+  VOID                                *FvInfo;
+  UINT32                              FvInfoSize;
+  UINT32                              AuthenticationStatus;
+  EFI_PEI_NOTIFY_DESCRIPTOR           NotifyDescriptor;
+} PEI_CORE_UNKNOW_FORMAT_FV_INFO;
+
+#define CACHE_SETION_MAX_NUMBER       0x10
+typedef struct {
+  EFI_COMMON_SECTION_HEADER*          Section[CACHE_SETION_MAX_NUMBER];
+  VOID*                               SectionData[CACHE_SETION_MAX_NUMBER];
+  UINTN                               SectionSize[CACHE_SETION_MAX_NUMBER];
+  UINT32                              AuthenticationStatus[CACHE_SETION_MAX_NUMBER];
+  UINTN                               AllSectionCount;
+  UINTN                               SectionIndex;
+} CACHE_SECTION_DATA;
+
+#define HOLE_MAX_NUMBER       0x3
+typedef struct {
+  EFI_PHYSICAL_ADDRESS               Base;
+  UINTN                              Size;
+  UINTN                              Offset;
+  BOOLEAN                            OffsetPositive;
+} HOLE_MEMORY_DATA;
+
+///
+/// Forward declaration for PEI_CORE_INSTANCE
+///
+typedef struct _PEI_CORE_INSTANCE  PEI_CORE_INSTANCE;
+
+
+/**
+  Function Pointer type for PeiCore function.
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
+                         and location of temporary RAM, the stack location and the BFV location.
+  @param PpiList         Points to a list of one or more PPI descriptors to be installed initially by the PEI core.
+                         An empty PPI list consists of a single descriptor with the end-tag
+                         EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST. As part of its initialization
+                         phase, the PEI Foundation will add these SEC-hosted PPIs to its PPI database such
+                         that both the PEI Foundation and any modules can leverage the associated service
+                         calls and/or code in these early PPIs
+  @param OldCoreData     Pointer to old core data that is used to initialize the
+                         core's data areas.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEICORE_FUNCTION_POINTER)(
+  IN CONST  EFI_SEC_PEI_HAND_OFF    *SecCoreData,
+  IN CONST  EFI_PEI_PPI_DESCRIPTOR  *PpiList,
+  IN PEI_CORE_INSTANCE              *OldCoreData
+  );
+
+#define PEI_CORE_HANDLE_SIGNATURE  SIGNATURE_32('P','e','i','C')
+
+///
+/// Pei Core private data structure instance
+///
+struct _PEI_CORE_INSTANCE {
+  UINTN                              Signature;
+  
+  ///
+  /// Point to ServiceTableShadow
+  ///
+  EFI_PEI_SERVICES                   *Ps;
+  PEI_PPI_DATABASE                   PpiData;
+  
+  ///
+  /// The count of FVs which contains FFS and could be dispatched by PeiCore.
+  ///
+  UINTN                              FvCount;
+  
+  ///
+  /// Pointer to the buffer with the PcdPeiCoreMaxFvSupported number of entries.
+  /// Each entry is for one FV which contains FFS and could be dispatched by PeiCore.
+  ///
+  PEI_CORE_FV_HANDLE                 *Fv;
+
+  ///
+  /// Pointer to the buffer with the PcdPeiCoreMaxFvSupported number of entries.
+  /// Each entry is for one FV which could not be dispatched by PeiCore.
+  ///
+  PEI_CORE_UNKNOW_FORMAT_FV_INFO     *UnknownFvInfo;
+  UINTN                              UnknownFvInfoCount;
+  
+  ///
+  /// Pointer to the buffer with the PcdPeiCoreMaxPeimPerFv number of entries.
+  ///
+  EFI_PEI_FILE_HANDLE                *CurrentFvFileHandles;
+  UINTN                              AprioriCount;
+  UINTN                              CurrentPeimFvCount;
+  UINTN                              CurrentPeimCount;
+  EFI_PEI_FILE_HANDLE                CurrentFileHandle;
+  BOOLEAN                            PeimNeedingDispatch;
+  BOOLEAN                            PeimDispatchOnThisPass;
+  BOOLEAN                            PeimDispatcherReenter;
+  EFI_PEI_HOB_POINTERS               HobList;
+  BOOLEAN                            SwitchStackSignal;
+  BOOLEAN                            PeiMemoryInstalled;
+  VOID                               *CpuIo;
+  EFI_PEI_SECURITY2_PPI              *PrivateSecurityPpi;
+  EFI_PEI_SERVICES                   ServiceTableShadow;
+  EFI_PEI_PPI_DESCRIPTOR             *XipLoadFile;
+  EFI_PHYSICAL_ADDRESS               PhysicalMemoryBegin;
+  UINT64                             PhysicalMemoryLength;
+  EFI_PHYSICAL_ADDRESS               FreePhysicalMemoryTop;
+  UINTN                              HeapOffset;
+  BOOLEAN                            HeapOffsetPositive;
+  UINTN                              StackOffset;
+  BOOLEAN                            StackOffsetPositive;
+  PEICORE_FUNCTION_POINTER           ShadowedPeiCore;
+  CACHE_SECTION_DATA                 CacheSection;
+  //
+  // For Loading modules at fixed address feature to cache the top address below which the 
+  // Runtime code, boot time code and PEI memory will be placed. Please note that the offset between this field 
+  // and  Ps should not be changed since maybe user could get this top address by using the offet to Ps. 
+  //
+  EFI_PHYSICAL_ADDRESS               LoadModuleAtFixAddressTopAddress;
+  //
+  // The field is define for Loading modules at fixed address feature to tracker the PEI code
+  // memory range usage. It is a bit mapped array in which every bit indicates the correspoding memory page
+  // available or not. 
+  //
+  UINT64                            *PeiCodeMemoryRangeUsageBitMap;
+  //
+  // This field points to the shadowed image read function
+  //
+  PE_COFF_LOADER_READ_FILE          ShadowedImageRead;
+
+  //
+  // Pointer to the temp buffer with the PcdPeiCoreMaxPeimPerFv + 1 number of entries.
+  //
+  EFI_PEI_FILE_HANDLE               *FileHandles;
+  //
+  // Pointer to the temp buffer with the PcdPeiCoreMaxPeimPerFv number of entries.
+  //
+  EFI_GUID                          *FileGuid;
+
+  //
+  // Temp Memory Range is not covered by PeiTempMem and Stack.
+  // Those Memory Range will be migrated into phisical memory. 
+  //
+  HOLE_MEMORY_DATA                  HoleData[HOLE_MAX_NUMBER];
+};
+
+///
+/// Pei Core Instance Data Macros
+///
+#define PEI_CORE_INSTANCE_FROM_PS_THIS(a) \
+  CR(a, PEI_CORE_INSTANCE, Ps, PEI_CORE_HANDLE_SIGNATURE)
+
+///
+/// Union of temporarily used function pointers (to save stack space)
+///
+typedef union {
+  PEICORE_FUNCTION_POINTER     PeiCore;
+  EFI_PEIM_ENTRY_POINT2        PeimEntry;
+  EFI_PEIM_NOTIFY_ENTRY_POINT  PeimNotifyEntry;
+  EFI_DXE_IPL_PPI              *DxeIpl;
+  EFI_PEI_PPI_DESCRIPTOR       *PpiDescriptor;
+  EFI_PEI_NOTIFY_DESCRIPTOR    *NotifyDescriptor;
+  VOID                         *Raw;
+} PEI_CORE_TEMP_POINTERS;
+
+typedef struct {
+  CONST EFI_SEC_PEI_HAND_OFF    *SecCoreData;
+  EFI_PEI_PPI_DESCRIPTOR        *PpiList;
+  VOID                          *Data;
+} PEI_CORE_PARAMETERS;
+
+//
+// PeiCore function
+//
+/**
+
+  The entry routine to Pei Core, invoked by PeiMain during transition
+  from SEC to PEI. After switching stack in the PEI core, it will restart
+  with the old core data.
+
+
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
+                         and location of temporary RAM, the stack location and the BFV location.
+  @param PpiList         Points to a list of one or more PPI descriptors to be installed initially by the PEI core.
+                         An empty PPI list consists of a single descriptor with the end-tag
+                         EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST. As part of its initialization
+                         phase, the PEI Foundation will add these SEC-hosted PPIs to its PPI database such
+                         that both the PEI Foundation and any modules can leverage the associated service
+                         calls and/or code in these early PPIs
+  @param Data            Pointer to old core data that is used to initialize the
+                         core's data areas.
+
+**/
+VOID
+EFIAPI
+PeiCore (
+  IN CONST EFI_SEC_PEI_HAND_OFF        *SecCoreData,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR      *PpiList,
+  IN VOID                              *Data
+  );
+
+//
+// Dispatcher support functions
+//
+
+/**
+
+  This is the POSTFIX version of the dependency evaluator.  When a
+  PUSH [PPI GUID] is encountered, a pointer to the GUID is stored on
+  the evaluation stack.  When that entry is poped from the evaluation
+  stack, the PPI is checked if it is installed.  This method allows
+  some time savings as not all PPIs must be checked for certain
+  operation types (AND, OR).
+
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param DependencyExpression   Pointer to a dependency expression.  The Grammar adheres to
+                                the BNF described above and is stored in postfix notation.
+
+  @retval TRUE      if it is a well-formed Grammar
+  @retval FALSE     if the dependency expression overflows the evaluation stack
+                    if the dependency expression underflows the evaluation stack
+                    if the dependency expression is not a well-formed Grammar.
+
+**/
+BOOLEAN
+PeimDispatchReadiness (
+  IN EFI_PEI_SERVICES   **PeiServices,
+  IN VOID               *DependencyExpression
+  );
+
+/**
+  Conduct PEIM dispatch.
+
+  @param SecCoreData     Pointer to the data structure containing SEC to PEI handoff data
+  @param PrivateData     Pointer to the private data passed in from caller
+
+**/
+VOID
+PeiDispatcher (
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *PrivateData
+  );
+
+/**
+  Initialize the Dispatcher's data members
+
+  @param PrivateData     PeiCore's private data structure
+  @param OldCoreData     Old data from SecCore
+                         NULL if being run in non-permament memory mode.
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size
+                         and location of temporary RAM, the stack location and the BFV location.
+
+**/
+VOID
+InitializeDispatcherData (
+  IN PEI_CORE_INSTANCE            *PrivateData,
+  IN PEI_CORE_INSTANCE            *OldCoreData,
+  IN CONST EFI_SEC_PEI_HAND_OFF   *SecCoreData
+  );
+
+/**
+  This routine parses the Dependency Expression, if available, and
+  decides if the module can be executed.
+
+
+  @param Private         PeiCore's private data structure
+  @param FileHandle      PEIM's file handle
+  @param PeimCount       The index of last dispatched PEIM.
+
+  @retval TRUE           Can be dispatched
+  @retval FALSE          Cannot be dispatched
+
+**/
+BOOLEAN
+DepexSatisfied (
+  IN PEI_CORE_INSTANCE          *Private,
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN UINTN                      PeimCount
+  );
+
+//
+// PPI support functions
+//
+/**
+
+  Initialize PPI services.
+
+  @param PrivateData     Pointer to the PEI Core data.
+  @param OldCoreData     Pointer to old PEI Core data. 
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializePpiServices (
+  IN PEI_CORE_INSTANCE   *PrivateData,
+  IN PEI_CORE_INSTANCE   *OldCoreData
+  );
+
+/**
+
+  Migrate the Hob list from the temporary memory stack to PEI installed memory.
+
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
+                         and location of temporary RAM, the stack location and the BFV location.
+  @param PrivateData     Pointer to PeiCore's private data structure.
+
+**/
+VOID
+ConvertPpiPointers (
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *PrivateData
+  );
+
+/**
+
+  Install PPI services. It is implementation of EFI_PEI_SERVICE.InstallPpi.
+
+  @param PeiServices                An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param PpiList                    Pointer to ppi array that want to be installed.
+
+  @retval EFI_SUCCESS               if all PPIs in PpiList are successfully installed.
+  @retval EFI_INVALID_PARAMETER     if PpiList is NULL pointer
+                                    if any PPI in PpiList is not valid
+  @retval EFI_OUT_OF_RESOURCES      if there is no more memory resource to install PPI
+
+**/
+EFI_STATUS
+EFIAPI
+PeiInstallPpi (
+  IN CONST EFI_PEI_SERVICES        **PeiServices,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *PpiList
+  );
+
+/**
+
+  Re-Install PPI services.
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param OldPpi                 Pointer to the old PEI PPI Descriptors.
+  @param NewPpi                 Pointer to the new PEI PPI Descriptors.
+
+  @retval EFI_SUCCESS           if the operation was successful
+  @retval EFI_INVALID_PARAMETER if OldPpi or NewPpi is NULL
+                                if NewPpi is not valid
+  @retval EFI_NOT_FOUND         if the PPI was not in the database
+
+**/
+EFI_STATUS
+EFIAPI
+PeiReInstallPpi (
+  IN CONST EFI_PEI_SERVICES        **PeiServices,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *OldPpi,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *NewPpi
+  );
+
+/**
+
+  Locate a given named PPI.
+
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Guid            Pointer to GUID of the PPI.
+  @param Instance        Instance Number to discover.
+  @param PpiDescriptor   Pointer to reference the found descriptor. If not NULL,
+                         returns a pointer to the descriptor (includes flags, etc)
+  @param Ppi             Pointer to reference the found PPI
+
+  @retval EFI_SUCCESS   if the PPI is in the database
+  @retval EFI_NOT_FOUND if the PPI is not in the database
+
+**/
+EFI_STATUS
+EFIAPI
+PeiLocatePpi (
+  IN CONST EFI_PEI_SERVICES      **PeiServices,
+  IN CONST EFI_GUID              *Guid,
+  IN UINTN                       Instance,
+  IN OUT EFI_PEI_PPI_DESCRIPTOR  **PpiDescriptor,
+  IN OUT VOID                    **Ppi
+  );
+
+/**
+
+  Install a notification for a given PPI.
+
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param NotifyList             Pointer to list of Descriptors to notify upon.
+
+  @retval EFI_SUCCESS           if successful
+  @retval EFI_OUT_OF_RESOURCES  if no space in the database
+  @retval EFI_INVALID_PARAMETER if not a good decriptor
+
+**/
+EFI_STATUS
+EFIAPI
+PeiNotifyPpi (
+  IN CONST EFI_PEI_SERVICES           **PeiServices,
+  IN CONST EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyList
+  );
+
+/**
+
+  Process the Notify List at dispatch level.
+
+  @param PrivateData  PeiCore's private data structure.
+
+**/
+VOID
+ProcessNotifyList (
+  IN PEI_CORE_INSTANCE  *PrivateData
+  );
+
+/**
+
+  Dispatch notifications.
+
+  @param PrivateData        PeiCore's private data structure
+  @param NotifyType         Type of notify to fire.
+  @param InstallStartIndex  Install Beginning index.
+  @param InstallStopIndex   Install Ending index.
+  @param NotifyStartIndex   Notify Beginning index.
+  @param NotifyStopIndex    Notify Ending index.
+
+**/
+VOID
+DispatchNotify (
+  IN PEI_CORE_INSTANCE  *PrivateData,
+  IN UINTN               NotifyType,
+  IN INTN                InstallStartIndex,
+  IN INTN                InstallStopIndex,
+  IN INTN                NotifyStartIndex,
+  IN INTN                NotifyStopIndex
+  );
+
+//
+// Boot mode support functions
+//
+/**
+  This service enables PEIMs to ascertain the present value of the boot mode.
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param BootMode               A pointer to contain the value of the boot mode.
+
+  @retval EFI_SUCCESS           The boot mode was returned successfully.
+  @retval EFI_INVALID_PARAMETER BootMode is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetBootMode (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN OUT   EFI_BOOT_MODE     *BootMode
+  );
+
+/**
+  This service enables PEIMs to update the boot mode variable.
+
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param BootMode        The value of the boot mode to set.
+
+  @return EFI_SUCCESS    The value was successfully updated
+
+**/
+EFI_STATUS
+EFIAPI
+PeiSetBootMode (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN EFI_BOOT_MODE     BootMode
+  );
+
+//
+// Security support functions
+//
+/**
+
+  Initialize the security services.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param OldCoreData     Pointer to the old core data.
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializeSecurityServices (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_CORE_INSTANCE *OldCoreData
+  );
+
+/**
+  Verify a Firmware volume.
+
+  @param CurrentFvAddress           Pointer to the current Firmware Volume under consideration
+
+  @retval EFI_SUCCESS               Firmware Volume is legal
+  @retval EFI_SECURITY_VIOLATION    Firmware Volume fails integrity test
+
+**/
+EFI_STATUS
+VerifyFv (
+  IN EFI_FIRMWARE_VOLUME_HEADER  *CurrentFvAddress
+  );
+
+/**
+  Provide a callout to the security verification service.
+
+  @param PrivateData     PeiCore's private data structure
+  @param VolumeHandle    Handle of FV
+  @param FileHandle      Handle of PEIM's ffs
+  @param AuthenticationStatus Authentication status
+
+  @retval EFI_SUCCESS              Image is OK
+  @retval EFI_SECURITY_VIOLATION   Image is illegal
+  @retval EFI_NOT_FOUND            If security PPI is not installed.
+**/
+EFI_STATUS
+VerifyPeim (
+  IN PEI_CORE_INSTANCE      *PrivateData,
+  IN EFI_PEI_FV_HANDLE      VolumeHandle,
+  IN EFI_PEI_FILE_HANDLE    FileHandle,
+  IN UINT32                 AuthenticationStatus
+  );
+
+/**
+
+  Gets the pointer to the HOB List.
+
+
+  @param PeiServices                   An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param HobList                       Pointer to the HOB List.
+
+  @retval EFI_SUCCESS                  Get the pointer of HOB List
+  @retval EFI_NOT_AVAILABLE_YET        the HOB List is not yet published
+  @retval EFI_INVALID_PARAMETER        HobList is NULL (in debug mode)
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetHobList (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN OUT VOID          **HobList
+  );
+
+/**
+  Add a new HOB to the HOB List.
+
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Type               Type of the new HOB.
+  @param Length             Length of the new HOB to allocate.
+  @param Hob                Pointer to the new HOB.
+
+  @return  EFI_SUCCESS           Success to create hob.
+  @retval  EFI_INVALID_PARAMETER if Hob is NULL
+  @retval  EFI_NOT_AVAILABLE_YET if HobList is still not available.
+  @retval  EFI_OUT_OF_RESOURCES  if there is no more memory to grow the Hoblist.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiCreateHob (
+  IN CONST EFI_PEI_SERVICES  **PeiServices,
+  IN UINT16            Type,
+  IN UINT16            Length,
+  IN OUT VOID          **Hob
+  );
+
+/**
+
+  Builds a Handoff Information Table HOB
+
+  @param BootMode        - Current Bootmode
+  @param MemoryBegin     - Start Memory Address.
+  @param MemoryLength    - Length of Memory.
+
+  @return EFI_SUCCESS Always success to initialize HOB.
+
+**/
+EFI_STATUS
+PeiCoreBuildHobHandoffInfoTable (
+  IN EFI_BOOT_MODE         BootMode,
+  IN EFI_PHYSICAL_ADDRESS  MemoryBegin,
+  IN UINT64                MemoryLength
+  );
+
+
+//
+// FFS Fw Volume support functions
+//
+/**
+  Searches for the next matching file in the firmware volume.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param SearchType      Filter to find only files of this type.
+                         Type EFI_FV_FILETYPE_ALL causes no filtering to be done.
+  @param FvHandle        Handle of firmware volume in which to search.
+  @param FileHandle      On entry, points to the current handle from which to begin searching or NULL to start
+                         at the beginning of the firmware volume. On exit, points the file handle of the next file
+                         in the volume or NULL if there are no more files.
+
+  @retval EFI_NOT_FOUND  The file was not found.
+  @retval EFI_NOT_FOUND  The header checksum was not zero.
+  @retval EFI_SUCCESS    The file was found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindNextFile (
+  IN CONST EFI_PEI_SERVICES      **PeiServices,
+  IN UINT8                       SearchType,
+  IN EFI_PEI_FV_HANDLE           FvHandle,
+  IN OUT EFI_PEI_FILE_HANDLE     *FileHandle
+  );
+
+/**
+  Searches for the next matching section within the specified file.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param SectionType     Filter to find only sections of this type.
+  @param FileHandle      Pointer to the current file to search.
+  @param SectionData     A pointer to the discovered section, if successful.
+                         NULL if section not found
+
+  @retval EFI_NOT_FOUND  The section was not found.
+  @retval EFI_SUCCESS    The section was found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindSectionData (
+  IN CONST EFI_PEI_SERVICES    **PeiServices,
+  IN     EFI_SECTION_TYPE      SectionType,
+  IN     EFI_PEI_FILE_HANDLE   FileHandle,
+  OUT VOID                     **SectionData
+  );
+
+/**
+  Searches for the next matching section within the specified file.
+
+  @param  PeiServices           An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  SectionType           The value of the section type to find.
+  @param  SectionInstance       Section instance to find.
+  @param  FileHandle            Handle of the firmware file to search.
+  @param  SectionData           A pointer to the discovered section, if successful.
+  @param  AuthenticationStatus  A pointer to the authentication status for this section.
+
+  @retval EFI_SUCCESS      The section was found.
+  @retval EFI_NOT_FOUND    The section was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindSectionData3 (
+  IN CONST EFI_PEI_SERVICES    **PeiServices,
+  IN     EFI_SECTION_TYPE      SectionType,
+  IN     UINTN                 SectionInstance,
+  IN     EFI_PEI_FILE_HANDLE   FileHandle,
+  OUT VOID                     **SectionData,
+  OUT UINT32                   *AuthenticationStatus
+  );
+
+/**
+  Search the firmware volumes by index
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation
+  @param Instance        This instance of the firmware volume to find. The value 0 is the Boot Firmware
+                         Volume (BFV).
+  @param VolumeHandle    On exit, points to the next volume handle or NULL if it does not exist.
+
+  @retval EFI_INVALID_PARAMETER  VolumeHandle is NULL
+  @retval EFI_NOT_FOUND          The volume was not found.
+  @retval EFI_SUCCESS            The volume was found.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindNextVolume (
+  IN CONST EFI_PEI_SERVICES          **PeiServices,
+  IN UINTN                           Instance,
+  IN OUT EFI_PEI_FV_HANDLE           *VolumeHandle
+  );
+
+//
+// Memory support functions
+//
+/**
+
+  Initialize the memory services.
+
+  @param PrivateData     PeiCore's private data structure
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
+                         and location of temporary RAM, the stack location and the BFV location.
+  @param OldCoreData     Pointer to the PEI Core data.
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializeMemoryServices (
+  IN PEI_CORE_INSTANCE           *PrivateData,
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *OldCoreData
+  );
+
+/**
+
+  Install the permanent memory is now available.
+  Creates HOB (PHIT and Stack).
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param MemoryBegin     Start of memory address.
+  @param MemoryLength    Length of memory.
+
+  @return EFI_SUCCESS Always success.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiInstallPeiMemory (
+  IN CONST EFI_PEI_SERVICES      **PeiServices,
+  IN EFI_PHYSICAL_ADDRESS  MemoryBegin,
+  IN UINT64                MemoryLength
+  );
+
+/**
+
+  Memory allocation service on permanent memory,
+  not usable prior to the memory installation.
+
+
+  @param PeiServices               An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param MemoryType                Type of memory to allocate.
+  @param Pages                     Number of pages to allocate.
+  @param Memory                    Pointer of memory allocated.
+
+  @retval EFI_SUCCESS              The allocation was successful
+  @retval EFI_INVALID_PARAMETER    Only AllocateAnyAddress is supported.
+  @retval EFI_NOT_AVAILABLE_YET    Called with permanent memory not available
+  @retval EFI_OUT_OF_RESOURCES     There is not enough HOB heap to satisfy the requirement
+                                   to allocate the number of pages.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiAllocatePages (
+  IN CONST EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_MEMORY_TYPE            MemoryType,
+  IN UINTN                      Pages,
+  OUT EFI_PHYSICAL_ADDRESS      *Memory
+  );
+
+/**
+
+  Memory allocation service on the temporary memory.
+
+
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Size               Amount of memory required
+  @param Buffer             Address of pointer to the buffer
+
+  @retval EFI_SUCCESS              The allocation was successful
+  @retval EFI_OUT_OF_RESOURCES     There is not enough heap to satisfy the requirement
+                                   to allocate the requested size.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiAllocatePool (
+  IN CONST EFI_PEI_SERVICES           **PeiServices,
+  IN UINTN                      Size,
+  OUT VOID                      **Buffer
+  );
+
+/**
+
+  Routine for load image file.
+
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param FileHandle             Pointer to the FFS file header of the image.
+  @param PeimState              The dispatch state of the input PEIM handle.
+  @param EntryPoint             Pointer to entry point of specified image file for output.
+  @param AuthenticationState    Pointer to attestation authentication state of image.
+
+  @retval EFI_SUCCESS     Image is successfully loaded.
+  @retval EFI_NOT_FOUND   Fail to locate necessary PPI
+  @retval Others          Fail to load file.
+
+**/
+EFI_STATUS
+PeiLoadImage (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  EFI_PEI_FILE_HANDLE         FileHandle,
+  IN  UINT8                       PeimState,
+  OUT    EFI_PHYSICAL_ADDRESS     *EntryPoint,
+  OUT    UINT32                   *AuthenticationState
+  );
+
+/**
+
+  Core version of the Status Code reporter
+
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param CodeType               Type of Status Code.
+  @param Value                  Value to output for Status Code.
+  @param Instance               Instance Number of this status code.
+  @param CallerId               ID of the caller of this status code.
+  @param Data                   Optional data associated with this status code.
+
+  @retval EFI_SUCCESS             if status code is successfully reported
+  @retval EFI_NOT_AVAILABLE_YET   if StatusCodePpi has not been installed
+
+**/
+EFI_STATUS
+EFIAPI
+PeiReportStatusCode (
+  IN CONST EFI_PEI_SERVICES         **PeiServices,
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN CONST EFI_GUID                 *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  );
+
+/**
+
+  Core version of the Reset System
+
+
+  @param PeiServices                An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+
+  @retval EFI_NOT_AVAILABLE_YET     PPI not available yet.
+  @retval EFI_DEVICE_ERROR          Did not reset system.
+                                    Otherwise, resets the system.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiResetSystem (
+  IN CONST EFI_PEI_SERVICES   **PeiServices
+  );
+
+/**
+  Resets the entire platform.
+
+  @param[in] ResetType      The type of reset to perform.
+  @param[in] ResetStatus    The status code for the reset.
+  @param[in] DataSize       The size, in bytes, of WatchdogData.
+  @param[in] ResetData      For a ResetType of EfiResetCold, EfiResetWarm, or EfiResetShutdown
+                            the data buffer starts with a Null-terminated string, optionally
+                            followed by additional binary data. The string is a description
+                            that the caller may use to further indicate the reason for the
+                            system reset. ResetData is only valid if ResetStatus is something
+                            other than EFI_SUCCESS unless the ResetType is EfiResetPlatformSpecific
+                            where a minimum amount of ResetData is always required.
+
+**/
+VOID
+EFIAPI
+PeiResetSystem2 (
+  IN EFI_RESET_TYPE     ResetType,
+  IN EFI_STATUS         ResetStatus,
+  IN UINTN              DataSize,
+  IN VOID               *ResetData OPTIONAL
+  );
+
+/**
+
+  Initialize PeiCore Fv List.
+
+
+  @param PrivateData     - Pointer to PEI_CORE_INSTANCE.
+  @param SecCoreData     - Pointer to EFI_SEC_PEI_HAND_OFF.
+
+**/
+VOID
+PeiInitializeFv (
+  IN  PEI_CORE_INSTANCE           *PrivateData,
+  IN CONST EFI_SEC_PEI_HAND_OFF   *SecCoreData
+  );
+
+/**
+  Process Firmware Volum Information once FvInfoPPI install.
+
+  @param PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param NotifyDescriptor  Address of the notification descriptor data structure.
+  @param Ppi               Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS if the interface could be successfully installed
+
+**/
+EFI_STATUS
+EFIAPI
+FirmwareVolmeInfoPpiNotifyCallback (
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR     *NotifyDescriptor,
+  IN VOID                          *Ppi
+  );
+
+/**
+
+  Given the input VolumeHandle, search for the next matching name file.
+
+  @param FileName        File name to search.
+  @param VolumeHandle    The current FV to search.
+  @param FileHandle      Pointer to the file matching name in VolumeHandle.
+                         NULL if file not found
+
+  @retval EFI_NOT_FOUND  No files matching the search criteria were found
+  @retval EFI_SUCCESS    Success to search given file
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsFindFileByName (
+  IN  CONST EFI_GUID        *FileName,
+  IN  EFI_PEI_FV_HANDLE     VolumeHandle,
+  OUT EFI_PEI_FILE_HANDLE   *FileHandle
+  );
+
+/**
+  Returns information about a specific file.
+
+  @param FileHandle       Handle of the file.
+  @param FileInfo         Upon exit, points to the file's information.
+
+  @retval EFI_INVALID_PARAMETER If FileInfo is NULL.
+  @retval EFI_INVALID_PARAMETER If FileHandle does not represent a valid file.
+  @retval EFI_SUCCESS           File information returned.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsGetFileInfo (
+  IN EFI_PEI_FILE_HANDLE  FileHandle,
+  OUT EFI_FV_FILE_INFO    *FileInfo
+  );
+
+/**
+  Returns information about a specific file.
+
+  @param FileHandle       Handle of the file.
+  @param FileInfo         Upon exit, points to the file's information.
+
+  @retval EFI_INVALID_PARAMETER If FileInfo is NULL.
+  @retval EFI_INVALID_PARAMETER If FileHandle does not represent a valid file.
+  @retval EFI_SUCCESS           File information returned.
+
+**/
+EFI_STATUS
+EFIAPI 
+PeiFfsGetFileInfo2 (
+  IN EFI_PEI_FILE_HANDLE  FileHandle,
+  OUT EFI_FV_FILE_INFO2   *FileInfo
+  );
+
+/**
+  Returns information about the specified volume.
+
+  @param VolumeHandle    Handle of the volume.
+  @param VolumeInfo      Upon exit, points to the volume's information.
+
+  @retval EFI_INVALID_PARAMETER If VolumeHandle does not represent a valid volume.
+  @retval EFI_INVALID_PARAMETER If VolumeInfo is NULL.
+  @retval EFI_SUCCESS           Volume information returned.
+**/
+EFI_STATUS
+EFIAPI
+PeiFfsGetVolumeInfo (
+  IN EFI_PEI_FV_HANDLE  VolumeHandle,
+  OUT EFI_FV_INFO       *VolumeInfo
+  );
+
+/**
+  This routine enable a PEIM to register itself to shadow when PEI Foundation
+  discovery permanent memory.
+
+  @param FileHandle             File handle of a PEIM.
+
+  @retval EFI_NOT_FOUND         The file handle doesn't point to PEIM itself.
+  @retval EFI_ALREADY_STARTED   Indicate that the PEIM has been registered itself.
+  @retval EFI_SUCCESS           Successfully to register itself.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiRegisterForShadow (
+  IN EFI_PEI_FILE_HANDLE       FileHandle
+  );
+
+/**
+  Initialize image service that install PeiLoadFilePpi.
+
+  @param PrivateData     Pointer to PeiCore's private data structure PEI_CORE_INSTANCE.
+  @param OldCoreData     Pointer to Old PeiCore's private data.
+                         If NULL, PeiCore is entered at first time, stack/heap in temporary memory.
+                         If not NULL, PeiCore is entered at second time, stack/heap has been moved
+                         to permenent memory.
+
+**/
+VOID
+InitializeImageServices (
+  IN  PEI_CORE_INSTANCE   *PrivateData,
+  IN  PEI_CORE_INSTANCE   *OldCoreData
+  );
+
+/**
+  The wrapper function of PeiLoadImageLoadImage().
+
+  @param This                 Pointer to EFI_PEI_LOAD_FILE_PPI.
+  @param FileHandle           Pointer to the FFS file header of the image.
+  @param ImageAddressArg      Pointer to PE/TE image.
+  @param ImageSizeArg         Size of PE/TE image.
+  @param EntryPoint           Pointer to entry point of specified image file for output.
+  @param AuthenticationState  Pointer to attestation authentication state of image.
+
+  @return Status of PeiLoadImageLoadImage().
+
+**/
+EFI_STATUS
+EFIAPI
+PeiLoadImageLoadImageWrapper (
+  IN     CONST EFI_PEI_LOAD_FILE_PPI  *This,
+  IN     EFI_PEI_FILE_HANDLE          FileHandle,
+  OUT    EFI_PHYSICAL_ADDRESS         *ImageAddressArg,  OPTIONAL
+  OUT    UINT64                       *ImageSizeArg,     OPTIONAL
+  OUT    EFI_PHYSICAL_ADDRESS         *EntryPoint,
+  OUT    UINT32                       *AuthenticationState
+  );
+
+/**
+
+  Provide a callback for when the security PPI is installed.
+
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param NotifyDescriptor   The descriptor for the notification event.
+  @param Ppi                Pointer to the PPI in question.
+
+  @return Always success
+
+**/
+EFI_STATUS
+EFIAPI
+SecurityPpiNotifyCallback (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyDescriptor,
+  IN VOID                       *Ppi
+  );
+
+/**
+  Get Fv image from the FV type file, then install FV INFO(2) ppi, Build FV hob.
+
+  @param PrivateData          PeiCore's private data structure
+  @param ParentFvCoreHandle   Pointer of EFI_CORE_FV_HANDLE to parent Fv image that contain this Fv image.
+  @param ParentFvFileHandle   File handle of a Fv type file that contain this Fv image.
+
+  @retval EFI_NOT_FOUND         FV image can't be found.
+  @retval EFI_SUCCESS           Successfully to process it.
+  @retval EFI_OUT_OF_RESOURCES  Can not allocate page when aligning FV image
+  @retval EFI_SECURITY_VIOLATION Image is illegal
+  @retval Others                Can not find EFI_SECTION_FIRMWARE_VOLUME_IMAGE section
+  
+**/
+EFI_STATUS
+ProcessFvFile (
+  IN  PEI_CORE_INSTANCE           *PrivateData,
+  IN  PEI_CORE_FV_HANDLE          *ParentFvCoreHandle,
+  IN  EFI_PEI_FILE_HANDLE         ParentFvFileHandle
+  );
+  
+/**
+  Get instance of PEI_CORE_FV_HANDLE for next volume according to given index.
+  
+  This routine also will install FvInfo ppi for FV hob in PI ways.
+  
+  @param Private    Pointer of PEI_CORE_INSTANCE
+  @param Instance   The index of FV want to be searched.
+  
+  @return Instance of PEI_CORE_FV_HANDLE.
+**/
+PEI_CORE_FV_HANDLE *
+FindNextCoreFvHandle (
+  IN PEI_CORE_INSTANCE  *Private,
+  IN UINTN              Instance
+  );
+    
+//
+// Default EFI_PEI_CPU_IO_PPI support for EFI_PEI_SERVICES table when PeiCore initialization.
+//    
+
+/**
+  Memory-based read services.
+  
+  This function is to perform the Memory Access Read service based on installed 
+  instance of the EFI_PEI_CPU_IO_PPI. 
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+   
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.     
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultMemRead (
+  IN  CONST EFI_PEI_SERVICES            **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI          *This,
+  IN  EFI_PEI_CPU_IO_PPI_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  OUT VOID                          *Buffer
+  );
+  
+/**
+  Memory-based write services.
+   
+  This function is to perform the Memory Access Write service based on installed 
+  instance of the EFI_PEI_CPU_IO_PPI. 
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+   
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.     
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultMemWrite (
+  IN  CONST EFI_PEI_SERVICES            **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI          *This,
+  IN  EFI_PEI_CPU_IO_PPI_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  OUT VOID                          *Buffer
+  );
+  
+/**
+  IO-based read services.
+  
+  This function is to perform the IO-base read service for the EFI_PEI_CPU_IO_PPI.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultIoRead (
+  IN      CONST EFI_PEI_SERVICES          **PeiServices,
+  IN      CONST EFI_PEI_CPU_IO_PPI        *This,
+  IN      EFI_PEI_CPU_IO_PPI_WIDTH        Width,
+  IN      UINT64                          Address,
+  IN      UINTN                           Count,
+  IN OUT  VOID                            *Buffer
+  );
+  
+/**
+  IO-based write services.
+  
+  This function is to perform the IO-base write service for the EFI_PEI_CPU_IO_PPI.
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices           An indirect pointer to the PEI Services Table
+                                published by the PEI Foundation.
+  @param  This                  Pointer to local data for the interface.
+  @param  Width                 The width of the access. Enumerated in bytes.
+  @param  Address               The physical address of the access.
+  @param  Count                 The number of accesses to perform.
+  @param  Buffer                A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_NOT_YET_AVAILABLE The service has not been installed.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultIoWrite (
+  IN      CONST EFI_PEI_SERVICES          **PeiServices,
+  IN      CONST EFI_PEI_CPU_IO_PPI        *This,
+  IN      EFI_PEI_CPU_IO_PPI_WIDTH        Width,
+  IN      UINT64                          Address,
+  IN      UINTN                           Count,
+  IN OUT  VOID                            *Buffer
+  );
+  
+/**
+  8-bit I/O read operations.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 8-bit value returned from the I/O space.
+**/
+UINT8
+EFIAPI
+PeiDefaultIoRead8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  Reads an 16-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 16-bit value returned from the I/O space.
+**/
+UINT16
+EFIAPI
+PeiDefaultIoRead16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  Reads an 32-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 32-bit value returned from the I/O space.
+**/
+UINT32
+EFIAPI
+PeiDefaultIoRead32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  Reads an 64-bit I/O port.
+  
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return A 64-bit value returned from the I/O space.
+**/
+UINT64
+EFIAPI
+PeiDefaultIoRead64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  8-bit I/O write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT8                       Data
+  );
+  
+/**
+  16-bit I/O write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT16                      Data
+  );
+  
+/**
+  32-bit I/O write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT32                      Data
+  );
+  
+/**
+  64-bit I/O write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+**/
+VOID
+EFIAPI
+PeiDefaultIoWrite64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address,
+  IN  UINT64                      Data
+  );
+  
+/**
+  8-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 8-bit value returned from the memory space.
+
+**/
+UINT8
+EFIAPI
+PeiDefaultMemRead8 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  16-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 16-bit value returned from the memory space.
+
+**/
+UINT16
+EFIAPI
+PeiDefaultMemRead16 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  32-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 32-bit value returned from the memory space.
+
+**/
+UINT32
+EFIAPI
+PeiDefaultMemRead32 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  64-bit memory read operations.
+
+  If the EFI_PEI_CPU_IO_PPI is not installed by platform/chipset PEIM, then 
+  return 0.
+  
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+
+  @return An 64-bit value returned from the memory space.
+
+**/
+UINT64
+EFIAPI
+PeiDefaultMemRead64 (
+  IN  CONST EFI_PEI_SERVICES      **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI    *This,
+  IN  UINT64                      Address
+  );
+  
+/**
+  8-bit memory write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite8 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT8                         Data
+  );
+  
+/**
+  16-bit memory write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite16 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT16                        Data
+  );
+
+/**
+  32-bit memory write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite32 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT32                        Data
+  );
+  
+/**
+  64-bit memory write operations.
+
+  @param  PeiServices    An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This           Pointer to local data for the interface.
+  @param  Address        The physical address of the access.
+  @param  Data           The data to write.
+
+**/
+VOID
+EFIAPI
+PeiDefaultMemWrite64 (
+  IN  CONST EFI_PEI_SERVICES        **PeiServices,
+  IN  CONST EFI_PEI_CPU_IO_PPI      *This,
+  IN  UINT64                        Address,
+  IN  UINT64                        Data
+  );
+  
+extern EFI_PEI_CPU_IO_PPI gPeiDefaultCpuIoPpi;                                        
+
+//
+// Default EFI_PEI_PCI_CFG2_PPI support for EFI_PEI_SERVICES table when PeiCore initialization.
+// 
+
+/**
+  Reads from a given location in the PCI configuration space.
+
+  If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Read (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  );
+  
+/**
+  Write to a given location in the PCI configuration space.
+
+  If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM, then 
+  return EFI_NOT_YET_AVAILABLE. 
+  
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Write (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  );
+  
+/**
+  This function performs a read-modify-write operation on the contents from a given
+  location in the PCI configuration space.
+
+  @param  PeiServices     An indirect pointer to the PEI Services Table
+                          published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes. Type
+                          EFI_PEI_PCI_CFG_PPI_WIDTH is defined in Read().
+  @param  Address         The physical address of the access.
+  @param  SetBits         Points to value to bitwise-OR with the read configuration value.
+                          The size of the value is determined by Width.
+  @param  ClearBits       Points to the value to negate and bitwise-AND with the read configuration value.
+                          The size of the value is determined by Width.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  @retval EFI_NOT_YET_AVAILABLE If the EFI_PEI_PCI_CFG2_PPI is not installed by platform/chipset PEIM.
+**/
+EFI_STATUS
+EFIAPI
+PeiDefaultPciCfg2Modify (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN        VOID                      *SetBits,
+  IN        VOID                      *ClearBits
+  );    
+  
+extern EFI_PEI_PCI_CFG2_PPI gPeiDefaultPciCfg2Ppi;
+
+/**
+  After PeiCore image is shadowed into permanent memory, all build-in FvPpi should
+  be re-installed with the instance in permanent memory and all cached FvPpi pointers in 
+  PrivateData->Fv[] array should be fixed up to be pointed to the one in permenant
+  memory.
+  
+  @param PrivateData   Pointer to PEI_CORE_INSTANCE.
+**/  
+VOID
+PeiReinitializeFv (
+  IN  PEI_CORE_INSTANCE           *PrivateData
+  );
+      
+#endif
diff --git a/Core/MdeModulePkg/Core/Pei/PeiMain.inf b/Core/MdeModulePkg/Core/Pei/PeiMain.inf
new file mode 100644
index 0000000000..39a464f326
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PeiMain.inf
@@ -0,0 +1,134 @@
+## @file
+# PeiMain module is core module in PEI phase.
+#
+# It takes responsibilities of:
+# 1) Initialize memory, PPI, image services etc, to establish PEIM runtime environment.
+# 2) Dispatch PEIM from discovered FV.
+# 3) Handoff control to DxeIpl to load DXE core and enter DXE phase.
+#
+# Copyright (c) 2006 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiCore
+  MODULE_UNI_FILE                = PeiCore.uni
+  FILE_GUID                      = 52C05B14-0B98-496c-BC3B-04B50211D680
+  MODULE_TYPE                    = PEI_CORE
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PeiCore
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  StatusCode/StatusCode.c
+  Security/Security.c
+  Reset/Reset.c
+  Ppi/Ppi.c
+  PeiMain/PeiMain.c
+  Memory/MemoryServices.c
+  Image/Image.c
+  Hob/Hob.c
+  FwVol/FwVol.c
+  FwVol/FwVol.h
+  Dispatcher/Dispatcher.c
+  Dependency/Dependency.c
+  Dependency/Dependency.h
+  BootMode/BootMode.c
+  CpuIo/CpuIo.c
+  PciCfg2/PciCfg2.c
+  PeiMain.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  PeCoffGetEntryPointLib
+  ReportStatusCodeLib
+  PeiServicesLib
+  PerformanceLib
+  HobLib
+  BaseLib
+  PeiCoreEntryPoint
+  DebugLib
+  MemoryAllocationLib
+  CacheMaintenanceLib
+  PeCoffLib
+  PeiServicesTablePointerLib
+  PcdLib
+
+[Guids]
+  gPeiAprioriFileNameGuid       ## SOMETIMES_CONSUMES   ## File
+  ## PRODUCES   ## UNDEFINED # Install ppi
+  ## CONSUMES   ## UNDEFINED # Locate ppi
+  gEfiFirmwareFileSystem2Guid
+  ## PRODUCES   ## UNDEFINED # Install ppi
+  ## CONSUMES   ## UNDEFINED # Locate ppi
+  ## CONSUMES   ## GUID      # Used to compare with FV's file system guid and get the FV's file system format
+  gEfiFirmwareFileSystem3Guid
+  
+[Ppis]
+  gEfiPeiStatusCodePpiGuid                      ## SOMETIMES_CONSUMES # PeiReportStatusService is not ready if this PPI doesn't exist
+  gEfiPeiResetPpiGuid                           ## SOMETIMES_CONSUMES # PeiResetService is not ready if this PPI doesn't exist 
+  gEfiDxeIplPpiGuid                             ## CONSUMES
+  gEfiPeiMemoryDiscoveredPpiGuid                ## PRODUCES
+  gEfiPeiDecompressPpiGuid                      ## SOMETIMES_CONSUMES
+  ## NOTIFY
+  ## SOMETIMES_PRODUCES # Produce FvInfoPpi if the encapsulated FvImage is found
+  gEfiPeiFirmwareVolumeInfoPpiGuid
+  ## NOTIFY
+  ## SOMETIMES_PRODUCES # Produce FvInfoPpi2 if the encapsulated FvImage is found
+  gEfiPeiFirmwareVolumeInfo2PpiGuid
+  ## PRODUCES
+  ## CONSUMES
+  gEfiPeiLoadFilePpiGuid
+  gEfiPeiSecurity2PpiGuid                       ## NOTIFY
+  gEfiTemporaryRamSupportPpiGuid                ## SOMETIMES_CONSUMES
+  gEfiTemporaryRamDonePpiGuid                   ## SOMETIMES_CONSUMES
+  gEfiPeiReset2PpiGuid                          ## SOMETIMES_CONSUMES
+
+[Pcd]  
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxFvSupported                   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPeimPerFv                     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPpiSupported                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPeiStackSize                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreImageLoaderSearchTeSectionFirst  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrameworkCompatibilitySupport           ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressPeiCodePageNumber         ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressBootTimeCodePageNumber    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressRuntimeCodePageNumber     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdShadowPeimOnS3Boot                      ## CONSUMES 
+  gEfiMdeModulePkgTokenSpaceGuid.PcdShadowPeimOnBoot                        ## CONSUMES 
+
+# [BootMode]
+# S3_RESUME             ## SOMETIMES_CONSUMES
+
+# [Hob]
+# PHIT                  ## PRODUCES
+# RESOURCE_DESCRIPTOR   ## SOMETIMES_PRODUCES
+# RESOURCE_DESCRIPTOR   ## SOMETIMES_CONSUMES
+# MEMORY_ALLOCATION     ## SOMETIMES_CONSUMES
+# FIRMWARE_VOLUME       ## SOMETIMES_PRODUCES
+# FIRMWARE_VOLUME       ## SOMETIMES_CONSUMES
+# MEMORY_ALLOCATION     ## SOMETIMES_PRODUCES
+# MEMORY_ALLOCATION     ## PRODUCES # MEMORY_ALLOCATION_STACK
+# UNDEFINED             ## PRODUCES # MEMORY_POOL
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PeiCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/Pei/PeiMain/PeiMain.c b/Core/MdeModulePkg/Core/Pei/PeiMain/PeiMain.c
new file mode 100644
index 0000000000..d36f89c3dd
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/PeiMain/PeiMain.c
@@ -0,0 +1,469 @@
+/** @file
+  Pei Core Main Entry Point
+  
+Copyright (c) 2006 - 2015, 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 "PeiMain.h"
+
+EFI_PEI_PPI_DESCRIPTOR mMemoryDiscoveredPpi = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiMemoryDiscoveredPpiGuid,
+  NULL
+};
+
+///
+/// Pei service instance
+///
+EFI_PEI_SERVICES  gPs = {
+  {
+    PEI_SERVICES_SIGNATURE,
+    PEI_SERVICES_REVISION,
+    sizeof (EFI_PEI_SERVICES),
+    0,
+    0
+  },
+  PeiInstallPpi,
+  PeiReInstallPpi,
+  PeiLocatePpi,
+  PeiNotifyPpi,
+
+  PeiGetBootMode,
+  PeiSetBootMode,
+
+  PeiGetHobList,
+  PeiCreateHob,
+
+  PeiFfsFindNextVolume,
+  PeiFfsFindNextFile,
+  PeiFfsFindSectionData,
+
+  PeiInstallPeiMemory,      
+  PeiAllocatePages,
+  PeiAllocatePool,
+  (EFI_PEI_COPY_MEM)CopyMem,
+  (EFI_PEI_SET_MEM)SetMem,
+
+  PeiReportStatusCode,
+  PeiResetSystem,
+
+  &gPeiDefaultCpuIoPpi,
+  &gPeiDefaultPciCfg2Ppi,
+
+  PeiFfsFindFileByName,
+  PeiFfsGetFileInfo,
+  PeiFfsGetVolumeInfo,
+  PeiRegisterForShadow,
+  PeiFfsFindSectionData3,
+  PeiFfsGetFileInfo2,
+  PeiResetSystem2
+};
+
+/**
+  Shadow PeiCore module from flash to installed memory.
+  
+  @param PrivateData    PeiCore's private data structure
+
+  @return PeiCore function address after shadowing.
+**/
+PEICORE_FUNCTION_POINTER
+ShadowPeiCore (
+  IN PEI_CORE_INSTANCE  *PrivateData
+  )
+{
+  EFI_PEI_FILE_HANDLE  PeiCoreFileHandle;
+  EFI_PHYSICAL_ADDRESS EntryPoint;
+  EFI_STATUS           Status;
+  UINT32               AuthenticationState;
+
+  PeiCoreFileHandle = NULL;
+
+  //
+  // Find the PEI Core in the BFV
+  //
+  Status = PrivateData->Fv[0].FvPpi->FindFileByType (
+                                       PrivateData->Fv[0].FvPpi,
+                                       EFI_FV_FILETYPE_PEI_CORE,
+                                       PrivateData->Fv[0].FvHandle,
+                                       &PeiCoreFileHandle
+                                       );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Shadow PEI Core into memory so it will run faster
+  //
+  Status = PeiLoadImage (
+              GetPeiServicesTablePointer (),
+              *((EFI_PEI_FILE_HANDLE*)&PeiCoreFileHandle),
+              PEIM_STATE_REGISITER_FOR_SHADOW,
+              &EntryPoint,
+              &AuthenticationState
+              );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Compute the PeiCore's function address after shaowed PeiCore.
+  // _ModuleEntryPoint is PeiCore main function entry
+  //
+  return (PEICORE_FUNCTION_POINTER)((UINTN) EntryPoint + (UINTN) PeiCore - (UINTN) _ModuleEntryPoint);
+}
+
+/**
+  This routine is invoked by main entry of PeiMain module during transition
+  from SEC to PEI. After switching stack in the PEI core, it will restart
+  with the old core data.
+
+  @param SecCoreDataPtr  Points to a data structure containing information about the PEI core's operating
+                         environment, such as the size and location of temporary RAM, the stack location and
+                         the BFV location.
+  @param PpiList         Points to a list of one or more PPI descriptors to be installed initially by the PEI core.
+                         An empty PPI list consists of a single descriptor with the end-tag
+                         EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST. As part of its initialization
+                         phase, the PEI Foundation will add these SEC-hosted PPIs to its PPI database such
+                         that both the PEI Foundation and any modules can leverage the associated service
+                         calls and/or code in these early PPIs
+  @param Data            Pointer to old core data that is used to initialize the
+                         core's data areas.
+                         If NULL, it is first PeiCore entering.
+
+**/
+VOID
+EFIAPI
+PeiCore (
+  IN CONST EFI_SEC_PEI_HAND_OFF        *SecCoreDataPtr,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR      *PpiList,
+  IN VOID                              *Data
+  )
+{
+  PEI_CORE_INSTANCE           PrivateData;
+  EFI_SEC_PEI_HAND_OFF        *SecCoreData;
+  EFI_SEC_PEI_HAND_OFF        NewSecCoreData;
+  EFI_STATUS                  Status;
+  PEI_CORE_TEMP_POINTERS      TempPtr;
+  PEI_CORE_INSTANCE           *OldCoreData;
+  EFI_PEI_CPU_IO_PPI          *CpuIo;
+  EFI_PEI_PCI_CFG2_PPI        *PciCfg;
+  EFI_HOB_HANDOFF_INFO_TABLE  *HandoffInformationTable;
+  EFI_PEI_TEMPORARY_RAM_DONE_PPI *TemporaryRamDonePpi;
+  UINTN                       Index;
+  
+  //
+  // Retrieve context passed into PEI Core
+  //
+  OldCoreData = (PEI_CORE_INSTANCE *) Data;
+  SecCoreData = (EFI_SEC_PEI_HAND_OFF *) SecCoreDataPtr;
+
+  //
+  // Perform PEI Core phase specific actions.
+  //
+  if (OldCoreData == NULL) {
+    //
+    // If OldCoreData is NULL, means current is the first entry into the PEI Core before memory is available.
+    //
+    ZeroMem (&PrivateData, sizeof (PEI_CORE_INSTANCE));
+    PrivateData.Signature = PEI_CORE_HANDLE_SIGNATURE;
+    CopyMem (&PrivateData.ServiceTableShadow, &gPs, sizeof (gPs));
+  } else {
+    //
+    // Memory is available to the PEI Core.  See if the PEI Core has been shadowed to memory yet.
+    //
+    if (OldCoreData->ShadowedPeiCore == NULL) {
+      //
+      // Fixup the PeiCore's private data
+      //
+      OldCoreData->Ps    = &OldCoreData->ServiceTableShadow;
+      OldCoreData->CpuIo = &OldCoreData->ServiceTableShadow.CpuIo;
+      if (OldCoreData->HeapOffsetPositive) {
+        OldCoreData->HobList.Raw = (VOID *)(OldCoreData->HobList.Raw + OldCoreData->HeapOffset);
+        OldCoreData->UnknownFvInfo        = (PEI_CORE_UNKNOW_FORMAT_FV_INFO *) ((UINT8 *) OldCoreData->UnknownFvInfo + OldCoreData->HeapOffset);
+        OldCoreData->CurrentFvFileHandles = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->CurrentFvFileHandles + OldCoreData->HeapOffset);
+        OldCoreData->PpiData.PpiListPtrs  = (PEI_PPI_LIST_POINTERS *) ((UINT8 *) OldCoreData->PpiData.PpiListPtrs + OldCoreData->HeapOffset);
+        OldCoreData->Fv                   = (PEI_CORE_FV_HANDLE *) ((UINT8 *) OldCoreData->Fv + OldCoreData->HeapOffset);
+        for (Index = 0; Index < PcdGet32 (PcdPeiCoreMaxFvSupported); Index ++) {
+          OldCoreData->Fv[Index].PeimState     = (UINT8 *) OldCoreData->Fv[Index].PeimState + OldCoreData->HeapOffset;
+          OldCoreData->Fv[Index].FvFileHandles = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->Fv[Index].FvFileHandles + OldCoreData->HeapOffset);
+        }
+        OldCoreData->FileGuid             = (EFI_GUID *) ((UINT8 *) OldCoreData->FileGuid + OldCoreData->HeapOffset);
+        OldCoreData->FileHandles          = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->FileHandles + OldCoreData->HeapOffset);
+      } else {
+        OldCoreData->HobList.Raw = (VOID *)(OldCoreData->HobList.Raw - OldCoreData->HeapOffset);
+        OldCoreData->UnknownFvInfo        = (PEI_CORE_UNKNOW_FORMAT_FV_INFO *) ((UINT8 *) OldCoreData->UnknownFvInfo - OldCoreData->HeapOffset);
+        OldCoreData->CurrentFvFileHandles = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->CurrentFvFileHandles - OldCoreData->HeapOffset);
+        OldCoreData->PpiData.PpiListPtrs  = (PEI_PPI_LIST_POINTERS *) ((UINT8 *) OldCoreData->PpiData.PpiListPtrs - OldCoreData->HeapOffset);
+        OldCoreData->Fv                   = (PEI_CORE_FV_HANDLE *) ((UINT8 *) OldCoreData->Fv - OldCoreData->HeapOffset);
+        for (Index = 0; Index < PcdGet32 (PcdPeiCoreMaxFvSupported); Index ++) {
+          OldCoreData->Fv[Index].PeimState     = (UINT8 *) OldCoreData->Fv[Index].PeimState - OldCoreData->HeapOffset;
+          OldCoreData->Fv[Index].FvFileHandles = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->Fv[Index].FvFileHandles - OldCoreData->HeapOffset);
+        }
+        OldCoreData->FileGuid             = (EFI_GUID *) ((UINT8 *) OldCoreData->FileGuid - OldCoreData->HeapOffset);
+        OldCoreData->FileHandles          = (EFI_PEI_FILE_HANDLE *) ((UINT8 *) OldCoreData->FileHandles - OldCoreData->HeapOffset);
+      }
+
+      //
+      // Initialize libraries that the PEI Core is linked against
+      //
+      ProcessLibraryConstructorList (NULL, (CONST EFI_PEI_SERVICES **)&OldCoreData->Ps);
+      
+      //
+      // Fixup for PeiService's address
+      //
+      SetPeiServicesTablePointer ((CONST EFI_PEI_SERVICES **)&OldCoreData->Ps);
+
+      //
+      // Update HandOffHob for new installed permenent memory
+      //
+      HandoffInformationTable = OldCoreData->HobList.HandoffInformationTable;
+      if (OldCoreData->HeapOffsetPositive) {
+        HandoffInformationTable->EfiEndOfHobList   = HandoffInformationTable->EfiEndOfHobList + OldCoreData->HeapOffset;
+      } else {
+        HandoffInformationTable->EfiEndOfHobList   = HandoffInformationTable->EfiEndOfHobList - OldCoreData->HeapOffset;
+      }
+      HandoffInformationTable->EfiMemoryTop        = OldCoreData->PhysicalMemoryBegin + OldCoreData->PhysicalMemoryLength;
+      HandoffInformationTable->EfiMemoryBottom     = OldCoreData->PhysicalMemoryBegin;
+      HandoffInformationTable->EfiFreeMemoryTop    = OldCoreData->FreePhysicalMemoryTop;
+      HandoffInformationTable->EfiFreeMemoryBottom = HandoffInformationTable->EfiEndOfHobList + sizeof (EFI_HOB_GENERIC_HEADER);
+
+      //
+      // We need convert the PPI descriptor's pointer
+      //
+      ConvertPpiPointers (SecCoreData, OldCoreData);
+
+      //
+      // After the whole temporary memory is migrated, then we can allocate page in
+      // permenent memory.
+      //
+      OldCoreData->PeiMemoryInstalled = TRUE;
+
+      //
+      // Indicate that PeiCore reenter
+      //
+      OldCoreData->PeimDispatcherReenter = TRUE;
+      
+      if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0 && (OldCoreData->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME)) {
+        //
+        // if Loading Module at Fixed Address is enabled, allocate the PEI code memory range usage bit map array.
+        // Every bit in the array indicate the status of the corresponding memory page available or not
+        //
+        OldCoreData->PeiCodeMemoryRangeUsageBitMap = AllocateZeroPool (((PcdGet32(PcdLoadFixAddressPeiCodePageNumber)>>6) + 1)*sizeof(UINT64));
+      }
+
+      //
+      // Shadow PEI Core. When permanent memory is avaiable, shadow
+      // PEI Core and PEIMs to get high performance.
+      //
+      OldCoreData->ShadowedPeiCore = (PEICORE_FUNCTION_POINTER) (UINTN) PeiCore;
+      if ((HandoffInformationTable->BootMode == BOOT_ON_S3_RESUME && PcdGetBool (PcdShadowPeimOnS3Boot))
+          || (HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME && PcdGetBool (PcdShadowPeimOnBoot))) {
+        OldCoreData->ShadowedPeiCore = ShadowPeiCore (OldCoreData);
+      }
+      
+      //
+      // PEI Core has now been shadowed to memory.  Restart PEI Core in memory.
+      //
+      OldCoreData->ShadowedPeiCore (SecCoreData, PpiList, OldCoreData);
+      
+      //
+      // Should never reach here.
+      //
+      ASSERT (FALSE);
+      CpuDeadLoop();
+    }
+
+    //
+    // Memory is available to the PEI Core and the PEI Core has been shadowed to memory.
+    //
+    CopyMem (&NewSecCoreData, SecCoreDataPtr, sizeof (NewSecCoreData));
+    SecCoreData = &NewSecCoreData;
+
+    CopyMem (&PrivateData, OldCoreData, sizeof (PrivateData));
+
+    CpuIo = (VOID*)PrivateData.ServiceTableShadow.CpuIo;
+    PciCfg = (VOID*)PrivateData.ServiceTableShadow.PciCfg;
+    
+    CopyMem (&PrivateData.ServiceTableShadow, &gPs, sizeof (gPs));
+    
+    PrivateData.ServiceTableShadow.CpuIo  = CpuIo;
+    PrivateData.ServiceTableShadow.PciCfg = PciCfg;
+  }
+  
+  //
+  // Cache a pointer to the PEI Services Table that is either in temporary memory or permanent memory
+  //
+  PrivateData.Ps = &PrivateData.ServiceTableShadow;
+
+  //
+  // Initialize libraries that the PEI Core is linked against
+  //
+  ProcessLibraryConstructorList (NULL, (CONST EFI_PEI_SERVICES **)&PrivateData.Ps);
+
+  //
+  // Save PeiServicePointer so that it can be retrieved anywhere.
+  //
+  SetPeiServicesTablePointer ((CONST EFI_PEI_SERVICES **)&PrivateData.Ps);
+
+  //
+  // Initialize PEI Core Services
+  //
+  InitializeMemoryServices   (&PrivateData,    SecCoreData, OldCoreData);
+  if (OldCoreData == NULL) {
+    //
+    // Initialize PEI Core Private Data Buffer
+    //
+    PrivateData.PpiData.PpiListPtrs  = AllocateZeroPool (sizeof (PEI_PPI_LIST_POINTERS) * PcdGet32 (PcdPeiCoreMaxPpiSupported));
+    ASSERT (PrivateData.PpiData.PpiListPtrs != NULL);
+    PrivateData.Fv                   = AllocateZeroPool (sizeof (PEI_CORE_FV_HANDLE) * PcdGet32 (PcdPeiCoreMaxFvSupported));
+    ASSERT (PrivateData.Fv != NULL);
+    PrivateData.Fv[0].PeimState      = AllocateZeroPool (sizeof (UINT8) * PcdGet32 (PcdPeiCoreMaxPeimPerFv) * PcdGet32 (PcdPeiCoreMaxFvSupported));
+    ASSERT (PrivateData.Fv[0].PeimState != NULL);
+    PrivateData.Fv[0].FvFileHandles  = AllocateZeroPool (sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv) * PcdGet32 (PcdPeiCoreMaxFvSupported));
+    ASSERT (PrivateData.Fv[0].FvFileHandles != NULL);
+    for (Index = 1; Index < PcdGet32 (PcdPeiCoreMaxFvSupported); Index ++) {
+      PrivateData.Fv[Index].PeimState     = PrivateData.Fv[Index - 1].PeimState + PcdGet32 (PcdPeiCoreMaxPeimPerFv);
+      PrivateData.Fv[Index].FvFileHandles = PrivateData.Fv[Index - 1].FvFileHandles + PcdGet32 (PcdPeiCoreMaxPeimPerFv);
+    }
+    PrivateData.UnknownFvInfo        = AllocateZeroPool (sizeof (PEI_CORE_UNKNOW_FORMAT_FV_INFO) * PcdGet32 (PcdPeiCoreMaxFvSupported));
+    ASSERT (PrivateData.UnknownFvInfo != NULL);
+    PrivateData.CurrentFvFileHandles = AllocateZeroPool (sizeof (EFI_PEI_FILE_HANDLE) * PcdGet32 (PcdPeiCoreMaxPeimPerFv));
+    ASSERT (PrivateData.CurrentFvFileHandles != NULL);
+    PrivateData.FileGuid             = AllocatePool (sizeof (EFI_GUID) * PcdGet32 (PcdPeiCoreMaxPeimPerFv));
+    ASSERT (PrivateData.FileGuid != NULL);
+    PrivateData.FileHandles          = AllocatePool (sizeof (EFI_PEI_FILE_HANDLE) * (PcdGet32 (PcdPeiCoreMaxPeimPerFv) + 1));
+    ASSERT (PrivateData.FileHandles != NULL);
+  }
+  InitializePpiServices      (&PrivateData,    OldCoreData);
+  
+  //
+  // Update performance measurements 
+  //
+  if (OldCoreData == NULL) {
+    PERF_START (NULL, "SEC", NULL, 1);
+    PERF_END   (NULL, "SEC", NULL, 0);
+
+    //
+    // If first pass, start performance measurement.
+    //
+    PERF_START (NULL,"PEI",    NULL, 0);
+    PERF_START (NULL,"PreMem", NULL, 0);
+
+  } else {
+    PERF_END   (NULL,"PreMem",  NULL, 0);
+    PERF_START (NULL,"PostMem", NULL, 0);
+  }
+
+  //
+  // Complete PEI Core Service initialization
+  //  
+  InitializeSecurityServices (&PrivateData.Ps, OldCoreData);
+  InitializeDispatcherData   (&PrivateData,    OldCoreData, SecCoreData);
+  InitializeImageServices    (&PrivateData,    OldCoreData);
+
+  //
+  // Perform PEI Core Phase specific actions
+  //  
+  if (OldCoreData == NULL) {
+    //
+    // Report Status Code EFI_SW_PC_INIT
+    //
+    REPORT_STATUS_CODE (
+      EFI_PROGRESS_CODE,
+      (EFI_SOFTWARE_PEI_CORE | EFI_SW_PC_INIT)
+      );
+      
+    //
+    // If SEC provided any PPI services to PEI, install them.
+    //
+    if (PpiList != NULL) {
+      Status = PeiServicesInstallPpi (PpiList);
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    //
+    // Try to locate Temporary RAM Done Ppi.
+    //
+    Status = PeiServicesLocatePpi (
+               &gEfiTemporaryRamDonePpiGuid,
+               0,
+               NULL,
+               (VOID**)&TemporaryRamDonePpi
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Disable the use of Temporary RAM after the transition from Temporary RAM to Permanent RAM is complete.
+      //
+      TemporaryRamDonePpi->TemporaryRamDone ();
+    }
+
+    //
+    // Alert any listeners that there is permanent memory available
+    //
+    PERF_START (NULL,"DisMem", NULL, 0);
+    Status = PeiServicesInstallPpi (&mMemoryDiscoveredPpi);
+
+    //
+    // Process the Notify list and dispatch any notifies for the Memory Discovered PPI
+    //
+    ProcessNotifyList (&PrivateData);
+
+    PERF_END (NULL,"DisMem", NULL, 0);
+  }
+
+  //
+  // Call PEIM dispatcher
+  //
+  PeiDispatcher (SecCoreData, &PrivateData);
+
+  //
+  // Check if InstallPeiMemory service was called.
+  //
+  ASSERT(PrivateData.PeiMemoryInstalled == TRUE);
+
+  //
+  // Measure PEI Core execution time.
+  //
+  PERF_END (NULL, "PostMem", NULL, 0);
+
+  //
+  // Lookup DXE IPL PPI
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiDxeIplPpiGuid,
+             0,
+             NULL,
+             (VOID **)&TempPtr.DxeIpl
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Report status code to indicate DXE IPL PPI could not be found.
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+      (EFI_SOFTWARE_PEI_CORE | EFI_SW_PEI_CORE_EC_DXEIPL_NOT_FOUND)
+      );
+    CpuDeadLoop ();
+  }
+
+  //
+  // Enter DxeIpl to load Dxe core.
+  //
+  DEBUG ((EFI_D_INFO, "DXE IPL Entry\n"));
+  Status = TempPtr.DxeIpl->Entry (
+                             TempPtr.DxeIpl,
+                             &PrivateData.Ps,
+                             PrivateData.HobList
+                             );
+  //
+  // Should never reach here.
+  //
+  ASSERT_EFI_ERROR (Status);
+  CpuDeadLoop();
+}
diff --git a/Core/MdeModulePkg/Core/Pei/Ppi/Ppi.c b/Core/MdeModulePkg/Core/Pei/Ppi/Ppi.c
new file mode 100644
index 0000000000..706e835a70
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Ppi/Ppi.c
@@ -0,0 +1,646 @@
+/** @file
+  EFI PEI Core PPI services
+  
+Copyright (c) 2006 - 2014, 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 "PeiMain.h"
+
+/**
+
+  Initialize PPI services.
+
+  @param PrivateData     Pointer to the PEI Core data.
+  @param OldCoreData     Pointer to old PEI Core data. 
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializePpiServices (
+  IN PEI_CORE_INSTANCE *PrivateData,
+  IN PEI_CORE_INSTANCE *OldCoreData
+  )
+{
+  if (OldCoreData == NULL) {
+    PrivateData->PpiData.NotifyListEnd = PcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
+    PrivateData->PpiData.DispatchListEnd = PcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
+    PrivateData->PpiData.LastDispatchedNotify = PcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
+  }
+}
+
+/**
+
+  Migrate Single PPI Pointer from the temporary memory to PEI installed memory.
+
+  @param PpiPointer      Pointer to Ppi
+  @param TempBottom      Base of old temporary memory
+  @param TempTop         Top of old temporary memory
+  @param Offset          Offset of new memory to old temporary memory.
+  @param OffsetPositive  Positive flag of Offset value. 
+
+**/
+VOID
+ConverSinglePpiPointer (
+  IN PEI_PPI_LIST_POINTERS *PpiPointer,
+  IN UINTN                 TempBottom,
+  IN UINTN                 TempTop,
+  IN UINTN                 Offset,
+  IN BOOLEAN               OffsetPositive
+  )
+{
+  if (((UINTN)PpiPointer->Raw < TempTop) &&
+      ((UINTN)PpiPointer->Raw >= TempBottom)) {
+    //
+    // Convert the pointer to the PPI descriptor from the old TempRam
+    // to the relocated physical memory.
+    //
+    if (OffsetPositive) {
+      PpiPointer->Raw = (VOID *) ((UINTN)PpiPointer->Raw + Offset);
+    } else {
+      PpiPointer->Raw = (VOID *) ((UINTN)PpiPointer->Raw - Offset);
+    }
+
+    //
+    // Only when the PEIM descriptor is in the old TempRam should it be necessary
+    // to try to convert the pointers in the PEIM descriptor
+    //
+
+    if (((UINTN)PpiPointer->Ppi->Guid < TempTop) &&
+        ((UINTN)PpiPointer->Ppi->Guid >= TempBottom)) {
+      //
+      // Convert the pointer to the GUID in the PPI or NOTIFY descriptor
+      // from the old TempRam to the relocated physical memory.
+      //
+      if (OffsetPositive) {
+        PpiPointer->Ppi->Guid = (VOID *) ((UINTN)PpiPointer->Ppi->Guid + Offset);
+      } else {
+        PpiPointer->Ppi->Guid = (VOID *) ((UINTN)PpiPointer->Ppi->Guid - Offset);
+      }
+    }
+
+    //
+    // Convert the pointer to the PPI interface structure in the PPI descriptor
+    // from the old TempRam to the relocated physical memory.
+    //
+    if ((UINTN)PpiPointer->Ppi->Ppi < TempTop &&
+        (UINTN)PpiPointer->Ppi->Ppi >= TempBottom) {
+      if (OffsetPositive) {
+        PpiPointer->Ppi->Ppi = (VOID *) ((UINTN)PpiPointer->Ppi->Ppi + Offset);
+      } else {
+        PpiPointer->Ppi->Ppi = (VOID *) ((UINTN)PpiPointer->Ppi->Ppi - Offset);
+      }
+    }
+  }
+}
+
+/**
+
+  Migrate PPI Pointers from the temporary memory stack to PEI installed memory.
+
+  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
+                         and location of temporary RAM, the stack location and the BFV location.
+  @param PrivateData     Pointer to PeiCore's private data structure.
+
+**/
+VOID
+ConvertPpiPointers (
+  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
+  IN PEI_CORE_INSTANCE           *PrivateData
+  )
+{
+  UINT8                 Index;
+  UINT8                 IndexHole;
+
+  for (Index = 0; Index < PcdGet32 (PcdPeiCoreMaxPpiSupported); Index++) {
+    if (Index < PrivateData->PpiData.PpiListEnd || Index > PrivateData->PpiData.NotifyListEnd) {
+      //
+      // Convert PPI pointer in old Heap
+      //
+      ConverSinglePpiPointer (
+        &PrivateData->PpiData.PpiListPtrs[Index],
+        (UINTN)SecCoreData->PeiTemporaryRamBase,
+        (UINTN)SecCoreData->PeiTemporaryRamBase + SecCoreData->PeiTemporaryRamSize,
+        PrivateData->HeapOffset,
+        PrivateData->HeapOffsetPositive
+        );
+        
+      //
+      // Convert PPI pointer in old Stack
+      //
+      ConverSinglePpiPointer (
+        &PrivateData->PpiData.PpiListPtrs[Index],
+        (UINTN)SecCoreData->StackBase,
+        (UINTN)SecCoreData->StackBase + SecCoreData->StackSize,
+        PrivateData->StackOffset,
+        PrivateData->StackOffsetPositive
+        );
+        
+      //
+      // Convert PPI pointer in old TempRam Hole
+      //
+      for (IndexHole = 0; IndexHole < HOLE_MAX_NUMBER; IndexHole ++) {
+        if (PrivateData->HoleData[IndexHole].Size == 0) {
+          continue;
+        }
+        
+        ConverSinglePpiPointer (
+          &PrivateData->PpiData.PpiListPtrs[Index],
+          (UINTN)PrivateData->HoleData[IndexHole].Base,
+          (UINTN)PrivateData->HoleData[IndexHole].Base + PrivateData->HoleData[IndexHole].Size,
+          PrivateData->HoleData[IndexHole].Offset,
+          PrivateData->HoleData[IndexHole].OffsetPositive
+          );
+      }
+    }
+  }
+}
+
+/**
+
+  This function installs an interface in the PEI PPI database by GUID. 
+  The purpose of the service is to publish an interface that other parties
+  can use to call additional PEIMs.
+
+  @param PeiServices                An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param PpiList                    Pointer to a list of PEI PPI Descriptors.
+
+  @retval EFI_SUCCESS              if all PPIs in PpiList are successfully installed.
+  @retval EFI_INVALID_PARAMETER    if PpiList is NULL pointer
+                                   if any PPI in PpiList is not valid
+  @retval EFI_OUT_OF_RESOURCES     if there is no more memory resource to install PPI
+
+**/
+EFI_STATUS
+EFIAPI
+PeiInstallPpi (
+  IN CONST EFI_PEI_SERVICES        **PeiServices,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *PpiList
+  )
+{
+  PEI_CORE_INSTANCE *PrivateData;
+  INTN              Index;
+  INTN              LastCallbackInstall;
+
+
+  if (PpiList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+
+  Index = PrivateData->PpiData.PpiListEnd;
+  LastCallbackInstall = Index;
+
+  //
+  // This is loop installs all PPI descriptors in the PpiList.  It is terminated
+  // by the EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST being set in the last
+  // EFI_PEI_PPI_DESCRIPTOR in the list.
+  //
+
+  for (;;) {
+    //
+    // Since PpiData is used for NotifyList and PpiList, max resource
+    // is reached if the Install reaches the NotifyList
+    // PcdPeiCoreMaxPpiSupported can be set to a larger value in DSC to satisfy more PPI requirement.
+    //
+    if (Index == PrivateData->PpiData.NotifyListEnd + 1) {
+      return  EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Check if it is a valid PPI.
+    // If not, rollback list to exclude all in this list.
+    // Try to indicate which item failed.
+    //
+    if ((PpiList->Flags & EFI_PEI_PPI_DESCRIPTOR_PPI) == 0) {
+      PrivateData->PpiData.PpiListEnd = LastCallbackInstall;
+      DEBUG((EFI_D_ERROR, "ERROR -> InstallPpi: %g %p\n", PpiList->Guid, PpiList->Ppi));
+      return  EFI_INVALID_PARAMETER;
+    }
+
+    DEBUG((EFI_D_INFO, "Install PPI: %g\n", PpiList->Guid));
+    PrivateData->PpiData.PpiListPtrs[Index].Ppi = (EFI_PEI_PPI_DESCRIPTOR*) PpiList;
+    PrivateData->PpiData.PpiListEnd++;
+
+    //
+    // Continue until the end of the PPI List.
+    //
+    if ((PpiList->Flags & EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST) ==
+        EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST) {
+      break;
+    }
+    PpiList++;
+    Index++;
+  }
+
+  //
+  // Dispatch any callback level notifies for newly installed PPIs.
+  //
+  DispatchNotify (
+    PrivateData,
+    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
+    LastCallbackInstall,
+    PrivateData->PpiData.PpiListEnd,
+    PrivateData->PpiData.DispatchListEnd,
+    PrivateData->PpiData.NotifyListEnd
+    );
+
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  This function reinstalls an interface in the PEI PPI database by GUID. 
+  The purpose of the service is to publish an interface that other parties can 
+  use to replace an interface of the same name in the protocol database with a 
+  different interface.
+
+  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param OldPpi                 Pointer to the old PEI PPI Descriptors.
+  @param NewPpi                 Pointer to the new PEI PPI Descriptors.
+
+  @retval EFI_SUCCESS           if the operation was successful
+  @retval EFI_INVALID_PARAMETER if OldPpi or NewPpi is NULL
+  @retval EFI_INVALID_PARAMETER if NewPpi is not valid
+  @retval EFI_NOT_FOUND         if the PPI was not in the database
+
+**/
+EFI_STATUS
+EFIAPI
+PeiReInstallPpi (
+  IN CONST EFI_PEI_SERVICES        **PeiServices,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *OldPpi,
+  IN CONST EFI_PEI_PPI_DESCRIPTOR  *NewPpi
+  )
+{
+  PEI_CORE_INSTANCE   *PrivateData;
+  INTN                Index;
+
+
+  if ((OldPpi == NULL) || (NewPpi == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((NewPpi->Flags & EFI_PEI_PPI_DESCRIPTOR_PPI) == 0) {
+    return  EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+
+  //
+  // Find the old PPI instance in the database.  If we can not find it,
+  // return the EFI_NOT_FOUND error.
+  //
+  for (Index = 0; Index < PrivateData->PpiData.PpiListEnd; Index++) {
+    if (OldPpi == PrivateData->PpiData.PpiListPtrs[Index].Ppi) {
+      break;
+    }
+  }
+  if (Index == PrivateData->PpiData.PpiListEnd) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Remove the old PPI from the database, add the new one.
+  //
+  DEBUG((EFI_D_INFO, "Reinstall PPI: %g\n", NewPpi->Guid));
+  ASSERT (Index < (INTN)(PcdGet32 (PcdPeiCoreMaxPpiSupported)));
+  PrivateData->PpiData.PpiListPtrs[Index].Ppi = (EFI_PEI_PPI_DESCRIPTOR *) NewPpi;
+
+  //
+  // Dispatch any callback level notifies for the newly installed PPI.
+  //
+  DispatchNotify (
+    PrivateData,
+    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
+    Index,
+    Index+1,
+    PrivateData->PpiData.DispatchListEnd,
+    PrivateData->PpiData.NotifyListEnd
+    );
+
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Locate a given named PPI.
+
+
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param Guid               Pointer to GUID of the PPI.
+  @param Instance           Instance Number to discover.
+  @param PpiDescriptor      Pointer to reference the found descriptor. If not NULL,
+                            returns a pointer to the descriptor (includes flags, etc)
+  @param Ppi                Pointer to reference the found PPI
+
+  @retval EFI_SUCCESS   if the PPI is in the database
+  @retval EFI_NOT_FOUND if the PPI is not in the database
+
+**/
+EFI_STATUS
+EFIAPI
+PeiLocatePpi (
+  IN CONST EFI_PEI_SERVICES        **PeiServices,
+  IN CONST EFI_GUID                *Guid,
+  IN UINTN                         Instance,
+  IN OUT EFI_PEI_PPI_DESCRIPTOR    **PpiDescriptor,
+  IN OUT VOID                      **Ppi
+  )
+{
+  PEI_CORE_INSTANCE   *PrivateData;
+  INTN                Index;
+  EFI_GUID            *CheckGuid;
+  EFI_PEI_PPI_DESCRIPTOR  *TempPtr;
+
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+
+  //
+  // Search the data base for the matching instance of the GUIDed PPI.
+  //
+  for (Index = 0; Index < PrivateData->PpiData.PpiListEnd; Index++) {
+    TempPtr = PrivateData->PpiData.PpiListPtrs[Index].Ppi;
+    CheckGuid = TempPtr->Guid;
+
+    //
+    // Don't use CompareGuid function here for performance reasons.
+    // Instead we compare the GUID as INT32 at a time and branch
+    // on the first failed comparison.
+    //
+    if ((((INT32 *)Guid)[0] == ((INT32 *)CheckGuid)[0]) &&
+        (((INT32 *)Guid)[1] == ((INT32 *)CheckGuid)[1]) &&
+        (((INT32 *)Guid)[2] == ((INT32 *)CheckGuid)[2]) &&
+        (((INT32 *)Guid)[3] == ((INT32 *)CheckGuid)[3])) {
+      if (Instance == 0) {
+
+        if (PpiDescriptor != NULL) {
+          *PpiDescriptor = TempPtr;
+        }
+
+        if (Ppi != NULL) {
+          *Ppi = TempPtr->Ppi;
+        }
+
+
+        return EFI_SUCCESS;
+      }
+      Instance--;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+
+  This function installs a notification service to be called back when a given 
+  interface is installed or reinstalled. The purpose of the service is to publish 
+  an interface that other parties can use to call additional PPIs that may materialize later.
+
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param NotifyList         Pointer to list of Descriptors to notify upon.
+
+  @retval EFI_SUCCESS           if successful
+  @retval EFI_OUT_OF_RESOURCES  if no space in the database
+  @retval EFI_INVALID_PARAMETER if not a good decriptor
+
+**/
+EFI_STATUS
+EFIAPI
+PeiNotifyPpi (
+  IN CONST EFI_PEI_SERVICES           **PeiServices,
+  IN CONST EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyList
+  )
+{
+  PEI_CORE_INSTANCE                *PrivateData;
+  INTN                             Index;
+  INTN                             NotifyIndex;
+  INTN                             LastCallbackNotify;
+  EFI_PEI_NOTIFY_DESCRIPTOR        *NotifyPtr;
+  UINTN                            NotifyDispatchCount;
+
+
+  NotifyDispatchCount = 0;
+
+  if (NotifyList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);
+
+  Index = PrivateData->PpiData.NotifyListEnd;
+  LastCallbackNotify = Index;
+
+  //
+  // This is loop installs all Notify descriptors in the NotifyList.  It is
+  // terminated by the EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST being set in the last
+  // EFI_PEI_NOTIFY_DESCRIPTOR in the list.
+  //
+
+  for (;;) {
+    //
+    // Since PpiData is used for NotifyList and InstallList, max resource
+    // is reached if the Install reaches the PpiList
+    // PcdPeiCoreMaxPpiSupported can be set to a larger value in DSC to satisfy more Notify PPIs requirement.
+    //
+    if (Index == PrivateData->PpiData.PpiListEnd - 1) {
+      return  EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // If some of the PPI data is invalid restore original Notify PPI database value
+    //
+    if ((NotifyList->Flags & EFI_PEI_PPI_DESCRIPTOR_NOTIFY_TYPES) == 0) {
+        PrivateData->PpiData.NotifyListEnd = LastCallbackNotify;
+        DEBUG((EFI_D_ERROR, "ERROR -> InstallNotify: %g %p\n", NotifyList->Guid, NotifyList->Notify));
+      return  EFI_INVALID_PARAMETER;
+    }
+
+    if ((NotifyList->Flags & EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH) != 0) {
+      NotifyDispatchCount ++;
+    }
+
+    PrivateData->PpiData.PpiListPtrs[Index].Notify = (EFI_PEI_NOTIFY_DESCRIPTOR *) NotifyList;
+
+    PrivateData->PpiData.NotifyListEnd--;
+    DEBUG((EFI_D_INFO, "Register PPI Notify: %g\n", NotifyList->Guid));
+    if ((NotifyList->Flags & EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST) ==
+        EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST) {
+      break;
+    }
+    //
+    // Go the next descriptor. Remember the NotifyList moves down.
+    //
+    NotifyList++;
+    Index--;
+  }
+
+  //
+  // If there is Dispatch Notify PPI installed put them on the bottom
+  //
+  if (NotifyDispatchCount > 0) {
+    for (NotifyIndex = LastCallbackNotify; NotifyIndex > PrivateData->PpiData.NotifyListEnd; NotifyIndex--) {
+      if ((PrivateData->PpiData.PpiListPtrs[NotifyIndex].Notify->Flags & EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH) != 0) {
+        NotifyPtr = PrivateData->PpiData.PpiListPtrs[NotifyIndex].Notify;
+
+        for (Index = NotifyIndex; Index < PrivateData->PpiData.DispatchListEnd; Index++){
+          PrivateData->PpiData.PpiListPtrs[Index].Notify = PrivateData->PpiData.PpiListPtrs[Index + 1].Notify;
+        }
+        PrivateData->PpiData.PpiListPtrs[Index].Notify = NotifyPtr;
+        PrivateData->PpiData.DispatchListEnd--;
+      }
+    }
+
+    LastCallbackNotify -= NotifyDispatchCount;
+  }
+
+  //
+  // Dispatch any callback level notifies for all previously installed PPIs.
+  //
+  DispatchNotify (
+    PrivateData,
+    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
+    0,
+    PrivateData->PpiData.PpiListEnd,
+    LastCallbackNotify,
+    PrivateData->PpiData.NotifyListEnd
+    );
+
+  return  EFI_SUCCESS;
+}
+
+
+/**
+
+  Process the Notify List at dispatch level.
+
+  @param PrivateData  PeiCore's private data structure.
+
+**/
+VOID
+ProcessNotifyList (
+  IN PEI_CORE_INSTANCE  *PrivateData
+  )
+{
+  INTN                    TempValue;
+
+  while (TRUE) {
+    //
+    // Check if the PEIM that was just dispatched resulted in any
+    // Notifies getting installed.  If so, go process any dispatch
+    // level Notifies that match the previouly installed PPIs.
+    // Use "while" instead of "if" since DispatchNotify can modify
+    // DispatchListEnd (with NotifyPpi) so we have to iterate until the same.
+    //
+    while (PrivateData->PpiData.LastDispatchedNotify != PrivateData->PpiData.DispatchListEnd) {
+      TempValue = PrivateData->PpiData.DispatchListEnd;
+      DispatchNotify (
+        PrivateData,
+        EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH,
+        0,
+        PrivateData->PpiData.LastDispatchedInstall,
+        PrivateData->PpiData.LastDispatchedNotify,
+        PrivateData->PpiData.DispatchListEnd
+        );
+      PrivateData->PpiData.LastDispatchedNotify = TempValue;
+    }
+
+
+    //
+    // Check if the PEIM that was just dispatched resulted in any
+    // PPIs getting installed.  If so, go process any dispatch
+    // level Notifies that match the installed PPIs.
+    // Use "while" instead of "if" since DispatchNotify can modify
+    // PpiListEnd (with InstallPpi) so we have to iterate until the same.
+    //
+    while (PrivateData->PpiData.LastDispatchedInstall != PrivateData->PpiData.PpiListEnd) {
+      TempValue = PrivateData->PpiData.PpiListEnd;
+      DispatchNotify (
+        PrivateData,
+        EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH,
+        PrivateData->PpiData.LastDispatchedInstall,
+        PrivateData->PpiData.PpiListEnd,
+        PcdGet32 (PcdPeiCoreMaxPpiSupported)-1,
+        PrivateData->PpiData.DispatchListEnd
+        );
+      PrivateData->PpiData.LastDispatchedInstall = TempValue;
+    }
+
+    if (PrivateData->PpiData.LastDispatchedNotify == PrivateData->PpiData.DispatchListEnd) {
+      break;
+    }
+  }
+  return;
+}
+
+/**
+
+  Dispatch notifications.
+
+  @param PrivateData        PeiCore's private data structure
+  @param NotifyType         Type of notify to fire.
+  @param InstallStartIndex  Install Beginning index.
+  @param InstallStopIndex   Install Ending index.
+  @param NotifyStartIndex   Notify Beginning index.
+  @param NotifyStopIndex    Notify Ending index.
+
+**/
+VOID
+DispatchNotify (
+  IN PEI_CORE_INSTANCE  *PrivateData,
+  IN UINTN               NotifyType,
+  IN INTN                InstallStartIndex,
+  IN INTN                InstallStopIndex,
+  IN INTN                NotifyStartIndex,
+  IN INTN                NotifyStopIndex
+  )
+{
+  INTN                   Index1;
+  INTN                   Index2;
+  EFI_GUID                *SearchGuid;
+  EFI_GUID                *CheckGuid;
+  EFI_PEI_NOTIFY_DESCRIPTOR   *NotifyDescriptor;
+
+  //
+  // Remember that Installs moves up and Notifies moves down.
+  //
+  for (Index1 = NotifyStartIndex; Index1 > NotifyStopIndex; Index1--) {
+    NotifyDescriptor = PrivateData->PpiData.PpiListPtrs[Index1].Notify;
+
+    CheckGuid = NotifyDescriptor->Guid;
+
+    for (Index2 = InstallStartIndex; Index2 < InstallStopIndex; Index2++) {
+      SearchGuid = PrivateData->PpiData.PpiListPtrs[Index2].Ppi->Guid;
+      //
+      // Don't use CompareGuid function here for performance reasons.
+      // Instead we compare the GUID as INT32 at a time and branch
+      // on the first failed comparison.
+      //
+      if ((((INT32 *)SearchGuid)[0] == ((INT32 *)CheckGuid)[0]) &&
+          (((INT32 *)SearchGuid)[1] == ((INT32 *)CheckGuid)[1]) &&
+          (((INT32 *)SearchGuid)[2] == ((INT32 *)CheckGuid)[2]) &&
+          (((INT32 *)SearchGuid)[3] == ((INT32 *)CheckGuid)[3])) {
+        DEBUG ((EFI_D_INFO, "Notify: PPI Guid: %g, Peim notify entry point: %p\n",
+          SearchGuid,
+          NotifyDescriptor->Notify
+          ));
+        NotifyDescriptor->Notify (
+                            (EFI_PEI_SERVICES **) GetPeiServicesTablePointer (),
+                            NotifyDescriptor,
+                            (PrivateData->PpiData.PpiListPtrs[Index2].Ppi)->Ppi
+                            );
+      }
+    }
+  }
+}
+
diff --git a/Core/MdeModulePkg/Core/Pei/Reset/Reset.c b/Core/MdeModulePkg/Core/Pei/Reset/Reset.c
new file mode 100644
index 0000000000..2e9ac8215b
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Reset/Reset.c
@@ -0,0 +1,108 @@
+/** @file
+  Pei Core Reset System Support
+  
+Copyright (c) 2006, 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 "PeiMain.h"
+
+/**
+
+  Core version of the Reset System
+
+
+  @param PeiServices                An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+
+  @retval EFI_NOT_AVAILABLE_YET     PPI not available yet.
+  @retval EFI_DEVICE_ERROR          Did not reset system.
+                                    Otherwise, resets the system.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiResetSystem (
+  IN CONST EFI_PEI_SERVICES         **PeiServices
+  )
+{
+  EFI_STATUS        Status;
+  EFI_PEI_RESET_PPI *ResetPpi;
+
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiResetPpiGuid,         
+             0,                         
+             NULL,                      
+             (VOID **)&ResetPpi                  
+             );
+
+  //
+  // LocatePpi returns EFI_NOT_FOUND on error
+  //
+  if (!EFI_ERROR (Status)) {
+    return ResetPpi->ResetSystem (PeiServices);
+  } 
+  //
+  // Report Status Code that Reset PPI is not available
+  //
+  REPORT_STATUS_CODE (
+    EFI_ERROR_CODE | EFI_ERROR_MINOR,
+    (EFI_SOFTWARE_PEI_CORE | EFI_SW_PS_EC_RESET_NOT_AVAILABLE)
+    );
+  return  EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  Resets the entire platform.
+
+  @param[in] ResetType      The type of reset to perform.
+  @param[in] ResetStatus    The status code for the reset.
+  @param[in] DataSize       The size, in bytes, of WatchdogData.
+  @param[in] ResetData      For a ResetType of EfiResetCold, EfiResetWarm, or EfiResetShutdown
+                            the data buffer starts with a Null-terminated string, optionally
+                            followed by additional binary data. The string is a description
+                            that the caller may use to further indicate the reason for the
+                            system reset. ResetData is only valid if ResetStatus is something
+                            other than EFI_SUCCESS unless the ResetType is EfiResetPlatformSpecific
+                            where a minimum amount of ResetData is always required.
+
+**/
+VOID
+EFIAPI
+PeiResetSystem2 (
+  IN EFI_RESET_TYPE     ResetType,
+  IN EFI_STATUS         ResetStatus,
+  IN UINTN              DataSize,
+  IN VOID               *ResetData OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PEI_RESET2_PPI    *Reset2Ppi;
+
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiReset2PpiGuid,
+             0,
+             NULL,
+             (VOID **)&Reset2Ppi
+             );
+
+  if (!EFI_ERROR (Status)) {
+    Reset2Ppi->ResetSystem (ResetType, ResetStatus, DataSize, ResetData);
+    return;
+  }
+
+  //
+  // Report Status Code that Reset2 PPI is not available.
+  //
+  REPORT_STATUS_CODE (
+    EFI_ERROR_CODE | EFI_ERROR_MINOR,
+    (EFI_SOFTWARE_PEI_CORE | EFI_SW_PS_EC_RESET_NOT_AVAILABLE)
+    );
+}
+
diff --git a/Core/MdeModulePkg/Core/Pei/Security/Security.c b/Core/MdeModulePkg/Core/Pei/Security/Security.c
new file mode 100644
index 0000000000..763126057d
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/Security/Security.c
@@ -0,0 +1,151 @@
+/** @file
+  EFI PEI Core Security services
+  
+Copyright (c) 2006 - 2014, 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 "PeiMain.h"
+
+
+EFI_PEI_NOTIFY_DESCRIPTOR mNotifyList = {
+   EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+   &gEfiPeiSecurity2PpiGuid,
+   SecurityPpiNotifyCallback
+};
+
+/**
+  Initialize the security services.
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param OldCoreData     Pointer to the old core data.
+                         NULL if being run in non-permament memory mode.
+
+**/
+VOID
+InitializeSecurityServices (
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_CORE_INSTANCE *OldCoreData
+  )
+{
+  if (OldCoreData == NULL) {
+    PeiServicesNotifyPpi (&mNotifyList);
+  }
+  return;
+}
+
+/**
+
+  Provide a callback for when the security PPI is installed.
+  This routine will cache installed security PPI into PeiCore's private data.
+  
+  @param PeiServices        An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param NotifyDescriptor   The descriptor for the notification event.
+  @param Ppi                Pointer to the PPI in question.
+
+  @return Always success
+
+**/
+EFI_STATUS
+EFIAPI
+SecurityPpiNotifyCallback (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyDescriptor,
+  IN VOID                       *Ppi
+  )
+{
+  PEI_CORE_INSTANCE                       *PrivateData;
+
+  //
+  // Get PEI Core private data
+  //
+  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS (PeiServices);
+  
+  //
+  // If there isn't a security PPI installed, use the one from notification
+  //
+  if (PrivateData->PrivateSecurityPpi == NULL) {
+    PrivateData->PrivateSecurityPpi = (EFI_PEI_SECURITY2_PPI *)Ppi;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Provide a callout to the security verification service.
+
+  @param PrivateData     PeiCore's private data structure
+  @param VolumeHandle    Handle of FV
+  @param FileHandle      Handle of PEIM's ffs
+  @param AuthenticationStatus Authentication status
+
+  @retval EFI_SUCCESS              Image is OK
+  @retval EFI_SECURITY_VIOLATION   Image is illegal
+  @retval EFI_NOT_FOUND            If security PPI is not installed.
+**/
+EFI_STATUS
+VerifyPeim (
+  IN PEI_CORE_INSTANCE      *PrivateData,
+  IN EFI_PEI_FV_HANDLE      VolumeHandle,
+  IN EFI_PEI_FILE_HANDLE    FileHandle,
+  IN UINT32                 AuthenticationStatus
+  )
+{
+  EFI_STATUS                      Status;
+  BOOLEAN                         DeferExection;
+
+  Status = EFI_NOT_FOUND;
+  if (PrivateData->PrivateSecurityPpi == NULL) {
+    //
+    // Check AuthenticationStatus first.
+    //
+    if ((AuthenticationStatus & EFI_AUTH_STATUS_IMAGE_SIGNED) != 0) {
+      if ((AuthenticationStatus & (EFI_AUTH_STATUS_TEST_FAILED | EFI_AUTH_STATUS_NOT_TESTED)) != 0) {
+        Status = EFI_SECURITY_VIOLATION;
+      }
+    }
+  } else {
+    //
+    // Check to see if the image is OK
+    //
+    Status = PrivateData->PrivateSecurityPpi->AuthenticationState (
+                                                (CONST EFI_PEI_SERVICES **) &PrivateData->Ps,
+                                                PrivateData->PrivateSecurityPpi,
+                                                AuthenticationStatus,
+                                                VolumeHandle,
+                                                FileHandle,
+                                                &DeferExection
+                                                );
+    if (DeferExection) {
+      Status = EFI_SECURITY_VIOLATION;
+    }
+  }
+  return Status;
+}
+
+
+/**
+  Verify a Firmware volume.
+
+  @param CurrentFvAddress   Pointer to the current Firmware Volume under consideration
+
+  @retval EFI_SUCCESS       Firmware Volume is legal
+
+**/
+EFI_STATUS
+VerifyFv (
+  IN EFI_FIRMWARE_VOLUME_HEADER  *CurrentFvAddress
+  )
+{
+  //
+  // Right now just pass the test.  Future can authenticate and/or check the
+  // FV-header or other metric for goodness of binary.
+  //
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/Pei/StatusCode/StatusCode.c b/Core/MdeModulePkg/Core/Pei/StatusCode/StatusCode.c
new file mode 100644
index 0000000000..a7fb524ff4
--- /dev/null
+++ b/Core/MdeModulePkg/Core/Pei/StatusCode/StatusCode.c
@@ -0,0 +1,74 @@
+/** @file
+  Pei Core Status Code Support
+  
+Copyright (c) 2006, 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 "PeiMain.h"
+
+/**
+
+  Core version of the Status Code reporter
+
+
+  @param PeiServices     An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param CodeType        Type of Status Code.
+  @param Value           Value to output for Status Code.
+  @param Instance        Instance Number of this status code.
+  @param CallerId        ID of the caller of this status code.
+  @param Data            Optional data associated with this status code.
+
+  @retval EFI_SUCCESS             if status code is successfully reported
+  @retval EFI_NOT_AVAILABLE_YET   if StatusCodePpi has not been installed
+
+**/
+EFI_STATUS
+EFIAPI
+PeiReportStatusCode (
+  IN CONST EFI_PEI_SERVICES         **PeiServices,
+  IN EFI_STATUS_CODE_TYPE           CodeType,
+  IN EFI_STATUS_CODE_VALUE          Value, 
+  IN UINT32                         Instance,
+  IN CONST EFI_GUID                 *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+  EFI_PEI_PROGRESS_CODE_PPI *StatusCodePpi;
+
+  //
+  // Locate StatusCode Ppi.
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiStatusCodePpiGuid,         
+             0,                         
+             NULL,                      
+             (VOID **)&StatusCodePpi                  
+             );
+
+  if (!EFI_ERROR (Status)) {
+    Status = StatusCodePpi->ReportStatusCode (
+                            PeiServices,
+                            CodeType,
+                            Value,
+                            Instance,
+                            CallerId,
+                            Data
+                            );
+ 
+   return Status;   
+  } 
+  
+  return  EFI_NOT_AVAILABLE_YET; 
+}
+
+
+
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Dependency.c b/Core/MdeModulePkg/Core/PiSmmCore/Dependency.c
new file mode 100644
index 0000000000..44f693ac83
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Dependency.c
@@ -0,0 +1,388 @@
+/** @file
+  SMM Driver Dispatcher Dependency Evaluator
+
+  This routine evaluates a dependency expression (DEPENDENCY_EXPRESSION) to determine
+  if a driver can be scheduled for execution.  The criteria for
+  schedulability is that the dependency expression is satisfied.
+
+  Copyright (c) 2009 - 2010, 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 "PiSmmCore.h"
+
+///
+/// EFI_DEP_REPLACE_TRUE - Used to dynamically patch the dependecy expression
+///                        to save time.  A EFI_DEP_PUSH is evauated one an
+///                        replaced with EFI_DEP_REPLACE_TRUE. If PI spec's Vol 2
+///                        Driver Execution Environment Core Interface use 0xff
+///                        as new DEPEX opcode. EFI_DEP_REPLACE_TRUE should be
+///                        defined to a new value that is not conflicting with PI spec.
+///
+#define EFI_DEP_REPLACE_TRUE  0xff
+
+///
+/// Define the initial size of the dependency expression evaluation stack
+///
+#define DEPEX_STACK_SIZE_INCREMENT  0x1000
+
+//
+// Global stack used to evaluate dependency expressions
+//
+BOOLEAN  *mDepexEvaluationStack        = NULL;
+BOOLEAN  *mDepexEvaluationStackEnd     = NULL;
+BOOLEAN  *mDepexEvaluationStackPointer = NULL;
+
+/**
+  Grow size of the Depex stack
+
+  @retval EFI_SUCCESS           Stack successfully growed.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+GrowDepexStack (
+  VOID
+  )
+{
+  BOOLEAN     *NewStack;
+  UINTN       Size;
+
+  Size = DEPEX_STACK_SIZE_INCREMENT;
+  if (mDepexEvaluationStack != NULL) {
+    Size = Size + (mDepexEvaluationStackEnd - mDepexEvaluationStack);
+  }
+
+  NewStack = AllocatePool (Size * sizeof (BOOLEAN));
+  if (NewStack == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (mDepexEvaluationStack != NULL) {
+    //
+    // Copy to Old Stack to the New Stack
+    //
+    CopyMem (
+      NewStack,
+      mDepexEvaluationStack,
+      (mDepexEvaluationStackEnd - mDepexEvaluationStack) * sizeof (BOOLEAN)
+      );
+
+    //
+    // Free The Old Stack
+    //
+    FreePool (mDepexEvaluationStack);
+  }
+
+  //
+  // Make the Stack pointer point to the old data in the new stack
+  //
+  mDepexEvaluationStackPointer = NewStack + (mDepexEvaluationStackPointer - mDepexEvaluationStack);
+  mDepexEvaluationStack        = NewStack;
+  mDepexEvaluationStackEnd     = NewStack + Size;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Push an element onto the Boolean Stack.
+
+  @param  Value                 BOOLEAN to push.
+
+  @retval EFI_SUCCESS           The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushBool (
+  IN BOOLEAN  Value
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check for a stack overflow condition
+  //
+  if (mDepexEvaluationStackPointer == mDepexEvaluationStackEnd) {
+    //
+    // Grow the stack
+    //
+    Status = GrowDepexStack ();
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Push the item onto the stack
+  //
+  *mDepexEvaluationStackPointer = Value;
+  mDepexEvaluationStackPointer++;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Pop an element from the Boolean stack.
+
+  @param  Value                 BOOLEAN to pop.
+
+  @retval EFI_SUCCESS           The value was popped onto the stack.
+  @retval EFI_ACCESS_DENIED     The pop operation underflowed the stack.
+
+**/
+EFI_STATUS
+PopBool (
+  OUT BOOLEAN  *Value
+  )
+{
+  //
+  // Check for a stack underflow condition
+  //
+  if (mDepexEvaluationStackPointer == mDepexEvaluationStack) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Pop the item off the stack
+  //
+  mDepexEvaluationStackPointer--;
+  *Value = *mDepexEvaluationStackPointer;
+  return EFI_SUCCESS;
+}
+
+/**
+  This is the POSTFIX version of the dependency evaluator.  This code does
+  not need to handle Before or After, as it is not valid to call this
+  routine in this case. POSTFIX means all the math is done on top of the stack.
+
+  @param  DriverEntry           DriverEntry element to update.
+
+  @retval TRUE                  If driver is ready to run.
+  @retval FALSE                 If driver is not ready to run or some fatal error
+                                was found.
+
+**/
+BOOLEAN
+SmmIsSchedulable (
+  IN  EFI_SMM_DRIVER_ENTRY   *DriverEntry
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       *Iterator;
+  BOOLEAN     Operator;
+  BOOLEAN     Operator2;
+  EFI_GUID    DriverGuid;
+  VOID        *Interface;
+
+  Operator = FALSE;
+  Operator2 = FALSE;
+
+  if (DriverEntry->After || DriverEntry->Before) {
+    //
+    // If Before or After Depex skip as SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter ()
+    // processes them.
+    //
+    return FALSE;
+  }
+
+  DEBUG ((DEBUG_DISPATCH, "Evaluate SMM DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+  
+  if (DriverEntry->Depex == NULL) {
+    //
+    // A NULL Depex means that the SMM driver is not built correctly.  
+    // All SMM drivers must have a valid depex expressiion.
+    //
+    DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Depex is empty)\n"));
+    ASSERT (FALSE);
+    return FALSE;
+  }
+
+  //
+  // Clean out memory leaks in Depex Boolean stack. Leaks are only caused by
+  // incorrectly formed DEPEX expressions
+  //
+  mDepexEvaluationStackPointer = mDepexEvaluationStack;
+
+
+  Iterator = DriverEntry->Depex;
+
+  while (TRUE) {
+    //
+    // Check to see if we are attempting to fetch dependency expression instructions
+    // past the end of the dependency expression.
+    //
+    if (((UINTN)Iterator - (UINTN)DriverEntry->Depex) >= DriverEntry->DepexSize) {
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Attempt to fetch past end of depex)\n"));
+      return FALSE;
+    }
+
+    //
+    // Look at the opcode of the dependency expression instruction.
+    //
+    switch (*Iterator) {
+    case EFI_DEP_BEFORE:
+    case EFI_DEP_AFTER:
+      //
+      // For a well-formed Dependency Expression, the code should never get here.
+      // The BEFORE and AFTER are processed prior to this routine's invocation.
+      // If the code flow arrives at this point, there was a BEFORE or AFTER
+      // that were not the first opcodes.
+      //
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected BEFORE or AFTER opcode)\n"));
+      ASSERT (FALSE);
+
+    case EFI_DEP_PUSH:
+      //
+      // Push operator is followed by a GUID. Test to see if the GUID protocol
+      // is installed and push the boolean result on the stack.
+      //
+      CopyMem (&DriverGuid, Iterator + 1, sizeof (EFI_GUID));
+
+      Status = SmmLocateProtocol (&DriverGuid, NULL, &Interface);
+      if (EFI_ERROR (Status)) {
+        //
+        // For SMM Driver, it may depend on uefi protocols
+        //
+        Status = gBS->LocateProtocol (&DriverGuid, NULL, &Interface);
+      }
+
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = FALSE\n", &DriverGuid));
+        Status = PushBool (FALSE);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = TRUE\n", &DriverGuid));
+        *Iterator = EFI_DEP_REPLACE_TRUE;
+        Status = PushBool (TRUE);
+      }
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Iterator += sizeof (EFI_GUID);
+      break;
+
+    case EFI_DEP_AND:
+      DEBUG ((DEBUG_DISPATCH, "  AND\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PopBool (&Operator2);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(Operator && Operator2));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_OR:
+      DEBUG ((DEBUG_DISPATCH, "  OR\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PopBool (&Operator2);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(Operator || Operator2));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_NOT:
+      DEBUG ((DEBUG_DISPATCH, "  NOT\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Status = PushBool ((BOOLEAN)(!Operator));
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_TRUE:
+      DEBUG ((DEBUG_DISPATCH, "  TRUE\n"));
+      Status = PushBool (TRUE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_FALSE:
+      DEBUG ((DEBUG_DISPATCH, "  FALSE\n"));
+      Status = PushBool (FALSE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      break;
+
+    case EFI_DEP_END:
+      DEBUG ((DEBUG_DISPATCH, "  END\n"));
+      Status = PopBool (&Operator);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = %a\n", Operator ? "TRUE" : "FALSE"));
+      return Operator;
+
+    case EFI_DEP_REPLACE_TRUE:
+      CopyMem (&DriverGuid, Iterator + 1, sizeof (EFI_GUID));
+      DEBUG ((DEBUG_DISPATCH, "  PUSH GUID(%g) = TRUE\n", &DriverGuid));
+      Status = PushBool (TRUE);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unexpected error)\n"));
+        return FALSE;
+      }
+
+      Iterator += sizeof (EFI_GUID);
+      break;
+
+    default:
+      DEBUG ((DEBUG_DISPATCH, "  RESULT = FALSE (Unknown opcode)\n"));
+      goto Done;
+    }
+
+    //
+    // Skip over the Dependency Op Code we just processed in the switch.
+    // The math is done out of order, but it should not matter. That is
+    // we may add in the sizeof (EFI_GUID) before we account for the OP Code.
+    // This is not an issue, since we just need the correct end result. You
+    // need to be careful using Iterator in the loop as it's intermediate value
+    // may be strange.
+    //
+    Iterator++;
+  }
+
+Done:
+  return FALSE;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Dispatcher.c b/Core/MdeModulePkg/Core/PiSmmCore/Dispatcher.c
new file mode 100644
index 0000000000..1f85ae8def
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Dispatcher.c
@@ -0,0 +1,1440 @@
+/** @file
+  SMM Driver Dispatcher.
+
+  Step #1 - When a FV protocol is added to the system every driver in the FV
+            is added to the mDiscoveredList. The Before, and After Depex are
+            pre-processed as drivers are added to the mDiscoveredList. If an Apriori
+            file exists in the FV those drivers are addeded to the
+            mScheduledQueue. The mFvHandleList is used to make sure a
+            FV is only processed once.
+
+  Step #2 - Dispatch. Remove driver from the mScheduledQueue and load and
+            start it. After mScheduledQueue is drained check the
+            mDiscoveredList to see if any item has a Depex that is ready to
+            be placed on the mScheduledQueue.
+
+  Step #3 - Adding to the mScheduledQueue requires that you process Before
+            and After dependencies. This is done recursively as the call to add
+            to the mScheduledQueue checks for Before and recursively adds
+            all Befores. It then addes the item that was passed in and then
+            processess the After dependecies by recursively calling the routine.
+
+  Dispatcher Rules:
+  The rules for the dispatcher are similar to the DXE dispatcher.
+
+  The rules for DXE dispatcher are in chapter 10 of the DXE CIS. Figure 10-3
+  is the state diagram for the DXE dispatcher
+
+  Depex - Dependency Expresion.
+
+  Copyright (c) 2014, Hewlett-Packard Development Company, L.P.
+  Copyright (c) 2009 - 2014, 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 "PiSmmCore.h"
+
+//
+// SMM Dispatcher Data structures
+//
+#define KNOWN_HANDLE_SIGNATURE  SIGNATURE_32('k','n','o','w')
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Link;         // mFvHandleList
+  EFI_HANDLE      Handle;
+} KNOWN_HANDLE;
+
+//
+// Function Prototypes
+//
+
+/**
+  Insert InsertedDriverEntry onto the mScheduledQueue. To do this you
+  must add any driver with a before dependency on InsertedDriverEntry first.
+  You do this by recursively calling this routine. After all the Befores are
+  processed you can add InsertedDriverEntry to the mScheduledQueue.
+  Then you can add any driver with an After dependency on InsertedDriverEntry
+  by recursively calling this routine.
+
+  @param  InsertedDriverEntry   The driver to insert on the ScheduledLink Queue
+
+**/
+VOID
+SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter (
+  IN  EFI_SMM_DRIVER_ENTRY   *InsertedDriverEntry
+  );
+
+//
+// The Driver List contains one copy of every driver that has been discovered.
+// Items are never removed from the driver list. List of EFI_SMM_DRIVER_ENTRY
+//
+LIST_ENTRY  mDiscoveredList = INITIALIZE_LIST_HEAD_VARIABLE (mDiscoveredList);
+
+//
+// Queue of drivers that are ready to dispatch. This queue is a subset of the
+// mDiscoveredList.list of EFI_SMM_DRIVER_ENTRY.
+//
+LIST_ENTRY  mScheduledQueue = INITIALIZE_LIST_HEAD_VARIABLE (mScheduledQueue);
+
+//
+// List of handles who's Fv's have been parsed and added to the mFwDriverList.
+//
+LIST_ENTRY  mFvHandleList = INITIALIZE_LIST_HEAD_VARIABLE (mFvHandleList);
+
+//
+// Flag for the SMM Dispacher.  TRUE if dispatcher is execuing.
+//
+BOOLEAN  gDispatcherRunning = FALSE;
+
+//
+// Flag for the SMM Dispacher.  TRUE if there is one or more SMM drivers ready to be dispatched
+//
+BOOLEAN  gRequestDispatch = FALSE;
+
+//
+// List of file types supported by dispatcher
+//
+EFI_FV_FILETYPE mSmmFileTypes[] = {
+  EFI_FV_FILETYPE_SMM,
+  EFI_FV_FILETYPE_COMBINED_SMM_DXE,
+  EFI_FV_FILETYPE_SMM_CORE,
+  //
+  // Note: DXE core will process the FV image file, so skip it in SMM core
+  // EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE
+  //
+};
+
+typedef struct {
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH  File;
+  EFI_DEVICE_PATH_PROTOCOL           End;
+} FV_FILEPATH_DEVICE_PATH;
+
+FV_FILEPATH_DEVICE_PATH  mFvDevicePath;
+
+//
+// DXE Architecture Protocols
+//
+EFI_SECURITY_ARCH_PROTOCOL  *mSecurity = NULL;
+EFI_SECURITY2_ARCH_PROTOCOL *mSecurity2 = NULL;
+
+//
+// The global variable is defined for Loading modules at fixed address feature to track the SMM code
+// memory range usage. It is a bit mapped array in which every bit indicates the correspoding 
+// memory page available or not. 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED    UINT64                *mSmmCodeMemoryRangeUsageBitMap=NULL;
+
+/**
+  To check memory usage bit map array to figure out if the memory range in which the image will be loaded is available or not. If 
+  memory range is avaliable, the function will mark the correponding bits to 1 which indicates the memory range is used.
+  The function is only invoked when load modules at fixed address feature is enabled. 
+  
+  @param  ImageBase                The base addres the image will be loaded at.
+  @param  ImageSize                The size of the image
+  
+  @retval EFI_SUCCESS              The memory range the image will be loaded in is available
+  @retval EFI_NOT_FOUND            The memory range the image will be loaded in is not available
+**/
+EFI_STATUS
+CheckAndMarkFixLoadingMemoryUsageBitMap (
+  IN  EFI_PHYSICAL_ADDRESS          ImageBase,
+  IN  UINTN                         ImageSize
+  )
+{
+   UINT32                             SmmCodePageNumber;
+   UINT64                             SmmCodeSize; 
+   EFI_PHYSICAL_ADDRESS               SmmCodeBase;
+   UINTN                              BaseOffsetPageNumber;
+   UINTN                              TopOffsetPageNumber;
+   UINTN                              Index;
+   //
+   // Build tool will calculate the smm code size and then patch the PcdLoadFixAddressSmmCodePageNumber
+   //
+   SmmCodePageNumber = PcdGet32(PcdLoadFixAddressSmmCodePageNumber);
+   SmmCodeSize = EFI_PAGES_TO_SIZE (SmmCodePageNumber);
+   SmmCodeBase = gLoadModuleAtFixAddressSmramBase;
+   
+   //
+   // If the memory usage bit map is not initialized,  do it. Every bit in the array 
+   // indicate the status of the corresponding memory page, available or not
+   // 
+   if (mSmmCodeMemoryRangeUsageBitMap == NULL) {
+     mSmmCodeMemoryRangeUsageBitMap = AllocateZeroPool(((SmmCodePageNumber / 64) + 1)*sizeof(UINT64));
+   }
+   //
+   // If the Dxe code memory range is not allocated or the bit map array allocation failed, return EFI_NOT_FOUND
+   //
+   if (mSmmCodeMemoryRangeUsageBitMap == NULL) {
+     return EFI_NOT_FOUND;
+   }
+   //
+   // see if the memory range for loading the image is in the SMM code range.
+   //
+   if (SmmCodeBase + SmmCodeSize <  ImageBase + ImageSize || SmmCodeBase >  ImageBase) {
+     return EFI_NOT_FOUND;   
+   }   
+   //
+   // Test if the memory is avalaible or not.
+   // 
+   BaseOffsetPageNumber = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase - SmmCodeBase));
+   TopOffsetPageNumber  = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase + ImageSize - SmmCodeBase));
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     if ((mSmmCodeMemoryRangeUsageBitMap[Index / 64] & LShiftU64(1, (Index % 64))) != 0) {
+       //
+       // This page is already used.
+       //
+       return EFI_NOT_FOUND;  
+     }
+   }
+   
+   //
+   // Being here means the memory range is available.  So mark the bits for the memory range
+   // 
+   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
+     mSmmCodeMemoryRangeUsageBitMap[Index / 64] |= LShiftU64(1, (Index % 64));
+   }
+   return  EFI_SUCCESS;   
+}
+/**
+  Get the fixed loadding address from image header assigned by build tool. This function only be called 
+  when Loading module at Fixed address feature enabled.
+  
+  @param  ImageContext              Pointer to the image context structure that describes the PE/COFF
+                                    image that needs to be examined by this function.
+  @retval EFI_SUCCESS               An fixed loading address is assigned to this image by build tools .
+  @retval EFI_NOT_FOUND             The image has no assigned fixed loadding address.
+
+**/
+EFI_STATUS
+GetPeCoffImageFixLoadingAssignedAddress(
+  IN OUT PE_COFF_LOADER_IMAGE_CONTEXT  *ImageContext
+  )
+{
+	 UINTN                              SectionHeaderOffset;
+	 EFI_STATUS                         Status;
+	 EFI_IMAGE_SECTION_HEADER           SectionHeader;
+	 EFI_IMAGE_OPTIONAL_HEADER_UNION    *ImgHdr;
+	 EFI_PHYSICAL_ADDRESS               FixLoaddingAddress;
+	 UINT16                             Index;
+	 UINTN                              Size; 
+	 UINT16                             NumberOfSections;
+	 UINT64                             ValueInSectionHeader;
+	 
+	 FixLoaddingAddress = 0;
+	 Status = EFI_NOT_FOUND;
+	
+	 //
+   // Get PeHeader pointer
+   //
+   ImgHdr = (EFI_IMAGE_OPTIONAL_HEADER_UNION *)((CHAR8* )ImageContext->Handle + ImageContext->PeCoffHeaderOffset);
+	 SectionHeaderOffset = (UINTN)(
+                                 ImageContext->PeCoffHeaderOffset +
+                                 sizeof (UINT32) +
+                                 sizeof (EFI_IMAGE_FILE_HEADER) +
+                                 ImgHdr->Pe32.FileHeader.SizeOfOptionalHeader
+                                 );
+   NumberOfSections = ImgHdr->Pe32.FileHeader.NumberOfSections;
+     
+   //
+   // Get base address from the first section header that doesn't point to code section.
+   //
+   for (Index = 0; Index < NumberOfSections; Index++) {
+     //
+     // Read section header from file
+     //
+     Size = sizeof (EFI_IMAGE_SECTION_HEADER);
+     Status = ImageContext->ImageRead (
+                              ImageContext->Handle,
+                              SectionHeaderOffset,
+                              &Size,
+                              &SectionHeader
+                              );
+     if (EFI_ERROR (Status)) {
+       return Status;
+     }
+     
+     Status = EFI_NOT_FOUND;
+     
+     if ((SectionHeader.Characteristics & EFI_IMAGE_SCN_CNT_CODE) == 0) {
+       //
+       // Build tool will save the address in PointerToRelocations & PointerToLineNumbers fields in the first section header 
+       // that doesn't point to code section in image header.So there is an assumption that when the feature is enabled,
+       // if a module with a loading address assigned by tools, the PointerToRelocations & PointerToLineNumbers fields
+       // should not be Zero, or else, these 2 fileds should be set to Zero
+       //
+       ValueInSectionHeader = ReadUnaligned64((UINT64*)&SectionHeader.PointerToRelocations);
+       if (ValueInSectionHeader != 0) {
+         //
+         // Found first section header that doesn't point to code section in which uild tool saves the
+         // offset to SMRAM base as image base in PointerToRelocations & PointerToLineNumbers fields
+         //      
+         FixLoaddingAddress = (EFI_PHYSICAL_ADDRESS)(gLoadModuleAtFixAddressSmramBase + (INT64)ValueInSectionHeader);
+         //
+         // Check if the memory range is avaliable.
+         //
+         Status = CheckAndMarkFixLoadingMemoryUsageBitMap (FixLoaddingAddress, (UINTN)(ImageContext->ImageSize + ImageContext->SectionAlignment));
+         if (!EFI_ERROR(Status)) {
+           //
+           // The assigned address is valid. Return the specified loadding address
+           //
+           ImageContext->ImageAddress = FixLoaddingAddress;
+         }
+       }
+       break;     
+     }
+     SectionHeaderOffset += sizeof (EFI_IMAGE_SECTION_HEADER);     
+   }
+   DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED INFO: Loading module at fixed address %x, Status = %r\n", FixLoaddingAddress, Status));
+   return Status;
+}
+/**
+  Loads an EFI image into SMRAM.
+
+  @param  DriverEntry             EFI_SMM_DRIVER_ENTRY instance
+
+  @return EFI_STATUS
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLoadImage (
+  IN OUT EFI_SMM_DRIVER_ENTRY  *DriverEntry
+  )
+{
+  UINT32                         AuthenticationStatus;
+  UINTN                          FilePathSize;
+  VOID                           *Buffer;
+  UINTN                          Size;
+  UINTN                          PageCount;
+  EFI_GUID                       *NameGuid;
+  EFI_STATUS                     Status;
+  EFI_STATUS                     SecurityStatus;
+  EFI_HANDLE                     DeviceHandle;
+  EFI_PHYSICAL_ADDRESS           DstBuffer;
+  EFI_DEVICE_PATH_PROTOCOL       *FilePath;
+  EFI_DEVICE_PATH_PROTOCOL       *OriginalFilePath;
+  EFI_DEVICE_PATH_PROTOCOL       *HandleFilePath;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL  *Fv;
+  PE_COFF_LOADER_IMAGE_CONTEXT   ImageContext;
+  UINT64                         Tick;
+
+  Tick = 0;
+  PERF_CODE (
+    Tick = GetPerformanceCounter ();
+  );
+   
+  Buffer               = NULL;
+  Size                 = 0;
+  Fv                   = DriverEntry->Fv;
+  NameGuid             = &DriverEntry->FileName;
+  FilePath             = DriverEntry->FvFileDevicePath;
+
+  OriginalFilePath     = FilePath;
+  HandleFilePath       = FilePath;
+  DeviceHandle         = NULL;
+  SecurityStatus       = EFI_SUCCESS;
+  Status               = EFI_SUCCESS;
+  AuthenticationStatus = 0;
+
+  //
+  // Try to get the image device handle by checking the match protocol.
+  //
+  Status = gBS->LocateDevicePath (&gEfiFirmwareVolume2ProtocolGuid, &HandleFilePath, &DeviceHandle);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // If the Security2 and Security Architectural Protocol has not been located yet, then attempt to locate it
+  //
+  if (mSecurity2 == NULL) {
+    gBS->LocateProtocol (&gEfiSecurity2ArchProtocolGuid, NULL, (VOID**)&mSecurity2);
+  }
+  if (mSecurity == NULL) {
+    gBS->LocateProtocol (&gEfiSecurityArchProtocolGuid, NULL, (VOID**)&mSecurity);
+  }
+  //
+  // When Security2 is installed, Security Architectural Protocol must be published.
+  //
+  ASSERT (mSecurity2 == NULL || mSecurity != NULL);
+
+  //
+  // Pull out just the file portion of the DevicePath for the LoadedImage FilePath
+  //
+  FilePath = OriginalFilePath;
+  Status = gBS->HandleProtocol (DeviceHandle, &gEfiDevicePathProtocolGuid, (VOID **)&HandleFilePath);
+  if (!EFI_ERROR (Status)) {
+    FilePathSize = GetDevicePathSize (HandleFilePath) - sizeof(EFI_DEVICE_PATH_PROTOCOL);
+    FilePath = (EFI_DEVICE_PATH_PROTOCOL *) (((UINT8 *)FilePath) + FilePathSize );
+  }
+
+  //
+  // Try reading PE32 section firstly
+  //
+  Status = Fv->ReadSection (
+                 Fv,
+                 NameGuid,
+                 EFI_SECTION_PE32,
+                 0,
+                 &Buffer,
+                 &Size,
+                 &AuthenticationStatus
+                 );
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Try reading TE section secondly
+    //
+    Buffer = NULL;
+    Size   = 0;
+    Status = Fv->ReadSection (
+                  Fv,
+                  NameGuid,
+                  EFI_SECTION_TE,
+                  0,
+                  &Buffer,
+                  &Size,
+                  &AuthenticationStatus
+                  );
+  }
+  
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    return Status;
+  }
+
+  //
+  // Verify File Authentication through the Security2 Architectural Protocol
+  //
+  if (mSecurity2 != NULL) {
+    SecurityStatus = mSecurity2->FileAuthentication (
+                                  mSecurity2,
+                                  OriginalFilePath,
+                                  Buffer,
+                                  Size,
+                                  FALSE
+                                  );
+  }
+
+  //
+  // Verify the Authentication Status through the Security Architectural Protocol
+  // Only on images that have been read using Firmware Volume protocol.
+  // All SMM images are from FV protocol. 
+  //
+  if (!EFI_ERROR (SecurityStatus) && (mSecurity != NULL)) {
+    SecurityStatus = mSecurity->FileAuthenticationState (
+                                  mSecurity,
+                                  AuthenticationStatus,
+                                  OriginalFilePath
+                                  );
+  }
+
+  if (EFI_ERROR (SecurityStatus) && SecurityStatus != EFI_SECURITY_VIOLATION) {
+    Status = SecurityStatus;
+    return Status;
+  }
+  
+  //
+  // Initialize ImageContext
+  //
+  ImageContext.Handle = Buffer;
+  ImageContext.ImageRead = PeCoffLoaderImageReadFromMemory;
+
+  //
+  // Get information about the image being loaded
+  //
+  Status = PeCoffLoaderGetImageInfo (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    return Status;
+  }
+  //
+  // if Loading module at Fixed Address feature is enabled, then  cut out a memory range started from TESG BASE
+  // to hold the Smm driver code
+  //
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+    //
+    // Get the fixed loading address assigned by Build tool
+    //
+    Status = GetPeCoffImageFixLoadingAssignedAddress (&ImageContext);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Since the memory range to load Smm core alreay been cut out, so no need to allocate and free this range
+      // following statements is to bypass SmmFreePages
+      //
+      PageCount = 0;
+      DstBuffer = (UINTN)gLoadModuleAtFixAddressSmramBase;   
+    } else {
+       DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED ERROR: Failed to load module at fixed address. \n"));
+       //
+       // allocate the memory to load the SMM driver
+       //
+       PageCount = (UINTN)EFI_SIZE_TO_PAGES((UINTN)ImageContext.ImageSize + ImageContext.SectionAlignment);
+       DstBuffer = (UINTN)(-1);
+     
+       Status = SmmAllocatePages (
+                   AllocateMaxAddress,
+                   EfiRuntimeServicesCode,
+                   PageCount,
+                   &DstBuffer
+                   );
+       if (EFI_ERROR (Status)) {
+         if (Buffer != NULL) {
+           gBS->FreePool (Buffer);
+         } 
+         return Status;
+       }     
+      ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)DstBuffer;
+    }
+  } else {
+     PageCount = (UINTN)EFI_SIZE_TO_PAGES((UINTN)ImageContext.ImageSize + ImageContext.SectionAlignment);
+     DstBuffer = (UINTN)(-1);
+     
+     Status = SmmAllocatePages (
+                  AllocateMaxAddress,
+                  EfiRuntimeServicesCode,
+                  PageCount,
+                  &DstBuffer
+                  );
+     if (EFI_ERROR (Status)) {
+       if (Buffer != NULL) {
+         gBS->FreePool (Buffer);
+       }
+       return Status;
+     }
+     
+     ImageContext.ImageAddress = (EFI_PHYSICAL_ADDRESS)DstBuffer;
+  }
+  //
+  // Align buffer on section boundry
+  //
+  ImageContext.ImageAddress += ImageContext.SectionAlignment - 1;
+  ImageContext.ImageAddress &= ~((EFI_PHYSICAL_ADDRESS)(ImageContext.SectionAlignment - 1));
+
+  //
+  // Load the image to our new buffer
+  //
+  Status = PeCoffLoaderLoadImage (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    SmmFreePages (DstBuffer, PageCount);
+    return Status;
+  }
+
+  //
+  // Relocate the image in our new buffer
+  //
+  Status = PeCoffLoaderRelocateImage (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    SmmFreePages (DstBuffer, PageCount);
+    return Status;
+  }
+
+  //
+  // Flush the instruction cache so the image data are written before we execute it
+  //
+  InvalidateInstructionCacheRange ((VOID *)(UINTN) ImageContext.ImageAddress, (UINTN) ImageContext.ImageSize);
+
+  //
+  // Save Image EntryPoint in DriverEntry
+  //
+  DriverEntry->ImageEntryPoint  = ImageContext.EntryPoint;
+  DriverEntry->ImageBuffer      = DstBuffer; 
+  DriverEntry->NumberOfPage     = PageCount;
+
+  //
+  // Allocate a Loaded Image Protocol in EfiBootServicesData
+  //
+  Status = gBS->AllocatePool (EfiBootServicesData, sizeof (EFI_LOADED_IMAGE_PROTOCOL), (VOID **)&DriverEntry->LoadedImage);
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    SmmFreePages (DstBuffer, PageCount);
+    return Status;
+  }
+
+  ZeroMem (DriverEntry->LoadedImage, sizeof (EFI_LOADED_IMAGE_PROTOCOL));
+  //
+  // Fill in the remaining fields of the Loaded Image Protocol instance.
+  // Note: ImageBase is an SMRAM address that can not be accessed outside of SMRAM if SMRAM window is closed.
+  //
+  DriverEntry->LoadedImage->Revision      = EFI_LOADED_IMAGE_PROTOCOL_REVISION;
+  DriverEntry->LoadedImage->ParentHandle  = gSmmCorePrivate->SmmIplImageHandle;
+  DriverEntry->LoadedImage->SystemTable   = gST;
+  DriverEntry->LoadedImage->DeviceHandle  = DeviceHandle;
+
+  //
+  // Make an EfiBootServicesData buffer copy of FilePath
+  //
+  Status = gBS->AllocatePool (EfiBootServicesData, GetDevicePathSize (FilePath), (VOID **)&DriverEntry->LoadedImage->FilePath);
+  if (EFI_ERROR (Status)) {
+    if (Buffer != NULL) {
+      gBS->FreePool (Buffer);
+    }
+    SmmFreePages (DstBuffer, PageCount);
+    return Status;
+  }
+  CopyMem (DriverEntry->LoadedImage->FilePath, FilePath, GetDevicePathSize (FilePath));
+
+  DriverEntry->LoadedImage->ImageBase     = (VOID *)(UINTN)DriverEntry->ImageBuffer;
+  DriverEntry->LoadedImage->ImageSize     = ImageContext.ImageSize;
+  DriverEntry->LoadedImage->ImageCodeType = EfiRuntimeServicesCode;
+  DriverEntry->LoadedImage->ImageDataType = EfiRuntimeServicesData;
+
+  //
+  // Create a new image handle in the UEFI handle database for the SMM Driver
+  //
+  DriverEntry->ImageHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &DriverEntry->ImageHandle,
+                  &gEfiLoadedImageProtocolGuid, DriverEntry->LoadedImage,
+                  NULL
+                  );
+
+  PERF_START (DriverEntry->ImageHandle, "LoadImage:", NULL, Tick);
+  PERF_END (DriverEntry->ImageHandle, "LoadImage:", NULL, 0);
+
+  //
+  // Print the load address and the PDB file name if it is available
+  //
+
+  DEBUG_CODE_BEGIN ();
+
+    UINTN Index;
+    UINTN StartIndex;
+    CHAR8 EfiFileName[256];
+
+
+    DEBUG ((DEBUG_INFO | DEBUG_LOAD,
+           "Loading SMM driver at 0x%11p EntryPoint=0x%11p ",
+           (VOID *)(UINTN) ImageContext.ImageAddress,
+           FUNCTION_ENTRY_POINT (ImageContext.EntryPoint)));
+
+
+    //
+    // Print Module Name by Pdb file path.
+    // Windows and Unix style file path are all trimmed correctly.
+    //
+    if (ImageContext.PdbPointer != NULL) {
+      StartIndex = 0;
+      for (Index = 0; ImageContext.PdbPointer[Index] != 0; Index++) {
+        if ((ImageContext.PdbPointer[Index] == '\\') || (ImageContext.PdbPointer[Index] == '/')) {
+          StartIndex = Index + 1;
+        }
+      }
+      //
+      // Copy the PDB file name to our temporary string, and replace .pdb with .efi
+      // The PDB file name is limited in the range of 0~255.
+      // If the length is bigger than 255, trim the redudant characters to avoid overflow in array boundary.
+      //
+      for (Index = 0; Index < sizeof (EfiFileName) - 4; Index++) {
+        EfiFileName[Index] = ImageContext.PdbPointer[Index + StartIndex];
+        if (EfiFileName[Index] == 0) {
+          EfiFileName[Index] = '.';
+        }
+        if (EfiFileName[Index] == '.') {
+          EfiFileName[Index + 1] = 'e';
+          EfiFileName[Index + 2] = 'f';
+          EfiFileName[Index + 3] = 'i';
+          EfiFileName[Index + 4] = 0;
+          break;
+        }
+      }
+
+      if (Index == sizeof (EfiFileName) - 4) {
+        EfiFileName[Index] = 0;
+      }
+      DEBUG ((DEBUG_INFO | DEBUG_LOAD, "%a", EfiFileName)); // &Image->ImageContext.PdbPointer[StartIndex]));
+    }
+    DEBUG ((DEBUG_INFO | DEBUG_LOAD, "\n"));
+
+  DEBUG_CODE_END ();
+
+  //
+  // Free buffer allocated by Fv->ReadSection.
+  //
+  // The UEFI Boot Services FreePool() function must be used because Fv->ReadSection 
+  // used the UEFI Boot Services AllocatePool() function
+  //
+  Status = gBS->FreePool(Buffer);
+  if (!EFI_ERROR (Status) && EFI_ERROR (SecurityStatus)) {
+    Status = SecurityStatus;
+  }
+  return Status;  
+}
+
+/**
+  Preprocess dependency expression and update DriverEntry to reflect the
+  state of  Before and After dependencies. If DriverEntry->Before
+  or DriverEntry->After is set it will never be cleared. 
+
+  @param  DriverEntry           DriverEntry element to update .
+
+  @retval EFI_SUCCESS           It always works.
+
+**/
+EFI_STATUS
+SmmPreProcessDepex (
+  IN EFI_SMM_DRIVER_ENTRY  *DriverEntry
+  )
+{
+  UINT8  *Iterator;
+
+  Iterator = DriverEntry->Depex;
+  DriverEntry->Dependent = TRUE;
+
+  if (*Iterator == EFI_DEP_BEFORE) {
+    DriverEntry->Before = TRUE;
+  } else if (*Iterator == EFI_DEP_AFTER) {
+    DriverEntry->After = TRUE;
+  }
+
+  if (DriverEntry->Before || DriverEntry->After) {
+    CopyMem (&DriverEntry->BeforeAfterGuid, Iterator + 1, sizeof (EFI_GUID));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read Depex and pre-process the Depex for Before and After. If Section Extraction
+  protocol returns an error via ReadSection defer the reading of the Depex.
+
+  @param  DriverEntry           Driver to work on.
+
+  @retval EFI_SUCCESS           Depex read and preprossesed
+  @retval EFI_PROTOCOL_ERROR    The section extraction protocol returned an error
+                                and  Depex reading needs to be retried.
+  @retval Error                 DEPEX not found.
+
+**/
+EFI_STATUS
+SmmGetDepexSectionAndPreProccess (
+  IN EFI_SMM_DRIVER_ENTRY  *DriverEntry
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_SECTION_TYPE               SectionType;
+  UINT32                         AuthenticationStatus;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL  *Fv;
+
+  Fv = DriverEntry->Fv;
+
+  //
+  // Grab Depex info, it will never be free'ed.
+  // (Note: DriverEntry->Depex is in DXE memory)
+  //
+  SectionType         = EFI_SECTION_SMM_DEPEX;
+  Status = Fv->ReadSection (
+                DriverEntry->Fv,
+                &DriverEntry->FileName,
+                SectionType,
+                0,
+                &DriverEntry->Depex,
+                (UINTN *)&DriverEntry->DepexSize,
+                &AuthenticationStatus
+                );
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_PROTOCOL_ERROR) {
+      //
+      // The section extraction protocol failed so set protocol error flag
+      //
+      DriverEntry->DepexProtocolError = TRUE;
+    } else {
+      //
+      // If no Depex assume depend on all architectural protocols
+      //
+      DriverEntry->Depex = NULL;
+      DriverEntry->Dependent = TRUE;
+      DriverEntry->DepexProtocolError = FALSE;
+    }
+  } else {
+    //
+    // Set Before and After state information based on Depex
+    // Driver will be put in Dependent state
+    //
+    SmmPreProcessDepex (DriverEntry);
+    DriverEntry->DepexProtocolError = FALSE;
+  }
+
+  return Status;
+}
+
+/**
+  This is the main Dispatcher for SMM and it exits when there are no more
+  drivers to run. Drain the mScheduledQueue and load and start a PE
+  image for each driver. Search the mDiscoveredList to see if any driver can
+  be placed on the mScheduledQueue. If no drivers are placed on the
+  mScheduledQueue exit the function. 
+
+  @retval EFI_SUCCESS           All of the SMM Drivers that could be dispatched
+                                have been run and the SMM Entry Point has been
+                                registered.
+  @retval EFI_NOT_READY         The SMM Driver that registered the SMM Entry Point
+                                was just dispatched.
+  @retval EFI_NOT_FOUND         There are no SMM Drivers available to be dispatched.
+  @retval EFI_ALREADY_STARTED   The SMM Dispatcher is already running
+
+**/
+EFI_STATUS
+SmmDispatcher (
+  VOID
+  )
+{
+  EFI_STATUS            Status;
+  LIST_ENTRY            *Link;
+  EFI_SMM_DRIVER_ENTRY  *DriverEntry;
+  BOOLEAN               ReadyToRun;
+  BOOLEAN               PreviousSmmEntryPointRegistered;
+
+  if (!gRequestDispatch) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (gDispatcherRunning) {
+    //
+    // If the dispatcher is running don't let it be restarted.
+    //
+    return EFI_ALREADY_STARTED;
+  }
+
+  gDispatcherRunning = TRUE;
+
+  do {
+    //
+    // Drain the Scheduled Queue
+    //
+    while (!IsListEmpty (&mScheduledQueue)) {
+      DriverEntry = CR (
+                      mScheduledQueue.ForwardLink,
+                      EFI_SMM_DRIVER_ENTRY,
+                      ScheduledLink,
+                      EFI_SMM_DRIVER_ENTRY_SIGNATURE
+                      );
+
+      //
+      // Load the SMM Driver image into memory. If the Driver was transitioned from
+      // Untrused to Scheduled it would have already been loaded so we may need to
+      // skip the LoadImage
+      //
+      if (DriverEntry->ImageHandle == NULL) {
+        Status = SmmLoadImage (DriverEntry);
+
+        //
+        // Update the driver state to reflect that it's been loaded
+        //
+        if (EFI_ERROR (Status)) {
+          //
+          // The SMM Driver could not be loaded, and do not attempt to load or start it again.
+          // Take driver from Scheduled to Initialized.
+          //
+          DriverEntry->Initialized  = TRUE;
+          DriverEntry->Scheduled = FALSE;
+          RemoveEntryList (&DriverEntry->ScheduledLink);
+
+          //
+          // If it's an error don't try the StartImage
+          //
+          continue;
+        }
+      }
+
+      DriverEntry->Scheduled    = FALSE;
+      DriverEntry->Initialized  = TRUE;
+      RemoveEntryList (&DriverEntry->ScheduledLink);
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+        EFI_PROGRESS_CODE,
+        EFI_SOFTWARE_SMM_DRIVER | EFI_SW_PC_INIT_BEGIN,
+        &DriverEntry->ImageHandle,
+        sizeof (DriverEntry->ImageHandle)
+        );
+
+      //
+      // Cache state of SmmEntryPointRegistered before calling entry point
+      //
+      PreviousSmmEntryPointRegistered = gSmmCorePrivate->SmmEntryPointRegistered;
+
+      //
+      // For each SMM driver, pass NULL as ImageHandle
+      //
+      RegisterSmramProfileImage (DriverEntry, TRUE);
+      PERF_START (DriverEntry->ImageHandle, "StartImage:", NULL, 0);
+      Status = ((EFI_IMAGE_ENTRY_POINT)(UINTN)DriverEntry->ImageEntryPoint)(DriverEntry->ImageHandle, gST);
+      PERF_END (DriverEntry->ImageHandle, "StartImage:", NULL, 0);
+      if (EFI_ERROR(Status)){
+        UnregisterSmramProfileImage (DriverEntry, TRUE);
+        SmmFreePages(DriverEntry->ImageBuffer, DriverEntry->NumberOfPage);
+        //
+        // Uninstall LoadedImage
+        //
+        Status = gBS->UninstallProtocolInterface (
+                        DriverEntry->ImageHandle,
+                        &gEfiLoadedImageProtocolGuid,
+                        DriverEntry->LoadedImage
+                        );
+        if (!EFI_ERROR (Status)) {
+          if (DriverEntry->LoadedImage->FilePath != NULL) {
+            gBS->FreePool (DriverEntry->LoadedImage->FilePath);
+          }
+          gBS->FreePool (DriverEntry->LoadedImage);
+        }
+      }
+
+      REPORT_STATUS_CODE_WITH_EXTENDED_DATA (
+        EFI_PROGRESS_CODE,
+        EFI_SOFTWARE_SMM_DRIVER | EFI_SW_PC_INIT_END,
+        &DriverEntry->ImageHandle,
+        sizeof (DriverEntry->ImageHandle)
+        );
+
+      if (!PreviousSmmEntryPointRegistered && gSmmCorePrivate->SmmEntryPointRegistered) {
+        //
+        // Return immediately if the SMM Entry Point was registered by the SMM 
+        // Driver that was just dispatched.  The SMM IPL will reinvoke the SMM
+        // Core Dispatcher.  This is required so SMM Mode may be enabled as soon 
+        // as all the dependent SMM Drivers for SMM Mode have been dispatched.  
+        // Once the SMM Entry Point has been registered, then SMM Mode will be 
+        // used.
+        //
+        gRequestDispatch = TRUE;
+        gDispatcherRunning = FALSE;
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Search DriverList for items to place on Scheduled Queue
+    //
+    ReadyToRun = FALSE;
+    for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+      DriverEntry = CR (Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+
+      if (DriverEntry->DepexProtocolError){
+        //
+        // If Section Extraction Protocol did not let the Depex be read before retry the read
+        //
+        Status = SmmGetDepexSectionAndPreProccess (DriverEntry);
+      }
+
+      if (DriverEntry->Dependent) {
+        if (SmmIsSchedulable (DriverEntry)) {
+          SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+          ReadyToRun = TRUE;
+        }
+      }
+    }
+  } while (ReadyToRun);
+
+  //
+  // If there is no more SMM driver to dispatch, stop the dispatch request
+  //
+  gRequestDispatch = FALSE;
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR (Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+
+    if (!DriverEntry->Initialized){
+      //
+      // We have SMM driver pending to dispatch
+      //
+      gRequestDispatch = TRUE;
+      break;
+    }
+  }
+
+  gDispatcherRunning = FALSE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Insert InsertedDriverEntry onto the mScheduledQueue. To do this you
+  must add any driver with a before dependency on InsertedDriverEntry first.
+  You do this by recursively calling this routine. After all the Befores are
+  processed you can add InsertedDriverEntry to the mScheduledQueue.
+  Then you can add any driver with an After dependency on InsertedDriverEntry
+  by recursively calling this routine.
+
+  @param  InsertedDriverEntry   The driver to insert on the ScheduledLink Queue
+
+**/
+VOID
+SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter (
+  IN  EFI_SMM_DRIVER_ENTRY   *InsertedDriverEntry
+  )
+{
+  LIST_ENTRY            *Link;
+  EFI_SMM_DRIVER_ENTRY *DriverEntry;
+
+  //
+  // Process Before Dependency
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->Before && DriverEntry->Dependent && DriverEntry != InsertedDriverEntry) {
+      DEBUG ((DEBUG_DISPATCH, "Evaluate SMM DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+      DEBUG ((DEBUG_DISPATCH, "  BEFORE FFS(%g) = ", &DriverEntry->BeforeAfterGuid));
+      if (CompareGuid (&InsertedDriverEntry->FileName, &DriverEntry->BeforeAfterGuid)) {
+        //
+        // Recursively process BEFORE
+        //
+        DEBUG ((DEBUG_DISPATCH, "TRUE\n  END\n  RESULT = TRUE\n"));
+        SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "FALSE\n  END\n  RESULT = FALSE\n"));
+      }
+    }
+  }
+
+  //
+  // Convert driver from Dependent to Scheduled state
+  //
+
+  InsertedDriverEntry->Dependent = FALSE;
+  InsertedDriverEntry->Scheduled = TRUE;
+  InsertTailList (&mScheduledQueue, &InsertedDriverEntry->ScheduledLink);
+
+
+  //
+  // Process After Dependency
+  //
+  for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->After && DriverEntry->Dependent && DriverEntry != InsertedDriverEntry) {
+      DEBUG ((DEBUG_DISPATCH, "Evaluate SMM DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+      DEBUG ((DEBUG_DISPATCH, "  AFTER FFS(%g) = ", &DriverEntry->BeforeAfterGuid));
+      if (CompareGuid (&InsertedDriverEntry->FileName, &DriverEntry->BeforeAfterGuid)) {
+        //
+        // Recursively process AFTER
+        //
+        DEBUG ((DEBUG_DISPATCH, "TRUE\n  END\n  RESULT = TRUE\n"));
+        SmmInsertOnScheduledQueueWhileProcessingBeforeAndAfter (DriverEntry);
+      } else {
+        DEBUG ((DEBUG_DISPATCH, "FALSE\n  END\n  RESULT = FALSE\n"));
+      }
+    }
+  }
+}
+
+/**
+  Return TRUE if the Fv has been processed, FALSE if not.
+
+  @param  FvHandle              The handle of a FV that's being tested
+
+  @retval TRUE                  Fv protocol on FvHandle has been processed
+  @retval FALSE                 Fv protocol on FvHandle has not yet been
+                                processed
+
+**/
+BOOLEAN
+FvHasBeenProcessed (
+  IN EFI_HANDLE  FvHandle
+  )
+{
+  LIST_ENTRY    *Link;
+  KNOWN_HANDLE  *KnownHandle;
+
+  for (Link = mFvHandleList.ForwardLink; Link != &mFvHandleList; Link = Link->ForwardLink) {
+    KnownHandle = CR(Link, KNOWN_HANDLE, Link, KNOWN_HANDLE_SIGNATURE);
+    if (KnownHandle->Handle == FvHandle) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Remember that Fv protocol on FvHandle has had it's drivers placed on the
+  mDiscoveredList. This fucntion adds entries on the mFvHandleList. Items are
+  never removed/freed from the mFvHandleList.
+
+  @param  FvHandle              The handle of a FV that has been processed
+
+**/
+VOID
+FvIsBeingProcesssed (
+  IN EFI_HANDLE  FvHandle
+  )
+{
+  KNOWN_HANDLE  *KnownHandle;
+
+  KnownHandle = AllocatePool (sizeof (KNOWN_HANDLE));
+  ASSERT (KnownHandle != NULL);
+
+  KnownHandle->Signature = KNOWN_HANDLE_SIGNATURE;
+  KnownHandle->Handle = FvHandle;
+  InsertTailList (&mFvHandleList, &KnownHandle->Link);
+}
+
+/**
+  Convert FvHandle and DriverName into an EFI device path
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_SMM_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+
+  @return Pointer to device path constructed from FvHandle and DriverName
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+SmmFvToDevicePath (
+  IN  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv,
+  IN  EFI_HANDLE                      FvHandle,
+  IN  EFI_GUID                        *DriverName
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_DEVICE_PATH_PROTOCOL            *FvDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *FileNameDevicePath;
+
+  //
+  // Remember the device path of the FV
+  //
+  Status = gBS->HandleProtocol (FvHandle, &gEfiDevicePathProtocolGuid, (VOID **)&FvDevicePath);
+  if (EFI_ERROR (Status)) {
+    FileNameDevicePath = NULL;
+  } else {
+    //
+    // Build a device path to the file in the FV to pass into gBS->LoadImage
+    //
+    EfiInitializeFwVolDevicepathNode (&mFvDevicePath.File, DriverName);
+    SetDevicePathEndNode (&mFvDevicePath.End);
+
+    //
+    // Note: FileNameDevicePath is in DXE memory
+    //
+    FileNameDevicePath = AppendDevicePath (
+                            FvDevicePath,
+                            (EFI_DEVICE_PATH_PROTOCOL *)&mFvDevicePath
+                            );
+  }
+  return FileNameDevicePath;
+}
+
+/**
+  Add an entry to the mDiscoveredList. Allocate memory to store the DriverEntry,
+  and initilize any state variables. Read the Depex from the FV and store it
+  in DriverEntry. Pre-process the Depex to set the Before and After state.
+  The Discovered list is never free'ed and contains booleans that represent the
+  other possible SMM driver states.
+
+  @param  Fv                    Fv protocol, needed to read Depex info out of
+                                FLASH.
+  @param  FvHandle              Handle for Fv, needed in the
+                                EFI_SMM_DRIVER_ENTRY so that the PE image can be
+                                read out of the FV at a later time.
+  @param  DriverName            Name of driver to add to mDiscoveredList.
+
+  @retval EFI_SUCCESS           If driver was added to the mDiscoveredList.
+  @retval EFI_ALREADY_STARTED   The driver has already been started. Only one
+                                DriverName may be active in the system at any one
+                                time.
+
+**/
+EFI_STATUS
+SmmAddToDriverList (
+  IN EFI_FIRMWARE_VOLUME2_PROTOCOL  *Fv,
+  IN EFI_HANDLE                     FvHandle,
+  IN EFI_GUID                       *DriverName
+  )
+{
+  EFI_SMM_DRIVER_ENTRY  *DriverEntry;
+
+  //
+  // Create the Driver Entry for the list. ZeroPool initializes lots of variables to
+  // NULL or FALSE.
+  //
+  DriverEntry = AllocateZeroPool (sizeof (EFI_SMM_DRIVER_ENTRY));
+  ASSERT (DriverEntry != NULL);
+
+  DriverEntry->Signature        = EFI_SMM_DRIVER_ENTRY_SIGNATURE;
+  CopyGuid (&DriverEntry->FileName, DriverName);
+  DriverEntry->FvHandle         = FvHandle;
+  DriverEntry->Fv               = Fv;
+  DriverEntry->FvFileDevicePath = SmmFvToDevicePath (Fv, FvHandle, DriverName);
+
+  SmmGetDepexSectionAndPreProccess (DriverEntry);
+
+  InsertTailList (&mDiscoveredList, &DriverEntry->Link);
+  gRequestDispatch = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  Event notification that is fired every time a FV dispatch protocol is added.
+  More than one protocol may have been added when this event is fired, so you
+  must loop on SmmLocateHandle () to see how many protocols were added and
+  do the following to each FV:
+  If the Fv has already been processed, skip it. If the Fv has not been
+  processed then mark it as being processed, as we are about to process it.
+  Read the Fv and add any driver in the Fv to the mDiscoveredList.The
+  mDiscoveredList is never free'ed and contains variables that define
+  the other states the SMM driver transitions to..
+  While you are at it read the A Priori file into memory.
+  Place drivers in the A Priori list onto the mScheduledQueue.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmDriverDispatchHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         HandleCount;
+  EFI_HANDLE                    *HandleBuffer;
+  EFI_STATUS                    GetNextFileStatus;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *Fv;
+  EFI_DEVICE_PATH_PROTOCOL      *FvDevicePath;
+  EFI_HANDLE                    FvHandle;
+  EFI_GUID                      NameGuid;
+  UINTN                         Key;
+  EFI_FV_FILETYPE               Type;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  UINTN                         Size;
+  EFI_SMM_DRIVER_ENTRY          *DriverEntry;
+  EFI_GUID                      *AprioriFile;
+  UINTN                         AprioriEntryCount;
+  UINTN                         HandleIndex;
+  UINTN                         SmmTypeIndex;
+  UINTN                         AprioriIndex;
+  LIST_ENTRY                    *Link;
+  UINT32                        AuthenticationStatus;
+  UINTN                         SizeOfBuffer;
+
+  HandleBuffer = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  for (HandleIndex = 0; HandleIndex < HandleCount; HandleIndex++) {
+    FvHandle = HandleBuffer[HandleIndex];
+
+    if (FvHasBeenProcessed (FvHandle)) {
+      //
+      // This Fv has already been processed so lets skip it!
+      //
+      continue;
+    }
+
+    //
+    // Since we are about to process this Fv mark it as processed.
+    //
+    FvIsBeingProcesssed (FvHandle);
+
+    Status = gBS->HandleProtocol (FvHandle, &gEfiFirmwareVolume2ProtocolGuid, (VOID **)&Fv);
+    if (EFI_ERROR (Status)) {
+      //
+      // FvHandle must have a Firmware Volume2 Protocol thus we should never get here.
+      //
+      ASSERT (FALSE);
+      continue;
+    }
+
+    Status = gBS->HandleProtocol (FvHandle, &gEfiDevicePathProtocolGuid, (VOID **)&FvDevicePath);
+    if (EFI_ERROR (Status)) {
+      //
+      // The Firmware volume doesn't have device path, can't be dispatched.
+      //
+      continue;
+    }
+
+    //
+    // Discover Drivers in FV and add them to the Discovered Driver List.
+    // Process EFI_FV_FILETYPE_SMM type and then EFI_FV_FILETYPE_COMBINED_SMM_DXE
+    //  EFI_FV_FILETYPE_SMM_CORE is processed to produce a Loaded Image protocol for the core
+    //
+    for (SmmTypeIndex = 0; SmmTypeIndex < sizeof (mSmmFileTypes)/sizeof (EFI_FV_FILETYPE); SmmTypeIndex++) {
+      //
+      // Initialize the search key
+      //
+      Key = 0;
+      do {
+        Type = mSmmFileTypes[SmmTypeIndex];
+        GetNextFileStatus = Fv->GetNextFile (
+                                  Fv,
+                                  &Key,
+                                  &Type,
+                                  &NameGuid,
+                                  &Attributes,
+                                  &Size
+                                  );
+        if (!EFI_ERROR (GetNextFileStatus)) {
+          if (Type == EFI_FV_FILETYPE_SMM_CORE) {
+            //
+            // If this is the SMM core fill in it's DevicePath & DeviceHandle
+            //
+            if (mSmmCoreLoadedImage->FilePath == NULL) {
+              //
+              // Maybe one special FV contains only one SMM_CORE module, so its device path must
+              // be initialized completely.
+              //
+              EfiInitializeFwVolDevicepathNode (&mFvDevicePath.File, &NameGuid);
+              SetDevicePathEndNode (&mFvDevicePath.End);
+
+              //
+              // Make an EfiBootServicesData buffer copy of FilePath
+              //
+              Status = gBS->AllocatePool (
+                              EfiBootServicesData,
+                              GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *)&mFvDevicePath),
+                              (VOID **)&mSmmCoreLoadedImage->FilePath
+                              );
+              ASSERT_EFI_ERROR (Status);
+              CopyMem (mSmmCoreLoadedImage->FilePath, &mFvDevicePath, GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *)&mFvDevicePath));
+
+              mSmmCoreLoadedImage->DeviceHandle = FvHandle;
+            }
+          } else {
+            SmmAddToDriverList (Fv, FvHandle, &NameGuid);
+          }
+        }
+      } while (!EFI_ERROR (GetNextFileStatus));
+    }
+
+    //
+    // Read the array of GUIDs from the Apriori file if it is present in the firmware volume
+    // (Note: AprioriFile is in DXE memory)
+    //
+    AprioriFile = NULL;
+    Status = Fv->ReadSection (
+                  Fv,
+                  &gAprioriGuid,
+                  EFI_SECTION_RAW,
+                  0,
+                  (VOID **)&AprioriFile,
+                  &SizeOfBuffer,
+                  &AuthenticationStatus
+                  );
+    if (!EFI_ERROR (Status)) {
+      AprioriEntryCount = SizeOfBuffer / sizeof (EFI_GUID);
+    } else {
+      AprioriEntryCount = 0;
+    }
+
+    //
+    // Put drivers on Apriori List on the Scheduled queue. The Discovered List includes
+    // drivers not in the current FV and these must be skipped since the a priori list
+    // is only valid for the FV that it resided in.
+    //
+
+    for (AprioriIndex = 0; AprioriIndex < AprioriEntryCount; AprioriIndex++) {
+      for (Link = mDiscoveredList.ForwardLink; Link != &mDiscoveredList; Link = Link->ForwardLink) {
+        DriverEntry = CR(Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+        if (CompareGuid (&DriverEntry->FileName, &AprioriFile[AprioriIndex]) &&
+            (FvHandle == DriverEntry->FvHandle)) {
+          DriverEntry->Dependent = FALSE;
+          DriverEntry->Scheduled = TRUE;
+          InsertTailList (&mScheduledQueue, &DriverEntry->ScheduledLink);
+          DEBUG ((DEBUG_DISPATCH, "Evaluate SMM DEPEX for FFS(%g)\n", &DriverEntry->FileName));
+          DEBUG ((DEBUG_DISPATCH, "  RESULT = TRUE (Apriori)\n"));
+          break;
+        }
+      }
+    }
+
+    //
+    // Free data allocated by Fv->ReadSection ()
+    //
+    // The UEFI Boot Services FreePool() function must be used because Fv->ReadSection 
+    // used the UEFI Boot Services AllocatePool() function
+    //
+    gBS->FreePool (AprioriFile);
+  }
+
+  //
+  // Execute the SMM Dispatcher on any newly discovered FVs and previously 
+  // discovered SMM drivers that have been discovered but not dispatched.
+  //
+  Status = SmmDispatcher ();
+
+  //
+  // Check to see if CommBuffer and CommBufferSize are valid
+  //
+  if (CommBuffer != NULL && CommBufferSize != NULL) {
+    if (*CommBufferSize > 0) {
+      if (Status == EFI_NOT_READY) {
+        //
+        // If a the SMM Core Entry Point was just registered, then set flag to 
+        // request the SMM Dispatcher to be restarted.
+        //
+        *(UINT8 *)CommBuffer = COMM_BUFFER_SMM_DISPATCH_RESTART;
+      } else if (!EFI_ERROR (Status)) {
+        //
+        // Set the flag to show that the SMM Dispatcher executed without errors
+        //
+        *(UINT8 *)CommBuffer = COMM_BUFFER_SMM_DISPATCH_SUCCESS;
+      } else {
+        //
+        // Set the flag to show that the SMM Dispatcher encountered an error
+        //
+        *(UINT8 *)CommBuffer = COMM_BUFFER_SMM_DISPATCH_ERROR;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Traverse the discovered list for any drivers that were discovered but not loaded
+  because the dependency experessions evaluated to false.
+
+**/
+VOID
+SmmDisplayDiscoveredNotDispatched (
+  VOID
+  )
+{
+  LIST_ENTRY                   *Link;
+  EFI_SMM_DRIVER_ENTRY         *DriverEntry;
+
+  for (Link = mDiscoveredList.ForwardLink;Link !=&mDiscoveredList; Link = Link->ForwardLink) {
+    DriverEntry = CR(Link, EFI_SMM_DRIVER_ENTRY, Link, EFI_SMM_DRIVER_ENTRY_SIGNATURE);
+    if (DriverEntry->Dependent) {
+      DEBUG ((DEBUG_LOAD, "SMM Driver %g was discovered but not loaded!!\n", &DriverEntry->FileName));
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Handle.c b/Core/MdeModulePkg/Core/PiSmmCore/Handle.c
new file mode 100644
index 0000000000..9cedb2aeb5
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Handle.c
@@ -0,0 +1,532 @@
+/** @file
+  SMM handle & protocol handling.
+
+  Copyright (c) 2009 - 2010, 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 "PiSmmCore.h"
+
+//
+// mProtocolDatabase     - A list of all protocols in the system.  (simple list for now)
+// gHandleList           - A list of all the handles in the system
+//
+LIST_ENTRY  mProtocolDatabase  = INITIALIZE_LIST_HEAD_VARIABLE (mProtocolDatabase);
+LIST_ENTRY  gHandleList        = INITIALIZE_LIST_HEAD_VARIABLE (gHandleList);
+
+/**
+  Check whether a handle is a valid EFI_HANDLE
+
+  @param  UserHandle             The handle to check
+
+  @retval EFI_INVALID_PARAMETER  The handle is NULL or not a valid EFI_HANDLE.
+  @retval EFI_SUCCESS            The handle is valid EFI_HANDLE.
+
+**/
+EFI_STATUS
+SmmValidateHandle (
+  IN EFI_HANDLE  UserHandle
+  )
+{
+  IHANDLE  *Handle;
+
+  Handle = (IHANDLE *)UserHandle;
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Handle->Signature != EFI_HANDLE_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Finds the protocol entry for the requested protocol.
+
+  @param  Protocol               The ID of the protocol
+  @param  Create                 Create a new entry if not found
+
+  @return Protocol entry
+
+**/
+PROTOCOL_ENTRY  *
+SmmFindProtocolEntry (
+  IN EFI_GUID   *Protocol,
+  IN BOOLEAN    Create
+  )
+{
+  LIST_ENTRY          *Link;
+  PROTOCOL_ENTRY      *Item;
+  PROTOCOL_ENTRY      *ProtEntry;
+
+  //
+  // Search the database for the matching GUID
+  //
+
+  ProtEntry = NULL;
+  for (Link = mProtocolDatabase.ForwardLink;
+       Link != &mProtocolDatabase;
+       Link = Link->ForwardLink) {
+
+    Item = CR(Link, PROTOCOL_ENTRY, AllEntries, PROTOCOL_ENTRY_SIGNATURE);
+    if (CompareGuid (&Item->ProtocolID, Protocol)) {
+      //
+      // This is the protocol entry
+      //
+      ProtEntry = Item;
+      break;
+    }
+  }
+
+  //
+  // If the protocol entry was not found and Create is TRUE, then
+  // allocate a new entry
+  //
+  if ((ProtEntry == NULL) && Create) {
+    ProtEntry = AllocatePool (sizeof(PROTOCOL_ENTRY));
+    if (ProtEntry != NULL) {
+      //
+      // Initialize new protocol entry structure
+      //
+      ProtEntry->Signature = PROTOCOL_ENTRY_SIGNATURE;
+      CopyGuid ((VOID *)&ProtEntry->ProtocolID, Protocol);
+      InitializeListHead (&ProtEntry->Protocols);
+      InitializeListHead (&ProtEntry->Notify);
+
+      //
+      // Add it to protocol database
+      //
+      InsertTailList (&mProtocolDatabase, &ProtEntry->AllEntries);
+    }
+  }
+  return ProtEntry;
+}
+
+/**
+  Finds the protocol instance for the requested handle and protocol.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  Handle                 The handle to search the protocol on
+  @param  Protocol               GUID of the protocol
+  @param  Interface              The interface for the protocol being searched
+
+  @return Protocol instance (NULL: Not found)
+
+**/
+PROTOCOL_INTERFACE *
+SmmFindProtocolInterface (
+  IN IHANDLE   *Handle,
+  IN EFI_GUID  *Protocol,
+  IN VOID      *Interface
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_ENTRY      *ProtEntry;
+  LIST_ENTRY          *Link;
+
+  Prot = NULL;
+
+  //
+  // Lookup the protocol entry for this protocol ID
+  //
+  ProtEntry = SmmFindProtocolEntry (Protocol, FALSE);
+  if (ProtEntry != NULL) {
+    //
+    // Look at each protocol interface for any matches
+    //
+    for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link=Link->ForwardLink) {
+      //
+      // If this protocol interface matches, remove it
+      //
+      Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+      if (Prot->Interface == Interface && Prot->Protocol == ProtEntry) {
+        break;
+      }
+      Prot = NULL;
+    }
+  }
+  return Prot;
+}
+
+/**
+  Wrapper function to SmmInstallProtocolInterfaceNotify.  This is the public API which
+  Calls the private one which contains a BOOLEAN parameter for notifications
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInstallProtocolInterface (
+  IN OUT EFI_HANDLE      *UserHandle,
+  IN EFI_GUID            *Protocol,
+  IN EFI_INTERFACE_TYPE  InterfaceType,
+  IN VOID                *Interface
+  )
+{
+  return SmmInstallProtocolInterfaceNotify (
+           UserHandle,
+           Protocol,
+           InterfaceType,
+           Interface,
+           TRUE
+           );
+}
+
+/**
+  Installs a protocol interface into the boot services environment.
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+  @param  Notify                 indicates whether notify the notification list
+                                 for this protocol
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Protocol interface successfully installed
+
+**/
+EFI_STATUS
+SmmInstallProtocolInterfaceNotify (
+  IN OUT EFI_HANDLE          *UserHandle,
+  IN     EFI_GUID            *Protocol,
+  IN     EFI_INTERFACE_TYPE  InterfaceType,
+  IN     VOID                *Interface,
+  IN     BOOLEAN             Notify
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_ENTRY      *ProtEntry;
+  IHANDLE             *Handle;
+  EFI_STATUS          Status;
+  VOID                *ExistingInterface;
+
+  //
+  // returns EFI_INVALID_PARAMETER if InterfaceType is invalid.
+  // Also added check for invalid UserHandle and Protocol pointers.
+  //
+  if (UserHandle == NULL || Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (InterfaceType != EFI_NATIVE_INTERFACE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Print debug message
+  //
+  DEBUG((DEBUG_LOAD | DEBUG_INFO, "SmmInstallProtocolInterface: %g %p\n", Protocol, Interface));
+
+  Status = EFI_OUT_OF_RESOURCES;
+  Prot = NULL;
+  Handle = NULL;
+
+  if (*UserHandle != NULL) {
+    Status = SmmHandleProtocol (*UserHandle, Protocol, (VOID **)&ExistingInterface);
+    if (!EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Lookup the Protocol Entry for the requested protocol
+  //
+  ProtEntry = SmmFindProtocolEntry (Protocol, TRUE);
+  if (ProtEntry == NULL) {
+    goto Done;
+  }
+
+  //
+  // Allocate a new protocol interface structure
+  //
+  Prot = AllocateZeroPool (sizeof(PROTOCOL_INTERFACE));
+  if (Prot == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // If caller didn't supply a handle, allocate a new one
+  //
+  Handle = (IHANDLE *)*UserHandle;
+  if (Handle == NULL) {
+    Handle = AllocateZeroPool (sizeof(IHANDLE));
+    if (Handle == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    //
+    // Initialize new handler structure
+    //
+    Handle->Signature = EFI_HANDLE_SIGNATURE;
+    InitializeListHead (&Handle->Protocols);
+
+    //
+    // Add this handle to the list global list of all handles
+    // in the system
+    //
+    InsertTailList (&gHandleList, &Handle->AllHandles);
+  }
+
+  Status = SmmValidateHandle (Handle);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Each interface that is added must be unique
+  //
+  ASSERT (SmmFindProtocolInterface (Handle, Protocol, Interface) == NULL);
+
+  //
+  // Initialize the protocol interface structure
+  //
+  Prot->Signature = PROTOCOL_INTERFACE_SIGNATURE;
+  Prot->Handle = Handle;
+  Prot->Protocol = ProtEntry;
+  Prot->Interface = Interface;
+
+  //
+  // Add this protocol interface to the head of the supported
+  // protocol list for this handle
+  //
+  InsertHeadList (&Handle->Protocols, &Prot->Link);
+
+  //
+  // Add this protocol interface to the tail of the
+  // protocol entry
+  //
+  InsertTailList (&ProtEntry->Protocols, &Prot->ByProtocol);
+
+  //
+  // Notify the notification list for this protocol
+  //
+  if (Notify) {
+    SmmNotifyProtocol (Prot);
+  }
+  Status = EFI_SUCCESS;
+
+Done:
+  if (!EFI_ERROR (Status)) {
+    //
+    // Return the new handle back to the caller
+    //
+    *UserHandle = Handle;
+  } else {
+    //
+    // There was an error, clean up
+    //
+    if (Prot != NULL) {
+      FreePool (Prot);
+    }
+  }
+  return Status;
+}
+
+/**
+  Uninstalls all instances of a protocol:interfacer from a handle.
+  If the last protocol interface is remove from the handle, the
+  handle is freed.
+
+  @param  UserHandle             The handle to remove the protocol handler from
+  @param  Protocol               The protocol, of protocol:interface, to remove
+  @param  Interface              The interface, of protocol:interface, to remove
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Protocol interface successfully uninstalled.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmUninstallProtocolInterface (
+  IN EFI_HANDLE  UserHandle,
+  IN EFI_GUID    *Protocol,
+  IN VOID        *Interface
+  )
+{
+  EFI_STATUS          Status;
+  IHANDLE             *Handle;
+  PROTOCOL_INTERFACE  *Prot;
+
+  //
+  // Check that Protocol is valid
+  //
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check that UserHandle is a valid handle
+  //
+  Status = SmmValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check that Protocol exists on UserHandle, and Interface matches the interface in the database
+  //
+  Prot = SmmFindProtocolInterface (UserHandle, Protocol, Interface);
+  if (Prot == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Remove the protocol interface from the protocol
+  //
+  Status = EFI_NOT_FOUND;
+  Handle = (IHANDLE *)UserHandle;
+  Prot   = SmmRemoveInterfaceFromProtocol (Handle, Protocol, Interface);
+
+  if (Prot != NULL) {
+    //
+    // Remove the protocol interface from the handle
+    //
+    RemoveEntryList (&Prot->Link);
+
+    //
+    // Free the memory
+    //
+    Prot->Signature = 0;
+    FreePool (Prot);
+    Status = EFI_SUCCESS;
+  }
+
+  //
+  // If there are no more handlers for the handle, free the handle
+  //
+  if (IsListEmpty (&Handle->Protocols)) {
+    Handle->Signature = 0;
+    RemoveEntryList (&Handle->AllHandles);
+    FreePool (Handle);
+  }
+  return Status;
+}
+
+/**
+  Locate a certain GUID protocol interface in a Handle's protocols.
+
+  @param  UserHandle             The handle to obtain the protocol interface on
+  @param  Protocol               The GUID of the protocol
+
+  @return The requested protocol interface for the handle
+
+**/
+PROTOCOL_INTERFACE  *
+SmmGetProtocolInterface (
+  IN EFI_HANDLE  UserHandle,
+  IN EFI_GUID    *Protocol
+  )
+{
+  EFI_STATUS          Status;
+  PROTOCOL_ENTRY      *ProtEntry;
+  PROTOCOL_INTERFACE  *Prot;
+  IHANDLE             *Handle;
+  LIST_ENTRY          *Link;
+
+  Status = SmmValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Handle = (IHANDLE *)UserHandle;
+
+  //
+  // Look at each protocol interface for a match
+  //
+  for (Link = Handle->Protocols.ForwardLink; Link != &Handle->Protocols; Link = Link->ForwardLink) {
+    Prot = CR(Link, PROTOCOL_INTERFACE, Link, PROTOCOL_INTERFACE_SIGNATURE);
+    ProtEntry = Prot->Protocol;
+    if (CompareGuid (&ProtEntry->ProtocolID, Protocol)) {
+      return Prot;
+    }
+  }
+  return NULL;
+}
+
+/**
+  Queries a handle to determine if it supports a specified protocol.
+
+  @param  UserHandle             The handle being queried.
+  @param  Protocol               The published unique identifier of the protocol.
+  @param  Interface              Supplies the address where a pointer to the
+                                 corresponding Protocol Interface is returned.
+
+  @retval EFI_SUCCESS            The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED        The device does not support the specified protocol.
+  @retval EFI_INVALID_PARAMETER  Handle is not a valid EFI_HANDLE..
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_INVALID_PARAMETER  Interface is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmHandleProtocol (
+  IN  EFI_HANDLE  UserHandle,
+  IN  EFI_GUID    *Protocol,
+  OUT VOID        **Interface
+  )
+{
+  EFI_STATUS          Status;
+  PROTOCOL_INTERFACE  *Prot;
+
+  //
+  // Check for invalid Protocol
+  //
+  if (Protocol == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check for invalid Interface
+  //
+  if (Interface == NULL) {
+    return EFI_INVALID_PARAMETER;
+  } else {
+    *Interface = NULL;
+  }
+
+  //
+  // Check for invalid UserHandle
+  //
+  Status = SmmValidateHandle (UserHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Look at each protocol interface for a match
+  //
+  Prot = SmmGetProtocolInterface (UserHandle, Protocol);
+  if (Prot == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // This is the protocol interface entry for this protocol
+  //
+  *Interface = Prot->Interface;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/InstallConfigurationTable.c b/Core/MdeModulePkg/Core/PiSmmCore/InstallConfigurationTable.c
new file mode 100644
index 0000000000..b2f6769c10
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/InstallConfigurationTable.c
@@ -0,0 +1,161 @@
+/** @file
+  System Management System Table Services SmmInstallConfigurationTable service
+
+  Copyright (c) 2009 - 2010, 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 "PiSmmCore.h"
+
+#define CONFIG_TABLE_SIZE_INCREASED 0x10
+
+UINTN  mSmmSystemTableAllocateSize = 0;
+
+/**
+  The SmmInstallConfigurationTable() function is used to maintain the list
+  of configuration tables that are stored in the System Management System
+  Table.  The list is stored as an array of (GUID, Pointer) pairs.  The list
+  must be allocated from pool memory with PoolType set to EfiRuntimeServicesData.
+
+  @param  SystemTable      A pointer to the SMM System Table (SMST).
+  @param  Guid             A pointer to the GUID for the entry to add, update, or remove.
+  @param  Table            A pointer to the buffer of the table to add.
+  @param  TableSize        The size of the table to install.
+
+  @retval EFI_SUCCESS           The (Guid, Table) pair was added, updated, or removed.
+  @retval EFI_INVALID_PARAMETER Guid is not valid.
+  @retval EFI_NOT_FOUND         An attempt was made to delete a non-existent entry.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory available to complete the operation.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInstallConfigurationTable (
+  IN  CONST EFI_SMM_SYSTEM_TABLE2  *SystemTable,
+  IN  CONST EFI_GUID               *Guid,
+  IN  VOID                         *Table,
+  IN  UINTN                        TableSize
+  )
+{
+  UINTN                    Index;
+  EFI_CONFIGURATION_TABLE  *ConfigurationTable;
+
+  //
+  // If Guid is NULL, then this operation cannot be performed
+  //
+  if (Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ConfigurationTable = gSmmCoreSmst.SmmConfigurationTable;
+
+  //
+  // Search all the table for an entry that matches Guid
+  //
+  for (Index = 0; Index < gSmmCoreSmst.NumberOfTableEntries; Index++) {
+    if (CompareGuid (Guid, &(ConfigurationTable[Index].VendorGuid))) {
+      break;
+    }
+  }
+
+  if (Index < gSmmCoreSmst.NumberOfTableEntries) {
+    //
+    // A match was found, so this is either a modify or a delete operation
+    //
+    if (Table != NULL) {
+      //
+      // If Table is not NULL, then this is a modify operation.
+      // Modify the table enty and return.
+      //
+      ConfigurationTable[Index].VendorTable = Table;
+      return EFI_SUCCESS;
+    }
+
+    //
+    // A match was found and Table is NULL, so this is a delete operation.
+    //
+    gSmmCoreSmst.NumberOfTableEntries--;
+
+    //
+    // Copy over deleted entry
+    //
+    CopyMem (
+      &(ConfigurationTable[Index]),
+      &(ConfigurationTable[Index + 1]),
+      (gSmmCoreSmst.NumberOfTableEntries - Index) * sizeof (EFI_CONFIGURATION_TABLE)
+      );
+
+  } else {
+    //
+    // No matching GUIDs were found, so this is an add operation.
+    //
+    if (Table == NULL) {
+      //
+      // If Table is NULL on an add operation, then return an error.
+      //
+      return EFI_NOT_FOUND;
+    }
+
+    //
+    // Assume that Index == gSmmCoreSmst.NumberOfTableEntries
+    //
+    if ((Index * sizeof (EFI_CONFIGURATION_TABLE)) >= mSmmSystemTableAllocateSize) {
+      //
+      // Allocate a table with one additional entry.
+      //
+      mSmmSystemTableAllocateSize += (CONFIG_TABLE_SIZE_INCREASED * sizeof (EFI_CONFIGURATION_TABLE));
+      ConfigurationTable = AllocatePool (mSmmSystemTableAllocateSize);
+      if (ConfigurationTable == NULL) {
+        //
+        // If a new table could not be allocated, then return an error.
+        //
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      if (gSmmCoreSmst.SmmConfigurationTable != NULL) {
+        //
+        // Copy the old table to the new table.
+        //
+        CopyMem (
+          ConfigurationTable,
+          gSmmCoreSmst.SmmConfigurationTable,
+          Index * sizeof (EFI_CONFIGURATION_TABLE)
+          );
+
+        //
+        // Free Old Table
+        //
+        FreePool (gSmmCoreSmst.SmmConfigurationTable);
+      }
+
+      //
+      // Update System Table
+      //
+      gSmmCoreSmst.SmmConfigurationTable = ConfigurationTable;
+    }
+
+    //
+    // Fill in the new entry
+    //
+    CopyGuid ((VOID *)&ConfigurationTable[Index].VendorGuid, Guid);
+    ConfigurationTable[Index].VendorTable = Table;
+
+    //
+    // This is an add operation, so increment the number of table entries
+    //
+    gSmmCoreSmst.NumberOfTableEntries++;
+  }
+
+  //
+  // CRC-32 field is ignorable for SMM System Table and should be set to zero
+  //
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Locate.c b/Core/MdeModulePkg/Core/PiSmmCore/Locate.c
new file mode 100644
index 0000000000..705d6d4df9
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Locate.c
@@ -0,0 +1,499 @@
+/** @file
+  Locate handle functions
+
+  Copyright (c) 2009 - 2010, 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 "PiSmmCore.h"
+
+//
+// ProtocolRequest - Last LocateHandle request ID
+//
+UINTN mEfiLocateHandleRequest = 0;
+
+//
+// Internal prototypes
+//
+
+typedef struct {
+  EFI_GUID        *Protocol;
+  VOID            *SearchKey;
+  LIST_ENTRY      *Position;
+  PROTOCOL_ENTRY  *ProtEntry;
+} LOCATE_POSITION;
+
+typedef
+IHANDLE *
+(* CORE_GET_NEXT) (
+  IN OUT LOCATE_POSITION    *Position,
+  OUT VOID                  **Interface
+  );
+
+/**
+  Routine to get the next Handle, when you are searching for all handles.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+SmmGetNextLocateAllHandles (
+  IN OUT LOCATE_POSITION  *Position,
+  OUT    VOID             **Interface
+  )
+{
+  IHANDLE     *Handle;
+
+  //
+  // Next handle
+  //
+  Position->Position = Position->Position->ForwardLink;
+
+  //
+  // If not at the end of the list, get the handle
+  //
+  Handle      = NULL;
+  *Interface  = NULL;
+  if (Position->Position != &gHandleList) {
+    Handle = CR (Position->Position, IHANDLE, AllHandles, EFI_HANDLE_SIGNATURE);
+  }
+  return Handle;
+}
+
+/**
+  Routine to get the next Handle, when you are searching for register protocol
+  notifies.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+SmmGetNextLocateByRegisterNotify (
+  IN OUT LOCATE_POSITION  *Position,
+  OUT    VOID             **Interface
+  )
+{
+  IHANDLE             *Handle;
+  PROTOCOL_NOTIFY     *ProtNotify;
+  PROTOCOL_INTERFACE  *Prot;
+  LIST_ENTRY          *Link;
+
+  Handle      = NULL;
+  *Interface  = NULL;
+  ProtNotify = Position->SearchKey;
+
+  //
+  // If this is the first request, get the next handle
+  //
+  if (ProtNotify != NULL) {
+    ASSERT(ProtNotify->Signature == PROTOCOL_NOTIFY_SIGNATURE);
+    Position->SearchKey = NULL;
+
+    //
+    // If not at the end of the list, get the next handle
+    //
+    Link = ProtNotify->Position->ForwardLink;
+    if (Link != &ProtNotify->Protocol->Protocols) {
+      Prot = CR (Link, PROTOCOL_INTERFACE, ByProtocol, PROTOCOL_INTERFACE_SIGNATURE);
+      Handle = Prot->Handle;
+      *Interface = Prot->Interface;
+    }
+  }
+  return Handle;
+}
+
+/**
+  Routine to get the next Handle, when you are searching for a given protocol.
+
+  @param  Position               Information about which Handle to seach for.
+  @param  Interface              Return the interface structure for the matching
+                                 protocol.
+
+  @return An pointer to IHANDLE if the next Position is not the end of the list.
+          Otherwise,NULL is returned.
+
+**/
+IHANDLE *
+SmmGetNextLocateByProtocol (
+  IN OUT LOCATE_POSITION  *Position,
+  OUT    VOID             **Interface
+  )
+{
+  IHANDLE             *Handle;
+  LIST_ENTRY          *Link;
+  PROTOCOL_INTERFACE  *Prot;
+
+  Handle      = NULL;
+  *Interface  = NULL;
+  for (; ;) {
+    //
+    // Next entry
+    //
+    Link = Position->Position->ForwardLink;
+    Position->Position = Link;
+
+    //
+    // If not at the end, return the handle
+    //
+    if (Link == &Position->ProtEntry->Protocols) {
+      Handle = NULL;
+      break;
+    }
+
+    //
+    // Get the handle
+    //
+    Prot = CR(Link, PROTOCOL_INTERFACE, ByProtocol, PROTOCOL_INTERFACE_SIGNATURE);
+    Handle = Prot->Handle;
+    *Interface = Prot->Interface;
+
+    //
+    // If this handle has not been returned this request, then
+    // return it now
+    //
+    if (Handle->LocateRequest != mEfiLocateHandleRequest) {
+      Handle->LocateRequest = mEfiLocateHandleRequest;
+      break;
+    }
+  }
+  return Handle;
+}
+
+/**
+  Return the first Protocol Interface that matches the Protocol GUID. If
+  Registration is pasased in return a Protocol Instance that was just add
+  to the system. If Retistration is NULL return the first Protocol Interface
+  you find.
+
+  @param  Protocol               The protocol to search for
+  @param  Registration           Optional Registration Key returned from
+                                 RegisterProtocolNotify()
+  @param  Interface              Return the Protocol interface (instance).
+
+  @retval EFI_SUCCESS            If a valid Interface is returned
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Protocol interface not found
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLocateProtocol (
+  IN  EFI_GUID  *Protocol,
+  IN  VOID      *Registration OPTIONAL,
+  OUT VOID      **Interface
+  )
+{
+  EFI_STATUS              Status;
+  LOCATE_POSITION         Position;
+  PROTOCOL_NOTIFY         *ProtNotify;
+  IHANDLE                 *Handle;
+
+  if (Interface == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Protocol == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Interface = NULL;
+  Status = EFI_SUCCESS;
+
+  //
+  // Set initial position
+  //
+  Position.Protocol  = Protocol;
+  Position.SearchKey = Registration;
+  Position.Position  = &gHandleList;
+
+  mEfiLocateHandleRequest += 1;
+
+  if (Registration == NULL) {
+    //
+    // Look up the protocol entry and set the head pointer
+    //
+    Position.ProtEntry = SmmFindProtocolEntry (Protocol, FALSE);
+    if (Position.ProtEntry == NULL) {
+      return EFI_NOT_FOUND;
+    }
+    Position.Position = &Position.ProtEntry->Protocols;
+
+    Handle = SmmGetNextLocateByProtocol (&Position, Interface);
+  } else {
+    Handle = SmmGetNextLocateByRegisterNotify (&Position, Interface);
+  }
+
+  if (Handle == NULL) {
+    Status = EFI_NOT_FOUND;
+  } else if (Registration != NULL) {
+    //
+    // If this is a search by register notify and a handle was
+    // returned, update the register notification position
+    //
+    ProtNotify = Registration;
+    ProtNotify->Position = ProtNotify->Position->ForwardLink;
+  }
+
+  return Status;
+}
+
+/**
+  Locates the requested handle(s) and returns them in Buffer.
+
+  @param  SearchType             The type of search to perform to locate the
+                                 handles
+  @param  Protocol               The protocol to search for
+  @param  SearchKey              Dependant on SearchType
+  @param  BufferSize             On input the size of Buffer.  On output the
+                                 size of data returned.
+  @param  Buffer                 The buffer to return the results in
+
+  @retval EFI_BUFFER_TOO_SMALL   Buffer too small, required buffer size is
+                                 returned in BufferSize.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully found the requested handle(s) and
+                                 returns them in Buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLocateHandle (
+  IN     EFI_LOCATE_SEARCH_TYPE  SearchType,
+  IN     EFI_GUID                *Protocol   OPTIONAL,
+  IN     VOID                    *SearchKey  OPTIONAL,
+  IN OUT UINTN                   *BufferSize,
+  OUT    EFI_HANDLE              *Buffer
+  )
+{
+  EFI_STATUS       Status;
+  LOCATE_POSITION  Position;
+  PROTOCOL_NOTIFY  *ProtNotify;
+  CORE_GET_NEXT    GetNext;
+  UINTN            ResultSize;
+  IHANDLE          *Handle;
+  IHANDLE          **ResultBuffer;
+  VOID             *Interface;
+
+  if (BufferSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*BufferSize > 0) && (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  GetNext = NULL;
+
+  //
+  // Set initial position
+  //
+  Position.Protocol  = Protocol;
+  Position.SearchKey = SearchKey;
+  Position.Position  = &gHandleList;
+
+  ResultSize = 0;
+  ResultBuffer = (IHANDLE **) Buffer;
+  Status = EFI_SUCCESS;
+
+  //
+  // Get the search function based on type
+  //
+  switch (SearchType) {
+  case AllHandles:
+    GetNext = SmmGetNextLocateAllHandles;
+    break;
+
+  case ByRegisterNotify:
+    GetNext = SmmGetNextLocateByRegisterNotify;
+    //
+    // Must have SearchKey for locate ByRegisterNotify
+    //
+    if (SearchKey == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+    }
+    break;
+
+  case ByProtocol:
+    GetNext = SmmGetNextLocateByProtocol;
+    if (Protocol == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      break;
+    }
+    //
+    // Look up the protocol entry and set the head pointer
+    //
+    Position.ProtEntry = SmmFindProtocolEntry (Protocol, FALSE);
+    if (Position.ProtEntry == NULL) {
+      Status = EFI_NOT_FOUND;
+      break;
+    }
+    Position.Position = &Position.ProtEntry->Protocols;
+    break;
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Enumerate out the matching handles
+  //
+  mEfiLocateHandleRequest += 1;
+  for (; ;) {
+    //
+    // Get the next handle.  If no more handles, stop
+    //
+    Handle = GetNext (&Position, &Interface);
+    if (NULL == Handle) {
+      break;
+    }
+
+    //
+    // Increase the resulting buffer size, and if this handle
+    // fits return it
+    //
+    ResultSize += sizeof(Handle);
+    if (ResultSize <= *BufferSize) {
+        *ResultBuffer = Handle;
+        ResultBuffer += 1;
+    }
+  }
+
+  //
+  // If the result is a zero length buffer, then there were no
+  // matching handles
+  //
+  if (ResultSize == 0) {
+    Status = EFI_NOT_FOUND;
+  } else {
+    //
+    // Return the resulting buffer size.  If it's larger than what
+    // was passed, then set the error code
+    //
+    if (ResultSize > *BufferSize) {
+      Status = EFI_BUFFER_TOO_SMALL;
+    }
+
+    *BufferSize = ResultSize;
+
+    if (SearchType == ByRegisterNotify && !EFI_ERROR(Status)) {
+      ASSERT (SearchKey != NULL);
+      //
+      // If this is a search by register notify and a handle was
+      // returned, update the register notification position
+      //
+      ProtNotify = SearchKey;
+      ProtNotify->Position = ProtNotify->Position->ForwardLink;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Function returns an array of handles that support the requested protocol
+  in a buffer allocated from pool. This is a version of SmmLocateHandle()
+  that allocates a buffer for the caller.
+
+  @param  SearchType             Specifies which handle(s) are to be returned.
+  @param  Protocol               Provides the protocol to search by.    This
+                                 parameter is only valid for SearchType
+                                 ByProtocol.
+  @param  SearchKey              Supplies the search key depending on the
+                                 SearchType.
+  @param  NumberHandles          The number of handles returned in Buffer.
+  @param  Buffer                 A pointer to the buffer to return the requested
+                                 array of  handles that support Protocol.
+
+  @retval EFI_SUCCESS            The result array of handles was returned.
+  @retval EFI_NOT_FOUND          No handles match the search.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the
+                                 matching results.
+  @retval EFI_INVALID_PARAMETER  One or more paramters are not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLocateHandleBuffer (
+  IN     EFI_LOCATE_SEARCH_TYPE  SearchType,
+  IN     EFI_GUID                *Protocol OPTIONAL,
+  IN     VOID                    *SearchKey OPTIONAL,
+  IN OUT UINTN                   *NumberHandles,
+  OUT    EFI_HANDLE              **Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       BufferSize;
+
+  if (NumberHandles == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BufferSize = 0;
+  *NumberHandles = 0;
+  *Buffer = NULL;
+  Status = SmmLocateHandle (
+             SearchType,
+             Protocol,
+             SearchKey,
+             &BufferSize,
+             *Buffer
+             );
+  //
+  // LocateHandleBuffer() returns incorrect status code if SearchType is
+  // invalid.
+  //
+  // Add code to correctly handle expected errors from SmmLocateHandle().
+  //
+  if (EFI_ERROR(Status) && Status != EFI_BUFFER_TOO_SMALL) {
+    if (Status != EFI_INVALID_PARAMETER) {
+      Status = EFI_NOT_FOUND;
+    }
+    return Status;
+  }
+
+  *Buffer = AllocatePool (BufferSize);
+  if (*Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = SmmLocateHandle (
+             SearchType,
+             Protocol,
+             SearchKey,
+             &BufferSize,
+             *Buffer
+             );
+
+  *NumberHandles = BufferSize / sizeof(EFI_HANDLE);
+  if (EFI_ERROR(Status)) {
+    *NumberHandles = 0;
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Notify.c b/Core/MdeModulePkg/Core/PiSmmCore/Notify.c
new file mode 100644
index 0000000000..0054fe66aa
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Notify.c
@@ -0,0 +1,202 @@
+/** @file
+  Support functions for UEFI protocol notification infrastructure.
+
+  Copyright (c) 2009 - 2015, 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 "PiSmmCore.h"
+
+/**
+  Signal event for every protocol in protocol entry.
+
+  @param  Prot                   Protocol interface
+
+**/
+VOID
+SmmNotifyProtocol (
+  IN PROTOCOL_INTERFACE  *Prot
+  )
+{
+  PROTOCOL_ENTRY   *ProtEntry;
+  PROTOCOL_NOTIFY  *ProtNotify;
+  LIST_ENTRY       *Link;
+
+  ProtEntry = Prot->Protocol;
+  for (Link=ProtEntry->Notify.ForwardLink; Link != &ProtEntry->Notify; Link=Link->ForwardLink) {
+    ProtNotify = CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+    ProtNotify->Function (&ProtEntry->ProtocolID, Prot->Interface, Prot->Handle);
+  }
+}
+
+/**
+  Removes Protocol from the protocol list (but not the handle list).
+
+  @param  Handle                 The handle to remove protocol on.
+  @param  Protocol               GUID of the protocol to be moved
+  @param  Interface              The interface of the protocol
+
+  @return Protocol Entry
+
+**/
+PROTOCOL_INTERFACE *
+SmmRemoveInterfaceFromProtocol (
+  IN IHANDLE   *Handle,
+  IN EFI_GUID  *Protocol,
+  IN VOID      *Interface
+  )
+{
+  PROTOCOL_INTERFACE  *Prot;
+  PROTOCOL_NOTIFY     *ProtNotify;
+  PROTOCOL_ENTRY      *ProtEntry;
+  LIST_ENTRY          *Link;
+
+  Prot = SmmFindProtocolInterface (Handle, Protocol, Interface);
+  if (Prot != NULL) {
+
+    ProtEntry = Prot->Protocol;
+
+    //
+    // If there's a protocol notify location pointing to this entry, back it up one
+    //
+    for(Link = ProtEntry->Notify.ForwardLink; Link != &ProtEntry->Notify; Link=Link->ForwardLink) {
+      ProtNotify = CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+
+      if (ProtNotify->Position == &Prot->ByProtocol) {
+        ProtNotify->Position = Prot->ByProtocol.BackLink;
+      }
+    }
+
+    //
+    // Remove the protocol interface entry
+    //
+    RemoveEntryList (&Prot->ByProtocol);
+  }
+
+  return Prot;
+}
+
+/**
+  Add a new protocol notification record for the request protocol.
+
+  @param  Protocol               The requested protocol to add the notify
+                                 registration
+  @param  Function               Points to the notification function
+  @param  Registration           Returns the registration record
+
+  @retval EFI_SUCCESS            Successfully returned the registration record
+                                 that has been added or unhooked
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL or Registration is NULL
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory resource to finish the request
+  @retval EFI_NOT_FOUND          If the registration is not found when Function == NULL
+
+**/
+EFI_STATUS
+EFIAPI
+SmmRegisterProtocolNotify (
+  IN  CONST EFI_GUID     *Protocol,
+  IN  EFI_SMM_NOTIFY_FN  Function,
+  OUT VOID               **Registration
+  )
+{
+  PROTOCOL_ENTRY   *ProtEntry;
+  PROTOCOL_NOTIFY  *ProtNotify;
+  LIST_ENTRY       *Link;
+  EFI_STATUS       Status;
+
+  if (Protocol == NULL || Registration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Function == NULL) {
+  	//
+    // Get the protocol entry per Protocol
+    //
+    ProtEntry = SmmFindProtocolEntry ((EFI_GUID *) Protocol, FALSE);
+    if (ProtEntry != NULL) {
+      ProtNotify = (PROTOCOL_NOTIFY * )*Registration;
+      for (Link = ProtEntry->Notify.ForwardLink;
+           Link != &ProtEntry->Notify;
+           Link = Link->ForwardLink) {
+        //
+        // Compare the notification record 
+        //
+        if (ProtNotify == (CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE))){
+          //
+          // If Registration is an existing registration, then unhook it
+          //
+          ProtNotify->Signature = 0;
+          RemoveEntryList (&ProtNotify->Link);
+          FreePool (ProtNotify);
+          return EFI_SUCCESS;
+        }
+      }
+    }
+    //
+    // If the registration is not found
+    //
+    return EFI_NOT_FOUND;
+  } 
+
+  ProtNotify = NULL;
+
+  //
+  // Get the protocol entry to add the notification too
+  //
+  ProtEntry = SmmFindProtocolEntry ((EFI_GUID *) Protocol, TRUE);
+  if (ProtEntry != NULL) {
+    //
+    // Find whether notification already exist
+    //
+    for (Link = ProtEntry->Notify.ForwardLink;
+         Link != &ProtEntry->Notify;
+         Link = Link->ForwardLink) {
+
+      ProtNotify = CR(Link, PROTOCOL_NOTIFY, Link, PROTOCOL_NOTIFY_SIGNATURE);
+      if (CompareGuid (&ProtNotify->Protocol->ProtocolID, Protocol) &&
+          (ProtNotify->Function == Function)) {
+
+        //
+        // Notification already exist
+        //
+        *Registration = ProtNotify;
+
+        return EFI_SUCCESS;
+      }
+    }
+
+    //
+    // Allocate a new notification record
+    //
+    ProtNotify = AllocatePool (sizeof(PROTOCOL_NOTIFY));
+    if (ProtNotify != NULL) {
+      ProtNotify->Signature = PROTOCOL_NOTIFY_SIGNATURE;
+      ProtNotify->Protocol = ProtEntry;
+      ProtNotify->Function = Function;
+      //
+      // Start at the ending
+      //
+      ProtNotify->Position = ProtEntry->Protocols.BackLink;
+
+      InsertTailList (&ProtEntry->Notify, &ProtNotify->Link);
+    }
+  }
+
+  //
+  // Done.  If we have a protocol notify entry, then return it.
+  // Otherwise, we must have run out of resources trying to add one
+  //
+  Status = EFI_OUT_OF_RESOURCES;
+  if (ProtNotify != NULL) {
+    *Registration = ProtNotify;
+    Status = EFI_SUCCESS;
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Page.c b/Core/MdeModulePkg/Core/PiSmmCore/Page.c
new file mode 100644
index 0000000000..9cc2a4cabc
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Page.c
@@ -0,0 +1,384 @@
+/** @file
+  SMM Memory page management functions.
+
+  Copyright (c) 2009 - 2014, 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 "PiSmmCore.h"
+
+#define TRUNCATE_TO_PAGES(a)  ((a) >> EFI_PAGE_SHIFT)
+
+LIST_ENTRY  mSmmMemoryMap = INITIALIZE_LIST_HEAD_VARIABLE (mSmmMemoryMap);
+
+/**
+  Internal Function. Allocate n pages from given free page node.
+
+  @param  Pages                  The free page node.
+  @param  NumberOfPages          Number of pages to be allocated.
+  @param  MaxAddress             Request to allocate memory below this address.
+
+  @return Memory address of allocated pages.
+
+**/
+UINTN
+InternalAllocPagesOnOneNode (
+  IN OUT FREE_PAGE_LIST  *Pages,
+  IN     UINTN           NumberOfPages,
+  IN     UINTN           MaxAddress
+  )
+{
+  UINTN           Top;
+  UINTN           Bottom;
+  FREE_PAGE_LIST  *Node;
+
+  Top = TRUNCATE_TO_PAGES (MaxAddress + 1 - (UINTN)Pages);
+  if (Top > Pages->NumberOfPages) {
+    Top = Pages->NumberOfPages;
+  }
+  Bottom = Top - NumberOfPages;
+
+  if (Top < Pages->NumberOfPages) {
+    Node = (FREE_PAGE_LIST*)((UINTN)Pages + EFI_PAGES_TO_SIZE (Top));
+    Node->NumberOfPages = Pages->NumberOfPages - Top;
+    InsertHeadList (&Pages->Link, &Node->Link);
+  }
+
+  if (Bottom > 0) {
+    Pages->NumberOfPages = Bottom;
+  } else {
+    RemoveEntryList (&Pages->Link);
+  }
+
+  return (UINTN)Pages + EFI_PAGES_TO_SIZE (Bottom);
+}
+
+/**
+  Internal Function. Allocate n pages from free page list below MaxAddress.
+
+  @param  FreePageList           The free page node.
+  @param  NumberOfPages          Number of pages to be allocated.
+  @param  MaxAddress             Request to allocate memory below this address.
+
+  @return Memory address of allocated pages.
+
+**/
+UINTN
+InternalAllocMaxAddress (
+  IN OUT LIST_ENTRY  *FreePageList,
+  IN     UINTN       NumberOfPages,
+  IN     UINTN       MaxAddress
+  )
+{
+  LIST_ENTRY      *Node;
+  FREE_PAGE_LIST  *Pages;
+
+  for (Node = FreePageList->BackLink; Node != FreePageList; Node = Node->BackLink) {
+    Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+    if (Pages->NumberOfPages >= NumberOfPages &&
+        (UINTN)Pages + EFI_PAGES_TO_SIZE (NumberOfPages) - 1 <= MaxAddress) {
+      return InternalAllocPagesOnOneNode (Pages, NumberOfPages, MaxAddress);
+    }
+  }
+  return (UINTN)(-1);
+}
+
+/**
+  Internal Function. Allocate n pages from free page list at given address.
+
+  @param  FreePageList           The free page node.
+  @param  NumberOfPages          Number of pages to be allocated.
+  @param  MaxAddress             Request to allocate memory below this address.
+
+  @return Memory address of allocated pages.
+
+**/
+UINTN
+InternalAllocAddress (
+  IN OUT LIST_ENTRY  *FreePageList,
+  IN     UINTN       NumberOfPages,
+  IN     UINTN       Address
+  )
+{
+  UINTN           EndAddress;
+  LIST_ENTRY      *Node;
+  FREE_PAGE_LIST  *Pages;
+
+  if ((Address & EFI_PAGE_MASK) != 0) {
+    return ~Address;
+  }
+
+  EndAddress = Address + EFI_PAGES_TO_SIZE (NumberOfPages);
+  for (Node = FreePageList->BackLink; Node!= FreePageList; Node = Node->BackLink) {
+    Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+    if ((UINTN)Pages <= Address) {
+      if ((UINTN)Pages + EFI_PAGES_TO_SIZE (Pages->NumberOfPages) < EndAddress) {
+        break;
+      }
+      return InternalAllocPagesOnOneNode (Pages, NumberOfPages, EndAddress);
+    }
+  }
+  return ~Address;
+}
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform.
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into.
+  @param  NumberOfPages          The number of pages to allocate.
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address.
+
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalAllocatePages (
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 NumberOfPages,
+  OUT EFI_PHYSICAL_ADDRESS  *Memory
+  )
+{
+  UINTN  RequestedAddress;
+
+  if (MemoryType != EfiRuntimeServicesCode &&
+      MemoryType != EfiRuntimeServicesData) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NumberOfPages > TRUNCATE_TO_PAGES ((UINTN)-1) + 1) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // We don't track memory type in SMM
+  //
+  RequestedAddress = (UINTN)*Memory;
+  switch (Type) {
+    case AllocateAnyPages:
+      RequestedAddress = (UINTN)(-1);
+    case AllocateMaxAddress:
+      *Memory = InternalAllocMaxAddress (
+                  &mSmmMemoryMap,
+                  NumberOfPages,
+                  RequestedAddress
+                  );
+      if (*Memory == (UINTN)-1) {
+        return EFI_OUT_OF_RESOURCES;
+      } 
+      break;
+    case AllocateAddress:
+      *Memory = InternalAllocAddress (
+                  &mSmmMemoryMap,
+                  NumberOfPages,
+                  RequestedAddress
+                  );
+      if (*Memory != RequestedAddress) {
+        return EFI_NOT_FOUND;
+      }
+      break;
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform.
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into.
+  @param  NumberOfPages          The number of pages to allocate.
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address.
+
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePages (
+  IN  EFI_ALLOCATE_TYPE     Type,
+  IN  EFI_MEMORY_TYPE       MemoryType,
+  IN  UINTN                 NumberOfPages,
+  OUT EFI_PHYSICAL_ADDRESS  *Memory
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = SmmInternalAllocatePages (Type, MemoryType, NumberOfPages, Memory);
+  if (!EFI_ERROR (Status)) {
+    SmmCoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionAllocatePages, MemoryType, EFI_PAGES_TO_SIZE (NumberOfPages), (VOID *) (UINTN) *Memory);
+  }
+  return Status;
+}
+
+/**
+  Internal Function. Merge two adjacent nodes.
+
+  @param  First             The first of two nodes to merge.
+
+  @return Pointer to node after merge (if success) or pointer to next node (if fail).
+
+**/
+FREE_PAGE_LIST *
+InternalMergeNodes (
+  IN FREE_PAGE_LIST  *First
+  )
+{
+  FREE_PAGE_LIST  *Next;
+
+  Next = BASE_CR (First->Link.ForwardLink, FREE_PAGE_LIST, Link);
+  ASSERT (
+    TRUNCATE_TO_PAGES ((UINTN)Next - (UINTN)First) >= First->NumberOfPages);
+
+  if (TRUNCATE_TO_PAGES ((UINTN)Next - (UINTN)First) == First->NumberOfPages) {
+    First->NumberOfPages += Next->NumberOfPages;
+    RemoveEntryList (&Next->Link);
+    Next = First;
+  }
+  return Next;
+}
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed.
+  @param  NumberOfPages          The number of pages to free.
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range.
+  @retval EFI_INVALID_PARAMETER  Address not aligned.
+  @return EFI_SUCCESS            Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalFreePages (
+  IN EFI_PHYSICAL_ADDRESS  Memory,
+  IN UINTN                 NumberOfPages
+  )
+{
+  LIST_ENTRY      *Node;
+  FREE_PAGE_LIST  *Pages;
+
+  if ((Memory & EFI_PAGE_MASK) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Pages = NULL;
+  Node = mSmmMemoryMap.ForwardLink;
+  while (Node != &mSmmMemoryMap) {
+    Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+    if (Memory < (UINTN)Pages) {
+      break;
+    }
+    Node = Node->ForwardLink;
+  }
+
+  if (Node != &mSmmMemoryMap &&
+      Memory + EFI_PAGES_TO_SIZE (NumberOfPages) > (UINTN)Pages) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Node->BackLink != &mSmmMemoryMap) {
+    Pages = BASE_CR (Node->BackLink, FREE_PAGE_LIST, Link);
+    if ((UINTN)Pages + EFI_PAGES_TO_SIZE (Pages->NumberOfPages) > Memory) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  Pages = (FREE_PAGE_LIST*)(UINTN)Memory;
+  Pages->NumberOfPages = NumberOfPages;
+  InsertTailList (Node, &Pages->Link);
+
+  if (Pages->Link.BackLink != &mSmmMemoryMap) {
+    Pages = InternalMergeNodes (
+              BASE_CR (Pages->Link.BackLink, FREE_PAGE_LIST, Link)
+              );
+  }
+
+  if (Node != &mSmmMemoryMap) {
+    InternalMergeNodes (Pages);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed.
+  @param  NumberOfPages          The number of pages to free.
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range.
+  @retval EFI_INVALID_PARAMETER  Address not aligned.
+  @return EFI_SUCCESS            Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePages (
+  IN EFI_PHYSICAL_ADDRESS  Memory,
+  IN UINTN                 NumberOfPages
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = SmmInternalFreePages (Memory, NumberOfPages);
+  if (!EFI_ERROR (Status)) {
+    SmmCoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionFreePages, 0, EFI_PAGES_TO_SIZE (NumberOfPages), (VOID *) (UINTN) Memory);
+  }
+  return Status;
+}
+
+/**
+  Add free SMRAM region for use by memory service.
+
+  @param  MemBase                Base address of memory region.
+  @param  MemLength              Length of the memory region.
+  @param  Type                   Memory type.
+  @param  Attributes             Memory region state.
+
+**/
+VOID
+SmmAddMemoryRegion (
+  IN  EFI_PHYSICAL_ADDRESS  MemBase,
+  IN  UINT64                MemLength,
+  IN  EFI_MEMORY_TYPE       Type,
+  IN  UINT64                Attributes
+  )
+{
+  UINTN  AlignedMemBase;
+
+  //
+  // Do not add memory regions that is already allocated, needs testing, or needs ECC initialization
+  //
+  if ((Attributes & (EFI_ALLOCATED | EFI_NEEDS_TESTING | EFI_NEEDS_ECC_INITIALIZATION)) != 0) {
+    return;
+  }
+  
+  //
+  // Align range on an EFI_PAGE_SIZE boundary
+  //  
+  AlignedMemBase = (UINTN)(MemBase + EFI_PAGE_MASK) & ~EFI_PAGE_MASK;
+  MemLength -= AlignedMemBase - MemBase;
+  SmmFreePages (AlignedMemBase, TRUNCATE_TO_PAGES ((UINTN)MemLength));
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.c b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.c
new file mode 100644
index 0000000000..aaa7efd8e0
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.c
@@ -0,0 +1,635 @@
+/** @file
+  SMM Core Main Entry Point
+
+  Copyright (c) 2009 - 2015, 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 "PiSmmCore.h"
+
+//
+// Physical pointer to private structure shared between SMM IPL and the SMM Core
+//
+SMM_CORE_PRIVATE_DATA  *gSmmCorePrivate;
+
+//
+// SMM Core global variable for SMM System Table.  Only accessed as a physical structure in SMRAM.
+//
+EFI_SMM_SYSTEM_TABLE2  gSmmCoreSmst = {
+  {
+    SMM_SMST_SIGNATURE,
+    EFI_SMM_SYSTEM_TABLE2_REVISION,
+    sizeof (gSmmCoreSmst.Hdr)
+  },
+  NULL,                          // SmmFirmwareVendor
+  0,                             // SmmFirmwareRevision
+  SmmInstallConfigurationTable,
+  {
+    {
+      (EFI_SMM_CPU_IO2) SmmEfiNotAvailableYetArg5,       // SmmMemRead
+      (EFI_SMM_CPU_IO2) SmmEfiNotAvailableYetArg5        // SmmMemWrite
+    },
+    {
+      (EFI_SMM_CPU_IO2) SmmEfiNotAvailableYetArg5,       // SmmIoRead
+      (EFI_SMM_CPU_IO2) SmmEfiNotAvailableYetArg5        // SmmIoWrite
+    }
+  },
+  SmmAllocatePool,
+  SmmFreePool,
+  SmmAllocatePages,
+  SmmFreePages,
+  NULL,                          // SmmStartupThisAp
+  0,                             // CurrentlyExecutingCpu
+  0,                             // NumberOfCpus
+  NULL,                          // CpuSaveStateSize
+  NULL,                          // CpuSaveState
+  0,                             // NumberOfTableEntries
+  NULL,                          // SmmConfigurationTable
+  SmmInstallProtocolInterface,
+  SmmUninstallProtocolInterface,
+  SmmHandleProtocol,
+  SmmRegisterProtocolNotify,
+  SmmLocateHandle,
+  SmmLocateProtocol,
+  SmiManage,
+  SmiHandlerRegister,
+  SmiHandlerUnRegister
+};
+
+//
+// Flag to determine if the platform has performed a legacy boot.
+// If this flag is TRUE, then the runtime code and runtime data associated with the 
+// SMM IPL are converted to free memory, so the SMM Core must guarantee that is
+// does not touch of the code/data associated with the SMM IPL if this flag is TRUE.
+//
+BOOLEAN  mInLegacyBoot = FALSE;
+
+//
+// Table of SMI Handlers that are registered by the SMM Core when it is initialized
+//
+SMM_CORE_SMI_HANDLERS  mSmmCoreSmiHandlers[] = {
+  { SmmDriverDispatchHandler,   &gEfiEventDxeDispatchGuid,          NULL, TRUE  },
+  { SmmReadyToLockHandler,      &gEfiDxeSmmReadyToLockProtocolGuid, NULL, TRUE }, 
+  { SmmLegacyBootHandler,       &gEfiEventLegacyBootGuid,           NULL, FALSE },
+  { SmmExitBootServicesHandler, &gEfiEventExitBootServicesGuid,     NULL, FALSE },
+  { SmmReadyToBootHandler,      &gEfiEventReadyToBootGuid,          NULL, FALSE },
+  { SmmEndOfDxeHandler,         &gEfiEndOfDxeEventGroupGuid,        NULL, TRUE },
+  { NULL,                       NULL,                               NULL, FALSE }
+};
+
+UINTN                           mFullSmramRangeCount;
+EFI_SMRAM_DESCRIPTOR            *mFullSmramRanges;
+
+EFI_LOADED_IMAGE_PROTOCOL       *mSmmCoreLoadedImage;
+
+/**
+  Place holder function until all the SMM System Table Service are available.
+
+  Note: This function is only used by SMRAM invocation.  It is never used by DXE invocation.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+  @param  Arg5                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEfiNotAvailableYetArg5 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4,
+  UINTN Arg5
+  )
+{
+  //
+  // This function should never be executed.  If it does, then the architectural protocols
+  // have not been designed correctly.
+  //
+  return EFI_NOT_AVAILABLE_YET;
+}
+
+/**
+  Software SMI handler that is called when a Legacy Boot event is signalled.  The SMM
+  Core uses this signal to know that a Legacy Boot has been performed and that 
+  gSmmCorePrivate that is shared between the UEFI and SMM execution environments can
+  not be accessed from SMM anymore since that structure is considered free memory by
+  a legacy OS. Then the SMM Core also install SMM Legacy Boot protocol to notify SMM
+  driver that system enter legacy boot.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLegacyBootHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+  EFI_HANDLE    SmmHandle;
+
+  //
+  // Install SMM Legacy Boot protocol.
+  //
+  SmmHandle = NULL;
+  Status = SmmInstallProtocolInterface (
+             &SmmHandle,
+             &gEdkiiSmmLegacyBootProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             NULL
+             );
+
+  mInLegacyBoot = TRUE;
+
+  SmiHandlerUnRegister (DispatchHandle);
+
+  return Status;
+}
+
+/**
+  Software SMI handler that is called when an Exit Boot Services event is signalled.
+  Then the SMM Core also install SMM Exit Boot Services protocol to notify SMM driver
+  that system enter exit boot services.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmExitBootServicesHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+  EFI_HANDLE    SmmHandle;
+
+  //
+  // Install SMM Exit Boot Services protocol.
+  //
+  SmmHandle = NULL;
+  Status = SmmInstallProtocolInterface (
+             &SmmHandle,
+             &gEdkiiSmmExitBootServicesProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             NULL
+             );
+
+  SmiHandlerUnRegister (DispatchHandle);
+
+  return Status;
+}
+
+/**
+  Software SMI handler that is called when an Ready To Boot event is signalled.
+  Then the SMM Core also install SMM Ready To Boot protocol to notify SMM driver
+  that system enter ready to boot.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmReadyToBootHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+  EFI_HANDLE    SmmHandle;
+
+  //
+  // Install SMM Ready To Boot protocol.
+  //
+  SmmHandle = NULL;
+  Status = SmmInstallProtocolInterface (
+             &SmmHandle,
+             &gEdkiiSmmReadyToBootProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             NULL
+             );
+
+  SmiHandlerUnRegister (DispatchHandle);
+
+  return Status;
+}
+
+/**
+  Software SMI handler that is called when the DxeSmmReadyToLock protocol is added
+  or if gEfiEventReadyToBootGuid is signalled.  This function unregisters the 
+  Software SMIs that are nor required after SMRAM is locked and installs the 
+  SMM Ready To Lock Protocol so SMM Drivers are informed that SMRAM is about 
+  to be locked.  It also verifies the the SMM CPU I/O 2 Protocol has been installed
+  and NULLs gBS and gST because they can not longer be used after SMRAM is locked.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmReadyToLockHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  EFI_HANDLE  SmmHandle;
+  VOID        *Interface;
+
+  //
+  // Unregister SMI Handlers that are no required after the SMM driver dispatch is stopped
+  //
+  for (Index = 0; mSmmCoreSmiHandlers[Index].HandlerType != NULL; Index++) {
+    if (mSmmCoreSmiHandlers[Index].UnRegister) {
+      SmiHandlerUnRegister (mSmmCoreSmiHandlers[Index].DispatchHandle);
+    }
+  }
+
+  //
+  // Install SMM Ready to lock protocol
+  //
+  SmmHandle = NULL;
+  Status = SmmInstallProtocolInterface (
+             &SmmHandle,
+             &gEfiSmmReadyToLockProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             NULL
+             );
+
+  //
+  // Make sure SMM CPU I/O 2 Procol has been installed into the handle database
+  //
+  Status = SmmLocateProtocol (&gEfiSmmCpuIo2ProtocolGuid, NULL, &Interface);
+
+  //
+  // Print a message on a debug build if the SMM CPU I/O 2 Protocol is not installed
+  //
+  DEBUG_CODE_BEGIN ();
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "\nSMM: SmmCpuIo Arch Protocol not present!!\n"));
+    }
+  DEBUG_CODE_END ();
+
+  //
+  // Assert if the CPU I/O 2 Protocol is not installed
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Display any drivers that were not dispatched because dependency expression
+  // evaluated to false if this is a debug build
+  //
+  DEBUG_CODE_BEGIN ();
+    SmmDisplayDiscoveredNotDispatched ();
+  DEBUG_CODE_END ();
+
+  //
+  // Not allowed to use gST or gBS after lock
+  //
+  gST = NULL;
+  gBS = NULL;
+
+  SmramProfileReadyToLock ();
+
+  return Status;
+}
+
+/**
+  Software SMI handler that is called when the EndOfDxe event is signalled.
+  This function installs the SMM EndOfDxe Protocol so SMM Drivers are informed that
+  platform code will invoke 3rd part code.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEndOfDxeHandler (
+  IN     EFI_HANDLE  DispatchHandle,
+  IN     CONST VOID  *Context,        OPTIONAL
+  IN OUT VOID        *CommBuffer,     OPTIONAL
+  IN OUT UINTN       *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  SmmHandle;
+
+  DEBUG ((EFI_D_INFO, "SmmEndOfDxeHandler\n"));
+  //
+  // Install SMM EndOfDxe protocol
+  //
+  SmmHandle = NULL;
+  Status = SmmInstallProtocolInterface (
+             &SmmHandle,
+             &gEfiSmmEndOfDxeProtocolGuid,
+             EFI_NATIVE_INTERFACE,
+             NULL
+             );
+  return Status;
+}
+
+/**
+  Determine if two buffers overlap in memory.
+
+  @param[in] Buff1  Pointer to first buffer
+  @param[in] Size1  Size of Buff1
+  @param[in] Buff2  Pointer to second buffer
+  @param[in] Size2  Size of Buff2
+
+  @retval TRUE      Buffers overlap in memory.
+  @retval FALSE     Buffer doesn't overlap.
+
+**/
+BOOLEAN
+InternalIsBufferOverlapped (
+  IN UINT8      *Buff1,
+  IN UINTN      Size1,
+  IN UINT8      *Buff2,
+  IN UINTN      Size2
+  )
+{
+  //
+  // If buff1's end is less than the start of buff2, then it's ok.
+  // Also, if buff1's start is beyond buff2's end, then it's ok.
+  //
+  if (((Buff1 + Size1) <= Buff2) || (Buff1 >= (Buff2 + Size2))) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  The main entry point to SMM Foundation.
+
+  Note: This function is only used by SMRAM invocation.  It is never used by DXE invocation.
+
+  @param  SmmEntryContext           Processor information and functionality
+                                    needed by SMM Foundation.
+
+**/
+VOID
+EFIAPI
+SmmEntryPoint (
+  IN CONST EFI_SMM_ENTRY_CONTEXT  *SmmEntryContext
+)
+{
+  EFI_STATUS                  Status;
+  EFI_SMM_COMMUNICATE_HEADER  *CommunicateHeader;
+  BOOLEAN                     InLegacyBoot;
+  BOOLEAN                     IsOverlapped;
+
+  PERF_START (NULL, "SMM", NULL, 0) ;
+
+  //
+  // Update SMST using the context
+  //
+  CopyMem (&gSmmCoreSmst.SmmStartupThisAp, SmmEntryContext, sizeof (EFI_SMM_ENTRY_CONTEXT));
+
+  //
+  // Call platform hook before Smm Dispatch
+  //
+  PlatformHookBeforeSmmDispatch ();
+
+  //
+  // If a legacy boot has occured, then make sure gSmmCorePrivate is not accessed
+  //
+  InLegacyBoot = mInLegacyBoot;
+  if (!InLegacyBoot) {
+    //
+    // Mark the InSmm flag as TRUE, it will be used by SmmBase2 protocol
+    //
+    gSmmCorePrivate->InSmm = TRUE;
+
+    //
+    // Check to see if this is a Synchronous SMI sent through the SMM Communication 
+    // Protocol or an Asynchronous SMI
+    //
+    if (gSmmCorePrivate->CommunicationBuffer != NULL) {
+      //
+      // Synchronous SMI for SMM Core or request from Communicate protocol
+      //
+      IsOverlapped = InternalIsBufferOverlapped (
+                       (UINT8 *) gSmmCorePrivate->CommunicationBuffer,
+                       gSmmCorePrivate->BufferSize,
+                       (UINT8 *) gSmmCorePrivate,
+                       sizeof (*gSmmCorePrivate)
+                       );
+      if (!SmmIsBufferOutsideSmmValid ((UINTN)gSmmCorePrivate->CommunicationBuffer, gSmmCorePrivate->BufferSize) || IsOverlapped) {
+        //
+        // If CommunicationBuffer is not in valid address scope,
+        // or there is overlap between gSmmCorePrivate and CommunicationBuffer,
+        // return EFI_INVALID_PARAMETER
+        //
+        gSmmCorePrivate->CommunicationBuffer = NULL;
+        gSmmCorePrivate->ReturnStatus = EFI_INVALID_PARAMETER;
+      } else {
+        CommunicateHeader = (EFI_SMM_COMMUNICATE_HEADER *)gSmmCorePrivate->CommunicationBuffer;
+        gSmmCorePrivate->BufferSize -= OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data);
+        Status = SmiManage (
+                   &CommunicateHeader->HeaderGuid, 
+                   NULL, 
+                   CommunicateHeader->Data, 
+                   &gSmmCorePrivate->BufferSize
+                   );
+        //
+        // Update CommunicationBuffer, BufferSize and ReturnStatus
+        // Communicate service finished, reset the pointer to CommBuffer to NULL
+        //
+        gSmmCorePrivate->BufferSize += OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data);
+        gSmmCorePrivate->CommunicationBuffer = NULL;
+        gSmmCorePrivate->ReturnStatus = (Status == EFI_SUCCESS) ? EFI_SUCCESS : EFI_NOT_FOUND;
+      }
+    }
+  }
+
+  //
+  // Process Asynchronous SMI sources
+  //
+  SmiManage (NULL, NULL, NULL, NULL);
+  
+  //
+  // Call platform hook after Smm Dispatch
+  //
+  PlatformHookAfterSmmDispatch ();
+
+  //
+  // If a legacy boot has occured, then make sure gSmmCorePrivate is not accessed
+  //
+  if (!InLegacyBoot) {
+    //
+    // Clear the InSmm flag as we are going to leave SMM
+    //
+    gSmmCorePrivate->InSmm = FALSE;
+  }
+
+  PERF_END (NULL, "SMM", NULL, 0) ;
+}
+
+/**
+  Install LoadedImage protocol for SMM Core.
+**/
+VOID
+SmmCoreInstallLoadedImage (
+  VOID
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_HANDLE                 Handle;
+
+  //
+  // Allocate a Loaded Image Protocol in EfiBootServicesData
+  //
+  Status = gBS->AllocatePool (EfiBootServicesData, sizeof(EFI_LOADED_IMAGE_PROTOCOL), (VOID **)&mSmmCoreLoadedImage);
+  ASSERT_EFI_ERROR (Status);
+
+  ZeroMem (mSmmCoreLoadedImage, sizeof (EFI_LOADED_IMAGE_PROTOCOL));
+  //
+  // Fill in the remaining fields of the Loaded Image Protocol instance.
+  // Note: ImageBase is an SMRAM address that can not be accessed outside of SMRAM if SMRAM window is closed.
+  //
+  mSmmCoreLoadedImage->Revision      = EFI_LOADED_IMAGE_PROTOCOL_REVISION;
+  mSmmCoreLoadedImage->ParentHandle  = gSmmCorePrivate->SmmIplImageHandle;
+  mSmmCoreLoadedImage->SystemTable   = gST;
+
+  mSmmCoreLoadedImage->ImageBase     = (VOID *)(UINTN)gSmmCorePrivate->PiSmmCoreImageBase;
+  mSmmCoreLoadedImage->ImageSize     = gSmmCorePrivate->PiSmmCoreImageSize;
+  mSmmCoreLoadedImage->ImageCodeType = EfiRuntimeServicesCode;
+  mSmmCoreLoadedImage->ImageDataType = EfiRuntimeServicesData;
+
+  //
+  // Create a new image handle in the UEFI handle database for the SMM Driver
+  //
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiLoadedImageProtocolGuid, mSmmCoreLoadedImage,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return ;
+}
+
+/**
+  The Entry Point for SMM Core
+
+  Install DXE Protocols and reload SMM Core into SMRAM and register SMM Core 
+  EntryPoint on the SMI vector.
+
+  Note: This function is called for both DXE invocation and SMRAM invocation.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurred when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmMain (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+
+  //
+  // Get SMM Core Private context passed in from SMM IPL in ImageHandle.
+  //
+  gSmmCorePrivate = (SMM_CORE_PRIVATE_DATA *)ImageHandle;
+
+  //
+  // Fill in SMRAM physical address for the SMM Services Table and the SMM Entry Point.
+  //
+  gSmmCorePrivate->Smst          = &gSmmCoreSmst;
+  gSmmCorePrivate->SmmEntryPoint = SmmEntryPoint;
+  
+  //
+  // No need to initialize memory service.
+  // It is done in constructor of PiSmmCoreMemoryAllocationLib(),
+  // so that the library linked with PiSmmCore can use AllocatePool() in constuctor.
+  //
+
+  SmramProfileInit ();
+
+  //
+  // Copy FullSmramRanges to SMRAM
+  //
+  mFullSmramRangeCount = gSmmCorePrivate->SmramRangeCount;
+  mFullSmramRanges = AllocatePool (mFullSmramRangeCount * sizeof (EFI_SMRAM_DESCRIPTOR));
+  ASSERT (mFullSmramRanges != NULL);
+  CopyMem (mFullSmramRanges, gSmmCorePrivate->SmramRanges, mFullSmramRangeCount * sizeof (EFI_SMRAM_DESCRIPTOR));
+
+  //
+  // Register all SMI Handlers required by the SMM Core
+  //
+  for (Index = 0; mSmmCoreSmiHandlers[Index].HandlerType != NULL; Index++) {
+    Status = SmiHandlerRegister (
+               mSmmCoreSmiHandlers[Index].Handler,
+               mSmmCoreSmiHandlers[Index].HandlerType,
+               &mSmmCoreSmiHandlers[Index].DispatchHandle
+               );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  RegisterSmramProfileHandler ();
+
+  SmmCoreInstallLoadedImage ();
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.h b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.h
new file mode 100644
index 0000000000..0e9c92abef
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.h
@@ -0,0 +1,1008 @@
+/** @file
+  The internal header file includes the common header files, defines
+  internal structure and functions used by SmmCore module.
+
+  Copyright (c) 2009 - 2015, 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.             
+
+**/
+
+#ifndef _SMM_CORE_H_
+#define _SMM_CORE_H_
+
+#include <PiSmm.h>
+
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <Protocol/SmmReadyToLock.h>
+#include <Protocol/SmmEndOfDxe.h>
+#include <Protocol/CpuIo2.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/SmmAccess2.h>
+#include <Protocol/FirmwareVolume2.h>   
+#include <Protocol/LoadedImage.h>       
+#include <Protocol/DevicePath.h>        
+#include <Protocol/Security.h>          
+#include <Protocol/Security2.h>
+#include <Protocol/SmmExitBootServices.h>
+#include <Protocol/SmmLegacyBoot.h>
+#include <Protocol/SmmReadyToBoot.h>
+
+#include <Guid/Apriori.h>
+#include <Guid/EventGroup.h>
+#include <Guid/EventLegacyBios.h>
+#include <Guid/ZeroGuid.h>
+#include <Guid/MemoryProfile.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>             
+#include <Library/UefiLib.h>                   
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/PcdLib.h>
+#include <Library/SmmCorePlatformHookLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/TimerLib.h>
+#include <Library/HobLib.h>
+#include <Library/SmmMemLib.h>
+
+#include "PiSmmCorePrivateData.h"
+
+//
+// Used to build a table of SMI Handlers that the SMM Core registers
+//
+typedef struct {
+  EFI_SMM_HANDLER_ENTRY_POINT2  Handler;
+  EFI_GUID                      *HandlerType;
+  EFI_HANDLE                    DispatchHandle;
+  BOOLEAN                       UnRegister;
+} SMM_CORE_SMI_HANDLERS;
+
+//
+// Structure for recording the state of an SMM Driver
+//
+#define EFI_SMM_DRIVER_ENTRY_SIGNATURE SIGNATURE_32('s', 'd','r','v')
+
+typedef struct {
+  UINTN                           Signature;
+  LIST_ENTRY                      Link;             // mDriverList
+
+  LIST_ENTRY                      ScheduledLink;    // mScheduledQueue
+
+  EFI_HANDLE                      FvHandle;
+  EFI_GUID                        FileName;
+  EFI_DEVICE_PATH_PROTOCOL        *FvFileDevicePath;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL   *Fv;
+
+  VOID                            *Depex;
+  UINTN                           DepexSize;
+
+  BOOLEAN                         Before;
+  BOOLEAN                         After;
+  EFI_GUID                        BeforeAfterGuid;
+
+  BOOLEAN                         Dependent;
+  BOOLEAN                         Scheduled;
+  BOOLEAN                         Initialized;
+  BOOLEAN                         DepexProtocolError;
+
+  EFI_HANDLE                      ImageHandle;
+  EFI_LOADED_IMAGE_PROTOCOL       *LoadedImage;
+  //
+  // Image EntryPoint in SMRAM
+  //
+  PHYSICAL_ADDRESS                ImageEntryPoint;
+  //
+  // Image Buffer in SMRAM  
+  //
+  PHYSICAL_ADDRESS                ImageBuffer;
+  //
+  // Image Page Number
+  //
+  UINTN                           NumberOfPage;
+} EFI_SMM_DRIVER_ENTRY;
+
+#define EFI_HANDLE_SIGNATURE            SIGNATURE_32('h','n','d','l')
+
+///
+/// IHANDLE - contains a list of protocol handles
+///
+typedef struct {
+  UINTN               Signature;
+  /// All handles list of IHANDLE
+  LIST_ENTRY          AllHandles;
+  /// List of PROTOCOL_INTERFACE's for this handle
+  LIST_ENTRY          Protocols;
+  UINTN               LocateRequest;
+} IHANDLE;
+
+#define ASSERT_IS_HANDLE(a)  ASSERT((a)->Signature == EFI_HANDLE_SIGNATURE)
+
+#define PROTOCOL_ENTRY_SIGNATURE        SIGNATURE_32('p','r','t','e')
+
+///
+/// PROTOCOL_ENTRY - each different protocol has 1 entry in the protocol
+/// database.  Each handler that supports this protocol is listed, along
+/// with a list of registered notifies.
+///
+typedef struct {
+  UINTN               Signature;
+  /// Link Entry inserted to mProtocolDatabase
+  LIST_ENTRY          AllEntries;
+  /// ID of the protocol
+  EFI_GUID            ProtocolID;
+  /// All protocol interfaces
+  LIST_ENTRY          Protocols;
+  /// Registerd notification handlers
+  LIST_ENTRY          Notify;
+} PROTOCOL_ENTRY;
+
+#define PROTOCOL_INTERFACE_SIGNATURE  SIGNATURE_32('p','i','f','c')
+
+///
+/// PROTOCOL_INTERFACE - each protocol installed on a handle is tracked
+/// with a protocol interface structure
+///
+typedef struct {
+  UINTN                       Signature;
+  /// Link on IHANDLE.Protocols
+  LIST_ENTRY                  Link;
+  /// Back pointer
+  IHANDLE                     *Handle;
+  /// Link on PROTOCOL_ENTRY.Protocols
+  LIST_ENTRY                  ByProtocol;
+  /// The protocol ID
+  PROTOCOL_ENTRY              *Protocol;
+  /// The interface value
+  VOID                        *Interface;
+} PROTOCOL_INTERFACE;
+
+#define PROTOCOL_NOTIFY_SIGNATURE       SIGNATURE_32('p','r','t','n')
+
+///
+/// PROTOCOL_NOTIFY - used for each register notification for a protocol
+///
+typedef struct {
+  UINTN               Signature;
+  PROTOCOL_ENTRY      *Protocol;
+  /// All notifications for this protocol
+  LIST_ENTRY          Link;
+  /// Notification function
+  EFI_SMM_NOTIFY_FN   Function;
+  /// Last position notified
+  LIST_ENTRY          *Position;
+} PROTOCOL_NOTIFY;
+
+//
+// SMM Core Global Variables
+//
+extern SMM_CORE_PRIVATE_DATA  *gSmmCorePrivate;
+extern EFI_SMM_SYSTEM_TABLE2  gSmmCoreSmst;
+extern LIST_ENTRY             gHandleList;
+extern EFI_PHYSICAL_ADDRESS   gLoadModuleAtFixAddressSmramBase;
+
+/**
+  Called to initialize the memory service.
+
+  @param   SmramRangeCount       Number of SMRAM Regions
+  @param   SmramRanges           Pointer to SMRAM Descriptors
+
+**/
+VOID
+SmmInitializeMemoryServices (
+  IN UINTN                 SmramRangeCount,
+  IN EFI_SMRAM_DESCRIPTOR  *SmramRanges
+  );
+
+/**
+  The SmmInstallConfigurationTable() function is used to maintain the list
+  of configuration tables that are stored in the System Management System
+  Table.  The list is stored as an array of (GUID, Pointer) pairs.  The list
+  must be allocated from pool memory with PoolType set to EfiRuntimeServicesData.
+
+  @param  SystemTable      A pointer to the SMM System Table (SMST).
+  @param  Guid             A pointer to the GUID for the entry to add, update, or remove.
+  @param  Table            A pointer to the buffer of the table to add.
+  @param  TableSize        The size of the table to install.
+
+  @retval EFI_SUCCESS           The (Guid, Table) pair was added, updated, or removed.
+  @retval EFI_INVALID_PARAMETER Guid is not valid.
+  @retval EFI_NOT_FOUND         An attempt was made to delete a non-existent entry.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory available to complete the operation.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInstallConfigurationTable (
+  IN  CONST EFI_SMM_SYSTEM_TABLE2  *SystemTable,
+  IN  CONST EFI_GUID              *Guid,
+  IN  VOID                        *Table,
+  IN  UINTN                       TableSize
+  );
+
+/**
+  Wrapper function to SmmInstallProtocolInterfaceNotify.  This is the public API which
+  Calls the private one which contains a BOOLEAN parameter for notifications
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+
+  @return Status code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInstallProtocolInterface (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface
+  );
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePages (
+  IN      EFI_ALLOCATE_TYPE         Type,
+  IN      EFI_MEMORY_TYPE           MemoryType,
+  IN      UINTN                     NumberOfPages,
+  OUT     EFI_PHYSICAL_ADDRESS      *Memory
+  );
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalAllocatePages (
+  IN      EFI_ALLOCATE_TYPE         Type,
+  IN      EFI_MEMORY_TYPE           MemoryType,
+  IN      UINTN                     NumberOfPages,
+  OUT     EFI_PHYSICAL_ADDRESS      *Memory
+  );
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS            Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePages (
+  IN      EFI_PHYSICAL_ADDRESS      Memory,
+  IN      UINTN                     NumberOfPages
+  );
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS            Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalFreePages (
+  IN      EFI_PHYSICAL_ADDRESS      Memory,
+  IN      UINTN                     NumberOfPages
+  );
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePool (
+  IN      EFI_MEMORY_TYPE           PoolType,
+  IN      UINTN                     Size,
+  OUT     VOID                      **Buffer
+  );
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalAllocatePool (
+  IN      EFI_MEMORY_TYPE           PoolType,
+  IN      UINTN                     Size,
+  OUT     VOID                      **Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePool (
+  IN      VOID                      *Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalFreePool (
+  IN      VOID                      *Buffer
+  );
+
+/**
+  Installs a protocol interface into the boot services environment.
+
+  @param  UserHandle             The handle to install the protocol handler on,
+                                 or NULL if a new handle is to be allocated
+  @param  Protocol               The protocol to add to the handle
+  @param  InterfaceType          Indicates whether Interface is supplied in
+                                 native form.
+  @param  Interface              The interface for the protocol being added
+  @param  Notify                 indicates whether notify the notification list
+                                 for this protocol
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_OUT_OF_RESOURCES   No enough buffer to allocate
+  @retval EFI_SUCCESS            Protocol interface successfully installed
+
+**/
+EFI_STATUS
+SmmInstallProtocolInterfaceNotify (
+  IN OUT EFI_HANDLE     *UserHandle,
+  IN EFI_GUID           *Protocol,
+  IN EFI_INTERFACE_TYPE InterfaceType,
+  IN VOID               *Interface,
+  IN BOOLEAN            Notify
+  );
+
+/**
+  Uninstalls all instances of a protocol:interfacer from a handle.
+  If the last protocol interface is remove from the handle, the
+  handle is freed.
+
+  @param  UserHandle             The handle to remove the protocol handler from
+  @param  Protocol               The protocol, of protocol:interface, to remove
+  @param  Interface              The interface, of protocol:interface, to remove
+
+  @retval EFI_INVALID_PARAMETER  Protocol is NULL.
+  @retval EFI_SUCCESS            Protocol interface successfully uninstalled.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmUninstallProtocolInterface (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  IN VOID             *Interface
+  );
+
+/**
+  Queries a handle to determine if it supports a specified protocol.
+
+  @param  UserHandle             The handle being queried.
+  @param  Protocol               The published unique identifier of the protocol.
+  @param  Interface              Supplies the address where a pointer to the
+                                 corresponding Protocol Interface is returned.
+
+  @return The requested protocol interface for the handle
+
+**/
+EFI_STATUS
+EFIAPI
+SmmHandleProtocol (
+  IN EFI_HANDLE       UserHandle,
+  IN EFI_GUID         *Protocol,
+  OUT VOID            **Interface
+  );
+
+/**
+  Add a new protocol notification record for the request protocol.
+
+  @param  Protocol               The requested protocol to add the notify
+                                 registration
+  @param  Function               Points to the notification function
+  @param  Registration           Returns the registration record
+
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully returned the registration record
+                                 that has been added
+
+**/
+EFI_STATUS
+EFIAPI
+SmmRegisterProtocolNotify (
+  IN  CONST EFI_GUID              *Protocol,
+  IN  EFI_SMM_NOTIFY_FN           Function,
+  OUT VOID                        **Registration
+  );
+
+/**
+  Locates the requested handle(s) and returns them in Buffer.
+
+  @param  SearchType             The type of search to perform to locate the
+                                 handles
+  @param  Protocol               The protocol to search for
+  @param  SearchKey              Dependant on SearchType
+  @param  BufferSize             On input the size of Buffer.  On output the
+                                 size of data returned.
+  @param  Buffer                 The buffer to return the results in
+
+  @retval EFI_BUFFER_TOO_SMALL   Buffer too small, required buffer size is
+                                 returned in BufferSize.
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_SUCCESS            Successfully found the requested handle(s) and
+                                 returns them in Buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLocateHandle (
+  IN EFI_LOCATE_SEARCH_TYPE   SearchType,
+  IN EFI_GUID                 *Protocol   OPTIONAL,
+  IN VOID                     *SearchKey  OPTIONAL,
+  IN OUT UINTN                *BufferSize,
+  OUT EFI_HANDLE              *Buffer
+  );
+
+/**
+  Return the first Protocol Interface that matches the Protocol GUID. If
+  Registration is pasased in return a Protocol Instance that was just add
+  to the system. If Retistration is NULL return the first Protocol Interface
+  you find.
+
+  @param  Protocol               The protocol to search for
+  @param  Registration           Optional Registration Key returned from
+                                 RegisterProtocolNotify()
+  @param  Interface              Return the Protocol interface (instance).
+
+  @retval EFI_SUCCESS            If a valid Interface is returned
+  @retval EFI_INVALID_PARAMETER  Invalid parameter
+  @retval EFI_NOT_FOUND          Protocol interface not found
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLocateProtocol (
+  IN  EFI_GUID  *Protocol,
+  IN  VOID      *Registration OPTIONAL,
+  OUT VOID      **Interface
+  );
+
+/**
+  Manage SMI of a particular type.
+
+  @param  HandlerType    Points to the handler type or NULL for root SMI handlers.
+  @param  Context        Points to an optional context buffer.
+  @param  CommBuffer     Points to the optional communication buffer.
+  @param  CommBufferSize Points to the size of the optional communication buffer.
+
+  @retval EFI_SUCCESS                        Interrupt source was processed successfully but not quiesced.
+  @retval EFI_INTERRUPT_PENDING              One or more SMI sources could not be quiesced.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING  Interrupt source was not handled or quiesced.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED Interrupt source was handled and quiesced.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiManage (
+  IN     CONST EFI_GUID           *HandlerType,
+  IN     CONST VOID               *Context         OPTIONAL,
+  IN OUT VOID                     *CommBuffer      OPTIONAL,
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  Registers a handler to execute within SMM.
+
+  @param  Handler        Handler service funtion pointer.
+  @param  HandlerType    Points to the handler type or NULL for root SMI handlers.
+  @param  DispatchHandle On return, contains a unique handle which can be used to later unregister the handler function.
+
+  @retval EFI_SUCCESS           Handler register success.
+  @retval EFI_INVALID_PARAMETER Handler or DispatchHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiHandlerRegister (
+  IN   EFI_SMM_HANDLER_ENTRY_POINT2   Handler,
+  IN   CONST EFI_GUID                 *HandlerType  OPTIONAL,
+  OUT  EFI_HANDLE                     *DispatchHandle
+  );
+
+/**
+  Unregister a handler in SMM.
+
+  @param  DispatchHandle  The handle that was specified when the handler was registered.
+
+  @retval EFI_SUCCESS           Handler function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER DispatchHandle does not refer to a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiHandlerUnRegister (
+  IN  EFI_HANDLE                      DispatchHandle
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmDriverDispatchHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLegacyBootHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmReadyToLockHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEndOfDxeHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmExitBootServicesHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  This function is the main entry point for an SMM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-SMM environment into an SMM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+SmmReadyToBootHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+/**
+  Place holder function until all the SMM System Table Service are available.
+
+  @param  Arg1                   Undefined
+  @param  Arg2                   Undefined
+  @param  Arg3                   Undefined
+  @param  Arg4                   Undefined
+  @param  Arg5                   Undefined
+
+  @return EFI_NOT_AVAILABLE_YET
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEfiNotAvailableYetArg5 (
+  UINTN Arg1,
+  UINTN Arg2,
+  UINTN Arg3,
+  UINTN Arg4,
+  UINTN Arg5
+  );
+
+//
+//Functions used during debug buils
+//
+
+/**
+  Traverse the discovered list for any drivers that were discovered but not loaded
+  because the dependency experessions evaluated to false.
+
+**/
+VOID
+SmmDisplayDiscoveredNotDispatched (
+  VOID
+  );
+
+/**
+  Add free SMRAM region for use by memory service.
+
+  @param  MemBase                Base address of memory region.
+  @param  MemLength              Length of the memory region.
+  @param  Type                   Memory type.
+  @param  Attributes             Memory region state.
+
+**/
+VOID
+SmmAddMemoryRegion (
+  IN      EFI_PHYSICAL_ADDRESS      MemBase,
+  IN      UINT64                    MemLength,
+  IN      EFI_MEMORY_TYPE           Type,
+  IN      UINT64                    Attributes
+  );
+
+/**
+  Finds the protocol entry for the requested protocol.
+
+  @param  Protocol               The ID of the protocol
+  @param  Create                 Create a new entry if not found
+
+  @return Protocol entry
+
+**/
+PROTOCOL_ENTRY  *
+SmmFindProtocolEntry (
+  IN EFI_GUID   *Protocol,
+  IN BOOLEAN    Create
+  );
+
+/**
+  Signal event for every protocol in protocol entry.
+
+  @param  Prot                   Protocol interface
+
+**/
+VOID
+SmmNotifyProtocol (
+  IN PROTOCOL_INTERFACE   *Prot
+  );
+
+/**
+  Finds the protocol instance for the requested handle and protocol.
+  Note: This function doesn't do parameters checking, it's caller's responsibility
+  to pass in valid parameters.
+
+  @param  Handle                 The handle to search the protocol on
+  @param  Protocol               GUID of the protocol
+  @param  Interface              The interface for the protocol being searched
+
+  @return Protocol instance (NULL: Not found)
+
+**/
+PROTOCOL_INTERFACE *
+SmmFindProtocolInterface (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  );
+
+/**
+  Removes Protocol from the protocol list (but not the handle list).
+
+  @param  Handle                 The handle to remove protocol on.
+  @param  Protocol               GUID of the protocol to be moved
+  @param  Interface              The interface of the protocol
+
+  @return Protocol Entry
+
+**/
+PROTOCOL_INTERFACE *
+SmmRemoveInterfaceFromProtocol (
+  IN IHANDLE        *Handle,
+  IN EFI_GUID       *Protocol,
+  IN VOID           *Interface
+  );
+
+/**
+  This is the POSTFIX version of the dependency evaluator.  This code does
+  not need to handle Before or After, as it is not valid to call this
+  routine in this case. POSTFIX means all the math is done on top of the stack.
+
+  @param  DriverEntry           DriverEntry element to update.
+
+  @retval TRUE                  If driver is ready to run.
+  @retval FALSE                 If driver is not ready to run or some fatal error
+                                was found.
+
+**/
+BOOLEAN
+SmmIsSchedulable (
+  IN  EFI_SMM_DRIVER_ENTRY   *DriverEntry
+  );
+
+//
+// SmramProfile
+//
+
+/**
+  Initialize SMRAM profile.
+
+**/
+VOID
+SmramProfileInit (
+  VOID
+  );
+
+/**
+  Register SMM image to SMRAM profile.
+
+  @param DriverEntry    SMM image info.
+  @param RegisterToDxe  Register image to DXE.
+
+  @retval TRUE          Register success.
+  @retval FALSE         Register fail.
+
+**/
+BOOLEAN
+RegisterSmramProfileImage (
+  IN EFI_SMM_DRIVER_ENTRY   *DriverEntry,
+  IN BOOLEAN                RegisterToDxe
+  );
+
+/**
+  Unregister image from SMRAM profile.
+
+  @param DriverEntry        SMM image info.
+  @param UnregisterToDxe    Unregister image from DXE.
+
+  @retval TRUE              Unregister success.
+  @retval FALSE             Unregister fail.
+
+**/
+BOOLEAN
+UnregisterSmramProfileImage (
+  IN EFI_SMM_DRIVER_ENTRY   *DriverEntry,
+  IN BOOLEAN                UnregisterToDxe
+  );
+
+/**
+  Update SMRAM profile information.
+
+  @param CallerAddress  Address of caller who call Allocate or Free.
+  @param Action         This Allocate or Free action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+SmmCoreUpdateProfile (
+  IN EFI_PHYSICAL_ADDRESS CallerAddress,
+  IN MEMORY_PROFILE_ACTION Action,
+  IN EFI_MEMORY_TYPE      MemoryType, // Valid for AllocatePages/AllocatePool
+  IN UINTN                Size,       // Valid for AllocatePages/FreePages/AllocatePool
+  IN VOID                 *Buffer
+  );
+
+/**
+  Register SMRAM profile handler.
+
+**/
+VOID
+RegisterSmramProfileHandler (
+  VOID
+  );
+
+/**
+  SMRAM profile ready to lock callback function.
+
+**/
+VOID
+SmramProfileReadyToLock (
+  VOID
+  );
+
+extern UINTN                    mFullSmramRangeCount;
+extern EFI_SMRAM_DESCRIPTOR     *mFullSmramRanges;
+
+extern EFI_LOADED_IMAGE_PROTOCOL  *mSmmCoreLoadedImage;
+
+//
+// Page management
+//
+
+typedef struct {
+  LIST_ENTRY  Link;
+  UINTN       NumberOfPages;
+} FREE_PAGE_LIST;
+
+extern LIST_ENTRY  mSmmMemoryMap;
+
+//
+// Pool management
+//
+
+//
+// MIN_POOL_SHIFT must not be less than 5
+//
+#define MIN_POOL_SHIFT  6
+#define MIN_POOL_SIZE   (1 << MIN_POOL_SHIFT)
+
+//
+// MAX_POOL_SHIFT must not be less than EFI_PAGE_SHIFT - 1
+//
+#define MAX_POOL_SHIFT  (EFI_PAGE_SHIFT - 1)
+#define MAX_POOL_SIZE   (1 << MAX_POOL_SHIFT)
+
+//
+// MAX_POOL_INDEX are calculated by maximum and minimum pool sizes
+//
+#define MAX_POOL_INDEX  (MAX_POOL_SHIFT - MIN_POOL_SHIFT + 1)
+
+typedef struct {
+  UINTN        Size;
+  BOOLEAN      Available;
+} POOL_HEADER;
+
+typedef struct {
+  POOL_HEADER  Header;
+  LIST_ENTRY   Link;
+} FREE_POOL_HEADER;
+
+extern LIST_ENTRY  mSmmPoolLists[MAX_POOL_INDEX];
+
+#endif
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.inf b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.inf
new file mode 100644
index 0000000000..9c06b2ac2f
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.inf
@@ -0,0 +1,98 @@
+## @file
+# This module provide an SMM CIS compliant implementation of SMM Core.
+#
+# Copyright (c) 2009 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PiSmmCore
+  MODULE_UNI_FILE                = PiSmmCore.uni
+  FILE_GUID                      = E94F54CD-81EB-47ed-AEC3-856F5DC157A9
+  MODULE_TYPE                    = SMM_CORE
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = SmmMain
+
+#  VALID_ARCHITECTURES           = IA32 X64
+
+[Sources]
+  PiSmmCore.c
+  PiSmmCore.h
+  PiSmmCorePrivateData.h
+  Page.c
+  Pool.c
+  Handle.c
+  Locate.c
+  Notify.c
+  Dependency.c
+  Dispatcher.c
+  Smi.c
+  InstallConfigurationTable.c
+  SmramProfileRecord.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  
+[LibraryClasses]
+  UefiDriverEntryPoint
+  BaseLib
+  BaseMemoryLib
+  PeCoffLib
+  CacheMaintenanceLib  
+  DebugLib
+  ReportStatusCodeLib
+  DevicePathLib               
+  UefiLib                     
+  UefiBootServicesTableLib    
+  MemoryAllocationLib
+  PcdLib
+  SmmCorePlatformHookLib
+  PerformanceLib
+  TimerLib
+  HobLib
+  SmmMemLib
+
+[Protocols]
+  gEfiDxeSmmReadyToLockProtocolGuid             ## UNDEFINED # SmiHandlerRegister
+  gEfiSmmReadyToLockProtocolGuid                ## PRODUCES
+  gEfiSmmCpuIo2ProtocolGuid                     ## CONSUMES
+  gEfiFirmwareVolume2ProtocolGuid               ## CONSUMES
+  gEfiSmmEndOfDxeProtocolGuid                   ## PRODUCES
+  gEfiSecurityArchProtocolGuid                  ## SOMETIMES_CONSUMES
+  gEfiSecurity2ArchProtocolGuid                 ## SOMETIMES_CONSUMES
+  gEfiLoadedImageProtocolGuid                   ## PRODUCES
+  gEfiDevicePathProtocolGuid                    ## CONSUMES
+  gEdkiiSmmExitBootServicesProtocolGuid         ## SOMETIMES_PRODUCES
+  gEdkiiSmmLegacyBootProtocolGuid               ## SOMETIMES_PRODUCES
+  gEdkiiSmmReadyToBootProtocolGuid              ## PRODUCES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressSmmCodePageNumber     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable        ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfileMemoryType             ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfilePropertyMask           ## CONSUMES
+
+[Guids]
+  gAprioriGuid                                  ## SOMETIMES_CONSUMES   ## File
+  gEfiEventDxeDispatchGuid                      ## PRODUCES             ## GUID # SmiHandlerRegister
+  gEfiEventLegacyBootGuid                       ## PRODUCES             ## GUID # SmiHandlerRegister
+  gEfiEventExitBootServicesGuid                 ## PRODUCES             ## GUID # SmiHandlerRegister
+  gEfiEventReadyToBootGuid                      ## PRODUCES             ## GUID # SmiHandlerRegister
+  gEfiEndOfDxeEventGroupGuid                    ## PRODUCES             ## GUID # SmiHandlerRegister
+  ## SOMETIMES_CONSUMES   ## GUID # Locate protocol
+  ## SOMETIMES_PRODUCES   ## GUID # SmiHandlerRegister
+  gEdkiiMemoryProfileGuid
+  gZeroGuid                                     ## SOMETIMES_CONSUMES   ## GUID
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PiSmmCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.uni b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.uni
new file mode 100644
index 0000000000..33864d233d
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCore.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCoreExtra.uni b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCoreExtra.uni
new file mode 100644
index 0000000000..5faa16de92
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCoreExtra.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCorePrivateData.h b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCorePrivateData.h
new file mode 100644
index 0000000000..8aec7a6568
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmCorePrivateData.h
@@ -0,0 +1,125 @@
+/** @file
+  The internal header file that declared a data structure that is shared
+  between the SMM IPL and the SMM Core.
+
+  Copyright (c) 2009 - 2015, 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.             
+
+**/
+
+#ifndef _PI_SMM_CORE_PRIVATE_DATA_H_
+#define _PI_SMM_CORE_PRIVATE_DATA_H_
+
+///
+/// Define values for the communications buffer used when gEfiEventDxeDispatchGuid is
+/// event signaled.  This event is signaled by the DXE Core each time the DXE Core 
+/// dispatcher has completed its work.  When this event is signaled, the SMM Core
+/// if notified, so the SMM Core can dispatch SMM drivers.  If COMM_BUFFER_SMM_DISPATCH_ERROR
+/// is returned in the communication buffer, then an error occurred dispatching SMM
+/// Drivers.  If COMM_BUFFER_SMM_DISPATCH_SUCCESS is returned, then the SMM Core 
+/// dispatched all the drivers it could.  If COMM_BUFFER_SMM_DISPATCH_RESTART is 
+/// returned, then the SMM Core just dispatched the SMM Driver that registered
+/// the SMM Entry Point enabling the use of SMM Mode.  In this case, the SMM Core
+/// should be notified again to dispatch more SMM Drivers using SMM Mode.
+///
+#define COMM_BUFFER_SMM_DISPATCH_ERROR    0x00
+#define COMM_BUFFER_SMM_DISPATCH_SUCCESS  0x01
+#define COMM_BUFFER_SMM_DISPATCH_RESTART  0x02
+
+///
+/// Signature for the private structure shared between the SMM IPL and the SMM Core
+///
+#define SMM_CORE_PRIVATE_DATA_SIGNATURE  SIGNATURE_32 ('s', 'm', 'm', 'c')
+
+///
+/// Private structure that is used to share information between the SMM IPL and 
+/// the SMM Core.  This structure is allocated from memory of type EfiRuntimeServicesData.
+/// Since runtime memory types are converted to available memory when a legacy boot 
+/// is performed, the SMM Core must not access any fields of this structure if a legacy 
+/// boot is performed.  As a result, the SMM IPL must create an event notification 
+/// for the Legacy Boot event and notify the SMM Core that a legacy boot is being 
+/// performed.  The SMM Core can then use this information to filter accesses to 
+/// thos structure.
+///
+typedef struct {
+  UINTN                           Signature;
+
+  ///
+  /// The ImageHandle passed into the entry point of the SMM IPL.  This ImageHandle
+  /// is used by the SMM Core to fill in the ParentImageHandle field of the Loaded
+  /// Image Protocol for each SMM Driver that is dispatched by the SMM Core.
+  ///
+  EFI_HANDLE                      SmmIplImageHandle;
+
+  ///
+  /// The number of SMRAM ranges passed from the SMM IPL to the SMM Core.  The SMM
+  /// Core uses these ranges of SMRAM to initialize the SMM Core memory manager.
+  ///
+  UINTN                           SmramRangeCount;
+
+  ///
+  /// A table of SMRAM ranges passed from the SMM IPL to the SMM Core.  The SMM
+  /// Core uses these ranges of SMRAM to initialize the SMM Core memory manager.
+  ///
+  EFI_SMRAM_DESCRIPTOR            *SmramRanges;
+
+  ///
+  /// The SMM Foundation Entry Point.  The SMM Core fills in this field when the 
+  /// SMM Core is initialized.  The SMM IPL is responsbile for registering this entry 
+  /// point with the SMM Configuration Protocol.  The SMM Configuration Protocol may 
+  /// not be available at the time the SMM IPL and SMM Core are started, so the SMM IPL
+  /// sets up a protocol notification on the SMM Configuration Protocol and registers 
+  /// the SMM Foundation Entry Point as soon as the SMM Configuration Protocol is 
+  /// available.
+  ///
+  EFI_SMM_ENTRY_POINT             SmmEntryPoint;
+  
+  ///
+  /// Boolean flag set to TRUE while an SMI is being processed by the SMM Core.
+  /// 
+  BOOLEAN                         SmmEntryPointRegistered;
+
+  ///
+  /// Boolean flag set to TRUE while an SMI is being processed by the SMM Core.
+  /// 
+  BOOLEAN                         InSmm;
+
+  ///
+  /// This field is set by the SMM Core then the SMM Core is initialized.  This field is
+  /// used by the SMM Base 2 Protocol and SMM Communication Protocol implementations in
+  /// the SMM IPL.  
+  ///
+  EFI_SMM_SYSTEM_TABLE2           *Smst;
+
+  ///
+  /// This field is used by the SMM Communicatioon Protocol to pass a buffer into 
+  /// a software SMI handler and for the software SMI handler to pass a buffer back to
+  /// the caller of the SMM Communication Protocol.  
+  ///
+  VOID                            *CommunicationBuffer;
+
+  ///
+  /// This field is used by the SMM Communicatioon Protocol to pass the size of a buffer,
+  /// in bytes, into a software SMI handler and for the software SMI handler to pass the 
+  /// size, in bytes, of a buffer back to the caller of the SMM Communication Protocol.
+  ///
+  UINTN                           BufferSize;
+
+  ///
+  /// This field is used by the SMM Communication Protocol to pass the return status from
+  /// a software SMI handler back to the caller of the SMM Communication Protocol.
+  ///
+  EFI_STATUS                      ReturnStatus;
+
+  EFI_PHYSICAL_ADDRESS            PiSmmCoreImageBase;
+  UINT64                          PiSmmCoreImageSize;
+  EFI_PHYSICAL_ADDRESS            PiSmmCoreEntryPoint;
+} SMM_CORE_PRIVATE_DATA;
+
+#endif
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.c b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.c
new file mode 100644
index 0000000000..50c3b34dfd
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.c
@@ -0,0 +1,1673 @@
+/** @file
+  SMM IPL that produces SMM related runtime protocols and load the SMM Core into SMRAM
+
+  Copyright (c) 2009 - 2015, 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 <PiDxe.h>
+
+#include <Protocol/SmmBase2.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/SmmAccess2.h>
+#include <Protocol/SmmConfiguration.h>
+#include <Protocol/SmmControl2.h>
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <Protocol/Cpu.h>
+
+#include <Guid/EventGroup.h>
+#include <Guid/EventLegacyBios.h>
+#include <Guid/LoadModuleAtFixedAddress.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/PcdLib.h>
+
+#include "PiSmmCorePrivateData.h"
+
+//
+// Function prototypes from produced protocols
+//
+
+/**
+  Indicate whether the driver is currently executing in the SMM Initialization phase.
+
+  @param   This                    The EFI_SMM_BASE2_PROTOCOL instance.
+  @param   InSmram                 Pointer to a Boolean which, on return, indicates that the driver is currently executing
+                                   inside of SMRAM (TRUE) or outside of SMRAM (FALSE).
+
+  @retval  EFI_INVALID_PARAMETER   InSmram was NULL.
+  @retval  EFI_SUCCESS             The call returned successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmBase2InSmram (
+  IN CONST EFI_SMM_BASE2_PROTOCOL  *This,
+  OUT      BOOLEAN                 *InSmram
+  );
+
+/**
+  Retrieves the location of the System Management System Table (SMST).
+
+  @param   This                    The EFI_SMM_BASE2_PROTOCOL instance.
+  @param   Smst                    On return, points to a pointer to the System Management Service Table (SMST).
+
+  @retval  EFI_INVALID_PARAMETER   Smst or This was invalid.
+  @retval  EFI_SUCCESS             The memory was returned to the system.
+  @retval  EFI_UNSUPPORTED         Not in SMM.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmBase2GetSmstLocation (
+  IN CONST EFI_SMM_BASE2_PROTOCOL  *This,
+  OUT      EFI_SMM_SYSTEM_TABLE2   **Smst
+  );
+
+/**
+  Communicates with a registered handler.
+  
+  This function provides a service to send and receive messages from a registered 
+  UEFI service.  This function is part of the SMM Communication Protocol that may 
+  be called in physical mode prior to SetVirtualAddressMap() and in virtual mode 
+  after SetVirtualAddressMap().
+
+  @param[in]     This                The EFI_SMM_COMMUNICATION_PROTOCOL instance.
+  @param[in, out] CommBuffer          A pointer to the buffer to convey into SMRAM.
+  @param[in, out] CommSize            The size of the data buffer being passed in.On exit, the size of data
+                                     being returned. Zero if the handler does not wish to reply with any data.
+
+  @retval EFI_SUCCESS                The message was successfully posted.
+  @retval EFI_INVALID_PARAMETER      The CommBuffer was NULL.
+**/
+EFI_STATUS
+EFIAPI
+SmmCommunicationCommunicate (
+  IN CONST EFI_SMM_COMMUNICATION_PROTOCOL  *This,
+  IN OUT VOID                              *CommBuffer,
+  IN OUT UINTN                             *CommSize
+  );
+
+/**
+  Event notification that is fired every time a gEfiSmmConfigurationProtocol installs.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplSmmConfigurationEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Event notification that is fired every time a DxeSmmReadyToLock protocol is added
+  or if gEfiEventReadyToBootGuid is signalled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplReadyToLockEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Event notification that is fired when DxeDispatch Event Group is signaled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplDxeDispatchEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Event notification that is fired when a GUIDed Event Group is signaled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplGuidedEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+SmmIplSetVirtualAddressNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+//
+// Data structure used to declare a table of protocol notifications and event 
+// notifications required by the SMM IPL
+//
+typedef struct {
+  BOOLEAN           Protocol;
+  BOOLEAN           CloseOnLock;
+  EFI_GUID          *Guid;
+  EFI_EVENT_NOTIFY  NotifyFunction;
+  VOID              *NotifyContext;
+  EFI_TPL           NotifyTpl;
+  EFI_EVENT         Event;
+} SMM_IPL_EVENT_NOTIFICATION;
+
+//
+// Handle to install the SMM Base2 Protocol and the SMM Communication Protocol
+//
+EFI_HANDLE  mSmmIplHandle = NULL;
+
+//
+// SMM Base 2 Protocol instance
+//
+EFI_SMM_BASE2_PROTOCOL  mSmmBase2 = {
+  SmmBase2InSmram,
+  SmmBase2GetSmstLocation
+};
+
+//
+// SMM Communication Protocol instance
+//
+EFI_SMM_COMMUNICATION_PROTOCOL  mSmmCommunication = {
+  SmmCommunicationCommunicate
+};
+
+//
+// SMM Core Private Data structure that contains the data shared between
+// the SMM IPL and the SMM Core.
+//
+SMM_CORE_PRIVATE_DATA  mSmmCorePrivateData = {
+  SMM_CORE_PRIVATE_DATA_SIGNATURE,    // Signature
+  NULL,                               // SmmIplImageHandle
+  0,                                  // SmramRangeCount
+  NULL,                               // SmramRanges
+  NULL,                               // SmmEntryPoint
+  FALSE,                              // SmmEntryPointRegistered
+  FALSE,                              // InSmm
+  NULL,                               // Smst
+  NULL,                               // CommunicationBuffer
+  0,                                  // BufferSize
+  EFI_SUCCESS                         // ReturnStatus
+};
+
+//
+// Global pointer used to access mSmmCorePrivateData from outside and inside SMM
+//
+SMM_CORE_PRIVATE_DATA  *gSmmCorePrivate = &mSmmCorePrivateData;
+
+//
+// SMM IPL global variables
+//
+EFI_SMM_CONTROL2_PROTOCOL  *mSmmControl2;
+EFI_SMM_ACCESS2_PROTOCOL   *mSmmAccess;
+EFI_SMRAM_DESCRIPTOR       *mCurrentSmramRange;
+BOOLEAN                    mSmmLocked = FALSE;
+EFI_PHYSICAL_ADDRESS       mSmramCacheBase;
+UINT64                     mSmramCacheSize;
+
+EFI_SMM_COMMUNICATE_HEADER mCommunicateHeader;
+
+//
+// Table of Protocol notification and GUIDed Event notifications that the SMM IPL requires
+//
+SMM_IPL_EVENT_NOTIFICATION  mSmmIplEvents[] = {
+  //
+  // Declare protocol notification on the SMM Configuration protocol.  When this notification is etablished, 
+  // the associated event is immediately signalled, so the notification function will be executed and the 
+  // SMM Configuration Protocol will be found if it is already in the handle database.
+  //
+  { TRUE,  FALSE, &gEfiSmmConfigurationProtocolGuid,  SmmIplSmmConfigurationEventNotify, &gEfiSmmConfigurationProtocolGuid,  TPL_NOTIFY,   NULL },
+  //
+  // Declare protocol notification on DxeSmmReadyToLock protocols.  When this notification is established, 
+  // the associated event is immediately signalled, so the notification function will be executed and the 
+  // DXE SMM Ready To Lock Protocol will be found if it is already in the handle database.
+  //
+  { TRUE,  TRUE,  &gEfiDxeSmmReadyToLockProtocolGuid, SmmIplReadyToLockEventNotify,      &gEfiDxeSmmReadyToLockProtocolGuid, TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on EndOfDxe event.  When this notification is etablished, 
+  // the associated event is immediately signalled, so the notification function will be executed and the 
+  // SMM End Of Dxe Protocol will be found if it is already in the handle database.
+  //
+  { FALSE, TRUE,  &gEfiEndOfDxeEventGroupGuid,        SmmIplGuidedEventNotify,           &gEfiEndOfDxeEventGroupGuid,        TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on the DXE Dispatch Event Group.  This event is signaled by the DXE Core
+  // each time the DXE Core dispatcher has completed its work.  When this event is signalled, the SMM Core
+  // if notified, so the SMM Core can dispatch SMM drivers.
+  //
+  { FALSE, TRUE,  &gEfiEventDxeDispatchGuid,          SmmIplDxeDispatchEventNotify,      &gEfiEventDxeDispatchGuid,          TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on Ready To Boot Event Group.  This is an extra event notification that is
+  // used to make sure SMRAM is locked before any boot options are processed.
+  //
+  { FALSE, TRUE,  &gEfiEventReadyToBootGuid,          SmmIplReadyToLockEventNotify,      &gEfiEventReadyToBootGuid,          TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on Legacy Boot Event Group.  This is used to inform the SMM Core that the platform 
+  // is performing a legacy boot operation, and that the UEFI environment is no longer available and the SMM Core 
+  // must guarantee that it does not access any UEFI related structures outside of SMRAM.
+  // It is also to inform the SMM Core to notify SMM driver that system enter legacy boot.
+  //
+  { FALSE, FALSE, &gEfiEventLegacyBootGuid,           SmmIplGuidedEventNotify,           &gEfiEventLegacyBootGuid,           TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on Exit Boot Services Event Group.  This is used to inform the SMM Core
+  // to notify SMM driver that system enter exit boot services.
+  //
+  { FALSE, FALSE, &gEfiEventExitBootServicesGuid,     SmmIplGuidedEventNotify,           &gEfiEventExitBootServicesGuid,     TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on Ready To Boot Event Group.  This is used to inform the SMM Core
+  // to notify SMM driver that system enter ready to boot.
+  //
+  { FALSE, FALSE, &gEfiEventReadyToBootGuid,          SmmIplGuidedEventNotify,           &gEfiEventReadyToBootGuid,          TPL_CALLBACK, NULL },
+  //
+  // Declare event notification on SetVirtualAddressMap() Event Group.  This is used to convert gSmmCorePrivate 
+  // and mSmmControl2 from physical addresses to virtual addresses.
+  //
+  { FALSE, FALSE, &gEfiEventVirtualAddressChangeGuid, SmmIplSetVirtualAddressNotify,     NULL,                               TPL_CALLBACK, NULL },
+  //
+  // Terminate the table of event notifications
+  //
+  { FALSE, FALSE, NULL,                               NULL,                              NULL,                               TPL_CALLBACK, NULL }
+};
+
+/**
+  Find the maximum SMRAM cache range that covers the range specified by SmramRange.
+  
+  This function searches and joins all adjacent ranges of SmramRange into a range to be cached.
+
+  @param   SmramRange       The SMRAM range to search from.
+  @param   SmramCacheBase   The returned cache range base.
+  @param   SmramCacheSize   The returned cache range size.
+
+**/
+VOID
+GetSmramCacheRange (
+  IN  EFI_SMRAM_DESCRIPTOR *SmramRange,
+  OUT EFI_PHYSICAL_ADDRESS *SmramCacheBase,
+  OUT UINT64               *SmramCacheSize
+  )
+{
+  UINTN                Index;
+  EFI_PHYSICAL_ADDRESS RangeCpuStart;
+  UINT64               RangePhysicalSize;
+  BOOLEAN              FoundAjacentRange;
+
+  *SmramCacheBase = SmramRange->CpuStart;
+  *SmramCacheSize = SmramRange->PhysicalSize;
+
+  do {
+    FoundAjacentRange = FALSE;
+    for (Index = 0; Index < gSmmCorePrivate->SmramRangeCount; Index++) {
+      RangeCpuStart     = gSmmCorePrivate->SmramRanges[Index].CpuStart;
+      RangePhysicalSize = gSmmCorePrivate->SmramRanges[Index].PhysicalSize;
+      if (RangeCpuStart < *SmramCacheBase && *SmramCacheBase == (RangeCpuStart + RangePhysicalSize)) {
+        *SmramCacheBase   = RangeCpuStart;
+        *SmramCacheSize  += RangePhysicalSize;
+        FoundAjacentRange = TRUE;
+      } else if ((*SmramCacheBase + *SmramCacheSize) == RangeCpuStart && RangePhysicalSize > 0) {
+        *SmramCacheSize  += RangePhysicalSize;
+        FoundAjacentRange = TRUE;
+      }
+    }
+  } while (FoundAjacentRange);
+  
+}
+
+/**
+  Indicate whether the driver is currently executing in the SMM Initialization phase.
+
+  @param   This                    The EFI_SMM_BASE2_PROTOCOL instance.
+  @param   InSmram                 Pointer to a Boolean which, on return, indicates that the driver is currently executing
+                                   inside of SMRAM (TRUE) or outside of SMRAM (FALSE).
+
+  @retval  EFI_INVALID_PARAMETER   InSmram was NULL.
+  @retval  EFI_SUCCESS             The call returned successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmBase2InSmram (
+  IN CONST EFI_SMM_BASE2_PROTOCOL  *This,
+  OUT      BOOLEAN                 *InSmram
+  )
+{
+  if (InSmram == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *InSmram = gSmmCorePrivate->InSmm;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the location of the System Management System Table (SMST).
+
+  @param   This                    The EFI_SMM_BASE2_PROTOCOL instance.
+  @param   Smst                    On return, points to a pointer to the System Management Service Table (SMST).
+
+  @retval  EFI_INVALID_PARAMETER   Smst or This was invalid.
+  @retval  EFI_SUCCESS             The memory was returned to the system.
+  @retval  EFI_UNSUPPORTED         Not in SMM.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmBase2GetSmstLocation (
+  IN CONST EFI_SMM_BASE2_PROTOCOL  *This,
+  OUT      EFI_SMM_SYSTEM_TABLE2   **Smst
+  )
+{
+  if ((This == NULL) ||(Smst == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  if (!gSmmCorePrivate->InSmm) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  *Smst = gSmmCorePrivate->Smst;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Communicates with a registered handler.
+  
+  This function provides a service to send and receive messages from a registered 
+  UEFI service.  This function is part of the SMM Communication Protocol that may 
+  be called in physical mode prior to SetVirtualAddressMap() and in virtual mode 
+  after SetVirtualAddressMap().
+
+  @param[in] This                The EFI_SMM_COMMUNICATION_PROTOCOL instance.
+  @param[in, out] CommBuffer          A pointer to the buffer to convey into SMRAM.
+  @param[in, out] CommSize            The size of the data buffer being passed in.On exit, the size of data
+                                 being returned. Zero if the handler does not wish to reply with any data.
+
+  @retval EFI_SUCCESS            The message was successfully posted.
+  @retval EFI_INVALID_PARAMETER  The CommBuffer was NULL.
+**/
+EFI_STATUS
+EFIAPI
+SmmCommunicationCommunicate (
+  IN CONST EFI_SMM_COMMUNICATION_PROTOCOL  *This,
+  IN OUT VOID                              *CommBuffer,
+  IN OUT UINTN                             *CommSize
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SMM_COMMUNICATE_HEADER  *CommunicateHeader;
+  BOOLEAN                     OldInSmm;
+
+  //
+  // Check parameters
+  //
+  if ((CommBuffer == NULL) || (CommSize == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // CommSize must hold HeaderGuid and MessageLength
+  //
+  if (*CommSize < OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If not already in SMM, then generate a Software SMI
+  //
+  if (!gSmmCorePrivate->InSmm && gSmmCorePrivate->SmmEntryPointRegistered) {
+    //
+    // Put arguments for Software SMI in gSmmCorePrivate
+    //
+    gSmmCorePrivate->CommunicationBuffer = CommBuffer;
+    gSmmCorePrivate->BufferSize          = *CommSize;
+
+    //
+    // Generate Software SMI
+    //
+    Status = mSmmControl2->Trigger (mSmmControl2, NULL, NULL, FALSE, 0);
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Return status from software SMI 
+    //
+    *CommSize = gSmmCorePrivate->BufferSize;
+    return gSmmCorePrivate->ReturnStatus;
+  }
+
+  //
+  // If we are in SMM, then the execution mode must be physical, which means that
+  // OS established virtual addresses can not be used.  If SetVirtualAddressMap()
+  // has been called, then a direct invocation of the Software SMI is not 
+  // not allowed so return EFI_INVALID_PARAMETER.
+  //
+  if (EfiGoneVirtual()) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If we are not in SMM, don't allow call SmiManage() directly when SMRAM is closed or locked.
+  //
+  if ((!gSmmCorePrivate->InSmm) && (!mSmmAccess->OpenState || mSmmAccess->LockState)) {
+    return EFI_INVALID_PARAMETER;
+  }
+ 
+  //
+  // Save current InSmm state and set InSmm state to TRUE
+  //
+  OldInSmm = gSmmCorePrivate->InSmm;
+  gSmmCorePrivate->InSmm = TRUE;
+
+  //
+  // Already in SMM and before SetVirtualAddressMap(), so call SmiManage() directly.
+  //
+  CommunicateHeader = (EFI_SMM_COMMUNICATE_HEADER *)CommBuffer;
+  *CommSize -= OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data);
+  Status = gSmmCorePrivate->Smst->SmiManage (
+                                    &CommunicateHeader->HeaderGuid, 
+                                    NULL, 
+                                    CommunicateHeader->Data, 
+                                    CommSize
+                                    );
+
+  //
+  // Update CommunicationBuffer, BufferSize and ReturnStatus
+  // Communicate service finished, reset the pointer to CommBuffer to NULL
+  //
+  *CommSize += OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data);
+
+  //
+  // Restore original InSmm state
+  //
+  gSmmCorePrivate->InSmm = OldInSmm;
+
+  return (Status == EFI_SUCCESS) ? EFI_SUCCESS : EFI_NOT_FOUND;
+}
+
+/**
+  Event notification that is fired when GUIDed Event Group is signaled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplGuidedEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  UINTN                       Size;
+
+  //
+  // Use Guid to initialize EFI_SMM_COMMUNICATE_HEADER structure 
+  //
+  CopyGuid (&mCommunicateHeader.HeaderGuid, (EFI_GUID *)Context);
+  mCommunicateHeader.MessageLength = 1;
+  mCommunicateHeader.Data[0] = 0;
+
+  //
+  // Generate the Software SMI and return the result
+  //
+  Size = sizeof (mCommunicateHeader);
+  SmmCommunicationCommunicate (&mSmmCommunication, &mCommunicateHeader, &Size);
+}
+
+/**
+  Event notification that is fired when DxeDispatch Event Group is signaled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplDxeDispatchEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  UINTN                       Size;
+  EFI_STATUS                  Status;
+
+  //
+  // Keep calling the SMM Core Dispatcher until there is no request to restart it.
+  //
+  while (TRUE) {
+    //
+    // Use Guid to initialize EFI_SMM_COMMUNICATE_HEADER structure
+    // Clear the buffer passed into the Software SMI.  This buffer will return
+    // the status of the SMM Core Dispatcher.
+    //
+    CopyGuid (&mCommunicateHeader.HeaderGuid, (EFI_GUID *)Context);
+    mCommunicateHeader.MessageLength = 1;
+    mCommunicateHeader.Data[0] = 0;
+
+    //
+    // Generate the Software SMI and return the result
+    //
+    Size = sizeof (mCommunicateHeader);
+    SmmCommunicationCommunicate (&mSmmCommunication, &mCommunicateHeader, &Size);
+    
+    //
+    // Return if there is no request to restart the SMM Core Dispatcher
+    //
+    if (mCommunicateHeader.Data[0] != COMM_BUFFER_SMM_DISPATCH_RESTART) {
+      return;
+    }
+      
+    //
+    // Attempt to reset SMRAM cacheability to UC
+    // Assume CPU AP is available at this time
+    //
+    Status = gDS->SetMemorySpaceAttributes(
+                    mSmramCacheBase, 
+                    mSmramCacheSize,
+                    EFI_MEMORY_UC
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_WARN, "SMM IPL failed to reset SMRAM window to EFI_MEMORY_UC\n"));
+    }  
+
+    //
+    // Close all SMRAM ranges to protect SMRAM
+    //
+    Status = mSmmAccess->Close (mSmmAccess);
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Print debug message that the SMRAM window is now closed.
+    //
+    DEBUG ((DEBUG_INFO, "SMM IPL closed SMRAM window\n"));
+  }
+}
+
+/**
+  Event notification that is fired every time a gEfiSmmConfigurationProtocol installs.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplSmmConfigurationEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SMM_CONFIGURATION_PROTOCOL  *SmmConfiguration;
+
+  //
+  // Make sure this notification is for this handler
+  //
+  Status = gBS->LocateProtocol (Context, NULL, (VOID **)&SmmConfiguration);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Register the SMM Entry Point provided by the SMM Core with the SMM COnfiguration protocol
+  //
+  Status = SmmConfiguration->RegisterSmmEntry (SmmConfiguration, gSmmCorePrivate->SmmEntryPoint);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Set flag to indicate that the SMM Entry Point has been registered which 
+  // means that SMIs are now fully operational.
+  //
+  gSmmCorePrivate->SmmEntryPointRegistered = TRUE;
+
+  //
+  // Print debug message showing SMM Core entry point address.
+  //
+  DEBUG ((DEBUG_INFO, "SMM IPL registered SMM Entry Point address %p\n", (VOID *)(UINTN)gSmmCorePrivate->SmmEntryPoint));
+}
+
+/**
+  Event notification that is fired every time a DxeSmmReadyToLock protocol is added
+  or if gEfiEventReadyToBootGuid is signaled.
+
+  @param  Event                 The Event that is being processed, not used.
+  @param  Context               Event Context, not used.
+
+**/
+VOID
+EFIAPI
+SmmIplReadyToLockEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Interface;
+  UINTN       Index;
+
+  //
+  // See if we are already locked
+  //
+  if (mSmmLocked) {
+    return;
+  }
+  
+  //
+  // Make sure this notification is for this handler
+  //
+  if (CompareGuid ((EFI_GUID *)Context, &gEfiDxeSmmReadyToLockProtocolGuid)) {
+    Status = gBS->LocateProtocol (&gEfiDxeSmmReadyToLockProtocolGuid, NULL, &Interface);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+  } else {
+    //
+    // If SMM is not locked yet and we got here from gEfiEventReadyToBootGuid being 
+    // signaled, then gEfiDxeSmmReadyToLockProtocolGuid was not installed as expected.
+    // Print a warning on debug builds.
+    //
+    DEBUG ((DEBUG_WARN, "SMM IPL!  DXE SMM Ready To Lock Protocol not installed before Ready To Boot signal\n"));
+  }
+
+  //
+  // Lock the SMRAM (Note: Locking SMRAM may not be supported on all platforms)
+  //
+  mSmmAccess->Lock (mSmmAccess);
+  
+  //
+  // Close protocol and event notification events that do not apply after the 
+  // DXE SMM Ready To Lock Protocol has been installed or the Ready To Boot 
+  // event has been signalled.
+  //
+  for (Index = 0; mSmmIplEvents[Index].NotifyFunction != NULL; Index++) {
+    if (mSmmIplEvents[Index].CloseOnLock) {
+      gBS->CloseEvent (mSmmIplEvents[Index].Event);
+    }
+  }
+
+  //
+  // Inform SMM Core that the DxeSmmReadyToLock protocol was installed
+  //
+  SmmIplGuidedEventNotify (Event, (VOID *)&gEfiDxeSmmReadyToLockProtocolGuid);
+
+  //
+  // Print debug message that the SMRAM window is now locked.
+  //
+  DEBUG ((DEBUG_INFO, "SMM IPL locked SMRAM window\n"));
+  
+  //
+  // Set flag so this operation will not be performed again
+  //
+  mSmmLocked = TRUE;
+}
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+SmmIplSetVirtualAddressNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EfiConvertPointer (0x0, (VOID **)&mSmmControl2);
+}
+
+/**
+  Get the fixed loadding address from image header assigned by build tool. This function only be called
+  when Loading module at Fixed address feature enabled.
+
+  @param  ImageContext              Pointer to the image context structure that describes the PE/COFF
+                                    image that needs to be examined by this function.
+  @retval EFI_SUCCESS               An fixed loading address is assigned to this image by build tools .
+  @retval EFI_NOT_FOUND             The image has no assigned fixed loadding address.
+**/
+EFI_STATUS
+GetPeCoffImageFixLoadingAssignedAddress(
+  IN OUT PE_COFF_LOADER_IMAGE_CONTEXT  *ImageContext
+  )
+{
+   UINTN                              SectionHeaderOffset;
+   EFI_STATUS                         Status;
+   EFI_IMAGE_SECTION_HEADER           SectionHeader;
+   EFI_IMAGE_OPTIONAL_HEADER_UNION    *ImgHdr;
+   EFI_PHYSICAL_ADDRESS               FixLoaddingAddress;
+   UINT16                             Index;
+   UINTN                              Size;
+   UINT16                             NumberOfSections;
+   EFI_PHYSICAL_ADDRESS               SmramBase;
+   UINT64                             SmmCodeSize;
+   UINT64                             ValueInSectionHeader;
+   //
+   // Build tool will calculate the smm code size and then patch the PcdLoadFixAddressSmmCodePageNumber
+   //
+   SmmCodeSize = EFI_PAGES_TO_SIZE (PcdGet32(PcdLoadFixAddressSmmCodePageNumber));
+ 
+   FixLoaddingAddress = 0;
+   Status = EFI_NOT_FOUND;
+   SmramBase = mCurrentSmramRange->CpuStart;
+   //
+   // Get PeHeader pointer
+   //
+   ImgHdr = (EFI_IMAGE_OPTIONAL_HEADER_UNION *)((CHAR8* )ImageContext->Handle + ImageContext->PeCoffHeaderOffset);
+   SectionHeaderOffset = (UINTN)(
+                                 ImageContext->PeCoffHeaderOffset +
+                                 sizeof (UINT32) +
+                                 sizeof (EFI_IMAGE_FILE_HEADER) +
+                                 ImgHdr->Pe32.FileHeader.SizeOfOptionalHeader
+                                 );
+   NumberOfSections = ImgHdr->Pe32.FileHeader.NumberOfSections;
+
+   //
+   // Get base address from the first section header that doesn't point to code section.
+   //
+   for (Index = 0; Index < NumberOfSections; Index++) {
+     //
+     // Read section header from file
+     //
+     Size = sizeof (EFI_IMAGE_SECTION_HEADER);
+     Status = ImageContext->ImageRead (
+                              ImageContext->Handle,
+                              SectionHeaderOffset,
+                              &Size,
+                              &SectionHeader
+                              );
+     if (EFI_ERROR (Status)) {
+       return Status;
+     }
+     
+     Status = EFI_NOT_FOUND;
+     
+     if ((SectionHeader.Characteristics & EFI_IMAGE_SCN_CNT_CODE) == 0) {
+       //
+       // Build tool saves the offset to SMRAM base as image base in PointerToRelocations & PointerToLineNumbers fields in the
+       // first section header that doesn't point to code section in image header. And there is an assumption that when the
+       // feature is enabled, if a module is assigned a loading address by tools, PointerToRelocations & PointerToLineNumbers
+       // fields should NOT be Zero, or else, these 2 fileds should be set to Zero
+       //
+       ValueInSectionHeader = ReadUnaligned64((UINT64*)&SectionHeader.PointerToRelocations);
+       if (ValueInSectionHeader != 0) {
+         //
+         // Found first section header that doesn't point to code section in which uild tool saves the
+         // offset to SMRAM base as image base in PointerToRelocations & PointerToLineNumbers fields
+         //
+         FixLoaddingAddress = (EFI_PHYSICAL_ADDRESS)(SmramBase + (INT64)ValueInSectionHeader);
+
+         if (SmramBase + SmmCodeSize > FixLoaddingAddress && SmramBase <=  FixLoaddingAddress) {
+           //
+           // The assigned address is valid. Return the specified loadding address
+           //
+           ImageContext->ImageAddress = FixLoaddingAddress;
+           Status = EFI_SUCCESS;
+         }
+       }
+       break;
+     }
+     SectionHeaderOffset += sizeof (EFI_IMAGE_SECTION_HEADER);
+   }
+   DEBUG ((EFI_D_INFO|EFI_D_LOAD, "LOADING MODULE FIXED INFO: Loading module at fixed address %x, Status = %r \n", FixLoaddingAddress, Status));
+   return Status;
+}
+/**
+  Load the SMM Core image into SMRAM and executes the SMM Core from SMRAM.
+
+  @param[in, out] SmramRange            Descriptor for the range of SMRAM to reload the 
+                                        currently executing image, the rang of SMRAM to
+                                        hold SMM Core will be excluded.
+  @param[in, out] SmramRangeSmmCore     Descriptor for the range of SMRAM to hold SMM Core.
+
+  @param[in]      Context               Context to pass into SMM Core
+
+  @return  EFI_STATUS
+
+**/
+EFI_STATUS
+ExecuteSmmCoreFromSmram (
+  IN OUT EFI_SMRAM_DESCRIPTOR   *SmramRange,
+  IN OUT EFI_SMRAM_DESCRIPTOR   *SmramRangeSmmCore,
+  IN     VOID                   *Context
+  )
+{
+  EFI_STATUS                    Status;
+  VOID                          *SourceBuffer;
+  UINTN                         SourceSize;
+  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext;
+  UINTN                         PageCount;
+  EFI_IMAGE_ENTRY_POINT         EntryPoint;
+
+  //
+  // Search all Firmware Volumes for a PE/COFF image in a file of type SMM_CORE
+  //  
+  Status = GetSectionFromAnyFvByFileType (
+             EFI_FV_FILETYPE_SMM_CORE, 
+             0,
+             EFI_SECTION_PE32, 
+             0,
+             &SourceBuffer, 
+             &SourceSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Initilize ImageContext
+  //
+  ImageContext.Handle    = SourceBuffer;
+  ImageContext.ImageRead = PeCoffLoaderImageReadFromMemory;
+
+  //
+  // Get information about the image being loaded
+  //
+  Status = PeCoffLoaderGetImageInfo (&ImageContext);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // if Loading module at Fixed Address feature is enabled, the SMM core driver will be loaded to 
+  // the address assigned by build tool.
+  //
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+    //
+    // Get the fixed loading address assigned by Build tool
+    //
+    Status = GetPeCoffImageFixLoadingAssignedAddress (&ImageContext);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Since the memory range to load SMM CORE will be cut out in SMM core, so no need to allocate and free this range
+      //
+      PageCount = 0;
+    } else {
+      DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED ERROR: Loading module at fixed address at address failed\n"));
+      //
+      // Allocate memory for the image being loaded from the EFI_SRAM_DESCRIPTOR 
+      // specified by SmramRange
+      //
+      PageCount = (UINTN)EFI_SIZE_TO_PAGES((UINTN)ImageContext.ImageSize + ImageContext.SectionAlignment);
+
+      ASSERT ((SmramRange->PhysicalSize & EFI_PAGE_MASK) == 0);
+      ASSERT (SmramRange->PhysicalSize > EFI_PAGES_TO_SIZE (PageCount));
+
+      SmramRange->PhysicalSize -= EFI_PAGES_TO_SIZE (PageCount);
+      SmramRangeSmmCore->CpuStart = SmramRange->CpuStart + SmramRange->PhysicalSize;
+      SmramRangeSmmCore->PhysicalStart = SmramRange->PhysicalStart + SmramRange->PhysicalSize;
+      SmramRangeSmmCore->RegionState = SmramRange->RegionState | EFI_ALLOCATED;
+      SmramRangeSmmCore->PhysicalSize = EFI_PAGES_TO_SIZE (PageCount);
+
+      //
+      // Align buffer on section boundary
+      //
+      ImageContext.ImageAddress = SmramRangeSmmCore->CpuStart;
+    }
+  } else {
+    //
+    // Allocate memory for the image being loaded from the EFI_SRAM_DESCRIPTOR 
+    // specified by SmramRange
+    //
+    PageCount = (UINTN)EFI_SIZE_TO_PAGES((UINTN)ImageContext.ImageSize + ImageContext.SectionAlignment);
+
+    ASSERT ((SmramRange->PhysicalSize & EFI_PAGE_MASK) == 0);
+    ASSERT (SmramRange->PhysicalSize > EFI_PAGES_TO_SIZE (PageCount));
+
+    SmramRange->PhysicalSize -= EFI_PAGES_TO_SIZE (PageCount);
+    SmramRangeSmmCore->CpuStart = SmramRange->CpuStart + SmramRange->PhysicalSize;
+    SmramRangeSmmCore->PhysicalStart = SmramRange->PhysicalStart + SmramRange->PhysicalSize;
+    SmramRangeSmmCore->RegionState = SmramRange->RegionState | EFI_ALLOCATED;
+    SmramRangeSmmCore->PhysicalSize = EFI_PAGES_TO_SIZE (PageCount);
+
+    //
+    // Align buffer on section boundary
+    //
+    ImageContext.ImageAddress = SmramRangeSmmCore->CpuStart;
+  }
+  
+  ImageContext.ImageAddress += ImageContext.SectionAlignment - 1;
+  ImageContext.ImageAddress &= ~((EFI_PHYSICAL_ADDRESS)(ImageContext.SectionAlignment - 1));
+
+  //
+  // Print debug message showing SMM Core load address.
+  //
+  DEBUG ((DEBUG_INFO, "SMM IPL loading SMM Core at SMRAM address %p\n", (VOID *)(UINTN)ImageContext.ImageAddress));
+
+  //
+  // Load the image to our new buffer
+  //
+  Status = PeCoffLoaderLoadImage (&ImageContext);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Relocate the image in our new buffer
+    //
+    Status = PeCoffLoaderRelocateImage (&ImageContext);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Flush the instruction cache so the image data are written before we execute it
+      //
+      InvalidateInstructionCacheRange ((VOID *)(UINTN)ImageContext.ImageAddress, (UINTN)ImageContext.ImageSize);
+
+      //
+      // Print debug message showing SMM Core entry point address.
+      //
+      DEBUG ((DEBUG_INFO, "SMM IPL calling SMM Core at SMRAM address %p\n", (VOID *)(UINTN)ImageContext.EntryPoint));
+
+      gSmmCorePrivate->PiSmmCoreImageBase = ImageContext.ImageAddress;
+      gSmmCorePrivate->PiSmmCoreImageSize = ImageContext.ImageSize;
+      DEBUG ((DEBUG_INFO, "PiSmmCoreImageBase - 0x%016lx\n", gSmmCorePrivate->PiSmmCoreImageBase));
+      DEBUG ((DEBUG_INFO, "PiSmmCoreImageSize - 0x%016lx\n", gSmmCorePrivate->PiSmmCoreImageSize));
+
+      gSmmCorePrivate->PiSmmCoreEntryPoint = ImageContext.EntryPoint;
+
+      //
+      // Execute image
+      //
+      EntryPoint = (EFI_IMAGE_ENTRY_POINT)(UINTN)ImageContext.EntryPoint;
+      Status = EntryPoint ((EFI_HANDLE)Context, gST);
+    }
+  }
+
+  //
+  // Always free memory allocted by GetFileBufferByFilePath ()
+  //
+  FreePool (SourceBuffer);
+
+  return Status;
+}
+
+/**
+  SMM split SMRAM entry.
+
+  @param[in, out] RangeToCompare             Pointer to EFI_SMRAM_DESCRIPTOR to compare.
+  @param[in, out] ReservedRangeToCompare     Pointer to EFI_SMM_RESERVED_SMRAM_REGION to compare.
+  @param[out]     Ranges                     Output pointer to hold split EFI_SMRAM_DESCRIPTOR entry.
+  @param[in, out] RangeCount                 Pointer to range count.
+  @param[out]     ReservedRanges             Output pointer to hold split EFI_SMM_RESERVED_SMRAM_REGION entry.
+  @param[in, out] ReservedRangeCount         Pointer to reserved range count.
+  @param[out]     FinalRanges                Output pointer to hold split final EFI_SMRAM_DESCRIPTOR entry
+                                             that no need to be split anymore.
+  @param[in, out] FinalRangeCount            Pointer to final range count.
+
+**/
+VOID
+SmmSplitSmramEntry (
+  IN OUT EFI_SMRAM_DESCRIPTOR           *RangeToCompare,
+  IN OUT EFI_SMM_RESERVED_SMRAM_REGION  *ReservedRangeToCompare,
+  OUT    EFI_SMRAM_DESCRIPTOR           *Ranges,
+  IN OUT UINTN                          *RangeCount,
+  OUT    EFI_SMM_RESERVED_SMRAM_REGION  *ReservedRanges,
+  IN OUT UINTN                          *ReservedRangeCount,
+  OUT    EFI_SMRAM_DESCRIPTOR           *FinalRanges,
+  IN OUT UINTN                          *FinalRangeCount
+  )
+{
+  UINT64    RangeToCompareEnd;
+  UINT64    ReservedRangeToCompareEnd;
+
+  RangeToCompareEnd         = RangeToCompare->CpuStart + RangeToCompare->PhysicalSize;
+  ReservedRangeToCompareEnd = ReservedRangeToCompare->SmramReservedStart + ReservedRangeToCompare->SmramReservedSize;
+
+  if ((RangeToCompare->CpuStart >= ReservedRangeToCompare->SmramReservedStart) &&
+      (RangeToCompare->CpuStart < ReservedRangeToCompareEnd)) {
+    if (RangeToCompareEnd < ReservedRangeToCompareEnd) {
+      //
+      // RangeToCompare  ReservedRangeToCompare
+      //                 ----                    ----    --------------------------------------
+      //                 |  |                    |  | -> 1. ReservedRangeToCompare
+      // ----            |  |                    |--|    --------------------------------------
+      // |  |            |  |                    |  |
+      // |  |            |  |                    |  | -> 2. FinalRanges[*FinalRangeCount] and increment *FinalRangeCount
+      // |  |            |  |                    |  |       RangeToCompare->PhysicalSize = 0
+      // ----            |  |                    |--|    --------------------------------------
+      //                 |  |                    |  | -> 3. ReservedRanges[*ReservedRangeCount] and increment *ReservedRangeCount
+      //                 ----                    ----    --------------------------------------
+      //
+
+      //
+      // 1. Update ReservedRangeToCompare.
+      //
+      ReservedRangeToCompare->SmramReservedSize = RangeToCompare->CpuStart - ReservedRangeToCompare->SmramReservedStart;
+      //
+      // 2. Update FinalRanges[FinalRangeCount] and increment *FinalRangeCount.
+      //    Zero RangeToCompare->PhysicalSize.
+      //
+      FinalRanges[*FinalRangeCount].CpuStart      = RangeToCompare->CpuStart;
+      FinalRanges[*FinalRangeCount].PhysicalStart = RangeToCompare->PhysicalStart;
+      FinalRanges[*FinalRangeCount].RegionState   = RangeToCompare->RegionState | EFI_ALLOCATED;
+      FinalRanges[*FinalRangeCount].PhysicalSize  = RangeToCompare->PhysicalSize;
+      *FinalRangeCount += 1;
+      RangeToCompare->PhysicalSize = 0;
+      //
+      // 3. Update ReservedRanges[*ReservedRangeCount] and increment *ReservedRangeCount.
+      //
+      ReservedRanges[*ReservedRangeCount].SmramReservedStart = FinalRanges[*FinalRangeCount - 1].CpuStart + FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      ReservedRanges[*ReservedRangeCount].SmramReservedSize  = ReservedRangeToCompareEnd - RangeToCompareEnd;
+      *ReservedRangeCount += 1;
+    } else {
+      //
+      // RangeToCompare  ReservedRangeToCompare
+      //                 ----                    ----    --------------------------------------
+      //                 |  |                    |  | -> 1. ReservedRangeToCompare
+      // ----            |  |                    |--|    --------------------------------------
+      // |  |            |  |                    |  |
+      // |  |            |  |                    |  | -> 2. FinalRanges[*FinalRangeCount] and increment *FinalRangeCount
+      // |  |            |  |                    |  |
+      // |  |            ----                    |--|    --------------------------------------
+      // |  |                                    |  | -> 3. RangeToCompare
+      // ----                                    ----    --------------------------------------
+      //
+
+      //
+      // 1. Update ReservedRangeToCompare.
+      //
+      ReservedRangeToCompare->SmramReservedSize = RangeToCompare->CpuStart - ReservedRangeToCompare->SmramReservedStart;
+      //
+      // 2. Update FinalRanges[FinalRangeCount] and increment *FinalRangeCount.
+      //
+      FinalRanges[*FinalRangeCount].CpuStart      = RangeToCompare->CpuStart;
+      FinalRanges[*FinalRangeCount].PhysicalStart = RangeToCompare->PhysicalStart;
+      FinalRanges[*FinalRangeCount].RegionState   = RangeToCompare->RegionState | EFI_ALLOCATED;
+      FinalRanges[*FinalRangeCount].PhysicalSize  = ReservedRangeToCompareEnd - RangeToCompare->CpuStart;
+      *FinalRangeCount += 1;
+      //
+      // 3. Update RangeToCompare.
+      //
+      RangeToCompare->CpuStart      += FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      RangeToCompare->PhysicalStart += FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      RangeToCompare->PhysicalSize  -= FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+    }
+  } else if ((ReservedRangeToCompare->SmramReservedStart >= RangeToCompare->CpuStart) &&
+             (ReservedRangeToCompare->SmramReservedStart < RangeToCompareEnd)) {
+    if (ReservedRangeToCompareEnd < RangeToCompareEnd) {
+      //
+      // RangeToCompare  ReservedRangeToCompare
+      // ----                                    ----    --------------------------------------
+      // |  |                                    |  | -> 1. RangeToCompare
+      // |  |            ----                    |--|    --------------------------------------
+      // |  |            |  |                    |  |
+      // |  |            |  |                    |  | -> 2. FinalRanges[*FinalRangeCount] and increment *FinalRangeCount
+      // |  |            |  |                    |  |       ReservedRangeToCompare->SmramReservedSize = 0
+      // |  |            ----                    |--|    --------------------------------------
+      // |  |                                    |  | -> 3. Ranges[*RangeCount] and increment *RangeCount
+      // ----                                    ----    --------------------------------------
+      //
+
+      //
+      // 1. Update RangeToCompare.
+      //
+      RangeToCompare->PhysicalSize = ReservedRangeToCompare->SmramReservedStart - RangeToCompare->CpuStart;
+      //
+      // 2. Update FinalRanges[FinalRangeCount] and increment *FinalRangeCount.
+      //    ReservedRangeToCompare->SmramReservedSize = 0
+      //
+      FinalRanges[*FinalRangeCount].CpuStart      = ReservedRangeToCompare->SmramReservedStart;
+      FinalRanges[*FinalRangeCount].PhysicalStart = RangeToCompare->PhysicalStart + RangeToCompare->PhysicalSize;
+      FinalRanges[*FinalRangeCount].RegionState   = RangeToCompare->RegionState | EFI_ALLOCATED;
+      FinalRanges[*FinalRangeCount].PhysicalSize  = ReservedRangeToCompare->SmramReservedSize;
+      *FinalRangeCount += 1;
+      ReservedRangeToCompare->SmramReservedSize = 0;
+      //
+      // 3. Update Ranges[*RangeCount] and increment *RangeCount.
+      //
+      Ranges[*RangeCount].CpuStart      = FinalRanges[*FinalRangeCount - 1].CpuStart + FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      Ranges[*RangeCount].PhysicalStart = FinalRanges[*FinalRangeCount - 1].PhysicalStart + FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      Ranges[*RangeCount].RegionState   = RangeToCompare->RegionState;
+      Ranges[*RangeCount].PhysicalSize  = RangeToCompareEnd - ReservedRangeToCompareEnd;
+      *RangeCount += 1;
+    } else {
+      //
+      // RangeToCompare  ReservedRangeToCompare
+      // ----                                    ----    --------------------------------------
+      // |  |                                    |  | -> 1. RangeToCompare
+      // |  |            ----                    |--|    --------------------------------------
+      // |  |            |  |                    |  |
+      // |  |            |  |                    |  | -> 2. FinalRanges[*FinalRangeCount] and increment *FinalRangeCount
+      // |  |            |  |                    |  |
+      // ----            |  |                    |--|    --------------------------------------
+      //                 |  |                    |  | -> 3. ReservedRangeToCompare
+      //                 ----                    ----    --------------------------------------
+      //
+
+      //
+      // 1. Update RangeToCompare.
+      //
+      RangeToCompare->PhysicalSize = ReservedRangeToCompare->SmramReservedStart - RangeToCompare->CpuStart;
+      //
+      // 2. Update FinalRanges[FinalRangeCount] and increment *FinalRangeCount.
+      //    ReservedRangeToCompare->SmramReservedSize = 0
+      //
+      FinalRanges[*FinalRangeCount].CpuStart      = ReservedRangeToCompare->SmramReservedStart;
+      FinalRanges[*FinalRangeCount].PhysicalStart = RangeToCompare->PhysicalStart + RangeToCompare->PhysicalSize;
+      FinalRanges[*FinalRangeCount].RegionState   = RangeToCompare->RegionState | EFI_ALLOCATED;
+      FinalRanges[*FinalRangeCount].PhysicalSize  = RangeToCompareEnd - ReservedRangeToCompare->SmramReservedStart;
+      *FinalRangeCount += 1;
+      //
+      // 3. Update ReservedRangeToCompare.
+      //
+      ReservedRangeToCompare->SmramReservedStart += FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+      ReservedRangeToCompare->SmramReservedSize  -= FinalRanges[*FinalRangeCount - 1].PhysicalSize;
+    }
+  }
+}
+
+/**
+  Returns if SMRAM range and SMRAM reserved range are overlapped.
+
+  @param[in] RangeToCompare             Pointer to EFI_SMRAM_DESCRIPTOR to compare.
+  @param[in] ReservedRangeToCompare     Pointer to EFI_SMM_RESERVED_SMRAM_REGION to compare.
+
+  @retval TRUE  There is overlap.
+  @retval FALSE There is no overlap.
+
+**/
+BOOLEAN
+SmmIsSmramOverlap (
+  IN EFI_SMRAM_DESCRIPTOR           *RangeToCompare,
+  IN EFI_SMM_RESERVED_SMRAM_REGION  *ReservedRangeToCompare
+  )
+{
+  UINT64    RangeToCompareEnd;
+  UINT64    ReservedRangeToCompareEnd;
+
+  RangeToCompareEnd         = RangeToCompare->CpuStart + RangeToCompare->PhysicalSize;
+  ReservedRangeToCompareEnd = ReservedRangeToCompare->SmramReservedStart + ReservedRangeToCompare->SmramReservedSize;
+
+  if ((RangeToCompare->CpuStart >= ReservedRangeToCompare->SmramReservedStart) &&
+      (RangeToCompare->CpuStart < ReservedRangeToCompareEnd)) {
+    return TRUE;
+  } else if ((ReservedRangeToCompare->SmramReservedStart >= RangeToCompare->CpuStart) &&
+             (ReservedRangeToCompare->SmramReservedStart < RangeToCompareEnd)) {
+    return TRUE;
+  }
+  return FALSE;
+}
+
+/**
+  Get full SMRAM ranges.
+
+  It will get SMRAM ranges from SmmAccess protocol and SMRAM reserved ranges from
+  SmmConfiguration protocol, split the entries if there is overlap between them.
+  It will also reserve one entry for SMM core.
+
+  @param[out] FullSmramRangeCount   Output pointer to full SMRAM range count.
+
+  @return Pointer to full SMRAM ranges.
+
+**/
+EFI_SMRAM_DESCRIPTOR *
+GetFullSmramRanges (
+  OUT UINTN     *FullSmramRangeCount
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SMM_CONFIGURATION_PROTOCOL    *SmmConfiguration;
+  UINTN                             Size;
+  UINTN                             Index;
+  UINTN                             Index2;
+  EFI_SMRAM_DESCRIPTOR              *FullSmramRanges;
+  UINTN                             TempSmramRangeCount;
+  EFI_SMRAM_DESCRIPTOR              *TempSmramRanges;
+  UINTN                             SmramRangeCount;
+  EFI_SMRAM_DESCRIPTOR              *SmramRanges;
+  UINTN                             SmramReservedCount;
+  EFI_SMM_RESERVED_SMRAM_REGION     *SmramReservedRanges;
+  UINTN                             MaxCount;
+  BOOLEAN                           Rescan;
+
+  //
+  // Get SMM Configuration Protocol if it is present.
+  //
+  SmmConfiguration = NULL;
+  Status = gBS->LocateProtocol (&gEfiSmmConfigurationProtocolGuid, NULL, (VOID **) &SmmConfiguration);
+
+  //
+  // Get SMRAM information.
+  //
+  Size = 0;
+  Status = mSmmAccess->GetCapabilities (mSmmAccess, &Size, NULL);
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+
+  SmramRangeCount = Size / sizeof (EFI_SMRAM_DESCRIPTOR);
+
+  //
+  // Get SMRAM reserved region count.
+  //
+  SmramReservedCount = 0;
+  if (SmmConfiguration != NULL) {
+    while (SmmConfiguration->SmramReservedRegions[SmramReservedCount].SmramReservedSize != 0) {
+      SmramReservedCount++;
+    }
+  }
+
+  if (SmramReservedCount == 0) {
+    //
+    // No reserved SMRAM entry from SMM Configuration Protocol.
+    // Reserve one entry for SMM Core in the full SMRAM ranges.
+    //
+    *FullSmramRangeCount = SmramRangeCount + 1;
+    Size = (*FullSmramRangeCount) * sizeof (EFI_SMRAM_DESCRIPTOR);
+    FullSmramRanges = (EFI_SMRAM_DESCRIPTOR *) AllocateZeroPool (Size);
+    ASSERT (FullSmramRanges != NULL);
+
+    Status = mSmmAccess->GetCapabilities (mSmmAccess, &Size, FullSmramRanges);
+    ASSERT_EFI_ERROR (Status);
+
+    return FullSmramRanges;
+  }
+
+  //
+  // Why MaxCount = X + 2 * Y?
+  // Take Y = 1 as example below, Y > 1 case is just the iteration of Y = 1.
+  //
+  //   X = 1 Y = 1     MaxCount = 3 = 1 + 2 * 1
+  //   ----            ----
+  //   |  |  ----      |--|
+  //   |  |  |  |  ->  |  |
+  //   |  |  ----      |--|
+  //   ----            ----
+  //
+  //   X = 2 Y = 1     MaxCount = 4 = 2 + 2 * 1
+  //   ----            ----
+  //   |  |            |  |
+  //   |  |  ----      |--|
+  //   |  |  |  |      |  |
+  //   |--|  |  |  ->  |--|
+  //   |  |  |  |      |  |
+  //   |  |  ----      |--|
+  //   |  |            |  |
+  //   ----            ----
+  //
+  //   X = 3 Y = 1     MaxCount = 5 = 3 + 2 * 1
+  //   ----            ----
+  //   |  |            |  |
+  //   |  |  ----      |--|
+  //   |--|  |  |      |--|
+  //   |  |  |  |  ->  |  |
+  //   |--|  |  |      |--|
+  //   |  |  ----      |--|
+  //   |  |            |  |
+  //   ----            ----
+  //
+  //   ......
+  //
+  MaxCount = SmramRangeCount + 2 * SmramReservedCount;
+
+  Size = MaxCount * sizeof (EFI_SMM_RESERVED_SMRAM_REGION);
+  SmramReservedRanges = (EFI_SMM_RESERVED_SMRAM_REGION *) AllocatePool (Size);
+  ASSERT (SmramReservedRanges != NULL);
+  for (Index = 0; Index < SmramReservedCount; Index++) {
+    CopyMem (&SmramReservedRanges[Index], &SmmConfiguration->SmramReservedRegions[Index], sizeof (EFI_SMM_RESERVED_SMRAM_REGION));
+  }
+
+  Size = MaxCount * sizeof (EFI_SMRAM_DESCRIPTOR);
+  TempSmramRanges = (EFI_SMRAM_DESCRIPTOR *) AllocatePool (Size);
+  ASSERT (TempSmramRanges != NULL);
+  TempSmramRangeCount = 0;
+
+  SmramRanges = (EFI_SMRAM_DESCRIPTOR *) AllocatePool (Size);
+  ASSERT (SmramRanges != NULL);
+  Status = mSmmAccess->GetCapabilities (mSmmAccess, &Size, SmramRanges);
+  ASSERT_EFI_ERROR (Status);
+
+  do {
+    Rescan = FALSE;
+    for (Index = 0; (Index < SmramRangeCount) && !Rescan; Index++) {
+      //
+      // Skip zero size entry.
+      //
+      if (SmramRanges[Index].PhysicalSize != 0) {
+        for (Index2 = 0; (Index2 < SmramReservedCount) && !Rescan; Index2++) {
+          //
+          // Skip zero size entry.
+          //
+          if (SmramReservedRanges[Index2].SmramReservedSize != 0) {
+            if (SmmIsSmramOverlap (
+                  &SmramRanges[Index],
+                  &SmramReservedRanges[Index2]
+                  )) {
+              //
+              // There is overlap, need to split entry and then rescan.
+              //
+              SmmSplitSmramEntry (
+                &SmramRanges[Index],
+                &SmramReservedRanges[Index2],
+                SmramRanges,
+                &SmramRangeCount,
+                SmramReservedRanges,
+                &SmramReservedCount,
+                TempSmramRanges,
+                &TempSmramRangeCount
+                );
+              Rescan = TRUE;
+            }
+          }
+        }
+        if (!Rescan) {
+          //
+          // No any overlap, copy the entry to the temp SMRAM ranges.
+          // Zero SmramRanges[Index].PhysicalSize = 0;
+          //
+          CopyMem (&TempSmramRanges[TempSmramRangeCount++], &SmramRanges[Index], sizeof (EFI_SMRAM_DESCRIPTOR));
+          SmramRanges[Index].PhysicalSize = 0;
+        }
+      }
+    }
+  } while (Rescan);
+  ASSERT (TempSmramRangeCount <= MaxCount);
+
+  //
+  // Sort the entries,
+  // and reserve one entry for SMM Core in the full SMRAM ranges.
+  //
+  FullSmramRanges = AllocateZeroPool ((TempSmramRangeCount + 1) * sizeof (EFI_SMRAM_DESCRIPTOR));
+  ASSERT (FullSmramRanges != NULL);
+  *FullSmramRangeCount = 0;
+  do {
+    for (Index = 0; Index < TempSmramRangeCount; Index++) {
+      if (TempSmramRanges[Index].PhysicalSize != 0) {
+        break;
+      }
+    }
+    ASSERT (Index < TempSmramRangeCount);
+    for (Index2 = 0; Index2 < TempSmramRangeCount; Index2++) {
+      if ((Index2 != Index) && (TempSmramRanges[Index2].PhysicalSize != 0) && (TempSmramRanges[Index2].CpuStart < TempSmramRanges[Index].CpuStart)) {
+        Index = Index2;
+      }
+    }
+    CopyMem (&FullSmramRanges[*FullSmramRangeCount], &TempSmramRanges[Index], sizeof (EFI_SMRAM_DESCRIPTOR));
+    *FullSmramRangeCount += 1;
+    TempSmramRanges[Index].PhysicalSize = 0;
+  } while (*FullSmramRangeCount < TempSmramRangeCount);
+  ASSERT (*FullSmramRangeCount == TempSmramRangeCount);
+  *FullSmramRangeCount += 1;
+
+  FreePool (SmramRanges);
+  FreePool (SmramReservedRanges);
+  FreePool (TempSmramRanges);
+
+  return FullSmramRanges;
+}
+
+/**
+  The Entry Point for SMM IPL
+
+  Load SMM Core into SMRAM, register SMM Core entry point for SMIs, install 
+  SMM Base 2 Protocol and SMM Communication Protocol, and register for the 
+  critical events required to coordinate between DXE and SMM environments.
+  
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurred when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmIplEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           Index;
+  UINT64                          MaxSize;
+  VOID                            *Registration;
+  UINT64                          SmmCodeSize;
+  EFI_LOAD_FIXED_ADDRESS_CONFIGURATION_TABLE    *LMFAConfigurationTable;
+  EFI_CPU_ARCH_PROTOCOL           *CpuArch;
+  EFI_STATUS                      SetAttrStatus;
+
+  //
+  // Fill in the image handle of the SMM IPL so the SMM Core can use this as the 
+  // ParentImageHandle field of the Load Image Protocol for all SMM Drivers loaded 
+  // by the SMM Core
+  //
+  mSmmCorePrivateData.SmmIplImageHandle = ImageHandle;
+
+  //
+  // Get SMM Access Protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiSmmAccess2ProtocolGuid, NULL, (VOID **)&mSmmAccess);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Get SMM Control2 Protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiSmmControl2ProtocolGuid, NULL, (VOID **)&mSmmControl2);
+  ASSERT_EFI_ERROR (Status);
+
+  gSmmCorePrivate->SmramRanges = GetFullSmramRanges (&gSmmCorePrivate->SmramRangeCount);
+
+  //
+  // Open all SMRAM ranges
+  //
+  Status = mSmmAccess->Open (mSmmAccess);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Print debug message that the SMRAM window is now open.
+  //
+  DEBUG ((DEBUG_INFO, "SMM IPL opened SMRAM window\n"));
+  
+  //
+  // Find the largest SMRAM range between 1MB and 4GB that is at least 256KB - 4K in size
+  //
+  mCurrentSmramRange = NULL;
+  for (Index = 0, MaxSize = SIZE_256KB - EFI_PAGE_SIZE; Index < gSmmCorePrivate->SmramRangeCount; Index++) {
+    //
+    // Skip any SMRAM region that is already allocated, needs testing, or needs ECC initialization
+    //
+    if ((gSmmCorePrivate->SmramRanges[Index].RegionState & (EFI_ALLOCATED | EFI_NEEDS_TESTING | EFI_NEEDS_ECC_INITIALIZATION)) != 0) {
+      continue;
+    }
+
+    if (gSmmCorePrivate->SmramRanges[Index].CpuStart >= BASE_1MB) {
+      if ((gSmmCorePrivate->SmramRanges[Index].CpuStart + gSmmCorePrivate->SmramRanges[Index].PhysicalSize) <= BASE_4GB) {
+        if (gSmmCorePrivate->SmramRanges[Index].PhysicalSize >= MaxSize) {
+          MaxSize = gSmmCorePrivate->SmramRanges[Index].PhysicalSize;
+          mCurrentSmramRange = &gSmmCorePrivate->SmramRanges[Index];
+        }
+      }
+    }
+  }
+
+  if (mCurrentSmramRange != NULL) {
+    //
+    // Print debug message showing SMRAM window that will be used by SMM IPL and SMM Core
+    //
+    DEBUG ((DEBUG_INFO, "SMM IPL found SMRAM window %p - %p\n", 
+      (VOID *)(UINTN)mCurrentSmramRange->CpuStart, 
+      (VOID *)(UINTN)(mCurrentSmramRange->CpuStart + mCurrentSmramRange->PhysicalSize - 1)
+      ));
+
+    GetSmramCacheRange (mCurrentSmramRange, &mSmramCacheBase, &mSmramCacheSize);
+    //
+    // If CPU AP is present, attempt to set SMRAM cacheability to WB
+    // Note that it is expected that cacheability of SMRAM has been set to WB if CPU AP
+    // is not available here.
+    //
+    CpuArch = NULL;
+    Status = gBS->LocateProtocol (&gEfiCpuArchProtocolGuid, NULL, (VOID **)&CpuArch);
+    if (!EFI_ERROR (Status)) {
+      Status = gDS->SetMemorySpaceAttributes(
+                      mSmramCacheBase, 
+                      mSmramCacheSize,
+                      EFI_MEMORY_WB
+                      );
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_WARN, "SMM IPL failed to set SMRAM window to EFI_MEMORY_WB\n"));
+      }  
+    }
+    //
+    // if Loading module at Fixed Address feature is enabled, save the SMRAM base to Load
+    // Modules At Fixed Address Configuration Table.
+    //
+    if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+      //
+      // Build tool will calculate the smm code size and then patch the PcdLoadFixAddressSmmCodePageNumber
+      //
+      SmmCodeSize = LShiftU64 (PcdGet32(PcdLoadFixAddressSmmCodePageNumber), EFI_PAGE_SHIFT);
+      //
+      // The SMRAM available memory is assumed to be larger than SmmCodeSize
+      //
+      ASSERT (mCurrentSmramRange->PhysicalSize > SmmCodeSize);
+      //
+      // Retrieve Load modules At fixed address configuration table and save the SMRAM base.
+      //
+      Status = EfiGetSystemConfigurationTable (
+                &gLoadFixedAddressConfigurationTableGuid,
+               (VOID **) &LMFAConfigurationTable
+               );
+      if (!EFI_ERROR (Status) && LMFAConfigurationTable != NULL) {
+        LMFAConfigurationTable->SmramBase = mCurrentSmramRange->CpuStart;
+        //
+        // Print the SMRAM base
+        //
+        DEBUG ((EFI_D_INFO, "LOADING MODULE FIXED INFO: TSEG BASE is %x. \n", LMFAConfigurationTable->SmramBase));
+      }
+    }
+    //
+    // Load SMM Core into SMRAM and execute it from SMRAM
+    //
+    Status = ExecuteSmmCoreFromSmram (
+               mCurrentSmramRange,
+               &gSmmCorePrivate->SmramRanges[gSmmCorePrivate->SmramRangeCount - 1],
+               gSmmCorePrivate
+               );
+    if (EFI_ERROR (Status)) {
+      //
+      // Print error message that the SMM Core failed to be loaded and executed.
+      //
+      DEBUG ((DEBUG_ERROR, "SMM IPL could not load and execute SMM Core from SMRAM\n"));
+
+      //
+      // Attempt to reset SMRAM cacheability to UC
+      //
+      if (CpuArch != NULL) {
+        SetAttrStatus = gDS->SetMemorySpaceAttributes(
+                               mSmramCacheBase, 
+                               mSmramCacheSize,
+                               EFI_MEMORY_UC
+                               );
+        if (EFI_ERROR (SetAttrStatus)) {
+          DEBUG ((DEBUG_WARN, "SMM IPL failed to reset SMRAM window to EFI_MEMORY_UC\n"));
+        }  
+      }
+    }
+  } else {
+    //
+    // Print error message that there are not enough SMRAM resources to load the SMM Core.
+    //
+    DEBUG ((DEBUG_ERROR, "SMM IPL could not find a large enough SMRAM region to load SMM Core\n"));
+  }
+
+  //
+  // If the SMM Core could not be loaded then close SMRAM window, free allocated 
+  // resources, and return an error so SMM IPL will be unloaded.
+  //
+  if (mCurrentSmramRange == NULL || EFI_ERROR (Status)) {
+    //
+    // Close all SMRAM ranges
+    //
+    Status = mSmmAccess->Close (mSmmAccess);
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Print debug message that the SMRAM window is now closed.
+    //
+    DEBUG ((DEBUG_INFO, "SMM IPL closed SMRAM window\n"));
+
+    //
+    // Free all allocated resources
+    //
+    FreePool (gSmmCorePrivate->SmramRanges);
+
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // Install SMM Base2 Protocol and SMM Communication Protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mSmmIplHandle,
+                  &gEfiSmmBase2ProtocolGuid,         &mSmmBase2,
+                  &gEfiSmmCommunicationProtocolGuid, &mSmmCommunication,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Create the set of protocol and event notififcations that the SMM IPL requires
+  //
+  for (Index = 0; mSmmIplEvents[Index].NotifyFunction != NULL; Index++) {
+    if (mSmmIplEvents[Index].Protocol) {
+      mSmmIplEvents[Index].Event = EfiCreateProtocolNotifyEvent (
+                                     mSmmIplEvents[Index].Guid,
+                                     mSmmIplEvents[Index].NotifyTpl,
+                                     mSmmIplEvents[Index].NotifyFunction,
+                                     mSmmIplEvents[Index].NotifyContext,
+                                    &Registration
+                                    );
+    } else {
+      Status = gBS->CreateEventEx (
+                      EVT_NOTIFY_SIGNAL,
+                      mSmmIplEvents[Index].NotifyTpl,
+                      mSmmIplEvents[Index].NotifyFunction,
+                      mSmmIplEvents[Index].NotifyContext,
+                      mSmmIplEvents[Index].Guid,
+                      &mSmmIplEvents[Index].Event
+                      );
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.inf b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.inf
new file mode 100644
index 0000000000..6f027a6056
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.inf
@@ -0,0 +1,94 @@
+## @file
+#  This module provide an SMM CIS compliant implementation of SMM IPL.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PiSmmIpl
+  MODULE_UNI_FILE                = PiSmmIpl.uni
+  FILE_GUID                      = 2FA2A6DA-11D5-4dc3-999A-749648B03C56
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = SmmIplEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  PiSmmIpl.c
+  PiSmmCorePrivateData.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  
+[LibraryClasses]
+  UefiDriverEntryPoint
+  BaseLib
+  BaseMemoryLib
+  PeCoffLib
+  CacheMaintenanceLib  
+  MemoryAllocationLib
+  DebugLib
+  UefiBootServicesTableLib
+  DxeServicesTableLib
+  UefiLib
+  UefiRuntimeLib
+  DxeServicesLib
+  PcdLib
+  
+[Protocols]
+  gEfiSmmBase2ProtocolGuid                      ## PRODUCES
+  gEfiSmmCommunicationProtocolGuid              ## PRODUCES
+  gEfiSmmAccess2ProtocolGuid                    ## CONSUMES
+  ## NOTIFY
+  ## CONSUMES
+  gEfiSmmConfigurationProtocolGuid
+  gEfiSmmControl2ProtocolGuid                   ## CONSUMES
+  ## NOTIFY
+  ## SOMETIMES_CONSUMES
+  ## UNDEFINED # Used to do smm communcation
+  gEfiDxeSmmReadyToLockProtocolGuid
+  gEfiCpuArchProtocolGuid                       ## SOMETIMES_CONSUMES
+
+[Guids]
+  ## CONSUMES           ## Event
+  ## PRODUCES           ## UNDEFINED # Used to do smm communcation
+  gEfiEventDxeDispatchGuid
+  gEfiEventReadyToBootGuid                      ## CONSUMES             ## Event
+  ## SOMETIMES_CONSUMES ## Event
+  ## SOMETIMES_PRODUCES ## UNDEFINED # Used to do smm communcation
+  gEfiEventLegacyBootGuid
+  ## SOMETIMES_CONSUMES ## Event
+  ## SOMETIMES_PRODUCES ## UNDEFINED # Used to do smm communcation
+  gEfiEventExitBootServicesGuid
+  ## SOMETIMES_CONSUMES ## Event
+  ## SOMETIMES_PRODUCES ## UNDEFINED # Used to do smm communcation
+  gEfiEventReadyToBootGuid
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES             ## Event
+  gEfiEndOfDxeEventGroupGuid                    ## CONSUMES             ## Event
+  gLoadFixedAddressConfigurationTableGuid       ## SOMETIMES_CONSUMES   ## SystemTable
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressSmmCodePageNumber     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable        ## CONSUMES
+
+[Depex]
+  gEfiSmmAccess2ProtocolGuid AND gEfiSmmControl2ProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PiSmmIplExtra.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.uni b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.uni
new file mode 100644
index 0000000000..5c98fd3b12
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIpl.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIplExtra.uni b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIplExtra.uni
new file mode 100644
index 0000000000..352b7722fc
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/PiSmmIplExtra.uni
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Pool.c b/Core/MdeModulePkg/Core/PiSmmCore/Pool.c
new file mode 100644
index 0000000000..761988e416
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Pool.c
@@ -0,0 +1,325 @@
+/** @file
+  SMM Memory pool management functions.
+
+  Copyright (c) 2009 - 2015, 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 "PiSmmCore.h"
+
+LIST_ENTRY  mSmmPoolLists[MAX_POOL_INDEX];
+//
+// To cache the SMRAM base since when Loading modules At fixed address feature is enabled, 
+// all module is assigned an offset relative the SMRAM base in build time.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED  EFI_PHYSICAL_ADDRESS       gLoadModuleAtFixAddressSmramBase = 0;
+
+/**
+  Called to initialize the memory service.
+
+  @param   SmramRangeCount       Number of SMRAM Regions
+  @param   SmramRanges           Pointer to SMRAM Descriptors
+
+**/
+VOID
+SmmInitializeMemoryServices (
+  IN UINTN                 SmramRangeCount,
+  IN EFI_SMRAM_DESCRIPTOR  *SmramRanges
+  )
+{
+  UINTN                  Index;
+ 	UINT64                 SmmCodeSize;
+ 	UINTN                  CurrentSmramRangesIndex;
+ 	UINT64                 MaxSize;
+
+  //
+  // Initialize Pool list
+  //
+  for (Index = sizeof (mSmmPoolLists) / sizeof (*mSmmPoolLists); Index > 0;) {
+    InitializeListHead (&mSmmPoolLists[--Index]);
+  }
+  CurrentSmramRangesIndex = 0;
+  //
+  // If Loadding Module At fixed Address feature is enabled, cache the SMRAM base here
+  //
+  if (PcdGet64(PcdLoadModuleAtFixAddressEnable) != 0) {
+    //
+    // Build tool will calculate the smm code size and then patch the PcdLoadFixAddressSmmCodePageNumber
+    //
+    SmmCodeSize = LShiftU64 (PcdGet32(PcdLoadFixAddressSmmCodePageNumber), EFI_PAGE_SHIFT);
+    
+    //
+    // Find the largest SMRAM range between 1MB and 4GB that is at least 256KB - 4K in size
+    //
+    for (Index = 0, MaxSize = SIZE_256KB - EFI_PAGE_SIZE; Index < SmramRangeCount; Index++) {
+      //
+      // Skip any SMRAM region that is already allocated, needs testing, or needs ECC initialization
+      //
+      if ((SmramRanges[Index].RegionState & (EFI_ALLOCATED | EFI_NEEDS_TESTING | EFI_NEEDS_ECC_INITIALIZATION)) != 0) {
+        continue;
+      }
+
+      if (SmramRanges[Index].CpuStart >= BASE_1MB) {
+        if ((SmramRanges[Index].CpuStart + SmramRanges[Index].PhysicalSize) <= BASE_4GB) {
+          if (SmramRanges[Index].PhysicalSize >= MaxSize) {
+            MaxSize = SmramRanges[Index].PhysicalSize;
+            CurrentSmramRangesIndex = Index;
+          }
+        }
+      }
+    }
+    gLoadModuleAtFixAddressSmramBase = SmramRanges[CurrentSmramRangesIndex].CpuStart;
+    
+    //
+    // cut out a memory range from this SMRAM range with the size SmmCodeSize to hold SMM driver code
+    // A notable thing is that SMM core is already loaded into this range.
+    //
+    SmramRanges[CurrentSmramRangesIndex].CpuStart     = SmramRanges[CurrentSmramRangesIndex].CpuStart + SmmCodeSize; 
+    SmramRanges[CurrentSmramRangesIndex].PhysicalSize = SmramRanges[CurrentSmramRangesIndex].PhysicalSize - SmmCodeSize;
+  }
+  //
+  // Initialize free SMRAM regions
+  //
+  for (Index = 0; Index < SmramRangeCount; Index++) {
+    SmmAddMemoryRegion (
+      SmramRanges[Index].CpuStart,
+      SmramRanges[Index].PhysicalSize,
+      EfiConventionalMemory,
+      SmramRanges[Index].RegionState
+      );
+  }
+
+}
+
+/**
+  Internal Function. Allocate a pool by specified PoolIndex.
+
+  @param  PoolIndex             Index which indicate the Pool size.
+  @param  FreePoolHdr           The returned Free pool.
+
+  @retval EFI_OUT_OF_RESOURCES   Allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+InternalAllocPoolByIndex (
+  IN  UINTN             PoolIndex,
+  OUT FREE_POOL_HEADER  **FreePoolHdr
+  )
+{
+  EFI_STATUS            Status;
+  FREE_POOL_HEADER      *Hdr;
+  EFI_PHYSICAL_ADDRESS  Address;
+
+  ASSERT (PoolIndex <= MAX_POOL_INDEX);
+  Status = EFI_SUCCESS;
+  Hdr = NULL;
+  if (PoolIndex == MAX_POOL_INDEX) {
+    Status = SmmInternalAllocatePages (AllocateAnyPages, EfiRuntimeServicesData, EFI_SIZE_TO_PAGES (MAX_POOL_SIZE << 1), &Address);
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    Hdr = (FREE_POOL_HEADER *) (UINTN) Address;
+  } else if (!IsListEmpty (&mSmmPoolLists[PoolIndex])) {
+    Hdr = BASE_CR (GetFirstNode (&mSmmPoolLists[PoolIndex]), FREE_POOL_HEADER, Link);
+    RemoveEntryList (&Hdr->Link);
+  } else {
+    Status = InternalAllocPoolByIndex (PoolIndex + 1, &Hdr);
+    if (!EFI_ERROR (Status)) {
+      Hdr->Header.Size >>= 1;
+      Hdr->Header.Available = TRUE;
+      InsertHeadList (&mSmmPoolLists[PoolIndex], &Hdr->Link);
+      Hdr = (FREE_POOL_HEADER*)((UINT8*)Hdr + Hdr->Header.Size);
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    Hdr->Header.Size = MIN_POOL_SIZE << PoolIndex;
+    Hdr->Header.Available = FALSE;
+  }
+
+  *FreePoolHdr = Hdr;
+  return Status;
+}
+
+/**
+  Internal Function. Free a pool by specified PoolIndex.
+
+  @param  FreePoolHdr           The pool to free.
+
+  @retval EFI_SUCCESS           Pool successfully freed.
+
+**/
+EFI_STATUS
+InternalFreePoolByIndex (
+  IN FREE_POOL_HEADER  *FreePoolHdr
+  )
+{
+  UINTN  PoolIndex;
+
+  ASSERT ((FreePoolHdr->Header.Size & (FreePoolHdr->Header.Size - 1)) == 0);
+  ASSERT (((UINTN)FreePoolHdr & (FreePoolHdr->Header.Size - 1)) == 0);
+  ASSERT (FreePoolHdr->Header.Size >= MIN_POOL_SIZE);
+
+  PoolIndex = (UINTN) (HighBitSet32 ((UINT32)FreePoolHdr->Header.Size) - MIN_POOL_SHIFT);
+  FreePoolHdr->Header.Available = TRUE;
+  ASSERT (PoolIndex < MAX_POOL_INDEX);
+  InsertHeadList (&mSmmPoolLists[PoolIndex], &FreePoolHdr->Link);
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate.
+  @param  Size                   The amount of pool to allocate.
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool.
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid.
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalAllocatePool (
+  IN   EFI_MEMORY_TYPE  PoolType,
+  IN   UINTN            Size,
+  OUT  VOID             **Buffer
+  )
+{
+  POOL_HEADER           *PoolHdr;
+  FREE_POOL_HEADER      *FreePoolHdr;
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Address;
+  UINTN                 PoolIndex;
+
+  if (PoolType != EfiRuntimeServicesCode &&
+      PoolType != EfiRuntimeServicesData) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Size += sizeof (*PoolHdr);
+  if (Size > MAX_POOL_SIZE) {
+    Size = EFI_SIZE_TO_PAGES (Size);
+    Status = SmmInternalAllocatePages (AllocateAnyPages, PoolType, Size, &Address);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    PoolHdr = (POOL_HEADER*)(UINTN)Address;
+    PoolHdr->Size = EFI_PAGES_TO_SIZE (Size);
+    PoolHdr->Available = FALSE;
+    *Buffer = PoolHdr + 1;
+    return Status;
+  }
+
+  Size = (Size + MIN_POOL_SIZE - 1) >> MIN_POOL_SHIFT;
+  PoolIndex = (UINTN) HighBitSet32 ((UINT32)Size);
+  if ((Size & (Size - 1)) != 0) {
+    PoolIndex++;
+  }
+
+  Status = InternalAllocPoolByIndex (PoolIndex, &FreePoolHdr);
+  if (!EFI_ERROR(Status)) {
+    *Buffer = &FreePoolHdr->Header + 1;
+  }
+  return Status;
+}
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate.
+  @param  Size                   The amount of pool to allocate.
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool.
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid.
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePool (
+  IN   EFI_MEMORY_TYPE  PoolType,
+  IN   UINTN            Size,
+  OUT  VOID             **Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = SmmInternalAllocatePool (PoolType, Size, Buffer);
+  if (!EFI_ERROR (Status)) {
+    SmmCoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionAllocatePool, PoolType, Size, *Buffer);
+  }
+  return Status;
+}
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free.
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmInternalFreePool (
+  IN VOID  *Buffer
+  )
+{
+  FREE_POOL_HEADER  *FreePoolHdr;
+
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FreePoolHdr = (FREE_POOL_HEADER*)((POOL_HEADER*)Buffer - 1);
+  ASSERT (!FreePoolHdr->Header.Available);
+
+  if (FreePoolHdr->Header.Size > MAX_POOL_SIZE) {
+    ASSERT (((UINTN)FreePoolHdr & EFI_PAGE_MASK) == 0);
+    ASSERT ((FreePoolHdr->Header.Size & EFI_PAGE_MASK) == 0);
+    return SmmInternalFreePages (
+             (EFI_PHYSICAL_ADDRESS)(UINTN)FreePoolHdr,
+             EFI_SIZE_TO_PAGES (FreePoolHdr->Header.Size)
+             );
+  }
+  return InternalFreePoolByIndex (FreePoolHdr);
+}
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free.
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePool (
+  IN VOID  *Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = SmmInternalFreePool (Buffer);
+  if (!EFI_ERROR (Status)) {
+    SmmCoreUpdateProfile ((EFI_PHYSICAL_ADDRESS) (UINTN) RETURN_ADDRESS (0), MemoryProfileActionFreePool, 0, 0, Buffer);
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/Smi.c b/Core/MdeModulePkg/Core/PiSmmCore/Smi.c
new file mode 100644
index 0000000000..816d0f5193
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/Smi.c
@@ -0,0 +1,332 @@
+/** @file
+  SMI management.
+
+  Copyright (c) 2009 - 2013, 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 "PiSmmCore.h"
+
+//
+// SMM_HANDLER - used for each SMM handler
+//
+
+#define SMI_ENTRY_SIGNATURE  SIGNATURE_32('s','m','i','e')
+
+ typedef struct {
+  UINTN       Signature;
+  LIST_ENTRY  AllEntries;  // All entries
+
+  EFI_GUID    HandlerType; // Type of interrupt
+  LIST_ENTRY  SmiHandlers; // All handlers
+} SMI_ENTRY;
+
+#define SMI_HANDLER_SIGNATURE  SIGNATURE_32('s','m','i','h')
+
+ typedef struct {
+  UINTN                         Signature;
+  LIST_ENTRY                    Link;        // Link on SMI_ENTRY.SmiHandlers
+  EFI_SMM_HANDLER_ENTRY_POINT2  Handler;     // The smm handler's entry point
+  SMI_ENTRY                     *SmiEntry;
+} SMI_HANDLER;
+
+LIST_ENTRY  mRootSmiHandlerList = INITIALIZE_LIST_HEAD_VARIABLE (mRootSmiHandlerList);
+LIST_ENTRY  mSmiEntryList       = INITIALIZE_LIST_HEAD_VARIABLE (mSmiEntryList);
+
+/**
+  Finds the SMI entry for the requested handler type.
+
+  @param  HandlerType            The type of the interrupt
+  @param  Create                 Create a new entry if not found
+
+  @return SMI entry
+
+**/
+SMI_ENTRY  *
+EFIAPI
+SmmCoreFindSmiEntry (
+  IN EFI_GUID  *HandlerType,
+  IN BOOLEAN   Create
+  )
+{
+  LIST_ENTRY  *Link;
+  SMI_ENTRY   *Item;
+  SMI_ENTRY   *SmiEntry;
+
+  //
+  // Search the SMI entry list for the matching GUID
+  //
+  SmiEntry = NULL;
+  for (Link = mSmiEntryList.ForwardLink;
+       Link != &mSmiEntryList;
+       Link = Link->ForwardLink) {
+
+    Item = CR (Link, SMI_ENTRY, AllEntries, SMI_ENTRY_SIGNATURE);
+    if (CompareGuid (&Item->HandlerType, HandlerType)) {
+      //
+      // This is the SMI entry
+      //
+      SmiEntry = Item;
+      break;
+    }
+  }
+
+  //
+  // If the protocol entry was not found and Create is TRUE, then
+  // allocate a new entry
+  //
+  if ((SmiEntry == NULL) && Create) {
+    SmiEntry = AllocatePool (sizeof(SMI_ENTRY));
+    if (SmiEntry != NULL) {
+      //
+      // Initialize new SMI entry structure
+      //
+      SmiEntry->Signature = SMI_ENTRY_SIGNATURE;
+      CopyGuid ((VOID *)&SmiEntry->HandlerType, HandlerType);
+      InitializeListHead (&SmiEntry->SmiHandlers);
+
+      //
+      // Add it to SMI entry list
+      //
+      InsertTailList (&mSmiEntryList, &SmiEntry->AllEntries);
+    }
+  }
+  return SmiEntry;
+}
+
+/**
+  Manage SMI of a particular type.
+
+  @param  HandlerType    Points to the handler type or NULL for root SMI handlers.
+  @param  Context        Points to an optional context buffer.
+  @param  CommBuffer     Points to the optional communication buffer.
+  @param  CommBufferSize Points to the size of the optional communication buffer.
+
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING  Interrupt source was processed successfully but not quiesced.
+  @retval EFI_INTERRUPT_PENDING              One or more SMI sources could not be quiesced.
+  @retval EFI_NOT_FOUND                      Interrupt source was not handled or quiesced.
+  @retval EFI_SUCCESS                        Interrupt source was handled and quiesced.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiManage (
+  IN     CONST EFI_GUID  *HandlerType,
+  IN     CONST VOID      *Context         OPTIONAL,
+  IN OUT VOID            *CommBuffer      OPTIONAL,
+  IN OUT UINTN           *CommBufferSize  OPTIONAL
+  )
+{
+  LIST_ENTRY   *Link;
+  LIST_ENTRY   *Head;
+  SMI_ENTRY    *SmiEntry;
+  SMI_HANDLER  *SmiHandler;
+  BOOLEAN      SuccessReturn;
+  EFI_STATUS   Status;
+  
+  Status = EFI_NOT_FOUND;
+  SuccessReturn = FALSE;
+  if (HandlerType == NULL) {
+    //
+    // Root SMI handler
+    //
+
+    Head = &mRootSmiHandlerList;
+  } else {
+    //
+    // Non-root SMI handler
+    //
+    SmiEntry = SmmCoreFindSmiEntry ((EFI_GUID *) HandlerType, FALSE);
+    if (SmiEntry == NULL) {
+      //
+      // There is no handler registered for this interrupt source
+      //
+      return Status;
+    }
+
+    Head = &SmiEntry->SmiHandlers;
+  }
+
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    SmiHandler = CR (Link, SMI_HANDLER, Link, SMI_HANDLER_SIGNATURE);
+
+    Status = SmiHandler->Handler (
+               (EFI_HANDLE) SmiHandler,
+               Context,
+               CommBuffer,
+               CommBufferSize
+               );
+
+    switch (Status) {
+    case EFI_INTERRUPT_PENDING:
+      //
+      // If a handler returns EFI_INTERRUPT_PENDING and HandlerType is not NULL then
+      // no additional handlers will be processed and EFI_INTERRUPT_PENDING will be returned.
+      //
+      if (HandlerType != NULL) {
+        return EFI_INTERRUPT_PENDING;
+      }
+      break;
+
+    case EFI_SUCCESS:
+      //
+      // If at least one of the handlers returns EFI_SUCCESS then the function will return
+      // EFI_SUCCESS. If a handler returns EFI_SUCCESS and HandlerType is not NULL then no
+      // additional handlers will be processed.
+      //
+      if (HandlerType != NULL) {
+        return EFI_SUCCESS;
+      }
+      SuccessReturn = TRUE;
+      break;
+
+    case EFI_WARN_INTERRUPT_SOURCE_QUIESCED:
+      //
+      // If at least one of the handlers returns EFI_WARN_INTERRUPT_SOURCE_QUIESCED
+      // then the function will return EFI_SUCCESS. 
+      //
+      SuccessReturn = TRUE;
+      break;
+
+    case EFI_WARN_INTERRUPT_SOURCE_PENDING:
+      //
+      // If all the handlers returned EFI_WARN_INTERRUPT_SOURCE_PENDING
+      // then EFI_WARN_INTERRUPT_SOURCE_PENDING will be returned.
+      //
+      break;
+
+    default:
+      //
+      // Unexpected status code returned.
+      //
+      ASSERT (FALSE);
+      break;
+    }
+  }
+
+  if (SuccessReturn) {
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+/**
+  Registers a handler to execute within SMM.
+
+  @param  Handler        Handler service funtion pointer.
+  @param  HandlerType    Points to the handler type or NULL for root SMI handlers.
+  @param  DispatchHandle On return, contains a unique handle which can be used to later unregister the handler function.
+
+  @retval EFI_SUCCESS           Handler register success.
+  @retval EFI_INVALID_PARAMETER Handler or DispatchHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiHandlerRegister (
+  IN  EFI_SMM_HANDLER_ENTRY_POINT2  Handler,
+  IN  CONST EFI_GUID                *HandlerType  OPTIONAL,
+  OUT EFI_HANDLE                    *DispatchHandle
+  )
+{
+  SMI_HANDLER  *SmiHandler;
+  SMI_ENTRY    *SmiEntry;
+  LIST_ENTRY   *List;
+
+  if (Handler == NULL || DispatchHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  SmiHandler = AllocateZeroPool (sizeof (SMI_HANDLER));
+  if (SmiHandler == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SmiHandler->Signature = SMI_HANDLER_SIGNATURE;
+  SmiHandler->Handler = Handler;
+
+  if (HandlerType == NULL) {
+    //
+    // This is root SMI handler
+    //
+    SmiEntry = NULL;
+    List = &mRootSmiHandlerList;
+  } else {
+    //
+    // None root SMI handler
+    //
+    SmiEntry = SmmCoreFindSmiEntry ((EFI_GUID *) HandlerType, TRUE);
+    if (SmiEntry == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    List = &SmiEntry->SmiHandlers;
+  }
+
+  SmiHandler->SmiEntry = SmiEntry;
+  InsertTailList (List, &SmiHandler->Link);
+
+  *DispatchHandle = (EFI_HANDLE) SmiHandler;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unregister a handler in SMM.
+
+  @param  DispatchHandle  The handle that was specified when the handler was registered.
+
+  @retval EFI_SUCCESS           Handler function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER DispatchHandle does not refer to a valid handle.
+
+**/
+EFI_STATUS
+EFIAPI
+SmiHandlerUnRegister (
+  IN EFI_HANDLE  DispatchHandle
+  )
+{
+  SMI_HANDLER  *SmiHandler;
+  SMI_ENTRY    *SmiEntry;
+
+  SmiHandler = (SMI_HANDLER *) DispatchHandle;
+
+  if (SmiHandler == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (SmiHandler->Signature != SMI_HANDLER_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  SmiEntry = SmiHandler->SmiEntry;
+
+  RemoveEntryList (&SmiHandler->Link);
+  FreePool (SmiHandler);
+
+  if (SmiEntry == NULL) {
+    //
+    // This is root SMI handler
+    //
+    return EFI_SUCCESS;
+  }
+
+  if (IsListEmpty (&SmiEntry->SmiHandlers)) {
+    //
+    // No handler registered for this interrupt now, remove the SMI_ENTRY
+    //
+    RemoveEntryList (&SmiEntry->AllEntries);
+
+    FreePool (SmiEntry);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Core/PiSmmCore/SmramProfileRecord.c b/Core/MdeModulePkg/Core/PiSmmCore/SmramProfileRecord.c
new file mode 100644
index 0000000000..51fa7f947d
--- /dev/null
+++ b/Core/MdeModulePkg/Core/PiSmmCore/SmramProfileRecord.c
@@ -0,0 +1,2104 @@
+/** @file
+  Support routines for SMRAM profile.
+
+  Copyright (c) 2014 - 2016, 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 "PiSmmCore.h"
+
+#define IS_SMRAM_PROFILE_ENABLED ((PcdGet8 (PcdMemoryProfilePropertyMask) & BIT1) != 0)
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_CONTEXT        Context;
+  LIST_ENTRY                    *DriverInfoList;
+} MEMORY_PROFILE_CONTEXT_DATA;
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_DRIVER_INFO    DriverInfo;
+  LIST_ENTRY                    *AllocInfoList;
+  LIST_ENTRY                    Link;
+} MEMORY_PROFILE_DRIVER_INFO_DATA;
+
+typedef struct {
+  UINT32                        Signature;
+  MEMORY_PROFILE_ALLOC_INFO     AllocInfo;
+  LIST_ENTRY                    Link;
+} MEMORY_PROFILE_ALLOC_INFO_DATA;
+
+//
+// When free memory less than 4 pages, dump it.
+//
+#define SMRAM_INFO_DUMP_PAGE_THRESHOLD  4
+
+GLOBAL_REMOVE_IF_UNREFERENCED MEMORY_PROFILE_FREE_MEMORY mSmramFreeMemory = {
+  {
+    MEMORY_PROFILE_FREE_MEMORY_SIGNATURE,
+    sizeof (MEMORY_PROFILE_FREE_MEMORY),
+    MEMORY_PROFILE_FREE_MEMORY_REVISION
+  },
+  0,
+  0
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED LIST_ENTRY  mImageQueue = INITIALIZE_LIST_HEAD_VARIABLE (mImageQueue);
+GLOBAL_REMOVE_IF_UNREFERENCED MEMORY_PROFILE_CONTEXT_DATA mSmramProfileContext = {
+  MEMORY_PROFILE_CONTEXT_SIGNATURE,
+  {
+    {
+      MEMORY_PROFILE_CONTEXT_SIGNATURE,
+      sizeof (MEMORY_PROFILE_CONTEXT),
+      MEMORY_PROFILE_CONTEXT_REVISION
+    },
+    0,
+    0,
+    {0},
+    {0},
+    0,
+    0,
+    0
+  },
+  &mImageQueue,
+};
+GLOBAL_REMOVE_IF_UNREFERENCED MEMORY_PROFILE_CONTEXT_DATA *mSmramProfileContextPtr = NULL;
+
+BOOLEAN mSmramReadyToLock;
+BOOLEAN mSmramProfileRecordingStatus = FALSE;
+
+/**
+  Dump SMRAM infromation.
+
+**/
+VOID
+DumpSmramInfo (
+  VOID
+  );
+
+/**
+  Return SMRAM profile context.
+
+  @return SMRAM profile context.
+
+**/
+MEMORY_PROFILE_CONTEXT_DATA *
+GetSmramProfileContext (
+  VOID
+  )
+{
+  return mSmramProfileContextPtr;
+}
+
+/**
+  Retrieves the magic value from the PE/COFF header.
+
+  @param Hdr    The buffer in which to return the PE32, PE32+, or TE header.
+
+  @return EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC - Image is PE32
+  @return EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC - Image is PE32+
+
+**/
+UINT16
+InternalPeCoffGetPeHeaderMagicValue (
+  IN  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr
+  )
+{
+  //
+  // NOTE: Some versions of Linux ELILO for Itanium have an incorrect magic value
+  //       in the PE/COFF Header.  If the MachineType is Itanium(IA64) and the
+  //       Magic value in the OptionalHeader is  EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC
+  //       then override the returned value to EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC
+  //
+  if (Hdr.Pe32->FileHeader.Machine == IMAGE_FILE_MACHINE_IA64 && Hdr.Pe32->OptionalHeader.Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+    return EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC;
+  }
+  //
+  // Return the magic value from the PC/COFF Optional Header
+  //
+  return Hdr.Pe32->OptionalHeader.Magic;
+}
+
+/**
+  Retrieves and returns the Subsystem of a PE/COFF image that has been loaded into system memory.
+  If Pe32Data is NULL, then ASSERT().
+
+  @param Pe32Data   The pointer to the PE/COFF image that is loaded in system memory.
+
+  @return The Subsystem of the PE/COFF image.
+
+**/
+UINT16
+InternalPeCoffGetSubsystem (
+  IN VOID  *Pe32Data
+  )
+{
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION  Hdr;
+  EFI_IMAGE_DOS_HEADER                 *DosHdr;
+  UINT16                               Magic;
+
+  ASSERT (Pe32Data != NULL);
+
+  DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    //
+    // DOS image header is present, so read the PE header after the DOS image header.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) ((UINTN) Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+  } else {
+    //
+    // DOS image header is not present, so PE header is at the image base.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) Pe32Data;
+  }
+
+  if (Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) {
+    return Hdr.Te->Subsystem;
+  } else if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE)  {
+    Magic = InternalPeCoffGetPeHeaderMagicValue (Hdr);
+    if (Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC) {
+      return Hdr.Pe32->OptionalHeader.Subsystem;
+    } else if (Magic == EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC) {
+      return Hdr.Pe32Plus->OptionalHeader.Subsystem;
+    }
+  }
+
+  return 0x0000;
+}
+
+/**
+  Retrieves and returns a pointer to the entry point to a PE/COFF image that has been loaded
+  into system memory with the PE/COFF Loader Library functions.
+
+  Retrieves the entry point to the PE/COFF image specified by Pe32Data and returns this entry
+  point in EntryPoint.  If the entry point could not be retrieved from the PE/COFF image, then
+  return RETURN_INVALID_PARAMETER.  Otherwise return RETURN_SUCCESS.
+  If Pe32Data is NULL, then ASSERT().
+  If EntryPoint is NULL, then ASSERT().
+
+  @param  Pe32Data                  The pointer to the PE/COFF image that is loaded in system memory.
+  @param  EntryPoint                The pointer to entry point to the PE/COFF image to return.
+
+  @retval RETURN_SUCCESS            EntryPoint was returned.
+  @retval RETURN_INVALID_PARAMETER  The entry point could not be found in the PE/COFF image.
+
+**/
+RETURN_STATUS
+InternalPeCoffGetEntryPoint (
+  IN  VOID  *Pe32Data,
+  OUT VOID  **EntryPoint
+  )
+{
+  EFI_IMAGE_DOS_HEADER                  *DosHdr;
+  EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION   Hdr;
+
+  ASSERT (Pe32Data   != NULL);
+  ASSERT (EntryPoint != NULL);
+
+  DosHdr = (EFI_IMAGE_DOS_HEADER *) Pe32Data;
+  if (DosHdr->e_magic == EFI_IMAGE_DOS_SIGNATURE) {
+    //
+    // DOS image header is present, so read the PE header after the DOS image header.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) ((UINTN) Pe32Data + (UINTN) ((DosHdr->e_lfanew) & 0x0ffff));
+  } else {
+    //
+    // DOS image header is not present, so PE header is at the image base.
+    //
+    Hdr.Pe32 = (EFI_IMAGE_NT_HEADERS32 *) Pe32Data;
+  }
+
+  //
+  // Calculate the entry point relative to the start of the image.
+  // AddressOfEntryPoint is common for PE32 & PE32+
+  //
+  if (Hdr.Te->Signature == EFI_TE_IMAGE_HEADER_SIGNATURE) {
+    *EntryPoint = (VOID *) ((UINTN) Pe32Data + (UINTN) (Hdr.Te->AddressOfEntryPoint & 0x0ffffffff) + sizeof (EFI_TE_IMAGE_HEADER) - Hdr.Te->StrippedSize);
+    return RETURN_SUCCESS;
+  } else if (Hdr.Pe32->Signature == EFI_IMAGE_NT_SIGNATURE) {
+    *EntryPoint = (VOID *) ((UINTN) Pe32Data + (UINTN) (Hdr.Pe32->OptionalHeader.AddressOfEntryPoint & 0x0ffffffff));
+    return RETURN_SUCCESS;
+  }
+
+  return RETURN_UNSUPPORTED;
+}
+
+/**
+  Build driver info.
+
+  @param ContextData    Memory profile context.
+  @param FileName       File name of the image.
+  @param ImageBase      Image base address.
+  @param ImageSize      Image size.
+  @param EntryPoint     Entry point of the image.
+  @param ImageSubsystem Image subsystem of the image.
+
+  @param FileType       File type of the image.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+BuildDriverInfo (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize,
+  IN PHYSICAL_ADDRESS               EntryPoint,
+  IN UINT16                         ImageSubsystem,
+  IN EFI_FV_FILETYPE                FileType
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  VOID                              *EntryPointInImage;
+
+  //
+  // Use SmmInternalAllocatePool() that will not update profile for this AllocatePool action.
+  //
+  Status = SmmInternalAllocatePool (
+             EfiRuntimeServicesData,
+             sizeof (*DriverInfoData) + sizeof (LIST_ENTRY),
+             (VOID **) &DriverInfoData
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ASSERT (DriverInfoData != NULL);
+
+  ZeroMem (DriverInfoData, sizeof (*DriverInfoData));
+
+  DriverInfo = &DriverInfoData->DriverInfo;
+  DriverInfoData->Signature = MEMORY_PROFILE_DRIVER_INFO_SIGNATURE;
+  DriverInfo->Header.Signature = MEMORY_PROFILE_DRIVER_INFO_SIGNATURE;
+  DriverInfo->Header.Length = sizeof (MEMORY_PROFILE_DRIVER_INFO);
+  DriverInfo->Header.Revision = MEMORY_PROFILE_DRIVER_INFO_REVISION;
+  if (FileName != NULL) {
+    CopyMem (&DriverInfo->FileName, FileName, sizeof (EFI_GUID));
+  }
+  DriverInfo->ImageBase = ImageBase;
+  DriverInfo->ImageSize = ImageSize;
+  DriverInfo->EntryPoint = EntryPoint;
+  DriverInfo->ImageSubsystem = ImageSubsystem;
+  if ((EntryPoint != 0) && ((EntryPoint < ImageBase) || (EntryPoint >= (ImageBase + ImageSize)))) {
+    //
+    // If the EntryPoint is not in the range of image buffer, it should come from emulation environment.
+    // So patch ImageBuffer here to align the EntryPoint.
+    //
+    Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageBase, &EntryPointInImage);
+    ASSERT_EFI_ERROR (Status);
+    DriverInfo->ImageBase = ImageBase + EntryPoint - (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+  }
+  DriverInfo->FileType = FileType;
+  DriverInfoData->AllocInfoList = (LIST_ENTRY *) (DriverInfoData + 1);
+  InitializeListHead (DriverInfoData->AllocInfoList);
+  DriverInfo->CurrentUsage = 0;
+  DriverInfo->PeakUsage = 0;
+  DriverInfo->AllocRecordCount = 0;
+
+  InsertTailList (ContextData->DriverInfoList, &DriverInfoData->Link);
+  ContextData->Context.ImageCount ++;
+  ContextData->Context.TotalImageSize += DriverInfo->ImageSize;
+
+  return DriverInfoData;
+}
+
+/**
+  Register image to DXE.
+
+  @param FileName       File name of the image.
+  @param ImageBase      Image base address.
+  @param ImageSize      Image size.
+  @param FileType       File type of the image.
+
+**/
+VOID
+RegisterImageToDxe (
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize,
+  IN EFI_FV_FILETYPE                FileType
+  )
+{
+  EFI_STATUS                        Status;
+  EDKII_MEMORY_PROFILE_PROTOCOL     *ProfileProtocol;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *FilePath;
+  UINT8                             TempBuffer[sizeof (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH) + sizeof (EFI_DEVICE_PATH_PROTOCOL)];
+
+  if (IS_SMRAM_PROFILE_ENABLED) {
+
+    FilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *)TempBuffer;
+    Status = gBS->LocateProtocol (&gEdkiiMemoryProfileGuid, NULL, (VOID **) &ProfileProtocol);
+    if (!EFI_ERROR (Status)) {
+      EfiInitializeFwVolDevicepathNode (FilePath, FileName);
+      SetDevicePathEndNode (FilePath + 1);
+
+      Status = ProfileProtocol->RegisterImage (
+                                  ProfileProtocol,
+                                  (EFI_DEVICE_PATH_PROTOCOL *) FilePath,
+                                  ImageBase,
+                                  ImageSize,
+                                  FileType
+                                  );
+    }
+  }
+}
+
+/**
+  Unregister image from DXE.
+
+  @param FileName       File name of the image.
+  @param ImageBase      Image base address.
+  @param ImageSize      Image size.
+
+**/
+VOID
+UnregisterImageFromDxe (
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize
+  )
+{
+  EFI_STATUS                        Status;
+  EDKII_MEMORY_PROFILE_PROTOCOL     *ProfileProtocol;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *FilePath;
+  UINT8                             TempBuffer[sizeof (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH) + sizeof (EFI_DEVICE_PATH_PROTOCOL)];
+
+  if (IS_SMRAM_PROFILE_ENABLED) {
+
+    FilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *)TempBuffer;
+    Status = gBS->LocateProtocol (&gEdkiiMemoryProfileGuid, NULL, (VOID *) &ProfileProtocol);
+    if (!EFI_ERROR (Status)) {
+      EfiInitializeFwVolDevicepathNode (FilePath, FileName);
+      SetDevicePathEndNode (FilePath + 1);
+
+      Status = ProfileProtocol->UnregisterImage (
+                                  ProfileProtocol,
+                                  (EFI_DEVICE_PATH_PROTOCOL *) FilePath,
+                                  ImageBase,
+                                  ImageSize
+                                  );
+    }
+  }
+}
+
+/**
+  Register SMM Core to SMRAM profile.
+
+  @param ContextData    SMRAM profile context.
+
+  @retval TRUE          Register success.
+  @retval FALSE         Register fail.
+
+**/
+BOOLEAN
+RegisterSmmCore (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  PHYSICAL_ADDRESS                  ImageBase;
+
+  ASSERT (ContextData != NULL);
+
+  RegisterImageToDxe (
+    &gEfiCallerIdGuid,
+    gSmmCorePrivate->PiSmmCoreImageBase,
+    gSmmCorePrivate->PiSmmCoreImageSize,
+    EFI_FV_FILETYPE_SMM_CORE
+    );
+
+  ImageBase = gSmmCorePrivate->PiSmmCoreImageBase;
+  DriverInfoData = BuildDriverInfo (
+                     ContextData,
+                     &gEfiCallerIdGuid,
+                     ImageBase,
+                     gSmmCorePrivate->PiSmmCoreImageSize,
+                     gSmmCorePrivate->PiSmmCoreEntryPoint,
+                     InternalPeCoffGetSubsystem ((VOID *) (UINTN) ImageBase),
+                     EFI_FV_FILETYPE_SMM_CORE
+                     );
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Initialize SMRAM profile.
+
+**/
+VOID
+SmramProfileInit (
+  VOID
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA *SmramProfileContext;
+
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return;
+  }
+
+  SmramProfileContext = GetSmramProfileContext ();
+  if (SmramProfileContext != NULL) {
+    return;
+  }
+
+  mSmramProfileRecordingStatus = TRUE;
+  mSmramProfileContextPtr = &mSmramProfileContext;
+
+  RegisterSmmCore (&mSmramProfileContext);
+
+  DEBUG ((EFI_D_INFO, "SmramProfileInit SmramProfileContext - 0x%x\n", &mSmramProfileContext));
+}
+
+/**
+  Register SMM image to SMRAM profile.
+
+  @param DriverEntry    SMM image info.
+  @param RegisterToDxe  Register image to DXE.
+
+  @retval TRUE          Register success.
+  @retval FALSE         Register fail.
+
+**/
+BOOLEAN
+RegisterSmramProfileImage (
+  IN EFI_SMM_DRIVER_ENTRY   *DriverEntry,
+  IN BOOLEAN                RegisterToDxe
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  if (RegisterToDxe) {
+    RegisterImageToDxe (
+      &DriverEntry->FileName,
+      DriverEntry->ImageBuffer,
+      EFI_PAGES_TO_SIZE (DriverEntry->NumberOfPage),
+      EFI_FV_FILETYPE_SMM
+      );
+  }
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = BuildDriverInfo (
+                     ContextData,
+                     &DriverEntry->FileName,
+                     DriverEntry->ImageBuffer,
+                     EFI_PAGES_TO_SIZE (DriverEntry->NumberOfPage),
+                     DriverEntry->ImageEntryPoint,
+                     InternalPeCoffGetSubsystem ((VOID *) (UINTN) DriverEntry->ImageBuffer),
+                     EFI_FV_FILETYPE_SMM
+                     );
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Search image from memory profile.
+
+  @param ContextData    Memory profile context.
+  @param FileName       Image file name.
+  @param Address        Image Address.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+GetMemoryProfileDriverInfoByFileNameAndAddress (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               Address
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    DriverInfo = &DriverInfoData->DriverInfo;
+    if ((CompareGuid (&DriverInfo->FileName, FileName)) &&
+        (Address >= DriverInfo->ImageBase) &&
+        (Address < (DriverInfo->ImageBase + DriverInfo->ImageSize))) {
+      return DriverInfoData;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Search dummy image from SMRAM profile.
+
+  @param ContextData    Memory profile context.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+FindDummyImage (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                   DriverLink,
+                   MEMORY_PROFILE_DRIVER_INFO_DATA,
+                   Link,
+                   MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                   );
+    if (CompareGuid (&gZeroGuid, &DriverInfoData->DriverInfo.FileName)) {
+      return DriverInfoData;
+    }
+  }
+
+  return BuildDriverInfo (ContextData, &gZeroGuid, 0, 0, 0, 0, 0);
+}
+
+/**
+  Search image from memory profile.
+  It will return image, if (Address >= ImageBuffer) AND (Address < ImageBuffer + ImageSize)
+
+  @param ContextData    Memory profile context.
+  @param Address        Image or Function address.
+
+  @return Pointer to memory profile driver info.
+
+**/
+MEMORY_PROFILE_DRIVER_INFO_DATA *
+GetMemoryProfileDriverInfoFromAddress (
+  IN MEMORY_PROFILE_CONTEXT_DATA    *ContextData,
+  IN PHYSICAL_ADDRESS               Address
+  )
+{
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *DriverInfoList;
+
+  DriverInfoList = ContextData->DriverInfoList;
+
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    DriverInfo = &DriverInfoData->DriverInfo;
+    if ((Address >= DriverInfo->ImageBase) &&
+        (Address < (DriverInfo->ImageBase + DriverInfo->ImageSize))) {
+      return DriverInfoData;
+    }
+  }
+
+  //
+  // Should never come here.
+  //
+  return FindDummyImage (ContextData);
+}
+
+/**
+  Unregister image from SMRAM profile.
+
+  @param DriverEntry        SMM image info.
+  @param UnregisterFromDxe  Unregister image from DXE.
+
+  @retval TRUE              Unregister success.
+  @retval FALSE             Unregister fail.
+
+**/
+BOOLEAN
+UnregisterSmramProfileImage (
+  IN EFI_SMM_DRIVER_ENTRY  *DriverEntry,
+  IN BOOLEAN               UnregisterFromDxe
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  EFI_GUID                          *FileName;
+  PHYSICAL_ADDRESS                  ImageAddress;
+  VOID                              *EntryPointInImage;
+
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  if (UnregisterFromDxe) {
+    UnregisterImageFromDxe (
+      &DriverEntry->FileName,
+      DriverEntry->ImageBuffer,
+      EFI_PAGES_TO_SIZE (DriverEntry->NumberOfPage)
+      );
+  }
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = NULL;
+  FileName = &DriverEntry->FileName;
+  ImageAddress = DriverEntry->ImageBuffer;
+  if ((DriverEntry->ImageEntryPoint < ImageAddress) || (DriverEntry->ImageEntryPoint >= (ImageAddress + EFI_PAGES_TO_SIZE (DriverEntry->NumberOfPage)))) {
+    //
+    // If the EntryPoint is not in the range of image buffer, it should come from emulation environment.
+    // So patch ImageAddress here to align the EntryPoint.
+    //
+    Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) ImageAddress, &EntryPointInImage);
+    ASSERT_EFI_ERROR (Status);
+    ImageAddress = ImageAddress + (UINTN) DriverEntry->ImageEntryPoint - (UINTN) EntryPointInImage;
+  }
+  if (FileName != NULL) {
+    DriverInfoData = GetMemoryProfileDriverInfoByFileNameAndAddress (ContextData, FileName, ImageAddress);
+  }
+  if (DriverInfoData == NULL) {
+    DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, ImageAddress);
+  }
+  if (DriverInfoData == NULL) {
+    return FALSE;
+  }
+
+  ContextData->Context.TotalImageSize -= DriverInfoData->DriverInfo.ImageSize;
+
+  DriverInfoData->DriverInfo.ImageBase = 0;
+  DriverInfoData->DriverInfo.ImageSize = 0;
+
+  if (DriverInfoData->DriverInfo.PeakUsage == 0) {
+    ContextData->Context.ImageCount --;
+    RemoveEntryList (&DriverInfoData->Link);
+    //
+    // Use SmmInternalFreePool() that will not update profile for this FreePool action.
+    //
+    SmmInternalFreePool (DriverInfoData);
+  }
+
+  return TRUE;
+}
+
+/**
+  Return if this memory type needs to be recorded into memory profile.
+  Only need to record EfiRuntimeServicesCode and EfiRuntimeServicesData for SMRAM profile.
+
+  @param MemoryType     Memory type.
+
+  @retval TRUE          This memory type need to be recorded.
+  @retval FALSE         This memory type need not to be recorded.
+
+**/
+BOOLEAN
+SmmCoreNeedRecordProfile (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  UINT64 TestBit;
+
+  if (MemoryType != EfiRuntimeServicesCode &&
+      MemoryType != EfiRuntimeServicesData) {
+    return FALSE;
+  }
+
+  TestBit = LShiftU64 (1, MemoryType);
+
+  if ((PcdGet64 (PcdMemoryProfileMemoryType) & TestBit) != 0) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Convert EFI memory type to profile memory index. The rule is:
+  If BIOS memory type (0 ~ EfiMaxMemoryType - 1), ProfileMemoryIndex = MemoryType.
+  As SMRAM profile is only to record EfiRuntimeServicesCode and EfiRuntimeServicesData,
+  so return input memory type directly.
+
+  @param MemoryType     Memory type.
+
+  @return EFI memory type as profile memory index.
+
+**/
+EFI_MEMORY_TYPE
+GetProfileMemoryIndex (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  return MemoryType;
+}
+
+/**
+  Update SMRAM profile FreeMemoryPages information
+
+  @param ContextData    Memory profile context.
+
+**/
+VOID
+SmramProfileUpdateFreePages (
+  IN MEMORY_PROFILE_CONTEXT_DATA  *ContextData
+  )
+{
+  LIST_ENTRY                      *Node;
+  FREE_PAGE_LIST                  *Pages;
+  LIST_ENTRY                      *FreePageList;
+  UINTN                           NumberOfPages;
+
+  NumberOfPages = 0;
+  FreePageList = &mSmmMemoryMap;
+  for (Node = FreePageList->BackLink;
+       Node != FreePageList;
+       Node = Node->BackLink) {
+    Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+    NumberOfPages += Pages->NumberOfPages;
+  }
+
+  mSmramFreeMemory.TotalFreeMemoryPages = NumberOfPages;
+
+  if (NumberOfPages <= SMRAM_INFO_DUMP_PAGE_THRESHOLD) {
+    DumpSmramInfo ();
+  }
+}
+
+/**
+  Update SMRAM profile Allocate information.
+
+  @param CallerAddress  Address of caller who call Allocate.
+  @param Action         This Allocate action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+SmmCoreUpdateProfileAllocate (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  Size,
+  IN VOID                   *Buffer
+  )
+{
+  EFI_STATUS                        Status;
+  MEMORY_PROFILE_CONTEXT           *Context;
+  MEMORY_PROFILE_DRIVER_INFO       *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO        *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+  EFI_MEMORY_TYPE                   ProfileMemoryIndex;
+
+  AllocInfoData = NULL;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, CallerAddress);
+  ASSERT (DriverInfoData != NULL);
+
+  //
+  // Use SmmInternalAllocatePool() that will not update profile for this AllocatePool action.
+  //
+  Status = SmmInternalAllocatePool (
+             EfiRuntimeServicesData,
+             sizeof (*AllocInfoData),
+             (VOID **) &AllocInfoData
+             );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  AllocInfo = &AllocInfoData->AllocInfo;
+  AllocInfoData->Signature      = MEMORY_PROFILE_ALLOC_INFO_SIGNATURE;
+  AllocInfo->Header.Signature   = MEMORY_PROFILE_ALLOC_INFO_SIGNATURE;
+  AllocInfo->Header.Length      = sizeof (MEMORY_PROFILE_ALLOC_INFO);
+  AllocInfo->Header.Revision    = MEMORY_PROFILE_ALLOC_INFO_REVISION;
+  AllocInfo->CallerAddress      = CallerAddress;
+  AllocInfo->SequenceId         = ContextData->Context.SequenceCount;
+  AllocInfo->Action             = Action;
+  AllocInfo->MemoryType         = MemoryType;
+  AllocInfo->Buffer             = (PHYSICAL_ADDRESS) (UINTN) Buffer;
+  AllocInfo->Size               = Size;
+
+  InsertTailList (DriverInfoData->AllocInfoList, &AllocInfoData->Link);
+
+  ProfileMemoryIndex = GetProfileMemoryIndex (MemoryType);
+
+  DriverInfo = &DriverInfoData->DriverInfo;
+  DriverInfo->CurrentUsage += Size;
+  if (DriverInfo->PeakUsage < DriverInfo->CurrentUsage) {
+    DriverInfo->PeakUsage = DriverInfo->CurrentUsage;
+  }
+  DriverInfo->CurrentUsageByType[ProfileMemoryIndex] += Size;
+  if (DriverInfo->PeakUsageByType[ProfileMemoryIndex] < DriverInfo->CurrentUsageByType[ProfileMemoryIndex]) {
+    DriverInfo->PeakUsageByType[ProfileMemoryIndex] = DriverInfo->CurrentUsageByType[ProfileMemoryIndex];
+  }
+  DriverInfo->AllocRecordCount ++;
+
+  Context = &ContextData->Context;
+  Context->CurrentTotalUsage += Size;
+  if (Context->PeakTotalUsage < Context->CurrentTotalUsage) {
+    Context->PeakTotalUsage = Context->CurrentTotalUsage;
+  }
+  Context->CurrentTotalUsageByType[ProfileMemoryIndex] += Size;
+  if (Context->PeakTotalUsageByType[ProfileMemoryIndex] < Context->CurrentTotalUsageByType[ProfileMemoryIndex]) {
+    Context->PeakTotalUsageByType[ProfileMemoryIndex] = Context->CurrentTotalUsageByType[ProfileMemoryIndex];
+  }
+  Context->SequenceCount ++;
+
+  SmramProfileUpdateFreePages (ContextData);
+  return TRUE;
+}
+
+/**
+  Get memory profile alloc info from memory profile
+
+  @param DriverInfoData     Driver info
+  @param Action             This Free action
+  @param Size               Buffer size
+  @param Buffer             Buffer address
+
+  @return Pointer to memory profile alloc info.
+**/
+MEMORY_PROFILE_ALLOC_INFO_DATA *
+GetMemoryProfileAllocInfoFromAddress (
+  IN MEMORY_PROFILE_DRIVER_INFO_DATA    *DriverInfoData,
+  IN MEMORY_PROFILE_ACTION              Action,
+  IN UINTN                              Size,
+  IN VOID                               *Buffer
+  )
+{
+  LIST_ENTRY                        *AllocInfoList;
+  LIST_ENTRY                        *AllocLink;
+  MEMORY_PROFILE_ALLOC_INFO         *AllocInfo;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+
+  AllocInfoList = DriverInfoData->AllocInfoList;
+
+  for (AllocLink = AllocInfoList->ForwardLink;
+       AllocLink != AllocInfoList;
+       AllocLink = AllocLink->ForwardLink) {
+    AllocInfoData = CR (
+                      AllocLink,
+                      MEMORY_PROFILE_ALLOC_INFO_DATA,
+                      Link,
+                      MEMORY_PROFILE_ALLOC_INFO_SIGNATURE
+                      );
+    AllocInfo = &AllocInfoData->AllocInfo;
+    if (AllocInfo->Action != Action) {
+      continue;
+    }
+    switch (Action) {
+      case MemoryProfileActionAllocatePages:
+        if ((AllocInfo->Buffer <= (PHYSICAL_ADDRESS) (UINTN) Buffer) &&
+            ((AllocInfo->Buffer + AllocInfo->Size) >= ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size))) {
+          return AllocInfoData;
+        }
+        break;
+      case MemoryProfileActionAllocatePool:
+        if (AllocInfo->Buffer == (PHYSICAL_ADDRESS) (UINTN) Buffer) {
+          return AllocInfoData;
+        }
+        break;
+      default:
+        ASSERT (FALSE);
+        break;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Update SMRAM profile Free information.
+
+  @param CallerAddress  Address of caller who call Free.
+  @param Action         This Free action.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+SmmCoreUpdateProfileFree (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN UINTN                  Size,
+  IN VOID                   *Buffer
+  )
+{
+  MEMORY_PROFILE_CONTEXT           *Context;
+  MEMORY_PROFILE_DRIVER_INFO       *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO        *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA      *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *DriverInfoData;
+  LIST_ENTRY                       *DriverLink;
+  LIST_ENTRY                       *DriverInfoList;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *ThisDriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA   *AllocInfoData;
+  EFI_MEMORY_TYPE                  ProfileMemoryIndex;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  DriverInfoData = GetMemoryProfileDriverInfoFromAddress (ContextData, CallerAddress);
+  ASSERT (DriverInfoData != NULL);
+
+  switch (Action) {
+    case MemoryProfileActionFreePages:
+      AllocInfoData = GetMemoryProfileAllocInfoFromAddress (DriverInfoData, MemoryProfileActionAllocatePages, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePool:
+      AllocInfoData = GetMemoryProfileAllocInfoFromAddress (DriverInfoData, MemoryProfileActionAllocatePool, 0, Buffer);
+      break;
+    default:
+      ASSERT (FALSE);
+      AllocInfoData = NULL;
+      break;
+  }
+  if (AllocInfoData == NULL) {
+    //
+    // Legal case, because driver A might free memory allocated by driver B, by some protocol.
+    //
+    DriverInfoList = ContextData->DriverInfoList;
+
+    for (DriverLink = DriverInfoList->ForwardLink;
+         DriverLink != DriverInfoList;
+         DriverLink = DriverLink->ForwardLink) {
+      ThisDriverInfoData = CR (
+                             DriverLink,
+                             MEMORY_PROFILE_DRIVER_INFO_DATA,
+                             Link,
+                             MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                             );
+      switch (Action) {
+        case MemoryProfileActionFreePages:
+          AllocInfoData = GetMemoryProfileAllocInfoFromAddress (ThisDriverInfoData, MemoryProfileActionAllocatePages, Size, Buffer);
+          break;
+        case MemoryProfileActionFreePool:
+          AllocInfoData = GetMemoryProfileAllocInfoFromAddress (ThisDriverInfoData, MemoryProfileActionAllocatePool, 0, Buffer);
+          break;
+        default:
+          ASSERT (FALSE);
+          AllocInfoData = NULL;
+          break;
+      }
+      if (AllocInfoData != NULL) {
+        DriverInfoData = ThisDriverInfoData;
+        break;
+      }
+    }
+
+    if (AllocInfoData == NULL) {
+      //
+      // No matched allocate operation is found for this free operation.
+      // It is because the specified memory type allocate operation has been
+      // filtered by CoreNeedRecordProfile(), but free operations have no
+      // memory type information, they can not be filtered by CoreNeedRecordProfile().
+      // Then, they will be filtered here.
+      //
+      return FALSE;
+    }
+  }
+
+  Context = &ContextData->Context;
+  DriverInfo = &DriverInfoData->DriverInfo;
+  AllocInfo = &AllocInfoData->AllocInfo;
+
+  ProfileMemoryIndex = GetProfileMemoryIndex (AllocInfo->MemoryType);
+
+  Context->CurrentTotalUsage -= AllocInfo->Size;
+  Context->CurrentTotalUsageByType[ProfileMemoryIndex] -= AllocInfo->Size;
+
+  DriverInfo->CurrentUsage -= AllocInfo->Size;
+  DriverInfo->CurrentUsageByType[ProfileMemoryIndex] -= AllocInfo->Size;
+  DriverInfo->AllocRecordCount --;
+
+  RemoveEntryList (&AllocInfoData->Link);
+
+  if (Action == MemoryProfileActionFreePages) {
+    if (AllocInfo->Buffer != (PHYSICAL_ADDRESS) (UINTN) Buffer) {
+      SmmCoreUpdateProfileAllocate (
+        AllocInfo->CallerAddress,
+        MemoryProfileActionAllocatePages,
+        AllocInfo->MemoryType,
+        (UINTN) ((PHYSICAL_ADDRESS) (UINTN) Buffer - AllocInfo->Buffer),
+        (VOID *) (UINTN) AllocInfo->Buffer
+        );
+    }
+    if (AllocInfo->Buffer + AllocInfo->Size != ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size)) {
+      SmmCoreUpdateProfileAllocate (
+        AllocInfo->CallerAddress,
+        MemoryProfileActionAllocatePages,
+        AllocInfo->MemoryType,
+        (UINTN) ((AllocInfo->Buffer + AllocInfo->Size) - ((PHYSICAL_ADDRESS) (UINTN) Buffer + Size)),
+        (VOID *) ((UINTN) Buffer + Size)
+        );
+    }
+  }
+
+  //
+  // Use SmmInternalFreePool() that will not update profile for this FreePool action.
+  //
+  SmmInternalFreePool (AllocInfoData);
+
+  return TRUE;
+}
+
+/**
+  Update SMRAM profile information.
+
+  @param CallerAddress  Address of caller who call Allocate or Free.
+  @param Action         This Allocate or Free action.
+  @param MemoryType     Memory type.
+  @param Size           Buffer size.
+  @param Buffer         Buffer address.
+
+  @retval TRUE          Profile udpate success.
+  @retval FALSE         Profile update fail.
+
+**/
+BOOLEAN
+SmmCoreUpdateProfile (
+  IN PHYSICAL_ADDRESS       CallerAddress,
+  IN MEMORY_PROFILE_ACTION  Action,
+  IN EFI_MEMORY_TYPE        MemoryType, // Valid for AllocatePages/AllocatePool
+  IN UINTN                  Size,       // Valid for AllocatePages/FreePages/AllocatePool
+  IN VOID                   *Buffer
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return FALSE;
+  }
+
+  if (!mSmramProfileRecordingStatus) {
+    return FALSE;
+  }
+
+  //
+  // Free operations have no memory type information, so skip the check.
+  //
+  if ((Action == MemoryProfileActionAllocatePages) || (Action == MemoryProfileActionAllocatePool)) {
+    //
+    // Only record limited MemoryType.
+    //
+    if (!SmmCoreNeedRecordProfile (MemoryType)) {
+      return FALSE;
+    }
+  }
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return FALSE;
+  }
+
+  switch (Action) {
+    case MemoryProfileActionAllocatePages:
+      SmmCoreUpdateProfileAllocate (CallerAddress, Action, MemoryType, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePages:
+      SmmCoreUpdateProfileFree (CallerAddress, Action, Size, Buffer);
+      break;
+    case MemoryProfileActionAllocatePool:
+      SmmCoreUpdateProfileAllocate (CallerAddress, Action, MemoryType, Size, Buffer);
+      break;
+    case MemoryProfileActionFreePool:
+      SmmCoreUpdateProfileFree (CallerAddress, Action, 0, Buffer);
+      break;
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+
+  return TRUE;
+}
+
+/**
+  SMRAM profile ready to lock callback function.
+
+**/
+VOID
+SmramProfileReadyToLock (
+  VOID
+  )
+{
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "SmramProfileReadyToLock\n"));
+  mSmramReadyToLock = TRUE;
+}
+
+////////////////////
+
+/**
+  Get SMRAM profile data size.
+
+  @return SMRAM profile data size.
+
+**/
+UINTN
+SmramProfileGetDataSize (
+  VOID
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA      *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *DriverInfoData;
+  LIST_ENTRY                      *DriverInfoList;
+  LIST_ENTRY                      *DriverLink;
+  UINTN                           TotalSize;
+  LIST_ENTRY                      *Node;
+  LIST_ENTRY                      *FreePageList;
+  LIST_ENTRY                      *FreePoolList;
+  FREE_POOL_HEADER                *Pool;
+  UINTN                           PoolListIndex;
+  UINTN                           Index;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return 0;
+  }
+
+  TotalSize = sizeof (MEMORY_PROFILE_CONTEXT);
+  TotalSize += sizeof (MEMORY_PROFILE_DRIVER_INFO) * (UINTN) ContextData->Context.ImageCount;
+
+  DriverInfoList = ContextData->DriverInfoList;
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                   DriverLink,
+                   MEMORY_PROFILE_DRIVER_INFO_DATA,
+                   Link,
+                   MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                   );
+    TotalSize += sizeof (MEMORY_PROFILE_ALLOC_INFO) * (UINTN) DriverInfoData->DriverInfo.AllocRecordCount;
+  }
+
+
+  Index = 0;
+  FreePageList = &mSmmMemoryMap;
+  for (Node = FreePageList->BackLink;
+       Node != FreePageList;
+       Node = Node->BackLink) {
+    Index++;
+  }
+  for (PoolListIndex = 0; PoolListIndex < MAX_POOL_INDEX; PoolListIndex++) {
+    FreePoolList = &mSmmPoolLists[PoolListIndex];
+    for (Node = FreePoolList->BackLink;
+         Node != FreePoolList;
+         Node = Node->BackLink) {
+      Pool = BASE_CR (Node, FREE_POOL_HEADER, Link);
+      if (Pool->Header.Available) {
+        Index++;
+      }
+    }
+  }
+
+
+  TotalSize += (sizeof (MEMORY_PROFILE_FREE_MEMORY) + Index * sizeof (MEMORY_PROFILE_DESCRIPTOR));
+  TotalSize += (sizeof (MEMORY_PROFILE_MEMORY_RANGE) + mFullSmramRangeCount * sizeof (MEMORY_PROFILE_DESCRIPTOR));
+
+  return TotalSize;
+}
+
+/**
+  Copy SMRAM profile data.
+
+  @param ProfileBuffer  The buffer to hold SMRAM profile data.
+  @param ProfileSize    On input, profile buffer size.
+                        On output, actual profile data size copied.
+  @param ProfileOffset  On input, profile buffer offset to copy.
+                        On output, next time profile buffer offset to copy.
+
+**/
+VOID
+SmramProfileCopyData (
+  OUT VOID      *ProfileBuffer,
+  IN OUT UINT64 *ProfileSize,
+  IN OUT UINT64 *ProfileOffset
+  )
+{
+  MEMORY_PROFILE_CONTEXT           *Context;
+  MEMORY_PROFILE_DRIVER_INFO       *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO        *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA      *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA  *DriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA   *AllocInfoData;
+  LIST_ENTRY                      *DriverInfoList;
+  LIST_ENTRY                      *DriverLink;
+  LIST_ENTRY                      *AllocInfoList;
+  LIST_ENTRY                      *AllocLink;
+  LIST_ENTRY                      *Node;
+  FREE_PAGE_LIST                  *Pages;
+  LIST_ENTRY                      *FreePageList;
+  LIST_ENTRY                      *FreePoolList;
+  FREE_POOL_HEADER                *Pool;
+  UINTN                           PoolListIndex;
+  UINT32                          Index;
+  MEMORY_PROFILE_FREE_MEMORY      *FreeMemory;
+  MEMORY_PROFILE_MEMORY_RANGE     *MemoryRange;
+  MEMORY_PROFILE_DESCRIPTOR       *MemoryProfileDescriptor;
+  UINT64                          Offset;
+  UINT64                          RemainingSize;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  RemainingSize = *ProfileSize;
+  Offset = 0;
+
+  if (*ProfileOffset < sizeof (MEMORY_PROFILE_CONTEXT)) {
+    if (RemainingSize >= sizeof (MEMORY_PROFILE_CONTEXT)) {
+      Context = ProfileBuffer;
+      CopyMem (Context, &ContextData->Context, sizeof (MEMORY_PROFILE_CONTEXT));
+      RemainingSize -= sizeof (MEMORY_PROFILE_CONTEXT);
+      ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_CONTEXT);
+    } else {
+      goto Done;
+    }
+  }
+  Offset += sizeof (MEMORY_PROFILE_CONTEXT);
+
+  DriverInfoList = ContextData->DriverInfoList;
+  for (DriverLink = DriverInfoList->ForwardLink;
+       DriverLink != DriverInfoList;
+       DriverLink = DriverLink->ForwardLink) {
+    DriverInfoData = CR (
+                       DriverLink,
+                       MEMORY_PROFILE_DRIVER_INFO_DATA,
+                       Link,
+                       MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                       );
+    if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_DRIVER_INFO))) {
+      if (RemainingSize >= sizeof (MEMORY_PROFILE_DRIVER_INFO)) {
+        DriverInfo = ProfileBuffer;
+        CopyMem (DriverInfo, &DriverInfoData->DriverInfo, sizeof (MEMORY_PROFILE_DRIVER_INFO));
+        RemainingSize -= sizeof (MEMORY_PROFILE_DRIVER_INFO);
+        ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_DRIVER_INFO);
+      } else {
+        goto Done;
+      }
+    }
+    Offset += sizeof (MEMORY_PROFILE_DRIVER_INFO);
+
+    AllocInfoList = DriverInfoData->AllocInfoList;
+    for (AllocLink = AllocInfoList->ForwardLink;
+         AllocLink != AllocInfoList;
+         AllocLink = AllocLink->ForwardLink) {
+      AllocInfoData = CR (
+                        AllocLink,
+                        MEMORY_PROFILE_ALLOC_INFO_DATA,
+                        Link,
+                        MEMORY_PROFILE_ALLOC_INFO_SIGNATURE
+                        );
+      if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_ALLOC_INFO))) {
+        if (RemainingSize >= sizeof (MEMORY_PROFILE_ALLOC_INFO)) {
+          AllocInfo = ProfileBuffer;
+          CopyMem (AllocInfo, &AllocInfoData->AllocInfo, sizeof (MEMORY_PROFILE_ALLOC_INFO));
+          RemainingSize -= sizeof (MEMORY_PROFILE_ALLOC_INFO);
+          ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_ALLOC_INFO);
+        } else {
+          goto Done;
+        }
+      }
+      Offset += sizeof (MEMORY_PROFILE_ALLOC_INFO);
+    }
+  }
+
+
+  if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_FREE_MEMORY))) {
+    if (RemainingSize >= sizeof (MEMORY_PROFILE_FREE_MEMORY)) {
+      FreeMemory = ProfileBuffer;
+      CopyMem (FreeMemory, &mSmramFreeMemory, sizeof (MEMORY_PROFILE_FREE_MEMORY));
+      Index = 0;
+      FreePageList = &mSmmMemoryMap;
+      for (Node = FreePageList->BackLink;
+           Node != FreePageList;
+           Node = Node->BackLink) {
+        Index++;
+      }
+      for (PoolListIndex = 0; PoolListIndex < MAX_POOL_INDEX; PoolListIndex++) {
+        FreePoolList = &mSmmPoolLists[MAX_POOL_INDEX - PoolListIndex - 1];
+        for (Node = FreePoolList->BackLink;
+             Node != FreePoolList;
+             Node = Node->BackLink) {
+          Pool = BASE_CR (Node, FREE_POOL_HEADER, Link);
+          if (Pool->Header.Available) {
+            Index++;
+          }
+        }
+      }
+      FreeMemory->FreeMemoryEntryCount = Index;
+
+      RemainingSize -= sizeof (MEMORY_PROFILE_FREE_MEMORY);
+      ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_FREE_MEMORY);
+    } else {
+      goto Done;
+    }
+  }
+  Offset += sizeof (MEMORY_PROFILE_FREE_MEMORY);
+  FreePageList = &mSmmMemoryMap;
+  for (Node = FreePageList->BackLink;
+       Node != FreePageList;
+       Node = Node->BackLink) {
+    if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_DESCRIPTOR))) {
+      if (RemainingSize >= sizeof (MEMORY_PROFILE_DESCRIPTOR)) {
+        Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+        MemoryProfileDescriptor = ProfileBuffer;
+        MemoryProfileDescriptor->Header.Signature = MEMORY_PROFILE_DESCRIPTOR_SIGNATURE;
+        MemoryProfileDescriptor->Header.Length = sizeof (MEMORY_PROFILE_DESCRIPTOR);
+        MemoryProfileDescriptor->Header.Revision = MEMORY_PROFILE_DESCRIPTOR_REVISION;
+        MemoryProfileDescriptor->Address = (PHYSICAL_ADDRESS) (UINTN) Pages;
+        MemoryProfileDescriptor->Size = EFI_PAGES_TO_SIZE (Pages->NumberOfPages);
+
+        RemainingSize -= sizeof (MEMORY_PROFILE_DESCRIPTOR);
+        ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_DESCRIPTOR);
+      } else {
+        goto Done;
+      }
+    }
+    Offset += sizeof (MEMORY_PROFILE_DESCRIPTOR);
+  }
+  for (PoolListIndex = 0; PoolListIndex < MAX_POOL_INDEX; PoolListIndex++) {
+    FreePoolList = &mSmmPoolLists[MAX_POOL_INDEX - PoolListIndex - 1];
+    for (Node = FreePoolList->BackLink;
+         Node != FreePoolList;
+         Node = Node->BackLink) {
+      Pool = BASE_CR (Node, FREE_POOL_HEADER, Link);
+      if (Pool->Header.Available) {
+        if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_DESCRIPTOR))) {
+          if (RemainingSize >= sizeof (MEMORY_PROFILE_DESCRIPTOR)) {
+            MemoryProfileDescriptor = ProfileBuffer;
+            MemoryProfileDescriptor->Header.Signature = MEMORY_PROFILE_DESCRIPTOR_SIGNATURE;
+            MemoryProfileDescriptor->Header.Length = sizeof (MEMORY_PROFILE_DESCRIPTOR);
+            MemoryProfileDescriptor->Header.Revision = MEMORY_PROFILE_DESCRIPTOR_REVISION;
+            MemoryProfileDescriptor->Address = (PHYSICAL_ADDRESS) (UINTN) Pool;
+            MemoryProfileDescriptor->Size = Pool->Header.Size;
+
+            RemainingSize -= sizeof (MEMORY_PROFILE_DESCRIPTOR);
+            ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_DESCRIPTOR);
+          } else {
+            goto Done;
+          }
+        }
+        Offset += sizeof (MEMORY_PROFILE_DESCRIPTOR);
+      }
+    }
+  }
+
+  if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_MEMORY_RANGE))) {
+    if (RemainingSize >= sizeof (MEMORY_PROFILE_MEMORY_RANGE)) {
+      MemoryRange = ProfileBuffer;
+      MemoryRange->Header.Signature = MEMORY_PROFILE_MEMORY_RANGE_SIGNATURE;
+      MemoryRange->Header.Length = sizeof (MEMORY_PROFILE_MEMORY_RANGE);
+      MemoryRange->Header.Revision = MEMORY_PROFILE_MEMORY_RANGE_REVISION;
+      MemoryRange->MemoryRangeCount = (UINT32) mFullSmramRangeCount;
+
+      RemainingSize -= sizeof (MEMORY_PROFILE_MEMORY_RANGE);
+      ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_MEMORY_RANGE);
+    } else {
+      goto Done;
+    }
+  }
+  Offset += sizeof (MEMORY_PROFILE_MEMORY_RANGE);
+  for (Index = 0; Index < mFullSmramRangeCount; Index++) {
+    if (*ProfileOffset < (Offset + sizeof (MEMORY_PROFILE_DESCRIPTOR))) {
+      if (RemainingSize >= sizeof (MEMORY_PROFILE_DESCRIPTOR)) {
+        MemoryProfileDescriptor = ProfileBuffer;
+        MemoryProfileDescriptor->Header.Signature = MEMORY_PROFILE_DESCRIPTOR_SIGNATURE;
+        MemoryProfileDescriptor->Header.Length = sizeof (MEMORY_PROFILE_DESCRIPTOR);
+        MemoryProfileDescriptor->Header.Revision = MEMORY_PROFILE_DESCRIPTOR_REVISION;
+        MemoryProfileDescriptor->Address = mFullSmramRanges[Index].PhysicalStart;
+        MemoryProfileDescriptor->Size = mFullSmramRanges[Index].PhysicalSize;
+
+        RemainingSize -= sizeof (MEMORY_PROFILE_DESCRIPTOR);
+        ProfileBuffer = (UINT8 *) ProfileBuffer + sizeof (MEMORY_PROFILE_DESCRIPTOR);
+      } else {
+        goto Done;
+      }
+    }
+    Offset += sizeof (MEMORY_PROFILE_DESCRIPTOR);
+  }
+
+Done:
+  //
+  // On output, actual profile data size copied.
+  //
+  *ProfileSize -= RemainingSize;
+  //
+  // On output, next time profile buffer offset to copy.
+  //
+  *ProfileOffset = Offset;
+}
+
+/**
+  SMRAM profile handler to get profile info.
+
+  @param SmramProfileParameterGetInfo The parameter of SMM profile get size.
+
+**/
+VOID
+SmramProfileHandlerGetInfo (
+  IN SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO   *SmramProfileParameterGetInfo
+  )
+{
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+  BOOLEAN                       SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+  SmramProfileParameterGetInfo->ProfileSize = SmramProfileGetDataSize();
+  SmramProfileParameterGetInfo->Header.ReturnStatus = 0;
+
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  SMRAM profile handler to get profile data.
+
+  @param SmramProfileParameterGetData The parameter of SMM profile get data.
+
+**/
+VOID
+SmramProfileHandlerGetData (
+  IN SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA   *SmramProfileParameterGetData
+  )
+{
+  UINT64                                    ProfileSize;
+  UINT64                                    ProfileOffset;
+  SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA  SmramProfileGetData;
+  MEMORY_PROFILE_CONTEXT_DATA               *ContextData;
+  BOOLEAN                                   SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+
+  CopyMem (&SmramProfileGetData, SmramProfileParameterGetData, sizeof (SmramProfileGetData));
+
+  ProfileSize = SmramProfileGetDataSize();
+
+  //
+  // Sanity check
+  //
+  if (!SmmIsBufferOutsideSmmValid ((UINTN) SmramProfileGetData.ProfileBuffer, (UINTN) ProfileSize)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerGetData: SMM ProfileBuffer in SMRAM or overflow!\n"));
+    SmramProfileParameterGetData->ProfileSize = ProfileSize;
+    SmramProfileParameterGetData->Header.ReturnStatus = (UINT64) (INT64) (INTN) EFI_ACCESS_DENIED;
+    goto Done;
+  }
+
+  if (SmramProfileGetData.ProfileSize < ProfileSize) {
+    SmramProfileParameterGetData->ProfileSize = ProfileSize;
+    SmramProfileParameterGetData->Header.ReturnStatus = (UINT64) (INT64) (INTN) EFI_BUFFER_TOO_SMALL;
+    goto Done;
+  }
+
+  ProfileOffset = 0;
+  SmramProfileCopyData ((VOID *) (UINTN) SmramProfileGetData.ProfileBuffer, &ProfileSize, &ProfileOffset);
+  SmramProfileParameterGetData->ProfileSize = ProfileSize;
+  SmramProfileParameterGetData->Header.ReturnStatus = 0;
+
+Done:
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  SMRAM profile handler to get profile data by offset.
+
+  @param SmramProfileParameterGetDataByOffset   The parameter of SMM profile get data by offset.
+
+**/
+VOID
+SmramProfileHandlerGetDataByOffset (
+  IN SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET     *SmramProfileParameterGetDataByOffset
+  )
+{
+  SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET    SmramProfileGetDataByOffset;
+  MEMORY_PROFILE_CONTEXT_DATA                           *ContextData;
+  BOOLEAN                                               SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+
+  CopyMem (&SmramProfileGetDataByOffset, SmramProfileParameterGetDataByOffset, sizeof (SmramProfileGetDataByOffset));
+
+  //
+  // Sanity check
+  //
+  if (!SmmIsBufferOutsideSmmValid ((UINTN) SmramProfileGetDataByOffset.ProfileBuffer, (UINTN) SmramProfileGetDataByOffset.ProfileSize)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerGetDataByOffset: SMM ProfileBuffer in SMRAM or overflow!\n"));
+    SmramProfileParameterGetDataByOffset->Header.ReturnStatus = (UINT64) (INT64) (INTN) EFI_ACCESS_DENIED;
+    goto Done;
+  }
+
+  SmramProfileCopyData ((VOID *) (UINTN) SmramProfileGetDataByOffset.ProfileBuffer, &SmramProfileGetDataByOffset.ProfileSize, &SmramProfileGetDataByOffset.ProfileOffset);
+  CopyMem (SmramProfileParameterGetDataByOffset, &SmramProfileGetDataByOffset, sizeof (SmramProfileGetDataByOffset));
+  SmramProfileParameterGetDataByOffset->Header.ReturnStatus = 0;
+
+Done:
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  SMRAM profile handler to register SMM image.
+
+  @param SmramProfileParameterRegisterImage The parameter of SMM profile register image.
+
+**/
+VOID
+SmramProfileHandlerRegisterImage (
+  IN SMRAM_PROFILE_PARAMETER_REGISTER_IMAGE *SmramProfileParameterRegisterImage
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SMM_DRIVER_ENTRY              DriverEntry;
+  VOID                              *EntryPointInImage;
+  BOOLEAN                           Ret;
+
+  ZeroMem (&DriverEntry, sizeof (DriverEntry));
+  CopyMem (&DriverEntry.FileName, &SmramProfileParameterRegisterImage->FileName, sizeof(EFI_GUID));
+  DriverEntry.ImageBuffer = SmramProfileParameterRegisterImage->ImageBuffer;
+  DriverEntry.NumberOfPage = (UINTN) SmramProfileParameterRegisterImage->NumberOfPage;
+  Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) DriverEntry.ImageBuffer, &EntryPointInImage);
+  ASSERT_EFI_ERROR (Status);
+  DriverEntry.ImageEntryPoint = (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+
+  Ret = RegisterSmramProfileImage (&DriverEntry, FALSE);
+  if (Ret) {
+    SmramProfileParameterRegisterImage->Header.ReturnStatus = 0;
+  }
+}
+
+/**
+  SMRAM profile handler to unregister SMM image.
+
+  @param SmramProfileParameterUnregisterImage The parameter of SMM profile unregister image.
+
+**/
+VOID
+SmramProfileHandlerUnregisterImage (
+  IN SMRAM_PROFILE_PARAMETER_UNREGISTER_IMAGE *SmramProfileParameterUnregisterImage
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SMM_DRIVER_ENTRY              DriverEntry;
+  VOID                              *EntryPointInImage;
+  BOOLEAN                           Ret;
+
+  ZeroMem (&DriverEntry, sizeof (DriverEntry));
+  CopyMem (&DriverEntry.FileName, &SmramProfileParameterUnregisterImage->FileName, sizeof (EFI_GUID));
+  DriverEntry.ImageBuffer = SmramProfileParameterUnregisterImage->ImageBuffer;
+  DriverEntry.NumberOfPage = (UINTN) SmramProfileParameterUnregisterImage->NumberOfPage;
+  Status = InternalPeCoffGetEntryPoint ((VOID *) (UINTN) DriverEntry.ImageBuffer, &EntryPointInImage);
+  ASSERT_EFI_ERROR (Status);
+  DriverEntry.ImageEntryPoint = (PHYSICAL_ADDRESS) (UINTN) EntryPointInImage;
+
+  Ret = UnregisterSmramProfileImage (&DriverEntry, FALSE);
+  if (Ret) {
+    SmramProfileParameterUnregisterImage->Header.ReturnStatus = 0;
+  }
+}
+
+/**
+  Dispatch function for a Software SMI handler.
+
+  Caution: This function may receive untrusted input.
+  Communicate buffer and buffer size are external input, so this function will do basic validation.
+
+  @param DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param Context         Points to an optional handler context which was specified when the
+                         handler was registered.
+  @param CommBuffer      A pointer to a collection of data in memory that will
+                         be conveyed from a non-SMM environment into an SMM environment.
+  @param CommBufferSize  The size of the CommBuffer.
+
+  @retval EFI_SUCCESS Command is handled successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SmramProfileHandler (
+  IN EFI_HANDLE  DispatchHandle,
+  IN CONST VOID  *Context         OPTIONAL,
+  IN OUT VOID    *CommBuffer      OPTIONAL,
+  IN OUT UINTN   *CommBufferSize  OPTIONAL
+  )
+{
+  SMRAM_PROFILE_PARAMETER_HEADER           *SmramProfileParameterHeader;
+  UINTN                                    TempCommBufferSize;
+
+  DEBUG ((EFI_D_ERROR, "SmramProfileHandler Enter\n"));
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if (TempCommBufferSize < sizeof (SMRAM_PROFILE_PARAMETER_HEADER)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (mSmramReadyToLock && !SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  SmramProfileParameterHeader = (SMRAM_PROFILE_PARAMETER_HEADER *) ((UINTN) CommBuffer);
+
+  SmramProfileParameterHeader->ReturnStatus = (UINT64)-1;
+
+  if (GetSmramProfileContext () == NULL) {
+    SmramProfileParameterHeader->ReturnStatus = (UINT64) (INT64) (INTN) EFI_UNSUPPORTED;
+    return EFI_SUCCESS;
+  }
+
+  switch (SmramProfileParameterHeader->Command) {
+  case SMRAM_PROFILE_COMMAND_GET_PROFILE_INFO:
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerGetInfo\n"));
+    if (TempCommBufferSize != sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO)) {
+      DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+      return EFI_SUCCESS;
+    }
+    SmramProfileHandlerGetInfo ((SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO *) (UINTN) CommBuffer);
+    break;
+  case SMRAM_PROFILE_COMMAND_GET_PROFILE_DATA:
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerGetData\n"));
+    if (TempCommBufferSize != sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA)) {
+      DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+      return EFI_SUCCESS;
+    }
+    SmramProfileHandlerGetData ((SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA *) (UINTN) CommBuffer);
+    break;
+  case SMRAM_PROFILE_COMMAND_GET_PROFILE_DATA_BY_OFFSET:
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerGetDataByOffset\n"));
+    if (TempCommBufferSize != sizeof (SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET)) {
+      DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+      return EFI_SUCCESS;
+    }
+    SmramProfileHandlerGetDataByOffset ((SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET *) (UINTN) CommBuffer);
+    break;
+  case SMRAM_PROFILE_COMMAND_REGISTER_IMAGE:
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerRegisterImage\n"));
+    if (TempCommBufferSize != sizeof (SMRAM_PROFILE_PARAMETER_REGISTER_IMAGE)) {
+      DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+      return EFI_SUCCESS;
+    }
+    if (mSmramReadyToLock) {
+      return EFI_SUCCESS;
+    }
+    SmramProfileHandlerRegisterImage ((SMRAM_PROFILE_PARAMETER_REGISTER_IMAGE *) (UINTN) CommBuffer);
+    break;
+  case SMRAM_PROFILE_COMMAND_UNREGISTER_IMAGE:
+    DEBUG ((EFI_D_ERROR, "SmramProfileHandlerUnregisterImage\n"));
+    if (TempCommBufferSize != sizeof (SMRAM_PROFILE_PARAMETER_UNREGISTER_IMAGE)) {
+      DEBUG ((EFI_D_ERROR, "SmramProfileHandler: SMM communication buffer size invalid!\n"));
+      return EFI_SUCCESS;
+    }
+    if (mSmramReadyToLock) {
+      return EFI_SUCCESS;
+    }
+    SmramProfileHandlerUnregisterImage ((SMRAM_PROFILE_PARAMETER_UNREGISTER_IMAGE *) (UINTN) CommBuffer);
+    break;
+  default:
+    break;
+  }
+
+  DEBUG ((EFI_D_ERROR, "SmramProfileHandler Exit\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register SMRAM profile handler.
+
+**/
+VOID
+RegisterSmramProfileHandler (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+  EFI_HANDLE    DispatchHandle;
+
+  if (!IS_SMRAM_PROFILE_ENABLED) {
+    return;
+  }
+
+  Status = SmiHandlerRegister (
+             SmramProfileHandler,
+             &gEdkiiMemoryProfileGuid,
+             &DispatchHandle
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+////////////////////
+
+/**
+  Dump SMRAM range.
+
+**/
+VOID
+DumpSmramRange (
+  VOID
+  )
+{
+  UINTN                         Index;
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+  BOOLEAN                       SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+  DEBUG ((EFI_D_INFO, "FullSmramRange address - 0x%08x\n", mFullSmramRanges));
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile begin =======\n"));
+
+  DEBUG ((EFI_D_INFO, "FullSmramRange:\n"));
+  for (Index = 0; Index < mFullSmramRangeCount; Index++) {
+    DEBUG ((EFI_D_INFO, "  FullSmramRange (0x%x)\n", Index));
+    DEBUG ((EFI_D_INFO, "    PhysicalStart      - 0x%016lx\n", mFullSmramRanges[Index].PhysicalStart));
+    DEBUG ((EFI_D_INFO, "    CpuStart           - 0x%016lx\n", mFullSmramRanges[Index].CpuStart));
+    DEBUG ((EFI_D_INFO, "    PhysicalSize       - 0x%016lx\n", mFullSmramRanges[Index].PhysicalSize));
+    DEBUG ((EFI_D_INFO, "    RegionState        - 0x%016lx\n", mFullSmramRanges[Index].RegionState));
+  }
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile end =======\n"));
+
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  Dump SMRAM free page list.
+
+**/
+VOID
+DumpFreePagesList (
+  VOID
+  )
+{
+  LIST_ENTRY                    *FreePageList;
+  LIST_ENTRY                    *Node;
+  FREE_PAGE_LIST                *Pages;
+  UINTN                         Index;
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+  BOOLEAN                       SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile begin =======\n"));
+
+  DEBUG ((EFI_D_INFO, "FreePagesList:\n"));
+  FreePageList = &mSmmMemoryMap;
+  for (Node = FreePageList->BackLink, Index = 0;
+       Node != FreePageList;
+       Node = Node->BackLink, Index++) {
+    Pages = BASE_CR (Node, FREE_PAGE_LIST, Link);
+    DEBUG ((EFI_D_INFO, "  Index - 0x%x\n", Index));
+    DEBUG ((EFI_D_INFO, "    PhysicalStart - 0x%016lx\n", (PHYSICAL_ADDRESS) (UINTN) Pages));
+    DEBUG ((EFI_D_INFO, "    NumberOfPages - 0x%08x\n", Pages->NumberOfPages));
+  }
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile end =======\n"));
+
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  Dump SMRAM free pool list.
+
+**/
+VOID
+DumpFreePoolList (
+  VOID
+  )
+{
+  LIST_ENTRY                     *FreePoolList;
+  LIST_ENTRY                     *Node;
+  FREE_POOL_HEADER               *Pool;
+  UINTN                          Index;
+  UINTN                          PoolListIndex;
+  MEMORY_PROFILE_CONTEXT_DATA   *ContextData;
+  BOOLEAN                       SmramProfileRecordingStatus;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile begin =======\n"));
+
+  for (PoolListIndex = 0; PoolListIndex < MAX_POOL_INDEX; PoolListIndex++) {
+    DEBUG ((EFI_D_INFO, "FreePoolList (%d):\n", PoolListIndex));
+    FreePoolList = &mSmmPoolLists[PoolListIndex];
+    for (Node = FreePoolList->BackLink, Index = 0;
+         Node != FreePoolList;
+         Node = Node->BackLink, Index++) {
+      Pool = BASE_CR (Node, FREE_POOL_HEADER, Link);
+      DEBUG ((EFI_D_INFO, "  Index - 0x%x\n", Index));
+      DEBUG ((EFI_D_INFO, "    PhysicalStart - 0x%016lx\n", (PHYSICAL_ADDRESS) (UINTN) Pool));
+      DEBUG ((EFI_D_INFO, "    Size          - 0x%08x\n", Pool->Header.Size));
+      DEBUG ((EFI_D_INFO, "    Available     - 0x%02x\n", Pool->Header.Available));
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile end =======\n"));
+
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR16 *mActionString[] = {
+  L"Unknown",
+  L"AllocatePages",
+  L"FreePages",
+  L"AllocatePool",
+  L"FreePool",
+};
+
+typedef struct {
+  EFI_MEMORY_TYPE   MemoryType;
+  CHAR16            *MemoryTypeStr;
+} PROFILE_MEMORY_TYPE_STRING;
+
+GLOBAL_REMOVE_IF_UNREFERENCED PROFILE_MEMORY_TYPE_STRING mMemoryTypeString[] = {
+  {EfiRuntimeServicesCode, L"EfiRuntimeServicesCode"},
+  {EfiRuntimeServicesData, L"EfiRuntimeServicesData"}
+};
+
+/**
+  Memory type to string.
+
+  @param[in] MemoryType Memory type.
+
+  @return Pointer to string.
+
+**/
+CHAR16 *
+ProfileMemoryTypeToStr (
+  IN EFI_MEMORY_TYPE    MemoryType
+  )
+{
+  UINTN     Index;
+  for (Index = 0; Index < sizeof (mMemoryTypeString) / sizeof (mMemoryTypeString[0]); Index++) {
+    if (mMemoryTypeString[Index].MemoryType == MemoryType) {
+      return mMemoryTypeString[Index].MemoryTypeStr;
+    }
+  }
+
+  return L"UnexpectedMemoryType";
+}
+
+/**
+  Dump SMRAM profile.
+
+**/
+VOID
+DumpSmramProfile (
+  VOID
+  )
+{
+  MEMORY_PROFILE_CONTEXT            *Context;
+  MEMORY_PROFILE_DRIVER_INFO        *DriverInfo;
+  MEMORY_PROFILE_ALLOC_INFO         *AllocInfo;
+  MEMORY_PROFILE_CONTEXT_DATA       *ContextData;
+  MEMORY_PROFILE_DRIVER_INFO_DATA   *DriverInfoData;
+  MEMORY_PROFILE_ALLOC_INFO_DATA    *AllocInfoData;
+  LIST_ENTRY                        *SmramDriverInfoList;
+  UINTN                             DriverIndex;
+  LIST_ENTRY                        *DriverLink;
+  LIST_ENTRY                        *AllocInfoList;
+  UINTN                             AllocIndex;
+  LIST_ENTRY                        *AllocLink;
+  BOOLEAN                           SmramProfileRecordingStatus;
+  UINTN                             TypeIndex;
+
+  ContextData = GetSmramProfileContext ();
+  if (ContextData == NULL) {
+    return ;
+  }
+
+  SmramProfileRecordingStatus = mSmramProfileRecordingStatus;
+  mSmramProfileRecordingStatus = FALSE;
+
+  Context = &ContextData->Context;
+  DEBUG ((EFI_D_INFO, "======= SmramProfile begin =======\n"));
+  DEBUG ((EFI_D_INFO, "MEMORY_PROFILE_CONTEXT\n"));
+
+  DEBUG ((EFI_D_INFO, "  CurrentTotalUsage     - 0x%016lx\n", Context->CurrentTotalUsage));
+  DEBUG ((EFI_D_INFO, "  PeakTotalUsage        - 0x%016lx\n", Context->PeakTotalUsage));
+  for (TypeIndex = 0; TypeIndex < sizeof (Context->CurrentTotalUsageByType) / sizeof (Context->CurrentTotalUsageByType[0]); TypeIndex++) {
+    if ((Context->CurrentTotalUsageByType[TypeIndex] != 0) ||
+        (Context->PeakTotalUsageByType[TypeIndex] != 0)) {
+      DEBUG ((EFI_D_INFO, "  CurrentTotalUsage[0x%02x]  - 0x%016lx (%s)\n", TypeIndex, Context->CurrentTotalUsageByType[TypeIndex], ProfileMemoryTypeToStr (TypeIndex)));
+      DEBUG ((EFI_D_INFO, "  PeakTotalUsage[0x%02x]     - 0x%016lx (%s)\n", TypeIndex, Context->PeakTotalUsageByType[TypeIndex], ProfileMemoryTypeToStr (TypeIndex)));
+    }
+  }
+  DEBUG ((EFI_D_INFO, "  TotalImageSize        - 0x%016lx\n", Context->TotalImageSize));
+  DEBUG ((EFI_D_INFO, "  ImageCount            - 0x%08x\n", Context->ImageCount));
+  DEBUG ((EFI_D_INFO, "  SequenceCount         - 0x%08x\n", Context->SequenceCount));
+
+  SmramDriverInfoList = ContextData->DriverInfoList;
+  for (DriverLink = SmramDriverInfoList->ForwardLink, DriverIndex = 0;
+       DriverLink != SmramDriverInfoList;
+       DriverLink = DriverLink->ForwardLink, DriverIndex++) {
+    DriverInfoData = CR (
+                   DriverLink,
+                   MEMORY_PROFILE_DRIVER_INFO_DATA,
+                   Link,
+                   MEMORY_PROFILE_DRIVER_INFO_SIGNATURE
+                   );
+    DriverInfo = &DriverInfoData->DriverInfo;
+    DEBUG ((EFI_D_INFO, "  MEMORY_PROFILE_DRIVER_INFO (0x%x)\n", DriverIndex));
+    DEBUG ((EFI_D_INFO, "    FileName            - %g\n", &DriverInfo->FileName));
+    DEBUG ((EFI_D_INFO, "    ImageBase           - 0x%016lx\n", DriverInfo->ImageBase));
+    DEBUG ((EFI_D_INFO, "    ImageSize           - 0x%016lx\n", DriverInfo->ImageSize));
+    DEBUG ((EFI_D_INFO, "    EntryPoint          - 0x%016lx\n", DriverInfo->EntryPoint));
+    DEBUG ((EFI_D_INFO, "    ImageSubsystem      - 0x%04x\n", DriverInfo->ImageSubsystem));
+    DEBUG ((EFI_D_INFO, "    FileType            - 0x%02x\n", DriverInfo->FileType));
+    DEBUG ((EFI_D_INFO, "    CurrentUsage        - 0x%016lx\n", DriverInfo->CurrentUsage));
+    DEBUG ((EFI_D_INFO, "    PeakUsage           - 0x%016lx\n", DriverInfo->PeakUsage));
+    for (TypeIndex = 0; TypeIndex < sizeof (DriverInfo->CurrentUsageByType) / sizeof (DriverInfo->CurrentUsageByType[0]); TypeIndex++) {
+      if ((DriverInfo->CurrentUsageByType[TypeIndex] != 0) ||
+          (DriverInfo->PeakUsageByType[TypeIndex] != 0)) {
+        DEBUG ((EFI_D_INFO, "    CurrentUsage[0x%02x]     - 0x%016lx (%s)\n", TypeIndex, DriverInfo->CurrentUsageByType[TypeIndex], ProfileMemoryTypeToStr (TypeIndex)));
+        DEBUG ((EFI_D_INFO, "    PeakUsage[0x%02x]        - 0x%016lx (%s)\n", TypeIndex, DriverInfo->PeakUsageByType[TypeIndex], ProfileMemoryTypeToStr (TypeIndex)));
+      }
+    }
+    DEBUG ((EFI_D_INFO, "    AllocRecordCount    - 0x%08x\n", DriverInfo->AllocRecordCount));
+
+    AllocInfoList = DriverInfoData->AllocInfoList;
+    for (AllocLink = AllocInfoList->ForwardLink, AllocIndex = 0;
+         AllocLink != AllocInfoList;
+         AllocLink = AllocLink->ForwardLink, AllocIndex++) {
+      AllocInfoData = CR (
+                     AllocLink,
+                     MEMORY_PROFILE_ALLOC_INFO_DATA,
+                     Link,
+                     MEMORY_PROFILE_ALLOC_INFO_SIGNATURE
+                     );
+      AllocInfo = &AllocInfoData->AllocInfo;
+      DEBUG ((EFI_D_INFO, "    MEMORY_PROFILE_ALLOC_INFO (0x%x)\n", AllocIndex));
+      DEBUG ((EFI_D_INFO, "      CallerAddress  - 0x%016lx (Offset: 0x%08x)\n", AllocInfo->CallerAddress, AllocInfo->CallerAddress - DriverInfo->ImageBase));
+      DEBUG ((EFI_D_INFO, "      SequenceId     - 0x%08x\n", AllocInfo->SequenceId));
+      DEBUG ((EFI_D_INFO, "      Action         - 0x%08x (%s)\n", AllocInfo->Action, mActionString[(AllocInfo->Action < sizeof(mActionString)/sizeof(mActionString[0])) ? AllocInfo->Action : 0]));
+      DEBUG ((EFI_D_INFO, "      MemoryType     - 0x%08x\n", AllocInfo->MemoryType));
+      DEBUG ((EFI_D_INFO, "      Buffer         - 0x%016lx\n", AllocInfo->Buffer));
+      DEBUG ((EFI_D_INFO, "      Size           - 0x%016lx\n", AllocInfo->Size));
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "======= SmramProfile end =======\n"));
+
+  mSmramProfileRecordingStatus = SmramProfileRecordingStatus;
+}
+
+/**
+  Dump SMRAM infromation.
+
+**/
+VOID
+DumpSmramInfo (
+  VOID
+  )
+{
+  DEBUG_CODE (
+    if (IS_SMRAM_PROFILE_ENABLED) {
+      DumpSmramProfile ();
+      DumpFreePagesList ();
+      DumpFreePoolList ();
+      DumpSmramRange ();
+    }
+  );
+}
+
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/Crc32.c b/Core/MdeModulePkg/Core/RuntimeDxe/Crc32.c
new file mode 100644
index 0000000000..a6fe77fa34
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/Crc32.c
@@ -0,0 +1,115 @@
+/** @file
+  This file implements CalculateCrc32 Boot Services as defined in
+  Platform Initialization specification 1.0 VOLUME 2 DXE Core Interface.
+
+  This Boot Services is in the Runtime Driver because this service is
+  also required by SetVirtualAddressMap() when the EFI System Table and
+  EFI Runtime Services Table are converted from physical address to
+  virtual addresses.  This requires that the 32-bit CRC be recomputed.
+
+Copyright (c) 2006 - 2008, 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 <Uefi.h>
+
+UINT32  mCrcTable[256];
+
+/**
+  Calculate CRC32 for target data.
+
+  @param  Data                  The target data.
+  @param  DataSize              The target data size.
+  @param  CrcOut                The CRC32 for target data.
+
+  @retval EFI_SUCCESS           The CRC32 for target data is calculated successfully.
+  @retval EFI_INVALID_PARAMETER Some parameter is not valid, so the CRC32 is not
+                                calculated.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverCalculateCrc32 (
+  IN  VOID    *Data,
+  IN  UINTN   DataSize,
+  OUT UINT32  *CrcOut
+  )
+{
+  UINT32  Crc;
+  UINTN   Index;
+  UINT8   *Ptr;
+
+  if (Data == NULL || DataSize == 0 || CrcOut == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Crc = 0xffffffff;
+  for (Index = 0, Ptr = Data; Index < DataSize; Index++, Ptr++) {
+    Crc = (Crc >> 8) ^ mCrcTable[(UINT8) Crc ^ *Ptr];
+  }
+
+  *CrcOut = Crc ^ 0xffffffff;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This internal function reverses bits for 32bit data.
+
+  @param  Value                 The data to be reversed.
+
+  @return                       Data reversed.
+
+**/
+UINT32
+ReverseBits (
+  UINT32  Value
+  )
+{
+  UINTN   Index;
+  UINT32  NewValue;
+
+  NewValue = 0;
+  for (Index = 0; Index < 32; Index++) {
+    if ((Value & (1 << Index)) != 0) {
+      NewValue = NewValue | (1 << (31 - Index));
+    }
+  }
+
+  return NewValue;
+}
+
+/**
+  Initialize CRC32 table.
+
+**/
+VOID
+RuntimeDriverInitializeCrc32Table (
+  VOID
+  )
+{
+  UINTN   TableEntry;
+  UINTN   Index;
+  UINT32  Value;
+
+  for (TableEntry = 0; TableEntry < 256; TableEntry++) {
+    Value = ReverseBits ((UINT32) TableEntry);
+    for (Index = 0; Index < 8; Index++) {
+      if ((Value & 0x80000000) != 0) {
+        Value = (Value << 1) ^ 0x04c11db7;
+      } else {
+        Value = Value << 1;
+      }
+    }
+
+    mCrcTable[TableEntry] = ReverseBits (Value);
+  }
+}
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.c b/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.c
new file mode 100644
index 0000000000..c61301cf80
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.c
@@ -0,0 +1,427 @@
+/** @file
+  This file implements Runtime Architectural Protocol as defined in the
+  Platform Initialization specification 1.0 VOLUME 2 DXE Core Interface.
+
+  This code is used to produce the EFI runtime virtual switch over
+
+  THIS IS VERY DANGEROUS CODE BE VERY CAREFUL IF YOU CHANGE IT
+
+  The transition for calling EFI Runtime functions in physical mode to calling
+  them in virtual mode is very very complex. Every pointer in needs to be
+  converted from physical mode to virtual mode. Be very careful walking linked
+  lists! Then to make it really hard the code it's self needs be relocated into
+  the new virtual address space.
+
+  So here is the concept. The code in this module will never ever be called in
+  virtual mode. This is the code that collects the information needed to convert
+  to virtual mode (DXE core registers runtime stuff with this code). Since this
+  code is used to fix up all runtime images, it CAN NOT fix it's self up. So some
+  code has to stay behind and that is us.
+
+  Also you need to be careful about when you allocate memory, as once we are in
+  runtime (including our EVT_SIGNAL_EXIT_BOOT_SERVICES event) you can no longer
+  allocate memory.
+
+  Any runtime driver that gets loaded before us will not be callable in virtual
+  mode. This is due to the fact that the DXE core can not register the info
+  needed with us. This is good, since it keeps the code in this file from
+  getting registered.
+
+
+Revision History:
+
+  - Move the CalculateCrc32 function from Runtime Arch Protocol to Boot Service.
+  Runtime Arch Protocol definition no longer contains CalculateCrc32. Boot Service
+  Table now contains an item named CalculateCrc32.
+
+
+Copyright (c) 2006 - 2015, 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 "Runtime.h"
+
+//
+// Global Variables
+//
+EFI_MEMORY_DESCRIPTOR         *mVirtualMap                = NULL;
+UINTN                         mVirtualMapDescriptorSize;
+UINTN                         mVirtualMapMaxIndex;
+VOID                          *mMyImageBase;
+
+//
+// The handle onto which the Runtime Architectural Protocol instance is installed
+//
+EFI_HANDLE                    mRuntimeHandle = NULL;
+
+//
+// The Runtime Architectural Protocol instance produced by this driver
+//
+EFI_RUNTIME_ARCH_PROTOCOL     mRuntime = {
+  INITIALIZE_LIST_HEAD_VARIABLE (mRuntime.ImageHead),
+  INITIALIZE_LIST_HEAD_VARIABLE (mRuntime.EventHead),
+
+  //
+  // Make sure Size != sizeof (EFI_MEMORY_DESCRIPTOR). This will
+  // prevent people from having pointer math bugs in their code.
+  // now you have to use *DescriptorSize to make things work.
+  //
+  sizeof (EFI_MEMORY_DESCRIPTOR) + sizeof (UINT64) - (sizeof (EFI_MEMORY_DESCRIPTOR) % sizeof (UINT64)),
+  EFI_MEMORY_DESCRIPTOR_VERSION,
+  0,
+  NULL,
+  NULL,
+  FALSE,
+  FALSE
+};
+
+//
+// Worker Functions
+//
+/**
+
+  Calculate the 32-bit CRC in a EFI table using the Runtime Drivers
+  internal function.  The EFI Boot Services Table can not be used because
+  the EFI Boot Services Table was destroyed at ExitBootServices().
+  This is a internal function.
+
+
+  @param Hdr             Pointer to an EFI standard header
+
+**/
+VOID
+RuntimeDriverCalculateEfiHdrCrc (
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  )
+{
+  UINT32  Crc;
+
+  Hdr->CRC32  = 0;
+
+  Crc         = 0;
+  RuntimeDriverCalculateCrc32 ((UINT8 *) Hdr, Hdr->HeaderSize, &Crc);
+  Hdr->CRC32 = Crc;
+}
+
+/**
+
+  Determines the new virtual address that is to be used on subsequent memory accesses.
+
+
+  @param DebugDisposition Supplies type information for the pointer being converted.
+  @param ConvertAddress  A pointer to a pointer that is to be fixed to be the value needed
+                         for the new virtual address mappings being applied.
+
+  @retval  EFI_SUCCESS              The pointer pointed to by Address was modified.
+  @retval  EFI_NOT_FOUND            The pointer pointed to by Address was not found to be part
+                                    of the current memory map. This is normally fatal.
+  @retval  EFI_INVALID_PARAMETER    One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverConvertPointer (
+  IN     UINTN  DebugDisposition,
+  IN OUT VOID   **ConvertAddress
+  )
+{
+  UINTN                 Address;
+  UINT64                VirtEndOfRange;
+  EFI_MEMORY_DESCRIPTOR *VirtEntry;
+  UINTN                 Index;
+
+  //
+  // Make sure ConvertAddress is a valid pointer
+  //
+  if (ConvertAddress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Get the address to convert
+  //
+  Address = (UINTN) *ConvertAddress;
+
+  //
+  // If this is a null pointer, return if it's allowed
+  //
+  if (Address == 0) {
+    if ((DebugDisposition & EFI_OPTIONAL_PTR) != 0) {
+      return EFI_SUCCESS;
+    }
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VirtEntry = mVirtualMap;
+  for (Index = 0; Index < mVirtualMapMaxIndex; Index++) {
+    //
+    //  To prevent the inclusion of 64-bit math functions a UINTN was placed in
+    //  front of VirtEntry->NumberOfPages to cast it to a 32-bit thing on IA-32
+    //  platforms. If you get this ASSERT remove the UINTN and do a 64-bit
+    //  multiply.
+    //
+    ASSERT (((UINTN) VirtEntry->NumberOfPages < 0xffffffff) || (sizeof (UINTN) > 4));
+
+    if ((VirtEntry->Attribute & EFI_MEMORY_RUNTIME) == EFI_MEMORY_RUNTIME) {
+      if (Address >= VirtEntry->PhysicalStart) {
+        VirtEndOfRange = VirtEntry->PhysicalStart + (((UINTN) VirtEntry->NumberOfPages) * EFI_PAGE_SIZE);
+        if (Address < VirtEndOfRange) {
+          //
+          // Compute new address
+          //
+          *ConvertAddress = (VOID *) (Address - (UINTN) VirtEntry->PhysicalStart + (UINTN) VirtEntry->VirtualStart);
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    VirtEntry = NEXT_MEMORY_DESCRIPTOR (VirtEntry, mVirtualMapDescriptorSize);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+
+  Determines the new virtual address that is to be used on subsequent memory accesses
+  for internal pointers.
+  This is a internal function.
+
+
+  @param ConvertAddress  A pointer to a pointer that is to be fixed to be the value needed
+                         for the new virtual address mappings being applied.
+
+  @retval  EFI_SUCCESS              The pointer pointed to by Address was modified.
+  @retval  EFI_NOT_FOUND            The pointer pointed to by Address was not found to be part
+                                    of the current memory map. This is normally fatal.
+  @retval  EFI_INVALID_PARAMETER    One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+RuntimeDriverConvertInternalPointer (
+  IN OUT VOID   **ConvertAddress
+  )
+{
+  return RuntimeDriverConvertPointer (0x0, ConvertAddress);
+}
+
+/**
+
+  Changes the runtime addressing mode of EFI firmware from physical to virtual.
+
+
+  @param MemoryMapSize   The size in bytes of VirtualMap.
+  @param DescriptorSize  The size in bytes of an entry in the VirtualMap.
+  @param DescriptorVersion The version of the structure entries in VirtualMap.
+  @param VirtualMap      An array of memory descriptors which contain new virtual
+                         address mapping information for all runtime ranges.
+
+  @retval  EFI_SUCCESS            The virtual address map has been applied.
+  @retval  EFI_UNSUPPORTED        EFI firmware is not at runtime, or the EFI firmware is already in
+                                  virtual address mapped mode.
+  @retval  EFI_INVALID_PARAMETER  DescriptorSize or DescriptorVersion is invalid.
+  @retval  EFI_NO_MAPPING         A virtual address was not supplied for a range in the memory
+                                  map that requires a mapping.
+  @retval  EFI_NOT_FOUND          A virtual address was supplied for an address that is not found
+                                  in the memory map.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverSetVirtualAddressMap (
+  IN UINTN                  MemoryMapSize,
+  IN UINTN                  DescriptorSize,
+  IN UINT32                 DescriptorVersion,
+  IN EFI_MEMORY_DESCRIPTOR  *VirtualMap
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_RUNTIME_EVENT_ENTRY       *RuntimeEvent;
+  EFI_RUNTIME_IMAGE_ENTRY       *RuntimeImage;
+  LIST_ENTRY                    *Link;
+  EFI_PHYSICAL_ADDRESS          VirtImageBase;
+
+  //
+  // Can only switch to virtual addresses once the memory map is locked down,
+  // and can only set it once
+  //
+  if (!mRuntime.AtRuntime || mRuntime.VirtualMode) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Only understand the original descriptor format
+  //
+  if (DescriptorVersion != EFI_MEMORY_DESCRIPTOR_VERSION || DescriptorSize < sizeof (EFI_MEMORY_DESCRIPTOR)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // We are now committed to go to virtual mode, so lets get to it!
+  //
+  mRuntime.VirtualMode = TRUE;
+
+  //
+  // ConvertPointer() needs this mVirtualMap to do the conversion. So set up
+  // globals we need to parse the virtual address map.
+  //
+  mVirtualMapDescriptorSize = DescriptorSize;
+  mVirtualMapMaxIndex       = MemoryMapSize / DescriptorSize;
+  mVirtualMap               = VirtualMap;
+
+  //
+  // ReporstStatusCodeLib will check and make sure this service can be called in runtime mode.
+  //
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_EFI_RUNTIME_SERVICE | EFI_SW_RS_PC_SET_VIRTUAL_ADDRESS_MAP));
+
+  //
+  // Report Status Code here since EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event will be signalled.
+  //
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_PC_VIRTUAL_ADDRESS_CHANGE_EVENT));
+
+  //
+  // Signal all the EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE events.
+  // All runtime events are stored in a list in Runtime AP.
+  //
+  for (Link = mRuntime.EventHead.ForwardLink; Link != &mRuntime.EventHead; Link = Link->ForwardLink) {
+    RuntimeEvent = BASE_CR (Link, EFI_RUNTIME_EVENT_ENTRY, Link);
+    if ((RuntimeEvent->Type & EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE) == EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE) {
+      RuntimeEvent->NotifyFunction (
+                      RuntimeEvent->Event,
+                      RuntimeEvent->NotifyContext
+                      );
+    }
+  }
+
+  //
+  // Relocate runtime images. All runtime images are stored in a list in Runtime AP.
+  //
+  for (Link = mRuntime.ImageHead.ForwardLink; Link != &mRuntime.ImageHead; Link = Link->ForwardLink) {
+    RuntimeImage = BASE_CR (Link, EFI_RUNTIME_IMAGE_ENTRY, Link);
+    //
+    // We don't want to relocate our selves, as we only run in physical mode.
+    //
+    if (mMyImageBase != RuntimeImage->ImageBase) {
+
+      VirtImageBase = (EFI_PHYSICAL_ADDRESS) (UINTN) RuntimeImage->ImageBase;
+      Status  = RuntimeDriverConvertPointer (0, (VOID **) &VirtImageBase);
+      ASSERT_EFI_ERROR (Status);
+
+      PeCoffLoaderRelocateImageForRuntime (
+        (EFI_PHYSICAL_ADDRESS) (UINTN) RuntimeImage->ImageBase,
+        VirtImageBase,
+        (UINTN) RuntimeImage->ImageSize,
+        RuntimeImage->RelocationData
+        );
+
+      InvalidateInstructionCacheRange (RuntimeImage->ImageBase, (UINTN) RuntimeImage->ImageSize);
+    }
+  }
+
+  //
+  // Convert all the Runtime Services except ConvertPointer() and SetVirtualAddressMap()
+  // and recompute the CRC-32
+  //
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->GetTime);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->SetTime);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->GetWakeupTime);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->SetWakeupTime);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->ResetSystem);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->GetNextHighMonotonicCount);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->GetVariable);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->SetVariable);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->GetNextVariableName);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->QueryVariableInfo);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->UpdateCapsule);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gRT->QueryCapsuleCapabilities);
+  RuntimeDriverCalculateEfiHdrCrc (&gRT->Hdr);
+
+  //
+  // UEFI don't require System Configuration Tables Conversion.
+  //
+
+  //
+  // Convert the runtime fields of the EFI System Table and recompute the CRC-32
+  //
+  RuntimeDriverConvertInternalPointer ((VOID **) &gST->FirmwareVendor);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gST->ConfigurationTable);
+  RuntimeDriverConvertInternalPointer ((VOID **) &gST->RuntimeServices);
+  RuntimeDriverCalculateEfiHdrCrc (&gST->Hdr);
+
+  //
+  // At this point, gRT and gST are physical pointers, but the contents of these tables
+  // have been converted to runtime.
+  //
+  //
+  // mVirtualMap is only valid during SetVirtualAddressMap() call
+  //
+  mVirtualMap = NULL;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Entry Point for Runtime driver.
+
+  This function installs Runtime Architectural Protocol and registers CalculateCrc32 boot services table,
+  SetVirtualAddressMap & ConvertPointer runtime services table.
+
+  @param ImageHandle     Image handle of this driver.
+  @param SystemTable     a Pointer to the EFI System Table.
+
+  @retval  EFI_SUCEESS  Runtime Driver Architectural Protocol is successfully installed
+  @return  Others       Some error occurs when installing Runtime Driver Architectural Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverInitialize (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+  EFI_LOADED_IMAGE_PROTOCOL *MyLoadedImage;
+
+  //
+  // This image needs to be excluded from relocation for virtual mode, so cache
+  // a copy of the Loaded Image protocol to test later.
+  //
+  Status = gBS->HandleProtocol (
+                  ImageHandle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID**)&MyLoadedImage
+                  );
+  ASSERT_EFI_ERROR (Status);
+  mMyImageBase = MyLoadedImage->ImageBase;
+
+  //
+  // Initialize the table used to compute 32-bit CRCs
+  //
+  RuntimeDriverInitializeCrc32Table ();
+
+  //
+  // Fill in the entries of the EFI Boot Services and EFI Runtime Services Tables
+  //
+  gBS->CalculateCrc32       = RuntimeDriverCalculateCrc32;
+  gRT->SetVirtualAddressMap = RuntimeDriverSetVirtualAddressMap;
+  gRT->ConvertPointer       = RuntimeDriverConvertPointer;
+
+  //
+  // Install the Runtime Architectural Protocol onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mRuntimeHandle,
+                  &gEfiRuntimeArchProtocolGuid,
+                  &mRuntime,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.h b/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.h
new file mode 100644
index 0000000000..f2cee9c7c6
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/Runtime.h
@@ -0,0 +1,134 @@
+/** @file
+  Runtime Architectural Protocol as defined in the DXE CIS.
+
+  This code is used to produce the EFI runtime architectural protocol.
+
+Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef _RUNTIME_H_
+#define _RUNTIME_H_
+
+#include <PiDxe.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/Runtime.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/PeCoffLib.h>
+
+
+//
+// Function Prototypes
+//
+/**
+  Calculate CRC32 for target data.
+
+  @param Data            The target data.
+  @param DataSize        The target data size.
+  @param CrcOut          The CRC32 for target data.
+
+  @retval  EFI_SUCCESS            The CRC32 for target data is calculated successfully.
+  @retval  EFI_INVALID_PARAMETER  Some parameter is not valid, so the CRC32 is not
+                                  calculated.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverCalculateCrc32 (
+  IN  VOID    *Data,
+  IN  UINTN   DataSize,
+  OUT UINT32  *CrcOut
+  );
+
+/**
+  Determines the new virtual address that is to be used on subsequent memory accesses.
+
+
+  @param DebugDisposition Supplies type information for the pointer being converted.
+  @param ConvertAddress  A pointer to a pointer that is to be fixed to be the value needed
+                         for the new virtual address mappings being applied.
+
+  @retval  EFI_SUCCESS              The pointer pointed to by Address was modified.
+  @retval  EFI_NOT_FOUND            The pointer pointed to by Address was not found to be part
+                                    of the current memory map. This is normally fatal.
+  @retval  EFI_INVALID_PARAMETER    One of the parameters has an invalid value.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverConvertPointer (
+  IN     UINTN  DebugDisposition,
+  IN OUT VOID   **ConvertAddress
+  );
+
+/**
+  Changes the runtime addressing mode of EFI firmware from physical to virtual.
+
+  @param  MemoryMapSize   The size in bytes of VirtualMap.
+  @param  DescriptorSize  The size in bytes of an entry in the VirtualMap.
+  @param  DescriptorVersion The version of the structure entries in VirtualMap.
+  @param  VirtualMap      An array of memory descriptors which contain new virtual
+                         address mapping information for all runtime ranges.
+
+  @retval  EFI_SUCCESS            The virtual address map has been applied.
+  @retval  EFI_UNSUPPORTED        EFI firmware is not at runtime, or the EFI firmware is already in
+                                  virtual address mapped mode.
+  @retval  EFI_INVALID_PARAMETER  DescriptorSize or DescriptorVersion is invalid.
+  @retval  EFI_NO_MAPPING         A virtual address was not supplied for a range in the memory
+                                  map that requires a mapping.
+  @retval  EFI_NOT_FOUND          A virtual address was supplied for an address that is not found
+                                  in the memory map.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverSetVirtualAddressMap (
+  IN UINTN                  MemoryMapSize,
+  IN UINTN                  DescriptorSize,
+  IN UINT32                 DescriptorVersion,
+  IN EFI_MEMORY_DESCRIPTOR  *VirtualMap
+  );
+
+/**
+  Initialize CRC32 table.
+
+**/
+VOID
+RuntimeDriverInitializeCrc32Table (
+  VOID
+  );
+
+/**
+  Install Runtime AP. This code includes the EfiRuntimeLib, but it only
+  functions at RT in physical mode.
+
+  @param  ImageHandle     Image handle of this driver.
+  @param  SystemTable     Pointer to the EFI System Table.
+
+  @retval EFI_SUCEESS  Runtime Driver Architectural Protocol Installed
+  @return Other value if gBS->InstallMultipleProtocolInterfaces fails. Check
+           gBS->InstallMultipleProtocolInterfaces for details.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeDriverInitialize (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.inf b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.inf
new file mode 100644
index 0000000000..035aa9a596
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.inf
@@ -0,0 +1,65 @@
+## @file
+#  Module that produces EFI runtime virtual switch over services.
+#
+#  This runtime module installs Runtime Architectural Protocol and registers
+#  CalculateCrc32 boot services table, SetVirtualAddressMap & ConvertPointer
+#  runtime services table.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = RuntimeDxe
+  MODULE_UNI_FILE                = RuntimeDxe.uni
+  FILE_GUID                      = B601F8C4-43B7-4784-95B1-F4226CB40CEE
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = RuntimeDriverInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Crc32.c
+  Runtime.h
+  Runtime.c
+ 
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  PeCoffLib
+  CacheMaintenanceLib
+  UefiBootServicesTableLib
+  UefiLib
+  UefiRuntimeServicesTableLib
+  ReportStatusCodeLib
+  DebugLib
+  UefiDriverEntryPoint
+  BaseLib
+
+[Protocols]
+  gEfiRuntimeArchProtocolGuid                   ## PRODUCES
+  gEfiLoadedImageProtocolGuid                   ## CONSUMES
+ 
+[depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  RuntimeDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.uni b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.uni
new file mode 100644
index 0000000000..c0fe02724c
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxeExtra.uni b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxeExtra.uni
new file mode 100644
index 0000000000..4049b83022
--- /dev/null
+++ b/Core/MdeModulePkg/Core/RuntimeDxe/RuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Include/Guid/AcpiS3Context.h b/Core/MdeModulePkg/Include/Guid/AcpiS3Context.h
new file mode 100644
index 0000000000..de0f87cfb5
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/AcpiS3Context.h
@@ -0,0 +1,73 @@
+/** @file
+  Definitions for data structures used in S3 resume.
+
+Copyright (c) 2011 - 2012, 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.
+
+**/
+
+#ifndef _ACPI_S3_DATA_H_
+#define _ACPI_S3_DATA_H_
+
+#include <Library/BaseLib.h>
+
+#define SMM_S3_RESUME_SMM_32 SIGNATURE_64 ('S','M','M','S','3','_','3','2')
+#define SMM_S3_RESUME_SMM_64 SIGNATURE_64 ('S','M','M','S','3','_','6','4')
+
+#pragma pack(1)
+
+typedef struct {
+  UINT64                Signature;
+  EFI_PHYSICAL_ADDRESS  SmmS3ResumeEntryPoint;
+  EFI_PHYSICAL_ADDRESS  SmmS3StackBase;
+  UINT64                SmmS3StackSize;
+  UINT64                SmmS3Cr0;
+  UINT64                SmmS3Cr3;
+  UINT64                SmmS3Cr4;
+  UINT16                ReturnCs;
+  EFI_PHYSICAL_ADDRESS  ReturnEntryPoint;
+  EFI_PHYSICAL_ADDRESS  ReturnContext1;
+  EFI_PHYSICAL_ADDRESS  ReturnContext2;
+  EFI_PHYSICAL_ADDRESS  ReturnStackPointer;
+  EFI_PHYSICAL_ADDRESS  Smst;
+} SMM_S3_RESUME_STATE;
+
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  AcpiFacsTable;
+  EFI_PHYSICAL_ADDRESS  IdtrProfile;
+  EFI_PHYSICAL_ADDRESS  S3NvsPageTableAddress;
+  EFI_PHYSICAL_ADDRESS  BootScriptStackBase;
+  UINT64                BootScriptStackSize;
+  EFI_PHYSICAL_ADDRESS  S3DebugBufferAddress;  
+} ACPI_S3_CONTEXT;
+
+typedef struct {
+  UINT16                ReturnCs;
+  UINT64                ReturnStatus;
+  EFI_PHYSICAL_ADDRESS  ReturnEntryPoint;
+  EFI_PHYSICAL_ADDRESS  ReturnStackPointer;
+  EFI_PHYSICAL_ADDRESS  AsmTransferControl;
+  IA32_DESCRIPTOR       Idtr;
+} PEI_S3_RESUME_STATE;
+
+#pragma pack()
+
+#define EFI_ACPI_S3_CONTEXT_GUID \
+  { \
+    0xef98d3a, 0x3e33, 0x497a, {0xa4, 0x1, 0x77, 0xbe, 0x3e, 0xb7, 0x4f, 0x38} \
+  }
+
+extern EFI_GUID gEfiAcpiS3ContextGuid;
+
+extern EFI_GUID gEfiAcpiVariableGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/BootScriptExecutorVariable.h b/Core/MdeModulePkg/Include/Guid/BootScriptExecutorVariable.h
new file mode 100644
index 0000000000..420ccea255
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/BootScriptExecutorVariable.h
@@ -0,0 +1,49 @@
+/** @file
+  Define Name, GUID and data format for an EFI Variable that is used to save the entry point
+  of a code segment which will be loaded and executed by a standalone boot script 
+  executor on S3 boot path.
+
+  Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef _BOOT_SCRIPT_EXECUTOR_VARIABLE_H_
+#define _BOOT_SCRIPT_EXECUTOR_VARIABLE_H_
+
+#define EFI_BOOT_SCRIPT_EXECUTOR_VARIABLE_GUID \
+  { \
+    0x3079818c, 0x46d4, 0x4a73, {0xae, 0xf3, 0xe3, 0xe4, 0x6c, 0xf1, 0xee, 0xdb} \
+  }
+
+//
+// The following structure boosts performance by combining structure all ACPI related variables into one.
+//
+#pragma pack(1)
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  BootScriptExecutorEntrypoint;
+} BOOT_SCRIPT_EXECUTOR_VARIABLE;
+
+#pragma pack()
+
+#define BOOT_SCRIPT_EXECUTOR_VARIABLE_NAME  L"BootScriptExecutorVariable"
+
+extern EFI_GUID gEfiBootScriptExecutorVariableGuid;
+
+#define EFI_BOOT_SCRIPT_EXECUTOR_CONTEXT_GUID \
+  { \
+    0x79cb58c4, 0xac51, 0x442f, {0xaf, 0xd7, 0x98, 0xe4, 0x7d, 0x2e, 0x99, 0x8} \
+  }
+
+extern EFI_GUID gEfiBootScriptExecutorContextGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/CapsuleVendor.h b/Core/MdeModulePkg/Include/Guid/CapsuleVendor.h
new file mode 100644
index 0000000000..15ca1055b6
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/CapsuleVendor.h
@@ -0,0 +1,65 @@
+/** @file
+  This file defines:
+  * the capsule vendor GUID for capsule variables and the HOB.
+  * the capsule variable name.
+  * the capsule GUID HOB data structure.
+  The capsule HOB and variable can be used to store the capsule image start address and length.
+  They are used by EDKII implementation of capsule update across a system reset.
+  
+  @par Note: EDKII implementation of capsule updating has discarded this capsule GUID HOB data
+             structure and used one UEFI Capsule HOB (defined in PI Specification 1.2) instead.
+
+Copyright (c) 2006 - 2011, 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 that 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.
+
+**/
+
+#ifndef __EFI_CAPSULE_VENDOR_GUID_H__
+#define __EFI_CAPSULE_VENDOR_GUID_H__
+
+///
+/// This guid is used as a variable GUID for the capsule variable 
+/// if the capsule pointer is passed through reset via a variable.
+///
+/// This guid is also used as a hob GUID for the capsule data 
+/// when the capsule pointer is passed from PEI phase to DXE phase.
+///
+#define EFI_CAPSULE_VENDOR_GUID  \
+  { 0x711C703F, 0xC285, 0x4B10, { 0xA3, 0xB0, 0x36, 0xEC, 0xBD, 0x3C, 0x8B, 0xE2 } }
+
+///
+/// Name of capsule variable.
+/// 
+#define EFI_CAPSULE_VARIABLE_NAME L"CapsuleUpdateData"
+
+///
+/// The data structure of the capsule guid hob entry.
+/// Note: EDKII implementation has discarded this structure and used 
+///       UEFI_CAPSULE_HOB instead.
+///
+typedef struct {
+  EFI_PHYSICAL_ADDRESS BaseAddress;  ///< Capsule data start address.
+  UINT32 Length;                     ///< Length of capsule data.
+} CAPSULE_HOB_INFO;
+
+//
+// The variable describes the long mode buffer used by IA32 Capsule PEIM
+// to call X64 CapsuleCoalesce code to handle >4GB capsule blocks.
+//
+#define EFI_CAPSULE_LONG_MODE_BUFFER_NAME L"CapsuleLongModeBuffer"
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS   PageTableAddress;
+  EFI_PHYSICAL_ADDRESS   StackBaseAddress;
+  UINT64                 StackSize;
+} EFI_CAPSULE_LONG_MODE_BUFFER;
+
+extern EFI_GUID gEfiCapsuleVendorGuid;
+
+#endif // #ifndef _EFI_CAPSULE_VENDOR_GUID_H_
diff --git a/Core/MdeModulePkg/Include/Guid/ConnectConInEvent.h b/Core/MdeModulePkg/Include/Guid/ConnectConInEvent.h
new file mode 100644
index 0000000000..28dd4b93de
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/ConnectConInEvent.h
@@ -0,0 +1,24 @@
+/** @file
+  GUID for an event that is signaled on the first attempt to check for a keystroke 
+  from the ConIn device.
+
+  Copyright (c) 2012, 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.
+
+**/
+
+#ifndef __CONNECT_CONIN_EVENT_GUID_H__
+#define __CONNECT_CONIN_EVENT_GUID_H__
+
+#define CONNECT_CONIN_EVENT_GUID \
+    { 0xdb4e8151, 0x57ed, 0x4bed, { 0x88, 0x33, 0x67, 0x51, 0xb5, 0xd1, 0xa8, 0xd7 }}
+
+extern EFI_GUID gConnectConInEventGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/ConsoleInDevice.h b/Core/MdeModulePkg/Include/Guid/ConsoleInDevice.h
new file mode 100644
index 0000000000..014594f188
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/ConsoleInDevice.h
@@ -0,0 +1,24 @@
+/** @file
+  This GUID can be installed to the device handle to specify that the device is the console-in device.
+  
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __CONSOLE_IN_DEVICE_H__
+#define __CONSOLE_IN_DEVICE_H__
+
+#define EFI_CONSOLE_IN_DEVICE_GUID    \
+    { 0xd3b36f2b, 0xd551, 0x11d4, {0x9a, 0x46, 0x0, 0x90, 0x27, 0x3f, 0xc1, 0x4d } }
+
+extern EFI_GUID gEfiConsoleInDeviceGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/ConsoleOutDevice.h b/Core/MdeModulePkg/Include/Guid/ConsoleOutDevice.h
new file mode 100644
index 0000000000..4d7de3d83f
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/ConsoleOutDevice.h
@@ -0,0 +1,23 @@
+/** @file
+  This GUID can be installed to the device handle to specify that the device is the console-out device.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __CONSOLE_OUT_DEVICE_H__
+#define __CONSOLE_OUT_DEVICE_H__
+
+#define EFI_CONSOLE_OUT_DEVICE_GUID    \
+    { 0xd3b36f2c, 0xd551, 0x11d4, {0x9a, 0x46, 0x0, 0x90, 0x27, 0x3f, 0xc1, 0x4d } }
+
+extern EFI_GUID gEfiConsoleOutDeviceGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/Crc32GuidedSectionExtraction.h b/Core/MdeModulePkg/Include/Guid/Crc32GuidedSectionExtraction.h
new file mode 100644
index 0000000000..896f0767db
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/Crc32GuidedSectionExtraction.h
@@ -0,0 +1,24 @@
+/** @file
+  This file defines CRC32 GUID to specify the CRC32 
+  encapsulation scheme for the GUIDed section.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __CRC32_GUIDED_SECTION_EXTRACTION_H__
+#define __CRC32_GUIDED_SECTION_EXTRACTION_H__
+
+#define EFI_CRC32_GUIDED_SECTION_EXTRACTION_GUID \
+  { 0xFC1BCDB0, 0x7D31, 0x49aa, {0x93, 0x6A, 0xA4, 0x60, 0x0D, 0x9D, 0xD0, 0x83 } }
+
+extern EFI_GUID gEfiCrc32GuidedSectionExtractionGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/DebugMask.h b/Core/MdeModulePkg/Include/Guid/DebugMask.h
new file mode 100644
index 0000000000..1e6beb562e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/DebugMask.h
@@ -0,0 +1,74 @@
+/** @file
+
+  Debug Mask Protocol.
+
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __DEBUG_MASK_H__
+#define __DEBUG_MASK_H__
+
+///
+/// Protocol GUID for DXE Phase Debug Mask support
+///
+#define EFI_DEBUG_MASK_PROTOCOL_GUID \
+  { 0x4c8a2451, 0xc207, 0x405b, {0x96, 0x94, 0x99, 0xea, 0x13, 0x25, 0x13, 0x41} }
+
+///
+/// Forward reference for pure ANSI compatability
+///
+typedef struct _EFI_DEBUG_MASK_PROTOCOL  EFI_DEBUG_MASK_PROTOCOL;
+
+///
+///
+///  
+#define EFI_DEBUG_MASK_REVISION        0x00010000
+
+//
+// DebugMask member functions definition
+//
+typedef
+EFI_STATUS
+(EFIAPI * EFI_GET_DEBUG_MASK) (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,             
+  IN OUT UINTN                *CurrentDebugMask  
+  );
+
+typedef 
+EFI_STATUS
+(EFIAPI *EFI_SET_DEBUG_MASK) (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,
+  IN UINTN                    NewDebugMask
+  );
+
+///
+/// DebugMask protocol definition
+///
+struct _EFI_DEBUG_MASK_PROTOCOL {
+  INT64               Revision;
+  EFI_GET_DEBUG_MASK  GetDebugMask;
+  EFI_SET_DEBUG_MASK  SetDebugMask;
+};
+
+extern EFI_GUID gEfiDebugMaskProtocolGuid;
+
+///
+/// GUID used to store the global debug mask in an the "EFIDebug" EFI Variabe
+/// Also used as a GUIDed HOB that contains a UINT32 debug mask default value
+///
+#define EFI_GENERIC_VARIABLE_GUID \
+  { 0x59d1c24f, 0x50f1, 0x401a, {0xb1, 0x01, 0xf3, 0x3e, 0x0d, 0xae, 0xd4, 0x43} }
+  
+#define DEBUG_MASK_VARIABLE_NAME  L"EFIDebug"
+
+extern EFI_GUID gEfiGenericVariableGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/DriverSampleHii.h b/Core/MdeModulePkg/Include/Guid/DriverSampleHii.h
new file mode 100644
index 0000000000..b5e1dbc099
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/DriverSampleHii.h
@@ -0,0 +1,37 @@
+/** @file
+  GUIDs used as HII FormSet and HII Package list GUID in Driver Sample driver. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __DRIVER_SAMPLE_HII_GUID_H__
+#define __DRIVER_SAMPLE_HII_GUID_H__
+
+#define DRIVER_SAMPLE_FORMSET_GUID \
+  { \
+    0xA04A27f4, 0xDF00, 0x4D42, {0xB5, 0x52, 0x39, 0x51, 0x13, 0x02, 0x11, 0x3D} \
+  }
+
+#define DRIVER_SAMPLE_INVENTORY_GUID \
+  { \
+    0xb3f56470, 0x6141, 0x4621, {0x8f, 0x19, 0x70, 0x4e, 0x57, 0x7a, 0xa9, 0xe8} \
+  }
+
+#define EFI_IFR_REFRESH_ID_OP_GUID \
+  { \
+    0xF5E655D9, 0x02A6, 0x46f2, {0x9E, 0x76, 0xB8, 0xBE, 0x8E, 0x60, 0xAB, 0x22} \
+  }
+
+extern EFI_GUID gDriverSampleFormSetGuid;
+extern EFI_GUID gDriverSampleInventoryGuid;
+extern EFI_GUID gEfiIfrRefreshIdOpGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/EventExitBootServiceFailed.h b/Core/MdeModulePkg/Include/Guid/EventExitBootServiceFailed.h
new file mode 100644
index 0000000000..aa34d459ba
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/EventExitBootServiceFailed.h
@@ -0,0 +1,24 @@
+/** @file
+  GUID is the name of events used with ExitBootServices in order to be notified
+  when this ExitBootServices Call is failed.
+
+  Copyright (c) 2012, 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.
+
+**/
+
+#ifndef __EVENT_EXIT_BOOT_FAILED_GUID_H__
+#define __EVENT_EXIT_BOOT_FAILED_GUID_H__
+                                             
+#define EVENT_GROUP_EXIT_BOOT_SERVICES_FAILED \
+  { 0x4f6c5507, 0x232f, 0x4787, { 0xb9, 0x5e, 0x72, 0xf8, 0x62, 0x49, 0xc, 0xb1 } }
+
+extern EFI_GUID gEventExitBootServicesFailedGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/FaultTolerantWrite.h b/Core/MdeModulePkg/Include/Guid/FaultTolerantWrite.h
new file mode 100644
index 0000000000..cb7b454b2b
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/FaultTolerantWrite.h
@@ -0,0 +1,54 @@
+/** @file
+ Define the GUID gEdkiiFaultTolerantWriteGuid that will be used to build
+ FAULT_TOLERANT_WRITE_LAST_WRITE_DATA GUID hob and install PPI to inform the check
+ for FTW last write data has been done. The GUID hob will be only built if FTW last write was
+ still in progress with SpareComplete set and DestinationComplete not set.
+
+Copyright (c) 2013, 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 that 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. 
+
+**/
+
+#ifndef _FAULT_TOLERANT_WRITE_H_
+#define _FAULT_TOLERANT_WRITE_H_
+
+#define EDKII_FAULT_TOLERANT_WRITE_GUID \
+  { \
+    0x1d3e9cb8, 0x43af, 0x490b, { 0x83,  0xa, 0x35, 0x16, 0xaa, 0x53, 0x20, 0x47 } \
+  }
+
+//
+// FTW Last write data. It will be used as gEdkiiFaultTolerantWriteGuid GUID hob data.
+//
+typedef struct {
+  ///
+  /// Target address to be updated in FTW last write.
+  ///
+  EFI_PHYSICAL_ADDRESS      TargetAddress;
+  ///
+  /// Spare address to back up the updated buffer.
+  ///
+  EFI_PHYSICAL_ADDRESS      SpareAddress;
+  ///
+  /// The length of data that have been backed up in spare block.
+  /// It is also the length of target block that has been erased.
+  ///
+  UINT64                    Length;
+} FAULT_TOLERANT_WRITE_LAST_WRITE_DATA;
+
+//
+// This GUID will be used to install PPI to inform the check for FTW last write data has been done.
+// The related FAULT_TOLERANT_WRITE_LAST_WRITE_DATA GUID hob will be only built if
+// FTW last write was still in progress with SpareComplete set and DestinationComplete not set.
+// It means the target buffer has been backed up in spare block, then target block has been erased,
+// but the target buffer has not been writen in target block from spare block.
+//
+extern EFI_GUID gEdkiiFaultTolerantWriteGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/FirmwarePerformance.h b/Core/MdeModulePkg/Include/Guid/FirmwarePerformance.h
new file mode 100644
index 0000000000..4697a2c41e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/FirmwarePerformance.h
@@ -0,0 +1,134 @@
+/** @file
+  ACPI Firmware Performance Data Table (FPDT) implementation specific definitions.
+
+  Copyright (c) 2011 - 2016, 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.
+
+**/
+
+#ifndef _FIRMWARE_PERFORMANCE_GUID_H_
+#define _FIRMWARE_PERFORMANCE_GUID_H_
+
+#include <PiPei.h>
+#include <IndustryStandard/Acpi.h>
+#include <Ppi/SecPerformance.h>
+
+///
+/// This GUID is used for FPDT implementation specific EFI Variable, LockBox and Hob.
+///
+/// EFI Variable:
+///   GUID - gEfiFirmwarePerformanceGuid
+///   Name - EFI_FIRMWARE_PERFORMANCE_VARIABLE_NAME
+///   Data - FIRMWARE_PERFORMANCE_VARIABLE
+///
+/// LockBox:
+///   GUID - gEfiFirmwarePerformanceGuid
+///   Data - EFI_ACPI_BASIC_S3_SUSPEND_PERFORMANCE_RECORD
+///
+/// Hob:
+///   GUID - gEfiFirmwarePerformanceGuid
+///   Data - FIRMWARE_SEC_PERFORMANCE (defined in <Ppi/SecPerformance.h>)
+///
+/// SMI:
+///   GUID - gEfiFirmwarePerformanceGuid
+///   Data - SMM_BOOT_RECORD_COMMUNICATE
+///
+/// StatusCodeData:
+///   Type - gEfiFirmwarePerformanceGuid
+///   Data - One or more boot record
+///
+#define EFI_FIRMWARE_PERFORMANCE_GUID \
+  { \
+    0xc095791a, 0x3001, 0x47b2, {0x80, 0xc9, 0xea, 0xc7, 0x31, 0x9f, 0x2f, 0xa4 } \
+  }
+
+#define EFI_FIRMWARE_PERFORMANCE_VARIABLE_NAME  L"FirmwarePerformance"
+
+/// LockBox:
+///   GUID - gFirmwarePerformanceS3PointerGuid
+///   Data - S3 performance table pointer
+///
+#define FIRMWARE_PERFORMANCE_S3_POINTER_GUID \
+  { \
+    0xdc65adc, 0xa973, 0x4130, { 0x8d, 0xf0, 0x2a, 0xdb, 0xeb, 0x9e, 0x4a, 0x31 } \
+  }
+
+#pragma pack(1)
+
+///
+/// Firmware Performance Data Table.
+/// This structure will be installed into ACPI table as FPDT in normal boot path.
+///
+typedef struct {
+  EFI_ACPI_DESCRIPTION_HEADER                             Header;            ///< Common ACPI description table header.
+  EFI_ACPI_5_0_FPDT_BOOT_PERFORMANCE_TABLE_POINTER_RECORD BootPointerRecord; ///< Basic Boot Performance Table Pointer record.
+  EFI_ACPI_5_0_FPDT_S3_PERFORMANCE_TABLE_POINTER_RECORD   S3PointerRecord;   ///< S3 Performance Table Pointer record.
+} FIRMWARE_PERFORMANCE_TABLE;
+
+///
+/// S3 Performance Data Table.
+/// This structure contains S3 performance records which will be updated in S3
+/// suspend and S3 resume boot path.
+///
+typedef struct {
+  EFI_ACPI_5_0_FPDT_PERFORMANCE_TABLE_HEADER  Header;    ///< Common ACPI table header.
+  EFI_ACPI_5_0_FPDT_S3_RESUME_RECORD          S3Resume;  ///< Basic S3 Resume performance record.
+  EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD         S3Suspend; ///< Basic S3 Suspend performance record.
+} S3_PERFORMANCE_TABLE;
+
+///
+/// Basic Boot Performance Data Table.
+/// This structure contains BasicBoot performance record.
+///
+typedef struct {
+  EFI_ACPI_5_0_FPDT_PERFORMANCE_TABLE_HEADER   Header;     ///< Common ACPI table header.
+  EFI_ACPI_5_0_FPDT_FIRMWARE_BASIC_BOOT_RECORD BasicBoot;  ///< Basic Boot Resume performance record.
+  //
+  // one or more boot performance records.
+  //
+} BOOT_PERFORMANCE_TABLE;
+
+///
+/// Performance data pointed by Performance Pointer Record.
+///
+typedef struct {
+  BOOT_PERFORMANCE_TABLE         BootPerformance; ///< Basic Boot Performance.
+  S3_PERFORMANCE_TABLE           S3Performance;   ///< S3 performance.
+} FIRMWARE_PERFORMANCE_RUNTIME_DATA;
+
+///
+/// Variable defined for FPDT implementation.
+/// This Variable is produced by FPDT DXE module.
+///
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  BootPerformanceTablePointer; ///< Pointer to Boot Performance Table.
+  EFI_PHYSICAL_ADDRESS  S3PerformanceTablePointer;   ///< Pointer to S3 Performance Table.
+} FIRMWARE_PERFORMANCE_VARIABLE;
+
+#pragma pack()
+
+//
+// Log BOOT RECORD from SMM driver on boot time.
+//
+#define SMM_FPDT_FUNCTION_GET_BOOT_RECORD_SIZE           1
+#define SMM_FPDT_FUNCTION_GET_BOOT_RECORD_DATA           2
+#define SMM_FPDT_FUNCTION_GET_BOOT_RECORD_DATA_BY_OFFSET 3
+
+typedef struct {
+  UINTN             Function;
+  EFI_STATUS        ReturnStatus;
+  UINTN             BootRecordSize;
+  VOID              *BootRecordData;
+  UINTN             BootRecordOffset;
+} SMM_BOOT_RECORD_COMMUNICATE;
+
+extern EFI_GUID gEfiFirmwarePerformanceGuid;
+extern EFI_GUID gFirmwarePerformanceS3PointerGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/HiiBootMaintenanceFormset.h b/Core/MdeModulePkg/Include/Guid/HiiBootMaintenanceFormset.h
new file mode 100644
index 0000000000..25dcff81fd
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/HiiBootMaintenanceFormset.h
@@ -0,0 +1,28 @@
+/** @file
+  Guid definition for Boot Maintainence Formset.
+
+Copyright (c) 2015, 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.
+
+**/
+
+
+#ifndef __HII_BOOT_MAINTENANCE_FORMSET_H__
+#define __HII_BOOT_MAINTENANCE_FORMSET_H__
+
+///
+/// Guid define to group the item show on the Boot Menaintenance Manager Menu.
+///
+#define EFI_IFR_BOOT_MAINTENANCE_GUID \
+  { 0xb2dedc91, 0xd59f, 0x48d2, { 0x89, 0x8a, 0x12, 0x49, 0xc, 0x74, 0xa4, 0xe0 } }
+
+
+extern EFI_GUID gEfiIfrBootMaintenanceGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/HiiResourceSampleHii.h b/Core/MdeModulePkg/Include/Guid/HiiResourceSampleHii.h
new file mode 100644
index 0000000000..2a86cad7bf
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/HiiResourceSampleHii.h
@@ -0,0 +1,23 @@
+/** @file
+  GUID used as HII FormSet GUID in HII Resource Sample driver. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __HII_RESOURCE_SAMPLE_HII_GUID_H__
+#define __HII_RESOURCE_SAMPLE_HII_GUID_H__
+
+#define HII_RESOURCE_SAMPLE_FORM_SET_GUID  \
+    { 0x4f4ef7f0, 0xaa29, 0x4ce9, { 0xba, 0x41, 0x64, 0x3e, 0x1, 0x23, 0xa9, 0x9f }}
+
+extern EFI_GUID gHiiResourceSamleFormSetGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/IdleLoopEvent.h b/Core/MdeModulePkg/Include/Guid/IdleLoopEvent.h
new file mode 100644
index 0000000000..3673207674
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/IdleLoopEvent.h
@@ -0,0 +1,24 @@
+/** @file
+  GUID is the name of events used with CreateEventEx in order to be notified
+  when the DXE Core is idle.
+
+  Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef __IDLE_LOOP_EVENT_GUID_H__
+#define __IDLE_LOOP_EVENT_GUID_H__
+
+#define IDLE_LOOP_EVENT_GUID \
+   { 0x3c8d294c, 0x5fc3, 0x4451, { 0xbb, 0x31, 0xc4, 0xc0, 0x32, 0x29, 0x5e, 0x6c } }
+
+extern EFI_GUID gIdleLoopEventGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/Ip4Config2Hii.h b/Core/MdeModulePkg/Include/Guid/Ip4Config2Hii.h
new file mode 100644
index 0000000000..b1b41b9230
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/Ip4Config2Hii.h
@@ -0,0 +1,25 @@
+/** @file
+  GUIDs used as HII FormSet and HII Package list GUID in Ip4Dxe driver. 
+  
+Copyright (c) 2015, 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 that 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.
+
+**/
+
+#ifndef __IP4_CONFIG2_HII_GUID_H__
+#define __IP4_CONFIG2_HII_GUID_H__
+
+#define IP4_CONFIG2_NVDATA_GUID \
+  { \
+    0x9b942747, 0x154e, 0x4d29, { 0xa4, 0x36, 0xbf, 0x71, 0x0, 0xc8, 0xb5, 0x3b } \
+  }
+
+extern EFI_GUID gIp4Config2NvDataGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/Ip4IScsiConfigHii.h b/Core/MdeModulePkg/Include/Guid/Ip4IScsiConfigHii.h
new file mode 100644
index 0000000000..4eb4c828ff
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/Ip4IScsiConfigHii.h
@@ -0,0 +1,31 @@
+/** @file
+  GUIDs used as HII FormSet and HII Package list GUID in IP4 IScsiDxe driver. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __IP4_ISCSI_CONFIG_HII_GUID_H__
+#define __IP4_ISCSI_CONFIG_HII_GUID_H__
+
+#define IP4_ISCSI_CONFIG_GUID \
+  { \
+    0x6456ed61, 0x3579, 0x41c9, { 0x8a, 0x26, 0x0a, 0x0b, 0xd6, 0x2b, 0x78, 0xfc } \
+  }
+
+#define ISCSI_CHAP_AUTH_INFO_GUID \
+  { \
+    0x786ec0ac, 0x65ae, 0x4d1b, {0xb1, 0x37, 0xd, 0x11, 0xa, 0x48, 0x37, 0x97} \
+  }
+
+extern EFI_GUID gIp4IScsiConfigGuid;
+extern EFI_GUID gIScsiCHAPAuthInfoGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/LoadModuleAtFixedAddress.h b/Core/MdeModulePkg/Include/Guid/LoadModuleAtFixedAddress.h
new file mode 100644
index 0000000000..ec467bbcb2
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/LoadModuleAtFixedAddress.h
@@ -0,0 +1,34 @@
+/** @file
+  This file defines a configuration Table Guid for Load module at fixed address.
+   
+  This configuration table is to hold  the top address below which the Dxe runtime code and 
+  boot time code will be loaded and Tseg base. When this feature is enabled, Build tools will assigned 
+  module loading address relative to these two addresses.
+  
+
+Copyright (c) 2010, 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 that 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.
+
+**/
+
+#ifndef __LOAD_MODULE_AT_FIX_ADDRESS_GUID_H__
+#define __LOAD_MODULE_AT_FIX_ADDRESS_GUID_H__
+
+#define EFI_LOAD_FIXED_ADDRESS_CONFIGURATION_TABLE_GUID \
+  { 0x2CA88B53,0xD296,0x4080, { 0xA4,0xA5,0xCA,0xD9,0xBA,0xE2,0x4B,0x9} }
+
+
+extern EFI_GUID gLoadFixedAddressConfigurationTableGuid;
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS    DxeCodeTopAddress;   ///< The top address below which the Dxe runtime code and below which the Dxe runtime/boot code and PEI code.
+  EFI_PHYSICAL_ADDRESS    SmramBase;           ///< SMRAM base address. The build tool assigns an offset relative to the SMRAM base for a SMM driver.
+} EFI_LOAD_FIXED_ADDRESS_CONFIGURATION_TABLE;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/LzmaDecompress.h b/Core/MdeModulePkg/Include/Guid/LzmaDecompress.h
new file mode 100644
index 0000000000..18a6552f9c
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/LzmaDecompress.h
@@ -0,0 +1,35 @@
+/** @file
+  Lzma Custom decompress algorithm Guid definition.
+
+Copyright (c) 2009 - 2012, 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 that 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.
+
+**/
+
+#ifndef __LZMA_DECOMPRESS_GUID_H__
+#define __LZMA_DECOMPRESS_GUID_H__
+
+///
+/// The Global ID used to identify a section of an FFS file of type 
+/// EFI_SECTION_GUID_DEFINED, whose contents have been compressed using LZMA.
+///
+#define LZMA_CUSTOM_DECOMPRESS_GUID  \
+  { 0xEE4E5898, 0x3914, 0x4259, { 0x9D, 0x6E, 0xDC, 0x7B, 0xD7, 0x94, 0x03, 0xCF } }
+
+///
+/// The Global ID used to identify a section of an FFS file of type 
+/// EFI_SECTION_GUID_DEFINED, whose contents have been compressed using LZMA with X86 code Converter.
+///
+#define LZMAF86_CUSTOM_DECOMPRESS_GUID  \
+  { 0xD42AE6BD, 0x1352, 0x4bfb, { 0x90, 0x9A, 0xCA, 0x72, 0xA6, 0xEA, 0xE8, 0x89 } }
+
+extern GUID gLzmaCustomDecompressGuid;
+extern GUID gLzmaF86CustomDecompressGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/MdeModuleHii.h b/Core/MdeModulePkg/Include/Guid/MdeModuleHii.h
new file mode 100644
index 0000000000..81821da5a9
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MdeModuleHii.h
@@ -0,0 +1,220 @@
+/** @file
+  EDKII extented HII IFR guid opcodes.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __MDEMODULE_HII_H__
+#define __MDEMODULE_HII_H__
+
+#define NARROW_CHAR         0xFFF0
+#define WIDE_CHAR           0xFFF1
+#define NON_BREAKING_CHAR   0xFFF2
+
+///
+/// State defined for password statemachine .
+///
+#define BROWSER_STATE_VALIDATE_PASSWORD  0
+#define BROWSER_STATE_SET_PASSWORD       1
+
+///
+/// GUIDed opcodes defined for EDKII implementation.
+///
+#define EFI_IFR_TIANO_GUID \
+  { 0xf0b1735, 0x87a0, 0x4193, {0xb2, 0x66, 0x53, 0x8c, 0x38, 0xaf, 0x48, 0xce} }
+
+#pragma pack(1)
+
+///
+/// EDKII implementation extension opcodes, new extension can be added here later.
+///
+#define EFI_IFR_EXTEND_OP_LABEL       0x0
+#define EFI_IFR_EXTEND_OP_BANNER      0x1
+#define EFI_IFR_EXTEND_OP_TIMEOUT     0x2
+#define EFI_IFR_EXTEND_OP_CLASS       0x3
+#define EFI_IFR_EXTEND_OP_SUBCLASS    0x4
+
+///
+/// Label opcode.
+///
+typedef struct _EFI_IFR_GUID_LABEL {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_TIANO_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_LABEL.
+  ///
+  UINT8               ExtendOpCode;
+  ///
+  /// Label Number.
+  ///
+  UINT16              Number;
+} EFI_IFR_GUID_LABEL;
+
+#define EFI_IFR_BANNER_ALIGN_LEFT     0
+#define EFI_IFR_BANNER_ALIGN_CENTER   1
+#define EFI_IFR_BANNER_ALIGN_RIGHT    2
+
+///
+/// Banner opcode.
+///
+typedef struct _EFI_IFR_GUID_BANNER {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_TIANO_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_BANNER
+  ///
+  UINT8               ExtendOpCode;
+  EFI_STRING_ID       Title;        ///< The string token for the banner title.
+  UINT16              LineNumber;   ///< 1-based line number.
+  UINT8               Alignment;    ///< left, center, or right-aligned.
+} EFI_IFR_GUID_BANNER;
+
+///
+/// Timeout opcode.
+///
+typedef struct _EFI_IFR_GUID_TIMEOUT {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_TIANO_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_TIMEOUT.
+  ///
+  UINT8               ExtendOpCode;
+  UINT16              TimeOut;       ///< TimeOut Value.
+} EFI_IFR_GUID_TIMEOUT;
+
+#define EFI_NON_DEVICE_CLASS              0x00
+#define EFI_DISK_DEVICE_CLASS             0x01
+#define EFI_VIDEO_DEVICE_CLASS            0x02
+#define EFI_NETWORK_DEVICE_CLASS          0x04
+#define EFI_INPUT_DEVICE_CLASS            0x08
+#define EFI_ON_BOARD_DEVICE_CLASS         0x10
+#define EFI_OTHER_DEVICE_CLASS            0x20
+
+///
+/// Device Class opcode.
+///
+typedef struct _EFI_IFR_GUID_CLASS {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_TIANO_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_CLASS.
+  ///
+  UINT8               ExtendOpCode;
+  UINT16              Class;           ///< Device Class from the above.
+} EFI_IFR_GUID_CLASS;
+
+#define EFI_SETUP_APPLICATION_SUBCLASS    0x00
+#define EFI_GENERAL_APPLICATION_SUBCLASS  0x01
+#define EFI_FRONT_PAGE_SUBCLASS           0x02
+#define EFI_SINGLE_USE_SUBCLASS           0x03
+
+///
+/// SubClass opcode
+///
+typedef struct _EFI_IFR_GUID_SUBCLASS {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_TIANO_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_SUBCLASS.
+  ///
+  UINT8               ExtendOpCode;
+  UINT16              SubClass;      ///< Sub Class type from the above.
+} EFI_IFR_GUID_SUBCLASS;
+
+///
+/// GUIDed opcodes support for framework vfr.
+///
+#define EFI_IFR_FRAMEWORK_GUID \
+  { 0x31ca5d1a, 0xd511, 0x4931, { 0xb7, 0x82, 0xae, 0x6b, 0x2b, 0x17, 0x8c, 0xd7 } }
+
+///
+/// Two extended opcodes are added, and new extensions can be added here later.
+/// One is for framework OneOf question Option Key value;
+/// another is for framework vareqval.
+///
+#define EFI_IFR_EXTEND_OP_OPTIONKEY   0x0
+#define EFI_IFR_EXTEND_OP_VAREQNAME   0x1
+
+///
+/// Store the framework vfr option key value.
+///
+typedef struct _EFI_IFR_GUID_OPTIONKEY {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_FRAMEWORK_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_OPTIONKEY.
+  ///
+  UINT8               ExtendOpCode;
+  ///
+  /// OneOf Questiond ID binded by OneOf Option.
+  ///
+  EFI_QUESTION_ID     QuestionId;
+  ///
+  /// The OneOf Option Value.
+  ///
+  EFI_IFR_TYPE_VALUE  OptionValue;
+  ///
+  /// The Framework OneOf Option Key Value.
+  ///
+  UINT16              KeyValue;
+} EFI_IFR_GUID_OPTIONKEY;
+
+///
+/// Store the framework vfr vareqval name number.
+///
+typedef struct _EFI_IFR_GUID_VAREQNAME {
+  EFI_IFR_OP_HEADER   Header;
+  ///
+  /// EFI_IFR_FRAMEWORK_GUID.
+  ///
+  EFI_GUID            Guid;
+  ///
+  /// EFI_IFR_EXTEND_OP_VAREQNAME.
+  ///
+  UINT8               ExtendOpCode;
+  ///
+  /// Question ID of the Numeric Opcode created.
+  ///
+  EFI_QUESTION_ID     QuestionId;
+  ///
+  /// For vareqval (0x100), NameId is 0x100.
+  /// This value will convert to a Unicode String following this rule;
+  ///            sprintf(StringBuffer, "%d", NameId) .
+  /// The the Unicode String will be used as a EFI Variable Name.
+  ///
+  UINT16              NameId;
+} EFI_IFR_GUID_VAREQNAME;
+
+#pragma pack()
+
+extern EFI_GUID gEfiIfrTianoGuid;
+extern EFI_GUID gEfiIfrFrameworkGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Guid/MdeModulePkgTokenSpace.h b/Core/MdeModulePkg/Include/Guid/MdeModulePkgTokenSpace.h
new file mode 100644
index 0000000000..6ba7cd9df1
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MdeModulePkgTokenSpace.h
@@ -0,0 +1,25 @@
+/** @file
+  GUID for MdeModulePkg PCD Token Space. 
+
+Copyright (c) 2009 - 2010, 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 that 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.
+
+**/
+
+#ifndef _MDEMODULEPKG_TOKEN_SPACE_GUID_H_
+#define _MDEMODULEPKG_TOKEN_SPACE_GUID_H_
+
+#define MDEMODULEPKG_TOKEN_SPACE_GUID \
+  { \
+    0xA1AFF049, 0xFDEB, 0x442a, { 0xB3, 0x20, 0x13, 0xAB, 0x4C, 0xB7, 0x2B, 0xBC } \
+  }
+
+extern EFI_GUID gEfiMdeModulePkgTokenSpaceGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/MemoryProfile.h b/Core/MdeModulePkg/Include/Guid/MemoryProfile.h
new file mode 100644
index 0000000000..9c70b9df39
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MemoryProfile.h
@@ -0,0 +1,304 @@
+/** @file
+  Memory profile data structure.
+
+  Copyright (c) 2014 - 2016, 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.
+
+**/
+
+#ifndef _MEMORY_PROFILE_H_
+#define _MEMORY_PROFILE_H_
+
+//
+// For BIOS MemoryType (0 ~ EfiMaxMemoryType - 1), it is recorded in UsageByType[MemoryType]. (Each valid entry has one entry)
+// For OS MemoryType (0x80000000 ~ 0xFFFFFFFF), it is recorded in UsageByType[EfiMaxMemoryType]. (All types are combined into one entry)
+// For OEM MemoryType (0x70000000 ~ 0x7FFFFFFF), it is recorded in UsageByType[EfiMaxMemoryType + 1]. (All types are combined into one entry)
+//
+
+typedef struct {
+  UINT32                       Signature;
+  UINT16                       Length;
+  UINT16                       Revision;
+} MEMORY_PROFILE_COMMON_HEADER;
+
+#define MEMORY_PROFILE_CONTEXT_SIGNATURE SIGNATURE_32 ('M','P','C','T')
+#define MEMORY_PROFILE_CONTEXT_REVISION 0x0002
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  UINT64                        CurrentTotalUsage;
+  UINT64                        PeakTotalUsage;
+  UINT64                        CurrentTotalUsageByType[EfiMaxMemoryType + 2];
+  UINT64                        PeakTotalUsageByType[EfiMaxMemoryType + 2];
+  UINT64                        TotalImageSize;
+  UINT32                        ImageCount;
+  UINT32                        SequenceCount;
+} MEMORY_PROFILE_CONTEXT;
+
+#define MEMORY_PROFILE_DRIVER_INFO_SIGNATURE SIGNATURE_32 ('M','P','D','I')
+#define MEMORY_PROFILE_DRIVER_INFO_REVISION 0x0002
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  EFI_GUID                      FileName;
+  PHYSICAL_ADDRESS              ImageBase;
+  UINT64                        ImageSize;
+  PHYSICAL_ADDRESS              EntryPoint;
+  UINT16                        ImageSubsystem;
+  EFI_FV_FILETYPE               FileType;
+  UINT8                         Reserved[1];
+  UINT32                        AllocRecordCount;
+  UINT64                        CurrentUsage;
+  UINT64                        PeakUsage;
+  UINT64                        CurrentUsageByType[EfiMaxMemoryType + 2];
+  UINT64                        PeakUsageByType[EfiMaxMemoryType + 2];
+} MEMORY_PROFILE_DRIVER_INFO;
+
+typedef enum {
+  MemoryProfileActionAllocatePages = 1,
+  MemoryProfileActionFreePages = 2,
+  MemoryProfileActionAllocatePool = 3,
+  MemoryProfileActionFreePool = 4,
+} MEMORY_PROFILE_ACTION;
+
+#define MEMORY_PROFILE_ALLOC_INFO_SIGNATURE SIGNATURE_32 ('M','P','A','I')
+#define MEMORY_PROFILE_ALLOC_INFO_REVISION 0x0001
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  PHYSICAL_ADDRESS              CallerAddress;
+  UINT32                        SequenceId;
+  UINT8                         Reserved[4];
+  MEMORY_PROFILE_ACTION         Action;
+  EFI_MEMORY_TYPE               MemoryType;
+  PHYSICAL_ADDRESS              Buffer;
+  UINT64                        Size;
+} MEMORY_PROFILE_ALLOC_INFO;
+
+#define MEMORY_PROFILE_DESCRIPTOR_SIGNATURE SIGNATURE_32 ('M','P','D','R')
+#define MEMORY_PROFILE_DESCRIPTOR_REVISION 0x0001
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  PHYSICAL_ADDRESS              Address;
+  UINT64                        Size;
+} MEMORY_PROFILE_DESCRIPTOR;
+
+#define MEMORY_PROFILE_FREE_MEMORY_SIGNATURE SIGNATURE_32 ('M','P','R','M')
+#define MEMORY_PROFILE_FREE_MEMORY_REVISION 0x0001
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  UINT64                        TotalFreeMemoryPages;
+  UINT32                        FreeMemoryEntryCount;
+  UINT8                         Reserved[4];
+  //MEMORY_PROFILE_DESCRIPTOR     MemoryDescriptor[FreeMemoryEntryCount];
+} MEMORY_PROFILE_FREE_MEMORY;
+
+#define MEMORY_PROFILE_MEMORY_RANGE_SIGNATURE SIGNATURE_32 ('M','P','M','R')
+#define MEMORY_PROFILE_MEMORY_RANGE_REVISION 0x0001
+
+typedef struct {
+  MEMORY_PROFILE_COMMON_HEADER  Header;
+  UINT32                        MemoryRangeCount;
+  UINT8                         Reserved[4];
+  //MEMORY_PROFILE_DESCRIPTOR     MemoryDescriptor[MemoryRangeCount];
+} MEMORY_PROFILE_MEMORY_RANGE;
+
+//
+// UEFI memory profile layout:
+// +--------------------------------+
+// | CONTEXT                        |
+// +--------------------------------+
+// | DRIVER_INFO(1)                 |
+// +--------------------------------+
+// | ALLOC_INFO(1, 1)               |
+// +--------------------------------+
+// | ALLOC_INFO(1, m1)              |
+// +--------------------------------+
+// | DRIVER_INFO(n)                 |
+// +--------------------------------+
+// | ALLOC_INFO(n, 1)               |
+// +--------------------------------+
+// | ALLOC_INFO(n, mn)              |
+// +--------------------------------+
+//
+
+typedef struct _EDKII_MEMORY_PROFILE_PROTOCOL EDKII_MEMORY_PROFILE_PROTOCOL;
+
+/**
+  Get memory profile data.
+
+  @param[in]      This              The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in, out] ProfileSize       On entry, points to the size in bytes of the ProfileBuffer.
+                                    On return, points to the size of the data returned in ProfileBuffer.
+  @param[out]     ProfileBuffer     Profile buffer.
+                      
+  @return EFI_SUCCESS               Get the memory profile data successfully.
+  @return EFI_BUFFER_TO_SMALL       The ProfileSize is too small for the resulting data. 
+                                    ProfileSize is updated with the size required.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_MEMORY_PROFILE_GET_DATA)(
+  IN     EDKII_MEMORY_PROFILE_PROTOCOL  *This,
+  IN OUT UINT64                         *ProfileSize,
+     OUT VOID                           *ProfileBuffer
+  );
+
+/**
+  Register image to memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+  @param[in] FileType           File type of the image.
+
+  @return EFI_SUCCESS           Register success.
+  @return EFI_OUT_OF_RESOURCE   No enough resource for this register.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_MEMORY_PROFILE_REGISTER_IMAGE)(
+  IN EDKII_MEMORY_PROFILE_PROTOCOL      *This,
+  IN EFI_DEVICE_PATH_PROTOCOL           *FilePath,
+  IN PHYSICAL_ADDRESS                   ImageBase,
+  IN UINT64                             ImageSize,
+  IN EFI_FV_FILETYPE                    FileType
+  );
+
+/**
+  Unregister image from memory profile.
+
+  @param[in] This               The EDKII_MEMORY_PROFILE_PROTOCOL instance.
+  @param[in] FilePath           File path of the image.
+  @param[in] ImageBase          Image base address.
+  @param[in] ImageSize          Image size.
+
+  @return EFI_SUCCESS           Unregister success.
+  @return EFI_NOT_FOUND         The image is not found.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_MEMORY_PROFILE_UNREGISTER_IMAGE)(
+  IN EDKII_MEMORY_PROFILE_PROTOCOL      *This,
+  IN EFI_DEVICE_PATH_PROTOCOL           *FilePath,
+  IN PHYSICAL_ADDRESS                   ImageBase,
+  IN UINT64                             ImageSize
+  );
+
+struct _EDKII_MEMORY_PROFILE_PROTOCOL {
+  EDKII_MEMORY_PROFILE_GET_DATA         GetData;
+  EDKII_MEMORY_PROFILE_REGISTER_IMAGE   RegisterImage;
+  EDKII_MEMORY_PROFILE_UNREGISTER_IMAGE UnregisterImage;
+};
+
+//
+// SMRAM profile layout:
+// +--------------------------------+
+// | CONTEXT                        |
+// +--------------------------------+
+// | DRIVER_INFO(1)                 |
+// +--------------------------------+
+// | ALLOC_INFO(1, 1)               |
+// +--------------------------------+
+// | ALLOC_INFO(1, m1)              |
+// +--------------------------------+
+// | DRIVER_INFO(n)                 |
+// +--------------------------------+
+// | ALLOC_INFO(n, 1)               |
+// +--------------------------------+
+// | ALLOC_INFO(n, mn)              |
+// +--------------------------------+
+// | FREE_MEMORY                    |
+// +--------------------------------+
+// | FREE MEMORY DESCRIPTOR(1)      |
+// +--------------------------------+
+// | FREE MEMORY DESCRIPTOR(p)      |
+// +--------------------------------+
+// | MEMORY_RANGE                   |
+// +--------------------------------+
+// | MEMORY RANGE DESCRIPTOR(1)     |
+// +--------------------------------+
+// | MEMORY RANGE DESCRIPTOR(q)     |
+// +--------------------------------+
+//
+
+//
+// SMRAM profile command
+//
+#define SMRAM_PROFILE_COMMAND_GET_PROFILE_INFO           0x1
+#define SMRAM_PROFILE_COMMAND_GET_PROFILE_DATA           0x2
+//
+// Below 2 commands are now used by ECP only and only valid before SmmReadyToLock
+//
+#define SMRAM_PROFILE_COMMAND_REGISTER_IMAGE             0x3
+#define SMRAM_PROFILE_COMMAND_UNREGISTER_IMAGE           0x4
+
+#define SMRAM_PROFILE_COMMAND_GET_PROFILE_DATA_BY_OFFSET 0x5
+
+typedef struct {
+  UINT32                            Command;
+  UINT32                            DataLength;
+  UINT64                            ReturnStatus;
+} SMRAM_PROFILE_PARAMETER_HEADER;
+
+typedef struct {
+  SMRAM_PROFILE_PARAMETER_HEADER    Header;
+  UINT64                            ProfileSize;
+} SMRAM_PROFILE_PARAMETER_GET_PROFILE_INFO;
+
+typedef struct {
+  SMRAM_PROFILE_PARAMETER_HEADER    Header;
+  UINT64                            ProfileSize;
+  PHYSICAL_ADDRESS                  ProfileBuffer;
+} SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA;
+
+typedef struct {
+  SMRAM_PROFILE_PARAMETER_HEADER    Header;
+  //
+  // On input, profile buffer size.
+  // On output, actual profile data size copied.
+  //
+  UINT64                            ProfileSize;
+  PHYSICAL_ADDRESS                  ProfileBuffer;
+  //
+  // On input, profile buffer offset to copy.
+  // On output, next time profile buffer offset to copy.
+  //
+  UINT64                            ProfileOffset;
+} SMRAM_PROFILE_PARAMETER_GET_PROFILE_DATA_BY_OFFSET;
+
+typedef struct {
+  SMRAM_PROFILE_PARAMETER_HEADER    Header;
+  EFI_GUID                          FileName;
+  PHYSICAL_ADDRESS                  ImageBuffer;
+  UINT64                            NumberOfPage;
+} SMRAM_PROFILE_PARAMETER_REGISTER_IMAGE;
+
+typedef struct {
+  SMRAM_PROFILE_PARAMETER_HEADER    Header;
+  EFI_GUID                          FileName;
+  PHYSICAL_ADDRESS                  ImageBuffer;
+  UINT64                            NumberOfPage;
+} SMRAM_PROFILE_PARAMETER_UNREGISTER_IMAGE;
+
+
+#define EDKII_MEMORY_PROFILE_GUID { \
+  0x821c9a09, 0x541a, 0x40f6, 0x9f, 0x43, 0xa, 0xd1, 0x93, 0xa1, 0x2c, 0xfe \
+}
+
+extern EFI_GUID gEdkiiMemoryProfileGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Guid/MemoryStatusCodeRecord.h b/Core/MdeModulePkg/Include/Guid/MemoryStatusCodeRecord.h
new file mode 100644
index 0000000000..d6c3243094
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MemoryStatusCodeRecord.h
@@ -0,0 +1,83 @@
+/** @file
+  GUID used to identify status code records HOB that originate from the PEI status code.    
+  
+Copyright (c) 2006 - 2010, 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 that 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.             
+
+**/
+
+#ifndef __MEMORY_STATUS_CODE_RECORD_H__
+#define __MEMORY_STATUS_CODE_RECORD_H__
+
+///
+/// Global ID used to identify GUIDed HOBs that start with a structure of type 
+/// MEMORY_STATUSCODE_PACKET_HEADER, followed by an array of structures of type 
+/// MEMORY_STATUSCODE_RECORD.  These GUIDed HOBs record all the information 
+/// passed into the ReportStatusCode() service of PEI Services Table.
+///
+///  <pre>
+///  Memory status code records packet structure :
+///  +---------------+----------+----------+-----+----------+-----+----------+
+///  | Packet Header | Record 1 | Record 2 | ... + Record n | ... | Record m |
+///  +---------------+----------+----------+-----+----------+-----+----------+
+///                  ^                                 ^                     ^
+///                  +--------- RecordIndex -----------+                     |
+///                  +---------------- MaxRecordsNumber----------------------+
+///  </pre>
+///
+#define MEMORY_STATUS_CODE_RECORD_GUID \
+  { \
+    0x60cc026, 0x4c0d, 0x4dda, {0x8f, 0x41, 0x59, 0x5f, 0xef, 0x0, 0xa5, 0x2} \
+  }
+
+///
+/// A header structure that is followed by an array of records that contain the 
+/// parameters passed into the ReportStatusCode() service in the PEI Services Table.
+///
+typedef struct {
+  ///
+  /// Index of the packet.
+  ///
+  UINT16  PacketIndex;
+  ///
+  /// The number of active records in the packet.
+  ///
+  UINT16  RecordIndex;
+  ///
+  /// The maximum number of records that the packet can store.
+  ///
+  UINT32  MaxRecordsNumber;
+} MEMORY_STATUSCODE_PACKET_HEADER;
+
+///
+/// A structure that contains the parameters passed into the ReportStatusCode() 
+/// service in the PEI Services Table.
+///
+typedef struct {
+  ///
+  /// Status Code type to be reported.
+  ///
+  EFI_STATUS_CODE_TYPE   CodeType;
+
+  ///
+  /// An operation, plus value information about the class and subclass, used to
+  /// classify the hardware and software entity.
+  ///
+  EFI_STATUS_CODE_VALUE  Value;
+
+  ///
+  /// The enumeration of a hardware or software entity within
+  /// the system. Valid instance numbers start with the number 1.
+  ///
+  UINT32                 Instance;
+} MEMORY_STATUSCODE_RECORD;
+
+extern EFI_GUID gMemoryStatusCodeRecordGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/MemoryTypeInformation.h b/Core/MdeModulePkg/Include/Guid/MemoryTypeInformation.h
new file mode 100644
index 0000000000..57896d9463
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MemoryTypeInformation.h
@@ -0,0 +1,36 @@
+/** @file
+  This file defines:
+  * Memory Type Information GUID for HOB and Variable.
+  * Memory Type Information Variable Name.
+  * Memory Type Information GUID HOB data structure.
+
+  The memory type information HOB and variable can 
+  be used to store the information for each memory type in Variable or HOB.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __MEMORY_TYPE_INFORMATION_GUID_H__
+#define __MEMORY_TYPE_INFORMATION_GUID_H__
+
+#define EFI_MEMORY_TYPE_INFORMATION_GUID \
+  { 0x4c19049f,0x4137,0x4dd3, { 0x9c,0x10,0x8b,0x97,0xa8,0x3f,0xfd,0xfa } }
+
+#define EFI_MEMORY_TYPE_INFORMATION_VARIABLE_NAME L"MemoryTypeInformation"
+
+extern EFI_GUID gEfiMemoryTypeInformationGuid;
+
+typedef struct {
+  UINT32  Type;             ///< EFI memory type defined in UEFI specification.
+  UINT32  NumberOfPages;    ///< The pages of this type memory.
+} EFI_MEMORY_TYPE_INFORMATION;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/MtcVendor.h b/Core/MdeModulePkg/Include/Guid/MtcVendor.h
new file mode 100644
index 0000000000..77bc2ae1b8
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/MtcVendor.h
@@ -0,0 +1,31 @@
+/** @file
+  GUID is for MTC variable. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __MTC_VENDOR_GUID_H__
+#define __MTC_VENDOR_GUID_H__
+
+//
+// Vendor GUID of the variable for the high part of monotonic counter (UINT32).
+//
+#define MTC_VENDOR_GUID \
+  { 0xeb704011, 0x1402, 0x11d3, { 0x8e, 0x77, 0x0, 0xa0, 0xc9, 0x69, 0x72, 0x3b } }
+
+//
+// Name of the variable for the high part of monotonic counter
+//
+#define MTC_VARIABLE_NAME L"MTC"
+
+extern EFI_GUID gMtcVendorGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/PcdDataBaseHobGuid.h b/Core/MdeModulePkg/Include/Guid/PcdDataBaseHobGuid.h
new file mode 100644
index 0000000000..1908eb84b3
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/PcdDataBaseHobGuid.h
@@ -0,0 +1,25 @@
+/** @file
+  Hob guid for Pcd DataBase.
+
+Copyright (c) 2009 - 2010, 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 that 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.
+
+**/
+
+#ifndef _PCD_DATABASE_HOB_GUID_H_
+#define _PCD_DATABASE_HOB_GUID_H_
+
+#define PCD_DATABASE_HOB_GUID \
+  { \
+    0xEA296D92, 0x0B69, 0x423C, { 0x8C, 0x28, 0x33, 0xB4, 0xE0, 0xA9, 0x12, 0x68 } \
+  }
+
+extern EFI_GUID gPcdDataBaseHobGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/PcdDataBaseSignatureGuid.h b/Core/MdeModulePkg/Include/Guid/PcdDataBaseSignatureGuid.h
new file mode 100644
index 0000000000..ac95f7e21a
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/PcdDataBaseSignatureGuid.h
@@ -0,0 +1,150 @@
+/** @file
+  Guid for Pcd DataBase Signature.
+
+Copyright (c) 2012 - 2015, 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 that 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.
+
+**/
+
+#ifndef _PCD_DATABASE_SIGNATURE_GUID_H_
+#define _PCD_DATABASE_SIGNATURE_GUID_H_
+
+#define PCD_DATA_BASE_SIGNATURE_GUID \
+{ 0x3c7d193c, 0x682c, 0x4c14, { 0xa6, 0x8f, 0x55, 0x2d, 0xea, 0x4f, 0x43, 0x7e } }
+
+extern EFI_GUID gPcdDataBaseSignatureGuid;
+
+//
+// Common definitions
+//
+typedef UINT8 SKU_ID;
+
+#define PCD_TYPE_SHIFT        28
+
+#define PCD_TYPE_DATA         (0x0U << PCD_TYPE_SHIFT)
+#define PCD_TYPE_HII          (0x8U << PCD_TYPE_SHIFT)
+#define PCD_TYPE_VPD          (0x4U << PCD_TYPE_SHIFT)
+#define PCD_TYPE_SKU_ENABLED  (0x2U << PCD_TYPE_SHIFT)
+#define PCD_TYPE_STRING       (0x1U << PCD_TYPE_SHIFT)
+
+#define PCD_TYPE_ALL_SET      (PCD_TYPE_DATA | PCD_TYPE_HII | PCD_TYPE_VPD | PCD_TYPE_SKU_ENABLED | PCD_TYPE_STRING)
+
+#define PCD_DATUM_TYPE_SHIFT  24
+
+#define PCD_DATUM_TYPE_POINTER  (0x0U << PCD_DATUM_TYPE_SHIFT)
+#define PCD_DATUM_TYPE_UINT8    (0x1U << PCD_DATUM_TYPE_SHIFT)
+#define PCD_DATUM_TYPE_UINT16   (0x2U << PCD_DATUM_TYPE_SHIFT)
+#define PCD_DATUM_TYPE_UINT32   (0x4U << PCD_DATUM_TYPE_SHIFT)
+#define PCD_DATUM_TYPE_UINT64   (0x8U << PCD_DATUM_TYPE_SHIFT)
+
+#define PCD_DATUM_TYPE_ALL_SET  (PCD_DATUM_TYPE_POINTER | \
+                                 PCD_DATUM_TYPE_UINT8   | \
+                                 PCD_DATUM_TYPE_UINT16  | \
+                                 PCD_DATUM_TYPE_UINT32  | \
+                                 PCD_DATUM_TYPE_UINT64)
+
+#define PCD_DATUM_TYPE_SHIFT2 20
+
+#define PCD_DATUM_TYPE_UINT8_BOOLEAN (0x1U << PCD_DATUM_TYPE_SHIFT2)
+
+#define PCD_DATABASE_OFFSET_MASK (~(PCD_TYPE_ALL_SET | PCD_DATUM_TYPE_ALL_SET | PCD_DATUM_TYPE_UINT8_BOOLEAN))
+
+typedef struct  {
+  UINT32  ExTokenNumber;
+  UINT16  TokenNumber;          // Token Number for Dynamic-Ex PCD.
+  UINT16  ExGuidIndex;          // Index of GuidTable in units of GUID.
+} DYNAMICEX_MAPPING;
+
+typedef struct {
+  UINT32  SkuDataStartOffset;   // Offset(with TYPE info) from the PCD_DB.
+  UINT32  SkuIdTableOffset;     // Offset from the PCD_DB.
+} SKU_HEAD;
+
+typedef struct {
+  UINT32  StringIndex;          // Offset in String Table in units of UINT8.
+  UINT32  DefaultValueOffset;   // Offset of the Default Value.
+  UINT16  GuidTableIndex;       // Offset in Guid Table in units of GUID.
+  UINT16  Offset;               // Offset in Variable.
+  UINT32  Attributes;           // Variable attributes.
+  UINT16  Property;             // Variable property.
+  UINT16  Reserved;
+} VARIABLE_HEAD;
+
+typedef struct {
+  UINT32  Offset;
+} VPD_HEAD;
+
+typedef UINT32 STRING_HEAD;
+
+typedef UINT16 SIZE_INFO;
+
+typedef struct {
+  UINT32  TokenSpaceCNameIndex; // Offset in String Table in units of UINT8.
+  UINT32  PcdCNameIndex;        // Offset in String Table in units of UINT8.
+} PCD_NAME_INDEX;
+
+typedef UINT32 TABLE_OFFSET;
+
+typedef struct {
+    GUID                  Signature;            // PcdDataBaseGuid.
+    UINT32                BuildVersion;
+    UINT32                Length;
+    UINT32                UninitDataBaseSize;   // Total size for PCD those default value with 0.
+    TABLE_OFFSET          LocalTokenNumberTableOffset;
+    TABLE_OFFSET          ExMapTableOffset;
+    TABLE_OFFSET          GuidTableOffset;
+    TABLE_OFFSET          StringTableOffset;
+    TABLE_OFFSET          SizeTableOffset;
+    TABLE_OFFSET          SkuIdTableOffset;
+    TABLE_OFFSET          PcdNameTableOffset;
+    UINT16                LocalTokenCount;      // LOCAL_TOKEN_NUMBER for all.
+    UINT16                ExTokenCount;         // EX_TOKEN_NUMBER for DynamicEx.
+    UINT16                GuidTableCount;       // The Number of Guid in GuidTable.
+    SKU_ID                SystemSkuId;          // Current SkuId value.
+    UINT8                 Pad;                  // Pad bytes to satisfy the alignment.
+
+    //
+    // Default initialized external PCD database binary structure
+    //
+    // Padding is needed to keep necessary alignment
+    //
+    //UINT64                         ValueUint64[];
+    //UINT32                         ValueUint32[];
+    //VPD_HEAD                       VpdHead[];               // VPD Offset
+    //DYNAMICEX_MAPPING              ExMapTable[];            // DynamicEx PCD mapped to LocalIndex in LocalTokenNumberTable. It can be accessed by the ExMapTableOffset.
+    //UINT32                         LocalTokenNumberTable[]; // Offset | DataType | PCD Type. It can be accessed by LocalTokenNumberTableOffset.
+    //GUID                           GuidTable[];             // GUID for DynamicEx and HII PCD variable Guid. It can be accessed by the GuidTableOffset.
+    //STRING_HEAD                    StringHead[];            // String PCD
+    //PCD_NAME_INDEX                 PcdNameTable[];          // PCD name index info. It can be accessed by the PcdNameTableOffset.
+    //VARIABLE_HEAD                  VariableHead[];          // HII PCD
+    //SKU_HEAD                       SkuHead[];               // Store SKU info for each PCD with SKU enable.
+    //UINT8                          StringTable[];           // String for String PCD value and HII PCD Variable Name. It can be accessed by StringTableOffset.
+    //SIZE_INFO                      SizeTable[];             // MaxSize and CurSize for String PCD. It can be accessed by SizeTableOffset.
+    //UINT16                         ValueUint16[];
+    //UINT8                          ValueUint8[];
+    //BOOLEAN                        ValueBoolean[];
+    //UINT8                          SkuIdTable[];            // SkuIds system supports.
+    //UINT8                          SkuIndexTable[];         // SkuIds for each PCD with SKU enable.
+
+} PCD_DATABASE_INIT;
+
+//
+// PEI and DXE Pcd driver use the same PCD database
+//
+typedef PCD_DATABASE_INIT  PEI_PCD_DATABASE;
+typedef PCD_DATABASE_INIT  DXE_PCD_DATABASE;
+
+
+typedef struct {
+  PEI_PCD_DATABASE  *PeiDb;
+  DXE_PCD_DATABASE  *DxeDb;
+} PCD_DATABASE;
+
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/Performance.h b/Core/MdeModulePkg/Include/Guid/Performance.h
new file mode 100644
index 0000000000..c40046c878
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/Performance.h
@@ -0,0 +1,358 @@
+/** @file
+  This file defines performance-related definitions, including the format of:
+  * performance GUID HOB.
+  * performance protocol interfaces.
+  * performance variables.  
+
+Copyright (c) 2009 - 2013, 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 that 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.
+
+**/
+
+#ifndef __PERFORMANCE_DATA_H__
+#define __PERFORMANCE_DATA_H__
+
+//
+// PEI_PERFORMANCE_STRING_SIZE must be a multiple of 8.
+//
+#define PEI_PERFORMANCE_STRING_SIZE     8
+#define PEI_PERFORMANCE_STRING_LENGTH   (PEI_PERFORMANCE_STRING_SIZE - 1)
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  Handle;
+  CHAR8                 Token[PEI_PERFORMANCE_STRING_SIZE];   ///< Measured token string name. 
+  CHAR8                 Module[PEI_PERFORMANCE_STRING_SIZE];  ///< Module string name.
+  UINT64                StartTimeStamp;                       ///< Start time point.
+  UINT64                EndTimeStamp;                         ///< End time point.
+} PEI_PERFORMANCE_LOG_ENTRY;
+
+//
+// The header must be aligned at 8 bytes.
+// 
+typedef struct {
+  UINT32                NumberOfEntries;  ///< The number of all performance log entries.
+  UINT32                Reserved;
+} PEI_PERFORMANCE_LOG_HEADER;
+
+
+//
+// The data structure for performance data in ACPI memory.
+//
+#define PERFORMANCE_SIGNATURE   SIGNATURE_32 ('P', 'e', 'r', 'f')
+#define PERF_TOKEN_SIZE         28
+#define PERF_TOKEN_LENGTH       (PERF_TOKEN_SIZE - 1)
+#define PERF_PEI_ENTRY_MAX_NUM  50
+#define PERF_DATA_MAX_LENGTH    0x4000
+
+typedef struct {
+  CHAR8   Token[PERF_TOKEN_SIZE];
+  UINT32  Duration;
+} PERF_DATA;
+
+typedef struct {
+  UINT64        BootToOs;
+  UINT64        S3Resume;
+  UINT32        S3EntryNum;
+  PERF_DATA     S3Entry[PERF_PEI_ENTRY_MAX_NUM];
+  UINT64        CpuFreq;
+  UINT64        BDSRaw;
+  UINT32        Count;
+  UINT32        Signiture;
+} PERF_HEADER;
+
+#define PERFORMANCE_PROTOCOL_GUID \
+  { 0x76b6bdfa, 0x2acd, 0x4462, { 0x9E, 0x3F, 0xcb, 0x58, 0xC9, 0x69, 0xd9, 0x37 } }
+
+#define PERFORMANCE_EX_PROTOCOL_GUID \
+  { 0x1ea81bec, 0xf01a, 0x4d98, { 0xa2, 0x1,  0x4a, 0x61, 0xce, 0x2f, 0xc0, 0x22 } }
+
+//
+// Forward reference for pure ANSI compatibility
+//
+typedef struct _PERFORMANCE_PROTOCOL PERFORMANCE_PROTOCOL;
+typedef struct _PERFORMANCE_EX_PROTOCOL PERFORMANCE_EX_PROTOCOL;
+
+//
+// DXE_PERFORMANCE_STRING_SIZE must be a multiple of 8.
+//
+#define DXE_PERFORMANCE_STRING_SIZE     32
+#define DXE_PERFORMANCE_STRING_LENGTH   (DXE_PERFORMANCE_STRING_SIZE - 1)
+
+//
+// The default guage entries number for DXE phase.
+//
+#define INIT_DXE_GAUGE_DATA_ENTRIES     800
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  Handle;
+  CHAR8                 Token[DXE_PERFORMANCE_STRING_SIZE];  ///< Measured token string name. 
+  CHAR8                 Module[DXE_PERFORMANCE_STRING_SIZE]; ///< Module string name.
+  UINT64                StartTimeStamp;                      ///< Start time point.
+  UINT64                EndTimeStamp;                        ///< End time point.
+} GAUGE_DATA_ENTRY;
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  Handle;
+  CHAR8                 Token[DXE_PERFORMANCE_STRING_SIZE];  ///< Measured token string name. 
+  CHAR8                 Module[DXE_PERFORMANCE_STRING_SIZE]; ///< Module string name.
+  UINT64                StartTimeStamp;                      ///< Start time point.
+  UINT64                EndTimeStamp;                        ///< End time point.
+  UINT32                Identifier;                          ///< Identifier.
+} GAUGE_DATA_ENTRY_EX;
+
+//
+// The header must be aligned at 8 bytes
+//
+typedef struct {
+  UINT32                NumberOfEntries; ///< The number of all performance gauge entries.
+  UINT32                Reserved;
+} GAUGE_DATA_HEADER;
+
+//
+// SMM Performance Protocol definitions
+//
+
+#define SMM_PERFORMANCE_PROTOCOL_GUID \
+  { 0xf866226a, 0xeaa5, 0x4f5a, { 0xa9, 0xa,  0x6c, 0xfb, 0xa5, 0x7c, 0x58, 0x8e } }
+
+#define SMM_PERFORMANCE_EX_PROTOCOL_GUID \
+  { 0x931fc048, 0xc71d, 0x4455, { 0x89, 0x30, 0x47, 0x6,  0x30, 0xe3, 0xe,  0xe5 } }
+
+//
+// SMM_PERFORMANCE_STRING_SIZE.
+//
+#define SMM_PERFORMANCE_STRING_SIZE     32
+#define SMM_PERFORMANCE_STRING_LENGTH   (SMM_PERFORMANCE_STRING_SIZE - 1)
+
+//
+// The default guage entries number for SMM phase.
+//
+#define INIT_SMM_GAUGE_DATA_ENTRIES     200
+
+typedef struct {
+  UINTN             Function;
+  EFI_STATUS        ReturnStatus;
+  UINTN             NumberOfEntries;
+  UINTN             LogEntryKey;
+  GAUGE_DATA_ENTRY  *GaugeData;
+} SMM_PERF_COMMUNICATE;
+
+typedef struct {
+  UINTN                Function;
+  EFI_STATUS           ReturnStatus;
+  UINTN                NumberOfEntries;
+  UINTN                LogEntryKey;
+  GAUGE_DATA_ENTRY_EX  *GaugeDataEx;
+} SMM_PERF_COMMUNICATE_EX;
+
+#define SMM_PERF_FUNCTION_GET_GAUGE_ENTRY_NUMBER          1
+#define SMM_PERF_FUNCTION_GET_GAUGE_DATA                  2
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  The added record contains the Handle, Token, and Module.
+  The end time of the new record is not recorded, so it is set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  The pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   The pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  The pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               The 64-bit time stamp.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_START_GAUGE)(
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module, and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  The pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   The pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  The pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               The 64-bit time stamp.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_END_GAUGE)(
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGaugeEx and EndGaugeEx of PERFORMANCE_EX_PROTOCOL,
+  and then eliminate the Identifier.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntry stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntry          Out parameter for the indirect pointer to the gauge data entry specified by LogEntryKey.
+                                  
+  @retval EFI_SUCCESS             The GuageDataEntry is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           There is no entry after the measurement referred to by LogEntryKey.
+  @retval EFI_INVALID_PARAMETER   The LogEntryKey is not a valid entry, or GaugeDataEntry is NULL.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_GET_GAUGE)(
+  IN  UINTN               LogEntryKey,
+  OUT GAUGE_DATA_ENTRY    **GaugeDataEntry
+  );
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  The added record contains the Handle, Token, Module and Identifier.
+  The end time of the new record is not recorded, so it is set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  The pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   The pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  The pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               The 64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_START_GAUGE_EX)(
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier, and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  The pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   The pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  The pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               The 64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_END_GAUGE_EX)(
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGauge and EndGauge of PERFORMANCE_PROTOCOL,
+  and then assign the Identifier with 0.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntryEx stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntryEx        Out parameter for the indirect pointer to the extented gauge data entry specified by LogEntryKey.
+                                  
+  @retval EFI_SUCCESS             The GuageDataEntryEx is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           There is no entry after the measurement referred to by LogEntryKey.
+  @retval EFI_INVALID_PARAMETER   The LogEntryKey is not a valid entry, or GaugeDataEntryEx is NULL.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * PERFORMANCE_GET_GAUGE_EX)(
+  IN  UINTN                 LogEntryKey,
+  OUT GAUGE_DATA_ENTRY_EX   **GaugeDataEntryEx
+  );
+
+struct _PERFORMANCE_PROTOCOL {
+  PERFORMANCE_START_GAUGE             StartGauge;
+  PERFORMANCE_END_GAUGE               EndGauge;
+  PERFORMANCE_GET_GAUGE               GetGauge;
+};
+
+struct _PERFORMANCE_EX_PROTOCOL {
+  PERFORMANCE_START_GAUGE_EX          StartGaugeEx;
+  PERFORMANCE_END_GAUGE_EX            EndGaugeEx;
+  PERFORMANCE_GET_GAUGE_EX            GetGaugeEx;
+};
+
+extern EFI_GUID gPerformanceProtocolGuid;
+extern EFI_GUID gSmmPerformanceProtocolGuid;
+extern EFI_GUID gPerformanceExProtocolGuid;
+extern EFI_GUID gSmmPerformanceExProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/PiSmmCommunicationRegionTable.h b/Core/MdeModulePkg/Include/Guid/PiSmmCommunicationRegionTable.h
new file mode 100644
index 0000000000..e3021ef267
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/PiSmmCommunicationRegionTable.h
@@ -0,0 +1,63 @@
+/** @file
+
+Copyright (c) 2015, 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 that 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.
+
+**/
+
+#ifndef _PI_SMM_COMMUNICATION_REGION_TABLE_H_
+#define _PI_SMM_COMMUNICATION_REGION_TABLE_H_
+
+#define EDKII_PI_SMM_COMMUNICATION_REGION_TABLE_GUID {\
+  0x4e28ca50, 0xd582, 0x44ac, {0xa1, 0x1f, 0xe3, 0xd5, 0x65, 0x26, 0xdb, 0x34} \
+}
+
+//
+// This table to declare the generic SMM communication buffer location.
+// If this table is present, it means the SMM communication buffer is restricted to
+// EfiReservedMemoryType, EfiACPIMemoryNVS, or EfiRuntimeServicesData.
+//
+// This table is installed to UEFI configuration table by generic driver
+// or platform driver, at early DXE phase.
+// 
+// The EFI_MEMORY_DESCRIPTOR entry must contain at least one entry.
+// The entries must be normal memory region in EfiReservedMemoryType, EfiACPIMemoryNVS,
+// or EfiRuntimeServicesData.
+// If the Entry.Type is EfiConventionalMemory, it means this entry is free to use.
+// If the Entry.Type is other, it means this entry is occupied.
+//
+// Any non-SMM component may put communication data there, then use 
+// UEFI defined SMM Communication ACPI Table, or PI defined EFI_SMM_COMMUNICATION_PROTOCOL
+// to communicate with SMI handler. The process is:
+// 1) Find an entry whose type is EfiConventional.
+// 2) Change type to be EfiReservedMemoryType before use.
+// 3) Use it.
+// 4) Restore type be EfiConventional.
+//    The step 2) must be performed as an atomic transaction, if there might be conflict during runtime.
+//    For example, on IA-32/x64 platforms, this can be done using the CMPXCHG CPU instruction.
+//    If there is guarantee on no conflict during boot time, these steps can be skipped.
+//    For example, DXE, UEFI driver and UEFI application runs in sequence.
+//
+// For example, FPDT driver can use this communication buffer to get SMM
+// performance data in SMM. Profile driver can use this communication buffer
+// to get SMM profile data in SMM.
+//
+typedef struct {
+  UINT32                Version;
+  UINT32                NumberOfEntries;
+  UINT32                DescriptorSize;
+  UINT32                Reserved;
+//EFI_MEMORY_DESCRIPTOR Entry[1];
+} EDKII_PI_SMM_COMMUNICATION_REGION_TABLE;
+
+#define EDKII_PI_SMM_COMMUNICATION_REGION_TABLE_VERSION  0x00000001
+
+extern EFI_GUID gEdkiiPiSmmCommunicationRegionTableGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/PlatDriOverrideHii.h b/Core/MdeModulePkg/Include/Guid/PlatDriOverrideHii.h
new file mode 100644
index 0000000000..bd8e493ef9
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/PlatDriOverrideHii.h
@@ -0,0 +1,25 @@
+/** @file
+  GUIDs used as HII FormSet and HII Package list GUID in PlatDriOverride driver. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __PLATFORM_DRIVER_OVERRIDE_HII_GUID_H__
+#define __PLATFORM_DRIVER_OVERRIDE_HII_GUID_H__
+
+#define PLAT_OVER_MNGR_GUID \
+  { \
+    0x8614567d, 0x35be, 0x4415, {0x8d, 0x88, 0xbd, 0x7d, 0xc, 0x9c, 0x70, 0xc0} \
+  }
+
+extern EFI_GUID gPlatformOverridesManagerGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/RecoveryDevice.h b/Core/MdeModulePkg/Include/Guid/RecoveryDevice.h
new file mode 100644
index 0000000000..ad25065c6f
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/RecoveryDevice.h
@@ -0,0 +1,60 @@
+/** @file
+  Defines Name GUIDs to represent a Recovery Capsule loaded from a recovery device.
+
+  These are contracts between the recovery module and device recovery module
+  that convey the name of a given recovery module type.
+
+Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef _RECOVERY_DEVICE_H_
+#define _RECOVERY_DEVICE_H_
+
+///
+/// The Global ID used to identify a recovery capsule that was loaded from a CD/DVD device.
+///
+#define RECOVERY_ON_DATA_CD_GUID \
+  { \
+    0x5cac0099, 0x0dc9, 0x48e5, {0x80, 0x68, 0xbb, 0x95, 0xf5, 0x40, 0x0a, 0x9f } \
+  };
+
+///
+/// The Global ID used to identify a recovery capsule that was loaded from floppy device.
+///
+#define RECOVERY_ON_FAT_FLOPPY_DISK_GUID \
+  { \
+    0x2e3d2e75, 0x9b2e, 0x412d, {0xb4, 0xb1, 0x70, 0x41, 0x6b, 0x87, 0x0, 0xff } \
+  };
+
+///
+/// The Global ID used to identify a recovery capsule that was loaded from IDE hard drive.
+///
+#define RECOVERY_ON_FAT_IDE_DISK_GUID \
+  { \
+    0xb38573b6, 0x6200, 0x4ac5, {0xb5, 0x1d, 0x82, 0xe6, 0x59, 0x38, 0xd7, 0x83 } \
+  };
+
+///
+/// The Global ID used to identify a recovery capsule that was loaded from USB BOT device.
+///
+#define RECOVERY_ON_FAT_USB_DISK_GUID \
+  { \
+    0x0ffbce19, 0x324c, 0x4690, {0xa0, 0x09, 0x98, 0xc6, 0xae, 0x2e, 0xb1, 0x86 } \
+  };
+
+extern EFI_GUID gRecoveryOnDataCdGuid;
+extern EFI_GUID gRecoveryOnFatFloppyDiskGuid;
+extern EFI_GUID gRecoveryOnFatIdeDiskGuid;
+extern EFI_GUID gRecoveryOnFatUsbDiskGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/SmmLockBox.h b/Core/MdeModulePkg/Include/Guid/SmmLockBox.h
new file mode 100644
index 0000000000..8422847c5a
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/SmmLockBox.h
@@ -0,0 +1,73 @@
+/** @file
+  SmmLockBox guid header file.
+
+Copyright (c) 2010 - 2011, 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.
+
+**/
+
+#ifndef _SMM_LOCK_BOX_GUID_H_
+#define _SMM_LOCK_BOX_GUID_H_
+
+#define EFI_SMM_LOCK_BOX_COMMUNICATION_GUID \
+  {0x2a3cfebd, 0x27e8, 0x4d0a, {0x8b, 0x79, 0xd6, 0x88, 0xc2, 0xa3, 0xe1, 0xc0}}
+
+//
+// Below data structure is used for communication between PEI/DXE to SMM.
+//
+
+#define EFI_SMM_LOCK_BOX_COMMAND_SAVE                 0x1
+#define EFI_SMM_LOCK_BOX_COMMAND_UPDATE               0x2
+#define EFI_SMM_LOCK_BOX_COMMAND_RESTORE              0x3
+#define EFI_SMM_LOCK_BOX_COMMAND_SET_ATTRIBUTES       0x4
+#define EFI_SMM_LOCK_BOX_COMMAND_RESTORE_ALL_IN_PLACE 0x5
+
+typedef struct {
+  UINT32                         Command;
+  UINT32                         DataLength;
+  UINT64                         ReturnStatus;
+} EFI_SMM_LOCK_BOX_PARAMETER_HEADER;
+
+typedef struct {
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER  Header;
+  GUID                               Guid;
+  PHYSICAL_ADDRESS                   Buffer;
+  UINT64                             Length;
+} EFI_SMM_LOCK_BOX_PARAMETER_SAVE;
+
+typedef struct {
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER  Header;
+  GUID                               Guid;
+  UINT64                             Offset;
+  PHYSICAL_ADDRESS                   Buffer;
+  UINT64                             Length;
+} EFI_SMM_LOCK_BOX_PARAMETER_UPDATE;
+
+typedef struct {
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER  Header;
+  GUID                               Guid;
+  PHYSICAL_ADDRESS                   Buffer;
+  UINT64                             Length;
+} EFI_SMM_LOCK_BOX_PARAMETER_RESTORE;
+
+typedef struct {
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER  Header;
+  GUID                               Guid;
+  UINT64                             Attributes;
+} EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES;
+
+typedef struct {
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER  Header;
+} EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE;
+
+extern EFI_GUID gEfiSmmLockBoxCommunicationGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/SmmVariableCommon.h b/Core/MdeModulePkg/Include/Guid/SmmVariableCommon.h
new file mode 100644
index 0000000000..734eb19681
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/SmmVariableCommon.h
@@ -0,0 +1,129 @@
+/** @file
+  The file defined some common structures used for communicating between SMM variable module and SMM variable wrapper module.
+
+Copyright (c) 2011 - 2015, 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 that 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.
+
+**/
+
+#ifndef _SMM_VARIABLE_COMMON_H_
+#define _SMM_VARIABLE_COMMON_H_
+
+#include <Protocol/VarCheck.h>
+
+#define EFI_SMM_VARIABLE_WRITE_GUID \
+  { 0x93ba1826, 0xdffb, 0x45dd, { 0x82, 0xa7, 0xe7, 0xdc, 0xaa, 0x3b, 0xbd, 0xf3 } }
+
+extern EFI_GUID gSmmVariableWriteGuid;
+
+//
+// This structure is used for SMM variable. the collected statistics data is saved in SMRAM. It can be got from
+// SMI handler. The communication buffer should be:
+// EFI_SMM_COMMUNICATE_HEADER + SMM_VARIABLE_COMMUNICATE_HEADER + payload.
+//
+typedef struct {
+  UINTN       Function;
+  EFI_STATUS  ReturnStatus;
+  UINT8       Data[1];
+} SMM_VARIABLE_COMMUNICATE_HEADER;
+
+//
+// The payload for this function is SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+//
+#define SMM_VARIABLE_FUNCTION_GET_VARIABLE            1
+//
+// The payload for this function is SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME.
+//
+#define SMM_VARIABLE_FUNCTION_GET_NEXT_VARIABLE_NAME  2
+//
+// The payload for this function is SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+//
+#define SMM_VARIABLE_FUNCTION_SET_VARIABLE            3
+//
+// The payload for this function is SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO.
+//
+#define SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO     4
+//
+// It is a notify event, no extra payload for this function.
+//
+#define SMM_VARIABLE_FUNCTION_READY_TO_BOOT           5
+//
+// It is a notify event, no extra payload for this function.
+//
+#define SMM_VARIABLE_FUNCTION_EXIT_BOOT_SERVICE       6
+//
+// The payload for this function is VARIABLE_INFO_ENTRY. The GUID in EFI_SMM_COMMUNICATE_HEADER
+// is gEfiSmmVariableProtocolGuid.
+//
+#define SMM_VARIABLE_FUNCTION_GET_STATISTICS          7
+//
+// The payload for this function is SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE
+//
+#define SMM_VARIABLE_FUNCTION_LOCK_VARIABLE           8
+
+#define SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_SET  9
+
+#define SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_GET  10
+
+#define SMM_VARIABLE_FUNCTION_GET_PAYLOAD_SIZE        11
+
+///
+/// Size of SMM communicate header, without including the payload.
+///
+#define SMM_COMMUNICATE_HEADER_SIZE  (OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data))
+
+///
+/// Size of SMM variable communicate header, without including the payload.
+///
+#define SMM_VARIABLE_COMMUNICATE_HEADER_SIZE  (OFFSET_OF (SMM_VARIABLE_COMMUNICATE_HEADER, Data))
+
+///
+/// This structure is used to communicate with SMI handler by SetVariable and GetVariable.
+///
+typedef struct {
+  EFI_GUID    Guid;
+  UINTN       DataSize;
+  UINTN       NameSize;
+  UINT32      Attributes;
+  CHAR16      Name[1];
+} SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE;
+
+///
+/// This structure is used to communicate with SMI handler by GetNextVariableName.
+///
+typedef struct {
+  EFI_GUID    Guid;
+  UINTN       NameSize;     // Return name buffer size
+  CHAR16      Name[1];
+} SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME;
+
+///
+/// This structure is used to communicate with SMI handler by QueryVariableInfo.
+///
+typedef struct {
+  UINT64          MaximumVariableStorageSize;
+  UINT64          RemainingVariableStorageSize;
+  UINT64          MaximumVariableSize;
+  UINT32          Attributes;
+} SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO;
+
+typedef SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE;
+
+typedef struct {
+  EFI_GUID                      Guid;
+  UINTN                         NameSize;
+  VAR_CHECK_VARIABLE_PROPERTY   VariableProperty;
+  CHAR16                        Name[1];
+} SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY;
+
+typedef struct {
+  UINTN                         VariablePayloadSize;
+} SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE;
+
+#endif // _SMM_VARIABLE_COMMON_H_
diff --git a/Core/MdeModulePkg/Include/Guid/StandardErrorDevice.h b/Core/MdeModulePkg/Include/Guid/StandardErrorDevice.h
new file mode 100644
index 0000000000..7bc390241d
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/StandardErrorDevice.h
@@ -0,0 +1,24 @@
+/** @file
+  This GUID is installed to the device handler to specify that the device is a StdErr device.
+  
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __STANDARD_ERROR_DEVICE_H__
+#define __STANDARD_ERROR_DEVICE_H__
+
+#define EFI_STANDARD_ERROR_DEVICE_GUID    \
+    { 0xd3b36f2d, 0xd551, 0x11d4, {0x9a, 0x46, 0x0, 0x90, 0x27, 0x3f, 0xc1, 0x4d } }
+
+extern EFI_GUID gEfiStandardErrorDeviceGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/StatusCodeCallbackGuid.h b/Core/MdeModulePkg/Include/Guid/StatusCodeCallbackGuid.h
new file mode 100644
index 0000000000..2a83471a99
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/StatusCodeCallbackGuid.h
@@ -0,0 +1,26 @@
+/** @file
+  GUID used to identify HOB for pointers to callback functios registered on
+  PEI report status code router.
+  
+Copyright (c) 2009 - 2010, 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 that 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.             
+
+**/
+
+#ifndef __STATUS_CODE_CALLBACK_H__
+#define __STATUS_CODE_CALLBACK_H__
+
+#define STATUS_CODE_CALLBACK_GUID \
+  { \
+    0xe701458c, 0x4900, 0x4ca5, {0xb7, 0x72, 0x3d, 0x37, 0x94, 0x9f, 0x79, 0x27} \
+  }
+
+extern EFI_GUID gStatusCodeCallbackGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeDebug.h b/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeDebug.h
new file mode 100644
index 0000000000..f478f801b6
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeDebug.h
@@ -0,0 +1,49 @@
+/** @file
+  This file defines the GUID and data structure used to pass DEBUG() macro 
+  information to the Status Code Protocol and Status Code PPI.
+
+Copyright (c) 2007 - 2010, 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 that 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.
+
+**/
+
+#ifndef _STATUS_CODE_DATA_TYPE_DEBUG_H_
+#define _STATUS_CODE_DATA_TYPE_DEBUG_H_
+
+///
+/// The Global ID used to identify a structure of type EFI_DEBUG_INFO.
+///
+#define EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID \
+  { \
+    0x9A4E9246, 0xD553, 0x11D5, { 0x87, 0xE2, 0x00, 0x06, 0x29, 0x45, 0xC3, 0xb9 } \
+  }
+
+///
+/// The maximum size of an EFI_DEBUG_INFO structure.
+///
+#define EFI_STATUS_CODE_DATA_MAX_SIZE 200
+
+///
+/// This structure contains the ErrorLevel passed into the DEBUG() macro, followed 
+/// by a 96-byte buffer that contains the variable argument list passed to the 
+/// DEBUG() macro that has been converted to a BASE_LIST.  The 96-byte buffer is 
+/// followed by a Null-terminated ASCII string that is the Format string passed 
+/// to the DEBUG() macro.  The maximum size of this structure is defined by 
+/// EFI_STATUS_CODE_DATA_MAX_SIZE.
+///
+typedef struct {
+  ///
+  /// The debug error level passed into a DEBUG() macro.
+  ///
+  UINT32  ErrorLevel;
+} EFI_DEBUG_INFO;
+
+extern EFI_GUID gEfiStatusCodeDataTypeDebugGuid;
+
+#endif // _STATUS_CODE_DATA_TYPE_DEBUG_H_
diff --git a/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeVariable.h b/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeVariable.h
new file mode 100644
index 0000000000..9ea85f8099
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/StatusCodeDataTypeVariable.h
@@ -0,0 +1,40 @@
+/** @file
+  This file defines the GUID and data structure used to pass variable setting 
+  failure information to the Status Code Protocol.
+
+Copyright (c) 2014, 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 that 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.
+
+**/
+
+#ifndef _STATUS_CODE_DATA_TYPE_VARIABLE_H_
+#define _STATUS_CODE_DATA_TYPE_VARIABLE_H_
+
+///
+/// The Global ID used to identify a structure of type EDKII_SET_VARIABLE_STATUS.
+/// The status code value is PcdGet32 (PcdErrorCodeSetVariable).
+///
+#define EDKII_STATUS_CODE_DATA_TYPE_VARIABLE_GUID \
+  { \
+    0xf6ee6dbb, 0xd67f, 0x4ea0, { 0x8b, 0x96, 0x6a, 0x71, 0xb1, 0x9d, 0x84, 0xad } \
+  }
+
+typedef struct {
+  EFI_GUID   Guid;
+  UINTN      NameSize;
+  UINTN      DataSize;
+  EFI_STATUS SetStatus;
+  UINT32     Attributes;
+  // CHAR16  Name[];
+  // UINT8   Data[];
+} EDKII_SET_VARIABLE_STATUS;
+
+extern EFI_GUID gEdkiiStatusCodeDataTypeVariableGuid;
+
+#endif // _STATUS_CODE_DATA_TYPE_VARIABLE_H_
diff --git a/Core/MdeModulePkg/Include/Guid/SystemNvDataGuid.h b/Core/MdeModulePkg/Include/Guid/SystemNvDataGuid.h
new file mode 100644
index 0000000000..67c666259f
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/SystemNvDataGuid.h
@@ -0,0 +1,117 @@
+/** @file  
+  This file defines NvDataFv GUID and FTW working block structures.
+  The NvDataFv GUID can be used as FileSystemGuid in EFI_FIRMWARE_VOLUME_HEADER if 
+  this FV image contains NV data, such as NV variable data.
+  This file also defines WorkingBlockSignature GUID for FTW working block signature.
+
+Copyright (c) 2006 - 2015, 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 that 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.             
+
+**/
+
+#ifndef __SYSTEM_NV_DATA_GUID_H__
+#define __SYSTEM_NV_DATA_GUID_H__
+
+#define EFI_SYSTEM_NV_DATA_FV_GUID \
+  {0xfff12b8d, 0x7696, 0x4c8b, {0xa9, 0x85, 0x27, 0x47, 0x7, 0x5b, 0x4f, 0x50} }
+
+#define EDKII_WORKING_BLOCK_SIGNATURE_GUID \
+  {0x9e58292b, 0x7c68, 0x497d, {0xa0, 0xce, 0x65,  0x0, 0xfd, 0x9f, 0x1b, 0x95} }
+
+extern EFI_GUID gEfiSystemNvDataFvGuid;
+extern EFI_GUID gEdkiiWorkingBlockSignatureGuid;
+
+#define WORKING_BLOCK_VALID   0x1
+#define WORKING_BLOCK_INVALID 0x2
+
+///
+/// The EDKII Fault tolerant working block header.
+/// The header is immediately followed by the write queue data.
+///
+typedef struct {
+  ///
+  /// FTW working block signature.
+  /// Its value has be updated from gEfiSystemNvDataFvGuid to gEdkiiWorkingBlockSignatureGuid,
+  /// because its write queue data format has been updated to support the crossing archs.
+  ///
+  EFI_GUID  Signature;
+  ///
+  /// 32bit CRC calculated for this header.
+  ///
+  UINT32    Crc;
+  ///
+  /// Working block valid bit.
+  ///
+  UINT8     WorkingBlockValid : 1;
+  UINT8     WorkingBlockInvalid : 1;
+  UINT8     Reserved : 6;
+  UINT8     Reserved3[3];
+  ///
+  /// Total size of the following write queue range.
+  ///
+  UINT64    WriteQueueSize;
+  ///
+  /// Write Queue data.
+  ///
+  /// EFI_FAULT_TOLERANT_WRITE_HEADER FtwHeader;
+  /// EFI_FAULT_TOLERANT_WRITE_RECORD FtwRecord[FtwHeader.NumberOfWrites]
+  /// EFI_FAULT_TOLERANT_WRITE_HEADER FtwHeader2;
+  /// EFI_FAULT_TOLERANT_WRITE_RECORD FtwRecord2[FtwHeader2.NumberOfWrites]
+  /// ...
+  ///
+} EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER;
+
+#define FTW_VALID_STATE     0
+#define FTW_INVALID_STATE   1
+
+//
+// EFI Fault tolerant block update write queue entry.
+//
+typedef struct {
+  UINT8     HeaderAllocated : 1;
+  UINT8     WritesAllocated : 1;
+  UINT8     Complete : 1;
+  UINT8     Reserved : 5;
+  EFI_GUID  CallerId;
+  UINT64    NumberOfWrites;
+  UINT64    PrivateDataSize;
+} EFI_FAULT_TOLERANT_WRITE_HEADER;
+
+//
+// EFI Fault tolerant block update write queue record.
+//
+typedef struct {
+  UINT8   BootBlockUpdate : 1;
+  UINT8   SpareComplete : 1;
+  UINT8   DestinationComplete : 1;
+  UINT8   Reserved : 5;
+  EFI_LBA Lba;
+  UINT64  Offset;
+  UINT64  Length;
+  //
+  // Relative offset to spare block.
+  //
+  INT64   RelativeOffset;
+  //
+  // UINT8    PrivateData[PrivateDataSize]
+  //
+} EFI_FAULT_TOLERANT_WRITE_RECORD;
+
+#define FTW_RECORD_SIZE(PrivateDataSize)  (sizeof (EFI_FAULT_TOLERANT_WRITE_RECORD) + (UINTN) PrivateDataSize)
+
+#define FTW_RECORD_TOTAL_SIZE(NumberOfWrites, PrivateDataSize) \
+    ((UINTN) (NumberOfWrites) * (sizeof (EFI_FAULT_TOLERANT_WRITE_RECORD) + (UINTN) PrivateDataSize))
+
+#define FTW_WRITE_TOTAL_SIZE(NumberOfWrites, PrivateDataSize) \
+    ( \
+      sizeof (EFI_FAULT_TOLERANT_WRITE_HEADER) + (UINTN) (NumberOfWrites) * \
+      (sizeof (EFI_FAULT_TOLERANT_WRITE_RECORD) + (UINTN) PrivateDataSize) \
+    )
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/TtyTerm.h b/Core/MdeModulePkg/Include/Guid/TtyTerm.h
new file mode 100644
index 0000000000..900e5af892
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/TtyTerm.h
@@ -0,0 +1,25 @@
+/** @file
+GUID definition for TtyTerm terminal type.  The TtyTerm terminal aims to
+provide support for modern *nix terminals.
+
+
+Copyright (c) 2015  Linaro Ltd.
+This program and the accompanying materials are licensed and made
+available under the terms and conditions of the BSD License that
+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.
+
+**/
+
+#ifndef __TTYTERM_H__
+#define __TTYTERM_H__
+
+#define EFI_TTY_TERM_GUID    \
+    {0x7d916d80, 0x5bb1, 0x458c, {0xa4, 0x8f, 0xe2, 0x5f, 0xdd, 0x51, 0xef, 0x94 } }
+
+extern EFI_GUID gEfiTtyTermGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/UsbKeyBoardLayout.h b/Core/MdeModulePkg/Include/Guid/UsbKeyBoardLayout.h
new file mode 100644
index 0000000000..57d81643c6
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/UsbKeyBoardLayout.h
@@ -0,0 +1,37 @@
+/** @file
+  USB KeyBoard Layout GUIDs
+
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __USB_KEYBOARD_LAYOUT_GUID_H__
+#define __USB_KEYBOARD_LAYOUT_GUID_H__
+
+//
+// GUID for USB keyboard HII package list.
+//
+#define USB_KEYBOARD_LAYOUT_PACKAGE_GUID \
+  { \
+    0xc0f3b43, 0x44de, 0x4907, { 0xb4, 0x78, 0x22, 0x5f, 0x6f, 0x62, 0x89, 0xdc } \
+  }
+
+//
+// GUID for USB keyboard layout
+//
+#define USB_KEYBOARD_LAYOUT_KEY_GUID \
+  { \
+    0x3a4d7a7c, 0x18a, 0x4b42, { 0x81, 0xb3, 0xdc, 0x10, 0xe3, 0xb5, 0x91, 0xbd } \
+  }
+
+extern EFI_GUID  gUsbKeyboardLayoutPackageGuid;
+extern EFI_GUID  gUsbKeyboardLayoutKeyGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/VarErrorFlag.h b/Core/MdeModulePkg/Include/Guid/VarErrorFlag.h
new file mode 100644
index 0000000000..1f05396763
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/VarErrorFlag.h
@@ -0,0 +1,41 @@
+/** @file
+  Variable error flag definitions.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VARIABLE_ERROR_FLAG_H_
+#define _VARIABLE_ERROR_FLAG_H_
+
+//
+// Before EndOfDxe, the variable indicates the last boot variable error flag,
+// then it means the last boot variable error flag must be got before EndOfDxe.
+// After EndOfDxe, the variable indicates the current boot variable error flag,
+// then it means the current boot variable error flag must be got after EndOfDxe.
+//
+// If the variable is not present, it has the same meaning with VAR_ERROR_FLAG_NO_ERROR.
+//
+#define VAR_ERROR_FLAG_NAME             L"VarErrorFlag"
+
+#define VAR_ERROR_FLAG_NO_ERROR         0xFF // 1111-1111
+#define VAR_ERROR_FLAG_SYSTEM_ERROR     0xEF // 1110-1111
+#define VAR_ERROR_FLAG_USER_ERROR       0xFE // 1111-1110
+
+typedef UINT8 VAR_ERROR_FLAG;
+
+#define EDKII_VAR_ERROR_FLAG_GUID { \
+  0x4b37fe8, 0xf6ae, 0x480b, { 0xbd, 0xd5, 0x37, 0xd9, 0x8c, 0x5e, 0x89, 0xaa } \
+};
+
+extern EFI_GUID gEdkiiVarErrorFlagGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Guid/VariableFormat.h b/Core/MdeModulePkg/Include/Guid/VariableFormat.h
new file mode 100644
index 0000000000..5fa75e6ca9
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/VariableFormat.h
@@ -0,0 +1,226 @@
+/** @file
+  The variable data structures are related to EDK II-specific implementation of UEFI variables.
+  VariableFormat.h defines variable data headers and variable storage region headers.
+
+Copyright (c) 2006 - 2015, 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 that 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.
+
+**/
+
+#ifndef __VARIABLE_FORMAT_H__
+#define __VARIABLE_FORMAT_H__
+
+#define EFI_VARIABLE_GUID \
+  { 0xddcf3616, 0x3275, 0x4164, { 0x98, 0xb6, 0xfe, 0x85, 0x70, 0x7f, 0xfe, 0x7d } }
+
+#define EFI_AUTHENTICATED_VARIABLE_GUID \
+  { 0xaaf32c78, 0x947b, 0x439a, { 0xa1, 0x80, 0x2e, 0x14, 0x4e, 0xc3, 0x77, 0x92 } }
+
+extern EFI_GUID gEfiVariableGuid;
+extern EFI_GUID gEfiAuthenticatedVariableGuid;
+
+///
+/// Alignment of variable name and data, according to the architecture:
+/// * For IA-32 and Intel(R) 64 architectures: 1.
+/// * For IA-64 architecture: 8.
+///
+#if defined (MDE_CPU_IPF)
+#define ALIGNMENT         8
+#else
+#define ALIGNMENT         1
+#endif
+
+//
+// GET_PAD_SIZE calculates the miminal pad bytes needed to make the current pad size satisfy the alignment requirement.
+//
+#if (ALIGNMENT == 1)
+#define GET_PAD_SIZE(a) (0)
+#else
+#define GET_PAD_SIZE(a) (((~a) + 1) & (ALIGNMENT - 1))
+#endif
+
+///
+/// Alignment of Variable Data Header in Variable Store region.
+///
+#define HEADER_ALIGNMENT  4
+#define HEADER_ALIGN(Header)  (((UINTN) (Header) + HEADER_ALIGNMENT - 1) & (~(HEADER_ALIGNMENT - 1)))
+
+///
+/// Status of Variable Store Region.
+///
+typedef enum {
+  EfiRaw,
+  EfiValid,
+  EfiInvalid,
+  EfiUnknown
+} VARIABLE_STORE_STATUS;
+
+#pragma pack(1)
+
+#define VARIABLE_STORE_SIGNATURE  EFI_VARIABLE_GUID
+#define AUTHENTICATED_VARIABLE_STORE_SIGNATURE  EFI_AUTHENTICATED_VARIABLE_GUID
+
+///
+/// Variable Store Header Format and State.
+///
+#define VARIABLE_STORE_FORMATTED          0x5a
+#define VARIABLE_STORE_HEALTHY            0xfe
+
+///
+/// Variable Store region header.
+///
+typedef struct {
+  ///
+  /// Variable store region signature.
+  ///
+  EFI_GUID  Signature;
+  ///
+  /// Size of entire variable store,
+  /// including size of variable store header but not including the size of FvHeader.
+  ///
+  UINT32  Size;
+  ///
+  /// Variable region format state.
+  ///
+  UINT8   Format;
+  ///
+  /// Variable region healthy state.
+  ///
+  UINT8   State;
+  UINT16  Reserved;
+  UINT32  Reserved1;
+} VARIABLE_STORE_HEADER;
+
+///
+/// Variable data start flag.
+///
+#define VARIABLE_DATA                     0x55AA
+
+///
+/// Variable State flags.
+///
+#define VAR_IN_DELETED_TRANSITION     0xfe  ///< Variable is in obsolete transition.
+#define VAR_DELETED                   0xfd  ///< Variable is obsolete.
+#define VAR_HEADER_VALID_ONLY         0x7f  ///< Variable header has been valid.
+#define VAR_ADDED                     0x3f  ///< Variable has been completely added.
+
+///
+/// Variable Attribute combinations.
+///
+#define VARIABLE_ATTRIBUTE_NV_BS        (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS)
+#define VARIABLE_ATTRIBUTE_BS_RT        (EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS)
+#define VARIABLE_ATTRIBUTE_AT_AW        (EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS | EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
+#define VARIABLE_ATTRIBUTE_NV_BS_RT     (VARIABLE_ATTRIBUTE_BS_RT | EFI_VARIABLE_NON_VOLATILE)
+#define VARIABLE_ATTRIBUTE_NV_BS_RT_HR  (VARIABLE_ATTRIBUTE_NV_BS_RT | EFI_VARIABLE_HARDWARE_ERROR_RECORD)
+#define VARIABLE_ATTRIBUTE_NV_BS_RT_AT  (VARIABLE_ATTRIBUTE_NV_BS_RT | EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
+#define VARIABLE_ATTRIBUTE_NV_BS_RT_AW  (VARIABLE_ATTRIBUTE_NV_BS_RT | EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
+#define VARIABLE_ATTRIBUTE_NV_BS_RT_HR_AT_AW    (VARIABLE_ATTRIBUTE_NV_BS_RT_HR | VARIABLE_ATTRIBUTE_AT_AW)
+
+///
+/// Single Variable Data Header Structure.
+///
+typedef struct {
+  ///
+  /// Variable Data Start Flag.
+  ///
+  UINT16      StartId;
+  ///
+  /// Variable State defined above.
+  ///
+  UINT8       State;
+  UINT8       Reserved;
+  ///
+  /// Attributes of variable defined in UEFI specification.
+  ///
+  UINT32      Attributes;
+  ///
+  /// Size of variable null-terminated Unicode string name.
+  ///
+  UINT32      NameSize;
+  ///
+  /// Size of the variable data without this header.
+  ///
+  UINT32      DataSize;
+  ///
+  /// A unique identifier for the vendor that produces and consumes this varaible.
+  ///
+  EFI_GUID    VendorGuid;
+} VARIABLE_HEADER;
+
+///
+/// Single Authenticated Variable Data Header Structure.
+///
+typedef struct {
+  ///
+  /// Variable Data Start Flag.
+  ///
+  UINT16      StartId;
+  ///
+  /// Variable State defined above.
+  ///
+  UINT8       State;
+  UINT8       Reserved;
+  ///
+  /// Attributes of variable defined in UEFI specification.
+  ///
+  UINT32      Attributes;
+  ///
+  /// Associated monotonic count value against replay attack.
+  ///
+  UINT64      MonotonicCount;
+  ///
+  /// Associated TimeStamp value against replay attack.
+  ///
+  EFI_TIME    TimeStamp;
+  ///
+  /// Index of associated public key in database.
+  ///
+  UINT32      PubKeyIndex;
+  ///
+  /// Size of variable null-terminated Unicode string name.
+  ///
+  UINT32      NameSize;
+  ///
+  /// Size of the variable data without this header.
+  ///
+  UINT32      DataSize;
+  ///
+  /// A unique identifier for the vendor that produces and consumes this varaible.
+  ///
+  EFI_GUID    VendorGuid;
+} AUTHENTICATED_VARIABLE_HEADER;
+
+typedef struct {
+  EFI_GUID    *Guid;
+  CHAR16      *Name;
+  UINTN       VariableSize;
+} VARIABLE_ENTRY_CONSISTENCY;
+
+#pragma pack()
+
+typedef struct _VARIABLE_INFO_ENTRY  VARIABLE_INFO_ENTRY;
+
+///
+/// This structure contains the variable list that is put in EFI system table.
+/// The variable driver collects all variables that were used at boot service time and produces this list.
+/// This is an optional feature to dump all used variables in shell environment.
+///
+struct _VARIABLE_INFO_ENTRY {
+  VARIABLE_INFO_ENTRY *Next;       ///< Pointer to next entry.
+  EFI_GUID            VendorGuid;  ///< Guid of Variable.
+  CHAR16              *Name;       ///< Name of Variable.
+  UINT32              Attributes;  ///< Attributes of variable defined in UEFI specification.
+  UINT32              ReadCount;   ///< Number of times to read this variable.
+  UINT32              WriteCount;  ///< Number of times to write this variable.
+  UINT32              DeleteCount; ///< Number of times to delete this variable.
+  UINT32              CacheCount;  ///< Number of times that cache hits this variable.
+  BOOLEAN             Volatile;    ///< TRUE if volatile, FALSE if non-volatile.
+};
+
+#endif // _EFI_VARIABLE_H_
diff --git a/Core/MdeModulePkg/Include/Guid/VariableIndexTable.h b/Core/MdeModulePkg/Include/Guid/VariableIndexTable.h
new file mode 100644
index 0000000000..ee0722fd48
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/VariableIndexTable.h
@@ -0,0 +1,47 @@
+/** @file
+  The variable data structures are related to EDK II-specific implementation of UEFI variables.
+  VariableFormat.h defines variable data headers and variable storage region headers.
+
+Copyright (c) 2006 - 2011, 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 that 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.
+
+**/
+
+#ifndef __VARIABLE_INDEX_TABLE_H__
+#define __VARIABLE_INDEX_TABLE_H__
+
+typedef struct {
+  VARIABLE_HEADER *CurrPtr;
+  VARIABLE_HEADER *EndPtr;
+  VARIABLE_HEADER *StartPtr;
+} VARIABLE_POINTER_TRACK;
+
+#define VARIABLE_INDEX_TABLE_VOLUME 122
+
+#define EFI_VARIABLE_INDEX_TABLE_GUID \
+  { 0x8cfdb8c8, 0xd6b2, 0x40f3, { 0x8e, 0x97, 0x02, 0x30, 0x7c, 0xc9, 0x8b, 0x7c } }
+
+extern EFI_GUID gEfiVariableIndexTableGuid;
+
+///
+/// Use this data structure to store variable-related info, which can decrease
+/// the cost of access to NV.
+///
+typedef struct {
+  UINT16          Length;
+  UINT16          GoneThrough;
+  VARIABLE_HEADER *EndPtr;
+  VARIABLE_HEADER *StartPtr;
+  ///
+  /// This field is used to store the distance of two neighbouring VAR_ADDED type variables.
+  /// The meaning of the field is implement-dependent.
+  UINT16          Index[VARIABLE_INDEX_TABLE_VOLUME];
+} VARIABLE_INDEX_TABLE;
+
+#endif // __VARIABLE_INDEX_TABLE_H__
diff --git a/Core/MdeModulePkg/Include/Guid/VlanConfigHii.h b/Core/MdeModulePkg/Include/Guid/VlanConfigHii.h
new file mode 100644
index 0000000000..3848ce733d
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/VlanConfigHii.h
@@ -0,0 +1,25 @@
+/** @file
+  GUIDs used as HII FormSet and HII Package list GUID in VlanConfig driver. 
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __VLAN_CONFIG_HII_GUID_H__
+#define __VLAN_CONFIG_HII_GUID_H__
+
+#define VLAN_CONFIG_FORM_SET_GUID \
+  { \
+    0xd79df6b0, 0xef44, 0x43bd, {0x97, 0x97, 0x43, 0xe9, 0x3b, 0xcf, 0x5f, 0xa8 } \
+  }
+
+extern EFI_GUID gVlanConfigFormSetGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Guid/ZeroGuid.h b/Core/MdeModulePkg/Include/Guid/ZeroGuid.h
new file mode 100644
index 0000000000..7ba8dda205
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Guid/ZeroGuid.h
@@ -0,0 +1,25 @@
+/** @file
+  GUID has all zero values.
+  
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __ZERO_GUID_H__
+#define __ZERO_GUID_H__
+
+#define ZERO_GUID \
+  { \
+    0x0, 0x0, 0x0, {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0} \
+  }
+
+extern EFI_GUID gZeroGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/AuthVariableLib.h b/Core/MdeModulePkg/Include/Library/AuthVariableLib.h
new file mode 100644
index 0000000000..0731b8d748
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/AuthVariableLib.h
@@ -0,0 +1,261 @@
+/** @file
+  Provides services to initialize and process authenticated variables.
+
+Copyright (c) 2015, 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 that 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.
+
+**/
+
+#ifndef _AUTH_VARIABLE_LIB_H_
+#define _AUTH_VARIABLE_LIB_H_
+
+#include <Protocol/VarCheck.h>
+
+///
+/// Size of AuthInfo prior to the data payload.
+///
+#define AUTHINFO_SIZE ((OFFSET_OF (EFI_VARIABLE_AUTHENTICATION, AuthInfo)) + \
+                       (OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData)) + \
+                       sizeof (EFI_CERT_BLOCK_RSA_2048_SHA256))
+
+#define AUTHINFO2_SIZE(VarAuth2) ((OFFSET_OF (EFI_VARIABLE_AUTHENTICATION_2, AuthInfo)) + \
+                                  (UINTN) ((EFI_VARIABLE_AUTHENTICATION_2 *) (VarAuth2))->AuthInfo.Hdr.dwLength)
+
+#define OFFSET_OF_AUTHINFO2_CERT_DATA ((OFFSET_OF (EFI_VARIABLE_AUTHENTICATION_2, AuthInfo)) + \
+                                       (OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData)))
+
+typedef struct {
+  CHAR16        *VariableName;
+  EFI_GUID      *VendorGuid;
+  UINT32        Attributes;
+  UINTN         DataSize;
+  VOID          *Data;
+  UINT32        PubKeyIndex;
+  UINT64        MonotonicCount;
+  EFI_TIME      *TimeStamp;
+} AUTH_VARIABLE_INFO;
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the variable found.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *AUTH_VAR_LIB_FIND_VARIABLE) (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  );
+
+/**
+  Finds next variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds next variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the next variable.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *AUTH_VAR_LIB_FIND_NEXT_VARIABLE) (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  );
+
+/**
+  Update the variable region with Variable information.
+
+  @param[in] AuthVariableInfo       Pointer AUTH_VARIABLE_INFO structure for
+                                    input of the variable.
+
+  @retval EFI_SUCCESS               The update operation is success.
+  @retval EFI_INVALID_PARAMETER     Invalid parameter.
+  @retval EFI_WRITE_PROTECTED       Variable is write-protected.
+  @retval EFI_OUT_OF_RESOURCES      There is not enough resource.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *AUTH_VAR_LIB_UPDATE_VARIABLE) (
+  IN AUTH_VARIABLE_INFO     *AuthVariableInfo
+  );
+
+/**
+  Get scratch buffer.
+
+  @param[in, out] ScratchBufferSize Scratch buffer size. If input size is greater than
+                                    the maximum supported buffer size, this value contains
+                                    the maximum supported buffer size as output.
+  @param[out]     ScratchBuffer     Pointer to scratch buffer address.
+
+  @retval EFI_SUCCESS       Get scratch buffer successfully.
+  @retval EFI_UNSUPPORTED   If input size is greater than the maximum supported buffer size.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *AUTH_VAR_LIB_GET_SCRATCH_BUFFER) (
+  IN OUT UINTN      *ScratchBufferSize,
+  OUT    VOID       **ScratchBuffer
+  );
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param ...                    The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+typedef
+BOOLEAN
+(EFIAPI *AUTH_VAR_LIB_CHECK_REMAINING_SPACE) (
+  IN UINT32                     Attributes,
+  ...
+  );
+
+/**
+  Return TRUE if at OS runtime.
+
+  @retval TRUE If at OS runtime.
+  @retval FALSE If at boot time.
+
+**/
+typedef
+BOOLEAN
+(EFIAPI *AUTH_VAR_LIB_AT_RUNTIME) (
+  VOID
+  );
+
+#define AUTH_VAR_LIB_CONTEXT_IN_STRUCT_VERSION  0x01
+
+typedef struct {
+  UINTN                                 StructVersion;
+  UINTN                                 StructSize;
+  //
+  // Reflect the overhead associated with the saving
+  // of a single EFI authenticated variable with the exception
+  // of the overhead associated with the length
+  // of the string name of the EFI variable.
+  //
+  UINTN                                 MaxAuthVariableSize;
+  AUTH_VAR_LIB_FIND_VARIABLE            FindVariable;
+  AUTH_VAR_LIB_FIND_NEXT_VARIABLE       FindNextVariable;
+  AUTH_VAR_LIB_UPDATE_VARIABLE          UpdateVariable;
+  AUTH_VAR_LIB_GET_SCRATCH_BUFFER       GetScratchBuffer;
+  AUTH_VAR_LIB_CHECK_REMAINING_SPACE    CheckRemainingSpaceForConsistency;
+  AUTH_VAR_LIB_AT_RUNTIME               AtRuntime;
+} AUTH_VAR_LIB_CONTEXT_IN;
+
+#define AUTH_VAR_LIB_CONTEXT_OUT_STRUCT_VERSION 0x01
+
+typedef struct {
+  UINTN                                 StructVersion;
+  UINTN                                 StructSize;
+  //
+  // Caller needs to set variable property for the variables.
+  //
+  VARIABLE_ENTRY_PROPERTY               *AuthVarEntry;
+  UINTN                                 AuthVarEntryCount;
+  //
+  // Caller needs to ConvertPointer() for the pointers.
+  //
+  VOID                                  ***AddressPointer;
+  UINTN                                 AddressPointerCount;
+} AUTH_VAR_LIB_CONTEXT_OUT;
+
+/**
+  Initialization for authenticated varibale services.
+  If this initialization returns error status, other APIs will not work
+  and expect to be not called then.
+
+  @param[in]  AuthVarLibContextIn   Pointer to input auth variable lib context.
+  @param[out] AuthVarLibContextOut  Pointer to output auth variable lib context.
+
+  @retval EFI_SUCCESS               Function successfully executed.
+  @retval EFI_INVALID_PARAMETER     If AuthVarLibContextIn == NULL or AuthVarLibContextOut == NULL.
+  @retval EFI_OUT_OF_RESOURCES      Fail to allocate enough resource.
+  @retval EFI_UNSUPPORTED           Unsupported to process authenticated variable.
+
+**/
+EFI_STATUS
+EFIAPI
+AuthVariableLibInitialize (
+  IN  AUTH_VAR_LIB_CONTEXT_IN   *AuthVarLibContextIn,
+  OUT AUTH_VAR_LIB_CONTEXT_OUT  *AuthVarLibContextOut
+  );
+
+/**
+  Process variable with EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS/EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS set.
+
+  @param[in] VariableName           Name of the variable.
+  @param[in] VendorGuid             Variable vendor GUID.
+  @param[in] Data                   Data pointer.
+  @param[in] DataSize               Size of Data.
+  @param[in] Attributes             Attribute value of the variable.
+
+  @retval EFI_SUCCESS               The firmware has successfully stored the variable and its data as
+                                    defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER     Invalid parameter.
+  @retval EFI_WRITE_PROTECTED       Variable is write-protected.
+  @retval EFI_OUT_OF_RESOURCES      There is not enough resource.
+  @retval EFI_SECURITY_VIOLATION    The variable is with EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS
+                                    or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS
+                                    set, but the AuthInfo does NOT pass the validation
+                                    check carried out by the firmware.
+  @retval EFI_UNSUPPORTED           Unsupported to process authenticated variable.
+
+**/
+EFI_STATUS
+EFIAPI
+AuthVariableLibProcessVariable (
+  IN CHAR16         *VariableName,
+  IN EFI_GUID       *VendorGuid,
+  IN VOID           *Data,
+  IN UINTN          DataSize,
+  IN UINT32         Attributes
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/CapsuleLib.h b/Core/MdeModulePkg/Include/Library/CapsuleLib.h
new file mode 100644
index 0000000000..487cb0f311
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/CapsuleLib.h
@@ -0,0 +1,50 @@
+/** @file
+
+  This library class defines a set of interfaces for how to process capsule image updates.
+
+Copyright (c) 2007 - 2010, 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 that 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.
+
+**/
+
+#ifndef __CAPSULE_LIB_H__
+#define __CAPSULE_LIB_H__
+
+/**
+  The firmware checks whether the capsule image is supported 
+  by the CapsuleGuid in CapsuleHeader or if there is other specific information in 
+  the capsule image.
+  
+  @param  CapsuleHeader    Pointer to the UEFI capsule image to be checked.
+  
+  @retval EFI_SUCESS       Input capsule is supported by firmware.
+  @retval EFI_UNSUPPORTED  Input capsule is not supported by the firmware.
+**/
+EFI_STATUS
+EFIAPI
+SupportCapsuleImage (
+  IN EFI_CAPSULE_HEADER *CapsuleHeader
+  );
+
+/**
+  The firmware-specific implementation processes the capsule image
+  if it recognized the format of this capsule image.
+  
+  @param  CapsuleHeader    Pointer to the UEFI capsule image to be processed. 
+   
+  @retval EFI_SUCESS       Capsule Image processed successfully. 
+  @retval EFI_UNSUPPORTED  Capsule image is not supported by the firmware.
+**/
+EFI_STATUS
+EFIAPI
+ProcessCapsuleImage (
+  IN EFI_CAPSULE_HEADER *CapsuleHeader
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/CpuExceptionHandlerLib.h b/Core/MdeModulePkg/Include/Library/CpuExceptionHandlerLib.h
new file mode 100644
index 0000000000..b3016eee5d
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/CpuExceptionHandlerLib.h
@@ -0,0 +1,96 @@
+/** @file
+  CPU Exception library provides the default CPU interrupt/exception handler.
+  It also provides capability to register user interrupt/exception handler.
+
+  Copyright (c) 2012 - 2013, 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.
+
+**/
+
+#ifndef __CPU_EXCEPTION_HANDLER_LIB_H__
+#define __CPU_EXCEPTION_HANDLER_LIB_H__
+
+#include <Ppi/VectorHandoffInfo.h>
+#include <Protocol/Cpu.h>
+
+/**
+  Initializes all CPU exceptions entries and provides the default exception handlers.
+  
+  Caller should try to get an array of interrupt and/or exception vectors that are in use and need to
+  persist by EFI_VECTOR_HANDOFF_INFO defined in PI 1.3 specification.
+  If caller cannot get reserved vector list or it does not exists, set VectorInfo to NULL. 
+  If VectorInfo is not NULL, the exception vectors will be initialized per vector attribute accordingly.
+
+  @param[in]  VectorInfo    Pointer to reserved vector list.
+  
+  @retval EFI_SUCCESS           CPU Exception Entries have been successfully initialized 
+                                with default exception handlers.
+  @retval EFI_INVALID_PARAMETER VectorInfo includes the invalid content if VectorInfo is not NULL.
+  @retval EFI_UNSUPPORTED       This function is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuExceptionHandlers (
+  IN EFI_VECTOR_HANDOFF_INFO       *VectorInfo OPTIONAL
+  );
+
+/**
+  Initializes all CPU interrupt/exceptions entries and provides the default interrupt/exception handlers.
+  
+  Caller should try to get an array of interrupt and/or exception vectors that are in use and need to
+  persist by EFI_VECTOR_HANDOFF_INFO defined in PI 1.3 specification.
+  If caller cannot get reserved vector list or it does not exists, set VectorInfo to NULL. 
+  If VectorInfo is not NULL, the exception vectors will be initialized per vector attribute accordingly.
+
+  @param[in]  VectorInfo    Pointer to reserved vector list.
+  
+  @retval EFI_SUCCESS           All CPU interrupt/exception entries have been successfully initialized 
+                                with default interrupt/exception handlers.
+  @retval EFI_INVALID_PARAMETER VectorInfo includes the invalid content if VectorInfo is not NULL.
+  @retval EFI_UNSUPPORTED       This function is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuInterruptHandlers (
+  IN EFI_VECTOR_HANDOFF_INFO       *VectorInfo OPTIONAL
+  );
+
+/**
+  Registers a function to be called from the processor interrupt handler.
+
+  This function registers and enables the handler specified by InterruptHandler for a processor 
+  interrupt or exception type specified by InterruptType. If InterruptHandler is NULL, then the 
+  handler for the processor interrupt or exception type specified by InterruptType is uninstalled. 
+  The installed handler is called once for each processor interrupt or exception.
+  NOTE: This function should be invoked after InitializeCpuExceptionHandlers() or
+  InitializeCpuInterruptHandlers() invoked, otherwise EFI_UNSUPPORTED returned.
+
+  @param[in]  InterruptType     Defines which interrupt or exception to hook.
+  @param[in]  InterruptHandler  A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER that is called
+                                when a processor interrupt occurs. If this parameter is NULL, then the handler
+                                will be uninstalled.
+
+  @retval EFI_SUCCESS           The handler for the processor interrupt was successfully installed or uninstalled.
+  @retval EFI_ALREADY_STARTED   InterruptHandler is not NULL, and a handler for InterruptType was
+                                previously installed.
+  @retval EFI_INVALID_PARAMETER InterruptHandler is NULL, and a handler for InterruptType was not
+                                previously installed.
+  @retval EFI_UNSUPPORTED       The interrupt specified by InterruptType is not supported,
+                                or this function is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterCpuInterruptHandler (
+  IN EFI_EXCEPTION_TYPE            InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER     InterruptHandler
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/CustomizedDisplayLib.h b/Core/MdeModulePkg/Include/Library/CustomizedDisplayLib.h
new file mode 100644
index 0000000000..31e1091e88
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/CustomizedDisplayLib.h
@@ -0,0 +1,356 @@
+/** @file
+  This library class defines a set of interfaces to customize Display module
+
+Copyright (c) 2013, 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 that 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.
+
+**/
+
+#ifndef __CUSTOMIZED_DISPLAY_LIB_H__
+#define __CUSTOMIZED_DISPLAY_LIB_H__
+
+#include <Protocol/DisplayProtocol.h>
+
+/**
++------------------------------------------------------------------------------+
+|                                 Setup Page                                   |
++------------------------------------------------------------------------------+
+
+Statement
+Statement
+Statement
+
+
+
+
+
++------------------------------------------------------------------------------+
+|                                F9=Reset to Defaults        F10=Save          |
+| ^"=Move Highlight          <Spacebar> Toggles Checkbox     Esc=Exit          |
++------------------------------------------------------------------------------+
+  StatusBar
+**/
+
+/**
+  This funtion defines Page Frame and Backgroud. 
+  
+  Based on the above layout, it will be responsible for HeaderHeight, FooterHeight, 
+  StatusBarHeight and Backgroud. And, it will reserve Screen for Statement. 
+
+  @param[in]  FormData             Form Data to be shown in Page.
+  @param[out] ScreenForStatement   Screen to be used for Statement. (Prompt, Value and Help)
+  
+  @return Status
+**/
+EFI_STATUS
+EFIAPI
+DisplayPageFrame (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData,
+  OUT EFI_SCREEN_DESCRIPTOR         *ScreenForStatement
+  );
+
+/**
+  Clear Screen to the initial state.
+**/
+VOID
+EFIAPI 
+ClearDisplayPage (
+  VOID
+  );
+
+/**
+  This function updates customized key panel's help information.
+  The library will prepare those Strings for the basic key, ESC, Enter, Up/Down/Left/Right, +/-.
+  and arrange them in Footer panel.
+  
+  @param[in]  FormData       Form Data to be shown in Page. FormData has the highlighted statement. 
+  @param[in]  Statement      The statement current selected.
+  @param[in]  Selected       Whether or not a tag be selected. TRUE means Enter has hit this question.
+**/
+VOID
+EFIAPI
+RefreshKeyHelp (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN  BOOLEAN                      Selected
+  );
+
+/**
+  Update status bar.
+
+  This function updates the status bar on the bottom of menu screen. It just shows StatusBar. 
+  Original logic in this function should be splitted out.
+
+  @param[in]  MessageType            The type of message to be shown. InputError or Configuration Changed. 
+  @param[in]  State                  Show or Clear Message.
+**/
+VOID
+EFIAPI
+UpdateStatusBar (
+  IN  UINTN                  MessageType,
+  IN  BOOLEAN                State
+  );
+
+/**
+  Create popup window. 
+
+  This function draws OEM/Vendor specific pop up windows.
+
+  @param[out]  Key    User Input Key
+  @param       ...    String to be shown in Popup. The variable argument list is terminated by a NULL.
+  
+**/
+VOID
+EFIAPI
+CreateDialog (
+  OUT EFI_INPUT_KEY  *Key,        OPTIONAL
+  ...
+  );
+
+/**
+  Confirm how to handle the changed data. 
+  
+  @return Action BROWSER_ACTION_SUBMIT, BROWSER_ACTION_DISCARD or other values.
+**/
+UINTN
+EFIAPI
+ConfirmDataChange (
+  VOID
+  );
+
+/**
+  OEM specifies whether Setup exits Page by ESC key.
+
+  This function customized the behavior that whether Setup exits Page so that 
+  system able to boot when configuration is not changed.
+
+  @retval  TRUE     Exits FrontPage
+  @retval  FALSE    Don't exit FrontPage.
+**/
+BOOLEAN
+EFIAPI
+FormExitPolicy (
+  VOID
+  );
+
+/**
+  Set Timeout value for a ceratain Form to get user response. 
+  
+  This function allows to set timeout value on a ceratain form if necessary.
+  If timeout is not zero, the form will exit if user has no response in timeout. 
+  
+  @param[in]  FormData   Form Data to be shown in Page
+
+  @return 0     No timeout for this form. 
+  @return > 0   Timeout value in 100 ns units.
+**/
+UINT64
+EFIAPI
+FormExitTimeout (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData
+  );
+
+//
+// Print Functions
+//
+/**
+  Prints a unicode string to the default console, at
+  the supplied cursor position, using L"%s" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  String     String pointer.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintStringAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *String
+  );
+
+
+/**
+  Prints a unicode string with the specified width to the default console, at
+  the supplied cursor position, using L"%s" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  String     String pointer.
+  @param  Width      Width for String to be printed. If the print length of String < Width, 
+                     Space char (L' ') will be used to append String. 
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintStringAtWithWidth (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *String,
+  IN UINTN     Width
+  );
+
+/**
+  Prints a chracter to the default console, at
+  the supplied cursor position, using L"%c" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  Character  Character to print.
+
+  @return Length of string printed to the console.
+
+**/
+UINTN
+EFIAPI
+PrintCharAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  CHAR16       Character
+  );
+
+/**
+  Clear retangle with specified text attribute.
+
+  @param  LeftColumn     Left column of retangle.
+  @param  RightColumn    Right column of retangle.
+  @param  TopRow         Start row of retangle.
+  @param  BottomRow      End row of retangle.
+  @param  TextAttribute  The character foreground and background.
+
+**/
+VOID
+EFIAPI
+ClearLines (
+  IN UINTN               LeftColumn,
+  IN UINTN               RightColumn,
+  IN UINTN               TopRow,
+  IN UINTN               BottomRow,
+  IN UINTN               TextAttribute
+  );
+
+//
+// Color Setting Functions
+//
+/**
+  Get OEM/Vendor specific popup attribute colors.
+
+  @retval  Byte code color setting for popup color.
+**/
+UINT8
+EFIAPI
+GetPopupColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific popup attribute colors.
+
+  @retval  Byte code color setting for popup inverse color.
+**/
+UINT8
+EFIAPI
+GetPopupInverseColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific PickList color attribute.
+
+  @retval  Byte code color setting for pick list color.
+**/
+UINT8
+EFIAPI
+GetPickListColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific arrow color attribute.
+
+  @retval  Byte code color setting for arrow color.
+**/
+UINT8
+EFIAPI
+GetArrowColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific info text color attribute.
+
+  @retval  Byte code color setting for info text color.
+**/
+UINT8
+EFIAPI
+GetInfoTextColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific help text color attribute.
+
+  @retval  Byte code color setting for help text color.
+**/
+UINT8
+EFIAPI
+GetHelpTextColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific grayed out text color attribute.
+
+  @retval  Byte code color setting for grayed out text color.
+**/
+UINT8
+EFIAPI
+GetGrayedTextColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific highlighted text color attribute.
+
+  @retval  Byte code color setting for highlight text color.
+**/
+UINT8
+EFIAPI
+GetHighlightTextColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific field text color attribute.
+
+  @retval  Byte code color setting for field text color.
+**/
+UINT8
+EFIAPI
+GetFieldTextColor (
+  VOID
+  );
+
+/**
+  Get OEM/Vendor specific subtitle text color attribute.
+
+  @retval  Byte code color setting for subtitle text color.
+**/
+UINT8
+EFIAPI
+GetSubTitleTextColor (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/DebugAgentLib.h b/Core/MdeModulePkg/Include/Library/DebugAgentLib.h
new file mode 100644
index 0000000000..01b06a73fb
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/DebugAgentLib.h
@@ -0,0 +1,103 @@
+/** @file
+  Debug Agent Library provide source-level debug capability.
+
+Copyright (c) 2010 - 2012, 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 that 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.
+
+**/
+
+#ifndef __DEBUG_AGENT_LIB_H__
+#define __DEBUG_AGENT_LIB_H__
+
+#define DEBUG_AGENT_INIT_PREMEM_SEC              1
+#define DEBUG_AGENT_INIT_POSTMEM_SEC             2
+#define DEBUG_AGENT_INIT_DXE_CORE                3
+#define DEBUG_AGENT_INIT_SMM                     4
+#define DEBUG_AGENT_INIT_ENTER_SMI               5
+#define DEBUG_AGENT_INIT_EXIT_SMI                6
+#define DEBUG_AGENT_INIT_S3                      7
+#define DEBUG_AGENT_INIT_DXE_AP                  8
+#define DEBUG_AGENT_INIT_PEI                     9
+#define DEBUG_AGENT_INIT_DXE_LOAD               10
+#define DEBUG_AGENT_INIT_DXE_UNLOAD             11
+#define DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64    12
+
+//
+// Context for DEBUG_AGENT_INIT_POSTMEM_SEC
+//
+typedef struct {
+  UINTN          HeapMigrateOffset;
+  UINTN          StackMigrateOffset;
+} DEBUG_AGENT_CONTEXT_POSTMEM_SEC;
+
+/**
+  Caller provided function to be invoked at the end of InitializeDebugAgent().
+
+  Refer to the descrption for InitializeDebugAgent() for more details.
+
+  @param[in] Context    The first input parameter of InitializeDebugAgent().
+
+**/
+typedef
+VOID
+(EFIAPI * DEBUG_AGENT_CONTINUE)(
+  IN VOID        *Context
+  );
+
+
+/**
+  Initialize debug agent.
+
+  This function is used to set up debug environment to support source level debugging.
+  If certain Debug Agent Library instance has to save some private data in the stack,
+  this function must work on the mode that doesn't return to the caller, then
+  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
+  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
+  responsible to invoke the passing-in function at the end of InitializeDebugAgent().
+
+  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by
+  passing in the Context to be its parameter.
+
+  If Function() is NULL, Debug Agent Library instance will return after setup debug
+  environment.
+
+  @param[in] InitFlag     Init flag is used to decide the initialize process.
+  @param[in] Context      Context needed according to InitFlag; it was optional.
+  @param[in] Function     Continue function called by debug agent library; it was
+                          optional.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgent (
+  IN UINT32                InitFlag,
+  IN VOID                  *Context, OPTIONAL
+  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
+  );
+
+/**
+  Enable/Disable the interrupt of debug timer and return the interrupt state
+  prior to the operation.
+
+  If EnableStatus is TRUE, enable the interrupt of debug timer.
+  If EnableStatus is FALSE, disable the interrupt of debug timer.
+
+  @param[in] EnableStatus    Enable/Disable.
+
+  @retval TRUE  Debug timer interrupt were enabled on entry to this call.
+  @retval FALSE Debug timer interrupt were disabled on entry to this call.
+
+**/
+BOOLEAN
+EFIAPI
+SaveAndSetDebugTimerInterrupt (
+  IN BOOLEAN                EnableStatus
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/DpcLib.h b/Core/MdeModulePkg/Include/Library/DpcLib.h
new file mode 100644
index 0000000000..c392e3e3d7
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/DpcLib.h
@@ -0,0 +1,59 @@
+/** @file
+  DpcLib.h.
+  
+Copyright (c) 2007 - 2010, 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 that 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.
+
+**/
+
+#ifndef _DPC_LIB_H_
+#define _DPC_LIB_H_
+
+#include <Protocol/Dpc.h>
+
+/**
+  Add a Deferred Procedure Call to the end of the DPC queue.
+
+  @param[in]  DpcTpl        The EFI_TPL that the DPC should invoke.
+  @param[in]  DpcProcedure  The pointer to the DPC's function.
+  @param[in]  DpcContext    The pointer to the DPC's context.  Passed to DpcProcedure
+                            when DpcProcedure is invoked.
+
+  @retval EFI_SUCCESS            The DPC was queued.
+  @retval EFI_INVALID_PARAMETER  DpcTpl is not a valid EFI_TPL.
+  @retval EFI_INVALID_PARAMETER  DpcProcedure is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to
+                                 add the DPC to the queue.
+
+**/
+EFI_STATUS
+EFIAPI
+QueueDpc (
+  IN EFI_TPL            DpcTpl,
+  IN EFI_DPC_PROCEDURE  DpcProcedure,
+  IN VOID               *DpcContext    OPTIONAL
+  );
+
+/**
+  Dispatch the queue of DPCs. All DPCs that have been queued with a DpcTpl
+  value greater than or equal to the current TPL are invoked in the order that
+  they were queued.  DPCs with higher DpcTpl values are invoked before DPCs with
+  lower DpcTpl values.
+
+  @retval EFI_SUCCESS    One or more DPCs were invoked.
+  @retval EFI_NOT_FOUND  No DPCs were invoked.
+
+**/
+EFI_STATUS
+EFIAPI
+DispatchDpc (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/HiiLib.h b/Core/MdeModulePkg/Include/Library/HiiLib.h
new file mode 100644
index 0000000000..0e323df221
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/HiiLib.h
@@ -0,0 +1,1096 @@
+/** @file
+  Public include file for the HII Library
+
+Copyright (c) 2007 - 2013, 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 that 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.             
+
+**/
+
+#ifndef __HII_LIB_H__
+#define __HII_LIB_H__
+
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
+// HiiLib Functions
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
+
+/**
+  Registers a list of packages in the HII Database and returns the HII Handle
+  associated with that registration.  If an HII Handle has already been registered
+  with the same PackageListGuid and DeviceHandle, then NULL is returned.  If there
+  are not enough resources to perform the registration, then NULL is returned.
+  If an empty list of packages is passed in, then NULL is returned.  If the size of
+  the list of package is 0, then NULL is returned.
+
+  The variable arguments are pointers that point to package headers defined 
+  by UEFI VFR compiler and StringGather tool.
+
+  #pragma pack (push, 1)
+  typedef struct {
+    UINT32                  BinaryLength;
+    EFI_HII_PACKAGE_HEADER  PackageHeader;
+  } EDKII_AUTOGEN_PACKAGES_HEADER;
+  #pragma pack (pop)
+  
+  @param[in]  PackageListGuid  The GUID of the package list.
+  @param[in]  DeviceHandle     If not NULL, the Device Handle on which 
+                               an instance of DEVICE_PATH_PROTOCOL is installed.
+                               This Device Handle uniquely defines the device that 
+                               the added packages are associated with.
+  @param[in]  ...              The variable argument list that contains pointers 
+                               to packages terminated by a NULL.
+
+  @retval NULL   An HII Handle has already been registered in the HII Database with
+                 the same PackageListGuid and DeviceHandle.
+  @retval NULL   The HII Handle could not be created.
+  @retval NULL   An empty list of packages was passed in.
+  @retval NULL   All packages are empty.
+  @retval Other  The HII Handle associated with the newly registered package list.
+
+**/
+EFI_HII_HANDLE
+EFIAPI
+HiiAddPackages (
+  IN CONST EFI_GUID    *PackageListGuid,
+  IN       EFI_HANDLE  DeviceHandle  OPTIONAL,
+  ...
+  )
+;
+
+/**
+  Removes a package list from the HII database.
+
+  If HiiHandle is NULL, then ASSERT().
+  If HiiHandle is not a valid EFI_HII_HANDLE in the HII database, then ASSERT().
+
+  @param[in]  HiiHandle   The handle that was previously registered in the HII database
+
+**/
+VOID
+EFIAPI
+HiiRemovePackages (
+  IN      EFI_HII_HANDLE      HiiHandle
+  )
+;
+
+/**
+  This function creates a new string in String Package or updates an existing 
+  string in a String Package.  If StringId is 0, then a new string is added to
+  a String Package.  If StringId is not zero, then a string in String Package is
+  updated.  If SupportedLanguages is NULL, then the string is added or updated
+  for all the languages that the String Package supports.  If SupportedLanguages
+  is not NULL, then the string is added or updated for the set of languages 
+  specified by SupportedLanguages.
+    
+  If HiiHandle is NULL, then ASSERT().
+  If String is NULL, then ASSERT().
+
+  @param[in]  HiiHandle           A handle that was previously registered in the 
+                                  HII Database.
+  @param[in]  StringId            If zero, then a new string is created in the 
+                                  String Package associated with HiiHandle.  If 
+                                  non-zero, then the string specified by StringId 
+                                  is updated in the String Package associated 
+                                  with HiiHandle. 
+  @param[in]  String              A pointer to the Null-terminated Unicode string 
+                                  to add or update in the String Package associated 
+                                  with HiiHandle.
+  @param[in]  SupportedLanguages  A pointer to a Null-terminated ASCII string of 
+                                  language codes.  If this parameter is NULL, then 
+                                  String is added or updated in the String Package 
+                                  associated with HiiHandle for all the languages 
+                                  that the String Package supports.  If this 
+                                  parameter is not NULL, then String is added 
+                                  or updated in the String Package associated with 
+                                  HiiHandle for the set of languages specified by 
+                                  SupportedLanguages.  The format of 
+                                  SupportedLanguages must follow the language 
+                                  format assumed in the HII Database.
+
+  @retval 0      The string could not be added or updated in the String Package.
+  @retval Other  The EFI_STRING_ID of the newly added or updated string.
+
+**/
+EFI_STRING_ID
+EFIAPI
+HiiSetString (
+  IN EFI_HII_HANDLE    HiiHandle,
+  IN EFI_STRING_ID     StringId,            OPTIONAL
+  IN CONST EFI_STRING  String,
+  IN CONST CHAR8       *SupportedLanguages  OPTIONAL
+  )
+;
+
+/**
+  Retrieves a string from a string package in a specific language.  If the language
+  is not specified, then a string from a string package in the current platform 
+  language is retrieved.  If the string cannot be retrieved using the specified 
+  language or the current platform language, then the string is retrieved from 
+  the string package in the first language the string package supports.  The 
+  returned string is allocated using AllocatePool().  The caller is responsible 
+  for freeing the allocated buffer using FreePool().
+  
+  If HiiHandle is NULL, then ASSERT().
+  If StringId is 0, then ASSERT().
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+  @param[in]  StringId   The identifier of the string to retrieved from the string 
+                         package associated with HiiHandle.
+  @param[in]  Language   The language of the string to retrieve.  If this parameter 
+                         is NULL, then the current platform language is used.  The 
+                         format of Language must follow the language format assumed in
+                         the HII Database.
+
+  @retval NULL   The string specified by StringId is not present in the string package.
+  @retval Other  The string was returned.
+
+**/
+EFI_STRING
+EFIAPI
+HiiGetString (
+  IN EFI_HII_HANDLE  HiiHandle,
+  IN EFI_STRING_ID   StringId,
+  IN CONST CHAR8     *Language  OPTIONAL
+  )
+;
+
+/**
+  Retrieves a string from a string package named by GUID, in the specified language.  
+  If the language is not specified, then a string from a string package in the 
+  current platform  language is retrieved.  If the string cannot be retrieved 
+  using the specified language or the current platform language, then the string 
+  is retrieved from the string package in the first language the string package 
+  supports.  The returned string is allocated using AllocatePool().  The caller 
+  is responsible for freeing the allocated buffer using FreePool().
+  
+  If PackageListGuid is NULL, then ASSERT().
+  If StringId is 0, then ASSERT().
+
+  @param[in]  PackageListGuid  The GUID of a package list that was previously 
+                               registered in the HII Database.
+  @param[in]  StringId         The identifier of the string to retrieved from the 
+                               string package associated with PackageListGuid.
+  @param[in]  Language         The language of the string to retrieve.  If this 
+                               parameter is NULL, then the current platform 
+                               language is used.  The format of Language must 
+                               follow the language format assumed in the HII Database.
+
+  @retval NULL   The package list specified by PackageListGuid is not present in the
+                 HII Database.
+  @retval NULL   The string specified by StringId is not present in the string package.
+  @retval Other  The string was returned.
+
+**/
+EFI_STRING
+EFIAPI
+HiiGetPackageString (
+  IN CONST EFI_GUID  *PackageListGuid,
+  IN EFI_STRING_ID   StringId,
+  IN CONST CHAR8     *Language  OPTIONAL
+  )
+;
+
+/**
+  Retrieves the array of all the HII Handles or the HII handles of a specific
+  package list GUID in the HII Database.
+  This array is terminated with a NULL HII Handle.
+  This function allocates the returned array using AllocatePool().
+  The caller is responsible for freeing the array with FreePool().
+
+  @param[in]  PackageListGuid  An optional parameter that is used to request 
+                               HII Handles associated with a specific
+                               Package List GUID.  If this parameter is NULL,
+                               then all the HII Handles in the HII Database
+                               are returned.  If this parameter is not NULL,
+                               then zero or more HII Handles associated with 
+                               PackageListGuid are returned.
+
+  @retval NULL   No HII handles were found in the HII database
+  @retval NULL   The array of HII Handles could not be retrieved
+  @retval Other  A pointer to the NULL terminated array of HII Handles
+
+**/
+EFI_HII_HANDLE *
+EFIAPI
+HiiGetHiiHandles (
+  IN CONST EFI_GUID  *PackageListGuid  OPTIONAL
+  )
+;
+
+/**
+  Retrieves a pointer to a Null-terminated ASCII string containing the list 
+  of languages that an HII handle in the HII Database supports.  The returned 
+  string is allocated using AllocatePool().  The caller is responsible for freeing
+  the returned string using FreePool().  The format of the returned string follows
+  the language format assumed in the HII Database.
+  
+  If HiiHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+
+  @retval NULL   HiiHandle is not registered in the HII database
+  @retval NULL   There are not enough resources available to retrieve the suported 
+                 languages.
+  @retval NULL   The list of suported languages could not be retrieved.
+  @retval Other  A pointer to the Null-terminated ASCII string of supported languages.
+
+**/
+CHAR8 *
+EFIAPI
+HiiGetSupportedLanguages (
+  IN EFI_HII_HANDLE           HiiHandle
+  )
+;
+
+/**
+  Allocates and returns a Null-terminated Unicode <ConfigHdr> string using routing 
+  information that includes a GUID, an optional Unicode string name, and a device
+  path. The string returned is allocated with AllocatePool().  The caller is 
+  responsible for freeing the allocated string with FreePool().
+  
+  The format of a <ConfigHdr> is as follows:
+
+    GUID=<HexCh>32&NAME=<Char>NameLength&PATH=<HexChar>DevicePathSize<Null>
+
+  @param[in]  Guid          The pointer to an EFI_GUID that is the routing information
+                            GUID.  Each of the 16 bytes in Guid is converted to 
+                            a 2 Unicode character hexidecimal string.  This is 
+                            an optional parameter that may be NULL.
+  @param[in]  Name          The pointer to a Null-terminated Unicode string that is 
+                            the routing information NAME.  This is an optional 
+                            parameter that may be NULL.  Each 16-bit Unicode 
+                            character in Name is converted to a 4 character Unicode 
+                            hexidecimal string.                        
+  @param[in]  DriverHandle  The driver handle that supports a Device Path Protocol
+                            that is the routing information PATH.  Each byte of
+                            the Device Path associated with DriverHandle is converted
+                            to a two (Unicode) character hexidecimal string.
+
+  @retval NULL   DriverHandle does not support the Device Path Protocol.
+  @retval NULL   DriverHandle does not support the Device Path Protocol.
+  @retval Other  A pointer to the Null-terminate Unicode <ConfigHdr> string
+
+**/
+EFI_STRING
+EFIAPI
+HiiConstructConfigHdr (
+  IN CONST EFI_GUID  *Guid,  OPTIONAL
+  IN CONST CHAR16    *Name,  OPTIONAL
+  IN EFI_HANDLE      DriverHandle
+  );
+
+/**
+  Reset the default value specified by DefaultId to the driver
+  configuration specified by the Request string. 
+
+  NULL request string support depends on the ExportConfig interface of
+  HiiConfigRouting protocol in UEFI specification.
+  
+  @param Request    A null-terminated Unicode string in 
+                    <MultiConfigRequest> format. It can be NULL.
+                    If it is NULL, all configurations for the
+                    entirety of the current HII database will be reset.
+  @param DefaultId  Specifies the type of defaults to retrieve.
+  
+  @retval TURE    The default value was set successfully.
+  @retval FALSE   The default value was not found.
+**/
+BOOLEAN
+EFIAPI                               
+HiiSetToDefaults (     
+  IN CONST EFI_STRING  Request,  OPTIONAL
+  IN UINT16            DefaultId
+  );
+
+/**
+  Validate the current configuration by parsing the IFR opcode in HII form.
+
+  NULL request string support depends on the ExportConfig interface of
+  HiiConfigRouting protocol in the UEFI specification.
+  
+  @param  Request   A null-terminated Unicode string in 
+                    <MultiConfigRequest> format. It can be NULL.
+                    If it is NULL, all current configurations for the
+                    entirety of the current HII database will be validated.
+  
+  @retval TRUE    The current configuration is valid.
+  @retval FALSE   The current configuration is invalid.
+**/
+BOOLEAN
+EFIAPI                               
+HiiValidateSettings (
+  IN CONST EFI_STRING  Request  OPTIONAL
+  );
+
+/**
+  Determines if the routing data specified by GUID and NAME match a <ConfigHdr>.
+
+  If ConfigHdr is NULL, then ASSERT().
+
+  @param[in] ConfigHdr  Either <ConfigRequest> or <ConfigResp>.
+  @param[in] Guid       The GUID of the storage.
+  @param[in] Name       The NAME of the storage.
+
+  @retval TRUE   Routing information matches <ConfigHdr>.
+  @retval FALSE  Routing information does not match <ConfigHdr>.
+
+**/
+BOOLEAN
+EFIAPI
+HiiIsConfigHdrMatch (
+  IN CONST EFI_STRING  ConfigHdr,
+  IN CONST EFI_GUID    *Guid,     OPTIONAL
+  IN CONST CHAR16      *Name      OPTIONAL
+  );
+
+/**
+  Retrieves uncommitted data from the Form Browser and converts it to a binary
+  buffer.
+
+  @param[in]  VariableGuid  The pointer to an EFI_GUID structure.  This is an optional 
+                            parameter that may be NULL.
+  @param[in]  VariableName  The pointer to a Null-terminated Unicode string.  This 
+                            is an optional parameter that may be NULL.
+  @param[in]  BufferSize    The length in bytes of buffer to hold retrieved data. 
+  @param[out] Buffer        The buffer of data to be updated.
+
+  @retval FALSE  The uncommitted data could not be retrieved.
+  @retval TRUE   The uncommitted data was retrieved.
+
+**/
+BOOLEAN
+EFIAPI
+HiiGetBrowserData (
+  IN CONST EFI_GUID  *VariableGuid,  OPTIONAL
+  IN CONST CHAR16    *VariableName,  OPTIONAL
+  IN UINTN           BufferSize,
+  OUT UINT8          *Buffer
+  );
+
+/**
+  Updates uncommitted data in the Form Browser.
+
+  If Buffer is NULL, then ASSERT().
+
+  @param[in]  VariableGuid    The pointer to an EFI_GUID structure.  This is an optional
+                              parameter that may be NULL.
+  @param[in]  VariableName    The pointer to a Null-terminated Unicode string.  This
+                              is an optional parameter that may be NULL.
+  @param[in]  BufferSize      The length, in bytes, of Buffer.
+  @param[in]  Buffer          The buffer of data to commit.
+  @param[in]  RequestElement  An optional field to specify which part of the
+                              buffer data will be send back to Browser. If NULL,
+                              the whole buffer of data will be committed to
+                              Browser. 
+                              <RequestElement> ::= &OFFSET=<Number>&WIDTH=<Number>*
+
+  @retval FALSE  The uncommitted data could not be updated.
+  @retval TRUE   The uncommitted data was updated.
+
+**/
+BOOLEAN
+EFIAPI
+HiiSetBrowserData (
+  IN CONST EFI_GUID  *VariableGuid, OPTIONAL
+  IN CONST CHAR16    *VariableName, OPTIONAL
+  IN UINTN           BufferSize,
+  IN CONST UINT8     *Buffer,
+  IN CONST CHAR16    *RequestElement  OPTIONAL
+  );
+
+/////////////////////////////////////////
+/////////////////////////////////////////
+/// IFR Functions
+/////////////////////////////////////////
+/////////////////////////////////////////
+
+/**
+  Returns a UINT64 value that contains bitfields for Hour, Minute, and Second.
+  The lower 8-bits of Hour are placed in bits 0..7.  The lower 8-bits of Minute 
+  are placed in bits 8..15, and the lower 8-bits of Second are placed in bits 
+  16..23.  This format was selected because it can be easily translated to 
+  an EFI_HII_TIME structure in an EFI_IFR_TYPE_VALUE union.
+
+  @param  Hour    The hour value to be encoded.
+  @param  Minute  The minute value to be encoded.
+  @param  Second  The second value to be encoded.
+
+  @return A 64-bit containing Hour, Minute, and Second.
+**/
+#define EFI_HII_TIME_UINT64(Hour, Minute, Second) \
+  (UINT64)((Hour & 0xff) | ((Minute & 0xff) << 8) | ((Second & 0xff) << 16))
+
+/**
+  Returns a UINT64 value that contains bit fields for Year, Month, and Day.
+  The lower 16-bits of Year are placed in bits 0..15.  The lower 8-bits of Month 
+  are placed in bits 16..23, and the lower 8-bits of Day are placed in bits 
+  24..31.  This format was selected because it can be easily translated to 
+  an EFI_HII_DATE structure in an EFI_IFR_TYPE_VALUE union.
+
+  @param  Year   The year value to be encoded.
+  @param  Month  The month value to be encoded.
+  @param  Day    The day value to be encoded.
+
+  @return A 64-bit containing Year, Month, and Day.
+**/
+#define EFI_HII_DATE_UINT64(Year, Month, Day) \
+  (UINT64)((Year & 0xffff) | ((Month & 0xff) << 16) | ((Day & 0xff) << 24))
+
+/**
+  Allocates and returns a new OpCode Handle.  OpCode Handles must be freed with 
+  HiiFreeOpCodeHandle().
+
+  @retval NULL   There are not enough resources to allocate a new OpCode Handle.
+  @retval Other  A new OpCode handle.
+
+**/
+VOID *
+EFIAPI
+HiiAllocateOpCodeHandle (
+  VOID
+  );
+
+/**
+  Frees an OpCode Handle that was previously allocated with HiiAllocateOpCodeHandle().
+  When an OpCode Handle is freed, all of the opcodes associated with the OpCode
+  Handle are also freed.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle   The handle to the buffer of opcodes.
+
+**/
+VOID
+EFIAPI
+HiiFreeOpCodeHandle (
+  VOID  *OpCodeHandle
+  );
+
+/**
+  Append raw opcodes to an OpCodeHandle.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If RawBuffer is NULL, then ASSERT();
+
+  @param[in]  OpCodeHandle   The handle to the buffer of opcodes.
+  @param[in]  RawBuffer      The buffer of opcodes to append.
+  @param[in]  RawBufferSize  The size, in bytes, of Buffer.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the appended opcodes.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateRawOpCodes (
+  IN VOID   *OpCodeHandle,
+  IN UINT8  *RawBuffer,
+  IN UINTN  RawBufferSize
+  );
+
+/**
+  Create EFI_IFR_END_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateEndOpCode (
+  IN VOID  *OpCodeHandle
+  );
+
+/**
+  Create EFI_IFR_ONE_OF_OPTION_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Type is invalid, then ASSERT().
+  If Flags is invalid, then ASSERT().
+
+  @param[in]  OpCodeHandle  The handle to the buffer of opcodes.
+  @param[in]  StringId      StringId for the option.
+  @param[in]  Flags         The flags for the option.
+  @param[in]  Type          The type for the option.
+  @param[in]  Value         The value for the option.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOneOfOptionOpCode (
+  IN VOID    *OpCodeHandle,
+  IN UINT16  StringId,
+  IN UINT8   Flags,
+  IN UINT8   Type,
+  IN UINT64  Value
+  );
+
+/**
+  Create EFI_IFR_DEFAULT_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Type is invalid, then ASSERT().
+
+  @param[in]  OpCodeHandle  The handle to the buffer of opcodes.
+  @param[in]  DefaultId     The DefaultId for the default.
+  @param[in]  Type          The type for the default.
+  @param[in]  Value         The value for the default.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateDefaultOpCode (
+  IN VOID    *OpCodeHandle,
+  IN UINT16  DefaultId,
+  IN UINT8   Type,
+  IN UINT64  Value
+  );
+
+/**
+  Create EFI_IFR_GUID opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Guid is NULL, then ASSERT().
+  If OpCodeSize < sizeof (EFI_IFR_GUID), then ASSERT().
+
+  @param[in]  OpCodeHandle  The handle to the buffer of opcodes.
+  @param[in]  Guid          The pointer to EFI_GUID of this guided opcode.
+  @param[in]  GuidOpCode    The pointer to an EFI_IFR_GUID opcode.  This is an 
+                            optional parameter that may be NULL.  If this
+                            parameter is NULL, then the GUID extension 
+                            region of the created opcode is filled with zeros.
+                            If this parameter is not NULL, then the GUID 
+                            extension region of GuidData will be copied to 
+                            the GUID extension region of the created opcode.
+  @param[in]  OpCodeSize    The size, in bytes, of created opcode.  This value 
+                            must be >= sizeof(EFI_IFR_GUID).
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGuidOpCode (
+  IN VOID            *OpCodeHandle,
+  IN CONST EFI_GUID  *Guid,
+  IN CONST VOID      *GuidOpCode,    OPTIONAL
+  IN UINTN           OpCodeSize
+  );
+
+/**
+  Create EFI_IFR_ACTION_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle  The handle to the buffer of opcodes.
+  @param[in]  QuestionId      The Question ID.
+  @param[in]  Prompt          The String ID for Prompt.
+  @param[in]  Help            The String ID for Help.
+  @param[in]  QuestionFlags   The flags in the Question Header.
+  @param[in]  QuestionConfig  The String ID for the configuration.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateActionOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_STRING_ID    QuestionConfig
+  );
+
+/**
+  Create EFI_IFR_SUBTITLE_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in Flags, then ASSERT().
+  If Scope > 1, then ASSERT().
+
+  @param[in]  OpCodeHandle  The handle to the buffer of opcodes.
+  @param[in]  Prompt      The string ID for Prompt.
+  @param[in]  Help        The string ID for Help.
+  @param[in]  Flags       The subtitle opcode flags.
+  @param[in]  Scope       1 if this opcode is the beginning of a new scope.
+                          0 if this opcode is within the current scope.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateSubTitleOpCode (
+  IN VOID           *OpCodeHandle,
+  IN EFI_STRING_ID  Prompt,
+  IN EFI_STRING_ID  Help,
+  IN UINT8          Flags,
+  IN UINT8          Scope
+  );
+
+/**
+  Create EFI_IFR_REF_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle   The handle to the buffer of opcodes.
+  @param[in]  FormId         The Destination Form ID.
+  @param[in]  Prompt         The string ID for Prompt.
+  @param[in]  Help           The string ID for Help.
+  @param[in]  QuestionFlags  The flags in Question Header
+  @param[in]  QuestionId     Question ID.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGotoOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_FORM_ID      FormId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_QUESTION_ID  QuestionId
+  );
+
+/**
+  Create EFI_IFR_REF_OP, EFI_IFR_REF2_OP, EFI_IFR_REF3_OP and EFI_IFR_REF4_OP opcode.
+
+  When RefDevicePath is not zero, EFI_IFR_REF4 opcode will be created. 
+  When RefDevicePath is zero and RefFormSetId is not NULL, EFI_IFR_REF3 opcode will be created.
+  When RefDevicePath is zero, RefFormSetId is NULL and RefQuestionId is not zero, EFI_IFR_REF2 opcode will be created.
+  When RefDevicePath is zero, RefFormSetId is NULL and RefQuestionId is zero, EFI_IFR_REF opcode will be created.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle   The handle to the buffer of opcodes.
+  @param[in]  RefFormId      The Destination Form ID.
+  @param[in]  Prompt         The string ID for Prompt.
+  @param[in]  Help           The string ID for Help.
+  @param[in]  QuestionFlags  The flags in Question Header
+  @param[in]  QuestionId     Question ID.
+  @param[in]  RefQuestionId  The question on the form to which this link is referring. 
+                             If its value is zero, then the link refers to the top of the form.
+  @param[in]  RefFormSetId   The form set to which this link is referring. If its value is NULL, and RefDevicePath is 
+                             zero, then the link is to the current form set.
+  @param[in]  RefDevicePath  The string identifier that specifies the string containing the text representation of 
+                             the device path to which the form set containing the form specified by FormId.
+                             If its value is zero, then the link refers to the current page.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGotoExOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_FORM_ID      RefFormId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_QUESTION_ID  RefQuestionId,
+  IN EFI_GUID         *RefFormSetId,    OPTIONAL
+  IN EFI_STRING_ID    RefDevicePath
+  );
+
+/**
+  Create EFI_IFR_CHECKBOX_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in CheckBoxFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          The handle to the buffer of opcodes.
+  @param[in]  QuestionId            The question ID.
+  @param[in]  VarStoreId            The storage ID.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                The string ID for Prompt.
+  @param[in]  Help                  The string ID for Help.
+  @param[in]  QuestionFlags         The flags in Question Header.
+  @param[in]  CheckBoxFlags         The flags for checkbox opcode.
+  @param[in]  DefaultsOpCodeHandle  The handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateCheckBoxOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            CheckBoxFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_NUMERIC_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in NumericFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          The handle to the buffer of opcodes.
+  @param[in]  QuestionId            The question ID.
+  @param[in]  VarStoreId            The storage ID.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                The string ID for Prompt.
+  @param[in]  Help                  The string ID for Help.
+  @param[in]  QuestionFlags         The flags in Question Header.
+  @param[in]  NumericFlags          The flags for a numeric opcode.
+  @param[in]  Minimum               The numeric minimum value.
+  @param[in]  Maximum               The numeric maximum value.
+  @param[in]  Step                  The numeric step for edit.
+  @param[in]  DefaultsOpCodeHandle  The handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateNumericOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            NumericFlags,
+  IN UINT64           Minimum,
+  IN UINT64           Maximum,
+  IN UINT64           Step,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_STRING_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in StringFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          The handle to the buffer of opcodes.
+  @param[in]  QuestionId            The question ID.
+  @param[in]  VarStoreId            The storage ID.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                The string ID for Prompt.
+  @param[in]  Help                  The string ID for Help.
+  @param[in]  QuestionFlags         The flags in Question Header.
+  @param[in]  StringFlags           The flags for a string opcode.
+  @param[in]  MinSize               The string minimum length.
+  @param[in]  MaxSize               The string maximum length.
+  @param[in]  DefaultsOpCodeHandle  The handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateStringOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            StringFlags,
+  IN UINT8            MinSize,
+  IN UINT8            MaxSize,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_ONE_OF_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in OneOfFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          The handle to the buffer of opcodes.
+  @param[in]  QuestionId            The question ID.
+  @param[in]  VarStoreId            The storage ID.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                The string ID for Prompt.
+  @param[in]  Help                  The string ID for Help.
+  @param[in]  QuestionFlags         The flags in Question Header.
+  @param[in]  OneOfFlags            The flags for a oneof opcode.
+  @param[in]  OptionsOpCodeHandle   The handle for a buffer of ONE_OF_OPTION opcodes.
+  @param[in]  DefaultsOpCodeHandle  The handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOneOfOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            OneOfFlags,
+  IN VOID             *OptionsOpCodeHandle,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_ORDERED_LIST_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in OrderedListFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          The handle to the buffer of opcodes.
+  @param[in]  QuestionId            The question ID.
+  @param[in]  VarStoreId            The storage ID.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                The string ID for Prompt.
+  @param[in]  Help                  The string ID for Help.
+  @param[in]  QuestionFlags         The flags in Question Header.
+  @param[in]  OrderedListFlags      The flags for an ordered list opcode.
+  @param[in]  DataType              The type for option value.
+  @param[in]  MaxContainers         Maximum count for options in this ordered list
+  @param[in]  OptionsOpCodeHandle   The handle for a buffer of ONE_OF_OPTION opcodes.
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOrderedListOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            OrderedListFlags,
+  IN UINT8            DataType,
+  IN UINT8            MaxContainers,
+  IN VOID             *OptionsOpCodeHandle,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_TEXT_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  Prompt        String ID for Prompt.
+  @param[in]  Help          String ID for Help.
+  @param[in]  TextTwo       String ID for TextTwo.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateTextOpCode (
+  IN VOID           *OpCodeHandle,
+  IN EFI_STRING_ID  Prompt,
+  IN EFI_STRING_ID  Help,
+  IN EFI_STRING_ID  TextTwo
+  );
+
+/**
+  Create EFI_IFR_DATE_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in DateFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID, optional. If DateFlags is not
+                                    QF_DATE_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair, optional. If DateFlags is not
+                                    QF_DATE_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  DateFlags             Flags for date opcode
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateDateOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,   OPTIONAL
+  IN UINT16           VarOffset,    OPTIONAL
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            DateFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  Create EFI_IFR_TIME_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in TimeFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID, optional. If TimeFlags is not
+                                    QF_TIME_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair, optional. If TimeFlags is not
+                                    QF_TIME_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  TimeFlags             Flags for time opcode
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateTimeOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,   OPTIONAL
+  IN UINT16           VarOffset,    OPTIONAL
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            TimeFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  );
+
+/**
+  This function updates a form that has previously been registered with the HII 
+  Database.  This function will perform at most one update operation.
+    
+  The form to update is specified by Handle, FormSetGuid, and FormId.  Binary 
+  comparisons of IFR opcodes are performed from the beginning of the form being 
+  updated until an IFR opcode is found that exactly matches the first IFR opcode 
+  specified by StartOpCodeHandle.  The following rules are used to determine if
+  an insert, replace, or delete operation is performed:
+  
+  1) If no matches are found, then NULL is returned.  
+  2) If a match is found, and EndOpCodeHandle is NULL, then all of the IFR opcodes
+     from StartOpCodeHandle except the first opcode are inserted immediately after 
+     the matching IFR opcode in the form to be updated.
+  3) If a match is found, and EndOpCodeHandle is not NULL, then a search is made 
+     from the matching IFR opcode until an IFR opcode exactly matches the first 
+     IFR opcode specified by EndOpCodeHandle.  If no match is found for the first
+     IFR opcode specified by EndOpCodeHandle, then NULL is returned.  If a match
+     is found, then all of the IFR opcodes between the start match and the end 
+     match are deleted from the form being updated and all of the IFR opcodes
+     from StartOpCodeHandle except the first opcode are inserted immediately after 
+     the matching start IFR opcode.  If StartOpCcodeHandle only contains one
+     IFR instruction, then the result of this operation will delete all of the IFR
+     opcodes between the start end matches.
+
+  If HiiHandle is NULL, then ASSERT().
+  If StartOpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle          The HII Handle of the form to update.
+  @param[in]  FormSetGuid        The Formset GUID of the form to update.  This
+                                 is an optional parameter that may be NULL.
+                                 If it is NULL, all FormSet will be updated.
+  @param[in]  FormId             The ID of the form to update.
+  @param[in]  StartOpCodeHandle  An OpCode Handle that contains the set of IFR 
+                                 opcodes to be inserted or replaced in the form.
+                                 The first IFR instruction in StartOpCodeHandle 
+                                 is used to find matching IFR opcode in the 
+                                 form. 
+  @param[in]  EndOpCodeHandle    An OpCcode Handle that contains the IFR opcode
+                                 that marks the end of a replace operation in
+                                 the form.  This is an optional parameter that
+                                 may be NULL.  If it is NULL, then the IFR
+                                 opcodes specified by StartOpCodeHandle are 
+                                 inserted into the form.
+  
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory resources are allocated.
+  @retval EFI_NOT_FOUND          The following cases will return EFI_NOT_FOUND:
+                                 1) The form specified by HiiHandle, FormSetGuid, 
+                                 and FormId could not be found in the HII Database.
+                                 2) No IFR opcodes in the target form match the first
+                                 IFR opcode in StartOpCodeHandle.
+                                 3) EndOpCOde is not NULL, and no IFR opcodes in the 
+                                 target form following a matching start opcode match 
+                                 the first IFR opcode in EndOpCodeHandle.
+  @retval EFI_SUCCESS            The matched form is updated by StartOpcode.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUpdateForm (
+  IN EFI_HII_HANDLE  HiiHandle,
+  IN EFI_GUID        *FormSetGuid,        OPTIONAL
+  IN EFI_FORM_ID     FormId,
+  IN VOID            *StartOpCodeHandle,
+  IN VOID            *EndOpCodeHandle     OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/HttpLib.h b/Core/MdeModulePkg/Include/Library/HttpLib.h
new file mode 100644
index 0000000000..ce5a839c9b
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/HttpLib.h
@@ -0,0 +1,323 @@
+/** @file
+  This library is used to share code between UEFI network stack modules.
+  It provides the helper routines to parse the HTTP message byte stream.
+
+Copyright (c) 2015, 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<BR>
+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.
+
+**/
+
+#ifndef _HTTP_LIB_H_
+#define _HTTP_LIB_H_
+
+#include <Protocol/Http.h>
+
+/**
+  Decode a percent-encoded URI component to the ASCII character.
+  
+  Decode the input component in Buffer according to RFC 3986. The caller is responsible to make 
+  sure ResultBuffer points to a buffer with size equal or greater than ((AsciiStrSize (Buffer))
+  in bytes. 
+
+  @param[in]    Buffer           The pointer to a percent-encoded URI component.
+  @param[in]    BufferLength     Length of Buffer in bytes.
+  @param[out]   ResultBuffer     Point to the buffer to store the decode result.
+  @param[out]   ResultLength     Length of decoded string in ResultBuffer in bytes.
+
+  @retval EFI_SUCCESS            Successfully decoded the URI.
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid percent-encoded string.
+  
+**/
+EFI_STATUS
+EFIAPI
+UriPercentDecode (
+  IN      CHAR8            *Buffer,
+  IN      UINT32            BufferLength,
+     OUT  CHAR8            *ResultBuffer,
+     OUT  UINT32           *ResultLength
+  );
+
+/**
+  Create a URL parser for the input URL string.
+
+  This function will parse and dereference the input HTTP URL into it components. The original
+  content of the URL won't be modified and the result will be returned in UrlParser, which can
+  be used in other functions like NetHttpUrlGetHostName(). It is the caller's responsibility to
+  free the buffer returned in *UrlParser by HttpUrlFreeParser().
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    Length             Length of Url in bytes.
+  @param[in]    IsConnectMethod    Whether the Url is used in HTTP CONNECT method or not.
+  @param[out]   UrlParser          Pointer to the returned buffer to store the parse result.
+
+  @retval EFI_SUCCESS              Successfully dereferenced the HTTP URL.
+  @retval EFI_INVALID_PARAMETER    UrlParser is NULL or Url is not a valid HTTP URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpParseUrl (
+  IN      CHAR8              *Url,
+  IN      UINT32             Length,
+  IN      BOOLEAN            IsConnectMethod,
+     OUT  VOID               **UrlParser
+  );
+
+/**
+  Get the Hostname from a HTTP URL.
+
+  This function will return the HostName according to the Url and previous parse result ,and
+  it is the caller's responsibility to free the buffer returned in *HostName.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   HostName           Pointer to a buffer to store the HostName.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or HostName is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No hostName component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetHostName (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  CHAR8              **HostName
+  );
+
+/**
+  Get the IPv4 address from a HTTP URL.
+
+  This function will return the IPv4 address according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Ip4Address         Pointer to a buffer to store the IP address.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Ip4Address is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No IPv4 address component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetIp4 (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  EFI_IPv4_ADDRESS   *Ip4Address
+  );
+
+/**
+  Get the IPv6 address from a HTTP URL.
+
+  This function will return the IPv6 address according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Ip6Address         Pointer to a buffer to store the IP address.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Ip6Address is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No IPv6 address component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetIp6 (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  EFI_IPv6_ADDRESS   *Ip6Address
+  );
+
+/**
+  Get the port number from a HTTP URL.
+
+  This function will return the port number according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Port               Pointer to a buffer to store the port number.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Port is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No port number in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetPort (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  UINT16             *Port
+  );
+
+/**
+  Release the resource of the URL parser.
+
+  @param[in]    UrlParser            Pointer to the parser.
+  
+**/
+VOID
+EFIAPI
+HttpUrlFreeParser (
+  IN      VOID               *UrlParser
+  );
+
+//
+// HTTP body parser interface.
+//
+
+typedef enum {
+  //
+  // Part of entity data.
+  // Length of entity body in Data.
+  //
+  BodyParseEventOnData,
+  //
+  // End of message body.
+  // Length is 0 and Data points to next byte after the end of the message.
+  //
+  BodyParseEventOnComplete
+} HTTP_BODY_PARSE_EVENT;
+
+/**
+  A callback function to intercept events during message parser.
+
+  This function will be invoked during HttpParseMessageBody() with various events type. An error
+  return status of the callback function will cause the HttpParseMessageBody() aborted.
+
+  @param[in]    EventType          Event type of this callback call.
+  @param[in]    Data               A pointer to data buffer.
+  @param[in]    Length             Length in bytes of the Data.
+  @param[in]    Context            Callback context set by HttpInitMsgParser().
+
+  @retval EFI_SUCCESS              Continue to parser the message body.
+  @retval Others                   Abort the parse.
+ 
+**/
+typedef
+EFI_STATUS
+(EFIAPI *HTTP_BODY_PARSER_CALLBACK) (
+  IN HTTP_BODY_PARSE_EVENT      EventType,
+  IN CHAR8                      *Data,
+  IN UINTN                      Length,
+  IN VOID                       *Context
+);
+
+/**
+  Initialize a HTTP message-body parser.
+
+  This function will create and initialize a HTTP message parser according to caller provided HTTP message
+  header information. It is the caller's responsibility to free the buffer returned in *UrlParser by HttpFreeMsgParser().
+
+  @param[in]    Method             The HTTP method (e.g. GET, POST) for this HTTP message.
+  @param[in]    StatusCode         Response status code returned by the remote host.
+  @param[in]    HeaderCount        Number of HTTP header structures in Headers.
+  @param[in]    Headers            Array containing list of HTTP headers.
+  @param[in]    Callback           Callback function that is invoked when parsing the HTTP message-body,
+                                   set to NULL to ignore all events.
+  @param[in]    Context            Pointer to the context that will be passed to Callback.
+  @param[out]   MsgParser          Pointer to the returned buffer to store the message parser.
+
+  @retval EFI_SUCCESS              Successfully initialized the parser.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  @retval EFI_INVALID_PARAMETER    MsgParser is NULL or HeaderCount is not NULL but Headers is NULL.
+  @retval Others                   Failed to initialize the parser.
+
+**/
+EFI_STATUS
+EFIAPI
+HttpInitMsgParser (
+  IN     EFI_HTTP_METHOD               Method,
+  IN     EFI_HTTP_STATUS_CODE          StatusCode,
+  IN     UINTN                         HeaderCount,
+  IN     EFI_HTTP_HEADER               *Headers,
+  IN     HTTP_BODY_PARSER_CALLBACK     Callback,
+  IN     VOID                          *Context,
+    OUT  VOID                          **MsgParser
+  );
+
+/**
+  Parse message body.
+
+  Parse BodyLength of message-body. This function can be called repeatedly to parse the message-body partially.
+
+  @param[in, out]    MsgParser            Pointer to the message parser.
+  @param[in]         BodyLength           Length in bytes of the Body.
+  @param[in]         Body                 Pointer to the buffer of the message-body to be parsed.
+
+  @retval EFI_SUCCESS                Successfully parse the message-body.
+  @retval EFI_INVALID_PARAMETER      MsgParser is NULL or Body is NULL or BodyLength is 0.
+  @retval Others                     Operation aborted.
+
+**/
+EFI_STATUS
+EFIAPI
+HttpParseMessageBody (
+  IN OUT VOID              *MsgParser,
+  IN     UINTN             BodyLength,
+  IN     CHAR8             *Body
+  );
+
+/**
+  Check whether the message-body is complete or not.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+
+  @retval TRUE                       Message-body is complete.
+  @retval FALSE                      Message-body is not complete.
+
+**/
+BOOLEAN
+EFIAPI
+HttpIsMessageComplete (
+  IN VOID           *MsgParser
+  );
+
+/**
+  Get the content length of the entity.
+
+  Note that in trunk transfer, the entity length is not valid until the whole message body is received.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+  @param[out]   ContentLength        Pointer to store the length of the entity.
+
+  @retval EFI_SUCCESS                Successfully to get the entity length.
+  @retval EFI_NOT_READY              Entity length is not valid yet.
+  @retval EFI_INVALID_PARAMETER      MsgParser is NULL or ContentLength is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpGetEntityLength (
+  IN  VOID           *MsgParser,
+  OUT UINTN          *ContentLength
+  );
+
+/**
+  Release the resource of the message parser.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+  
+**/
+VOID
+EFIAPI
+HttpFreeMsgParser (
+  IN  VOID           *MsgParser
+  );
+
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/IpIoLib.h b/Core/MdeModulePkg/Include/Library/IpIoLib.h
new file mode 100644
index 0000000000..37cba070a1
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/IpIoLib.h
@@ -0,0 +1,586 @@
+/** @file
+  This library is only intended to be used by UEFI network stack modules.
+  It provides the combined IpIo layer on the EFI IP4 Protocol and EFI IP6 protocol.
+
+Copyright (c) 2005 - 2010, 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 that 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.
+
+**/
+
+#ifndef _IP_IO_H_
+#define _IP_IO_H_
+
+#include <Protocol/Ip4.h>
+#include <Protocol/Ip6.h>
+
+#include <Library/NetLib.h>
+
+//
+// type and code define for ICMP protocol error 
+// from IP
+//
+#define ICMP_TYPE_UNREACH              3
+#define ICMP_TYPE_TIMXCEED             11
+#define ICMP_TYPE_PARAMPROB            12
+#define ICMP_TYPE_SOURCEQUENCH         4
+
+#define ICMP_CODE_UNREACH_NET          0
+#define ICMP_CODE_UNREACH_HOST         1
+#define ICMP_CODE_UNREACH_PROTOCOL     2
+#define ICMP_CODE_UNREACH_PORT         3
+#define ICMP_CODE_UNREACH_NEEDFRAG     4
+#define ICMP_CODE_UNREACH_SRCFAIL      5
+#define ICMP_CODE_UNREACH_NET_UNKNOWN  6
+#define ICMP_CODE_UNREACH_HOST_UNKNOWN 7
+#define ICMP_CODE_UNREACH_ISOLATED     8
+#define ICMP_CODE_UNREACH_NET_PROHIB   9
+#define ICMP_CODE_UNREACH_HOST_PROHIB  10
+#define ICMP_CODE_UNREACH_TOSNET       11
+#define ICMP_CODE_UNREACH_TOSHOST      12
+
+/**
+  Get the IP header length from the struct EFI_IP4_HEADER. HeaderLength is
+  Internet header length in 32-bit words, so HeaderLength<<2 is the real
+  length of IP header.
+  
+  @param[out] HdrPtr   A pointer to EFI_IP4_HEADER.
+  
+  @return The IP header length.
+**/
+#define EFI_IP4_HEADER_LEN(HdrPtr) ((HdrPtr)->HeaderLength << 2)
+
+/**
+  To types of ICMP error which consist of ICMP header, IP header and original 
+  datagram's data, get length from sum of ICMP header length, IP header length 
+  and first 64 bits of datagram's data length.
+  
+  @param[in] IpHdr   A pointer to EFI_IP4_HEADER.
+  
+  @return The ICMP error length.
+**/
+#define ICMP_ERRLEN(IpHdr) \
+  (sizeof(IP4_ICMP_HEAD) + EFI_IP4_HEADER_LEN(IpHdr) + 8)
+
+/**
+  Get the packet header from NET_BUF.
+  
+  @param[out]  Buf    A pointer to NET_BUF.
+  @param[in]   Type   Header type.
+  
+  @return The pointer to packet header.
+**/
+#define NET_PROTO_HDR(Buf, Type)  ((Type *) ((Buf)->BlockOp[0].Head))
+
+  
+extern EFI_IP4_CONFIG_DATA  mIp4IoDefaultIpConfigData;
+extern EFI_IP6_CONFIG_DATA  mIp6IoDefaultIpConfigData;
+
+
+///
+/// This error will be delivered to the
+/// listening transportation layer protocol
+/// that consumes IpIO.
+///
+
+#define  ICMP_ERR_UNREACH_NET           0
+#define  ICMP_ERR_UNREACH_HOST          1
+#define  ICMP_ERR_UNREACH_PROTOCOL      2
+#define  ICMP_ERR_UNREACH_PORT          3
+#define  ICMP_ERR_MSGSIZE               4
+#define  ICMP_ERR_UNREACH_SRCFAIL       5
+#define  ICMP_ERR_TIMXCEED_INTRANS      6
+#define  ICMP_ERR_TIMXCEED_REASS        7
+#define  ICMP_ERR_QUENCH                8
+#define  ICMP_ERR_PARAMPROB             9
+
+#define  ICMP6_ERR_UNREACH_NET          0
+#define  ICMP6_ERR_UNREACH_HOST         1
+#define  ICMP6_ERR_UNREACH_PROTOCOL     2
+#define  ICMP6_ERR_UNREACH_PORT         3
+#define  ICMP6_ERR_PACKAGE_TOOBIG       4
+#define  ICMP6_ERR_TIMXCEED_HOPLIMIT    5
+#define  ICMP6_ERR_TIMXCEED_REASS       6
+#define  ICMP6_ERR_PARAMPROB_HEADER     7
+#define  ICMP6_ERR_PARAMPROB_NEXHEADER  8
+#define  ICMP6_ERR_PARAMPROB_IPV6OPTION 9
+
+///
+/// The helper struct for IpIoGetIcmpErrStatus(). It is for internal use only.
+///
+typedef struct {
+  BOOLEAN                   IsHard;
+  BOOLEAN                   Notify;
+} ICMP_ERROR_INFO;
+
+typedef union {
+  EFI_IP4_COMPLETION_TOKEN  Ip4Token;
+  EFI_IP6_COMPLETION_TOKEN  Ip6Token;
+} IP_IO_IP_COMPLETION_TOKEN;
+
+typedef union {
+  EFI_IP4_TRANSMIT_DATA     Ip4TxData;
+  EFI_IP6_TRANSMIT_DATA     Ip6TxData;
+} IP_IO_IP_TX_DATA;
+
+typedef union {
+  EFI_IP4_RECEIVE_DATA      Ip4RxData;
+  EFI_IP6_RECEIVE_DATA      Ip6RxData;
+} IP_IO_IP_RX_DATA;
+
+typedef union {
+  EFI_IP4_OVERRIDE_DATA     Ip4OverrideData;
+  EFI_IP6_OVERRIDE_DATA     Ip6OverrideData;
+} IP_IO_OVERRIDE;
+
+typedef union {
+  EFI_IP4_CONFIG_DATA       Ip4CfgData;
+  EFI_IP6_CONFIG_DATA       Ip6CfgData;
+} IP_IO_IP_CONFIG_DATA;
+
+typedef union {
+  EFI_IP4_HEADER            *Ip4Hdr;
+  EFI_IP6_HEADER            *Ip6Hdr;
+} IP_IO_IP_HEADER;
+
+typedef union {
+  IP4_ADDR                  SubnetMask;
+  UINT8                     PrefixLength;
+} IP_IO_IP_MASK;
+
+typedef union {
+  EFI_IP4_PROTOCOL  *Ip4;
+  EFI_IP6_PROTOCOL  *Ip6;
+} IP_IO_IP_PROTOCOL;
+
+///
+/// The IP session for an IP receive packet.
+///
+typedef struct _EFI_NET_SESSION_DATA {
+  EFI_IP_ADDRESS        Source;     ///< Source IP of the received packet.
+  EFI_IP_ADDRESS        Dest;       ///< Destination IP of the received packet.
+  IP_IO_IP_HEADER       IpHdr;      ///< IP header of the received packet.
+  UINT32                IpHdrLen;   ///< IP header length of the received packet. 
+                                    ///< For IPv6, it includes the IP6 header 
+                                    ///< length and extension header length. For
+                                    ///< IPv4, it includes the IP4 header length
+                                    ///< and options length.
+  UINT8                 IpVersion;  ///< The IP version of the received packet.
+} EFI_NET_SESSION_DATA;
+
+/**
+  The prototype is called back when an IP packet is received.
+  
+  @param[in] Status        The result of the receive request.
+  @param[in] IcmpErr       Valid when Status is EFI_ICMP_ERROR.
+  @param[in] NetSession    The IP session for the received packet.
+  @param[in] Pkt           The packet received.
+  @param[in] Context       The data provided by the user for the received packet when
+                           the callback is registered in IP_IO_OPEN_DATA::RcvdContext.
+  
+**/
+typedef
+VOID
+(EFIAPI *PKT_RCVD_NOTIFY) (
+  IN EFI_STATUS           Status, 
+  IN UINT8                IcmpErr,
+  IN EFI_NET_SESSION_DATA *NetSession,
+  IN NET_BUF              *Pkt,
+  IN VOID                 *Context
+  );
+
+/**
+  The prototype is called back when an IP packet is sent.
+  
+  @param[in] Status        Result of the IP packet being sent.
+  @param[in] Context       The data provided by user for the received packet when
+                           the callback is registered in IP_IO_OPEN_DATA::SndContext.
+  @param[in] Sender        A Union type to specify a pointer of EFI_IP4_PROTOCOL 
+                           or EFI_IP6_PROTOCOL.
+  @param[in] NotifyData    The Context data specified when calling IpIoSend()
+  
+**/
+typedef
+VOID
+(EFIAPI *PKT_SENT_NOTIFY) (
+  IN EFI_STATUS        Status,
+  IN VOID              *Context,
+  IN IP_IO_IP_PROTOCOL Sender,
+  IN VOID              *NotifyData
+  );
+
+///
+/// This data structure wraps Ip4/Ip6 instances. The IpIo Library uses it for all
+/// Ip4/Ip6 operations.
+///
+typedef struct _IP_IO {
+  ///
+  /// The node used to link this IpIo to the active IpIo list.
+  ///
+  LIST_ENTRY                    Entry;
+
+  ///
+  /// The list used to maintain the IP instance for different sending purpose.
+  ///
+  LIST_ENTRY                    IpList;
+  
+  EFI_HANDLE                    Controller;
+  EFI_HANDLE                    Image;
+  EFI_HANDLE                    ChildHandle;
+  //
+  // The IP instance consumed by this IP_IO
+  //
+  IP_IO_IP_PROTOCOL             Ip;
+  BOOLEAN                       IsConfigured;
+
+  ///
+  /// Some ip configuration data can be changed.
+  ///
+  UINT8                         Protocol;
+
+  ///
+  /// Token and event used to get data from IP.
+  ///
+  IP_IO_IP_COMPLETION_TOKEN     RcvToken; 
+
+  ///
+  /// List entry used to link the token passed to IP_IO.
+  ///
+  LIST_ENTRY                    PendingSndList;
+
+  //
+  // User interface used to get notify from IP_IO
+  //
+  VOID                          *RcvdContext;    ///< See IP_IO_OPEN_DATA::RcvdContext.
+  VOID                          *SndContext;     ///< See IP_IO_OPEN_DATA::SndContext.
+  PKT_RCVD_NOTIFY               PktRcvdNotify;   ///< See IP_IO_OPEN_DATA::PktRcvdNotify.
+  PKT_SENT_NOTIFY               PktSentNotify;   ///< See IP_IO_OPEN_DATA::PktSentNotify.
+  UINT8                         IpVersion;
+} IP_IO;
+
+///
+/// The struct is for the user to pass IP configuration and callbacks to IP_IO.
+/// It is used by IpIoOpen().
+///
+typedef struct _IP_IO_OPEN_DATA {
+  IP_IO_IP_CONFIG_DATA IpConfigData;    ///< Configuration of the IP instance.
+  VOID                 *RcvdContext;    ///< Context data used by receive callback.
+  VOID                 *SndContext;     ///< Context data used by send callback.
+  PKT_RCVD_NOTIFY      PktRcvdNotify;   ///< Receive callback.
+  PKT_SENT_NOTIFY      PktSentNotify;   ///< Send callback.
+} IP_IO_OPEN_DATA;
+
+///
+/// Internal struct book-keeping send request of IP_IO.
+///
+/// An IP_IO_SEND_ENTRY will be created each time a send request is issued to
+/// IP_IO via IpIoSend().
+///
+typedef struct _IP_IO_SEND_ENTRY {
+  LIST_ENTRY                Entry;
+  IP_IO                     *IpIo;
+  VOID                      *Context;
+  VOID                      *NotifyData;
+  IP_IO_IP_PROTOCOL         Ip;
+  NET_BUF                   *Pkt;
+  IP_IO_IP_COMPLETION_TOKEN SndToken;
+} IP_IO_SEND_ENTRY;
+
+///
+/// The IP_IO_IP_INFO is used in IpIoSend() to override the default IP instance
+/// in IP_IO.
+///
+typedef struct _IP_IO_IP_INFO {
+  EFI_IP_ADDRESS            Addr;
+  IP_IO_IP_MASK             PreMask;
+  LIST_ENTRY                Entry;
+  EFI_HANDLE                ChildHandle;
+  IP_IO_IP_PROTOCOL         Ip;
+  IP_IO_IP_COMPLETION_TOKEN DummyRcvToken;
+  INTN                      RefCnt;
+  UINT8                     IpVersion;
+} IP_IO_IP_INFO;
+
+/**
+  Create a new IP_IO instance.
+  
+  This function uses IP4/IP6 service binding protocol in Controller to create
+  an IP4/IP6 child (aka IP4/IP6 instance).
+
+  @param[in]  Image             The image handle of the driver or application that
+                                consumes IP_IO.
+  @param[in]  Controller        The controller handle that has IP4 or IP6 service
+                                binding protocol installed.
+  @param[in]  IpVersion         The version of the IP protocol to use, either
+                                IPv4 or IPv6.                            
+
+  @return The pointer to a newly created IP_IO instance, or NULL if failed.
+
+**/
+IP_IO *
+EFIAPI
+IpIoCreate (
+  IN EFI_HANDLE Image,
+  IN EFI_HANDLE Controller,
+  IN UINT8      IpVersion
+  );
+
+/**
+  Destroy an IP_IO instance.
+  
+  This function is paired with IpIoCreate(). The IP_IO will be closed first.
+  Resource will be freed afterwards. See IpIoClose().
+
+  @param[in, out]  IpIo         The pointer to the IP_IO instance that needs to be
+                                destroyed.
+
+  @retval          EFI_SUCCESS  The IP_IO instance was destroyed successfully.
+  @retval          Others       An error condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoDestroy (
+  IN OUT IP_IO *IpIo
+  );
+
+/**
+  Stop an IP_IO instance.
+  
+  This function is paired with IpIoOpen(). The IP_IO will be unconfigured, and all
+  pending send/receive tokens will be canceled.
+
+  @param[in, out]  IpIo            The pointer to the IP_IO instance that needs to stop.
+
+  @retval          EFI_SUCCESS     The IP_IO instance stopped successfully.
+  @retval          Others          Anrror condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoStop (
+  IN OUT IP_IO *IpIo
+  );
+
+/**
+  Open an IP_IO instance for use.
+  
+  This function is called after IpIoCreate(). It is used for configuring the IP
+  instance and register the callbacks and their context data for sending and
+  receiving IP packets.
+
+  @param[in, out]  IpIo               The pointer to an IP_IO instance that needs
+                                      to open.
+  @param[in]       OpenData           The configuration data and callbacks for
+                                      the IP_IO instance.
+
+  @retval          EFI_SUCCESS        The IP_IO instance opened with OpenData
+                                      successfully.
+  @retval          EFI_ACCESS_DENIED  The IP_IO instance is configured; avoid  
+                                      reopening it.
+  @retval          Others             An error condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoOpen (
+  IN OUT IP_IO           *IpIo,
+  IN     IP_IO_OPEN_DATA *OpenData
+  );
+
+/**
+  Send out an IP packet.
+  
+  This function is called after IpIoOpen(). The data to be sent are wrapped in
+  Pkt. The IP instance wrapped in IpIo is used for sending by default, but can be
+  overriden by Sender. Other sending configurations, such as source address and gateway
+  address, are specified in OverrideData.
+
+  @param[in, out]  IpIo                  The pointer to an IP_IO instance used for sending IP
+                                         packet.
+  @param[in, out]  Pkt                   The pointer to the IP packet to be sent.
+  @param[in]       Sender                Optional. The IP protocol instance used for sending.
+  @param[in]       Context               The optional context data.
+  @param[in]       NotifyData            The optional notify data.
+  @param[in]       Dest                  The destination IP address to send this packet to.
+  @param[in]       OverrideData          The data to override some configuration of the IP
+                                         instance used for sending.
+
+  @retval          EFI_SUCCESS           The operation completed successfully.
+  @retval          EFI_NOT_STARTED       The IpIo is not configured.
+  @retval          EFI_OUT_OF_RESOURCES  Failed due to resource limitations.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoSend (
+  IN OUT IP_IO          *IpIo,
+  IN OUT NET_BUF        *Pkt,
+  IN     IP_IO_IP_INFO  *Sender        OPTIONAL,
+  IN     VOID           *Context       OPTIONAL,
+  IN     VOID           *NotifyData    OPTIONAL,
+  IN     EFI_IP_ADDRESS *Dest,
+  IN     IP_IO_OVERRIDE *OverrideData  OPTIONAL
+  );
+
+/**
+  Cancel the IP transmit token that wraps this Packet.
+
+  @param[in]  IpIo                  The pointer to the IP_IO instance.
+  @param[in]  Packet                The pointer to the packet of NET_BUF to cancel.
+
+**/
+VOID
+EFIAPI
+IpIoCancelTxToken (
+  IN IP_IO  *IpIo,
+  IN VOID   *Packet
+  );
+
+/**
+  Add a new IP instance for sending data.
+  
+  The function is used to add the IP_IO to the IP_IO sending list. The caller
+  can later use IpIoFindSender() to get the IP_IO and call IpIoSend() to send
+  data.
+
+  @param[in, out]  IpIo               The pointer to an IP_IO instance to add a new IP
+                                      instance for sending purposes.
+
+  @return The pointer to the created IP_IO_IP_INFO structure; NULL if failed.
+
+**/
+IP_IO_IP_INFO *
+EFIAPI
+IpIoAddIp (
+  IN OUT IP_IO  *IpIo
+  );
+
+/**
+  Configure the IP instance of this IpInfo and start the receiving if IpConfigData
+  is not NULL.
+
+  @param[in, out]  IpInfo          The pointer to the IP_IO_IP_INFO instance.
+  @param[in, out]  IpConfigData    The IP4 or IP6 configure data used to configure 
+                                   the IP instance. If NULL, the IP instance is reset.
+                                   If UseDefaultAddress is set to TRUE, and the configure
+                                   operation succeeds, the default address information
+                                   is written back in this IpConfigData.
+
+  @retval          EFI_SUCCESS     The IP instance of this IpInfo was configured successfully,
+                                   or there is no need to reconfigure it.
+  @retval          Others          The configuration failed.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoConfigIp (
+  IN OUT IP_IO_IP_INFO        *IpInfo,
+  IN OUT VOID                 *IpConfigData OPTIONAL
+  );
+
+/**
+  Destroy an IP instance maintained in IpIo->IpList for
+  sending purpose.
+  
+  This function pairs with IpIoAddIp(). The IpInfo is previously created by
+  IpIoAddIp(). The IP_IO_IP_INFO::RefCnt is decremented and the IP instance
+  will be dstroyed if the RefCnt is zero.
+
+  @param[in]  IpIo                  The pointer to the IP_IO instance.
+  @param[in]  IpInfo                The pointer to the IpInfo to be removed.
+
+**/
+VOID
+EFIAPI
+IpIoRemoveIp (
+  IN IP_IO            *IpIo,
+  IN IP_IO_IP_INFO    *IpInfo
+  );
+
+/**
+  Find the first IP protocol maintained in IpIo whose local
+  address is the same as Src.
+  
+  This function is called when the caller needs the IpIo to send data to the
+  specified Src. The IpIo was added previously by IpIoAddIp().
+
+  @param[in, out]  IpIo              The pointer to the pointer of the IP_IO instance.
+  @param[in]       IpVersion         The version of the IP protocol to use, either
+                                     IPv4 or IPv6.
+  @param[in]       Src               The local IP address.
+
+  @return The pointer to the IP protocol can be used for sending purpose and its local
+          address is the same with Src.
+
+**/
+IP_IO_IP_INFO *
+EFIAPI
+IpIoFindSender (
+  IN OUT IP_IO           **IpIo,
+  IN     UINT8           IpVersion,
+  IN     EFI_IP_ADDRESS  *Src
+  );
+
+/**
+  Get the ICMP error map information.
+  
+  The ErrorStatus will be returned. The IsHard and Notify are optional. If they
+  are not NULL, this routine will fill them.
+
+  @param[in]   IcmpError             IcmpError Type.
+  @param[in]   IpVersion             The version of the IP protocol to use,
+                                     either IPv4 or IPv6. 
+  @param[out]  IsHard                If TRUE, indicates that it is a hard error.
+  @param[out]  Notify                If TRUE, SockError needs to be notified.
+
+  @return The ICMP Error Status, such as EFI_NETWORK_UNREACHABLE.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoGetIcmpErrStatus (
+  IN  UINT8       IcmpError,
+  IN  UINT8       IpVersion,
+  OUT BOOLEAN     *IsHard  OPTIONAL,
+  OUT BOOLEAN     *Notify  OPTIONAL
+  );
+
+/**
+  Refresh the remote peer's Neighbor Cache entries.
+
+  This function is called when the caller needs the IpIo to refresh the existing
+  IPv6 neighbor cache entries since the neighbor is considered reachable by the 
+  node has recently received a confirmation that packets sent recently to the 
+  neighbor were received by its IP layer. 
+
+  @param[in]   IpIo                  The pointer to an IP_IO instance
+  @param[in]   Neighbor              The IP address of the neighbor
+  @param[in]   Timeout               The time in 100-ns units that this entry will
+                                     remain in the neighbor cache. A value of 
+                                     zero means that the entry is permanent. 
+                                     A value of non-zero means that the entry is 
+                                     dynamic and will be deleted after Timeout.
+
+  @retval      EFI_SUCCESS           The operation completed successfully.
+  @retval      EFI_NOT_STARTED       The IpIo is not configured.
+  @retval      EFI_INVALID_PARAMETER The Neighbor Address is invalid.
+  @retval      EFI_NOT_FOUND         The neighbor cache entry is not in the 
+                                     neighbor table.  
+  @retval      EFI_OUT_OF_RESOURCES  Failed due to resource limitations.
+
+**/
+EFI_STATUS
+IpIoRefreshNeighbor (
+  IN IP_IO           *IpIo,
+  IN EFI_IP_ADDRESS  *Neighbor,
+  IN UINT32          Timeout  
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/IpmiLib.h b/Core/MdeModulePkg/Include/Library/IpmiLib.h
new file mode 100644
index 0000000000..71b0c1a5a3
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/IpmiLib.h
@@ -0,0 +1,51 @@
+/** @file
+  This library abstract how to access IPMI device via IPMI command.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+#ifndef _IPMI_LIB_H_
+#define _IPMI_LIB_H_
+
+#include <Uefi.h>
+#include <IndustryStandard/Ipmi.h>
+
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+EFI_STATUS
+EFIAPI
+IpmiSubmitCommand (
+  IN     UINT8     NetFunction,
+  IN     UINT8     Command,
+  IN     UINT8     *RequestData,
+  IN     UINT32    RequestDataSize,
+     OUT UINT8     *ResponseData,
+  IN OUT UINT32    *ResponseDataSize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/LockBoxLib.h b/Core/MdeModulePkg/Include/Library/LockBoxLib.h
new file mode 100644
index 0000000000..db7fd05def
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/LockBoxLib.h
@@ -0,0 +1,133 @@
+/** @file
+  This library is only intended to be used by DXE modules that need save
+  confidential information to LockBox and get it by PEI modules in S3 phase.
+
+Copyright (c) 2010 - 2011, 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.
+
+**/
+
+#ifndef _LOCK_BOX_LIB_H_
+#define _LOCK_BOX_LIB_H_
+
+/**
+  This function will save confidential information to lockbox.
+
+  @param Guid       the guid to identify the confidential information
+  @param Buffer     the address of the confidential information
+  @param Length     the length of the confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0
+  @retval RETURN_ALREADY_STARTED    the requested GUID already exist.
+  @retval RETURN_OUT_OF_RESOURCES   no enough resource to save the information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SaveLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  );
+
+/**
+  This function will set lockbox attributes.
+
+  @param Guid       the guid to identify the confidential information
+  @param Attributes the attributes of the lockbox
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  attributes is invalid.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SetLockBoxAttributes (
+  IN  GUID                        *Guid,
+  IN  UINT64                      Attributes
+  );
+
+//
+// With this flag, this LockBox can be restored to this Buffer with RestoreAllLockBoxInPlace()
+//
+#define LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE  BIT0
+
+/**
+  This function will update confidential information to lockbox.
+
+  @param Guid   the guid to identify the original confidential information
+  @param Offset the offset of the original confidential information
+  @param Buffer the address of the updated confidential information
+  @param Length the length of the updated confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_BUFFER_TOO_SMALL   the original buffer to too small to hold new information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+UpdateLockBox (
+  IN  GUID                        *Guid,
+  IN  UINTN                       Offset,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  );
+
+/**
+  This function will restore confidential information from lockbox.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or one of Buffer and Length is NULL.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_ACCESS_DENIED      not allow to restore to the address
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  );
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreAllLockBoxInPlace (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/NetLib.h b/Core/MdeModulePkg/Include/Library/NetLib.h
new file mode 100644
index 0000000000..280c51a01d
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/NetLib.h
@@ -0,0 +1,2132 @@
+/** @file
+  This library is only intended to be used by UEFI network stack modules.
+  It provides basic functions for the UEFI network stack.
+
+Copyright (c) 2005 - 2012, 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<BR>
+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.
+
+**/
+
+#ifndef _NET_LIB_H_
+#define _NET_LIB_H_
+
+#include <Protocol/Ip6.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+
+typedef UINT32          IP4_ADDR;
+typedef UINT32          TCP_SEQNO;
+typedef UINT16          TCP_PORTNO;
+
+
+#define  NET_ETHER_ADDR_LEN    6
+#define  NET_IFTYPE_ETHERNET   0x01
+
+#define  NET_VLAN_TAG_LEN      4
+#define  ETHER_TYPE_VLAN       0x8100
+
+#define  EFI_IP_PROTO_UDP      0x11
+#define  EFI_IP_PROTO_TCP      0x06
+#define  EFI_IP_PROTO_ICMP     0x01
+#define  IP4_PROTO_IGMP        0x02
+#define  IP6_ICMP              58
+
+//
+// The address classification
+//
+#define  IP4_ADDR_CLASSA       1
+#define  IP4_ADDR_CLASSB       2
+#define  IP4_ADDR_CLASSC       3
+#define  IP4_ADDR_CLASSD       4
+#define  IP4_ADDR_CLASSE       5
+
+#define  IP4_MASK_NUM          33
+#define  IP6_PREFIX_NUM        129
+
+#define  IP6_HOP_BY_HOP        0
+#define  IP6_DESTINATION       60
+#define  IP6_ROUTING           43
+#define  IP6_FRAGMENT          44
+#define  IP6_AH                51
+#define  IP6_ESP               50
+#define  IP6_NO_NEXT_HEADER    59
+
+#define  IP_VERSION_4          4
+#define  IP_VERSION_6          6
+
+#define  IP6_PREFIX_LENGTH     64
+
+#pragma pack(1)
+
+//
+// Ethernet head definition
+//
+typedef struct {
+  UINT8                 DstMac [NET_ETHER_ADDR_LEN];
+  UINT8                 SrcMac [NET_ETHER_ADDR_LEN];
+  UINT16                EtherType;
+} ETHER_HEAD;
+
+//
+// 802.1Q VLAN Tag Control Information
+//
+typedef union {
+  struct {
+    UINT16              Vid      : 12;  // Unique VLAN identifier (0 to 4094)
+    UINT16              Cfi      : 1;   // Canonical Format Indicator
+    UINT16              Priority : 3;   // 802.1Q priority level (0 to 7)
+  } Bits;
+  UINT16                Uint16;
+} VLAN_TCI;
+
+#define VLAN_TCI_CFI_CANONICAL_MAC      0
+#define VLAN_TCI_CFI_NON_CANONICAL_MAC  1
+
+//
+// The EFI_IP4_HEADER is hard to use because the source and
+// destination address are defined as EFI_IPv4_ADDRESS, which
+// is a structure. Two structures can't be compared or masked
+// directly. This is why there is an internal representation.
+//
+typedef struct {
+  UINT8                 HeadLen : 4;
+  UINT8                 Ver     : 4;
+  UINT8                 Tos;
+  UINT16                TotalLen;
+  UINT16                Id;
+  UINT16                Fragment;
+  UINT8                 Ttl;
+  UINT8                 Protocol;
+  UINT16                Checksum;
+  IP4_ADDR              Src;
+  IP4_ADDR              Dst;
+} IP4_HEAD;
+
+
+//
+// ICMP head definition. Each ICMP message is categorized as either an error
+// message or query message. Two message types have their own head format.
+//
+typedef struct {
+  UINT8                 Type;
+  UINT8                 Code;
+  UINT16                Checksum;
+} IP4_ICMP_HEAD;
+
+typedef struct {
+  IP4_ICMP_HEAD         Head;
+  UINT32                Fourth; // 4th filed of the head, it depends on Type.
+  IP4_HEAD              IpHead;
+} IP4_ICMP_ERROR_HEAD;
+
+typedef struct {
+  IP4_ICMP_HEAD         Head;
+  UINT16                Id;
+  UINT16                Seq;
+} IP4_ICMP_QUERY_HEAD;
+
+typedef struct {
+  UINT8                 Type;
+  UINT8                 Code;
+  UINT16                Checksum;
+} IP6_ICMP_HEAD;
+
+typedef struct {
+  IP6_ICMP_HEAD         Head;
+  UINT32                Fourth;
+  EFI_IP6_HEADER        IpHead;
+} IP6_ICMP_ERROR_HEAD;
+
+typedef struct {
+  IP6_ICMP_HEAD         Head;
+  UINT32                Fourth;
+} IP6_ICMP_INFORMATION_HEAD;
+
+//
+// UDP header definition
+//
+typedef struct {
+  UINT16                SrcPort;
+  UINT16                DstPort;
+  UINT16                Length;
+  UINT16                Checksum;
+} EFI_UDP_HEADER;
+
+//
+// TCP header definition
+//
+typedef struct {
+  TCP_PORTNO            SrcPort;
+  TCP_PORTNO            DstPort;
+  TCP_SEQNO             Seq;
+  TCP_SEQNO             Ack;
+  UINT8                 Res     : 4;
+  UINT8                 HeadLen : 4;
+  UINT8                 Flag;
+  UINT16                Wnd;
+  UINT16                Checksum;
+  UINT16                Urg;
+} TCP_HEAD;
+
+#pragma pack()
+
+#define NET_MAC_EQUAL(pMac1, pMac2, Len)     \
+    (CompareMem ((pMac1), (pMac2), Len) == 0)
+
+#define NET_MAC_IS_MULTICAST(Mac, BMac, Len) \
+    (((*((UINT8 *) Mac) & 0x01) == 0x01) && (!NET_MAC_EQUAL (Mac, BMac, Len)))
+
+#define NTOHL(x)  SwapBytes32 (x)
+
+#define HTONL(x)  NTOHL(x)
+
+#define NTOHS(x)  SwapBytes16 (x)
+
+#define HTONS(x)   NTOHS(x)
+#define NTOHLL(x)  SwapBytes64 (x)
+#define HTONLL(x)  NTOHLL(x)
+#define NTOHLLL(x) Ip6Swap128 (x)
+#define HTONLLL(x) NTOHLLL(x)
+
+//
+// Test the IP's attribute, All the IPs are in host byte order.
+//
+#define IP4_IS_MULTICAST(Ip)              (((Ip) & 0xF0000000) == 0xE0000000)
+#define IP4_IS_LOCAL_BROADCAST(Ip)        ((Ip) == 0xFFFFFFFF)
+#define IP4_NET_EQUAL(Ip1, Ip2, NetMask)  (((Ip1) & (NetMask)) == ((Ip2) & (NetMask)))
+#define IP4_IS_VALID_NETMASK(Ip)          (NetGetMaskLength (Ip) != IP4_MASK_NUM)
+
+#define IP6_IS_MULTICAST(Ip6)             (((Ip6)->Addr[0]) == 0xFF)
+
+//
+// Convert the EFI_IP4_ADDRESS to plain UINT32 IP4 address.
+//
+#define EFI_IP4(EfiIpAddr)       (*(IP4_ADDR *) ((EfiIpAddr).Addr))
+#define EFI_NTOHL(EfiIp)         (NTOHL (EFI_IP4 ((EfiIp))))
+#define EFI_IP4_EQUAL(Ip1, Ip2)  (CompareMem ((Ip1), (Ip2), sizeof (EFI_IPv4_ADDRESS)) == 0)
+
+#define EFI_IP6_EQUAL(Ip1, Ip2)  (CompareMem ((Ip1), (Ip2), sizeof (EFI_IPv6_ADDRESS)) == 0)
+
+#define IP4_COPY_ADDRESS(Dest, Src) (CopyMem ((Dest), (Src), sizeof (EFI_IPv4_ADDRESS)))
+#define IP6_COPY_ADDRESS(Dest, Src) (CopyMem ((Dest), (Src), sizeof (EFI_IPv6_ADDRESS)))
+#define IP6_COPY_LINK_ADDRESS(Mac1, Mac2) (CopyMem ((Mac1), (Mac2), sizeof (EFI_MAC_ADDRESS)))
+
+//
+// The debug level definition. This value is also used as the
+// syslog's servity level. Don't change it.
+//
+#define NETDEBUG_LEVEL_TRACE   5
+#define NETDEBUG_LEVEL_WARNING 4
+#define NETDEBUG_LEVEL_ERROR   3
+
+//
+// Network debug message is sent out as syslog packet.
+//
+#define NET_SYSLOG_FACILITY    16                 // Syslog local facility local use
+#define NET_SYSLOG_PACKET_LEN  512
+#define NET_SYSLOG_TX_TIMEOUT  (500 * 1000 * 10)  // 500ms
+#define NET_DEBUG_MSG_LEN      470                // 512 - (ether+ip4+udp4 head length)
+
+//
+// The debug output expects the ASCII format string, Use %a to print ASCII
+// string, and %s to print UNICODE string. PrintArg must be enclosed in ().
+// For example: NET_DEBUG_TRACE ("Tcp", ("State transit to %a\n", Name));
+//
+#define NET_DEBUG_TRACE(Module, PrintArg) \
+  NetDebugOutput ( \
+    NETDEBUG_LEVEL_TRACE, \
+    Module, \
+    __FILE__, \
+    __LINE__, \
+    NetDebugASPrint PrintArg \
+    )
+
+#define NET_DEBUG_WARNING(Module, PrintArg) \
+  NetDebugOutput ( \
+    NETDEBUG_LEVEL_WARNING, \
+    Module, \
+    __FILE__, \
+    __LINE__, \
+    NetDebugASPrint PrintArg \
+    )
+
+#define NET_DEBUG_ERROR(Module, PrintArg) \
+  NetDebugOutput ( \
+    NETDEBUG_LEVEL_ERROR, \
+    Module, \
+    __FILE__, \
+    __LINE__, \
+    NetDebugASPrint PrintArg \
+    )
+
+/**
+  Allocate a buffer, then format the message to it. This is a
+  help function for the NET_DEBUG_XXX macros. The PrintArg of
+  these macros treats the variable length print parameters as a
+  single parameter, and pass it to the NetDebugASPrint. For
+  example, NET_DEBUG_TRACE ("Tcp", ("State transit to %a\n", Name))
+  if extracted to:
+
+         NetDebugOutput (
+           NETDEBUG_LEVEL_TRACE,
+           "Tcp",
+           __FILE__,
+           __LINE__,
+           NetDebugASPrint ("State transit to %a\n", Name)
+         )
+
+  @param Format  The ASCII format string.
+  @param ...     The variable length parameter whose format is determined
+                 by the Format string.
+
+  @return        The buffer containing the formatted message,
+                 or NULL if memory allocation failed.
+
+**/
+CHAR8 *
+EFIAPI
+NetDebugASPrint (
+  IN CHAR8                  *Format,
+  ...
+  );
+
+/**
+  Builds an UDP4 syslog packet and send it using SNP.
+
+  This function will locate a instance of SNP then send the message through it.
+  Because it isn't open the SNP BY_DRIVER, apply caution when using it.
+
+  @param Level    The servity level of the message.
+  @param Module   The Moudle that generates the log.
+  @param File     The file that contains the log.
+  @param Line     The exact line that contains the log.
+  @param Message  The user message to log.
+
+  @retval EFI_INVALID_PARAMETER Any input parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the packet
+  @retval EFI_SUCCESS           The log is discard because that it is more verbose
+                                than the mNetDebugLevelMax. Or, it has been sent out.
+**/
+EFI_STATUS
+EFIAPI
+NetDebugOutput (
+  IN UINT32                    Level,
+  IN UINT8                     *Module,
+  IN UINT8                     *File,
+  IN UINT32                    Line,
+  IN UINT8                     *Message
+  );
+
+
+/**
+  Return the length of the mask.
+
+  Return the length of the mask. Valid values are 0 to 32.
+  If the mask is invalid, return the invalid length 33, which is IP4_MASK_NUM.
+  NetMask is in the host byte order.
+
+  @param[in]  NetMask              The netmask to get the length from.
+
+  @return The length of the netmask, or IP4_MASK_NUM (33) if the mask is invalid.
+
+**/
+INTN
+EFIAPI
+NetGetMaskLength (
+  IN IP4_ADDR               NetMask
+  );
+
+/**
+  Return the class of the IP address, such as class A, B, C.
+  Addr is in host byte order.
+
+  The address of class A  starts with 0.
+  If the address belong to class A, return IP4_ADDR_CLASSA.
+  The address of class B  starts with 10.
+  If the address belong to class B, return IP4_ADDR_CLASSB.
+  The address of class C  starts with 110.
+  If the address belong to class C, return IP4_ADDR_CLASSC.
+  The address of class D  starts with 1110.
+  If the address belong to class D, return IP4_ADDR_CLASSD.
+  The address of class E  starts with 1111.
+  If the address belong to class E, return IP4_ADDR_CLASSE.
+
+
+  @param[in]   Addr                  The address to get the class from.
+
+  @return IP address class, such as IP4_ADDR_CLASSA.
+
+**/
+INTN
+EFIAPI
+NetGetIpClass (
+  IN IP4_ADDR               Addr
+  );
+
+/**
+  Check whether the IP is a valid unicast address according to
+  the netmask. If NetMask is zero, use the IP address's class to get the default mask.
+
+  If Ip is 0, IP is not a valid unicast address.
+  Class D address is used for multicasting and class E address is reserved for future. If Ip
+  belongs to class D or class E, Ip is not a valid unicast address.
+  If all bits of the host address of Ip are 0 or 1, Ip is not a valid unicast address.
+
+  @param[in]  Ip                    The IP to check against.
+  @param[in]  NetMask               The mask of the IP.
+
+  @return TRUE if Ip is a valid unicast address on the network, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp4IsUnicast (
+  IN IP4_ADDR               Ip,
+  IN IP4_ADDR               NetMask
+  );
+
+/**
+  Check whether the incoming IPv6 address is a valid unicast address.
+
+  If the address is a multicast address has binary 0xFF at the start, it is not
+  a valid unicast address. If the address is unspecified ::, it is not a valid
+  unicast address to be assigned to any node. If the address is loopback address
+  ::1, it is also not a valid unicast address to be assigned to any physical
+  interface.
+
+  @param[in]  Ip6                   The IPv6 address to check against.
+
+  @return TRUE if Ip6 is a valid unicast address on the network, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsValidUnicast (
+  IN EFI_IPv6_ADDRESS       *Ip6
+  );
+
+
+/**
+  Check whether the incoming Ipv6 address is the unspecified address or not.
+
+  @param[in] Ip6   - Ip6 address, in network order.
+
+  @retval TRUE     - Yes, incoming Ipv6 address is the unspecified address.
+  @retval FALSE    - The incoming Ipv6 address is not the unspecified address
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsUnspecifiedAddr (
+  IN EFI_IPv6_ADDRESS       *Ip6
+  );
+
+/**
+  Check whether the incoming Ipv6 address is a link-local address.
+
+  @param[in] Ip6   - Ip6 address, in network order.
+
+  @retval TRUE  - The incoming Ipv6 address is a link-local address.
+  @retval FALSE - The incoming Ipv6 address is not a link-local address.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsLinkLocalAddr (
+  IN EFI_IPv6_ADDRESS *Ip6
+  );
+
+/**
+  Check whether the Ipv6 address1 and address2 are on the connected network.
+
+  @param[in] Ip1          - Ip6 address1, in network order.
+  @param[in] Ip2          - Ip6 address2, in network order.
+  @param[in] PrefixLength - The prefix length of the checking net.
+
+  @retval TRUE            - Yes, the Ipv6 address1 and address2 are connected.
+  @retval FALSE           - No the Ipv6 address1 and address2 are not connected.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsNetEqual (
+  EFI_IPv6_ADDRESS *Ip1,
+  EFI_IPv6_ADDRESS *Ip2,
+  UINT8            PrefixLength
+  );
+
+/**
+  Switches the endianess of an IPv6 address.
+
+  This function swaps the bytes in a 128-bit IPv6 address to switch the value
+  from little endian to big endian or vice versa. The byte swapped value is
+  returned.
+
+  @param  Ip6 Points to an IPv6 address.
+
+  @return The byte swapped IPv6 address.
+
+**/
+EFI_IPv6_ADDRESS *
+EFIAPI
+Ip6Swap128 (
+  EFI_IPv6_ADDRESS *Ip6
+  );
+
+extern IP4_ADDR gIp4AllMasks[IP4_MASK_NUM];
+
+
+extern EFI_IPv4_ADDRESS  mZeroIp4Addr;
+
+#define NET_IS_DIGIT(Ch)            (('0' <= (Ch)) && ((Ch) <= '9'))
+#define NET_ROUNDUP(size, unit)     (((size) + (unit) - 1) & (~((unit) - 1)))
+#define NET_IS_LOWER_CASE_CHAR(Ch)  (('a' <= (Ch)) && ((Ch) <= 'z'))
+#define NET_IS_UPPER_CASE_CHAR(Ch)  (('A' <= (Ch)) && ((Ch) <= 'Z'))
+
+#define TICKS_PER_MS            10000U
+#define TICKS_PER_SECOND        10000000U
+
+#define NET_RANDOM(Seed)        ((UINT32) ((UINT32) (Seed) * 1103515245UL + 12345) % 4294967295UL)
+
+/**
+  Extract a UINT32 from a byte stream.
+
+  This function copies a UINT32 from a byte stream, and then converts it from Network
+  byte order to host byte order. Use this function to avoid alignment error.
+
+  @param[in]  Buf                 The buffer to extract the UINT32.
+
+  @return The UINT32 extracted.
+
+**/
+UINT32
+EFIAPI
+NetGetUint32 (
+  IN UINT8                  *Buf
+  );
+
+/**
+  Puts a UINT32 into the byte stream in network byte order.
+
+  Converts a UINT32 from host byte order to network byte order, then copies it to the
+  byte stream.
+
+  @param[in, out]  Buf          The buffer in which to put the UINT32.
+  @param[in]       Data         The data to be converted and put into the byte stream.
+
+**/
+VOID
+EFIAPI
+NetPutUint32 (
+  IN OUT UINT8                 *Buf,
+  IN     UINT32                Data
+  );
+
+/**
+  Initialize a random seed using current time and monotonic count.
+
+  Get current time and monotonic count first. Then initialize a random seed 
+  based on some basic mathematics operation on the hour, day, minute, second,
+  nanosecond and year of the current time and the monotonic count value.
+
+  @return The random seed initialized with current time.
+
+**/
+UINT32
+EFIAPI
+NetRandomInitSeed (
+  VOID
+  );
+
+
+#define NET_LIST_USER_STRUCT(Entry, Type, Field)        \
+          BASE_CR(Entry, Type, Field)
+
+#define NET_LIST_USER_STRUCT_S(Entry, Type, Field, Sig)  \
+          CR(Entry, Type, Field, Sig)
+
+//
+// Iterate through the double linked list. It is NOT delete safe
+//
+#define NET_LIST_FOR_EACH(Entry, ListHead) \
+  for(Entry = (ListHead)->ForwardLink; Entry != (ListHead); Entry = Entry->ForwardLink)
+
+//
+// Iterate through the double linked list. This is delete-safe.
+// Don't touch NextEntry. Also, don't use this macro if list
+// entries other than the Entry may be deleted when processing
+// the current Entry.
+//
+#define NET_LIST_FOR_EACH_SAFE(Entry, NextEntry, ListHead) \
+  for(Entry = (ListHead)->ForwardLink, NextEntry = Entry->ForwardLink; \
+      Entry != (ListHead); \
+      Entry = NextEntry, NextEntry = Entry->ForwardLink \
+     )
+
+//
+// Make sure the list isn't empty before getting the first/last record.
+//
+#define NET_LIST_HEAD(ListHead, Type, Field)  \
+          NET_LIST_USER_STRUCT((ListHead)->ForwardLink, Type, Field)
+
+#define NET_LIST_TAIL(ListHead, Type, Field)  \
+          NET_LIST_USER_STRUCT((ListHead)->BackLink, Type, Field)
+
+
+/**
+  Remove the first node entry on the list, and return the removed node entry.
+
+  Removes the first node entry from a doubly linked list. It is up to the caller of
+  this function to release the memory used by the first node, if that is required. On
+  exit, the removed node is returned.
+
+  If Head is NULL, then ASSERT().
+  If Head was not initialized, then ASSERT().
+  If PcdMaximumLinkedListLength is not zero, and the number of nodes in the
+  linked list including the head node is greater than or equal to PcdMaximumLinkedListLength,
+  then ASSERT().
+
+  @param[in, out]  Head                  The list header.
+
+  @return The first node entry that is removed from the list, NULL if the list is empty.
+
+**/
+LIST_ENTRY *
+EFIAPI
+NetListRemoveHead (
+  IN OUT LIST_ENTRY            *Head
+  );
+
+/**
+  Remove the last node entry on the list and return the removed node entry.
+
+  Removes the last node entry from a doubly linked list. It is up to the caller of
+  this function to release the memory used by the first node, if that is required. On
+  exit, the removed node is returned.
+
+  If Head is NULL, then ASSERT().
+  If Head was not initialized, then ASSERT().
+  If PcdMaximumLinkedListLength is not zero, and the number of nodes in the
+  linked list including the head node is greater than or equal to PcdMaximumLinkedListLength,
+  then ASSERT().
+
+  @param[in, out]  Head                  The list head.
+
+  @return The last node entry that is removed from the list, NULL if the list is empty.
+
+**/
+LIST_ENTRY *
+EFIAPI
+NetListRemoveTail (
+  IN OUT LIST_ENTRY            *Head
+  );
+
+/**
+  Insert a new node entry after a designated node entry of a doubly linked list.
+
+  Inserts a new node entry designated by NewEntry after the node entry designated by PrevEntry
+  of the doubly linked list.
+
+  @param[in, out]  PrevEntry             The entry after which to insert.
+  @param[in, out]  NewEntry              The new entry to insert.
+
+**/
+VOID
+EFIAPI
+NetListInsertAfter (
+  IN OUT LIST_ENTRY         *PrevEntry,
+  IN OUT LIST_ENTRY         *NewEntry
+  );
+
+/**
+  Insert a new node entry before a designated node entry of a doubly linked list.
+
+  Inserts a new node entry designated by NewEntry before the node entry designated by PostEntry
+  of the doubly linked list.
+
+  @param[in, out]  PostEntry             The entry to insert before.
+  @param[in, out]  NewEntry              The new entry to insert.
+
+**/
+VOID
+EFIAPI
+NetListInsertBefore (
+  IN OUT LIST_ENTRY     *PostEntry,
+  IN OUT LIST_ENTRY     *NewEntry
+  );
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *NET_DESTROY_LINK_LIST_CALLBACK) (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context   OPTIONAL
+  );
+
+/**
+  Safe destroy nodes in a linked list, and return the length of the list after all possible operations finished.
+
+  Destroy network children list by list traversals is not safe due to graph dependencies between nodes.
+  This function performs a safe traversal to destroy these nodes by checking to see if the node being destroyed
+  has been removed from the list or not.
+  If it has been removed, then restart the traversal from the head.
+  If it hasn't been removed, then continue with the next node directly.
+  This function will end the iterate and return the CallBack's last return value if error happens,
+  or retrun EFI_SUCCESS if 2 complete passes are made with no changes in the number of children in the list.  
+
+  @param[in]    List             The head of the list.
+  @param[in]    CallBack         Pointer to the callback function to destroy one node in the list.
+  @param[in]    Context          Pointer to the callback function's context: corresponds to the
+                                 parameter Context in NET_DESTROY_LINK_LIST_CALLBACK.
+  @param[out]   ListLength       The length of the link list if the function returns successfully.
+
+  @retval EFI_SUCCESS            Two complete passes are made with no changes in the number of children.
+  @retval EFI_INVALID_PARAMETER  The input parameter is invalid.
+  @retval Others                 Return the CallBack's last return value.
+
+**/
+EFI_STATUS
+EFIAPI
+NetDestroyLinkList (
+  IN   LIST_ENTRY                       *List,
+  IN   NET_DESTROY_LINK_LIST_CALLBACK   CallBack,
+  IN   VOID                             *Context,    OPTIONAL
+  OUT  UINTN                            *ListLength  OPTIONAL
+  );
+
+/**
+  This function checks the input Handle to see if it's one of these handles in ChildHandleBuffer.
+
+  @param[in]  Handle             Handle to be checked.
+  @param[in]  NumberOfChildren   Number of Handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer  An array of child handles to be freed. May be NULL
+                                 if NumberOfChildren is 0.
+
+  @retval TURE                   Found the input Handle in ChildHandleBuffer.
+  @retval FALSE                  Can't find the input Handle in ChildHandleBuffer.
+
+**/
+BOOLEAN
+EFIAPI
+NetIsInHandleBuffer (
+  IN  EFI_HANDLE          Handle,
+  IN  UINTN               NumberOfChildren,
+  IN  EFI_HANDLE          *ChildHandleBuffer OPTIONAL
+  );
+
+//
+// Object container: EFI network stack spec defines various kinds of
+// tokens. The drivers can share code to manage those objects.
+//
+typedef struct {
+  LIST_ENTRY                Link;
+  VOID                      *Key;
+  VOID                      *Value;
+} NET_MAP_ITEM;
+
+typedef struct {
+  LIST_ENTRY                Used;
+  LIST_ENTRY                Recycled;
+  UINTN                     Count;
+} NET_MAP;
+
+#define NET_MAP_INCREAMENT  64
+
+/**
+  Initialize the netmap. Netmap is a reposity to keep the <Key, Value> pairs.
+
+  Initialize the forward and backward links of two head nodes donated by Map->Used
+  and Map->Recycled of two doubly linked lists.
+  Initializes the count of the <Key, Value> pairs in the netmap to zero.
+
+  If Map is NULL, then ASSERT().
+  If the address of Map->Used is NULL, then ASSERT().
+  If the address of Map->Recycled is NULl, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to initialize.
+
+**/
+VOID
+EFIAPI
+NetMapInit (
+  IN OUT NET_MAP                *Map
+  );
+
+/**
+  To clean up the netmap, that is, release allocated memories.
+
+  Removes all nodes of the Used doubly linked list and frees memory of all related netmap items.
+  Removes all nodes of the Recycled doubly linked list and free memory of all related netmap items.
+  The number of the <Key, Value> pairs in the netmap is set to zero.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to clean up.
+
+**/
+VOID
+EFIAPI
+NetMapClean (
+  IN OUT NET_MAP            *Map
+  );
+
+/**
+  Test whether the netmap is empty and return true if it is.
+
+  If the number of the <Key, Value> pairs in the netmap is zero, return TRUE.
+
+  If Map is NULL, then ASSERT().
+
+
+  @param[in]  Map                   The net map to test.
+
+  @return TRUE if the netmap is empty, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetMapIsEmpty (
+  IN NET_MAP                *Map
+  );
+
+/**
+  Return the number of the <Key, Value> pairs in the netmap.
+
+  @param[in]  Map                   The netmap to get the entry number.
+
+  @return The entry number in the netmap.
+
+**/
+UINTN
+EFIAPI
+NetMapGetCount (
+  IN NET_MAP                *Map
+  );
+
+/**
+  Allocate an item to save the <Key, Value> pair to the head of the netmap.
+
+  Allocate an item to save the <Key, Value> pair and add corresponding node entry
+  to the beginning of the Used doubly linked list. The number of the <Key, Value>
+  pairs in the netmap increase by 1.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to insert into.
+  @param[in]       Key                   The user's key.
+  @param[in]       Value                 The user's value for the key.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the item.
+  @retval EFI_SUCCESS           The item is inserted to the head.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapInsertHead (
+  IN OUT NET_MAP            *Map,
+  IN VOID                   *Key,
+  IN VOID                   *Value    OPTIONAL
+  );
+
+/**
+  Allocate an item to save the <Key, Value> pair to the tail of the netmap.
+
+  Allocate an item to save the <Key, Value> pair and add corresponding node entry
+  to the tail of the Used doubly linked list. The number of the <Key, Value>
+  pairs in the netmap increase by 1.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to insert into.
+  @param[in]       Key                   The user's key.
+  @param[in]       Value                 The user's value for the key.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the item.
+  @retval EFI_SUCCESS           The item is inserted to the tail.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapInsertTail (
+  IN OUT NET_MAP            *Map,
+  IN VOID                   *Key,
+  IN VOID                   *Value    OPTIONAL
+  );
+
+/**
+  Finds the key in the netmap and returns the point to the item containing the Key.
+
+  Iterate the Used doubly linked list of the netmap to get every item. Compare the key of every
+  item with the key to search. It returns the point to the item contains the Key if found.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in]  Map                   The netmap to search within.
+  @param[in]  Key                   The key to search.
+
+  @return The point to the item contains the Key, or NULL if Key isn't in the map.
+
+**/
+NET_MAP_ITEM *
+EFIAPI
+NetMapFindKey (
+  IN  NET_MAP               *Map,
+  IN  VOID                  *Key
+  );
+
+/**
+  Remove the node entry of the item from the netmap and return the key of the removed item.
+
+  Remove the node entry of the item from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry of the item to the Recycled doubly linked list of the netmap. If Value is not NULL,
+  Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If Item is NULL, then ASSERT().
+  if item in not in the netmap, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the item from.
+  @param[in, out]  Item                  The item to remove.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the removed item.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveItem (
+  IN  OUT NET_MAP             *Map,
+  IN  OUT NET_MAP_ITEM        *Item,
+  OUT VOID                    **Value           OPTIONAL
+  );
+
+/**
+  Remove the first node entry on the netmap and return the key of the removed item.
+
+  Remove the first node entry from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry to the Recycled doubly linked list of the netmap. If parameter Value is not NULL,
+  parameter Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If the Used doubly linked list is empty, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the head from.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the item removed.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveHead (
+  IN OUT NET_MAP            *Map,
+  OUT VOID                  **Value         OPTIONAL
+  );
+
+/**
+  Remove the last node entry on the netmap and return the key of the removed item.
+
+  Remove the last node entry from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry to the Recycled doubly linked list of the netmap. If parameter Value is not NULL,
+  parameter Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If the Used doubly linked list is empty, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the tail from.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the item removed.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveTail (
+  IN OUT NET_MAP            *Map,
+  OUT VOID                  **Value       OPTIONAL
+  );
+
+typedef
+EFI_STATUS
+(EFIAPI *NET_MAP_CALLBACK) (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Arg
+  );
+
+/**
+  Iterate through the netmap and call CallBack for each item.
+
+  It will contiue the traverse if CallBack returns EFI_SUCCESS, otherwise, break
+  from the loop. It returns the CallBack's last return value. This function is
+  delete safe for the current item.
+
+  If Map is NULL, then ASSERT().
+  If CallBack is NULL, then ASSERT().
+
+  @param[in]  Map                   The Map to iterate through.
+  @param[in]  CallBack              The callback function to call for each item.
+  @param[in]  Arg                   The opaque parameter to the callback.
+
+  @retval EFI_SUCCESS            There is no item in the netmap, or CallBack for each item
+                                 returns EFI_SUCCESS.
+  @retval Others                 It returns the CallBack's last return value.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapIterate (
+  IN NET_MAP                *Map,
+  IN NET_MAP_CALLBACK       CallBack,
+  IN VOID                   *Arg      OPTIONAL
+  );
+
+
+//
+// Helper functions to implement driver binding and service binding protocols.
+//
+/**
+  Create a child of the service that is identified by ServiceBindingGuid.
+
+  Get the ServiceBinding Protocol first, then use it to create a child.
+
+  If ServiceBindingGuid is NULL, then ASSERT().
+  If ChildHandle is NULL, then ASSERT().
+
+  @param[in]       Controller            The controller which has the service installed.
+  @param[in]       Image                 The image handle used to open service.
+  @param[in]       ServiceBindingGuid    The service's Guid.
+  @param[in, out]  ChildHandle           The handle to receive the created child.
+
+  @retval EFI_SUCCESS           The child was successfully created.
+  @retval Others                Failed to create the child.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibCreateServiceChild (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            Image,
+  IN  EFI_GUID              *ServiceBindingGuid,
+  IN  OUT EFI_HANDLE        *ChildHandle
+  );
+
+/**
+  Destroy a child of the service that is identified by ServiceBindingGuid.
+
+  Get the ServiceBinding Protocol first, then use it to destroy a child.
+
+  If ServiceBindingGuid is NULL, then ASSERT().
+
+  @param[in]   Controller            The controller which has the service installed.
+  @param[in]   Image                 The image handle used to open service.
+  @param[in]   ServiceBindingGuid    The service's Guid.
+  @param[in]   ChildHandle           The child to destroy.
+
+  @retval EFI_SUCCESS           The child was destroyed.
+  @retval Others                Failed to destroy the child.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDestroyServiceChild (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            Image,
+  IN  EFI_GUID              *ServiceBindingGuid,
+  IN  EFI_HANDLE            ChildHandle
+  );
+
+/**
+  Get handle with Simple Network Protocol installed on it.
+
+  There should be MNP Service Binding Protocol installed on the input ServiceHandle.
+  If Simple Network Protocol is already installed on the ServiceHandle, the
+  ServiceHandle will be returned. If SNP is not installed on the ServiceHandle,
+  try to find its parent handle with SNP installed.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+  @param[out]  Snp              The pointer to store the address of the SNP instance.
+                                This is an optional parameter that may be NULL.
+
+  @return The SNP handle, or NULL if not found.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetSnpHandle (
+  IN   EFI_HANDLE                  ServiceHandle,
+  OUT  EFI_SIMPLE_NETWORK_PROTOCOL **Snp  OPTIONAL
+  );
+
+/**
+  Retrieve VLAN ID of a VLAN device handle.
+
+  Search VLAN device path node in Device Path of specified ServiceHandle and
+  return its VLAN ID. If no VLAN device path node found, then this ServiceHandle
+  is not a VLAN device handle, and 0 will be returned.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+
+  @return VLAN ID of the device handle, or 0 if not a VLAN device.
+
+**/
+UINT16
+EFIAPI
+NetLibGetVlanId (
+  IN EFI_HANDLE             ServiceHandle
+  );
+
+/**
+  Find VLAN device handle with specified VLAN ID.
+
+  The VLAN child device handle is created by VLAN Config Protocol on ControllerHandle.
+  This function will append VLAN device path node to the parent device path,
+  and then use LocateDevicePath() to find the correct VLAN device handle.
+
+  @param[in]   ControllerHandle The handle where network service binding protocols are
+                                installed on.
+  @param[in]   VlanId           The configured VLAN ID for the VLAN device.
+
+  @return The VLAN device handle, or NULL if not found.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetVlanHandle (
+  IN EFI_HANDLE             ControllerHandle,
+  IN UINT16                 VlanId
+  );
+
+/**
+  Get MAC address associated with the network service handle.
+
+  There should be MNP Service Binding Protocol installed on the input ServiceHandle.
+  If SNP is installed on the ServiceHandle or its parent handle, MAC address will
+  be retrieved from SNP. If no SNP found, try to get SNP mode data use MNP.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+  @param[out]  MacAddress       The pointer to store the returned MAC address.
+  @param[out]  AddressSize      The length of returned MAC address.
+
+  @retval EFI_SUCCESS           MAC address was returned successfully.
+  @retval Others                Failed to get SNP mode data.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetMacAddress (
+  IN  EFI_HANDLE            ServiceHandle,
+  OUT EFI_MAC_ADDRESS       *MacAddress,
+  OUT UINTN                 *AddressSize
+  );
+
+/**
+  Convert MAC address of the NIC associated with specified Service Binding Handle
+  to a unicode string. Callers are responsible for freeing the string storage.
+
+  Locate simple network protocol associated with the Service Binding Handle and
+  get the mac address from SNP. Then convert the mac address into a unicode
+  string. It takes 2 unicode characters to represent a 1 byte binary buffer.
+  Plus one unicode character for the null-terminator.
+
+  @param[in]   ServiceHandle         The handle where network service binding protocol is
+                                     installed.
+  @param[in]   ImageHandle           The image handle used to act as the agent handle to
+                                     get the simple network protocol. This parameter is
+                                     optional and may be NULL.
+  @param[out]  MacString             The pointer to store the address of the string
+                                     representation of  the mac address.
+
+  @retval EFI_SUCCESS           Converted the mac address a unicode string successfully.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough memory resources.
+  @retval Others                Failed to open the simple network protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetMacString (
+  IN  EFI_HANDLE            ServiceHandle,
+  IN  EFI_HANDLE            ImageHandle, OPTIONAL
+  OUT CHAR16                **MacString
+  );
+
+/**
+  Detect media status for specified network device.
+
+  The underlying UNDI driver may or may not support reporting media status from
+  GET_STATUS command (PXE_STATFLAGS_GET_STATUS_NO_MEDIA_SUPPORTED). This routine
+  will try to invoke Snp->GetStatus() to get the media status. If media is already
+  present, it returns directly. If media is not present, it will stop SNP and then
+  restart SNP to get the latest media status. This provides an opportunity to get 
+  the correct media status for old UNDI driver, which doesn't support reporting 
+  media status from GET_STATUS command.
+  Note: there are two limitations for the current algorithm:
+  1) For UNDI with this capability, when the cable is not attached, there will
+     be an redundant Stop/Start() process.
+  2) for UNDI without this capability, in case that network cable is attached when
+     Snp->Initialize() is invoked while network cable is unattached later,
+     NetLibDetectMedia() will report MediaPresent as TRUE, causing upper layer
+     apps to wait for timeout time.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed.
+  @param[out]  MediaPresent     The pointer to store the media status.
+
+  @retval EFI_SUCCESS           Media detection success.
+  @retval EFI_INVALID_PARAMETER ServiceHandle is not a valid network device handle.
+  @retval EFI_UNSUPPORTED       The network device does not support media detection.
+  @retval EFI_DEVICE_ERROR      SNP is in an unknown state.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDetectMedia (
+  IN  EFI_HANDLE            ServiceHandle,
+  OUT BOOLEAN               *MediaPresent
+  );
+
+/**
+  Create an IPv4 device path node.
+
+  The header type of IPv4 device path node is MESSAGING_DEVICE_PATH.
+  The header subtype of IPv4 device path node is MSG_IPv4_DP.
+  The length of the IPv4 device path node in bytes is 19.
+  Get other information from parameters to make up the whole IPv4 device path node.
+
+  @param[in, out]  Node                  The pointer to the IPv4 device path node.
+  @param[in]       Controller            The controller handle.
+  @param[in]       LocalIp               The local IPv4 address.
+  @param[in]       LocalPort             The local port.
+  @param[in]       RemoteIp              The remote IPv4 address.
+  @param[in]       RemotePort            The remote port.
+  @param[in]       Protocol              The protocol type in the IP header.
+  @param[in]       UseDefaultAddress     Whether this instance is using default address or not.
+
+**/
+VOID
+EFIAPI
+NetLibCreateIPv4DPathNode (
+  IN OUT IPv4_DEVICE_PATH  *Node,
+  IN EFI_HANDLE            Controller,
+  IN IP4_ADDR              LocalIp,
+  IN UINT16                LocalPort,
+  IN IP4_ADDR              RemoteIp,
+  IN UINT16                RemotePort,
+  IN UINT16                Protocol,
+  IN BOOLEAN               UseDefaultAddress
+  );
+
+/**
+  Create an IPv6 device path node.
+
+  The header type of IPv6 device path node is MESSAGING_DEVICE_PATH.
+  The header subtype of IPv6 device path node is MSG_IPv6_DP.
+  The length of the IPv6 device path node in bytes is 43.
+  Get other information from parameters to make up the whole IPv6 device path node.
+
+  @param[in, out]  Node                  The pointer to the IPv6 device path node.
+  @param[in]       Controller            The controller handle.
+  @param[in]       LocalIp               The local IPv6 address.
+  @param[in]       LocalPort             The local port.
+  @param[in]       RemoteIp              The remote IPv6 address.
+  @param[in]       RemotePort            The remote port.
+  @param[in]       Protocol              The protocol type in the IP header.
+
+**/
+VOID
+EFIAPI
+NetLibCreateIPv6DPathNode (
+  IN OUT IPv6_DEVICE_PATH  *Node,
+  IN EFI_HANDLE            Controller,
+  IN EFI_IPv6_ADDRESS      *LocalIp,
+  IN UINT16                LocalPort,
+  IN EFI_IPv6_ADDRESS      *RemoteIp,
+  IN UINT16                RemotePort,
+  IN UINT16                Protocol
+  );
+
+
+/**
+  Find the UNDI/SNP handle from controller and protocol GUID.
+
+  For example, IP will open an MNP child to transmit/receive
+  packets. When MNP is stopped, IP should also be stopped. IP
+  needs to find its own private data that is related the IP's
+  service binding instance that is installed on the UNDI/SNP handle.
+  The controller is then either an MNP or an ARP child handle. Note that
+  IP opens these handles using BY_DRIVER. Use that infomation to get the
+  UNDI/SNP handle.
+
+  @param[in]  Controller            The protocol handle to check.
+  @param[in]  ProtocolGuid          The protocol that is related with the handle.
+
+  @return The UNDI/SNP handle or NULL for errors.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetNicHandle (
+  IN EFI_HANDLE             Controller,
+  IN EFI_GUID               *ProtocolGuid
+  );
+
+/**
+  This is the default unload handle for all the network drivers.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in]  ImageHandle       The drivers' driver image.
+
+  @retval EFI_SUCCESS           The image is unloaded.
+  @retval Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDefaultUnload (
+  IN EFI_HANDLE             ImageHandle
+  );
+
+/**
+  Convert one Null-terminated ASCII string (decimal dotted) to EFI_IPv4_ADDRESS.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip4Address     The pointer to the converted IPv4 address.
+
+  @retval EFI_SUCCESS            Converted to an IPv4 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is malformated, or Ip4Address is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibAsciiStrToIp4 (
+  IN CONST CHAR8                 *String,
+  OUT      EFI_IPv4_ADDRESS      *Ip4Address
+  );
+
+/**
+  Convert one Null-terminated ASCII string to EFI_IPv6_ADDRESS. The format of the
+  string is defined in RFC 4291 - Text Pepresentation of Addresses.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+
+  @retval EFI_SUCCESS            Converted to an IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is malformated, or Ip6Address is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibAsciiStrToIp6 (
+  IN CONST CHAR8                 *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address
+  );
+
+/**
+  Convert one Null-terminated Unicode string (decimal dotted) to EFI_IPv4_ADDRESS.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip4Address     The pointer to the converted IPv4 address.
+
+  @retval EFI_SUCCESS            Converted to an IPv4 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip4Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Failed to perform the operation due to lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp4 (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv4_ADDRESS      *Ip4Address
+  );
+
+/**
+  Convert one Null-terminated Unicode string to EFI_IPv6_ADDRESS.  The format of
+  the string is defined in RFC 4291 - Text Pepresentation of Addresses.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+
+  @retval EFI_SUCCESS            Converted to an IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is malformated or Ip6Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Failed to perform the operation due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp6 (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address
+  );
+
+/**
+  Convert one Null-terminated Unicode string to EFI_IPv6_ADDRESS and prefix length.
+  The format of the string is defined in RFC 4291 - Text Pepresentation of Addresses
+  Prefixes: ipv6-address/prefix-length.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+  @param[out]     PrefixLength   The pointer to the converted prefix length.
+
+  @retval EFI_SUCCESS            Converted to an  IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is malformated, or Ip6Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Failed to perform the operation due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp6andPrefix (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address,
+  OUT      UINT8                 *PrefixLength
+  );
+
+/**
+
+  Convert one EFI_IPv6_ADDRESS to Null-terminated Unicode string.
+  The text representation of address is defined in RFC 4291.
+  
+  @param[in]       Ip6Address     The pointer to the IPv6 address.
+  @param[out]      String         The buffer to return the converted string.
+  @param[in]       StringSize     The length in bytes of the input String.
+                                  
+  @retval EFI_SUCCESS             Convert to string successfully.
+  @retval EFI_INVALID_PARAMETER   The input parameter is invalid.
+  @retval EFI_BUFFER_TOO_SMALL    The BufferSize is too small for the result. BufferSize has been 
+                                  updated with the size needed to complete the request.
+**/
+EFI_STATUS
+EFIAPI
+NetLibIp6ToStr (
+  IN         EFI_IPv6_ADDRESS      *Ip6Address,
+  OUT        CHAR16                *String,
+  IN         UINTN                 StringSize
+  );
+
+//
+// Various signatures
+//
+#define  NET_BUF_SIGNATURE    SIGNATURE_32 ('n', 'b', 'u', 'f')
+#define  NET_VECTOR_SIGNATURE SIGNATURE_32 ('n', 'v', 'e', 'c')
+#define  NET_QUE_SIGNATURE    SIGNATURE_32 ('n', 'b', 'q', 'u')
+
+
+#define  NET_PROTO_DATA       64   // Opaque buffer for protocols
+#define  NET_BUF_HEAD         1    // Trim or allocate space from head
+#define  NET_BUF_TAIL         0    // Trim or allocate space from tail
+#define  NET_VECTOR_OWN_FIRST 0x01  // We allocated the 1st block in the vector
+
+#define NET_CHECK_SIGNATURE(PData, SIGNATURE) \
+  ASSERT (((PData) != NULL) && ((PData)->Signature == (SIGNATURE)))
+
+//
+// Single memory block in the vector.
+//
+typedef struct {
+  UINT32              Len;        // The block's length
+  UINT8               *Bulk;      // The block's Data
+} NET_BLOCK;
+
+typedef VOID (EFIAPI *NET_VECTOR_EXT_FREE) (VOID *Arg);
+
+//
+//NET_VECTOR contains several blocks to hold all packet's
+//fragments and other house-keeping stuff for sharing. It
+//doesn't specify the where actual packet fragment begins.
+//
+typedef struct {
+  UINT32              Signature;
+  INTN                RefCnt;  // Reference count to share NET_VECTOR.
+  NET_VECTOR_EXT_FREE Free;    // external function to free NET_VECTOR
+  VOID                *Arg;    // opeque argument to Free
+  UINT32              Flag;    // Flags, NET_VECTOR_OWN_FIRST
+  UINT32              Len;     // Total length of the assocated BLOCKs
+
+  UINT32              BlockNum;
+  NET_BLOCK           Block[1];
+} NET_VECTOR;
+
+//
+//NET_BLOCK_OP operates on the NET_BLOCK. It specifies
+//where the actual fragment begins and ends
+//
+typedef struct {
+  UINT8               *BlockHead;   // Block's head, or the smallest valid Head
+  UINT8               *BlockTail;   // Block's tail. BlockTail-BlockHead=block length
+  UINT8               *Head;        // 1st byte of the data in the block
+  UINT8               *Tail;        // Tail of the data in the block, Tail-Head=Size
+  UINT32              Size;         // The size of the data
+} NET_BLOCK_OP;
+
+typedef union {
+  IP4_HEAD          *Ip4;
+  EFI_IP6_HEADER    *Ip6;
+} NET_IP_HEAD;
+
+//
+//NET_BUF is the buffer manage structure used by the
+//network stack. Every network packet may be fragmented. The Vector points to
+//memory blocks used by each fragment, and BlockOp
+//specifies where each fragment begins and ends.
+//
+//It also contains an opaque area for the protocol to store
+//per-packet information. Protocol must be careful not
+//to overwrite the members after that.
+//
+typedef struct {
+  UINT32         Signature;
+  INTN           RefCnt;
+  LIST_ENTRY     List;                       // The List this NET_BUF is on
+
+  NET_IP_HEAD    Ip;                         // Network layer header, for fast access
+  TCP_HEAD       *Tcp;                       // Transport layer header, for fast access
+  EFI_UDP_HEADER *Udp;                       // User Datagram Protocol header
+  UINT8          ProtoData [NET_PROTO_DATA]; //Protocol specific data
+
+  NET_VECTOR     *Vector;                    // The vector containing the packet
+
+  UINT32         BlockOpNum;                 // Total number of BlockOp in the buffer
+  UINT32         TotalSize;                  // Total size of the actual packet
+  NET_BLOCK_OP   BlockOp[1];                 // Specify the position of actual packet
+} NET_BUF;
+
+//
+//A queue of NET_BUFs. It is a thin extension of
+//NET_BUF functions.
+//
+typedef struct {
+  UINT32              Signature;
+  INTN                RefCnt;
+  LIST_ENTRY          List;       // The List this buffer queue is on
+
+  LIST_ENTRY          BufList;    // list of queued buffers
+  UINT32              BufSize;    // total length of DATA in the buffers
+  UINT32              BufNum;     // total number of buffers on the chain
+} NET_BUF_QUEUE;
+
+//
+// Pseudo header for TCP and UDP checksum
+//
+#pragma pack(1)
+typedef struct {
+  IP4_ADDR            SrcIp;
+  IP4_ADDR            DstIp;
+  UINT8               Reserved;
+  UINT8               Protocol;
+  UINT16              Len;
+} NET_PSEUDO_HDR;
+
+typedef struct {
+  EFI_IPv6_ADDRESS    SrcIp;
+  EFI_IPv6_ADDRESS    DstIp;
+  UINT32              Len;
+  UINT32              Reserved:24;
+  UINT32              NextHeader:8;
+} NET_IP6_PSEUDO_HDR;
+#pragma pack()
+
+//
+// The fragment entry table used in network interfaces. This is
+// the same as NET_BLOCK now. Use two different to distinguish
+// the two in case that NET_BLOCK be enhanced later.
+//
+typedef struct {
+  UINT32              Len;
+  UINT8               *Bulk;
+} NET_FRAGMENT;
+
+#define NET_GET_REF(PData)      ((PData)->RefCnt++)
+#define NET_PUT_REF(PData)      ((PData)->RefCnt--)
+#define NETBUF_FROM_PROTODATA(Info) BASE_CR((Info), NET_BUF, ProtoData)
+
+#define NET_BUF_SHARED(Buf) \
+  (((Buf)->RefCnt > 1) || ((Buf)->Vector->RefCnt > 1))
+
+#define NET_VECTOR_SIZE(BlockNum) \
+  (sizeof (NET_VECTOR) + ((BlockNum) - 1) * sizeof (NET_BLOCK))
+
+#define NET_BUF_SIZE(BlockOpNum)  \
+  (sizeof (NET_BUF) + ((BlockOpNum) - 1) * sizeof (NET_BLOCK_OP))
+
+#define NET_HEADSPACE(BlockOp)  \
+  (UINTN)((BlockOp)->Head - (BlockOp)->BlockHead)
+
+#define NET_TAILSPACE(BlockOp)  \
+  (UINTN)((BlockOp)->BlockTail - (BlockOp)->Tail)
+
+/**
+  Allocate a single block NET_BUF. Upon allocation, all the
+  free space is in the tail room.
+
+  @param[in]  Len              The length of the block.
+
+  @return                      The pointer to the allocated NET_BUF, or NULL if the
+                               allocation failed due to resource limitations.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufAlloc (
+  IN UINT32                 Len
+  );
+
+/**
+  Free the net buffer and its associated NET_VECTOR.
+
+  Decrease the reference count of the net buffer by one. Free the associated net
+  vector and itself if the reference count of the net buffer is decreased to 0.
+  The net vector free operation decreases the reference count of the net
+  vector by one, and performs the resource free operation when the reference count
+  of the net vector is 0.
+
+  @param[in]  Nbuf                  The pointer to the NET_BUF to be freed.
+
+**/
+VOID
+EFIAPI
+NetbufFree (
+  IN NET_BUF                *Nbuf
+  );
+
+/**
+  Get the index of NET_BLOCK_OP that contains the byte at Offset in the net
+  buffer.
+
+  For example, this function can be used to retrieve the IP header in the packet. It
+  also can be used to get the fragment that contains the byte used
+  mainly by the library implementation itself.
+
+  @param[in]   Nbuf      The pointer to the net buffer.
+  @param[in]   Offset    The offset of the byte.
+  @param[out]  Index     Index of the NET_BLOCK_OP that contains the byte at
+                         Offset.
+
+  @return       The pointer to the Offset'th byte of data in the net buffer, or NULL
+                if there is no such data in the net buffer.
+
+**/
+UINT8  *
+EFIAPI
+NetbufGetByte (
+  IN  NET_BUF               *Nbuf,
+  IN  UINT32                Offset,
+  OUT UINT32                *Index  OPTIONAL
+  );
+
+/**
+  Create a copy of the net buffer that shares the associated net vector.
+
+  The reference count of the newly created net buffer is set to 1. The reference
+  count of the associated net vector is increased by one.
+
+  @param[in]  Nbuf              The pointer to the net buffer to be cloned.
+
+  @return                       The pointer to the cloned net buffer, or NULL if the
+                                allocation failed due to resource limitations.
+
+**/
+NET_BUF *
+EFIAPI
+NetbufClone (
+  IN NET_BUF                *Nbuf
+  );
+
+/**
+  Create a duplicated copy of the net buffer with data copied and HeadSpace
+  bytes of head space reserved.
+
+  The duplicated net buffer will allocate its own memory to hold the data of the
+  source net buffer.
+
+  @param[in]       Nbuf         The pointer to the net buffer to be duplicated from.
+  @param[in, out]  Duplicate    The pointer to the net buffer to duplicate to. If
+                                NULL, a new net buffer is allocated.
+  @param[in]      HeadSpace     The length of the head space to reserve.
+
+  @return                       The pointer to the duplicated net buffer, or NULL if
+                                the allocation failed due to resource limitations.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufDuplicate (
+  IN NET_BUF                *Nbuf,
+  IN OUT NET_BUF            *Duplicate        OPTIONAL,
+  IN UINT32                 HeadSpace
+  );
+
+/**
+  Create a NET_BUF structure which contains Len byte data of Nbuf starting from
+  Offset.
+
+  A new NET_BUF structure will be created but the associated data in NET_VECTOR
+  is shared. This function exists to perform IP packet fragmentation.
+
+  @param[in]  Nbuf         The pointer to the net buffer to be extracted.
+  @param[in]  Offset       Starting point of the data to be included in the new
+                           net buffer.
+  @param[in]  Len          The bytes of data to be included in the new net buffer.
+  @param[in]  HeadSpace    The bytes of the head space to reserve for the protocol header.
+
+  @return                  The pointer to the cloned net buffer, or NULL if the
+                           allocation failed due to resource limitations.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufGetFragment (
+  IN NET_BUF                *Nbuf,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  IN UINT32                 HeadSpace
+  );
+
+/**
+  Reserve some space in the header room of the net buffer.
+
+  Upon allocation, all the space is in the tail room of the buffer. Call this
+  function to move space to the header room. This function is quite limited
+  in that it can only reserve space from the first block of an empty NET_BUF not
+  built from the external. However, it should be enough for the network stack.
+
+  @param[in, out]  Nbuf     The pointer to the net buffer.
+  @param[in]       Len      The length of buffer to be reserved from the header.
+
+**/
+VOID
+EFIAPI
+NetbufReserve (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len
+  );
+
+/**
+  Allocate Len bytes of space from the header or tail of the buffer.
+
+  @param[in, out]  Nbuf       The pointer to the net buffer.
+  @param[in]       Len        The length of the buffer to be allocated.
+  @param[in]       FromHead   The flag to indicate whether to reserve the data
+                              from head (TRUE) or tail (FALSE).
+
+  @return                     The pointer to the first byte of the allocated buffer,
+                              or NULL, if there is no sufficient space.
+
+**/
+UINT8*
+EFIAPI
+NetbufAllocSpace (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len,
+  IN BOOLEAN                FromHead
+  );
+
+/**
+  Trim Len bytes from the header or the tail of the net buffer.
+
+  @param[in, out]  Nbuf         The pointer to the net buffer.
+  @param[in]       Len          The length of the data to be trimmed.
+  @param[in]      FromHead      The flag to indicate whether trim data is from the 
+                                head (TRUE) or the tail (FALSE).
+
+  @return    The length of the actual trimmed data, which may be less
+             than Len if the TotalSize of Nbuf is less than Len.
+
+**/
+UINT32
+EFIAPI
+NetbufTrim (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len,
+  IN BOOLEAN                FromHead
+  );
+
+/**
+  Copy Len bytes of data from the specific offset of the net buffer to the
+  destination memory.
+
+  The Len bytes of data may cross several fragments of the net buffer.
+
+  @param[in]   Nbuf         The pointer to the net buffer.
+  @param[in]   Offset       The sequence number of the first byte to copy.
+  @param[in]   Len          The length of the data to copy.
+  @param[in]   Dest         The destination of the data to copy to.
+
+  @return           The length of the actual copied data, or 0 if the offset
+                    specified exceeds the total size of net buffer.
+
+**/
+UINT32
+EFIAPI
+NetbufCopy (
+  IN NET_BUF                *Nbuf,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  IN UINT8                  *Dest
+  );
+
+/**
+  Build a NET_BUF from external blocks.
+
+  A new NET_BUF structure will be created from external blocks. An additional block
+  of memory will be allocated to hold reserved HeadSpace bytes of header room
+  and existing HeadLen bytes of header, but the external blocks are shared by the
+  net buffer to avoid data copying.
+
+  @param[in]  ExtFragment           The pointer to the data block.
+  @param[in]  ExtNum                The number of the data blocks.
+  @param[in]  HeadSpace             The head space to be reserved.
+  @param[in]  HeadLen               The length of the protocol header. The function
+                                    pulls this amount of data into a linear block.
+  @param[in]  ExtFree               The pointer to the caller-provided free function.
+  @param[in]  Arg                   The argument passed to ExtFree when ExtFree is
+                                    called.
+
+  @return                  The pointer to the net buffer built from the data blocks,
+                           or NULL if the allocation failed due to resource
+                           limit.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufFromExt (
+  IN NET_FRAGMENT           *ExtFragment,
+  IN UINT32                 ExtNum,
+  IN UINT32                 HeadSpace,
+  IN UINT32                 HeadLen,
+  IN NET_VECTOR_EXT_FREE    ExtFree,
+  IN VOID                   *Arg          OPTIONAL
+  );
+
+/**
+  Build a fragment table to contain the fragments in the net buffer. This is the
+  opposite operation of the NetbufFromExt.
+
+  @param[in]       Nbuf                  Points to the net buffer.
+  @param[in, out]  ExtFragment           The pointer to the data block.
+  @param[in, out]  ExtNum                The number of the data blocks.
+
+  @retval EFI_BUFFER_TOO_SMALL  The number of non-empty blocks is bigger than
+                                ExtNum.
+  @retval EFI_SUCCESS           The fragment table was built successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NetbufBuildExt (
+  IN NET_BUF                *Nbuf,
+  IN OUT NET_FRAGMENT       *ExtFragment,
+  IN OUT UINT32             *ExtNum
+  );
+
+/**
+  Build a net buffer from a list of net buffers.
+
+  All the fragments will be collected from the list of NEW_BUF, and then a new
+  net buffer will be created through NetbufFromExt.
+
+  @param[in]   BufList    A List of the net buffer.
+  @param[in]   HeadSpace  The head space to be reserved.
+  @param[in]   HeaderLen  The length of the protocol header. The function
+                          pulls this amount of data into a linear block.
+  @param[in]   ExtFree    The pointer to the caller provided free function.
+  @param[in]   Arg        The argument passed to ExtFree when ExtFree is called.
+
+  @return                 The pointer to the net buffer built from the list of net
+                          buffers.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufFromBufList (
+  IN LIST_ENTRY             *BufList,
+  IN UINT32                 HeadSpace,
+  IN UINT32                 HeaderLen,
+  IN NET_VECTOR_EXT_FREE    ExtFree,
+  IN VOID                   *Arg              OPTIONAL
+  );
+
+/**
+  Free a list of net buffers.
+
+  @param[in, out]  Head              The pointer to the head of linked net buffers.
+
+**/
+VOID
+EFIAPI
+NetbufFreeList (
+  IN OUT LIST_ENTRY         *Head
+  );
+
+/**
+  Initiate the net buffer queue.
+
+  @param[in, out]  NbufQue   The pointer to the net buffer queue to be initialized.
+
+**/
+VOID
+EFIAPI
+NetbufQueInit (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  );
+
+/**
+  Allocate and initialize a net buffer queue.
+
+  @return         The pointer to the allocated net buffer queue, or NULL if the
+                  allocation failed due to resource limit.
+
+**/
+NET_BUF_QUEUE  *
+EFIAPI
+NetbufQueAlloc (
+  VOID
+  );
+
+/**
+  Free a net buffer queue.
+
+  Decrease the reference count of the net buffer queue by one. The real resource
+  free operation isn't performed until the reference count of the net buffer
+  queue is decreased to 0.
+
+  @param[in]  NbufQue               The pointer to the net buffer queue to be freed.
+
+**/
+VOID
+EFIAPI
+NetbufQueFree (
+  IN NET_BUF_QUEUE          *NbufQue
+  );
+
+/**
+  Remove a net buffer from the head in the specific queue and return it.
+
+  @param[in, out]  NbufQue               The pointer to the net buffer queue.
+
+  @return           The pointer to the net buffer removed from the specific queue,
+                    or NULL if there is no net buffer in the specific queue.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufQueRemove (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  );
+
+/**
+  Append a net buffer to the net buffer queue.
+
+  @param[in, out]  NbufQue            The pointer to the net buffer queue.
+  @param[in, out]  Nbuf               The pointer to the net buffer to be appended.
+
+**/
+VOID
+EFIAPI
+NetbufQueAppend (
+  IN OUT NET_BUF_QUEUE          *NbufQue,
+  IN OUT NET_BUF                *Nbuf
+  );
+
+/**
+  Copy Len bytes of data from the net buffer queue at the specific offset to the
+  destination memory.
+
+  The copying operation is the same as NetbufCopy, but applies to the net buffer
+  queue instead of the net buffer.
+
+  @param[in]   NbufQue         The pointer to the net buffer queue.
+  @param[in]   Offset          The sequence number of the first byte to copy.
+  @param[in]   Len             The length of the data to copy.
+  @param[out]  Dest            The destination of the data to copy to.
+
+  @return       The length of the actual copied data, or 0 if the offset
+                specified exceeds the total size of net buffer queue.
+
+**/
+UINT32
+EFIAPI
+NetbufQueCopy (
+  IN NET_BUF_QUEUE          *NbufQue,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  OUT UINT8                 *Dest
+  );
+
+/**
+  Trim Len bytes of data from the buffer queue and free any net buffer
+  that is completely trimmed.
+
+  The trimming operation is the same as NetbufTrim but applies to the net buffer
+  queue instead of the net buffer.
+
+  @param[in, out]  NbufQue               The pointer to the net buffer queue.
+  @param[in]       Len                   The length of the data to trim.
+
+  @return   The actual length of the data trimmed.
+
+**/
+UINT32
+EFIAPI
+NetbufQueTrim (
+  IN OUT NET_BUF_QUEUE      *NbufQue,
+  IN UINT32                 Len
+  );
+
+
+/**
+  Flush the net buffer queue.
+
+  @param[in, out]  NbufQue               The pointer to the queue to be flushed.
+
+**/
+VOID
+EFIAPI
+NetbufQueFlush (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  );
+
+/**
+  Compute the checksum for a bulk of data.
+
+  @param[in]   Bulk                  The pointer to the data.
+  @param[in]   Len                   The length of the data, in bytes.
+
+  @return    The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetblockChecksum (
+  IN UINT8                  *Bulk,
+  IN UINT32                 Len
+  );
+
+/**
+  Add two checksums.
+
+  @param[in]   Checksum1             The first checksum to be added.
+  @param[in]   Checksum2             The second checksum to be added.
+
+  @return         The new checksum.
+
+**/
+UINT16
+EFIAPI
+NetAddChecksum (
+  IN UINT16                 Checksum1,
+  IN UINT16                 Checksum2
+  );
+
+/**
+  Compute the checksum for a NET_BUF.
+
+  @param[in]   Nbuf                  The pointer to the net buffer.
+
+  @return    The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetbufChecksum (
+  IN NET_BUF                *Nbuf
+  );
+
+/**
+  Compute the checksum for TCP/UDP pseudo header.
+
+  Src and Dst are in network byte order, and Len is in host byte order.
+
+  @param[in]   Src                   The source address of the packet.
+  @param[in]   Dst                   The destination address of the packet.
+  @param[in]   Proto                 The protocol type of the packet.
+  @param[in]   Len                   The length of the packet.
+
+  @return   The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetPseudoHeadChecksum (
+  IN IP4_ADDR               Src,
+  IN IP4_ADDR               Dst,
+  IN UINT8                  Proto,
+  IN UINT16                 Len
+  );
+
+/**
+  Compute the checksum for the TCP6/UDP6 pseudo header.
+
+  Src and Dst are in network byte order, and Len is in host byte order.
+
+  @param[in]   Src                   The source address of the packet.
+  @param[in]   Dst                   The destination address of the packet.
+  @param[in]   NextHeader            The protocol type of the packet.
+  @param[in]   Len                   The length of the packet.
+
+  @return   The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetIp6PseudoHeadChecksum (
+  IN EFI_IPv6_ADDRESS       *Src,
+  IN EFI_IPv6_ADDRESS       *Dst,
+  IN UINT8                  NextHeader,
+  IN UINT32                 Len
+  );
+
+/**
+  The function frees the net buffer which allocated by the IP protocol. It releases 
+  only the net buffer and doesn't call the external free function. 
+
+  This function should be called after finishing the process of mIpSec->ProcessExt() 
+  for outbound traffic. The (EFI_IPSEC2_PROTOCOL)->ProcessExt() allocates a new 
+  buffer for the ESP, so there needs a function to free the old net buffer.
+
+  @param[in]  Nbuf       The network buffer to be freed.
+
+**/
+VOID
+NetIpSecNetbufFree (
+  NET_BUF   *Nbuf
+  );
+
+/**
+  This function obtains the system guid from the smbios table.
+
+  @param[out]  SystemGuid     The pointer of the returned system guid.
+
+  @retval EFI_SUCCESS         Successfully obtained the system guid.
+  @retval EFI_NOT_FOUND       Did not find the SMBIOS table.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetSystemGuid (
+  OUT EFI_GUID              *SystemGuid
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/OemHookStatusCodeLib.h b/Core/MdeModulePkg/Include/Library/OemHookStatusCodeLib.h
new file mode 100644
index 0000000000..edd370deb6
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/OemHookStatusCodeLib.h
@@ -0,0 +1,79 @@
+/** @file
+  OEM hook status code library. Platform can implement an instance to 
+  initialize the OEM devices to report status code information.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __OEM_HOOK_STATUSCODE_LIB__
+#define __OEM_HOOK_STATUSCODE_LIB__
+
+/**
+
+  Initialize OEM status code device.
+
+
+  @return    Status of initialization of OEM status code device.
+ 
+**/
+EFI_STATUS
+EFIAPI
+OemHookStatusCodeInitialize (
+  VOID
+  );
+
+/**
+  Report status code to OEM device.
+ 
+  @param  CodeType      Indicates the type of status code being reported.  
+ 
+  @param  Value         Describes the current status of a hardware or software entity.  
+                        This includes both an operation and classification information 
+                        about the class and subclass.
+                        For progress codes, the operation is the current activity. 
+                        For error codes, it is the exception.  For debug codes, 
+                        it is not defined at this time. 
+                        Specific values are discussed in the Intel Platform Innovation 
+                        Framework for EFI Status Code Specification.
+ 
+  @param  Instance      The enumeration of a hardware or software entity within the system.  
+                        A system may contain multiple entities that match a class/subclass 
+                        pairing. 
+                        The instance differentiates between them.  An instance of 0 
+                        indicates that instance information is unavailable, 
+                        not meaningful, or not relevant.  Valid instance numbers 
+                        start with 1.
+
+
+  @param  CallerId      This optional parameter may be used to identify the caller. 
+                        This parameter allows the status code driver to apply 
+                        different rules to different callers. 
+                        Type EFI_GUID is defined in InstallProtocolInterface() 
+                        in the UEFI 2.0 Specification.
+
+
+  @param  Data          This optional parameter may be used to pass additional data.
+ 
+  @return               The function always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+OemHookStatusCodeReport (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId, OPTIONAL
+  IN EFI_STATUS_CODE_DATA     *Data      OPTIONAL
+  );
+
+#endif // __OEM_HOOK_STATUSCODE_LIB__
+
diff --git a/Core/MdeModulePkg/Include/Library/PciHostBridgeLib.h b/Core/MdeModulePkg/Include/Library/PciHostBridgeLib.h
new file mode 100644
index 0000000000..d42e9ecdd7
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/PciHostBridgeLib.h
@@ -0,0 +1,103 @@
+/** @file
+  PCI Host Bridge Library consumed by PciHostBridgeDxe driver returning
+  the platform specific information about the PCI Host Bridge.
+
+  Copyright (c) 2016, 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.
+
+**/
+#ifndef __PCI_HOST_BRIDGE_LIB_H__
+#define __PCI_HOST_BRIDGE_LIB_H__
+
+//
+// (Base > Limit) indicates an aperture is not available.
+//
+typedef struct {
+  UINT64 Base;
+  UINT64 Limit;
+} PCI_ROOT_BRIDGE_APERTURE;
+
+typedef struct {
+  UINT32                   Segment;               ///< Segment number.
+  UINT64                   Supports;              ///< Supported attributes.
+                                                  ///< Refer to EFI_PCI_ATTRIBUTE_xxx used by GetAttributes()
+                                                  ///< and SetAttributes() in EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  UINT64                   Attributes;            ///< Initial attributes.
+                                                  ///< Refer to EFI_PCI_ATTRIBUTE_xxx used by GetAttributes()
+                                                  ///< and SetAttributes() in EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL.
+  BOOLEAN                  DmaAbove4G;            ///< DMA above 4GB memory.
+                                                  ///< Set to TRUE when root bridge supports DMA above 4GB memory.
+  BOOLEAN                  NoExtendedConfigSpace; ///< When FALSE, the root bridge supports
+                                                  ///< Extended (4096-byte) Configuration Space.
+                                                  ///< When TRUE, the root bridge supports
+                                                  ///< 256-byte Configuration Space only.
+  BOOLEAN                  ResourceAssigned;      ///< Resource assignment status of the root bridge.
+                                                  ///< Set to TRUE if Bus/IO/MMIO resources for root bridge have been assigned.
+  UINT64                   AllocationAttributes;  ///< Allocation attributes.
+                                                  ///< Refer to EFI_PCI_HOST_BRIDGE_COMBINE_MEM_PMEM and
+                                                  ///< EFI_PCI_HOST_BRIDGE_MEM64_DECODE used by GetAllocAttributes()
+                                                  ///< in EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.
+  PCI_ROOT_BRIDGE_APERTURE Bus;                   ///< Bus aperture which can be used by the root bridge.
+  PCI_ROOT_BRIDGE_APERTURE Io;                    ///< IO aperture which can be used by the root bridge.
+  PCI_ROOT_BRIDGE_APERTURE Mem;                   ///< MMIO aperture below 4GB which can be used by the root bridge.
+  PCI_ROOT_BRIDGE_APERTURE MemAbove4G;            ///< MMIO aperture above 4GB which can be used by the root bridge.
+  PCI_ROOT_BRIDGE_APERTURE PMem;                  ///< Prefetchable MMIO aperture below 4GB which can be used by the root bridge.
+  PCI_ROOT_BRIDGE_APERTURE PMemAbove4G;           ///< Prefetchable MMIO aperture above 4GB which can be used by the root bridge.
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;           ///< Device path.
+} PCI_ROOT_BRIDGE;
+
+/**
+  Return all the root bridge instances in an array.
+
+  @param Count  Return the count of root bridge instances.
+
+  @return All the root bridge instances in an array.
+          The array should be passed into PciHostBridgeFreeRootBridges()
+          when it's not used.
+**/
+PCI_ROOT_BRIDGE *
+EFIAPI
+PciHostBridgeGetRootBridges (
+  UINTN *Count
+  );
+
+/**
+  Free the root bridge instances array returned from PciHostBridgeGetRootBridges().
+
+  @param Bridges The root bridge instances array.
+  @param Count   The count of the array.
+**/
+VOID
+EFIAPI
+PciHostBridgeFreeRootBridges (
+  PCI_ROOT_BRIDGE *Bridges,
+  UINTN           Count
+  );
+
+/**
+  Inform the platform that the resource conflict happens.
+
+  @param HostBridgeHandle Handle of the Host Bridge.
+  @param Configuration    Pointer to PCI I/O and PCI memory resource descriptors.
+                          The Configuration contains the resources for all the
+                          root bridges. The resource for each root bridge is
+                          terminated with END descriptor and an additional END
+                          is appended indicating the end of the entire resources.
+                          The resource descriptor field values follow the description
+                          in EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL.SubmitResources().
+**/
+VOID
+EFIAPI
+PciHostBridgeResourceConflict (
+  EFI_HANDLE           HostBridgeHandle,
+  VOID                 *Configuration
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/PlatformBootManagerLib.h b/Core/MdeModulePkg/Include/Library/PlatformBootManagerLib.h
new file mode 100644
index 0000000000..52745032e4
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/PlatformBootManagerLib.h
@@ -0,0 +1,62 @@
+/** @file
+  Platform Boot Manager library definition. A platform can implement 
+  instances to support platform-specific behavior.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+
+#ifndef __PLATFORM_BOOT_MANAGER_LIB_H_
+#define __PLATFORM_BOOT_MANAGER_LIB_H_
+#include <Library/UefiBootManagerLib.h>
+
+/**
+  Do the platform specific action before the console is connected.
+
+  Such as:
+    Update console variable;
+    Register new Driver#### or Boot####;
+    Signal ReadyToLock event.
+**/
+VOID
+EFIAPI
+PlatformBootManagerBeforeConsole (
+  VOID
+  );
+
+/**
+  Do the platform specific action after the console is connected.
+
+  Such as:
+    Dynamically switch output mode;
+    Signal console ready platform customized event;
+    Run diagnostics like memory testing;
+    Connect certain devices;
+    Dispatch aditional option roms.
+**/
+VOID
+EFIAPI
+PlatformBootManagerAfterConsole (
+  VOID
+  );
+
+/**
+  This function is called each second during the boot manager waits the timeout.
+
+  @param TimeoutRemain  The remaining timeout.
+**/
+VOID
+EFIAPI
+PlatformBootManagerWaitCallback (
+  UINT16          TimeoutRemain
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/PlatformHookLib.h b/Core/MdeModulePkg/Include/Library/PlatformHookLib.h
new file mode 100644
index 0000000000..0f265d49a3
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/PlatformHookLib.h
@@ -0,0 +1,38 @@
+/** @file
+  Platform hook library. Platform can provide an implementation of this
+  library class to provide hooks that may be required for some type of 
+  platform initialization.
+
+Copyright (c) 2010, 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 that 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.
+
+**/
+
+#ifndef __PLATFORM_HOOK_LIB__
+#define __PLATFORM_HOOK_LIB__
+
+/**
+  Performs platform specific initialization required for the CPU to access
+  the hardware associated with a SerialPortLib instance.  This function does
+  not intiailzie the serial port hardware itself.  Instead, it initializes 
+  hardware devices that are required for the CPU to access the serial port 
+  hardware.  This function may be called more than once.
+
+  @retval RETURN_SUCCESS       The platform specific initialization succeeded.
+  @retval RETURN_DEVICE_ERROR  The platform specific initialization could not be completed.
+ 
+**/
+RETURN_STATUS
+EFIAPI
+PlatformHookSerialPortInitialize (
+  VOID
+  );
+
+#endif // __PLATFORM_HOOK_LIB__
+
diff --git a/Core/MdeModulePkg/Include/Library/PlatformVarCleanupLib.h b/Core/MdeModulePkg/Include/Library/PlatformVarCleanupLib.h
new file mode 100644
index 0000000000..a4691f0e65
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/PlatformVarCleanupLib.h
@@ -0,0 +1,61 @@
+/** @file
+  The library class provides platform variable cleanup services.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _PLATFORM_VARIABLE_CLEANUP_LIB_
+#define _PLATFORM_VARIABLE_CLEANUP_LIB_
+
+#include <Guid/VarErrorFlag.h>
+
+typedef enum {
+  VarCleanupAll,
+  VarCleanupManually,
+  VarCleanupMax,
+} VAR_CLEANUP_TYPE;
+
+/**
+  Get last boot variable error flag.
+
+  @return   Last boot variable error flag.
+
+**/
+VAR_ERROR_FLAG
+EFIAPI
+GetLastBootVarErrorFlag (
+  );
+
+/**
+  Platform variable cleanup.
+
+  @param[in] Flag                   Variable error flag.
+  @param[in] Type                   Variable cleanup type.
+                                    If it is VarCleanupManually, the interface must be called after console connected.
+
+  @retval EFI_SUCCESS               No error or error processed.
+  @retval EFI_UNSUPPORTED           The specified Flag or Type is not supported.
+                                    For example, system error may be not supported to process and Platform should have mechanism to reset system to manufacture mode.
+                                    Another, if system and user variables are wanted to be distinguished to process, the interface must be called after EndOfDxe.
+  @retval EFI_OUT_OF_RESOURCES      Not enough resource to process the error.
+  @retval EFI_INVALID_PARAMETER     The specified Flag or Type is an invalid value.
+  @retval Others                    Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformVarCleanup (
+  IN VAR_ERROR_FLAG     Flag,
+  IN VAR_CLEANUP_TYPE   Type
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/RecoveryLib.h b/Core/MdeModulePkg/Include/Library/RecoveryLib.h
new file mode 100644
index 0000000000..f49394a822
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/RecoveryLib.h
@@ -0,0 +1,35 @@
+/** @file
+  Recovery library class defines a set of methods related recovery boot mode. 
+  This library class is no longer used and modules using this library should
+  directly locate EFI_PEI_RECOVERY_MODULE_PPI, defined in the PI 1.2 specification.
+
+Copyright (c) 2005 - 2010, 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 that 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.
+
+**/
+
+#ifndef __RECOVERY_LIB_H__
+#define __RECOVERY_LIB_H__
+
+/**
+  Calling this function causes the system to carry out a recovery boot path.
+  
+  @retval EFI_SUCCESS   Recovery boot path succeeded.
+  @retval Others        Recovery boot path failure.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiRecoverFirmware (
+  VOID
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Include/Library/ResetSystemLib.h b/Core/MdeModulePkg/Include/Library/ResetSystemLib.h
new file mode 100644
index 0000000000..f95293470b
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/ResetSystemLib.h
@@ -0,0 +1,68 @@
+/** @file
+  System reset Library Services.  This library class defines a set of
+  methods that reset the whole system.
+
+Copyright (c) 2005 - 2010, 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 that 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.
+
+**/
+
+#ifndef __RESET_SYSTEM_LIB_H__
+#define __RESET_SYSTEM_LIB_H__
+
+/**
+  This function causes a system-wide reset (cold reset), in which
+  all circuitry within the system returns to its initial state. This type of reset 
+  is asynchronous to system operation and operates without regard to 
+  cycle boundaries.
+
+  If this function returns, it means that the system does not support cold reset. 
+**/
+VOID
+EFIAPI
+ResetCold (
+  VOID
+  );
+
+/**
+  This function causes a system-wide initialization (warm reset), in which all processors 
+  are set to their initial state. Pending cycles are not corrupted.
+
+  If this function returns, it means that the system does not support warm reset.
+**/
+VOID
+EFIAPI
+ResetWarm (
+  VOID
+  );
+
+/**
+  This function causes the system to enter a power state equivalent 
+  to the ACPI G2/S5 or G3 states.
+  
+  If this function returns, it means that the system does not support shutdown reset.
+**/
+VOID
+EFIAPI
+ResetShutdown (
+  VOID
+  );
+
+/**
+  This function causes the system to enter S3 and then wake up immediately.
+  
+  If this function returns, it means that the system does not support S3 feature.
+**/
+VOID
+EFIAPI
+EnterS3WithImmediateWake (
+  VOID
+  );
+  
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/S3Lib.h b/Core/MdeModulePkg/Include/Library/S3Lib.h
new file mode 100644
index 0000000000..fcb8abd802
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/S3Lib.h
@@ -0,0 +1,34 @@
+/** @file
+  S3 library class defines a set of methods related to S3 boot mode.
+  This library class is no longer used and modules using this library should
+  directly locate EFI_PEI_S3_RESUME_PPI, defined in the PI 1.2 specification.
+
+Copyright (c) 2005 - 2010, 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 that 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.
+
+**/
+
+#ifndef __S3_LIB_H__
+#define __S3_LIB_H__
+
+/**
+  This function is responsible for calling the S3 resume vector in the ACPI Tables.
+  
+  @retval EFI_SUCCESS   Successfully restored the configuration from S3.
+  @retval Others       Failed to restore the configuration from S3.
+
+**/
+EFI_STATUS
+EFIAPI
+AcpiS3ResumeOs (
+  VOID
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/SecurityManagementLib.h b/Core/MdeModulePkg/Include/Library/SecurityManagementLib.h
new file mode 100644
index 0000000000..de87d09c54
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/SecurityManagementLib.h
@@ -0,0 +1,276 @@
+/** @file
+  This library class defines a set of interfaces to abstract the policy of 
+  security measurement by managing the different security measurement services.
+  The library instances can be implemented according to the different security policy.
+
+Copyright (c) 2009 - 2012, 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 that 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.
+
+**/
+
+#ifndef __SECURITY_MANAGEMENT_LIB_H__
+#define __SECURITY_MANAGEMENT_LIB_H__
+
+//
+// Authentication Operation defintions for User Identity (UID), Measured and Secure boot.
+//
+#define EFI_AUTH_OPERATION_NONE                0x00
+#define EFI_AUTH_OPERATION_VERIFY_IMAGE        0x01
+#define EFI_AUTH_OPERATION_DEFER_IMAGE_LOAD    0x02
+#define EFI_AUTH_OPERATION_MEASURE_IMAGE       0x04
+#define EFI_AUTH_OPERATION_CONNECT_POLICY      0x08
+//
+// Authentication State Operation will check the authentication status of a file.
+//
+#define EFI_AUTH_OPERATION_AUTHENTICATION_STATE  0x10
+
+///
+/// Image buffer is required by the security handler.
+///
+#define EFI_AUTH_OPERATION_IMAGE_REQUIRED      0x80000000
+
+/**
+  The security handler is used to abstract platform-specific policy 
+  from the DXE core response to an attempt to use a file that returns a 
+  given status for the authentication check from the section extraction protocol.  
+
+  The possible responses in a given SAP implementation may include locking 
+  flash upon failure to authenticate, attestation logging for all signed drivers, 
+  and other exception operations.  The File parameter allows for possible logging 
+  within the SAP of the driver.
+  
+  If File is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is safe for the DXE Core to use, then EFI_SUCCESS is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is not safe for the DXE Core to use under any circumstances, 
+  then EFI_ACCESS_DENIED is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is not safe for the DXE Core to use at the time, but it 
+  might be possible to use it at a future time, then EFI_SECURITY_VIOLATION is 
+  returned.
+
+  FileBuffer will be NULL and FileSize will be 0 if the handler being called 
+  did not set EFI_AUTH_OPERATION_IMAGE_REQUIRED when it was registered.
+
+  @param[in]    AuthenticationStatus 
+                           The authentication status returned from the security
+                           measurement services for the input file.
+  @param[in]    File       The pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+  @param[in]    FileBuffer The file buffer matches the input file device path.
+  @param[in]    FileSize   The size of File buffer matches the input file device path.
+
+  @retval EFI_SUCCESS            The file specified by File did authenticate, and the
+                                 platform policy dictates that the DXE Core may use File.
+  @retval EFI_INVALID_PARAMETER  The file is NULL.
+  @retval EFI_SECURITY_VIOLATION The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should be placed
+                                 in the untrusted state. A file may be promoted from
+                                 the untrusted to the trusted state at a future time
+                                 with a call to the Trust() DXE Service.
+  @retval EFI_ACCESS_DENIED      The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should not be
+                                 used for any purpose.
+
+**/
+typedef 
+EFI_STATUS
+(EFIAPI *SECURITY_FILE_AUTHENTICATION_STATE_HANDLER)(
+  IN  OUT   UINT32                     AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL   *File,
+  IN  VOID                             *FileBuffer,
+  IN  UINTN                            FileSize
+  );
+
+/**
+  Register security measurement handler with its operation type. Different
+  handlers with the same operation can all be registered.
+
+  If SecurityHandler is NULL, then ASSERT().
+  If no enough resources available to register new handler, then ASSERT().
+  If AuthenticationOperation is not recongnized, then ASSERT().
+  If the previous register handler can't be executed before the later register handler, then ASSERT().
+
+  @param[in]  SecurityHandler           The security measurement service handler to be registered.
+  @param[in]  AuthenticationOperation   Theoperation type is specified for the registered handler.
+
+  @retval EFI_SUCCESS              The handlers were registered successfully.
+**/
+EFI_STATUS
+EFIAPI
+RegisterSecurityHandler (
+  IN  SECURITY_FILE_AUTHENTICATION_STATE_HANDLER  SecurityHandler,
+  IN  UINT32                                      AuthenticationOperation
+  );
+
+/**
+  Execute registered handlers until one returns an error and that error is returned.
+  If none of the handlers return an error, then EFI_SUCCESS is returned.
+
+  Before exectue handler, get the image buffer by file device path if a handler 
+  requires the image file. And return the image buffer to each handler when exectue handler.
+
+  The handlers are executed in same order to their registered order.
+
+  @param[in]  AuthenticationStatus 
+                           This is the authentication type returned from the Section
+                           Extraction protocol. See the Section Extraction Protocol
+                           Specification for details on this type.
+  @param[in]  FilePath     This is a pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+
+  @retval EFI_SUCCESS            The file specified by File authenticated when more
+                                 than one security handler services were registered, 
+                                 or the file did not authenticate when no security 
+                                 handler service was registered. And the platform policy 
+                                 dictates that the DXE Core may use File.
+  @retval EFI_INVALID_PARAMETER  File is NULL.
+  @retval EFI_SECURITY_VIOLATION The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should be placed
+                                 in the untrusted state. A file may be promoted from
+                                 the untrusted to the trusted state at a future time
+                                 with a call to the Trust() DXE Service.
+  @retval EFI_ACCESS_DENIED      The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should not be
+                                 used for any purpose.
+**/
+EFI_STATUS
+EFIAPI
+ExecuteSecurityHandlers (
+  IN  UINT32                            AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL    *FilePath
+  );
+
+/**
+  The security handler is used to abstracts security-specific functions from the DXE 
+  Foundation of UEFI Image Verification, Trusted Computing Group (TCG) measured boot, 
+  User Identity policy for image loading and consoles, and for purposes of 
+  handling GUIDed section encapsulations. 
+  
+  @param[in]    AuthenticationStatus 
+                           The authentication status for the input file. 
+  @param[in]    File       The pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+  @param[in]    FileBuffer A pointer to the buffer with the UEFI file image
+  @param[in]    FileSize   The size of File buffer.
+  @param[in]    BootPolicy A boot policy that was used to call LoadImage() UEFI service.
+
+  @retval EFI_SUCCESS             The file specified by DevicePath and non-NULL
+                                  FileBuffer did authenticate, and the platform policy dictates
+                                  that the DXE Foundation may use the file.
+  @retval EFI_SUCCESS             The device path specified by NULL device path DevicePath
+                                  and non-NULL FileBuffer did authenticate, and the platform
+                                  policy dictates that the DXE Foundation may execute the image in
+                                  FileBuffer.
+  @retval EFI_SUCCESS             FileBuffer is NULL and current user has permission to start
+                                  UEFI device drivers on the device path specified by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  The file specified by DevicePath and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the file should be
+                                  placed in the untrusted state. The image has been added to the file
+                                  execution table.
+  @retval EFI_ACCESS_DENIED       The file specified by File and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the DXE
+                                  Foundation may not use File.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is NULL and the user has no
+                                  permission to start UEFI device drivers on the device path specified
+                                  by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is not NULL and the user has no permission to load
+                                  drivers from the device path specified by DevicePath. The
+                                  image has been added into the list of the deferred images.
+**/
+typedef 
+EFI_STATUS
+(EFIAPI *SECURITY2_FILE_AUTHENTICATION_HANDLER) (
+  IN  UINT32                           AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL   *File,
+  IN  VOID                             *FileBuffer,
+  IN  UINTN                            FileSize,
+  IN  BOOLEAN                          BootPolicy
+  );
+
+/**
+  Register security measurement handler with its operation type. Different
+  handlers with the same operation can all be registered.
+
+  If SecurityHandler is NULL, then ASSERT().
+  If no enough resources available to register new handler, then ASSERT().
+  If AuthenticationOperation is not recongnized, then ASSERT().
+  If AuthenticationOperation is EFI_AUTH_OPERATION_NONE, then ASSERT().
+  If the previous register handler can't be executed before the later register handler, then ASSERT().
+
+  @param[in]  Security2Handler          The security measurement service handler to be registered.
+  @param[in]  AuthenticationOperation   The operation type is specified for the registered handler.
+
+  @retval EFI_SUCCESS              The handlers were registered successfully.
+**/
+EFI_STATUS
+EFIAPI
+RegisterSecurity2Handler (
+  IN  SECURITY2_FILE_AUTHENTICATION_HANDLER       Security2Handler,
+  IN  UINT32                                      AuthenticationOperation
+  );
+
+/**
+  Execute registered handlers based on input AuthenticationOperation until 
+  one returns an error and that error is returned. 
+  
+  If none of the handlers return an error, then EFI_SUCCESS is returned.
+  The handlers those satisfy AuthenticationOperation will only be executed.
+  The handlers are executed in same order to their registered order.
+
+  @param[in]  AuthenticationOperation   
+                           The operation type specifies which handlers will be executed.
+  @param[in]  AuthenticationStatus 
+                           The authentication status for the input file.
+  @param[in]  File         This is a pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+  @param[in]  FileBuffer   A pointer to the buffer with the UEFI file image
+  @param[in]  FileSize     The size of File buffer.
+  @param[in]  BootPolicy   A boot policy that was used to call LoadImage() UEFI service.
+
+  @retval EFI_SUCCESS             The file specified by DevicePath and non-NULL
+                                  FileBuffer did authenticate, and the platform policy dictates
+                                  that the DXE Foundation may use the file.
+  @retval EFI_SUCCESS             The device path specified by NULL device path DevicePath
+                                  and non-NULL FileBuffer did authenticate, and the platform
+                                  policy dictates that the DXE Foundation may execute the image in
+                                  FileBuffer.
+  @retval EFI_SUCCESS             FileBuffer is NULL and current user has permission to start
+                                  UEFI device drivers on the device path specified by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  The file specified by DevicePath and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the file should be
+                                  placed in the untrusted state. The image has been added to the file
+                                  execution table.
+  @retval EFI_ACCESS_DENIED       The file specified by File and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the DXE
+                                  Foundation may not use File.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is NULL and the user has no
+                                  permission to start UEFI device drivers on the device path specified
+                                  by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is not NULL and the user has no permission to load
+                                  drivers from the device path specified by DevicePath. The
+                                  image has been added into the list of the deferred images.
+  @retval EFI_INVALID_PARAMETER   File and FileBuffer are both NULL. 
+**/
+EFI_STATUS
+EFIAPI
+ExecuteSecurity2Handlers (
+  IN  UINT32                           AuthenticationOperation,
+  IN  UINT32                           AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL   *File,
+  IN  VOID                             *FileBuffer,
+  IN  UINTN                            FileSize,
+  IN  BOOLEAN                          BootPolicy
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/SmmCorePlatformHookLib.h b/Core/MdeModulePkg/Include/Library/SmmCorePlatformHookLib.h
new file mode 100644
index 0000000000..773d0615b7
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/SmmCorePlatformHookLib.h
@@ -0,0 +1,50 @@
+/** @file
+  Smm Core Platform Hook Library.  This library class defines a set of platform
+  hooks called by the SMM Core.
+
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef __SMM_CORE_PLATFORM_HOOK_LIB__
+#define __SMM_CORE_PLATFORM_HOOK_LIB__
+
+/**
+  Performs platform specific tasks before invoking registered SMI handlers.
+  
+  This function performs platform specific tasks before invoking registered SMI handlers.
+  
+  @retval EFI_SUCCESS       The platform hook completes successfully.
+  @retval Other values      The paltform hook cannot complete due to some error.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformHookBeforeSmmDispatch (
+  VOID
+  );
+
+
+/**
+  Performs platform specific tasks after invoking registered SMI handlers.
+  
+  This function performs platform specific tasks after invoking registered SMI handlers.
+  
+  @retval EFI_SUCCESS       The platform hook completes successfully.
+  @retval Other values      The paltform hook cannot complete due to some error.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformHookAfterSmmDispatch (
+  VOID
+  );
+  
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/SortLib.h b/Core/MdeModulePkg/Include/Library/SortLib.h
new file mode 100644
index 0000000000..a891cabee9
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/SortLib.h
@@ -0,0 +1,113 @@
+/** @file
+  Library used for sorting and comparison routines.
+
+  Copyright (c) 2009 - 2014, 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.
+
+**/
+#ifndef __SORT_LIB_H__
+#define __SORT_LIB_H__
+
+/**
+  Prototype for comparison function for any two element types.
+
+  @param[in] Buffer1                  The pointer to first buffer.
+  @param[in] Buffer2                  The pointer to second buffer.
+
+  @retval 0                           Buffer1 equal to Buffer2.
+  @return <0                          Buffer1 is less than Buffer2.
+  @return >0                          Buffer1 is greater than Buffer2.
+**/
+typedef
+INTN
+(EFIAPI *SORT_COMPARE)(
+  IN CONST VOID                 *Buffer1,
+  IN CONST VOID                 *Buffer2
+  );
+
+/**
+  Function to perform a Quick Sort on a buffer of comparable elements.
+
+  Each element must be equally sized.
+
+  If BufferToSort is NULL, then ASSERT.
+  If CompareFunction is NULL, then ASSERT.
+
+  If Count is < 2 , then perform no action.
+  If Size is < 1 , then perform no action.
+
+  @param[in, out] BufferToSort   On call, a Buffer of (possibly sorted) elements;
+                                 on return, a buffer of sorted elements.
+  @param[in]  Count              The number of elements in the buffer to sort.
+  @param[in]  ElementSize        The size of an element in bytes.
+  @param[in]  CompareFunction    The function to call to perform the comparison
+                                 of any two elements.
+**/
+VOID
+EFIAPI
+PerformQuickSort (
+  IN OUT VOID                   *BufferToSort,
+  IN CONST UINTN                Count,
+  IN CONST UINTN                ElementSize,
+  IN       SORT_COMPARE         CompareFunction
+  );
+
+
+/**
+  Function to compare 2 device paths for use as CompareFunction.
+
+  @param[in] Buffer1            The pointer to Device Path to compare.
+  @param[in] Buffer2            The pointer to second DevicePath to compare.
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @return < 0                   Buffer1 is less than Buffer2.
+  @return > 0                   Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+DevicePathCompare (
+  IN  CONST VOID                *Buffer1,
+  IN  CONST VOID                *Buffer2
+  );
+
+/**
+  Function to compare 2 strings without regard to case of the characters.
+
+  @param[in] Buffer1            The pointer to String to compare (CHAR16**).
+  @param[in] Buffer2            The pointer to second String to compare (CHAR16**).
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @return < 0                   Buffer1 is less than Buffer2.
+  @return > 0                   Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringNoCaseCompare (
+  IN  CONST VOID                *Buffer1,
+  IN  CONST VOID                *Buffer2
+  );
+
+/**
+  Function to compare 2 strings.
+
+  @param[in] Buffer1            The pointer to String to compare (CHAR16**).
+  @param[in] Buffer2            The pointer to second String to compare (CHAR16**).
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @return < 0                   Buffer1 is less than Buffer2.
+  @return > 0                   Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringCompare (
+  IN  CONST VOID                *Buffer1,
+  IN  CONST VOID                *Buffer2
+  );
+
+#endif //__SORT_LIB_H__
diff --git a/Core/MdeModulePkg/Include/Library/TcpIoLib.h b/Core/MdeModulePkg/Include/Library/TcpIoLib.h
new file mode 100644
index 0000000000..2871f6747e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/TcpIoLib.h
@@ -0,0 +1,253 @@
+/** @file
+  This library is used to share code between UEFI network stack modules.
+  It provides the helper routines to access TCP service.
+
+Copyright (c) 2010 - 2011, 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<BR>
+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.
+
+**/
+
+#ifndef _TCP_IO_H_
+#define _TCP_IO_H_
+
+
+#include <Protocol/Tcp4.h>
+#include <Protocol/Tcp6.h>
+
+#include <Library/NetLib.h>
+
+#define TCP_VERSION_4 IP_VERSION_4
+#define TCP_VERSION_6 IP_VERSION_6
+
+///
+/// 10 seconds
+///
+#define TCP_GET_MAPPING_TIMEOUT 100000000U
+
+
+typedef struct {
+  EFI_IPv4_ADDRESS          LocalIp;
+  EFI_IPv4_ADDRESS          SubnetMask;
+  EFI_IPv4_ADDRESS          Gateway;
+
+  UINT16                    StationPort;
+  EFI_IPv4_ADDRESS          RemoteIp;
+  UINT16                    RemotePort;
+  BOOLEAN                   ActiveFlag;
+} TCP4_IO_CONFIG_DATA;
+
+typedef struct {
+  UINT16                    StationPort;
+  EFI_IPv6_ADDRESS          RemoteIp;
+  UINT16                    RemotePort;
+  BOOLEAN                   ActiveFlag;
+} TCP6_IO_CONFIG_DATA;
+
+typedef union {
+  TCP4_IO_CONFIG_DATA       Tcp4IoConfigData;
+  TCP6_IO_CONFIG_DATA       Tcp6IoConfigData;
+} TCP_IO_CONFIG_DATA;
+
+typedef union {
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+} TCP_IO_PROTOCOL;
+
+typedef union {
+  EFI_TCP4_CONNECTION_TOKEN Tcp4Token;
+  EFI_TCP6_CONNECTION_TOKEN Tcp6Token;
+} TCP_IO_CONNECTION_TOKEN;
+
+typedef union {
+  EFI_TCP4_IO_TOKEN         Tcp4Token;
+  EFI_TCP6_IO_TOKEN         Tcp6Token;
+} TCP_IO_IO_TOKEN;
+
+typedef union {
+  EFI_TCP4_CLOSE_TOKEN      Tcp4Token;
+  EFI_TCP6_CLOSE_TOKEN      Tcp6Token;
+} TCP_IO_CLOSE_TOKEN;
+
+typedef union {
+  EFI_TCP4_LISTEN_TOKEN     Tcp4Token;
+  EFI_TCP6_LISTEN_TOKEN     Tcp6Token;
+} TCP_IO_LISTEN_TOKEN;
+
+
+typedef struct {
+  UINT8                     TcpVersion;
+  EFI_HANDLE                Image;
+  EFI_HANDLE                Controller;
+  EFI_HANDLE                Handle;
+  
+  TCP_IO_PROTOCOL           Tcp;
+  TCP_IO_PROTOCOL           NewTcp;
+  TCP_IO_CONNECTION_TOKEN   ConnToken;
+  TCP_IO_IO_TOKEN           TxToken;
+  TCP_IO_IO_TOKEN           RxToken;
+  TCP_IO_CLOSE_TOKEN        CloseToken;
+  TCP_IO_LISTEN_TOKEN       ListenToken;
+  
+  BOOLEAN                   IsConnDone;
+  BOOLEAN                   IsTxDone;
+  BOOLEAN                   IsRxDone;
+  BOOLEAN                   IsCloseDone;
+  BOOLEAN                   IsListenDone;
+} TCP_IO;
+
+/**
+  Create a TCP socket with the specified configuration data. 
+
+  @param[in]  Image      The handle of the driver image.
+  @param[in]  Controller The handle of the controller.
+  @param[in]  TcpVersion The version of Tcp, TCP_VERSION_4 or TCP_VERSION_6.
+  @param[in]  ConfigData The Tcp configuration data.
+  @param[out] TcpIo      The TcpIo.
+  
+  @retval EFI_SUCCESS            The TCP socket is created and configured.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory.
+  @retval Others                 Failed to create the TCP socket or configure it.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoCreateSocket (
+  IN EFI_HANDLE             Image,
+  IN EFI_HANDLE             Controller,
+  IN UINT8                  TcpVersion,
+  IN TCP_IO_CONFIG_DATA     *ConfigData,
+  OUT TCP_IO                *TcpIo
+  );
+
+/**
+  Destroy the socket. 
+
+  @param[in]  TcpIo The TcpIo which wraps the socket to be destroyed.
+
+**/
+VOID
+EFIAPI
+TcpIoDestroySocket (
+  IN TCP_IO                 *TcpIo
+  );
+
+/**
+  Connect to the other endpoint of the TCP socket.
+
+  @param[in, out]  TcpIo     The TcpIo wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+  
+  @retval EFI_SUCCESS            Connect to the other endpoint of the TCP socket
+                                 successfully.
+  @retval EFI_TIMEOUT            Failed to connect to the other endpoint of the
+                                 TCP socket in the specified time period.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoConnect (
+  IN OUT TCP_IO             *TcpIo,
+  IN     EFI_EVENT          Timeout
+  );
+
+/**
+  Accept the incomding request from the other endpoint of the TCP socket.
+
+  @param[in, out]  TcpIo     The TcpIo wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+
+  
+  @retval EFI_SUCCESS            Connect to the other endpoint of the TCP socket
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+
+  @retval EFI_TIMEOUT            Failed to connect to the other endpoint of the
+                                 TCP socket in the specified time period.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoAccept (
+  IN OUT TCP_IO             *TcpIo,
+  IN     EFI_EVENT          Timeout
+  );
+  
+/**
+  Reset the socket.
+
+  @param[in, out]  TcpIo The TcpIo wrapping the TCP socket.
+
+**/
+VOID
+EFIAPI
+TcpIoReset (
+  IN OUT TCP_IO             *TcpIo
+  );
+
+/**
+  Transmit the Packet to the other endpoint of the socket.
+
+  @param[in]   TcpIo           The TcpIo wrapping the TCP socket.
+  @param[in]   Packet          The packet to transmit.
+  
+  @retval EFI_SUCCESS            The packet is trasmitted.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error occurred.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoTransmit (
+  IN TCP_IO                 *TcpIo,
+  IN NET_BUF                *Packet
+  );
+
+/**
+  Receive data from the socket.
+
+  @param[in, out]  TcpIo       The TcpIo which wraps the socket to be destroyed.
+  @param[in]       Packet      The buffer to hold the data copy from the socket rx buffer.
+  @param[in]       AsyncMode   Is this receive asyncronous or not.
+  @param[in]       Timeout     The time to wait for receiving the amount of data the Packet
+                               can hold.
+
+  @retval EFI_SUCCESS            The required amount of data is received from the socket.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error occurred.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate momery.
+  @retval EFI_TIMEOUT            Failed to receive the required amount of data in the
+                                 specified time period.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoReceive (
+  IN OUT TCP_IO             *TcpIo,
+  IN     NET_BUF            *Packet,
+  IN     BOOLEAN            AsyncMode,
+  IN     EFI_EVENT          Timeout
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/TpmMeasurementLib.h b/Core/MdeModulePkg/Include/Library/TpmMeasurementLib.h
new file mode 100644
index 0000000000..68d35aa680
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/TpmMeasurementLib.h
@@ -0,0 +1,44 @@
+/** @file
+  This library is used by other modules to measure data to TPM.
+
+Copyright (c) 2012, 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.
+
+**/
+
+#ifndef _TPM_MEASUREMENT_LIB_H_
+#define _TPM_MEASUREMENT_LIB_H_
+
+/**
+  Tpm measure and log data, and extend the measurement result into a specific PCR.
+
+  @param[in]  PcrIndex         PCR Index.
+  @param[in]  EventType        Event type.
+  @param[in]  EventLog         Measurement event log.
+  @param[in]  LogLen           Event log length in bytes.
+  @param[in]  HashData         The start of the data buffer to be hashed, extended.
+  @param[in]  HashDataLen      The length, in bytes, of the buffer referenced by HashData
+
+  @retval EFI_SUCCESS           Operation completed successfully.
+  @retval EFI_UNSUPPORTED       TPM device not available.
+  @retval EFI_OUT_OF_RESOURCES  Out of memory.
+  @retval EFI_DEVICE_ERROR      The operation was unsuccessful.
+**/
+EFI_STATUS
+EFIAPI
+TpmMeasureAndLogData (
+  IN UINT32             PcrIndex,
+  IN UINT32             EventType,
+  IN VOID               *EventLog,
+  IN UINT32             LogLen,
+  IN VOID               *HashData,
+  IN UINT64             HashDataLen
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/UdpIoLib.h b/Core/MdeModulePkg/Include/Library/UdpIoLib.h
new file mode 100644
index 0000000000..e0b44ce18e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/UdpIoLib.h
@@ -0,0 +1,355 @@
+/** @file
+  This library is used to share code between UEFI network stack modules.
+  It provides the helper routines to access UDP service. It is used by both DHCP and MTFTP.
+
+Copyright (c) 2006 - 2012, 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<BR>
+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.
+
+**/
+
+#ifndef _UDP_IO_H_
+#define _UDP_IO_H_
+
+#include <Protocol/Udp4.h>
+#include <Protocol/Udp6.h>
+
+#include <Library/NetLib.h>
+
+typedef struct _UDP_IO UDP_IO;
+
+///
+/// Signatures used by UdpIo Library.
+///
+
+#define UDP_IO_RX_SIGNATURE  SIGNATURE_32 ('U', 'D', 'P', 'R')
+#define UDP_IO_TX_SIGNATURE  SIGNATURE_32 ('U', 'D', 'P', 'T')
+#define UDP_IO_SIGNATURE     SIGNATURE_32 ('U', 'D', 'P', 'I')
+
+#define UDP_IO_UDP4_VERSION  4
+#define UDP_IO_UDP6_VERSION  6
+
+///
+/// The UDP address pair.
+///
+typedef struct {
+  EFI_IP_ADDRESS            LocalAddr;
+  UINT16                    LocalPort;
+  EFI_IP_ADDRESS            RemoteAddr;
+  UINT16                    RemotePort;
+} UDP_END_POINT;
+
+/**
+  Prototype called when receiving or sending packets to or from a UDP point.
+
+  This prototype is used by both receive and sending when calling
+  UdpIoRecvDatagram() or UdpIoSendDatagram(). When receiving, Netbuf is allocated by the
+  UDP access point and released by the user. When sending, the user allocates the the NetBuf, 
+  which is then provided to the callback as a reference.
+
+  @param[in] Packet       The packet received or sent.
+  @param[in] EndPoint     The UDP address pair corresponds to the UDP IO.
+  @param[in] IoStatus     The packet receiving or sending status.
+  @param[in] Context      The user-defined data when calling UdpIoRecvDatagram() or
+                          UdpIoSendDatagram().
+**/
+typedef
+VOID
+(EFIAPI *UDP_IO_CALLBACK) (
+  IN NET_BUF                *Packet,
+  IN UDP_END_POINT          *EndPoint,
+  IN EFI_STATUS             IoStatus,
+  IN VOID                   *Context
+  );
+
+///
+/// This structure is used internally by the UdpIo Library.
+///
+/// Each receive request is wrapped in an UDP_RX_TOKEN. Upon completion,
+/// the CallBack will be called. Only one receive request is sent to UDP at a
+/// time. HeadLen gives the length of the application's header. UDP_IO will
+/// make the application's header continuous before delivering up.
+///
+typedef union {
+  EFI_UDP4_COMPLETION_TOKEN   Udp4;
+  EFI_UDP6_COMPLETION_TOKEN   Udp6;
+} UDP_COMPLETION_TOKEN;
+
+typedef struct {
+  UINT32                      Signature;
+  UDP_IO                      *UdpIo;
+
+  UDP_IO_CALLBACK             CallBack;
+  VOID                        *Context;
+  UINT32                      HeadLen;
+
+  UDP_COMPLETION_TOKEN        Token;
+} UDP_RX_TOKEN;
+
+
+
+///
+/// This structure is used internally by UdpIo Library.
+///
+/// Each transmit request is wrapped in an UDP_TX_TOKEN. Upon completion,
+/// the CallBack will be called. There can be several transmit requests. All transmit 
+/// requests are linked in a list.
+///
+
+typedef union {
+  EFI_UDP4_SESSION_DATA       Udp4;
+  EFI_UDP6_SESSION_DATA       Udp6;
+} UDP_SESSION_DATA;
+
+typedef union {
+  EFI_UDP4_TRANSMIT_DATA      Udp4;
+  EFI_UDP6_TRANSMIT_DATA      Udp6;
+} UDP_TRANSMIT_DATA;
+
+typedef struct {
+  UINT32                      Signature;
+  LIST_ENTRY                  Link;
+  UDP_IO                      *UdpIo;
+  UDP_IO_CALLBACK             CallBack;
+  NET_BUF                     *Packet;
+  VOID                        *Context;
+  EFI_IPv4_ADDRESS            Gateway;
+  UDP_SESSION_DATA            Session;
+  UDP_COMPLETION_TOKEN        Token;
+  UDP_TRANSMIT_DATA           Data;
+} UDP_TX_TOKEN;
+
+///
+/// Type defined as UDP_IO.
+///
+/// This data structure wraps the UDP instance and configuration.
+/// UdpIo Library uses this structure for all Udp4 or Udp6 operations.
+///
+struct _UDP_IO {
+  UINT32                    Signature;
+  LIST_ENTRY                Link;
+  INTN                      RefCnt;
+  UINT8                     UdpVersion;
+
+  //
+  // Handle used to create/destroy UDP child
+  //
+  EFI_HANDLE                Controller;
+  EFI_HANDLE                Image;
+  EFI_HANDLE                UdpHandle;
+
+  EFI_SIMPLE_NETWORK_MODE   SnpMode;
+
+  LIST_ENTRY                SentDatagram;   ///< A list of UDP_TX_TOKEN.
+  UDP_RX_TOKEN              *RecvRequest;
+
+  union {
+    EFI_UDP4_PROTOCOL       *Udp4;
+    EFI_UDP6_PROTOCOL       *Udp6;
+  } Protocol;
+
+  union {
+    EFI_UDP4_CONFIG_DATA    Udp4;
+    EFI_UDP6_CONFIG_DATA    Udp6;
+  } Config;
+};
+
+/**
+  The prototype called when UdpIo Library configures a UDP instance.
+
+  The prototype is set and called when creating a UDP_IO in UdpIoCreatePort().
+
+  @param[in] UdpIo         The UDP_IO to configure.
+  @param[in] Context       The user-defined data when calling UdpIoCreatePort().
+
+  @retval EFI_SUCCESS  The configuration succeeded.
+  @retval Others       The UDP_IO fails to configure indicating
+                       UdpIoCreatePort() should fail.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *UDP_IO_CONFIG) (
+  IN UDP_IO                 *UdpIo,
+  IN VOID                   *Context
+  );
+
+/**
+  The select function to decide whether to cancel the UDP_TX_TOKEN.
+
+  @param[in] Token        The UDP_TX_TOKEN to decide whether to cancel.
+  @param[in] Context      User-defined data in UdpIoCancelDgrams().
+
+  @retval TRUE        Cancel the UDP_TX_TOKEN.
+  @retval FALSE       Do not cancel this UDP_TX_TOKEN.
+
+**/
+typedef
+BOOLEAN
+(EFIAPI *UDP_IO_TO_CANCEL) (
+  IN UDP_TX_TOKEN           *Token,
+  IN VOID                   *Context
+  );
+
+/**
+  Cancel all the sent datagram that pass the selection criteria of ToCancel.
+  If ToCancel is NULL, all the datagrams are cancelled.
+
+  @param[in]  UdpIo                 The UDP_IO to cancel packet.
+  @param[in]  IoStatus              The IoStatus to return to the packet owners.
+  @param[in]  ToCancel              The select funtion to test whether to cancel this
+                                    packet or not.
+  @param[in]  Context               The opaque parameter to the ToCancel.
+
+**/
+VOID
+EFIAPI
+UdpIoCancelDgrams (
+  IN UDP_IO                 *UdpIo,
+  IN EFI_STATUS             IoStatus,
+  IN UDP_IO_TO_CANCEL       ToCancel,        OPTIONAL
+  IN VOID                   *Context
+  );
+
+/**
+  Creates a UDP_IO to access the UDP service. It creates and configures
+  a UDP child.
+
+  It locates the UDP service binding prototype on the Controller parameter
+  uses the UDP service binding prototype to create a UDP child (also known as
+  a UDP instance) configures the UDP child by calling Configure function prototype.
+  Any failures in creating or configuring the UDP child return NULL for failure.
+
+  @param[in]  Controller            The controller that has the UDP service binding.
+                                    protocol installed.
+  @param[in]  ImageHandle           The image handle for the driver.
+  @param[in]  Configure             The function to configure the created UDP child.
+  @param[in]  UdpVersion            The UDP protocol version, UDP4 or UDP6.
+  @param[in]  Context               The opaque parameter for the Configure funtion.
+
+  @return The newly-created UDP_IO, or NULL if failed.
+
+**/
+UDP_IO *
+EFIAPI
+UdpIoCreateIo (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            ImageHandle,
+  IN  UDP_IO_CONFIG         Configure,
+  IN  UINT8                 UdpVersion,
+  IN  VOID                  *Context
+  );
+
+/**
+  Free the UDP_IO and all its related resources.
+
+  The function cancels all sent datagrams and receive requests.
+
+  @param[in]  UdpIo             The UDP_IO to free.
+
+  @retval EFI_SUCCESS           The UDP_IO is freed.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoFreeIo (
+  IN  UDP_IO                *UdpIo
+  );
+
+/**
+  Cleans up the UDP_IO without freeing it. Call this function
+  if you intend to later re-use the UDP_IO.
+
+  This function releases all transmitted datagrams and receive requests and configures NULL for the UDP instance.
+
+  @param[in]  UdpIo                 The UDP_IO to clean up.
+
+**/
+VOID
+EFIAPI
+UdpIoCleanIo (
+  IN  UDP_IO                *UdpIo
+  );
+
+/**
+  Send a packet through the UDP_IO.
+
+  The packet will be wrapped in UDP_TX_TOKEN. Function Callback will be called
+  when the packet is sent. The optional parameter EndPoint overrides the default
+  address pair if specified.
+
+  @param[in]  UdpIo                 The UDP_IO to send the packet through.
+  @param[in]  Packet                The packet to send.
+  @param[in]  EndPoint              The local and remote access point. Override the
+                                    default address pair set during configuration.
+  @param[in]  Gateway               The gateway to use.
+  @param[in]  CallBack              The function being called when packet is
+                                    transmitted or failed.
+  @param[in]  Context               The opaque parameter passed to CallBack.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the packet.
+  @retval EFI_SUCCESS           The packet is successfully delivered to UDP for
+                                transmission.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoSendDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  NET_BUF               *Packet,
+  IN  UDP_END_POINT         *EndPoint OPTIONAL,
+  IN  EFI_IP_ADDRESS        *Gateway  OPTIONAL,
+  IN  UDP_IO_CALLBACK       CallBack,
+  IN  VOID                  *Context
+  );
+
+/**
+  Cancel a single sent datagram.
+
+  @param[in]  UdpIo                 The UDP_IO from which to cancel the packet
+  @param[in]  Packet                The packet to cancel
+
+**/
+VOID
+EFIAPI
+UdpIoCancelSentDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  NET_BUF               *Packet
+  );
+
+/**
+  Issue a receive request to the UDP_IO.
+
+  This function is called when upper-layer needs packet from UDP for processing.
+  Only one receive request is acceptable at a time. Therefore, one common usage model is
+  to invoke this function inside its Callback function when the former packet
+  is processed.
+
+  @param[in]  UdpIo                 The UDP_IO to receive the packet from.
+  @param[in]  CallBack              The call back function to execute when the packet
+                                    is received.
+  @param[in]  Context               The opaque context passed to Callback.
+  @param[in]  HeadLen               The length of the upper-layer's protocol header.
+
+  @retval EFI_ALREADY_STARTED   There is already a pending receive request. Only
+                                one receive request is supported at a time.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate needed resources.
+  @retval EFI_SUCCESS           The receive request was issued successfully.
+  @retval EFI_UNSUPPORTED       The UDP version in UDP_IO is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoRecvDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  UDP_IO_CALLBACK       CallBack,
+  IN  VOID                  *Context,
+  IN  UINT32                HeadLen
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Library/UefiBootManagerLib.h b/Core/MdeModulePkg/Include/Library/UefiBootManagerLib.h
new file mode 100644
index 0000000000..5538d90738
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/UefiBootManagerLib.h
@@ -0,0 +1,711 @@
+/** @file
+  Provide Boot Manager related library APIs.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+
+#ifndef _UEFI_BOOT_MANAGER_LIB_H_
+#define _UEFI_BOOT_MANAGER_LIB_H_
+
+#include <Protocol/DriverHealth.h>
+#include <Library/SortLib.h>
+
+//
+// Boot Manager load option library functions.
+//
+
+//
+// Load Option Type
+//
+typedef enum {
+  LoadOptionTypeDriver,
+  LoadOptionTypeSysPrep,
+  LoadOptionTypeBoot,
+  LoadOptionTypeMax
+} EFI_BOOT_MANAGER_LOAD_OPTION_TYPE;
+
+typedef enum {
+  LoadOptionNumberMax = 0x10000,
+  LoadOptionNumberUnassigned = LoadOptionNumberMax
+} EFI_BOOT_MANAGER_LOAD_OPTION_NUMBER;
+
+//
+// Common structure definition for DriverOption and BootOption
+//
+typedef struct {
+  //
+  // Data read from UEFI NV variables
+  //
+  UINTN                             OptionNumber;       // #### numerical value, could be LoadOptionNumberUnassigned
+  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE OptionType;         // LoadOptionTypeBoot or LoadOptionTypeDriver
+  UINT32                            Attributes;         // Load Option Attributes
+  CHAR16                            *Description;       // Load Option Description
+  EFI_DEVICE_PATH_PROTOCOL          *FilePath;          // Load Option Device Path
+  UINT8                             *OptionalData;      // Load Option optional data to pass into image
+  UINT32                            OptionalDataSize;   // Load Option size of OptionalData
+  EFI_GUID                          VendorGuid;
+
+  //
+  // Used at runtime
+  //
+  EFI_STATUS                        Status;             // Status returned from boot attempt gBS->StartImage ()
+  CHAR16                            *ExitData;          // Exit data returned from gBS->StartImage () 
+  UINTN                             ExitDataSize;       // Size of ExitData
+} EFI_BOOT_MANAGER_LOAD_OPTION;
+
+/**
+  Returns an array of load options based on the EFI variable
+  L"BootOrder"/L"DriverOrder" and the L"Boot####"/L"Driver####" variables impled by it.
+  #### is the hex value of the UINT16 in each BootOrder/DriverOrder entry. 
+
+  @param  LoadOptionCount   Returns number of entries in the array.
+  @param  LoadOptionType    The type of the load option.
+
+  @retval NULL  No load options exist.
+  @retval !NULL Array of load option entries.
+
+**/
+EFI_BOOT_MANAGER_LOAD_OPTION *
+EFIAPI
+EfiBootManagerGetLoadOptions (
+  OUT UINTN                            *LoadOptionCount,
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE LoadOptionType
+  );
+
+/**
+  Free an array of load options returned from EfiBootManagerGetLoadOptions().
+
+  @param  LoadOptions      Pointer to the array of load options to free.
+  @param  LoadOptionCount  Number of array entries in LoadOptions.
+
+  @return EFI_SUCCESS           LoadOptions was freed.
+  @return EFI_INVALID_PARAMETER LoadOptions is NULL.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeLoadOptions (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *LoadOptions,
+  IN  UINTN                         LoadOptionCount
+  );
+
+/**
+  Initialize a load option.
+
+  @param Option           Pointer to the load option to be initialized.
+  @param OptionNumber     Option number of the load option.
+  @param OptionType       Type of the load option.
+  @param Attributes       Attributes of the load option.
+  @param Description      Description of the load option.
+  @param FilePath         Device path of the load option.
+  @param OptionalData     Optional data of the load option.
+  @param OptionalDataSize Size of the optional data of the load option.
+
+  @retval EFI_SUCCESS           The load option was initialized successfully.
+  @retval EFI_INVALID_PARAMETER Option, Description or FilePath is NULL.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerInitializeLoadOption (
+  IN OUT EFI_BOOT_MANAGER_LOAD_OPTION   *Option,
+  IN  UINTN                             OptionNumber,
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE OptionType,
+  IN  UINT32                            Attributes,
+  IN  CHAR16                            *Description,
+  IN  EFI_DEVICE_PATH_PROTOCOL          *FilePath,
+  IN  UINT8                             *OptionalData,
+  IN  UINT32                            OptionalDataSize
+  );
+
+/**
+  Free a load option created by EfiBootManagerInitializeLoadOption()
+  or EfiBootManagerVariableToLoadOption().
+
+  @param  LoadOption   Pointer to the load option to free.
+  CONCERN: Check Boot#### instead of BootOrder, optimize, spec clarify
+  @return EFI_SUCCESS           LoadOption was freed.
+  @return EFI_INVALID_PARAMETER LoadOption is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeLoadOption (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *LoadOption
+  );
+
+/**
+  Initialize the load option from the VariableName.
+
+  @param  VariableName          EFI Variable name which could be Boot#### or
+                                Driver####
+  @param  LoadOption            Pointer to the load option to be initialized
+
+  @retval EFI_SUCCESS           The option was created
+  @retval EFI_INVALID_PARAMETER VariableName or LoadOption is NULL.
+  @retval EFI_NOT_FOUND         The variable specified by VariableName cannot be found.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerVariableToLoadOption (
+  IN CHAR16                           *VariableName,
+  IN OUT EFI_BOOT_MANAGER_LOAD_OPTION *LoadOption
+  );
+
+/**
+  Create the Boot#### or Driver#### variable from the load option.
+  
+  @param  LoadOption      Pointer to the load option.
+
+  @retval EFI_SUCCESS     The variable was created.
+  @retval Others          Error status returned by RT->SetVariable.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerLoadOptionToVariable (
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION     *LoadOption
+  );
+
+/**
+  This function will update the Boot####/Driver####/SysPrep#### and the 
+  BootOrder/DriverOrder/SysPrepOrder to add a new load option.
+
+  @param  Option        Pointer to load option to add.
+  @param  Position      Position of the new load option to put in the BootOrder/DriverOrder/SysPrepOrder.
+
+  @retval EFI_SUCCESS   The load option has been successfully added.
+  @retval Others        Error status returned by RT->SetVariable.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerAddLoadOptionVariable (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION  *Option,
+  IN UINTN                         Position
+  );
+
+/**
+  Delete the load option according to the OptionNumber and OptionType.
+  
+  Only the BootOrder/DriverOrder is updated to remove the reference of the OptionNumber.
+  
+  @param  OptionNumber        Option number of the load option.
+  @param  OptionType          Type of the load option.
+
+  @retval EFI_NOT_FOUND       The load option cannot be found.
+  @retval EFI_SUCCESS         The load option was deleted.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerDeleteLoadOptionVariable (
+  IN UINTN                              OptionNumber,
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE  OptionType
+  );
+
+/**
+  Sort the load options. The DriverOrder/BootOrder variables will be re-created to 
+  reflect the new order.
+
+  @param OptionType        The type of the load option.
+  @param CompareFunction   The comparator function pointer.
+**/
+VOID
+EFIAPI
+EfiBootManagerSortLoadOptionVariable (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE OptionType,
+  IN SORT_COMPARE                      CompareFunction
+  );
+
+//
+// Boot Manager hot key library functions.
+//
+
+#pragma pack(1)
+///
+/// EFI Key Option.
+///
+typedef struct {
+  ///
+  /// Specifies options about how the key will be processed.
+  ///
+  EFI_BOOT_KEY_DATA  KeyData;
+  ///
+  /// The CRC-32 which should match the CRC-32 of the entire EFI_LOAD_OPTION to
+  /// which BootOption refers. If the CRC-32s do not match this value, then this key
+  /// option is ignored.
+  ///
+  UINT32             BootOptionCrc;
+  ///
+  /// The Boot#### option which will be invoked if this key is pressed and the boot option
+  /// is active (LOAD_OPTION_ACTIVE is set).
+  ///
+  UINT16             BootOption;
+  ///
+  /// The key codes to compare against those returned by the
+  /// EFI_SIMPLE_TEXT_INPUT and EFI_SIMPLE_TEXT_INPUT_EX protocols.
+  /// The number of key codes (0-3) is specified by the EFI_KEY_CODE_COUNT field in KeyOptions.
+  ///
+  EFI_INPUT_KEY      Keys[3];
+  UINT16             OptionNumber;
+} EFI_BOOT_MANAGER_KEY_OPTION;
+#pragma pack()
+
+/**
+  Start the hot key service so that the key press can trigger the boot option.
+
+  @param HotkeyTriggered  Return the waitable event and it will be signaled 
+                          when a valid hot key is pressed.
+
+  @retval EFI_SUCCESS     The hot key service is started.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerStartHotkeyService (
+  IN EFI_EVENT      *HotkeyTriggered
+  );
+
+//
+// Modifier for EfiBootManagerAddKeyOptionVariable and EfiBootManagerDeleteKeyOptionVariable
+//
+#define EFI_BOOT_MANAGER_SHIFT_PRESSED    0x00000001
+#define EFI_BOOT_MANAGER_CONTROL_PRESSED  0x00000002
+#define EFI_BOOT_MANAGER_ALT_PRESSED      0x00000004
+#define EFI_BOOT_MANAGER_LOGO_PRESSED     0x00000008
+#define EFI_BOOT_MANAGER_MENU_KEY_PRESSED 0x00000010
+#define EFI_BOOT_MANAGER_SYS_REQ_PRESSED  0x00000020
+
+/**
+  Add the key option.
+  It adds the key option variable and the key option takes affect immediately.
+
+  @param AddedOption      Return the added key option.
+  @param BootOptionNumber The boot option number for the key option.
+  @param Modifier         Key shift state.
+  @param ...              Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS         The key option is added.
+  @retval EFI_ALREADY_STARTED The hot key is already used by certain key option.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerAddKeyOptionVariable (
+  OUT EFI_BOOT_MANAGER_KEY_OPTION *AddedOption,   OPTIONAL
+  IN UINT16                       BootOptionNumber,
+  IN UINT32                       Modifier,
+  ...
+  );
+
+/**
+  Delete the Key Option variable and unregister the hot key
+
+  @param DeletedOption  Return the deleted key options.
+  @param Modifier       Key shift state.
+  @param ...            Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS   The key option is deleted.
+  @retval EFI_NOT_FOUND The key option cannot be found.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerDeleteKeyOptionVariable (
+  IN EFI_BOOT_MANAGER_KEY_OPTION *DeletedOption, OPTIONAL
+  IN UINT32                      Modifier,
+  ...
+  );
+
+/**
+  Register the key option to exit the waiting of the Boot Manager timeout.
+  Platform should ensure that the continue key option isn't conflict with
+  other boot key options.
+
+  @param Modifier     Key shift state.
+  @param  ...         Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS         Successfully register the continue key option.
+  @retval EFI_ALREADY_STARTED The continue key option is already registered.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerRegisterContinueKeyOption (
+  IN UINT32           Modifier,
+  ...
+  );
+
+/**
+  Try to boot the boot option triggered by hot key.
+**/
+VOID
+EFIAPI
+EfiBootManagerHotkeyBoot (
+  VOID
+  );
+//
+// Boot Manager boot library functions.
+//
+
+/**
+  The function creates boot options for all possible bootable medias in the following order:
+  1. Removable BlockIo            - The boot option only points to the removable media
+                                    device, like USB key, DVD, Floppy etc.
+  2. Fixed BlockIo                - The boot option only points to a Fixed blockIo device,
+                                    like HardDisk.
+  3. Non-BlockIo SimpleFileSystem - The boot option points to a device supporting
+                                    SimpleFileSystem Protocol, but not supporting BlockIo
+                                    protocol.
+  4. LoadFile                     - The boot option points to the media supporting 
+                                    LoadFile protocol.
+  Reference: UEFI Spec chapter 3.3 Boot Option Variables Default Boot Behavior
+
+  The function won't delete the boot option not added by itself.
+**/
+VOID
+EFIAPI
+EfiBootManagerRefreshAllBootOption (
+  VOID
+  );
+
+/**
+  Attempt to boot the EFI boot option. This routine sets L"BootCurent" and
+  signals the EFI ready to boot event. If the device path for the option starts
+  with a BBS device path a legacy boot is attempted. Short form device paths are
+  also supported via this rountine. A device path starting with 
+  MEDIA_HARDDRIVE_DP, MSG_USB_WWID_DP, MSG_USB_CLASS_DP gets expaned out
+  to find the first device that matches. If the BootOption Device Path 
+  fails the removable media boot algorithm is attempted (\EFI\BOOTIA32.EFI,
+  \EFI\BOOTX64.EFI,... only one file type is tried per processor type)
+
+  @param  BootOption    Boot Option to try and boot.
+                        On return, BootOption->Status contains the boot status:
+                        EFI_SUCCESS     BootOption was booted
+                        EFI_UNSUPPORTED BootOption isn't supported.
+                        EFI_NOT_FOUND   The BootOption was not found on the system
+                        Others          BootOption failed with this error status
+
+**/
+VOID
+EFIAPI
+EfiBootManagerBoot (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption
+  );
+
+/**
+  Return the Boot Manager Menu.
+ 
+  @param BootOption    Return the Boot Manager Menu.
+
+  @retval EFI_SUCCESS   The Boot Manager Menu is successfully returned.
+  @retval EFI_NOT_FOUND The Boot Manager Menu is not found.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerGetBootManagerMenu (
+  EFI_BOOT_MANAGER_LOAD_OPTION *BootOption
+  );
+
+/**
+  The function enumerates all the legacy boot options, creates them and 
+  registers them in the BootOrder variable.
+**/
+typedef
+VOID
+(EFIAPI *EFI_BOOT_MANAGER_REFRESH_LEGACY_BOOT_OPTION) (
+  VOID
+  );
+
+/**
+  The function boots a legacy boot option.
+**/
+typedef
+VOID
+(EFIAPI *EFI_BOOT_MANAGER_LEGACY_BOOT) (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOption
+  );
+
+/**
+  The function registers the legacy boot support capabilities.
+
+  @param RefreshLegacyBootOption The function pointer to create all the legacy boot options.
+  @param LegacyBoot              The function pointer to boot the legacy boot option.
+**/
+VOID
+EFIAPI
+EfiBootManagerRegisterLegacyBootSupport (
+  EFI_BOOT_MANAGER_REFRESH_LEGACY_BOOT_OPTION   RefreshLegacyBootOption,
+  EFI_BOOT_MANAGER_LEGACY_BOOT                  LegacyBoot
+  );
+
+/**
+  Return the platform provided boot option description for the controller.
+
+  @param Handle                Controller handle.
+  @param DefaultDescription    Default boot description provided by core.
+
+  @return  The callee allocated description string
+           or NULL if the handler wants to use DefaultDescription.
+**/
+typedef
+CHAR16 *
+(EFIAPI *EFI_BOOT_MANAGER_BOOT_DESCRIPTION_HANDLER) (
+  IN EFI_HANDLE                  Handle,
+  IN CONST CHAR16                *DefaultDescription
+  );
+
+/**
+  Register the platform provided boot description handler.
+
+  @param Handler  The platform provided boot description handler
+
+  @retval EFI_SUCCESS          The handler was registered successfully.
+  @retval EFI_ALREADY_STARTED  The handler was already registered.
+  @retval EFI_OUT_OF_RESOURCES There is not enough resource to perform the registration.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerRegisterBootDescriptionHandler (
+  IN EFI_BOOT_MANAGER_BOOT_DESCRIPTION_HANDLER  Handler
+  );
+
+//
+// Boot Manager connect and disconnect library functions
+//
+
+/**
+  This function will connect all the system driver to controller
+  first, and then special connect the default console, this make
+  sure all the system controller available and the platform default
+  console connected.
+**/
+VOID
+EFIAPI
+EfiBootManagerConnectAll (
+  VOID
+  );
+
+/**
+  This function will create all handles associate with every device
+  path node. If the handle associate with one device path node can not
+  be created successfully, then still give chance to do the dispatch,
+  which load the missing drivers if possible.
+
+  @param  DevicePathToConnect   The device path which will be connected, it can be
+                                a multi-instance device path
+  @param  MatchingHandle        Return the controller handle closest to the DevicePathToConnect
+
+  @retval EFI_SUCCESS            All handles associate with every device path node
+                                 have been created.
+  @retval EFI_OUT_OF_RESOURCES   There is no resource to create new handles.
+  @retval EFI_NOT_FOUND          Create the handle associate with one device path
+                                 node failed.
+  @retval EFI_SECURITY_VIOLATION The user has no permission to start UEFI device 
+                                 drivers on the DevicePath.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *DevicePathToConnect,
+  OUT EFI_HANDLE                *MatchingHandle          OPTIONAL
+  );
+
+/**
+  This function will disconnect all current system handles. 
+  
+  gBS->DisconnectController() is invoked for each handle exists in system handle buffer.
+  If handle is a bus type handle, all childrens also are disconnected recursively by
+  gBS->DisconnectController().
+**/
+VOID
+EFIAPI
+EfiBootManagerDisconnectAll (
+  VOID
+  );
+
+
+//
+// Boot Manager console library functions
+//
+
+typedef enum {
+  ConIn,
+  ConOut,
+  ErrOut,
+  ConInDev,
+  ConOutDev,
+  ErrOutDev,
+  ConsoleTypeMax
+} CONSOLE_TYPE;
+
+/**
+  This function will connect all the console devices base on the console
+  device variable ConIn, ConOut and ErrOut.
+
+  @retval EFI_DEVICE_ERROR         All the consoles were not connected due to an error.
+  @retval EFI_SUCCESS              Success connect any one instance of the console
+                                   device path base on the variable ConVarName.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectAllDefaultConsoles (
+  VOID
+  );
+
+/**
+  This function updates the console variable based on ConVarName. It can
+  add or remove one specific console device path from the variable
+
+  @param  ConsoleType              ConIn, ConOut, ErrOut, ConInDev, ConOutDev or ErrOutDev.
+  @param  CustomizedConDevicePath  The console device path to be added to
+                                   the console variable. Cannot be multi-instance.
+  @param  ExclusiveDevicePath      The console device path to be removed
+                                   from the console variable. Cannot be multi-instance.
+
+  @retval EFI_UNSUPPORTED          The added device path is the same as a removed one.
+  @retval EFI_SUCCESS              Successfully added or removed the device path from the
+                                   console variable.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerUpdateConsoleVariable (
+  IN  CONSOLE_TYPE              ConsoleType,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *CustomizedConDevicePath,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *ExclusiveDevicePath
+  );
+
+/**
+  Connect the console device base on the variable ConVarName, if
+  device path of the ConVarName is multi-instance device path, if
+  anyone of the instances is connected success, then this function
+  will return success.
+
+  @param  ConsoleType              ConIn, ConOut or ErrOut.
+
+  @retval EFI_NOT_FOUND            There is not any console devices connected
+                                   success
+  @retval EFI_SUCCESS              Success connect any one instance of the console
+                                   device path base on the variable ConVarName.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectConsoleVariable (
+  IN  CONSOLE_TYPE              ConsoleType
+  );
+
+/**
+  Query all the children of VideoController and return the device paths of all the 
+  children that support GraphicsOutput protocol.
+
+  @param VideoController       PCI handle of video controller.
+
+  @return  Device paths of all the children that support GraphicsOutput protocol.
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+EFIAPI
+EfiBootManagerGetGopDevicePath (
+  IN  EFI_HANDLE               VideoController
+  );
+
+/**
+  Connect the platform active active video controller.
+
+  @param VideoController       PCI handle of video controller.
+
+  @retval EFI_NOT_FOUND There is no active video controller.
+  @retval EFI_SUCCESS   The video controller is connected.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectVideoController (
+  EFI_HANDLE                 VideoController  OPTIONAL
+  );
+
+//
+// Boot Manager driver health library functions.
+//
+
+typedef struct {
+  EFI_DRIVER_HEALTH_PROTOCOL      *DriverHealth;
+
+  ///
+  /// Driver relative handles
+  ///
+  EFI_HANDLE                      DriverHealthHandle;
+  EFI_HANDLE                      ControllerHandle;
+  EFI_HANDLE                      ChildHandle;
+
+  ///
+  /// Driver health messages of the specify Driver 
+  ///
+  EFI_DRIVER_HEALTH_HII_MESSAGE   *MessageList;
+
+  ///
+  /// HII relative handles
+  ///
+  EFI_HII_HANDLE                  HiiHandle;
+
+  ///
+  /// Driver Health status
+  ///
+  EFI_DRIVER_HEALTH_STATUS        HealthStatus;
+} EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO;
+
+/**
+  Return all the Driver Health information.
+
+  When the cumulative health status of all the controllers managed by the
+  driver who produces the EFI_DRIVER_HEALTH_PROTOCOL is healthy, only one
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO entry is created for such
+  EFI_DRIVER_HEALTH_PROTOCOL instance.
+  Otherwise, every controller creates one EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO
+  entry. Additionally every child controller creates one
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO entry if the driver is a bus driver.
+
+  @param Count      Return the count of the Driver Health information.
+
+  @retval NULL      No Driver Health information is returned.
+  @retval !NULL     Pointer to the Driver Health information array.
+**/
+EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO *
+EFIAPI
+EfiBootManagerGetDriverHealthInfo (
+  UINTN    *Count
+  );
+
+/**
+  Free the Driver Health information array.
+
+  @param DriverHealthInfo       Pointer to array of the Driver Health information.
+  @param Count                  Count of the array.
+
+  @retval EFI_SUCCESS           The array is freed.
+  @retval EFI_INVALID_PARAMETER The array is NULL.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeDriverHealthInfo (
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO  *DriverHealthInfo,
+  UINTN                                Count
+  );
+
+/**
+  Process (load and execute) the load option.
+
+  @param LoadOption  Pointer to the load option.
+
+  @retval EFI_INVALID_PARAMETER  The load option type is invalid, 
+                                 or the load option file path doesn't point to a valid file.
+  @retval EFI_UNSUPPORTED        The load option type is of LoadOptionTypeBoot.
+  @retval EFI_SUCCESS            The load option is inactive, or successfully loaded and executed.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerProcessLoadOption (
+  EFI_BOOT_MANAGER_LOAD_OPTION       *LoadOption
+  );
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/UefiHiiServicesLib.h b/Core/MdeModulePkg/Include/Library/UefiHiiServicesLib.h
new file mode 100644
index 0000000000..8c8beee555
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/UefiHiiServicesLib.h
@@ -0,0 +1,52 @@
+/** @file
+  Provides global variables that are pointers to the UEFI HII related protocols.
+  All of the UEFI HII related protocols are optional, so the consumers of this
+  library class must verify that the global variable pointers are not NULL before
+  use.   
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __UEFI_HII_SERVICES_LIB_H__
+#define __UEFI_HII_SERVICES_LIB_H__
+
+#include <Protocol/HiiFont.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/HiiImage.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiConfigRouting.h>
+
+///
+/// The pointer to the UEFI HII Font Protocol.
+///
+extern EFI_HII_FONT_PROTOCOL  *gHiiFont;
+
+///
+/// The pointer to the UEFI HII String Protocol.
+///
+extern EFI_HII_STRING_PROTOCOL  *gHiiString;
+
+///
+/// The pointer to the UEFI HII Image Protocol.
+///
+extern EFI_HII_IMAGE_PROTOCOL  *gHiiImage;
+
+///
+/// The pointer to the UEFI HII Database Protocol.
+///
+extern EFI_HII_DATABASE_PROTOCOL  *gHiiDatabase;
+
+///
+/// The pointer to the UEFI HII Config Rounting Protocol.
+///
+extern EFI_HII_CONFIG_ROUTING_PROTOCOL  *gHiiConfigRouting;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Library/VarCheckLib.h b/Core/MdeModulePkg/Include/Library/VarCheckLib.h
new file mode 100644
index 0000000000..a423bc0e54
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Library/VarCheckLib.h
@@ -0,0 +1,180 @@
+/** @file
+  Provides variable check services and database management.
+
+Copyright (c) 2015, 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 that 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.
+
+**/
+
+#ifndef _VARIABLE_CHECK_LIB_H_
+#define _VARIABLE_CHECK_LIB_H_
+
+#include <Protocol/VarCheck.h>
+
+typedef enum {
+  VarCheckRequestReserved0 = 0,
+  VarCheckRequestReserved1 = 1,
+  VarCheckFromTrusted = 2,
+  VarCheckFromUntrusted = 3,
+} VAR_CHECK_REQUEST_SOURCE;
+
+typedef
+VOID
+(EFIAPI *VAR_CHECK_END_OF_DXE_CALLBACK) (
+  VOID
+  );
+
+/**
+  Register END_OF_DXE callback.
+  The callback will be invoked by VarCheckLibInitializeAtEndOfDxe().
+
+  @param[in] Callback           END_OF_DXE callback.
+
+  @retval EFI_SUCCESS           The callback was registered successfully.
+  @retval EFI_INVALID_PARAMETER Callback is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the callback register request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterEndOfDxeCallback (
+  IN VAR_CHECK_END_OF_DXE_CALLBACK  Callback
+  );
+
+/**
+  Var check initialize at END_OF_DXE.
+
+  This function needs to be called at END_OF_DXE.
+  Address pointers may be returned,
+  and caller needs to ConvertPointer() for the pointers.
+
+  @param[in, out] AddressPointerCount   Output pointer to address pointer count.
+
+  @return Address pointer buffer, NULL if input AddressPointerCount is NULL.
+
+**/
+VOID ***
+EFIAPI
+VarCheckLibInitializeAtEndOfDxe (
+  IN OUT UINTN                  *AddressPointerCount OPTIONAL
+  );
+
+/**
+  Register address pointer.
+  The AddressPointer may be returned by VarCheckLibInitializeAtEndOfDxe().
+
+  @param[in] AddressPointer     Address pointer.
+
+  @retval EFI_SUCCESS           The address pointer was registered successfully.
+  @retval EFI_INVALID_PARAMETER AddressPointer is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the address pointer register request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterAddressPointer (
+  IN VOID                       **AddressPointer
+  );
+
+/**
+  Register SetVariable check handler.
+
+  @param[in] Handler            Pointer to check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterSetVariableCheckHandler (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  );
+
+/**
+  Variable property set.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibVariablePropertySet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  );
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibVariablePropertyGet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  );
+
+/**
+  SetVariable check.
+
+  @param[in] VariableName       Name of Variable to set.
+  @param[in] VendorGuid         Variable vendor GUID.
+  @param[in] Attributes         Attribute value of the variable.
+  @param[in] DataSize           Size of Data to set.
+  @param[in] Data               Data pointer.
+  @param[in] RequestSource      Request source.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER An invalid combination of attribute bits, name, GUID,
+                                DataSize and Data value was supplied.
+  @retval EFI_WRITE_PROTECTED   The variable in question is read-only.
+  @retval Others                The other return status from check handler.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibSetVariableCheck (
+  IN CHAR16                     *VariableName,
+  IN EFI_GUID                   *VendorGuid,
+  IN UINT32                     Attributes,
+  IN UINTN                      DataSize,
+  IN VOID                       *Data,
+  IN VAR_CHECK_REQUEST_SOURCE   RequestSource
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/AtaController.h b/Core/MdeModulePkg/Include/Ppi/AtaController.h
new file mode 100644
index 0000000000..aa4ae56d04
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/AtaController.h
@@ -0,0 +1,162 @@
+/** @file
+  Define the PPI to abstract the functions that enable IDE and SATA channels, and to retrieve
+  the base I/O port address for each of the enabled IDE and SATA channels.
+  
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _PEI_ATA_CONTROLLER_PPI_H_
+#define _PEI_ATA_CONTROLLER_PPI_H_
+
+///
+/// Global ID for the PEI_ATA_CONTROLLER_PPI. 
+///
+#define PEI_ATA_CONTROLLER_PPI_GUID \
+  { \
+    0xa45e60d1, 0xc719, 0x44aa, {0xb0, 0x7a, 0xaa, 0x77, 0x7f, 0x85, 0x90, 0x6d } \
+  }
+
+///
+/// Forward declaration for the PEI_ATA_CONTROLLER_PPI.
+///
+typedef struct _PEI_ATA_CONTROLLER_PPI PEI_ATA_CONTROLLER_PPI;
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// disable the IDE channels. 
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_IDE_NONE        0x00
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// enable the Primary IDE channel.
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_IDE_PRIMARY     0x01
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// enable the Secondary IDE channel.
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_IDE_SECONDARY   0x02
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// disable the SATA channel.
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_SATA_NONE       0x04
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// enable the Primary SATA channel.
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_SATA_PRIMARY    0x08
+
+///
+/// This bit is used in the ChannelMask parameter of EnableAtaChannel() to 
+/// enable the Secondary SATA channel.
+/// This is designed for old generation chipset with PATA/SATA controllers. 
+/// It may be ignored in PPI implementation for new generation chipset without PATA controller. 
+///
+#define PEI_ICH_SATA_SECONDARY  0x010
+
+///
+/// Structure that contains the base addresses for the IDE registers
+///
+typedef struct {
+  ///
+  /// Base I/O port address of the IDE controller's command block
+  ///
+  UINT16  CommandBlockBaseAddr;
+  ///
+  /// Base I/O port address of the IDE controller's control block
+  ///
+  UINT16  ControlBlockBaseAddr;
+} IDE_REGS_BASE_ADDR;
+
+/**
+  Sets IDE and SATA channels to an enabled or disabled state.
+
+  This service enables or disables the IDE and SATA channels specified by ChannelMask.
+  It may ignore ChannelMask setting to enable or disable IDE and SATA channels based on the platform policy. 
+  The number of the enabled channels will be returned by GET_IDE_REGS_BASE_ADDR() function. 
+
+  If the new state is set, then EFI_SUCCESS is returned.  If the new state can
+  not be set, then EFI_DEVICE_ERROR is returned.
+
+  @param[in] PeiServices   The pointer to the PEI Services Table.
+  @param[in] This          The pointer to this instance of the PEI_ATA_CONTROLLER_PPI.
+  @param[in] ChannelMask   The bitmask that identifies the IDE and SATA channels to 
+                           enable or disable. This paramter is optional. 
+
+  @retval EFI_SUCCESS        The IDE or SATA channels were enabled or disabled successfully.
+  @retval EFI_DEVICE_ERROR   The IDE or SATA channels could not be enabled or disabled.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_ENABLE_ATA)(
+  IN EFI_PEI_SERVICES        **PeiServices,
+  IN PEI_ATA_CONTROLLER_PPI  *This,
+  IN UINT8                   ChannelMask
+  );
+
+/**
+  Retrieves the I/O port base addresses for command and control registers of the 
+  enabled IDE/SATA channels.
+
+  This service fills in the structure poionted to by IdeRegsBaseAddr with the I/O
+  port base addresses for the command and control registers of the IDE and SATA
+  channels that were previously enabled in EnableAtaChannel().  The number of 
+  enabled IDE and SATA channels is returned.
+
+  @param[in]  PeiServices       The pointer to the PEI Services Table.
+  @param[in]  This              The pointer to this instance of the PEI_ATA_CONTROLLER_PPI.
+  @param[out] IdeRegsBaseAddr   The pointer to caller allocated space to return the 
+                                I/O port base addresses of the IDE and SATA channels 
+                                that were previosuly enabled with EnableAtaChannel().
+
+  @return   The number of enabled IDE and SATA channels in the platform.
+
+**/
+typedef
+UINT32
+(EFIAPI *GET_IDE_REGS_BASE_ADDR)(
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  PEI_ATA_CONTROLLER_PPI  *This,
+  OUT IDE_REGS_BASE_ADDR      *IdeRegsBaseAddr 
+  );
+
+///
+/// This PPI contains services to enable and disable IDE and SATA channels and
+/// retrieves the base I/O port addresses to the enabled IDE and SATA channels.
+///
+struct _PEI_ATA_CONTROLLER_PPI {
+  PEI_ENABLE_ATA          EnableAtaChannel;
+  GET_IDE_REGS_BASE_ADDR  GetIdeRegsBaseAddr;
+};
+
+extern EFI_GUID gPeiAtaControllerPpiGuid;
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Include/Ppi/IpmiPpi.h b/Core/MdeModulePkg/Include/Ppi/IpmiPpi.h
new file mode 100644
index 0000000000..b8f7063424
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/IpmiPpi.h
@@ -0,0 +1,65 @@
+/** @file
+  Ppi for Ipmi of SMS.
+
+  Copyright (c) 2014 - 2015, 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.
+
+**/
+
+#ifndef _IPMI_PPI_H_
+#define _IPMI_PPI_H_
+
+typedef struct _PEI_IPMI_PPI PEI_IPMI_PPI;
+
+#define PEI_IPMI_PPI_GUID \
+  { \
+    0xa9731431, 0xd968, 0x4277, 0xb7, 0x52, 0xa3, 0xa9, 0xa6, 0xae, 0x18, 0x98 \
+  }
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         This              This point for PEI_IPMI_PPI structure.
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_IPMI_SUBMIT_COMMAND) (
+  IN     PEI_IPMI_PPI                      *This,
+  IN     UINT8                             NetFunction,
+  IN     UINT8                             Command,
+  IN     UINT8                             *RequestData,
+  IN     UINT32                            RequestDataSize,
+     OUT UINT8                             *ResponseData,
+  IN OUT UINT32                            *ResponseDataSize
+  );
+
+//
+// IPMI PPI
+//
+struct _PEI_IPMI_PPI {
+  PEI_IPMI_SUBMIT_COMMAND       IpmiSubmitCommand;
+};
+
+extern EFI_GUID gPeiIpmiPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/PostBootScriptTable.h b/Core/MdeModulePkg/Include/Ppi/PostBootScriptTable.h
new file mode 100644
index 0000000000..5c152dfda0
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/PostBootScriptTable.h
@@ -0,0 +1,27 @@
+/** @file
+  POST BootScript Table PPI definition.
+
+  This PPI is used to be notification after boot script table execution.
+
+  Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _PEI_POST_BOOT_SCRIPT_TABLE_H_
+#define _PEI_POST_BOOT_SCRIPT_TABLE_H_
+
+#define PEI_POST_BOOT_SCRIPT_TABLE_PPI_GUID  \
+  {0x88c9d306, 0x900, 0x4eb5, 0x82, 0x60, 0x3e, 0x2d, 0xbe, 0xda, 0x1f, 0x89};
+
+extern EFI_GUID   gPeiPostScriptTablePpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/SecPerformance.h b/Core/MdeModulePkg/Include/Ppi/SecPerformance.h
new file mode 100644
index 0000000000..f8e2cf171e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/SecPerformance.h
@@ -0,0 +1,67 @@
+/** @file
+  Defines the interface to convey performance information from SEC phase to PEI.
+
+Copyright (c) 2011, 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.
+
+**/
+
+#ifndef _PEI_SEC_PERFORMANCE_PPI_H_
+#define _PEI_SEC_PERFORMANCE_PPI_H_
+
+#define PEI_SEC_PERFORMANCE_PPI_GUID \
+  { \
+    0x0ecc666b, 0x4662, 0x47f9, {0x9d, 0xd5, 0xd0, 0x96, 0xff, 0x7d, 0xa4, 0x9e } \
+  }
+
+typedef struct _PEI_SEC_PERFORMANCE_PPI PEI_SEC_PERFORMANCE_PPI;
+
+///
+/// Performance data collected in SEC phase.
+///
+typedef struct {
+  UINT64         ResetEnd; ///< Timer value logged at the beginning of firmware image execution, in unit of nanosecond.
+} FIRMWARE_SEC_PERFORMANCE;
+
+/**
+  This interface conveys performance information out of the Security (SEC) phase into PEI.
+
+  This service is published by the SEC phase. The SEC phase handoff has an optional
+  EFI_PEI_PPI_DESCRIPTOR list as its final argument when control is passed from SEC into the
+  PEI Foundation. As such, if the platform supports collecting performance data in SEC,
+  this information is encapsulated into the data structure abstracted by this service.
+  This information is collected for the boot-strap processor (BSP) on IA-32.
+
+  @param[in]  PeiServices  The pointer to the PEI Services Table.
+  @param[in]  This         The pointer to this instance of the PEI_SEC_PERFORMANCE_PPI.
+  @param[out] Performance  The pointer to performance data collected in SEC phase.
+
+  @retval EFI_SUCCESS      The performance data was successfully returned.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *GET_SEC_PERFORMANCE) (
+  IN CONST EFI_PEI_SERVICES          **PeiServices,
+  IN       PEI_SEC_PERFORMANCE_PPI   *This,
+  OUT      FIRMWARE_SEC_PERFORMANCE  *Performance
+  );
+
+///
+/// This PPI provides function to get performance data collected in SEC phase.
+///
+struct _PEI_SEC_PERFORMANCE_PPI {
+  GET_SEC_PERFORMANCE         GetPerformance;
+};
+
+extern EFI_GUID gPeiSecPerformancePpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/SerialPortPei.h b/Core/MdeModulePkg/Include/Ppi/SerialPortPei.h
new file mode 100644
index 0000000000..2b754f9cca
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/SerialPortPei.h
@@ -0,0 +1,26 @@
+/** @file
+  PPI that is installed after the initialization of a serial stream device 
+  is complete.  
+
+  Copyright (c) 2010 - 2014, 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.
+
+**/
+
+#ifndef __PEI_SERIAL_PORT_PPI_H__
+#define __PEI_SERIAL_PORT_PPI_H__
+
+#define PEI_SERIAL_PORT_PPI \
+  { \
+    0x490e9d85, 0x8aef, 0x4193, { 0x8e, 0x56, 0xf7, 0x34, 0xa9, 0xff, 0xac, 0x8b } \
+  }
+
+extern EFI_GUID gPeiSerialPortPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/SmmAccess.h b/Core/MdeModulePkg/Include/Ppi/SmmAccess.h
new file mode 100644
index 0000000000..085ad644c8
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/SmmAccess.h
@@ -0,0 +1,145 @@
+/** @file
+  EFI SMM Access PPI definition.
+
+  This PPI is used to control the visibility of the SMRAM on the platform.
+  It abstracts the location and characteristics of SMRAM.  The expectation is
+  that the north bridge or memory controller would publish this PPI.
+
+  The principal functionality found in the memory controller includes the following: 
+  - Exposing the SMRAM to all non-SMM agents, or the "open" state
+  - Shrouding the SMRAM to all but the SMM agents, or the "closed" state
+  - Preserving the system integrity, or "locking" the SMRAM, such that the settings cannot be 
+    perturbed by either boot service or runtime agents 
+
+Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _SMM_ACCESS_PPI_H_
+#define _SMM_ACCESS_PPI_H_
+
+#define PEI_SMM_ACCESS_PPI_GUID \
+  { 0x268f33a9, 0xcccd, 0x48be, { 0x88, 0x17, 0x86, 0x5, 0x3a, 0xc3, 0x2e, 0xd6 }}
+
+typedef struct _PEI_SMM_ACCESS_PPI  PEI_SMM_ACCESS_PPI;
+
+/**
+  Opens the SMRAM area to be accessible by a PEIM driver.
+
+  This function "opens" SMRAM so that it is visible while not inside of SMM. The function should 
+  return EFI_UNSUPPORTED if the hardware does not support hiding of SMRAM. The function 
+  should return EFI_DEVICE_ERROR if the SMRAM configuration is locked.
+
+  @param  PeiServices            General purpose services available to every PEIM.
+  @param  This                   The pointer to the SMM Access Interface.
+  @param  DescriptorIndex        The region of SMRAM to Open.
+  
+  @retval EFI_SUCCESS            The region was successfully opened.
+  @retval EFI_DEVICE_ERROR       The region could not be opened because locked by chipset.
+  @retval EFI_INVALID_PARAMETER  The descriptor index was out of bounds.
+  
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_OPEN)(
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN PEI_SMM_ACCESS_PPI              *This,
+  IN UINTN                           DescriptorIndex
+  );
+
+/**
+  Inhibits access to the SMRAM.
+
+  This function "closes" SMRAM so that it is not visible while outside of SMM. The function should 
+  return EFI_UNSUPPORTED if the hardware does not support hiding of SMRAM.
+
+  @param  PeiServices              General purpose services available to every PEIM.
+  @param  This                     The pointer to the SMM Access Interface.
+  @param  DescriptorIndex          The region of SMRAM to Close.
+  
+  @retval EFI_SUCCESS              The region was successfully closed.
+  @retval EFI_DEVICE_ERROR         The region could not be closed because locked by chipset.                           
+  @retval EFI_INVALID_PARAMETER    The descriptor index was out of bounds.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_CLOSE)(
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN PEI_SMM_ACCESS_PPI              *This,
+  IN UINTN                           DescriptorIndex
+  );
+
+/**
+  Inhibits access to the SMRAM.
+
+  This function prohibits access to the SMRAM region.  This function is usually implemented such 
+  that it is a write-once operation. 
+
+  @param  PeiServices              General purpose services available to every PEIM.
+  @param  This                     The pointer to the SMM Access Interface.
+  @param  DescriptorIndex          The region of SMRAM to Close.
+  
+  @retval EFI_SUCCESS            The region was successfully locked.
+  @retval EFI_DEVICE_ERROR       The region could not be locked because at least
+                                 one range is still open.
+  @retval EFI_INVALID_PARAMETER  The descriptor index was out of bounds.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_LOCK)(
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN PEI_SMM_ACCESS_PPI              *This,
+  IN UINTN                           DescriptorIndex
+  );
+
+/**
+  Queries the memory controller for the possible regions that will support SMRAM.
+
+  @param  PeiServices           General purpose services available to every PEIM.
+  @param This                   The pointer to the SmmAccessPpi Interface.
+  @param SmramMapSize           The pointer to the variable containing size of the
+                                buffer to contain the description information.
+  @param SmramMap               The buffer containing the data describing the Smram
+                                region descriptors.
+  
+  @retval EFI_BUFFER_TOO_SMALL  The user did not provide a sufficient buffer.
+  @retval EFI_SUCCESS           The user provided a sufficiently-sized buffer.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_CAPABILITIES)(
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN PEI_SMM_ACCESS_PPI              *This,
+  IN OUT UINTN                       *SmramMapSize,
+  IN OUT EFI_SMRAM_DESCRIPTOR        *SmramMap
+  );
+
+///
+///  EFI SMM Access PPI is used to control the visibility of the SMRAM on the platform.
+///  It abstracts the location and characteristics of SMRAM.  The expectation is
+///  that the north bridge or memory controller would publish this PPI.
+/// 
+struct _PEI_SMM_ACCESS_PPI {
+  PEI_SMM_OPEN          Open;
+  PEI_SMM_CLOSE         Close;
+  PEI_SMM_LOCK          Lock;
+  PEI_SMM_CAPABILITIES  GetCapabilities;
+  BOOLEAN               LockState;
+  BOOLEAN               OpenState;
+};
+
+extern EFI_GUID gPeiSmmAccessPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/SmmCommunication.h b/Core/MdeModulePkg/Include/Ppi/SmmCommunication.h
new file mode 100644
index 0000000000..a22ed9f6d3
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/SmmCommunication.h
@@ -0,0 +1,64 @@
+/** @file
+  EFI SMM Communication PPI definition.
+
+  This Ppi provides a means of communicating between PEIM and SMI 
+  handlers inside of SMM.
+  This Ppi is produced and consumed only in S3 resume boot path.
+  It is NOT available in normal boot path.
+
+Copyright (c) 2010, 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.
+
+**/
+
+
+#ifndef _SMM_COMMUNICATION_PPI_H_
+#define _SMM_COMMUNICATION_PPI_H_
+
+#define EFI_PEI_SMM_COMMUNICATION_PPI_GUID \
+  { \
+    0xae933e1c, 0xcc47, 0x4e38, { 0x8f, 0xe, 0xe2, 0xf6, 0x1d, 0x26, 0x5, 0xdf } \
+  }
+
+typedef struct _EFI_PEI_SMM_COMMUNICATION_PPI  EFI_PEI_SMM_COMMUNICATION_PPI;
+
+/**
+  Communicates with a registered handler.
+  
+  This function provides a service to send and receive messages from a registered UEFI service.
+
+  @param[in] This                The EFI_PEI_SMM_COMMUNICATION_PPI instance.
+  @param[in] CommBuffer          A pointer to the buffer to convey into SMRAM.
+  @param[in] CommSize            The size of the data buffer being passed in.On exit, the size of data
+                                 being returned. Zero if the handler does not wish to reply with any data.
+
+  @retval EFI_SUCCESS            The message was successfully posted.
+  @retval EFI_INVALID_PARAMETER  The CommBuffer was NULL.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_PEI_SMM_COMMUNICATE)(
+  IN CONST EFI_PEI_SMM_COMMUNICATION_PPI   *This,
+  IN OUT VOID                              *CommBuffer,
+  IN OUT UINTN                             *CommSize
+  );
+
+///
+/// EFI SMM Communication Protocol provides runtime services for communicating
+/// between DXE drivers and a registered SMI handler.
+///
+struct _EFI_PEI_SMM_COMMUNICATION_PPI {
+  EFI_PEI_SMM_COMMUNICATE  Communicate;
+};
+
+extern EFI_GUID gEfiPeiSmmCommunicationPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/SmmControl.h b/Core/MdeModulePkg/Include/Ppi/SmmControl.h
new file mode 100644
index 0000000000..341a29ba2a
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/SmmControl.h
@@ -0,0 +1,96 @@
+/** @file
+  EFI SMM Control PPI definition.
+
+  This PPI is used to initiate SMI/PMI activations. This protocol could be published by either:
+  - A processor driver to abstract the SMI/PMI IPI
+  - The driver that abstracts the ASIC that is supporting the APM port, such as the ICH in an
+  Intel chipset
+  Because of the possibility of performing SMI or PMI IPI transactions, the ability to generate this
+  event from a platform chipset agent is an optional capability for both IA-32 and Itanium-based
+  systems.
+
+  Copyright (c) 2010, 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.
+
+**/
+
+
+#ifndef _SMM_CONTROL_PPI_H_
+#define _SMM_CONTROL_PPI_H_
+
+#define PEI_SMM_CONTROL_PPI_GUID \
+  { 0x61c68702, 0x4d7e, 0x4f43, 0x8d, 0xef, 0xa7, 0x43, 0x5, 0xce, 0x74, 0xc5 }
+
+typedef struct _PEI_SMM_CONTROL_PPI  PEI_SMM_CONTROL_PPI;
+
+/**
+  Invokes SMI activation from either the preboot or runtime environment.
+
+  @param  PeiServices           General purpose services available to every PEIM.
+  @param  This                  The PEI_SMM_CONTROL_PPI instance.
+  @param  ArgumentBuffer        The optional sized data to pass into the protocol activation.
+  @param  ArgumentBufferSize    The optional size of the data.
+  @param  Periodic              An optional mechanism to periodically repeat activation.
+  @param  ActivationInterval    An optional parameter to repeat at this period one
+                                time or, if the Periodic Boolean is set, periodically.
+
+  @retval EFI_SUCCESS           The SMI/PMI has been engendered.
+  @retval EFI_DEVICE_ERROR      The timing is unsupported.
+  @retval EFI_INVALID_PARAMETER The activation period is unsupported.
+  @retval EFI_NOT_STARTED       The SMM base service has not been initialized.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_ACTIVATE) (
+  IN EFI_PEI_SERVICES                                **PeiServices,
+  IN PEI_SMM_CONTROL_PPI                             * This,
+  IN OUT INT8                                        *ArgumentBuffer OPTIONAL,
+  IN OUT UINTN                                       *ArgumentBufferSize OPTIONAL,
+  IN BOOLEAN                                         Periodic OPTIONAL,
+  IN UINTN                                           ActivationInterval OPTIONAL
+  );
+
+/**
+  Clears any system state that was created in response to the Active call.
+
+  @param  PeiServices           General purpose services available to every PEIM.
+  @param  This                  The PEI_SMM_CONTROL_PPI instance.
+  @param  Periodic              Optional parameter to repeat at this period one 
+                                time or, if the Periodic Boolean is set, periodically.
+
+  @retval EFI_SUCCESS           The SMI/PMI has been engendered.
+  @retval EFI_DEVICE_ERROR      The source could not be cleared.
+  @retval EFI_INVALID_PARAMETER The service did not support the Periodic input argument.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_SMM_DEACTIVATE) (
+  IN EFI_PEI_SERVICES                      **PeiServices,
+  IN PEI_SMM_CONTROL_PPI                   * This,
+  IN BOOLEAN                               Periodic OPTIONAL
+  );
+
+///
+///  PEI SMM Control PPI is used to initiate SMI/PMI activations. This protocol could be published by either:
+///  - A processor driver to abstract the SMI/PMI IPI
+///  - The driver that abstracts the ASIC that is supporting the APM port, such as the ICH in an
+///  Intel chipset
+/// 
+struct _PEI_SMM_CONTROL_PPI {
+  PEI_SMM_ACTIVATE    Trigger;
+  PEI_SMM_DEACTIVATE  Clear;
+};
+
+extern EFI_GUID gPeiSmmControlPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/UfsHostController.h b/Core/MdeModulePkg/Include/Ppi/UfsHostController.h
new file mode 100644
index 0000000000..129636580e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/UfsHostController.h
@@ -0,0 +1,60 @@
+/** @file
+
+Copyright (c) 2014, 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.
+
+**/
+
+#ifndef _EDKII_PEI_UFS_HOST_CONTROLLER_PPI_H_
+#define _EDKII_PEI_UFS_HOST_CONTROLLER_PPI_H_
+
+///
+/// Global ID for the EDKII_UFS_HOST_CONTROLLER_PPI.
+///
+#define EDKII_UFS_HOST_CONTROLLER_PPI_GUID \
+  { \
+    0xdc54b283, 0x1a77, 0x4cd6, { 0x83, 0xbb, 0xfd, 0xda, 0x46, 0x9a, 0x2e, 0xc6 } \
+  }
+
+///
+/// Forward declaration for the UFS_HOST_CONTROLLER_PPI.
+///
+typedef struct _EDKII_UFS_HOST_CONTROLLER_PPI  EDKII_UFS_HOST_CONTROLLER_PPI;
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param[in]  This               The protocol instance pointer.
+  @param[in]  ControllerId       The ID of the UFS host controller.
+  @param[out] MmioBar            Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_GET_MMIO_BAR)(
+  IN     EDKII_UFS_HOST_CONTROLLER_PPI    *This,
+  IN     UINT8                            ControllerId,
+     OUT UINTN                            *MmioBar
+  );
+
+///
+/// This PPI contains a set of services to interact with the UFS host controller.
+///
+struct _EDKII_UFS_HOST_CONTROLLER_PPI {
+  EDKII_UFS_HC_GET_MMIO_BAR               GetUfsHcMmioBar;
+};
+
+extern EFI_GUID gEdkiiPeiUfsHostControllerPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/Usb2HostController.h b/Core/MdeModulePkg/Include/Ppi/Usb2HostController.h
new file mode 100644
index 0000000000..7bcb341a94
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/Usb2HostController.h
@@ -0,0 +1,269 @@
+/** @file
+  Defines the USB Host Controller PPI that provides I/O services for a USB Host 
+  Controller that may be used to access recovery devices.  These interfaces are 
+  modeled on the UEFI 2.3 specification EFI_USB2_HOST_CONTROLLER_PROTOCOL.
+  Refer to section 16.1 of the UEFI 2.3 Specification for more information on 
+  these interfaces.
+ 
+Copyright (c) 2010 - 2013, 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.
+
+**/
+
+#ifndef _PEI_USB2_HOST_CONTROLLER_PPI_H_
+#define _PEI_USB2_HOST_CONTROLLER_PPI_H_
+
+#include <Protocol/Usb2HostController.h>
+
+///
+/// Global ID for the PEI_USB2_HOST_CONTROLLER_PPI.
+///
+#define PEI_USB2_HOST_CONTROLLER_PPI_GUID \
+  { \
+    0xa7d09fe1, 0x74d4, 0x4ba5, { 0x84, 0x7c, 0x12, 0xed, 0x5b, 0x19, 0xad, 0xe4 } \
+  }
+
+///
+/// Forward declaration for the PEI_USB2_HOST_CONTROLLER_PPI.
+///
+typedef struct _PEI_USB2_HOST_CONTROLLER_PPI PEI_USB2_HOST_CONTROLLER_PPI;
+
+/**
+  Initiate a USB control transfer using a specific USB Host controller on the USB bus. 
+
+  @param[in]     PeiServices           The pointer to the PEI Services Table.
+  @param[in]     This                  The pointer to this instance of the 
+                                       PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[in]     DeviceAddress         Represents the address of the target device 
+                                       on the USB.
+  @param[in]     DeviceSpeed           Indicates device speed.
+  @param[in]     MaximumPacketLength   Indicates the maximum packet size that the 
+                                       default control transfer
+                                       endpoint is capable of sending or receiving.
+  @param[in]     Request               A pointer to the USB device request that 
+                                       will be sent to the USB device.
+  @param[in]     TransferDirection     Specifies the data direction for the transfer. 
+                                       There are three values available: 
+                                       EfiUsbDataIn, EfiUsbDataOut and EfiUsbNoData.
+  @param[in,out] Data                  A pointer to the buffer of data that will 
+                                       be transmitted to USB device or
+                                       received from USB device.
+  @param[in,out] DataLength            On input, indicates the size, in bytes, of 
+                                       the data buffer specified by Data.
+                                       On output, indicates the amount of data 
+                                       actually transferred.
+  @param[in]     TimeOut               Indicates the maximum time, in milliseconds, 
+                                       that the transfer is allowed to complete.
+                                       If Timeout is 0, then the caller must wait for
+                                       the function to be completed until EFI_SUCCESS
+                                       or EFI_DEVICE_ERROR is returned.
+  @param[in]     Translator            A pointer to the transaction translator data.
+  @param[out]    TransferResult        A pointer to the detailed result information 
+                                       generated by this control transfer.
+
+  @retval EFI_SUCCESS           The control transfer was completed successfully.
+  @retval EFI_DEVICE_ERROR      The control transfer failed due to host controller 
+                                or device error.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The control transfer could not be completed due to a lack of resources.
+  @retval EFI_TIMEOUT           The control transfer failed due to timeout.
+  
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_CONTROL_TRANSFER)(
+  IN EFI_PEI_SERVICES                         **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI             *This,
+  IN     UINT8                                DeviceAddress,
+  IN     UINT8                                DeviceSpeed,
+  IN     UINTN                                MaximumPacketLength,
+  IN     USB_DEVICE_REQUEST                   *Request,
+  IN     EFI_USB_DATA_DIRECTION               TransferDirection,
+  IN OUT VOID                                 *Data OPTIONAL,
+  IN OUT UINTN                                *DataLength OPTIONAL,
+  IN     UINTN                                TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR   *Translator,
+  OUT    UINT32                               *TransferResult
+  );
+
+/**
+  Initiate a USB bulk transfer using a specific USB Host controller on the USB bus. 
+
+  @param[in]     PeiServices           The pointer to the PEI Services Table.
+  @param[in]     This                  The pointer to this instance of the 
+                                       PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[in]     DeviceAddress         Represents the address of the target device 
+                                       on the USB.
+  @param[in]     EndPointAddress       The combination of an endpoint number and 
+                                       an endpoint direction of the target USB device.
+  @param[in]     DeviceSpeed           Indicates device speed.
+  @param[in]     MaximumPacketLength   Indicates the maximum packet size the target 
+                                       endpoint is capable of sending or receiving.
+  @param[in,out] Data                  Array of pointers to the buffers of data 
+                                       that will be transmitted to USB device or 
+                                       received from USB device.
+  @param[in,out] DataLength            When input, indicates the size, in bytes, of  
+                                       the data buffers specified by Data. When output,
+                                       indicates the data size actually transferred.
+  @param[in,out] DataToggle            A pointer to the data toggle value.
+  @param[in]     TimeOut               Indicates the maximum time, in milliseconds,
+                                       in which the transfer is allowed to complete.
+                                       If Timeout is 0, then the caller must wait for
+                                       the function to be completed until EFI_SUCCESS
+                                       or EFI_DEVICE_ERROR is returned.
+  @param[in]     Translator            A pointer to the transaction translator data.
+  @param[out]    TransferResult        A pointer to the detailed result information 
+                                       of the bulk transfer.
+
+  @retval EFI_SUCCESS           The bulk transfer was completed successfully.
+  @retval EFI_DEVICE_ERROR      The bulk transfer failed due to host controller or device error.
+                                Caller should check TransferResult for detailed error information.
+  @retval EFI_INVALID_PARAMETER Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The bulk transfer could not be submitted due to a lack of resources.
+  @retval EFI_TIMEOUT           The bulk transfer failed due to timeout.  
+  
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_BULK_TRANSFER)(
+  IN     EFI_PEI_SERVICES                    **PeiServices,
+  IN     PEI_USB2_HOST_CONTROLLER_PPI        *This,
+  IN     UINT8                               DeviceAddress,
+  IN     UINT8                               EndPointAddress,
+  IN     UINT8                               DeviceSpeed,  
+  IN     UINTN                               MaximumPacketLength,
+  IN OUT VOID                                *Data[EFI_USB_MAX_BULK_BUFFER_NUM],
+  IN OUT UINTN                               *DataLength,
+  IN OUT UINT8                               *DataToggle,
+  IN     UINTN                               TimeOut,
+  IN     EFI_USB2_HC_TRANSACTION_TRANSLATOR  *Translator,
+  OUT    UINT32                              *TransferResult
+  );
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices       The pointer to the PEI Services Table.
+  @param[in]  This              The pointer to this instance of the 
+                                PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber        The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_GET_ROOTHUB_PORT_NUMBER)(
+  IN  EFI_PEI_SERVICES              **PeiServices,
+  IN  PEI_USB2_HOST_CONTROLLER_PPI  *This,
+  OUT UINT8                         *PortNumber
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param[in]  PeiServices       The pointer to the PEI Services Table.
+  @param[in]  This              The pointer to this instance of the 
+                                PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[in]  PortNumber        Specifies the root hub port from which the status is 
+                                to be retrieved.
+                                This value is zero based.
+  @param[out] PortStatus        A pointer to the current port status bits and port 
+                                status change bits.
+  
+  @retval EFI_SUCCESS           The status of the USB root hub port specified by 
+                                PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_GET_ROOTHUB_PORT_STATUS)(
+  IN  EFI_PEI_SERVICES              **PeiServices,
+  IN  PEI_USB2_HOST_CONTROLLER_PPI  *This,
+  IN  UINT8                         PortNumber,
+  OUT EFI_USB_PORT_STATUS           *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param[in] PeiServices        The pointer to the PEI Services Table.
+  @param[in] This               The pointer to this instance of the 
+                                PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[in] PortNumber         Specifies the root hub port whose feature is requested 
+                                to be set. This value is zero based.
+  @param[in] PortFeature        Indicates the feature selector associated with the feature 
+                                set request.
+  
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set for 
+                                the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid 
+                                for this function.
+  @retval EFI_TIMEOUT           The time out occurred                                
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_SET_ROOTHUB_PORT_FEATURE)(
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param[in] PeiServices          The pointer to the PEI Services Table.
+  @param[in] This                 The pointer to this instance of the 
+                                  PEI_USB2_HOST_CONTROLLER_PPI.
+  @param[in] PortNumber           Specifies the root hub port whose feature is
+                                  requested to be cleared.
+  @param[in] PortFeature          Indicates the feature selector associated with the
+                                  feature clear request.
+
+  @return EFI_SUCCESS             The feature specified by PortFeature was cleared
+                                  for the USB root hub port specified by PortNumber.
+  @return EFI_INVALID_PARAMETER   PortNumber is invalid or PortFeature is invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB2_HOST_CONTROLLER_CLEAR_ROOTHUB_PORT_FEATURE)(
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB2_HOST_CONTROLLER_PPI   *This,
+  IN UINT8                          PortNumber,
+  IN EFI_USB_PORT_FEATURE           PortFeature
+  );
+
+///
+/// This PPI contains a set of services to interact with the USB host controller.
+/// These interfaces are modeled on the UEFI 2.3 specification protocol
+/// EFI_USB2_HOST_CONTROLLER_PROTOCOL.  Refer to section 16.1 of the UEFI 2.3 
+/// Specification for more information on these interfaces.
+///
+struct _PEI_USB2_HOST_CONTROLLER_PPI {
+  PEI_USB2_HOST_CONTROLLER_CONTROL_TRANSFER            ControlTransfer;
+  PEI_USB2_HOST_CONTROLLER_BULK_TRANSFER               BulkTransfer;
+  PEI_USB2_HOST_CONTROLLER_GET_ROOTHUB_PORT_NUMBER     GetRootHubPortNumber;
+  PEI_USB2_HOST_CONTROLLER_GET_ROOTHUB_PORT_STATUS     GetRootHubPortStatus;
+  PEI_USB2_HOST_CONTROLLER_SET_ROOTHUB_PORT_FEATURE    SetRootHubPortFeature;
+  PEI_USB2_HOST_CONTROLLER_CLEAR_ROOTHUB_PORT_FEATURE  ClearRootHubPortFeature;
+};
+
+extern EFI_GUID gPeiUsb2HostControllerPpiGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Ppi/UsbController.h b/Core/MdeModulePkg/Include/Ppi/UsbController.h
new file mode 100644
index 0000000000..f0537ecb66
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/UsbController.h
@@ -0,0 +1,94 @@
+/** @file
+  Define APIs to retrieve USB Host Controller Info such as controller type and 
+  I/O Port Base Address.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _PEI_USB_CONTROLLER_PPI_H_
+#define _PEI_USB_CONTROLLER_PPI_H_
+
+///
+/// Global ID for the PEI_USB_CONTROLLER_PPI.
+///
+#define PEI_USB_CONTROLLER_PPI_GUID \
+  { \
+    0x3bc1f6de, 0x693e, 0x4547,{ 0xa3, 0x0, 0x21, 0x82, 0x3c, 0xa4, 0x20, 0xb2} \
+  }
+
+///
+/// Forward declaration for the PEI_USB_CONTROLLER_PPI.
+///
+typedef struct _PEI_USB_CONTROLLER_PPI PEI_USB_CONTROLLER_PPI;
+
+///
+/// This bit is used in the ControllerType return parameter of GetUsbController()
+/// to identify the USB Host Controller type as UHCI
+///
+#define PEI_UHCI_CONTROLLER 0x01
+
+///
+/// This bit is used in the ControllerType return parameter of GetUsbController()
+/// to identify the USB Host Controller type as OHCI
+///
+#define PEI_OHCI_CONTROLLER 0x02
+
+///
+/// This bit is used in the ControllerType return parameter of GetUsbController()
+/// to identify the USB Host Controller type as EHCI
+///
+#define PEI_EHCI_CONTROLLER 0x03
+
+///
+/// This bit is used in the ControllerType return parameter of GetUsbController()
+/// to identify the USB Host Controller type as XHCI
+///
+#define PEI_XHCI_CONTROLLER 0x04
+
+/**
+  Retrieve USB Host Controller Info such as controller type and I/O Base Address.
+
+  @param[in]  PeiServices      The pointer to the PEI Services Table.
+  @param[in]  This             The pointer to this instance of the PEI_USB_CONTROLLER_PPI.
+  @param[in]  ControllerId     The ID of the USB controller.
+  @param[out] ControllerType   On output, returns the type of the USB controller.
+  @param[out] BaseAddress      On output, returns the base address of UHCI's I/O ports
+                               if UHCI is enabled or the base address of EHCI's MMIO 
+                               if EHCI is enabled.
+
+  @retval EFI_SUCCESS             USB controller attributes were returned successfully.
+  @retval EFI_INVALID_PARAMETER   ControllerId is greater than the maximum number 
+                                  of USB controller supported by this platform.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_GET_USB_CONTROLLER)(
+  IN  EFI_PEI_SERVICES        **PeiServices,
+  IN  PEI_USB_CONTROLLER_PPI  *This,
+  IN  UINT8                   UsbControllerId,
+  OUT UINTN                   *ControllerType,
+  OUT UINTN                   *BaseAddress
+  );
+
+///
+/// This PPI contains a single service to retrieve the USB Host Controller type
+/// and the base address of the I/O ports used to access the USB Host Controller.
+///
+struct _PEI_USB_CONTROLLER_PPI {
+  PEI_GET_USB_CONTROLLER  GetUsbController;
+};
+
+extern EFI_GUID gPeiUsbControllerPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Ppi/UsbHostController.h b/Core/MdeModulePkg/Include/Ppi/UsbHostController.h
new file mode 100644
index 0000000000..232a666734
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/UsbHostController.h
@@ -0,0 +1,257 @@
+/** @file
+  Defines the USB Host Controller PPI that provides I/O services for a USB Host 
+  Controller that may be used to access recovery devices.  These interfaces are 
+  modeled on the UEFI 2.3 specification EFI_USB2_HOST_CONTROLLER_PROTOCOL.
+  Refer to section 16.1 of the UEFI 2.3 Specification for more information on 
+  these interfaces.
+ 
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef _PEI_USB_HOST_CONTROLLER_PPI_H_
+#define _PEI_USB_HOST_CONTROLLER_PPI_H_
+
+#include <Protocol/Usb2HostController.h>
+
+///
+/// Global ID for the PEI_USB_HOST_CONTROLLER_PPI.
+///
+#define PEI_USB_HOST_CONTROLLER_PPI_GUID \
+  { \
+    0x652b38a9, 0x77f4, 0x453f, { 0x89, 0xd5, 0xe7, 0xbd, 0xc3, 0x52, 0xfc, 0x53} \
+  }
+
+///
+/// Forward declaration for the PEI_USB_HOST_CONTROLLER_PPI.
+///
+typedef struct _PEI_USB_HOST_CONTROLLER_PPI PEI_USB_HOST_CONTROLLER_PPI;
+
+/**
+  Initiate a USB control transfer using a specific USB Host controller on the USB bus. 
+
+  @param[in]     PeiServices           The pointer to the PEI Services Table.
+  @param[in]     This                  The pointer to this instance of the 
+                                       PEI_USB_HOST_CONTROLLER_PPI.
+  @param[in]     DeviceAddress         Represents the address of the target device 
+                                       on the USB.
+  @param[in]     DeviceSpeed           Indicates device speed.
+  @param[in]     MaximumPacketLength   Indicates the maximum packet size that the 
+                                       default control transfer
+                                       endpoint is capable of sending or receiving.
+  @param[in]     Request               A pointer to the USB device request that 
+                                       will be sent to the USB device.
+  @param[in]     TransferDirection     Specifies the data direction for the transfer. 
+                                       There are three values available: 
+                                       EfiUsbDataIn, EfiUsbDataOut and EfiUsbNoData.
+  @param[in,out] Data                  A pointer to the buffer of data that will 
+                                       be transmitted to USB device or
+                                       received from USB device.
+  @param[in,out] DataLength            On input, indicates the size, in bytes, of 
+                                       the data buffer specified by Data.
+                                       On output, indicates the amount of data 
+                                       actually transferred.
+  @param[in]     TimeOut               Indicates the maximum time, in milliseconds, 
+                                       that the transfer is allowed to complete. 
+                                       If Timeout is 0, then the caller must wait for
+                                       the function to be completed until EFI_SUCCESS
+                                       or EFI_DEVICE_ERROR is returned.
+  @param[out]    TransferResult        A pointer to the detailed result information 
+                                       generated by this control transfer.
+
+  @retval EFI_DEVICE_ERROR   The control transfer failed due to host controller 
+                             or device error.
+  @retval EFI_SUCCESS        The control transfer was completed successfully.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_CONTROL_TRANSFER)(
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI  *This,
+  IN     UINT8                    DeviceAddress,
+  IN     UINT8                    DeviceSpeed,
+  IN     UINT8                    MaximumPacketLength,
+  IN     USB_DEVICE_REQUEST       *Request,
+  IN     EFI_USB_DATA_DIRECTION   TransferDirection,
+  IN OUT VOID                     *Data OPTIONAL,
+  IN OUT UINTN                    *DataLength OPTIONAL,
+  IN     UINTN                    TimeOut,
+  OUT    UINT32                   *TransferResult
+  );
+
+/**
+  Initiate a USB bulk transfer using a specific USB Host controller on the USB bus. 
+
+  @param[in]     PeiServices           The pointer to the PEI Services Table.
+  @param[in]     This                  The pointer to this instance of the 
+                                       PEI_USB_HOST_CONTROLLER_PPI.
+  @param[in]     DeviceAddress         Represents the address of the target device 
+                                       on the USB.
+  @param[in]     EndPointAddress       The combination of an endpoint number and 
+                                       an endpoint direction of the target USB device.
+  @param[in]     MaximumPacketLength   Indicates the maximum packet size the target 
+                                       endpoint is capable of sending or receiving.
+  @param[in,out] Data                  Array of pointers to the buffers of data 
+                                       that will be transmitted to USB device or 
+                                       received from USB device.
+  @param[in,out] DataLength            When input, indicates the size, in bytes, of  
+                                       the data buffers specified by Data. When output,
+                                       indicates the data size actually transferred.
+  @param[in,out] DataToggle            A pointer to the data toggle value.
+  @param[in]     TimeOut               Indicates the maximum time, in milliseconds,
+                                       in which the transfer is allowed to complete.
+                                       If Timeout is 0, then the caller must wait for
+                                       the function to be completed until EFI_SUCCESS
+                                       or EFI_DEVICE_ERROR is returned.
+  @param[out]    TransferResult        A pointer to the detailed result information 
+                                       of the bulk transfer.
+
+  @retval EFI_SUCCESS           The bulk transfer was completed successfully.
+  @retval EFI_DEVICE_ERROR      The bulk transfer failed due to host controller or device error.
+                                Caller should check TransferResult for detailed error information.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_BULK_TRANSFER)(
+  IN     EFI_PEI_SERVICES             **PeiServices,
+  IN     PEI_USB_HOST_CONTROLLER_PPI  *This,
+  IN     UINT8                        DeviceAddress,
+  IN     UINT8                        EndPointAddress,
+  IN     UINT8                        MaximumPacketLength,
+  IN OUT VOID                         *Data,
+  IN OUT UINTN                        *DataLength,
+  IN OUT UINT8                        *DataToggle,
+  IN     UINTN                        TimeOut,
+  OUT    UINT32                       *TransferResult
+  );
+
+/**
+  Retrieves the number of root hub ports.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[out] PortNumber    The pointer to the number of the root hub ports.                                
+                                
+  @retval EFI_SUCCESS           The port number was retrieved successfully.
+  @retval EFI_DEVICE_ERROR      An error was encountered while attempting to retrieve 
+                                the port number.
+  @retval EFI_INVALID_PARAMETER PortNumber is NULL.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_GET_ROOTHUB_PORT_NUMBER)(
+  IN  EFI_PEI_SERVICES             **PeiServices,
+  IN  PEI_USB_HOST_CONTROLLER_PPI  *This,
+  OUT UINT8                        *PortNumber
+  );
+
+/**
+  Retrieves the current status of a USB root hub port.
+
+  @param[in]  PeiServices   The pointer to the PEI Services Table.
+  @param[in]  This          The pointer to this instance of the 
+                            PEI_USB_HOST_CONTROLLER_PPI.
+  @param[in]  PortNumber    Specifies the root hub port from which the status is 
+                            to be retrieved.
+                            This value is zero based.
+  @param[out] PortStatus    A pointer to the current port status bits and port 
+                            status change bits.
+  
+  @retval EFI_SUCCESS           The status of the USB root hub port specified by 
+                                PortNumber was returned in PortStatus.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_GET_ROOTHUB_PORT_STATUS)(
+  IN  EFI_PEI_SERVICES             **PeiServices,
+  IN  PEI_USB_HOST_CONTROLLER_PPI  *This,
+  IN  UINT8                        PortNumber,
+  OUT EFI_USB_PORT_STATUS          *PortStatus
+  );
+
+/**
+  Sets a feature for the specified root hub port.
+
+  @param[in] PeiServices   The pointer to the PEI Services Table.
+  @param[in] This          The pointer to this instance of the 
+                           PEI_USB_HOST_CONTROLLER_PPI.
+  @param[in] PortNumber    Specifies the root hub port whose feature is requested 
+                           to be set. This value is zero based.
+  @param[in] PortFeature   Indicates the feature selector associated with the feature 
+                           set request.
+  
+  @retval EFI_SUCCESS           The feature specified by PortFeature was set for 
+                                the USB root hub port specified by PortNumber.
+  @retval EFI_INVALID_PARAMETER PortNumber is invalid or PortFeature is invalid 
+                                for this function.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_SET_ROOTHUB_PORT_FEATURE)(
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI  *This,
+  IN UINT8                        PortNumber,
+  IN EFI_USB_PORT_FEATURE         PortFeature
+  );
+
+/**
+  Clears a feature for the specified root hub port.
+
+  @param[in] PeiServices   The pointer to the PEI Services Table.
+  @param[in] This          The pointer to this instance of the 
+                           PEI_USB_HOST_CONTROLLER_PPI.
+  @param[in] PortNumber    Specifies the root hub port whose feature is
+                           requested to be cleared.
+  @param[in] PortFeature   Indicates the feature selector associated with the
+                           feature clear request.
+
+  @return EFI_SUCCESS             The feature specified by PortFeature was cleared
+                                  for the USB root hub port specified by PortNumber.
+  @return EFI_INVALID_PARAMETER   PortNumber is invalid or PortFeature is invalid.
+  @return EFI_DEVICE_ERROR        Can't read the register.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_HOST_CONTROLLER_CLEAR_ROOTHUB_PORT_FEATURE)(
+  IN EFI_PEI_SERVICES             **PeiServices,
+  IN PEI_USB_HOST_CONTROLLER_PPI  *This,
+  IN UINT8                        PortNumber,
+  IN EFI_USB_PORT_FEATURE         PortFeature
+  );
+
+///
+/// This PPI contains a set of services to interact with the USB host controller.
+/// These interfaces are modeled on the UEFI 2.3 specification protocol
+/// EFI_USB2_HOST_CONTROLLER_PROTOCOL.  Refer to section 16.1 of the UEFI 2.3 
+/// Specification for more information on these interfaces.
+///
+struct _PEI_USB_HOST_CONTROLLER_PPI {
+  PEI_USB_HOST_CONTROLLER_CONTROL_TRANSFER            ControlTransfer;
+  PEI_USB_HOST_CONTROLLER_BULK_TRANSFER               BulkTransfer;
+  PEI_USB_HOST_CONTROLLER_GET_ROOTHUB_PORT_NUMBER     GetRootHubPortNumber;
+  PEI_USB_HOST_CONTROLLER_GET_ROOTHUB_PORT_STATUS     GetRootHubPortStatus;
+  PEI_USB_HOST_CONTROLLER_SET_ROOTHUB_PORT_FEATURE    SetRootHubPortFeature;
+  PEI_USB_HOST_CONTROLLER_CLEAR_ROOTHUB_PORT_FEATURE  ClearRootHubPortFeature;
+};
+
+extern EFI_GUID gPeiUsbHostControllerPpiGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Ppi/UsbIo.h b/Core/MdeModulePkg/Include/Ppi/UsbIo.h
new file mode 100644
index 0000000000..b024d4640a
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Ppi/UsbIo.h
@@ -0,0 +1,196 @@
+/** @file
+  Defines the PEI_USB_IO_PPI that the USB-related PEIM can use for I/O operations 
+  on the USB BUS.  This interface enables recovery from a 
+  USB-class storage device, such as USB CD/DVD, USB hard drive, or USB FLASH 
+  drive.  These interfaces are modeled on the UEFI 2.3 specification EFI_USB_IO_PROTOCOL.
+  Refer to section 16.2.4 of the UEFI 2.3 Specification for more information on 
+  these interfaces.
+
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef _PEI_USB_IO_PPI_H_
+#define _PEI_USB_IO_PPI_H_
+
+#include <Protocol/Usb2HostController.h>
+
+///
+/// Global ID for the PEI_USB_IO_PPI.
+///
+#define PEI_USB_IO_PPI_GUID \
+  { \
+    0x7c29785c, 0x66b9, 0x49fc, { 0xb7, 0x97, 0x1c, 0xa5, 0x55, 0xe, 0xf2, 0x83} \
+  }
+
+///
+/// Forward declaration for the PEI_USB_IO_PPI.
+///
+typedef struct _PEI_USB_IO_PPI  PEI_USB_IO_PPI;
+
+/**
+  Submits control transfer to a target USB device.
+
+  @param[in]     PeiServices   The pointer to the PEI Services Table.
+  @param[in]     This          The pointer to this instance of the PEI_USB_IO_PPI.
+  @param[in]     Request       A pointer to the USB device request that will be 
+                               sent to the USB device.
+  @param[in]     Direction     Specifies the data direction for the transfer. There 
+                               are three values available: 
+                               EfiUsbDataIn, EfiUsbDataOut and EfiUsbNoData.
+  @param[in]     Timeout       Indicates the maximum time, in milliseconds, that 
+                               the transfer is allowed to complete.
+                               If Timeout is 0, then the caller must wait for the
+                               function to be completed until EFI_SUCCESS or
+                               EFI_DEVICE_ERROR is returned.
+  @param[in,out] Data          A pointer to the buffer of data that will be 
+                               transmitted to or received from the USB device.
+  @param[in]     DataLength    On input, indicates the size, in bytes, of the data 
+                               buffer specified by Data.
+                               
+  @retval EFI_SUCCESS             The control transfer was completed successfully.
+  @retval EFI_INVALID_PARAMETER   Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES    The control transfer could not be completed due 
+                                  to a lack of resources.
+  @retval EFI_TIMEOUT             The control transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR        The control transfer failed due to host controller 
+                                  or device error.
+                                  Caller should check TransferResult for detailed 
+                                  error information.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_CONTROL_TRANSFER)(
+  IN     EFI_PEI_SERVICES        **PeiServices,
+  IN     PEI_USB_IO_PPI          *This,
+  IN     EFI_USB_DEVICE_REQUEST  *Request,
+  IN     EFI_USB_DATA_DIRECTION  Direction,
+  IN     UINT32                  Timeout,
+  IN OUT VOID                    *Data OPTIONAL,
+  IN     UINTN                   DataLength  OPTIONAL
+  );
+
+/**
+  Submits bulk transfer to a target USB device.
+
+  @param[in] PeiServices       The pointer to the PEI Services Table.
+  @param[in] This              The pointer to this instance of the PEI_USB_IO_PPI.
+  @param[in] DeviceEndpoint    The endpoint address.
+  @param[in] Data              The data buffer to be transfered.
+  @param[in] DataLength        The length of data buffer.
+  @param[in] Timeout           The timeout for the transfer, in milliseconds.
+                               If Timeout is 0, then the caller must wait for the
+                               function to be completed until EFI_SUCCESS or
+                               EFI_DEVICE_ERROR is returned.
+
+  @retval EFI_SUCCESS             The bulk transfer completed successfully.
+  @retval EFI_INVALID_PARAMETER   Some parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES    The bulk transfer could not be completed due to 
+                                  a lack of resources.
+  @retval EFI_TIMEOUT             The bulk transfer failed due to timeout.
+  @retval EFI_DEVICE_ERROR        The bulk transfer failed due to host controller 
+                                  or device error.
+                                  Caller should check TransferResult for detailed 
+                                  error information.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_BULK_TRANSFER)(
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_USB_IO_PPI    *This,
+  IN UINT8             DeviceEndpoint,
+  IN OUT VOID          *Data,
+  IN OUT UINTN         *DataLength,
+  IN UINTN             Timeout
+  );
+
+/**
+  Get interface descriptor from a USB device.
+
+  @param[in] PeiServices           The pointer to the PEI Services Table.
+  @param[in] This                  The pointer to this instance of the PEI_USB_IO_PPI.
+  @param[in] InterfaceDescriptor   The interface descriptor.
+
+  @retval EFI_SUCCESS             The interface descriptor was returned.
+  @retval EFI_INVALID_PARAMETER   Some parameters are invalid.
+  @retval EFI_DEVICE_ERROR        A device error occurred, the function failed to 
+                                  get the interface descriptor.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_GET_INTERFACE_DESCRIPTOR)(
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN PEI_USB_IO_PPI                *This,
+  IN EFI_USB_INTERFACE_DESCRIPTOR  **InterfaceDescriptor
+  );
+
+/**
+  Get endpoint descriptor from a USB device.
+
+  @param[in] PeiServices          The pointer to the PEI Services Table.
+  @param[in] This                 The pointer to this instance of the PEI_USB_IO_PPI.
+  @param[in] EndPointIndex        The index of the end point.
+  @param[in] EndpointDescriptor   The endpoint descriptor.
+
+  @retval EFI_SUCCESS             The endpoint descriptor was returned.
+  @retval EFI_INVALID_PARAMETER   Some parameters are invalid.
+  @retval EFI_DEVICE_ERROR        A device error occurred, the function failed to 
+                                  get the endpoint descriptor.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_GET_ENDPOINT_DESCRIPTOR)(
+  IN EFI_PEI_SERVICES               **PeiServices,
+  IN PEI_USB_IO_PPI                 *This,
+  IN UINT8                          EndpointIndex,
+  IN EFI_USB_ENDPOINT_DESCRIPTOR    **EndpointDescriptor
+  );
+
+/**
+  Issue a port reset to the device.
+
+  @param[in] PeiServices   The pointer to the PEI Services Table.
+  @param[in] This          The pointer to this instance of the PEI_USB_IO_PPI.
+
+  @retval EFI_SUCCESS             The port reset was issued successfully.
+  @retval EFI_INVALID_PARAMETER   Some parameters are invalid.
+  @retval EFI_DEVICE_ERROR        Device error occurred.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PEI_USB_PORT_RESET)(
+  IN EFI_PEI_SERVICES  **PeiServices,
+  IN PEI_USB_IO_PPI    *This
+  );
+
+///
+/// This PPI contains a set of services to interact with the USB host controller.
+/// These interfaces are modeled on the UEFI 2.3 specification EFI_USB_IO_PROTOCOL.
+/// Refer to section 16.2.4 of the UEFI 2.3 Specification for more information on 
+/// these interfaces.
+///
+struct _PEI_USB_IO_PPI {
+  PEI_USB_CONTROL_TRANSFER          UsbControlTransfer;
+  PEI_USB_BULK_TRANSFER             UsbBulkTransfer;
+  PEI_USB_GET_INTERFACE_DESCRIPTOR  UsbGetInterfaceDescriptor;
+  PEI_USB_GET_ENDPOINT_DESCRIPTOR   UsbGetEndpointDescriptor;
+  PEI_USB_PORT_RESET                UsbPortReset;
+};
+
+extern EFI_GUID gPeiUsbIoPpiGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/BootLogo.h b/Core/MdeModulePkg/Include/Protocol/BootLogo.h
new file mode 100644
index 0000000000..1eba47a704
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/BootLogo.h
@@ -0,0 +1,65 @@
+/** @file
+  Boot Logo protocol is used to convey information of Logo dispayed during boot.
+
+Copyright (c) 2011, 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 that 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.
+
+**/
+
+#ifndef _BOOT_LOGO_H_
+#define _BOOT_LOGO_H_
+
+#include <Protocol/GraphicsOutput.h>
+
+#define EFI_BOOT_LOGO_PROTOCOL_GUID \
+  { \
+    0xcdea2bd3, 0xfc25, 0x4c1c, { 0xb9, 0x7c, 0xb3, 0x11, 0x86, 0x6, 0x49, 0x90 } \
+  }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EFI_BOOT_LOGO_PROTOCOL  EFI_BOOT_LOGO_PROTOCOL;
+
+/**
+  Update information of logo image drawn on screen.
+
+  @param  This           The pointer to the Boot Logo protocol instance.
+  @param  BltBuffer      The BLT buffer for logo drawn on screen. If BltBuffer
+                         is set to NULL, it indicates that logo image is no
+                         longer on the screen.
+  @param  DestinationX   X coordinate of destination for the BltBuffer.
+  @param  DestinationY   Y coordinate of destination for the BltBuffer.
+  @param  Width          Width of rectangle in BltBuffer in pixels.
+  @param  Height         Hight of rectangle in BltBuffer in pixels.
+
+  @retval EFI_SUCCESS             The boot logo information was updated.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_OUT_OF_RESOURCES    The logo information was not updated due to
+                                  insufficient memory resources.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_SET_BOOT_LOGO)(
+  IN EFI_BOOT_LOGO_PROTOCOL            *This,
+  IN EFI_GRAPHICS_OUTPUT_BLT_PIXEL     *BltBuffer       OPTIONAL,
+  IN UINTN                             DestinationX,
+  IN UINTN                             DestinationY,
+  IN UINTN                             Width,
+  IN UINTN                             Height
+  );
+
+struct _EFI_BOOT_LOGO_PROTOCOL {
+  EFI_SET_BOOT_LOGO        SetBootLogo;
+};
+
+extern EFI_GUID gEfiBootLogoProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/DisplayProtocol.h b/Core/MdeModulePkg/Include/Protocol/DisplayProtocol.h
new file mode 100644
index 0000000000..8ea9350419
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/DisplayProtocol.h
@@ -0,0 +1,358 @@
+/** @file
+  FormDiplay protocol to show Form
+
+Copyright (c) 2013 - 2015, 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 that 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.
+
+**/
+
+#ifndef __DISPLAY_PROTOCOL_H__
+#define __DISPLAY_PROTOCOL_H__
+
+#include <Protocol/FormBrowser2.h>
+
+#define EDKII_FORM_DISPLAY_ENGINE_PROTOCOL_GUID  \
+  { 0x9bbe29e9, 0xfda1, 0x41ec, { 0xad, 0x52, 0x45, 0x22, 0x13, 0x74, 0x2d, 0x2e } }
+
+// 
+// Do nothing.
+//
+#define BROWSER_ACTION_NONE         BIT16
+//
+// ESC Exit
+//
+#define BROWSER_ACTION_FORM_EXIT    BIT17
+
+#define BROWSER_SUCCESS                   0x0
+#define BROWSER_ERROR                     BIT31
+#define BROWSER_SUBMIT_FAIL               BROWSER_ERROR | 0x01
+#define BROWSER_NO_SUBMIT_IF              BROWSER_ERROR | 0x02
+#define BROWSER_FORM_NOT_FOUND            BROWSER_ERROR | 0x03
+#define BROWSER_FORM_SUPPRESS             BROWSER_ERROR | 0x04
+#define BROWSER_PROTOCOL_NOT_FOUND        BROWSER_ERROR | 0x05
+#define BROWSER_INCONSISTENT_IF           BROWSER_ERROR | 0x06
+#define BROWSER_WARNING_IF                BROWSER_ERROR | 0x07
+#define BROWSER_SUBMIT_FAIL_NO_SUBMIT_IF  BROWSER_ERROR | 0x08
+#define BROWSER_RECONNECT_REQUIRED        BROWSER_ERROR | 0x09
+#define BROWSER_RECONNECT_FAIL            BROWSER_ERROR | 0x0A
+#define BROWSER_RECONNECT_SAVE_CHANGES    BROWSER_ERROR | 0x0B
+
+#define FORM_DISPLAY_ENGINE_STATEMENT_VERSION_1  0x10000
+#define FORM_DISPLAY_ENGINE_VERSION_1            0x10000
+
+typedef struct {
+  //
+  // HII Data Type
+  //
+  UINT8               Type;
+  //
+  // Buffer Data and Length if Type is EFI_IFR_TYPE_BUFFER or EFI_IFR_TYPE_STRING
+  //
+  UINT8               *Buffer;
+  UINT16              BufferLen;
+  EFI_IFR_TYPE_VALUE  Value;
+} EFI_HII_VALUE;
+
+#define DISPLAY_QUESTION_OPTION_SIGNATURE  SIGNATURE_32 ('Q', 'O', 'P', 'T')
+
+typedef struct {
+  UINTN                  Signature;
+  LIST_ENTRY             Link;
+  //
+  // OneOfOption Data
+  //
+  EFI_IFR_ONE_OF_OPTION  *OptionOpCode;
+  //
+  // Option ImageId and AnimationId
+  //
+  EFI_IMAGE_ID           ImageId;
+  EFI_ANIMATION_ID       AnimationId;
+} DISPLAY_QUESTION_OPTION;
+
+#define DISPLAY_QUESTION_OPTION_FROM_LINK(a)  CR (a, DISPLAY_QUESTION_OPTION, Link, DISPLAY_QUESTION_OPTION_SIGNATURE)
+
+typedef struct _FORM_DISPLAY_ENGINE_STATEMENT FORM_DISPLAY_ENGINE_STATEMENT;
+typedef struct _FORM_DISPLAY_ENGINE_FORM      FORM_DISPLAY_ENGINE_FORM;
+
+#define STATEMENT_VALID             0x0
+#define STATEMENT_INVALID           BIT31
+
+#define INCOSISTENT_IF_TRUE         STATEMENT_INVALID | 0x01
+#define WARNING_IF_TRUE             STATEMENT_INVALID | 0x02
+#define STRING_TOO_LONG             STATEMENT_INVALID | 0x03
+// ... to be extended.
+
+typedef struct {
+  //
+  // StringId for INCONSITENT_IF or WARNING_IF
+  //
+  EFI_STRING_ID  StringId;
+  //
+  // TimeOut for WARNING_IF
+  //
+  UINT8          TimeOut;
+} STATEMENT_ERROR_INFO;
+
+/**
+  Perform value check for a question.
+  
+  @param  Form       Form where Statement is in.
+  @param  Statement  Value will check for it.
+  @param  Value      New value will be checked.
+  
+  @retval Status     Value Status
+
+**/
+typedef
+UINT32
+(EFIAPI *VALIDATE_QUESTION) (
+  IN FORM_DISPLAY_ENGINE_FORM      *Form,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN EFI_HII_VALUE                 *Value, 
+  OUT STATEMENT_ERROR_INFO         *ErrorInfo
+  );
+
+/**
+  Perform Password check. 
+  Passwork may be encrypted by driver that requires the specific check.
+  
+  @param  Form             Form where Password Statement is in.
+  @param  Statement        Password statement
+  @param  PasswordString   Password string to be checked. It may be NULL.
+                           NULL means to restore password.
+                           "" string can be used to checked whether old password does exist.
+  
+  @return Status     Status of Password check.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *PASSWORD_CHECK) (
+  IN FORM_DISPLAY_ENGINE_FORM      *Form,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN EFI_STRING                    PasswordString  OPTIONAL
+  );
+
+#define FORM_DISPLAY_ENGINE_STATEMENT_SIGNATURE  SIGNATURE_32 ('F', 'S', 'T', 'A')
+
+//
+// Attribute for Statement and Form
+//
+#define HII_DISPLAY_NONE             0
+#define HII_DISPLAY_GRAYOUT          BIT0
+#define HII_DISPLAY_LOCK             BIT1
+#define HII_DISPLAY_READONLY         BIT2
+#define HII_DISPLAY_MODAL            BIT3
+#define HII_DISPLAY_SUPPRESS         BIT4
+
+struct _FORM_DISPLAY_ENGINE_STATEMENT{
+  UINTN                 Signature;
+  //
+  // Version for future structure extension
+  //
+  UINTN                 Version;
+  //
+  // link to all the statement which will show in the display form.
+  //
+  LIST_ENTRY            DisplayLink;
+  //
+  // Pointer to statement opcode.
+  // for Guided Opcode. All buffers will be here if GUIDED opcode scope is set.
+  //
+  EFI_IFR_OP_HEADER     *OpCode;
+  //
+  // Question CurrentValue
+  //
+  EFI_HII_VALUE         CurrentValue;
+  //
+  // Flag to describe whether setting is changed or not.
+  // Displayer may depend on it to show it with the different color. 
+  //
+  BOOLEAN               SettingChangedFlag;
+  //
+  // nested Statement list inside of EFI_IFR_SUBTITLE
+  //
+  LIST_ENTRY            NestStatementList;
+  //
+  // nested EFI_IFR_ONE_OF_OPTION list (QUESTION_OPTION)
+  //
+  LIST_ENTRY            OptionListHead;
+  //
+  // Statement attributes: GRAYOUT, LOCK and READONLY
+  //
+  UINT32                Attribute;
+
+  //
+  // ValidateQuestion to do InconsistIf check
+  // It may be NULL if any value is valid.
+  //
+  VALIDATE_QUESTION     ValidateQuestion;
+  
+  //
+  // Password additional check. It may be NULL when the additional check is not required.
+  //
+  PASSWORD_CHECK        PasswordCheck;
+
+  //
+  // Statement ImageId and AnimationId
+  //
+  EFI_IMAGE_ID          ImageId;
+  EFI_ANIMATION_ID      AnimationId;
+};
+
+#define FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK(a)  CR (a, FORM_DISPLAY_ENGINE_STATEMENT, DisplayLink, FORM_DISPLAY_ENGINE_STATEMENT_SIGNATURE)
+
+#define BROWSER_HOT_KEY_SIGNATURE  SIGNATURE_32 ('B', 'H', 'K', 'S')
+
+typedef struct {
+  UINTN                 Signature;
+  LIST_ENTRY            Link;
+  
+  EFI_INPUT_KEY         *KeyData;
+  //
+  // Action is Discard, Default, Submit, Reset and Exit.
+  //
+  UINT32                 Action;
+  UINT16                 DefaultId;
+  //
+  // HotKey Help String
+  //
+  EFI_STRING             HelpString;
+} BROWSER_HOT_KEY;
+
+#define BROWSER_HOT_KEY_FROM_LINK(a)  CR (a, BROWSER_HOT_KEY, Link, BROWSER_HOT_KEY_SIGNATURE)
+
+#define FORM_DISPLAY_ENGINE_FORM_SIGNATURE  SIGNATURE_32 ('F', 'F', 'R', 'M')
+
+struct _FORM_DISPLAY_ENGINE_FORM {
+  UINTN                Signature;
+  //
+  // Version for future structure extension
+  //
+  UINTN                Version;
+  //
+  // Statement List inside of Form
+  //
+  LIST_ENTRY            StatementListHead;
+  //
+  // Statement List outside of Form  
+  //
+  LIST_ENTRY            StatementListOSF;
+  //
+  // The input screen dimenstions info.
+  //
+  EFI_SCREEN_DESCRIPTOR *ScreenDimensions;
+  //
+  // FormSet information
+  //
+  EFI_GUID             FormSetGuid;
+  //
+  // HiiHandle can be used to get String, Image or Animation
+  //
+  EFI_HII_HANDLE       HiiHandle;
+  
+  //
+  // Form ID and Title.
+  //
+  UINT16               FormId;
+  EFI_STRING_ID        FormTitle;
+  //
+  // Form Attributes: Lock, Modal.
+  //
+  UINT32               Attribute;
+  //
+  // Flag to describe whether setting is changed or not.
+  // Displayer depends on it to show ChangedFlag.
+  //
+  BOOLEAN              SettingChangedFlag;
+
+  //
+  // Statement to be HighLighted
+  //
+  FORM_DISPLAY_ENGINE_STATEMENT *HighLightedStatement;
+  //
+  // Event to notify Displayer that FormData is updated to be refreshed.
+  //
+  EFI_EVENT              FormRefreshEvent;
+  //
+  // Additional Hotkey registered by BrowserEx protocol.
+  //
+  LIST_ENTRY             HotKeyListHead;
+
+  //
+  // Form ImageId and AnimationId
+  //
+  EFI_IMAGE_ID         ImageId;
+  EFI_ANIMATION_ID     AnimationId;
+  
+  //
+  // If Status is error, display needs to handle it.  
+  //
+  UINT32               BrowserStatus;
+  //
+  // String for error status. It may be NULL. 
+  //
+  EFI_STRING           ErrorString;
+};
+
+#define FORM_DISPLAY_ENGINE_FORM_FROM_LINK(a)  CR (a, FORM_DISPLAY_ENGINE_FORM, Link, FORM_DISPLAY_ENGINE_FORM_SIGNATURE)
+
+typedef struct {
+  FORM_DISPLAY_ENGINE_STATEMENT  *SelectedStatement; // Selected Statement and InputValue
+  
+  EFI_HII_VALUE                  InputValue;
+  
+  UINT32                         Action;             // If SelectedStatement is NULL, Action will be used.
+                                                     // Trig Action (Discard, Default, Submit, Reset and Exit)
+  UINT16                         DefaultId;
+} USER_INPUT;
+
+/**
+  Display one form, and return user input.
+  
+  @param FormData                Form Data to be shown.
+  @param UserInputData           User input data.
+  
+  @retval EFI_SUCCESS            Form Data is shown, and user input is got.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *FORM_DISPLAY) (
+  IN FORM_DISPLAY_ENGINE_FORM  *FormData,
+  OUT USER_INPUT               *UserInputData
+);
+
+/**
+  Exit Display and Clear Screen to the original state.
+
+**/
+typedef
+VOID
+(EFIAPI *EXIT_DISPLAY) (
+  VOID
+);
+
+/**
+  Confirm how to handle the changed data. 
+  
+  @return Action of Submit, Discard and None
+**/
+typedef
+UINTN
+(EFIAPI *CONFIRM_DATA_CHANGE) (
+  VOID
+);
+
+typedef struct {
+  FORM_DISPLAY        FormDisplay;
+  EXIT_DISPLAY        ExitDisplay;
+  CONFIRM_DATA_CHANGE ConfirmDataChange;
+} EDKII_FORM_DISPLAY_ENGINE_PROTOCOL;
+
+extern EFI_GUID gEdkiiFormDisplayEngineProtocolGuid;
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/Dpc.h b/Core/MdeModulePkg/Include/Protocol/Dpc.h
new file mode 100644
index 0000000000..99293e2987
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/Dpc.h
@@ -0,0 +1,104 @@
+/** @file
+
+  EFI Deferred Procedure Call Protocol.
+
+Copyright (c) 2007 - 2010, 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 that 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.
+
+**/
+
+
+#ifndef __DPC_H__
+#define __DPC_H__
+
+//
+// DPC Protocol GUID value
+//
+#define EFI_DPC_PROTOCOL_GUID \
+    { \
+      0x480f8ae9, 0xc46, 0x4aa9, { 0xbc, 0x89, 0xdb, 0x9f, 0xba, 0x61, 0x98, 0x6 } \
+    }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EFI_DPC_PROTOCOL  EFI_DPC_PROTOCOL;
+
+
+/**
+  Invoke a Deferred Procedure Call.
+
+  @param  DpcContext           The pointer to the Deferred Procedure Call's context,
+                               which is implementation dependent.
+
+**/
+typedef
+VOID
+(EFIAPI *EFI_DPC_PROCEDURE)(
+  IN VOID  *DpcContext
+  );
+
+/**
+  Add a Deferred Procedure Call to the end of the DPC queue.
+
+  @param  This          The protocol instance pointer.
+  @param  DpcTpl        The EFI_TPL that the DPC should invoke.
+  @param  DpcProcedure  The pointer to the DPC's function.
+  @param  DpcContext    The pointer to the DPC's context.  Passed to DpcProcedure
+                        when DpcProcedure is invoked.
+
+  @retval EFI_SUCCESS            The DPC was queued.
+  @retval EFI_INVALID_PARAMETER  DpcTpl is not a valid EFI_TPL.
+  @retval EFI_INVALID_PARAMETER  DpcProcedure is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to
+                                 add the DPC to the queue.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_DPC_QUEUE_DPC)(
+  IN EFI_DPC_PROTOCOL   *This,
+  IN EFI_TPL            DpcTpl,
+  IN EFI_DPC_PROCEDURE  DpcProcedure,
+  IN VOID               *DpcContext    OPTIONAL
+  );
+
+/**
+  Dispatch the queue of DPCs.  
+  
+  DPCs with DpcTpl value greater than the current TPL value are queued, and then DPCs
+  with DpcTpl value lower than the current TPL value are queued. All DPCs in the first 
+  group (higher DpcTpl values) are invoked before DPCs in the second group (lower DpcTpl values). 
+
+  @param  This  Protocol instance pointer.
+
+  @retval EFI_SUCCESS    One or more DPCs were invoked.
+  @retval EFI_NOT_FOUND  No DPCs were invoked.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_DPC_DISPATCH_DPC)(
+  IN EFI_DPC_PROTOCOL  *This
+  );
+
+///
+/// DPC Protocol structure.
+///
+struct _EFI_DPC_PROTOCOL {
+  EFI_DPC_QUEUE_DPC     QueueDpc;
+  EFI_DPC_DISPATCH_DPC  DispatchDpc;
+};
+
+///
+/// DPC Protocol GUID variable.
+///
+extern EFI_GUID gEfiDpcProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/EbcSimpleDebugger.h b/Core/MdeModulePkg/Include/Protocol/EbcSimpleDebugger.h
new file mode 100644
index 0000000000..43b55b48eb
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/EbcSimpleDebugger.h
@@ -0,0 +1,124 @@
+/** @file
+  EBC Simple Debugger protocol for debug EBC code.
+
+Copyright (c) 2011, 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.
+
+**/
+
+#ifndef _EBC_SIMPLE_DEBUGGER_PROTOCOL_H_
+#define _EBC_SIMPLE_DEBUGGER_PROTOCOL_H_
+
+#include <Protocol/DebugSupport.h>
+#include <Protocol/EbcVmTest.h>
+
+#define EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL_GUID \
+  { \
+    0x2a72d11e, 0x7376, 0x40f6, { 0x9c, 0x68, 0x23, 0xfa, 0x2f, 0xe3, 0x63, 0xf1 } \
+  }
+
+//
+// Defines for a simple EBC debugger interface
+//
+typedef struct _EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL;
+
+/**
+  Trig Exception on EBC VM.
+
+  @param[in] This           A pointer to the EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL structure.
+  @param[in] VmPtr          A pointer to a VM context.
+  @param[in] ExceptionType  Exception to be trigged.
+
+  @retval EFI_UNSUPPORTED       No support for it.
+  @retval EFI_SUCCESS           Exception is trigged.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EBC_DEBUGGER_SIGNAL_EXCEPTION) (
+  IN EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL           *This,
+  IN VM_CONTEXT                                 *VmPtr,
+  IN EFI_EXCEPTION_TYPE                         ExceptionType
+  );
+
+/**
+  Given a pointer to a new VM context, debug one or more instructions.
+
+  @param[in] This           A pointer to the EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL structure.
+  @param[in] VmPtr          A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED       No support for it.
+  @retval EFI_SUCCESS           Debug one or more instructions.
+
+**/
+typedef
+VOID
+(EFIAPI *EBC_DEBUGGER_DEBUG) (
+  IN EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL           *This,
+  IN VM_CONTEXT                                 *VmPtr
+  );
+
+/**
+  Given a pointer to a new VM context, dump one or more instructions.
+
+  @param[in] This           A pointer to the EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL structure.
+  @param[in] VmPtr          A pointer to a VM context.
+  @param[in] DasmString     Dump string buffer. 
+  @param[in] DasmStringSize Dump string size.
+
+  @retval EFI_UNSUPPORTED       No support for it.
+  @retval EFI_SUCCESS           Dump one or more instructions.
+
+**/
+typedef
+UINT32
+(EFIAPI *EBC_DEBUGGER_DASM) (
+  IN EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL           *This,
+  IN VM_CONTEXT                                 *VmPtr,
+  IN UINT16                                     *DasmString OPTIONAL,
+  IN UINT32                                     DasmStringSize
+  );
+
+/**
+  This interface allows you to configure the EBC debug support
+  driver. For example, turn on or off saving and printing of
+  delta VM even if called. Or to even disable the entire interface,
+  in which case all functions become no-ops.
+
+  @param[in] This           A pointer to the EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL structure.
+  @param[in] ConfigId       ID to be configured.
+  @param[in] ConfigValue    Value to be set.
+
+  @retval EFI_UNSUPPORTED       No support for it.
+  @retval EFI_SUCCESS           Configure EBC debug.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EBC_DEBUGGER_CONFIGURE) (
+  IN EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL           *This,
+  IN UINT32                                     ConfigId,
+  IN UINTN                                      ConfigValue
+  );
+
+//
+// Prototype for the actual EBC debug support protocol interface
+//
+struct _EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL {
+  EBC_DEBUGGER_DEBUG            Debugger;
+  EBC_DEBUGGER_SIGNAL_EXCEPTION SignalException;
+  EBC_DEBUGGER_DASM             Dasm;
+  EBC_DEBUGGER_CONFIGURE        Configure;
+};
+
+extern EFI_GUID gEfiEbcSimpleDebuggerProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/EbcVmTest.h b/Core/MdeModulePkg/Include/Protocol/EbcVmTest.h
new file mode 100644
index 0000000000..b84608357b
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/EbcVmTest.h
@@ -0,0 +1,142 @@
+/** @file
+  EBC VM Test protocol for test purposes.
+
+Copyright (c) 2011, 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.
+
+**/
+
+#ifndef _EBC_VM_TEST_PROTOCOL_H_
+#define _EBC_VM_TEST_PROTOCOL_H_
+
+//
+// Define a protocol for an EBC VM test interface.
+//
+#define EFI_EBC_VM_TEST_PROTOCOL_GUID \
+  { \
+    0xAAEACCFD, 0xF27B, 0x4C17, { 0xB6, 0x10, 0x75, 0xCA, 0x1F, 0x2D, 0xFB, 0x52 } \
+  }
+
+//
+// Define for forward reference.
+//
+typedef struct _EFI_EBC_VM_TEST_PROTOCOL EFI_EBC_VM_TEST_PROTOCOL;
+
+///
+/// instruction pointer for the VM
+///
+typedef UINT8   *VMIP;
+
+typedef INT64   VM_REGISTER;
+typedef UINT32  EXCEPTION_FLAGS;
+
+typedef struct {
+  VM_REGISTER       Gpr[8];                 ///< General purpose registers.
+                                            ///< Flags register:
+                                            ///<   0  Set to 1 if the result of the last compare was true
+                                            ///<   1  Set to 1 if stepping
+  UINT64            Flags;                  ///<   2..63 Reserved.
+  VMIP              Ip;                     ///< Instruction pointer.
+  UINTN             LastException;
+  EXCEPTION_FLAGS   ExceptionFlags;         ///< to keep track of exceptions
+  UINT32            StopFlags;
+  UINT32            CompilerVersion;        ///< via break(6)
+  UINTN             HighStackBottom;        ///< bottom of the upper stack
+  UINTN             LowStackTop;            ///< top of the lower stack
+  UINT64            StackRetAddr;           ///< location of final return address on stack
+  UINTN             *StackMagicPtr;         ///< pointer to magic value on stack to detect corruption
+  EFI_HANDLE        ImageHandle;            ///< for this EBC driver
+  EFI_SYSTEM_TABLE  *SystemTable;           ///< for debugging only
+  UINTN             LastAddrConverted;      ///< for debug
+  UINTN             LastAddrConvertedValue; ///< for debug
+  VOID              *FramePtr;
+  VOID              *EntryPoint;            ///< entry point of EBC image
+  UINTN             ImageBase;
+  VOID              *StackPool;
+  VOID              *StackTop;
+} VM_CONTEXT;
+
+/**
+  Given a pointer to a new VM context, execute one or more instructions. This
+  function is only used for test purposes.
+
+  @param[in]      This              A pointer to the EFI_EBC_VM_TEST_PROTOCOL structure.
+  @param[in]      VmPtr             A pointer to a VM context.
+  @param[in, out] InstructionCount  A pointer to a UINTN value holding the number of
+                                    instructions to execute. If it holds value of 0,
+                                    then the instruction to be executed is 1.
+
+  @retval EFI_UNSUPPORTED       At least one of the opcodes is not supported.
+  @retval EFI_SUCCESS           All of the instructions are executed successfully.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EBC_VM_TEST_EXECUTE) (
+  IN EFI_EBC_VM_TEST_PROTOCOL         *This,
+  IN VM_CONTEXT                       *VmPtr,
+  IN OUT UINTN                        *InstructionCount
+  );
+
+/**
+  Convert AsmText to the instruction. This function is only used for test purposes.
+
+  @param[in]  This              A pointer to the EFI_EBC_VM_TEST_PROTOCOL structure.
+  @param[in]  AsmText           A pointer to EBC ASM text code.
+  @param[out] Buffer            Buffer to store the instruction.
+  @param[out] BufferLen         Size of buffer that is requried to store data.
+
+  @retval EFI_UNSUPPORTED       This functionality is unsupported.
+  @retval EFI_SUCCESS           Successfully convert AsmText to the instruction. 
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EBC_VM_TEST_ASM) (
+  IN EFI_EBC_VM_TEST_PROTOCOL         *This,
+  IN CHAR16                           *AsmText,
+  IN OUT INT8                         *Buffer,
+  IN OUT UINTN                        *BufferLen
+  );
+
+/**
+  Dump the executed instruction. This function is only used for test purposes.
+
+  @param[in]  This              A pointer to the EFI_EBC_VM_TEST_PROTOCOL structure.
+  @param[out] AsmText           Contain the disasm text.
+  @param[out] Buffer            Buffer to store the instruction.
+  @param[out] BufferLen         Size of buffer that is requried to store data.
+
+  @retval EFI_UNSUPPORTED       This functionality is unsupported.
+  @retval EFI_SUCCESS           Successfully dump the executed instruction.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EBC_VM_TEST_DASM) (
+  IN EFI_EBC_VM_TEST_PROTOCOL         *This,
+  IN OUT CHAR16                       *AsmText,
+  IN OUT INT8                         *Buffer,
+  IN OUT UINTN                        *Len
+  );
+
+//
+// Prototype for the actual EBC test protocol interface
+//
+struct _EFI_EBC_VM_TEST_PROTOCOL {
+  EBC_VM_TEST_EXECUTE Execute;
+  EBC_VM_TEST_ASM     Assemble;
+  EBC_VM_TEST_DASM    Disassemble;
+};
+
+extern EFI_GUID gEfiEbcVmTestProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/EsrtManagement.h b/Core/MdeModulePkg/Include/Protocol/EsrtManagement.h
new file mode 100644
index 0000000000..305fd5c908
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/EsrtManagement.h
@@ -0,0 +1,144 @@
+/** @file
+  The Esrt Management Protocol used to register/set/update an updatable firmware resource .
+
+Copyright (c) 2015, 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 that 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.
+
+**/
+
+#ifndef _ESRT_MANAGEMENT_H_
+#define _ESRT_MANAGEMENT_H_
+
+#include <Guid/SystemResourceTable.h>
+
+///
+/// Global ID for the ESRT_MANAGEMENT_PROTOCOL.
+///
+#define ESRT_MANAGEMENT_PROTOCOL_GUID \
+  { \
+    0xa340c064, 0x723c, 0x4a9c, { 0xa4, 0xdd, 0xd5, 0xb4, 0x7a, 0x26, 0xfb, 0xb0 } \
+  }
+
+///
+/// Forward declaration for the _ESRT_MANAGEMENT_PROTOCOL.
+///
+typedef struct _ESRT_MANAGEMENT_PROTOCOL  ESRT_MANAGEMENT_PROTOCOL;
+
+/**
+  Get Variable name and data by Esrt Entry FwClass 
+
+  @param[in]       FwClass                FwClass of Esrt entry to get  
+  @param[in out]  Entry                   Esrt entry returned 
+  
+  @retval EFI_SUCCESS                  The variable saving this Esrt Entry exists.
+  @retval EF_NOT_FOUND                   No correct variable found.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *GET_ESRT_ENTRY)(
+  IN     EFI_GUID                  *FwClass,
+  IN OUT EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+ 
+
+/**
+  Update one ESRT entry in ESRT Cache.
+
+  @param[in]  Entry                         Esrt entry to be updated
+  
+  @retval EFI_SUCCESS                   Successfully update an ESRT entry in cache.
+  @retval EFI_INVALID_PARAMETER  Entry does't exist in ESRT Cache
+  @retval EFI_WRITE_PROTECTED     ESRT Cache repositoy is locked
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *UPDATE_ESRT_ENTRY)(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+
+/**
+  Non-FMP instance to unregister Esrt Entry from ESRT Cache. 
+
+  @param[in]    FwClass                FwClass of Esrt entry to Unregister  
+  
+  @retval EFI_SUCCESS         Insert all entries Successfully 
+  @retval EFI_NOT_FOUND     FwClass does not exsit
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *UNREGISTER_ESRT_ENTRY)(
+  IN  EFI_GUID        *FwClass
+  );
+
+
+/**
+  Non-FMP instance to register one ESRT entry into ESRT Cache.
+
+  @param[in]  Entry                Esrt entry to be set
+
+  @retval EFI_SUCCESS                   Successfully set a variable.
+  @retval EFI_INVALID_PARAMETER  ESRT Entry is already exist
+  @retval EFI_OUT_OF_RESOURCES  Non-FMP ESRT repository is full
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *REGISTER_ESRT_ENTRY)(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+
+/**
+  This function syn up Cached ESRT with data from FMP instances
+  Function should be called after Connect All in order to locate all FMP protocols
+  installed
+
+  @retval EFI_SUCCESS                      Successfully sync cache repository from FMP instances
+  @retval EFI_NOT_FOUND                   No FMP Instance are found
+  @retval EFI_OUT_OF_RESOURCES     Resource allocaton fail
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *SYNC_ESRT_FMP)(
+  VOID
+  );
+
+
+/**
+  This function locks up Esrt repository to be readonly. It should be called 
+  before gEfiEndOfDxeEventGroupGuid event signaled
+
+  @retval EFI_SUCCESS              Locks up FMP Non-FMP repository successfully 
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *LOCK_ESRT_REPOSITORY)(
+  VOID
+  );
+
+
+struct _ESRT_MANAGEMENT_PROTOCOL {
+  GET_ESRT_ENTRY        GetEsrtEntry;
+  UPDATE_ESRT_ENTRY     UpdateEsrtEntry;
+  REGISTER_ESRT_ENTRY   RegisterEsrtEntry;
+  UNREGISTER_ESRT_ENTRY UnRegisterEsrtEntry;
+  SYNC_ESRT_FMP         SyncEsrtFmp;
+  LOCK_ESRT_REPOSITORY  LockEsrtRepository;
+};
+
+extern EFI_GUID gEsrtManagementProtocolGuid;
+
+#endif // #ifndef _ESRT_MANAGEMENT_H_
+
diff --git a/Core/MdeModulePkg/Include/Protocol/FaultTolerantWrite.h b/Core/MdeModulePkg/Include/Protocol/FaultTolerantWrite.h
new file mode 100644
index 0000000000..b96edfb68f
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/FaultTolerantWrite.h
@@ -0,0 +1,207 @@
+/** @file
+  Fault Tolerant Write protocol provides boot-time service for fault tolerant 
+  write capability for block devices.  The protocol provides for non-volatile 
+  storage of the intermediate data and private information a caller would need to 
+  recover from a critical fault, such as a power failure.   
+
+Copyright (c) 2009 - 2010, 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 that 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. 
+
+**/
+
+#ifndef _FW_FAULT_TOLERANT_WRITE_PROTOCOL_H_
+#define _FW_FAULT_TOLERANT_WRITE_PROTOCOL_H_
+
+#define EFI_FAULT_TOLERANT_WRITE_PROTOCOL_GUID \
+  { \
+    0x3ebd9e82, 0x2c78, 0x4de6, {0x97, 0x86, 0x8d, 0x4b, 0xfc, 0xb7, 0xc8, 0x81 } \
+  }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EFI_FAULT_TOLERANT_WRITE_PROTOCOL  EFI_FAULT_TOLERANT_WRITE_PROTOCOL;
+
+/**
+  Get the size of the largest block that can be updated in a fault-tolerant manner.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  BlockSize            A pointer to a caller-allocated UINTN that is
+                               updated to indicate the size of the largest block
+                               that can be updated.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_GET_MAX_BLOCK_SIZE)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    * This,
+  OUT UINTN                               *BlockSize
+  );
+
+/**
+  Allocates space for the protocol to maintain information about writes.
+  Since writes must be completed in a fault-tolerant manner and multiple
+  writes require more resources to be successful, this function
+  enables the protocol to ensure that enough space exists to track
+  information about upcoming writes.
+
+  @param  This                 A pointer to the calling context.
+  @param  CallerId             The GUID identifying the write.
+  @param  PrivateDataSize      The size of the caller's private data  that must be
+                               recorded for each write.
+  @param  NumberOfWrites       The number of fault tolerant block writes that will
+                               need to occur.
+
+  @retval EFI_SUCCESS          The function completed successfully
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    Not all allocated writes have been completed.  All
+                               writes must be completed or aborted before another
+                               fault tolerant write can occur.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_ALLOCATE)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    * This,
+  IN EFI_GUID                             * CallerId,
+  IN UINTN                                PrivateDataSize,
+  IN UINTN                                NumberOfWrites
+  );
+
+/**
+  Starts a target block update. This records information about the write
+  in fault tolerant storage, and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param  This                 The calling context.
+  @param  Lba                  The logical block address of the target block.
+  @param  Offset               The offset within the target block to place the
+                               data.
+  @param  Length               The number of bytes to write to the target block.
+  @param  PrivateData          A pointer to private data that the caller requires
+                               to complete any pending writes in the event of a
+                               fault.
+  @param  FvBlockHandle        The handle of FVB protocol that provides services
+                               for reading, writing, and erasing the target block.
+  @param  Buffer               The data to write.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_BAD_BUFFER_SIZE  The write would span a block boundary, which is not
+                               a valid action.
+  @retval EFI_ACCESS_DENIED    No writes have been allocated.
+  @retval EFI_NOT_READY        The last write has not been completed. Restart()
+                               must be called to complete it.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_WRITE)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     * This,
+  IN EFI_LBA                               Lba,
+  IN UINTN                                 Offset,
+  IN UINTN                                 Length,
+  IN VOID                                  *PrivateData,
+  IN EFI_HANDLE                            FvbHandle,
+  IN VOID                                  *Buffer
+  );
+
+/**
+  Restarts a previously interrupted write. The caller must provide the
+  block protocol needed to complete the interrupted write.
+
+  @param  This                 The calling context.
+  @param  FvBlockProtocol      The handle of FVB protocol that provides services.
+                               for reading, writing, and erasing the target block.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    No pending writes exist.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_RESTART)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     * This,
+  IN EFI_HANDLE                            FvbHandle
+  );
+
+/**
+  Aborts all previously allocated writes.
+
+  @param  This                 The calling context.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_ABORT)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     * This
+  );
+
+/**
+  Starts a target block update. This function records information about the write
+  in fault-tolerant storage and completes the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param  This                 Indicates a pointer to the calling context.
+  @param  CallerId             The GUID identifying the last write.
+  @param  Lba                  The logical block address of the last write.
+  @param  Offset               The offset within the block of the last write.
+  @param  Length               The length of the last write.
+  @param  PrivateDataSize      On input, the size of the PrivateData buffer. On
+                               output, the size of the private data stored for
+                               this write.
+  @param  PrivateData          A pointer to a buffer. The function will copy
+                               PrivateDataSize bytes from the private data stored
+                               for this write.
+  @param  Complete             A Boolean value with TRUE indicating that the write
+                               was completed.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_FAULT_TOLERANT_WRITE_GET_LAST_WRITE)(
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     * This,
+  OUT EFI_GUID                             * CallerId,
+  OUT EFI_LBA                              *Lba,
+  OUT UINTN                                *Offset,
+  OUT UINTN                                *Length,
+  IN OUT UINTN                             *PrivateDataSize,
+  OUT VOID                                 *PrivateData,
+  OUT BOOLEAN                              *Complete
+  );
+
+//
+// Protocol declaration
+//
+struct _EFI_FAULT_TOLERANT_WRITE_PROTOCOL {
+  EFI_FAULT_TOLERANT_WRITE_GET_MAX_BLOCK_SIZE GetMaxBlockSize;
+  EFI_FAULT_TOLERANT_WRITE_ALLOCATE           Allocate;
+  EFI_FAULT_TOLERANT_WRITE_WRITE              Write;
+  EFI_FAULT_TOLERANT_WRITE_RESTART            Restart;
+  EFI_FAULT_TOLERANT_WRITE_ABORT              Abort;
+  EFI_FAULT_TOLERANT_WRITE_GET_LAST_WRITE     GetLastWrite;
+};
+
+extern EFI_GUID gEfiFaultTolerantWriteProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/FormBrowserEx.h b/Core/MdeModulePkg/Include/Protocol/FormBrowserEx.h
new file mode 100644
index 0000000000..ef3e8cbceb
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/FormBrowserEx.h
@@ -0,0 +1,150 @@
+/** @file
+  Extension Form Browser Protocol provides the services that can be used to 
+  register the different hot keys for the standard Browser actions described in UEFI specification.
+
+Copyright (c) 2011 - 2014, 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 that 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.
+
+**/
+
+#ifndef __FORM_BROWSER_EXTENSION_H__
+#define __FORM_BROWSER_EXTENSION_H__
+
+#define FORM_BROWSER_EXTENSION_PROTOCOL_GUID  \
+  { 0x1f73b18d, 0x4630, 0x43c1, { 0xa1, 0xde, 0x6f, 0x80, 0x85, 0x5d, 0x7d, 0xa4 } }
+
+typedef struct _EFI_FORM_BROWSER_EXTENSION_PROTOCOL   EFI_FORM_BROWSER_EXTENSION_PROTOCOL;
+
+//
+// Return value of SAVE_REMINDER() that describes whether the changed data is saved or discarded.
+//
+#define BROWSER_NO_CHANGES          0
+#define BROWSER_SAVE_CHANGES        1
+#define BROWSER_DISCARD_CHANGES     2
+#define BROWSER_KEEP_CURRENT        3
+
+//
+// Browser actions. They can be cominbed together. 
+// If more than one actions are specified, the action with low bit will be executed first. 
+//
+#define BROWSER_ACTION_UNREGISTER   0
+#define BROWSER_ACTION_DISCARD      BIT0
+#define BROWSER_ACTION_DEFAULT      BIT1
+#define BROWSER_ACTION_SUBMIT       BIT2
+#define BROWSER_ACTION_RESET        BIT3
+#define BROWSER_ACTION_EXIT         BIT4
+#define BROWSER_ACTION_GOTO         BIT5
+
+//
+// Scope for Browser action. It may be Form, FormSet or System level.
+//
+typedef enum {
+  FormLevel,
+  FormSetLevel,
+  SystemLevel,
+  MaxLevel
+} BROWSER_SETTING_SCOPE;
+
+/**
+  Configure what scope the hot key will impact.
+  All hot keys have the same scope. The mixed hot keys with the different level are not supported.
+  If no scope is set, the default scope will be FormSet level.
+  After all registered hot keys are removed, previous Scope can reset to another level.
+  
+  @param[in] Scope               Scope level to be set. 
+  
+  @retval EFI_SUCCESS            Scope is set correctly.
+  @retval EFI_INVALID_PARAMETER  Scope is not the valid value specified in BROWSER_SETTING_SCOPE. 
+  @retval EFI_UNSPPORTED         Scope level is different from current one that the registered hot keys have.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *SET_SCOPE) (
+  IN BROWSER_SETTING_SCOPE Scope
+  );
+
+/**
+  Register the hot key with its browser action, or unregistered the hot key.
+  If the action value is zero, the hot key will be unregistered if it has been registered.
+  If the same hot key has been registered, the new action and help string will override the previous ones.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key. Its type is EFI_INPUT_KEY to 
+                         be supported by all ConsoleIn devices.
+  @param[in] Action      Action value that describes what action will be trigged when the hot key is pressed. 
+  @param[in] DefaultId   Specifies the type of defaults to retrieve, which is only for DEFAULT action.
+  @param[in] HelpString  Help string that describes the hot key information.
+                         Its value may be NULL for the unregistered hot key.
+  
+  @retval EFI_SUCCESS            Hot key is registered or unregistered.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *REGISTER_HOT_KEY) (
+  IN EFI_INPUT_KEY *KeyData,
+  IN UINT32        Action,
+  IN UINT16        DefaultId,
+  IN EFI_STRING    HelpString OPTIONAL
+  );
+
+/**
+  This handler is responsbile for the left things on normal boot after all UI forms are closed.
+  For example, it can continue to boot the first boot option. 
+
+  It will be used only when EXIT action is trigged as system level.
+**/
+typedef
+VOID
+(EFIAPI *EXIT_HANDLER) (
+  VOID
+  );
+
+/**
+  Register Exit handler function. 
+  When more than one handler function is registered, the latter one will override the previous one. 
+  When NULL handler is specified, the previous Exit handler will be unregistered. 
+  
+  @param[in] Handler      Pointer to handler function. 
+
+**/
+typedef
+VOID
+(EFIAPI *REGISTER_EXIT_HANDLER) (
+  IN EXIT_HANDLER Handler
+  );
+
+/**
+  Create reminder to let user to choose save or discard the changed browser data.
+  Caller can use it to actively check the changed browser data.
+
+  @retval BROWSER_NO_CHANGES       No browser data is changed.
+  @retval BROWSER_SAVE_CHANGES     The changed browser data is saved.
+  @retval BROWSER_DISCARD_CHANGES  The changed browser data is discard.
+  @retval BROWSER_KEEP_CURRENT     Browser keep current changes.
+
+**/
+typedef
+UINT32
+(EFIAPI *SAVE_REMINDER)(
+  VOID
+  );
+
+struct _EFI_FORM_BROWSER_EXTENSION_PROTOCOL {
+  SET_SCOPE              SetScope;
+  REGISTER_HOT_KEY       RegisterHotKey;
+  REGISTER_EXIT_HANDLER  RegiserExitHandler;
+  SAVE_REMINDER          SaveReminder;
+};
+
+extern EFI_GUID gEfiFormBrowserExProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/FormBrowserEx2.h b/Core/MdeModulePkg/Include/Protocol/FormBrowserEx2.h
new file mode 100644
index 0000000000..1ccc5bd964
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/FormBrowserEx2.h
@@ -0,0 +1,125 @@
+/** @file
+  Extension Form Browser Protocol provides the services that can be used to 
+  register the different hot keys for the standard Browser actions described in UEFI specification.
+
+Copyright (c) 2013 - 2014, 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 that 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.
+
+**/
+
+#ifndef __FORM_BROWSER_EXTENSION2_H__
+#define __FORM_BROWSER_EXTENSION2_H__
+
+#include <Protocol/FormBrowserEx.h>
+
+#define EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL_GUID  \
+  { 0xa770c357, 0xb693, 0x4e6d, { 0xa6, 0xcf, 0xd2, 0x1c, 0x72, 0x8e, 0x55, 0xb }}
+
+typedef struct _EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL   EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL;
+
+#define BROWSER_EXTENSION2_VERSION_1    0x10000
+#define BROWSER_EXTENSION2_VERSION_1_1  0x10001
+
+/**
+  Check whether the browser data has been modified.
+
+  @retval TRUE        Browser data is modified.
+  @retval FALSE       No browser data is modified.
+
+**/
+typedef
+BOOLEAN
+(EFIAPI *IS_BROWSER_DATA_MODIFIED) (
+  VOID
+  );
+
+/**
+  Execute the action requested by the Action parameter.
+
+  @param[in] Action     Execute the request action.
+  @param[in] DefaultId  The default Id info when need to load default value.
+
+  @retval EFI_SUCCESS              Execute the request action succss.
+
+**/
+typedef 
+EFI_STATUS 
+(EFIAPI *EXECUTE_ACTION) (
+  IN UINT32        Action,
+  IN UINT16        DefaultId
+  );
+
+/**
+  Check whether required reset when exit the browser
+
+  @retval TRUE      Browser required to reset after exit.
+  @retval FALSE     Browser not need to reset after exit.
+
+**/
+typedef
+BOOLEAN
+(EFIAPI *IS_RESET_REQUIRED) (
+  VOID
+  );
+
+#define FORM_ENTRY_INFO_SIGNATURE    SIGNATURE_32 ('f', 'e', 'i', 's')
+
+typedef struct {
+  UINTN           Signature;
+  LIST_ENTRY      Link;
+
+  EFI_HII_HANDLE  HiiHandle;
+  EFI_GUID        FormSetGuid;
+  EFI_FORM_ID     FormId;
+  EFI_QUESTION_ID QuestionId;
+} FORM_ENTRY_INFO;
+
+#define FORM_ENTRY_INFO_FROM_LINK(a)  CR (a, FORM_ENTRY_INFO, Link, FORM_ENTRY_INFO_SIGNATURE)
+
+#define FORM_QUESTION_ATTRIBUTE_OVERRIDE_SIGNATURE    SIGNATURE_32 ('f', 'q', 'o', 's')
+
+typedef struct {
+  UINTN            Signature;
+  LIST_ENTRY       Link;
+
+  EFI_QUESTION_ID  QuestionId;           // Find the question
+  EFI_FORM_ID      FormId;               // Find the form
+  EFI_GUID         FormSetGuid;          // Find the formset.
+  EFI_HII_HANDLE   HiiHandle;            // Find the HII handle
+  UINT32           Attribute;            // Hide or grayout ... 
+} QUESTION_ATTRIBUTE_OVERRIDE;
+
+#define FORM_QUESTION_ATTRIBUTE_OVERRIDE_FROM_LINK(a)  CR (a, QUESTION_ATTRIBUTE_OVERRIDE, Link, FORM_QUESTION_ATTRIBUTE_OVERRIDE_SIGNATURE)
+
+struct _EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL {
+  ///
+  /// Version for protocol future extension.
+  ///
+  UINT32                    Version;
+  SET_SCOPE                 SetScope;
+  REGISTER_HOT_KEY          RegisterHotKey;
+  REGISTER_EXIT_HANDLER     RegiserExitHandler;
+  IS_BROWSER_DATA_MODIFIED  IsBrowserDataModified;
+  EXECUTE_ACTION            ExecuteAction;
+  ///
+  /// A list of type FORMID_INFO is Browser View Form History List.
+  ///
+  LIST_ENTRY                FormViewHistoryHead;
+  ///
+  /// A list of type QUESTION_ATTRIBUTE_OVERRIDE.
+  ///
+  LIST_ENTRY                OverrideQestListHead;
+
+  IS_RESET_REQUIRED         IsResetRequired;
+};
+
+extern EFI_GUID gEdkiiFormBrowserEx2ProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/GenericMemoryTest.h b/Core/MdeModulePkg/Include/Protocol/GenericMemoryTest.h
new file mode 100644
index 0000000000..163e76d8f4
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/GenericMemoryTest.h
@@ -0,0 +1,126 @@
+/** @file
+  This protocol defines the generic memory test interfaces in Dxe phase.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __GENERIC_MEMORY_TEST_H__
+#define __GENERIC_MEMORY_TEST_H__
+
+#define EFI_GENERIC_MEMORY_TEST_PROTOCOL_GUID  \
+  { 0x309de7f1, 0x7f5e, 0x4ace, {0xb4, 0x9c, 0x53, 0x1b, 0xe5, 0xaa, 0x95, 0xef} }
+
+typedef struct _EFI_GENERIC_MEMORY_TEST_PROTOCOL  EFI_GENERIC_MEMORY_TEST_PROTOCOL;
+
+///
+/// Memory test coverage level.
+/// Ignore chooses not to test memory. Quick and Sparse test some memory, and Extensive performs a detailed memory test.
+///
+typedef enum {
+  IGNORE,
+  QUICK,
+  SPARSE,
+  EXTENSIVE,
+  MAXLEVEL
+} EXTENDMEM_COVERAGE_LEVEL;
+
+
+/**
+  Initialize the generic memory test.
+
+  @param  This                The protocol instance pointer. 
+  @param  Level               The coverage level of the memory test. 
+  @param  RequireSoftECCInit  Indicate if the memory need software ECC init. 
+
+  @retval EFI_SUCCESS         The generic memory test is initialized correctly. 
+  @retval EFI_NO_MEDIA        The system had no memory to be tested. 
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_MEMORY_TEST_INIT)(
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EXTENDMEM_COVERAGE_LEVEL                 Level,
+  OUT BOOLEAN                                  *RequireSoftECCInit
+  );
+
+
+/**
+  Perform the memory test.
+
+  @param  This                The protocol instance pointer. 
+  @param  TestedMemorySize    Return the tested extended memory size. 
+  @param  TotalMemorySize     Return the whole system physical memory size. 
+                              The total memory size does not include memory in a slot with a disabled DIMM.  
+  @param  ErrorOut            TRUE if the memory error occured.
+  @param  IfTestAbort         Indicates that the user pressed "ESC" to skip the memory test. 
+
+  @retval EFI_SUCCESS         One block of memory passed the test.
+  @retval EFI_NOT_FOUND       All memory blocks have already been tested.
+  @retval EFI_DEVICE_ERROR    Memory device error occured, and no agent can handle it.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_PERFORM_MEMORY_TEST)(
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  OUT UINT64                                   *TestedMemorySize,
+  OUT UINT64                                   *TotalMemorySize,
+  OUT BOOLEAN                                  *ErrorOut,
+  IN BOOLEAN                                   IfTestAbort
+  );
+
+
+/**
+  Finish the memory test.
+
+  @param  This                The protocol instance pointer. 
+
+  @retval EFI_SUCCESS         Success. All resources used in the memory test are freed.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_MEMORY_TEST_FINISHED)(
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL *This
+  );
+
+/**
+  Provides the capability to test the compatible range used by some special drivers.
+
+  @param  This                The protocol instance pointer. 
+  @param  StartAddress        The start address of the compatible memory range that
+                              must be below 16M.
+  @param  Length              The compatible memory range's length. 
+  
+  @retval EFI_SUCCESS           The compatible memory range pass the memory test. 
+  @retval EFI_INVALID_PARAMETER The compatible memory range are not below Low 16M.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_MEMORY_TEST_COMPATIBLE_RANGE)(
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EFI_PHYSICAL_ADDRESS                     StartAddress,
+  IN  UINT64                                   Length
+  );
+
+struct _EFI_GENERIC_MEMORY_TEST_PROTOCOL {
+  EFI_MEMORY_TEST_INIT              MemoryTestInit;
+  EFI_PERFORM_MEMORY_TEST           PerformMemoryTest;
+  EFI_MEMORY_TEST_FINISHED          Finished;
+  EFI_MEMORY_TEST_COMPATIBLE_RANGE  CompatibleRangeTest;
+};
+
+extern EFI_GUID gEfiGenericMemTestProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/IpmiProtocol.h b/Core/MdeModulePkg/Include/Protocol/IpmiProtocol.h
new file mode 100644
index 0000000000..a02ca6cb73
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/IpmiProtocol.h
@@ -0,0 +1,72 @@
+/** @file
+  Protocol of Ipmi for both SMS and SMM.
+
+  Copyright (c) 2009 - 2015, 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.
+
+**/
+
+#ifndef _IPMI_PROTOCOL_H_
+#define _IPMI_PROTOCOL_H_
+
+typedef struct _IPMI_PROTOCOL IPMI_PROTOCOL;
+
+#define IPMI_PROTOCOL_GUID \
+  { \
+    0xdbc6381f, 0x5554, 0x4d14, 0x8f, 0xfd, 0x76, 0xd7, 0x87, 0xb8, 0xac, 0xbf \
+  }
+
+#define SMM_IPMI_PROTOCOL_GUID \
+  { \
+    0x5169af60, 0x8c5a, 0x4243, 0xb3, 0xe9, 0x56, 0xc5, 0x6d, 0x18, 0xee, 0x26 \
+  }
+
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         This              This point for IPMI_PROTOCOL structure.
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *IPMI_SUBMIT_COMMAND) (
+  IN     IPMI_PROTOCOL                 *This,
+  IN     UINT8                         NetFunction,
+  IN     UINT8                         Command,
+  IN     UINT8                         *RequestData,
+  IN     UINT32                        RequestDataSize,
+     OUT UINT8                         *ResponseData,
+  IN OUT UINT32                        *ResponseDataSize
+  );
+
+//
+// IPMI COMMAND PROTOCOL
+//
+struct _IPMI_PROTOCOL{
+  IPMI_SUBMIT_COMMAND       IpmiSubmitCommand;
+};
+
+extern EFI_GUID gIpmiProtocolGuid;
+extern EFI_GUID gSmmIpmiProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/LoadPe32Image.h b/Core/MdeModulePkg/Include/Protocol/LoadPe32Image.h
new file mode 100644
index 0000000000..b56dd37ad0
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/LoadPe32Image.h
@@ -0,0 +1,103 @@
+/** @file
+
+  Load Pe32 Image protocol enables loading and unloading EFI images into memory and executing those images.
+  This protocol uses File Device Path to get an EFI image.
+
+Copyright (c) 2006 - 2011, 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 that 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.
+
+**/
+
+#ifndef __LOAD_PE32_IMAGE_H__
+#define __LOAD_PE32_IMAGE_H__
+
+#define PE32_IMAGE_PROTOCOL_GUID  \
+  {0x5cb5c776,0x60d5,0x45ee,{0x88,0x3c,0x45,0x27,0x8,0xcd,0x74,0x3f }}
+
+#define EFI_LOAD_PE_IMAGE_ATTRIBUTE_NONE                                 0x00
+#define EFI_LOAD_PE_IMAGE_ATTRIBUTE_RUNTIME_REGISTRATION                 0x01
+#define EFI_LOAD_PE_IMAGE_ATTRIBUTE_DEBUG_IMAGE_INFO_TABLE_REGISTRATION  0x02
+
+typedef struct _EFI_PE32_IMAGE_PROTOCOL   EFI_PE32_IMAGE_PROTOCOL;
+
+/**
+
+  Loads an EFI image into memory and returns a handle to the image with extended parameters.
+
+  @param  This                The pointer to the LoadPe32Image protocol instance
+  @param  ParentImageHandle   The caller's image handle.
+  @param  FilePath            The specific file path from which the image is loaded.
+  @param  SourceBuffer        If not NULL, a pointer to the memory location containing a copy of
+                              the image to be loaded.
+  @param  SourceSize          The size in bytes of SourceBuffer.
+  @param  DstBuffer           The buffer to store the image.
+  @param  NumberOfPages       For input, specifies the space size of the image by caller if not NULL.
+                              For output, specifies the actual space size needed.
+  @param  ImageHandle         The image handle for output.
+  @param  EntryPoint          The image entry point for output.
+  @param  Attribute           The bit mask of attributes to set for the load PE image.
+
+  @retval EFI_SUCCESS           The image was loaded into memory.
+  @retval EFI_NOT_FOUND         The FilePath was not found.
+  @retval EFI_INVALID_PARAMETER One of the parameters has an invalid value.
+  @retval EFI_UNSUPPORTED       The image type is not supported, or the device path cannot be
+                                parsed to locate the proper protocol for loading the file.
+  @retval EFI_OUT_OF_RESOURCES  The image was not loaded due to insufficient memory resources.
+  @retval EFI_LOAD_ERROR        Image was not loaded because the image format was corrupt or not
+                                understood.
+  @retval EFI_DEVICE_ERROR      Image was not loaded because the device returned a read error.
+  @retval EFI_ACCESS_DENIED     Image was not loaded because the platform policy prohibits the 
+                                image from being loaded. NULL is returned in *ImageHandle.
+  @retval EFI_SECURITY_VIOLATION Image was loaded and an ImageHandle was created with a 
+                                valid EFI_LOADED_IMAGE_PROTOCOL. However, the current 
+                                platform policy specifies that the image should not be started.
+**/
+typedef
+EFI_STATUS
+(EFIAPI *LOAD_PE_IMAGE)(
+  IN EFI_PE32_IMAGE_PROTOCOL           *This,
+  IN  EFI_HANDLE                       ParentImageHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL         *FilePath,
+  IN  VOID                             *SourceBuffer       OPTIONAL,
+  IN  UINTN                            SourceSize,
+  IN  EFI_PHYSICAL_ADDRESS             DstBuffer           OPTIONAL,
+  IN OUT UINTN                         *NumberOfPages      OPTIONAL,
+  OUT EFI_HANDLE                       *ImageHandle,
+  OUT EFI_PHYSICAL_ADDRESS             *EntryPoint         OPTIONAL,
+  IN  UINT32                           Attribute
+  );
+
+/**
+
+  Unload the specified image.
+
+  @param  This             The pointer to the LoadPe32Image protocol instance
+  @param  ImageHandle      The specified image handle to be unloaded.
+
+  @retval EFI_INVALID_PARAMETER Image handle is NULL.
+  @retval EFI_UNSUPPORTED       Attempted to unload an unsupported image.
+  @retval EFI_SUCCESS           The image successfully unloaded.
+
+--*/
+typedef
+EFI_STATUS
+(EFIAPI *UNLOAD_PE_IMAGE)(
+  IN EFI_PE32_IMAGE_PROTOCOL          *This,
+  IN EFI_HANDLE                       ImageHandle
+  );
+
+struct _EFI_PE32_IMAGE_PROTOCOL {
+  LOAD_PE_IMAGE     LoadPeImage;
+  UNLOAD_PE_IMAGE   UnLoadPeImage;
+};
+
+extern EFI_GUID gEfiLoadPeImageProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/LockBox.h b/Core/MdeModulePkg/Include/Protocol/LockBox.h
new file mode 100644
index 0000000000..a3533c536c
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/LockBox.h
@@ -0,0 +1,31 @@
+/** @file
+  LockBox protocol header file.
+  This is used to resolve dependency problem. The LockBox implementation
+  install this to broadcast that LockBox API is ready. The driver who will
+  use LockBox at its ENTRYPOINT should add this dependency.
+
+Copyright (c) 2010 - 2011, 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.
+
+**/
+
+#ifndef _LOCK_BOX_PROTOCOL_H_
+#define _LOCK_BOX_PROTOCOL_H_
+
+///
+/// Global ID for the EFI LOCK BOX Protocol.
+///
+#define EFI_LOCK_BOX_PROTOCOL_GUID \
+  { 0xbd445d79, 0xb7ad, 0x4f04, { 0x9a, 0xd8, 0x29, 0xbd, 0x20, 0x40, 0xeb, 0x3c }}
+
+extern EFI_GUID gEfiLockBoxProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/Print2.h b/Core/MdeModulePkg/Include/Protocol/Print2.h
new file mode 100644
index 0000000000..1c127d5542
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/Print2.h
@@ -0,0 +1,469 @@
+/** @file
+
+  This print protocol defines six basic print functions to 
+  print the format unicode and ascii string.
+
+Copyright (c) 2006 - 2010, 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 that 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.
+
+**/
+
+#ifndef __PPRINT2_H__
+#define __PPRINT2_H__
+
+#define EFI_PRINT2_PROTOCOL_GUID  \
+  { 0xf05976ef, 0x83f1, 0x4f3d, { 0x86, 0x19, 0xf7, 0x59, 0x5d, 0x41, 0xe5, 0x38 } }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EFI_PRINT2_PROTOCOL  EFI_PRINT2_PROTOCOL;
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer, based on 
+  a Null-terminated Unicode format string and a BASE_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer 
+  specified by StartOfBuffer and BufferSize.  
+  The Unicode string is produced by parsing the format string specified by FormatString.  
+  Arguments are pulled from the variable argument list specified by Marker.
+  Marker is constructed based on the contents of the format string.  
+  This function returns the number of Unicode characters in the produced output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If BufferSize > 1 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer, not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *UNICODE_BS_PRINT)(
+  OUT CHAR16        *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  BASE_LIST     Marker
+  );
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated 
+  Unicode format string and variable argument list.
+  
+  Produces a Null-terminated Unicode string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the format string.
+  This function returns the number of Unicode characters in the produced output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If BufferSize > 1 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *UNICODE_S_PRINT)(
+  OUT CHAR16        *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  ...
+  );
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer, based on a Null-terminated
+  ASCII format string and a BASE_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on the 
+  contents of the format string.
+  This function returns the number of Unicode characters in the produced output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated ASCII format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *UNICODE_BS_PRINT_ASCII_FORMAT)(
+  OUT CHAR16       *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  IN  BASE_LIST    Marker
+  );
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated 
+  ASCII format string and a variable argument list.
+  
+  Produces a Null-terminated Unicode string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the format string.
+  This function returns the number of Unicode characters in the produced output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated ASCII format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *UNICODE_S_PRINT_ASCII_FORMAT)(
+  OUT CHAR16       *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  ...
+  );
+
+/**
+  Converts a decimal value to a Null-terminated Unicode string.
+  
+  Converts the decimal number specified by Value to a Null-terminated Unicode 
+  string specified by Buffer containing at most Width characters. No padding of spaces 
+  is ever performed. If Width is 0, then a width of MAXIMUM_VALUE_CHARACTERS is assumed.
+  This function returns the number of Unicode characters in Buffer, not including
+  the Null-terminator.
+  If the conversion contains more than Width characters, this function returns 
+  the first Width characters in the conversion, along with the total number of characters in the conversion.
+  Additional conversion parameters are specified in Flags.  
+  
+  The Flags bit LEFT_JUSTIFY is always ignored.
+  All conversions are left justified in Buffer.
+  If Width is 0, PREFIX_ZERO is ignored in Flags.
+  If COMMA_TYPE is set in Flags, then PREFIX_ZERO is ignored in Flags, and commas
+  are inserted every 3rd digit starting from the right.
+  If RADIX_HEX is set in Flags, then the output buffer will be 
+  formatted in hexadecimal format.
+  If Value is < 0 and RADIX_HEX is not set in Flags, then the fist character in Buffer is a '-'.
+  If PREFIX_ZERO is set in Flags and PREFIX_ZERO is not being ignored, 
+  then Buffer is padded with '0' characters so the combination of the optional '-' 
+  sign character, '0' characters, digit characters for Value, and the Null-terminator
+  add up to Width characters.
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Buffer is NULL, then ASSERT().
+  If Buffer is not aligned on a 16-bit boundary, then ASSERT().
+  If unsupported bits are set in Flags, then ASSERT().
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Width >= MAXIMUM_VALUE_CHARACTERS, then ASSERT()
+
+  @param  Buffer  The pointer to the output buffer for the produced Null-terminated
+                  Unicode string.
+  @param  Flags   The bitmask of flags that specify left justification, zero pad, and commas.
+  @param  Value   The 64-bit signed value to convert to a string.
+  @param  Width   The maximum number of Unicode characters to place in Buffer, not including
+                  the Null-terminator.
+  
+  @return The number of Unicode characters in Buffer not including the Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *UNICODE_VALUE_TO_STRING)(
+  IN OUT CHAR16  *Buffer,
+  IN UINTN       Flags,
+  IN INT64       Value,
+  IN UINTN       Width
+  );
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and a BASE_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer 
+  pecified by StartOfBuffer and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  This function returns the number of ASCII characters in the output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated ASCII format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *ASCII_BS_PRINT)(
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR8   *FormatString,
+  IN  BASE_LIST     Marker
+  );
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and variable argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the format string.
+  This function returns the number of ASCII characters in the output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated ASCII format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+   
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *ASCII_S_PRINT)(
+  OUT CHAR8        *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  ...
+  );
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  Unicode format string and a BASE_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  This function returns the number of ASCII characters in the output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *ASCII_BS_PRINT_UNICODE_FORMAT)(
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  BASE_LIST     Marker
+  );
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  Unicode format string and  variable argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer 
+  specified by StartOfBuffer and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the format string.
+  This function returns the number of ASCII characters in the output buffer, 
+  not including the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *ASCII_S_PRINT_UNICODE_FORMAT)(
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  ...
+  );
+
+/**
+  Converts a decimal value to a Null-terminated ASCII string.
+  
+  Converts the decimal number specified by Value to a Null-terminated ASCII string 
+  specified by Buffer containing at most Width characters. No padding of spaces is ever performed.
+  If Width is 0 then a width of MAXIMUM_VALUE_CHARACTERS is assumed.
+  The number of ASCII characters in Buffer is returned not including the Null-terminator.
+  If the conversion contains more than Width characters, then only the first Width
+  characters are returned, and the total number of characters required to perform
+  the conversion is returned.
+  Additional conversion parameters are specified in Flags.  
+  The Flags bit LEFT_JUSTIFY is always ignored.
+  All conversions are left justified in Buffer.
+  If Width is 0, PREFIX_ZERO is ignored in Flags.
+  If COMMA_TYPE is set in Flags, then PREFIX_ZERO is ignored in Flags, and commas
+  are inserted every 3rd digit starting from the right.
+  If RADIX_HEX is set in Flags, then the output buffer will be 
+  formatted in hexadecimal format.
+  If Value is < 0 and RADIX_HEX is not set in Flags, then the fist character in Buffer is a '-'.
+  If PREFIX_ZERO is set in Flags and PREFIX_ZERO is not being ignored, 
+  then Buffer is padded with '0' characters so the combination of the optional '-' 
+  sign character, '0' characters, digit characters for Value, and the Null-terminator
+  add up to Width characters.
+  
+  If Buffer is NULL, then ASSERT().
+  If unsupported bits are set in Flags, then ASSERT().
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Width >= MAXIMUM_VALUE_CHARACTERS, then ASSERT()
+
+  @param  Buffer  The pointer to the output buffer for the produced Null-terminated
+                  ASCII string.
+  @param  Flags   The bitmask of flags that specify left justification, zero pad, and commas.
+  @param  Value   The 64-bit signed value to convert to a string.
+  @param  Width   The maximum number of ASCII characters to place in Buffer, not including
+                  the Null-terminator.
+  
+  @return The number of ASCII characters in Buffer not including the Null-terminator.
+
+**/
+typedef
+UINTN
+(EFIAPI *ASCII_VALUE_TO_STRING)(
+  OUT CHAR8      *Buffer,
+  IN  UINTN      Flags,
+  IN  INT64      Value,
+  IN  UINTN      Width
+  );
+
+struct _EFI_PRINT2_PROTOCOL {
+  UNICODE_BS_PRINT                     UnicodeBSPrint;
+  UNICODE_S_PRINT                      UnicodeSPrint;
+  UNICODE_BS_PRINT_ASCII_FORMAT        UnicodeBSPrintAsciiFormat;
+  UNICODE_S_PRINT_ASCII_FORMAT         UnicodeSPrintAsciiFormat;
+  UNICODE_VALUE_TO_STRING              UnicodeValueToString;
+  ASCII_BS_PRINT                       AsciiBSPrint;
+  ASCII_S_PRINT                        AsciiSPrint;
+  ASCII_BS_PRINT_UNICODE_FORMAT        AsciiBSPrintUnicodeFormat;
+  ASCII_S_PRINT_UNICODE_FORMAT         AsciiSPrintUnicodeFormat;
+  ASCII_VALUE_TO_STRING                AsciiValueToString;
+};
+
+extern EFI_GUID gEfiPrint2ProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmExitBootServices.h b/Core/MdeModulePkg/Include/Protocol/SmmExitBootServices.h
new file mode 100644
index 0000000000..f34376723e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmExitBootServices.h
@@ -0,0 +1,29 @@
+/** @file
+  EDKII SMM Exit Boot Services protocol.
+
+  This SMM protocol is to be published by the SMM Foundation code to associate
+  with EFI_EVENT_GROUP_EXIT_BOOT_SERVICES to notify SMM driver that system enter
+  exit boot services.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _SMM_EXIT_BOOT_SERVICES_H_
+#define _SMM_EXIT_BOOT_SERVICES_H_
+
+#define EDKII_SMM_EXIT_BOOT_SERVICES_PROTOCOL_GUID \
+  { \
+    0x296eb418, 0xc4c8, 0x4e05, { 0xab, 0x59, 0x39, 0xe8, 0xaf, 0x56, 0xf0, 0xa } \
+  }
+
+extern EFI_GUID gEdkiiSmmExitBootServicesProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmFaultTolerantWrite.h b/Core/MdeModulePkg/Include/Protocol/SmmFaultTolerantWrite.h
new file mode 100644
index 0000000000..a12e53b740
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmFaultTolerantWrite.h
@@ -0,0 +1,38 @@
+/** @file
+  SMM Fault Tolerant Write protocol is related to EDK II-specific implementation of FTW,
+  provides boot-time service for fault tolerant write capability for block devices in 
+  EFI SMM environment.  The protocol provides for non-volatile storage of the intermediate 
+  data and private information a caller would need to recover from a critical fault, 
+  such as a power failure.   
+
+Copyright (c) 2010, 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 that 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. 
+
+**/
+
+#ifndef __SMM_FAULT_TOLERANT_WRITE_H__
+#define __SMM_FAULT_TOLERANT_WRITE_H__
+
+#include <Protocol/FaultTolerantWrite.h>
+
+#define EFI_SMM_FAULT_TOLERANT_WRITE_PROTOCOL_GUID \
+  { \
+    0x3868fc3b, 0x7e45, 0x43a7, { 0x90, 0x6c, 0x4b, 0xa4, 0x7d, 0xe1, 0x75, 0x4d } \
+  }
+
+//
+// SMM Fault Tolerant Write protocol structure is the same as Fault Tolerant Write protocol.  
+// The SMM one is intend to run in SMM environment, which means it can be used by 
+// SMM drivers after ExitPmAuth. 
+// 
+typedef EFI_FAULT_TOLERANT_WRITE_PROTOCOL EFI_SMM_FAULT_TOLERANT_WRITE_PROTOCOL;
+
+extern EFI_GUID gEfiSmmFaultTolerantWriteProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmFirmwareVolumeBlock.h b/Core/MdeModulePkg/Include/Protocol/SmmFirmwareVolumeBlock.h
new file mode 100644
index 0000000000..53480d9737
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmFirmwareVolumeBlock.h
@@ -0,0 +1,36 @@
+/** @file
+  SMM Firmware Volume Block protocol is related to EDK II-specific implementation of
+  FVB driver, provides control over block-oriented firmware devices and is intended 
+  to use in the EFI SMM environment.
+
+Copyright (c) 2010 - 2011, 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 that 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.             
+
+**/
+
+#ifndef __SMM_FIRMWARE_VOLUME_BLOCK_H__
+#define __SMM_FIRMWARE_VOLUME_BLOCK_H__
+
+#include <Protocol/FirmwareVolumeBlock.h>
+
+#define EFI_SMM_FIRMWARE_VOLUME_BLOCK_PROTOCOL_GUID \
+  { \
+    0xd326d041, 0xbd31, 0x4c01, { 0xb5, 0xa8, 0x62, 0x8b, 0xe8, 0x7f, 0x6, 0x53 } \
+  }
+
+//
+// SMM Firmware Volume Block protocol structure is the same as Firmware Volume Block 
+// protocol. The SMM one is intend to run in SMM environment, which means it can be  
+// used by SMM drivers after ExitPmAuth. 
+// 
+typedef EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL EFI_SMM_FIRMWARE_VOLUME_BLOCK_PROTOCOL;
+
+extern EFI_GUID gEfiSmmFirmwareVolumeBlockProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmLegacyBoot.h b/Core/MdeModulePkg/Include/Protocol/SmmLegacyBoot.h
new file mode 100644
index 0000000000..68a04e84fd
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmLegacyBoot.h
@@ -0,0 +1,28 @@
+/** @file
+  EDKII SMM Legacy Boot protocol.
+
+  This SMM protocol is to be published by the SMM Foundation code to associate
+  with EFI_EVENT_LEGACY_BOOT_GUID to notify SMM driver that system enter legacy boot.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _SMM_LEGACY_BOOT_H_
+#define _SMM_LEGACY_BOOT_H_
+
+#define EDKII_SMM_LEGACY_BOOT_PROTOCOL_GUID \
+  { \
+    0x85a8ab57, 0x644, 0x4110, { 0x85, 0xf, 0x98, 0x13, 0x22, 0x4, 0x70, 0x70 } \
+  }
+
+extern EFI_GUID gEdkiiSmmLegacyBootProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmReadyToBoot.h b/Core/MdeModulePkg/Include/Protocol/SmmReadyToBoot.h
new file mode 100644
index 0000000000..b6867f5f9b
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmReadyToBoot.h
@@ -0,0 +1,29 @@
+/** @file
+  EDKII SMM Ready To Boot protocol.
+
+  This SMM protocol is to be published by the SMM Foundation code to associate
+  with EFI_EVENT_GROUP_READY_TO_BOOT to notify SMM driver that system enter
+  ready to boot.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _SMM_READY_TO_BOOT_H_
+#define _SMM_READY_TO_BOOT_H_
+
+#define EDKII_SMM_READY_TO_BOOT_PROTOCOL_GUID \
+  { \
+    0x6e057ecf, 0xfa99, 0x4f39, { 0x95, 0xbc, 0x59, 0xf9, 0x92, 0x1d, 0x17, 0xe4 } \
+  }
+
+extern EFI_GUID gEdkiiSmmReadyToBootProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmSwapAddressRange.h b/Core/MdeModulePkg/Include/Protocol/SmmSwapAddressRange.h
new file mode 100644
index 0000000000..60a0a2c4a2
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmSwapAddressRange.h
@@ -0,0 +1,40 @@
+/** @file
+  The EFI_SMM_SWAP_ADDRESS_RANGE_PROTOCOL is related to EDK II-specific implementation 
+  and used to abstract the swap operation of boot block and backup block of FV in EFI 
+  SMM environment. This swap is especially needed when updating the boot block of FV. 
+  If a power failure happens during the boot block update, the swapped backup block 
+  (now the boot block) can boot the machine with the old boot block backed up in it. 
+  The swap operation is platform dependent, so other protocols such as SMM FTW (Fault 
+  Tolerant Write) should use this protocol instead of handling hardware directly.
+
+Copyright (c) 2010, 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 that 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. 
+
+**/
+
+#ifndef __SMM_SWAP_ADDRESS_RANGE_H__
+#define __SMM_SWAP_ADDRESS_RANGE_H__
+
+#include <Protocol/SwapAddressRange.h>
+
+#define EFI_SMM_SWAP_ADDRESS_RANGE_PROTOCOL_GUID \
+  { \
+    0x67c4f112, 0x3385, 0x4e55, { 0x9c, 0x5b, 0xc0, 0x5b, 0x71, 0x7c, 0x42, 0x28 } \
+  }
+
+//
+// SMM Swap Address Range protocol structure is the same as Swap Address Range protocol.  
+// The SMM one is intend to run in SMM environment, which means it can be used by 
+// SMM drivers after ExitPmAuth. 
+// 
+typedef EFI_SWAP_ADDRESS_RANGE_PROTOCOL EFI_SMM_SWAP_ADDRESS_RANGE_PROTOCOL;
+
+extern EFI_GUID gEfiSmmSwapAddressRangeProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmVarCheck.h b/Core/MdeModulePkg/Include/Protocol/SmmVarCheck.h
new file mode 100644
index 0000000000..7faf5a95d3
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmVarCheck.h
@@ -0,0 +1,36 @@
+/** @file
+  SMM variable check definitions, it reuses the interface definitions of variable check.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef __SMM_VAR_CHECK_H__
+#define __SMM_VAR_CHECK_H__
+
+#include <Protocol/VarCheck.h>
+
+#define EDKII_SMM_VAR_CHECK_PROTOCOL_GUID \
+  { \
+    0xb0d8f3c1, 0xb7de, 0x4c11, { 0xbc, 0x89, 0x2f, 0xb5, 0x62, 0xc8, 0xc4, 0x11 } \
+  };
+
+typedef struct _EDKII_SMM_VAR_CHECK_PROTOCOL EDKII_SMM_VAR_CHECK_PROTOCOL;
+
+struct _EDKII_SMM_VAR_CHECK_PROTOCOL {
+  EDKII_VAR_CHECK_REGISTER_SET_VARIABLE_CHECK_HANDLER   SmmRegisterSetVariableCheckHandler;
+  EDKII_VAR_CHECK_VARIABLE_PROPERTY_SET                 SmmVariablePropertySet;
+  EDKII_VAR_CHECK_VARIABLE_PROPERTY_GET                 SmmVariablePropertyGet;
+};
+
+extern EFI_GUID gEdkiiSmmVarCheckProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/SmmVariable.h b/Core/MdeModulePkg/Include/Protocol/SmmVariable.h
new file mode 100644
index 0000000000..75ab6c3057
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SmmVariable.h
@@ -0,0 +1,39 @@
+/** @file
+  EFI SMM Variable Protocol is related to EDK II-specific implementation of variables
+  and intended for use as a means to store data in the EFI SMM environment.
+
+  Copyright (c) 2010, 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.             
+
+**/
+
+#ifndef __SMM_VARIABLE_H__
+#define __SMM_VARIABLE_H__
+
+#define EFI_SMM_VARIABLE_PROTOCOL_GUID \
+  { \
+    0xed32d533, 0x99e6, 0x4209, { 0x9c, 0xc0, 0x2d, 0x72, 0xcd, 0xd9, 0x98, 0xa7 } \
+  }
+
+typedef struct _EFI_SMM_VARIABLE_PROTOCOL  EFI_SMM_VARIABLE_PROTOCOL;
+
+///
+/// EFI SMM Variable Protocol is intended for use as a means 
+/// to store data in the EFI SMM environment.
+///
+struct _EFI_SMM_VARIABLE_PROTOCOL {
+  EFI_GET_VARIABLE            SmmGetVariable;
+  EFI_GET_NEXT_VARIABLE_NAME  SmmGetNextVariableName;
+  EFI_SET_VARIABLE            SmmSetVariable;
+  EFI_QUERY_VARIABLE_INFO     SmmQueryVariableInfo;
+};
+
+extern EFI_GUID gEfiSmmVariableProtocolGuid;
+
+#endif  
diff --git a/Core/MdeModulePkg/Include/Protocol/SwapAddressRange.h b/Core/MdeModulePkg/Include/Protocol/SwapAddressRange.h
new file mode 100644
index 0000000000..3ca87a0e6e
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/SwapAddressRange.h
@@ -0,0 +1,174 @@
+/** @file
+The EFI_SWAP_ADDRESS_RANGE_PROTOCOL is used to abstract the swap operation of boot block 
+and backup block of FV. This swap is especially needed when updating the boot block of FV. If a 
+power failure happens during the boot block update, the swapped backup block (now the boot block) 
+can boot the machine with the old boot block backed up in it. The swap operation is platform dependent, so 
+other protocols such as FTW (Fault Tolerant Write) should use this protocol instead of handling hardware directly.
+
+Copyright (c) 2009 - 2010, 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 that 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. 
+
+**/
+
+#ifndef _EFI_SWAP_ADDRESS_RANGE_PROTOCOL_H_
+#define _EFI_SWAP_ADDRESS_RANGE_PROTOCOL_H_
+
+#define EFI_SWAP_ADDRESS_RANGE_PROTOCOL_GUID \
+  { \
+    0x1259f60d, 0xb754, 0x468e, {0xa7, 0x89, 0x4d, 0xb8, 0x5d, 0x55, 0xe8, 0x7e } \
+  }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EFI_SWAP_ADDRESS_RANGE_PROTOCOL  EFI_SWAP_ADDRESS_RANGE_PROTOCOL;
+
+#define EFI_UNSUPPORT_LOCK  0
+#define EFI_SOFTWARE_LOCK   1
+#define EFI_HARDWARE_LOCK   2
+
+typedef UINT8 EFI_SWAP_LOCK_CAPABILITY;
+
+//
+// Protocol APIs
+//
+
+/**
+  This function gets the address range location of 
+  boot block and backup block. 
+
+  @param This             Indicates the calling context.  
+  @param BootBlockBase    The base address of current boot block.
+  @param BootBlockSize    The size (in bytes) of current boot block.
+  @param BackupBlockBase  The base address of current backup block.
+  @param BackupBlockSize  The size (in bytes) of current backup block.
+
+  @retval EFI_SUCCESS  The call was successful.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_GET_RANGE_LOCATION)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  OUT EFI_PHYSICAL_ADDRESS                      *BootBlockBase,
+  OUT UINTN                                     *BootBlockSize,
+  OUT EFI_PHYSICAL_ADDRESS                      *BackupBlockBase,
+  OUT UINTN                                     *BackupBlockSize
+  );
+
+/**
+  This service checks if the boot block and backup block has been swapped.
+
+  @param This          Indicates the calling context.  
+  @param SwapState     True if the boot block and backup block has been swapped. 
+                       False if the boot block and backup block has not been swapped.
+
+  @retval EFI_SUCCESS  The call was successful.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_GET_SWAP_STATE)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  OUT BOOLEAN                                   *SwapState
+  );
+
+/**
+  This service swaps the boot block and backup block, or swaps them back.
+
+  It also acquires and releases software swap lock during operation. The setting of the new swap state 
+  is not affected by the old swap state.
+
+  @param This            Indicates the calling context.  
+  @param NewSwapState    True to swap real boot block and backup block, False to swap them back.
+
+  @retval EFI_SUCCESS  The call was successful.
+  @retval EFI_ABORTED  Set swap state error.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_SET_SWAP_STATE)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  IN BOOLEAN                                    NewSwapState
+  );
+
+
+
+/**
+  This service checks if a Real Time Clock (RTC) power failure happened.
+
+  If parameter RtcPowerFailed is true after the function returns, RTC power supply failed or was removed. 
+  It is recommended to check RTC power status before calling GetSwapState().
+
+  @param This             Indicates the calling context.  
+  @param RtcPowerFailed   True if the RTC (Real Time Clock) power failed or was removed. 
+
+  @retval EFI_SUCCESS The call was successful.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_GET_RTC_POWER_STATUS)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  OUT BOOLEAN                                   *RtcPowerFailed
+  );
+
+/**
+  This service returns all lock methods for swap operations that the current platform 
+  supports. Could be software lock, hardware lock, or unsupport lock.
+  Note that software and hardware lock methods can be used simultaneously.
+
+  @param This             Indicates the calling context.
+  @param LockCapability   The current lock method for swap operations. 
+
+  @retval EFI_SUCCESS The call was successful.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_GET_SWAP_LOCK_CAPABILITY)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  OUT EFI_SWAP_LOCK_CAPABILITY                  *LockCapability
+  );
+
+
+
+/**
+  This service is used to acquire or release appointed kind of lock for Swap Address Range operations.
+
+  Note that software and hardware lock mothod can be used simultaneously.
+
+  @param This              Indicates the calling context.
+  @param LockCapability    Indicates which lock to acquire or release.
+  @param NewLockState      True to acquire lock; False to release lock.
+
+  @retval EFI_SUCCESS The call was successful.
+    
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EFI_SET_SWAP_LOCK)(
+  IN EFI_SWAP_ADDRESS_RANGE_PROTOCOL            *This,
+  IN EFI_SWAP_LOCK_CAPABILITY                   LockCapability,
+  IN BOOLEAN                                    NewLockState
+  );
+
+struct _EFI_SWAP_ADDRESS_RANGE_PROTOCOL {
+  EFI_GET_RANGE_LOCATION        GetRangeLocation;       // has output parameters for base and length
+  EFI_GET_SWAP_STATE            GetSwapState;           // are ranges swapped or not
+  EFI_SET_SWAP_STATE            SetSwapState;           // swap or unswap ranges
+  EFI_GET_RTC_POWER_STATUS      GetRtcPowerStatus;      // checks RTC battery, or whatever...
+  EFI_GET_SWAP_LOCK_CAPABILITY  GetSwapLockCapability;  // Get TOP_SWAP lock capability,
+  EFI_SET_SWAP_LOCK             SetSwapLock;            // Set TOP_SWAP lock state
+};
+
+extern EFI_GUID gEfiSwapAddressRangeProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/UfsHostController.h b/Core/MdeModulePkg/Include/Protocol/UfsHostController.h
new file mode 100644
index 0000000000..909c981729
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/UfsHostController.h
@@ -0,0 +1,243 @@
+/** @file
+
+  EDKII Universal Flash Storage Host Controller Protocol.
+
+Copyright (c) 2014 - 2015, 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 that 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.
+
+**/
+
+
+#ifndef __EDKII_UFS_HC_PROTOCOL_H__
+#define __EDKII_UFS_HC_PROTOCOL_H__
+
+//
+// UFS Host Controller Protocol GUID value
+//
+#define EDKII_UFS_HOST_CONTROLLER_PROTOCOL_GUID \
+    { \
+      0xebc01af5, 0x7a9, 0x489e, { 0xb7, 0xce, 0xdc, 0x8, 0x9e, 0x45, 0x9b, 0x2f } \
+    }
+
+//
+// Forward reference for pure ANSI compatability
+//
+typedef struct _EDKII_UFS_HOST_CONTROLLER_PROTOCOL  EDKII_UFS_HOST_CONTROLLER_PROTOCOL;
+
+
+/**
+  Get the MMIO base address of UFS host controller.
+
+  @param  This          The protocol instance pointer.
+  @param  MmioBar       Pointer to the UFS host controller MMIO base address.
+
+  @retval EFI_SUCCESS            The operation succeeds.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_GET_MMIO_BAR)(
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL     *This,
+     OUT UINTN                                  *MmioBar
+  );
+
+///
+/// *******************************************************
+/// EFI_UFS_HOST_CONTROLLER_OPERATION
+/// *******************************************************
+///
+typedef enum {
+  ///
+  /// A read operation from system memory by a bus master.
+  ///
+  EdkiiUfsHcOperationBusMasterRead,
+  ///
+  /// A write operation from system memory by a bus master.
+  ///
+  EdkiiUfsHcOperationBusMasterWrite,
+  ///
+  /// Provides both read and write access to system memory by both the processor and a
+  /// bus master. The buffer is coherent from both the processor's and the bus master's point of view.
+  ///
+  EdkiiUfsHcOperationBusMasterCommonBuffer,
+  EdkiiUfsHcOperationMaximum
+} EDKII_UFS_HOST_CONTROLLER_OPERATION;
+
+/**                                                                 
+  Provides the UFS controller-specific addresses needed to access system memory.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Operation             Indicates if the bus master is going to read or write to system memory.
+  @param  HostAddress           The system memory address to map to the UFS controller.
+  @param  NumberOfBytes         On input the number of bytes to map. On output the number of bytes
+                                that were mapped.                                                 
+  @param  DeviceAddress         The resulting map address for the bus master UFS controller to use to
+                                access the hosts HostAddress.                                        
+  @param  Mapping               A resulting value to pass to Unmap().
+                                  
+  @retval EFI_SUCCESS           The range was mapped for the returned NumberOfBytes.
+  @retval EFI_UNSUPPORTED       The HostAddress cannot be mapped as a common buffer.                                
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested address.
+                                   
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_MAP)(
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_OPERATION  Operation,
+  IN     VOID                                 *HostAddress,
+  IN OUT UINTN                                *NumberOfBytes,
+     OUT EFI_PHYSICAL_ADDRESS                 *DeviceAddress,
+     OUT VOID                                 **Mapping
+  );
+
+/**                                                                 
+  Completes the Map() operation and releases any corresponding resources.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.                                      
+  @param  Mapping               The mapping value returned from Map().
+                                  
+  @retval EFI_SUCCESS           The range was unmapped.
+  @retval EFI_DEVICE_ERROR      The data was not committed to the target system memory.
+                                   
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_UNMAP)(
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL     *This,
+  IN  VOID                                   *Mapping
+  );
+
+/**                                                                 
+  Allocates pages that are suitable for an EfiUfsHcOperationBusMasterCommonBuffer
+  mapping.                                                                       
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Type                  This parameter is not used and must be ignored.
+  @param  MemoryType            The type of memory to allocate, EfiBootServicesData or
+                                EfiRuntimeServicesData.                               
+  @param  Pages                 The number of pages to allocate.                                
+  @param  HostAddress           A pointer to store the base system memory address of the
+                                allocated range.                                        
+  @param  Attributes            The requested bit mask of attributes for the allocated range.
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were allocated.
+  @retval EFI_UNSUPPORTED       Attributes is unsupported. The only legal attribute bits are
+                                MEMORY_WRITE_COMBINE and MEMORY_CACHED.                     
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  The memory pages could not be allocated.  
+                                   
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_ALLOCATE_BUFFER)(
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN     EFI_ALLOCATE_TYPE                    Type,
+  IN     EFI_MEMORY_TYPE                      MemoryType,
+  IN     UINTN                                Pages,
+     OUT VOID                                 **HostAddress,
+  IN     UINT64                               Attributes
+  );
+
+/**                                                                 
+  Frees memory that was allocated with AllocateBuffer().
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+  @param  Pages                 The number of pages to free.                                
+  @param  HostAddress           The base system memory address of the allocated range.                                    
+                                  
+  @retval EFI_SUCCESS           The requested memory pages were freed.
+  @retval EFI_INVALID_PARAMETER The memory range specified by HostAddress and Pages
+                                was not allocated with AllocateBuffer().
+                                     
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_FREE_BUFFER)(
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This,
+  IN  UINTN                                Pages,
+  IN  VOID                                 *HostAddress
+  );
+
+/**                                                                 
+  Flushes all posted write transactions from the UFS bus to attached UFS device.
+            
+  @param  This                  A pointer to the EFI_UFS_HOST_CONTROLLER_PROTOCOL instance.  
+                                  
+  @retval EFI_SUCCESS           The posted write transactions were flushed from the UFS bus
+                                to attached UFS device.                                      
+  @retval EFI_DEVICE_ERROR      The posted write transactions were not flushed from the UFS
+                                bus to attached UFS device due to a hardware error.                           
+                                     
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_FLUSH)(
+  IN  EDKII_UFS_HOST_CONTROLLER_PROTOCOL   *This
+  );
+
+typedef enum {
+  EfiUfsHcWidthUint8      = 0,
+  EfiUfsHcWidthUint16,
+  EfiUfsHcWidthUint32,
+  EfiUfsHcWidthUint64,
+  EfiUfsHcWidthMaximum
+} EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH;
+
+/**
+  Enable a UFS bus driver to access UFS MMIO registers in the UFS Host Controller memory space.
+
+  @param  This                  A pointer to the EDKII_UFS_HOST_CONTROLLER_PROTOCOL instance.
+  @param  Width                 Signifies the width of the memory operations.
+  @param  Offset                The offset within the UFS Host Controller MMIO space to start the
+                                memory operation.
+  @param  Count                 The number of memory operations to perform.
+  @param  Buffer                For read operations, the destination buffer to store the results.
+                                For write operations, the source buffer to write data from.
+
+  @retval EFI_SUCCESS           The data was read from or written to the UFS host controller.
+  @retval EFI_UNSUPPORTED       The address range specified by Offset, Width, and Count is not
+                                valid for the UFS Host Controller memory space.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI *EDKII_UFS_HC_MMIO_READ_WRITE)(
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL        *This,
+  IN     EDKII_UFS_HOST_CONTROLLER_PROTOCOL_WIDTH  Width,
+  IN     UINT64                                    Offset,
+  IN     UINTN                                     Count,
+  IN OUT VOID                                      *Buffer
+  );
+
+///
+///  UFS Host Controller Protocol structure.
+///
+struct _EDKII_UFS_HOST_CONTROLLER_PROTOCOL {
+  EDKII_UFS_HC_GET_MMIO_BAR           GetUfsHcMmioBar;
+  EDKII_UFS_HC_ALLOCATE_BUFFER        AllocateBuffer;
+  EDKII_UFS_HC_FREE_BUFFER            FreeBuffer;
+  EDKII_UFS_HC_MAP                    Map;
+  EDKII_UFS_HC_UNMAP                  Unmap;
+  EDKII_UFS_HC_FLUSH                  Flush;
+  EDKII_UFS_HC_MMIO_READ_WRITE        Read;
+  EDKII_UFS_HC_MMIO_READ_WRITE        Write;
+};
+
+///
+///  UFS Host Controller Protocol GUID variable.
+///
+extern EFI_GUID gEdkiiUfsHostControllerProtocolGuid;
+
+#endif
diff --git a/Core/MdeModulePkg/Include/Protocol/VarCheck.h b/Core/MdeModulePkg/Include/Protocol/VarCheck.h
new file mode 100644
index 0000000000..3525989c55
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/VarCheck.h
@@ -0,0 +1,126 @@
+/** @file
+  Variable check definitions.
+
+  Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VARIABLE_CHECK_H_
+#define _VARIABLE_CHECK_H_
+
+#include <Uefi/UefiSpec.h>
+
+typedef struct _EDKII_VAR_CHECK_PROTOCOL    EDKII_VAR_CHECK_PROTOCOL;
+
+#define EDKII_VAR_CHECK_PROTOCOL_GUID { \
+  0xaf23b340, 0x97b4, 0x4685, { 0x8d, 0x4f, 0xa3, 0xf2, 0x81, 0x69, 0xb2, 0x1d } \
+};
+
+typedef EFI_SET_VARIABLE VAR_CHECK_SET_VARIABLE_CHECK_HANDLER;
+
+/**
+  Register SetVariable check handler.
+  Variable driver will call the handler to do check before
+  really setting the variable into variable storage.
+
+  @param[in] Handler            Pointer to the check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * EDKII_VAR_CHECK_REGISTER_SET_VARIABLE_CHECK_HANDLER) (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  );
+
+#define VAR_CHECK_VARIABLE_PROPERTY_REVISION      0x0001
+//
+// 1. Set by VariableLock PROTOCOL
+// 2. Set by VarCheck PROTOCOL
+//
+// If set, other fields for check will be ignored.
+//
+#define VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY     BIT0
+
+typedef struct {
+  UINT16                            Revision;
+  UINT16                            Property;
+  UINT32                            Attributes;
+  UINTN                             MinSize;
+  UINTN                             MaxSize;
+} VAR_CHECK_VARIABLE_PROPERTY;
+
+typedef struct {
+  EFI_GUID                      *Guid;
+  CHAR16                        *Name;
+  VAR_CHECK_VARIABLE_PROPERTY   VariableProperty;
+} VARIABLE_ENTRY_PROPERTY;
+
+/**
+  Variable property set.
+  Variable driver will do check according to the VariableProperty before
+  really setting the variable into variable storage.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * EDKII_VAR_CHECK_VARIABLE_PROPERTY_SET) (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  );
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+typedef
+EFI_STATUS
+(EFIAPI * EDKII_VAR_CHECK_VARIABLE_PROPERTY_GET) (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  );
+
+struct _EDKII_VAR_CHECK_PROTOCOL {
+  EDKII_VAR_CHECK_REGISTER_SET_VARIABLE_CHECK_HANDLER   RegisterSetVariableCheckHandler;
+  EDKII_VAR_CHECK_VARIABLE_PROPERTY_SET                 VariablePropertySet;
+  EDKII_VAR_CHECK_VARIABLE_PROPERTY_GET                 VariablePropertyGet;
+};
+
+extern EFI_GUID gEdkiiVarCheckProtocolGuid;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Include/Protocol/VariableLock.h b/Core/MdeModulePkg/Include/Protocol/VariableLock.h
new file mode 100644
index 0000000000..a2a73bc161
--- /dev/null
+++ b/Core/MdeModulePkg/Include/Protocol/VariableLock.h
@@ -0,0 +1,63 @@
+/** @file
+  Variable Lock Protocol is related to EDK II-specific implementation of variables
+  and intended for use as a means to mark a variable read-only after the event
+  EFI_END_OF_DXE_EVENT_GUID is signaled.
+
+  Copyright (c) 2013, 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.             
+
+**/
+
+#ifndef __VARIABLE_LOCK_H__
+#define __VARIABLE_LOCK_H__
+
+#define EDKII_VARIABLE_LOCK_PROTOCOL_GUID \
+  { \
+    0xcd3d0a05, 0x9e24, 0x437c, { 0xa8, 0x91, 0x1e, 0xe0, 0x53, 0xdb, 0x76, 0x38 } \
+  }
+
+typedef struct _EDKII_VARIABLE_LOCK_PROTOCOL  EDKII_VARIABLE_LOCK_PROTOCOL;
+
+/**
+  Mark a variable that will become read-only after leaving the DXE phase of execution.
+  Write request coming from SMM environment through EFI_SMM_VARIABLE_PROTOCOL is allowed.
+
+  @param[in] This          The EDKII_VARIABLE_LOCK_PROTOCOL instance.
+  @param[in] VariableName  A pointer to the variable name that will be made read-only subsequently.
+  @param[in] VendorGuid    A pointer to the vendor GUID that will be made read-only subsequently.
+
+  @retval EFI_SUCCESS           The variable specified by the VariableName and the VendorGuid was marked
+                                as pending to be read-only.
+  @retval EFI_INVALID_PARAMETER VariableName or VendorGuid is NULL.
+                                Or VariableName is an empty string.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource to hold the lock request.
+**/
+typedef
+EFI_STATUS
+(EFIAPI * EDKII_VARIABLE_LOCK_PROTOCOL_REQUEST_TO_LOCK) (
+  IN CONST EDKII_VARIABLE_LOCK_PROTOCOL *This,
+  IN       CHAR16                       *VariableName,
+  IN       EFI_GUID                     *VendorGuid
+  );
+
+///
+/// Variable Lock Protocol is related to EDK II-specific implementation of variables
+/// and intended for use as a means to mark a variable read-only after the event
+/// EFI_END_OF_DXE_EVENT_GUID is signaled.
+///
+struct _EDKII_VARIABLE_LOCK_PROTOCOL {
+  EDKII_VARIABLE_LOCK_PROTOCOL_REQUEST_TO_LOCK RequestToLock;
+};
+
+extern EFI_GUID gEdkiiVariableLockProtocolGuid;
+
+#endif  
+
diff --git a/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.c b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.c
new file mode 100644
index 0000000000..054131fd3e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.c
@@ -0,0 +1,78 @@
+/** @file
+  Implements NULL authenticated variable services.
+
+Copyright (c) 2015, 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 <Library/AuthVariableLib.h>
+#include <Library/DebugLib.h>
+
+/**
+  Initialization for authenticated varibale services.
+  If this initialization returns error status, other APIs will not work
+  and expect to be not called then.
+
+  @param[in]  AuthVarLibContextIn   Pointer to input auth variable lib context.
+  @param[out] AuthVarLibContextOut  Pointer to output auth variable lib context.
+
+  @retval EFI_SUCCESS               Function successfully executed.
+  @retval EFI_INVALID_PARAMETER     If AuthVarLibContextIn == NULL or AuthVarLibContextOut == NULL.
+  @retval EFI_OUT_OF_RESOURCES      Fail to allocate enough resource.
+  @retval EFI_UNSUPPORTED           Unsupported to process authenticated variable.
+
+**/
+EFI_STATUS
+EFIAPI
+AuthVariableLibInitialize (
+  IN  AUTH_VAR_LIB_CONTEXT_IN   *AuthVarLibContextIn,
+  OUT AUTH_VAR_LIB_CONTEXT_OUT  *AuthVarLibContextOut
+  )
+{
+  //
+  // Do nothing, just return EFI_UNSUPPORTED.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Process variable with EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS/EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS set.
+
+  @param[in] VariableName           Name of the variable.
+  @param[in] VendorGuid             Variable vendor GUID.
+  @param[in] Data                   Data pointer.
+  @param[in] DataSize               Size of Data.
+  @param[in] Attributes             Attribute value of the variable.
+
+  @retval EFI_SUCCESS               The firmware has successfully stored the variable and its data as
+                                    defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER     Invalid parameter.
+  @retval EFI_WRITE_PROTECTED       Variable is write-protected.
+  @retval EFI_OUT_OF_RESOURCES      There is not enough resource.
+  @retval EFI_SECURITY_VIOLATION    The variable is with EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS
+                                    or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS
+                                    set, but the AuthInfo does NOT pass the validation
+                                    check carried out by the firmware.
+  @retval EFI_UNSUPPORTED           Unsupported to process authenticated variable.
+
+**/
+EFI_STATUS
+EFIAPI
+AuthVariableLibProcessVariable (
+  IN CHAR16         *VariableName,
+  IN EFI_GUID       *VendorGuid,
+  IN VOID           *Data,
+  IN UINTN          DataSize,
+  IN UINT32         Attributes
+  )
+{
+  ASSERT (FALSE);
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.inf b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.inf
new file mode 100644
index 0000000000..900fef5d49
--- /dev/null
+++ b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.inf
@@ -0,0 +1,40 @@
+## @file
+#  Provides NULL authenticated variable services.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AuthVariableLibNull
+  MODULE_UNI_FILE                = AuthVariableLibNull.uni
+  FILE_GUID                      = 435CB0E4-7C9A-4BB7-9907-8FD4643E978A
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = AuthVariableLib|DXE_RUNTIME_DRIVER DXE_SMM_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  AuthVariableLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DebugLib
diff --git a/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.uni b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.uni
new file mode 100644
index 0000000000..02237fffe2
--- /dev/null
+++ b/Core/MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.uni
diff --git a/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.c b/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.c
new file mode 100644
index 0000000000..a7db3f13f8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.c
@@ -0,0 +1,53 @@
+/** @file
+  A emptry template implementation of Ipmi Library.
+
+  Copyright (c) 2011 - 2015, 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 <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/IpmiLib.h>
+
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+EFI_STATUS
+EFIAPI
+IpmiSubmitCommand (
+  IN     UINT8     NetFunction,
+  IN     UINT8     Command,
+  IN     UINT8     *RequestData,
+  IN     UINT32    RequestDataSize,
+     OUT UINT8     *ResponseData,
+  IN OUT UINT32    *ResponseDataSize
+  )
+{
+  //
+  // Do nothing, just return EFI_UNSUPPORTED.
+  //
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.inf b/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.inf
new file mode 100644
index 0000000000..fc9d06d24b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.inf
@@ -0,0 +1,38 @@
+## @file
+#  Null Instance of IPMI Library.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                   = 0x00010005
+  BASE_NAME                     = BaseIpmiLibNull
+  FILE_GUID                     = 46805D61-0BB8-4680-A9BE-C96C751AB5A4
+  MODULE_TYPE                   = BASE
+  VERSION_STRING                = 1.0
+  LIBRARY_CLASS                 = IpmiLib
+
+#
+#  VALID_ARCHITECTURES          = IA32 X64
+#
+
+[Sources]
+  BaseIpmiLibNull.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
diff --git a/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.c b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.c
new file mode 100644
index 0000000000..bd1a31a36e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.c
@@ -0,0 +1,37 @@
+/** @file
+  Null Platform Hook Library instance.
+
+  Copyright (c) 2010, 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 <Base.h>
+#include <Library/PlatformHookLib.h>
+
+/**
+  Performs platform specific initialization required for the CPU to access
+  the hardware associated with a SerialPortLib instance.  This function does
+  not intiailzie the serial port hardware itself.  Instead, it initializes 
+  hardware devices that are required for the CPU to access the serial port 
+  hardware.  This function may be called more than once.
+
+  @retval RETURN_SUCCESS       The platform specific initialization succeeded.
+  @retval RETURN_DEVICE_ERROR  The platform specific initialization could not be completed.
+ 
+**/
+RETURN_STATUS
+EFIAPI
+PlatformHookSerialPortInitialize (
+  VOID
+  )
+{
+  return  RETURN_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.inf b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.inf
new file mode 100644
index 0000000000..6d5195576f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.inf
@@ -0,0 +1,35 @@
+## @file
+#  Null Platform Hook Library instance.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BasePlatformHookLibNull
+  MODULE_UNI_FILE                = BasePlatformHookLibNull.uni
+  FILE_GUID                      = EBC3AEAD-CC13-49b0-A678-5BED93956955
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PlatformHookLib
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BasePlatformHookLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.uni b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.uni
new file mode 100644
index 0000000000..b45b1ae3ce
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.uni
diff --git a/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.c b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.c
new file mode 100644
index 0000000000..d21a77401c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.c
@@ -0,0 +1,79 @@
+/** @file
+  Null Reset System Library instance that only generates ASSERT() conditions.
+
+  Copyright (c) 2006 - 2009, 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 <Base.h>
+
+#include <Library/ResetSystemLib.h>
+#include <Library/DebugLib.h>
+
+/**
+  This function causes a system-wide reset (cold reset), in which
+  all circuitry within the system returns to its initial state. This type of reset 
+  is asynchronous to system operation and operates without regard to
+  cycle boundaries.
+
+  If this function returns, it means that the system does not support cold reset. 
+**/
+VOID
+EFIAPI
+ResetCold (
+  VOID
+  )
+{
+  ASSERT (FALSE);
+}
+
+/**
+  This function causes a system-wide initialization (warm reset), in which all processors 
+  are set to their initial state. Pending cycles are not corrupted.
+
+  If this function returns, it means that the system does not support warm reset.
+**/
+VOID
+EFIAPI
+ResetWarm (
+  VOID
+  )
+{
+  ASSERT (FALSE);
+}
+
+/**
+  This function causes the system to enter a power state equivalent 
+  to the ACPI G2/S5 or G3 states.
+
+  If this function returns, it means that the system does not support shut down reset.
+**/
+VOID
+EFIAPI
+ResetShutdown (
+  VOID
+  )
+{
+  ASSERT (FALSE);
+}
+
+/**
+  This function causes the system to enter S3 and then wake up immediately.
+
+  If this function returns, it means that the system does not support S3 feature.
+**/
+VOID
+EFIAPI
+EnterS3WithImmediateWake (
+  VOID
+  )
+{
+  ASSERT (FALSE);
+}
diff --git a/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.inf b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.inf
new file mode 100644
index 0000000000..96e0ebb212
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.inf
@@ -0,0 +1,38 @@
+## @file
+#  Null Reset System Library instance that only generates ASSERT() conditions.
+#
+#  Copyright (c) 2007 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BaseResetSystemLibNull
+  MODULE_UNI_FILE                = BaseResetSystemLibNull.uni
+  FILE_GUID                      = 667A8B1C-9C97-4b2a-AE7E-568772FE45F3
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ResetSystemLib
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BaseResetSystemLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DebugLib
diff --git a/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.uni b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.uni
new file mode 100644
index 0000000000..a097a82b30
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.uni
diff --git a/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.c b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.c
new file mode 100644
index 0000000000..f4fc31913b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.c
@@ -0,0 +1,1100 @@
+/** @file
+  16550 UART Serial Port library functions
+
+  (C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+  Copyright (c) 2006 - 2015, 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 <Base.h>
+#include <IndustryStandard/Pci.h>
+#include <Library/SerialPortLib.h>
+#include <Library/PcdLib.h>
+#include <Library/IoLib.h>
+#include <Library/PciLib.h>
+#include <Library/PlatformHookLib.h>
+#include <Library/BaseLib.h>
+
+//
+// PCI Defintions.
+//
+#define PCI_BRIDGE_32_BIT_IO_SPACE              0x01
+
+//
+// 16550 UART register offsets and bitfields
+//
+#define R_UART_RXBUF          0
+#define R_UART_TXBUF          0
+#define R_UART_BAUD_LOW       0
+#define R_UART_BAUD_HIGH      1
+#define R_UART_FCR            2
+#define   B_UART_FCR_FIFOE    BIT0
+#define   B_UART_FCR_FIFO64   BIT5
+#define R_UART_LCR            3
+#define   B_UART_LCR_DLAB     BIT7
+#define R_UART_MCR            4
+#define   B_UART_MCR_DTRC     BIT0
+#define   B_UART_MCR_RTS      BIT1
+#define R_UART_LSR            5
+#define   B_UART_LSR_RXRDY    BIT0
+#define   B_UART_LSR_TXRDY    BIT5
+#define   B_UART_LSR_TEMT     BIT6
+#define R_UART_MSR            6
+#define   B_UART_MSR_CTS      BIT4
+#define   B_UART_MSR_DSR      BIT5
+#define   B_UART_MSR_RI       BIT6
+#define   B_UART_MSR_DCD      BIT7
+
+//
+// 4-byte structure for each PCI node in PcdSerialPciDeviceInfo
+//
+typedef struct {
+  UINT8   Device;
+  UINT8   Function;
+  UINT16  PowerManagementStatusAndControlRegister;
+} PCI_UART_DEVICE_INFO;
+
+/**
+  Read an 8-bit 16550 register.  If PcdSerialUseMmio is TRUE, then the value is read from 
+  MMIO space.  If PcdSerialUseMmio is FALSE, then the value is read from I/O space.  The
+  parameter Offset is added to the base address of the 16550 registers that is specified 
+  by PcdSerialRegisterBase. 
+  
+  @param  Base    The base address register of UART device.
+  @param  Offset  The offset of the 16550 register to read.
+
+  @return The value read from the 16550 register.
+
+**/
+UINT8
+SerialPortReadRegister (
+  UINTN  Base,
+  UINTN  Offset
+  )
+{
+  if (PcdGetBool (PcdSerialUseMmio)) {
+    return MmioRead8 (Base + Offset * PcdGet32 (PcdSerialRegisterStride));
+  } else {
+    return IoRead8 (Base + Offset * PcdGet32 (PcdSerialRegisterStride));
+  }
+}
+
+/**
+  Write an 8-bit 16550 register.  If PcdSerialUseMmio is TRUE, then the value is written to
+  MMIO space.  If PcdSerialUseMmio is FALSE, then the value is written to I/O space.  The
+  parameter Offset is added to the base address of the 16550 registers that is specified 
+  by PcdSerialRegisterBase. 
+  
+  @param  Base    The base address register of UART device.
+  @param  Offset  The offset of the 16550 register to write.
+  @param  Value   The value to write to the 16550 register specified by Offset.
+
+  @return The value written to the 16550 register.
+
+**/
+UINT8
+SerialPortWriteRegister (
+  UINTN  Base,
+  UINTN  Offset,
+  UINT8  Value
+  )
+{
+  if (PcdGetBool (PcdSerialUseMmio)) {
+    return MmioWrite8 (Base + Offset * PcdGet32 (PcdSerialRegisterStride), Value);
+  } else {
+    return IoWrite8 (Base + Offset * PcdGet32 (PcdSerialRegisterStride), Value);
+  }
+}
+
+/**
+  Update the value of an 16-bit PCI configuration register in a PCI device.  If the  
+  PCI Configuration register specified by PciAddress is already programmed with a 
+  non-zero value, then return the current value.  Otherwise update the PCI configuration 
+  register specified by PciAddress with the value specified by Value and return the
+  value programmed into the PCI configuration register.  All values must be masked 
+  using the bitmask specified by Mask.
+
+  @param  PciAddress  PCI Library address of the PCI Configuration register to update.
+  @param  Value       The value to program into the PCI Configuration Register.
+  @param  Mask        Bitmask of the bits to check and update in the PCI configuration register.
+
+**/
+UINT16
+SerialPortLibUpdatePciRegister16 (
+  UINTN   PciAddress,
+  UINT16  Value,
+  UINT16  Mask
+  )
+{
+  UINT16  CurrentValue;
+  
+  CurrentValue = PciRead16 (PciAddress) & Mask;
+  if (CurrentValue != 0) {
+    return CurrentValue;
+  }
+  return PciWrite16 (PciAddress, Value & Mask);
+}
+
+/**
+  Update the value of an 32-bit PCI configuration register in a PCI device.  If the  
+  PCI Configuration register specified by PciAddress is already programmed with a 
+  non-zero value, then return the current value.  Otherwise update the PCI configuration 
+  register specified by PciAddress with the value specified by Value and return the
+  value programmed into the PCI configuration register.  All values must be masked 
+  using the bitmask specified by Mask.
+
+  @param  PciAddress  PCI Library address of the PCI Configuration register to update.
+  @param  Value       The value to program into the PCI Configuration Register.
+  @param  Mask        Bitmask of the bits to check and update in the PCI configuration register.
+
+  @return  The Secondary bus number that is actually programed into the PCI to PCI Bridge device.
+
+**/
+UINT32
+SerialPortLibUpdatePciRegister32 (
+  UINTN   PciAddress,
+  UINT32  Value,
+  UINT32  Mask
+  )
+{
+  UINT32  CurrentValue;
+  
+  CurrentValue = PciRead32 (PciAddress) & Mask;
+  if (CurrentValue != 0) {
+    return CurrentValue;
+  }
+  return PciWrite32 (PciAddress, Value & Mask);
+}
+
+/**
+  Retrieve the I/O or MMIO base address register for the PCI UART device. 
+  
+  This function assumes Root Bus Numer is Zero, and enables I/O and MMIO in PCI UART 
+  Device if they are not already enabled. 
+  
+  @return  The base address register of the UART device.
+
+**/
+UINTN
+GetSerialRegisterBase (
+  VOID
+  )
+{
+  UINTN                 PciLibAddress;
+  UINTN                 BusNumber;
+  UINTN                 SubordinateBusNumber;
+  UINT32                ParentIoBase;
+  UINT32                ParentIoLimit;
+  UINT16                ParentMemoryBase;
+  UINT16                ParentMemoryLimit;
+  UINT32                IoBase;
+  UINT32                IoLimit;
+  UINT16                MemoryBase;
+  UINT16                MemoryLimit;
+  UINTN                 SerialRegisterBase;
+  UINTN                 BarIndex;
+  UINT32                RegisterBaseMask;
+  PCI_UART_DEVICE_INFO  *DeviceInfo;
+
+  //
+  // Get PCI Device Info
+  //
+  DeviceInfo = (PCI_UART_DEVICE_INFO *) PcdGetPtr (PcdSerialPciDeviceInfo);
+  
+  //
+  // If PCI Device Info is empty, then assume fixed address UART and return PcdSerialRegisterBase
+  //  
+  if (DeviceInfo->Device == 0xff) {
+    return (UINTN)PcdGet64 (PcdSerialRegisterBase);
+  }
+
+  //
+  // Assume PCI Bus 0 I/O window is 0-64KB and MMIO windows is 0-4GB
+  //
+  ParentMemoryBase  = 0 >> 16;
+  ParentMemoryLimit = 0xfff00000 >> 16;
+  ParentIoBase      = 0 >> 12;
+  ParentIoLimit     = 0xf000 >> 12;
+  
+  //
+  // Enable I/O and MMIO in PCI Bridge
+  // Assume Root Bus Numer is Zero. 
+  //
+  for (BusNumber = 0; (DeviceInfo + 1)->Device != 0xff; DeviceInfo++) {
+    //
+    // Compute PCI Lib Address to PCI to PCI Bridge
+    //
+    PciLibAddress = PCI_LIB_ADDRESS (BusNumber, DeviceInfo->Device, DeviceInfo->Function, 0);
+    
+    //
+    // Retrieve and verify the bus numbers in the PCI to PCI Bridge
+    //
+    BusNumber            = PciRead8 (PciLibAddress + PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET);
+    SubordinateBusNumber = PciRead8 (PciLibAddress + PCI_BRIDGE_SUBORDINATE_BUS_REGISTER_OFFSET);
+    if (BusNumber == 0 || BusNumber > SubordinateBusNumber) {
+      return 0;
+    }
+
+    //
+    // Retrieve and verify the I/O or MMIO decode window in the PCI to PCI Bridge
+    //
+    if (PcdGetBool (PcdSerialUseMmio)) {
+      MemoryLimit = PciRead16 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.MemoryLimit)) & 0xfff0;
+      MemoryBase  = PciRead16 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.MemoryBase))  & 0xfff0;
+
+      //
+      // If PCI Bridge MMIO window is disabled, then return 0
+      //
+      if (MemoryLimit < MemoryBase) {
+        return 0;
+      }
+      
+      //
+      // If PCI Bridge MMIO window is not in the address range decoded by the parent PCI Bridge, then return 0
+      //  
+      if (MemoryBase < ParentMemoryBase || MemoryBase > ParentMemoryLimit || MemoryLimit > ParentMemoryLimit) {
+        return 0;
+      }
+      ParentMemoryBase  = MemoryBase;
+      ParentMemoryLimit = MemoryLimit;
+    } else {
+      IoLimit = PciRead8 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.IoLimit));
+      if ((IoLimit & PCI_BRIDGE_32_BIT_IO_SPACE ) == 0) {
+        IoLimit = IoLimit >> 4;
+      } else {
+        IoLimit = (PciRead16 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.IoLimitUpper16)) << 4) | (IoLimit >> 4);
+      }
+      IoBase = PciRead8 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.IoBase));
+      if ((IoBase & PCI_BRIDGE_32_BIT_IO_SPACE ) == 0) {
+        IoBase = IoBase >> 4;
+      } else {
+        IoBase = (PciRead16 (PciLibAddress + OFFSET_OF (PCI_TYPE01, Bridge.IoBaseUpper16)) << 4) | (IoBase >> 4);
+      }
+      
+      //
+      // If PCI Bridge I/O window is disabled, then return 0
+      //
+      if (IoLimit < IoBase) {
+        return 0;
+      }
+      
+      //
+      // If PCI Bridge I/O window is not in the address range decoded by the parent PCI Bridge, then return 0
+      //  
+      if (IoBase < ParentIoBase || IoBase > ParentIoLimit || IoLimit > ParentIoLimit) {
+        return 0;
+      }
+      ParentIoBase  = IoBase;
+      ParentIoLimit = IoLimit;
+    }
+  }
+
+  //
+  // Compute PCI Lib Address to PCI UART
+  //
+  PciLibAddress = PCI_LIB_ADDRESS (BusNumber, DeviceInfo->Device, DeviceInfo->Function, 0);
+  
+  //
+  // Find the first IO or MMIO BAR
+  //
+  RegisterBaseMask = 0xFFFFFFF0;
+  for (BarIndex = 0; BarIndex < PCI_MAX_BAR; BarIndex ++) {
+    SerialRegisterBase = PciRead32 (PciLibAddress + PCI_BASE_ADDRESSREG_OFFSET + BarIndex * 4);
+    if (PcdGetBool (PcdSerialUseMmio) && ((SerialRegisterBase & BIT0) == 0)) {
+      //
+      // MMIO BAR is found
+      //
+      RegisterBaseMask = 0xFFFFFFF0;
+      break;
+    }
+
+    if ((!PcdGetBool (PcdSerialUseMmio)) && ((SerialRegisterBase & BIT0) != 0)) {
+      //
+      // IO BAR is found
+      //
+      RegisterBaseMask = 0xFFFFFFF8;
+      break;
+    }
+  }
+
+  //
+  // MMIO or IO BAR is not found.
+  //
+  if (BarIndex == PCI_MAX_BAR) {
+    return 0;
+  }
+
+  //
+  // Program UART BAR
+  //  
+  SerialRegisterBase = SerialPortLibUpdatePciRegister32 (
+                         PciLibAddress + PCI_BASE_ADDRESSREG_OFFSET + BarIndex * 4,
+                         (UINT32)PcdGet64 (PcdSerialRegisterBase), 
+                         RegisterBaseMask
+                         );
+
+  //
+  // Verify that the UART BAR is in the address range decoded by the parent PCI Bridge
+  //  
+  if (PcdGetBool (PcdSerialUseMmio)) {
+    if (((SerialRegisterBase >> 16) & 0xfff0) < ParentMemoryBase || ((SerialRegisterBase >> 16) & 0xfff0) > ParentMemoryLimit) {
+      return 0;
+    }
+  } else {
+    if ((SerialRegisterBase >> 12) < ParentIoBase || (SerialRegisterBase >> 12) > ParentIoLimit) {
+      return 0;
+    }
+  }
+  
+  //
+  // Enable I/O and MMIO in PCI UART Device if they are not already enabled
+  //
+  PciOr16 (
+    PciLibAddress + PCI_COMMAND_OFFSET,
+    PcdGetBool (PcdSerialUseMmio) ? EFI_PCI_COMMAND_MEMORY_SPACE : EFI_PCI_COMMAND_IO_SPACE
+    );
+
+  //
+  // Force D0 state if a Power Management and Status Register is specified
+  //
+  if (DeviceInfo->PowerManagementStatusAndControlRegister != 0x00) {
+    if ((PciRead16 (PciLibAddress + DeviceInfo->PowerManagementStatusAndControlRegister) & (BIT0 | BIT1)) != 0x00) {
+      PciAnd16 (PciLibAddress + DeviceInfo->PowerManagementStatusAndControlRegister, (UINT16)~(BIT0 | BIT1));
+      //
+      // If PCI UART was not in D0, then make sure FIFOs are enabled, but do not reset FIFOs
+      //
+      SerialPortWriteRegister (SerialRegisterBase, R_UART_FCR, (UINT8)(PcdGet8 (PcdSerialFifoControl) & (B_UART_FCR_FIFOE | B_UART_FCR_FIFO64)));
+    }
+  }
+  
+  //
+  // Get PCI Device Info
+  //
+  DeviceInfo = (PCI_UART_DEVICE_INFO *) PcdGetPtr (PcdSerialPciDeviceInfo);
+
+  //
+  // Enable I/O or MMIO in PCI Bridge
+  // Assume Root Bus Numer is Zero. 
+  //
+  for (BusNumber = 0; (DeviceInfo + 1)->Device != 0xff; DeviceInfo++) {
+    //
+    // Compute PCI Lib Address to PCI to PCI Bridge
+    //
+    PciLibAddress = PCI_LIB_ADDRESS (BusNumber, DeviceInfo->Device, DeviceInfo->Function, 0);
+    
+    //
+    // Enable the I/O or MMIO decode windows in the PCI to PCI Bridge
+    //
+    PciOr16 (
+      PciLibAddress + PCI_COMMAND_OFFSET, 
+      PcdGetBool (PcdSerialUseMmio) ? EFI_PCI_COMMAND_MEMORY_SPACE : EFI_PCI_COMMAND_IO_SPACE
+      );
+      
+    //
+    // Force D0 state if a Power Management and Status Register is specified
+    //
+    if (DeviceInfo->PowerManagementStatusAndControlRegister != 0x00) {
+      if ((PciRead16 (PciLibAddress + DeviceInfo->PowerManagementStatusAndControlRegister) & (BIT0 | BIT1)) != 0x00) {
+        PciAnd16 (PciLibAddress + DeviceInfo->PowerManagementStatusAndControlRegister, (UINT16)~(BIT0 | BIT1));
+      }
+    }
+      
+    BusNumber = PciRead8 (PciLibAddress + PCI_BRIDGE_SECONDARY_BUS_REGISTER_OFFSET);
+  }
+  
+  return SerialRegisterBase;
+}
+
+/**
+  Return whether the hardware flow control signal allows writing.
+
+  @param  SerialRegisterBase The base address register of UART device.
+
+  @retval TRUE  The serial port is writable.
+  @retval FALSE The serial port is not writable.
+**/
+BOOLEAN
+SerialPortWritable (
+  UINTN  SerialRegisterBase
+  )
+{
+  if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+    if (PcdGetBool (PcdSerialDetectCable)) {
+      //
+      // Wait for both DSR and CTS to be set
+      //   DSR is set if a cable is connected.
+      //   CTS is set if it is ok to transmit data
+      //
+      //   DSR  CTS  Description                               Action
+      //   ===  ===  ========================================  ========
+      //    0    0   No cable connected.                       Wait
+      //    0    1   No cable connected.                       Wait
+      //    1    0   Cable connected, but not clear to send.   Wait
+      //    1    1   Cable connected, and clear to send.       Transmit
+      //
+      return (BOOLEAN) ((SerialPortReadRegister (SerialRegisterBase, R_UART_MSR) & (B_UART_MSR_DSR | B_UART_MSR_CTS)) == (B_UART_MSR_DSR | B_UART_MSR_CTS));
+    } else {
+      //
+      // Wait for both DSR and CTS to be set OR for DSR to be clear.  
+      //   DSR is set if a cable is connected.
+      //   CTS is set if it is ok to transmit data
+      //
+      //   DSR  CTS  Description                               Action
+      //   ===  ===  ========================================  ========
+      //    0    0   No cable connected.                       Transmit
+      //    0    1   No cable connected.                       Transmit
+      //    1    0   Cable connected, but not clear to send.   Wait
+      //    1    1   Cable connected, and clar to send.        Transmit
+      //
+      return (BOOLEAN) ((SerialPortReadRegister (SerialRegisterBase, R_UART_MSR) & (B_UART_MSR_DSR | B_UART_MSR_CTS)) != (B_UART_MSR_DSR));
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Initialize the serial device hardware.
+  
+  If no initialization is required, then return RETURN_SUCCESS.
+  If the serial device was successfully initialized, then return RETURN_SUCCESS.
+  If the serial device could not be initialized, then return RETURN_DEVICE_ERROR.
+  
+  @retval RETURN_SUCCESS        The serial device was initialized.
+  @retval RETURN_DEVICE_ERROR   The serial device could not be initialized.
+
+**/
+RETURN_STATUS
+EFIAPI
+SerialPortInitialize (
+  VOID
+  )
+{
+  RETURN_STATUS  Status;
+  UINTN          SerialRegisterBase;
+  UINT32         Divisor;
+  UINT32         CurrentDivisor;  
+  BOOLEAN        Initialized;
+
+  //
+  // Perform platform specific initialization required to enable use of the 16550 device
+  // at the location specified by PcdSerialUseMmio and PcdSerialRegisterBase.
+  //
+  Status = PlatformHookSerialPortInitialize ();
+  if (RETURN_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Calculate divisor for baud generator
+  //    Ref_Clk_Rate / Baud_Rate / 16
+  //
+  Divisor = PcdGet32 (PcdSerialClockRate) / (PcdGet32 (PcdSerialBaudRate) * 16);
+  if ((PcdGet32 (PcdSerialClockRate) % (PcdGet32 (PcdSerialBaudRate) * 16)) >= PcdGet32 (PcdSerialBaudRate) * 8) {
+    Divisor++;
+  }
+
+  //
+  // Get the base address of the serial port in either I/O or MMIO space
+  //
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return RETURN_DEVICE_ERROR;
+  }
+
+  //
+  // See if the serial port is already initialized
+  //
+  Initialized = TRUE;
+  if ((SerialPortReadRegister (SerialRegisterBase, R_UART_LCR) & 0x3F) != (PcdGet8 (PcdSerialLineControl) & 0x3F)) {
+    Initialized = FALSE;
+  }
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, (UINT8)(SerialPortReadRegister (SerialRegisterBase, R_UART_LCR) | B_UART_LCR_DLAB));
+  CurrentDivisor =  SerialPortReadRegister (SerialRegisterBase, R_UART_BAUD_HIGH) << 8;
+  CurrentDivisor |= (UINT32) SerialPortReadRegister (SerialRegisterBase, R_UART_BAUD_LOW);
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, (UINT8)(SerialPortReadRegister (SerialRegisterBase, R_UART_LCR) & ~B_UART_LCR_DLAB));
+  if (CurrentDivisor != Divisor) {
+    Initialized = FALSE;
+  }
+  if (Initialized) {
+    return RETURN_SUCCESS;
+  }
+
+  //
+  // Wait for the serial port to be ready.
+  // Verify that both the transmit FIFO and the shift register are empty.
+  //
+  while ((SerialPortReadRegister (SerialRegisterBase, R_UART_LSR) & (B_UART_LSR_TEMT | B_UART_LSR_TXRDY)) != (B_UART_LSR_TEMT | B_UART_LSR_TXRDY));
+  
+  //
+  // Configure baud rate
+  //
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, B_UART_LCR_DLAB);
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_BAUD_HIGH, (UINT8) (Divisor >> 8));
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_BAUD_LOW, (UINT8) (Divisor & 0xff));
+
+  //
+  // Clear DLAB and configure Data Bits, Parity, and Stop Bits.
+  // Strip reserved bits from PcdSerialLineControl
+  //
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, (UINT8)(PcdGet8 (PcdSerialLineControl) & 0x3F));
+
+  //
+  // Enable and reset FIFOs
+  // Strip reserved bits from PcdSerialFifoControl
+  //
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_FCR, 0x00);
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_FCR, (UINT8)(PcdGet8 (PcdSerialFifoControl) & (B_UART_FCR_FIFOE | B_UART_FCR_FIFO64)));
+
+  //
+  // Put Modem Control Register(MCR) into its reset state of 0x00.
+  //  
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, 0x00);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Write data from buffer to serial device. 
+
+  Writes NumberOfBytes data bytes from Buffer to the serial device.  
+  The number of bytes actually written to the serial device is returned.
+  If the return value is less than NumberOfBytes, then the write operation failed.
+
+  If Buffer is NULL, then ASSERT(). 
+
+  If NumberOfBytes is zero, then return 0.
+
+  @param  Buffer           Pointer to the data buffer to be written.
+  @param  NumberOfBytes    Number of bytes to written to the serial device.
+
+  @retval 0                NumberOfBytes is 0.
+  @retval >0               The number of bytes written to the serial device.  
+                           If this value is less than NumberOfBytes, then the write operation failed.
+
+**/
+UINTN
+EFIAPI
+SerialPortWrite (
+  IN UINT8     *Buffer,
+  IN UINTN     NumberOfBytes
+  )
+{
+  UINTN  SerialRegisterBase;
+  UINTN  Result;
+  UINTN  Index;
+  UINTN  FifoSize;
+
+  if (Buffer == NULL) {
+    return 0;
+  }
+
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return 0;
+  }
+  
+  if (NumberOfBytes == 0) {
+    //
+    // Flush the hardware
+    //
+
+    //
+    // Wait for both the transmit FIFO and shift register empty.
+    //
+    while ((SerialPortReadRegister (SerialRegisterBase, R_UART_LSR) & (B_UART_LSR_TEMT | B_UART_LSR_TXRDY)) != (B_UART_LSR_TEMT | B_UART_LSR_TXRDY));
+
+    //
+    // Wait for the hardware flow control signal
+    //
+    while (!SerialPortWritable (SerialRegisterBase));
+    return 0;
+  }
+
+  //
+  // Compute the maximum size of the Tx FIFO
+  //
+  FifoSize = 1;
+  if ((PcdGet8 (PcdSerialFifoControl) & B_UART_FCR_FIFOE) != 0) {
+    if ((PcdGet8 (PcdSerialFifoControl) & B_UART_FCR_FIFO64) == 0) {
+      FifoSize = 16;
+    } else {
+      FifoSize = PcdGet32 (PcdSerialExtendedTxFifoSize);
+    }
+  }
+
+  Result = NumberOfBytes;
+  while (NumberOfBytes != 0) {
+    //
+    // Wait for the serial port to be ready, to make sure both the transmit FIFO
+    // and shift register empty.
+    //
+    while ((SerialPortReadRegister (SerialRegisterBase, R_UART_LSR) & B_UART_LSR_TEMT) == 0);
+
+    //
+    // Fill then entire Tx FIFO
+    //
+    for (Index = 0; Index < FifoSize && NumberOfBytes != 0; Index++, NumberOfBytes--, Buffer++) {
+      //
+      // Wait for the hardware flow control signal
+      //
+      while (!SerialPortWritable (SerialRegisterBase));
+
+      //
+      // Write byte to the transmit buffer.
+      //
+      SerialPortWriteRegister (SerialRegisterBase, R_UART_TXBUF, *Buffer);
+    }
+  }
+  return Result;
+}
+
+/**
+  Reads data from a serial device into a buffer.
+
+  @param  Buffer           Pointer to the data buffer to store the data read from the serial device.
+  @param  NumberOfBytes    Number of bytes to read from the serial device.
+
+  @retval 0                NumberOfBytes is 0.
+  @retval >0               The number of bytes read from the serial device.  
+                           If this value is less than NumberOfBytes, then the read operation failed.
+
+**/
+UINTN
+EFIAPI
+SerialPortRead (
+  OUT UINT8     *Buffer,
+  IN  UINTN     NumberOfBytes
+  )
+{
+  UINTN  SerialRegisterBase;
+  UINTN  Result;
+  UINT8  Mcr;
+
+  if (NULL == Buffer) {
+    return 0;
+  }
+
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return 0;
+  }
+
+  Mcr = (UINT8)(SerialPortReadRegister (SerialRegisterBase, R_UART_MCR) & ~B_UART_MCR_RTS);
+  
+  for (Result = 0; NumberOfBytes-- != 0; Result++, Buffer++) {
+    //
+    // Wait for the serial port to have some data.
+    //
+    while ((SerialPortReadRegister (SerialRegisterBase, R_UART_LSR) & B_UART_LSR_RXRDY) == 0) {
+      if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+        //
+        // Set RTS to let the peer send some data
+        //
+        SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, (UINT8)(Mcr | B_UART_MCR_RTS));
+      }
+    }
+    if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+      //
+      // Clear RTS to prevent peer from sending data
+      //
+      SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, Mcr);
+    }
+    
+    //
+    // Read byte from the receive buffer.
+    //
+    *Buffer = SerialPortReadRegister (SerialRegisterBase, R_UART_RXBUF);
+  }
+  
+  return Result;
+}
+
+
+/**
+  Polls a serial device to see if there is any data waiting to be read.
+
+  Polls aserial device to see if there is any data waiting to be read.
+  If there is data waiting to be read from the serial device, then TRUE is returned.
+  If there is no data waiting to be read from the serial device, then FALSE is returned.
+
+  @retval TRUE             Data is waiting to be read from the serial device.
+  @retval FALSE            There is no data waiting to be read from the serial device.
+
+**/
+BOOLEAN
+EFIAPI
+SerialPortPoll (
+  VOID
+  )
+{
+  UINTN  SerialRegisterBase;
+  
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return FALSE;
+  }
+
+  //
+  // Read the serial port status
+  //
+  if ((SerialPortReadRegister (SerialRegisterBase, R_UART_LSR) & B_UART_LSR_RXRDY) != 0) {
+    if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+      //
+      // Clear RTS to prevent peer from sending data
+      //
+      SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, (UINT8)(SerialPortReadRegister (SerialRegisterBase, R_UART_MCR) & ~B_UART_MCR_RTS));
+    }
+    return TRUE;
+  }    
+  
+  if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+    //
+    // Set RTS to let the peer send some data
+    //
+    SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, (UINT8)(SerialPortReadRegister (SerialRegisterBase, R_UART_MCR) | B_UART_MCR_RTS));
+  }
+  
+  return FALSE;
+}
+
+/**
+  Sets the control bits on a serial device.
+
+  @param Control                Sets the bits of Control that are settable.
+
+  @retval RETURN_SUCCESS        The new control bits were set on the serial device.
+  @retval RETURN_UNSUPPORTED    The serial device does not support this operation.
+  @retval RETURN_DEVICE_ERROR   The serial device is not functioning correctly.
+
+**/
+RETURN_STATUS
+EFIAPI
+SerialPortSetControl (
+  IN UINT32 Control
+  )
+{
+  UINTN SerialRegisterBase;
+  UINT8 Mcr;
+
+  //
+  // First determine the parameter is invalid.
+  //
+  if ((Control & (~(EFI_SERIAL_REQUEST_TO_SEND | EFI_SERIAL_DATA_TERMINAL_READY |
+                    EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE))) != 0) {
+    return RETURN_UNSUPPORTED;
+  }
+
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return RETURN_UNSUPPORTED;
+  }
+
+  //
+  // Read the Modem Control Register.
+  //
+  Mcr = SerialPortReadRegister (SerialRegisterBase, R_UART_MCR);
+  Mcr &= (~(B_UART_MCR_DTRC | B_UART_MCR_RTS));
+
+  if ((Control & EFI_SERIAL_DATA_TERMINAL_READY) == EFI_SERIAL_DATA_TERMINAL_READY) {
+    Mcr |= B_UART_MCR_DTRC;
+  }
+
+  if ((Control & EFI_SERIAL_REQUEST_TO_SEND) == EFI_SERIAL_REQUEST_TO_SEND) {
+    Mcr |= B_UART_MCR_RTS;
+  }
+
+  //
+  // Write the Modem Control Register.
+  //
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_MCR, Mcr);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Retrieve the status of the control bits on a serial device.
+
+  @param Control                A pointer to return the current control signals from the serial device.
+
+  @retval RETURN_SUCCESS        The control bits were read from the serial device.
+  @retval RETURN_UNSUPPORTED    The serial device does not support this operation.
+  @retval RETURN_DEVICE_ERROR   The serial device is not functioning correctly.
+
+**/
+RETURN_STATUS
+EFIAPI
+SerialPortGetControl (
+  OUT UINT32 *Control
+  )
+{
+  UINTN SerialRegisterBase;
+  UINT8 Msr;
+  UINT8 Mcr;
+  UINT8 Lsr;
+
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return RETURN_UNSUPPORTED;
+  }
+
+  *Control = 0;
+
+  //
+  // Read the Modem Status Register.
+  //
+  Msr = SerialPortReadRegister (SerialRegisterBase, R_UART_MSR);
+
+  if ((Msr & B_UART_MSR_CTS) == B_UART_MSR_CTS) {
+    *Control |= EFI_SERIAL_CLEAR_TO_SEND;
+  }
+
+  if ((Msr & B_UART_MSR_DSR) == B_UART_MSR_DSR) {
+    *Control |= EFI_SERIAL_DATA_SET_READY;
+  }
+
+  if ((Msr & B_UART_MSR_RI) == B_UART_MSR_RI) {
+    *Control |= EFI_SERIAL_RING_INDICATE;
+  }
+
+  if ((Msr & B_UART_MSR_DCD) == B_UART_MSR_DCD) {
+    *Control |= EFI_SERIAL_CARRIER_DETECT;
+  }
+
+  //
+  // Read the Modem Control Register.
+  //
+  Mcr = SerialPortReadRegister (SerialRegisterBase, R_UART_MCR);
+
+  if ((Mcr & B_UART_MCR_DTRC) == B_UART_MCR_DTRC) {
+    *Control |= EFI_SERIAL_DATA_TERMINAL_READY;
+  }
+
+  if ((Mcr & B_UART_MCR_RTS) == B_UART_MCR_RTS) {
+    *Control |= EFI_SERIAL_REQUEST_TO_SEND;
+  }
+
+  if (PcdGetBool (PcdSerialUseHardwareFlowControl)) {
+    *Control |= EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE;
+  }
+
+  //
+  // Read the Line Status Register.
+  //
+  Lsr = SerialPortReadRegister (SerialRegisterBase, R_UART_LSR);
+
+  if ((Lsr & (B_UART_LSR_TEMT | B_UART_LSR_TXRDY)) == (B_UART_LSR_TEMT | B_UART_LSR_TXRDY)) {
+    *Control |= EFI_SERIAL_OUTPUT_BUFFER_EMPTY;
+  }
+
+  if ((Lsr & B_UART_LSR_RXRDY) == 0) {
+    *Control |= EFI_SERIAL_INPUT_BUFFER_EMPTY;
+  }
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Sets the baud rate, receive FIFO depth, transmit/receice time out, parity,
+  data bits, and stop bits on a serial device.
+
+  @param BaudRate           The requested baud rate. A BaudRate value of 0 will use the
+                            device's default interface speed.
+                            On output, the value actually set.
+  @param ReveiveFifoDepth   The requested depth of the FIFO on the receive side of the
+                            serial interface. A ReceiveFifoDepth value of 0 will use
+                            the device's default FIFO depth.
+                            On output, the value actually set.
+  @param Timeout            The requested time out for a single character in microseconds.
+                            This timeout applies to both the transmit and receive side of the
+                            interface. A Timeout value of 0 will use the device's default time
+                            out value.
+                            On output, the value actually set.
+  @param Parity             The type of parity to use on this serial device. A Parity value of
+                            DefaultParity will use the device's default parity value.
+                            On output, the value actually set.
+  @param DataBits           The number of data bits to use on the serial device. A DataBits
+                            vaule of 0 will use the device's default data bit setting.
+                            On output, the value actually set.
+  @param StopBits           The number of stop bits to use on this serial device. A StopBits
+                            value of DefaultStopBits will use the device's default number of
+                            stop bits.
+                            On output, the value actually set.
+
+  @retval RETURN_SUCCESS            The new attributes were set on the serial device.
+  @retval RETURN_UNSUPPORTED        The serial device does not support this operation.
+  @retval RETURN_INVALID_PARAMETER  One or more of the attributes has an unsupported value.
+  @retval RETURN_DEVICE_ERROR       The serial device is not functioning correctly.
+
+**/
+RETURN_STATUS
+EFIAPI
+SerialPortSetAttributes (
+  IN OUT UINT64             *BaudRate,
+  IN OUT UINT32             *ReceiveFifoDepth,
+  IN OUT UINT32             *Timeout,
+  IN OUT EFI_PARITY_TYPE    *Parity,
+  IN OUT UINT8              *DataBits,
+  IN OUT EFI_STOP_BITS_TYPE *StopBits
+  )
+{
+  UINTN     SerialRegisterBase;
+  UINT32    SerialBaudRate;
+  UINTN     Divisor;
+  UINT8     Lcr;
+  UINT8     LcrData;
+  UINT8     LcrParity;
+  UINT8     LcrStop;
+
+  SerialRegisterBase = GetSerialRegisterBase ();
+  if (SerialRegisterBase ==0) {
+    return RETURN_UNSUPPORTED;
+  }
+
+  //
+  // Check for default settings and fill in actual values.
+  //
+  if (*BaudRate == 0) {
+    *BaudRate = PcdGet32 (PcdSerialBaudRate);
+  }
+  SerialBaudRate = (UINT32) *BaudRate;
+
+  if (*DataBits == 0) {
+    LcrData = (UINT8) (PcdGet8 (PcdSerialLineControl) & 0x3);
+    *DataBits = LcrData + 5;
+  } else {
+    if ((*DataBits < 5) || (*DataBits > 8)) {
+      return RETURN_INVALID_PARAMETER;
+    }
+    //
+    // Map 5..8 to 0..3
+    //
+    LcrData = (UINT8) (*DataBits - (UINT8) 5);
+  }
+
+  if (*Parity == DefaultParity) {
+    LcrParity = (UINT8) ((PcdGet8 (PcdSerialLineControl) >> 3) & 0x7);
+    switch (LcrParity) {
+      case 0:
+        *Parity = NoParity;
+        break;
+
+      case 3:
+        *Parity = EvenParity;
+        break;
+
+      case 1:
+        *Parity = OddParity;
+        break;
+
+      case 7:
+        *Parity = SpaceParity;
+        break;
+
+      case 5:
+        *Parity = MarkParity;
+        break;
+
+      default:
+        break;
+    }
+  } else {
+    if ((*Parity < NoParity) || (*Parity > SpaceParity)) {
+      return RETURN_INVALID_PARAMETER;
+    }
+    switch (*Parity) {
+      case NoParity:
+        LcrParity = 0;
+        break;
+
+      case EvenParity:
+        LcrParity = 3;
+        break;
+
+      case OddParity:
+        LcrParity = 1;
+        break;
+
+      case SpaceParity:
+        LcrParity = 7;
+        break;
+
+      case MarkParity:
+        LcrParity = 5;
+        break;
+
+      default:
+        break;
+    }
+  }
+
+  if (*StopBits == DefaultStopBits) {
+    LcrStop = (UINT8) ((PcdGet8 (PcdSerialLineControl) >> 2) & 0x1);
+    switch (LcrStop) {
+      case 0:
+        *StopBits = OneStopBit;
+        break;
+
+      case 1:
+        if (*DataBits == 5) {
+          *StopBits = OneFiveStopBits;
+        } else {
+          *StopBits = TwoStopBits;
+        }
+        break;
+
+      default:
+        break;
+    }
+  } else {
+    if ((*StopBits < OneStopBit) || (*StopBits > TwoStopBits)) {
+      return RETURN_INVALID_PARAMETER;
+    }
+    switch (*StopBits) {
+      case OneStopBit:
+        LcrStop = 0;
+        break;
+
+      case OneFiveStopBits:
+      case TwoStopBits:
+        LcrStop = 1;
+        break;
+
+      default:
+        break;
+    }
+  }
+
+  //
+  // Calculate divisor for baud generator
+  //    Ref_Clk_Rate / Baud_Rate / 16
+  //
+  Divisor = PcdGet32 (PcdSerialClockRate) / (SerialBaudRate * 16);
+  if ((PcdGet32 (PcdSerialClockRate) % (SerialBaudRate * 16)) >= SerialBaudRate * 8) {
+    Divisor++;
+  }
+
+  //
+  // Configure baud rate
+  //
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, B_UART_LCR_DLAB);
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_BAUD_HIGH, (UINT8) (Divisor >> 8));
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_BAUD_LOW, (UINT8) (Divisor & 0xff));
+
+  //
+  // Clear DLAB and configure Data Bits, Parity, and Stop Bits.
+  // Strip reserved bits from line control value
+  //
+  Lcr = (UINT8) ((LcrParity << 3) | (LcrStop << 2) | LcrData);
+  SerialPortWriteRegister (SerialRegisterBase, R_UART_LCR, (UINT8) (Lcr & 0x3F));
+
+  return RETURN_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.inf b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.inf
new file mode 100644
index 0000000000..bb42f89676
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.inf
@@ -0,0 +1,48 @@
+## @file
+#  SerialPortLib instance for 16550 UART.
+#
+#  Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BaseSerialPortLib16550
+  MODULE_UNI_FILE                = BaseSerialPortLib16550.uni
+  FILE_GUID                      = 9E7C00CF-355A-4d4e-BF60-0428CFF95540
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.1
+  LIBRARY_CLASS                  = SerialPortLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  IoLib
+  PlatformHookLib
+  PciLib
+
+[Sources]
+  BaseSerialPortLib16550.c
+  
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseMmio                 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseHardwareFlowControl  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialDetectCable             ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialRegisterBase            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialBaudRate                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialLineControl             ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialFifoControl             ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialClockRate               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialPciDeviceInfo           ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialExtendedTxFifoSize      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialRegisterStride          ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.uni b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.uni
new file mode 100644
index 0000000000..641a86efc7
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.uni
diff --git a/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.c b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.c
new file mode 100644
index 0000000000..ab8a60585e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.c
@@ -0,0 +1,238 @@
+/** @file
+  Library used for sorting routines.
+
+  Copyright (c) 2009 - 2014, 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 <Uefi.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/SortLib.h>
+
+/**
+  Worker function for QuickSorting.  This function is identical to PerformQuickSort,
+  except that is uses the pre-allocated buffer so the in place sorting does not need to
+  allocate and free buffers constantly.
+
+  Each element must be equal sized.
+
+  if BufferToSort is NULL, then ASSERT.
+  if CompareFunction is NULL, then ASSERT.
+  if Buffer is NULL, then ASSERT.
+
+  if Count is < 2 then perform no action.
+  if Size is < 1 then perform no action.
+
+  @param[in, out] BufferToSort   on call a Buffer of (possibly sorted) elements
+                                 on return a buffer of sorted elements
+  @param[in] Count               the number of elements in the buffer to sort
+  @param[in] ElementSize         Size of an element in bytes
+  @param[in] CompareFunction     The function to call to perform the comparison
+                                 of any 2 elements
+  @param[in] Buffer              Buffer of size ElementSize for use in swapping
+**/
+VOID
+EFIAPI
+QuickSortWorker (
+  IN OUT VOID                           *BufferToSort,
+  IN CONST UINTN                        Count,
+  IN CONST UINTN                        ElementSize,
+  IN       SORT_COMPARE                 CompareFunction,
+  IN VOID                               *Buffer
+  )
+{
+  VOID        *Pivot;
+  UINTN       LoopCount;
+  UINTN       NextSwapLocation;
+
+  ASSERT(BufferToSort     != NULL);
+  ASSERT(CompareFunction  != NULL);
+  ASSERT(Buffer  != NULL);
+
+  if ( Count < 2
+    || ElementSize  < 1
+   ){
+    return;
+  }
+
+  NextSwapLocation = 0;
+
+  //
+  // pick a pivot (we choose last element)
+  //
+  Pivot = ((UINT8*)BufferToSort+((Count-1)*ElementSize));
+
+  //
+  // Now get the pivot such that all on "left" are below it
+  // and everything "right" are above it
+  //
+  for ( LoopCount = 0
+      ; LoopCount < Count -1
+      ; LoopCount++
+     ){
+    //
+    // if the element is less than the pivot
+    //
+    if (CompareFunction((VOID*)((UINT8*)BufferToSort+((LoopCount)*ElementSize)),Pivot) <= 0){
+      //
+      // swap
+      //
+      CopyMem (Buffer, (UINT8*)BufferToSort+(NextSwapLocation*ElementSize), ElementSize);
+      CopyMem ((UINT8*)BufferToSort+(NextSwapLocation*ElementSize), (UINT8*)BufferToSort+((LoopCount)*ElementSize), ElementSize);
+      CopyMem ((UINT8*)BufferToSort+((LoopCount)*ElementSize), Buffer, ElementSize);
+
+      //
+      // increment NextSwapLocation
+      //
+      NextSwapLocation++;
+    }
+  }
+  //
+  // swap pivot to it's final position (NextSwapLocaiton)
+  //
+  CopyMem (Buffer, Pivot, ElementSize);
+  CopyMem (Pivot, (UINT8*)BufferToSort+(NextSwapLocation*ElementSize), ElementSize);
+  CopyMem ((UINT8*)BufferToSort+(NextSwapLocation*ElementSize), Buffer, ElementSize);
+
+  //
+  // Now recurse on 2 paritial lists.  neither of these will have the 'pivot' element
+  // IE list is sorted left half, pivot element, sorted right half...
+  //
+  if (NextSwapLocation >= 2) {
+    QuickSortWorker(
+      BufferToSort,
+      NextSwapLocation,
+      ElementSize,
+      CompareFunction,
+      Buffer);
+  }
+
+  if ((Count - NextSwapLocation - 1) >= 2) {
+    QuickSortWorker(
+      (UINT8 *)BufferToSort + (NextSwapLocation+1) * ElementSize,
+      Count - NextSwapLocation - 1,
+      ElementSize,
+      CompareFunction,
+      Buffer);
+  }
+  return;
+}
+/**
+  Function to perform a Quick Sort alogrithm on a buffer of comparable elements.
+
+  Each element must be equal sized.
+
+  if BufferToSort is NULL, then ASSERT.
+  if CompareFunction is NULL, then ASSERT.
+
+  if Count is < 2 then perform no action.
+  if Size is < 1 then perform no action.
+
+  @param[in, out] BufferToSort   on call a Buffer of (possibly sorted) elements
+                                 on return a buffer of sorted elements
+  @param[in] Count               the number of elements in the buffer to sort
+  @param[in] ElementSize         Size of an element in bytes
+  @param[in] CompareFunction     The function to call to perform the comparison
+                                 of any 2 elements
+**/
+VOID
+EFIAPI
+PerformQuickSort (
+  IN OUT VOID                           *BufferToSort,
+  IN CONST UINTN                        Count,
+  IN CONST UINTN                        ElementSize,
+  IN       SORT_COMPARE                 CompareFunction
+  )
+{
+  VOID  *Buffer;
+
+  ASSERT(BufferToSort     != NULL);
+  ASSERT(CompareFunction  != NULL);
+
+  Buffer = AllocateZeroPool(ElementSize);
+  ASSERT(Buffer != NULL);
+
+  QuickSortWorker(
+    BufferToSort,
+    Count,
+    ElementSize,
+    CompareFunction,
+    Buffer);
+
+  FreePool(Buffer);
+  return;
+}
+
+/**
+  Not supported in Base version.
+
+  @param[in] Buffer1  Ignored.
+  @param[in] Buffer2  Ignored.
+
+  ASSERT and return 0.
+**/
+INTN
+EFIAPI
+DevicePathCompare (
+  IN  CONST VOID             *Buffer1,
+  IN  CONST VOID             *Buffer2
+  )
+{
+  ASSERT(FALSE);
+  return 0;
+}
+
+/**
+  Function to compare 2 strings without regard to case of the characters.
+
+  @param[in] Buffer1            Pointer to String to compare.
+  @param[in] Buffer2            Pointer to second String to compare.
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @return < 0                   Buffer1 is less than Buffer2.
+  @return > 0                   Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringNoCaseCompare (
+  IN  CONST VOID             *Buffer1,
+  IN  CONST VOID             *Buffer2
+  )
+{
+  ASSERT(FALSE);
+  return 0;
+}
+
+
+/**
+  Function to compare 2 strings.
+
+  @param[in] Buffer1            Pointer to String to compare (CHAR16**).
+  @param[in] Buffer2            Pointer to second String to compare (CHAR16**).
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @return < 0                   Buffer1 is less than Buffer2.
+  @return > 0                   Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringCompare (
+  IN  CONST VOID                *Buffer1,
+  IN  CONST VOID                *Buffer2
+  )
+{
+  ASSERT(FALSE);
+  return 0;
+}
+
+
diff --git a/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.inf b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.inf
new file mode 100644
index 0000000000..4b493f4eb6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.inf
@@ -0,0 +1,40 @@
+##  @file
+#   Library used for sorting routines.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010006
+  BASE_NAME                      = BaseSortLib
+  MODULE_UNI_FILE                = BaseSortLib.uni
+  FILE_GUID                      = 03F3331B-F12D-494f-BF37-E55A657F2497
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SortLib|DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  BaseSortLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  BaseMemoryLib
+  DebugLib
diff --git a/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.uni b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.uni
new file mode 100644
index 0000000000..2163301c42
--- /dev/null
+++ b/Core/MdeModulePkg/Library/BaseSortLib/BaseSortLib.uni
diff --git a/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.c b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.c
new file mode 100644
index 0000000000..2fea24ac31
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.c
@@ -0,0 +1,99 @@
+/** @file
+  CPU Exception Handler library implementition with empty functions.
+
+  Copyright (c) 2012 - 2013, 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 <PiPei.h>
+#include <Library/CpuExceptionHandlerLib.h>
+
+/**
+  Initializes all CPU exceptions entries and provides the default exception handlers.
+  
+  Caller should try to get an array of interrupt and/or exception vectors that are in use and need to
+  persist by EFI_VECTOR_HANDOFF_INFO defined in PI 1.3 specification.
+  If caller cannot get reserved vector list or it does not exists, set VectorInfo to NULL. 
+  If VectorInfo is not NULL, the exception vectors will be initialized per vector attribute accordingly.
+
+  @param[in]  VectorInfo    Pointer to reserved vector list.
+  
+  @retval EFI_SUCCESS           CPU Exception Entries have been successfully initialized 
+                                with default exception handlers.
+  @retval EFI_INVALID_PARAMETER VectorInfo includes the invalid content if VectorInfo is not NULL.
+  @retval EFI_UNSUPPORTED       This function is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuExceptionHandlers (
+  IN EFI_VECTOR_HANDOFF_INFO       *VectorInfo OPTIONAL
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Initializes all CPU interrupt/exceptions entries and provides the default interrupt/exception handlers.
+  
+  Caller should try to get an array of interrupt and/or exception vectors that are in use and need to
+  persist by EFI_VECTOR_HANDOFF_INFO defined in PI 1.3 specification.
+  If caller cannot get reserved vector list or it does not exists, set VectorInfo to NULL. 
+  If VectorInfo is not NULL, the exception vectors will be initialized per vector attribute accordingly.
+
+  @param[in]  VectorInfo    Pointer to reserved vector list.
+  
+  @retval EFI_SUCCESS           All CPU interrupt/exception entries have been successfully initialized 
+                                with default interrupt/exception handlers.
+  @retval EFI_INVALID_PARAMETER VectorInfo includes the invalid content if VectorInfo is not NULL.
+  @retval EFI_UNSUPPORTED       This function is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeCpuInterruptHandlers (
+  IN EFI_VECTOR_HANDOFF_INFO       *VectorInfo OPTIONAL
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Registers a function to be called from the processor interrupt handler.
+
+  This function registers and enables the handler specified by InterruptHandler for a processor 
+  interrupt or exception type specified by InterruptType. If InterruptHandler is NULL, then the 
+  handler for the processor interrupt or exception type specified by InterruptType is uninstalled. 
+  The installed handler is called once for each processor interrupt or exception.
+  NOTE: This function should be invoked after InitializeCpuExceptionHandlers() or
+  InitializeCpuInterruptHandlers() invoked, otherwise EFI_UNSUPPORTED returned.
+
+  @param[in]  InterruptType     Defines which interrupt or exception to hook.
+  @param[in]  InterruptHandler  A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER that is called
+                                when a processor interrupt occurs. If this parameter is NULL, then the handler
+                                will be uninstalled.
+
+  @retval EFI_SUCCESS           The handler for the processor interrupt was successfully installed or uninstalled.
+  @retval EFI_ALREADY_STARTED   InterruptHandler is not NULL, and a handler for InterruptType was
+                                previously installed.
+  @retval EFI_INVALID_PARAMETER InterruptHandler is NULL, and a handler for InterruptType was not
+                                previously installed.
+  @retval EFI_UNSUPPORTED       The interrupt specified by InterruptType is not supported,
+                                or this function is not supported.
+**/
+EFI_STATUS
+EFIAPI
+RegisterCpuInterruptHandler (
+  IN EFI_EXCEPTION_TYPE            InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER     InterruptHandler
+  )
+{
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.inf b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.inf
new file mode 100644
index 0000000000..c79c5a76ee
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.inf
@@ -0,0 +1,36 @@
+## @file
+# Null instance of CPU Exception Handler Library with empty functions.
+#
+#  Copyright (c) 2012 - 2014, 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.
+#
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CpuExceptionHandlerLibNull
+  MODULE_UNI_FILE                = CpuExceptionHandlerLibNull.uni
+  FILE_GUID                      = 3175E6B9-4B01-496a-9A2B-64AF02D87E34
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = CpuExceptionHandlerLib
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  CpuExceptionHandlerLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.uni b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.uni
new file mode 100644
index 0000000000..9dfc23603f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.uni
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/Colors.h b/Core/MdeModulePkg/Library/CustomizedDisplayLib/Colors.h
new file mode 100644
index 0000000000..2db8b99614
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/Colors.h
@@ -0,0 +1,44 @@
+/** @file
+MACRO definitions for color used in Setup Browser.
+
+Copyright (c) 2004 - 2011, 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.
+
+**/
+//
+// Unicode collation protocol in
+
+#ifndef _COLORS_H_
+#define _COLORS_H_
+
+//
+// Screen Color Settings
+//
+#define PICKLIST_HIGHLIGHT_TEXT       EFI_WHITE
+#define PICKLIST_HIGHLIGHT_BACKGROUND EFI_BACKGROUND_CYAN
+#define TITLE_TEXT                    EFI_WHITE
+#define TITLE_BACKGROUND              EFI_BACKGROUND_BLUE
+#define KEYHELP_TEXT                  EFI_LIGHTGRAY
+#define KEYHELP_BACKGROUND            EFI_BACKGROUND_BLACK
+#define SUBTITLE_BACKGROUND           EFI_BACKGROUND_LIGHTGRAY
+#define BANNER_TEXT                   EFI_BLUE
+#define BANNER_BACKGROUND             EFI_BACKGROUND_LIGHTGRAY
+#define FIELD_TEXT_GRAYED             EFI_DARKGRAY
+#define FIELD_BACKGROUND              EFI_BACKGROUND_LIGHTGRAY
+#define POPUP_TEXT                    EFI_LIGHTGRAY
+#define POPUP_BACKGROUND              EFI_BACKGROUND_BLUE
+#define POPUP_INVERSE_TEXT            EFI_LIGHTGRAY
+#define POPUP_INVERSE_BACKGROUND      EFI_BACKGROUND_BLACK
+#define HELP_TEXT                     EFI_BLUE
+#define ERROR_TEXT                    EFI_RED | EFI_BRIGHT
+#define INFO_TEXT                     EFI_YELLOW | EFI_BRIGHT
+#define ARROW_TEXT                    EFI_RED | EFI_BRIGHT
+#define ARROW_BACKGROUND              EFI_BACKGROUND_LIGHTGRAY
+
+#endif
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.c b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.c
new file mode 100644
index 0000000000..622410359c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.c
@@ -0,0 +1,958 @@
+/** @file
+
+  This library class defines a set of interfaces to customize Display module
+
+Copyright (c) 2013 - 2014, 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 that 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 "CustomizedDisplayLibInternal.h"
+
+EFI_GUID          gCustomizedDisplayLibGuid = { 0x99fdc8fd, 0x849b, 0x4eba, { 0xad, 0x13, 0xfb, 0x96, 0x99, 0xc9, 0xa, 0x4d } };
+
+EFI_HII_HANDLE    mCDLStringPackHandle;
+UINT16            gClassOfVfr;                 // Formset class information
+BOOLEAN           gLibIsFirstForm = TRUE;
+BANNER_DATA       *gBannerData;
+
+UINTN             gFooterHeight;
+
+/**
++------------------------------------------------------------------------------+
+|                                 Setup Page                                   |
++------------------------------------------------------------------------------+
+
+Statement
+Statement
+Statement
+
+
+
+
+
++------------------------------------------------------------------------------+
+|                                F9=Reset to Defaults        F10=Save          |
+| ^"=Move Highlight          <Spacebar> Toggles Checkbox     Esc=Exit          |
++------------------------------------------------------------------------------+
+  StatusBar
+**/
+
+/**
+  This funtion defines Page Frame and Backgroud. 
+  
+  Based on the above layout, it will be responsible for HeaderHeight, FooterHeight, 
+  StatusBarHeight and Backgroud. And, it will reserve Screen for Statement. 
+
+  @param[in]  FormData             Form Data to be shown in Page.
+  @param[out] ScreenForStatement   Screen to be used for Statement. (Prompt, Value and Help)
+  
+  @return Status
+**/
+EFI_STATUS
+EFIAPI
+DisplayPageFrame (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData,
+  OUT EFI_SCREEN_DESCRIPTOR         *ScreenForStatement
+  )
+{
+  EFI_STATUS             Status;
+
+  ASSERT (FormData != NULL && ScreenForStatement != NULL);
+  if (FormData == NULL || ScreenForStatement == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ScreenDiemensionInfoValidate (FormData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gClassOfVfr = FORMSET_CLASS_PLATFORM_SETUP;
+
+  ProcessExternedOpcode(FormData);
+
+  //
+  // Calculate the ScreenForStatement.
+  //
+  ScreenForStatement->BottomRow   = gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - gFooterHeight;
+  if (gClassOfVfr == FORMSET_CLASS_FRONT_PAGE) {
+    ScreenForStatement->TopRow    = gScreenDimensions.TopRow + FRONT_PAGE_HEADER_HEIGHT;
+  } else {
+    ScreenForStatement->TopRow    = gScreenDimensions.TopRow + NONE_FRONT_PAGE_HEADER_HEIGHT;
+  }
+  ScreenForStatement->LeftColumn  = gScreenDimensions.LeftColumn;
+  ScreenForStatement->RightColumn = gScreenDimensions.RightColumn;
+
+  if ((gLibIsFirstForm) || ((FormData->Attribute & HII_DISPLAY_MODAL) != 0)) {
+    //
+    // Ensure we are in Text mode
+    //
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+    ClearLines (0, gScreenDimensions.RightColumn, 0, gScreenDimensions.BottomRow, KEYHELP_BACKGROUND);
+    gLibIsFirstForm = FALSE;
+  }
+
+  //
+  // Don't print frame for modal form.
+  //
+  if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (gClassOfVfr == FORMSET_CLASS_FRONT_PAGE) {
+    PrintBannerInfo (FormData);
+  }
+
+  PrintFramework (FormData);
+
+  UpdateStatusBar(NV_UPDATE_REQUIRED, FormData->SettingChangedFlag);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function updates customized key panel's help information.
+  The library will prepare those Strings for the basic key, ESC, Enter, Up/Down/Left/Right, +/-.
+  and arrange them in Footer panel.
+  
+  @param[in]  FormData       Form Data to be shown in Page. FormData has the highlighted statement. 
+  @param[in]  Statement      The statement current selected.
+  @param[in]  Selected       Whether or not a tag be selected. TRUE means Enter has hit this question.
+**/
+VOID
+EFIAPI
+RefreshKeyHelp (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN  BOOLEAN                      Selected
+  )
+{
+  UINTN                  SecCol;
+  UINTN                  ThdCol;
+  UINTN                  RightColumnOfHelp;
+  UINTN                  TopRowOfHelp;
+  UINTN                  BottomRowOfHelp;
+  UINTN                  StartColumnOfHelp;
+  EFI_IFR_NUMERIC        *NumericOp;
+  EFI_IFR_DATE           *DateOp;
+  EFI_IFR_TIME           *TimeOp;
+  BOOLEAN                HexDisplay;
+  UINTN                  ColumnWidth1;
+  UINTN                  ColumnWidth2;
+  UINTN                  ColumnWidth3;
+  CHAR16                 *ColumnStr1;
+  CHAR16                 *ColumnStr2;
+  CHAR16                 *ColumnStr3;
+
+  ASSERT (FormData != NULL);
+  if (FormData == NULL) {
+    return;
+  }
+
+  gST->ConOut->SetAttribute (gST->ConOut, KEYHELP_TEXT | KEYHELP_BACKGROUND);
+
+  if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+    return;
+  }
+
+  SecCol            = gScreenDimensions.LeftColumn + (gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn) / 3;
+  ThdCol            = gScreenDimensions.LeftColumn + (gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn) / 3 * 2;
+
+  //
+  // + 2 means leave 1 space before the first hotkey info.
+  //
+  StartColumnOfHelp = gScreenDimensions.LeftColumn + 2;
+  RightColumnOfHelp = gScreenDimensions.RightColumn - 1;
+  TopRowOfHelp      = gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - gFooterHeight + 1;
+  BottomRowOfHelp   = gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - 2;
+
+  ColumnWidth1      = SecCol - StartColumnOfHelp;
+  ColumnWidth2      = ThdCol - SecCol;
+  ColumnWidth3      = RightColumnOfHelp - ThdCol;
+  ColumnStr1        = gLibEmptyString;
+  ColumnStr2        = gLibEmptyString;
+  ColumnStr3        = gLibEmptyString;
+
+  //
+  // Clean the space at gScreenDimensions.LeftColumn + 1.
+  //
+  PrintStringAtWithWidth (StartColumnOfHelp - 1, BottomRowOfHelp, gLibEmptyString, 1);
+  PrintStringAtWithWidth (StartColumnOfHelp - 1, TopRowOfHelp, gLibEmptyString, 1);
+
+  if (Statement == NULL) {
+    //
+    // Print Key for Form without showable statement.
+    //
+    PrintHotKeyHelpString (FormData, TRUE);
+    PrintStringAtWithWidth (StartColumnOfHelp, BottomRowOfHelp, gLibEmptyString, ColumnWidth1);
+    PrintStringAtWithWidth (SecCol, BottomRowOfHelp, gLibEmptyString, ColumnWidth2);
+    PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, gLibEmptyString, ColumnWidth1);
+    if (gClassOfVfr == FORMSET_CLASS_PLATFORM_SETUP) {
+      ColumnStr3 = gEscapeString;
+    }
+    PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, ColumnStr3, ColumnWidth3);
+
+    return;
+  }
+
+  HexDisplay = FALSE;
+  NumericOp = NULL;
+  DateOp    = NULL;
+  TimeOp    = NULL;
+  if (Statement->OpCode->OpCode == EFI_IFR_NUMERIC_OP) {
+    NumericOp = (EFI_IFR_NUMERIC *) Statement->OpCode;
+    HexDisplay = (NumericOp->Flags & EFI_IFR_DISPLAY_UINT_HEX) == EFI_IFR_DISPLAY_UINT_HEX;
+  } else if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP) {
+    DateOp   = (EFI_IFR_DATE *) Statement->OpCode;
+    HexDisplay = (DateOp->Flags & EFI_IFR_DISPLAY_UINT_HEX) == EFI_IFR_DISPLAY_UINT_HEX;
+  } else if (Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+    TimeOp  = (EFI_IFR_TIME *) Statement->OpCode;
+    HexDisplay = (TimeOp->Flags & EFI_IFR_DISPLAY_UINT_HEX) == EFI_IFR_DISPLAY_UINT_HEX;
+  }  
+  switch (Statement->OpCode->OpCode) {
+  case EFI_IFR_ORDERED_LIST_OP:
+  case EFI_IFR_ONE_OF_OP:
+  case EFI_IFR_NUMERIC_OP:
+  case EFI_IFR_TIME_OP:
+  case EFI_IFR_DATE_OP:
+    if (!Selected) {
+      PrintHotKeyHelpString (FormData, TRUE);
+
+      if (gClassOfVfr == FORMSET_CLASS_PLATFORM_SETUP) {
+        ColumnStr3 = gEscapeString;
+      }
+      PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, ColumnStr3, ColumnWidth3);
+
+      if ((Statement->OpCode->OpCode == EFI_IFR_DATE_OP) ||
+          (Statement->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+        PrintAt (
+          ColumnWidth1, 
+          StartColumnOfHelp,
+          BottomRowOfHelp,
+          L"%c%c%c%c%s",
+          ARROW_UP,
+          ARROW_DOWN,
+          ARROW_RIGHT,
+          ARROW_LEFT,
+          gMoveHighlight
+          );
+        PrintStringAtWithWidth (SecCol, BottomRowOfHelp, gEnterString, ColumnWidth2);
+        PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, gAdjustNumber, ColumnWidth1);
+      } else {
+        PrintAt (ColumnWidth1, StartColumnOfHelp, BottomRowOfHelp, L"%c%c%s", ARROW_UP, ARROW_DOWN, gMoveHighlight);
+        if (Statement->OpCode->OpCode == EFI_IFR_NUMERIC_OP && NumericOp != NULL && LibGetFieldFromNum(Statement->OpCode) != 0) {
+          ColumnStr1 = gAdjustNumber;
+        }
+        PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, ColumnStr1, ColumnWidth1);
+        PrintStringAtWithWidth (SecCol, BottomRowOfHelp, gEnterString, ColumnWidth2);
+      }
+    } else {
+      PrintHotKeyHelpString (FormData, FALSE);
+      PrintStringAtWithWidth (SecCol, BottomRowOfHelp, gEnterCommitString, ColumnWidth2);
+
+      //
+      // If it is a selected numeric with manual input, display different message
+      //
+      if ((Statement->OpCode->OpCode == EFI_IFR_NUMERIC_OP) || 
+          (Statement->OpCode->OpCode == EFI_IFR_DATE_OP) ||
+          (Statement->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+        ColumnStr2 = HexDisplay ? gHexNumericInput : gDecNumericInput;
+        PrintStringAtWithWidth (StartColumnOfHelp, BottomRowOfHelp, gLibEmptyString, ColumnWidth1);
+      } else {
+        PrintAt (ColumnWidth1, StartColumnOfHelp, BottomRowOfHelp, L"%c%c%s", ARROW_UP, ARROW_DOWN, gMoveHighlight);
+      }
+
+      if (Statement->OpCode->OpCode == EFI_IFR_ORDERED_LIST_OP) {
+        ColumnStr1 = gPlusString;
+        ColumnStr3 = gMinusString;
+      }
+      PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, ColumnStr1, ColumnWidth1);
+      PrintStringAtWithWidth (ThdCol, TopRowOfHelp, ColumnStr3, ColumnWidth3);
+      PrintStringAtWithWidth (SecCol, TopRowOfHelp, ColumnStr2, ColumnWidth2);
+
+      PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, gEnterEscapeString, ColumnWidth3);
+    }
+    break;
+
+  case EFI_IFR_CHECKBOX_OP:
+    PrintHotKeyHelpString (FormData, TRUE);
+
+    if (gClassOfVfr == FORMSET_CLASS_PLATFORM_SETUP) {
+      ColumnStr3 = gEscapeString;
+    }
+    PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, ColumnStr3, ColumnWidth3);
+
+    PrintAt (ColumnWidth1, StartColumnOfHelp, BottomRowOfHelp, L"%c%c%s", ARROW_UP, ARROW_DOWN, gMoveHighlight);
+    PrintStringAtWithWidth (SecCol, BottomRowOfHelp, gToggleCheckBox, ColumnWidth2);
+    PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, gLibEmptyString, ColumnWidth1);
+    break;
+
+  case EFI_IFR_REF_OP:
+  case EFI_IFR_PASSWORD_OP:
+  case EFI_IFR_STRING_OP:
+  case EFI_IFR_TEXT_OP:
+  case EFI_IFR_ACTION_OP:
+  case EFI_IFR_RESET_BUTTON_OP:
+  case EFI_IFR_SUBTITLE_OP:
+     if (!Selected) {
+      PrintHotKeyHelpString (FormData, TRUE);
+
+      if (gClassOfVfr == FORMSET_CLASS_PLATFORM_SETUP) {
+        ColumnStr3 = gEscapeString;
+      }
+      PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, ColumnStr3, ColumnWidth3);
+
+      PrintAt (ColumnWidth1, StartColumnOfHelp, BottomRowOfHelp, L"%c%c%s", ARROW_UP, ARROW_DOWN, gMoveHighlight);
+      if (Statement->OpCode->OpCode != EFI_IFR_TEXT_OP && Statement->OpCode->OpCode != EFI_IFR_SUBTITLE_OP) {
+        ColumnStr2 = gEnterString;
+      }
+      PrintStringAtWithWidth (SecCol, BottomRowOfHelp, ColumnStr2, ColumnWidth2);
+      PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, ColumnStr1, ColumnWidth1);
+    } else {
+      PrintHotKeyHelpString (FormData, FALSE);
+      if (Statement->OpCode->OpCode != EFI_IFR_REF_OP) {
+        ColumnStr2 = gEnterCommitString;
+        ColumnStr3 = gEnterEscapeString;
+      }
+      PrintStringAtWithWidth (StartColumnOfHelp, TopRowOfHelp, ColumnStr1, ColumnWidth1);
+      PrintStringAtWithWidth (StartColumnOfHelp, BottomRowOfHelp, ColumnStr1, ColumnWidth1);
+      PrintStringAtWithWidth (SecCol, BottomRowOfHelp, ColumnStr2, ColumnWidth2);
+      PrintStringAtWithWidth (ThdCol, BottomRowOfHelp, ColumnStr3, ColumnWidth3);
+    }
+    break;
+
+  default:
+    break;
+  }  
+}
+
+/**
+  Update status bar.
+
+  This function updates the status bar on the bottom of menu screen. It just shows StatusBar. 
+  Original logic in this function should be splitted out.
+
+  @param[in]  MessageType            The type of message to be shown. InputError or Configuration Changed. 
+  @param[in]  State                  Show or Clear Message.
+**/
+VOID
+EFIAPI
+UpdateStatusBar (
+  IN  UINTN                  MessageType,
+  IN  BOOLEAN                State
+  )
+{
+  UINTN           Index;
+  CHAR16          OptionWidth;
+
+  OptionWidth = (CHAR16) ((gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn) / 3);
+
+  switch (MessageType) {
+  case INPUT_ERROR:
+    if (State) {
+      gST->ConOut->SetAttribute (gST->ConOut, ERROR_TEXT);
+      PrintStringAt (
+        gScreenDimensions.LeftColumn + OptionWidth,
+        gScreenDimensions.BottomRow - 1,
+        gInputErrorMessage
+        );
+    } else {
+      gST->ConOut->SetAttribute (gST->ConOut, KEYHELP_BACKGROUND);
+      for (Index = 0; Index < (LibGetStringWidth (gInputErrorMessage) - 2) / 2; Index++) {
+        PrintStringAt (gScreenDimensions.LeftColumn + OptionWidth + Index, gScreenDimensions.BottomRow - 1, L"  ");
+      }
+    }
+    break;
+
+  case NV_UPDATE_REQUIRED:
+    //
+    // Global setting support. Show configuration change on every form.
+    //
+    if (State) {
+      gST->ConOut->SetAttribute (gST->ConOut, INFO_TEXT);
+      PrintStringAt (
+        gScreenDimensions.LeftColumn + OptionWidth * 2,
+        gScreenDimensions.BottomRow - 1,
+        gNvUpdateMessage
+        );
+    } else {
+      gST->ConOut->SetAttribute (gST->ConOut, KEYHELP_BACKGROUND);
+      for (Index = 0; Index < (LibGetStringWidth (gNvUpdateMessage) - 2) / 2; Index++) {
+        PrintStringAt (
+          (gScreenDimensions.LeftColumn + OptionWidth * 2 + Index),
+          gScreenDimensions.BottomRow - 1,
+          L"  "
+          );
+      }
+    }
+    break;
+
+  default:
+    break;
+  } 
+}
+
+/**
+  Create popup window. It will replace CreateDialog(). 
+
+  This function draws OEM/Vendor specific pop up windows.
+
+  @param[out]  Key    User Input Key
+  @param       ...    String to be shown in Popup. The variable argument list is terminated by a NULL.
+  
+**/
+VOID
+EFIAPI
+CreateDialog (
+  OUT EFI_INPUT_KEY  *Key,        OPTIONAL
+  ...
+  )
+{
+  VA_LIST       Marker;
+  EFI_INPUT_KEY KeyValue;
+  EFI_STATUS    Status;
+  UINTN         LargestString;
+  UINTN         LineNum;
+  UINTN   Index;
+  UINTN   Count;
+  CHAR16  Character;
+  UINTN   Start;
+  UINTN   End;
+  UINTN   Top;
+  UINTN   Bottom;
+  CHAR16  *String;
+  UINTN   DimensionsWidth;
+  UINTN   DimensionsHeight;
+  UINTN   CurrentAttribute;
+  BOOLEAN CursorVisible;
+
+  //
+  // If screen dimension info is not ready, get it from console.
+  //
+  if (gScreenDimensions.RightColumn == 0 || gScreenDimensions.BottomRow == 0) {
+    ZeroMem (&gScreenDimensions, sizeof (EFI_SCREEN_DESCRIPTOR));
+    gST->ConOut->QueryMode (
+                   gST->ConOut,
+                   gST->ConOut->Mode->Mode,
+                   &gScreenDimensions.RightColumn,
+                   &gScreenDimensions.BottomRow
+                   );
+  }
+
+  DimensionsWidth   = gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn;
+  DimensionsHeight  = gScreenDimensions.BottomRow - gScreenDimensions.TopRow;
+
+  LargestString = 0;
+  LineNum       = 0;
+  VA_START (Marker, Key);
+  while  ((String = VA_ARG (Marker, CHAR16 *)) != NULL) {
+    LineNum ++;
+    
+    if ((LibGetStringWidth (String) / 2) > LargestString) {
+      LargestString = (LibGetStringWidth (String) / 2);
+    }
+  } 
+  VA_END (Marker);
+
+  if ((LargestString + 2) > DimensionsWidth) {
+    LargestString = DimensionsWidth - 2;
+  }
+  
+  CurrentAttribute  = gST->ConOut->Mode->Attribute;
+  CursorVisible     = gST->ConOut->Mode->CursorVisible;
+  gST->ConOut->EnableCursor (gST->ConOut, FALSE);
+  gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+
+  //
+  // Subtract the PopUp width from total Columns, allow for one space extra on
+  // each end plus a border.
+  //
+  Start     = (DimensionsWidth - LargestString - 2) / 2 + gScreenDimensions.LeftColumn + 1;
+  End       = Start + LargestString + 1;
+
+  Top       = ((DimensionsHeight - LineNum - 2) / 2) + gScreenDimensions.TopRow - 1;
+  Bottom    = Top + LineNum + 2;
+
+  Character = BOXDRAW_DOWN_RIGHT;
+  PrintCharAt (Start, Top, Character);
+  Character = BOXDRAW_HORIZONTAL;
+  for (Index = Start; Index + 2 < End; Index++) {
+    PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+  }
+
+  Character = BOXDRAW_DOWN_LEFT;
+  PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+  Character = BOXDRAW_VERTICAL;
+
+  Count = 0;
+  VA_START (Marker, Key);
+  for (Index = Top; Index + 2 < Bottom; Index++, Count++) {
+    String = VA_ARG (Marker, CHAR16*);
+
+    if (String[0] == CHAR_NULL) {
+      //
+      // Passing in a NULL results in a blank space
+      //
+      ClearLines (Start, End, Index + 1, Index + 1, GetPopupColor ());
+    } else if (String[0] == L' ') {
+      //
+      // Passing in a space results in the assumption that this is where typing will occur
+      //
+      ClearLines (Start + 1, End - 1, Index + 1, Index + 1, POPUP_INVERSE_TEXT | POPUP_INVERSE_BACKGROUND);
+      PrintStringAt (
+        ((DimensionsWidth - LibGetStringWidth (String) / 2) / 2) + gScreenDimensions.LeftColumn + 1,
+        Index + 1,
+        String + 1
+        );
+    } else {
+      //
+      // This will clear the background of the line - we never know who might have been
+      // here before us.  This differs from the next clear in that it used the non-reverse
+      // video for normal printing.
+      //
+      ClearLines (Start, End, Index + 1, Index + 1, GetPopupColor ());
+      PrintStringAt (
+        ((DimensionsWidth - LibGetStringWidth (String) / 2) / 2) + gScreenDimensions.LeftColumn + 1,
+        Index + 1,
+        String
+        );
+    }
+
+    gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+    PrintCharAt (Start, Index + 1, Character);
+    PrintCharAt (End - 1, Index + 1, Character);
+  }
+  VA_END (Marker);
+
+  Character = BOXDRAW_UP_RIGHT;
+  PrintCharAt (Start, Bottom - 1, Character);
+  Character = BOXDRAW_HORIZONTAL;
+  for (Index = Start; Index + 2 < End; Index++) {
+    PrintCharAt ((UINTN)-1, (UINTN) -1, Character);
+  }
+
+  Character = BOXDRAW_UP_LEFT;
+  PrintCharAt ((UINTN)-1, (UINTN) -1, Character);
+
+  if (Key != NULL) {
+    Status = WaitForKeyStroke (&KeyValue);
+    ASSERT_EFI_ERROR (Status);
+    CopyMem (Key, &KeyValue, sizeof (EFI_INPUT_KEY));
+  }
+
+  gST->ConOut->SetAttribute (gST->ConOut, CurrentAttribute);
+  gST->ConOut->EnableCursor (gST->ConOut, CursorVisible);
+}
+
+/**
+  Confirm how to handle the changed data. 
+  
+  @return Action BROWSER_ACTION_SUBMIT, BROWSER_ACTION_DISCARD or other values.
+**/
+UINTN
+EFIAPI
+ConfirmDataChange (
+  VOID
+  )
+{
+  CHAR16                  YesResponse;
+  CHAR16                  NoResponse;
+  EFI_INPUT_KEY           Key;
+
+  gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+  
+  YesResponse = gYesResponse[0];
+  NoResponse  = gNoResponse[0];
+  
+  //
+  // If NV flag is up, prompt user
+  //
+  do {
+    CreateDialog (&Key, gLibEmptyString, gSaveChanges, gAreYouSure, gLibEmptyString, NULL);
+  } while
+  (
+    (Key.ScanCode != SCAN_ESC) &&
+    ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (NoResponse | UPPER_LOWER_CASE_OFFSET)) &&
+    ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (YesResponse | UPPER_LOWER_CASE_OFFSET))
+  );
+  
+  if (Key.ScanCode == SCAN_ESC) {
+    return BROWSER_ACTION_NONE;
+  } else if ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) == (YesResponse | UPPER_LOWER_CASE_OFFSET)) {
+    return BROWSER_ACTION_SUBMIT;
+  } else {
+    return BROWSER_ACTION_DISCARD;
+  }
+}
+
+/**
+  OEM specifies whether Setup exits Page by ESC key.
+
+  This function customized the behavior that whether Setup exits Page so that 
+  system able to boot when configuration is not changed.
+
+  @retval  TRUE     Exits FrontPage
+  @retval  FALSE    Don't exit FrontPage.
+**/
+BOOLEAN
+EFIAPI
+FormExitPolicy (
+  VOID
+  )
+{
+  return gClassOfVfr == FORMSET_CLASS_FRONT_PAGE ? FALSE : TRUE;
+}
+
+/**
+  Set Timeout value for a ceratain Form to get user response. 
+  
+  This function allows to set timeout value on a ceratain form if necessary.
+  If timeout is not zero, the form will exit if user has no response in timeout. 
+  
+  @param[in]  FormData   Form Data to be shown in Page
+
+  @return 0     No timeout for this form. 
+  @return > 0   Timeout value in 100 ns units.
+**/
+UINT64
+EFIAPI
+FormExitTimeout (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData
+  )
+{
+  return 0;
+}
+//
+// Print Functions
+//
+/**
+  Prints a unicode string to the default console, at
+  the supplied cursor position, using L"%s" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  String     String pointer.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintStringAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *String
+  )
+{
+  return PrintAt (0, Column, Row, L"%s", String);
+}
+
+/**
+  Prints a unicode string to the default console, at
+  the supplied cursor position, using L"%s" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  String     String pointer.
+  @param  Width      Width for String.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintStringAtWithWidth (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *String,
+  IN UINTN     Width
+  )
+{
+  return PrintAt (Width, Column, Row, L"%s", String);
+}
+
+/**
+  Prints a chracter to the default console, at
+  the supplied cursor position, using L"%c" format.
+
+  @param  Column     The cursor position to print the string at. When it is -1, use current Position.
+  @param  Row        The cursor position to print the string at. When it is -1, use current Position.
+  @param  Character  Character to print.
+
+  @return Length of string printed to the console.
+
+**/
+UINTN
+EFIAPI
+PrintCharAt (
+  IN UINTN     Column,
+  IN UINTN     Row,
+  CHAR16       Character
+  )
+{
+  return PrintAt (0, Column, Row, L"%c", Character);
+}
+
+/**
+  Clear retangle with specified text attribute.
+
+  @param  LeftColumn     Left column of retangle.
+  @param  RightColumn    Right column of retangle.
+  @param  TopRow         Start row of retangle.
+  @param  BottomRow      End row of retangle.
+  @param  TextAttribute  The character foreground and background.
+
+**/
+VOID
+EFIAPI
+ClearLines (
+  IN UINTN               LeftColumn,
+  IN UINTN               RightColumn,
+  IN UINTN               TopRow,
+  IN UINTN               BottomRow,
+  IN UINTN               TextAttribute
+  )
+{
+  CHAR16  *Buffer;
+  UINTN   Row;
+
+  //
+  // For now, allocate an arbitrarily long buffer
+  //
+  Buffer = AllocateZeroPool (0x10000);
+  ASSERT (Buffer != NULL);
+
+  //
+  // Set foreground and background as defined
+  //
+  gST->ConOut->SetAttribute (gST->ConOut, TextAttribute);
+
+  //
+  // Much faster to buffer the long string instead of print it a character at a time
+  //
+  LibSetUnicodeMem (Buffer, RightColumn - LeftColumn, L' ');
+
+  //
+  // Clear the desired area with the appropriate foreground/background
+  //
+  for (Row = TopRow; Row <= BottomRow; Row++) {
+    PrintStringAt (LeftColumn, Row, Buffer);
+  }
+
+  gST->ConOut->SetCursorPosition (gST->ConOut, LeftColumn, TopRow);
+
+  FreePool (Buffer);
+}
+
+//
+// Color Setting Functions
+//
+
+/**
+  Get OEM/Vendor specific popup attribute colors.
+
+  @retval  Byte code color setting for popup color.
+**/
+UINT8
+EFIAPI
+GetPopupColor (
+  VOID
+  )
+{
+  return POPUP_TEXT | POPUP_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific popup attribute colors.
+
+  @retval  Byte code color setting for popup inverse color.
+**/
+UINT8
+EFIAPI
+GetPopupInverseColor (
+  VOID
+  )
+{
+  return POPUP_INVERSE_TEXT | POPUP_INVERSE_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific PickList color attribute.
+
+  @retval  Byte code color setting for pick list color.
+**/
+UINT8
+EFIAPI
+GetPickListColor (
+  VOID
+  )
+{
+  return PICKLIST_HIGHLIGHT_TEXT | PICKLIST_HIGHLIGHT_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific arrow color attribute.
+
+  @retval  Byte code color setting for arrow color.
+**/
+UINT8
+EFIAPI
+GetArrowColor (
+  VOID
+  )
+{
+  return ARROW_TEXT | ARROW_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific info text color attribute.
+
+  @retval  Byte code color setting for info text color.
+**/
+UINT8
+EFIAPI
+GetInfoTextColor (
+  VOID
+  )
+{
+  return INFO_TEXT | FIELD_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific help text color attribute.
+
+  @retval  Byte code color setting for help text color.
+**/
+UINT8
+EFIAPI
+GetHelpTextColor (
+  VOID
+  )
+{
+  return HELP_TEXT | FIELD_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific grayed out text color attribute.
+
+  @retval  Byte code color setting for grayed out text color.
+**/
+UINT8
+EFIAPI
+GetGrayedTextColor (
+  VOID
+  )
+{
+  return FIELD_TEXT_GRAYED | FIELD_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific highlighted text color attribute.
+
+  @retval  Byte code color setting for highlight text color.
+**/
+UINT8
+EFIAPI
+GetHighlightTextColor (
+  VOID
+  )
+{
+  return PcdGet8 (PcdBrowserFieldTextHighlightColor) | PcdGet8 (PcdBrowserFieldBackgroundHighlightColor);
+}
+
+/**
+  Get OEM/Vendor specific field text color attribute.
+
+  @retval  Byte code color setting for field text color.
+**/
+UINT8
+EFIAPI
+GetFieldTextColor (
+  VOID
+  )
+{
+  return PcdGet8 (PcdBrowserFieldTextColor) | FIELD_BACKGROUND;
+}
+
+/**
+  Get OEM/Vendor specific subtitle text color attribute.
+
+  @retval  Byte code color setting for subtitle text color.
+**/
+UINT8
+EFIAPI
+GetSubTitleTextColor (
+  VOID
+  )
+{
+  return PcdGet8 (PcdBrowserSubtitleTextColor) | FIELD_BACKGROUND;
+}
+
+/**
+  Clear Screen to the initial state.
+**/
+VOID
+EFIAPI 
+ClearDisplayPage (
+  VOID
+  )
+{
+  gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+  gST->ConOut->ClearScreen (gST->ConOut);
+  gLibIsFirstForm = TRUE;
+}
+
+/**
+  Constructor of Customized Display Library Instance.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomizedDisplayLibConstructor (
+  IN      EFI_HANDLE                ImageHandle,
+  IN      EFI_SYSTEM_TABLE          *SystemTable
+  )
+{
+  mCDLStringPackHandle = HiiAddPackages (&gCustomizedDisplayLibGuid, ImageHandle, CustomizedDisplayLibStrings, NULL);
+  ASSERT (mCDLStringPackHandle != NULL);
+
+  InitializeLibStrings();
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Destructor of Customized Display Library Instance.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The destructor completed successfully.
+  @retval Other value   The destructor did not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomizedDisplayLibDestructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  HiiRemovePackages(mCDLStringPackHandle);
+  
+  FreeLibStrings ();
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.inf b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.inf
new file mode 100644
index 0000000000..23528948a3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.inf
@@ -0,0 +1,65 @@
+## @file
+# Customize display library used by display engine.
+#
+#  Copyright (c) 2013 - 2015, 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.
+#
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CustomizedDisplayLib
+  MODULE_UNI_FILE                = CustomizedDisplayLibModStrs.uni
+  FILE_GUID                      = 80B92017-EC64-4923-938D-94FAEE85832E
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = CustomizedDisplayLib|DXE_DRIVER UEFI_APPLICATION
+  CONSTRUCTOR                    = CustomizedDisplayLibConstructor
+  DESTRUCTOR                     = CustomizedDisplayLibDestructor
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  CustomizedDisplayLib.c
+  Colors.h
+  CustomizedDisplayLibInternal.h
+  CustomizedDisplayLibInternal.c
+  CustomizedDisplayLib.uni
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  PrintLib
+  HiiLib
+  DevicePathLib
+  PcdLib
+
+[Guids]
+  gEfiIfrTianoGuid  ## SOMETIMES_CONSUMES   ## UNDEFINED
+  
+[Protocols]
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserSubtitleTextColor               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldTextColor                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldTextHighlightColor         ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldBackgroundHighlightColor   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrontPageFormSetGuid                   ## CONSUMES
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.uni b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.uni
new file mode 100644
index 0000000000..18a5c3b563
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.uni
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.c b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.c
new file mode 100644
index 0000000000..bc14a9dd76
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.c
@@ -0,0 +1,984 @@
+/** @file
+
+  This library class defines a set of interfaces to customize Display module
+
+Copyright (c) 2013-2015, 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 that 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 "CustomizedDisplayLibInternal.h"
+
+EFI_SCREEN_DESCRIPTOR         gScreenDimensions;
+CHAR16                        *mLibUnknownString;
+extern EFI_HII_HANDLE         mCDLStringPackHandle;
+CHAR16                        *mSpaceBuffer;
+#define SPACE_BUFFER_SIZE      1000
+
+//
+// Browser Global Strings
+//
+CHAR16            *gEnterString;
+CHAR16            *gEnterCommitString;
+CHAR16            *gEnterEscapeString;
+CHAR16            *gEscapeString;
+CHAR16            *gMoveHighlight;
+CHAR16            *gDecNumericInput;
+CHAR16            *gHexNumericInput;
+CHAR16            *gToggleCheckBox;
+CHAR16            *gLibEmptyString;
+CHAR16            *gAreYouSure;
+CHAR16            *gYesResponse;
+CHAR16            *gNoResponse;
+CHAR16            *gPlusString;
+CHAR16            *gMinusString;
+CHAR16            *gAdjustNumber;
+CHAR16            *gSaveChanges;
+CHAR16            *gNvUpdateMessage;
+CHAR16            *gInputErrorMessage;
+
+/**
+
+  Print banner info for front page.
+
+  @param[in]  FormData             Form Data to be shown in Page
+  
+**/
+VOID
+PrintBannerInfo ( 
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  )
+{
+  UINT8                  Line;
+  UINT8                  Alignment;
+  CHAR16                 *StrFrontPageBanner;
+  UINT8                  RowIdx;
+  UINT8                  ColumnIdx;
+
+  //
+  //    ClearLines(0, LocalScreen.RightColumn, 0, BANNER_HEIGHT-1, BANNER_TEXT | BANNER_BACKGROUND);
+  //
+  ClearLines (
+    gScreenDimensions.LeftColumn,
+    gScreenDimensions.RightColumn,
+    gScreenDimensions.TopRow,
+    FRONT_PAGE_HEADER_HEIGHT - 1 + gScreenDimensions.TopRow,
+    BANNER_TEXT | BANNER_BACKGROUND
+    );
+
+  //
+  //    for (Line = 0; Line < BANNER_HEIGHT; Line++) {
+  //
+  for (Line = (UINT8) gScreenDimensions.TopRow; Line < BANNER_HEIGHT + (UINT8) gScreenDimensions.TopRow; Line++) {
+    //
+    //      for (Alignment = 0; Alignment < BANNER_COLUMNS; Alignment++) {
+    //
+    for (Alignment = (UINT8) gScreenDimensions.LeftColumn;
+         Alignment < BANNER_COLUMNS + (UINT8) gScreenDimensions.LeftColumn;
+         Alignment++
+        ) {
+      RowIdx    = (UINT8) (Line - (UINT8) gScreenDimensions.TopRow);
+      ColumnIdx = (UINT8) (Alignment - (UINT8) gScreenDimensions.LeftColumn);
+  
+      ASSERT (RowIdx < BANNER_HEIGHT && ColumnIdx < BANNER_COLUMNS);
+  
+      if (gBannerData!= NULL && gBannerData->Banner[RowIdx][ColumnIdx] != 0x0000) {
+        StrFrontPageBanner = LibGetToken (gBannerData->Banner[RowIdx][ColumnIdx], FormData->HiiHandle);
+      } else {
+        continue;
+      }
+  
+      switch (Alignment - gScreenDimensions.LeftColumn) {
+      case 0:
+        //
+        // Handle left column
+        //
+        PrintStringAt (gScreenDimensions.LeftColumn + BANNER_LEFT_COLUMN_INDENT, Line, StrFrontPageBanner);
+        break;
+  
+      case 1:
+        //
+        // Handle center column
+        //
+        PrintStringAt (
+          gScreenDimensions.LeftColumn + (gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn) / 3,
+          Line,
+          StrFrontPageBanner
+          );
+        break;
+  
+      case 2:
+        //
+        // Handle right column
+        //
+        PrintStringAt (
+          gScreenDimensions.LeftColumn + (gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn) * 2 / 3,
+          Line,
+          StrFrontPageBanner
+          );
+        break;
+      }
+  
+      FreePool (StrFrontPageBanner);
+    }
+  }
+}
+
+/**
+  Print framework and form title for a page.
+
+  @param[in]  FormData             Form Data to be shown in Page
+**/
+VOID
+PrintFramework (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  )
+{
+  UINTN                  Index;
+  CHAR16                 Character;
+  CHAR16                 *Buffer;
+  UINTN                  Row;
+  CHAR16                 *TitleStr;
+  UINTN                  TitleColumn;
+
+  if (gClassOfVfr != FORMSET_CLASS_PLATFORM_SETUP) {
+    //
+    // Only Setup page needs Framework
+    //
+    ClearLines (
+      gScreenDimensions.LeftColumn,
+      gScreenDimensions.RightColumn,
+      gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - gFooterHeight,
+      gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - 1,
+      KEYHELP_TEXT | KEYHELP_BACKGROUND
+      );
+    return;
+  }
+    
+  Buffer = AllocateZeroPool (0x10000);
+  ASSERT (Buffer != NULL);
+  Character = BOXDRAW_HORIZONTAL;
+  for (Index = 0; Index + 2 < (gScreenDimensions.RightColumn - gScreenDimensions.LeftColumn); Index++) {
+    Buffer[Index] = Character;
+  }
+
+  //
+  // Print Top border line
+  // +------------------------------------------------------------------------------+
+  // ?                                                                             ?
+  // +------------------------------------------------------------------------------+
+  //
+  gST->ConOut->SetAttribute (gST->ConOut, TITLE_TEXT | TITLE_BACKGROUND);
+  Character = BOXDRAW_DOWN_RIGHT;
+
+  PrintCharAt (gScreenDimensions.LeftColumn, gScreenDimensions.TopRow, Character);
+  PrintStringAt ((UINTN) -1, (UINTN) -1, Buffer);
+
+  Character = BOXDRAW_DOWN_LEFT;
+  PrintCharAt ((UINTN) -1, (UINTN) -1, Character);
+
+  Character = BOXDRAW_VERTICAL;
+  for (Row = gScreenDimensions.TopRow + 1; Row <= gScreenDimensions.TopRow + NONE_FRONT_PAGE_HEADER_HEIGHT - 2; Row++) {
+    PrintCharAt (gScreenDimensions.LeftColumn, Row, Character);
+    PrintCharAt (gScreenDimensions.RightColumn - 1, Row, Character);
+  }
+  
+  //
+  // Print Form Title
+  //
+  TitleStr = LibGetToken (FormData->FormTitle, FormData->HiiHandle);
+  ASSERT (TitleStr != NULL);
+  TitleColumn = (gScreenDimensions.RightColumn + gScreenDimensions.LeftColumn - LibGetStringWidth (TitleStr) / 2) / 2;
+  PrintStringAtWithWidth (gScreenDimensions.LeftColumn + 1, gScreenDimensions.TopRow + 1, gLibEmptyString, TitleColumn - gScreenDimensions.LeftColumn - 1);
+  PrintStringAtWithWidth (
+    TitleColumn,
+    gScreenDimensions.TopRow + 1,
+    TitleStr,
+    gScreenDimensions.RightColumn - 1 - TitleColumn
+    );
+  FreePool (TitleStr);
+
+  Character = BOXDRAW_UP_RIGHT;
+  PrintCharAt (gScreenDimensions.LeftColumn, gScreenDimensions.TopRow + NONE_FRONT_PAGE_HEADER_HEIGHT - 1, Character);
+  PrintStringAt ((UINTN) -1, (UINTN) -1, Buffer);
+
+  Character = BOXDRAW_UP_LEFT;
+  PrintCharAt ((UINTN) -1, (UINTN) -1, Character);
+
+  //
+  // Print Bottom border line
+  // +------------------------------------------------------------------------------+
+  // ?                                                                             ?
+  // +------------------------------------------------------------------------------+
+  //
+  Character = BOXDRAW_DOWN_RIGHT;
+  PrintCharAt (gScreenDimensions.LeftColumn, gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - gFooterHeight, Character);
+
+  PrintStringAt ((UINTN) -1, (UINTN) -1, Buffer);
+
+  Character = BOXDRAW_DOWN_LEFT;
+  PrintCharAt ((UINTN) -1, (UINTN) -1, Character);
+  Character = BOXDRAW_VERTICAL;
+  for (Row = gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - gFooterHeight + 1;
+       Row <= gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - 2;
+       Row++
+      ) {
+    PrintCharAt (gScreenDimensions.LeftColumn, Row, Character);
+    PrintCharAt (gScreenDimensions.RightColumn - 1, Row, Character);
+  }
+
+  Character = BOXDRAW_UP_RIGHT;
+  PrintCharAt (gScreenDimensions.LeftColumn, gScreenDimensions.BottomRow - STATUS_BAR_HEIGHT - 1, Character);
+
+  PrintStringAt ((UINTN) -1, (UINTN) -1, Buffer);
+
+  Character = BOXDRAW_UP_LEFT;
+  PrintCharAt ((UINTN) -1, (UINTN) -1, Character);
+  
+  FreePool (Buffer);
+}
+
+/**
+  Process some op code which is not recognized by browser core.
+
+  @param OpCodeData                  The pointer to the op code buffer.
+
+  @return EFI_SUCCESS            Pass the statement success.
+
+**/
+VOID
+ProcessUserOpcode(
+  IN  EFI_IFR_OP_HEADER         *OpCodeData
+  )
+{
+  EFI_GUID *   ClassGuid;
+  UINT8        ClassGuidNum;
+
+  ClassGuid    = NULL;
+  ClassGuidNum = 0;
+
+  switch (OpCodeData->OpCode) {
+    case EFI_IFR_FORM_SET_OP:
+      //
+      // process the statement outside of form,if it is formset op, get its formsetguid or classguid and compared with gFrontPageFormSetGuid
+      //
+      if (CompareMem (PcdGetPtr (PcdFrontPageFormSetGuid), &((EFI_IFR_FORM_SET *) OpCodeData)->Guid, sizeof (EFI_GUID)) == 0){
+        gClassOfVfr = FORMSET_CLASS_FRONT_PAGE;
+      } else{
+        ClassGuidNum = (UINT8)(((EFI_IFR_FORM_SET *)OpCodeData)->Flags & 0x3);
+        ClassGuid    = (EFI_GUID *)(VOID *)((UINT8 *)OpCodeData + sizeof (EFI_IFR_FORM_SET));
+        while (ClassGuidNum-- > 0){
+          if (CompareGuid((EFI_GUID*)PcdGetPtr (PcdFrontPageFormSetGuid),ClassGuid)){
+            gClassOfVfr = FORMSET_CLASS_FRONT_PAGE;
+            break;
+          }
+          ClassGuid ++;
+        }
+      }
+      break;
+
+    case EFI_IFR_GUID_OP:     
+      if (CompareGuid (&gEfiIfrTianoGuid, (EFI_GUID *)((CHAR8*) OpCodeData + sizeof (EFI_IFR_OP_HEADER)))) {
+        //
+        // Tiano specific GUIDed opcodes
+        //
+        switch (((EFI_IFR_GUID_LABEL *) OpCodeData)->ExtendOpCode) {
+        case EFI_IFR_EXTEND_OP_LABEL:
+          //
+          // just ignore label
+          //
+          break;
+
+        case EFI_IFR_EXTEND_OP_BANNER:
+          //
+          // Only in front page form set, we care about the banner data.
+          //
+          if (gClassOfVfr == FORMSET_CLASS_FRONT_PAGE) {
+            //
+            // Initialize Driver private data
+            //
+            if (gBannerData == NULL) {
+              gBannerData = AllocateZeroPool (sizeof (BANNER_DATA));
+              ASSERT (gBannerData != NULL);
+            }
+            
+            CopyMem (
+              &gBannerData->Banner[((EFI_IFR_GUID_BANNER *) OpCodeData)->LineNumber][
+              ((EFI_IFR_GUID_BANNER *) OpCodeData)->Alignment],
+              &((EFI_IFR_GUID_BANNER *) OpCodeData)->Title,
+              sizeof (EFI_STRING_ID)
+              );
+          }
+          break;
+
+        case EFI_IFR_EXTEND_OP_SUBCLASS:
+          if (((EFI_IFR_GUID_SUBCLASS *) OpCodeData)->SubClass == EFI_FRONT_PAGE_SUBCLASS) {
+            gClassOfVfr = FORMSET_CLASS_FRONT_PAGE;
+          }
+          break;
+
+        default:
+          break;
+        }
+      }
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  Process some op codes which is out side of current form.
+  
+  @param FormData                Pointer to the form data.
+
+  @return EFI_SUCCESS            Pass the statement success.
+
+**/
+VOID
+ProcessExternedOpcode (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  )
+{
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *NestLink;
+  FORM_DISPLAY_ENGINE_STATEMENT *Statement;
+  FORM_DISPLAY_ENGINE_STATEMENT *NestStatement;
+
+  Link = GetFirstNode (&FormData->StatementListOSF);
+  while (!IsNull (&FormData->StatementListOSF, Link)) {
+    Statement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&FormData->StatementListOSF, Link);
+
+    ProcessUserOpcode(Statement->OpCode);
+  }
+
+  Link = GetFirstNode (&FormData->StatementListHead);
+  while (!IsNull (&FormData->StatementListHead, Link)) {
+    Statement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&FormData->StatementListHead, Link);
+
+    ProcessUserOpcode(Statement->OpCode);
+
+    NestLink = GetFirstNode (&Statement->NestStatementList);
+    while (!IsNull (&Statement->NestStatementList, NestLink)) {
+      NestStatement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (NestLink);
+      NestLink = GetNextNode (&Statement->NestStatementList, NestLink);
+
+      ProcessUserOpcode(NestStatement->OpCode);
+    }
+
+  }
+}
+
+/**
+  Validate the input screen diemenstion info.
+
+  @param  FormData               The input form data info.
+
+  @return EFI_SUCCESS            The input screen info is acceptable.
+  @return EFI_INVALID_PARAMETER  The input screen info is not acceptable.
+
+**/
+EFI_STATUS 
+ScreenDiemensionInfoValidate (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  )
+{
+  LIST_ENTRY           *Link;
+  UINTN                Index;
+
+  //
+  // Calculate total number of Register HotKeys. 
+  //
+  Index = 0;
+  if (!IsListEmpty (&FormData->HotKeyListHead)){
+    Link  = GetFirstNode (&FormData->HotKeyListHead);
+    while (!IsNull (&FormData->HotKeyListHead, Link)) {
+      Link = GetNextNode (&FormData->HotKeyListHead, Link);
+      Index ++;
+    }
+  }
+
+  //
+  // Show three HotKeys help information on one row.
+  //
+  gFooterHeight = FOOTER_HEIGHT + (Index / 3);
+
+
+  ZeroMem (&gScreenDimensions, sizeof (EFI_SCREEN_DESCRIPTOR));
+  gST->ConOut->QueryMode (
+                 gST->ConOut,
+                 gST->ConOut->Mode->Mode,
+                 &gScreenDimensions.RightColumn,
+                 &gScreenDimensions.BottomRow
+                 );
+
+  //
+  // Check local dimension vs. global dimension.
+  //
+  if (FormData->ScreenDimensions != NULL) {
+    if ((gScreenDimensions.RightColumn < FormData->ScreenDimensions->RightColumn) ||
+        (gScreenDimensions.BottomRow < FormData->ScreenDimensions->BottomRow)
+        ) {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      //
+      // Local dimension validation.
+      //
+      if ((FormData->ScreenDimensions->RightColumn > FormData->ScreenDimensions->LeftColumn) &&
+          (FormData->ScreenDimensions->BottomRow > FormData->ScreenDimensions->TopRow) &&
+          ((FormData->ScreenDimensions->RightColumn - FormData->ScreenDimensions->LeftColumn) > 2) &&
+          ((FormData->ScreenDimensions->BottomRow - FormData->ScreenDimensions->TopRow) > STATUS_BAR_HEIGHT +
+            FRONT_PAGE_HEADER_HEIGHT + gFooterHeight + 3)) {
+        CopyMem (&gScreenDimensions, (VOID *) FormData->ScreenDimensions, sizeof (EFI_SCREEN_DESCRIPTOR));
+      } else {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The package list in the HII database to search for
+                                 the specified string.
+
+  @return The output string.
+
+**/
+CHAR16 *
+LibGetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  )
+{
+  EFI_STRING  String;
+
+  String = HiiGetString (HiiHandle, Token, NULL);
+  if (String == NULL) {
+    String = AllocateCopyPool (StrSize (mLibUnknownString), mLibUnknownString);
+    ASSERT (String != NULL);
+  }
+
+  return (CHAR16 *) String;
+}
+
+
+/**
+  Count the storage space of a Unicode string.
+
+  This function handles the Unicode string with NARROW_CHAR
+  and WIDE_CHAR control characters. NARROW_HCAR and WIDE_CHAR
+  does not count in the resultant output. If a WIDE_CHAR is
+  hit, then 2 Unicode character will consume an output storage
+  space with size of CHAR16 till a NARROW_CHAR is hit.
+
+  If String is NULL, then ASSERT ().
+
+  @param String          The input string to be counted.
+
+  @return Storage space for the input string.
+
+**/
+UINTN
+LibGetStringWidth (
+  IN CHAR16               *String
+  )
+{
+  UINTN Index;
+  UINTN Count;
+  UINTN IncrementValue;
+
+  ASSERT (String != NULL);
+  if (String == NULL) {
+    return 0;
+  }
+
+  Index           = 0;
+  Count           = 0;
+  IncrementValue  = 1;
+
+  do {
+    //
+    // Advance to the null-terminator or to the first width directive
+    //
+    for (;
+         (String[Index] != NARROW_CHAR) && (String[Index] != WIDE_CHAR) && (String[Index] != 0);
+         Index++, Count = Count + IncrementValue
+        )
+      ;
+
+    //
+    // We hit the null-terminator, we now have a count
+    //
+    if (String[Index] == 0) {
+      break;
+    }
+    //
+    // We encountered a narrow directive - strip it from the size calculation since it doesn't get printed
+    // and also set the flag that determines what we increment by.(if narrow, increment by 1, if wide increment by 2)
+    //
+    if (String[Index] == NARROW_CHAR) {
+      //
+      // Skip to the next character
+      //
+      Index++;
+      IncrementValue = 1;
+    } else {
+      //
+      // Skip to the next character
+      //
+      Index++;
+      IncrementValue = 2;
+    }
+  } while (String[Index] != 0);
+
+  //
+  // Increment by one to include the null-terminator in the size
+  //
+  Count++;
+
+  return Count * sizeof (CHAR16);
+}
+
+/**
+  Show all registered HotKey help strings on bottom Rows.
+
+  @param FormData          The curent input form data info.
+  @param SetState          Set HotKey or Clear HotKey
+
+**/
+VOID
+PrintHotKeyHelpString (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData,
+  IN BOOLEAN                       SetState
+  )
+{
+  UINTN                  CurrentCol;
+  UINTN                  CurrentRow;
+  UINTN                  BottomRowOfHotKeyHelp;
+  UINTN                  ColumnIndexWidth;
+  UINTN                  ColumnWidth;
+  UINTN                  ColumnIndex;
+  UINTN                  Index;
+  EFI_SCREEN_DESCRIPTOR  LocalScreen;
+  LIST_ENTRY             *Link;
+  BROWSER_HOT_KEY        *HotKey;
+  CHAR16                 BakChar;
+  CHAR16                 *ColumnStr;
+
+  CopyMem (&LocalScreen, &gScreenDimensions, sizeof (EFI_SCREEN_DESCRIPTOR));
+  ColumnWidth            = (LocalScreen.RightColumn - LocalScreen.LeftColumn) / 3;
+  BottomRowOfHotKeyHelp  = LocalScreen.BottomRow - STATUS_BAR_HEIGHT - 3;
+  ColumnStr              = gLibEmptyString;
+
+  //
+  // Calculate total number of Register HotKeys. 
+  //
+  Index = 0;
+  Link  = GetFirstNode (&FormData->HotKeyListHead);
+  while (!IsNull (&FormData->HotKeyListHead, Link)) {
+    HotKey = BROWSER_HOT_KEY_FROM_LINK (Link);
+    //
+    // Calculate help information Column and Row.
+    //
+    ColumnIndex = Index % 3;
+    if (ColumnIndex == 0) {
+      CurrentCol       = LocalScreen.LeftColumn + 2 * ColumnWidth;
+      ColumnIndexWidth = ColumnWidth - 1;
+    } else if (ColumnIndex == 1) {
+      CurrentCol       = LocalScreen.LeftColumn + ColumnWidth;
+      ColumnIndexWidth = ColumnWidth;
+    } else {
+      CurrentCol       = LocalScreen.LeftColumn + 2;
+      ColumnIndexWidth = ColumnWidth - 2;
+    }
+    CurrentRow = BottomRowOfHotKeyHelp - Index / 3;
+
+    //
+    // Help string can't exceed ColumnWidth. One Row will show three Help information. 
+    //
+    BakChar = L'\0';
+    if (StrLen (HotKey->HelpString) > ColumnIndexWidth) {
+      BakChar = HotKey->HelpString[ColumnIndexWidth];
+      HotKey->HelpString[ColumnIndexWidth] = L'\0';
+    }
+
+    //
+    // Print HotKey help string on bottom Row.
+    //
+    if (SetState) {
+      ColumnStr = HotKey->HelpString;
+    }
+    PrintStringAtWithWidth (CurrentCol, CurrentRow, ColumnStr, ColumnIndexWidth);
+
+    if (BakChar != L'\0') {
+      HotKey->HelpString[ColumnIndexWidth] = BakChar;
+    }
+    //
+    // Get Next Hot Key.
+    //
+    Link = GetNextNode (&FormData->HotKeyListHead, Link);
+    Index ++;
+  }
+  
+  if (SetState) {
+    //
+    // Clear KeyHelp
+    //
+    CurrentRow  = BottomRowOfHotKeyHelp - Index / 3;
+    ColumnIndex = Index % 3;
+    if (ColumnIndex == 0) {
+      CurrentCol       = LocalScreen.LeftColumn + 2 * ColumnWidth;
+      ColumnIndexWidth = ColumnWidth - 1;
+      ColumnIndex ++;
+      PrintStringAtWithWidth (CurrentCol, CurrentRow, gLibEmptyString, ColumnIndexWidth);
+    }
+    if (ColumnIndex == 1) {
+      CurrentCol       = LocalScreen.LeftColumn + ColumnWidth;
+      ColumnIndexWidth = ColumnWidth;
+      PrintStringAtWithWidth (CurrentCol, CurrentRow, gLibEmptyString, ColumnIndexWidth);
+    }
+  }
+  
+  return;
+}
+
+/**
+  Get step info from numeric opcode.
+  
+  @param[in] OpCode     The input numeric op code.
+
+  @return step info for this opcode.
+**/
+UINT64
+LibGetFieldFromNum (
+  IN  EFI_IFR_OP_HEADER     *OpCode
+  )
+{
+  EFI_IFR_NUMERIC       *NumericOp;
+  UINT64                Step;
+
+  NumericOp = (EFI_IFR_NUMERIC *) OpCode;
+  
+  switch (NumericOp->Flags & EFI_IFR_NUMERIC_SIZE) {
+  case EFI_IFR_NUMERIC_SIZE_1:
+    Step    = NumericOp->data.u8.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_2:
+    Step    = NumericOp->data.u16.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_4:
+    Step    = NumericOp->data.u32.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_8:
+    Step    = NumericOp->data.u64.Step;
+    break;
+  
+  default:
+    Step = 0;
+    break;
+  }
+
+  return Step;
+}
+
+/**
+  Initialize the HII String Token to the correct values.
+
+**/
+VOID
+InitializeLibStrings (
+  VOID
+  )
+{
+  mLibUnknownString        = L"!";
+
+  gEnterString          = LibGetToken (STRING_TOKEN (ENTER_STRING), mCDLStringPackHandle);
+  gEnterCommitString    = LibGetToken (STRING_TOKEN (ENTER_COMMIT_STRING), mCDLStringPackHandle);
+  gEnterEscapeString    = LibGetToken (STRING_TOKEN (ENTER_ESCAPE_STRING), mCDLStringPackHandle);
+  gEscapeString         = LibGetToken (STRING_TOKEN (ESCAPE_STRING), mCDLStringPackHandle);
+  gMoveHighlight        = LibGetToken (STRING_TOKEN (MOVE_HIGHLIGHT), mCDLStringPackHandle);
+  gDecNumericInput      = LibGetToken (STRING_TOKEN (DEC_NUMERIC_INPUT), mCDLStringPackHandle);
+  gHexNumericInput      = LibGetToken (STRING_TOKEN (HEX_NUMERIC_INPUT), mCDLStringPackHandle);
+  gToggleCheckBox       = LibGetToken (STRING_TOKEN (TOGGLE_CHECK_BOX), mCDLStringPackHandle);
+
+  gAreYouSure           = LibGetToken (STRING_TOKEN (ARE_YOU_SURE), mCDLStringPackHandle);
+  gYesResponse          = LibGetToken (STRING_TOKEN (ARE_YOU_SURE_YES), mCDLStringPackHandle);
+  gNoResponse           = LibGetToken (STRING_TOKEN (ARE_YOU_SURE_NO), mCDLStringPackHandle);
+  gPlusString           = LibGetToken (STRING_TOKEN (PLUS_STRING), mCDLStringPackHandle);
+  gMinusString          = LibGetToken (STRING_TOKEN (MINUS_STRING), mCDLStringPackHandle);
+  gAdjustNumber         = LibGetToken (STRING_TOKEN (ADJUST_NUMBER), mCDLStringPackHandle);
+  gSaveChanges          = LibGetToken (STRING_TOKEN (SAVE_CHANGES), mCDLStringPackHandle);
+
+  gLibEmptyString       = LibGetToken (STRING_TOKEN (EMPTY_STRING), mCDLStringPackHandle);
+
+  gNvUpdateMessage      = LibGetToken (STRING_TOKEN (NV_UPDATE_MESSAGE), mCDLStringPackHandle);
+  gInputErrorMessage    = LibGetToken (STRING_TOKEN (INPUT_ERROR_MESSAGE), mCDLStringPackHandle);
+  
+  //
+  // SpaceBuffer;
+  //
+  mSpaceBuffer = AllocatePool ((SPACE_BUFFER_SIZE + 1) * sizeof (CHAR16));
+  ASSERT (mSpaceBuffer != NULL);
+  LibSetUnicodeMem (mSpaceBuffer, SPACE_BUFFER_SIZE, L' ');
+  mSpaceBuffer[SPACE_BUFFER_SIZE] = L'\0';
+}
+
+
+/**
+  Free the HII String.
+
+**/
+VOID
+FreeLibStrings (
+  VOID
+  )
+{
+  FreePool (gEnterString);
+  FreePool (gEnterCommitString);
+  FreePool (gEnterEscapeString);
+  FreePool (gEscapeString);
+  FreePool (gMoveHighlight);
+  FreePool (gDecNumericInput);
+  FreePool (gHexNumericInput);
+  FreePool (gToggleCheckBox);
+
+  FreePool (gAreYouSure);
+  FreePool (gYesResponse);
+  FreePool (gNoResponse);
+  FreePool (gPlusString);
+  FreePool (gMinusString);
+  FreePool (gAdjustNumber);
+  FreePool (gSaveChanges);
+
+  FreePool (gLibEmptyString);
+
+  FreePool (gNvUpdateMessage);
+  FreePool (gInputErrorMessage);
+  
+  FreePool (mSpaceBuffer);
+}
+
+/**
+  Wait for a key to be pressed by user.
+
+  @param Key         The key which is pressed by user.
+
+  @retval EFI_SUCCESS The function always completed successfully.
+
+**/
+EFI_STATUS
+WaitForKeyStroke (
+  OUT  EFI_INPUT_KEY           *Key
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+
+  while (TRUE) {
+    Status = gST->ConIn->ReadKeyStroke (gST->ConIn, Key);
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+
+    if (Status != EFI_NOT_READY) {
+      continue;
+    }
+    
+    gBS->WaitForEvent (1, &gST->ConIn->WaitForKey, &Index);
+  }
+  return Status;
+}
+
+
+/**
+  Set Buffer to Value for Size bytes.
+
+  @param  Buffer                 Memory to set.
+  @param  Size                   Number of bytes to set
+  @param  Value                  Value of the set operation.
+
+**/
+VOID
+LibSetUnicodeMem (
+  IN VOID   *Buffer,
+  IN UINTN  Size,
+  IN CHAR16 Value
+  )
+{
+  CHAR16  *Ptr;
+
+  Ptr = Buffer;
+  while ((Size--)  != 0) {
+    *(Ptr++) = Value;
+  }
+}
+
+/**
+  The internal function prints to the EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL
+  protocol instance.
+
+  @param Width           Width of string to be print.
+  @param Column          The position of the output string.
+  @param Row             The position of the output string.
+  @param Out             The EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL instance.
+  @param Fmt             The format string.
+  @param Args            The additional argument for the variables in the format string.
+
+  @return Number of Unicode character printed.
+
+**/
+UINTN
+PrintInternal (
+  IN UINTN                            Width, 
+  IN UINTN                            Column,
+  IN UINTN                            Row,
+  IN EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *Out,
+  IN CHAR16                           *Fmt,
+  IN VA_LIST                          Args
+  )
+{
+  CHAR16  *Buffer;
+  CHAR16  *BackupBuffer;
+  UINTN   Index;
+  UINTN   PreviousIndex;
+  UINTN   Count;
+  UINTN   TotalCount;
+  UINTN   PrintWidth;
+  UINTN   CharWidth;
+
+  //
+  // For now, allocate an arbitrarily long buffer
+  //
+  Buffer        = AllocateZeroPool (0x10000);
+  BackupBuffer  = AllocateZeroPool (0x10000);
+  ASSERT (Buffer);
+  ASSERT (BackupBuffer);
+
+  if (Column != (UINTN) -1) {
+    Out->SetCursorPosition (Out, Column, Row);
+  }
+
+  UnicodeVSPrint (Buffer, 0x10000, Fmt, Args);
+
+  Out->Mode->Attribute = Out->Mode->Attribute & 0x7f;
+
+  Out->SetAttribute (Out, Out->Mode->Attribute);
+
+  Index         = 0;
+  PreviousIndex = 0;
+  Count         = 0;
+  TotalCount    = 0;
+  PrintWidth    = 0;
+  CharWidth     = 1;
+
+  do {
+    for (; (Buffer[Index] != NARROW_CHAR) && (Buffer[Index] != WIDE_CHAR) && (Buffer[Index] != 0); Index++) {
+      BackupBuffer[Index] = Buffer[Index];
+    }
+
+    if (Buffer[Index] == 0) {
+      break;
+    }
+
+    //
+    // Print this out, we are about to switch widths
+    //
+    Out->OutputString (Out, &BackupBuffer[PreviousIndex]);
+    Count = StrLen (&BackupBuffer[PreviousIndex]);
+    PrintWidth += Count * CharWidth;
+    TotalCount += Count;
+
+    //
+    // Preserve the current index + 1, since this is where we will start printing from next
+    //
+    PreviousIndex = Index + 1;
+
+    //
+    // We are at a narrow or wide character directive.  Set attributes and strip it and print it
+    //
+    if (Buffer[Index] == NARROW_CHAR) {
+      //
+      // Preserve bits 0 - 6 and zero out the rest
+      //
+      Out->Mode->Attribute = Out->Mode->Attribute & 0x7f;
+      Out->SetAttribute (Out, Out->Mode->Attribute);
+      CharWidth = 1;
+    } else {
+      //
+      // Must be wide, set bit 7 ON
+      //
+      Out->Mode->Attribute = Out->Mode->Attribute | EFI_WIDE_ATTRIBUTE;
+      Out->SetAttribute (Out, Out->Mode->Attribute);
+      CharWidth = 2;
+    }
+
+    Index++;
+
+  } while (Buffer[Index] != 0);
+
+  //
+  // We hit the end of the string - print it
+  //
+  Out->OutputString (Out, &BackupBuffer[PreviousIndex]);
+  Count = StrLen (&BackupBuffer[PreviousIndex]);
+  PrintWidth += Count * CharWidth;
+  TotalCount += Count;
+  if (PrintWidth < Width) {
+    Out->Mode->Attribute = Out->Mode->Attribute & 0x7f;
+    Out->SetAttribute (Out, Out->Mode->Attribute);
+    Out->OutputString (Out, &mSpaceBuffer[SPACE_BUFFER_SIZE - Width + PrintWidth]);
+  }
+
+  FreePool (Buffer);
+  FreePool (BackupBuffer);
+  return TotalCount;
+}
+
+/**
+  Prints a formatted unicode string to the default console, at
+  the supplied cursor position.
+
+  @param  Width      Width of String to be printed.
+  @param  Column     The cursor position to print the string at.
+  @param  Row        The cursor position to print the string at.
+  @param  Fmt        Format string.
+  @param  ...        Variable argument list for format string.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintAt (
+  IN UINTN     Width,
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *Fmt,
+  ...
+  )
+{
+  VA_LIST Args;
+  UINTN   LengthOfPrinted;
+
+  VA_START (Args, Fmt);
+  LengthOfPrinted = PrintInternal (Width, Column, Row, gST->ConOut, Fmt, Args);
+  VA_END (Args);
+  return LengthOfPrinted;
+}
+
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.h b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.h
new file mode 100644
index 0000000000..7342b508b0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibInternal.h
@@ -0,0 +1,297 @@
+/** @file
+
+  This library class defines a set of interfaces to customize Display module
+
+Copyright (c) 2013, 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 that 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.
+
+**/
+
+#ifndef __CUSTOMIZED_DISPLAY_LIB_INTERNAL_H__
+#define __CUSTOMIZED_DISPLAY_LIB_INTERNAL_H__
+
+
+
+#include <PiDxe.h>
+
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/FormBrowserEx2.h>
+#include <Protocol/DisplayProtocol.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/UnicodeCollation.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/UserManager.h>
+#include <Protocol/DevicePathFromText.h>
+
+#include <Guid/MdeModuleHii.h>
+#include <Guid/HiiPlatformSetupFormset.h>
+#include <Guid/HiiFormMapMethodGuid.h>
+
+#include <Library/PrintLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/HiiLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/CustomizedDisplayLib.h>
+
+#include "Colors.h"
+
+
+
+#define FORMSET_CLASS_PLATFORM_SETUP         0x0001
+#define FORMSET_CLASS_FRONT_PAGE             0x0002
+
+
+#define FRONT_PAGE_HEADER_HEIGHT      6
+#define NONE_FRONT_PAGE_HEADER_HEIGHT 3
+#define FOOTER_HEIGHT                 4
+#define STATUS_BAR_HEIGHT             1
+
+//
+// Screen definitions
+//
+#define BANNER_HEIGHT                 6
+#define BANNER_COLUMNS                3
+#define BANNER_LEFT_COLUMN_INDENT     1
+
+//
+// Character definitions
+//
+#define UPPER_LOWER_CASE_OFFSET 0x20
+
+//
+// This is the Input Error Message
+//
+#define INPUT_ERROR 1
+
+//
+// This is the NV RAM update required Message
+//
+#define NV_UPDATE_REQUIRED  2
+
+typedef struct {
+  EFI_STRING_ID  Banner[BANNER_HEIGHT][BANNER_COLUMNS];
+} BANNER_DATA;
+
+extern  UINT16                        gClassOfVfr;                 // Formset class information
+extern  BANNER_DATA                   *gBannerData;
+extern  EFI_SCREEN_DESCRIPTOR         gScreenDimensions;
+extern  UINTN                         gFooterHeight;
+
+//
+// Browser Global Strings
+//
+extern CHAR16            *gEnterString;
+extern CHAR16            *gEnterCommitString;
+extern CHAR16            *gEnterEscapeString;
+extern CHAR16            *gEscapeString;
+extern CHAR16            *gMoveHighlight;
+extern CHAR16            *gDecNumericInput;
+extern CHAR16            *gHexNumericInput;
+extern CHAR16            *gToggleCheckBox;
+extern CHAR16            *gLibEmptyString;
+extern CHAR16            *gAreYouSure;
+extern CHAR16            *gYesResponse;
+extern CHAR16            *gNoResponse;
+extern CHAR16            *gPlusString;
+extern CHAR16            *gMinusString;
+extern CHAR16            *gAdjustNumber;
+extern CHAR16            *gSaveChanges;
+extern CHAR16            *gNvUpdateMessage;
+extern CHAR16            *gInputErrorMessage;
+/**
+
+  Print banner info for front page.
+
+  @param[in]  FormData             Form Data to be shown in Page
+  
+**/
+VOID
+PrintBannerInfo ( 
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  );
+
+/**
+  Print framework and form title for a page.
+
+  @param[in]  FormData             Form Data to be shown in Page
+**/
+VOID
+PrintFramework (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  );
+
+/**
+  Validate the input screen diemenstion info.
+
+  @param  FormData               The input form data info.
+
+  @return EFI_SUCCESS            The input screen info is acceptable.
+  @return EFI_INVALID_PARAMETER  The input screen info is not acceptable.
+
+**/
+EFI_STATUS 
+ScreenDiemensionInfoValidate (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  );
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The package list in the HII database to search for
+                                 the specified string.
+
+  @return The output string.
+
+**/
+CHAR16 *
+LibGetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  );
+
+/**
+  Count the storage space of a Unicode string.
+
+  This function handles the Unicode string with NARROW_CHAR
+  and WIDE_CHAR control characters. NARROW_HCAR and WIDE_CHAR
+  does not count in the resultant output. If a WIDE_CHAR is
+  hit, then 2 Unicode character will consume an output storage
+  space with size of CHAR16 till a NARROW_CHAR is hit.
+
+  If String is NULL, then ASSERT ().
+
+  @param String          The input string to be counted.
+
+  @return Storage space for the input string.
+
+**/
+UINTN
+LibGetStringWidth (
+  IN CHAR16               *String
+  );
+
+/**
+  Show all registered HotKey help strings on bottom Rows.
+
+  @param FormData          The curent input form data info.
+  @param SetState          Set HotKey or Clear HotKey
+
+**/
+VOID
+PrintHotKeyHelpString (
+  IN FORM_DISPLAY_ENGINE_FORM      *FormData,
+  IN BOOLEAN                       SetState
+  );
+  
+/**
+  Get step info from numeric opcode.
+  
+  @param[in] OpCode     The input numeric op code.
+
+  @return step info for this opcode.
+**/
+UINT64
+LibGetFieldFromNum (
+  IN  EFI_IFR_OP_HEADER     *OpCode
+  );
+
+/**
+  Initialize the HII String Token to the correct values.
+
+**/
+VOID
+InitializeLibStrings (
+  VOID
+  );
+
+/**
+  Free the HII String.
+
+**/
+VOID
+FreeLibStrings (
+  VOID
+  );
+
+/**
+  Wait for a key to be pressed by user.
+
+  @param Key         The key which is pressed by user.
+
+  @retval EFI_SUCCESS The function always completed successfully.
+
+**/
+EFI_STATUS
+WaitForKeyStroke (
+  OUT  EFI_INPUT_KEY           *Key
+  );
+
+/**
+  Set Buffer to Value for Size bytes.
+
+  @param  Buffer                 Memory to set.
+  @param  Size                   Number of bytes to set
+  @param  Value                  Value of the set operation.
+
+**/
+VOID
+LibSetUnicodeMem (
+  IN VOID   *Buffer,
+  IN UINTN  Size,
+  IN CHAR16 Value
+  );
+
+/**
+  Prints a formatted unicode string to the default console, at
+  the supplied cursor position.
+
+  @param  Width      Width of String to be printed.
+  @param  Column     The cursor position to print the string at.
+  @param  Row        The cursor position to print the string at.
+  @param  Fmt        Format string.
+  @param  ...        Variable argument list for format string.
+
+  @return Length of string printed to the console
+
+**/
+UINTN
+EFIAPI
+PrintAt (
+  IN UINTN     Width,
+  IN UINTN     Column,
+  IN UINTN     Row,
+  IN CHAR16    *Fmt,
+  ...
+  );
+
+/**
+  Process some op codes which is out side of current form.
+  
+  @param FormData                Pointer to the form data.
+
+**/
+VOID
+ProcessExternedOpcode (
+  IN FORM_DISPLAY_ENGINE_FORM       *FormData
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibModStrs.uni b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibModStrs.uni
new file mode 100644
index 0000000000..64046df4c0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLibModStrs.uni
diff --git a/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.c b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.c
new file mode 100644
index 0000000000..09e1366189
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.c
@@ -0,0 +1,72 @@
+/** @file
+  Debug Agent library implementition with empty functions.
+
+  Copyright (c) 2010, 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 <Library/DebugAgentLib.h>
+
+/**
+  Initialize debug agent.
+
+  This function is used to set up debug environment to support source level debugging.
+  If certain Debug Agent Library instance has to save some private data in the stack,
+  this function must work on the mode that doesn't return to the caller, then
+  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
+  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
+  responsible to invoke the passing-in function at the end of InitializeDebugAgent().
+
+  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by
+  passing in the Context to be its parameter.
+
+  If Function() is NULL, Debug Agent Library instance will return after setup debug
+  environment.
+
+  @param[in] InitFlag     Init flag is used to decide the initialize process.
+  @param[in] Context      Context needed according to InitFlag; it was optional.
+  @param[in] Function     Continue function called by debug agent library; it was
+                          optional.
+
+**/
+VOID
+EFIAPI
+InitializeDebugAgent (
+  IN UINT32                InitFlag,
+  IN VOID                  *Context, OPTIONAL
+  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
+  )
+{
+  if (Function != NULL) {
+    Function (Context);
+  }
+}
+
+/**
+  Enable/Disable the interrupt of debug timer and return the interrupt state
+  prior to the operation.
+
+  If EnableStatus is TRUE, enable the interrupt of debug timer.
+  If EnableStatus is FALSE, disable the interrupt of debug timer.
+
+  @param[in] EnableStatus    Enable/Disable.
+
+  @return FALSE always.
+
+**/
+BOOLEAN
+EFIAPI
+SaveAndSetDebugTimerInterrupt (
+  IN BOOLEAN                EnableStatus
+  )
+{
+  return FALSE;
+}
+
diff --git a/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf
new file mode 100644
index 0000000000..ce1eab2623
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf
@@ -0,0 +1,36 @@
+## @file
+#  Null instance of Debug Agent Library with empty functions.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DebugAgentLibNull
+  MODULE_UNI_FILE                = DebugAgentLibNull.uni
+  FILE_GUID                      = 4904B42F-9FC0-4c2e-BB3F-A2AB35123530
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = DebugAgentLib
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  DebugAgentLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.uni b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.uni
new file mode 100644
index 0000000000..797c0f4076
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.uni
diff --git a/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.c b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.c
new file mode 100644
index 0000000000..fb5c25536f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.c
@@ -0,0 +1,51 @@
+/** @file
+  Null Dxe Capsule Library instance does nothing and returns unsupport status.
+
+Copyright (c) 2007 - 2008, 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 <Uefi.h>
+#include <Library/CapsuleLib.h>
+
+/**
+  The firmware checks whether the capsule image is supported 
+  by the CapsuleGuid in CapsuleHeader or other specific information in capsule image.
+  
+  @param  CapsuleHeader    Point to the UEFI capsule image to be checked.
+  
+  @retval EFI_UNSUPPORTED  Input capsule is not supported by the firmware.
+**/
+EFI_STATUS
+EFIAPI
+SupportCapsuleImage (
+  IN EFI_CAPSULE_HEADER *CapsuleHeader
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  The firmware specific implementation processes the capsule image
+  if it recognized the format of this capsule image.
+  
+  @param  CapsuleHeader    Point to the UEFI capsule image to be processed. 
+   
+  @retval EFI_UNSUPPORTED  Capsule image is not supported by the firmware.
+**/
+EFI_STATUS
+EFIAPI
+ProcessCapsuleImage (
+  IN EFI_CAPSULE_HEADER *CapsuleHeader
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
diff --git a/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.inf b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.inf
new file mode 100644
index 0000000000..b836607aae
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.inf
@@ -0,0 +1,38 @@
+## @file
+#  NULL Dxe Capsule library instance.
+#  It can make core modules pass package level build.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeCapsuleLibNull
+  MODULE_UNI_FILE                = DxeCapsuleLibNull.uni
+  FILE_GUID                      = 4004de5a-09a5-4f0c-94d7-82322e096aa7
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = CapsuleLib|DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_APPLICATION
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeCapsuleLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.uni b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.uni
new file mode 100644
index 0000000000..f669eccc71
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.uni
diff --git a/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.inf b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.inf
new file mode 100644
index 0000000000..0747f6e697
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.inf
@@ -0,0 +1,46 @@
+## @file
+# Memory Allocation Library instance dedicated to DXE Core.
+# The implementation borrows the DxeCore Memory Allocation services as the primitive
+# for memory allocation instead of using UEFI boot servces in an indirect way.
+# It is assumed that this library instance must be linked with DxeCore in this package. 
+#
+# Copyright (c) 2008 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeCoreMemoryAllocationLib
+  MODULE_UNI_FILE                = DxeCoreMemoryAllocationLib.uni
+  FILE_GUID                      = 632F3FAC-1CA4-4725-BAA2-BDECCF9A111C
+  MODULE_TYPE                    = DXE_CORE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = MemoryAllocationLib|DXE_CORE
+  
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  MemoryAllocationLib.c
+  DxeCoreMemoryAllocationServices.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  DebugLib
+  BaseMemoryLib
+
+
diff --git a/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.uni b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.uni
new file mode 100644
index 0000000000..6fffafc5ed
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationServices.h b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationServices.h
new file mode 100644
index 0000000000..04ad3d6da3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationServices.h
@@ -0,0 +1,106 @@
+/** @file
+  Contains function prototypes for Memory Services in DxeCore.
+
+  This header file borrows the DxeCore Memory Allocation services as the primitive
+  for memory allocation. 
+
+  Copyright (c) 2008, 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.             
+
+**/
+
+#ifndef _DXE_CORE_MEMORY_ALLOCATION_SERVICES_H_
+#define _DXE_CORE_MEMORY_ALLOCATION_SERVICES_H_
+
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @return Status. On success, Memory is filled in with the base address allocated
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in
+                                 spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePages (
+  IN EFI_ALLOCATE_TYPE      Type,
+  IN EFI_MEMORY_TYPE        MemoryType,
+  IN UINTN                  NumberOfPages,
+  IN OUT EFI_PHYSICAL_ADDRESS  *Memory
+  );
+
+
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS         -Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePages (
+  IN EFI_PHYSICAL_ADDRESS   Memory,
+  IN UINTN                  NumberOfPages
+  );
+
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreAllocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,
+  IN UINTN            Size,
+  OUT VOID            **Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+CoreFreePool (
+  IN VOID        *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/MemoryAllocationLib.c b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/MemoryAllocationLib.c
new file mode 100644
index 0000000000..d7d9ff311e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCoreMemoryAllocationLib/MemoryAllocationLib.c
@@ -0,0 +1,823 @@
+/** @file
+  Support routines for memory allocation routines based 
+  on boot services for Dxe phase drivers.
+
+  Copyright (c) 2006 - 2015, 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 <PiDxe.h>
+
+
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include "DxeCoreMemoryAllocationServices.h"
+
+/**
+  Allocates one or more 4KB pages of a certain memory type.
+
+  Allocates the number of 4KB pages of a certain memory type and returns a pointer to the allocated
+  buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL is returned.
+  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocatePages (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            Pages
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory; 
+
+  if (Pages == 0) {
+    return NULL;
+  }
+
+  Status = CoreAllocatePages (AllocateAnyPages, MemoryType, Pages, &Memory);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  return (VOID *) (UINTN) Memory;
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiBootServicesData.
+
+  Allocates the number of 4KB pages of type EfiBootServicesData and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocatePages (
+  IN UINTN  Pages
+  )
+{
+  return InternalAllocatePages (EfiBootServicesData, Pages);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData.
+
+  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimePages (
+  IN UINTN  Pages
+  )
+{
+  return InternalAllocatePages (EfiRuntimeServicesData, Pages);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiReservedMemoryType.
+
+  Allocates the number of 4KB pages of type EfiReservedMemoryType and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedPages (
+  IN UINTN  Pages
+  )
+{
+  return InternalAllocatePages (EfiReservedMemoryType, Pages);
+}
+
+/**
+  Frees one or more 4KB pages that were previously allocated with one of the page allocation
+  functions in the Memory Allocation Library.
+
+  Frees the number of 4KB pages specified by Pages from the buffer specified by Buffer.  Buffer
+  must have been allocated on a previous call to the page allocation services of the Memory
+  Allocation Library.  If it is not possible to free allocated pages, then this function will
+  perform no actions.
+  
+  If Buffer was not allocated with a page allocation function in the Memory Allocation Library,
+  then ASSERT().
+  If Pages is zero, then ASSERT().
+ 
+  @param  Buffer                Pointer to the buffer of pages to free.
+  @param  Pages                 The number of 4 KB pages to free.
+
+**/
+VOID
+EFIAPI
+FreePages (
+  IN VOID   *Buffer,
+  IN UINTN  Pages
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Pages != 0);
+  Status = CoreFreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates one or more 4KB pages of a certain memory type at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of a certain memory type with an alignment
+  specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is returned.
+  If there is not enough memory at the specified alignment remaining to satisfy the request, then
+  NULL is returned.
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateAlignedPages (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            Pages,
+  IN UINTN            Alignment
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 UnalignedPages;
+  UINTN                 RealPages;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+ 
+  if (Pages == 0) {
+    return NULL;
+  }
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+ 
+    Status         = CoreAllocatePages (AllocateAnyPages, MemoryType, RealPages, &Memory);
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+    AlignedMemory  = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+    UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN) Memory);
+    if (UnalignedPages > 0) {
+      //
+      // Free first unaligned page(s).
+      //
+      Status = CoreFreePages (Memory, UnalignedPages);
+      ASSERT_EFI_ERROR (Status);
+    }
+    Memory         = (EFI_PHYSICAL_ADDRESS) (AlignedMemory + EFI_PAGES_TO_SIZE (Pages));
+    UnalignedPages = RealPages - Pages - UnalignedPages;
+    if (UnalignedPages > 0) {
+      //
+      // Free last unaligned page(s).
+      //
+      Status = CoreFreePages (Memory, UnalignedPages);
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = CoreAllocatePages (AllocateAnyPages, MemoryType, Pages, &Memory);
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+    AlignedMemory  = (UINTN) Memory;
+  }
+  return (VOID *) AlignedMemory;
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiBootServicesData at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiBootServicesData with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedPages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return InternalAllocateAlignedPages (EfiBootServicesData, Pages, Alignment);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiRuntimeServicesData with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedRuntimePages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return InternalAllocateAlignedPages (EfiRuntimeServicesData, Pages, Alignment);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiReservedMemoryType at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiReservedMemoryType with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedReservedPages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return InternalAllocateAlignedPages (EfiReservedMemoryType, Pages, Alignment);
+}
+
+/**
+  Frees one or more 4KB pages that were previously allocated with one of the aligned page
+  allocation functions in the Memory Allocation Library.
+
+  Frees the number of 4KB pages specified by Pages from the buffer specified by Buffer.  Buffer
+  must have been allocated on a previous call to the aligned page allocation services of the Memory
+  Allocation Library.  If it is not possible to free allocated pages, then this function will 
+  perform no actions.
+  
+  If Buffer was not allocated with an aligned page allocation function in the Memory Allocation
+  Library, then ASSERT().
+  If Pages is zero, then ASSERT().
+  
+  @param  Buffer                Pointer to the buffer of pages to free.
+  @param  Pages                 The number of 4 KB pages to free.
+
+**/
+VOID
+EFIAPI
+FreeAlignedPages (
+  IN VOID   *Buffer,
+  IN UINTN  Pages
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Pages != 0);
+  Status = CoreFreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates a buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type and returns a
+  pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocatePool (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            AllocationSize
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Memory;
+
+  Memory = NULL;
+
+  Status = CoreAllocatePool (MemoryType, AllocationSize, &Memory);
+  if (EFI_ERROR (Status)) {
+    Memory = NULL;
+  }
+  return Memory;
+}
+
+/**
+  Allocates a buffer of type EfiBootServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiBootServicesData and returns a
+  pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocatePool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocatePool (EfiBootServicesData, AllocationSize);
+}
+
+/**
+  Allocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData and returns
+  a pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimePool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocatePool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates a buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType and returns
+  a pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocatePool (EfiReservedMemoryType, AllocationSize);
+}
+
+/**
+  Allocates and zeros a buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type, clears the buffer
+  with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a valid
+  buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the request,
+  then NULL is returned.
+
+  @param  PoolType              The type of memory to allocate.
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateZeroPool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            AllocationSize
+  ) 
+{
+  VOID  *Memory;
+
+  Memory = InternalAllocatePool (PoolType, AllocationSize);
+  if (Memory != NULL) {
+    Memory = ZeroMem (Memory, AllocationSize);
+  }
+  return Memory;
+}
+
+/**
+  Allocates and zeros a buffer of type EfiBootServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiBootServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocateZeroPool (EfiBootServicesData, AllocationSize);
+}
+
+/**
+  Allocates and zeros a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimeZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocateZeroPool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates and zeros a buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocateZeroPool (EfiReservedMemoryType, AllocationSize);
+}
+
+/**
+  Copies a buffer to an allocated buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  PoolType              The type of pool to allocate.
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateCopyPool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            AllocationSize,
+  IN CONST VOID       *Buffer
+  ) 
+{
+  VOID  *Memory;
+
+  ASSERT (Buffer != NULL);
+  ASSERT (AllocationSize <= (MAX_ADDRESS - (UINTN) Buffer + 1));
+
+  Memory = InternalAllocatePool (PoolType, AllocationSize);
+  if (Memory != NULL) {
+     Memory = CopyMem (Memory, Buffer, AllocationSize);
+  }
+  return Memory;
+} 
+
+/**
+  Copies a buffer to an allocated buffer of type EfiBootServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiBootServicesData, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return InternalAllocateCopyPool (EfiBootServicesData, AllocationSize, Buffer);
+}
+
+/**
+  Copies a buffer to an allocated buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimeCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return InternalAllocateCopyPool (EfiRuntimeServicesData, AllocationSize, Buffer);
+}
+
+/**
+  Copies a buffer to an allocated buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return InternalAllocateCopyPool (EfiReservedMemoryType, AllocationSize, Buffer);
+}
+
+/**
+  Reallocates a buffer of a specified memory type.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of the type
+  specified by PoolType.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  PoolType       The type of pool to allocate.
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalReallocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            OldSize,
+  IN UINTN            NewSize,
+  IN VOID             *OldBuffer  OPTIONAL
+  )
+{
+  VOID  *NewBuffer;
+
+  NewBuffer = InternalAllocateZeroPool (PoolType, NewSize);
+  if (NewBuffer != NULL && OldBuffer != NULL) {
+    CopyMem (NewBuffer, OldBuffer, MIN (OldSize, NewSize));
+    FreePool (OldBuffer);
+  }
+  return NewBuffer;
+}
+
+/**
+  Reallocates a buffer of type EfiBootServicesData.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiBootServicesData.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocatePool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return InternalReallocatePool (EfiBootServicesData, OldSize, NewSize, OldBuffer);
+}
+
+/**
+  Reallocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiRuntimeServicesData.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocateRuntimePool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return InternalReallocatePool (EfiRuntimeServicesData, OldSize, NewSize, OldBuffer);
+}
+
+/**
+  Reallocates a buffer of type EfiReservedMemoryType.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiReservedMemoryType.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocateReservedPool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return InternalReallocatePool (EfiReservedMemoryType, OldSize, NewSize, OldBuffer);
+}
+
+/**
+  Frees a buffer that was previously allocated with one of the pool allocation functions in the
+  Memory Allocation Library.
+
+  Frees the buffer specified by Buffer.  Buffer must have been allocated on a previous call to the
+  pool allocation services of the Memory Allocation Library.  If it is not possible to free pool
+  resources, then this function will perform no actions.
+  
+  If Buffer was not allocated with a pool allocation function in the Memory Allocation Library,
+  then ASSERT().
+
+  @param  Buffer                Pointer to the buffer to free.
+
+**/
+VOID
+EFIAPI
+FreePool (
+  IN VOID   *Buffer
+  )
+{
+  EFI_STATUS    Status;
+
+  Status = CoreFreePool (Buffer);
+  ASSERT_EFI_ERROR (Status);
+}
+
diff --git a/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.c b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.c
new file mode 100644
index 0000000000..0eb8e579d7
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.c
@@ -0,0 +1,847 @@
+/** @file
+  Performance library instance mainly used by DxeCore.
+
+  This library provides the performance measurement interfaces and initializes performance
+  logging for DXE phase. It first initializes its private global data structure for
+  performance logging and saves the performance GUIDed HOB passed from PEI phase. 
+  It initializes DXE phase performance logging by publishing the Performance and PerformanceEx Protocol,
+  which are consumed by DxePerformanceLib to logging performance data in DXE phase.
+
+  This library is mainly used by DxeCore to start performance logging to ensure that
+  Performance Protocol is installed at the very beginning of DXE phase.
+
+Copyright (c) 2006 - 2015, 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 "DxeCorePerformanceLibInternal.h"
+
+
+//
+// The data structure to hold global performance data.
+//
+GAUGE_DATA_HEADER    *mGaugeData;
+
+//
+// The current maximum number of logging entries. If current number of 
+// entries exceeds this value, it will re-allocate a larger array and
+// migration the old data to the larger array.
+//
+UINT32               mMaxGaugeRecords;
+
+//
+// The handle to install Performance Protocol instance.
+//
+EFI_HANDLE           mHandle = NULL;
+
+//
+// Interfaces for Performance Protocol.
+//
+PERFORMANCE_PROTOCOL mPerformanceInterface = {
+  StartGauge,
+  EndGauge,
+  GetGauge
+  };
+
+//
+// Interfaces for PerformanceEx Protocol.
+//
+PERFORMANCE_EX_PROTOCOL mPerformanceExInterface = {
+  StartGaugeEx,
+  EndGaugeEx,
+  GetGaugeEx
+  };
+
+/**
+  Searches in the gauge array with keyword Handle, Token, Module and Identifier.
+
+  This internal function searches for the gauge entry in the gauge array.
+  If there is an entry that exactly matches the given keywords
+  and its end time stamp is zero, then the index of that gauge entry is returned;
+  otherwise, the the number of gauge entries in the array is returned.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  Identifier              32-bit identifier.
+
+  @retval The index of gauge entry in the array.
+
+**/
+UINT32
+InternalSearchForGaugeEntry (
+  IN CONST VOID                 *Handle,  OPTIONAL
+  IN CONST CHAR8                *Token,   OPTIONAL
+  IN CONST CHAR8                *Module,   OPTIONAL
+  IN UINT32                     Identifier
+  )
+{
+  UINT32                    Index;
+  UINT32                    Index2;
+  UINT32                    NumberOfEntries;
+  GAUGE_DATA_ENTRY_EX       *GaugeEntryExArray;
+
+  if (Token == NULL) {
+    Token = "";
+  }
+  if (Module == NULL) {
+    Module = "";
+  }
+
+  NumberOfEntries = mGaugeData->NumberOfEntries;
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+  Index2 = 0;
+
+  for (Index = 0; Index < NumberOfEntries; Index++) {
+    Index2 = NumberOfEntries - 1 - Index;
+    if (GaugeEntryExArray[Index2].EndTimeStamp == 0 &&
+        (GaugeEntryExArray[Index2].Handle == (EFI_PHYSICAL_ADDRESS) (UINTN) Handle) &&
+        AsciiStrnCmp (GaugeEntryExArray[Index2].Token, Token, DXE_PERFORMANCE_STRING_LENGTH) == 0 &&
+        AsciiStrnCmp (GaugeEntryExArray[Index2].Module, Module, DXE_PERFORMANCE_STRING_LENGTH) == 0 &&
+        (GaugeEntryExArray[Index2].Identifier == Identifier)) {
+      Index = Index2;
+      break;
+    }
+  }
+
+  return Index;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  GAUGE_DATA_ENTRY_EX       *GaugeEntryExArray;
+  UINTN                     GaugeDataSize;
+  GAUGE_DATA_HEADER         *NewGaugeData;
+  UINTN                     OldGaugeDataSize;
+  GAUGE_DATA_HEADER         *OldGaugeData;
+  UINT32                    Index;
+
+  Index = mGaugeData->NumberOfEntries;
+  if (Index >= mMaxGaugeRecords) {
+    //
+    // Try to enlarge the scale of gauge array.
+    //
+    OldGaugeData      = mGaugeData;
+    OldGaugeDataSize  = sizeof (GAUGE_DATA_HEADER) + sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords;
+
+    GaugeDataSize     = sizeof (GAUGE_DATA_HEADER) + sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords * 2;
+
+    NewGaugeData = AllocateZeroPool (GaugeDataSize);
+    if (NewGaugeData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    mGaugeData = NewGaugeData;
+    mMaxGaugeRecords *= 2;
+
+    //
+    // Initialize new data array and migrate old data one.
+    //
+    mGaugeData = CopyMem (mGaugeData, OldGaugeData, OldGaugeDataSize);
+
+    FreePool (OldGaugeData);
+  }
+
+  GaugeEntryExArray               = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+  GaugeEntryExArray[Index].Handle = (EFI_PHYSICAL_ADDRESS) (UINTN) Handle;
+
+  if (Token != NULL) {
+    AsciiStrnCpyS (GaugeEntryExArray[Index].Token, DXE_PERFORMANCE_STRING_SIZE, Token, DXE_PERFORMANCE_STRING_LENGTH);
+  }
+  if (Module != NULL) {
+    AsciiStrnCpyS (GaugeEntryExArray[Index].Module, DXE_PERFORMANCE_STRING_SIZE, Module, DXE_PERFORMANCE_STRING_LENGTH);
+  }
+
+  GaugeEntryExArray[Index].EndTimeStamp = 0;
+  GaugeEntryExArray[Index].Identifier = Identifier;
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+  GaugeEntryExArray[Index].StartTimeStamp = TimeStamp;
+
+  mGaugeData->NumberOfEntries++;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  GAUGE_DATA_ENTRY_EX *GaugeEntryExArray;
+  UINT32              Index;
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+
+  Index = InternalSearchForGaugeEntry (Handle, Token, Module, Identifier);
+  if (Index >= mGaugeData->NumberOfEntries) {
+    return EFI_NOT_FOUND;
+  }
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+  GaugeEntryExArray[Index].EndTimeStamp = TimeStamp;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGauge and EndGauge of PERFORMANCE_PROTOCOL,
+  and then assign the Identifier with 0.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntryEx stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntryEx        The indirect pointer to the extended gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntryEx is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntryEx is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGaugeEx (
+  IN  UINTN                 LogEntryKey,
+  OUT GAUGE_DATA_ENTRY_EX   **GaugeDataEntryEx
+  )
+{
+  UINTN               NumberOfEntries;
+  GAUGE_DATA_ENTRY_EX *GaugeEntryExArray;
+
+  NumberOfEntries = (UINTN) (mGaugeData->NumberOfEntries);
+  if (LogEntryKey > NumberOfEntries) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (LogEntryKey == NumberOfEntries) {
+    return EFI_NOT_FOUND;
+  }
+
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+  if (GaugeDataEntryEx == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *GaugeDataEntryEx = &GaugeEntryExArray[LogEntryKey];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartGaugeEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndGaugeEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGaugeEx and EndGaugeEx of PERFORMANCE_EX_PROTOCOL,
+  and then eliminate the Identifier.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntry stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntry          The indirect pointer to the gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntry is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntry is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGauge (
+  IN  UINTN               LogEntryKey,
+  OUT GAUGE_DATA_ENTRY    **GaugeDataEntry
+  )
+{
+  EFI_STATUS          Status;
+  GAUGE_DATA_ENTRY_EX *GaugeEntryEx;
+
+  GaugeEntryEx = NULL;
+
+  Status = GetGaugeEx (LogEntryKey, &GaugeEntryEx);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (GaugeDataEntry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *GaugeDataEntry = (GAUGE_DATA_ENTRY *) GaugeEntryEx;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Dumps all the PEI performance log to DXE performance gauge array.
+
+  This internal function dumps all the PEI performance log to the DXE performance gauge array.
+  It retrieves the optional GUID HOB for PEI performance and then saves the performance data
+  to DXE performance data structures.
+
+**/
+VOID
+InternalGetPeiPerformance (
+  VOID
+  )
+{
+  EFI_HOB_GUID_TYPE                 *GuidHob;
+  PEI_PERFORMANCE_LOG_HEADER        *LogHob;
+  PEI_PERFORMANCE_LOG_ENTRY         *LogEntryArray;
+  UINT32                            *LogIdArray;
+  GAUGE_DATA_ENTRY_EX               *GaugeEntryExArray;
+  UINT32                            Index;
+  UINT32                            NumberOfEntries;
+
+  NumberOfEntries = 0;
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+  //
+  // Dump PEI Log Entries to DXE Guage Data structure.
+  //
+  GuidHob = GetFirstGuidHob (&gPerformanceProtocolGuid);
+  if (GuidHob != NULL) {
+    LogHob          = GET_GUID_HOB_DATA (GuidHob);
+    LogEntryArray   = (PEI_PERFORMANCE_LOG_ENTRY *) (LogHob + 1);
+
+    NumberOfEntries = LogHob->NumberOfEntries;
+    for (Index = 0; Index < NumberOfEntries; Index++) {
+      GaugeEntryExArray[Index].Handle         = LogEntryArray[Index].Handle;
+      AsciiStrCpyS (GaugeEntryExArray[Index].Token,  DXE_PERFORMANCE_STRING_SIZE, LogEntryArray[Index].Token);
+      AsciiStrCpyS (GaugeEntryExArray[Index].Module, DXE_PERFORMANCE_STRING_SIZE, LogEntryArray[Index].Module);
+      GaugeEntryExArray[Index].StartTimeStamp = LogEntryArray[Index].StartTimeStamp;
+      GaugeEntryExArray[Index].EndTimeStamp   = LogEntryArray[Index].EndTimeStamp;
+      GaugeEntryExArray[Index].Identifier     = 0;
+    }
+
+    GuidHob = GetFirstGuidHob (&gPerformanceExProtocolGuid);
+    if (GuidHob != NULL) {
+      LogIdArray    = GET_GUID_HOB_DATA (GuidHob);
+      for (Index = 0; Index < NumberOfEntries; Index++) {
+        GaugeEntryExArray[Index].Identifier   = LogIdArray[Index];
+      }
+    }
+  }
+  mGaugeData->NumberOfEntries = NumberOfEntries;
+}
+
+/**
+  The constructor function initializes Performance infrastructure for DXE phase.
+
+  The constructor function publishes Performance and PerformanceEx protocol, allocates memory to log DXE performance
+  and merges PEI performance data to DXE performance log.
+  It will ASSERT() if one of these operations fails and it will always return EFI_SUCCESS.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeCorePerformanceLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+
+  if (!PerformanceMeasurementEnabled ()) {
+    //
+    // Do not initialize performance infrastructure if not required.
+    //
+    return EFI_SUCCESS;
+  }
+  //
+  // Install the protocol interfaces.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gPerformanceProtocolGuid,
+                  &mPerformanceInterface,
+                  &gPerformanceExProtocolGuid,
+                  &mPerformanceExInterface,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mMaxGaugeRecords = INIT_DXE_GAUGE_DATA_ENTRIES + PcdGet8 (PcdMaxPeiPerformanceLogEntries);
+
+  mGaugeData = AllocateZeroPool (sizeof (GAUGE_DATA_HEADER) + (sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords));
+  ASSERT (mGaugeData != NULL);
+
+  InternalGetPeiPerformance ();
+
+  return Status;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return (RETURN_STATUS) StartGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return (RETURN_STATUS) EndGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  EFI_STATUS           Status;
+  GAUGE_DATA_ENTRY_EX  *GaugeData;
+
+  GaugeData = NULL;
+  
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  Status = GetGaugeEx (LogEntryKey++, &GaugeData);
+
+  //
+  // Make sure that LogEntryKey is a valid log entry key,
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // The LogEntryKey is the last entry (equals to the total entry number).
+    //
+    return 0;
+  }
+
+  ASSERT (GaugeData != NULL);
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+  *Identifier     = GaugeData->Identifier;
+
+  return LogEntryKey;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.inf b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.inf
new file mode 100644
index 0000000000..5f290634c5
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.inf
@@ -0,0 +1,70 @@
+## @file
+#  Performance library instance mainly for DxeCore usage.
+#  
+#  This library provides the performance measurement interfaces and initializes performance
+#  logging for DXE phase. It first initializes its private global data structure for
+#  performance logging and saves the performance GUIDed HOB passed from PEI phase. 
+#  It initializes DXE phase performance logging by publishing the Performance and PerformanceEx Protocol,
+#  which is consumed by DxePerformanceLib to logging performance data in DXE phase.
+#  This library is mainly used by DxeCore to start performance logging to ensure that
+#  Performance and PerformanceEx Protocol are installed at the very beginning of DXE phase.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeCorePerformanceLib
+  MODULE_UNI_FILE                = DxeCorePerformanceLib.uni
+  FILE_GUID                      = D0F78BBF-0A30-4c63-8A48-0F618A4AFACD
+  MODULE_TYPE                    = DXE_CORE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PerformanceLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+  CONSTRUCTOR                    = DxeCorePerformanceLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeCorePerformanceLib.c
+  DxeCorePerformanceLibInternal.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  PcdLib
+  TimerLib
+  BaseMemoryLib
+  BaseLib
+  HobLib
+  DebugLib
+
+
+[Guids]
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## PRODUCES             ## UNDEFINED # Install protocol
+  gPerformanceProtocolGuid
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## PRODUCES             ## UNDEFINED # Install protocol
+  gPerformanceExProtocolGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPerformanceLogEntries ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.uni b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.uni
new file mode 100644
index 0000000000..094a214862
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLibInternal.h b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLibInternal.h
new file mode 100644
index 0000000000..2b9ccd2fee
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLibInternal.h
@@ -0,0 +1,233 @@
+/** @file
+  Master header files for DxeCorePerformanceLib instance.
+
+  This header file holds the prototypes of the Performance and PerformanceEx Protocol published by this
+  library instance at its constructor.
+
+Copyright (c) 2006 - 2012, 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.
+
+**/
+  
+#ifndef _DXE_CORE_PERFORMANCE_LIB_INTERNAL_H_
+#define _DXE_CORE_PERFORMANCE_LIB_INTERNAL_H_
+
+
+#include <PiDxe.h>
+
+#include <Guid/Performance.h>
+
+#include <Library/PerformanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+//
+// Interface declarations for PerformanceEx Protocol.
+//
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGauge and EndGauge of PERFORMANCE_PROTOCOL,
+  and then assign the Identifier with 0.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntryEx stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntryEx        The indirect pointer to the extended gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntryEx is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntryEx is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGaugeEx (
+  IN  UINTN                 LogEntryKey,
+  OUT GAUGE_DATA_ENTRY_EX   **GaugeDataEntryEx
+  );
+
+//
+// Interface declarations for Performance Protocol.
+//
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGaugeEx and EndGaugeEx of PERFORMANCE_EX_PROTOCOL,
+  and then eliminate the Identifier.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntry stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntry          The indirect pointer to the gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntry is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntry is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGauge (
+  IN  UINTN               LogEntryKey,
+  OUT GAUGE_DATA_ENTRY    **GaugeDataEntry
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.c b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.c
new file mode 100644
index 0000000000..de1b1368da
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.c
@@ -0,0 +1,236 @@
+/** @file
+
+  This library registers CRC32 guided section handler 
+  to parse CRC32 encapsulation section and extract raw data.
+  It uses UEFI boot service CalculateCrc32 to authenticate 32 bit CRC value.
+
+Copyright (c) 2007 - 2011, 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 <PiDxe.h>
+#include <Guid/Crc32GuidedSectionExtraction.h>
+#include <Protocol/SecurityPolicy.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+///
+/// CRC32 Guided Section header
+///
+typedef struct {
+  EFI_GUID_DEFINED_SECTION  GuidedSectionHeader; ///< EFI guided section header
+  UINT32                    CRC32Checksum;       ///< 32bit CRC check sum
+} CRC32_SECTION_HEADER;
+
+typedef struct {
+  EFI_GUID_DEFINED_SECTION2 GuidedSectionHeader; ///< EFI guided section header
+  UINT32                    CRC32Checksum;       ///< 32bit CRC check sum
+} CRC32_SECTION2_HEADER;
+
+/**
+
+  GetInfo gets raw data size and attribute of the input guided section.
+  It first checks whether the input guid section is supported. 
+  If not, EFI_INVALID_PARAMETER will return.
+
+  @param InputSection       Buffer containing the input GUIDed section to be processed.
+  @param OutputBufferSize   The size of OutputBuffer.
+  @param ScratchBufferSize  The size of ScratchBuffer.
+  @param SectionAttribute   The attribute of the input guided section.
+
+  @retval EFI_SUCCESS            The size of destination buffer, the size of scratch buffer and 
+                                 the attribute of the input section are successull retrieved.
+  @retval EFI_INVALID_PARAMETER  The GUID in InputSection does not match this instance guid.
+
+**/
+EFI_STATUS
+EFIAPI
+Crc32GuidedSectionGetInfo (
+  IN  CONST VOID  *InputSection,
+  OUT UINT32      *OutputBufferSize,
+  OUT UINT32      *ScratchBufferSize,
+  OUT UINT16      *SectionAttribute
+  )
+{
+  if (IS_SECTION2 (InputSection)) {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // Retrieve the size and attribute of the input section data.
+    //
+    *SectionAttribute  = ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes;
+    *ScratchBufferSize = 0;
+    *OutputBufferSize  = SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+  } else {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // Retrieve the size and attribute of the input section data.
+    //
+    *SectionAttribute  = ((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes;
+    *ScratchBufferSize = 0;
+    *OutputBufferSize  = SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Extraction handler tries to extract raw data from the input guided section.
+  It also does authentication check for 32bit CRC value in the input guided section.
+  It first checks whether the input guid section is supported. 
+  If not, EFI_INVALID_PARAMETER will return.
+
+  @param InputSection    Buffer containing the input GUIDed section to be processed.
+  @param OutputBuffer    Buffer to contain the output raw data allocated by the caller.
+  @param ScratchBuffer   A pointer to a caller-allocated buffer for function internal use.
+  @param AuthenticationStatus A pointer to a caller-allocated UINT32 that indicates the
+                              authentication status of the output buffer.
+
+  @retval EFI_SUCCESS            Section Data and Auth Status is extracted successfully.
+  @retval EFI_INVALID_PARAMETER  The GUID in InputSection does not match this instance guid.
+
+**/
+EFI_STATUS
+EFIAPI
+Crc32GuidedSectionHandler (
+  IN CONST  VOID    *InputSection,
+  OUT       VOID    **OutputBuffer,
+  IN        VOID    *ScratchBuffer,        OPTIONAL
+  OUT       UINT32  *AuthenticationStatus
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    SectionCrc32Checksum;
+  UINT32                    Crc32Checksum;
+  UINT32                    OutputBufferSize;
+  VOID                      *DummyInterface;
+
+  if (IS_SECTION2 (InputSection)) {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  
+    //
+    // Get section Crc32 checksum.
+    //
+    SectionCrc32Checksum = ((CRC32_SECTION2_HEADER *) InputSection)->CRC32Checksum;
+    *OutputBuffer      = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+    OutputBufferSize   = SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+
+    //
+    // Implicitly CRC32 GUIDed section should have STATUS_VALID bit set
+    //
+    ASSERT (((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID);
+    *AuthenticationStatus = EFI_AUTH_STATUS_IMAGE_SIGNED;
+  } else {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  
+    //
+    // Get section Crc32 checksum.
+    //
+    SectionCrc32Checksum = ((CRC32_SECTION_HEADER *) InputSection)->CRC32Checksum;
+    *OutputBuffer      = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+    OutputBufferSize   = SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+
+    //
+    // Implicitly CRC32 GUIDed section should have STATUS_VALID bit set
+    //
+    ASSERT (((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID);
+    *AuthenticationStatus = EFI_AUTH_STATUS_IMAGE_SIGNED;
+  }
+
+  //
+  // Init Checksum value to Zero.
+  //
+  Crc32Checksum = 0;
+
+  //
+  // Check whether there exists EFI_SECURITY_POLICY_PROTOCOL_GUID.
+  //
+  Status = gBS->LocateProtocol (&gEfiSecurityPolicyProtocolGuid, NULL, &DummyInterface);
+  if (!EFI_ERROR (Status)) {
+    //
+    // If SecurityPolicy Protocol exist, AUTH platform override bit is set.
+    //
+    *AuthenticationStatus |= EFI_AUTH_STATUS_PLATFORM_OVERRIDE;
+  } else {
+    //
+    // Calculate CRC32 Checksum of Image
+    //
+    Status = gBS->CalculateCrc32 (*OutputBuffer, OutputBufferSize, &Crc32Checksum);
+    if (Status == EFI_SUCCESS) {
+      if (Crc32Checksum != SectionCrc32Checksum) {
+        //
+        // If Crc32 checksum is not matched, AUTH tested failed bit is set.
+        //
+        *AuthenticationStatus |= EFI_AUTH_STATUS_TEST_FAILED;
+      }
+    } else {
+      //
+      // If Crc32 checksum is not calculated, AUTH not tested bit is set.
+      //
+      *AuthenticationStatus |= EFI_AUTH_STATUS_NOT_TESTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register the handler to extract CRC32 guided section.
+
+  @param  ImageHandle  ImageHandle of the loaded driver.
+  @param  SystemTable  Pointer to the EFI System Table.
+
+  @retval  EFI_SUCCESS            Register successfully.
+  @retval  EFI_OUT_OF_RESOURCES   No enough memory to register this handler.
+**/
+EFI_STATUS
+EFIAPI
+DxeCrc32GuidedSectionExtractLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  return ExtractGuidedSectionRegisterHandlers (
+          &gEfiCrc32GuidedSectionExtractionGuid,
+          Crc32GuidedSectionGetInfo,
+          Crc32GuidedSectionHandler
+          );
+}
+
diff --git a/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf
new file mode 100644
index 0000000000..d244897ab6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf
@@ -0,0 +1,55 @@
+## @file
+#  Dxe Crc32 Guided Section Extract library.
+#
+#  This library doesn't produce any library class. The constructor function uses 
+#  ExtractGuidedSectionLib service to register CRC32 guided section handler
+#  that parses CRC32 encapsulation section and extracts raw data.
+#
+#  It uses UEFI boot service CalculateCrc32 to authenticate 32 bit CRC value.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeCrc32GuidedSectionExtractLib
+  MODULE_UNI_FILE                = DxeCrc32GuidedSectionExtractLib.uni
+  FILE_GUID                      = 387A2490-81FC-4E7C-8E0A-3E58C30FCD0B
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+  CONSTRUCTOR                    = DxeCrc32GuidedSectionExtractLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeCrc32GuidedSectionExtractLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  ExtractGuidedSectionLib
+  UefiBootServicesTableLib
+  DebugLib
+  BaseMemoryLib
+
+[Guids]
+  gEfiCrc32GuidedSectionExtractionGuid             ## PRODUCES ## UNDEFINED
+
+[Protocols]
+  gEfiSecurityPolicyProtocolGuid                   ## SOMETIMES_CONSUMES # Set platform override AUTH status if exist
diff --git a/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.uni b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.uni
new file mode 100644
index 0000000000..7e6e0c4e8e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.c b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.c
new file mode 100644
index 0000000000..cad57024cd
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.c
@@ -0,0 +1,388 @@
+/** @file
+  Debug Print Error Level library instance that provide compatibility with the 
+  "err" shell command.  This includes support for the Debug Mask Protocol
+  supports for global debug print error level mask stored in an EFI Variable.
+  This library instance only support DXE Phase modules.
+
+  Copyright (c) 2011, 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 <PiDxe.h>
+
+#include <Library/DebugPrintErrorLevelLib.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+
+#include <Guid/DebugMask.h>
+
+///
+/// Debug Mask Protocol function prototypes
+///
+
+/**
+  Retrieves the current debug print error level mask for a module are returns
+  it in CurrentDebugMask.
+
+  @param  This              The protocol instance pointer.
+  @param  CurrentDebugMask  Pointer to the debug print error level mask that 
+                            is returned.
+
+  @retval EFI_SUCCESS            The current debug print error level mask was
+                                 returned in CurrentDebugMask.
+  @retval EFI_INVALID_PARAMETER  CurrentDebugMask is NULL.
+  @retval EFI_DEVICE_ERROR       The current debug print error level mask could
+                                 not be retrieved.
+
+**/
+EFI_STATUS
+EFIAPI
+GetDebugMask (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,             
+  IN OUT UINTN                *CurrentDebugMask  
+  );
+
+/**
+  Sets the current debug print error level mask for a module to the value
+  specified by NewDebugMask.
+
+  @param  This          The protocol instance pointer.
+  @param  NewDebugMask  The new debug print error level mask for this module.
+
+  @retval EFI_SUCCESS            The current debug print error level mask was
+                                 set to the value specified by NewDebugMask.
+  @retval EFI_DEVICE_ERROR       The current debug print error level mask could
+                                 not be set to the value specified by NewDebugMask.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDebugMask (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,
+  IN UINTN                    NewDebugMask
+  );
+
+///
+/// Debug Mask Protocol instance
+///
+EFI_DEBUG_MASK_PROTOCOL  mDebugMaskProtocol = {
+  EFI_DEBUG_MASK_REVISION,
+  GetDebugMask,
+  SetDebugMask
+};
+
+///
+/// Global variable that is set to TRUE after the first attempt is made to 
+/// retrieve the global error level mask through the EFI Varibale Services.
+/// This variable prevents the EFI Variable Services from being called fort
+/// every DEBUG() macro.
+///
+BOOLEAN           mGlobalErrorLevelInitialized = FALSE;
+
+///
+/// Global variable that contains the current debug error level mask for the
+/// module that is using this library instance.  This variable is initially
+/// set to the PcdDebugPrintErrorLevel value.  If the EFI Variable exists that
+/// contains the global debug print error level mask, then that overrides the
+/// PcdDebugPrintErrorLevel value. The EFI Variable can optionally be 
+/// discovered via a HOB so early DXE drivers can access the variable. If the
+/// Debug Mask Protocol SetDebugMask() service is called, then that overrides 
+/// the PcdDebugPrintErrorLevel and the EFI Variable setting.
+///
+UINT32            mDebugPrintErrorLevel        = 0;
+
+///
+/// Global variable that is used to cache a pointer to the EFI System Table
+/// that is required to access the EFI Variable Services to get and set
+/// the global debug print error level mask value.  The UefiBootServicesTableLib
+/// is not used to prevent a circular dependency between these libraries.
+///
+EFI_SYSTEM_TABLE  *mSystemTable                         = NULL;
+
+/**
+  The constructor function caches the PCI Express Base Address and creates a 
+  Set Virtual Address Map event to convert physical address to virtual addresses.
+  
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The constructor completed successfully.
+  @retval Other value   The constructor did not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeDebugPrintErrorLevelLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  
+  //
+  // Initialize the error level mask from PCD setting.
+  //
+  mDebugPrintErrorLevel = PcdGet32 (PcdDebugPrintErrorLevel);
+    
+  //
+  // Install Debug Mask Protocol onto ImageHandle
+  //  
+  mSystemTable = SystemTable;
+  Status = SystemTable->BootServices->InstallMultipleProtocolInterfaces (
+                                        &ImageHandle,
+                                        &gEfiDebugMaskProtocolGuid, &mDebugMaskProtocol,
+                                        NULL
+                                        );
+
+  //
+  // Attempt to retrieve the global debug print error level mask from the EFI Variable
+  // If the EFI Variable can not be accessed when this module's library constructors are
+  // executed a HOB can be used to set the global debug print error level. If no value 
+  // was found then the EFI Variable access will be reattempted on every DEBUG() print
+  // from this module until the EFI Variable services are available.
+  //
+  GetDebugPrintErrorLevel ();
+  
+  return Status;
+}
+
+/**
+  The destructor function frees any allocated buffers and closes the Set Virtual 
+  Address Map event.
+  
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The destructor completed successfully.
+  @retval Other value   The destructor did not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeDebugPrintErrorLevelLibDestructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  //
+  // Uninstall the Debug Mask Protocol from ImageHandle
+  //  
+  return SystemTable->BootServices->UninstallMultipleProtocolInterfaces (
+                                      ImageHandle,
+                                      &gEfiDebugMaskProtocolGuid, &mDebugMaskProtocol,
+                                      NULL
+                                      );
+}
+
+/**
+  Returns the debug print error level mask for the current module.
+
+  @return  Debug print error level mask for the current module.
+
+**/
+UINT32
+EFIAPI
+GetDebugPrintErrorLevel (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  EFI_TPL     CurrentTpl;
+  UINTN       Size;
+  UINTN       GlobalErrorLevel;
+  VOID        *Hob;
+
+  //
+  // If the constructor has not been executed yet, then just return the PCD value.
+  // This case should only occur if debug print is generated by a library
+  // constructor for this module
+  //
+  if (mSystemTable == NULL) {
+    return PcdGet32 (PcdDebugPrintErrorLevel);
+  }
+  
+  //
+  // Check to see if an attempt has been made to retrieve the global debug print 
+  // error level mask.  Since this library instance stores the global debug print
+  // error level mask in an EFI Variable, the EFI Variable should only be accessed
+  // once to reduce the overhead of reading the EFI Variable on every debug print
+  //  
+  if (!mGlobalErrorLevelInitialized) {
+    //
+    // Make sure the TPL Level is low enough for EFI Variable Services to be called
+    //
+    CurrentTpl = mSystemTable->BootServices->RaiseTPL (TPL_HIGH_LEVEL);
+    mSystemTable->BootServices->RestoreTPL (CurrentTpl);
+    if (CurrentTpl <= TPL_CALLBACK) {
+      //
+      // Attempt to retrieve the global debug print error level mask from the 
+      // EFI Variable
+      //
+      Size = sizeof (GlobalErrorLevel);
+      Status = mSystemTable->RuntimeServices->GetVariable (
+                                       DEBUG_MASK_VARIABLE_NAME, 
+                                       &gEfiGenericVariableGuid, 
+                                       NULL, 
+                                       &Size, 
+                                       &GlobalErrorLevel
+                                       );
+      if (Status != EFI_NOT_AVAILABLE_YET) {
+        //
+        // If EFI Variable Services are available, then set a flag so the EFI
+        // Variable will not be read again by this module.
+        //
+        mGlobalErrorLevelInitialized = TRUE;
+        if (!EFI_ERROR (Status)) {
+          //
+          // If the EFI Varible exists, then set this module's module's mask to
+          // the global debug print error level mask value.
+          //
+          mDebugPrintErrorLevel = (UINT32)GlobalErrorLevel;
+        }
+      } else {
+        //
+        // If variable services are not yet availible optionally get the global
+        // debug print error level mask from a HOB.
+        //
+        Hob = GetFirstGuidHob (&gEfiGenericVariableGuid);
+        if (Hob != NULL) {
+          if (GET_GUID_HOB_DATA_SIZE (Hob) == sizeof (UINT32)) {
+            mDebugPrintErrorLevel = *(UINT32 *)GET_GUID_HOB_DATA (Hob);
+            mGlobalErrorLevelInitialized = TRUE;
+          }
+        }
+      }
+    }
+  }
+
+  //
+  // Return the current mask value for this module.
+  //
+  return mDebugPrintErrorLevel;
+}
+
+/**
+  Sets the global debug print error level mask fpr the entire platform.
+  
+  @param   ErrorLevel     Global debug print error level
+  
+  @retval  TRUE           The debug print error level mask was sucessfully set.
+  @retval  FALSE          The debug print error level mask could not be set.
+
+**/
+BOOLEAN
+EFIAPI
+SetDebugPrintErrorLevel (
+  UINT32  ErrorLevel
+  )
+{
+  EFI_STATUS  Status;
+  EFI_TPL     CurrentTpl;
+  UINTN       Size;
+  UINTN       GlobalErrorLevel;
+
+  //
+  // Make sure the constructor has been executed
+  //
+  if (mSystemTable != NULL) {
+    //
+    // Make sure the TPL Level is low enough for EFI Variable Services
+    //
+    CurrentTpl = mSystemTable->BootServices->RaiseTPL (TPL_HIGH_LEVEL);
+    mSystemTable->BootServices->RestoreTPL (CurrentTpl);
+    if (CurrentTpl <= TPL_CALLBACK) {
+      //
+      // Attempt to store the global debug print error level mask in an EFI Variable
+      //
+      GlobalErrorLevel = (UINTN)ErrorLevel;
+      Size = sizeof (GlobalErrorLevel);
+      Status = mSystemTable->RuntimeServices->SetVariable (
+                                       DEBUG_MASK_VARIABLE_NAME, 
+                                       &gEfiGenericVariableGuid, 
+                                       (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS),
+                                       Size,
+                                       &GlobalErrorLevel
+                                       );
+      if (!EFI_ERROR (Status)) {
+        //
+        // If the EFI Variable was updated, then update the mask value for this 
+        // module and return TRUE.
+        //
+        mGlobalErrorLevelInitialized = TRUE;    
+        mDebugPrintErrorLevel = ErrorLevel;
+        return TRUE;
+      }
+    }
+  }
+  //
+  // Return FALSE since the EFI Variable could not be updated.
+  //
+  return FALSE;
+}
+
+/**
+  Retrieves the current debug print error level mask for a module are returns
+  it in CurrentDebugMask.
+
+  @param  This              The protocol instance pointer.
+  @param  CurrentDebugMask  Pointer to the debug print error level mask that 
+                            is returned.
+
+  @retval EFI_SUCCESS            The current debug print error level mask was
+                                 returned in CurrentDebugMask.
+  @retval EFI_INVALID_PARAMETER  CurrentDebugMask is NULL.
+  @retval EFI_DEVICE_ERROR       The current debug print error level mask could
+                                 not be retrieved.
+
+**/
+EFI_STATUS
+EFIAPI
+GetDebugMask (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,             
+  IN OUT UINTN                *CurrentDebugMask  
+  )
+{
+  if (CurrentDebugMask == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Retrieve the current debug mask from mDebugPrintErrorLevel
+  //
+  *CurrentDebugMask = (UINTN)mDebugPrintErrorLevel;
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the current debug print error level mask for a module to the value
+  specified by NewDebugMask.
+
+  @param  This          The protocol instance pointer.
+  @param  NewDebugMask  The new debug print error level mask for this module.
+
+  @retval EFI_SUCCESS            The current debug print error level mask was
+                                 set to the value specified by NewDebugMask.
+  @retval EFI_DEVICE_ERROR       The current debug print error level mask could
+                                 not be set to the value specified by NewDebugMask.
+
+**/
+EFI_STATUS
+EFIAPI
+SetDebugMask (
+  IN EFI_DEBUG_MASK_PROTOCOL  *This,
+  IN UINTN                    NewDebugMask
+  )
+{
+  //
+  // Store the new debug mask into mDebugPrintErrorLevel
+  //
+  mDebugPrintErrorLevel = (UINT32)NewDebugMask;
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.inf b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.inf
new file mode 100644
index 0000000000..b471af7a88
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.inf
@@ -0,0 +1,54 @@
+## @file
+#  Debug Print Error Level library instance that provide compatibility with the "err" shell command.
+#  This includes support for the Debug Mask Protocol supports for global debug print error level mask
+#  stored in an EFI Variable. This library instance only support DXE Phase modules.
+#
+#  Copyright (c) 2011 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeDebugPrintErrorLevelLib
+  MODULE_UNI_FILE                = DxeDebugPrintErrorLevelLib.uni
+  FILE_GUID                      = 1D564EC9-9373-49a4-9E3F-E4D7B9974C84
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = DebugPrintErrorLevelLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER UEFI_APPLICATION UEFI_DRIVER 
+  CONSTRUCTOR                    = DxeDebugPrintErrorLevelLibConstructor
+  DESTRUCTOR                     = DxeDebugPrintErrorLevelLibDestructor
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeDebugPrintErrorLevelLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  HobLib
+  
+[Protocols]
+  gEfiDebugMaskProtocolGuid     ## PRODUCES
+  
+[Guids]
+  ## SOMETIMES_PRODUCES   ## Variable:L"EFIDebug"
+  ## SOMETIMES_CONSUMES   ## Variable:L"EFIDebug"
+  ## SOMETIMES_CONSUMES   ## HOB
+  gEfiGenericVariableGuid
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdDebugPrintErrorLevel  ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.uni b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.uni
new file mode 100644
index 0000000000..b1e53686ae
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeDpcLib/DpcLib.c b/Core/MdeModulePkg/Library/DxeDpcLib/DpcLib.c
new file mode 100644
index 0000000000..6eda8ca0fb
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDpcLib/DpcLib.c
@@ -0,0 +1,100 @@
+/** @file
+  Help functions to access UDP service.
+  
+Copyright (c) 2005 - 2007, 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 <Uefi.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Protocol/Dpc.h>
+
+//
+// Pointer to the DPC Protocol
+//
+EFI_DPC_PROTOCOL  *mDpc;
+
+/**
+  This constructor function caches the EFI_DPC_PROTOCOL pointer.
+
+  @param[in] ImageHandle The firmware allocated handle for the EFI image.
+  @param[in] SystemTable A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS The constructor always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Locate the EFI_DPC_PROTOCOL in the handle database
+  //
+  Status = gBS->LocateProtocol (&gEfiDpcProtocolGuid, NULL, (VOID **)&mDpc);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Add a Deferred Procedure Call to the end of the DPC queue.
+
+  @param[in]  DpcTpl        The EFI_TPL that the DPC should be invoked.
+  @param[in]  DpcProcedure  Pointer to the DPC's function.
+  @param[in]  DpcContext    Pointer to the DPC's context.  Passed to DpcProcedure
+                            when DpcProcedure is invoked.
+
+  @retval EFI_SUCCESS            The DPC was queued.
+  @retval EFI_INVALID_PARAMETER  DpcTpl is not a valid EFI_TPL.
+  @retval EFI_INVALID_PARAMETER  DpcProcedure is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to
+                                 add the DPC to the queue.
+
+**/
+EFI_STATUS
+EFIAPI
+QueueDpc (
+  IN EFI_TPL            DpcTpl,
+  IN EFI_DPC_PROCEDURE  DpcProcedure,
+  IN VOID               *DpcContext    OPTIONAL
+  )
+{
+  //
+  // Call the EFI_DPC_PROTOCOL to queue the DPC
+  //
+  return mDpc->QueueDpc (mDpc, DpcTpl, DpcProcedure, DpcContext);
+}
+
+/**
+  Dispatch the queue of DPCs.  ALL DPCs that have been queued with a DpcTpl
+  value greater than or equal to the current TPL are invoked in the order that
+  they were queued.  DPCs with higher DpcTpl values are invoked before DPCs with
+  lower DpcTpl values.
+
+  @retval EFI_SUCCESS    One or more DPCs were invoked.
+  @retval EFI_NOT_FOUND  No DPCs were invoked.
+
+**/
+EFI_STATUS
+EFIAPI
+DispatchDpc (
+  VOID
+  )
+{
+  //
+  // Call the EFI_DPC_PROTOCOL to dispatch previously queued DPCs
+  //
+  return mDpc->DispatchDpc (mDpc);
+}
diff --git a/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.inf b/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.inf
new file mode 100644
index 0000000000..d541acd5a9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.inf
@@ -0,0 +1,46 @@
+## @file
+#  This library instance provides DPC service by consuming EFI DPC Protocol.
+#
+#  Copyright (c) 2007 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeDpcLib
+  MODULE_UNI_FILE                = DxeDpcLib.uni
+  FILE_GUID                      = 38897D86-FF36-4472-AE64-1DB9AE715C81
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = DpcLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+  CONSTRUCTOR                    = DpcLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DpcLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DebugLib
+  UefiBootServicesTableLib
+
+[Protocols]
+  gEfiDpcProtocolGuid                  ## CONSUMES
+
+[Depex.common.DXE_DRIVER, Depex.common.DXE_RUNTIME_DRIVER, Depex.common.DXE_SAL_DRIVER, Depex.common.DXE_SMM_DRIVER]
+  gEfiDpcProtocolGuid
diff --git a/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.uni b/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.uni
new file mode 100644
index 0000000000..3624f669f8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.c b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.c
new file mode 100644
index 0000000000..143baabdec
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.c
@@ -0,0 +1,1427 @@
+/** @file
+  This library is used to share code between UEFI network stack modules.
+  It provides the helper routines to parse the HTTP message byte stream.
+
+Copyright (c) 2015, 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<BR>
+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 <Uefi.h>
+#include <Library/NetLib.h>
+#include <Library/HttpLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#define BIT(x)  (1 << x)
+
+#define NET_IS_HEX_CHAR(Ch)   \
+  ((('0' <= (Ch)) && ((Ch) <= '9')) ||  \
+   (('A' <= (Ch)) && ((Ch) <= 'F')) ||  \
+   (('a' <= (Ch)) && ((Ch) <= 'f')))
+
+//
+// Field index of the HTTP URL parse result.
+//
+#define   HTTP_URI_FIELD_SCHEME           0
+#define   HTTP_URI_FIELD_AUTHORITY        1
+#define   HTTP_URI_FIELD_PATH             2
+#define   HTTP_URI_FIELD_QUERY            3
+#define   HTTP_URI_FIELD_FRAGMENT         4
+#define   HTTP_URI_FIELD_USERINFO         5
+#define   HTTP_URI_FIELD_HOST             6
+#define   HTTP_URI_FIELD_PORT             7
+#define   HTTP_URI_FIELD_MAX              8
+
+//
+// Structure to store the parse result of a HTTP URL.
+//
+typedef struct {
+    UINT32      Offset;
+    UINT32      Length;
+} HTTP_URL_FILED_DATA;
+
+typedef struct {
+  UINT16                  FieldBitMap;
+  HTTP_URL_FILED_DATA     FieldData[HTTP_URI_FIELD_MAX];
+} HTTP_URL_PARSER;
+
+typedef enum {
+  UrlParserUrlStart,
+  UrlParserScheme,
+  UrlParserSchemeColon,            // ":"
+  UrlParserSchemeColonSlash,       // ":/"
+  UrlParserSchemeColonSlashSlash,  // "://"
+  UrlParserAuthority,
+  UrlParserAtInAuthority,
+  UrlParserPath,
+  UrlParserQueryStart,    // "?"
+  UrlParserQuery,
+  UrlParserFragmentStart, // "#"
+  UrlParserFragment,
+  UrlParserUserInfo,
+  UrlParserHostStart,     // "@"
+  UrlParserHost,
+  UrlParserHostIpv6,      // "["(Ipv6 address) "]"
+  UrlParserPortStart,     // ":"
+  UrlParserPort,
+  UrlParserStateMax
+} HTTP_URL_PARSE_STATE;
+
+/**
+  Decode a percent-encoded URI component to the ASCII character.
+  
+  Decode the input component in Buffer according to RFC 3986. The caller is responsible to make 
+  sure ResultBuffer points to a buffer with size equal or greater than ((AsciiStrSize (Buffer))
+  in bytes. 
+
+  @param[in]    Buffer           The pointer to a percent-encoded URI component.
+  @param[in]    BufferLength     Length of Buffer in bytes.
+  @param[out]   ResultBuffer     Point to the buffer to store the decode result.
+  @param[out]   ResultLength     Length of decoded string in ResultBuffer in bytes.
+
+  @retval EFI_SUCCESS            Successfully decoded the URI.
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid percent-encoded string.
+  
+**/
+EFI_STATUS
+EFIAPI
+UriPercentDecode (
+  IN      CHAR8            *Buffer,
+  IN      UINT32            BufferLength,
+     OUT  CHAR8            *ResultBuffer,
+     OUT  UINT32           *ResultLength
+  )
+{
+  UINTN           Index;
+  UINTN           Offset;
+  CHAR8           HexStr[3];
+
+  if (Buffer == NULL || BufferLength == 0 || ResultBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Index = 0;
+  Offset = 0;
+  HexStr[2] = '\0';
+  while (Index < BufferLength) {
+    if (Buffer[Index] == '%') {
+      if (!NET_IS_HEX_CHAR (Buffer[Index+1]) || !NET_IS_HEX_CHAR (Buffer[Index+2])) {
+        return EFI_INVALID_PARAMETER;
+      }
+      HexStr[0] = Buffer[Index+1];
+      HexStr[1] = Buffer[Index+2];
+      ResultBuffer[Offset] = (CHAR8) AsciiStrHexToUintn (HexStr);
+      Index += 3;
+    } else {
+      ResultBuffer[Offset] = Buffer[Index];
+      Index++;
+    }
+    Offset++;
+  }
+
+  *ResultLength = (UINT32) Offset;
+    
+  return EFI_SUCCESS;
+}
+
+/**
+  This function return the updated state accroding to the input state and next character of
+  the authority.
+
+  @param[in]       Char           Next character.
+  @param[in]       State          Current value of the parser state machine.
+  @param[in]       IsRightBracket TRUE if there is an sign ']' in the authority component and 
+                                  indicates the next part is ':' before Port.                                
+
+  @return          Updated state value.
+**/
+HTTP_URL_PARSE_STATE
+NetHttpParseAuthorityChar (
+  IN  CHAR8                  Char,
+  IN  HTTP_URL_PARSE_STATE   State,
+  IN  BOOLEAN                *IsRightBracket
+  )
+{
+
+  //
+  // RFC 3986:
+  // The authority component is preceded by a double slash ("//") and is
+  // terminated by the next slash ("/"), question mark ("?"), or number
+  // sign ("#") character, or by the end of the URI.
+  //
+  if (Char == ' ' || Char == '\r' || Char == '\n') {
+    return UrlParserStateMax;
+  }
+
+  //
+  // authority   = [ userinfo "@" ] host [ ":" port ]
+  //
+  switch (State) {
+  case UrlParserUserInfo:
+    if (Char == '@') {
+      return UrlParserHostStart;
+    }
+    break;
+
+  case UrlParserHost:
+  case UrlParserHostStart:  
+    if (Char == '[') {
+      return UrlParserHostIpv6;
+    }
+    
+    if (Char == ':') {
+      return UrlParserPortStart;
+    }
+    
+    return UrlParserHost;
+    
+  case UrlParserHostIpv6:  
+    if (Char == ']') {
+      *IsRightBracket = TRUE;
+    }
+    
+    if (Char == ':' && *IsRightBracket) {
+      return UrlParserPortStart;
+    }
+    return UrlParserHostIpv6;
+    
+  case UrlParserPort:
+  case UrlParserPortStart:
+    return UrlParserPort;
+
+  default:
+    break;
+  }
+
+  return State;
+}
+
+/**
+  This function parse the authority component of the input URL and update the parser.
+
+  @param[in]       Url            The pointer to a HTTP URL string.
+  @param[in]       FoundAt        TRUE if there is an at sign ('@') in the authority component.
+  @param[in, out]  UrlParser      Pointer to the buffer of the parse result.
+
+  @retval EFI_SUCCESS             Successfully parse the authority.
+  @retval Other                   Error happened.
+
+**/
+EFI_STATUS
+NetHttpParseAuthority (
+  IN      CHAR8              *Url,
+  IN      BOOLEAN            FoundAt,
+  IN OUT  HTTP_URL_PARSER    *UrlParser
+  )
+{
+  CHAR8                 *Char;
+  CHAR8                 *Authority;
+  UINT32                Length;
+  HTTP_URL_PARSE_STATE  State;
+  UINT32                Field;
+  UINT32                OldField;
+  BOOLEAN               IsrightBracket;
+  
+  ASSERT ((UrlParser->FieldBitMap & BIT (HTTP_URI_FIELD_AUTHORITY)) != 0);
+
+  //
+  // authority   = [ userinfo "@" ] host [ ":" port ]
+  //
+  if (FoundAt) {
+    State = UrlParserUserInfo;
+  } else {
+    State = UrlParserHost;
+  }
+
+  IsrightBracket = FALSE;
+  Field = HTTP_URI_FIELD_MAX;
+  OldField = Field;
+  Authority = Url + UrlParser->FieldData[HTTP_URI_FIELD_AUTHORITY].Offset;
+  Length = UrlParser->FieldData[HTTP_URI_FIELD_AUTHORITY].Length;
+  for (Char = Authority; Char < Authority + Length; Char++) {
+    State = NetHttpParseAuthorityChar (*Char, State, &IsrightBracket);
+    switch (State) {
+    case UrlParserStateMax:
+      return EFI_INVALID_PARAMETER;
+
+    case UrlParserHostStart:
+    case UrlParserPortStart:
+      continue;
+
+    case UrlParserUserInfo:
+      Field = HTTP_URI_FIELD_USERINFO;
+      break;
+      
+    case UrlParserHost:
+      Field = HTTP_URI_FIELD_HOST;
+      break;
+
+    case UrlParserHostIpv6:
+      Field = HTTP_URI_FIELD_HOST;
+      break;
+      
+    case UrlParserPort:
+      Field = HTTP_URI_FIELD_PORT;
+      break;
+
+    default:
+      ASSERT (FALSE);
+    }
+
+    //
+    // Field not changed, count the length.
+    //
+    ASSERT (Field < HTTP_URI_FIELD_MAX);
+    if (Field == OldField) {
+      UrlParser->FieldData[Field].Length++;
+      continue;
+    }
+
+    //
+    // New field start
+    //
+    UrlParser->FieldBitMap |= BIT (Field);
+    UrlParser->FieldData[Field].Offset = (UINT32) (Char - Url);
+    UrlParser->FieldData[Field].Length = 1;
+    OldField = Field;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function return the updated state accroding to the input state and next character of a URL.
+
+  @param[in]       Char           Next character.
+  @param[in]       State          Current value of the parser state machine.
+
+  @return          Updated state value.
+
+**/
+HTTP_URL_PARSE_STATE
+NetHttpParseUrlChar (
+  IN  CHAR8                  Char,
+  IN  HTTP_URL_PARSE_STATE   State
+  )
+{
+  if (Char == ' ' || Char == '\r' || Char == '\n') {
+    return UrlParserStateMax;
+  }
+  
+  //
+  // http_URL = "http:" "//" host [ ":" port ] [ abs_path [ "?" query ]]
+  // 
+  // Request-URI    = "*" | absolute-URI | path-absolute | authority
+  // 
+  // absolute-URI  = scheme ":" hier-part [ "?" query ]
+  // path-absolute = "/" [ segment-nz *( "/" segment ) ]
+  // authority   = [ userinfo "@" ] host [ ":" port ]
+  //
+  switch (State) {
+  case UrlParserUrlStart:
+    if (Char == '*' || Char == '/') {
+      return UrlParserPath;
+    }
+    return UrlParserScheme;
+
+  case UrlParserScheme:
+    if (Char == ':') {
+      return UrlParserSchemeColon;
+    }
+    break;
+
+  case UrlParserSchemeColon:
+    if (Char == '/') {
+      return UrlParserSchemeColonSlash;
+    }
+    break;
+
+  case UrlParserSchemeColonSlash:
+    if (Char == '/') {
+      return UrlParserSchemeColonSlashSlash;
+    }
+    break;
+
+  case UrlParserAtInAuthority:
+    if (Char == '@') {
+      return UrlParserStateMax;
+    }
+
+  case UrlParserAuthority:
+  case UrlParserSchemeColonSlashSlash:
+    if (Char == '@') {
+      return UrlParserAtInAuthority;
+    }
+    if (Char == '/') {
+      return UrlParserPath;
+    }
+    if (Char == '?') {
+      return UrlParserQueryStart;
+    }
+    if (Char == '#') {
+      return UrlParserFragmentStart;
+    }
+    return UrlParserAuthority;
+
+  case UrlParserPath:
+    if (Char == '?') {
+      return UrlParserQueryStart;
+    }
+    if (Char == '#') {
+      return UrlParserFragmentStart;
+    }
+    break;
+
+  case UrlParserQuery:
+  case UrlParserQueryStart:
+    if (Char == '#') {
+      return UrlParserFragmentStart;
+    }
+    return UrlParserQuery;
+
+  case UrlParserFragmentStart:
+    return UrlParserFragment;
+    
+  default:
+    break;
+  }
+
+  return State;
+}
+/**
+  Create a URL parser for the input URL string.
+
+  This function will parse and dereference the input HTTP URL into it components. The original
+  content of the URL won't be modified and the result will be returned in UrlParser, which can
+  be used in other functions like NetHttpUrlGetHostName().
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    Length             Length of Url in bytes.
+  @param[in]    IsConnectMethod    Whether the Url is used in HTTP CONNECT method or not.
+  @param[out]   UrlParser          Pointer to the returned buffer to store the parse result.
+
+  @retval EFI_SUCCESS              Successfully dereferenced the HTTP URL.
+  @retval EFI_INVALID_PARAMETER    UrlParser is NULL or Url is not a valid HTTP URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+
+**/
+EFI_STATUS
+EFIAPI
+HttpParseUrl (
+  IN      CHAR8              *Url,
+  IN      UINT32             Length,
+  IN      BOOLEAN            IsConnectMethod,
+     OUT  VOID               **UrlParser
+  )
+{
+  HTTP_URL_PARSE_STATE  State;
+  CHAR8                 *Char;
+  UINT32                Field;
+  UINT32                OldField;
+  BOOLEAN               FoundAt;
+  EFI_STATUS            Status;
+  HTTP_URL_PARSER       *Parser;
+  
+  if (Url == NULL || Length == 0 || UrlParser == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = AllocateZeroPool (sizeof (HTTP_URL_PARSER));
+  if (Parser == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  if (IsConnectMethod) {
+    //
+    // According to RFC 2616, the authority form is only used by the CONNECT method.
+    //
+    State = UrlParserAuthority;
+  } else {
+    State = UrlParserUrlStart;
+  }
+
+  Field = HTTP_URI_FIELD_MAX;
+  OldField = Field;
+  FoundAt = FALSE;
+  for (Char = Url; Char < Url + Length; Char++) {
+    //
+    // Update state machine accoring to next char.
+    //
+    State = NetHttpParseUrlChar (*Char, State);
+
+    switch (State) {
+    case UrlParserStateMax:
+      return EFI_INVALID_PARAMETER;
+      
+    case UrlParserSchemeColon:
+    case UrlParserSchemeColonSlash:
+    case UrlParserSchemeColonSlashSlash:
+    case UrlParserQueryStart:
+    case UrlParserFragmentStart:
+      //
+      // Skip all the delimiting char: "://" "?" "@"
+      //
+      continue;
+    
+    case UrlParserScheme:
+      Field = HTTP_URI_FIELD_SCHEME;
+      break;
+
+    case UrlParserAtInAuthority:
+      FoundAt = TRUE;
+    case UrlParserAuthority:
+      Field = HTTP_URI_FIELD_AUTHORITY;
+      break;
+
+    case UrlParserPath:
+      Field = HTTP_URI_FIELD_PATH;
+      break;
+
+    case UrlParserQuery:
+      Field = HTTP_URI_FIELD_QUERY;
+      break;
+
+    case UrlParserFragment:
+      Field = HTTP_URI_FIELD_FRAGMENT;
+      break;
+
+    default:
+      ASSERT (FALSE);
+    }
+
+    //
+    // Field not changed, count the length.
+    //
+    ASSERT (Field < HTTP_URI_FIELD_MAX);
+    if (Field == OldField) {
+      Parser->FieldData[Field].Length++;
+      continue;
+    }
+
+    //
+    // New field start
+    //
+    Parser->FieldBitMap |= BIT (Field);
+    Parser->FieldData[Field].Offset = (UINT32) (Char - Url);
+    Parser->FieldData[Field].Length = 1;
+    OldField = Field;
+  }
+
+  //
+  // If has authority component, continue to parse the username, host and port.
+  //
+  if ((Parser->FieldBitMap & BIT (HTTP_URI_FIELD_AUTHORITY)) != 0) {
+    Status = NetHttpParseAuthority (Url, FoundAt, Parser);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  *UrlParser = Parser;
+  return EFI_SUCCESS;  
+}
+
+/**
+  Get the Hostname from a HTTP URL.
+
+  This function will return the HostName according to the Url and previous parse result ,and
+  it is the caller's responsibility to free the buffer returned in *HostName.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   HostName           Pointer to a buffer to store the HostName.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or HostName is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No hostName component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetHostName (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  CHAR8              **HostName
+  )
+{
+  CHAR8                *Name;
+  EFI_STATUS           Status;
+  UINT32               ResultLength;
+  HTTP_URL_PARSER      *Parser;
+
+  if (Url == NULL || UrlParser == NULL || HostName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_URL_PARSER*) UrlParser;
+
+  if ((Parser->FieldBitMap & BIT (HTTP_URI_FIELD_HOST)) == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  Name = AllocatePool (Parser->FieldData[HTTP_URI_FIELD_HOST].Length + 1);
+  if (Name == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  Status = UriPercentDecode (
+             Url + Parser->FieldData[HTTP_URI_FIELD_HOST].Offset,
+             Parser->FieldData[HTTP_URI_FIELD_HOST].Length,
+             Name,
+             &ResultLength
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Name[ResultLength] = '\0';
+  *HostName = Name;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the IPv4 address from a HTTP URL.
+
+  This function will return the IPv4 address according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Ip4Address         Pointer to a buffer to store the IP address.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Ip4Address is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No IPv4 address component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetIp4 (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  EFI_IPv4_ADDRESS   *Ip4Address
+  )
+{
+  CHAR8                *Ip4String;
+  EFI_STATUS           Status;
+  UINT32               ResultLength;
+  HTTP_URL_PARSER      *Parser;
+  
+  if (Url == NULL || UrlParser == NULL || Ip4Address == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_URL_PARSER*) UrlParser;
+
+  if ((Parser->FieldBitMap & BIT (HTTP_URI_FIELD_HOST)) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ip4String = AllocatePool (Parser->FieldData[HTTP_URI_FIELD_HOST].Length + 1);
+  if (Ip4String == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  Status = UriPercentDecode (
+             Url + Parser->FieldData[HTTP_URI_FIELD_HOST].Offset,
+             Parser->FieldData[HTTP_URI_FIELD_HOST].Length,
+             Ip4String,
+             &ResultLength
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Ip4String[ResultLength] = '\0';
+  Status = NetLibAsciiStrToIp4 (Ip4String, Ip4Address);
+  FreePool (Ip4String);
+
+  return Status;
+}
+
+/**
+  Get the IPv6 address from a HTTP URL.
+
+  This function will return the IPv6 address according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Ip6Address         Pointer to a buffer to store the IP address.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Ip6Address is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No IPv6 address component in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetIp6 (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  EFI_IPv6_ADDRESS   *Ip6Address
+  )
+{
+  CHAR8                *Ip6String;
+  CHAR8                *Ptr;
+  UINT32               Length;
+  EFI_STATUS           Status;
+  UINT32               ResultLength;
+  HTTP_URL_PARSER      *Parser;
+  
+  if (Url == NULL || UrlParser == NULL || Ip6Address == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_URL_PARSER*) UrlParser;
+
+  if ((Parser->FieldBitMap & BIT (HTTP_URI_FIELD_HOST)) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // IP-literal = "[" ( IPv6address / IPvFuture  ) "]"
+  //
+  Length = Parser->FieldData[HTTP_URI_FIELD_HOST].Length;
+  if (Length < 2) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ptr    = Url + Parser->FieldData[HTTP_URI_FIELD_HOST].Offset;
+  if ((Ptr[0] != '[') || (Ptr[Length - 1] != ']')) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ip6String = AllocatePool (Length);
+  if (Ip6String == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  Status = UriPercentDecode (
+             Ptr + 1,
+             Length - 2,
+             Ip6String,
+             &ResultLength
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Ip6String[ResultLength] = '\0';
+  Status = NetLibAsciiStrToIp6 (Ip6String, Ip6Address);
+  FreePool (Ip6String);
+
+  return Status;
+}
+
+/**
+  Get the port number from a HTTP URL.
+
+  This function will return the port number according to the Url and previous parse result.
+
+  @param[in]    Url                The pointer to a HTTP URL string.
+  @param[in]    UrlParser          URL Parse result returned by NetHttpParseUrl().
+  @param[out]   Port               Pointer to a buffer to store the port number.
+
+  @retval EFI_SUCCESS              Successfully get the required component.
+  @retval EFI_INVALID_PARAMETER    Uri is NULL or Port is NULL or UrlParser is invalid.
+  @retval EFI_NOT_FOUND            No port number in the URL.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpUrlGetPort (
+  IN      CHAR8              *Url,
+  IN      VOID               *UrlParser,
+     OUT  UINT16             *Port
+  )
+{
+  CHAR8         *PortString;
+  EFI_STATUS    Status;
+  UINT32        ResultLength;
+  HTTP_URL_PARSER      *Parser;
+
+  if (Url == NULL || UrlParser == NULL || Port == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_URL_PARSER*) UrlParser;
+
+  if ((Parser->FieldBitMap & BIT (HTTP_URI_FIELD_PORT)) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PortString = AllocatePool (Parser->FieldData[HTTP_URI_FIELD_PORT].Length + 1);
+  if (PortString == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = UriPercentDecode (
+             Url + Parser->FieldData[HTTP_URI_FIELD_PORT].Offset,
+             Parser->FieldData[HTTP_URI_FIELD_PORT].Length,
+             PortString,
+             &ResultLength
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PortString[ResultLength] = '\0';
+  *Port = (UINT16) AsciiStrDecimalToUintn (Url + Parser->FieldData[HTTP_URI_FIELD_PORT].Offset);
+
+  return  EFI_SUCCESS;
+}
+
+/**
+  Release the resource of the URL parser.
+
+  @param[in]    UrlParser            Pointer to the parser.
+  
+**/
+VOID
+EFIAPI
+HttpUrlFreeParser (
+  IN      VOID               *UrlParser
+  )
+{
+  FreePool (UrlParser);
+}
+
+/**
+  Find a specified header field according to the field name.
+
+  @param[in]   HeaderCount      Number of HTTP header structures in Headers list. 
+  @param[in]   Headers          Array containing list of HTTP headers.
+  @param[in]   FieldName        Null terminated string which describes a field name. 
+
+  @return    Pointer to the found header or NULL.
+
+**/
+EFI_HTTP_HEADER *
+HttpIoFindHeader (
+  IN  UINTN                HeaderCount,
+  IN  EFI_HTTP_HEADER      *Headers,
+  IN  CHAR8                *FieldName
+  )
+{
+  UINTN                 Index;
+  
+  if (HeaderCount == 0 || Headers == NULL || FieldName == NULL) {
+    return NULL;
+  }
+
+  for (Index = 0; Index < HeaderCount; Index++){
+    //
+    // Field names are case-insensitive (RFC 2616).
+    //
+    if (AsciiStriCmp (Headers[Index].FieldName, FieldName) == 0) {
+      return &Headers[Index];
+    }
+  }
+  return NULL;
+}
+
+typedef enum {
+  BodyParserBodyStart,
+  BodyParserBodyIdentity,
+  BodyParserChunkSizeStart,
+  BodyParserChunkSize,
+  BodyParserChunkSizeEndCR,
+  BodyParserChunkExtStart,
+  BodyParserChunkDataStart,
+  BodyParserChunkDataEnd,
+  BodyParserChunkDataEndCR,
+  BodyParserTrailer,
+  BodyParserLastCRLF,
+  BodyParserLastCRLFEnd,
+  BodyParserComplete,
+  BodyParserStateMax
+} HTTP_BODY_PARSE_STATE;
+
+typedef struct {
+  BOOLEAN                       IgnoreBody;    // "MUST NOT" include a message-body
+  BOOLEAN                       IsChunked;     // "chunked" transfer-coding.
+  BOOLEAN                       ContentLengthIsValid;
+  UINTN                         ContentLength; // Entity length (not the message-body length), invalid until ContentLengthIsValid is TRUE
+  
+  HTTP_BODY_PARSER_CALLBACK     Callback;
+  VOID                          *Context;
+  UINTN                         ParsedBodyLength;
+  HTTP_BODY_PARSE_STATE         State;
+  UINTN                         CurrentChunkSize;
+  UINTN                         CurrentChunkParsedSize;
+} HTTP_BODY_PARSER;
+
+/**
+
+  Convert an Ascii char to its uppercase.
+
+  @param[in]       Char           Ascii character.
+
+  @return          Uppercase value of the input Char.
+
+**/
+CHAR8
+HttpIoCharToUpper (
+  IN      CHAR8                    Char
+  )
+{
+  if (Char >= 'a' && Char <= 'z') {
+    return  Char - ('a' - 'A');
+  }
+
+  return Char;
+}
+
+/**
+  Convert an hexadecimal char to a value of type UINTN.
+
+  @param[in]       Char           Ascii character.
+
+  @return          Value translated from Char.
+
+**/
+UINTN
+HttpIoHexCharToUintn (
+  IN CHAR8           Char
+  )
+{
+  if (Char >= '0' && Char <= '9') {
+    return Char - '0';
+  }
+
+  return (10 + HttpIoCharToUpper (Char) - 'A');
+}
+
+/**
+  Get the value of the content length if there is a "Content-Length" header.
+
+  @param[in]    HeaderCount        Number of HTTP header structures in Headers.
+  @param[in]    Headers            Array containing list of HTTP headers.
+  @param[out]   ContentLength      Pointer to save the value of the content length.
+
+  @retval EFI_SUCCESS              Successfully get the content length.
+  @retval EFI_NOT_FOUND            No "Content-Length" header in the Headers.
+
+**/
+EFI_STATUS
+HttpIoParseContentLengthHeader (
+  IN     UINTN                HeaderCount,
+  IN     EFI_HTTP_HEADER      *Headers,
+     OUT UINTN                *ContentLength
+  )
+{
+  EFI_HTTP_HEADER       *Header;
+  
+  Header = HttpIoFindHeader (HeaderCount, Headers, "Content-Length");
+  if (Header == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *ContentLength = AsciiStrDecimalToUintn (Header->FieldValue);
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Check whether the HTTP message is using the "chunked" transfer-coding.
+
+  @param[in]    HeaderCount        Number of HTTP header structures in Headers.
+  @param[in]    Headers            Array containing list of HTTP headers.
+
+  @return       The message is "chunked" transfer-coding (TRUE) or not (FALSE).
+ 
+**/
+BOOLEAN
+HttpIoIsChunked (
+  IN   UINTN                    HeaderCount,
+  IN   EFI_HTTP_HEADER          *Headers
+  )
+{
+  EFI_HTTP_HEADER       *Header;
+
+
+  Header = HttpIoFindHeader (HeaderCount, Headers, "Transfer-Encoding");
+  if (Header == NULL) {
+    return FALSE;
+  }
+
+  if (AsciiStriCmp (Header->FieldValue, "identity") != 0) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether the HTTP message should have a message-body.
+
+  @param[in]    Method             The HTTP method (e.g. GET, POST) for this HTTP message.
+  @param[in]    StatusCode         Response status code returned by the remote host.
+
+  @return       The message should have a message-body (FALSE) or not (TRUE).
+
+**/
+BOOLEAN
+HttpIoNoMessageBody (
+  IN   EFI_HTTP_METHOD          Method,
+  IN   EFI_HTTP_STATUS_CODE     StatusCode
+  )
+{
+  //
+  // RFC 2616:
+  // All responses to the HEAD request method
+  // MUST NOT include a message-body, even though the presence of entity-
+  // header fields might lead one to believe they do. All 1xx
+  // (informational), 204 (no content), and 304 (not modified) responses
+  // MUST NOT include a message-body. All other responses do include a
+  // message-body, although it MAY be of zero length.
+  //
+  if (Method == HttpMethodHead) {
+    return TRUE;
+  }
+
+  if ((StatusCode == HTTP_STATUS_100_CONTINUE) ||
+      (StatusCode == HTTP_STATUS_101_SWITCHING_PROTOCOLS) ||
+      (StatusCode == HTTP_STATUS_204_NO_CONTENT) ||
+      (StatusCode == HTTP_STATUS_304_NOT_MODIFIED))
+  {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Initialize a HTTP message-body parser.
+
+  This function will create and initialize a HTTP message parser according to caller provided HTTP message
+  header information. It is the caller's responsibility to free the buffer returned in *UrlParser by HttpFreeMsgParser().
+
+  @param[in]    Method             The HTTP method (e.g. GET, POST) for this HTTP message.
+  @param[in]    StatusCode         Response status code returned by the remote host.
+  @param[in]    HeaderCount        Number of HTTP header structures in Headers.
+  @param[in]    Headers            Array containing list of HTTP headers.
+  @param[in]    Callback           Callback function that is invoked when parsing the HTTP message-body,
+                                   set to NULL to ignore all events.
+  @param[in]    Context            Pointer to the context that will be passed to Callback.
+  @param[out]   MsgParser          Pointer to the returned buffer to store the message parser.
+
+  @retval EFI_SUCCESS              Successfully initialized the parser.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate needed resources.
+  @retval EFI_INVALID_PARAMETER    MsgParser is NULL or HeaderCount is not NULL but Headers is NULL.
+  @retval Others                   Failed to initialize the parser.
+
+**/
+EFI_STATUS
+EFIAPI
+HttpInitMsgParser (
+  IN     EFI_HTTP_METHOD               Method,
+  IN     EFI_HTTP_STATUS_CODE          StatusCode,
+  IN     UINTN                         HeaderCount,
+  IN     EFI_HTTP_HEADER               *Headers,
+  IN     HTTP_BODY_PARSER_CALLBACK     Callback,
+  IN     VOID                          *Context,
+    OUT  VOID                          **MsgParser
+  )
+{
+  EFI_STATUS            Status;
+  HTTP_BODY_PARSER      *Parser;
+  
+  if (HeaderCount != 0 && Headers == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MsgParser == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = AllocateZeroPool (sizeof (HTTP_BODY_PARSER));
+  if (Parser == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Parser->State = BodyParserBodyStart;
+  
+  //
+  // Determine the message length accroding to RFC 2616.
+  // 1. Check whether the message "MUST NOT" have a message-body.
+  //
+  Parser->IgnoreBody = HttpIoNoMessageBody (Method, StatusCode);
+  //
+  // 2. Check whether the message using "chunked" transfer-coding.
+  //
+  Parser->IsChunked  = HttpIoIsChunked (HeaderCount, Headers);
+  //
+  // 3. Check whether the message has a Content-Length header field.
+  //
+  Status = HttpIoParseContentLengthHeader (HeaderCount, Headers, &Parser->ContentLength);
+  if (!EFI_ERROR (Status)) {
+    Parser->ContentLengthIsValid = TRUE;
+  }
+  //
+  // 4. Range header is not supported now, so we won't meet media type "multipart/byteranges".
+  // 5. By server closing the connection
+  //
+  
+  //
+  // Set state to skip body parser if the message shouldn't have a message body.
+  //
+  if (Parser->IgnoreBody) {
+    Parser->State = BodyParserComplete;
+  } else {
+    Parser->Callback = Callback;
+    Parser->Context  = Context;
+  }
+
+  *MsgParser = Parser;
+  return EFI_SUCCESS;
+}
+
+/**
+  Parse message body.
+
+  Parse BodyLength of message-body. This function can be called repeatedly to parse the message-body partially.
+
+  @param[in, out]    MsgParser            Pointer to the message parser.
+  @param[in]         BodyLength           Length in bytes of the Body.
+  @param[in]         Body                 Pointer to the buffer of the message-body to be parsed.
+
+  @retval EFI_SUCCESS                Successfully parse the message-body.
+  @retval EFI_INVALID_PARAMETER      MsgParser is NULL or Body is NULL or BodyLength is 0.
+  @retval Others                     Operation aborted.
+
+**/
+EFI_STATUS
+EFIAPI
+HttpParseMessageBody (
+  IN OUT VOID              *MsgParser,
+  IN     UINTN             BodyLength,
+  IN     CHAR8             *Body
+  )
+{
+  CHAR8                 *Char;
+  UINTN                 RemainderLengthInThis;
+  UINTN                 LengthForCallback;
+  EFI_STATUS            Status;
+  HTTP_BODY_PARSER      *Parser;
+  
+  if (BodyLength == 0 || Body == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MsgParser == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_BODY_PARSER*) MsgParser;
+
+  if (Parser->IgnoreBody) {
+    Parser->State = BodyParserComplete;
+    if (Parser->Callback != NULL) {
+      Status = Parser->Callback (
+                 BodyParseEventOnComplete,
+                 Body,
+                 0,
+                 Parser->Context
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (Parser->State == BodyParserBodyStart) {
+    Parser->ParsedBodyLength = 0;
+    if (Parser->IsChunked) {
+      Parser->State = BodyParserChunkSizeStart;
+    } else {
+      Parser->State = BodyParserBodyIdentity;
+    }
+  }
+
+  //
+  // The message body might be truncated in anywhere, so we need to parse is byte-by-byte.
+  //
+  for (Char = Body; Char < Body + BodyLength; ) {
+
+    switch (Parser->State) {
+    case BodyParserStateMax:
+      return EFI_ABORTED;
+ 
+    case BodyParserBodyIdentity:
+      //
+      // Identity transfer-coding, just notify user to save the body data.
+      //
+      if (Parser->Callback != NULL) {
+        Status = Parser->Callback (
+                   BodyParseEventOnData,
+                   Char,
+                   MIN (BodyLength, Parser->ContentLength - Parser->ParsedBodyLength),
+                   Parser->Context
+                   );
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+      }
+      Char += MIN (BodyLength, Parser->ContentLength - Parser->ParsedBodyLength);
+      Parser->ParsedBodyLength += MIN (BodyLength, Parser->ContentLength - Parser->ParsedBodyLength);
+      if (Parser->ParsedBodyLength == Parser->ContentLength) {
+        Parser->State = BodyParserComplete;
+        if (Parser->Callback != NULL) {
+          Status = Parser->Callback (
+                     BodyParseEventOnComplete,
+                     Char,
+                     0,
+                     Parser->Context
+                     );
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+      }
+      break;
+
+    case BodyParserChunkSizeStart:
+      //
+      // First byte of chunk-size, the chunk-size might be truncated.
+      //
+      Parser->CurrentChunkSize = 0;
+      Parser->State = BodyParserChunkSize;
+    case BodyParserChunkSize:
+      if (!NET_IS_HEX_CHAR (*Char)) {
+        if (*Char == ';') {
+          Parser->State = BodyParserChunkExtStart;
+          Char++;
+        } else if (*Char == '\r') {
+          Parser->State = BodyParserChunkSizeEndCR;
+          Char++;
+        } else {
+          Parser->State = BodyParserStateMax;
+        }
+        break;
+      }
+
+      if (Parser->CurrentChunkSize > (((~((UINTN) 0)) - 16) / 16)) {
+        return EFI_INVALID_PARAMETER;
+      }
+      Parser->CurrentChunkSize = Parser->CurrentChunkSize * 16 + HttpIoHexCharToUintn (*Char);
+      Char++;
+      break;
+
+    case BodyParserChunkExtStart:
+      //
+      // Ignore all the chunk extensions.
+      //
+      if (*Char == '\r') {
+        Parser->State = BodyParserChunkSizeEndCR;
+       }
+      Char++;
+      break;
+      
+    case BodyParserChunkSizeEndCR:
+      if (*Char != '\n') {
+        Parser->State = BodyParserStateMax;
+        break;
+      }
+      Char++;
+      if (Parser->CurrentChunkSize == 0) {
+        //
+        // The last chunk has been parsed and now assumed the state 
+        // of HttpBodyParse is ParserLastCRLF. So it need to decide
+        // whether the rest message is trailer or last CRLF in the next round.
+        //
+        Parser->ContentLengthIsValid = TRUE;
+        Parser->State = BodyParserLastCRLF;
+        break;
+      }
+      Parser->State = BodyParserChunkDataStart;
+      Parser->CurrentChunkParsedSize = 0;
+      break;
+      
+    case BodyParserLastCRLF:
+      //
+      // Judge the byte is belong to the Last CRLF or trailer, and then 
+      // configure the state of HttpBodyParse to corresponding state.
+      //
+      if (*Char == '\r') {
+        Char++;
+        Parser->State = BodyParserLastCRLFEnd;
+        break;
+      } else {
+        Parser->State = BodyParserTrailer;
+        break;
+      }
+      
+    case BodyParserLastCRLFEnd:
+      if (*Char == '\n') {
+        Parser->State = BodyParserComplete;
+        Char++;
+        if (Parser->Callback != NULL) {
+          Status = Parser->Callback (
+                     BodyParseEventOnComplete,
+                     Char,
+                     0,
+                     Parser->Context
+                     );
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+        break;
+      } else {
+        Parser->State = BodyParserStateMax;
+        break;
+      }
+      
+    case BodyParserTrailer:
+      if (*Char == '\r') {
+        Parser->State = BodyParserChunkSizeEndCR;
+      }
+      Char++;
+      break;      
+
+    case BodyParserChunkDataStart:
+      //
+      // First byte of chunk-data, the chunk data also might be truncated.
+      //
+      RemainderLengthInThis = BodyLength - (Char - Body);
+      LengthForCallback = MIN (Parser->CurrentChunkSize - Parser->CurrentChunkParsedSize, RemainderLengthInThis);
+      if (Parser->Callback != NULL) {
+        Status = Parser->Callback (
+                   BodyParseEventOnData,
+                   Char,
+                   LengthForCallback,
+                   Parser->Context
+                   );
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+      }
+      Char += LengthForCallback;
+      Parser->ContentLength += LengthForCallback;
+      Parser->CurrentChunkParsedSize += LengthForCallback;
+      if (Parser->CurrentChunkParsedSize == Parser->CurrentChunkSize) {
+        Parser->State = BodyParserChunkDataEnd;
+      }           
+      break;
+
+    case BodyParserChunkDataEnd:
+      if (*Char == '\r') {
+        Parser->State = BodyParserChunkDataEndCR;
+      } else {
+        Parser->State = BodyParserStateMax;
+      }
+      Char++;
+      break;
+
+    case BodyParserChunkDataEndCR:
+      if (*Char != '\n') {
+        Parser->State = BodyParserStateMax;
+        break;
+      }
+      Char++;
+      Parser->State = BodyParserChunkSizeStart;
+      break;     
+
+    default:
+      break;
+    }
+
+  }
+
+  if (Parser->State == BodyParserStateMax) {
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether the message-body is complete or not.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+
+  @retval TRUE                       Message-body is complete.
+  @retval FALSE                      Message-body is not complete.
+
+**/
+BOOLEAN
+EFIAPI
+HttpIsMessageComplete (
+  IN VOID              *MsgParser
+  )
+{
+  HTTP_BODY_PARSER      *Parser;
+
+  Parser = (HTTP_BODY_PARSER*) MsgParser;
+
+  if (Parser->State == BodyParserComplete) {
+    return TRUE;
+  }
+  return FALSE;
+}
+
+/**
+  Get the content length of the entity.
+
+  Note that in trunk transfer, the entity length is not valid until the whole message body is received.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+  @param[out]   ContentLength        Pointer to store the length of the entity.
+
+  @retval EFI_SUCCESS                Successfully to get the entity length.
+  @retval EFI_NOT_READY              Entity length is not valid yet.
+  @retval EFI_INVALID_PARAMETER      MsgParser is NULL or ContentLength is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+HttpGetEntityLength (
+  IN  VOID              *MsgParser,
+  OUT UINTN             *ContentLength
+  )
+{
+  HTTP_BODY_PARSER      *Parser;
+
+  if (MsgParser == NULL || ContentLength == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Parser = (HTTP_BODY_PARSER*) MsgParser;
+
+  if (!Parser->ContentLengthIsValid) {
+    return EFI_NOT_READY;
+  }
+
+  *ContentLength = Parser->ContentLength;
+  return EFI_SUCCESS;
+}
+
+/**
+  Release the resource of the message parser.
+
+  @param[in]    MsgParser            Pointer to the message parser.
+  
+**/
+VOID
+EFIAPI
+HttpFreeMsgParser (
+  IN  VOID           *MsgParser
+  )
+{
+  FreePool (MsgParser);
+}
diff --git a/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.inf b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.inf
new file mode 100644
index 0000000000..e26f6fa691
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.inf
@@ -0,0 +1,42 @@
+## @file
+#  It provides the helper routines to parse the HTTP message byte stream.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeHttpLib
+  MODULE_UNI_FILE                = DxeHttpLib.uni
+  FILE_GUID                      = ABBAB4CD-EA88-45b9-8234-C8A7450531FC
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = HttpLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeHttpLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
diff --git a/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.uni b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.uni
new file mode 100644
index 0000000000..806c443929
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeHttpLib/DxeHttpLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.c b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.c
new file mode 100644
index 0000000000..cc93c2b89c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.c
@@ -0,0 +1,2150 @@
+/** @file
+  IpIo Library.
+
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2005 - 2009, 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 <Uefi.h>
+
+#include <Protocol/Udp4.h>
+
+#include <Library/IpIoLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DpcLib.h>
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED LIST_ENTRY  mActiveIpIoList = {
+  &mActiveIpIoList,
+  &mActiveIpIoList
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_IP4_CONFIG_DATA  mIp4IoDefaultIpConfigData = {
+  EFI_IP_PROTO_UDP,
+  FALSE,
+  TRUE,
+  FALSE,
+  FALSE,
+  FALSE,
+  {{0, 0, 0, 0}},
+  {{0, 0, 0, 0}},
+  0,
+  255,
+  FALSE,
+  FALSE,
+  0,
+  0
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_IP6_CONFIG_DATA  mIp6IoDefaultIpConfigData = {
+  EFI_IP_PROTO_UDP,
+  FALSE,
+  TRUE,
+  FALSE,
+  {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
+  {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
+  0,
+  255,
+  0,
+  0,
+  0
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED ICMP_ERROR_INFO  mIcmpErrMap[10] = {
+  {FALSE, TRUE }, // ICMP_ERR_UNREACH_NET
+  {FALSE, TRUE }, // ICMP_ERR_UNREACH_HOST
+  {TRUE,  TRUE }, // ICMP_ERR_UNREACH_PROTOCOL
+  {TRUE,  TRUE }, // ICMP_ERR_UNREACH_PORT
+  {TRUE,  TRUE }, // ICMP_ERR_MSGSIZE
+  {FALSE, TRUE }, // ICMP_ERR_UNREACH_SRCFAIL
+  {FALSE, TRUE }, // ICMP_ERR_TIMXCEED_INTRANS
+  {FALSE, TRUE }, // ICMP_ERR_TIMEXCEED_REASS
+  {FALSE, FALSE}, // ICMP_ERR_QUENCH
+  {FALSE, TRUE }  // ICMP_ERR_PARAMPROB
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED ICMP_ERROR_INFO  mIcmp6ErrMap[10] = {
+  {FALSE, TRUE}, // ICMP6_ERR_UNREACH_NET
+  {FALSE, TRUE}, // ICMP6_ERR_UNREACH_HOST
+  {TRUE,  TRUE}, // ICMP6_ERR_UNREACH_PROTOCOL
+  {TRUE,  TRUE}, // ICMP6_ERR_UNREACH_PORT    
+  {TRUE,  TRUE}, // ICMP6_ERR_PACKAGE_TOOBIG
+  {FALSE, TRUE}, // ICMP6_ERR_TIMXCEED_HOPLIMIT
+  {FALSE, TRUE}, // ICMP6_ERR_TIMXCEED_REASS
+  {FALSE, TRUE}, // ICMP6_ERR_PARAMPROB_HEADER    
+  {FALSE, TRUE}, // ICMP6_ERR_PARAMPROB_NEXHEADER
+  {FALSE, TRUE}  // ICMP6_ERR_PARAMPROB_IPV6OPTION
+};
+
+
+/**
+  Notify function for IP transmit token.
+
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoTransmitHandlerDpc (
+  IN VOID      *Context
+  );
+
+
+/**
+  Notify function for IP transmit token.
+
+  @param[in]  Event                 The event signaled.
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoTransmitHandler (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+
+/**
+  This function create an IP child ,open the IP protocol, and return the opened
+  IP protocol as Interface.
+
+  @param[in]    ControllerHandle   The controller handle.
+  @param[in]    ImageHandle        The image handle.
+  @param[in]    ChildHandle        Pointer to the buffer to save the IP child handle.
+  @param[in]    IpVersion          The version of the IP protocol to use, either
+                                   IPv4 or IPv6.
+  @param[out]   Interface          Pointer used to get the IP protocol interface.
+
+  @retval       EFI_SUCCESS        The IP child is created and the IP protocol
+                                   interface is retrieved.
+  @retval       Others             The required operation failed.
+
+**/
+EFI_STATUS
+IpIoCreateIpChildOpenProtocol (
+  IN  EFI_HANDLE  ControllerHandle,
+  IN  EFI_HANDLE  ImageHandle,
+  IN  EFI_HANDLE  *ChildHandle,
+  IN  UINT8       IpVersion,
+  OUT VOID        **Interface
+  )
+{
+  EFI_STATUS  Status;
+  EFI_GUID    *ServiceBindingGuid;
+  EFI_GUID    *IpProtocolGuid;
+
+  if (IpVersion == IP_VERSION_4) {
+    ServiceBindingGuid = &gEfiIp4ServiceBindingProtocolGuid;
+    IpProtocolGuid     = &gEfiIp4ProtocolGuid;
+  } else if (IpVersion == IP_VERSION_6){
+    ServiceBindingGuid = &gEfiIp6ServiceBindingProtocolGuid;
+    IpProtocolGuid     = &gEfiIp6ProtocolGuid;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Create an IP child.
+  //
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             ImageHandle,
+             ServiceBindingGuid,
+             ChildHandle
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the IP protocol installed on the *ChildHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  *ChildHandle,
+                  IpProtocolGuid,
+                  Interface,
+                  ImageHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // On failure, destroy the IP child.
+    //
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      ImageHandle,
+      ServiceBindingGuid,
+      *ChildHandle
+      );
+  }
+
+  return Status;
+}
+
+
+/**
+  This function close the previously openned IP protocol and destroy the IP child.
+
+  @param[in]  ControllerHandle    The controller handle.
+  @param[in]  ImageHandle         The image handle.
+  @param[in]  ChildHandle         The child handle of the IP child.
+  @param[in]  IpVersion           The version of the IP protocol to use, either
+                                  IPv4 or IPv6.
+
+  @retval     EFI_SUCCESS         The IP protocol is closed and the relevant IP child
+                                  is destroyed.
+  @retval     Others              The required operation failed.
+
+**/
+EFI_STATUS
+IpIoCloseProtocolDestroyIpChild (
+  IN EFI_HANDLE  ControllerHandle,
+  IN EFI_HANDLE  ImageHandle,
+  IN EFI_HANDLE  ChildHandle,
+  IN UINT8       IpVersion
+  )
+{
+  EFI_STATUS  Status;
+  EFI_GUID    *ServiceBindingGuid;
+  EFI_GUID    *IpProtocolGuid;
+
+  if (IpVersion == IP_VERSION_4) {
+    ServiceBindingGuid = &gEfiIp4ServiceBindingProtocolGuid;
+    IpProtocolGuid     = &gEfiIp4ProtocolGuid;
+  } else if (IpVersion == IP_VERSION_6) {
+    ServiceBindingGuid = &gEfiIp6ServiceBindingProtocolGuid;
+    IpProtocolGuid     = &gEfiIp6ProtocolGuid;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Close the previously openned IP protocol.
+  //
+  gBS->CloseProtocol (
+         ChildHandle,
+         IpProtocolGuid,
+         ImageHandle,
+         ControllerHandle
+         );
+
+  //
+  // Destroy the IP child.
+  //
+  Status = NetLibDestroyServiceChild (
+             ControllerHandle,
+             ImageHandle,
+             ServiceBindingGuid,
+             ChildHandle
+             );
+
+  return Status;
+}
+
+/**
+  This function handles ICMPv4 packets. It is the worker function of 
+  IpIoIcmpHandler.
+
+  @param[in]       IpIo            Pointer to the IP_IO instance.
+  @param[in, out]  Pkt             Pointer to the ICMPv4 packet.
+  @param[in]       Session         Pointer to the net session of this ICMPv4 packet.
+
+  @retval          EFI_SUCCESS     The ICMPv4 packet is handled successfully.
+  @retval          EFI_ABORTED     This type of ICMPv4 packet is not supported.
+
+**/
+EFI_STATUS
+IpIoIcmpv4Handler (
+  IN     IP_IO                *IpIo,
+  IN OUT NET_BUF              *Pkt,
+  IN     EFI_NET_SESSION_DATA *Session
+  )
+{
+  IP4_ICMP_ERROR_HEAD  *IcmpHdr;
+  EFI_IP4_HEADER       *IpHdr;
+  UINT8                IcmpErr;
+  UINT8                *PayLoadHdr;
+  UINT8                Type;
+  UINT8                Code;
+  UINT32               TrimBytes;
+
+  ASSERT (IpIo->IpVersion == IP_VERSION_4);
+
+  IcmpHdr = NET_PROTO_HDR (Pkt, IP4_ICMP_ERROR_HEAD);
+  IpHdr   = (EFI_IP4_HEADER *) (&IcmpHdr->IpHead);
+
+  //
+  // Check the ICMP packet length.
+  //
+  if (Pkt->TotalSize < ICMP_ERRLEN (IpHdr)) {
+
+    return EFI_ABORTED;
+  }
+
+  Type = IcmpHdr->Head.Type;
+  Code = IcmpHdr->Head.Code;
+
+  //
+  // Analyze the ICMP Error in this ICMP pkt
+  //
+  switch (Type) {
+  case ICMP_TYPE_UNREACH:
+    switch (Code) {
+    case ICMP_CODE_UNREACH_NET:
+    case ICMP_CODE_UNREACH_HOST:
+    case ICMP_CODE_UNREACH_PROTOCOL:
+    case ICMP_CODE_UNREACH_PORT:
+    case ICMP_CODE_UNREACH_SRCFAIL:
+      IcmpErr = (UINT8) (ICMP_ERR_UNREACH_NET + Code);
+
+      break;
+
+    case ICMP_CODE_UNREACH_NEEDFRAG:
+      IcmpErr = ICMP_ERR_MSGSIZE;
+
+      break;
+
+    case ICMP_CODE_UNREACH_NET_UNKNOWN:
+    case ICMP_CODE_UNREACH_NET_PROHIB:
+    case ICMP_CODE_UNREACH_TOSNET:
+      IcmpErr = ICMP_ERR_UNREACH_NET;
+
+      break;
+
+    case ICMP_CODE_UNREACH_HOST_UNKNOWN:
+    case ICMP_CODE_UNREACH_ISOLATED:
+    case ICMP_CODE_UNREACH_HOST_PROHIB:
+    case ICMP_CODE_UNREACH_TOSHOST:
+      IcmpErr = ICMP_ERR_UNREACH_HOST;
+
+      break;
+
+    default:
+      return EFI_ABORTED;
+    }
+
+    break;
+
+  case ICMP_TYPE_TIMXCEED:
+    if (Code > 1) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = (UINT8) (Code + ICMP_ERR_TIMXCEED_INTRANS);
+
+    break;
+
+  case ICMP_TYPE_PARAMPROB:
+    if (Code > 1) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = ICMP_ERR_PARAMPROB;
+
+    break;
+
+  case ICMP_TYPE_SOURCEQUENCH:
+    if (Code != 0) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = ICMP_ERR_QUENCH;
+
+    break;
+
+  default:
+    return EFI_ABORTED;
+  }
+
+  //
+  // Notify user the ICMP pkt only containing payload except
+  // IP and ICMP header
+  //
+  PayLoadHdr = (UINT8 *) ((UINT8 *) IpHdr + EFI_IP4_HEADER_LEN (IpHdr));
+  TrimBytes  = (UINT32) (PayLoadHdr - (UINT8 *) IcmpHdr);
+
+  NetbufTrim (Pkt, TrimBytes, TRUE);
+
+  IpIo->PktRcvdNotify (EFI_ICMP_ERROR, IcmpErr, Session, Pkt, IpIo->RcvdContext);
+
+  return EFI_SUCCESS;  
+}
+
+/**
+  This function handles ICMPv6 packets. It is the worker function of 
+  IpIoIcmpHandler.
+
+  @param[in]       IpIo            Pointer to the IP_IO instance.
+  @param[in, out]  Pkt             Pointer to the ICMPv6 packet.
+  @param[in]       Session         Pointer to the net session of this ICMPv6 packet.
+
+  @retval          EFI_SUCCESS     The ICMPv6 packet is handled successfully.
+  @retval          EFI_ABORTED     This type of ICMPv6 packet is not supported.
+
+**/
+EFI_STATUS
+IpIoIcmpv6Handler (
+  IN     IP_IO                *IpIo,
+  IN OUT NET_BUF              *Pkt,
+  IN     EFI_NET_SESSION_DATA *Session
+  )
+{
+  IP6_ICMP_ERROR_HEAD  *IcmpHdr;
+  EFI_IP6_HEADER       *IpHdr;
+  UINT8                IcmpErr;
+  UINT8                *PayLoadHdr;
+  UINT8                Type;
+  UINT8                Code;
+  UINT8                NextHeader;
+  UINT32               TrimBytes;
+  BOOLEAN              Flag;
+
+  ASSERT (IpIo->IpVersion == IP_VERSION_6);
+
+  //
+  // Check the ICMPv6 packet length.
+  //
+  if (Pkt->TotalSize < sizeof (IP6_ICMP_ERROR_HEAD)) {
+
+    return EFI_ABORTED;
+  }
+
+  IcmpHdr = NET_PROTO_HDR (Pkt, IP6_ICMP_ERROR_HEAD);
+  Type    = IcmpHdr->Head.Type;
+  Code    = IcmpHdr->Head.Code;
+
+  //
+  // Analyze the ICMPv6 Error in this ICMPv6 packet
+  // 
+  switch (Type) {
+  case ICMP_V6_DEST_UNREACHABLE:
+    switch (Code) {
+    case ICMP_V6_NO_ROUTE_TO_DEST:
+    case ICMP_V6_BEYOND_SCOPE:
+    case ICMP_V6_ROUTE_REJECTED:
+      IcmpErr = ICMP6_ERR_UNREACH_NET;
+
+      break;
+
+    case ICMP_V6_COMM_PROHIBITED:
+    case ICMP_V6_ADDR_UNREACHABLE:
+    case ICMP_V6_SOURCE_ADDR_FAILED:
+      IcmpErr = ICMP6_ERR_UNREACH_HOST;
+
+      break;
+
+    case ICMP_V6_PORT_UNREACHABLE:
+      IcmpErr = ICMP6_ERR_UNREACH_PORT;
+
+      break;
+
+     default:
+      return EFI_ABORTED;
+    }
+
+    break;
+
+  case ICMP_V6_PACKET_TOO_BIG:
+    if (Code >= 1) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = ICMP6_ERR_PACKAGE_TOOBIG;
+
+    break;
+
+  case ICMP_V6_TIME_EXCEEDED:
+    if (Code > 1) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = (UINT8) (ICMP6_ERR_TIMXCEED_HOPLIMIT + Code);
+
+    break;
+
+  case ICMP_V6_PARAMETER_PROBLEM:
+    if (Code > 3) {
+      return EFI_ABORTED;
+    }
+
+    IcmpErr = (UINT8) (ICMP6_ERR_PARAMPROB_HEADER + Code);
+
+    break;
+
+   default:
+
+     return EFI_ABORTED;
+   } 
+
+  //
+  // Notify user the ICMPv6 packet only containing payload except
+  // IPv6 basic header, extension header and ICMP header
+  //
+
+  IpHdr      = (EFI_IP6_HEADER *) (&IcmpHdr->IpHead);
+  NextHeader = IpHdr->NextHeader;
+  PayLoadHdr = (UINT8 *) ((UINT8 *) IcmpHdr + sizeof (IP6_ICMP_ERROR_HEAD));
+  Flag       = TRUE;
+  
+  do {
+    switch (NextHeader) {
+    case EFI_IP_PROTO_UDP:
+    case EFI_IP_PROTO_TCP:
+    case EFI_IP_PROTO_ICMP:
+    case IP6_NO_NEXT_HEADER:
+      Flag = FALSE;
+
+      break;
+
+    case IP6_HOP_BY_HOP:
+    case IP6_DESTINATION:
+      //
+      // The Hdr Ext Len is 8-bit unsigned integer in 8-octet units, not including
+      // the first 8 octets.
+      //
+      NextHeader = *(PayLoadHdr);
+      PayLoadHdr = (UINT8 *) (PayLoadHdr + (*(PayLoadHdr + 1) + 1) * 8);
+
+      break;
+
+    case IP6_FRAGMENT:
+      //
+      // The Fragment Header Length is 8 octets.
+      //
+      NextHeader = *(PayLoadHdr);
+      PayLoadHdr = (UINT8 *) (PayLoadHdr + 8);
+
+      break;
+
+    default:
+
+      return EFI_ABORTED;
+    }
+  } while (Flag);
+
+  TrimBytes = (UINT32) (PayLoadHdr - (UINT8 *) IcmpHdr);
+  
+  NetbufTrim (Pkt, TrimBytes, TRUE);
+
+  IpIo->PktRcvdNotify (EFI_ICMP_ERROR, IcmpErr, Session, Pkt, IpIo->RcvdContext);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function handles ICMP packets.
+
+  @param[in]       IpIo            Pointer to the IP_IO instance.
+  @param[in, out]  Pkt             Pointer to the ICMP packet.
+  @param[in]       Session         Pointer to the net session of this ICMP packet.
+
+  @retval          EFI_SUCCESS     The ICMP packet is handled successfully.
+  @retval          EFI_ABORTED     This type of ICMP packet is not supported.
+  @retval          EFI_UNSUPPORTED The IP protocol version in IP_IO is not supported.
+
+**/
+EFI_STATUS
+IpIoIcmpHandler (
+  IN     IP_IO                *IpIo,
+  IN OUT NET_BUF              *Pkt,
+  IN     EFI_NET_SESSION_DATA *Session
+  )
+{
+
+  if (IpIo->IpVersion == IP_VERSION_4) {
+
+    return IpIoIcmpv4Handler (IpIo, Pkt, Session);
+
+  } else if (IpIo->IpVersion == IP_VERSION_6) {
+
+    return IpIoIcmpv6Handler (IpIo, Pkt, Session);
+
+  } else {
+
+    return EFI_UNSUPPORTED;
+  }
+}
+
+
+/**
+  Free function for receive token of IP_IO. It is used to
+  signal the recycle event to notify IP to recycle the
+  data buffer.
+
+  @param[in]  Event                 The event to be signaled.
+
+**/
+VOID
+EFIAPI
+IpIoExtFree (
+  IN VOID  *Event
+  )
+{
+  gBS->SignalEvent ((EFI_EVENT) Event);
+}
+
+
+/**
+  Create a send entry to wrap a packet before sending
+  out it through IP.
+
+  @param[in, out]  IpIo                 Pointer to the IP_IO instance.
+  @param[in, out]  Pkt                  Pointer to the packet.
+  @param[in]       Sender               Pointer to the IP sender.
+  @param[in]       Context              Pointer to the context.
+  @param[in]       NotifyData           Pointer to the notify data.
+  @param[in]       Dest                 Pointer to the destination IP address.
+  @param[in]       Override             Pointer to the overriden IP_IO data.
+
+  @return Pointer to the data structure created to wrap the packet. If NULL,
+  @return resource limit occurred.
+
+**/
+IP_IO_SEND_ENTRY *
+IpIoCreateSndEntry (
+  IN OUT IP_IO             *IpIo,
+  IN OUT NET_BUF           *Pkt,
+  IN     IP_IO_IP_PROTOCOL Sender,
+  IN     VOID              *Context    OPTIONAL,
+  IN     VOID              *NotifyData OPTIONAL,
+  IN     EFI_IP_ADDRESS    *Dest       OPTIONAL,
+  IN     IP_IO_OVERRIDE    *Override
+  )
+{
+  IP_IO_SEND_ENTRY          *SndEntry;
+  EFI_EVENT                 Event;  
+  EFI_STATUS                Status;
+  NET_FRAGMENT              *ExtFragment;
+  UINT32                    FragmentCount;
+  IP_IO_OVERRIDE            *OverrideData;
+  IP_IO_IP_TX_DATA          *TxData;
+  EFI_IP4_TRANSMIT_DATA     *Ip4TxData;
+  EFI_IP6_TRANSMIT_DATA     *Ip6TxData;
+
+  if ((IpIo->IpVersion != IP_VERSION_4) && (IpIo->IpVersion != IP_VERSION_6)) {
+    return NULL;
+  }
+
+  Event        = NULL;
+  TxData       = NULL;
+  OverrideData = NULL;
+
+  //
+  // Allocate resource for SndEntry
+  //
+  SndEntry = AllocatePool (sizeof (IP_IO_SEND_ENTRY));
+  if (NULL == SndEntry) {
+    return NULL;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  IpIoTransmitHandler,
+                  SndEntry,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  FragmentCount = Pkt->BlockOpNum;
+
+  //
+  // Allocate resource for TxData
+  //
+  TxData = (IP_IO_IP_TX_DATA *) AllocatePool (
+    sizeof (IP_IO_IP_TX_DATA) + sizeof (NET_FRAGMENT) * (FragmentCount - 1)
+    );
+
+  if (NULL == TxData) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Build a fragment table to contain the fragments in the packet. 
+  //
+  if (IpIo->IpVersion == IP_VERSION_4) {
+    ExtFragment = (NET_FRAGMENT *) TxData->Ip4TxData.FragmentTable;
+  } else {
+    ExtFragment = (NET_FRAGMENT *) TxData->Ip6TxData.FragmentTable;
+  }
+
+  NetbufBuildExt (Pkt, ExtFragment, &FragmentCount);
+
+
+  //
+  // Allocate resource for OverrideData if needed
+  //
+  if (NULL != Override) {
+
+    OverrideData = AllocateCopyPool (sizeof (IP_IO_OVERRIDE), Override);
+    if (NULL == OverrideData) {
+      goto ON_ERROR;
+    }
+  }
+
+  //
+  // Set other fields of TxData except the fragment table
+  //
+  if (IpIo->IpVersion == IP_VERSION_4) {
+
+    Ip4TxData = &TxData->Ip4TxData;
+
+    IP4_COPY_ADDRESS (&Ip4TxData->DestinationAddress, Dest);
+
+    Ip4TxData->OverrideData    = &OverrideData->Ip4OverrideData;
+    Ip4TxData->OptionsLength   = 0;
+    Ip4TxData->OptionsBuffer   = NULL;
+    Ip4TxData->TotalDataLength = Pkt->TotalSize;
+    Ip4TxData->FragmentCount   = FragmentCount;
+
+    //
+    // Set the fields of SndToken
+    //
+    SndEntry->SndToken.Ip4Token.Event         = Event;
+    SndEntry->SndToken.Ip4Token.Packet.TxData = Ip4TxData;
+  } else {
+
+    Ip6TxData = &TxData->Ip6TxData;
+
+    if (Dest != NULL) {
+      CopyMem (&Ip6TxData->DestinationAddress, Dest, sizeof (EFI_IPv6_ADDRESS));
+    } else {
+      ZeroMem (&Ip6TxData->DestinationAddress, sizeof (EFI_IPv6_ADDRESS));
+    }
+
+    Ip6TxData->OverrideData  = &OverrideData->Ip6OverrideData;
+    Ip6TxData->DataLength    = Pkt->TotalSize;
+    Ip6TxData->FragmentCount = FragmentCount;
+    Ip6TxData->ExtHdrsLength = 0;
+    Ip6TxData->ExtHdrs       = NULL;
+    
+    //
+    // Set the fields of SndToken
+    //
+    SndEntry->SndToken.Ip6Token.Event         = Event;
+    SndEntry->SndToken.Ip6Token.Packet.TxData = Ip6TxData;
+  }
+
+  //
+  // Set the fields of SndEntry
+  //
+  SndEntry->IpIo        = IpIo;
+  SndEntry->Ip          = Sender;
+  SndEntry->Context     = Context;
+  SndEntry->NotifyData  = NotifyData;
+
+  SndEntry->Pkt         = Pkt;
+  NET_GET_REF (Pkt);
+
+  InsertTailList (&IpIo->PendingSndList, &SndEntry->Entry);
+
+  return SndEntry;
+
+ON_ERROR:
+
+  if (OverrideData != NULL) {
+    FreePool (OverrideData);
+  }
+
+  if (TxData != NULL) {
+    FreePool (TxData);
+  }
+
+  if (SndEntry != NULL) {
+    FreePool (SndEntry);
+  }
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Destroy the SndEntry.
+  
+  This function pairs with IpIoCreateSndEntry().
+
+  @param[in]  SndEntry              Pointer to the send entry to be destroyed.
+
+**/
+VOID
+IpIoDestroySndEntry (
+  IN IP_IO_SEND_ENTRY  *SndEntry
+  )
+{
+  EFI_EVENT         Event;
+  IP_IO_IP_TX_DATA  *TxData;
+  IP_IO_OVERRIDE    *Override;
+
+  if (SndEntry->IpIo->IpVersion == IP_VERSION_4) {
+    Event              = SndEntry->SndToken.Ip4Token.Event;
+    TxData             = (IP_IO_IP_TX_DATA *) SndEntry->SndToken.Ip4Token.Packet.TxData;
+    Override           = (IP_IO_OVERRIDE *) TxData->Ip4TxData.OverrideData;
+  } else if (SndEntry->IpIo->IpVersion == IP_VERSION_6) {
+    Event              = SndEntry->SndToken.Ip6Token.Event;
+    TxData             = (IP_IO_IP_TX_DATA *) SndEntry->SndToken.Ip6Token.Packet.TxData;
+    Override           = (IP_IO_OVERRIDE *) TxData->Ip6TxData.OverrideData;
+  } else {
+    return ;
+  }
+
+  gBS->CloseEvent (Event);
+
+  FreePool (TxData);
+
+  if (NULL != Override) {
+    FreePool (Override);
+  }
+
+  NetbufFree (SndEntry->Pkt);
+
+  RemoveEntryList (&SndEntry->Entry);
+
+  FreePool (SndEntry);
+}
+
+
+/**
+  Notify function for IP transmit token.
+
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoTransmitHandlerDpc (
+  IN VOID      *Context
+  )
+{
+  IP_IO             *IpIo;
+  IP_IO_SEND_ENTRY  *SndEntry;
+  EFI_STATUS        Status;
+
+  SndEntry  = (IP_IO_SEND_ENTRY *) Context;
+
+  IpIo      = SndEntry->IpIo;
+
+  if (IpIo->IpVersion == IP_VERSION_4) {
+    Status = SndEntry->SndToken.Ip4Token.Status;
+  } else if (IpIo->IpVersion == IP_VERSION_6){
+    Status = SndEntry->SndToken.Ip6Token.Status;
+  } else {
+    return ;
+  }
+
+  if ((IpIo->PktSentNotify != NULL) && (SndEntry->NotifyData != NULL)) {
+    IpIo->PktSentNotify (
+            Status,
+            SndEntry->Context,
+            SndEntry->Ip,
+            SndEntry->NotifyData
+            );
+  }
+
+  IpIoDestroySndEntry (SndEntry);
+}
+
+
+/**
+  Notify function for IP transmit token.
+
+  @param[in]  Event                 The event signaled.
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoTransmitHandler (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  //
+  // Request IpIoTransmitHandlerDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, IpIoTransmitHandlerDpc, Context);
+}
+
+
+/**
+  The dummy handler for the dummy IP receive token.
+
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoDummyHandlerDpc (
+  IN VOID      *Context
+  )
+{
+  IP_IO_IP_INFO             *IpInfo;
+  EFI_STATUS                 Status;
+  EFI_EVENT                  RecycleEvent;
+
+  IpInfo      = (IP_IO_IP_INFO *) Context;
+
+  if ((IpInfo->IpVersion != IP_VERSION_4) && (IpInfo->IpVersion != IP_VERSION_6)) {
+    return ;
+  }
+
+  RecycleEvent = NULL;
+
+  if (IpInfo->IpVersion == IP_VERSION_4) {
+    Status = IpInfo->DummyRcvToken.Ip4Token.Status;
+
+    if (IpInfo->DummyRcvToken.Ip4Token.Packet.RxData != NULL) {
+      RecycleEvent = IpInfo->DummyRcvToken.Ip4Token.Packet.RxData->RecycleSignal;
+    }
+  } else {
+    Status = IpInfo->DummyRcvToken.Ip6Token.Status;
+
+    if (IpInfo->DummyRcvToken.Ip6Token.Packet.RxData != NULL) {
+      RecycleEvent = IpInfo->DummyRcvToken.Ip6Token.Packet.RxData->RecycleSignal;
+    }
+  }
+
+
+
+  if (EFI_ABORTED == Status) {
+    //
+    // The reception is actively aborted by the consumer, directly return.
+    //
+    return;
+  } else if (EFI_SUCCESS == Status) {
+    //
+    // Recycle the RxData.
+    //
+    ASSERT (RecycleEvent != NULL);
+
+    gBS->SignalEvent (RecycleEvent);
+  }
+
+  //
+  // Continue the receive.
+  //
+  if (IpInfo->IpVersion == IP_VERSION_4) {
+    IpInfo->Ip.Ip4->Receive (
+                      IpInfo->Ip.Ip4,
+                      &IpInfo->DummyRcvToken.Ip4Token
+                      );
+  } else {
+    IpInfo->Ip.Ip6->Receive (
+                      IpInfo->Ip.Ip6,
+                      &IpInfo->DummyRcvToken.Ip6Token
+                      );
+  }
+}
+
+
+/**
+  This function add IpIoDummyHandlerDpc to the end of the DPC queue.
+
+  @param[in]  Event                 The event signaled.
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoDummyHandler (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  //
+  // Request IpIoDummyHandlerDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, IpIoDummyHandlerDpc, Context);
+}
+
+
+/**
+  Notify function for the IP receive token, used to process
+  the received IP packets.
+
+  @param[in]  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoListenHandlerDpc (
+  IN VOID      *Context
+  )
+{
+  IP_IO                 *IpIo;
+  EFI_STATUS            Status;
+  IP_IO_IP_RX_DATA      *RxData;
+  EFI_NET_SESSION_DATA  Session;
+  NET_BUF               *Pkt;
+
+  IpIo = (IP_IO *) Context;
+
+  if (IpIo->IpVersion == IP_VERSION_4) {
+    Status = IpIo->RcvToken.Ip4Token.Status;
+    RxData = (IP_IO_IP_RX_DATA *) IpIo->RcvToken.Ip4Token.Packet.RxData;
+  } else if (IpIo->IpVersion == IP_VERSION_6) {
+    Status = IpIo->RcvToken.Ip6Token.Status;
+    RxData = (IP_IO_IP_RX_DATA *) IpIo->RcvToken.Ip6Token.Packet.RxData;
+  } else {
+    return;
+  }
+
+  if (EFI_ABORTED == Status) {
+    //
+    // The reception is actively aborted by the consumer, directly return.
+    //
+    return;
+  }
+
+  if (((EFI_SUCCESS != Status) && (EFI_ICMP_ERROR != Status)) || (NULL == RxData)) {
+    //
+    // @bug Only process the normal packets and the icmp error packets, if RxData is NULL
+    // @bug with Status == EFI_SUCCESS or EFI_ICMP_ERROR, just resume the receive although
+    // @bug this should be a bug of the low layer (IP).
+    //
+    goto Resume;
+  }
+
+  if (NULL == IpIo->PktRcvdNotify) {
+    goto CleanUp;
+  }
+
+  if (IpIo->IpVersion == IP_VERSION_4) {
+    if ((EFI_IP4 (RxData->Ip4RxData.Header->SourceAddress) != 0) &&
+      !NetIp4IsUnicast (EFI_NTOHL (((EFI_IP4_RECEIVE_DATA *) RxData)->Header->SourceAddress), 0)) {
+    //
+    // The source address is not zero and it's not a unicast IP address, discard it.
+    //
+    goto CleanUp;
+  }
+
+  //
+  // Create a netbuffer representing IPv4 packet
+  //
+  Pkt = NetbufFromExt (
+          (NET_FRAGMENT *) RxData->Ip4RxData.FragmentTable,
+          RxData->Ip4RxData.FragmentCount,
+          0,
+          0,
+          IpIoExtFree,
+          RxData->Ip4RxData.RecycleSignal
+          );
+  if (NULL == Pkt) {
+    goto CleanUp;
+  }
+
+  //
+  // Create a net session
+  //
+  Session.Source.Addr[0] = EFI_IP4 (RxData->Ip4RxData.Header->SourceAddress);
+  Session.Dest.Addr[0]   = EFI_IP4 (RxData->Ip4RxData.Header->DestinationAddress);
+  Session.IpHdr.Ip4Hdr   = RxData->Ip4RxData.Header;
+  Session.IpHdrLen       = RxData->Ip4RxData.HeaderLength;
+  Session.IpVersion      = IP_VERSION_4;
+  } else {
+
+    if (!NetIp6IsValidUnicast(&RxData->Ip6RxData.Header->SourceAddress)) {
+      goto CleanUp;
+    }
+    
+    //
+    // Create a netbuffer representing IPv6 packet
+    //
+    Pkt = NetbufFromExt (
+            (NET_FRAGMENT *) RxData->Ip6RxData.FragmentTable,
+            RxData->Ip6RxData.FragmentCount,
+            0,
+            0,
+            IpIoExtFree,
+            RxData->Ip6RxData.RecycleSignal
+            );
+    if (NULL == Pkt) {
+      goto CleanUp;
+    }
+
+    //
+    // Create a net session
+    //
+    CopyMem (
+      &Session.Source, 
+      &RxData->Ip6RxData.Header->SourceAddress,
+      sizeof(EFI_IPv6_ADDRESS)
+      );
+    CopyMem (
+      &Session.Dest, 
+      &RxData->Ip6RxData.Header->DestinationAddress, 
+      sizeof(EFI_IPv6_ADDRESS)
+      );
+    Session.IpHdr.Ip6Hdr = RxData->Ip6RxData.Header;
+    Session.IpHdrLen     = RxData->Ip6RxData.HeaderLength;
+    Session.IpVersion    = IP_VERSION_6;
+  } 
+
+  if (EFI_SUCCESS == Status) {
+
+    IpIo->PktRcvdNotify (EFI_SUCCESS, 0, &Session, Pkt, IpIo->RcvdContext);
+  } else {
+    //
+    // Status is EFI_ICMP_ERROR
+    //
+    Status = IpIoIcmpHandler (IpIo, Pkt, &Session);
+    if (EFI_ERROR (Status)) {
+      NetbufFree (Pkt);
+    }
+  }
+
+  goto Resume;
+
+CleanUp:
+
+  if (IpIo->IpVersion == IP_VERSION_4){
+    gBS->SignalEvent (RxData->Ip4RxData.RecycleSignal);
+  } else {
+    gBS->SignalEvent (RxData->Ip6RxData.RecycleSignal); 
+  }
+
+Resume:
+
+  if (IpIo->IpVersion == IP_VERSION_4){
+    IpIo->Ip.Ip4->Receive (IpIo->Ip.Ip4, &(IpIo->RcvToken.Ip4Token));
+  } else {
+    IpIo->Ip.Ip6->Receive (IpIo->Ip.Ip6, &(IpIo->RcvToken.Ip6Token));
+  }
+}
+
+/**
+  This function add IpIoListenHandlerDpc to the end of the DPC queue.
+
+  @param[in]  Event                The event signaled.
+  @param[in]  Context              The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IpIoListenHandler (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  //
+  // Request IpIoListenHandlerDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, IpIoListenHandlerDpc, Context);
+}
+
+
+/**
+  Create a new IP_IO instance.
+  
+  This function uses IP4/IP6 service binding protocol in Controller to create
+  an IP4/IP6 child (aka IP4/IP6 instance).
+
+  @param[in]  Image             The image handle of the driver or application that
+                                consumes IP_IO.
+  @param[in]  Controller        The controller handle that has IP4 or IP6 service
+                                binding protocol installed.
+  @param[in]  IpVersion         The version of the IP protocol to use, either
+                                IPv4 or IPv6.                            
+
+  @return Pointer to a newly created IP_IO instance, or NULL if failed.
+
+**/
+IP_IO *
+EFIAPI
+IpIoCreate (
+  IN EFI_HANDLE Image,
+  IN EFI_HANDLE Controller,
+  IN UINT8      IpVersion
+  )
+{
+  EFI_STATUS  Status;
+  IP_IO       *IpIo;
+  EFI_EVENT   Event;
+
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));
+
+  IpIo = AllocateZeroPool (sizeof (IP_IO));
+  if (NULL == IpIo) {
+    return NULL;
+  }
+
+  InitializeListHead (&(IpIo->PendingSndList));
+  InitializeListHead (&(IpIo->IpList));
+  IpIo->Controller  = Controller;
+  IpIo->Image       = Image;
+  IpIo->IpVersion   = IpVersion;
+  Event             = NULL;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  IpIoListenHandler,
+                  IpIo,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ReleaseIpIo;
+  }
+
+  if (IpVersion == IP_VERSION_4) {
+    IpIo->RcvToken.Ip4Token.Event = Event;
+  } else {
+    IpIo->RcvToken.Ip6Token.Event = Event;
+  }
+
+  //
+  // Create an IP child and open IP protocol
+  //
+  Status = IpIoCreateIpChildOpenProtocol (
+             Controller,
+             Image,
+             &IpIo->ChildHandle,
+             IpVersion,             
+             (VOID **)&(IpIo->Ip)
+             );
+  if (EFI_ERROR (Status)) {
+    goto ReleaseIpIo;
+  }
+
+  return IpIo;
+
+ReleaseIpIo:
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  gBS->FreePool (IpIo);
+
+  return NULL;
+}
+
+
+/**
+  Open an IP_IO instance for use.
+  
+  This function is called after IpIoCreate(). It is used for configuring the IP
+  instance and register the callbacks and their context data for sending and
+  receiving IP packets.
+
+  @param[in, out]  IpIo               Pointer to an IP_IO instance that needs
+                                      to open.
+  @param[in]       OpenData           The configuration data and callbacks for
+                                      the IP_IO instance.
+
+  @retval          EFI_SUCCESS        The IP_IO instance opened with OpenData
+                                      successfully.
+  @retval          EFI_ACCESS_DENIED  The IP_IO instance is configured, avoid to 
+                                      reopen it.
+  @retval          Others             Error condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoOpen (
+  IN OUT IP_IO           *IpIo,
+  IN     IP_IO_OPEN_DATA *OpenData
+  )
+{
+  EFI_STATUS        Status;
+  UINT8             IpVersion;
+
+  if (IpIo->IsConfigured) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  IpVersion = IpIo->IpVersion;
+
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));
+
+  //
+  // configure ip
+  //
+  if (IpVersion == IP_VERSION_4){
+    Status = IpIo->Ip.Ip4->Configure (
+                             IpIo->Ip.Ip4,
+                             &OpenData->IpConfigData.Ip4CfgData
+                             );
+  } else {
+
+    Status = IpIo->Ip.Ip6->Configure (
+                             IpIo->Ip.Ip6, 
+                             &OpenData->IpConfigData.Ip6CfgData
+                             );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // @bug To delete the default route entry in this Ip, if it is:
+  // @bug (0.0.0.0, 0.0.0.0, 0.0.0.0). Delete this statement if Ip modified
+  // @bug its code
+  //
+  if (IpVersion == IP_VERSION_4){
+    Status = IpIo->Ip.Ip4->Routes (
+                             IpIo->Ip.Ip4,
+                             TRUE,
+                             &mZeroIp4Addr,
+                             &mZeroIp4Addr,
+                             &mZeroIp4Addr
+                             );
+
+    if (EFI_ERROR (Status) && (EFI_NOT_FOUND != Status)) {
+      return Status;
+    }
+  }
+
+  IpIo->PktRcvdNotify = OpenData->PktRcvdNotify;
+  IpIo->PktSentNotify = OpenData->PktSentNotify;
+
+  IpIo->RcvdContext   = OpenData->RcvdContext;
+  IpIo->SndContext    = OpenData->SndContext;
+
+  if (IpVersion == IP_VERSION_4){
+    IpIo->Protocol = OpenData->IpConfigData.Ip4CfgData.DefaultProtocol;
+
+    //
+    // start to listen incoming packet
+    //
+    Status = IpIo->Ip.Ip4->Receive (
+                             IpIo->Ip.Ip4,
+                             &(IpIo->RcvToken.Ip4Token)
+                             );
+    if (EFI_ERROR (Status)) {
+      IpIo->Ip.Ip4->Configure (IpIo->Ip.Ip4, NULL);
+      goto ErrorExit;
+    }
+
+  } else {
+
+    IpIo->Protocol = OpenData->IpConfigData.Ip6CfgData.DefaultProtocol;
+    Status = IpIo->Ip.Ip6->Receive (
+                             IpIo->Ip.Ip6,
+                             &(IpIo->RcvToken.Ip6Token)
+                             );
+    if (EFI_ERROR (Status)) {
+      IpIo->Ip.Ip6->Configure (IpIo->Ip.Ip6, NULL);
+      goto ErrorExit;
+    }
+  }
+
+  IpIo->IsConfigured = TRUE;
+  InsertTailList (&mActiveIpIoList, &IpIo->Entry);
+
+ErrorExit:
+
+  return Status;
+}
+
+
+/**
+  Stop an IP_IO instance.
+  
+  This function is paired with IpIoOpen(). The IP_IO will be unconfigured and all
+  the pending send/receive tokens will be canceled.
+
+  @param[in, out]  IpIo            Pointer to the IP_IO instance that needs to stop.
+
+  @retval          EFI_SUCCESS     The IP_IO instance stopped successfully.
+  @retval          Others          Error condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoStop (
+  IN OUT IP_IO *IpIo
+  )
+{
+  EFI_STATUS        Status;
+  IP_IO_IP_INFO     *IpInfo;
+  UINT8             IpVersion;
+
+  if (!IpIo->IsConfigured) {
+    return EFI_SUCCESS;
+  }
+
+  IpVersion = IpIo->IpVersion;
+
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));
+
+  //
+  // Remove the IpIo from the active IpIo list.
+  //
+  RemoveEntryList (&IpIo->Entry);
+
+  //
+  // Configure NULL Ip
+  //
+  if (IpVersion == IP_VERSION_4) {
+    Status = IpIo->Ip.Ip4->Configure (IpIo->Ip.Ip4, NULL);
+  } else {
+    Status = IpIo->Ip.Ip6->Configure (IpIo->Ip.Ip6, NULL);
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  IpIo->IsConfigured = FALSE;
+
+  //
+  // Detroy the Ip List used by IpIo
+  //
+
+  while (!IsListEmpty (&(IpIo->IpList))) {
+    IpInfo = NET_LIST_HEAD (&(IpIo->IpList), IP_IO_IP_INFO, Entry);
+
+    IpIoRemoveIp (IpIo, IpInfo);
+  }
+
+  //
+  // All pending send tokens should be flushed by reseting the IP instances.
+  //
+  ASSERT (IsListEmpty (&IpIo->PendingSndList));
+
+  //
+  // Close the receive event.
+  //
+  if (IpVersion == IP_VERSION_4){
+    gBS->CloseEvent (IpIo->RcvToken.Ip4Token.Event);
+  } else {
+    gBS->CloseEvent (IpIo->RcvToken.Ip6Token.Event);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Destroy an IP_IO instance.
+  
+  This function is paired with IpIoCreate(). The IP_IO will be closed first.
+  Resource will be freed afterwards. See IpIoCloseProtocolDestroyIpChild().
+
+  @param[in, out]  IpIo         Pointer to the IP_IO instance that needs to be
+                                destroyed.
+
+  @retval          EFI_SUCCESS  The IP_IO instance destroyed successfully.
+  @retval          Others       Error condition occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoDestroy (
+  IN OUT IP_IO *IpIo
+  )
+{
+  //
+  // Stop the IpIo.
+  //
+  IpIoStop (IpIo);
+
+  //
+  // Close the IP protocol and destroy the child.
+  //
+  IpIoCloseProtocolDestroyIpChild (
+    IpIo->Controller,
+    IpIo->Image,
+    IpIo->ChildHandle,
+    IpIo->IpVersion
+    );
+
+  gBS->FreePool (IpIo);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Send out an IP packet.
+  
+  This function is called after IpIoOpen(). The data to be sent are wrapped in
+  Pkt. The IP instance wrapped in IpIo is used for sending by default but can be
+  overriden by Sender. Other sending configs, like source address and gateway
+  address etc., are specified in OverrideData.
+
+  @param[in, out]  IpIo                  Pointer to an IP_IO instance used for sending IP
+                                         packet.
+  @param[in, out]  Pkt                   Pointer to the IP packet to be sent.
+  @param[in]       Sender                The IP protocol instance used for sending.
+  @param[in]       Context               Optional context data.
+  @param[in]       NotifyData            Optional notify data.
+  @param[in]       Dest                  The destination IP address to send this packet to.
+  @param[in]       OverrideData          The data to override some configuration of the IP
+                                         instance used for sending.
+
+  @retval          EFI_SUCCESS           The operation is completed successfully.
+  @retval          EFI_NOT_STARTED       The IpIo is not configured.
+  @retval          EFI_OUT_OF_RESOURCES  Failed due to resource limit.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoSend (
+  IN OUT IP_IO          *IpIo,
+  IN OUT NET_BUF        *Pkt,
+  IN     IP_IO_IP_INFO  *Sender        OPTIONAL,
+  IN     VOID           *Context       OPTIONAL,
+  IN     VOID           *NotifyData    OPTIONAL,
+  IN     EFI_IP_ADDRESS *Dest,
+  IN     IP_IO_OVERRIDE *OverrideData  OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  IP_IO_IP_PROTOCOL Ip;
+  IP_IO_SEND_ENTRY  *SndEntry;
+
+  ASSERT ((IpIo->IpVersion != IP_VERSION_4) || (Dest != NULL));
+
+  if (!IpIo->IsConfigured) {
+    return EFI_NOT_STARTED;
+  }
+
+  Ip = (NULL == Sender) ? IpIo->Ip : Sender->Ip;
+
+  //
+  // create a new SndEntry
+  //
+  SndEntry = IpIoCreateSndEntry (IpIo, Pkt, Ip, Context, NotifyData, Dest, OverrideData);
+  if (NULL == SndEntry) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Send this Packet
+  //
+  if (IpIo->IpVersion == IP_VERSION_4){
+    Status = Ip.Ip4->Transmit (
+                       Ip.Ip4,
+                       &SndEntry->SndToken.Ip4Token
+                       );
+  } else {
+    Status = Ip.Ip6->Transmit (
+                       Ip.Ip6,
+                       &SndEntry->SndToken.Ip6Token
+                       );
+  }
+
+  if (EFI_ERROR (Status)) {
+    IpIoDestroySndEntry (SndEntry);
+  }
+
+  return Status;
+}
+
+
+/**
+  Cancel the IP transmit token which wraps this Packet.
+
+  @param[in]  IpIo                  Pointer to the IP_IO instance.
+  @param[in]  Packet                Pointer to the packet of NET_BUF to cancel.
+
+**/
+VOID
+EFIAPI
+IpIoCancelTxToken (
+  IN IP_IO  *IpIo,
+  IN VOID   *Packet
+  )
+{
+  LIST_ENTRY        *Node;
+  IP_IO_SEND_ENTRY  *SndEntry;
+  IP_IO_IP_PROTOCOL Ip;
+
+  ASSERT ((IpIo != NULL) && (Packet != NULL));
+
+  NET_LIST_FOR_EACH (Node, &IpIo->PendingSndList) {
+
+    SndEntry = NET_LIST_USER_STRUCT (Node, IP_IO_SEND_ENTRY, Entry);
+
+    if (SndEntry->Pkt == Packet) {
+
+      Ip = SndEntry->Ip;
+
+      if (IpIo->IpVersion == IP_VERSION_4) {
+        Ip.Ip4->Cancel (
+                  Ip.Ip4,
+                  &SndEntry->SndToken.Ip4Token
+                  );
+      } else {
+        Ip.Ip6->Cancel (
+                  Ip.Ip6,
+                  &SndEntry->SndToken.Ip6Token
+                  );
+      }
+
+      break;
+    }
+  }
+
+}
+
+
+/**
+  Add a new IP instance for sending data.
+  
+  The function is used to add the IP_IO to the IP_IO sending list. The caller
+  can later use IpIoFindSender() to get the IP_IO and call IpIoSend() to send
+  data.
+
+  @param[in, out]  IpIo               Pointer to a IP_IO instance to add a new IP
+                                      instance for sending purpose.
+
+  @return Pointer to the created IP_IO_IP_INFO structure, NULL if failed.
+
+**/
+IP_IO_IP_INFO *
+EFIAPI
+IpIoAddIp (
+  IN OUT IP_IO  *IpIo
+  )
+{
+  EFI_STATUS     Status;
+  IP_IO_IP_INFO  *IpInfo;
+  EFI_EVENT      Event;
+
+  ASSERT (IpIo != NULL);
+
+  IpInfo = AllocatePool (sizeof (IP_IO_IP_INFO));
+  if (IpInfo == NULL) {
+    return NULL;
+  }
+
+  //
+  // Init this IpInfo, set the Addr and SubnetMask to 0 before we configure the IP
+  // instance.
+  //
+  InitializeListHead (&IpInfo->Entry);
+  IpInfo->ChildHandle = NULL;
+  ZeroMem (&IpInfo->Addr, sizeof (IpInfo->Addr));
+  ZeroMem (&IpInfo->PreMask, sizeof (IpInfo->PreMask));
+
+  IpInfo->RefCnt    = 1;
+  IpInfo->IpVersion = IpIo->IpVersion;
+
+  //
+  // Create the IP instance and open the IP protocol.
+  //
+  Status = IpIoCreateIpChildOpenProtocol (
+             IpIo->Controller,
+             IpIo->Image,
+             &IpInfo->ChildHandle,
+             IpInfo->IpVersion,
+             (VOID **) &IpInfo->Ip
+             );
+  if (EFI_ERROR (Status)) {
+    goto ReleaseIpInfo;
+  }
+
+  //
+  // Create the event for the DummyRcvToken.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  IpIoDummyHandler,
+                  IpInfo,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ReleaseIpChild;
+  }
+
+  if (IpInfo->IpVersion == IP_VERSION_4) {
+    IpInfo->DummyRcvToken.Ip4Token.Event = Event;
+  } else {
+    IpInfo->DummyRcvToken.Ip6Token.Event = Event;
+  }
+
+  //
+  // Link this IpInfo into the IpIo.
+  //
+  InsertTailList (&IpIo->IpList, &IpInfo->Entry);
+
+  return IpInfo;
+
+ReleaseIpChild:
+
+  IpIoCloseProtocolDestroyIpChild (
+    IpIo->Controller,
+    IpIo->Image,
+    IpInfo->ChildHandle,
+    IpInfo->IpVersion
+    );
+
+ReleaseIpInfo:
+
+  gBS->FreePool (IpInfo);
+
+  return NULL;
+}
+
+
+/**
+  Configure the IP instance of this IpInfo and start the receiving if IpConfigData
+  is not NULL.
+
+  @param[in, out]  IpInfo          Pointer to the IP_IO_IP_INFO instance.
+  @param[in, out]  IpConfigData    The IP configure data used to configure the IP
+                                   instance, if NULL the IP instance is reset. If
+                                   UseDefaultAddress is set to TRUE, and the configure
+                                   operation succeeds, the default address information
+                                   is written back in this IpConfigData.
+
+  @retval          EFI_SUCCESS     The IP instance of this IpInfo is configured successfully
+                                   or no need to reconfigure it.
+  @retval          Others          Configuration fails.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoConfigIp (
+  IN OUT IP_IO_IP_INFO        *IpInfo,
+  IN OUT VOID                 *IpConfigData OPTIONAL
+  )
+{
+  EFI_STATUS         Status;
+  IP_IO_IP_PROTOCOL  Ip;
+  UINT8              IpVersion;
+  EFI_IP4_MODE_DATA  Ip4ModeData;
+  EFI_IP6_MODE_DATA  Ip6ModeData;
+
+  ASSERT (IpInfo != NULL);
+
+  if (IpInfo->RefCnt > 1) {
+    //
+    // This IP instance is shared, don't reconfigure it until it has only one
+    // consumer. Currently, only the tcp children cloned from their passive parent
+    // will share the same IP. So this cases only happens while IpConfigData is NULL,
+    // let the last consumer clean the IP instance.
+    //
+    return EFI_SUCCESS;
+  }
+
+  IpVersion = IpInfo->IpVersion;
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));
+
+  Ip = IpInfo->Ip;
+
+  if (IpInfo->IpVersion == IP_VERSION_4) {
+    Status = Ip.Ip4->Configure (Ip.Ip4, IpConfigData);
+  } else {
+    Status = Ip.Ip6->Configure (Ip.Ip6, IpConfigData);
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto OnExit;
+  }
+
+  if (IpConfigData != NULL) {
+    if (IpInfo->IpVersion == IP_VERSION_4){
+
+      if (((EFI_IP4_CONFIG_DATA *) IpConfigData)->UseDefaultAddress) {
+        Ip.Ip4->GetModeData (
+                  Ip.Ip4, 
+                  &Ip4ModeData, 
+                  NULL, 
+                  NULL
+                  );
+
+        IP4_COPY_ADDRESS (&((EFI_IP4_CONFIG_DATA*) IpConfigData)->StationAddress, &Ip4ModeData.ConfigData.StationAddress);
+        IP4_COPY_ADDRESS (&((EFI_IP4_CONFIG_DATA*) IpConfigData)->SubnetMask, &Ip4ModeData.ConfigData.SubnetMask);
+    }
+
+      CopyMem (
+        &IpInfo->Addr.Addr, 
+        &((EFI_IP4_CONFIG_DATA *) IpConfigData)->StationAddress, 
+        sizeof (IP4_ADDR)
+        );
+      CopyMem (
+        &IpInfo->PreMask.SubnetMask, 
+        &((EFI_IP4_CONFIG_DATA *) IpConfigData)->SubnetMask,
+        sizeof (IP4_ADDR)
+        );
+
+      Status = Ip.Ip4->Receive (
+                         Ip.Ip4,
+                         &IpInfo->DummyRcvToken.Ip4Token
+                         );
+    if (EFI_ERROR (Status)) {
+      Ip.Ip4->Configure (Ip.Ip4, NULL);
+    }
+  } else {
+    Ip.Ip6->GetModeData (
+              Ip.Ip6,
+              &Ip6ModeData,
+              NULL,
+              NULL
+              );
+
+      if (Ip6ModeData.IsConfigured) {
+        CopyMem (
+          &((EFI_IP6_CONFIG_DATA *) IpConfigData)->StationAddress,
+          &Ip6ModeData.ConfigData.StationAddress,
+          sizeof (EFI_IPv6_ADDRESS)
+          );
+
+        if (Ip6ModeData.AddressList != NULL) {
+          FreePool (Ip6ModeData.AddressList);
+        }
+
+        if (Ip6ModeData.GroupTable != NULL) {
+          FreePool (Ip6ModeData.GroupTable);
+        }
+
+        if (Ip6ModeData.RouteTable != NULL) {
+          FreePool (Ip6ModeData.RouteTable);
+        }
+
+        if (Ip6ModeData.NeighborCache != NULL) {
+          FreePool (Ip6ModeData.NeighborCache);
+        }
+
+        if (Ip6ModeData.PrefixTable != NULL) {
+          FreePool (Ip6ModeData.PrefixTable);
+        }
+
+        if (Ip6ModeData.IcmpTypeList != NULL) {
+          FreePool (Ip6ModeData.IcmpTypeList);
+        }
+
+      } else {
+        Status = EFI_NO_MAPPING;
+        goto OnExit;
+      } 
+
+      CopyMem (
+        &IpInfo->Addr, 
+        &Ip6ModeData.ConfigData.StationAddress, 
+        sizeof (EFI_IPv6_ADDRESS)
+        );
+
+      Status = Ip.Ip6->Receive (
+                         Ip.Ip6,
+                         &IpInfo->DummyRcvToken.Ip6Token
+                         );
+      if (EFI_ERROR (Status)) {
+        Ip.Ip6->Configure (Ip.Ip6, NULL);
+      }
+    }  
+  } else {
+    //
+    // The IP instance is reset, set the stored Addr and SubnetMask to zero.
+    //
+    ZeroMem (&IpInfo->Addr, sizeof (IpInfo->Addr));
+    ZeroMem (&IpInfo->PreMask, sizeof (IpInfo->PreMask));
+  }
+
+OnExit:
+
+  return Status;
+}
+
+
+/**
+  Destroy an IP instance maintained in IpIo->IpList for
+  sending purpose.
+  
+  This function pairs with IpIoAddIp(). The IpInfo is previously created by
+  IpIoAddIp(). The IP_IO_IP_INFO::RefCnt is decremented and the IP instance
+  will be dstroyed if the RefCnt is zero.
+
+  @param[in]  IpIo                  Pointer to the IP_IO instance.
+  @param[in]  IpInfo                Pointer to the IpInfo to be removed.
+
+**/
+VOID
+EFIAPI
+IpIoRemoveIp (
+  IN IP_IO            *IpIo,
+  IN IP_IO_IP_INFO    *IpInfo
+  )
+{
+
+  UINT8               IpVersion;
+
+  ASSERT (IpInfo->RefCnt > 0);
+
+  NET_PUT_REF (IpInfo);
+
+  if (IpInfo->RefCnt > 0) {
+
+    return;
+  }
+
+  IpVersion = IpIo->IpVersion;
+
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));
+
+  RemoveEntryList (&IpInfo->Entry);
+
+  if (IpVersion == IP_VERSION_4){
+    IpInfo->Ip.Ip4->Configure (
+                      IpInfo->Ip.Ip4,
+                      NULL
+                      );
+    IpIoCloseProtocolDestroyIpChild (
+      IpIo->Controller,
+      IpIo->Image,
+      IpInfo->ChildHandle,
+      IP_VERSION_4
+      );
+
+    gBS->CloseEvent (IpInfo->DummyRcvToken.Ip4Token.Event);
+
+  } else {
+
+    IpInfo->Ip.Ip6->Configure (
+                      IpInfo->Ip.Ip6,
+                      NULL
+                      );
+
+    IpIoCloseProtocolDestroyIpChild (
+      IpIo->Controller,
+      IpIo->Image,
+      IpInfo->ChildHandle,
+      IP_VERSION_6
+      );
+
+    gBS->CloseEvent (IpInfo->DummyRcvToken.Ip6Token.Event);
+  }
+
+  FreePool (IpInfo);
+}
+
+
+/**
+  Find the first IP protocol maintained in IpIo whose local
+  address is the same as Src.
+  
+  This function is called when the caller needs the IpIo to send data to the
+  specified Src. The IpIo was added previously by IpIoAddIp().
+
+  @param[in, out]  IpIo              Pointer to the pointer of the IP_IO instance.
+  @param[in]       IpVersion         The version of the IP protocol to use, either
+                                     IPv4 or IPv6.
+  @param[in]       Src               The local IP address.
+
+  @return Pointer to the IP protocol can be used for sending purpose and its local
+          address is the same with Src.
+
+**/
+IP_IO_IP_INFO *
+EFIAPI
+IpIoFindSender (
+  IN OUT IP_IO           **IpIo,
+  IN     UINT8           IpVersion,
+  IN     EFI_IP_ADDRESS  *Src
+  )
+{
+  LIST_ENTRY      *IpIoEntry;
+  IP_IO           *IpIoPtr;
+  LIST_ENTRY      *IpInfoEntry;
+  IP_IO_IP_INFO   *IpInfo;
+
+  ASSERT ((IpVersion == IP_VERSION_4) || (IpVersion == IP_VERSION_6));  
+
+  NET_LIST_FOR_EACH (IpIoEntry, &mActiveIpIoList) {
+    IpIoPtr = NET_LIST_USER_STRUCT (IpIoEntry, IP_IO, Entry);
+
+    if (((*IpIo != NULL) && (*IpIo != IpIoPtr)) || (IpIoPtr->IpVersion != IpVersion)) {
+      continue;
+    }
+
+    NET_LIST_FOR_EACH (IpInfoEntry, &IpIoPtr->IpList) {
+      IpInfo = NET_LIST_USER_STRUCT (IpInfoEntry, IP_IO_IP_INFO, Entry);
+      if (IpInfo->IpVersion == IP_VERSION_4){
+
+        if (EFI_IP4_EQUAL (&IpInfo->Addr.v4, &Src->v4)) {
+          *IpIo = IpIoPtr;
+          return IpInfo;
+        }
+
+      } else {
+
+        if (EFI_IP6_EQUAL (&IpInfo->Addr.v6, &Src->v6)) {
+          *IpIo = IpIoPtr;
+          return IpInfo;       
+        }
+      }      
+
+    }
+  }
+
+  //
+  // No match.
+  //
+  return NULL;
+}
+
+
+/**
+  Get the ICMP error map information.
+  
+  The ErrorStatus will be returned. The IsHard and Notify are optional. If they
+  are not NULL, this routine will fill them.
+
+  @param[in]   IcmpError             IcmpError Type.
+  @param[in]   IpVersion             The version of the IP protocol to use,
+                                     either IPv4 or IPv6. 
+  @param[out]  IsHard                If TRUE, indicates that it is a hard error.
+  @param[out]  Notify                If TRUE, SockError needs to be notified.
+
+  @return ICMP Error Status, such as EFI_NETWORK_UNREACHABLE.
+
+**/
+EFI_STATUS
+EFIAPI
+IpIoGetIcmpErrStatus (
+  IN  UINT8       IcmpError,
+  IN  UINT8       IpVersion,
+  OUT BOOLEAN     *IsHard  OPTIONAL,
+  OUT BOOLEAN     *Notify  OPTIONAL
+  )
+{
+  if (IpVersion == IP_VERSION_4 ) {
+    ASSERT (IcmpError <= ICMP_ERR_PARAMPROB);
+
+    if (IsHard != NULL) {
+      *IsHard = mIcmpErrMap[IcmpError].IsHard;
+    }
+
+    if (Notify != NULL) {
+      *Notify = mIcmpErrMap[IcmpError].Notify;
+    }
+
+    switch (IcmpError) {
+    case ICMP_ERR_UNREACH_NET:
+      return  EFI_NETWORK_UNREACHABLE;
+
+    case ICMP_ERR_TIMXCEED_INTRANS:
+    case ICMP_ERR_TIMXCEED_REASS:
+    case ICMP_ERR_UNREACH_HOST:
+      return  EFI_HOST_UNREACHABLE;
+
+    case ICMP_ERR_UNREACH_PROTOCOL:
+      return  EFI_PROTOCOL_UNREACHABLE;
+
+    case ICMP_ERR_UNREACH_PORT:
+      return  EFI_PORT_UNREACHABLE;
+
+    case ICMP_ERR_MSGSIZE:
+    case ICMP_ERR_UNREACH_SRCFAIL:
+    case ICMP_ERR_QUENCH:
+    case ICMP_ERR_PARAMPROB:
+      return  EFI_ICMP_ERROR;
+
+    default:
+      ASSERT (FALSE);
+      return EFI_UNSUPPORTED;
+    }
+
+  } else if (IpVersion == IP_VERSION_6) {
+
+    ASSERT (IcmpError <= ICMP6_ERR_PARAMPROB_IPV6OPTION);
+
+    if (IsHard != NULL) {
+      *IsHard = mIcmp6ErrMap[IcmpError].IsHard;
+    }
+
+    if (Notify != NULL) {
+      *Notify = mIcmp6ErrMap[IcmpError].Notify;
+    }
+
+    switch (IcmpError) {
+    case ICMP6_ERR_UNREACH_NET:
+      return EFI_NETWORK_UNREACHABLE;
+
+    case ICMP6_ERR_UNREACH_HOST:
+    case ICMP6_ERR_TIMXCEED_HOPLIMIT:
+    case ICMP6_ERR_TIMXCEED_REASS:       
+      return EFI_HOST_UNREACHABLE;
+
+    case ICMP6_ERR_UNREACH_PROTOCOL:
+      return EFI_PROTOCOL_UNREACHABLE;
+      
+    case ICMP6_ERR_UNREACH_PORT:
+      return EFI_PORT_UNREACHABLE;
+
+    case ICMP6_ERR_PACKAGE_TOOBIG:
+    case ICMP6_ERR_PARAMPROB_HEADER:
+    case ICMP6_ERR_PARAMPROB_NEXHEADER:
+    case ICMP6_ERR_PARAMPROB_IPV6OPTION:
+      return EFI_ICMP_ERROR;
+
+    default:
+      ASSERT (FALSE);
+      return EFI_UNSUPPORTED;
+    }
+
+  } else {
+    //
+    // Should never be here
+    //
+    ASSERT (FALSE);
+    return EFI_UNSUPPORTED;
+  }
+}
+
+
+/**
+  Refresh the remote peer's Neighbor Cache entries.
+
+  This function is called when the caller needs the IpIo to refresh the existing
+  IPv6 neighbor cache entries since the neighbor is considered reachable by the 
+  node has recently received a confirmation that packets sent recently to the 
+  neighbor were received by its IP layer. 
+
+  @param[in]   IpIo                  Pointer to an IP_IO instance
+  @param[in]   Neighbor              The IP address of the neighbor
+  @param[in]   Timeout               Time in 100-ns units that this entry will
+                                     remain in the neighbor cache. A value of 
+                                     zero means that the entry is permanent. 
+                                     A value of non-zero means that the entry is 
+                                     dynamic and will be deleted after Timeout.
+
+  @retval      EFI_SUCCESS           The operation is completed successfully.
+  @retval      EFI_NOT_STARTED       The IpIo is not configured.
+  @retval      EFI_INVALID_PARAMETER Neighbor Address is invalid.
+  @retval      EFI_NOT_FOUND         The neighbor cache entry is not in the 
+                                     neighbor table.  
+  @retval      EFI_OUT_OF_RESOURCES  Failed due to resource limit.
+
+**/
+EFI_STATUS
+IpIoRefreshNeighbor (
+  IN IP_IO           *IpIo,
+  IN EFI_IP_ADDRESS  *Neighbor,
+  IN UINT32          Timeout  
+  )
+{
+  EFI_IP6_PROTOCOL  *Ip;
+
+  if (!IpIo->IsConfigured || IpIo->IpVersion != IP_VERSION_6) {
+    return EFI_NOT_STARTED;
+  }
+
+  Ip = IpIo->Ip.Ip6;
+
+  return Ip->Neighbors (Ip, FALSE, &Neighbor->v6, NULL, Timeout, TRUE);
+}
+
diff --git a/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.inf b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.inf
new file mode 100644
index 0000000000..f62a36fd50
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.inf
@@ -0,0 +1,53 @@
+## @file
+#  This library instance provides IP services upon EFI IPv4/IPv6 Protocols.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeIpIoLib
+  MODULE_UNI_FILE                = DxeIpIoLib.uni
+  FILE_GUID                      = A302F877-8625-425c-B1EC-7487B62C4FDA
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = IpIoLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeIpIoLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  IpIoLib
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  DpcLib
+
+[Protocols]
+  gEfiIp4ProtocolGuid                           ## SOMETIMES_CONSUMES
+  gEfiIp4ServiceBindingProtocolGuid             ## SOMETIMES_CONSUMES
+  gEfiIp6ProtocolGuid                           ## SOMETIMES_CONSUMES
+  gEfiIp6ServiceBindingProtocolGuid             ## SOMETIMES_CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.uni b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.uni
new file mode 100644
index 0000000000..5545a50671
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.c b/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.c
new file mode 100644
index 0000000000..d15c6bd561
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.c
@@ -0,0 +1,81 @@
+/** @file
+  Implementation of Ipmi Library in DXE Phase for SMS.
+
+  Copyright (c) 2009 - 2015, 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 <PiDxe.h>
+#include <Protocol/IpmiProtocol.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+
+IPMI_PROTOCOL *mIpmiProtocol = NULL;
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+EFI_STATUS
+EFIAPI
+IpmiSubmitCommand (
+  IN     UINT8     NetFunction,
+  IN     UINT8     Command,
+  IN     UINT8     *RequestData,
+  IN     UINT32    RequestDataSize,
+     OUT UINT8     *ResponseData,
+  IN OUT UINT32    *ResponseDataSize
+  )
+{
+  EFI_STATUS  Status;
+
+  if (mIpmiProtocol == NULL) {
+    Status = gBS->LocateProtocol (
+              &gIpmiProtocolGuid,
+              NULL,
+              (VOID **) &mIpmiProtocol
+              );
+    if (EFI_ERROR (Status)) {
+      //
+      // Dxe Ipmi Protocol is not installed. So, IPMI device is not present.
+      //
+      DEBUG ((EFI_D_ERROR, "IpmiSubmitCommand in Dxe Phase under SMS Status - %r\n", Status));
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  Status = mIpmiProtocol->IpmiSubmitCommand (
+                            mIpmiProtocol,
+                            NetFunction,
+                            Command,
+                            RequestData,
+                            RequestDataSize,
+                            ResponseData,
+                            ResponseDataSize
+                            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.inf b/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.inf
new file mode 100644
index 0000000000..5b56b39656
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.inf
@@ -0,0 +1,40 @@
+## @file
+#  Instance of IPMI Library in DXE phase for SMS.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeIpmiLibIpmiProtocol
+  FILE_GUID                      = 62408AD5-4EAC-432B-AB9B-C4B85BFAED02
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = IpmiLib|DXE_RUNTIME_DRIVER DXE_DRIVER DXE_CORE UEFI_DRIVER UEFI_APPLICATION
+
+#
+#  VALID_ARCHITECTURES          = IA32 X64
+#
+
+[Sources]
+  DxeIpmiLibIpmiProtocol.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  DebugLib
+
+[Protocols]
+  gIpmiProtocolGuid      ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.c b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.c
new file mode 100644
index 0000000000..6a84847039
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.c
@@ -0,0 +1,3334 @@
+/** @file
+  Network library.
+
+Copyright (c) 2005 - 2015, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett Packard Enterprise Development LP<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 <Uefi.h>
+
+#include <IndustryStandard/SmBios.h>
+
+#include <Protocol/DriverBinding.h>
+#include <Protocol/ServiceBinding.h>
+#include <Protocol/SimpleNetwork.h>
+#include <Protocol/ManagedNetwork.h>
+#include <Protocol/Ip4Config2.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+
+#include <Guid/SmBios.h>
+
+#include <Library/NetLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiLib.h>
+
+#define NIC_ITEM_CONFIG_SIZE   sizeof (NIC_IP4_CONFIG_INFO) + sizeof (EFI_IP4_ROUTE_TABLE) * MAX_IP4_CONFIG_IN_VARIABLE
+#define DEFAULT_ZERO_START     ((UINTN) ~0)
+
+//
+// All the supported IP4 maskes in host byte order.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED IP4_ADDR  gIp4AllMasks[IP4_MASK_NUM] = {
+  0x00000000,
+  0x80000000,
+  0xC0000000,
+  0xE0000000,
+  0xF0000000,
+  0xF8000000,
+  0xFC000000,
+  0xFE000000,
+
+  0xFF000000,
+  0xFF800000,
+  0xFFC00000,
+  0xFFE00000,
+  0xFFF00000,
+  0xFFF80000,
+  0xFFFC0000,
+  0xFFFE0000,
+
+  0xFFFF0000,
+  0xFFFF8000,
+  0xFFFFC000,
+  0xFFFFE000,
+  0xFFFFF000,
+  0xFFFFF800,
+  0xFFFFFC00,
+  0xFFFFFE00,
+
+  0xFFFFFF00,
+  0xFFFFFF80,
+  0xFFFFFFC0,
+  0xFFFFFFE0,
+  0xFFFFFFF0,
+  0xFFFFFFF8,
+  0xFFFFFFFC,
+  0xFFFFFFFE,
+  0xFFFFFFFF,
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_IPv4_ADDRESS  mZeroIp4Addr = {{0, 0, 0, 0}};
+
+//
+// Any error level digitally larger than mNetDebugLevelMax
+// will be silently discarded.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINTN  mNetDebugLevelMax = NETDEBUG_LEVEL_ERROR;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mSyslogPacketSeq  = 0xDEADBEEF;
+
+//
+// You can change mSyslogDstMac mSyslogDstIp and mSyslogSrcIp
+// here to direct the syslog packets to the syslog deamon. The
+// default is broadcast to both the ethernet and IP.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED UINT8  mSyslogDstMac[NET_ETHER_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mSyslogDstIp                      = 0xffffffff;
+GLOBAL_REMOVE_IF_UNREFERENCED UINT32 mSyslogSrcIp                      = 0;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CHAR8 *mMonthName[] = {
+  "Jan",
+  "Feb",
+  "Mar",
+  "Apr",
+  "May",
+  "Jun",
+  "Jul",
+  "Aug",
+  "Sep",
+  "Oct",
+  "Nov",
+  "Dec"
+};
+
+//
+// VLAN device path node template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED VLAN_DEVICE_PATH mNetVlanDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_VLAN_DP,
+    {
+      (UINT8) (sizeof (VLAN_DEVICE_PATH)),
+      (UINT8) ((sizeof (VLAN_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+/**
+  Locate the handles that support SNP, then open one of them
+  to send the syslog packets. The caller isn't required to close
+  the SNP after use because the SNP is opened by HandleProtocol.
+
+  @return The point to SNP if one is properly openned. Otherwise NULL
+
+**/
+EFI_SIMPLE_NETWORK_PROTOCOL *
+SyslogLocateSnp (
+  VOID
+  )
+{
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  EFI_STATUS                  Status;
+  EFI_HANDLE                  *Handles;
+  UINTN                       HandleCount;
+  UINTN                       Index;
+
+  //
+  // Locate the handles which has SNP installed.
+  //
+  Handles = NULL;
+  Status  = gBS->LocateHandleBuffer (
+                   ByProtocol,
+                   &gEfiSimpleNetworkProtocolGuid,
+                   NULL,
+                   &HandleCount,
+                   &Handles
+                   );
+
+  if (EFI_ERROR (Status) || (HandleCount == 0)) {
+    return NULL;
+  }
+
+  //
+  // Try to open one of the ethernet SNP protocol to send packet
+  //
+  Snp = NULL;
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (
+                    Handles[Index],
+                    &gEfiSimpleNetworkProtocolGuid,
+                    (VOID **) &Snp
+                    );
+
+    if ((Status == EFI_SUCCESS) && (Snp != NULL) &&
+        (Snp->Mode->IfType == NET_IFTYPE_ETHERNET) &&
+        (Snp->Mode->MaxPacketSize >= NET_SYSLOG_PACKET_LEN)) {
+
+      break;
+    }
+
+    Snp = NULL;
+  }
+
+  FreePool (Handles);
+  return Snp;
+}
+
+/**
+  Transmit a syslog packet synchronously through SNP. The Packet
+  already has the ethernet header prepended. This function should
+  fill in the source MAC because it will try to locate a SNP each
+  time it is called to avoid the problem if SNP is unloaded.
+  This code snip is copied from MNP.
+
+  @param[in] Packet          The Syslog packet
+  @param[in] Length          The length of the packet
+
+  @retval EFI_DEVICE_ERROR   Failed to locate a usable SNP protocol
+  @retval EFI_TIMEOUT        Timeout happened to send the packet.
+  @retval EFI_SUCCESS        Packet is sent.
+
+**/
+EFI_STATUS
+SyslogSendPacket (
+  IN CHAR8                    *Packet,
+  IN UINT32                   Length
+  )
+{
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  ETHER_HEAD                  *Ether;
+  EFI_STATUS                  Status;
+  EFI_EVENT                   TimeoutEvent;
+  UINT8                       *TxBuf;
+
+  Snp = SyslogLocateSnp ();
+
+  if (Snp == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Ether = (ETHER_HEAD *) Packet;
+  CopyMem (Ether->SrcMac, Snp->Mode->CurrentAddress.Addr, NET_ETHER_ADDR_LEN);
+
+  //
+  // Start the timeout event.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_NOTIFY,
+                  NULL,
+                  NULL,
+                  &TimeoutEvent
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (TimeoutEvent, TimerRelative, NET_SYSLOG_TX_TIMEOUT);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  for (;;) {
+    //
+    // Transmit the packet through SNP.
+    //
+    Status = Snp->Transmit (Snp, 0, Length, Packet, NULL, NULL, NULL);
+
+    if ((Status != EFI_SUCCESS) && (Status != EFI_NOT_READY)) {
+      Status = EFI_DEVICE_ERROR;
+      break;
+    }
+
+    //
+    // If Status is EFI_SUCCESS, the packet is put in the transmit queue.
+    // if Status is EFI_NOT_READY, the transmit engine of the network
+    // interface is busy. Both need to sync SNP.
+    //
+    TxBuf = NULL;
+
+    do {
+      //
+      // Get the recycled transmit buffer status.
+      //
+      Snp->GetStatus (Snp, NULL, (VOID **) &TxBuf);
+
+      if (!EFI_ERROR (gBS->CheckEvent (TimeoutEvent))) {
+        Status = EFI_TIMEOUT;
+        break;
+      }
+
+    } while (TxBuf == NULL);
+
+    if ((Status == EFI_SUCCESS) || (Status == EFI_TIMEOUT)) {
+      break;
+    }
+
+    //
+    // Status is EFI_NOT_READY. Restart the timer event and
+    // call Snp->Transmit again.
+    //
+    gBS->SetTimer (TimeoutEvent, TimerRelative, NET_SYSLOG_TX_TIMEOUT);
+  }
+
+  gBS->SetTimer (TimeoutEvent, TimerCancel, 0);
+
+ON_EXIT:
+  gBS->CloseEvent (TimeoutEvent);
+  return Status;
+}
+
+/**
+  Build a syslog packet, including the Ethernet/Ip/Udp headers
+  and user's message.
+
+  @param[in]  Level     Syslog servity level
+  @param[in]  Module    The module that generates the log
+  @param[in]  File      The file that contains the current log
+  @param[in]  Line      The line of code in the File that contains the current log
+  @param[in]  Message   The log message
+  @param[in]  BufLen    The lenght of the Buf
+  @param[out] Buf       The buffer to put the packet data
+
+  @return The length of the syslog packet built.
+
+**/
+UINT32
+SyslogBuildPacket (
+  IN  UINT32                Level,
+  IN  UINT8                 *Module,
+  IN  UINT8                 *File,
+  IN  UINT32                Line,
+  IN  UINT8                 *Message,
+  IN  UINT32                BufLen,
+  OUT CHAR8                 *Buf
+  )
+{
+  ETHER_HEAD                *Ether;
+  IP4_HEAD                  *Ip4;
+  EFI_UDP_HEADER            *Udp4;
+  EFI_TIME                  Time;
+  UINT32                    Pri;
+  UINT32                    Len;
+
+  //
+  // Fill in the Ethernet header. Leave alone the source MAC.
+  // SyslogSendPacket will fill in the address for us.
+  //
+  Ether = (ETHER_HEAD *) Buf;
+  CopyMem (Ether->DstMac, mSyslogDstMac, NET_ETHER_ADDR_LEN);
+  ZeroMem (Ether->SrcMac, NET_ETHER_ADDR_LEN);
+
+  Ether->EtherType = HTONS (0x0800);    // IPv4 protocol
+
+  Buf             += sizeof (ETHER_HEAD);
+  BufLen          -= sizeof (ETHER_HEAD);
+
+  //
+  // Fill in the IP header
+  //
+  Ip4              = (IP4_HEAD *) Buf;
+  Ip4->HeadLen     = 5;
+  Ip4->Ver         = 4;
+  Ip4->Tos         = 0;
+  Ip4->TotalLen    = 0;
+  Ip4->Id          = (UINT16) mSyslogPacketSeq;
+  Ip4->Fragment    = 0;
+  Ip4->Ttl         = 16;
+  Ip4->Protocol    = 0x11;
+  Ip4->Checksum    = 0;
+  Ip4->Src         = mSyslogSrcIp;
+  Ip4->Dst         = mSyslogDstIp;
+
+  Buf             += sizeof (IP4_HEAD);
+  BufLen          -= sizeof (IP4_HEAD);
+
+  //
+  // Fill in the UDP header, Udp checksum is optional. Leave it zero.
+  //
+  Udp4             = (EFI_UDP_HEADER *) Buf;
+  Udp4->SrcPort    = HTONS (514);
+  Udp4->DstPort    = HTONS (514);
+  Udp4->Length     = 0;
+  Udp4->Checksum   = 0;
+
+  Buf             += sizeof (EFI_UDP_HEADER);
+  BufLen          -= sizeof (EFI_UDP_HEADER);
+
+  //
+  // Build the syslog message body with <PRI> Timestamp  machine module Message
+  //
+  Pri = ((NET_SYSLOG_FACILITY & 31) << 3) | (Level & 7);
+  gRT->GetTime (&Time, NULL);
+  ASSERT ((Time.Month <= 12) && (Time.Month >= 1));
+
+  //
+  // Use %a to format the ASCII strings, %s to format UNICODE strings
+  //
+  Len  = 0;
+  Len += (UINT32) AsciiSPrint (
+                    Buf,
+                    BufLen,
+                    "<%d> %a %d %d:%d:%d ",
+                    Pri,
+                    mMonthName [Time.Month-1],
+                    Time.Day,
+                    Time.Hour,
+                    Time.Minute,
+                    Time.Second
+                    );
+  Len--;
+
+  Len += (UINT32) AsciiSPrint (
+                    Buf + Len,
+                    BufLen - Len,
+                    "Tiano %a: %a (Line: %d File: %a)",
+                    Module,
+                    Message,
+                    Line,
+                    File
+                    );
+  Len--;
+
+  //
+  // OK, patch the IP length/checksum and UDP length fields.
+  //
+  Len           += sizeof (EFI_UDP_HEADER);
+  Udp4->Length   = HTONS ((UINT16) Len);
+
+  Len           += sizeof (IP4_HEAD);
+  Ip4->TotalLen  = HTONS ((UINT16) Len);
+  Ip4->Checksum  = (UINT16) (~NetblockChecksum ((UINT8 *) Ip4, sizeof (IP4_HEAD)));
+
+  return Len + sizeof (ETHER_HEAD);
+}
+
+/**
+  Allocate a buffer, then format the message to it. This is a
+  help function for the NET_DEBUG_XXX macros. The PrintArg of
+  these macros treats the variable length print parameters as a
+  single parameter, and pass it to the NetDebugASPrint. For
+  example, NET_DEBUG_TRACE ("Tcp", ("State transit to %a\n", Name))
+  if extracted to:
+
+         NetDebugOutput (
+           NETDEBUG_LEVEL_TRACE,
+           "Tcp",
+           __FILE__,
+           __LINE__,
+           NetDebugASPrint ("State transit to %a\n", Name)
+         )
+
+  @param Format  The ASCII format string.
+  @param ...     The variable length parameter whose format is determined
+                 by the Format string.
+
+  @return        The buffer containing the formatted message,
+                 or NULL if failed to allocate memory.
+
+**/
+CHAR8 *
+EFIAPI
+NetDebugASPrint (
+  IN CHAR8                  *Format,
+  ...
+  )
+{
+  VA_LIST                   Marker;
+  CHAR8                     *Buf;
+
+  Buf = (CHAR8 *) AllocatePool (NET_DEBUG_MSG_LEN);
+
+  if (Buf == NULL) {
+    return NULL;
+  }
+
+  VA_START (Marker, Format);
+  AsciiVSPrint (Buf, NET_DEBUG_MSG_LEN, Format, Marker);
+  VA_END (Marker);
+
+  return Buf;
+}
+
+/**
+  Builds an UDP4 syslog packet and send it using SNP.
+
+  This function will locate a instance of SNP then send the message through it.
+  Because it isn't open the SNP BY_DRIVER, apply caution when using it.
+
+  @param Level    The servity level of the message.
+  @param Module   The Moudle that generates the log.
+  @param File     The file that contains the log.
+  @param Line     The exact line that contains the log.
+  @param Message  The user message to log.
+
+  @retval EFI_INVALID_PARAMETER Any input parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the packet
+  @retval EFI_SUCCESS           The log is discard because that it is more verbose
+                                than the mNetDebugLevelMax. Or, it has been sent out.
+**/
+EFI_STATUS
+EFIAPI
+NetDebugOutput (
+  IN UINT32                    Level,
+  IN UINT8                     *Module,
+  IN UINT8                     *File,
+  IN UINT32                    Line,
+  IN UINT8                     *Message
+  )
+{
+  CHAR8                        *Packet;
+  UINT32                       Len;
+  EFI_STATUS                   Status;
+
+  //
+  // Check whether the message should be sent out
+  //
+  if (Message == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Level > mNetDebugLevelMax) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Allocate a maxium of 1024 bytes, the caller should ensure
+  // that the message plus the ethernet/ip/udp header is shorter
+  // than this
+  //
+  Packet = (CHAR8 *) AllocatePool (NET_SYSLOG_PACKET_LEN);
+
+  if (Packet == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  //
+  // Build the message: Ethernet header + IP header + Udp Header + user data
+  //
+  Len = SyslogBuildPacket (
+          Level,
+          Module,
+          File,
+          Line,
+          Message,
+          NET_SYSLOG_PACKET_LEN,
+          Packet
+          );
+
+  mSyslogPacketSeq++;
+  Status = SyslogSendPacket (Packet, Len);
+  FreePool (Packet);
+
+ON_EXIT:
+  FreePool (Message);
+  return Status;
+}
+/**
+  Return the length of the mask.
+
+  Return the length of the mask, the correct value is from 0 to 32.
+  If the mask is invalid, return the invalid length 33, which is IP4_MASK_NUM.
+  NetMask is in the host byte order.
+
+  @param[in]  NetMask              The netmask to get the length from.
+
+  @return The length of the netmask, IP4_MASK_NUM if the mask is invalid.
+
+**/
+INTN
+EFIAPI
+NetGetMaskLength (
+  IN IP4_ADDR               NetMask
+  )
+{
+  INTN                      Index;
+
+  for (Index = 0; Index < IP4_MASK_NUM; Index++) {
+    if (NetMask == gIp4AllMasks[Index]) {
+      break;
+    }
+  }
+
+  return Index;
+}
+
+
+
+/**
+  Return the class of the IP address, such as class A, B, C.
+  Addr is in host byte order.
+
+  The address of class A  starts with 0.
+  If the address belong to class A, return IP4_ADDR_CLASSA.
+  The address of class B  starts with 10.
+  If the address belong to class B, return IP4_ADDR_CLASSB.
+  The address of class C  starts with 110.
+  If the address belong to class C, return IP4_ADDR_CLASSC.
+  The address of class D  starts with 1110.
+  If the address belong to class D, return IP4_ADDR_CLASSD.
+  The address of class E  starts with 1111.
+  If the address belong to class E, return IP4_ADDR_CLASSE.
+
+
+  @param[in]   Addr                  The address to get the class from.
+
+  @return IP address class, such as IP4_ADDR_CLASSA.
+
+**/
+INTN
+EFIAPI
+NetGetIpClass (
+  IN IP4_ADDR               Addr
+  )
+{
+  UINT8                     ByteOne;
+
+  ByteOne = (UINT8) (Addr >> 24);
+
+  if ((ByteOne & 0x80) == 0) {
+    return IP4_ADDR_CLASSA;
+
+  } else if ((ByteOne & 0xC0) == 0x80) {
+    return IP4_ADDR_CLASSB;
+
+  } else if ((ByteOne & 0xE0) == 0xC0) {
+    return IP4_ADDR_CLASSC;
+
+  } else if ((ByteOne & 0xF0) == 0xE0) {
+    return IP4_ADDR_CLASSD;
+
+  } else {
+    return IP4_ADDR_CLASSE;
+
+  }
+}
+
+
+/**
+  Check whether the IP is a valid unicast address according to
+  the netmask. If NetMask is zero, use the IP address's class to get the default mask.
+
+  If Ip is 0, IP is not a valid unicast address.
+  Class D address is used for multicasting and class E address is reserved for future. If Ip
+  belongs to class D or class E, IP is not a valid unicast address.
+  If all bits of the host address of IP are 0 or 1, IP is also not a valid unicast address.
+
+  @param[in]  Ip                    The IP to check against.
+  @param[in]  NetMask               The mask of the IP.
+
+  @return TRUE if IP is a valid unicast address on the network, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp4IsUnicast (
+  IN IP4_ADDR               Ip,
+  IN IP4_ADDR               NetMask
+  )
+{
+  INTN                      Class;
+
+  Class = NetGetIpClass (Ip);
+
+  if ((Ip == 0) || (Class >= IP4_ADDR_CLASSD)) {
+    return FALSE;
+  }
+
+  if (NetMask == 0) {
+    NetMask = gIp4AllMasks[Class << 3];
+  }
+
+  if (((Ip &~NetMask) == ~NetMask) || ((Ip &~NetMask) == 0)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Check whether the incoming IPv6 address is a valid unicast address.
+
+  If the address is a multicast address has binary 0xFF at the start, it is not
+  a valid unicast address. If the address is unspecified ::, it is not a valid
+  unicast address to be assigned to any node. If the address is loopback address
+  ::1, it is also not a valid unicast address to be assigned to any physical
+  interface.
+
+  @param[in]  Ip6                   The IPv6 address to check against.
+
+  @return TRUE if Ip6 is a valid unicast address on the network, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsValidUnicast (
+  IN EFI_IPv6_ADDRESS       *Ip6
+  )
+{
+  UINT8 Byte;
+  UINT8 Index;
+
+  if (Ip6->Addr[0] == 0xFF) {
+    return FALSE;
+  }
+
+  for (Index = 0; Index < 15; Index++) {
+    if (Ip6->Addr[Index] != 0) {
+      return TRUE;
+    }
+  }
+
+  Byte = Ip6->Addr[Index];
+
+  if (Byte == 0x0 || Byte == 0x1) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Check whether the incoming Ipv6 address is the unspecified address or not.
+
+  @param[in] Ip6   - Ip6 address, in network order.
+
+  @retval TRUE     - Yes, unspecified
+  @retval FALSE    - No
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsUnspecifiedAddr (
+  IN EFI_IPv6_ADDRESS       *Ip6
+  )
+{
+  UINT8 Index;
+
+  for (Index = 0; Index < 16; Index++) {
+    if (Ip6->Addr[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Check whether the incoming Ipv6 address is a link-local address.
+
+  @param[in] Ip6   - Ip6 address, in network order.
+
+  @retval TRUE  - Yes, link-local address
+  @retval FALSE - No
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsLinkLocalAddr (
+  IN EFI_IPv6_ADDRESS *Ip6
+  )
+{
+  UINT8 Index;
+
+  ASSERT (Ip6 != NULL);
+
+  if (Ip6->Addr[0] != 0xFE) {
+    return FALSE;
+  }
+
+  if (Ip6->Addr[1] != 0x80) {
+    return FALSE;
+  }
+
+  for (Index = 2; Index < 8; Index++) {
+    if (Ip6->Addr[Index] != 0) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Check whether the Ipv6 address1 and address2 are on the connected network.
+
+  @param[in] Ip1          - Ip6 address1, in network order.
+  @param[in] Ip2          - Ip6 address2, in network order.
+  @param[in] PrefixLength - The prefix length of the checking net.
+
+  @retval TRUE            - Yes, connected.
+  @retval FALSE           - No.
+
+**/
+BOOLEAN
+EFIAPI
+NetIp6IsNetEqual (
+  EFI_IPv6_ADDRESS *Ip1,
+  EFI_IPv6_ADDRESS *Ip2,
+  UINT8            PrefixLength
+  )
+{
+  UINT8 Byte;
+  UINT8 Bit;
+  UINT8 Mask;
+
+  ASSERT ((Ip1 != NULL) && (Ip2 != NULL) && (PrefixLength < IP6_PREFIX_NUM));
+
+  if (PrefixLength == 0) {
+    return TRUE;
+  }
+
+  Byte = (UINT8) (PrefixLength / 8);
+  Bit  = (UINT8) (PrefixLength % 8);
+
+  if (CompareMem (Ip1, Ip2, Byte) != 0) {
+    return FALSE;
+  }
+
+  if (Bit > 0) {
+    Mask = (UINT8) (0xFF << (8 - Bit));
+
+    ASSERT (Byte < 16);
+    if ((Ip1->Addr[Byte] & Mask) != (Ip2->Addr[Byte] & Mask)) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Switches the endianess of an IPv6 address
+
+  This function swaps the bytes in a 128-bit IPv6 address to switch the value
+  from little endian to big endian or vice versa. The byte swapped value is
+  returned.
+
+  @param  Ip6 Points to an IPv6 address
+
+  @return The byte swapped IPv6 address.
+
+**/
+EFI_IPv6_ADDRESS *
+EFIAPI
+Ip6Swap128 (
+  EFI_IPv6_ADDRESS *Ip6
+  )
+{
+  UINT64 High;
+  UINT64 Low;
+
+  CopyMem (&High, Ip6, sizeof (UINT64));
+  CopyMem (&Low, &Ip6->Addr[8], sizeof (UINT64));
+
+  High = SwapBytes64 (High);
+  Low  = SwapBytes64 (Low);
+
+  CopyMem (Ip6, &Low, sizeof (UINT64));
+  CopyMem (&Ip6->Addr[8], &High, sizeof (UINT64));
+
+  return Ip6;
+}
+
+/**
+  Initialize a random seed using current time and monotonic count.
+
+  Get current time and monotonic count first. Then initialize a random seed 
+  based on some basic mathematics operation on the hour, day, minute, second,
+  nanosecond and year of the current time and the monotonic count value.
+
+  @return The random seed initialized with current time.
+
+**/
+UINT32
+EFIAPI
+NetRandomInitSeed (
+  VOID
+  )
+{
+  EFI_TIME                  Time;
+  UINT32                    Seed;
+  UINT64                    MonotonicCount;
+
+  gRT->GetTime (&Time, NULL);
+  Seed = (~Time.Hour << 24 | Time.Day << 16 | Time.Minute << 8 | Time.Second);
+  Seed ^= Time.Nanosecond;
+  Seed ^= Time.Year << 7;
+
+  gBS->GetNextMonotonicCount (&MonotonicCount);
+  Seed += (UINT32) MonotonicCount;
+
+  return Seed;
+}
+
+
+/**
+  Extract a UINT32 from a byte stream.
+
+  Copy a UINT32 from a byte stream, then converts it from Network
+  byte order to host byte order. Use this function to avoid alignment error.
+
+  @param[in]  Buf                 The buffer to extract the UINT32.
+
+  @return The UINT32 extracted.
+
+**/
+UINT32
+EFIAPI
+NetGetUint32 (
+  IN UINT8                  *Buf
+  )
+{
+  UINT32                    Value;
+
+  CopyMem (&Value, Buf, sizeof (UINT32));
+  return NTOHL (Value);
+}
+
+
+/**
+  Put a UINT32 to the byte stream in network byte order.
+
+  Converts a UINT32 from host byte order to network byte order. Then copy it to the
+  byte stream.
+
+  @param[in, out]  Buf          The buffer to put the UINT32.
+  @param[in]       Data         The data to be converted and put into the byte stream.
+
+**/
+VOID
+EFIAPI
+NetPutUint32 (
+  IN OUT UINT8                 *Buf,
+  IN     UINT32                Data
+  )
+{
+  Data = HTONL (Data);
+  CopyMem (Buf, &Data, sizeof (UINT32));
+}
+
+
+/**
+  Remove the first node entry on the list, and return the removed node entry.
+
+  Removes the first node Entry from a doubly linked list. It is up to the caller of
+  this function to release the memory used by the first node if that is required. On
+  exit, the removed node is returned.
+
+  If Head is NULL, then ASSERT().
+  If Head was not initialized, then ASSERT().
+  If PcdMaximumLinkedListLength is not zero, and the number of nodes in the
+  linked list including the head node is greater than or equal to PcdMaximumLinkedListLength,
+  then ASSERT().
+
+  @param[in, out]  Head                  The list header.
+
+  @return The first node entry that is removed from the list, NULL if the list is empty.
+
+**/
+LIST_ENTRY *
+EFIAPI
+NetListRemoveHead (
+  IN OUT LIST_ENTRY            *Head
+  )
+{
+  LIST_ENTRY            *First;
+
+  ASSERT (Head != NULL);
+
+  if (IsListEmpty (Head)) {
+    return NULL;
+  }
+
+  First                         = Head->ForwardLink;
+  Head->ForwardLink             = First->ForwardLink;
+  First->ForwardLink->BackLink  = Head;
+
+  DEBUG_CODE (
+    First->ForwardLink  = (LIST_ENTRY *) NULL;
+    First->BackLink     = (LIST_ENTRY *) NULL;
+  );
+
+  return First;
+}
+
+
+/**
+  Remove the last node entry on the list and and return the removed node entry.
+
+  Removes the last node entry from a doubly linked list. It is up to the caller of
+  this function to release the memory used by the first node if that is required. On
+  exit, the removed node is returned.
+
+  If Head is NULL, then ASSERT().
+  If Head was not initialized, then ASSERT().
+  If PcdMaximumLinkedListLength is not zero, and the number of nodes in the
+  linked list including the head node is greater than or equal to PcdMaximumLinkedListLength,
+  then ASSERT().
+
+  @param[in, out]  Head                  The list head.
+
+  @return The last node entry that is removed from the list, NULL if the list is empty.
+
+**/
+LIST_ENTRY *
+EFIAPI
+NetListRemoveTail (
+  IN OUT LIST_ENTRY            *Head
+  )
+{
+  LIST_ENTRY            *Last;
+
+  ASSERT (Head != NULL);
+
+  if (IsListEmpty (Head)) {
+    return NULL;
+  }
+
+  Last                        = Head->BackLink;
+  Head->BackLink              = Last->BackLink;
+  Last->BackLink->ForwardLink = Head;
+
+  DEBUG_CODE (
+    Last->ForwardLink = (LIST_ENTRY *) NULL;
+    Last->BackLink    = (LIST_ENTRY *) NULL;
+  );
+
+  return Last;
+}
+
+
+/**
+  Insert a new node entry after a designated node entry of a doubly linked list.
+
+  Inserts a new node entry donated by NewEntry after the node entry donated by PrevEntry
+  of the doubly linked list.
+
+  @param[in, out]  PrevEntry             The previous entry to insert after.
+  @param[in, out]  NewEntry              The new entry to insert.
+
+**/
+VOID
+EFIAPI
+NetListInsertAfter (
+  IN OUT LIST_ENTRY         *PrevEntry,
+  IN OUT LIST_ENTRY         *NewEntry
+  )
+{
+  NewEntry->BackLink                = PrevEntry;
+  NewEntry->ForwardLink             = PrevEntry->ForwardLink;
+  PrevEntry->ForwardLink->BackLink  = NewEntry;
+  PrevEntry->ForwardLink            = NewEntry;
+}
+
+
+/**
+  Insert a new node entry before a designated node entry of a doubly linked list.
+
+  Inserts a new node entry donated by NewEntry after the node entry donated by PostEntry
+  of the doubly linked list.
+
+  @param[in, out]  PostEntry             The entry to insert before.
+  @param[in, out]  NewEntry              The new entry to insert.
+
+**/
+VOID
+EFIAPI
+NetListInsertBefore (
+  IN OUT LIST_ENTRY     *PostEntry,
+  IN OUT LIST_ENTRY     *NewEntry
+  )
+{
+  NewEntry->ForwardLink             = PostEntry;
+  NewEntry->BackLink                = PostEntry->BackLink;
+  PostEntry->BackLink->ForwardLink  = NewEntry;
+  PostEntry->BackLink               = NewEntry;
+}
+
+/**
+  Safe destroy nodes in a linked list, and return the length of the list after all possible operations finished.
+
+  Destroy network child instance list by list traversals is not safe due to graph dependencies between nodes.
+  This function performs a safe traversal to destroy these nodes by checking to see if the node being destroyed
+  has been removed from the list or not.
+  If it has been removed, then restart the traversal from the head.
+  If it hasn't been removed, then continue with the next node directly.
+  This function will end the iterate and return the CallBack's last return value if error happens,
+  or retrun EFI_SUCCESS if 2 complete passes are made with no changes in the number of children in the list.  
+
+  @param[in]    List             The head of the list.
+  @param[in]    CallBack         Pointer to the callback function to destroy one node in the list.
+  @param[in]    Context          Pointer to the callback function's context: corresponds to the
+                                 parameter Context in NET_DESTROY_LINK_LIST_CALLBACK.
+  @param[out]   ListLength       The length of the link list if the function returns successfully.
+
+  @retval EFI_SUCCESS            Two complete passes are made with no changes in the number of children.
+  @retval EFI_INVALID_PARAMETER  The input parameter is invalid.
+  @retval Others                 Return the CallBack's last return value.
+
+**/
+EFI_STATUS
+EFIAPI
+NetDestroyLinkList (
+  IN   LIST_ENTRY                       *List,
+  IN   NET_DESTROY_LINK_LIST_CALLBACK   CallBack,
+  IN   VOID                             *Context,    OPTIONAL
+  OUT  UINTN                            *ListLength  OPTIONAL
+  )
+{
+  UINTN                         PreviousLength;
+  LIST_ENTRY                    *Entry;
+  LIST_ENTRY                    *Ptr;
+  UINTN                         Length;
+  EFI_STATUS                    Status;
+
+  if (List == NULL || CallBack == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Length = 0;
+  do {
+    PreviousLength = Length;
+    Entry = GetFirstNode (List);
+    while (!IsNull (List, Entry)) {
+      Status = CallBack (Entry, Context);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      //
+      // Walk through the list to see whether the Entry has been removed or not.
+      // If the Entry still exists, just try to destroy the next one.
+      // If not, go back to the start point to iterate the list again.
+      //
+      for (Ptr = List->ForwardLink; Ptr != List; Ptr = Ptr->ForwardLink) {
+        if (Ptr == Entry) {
+          break;
+        }
+      }
+      if (Ptr == Entry) {
+        Entry = GetNextNode (List, Entry);
+      } else {
+        Entry = GetFirstNode (List);
+      }
+    }
+    for (Length = 0, Ptr = List->ForwardLink; Ptr != List; Length++, Ptr = Ptr->ForwardLink);
+  } while (Length != PreviousLength);
+
+  if (ListLength != NULL) {
+    *ListLength = Length;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  This function checks the input Handle to see if it's one of these handles in ChildHandleBuffer.
+
+  @param[in]  Handle             Handle to be checked.
+  @param[in]  NumberOfChildren   Number of Handles in ChildHandleBuffer.
+  @param[in]  ChildHandleBuffer  An array of child handles to be freed. May be NULL
+                                 if NumberOfChildren is 0.
+
+  @retval TURE                   Found the input Handle in ChildHandleBuffer.
+  @retval FALSE                  Can't find the input Handle in ChildHandleBuffer.
+
+**/
+BOOLEAN
+EFIAPI
+NetIsInHandleBuffer (
+  IN  EFI_HANDLE          Handle,
+  IN  UINTN               NumberOfChildren,
+  IN  EFI_HANDLE          *ChildHandleBuffer OPTIONAL
+  )
+{
+  UINTN     Index;
+  
+  if (NumberOfChildren == 0 || ChildHandleBuffer == NULL) {
+    return FALSE;
+  }
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    if (Handle == ChildHandleBuffer[Index]) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Initialize the netmap. Netmap is a reposity to keep the <Key, Value> pairs.
+
+  Initialize the forward and backward links of two head nodes donated by Map->Used
+  and Map->Recycled of two doubly linked lists.
+  Initializes the count of the <Key, Value> pairs in the netmap to zero.
+
+  If Map is NULL, then ASSERT().
+  If the address of Map->Used is NULL, then ASSERT().
+  If the address of Map->Recycled is NULl, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to initialize.
+
+**/
+VOID
+EFIAPI
+NetMapInit (
+  IN OUT NET_MAP                *Map
+  )
+{
+  ASSERT (Map != NULL);
+
+  InitializeListHead (&Map->Used);
+  InitializeListHead (&Map->Recycled);
+  Map->Count = 0;
+}
+
+
+/**
+  To clean up the netmap, that is, release allocated memories.
+
+  Removes all nodes of the Used doubly linked list and free memory of all related netmap items.
+  Removes all nodes of the Recycled doubly linked list and free memory of all related netmap items.
+  The number of the <Key, Value> pairs in the netmap is set to be zero.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to clean up.
+
+**/
+VOID
+EFIAPI
+NetMapClean (
+  IN OUT NET_MAP            *Map
+  )
+{
+  NET_MAP_ITEM              *Item;
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+
+  ASSERT (Map != NULL);
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Map->Used) {
+    Item = NET_LIST_USER_STRUCT (Entry, NET_MAP_ITEM, Link);
+
+    RemoveEntryList (&Item->Link);
+    Map->Count--;
+
+    gBS->FreePool (Item);
+  }
+
+  ASSERT ((Map->Count == 0) && IsListEmpty (&Map->Used));
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Map->Recycled) {
+    Item = NET_LIST_USER_STRUCT (Entry, NET_MAP_ITEM, Link);
+
+    RemoveEntryList (&Item->Link);
+    gBS->FreePool (Item);
+  }
+
+  ASSERT (IsListEmpty (&Map->Recycled));
+}
+
+
+/**
+  Test whether the netmap is empty and return true if it is.
+
+  If the number of the <Key, Value> pairs in the netmap is zero, return TRUE.
+
+  If Map is NULL, then ASSERT().
+
+
+  @param[in]  Map                   The net map to test.
+
+  @return TRUE if the netmap is empty, otherwise FALSE.
+
+**/
+BOOLEAN
+EFIAPI
+NetMapIsEmpty (
+  IN NET_MAP                *Map
+  )
+{
+  ASSERT (Map != NULL);
+  return (BOOLEAN) (Map->Count == 0);
+}
+
+
+/**
+  Return the number of the <Key, Value> pairs in the netmap.
+
+  @param[in]  Map                   The netmap to get the entry number.
+
+  @return The entry number in the netmap.
+
+**/
+UINTN
+EFIAPI
+NetMapGetCount (
+  IN NET_MAP                *Map
+  )
+{
+  return Map->Count;
+}
+
+
+/**
+  Return one allocated item.
+
+  If the Recycled doubly linked list of the netmap is empty, it will try to allocate
+  a batch of items if there are enough resources and add corresponding nodes to the begining
+  of the Recycled doubly linked list of the netmap. Otherwise, it will directly remove
+  the fist node entry of the Recycled doubly linked list and return the corresponding item.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map          The netmap to allocate item for.
+
+  @return                       The allocated item. If NULL, the
+                                allocation failed due to resource limit.
+
+**/
+NET_MAP_ITEM *
+NetMapAllocItem (
+  IN OUT NET_MAP            *Map
+  )
+{
+  NET_MAP_ITEM              *Item;
+  LIST_ENTRY                *Head;
+  UINTN                     Index;
+
+  ASSERT (Map != NULL);
+
+  Head = &Map->Recycled;
+
+  if (IsListEmpty (Head)) {
+    for (Index = 0; Index < NET_MAP_INCREAMENT; Index++) {
+      Item = AllocatePool (sizeof (NET_MAP_ITEM));
+
+      if (Item == NULL) {
+        if (Index == 0) {
+          return NULL;
+        }
+
+        break;
+      }
+
+      InsertHeadList (Head, &Item->Link);
+    }
+  }
+
+  Item = NET_LIST_HEAD (Head, NET_MAP_ITEM, Link);
+  NetListRemoveHead (Head);
+
+  return Item;
+}
+
+
+/**
+  Allocate an item to save the <Key, Value> pair to the head of the netmap.
+
+  Allocate an item to save the <Key, Value> pair and add corresponding node entry
+  to the beginning of the Used doubly linked list. The number of the <Key, Value>
+  pairs in the netmap increase by 1.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to insert into.
+  @param[in]       Key                   The user's key.
+  @param[in]       Value                 The user's value for the key.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the item.
+  @retval EFI_SUCCESS           The item is inserted to the head.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapInsertHead (
+  IN OUT NET_MAP            *Map,
+  IN VOID                   *Key,
+  IN VOID                   *Value    OPTIONAL
+  )
+{
+  NET_MAP_ITEM              *Item;
+
+  ASSERT (Map != NULL);
+
+  Item = NetMapAllocItem (Map);
+
+  if (Item == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Item->Key   = Key;
+  Item->Value = Value;
+  InsertHeadList (&Map->Used, &Item->Link);
+
+  Map->Count++;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate an item to save the <Key, Value> pair to the tail of the netmap.
+
+  Allocate an item to save the <Key, Value> pair and add corresponding node entry
+  to the tail of the Used doubly linked list. The number of the <Key, Value>
+  pairs in the netmap increase by 1.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to insert into.
+  @param[in]       Key                   The user's key.
+  @param[in]       Value                 The user's value for the key.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the item.
+  @retval EFI_SUCCESS           The item is inserted to the tail.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapInsertTail (
+  IN OUT NET_MAP            *Map,
+  IN VOID                   *Key,
+  IN VOID                   *Value    OPTIONAL
+  )
+{
+  NET_MAP_ITEM              *Item;
+
+  ASSERT (Map != NULL);
+
+  Item = NetMapAllocItem (Map);
+
+  if (Item == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Item->Key   = Key;
+  Item->Value = Value;
+  InsertTailList (&Map->Used, &Item->Link);
+
+  Map->Count++;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether the item is in the Map and return TRUE if it is.
+
+  @param[in]  Map                   The netmap to search within.
+  @param[in]  Item                  The item to search.
+
+  @return TRUE if the item is in the netmap, otherwise FALSE.
+
+**/
+BOOLEAN
+NetItemInMap (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item
+  )
+{
+  LIST_ENTRY            *ListEntry;
+
+  NET_LIST_FOR_EACH (ListEntry, &Map->Used) {
+    if (ListEntry == &Item->Link) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Find the key in the netmap and returns the point to the item contains the Key.
+
+  Iterate the Used doubly linked list of the netmap to get every item. Compare the key of every
+  item with the key to search. It returns the point to the item contains the Key if found.
+
+  If Map is NULL, then ASSERT().
+
+  @param[in]  Map                   The netmap to search within.
+  @param[in]  Key                   The key to search.
+
+  @return The point to the item contains the Key, or NULL if Key isn't in the map.
+
+**/
+NET_MAP_ITEM *
+EFIAPI
+NetMapFindKey (
+  IN  NET_MAP               *Map,
+  IN  VOID                  *Key
+  )
+{
+  LIST_ENTRY              *Entry;
+  NET_MAP_ITEM            *Item;
+
+  ASSERT (Map != NULL);
+
+  NET_LIST_FOR_EACH (Entry, &Map->Used) {
+    Item = NET_LIST_USER_STRUCT (Entry, NET_MAP_ITEM, Link);
+
+    if (Item->Key == Key) {
+      return Item;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Remove the node entry of the item from the netmap and return the key of the removed item.
+
+  Remove the node entry of the item from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry of the item to the Recycled doubly linked list of the netmap. If Value is not NULL,
+  Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If Item is NULL, then ASSERT().
+  if item in not in the netmap, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the item from.
+  @param[in, out]  Item                  The item to remove.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the removed item.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveItem (
+  IN  OUT NET_MAP             *Map,
+  IN  OUT NET_MAP_ITEM        *Item,
+  OUT VOID                    **Value           OPTIONAL
+  )
+{
+  ASSERT ((Map != NULL) && (Item != NULL));
+  ASSERT (NetItemInMap (Map, Item));
+
+  RemoveEntryList (&Item->Link);
+  Map->Count--;
+  InsertHeadList (&Map->Recycled, &Item->Link);
+
+  if (Value != NULL) {
+    *Value = Item->Value;
+  }
+
+  return Item->Key;
+}
+
+
+/**
+  Remove the first node entry on the netmap and return the key of the removed item.
+
+  Remove the first node entry from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry to the Recycled doubly linked list of the netmap. If parameter Value is not NULL,
+  parameter Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If the Used doubly linked list is empty, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the head from.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the item removed.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveHead (
+  IN OUT NET_MAP            *Map,
+  OUT VOID                  **Value         OPTIONAL
+  )
+{
+  NET_MAP_ITEM  *Item;
+
+  //
+  // Often, it indicates a programming error to remove
+  // the first entry in an empty list
+  //
+  ASSERT (Map && !IsListEmpty (&Map->Used));
+
+  Item = NET_LIST_HEAD (&Map->Used, NET_MAP_ITEM, Link);
+  RemoveEntryList (&Item->Link);
+  Map->Count--;
+  InsertHeadList (&Map->Recycled, &Item->Link);
+
+  if (Value != NULL) {
+    *Value = Item->Value;
+  }
+
+  return Item->Key;
+}
+
+
+/**
+  Remove the last node entry on the netmap and return the key of the removed item.
+
+  Remove the last node entry from the Used doubly linked list of the netmap.
+  The number of the <Key, Value> pairs in the netmap decrease by 1. Then add the node
+  entry to the Recycled doubly linked list of the netmap. If parameter Value is not NULL,
+  parameter Value will point to the value of the item. It returns the key of the removed item.
+
+  If Map is NULL, then ASSERT().
+  If the Used doubly linked list is empty, then ASSERT().
+
+  @param[in, out]  Map                   The netmap to remove the tail from.
+  @param[out]      Value                 The variable to receive the value if not NULL.
+
+  @return                                The key of the item removed.
+
+**/
+VOID *
+EFIAPI
+NetMapRemoveTail (
+  IN OUT NET_MAP            *Map,
+  OUT VOID                  **Value       OPTIONAL
+  )
+{
+  NET_MAP_ITEM              *Item;
+
+  //
+  // Often, it indicates a programming error to remove
+  // the last entry in an empty list
+  //
+  ASSERT (Map && !IsListEmpty (&Map->Used));
+
+  Item = NET_LIST_TAIL (&Map->Used, NET_MAP_ITEM, Link);
+  RemoveEntryList (&Item->Link);
+  Map->Count--;
+  InsertHeadList (&Map->Recycled, &Item->Link);
+
+  if (Value != NULL) {
+    *Value = Item->Value;
+  }
+
+  return Item->Key;
+}
+
+
+/**
+  Iterate through the netmap and call CallBack for each item.
+
+  It will contiue the traverse if CallBack returns EFI_SUCCESS, otherwise, break
+  from the loop. It returns the CallBack's last return value. This function is
+  delete safe for the current item.
+
+  If Map is NULL, then ASSERT().
+  If CallBack is NULL, then ASSERT().
+
+  @param[in]  Map                   The Map to iterate through.
+  @param[in]  CallBack              The callback function to call for each item.
+  @param[in]  Arg                   The opaque parameter to the callback.
+
+  @retval EFI_SUCCESS            There is no item in the netmap or CallBack for each item
+                                 return EFI_SUCCESS.
+  @retval Others                 It returns the CallBack's last return value.
+
+**/
+EFI_STATUS
+EFIAPI
+NetMapIterate (
+  IN NET_MAP                *Map,
+  IN NET_MAP_CALLBACK       CallBack,
+  IN VOID                   *Arg      OPTIONAL
+  )
+{
+
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *Next;
+  LIST_ENTRY            *Head;
+  NET_MAP_ITEM          *Item;
+  EFI_STATUS            Result;
+
+  ASSERT ((Map != NULL) && (CallBack != NULL));
+
+  Head = &Map->Used;
+
+  if (IsListEmpty (Head)) {
+    return EFI_SUCCESS;
+  }
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {
+    Item   = NET_LIST_USER_STRUCT (Entry, NET_MAP_ITEM, Link);
+    Result = CallBack (Map, Item, Arg);
+
+    if (EFI_ERROR (Result)) {
+      return Result;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This is the default unload handle for all the network drivers.
+
+  Disconnect the driver specified by ImageHandle from all the devices in the handle database.
+  Uninstall all the protocols installed in the driver entry point.
+
+  @param[in]  ImageHandle       The drivers' driver image.
+
+  @retval EFI_SUCCESS           The image is unloaded.
+  @retval Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDefaultUnload (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             DeviceHandleCount;
+  UINTN                             Index;
+  UINTN                             Index2;
+  EFI_DRIVER_BINDING_PROTOCOL       *DriverBinding;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Get the list of all the handles in the handle database.
+  // If there is an error getting the list, then the unload
+  // operation fails.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  AllHandles,
+                  NULL,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  for (Index = 0; Index < DeviceHandleCount; Index++) {
+    Status = gBS->HandleProtocol (
+                    DeviceHandleBuffer[Index],
+                    &gEfiDriverBindingProtocolGuid,
+                    (VOID **) &DriverBinding
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (DriverBinding->ImageHandle != ImageHandle) {
+      continue;
+    }
+    
+    //
+    // Disconnect the driver specified by ImageHandle from all
+    // the devices in the handle database.
+    //
+    for (Index2 = 0; Index2 < DeviceHandleCount; Index2++) {
+      Status = gBS->DisconnectController (
+                      DeviceHandleBuffer[Index2],
+                      DriverBinding->DriverBindingHandle,
+                      NULL
+                      );
+    }
+    
+    //
+    // Uninstall all the protocols installed in the driver entry point
+    //    
+    gBS->UninstallProtocolInterface (
+          DriverBinding->DriverBindingHandle,
+          &gEfiDriverBindingProtocolGuid,
+          DriverBinding
+          );
+    
+    Status = gBS->HandleProtocol (
+                    DeviceHandleBuffer[Index],
+                    &gEfiComponentNameProtocolGuid,
+                    (VOID **) &ComponentName
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallProtocolInterface (
+             DriverBinding->DriverBindingHandle,
+             &gEfiComponentNameProtocolGuid,
+             ComponentName
+             );
+    }
+
+    Status = gBS->HandleProtocol (
+                    DeviceHandleBuffer[Index],
+                    &gEfiComponentName2ProtocolGuid,
+                    (VOID **) &ComponentName2
+                    );
+    if (!EFI_ERROR (Status)) {
+      gBS->UninstallProtocolInterface (
+             DriverBinding->DriverBindingHandle,
+             &gEfiComponentName2ProtocolGuid,
+             ComponentName2
+             );
+    }
+  }
+
+  //
+  // Free the buffer containing the list of handles from the handle database
+  //
+  if (DeviceHandleBuffer != NULL) {
+    gBS->FreePool (DeviceHandleBuffer);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Create a child of the service that is identified by ServiceBindingGuid.
+
+  Get the ServiceBinding Protocol first, then use it to create a child.
+
+  If ServiceBindingGuid is NULL, then ASSERT().
+  If ChildHandle is NULL, then ASSERT().
+
+  @param[in]       Controller            The controller which has the service installed.
+  @param[in]       Image                 The image handle used to open service.
+  @param[in]       ServiceBindingGuid    The service's Guid.
+  @param[in, out]  ChildHandle           The handle to receive the create child.
+
+  @retval EFI_SUCCESS           The child is successfully created.
+  @retval Others                Failed to create the child.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibCreateServiceChild (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            Image,
+  IN  EFI_GUID              *ServiceBindingGuid,
+  IN  OUT EFI_HANDLE        *ChildHandle
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SERVICE_BINDING_PROTOCOL  *Service;
+
+
+  ASSERT ((ServiceBindingGuid != NULL) && (ChildHandle != NULL));
+
+  //
+  // Get the ServiceBinding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  ServiceBindingGuid,
+                  (VOID **) &Service,
+                  Image,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Create a child
+  //
+  Status = Service->CreateChild (Service, ChildHandle);
+  return Status;
+}
+
+
+/**
+  Destroy a child of the service that is identified by ServiceBindingGuid.
+
+  Get the ServiceBinding Protocol first, then use it to destroy a child.
+
+  If ServiceBindingGuid is NULL, then ASSERT().
+
+  @param[in]   Controller            The controller which has the service installed.
+  @param[in]   Image                 The image handle used to open service.
+  @param[in]   ServiceBindingGuid    The service's Guid.
+  @param[in]   ChildHandle           The child to destroy.
+
+  @retval EFI_SUCCESS           The child is successfully destroyed.
+  @retval Others                Failed to destroy the child.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDestroyServiceChild (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            Image,
+  IN  EFI_GUID              *ServiceBindingGuid,
+  IN  EFI_HANDLE            ChildHandle
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SERVICE_BINDING_PROTOCOL  *Service;
+
+  ASSERT (ServiceBindingGuid != NULL);
+
+  //
+  // Get the ServiceBinding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  ServiceBindingGuid,
+                  (VOID **) &Service,
+                  Image,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // destroy the child
+  //
+  Status = Service->DestroyChild (Service, ChildHandle);
+  return Status;
+}
+
+/**
+  Get handle with Simple Network Protocol installed on it.
+
+  There should be MNP Service Binding Protocol installed on the input ServiceHandle.
+  If Simple Network Protocol is already installed on the ServiceHandle, the
+  ServiceHandle will be returned. If SNP is not installed on the ServiceHandle,
+  try to find its parent handle with SNP installed.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+  @param[out]  Snp              The pointer to store the address of the SNP instance.
+                                This is an optional parameter that may be NULL.
+
+  @return The SNP handle, or NULL if not found.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetSnpHandle (
+  IN   EFI_HANDLE                  ServiceHandle,
+  OUT  EFI_SIMPLE_NETWORK_PROTOCOL **Snp  OPTIONAL
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL  *SnpInstance;
+  EFI_DEVICE_PATH_PROTOCOL     *DevicePath;
+  EFI_HANDLE                   SnpHandle;
+
+  //
+  // Try to open SNP from ServiceHandle
+  //
+  SnpInstance = NULL;
+  Status = gBS->HandleProtocol (ServiceHandle, &gEfiSimpleNetworkProtocolGuid, (VOID **) &SnpInstance);
+  if (!EFI_ERROR (Status)) {
+    if (Snp != NULL) {
+      *Snp = SnpInstance;
+    }
+    return ServiceHandle;
+  }
+
+  //
+  // Failed to open SNP, try to get SNP handle by LocateDevicePath()
+  //
+  DevicePath = DevicePathFromHandle (ServiceHandle);
+  if (DevicePath == NULL) {
+    return NULL;
+  }
+
+  SnpHandle = NULL;
+  Status = gBS->LocateDevicePath (&gEfiSimpleNetworkProtocolGuid, &DevicePath, &SnpHandle);
+  if (EFI_ERROR (Status)) {
+    //
+    // Failed to find SNP handle
+    //
+    return NULL;
+  }
+
+  Status = gBS->HandleProtocol (SnpHandle, &gEfiSimpleNetworkProtocolGuid, (VOID **) &SnpInstance);
+  if (!EFI_ERROR (Status)) {
+    if (Snp != NULL) {
+      *Snp = SnpInstance;
+    }
+    return SnpHandle;
+  }
+
+  return NULL;
+}
+
+/**
+  Retrieve VLAN ID of a VLAN device handle.
+
+  Search VLAN device path node in Device Path of specified ServiceHandle and
+  return its VLAN ID. If no VLAN device path node found, then this ServiceHandle
+  is not a VLAN device handle, and 0 will be returned.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+
+  @return VLAN ID of the device handle, or 0 if not a VLAN device.
+
+**/
+UINT16
+EFIAPI
+NetLibGetVlanId (
+  IN EFI_HANDLE             ServiceHandle
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+
+  DevicePath = DevicePathFromHandle (ServiceHandle);
+  if (DevicePath == NULL) {
+    return 0;
+  }
+
+  Node = DevicePath;
+  while (!IsDevicePathEnd (Node)) {
+    if (Node->Type == MESSAGING_DEVICE_PATH && Node->SubType == MSG_VLAN_DP) {
+      return ((VLAN_DEVICE_PATH *) Node)->VlanId;
+    }
+    Node = NextDevicePathNode (Node);
+  }
+
+  return 0;
+}
+
+/**
+  Find VLAN device handle with specified VLAN ID.
+
+  The VLAN child device handle is created by VLAN Config Protocol on ControllerHandle.
+  This function will append VLAN device path node to the parent device path,
+  and then use LocateDevicePath() to find the correct VLAN device handle.
+
+  @param[in]   ControllerHandle The handle where network service binding protocols are
+                                installed on.
+  @param[in]   VlanId           The configured VLAN ID for the VLAN device.
+
+  @return The VLAN device handle, or NULL if not found.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetVlanHandle (
+  IN EFI_HANDLE             ControllerHandle,
+  IN UINT16                 VlanId
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *VlanDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  VLAN_DEVICE_PATH          VlanNode;
+  EFI_HANDLE                Handle;
+
+  ParentDevicePath = DevicePathFromHandle (ControllerHandle);
+  if (ParentDevicePath == NULL) {
+    return NULL;
+  }
+
+  //
+  // Construct VLAN device path
+  //
+  CopyMem (&VlanNode, &mNetVlanDevicePathTemplate, sizeof (VLAN_DEVICE_PATH));
+  VlanNode.VlanId = VlanId;
+  VlanDevicePath = AppendDevicePathNode (
+                     ParentDevicePath,
+                     (EFI_DEVICE_PATH_PROTOCOL *) &VlanNode
+                     );
+  if (VlanDevicePath == NULL) {
+    return NULL;
+  }
+
+  //
+  // Find VLAN device handle
+  //
+  Handle = NULL;
+  DevicePath = VlanDevicePath;
+  gBS->LocateDevicePath (
+         &gEfiDevicePathProtocolGuid,
+         &DevicePath,
+         &Handle
+         );
+  if (!IsDevicePathEnd (DevicePath)) {
+    //
+    // Device path is not exactly match
+    //
+    Handle = NULL;
+  }
+
+  FreePool (VlanDevicePath);
+  return Handle;
+}
+
+/**
+  Get MAC address associated with the network service handle.
+
+  There should be MNP Service Binding Protocol installed on the input ServiceHandle.
+  If SNP is installed on the ServiceHandle or its parent handle, MAC address will
+  be retrieved from SNP. If no SNP found, try to get SNP mode data use MNP.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+  @param[out]  MacAddress       The pointer to store the returned MAC address.
+  @param[out]  AddressSize      The length of returned MAC address.
+
+  @retval EFI_SUCCESS           MAC address is returned successfully.
+  @retval Others                Failed to get SNP mode data.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetMacAddress (
+  IN  EFI_HANDLE            ServiceHandle,
+  OUT EFI_MAC_ADDRESS       *MacAddress,
+  OUT UINTN                 *AddressSize
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL  *Snp;
+  EFI_SIMPLE_NETWORK_MODE      *SnpMode;
+  EFI_SIMPLE_NETWORK_MODE      SnpModeData;
+  EFI_MANAGED_NETWORK_PROTOCOL *Mnp;
+  EFI_SERVICE_BINDING_PROTOCOL *MnpSb;
+  EFI_HANDLE                   *SnpHandle;
+  EFI_HANDLE                   MnpChildHandle;
+
+  ASSERT (MacAddress != NULL);
+  ASSERT (AddressSize != NULL);
+
+  //
+  // Try to get SNP handle
+  //
+  Snp = NULL;
+  SnpHandle = NetLibGetSnpHandle (ServiceHandle, &Snp);
+  if (SnpHandle != NULL) {
+    //
+    // SNP found, use it directly
+    //
+    SnpMode = Snp->Mode;
+  } else {
+    //
+    // Failed to get SNP handle, try to get MAC address from MNP
+    //
+    MnpChildHandle = NULL;
+    Status = gBS->HandleProtocol (
+                    ServiceHandle,
+                    &gEfiManagedNetworkServiceBindingProtocolGuid,
+                    (VOID **) &MnpSb
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Create a MNP child
+    //
+    Status = MnpSb->CreateChild (MnpSb, &MnpChildHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Open MNP protocol
+    //
+    Status = gBS->HandleProtocol (
+                    MnpChildHandle,
+                    &gEfiManagedNetworkProtocolGuid,
+                    (VOID **) &Mnp
+                    );
+    if (EFI_ERROR (Status)) {
+      MnpSb->DestroyChild (MnpSb, MnpChildHandle);
+      return Status;
+    }
+
+    //
+    // Try to get SNP mode from MNP
+    //
+    Status = Mnp->GetModeData (Mnp, NULL, &SnpModeData);
+    if (EFI_ERROR (Status) && (Status != EFI_NOT_STARTED)) {
+      MnpSb->DestroyChild (MnpSb, MnpChildHandle);
+      return Status;
+    }
+    SnpMode = &SnpModeData;
+
+    //
+    // Destroy the MNP child
+    //
+    MnpSb->DestroyChild (MnpSb, MnpChildHandle);
+  }
+
+  *AddressSize = SnpMode->HwAddressSize;
+  CopyMem (MacAddress->Addr, SnpMode->CurrentAddress.Addr, SnpMode->HwAddressSize);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert MAC address of the NIC associated with specified Service Binding Handle
+  to a unicode string. Callers are responsible for freeing the string storage.
+
+  Locate simple network protocol associated with the Service Binding Handle and
+  get the mac address from SNP. Then convert the mac address into a unicode
+  string. It takes 2 unicode characters to represent a 1 byte binary buffer.
+  Plus one unicode character for the null-terminator.
+
+  @param[in]   ServiceHandle         The handle where network service binding protocol is
+                                     installed on.
+  @param[in]   ImageHandle           The image handle used to act as the agent handle to
+                                     get the simple network protocol. This parameter is
+                                     optional and may be NULL.
+  @param[out]  MacString             The pointer to store the address of the string
+                                     representation of  the mac address.
+
+  @retval EFI_SUCCESS           Convert the mac address a unicode string successfully.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough memory resource.
+  @retval Others                Failed to open the simple network protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetMacString (
+  IN  EFI_HANDLE            ServiceHandle,
+  IN  EFI_HANDLE            ImageHandle, OPTIONAL
+  OUT CHAR16                **MacString
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_MAC_ADDRESS              MacAddress;
+  UINT8                        *HwAddress;
+  UINTN                        HwAddressSize;
+  UINT16                       VlanId;
+  CHAR16                       *String;
+  UINTN                        Index;
+
+  ASSERT (MacString != NULL);
+
+  //
+  // Get MAC address of the network device
+  //
+  Status = NetLibGetMacAddress (ServiceHandle, &MacAddress, &HwAddressSize);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // It takes 2 unicode characters to represent a 1 byte binary buffer.
+  // If VLAN is configured, it will need extra 5 characters like "\0005".
+  // Plus one unicode character for the null-terminator.
+  //
+  String = AllocateZeroPool ((2 * HwAddressSize + 5 + 1) * sizeof (CHAR16));
+  if (String == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  *MacString = String;
+
+  //
+  // Convert the MAC address into a unicode string.
+  //
+  HwAddress = &MacAddress.Addr[0];
+  for (Index = 0; Index < HwAddressSize; Index++) {
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(HwAddress++), 2);
+  }
+
+  //
+  // Append VLAN ID if any
+  //
+  VlanId = NetLibGetVlanId (ServiceHandle);
+  if (VlanId != 0) {
+    *String++ = L'\\';
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, VlanId, 4);
+  }
+
+  //
+  // Null terminate the Unicode string
+  //
+  *String = L'\0';
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detect media status for specified network device.
+
+  The underlying UNDI driver may or may not support reporting media status from
+  GET_STATUS command (PXE_STATFLAGS_GET_STATUS_NO_MEDIA_SUPPORTED). This routine
+  will try to invoke Snp->GetStatus() to get the media status: if media already
+  present, it return directly; if media not present, it will stop SNP and then
+  restart SNP to get the latest media status, this give chance to get the correct
+  media status for old UNDI driver which doesn't support reporting media status
+  from GET_STATUS command.
+  Note: there will be two limitations for current algorithm:
+  1) for UNDI with this capability, in case of cable is not attached, there will
+     be an redundant Stop/Start() process;
+  2) for UNDI without this capability, in case that network cable is attached when
+     Snp->Initialize() is invoked while network cable is unattached later,
+     NetLibDetectMedia() will report MediaPresent as TRUE, causing upper layer
+     apps to wait for timeout time.
+
+  @param[in]   ServiceHandle    The handle where network service binding protocols are
+                                installed on.
+  @param[out]  MediaPresent     The pointer to store the media status.
+
+  @retval EFI_SUCCESS           Media detection success.
+  @retval EFI_INVALID_PARAMETER ServiceHandle is not valid network device handle.
+  @retval EFI_UNSUPPORTED       Network device does not support media detection.
+  @retval EFI_DEVICE_ERROR      SNP is in unknown state.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibDetectMedia (
+  IN  EFI_HANDLE            ServiceHandle,
+  OUT BOOLEAN               *MediaPresent
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HANDLE                   SnpHandle;
+  EFI_SIMPLE_NETWORK_PROTOCOL  *Snp;
+  UINT32                       InterruptStatus;
+  UINT32                       OldState;
+  EFI_MAC_ADDRESS              *MCastFilter;
+  UINT32                       MCastFilterCount;
+  UINT32                       EnableFilterBits;
+  UINT32                       DisableFilterBits;
+  BOOLEAN                      ResetMCastFilters;
+
+  ASSERT (MediaPresent != NULL);
+
+  //
+  // Get SNP handle
+  //
+  Snp = NULL;
+  SnpHandle = NetLibGetSnpHandle (ServiceHandle, &Snp);
+  if (SnpHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether SNP support media detection
+  //
+  if (!Snp->Mode->MediaPresentSupported) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Invoke Snp->GetStatus() to refresh MediaPresent field in SNP mode data
+  //
+  Status = Snp->GetStatus (Snp, &InterruptStatus, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Snp->Mode->MediaPresent) {
+    //
+    // Media is present, return directly
+    //
+    *MediaPresent = TRUE;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Till now, GetStatus() report no media; while, in case UNDI not support
+  // reporting media status from GetStatus(), this media status may be incorrect.
+  // So, we will stop SNP and then restart it to get the correct media status.
+  //
+  OldState = Snp->Mode->State;
+  if (OldState >= EfiSimpleNetworkMaxState) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  MCastFilter = NULL;
+
+  if (OldState == EfiSimpleNetworkInitialized) {
+    //
+    // SNP is already in use, need Shutdown/Stop and then Start/Initialize
+    //
+
+    //
+    // Backup current SNP receive filter settings
+    //
+    EnableFilterBits  = Snp->Mode->ReceiveFilterSetting;
+    DisableFilterBits = Snp->Mode->ReceiveFilterMask ^ EnableFilterBits;
+
+    ResetMCastFilters = TRUE;
+    MCastFilterCount  = Snp->Mode->MCastFilterCount;
+    if (MCastFilterCount != 0) {
+      MCastFilter = AllocateCopyPool (
+                      MCastFilterCount * sizeof (EFI_MAC_ADDRESS),
+                      Snp->Mode->MCastFilter
+                      );
+      ASSERT (MCastFilter != NULL);
+
+      ResetMCastFilters = FALSE;
+    }
+
+    //
+    // Shutdown/Stop the simple network
+    //
+    Status = Snp->Shutdown (Snp);
+    if (!EFI_ERROR (Status)) {
+      Status = Snp->Stop (Snp);
+    }
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Start/Initialize the simple network
+    //
+    Status = Snp->Start (Snp);
+    if (!EFI_ERROR (Status)) {
+      Status = Snp->Initialize (Snp, 0, 0);
+    }
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Here we get the correct media status
+    //
+    *MediaPresent = Snp->Mode->MediaPresent;
+
+    //
+    // Restore SNP receive filter settings
+    //
+    Status = Snp->ReceiveFilters (
+                    Snp,
+                    EnableFilterBits,
+                    DisableFilterBits,
+                    ResetMCastFilters,
+                    MCastFilterCount,
+                    MCastFilter
+                    );
+
+    if (MCastFilter != NULL) {
+      FreePool (MCastFilter);
+    }
+
+    return Status;
+  }
+
+  //
+  // SNP is not in use, it's in state of EfiSimpleNetworkStopped or EfiSimpleNetworkStarted
+  //
+  if (OldState == EfiSimpleNetworkStopped) {
+    //
+    // SNP not start yet, start it
+    //
+    Status = Snp->Start (Snp);
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  }
+
+  //
+  // Initialize the simple network
+  //
+  Status = Snp->Initialize (Snp, 0, 0);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Exit;
+  }
+
+  //
+  // Here we get the correct media status
+  //
+  *MediaPresent = Snp->Mode->MediaPresent;
+
+  //
+  // Shut down the simple network
+  //
+  Snp->Shutdown (Snp);
+
+Exit:
+  if (OldState == EfiSimpleNetworkStopped) {
+    //
+    // Original SNP sate is Stopped, restore to original state
+    //
+    Snp->Stop (Snp);
+  }
+
+  if (MCastFilter != NULL) {
+    FreePool (MCastFilter);
+  }
+
+  return Status;
+}
+
+/**
+  Check the default address used by the IPv4 driver is static or dynamic (acquired
+  from DHCP).
+
+  If the controller handle does not have the EFI_IP4_CONFIG2_PROTOCOL installed, the
+  default address is static. If failed to get the policy from Ip4 Config2 Protocol, 
+  the default address is static. Otherwise, get the result from Ip4 Config2 Protocol.
+
+  @param[in]   Controller     The controller handle which has the EFI_IP4_CONFIG2_PROTOCOL 
+                              relative with the default address to judge.
+
+  @retval TRUE           If the default address is static.
+  @retval FALSE          If the default address is acquired from DHCP.
+
+**/
+BOOLEAN
+NetLibDefaultAddressIsStatic (
+  IN EFI_HANDLE  Controller
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_IP4_CONFIG2_PROTOCOL         *Ip4Config2;
+  UINTN                            DataSize;  
+  EFI_IP4_CONFIG2_POLICY           Policy;
+  BOOLEAN                          IsStatic;
+
+  Ip4Config2 = NULL;
+  
+  DataSize = sizeof (EFI_IP4_CONFIG2_POLICY);
+
+  IsStatic   = TRUE;
+
+  //
+  // Get Ip4Config2 policy.
+  //
+  Status = gBS->HandleProtocol (Controller, &gEfiIp4Config2ProtocolGuid, (VOID **) &Ip4Config2);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = Ip4Config2->GetData (Ip4Config2, Ip4Config2DataTypePolicy, &DataSize, &Policy);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  
+  IsStatic = (BOOLEAN) (Policy == Ip4Config2PolicyStatic);
+
+ON_EXIT:
+  
+  return IsStatic;
+}
+
+/**
+  Create an IPv4 device path node.
+
+  The header type of IPv4 device path node is MESSAGING_DEVICE_PATH.
+  The header subtype of IPv4 device path node is MSG_IPv4_DP.
+  Get other info from parameters to make up the whole IPv4 device path node.
+
+  @param[in, out]  Node                  Pointer to the IPv4 device path node.
+  @param[in]       Controller            The controller handle.
+  @param[in]       LocalIp               The local IPv4 address.
+  @param[in]       LocalPort             The local port.
+  @param[in]       RemoteIp              The remote IPv4 address.
+  @param[in]       RemotePort            The remote port.
+  @param[in]       Protocol              The protocol type in the IP header.
+  @param[in]       UseDefaultAddress     Whether this instance is using default address or not.
+
+**/
+VOID
+EFIAPI
+NetLibCreateIPv4DPathNode (
+  IN OUT IPv4_DEVICE_PATH  *Node,
+  IN EFI_HANDLE            Controller,
+  IN IP4_ADDR              LocalIp,
+  IN UINT16                LocalPort,
+  IN IP4_ADDR              RemoteIp,
+  IN UINT16                RemotePort,
+  IN UINT16                Protocol,
+  IN BOOLEAN               UseDefaultAddress
+  )
+{
+  Node->Header.Type    = MESSAGING_DEVICE_PATH;
+  Node->Header.SubType = MSG_IPv4_DP;
+  SetDevicePathNodeLength (&Node->Header, sizeof (IPv4_DEVICE_PATH));
+
+  CopyMem (&Node->LocalIpAddress, &LocalIp, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Node->RemoteIpAddress, &RemoteIp, sizeof (EFI_IPv4_ADDRESS));
+
+  Node->LocalPort  = LocalPort;
+  Node->RemotePort = RemotePort;
+
+  Node->Protocol = Protocol;
+
+  if (!UseDefaultAddress) {
+    Node->StaticIpAddress = TRUE;
+  } else {
+    Node->StaticIpAddress = NetLibDefaultAddressIsStatic (Controller);
+  }
+
+  //
+  // Set the Gateway IP address to default value 0:0:0:0.
+  // Set the Subnet mask to default value 255:255:255:0.
+  //
+  ZeroMem (&Node->GatewayIpAddress, sizeof (EFI_IPv4_ADDRESS));
+  SetMem (&Node->SubnetMask, sizeof (EFI_IPv4_ADDRESS), 0xff);
+  Node->SubnetMask.Addr[3] = 0;
+}
+
+/**
+  Create an IPv6 device path node.
+
+  The header type of IPv6 device path node is MESSAGING_DEVICE_PATH.
+  The header subtype of IPv6 device path node is MSG_IPv6_DP.
+  Get other info from parameters to make up the whole IPv6 device path node.
+
+  @param[in, out]  Node                  Pointer to the IPv6 device path node.
+  @param[in]       Controller            The controller handle.
+  @param[in]       LocalIp               The local IPv6 address.
+  @param[in]       LocalPort             The local port.
+  @param[in]       RemoteIp              The remote IPv6 address.
+  @param[in]       RemotePort            The remote port.
+  @param[in]       Protocol              The protocol type in the IP header.
+
+**/
+VOID
+EFIAPI
+NetLibCreateIPv6DPathNode (
+  IN OUT IPv6_DEVICE_PATH  *Node,
+  IN EFI_HANDLE            Controller,
+  IN EFI_IPv6_ADDRESS      *LocalIp,
+  IN UINT16                LocalPort,
+  IN EFI_IPv6_ADDRESS      *RemoteIp,
+  IN UINT16                RemotePort,
+  IN UINT16                Protocol
+  )
+{
+  Node->Header.Type    = MESSAGING_DEVICE_PATH;
+  Node->Header.SubType = MSG_IPv6_DP;
+  SetDevicePathNodeLength (&Node->Header, sizeof (IPv6_DEVICE_PATH));
+
+  CopyMem (&Node->LocalIpAddress, LocalIp, sizeof (EFI_IPv6_ADDRESS));
+  CopyMem (&Node->RemoteIpAddress, RemoteIp, sizeof (EFI_IPv6_ADDRESS));
+
+  Node->LocalPort  = LocalPort;
+  Node->RemotePort = RemotePort;
+
+  Node->Protocol        = Protocol;
+
+  //
+  // Set default value to IPAddressOrigin, PrefixLength.
+  // Set the Gateway IP address to unspecified address.
+  //
+  Node->IpAddressOrigin = 0;
+  Node->PrefixLength    = IP6_PREFIX_LENGTH;
+  ZeroMem (&Node->GatewayIpAddress, sizeof (EFI_IPv6_ADDRESS));
+}
+
+/**
+  Find the UNDI/SNP handle from controller and protocol GUID.
+
+  For example, IP will open a MNP child to transmit/receive
+  packets, when MNP is stopped, IP should also be stopped. IP
+  needs to find its own private data which is related the IP's
+  service binding instance that is install on UNDI/SNP handle.
+  Now, the controller is either a MNP or ARP child handle. But
+  IP opens these handle BY_DRIVER, use that info, we can get the
+  UNDI/SNP handle.
+
+  @param[in]  Controller            Then protocol handle to check.
+  @param[in]  ProtocolGuid          The protocol that is related with the handle.
+
+  @return The UNDI/SNP handle or NULL for errors.
+
+**/
+EFI_HANDLE
+EFIAPI
+NetLibGetNicHandle (
+  IN EFI_HANDLE             Controller,
+  IN EFI_GUID               *ProtocolGuid
+  )
+{
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY *OpenBuffer;
+  EFI_HANDLE                          Handle;
+  EFI_STATUS                          Status;
+  UINTN                               OpenCount;
+  UINTN                               Index;
+
+  Status = gBS->OpenProtocolInformation (
+                  Controller,
+                  ProtocolGuid,
+                  &OpenBuffer,
+                  &OpenCount
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Handle = NULL;
+
+  for (Index = 0; Index < OpenCount; Index++) {
+    if ((OpenBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) != 0) {
+      Handle = OpenBuffer[Index].ControllerHandle;
+      break;
+    }
+  }
+
+  gBS->FreePool (OpenBuffer);
+  return Handle;
+}
+
+/**
+  Convert one Null-terminated ASCII string (decimal dotted) to EFI_IPv4_ADDRESS.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip4Address     The pointer to the converted IPv4 address.
+
+  @retval EFI_SUCCESS            Convert to IPv4 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip4Address is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibAsciiStrToIp4 (
+  IN CONST CHAR8                 *String,
+  OUT      EFI_IPv4_ADDRESS      *Ip4Address
+  )
+{
+  UINT8                          Index;
+  CHAR8                          *Ip4Str;
+  CHAR8                          *TempStr;
+  UINTN                          NodeVal;
+
+  if ((String == NULL) || (Ip4Address == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ip4Str = (CHAR8 *) String;
+
+  for (Index = 0; Index < 4; Index++) {
+    TempStr = Ip4Str;
+
+    while ((*Ip4Str != '\0') && (*Ip4Str != '.')) {
+      Ip4Str++;
+    }
+
+    //
+    // The IPv4 address is X.X.X.X
+    //
+    if (*Ip4Str == '.') {
+      if (Index == 3) {
+        return EFI_INVALID_PARAMETER;
+      }
+    } else {
+      if (Index != 3) {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+
+    //
+    // Convert the string to IPv4 address. AsciiStrDecimalToUintn stops at the
+    // first character that is not a valid decimal character, '.' or '\0' here.
+    //
+    NodeVal = AsciiStrDecimalToUintn (TempStr);
+    if (NodeVal > 0xFF) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Ip4Address->Addr[Index] = (UINT8) NodeVal;
+
+    Ip4Str++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Convert one Null-terminated ASCII string to EFI_IPv6_ADDRESS. The format of the
+  string is defined in RFC 4291 - Text Pepresentation of Addresses.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+
+  @retval EFI_SUCCESS            Convert to IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip6Address is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibAsciiStrToIp6 (
+  IN CONST CHAR8                 *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address
+  )
+{
+  UINT8                          Index;
+  CHAR8                          *Ip6Str;
+  CHAR8                          *TempStr;
+  CHAR8                          *TempStr2;
+  UINT8                          NodeCnt;
+  UINT8                          TailNodeCnt;
+  UINT8                          AllowedCnt;
+  UINTN                          NodeVal;
+  BOOLEAN                        Short;
+  BOOLEAN                        Update;
+  BOOLEAN                        LeadZero;
+  UINT8                          LeadZeroCnt;
+  UINT8                          Cnt;
+
+  if ((String == NULL) || (Ip6Address == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ip6Str      = (CHAR8 *) String;
+  AllowedCnt  = 6;
+  LeadZeroCnt = 0;
+
+  //
+  // An IPv6 address leading with : looks strange.
+  //
+  if (*Ip6Str == ':') {
+    if (*(Ip6Str + 1) != ':') {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      AllowedCnt = 7;
+    }
+  }
+
+  ZeroMem (Ip6Address, sizeof (EFI_IPv6_ADDRESS));
+
+  NodeCnt     = 0;
+  TailNodeCnt = 0;
+  Short       = FALSE;
+  Update      = FALSE;
+  LeadZero    = FALSE;
+
+  for (Index = 0; Index < 15; Index = (UINT8) (Index + 2)) {
+    TempStr = Ip6Str;
+
+    while ((*Ip6Str != '\0') && (*Ip6Str != ':')) {
+      Ip6Str++;
+    }
+
+    if ((*Ip6Str == '\0') && (Index != 14)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (*Ip6Str == ':') {
+      if (*(Ip6Str + 1) == ':') {
+        if ((NodeCnt > 6) || 
+            ((*(Ip6Str + 2) != '\0') && (AsciiStrHexToUintn (Ip6Str + 2) == 0))) {
+          //
+          // ::0 looks strange. report error to user.
+          //
+          return EFI_INVALID_PARAMETER;
+        }
+        if ((NodeCnt == 6) && (*(Ip6Str + 2) != '\0') && 
+            (AsciiStrHexToUintn (Ip6Str + 2) != 0)) {
+          return EFI_INVALID_PARAMETER;
+        }
+
+        //
+        // Skip the abbreviation part of IPv6 address.
+        //
+        TempStr2 = Ip6Str + 2;
+        while ((*TempStr2 != '\0')) {
+          if (*TempStr2 == ':') {
+            if (*(TempStr2 + 1) == ':') {
+              //
+              // :: can only appear once in IPv6 address.
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+
+            TailNodeCnt++;
+            if (TailNodeCnt >= (AllowedCnt - NodeCnt)) {
+              //
+              // :: indicates one or more groups of 16 bits of zeros.
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+          }
+
+          TempStr2++;
+        }
+
+        Short  = TRUE;
+        Update = TRUE;
+
+        Ip6Str = Ip6Str + 2;
+      } else {
+        if (*(Ip6Str + 1) == '\0') {
+          return EFI_INVALID_PARAMETER;
+        }
+        Ip6Str++;
+        NodeCnt++;
+        if ((Short && (NodeCnt > 6)) || (!Short && (NodeCnt > 7))) {
+          //
+          // There are more than 8 groups of 16 bits of zeros.
+          //
+          return EFI_INVALID_PARAMETER;
+        }
+      }
+    }
+
+    //
+    // Convert the string to IPv6 address. AsciiStrHexToUintn stops at the first
+    // character that is not a valid hexadecimal character, ':' or '\0' here.
+    //
+    NodeVal = AsciiStrHexToUintn (TempStr);
+    if ((NodeVal > 0xFFFF) || (Index > 14)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if (NodeVal != 0) {
+      if ((*TempStr  == '0') && 
+          ((*(TempStr + 2) == ':') || (*(TempStr + 3) == ':') || 
+          (*(TempStr + 2) == '\0') || (*(TempStr + 3) == '\0'))) {
+        return EFI_INVALID_PARAMETER;
+      }
+      if ((*TempStr  == '0') && (*(TempStr + 4) != '\0') && 
+          (*(TempStr + 4) != ':')) { 
+        return EFI_INVALID_PARAMETER;
+      }
+    } else {
+      if (((*TempStr  == '0') && (*(TempStr + 1) == '0') && 
+          ((*(TempStr + 2) == ':') || (*(TempStr + 2) == '\0'))) ||
+          ((*TempStr  == '0') && (*(TempStr + 1) == '0') && (*(TempStr + 2) == '0') && 
+          ((*(TempStr + 3) == ':') || (*(TempStr + 3) == '\0')))) {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+
+    Cnt = 0;
+    while ((TempStr[Cnt] != ':') && (TempStr[Cnt] != '\0')) {
+      Cnt++; 
+    }
+    if (LeadZeroCnt == 0) {
+      if ((Cnt == 4) && (*TempStr  == '0')) {
+        LeadZero = TRUE;
+        LeadZeroCnt++;
+      }
+      if ((Cnt != 0) && (Cnt < 4)) {
+        LeadZero = FALSE;
+        LeadZeroCnt++;
+      }
+    } else {
+      if ((Cnt == 4) && (*TempStr  == '0') && !LeadZero) {
+        return EFI_INVALID_PARAMETER;
+      }
+      if ((Cnt != 0) && (Cnt < 4) && LeadZero) {
+        return EFI_INVALID_PARAMETER;
+      }
+    } 
+
+    Ip6Address->Addr[Index] = (UINT8) (NodeVal >> 8);
+    Ip6Address->Addr[Index + 1] = (UINT8) (NodeVal & 0xFF);
+
+    //
+    // Skip the groups of zeros by ::
+    //
+    if (Short && Update) {
+      Index  = (UINT8) (16 - (TailNodeCnt + 2) * 2);
+      Update = FALSE;
+    }
+  }
+
+  if ((!Short && Index != 16) || (*Ip6Str != '\0')) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Convert one Null-terminated Unicode string (decimal dotted) to EFI_IPv4_ADDRESS.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip4Address     The pointer to the converted IPv4 address.
+
+  @retval EFI_SUCCESS            Convert to IPv4 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip4Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Fail to perform the operation due to lack of resource.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp4 (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv4_ADDRESS      *Ip4Address
+  )
+{
+  CHAR8                          *Ip4Str;
+  UINTN                          StringSize;
+  EFI_STATUS                     Status;
+
+  if ((String == NULL) || (Ip4Address == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StringSize = StrLen (String) + 1;
+  Ip4Str = (CHAR8 *) AllocatePool (StringSize * sizeof (CHAR8));
+  if (Ip4Str == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UnicodeStrToAsciiStrS (String, Ip4Str, StringSize);
+
+  Status = NetLibAsciiStrToIp4 (Ip4Str, Ip4Address);
+
+  FreePool (Ip4Str);
+
+  return Status;
+}
+
+
+/**
+  Convert one Null-terminated Unicode string to EFI_IPv6_ADDRESS.  The format of
+  the string is defined in RFC 4291 - Text Pepresentation of Addresses.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+
+  @retval EFI_SUCCESS            Convert to IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip6Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Fail to perform the operation due to lack of resource.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp6 (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address
+  )
+{
+  CHAR8                          *Ip6Str;
+  UINTN                          StringSize;
+  EFI_STATUS                     Status;
+
+  if ((String == NULL) || (Ip6Address == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StringSize = StrLen (String) + 1;
+  Ip6Str = (CHAR8 *) AllocatePool (StringSize * sizeof (CHAR8));
+  if (Ip6Str == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UnicodeStrToAsciiStrS (String, Ip6Str, StringSize);
+
+  Status = NetLibAsciiStrToIp6 (Ip6Str, Ip6Address);
+
+  FreePool (Ip6Str);
+
+  return Status;
+}
+
+/**
+  Convert one Null-terminated Unicode string to EFI_IPv6_ADDRESS and prefix length.
+  The format of the string is defined in RFC 4291 - Text Pepresentation of Addresses
+  Prefixes: ipv6-address/prefix-length.
+
+  @param[in]      String         The pointer to the Ascii string.
+  @param[out]     Ip6Address     The pointer to the converted IPv6 address.
+  @param[out]     PrefixLength   The pointer to the converted prefix length.
+
+  @retval EFI_SUCCESS            Convert to IPv6 address successfully.
+  @retval EFI_INVALID_PARAMETER  The string is mal-formated or Ip6Address is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Fail to perform the operation due to lack of resource.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibStrToIp6andPrefix (
+  IN CONST CHAR16                *String,
+  OUT      EFI_IPv6_ADDRESS      *Ip6Address,
+  OUT      UINT8                 *PrefixLength
+  )
+{
+  CHAR8                          *Ip6Str;
+  UINTN                          StringSize;
+  CHAR8                          *PrefixStr;
+  CHAR8                          *TempStr;
+  EFI_STATUS                     Status;
+  UINT8                          Length;
+
+  if ((String == NULL) || (Ip6Address == NULL) || (PrefixLength == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StringSize = StrLen (String) + 1;
+  Ip6Str = (CHAR8 *) AllocatePool (StringSize * sizeof (CHAR8));
+  if (Ip6Str == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UnicodeStrToAsciiStrS (String, Ip6Str, StringSize);
+
+  //
+  // Get the sub string describing prefix length.
+  //
+  TempStr = Ip6Str;
+  while (*TempStr != '\0' && (*TempStr != '/')) {
+    TempStr++;
+  }
+
+  if (*TempStr == '/') {
+    PrefixStr = TempStr + 1;
+  } else {
+    PrefixStr = NULL;
+  }
+
+  //
+  // Get the sub string describing IPv6 address and convert it.
+  //
+  *TempStr = '\0';
+
+  Status = NetLibAsciiStrToIp6 (Ip6Str, Ip6Address);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // If input string doesn't indicate the prefix length, return 0xff.
+  //
+  Length = 0xFF;
+
+  //
+  // Convert the string to prefix length
+  //
+  if (PrefixStr != NULL) {
+
+    Status = EFI_INVALID_PARAMETER;
+    Length = 0;
+    while (*PrefixStr != '\0') {
+      if (NET_IS_DIGIT (*PrefixStr)) {
+        Length = (UINT8) (Length * 10 + (*PrefixStr - '0'));
+        if (Length >= IP6_PREFIX_NUM) {
+          goto Exit;
+        }
+      } else {
+        goto Exit;
+      }
+
+      PrefixStr++;
+    }
+  }
+
+  *PrefixLength = Length;
+  Status        = EFI_SUCCESS;
+
+Exit:
+
+  FreePool (Ip6Str);
+  return Status;
+}
+
+/**
+
+  Convert one EFI_IPv6_ADDRESS to Null-terminated Unicode string.
+  The text representation of address is defined in RFC 4291.
+  
+  @param[in]       Ip6Address     The pointer to the IPv6 address.
+  @param[out]      String         The buffer to return the converted string.
+  @param[in]       StringSize     The length in bytes of the input String.
+                                  
+  @retval EFI_SUCCESS             Convert to string successfully.
+  @retval EFI_INVALID_PARAMETER   The input parameter is invalid.
+  @retval EFI_BUFFER_TOO_SMALL    The BufferSize is too small for the result. BufferSize has been 
+                                  updated with the size needed to complete the request.
+**/
+EFI_STATUS
+EFIAPI
+NetLibIp6ToStr (
+  IN         EFI_IPv6_ADDRESS      *Ip6Address,
+  OUT        CHAR16                *String,
+  IN         UINTN                 StringSize
+  )
+{
+  UINT16     Ip6Addr[8];
+  UINTN      Index;
+  UINTN      LongestZerosStart;
+  UINTN      LongestZerosLength;
+  UINTN      CurrentZerosStart;
+  UINTN      CurrentZerosLength;
+  CHAR16     Buffer[sizeof"ffff:ffff:ffff:ffff:ffff:ffff:ffff:ffff"];
+  CHAR16     *Ptr;
+
+  if (Ip6Address == NULL || String == NULL || StringSize == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Convert the UINT8 array to an UINT16 array for easy handling.
+  // 
+  ZeroMem (Ip6Addr, sizeof (Ip6Addr));
+  for (Index = 0; Index < 16; Index++) {
+    Ip6Addr[Index / 2] |= (Ip6Address->Addr[Index] << ((1 - (Index % 2)) << 3));
+  }
+
+  //
+  // Find the longest zeros and mark it.
+  //
+  CurrentZerosStart  = DEFAULT_ZERO_START;
+  CurrentZerosLength = 0;
+  LongestZerosStart  = DEFAULT_ZERO_START;
+  LongestZerosLength = 0;
+  for (Index = 0; Index < 8; Index++) {
+    if (Ip6Addr[Index] == 0) {
+      if (CurrentZerosStart == DEFAULT_ZERO_START) {
+        CurrentZerosStart = Index;
+        CurrentZerosLength = 1;
+      } else {
+        CurrentZerosLength++;
+      }
+    } else {
+      if (CurrentZerosStart != DEFAULT_ZERO_START) {
+        if (CurrentZerosLength > 2 && (LongestZerosStart == (DEFAULT_ZERO_START) || CurrentZerosLength > LongestZerosLength)) {
+          LongestZerosStart  = CurrentZerosStart;
+          LongestZerosLength = CurrentZerosLength;
+        }
+        CurrentZerosStart  = DEFAULT_ZERO_START;
+        CurrentZerosLength = 0;
+      }
+    }
+  }
+  
+  if (CurrentZerosStart != DEFAULT_ZERO_START && CurrentZerosLength > 2) {
+    if (LongestZerosStart == DEFAULT_ZERO_START || LongestZerosLength < CurrentZerosLength) {
+      LongestZerosStart  = CurrentZerosStart;
+      LongestZerosLength = CurrentZerosLength;
+    }
+  }
+
+  Ptr = Buffer;
+  for (Index = 0; Index < 8; Index++) {
+    if (LongestZerosStart != DEFAULT_ZERO_START && Index >= LongestZerosStart && Index < LongestZerosStart + LongestZerosLength) {
+      if (Index == LongestZerosStart) {
+        *Ptr++ = L':';
+      }
+      continue;
+    }
+    if (Index != 0) {
+      *Ptr++ = L':';
+    }
+    Ptr += UnicodeSPrint(Ptr, 10, L"%x", Ip6Addr[Index]);
+  }
+  
+  if (LongestZerosStart != DEFAULT_ZERO_START && LongestZerosStart + LongestZerosLength == 8) {
+    *Ptr++ = L':';
+  }
+  *Ptr = L'\0';
+
+  if ((UINTN)Ptr - (UINTN)Buffer > StringSize) {
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  StrCpyS (String, StringSize / sizeof (CHAR16), Buffer);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function obtains the system guid from the smbios table.
+
+  @param[out]  SystemGuid     The pointer of the returned system guid.
+
+  @retval EFI_SUCCESS         Successfully obtained the system guid.
+  @retval EFI_NOT_FOUND       Did not find the SMBIOS table.
+
+**/
+EFI_STATUS
+EFIAPI
+NetLibGetSystemGuid (
+  OUT EFI_GUID              *SystemGuid
+  )
+{
+  EFI_STATUS                    Status;
+  SMBIOS_TABLE_ENTRY_POINT      *SmbiosTable;
+  SMBIOS_TABLE_3_0_ENTRY_POINT  *Smbios30Table;
+  SMBIOS_STRUCTURE_POINTER      Smbios;
+  SMBIOS_STRUCTURE_POINTER      SmbiosEnd;
+  CHAR8                         *String;
+
+  SmbiosTable = NULL;
+  Status = EfiGetSystemConfigurationTable (&gEfiSmbios3TableGuid, (VOID **) &Smbios30Table);
+  if (!(EFI_ERROR (Status) || Smbios30Table == NULL)) {
+    Smbios.Hdr = (SMBIOS_STRUCTURE *) (UINTN) Smbios30Table->TableAddress;
+    SmbiosEnd.Raw = (UINT8 *) (UINTN) (Smbios30Table->TableAddress + Smbios30Table->TableMaximumSize);
+  } else {
+    Status = EfiGetSystemConfigurationTable (&gEfiSmbiosTableGuid, (VOID **) &SmbiosTable);
+    if (EFI_ERROR (Status) || SmbiosTable == NULL) {
+      return EFI_NOT_FOUND;
+    }
+    Smbios.Hdr    = (SMBIOS_STRUCTURE *) (UINTN) SmbiosTable->TableAddress;
+    SmbiosEnd.Raw = (UINT8 *) (UINTN) (SmbiosTable->TableAddress + SmbiosTable->TableLength);
+  }
+
+  do {
+    if (Smbios.Hdr->Type == 1) {
+      if (Smbios.Hdr->Length < 0x19) {
+        //
+        // Older version did not support UUID.
+        //
+        return EFI_NOT_FOUND;
+      }
+      
+      //
+      // SMBIOS tables are byte packed so we need to do a byte copy to
+      // prevend alignment faults on Itanium-based platform.
+      //
+      CopyMem (SystemGuid, &Smbios.Type1->Uuid, sizeof (EFI_GUID));
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Go to the next SMBIOS structure. Each SMBIOS structure may include 2 parts:
+    // 1. Formatted section; 2. Unformatted string section. So, 2 steps are needed
+    // to skip one SMBIOS structure.
+    //
+    
+    //
+    // Step 1: Skip over formatted section.
+    //
+    String = (CHAR8 *) (Smbios.Raw + Smbios.Hdr->Length);
+  
+    //
+    // Step 2: Skip over unformated string section.
+    //
+    do {
+      //
+      // Each string is terminated with a NULL(00h) BYTE and the sets of strings
+      // is terminated with an additional NULL(00h) BYTE.
+      //
+      for ( ; *String != 0; String++) {
+      }
+
+      if (*(UINT8*)++String == 0) {
+        //
+        // Pointer to the next SMBIOS structure.
+        //
+        Smbios.Raw = (UINT8 *)++String;
+        break;
+      }    
+    } while (TRUE);
+  } while (Smbios.Raw < SmbiosEnd.Raw);
+  return EFI_NOT_FOUND;
+}
diff --git a/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.inf b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.inf
new file mode 100644
index 0000000000..1ff3a4fe55
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.inf
@@ -0,0 +1,65 @@
+## @file
+#  This library instance provides the basic network services.
+#
+#  Copyright (c) 2006 - 2014, Intel Corporation. All rights reserved.<BR>
+#  (C) Copyright 2015 Hewlett Packard Enterprise Development LP<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.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeNetLib
+  MODULE_UNI_FILE                = DxeNetLib.uni
+  FILE_GUID                      = db6dcef3-9f4e-4340-9351-fc35aa8a5888
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NetLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeNetLib.c
+  NetBuffer.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  UefiLib
+  MemoryAllocationLib
+  DevicePathLib
+  PrintLib
+
+
+[Guids]
+  gEfiSmbiosTableGuid                           ## SOMETIMES_CONSUMES  ## SystemTable
+  gEfiSmbios3TableGuid                          ## SOMETIMES_CONSUMES  ## SystemTable
+
+
+[Protocols]
+  gEfiSimpleNetworkProtocolGuid                 ## SOMETIMES_CONSUMES
+  gEfiManagedNetworkProtocolGuid                ## SOMETIMES_CONSUMES
+  gEfiManagedNetworkServiceBindingProtocolGuid  ## SOMETIMES_CONSUMES
+  gEfiIp4Config2ProtocolGuid                    ## SOMETIMES_CONSUMES
+  gEfiComponentNameProtocolGuid                 ## SOMETIMES_CONSUMES
+  gEfiComponentName2ProtocolGuid                ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.uni b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.uni
new file mode 100644
index 0000000000..bd74cfacbd
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeNetLib/DxeNetLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeNetLib/NetBuffer.c b/Core/MdeModulePkg/Library/DxeNetLib/NetBuffer.c
new file mode 100644
index 0000000000..bbbdbc048a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeNetLib/NetBuffer.c
@@ -0,0 +1,1892 @@
+/** @file
+  Network library functions providing net buffer operation support.
+
+Copyright (c) 2005 - 2010, 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 <Uefi.h>
+
+#include <Library/NetLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+
+/**
+  Allocate and build up the sketch for a NET_BUF.
+
+  The net buffer allocated has the BlockOpNum's NET_BLOCK_OP, and its associated
+  NET_VECTOR has the BlockNum's NET_BLOCK. But all the NET_BLOCK_OP and
+  NET_BLOCK remain un-initialized.
+
+  @param[in]  BlockNum       The number of NET_BLOCK in the vector of net buffer
+  @param[in]  BlockOpNum     The number of NET_BLOCK_OP in the net buffer
+
+  @return                    Pointer to the allocated NET_BUF, or NULL if the
+                             allocation failed due to resource limit.
+
+**/
+NET_BUF *
+NetbufAllocStruct (
+  IN UINT32                 BlockNum,
+  IN UINT32                 BlockOpNum
+  )
+{
+  NET_BUF                   *Nbuf;
+  NET_VECTOR                *Vector;
+
+  ASSERT (BlockOpNum >= 1);
+
+  //
+  // Allocate three memory blocks.
+  //
+  Nbuf = AllocateZeroPool (NET_BUF_SIZE (BlockOpNum));
+
+  if (Nbuf == NULL) {
+    return NULL;
+  }
+
+  Nbuf->Signature           = NET_BUF_SIGNATURE;
+  Nbuf->RefCnt              = 1;
+  Nbuf->BlockOpNum          = BlockOpNum;
+  InitializeListHead (&Nbuf->List);
+
+  if (BlockNum != 0) {
+    Vector = AllocateZeroPool (NET_VECTOR_SIZE (BlockNum));
+
+    if (Vector == NULL) {
+      goto FreeNbuf;
+    }
+
+    Vector->Signature = NET_VECTOR_SIGNATURE;
+    Vector->RefCnt    = 1;
+    Vector->BlockNum  = BlockNum;
+    Nbuf->Vector      = Vector;
+  }
+
+  return Nbuf;
+
+FreeNbuf:
+
+  FreePool (Nbuf);
+  return NULL;
+}
+
+
+/**
+  Allocate a single block NET_BUF. Upon allocation, all the
+  free space is in the tail room.
+
+  @param[in]  Len              The length of the block.
+
+  @return                      Pointer to the allocated NET_BUF, or NULL if the
+                               allocation failed due to resource limit.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufAlloc (
+  IN UINT32                 Len
+  )
+{
+  NET_BUF                   *Nbuf;
+  NET_VECTOR                *Vector;
+  UINT8                     *Bulk;
+
+  ASSERT (Len > 0);
+
+  Nbuf = NetbufAllocStruct (1, 1);
+
+  if (Nbuf == NULL) {
+    return NULL;
+  }
+
+  Bulk = AllocatePool (Len);
+
+  if (Bulk == NULL) {
+    goto FreeNBuf;
+  }
+
+  Vector = Nbuf->Vector;
+  Vector->Len                 = Len;
+
+  Vector->Block[0].Bulk       = Bulk;
+  Vector->Block[0].Len        = Len;
+
+  Nbuf->BlockOp[0].BlockHead  = Bulk;
+  Nbuf->BlockOp[0].BlockTail  = Bulk + Len;
+
+  Nbuf->BlockOp[0].Head       = Bulk;
+  Nbuf->BlockOp[0].Tail       = Bulk;
+  Nbuf->BlockOp[0].Size       = 0;
+
+  return Nbuf;
+
+FreeNBuf:
+  FreePool (Nbuf);
+  return NULL;
+}
+
+/**
+  Free the net vector.
+
+  Decrease the reference count of the net vector by one. The real resource free
+  operation isn't performed until the reference count of the net vector is
+  decreased to 0.
+
+  @param[in]  Vector                Pointer to the NET_VECTOR to be freed.
+
+**/
+VOID
+NetbufFreeVector (
+  IN NET_VECTOR             *Vector
+  )
+{
+  UINT32                    Index;
+
+  ASSERT (Vector != NULL);
+  NET_CHECK_SIGNATURE (Vector, NET_VECTOR_SIGNATURE);
+  ASSERT (Vector->RefCnt > 0);
+
+  Vector->RefCnt--;
+
+  if (Vector->RefCnt > 0) {
+    return;
+  }
+
+  if (Vector->Free != NULL) {
+    //
+    // Call external free function to free the vector if it
+    // isn't NULL. If NET_VECTOR_OWN_FIRST is set, release the
+    // first block since it is allocated by us
+    //
+    if ((Vector->Flag & NET_VECTOR_OWN_FIRST) != 0) {
+      gBS->FreePool (Vector->Block[0].Bulk);
+    }
+
+    Vector->Free (Vector->Arg);
+
+  } else {
+    //
+    // Free each memory block associated with the Vector
+    //
+    for (Index = 0; Index < Vector->BlockNum; Index++) {
+      gBS->FreePool (Vector->Block[Index].Bulk);
+    }
+  }
+
+  FreePool (Vector);
+}
+
+
+/**
+  Free the net buffer and its associated NET_VECTOR.
+
+  Decrease the reference count of the net buffer by one. Free the associated net
+  vector and itself if the reference count of the net buffer is decreased to 0.
+  The net vector free operation just decrease the reference count of the net
+  vector by one and do the real resource free operation when the reference count
+  of the net vector is 0.
+
+  @param[in]  Nbuf                  Pointer to the NET_BUF to be freed.
+
+**/
+VOID
+EFIAPI
+NetbufFree (
+  IN NET_BUF                *Nbuf
+  )
+{
+  ASSERT (Nbuf != NULL);
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  ASSERT (Nbuf->RefCnt > 0);
+
+  Nbuf->RefCnt--;
+
+  if (Nbuf->RefCnt == 0) {
+    //
+    // Update Vector only when NBuf is to be released. That is,
+    // all the sharing of Nbuf increse Vector's RefCnt by one
+    //
+    NetbufFreeVector (Nbuf->Vector);
+    FreePool (Nbuf);
+  }
+}
+
+
+/**
+  Create a copy of the net buffer that shares the associated net vector.
+
+  The reference count of the newly created net buffer is set to 1. The reference
+  count of the associated net vector is increased by one.
+
+  @param[in]  Nbuf              Pointer to the net buffer to be cloned.
+
+  @return                       Pointer to the cloned net buffer, or NULL if the
+                                allocation failed due to resource limit.
+
+**/
+NET_BUF *
+EFIAPI
+NetbufClone (
+  IN NET_BUF                *Nbuf
+  )
+{
+  NET_BUF                   *Clone;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  Clone = AllocatePool (NET_BUF_SIZE (Nbuf->BlockOpNum));
+
+  if (Clone == NULL) {
+    return NULL;
+  }
+
+  Clone->Signature  = NET_BUF_SIGNATURE;
+  Clone->RefCnt     = 1;
+  InitializeListHead (&Clone->List);
+
+  Clone->Ip   = Nbuf->Ip;
+  Clone->Tcp  = Nbuf->Tcp;
+
+  CopyMem (Clone->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
+
+  NET_GET_REF (Nbuf->Vector);
+
+  Clone->Vector     = Nbuf->Vector;
+  Clone->BlockOpNum = Nbuf->BlockOpNum;
+  Clone->TotalSize  = Nbuf->TotalSize;
+  CopyMem (Clone->BlockOp, Nbuf->BlockOp, sizeof (NET_BLOCK_OP) * Nbuf->BlockOpNum);
+
+  return Clone;
+}
+
+
+/**
+  Create a duplicated copy of the net buffer with data copied and HeadSpace
+  bytes of head space reserved.
+
+  The duplicated net buffer will allocate its own memory to hold the data of the
+  source net buffer.
+
+  @param[in]       Nbuf         Pointer to the net buffer to be duplicated from.
+  @param[in, out]  Duplicate    Pointer to the net buffer to duplicate to, if
+                                NULL a new net buffer is allocated.
+  @param[in]      HeadSpace     Length of the head space to reserve.
+
+  @return                       Pointer to the duplicated net buffer, or NULL if
+                                the allocation failed due to resource limit.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufDuplicate (
+  IN NET_BUF                *Nbuf,
+  IN OUT NET_BUF            *Duplicate        OPTIONAL,
+  IN UINT32                 HeadSpace
+  )
+{
+  UINT8                     *Dst;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  if (Duplicate == NULL) {
+    Duplicate = NetbufAlloc (Nbuf->TotalSize + HeadSpace);
+  }
+
+  if (Duplicate == NULL) {
+    return NULL;
+  }
+
+  //
+  // Don't set the IP and TCP head point, since it is most
+  // like that they are pointing to the memory of Nbuf.
+  //
+  CopyMem (Duplicate->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
+  NetbufReserve (Duplicate, HeadSpace);
+
+  Dst = NetbufAllocSpace (Duplicate, Nbuf->TotalSize, NET_BUF_TAIL);
+  NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dst);
+
+  return Duplicate;
+}
+
+
+/**
+  Free a list of net buffers.
+
+  @param[in, out]  Head              Pointer to the head of linked net buffers.
+
+**/
+VOID
+EFIAPI
+NetbufFreeList (
+  IN OUT LIST_ENTRY         *Head
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  NET_BUF                   *Nbuf;
+
+  Entry = Head->ForwardLink;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+    RemoveEntryList (Entry);
+    NetbufFree (Nbuf);
+  }
+
+  ASSERT (IsListEmpty (Head));
+}
+
+
+/**
+  Get the index of NET_BLOCK_OP that contains the byte at Offset in the net
+  buffer.
+
+  This can be used to, for example, retrieve the IP header in the packet. It
+  also can be used to get the fragment that contains the byte which is used
+  mainly by the library implementation itself.
+
+  @param[in]   Nbuf      Pointer to the net buffer.
+  @param[in]   Offset    The offset of the byte.
+  @param[out]  Index     Index of the NET_BLOCK_OP that contains the byte at
+                         Offset.
+
+  @return       Pointer to the Offset'th byte of data in the net buffer, or NULL
+                if there is no such data in the net buffer.
+
+**/
+UINT8  *
+EFIAPI
+NetbufGetByte (
+  IN  NET_BUF               *Nbuf,
+  IN  UINT32                Offset,
+  OUT UINT32                *Index  OPTIONAL
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    Loop;
+  UINT32                    Len;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  if (Offset >= Nbuf->TotalSize) {
+    return NULL;
+  }
+
+  BlockOp = Nbuf->BlockOp;
+  Len     = 0;
+
+  for (Loop = 0; Loop < Nbuf->BlockOpNum; Loop++) {
+
+    if (Len + BlockOp[Loop].Size <= Offset) {
+      Len += BlockOp[Loop].Size;
+      continue;
+    }
+
+    if (Index != NULL) {
+      *Index = Loop;
+    }
+
+    return BlockOp[Loop].Head + (Offset - Len);
+  }
+
+  return NULL;
+}
+
+
+
+/**
+  Set the NET_BLOCK and corresponding NET_BLOCK_OP in the net buffer and
+  corresponding net vector according to the bulk pointer and bulk length.
+
+  All the pointers in the Index'th NET_BLOCK and NET_BLOCK_OP are set to the
+  bulk's head and tail respectively. So, this function alone can't be used by
+  NetbufAlloc.
+
+  @param[in, out]  Nbuf       Pointer to the net buffer.
+  @param[in]       Bulk       Pointer to the data.
+  @param[in]       Len        Length of the bulk data.
+  @param[in]       Index      The data block index in the net buffer the bulk
+                              data should belong to.
+
+**/
+VOID
+NetbufSetBlock (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT8                  *Bulk,
+  IN UINT32                 Len,
+  IN UINT32                 Index
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  NET_BLOCK                 *Block;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
+  ASSERT (Index < Nbuf->BlockOpNum);
+
+  Block               = &(Nbuf->Vector->Block[Index]);
+  BlockOp             = &(Nbuf->BlockOp[Index]);
+  Block->Len          = Len;
+  Block->Bulk         = Bulk;
+  BlockOp->BlockHead  = Bulk;
+  BlockOp->BlockTail  = Bulk + Len;
+  BlockOp->Head       = Bulk;
+  BlockOp->Tail       = Bulk + Len;
+  BlockOp->Size       = Len;
+}
+
+
+
+/**
+  Set the NET_BLOCK_OP in the net buffer. The corresponding NET_BLOCK
+  structure is left untouched.
+
+  Some times, there is no 1:1 relationship between NET_BLOCK and NET_BLOCK_OP.
+  For example, that in NetbufGetFragment.
+
+  @param[in, out]  Nbuf       Pointer to the net buffer.
+  @param[in]       Bulk       Pointer to the data.
+  @param[in]       Len        Length of the bulk data.
+  @param[in]       Index      The data block index in the net buffer the bulk
+                              data should belong to.
+
+**/
+VOID
+NetbufSetBlockOp (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT8                  *Bulk,
+  IN UINT32                 Len,
+  IN UINT32                 Index
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  ASSERT (Index < Nbuf->BlockOpNum);
+
+  BlockOp             = &(Nbuf->BlockOp[Index]);
+  BlockOp->BlockHead  = Bulk;
+  BlockOp->BlockTail  = Bulk + Len;
+  BlockOp->Head       = Bulk;
+  BlockOp->Tail       = Bulk + Len;
+  BlockOp->Size       = Len;
+}
+
+
+/**
+  Helper function for NetbufGetFragment. NetbufGetFragment may allocate the
+  first block to reserve HeadSpace bytes header space. So it needs to create a
+  new net vector for the first block and can avoid copy for the remaining data
+  by sharing the old net vector.
+
+  @param[in]  Arg                   Point to the old NET_VECTOR.
+
+**/
+VOID
+EFIAPI
+NetbufGetFragmentFree (
+  IN VOID                   *Arg
+  )
+{
+  NET_VECTOR                *Vector;
+
+  Vector = (NET_VECTOR *)Arg;
+  NetbufFreeVector (Vector);
+}
+
+
+/**
+  Create a NET_BUF structure which contains Len byte data of Nbuf starting from
+  Offset.
+
+  A new NET_BUF structure will be created but the associated data in NET_VECTOR
+  is shared. This function exists to do IP packet fragmentation.
+
+  @param[in]  Nbuf         Pointer to the net buffer to be extracted.
+  @param[in]  Offset       Starting point of the data to be included in the new
+                           net buffer.
+  @param[in]  Len          Bytes of data to be included in the new net buffer.
+  @param[in]  HeadSpace    Bytes of head space to reserve for protocol header.
+
+  @return                  Pointer to the cloned net buffer, or NULL if the
+                           allocation failed due to resource limit.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufGetFragment (
+  IN NET_BUF                *Nbuf,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  IN UINT32                 HeadSpace
+  )
+{
+  NET_BUF                   *Child;
+  NET_VECTOR                *Vector;
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    CurBlockOp;
+  UINT32                    BlockOpNum;
+  UINT8                     *FirstBulk;
+  UINT32                    Index;
+  UINT32                    First;
+  UINT32                    Last;
+  UINT32                    FirstSkip;
+  UINT32                    FirstLen;
+  UINT32                    LastLen;
+  UINT32                    Cur;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  if ((Len == 0) || (Offset + Len > Nbuf->TotalSize)) {
+    return NULL;
+  }
+
+  //
+  // First find the first and last BlockOp that contains
+  // the valid data, and compute the offset of the first
+  // BlockOp and length of the last BlockOp
+  //
+  BlockOp = Nbuf->BlockOp;
+  Cur     = 0;
+
+  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
+    if (Offset < Cur + BlockOp[Index].Size) {
+      break;
+    }
+
+    Cur += BlockOp[Index].Size;
+  }
+
+  //
+  // First is the index of the first BlockOp, FirstSkip is
+  // the offset of the first byte in the first BlockOp.
+  //
+  First     = Index;
+  FirstSkip = Offset - Cur;
+  FirstLen  = BlockOp[Index].Size - FirstSkip;
+
+  Last      = 0;
+  LastLen   = 0;
+
+  if (Len > FirstLen) {
+    Cur += BlockOp[Index].Size;
+    Index++;
+
+    for (; Index < Nbuf->BlockOpNum; Index++) {
+      if (Offset + Len <= Cur + BlockOp[Index].Size) {
+        Last    = Index;
+        LastLen = Offset + Len - Cur;
+        break;
+      }
+
+      Cur += BlockOp[Index].Size;
+    }
+
+  } else {
+    Last     = First;
+    LastLen  = Len;
+    FirstLen = Len;
+  }
+
+  ASSERT (Last >= First);
+  BlockOpNum = Last - First + 1;
+  CurBlockOp = 0;
+
+  if (HeadSpace != 0) {
+    //
+    // Allocate an extra block to accomdate the head space.
+    //
+    BlockOpNum++;
+
+    Child = NetbufAllocStruct (1, BlockOpNum);
+
+    if (Child == NULL) {
+      return NULL;
+    }
+
+    FirstBulk = AllocatePool (HeadSpace);
+
+    if (FirstBulk == NULL) {
+      goto FreeChild;
+    }
+
+    Vector        = Child->Vector;
+    Vector->Free  = NetbufGetFragmentFree;
+    Vector->Arg   = Nbuf->Vector;
+    Vector->Flag  = NET_VECTOR_OWN_FIRST;
+    Vector->Len   = HeadSpace;
+
+    //
+    // Reserve the head space in the first block
+    //
+    NetbufSetBlock (Child, FirstBulk, HeadSpace, 0);
+    Child->BlockOp[0].Head += HeadSpace;
+    Child->BlockOp[0].Size =  0;
+    CurBlockOp++;
+
+  } else {
+    Child = NetbufAllocStruct (0, BlockOpNum);
+
+    if (Child == NULL) {
+      return NULL;
+    }
+
+    Child->Vector = Nbuf->Vector;
+  }
+
+  NET_GET_REF (Nbuf->Vector);
+  Child->TotalSize = Len;
+
+  //
+  // Set all the BlockOp up, the first and last one are special
+  // and need special process.
+  //
+  NetbufSetBlockOp (
+    Child,
+    Nbuf->BlockOp[First].Head + FirstSkip,
+    FirstLen,
+    CurBlockOp++
+    );
+
+  for (Index = First + 1; Index < Last; Index++) {
+    NetbufSetBlockOp (
+      Child,
+      BlockOp[Index].Head,
+      BlockOp[Index].Size,
+      CurBlockOp++
+      );
+  }
+
+  if (First != Last) {
+    NetbufSetBlockOp (
+      Child,
+      BlockOp[Last].Head,
+      LastLen,
+      CurBlockOp
+      );
+  }
+
+  CopyMem (Child->ProtoData, Nbuf->ProtoData, NET_PROTO_DATA);
+  return Child;
+
+FreeChild:
+
+  FreePool (Child);
+  return NULL;
+}
+
+
+
+/**
+  Build a NET_BUF from external blocks.
+
+  A new NET_BUF structure will be created from external blocks. Additional block
+  of memory will be allocated to hold reserved HeadSpace bytes of header room
+  and existing HeadLen bytes of header but the external blocks are shared by the
+  net buffer to avoid data copying.
+
+  @param[in]  ExtFragment           Pointer to the data block.
+  @param[in]  ExtNum                The number of the data blocks.
+  @param[in]  HeadSpace             The head space to be reserved.
+  @param[in]  HeadLen               The length of the protocol header, This function
+                                    will pull that number of data into a linear block.
+  @param[in]  ExtFree               Pointer to the caller provided free function.
+  @param[in]  Arg                   The argument passed to ExtFree when ExtFree is
+                                    called.
+
+  @return                  Pointer to the net buffer built from the data blocks,
+                           or NULL if the allocation failed due to resource
+                           limit.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufFromExt (
+  IN NET_FRAGMENT           *ExtFragment,
+  IN UINT32                 ExtNum,
+  IN UINT32                 HeadSpace,
+  IN UINT32                 HeadLen,
+  IN NET_VECTOR_EXT_FREE    ExtFree,
+  IN VOID                   *Arg          OPTIONAL
+  )
+{
+  NET_BUF                   *Nbuf;
+  NET_VECTOR                *Vector;
+  NET_FRAGMENT              SavedFragment;
+  UINT32                    SavedIndex;
+  UINT32                    TotalLen;
+  UINT32                    BlockNum;
+  UINT8                     *FirstBlock;
+  UINT32                    FirstBlockLen;
+  UINT8                     *Header;
+  UINT32                    CurBlock;
+  UINT32                    Index;
+  UINT32                    Len;
+  UINT32                    Copied;
+
+  ASSERT ((ExtFragment != NULL) && (ExtNum > 0) && (ExtFree != NULL));
+
+  SavedFragment.Bulk = NULL;
+  SavedFragment.Len  = 0;
+
+  FirstBlockLen  = 0;
+  FirstBlock     = NULL;
+  BlockNum       = ExtNum;
+  Index          = 0;
+  TotalLen       = 0;
+  SavedIndex     = 0;
+  Len            = 0;
+  Copied         = 0;
+
+  //
+  // No need to consolidate the header if the first block is
+  // longer than the header length or there is only one block.
+  //
+  if ((ExtFragment[0].Len >= HeadLen) || (ExtNum == 1)) {
+    HeadLen = 0;
+  }
+
+  //
+  // Allocate an extra block if we need to:
+  //  1. Allocate some header space
+  //  2. aggreate the packet header
+  //
+  if ((HeadSpace != 0) || (HeadLen != 0)) {
+    FirstBlockLen = HeadLen + HeadSpace;
+    FirstBlock    = AllocatePool (FirstBlockLen);
+
+    if (FirstBlock == NULL) {
+      return NULL;
+    }
+
+    BlockNum++;
+  }
+
+  //
+  // Copy the header to the first block, reduce the NET_BLOCK
+  // to allocate by one for each block that is completely covered
+  // by the first bulk.
+  //
+  if (HeadLen != 0) {
+    Len    = HeadLen;
+    Header = FirstBlock + HeadSpace;
+
+    for (Index = 0; Index < ExtNum; Index++) {
+      if (Len >= ExtFragment[Index].Len) {
+        CopyMem (Header, ExtFragment[Index].Bulk, ExtFragment[Index].Len);
+
+        Copied    += ExtFragment[Index].Len;
+        Len       -= ExtFragment[Index].Len;
+        Header    += ExtFragment[Index].Len;
+        TotalLen  += ExtFragment[Index].Len;
+        BlockNum--;
+
+        if (Len == 0) {
+          //
+          // Increament the index number to point to the next
+          // non-empty fragment.
+          //
+          Index++;
+          break;
+        }
+
+      } else {
+        CopyMem (Header, ExtFragment[Index].Bulk, Len);
+
+        Copied    += Len;
+        TotalLen  += Len;
+
+        //
+        // Adjust the block structure to exclude the data copied,
+        // So, the left-over block can be processed as other blocks.
+        // But it must be recovered later. (SavedIndex > 0) always
+        // holds since we don't aggreate the header if the first block
+        // is bigger enough that the header is continuous
+        //
+        SavedIndex    = Index;
+        SavedFragment = ExtFragment[Index];
+        ExtFragment[Index].Bulk += Len;
+        ExtFragment[Index].Len  -= Len;
+        break;
+      }
+    }
+  }
+
+  Nbuf = NetbufAllocStruct (BlockNum, BlockNum);
+
+  if (Nbuf == NULL) {
+    goto FreeFirstBlock;
+  }
+
+  Vector       = Nbuf->Vector;
+  Vector->Free = ExtFree;
+  Vector->Arg  = Arg;
+  Vector->Flag = ((FirstBlockLen != 0) ? NET_VECTOR_OWN_FIRST : 0);
+
+  //
+  // Set the first block up which may contain
+  // some head space and aggregated header
+  //
+  CurBlock = 0;
+
+  if (FirstBlockLen != 0) {
+    NetbufSetBlock (Nbuf, FirstBlock, HeadSpace + Copied, 0);
+    Nbuf->BlockOp[0].Head += HeadSpace;
+    Nbuf->BlockOp[0].Size =  Copied;
+
+    CurBlock++;
+  }
+
+  for (; Index < ExtNum; Index++) {
+    NetbufSetBlock (Nbuf, ExtFragment[Index].Bulk, ExtFragment[Index].Len, CurBlock);
+    TotalLen += ExtFragment[Index].Len;
+    CurBlock++;
+  }
+
+  Vector->Len     = TotalLen + HeadSpace;
+  Nbuf->TotalSize = TotalLen;
+
+  if (SavedIndex != 0) {
+    ExtFragment[SavedIndex] = SavedFragment;
+  }
+
+  return Nbuf;
+
+FreeFirstBlock:
+  if (FirstBlock != NULL) {
+    FreePool (FirstBlock);
+  }
+  return NULL;
+}
+
+
+/**
+  Build a fragment table to contain the fragments in the net buffer. This is the
+  opposite operation of the NetbufFromExt.
+
+  @param[in]       Nbuf                  Point to the net buffer.
+  @param[in, out]  ExtFragment           Pointer to the data block.
+  @param[in, out]  ExtNum                The number of the data blocks.
+
+  @retval EFI_BUFFER_TOO_SMALL  The number of non-empty block is bigger than
+                                ExtNum.
+  @retval EFI_SUCCESS           Fragment table is built successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+NetbufBuildExt (
+  IN NET_BUF                *Nbuf,
+  IN OUT NET_FRAGMENT       *ExtFragment,
+  IN OUT UINT32             *ExtNum
+  )
+{
+  UINT32                    Index;
+  UINT32                    Current;
+
+  Current = 0;
+
+  for (Index = 0; (Index < Nbuf->BlockOpNum); Index++) {
+    if (Nbuf->BlockOp[Index].Size == 0) {
+      continue;
+    }
+
+    if (Current < *ExtNum) {
+      ExtFragment[Current].Bulk = Nbuf->BlockOp[Index].Head;
+      ExtFragment[Current].Len  = Nbuf->BlockOp[Index].Size;
+      Current++;
+    } else {
+      return EFI_BUFFER_TOO_SMALL;
+    }
+  }
+
+  *ExtNum = Current;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Build a net buffer from a list of net buffers.
+
+  All the fragments will be collected from the list of NEW_BUF and then a new
+  net buffer will be created through NetbufFromExt.
+
+  @param[in]   BufList    A List of the net buffer.
+  @param[in]   HeadSpace  The head space to be reserved.
+  @param[in]   HeaderLen  The length of the protocol header, This function
+                          will pull that number of data into a linear block.
+  @param[in]   ExtFree    Pointer to the caller provided free function.
+  @param[in]   Arg        The argument passed to ExtFree when ExtFree is called.
+
+  @return                 Pointer to the net buffer built from the list of net
+                          buffers.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufFromBufList (
+  IN LIST_ENTRY             *BufList,
+  IN UINT32                 HeadSpace,
+  IN UINT32                 HeaderLen,
+  IN NET_VECTOR_EXT_FREE    ExtFree,
+  IN VOID                   *Arg              OPTIONAL
+  )
+{
+  NET_FRAGMENT              *Fragment;
+  UINT32                    FragmentNum;
+  LIST_ENTRY                *Entry;
+  NET_BUF                   *Nbuf;
+  UINT32                    Index;
+  UINT32                    Current;
+
+  //
+  //Compute how many blocks are there
+  //
+  FragmentNum = 0;
+
+  NET_LIST_FOR_EACH (Entry, BufList) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+    FragmentNum += Nbuf->BlockOpNum;
+  }
+
+  //
+  //Allocate and copy block points
+  //
+  Fragment = AllocatePool (sizeof (NET_FRAGMENT) * FragmentNum);
+
+  if (Fragment == NULL) {
+    return NULL;
+  }
+
+  Current = 0;
+
+  NET_LIST_FOR_EACH (Entry, BufList) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+    NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+    for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
+      if (Nbuf->BlockOp[Index].Size != 0) {
+        Fragment[Current].Bulk = Nbuf->BlockOp[Index].Head;
+        Fragment[Current].Len  = Nbuf->BlockOp[Index].Size;
+        Current++;
+      }
+    }
+  }
+
+  Nbuf = NetbufFromExt (Fragment, Current, HeadSpace, HeaderLen, ExtFree, Arg);
+  FreePool (Fragment);
+
+  return Nbuf;
+}
+
+
+/**
+  Reserve some space in the header room of the net buffer.
+
+  Upon allocation, all the space are in the tail room of the buffer. Call this
+  function to move some space to the header room. This function is quite limited
+  in that it can only reserve space from the first block of an empty NET_BUF not
+  built from the external. But it should be enough for the network stack.
+
+  @param[in, out]  Nbuf     Pointer to the net buffer.
+  @param[in]       Len      The length of buffer to be reserved from the header.
+
+**/
+VOID
+EFIAPI
+NetbufReserve (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len
+  )
+{
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
+
+  ASSERT ((Nbuf->BlockOpNum == 1) && (Nbuf->TotalSize == 0));
+  ASSERT ((Nbuf->Vector->Free == NULL) && (Nbuf->Vector->Len >= Len));
+
+  Nbuf->BlockOp[0].Head += Len;
+  Nbuf->BlockOp[0].Tail += Len;
+
+  ASSERT (Nbuf->BlockOp[0].Tail <= Nbuf->BlockOp[0].BlockTail);
+}
+
+
+/**
+  Allocate Len bytes of space from the header or tail of the buffer.
+
+  @param[in, out]  Nbuf       Pointer to the net buffer.
+  @param[in]       Len        The length of the buffer to be allocated.
+  @param[in]       FromHead   The flag to indicate whether reserve the data
+                              from head (TRUE) or tail (FALSE).
+
+  @return                     Pointer to the first byte of the allocated buffer,
+                              or NULL if there is no sufficient space.
+
+**/
+UINT8*
+EFIAPI
+NetbufAllocSpace (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len,
+  IN BOOLEAN                FromHead
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    Index;
+  UINT8                     *SavedTail;
+
+  Index = 0;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
+
+  ASSERT (Len > 0);
+
+  if (FromHead) {
+    //
+    // Allocate some space from head. If the buffer is empty,
+    // allocate from the first block. If it isn't, allocate
+    // from the first non-empty block, or the block before that.
+    //
+    if (Nbuf->TotalSize == 0) {
+      Index = 0;
+    } else {
+      NetbufGetByte (Nbuf, 0, &Index);
+
+      if ((NET_HEADSPACE(&(Nbuf->BlockOp[Index])) < Len) && (Index > 0)) {
+        Index--;
+      }
+    }
+
+    BlockOp = &(Nbuf->BlockOp[Index]);
+
+    if (NET_HEADSPACE (BlockOp) < Len) {
+      return NULL;
+    }
+
+    BlockOp->Head   -= Len;
+    BlockOp->Size   += Len;
+    Nbuf->TotalSize += Len;
+
+    return BlockOp->Head;
+
+  } else {
+    //
+    // Allocate some space from the tail. If the buffer is empty,
+    // allocate from the first block. If it isn't, allocate
+    // from the last non-empty block, or the block after that.
+    //
+    if (Nbuf->TotalSize == 0) {
+      Index = 0;
+    } else {
+      NetbufGetByte (Nbuf, Nbuf->TotalSize - 1, &Index);
+
+      if ((NET_TAILSPACE(&(Nbuf->BlockOp[Index])) < Len) &&
+          (Index < Nbuf->BlockOpNum - 1)) {
+
+        Index++;
+      }
+    }
+
+    BlockOp = &(Nbuf->BlockOp[Index]);
+
+    if (NET_TAILSPACE (BlockOp) < Len) {
+      return NULL;
+    }
+
+    SavedTail       = BlockOp->Tail;
+
+    BlockOp->Tail   += Len;
+    BlockOp->Size   += Len;
+    Nbuf->TotalSize += Len;
+
+    return SavedTail;
+  }
+}
+
+
+/**
+  Trim a single NET_BLOCK by Len bytes from the header or tail.
+
+  @param[in, out]  BlockOp      Pointer to the NET_BLOCK.
+  @param[in]       Len          The length of the data to be trimmed.
+  @param[in]       FromHead     The flag to indicate whether trim data from head
+                                (TRUE) or tail (FALSE).
+
+**/
+VOID
+NetblockTrim (
+  IN OUT NET_BLOCK_OP       *BlockOp,
+  IN UINT32                 Len,
+  IN BOOLEAN                FromHead
+  )
+{
+  ASSERT ((BlockOp != NULL) && (BlockOp->Size >= Len));
+
+  BlockOp->Size -= Len;
+
+  if (FromHead) {
+    BlockOp->Head += Len;
+  } else {
+    BlockOp->Tail -= Len;
+  }
+}
+
+
+/**
+  Trim Len bytes from the header or tail of the net buffer.
+
+  @param[in, out]  Nbuf         Pointer to the net buffer.
+  @param[in]       Len          The length of the data to be trimmed.
+  @param[in]      FromHead      The flag to indicate whether trim data from head
+                                (TRUE) or tail (FALSE).
+
+  @return    Length of the actually trimmed data, which is possible to be less
+             than Len because the TotalSize of Nbuf is less than Len.
+
+**/
+UINT32
+EFIAPI
+NetbufTrim (
+  IN OUT NET_BUF            *Nbuf,
+  IN UINT32                 Len,
+  IN BOOLEAN                FromHead
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    Index;
+  UINT32                    Trimmed;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  if (Len > Nbuf->TotalSize) {
+    Len = Nbuf->TotalSize;
+  }
+
+  //
+  // If FromTail is true, iterate backward. That
+  // is, init Index to NBuf->BlockNum - 1, and
+  // decrease it by 1 during each loop. Otherwise,
+  // iterate forward. That is, init Index to 0, and
+  // increase it by 1 during each loop.
+  //
+  Trimmed          = 0;
+  Nbuf->TotalSize -= Len;
+
+  Index   = (FromHead ? 0 : Nbuf->BlockOpNum - 1);
+  BlockOp = Nbuf->BlockOp;
+
+  for (;;) {
+    if (BlockOp[Index].Size == 0) {
+      Index += (FromHead ? 1 : -1);
+      continue;
+    }
+
+    if (Len > BlockOp[Index].Size) {
+      Len     -= BlockOp[Index].Size;
+      Trimmed += BlockOp[Index].Size;
+      NetblockTrim (&BlockOp[Index], BlockOp[Index].Size, FromHead);
+    } else {
+      Trimmed += Len;
+      NetblockTrim (&BlockOp[Index], Len, FromHead);
+      break;
+    }
+
+    Index += (FromHead ? 1 : -1);
+  }
+
+  return Trimmed;
+}
+
+
+/**
+  Copy Len bytes of data from the specific offset of the net buffer to the
+  destination memory.
+
+  The Len bytes of data may cross the several fragments of the net buffer.
+
+  @param[in]   Nbuf         Pointer to the net buffer.
+  @param[in]   Offset       The sequence number of the first byte to copy.
+  @param[in]   Len          Length of the data to copy.
+  @param[in]   Dest         The destination of the data to copy to.
+
+  @return           The length of the actual copied data, or 0 if the offset
+                    specified exceeds the total size of net buffer.
+
+**/
+UINT32
+EFIAPI
+NetbufCopy (
+  IN NET_BUF                *Nbuf,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  IN UINT8                  *Dest
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    Skip;
+  UINT32                    Left;
+  UINT32                    Copied;
+  UINT32                    Index;
+  UINT32                    Cur;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  ASSERT (Dest);
+
+  if ((Len == 0) || (Nbuf->TotalSize <= Offset)) {
+    return 0;
+  }
+
+  if (Nbuf->TotalSize - Offset < Len) {
+    Len = Nbuf->TotalSize - Offset;
+  }
+
+  BlockOp = Nbuf->BlockOp;
+
+  //
+  // Skip to the offset. Don't make "Offset-By-One" error here.
+  // Cur + BLOCK.SIZE is the first sequence number of next block.
+  // So, (Offset < Cur + BLOCK.SIZE) means that the  first byte
+  // is in the current block. if (Offset == Cur + BLOCK.SIZE), the
+  // first byte is the next block's first byte.
+  //
+  Cur = 0;
+
+  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
+    if (BlockOp[Index].Size == 0) {
+      continue;
+    }
+
+    if (Offset < Cur + BlockOp[Index].Size) {
+      break;
+    }
+
+    Cur += BlockOp[Index].Size;
+  }
+
+  //
+  // Cur is the sequence number of the first byte in the block
+  // Offset - Cur is the number of bytes before first byte to
+  // to copy in the current block.
+  //
+  Skip  = Offset - Cur;
+  Left  = BlockOp[Index].Size - Skip;
+
+  if (Len <= Left) {
+    CopyMem (Dest, BlockOp[Index].Head + Skip, Len);
+    return Len;
+  }
+
+  CopyMem (Dest, BlockOp[Index].Head + Skip, Left);
+
+  Dest  += Left;
+  Len   -= Left;
+  Copied = Left;
+
+  Index++;
+
+  for (; Index < Nbuf->BlockOpNum; Index++) {
+    if (Len > BlockOp[Index].Size) {
+      Len    -= BlockOp[Index].Size;
+      Copied += BlockOp[Index].Size;
+
+      CopyMem (Dest, BlockOp[Index].Head, BlockOp[Index].Size);
+      Dest   += BlockOp[Index].Size;
+    } else {
+      Copied += Len;
+      CopyMem (Dest, BlockOp[Index].Head, Len);
+      break;
+    }
+  }
+
+  return Copied;
+}
+
+
+/**
+  Initiate the net buffer queue.
+
+  @param[in, out]  NbufQue   Pointer to the net buffer queue to be initialized.
+
+**/
+VOID
+EFIAPI
+NetbufQueInit (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  )
+{
+  NbufQue->Signature  = NET_QUE_SIGNATURE;
+  NbufQue->RefCnt     = 1;
+  InitializeListHead (&NbufQue->List);
+
+  InitializeListHead (&NbufQue->BufList);
+  NbufQue->BufSize  = 0;
+  NbufQue->BufNum   = 0;
+}
+
+
+/**
+  Allocate and initialize a net buffer queue.
+
+  @return         Pointer to the allocated net buffer queue, or NULL if the
+                  allocation failed due to resource limit.
+
+**/
+NET_BUF_QUEUE  *
+EFIAPI
+NetbufQueAlloc (
+  VOID
+  )
+{
+  NET_BUF_QUEUE             *NbufQue;
+
+  NbufQue = AllocatePool (sizeof (NET_BUF_QUEUE));
+  if (NbufQue == NULL) {
+    return NULL;
+  }
+
+  NetbufQueInit (NbufQue);
+
+  return NbufQue;
+}
+
+
+/**
+  Free a net buffer queue.
+
+  Decrease the reference count of the net buffer queue by one. The real resource
+  free operation isn't performed until the reference count of the net buffer
+  queue is decreased to 0.
+
+  @param[in]  NbufQue               Pointer to the net buffer queue to be freed.
+
+**/
+VOID
+EFIAPI
+NetbufQueFree (
+  IN NET_BUF_QUEUE          *NbufQue
+  )
+{
+  ASSERT (NbufQue != NULL);
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+
+  NbufQue->RefCnt--;
+
+  if (NbufQue->RefCnt == 0) {
+    NetbufQueFlush (NbufQue);
+    FreePool (NbufQue);
+  }
+}
+
+
+/**
+  Append a net buffer to the net buffer queue.
+
+  @param[in, out]  NbufQue            Pointer to the net buffer queue.
+  @param[in, out]  Nbuf               Pointer to the net buffer to be appended.
+
+**/
+VOID
+EFIAPI
+NetbufQueAppend (
+  IN OUT NET_BUF_QUEUE          *NbufQue,
+  IN OUT NET_BUF                *Nbuf
+  )
+{
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  InsertTailList (&NbufQue->BufList, &Nbuf->List);
+
+  NbufQue->BufSize += Nbuf->TotalSize;
+  NbufQue->BufNum++;
+}
+
+
+/**
+  Remove a net buffer from the head in the specific queue and return it.
+
+  @param[in, out]  NbufQue               Pointer to the net buffer queue.
+
+  @return           Pointer to the net buffer removed from the specific queue,
+                    or NULL if there is no net buffer in the specific queue.
+
+**/
+NET_BUF  *
+EFIAPI
+NetbufQueRemove (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  )
+{
+  NET_BUF                   *First;
+
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+
+  if (NbufQue->BufNum == 0) {
+    return NULL;
+  }
+
+  First = NET_LIST_USER_STRUCT (NbufQue->BufList.ForwardLink, NET_BUF, List);
+
+  NetListRemoveHead (&NbufQue->BufList);
+
+  NbufQue->BufSize -= First->TotalSize;
+  NbufQue->BufNum--;
+  return First;
+}
+
+
+/**
+  Copy Len bytes of data from the net buffer queue at the specific offset to the
+  destination memory.
+
+  The copying operation is the same as NetbufCopy but applies to the net buffer
+  queue instead of the net buffer.
+
+  @param[in]   NbufQue         Pointer to the net buffer queue.
+  @param[in]   Offset          The sequence number of the first byte to copy.
+  @param[in]   Len             Length of the data to copy.
+  @param[out]  Dest            The destination of the data to copy to.
+
+  @return       The length of the actual copied data, or 0 if the offset
+                specified exceeds the total size of net buffer queue.
+
+**/
+UINT32
+EFIAPI
+NetbufQueCopy (
+  IN NET_BUF_QUEUE          *NbufQue,
+  IN UINT32                 Offset,
+  IN UINT32                 Len,
+  OUT UINT8                 *Dest
+  )
+{
+  LIST_ENTRY                *Entry;
+  NET_BUF                   *Nbuf;
+  UINT32                    Skip;
+  UINT32                    Left;
+  UINT32                    Cur;
+  UINT32                    Copied;
+
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+  ASSERT (Dest != NULL);
+
+  if ((Len == 0) || (NbufQue->BufSize <= Offset)) {
+    return 0;
+  }
+
+  if (NbufQue->BufSize - Offset < Len) {
+    Len = NbufQue->BufSize - Offset;
+  }
+
+  //
+  // skip to the Offset
+  //
+  Cur   = 0;
+  Nbuf  = NULL;
+
+  NET_LIST_FOR_EACH (Entry, &NbufQue->BufList) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    if (Offset < Cur + Nbuf->TotalSize) {
+      break;
+    }
+
+    Cur += Nbuf->TotalSize;
+  }
+
+  ASSERT (Nbuf != NULL);
+
+  //
+  // Copy the data in the first buffer.
+  //
+  Skip  = Offset - Cur;
+  Left  = Nbuf->TotalSize - Skip;
+
+  if (Len < Left) {
+    return NetbufCopy (Nbuf, Skip, Len, Dest);
+  }
+
+  NetbufCopy (Nbuf, Skip, Left, Dest);
+  Dest  += Left;
+  Len   -= Left;
+  Copied = Left;
+
+  //
+  // Iterate over the others
+  //
+  Entry = Entry->ForwardLink;
+
+  while ((Len > 0) && (Entry != &NbufQue->BufList)) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    if (Len > Nbuf->TotalSize) {
+      Len -= Nbuf->TotalSize;
+      Copied += Nbuf->TotalSize;
+
+      NetbufCopy (Nbuf, 0, Nbuf->TotalSize, Dest);
+      Dest += Nbuf->TotalSize;
+
+    } else {
+      NetbufCopy (Nbuf, 0, Len, Dest);
+      Copied += Len;
+      break;
+    }
+
+    Entry = Entry->ForwardLink;
+  }
+
+  return Copied;
+}
+
+
+/**
+  Trim Len bytes of data from the buffer queue and free any net buffer
+  that is completely trimmed.
+
+  The trimming operation is the same as NetbufTrim but applies to the net buffer
+  queue instead of the net buffer.
+
+  @param[in, out]  NbufQue               Pointer to the net buffer queue.
+  @param[in]       Len                   Length of the data to trim.
+
+  @return   The actual length of the data trimmed.
+
+**/
+UINT32
+EFIAPI
+NetbufQueTrim (
+  IN OUT NET_BUF_QUEUE      *NbufQue,
+  IN UINT32                 Len
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  NET_BUF                   *Nbuf;
+  UINT32                    Trimmed;
+
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+
+  if (Len == 0) {
+    return 0;
+  }
+
+  if (Len > NbufQue->BufSize) {
+    Len = NbufQue->BufSize;
+  }
+
+  NbufQue->BufSize -= Len;
+  Trimmed = 0;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &NbufQue->BufList) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    if (Len >= Nbuf->TotalSize) {
+      Trimmed += Nbuf->TotalSize;
+      Len -= Nbuf->TotalSize;
+
+      RemoveEntryList (Entry);
+      NetbufFree (Nbuf);
+
+      NbufQue->BufNum--;
+
+      if (Len == 0) {
+        break;
+      }
+
+    } else {
+      Trimmed += NetbufTrim (Nbuf, Len, NET_BUF_HEAD);
+      break;
+    }
+  }
+
+  return Trimmed;
+}
+
+
+/**
+  Flush the net buffer queue.
+
+  @param[in, out]  NbufQue               Pointer to the queue to be flushed.
+
+**/
+VOID
+EFIAPI
+NetbufQueFlush (
+  IN OUT NET_BUF_QUEUE          *NbufQue
+  )
+{
+  NET_CHECK_SIGNATURE (NbufQue, NET_QUE_SIGNATURE);
+
+  NetbufFreeList (&NbufQue->BufList);
+
+  NbufQue->BufNum   = 0;
+  NbufQue->BufSize  = 0;
+}
+
+
+/**
+  Compute the checksum for a bulk of data.
+
+  @param[in]   Bulk                  Pointer to the data.
+  @param[in]   Len                   Length of the data, in bytes.
+
+  @return    The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetblockChecksum (
+  IN UINT8                  *Bulk,
+  IN UINT32                 Len
+  )
+{
+  register UINT32           Sum;
+
+  Sum = 0;
+
+  while (Len > 1) {
+    Sum += *(UINT16 *) Bulk;
+    Bulk += 2;
+    Len -= 2;
+  }
+
+  //
+  // Add left-over byte, if any
+  //
+  if (Len > 0) {
+    Sum += *(UINT8 *) Bulk;
+  }
+
+  //
+  // Fold 32-bit sum to 16 bits
+  //
+  while ((Sum >> 16) != 0) {
+    Sum = (Sum & 0xffff) + (Sum >> 16);
+
+  }
+
+  return (UINT16) Sum;
+}
+
+
+/**
+  Add two checksums.
+
+  @param[in]   Checksum1             The first checksum to be added.
+  @param[in]   Checksum2             The second checksum to be added.
+
+  @return         The new checksum.
+
+**/
+UINT16
+EFIAPI
+NetAddChecksum (
+  IN UINT16                 Checksum1,
+  IN UINT16                 Checksum2
+  )
+{
+  UINT32                    Sum;
+
+  Sum = Checksum1 + Checksum2;
+
+  //
+  // two UINT16 can only add up to a carry of 1.
+  //
+  if ((Sum >> 16) != 0) {
+    Sum = (Sum & 0xffff) + 1;
+
+  }
+
+  return (UINT16) Sum;
+}
+
+
+/**
+  Compute the checksum for a NET_BUF.
+
+  @param[in]   Nbuf                  Pointer to the net buffer.
+
+  @return    The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetbufChecksum (
+  IN NET_BUF                *Nbuf
+  )
+{
+  NET_BLOCK_OP              *BlockOp;
+  UINT32                    Offset;
+  UINT16                    TotalSum;
+  UINT16                    BlockSum;
+  UINT32                    Index;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  TotalSum  = 0;
+  Offset    = 0;
+  BlockOp   = Nbuf->BlockOp;
+
+  for (Index = 0; Index < Nbuf->BlockOpNum; Index++) {
+    if (BlockOp[Index].Size == 0) {
+      continue;
+    }
+
+    BlockSum = NetblockChecksum (BlockOp[Index].Head, BlockOp[Index].Size);
+
+    if ((Offset & 0x01) != 0) {
+      //
+      // The checksum starts with an odd byte, swap
+      // the checksum before added to total checksum
+      //
+      BlockSum = SwapBytes16 (BlockSum);
+    }
+
+    TotalSum = NetAddChecksum (BlockSum, TotalSum);
+    Offset  += BlockOp[Index].Size;
+  }
+
+  return TotalSum;
+}
+
+
+/**
+  Compute the checksum for TCP/UDP pseudo header.
+
+  Src and Dst are in network byte order, and Len is in host byte order.
+
+  @param[in]   Src                   The source address of the packet.
+  @param[in]   Dst                   The destination address of the packet.
+  @param[in]   Proto                 The protocol type of the packet.
+  @param[in]   Len                   The length of the packet.
+
+  @return   The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetPseudoHeadChecksum (
+  IN IP4_ADDR               Src,
+  IN IP4_ADDR               Dst,
+  IN UINT8                  Proto,
+  IN UINT16                 Len
+  )
+{
+  NET_PSEUDO_HDR            Hdr;
+
+  //
+  // Zero the memory to relieve align problems
+  //
+  ZeroMem (&Hdr, sizeof (Hdr));
+
+  Hdr.SrcIp     = Src;
+  Hdr.DstIp     = Dst;
+  Hdr.Protocol  = Proto;
+  Hdr.Len       = HTONS (Len);
+
+  return NetblockChecksum ((UINT8 *) &Hdr, sizeof (Hdr));
+}
+
+/**
+  Compute the checksum for TCP6/UDP6 pseudo header.
+
+  Src and Dst are in network byte order, and Len is in host byte order.
+
+  @param[in]   Src                   The source address of the packet.
+  @param[in]   Dst                   The destination address of the packet.
+  @param[in]   NextHeader            The protocol type of the packet.
+  @param[in]   Len                   The length of the packet.
+
+  @return   The computed checksum.
+
+**/
+UINT16
+EFIAPI
+NetIp6PseudoHeadChecksum (
+  IN EFI_IPv6_ADDRESS       *Src,
+  IN EFI_IPv6_ADDRESS       *Dst,
+  IN UINT8                  NextHeader,
+  IN UINT32                 Len
+  )
+{
+  NET_IP6_PSEUDO_HDR        Hdr;
+
+  //
+  // Zero the memory to relieve align problems
+  //
+  ZeroMem (&Hdr, sizeof (Hdr));
+
+  IP6_COPY_ADDRESS (&Hdr.SrcIp, Src);
+  IP6_COPY_ADDRESS (&Hdr.DstIp, Dst);
+
+  Hdr.NextHeader = NextHeader;
+  Hdr.Len        = HTONL (Len);
+
+  return NetblockChecksum ((UINT8 *) &Hdr, sizeof (Hdr));
+}
+
+/**
+  The function frees the net buffer which allocated by the IP protocol. It releases 
+  only the net buffer and doesn't call the external free function. 
+
+  This function should be called after finishing the process of mIpSec->ProcessExt() 
+  for outbound traffic. The (EFI_IPSEC2_PROTOCOL)->ProcessExt() allocates a new 
+  buffer for the ESP, so there needs a function to free the old net buffer.
+
+  @param[in]  Nbuf       The network buffer to be freed.
+
+**/
+VOID
+NetIpSecNetbufFree (
+  NET_BUF   *Nbuf
+  )
+{
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+  ASSERT (Nbuf->RefCnt > 0);
+
+  Nbuf->RefCnt--;
+
+  if (Nbuf->RefCnt == 0) {
+    
+    //
+    // Update Vector only when NBuf is to be released. That is,
+    // all the sharing of Nbuf increse Vector's RefCnt by one
+    //
+    NET_CHECK_SIGNATURE (Nbuf->Vector, NET_VECTOR_SIGNATURE);
+    ASSERT (Nbuf->Vector->RefCnt > 0);
+
+    Nbuf->Vector->RefCnt--;
+
+    if (Nbuf->Vector->RefCnt > 0) {
+      return;
+    }
+
+    //
+    // If NET_VECTOR_OWN_FIRST is set, release the first block since it is 
+    // allocated by us
+    //
+    if ((Nbuf->Vector->Flag & NET_VECTOR_OWN_FIRST) != 0) {
+      FreePool (Nbuf->Vector->Block[0].Bulk);
+    }
+    FreePool (Nbuf->Vector);
+    FreePool (Nbuf); 
+  } 
+}
+
diff --git a/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.c b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.c
new file mode 100644
index 0000000000..2ddd983126
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.c
@@ -0,0 +1,429 @@
+/** @file
+  Performance Library
+
+  This library instance provides infrastructure for DXE phase drivers to log performance
+  data. It consumes PerformanceEx or Performance Protocol published by DxeCorePerformanceLib
+  to log performance data. If both PerformanceEx and Performance Protocol is not available, it does not log any
+  performance information.
+
+  Copyright (c) 2006 - 2012, 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 <PiDxe.h>
+
+#include <Guid/Performance.h>
+
+#include <Library/PerformanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/PcdLib.h>
+
+//
+// The cached Performance Protocol and PerformanceEx Protocol interface.
+//
+PERFORMANCE_PROTOCOL        *mPerformance = NULL;
+PERFORMANCE_EX_PROTOCOL     *mPerformanceEx = NULL;
+
+/**
+  The function caches the pointers to PerformanceEx protocol and Performance Protocol.
+
+  The function locates PerformanceEx protocol and Performance Protocol from protocol database.
+
+  @retval EFI_SUCCESS     PerformanceEx protocol or Performance Protocol is successfully located.
+  @retval EFI_NOT_FOUND   Both PerformanceEx protocol and Performance Protocol are not located to log performance.
+
+**/
+EFI_STATUS
+GetPerformanceProtocol (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  PERFORMANCE_PROTOCOL      *Performance;
+  PERFORMANCE_EX_PROTOCOL   *PerformanceEx;
+
+  if (mPerformanceEx != NULL || mPerformance != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->LocateProtocol (&gPerformanceExProtocolGuid, NULL, (VOID **) &PerformanceEx);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (PerformanceEx != NULL);
+    //
+    // Cache PerformanceEx Protocol.
+    //
+    mPerformanceEx = PerformanceEx;
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->LocateProtocol (&gPerformanceProtocolGuid, NULL, (VOID **) &Performance);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (Performance != NULL);
+    //
+    // Cache performance protocol.
+    //
+    mPerformance = Performance;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, Module and Identifier.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->StartGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->StartGauge (Handle, Token, Module, TimeStamp);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return (RETURN_STATUS) Status;
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, Module and Identifier.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return RETURN_NOT_FOUND;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->EndGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->EndGauge (Handle, Token, Module, TimeStamp);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return (RETURN_STATUS) Status;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  EFI_STATUS            Status;
+  GAUGE_DATA_ENTRY_EX   *GaugeData;
+
+  GaugeData = NULL;
+
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->GetGaugeEx (LogEntryKey++, &GaugeData);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->GetGauge (LogEntryKey++, (GAUGE_DATA_ENTRY **) &GaugeData);
+  } else {
+    ASSERT (FALSE);
+    return 0;
+  }
+
+  //
+  // Make sure that LogEntryKey is a valid log entry key,
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // The LogEntryKey is the last entry (equals to the total entry number).
+    //
+    return 0;
+  }
+
+  ASSERT (GaugeData != NULL);
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+  if (mPerformanceEx != NULL) {
+    *Identifier   = GaugeData->Identifier;
+  } else {
+    *Identifier   = 0;
+  }
+
+  return LogEntryKey;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, and Module.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, and Module.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.inf b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.inf
new file mode 100644
index 0000000000..edc63c6f0b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.inf
@@ -0,0 +1,57 @@
+## @file
+#  Performance library instance used in DXE phase.
+#  
+#  This library instance provides infrastructure for DXE phase drivers to log performance
+#  data. It consumes PerformanceEx or Performance Protocol published by DxeCorePerformanceLib
+#  to log performance data. If both PerformanceEx and Performance Protocol are not available,
+#  it does not log any performance information.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxePerformanceLib
+  MODULE_UNI_FILE                = DxePerformanceLib.uni
+  FILE_GUID                      = 8B8B4CCC-65FC-41a5-8067-308B8E42CCF2
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PerformanceLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxePerformanceLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  PcdLib
+  UefiBootServicesTableLib
+  DebugLib
+
+
+[Guids]
+  gPerformanceProtocolGuid      ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+  gPerformanceExProtocolGuid    ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.uni b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.uni
new file mode 100644
index 0000000000..d2997bce98
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.inf b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.inf
new file mode 100644
index 0000000000..3d09b4be4d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.inf
@@ -0,0 +1,41 @@
+## @file
+#  Library instance that implements Print Library class based on protocol gEfiPrint2ProtocolGuid.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxePrintLibPrint2Protocol
+  MODULE_UNI_FILE                = DxePrintLibPrint2Protocol.uni
+  FILE_GUID                      = 55D460DB-8FEA-415a-B95D-70145AE0675C
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PrintLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+  CONSTRUCTOR                    = PrintLibConstructor
+
+[Sources]
+  PrintLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+
+[Protocols]
+  gEfiPrint2ProtocolGuid                         ## CONSUMES
+
+[Depex.common.DXE_DRIVER, Depex.common.DXE_RUNTIME_DRIVER, Depex.common.DXE_SAL_DRIVER, Depex.common.DXE_SMM_DRIVER]
+  gEfiPrint2ProtocolGuid
diff --git a/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.uni b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.uni
new file mode 100644
index 0000000000..96a8bd0a09
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.uni
diff --git a/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/PrintLib.c b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/PrintLib.c
new file mode 100644
index 0000000000..4acdcb8714
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxePrintLibPrint2Protocol/PrintLib.c
@@ -0,0 +1,1921 @@
+/** @file
+  Instance of Print Library based on gEfiPrint2ProtocolGuid.
+
+  Implement the print library instance by wrap the interface 
+  provided in the Print2 protocol. This protocol is defined as the internal
+  protocol related to this implementation, not in the public spec. So, this 
+  library instance is only for this code base.
+
+Copyright (c) 2009 - 2015, 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 <Uefi.h>
+#include <Base.h>
+#include <Protocol/Print2.h>
+
+#include <Library/PrintLib.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+
+EFI_PRINT2_PROTOCOL  *mPrint2Protocol = NULL;
+
+/**
+  The constructor function caches the pointer to Print2 protocol.
+  
+  The constructor function locates Print2 protocol from protocol database.
+  It will ASSERT() if that operation fails and it will always return EFI_SUCCESS. 
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+PrintLibConstructor (
+  IN EFI_HANDLE                ImageHandle,
+  IN EFI_SYSTEM_TABLE          *SystemTable
+  )
+{
+  EFI_STATUS                   Status;
+
+  Status = SystemTable->BootServices->LocateProtocol (
+                                        &gEfiPrint2ProtocolGuid,
+                                        NULL,
+                                        (VOID**) &mPrint2Protocol
+                                        );
+  ASSERT_EFI_ERROR (Status);
+  ASSERT (mPrint2Protocol != NULL);
+
+  return Status;
+}
+
+
+/**
+  Worker function that converts a VA_LIST to a BASE_LIST based on a Null-terminated 
+  format string.
+
+  @param  AsciiFormat     TRUE if Format is an ASCII string.  FALSE if Format is a Unicode string.
+  @param  Format          Null-terminated format string.
+  @param  VaListMarker    VA_LIST style variable argument list consumed by processing Format.
+  @param  BaseListMarker  BASE_LIST style variable argument list consumed by processing Format.
+  @param  Size            The size, in bytes, of the BaseListMarker buffer.
+
+  @return The number of bytes in BaseListMarker.  0 if BaseListMarker is too small.
+
+**/
+BOOLEAN
+DxePrintLibPrint2ProtocolVaListToBaseList (
+  IN  BOOLEAN      AsciiFormat,
+  IN  CONST CHAR8  *Format,
+  IN  VA_LIST      VaListMarker,
+  OUT BASE_LIST    BaseListMarker,
+  IN  UINTN        Size
+  )
+{
+  BASE_LIST  BaseListStart;
+  UINTN      BytesPerFormatCharacter;
+  UINTN      FormatMask;
+  UINTN      FormatCharacter;
+  BOOLEAN    Long;
+  BOOLEAN    Done;
+
+  ASSERT (Format         != NULL);
+  ASSERT (BaseListMarker != NULL);
+
+  BaseListStart = BaseListMarker;
+
+  if (AsciiFormat) {
+    ASSERT (AsciiStrSize (Format) != 0);
+    BytesPerFormatCharacter = 1;
+    FormatMask = 0xff;
+  } else {
+    ASSERT (StrSize ((CHAR16 *) Format) != 0);
+    BytesPerFormatCharacter = 2;
+    FormatMask = 0xffff;
+  }
+
+  //
+  // Get the first character from the format string
+  //
+  FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+
+  while (FormatCharacter != 0) {
+    if (FormatCharacter == '%') {
+      Long = FALSE;
+
+      //
+      // Parse Flags and Width
+      //
+      for (Done = FALSE; !Done; ) {
+        //
+        // Get the next character from the format string
+        //
+        Format += BytesPerFormatCharacter;
+
+        //
+        // Get the next character from the format string
+        //
+        FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+
+        switch (FormatCharacter) {
+        case '.': 
+        case '-': 
+        case '+': 
+        case ' ': 
+        case ',': 
+        case '0':
+        case '1':
+        case '2':
+        case '3':
+        case '4':
+        case '5':
+        case '6':
+        case '7':
+        case '8':
+        case '9':
+          break;
+        case 'L':
+        case 'l': 
+          Long = TRUE;
+          break;
+        case '*':
+          BASE_ARG (BaseListMarker, UINTN) = VA_ARG (VaListMarker, UINTN);
+          break;
+        case '\0':
+          //
+          // Make no output if Format string terminates unexpectedly when
+          // looking up for flag, width, precision and type. 
+          //
+          Format -= BytesPerFormatCharacter;
+          //
+          // break skipped on purpose.
+          //
+        default:
+          Done = TRUE;
+          break;
+        }
+      } 
+        
+      //
+      // Handle each argument type
+      //
+      switch (FormatCharacter) {
+      case 'p':
+        if (sizeof (VOID *) > 4) {
+          Long = TRUE;
+        }
+      case 'X':
+      case 'x':
+      case 'u':
+      case 'd':
+        if (Long) {
+          BASE_ARG (BaseListMarker, INT64) = VA_ARG (VaListMarker, INT64);
+        } else {
+          BASE_ARG (BaseListMarker, int) = VA_ARG (VaListMarker, int);
+        }
+        break;
+      case 's':
+      case 'S':
+      case 'a':
+      case 'g':
+      case 't':
+        BASE_ARG (BaseListMarker, VOID *) = VA_ARG (VaListMarker, VOID *);
+        break;
+      case 'c':
+        BASE_ARG (BaseListMarker, UINTN) = VA_ARG (VaListMarker, UINTN);
+        break;
+      case 'r':
+        BASE_ARG (BaseListMarker, RETURN_STATUS) = VA_ARG (VaListMarker, RETURN_STATUS);
+        break;
+      }
+    }
+
+    //
+    // If BASE_LIST is larger than Size, then return FALSE
+    //
+    if ((UINTN)((UINT8 *)BaseListMarker - (UINT8 *)BaseListStart) > Size) {
+      return FALSE;
+    }
+
+    //
+    // Get the next character from the format string
+    //
+    Format += BytesPerFormatCharacter;
+
+    //
+    // Get the next character from the format string
+    //
+    FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+  }
+  return TRUE;
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on 
+  a Null-terminated Unicode format string and a VA_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.  
+  The Unicode string is produced by parsing the format string specified by FormatString.  
+  Arguments are pulled from the variable argument list specified by Marker based on the 
+  contents of the format string.  
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If BufferSize > 1 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          VA_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeVSPrint (
+  OUT CHAR16        *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  VA_LIST       Marker
+  )
+{
+  UINT64  BaseListMarker[256 / sizeof (UINT64)];
+
+  DxePrintLibPrint2ProtocolVaListToBaseList (
+    FALSE, 
+    (CHAR8 *)FormatString, 
+    Marker, 
+    (BASE_LIST)BaseListMarker, 
+    sizeof (BaseListMarker) - 8
+    );
+
+  return UnicodeBSPrint (StartOfBuffer, BufferSize, FormatString, (BASE_LIST)BaseListMarker);
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on 
+  a Null-terminated Unicode format string and a BASE_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.  
+  The Unicode string is produced by parsing the format string specified by FormatString.  
+  Arguments are pulled from the variable argument list specified by Marker based on the 
+  contents of the format string.  
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If BufferSize > 1 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeBSPrint (
+  OUT CHAR16        *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  BASE_LIST     Marker
+  )
+{
+  return mPrint2Protocol->UnicodeBSPrint (StartOfBuffer, BufferSize, FormatString, Marker);
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated 
+  Unicode format string and variable argument list.
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the format string.
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If BufferSize > 1 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than 
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeSPrint (
+  OUT CHAR16        *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  ...
+  )
+{
+  VA_LIST Marker;
+  UINTN   NumberOfPrinted;
+
+  VA_START (Marker, FormatString);
+  NumberOfPrinted = UnicodeVSPrint (StartOfBuffer, BufferSize, FormatString, Marker);
+  VA_END (Marker);
+  return NumberOfPrinted;
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated
+  ASCII format string and a VA_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on the 
+  contents of the format string.
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          VA_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeVSPrintAsciiFormat (
+  OUT CHAR16       *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  IN  VA_LIST      Marker
+  )
+{
+  UINT64  BaseListMarker[256 / sizeof (UINT64)];
+
+  DxePrintLibPrint2ProtocolVaListToBaseList (
+    TRUE, 
+    FormatString, 
+    Marker, 
+    (BASE_LIST)BaseListMarker, 
+    sizeof (BaseListMarker) - 8
+    );
+
+  return UnicodeBSPrintAsciiFormat (StartOfBuffer, BufferSize, FormatString, (BASE_LIST)BaseListMarker);
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated
+  ASCII format string and a BASE_LIST argument list
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on the 
+  contents of the format string.
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeBSPrintAsciiFormat (
+  OUT CHAR16       *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  IN  BASE_LIST    Marker
+  )
+{
+  return mPrint2Protocol->UnicodeBSPrintAsciiFormat (StartOfBuffer, BufferSize, FormatString, Marker);
+}
+
+/**
+  Produces a Null-terminated Unicode string in an output buffer based on a Null-terminated 
+  ASCII format string and  variable argument list.
+  
+  Produces a Null-terminated Unicode string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The Unicode string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the 
+  format string.
+  The number of Unicode characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0 or 1, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 1 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 1 and StartOfBuffer is not aligned on a 16-bit boundary, then ASSERT().
+  If BufferSize > 1 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and produced Null-terminated Unicode string
+  contains more than PcdMaximumUnicodeStringLength Unicode characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          Unicode string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+  
+  @return The number of Unicode characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeSPrintAsciiFormat (
+  OUT CHAR16       *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  ...
+  )
+{
+  VA_LIST Marker;
+  UINTN   NumberOfPrinted;
+
+  VA_START (Marker, FormatString);
+  NumberOfPrinted = UnicodeVSPrintAsciiFormat (StartOfBuffer, BufferSize, FormatString, Marker);
+  VA_END (Marker);
+  return NumberOfPrinted;
+}
+
+/**
+  Converts a decimal value to a Null-terminated Unicode string.
+  
+  Converts the decimal number specified by Value to a Null-terminated Unicode 
+  string specified by Buffer containing at most Width characters. No padding of spaces 
+  is ever performed. If Width is 0 then a width of MAXIMUM_VALUE_CHARACTERS is assumed.
+  The number of Unicode characters in Buffer is returned not including the Null-terminator.
+  If the conversion contains more than Width characters, then only the first
+  Width characters are returned, and the total number of characters 
+  required to perform the conversion is returned.
+  Additional conversion parameters are specified in Flags.  
+  
+  The Flags bit LEFT_JUSTIFY is always ignored.
+  All conversions are left justified in Buffer.
+  If Width is 0, PREFIX_ZERO is ignored in Flags.
+  If COMMA_TYPE is set in Flags, then PREFIX_ZERO is ignored in Flags, and commas
+  are inserted every 3rd digit starting from the right.
+  If RADIX_HEX is set in Flags, then the output buffer will be 
+  formatted in hexadecimal format.
+  If Value is < 0 and RADIX_HEX is not set in Flags, then the fist character in Buffer is a '-'.
+  If PREFIX_ZERO is set in Flags and PREFIX_ZERO is not being ignored, 
+  then Buffer is padded with '0' characters so the combination of the optional '-' 
+  sign character, '0' characters, digit characters for Value, and the Null-terminator
+  add up to Width characters.
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Buffer is NULL, then ASSERT().
+  If Buffer is not aligned on a 16-bit boundary, then ASSERT().
+  If unsupported bits are set in Flags, then ASSERT().
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Width >= MAXIMUM_VALUE_CHARACTERS, then ASSERT()
+
+  @param  Buffer  Pointer to the output buffer for the produced Null-terminated
+                  Unicode string.
+  @param  Flags   The bitmask of flags that specify left justification, zero pad, and commas.
+  @param  Value   The 64-bit signed value to convert to a string.
+  @param  Width   The maximum number of Unicode characters to place in Buffer, not including
+                  the Null-terminator.
+  
+  @return The number of Unicode characters in Buffer not including the Null-terminator.
+
+**/
+UINTN
+EFIAPI
+UnicodeValueToString (
+  IN OUT CHAR16  *Buffer,
+  IN UINTN       Flags,
+  IN INT64       Value,
+  IN UINTN       Width
+  )
+{
+  return mPrint2Protocol->UnicodeValueToString (Buffer, Flags, Value, Width);
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and a VA_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          VA_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiVSPrint (
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR8   *FormatString,
+  IN  VA_LIST       Marker
+  )
+{
+  UINT64  BaseListMarker[256 / sizeof (UINT64)];
+
+  DxePrintLibPrint2ProtocolVaListToBaseList (
+    TRUE, 
+    FormatString, 
+    Marker, 
+    (BASE_LIST)BaseListMarker, 
+    sizeof (BaseListMarker) - 8
+    );
+
+  return AsciiBSPrint (StartOfBuffer, BufferSize, FormatString, (BASE_LIST)BaseListMarker);
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and a BASE_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiBSPrint (
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR8   *FormatString,
+  IN  BASE_LIST     Marker
+  )
+{
+  return mPrint2Protocol->AsciiBSPrint (StartOfBuffer, BufferSize, FormatString, Marker);
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and  variable argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the 
+  format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and FormatString contains more than
+  PcdMaximumAsciiStringLength ASCII characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiSPrint (
+  OUT CHAR8        *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  CONST CHAR8  *FormatString,
+  ...
+  )
+{
+  VA_LIST Marker;
+  UINTN   NumberOfPrinted;
+
+  VA_START (Marker, FormatString);
+  NumberOfPrinted = AsciiVSPrint (StartOfBuffer, BufferSize, FormatString, Marker);
+  VA_END (Marker);
+  return NumberOfPrinted;
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and a VA_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          VA_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiVSPrintUnicodeFormat (
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  VA_LIST       Marker
+  )
+{
+  UINT64  BaseListMarker[256 / sizeof (UINT64)];
+
+  DxePrintLibPrint2ProtocolVaListToBaseList (
+    FALSE, 
+    (CHAR8 *)FormatString, 
+    Marker, 
+    (BASE_LIST)BaseListMarker, 
+    sizeof (BaseListMarker) - 8
+    );
+
+  return AsciiBSPrintUnicodeFormat (StartOfBuffer, BufferSize, FormatString, (BASE_LIST)BaseListMarker);
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and a BASE_LIST argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list specified by Marker based on 
+  the contents of the format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  Marker          BASE_LIST marker for the variable argument list.
+  
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiBSPrintUnicodeFormat (
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  IN  BASE_LIST     Marker
+  )
+{
+  return mPrint2Protocol->AsciiBSPrintUnicodeFormat (StartOfBuffer, BufferSize, FormatString, Marker);
+}
+
+/**
+  Produces a Null-terminated ASCII string in an output buffer based on a Null-terminated
+  ASCII format string and  variable argument list.
+  
+  Produces a Null-terminated ASCII string in the output buffer specified by StartOfBuffer
+  and BufferSize.
+  The ASCII string is produced by parsing the format string specified by FormatString.
+  Arguments are pulled from the variable argument list based on the contents of the 
+  format string.
+  The number of ASCII characters in the produced output buffer is returned not including
+  the Null-terminator.
+  If BufferSize is 0, then no output buffer is produced and 0 is returned.
+
+  If BufferSize > 0 and StartOfBuffer is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is NULL, then ASSERT().
+  If BufferSize > 0 and FormatString is not aligned on a 16-bit boundary, then ASSERT().
+  If PcdMaximumUnicodeStringLength is not zero, and FormatString contains more than
+  PcdMaximumUnicodeStringLength Unicode characters not including the Null-terminator, then
+  ASSERT().
+  If PcdMaximumAsciiStringLength is not zero, and produced Null-terminated ASCII string
+  contains more than PcdMaximumAsciiStringLength ASCII characters not including the
+  Null-terminator, then ASSERT().
+
+  @param  StartOfBuffer   A pointer to the output buffer for the produced Null-terminated 
+                          ASCII string.
+  @param  BufferSize      The size, in bytes, of the output buffer specified by StartOfBuffer.
+  @param  FormatString    Null-terminated Unicode format string.
+  @param  ...             Variable argument list whose contents are accessed based on the 
+                          format string specified by FormatString.
+
+  @return The number of ASCII characters in the produced output buffer not including the
+          Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiSPrintUnicodeFormat (
+  OUT CHAR8         *StartOfBuffer,
+  IN  UINTN         BufferSize,
+  IN  CONST CHAR16  *FormatString,
+  ...
+  )
+{
+  VA_LIST Marker;
+  UINTN   NumberOfPrinted;
+
+  VA_START (Marker, FormatString);
+  NumberOfPrinted = AsciiVSPrintUnicodeFormat (StartOfBuffer, BufferSize, FormatString, Marker);
+  VA_END (Marker);
+  return NumberOfPrinted;
+}
+
+
+/**
+  Converts a decimal value to a Null-terminated ASCII string.
+  
+  Converts the decimal number specified by Value to a Null-terminated ASCII string 
+  specified by Buffer containing at most Width characters. No padding of spaces 
+  is ever performed.
+  If Width is 0 then a width of  MAXIMUM_VALUE_CHARACTERS is assumed.
+  The number of ASCII characters in Buffer is returned not including the Null-terminator.
+  If the conversion contains more than Width characters, then only the first Width
+  characters are returned, and the total number of characters required to perform
+  the conversion is returned.
+  Additional conversion parameters are specified in Flags.  
+  The Flags bit LEFT_JUSTIFY is always ignored.
+  All conversions are left justified in Buffer.
+  If Width is 0, PREFIX_ZERO is ignored in Flags.
+  If COMMA_TYPE is set in Flags, then PREFIX_ZERO is ignored in Flags, and commas
+  are inserted every 3rd digit starting from the right.
+  If RADIX_HEX is set in Flags, then the output buffer will be 
+  formatted in hexadecimal format.
+  If Value is < 0 and RADIX_HEX is not set in Flags, then the fist character in Buffer is a '-'.
+  If PREFIX_ZERO is set in Flags and PREFIX_ZERO is not being ignored, 
+  then Buffer is padded with '0' characters so the combination of the optional '-' 
+  sign character, '0' characters, digit characters for Value, and the Null-terminator
+  add up to Width characters.
+  
+  If Buffer is NULL, then ASSERT().
+  If unsupported bits are set in Flags, then ASSERT().
+  If both COMMA_TYPE and RADIX_HEX are set in Flags, then ASSERT().
+  If Width >= MAXIMUM_VALUE_CHARACTERS, then ASSERT()
+
+  @param  Buffer  Pointer to the output buffer for the produced Null-terminated
+                  ASCII string.
+  @param  Flags   The bitmask of flags that specify left justification, zero pad, and commas.
+  @param  Value   The 64-bit signed value to convert to a string.
+  @param  Width   The maximum number of ASCII characters to place in Buffer, not including
+                  the Null-terminator.
+  
+  @return The number of ASCII characters in Buffer not including the Null-terminator.
+
+**/
+UINTN
+EFIAPI
+AsciiValueToString (
+  OUT CHAR8      *Buffer,
+  IN  UINTN      Flags,
+  IN  INT64      Value,
+  IN  UINTN      Width
+  )
+{
+  return mPrint2Protocol->AsciiValueToString (Buffer, Flags, Value, Width);
+}
+
+#define PREFIX_SIGN           BIT1
+#define PREFIX_BLANK          BIT2
+#define LONG_TYPE             BIT4
+#define OUTPUT_UNICODE        BIT6
+#define FORMAT_UNICODE        BIT8
+#define PAD_TO_WIDTH          BIT9
+#define ARGUMENT_UNICODE      BIT10
+#define PRECISION             BIT11
+#define ARGUMENT_REVERSED     BIT12
+#define COUNT_ONLY_NO_PRINT   BIT13
+#define UNSIGNED_TYPE         BIT14
+
+//
+// Record date and time information
+//
+typedef struct {
+  UINT16  Year;
+  UINT8   Month;
+  UINT8   Day;
+  UINT8   Hour;
+  UINT8   Minute;
+  UINT8   Second;
+  UINT8   Pad1;
+  UINT32  Nanosecond;
+  INT16   TimeZone;
+  UINT8   Daylight;
+  UINT8   Pad2;
+} TIME;
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 mHexStr[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
+
+/**
+  Internal function that convert a number to a string in Buffer.
+
+  Print worker function that converts a decimal or hexadecimal number to an ASCII string in Buffer.
+
+  @param  Buffer    Location to place the ASCII string of Value.
+  @param  Value     The value to convert to a Decimal or Hexadecimal string in Buffer.
+  @param  Radix     Radix of the value
+
+  @return A pointer to the end of buffer filled with ASCII string.
+
+**/
+CHAR8 *
+InternalPrintLibValueToString (
+  IN OUT CHAR8  *Buffer, 
+  IN INT64      Value, 
+  IN UINTN      Radix
+  )
+{
+  UINT32  Remainder;
+
+  //
+  // Loop to convert one digit at a time in reverse order
+  //
+  *Buffer = 0;
+  do {
+    Value = (INT64)DivU64x32Remainder ((UINT64)Value, (UINT32)Radix, &Remainder);
+    *(++Buffer) = mHexStr[Remainder];
+  } while (Value != 0);
+
+  //
+  // Return pointer of the end of filled buffer.
+  //
+  return Buffer;
+}
+
+/**
+  Worker function that produces a Null-terminated string in an output buffer 
+  based on a Null-terminated format string and a VA_LIST argument list.
+
+  VSPrint function to process format and place the results in Buffer. Since a 
+  VA_LIST is used this routine allows the nesting of Vararg routines. Thus 
+  this is the main print working routine.
+
+  If COUNT_ONLY_NO_PRINT is set in Flags, Buffer will not be modified at all.
+
+  @param[out] Buffer          The character buffer to print the results of the 
+                              parsing of Format into.
+  @param[in]  BufferSize      The maximum number of characters to put into 
+                              buffer.
+  @param[in]  Flags           Initial flags value.
+                              Can only have FORMAT_UNICODE, OUTPUT_UNICODE, 
+                              and COUNT_ONLY_NO_PRINT set.
+  @param[in]  Format          A Null-terminated format string.
+  @param[in]  VaListMarker    VA_LIST style variable argument list consumed by
+                              processing Format.
+  @param[in]  BaseListMarker  BASE_LIST style variable argument list consumed
+                              by processing Format.
+
+  @return The number of characters printed not including the Null-terminator.
+          If COUNT_ONLY_NO_PRINT was set returns the same, but without any
+          modification to Buffer.
+
+**/
+UINTN
+InternalPrintLibSPrintMarker (
+  OUT CHAR8        *Buffer,
+  IN  UINTN        BufferSize,
+  IN  UINTN        Flags,
+  IN  CONST CHAR8  *Format,
+  IN  VA_LIST      VaListMarker,   OPTIONAL
+  IN  BASE_LIST    BaseListMarker  OPTIONAL
+  );
+
+/**
+  Worker function that produces a Null-terminated string in an output buffer 
+  based on a Null-terminated format string and variable argument list.
+
+  VSPrint function to process format and place the results in Buffer. Since a 
+  VA_LIST is used this routine allows the nesting of Vararg routines. Thus 
+  this is the main print working routine
+
+  @param  StartOfBuffer The character buffer to print the results of the parsing
+                        of Format into.
+  @param  BufferSize    The maximum number of characters to put into buffer.
+                        Zero means no limit.
+  @param  Flags         Initial flags value.
+                        Can only have FORMAT_UNICODE and OUTPUT_UNICODE set
+  @param  FormatString  A Null-terminated format string.
+  @param  ...           The variable argument list.
+
+  @return The number of characters printed.
+
+**/
+UINTN
+EFIAPI
+InternalPrintLibSPrint (
+  OUT CHAR8        *StartOfBuffer,
+  IN  UINTN        BufferSize,
+  IN  UINTN        Flags,
+  IN  CONST CHAR8  *FormatString,
+  ...
+  )
+{
+  VA_LIST  Marker;
+  UINTN    NumberOfPrinted;
+
+  VA_START (Marker, FormatString);
+  NumberOfPrinted = InternalPrintLibSPrintMarker (StartOfBuffer, BufferSize, Flags, FormatString, Marker, NULL);
+  VA_END (Marker);
+  return NumberOfPrinted;
+}
+
+#define WARNING_STATUS_NUMBER         5
+#define ERROR_STATUS_NUMBER           33
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 * CONST mStatusString[] = {
+  "Success",                      //  RETURN_SUCCESS                = 0
+  "Warning Unknown Glyph",        //  RETURN_WARN_UNKNOWN_GLYPH     = 1
+  "Warning Delete Failure",       //  RETURN_WARN_DELETE_FAILURE    = 2
+  "Warning Write Failure",        //  RETURN_WARN_WRITE_FAILURE     = 3
+  "Warning Buffer Too Small",     //  RETURN_WARN_BUFFER_TOO_SMALL  = 4
+  "Warning Stale Data",           //  RETURN_WARN_STALE_DATA        = 5
+  "Load Error",                   //  RETURN_LOAD_ERROR             = 1  | MAX_BIT
+  "Invalid Parameter",            //  RETURN_INVALID_PARAMETER      = 2  | MAX_BIT
+  "Unsupported",                  //  RETURN_UNSUPPORTED            = 3  | MAX_BIT
+  "Bad Buffer Size",              //  RETURN_BAD_BUFFER_SIZE        = 4  | MAX_BIT
+  "Buffer Too Small",             //  RETURN_BUFFER_TOO_SMALL,      = 5  | MAX_BIT
+  "Not Ready",                    //  RETURN_NOT_READY              = 6  | MAX_BIT
+  "Device Error",                 //  RETURN_DEVICE_ERROR           = 7  | MAX_BIT
+  "Write Protected",              //  RETURN_WRITE_PROTECTED        = 8  | MAX_BIT
+  "Out of Resources",             //  RETURN_OUT_OF_RESOURCES       = 9  | MAX_BIT
+  "Volume Corrupt",               //  RETURN_VOLUME_CORRUPTED       = 10 | MAX_BIT
+  "Volume Full",                  //  RETURN_VOLUME_FULL            = 11 | MAX_BIT
+  "No Media",                     //  RETURN_NO_MEDIA               = 12 | MAX_BIT
+  "Media changed",                //  RETURN_MEDIA_CHANGED          = 13 | MAX_BIT
+  "Not Found",                    //  RETURN_NOT_FOUND              = 14 | MAX_BIT
+  "Access Denied",                //  RETURN_ACCESS_DENIED          = 15 | MAX_BIT
+  "No Response",                  //  RETURN_NO_RESPONSE            = 16 | MAX_BIT
+  "No mapping",                   //  RETURN_NO_MAPPING             = 17 | MAX_BIT
+  "Time out",                     //  RETURN_TIMEOUT                = 18 | MAX_BIT
+  "Not started",                  //  RETURN_NOT_STARTED            = 19 | MAX_BIT
+  "Already started",              //  RETURN_ALREADY_STARTED        = 20 | MAX_BIT
+  "Aborted",                      //  RETURN_ABORTED                = 21 | MAX_BIT
+  "ICMP Error",                   //  RETURN_ICMP_ERROR             = 22 | MAX_BIT
+  "TFTP Error",                   //  RETURN_TFTP_ERROR             = 23 | MAX_BIT
+  "Protocol Error",               //  RETURN_PROTOCOL_ERROR         = 24 | MAX_BIT
+  "Incompatible Version",         //  RETURN_INCOMPATIBLE_VERSION   = 25 | MAX_BIT
+  "Security Violation",           //  RETURN_SECURITY_VIOLATION     = 26 | MAX_BIT
+  "CRC Error",                    //  RETURN_CRC_ERROR              = 27 | MAX_BIT
+  "End of Media",                 //  RETURN_END_OF_MEDIA           = 28 | MAX_BIT
+  "Reserved (29)",                //  RESERVED                      = 29 | MAX_BIT
+  "Reserved (30)",                //  RESERVED                      = 30 | MAX_BIT
+  "End of File",                  //  RETURN_END_OF_FILE            = 31 | MAX_BIT
+  "Invalid Language",             //  RETURN_INVALID_LANGUAGE       = 32 | MAX_BIT
+  "Compromised Data"              //  RETURN_COMPROMISED_DATA       = 33 | MAX_BIT
+};
+
+/**
+  Internal function that places the character into the Buffer.
+
+  Internal function that places ASCII or Unicode character into the Buffer.
+
+  @param  Buffer      The buffer to place the Unicode or ASCII string.
+  @param  EndBuffer   The end of the input Buffer. No characters will be
+                      placed after that. 
+  @param  Length      The count of character to be placed into Buffer.
+                      (Negative value indicates no buffer fill.)
+  @param  Character   The character to be placed into Buffer.
+  @param  Increment   The character increment in Buffer.
+
+  @return Buffer.
+
+**/
+CHAR8 *
+InternalPrintLibFillBuffer (
+  OUT CHAR8   *Buffer,
+  IN  CHAR8   *EndBuffer,
+  IN  INTN    Length,
+  IN  UINTN   Character,
+  IN  INTN    Increment
+  )
+{
+  INTN  Index;
+  
+  for (Index = 0; Index < Length && Buffer < EndBuffer; Index++) {
+    *Buffer = (CHAR8) Character;
+    if (Increment != 1) {
+      *(Buffer + 1) = (CHAR8)(Character >> 8);
+    }
+    Buffer += Increment;
+  }
+
+  return Buffer;
+}
+
+/**
+  Worker function that produces a Null-terminated string in an output buffer 
+  based on a Null-terminated format string and a VA_LIST argument list.
+
+  VSPrint function to process format and place the results in Buffer. Since a 
+  VA_LIST is used this routine allows the nesting of Vararg routines. Thus 
+  this is the main print working routine.
+
+  If COUNT_ONLY_NO_PRINT is set in Flags, Buffer will not be modified at all.
+
+  @param[out] Buffer          The character buffer to print the results of the 
+                              parsing of Format into.
+  @param[in]  BufferSize      The maximum number of characters to put into 
+                              buffer.
+  @param[in]  Flags           Initial flags value.
+                              Can only have FORMAT_UNICODE, OUTPUT_UNICODE, 
+                              and COUNT_ONLY_NO_PRINT set.
+  @param[in]  Format          A Null-terminated format string.
+  @param[in]  VaListMarker    VA_LIST style variable argument list consumed by
+                              processing Format.
+  @param[in]  BaseListMarker  BASE_LIST style variable argument list consumed
+                              by processing Format.
+
+  @return The number of characters printed not including the Null-terminator.
+          If COUNT_ONLY_NO_PRINT was set returns the same, but without any
+          modification to Buffer.
+
+**/
+UINTN
+InternalPrintLibSPrintMarker (
+  OUT CHAR8        *Buffer,
+  IN  UINTN        BufferSize,
+  IN  UINTN        Flags,
+  IN  CONST CHAR8  *Format,
+  IN  VA_LIST      VaListMarker,   OPTIONAL
+  IN  BASE_LIST    BaseListMarker  OPTIONAL
+  )
+{
+  CHAR8             *OriginalBuffer;
+  CHAR8             *EndBuffer;
+  CHAR8             ValueBuffer[MAXIMUM_VALUE_CHARACTERS];
+  UINT32            BytesPerOutputCharacter;
+  UINTN             BytesPerFormatCharacter;
+  UINTN             FormatMask;
+  UINTN             FormatCharacter;
+  UINTN             Width;
+  UINTN             Precision;
+  INT64             Value;
+  CONST CHAR8       *ArgumentString;
+  UINTN             Character;
+  GUID              *TmpGuid;
+  TIME              *TmpTime;
+  UINTN             Count;
+  UINTN             ArgumentMask;
+  INTN              BytesPerArgumentCharacter;
+  UINTN             ArgumentCharacter;
+  BOOLEAN           Done;
+  UINTN             Index;
+  CHAR8             Prefix;
+  BOOLEAN           ZeroPad;
+  BOOLEAN           Comma;
+  UINTN             Digits;
+  UINTN             Radix;
+  RETURN_STATUS     Status;
+  UINT32            GuidData1;
+  UINT16            GuidData2;
+  UINT16            GuidData3;
+  UINTN             LengthToReturn;
+
+  //
+  // If you change this code be sure to match the 2 versions of this function.
+  // Nearly identical logic is found in the BasePrintLib and 
+  // DxePrintLibPrint2Protocol (both PrintLib instances).
+  //
+
+  if ((Flags & COUNT_ONLY_NO_PRINT) != 0) {
+    if (BufferSize == 0) {
+      Buffer = NULL;
+    }
+  } else {
+    //
+    // We can run without a Buffer for counting only.
+    //
+    if (BufferSize == 0) {
+      return 0;
+    }
+    ASSERT (Buffer != NULL);
+  }
+
+  if ((Flags & OUTPUT_UNICODE) != 0) {
+    BytesPerOutputCharacter = 2;
+  } else {
+    BytesPerOutputCharacter = 1;
+  }
+
+  LengthToReturn = 0;
+  EndBuffer = NULL;
+  OriginalBuffer = NULL;
+
+  //
+  // Reserve space for the Null terminator.
+  //
+  if (Buffer != NULL) {
+    BufferSize--;
+    OriginalBuffer = Buffer;
+
+    //
+    // Set the tag for the end of the input Buffer.
+    //
+    EndBuffer = Buffer + BufferSize * BytesPerOutputCharacter;
+  }
+
+  if ((Flags & FORMAT_UNICODE) != 0) {
+    //
+    // Make sure format string cannot contain more than PcdMaximumUnicodeStringLength
+    // Unicode characters if PcdMaximumUnicodeStringLength is not zero. 
+    //
+    ASSERT (StrSize ((CHAR16 *) Format) != 0);
+    BytesPerFormatCharacter = 2;
+    FormatMask = 0xffff;
+  } else {
+    //
+    // Make sure format string cannot contain more than PcdMaximumAsciiStringLength
+    // Ascii characters if PcdMaximumAsciiStringLength is not zero. 
+    //
+    ASSERT (AsciiStrSize (Format) != 0);
+    BytesPerFormatCharacter = 1;
+    FormatMask = 0xff;
+  }
+
+  //
+  // Get the first character from the format string
+  //
+  FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+
+  //
+  // Loop until the end of the format string is reached or the output buffer is full
+  //
+  while (FormatCharacter != 0) {
+    if ((Buffer != NULL) && (Buffer >= EndBuffer)) {
+      break;
+    }
+    //
+    // Clear all the flag bits except those that may have been passed in
+    //
+    Flags &= (UINTN) (OUTPUT_UNICODE | FORMAT_UNICODE | COUNT_ONLY_NO_PRINT);
+
+    //
+    // Set the default width to zero, and the default precision to 1
+    //
+    Width     = 0;
+    Precision = 1;
+    Prefix    = 0;
+    Comma     = FALSE;
+    ZeroPad   = FALSE;
+    Count     = 0;
+    Digits    = 0;
+
+    switch (FormatCharacter) {
+    case '%':
+      //
+      // Parse Flags and Width
+      //
+      for (Done = FALSE; !Done; ) {
+        Format += BytesPerFormatCharacter;
+        FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+        switch (FormatCharacter) {
+        case '.': 
+          Flags |= PRECISION; 
+          break;
+        case '-': 
+          Flags |= LEFT_JUSTIFY; 
+          break;
+        case '+': 
+          Flags |= PREFIX_SIGN;  
+          break;
+        case ' ': 
+          Flags |= PREFIX_BLANK; 
+          break;
+        case ',': 
+          Flags |= COMMA_TYPE; 
+          break;
+        case 'L':
+        case 'l': 
+          Flags |= LONG_TYPE;    
+          break;
+        case '*':
+          if ((Flags & PRECISION) == 0) {
+            Flags |= PAD_TO_WIDTH;
+            if (BaseListMarker == NULL) {
+              Width = VA_ARG (VaListMarker, UINTN);
+            } else {
+              Width = BASE_ARG (BaseListMarker, UINTN);
+            }
+          } else {
+            if (BaseListMarker == NULL) {
+              Precision = VA_ARG (VaListMarker, UINTN);
+            } else {
+              Precision = BASE_ARG (BaseListMarker, UINTN);
+            }
+          }
+          break;
+        case '0':
+          if ((Flags & PRECISION) == 0) {
+            Flags |= PREFIX_ZERO;
+          }
+        case '1':
+        case '2':
+        case '3':
+        case '4':
+        case '5':
+        case '6':
+        case '7':
+        case '8':
+        case '9':
+          for (Count = 0; ((FormatCharacter >= '0') &&  (FormatCharacter <= '9')); ){
+            Count = (Count * 10) + FormatCharacter - '0';
+            Format += BytesPerFormatCharacter;
+            FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+          }
+          Format -= BytesPerFormatCharacter;
+          if ((Flags & PRECISION) == 0) {
+            Flags |= PAD_TO_WIDTH;
+            Width = Count;
+          } else {
+            Precision = Count;
+          }
+          break;
+       
+        case '\0':
+          //
+          // Make no output if Format string terminates unexpectedly when
+          // looking up for flag, width, precision and type. 
+          //
+          Format   -= BytesPerFormatCharacter;
+          Precision = 0;
+          //
+          // break skipped on purpose.
+          //
+        default:
+          Done = TRUE;
+          break;
+        }
+      } 
+
+      //
+      // Handle each argument type
+      //
+      switch (FormatCharacter) {
+      case 'p':
+        //
+        // Flag space, +, 0, L & l are invalid for type p.
+        //
+        Flags &= ~((UINTN) (PREFIX_BLANK | PREFIX_SIGN | PREFIX_ZERO | LONG_TYPE));
+        if (sizeof (VOID *) > 4) {
+          Flags |= LONG_TYPE;
+        }
+        //
+        // break skipped on purpose
+        //
+      case 'X':
+        Flags |= PREFIX_ZERO;
+        //
+        // break skipped on purpose
+        //
+      case 'x':
+        Flags |= RADIX_HEX;
+        //
+        // break skipped on purpose
+        //
+      case 'u':
+        if ((Flags & RADIX_HEX) == 0) {
+          Flags &= ~((UINTN) (PREFIX_SIGN));
+          Flags |= UNSIGNED_TYPE;
+        }
+        //
+        // break skipped on purpose
+        //
+      case 'd':
+        if ((Flags & LONG_TYPE) == 0) {
+          //
+          // 'd', 'u', 'x', and 'X' that are not preceded by 'l' or 'L' are assumed to be type "int".
+          // This assumption is made so the format string definition is compatible with the ANSI C
+          // Specification for formatted strings.  It is recommended that the Base Types be used 
+          // everywhere, but in this one case, compliance with ANSI C is more important, and 
+          // provides an implementation that is compatible with that largest possible set of CPU 
+          // architectures.  This is why the type "int" is used in this one case.
+          //
+          if (BaseListMarker == NULL) {
+            Value = VA_ARG (VaListMarker, int);
+          } else {
+            Value = BASE_ARG (BaseListMarker, int);
+          }
+        } else {
+          if (BaseListMarker == NULL) {
+            Value = VA_ARG (VaListMarker, INT64);
+          } else {
+            Value = BASE_ARG (BaseListMarker, INT64);
+          }
+        }
+        if ((Flags & PREFIX_BLANK) != 0) {
+          Prefix = ' ';
+        }
+        if ((Flags & PREFIX_SIGN) != 0) {
+          Prefix = '+';
+        }
+        if ((Flags & COMMA_TYPE) != 0) {
+          Comma = TRUE;
+        }
+        if ((Flags & RADIX_HEX) == 0) {
+          Radix = 10;
+          if (Comma) {
+            Flags &= ~((UINTN) PREFIX_ZERO);
+            Precision = 1;
+          }
+          if (Value < 0 && (Flags & UNSIGNED_TYPE) == 0) {
+            Flags |= PREFIX_SIGN;
+            Prefix = '-';
+            Value = -Value;
+          } else if ((Flags & UNSIGNED_TYPE) != 0 && (Flags & LONG_TYPE) == 0) {
+            //
+            // 'd', 'u', 'x', and 'X' that are not preceded by 'l' or 'L' are assumed to be type "int".
+            // This assumption is made so the format string definition is compatible with the ANSI C
+            // Specification for formatted strings.  It is recommended that the Base Types be used 
+            // everywhere, but in this one case, compliance with ANSI C is more important, and 
+            // provides an implementation that is compatible with that largest possible set of CPU 
+            // architectures.  This is why the type "unsigned int" is used in this one case.
+            //
+            Value = (unsigned int)Value;
+          }
+        } else {
+          Radix = 16;
+          Comma = FALSE;
+          if ((Flags & LONG_TYPE) == 0 && Value < 0) {
+            //
+            // 'd', 'u', 'x', and 'X' that are not preceded by 'l' or 'L' are assumed to be type "int".
+            // This assumption is made so the format string definition is compatible with the ANSI C
+            // Specification for formatted strings.  It is recommended that the Base Types be used 
+            // everywhere, but in this one case, compliance with ANSI C is more important, and 
+            // provides an implementation that is compatible with that largest possible set of CPU 
+            // architectures.  This is why the type "unsigned int" is used in this one case.
+            //
+            Value = (unsigned int)Value;
+          }
+        }
+        //
+        // Convert Value to a reversed string
+        //
+        Count = InternalPrintLibValueToString (ValueBuffer, Value, Radix) - ValueBuffer;
+        if (Value == 0 && Precision == 0) {
+          Count = 0;
+        }
+        ArgumentString = (CHAR8 *)ValueBuffer + Count;
+        
+        Digits = Count % 3;
+        if (Digits != 0) {
+          Digits = 3 - Digits;
+        }
+        if (Comma && Count != 0) {
+          Count += ((Count - 1) / 3);
+        }
+        if (Prefix != 0) {
+          Count++;
+          Precision++;
+        }
+        Flags |= ARGUMENT_REVERSED;
+        ZeroPad = TRUE;
+        if ((Flags & PREFIX_ZERO) != 0) {
+          if ((Flags & LEFT_JUSTIFY) == 0) {
+            if ((Flags & PAD_TO_WIDTH) != 0) {
+              if ((Flags & PRECISION) == 0) {
+                Precision = Width;
+              }
+            }
+          }
+        }
+        break;
+
+      case 's':
+      case 'S':
+        Flags |= ARGUMENT_UNICODE;
+        //
+        // break skipped on purpose
+        //
+      case 'a':
+        if (BaseListMarker == NULL) {
+          ArgumentString = VA_ARG (VaListMarker, CHAR8 *);
+        } else {
+          ArgumentString = BASE_ARG (BaseListMarker, CHAR8 *);
+        }
+        if (ArgumentString == NULL) {
+          Flags &= (~(UINTN)ARGUMENT_UNICODE);
+          ArgumentString = "<null string>";
+        }
+        //
+        // Set the default precision for string to be zero if not specified.
+        //
+        if ((Flags & PRECISION) == 0) {
+          Precision = 0;
+        }
+        break;
+
+      case 'c':
+        if (BaseListMarker == NULL) {
+          Character = VA_ARG (VaListMarker, UINTN) & 0xffff;
+        } else {
+          Character = BASE_ARG (BaseListMarker, UINTN) & 0xffff;
+        }
+        ArgumentString = (CHAR8 *)&Character;
+        Flags |= ARGUMENT_UNICODE;
+        break;
+
+      case 'g':
+        if (BaseListMarker == NULL) {
+          TmpGuid = VA_ARG (VaListMarker, GUID *);
+        } else {
+          TmpGuid = BASE_ARG (BaseListMarker, GUID *);
+        }
+        if (TmpGuid == NULL) {
+          ArgumentString = "<null guid>";
+        } else {
+          GuidData1 = ReadUnaligned32 (&(TmpGuid->Data1));
+          GuidData2 = ReadUnaligned16 (&(TmpGuid->Data2));
+          GuidData3 = ReadUnaligned16 (&(TmpGuid->Data3));
+          InternalPrintLibSPrint (
+            ValueBuffer,
+            MAXIMUM_VALUE_CHARACTERS, 
+            0,
+            "%08x-%04x-%04x-%02x%02x-%02x%02x%02x%02x%02x%02x",
+            GuidData1,
+            GuidData2,
+            GuidData3,
+            TmpGuid->Data4[0],
+            TmpGuid->Data4[1],
+            TmpGuid->Data4[2],
+            TmpGuid->Data4[3],
+            TmpGuid->Data4[4],
+            TmpGuid->Data4[5],
+            TmpGuid->Data4[6],
+            TmpGuid->Data4[7]
+            );
+          ArgumentString = ValueBuffer;
+        }
+        break;
+
+      case 't':
+        if (BaseListMarker == NULL) {
+          TmpTime = VA_ARG (VaListMarker, TIME *); 
+        } else {
+          TmpTime = BASE_ARG (BaseListMarker, TIME *); 
+        }
+        if (TmpTime == NULL) {
+          ArgumentString = "<null time>";
+        } else {
+          InternalPrintLibSPrint (
+            ValueBuffer,
+            MAXIMUM_VALUE_CHARACTERS,
+            0,
+            "%02d/%02d/%04d  %02d:%02d",
+            TmpTime->Month,
+            TmpTime->Day,
+            TmpTime->Year,
+            TmpTime->Hour,
+            TmpTime->Minute
+            );
+          ArgumentString = ValueBuffer;
+        }
+        break;
+
+      case 'r':
+        if (BaseListMarker == NULL) {
+          Status = VA_ARG (VaListMarker, RETURN_STATUS);
+        } else {
+          Status = BASE_ARG (BaseListMarker, RETURN_STATUS);
+        }
+        ArgumentString = ValueBuffer;
+        if (RETURN_ERROR (Status)) {
+          //
+          // Clear error bit
+          //
+          Index = Status & ~MAX_BIT;
+          if (Index > 0 && Index <= ERROR_STATUS_NUMBER) {
+            ArgumentString = mStatusString [Index + WARNING_STATUS_NUMBER];
+          }
+        } else {
+          Index = Status;
+          if (Index <= WARNING_STATUS_NUMBER) {
+            ArgumentString = mStatusString [Index];
+          }
+        }
+        if (ArgumentString == ValueBuffer) {
+          InternalPrintLibSPrint ((CHAR8 *) ValueBuffer, MAXIMUM_VALUE_CHARACTERS, 0, "%08X", Status);
+        }
+        break;
+
+      case '\r':
+        Format += BytesPerFormatCharacter;
+        FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+        if (FormatCharacter == '\n') {
+          //
+          // Translate '\r\n' to '\r\n'
+          //
+          ArgumentString = "\r\n";
+        } else {
+          //
+          // Translate '\r' to '\r'
+          //
+          ArgumentString = "\r";
+          Format   -= BytesPerFormatCharacter;
+        }
+        break;
+
+      case '\n':
+        //
+        // Translate '\n' to '\r\n' and '\n\r' to '\r\n'
+        //
+        ArgumentString = "\r\n";
+        Format += BytesPerFormatCharacter;
+        FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+        if (FormatCharacter != '\r') {
+          Format   -= BytesPerFormatCharacter;
+        }
+        break;
+
+      case '%':
+      default:
+        //
+        // if the type is '%' or unknown, then print it to the screen
+        //
+        ArgumentString = (CHAR8 *)&FormatCharacter;
+        Flags |= ARGUMENT_UNICODE;
+        break;
+      }
+      break;
+ 
+    case '\r':
+      Format += BytesPerFormatCharacter;
+      FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+      if (FormatCharacter == '\n') {
+        //
+        // Translate '\r\n' to '\r\n'
+        //
+        ArgumentString = "\r\n";
+      } else {
+        //
+        // Translate '\r' to '\r'
+        //
+        ArgumentString = "\r";
+        Format   -= BytesPerFormatCharacter;
+      }
+      break;
+
+    case '\n':
+      //
+      // Translate '\n' to '\r\n' and '\n\r' to '\r\n'
+      //
+      ArgumentString = "\r\n";
+      Format += BytesPerFormatCharacter;
+      FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+      if (FormatCharacter != '\r') {
+        Format   -= BytesPerFormatCharacter;
+      }
+      break;
+
+    default:
+      ArgumentString = (CHAR8 *)&FormatCharacter;
+      Flags |= ARGUMENT_UNICODE;
+      break;
+    }
+
+    //
+    // Retrieve the ArgumentString attriubutes
+    //
+    if ((Flags & ARGUMENT_UNICODE) != 0) {
+      ArgumentMask = 0xffff;
+      BytesPerArgumentCharacter = 2;
+    } else {
+      ArgumentMask = 0xff;
+      BytesPerArgumentCharacter = 1;
+    }
+    if ((Flags & ARGUMENT_REVERSED) != 0) {
+      BytesPerArgumentCharacter = -BytesPerArgumentCharacter;
+    } else {
+      //
+      // Compute the number of characters in ArgumentString and store it in Count
+      // ArgumentString is either null-terminated, or it contains Precision characters
+      //
+      for (Count = 0; Count < Precision || ((Flags & PRECISION) == 0); Count++) {
+        ArgumentCharacter = ((ArgumentString[Count * BytesPerArgumentCharacter] & 0xff) | ((ArgumentString[Count * BytesPerArgumentCharacter + 1]) << 8)) & ArgumentMask;
+        if (ArgumentCharacter == 0) {
+          break;
+        }
+      }
+    }
+
+    if (Precision < Count) {
+      Precision = Count;
+    }
+
+    //
+    // Pad before the string
+    //
+    if ((Flags & (PAD_TO_WIDTH | LEFT_JUSTIFY)) == (PAD_TO_WIDTH)) {
+      LengthToReturn += ((Width - Precision) * BytesPerOutputCharacter);
+      if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+        Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, Width - Precision, ' ', BytesPerOutputCharacter);
+      }
+    }
+
+    if (ZeroPad) {
+      if (Prefix != 0) {
+        LengthToReturn += (1 * BytesPerOutputCharacter);
+        if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+          Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, 1, Prefix, BytesPerOutputCharacter);
+        }
+      }
+      LengthToReturn += ((Precision - Count) * BytesPerOutputCharacter);
+      if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+        Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, Precision - Count, '0', BytesPerOutputCharacter);
+      }
+    } else {
+      LengthToReturn += ((Precision - Count) * BytesPerOutputCharacter);
+      if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+        Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, Precision - Count, ' ', BytesPerOutputCharacter);
+      }
+      if (Prefix != 0) {
+        LengthToReturn += (1 * BytesPerOutputCharacter);
+        if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+          Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, 1, Prefix, BytesPerOutputCharacter);
+        }
+      }
+    }
+
+    //
+    // Output the Prefix character if it is present
+    //
+    Index = 0;
+    if (Prefix != 0) {
+      Index++;
+    }
+
+    //
+    // Copy the string into the output buffer performing the required type conversions
+    //
+    while (Index < Count) {
+      ArgumentCharacter = ((*ArgumentString & 0xff) | (*(ArgumentString + 1) << 8)) & ArgumentMask;
+
+      LengthToReturn += (1 * BytesPerOutputCharacter);
+      if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+        Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, 1, ArgumentCharacter, BytesPerOutputCharacter);
+      }
+      ArgumentString    += BytesPerArgumentCharacter;
+      Index++;
+      if (Comma) {
+        Digits++;
+        if (Digits == 3) {
+          Digits = 0;
+          Index++;
+          if (Index < Count) {
+            LengthToReturn += (1 * BytesPerOutputCharacter);
+            if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+              Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, 1, ',', BytesPerOutputCharacter);
+            }
+          }
+        }
+      }
+    }
+
+    //
+    // Pad after the string
+    //
+    if ((Flags & (PAD_TO_WIDTH | LEFT_JUSTIFY)) == (PAD_TO_WIDTH | LEFT_JUSTIFY)) {
+      LengthToReturn += ((Width - Precision) * BytesPerOutputCharacter);
+      if ((Flags & COUNT_ONLY_NO_PRINT) == 0 && Buffer != NULL) {
+        Buffer = InternalPrintLibFillBuffer (Buffer, EndBuffer, Width - Precision, ' ', BytesPerOutputCharacter);
+      }
+    }
+
+    //
+    // Get the next character from the format string
+    //
+    Format += BytesPerFormatCharacter;
+
+    //
+    // Get the next character from the format string
+    //
+    FormatCharacter = ((*Format & 0xff) | (*(Format + 1) << 8)) & FormatMask;
+  }
+
+  if ((Flags & COUNT_ONLY_NO_PRINT) != 0) {
+    return (LengthToReturn / BytesPerOutputCharacter);
+  }
+
+  ASSERT (Buffer != NULL);
+  //
+  // Null terminate the Unicode or ASCII string
+  //
+  InternalPrintLibFillBuffer (Buffer, EndBuffer + BytesPerOutputCharacter, 1, 0, BytesPerOutputCharacter);
+  //
+  // Make sure output buffer cannot contain more than PcdMaximumUnicodeStringLength
+  // Unicode characters if PcdMaximumUnicodeStringLength is not zero. 
+  //
+  ASSERT ((((Flags & OUTPUT_UNICODE) == 0)) || (StrSize ((CHAR16 *) OriginalBuffer) != 0));
+  //
+  // Make sure output buffer cannot contain more than PcdMaximumAsciiStringLength
+  // ASCII characters if PcdMaximumAsciiStringLength is not zero. 
+  //
+  ASSERT ((((Flags & OUTPUT_UNICODE) != 0)) || (AsciiStrSize (OriginalBuffer) != 0));
+
+  return ((Buffer - OriginalBuffer) / BytesPerOutputCharacter);
+}
+
+/**
+  Returns the number of characters that would be produced by if the formatted 
+  output were produced not including the Null-terminator.
+
+  If FormatString is NULL, then ASSERT().
+  If FormatString is not aligned on a 16-bit boundary, then ASSERT().
+
+  @param[in]  FormatString    A Null-terminated Unicode format string.
+  @param[in]  Marker          VA_LIST marker for the variable argument list.
+
+  @return The number of characters that would be produced, not including the 
+          Null-terminator.
+**/
+UINTN
+EFIAPI
+SPrintLength (
+  IN  CONST CHAR16   *FormatString,
+  IN  VA_LIST       Marker
+  )
+{
+  ASSERT(FormatString != NULL);
+  return InternalPrintLibSPrintMarker (NULL, 0, FORMAT_UNICODE | OUTPUT_UNICODE | COUNT_ONLY_NO_PRINT, (CHAR8 *)FormatString, Marker, NULL);
+}
+
+/**
+  Returns the number of characters that would be produced by if the formatted 
+  output were produced not including the Null-terminator.
+
+  If FormatString is NULL, then ASSERT().
+
+  @param[in]  FormatString    A Null-terminated ASCII format string.
+  @param[in]  Marker          VA_LIST marker for the variable argument list.
+
+  @return The number of characters that would be produced, not including the 
+          Null-terminator.
+**/
+UINTN
+EFIAPI
+SPrintLengthAsciiFormat (
+  IN  CONST CHAR8   *FormatString,
+  IN  VA_LIST       Marker
+  )
+{
+  ASSERT(FormatString != NULL);
+  return InternalPrintLibSPrintMarker (NULL, 0, OUTPUT_UNICODE | COUNT_ONLY_NO_PRINT, (CHAR8 *)FormatString, Marker, NULL);
+}
diff --git a/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.inf b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.inf
new file mode 100644
index 0000000000..962cf8b05b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.inf
@@ -0,0 +1,57 @@
+## @file
+#  DXE report status code library.
+#
+#  Retrieve status code and report status code in DXE phase.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeReportStatusCodeLib
+  MODULE_UNI_FILE                = DxeReportStatusCodeLib.uni
+  FILE_GUID                      = EBF144C8-70F5-4e09-ADE2-F41F5C59AFDA
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ReportStatusCodeLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER SMM_CORE
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  ReportStatusCodeLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  PcdLib
+  DevicePathLib
+
+[Guids]
+  gEfiStatusCodeSpecificDataGuid                ## SOMETIMES_CONSUMES ## UNDEFINED
+  gEfiStatusCodeDataTypeDebugGuid               ## SOMETIMES_CONSUMES ## UNDEFINED
+
+[Protocols]
+  gEfiStatusCodeRuntimeProtocolGuid             ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdReportStatusCodePropertyMask  ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.uni b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.uni
new file mode 100644
index 0000000000..61da4060f5
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/ReportStatusCodeLib.c b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/ReportStatusCodeLib.c
new file mode 100644
index 0000000000..ce843cc47f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeReportStatusCodeLib/ReportStatusCodeLib.c
@@ -0,0 +1,631 @@
+/** @file
+  Report Status Code Library for DXE Phase.
+
+  Copyright (c) 2006 - 2010, 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 <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <Protocol/StatusCode.h>
+
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+
+//
+// Define the maximum extended data size that is supported when a status code is
+// reported at TPL_HIGH_LEVEL.
+//
+#define MAX_EXTENDED_DATA_SIZE  0x200
+
+EFI_STATUS_CODE_PROTOCOL  *mReportStatusCodeLibStatusCodeProtocol = NULL;
+
+/**
+  Locate the report status code service.
+
+  Retrieve ReportStatusCode() API of Report Status Code Protocol.
+
+**/
+VOID
+InternalGetReportStatusCode (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  if (mReportStatusCodeLibStatusCodeProtocol != NULL) {
+    return;
+  }
+
+  //
+  // Check gBS just in case ReportStatusCode is called before gBS is initialized.
+  //
+  if (gBS != NULL && gBS->LocateProtocol != NULL) {
+    Status = gBS->LocateProtocol (&gEfiStatusCodeRuntimeProtocolGuid, NULL, (VOID**) &mReportStatusCodeLibStatusCodeProtocol);
+    if (EFI_ERROR (Status)) {
+      mReportStatusCodeLibStatusCodeProtocol = NULL;
+    }
+  }
+}
+
+/**
+  Internal worker function that reports a status code through the Report Status Code Protocol.
+
+  If status code service is not cached, then this function checks if Report Status Code
+  Protocol is available in system.  If Report Status Code Protocol is not available, then
+  EFI_UNSUPPORTED is returned.  If Report Status Code Protocol is present, then it is
+  cached in mReportStatusCodeLibStatusCodeProtocol. Finally this function reports status
+  code through the Report Status Code Protocol.
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  This is an optional parameter that may be
+                            NULL.
+  @param  Data              Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+
+  @retval EFI_SUCCESS       The status code was reported.
+  @retval EFI_UNSUPPORTED   Report Status Code Protocol is not available.
+  @retval EFI_UNSUPPORTED   Status code type is not supported.
+
+**/
+EFI_STATUS
+InternalReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN CONST EFI_GUID           *CallerId OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data     OPTIONAL
+  )
+{
+  if ((ReportProgressCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      (ReportErrorCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) ||
+      (ReportDebugCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_DEBUG_CODE)) {
+    //
+    // If mReportStatusCodeLibStatusCodeProtocol is NULL, then check if Report Status Code Protocol is available in system.
+    //
+    InternalGetReportStatusCode ();
+    if (mReportStatusCodeLibStatusCodeProtocol == NULL) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // A Report Status Code Protocol is present in system, so pass in all the parameters to the service.
+    //
+    return mReportStatusCodeLibStatusCodeProtocol->ReportStatusCode (Type, Value, Instance, (EFI_GUID *)CallerId, Data);
+  }
+  
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Converts a status code to an 8-bit POST code value.
+
+  Converts the status code specified by CodeType and Value to an 8-bit POST code
+  and returns the 8-bit POST code in PostCode.  If CodeType is an
+  EFI_PROGRESS_CODE or CodeType is an EFI_ERROR_CODE, then bits 0..4 of PostCode
+  are set to bits 16..20 of Value, and bits 5..7 of PostCode are set to bits
+  24..26 of Value., and TRUE is returned.  Otherwise, FALSE is returned.
+
+  If PostCode is NULL, then ASSERT().
+
+  @param  CodeType  The type of status code being converted.
+  @param  Value     The status code value being converted.
+  @param  PostCode  A pointer to the 8-bit POST code value to return.
+
+  @retval  TRUE   The status code specified by CodeType and Value was converted
+                  to an 8-bit POST code and returned in  PostCode.
+  @retval  FALSE  The status code specified by CodeType and Value could not be
+                  converted to an 8-bit POST code value.
+
+**/
+BOOLEAN
+EFIAPI
+CodeTypeToPostCode (
+  IN  EFI_STATUS_CODE_TYPE   CodeType,
+  IN  EFI_STATUS_CODE_VALUE  Value,
+  OUT UINT8                  *PostCode
+  )
+{
+  //
+  // If PostCode is NULL, then ASSERT()
+  //
+  ASSERT (PostCode != NULL);
+
+  //
+  // Convert Value to an 8 bit post code
+  //
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE)       ) {
+    *PostCode  = (UINT8) ((((Value & EFI_STATUS_CODE_CLASS_MASK) >> 24) << 5) |
+                          (((Value & EFI_STATUS_CODE_SUBCLASS_MASK) >> 16) & 0x1f));
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts ASSERT() information from a status code structure.
+
+  Converts the status code specified by CodeType, Value, and Data to the ASSERT()
+  arguments specified by Filename, Description, and LineNumber.  If CodeType is
+  an EFI_ERROR_CODE, and CodeType has a severity of EFI_ERROR_UNRECOVERED, and
+  Value has an operation mask of EFI_SW_EC_ILLEGAL_SOFTWARE_STATE, extract
+  Filename, Description, and LineNumber from the optional data area of the
+  status code buffer specified by Data.  The optional data area of Data contains
+  a Null-terminated ASCII string for the FileName, followed by a Null-terminated
+  ASCII string for the Description, followed by a 32-bit LineNumber.  If the
+  ASSERT() information could be extracted from Data, then return TRUE.
+  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If Filename is NULL, then ASSERT().
+  If Description is NULL, then ASSERT().
+  If LineNumber is NULL, then ASSERT().
+
+  @param  CodeType     The type of status code being converted.
+  @param  Value        The status code value being converted.
+  @param  Data         Pointer to status code data buffer.
+  @param  Filename     Pointer to the source file name that generated the ASSERT().
+  @param  Description  Pointer to the description of the ASSERT().
+  @param  LineNumber   Pointer to source line number that generated the ASSERT().
+
+  @retval  TRUE   The status code specified by CodeType, Value, and Data was
+                  converted ASSERT() arguments specified by Filename, Description,
+                  and LineNumber.
+  @retval  FALSE  The status code specified by CodeType, Value, and Data could
+                  not be converted to ASSERT() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractAssertInfo (
+  IN EFI_STATUS_CODE_TYPE        CodeType,
+  IN EFI_STATUS_CODE_VALUE       Value,
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT CHAR8                      **Filename,
+  OUT CHAR8                      **Description,
+  OUT UINT32                     *LineNumber
+  )
+{
+  EFI_DEBUG_ASSERT_DATA  *AssertData;
+
+  ASSERT (Data        != NULL);
+  ASSERT (Filename    != NULL);
+  ASSERT (Description != NULL);
+  ASSERT (LineNumber  != NULL);
+
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK)      == EFI_ERROR_CODE) &&
+      ((CodeType & EFI_STATUS_CODE_SEVERITY_MASK)  == EFI_ERROR_UNRECOVERED) &&
+      ((Value    & EFI_STATUS_CODE_OPERATION_MASK) == EFI_SW_EC_ILLEGAL_SOFTWARE_STATE)) {
+    AssertData   = (EFI_DEBUG_ASSERT_DATA *)(Data + 1);
+    *Filename    = (CHAR8 *)(AssertData + 1);
+    *Description = *Filename + AsciiStrLen (*Filename) + 1;
+    *LineNumber  = AssertData->LineNumber;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts DEBUG() information from a status code structure.
+
+  Converts the status code specified by Data to the DEBUG() arguments specified
+  by ErrorLevel, Marker, and Format.  If type GUID in Data is
+  EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID, then extract ErrorLevel, Marker, and
+  Format from the optional data area of the status code buffer specified by Data.
+  The optional data area of Data contains a 32-bit ErrorLevel followed by Marker
+  which is 12 UINTN parameters, followed by a Null-terminated ASCII string for
+  the Format.  If the DEBUG() information could be extracted from Data, then
+  return TRUE.  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If ErrorLevel is NULL, then ASSERT().
+  If Marker is NULL, then ASSERT().
+  If Format is NULL, then ASSERT().
+
+  @param  Data        Pointer to status code data buffer.
+  @param  ErrorLevel  Pointer to error level mask for a debug message.
+  @param  Marker      Pointer to the variable argument list associated with Format.
+  @param  Format      Pointer to a Null-terminated ASCII format string of a
+                      debug message.
+
+  @retval  TRUE   The status code specified by Data was converted DEBUG() arguments
+                  specified by ErrorLevel, Marker, and Format.
+  @retval  FALSE  The status code specified by Data could not be converted to
+                  DEBUG() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractDebugInfo (
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT UINT32                     *ErrorLevel,
+  OUT BASE_LIST                  *Marker,
+  OUT CHAR8                      **Format
+  )
+{
+  EFI_DEBUG_INFO  *DebugInfo;
+
+  ASSERT (Data       != NULL);
+  ASSERT (ErrorLevel != NULL);
+  ASSERT (Marker     != NULL);
+  ASSERT (Format     != NULL);
+
+  //
+  // If the GUID type is not EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID then return FALSE
+  //
+  if (!CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeDebugGuid)) {
+    return FALSE;
+  }
+
+  //
+  // Retrieve the debug information from the status code record
+  //
+  DebugInfo = (EFI_DEBUG_INFO *)(Data + 1);
+
+  *ErrorLevel = DebugInfo->ErrorLevel;
+
+  //
+  // The first 12 * sizeof (UINT64) bytes following EFI_DEBUG_INFO are for variable arguments
+  // of format in DEBUG string. Its address is returned in Marker and has to be 64-bit aligned.
+  // It must be noticed that EFI_DEBUG_INFO follows EFI_STATUS_CODE_DATA, whose size is
+  // 20 bytes. The size of EFI_DEBUG_INFO is 4 bytes, so we can ensure that Marker
+  // returned is 64-bit aligned.
+  // 64-bit aligned is a must, otherwise retrieving 64-bit parameter from BASE_LIST will
+  // cause unalignment exception.
+  //
+  *Marker = (BASE_LIST) (DebugInfo + 1);
+  *Format = (CHAR8 *)(((UINT64 *)*Marker) + 12);
+
+  return TRUE;
+}
+
+
+/**
+  Reports a status code.
+
+  Reports the status code specified by the parameters Type and Value.  Status
+  code also require an instance, caller ID, and extended data.  This function
+  passed in a zero instance, NULL extended data, and a caller ID of
+  gEfiCallerIdGuid, which is the GUID for the module.
+
+  ReportStatusCode()must actively prevent recusrsion.  If ReportStatusCode()
+  is called while processing another any other Report Status Code Library function,
+  then ReportStatusCode() must return immediately.
+
+  @param  Type   Status code type.
+  @param  Value  Status code value.
+
+  @retval  EFI_SUCCESS       The status code was reported.
+  @retval  EFI_DEVICE_ERROR  There status code could not be reported due to a
+                             device error.
+  @retval  EFI_UNSUPPORTED   Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value
+  )
+{
+  return InternalReportStatusCode (Type, Value, 0, &gEfiCallerIdGuid, NULL);
+}
+
+
+/**
+  Reports a status code with a Device Path Protocol as the extended data.
+
+  Allocates and fills in the extended data section of a status code with the
+  Device Path Protocol specified by DevicePath.  This function is responsible
+  for allocating a buffer large enough for the standard header and the device
+  path.  The standard header is filled in with a GUID of
+  gEfiStatusCodeSpecificDataGuid.  The status code is reported with a zero
+  instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithDevicePath()must actively prevent recursion.  If
+  ReportStatusCodeWithDevicePath() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithDevicePath()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If DevicePath is NULL, then ASSERT().
+
+  @param  Type        Status code type.
+  @param  Value       Status code value.
+  @param  DevicePath  Pointer to the Device Path Protocol to be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by DevicePath.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithDevicePath (
+  IN EFI_STATUS_CODE_TYPE            Type,
+  IN EFI_STATUS_CODE_VALUE           Value,
+  IN CONST EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  )
+{
+  ASSERT (DevicePath != NULL);
+  return ReportStatusCodeWithExtendedData (
+           Type,
+           Value,
+           (VOID *)DevicePath,
+           GetDevicePathSize (DevicePath)
+           );
+}
+
+
+/**
+  Reports a status code with an extended data buffer.
+
+  Allocates and fills in the extended data section of a status code with the
+  extended data specified by ExtendedData and ExtendedDataSize.  ExtendedData
+  is assumed to be one of the data structures specified in Related Definitions.
+  These data structure do not have the standard header, so this function is
+  responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled
+  in with a GUID of  gEfiStatusCodeSpecificDataGuid.  The status code is reported
+  with a zero instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithExtendedData()must actively prevent recursion.  If
+  ReportStatusCodeWithExtendedData() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithExtendedData()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL, then ASSERT().
+  If ExtendedDataSize is 0, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  ExtendedData      Pointer to the extended data buffer to be reported.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer to
+                            be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by ExtendedData and ExtendedDataSize.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithExtendedData (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN CONST VOID             *ExtendedData,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  ASSERT (ExtendedData     != NULL);
+  ASSERT (ExtendedDataSize != 0);
+  return ReportStatusCodeEx (
+           Type,
+           Value,
+           0,
+           NULL,
+           NULL,
+           ExtendedData,
+           ExtendedDataSize
+           );
+}
+
+
+/**
+  Reports a status code with full parameters.
+
+  The function reports a status code.  If ExtendedData is NULL and ExtendedDataSize
+  is 0, then an extended data buffer is not reported.  If ExtendedData is not
+  NULL and ExtendedDataSize is not 0, then an extended data buffer is allocated.
+  ExtendedData is assumed not have the standard status code header, so this function
+  is responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled in
+  with a GUID specified by ExtendedDataGuid.  If ExtendedDataGuid is NULL, then a
+  GUID of gEfiStatusCodeSpecificDataGuid is used.  The status code is reported with
+  an instance specified by Instance and a caller ID specified by CallerId.  If
+  CallerId is NULL, then a caller ID of gEfiCallerIdGuid is used.
+
+  ReportStatusCodeEx()must actively prevent recursion. If
+  ReportStatusCodeEx() is called while processing another any
+  other Report Status Code Library function, then
+  ReportStatusCodeEx() must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL and ExtendedDataSize is not zero, then ASSERT().
+  If ExtendedData is not NULL and ExtendedDataSize is zero, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  If this parameter is NULL, then a caller
+                            ID of gEfiCallerIdGuid is used.
+  @param  ExtendedDataGuid  Pointer to the GUID for the extended data buffer.
+                            If this parameter is NULL, then a the status code
+                            standard header is filled in with
+                            gEfiStatusCodeSpecificDataGuid.
+  @param  ExtendedData      Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer.
+
+  @retval  EFI_SUCCESS           The status code was reported.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate
+                                 the extended data section if it was specified.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeEx (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN UINT32                 Instance,
+  IN CONST EFI_GUID         *CallerId          OPTIONAL,
+  IN CONST EFI_GUID         *ExtendedDataGuid  OPTIONAL,
+  IN CONST VOID             *ExtendedData      OPTIONAL,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  EFI_STATUS            Status;
+  EFI_STATUS_CODE_DATA  *StatusCodeData;
+  EFI_TPL               Tpl;
+  UINT64                Buffer[(MAX_EXTENDED_DATA_SIZE / sizeof (UINT64)) + 1];
+
+  ASSERT (!((ExtendedData == NULL) && (ExtendedDataSize != 0)));
+  ASSERT (!((ExtendedData != NULL) && (ExtendedDataSize == 0)));
+
+  if (gBS == NULL || gBS->AllocatePool == NULL || gBS->FreePool == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Retrieve the current TPL
+  //
+  Tpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  gBS->RestoreTPL (Tpl);
+  
+  StatusCodeData = NULL;
+  if (Tpl <= TPL_NOTIFY) {
+    //
+    // Allocate space for the Status Code Header and its buffer
+    //
+    gBS->AllocatePool (EfiBootServicesData, sizeof (EFI_STATUS_CODE_DATA) + ExtendedDataSize, (VOID **)&StatusCodeData);
+  }
+
+  if (StatusCodeData == NULL) {
+    //
+    // If a buffer could not be allocated, then see if the local variable Buffer can be used
+    //
+    if (ExtendedDataSize > (MAX_EXTENDED_DATA_SIZE - sizeof (EFI_STATUS_CODE_DATA))) {
+      //
+      // The local variable Buffer not large enough to hold the extended data associated
+      // with the status code being reported.
+      //
+      DEBUG ((EFI_D_ERROR, "Status code extended data is too large to be reported!\n"));
+      return EFI_OUT_OF_RESOURCES;
+    }
+    StatusCodeData = (EFI_STATUS_CODE_DATA  *)Buffer;
+  }
+
+  //
+  // Fill in the extended data header
+  //
+  StatusCodeData->HeaderSize = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+  StatusCodeData->Size = (UINT16) ExtendedDataSize;
+  if (ExtendedDataGuid == NULL) {
+    ExtendedDataGuid = &gEfiStatusCodeSpecificDataGuid;
+  }
+  CopyGuid (&StatusCodeData->Type, ExtendedDataGuid);
+
+  //
+  // Fill in the extended data buffer
+  //
+  if (ExtendedData != NULL) {
+    CopyMem (StatusCodeData + 1, ExtendedData, ExtendedDataSize);
+  }
+
+  //
+  // Report the status code
+  //
+  if (CallerId == NULL) {
+    CallerId = &gEfiCallerIdGuid;
+  }
+  Status = InternalReportStatusCode (Type, Value, Instance, CallerId, StatusCodeData);
+
+  //
+  // Free the allocated buffer
+  //
+  if (StatusCodeData != (EFI_STATUS_CODE_DATA  *)Buffer) {
+    gBS->FreePool (StatusCodeData);
+  }
+
+  return Status;
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_PROGRESS_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportProgressCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_ERROR_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportErrorCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_DEBUG_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportDebugCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.c b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.c
new file mode 100644
index 0000000000..b96d78664d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.c
@@ -0,0 +1,533 @@
+/** @file
+  Provides generic security measurement functions for DXE module.
+
+Copyright (c) 2009 - 2015, 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 <PiDxe.h>
+#include <Protocol/LoadFile.h>
+#include <Library/DebugLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/SecurityManagementLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#define SECURITY_HANDLER_TABLE_SIZE   0x10
+
+//
+// Secruity Operation on Image and none Image.
+//
+#define EFI_AUTH_IMAGE_OPERATION_MASK       (EFI_AUTH_OPERATION_VERIFY_IMAGE \
+                                              | EFI_AUTH_OPERATION_DEFER_IMAGE_LOAD \
+                                              | EFI_AUTH_OPERATION_MEASURE_IMAGE)
+#define EFI_AUTH_NONE_IMAGE_OPERATION_MASK  (EFI_AUTH_OPERATION_CONNECT_POLICY \
+                                              | EFI_AUTH_OPERATION_AUTHENTICATION_STATE)
+
+typedef struct {
+  UINT32  SecurityOperation;
+  SECURITY_FILE_AUTHENTICATION_STATE_HANDLER  SecurityHandler;
+} SECURITY_INFO;
+
+typedef struct {
+  UINT32  Security2Operation;
+  SECURITY2_FILE_AUTHENTICATION_HANDLER  Security2Handler;
+} SECURITY2_INFO;
+
+UINT32  mCurrentAuthOperation       = 0;
+UINT32  mNumberOfSecurityHandler    = 0;
+UINT32  mMaxNumberOfSecurityHandler = 0;
+SECURITY_INFO  *mSecurityTable      = NULL;
+
+UINT32  mCurrentAuthOperation2       = 0;
+UINT32  mNumberOfSecurity2Handler    = 0;
+UINT32  mMaxNumberOfSecurity2Handler = 0;
+SECURITY2_INFO *mSecurity2Table      = NULL;
+
+/**
+  Reallocates more global memory to store the registered Handler list.
+
+  @retval  RETURN_SUCCESS            Reallocate memory successfully.
+  @retval  RETURN_OUT_OF_RESOURCES   No enough memory to allocated.
+**/
+RETURN_STATUS
+EFIAPI
+ReallocateSecurityHandlerTable (
+  )
+{
+  //
+  // Reallocate memory for security info structure.
+  //
+  mSecurityTable = ReallocatePool (
+                     mMaxNumberOfSecurityHandler * sizeof (SECURITY_INFO), 
+                     (mMaxNumberOfSecurityHandler + SECURITY_HANDLER_TABLE_SIZE) * sizeof (SECURITY_INFO), 
+                     mSecurityTable
+                     );
+
+  //
+  // No enough resource is allocated.
+  //
+  if (mSecurityTable == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Increase max handler number
+  //
+  mMaxNumberOfSecurityHandler = mMaxNumberOfSecurityHandler + SECURITY_HANDLER_TABLE_SIZE;
+  return RETURN_SUCCESS;
+}
+
+/**
+ Check whether an operation is valid according to the requirement of current operation, 
+ which must make sure that the measure image operation is the last one.
+
+ @param CurrentAuthOperation  Current operation.
+ @param CheckAuthOperation    Operation to be checked.
+
+ @retval  TRUE   Operation is valid for current operation.
+ @retval  FALSE  Operation is invalid for current operation.
+**/
+BOOLEAN
+CheckAuthenticationOperation (
+  IN  UINT32    CurrentAuthOperation,
+  IN  UINT32    CheckAuthOperation
+  )
+{ 
+  //
+  // Make sure new auth operation can be recognized.
+  //
+  ASSERT ((CheckAuthOperation & ~(EFI_AUTH_IMAGE_OPERATION_MASK | EFI_AUTH_OPERATION_AUTHENTICATION_STATE | EFI_AUTH_OPERATION_IMAGE_REQUIRED)) == 0);
+  
+  //
+  // When current operation includes measure image operation, 
+  // only another measure image operation or none operation will be allowed.
+  //
+  if ((CurrentAuthOperation & EFI_AUTH_OPERATION_MEASURE_IMAGE) == EFI_AUTH_OPERATION_MEASURE_IMAGE) {
+    if (((CheckAuthOperation & EFI_AUTH_OPERATION_MEASURE_IMAGE) == EFI_AUTH_OPERATION_MEASURE_IMAGE) ||
+        ((CheckAuthOperation & EFI_AUTH_IMAGE_OPERATION_MASK) == EFI_AUTH_OPERATION_NONE)) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  }
+  
+  //
+  // When current operation doesn't include measure image operation, 
+  // any new operation will be allowed.
+  //
+  return TRUE;
+}
+
+/**
+  Register security measurement handler with its operation type. The different
+  handler with the same operation can all be registered.
+
+  If SecurityHandler is NULL, then ASSERT().
+  If no enough resources available to register new handler, then ASSERT().
+  If AuthenticationOperation is not recongnized, then ASSERT().
+  If the previous register handler can't be executed before the later register handler, then ASSERT().
+
+  @param[in]  SecurityHandler           Security measurement service handler to be registered.
+  @param[in]  AuthenticationOperation   Operation type is specified for the registered handler.
+
+  @retval EFI_SUCCESS              The handlers were registered successfully.
+**/
+EFI_STATUS
+EFIAPI
+RegisterSecurityHandler (
+  IN  SECURITY_FILE_AUTHENTICATION_STATE_HANDLER  SecurityHandler,
+  IN  UINT32                                      AuthenticationOperation
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (SecurityHandler != NULL);
+
+  //
+  // Make sure AuthenticationOperation is valid in the register order.
+  //
+  ASSERT (CheckAuthenticationOperation (mCurrentAuthOperation, AuthenticationOperation));
+  mCurrentAuthOperation = mCurrentAuthOperation | AuthenticationOperation;
+
+  //
+  // Check whether the handler lists is enough to store new handler.
+  //
+  if (mNumberOfSecurityHandler == mMaxNumberOfSecurityHandler) {
+    //
+    // Allocate more resources for new handler.
+    //
+    Status = ReallocateSecurityHandlerTable();
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Register new handler into the handler list.
+  //
+  mSecurityTable[mNumberOfSecurityHandler].SecurityOperation = AuthenticationOperation;
+  mSecurityTable[mNumberOfSecurityHandler].SecurityHandler   = SecurityHandler;
+  mNumberOfSecurityHandler ++;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute registered handlers until one returns an error and that error is returned.
+  If none of the handlers return an error, then EFI_SUCCESS is returned.
+
+  Before exectue handler, get the image buffer by file device path if a handler 
+  requires the image file. And return the image buffer to each handler when exectue handler.
+
+  The handlers are executed in same order to their registered order.
+
+  @param[in]  AuthenticationStatus 
+                           This is the authentication type returned from the Section
+                           Extraction protocol. See the Section Extraction Protocol
+                           Specification for details on this type.
+  @param[in]  FilePath     This is a pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+
+  @retval EFI_SUCCESS            The file specified by File did authenticate when more
+                                 than one security handler services were registered, 
+                                 or the file did not authenticate when no security 
+                                 handler service was registered. And the platform policy 
+                                 dictates that the DXE Core may use File.
+  @retval EFI_INVALID_PARAMETER  File is NULL.
+  @retval EFI_SECURITY_VIOLATION The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should be placed
+                                 in the untrusted state. A file may be promoted from
+                                 the untrusted to the trusted state at a future time
+                                 with a call to the Trust() DXE Service.
+  @retval EFI_ACCESS_DENIED      The file specified by File did not authenticate, and
+                                 the platform policy dictates that File should not be
+                                 used for any purpose.
+**/
+EFI_STATUS
+EFIAPI
+ExecuteSecurityHandlers (
+  IN  UINT32                            AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL    *FilePath
+  )
+{
+  UINT32        Index;
+  EFI_STATUS    Status;
+  UINT32        HandlerAuthenticationStatus;
+  VOID          *FileBuffer;
+  UINTN         FileSize;
+  EFI_HANDLE    Handle;
+  EFI_DEVICE_PATH_PROTOCOL        *Node;
+  EFI_DEVICE_PATH_PROTOCOL        *FilePathToVerfiy;
+  
+  if (FilePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Directly return successfully when no handler is registered.
+  //
+  if (mNumberOfSecurityHandler == 0) {
+    return EFI_SUCCESS;
+  }
+  
+  Status                      = EFI_SUCCESS;
+  FileBuffer                  = NULL;
+  FileSize                    = 0;
+  HandlerAuthenticationStatus = AuthenticationStatus;
+  FilePathToVerfiy            = (EFI_DEVICE_PATH_PROTOCOL *) FilePath;
+  //
+  // Run security handler in same order to their registered list
+  //
+  for (Index = 0; Index < mNumberOfSecurityHandler; Index ++) {
+    if ((mSecurityTable[Index].SecurityOperation & EFI_AUTH_OPERATION_IMAGE_REQUIRED) == EFI_AUTH_OPERATION_IMAGE_REQUIRED) {
+      //
+      // Try get file buffer when the handler requires image buffer.
+      //
+      if (FileBuffer == NULL) {
+        Node   = FilePathToVerfiy;
+        Status = gBS->LocateDevicePath (&gEfiLoadFileProtocolGuid, &Node, &Handle);
+        //
+        // Try to get image by FALSE boot policy for the exact boot file path.
+        //
+        FileBuffer = GetFileBufferByFilePath (FALSE, FilePath, &FileSize, &AuthenticationStatus);
+        if (FileBuffer == NULL) {
+          //
+          // Try to get image by TRUE boot policy for the inexact boot file path.
+          //
+          FileBuffer = GetFileBufferByFilePath (TRUE, FilePath, &FileSize, &AuthenticationStatus);
+        }
+        if ((FileBuffer != NULL) && (!EFI_ERROR (Status))) {
+          //
+          // LoadFile () may cause the device path of the Handle be updated.
+          //
+          FilePathToVerfiy = AppendDevicePath (DevicePathFromHandle (Handle), Node);
+        }
+      }
+    }
+    Status = mSecurityTable[Index].SecurityHandler (
+               HandlerAuthenticationStatus,
+               FilePathToVerfiy,
+               FileBuffer,
+               FileSize
+               );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  if (FileBuffer != NULL) {
+    FreePool (FileBuffer);
+  }
+  if (FilePathToVerfiy != FilePath) {
+    FreePool (FilePathToVerfiy);
+  }
+
+  return Status;
+}
+
+/**
+  Reallocates more global memory to store the registered Securit2Handler list.
+
+  @retval  RETURN_SUCCESS            Reallocate memory successfully.
+  @retval  RETURN_OUT_OF_RESOURCES   No enough memory to allocated.
+**/
+RETURN_STATUS
+EFIAPI
+ReallocateSecurity2HandlerTable (
+  )
+{
+  //
+  // Reallocate memory for security info structure.
+  //
+  mSecurity2Table = ReallocatePool (
+                     mMaxNumberOfSecurity2Handler * sizeof (SECURITY2_INFO), 
+                     (mMaxNumberOfSecurity2Handler + SECURITY_HANDLER_TABLE_SIZE) * sizeof (SECURITY2_INFO), 
+                     mSecurity2Table
+                     );
+
+  //
+  // No enough resource is allocated.
+  //
+  if (mSecurity2Table == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Increase max handler number
+  //
+  mMaxNumberOfSecurity2Handler = mMaxNumberOfSecurity2Handler + SECURITY_HANDLER_TABLE_SIZE;
+  return RETURN_SUCCESS;
+}
+
+/**
+  Check whether an operation is valid according to the requirement of current operation, 
+  which must make sure that the measure image operation is the last one.
+  
+  If AuthenticationOperation is not recongnized, return FALSE.
+  If AuthenticationOperation is EFI_AUTH_OPERATION_NONE, return FALSE.
+  If AuthenticationOperation includes security operation and authentication operation, return FALSE.
+  If the previous register handler can't be executed before the later register handler, return FALSE.
+  
+  @param CurrentAuthOperation  Current operation.
+  @param CheckAuthOperation    Operation to be checked.
+  
+  @retval  TRUE   Operation is valid for current operation.
+  @retval  FALSE  Operation is invalid for current operation.
+**/
+BOOLEAN
+CheckAuthentication2Operation (
+  IN  UINT32    CurrentAuthOperation,
+  IN  UINT32    CheckAuthOperation
+  )
+{ 
+  //
+  // Make sure new auth operation can be recognized.
+  //
+  if (CheckAuthOperation == EFI_AUTH_OPERATION_NONE) {
+    return FALSE;
+  }
+  if ((CheckAuthOperation & ~(EFI_AUTH_IMAGE_OPERATION_MASK | 
+                              EFI_AUTH_NONE_IMAGE_OPERATION_MASK | 
+                              EFI_AUTH_OPERATION_IMAGE_REQUIRED)) != 0) {
+    return FALSE;
+  }
+
+  //
+  // When current operation includes measure image operation, 
+  // only another measure image or none image operation will be allowed.
+  //
+  if ((CurrentAuthOperation & EFI_AUTH_OPERATION_MEASURE_IMAGE) == EFI_AUTH_OPERATION_MEASURE_IMAGE) {
+    if (((CheckAuthOperation & EFI_AUTH_OPERATION_MEASURE_IMAGE) == EFI_AUTH_OPERATION_MEASURE_IMAGE) ||
+        ((CheckAuthOperation & EFI_AUTH_IMAGE_OPERATION_MASK) == 0)) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  }
+  
+  //
+  // Any other operation will be allowed.
+  //
+  return TRUE;
+}
+
+/**
+  Register security measurement handler with its operation type. Different
+  handlers with the same operation can all be registered.
+
+  If Security2Handler is NULL, then ASSERT().
+  If no enough resources available to register new handler, then ASSERT().
+  If AuthenticationOperation is not recongnized, then ASSERT().
+  If AuthenticationOperation is EFI_AUTH_OPERATION_NONE, then ASSERT().
+  If the previous register handler can't be executed before the later register handler, then ASSERT().
+
+  @param[in]  Security2Handler          The security measurement service handler to be registered.
+  @param[in]  AuthenticationOperation   The operation type is specified for the registered handler.
+
+  @retval EFI_SUCCESS              The handlers were registered successfully.
+**/
+EFI_STATUS
+EFIAPI
+RegisterSecurity2Handler (
+  IN  SECURITY2_FILE_AUTHENTICATION_HANDLER       Security2Handler,
+  IN  UINT32                                      AuthenticationOperation
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Security2Handler != NULL);
+
+  //
+  // Make sure AuthenticationOperation is valid in the register order.
+  //
+  ASSERT (CheckAuthentication2Operation (mCurrentAuthOperation2, AuthenticationOperation));
+  mCurrentAuthOperation2 = mCurrentAuthOperation2 | AuthenticationOperation;
+
+  //
+  // Check whether the handler lists is enough to store new handler.
+  //
+  if (mNumberOfSecurity2Handler == mMaxNumberOfSecurity2Handler) {
+    //
+    // Allocate more resources for new handler.
+    //
+    Status = ReallocateSecurity2HandlerTable();
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Register new handler into the handler list.
+  //
+  mSecurity2Table[mNumberOfSecurity2Handler].Security2Operation = AuthenticationOperation;
+  mSecurity2Table[mNumberOfSecurity2Handler].Security2Handler   = Security2Handler;
+  mNumberOfSecurity2Handler ++;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute registered handlers based on input AuthenticationOperation until 
+  one returns an error and that error is returned. 
+  
+  If none of the handlers return an error, then EFI_SUCCESS is returned.
+  The handlers those satisfy AuthenticationOperation will only be executed.
+  The handlers are executed in same order to their registered order.
+
+  @param[in]  AuthenticationOperation   
+                           The operation type specifies which handlers will be executed.
+  @param[in]  AuthenticationStatus 
+                           The authentication status for the input file.
+  @param[in]  File         This is a pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+  @param[in]  FileBuffer   A pointer to the buffer with the UEFI file image
+  @param[in]  FileSize     The size of File buffer.
+  @param[in]  BootPolicy   A boot policy that was used to call LoadImage() UEFI service.
+
+  @retval EFI_SUCCESS             The file specified by DevicePath and non-NULL
+                                  FileBuffer did authenticate, and the platform policy dictates
+                                  that the DXE Foundation may use the file.
+  @retval EFI_SUCCESS             The device path specified by NULL device path DevicePath
+                                  and non-NULL FileBuffer did authenticate, and the platform
+                                  policy dictates that the DXE Foundation may execute the image in
+                                  FileBuffer.
+  @retval EFI_SUCCESS             FileBuffer is NULL and current user has permission to start
+                                  UEFI device drivers on the device path specified by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  The file specified by File or FileBuffer did not
+                                  authenticate, and the platform policy dictates that 
+                                  the file should be placed in the untrusted state.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer FileBuffer is NULL and the user has no
+                                  permission to start UEFI device drivers on the device path specified
+                                  by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is not NULL and the user has no permission to load
+                                  drivers from the device path specified by DevicePath. The
+                                  image has been added into the list of the deferred images.
+  @retval EFI_ACCESS_DENIED       The file specified by File did not authenticate, and
+                                  the platform policy dictates that the DXE
+                                  Foundation may not use File.
+  @retval EFI_INVALID_PARAMETER   File and FileBuffer are both NULL. 
+**/
+EFI_STATUS
+EFIAPI
+ExecuteSecurity2Handlers (
+  IN  UINT32                           AuthenticationOperation,
+  IN  UINT32                           AuthenticationStatus,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL   *File,
+  IN  VOID                             *FileBuffer,
+  IN  UINTN                            FileSize,
+  IN  BOOLEAN                          BootPolicy
+  )
+{
+  UINT32        Index;
+  EFI_STATUS    Status;
+
+  //
+  // Invalid case if File and FileBuffer are both NULL.
+  //
+  if (File == NULL && FileBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Directly return successfully when no handler is registered.
+  //
+  if (mNumberOfSecurity2Handler == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Run security handler in same order to their registered list
+  //
+  for (Index = 0; Index < mNumberOfSecurity2Handler; Index ++) {
+    //
+    // If FileBuffer is not NULL, the input is Image, which will be handled by EFI_AUTH_IMAGE_OPERATION_MASK operation.
+    // If FileBuffer is NULL, the input is not Image, which will be handled by EFI_AUTH_NONE_IMAGE_OPERATION_MASK operation.
+    // Other cases are ignored.
+    //
+    if ((FileBuffer != NULL && (mSecurity2Table[Index].Security2Operation & EFI_AUTH_IMAGE_OPERATION_MASK) != 0) ||
+        (FileBuffer == NULL && (mSecurity2Table[Index].Security2Operation & EFI_AUTH_NONE_IMAGE_OPERATION_MASK) != 0)) {
+      //
+      // Execute registered handlers based on input AuthenticationOperation
+      //
+      if ((mSecurity2Table[Index].Security2Operation & AuthenticationOperation) != 0) {
+        Status = mSecurity2Table[Index].Security2Handler (
+                   AuthenticationStatus,
+                   File,
+                   FileBuffer,
+                   FileSize,
+                   BootPolicy
+                   );
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.inf b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.inf
new file mode 100644
index 0000000000..0f8a13b99d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.inf
@@ -0,0 +1,48 @@
+## @file
+# Instance of SecurityManagementLib Library for DXE phase.
+#
+# This library provides generic security measurement functions for DXE module.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeSecurityManagementLib
+  MODULE_UNI_FILE                = DxeSecurityManagementLib.uni
+  FILE_GUID                      = 7F61122C-19DF-47c3-BA0D-6C1149E30FA1
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SecurityManagementLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER 
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeSecurityManagementLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  DebugLib
+  DxeServicesLib
+  DevicePathLib
+  UefiBootServicesTableLib
+
+[Protocols]
+  gEfiLoadFileProtocolGuid                      ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.uni b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.uni
new file mode 100644
index 0000000000..6ead295caf
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.c b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.c
new file mode 100644
index 0000000000..7af6478bec
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.c
@@ -0,0 +1,866 @@
+/** @file
+  Performance library instance used in DXE phase to dump both PEI/DXE and SMM performance data.
+
+  This library instance allows a DXE driver or UEFI application to dump both PEI/DXE and SMM performance data.
+  StartPerformanceMeasurement(), EndPerformanceMeasurement(), StartPerformanceMeasurementEx()
+  and EndPerformanceMeasurementEx() are not implemented.
+
+  Copyright (c) 2011 - 2015, 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 <PiDxe.h>
+
+#include <Guid/Performance.h>
+
+#include <Library/PerformanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Protocol/SmmCommunication.h>
+
+#include <Guid/PiSmmCommunicationRegionTable.h>
+#include <Library/UefiLib.h>
+
+#define SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE (OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)  + sizeof (SMM_PERF_COMMUNICATE))
+
+EFI_SMM_COMMUNICATION_PROTOCOL  *mSmmCommunication = NULL;
+UINT8                           *mSmmPerformanceBuffer;
+GAUGE_DATA_ENTRY                *mGaugeData = NULL;
+UINTN                           mGaugeNumberOfEntries = 0;
+GAUGE_DATA_ENTRY_EX             *mGaugeDataEx = NULL;
+UINTN                           mGaugeNumberOfEntriesEx = 0;
+
+BOOLEAN                         mNoSmmPerfHandler = FALSE;
+BOOLEAN                         mNoSmmPerfExHandler = FALSE;
+
+//
+// The cached Performance Protocol and PerformanceEx Protocol interface.
+//
+PERFORMANCE_PROTOCOL            *mPerformance = NULL;
+PERFORMANCE_EX_PROTOCOL         *mPerformanceEx = NULL;
+
+/**
+  The function caches the pointer to SMM Communication protocol.
+
+  The function locates SMM Communication protocol from protocol database.
+
+  @retval EFI_SUCCESS     SMM Communication protocol is successfully located.
+  @retval Other           SMM Communication protocol is not located to log performance.
+
+**/
+EFI_STATUS
+GetCommunicationProtocol (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL  *Communication;
+
+  if (mSmmCommunication != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &Communication);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (Communication != NULL);
+    //
+    // Cache SMM Communication protocol.
+    //
+    mSmmCommunication = Communication;
+  }
+
+  return Status;
+}
+
+/**
+  The function caches the pointers to PerformanceEx protocol and Performance Protocol.
+
+  The function locates PerformanceEx protocol and Performance Protocol from protocol database.
+
+  @retval EFI_SUCCESS     PerformanceEx protocol or Performance Protocol is successfully located.
+  @retval EFI_NOT_FOUND   Both PerformanceEx protocol and Performance Protocol are not located to log performance.
+
+**/
+EFI_STATUS
+GetPerformanceProtocol (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  PERFORMANCE_PROTOCOL      *Performance;
+  PERFORMANCE_EX_PROTOCOL   *PerformanceEx;
+
+  if (mPerformanceEx != NULL || mPerformance != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->LocateProtocol (&gPerformanceExProtocolGuid, NULL, (VOID **) &PerformanceEx);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (PerformanceEx != NULL);
+    //
+    // Cache PerformanceEx Protocol.
+    //
+    mPerformanceEx = PerformanceEx;
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->LocateProtocol (&gPerformanceProtocolGuid, NULL, (VOID **) &Performance);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (Performance != NULL);
+    //
+    // Cache performance protocol.
+    //
+    mPerformance = Performance;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, Module and Identifier.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, Module and Identifier.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, and Module.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, and Module.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetByPerformanceProtocol (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  EFI_STATUS            Status;
+  GAUGE_DATA_ENTRY_EX   *GaugeData;
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->GetGaugeEx (LogEntryKey++, &GaugeData);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->GetGauge (LogEntryKey++, (GAUGE_DATA_ENTRY **) &GaugeData);
+  } else {
+    ASSERT (FALSE);
+    return 0;
+  }
+
+  //
+  // Make sure that LogEntryKey is a valid log entry key,
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // The LogEntryKey is the last entry (equals to the total entry number).
+    //
+    return 0;
+  }
+
+  ASSERT (GaugeData != NULL);
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+  if (mPerformanceEx != NULL) {
+    *Identifier   = GaugeData->Identifier;
+  } else {
+    *Identifier   = 0;
+  }
+
+  return LogEntryKey;
+}
+
+
+/**
+  Retrieves all previous logged performance measurement.
+  Function will use SMM communicate protocol to get all previous SMM performance measurement data.
+  If success, data buffer will be returned. If fail function will return NULL.
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+
+  @retval !NULL           Get all gauge data success.
+  @retval NULL            Get all gauge data failed.
+**/
+GAUGE_DATA_ENTRY *
+EFIAPI
+GetAllSmmGaugeData (
+  IN UINTN      LogEntryKey
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommBufferHeader;
+  SMM_PERF_COMMUNICATE                      *SmmPerfCommData;
+  UINTN                                     CommSize;
+  UINTN                                     DataSize;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE   *PiSmmCommunicationRegionTable;
+  UINT32                                    Index;
+  EFI_MEMORY_DESCRIPTOR                     *Entry;
+  UINT8                                     *Buffer;
+  UINTN                                     Size;
+  UINTN                                     NumberOfEntries;
+  UINTN                                     EntriesGot;
+
+  if (mNoSmmPerfHandler) {
+    //
+    // Not try to get the SMM gauge data again
+    // if no SMM Performance handler found.
+    //
+    return NULL;
+  }
+
+  if (LogEntryKey != 0) {
+    if (mGaugeData != NULL) {
+      return mGaugeData;
+    }
+  } else {
+    //
+    // Reget the SMM gauge data at the first entry get.
+    //
+    if (mGaugeData != NULL) {
+      FreePool (mGaugeData);
+      mGaugeData = NULL;
+      mGaugeNumberOfEntries = 0;
+    }
+  }
+
+  Status = GetCommunicationProtocol ();
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Status = EfiGetSystemConfigurationTable (
+             &gEdkiiPiSmmCommunicationRegionTableGuid,
+             (VOID **) &PiSmmCommunicationRegionTable
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ASSERT (PiSmmCommunicationRegionTable != NULL);
+  Entry = (EFI_MEMORY_DESCRIPTOR *) (PiSmmCommunicationRegionTable + 1);
+  Size = 0;
+  for (Index = 0; Index < PiSmmCommunicationRegionTable->NumberOfEntries; Index++) {
+    if (Entry->Type == EfiConventionalMemory) {
+      Size = EFI_PAGES_TO_SIZE ((UINTN) Entry->NumberOfPages);
+      if (Size >= (SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE + sizeof (GAUGE_DATA_ENTRY))) {
+        break;
+      }
+    }
+    Entry = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) Entry + PiSmmCommunicationRegionTable->DescriptorSize);
+  }
+  ASSERT (Index < PiSmmCommunicationRegionTable->NumberOfEntries);
+  mSmmPerformanceBuffer = (UINT8 *) (UINTN) Entry->PhysicalStart;
+
+  //
+  // Initialize communicate buffer 
+  //
+  SmmCommBufferHeader = (EFI_SMM_COMMUNICATE_HEADER *)mSmmPerformanceBuffer;
+  SmmPerfCommData = (SMM_PERF_COMMUNICATE *)SmmCommBufferHeader->Data;
+  ZeroMem((UINT8*)SmmPerfCommData, sizeof(SMM_PERF_COMMUNICATE));
+    
+  CopyGuid (&SmmCommBufferHeader->HeaderGuid, &gSmmPerformanceProtocolGuid);
+  SmmCommBufferHeader->MessageLength = sizeof(SMM_PERF_COMMUNICATE);
+  CommSize = SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE;
+
+  //
+  // Get total number of SMM gauge entries
+  //
+  SmmPerfCommData->Function = SMM_PERF_FUNCTION_GET_GAUGE_ENTRY_NUMBER;
+  Status = mSmmCommunication->Communicate (mSmmCommunication, mSmmPerformanceBuffer, &CommSize);
+  if (Status == EFI_NOT_FOUND) {
+    mNoSmmPerfHandler = TRUE;
+  }
+  if (EFI_ERROR (Status) || EFI_ERROR (SmmPerfCommData->ReturnStatus) || SmmPerfCommData->NumberOfEntries == 0) {
+    return NULL;
+  }
+
+  mGaugeNumberOfEntries = SmmPerfCommData->NumberOfEntries;
+
+  Buffer = mSmmPerformanceBuffer + SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE;
+  NumberOfEntries = (Size - SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE) / sizeof (GAUGE_DATA_ENTRY);
+  DataSize = mGaugeNumberOfEntries * sizeof(GAUGE_DATA_ENTRY);
+  mGaugeData = AllocateZeroPool(DataSize);
+  ASSERT (mGaugeData != NULL);
+
+  //
+  // Get all SMM gauge data
+  //  
+  SmmPerfCommData->Function = SMM_PERF_FUNCTION_GET_GAUGE_DATA;
+  SmmPerfCommData->GaugeData = (GAUGE_DATA_ENTRY *) Buffer;
+  EntriesGot = 0;
+  do {
+    SmmPerfCommData->LogEntryKey = EntriesGot;
+    if ((mGaugeNumberOfEntries - EntriesGot) >= NumberOfEntries) {
+      SmmPerfCommData->NumberOfEntries = NumberOfEntries;
+    } else {
+      SmmPerfCommData->NumberOfEntries = mGaugeNumberOfEntries - EntriesGot;
+    }
+    Status = mSmmCommunication->Communicate (mSmmCommunication, mSmmPerformanceBuffer, &CommSize);
+    if (EFI_ERROR (Status) || EFI_ERROR (SmmPerfCommData->ReturnStatus)) {
+      FreePool (mGaugeData);
+      mGaugeData = NULL;
+      mGaugeNumberOfEntries = 0;
+      return NULL;
+    } else {
+      CopyMem (&mGaugeData[EntriesGot], Buffer, SmmPerfCommData->NumberOfEntries * sizeof (GAUGE_DATA_ENTRY));
+    }
+    EntriesGot += SmmPerfCommData->NumberOfEntries;
+  } while (EntriesGot < mGaugeNumberOfEntries);
+
+  return mGaugeData;
+}
+
+/**
+  Retrieves all previous logged performance measurement.
+  Function will use SMM communicate protocol to get all previous SMM performance measurement data.
+  If success, data buffer will be returned. If fail function will return NULL.
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+
+  @retval !NULL           Get all gauge data success.
+  @retval NULL            Get all gauge data failed.
+**/
+GAUGE_DATA_ENTRY_EX *
+EFIAPI
+GetAllSmmGaugeDataEx (
+  IN UINTN      LogEntryKey
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommBufferHeader;
+  SMM_PERF_COMMUNICATE_EX                   *SmmPerfCommData;
+  UINTN                                     CommSize;
+  UINTN                                     DataSize;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE   *PiSmmCommunicationRegionTable;
+  UINT32                                    Index;
+  EFI_MEMORY_DESCRIPTOR                     *Entry;
+  UINT8                                     *Buffer;
+  UINTN                                     Size;
+  UINTN                                     NumberOfEntries;
+  UINTN                                     EntriesGot;
+
+  if (mNoSmmPerfExHandler) {
+    //
+    // Not try to get the SMM gauge data again
+    // if no SMM PerformanceEx handler found.
+    //
+    return NULL;
+  }
+
+  if (LogEntryKey != 0) {
+    if (mGaugeDataEx != NULL) {
+      return mGaugeDataEx;
+    }
+  } else {
+    //
+    // Reget the SMM gauge data at the first entry get.
+    //
+    if (mGaugeDataEx != NULL) {
+      FreePool (mGaugeDataEx);
+      mGaugeDataEx = NULL;
+      mGaugeNumberOfEntriesEx = 0;
+    }
+  }
+
+  Status = GetCommunicationProtocol ();
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Status = EfiGetSystemConfigurationTable (
+             &gEdkiiPiSmmCommunicationRegionTableGuid,
+             (VOID **) &PiSmmCommunicationRegionTable
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ASSERT (PiSmmCommunicationRegionTable != NULL);
+  Entry = (EFI_MEMORY_DESCRIPTOR *) (PiSmmCommunicationRegionTable + 1);
+  Size = 0;
+  for (Index = 0; Index < PiSmmCommunicationRegionTable->NumberOfEntries; Index++) {
+    if (Entry->Type == EfiConventionalMemory) {
+      Size = EFI_PAGES_TO_SIZE ((UINTN) Entry->NumberOfPages);
+      if (Size >= (SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE + sizeof (GAUGE_DATA_ENTRY_EX))) {
+        break;
+      }
+    }
+    Entry = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) Entry + PiSmmCommunicationRegionTable->DescriptorSize);
+  }
+  ASSERT (Index < PiSmmCommunicationRegionTable->NumberOfEntries);
+  mSmmPerformanceBuffer = (UINT8 *) (UINTN) Entry->PhysicalStart;
+  //
+  // Initialize communicate buffer 
+  //
+  SmmCommBufferHeader = (EFI_SMM_COMMUNICATE_HEADER *)mSmmPerformanceBuffer;
+  SmmPerfCommData = (SMM_PERF_COMMUNICATE_EX *)SmmCommBufferHeader->Data;
+  ZeroMem((UINT8*)SmmPerfCommData, sizeof(SMM_PERF_COMMUNICATE_EX));
+    
+  CopyGuid (&SmmCommBufferHeader->HeaderGuid, &gSmmPerformanceExProtocolGuid);
+  SmmCommBufferHeader->MessageLength = sizeof(SMM_PERF_COMMUNICATE_EX);
+  CommSize = SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE;
+
+  //
+  // Get total number of SMM gauge entries
+  //
+  SmmPerfCommData->Function = SMM_PERF_FUNCTION_GET_GAUGE_ENTRY_NUMBER;
+  Status = mSmmCommunication->Communicate (mSmmCommunication, mSmmPerformanceBuffer, &CommSize);
+  if (Status == EFI_NOT_FOUND) {
+    mNoSmmPerfExHandler = TRUE;
+  }
+  if (EFI_ERROR (Status) || EFI_ERROR (SmmPerfCommData->ReturnStatus) || SmmPerfCommData->NumberOfEntries == 0) {
+    return NULL;
+  }
+
+  mGaugeNumberOfEntriesEx = SmmPerfCommData->NumberOfEntries;
+
+  Buffer = mSmmPerformanceBuffer + SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE;
+  NumberOfEntries = (Size - SMM_PERFORMANCE_COMMUNICATION_BUFFER_SIZE) / sizeof (GAUGE_DATA_ENTRY_EX);
+  DataSize = mGaugeNumberOfEntriesEx * sizeof(GAUGE_DATA_ENTRY_EX);
+  mGaugeDataEx = AllocateZeroPool(DataSize);
+  ASSERT (mGaugeDataEx != NULL);
+
+  //
+  // Get all SMM gauge data
+  //  
+  SmmPerfCommData->Function = SMM_PERF_FUNCTION_GET_GAUGE_DATA;
+  SmmPerfCommData->GaugeDataEx = (GAUGE_DATA_ENTRY_EX *) Buffer;
+  EntriesGot = 0;
+  do {
+    SmmPerfCommData->LogEntryKey = EntriesGot;
+    if ((mGaugeNumberOfEntriesEx - EntriesGot) >= NumberOfEntries) {
+      SmmPerfCommData->NumberOfEntries = NumberOfEntries;
+    } else {
+      SmmPerfCommData->NumberOfEntries = mGaugeNumberOfEntriesEx - EntriesGot;
+    }
+    Status = mSmmCommunication->Communicate (mSmmCommunication, mSmmPerformanceBuffer, &CommSize);
+    if (EFI_ERROR (Status) || EFI_ERROR (SmmPerfCommData->ReturnStatus)) {
+      FreePool (mGaugeDataEx);
+      mGaugeDataEx = NULL;
+      mGaugeNumberOfEntriesEx = 0;
+      return NULL;
+    } else {
+      CopyMem (&mGaugeDataEx[EntriesGot], Buffer, SmmPerfCommData->NumberOfEntries * sizeof (GAUGE_DATA_ENTRY_EX));
+    }
+    EntriesGot += SmmPerfCommData->NumberOfEntries;
+  } while (EntriesGot < mGaugeNumberOfEntriesEx);
+
+  return mGaugeDataEx;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  GAUGE_DATA_ENTRY_EX   *GaugeData;
+
+  GaugeData = NULL;
+
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  mGaugeDataEx = GetAllSmmGaugeDataEx (LogEntryKey);
+  if (mGaugeDataEx != NULL) {
+    if (LogEntryKey >= mGaugeNumberOfEntriesEx) {
+      //
+      // Try to get the data by Performance Protocol.
+      //
+      LogEntryKey = LogEntryKey - mGaugeNumberOfEntriesEx;
+      LogEntryKey = GetByPerformanceProtocol (
+                      LogEntryKey,
+                      Handle,
+                      Token,
+                      Module,
+                      StartTimeStamp,
+                      EndTimeStamp,
+                      Identifier
+                      );
+      if (LogEntryKey == 0) {
+        //
+        // Last entry.
+        //
+        return LogEntryKey;
+      } else {
+        return (LogEntryKey + mGaugeNumberOfEntriesEx);
+      }
+    }
+
+    GaugeData = &mGaugeDataEx[LogEntryKey++];
+    *Identifier = GaugeData->Identifier;
+  } else {
+    mGaugeData = GetAllSmmGaugeData (LogEntryKey);
+    if (mGaugeData != NULL) {
+      if (LogEntryKey >= mGaugeNumberOfEntries) {
+        //
+        // Try to get the data by Performance Protocol.
+        //
+        LogEntryKey = LogEntryKey - mGaugeNumberOfEntries;
+        LogEntryKey = GetByPerformanceProtocol (
+                        LogEntryKey,
+                        Handle,
+                        Token,
+                        Module,
+                        StartTimeStamp,
+                        EndTimeStamp,
+                        Identifier
+                        );
+        if (LogEntryKey == 0) {
+          //
+          // Last entry.
+          //
+          return LogEntryKey;
+        } else {
+          return (LogEntryKey + mGaugeNumberOfEntries);
+        }
+      }
+
+      GaugeData = (GAUGE_DATA_ENTRY_EX *) &mGaugeData[LogEntryKey++];
+      *Identifier = 0;
+    } else {
+      return GetByPerformanceProtocol (
+               LogEntryKey,
+               Handle,
+               Token,
+               Module,
+               StartTimeStamp,
+               EndTimeStamp,
+               Identifier
+               );
+    }
+  }
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+
+  return LogEntryKey;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.inf b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.inf
new file mode 100644
index 0000000000..2b057704f8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.inf
@@ -0,0 +1,68 @@
+## @file
+#  Performance library instance used in DXE phase to dump SMM performance data.
+#  
+#  This library instance allows a DXE driver or UEFI application to dump both PEI/DXE and SMM performance data.
+#  StartPerformanceMeasurement(), EndPerformanceMeasurement(), StartPerformanceMeasurementEx()
+#  and EndPerformanceMeasurementEx() are not implemented.
+#  
+#  Copyright (c) 2011 - 2016, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeSmmPerformanceLib
+  MODULE_UNI_FILE                = DxeSmmPerformanceLib.uni
+  FILE_GUID                      = DA80C15C-0B4D-4e75-8946-4043DE559B0C
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PerformanceLib|DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_APPLICATION
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  DxeSmmPerformanceLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseLib
+  UefiBootServicesTableLib
+  PcdLib
+  DebugLib
+  BaseMemoryLib
+  UefiRuntimeServicesTableLib
+  MemoryAllocationLib
+  UefiLib
+
+[Guids]
+  gPerformanceProtocolGuid              ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+  gPerformanceExProtocolGuid            ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+  gSmmPerformanceProtocolGuid           ## SOMETIMES_PRODUCES   ## UNDEFINED # Used to do smm communication
+  gSmmPerformanceExProtocolGuid         ## SOMETIMES_PRODUCES   ## UNDEFINED # Used to do smm communication
+  gEdkiiPiSmmCommunicationRegionTableGuid    ## CONSUMES ## SystemTable
+
+[Protocols]
+  gEfiSmmCommunicationProtocolGuid      ## CONSUMES
+
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
+
+[Depex.common.DXE_DRIVER, Depex.common.DXE_RUNTIME_DRIVER]
+  gEfiSmmCommunicationProtocolGuid
diff --git a/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.uni b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.uni
new file mode 100644
index 0000000000..b92de07144
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.c b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.c
new file mode 100644
index 0000000000..17183e1a6c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.c
@@ -0,0 +1,1007 @@
+/** @file
+  This library is used to share code between UEFI network stack modules.
+  It provides the helper routines to access TCP service.
+
+Copyright (c) 2010 - 2011, 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<BR>
+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 <Uefi.h>
+
+#include <Library/TcpIoLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+
+/**
+  The common notify function associated with various TcpIo events. 
+
+  @param[in]  Event   The event signaled.
+  @param[in]  Context The context.
+
+**/
+VOID
+EFIAPI
+TcpIoCommonNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  if ((Event == NULL) || (Context == NULL)) {
+    return ;
+  }
+
+  *((BOOLEAN *) Context) = TRUE;
+}
+
+/**
+  The internal function for delay configuring TCP6 when IP6 driver is still in DAD.
+
+  @param[in]  Tcp6               The EFI_TCP6_PROTOCOL protocol instance.
+  @param[in]  Tcp6ConfigData     The Tcp6 configuration data.
+
+  @retval EFI_SUCCESS            The operational settings successfully
+                                 completed.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval Others                 Failed to finish the operation.
+
+**/
+EFI_STATUS
+TcpIoGetMapping (
+  IN EFI_TCP6_PROTOCOL    *Tcp6,
+  IN EFI_TCP6_CONFIG_DATA *Tcp6ConfigData
+  )
+{
+  EFI_STATUS              Status;
+  EFI_EVENT               Event;
+
+  if ((Tcp6 == NULL) || (Tcp6ConfigData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Event  = NULL;
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = gBS->SetTimer (
+                  Event,
+                  TimerRelative,
+                  TCP_GET_MAPPING_TIMEOUT
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  while (EFI_ERROR (gBS->CheckEvent (Event))) {
+
+    Tcp6->Poll (Tcp6);
+
+    Status = Tcp6->Configure (Tcp6, Tcp6ConfigData);
+
+    if (!EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+ON_EXIT:
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  return Status;
+}
+
+/**
+  Create a TCP socket with the specified configuration data. 
+
+  @param[in]  Image      The handle of the driver image.
+  @param[in]  Controller The handle of the controller.
+  @param[in]  TcpVersion The version of Tcp, TCP_VERSION_4 or TCP_VERSION_6.
+  @param[in]  ConfigData The Tcp configuration data.
+  @param[out] TcpIo      The TcpIo.
+  
+  @retval EFI_SUCCESS            The TCP socket is created and configured.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory.
+  @retval Others                 Failed to create the TCP socket or configure it.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoCreateSocket (
+  IN EFI_HANDLE             Image,
+  IN EFI_HANDLE             Controller,
+  IN UINT8                  TcpVersion,
+  IN TCP_IO_CONFIG_DATA     *ConfigData,
+  OUT TCP_IO                *TcpIo
+  )
+{
+  EFI_STATUS                Status;
+  EFI_EVENT                 Event;
+  EFI_GUID                  *ServiceBindingGuid;
+  EFI_GUID                  *ProtocolGuid;
+  VOID                      **Interface;
+  EFI_TCP4_OPTION           ControlOption;
+  EFI_TCP4_CONFIG_DATA      Tcp4ConfigData;
+  EFI_TCP4_ACCESS_POINT     *AccessPoint4;
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_CONFIG_DATA      Tcp6ConfigData;
+  EFI_TCP6_ACCESS_POINT     *AccessPoint6;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  EFI_TCP4_RECEIVE_DATA     *RxData;
+
+  if ((Image == NULL) || (Controller == NULL) || (ConfigData == NULL) || (TcpIo == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Tcp4 = NULL;
+  Tcp6 = NULL;
+
+  ZeroMem (TcpIo, sizeof (TCP_IO));
+
+  if (TcpVersion == TCP_VERSION_4) {
+    ServiceBindingGuid = &gEfiTcp4ServiceBindingProtocolGuid;
+    ProtocolGuid       = &gEfiTcp4ProtocolGuid;
+    Interface          = (VOID **) (&TcpIo->Tcp.Tcp4);
+  } else if (TcpVersion == TCP_VERSION_6) {
+    ServiceBindingGuid = &gEfiTcp6ServiceBindingProtocolGuid;
+    ProtocolGuid       = &gEfiTcp6ProtocolGuid;
+    Interface          = (VOID **) (&TcpIo->Tcp.Tcp6);
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  TcpIo->TcpVersion = TcpVersion;
+
+  //
+  // Create the TCP child instance and get the TCP protocol.
+  //  
+  Status = NetLibCreateServiceChild (
+             Controller,
+             Image,
+             ServiceBindingGuid,
+             &TcpIo->Handle
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  TcpIo->Handle,
+                  ProtocolGuid,
+                  Interface,
+                  Image,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) || (*Interface == NULL)) {
+    goto ON_ERROR;
+  }
+
+  if (TcpVersion == TCP_VERSION_4) {
+    Tcp4             = TcpIo->Tcp.Tcp4;
+  } else {
+    Tcp6             = TcpIo->Tcp.Tcp6;
+  }
+
+  TcpIo->Image       = Image;
+  TcpIo->Controller  = Controller;
+
+  //
+  // Set the configuration parameters.
+  //
+  ControlOption.ReceiveBufferSize       = 0x200000;
+  ControlOption.SendBufferSize          = 0x200000;
+  ControlOption.MaxSynBackLog           = 0;
+  ControlOption.ConnectionTimeout       = 0;
+  ControlOption.DataRetries             = 6;
+  ControlOption.FinTimeout              = 0;
+  ControlOption.TimeWaitTimeout         = 0;
+  ControlOption.KeepAliveProbes         = 4;
+  ControlOption.KeepAliveTime           = 0;
+  ControlOption.KeepAliveInterval       = 0;
+  ControlOption.EnableNagle             = FALSE;
+  ControlOption.EnableTimeStamp         = FALSE;
+  ControlOption.EnableWindowScaling     = TRUE;
+  ControlOption.EnableSelectiveAck      = FALSE;
+  ControlOption.EnablePathMtuDiscovery  = FALSE;
+
+  if (TcpVersion == TCP_VERSION_4) {
+    Tcp4ConfigData.TypeOfService        = 8;
+    Tcp4ConfigData.TimeToLive           = 255;
+    Tcp4ConfigData.ControlOption        = &ControlOption;
+
+    AccessPoint4                        = &Tcp4ConfigData.AccessPoint;
+
+    ZeroMem (AccessPoint4, sizeof (EFI_TCP4_ACCESS_POINT));
+    AccessPoint4->StationPort           = ConfigData->Tcp4IoConfigData.StationPort;
+    AccessPoint4->RemotePort            = ConfigData->Tcp4IoConfigData.RemotePort;
+    AccessPoint4->ActiveFlag            = ConfigData->Tcp4IoConfigData.ActiveFlag;
+
+    CopyMem (
+      &AccessPoint4->StationAddress,
+      &ConfigData->Tcp4IoConfigData.LocalIp,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+    CopyMem (
+      &AccessPoint4->SubnetMask,
+      &ConfigData->Tcp4IoConfigData.SubnetMask,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+    CopyMem (
+      &AccessPoint4->RemoteAddress,
+      &ConfigData->Tcp4IoConfigData.RemoteIp,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+
+    ASSERT (Tcp4 != NULL);
+
+    //
+    // Configure the TCP4 protocol.
+    //
+    Status = Tcp4->Configure (Tcp4, &Tcp4ConfigData);
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+
+    if (!EFI_IP4_EQUAL (&ConfigData->Tcp4IoConfigData.Gateway, &mZeroIp4Addr)) {
+      //
+      // The gateway is not zero. Add the default route manually.
+      //
+      Status = Tcp4->Routes (
+                       Tcp4,
+                       FALSE,
+                       &mZeroIp4Addr,
+                       &mZeroIp4Addr,
+                       &ConfigData->Tcp4IoConfigData.Gateway
+                       );
+      if (EFI_ERROR (Status)) {
+        goto ON_ERROR;
+      }
+    }
+  } else {
+    Tcp6ConfigData.TrafficClass         = 0;
+    Tcp6ConfigData.HopLimit             = 255;
+    Tcp6ConfigData.ControlOption        = (EFI_TCP6_OPTION *) &ControlOption;
+
+    AccessPoint6                        = &Tcp6ConfigData.AccessPoint;
+
+    ZeroMem (AccessPoint6, sizeof (EFI_TCP6_ACCESS_POINT));
+    AccessPoint6->StationPort           = ConfigData->Tcp6IoConfigData.StationPort;
+    AccessPoint6->RemotePort            = ConfigData->Tcp6IoConfigData.RemotePort;
+    AccessPoint6->ActiveFlag            = ConfigData->Tcp6IoConfigData.ActiveFlag;
+
+    IP6_COPY_ADDRESS (&AccessPoint6->RemoteAddress, &ConfigData->Tcp6IoConfigData.RemoteIp);
+
+
+    ASSERT (Tcp6 != NULL);
+    //
+    // Configure the TCP6 protocol.
+    //
+    Status = Tcp6->Configure (Tcp6, &Tcp6ConfigData);
+    if (Status == EFI_NO_MAPPING) {
+      Status = TcpIoGetMapping (Tcp6, &Tcp6ConfigData);
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+  }
+
+  //
+  // Create events for variuos asynchronous operations.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  TcpIoCommonNotify,
+                  &TcpIo->IsConnDone,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  TcpIo->ConnToken.Tcp4Token.CompletionToken.Event = Event;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  TcpIoCommonNotify,
+                  &TcpIo->IsListenDone,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  TcpIo->ListenToken.Tcp4Token.CompletionToken.Event = Event;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  TcpIoCommonNotify,
+                  &TcpIo->IsTxDone,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  TcpIo->TxToken.Tcp4Token.CompletionToken.Event = Event;
+
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  TcpIoCommonNotify,
+                  &TcpIo->IsRxDone,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  TcpIo->RxToken.Tcp4Token.CompletionToken.Event = Event;
+
+  RxData = (EFI_TCP4_RECEIVE_DATA *) AllocateZeroPool (sizeof (EFI_TCP4_RECEIVE_DATA));
+  if (RxData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  TcpIo->RxToken.Tcp4Token.Packet.RxData = RxData;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  TcpIoCommonNotify,
+                  &TcpIo->IsCloseDone,
+                  &Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  TcpIo->CloseToken.Tcp4Token.CompletionToken.Event = Event;
+
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  TcpIoDestroySocket (TcpIo);
+
+  return Status;
+}
+  
+/**
+  Destroy the socket. 
+
+  @param[in]  TcpIo The TcpIo which wraps the socket to be destroyed.
+
+**/
+VOID
+EFIAPI
+TcpIoDestroySocket (
+  IN TCP_IO                 *TcpIo
+  )
+{
+  EFI_EVENT                 Event;
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  UINT8                     TcpVersion;
+  EFI_GUID                  *ServiceBindingGuid;
+  EFI_GUID                  *ProtocolGuid;
+  EFI_HANDLE                ChildHandle;
+
+  if (TcpIo == NULL) {
+    return ;
+  }
+
+  TcpVersion = TcpIo->TcpVersion;
+
+  if ((TcpVersion != TCP_VERSION_4) && (TcpVersion != TCP_VERSION_6)) {
+    return ;
+  }
+
+  Event = TcpIo->ConnToken.Tcp4Token.CompletionToken.Event;
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  Event = TcpIo->ListenToken.Tcp4Token.CompletionToken.Event;
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  Event = TcpIo->TxToken.Tcp4Token.CompletionToken.Event;
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  Event = TcpIo->RxToken.Tcp4Token.CompletionToken.Event;
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  Event = TcpIo->CloseToken.Tcp4Token.CompletionToken.Event;
+
+  if (Event != NULL) {
+    gBS->CloseEvent (Event);
+  }
+
+  if (TcpIo->RxToken.Tcp4Token.Packet.RxData != NULL) {
+    FreePool (TcpIo->RxToken.Tcp4Token.Packet.RxData);
+  }
+
+  Tcp4 = NULL;
+  Tcp6 = NULL;
+
+
+  if (TcpVersion == TCP_VERSION_4) {
+    ServiceBindingGuid = &gEfiTcp4ServiceBindingProtocolGuid;
+    ProtocolGuid       = &gEfiTcp4ProtocolGuid;
+    Tcp4 = TcpIo->Tcp.Tcp4;
+    if (Tcp4 != NULL) {
+      Tcp4->Configure (Tcp4, NULL);
+    }
+  } else {
+    ServiceBindingGuid = &gEfiTcp6ServiceBindingProtocolGuid;
+    ProtocolGuid       = &gEfiTcp6ProtocolGuid;
+    Tcp6 = TcpIo->Tcp.Tcp6;
+    if (Tcp6 != NULL) {
+      Tcp6->Configure (Tcp6, NULL);
+    }
+  }
+
+  if ((Tcp4 != NULL) || (Tcp6 != NULL)) {
+
+    gBS->CloseProtocol (
+           TcpIo->Handle,
+           ProtocolGuid,
+           TcpIo->Image,
+           TcpIo->Controller
+           );
+  }
+
+  ChildHandle = NULL;
+
+  if (TcpIo->IsListenDone) {
+    if (TcpVersion == TCP_VERSION_4) {
+      Tcp4 = TcpIo->NewTcp.Tcp4;
+      if (Tcp4 != NULL) {
+        Tcp4->Configure (Tcp4, NULL);
+        ChildHandle = TcpIo->ListenToken.Tcp4Token.NewChildHandle;
+      }
+    } else {
+      Tcp6 = TcpIo->NewTcp.Tcp6;
+      if (Tcp6 != NULL) {
+        Tcp6->Configure (Tcp6, NULL);
+        ChildHandle = TcpIo->ListenToken.Tcp6Token.NewChildHandle;
+      }
+    }
+
+    if (ChildHandle != NULL) {
+
+      gBS->CloseProtocol (
+             ChildHandle,
+             ProtocolGuid,
+             TcpIo->Image,
+             TcpIo->Controller
+             );
+    }
+  }
+
+  NetLibDestroyServiceChild (
+    TcpIo->Controller,
+    TcpIo->Image,
+    ServiceBindingGuid,
+    TcpIo->Handle
+    );
+}
+
+/**
+  Connect to the other endpoint of the TCP socket.
+
+  @param[in, out]  TcpIo     The TcpIo wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+  
+  @retval EFI_SUCCESS            Connect to the other endpoint of the TCP socket
+                                 successfully.
+  @retval EFI_TIMEOUT            Failed to connect to the other endpoint of the
+                                 TCP socket in the specified time period.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoConnect (
+  IN OUT TCP_IO             *TcpIo,
+  IN     EFI_EVENT          Timeout
+  )
+{
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  EFI_STATUS                Status;
+
+  if ((TcpIo == NULL) || (TcpIo->Tcp.Tcp4 == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TcpIo->IsConnDone = FALSE;
+
+  Tcp4 = NULL;
+  Tcp6 = NULL;
+
+  if (TcpIo->TcpVersion == TCP_VERSION_4) {
+    Tcp4   = TcpIo->Tcp.Tcp4;
+    Status = Tcp4->Connect (Tcp4, &TcpIo->ConnToken.Tcp4Token);
+  } else if (TcpIo->TcpVersion == TCP_VERSION_6) {
+    Tcp6   = TcpIo->Tcp.Tcp6;
+    Status = Tcp6->Connect (Tcp6, &TcpIo->ConnToken.Tcp6Token);
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (!TcpIo->IsConnDone && EFI_ERROR (gBS->CheckEvent (Timeout))) {
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      Tcp4->Poll (Tcp4);
+    } else {
+      Tcp6->Poll (Tcp6);
+    }
+  }
+
+  if (!TcpIo->IsConnDone) {
+    Status = EFI_TIMEOUT;
+  } else {
+    Status = TcpIo->ConnToken.Tcp4Token.CompletionToken.Status;
+  }
+
+  return Status;
+}
+
+/**
+  Accept the incomding request from the other endpoint of the TCP socket.
+
+  @param[in, out]  TcpIo     The TcpIo wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+
+  
+  @retval EFI_SUCCESS            Connect to the other endpoint of the TCP socket
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+
+  @retval EFI_TIMEOUT            Failed to connect to the other endpoint of the
+                                 TCP socket in the specified time period.                      
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoAccept (
+  IN OUT TCP_IO             *TcpIo,
+  IN     EFI_EVENT          Timeout
+  )
+{
+  EFI_STATUS                Status;
+  EFI_GUID                  *ProtocolGuid;
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+
+  if ((TcpIo == NULL) || (TcpIo->Tcp.Tcp4 == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TcpIo->IsListenDone = FALSE;
+
+  Tcp4 = NULL;
+  Tcp6 = NULL;
+
+  if (TcpIo->TcpVersion == TCP_VERSION_4) {
+    Tcp4   = TcpIo->Tcp.Tcp4;
+    Status = Tcp4->Accept (Tcp4, &TcpIo->ListenToken.Tcp4Token);
+  } else if (TcpIo->TcpVersion == TCP_VERSION_6) {
+    Tcp6   = TcpIo->Tcp.Tcp6;
+    Status = Tcp6->Accept (Tcp6, &TcpIo->ListenToken.Tcp6Token);
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (!TcpIo->IsListenDone && EFI_ERROR (gBS->CheckEvent (Timeout))) {
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      Tcp4->Poll (Tcp4);
+    } else {
+      Tcp6->Poll (Tcp6);
+    }
+  }
+
+  if (!TcpIo->IsListenDone) {
+    Status = EFI_TIMEOUT;
+  } else {
+    Status = TcpIo->ListenToken.Tcp4Token.CompletionToken.Status;
+  }
+
+  //
+  // The new TCP instance handle created for the established connection is 
+  // in ListenToken.
+  //
+  if (!EFI_ERROR (Status)) {
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      ProtocolGuid = &gEfiTcp4ProtocolGuid;
+    } else {
+      ProtocolGuid = &gEfiTcp6ProtocolGuid;
+    }
+    
+    Status = gBS->OpenProtocol (
+                    TcpIo->ListenToken.Tcp4Token.NewChildHandle,
+                    ProtocolGuid,
+                    (VOID **) (&TcpIo->NewTcp.Tcp4),
+                    TcpIo->Image,
+                    TcpIo->Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+
+  }
+
+  return Status;
+}
+
+/**
+  Reset the socket.
+
+  @param[in, out]  TcpIo The TcpIo wrapping the TCP socket.
+
+**/
+VOID
+EFIAPI
+TcpIoReset (
+  IN OUT TCP_IO             *TcpIo
+  )
+{
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  EFI_STATUS                Status;
+
+  if ((TcpIo == NULL) || (TcpIo->Tcp.Tcp4 == NULL)) {
+    return ;
+  }
+
+  TcpIo->IsCloseDone = FALSE;
+  Tcp4               = NULL;
+  Tcp6               = NULL;
+
+  if (TcpIo->TcpVersion == TCP_VERSION_4) { 
+    TcpIo->CloseToken.Tcp4Token.AbortOnClose = TRUE;
+    Tcp4 = TcpIo->Tcp.Tcp4;
+    Status = Tcp4->Close (Tcp4, &TcpIo->CloseToken.Tcp4Token);
+  } else if (TcpIo->TcpVersion == TCP_VERSION_6) {
+    TcpIo->CloseToken.Tcp6Token.AbortOnClose = TRUE;
+    Tcp6 = TcpIo->Tcp.Tcp6;
+    Status = Tcp6->Close (Tcp6, &TcpIo->CloseToken.Tcp6Token);
+  } else {
+    return ;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  while (!TcpIo->IsCloseDone) {
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      Tcp4->Poll (Tcp4);
+    } else {
+      Tcp6->Poll (Tcp6);
+    }
+  }
+}
+
+  
+/**
+  Transmit the Packet to the other endpoint of the socket.
+
+  @param[in]   TcpIo           The TcpIo wrapping the TCP socket.
+  @param[in]   Packet          The packet to transmit.
+  
+  @retval EFI_SUCCESS            The packet is trasmitted.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_UNSUPPORTED        One or more of the control options are not
+                                 supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error occurred.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoTransmit (
+  IN TCP_IO                 *TcpIo,
+  IN NET_BUF                *Packet
+  )
+{
+  EFI_STATUS                Status;
+  VOID                      *Data;
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  UINTN                     Size;
+
+  if ((TcpIo == NULL) || (TcpIo->Tcp.Tcp4 == NULL)|| (Packet == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TcpIo->TcpVersion == TCP_VERSION_4) {
+
+    Size = sizeof (EFI_TCP4_TRANSMIT_DATA) + 
+           (Packet->BlockOpNum - 1) * sizeof (EFI_TCP4_FRAGMENT_DATA);
+  } else if (TcpIo->TcpVersion == TCP_VERSION_6) {
+    Size = sizeof (EFI_TCP6_TRANSMIT_DATA) +
+           (Packet->BlockOpNum - 1) * sizeof (EFI_TCP6_FRAGMENT_DATA);
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  Data = AllocatePool (Size);
+  if (Data == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ((EFI_TCP4_TRANSMIT_DATA *) Data)->Push        = TRUE;
+  ((EFI_TCP4_TRANSMIT_DATA *) Data)->Urgent      = FALSE;
+  ((EFI_TCP4_TRANSMIT_DATA *) Data)->DataLength  = Packet->TotalSize;
+
+  //
+  // Build the fragment table.
+  //
+  ((EFI_TCP4_TRANSMIT_DATA *) Data)->FragmentCount = Packet->BlockOpNum;
+
+  NetbufBuildExt (
+    Packet,
+    (NET_FRAGMENT *) &((EFI_TCP4_TRANSMIT_DATA *) Data)->FragmentTable[0],
+    &((EFI_TCP4_TRANSMIT_DATA *) Data)->FragmentCount
+    );
+
+  Tcp4   = NULL;
+  Tcp6   = NULL;
+  Status = EFI_DEVICE_ERROR;
+
+  //
+  // Trasnmit the packet.
+  //
+  if (TcpIo->TcpVersion == TCP_VERSION_4) {
+    TcpIo->TxToken.Tcp4Token.Packet.TxData = (EFI_TCP4_TRANSMIT_DATA *) Data;
+    Tcp4    = TcpIo->Tcp.Tcp4;
+    if (TcpIo->IsListenDone) {
+      Tcp4 = TcpIo->NewTcp.Tcp4;
+    }
+
+    if (Tcp4 == NULL) {
+      goto ON_EXIT;
+    }
+    
+    Status  = Tcp4->Transmit (Tcp4, &TcpIo->TxToken.Tcp4Token);
+  } else {
+    TcpIo->TxToken.Tcp6Token.Packet.TxData = (EFI_TCP6_TRANSMIT_DATA *) Data;
+    Tcp6    = TcpIo->Tcp.Tcp6;
+    if (TcpIo->IsListenDone) {
+      Tcp6 = TcpIo->NewTcp.Tcp6;
+    }
+
+    if (Tcp6 == NULL) {
+      goto ON_EXIT;
+    }
+
+    Status  = Tcp6->Transmit (Tcp6, &TcpIo->TxToken.Tcp6Token);
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  while (!TcpIo->IsTxDone) {
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      Tcp4->Poll (Tcp4);
+    } else {
+      Tcp6->Poll (Tcp6);
+    }
+  }
+
+  TcpIo->IsTxDone  = FALSE;
+  Status           = TcpIo->TxToken.Tcp4Token.CompletionToken.Status;
+
+ON_EXIT:
+
+  FreePool (Data);
+
+  return Status;
+}
+
+/**
+  Receive data from the socket.
+
+  @param[in, out]  TcpIo       The TcpIo which wraps the socket to be destroyed.
+  @param[in]       Packet      The buffer to hold the data copy from the socket rx buffer.
+  @param[in]       AsyncMode   Is this receive asyncronous or not.
+  @param[in]       Timeout     The time to wait for receiving the amount of data the Packet
+                               can hold.
+
+  @retval EFI_SUCCESS            The required amount of data is received from the socket.
+  @retval EFI_INVALID_PARAMETER  One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error occurred.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate momery.
+  @retval EFI_TIMEOUT            Failed to receive the required amount of data in the
+                                 specified time period.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpIoReceive (
+  IN OUT TCP_IO             *TcpIo,
+  IN     NET_BUF            *Packet,
+  IN     BOOLEAN            AsyncMode,
+  IN     EFI_EVENT          Timeout
+  )
+{
+  EFI_TCP4_PROTOCOL         *Tcp4;
+  EFI_TCP6_PROTOCOL         *Tcp6;
+  EFI_TCP4_RECEIVE_DATA     *RxData;
+  EFI_STATUS                Status;
+  NET_FRAGMENT              *Fragment;
+  UINT32                    FragmentCount;
+  UINT32                    CurrentFragment;
+
+  if ((TcpIo == NULL) || (TcpIo->Tcp.Tcp4 == NULL)|| (Packet == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  RxData = TcpIo->RxToken.Tcp4Token.Packet.RxData;
+  if (RxData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Tcp4 = NULL;
+  Tcp6 = NULL;
+
+  if (TcpIo->TcpVersion == TCP_VERSION_4) {
+    Tcp4 = TcpIo->Tcp.Tcp4;
+
+    if (TcpIo->IsListenDone) {
+      Tcp4 = TcpIo->NewTcp.Tcp4;
+    }
+
+    if (Tcp4 == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+  } else if (TcpIo->TcpVersion == TCP_VERSION_6) {
+    Tcp6 = TcpIo->Tcp.Tcp6;
+
+    if (TcpIo->IsListenDone) {
+      Tcp6 = TcpIo->NewTcp.Tcp6;
+    }
+
+    if (Tcp6 == NULL) {
+      return EFI_DEVICE_ERROR; 
+    }
+
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+
+  FragmentCount = Packet->BlockOpNum;
+  Fragment      = AllocatePool (FragmentCount * sizeof (NET_FRAGMENT));
+  if (Fragment == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+  //
+  // Build the fragment table.
+  //
+  NetbufBuildExt (Packet, Fragment, &FragmentCount);
+
+  RxData->FragmentCount         = 1;
+  CurrentFragment               = 0;
+  Status                        = EFI_SUCCESS;
+
+  while (CurrentFragment < FragmentCount) {
+    RxData->DataLength                       = Fragment[CurrentFragment].Len;
+    RxData->FragmentTable[0].FragmentLength  = Fragment[CurrentFragment].Len;
+    RxData->FragmentTable[0].FragmentBuffer  = Fragment[CurrentFragment].Bulk;
+
+    if (TcpIo->TcpVersion == TCP_VERSION_4) {
+      Status = Tcp4->Receive (Tcp4, &TcpIo->RxToken.Tcp4Token);
+    } else {
+      Status = Tcp6->Receive (Tcp6, &TcpIo->RxToken.Tcp6Token);
+    }
+    
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+    
+    while (!TcpIo->IsRxDone && ((Timeout == NULL) || EFI_ERROR (gBS->CheckEvent (Timeout)))) {
+      //
+      // Poll until some data is received or an error occurs.
+      //
+      if (TcpIo->TcpVersion == TCP_VERSION_4) {
+        Tcp4->Poll (Tcp4);
+      } else {
+        Tcp6->Poll (Tcp6);
+      }
+    }
+
+    if (!TcpIo->IsRxDone) {
+      //
+      // Timeout occurs, cancel the receive request.
+      //
+      if (TcpIo->TcpVersion == TCP_VERSION_4) {
+        Tcp4->Cancel (Tcp4, &TcpIo->RxToken.Tcp4Token.CompletionToken);
+      } else {
+        Tcp6->Cancel (Tcp6, &TcpIo->RxToken.Tcp6Token.CompletionToken);
+      }
+
+      Status = EFI_TIMEOUT;
+      goto ON_EXIT;
+    } else {
+      TcpIo->IsRxDone = FALSE;
+    }
+
+    Status = TcpIo->RxToken.Tcp4Token.CompletionToken.Status;
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+
+    Fragment[CurrentFragment].Len -= RxData->FragmentTable[0].FragmentLength;
+    if (Fragment[CurrentFragment].Len == 0) {
+      CurrentFragment++;
+    } else {
+      Fragment[CurrentFragment].Bulk += RxData->FragmentTable[0].FragmentLength;
+    }
+  }
+
+ON_EXIT:
+
+  if (Fragment != NULL) {
+    FreePool (Fragment);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.inf b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.inf
new file mode 100644
index 0000000000..4f11c6f0c8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.inf
@@ -0,0 +1,51 @@
+## @file
+#  This library instance provides TCP services by EFI TCPv4/TCPv6 Protocols.
+#  
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeTcpIoLib
+  MODULE_UNI_FILE                = DxeTcpIoLib.uni
+  FILE_GUID                      = D4608509-1AB0-4cc7-827A-AB8E1E7BD3E6
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = TcpIoLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeTcpIoLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  TcpIoLib
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+
+[Protocols]
+  gEfiTcp4ServiceBindingProtocolGuid            ## SOMETIMES_CONSUMES
+  gEfiTcp4ProtocolGuid                          ## SOMETIMES_CONSUMES
+  gEfiTcp6ServiceBindingProtocolGuid            ## SOMETIMES_CONSUMES
+  gEfiTcp6ProtocolGuid                          ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.uni b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.uni
new file mode 100644
index 0000000000..064d9e59b6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.uni
diff --git a/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.c b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.c
new file mode 100644
index 0000000000..e1d72f3f1c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.c
@@ -0,0 +1,1067 @@
+/** @file
+  Help functions to access UDP service, it is used by both the DHCP and MTFTP.
+
+Copyright (c) 2005 - 2012, 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<BR>
+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 <Uefi.h>
+
+#include <Protocol/Udp4.h>
+#include <Protocol/Udp6.h>
+
+#include <Library/UdpIoLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DpcLib.h>
+
+
+/**
+  Free a UDP_TX_TOKEN. The TX event is closed.
+
+  @param[in]  TxToken                 The UDP_TX_TOKEN to release.
+
+**/
+VOID
+UdpIoFreeTxToken (
+  IN UDP_TX_TOKEN           *TxToken
+  )
+{
+
+  if (TxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    gBS->CloseEvent (TxToken->Token.Udp4.Event);
+  } else if (TxToken->UdpIo->UdpVersion == UDP_IO_UDP6_VERSION) {
+    gBS->CloseEvent (TxToken->Token.Udp6.Event);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  FreePool (TxToken);
+}
+
+/**
+  Free a UDP_RX_TOKEN. The RX event is closed.
+
+  @param[in]  RxToken                 The UDP_RX_TOKEN to release.
+
+**/
+VOID
+UdpIoFreeRxToken (
+  IN UDP_RX_TOKEN           *RxToken
+  )
+{
+  if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    gBS->CloseEvent (RxToken->Token.Udp4.Event);
+  } else if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP6_VERSION) {
+    gBS->CloseEvent (RxToken->Token.Udp6.Event);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  FreePool (RxToken);
+}
+
+/**
+  The callback function when the packet is sent by UDP.
+
+  It will remove the packet from the local list then call
+  the packet owner's callback function set by UdpIoSendDatagram.
+
+  @param[in]  Context               The UDP TX Token.
+
+**/
+VOID
+EFIAPI
+UdpIoOnDgramSentDpc (
+  IN VOID                   *Context
+  )
+{
+  UDP_TX_TOKEN              *TxToken;
+
+  TxToken = (UDP_TX_TOKEN *) Context;
+  ASSERT (TxToken->Signature == UDP_IO_TX_SIGNATURE);
+  ASSERT ((TxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (TxToken->UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  RemoveEntryList (&TxToken->Link);
+
+  if (TxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    TxToken->CallBack (TxToken->Packet, NULL, TxToken->Token.Udp4.Status, TxToken->Context);
+  } else {
+    TxToken->CallBack (TxToken->Packet, NULL, TxToken->Token.Udp6.Status, TxToken->Context);
+  }
+
+  UdpIoFreeTxToken (TxToken);
+}
+
+/**
+  Request UdpIoOnDgramSentDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                 The event signaled.
+  @param[in]  Context               The UDP TX Token.
+
+**/
+VOID
+EFIAPI
+UdpIoOnDgramSent (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  //
+  // Request UdpIoOnDgramSentDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, UdpIoOnDgramSentDpc, Context);
+}
+
+/**
+  Recycle the received UDP data.
+
+  @param[in]  Context               The UDP_RX_TOKEN.
+
+**/
+VOID
+EFIAPI
+UdpIoRecycleDgram (
+  IN VOID                   *Context
+  )
+{
+  UDP_RX_TOKEN              *RxToken;
+
+  RxToken = (UDP_RX_TOKEN *) Context;
+
+  if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    gBS->SignalEvent (RxToken->Token.Udp4.Packet.RxData->RecycleSignal);
+  } else if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP6_VERSION) {
+    gBS->SignalEvent (RxToken->Token.Udp6.Packet.RxData->RecycleSignal);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  UdpIoFreeRxToken (RxToken);
+}
+
+/**
+  The event handle for UDP receive request.
+
+  It will build a NET_BUF from the recieved UDP data, then deliver it
+  to the receiver.
+
+  @param[in]  Context               The UDP RX token.
+
+**/
+VOID
+EFIAPI
+UdpIoOnDgramRcvdDpc (
+  IN VOID                   *Context
+  )
+{
+  EFI_STATUS                Status;
+  VOID                      *Token;
+  VOID                      *RxData;
+  VOID                      *Session;
+  UDP_RX_TOKEN              *RxToken;
+  UDP_END_POINT             EndPoint;
+  NET_BUF                   *Netbuf;
+
+  RxToken = (UDP_RX_TOKEN *) Context;
+
+  ZeroMem (&EndPoint, sizeof(UDP_END_POINT));
+
+  ASSERT ((RxToken->Signature == UDP_IO_RX_SIGNATURE) &&
+          (RxToken == RxToken->UdpIo->RecvRequest));
+
+  ASSERT ((RxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (RxToken->UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  //
+  // Clear the receive request first in case that the caller
+  // wants to restart the receive in the callback.
+  //
+  RxToken->UdpIo->RecvRequest = NULL;
+
+  if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    Token  = &RxToken->Token.Udp4;
+    RxData = ((EFI_UDP4_COMPLETION_TOKEN *) Token)->Packet.RxData;
+    Status = ((EFI_UDP4_COMPLETION_TOKEN *) Token)->Status;
+  } else {
+    Token  = &RxToken->Token.Udp6;
+    RxData = ((EFI_UDP6_COMPLETION_TOKEN *) Token)->Packet.RxData;
+    Status = ((EFI_UDP6_COMPLETION_TOKEN *) Token)->Status;
+  }
+
+  if (EFI_ERROR (Status) || RxData == NULL) {
+    if (Status != EFI_ABORTED) {
+      //
+      // Invoke the CallBack only if the reception is not actively aborted.
+      //
+      RxToken->CallBack (NULL, NULL, Status, RxToken->Context);
+    }
+
+    UdpIoFreeRxToken (RxToken);
+    return;
+  }
+
+  //
+  // Build a NET_BUF from the UDP receive data, then deliver it up.
+  //
+  if (RxToken->UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+
+    Netbuf = NetbufFromExt (
+               (NET_FRAGMENT *)((EFI_UDP4_RECEIVE_DATA *) RxData)->FragmentTable,
+               ((EFI_UDP4_RECEIVE_DATA *) RxData)->FragmentCount,
+               0,
+               (UINT32) RxToken->HeadLen,
+               UdpIoRecycleDgram,
+               RxToken
+               );
+
+    if (Netbuf == NULL) {
+      gBS->SignalEvent (((EFI_UDP4_RECEIVE_DATA *) RxData)->RecycleSignal);
+      RxToken->CallBack (NULL, NULL, EFI_OUT_OF_RESOURCES, RxToken->Context);
+
+      UdpIoFreeRxToken (RxToken);
+      return;
+    }
+
+    Session             = &((EFI_UDP4_RECEIVE_DATA *) RxData)->UdpSession;
+    EndPoint.LocalPort  = ((EFI_UDP4_SESSION_DATA *) Session)->DestinationPort;
+    EndPoint.RemotePort = ((EFI_UDP4_SESSION_DATA *) Session)->SourcePort;
+
+    CopyMem (
+      &EndPoint.LocalAddr,
+      &((EFI_UDP4_SESSION_DATA *) Session)->DestinationAddress,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+
+    CopyMem (
+      &EndPoint.RemoteAddr,
+      &((EFI_UDP4_SESSION_DATA *) Session)->SourceAddress,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+
+    EndPoint.LocalAddr.Addr[0]  = NTOHL (EndPoint.LocalAddr.Addr[0]);
+    EndPoint.RemoteAddr.Addr[0] = NTOHL (EndPoint.RemoteAddr.Addr[0]);
+  } else {
+
+    Netbuf = NetbufFromExt (
+               (NET_FRAGMENT *)((EFI_UDP6_RECEIVE_DATA *) RxData)->FragmentTable,
+               ((EFI_UDP6_RECEIVE_DATA *) RxData)->FragmentCount,
+               0,
+               (UINT32) RxToken->HeadLen,
+               UdpIoRecycleDgram,
+               RxToken
+               );
+
+    if (Netbuf == NULL) {
+      gBS->SignalEvent (((EFI_UDP6_RECEIVE_DATA *) RxData)->RecycleSignal);
+      RxToken->CallBack (NULL, NULL, EFI_OUT_OF_RESOURCES, RxToken->Context);
+
+      UdpIoFreeRxToken (RxToken);
+      return;
+    }
+
+    Session             = &((EFI_UDP6_RECEIVE_DATA *) RxData)->UdpSession;
+    EndPoint.LocalPort  = ((EFI_UDP6_SESSION_DATA *) Session)->DestinationPort;
+    EndPoint.RemotePort = ((EFI_UDP6_SESSION_DATA *) Session)->SourcePort;
+
+    CopyMem (
+      &EndPoint.LocalAddr,
+      &((EFI_UDP6_SESSION_DATA *) Session)->DestinationAddress,
+      sizeof (EFI_IPv6_ADDRESS)
+      );
+
+    CopyMem (
+      &EndPoint.RemoteAddr,
+      &((EFI_UDP6_SESSION_DATA *) Session)->SourceAddress,
+      sizeof (EFI_IPv6_ADDRESS)
+      );
+
+    Ip6Swap128 (&EndPoint.LocalAddr.v6);
+    Ip6Swap128 (&EndPoint.RemoteAddr.v6);
+  }
+
+  RxToken->CallBack (Netbuf, &EndPoint, EFI_SUCCESS, RxToken->Context);
+}
+
+/**
+  Request UdpIoOnDgramRcvdDpc() as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                 The UDP receive request event.
+  @param[in]  Context               The UDP RX token.
+
+**/
+VOID
+EFIAPI
+UdpIoOnDgramRcvd (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  //
+  // Request UdpIoOnDgramRcvdDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, UdpIoOnDgramRcvdDpc, Context);
+}
+
+/**
+  Create a UDP_RX_TOKEN to wrap the request.
+
+  @param[in]  UdpIo                 The UdpIo to receive packets from.
+  @param[in]  CallBack              The function to call when receive finished.
+  @param[in]  Context               The opaque parameter to the CallBack.
+  @param[in]  HeadLen               The head length to reserver for the packet.
+
+  @return The Wrapped request or NULL if failed to allocate resources or some errors happened.
+
+**/
+UDP_RX_TOKEN *
+UdpIoCreateRxToken (
+  IN UDP_IO                 *UdpIo,
+  IN UDP_IO_CALLBACK        CallBack,
+  IN VOID                   *Context,
+  IN UINT32                 HeadLen
+  )
+{
+  UDP_RX_TOKEN              *Token;
+  EFI_STATUS                Status;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  Token = AllocatePool (sizeof (UDP_RX_TOKEN));
+
+  if (Token == NULL) {
+    return NULL;
+  }
+
+  Token->Signature              = UDP_IO_RX_SIGNATURE;
+  Token->UdpIo                  = UdpIo;
+  Token->CallBack               = CallBack;
+  Token->Context                = Context;
+  Token->HeadLen                = HeadLen;
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+
+    Token->Token.Udp4.Status        = EFI_NOT_READY;
+    Token->Token.Udp4.Packet.RxData = NULL;
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    UdpIoOnDgramRcvd,
+                    Token,
+                    &Token->Token.Udp4.Event
+                    );
+    } else {
+
+    Token->Token.Udp6.Status        = EFI_NOT_READY;
+    Token->Token.Udp6.Packet.RxData = NULL;
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    UdpIoOnDgramRcvd,
+                    Token,
+                    &Token->Token.Udp6.Event
+                    );
+  }
+
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Token);
+    return NULL;
+  }
+
+  return Token;
+}
+
+/**
+  Wrap a transmit request into a new created UDP_TX_TOKEN.
+
+  @param[in]  UdpIo                 The UdpIo to send packet to.
+  @param[in]  Packet                The user's packet.
+  @param[in]  EndPoint              The local and remote access point.
+  @param[in]  Gateway               The overrided next hop.
+  @param[in]  CallBack              The function to call when transmission completed.
+  @param[in]  Context               The opaque parameter to the call back.
+
+  @return The wrapped transmission request or NULL if failed to allocate resources
+          or for some errors.
+
+**/
+UDP_TX_TOKEN *
+UdpIoCreateTxToken (
+  IN UDP_IO                 *UdpIo,
+  IN NET_BUF                *Packet,
+  IN UDP_END_POINT          *EndPoint OPTIONAL,
+  IN EFI_IP_ADDRESS         *Gateway  OPTIONAL,
+  IN UDP_IO_CALLBACK        CallBack,
+  IN VOID                   *Context
+  )
+{
+  UDP_TX_TOKEN              *TxToken;
+  VOID                      *Token;
+  VOID                      *Data;
+  EFI_STATUS                Status;
+  UINT32                    Count;
+  UINTN                     Size;
+  IP4_ADDR                  Ip;
+
+  ASSERT (Packet != NULL);
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    Size = sizeof (UDP_TX_TOKEN) + sizeof (EFI_UDP4_FRAGMENT_DATA) * (Packet->BlockOpNum - 1);
+  } else {
+    Size = sizeof (UDP_TX_TOKEN) + sizeof (EFI_UDP6_FRAGMENT_DATA) * (Packet->BlockOpNum - 1);
+  }
+
+  TxToken = AllocatePool (Size);
+
+  if (TxToken == NULL) {
+    return NULL;
+  }
+
+  TxToken->Signature = UDP_IO_TX_SIGNATURE;
+  InitializeListHead (&TxToken->Link);
+
+  TxToken->UdpIo     = UdpIo;
+  TxToken->CallBack  = CallBack;
+  TxToken->Packet    = Packet;
+  TxToken->Context   = Context;
+
+  Token              = &(TxToken->Token);
+  Count              = Packet->BlockOpNum;
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+
+    ((EFI_UDP4_COMPLETION_TOKEN *) Token)->Status = EFI_NOT_READY;
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    UdpIoOnDgramSent,
+                    TxToken,
+                    &((EFI_UDP4_COMPLETION_TOKEN *) Token)->Event
+                    );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (TxToken);
+      return NULL;
+    }
+
+    Data = &(TxToken->Data.Udp4);
+    ((EFI_UDP4_COMPLETION_TOKEN *) Token)->Packet.TxData = Data;
+
+    ((EFI_UDP4_TRANSMIT_DATA *) Data)->UdpSessionData    = NULL;
+    ((EFI_UDP4_TRANSMIT_DATA *) Data)->GatewayAddress    = NULL;
+    ((EFI_UDP4_TRANSMIT_DATA *) Data)->DataLength        = Packet->TotalSize;
+
+    NetbufBuildExt (
+      Packet,
+      (NET_FRAGMENT *)((EFI_UDP4_TRANSMIT_DATA *) Data)->FragmentTable,
+      &Count
+      );
+
+    ((EFI_UDP4_TRANSMIT_DATA *) Data)->FragmentCount     = Count;
+
+    if (EndPoint != NULL) {
+      Ip = HTONL (EndPoint->LocalAddr.Addr[0]);
+      CopyMem (
+        &TxToken->Session.Udp4.SourceAddress,
+        &Ip,
+        sizeof (EFI_IPv4_ADDRESS)
+        );
+
+      Ip = HTONL (EndPoint->RemoteAddr.Addr[0]);
+      CopyMem (
+        &TxToken->Session.Udp4.DestinationAddress,
+        &Ip,
+        sizeof (EFI_IPv4_ADDRESS)
+        );
+
+      TxToken->Session.Udp4.SourcePort                   = EndPoint->LocalPort;
+      TxToken->Session.Udp4.DestinationPort              = EndPoint->RemotePort;
+      ((EFI_UDP4_TRANSMIT_DATA *) Data)->UdpSessionData  = &(TxToken->Session.Udp4);
+    }
+
+    if (Gateway != NULL && (Gateway->Addr[0] != 0)) {
+      Ip = HTONL (Gateway->Addr[0]);
+      CopyMem (&TxToken->Gateway, &Ip, sizeof (EFI_IPv4_ADDRESS));
+      ((EFI_UDP4_TRANSMIT_DATA *) Data)->GatewayAddress = &TxToken->Gateway;
+    }
+
+  } else {
+
+    ((EFI_UDP6_COMPLETION_TOKEN *) Token)->Status = EFI_NOT_READY;
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    UdpIoOnDgramSent,
+                    TxToken,
+                    &((EFI_UDP6_COMPLETION_TOKEN *) Token)->Event
+                    );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (TxToken);
+      return NULL;
+    }
+
+    Data = &(TxToken->Data.Udp6);
+    ((EFI_UDP6_COMPLETION_TOKEN *) Token)->Packet.TxData  = Data;
+    ((EFI_UDP6_TRANSMIT_DATA *) Data)->UdpSessionData     = NULL;
+    ((EFI_UDP6_TRANSMIT_DATA *) Data)->DataLength         = Packet->TotalSize;
+
+    NetbufBuildExt (
+      Packet,
+      (NET_FRAGMENT *)((EFI_UDP6_TRANSMIT_DATA *) Data)->FragmentTable,
+      &Count
+      );
+
+    ((EFI_UDP6_TRANSMIT_DATA *) Data)->FragmentCount      = Count;
+
+    if (EndPoint != NULL) {
+      CopyMem (
+        &TxToken->Session.Udp6.SourceAddress,
+        &EndPoint->LocalAddr.v6,
+        sizeof(EFI_IPv6_ADDRESS)
+        );
+
+      CopyMem (
+        &TxToken->Session.Udp6.DestinationAddress,
+        &EndPoint->RemoteAddr.v6,
+        sizeof(EFI_IPv6_ADDRESS)
+        );
+
+      TxToken->Session.Udp6.SourcePort                   = EndPoint->LocalPort;
+      TxToken->Session.Udp6.DestinationPort              = EndPoint->RemotePort;
+      ((EFI_UDP6_TRANSMIT_DATA *) Data)->UdpSessionData  = &(TxToken->Session.Udp6);
+    }
+  }
+
+  return TxToken;
+}
+
+/**
+  Creates a UDP_IO to access the UDP service. It creates and configures
+  a UDP child.
+
+  It locates the UDP service binding prototype on the Controller parameter
+  uses the UDP service binding prototype to create a UDP child (also known as
+  a UDP instance) configures the UDP child by calling Configure function prototype.
+  Any failures in creating or configuring the UDP child return NULL for failure.
+
+  @param[in]  Controller            The controller that has the UDP service binding.
+                                    protocol installed.
+  @param[in]  ImageHandle           The image handle for the driver.
+  @param[in]  Configure             The function to configure the created UDP child.
+  @param[in]  UdpVersion            The UDP protocol version, UDP4 or UDP6.
+  @param[in]  Context               The opaque parameter for the Configure funtion.
+
+  @return Newly-created UDP_IO or NULL if failed.
+
+**/
+UDP_IO *
+EFIAPI
+UdpIoCreateIo (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            ImageHandle,
+  IN  UDP_IO_CONFIG         Configure,
+  IN  UINT8                 UdpVersion,
+  IN  VOID                  *Context
+  )
+{
+  UDP_IO                    *UdpIo;
+  EFI_STATUS                Status;
+
+  ASSERT (Configure != NULL);
+  ASSERT ((UdpVersion == UDP_IO_UDP4_VERSION) || (UdpVersion == UDP_IO_UDP6_VERSION));
+
+  UdpIo = AllocatePool (sizeof (UDP_IO));
+
+  if (UdpIo == NULL) {
+    return NULL;
+  }
+
+  UdpIo->UdpVersion   = UdpVersion;
+  UdpIo->Signature    = UDP_IO_SIGNATURE;
+  InitializeListHead (&UdpIo->Link);
+  UdpIo->RefCnt       = 1;
+
+  UdpIo->Controller   = Controller;
+  UdpIo->Image        = ImageHandle;
+
+  InitializeListHead (&UdpIo->SentDatagram);
+  UdpIo->RecvRequest  = NULL;
+  UdpIo->UdpHandle    = NULL;
+
+  if (UdpVersion == UDP_IO_UDP4_VERSION) {
+    //
+    // Create a UDP child then open and configure it
+    //
+    Status = NetLibCreateServiceChild (
+               Controller,
+               ImageHandle,
+               &gEfiUdp4ServiceBindingProtocolGuid,
+               &UdpIo->UdpHandle
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto FREE_MEM;
+    }
+
+    Status = gBS->OpenProtocol (
+                    UdpIo->UdpHandle,
+                    &gEfiUdp4ProtocolGuid,
+                    (VOID **) &UdpIo->Protocol.Udp4,
+                    ImageHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+
+    if (EFI_ERROR (Status)) {
+      goto FREE_CHILD;
+    }
+
+    if (EFI_ERROR (Configure (UdpIo, Context))) {
+      goto CLOSE_PROTOCOL;
+    }
+
+    Status = UdpIo->Protocol.Udp4->GetModeData (
+                                     UdpIo->Protocol.Udp4,
+                                     NULL,
+                                     NULL,
+                                     NULL,
+                                     &UdpIo->SnpMode
+                                     );
+
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PROTOCOL;
+    }
+
+  } else {
+
+    Status = NetLibCreateServiceChild (
+               Controller,
+               ImageHandle,
+               &gEfiUdp6ServiceBindingProtocolGuid,
+               &UdpIo->UdpHandle
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto FREE_MEM;
+    }
+
+    Status = gBS->OpenProtocol (
+                    UdpIo->UdpHandle,
+                    &gEfiUdp6ProtocolGuid,
+                    (VOID **) &UdpIo->Protocol.Udp6,
+                    ImageHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+
+    if (EFI_ERROR (Status)) {
+      goto FREE_CHILD;
+    }
+
+    if (EFI_ERROR (Configure (UdpIo, Context))) {
+      goto CLOSE_PROTOCOL;
+    }
+
+    Status = UdpIo->Protocol.Udp6->GetModeData (
+                                     UdpIo->Protocol.Udp6,
+                                     NULL,
+                                     NULL,
+                                     NULL,
+                                     &UdpIo->SnpMode
+                                     );
+
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_PROTOCOL;
+    }
+  }
+
+  return UdpIo;
+
+CLOSE_PROTOCOL:
+  if (UdpVersion == UDP_IO_UDP4_VERSION) {
+    gBS->CloseProtocol (UdpIo->UdpHandle, &gEfiUdp4ProtocolGuid, ImageHandle, Controller);
+  } else {
+    gBS->CloseProtocol (UdpIo->UdpHandle, &gEfiUdp6ProtocolGuid, ImageHandle, Controller);
+  }
+
+FREE_CHILD:
+  if (UdpVersion == UDP_IO_UDP4_VERSION) {
+    NetLibDestroyServiceChild (
+      Controller,
+      ImageHandle,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      UdpIo->UdpHandle
+      );
+  } else {
+    NetLibDestroyServiceChild (
+      Controller,
+      ImageHandle,
+      &gEfiUdp6ServiceBindingProtocolGuid,
+      UdpIo->UdpHandle
+      );
+  }
+
+FREE_MEM:
+  FreePool (UdpIo);
+  return NULL;
+}
+
+/**
+  Cancel all the sent datagram that pass the selection criteria of ToCancel.
+  If ToCancel is NULL, all the datagrams are cancelled.
+
+  @param[in]  UdpIo                 The UDP_IO to cancel packet.
+  @param[in]  IoStatus              The IoStatus to return to the packet owners.
+  @param[in]  ToCancel              The select funtion to test whether to cancel this
+                                    packet or not.
+  @param[in]  Context               The opaque parameter to the ToCancel.
+
+**/
+VOID
+EFIAPI
+UdpIoCancelDgrams (
+  IN UDP_IO                 *UdpIo,
+  IN EFI_STATUS             IoStatus,
+  IN UDP_IO_TO_CANCEL       ToCancel,        OPTIONAL
+  IN VOID                   *Context
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  UDP_TX_TOKEN              *TxToken;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &UdpIo->SentDatagram) {
+    TxToken = NET_LIST_USER_STRUCT (Entry, UDP_TX_TOKEN, Link);
+
+    if ((ToCancel == NULL) || (ToCancel (TxToken, Context))) {
+
+      if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+        UdpIo->Protocol.Udp4->Cancel (UdpIo->Protocol.Udp4, &TxToken->Token.Udp4);
+      } else {
+        UdpIo->Protocol.Udp6->Cancel (UdpIo->Protocol.Udp6, &TxToken->Token.Udp6);
+      }
+    }
+  }
+}
+
+/**
+  Free the UDP_IO and all its related resources.
+
+  The function will cancel all sent datagram and receive request.
+
+  @param[in]  UdpIo             The UDP_IO to free.
+
+  @retval EFI_SUCCESS           The UDP_IO is freed.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoFreeIo (
+  IN  UDP_IO           *UdpIo
+  )
+{
+  UDP_RX_TOKEN         *RxToken;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  //
+  // Cancel all the sent datagram and receive requests. The
+  // callbacks of transmit requests are executed to allow the
+  // caller to release the resource. The callback of receive
+  // request are NOT executed. This is because it is most
+  // likely that the current user of the UDP IO port is closing
+  // itself.
+  //
+  UdpIoCancelDgrams (UdpIo, EFI_ABORTED, NULL, NULL);
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+
+    if ((RxToken = UdpIo->RecvRequest) != NULL) {
+      UdpIo->Protocol.Udp4->Cancel (UdpIo->Protocol.Udp4, &RxToken->Token.Udp4);
+    }
+
+    //
+    // Close then destroy the Udp4 child
+    //
+    gBS->CloseProtocol (
+           UdpIo->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           UdpIo->Image,
+           UdpIo->Controller
+           );
+
+    NetLibDestroyServiceChild (
+      UdpIo->Controller,
+      UdpIo->Image,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      UdpIo->UdpHandle
+      );
+
+  } else {
+
+    if ((RxToken = UdpIo->RecvRequest) != NULL) {
+      UdpIo->Protocol.Udp6->Cancel (UdpIo->Protocol.Udp6, &RxToken->Token.Udp6);
+    }
+
+    //
+    // Close then destroy the Udp6 child
+    //
+    gBS->CloseProtocol (
+           UdpIo->UdpHandle,
+           &gEfiUdp6ProtocolGuid,
+           UdpIo->Image,
+           UdpIo->Controller
+           );
+
+    NetLibDestroyServiceChild (
+      UdpIo->Controller,
+      UdpIo->Image,
+      &gEfiUdp6ServiceBindingProtocolGuid,
+      UdpIo->UdpHandle
+      );
+    }
+
+  if (!IsListEmpty(&UdpIo->Link)) {
+    RemoveEntryList (&UdpIo->Link);
+  }
+
+  FreePool (UdpIo);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Clean up the UDP_IO without freeing it. The function is called when
+  user wants to re-use the UDP_IO later.
+
+  It will release all the transmitted datagrams and receive request. It will
+  also configure NULL for the UDP instance.
+
+  @param[in]  UdpIo                 The UDP_IO to clean up.
+
+**/
+VOID
+EFIAPI
+UdpIoCleanIo (
+  IN  UDP_IO                *UdpIo
+  )
+{
+  UDP_RX_TOKEN              *RxToken;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  //
+  // Cancel all the sent datagram and receive requests.
+  //
+  UdpIoCancelDgrams (UdpIo, EFI_ABORTED, NULL, NULL);
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    if ((RxToken = UdpIo->RecvRequest) != NULL) {
+      UdpIo->Protocol.Udp4->Cancel (UdpIo->Protocol.Udp4, &RxToken->Token.Udp4);
+    }
+
+    UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, NULL);
+
+  } else {
+    if ((RxToken = UdpIo->RecvRequest) != NULL) {
+      UdpIo->Protocol.Udp6->Cancel (UdpIo->Protocol.Udp6, &RxToken->Token.Udp6);
+    }
+
+    UdpIo->Protocol.Udp6->Configure (UdpIo->Protocol.Udp6, NULL);
+  }
+}
+
+/**
+  Send a packet through the UDP_IO.
+
+  The packet will be wrapped in UDP_TX_TOKEN. Function Callback will be called
+  when the packet is sent. The optional parameter EndPoint overrides the default
+  address pair if specified.
+
+  @param[in]  UdpIo                 The UDP_IO to send the packet through.
+  @param[in]  Packet                The packet to send.
+  @param[in]  EndPoint              The local and remote access point. Override the
+                                    default address pair set during configuration.
+  @param[in]  Gateway               The gateway to use.
+  @param[in]  CallBack              The function being called when packet is
+                                    transmitted or failed.
+  @param[in]  Context               The opaque parameter passed to CallBack.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource for the packet.
+  @retval EFI_SUCCESS           The packet is successfully delivered to UDP  for
+                                transmission.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoSendDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  NET_BUF               *Packet,
+  IN  UDP_END_POINT         *EndPoint OPTIONAL,
+  IN  EFI_IP_ADDRESS        *Gateway  OPTIONAL,
+  IN  UDP_IO_CALLBACK       CallBack,
+  IN  VOID                  *Context
+  )
+{
+  UDP_TX_TOKEN              *TxToken;
+  EFI_STATUS                Status;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  TxToken = UdpIoCreateTxToken (UdpIo, Packet, EndPoint, Gateway, CallBack, Context);
+
+  if (TxToken == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Insert the tx token into SendDatagram list before transmitting it. Remove
+  // it from the list if the returned status is not EFI_SUCCESS.
+  //
+  InsertHeadList (&UdpIo->SentDatagram, &TxToken->Link);
+
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    Status = UdpIo->Protocol.Udp4->Transmit (UdpIo->Protocol.Udp4, &TxToken->Token.Udp4);
+  } else {
+    Status = UdpIo->Protocol.Udp6->Transmit (UdpIo->Protocol.Udp6, &TxToken->Token.Udp6);
+  }
+
+  if (EFI_ERROR (Status)) {
+    RemoveEntryList (&TxToken->Link);
+    UdpIoFreeTxToken (TxToken);
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The select function to cancel a single sent datagram.
+
+  @param[in]  Token                 The UDP_TX_TOKEN to test against
+  @param[in]  Context               The NET_BUF of the sent datagram
+
+  @retval TRUE              The packet is to be cancelled.
+  @retval FALSE             The packet is not to be cancelled.
+**/
+BOOLEAN
+EFIAPI
+UdpIoCancelSingleDgram (
+  IN UDP_TX_TOKEN           *Token,
+  IN VOID                   *Context
+  )
+{
+  NET_BUF                   *Packet;
+
+  Packet = (NET_BUF *) Context;
+
+  if (Token->Packet == Packet) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Cancel a single sent datagram.
+
+  @param[in]  UdpIo                 The UDP_IO to cancel the packet from
+  @param[in]  Packet                The packet to cancel
+
+**/
+VOID
+EFIAPI
+UdpIoCancelSentDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  NET_BUF               *Packet
+  )
+{
+  UdpIoCancelDgrams (UdpIo, EFI_ABORTED, UdpIoCancelSingleDgram, Packet);
+}
+
+/**
+  Issue a receive request to the UDP_IO.
+
+  This function is called when upper-layer needs packet from UDP for processing.
+  Only one receive request is acceptable at a time so a common usage model is
+  to invoke this function inside its Callback function when the former packet
+  is processed.
+
+  @param[in]  UdpIo                 The UDP_IO to receive the packet from.
+  @param[in]  CallBack              The call back function to execute when the packet
+                                    is received.
+  @param[in]  Context               The opaque context passed to Callback.
+  @param[in]  HeadLen               The length of the upper-layer's protocol header.
+
+  @retval EFI_ALREADY_STARTED   There is already a pending receive request. Only
+                                one receive request is supported at a time.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate needed resources.
+  @retval EFI_SUCCESS           The receive request is issued successfully.
+  @retval EFI_UNSUPPORTED       The UDP version in UDP_IO is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpIoRecvDatagram (
+  IN  UDP_IO                *UdpIo,
+  IN  UDP_IO_CALLBACK       CallBack,
+  IN  VOID                  *Context,
+  IN  UINT32                HeadLen
+  )
+{
+  UDP_RX_TOKEN              *RxToken;
+  EFI_STATUS                Status;
+
+  ASSERT ((UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) ||
+          (UdpIo->UdpVersion == UDP_IO_UDP6_VERSION));
+
+  if (UdpIo->RecvRequest != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  RxToken = UdpIoCreateRxToken (UdpIo, CallBack, Context, HeadLen);
+
+  if (RxToken == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  UdpIo->RecvRequest = RxToken;
+  if (UdpIo->UdpVersion == UDP_IO_UDP4_VERSION) {
+    Status = UdpIo->Protocol.Udp4->Receive (UdpIo->Protocol.Udp4, &RxToken->Token.Udp4);
+  } else {
+    Status = UdpIo->Protocol.Udp6->Receive (UdpIo->Protocol.Udp6, &RxToken->Token.Udp6);
+  }
+
+  if (EFI_ERROR (Status)) {
+    UdpIo->RecvRequest = NULL;
+    UdpIoFreeRxToken (RxToken);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.inf b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.inf
new file mode 100644
index 0000000000..a9683c9a74
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.inf
@@ -0,0 +1,53 @@
+## @file
+#  This library instance provides UDP services by consuming EFI UDPv4/UDPv6 Protocols.
+#  
+#  Copyright (c) 2008 - 2014, 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.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeUpdIoLib
+  MODULE_UNI_FILE                = DxeUpdIoLib.uni
+  FILE_GUID                      = 7E615AA1-41EE-49d4-B7E9-1D7A60AA5C8D
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = UdpIoLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DxeUdpIoLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UdpIoLib
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  DpcLib
+
+[Protocols]
+  gEfiUdp4ServiceBindingProtocolGuid            ## SOMETIMES_CONSUMES
+  gEfiUdp4ProtocolGuid                          ## SOMETIMES_CONSUMES
+  gEfiUdp6ServiceBindingProtocolGuid            ## SOMETIMES_CONSUMES
+  gEfiUdp6ProtocolGuid                          ## SOMETIMES_CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUpdIoLib.uni b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUpdIoLib.uni
new file mode 100644
index 0000000000..0e96b4b76b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/DxeUdpIoLib/DxeUpdIoLib.uni
diff --git a/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.c b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.c
new file mode 100644
index 0000000000..5d7b52c06e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.c
@@ -0,0 +1,139 @@
+/** @file
+
+Copyright (c) 2010, 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 <Uefi.h>
+#include <Library/DebugLib.h>
+
+/**
+  This function will save confidential information to lockbox.
+
+  @param Guid       the guid to identify the confidential information
+  @param Buffer     the address of the confidential information
+  @param Length     the length of the confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0
+  @retval RETURN_ALREADY_STARTED    the requested GUID already exist.
+  @retval RETURN_OUT_OF_RESOURCES   no enough resource to save the information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SaveLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  This function will set lockbox attributes.
+
+  @param Guid       the guid to identify the confidential information
+  @param Attributes the attributes of the lockbox
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  attributes is invalid.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SetLockBoxAttributes (
+  IN  GUID                        *Guid,
+  IN  UINT64                      Attributes
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  This function will update confidential information to lockbox.
+
+  @param Guid   the guid to identify the original confidential information
+  @param Offset the offset of the original confidential information
+  @param Buffer the address of the updated confidential information
+  @param Length the length of the updated confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_BUFFER_TOO_SMALL   the original buffer to too small to hold new information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+UpdateLockBox (
+  IN  GUID                        *Guid,
+  IN  UINTN                       Offset,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  This function will restore confidential information from lockbox.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or one of Buffer and Length is NULL.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_ACCESS_DENIED      not allow to restore to the address
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  )
+{
+  return RETURN_SUCCESS;
+}
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreAllLockBoxInPlace (
+  VOID
+  )
+{
+  return RETURN_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.inf b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.inf
new file mode 100644
index 0000000000..1018a69e81
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.inf
@@ -0,0 +1,39 @@
+## @file
+#  NULL LockBox library instance.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = LockBoxNullLib
+  MODULE_UNI_FILE                = LockBoxNullLib.uni
+  FILE_GUID                      = 0BA38EBD-E190-4df7-8EC4-0A6E2B43772D
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = LockBoxLib|PEIM DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_DRIVER UEFI_APPLICATION DXE_SMM_DRIVER
+
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 
+#
+
+[Sources]
+  LockBoxNullLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.uni b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.uni
new file mode 100644
index 0000000000..5466eb1b67
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.uni
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/F86GuidedSectionExtraction.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/F86GuidedSectionExtraction.c
new file mode 100644
index 0000000000..ada9a809fa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/F86GuidedSectionExtraction.c
@@ -0,0 +1,218 @@
+/** @file
+  LZMA Decompress GUIDed Section Extraction Library, which produces LZMA custom 
+  decompression algorithm with the converter for the different arch code.
+  It wraps Lzma decompress interfaces to GUIDed Section Extraction interfaces
+  and registers them into GUIDed handler table.
+
+  Copyright (c) 2012, 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 "LzmaDecompressLibInternal.h"
+#include "Sdk/C/Bra.h"
+
+/**
+  Examines a GUIDed section and returns the size of the decoded buffer and the
+  size of an scratch buffer required to actually decode the data in a GUIDed section.
+
+  Examines a GUIDed section specified by InputSection.  
+  If GUID for InputSection does not match the GUID that this handler supports,
+  then RETURN_UNSUPPORTED is returned.  
+  If the required information can not be retrieved from InputSection,
+  then RETURN_INVALID_PARAMETER is returned.
+  If the GUID of InputSection does match the GUID that this handler supports,
+  then the size required to hold the decoded buffer is returned in OututBufferSize,
+  the size of an optional scratch buffer is returned in ScratchSize, and the Attributes field
+  from EFI_GUID_DEFINED_SECTION header of InputSection is returned in SectionAttribute.
+  
+  If InputSection is NULL, then ASSERT().
+  If OutputBufferSize is NULL, then ASSERT().
+  If ScratchBufferSize is NULL, then ASSERT().
+  If SectionAttribute is NULL, then ASSERT().
+
+
+  @param[in]  InputSection       A pointer to a GUIDed section of an FFS formatted file.
+  @param[out] OutputBufferSize   A pointer to the size, in bytes, of an output buffer required
+                                 if the buffer specified by InputSection were decoded.
+  @param[out] ScratchBufferSize  A pointer to the size, in bytes, required as scratch space
+                                 if the buffer specified by InputSection were decoded.
+  @param[out] SectionAttribute   A pointer to the attributes of the GUIDed section. See the Attributes
+                                 field of EFI_GUID_DEFINED_SECTION in the PI Specification.
+
+  @retval  RETURN_SUCCESS            The information about InputSection was returned.
+  @retval  RETURN_UNSUPPORTED        The section specified by InputSection does not match the GUID this handler supports.
+  @retval  RETURN_INVALID_PARAMETER  The information can not be retrieved from the section specified by InputSection.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaArchGuidedSectionGetInfo (
+  IN  CONST VOID  *InputSection,
+  OUT UINT32      *OutputBufferSize,
+  OUT UINT32      *ScratchBufferSize,
+  OUT UINT16      *SectionAttribute
+  )
+{
+  ASSERT (InputSection != NULL);
+  ASSERT (OutputBufferSize != NULL);
+  ASSERT (ScratchBufferSize != NULL);
+  ASSERT (SectionAttribute != NULL);
+
+  if (IS_SECTION2 (InputSection)) {
+    if (!CompareGuid (
+        &gLzmaF86CustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    *SectionAttribute = ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes;
+
+    return LzmaUefiDecompressGetInfo (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             OutputBufferSize,
+             ScratchBufferSize
+             );
+  } else {
+    if (!CompareGuid (
+        &gLzmaF86CustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    *SectionAttribute = ((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes;
+
+    return LzmaUefiDecompressGetInfo (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             OutputBufferSize,
+             ScratchBufferSize
+             );
+  }
+}
+
+/**
+  Decompress a LZAM compressed GUIDed section into a caller allocated output buffer.
+  
+  Decodes the GUIDed section specified by InputSection.  
+  If GUID for InputSection does not match the GUID that this handler supports, then RETURN_UNSUPPORTED is returned.  
+  If the data in InputSection can not be decoded, then RETURN_INVALID_PARAMETER is returned.
+  If the GUID of InputSection does match the GUID that this handler supports, then InputSection
+  is decoded into the buffer specified by OutputBuffer and the authentication status of this
+  decode operation is returned in AuthenticationStatus.  If the decoded buffer is identical to the
+  data in InputSection, then OutputBuffer is set to point at the data in InputSection.  Otherwise,
+  the decoded data will be placed in caller allocated buffer specified by OutputBuffer.
+  
+  If InputSection is NULL, then ASSERT().
+  If OutputBuffer is NULL, then ASSERT().
+  If ScratchBuffer is NULL and this decode operation requires a scratch buffer, then ASSERT().
+  If AuthenticationStatus is NULL, then ASSERT().
+
+
+  @param[in]  InputSection  A pointer to a GUIDed section of an FFS formatted file.
+  @param[out] OutputBuffer  A pointer to a buffer that contains the result of a decode operation. 
+  @param[out] ScratchBuffer A caller allocated buffer that may be required by this function
+                            as a scratch buffer to perform the decode operation. 
+  @param[out] AuthenticationStatus 
+                            A pointer to the authentication status of the decoded output buffer.
+                            See the definition of authentication status in the EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI
+                            section of the PI Specification. EFI_AUTH_STATUS_PLATFORM_OVERRIDE must
+                            never be set by this handler.
+
+  @retval  RETURN_SUCCESS            The buffer specified by InputSection was decoded.
+  @retval  RETURN_UNSUPPORTED        The section specified by InputSection does not match the GUID this handler supports.
+  @retval  RETURN_INVALID_PARAMETER  The section specified by InputSection can not be decoded.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaArchGuidedSectionExtraction (
+  IN CONST  VOID    *InputSection,
+  OUT       VOID    **OutputBuffer,
+  OUT       VOID    *ScratchBuffer,        OPTIONAL
+  OUT       UINT32  *AuthenticationStatus
+  )
+{
+  EFI_GUID          *InputGuid;
+  VOID              *Source;
+  UINTN             SourceSize;
+  EFI_STATUS        Status;
+  UINT32            X86State;
+  UINT32            OutputBufferSize;
+  UINT32            ScratchBufferSize;
+  
+  ASSERT (OutputBuffer != NULL);
+  ASSERT (InputSection != NULL);
+
+  if (IS_SECTION2 (InputSection)) {
+    InputGuid  = &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid);
+    Source     = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+    SourceSize = SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+  } else {
+    InputGuid  = &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid);
+    Source     = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+    SourceSize = SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+  }
+
+  if (!CompareGuid (&gLzmaF86CustomDecompressGuid, InputGuid)) {
+    return RETURN_INVALID_PARAMETER;
+  }
+
+  //
+  // Authentication is set to Zero, which may be ignored.
+  //
+  *AuthenticationStatus = 0;
+
+  Status = LzmaUefiDecompress (
+           Source,
+           SourceSize,
+           *OutputBuffer,
+           ScratchBuffer
+           );
+
+  //
+  // After decompress, the data need to be converted to the raw data. 
+  //
+  if (!EFI_ERROR (Status)) {
+    Status = LzmaUefiDecompressGetInfo (
+             Source,
+             (UINT32) SourceSize,
+             &OutputBufferSize,
+             &ScratchBufferSize
+             );
+    
+    if (!EFI_ERROR (Status)) {
+      x86_Convert_Init(X86State);
+      x86_Convert(*OutputBuffer, OutputBufferSize, 0, &X86State, 0);
+    }
+  }
+  
+  return Status;
+}
+
+
+/**
+  Register LzmaArchDecompress and LzmaArchDecompressGetInfo handlers with LzmaF86CustomDecompressGuid.
+
+  @retval  RETURN_SUCCESS            Register successfully.
+  @retval  RETURN_OUT_OF_RESOURCES   No enough memory to store this handler.
+**/
+EFI_STATUS
+EFIAPI
+LzmaArchDecompressLibConstructor (
+  )
+{
+  return ExtractGuidedSectionRegisterHandlers (
+          &gLzmaF86CustomDecompressGuid,
+          LzmaArchGuidedSectionGetInfo,
+          LzmaArchGuidedSectionExtraction
+          );
+}
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/GuidedSectionExtraction.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/GuidedSectionExtraction.c
new file mode 100644
index 0000000000..f19e0d28cd
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/GuidedSectionExtraction.c
@@ -0,0 +1,201 @@
+/** @file
+  LZMA Decompress GUIDed Section Extraction Library.
+  It wraps Lzma decompress interfaces to GUIDed Section Extraction interfaces
+  and registers them into GUIDed handler table.
+
+  Copyright (c) 2009 - 2011, 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 "LzmaDecompressLibInternal.h"
+
+/**
+  Examines a GUIDed section and returns the size of the decoded buffer and the
+  size of an scratch buffer required to actually decode the data in a GUIDed section.
+
+  Examines a GUIDed section specified by InputSection.  
+  If GUID for InputSection does not match the GUID that this handler supports,
+  then RETURN_UNSUPPORTED is returned.  
+  If the required information can not be retrieved from InputSection,
+  then RETURN_INVALID_PARAMETER is returned.
+  If the GUID of InputSection does match the GUID that this handler supports,
+  then the size required to hold the decoded buffer is returned in OututBufferSize,
+  the size of an optional scratch buffer is returned in ScratchSize, and the Attributes field
+  from EFI_GUID_DEFINED_SECTION header of InputSection is returned in SectionAttribute.
+  
+  If InputSection is NULL, then ASSERT().
+  If OutputBufferSize is NULL, then ASSERT().
+  If ScratchBufferSize is NULL, then ASSERT().
+  If SectionAttribute is NULL, then ASSERT().
+
+
+  @param[in]  InputSection       A pointer to a GUIDed section of an FFS formatted file.
+  @param[out] OutputBufferSize   A pointer to the size, in bytes, of an output buffer required
+                                 if the buffer specified by InputSection were decoded.
+  @param[out] ScratchBufferSize  A pointer to the size, in bytes, required as scratch space
+                                 if the buffer specified by InputSection were decoded.
+  @param[out] SectionAttribute   A pointer to the attributes of the GUIDed section. See the Attributes
+                                 field of EFI_GUID_DEFINED_SECTION in the PI Specification.
+
+  @retval  RETURN_SUCCESS            The information about InputSection was returned.
+  @retval  RETURN_UNSUPPORTED        The section specified by InputSection does not match the GUID this handler supports.
+  @retval  RETURN_INVALID_PARAMETER  The information can not be retrieved from the section specified by InputSection.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaGuidedSectionGetInfo (
+  IN  CONST VOID  *InputSection,
+  OUT UINT32      *OutputBufferSize,
+  OUT UINT32      *ScratchBufferSize,
+  OUT UINT16      *SectionAttribute
+  )
+{
+  ASSERT (InputSection != NULL);
+  ASSERT (OutputBufferSize != NULL);
+  ASSERT (ScratchBufferSize != NULL);
+  ASSERT (SectionAttribute != NULL);
+
+  if (IS_SECTION2 (InputSection)) {
+    if (!CompareGuid (
+        &gLzmaCustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    *SectionAttribute = ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes;
+
+    return LzmaUefiDecompressGetInfo (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             OutputBufferSize,
+             ScratchBufferSize
+             );
+  } else {
+    if (!CompareGuid (
+        &gLzmaCustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    *SectionAttribute = ((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes;
+
+    return LzmaUefiDecompressGetInfo (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             OutputBufferSize,
+             ScratchBufferSize
+             );
+  }
+}
+
+/**
+  Decompress a LZAM compressed GUIDed section into a caller allocated output buffer.
+  
+  Decodes the GUIDed section specified by InputSection.  
+  If GUID for InputSection does not match the GUID that this handler supports, then RETURN_UNSUPPORTED is returned.  
+  If the data in InputSection can not be decoded, then RETURN_INVALID_PARAMETER is returned.
+  If the GUID of InputSection does match the GUID that this handler supports, then InputSection
+  is decoded into the buffer specified by OutputBuffer and the authentication status of this
+  decode operation is returned in AuthenticationStatus.  If the decoded buffer is identical to the
+  data in InputSection, then OutputBuffer is set to point at the data in InputSection.  Otherwise,
+  the decoded data will be placed in caller allocated buffer specified by OutputBuffer.
+  
+  If InputSection is NULL, then ASSERT().
+  If OutputBuffer is NULL, then ASSERT().
+  If ScratchBuffer is NULL and this decode operation requires a scratch buffer, then ASSERT().
+  If AuthenticationStatus is NULL, then ASSERT().
+
+
+  @param[in]  InputSection  A pointer to a GUIDed section of an FFS formatted file.
+  @param[out] OutputBuffer  A pointer to a buffer that contains the result of a decode operation. 
+  @param[out] ScratchBuffer A caller allocated buffer that may be required by this function
+                            as a scratch buffer to perform the decode operation. 
+  @param[out] AuthenticationStatus 
+                            A pointer to the authentication status of the decoded output buffer.
+                            See the definition of authentication status in the EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI
+                            section of the PI Specification. EFI_AUTH_STATUS_PLATFORM_OVERRIDE must
+                            never be set by this handler.
+
+  @retval  RETURN_SUCCESS            The buffer specified by InputSection was decoded.
+  @retval  RETURN_UNSUPPORTED        The section specified by InputSection does not match the GUID this handler supports.
+  @retval  RETURN_INVALID_PARAMETER  The section specified by InputSection can not be decoded.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaGuidedSectionExtraction (
+  IN CONST  VOID    *InputSection,
+  OUT       VOID    **OutputBuffer,
+  OUT       VOID    *ScratchBuffer,        OPTIONAL
+  OUT       UINT32  *AuthenticationStatus
+  )
+{
+  ASSERT (OutputBuffer != NULL);
+  ASSERT (InputSection != NULL);
+
+  if (IS_SECTION2 (InputSection)) {
+    if (!CompareGuid (
+        &gLzmaCustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    //
+    // Authentication is set to Zero, which may be ignored.
+    //
+    *AuthenticationStatus = 0;
+
+    return LzmaUefiDecompress (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset,
+             *OutputBuffer,
+             ScratchBuffer
+             );
+  } else {
+    if (!CompareGuid (
+        &gLzmaCustomDecompressGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return RETURN_INVALID_PARAMETER;
+    }
+
+    //
+    // Authentication is set to Zero, which may be ignored.
+    //
+    *AuthenticationStatus = 0;
+
+    return LzmaUefiDecompress (
+             (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset,
+             *OutputBuffer,
+             ScratchBuffer
+    );
+  }
+}
+
+
+/**
+  Register LzmaDecompress and LzmaDecompressGetInfo handlers with LzmaCustomerDecompressGuid.
+
+  @retval  RETURN_SUCCESS            Register successfully.
+  @retval  RETURN_OUT_OF_RESOURCES   No enough memory to store this handler.
+**/
+EFI_STATUS
+EFIAPI
+LzmaDecompressLibConstructor (
+  )
+{
+  return ExtractGuidedSectionRegisterHandlers (
+          &gLzmaCustomDecompressGuid,
+          LzmaGuidedSectionGetInfo,
+          LzmaGuidedSectionExtraction
+          );      
+}
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LZMA-SDK-README.txt b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LZMA-SDK-README.txt
new file mode 100644
index 0000000000..e05b3bb853
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LZMA-SDK-README.txt
@@ -0,0 +1,4 @@
+LzmaCustomDecompressLib is based on the LZMA SDK 4.65.
+LZMA SDK 4.65 was placed in the public domain on
+2009-02-03.  It was released on the
+http://www.7-zip.org/sdk.html website.
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchCustomDecompressLib.inf b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchCustomDecompressLib.inf
new file mode 100644
index 0000000000..ec7585d55b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchCustomDecompressLib.inf
@@ -0,0 +1,66 @@
+## @file
+#  LzmaArchCustomDecompressLib produces LZMA custom decompression algorithm with the converter for the different arch code.
+#
+#  It is based on the LZMA SDK 4.65.
+#  LZMA SDK 4.65 was placed in the public domain on 2009-02-03.
+#  It was released on the http://www.7-zip.org/sdk.html website.
+#
+#  Copyright (c) 2012 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = LzmaArchDecompressLib
+  MODULE_UNI_FILE                = LzmaArchDecompressLib.uni
+  FILE_GUID                      = A853C1D2-E003-4cc4-9DD1-8824AD79FE48
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL
+  CONSTRUCTOR                    = LzmaArchDecompressLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  LzmaDecompress.c
+  Sdk/C/Bra.h
+  Sdk/C/LzFind.c
+  Sdk/C/LzmaDec.c
+  Sdk/C/7zVersion.h
+  Sdk/C/CpuArch.h
+  Sdk/C/LzFind.h
+  Sdk/C/LzHash.h
+  Sdk/C/LzmaDec.h
+  Sdk/C/Types.h  
+  UefiLzma.h
+  LzmaDecompressLibInternal.h
+
+[Sources.Ia32, Sources.X64]
+  Sdk/C/Bra86.c
+  F86GuidedSectionExtraction.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[Guids.Ia32, Guids.X64]
+  gLzmaF86CustomDecompressGuid    ## PRODUCES  ## GUID # specifies LZMA custom decompress algorithm with converter for x86 code.
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  ExtractGuidedSectionLib
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchDecompressLib.uni b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchDecompressLib.uni
new file mode 100644
index 0000000000..1830dae73c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchDecompressLib.uni
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaCustomDecompressLib.inf b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaCustomDecompressLib.inf
new file mode 100644
index 0000000000..f5624fd039
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaCustomDecompressLib.inf
@@ -0,0 +1,62 @@
+## @file
+#  LzmaCustomDecompressLib produces LZMA custom decompression algorithm.
+#
+#  It is based on the LZMA SDK 4.65.
+#  LZMA SDK 4.65 was placed in the public domain on 2009-02-03.
+#  It was released on the http://www.7-zip.org/sdk.html website.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = LzmaDecompressLib
+  MODULE_UNI_FILE                = LzmaDecompressLib.uni
+  FILE_GUID                      = 35194660-7421-44ad-9636-e44885f092d1
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL
+  CONSTRUCTOR                    = LzmaDecompressLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  LzmaDecompress.c
+  Sdk/C/LzFind.c
+  Sdk/C/LzmaDec.c
+  Sdk/C/7zVersion.h
+  Sdk/C/CpuArch.h
+  Sdk/C/LzFind.h
+  Sdk/C/LzHash.h
+  Sdk/C/LzmaDec.h
+  Sdk/C/Types.h  
+  GuidedSectionExtraction.c
+  UefiLzma.h
+  LzmaDecompressLibInternal.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[Guids]
+  gLzmaCustomDecompressGuid  ## PRODUCES  ## UNDEFINED # specifies LZMA custom decompress algorithm.
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  ExtractGuidedSectionLib
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompress.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompress.c
new file mode 100644
index 0000000000..e32b6a3025
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompress.c
@@ -0,0 +1,220 @@
+/** @file
+  LZMA Decompress interfaces
+
+  Copyright (c) 2009 - 2010, 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 "LzmaDecompressLibInternal.h"
+#include "Sdk/C/Types.h"
+#include "Sdk/C/7zVersion.h"
+#include "Sdk/C/LzmaDec.h"
+
+#define SCRATCH_BUFFER_REQUEST_SIZE SIZE_64KB
+
+typedef struct
+{
+  ISzAlloc Functions;
+  VOID     *Buffer;
+  UINTN    BufferSize;
+} ISzAllocWithData;
+
+/**
+  Allocation routine used by LZMA decompression.
+
+  @param P                Pointer to the ISzAlloc instance
+  @param Size             The size in bytes to be allocated
+
+  @return The allocated pointer address, or NULL on failure
+**/
+VOID *
+SzAlloc (
+  VOID *P,
+  size_t Size
+  )
+{
+  VOID *Addr;
+  ISzAllocWithData *Private;
+
+  Private = (ISzAllocWithData*) P;
+
+  if (Private->BufferSize >= Size) {
+    Addr = Private->Buffer;
+    Private->Buffer = (VOID*) ((UINT8*)Addr + Size);
+    Private->BufferSize -= Size;
+    return Addr;
+  } else {
+    ASSERT (FALSE);
+    return NULL;
+  }
+}
+
+/**
+  Free routine used by LZMA decompression.
+
+  @param P                Pointer to the ISzAlloc instance
+  @param Address          The address to be freed
+**/
+VOID
+SzFree (
+  VOID *P,
+  VOID *Address
+  )
+{
+  //
+  // We use the 'scratch buffer' for allocations, so there is no free
+  // operation required.  The scratch buffer will be freed by the caller
+  // of the decompression code.
+  //
+}
+
+#define LZMA_HEADER_SIZE (LZMA_PROPS_SIZE + 8)
+
+/**
+  Get the size of the uncompressed buffer by parsing EncodeData header.
+
+  @param EncodedData  Pointer to the compressed data.
+
+  @return The size of the uncompressed buffer.
+**/
+UINT64
+GetDecodedSizeOfBuf(
+  UINT8 *EncodedData
+  )
+{
+  UINT64 DecodedSize;
+  INTN   Index;
+
+  /* Parse header */
+  DecodedSize = 0;
+  for (Index = LZMA_PROPS_SIZE + 7; Index >= LZMA_PROPS_SIZE; Index--)
+    DecodedSize = LShiftU64(DecodedSize, 8) + EncodedData[Index];
+
+  return DecodedSize;
+}
+
+//
+// LZMA functions and data as defined in local LzmaDecompressLibInternal.h
+//
+
+/**
+  Given a Lzma compressed source buffer, this function retrieves the size of 
+  the uncompressed buffer and the size of the scratch buffer required 
+  to decompress the compressed source buffer.
+
+  Retrieves the size of the uncompressed buffer and the temporary scratch buffer 
+  required to decompress the buffer specified by Source and SourceSize.
+  The size of the uncompressed buffer is returned in DestinationSize, 
+  the size of the scratch buffer is returned in ScratchSize, and RETURN_SUCCESS is returned.
+  This function does not have scratch buffer available to perform a thorough 
+  checking of the validity of the source data. It just retrieves the "Original Size"
+  field from the LZMA_HEADER_SIZE beginning bytes of the source data and output it as DestinationSize.
+  And ScratchSize is specific to the decompression implementation.
+
+  If SourceSize is less than LZMA_HEADER_SIZE, then ASSERT().
+
+  @param  Source          The source buffer containing the compressed data.
+  @param  SourceSize      The size, in bytes, of the source buffer.
+  @param  DestinationSize A pointer to the size, in bytes, of the uncompressed buffer
+                          that will be generated when the compressed buffer specified
+                          by Source and SourceSize is decompressed.
+  @param  ScratchSize     A pointer to the size, in bytes, of the scratch buffer that
+                          is required to decompress the compressed buffer specified 
+                          by Source and SourceSize.
+
+  @retval  RETURN_SUCCESS The size of the uncompressed data was returned 
+                          in DestinationSize and the size of the scratch 
+                          buffer was returned in ScratchSize.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaUefiDecompressGetInfo (
+  IN  CONST VOID  *Source,
+  IN  UINT32      SourceSize,
+  OUT UINT32      *DestinationSize,
+  OUT UINT32      *ScratchSize
+  )
+{
+  UInt64  DecodedSize;
+
+  ASSERT(SourceSize >= LZMA_HEADER_SIZE);
+
+  DecodedSize = GetDecodedSizeOfBuf((UINT8*)Source);
+
+  *DestinationSize = (UINT32)DecodedSize;
+  *ScratchSize = SCRATCH_BUFFER_REQUEST_SIZE;
+  return RETURN_SUCCESS;
+}
+
+/**
+  Decompresses a Lzma compressed source buffer.
+
+  Extracts decompressed data to its original form.
+  If the compressed source data specified by Source is successfully decompressed 
+  into Destination, then RETURN_SUCCESS is returned.  If the compressed source data 
+  specified by Source is not in a valid compressed data format,
+  then RETURN_INVALID_PARAMETER is returned.
+
+  @param  Source      The source buffer containing the compressed data.
+  @param  SourceSize  The size of source buffer.
+  @param  Destination The destination buffer to store the decompressed data
+  @param  Scratch     A temporary scratch buffer that is used to perform the decompression.
+                      This is an optional parameter that may be NULL if the 
+                      required scratch buffer size is 0.
+                     
+  @retval  RETURN_SUCCESS Decompression completed successfully, and 
+                          the uncompressed buffer is returned in Destination.
+  @retval  RETURN_INVALID_PARAMETER 
+                          The source buffer specified by Source is corrupted 
+                          (not in a valid compressed format).
+**/
+RETURN_STATUS
+EFIAPI
+LzmaUefiDecompress (
+  IN CONST VOID  *Source,
+  IN UINTN       SourceSize,
+  IN OUT VOID    *Destination,
+  IN OUT VOID    *Scratch
+  )
+{
+  SRes              LzmaResult;
+  ELzmaStatus       Status;
+  SizeT             DecodedBufSize;
+  SizeT             EncodedDataSize;
+  ISzAllocWithData  AllocFuncs;
+
+  AllocFuncs.Functions.Alloc  = SzAlloc;
+  AllocFuncs.Functions.Free   = SzFree;
+  AllocFuncs.Buffer           = Scratch;
+  AllocFuncs.BufferSize       = SCRATCH_BUFFER_REQUEST_SIZE;
+  
+  DecodedBufSize = (SizeT)GetDecodedSizeOfBuf((UINT8*)Source);
+  EncodedDataSize = (SizeT) (SourceSize - LZMA_HEADER_SIZE);
+
+  LzmaResult = LzmaDecode(
+    Destination,
+    &DecodedBufSize,
+    (Byte*)((UINT8*)Source + LZMA_HEADER_SIZE),
+    &EncodedDataSize,
+    Source,
+    LZMA_PROPS_SIZE,
+    LZMA_FINISH_END,
+    &Status,
+    &(AllocFuncs.Functions)
+    );
+
+  if (LzmaResult == SZ_OK) {
+    return RETURN_SUCCESS;
+  } else {
+    return RETURN_INVALID_PARAMETER;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLib.uni b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLib.uni
new file mode 100644
index 0000000000..498e4e1088
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLib.uni
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLibInternal.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLibInternal.h
new file mode 100644
index 0000000000..3096e91dbe
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaDecompressLibInternal.h
@@ -0,0 +1,96 @@
+/** @file
+  LZMA Decompress Library internal header file declares Lzma decompress interfaces.
+
+  Copyright (c) 2009 - 2010, 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.
+
+**/
+
+#ifndef __LZMADECOMPRESSLIB_INTERNAL_H__
+#define __LZMADECOMPRESSLIB_INTERNAL_H__
+
+#include <PiPei.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Guid/LzmaDecompress.h>
+
+/**
+  Given a Lzma compressed source buffer, this function retrieves the size of 
+  the uncompressed buffer and the size of the scratch buffer required 
+  to decompress the compressed source buffer.
+
+  Retrieves the size of the uncompressed buffer and the temporary scratch buffer 
+  required to decompress the buffer specified by Source and SourceSize.
+  The size of the uncompressed buffer is returned in DestinationSize, 
+  the size of the scratch buffer is returned in ScratchSize, and RETURN_SUCCESS is returned.
+  This function does not have scratch buffer available to perform a thorough 
+  checking of the validity of the source data. It just retrieves the "Original Size"
+  field from the LZMA_HEADER_SIZE beginning bytes of the source data and output it as DestinationSize.
+  And ScratchSize is specific to the decompression implementation.
+
+  If SourceSize is less than LZMA_HEADER_SIZE, then ASSERT().
+
+  @param  Source          The source buffer containing the compressed data.
+  @param  SourceSize      The size, in bytes, of the source buffer.
+  @param  DestinationSize A pointer to the size, in bytes, of the uncompressed buffer
+                          that will be generated when the compressed buffer specified
+                          by Source and SourceSize is decompressed.
+  @param  ScratchSize     A pointer to the size, in bytes, of the scratch buffer that
+                          is required to decompress the compressed buffer specified 
+                          by Source and SourceSize.
+
+  @retval  RETURN_SUCCESS The size of the uncompressed data was returned 
+                          in DestinationSize and the size of the scratch 
+                          buffer was returned in ScratchSize.
+
+**/
+RETURN_STATUS
+EFIAPI
+LzmaUefiDecompressGetInfo (
+  IN  CONST VOID  *Source,
+  IN  UINT32      SourceSize,
+  OUT UINT32      *DestinationSize,
+  OUT UINT32      *ScratchSize
+  );
+
+/**
+  Decompresses a Lzma compressed source buffer.
+
+  Extracts decompressed data to its original form.
+  If the compressed source data specified by Source is successfully decompressed 
+  into Destination, then RETURN_SUCCESS is returned.  If the compressed source data 
+  specified by Source is not in a valid compressed data format,
+  then RETURN_INVALID_PARAMETER is returned.
+
+  @param  Source      The source buffer containing the compressed data.
+  @param  SourceSize  The size of source buffer.
+  @param  Destination The destination buffer to store the decompressed data
+  @param  Scratch     A temporary scratch buffer that is used to perform the decompression.
+                      This is an optional parameter that may be NULL if the 
+                      required scratch buffer size is 0.
+                     
+  @retval  RETURN_SUCCESS Decompression completed successfully, and 
+                          the uncompressed buffer is returned in Destination.
+  @retval  RETURN_INVALID_PARAMETER 
+                          The source buffer specified by Source is corrupted 
+                          (not in a valid compressed format).
+**/
+RETURN_STATUS
+EFIAPI
+LzmaUefiDecompress (
+  IN CONST VOID  *Source,
+  IN UINTN       SourceSize,
+  IN OUT VOID    *Destination,
+  IN OUT VOID    *Scratch
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/7zVersion.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/7zVersion.h
new file mode 100644
index 0000000000..5a6bcadac6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/7zVersion.h
@@ -0,0 +1,7 @@
+#define MY_VER_MAJOR 4
+#define MY_VER_MINOR 65
+#define MY_VER_BUILD 0
+#define MY_VERSION "4.65"
+#define MY_DATE "2009-02-03"
+#define MY_COPYRIGHT ": Igor Pavlov : Public domain"
+#define MY_VERSION_COPYRIGHT_DATE MY_VERSION " " MY_COPYRIGHT " : " MY_DATE
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra.h
new file mode 100644
index 0000000000..b9018eb991
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra.h
@@ -0,0 +1,60 @@
+/* Bra.h -- Branch converters for executables
+2008-10-04 : Igor Pavlov : Public domain */
+
+#ifndef __BRA_H
+#define __BRA_H
+
+#include "Types.h"
+
+/*
+These functions convert relative addresses to absolute addresses
+in CALL instructions to increase the compression ratio.
+  
+  In:
+    data     - data buffer
+    size     - size of data
+    ip       - current virtual Instruction Pinter (IP) value
+    state    - state variable for x86 converter
+    encoding - 0 (for decoding), 1 (for encoding)
+  
+  Out:
+    state    - state variable for x86 converter
+
+  Returns:
+    The number of processed bytes. If you call these functions with multiple calls,
+    you must start next call with first byte after block of processed bytes.
+  
+  Type   Endian  Alignment  LookAhead
+  
+  x86    little      1          4
+  ARMT   little      2          2
+  ARM    little      4          0
+  PPC     big        4          0
+  SPARC   big        4          0
+  IA64   little     16          0
+
+  size must be >= Alignment + LookAhead, if it's not last block.
+  If (size < Alignment + LookAhead), converter returns 0.
+
+  Example:
+
+    UInt32 ip = 0;
+    for ()
+    {
+      ; size must be >= Alignment + LookAhead, if it's not last block
+      SizeT processed = Convert(data, size, ip, 1);
+      data += processed;
+      size -= processed;
+      ip += processed;
+    }
+*/
+
+#define x86_Convert_Init(state) { state = 0; }
+SizeT x86_Convert(Byte *data, SizeT size, UInt32 ip, UInt32 *state, int encoding);
+SizeT ARM_Convert(Byte *data, SizeT size, UInt32 ip, int encoding);
+SizeT ARMT_Convert(Byte *data, SizeT size, UInt32 ip, int encoding);
+SizeT PPC_Convert(Byte *data, SizeT size, UInt32 ip, int encoding);
+SizeT SPARC_Convert(Byte *data, SizeT size, UInt32 ip, int encoding);
+SizeT IA64_Convert(Byte *data, SizeT size, UInt32 ip, int encoding);
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra86.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra86.c
new file mode 100644
index 0000000000..93566cb212
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Bra86.c
@@ -0,0 +1,85 @@
+/* Bra86.c -- Converter for x86 code (BCJ)
+2008-10-04 : Igor Pavlov : Public domain */
+
+#include "Bra.h"
+
+#define Test86MSByte(b) ((b) == 0 || (b) == 0xFF)
+
+const Byte kMaskToAllowedStatus[8] = {1, 1, 1, 0, 1, 0, 0, 0};
+const Byte kMaskToBitNumber[8] = {0, 1, 2, 2, 3, 3, 3, 3};
+
+SizeT x86_Convert(Byte *data, SizeT size, UInt32 ip, UInt32 *state, int encoding)
+{
+  SizeT bufferPos = 0, prevPosT;
+  UInt32 prevMask = *state & 0x7;
+  if (size < 5)
+    return 0;
+  ip += 5;
+  prevPosT = (SizeT)0 - 1;
+
+  for (;;)
+  {
+    Byte *p = data + bufferPos;
+    Byte *limit = data + size - 4;
+    for (; p < limit; p++)
+      if ((*p & 0xFE) == 0xE8)
+        break;
+    bufferPos = (SizeT)(p - data);
+    if (p >= limit)
+      break;
+    prevPosT = bufferPos - prevPosT;
+    if (prevPosT > 3)
+      prevMask = 0;
+    else
+    {
+      prevMask = (prevMask << ((int)prevPosT - 1)) & 0x7;
+      if (prevMask != 0)
+      {
+        Byte b = p[4 - kMaskToBitNumber[prevMask]];
+        if (!kMaskToAllowedStatus[prevMask] || Test86MSByte(b))
+        {
+          prevPosT = bufferPos;
+          prevMask = ((prevMask << 1) & 0x7) | 1;
+          bufferPos++;
+          continue;
+        }
+      }
+    }
+    prevPosT = bufferPos;
+
+    if (Test86MSByte(p[4]))
+    {
+      UInt32 src = ((UInt32)p[4] << 24) | ((UInt32)p[3] << 16) | ((UInt32)p[2] << 8) | ((UInt32)p[1]);
+      UInt32 dest;
+      for (;;)
+      {
+        Byte b;
+        int index;
+        if (encoding)
+          dest = (ip + (UInt32)bufferPos) + src;
+        else
+          dest = src - (ip + (UInt32)bufferPos);
+        if (prevMask == 0)
+          break;
+        index = kMaskToBitNumber[prevMask] * 8;
+        b = (Byte)(dest >> (24 - index));
+        if (!Test86MSByte(b))
+          break;
+        src = dest ^ ((1 << (32 - index)) - 1);
+      }
+      p[4] = (Byte)(~(((dest >> 24) & 1) - 1));
+      p[3] = (Byte)(dest >> 16);
+      p[2] = (Byte)(dest >> 8);
+      p[1] = (Byte)dest;
+      bufferPos += 5;
+    }
+    else
+    {
+      prevMask = ((prevMask << 1) & 0x7) | 1;
+      bufferPos++;
+    }
+  }
+  prevPosT = bufferPos - prevPosT;
+  *state = ((prevPosT > 3) ? 0 : ((prevMask << ((int)prevPosT - 1)) & 0x7));
+  return bufferPos;
+}
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/CpuArch.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/CpuArch.h
new file mode 100644
index 0000000000..006361f2f2
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/CpuArch.h
@@ -0,0 +1,69 @@
+/* CpuArch.h
+2008-08-05
+Igor Pavlov
+Public domain */
+
+#ifndef __CPUARCH_H
+#define __CPUARCH_H
+
+/*
+LITTLE_ENDIAN_UNALIGN means:
+  1) CPU is LITTLE_ENDIAN
+  2) it's allowed to make unaligned memory accesses
+if LITTLE_ENDIAN_UNALIGN is not defined, it means that we don't know
+about these properties of platform.
+*/
+
+#if defined(_M_IX86) || defined(_M_X64) || defined(_M_AMD64) || defined(__i386__) || defined(__x86_64__)
+#define LITTLE_ENDIAN_UNALIGN
+#endif
+
+#ifdef LITTLE_ENDIAN_UNALIGN
+
+#define GetUi16(p) (*(const UInt16 *)(p))
+#define GetUi32(p) (*(const UInt32 *)(p))
+#define GetUi64(p) (*(const UInt64 *)(p))
+#define SetUi32(p, d) *(UInt32 *)(p) = (d);
+
+#else
+
+#define GetUi16(p) (((const Byte *)(p))[0] | ((UInt16)((const Byte *)(p))[1] << 8))
+
+#define GetUi32(p) ( \
+             ((const Byte *)(p))[0]        | \
+    ((UInt32)((const Byte *)(p))[1] <<  8) | \
+    ((UInt32)((const Byte *)(p))[2] << 16) | \
+    ((UInt32)((const Byte *)(p))[3] << 24))
+
+#define GetUi64(p) (GetUi32(p) | ((UInt64)GetUi32(((const Byte *)(p)) + 4) << 32))
+
+#define SetUi32(p, d) { UInt32 _x_ = (d); \
+    ((Byte *)(p))[0] = (Byte)_x_; \
+    ((Byte *)(p))[1] = (Byte)(_x_ >> 8); \
+    ((Byte *)(p))[2] = (Byte)(_x_ >> 16); \
+    ((Byte *)(p))[3] = (Byte)(_x_ >> 24); }
+
+#endif
+
+#if defined(LITTLE_ENDIAN_UNALIGN) && defined(_WIN64) && (_MSC_VER >= 1300)
+
+#pragma intrinsic(_byteswap_ulong)
+#pragma intrinsic(_byteswap_uint64)
+#define GetBe32(p) _byteswap_ulong(*(const UInt32 *)(const Byte *)(p))
+#define GetBe64(p) _byteswap_uint64(*(const UInt64 *)(const Byte *)(p))
+
+#else
+
+#define GetBe32(p) ( \
+    ((UInt32)((const Byte *)(p))[0] << 24) | \
+    ((UInt32)((const Byte *)(p))[1] << 16) | \
+    ((UInt32)((const Byte *)(p))[2] <<  8) | \
+             ((const Byte *)(p))[3] )
+
+#define GetBe64(p) (((UInt64)GetBe32(p) << 32) | GetBe32(((const Byte *)(p)) + 4))
+
+#endif
+
+#define GetBe16(p) (((UInt16)((const Byte *)(p))[0] << 8) | ((const Byte *)(p))[1])
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.c
new file mode 100644
index 0000000000..492cea2e41
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.c
@@ -0,0 +1,770 @@
+/** @file
+  LzFind.c
+
+  Based on LZMA SDK 4.65:
+    LzFind.c -- Match finder for LZ algorithms
+    2008-10-04 : Igor Pavlov : Public domain
+
+  Copyright (c) 2009, 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.
+
+**/
+
+#ifndef EFIAPI
+
+#include <string.h>
+
+#endif // !EFIAPI
+
+#include "LzFind.h"
+#include "LzHash.h"
+
+#define kEmptyHashValue 0
+#define kMaxValForNormalize ((UInt32)0xFFFFFFFF)
+#define kNormalizeStepMin (1 << 10) /* it must be power of 2 */
+#define kNormalizeMask (~(kNormalizeStepMin - 1))
+#define kMaxHistorySize ((UInt32)3 << 30)
+
+#define kStartMaxLen 3
+
+static void LzInWindow_Free(CMatchFinder *p, ISzAlloc *alloc)
+{
+  if (!p->directInput)
+  {
+    alloc->Free(alloc, p->bufferBase);
+    p->bufferBase = 0;
+  }
+}
+
+/* keepSizeBefore + keepSizeAfter + keepSizeReserv must be < 4G) */
+
+static int LzInWindow_Create(CMatchFinder *p, UInt32 keepSizeReserv, ISzAlloc *alloc)
+{
+  UInt32 blockSize = p->keepSizeBefore + p->keepSizeAfter + keepSizeReserv;
+  if (p->directInput)
+  {
+    p->blockSize = blockSize;
+    return 1;
+  }
+  if (p->bufferBase == 0 || p->blockSize != blockSize)
+  {
+    LzInWindow_Free(p, alloc);
+    p->blockSize = blockSize;
+    p->bufferBase = (Byte *)alloc->Alloc(alloc, (size_t)blockSize);
+  }
+  return (p->bufferBase != 0);
+}
+
+Byte *MatchFinder_GetPointerToCurrentPos(CMatchFinder *p) { return p->buffer; }
+Byte MatchFinder_GetIndexByte(CMatchFinder *p, Int32 index) { return p->buffer[index]; }
+
+UInt32 MatchFinder_GetNumAvailableBytes(CMatchFinder *p) { return p->streamPos - p->pos; }
+
+void MatchFinder_ReduceOffsets(CMatchFinder *p, UInt32 subValue)
+{
+  p->posLimit -= subValue;
+  p->pos -= subValue;
+  p->streamPos -= subValue;
+}
+
+static void MatchFinder_ReadBlock(CMatchFinder *p)
+{
+  if (p->streamEndWasReached || p->result != SZ_OK)
+    return;
+  for (;;)
+  {
+    Byte *dest = p->buffer + (p->streamPos - p->pos);
+    size_t size = (p->bufferBase + p->blockSize - dest);
+    if (size == 0)
+      return;
+    p->result = p->stream->Read(p->stream, dest, &size);
+    if (p->result != SZ_OK)
+      return;
+    if (size == 0)
+    {
+      p->streamEndWasReached = 1;
+      return;
+    }
+    p->streamPos += (UInt32)size;
+    if (p->streamPos - p->pos > p->keepSizeAfter)
+      return;
+  }
+}
+
+void MatchFinder_MoveBlock(CMatchFinder *p)
+{
+  memmove(p->bufferBase,
+    p->buffer - p->keepSizeBefore,
+    (size_t)(p->streamPos - p->pos + p->keepSizeBefore));
+  p->buffer = p->bufferBase + p->keepSizeBefore;
+}
+
+int MatchFinder_NeedMove(CMatchFinder *p)
+{
+  /* if (p->streamEndWasReached) return 0; */
+  return ((size_t)(p->bufferBase + p->blockSize - p->buffer) <= p->keepSizeAfter);
+}
+
+void MatchFinder_ReadIfRequired(CMatchFinder *p)
+{
+  if (p->streamEndWasReached)
+    return;
+  if (p->keepSizeAfter >= p->streamPos - p->pos)
+    MatchFinder_ReadBlock(p);
+}
+
+static void MatchFinder_CheckAndMoveAndRead(CMatchFinder *p)
+{
+  if (MatchFinder_NeedMove(p))
+    MatchFinder_MoveBlock(p);
+  MatchFinder_ReadBlock(p);
+}
+
+static void MatchFinder_SetDefaultSettings(CMatchFinder *p)
+{
+  p->cutValue = 32;
+  p->btMode = 1;
+  p->numHashBytes = 4;
+  /* p->skipModeBits = 0; */
+  p->directInput = 0;
+  p->bigHash = 0;
+}
+
+#define kCrcPoly 0xEDB88320
+
+void MatchFinder_Construct(CMatchFinder *p)
+{
+  UInt32 i;
+  p->bufferBase = 0;
+  p->directInput = 0;
+  p->hash = 0;
+  MatchFinder_SetDefaultSettings(p);
+
+  for (i = 0; i < 256; i++)
+  {
+    UInt32 r = i;
+    int j;
+    for (j = 0; j < 8; j++)
+      r = (r >> 1) ^ (kCrcPoly & ~((r & 1) - 1));
+    p->crc[i] = r;
+  }
+}
+
+static void MatchFinder_FreeThisClassMemory(CMatchFinder *p, ISzAlloc *alloc)
+{
+  alloc->Free(alloc, p->hash);
+  p->hash = 0;
+}
+
+void MatchFinder_Free(CMatchFinder *p, ISzAlloc *alloc)
+{
+  MatchFinder_FreeThisClassMemory(p, alloc);
+  LzInWindow_Free(p, alloc);
+}
+
+static CLzRef* AllocRefs(UInt32 num, ISzAlloc *alloc)
+{
+  size_t sizeInBytes = (size_t)num * sizeof(CLzRef);
+  if (sizeInBytes / sizeof(CLzRef) != num)
+    return 0;
+  return (CLzRef *)alloc->Alloc(alloc, sizeInBytes);
+}
+
+int MatchFinder_Create(CMatchFinder *p, UInt32 historySize,
+    UInt32 keepAddBufferBefore, UInt32 matchMaxLen, UInt32 keepAddBufferAfter,
+    ISzAlloc *alloc)
+{
+  UInt32 sizeReserv;
+  if (historySize > kMaxHistorySize)
+  {
+    MatchFinder_Free(p, alloc);
+    return 0;
+  }
+  sizeReserv = historySize >> 1;
+  if (historySize > ((UInt32)2 << 30))
+    sizeReserv = historySize >> 2;
+  sizeReserv += (keepAddBufferBefore + matchMaxLen + keepAddBufferAfter) / 2 + (1 << 19);
+
+  p->keepSizeBefore = historySize + keepAddBufferBefore + 1;
+  p->keepSizeAfter = matchMaxLen + keepAddBufferAfter;
+  /* we need one additional byte, since we use MoveBlock after pos++ and before dictionary using */
+  if (LzInWindow_Create(p, sizeReserv, alloc))
+  {
+    UInt32 newCyclicBufferSize = (historySize /* >> p->skipModeBits */) + 1;
+    UInt32 hs;
+    p->matchMaxLen = matchMaxLen;
+    {
+      p->fixedHashSize = 0;
+      if (p->numHashBytes == 2)
+        hs = (1 << 16) - 1;
+      else
+      {
+        hs = historySize - 1;
+        hs |= (hs >> 1);
+        hs |= (hs >> 2);
+        hs |= (hs >> 4);
+        hs |= (hs >> 8);
+        hs >>= 1;
+        /* hs >>= p->skipModeBits; */
+        hs |= 0xFFFF; /* don't change it! It's required for Deflate */
+        if (hs > (1 << 24))
+        {
+          if (p->numHashBytes == 3)
+            hs = (1 << 24) - 1;
+          else
+            hs >>= 1;
+        }
+      }
+      p->hashMask = hs;
+      hs++;
+      if (p->numHashBytes > 2) p->fixedHashSize += kHash2Size;
+      if (p->numHashBytes > 3) p->fixedHashSize += kHash3Size;
+      if (p->numHashBytes > 4) p->fixedHashSize += kHash4Size;
+      hs += p->fixedHashSize;
+    }
+
+    {
+      UInt32 prevSize = p->hashSizeSum + p->numSons;
+      UInt32 newSize;
+      p->historySize = historySize;
+      p->hashSizeSum = hs;
+      p->cyclicBufferSize = newCyclicBufferSize;
+      p->numSons = (p->btMode ? newCyclicBufferSize * 2 : newCyclicBufferSize);
+      newSize = p->hashSizeSum + p->numSons;
+      if (p->hash != 0 && prevSize == newSize)
+        return 1;
+      MatchFinder_FreeThisClassMemory(p, alloc);
+      p->hash = AllocRefs(newSize, alloc);
+      if (p->hash != 0)
+      {
+        p->son = p->hash + p->hashSizeSum;
+        return 1;
+      }
+    }
+  }
+  MatchFinder_Free(p, alloc);
+  return 0;
+}
+
+static void MatchFinder_SetLimits(CMatchFinder *p)
+{
+  UInt32 limit = kMaxValForNormalize - p->pos;
+  UInt32 limit2 = p->cyclicBufferSize - p->cyclicBufferPos;
+  if (limit2 < limit)
+    limit = limit2;
+  limit2 = p->streamPos - p->pos;
+  if (limit2 <= p->keepSizeAfter)
+  {
+    if (limit2 > 0)
+      limit2 = 1;
+  }
+  else
+    limit2 -= p->keepSizeAfter;
+  if (limit2 < limit)
+    limit = limit2;
+  {
+    UInt32 lenLimit = p->streamPos - p->pos;
+    if (lenLimit > p->matchMaxLen)
+      lenLimit = p->matchMaxLen;
+    p->lenLimit = lenLimit;
+  }
+  p->posLimit = p->pos + limit;
+}
+
+void MatchFinder_Init(CMatchFinder *p)
+{
+  UInt32 i;
+  for (i = 0; i < p->hashSizeSum; i++)
+    p->hash[i] = kEmptyHashValue;
+  p->cyclicBufferPos = 0;
+  p->buffer = p->bufferBase;
+  p->pos = p->streamPos = p->cyclicBufferSize;
+  p->result = SZ_OK;
+  p->streamEndWasReached = 0;
+  MatchFinder_ReadBlock(p);
+  MatchFinder_SetLimits(p);
+}
+
+static UInt32 MatchFinder_GetSubValue(CMatchFinder *p)
+{
+  return (p->pos - p->historySize - 1) & kNormalizeMask;
+}
+
+void MatchFinder_Normalize3(UInt32 subValue, CLzRef *items, UInt32 numItems)
+{
+  UInt32 i;
+  for (i = 0; i < numItems; i++)
+  {
+    UInt32 value = items[i];
+    if (value <= subValue)
+      value = kEmptyHashValue;
+    else
+      value -= subValue;
+    items[i] = value;
+  }
+}
+
+static void MatchFinder_Normalize(CMatchFinder *p)
+{
+  UInt32 subValue = MatchFinder_GetSubValue(p);
+  MatchFinder_Normalize3(subValue, p->hash, p->hashSizeSum + p->numSons);
+  MatchFinder_ReduceOffsets(p, subValue);
+}
+
+static void MatchFinder_CheckLimits(CMatchFinder *p)
+{
+  if (p->pos == kMaxValForNormalize)
+    MatchFinder_Normalize(p);
+  if (!p->streamEndWasReached && p->keepSizeAfter == p->streamPos - p->pos)
+    MatchFinder_CheckAndMoveAndRead(p);
+  if (p->cyclicBufferPos == p->cyclicBufferSize)
+    p->cyclicBufferPos = 0;
+  MatchFinder_SetLimits(p);
+}
+
+static UInt32 * Hc_GetMatchesSpec(UInt32 lenLimit, UInt32 curMatch, UInt32 pos, const Byte *cur, CLzRef *son,
+    UInt32 _cyclicBufferPos, UInt32 _cyclicBufferSize, UInt32 cutValue,
+    UInt32 *distances, UInt32 maxLen)
+{
+  son[_cyclicBufferPos] = curMatch;
+  for (;;)
+  {
+    UInt32 delta = pos - curMatch;
+    if (cutValue-- == 0 || delta >= _cyclicBufferSize)
+      return distances;
+    {
+      const Byte *pb = cur - delta;
+      curMatch = son[_cyclicBufferPos - delta + ((delta > _cyclicBufferPos) ? _cyclicBufferSize : 0)];
+      if (pb[maxLen] == cur[maxLen] && *pb == *cur)
+      {
+        UInt32 len = 0;
+        while (++len != lenLimit)
+          if (pb[len] != cur[len])
+            break;
+        if (maxLen < len)
+        {
+          *distances++ = maxLen = len;
+          *distances++ = delta - 1;
+          if (len == lenLimit)
+            return distances;
+        }
+      }
+    }
+  }
+}
+
+UInt32 * GetMatchesSpec1(UInt32 lenLimit, UInt32 curMatch, UInt32 pos, const Byte *cur, CLzRef *son,
+    UInt32 _cyclicBufferPos, UInt32 _cyclicBufferSize, UInt32 cutValue,
+    UInt32 *distances, UInt32 maxLen)
+{
+  CLzRef *ptr0 = son + (_cyclicBufferPos << 1) + 1;
+  CLzRef *ptr1 = son + (_cyclicBufferPos << 1);
+  UInt32 len0 = 0, len1 = 0;
+  for (;;)
+  {
+    UInt32 delta = pos - curMatch;
+    if (cutValue-- == 0 || delta >= _cyclicBufferSize)
+    {
+      *ptr0 = *ptr1 = kEmptyHashValue;
+      return distances;
+    }
+    {
+      CLzRef *pair = son + ((_cyclicBufferPos - delta + ((delta > _cyclicBufferPos) ? _cyclicBufferSize : 0)) << 1);
+      const Byte *pb = cur - delta;
+      UInt32 len = (len0 < len1 ? len0 : len1);
+      if (pb[len] == cur[len])
+      {
+        if (++len != lenLimit && pb[len] == cur[len])
+          while (++len != lenLimit)
+            if (pb[len] != cur[len])
+              break;
+        if (maxLen < len)
+        {
+          *distances++ = maxLen = len;
+          *distances++ = delta - 1;
+          if (len == lenLimit)
+          {
+            *ptr1 = pair[0];
+            *ptr0 = pair[1];
+            return distances;
+          }
+        }
+      }
+      if (pb[len] < cur[len])
+      {
+        *ptr1 = curMatch;
+        ptr1 = pair + 1;
+        curMatch = *ptr1;
+        len1 = len;
+      }
+      else
+      {
+        *ptr0 = curMatch;
+        ptr0 = pair;
+        curMatch = *ptr0;
+        len0 = len;
+      }
+    }
+  }
+}
+
+static void SkipMatchesSpec(UInt32 lenLimit, UInt32 curMatch, UInt32 pos, const Byte *cur, CLzRef *son,
+    UInt32 _cyclicBufferPos, UInt32 _cyclicBufferSize, UInt32 cutValue)
+{
+  CLzRef *ptr0 = son + (_cyclicBufferPos << 1) + 1;
+  CLzRef *ptr1 = son + (_cyclicBufferPos << 1);
+  UInt32 len0 = 0, len1 = 0;
+  for (;;)
+  {
+    UInt32 delta = pos - curMatch;
+    if (cutValue-- == 0 || delta >= _cyclicBufferSize)
+    {
+      *ptr0 = *ptr1 = kEmptyHashValue;
+      return;
+    }
+    {
+      CLzRef *pair = son + ((_cyclicBufferPos - delta + ((delta > _cyclicBufferPos) ? _cyclicBufferSize : 0)) << 1);
+      const Byte *pb = cur - delta;
+      UInt32 len = (len0 < len1 ? len0 : len1);
+      if (pb[len] == cur[len])
+      {
+        while (++len != lenLimit)
+          if (pb[len] != cur[len])
+            break;
+        {
+          if (len == lenLimit)
+          {
+            *ptr1 = pair[0];
+            *ptr0 = pair[1];
+            return;
+          }
+        }
+      }
+      if (pb[len] < cur[len])
+      {
+        *ptr1 = curMatch;
+        ptr1 = pair + 1;
+        curMatch = *ptr1;
+        len1 = len;
+      }
+      else
+      {
+        *ptr0 = curMatch;
+        ptr0 = pair;
+        curMatch = *ptr0;
+        len0 = len;
+      }
+    }
+  }
+}
+
+#define MOVE_POS \
+  ++p->cyclicBufferPos; \
+  p->buffer++; \
+  if (++p->pos == p->posLimit) MatchFinder_CheckLimits(p);
+
+#define MOVE_POS_RET MOVE_POS return offset;
+
+static void MatchFinder_MovePos(CMatchFinder *p) { MOVE_POS; }
+
+#define GET_MATCHES_HEADER2(minLen, ret_op) \
+  UInt32 lenLimit; UInt32 hashValue; const Byte *cur; UInt32 curMatch; \
+  lenLimit = p->lenLimit; { if (lenLimit < minLen) { MatchFinder_MovePos(p); ret_op; }} \
+  cur = p->buffer;
+
+#define GET_MATCHES_HEADER(minLen) GET_MATCHES_HEADER2(minLen, return 0)
+#define SKIP_HEADER(minLen)        GET_MATCHES_HEADER2(minLen, continue)
+
+#define MF_PARAMS(p) p->pos, p->buffer, p->son, p->cyclicBufferPos, p->cyclicBufferSize, p->cutValue
+
+#define GET_MATCHES_FOOTER(offset, maxLen) \
+  offset = (UInt32)(GetMatchesSpec1(lenLimit, curMatch, MF_PARAMS(p), \
+  distances + offset, maxLen) - distances); MOVE_POS_RET;
+
+#define SKIP_FOOTER \
+  SkipMatchesSpec(lenLimit, curMatch, MF_PARAMS(p)); MOVE_POS;
+
+static UInt32 Bt2_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 offset;
+  GET_MATCHES_HEADER(2)
+  HASH2_CALC;
+  curMatch = p->hash[hashValue];
+  p->hash[hashValue] = p->pos;
+  offset = 0;
+  GET_MATCHES_FOOTER(offset, 1)
+}
+
+UInt32 Bt3Zip_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 offset;
+  GET_MATCHES_HEADER(3)
+  HASH_ZIP_CALC;
+  curMatch = p->hash[hashValue];
+  p->hash[hashValue] = p->pos;
+  offset = 0;
+  GET_MATCHES_FOOTER(offset, 2)
+}
+
+static UInt32 Bt3_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 hash2Value, delta2, maxLen, offset;
+  GET_MATCHES_HEADER(3)
+
+  HASH3_CALC;
+
+  delta2 = p->pos - p->hash[hash2Value];
+  curMatch = p->hash[kFix3HashSize + hashValue];
+  
+  p->hash[hash2Value] =
+  p->hash[kFix3HashSize + hashValue] = p->pos;
+
+
+  maxLen = 2;
+  offset = 0;
+  if (delta2 < p->cyclicBufferSize && *(cur - delta2) == *cur)
+  {
+    for (; maxLen != lenLimit; maxLen++)
+      if (cur[(ptrdiff_t)maxLen - delta2] != cur[maxLen])
+        break;
+    distances[0] = maxLen;
+    distances[1] = delta2 - 1;
+    offset = 2;
+    if (maxLen == lenLimit)
+    {
+      SkipMatchesSpec(lenLimit, curMatch, MF_PARAMS(p));
+      MOVE_POS_RET;
+    }
+  }
+  GET_MATCHES_FOOTER(offset, maxLen)
+}
+
+static UInt32 Bt4_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 hash2Value, hash3Value, delta2, delta3, maxLen, offset;
+  GET_MATCHES_HEADER(4)
+
+  HASH4_CALC;
+
+  delta2 = p->pos - p->hash[                hash2Value];
+  delta3 = p->pos - p->hash[kFix3HashSize + hash3Value];
+  curMatch = p->hash[kFix4HashSize + hashValue];
+  
+  p->hash[                hash2Value] =
+  p->hash[kFix3HashSize + hash3Value] =
+  p->hash[kFix4HashSize + hashValue] = p->pos;
+
+  maxLen = 1;
+  offset = 0;
+  if (delta2 < p->cyclicBufferSize && *(cur - delta2) == *cur)
+  {
+    distances[0] = maxLen = 2;
+    distances[1] = delta2 - 1;
+    offset = 2;
+  }
+  if (delta2 != delta3 && delta3 < p->cyclicBufferSize && *(cur - delta3) == *cur)
+  {
+    maxLen = 3;
+    distances[offset + 1] = delta3 - 1;
+    offset += 2;
+    delta2 = delta3;
+  }
+  if (offset != 0)
+  {
+    for (; maxLen != lenLimit; maxLen++)
+      if (cur[(ptrdiff_t)maxLen - delta2] != cur[maxLen])
+        break;
+    distances[offset - 2] = maxLen;
+    if (maxLen == lenLimit)
+    {
+      SkipMatchesSpec(lenLimit, curMatch, MF_PARAMS(p));
+      MOVE_POS_RET;
+    }
+  }
+  if (maxLen < 3)
+    maxLen = 3;
+  GET_MATCHES_FOOTER(offset, maxLen)
+}
+
+static UInt32 Hc4_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 hash2Value, hash3Value, delta2, delta3, maxLen, offset;
+  GET_MATCHES_HEADER(4)
+
+  HASH4_CALC;
+
+  delta2 = p->pos - p->hash[                hash2Value];
+  delta3 = p->pos - p->hash[kFix3HashSize + hash3Value];
+  curMatch = p->hash[kFix4HashSize + hashValue];
+
+  p->hash[                hash2Value] =
+  p->hash[kFix3HashSize + hash3Value] =
+  p->hash[kFix4HashSize + hashValue] = p->pos;
+
+  maxLen = 1;
+  offset = 0;
+  if (delta2 < p->cyclicBufferSize && *(cur - delta2) == *cur)
+  {
+    distances[0] = maxLen = 2;
+    distances[1] = delta2 - 1;
+    offset = 2;
+  }
+  if (delta2 != delta3 && delta3 < p->cyclicBufferSize && *(cur - delta3) == *cur)
+  {
+    maxLen = 3;
+    distances[offset + 1] = delta3 - 1;
+    offset += 2;
+    delta2 = delta3;
+  }
+  if (offset != 0)
+  {
+    for (; maxLen != lenLimit; maxLen++)
+      if (cur[(ptrdiff_t)maxLen - delta2] != cur[maxLen])
+        break;
+    distances[offset - 2] = maxLen;
+    if (maxLen == lenLimit)
+    {
+      p->son[p->cyclicBufferPos] = curMatch;
+      MOVE_POS_RET;
+    }
+  }
+  if (maxLen < 3)
+    maxLen = 3;
+  offset = (UInt32)(Hc_GetMatchesSpec(lenLimit, curMatch, MF_PARAMS(p),
+    distances + offset, maxLen) - (distances));
+  MOVE_POS_RET
+}
+
+UInt32 Hc3Zip_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances)
+{
+  UInt32 offset;
+  GET_MATCHES_HEADER(3)
+  HASH_ZIP_CALC;
+  curMatch = p->hash[hashValue];
+  p->hash[hashValue] = p->pos;
+  offset = (UInt32)(Hc_GetMatchesSpec(lenLimit, curMatch, MF_PARAMS(p),
+    distances, 2) - (distances));
+  MOVE_POS_RET
+}
+
+static void Bt2_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    SKIP_HEADER(2)
+    HASH2_CALC;
+    curMatch = p->hash[hashValue];
+    p->hash[hashValue] = p->pos;
+    SKIP_FOOTER
+  }
+  while (--num != 0);
+}
+
+void Bt3Zip_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    SKIP_HEADER(3)
+    HASH_ZIP_CALC;
+    curMatch = p->hash[hashValue];
+    p->hash[hashValue] = p->pos;
+    SKIP_FOOTER
+  }
+  while (--num != 0);
+}
+
+static void Bt3_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    UInt32 hash2Value;
+    SKIP_HEADER(3)
+    HASH3_CALC;
+    curMatch = p->hash[kFix3HashSize + hashValue];
+    p->hash[hash2Value] =
+    p->hash[kFix3HashSize + hashValue] = p->pos;
+    SKIP_FOOTER
+  }
+  while (--num != 0);
+}
+
+static void Bt4_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    UInt32 hash2Value, hash3Value;
+    SKIP_HEADER(4)
+    HASH4_CALC;
+    curMatch = p->hash[kFix4HashSize + hashValue];
+    p->hash[                hash2Value] =
+    p->hash[kFix3HashSize + hash3Value] = p->pos;
+    p->hash[kFix4HashSize + hashValue] = p->pos;
+    SKIP_FOOTER
+  }
+  while (--num != 0);
+}
+
+static void Hc4_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    UInt32 hash2Value, hash3Value;
+    SKIP_HEADER(4)
+    HASH4_CALC;
+    curMatch = p->hash[kFix4HashSize + hashValue];
+    p->hash[                hash2Value] =
+    p->hash[kFix3HashSize + hash3Value] =
+    p->hash[kFix4HashSize + hashValue] = p->pos;
+    p->son[p->cyclicBufferPos] = curMatch;
+    MOVE_POS
+  }
+  while (--num != 0);
+}
+
+void Hc3Zip_MatchFinder_Skip(CMatchFinder *p, UInt32 num)
+{
+  do
+  {
+    SKIP_HEADER(3)
+    HASH_ZIP_CALC;
+    curMatch = p->hash[hashValue];
+    p->hash[hashValue] = p->pos;
+    p->son[p->cyclicBufferPos] = curMatch;
+    MOVE_POS
+  }
+  while (--num != 0);
+}
+
+void MatchFinder_CreateVTable(CMatchFinder *p, IMatchFinder *vTable)
+{
+  vTable->Init = (Mf_Init_Func)MatchFinder_Init;
+  vTable->GetIndexByte = (Mf_GetIndexByte_Func)MatchFinder_GetIndexByte;
+  vTable->GetNumAvailableBytes = (Mf_GetNumAvailableBytes_Func)MatchFinder_GetNumAvailableBytes;
+  vTable->GetPointerToCurrentPos = (Mf_GetPointerToCurrentPos_Func)MatchFinder_GetPointerToCurrentPos;
+  if (!p->btMode)
+  {
+    vTable->GetMatches = (Mf_GetMatches_Func)Hc4_MatchFinder_GetMatches;
+    vTable->Skip = (Mf_Skip_Func)Hc4_MatchFinder_Skip;
+  }
+  else if (p->numHashBytes == 2)
+  {
+    vTable->GetMatches = (Mf_GetMatches_Func)Bt2_MatchFinder_GetMatches;
+    vTable->Skip = (Mf_Skip_Func)Bt2_MatchFinder_Skip;
+  }
+  else if (p->numHashBytes == 3)
+  {
+    vTable->GetMatches = (Mf_GetMatches_Func)Bt3_MatchFinder_GetMatches;
+    vTable->Skip = (Mf_Skip_Func)Bt3_MatchFinder_Skip;
+  }
+  else
+  {
+    vTable->GetMatches = (Mf_GetMatches_Func)Bt4_MatchFinder_GetMatches;
+    vTable->Skip = (Mf_Skip_Func)Bt4_MatchFinder_Skip;
+  }
+}
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.h
new file mode 100644
index 0000000000..423d67e0c3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzFind.h
@@ -0,0 +1,107 @@
+/* LzFind.h -- Match finder for LZ algorithms
+2008-10-04 : Igor Pavlov : Public domain */
+
+#ifndef __LZFIND_H
+#define __LZFIND_H
+
+#include "Types.h"
+
+typedef UInt32 CLzRef;
+
+typedef struct _CMatchFinder
+{
+  Byte *buffer;
+  UInt32 pos;
+  UInt32 posLimit;
+  UInt32 streamPos;
+  UInt32 lenLimit;
+
+  UInt32 cyclicBufferPos;
+  UInt32 cyclicBufferSize; /* it must be = (historySize + 1) */
+
+  UInt32 matchMaxLen;
+  CLzRef *hash;
+  CLzRef *son;
+  UInt32 hashMask;
+  UInt32 cutValue;
+
+  Byte *bufferBase;
+  ISeqInStream *stream;
+  int streamEndWasReached;
+
+  UInt32 blockSize;
+  UInt32 keepSizeBefore;
+  UInt32 keepSizeAfter;
+
+  UInt32 numHashBytes;
+  int directInput;
+  int btMode;
+  /* int skipModeBits; */
+  int bigHash;
+  UInt32 historySize;
+  UInt32 fixedHashSize;
+  UInt32 hashSizeSum;
+  UInt32 numSons;
+  SRes result;
+  UInt32 crc[256];
+} CMatchFinder;
+
+#define Inline_MatchFinder_GetPointerToCurrentPos(p) ((p)->buffer)
+#define Inline_MatchFinder_GetIndexByte(p, index) ((p)->buffer[(Int32)(index)])
+
+#define Inline_MatchFinder_GetNumAvailableBytes(p) ((p)->streamPos - (p)->pos)
+
+int MatchFinder_NeedMove(CMatchFinder *p);
+Byte *MatchFinder_GetPointerToCurrentPos(CMatchFinder *p);
+void MatchFinder_MoveBlock(CMatchFinder *p);
+void MatchFinder_ReadIfRequired(CMatchFinder *p);
+
+void MatchFinder_Construct(CMatchFinder *p);
+
+/* Conditions:
+     historySize <= 3 GB
+     keepAddBufferBefore + matchMaxLen + keepAddBufferAfter < 511MB
+*/
+int MatchFinder_Create(CMatchFinder *p, UInt32 historySize,
+    UInt32 keepAddBufferBefore, UInt32 matchMaxLen, UInt32 keepAddBufferAfter,
+    ISzAlloc *alloc);
+void MatchFinder_Free(CMatchFinder *p, ISzAlloc *alloc);
+void MatchFinder_Normalize3(UInt32 subValue, CLzRef *items, UInt32 numItems);
+void MatchFinder_ReduceOffsets(CMatchFinder *p, UInt32 subValue);
+
+UInt32 * GetMatchesSpec1(UInt32 lenLimit, UInt32 curMatch, UInt32 pos, const Byte *buffer, CLzRef *son,
+    UInt32 _cyclicBufferPos, UInt32 _cyclicBufferSize, UInt32 _cutValue,
+    UInt32 *distances, UInt32 maxLen);
+
+/*
+Conditions:
+  Mf_GetNumAvailableBytes_Func must be called before each Mf_GetMatchLen_Func.
+  Mf_GetPointerToCurrentPos_Func's result must be used only before any other function
+*/
+
+typedef void (*Mf_Init_Func)(void *object);
+typedef Byte (*Mf_GetIndexByte_Func)(void *object, Int32 index);
+typedef UInt32 (*Mf_GetNumAvailableBytes_Func)(void *object);
+typedef const Byte * (*Mf_GetPointerToCurrentPos_Func)(void *object);
+typedef UInt32 (*Mf_GetMatches_Func)(void *object, UInt32 *distances);
+typedef void (*Mf_Skip_Func)(void *object, UInt32);
+
+typedef struct _IMatchFinder
+{
+  Mf_Init_Func Init;
+  Mf_GetIndexByte_Func GetIndexByte;
+  Mf_GetNumAvailableBytes_Func GetNumAvailableBytes;
+  Mf_GetPointerToCurrentPos_Func GetPointerToCurrentPos;
+  Mf_GetMatches_Func GetMatches;
+  Mf_Skip_Func Skip;
+} IMatchFinder;
+
+void MatchFinder_CreateVTable(CMatchFinder *p, IMatchFinder *vTable);
+
+void MatchFinder_Init(CMatchFinder *p);
+UInt32 Bt3Zip_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances);
+UInt32 Hc3Zip_MatchFinder_GetMatches(CMatchFinder *p, UInt32 *distances);
+void Bt3Zip_MatchFinder_Skip(CMatchFinder *p, UInt32 num);
+void Hc3Zip_MatchFinder_Skip(CMatchFinder *p, UInt32 num);
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzHash.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzHash.h
new file mode 100644
index 0000000000..c923417501
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzHash.h
@@ -0,0 +1,54 @@
+/* LzHash.h -- HASH functions for LZ algorithms
+2008-10-04 : Igor Pavlov : Public domain */
+
+#ifndef __LZHASH_H
+#define __LZHASH_H
+
+#define kHash2Size (1 << 10)
+#define kHash3Size (1 << 16)
+#define kHash4Size (1 << 20)
+
+#define kFix3HashSize (kHash2Size)
+#define kFix4HashSize (kHash2Size + kHash3Size)
+#define kFix5HashSize (kHash2Size + kHash3Size + kHash4Size)
+
+#define HASH2_CALC hashValue = cur[0] | ((UInt32)cur[1] << 8);
+
+#define HASH3_CALC { \
+  UInt32 temp = p->crc[cur[0]] ^ cur[1]; \
+  hash2Value = temp & (kHash2Size - 1); \
+  hashValue = (temp ^ ((UInt32)cur[2] << 8)) & p->hashMask; }
+
+#define HASH4_CALC { \
+  UInt32 temp = p->crc[cur[0]] ^ cur[1]; \
+  hash2Value = temp & (kHash2Size - 1); \
+  hash3Value = (temp ^ ((UInt32)cur[2] << 8)) & (kHash3Size - 1); \
+  hashValue = (temp ^ ((UInt32)cur[2] << 8) ^ (p->crc[cur[3]] << 5)) & p->hashMask; }
+
+#define HASH5_CALC { \
+  UInt32 temp = p->crc[cur[0]] ^ cur[1]; \
+  hash2Value = temp & (kHash2Size - 1); \
+  hash3Value = (temp ^ ((UInt32)cur[2] << 8)) & (kHash3Size - 1); \
+  hash4Value = (temp ^ ((UInt32)cur[2] << 8) ^ (p->crc[cur[3]] << 5)); \
+  hashValue = (hash4Value ^ (p->crc[cur[4]] << 3)) & p->hashMask; \
+  hash4Value &= (kHash4Size - 1); }
+
+/* #define HASH_ZIP_CALC hashValue = ((cur[0] | ((UInt32)cur[1] << 8)) ^ p->crc[cur[2]]) & 0xFFFF; */
+#define HASH_ZIP_CALC hashValue = ((cur[2] | ((UInt32)cur[0] << 8)) ^ p->crc[cur[1]]) & 0xFFFF;
+
+
+#define MT_HASH2_CALC \
+  hash2Value = (p->crc[cur[0]] ^ cur[1]) & (kHash2Size - 1);
+
+#define MT_HASH3_CALC { \
+  UInt32 temp = p->crc[cur[0]] ^ cur[1]; \
+  hash2Value = temp & (kHash2Size - 1); \
+  hash3Value = (temp ^ ((UInt32)cur[2] << 8)) & (kHash3Size - 1); }
+
+#define MT_HASH4_CALC { \
+  UInt32 temp = p->crc[cur[0]] ^ cur[1]; \
+  hash2Value = temp & (kHash2Size - 1); \
+  hash3Value = (temp ^ ((UInt32)cur[2] << 8)) & (kHash3Size - 1); \
+  hash4Value = (temp ^ ((UInt32)cur[2] << 8) ^ (p->crc[cur[3]] << 5)) & (kHash4Size - 1); }
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.c b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.c
new file mode 100644
index 0000000000..e3db4edbb4
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.c
@@ -0,0 +1,1026 @@
+/** @file
+  LzmaDec.c
+
+  Based on LZMA SDK 4.65:
+    LzmaDec.c -- LZMA Decoder
+    2008-11-06 : Igor Pavlov : Public domain
+
+  Copyright (c) 2009, 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 "LzmaDec.h"
+
+#ifndef EFIAPI
+
+#include <string.h>
+
+#endif // !EFIAPI
+
+#define kNumTopBits 24
+#define kTopValue ((UInt32)1 << kNumTopBits)
+
+#define kNumBitModelTotalBits 11
+#define kBitModelTotal (1 << kNumBitModelTotalBits)
+#define kNumMoveBits 5
+
+#define RC_INIT_SIZE 5
+
+#define NORMALIZE if (range < kTopValue) { range <<= 8; code = (code << 8) | (*buf++); }
+
+#define IF_BIT_0(p) ttt = *(p); NORMALIZE; bound = (range >> kNumBitModelTotalBits) * ttt; if (code < bound)
+#define UPDATE_0(p) range = bound; *(p) = (CLzmaProb)(ttt + ((kBitModelTotal - ttt) >> kNumMoveBits));
+#define UPDATE_1(p) range -= bound; code -= bound; *(p) = (CLzmaProb)(ttt - (ttt >> kNumMoveBits));
+#define GET_BIT2(p, i, A0, A1) IF_BIT_0(p) \
+  { UPDATE_0(p); i = (i + i); A0; } else \
+  { UPDATE_1(p); i = (i + i) + 1; A1; }
+#define GET_BIT(p, i) GET_BIT2(p, i, ; , ;)
+
+#define TREE_GET_BIT(probs, i) { GET_BIT((probs + i), i); }
+#define TREE_DECODE(probs, limit, i) \
+  { i = 1; do { TREE_GET_BIT(probs, i); } while (i < limit); i -= limit; }
+
+/* #define _LZMA_SIZE_OPT */
+
+#ifdef _LZMA_SIZE_OPT
+#define TREE_6_DECODE(probs, i) TREE_DECODE(probs, (1 << 6), i)
+#else
+#define TREE_6_DECODE(probs, i) \
+  { i = 1; \
+  TREE_GET_BIT(probs, i); \
+  TREE_GET_BIT(probs, i); \
+  TREE_GET_BIT(probs, i); \
+  TREE_GET_BIT(probs, i); \
+  TREE_GET_BIT(probs, i); \
+  TREE_GET_BIT(probs, i); \
+  i -= 0x40; }
+#endif
+
+#define NORMALIZE_CHECK if (range < kTopValue) { if (buf >= bufLimit) return DUMMY_ERROR; range <<= 8; code = (code << 8) | (*buf++); }
+
+#define IF_BIT_0_CHECK(p) ttt = *(p); NORMALIZE_CHECK; bound = (range >> kNumBitModelTotalBits) * ttt; if (code < bound)
+#define UPDATE_0_CHECK range = bound;
+#define UPDATE_1_CHECK range -= bound; code -= bound;
+#define GET_BIT2_CHECK(p, i, A0, A1) IF_BIT_0_CHECK(p) \
+  { UPDATE_0_CHECK; i = (i + i); A0; } else \
+  { UPDATE_1_CHECK; i = (i + i) + 1; A1; }
+#define GET_BIT_CHECK(p, i) GET_BIT2_CHECK(p, i, ; , ;)
+#define TREE_DECODE_CHECK(probs, limit, i) \
+  { i = 1; do { GET_BIT_CHECK(probs + i, i) } while (i < limit); i -= limit; }
+
+
+#define kNumPosBitsMax 4
+#define kNumPosStatesMax (1 << kNumPosBitsMax)
+
+#define kLenNumLowBits 3
+#define kLenNumLowSymbols (1 << kLenNumLowBits)
+#define kLenNumMidBits 3
+#define kLenNumMidSymbols (1 << kLenNumMidBits)
+#define kLenNumHighBits 8
+#define kLenNumHighSymbols (1 << kLenNumHighBits)
+
+#define LenChoice 0
+#define LenChoice2 (LenChoice + 1)
+#define LenLow (LenChoice2 + 1)
+#define LenMid (LenLow + (kNumPosStatesMax << kLenNumLowBits))
+#define LenHigh (LenMid + (kNumPosStatesMax << kLenNumMidBits))
+#define kNumLenProbs (LenHigh + kLenNumHighSymbols)
+
+
+#define kNumStates 12
+#define kNumLitStates 7
+
+#define kStartPosModelIndex 4
+#define kEndPosModelIndex 14
+#define kNumFullDistances (1 << (kEndPosModelIndex >> 1))
+
+#define kNumPosSlotBits 6
+#define kNumLenToPosStates 4
+
+#define kNumAlignBits 4
+#define kAlignTableSize (1 << kNumAlignBits)
+
+#define kMatchMinLen 2
+#define kMatchSpecLenStart (kMatchMinLen + kLenNumLowSymbols + kLenNumMidSymbols + kLenNumHighSymbols)
+
+#define IsMatch 0
+#define IsRep (IsMatch + (kNumStates << kNumPosBitsMax))
+#define IsRepG0 (IsRep + kNumStates)
+#define IsRepG1 (IsRepG0 + kNumStates)
+#define IsRepG2 (IsRepG1 + kNumStates)
+#define IsRep0Long (IsRepG2 + kNumStates)
+#define PosSlot (IsRep0Long + (kNumStates << kNumPosBitsMax))
+#define SpecPos (PosSlot + (kNumLenToPosStates << kNumPosSlotBits))
+#define Align (SpecPos + kNumFullDistances - kEndPosModelIndex)
+#define LenCoder (Align + kAlignTableSize)
+#define RepLenCoder (LenCoder + kNumLenProbs)
+#define Literal (RepLenCoder + kNumLenProbs)
+
+#define LZMA_BASE_SIZE 1846
+#define LZMA_LIT_SIZE 768
+
+#define LzmaProps_GetNumProbs(p) ((UInt32)LZMA_BASE_SIZE + (LZMA_LIT_SIZE << ((p)->lc + (p)->lp)))
+
+#if Literal != LZMA_BASE_SIZE
+StopCompilingDueBUG
+#endif
+
+static const Byte kLiteralNextStates[kNumStates * 2] =
+{
+  0, 0, 0, 0, 1, 2, 3,  4,  5,  6,  4,  5,
+  7, 7, 7, 7, 7, 7, 7, 10, 10, 10, 10, 10
+};
+
+#define LZMA_DIC_MIN (1 << 12)
+
+/* First LZMA-symbol is always decoded.
+And it decodes new LZMA-symbols while (buf < bufLimit), but "buf" is without last normalization
+Out:
+  Result:
+    SZ_OK - OK
+    SZ_ERROR_DATA - Error
+  p->remainLen:
+    < kMatchSpecLenStart : normal remain
+    = kMatchSpecLenStart : finished
+    = kMatchSpecLenStart + 1 : Flush marker
+    = kMatchSpecLenStart + 2 : State Init Marker
+*/
+
+static int MY_FAST_CALL LzmaDec_DecodeReal(CLzmaDec *p, SizeT limit, const Byte *bufLimit)
+{
+  CLzmaProb *probs = p->probs;
+
+  unsigned state = p->state;
+  UInt32 rep0 = p->reps[0], rep1 = p->reps[1], rep2 = p->reps[2], rep3 = p->reps[3];
+  unsigned pbMask = ((unsigned)1 << (p->prop.pb)) - 1;
+  unsigned lpMask = ((unsigned)1 << (p->prop.lp)) - 1;
+  unsigned lc = p->prop.lc;
+
+  Byte *dic = p->dic;
+  SizeT dicBufSize = p->dicBufSize;
+  SizeT dicPos = p->dicPos;
+  
+  UInt32 processedPos = p->processedPos;
+  UInt32 checkDicSize = p->checkDicSize;
+  unsigned len = 0;
+
+  const Byte *buf = p->buf;
+  UInt32 range = p->range;
+  UInt32 code = p->code;
+
+  do
+  {
+    CLzmaProb *prob;
+    UInt32 bound;
+    unsigned ttt;
+    unsigned posState = processedPos & pbMask;
+
+    prob = probs + IsMatch + (state << kNumPosBitsMax) + posState;
+    IF_BIT_0(prob)
+    {
+      unsigned symbol;
+      UPDATE_0(prob);
+      prob = probs + Literal;
+      if (checkDicSize != 0 || processedPos != 0)
+        prob += (LZMA_LIT_SIZE * (((processedPos & lpMask) << lc) +
+        (dic[(dicPos == 0 ? dicBufSize : dicPos) - 1] >> (8 - lc))));
+
+      if (state < kNumLitStates)
+      {
+        symbol = 1;
+        do { GET_BIT(prob + symbol, symbol) } while (symbol < 0x100);
+      }
+      else
+      {
+        unsigned matchByte = p->dic[(dicPos - rep0) + ((dicPos < rep0) ? dicBufSize : 0)];
+        unsigned offs = 0x100;
+        symbol = 1;
+        do
+        {
+          unsigned bit;
+          CLzmaProb *probLit;
+          matchByte <<= 1;
+          bit = (matchByte & offs);
+          probLit = prob + offs + bit + symbol;
+          GET_BIT2(probLit, symbol, offs &= ~bit, offs &= bit)
+        }
+        while (symbol < 0x100);
+      }
+      dic[dicPos++] = (Byte)symbol;
+      processedPos++;
+
+      state = kLiteralNextStates[state];
+      /* if (state < 4) state = 0; else if (state < 10) state -= 3; else state -= 6; */
+      continue;
+    }
+    else
+    {
+      UPDATE_1(prob);
+      prob = probs + IsRep + state;
+      IF_BIT_0(prob)
+      {
+        UPDATE_0(prob);
+        state += kNumStates;
+        prob = probs + LenCoder;
+      }
+      else
+      {
+        UPDATE_1(prob);
+        if (checkDicSize == 0 && processedPos == 0)
+          return SZ_ERROR_DATA;
+        prob = probs + IsRepG0 + state;
+        IF_BIT_0(prob)
+        {
+          UPDATE_0(prob);
+          prob = probs + IsRep0Long + (state << kNumPosBitsMax) + posState;
+          IF_BIT_0(prob)
+          {
+            UPDATE_0(prob);
+            dic[dicPos] = dic[(dicPos - rep0) + ((dicPos < rep0) ? dicBufSize : 0)];
+            dicPos++;
+            processedPos++;
+            state = state < kNumLitStates ? 9 : 11;
+            continue;
+          }
+          UPDATE_1(prob);
+        }
+        else
+        {
+          UInt32 distance;
+          UPDATE_1(prob);
+          prob = probs + IsRepG1 + state;
+          IF_BIT_0(prob)
+          {
+            UPDATE_0(prob);
+            distance = rep1;
+          }
+          else
+          {
+            UPDATE_1(prob);
+            prob = probs + IsRepG2 + state;
+            IF_BIT_0(prob)
+            {
+              UPDATE_0(prob);
+              distance = rep2;
+            }
+            else
+            {
+              UPDATE_1(prob);
+              distance = rep3;
+              rep3 = rep2;
+            }
+            rep2 = rep1;
+          }
+          rep1 = rep0;
+          rep0 = distance;
+        }
+        state = state < kNumLitStates ? 8 : 11;
+        prob = probs + RepLenCoder;
+      }
+      {
+        unsigned limit2, offset;
+        CLzmaProb *probLen = prob + LenChoice;
+        IF_BIT_0(probLen)
+        {
+          UPDATE_0(probLen);
+          probLen = prob + LenLow + (posState << kLenNumLowBits);
+          offset = 0;
+          limit2 = (1 << kLenNumLowBits);
+        }
+        else
+        {
+          UPDATE_1(probLen);
+          probLen = prob + LenChoice2;
+          IF_BIT_0(probLen)
+          {
+            UPDATE_0(probLen);
+            probLen = prob + LenMid + (posState << kLenNumMidBits);
+            offset = kLenNumLowSymbols;
+            limit2 = (1 << kLenNumMidBits);
+          }
+          else
+          {
+            UPDATE_1(probLen);
+            probLen = prob + LenHigh;
+            offset = kLenNumLowSymbols + kLenNumMidSymbols;
+            limit2 = (1 << kLenNumHighBits);
+          }
+        }
+        TREE_DECODE(probLen, limit2, len);
+        len += offset;
+      }
+
+      if (state >= kNumStates)
+      {
+        UInt32 distance;
+        prob = probs + PosSlot +
+            ((len < kNumLenToPosStates ? len : kNumLenToPosStates - 1) << kNumPosSlotBits);
+        TREE_6_DECODE(prob, distance);
+        if (distance >= kStartPosModelIndex)
+        {
+          unsigned posSlot = (unsigned)distance;
+          int numDirectBits = (int)(((distance >> 1) - 1));
+          distance = (2 | (distance & 1));
+          if (posSlot < kEndPosModelIndex)
+          {
+            distance <<= numDirectBits;
+            prob = probs + SpecPos + distance - posSlot - 1;
+            {
+              UInt32 mask = 1;
+              unsigned i = 1;
+              do
+              {
+                GET_BIT2(prob + i, i, ; , distance |= mask);
+                mask <<= 1;
+              }
+              while (--numDirectBits != 0);
+            }
+          }
+          else
+          {
+            numDirectBits -= kNumAlignBits;
+            do
+            {
+              NORMALIZE
+              range >>= 1;
+              
+              {
+                UInt32 t;
+                code -= range;
+                t = (0 - ((UInt32)code >> 31)); /* (UInt32)((Int32)code >> 31) */
+                distance = (distance << 1) + (t + 1);
+                code += range & t;
+              }
+              /*
+              distance <<= 1;
+              if (code >= range)
+              {
+                code -= range;
+                distance |= 1;
+              }
+              */
+            }
+            while (--numDirectBits != 0);
+            prob = probs + Align;
+            distance <<= kNumAlignBits;
+            {
+              unsigned i = 1;
+              GET_BIT2(prob + i, i, ; , distance |= 1);
+              GET_BIT2(prob + i, i, ; , distance |= 2);
+              GET_BIT2(prob + i, i, ; , distance |= 4);
+              GET_BIT2(prob + i, i, ; , distance |= 8);
+            }
+            if (distance == (UInt32)0xFFFFFFFF)
+            {
+              len += kMatchSpecLenStart;
+              state -= kNumStates;
+              break;
+            }
+          }
+        }
+        rep3 = rep2;
+        rep2 = rep1;
+        rep1 = rep0;
+        rep0 = distance + 1;
+        if (checkDicSize == 0)
+        {
+          if (distance >= processedPos)
+            return SZ_ERROR_DATA;
+        }
+        else if (distance >= checkDicSize)
+          return SZ_ERROR_DATA;
+        state = (state < kNumStates + kNumLitStates) ? kNumLitStates : kNumLitStates + 3;
+        /* state = kLiteralNextStates[state]; */
+      }
+
+      len += kMatchMinLen;
+
+      if (limit == dicPos)
+        return SZ_ERROR_DATA;
+      {
+        SizeT rem = limit - dicPos;
+        unsigned curLen = ((rem < len) ? (unsigned)rem : len);
+        SizeT pos = (dicPos - rep0) + ((dicPos < rep0) ? dicBufSize : 0);
+
+        processedPos += curLen;
+
+        len -= curLen;
+        if (pos + curLen <= dicBufSize)
+        {
+          Byte *dest = dic + dicPos;
+          ptrdiff_t src = (ptrdiff_t)pos - (ptrdiff_t)dicPos;
+          const Byte *lim = dest + curLen;
+          dicPos += curLen;
+          do
+            *((volatile Byte *)dest) = (Byte)*(dest + src);
+          while (++dest != lim);
+        }
+        else
+        {
+          do
+          {
+            dic[dicPos++] = dic[pos];
+            if (++pos == dicBufSize)
+              pos = 0;
+          }
+          while (--curLen != 0);
+        }
+      }
+    }
+  }
+  while (dicPos < limit && buf < bufLimit);
+  NORMALIZE;
+  p->buf = buf;
+  p->range = range;
+  p->code = code;
+  p->remainLen = len;
+  p->dicPos = dicPos;
+  p->processedPos = processedPos;
+  p->reps[0] = rep0;
+  p->reps[1] = rep1;
+  p->reps[2] = rep2;
+  p->reps[3] = rep3;
+  p->state = state;
+
+  return SZ_OK;
+}
+
+static void MY_FAST_CALL LzmaDec_WriteRem(CLzmaDec *p, SizeT limit)
+{
+  if (p->remainLen != 0 && p->remainLen < kMatchSpecLenStart)
+  {
+    Byte *dic = p->dic;
+    SizeT dicPos = p->dicPos;
+    SizeT dicBufSize = p->dicBufSize;
+    unsigned len = p->remainLen;
+    UInt32 rep0 = p->reps[0];
+    if (limit - dicPos < len)
+      len = (unsigned)(limit - dicPos);
+
+    if (p->checkDicSize == 0 && p->prop.dicSize - p->processedPos <= len)
+      p->checkDicSize = p->prop.dicSize;
+
+    p->processedPos += len;
+    p->remainLen -= len;
+    while (len-- != 0)
+    {
+      dic[dicPos] = dic[(dicPos - rep0) + ((dicPos < rep0) ? dicBufSize : 0)];
+      dicPos++;
+    }
+    p->dicPos = dicPos;
+  }
+}
+
+static int MY_FAST_CALL LzmaDec_DecodeReal2(CLzmaDec *p, SizeT limit, const Byte *bufLimit)
+{
+  do
+  {
+    SizeT limit2 = limit;
+    if (p->checkDicSize == 0)
+    {
+      UInt32 rem = p->prop.dicSize - p->processedPos;
+      if (limit - p->dicPos > rem)
+        limit2 = p->dicPos + rem;
+    }
+    RINOK(LzmaDec_DecodeReal(p, limit2, bufLimit));
+    if (p->processedPos >= p->prop.dicSize)
+      p->checkDicSize = p->prop.dicSize;
+    LzmaDec_WriteRem(p, limit);
+  }
+  while (p->dicPos < limit && p->buf < bufLimit && p->remainLen < kMatchSpecLenStart);
+
+  if (p->remainLen > kMatchSpecLenStart)
+  {
+    p->remainLen = kMatchSpecLenStart;
+  }
+  return 0;
+}
+
+typedef enum
+{
+  DUMMY_ERROR, /* unexpected end of input stream */
+  DUMMY_LIT,
+  DUMMY_MATCH,
+  DUMMY_REP
+} ELzmaDummy;
+
+static ELzmaDummy LzmaDec_TryDummy(const CLzmaDec *p, const Byte *buf, SizeT inSize)
+{
+  UInt32 range = p->range;
+  UInt32 code = p->code;
+  const Byte *bufLimit = buf + inSize;
+  CLzmaProb *probs = p->probs;
+  unsigned state = p->state;
+  ELzmaDummy res;
+
+  {
+    CLzmaProb *prob;
+    UInt32 bound;
+    unsigned ttt;
+    unsigned posState = (p->processedPos) & ((1 << p->prop.pb) - 1);
+
+    prob = probs + IsMatch + (state << kNumPosBitsMax) + posState;
+    IF_BIT_0_CHECK(prob)
+    {
+      UPDATE_0_CHECK
+
+      /* if (bufLimit - buf >= 7) return DUMMY_LIT; */
+
+      prob = probs + Literal;
+      if (p->checkDicSize != 0 || p->processedPos != 0)
+        prob += (LZMA_LIT_SIZE *
+          ((((p->processedPos) & ((1 << (p->prop.lp)) - 1)) << p->prop.lc) +
+          (p->dic[(p->dicPos == 0 ? p->dicBufSize : p->dicPos) - 1] >> (8 - p->prop.lc))));
+
+      if (state < kNumLitStates)
+      {
+        unsigned symbol = 1;
+        do { GET_BIT_CHECK(prob + symbol, symbol) } while (symbol < 0x100);
+      }
+      else
+      {
+        unsigned matchByte = p->dic[p->dicPos - p->reps[0] +
+            ((p->dicPos < p->reps[0]) ? p->dicBufSize : 0)];
+        unsigned offs = 0x100;
+        unsigned symbol = 1;
+        do
+        {
+          unsigned bit;
+          CLzmaProb *probLit;
+          matchByte <<= 1;
+          bit = (matchByte & offs);
+          probLit = prob + offs + bit + symbol;
+          GET_BIT2_CHECK(probLit, symbol, offs &= ~bit, offs &= bit)
+        }
+        while (symbol < 0x100);
+      }
+      res = DUMMY_LIT;
+    }
+    else
+    {
+      unsigned len;
+      UPDATE_1_CHECK;
+
+      prob = probs + IsRep + state;
+      IF_BIT_0_CHECK(prob)
+      {
+        UPDATE_0_CHECK;
+        state = 0;
+        prob = probs + LenCoder;
+        res = DUMMY_MATCH;
+      }
+      else
+      {
+        UPDATE_1_CHECK;
+        res = DUMMY_REP;
+        prob = probs + IsRepG0 + state;
+        IF_BIT_0_CHECK(prob)
+        {
+          UPDATE_0_CHECK;
+          prob = probs + IsRep0Long + (state << kNumPosBitsMax) + posState;
+          IF_BIT_0_CHECK(prob)
+          {
+            UPDATE_0_CHECK;
+            NORMALIZE_CHECK;
+            return DUMMY_REP;
+          }
+          else
+          {
+            UPDATE_1_CHECK;
+          }
+        }
+        else
+        {
+          UPDATE_1_CHECK;
+          prob = probs + IsRepG1 + state;
+          IF_BIT_0_CHECK(prob)
+          {
+            UPDATE_0_CHECK;
+          }
+          else
+          {
+            UPDATE_1_CHECK;
+            prob = probs + IsRepG2 + state;
+            IF_BIT_0_CHECK(prob)
+            {
+              UPDATE_0_CHECK;
+            }
+            else
+            {
+              UPDATE_1_CHECK;
+            }
+          }
+        }
+        state = kNumStates;
+        prob = probs + RepLenCoder;
+      }
+      {
+        unsigned limit, offset;
+        CLzmaProb *probLen = prob + LenChoice;
+        IF_BIT_0_CHECK(probLen)
+        {
+          UPDATE_0_CHECK;
+          probLen = prob + LenLow + (posState << kLenNumLowBits);
+          offset = 0;
+          limit = 1 << kLenNumLowBits;
+        }
+        else
+        {
+          UPDATE_1_CHECK;
+          probLen = prob + LenChoice2;
+          IF_BIT_0_CHECK(probLen)
+          {
+            UPDATE_0_CHECK;
+            probLen = prob + LenMid + (posState << kLenNumMidBits);
+            offset = kLenNumLowSymbols;
+            limit = 1 << kLenNumMidBits;
+          }
+          else
+          {
+            UPDATE_1_CHECK;
+            probLen = prob + LenHigh;
+            offset = kLenNumLowSymbols + kLenNumMidSymbols;
+            limit = 1 << kLenNumHighBits;
+          }
+        }
+        TREE_DECODE_CHECK(probLen, limit, len);
+        len += offset;
+      }
+
+      if (state < 4)
+      {
+        unsigned posSlot;
+        prob = probs + PosSlot +
+            ((len < kNumLenToPosStates ? len : kNumLenToPosStates - 1) <<
+            kNumPosSlotBits);
+        TREE_DECODE_CHECK(prob, 1 << kNumPosSlotBits, posSlot);
+        if (posSlot >= kStartPosModelIndex)
+        {
+          int numDirectBits = ((posSlot >> 1) - 1);
+
+          /* if (bufLimit - buf >= 8) return DUMMY_MATCH; */
+
+          if (posSlot < kEndPosModelIndex)
+          {
+            prob = probs + SpecPos + ((2 | (posSlot & 1)) << numDirectBits) - posSlot - 1;
+          }
+          else
+          {
+            numDirectBits -= kNumAlignBits;
+            do
+            {
+              NORMALIZE_CHECK
+              range >>= 1;
+              code -= range & (((code - range) >> 31) - 1);
+              /* if (code >= range) code -= range; */
+            }
+            while (--numDirectBits != 0);
+            prob = probs + Align;
+            numDirectBits = kNumAlignBits;
+          }
+          {
+            unsigned i = 1;
+            do
+            {
+              GET_BIT_CHECK(prob + i, i);
+            }
+            while (--numDirectBits != 0);
+          }
+        }
+      }
+    }
+  }
+  NORMALIZE_CHECK;
+  return res;
+}
+
+static void LzmaDec_InitRc(CLzmaDec *p, const Byte *data)
+{
+  p->code = ((UInt32)data[1] << 24) | ((UInt32)data[2] << 16) | ((UInt32)data[3] << 8) | ((UInt32)data[4]);
+  p->range = 0xFFFFFFFF;
+  p->needFlush = 0;
+}
+
+void LzmaDec_InitDicAndState(CLzmaDec *p, Bool initDic, Bool initState)
+{
+  p->needFlush = 1;
+  p->remainLen = 0;
+  p->tempBufSize = 0;
+
+  if (initDic)
+  {
+    p->processedPos = 0;
+    p->checkDicSize = 0;
+    p->needInitState = 1;
+  }
+  if (initState)
+    p->needInitState = 1;
+}
+
+void LzmaDec_Init(CLzmaDec *p)
+{
+  p->dicPos = 0;
+  LzmaDec_InitDicAndState(p, True, True);
+}
+
+static void LzmaDec_InitStateReal(CLzmaDec *p)
+{
+  UInt32 numProbs = Literal + ((UInt32)LZMA_LIT_SIZE << (p->prop.lc + p->prop.lp));
+  UInt32 i;
+  CLzmaProb *probs = p->probs;
+  for (i = 0; i < numProbs; i++)
+    probs[i] = kBitModelTotal >> 1;
+  p->reps[0] = p->reps[1] = p->reps[2] = p->reps[3] = 1;
+  p->state = 0;
+  p->needInitState = 0;
+}
+
+SRes LzmaDec_DecodeToDic(CLzmaDec *p, SizeT dicLimit, const Byte *src, SizeT *srcLen,
+    ELzmaFinishMode finishMode, ELzmaStatus *status)
+{
+  SizeT inSize = *srcLen;
+  (*srcLen) = 0;
+  LzmaDec_WriteRem(p, dicLimit);
+  
+  *status = LZMA_STATUS_NOT_SPECIFIED;
+
+  while (p->remainLen != kMatchSpecLenStart)
+  {
+      int checkEndMarkNow;
+
+      if (p->needFlush != 0)
+      {
+        for (; inSize > 0 && p->tempBufSize < RC_INIT_SIZE; (*srcLen)++, inSize--)
+          p->tempBuf[p->tempBufSize++] = *src++;
+        if (p->tempBufSize < RC_INIT_SIZE)
+        {
+          *status = LZMA_STATUS_NEEDS_MORE_INPUT;
+          return SZ_OK;
+        }
+        if (p->tempBuf[0] != 0)
+          return SZ_ERROR_DATA;
+
+        LzmaDec_InitRc(p, p->tempBuf);
+        p->tempBufSize = 0;
+      }
+
+      checkEndMarkNow = 0;
+      if (p->dicPos >= dicLimit)
+      {
+        if (p->remainLen == 0 && p->code == 0)
+        {
+          *status = LZMA_STATUS_MAYBE_FINISHED_WITHOUT_MARK;
+          return SZ_OK;
+        }
+        if (finishMode == LZMA_FINISH_ANY)
+        {
+          *status = LZMA_STATUS_NOT_FINISHED;
+          return SZ_OK;
+        }
+        if (p->remainLen != 0)
+        {
+          *status = LZMA_STATUS_NOT_FINISHED;
+          return SZ_ERROR_DATA;
+        }
+        checkEndMarkNow = 1;
+      }
+
+      if (p->needInitState)
+        LzmaDec_InitStateReal(p);
+  
+      if (p->tempBufSize == 0)
+      {
+        SizeT processed;
+        const Byte *bufLimit;
+        if (inSize < LZMA_REQUIRED_INPUT_MAX || checkEndMarkNow)
+        {
+          int dummyRes = LzmaDec_TryDummy(p, src, inSize);
+          if (dummyRes == DUMMY_ERROR)
+          {
+            memcpy(p->tempBuf, src, inSize);
+            p->tempBufSize = (unsigned)inSize;
+            (*srcLen) += inSize;
+            *status = LZMA_STATUS_NEEDS_MORE_INPUT;
+            return SZ_OK;
+          }
+          if (checkEndMarkNow && dummyRes != DUMMY_MATCH)
+          {
+            *status = LZMA_STATUS_NOT_FINISHED;
+            return SZ_ERROR_DATA;
+          }
+          bufLimit = src;
+        }
+        else
+          bufLimit = src + inSize - LZMA_REQUIRED_INPUT_MAX;
+        p->buf = src;
+        if (LzmaDec_DecodeReal2(p, dicLimit, bufLimit) != 0)
+          return SZ_ERROR_DATA;
+        processed = (SizeT)(p->buf - src);
+        (*srcLen) += processed;
+        src += processed;
+        inSize -= processed;
+      }
+      else
+      {
+        unsigned rem = p->tempBufSize, lookAhead = 0;
+        while (rem < LZMA_REQUIRED_INPUT_MAX && lookAhead < inSize)
+          p->tempBuf[rem++] = src[lookAhead++];
+        p->tempBufSize = rem;
+        if (rem < LZMA_REQUIRED_INPUT_MAX || checkEndMarkNow)
+        {
+          int dummyRes = LzmaDec_TryDummy(p, p->tempBuf, rem);
+          if (dummyRes == DUMMY_ERROR)
+          {
+            (*srcLen) += lookAhead;
+            *status = LZMA_STATUS_NEEDS_MORE_INPUT;
+            return SZ_OK;
+          }
+          if (checkEndMarkNow && dummyRes != DUMMY_MATCH)
+          {
+            *status = LZMA_STATUS_NOT_FINISHED;
+            return SZ_ERROR_DATA;
+          }
+        }
+        p->buf = p->tempBuf;
+        if (LzmaDec_DecodeReal2(p, dicLimit, p->buf) != 0)
+          return SZ_ERROR_DATA;
+        lookAhead -= (rem - (unsigned)(p->buf - p->tempBuf));
+        (*srcLen) += lookAhead;
+        src += lookAhead;
+        inSize -= lookAhead;
+        p->tempBufSize = 0;
+      }
+  }
+  if (p->code == 0)
+    *status = LZMA_STATUS_FINISHED_WITH_MARK;
+  return (p->code == 0) ? SZ_OK : SZ_ERROR_DATA;
+}
+
+SRes LzmaDec_DecodeToBuf(CLzmaDec *p, Byte *dest, SizeT *destLen, const Byte *src, SizeT *srcLen, ELzmaFinishMode finishMode, ELzmaStatus *status)
+{
+  SizeT outSize = *destLen;
+  SizeT inSize = *srcLen;
+  *srcLen = *destLen = 0;
+  for (;;)
+  {
+    SizeT inSizeCur = inSize, outSizeCur, dicPos;
+    ELzmaFinishMode curFinishMode;
+    SRes res;
+    if (p->dicPos == p->dicBufSize)
+      p->dicPos = 0;
+    dicPos = p->dicPos;
+    if (outSize > p->dicBufSize - dicPos)
+    {
+      outSizeCur = p->dicBufSize;
+      curFinishMode = LZMA_FINISH_ANY;
+    }
+    else
+    {
+      outSizeCur = dicPos + outSize;
+      curFinishMode = finishMode;
+    }
+
+    res = LzmaDec_DecodeToDic(p, outSizeCur, src, &inSizeCur, curFinishMode, status);
+    src += inSizeCur;
+    inSize -= inSizeCur;
+    *srcLen += inSizeCur;
+    outSizeCur = p->dicPos - dicPos;
+    memcpy(dest, p->dic + dicPos, outSizeCur);
+    dest += outSizeCur;
+    outSize -= outSizeCur;
+    *destLen += outSizeCur;
+    if (res != 0)
+      return res;
+    if (outSizeCur == 0 || outSize == 0)
+      return SZ_OK;
+  }
+}
+
+void LzmaDec_FreeProbs(CLzmaDec *p, ISzAlloc *alloc)
+{
+  alloc->Free(alloc, p->probs);
+  p->probs = 0;
+}
+
+static void LzmaDec_FreeDict(CLzmaDec *p, ISzAlloc *alloc)
+{
+  alloc->Free(alloc, p->dic);
+  p->dic = 0;
+}
+
+void LzmaDec_Free(CLzmaDec *p, ISzAlloc *alloc)
+{
+  LzmaDec_FreeProbs(p, alloc);
+  LzmaDec_FreeDict(p, alloc);
+}
+
+SRes LzmaProps_Decode(CLzmaProps *p, const Byte *data, unsigned size)
+{
+  UInt32 dicSize;
+  Byte d;
+  
+  if (size < LZMA_PROPS_SIZE)
+    return SZ_ERROR_UNSUPPORTED;
+  else
+    dicSize = data[1] | ((UInt32)data[2] << 8) | ((UInt32)data[3] << 16) | ((UInt32)data[4] << 24);
+ 
+  if (dicSize < LZMA_DIC_MIN)
+    dicSize = LZMA_DIC_MIN;
+  p->dicSize = dicSize;
+
+  d = data[0];
+  if (d >= (9 * 5 * 5))
+    return SZ_ERROR_UNSUPPORTED;
+
+  p->lc = d % 9;
+  d /= 9;
+  p->pb = d / 5;
+  p->lp = d % 5;
+
+  return SZ_OK;
+}
+
+static SRes LzmaDec_AllocateProbs2(CLzmaDec *p, const CLzmaProps *propNew, ISzAlloc *alloc)
+{
+  UInt32 numProbs = LzmaProps_GetNumProbs(propNew);
+  if (p->probs == 0 || numProbs != p->numProbs)
+  {
+    LzmaDec_FreeProbs(p, alloc);
+    p->probs = (CLzmaProb *)alloc->Alloc(alloc, numProbs * sizeof(CLzmaProb));
+    p->numProbs = numProbs;
+    if (p->probs == 0)
+      return SZ_ERROR_MEM;
+  }
+  return SZ_OK;
+}
+
+SRes LzmaDec_AllocateProbs(CLzmaDec *p, const Byte *props, unsigned propsSize, ISzAlloc *alloc)
+{
+  CLzmaProps propNew;
+  RINOK(LzmaProps_Decode(&propNew, props, propsSize));
+  RINOK(LzmaDec_AllocateProbs2(p, &propNew, alloc));
+  p->prop = propNew;
+  return SZ_OK;
+}
+
+SRes LzmaDec_Allocate(CLzmaDec *p, const Byte *props, unsigned propsSize, ISzAlloc *alloc)
+{
+  CLzmaProps propNew;
+  SizeT dicBufSize;
+  RINOK(LzmaProps_Decode(&propNew, props, propsSize));
+  RINOK(LzmaDec_AllocateProbs2(p, &propNew, alloc));
+  dicBufSize = propNew.dicSize;
+  if (p->dic == 0 || dicBufSize != p->dicBufSize)
+  {
+    LzmaDec_FreeDict(p, alloc);
+    p->dic = (Byte *)alloc->Alloc(alloc, dicBufSize);
+    if (p->dic == 0)
+    {
+      LzmaDec_FreeProbs(p, alloc);
+      return SZ_ERROR_MEM;
+    }
+  }
+  p->dicBufSize = dicBufSize;
+  p->prop = propNew;
+  return SZ_OK;
+}
+
+SRes LzmaDecode(Byte *dest, SizeT *destLen, const Byte *src, SizeT *srcLen,
+    const Byte *propData, unsigned propSize, ELzmaFinishMode finishMode,
+    ELzmaStatus *status, ISzAlloc *alloc)
+{
+  CLzmaDec p;
+  SRes res;
+  SizeT inSize = *srcLen;
+  SizeT outSize = *destLen;
+  *srcLen = *destLen = 0;
+  if (inSize < RC_INIT_SIZE)
+    return SZ_ERROR_INPUT_EOF;
+
+  LzmaDec_Construct(&p);
+  res = LzmaDec_AllocateProbs(&p, propData, propSize, alloc);
+  if (res != 0)
+    return res;
+  p.dic = dest;
+  p.dicBufSize = outSize;
+
+  LzmaDec_Init(&p);
+  
+  *srcLen = inSize;
+  res = LzmaDec_DecodeToDic(&p, outSize, src, srcLen, finishMode, status);
+
+  if (res == SZ_OK && *status == LZMA_STATUS_NEEDS_MORE_INPUT)
+    res = SZ_ERROR_INPUT_EOF;
+
+  (*destLen) = p.dicPos;
+  LzmaDec_FreeProbs(&p, alloc);
+  return res;
+}
+
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.h
new file mode 100644
index 0000000000..ad7d7057a4
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/LzmaDec.h
@@ -0,0 +1,223 @@
+/* LzmaDec.h -- LZMA Decoder
+2008-10-04 : Igor Pavlov : Public domain */
+
+#ifndef __LZMADEC_H
+#define __LZMADEC_H
+
+#include "Types.h"
+
+/* #define _LZMA_PROB32 */
+/* _LZMA_PROB32 can increase the speed on some CPUs,
+   but memory usage for CLzmaDec::probs will be doubled in that case */
+
+#ifdef _LZMA_PROB32
+#define CLzmaProb UInt32
+#else
+#define CLzmaProb UInt16
+#endif
+
+
+/* ---------- LZMA Properties ---------- */
+
+#define LZMA_PROPS_SIZE 5
+
+typedef struct _CLzmaProps
+{
+  unsigned lc, lp, pb;
+  UInt32 dicSize;
+} CLzmaProps;
+
+/* LzmaProps_Decode - decodes properties
+Returns:
+  SZ_OK
+  SZ_ERROR_UNSUPPORTED - Unsupported properties
+*/
+
+SRes LzmaProps_Decode(CLzmaProps *p, const Byte *data, unsigned size);
+
+
+/* ---------- LZMA Decoder state ---------- */
+
+/* LZMA_REQUIRED_INPUT_MAX = number of required input bytes for worst case.
+   Num bits = log2((2^11 / 31) ^ 22) + 26 < 134 + 26 = 160; */
+
+#define LZMA_REQUIRED_INPUT_MAX 20
+
+typedef struct
+{
+  CLzmaProps prop;
+  CLzmaProb *probs;
+  Byte *dic;
+  const Byte *buf;
+  UInt32 range, code;
+  SizeT dicPos;
+  SizeT dicBufSize;
+  UInt32 processedPos;
+  UInt32 checkDicSize;
+  unsigned state;
+  UInt32 reps[4];
+  unsigned remainLen;
+  int needFlush;
+  int needInitState;
+  UInt32 numProbs;
+  unsigned tempBufSize;
+  Byte tempBuf[LZMA_REQUIRED_INPUT_MAX];
+} CLzmaDec;
+
+#define LzmaDec_Construct(p) { (p)->dic = 0; (p)->probs = 0; }
+
+void LzmaDec_Init(CLzmaDec *p);
+
+/* There are two types of LZMA streams:
+     0) Stream with end mark. That end mark adds about 6 bytes to compressed size.
+     1) Stream without end mark. You must know exact uncompressed size to decompress such stream. */
+
+typedef enum
+{
+  LZMA_FINISH_ANY,   /* finish at any point */
+  LZMA_FINISH_END    /* block must be finished at the end */
+} ELzmaFinishMode;
+
+/* ELzmaFinishMode has meaning only if the decoding reaches output limit !!!
+
+   You must use LZMA_FINISH_END, when you know that current output buffer
+   covers last bytes of block. In other cases you must use LZMA_FINISH_ANY.
+
+   If LZMA decoder sees end marker before reaching output limit, it returns SZ_OK,
+   and output value of destLen will be less than output buffer size limit.
+   You can check status result also.
+
+   You can use multiple checks to test data integrity after full decompression:
+     1) Check Result and "status" variable.
+     2) Check that output(destLen) = uncompressedSize, if you know real uncompressedSize.
+     3) Check that output(srcLen) = compressedSize, if you know real compressedSize.
+        You must use correct finish mode in that case. */
+
+typedef enum
+{
+  LZMA_STATUS_NOT_SPECIFIED,               /* use main error code instead */
+  LZMA_STATUS_FINISHED_WITH_MARK,          /* stream was finished with end mark. */
+  LZMA_STATUS_NOT_FINISHED,                /* stream was not finished */
+  LZMA_STATUS_NEEDS_MORE_INPUT,            /* you must provide more input bytes */
+  LZMA_STATUS_MAYBE_FINISHED_WITHOUT_MARK  /* there is probability that stream was finished without end mark */
+} ELzmaStatus;
+
+/* ELzmaStatus is used only as output value for function call */
+
+
+/* ---------- Interfaces ---------- */
+
+/* There are 3 levels of interfaces:
+     1) Dictionary Interface
+     2) Buffer Interface
+     3) One Call Interface
+   You can select any of these interfaces, but don't mix functions from different
+   groups for same object. */
+
+
+/* There are two variants to allocate state for Dictionary Interface:
+     1) LzmaDec_Allocate / LzmaDec_Free
+     2) LzmaDec_AllocateProbs / LzmaDec_FreeProbs
+   You can use variant 2, if you set dictionary buffer manually.
+   For Buffer Interface you must always use variant 1.
+
+LzmaDec_Allocate* can return:
+  SZ_OK
+  SZ_ERROR_MEM         - Memory allocation error
+  SZ_ERROR_UNSUPPORTED - Unsupported properties
+*/
+   
+SRes LzmaDec_AllocateProbs(CLzmaDec *p, const Byte *props, unsigned propsSize, ISzAlloc *alloc);
+void LzmaDec_FreeProbs(CLzmaDec *p, ISzAlloc *alloc);
+
+SRes LzmaDec_Allocate(CLzmaDec *state, const Byte *prop, unsigned propsSize, ISzAlloc *alloc);
+void LzmaDec_Free(CLzmaDec *state, ISzAlloc *alloc);
+
+/* ---------- Dictionary Interface ---------- */
+
+/* You can use it, if you want to eliminate the overhead for data copying from
+   dictionary to some other external buffer.
+   You must work with CLzmaDec variables directly in this interface.
+
+   STEPS:
+     LzmaDec_Constr()
+     LzmaDec_Allocate()
+     for (each new stream)
+     {
+       LzmaDec_Init()
+       while (it needs more decompression)
+       {
+         LzmaDec_DecodeToDic()
+         use data from CLzmaDec::dic and update CLzmaDec::dicPos
+       }
+     }
+     LzmaDec_Free()
+*/
+
+/* LzmaDec_DecodeToDic
+   
+   The decoding to internal dictionary buffer (CLzmaDec::dic).
+   You must manually update CLzmaDec::dicPos, if it reaches CLzmaDec::dicBufSize !!!
+
+finishMode:
+  It has meaning only if the decoding reaches output limit (dicLimit).
+  LZMA_FINISH_ANY - Decode just dicLimit bytes.
+  LZMA_FINISH_END - Stream must be finished after dicLimit.
+
+Returns:
+  SZ_OK
+    status:
+      LZMA_STATUS_FINISHED_WITH_MARK
+      LZMA_STATUS_NOT_FINISHED
+      LZMA_STATUS_NEEDS_MORE_INPUT
+      LZMA_STATUS_MAYBE_FINISHED_WITHOUT_MARK
+  SZ_ERROR_DATA - Data error
+*/
+
+SRes LzmaDec_DecodeToDic(CLzmaDec *p, SizeT dicLimit,
+    const Byte *src, SizeT *srcLen, ELzmaFinishMode finishMode, ELzmaStatus *status);
+
+
+/* ---------- Buffer Interface ---------- */
+
+/* It's zlib-like interface.
+   See LzmaDec_DecodeToDic description for information about STEPS and return results,
+   but you must use LzmaDec_DecodeToBuf instead of LzmaDec_DecodeToDic and you don't need
+   to work with CLzmaDec variables manually.
+
+finishMode:
+  It has meaning only if the decoding reaches output limit (*destLen).
+  LZMA_FINISH_ANY - Decode just destLen bytes.
+  LZMA_FINISH_END - Stream must be finished after (*destLen).
+*/
+
+SRes LzmaDec_DecodeToBuf(CLzmaDec *p, Byte *dest, SizeT *destLen,
+    const Byte *src, SizeT *srcLen, ELzmaFinishMode finishMode, ELzmaStatus *status);
+
+
+/* ---------- One Call Interface ---------- */
+
+/* LzmaDecode
+
+finishMode:
+  It has meaning only if the decoding reaches output limit (*destLen).
+  LZMA_FINISH_ANY - Decode just destLen bytes.
+  LZMA_FINISH_END - Stream must be finished after (*destLen).
+
+Returns:
+  SZ_OK
+    status:
+      LZMA_STATUS_FINISHED_WITH_MARK
+      LZMA_STATUS_NOT_FINISHED
+      LZMA_STATUS_MAYBE_FINISHED_WITHOUT_MARK
+  SZ_ERROR_DATA - Data error
+  SZ_ERROR_MEM  - Memory allocation error
+  SZ_ERROR_UNSUPPORTED - Unsupported properties
+  SZ_ERROR_INPUT_EOF - It needs more bytes in input buffer (src).
+*/
+
+SRes LzmaDecode(Byte *dest, SizeT *destLen, const Byte *src, SizeT *srcLen,
+    const Byte *propData, unsigned propSize, ELzmaFinishMode finishMode,
+    ELzmaStatus *status, ISzAlloc *alloc);
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Types.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Types.h
new file mode 100644
index 0000000000..30b16e3bb0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/C/Types.h
@@ -0,0 +1,231 @@
+/** @file
+  Types.h
+
+  Based on LZMA SDK 4.65:
+    Types.h -- Basic types
+    2008-11-23 : Igor Pavlov : Public domain
+
+  Copyright (c) 2009, 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.
+
+**/
+
+#ifndef __7Z_TYPES_H
+#define __7Z_TYPES_H
+
+#ifdef EFIAPI
+
+#include "UefiLzma.h"
+
+#else
+
+#include <stddef.h>
+
+#ifdef _WIN32
+#include <windows.h>
+#endif
+
+#endif
+
+#define SZ_OK 0
+
+#define SZ_ERROR_DATA 1
+#define SZ_ERROR_MEM 2
+#define SZ_ERROR_CRC 3
+#define SZ_ERROR_UNSUPPORTED 4
+#define SZ_ERROR_PARAM 5
+#define SZ_ERROR_INPUT_EOF 6
+#define SZ_ERROR_OUTPUT_EOF 7
+#define SZ_ERROR_READ 8
+#define SZ_ERROR_WRITE 9
+#define SZ_ERROR_PROGRESS 10
+#define SZ_ERROR_FAIL 11
+#define SZ_ERROR_THREAD 12
+
+#define SZ_ERROR_ARCHIVE 16
+#define SZ_ERROR_NO_ARCHIVE 17
+
+typedef int SRes;
+
+#ifdef _WIN32
+typedef DWORD WRes;
+#else
+typedef int WRes;
+#endif
+
+#ifndef RINOK
+#define RINOK(x) { int __result__ = (x); if (__result__ != 0) return __result__; }
+#endif
+
+typedef unsigned char Byte;
+typedef short Int16;
+typedef unsigned short UInt16;
+
+#ifdef _LZMA_UINT32_IS_ULONG
+typedef long Int32;
+typedef unsigned long UInt32;
+#else
+typedef int Int32;
+typedef unsigned int UInt32;
+#endif
+
+#ifdef _SZ_NO_INT_64
+
+/* define _SZ_NO_INT_64, if your compiler doesn't support 64-bit integers.
+   NOTES: Some code will work incorrectly in that case! */
+
+typedef long Int64;
+typedef unsigned long UInt64;
+
+#else
+
+#if defined(_MSC_VER) || defined(__BORLANDC__)
+typedef __int64 Int64;
+typedef unsigned __int64 UInt64;
+#else
+typedef long long int Int64;
+typedef unsigned long long int UInt64;
+#endif
+
+#endif
+
+#ifdef _LZMA_NO_SYSTEM_SIZE_T
+typedef UInt32 SizeT;
+#else
+typedef size_t SizeT;
+#endif
+
+typedef int Bool;
+#define True 1
+#define False 0
+
+
+#ifdef _MSC_VER
+
+#if _MSC_VER >= 1300
+#define MY_NO_INLINE __declspec(noinline)
+#else
+#define MY_NO_INLINE
+#endif
+
+#define MY_CDECL __cdecl
+#define MY_STD_CALL __stdcall
+#define MY_FAST_CALL MY_NO_INLINE __fastcall
+
+#else
+
+#define MY_CDECL
+#define MY_STD_CALL
+#define MY_FAST_CALL
+
+#endif
+
+
+/* The following interfaces use first parameter as pointer to structure */
+
+typedef struct
+{
+  SRes (*Read)(void *p, void *buf, size_t *size);
+    /* if (input(*size) != 0 && output(*size) == 0) means end_of_stream.
+       (output(*size) < input(*size)) is allowed */
+} ISeqInStream;
+
+/* it can return SZ_ERROR_INPUT_EOF */
+SRes SeqInStream_Read(ISeqInStream *stream, void *buf, size_t size);
+SRes SeqInStream_Read2(ISeqInStream *stream, void *buf, size_t size, SRes errorType);
+SRes SeqInStream_ReadByte(ISeqInStream *stream, Byte *buf);
+
+typedef struct
+{
+  size_t (*Write)(void *p, const void *buf, size_t size);
+    /* Returns: result - the number of actually written bytes.
+       (result < size) means error */
+} ISeqOutStream;
+
+typedef enum
+{
+  SZ_SEEK_SET = 0,
+  SZ_SEEK_CUR = 1,
+  SZ_SEEK_END = 2
+} ESzSeek;
+
+typedef struct
+{
+  SRes (*Read)(void *p, void *buf, size_t *size);  /* same as ISeqInStream::Read */
+  SRes (*Seek)(void *p, Int64 *pos, ESzSeek origin);
+} ISeekInStream;
+
+typedef struct
+{
+  SRes (*Look)(void *p, void **buf, size_t *size);
+    /* if (input(*size) != 0 && output(*size) == 0) means end_of_stream.
+       (output(*size) > input(*size)) is not allowed
+       (output(*size) < input(*size)) is allowed */
+  SRes (*Skip)(void *p, size_t offset);
+    /* offset must be <= output(*size) of Look */
+
+  SRes (*Read)(void *p, void *buf, size_t *size);
+    /* reads directly (without buffer). It's same as ISeqInStream::Read */
+  SRes (*Seek)(void *p, Int64 *pos, ESzSeek origin);
+} ILookInStream;
+
+SRes LookInStream_LookRead(ILookInStream *stream, void *buf, size_t *size);
+SRes LookInStream_SeekTo(ILookInStream *stream, UInt64 offset);
+
+/* reads via ILookInStream::Read */
+SRes LookInStream_Read2(ILookInStream *stream, void *buf, size_t size, SRes errorType);
+SRes LookInStream_Read(ILookInStream *stream, void *buf, size_t size);
+
+#define LookToRead_BUF_SIZE (1 << 14)
+
+typedef struct
+{
+  ILookInStream s;
+  ISeekInStream *realStream;
+  size_t pos;
+  size_t size;
+  Byte buf[LookToRead_BUF_SIZE];
+} CLookToRead;
+
+void LookToRead_CreateVTable(CLookToRead *p, int lookahead);
+void LookToRead_Init(CLookToRead *p);
+
+typedef struct
+{
+  ISeqInStream s;
+  ILookInStream *realStream;
+} CSecToLook;
+
+void SecToLook_CreateVTable(CSecToLook *p);
+
+typedef struct
+{
+  ISeqInStream s;
+  ILookInStream *realStream;
+} CSecToRead;
+
+void SecToRead_CreateVTable(CSecToRead *p);
+
+typedef struct
+{
+  SRes (*Progress)(void *p, UInt64 inSize, UInt64 outSize);
+    /* Returns: result. (result != SZ_OK) means break.
+       Value (UInt64)(Int64)-1 for size means unknown value. */
+} ICompressProgress;
+
+typedef struct
+{
+  void *(*Alloc)(void *p, size_t size);
+  void (*Free)(void *p, void *address); /* address can be 0 */
+} ISzAlloc;
+
+#define IAlloc_Alloc(p, size) (p)->Alloc((p), size)
+#define IAlloc_Free(p, a) (p)->Free((p), a)
+
+#endif
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/history.txt b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/history.txt
new file mode 100644
index 0000000000..9bed5ebbef
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/history.txt
@@ -0,0 +1,236 @@
+HISTORY of the LZMA SDK
+-----------------------
+
+4.65           2009-02-03
+-------------------------
+- Some minor fixes
+
+
+4.63           2008-12-31
+-------------------------
+- Some minor fixes
+
+
+4.61 beta      2008-11-23
+-------------------------
+- The bug in ANSI-C LZMA Decoder was fixed:
+    If encoded stream was corrupted, decoder could access memory 
+    outside of allocated range.
+- Some changes in ANSI-C 7z Decoder interfaces.
+- LZMA SDK is placed in the public domain.
+
+
+4.60 beta      2008-08-19
+-------------------------
+- Some minor fixes.
+
+
+4.59 beta      2008-08-13
+-------------------------
+- The bug was fixed:
+    LZMA Encoder in fast compression mode could access memory outside of 
+    allocated range in some rare cases.
+
+
+4.58 beta      2008-05-05
+-------------------------
+- ANSI-C LZMA Decoder was rewritten for speed optimizations.
+- ANSI-C LZMA Encoder was included to LZMA SDK.
+- C++ LZMA code now is just wrapper over ANSI-C code.
+
+
+4.57           2007-12-12
+-------------------------
+- Speed optimizations in �++ LZMA Decoder. 
+- Small changes for more compatibility with some C/C++ compilers.
+
+
+4.49 beta      2007-07-05
+-------------------------
+- .7z ANSI-C Decoder:
+     - now it supports BCJ and BCJ2 filters
+     - now it supports files larger than 4 GB.
+     - now it supports "Last Write Time" field for files.
+- C++ code for .7z archives compressing/decompressing from 7-zip 
+  was included to LZMA SDK.
+  
+
+4.43           2006-06-04
+-------------------------
+- Small changes for more compatibility with some C/C++ compilers.
+  
+
+4.42           2006-05-15
+-------------------------
+- Small changes in .h files in ANSI-C version.
+  
+
+4.39 beta      2006-04-14
+-------------------------
+- The bug in versions 4.33b:4.38b was fixed:
+  C++ version of LZMA encoder could not correctly compress 
+  files larger than 2 GB with HC4 match finder (-mfhc4).
+  
+
+4.37 beta      2005-04-06
+-------------------------
+- Fixes in C++ code: code could no be compiled if _NO_EXCEPTIONS was defined. 
+
+
+4.35 beta      2005-03-02
+-------------------------
+- The bug was fixed in C++ version of LZMA Decoder:
+    If encoded stream was corrupted, decoder could access memory 
+    outside of allocated range.
+
+
+4.34 beta      2006-02-27
+-------------------------
+- Compressing speed and memory requirements for compressing were increased
+- LZMA now can use only these match finders: HC4, BT2, BT3, BT4
+
+
+4.32           2005-12-09
+-------------------------
+- Java version of LZMA SDK was included
+
+
+4.30           2005-11-20
+-------------------------
+- Compression ratio was improved in -a2 mode
+- Speed optimizations for compressing in -a2 mode
+- -fb switch now supports values up to 273
+- The bug in 7z_C (7zIn.c) was fixed:
+  It used Alloc/Free functions from different memory pools.
+  So if program used two memory pools, it worked incorrectly.
+- 7z_C: .7z format supporting was improved
+- LZMA# SDK (C#.NET version) was included
+
+
+4.27 (Updated) 2005-09-21
+-------------------------
+- Some GUIDs/interfaces in C++ were changed.
+ IStream.h:
+   ISequentialInStream::Read now works as old ReadPart
+   ISequentialOutStream::Write now works as old WritePart
+
+
+4.27           2005-08-07
+-------------------------
+- The bug in LzmaDecodeSize.c was fixed:
+   if _LZMA_IN_CB and _LZMA_OUT_READ were defined,
+   decompressing worked incorrectly.
+
+
+4.26           2005-08-05
+-------------------------
+- Fixes in 7z_C code and LzmaTest.c:
+  previous versions could work incorrectly,
+  if malloc(0) returns 0
+
+
+4.23           2005-06-29
+-------------------------
+- Small fixes in C++ code
+
+
+4.22           2005-06-10
+-------------------------
+- Small fixes
+
+
+4.21           2005-06-08
+-------------------------
+- Interfaces for ANSI-C LZMA Decoder (LzmaDecode.c) were changed
+- New additional version of ANSI-C LZMA Decoder with zlib-like interface:
+    - LzmaStateDecode.h
+    - LzmaStateDecode.c
+    - LzmaStateTest.c
+- ANSI-C LZMA Decoder now can decompress files larger than 4 GB
+
+
+4.17           2005-04-18
+-------------------------
+- New example for RAM->RAM compressing/decompressing: 
+  LZMA + BCJ (filter for x86 code):
+    - LzmaRam.h
+    - LzmaRam.cpp
+    - LzmaRamDecode.h
+    - LzmaRamDecode.c
+    - -f86 switch for lzma.exe
+
+
+4.16           2005-03-29
+-------------------------
+- The bug was fixed in LzmaDecode.c (ANSI-C LZMA Decoder): 
+   If _LZMA_OUT_READ was defined, and if encoded stream was corrupted,
+   decoder could access memory outside of allocated range.
+- Speed optimization of ANSI-C LZMA Decoder (now it's about 20% faster).
+  Old version of LZMA Decoder now is in file LzmaDecodeSize.c. 
+  LzmaDecodeSize.c can provide slightly smaller code than LzmaDecode.c
+- Small speed optimization in LZMA C++ code
+- filter for SPARC's code was added
+- Simplified version of .7z ANSI-C Decoder was included
+
+
+4.06           2004-09-05
+-------------------------
+- The bug in v4.05 was fixed:
+    LZMA-Encoder didn't release output stream in some cases.
+
+
+4.05           2004-08-25
+-------------------------
+- Source code of filters for x86, IA-64, ARM, ARM-Thumb 
+  and PowerPC code was included to SDK
+- Some internal minor changes
+
+
+4.04           2004-07-28
+-------------------------
+- More compatibility with some C++ compilers
+
+
+4.03           2004-06-18
+-------------------------
+- "Benchmark" command was added. It measures compressing 
+  and decompressing speed and shows rating values. 
+  Also it checks hardware errors.
+
+
+4.02           2004-06-10
+-------------------------
+- C++ LZMA Encoder/Decoder code now is more portable
+  and it can be compiled by GCC on Linux.
+
+
+4.01           2004-02-15
+-------------------------
+- Some detection of data corruption was enabled.
+    LzmaDecode.c / RangeDecoderReadByte
+    .....
+    {
+      rd->ExtraBytes = 1;
+      return 0xFF;
+    }
+
+
+4.00           2004-02-13
+-------------------------
+- Original version of LZMA SDK
+
+
+
+HISTORY of the LZMA
+-------------------
+  2001-2008:  Improvements to LZMA compressing/decompressing code, 
+              keeping compatibility with original LZMA format
+  1996-2001:  Development of LZMA compression format
+
+  Some milestones:
+
+  2001-08-30: LZMA compression was added to 7-Zip
+  1999-01-02: First version of 7-Zip was released
+  
+
+End of document
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/lzma.txt b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/lzma.txt
new file mode 100644
index 0000000000..d4f4af929a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/Sdk/lzma.txt
@@ -0,0 +1,594 @@
+LZMA SDK 4.65
+-------------
+
+LZMA SDK provides the documentation, samples, header files, libraries, 
+and tools you need to develop applications that use LZMA compression.
+
+LZMA is default and general compression method of 7z format
+in 7-Zip compression program (www.7-zip.org). LZMA provides high 
+compression ratio and very fast decompression.
+
+LZMA is an improved version of famous LZ77 compression algorithm. 
+It was improved in way of maximum increasing of compression ratio,
+keeping high decompression speed and low memory requirements for 
+decompressing.
+
+
+
+LICENSE
+-------
+
+LZMA SDK is written and placed in the public domain by Igor Pavlov.
+
+
+LZMA SDK Contents
+-----------------
+
+LZMA SDK includes:
+
+  - ANSI-C/C++/C#/Java source code for LZMA compressing and decompressing
+  - Compiled file->file LZMA compressing/decompressing program for Windows system
+
+
+UNIX/Linux version 
+------------------
+To compile C++ version of file->file LZMA encoding, go to directory
+C++/7zip/Compress/LZMA_Alone 
+and call make to recompile it:
+  make -f makefile.gcc clean all
+
+In some UNIX/Linux versions you must compile LZMA with static libraries.
+To compile with static libraries, you can use 
+LIB = -lm -static
+
+
+Files
+---------------------
+lzma.txt     - LZMA SDK description (this file)
+7zFormat.txt - 7z Format description
+7zC.txt      - 7z ANSI-C Decoder description
+methods.txt  - Compression method IDs for .7z
+lzma.exe     - Compiled file->file LZMA encoder/decoder for Windows
+history.txt  - history of the LZMA SDK
+
+
+Source code structure
+---------------------
+
+C/  - C files
+        7zCrc*.*   - CRC code
+        Alloc.*    - Memory allocation functions
+        Bra*.*     - Filters for x86, IA-64, ARM, ARM-Thumb, PowerPC and SPARC code
+        LzFind.*   - Match finder for LZ (LZMA) encoders 
+        LzFindMt.* - Match finder for LZ (LZMA) encoders for multithreading encoding
+        LzHash.h   - Additional file for LZ match finder
+        LzmaDec.*  - LZMA decoding
+        LzmaEnc.*  - LZMA encoding
+        LzmaLib.*  - LZMA Library for DLL calling
+        Types.h    - Basic types for another .c files
+	Threads.*  - The code for multithreading.
+
+    LzmaLib  - LZMA Library (.DLL for Windows)
+    
+    LzmaUtil - LZMA Utility (file->file LZMA encoder/decoder).
+
+    Archive - files related to archiving
+      7z     - 7z ANSI-C Decoder
+
+CPP/ -- CPP files
+
+  Common  - common files for C++ projects
+  Windows - common files for Windows related code
+
+  7zip    - files related to 7-Zip Project
+
+    Common   - common files for 7-Zip
+
+    Compress - files related to compression/decompression
+
+      Copy         - Copy coder
+      RangeCoder   - Range Coder (special code of compression/decompression)
+      LZMA         - LZMA compression/decompression on C++
+      LZMA_Alone   - file->file LZMA compression/decompression
+      Branch       - Filters for x86, IA-64, ARM, ARM-Thumb, PowerPC and SPARC code
+
+    Archive - files related to archiving
+
+      Common   - common files for archive handling
+      7z       - 7z C++ Encoder/Decoder
+
+    Bundles    - Modules that are bundles of other modules
+  
+      Alone7z           - 7zr.exe: Standalone version of 7z.exe that supports only 7z/LZMA/BCJ/BCJ2
+      Format7zR         - 7zr.dll: Reduced version of 7za.dll: extracting/compressing to 7z/LZMA/BCJ/BCJ2
+      Format7zExtractR  - 7zxr.dll: Reduced version of 7zxa.dll: extracting from 7z/LZMA/BCJ/BCJ2.
+
+    UI        - User Interface files
+         
+      Client7z - Test application for 7za.dll,  7zr.dll, 7zxr.dll
+      Common   - Common UI files
+      Console  - Code for console archiver
+
+
+
+CS/ - C# files
+  7zip
+    Common   - some common files for 7-Zip
+    Compress - files related to compression/decompression
+      LZ     - files related to LZ (Lempel-Ziv) compression algorithm
+      LZMA         - LZMA compression/decompression
+      LzmaAlone    - file->file LZMA compression/decompression
+      RangeCoder   - Range Coder (special code of compression/decompression)
+
+Java/  - Java files
+  SevenZip
+    Compression    - files related to compression/decompression
+      LZ           - files related to LZ (Lempel-Ziv) compression algorithm
+      LZMA         - LZMA compression/decompression
+      RangeCoder   - Range Coder (special code of compression/decompression)
+
+
+C/C++ source code of LZMA SDK is part of 7-Zip project.
+7-Zip source code can be downloaded from 7-Zip's SourceForge page:
+
+  http://sourceforge.net/projects/sevenzip/
+
+
+
+LZMA features
+-------------
+  - Variable dictionary size (up to 1 GB)
+  - Estimated compressing speed: about 2 MB/s on 2 GHz CPU
+  - Estimated decompressing speed: 
+      - 20-30 MB/s on 2 GHz Core 2 or AMD Athlon 64
+      - 1-2 MB/s on 200 MHz ARM, MIPS, PowerPC or other simple RISC
+  - Small memory requirements for decompressing (16 KB + DictionarySize)
+  - Small code size for decompressing: 5-8 KB
+
+LZMA decoder uses only integer operations and can be 
+implemented in any modern 32-bit CPU (or on 16-bit CPU with some conditions).
+
+Some critical operations that affect the speed of LZMA decompression:
+  1) 32*16 bit integer multiply
+  2) Misspredicted branches (penalty mostly depends from pipeline length)
+  3) 32-bit shift and arithmetic operations
+
+The speed of LZMA decompressing mostly depends from CPU speed.
+Memory speed has no big meaning. But if your CPU has small data cache, 
+overall weight of memory speed will slightly increase.
+
+
+How To Use
+----------
+
+Using LZMA encoder/decoder executable
+--------------------------------------
+
+Usage:  LZMA <e|d> inputFile outputFile [<switches>...]
+
+  e: encode file
+
+  d: decode file
+
+  b: Benchmark. There are two tests: compressing and decompressing 
+     with LZMA method. Benchmark shows rating in MIPS (million 
+     instructions per second). Rating value is calculated from 
+     measured speed and it is normalized with Intel's Core 2 results.
+     Also Benchmark checks possible hardware errors (RAM 
+     errors in most cases). Benchmark uses these settings:
+     (-a1, -d21, -fb32, -mfbt4). You can change only -d parameter. 
+     Also you can change the number of iterations. Example for 30 iterations:
+       LZMA b 30
+     Default number of iterations is 10.
+
+<Switches>
+  
+
+  -a{N}:  set compression mode 0 = fast, 1 = normal
+          default: 1 (normal)
+
+  d{N}:   Sets Dictionary size - [0, 30], default: 23 (8MB)
+          The maximum value for dictionary size is 1 GB = 2^30 bytes.
+          Dictionary size is calculated as DictionarySize = 2^N bytes. 
+          For decompressing file compressed by LZMA method with dictionary 
+          size D = 2^N you need about D bytes of memory (RAM).
+
+  -fb{N}: set number of fast bytes - [5, 273], default: 128
+          Usually big number gives a little bit better compression ratio 
+          and slower compression process.
+
+  -lc{N}: set number of literal context bits - [0, 8], default: 3
+          Sometimes lc=4 gives gain for big files.
+
+  -lp{N}: set number of literal pos bits - [0, 4], default: 0
+          lp switch is intended for periodical data when period is 
+          equal 2^N. For example, for 32-bit (4 bytes) 
+          periodical data you can use lp=2. Often it's better to set lc0, 
+          if you change lp switch.
+
+  -pb{N}: set number of pos bits - [0, 4], default: 2
+          pb switch is intended for periodical data 
+          when period is equal 2^N.
+
+  -mf{MF_ID}: set Match Finder. Default: bt4. 
+              Algorithms from hc* group doesn't provide good compression 
+              ratio, but they often works pretty fast in combination with 
+              fast mode (-a0).
+
+              Memory requirements depend from dictionary size 
+              (parameter "d" in table below). 
+
+               MF_ID     Memory                   Description
+
+                bt2    d *  9.5 + 4MB  Binary Tree with 2 bytes hashing.
+                bt3    d * 11.5 + 4MB  Binary Tree with 3 bytes hashing.
+                bt4    d * 11.5 + 4MB  Binary Tree with 4 bytes hashing.
+                hc4    d *  7.5 + 4MB  Hash Chain with 4 bytes hashing.
+
+  -eos:   write End Of Stream marker. By default LZMA doesn't write 
+          eos marker, since LZMA decoder knows uncompressed size 
+          stored in .lzma file header.
+
+  -si:    Read data from stdin (it will write End Of Stream marker).
+  -so:    Write data to stdout
+
+
+Examples:
+
+1) LZMA e file.bin file.lzma -d16 -lc0 
+
+compresses file.bin to file.lzma with 64 KB dictionary (2^16=64K)  
+and 0 literal context bits. -lc0 allows to reduce memory requirements 
+for decompression.
+
+
+2) LZMA e file.bin file.lzma -lc0 -lp2
+
+compresses file.bin to file.lzma with settings suitable 
+for 32-bit periodical data (for example, ARM or MIPS code).
+
+3) LZMA d file.lzma file.bin
+
+decompresses file.lzma to file.bin.
+
+
+Compression ratio hints
+-----------------------
+
+Recommendations
+---------------
+
+To increase the compression ratio for LZMA compressing it's desirable 
+to have aligned data (if it's possible) and also it's desirable to locate
+data in such order, where code is grouped in one place and data is 
+grouped in other place (it's better than such mixing: code, data, code,
+data, ...).
+
+
+Filters
+-------
+You can increase the compression ratio for some data types, using
+special filters before compressing. For example, it's possible to 
+increase the compression ratio on 5-10% for code for those CPU ISAs: 
+x86, IA-64, ARM, ARM-Thumb, PowerPC, SPARC.
+
+You can find C source code of such filters in C/Bra*.* files
+
+You can check the compression ratio gain of these filters with such 
+7-Zip commands (example for ARM code):
+No filter:
+  7z a a1.7z a.bin -m0=lzma
+
+With filter for little-endian ARM code:
+  7z a a2.7z a.bin -m0=arm -m1=lzma        
+
+It works in such manner:
+Compressing    = Filter_encoding + LZMA_encoding
+Decompressing  = LZMA_decoding + Filter_decoding
+
+Compressing and decompressing speed of such filters is very high,
+so it will not increase decompressing time too much.
+Moreover, it reduces decompression time for LZMA_decoding, 
+since compression ratio with filtering is higher.
+
+These filters convert CALL (calling procedure) instructions 
+from relative offsets to absolute addresses, so such data becomes more 
+compressible.
+
+For some ISAs (for example, for MIPS) it's impossible to get gain from such filter.
+
+
+LZMA compressed file format
+---------------------------
+Offset Size Description
+  0     1   Special LZMA properties (lc,lp, pb in encoded form)
+  1     4   Dictionary size (little endian)
+  5     8   Uncompressed size (little endian). -1 means unknown size
+ 13         Compressed data
+
+
+ANSI-C LZMA Decoder
+~~~~~~~~~~~~~~~~~~~
+
+Please note that interfaces for ANSI-C code were changed in LZMA SDK 4.58.
+If you want to use old interfaces you can download previous version of LZMA SDK
+from sourceforge.net site.
+
+To use ANSI-C LZMA Decoder you need the following files:
+1) LzmaDec.h + LzmaDec.c + Types.h
+LzmaUtil/LzmaUtil.c is example application that uses these files.
+
+
+Memory requirements for LZMA decoding
+-------------------------------------
+
+Stack usage of LZMA decoding function for local variables is not 
+larger than 200-400 bytes.
+
+LZMA Decoder uses dictionary buffer and internal state structure.
+Internal state structure consumes
+  state_size = (4 + (1.5 << (lc + lp))) KB
+by default (lc=3, lp=0), state_size = 16 KB.
+
+
+How To decompress data
+----------------------
+
+LZMA Decoder (ANSI-C version) now supports 2 interfaces:
+1) Single-call Decompressing
+2) Multi-call State Decompressing (zlib-like interface)
+
+You must use external allocator:
+Example:
+void *SzAlloc(void *p, size_t size) { p = p; return malloc(size); }
+void SzFree(void *p, void *address) { p = p; free(address); }
+ISzAlloc alloc = { SzAlloc, SzFree };
+
+You can use p = p; operator to disable compiler warnings.
+
+
+Single-call Decompressing
+-------------------------
+When to use: RAM->RAM decompressing
+Compile files: LzmaDec.h + LzmaDec.c + Types.h
+Compile defines: no defines
+Memory Requirements:
+  - Input buffer: compressed size
+  - Output buffer: uncompressed size
+  - LZMA Internal Structures: state_size (16 KB for default settings) 
+
+Interface:
+  int LzmaDecode(Byte *dest, SizeT *destLen, const Byte *src, SizeT *srcLen,
+      const Byte *propData, unsigned propSize, ELzmaFinishMode finishMode, 
+      ELzmaStatus *status, ISzAlloc *alloc);
+  In: 
+    dest     - output data
+    destLen  - output data size
+    src      - input data
+    srcLen   - input data size
+    propData - LZMA properties  (5 bytes)
+    propSize - size of propData buffer (5 bytes)
+    finishMode - It has meaning only if the decoding reaches output limit (*destLen).
+	 LZMA_FINISH_ANY - Decode just destLen bytes.
+	 LZMA_FINISH_END - Stream must be finished after (*destLen).
+                           You can use LZMA_FINISH_END, when you know that 
+                           current output buffer covers last bytes of stream. 
+    alloc    - Memory allocator.
+
+  Out: 
+    destLen  - processed output size 
+    srcLen   - processed input size 
+
+  Output:
+    SZ_OK
+      status:
+        LZMA_STATUS_FINISHED_WITH_MARK
+        LZMA_STATUS_NOT_FINISHED 
+        LZMA_STATUS_MAYBE_FINISHED_WITHOUT_MARK
+    SZ_ERROR_DATA - Data error
+    SZ_ERROR_MEM  - Memory allocation error
+    SZ_ERROR_UNSUPPORTED - Unsupported properties
+    SZ_ERROR_INPUT_EOF - It needs more bytes in input buffer (src).
+
+  If LZMA decoder sees end_marker before reaching output limit, it returns OK result,
+  and output value of destLen will be less than output buffer size limit.
+
+  You can use multiple checks to test data integrity after full decompression:
+    1) Check Result and "status" variable.
+    2) Check that output(destLen) = uncompressedSize, if you know real uncompressedSize.
+    3) Check that output(srcLen) = compressedSize, if you know real compressedSize. 
+       You must use correct finish mode in that case. */ 
+
+
+Multi-call State Decompressing (zlib-like interface)
+----------------------------------------------------
+
+When to use: file->file decompressing 
+Compile files: LzmaDec.h + LzmaDec.c + Types.h
+
+Memory Requirements:
+ - Buffer for input stream: any size (for example, 16 KB)
+ - Buffer for output stream: any size (for example, 16 KB)
+ - LZMA Internal Structures: state_size (16 KB for default settings) 
+ - LZMA dictionary (dictionary size is encoded in LZMA properties header)
+
+1) read LZMA properties (5 bytes) and uncompressed size (8 bytes, little-endian) to header:
+   unsigned char header[LZMA_PROPS_SIZE + 8];
+   ReadFile(inFile, header, sizeof(header)
+
+2) Allocate CLzmaDec structures (state + dictionary) using LZMA properties
+
+  CLzmaDec state;
+  LzmaDec_Constr(&state);
+  res = LzmaDec_Allocate(&state, header, LZMA_PROPS_SIZE, &g_Alloc);
+  if (res != SZ_OK)
+    return res;
+
+3) Init LzmaDec structure before any new LZMA stream. And call LzmaDec_DecodeToBuf in loop
+
+  LzmaDec_Init(&state);
+  for (;;)
+  {
+    ... 
+    int res = LzmaDec_DecodeToBuf(CLzmaDec *p, Byte *dest, SizeT *destLen, 
+    	const Byte *src, SizeT *srcLen, ELzmaFinishMode finishMode);
+    ...
+  }
+
+
+4) Free all allocated structures
+  LzmaDec_Free(&state, &g_Alloc);
+
+For full code example, look at C/LzmaUtil/LzmaUtil.c code.
+
+
+How To compress data
+--------------------
+
+Compile files: LzmaEnc.h + LzmaEnc.c + Types.h +
+LzFind.c + LzFind.h + LzFindMt.c + LzFindMt.h + LzHash.h
+
+Memory Requirements:
+  - (dictSize * 11.5 + 6 MB) + state_size
+
+Lzma Encoder can use two memory allocators:
+1) alloc - for small arrays.
+2) allocBig - for big arrays.
+
+For example, you can use Large RAM Pages (2 MB) in allocBig allocator for 
+better compression speed. Note that Windows has bad implementation for 
+Large RAM Pages. 
+It's OK to use same allocator for alloc and allocBig.
+
+
+Single-call Compression with callbacks
+--------------------------------------
+
+Check C/LzmaUtil/LzmaUtil.c as example, 
+
+When to use: file->file decompressing 
+
+1) you must implement callback structures for interfaces:
+ISeqInStream
+ISeqOutStream
+ICompressProgress
+ISzAlloc
+
+static void *SzAlloc(void *p, size_t size) { p = p; return MyAlloc(size); }
+static void SzFree(void *p, void *address) {  p = p; MyFree(address); }
+static ISzAlloc g_Alloc = { SzAlloc, SzFree };
+
+  CFileSeqInStream inStream;
+  CFileSeqOutStream outStream;
+
+  inStream.funcTable.Read = MyRead;
+  inStream.file = inFile;
+  outStream.funcTable.Write = MyWrite;
+  outStream.file = outFile;
+
+
+2) Create CLzmaEncHandle object;
+
+  CLzmaEncHandle enc;
+
+  enc = LzmaEnc_Create(&g_Alloc);
+  if (enc == 0)
+    return SZ_ERROR_MEM;
+
+
+3) initialize CLzmaEncProps properties;
+
+  LzmaEncProps_Init(&props);
+
+  Then you can change some properties in that structure.
+
+4) Send LZMA properties to LZMA Encoder
+
+  res = LzmaEnc_SetProps(enc, &props);
+
+5) Write encoded properties to header
+
+    Byte header[LZMA_PROPS_SIZE + 8];
+    size_t headerSize = LZMA_PROPS_SIZE;
+    UInt64 fileSize;
+    int i;
+
+    res = LzmaEnc_WriteProperties(enc, header, &headerSize);
+    fileSize = MyGetFileLength(inFile);
+    for (i = 0; i < 8; i++)
+      header[headerSize++] = (Byte)(fileSize >> (8 * i));
+    MyWriteFileAndCheck(outFile, header, headerSize)
+
+6) Call encoding function:
+      res = LzmaEnc_Encode(enc, &outStream.funcTable, &inStream.funcTable, 
+        NULL, &g_Alloc, &g_Alloc);
+
+7) Destroy LZMA Encoder Object
+  LzmaEnc_Destroy(enc, &g_Alloc, &g_Alloc);
+
+
+If callback function return some error code, LzmaEnc_Encode also returns that code.
+
+
+Single-call RAM->RAM Compression
+--------------------------------
+
+Single-call RAM->RAM Compression is similar to Compression with callbacks,
+but you provide pointers to buffers instead of pointers to stream callbacks:
+
+HRes LzmaEncode(Byte *dest, SizeT *destLen, const Byte *src, SizeT srcLen,
+    CLzmaEncProps *props, Byte *propsEncoded, SizeT *propsSize, int writeEndMark, 
+    ICompressProgress *progress, ISzAlloc *alloc, ISzAlloc *allocBig);
+
+Return code:
+  SZ_OK               - OK
+  SZ_ERROR_MEM        - Memory allocation error 
+  SZ_ERROR_PARAM      - Incorrect paramater
+  SZ_ERROR_OUTPUT_EOF - output buffer overflow
+  SZ_ERROR_THREAD     - errors in multithreading functions (only for Mt version)
+
+
+
+LZMA Defines
+------------
+
+_LZMA_SIZE_OPT - Enable some optimizations in LZMA Decoder to get smaller executable code.
+
+_LZMA_PROB32   - It can increase the speed on some 32-bit CPUs, but memory usage for 
+                 some structures will be doubled in that case.
+
+_LZMA_UINT32_IS_ULONG  - Define it if int is 16-bit on your compiler and long is 32-bit.
+
+_LZMA_NO_SYSTEM_SIZE_T  - Define it if you don't want to use size_t type.
+
+
+C++ LZMA Encoder/Decoder 
+~~~~~~~~~~~~~~~~~~~~~~~~
+C++ LZMA code use COM-like interfaces. So if you want to use it, 
+you can study basics of COM/OLE.
+C++ LZMA code is just wrapper over ANSI-C code.
+
+
+C++ Notes
+~~~~~~~~~~~~~~~~~~~~~~~~
+If you use some C++ code folders in 7-Zip (for example, C++ code for .7z handling),
+you must check that you correctly work with "new" operator.
+7-Zip can be compiled with MSVC 6.0 that doesn't throw "exception" from "new" operator.
+So 7-Zip uses "CPP\Common\NewHandler.cpp" that redefines "new" operator:
+operator new(size_t size)
+{
+  void *p = ::malloc(size);
+  if (p == 0)
+    throw CNewException();
+  return p;
+}
+If you use MSCV that throws exception for "new" operator, you can compile without 
+"NewHandler.cpp". So standard exception will be used. Actually some code of 
+7-Zip catches any exception in internal code and converts it to HRESULT code.
+So you don't need to catch CNewException, if you call COM interfaces of 7-Zip.
+
+---
+
+http://www.7-zip.org
+http://www.7-zip.org/sdk.html
+http://www.7-zip.org/support.html
diff --git a/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/UefiLzma.h b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/UefiLzma.h
new file mode 100644
index 0000000000..863b3ef66b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/LzmaCustomDecompressLib/UefiLzma.h
@@ -0,0 +1,47 @@
+/** @file
+  LZMA UEFI header file
+
+  Allows LZMA code to build under UEFI (edk2) build environment
+
+  Copyright (c) 2009, 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.
+
+**/
+
+#ifndef __UEFILZMA_H__
+#define __UEFILZMA_H__
+
+#include <Uefi.h>
+#include <Library/BaseMemoryLib.h>
+
+#ifdef _WIN32
+#undef _WIN32
+#endif
+
+#ifndef _SIZE_T_DEFINED
+#if !defined(_WIN64) || defined(__GNUC__)
+typedef unsigned int size_t;
+#endif
+#endif
+
+#ifdef _WIN64
+#undef _WIN64
+#endif
+
+#ifndef _PTRDIFF_T_DEFINED
+typedef int ptrdiff_t;
+#endif
+
+#define memcpy CopyMem
+#define memmove CopyMem
+
+#define _LZMA_SIZE_OPT
+
+#endif // __UEFILZMA_H__
+
diff --git a/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.c b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.c
new file mode 100644
index 0000000000..0df144fe79
--- /dev/null
+++ b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.c
@@ -0,0 +1,62 @@
+/** @file
+  Null instance of OEM Hook Status Code Library with empty functions.
+
+  Copyright (c) 2006 - 2009, 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.
+
+**/
+
+
+/**
+  Initialize OEM status code device .
+
+  @retval  EFI_SUCCESS   Always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+OemHookStatusCodeInitialize (
+  VOID
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Report status code to OEM device.
+ 
+  @param  CodeType      Indicates the type of status code being reported.
+  @param  Value         Describes the current status of a hardware or software entity.  
+                        This included information about the class and subclass that is used to classify the entity 
+                        as well as an operation.  For progress codes, the operation is the current activity. 
+                        For error codes, it is the exception.  For debug codes, it is not defined at this time. 
+  @param  Instance      The enumeration of a hardware or software entity within the system.  
+                        A system may contain multiple entities that match a class/subclass pairing. 
+                        The instance differentiates between them.  An instance of 0 indicates that instance information is unavailable, 
+                        not meaningful, or not relevant.  Valid instance numbers start with 1.
+  @param  CallerId      This optional parameter may be used to identify the caller. 
+                        This parameter allows the status code driver to apply different rules to different callers. 
+  @param  Data          This optional parameter may be used to pass additional data
+ 
+  @retval EFI_SUCCESS   Always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+OemHookStatusCodeReport (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId, OPTIONAL
+  IN EFI_STATUS_CODE_DATA     *Data      OPTIONAL
+  )
+{
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.inf b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.inf
new file mode 100644
index 0000000000..32ed1a41bf
--- /dev/null
+++ b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.inf
@@ -0,0 +1,35 @@
+## @file
+#  Null instance of OEM Hook Status Code Library with empty functions.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = OemHookStatusCodeLibNull
+  MODULE_UNI_FILE                = OemHookStatusCodeLibNull.uni
+  FILE_GUID                      = 54D2878F-25CD-4a2b-8420-EBD18E609C76
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = OemHookStatusCodeLib 
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  OemHookStatusCodeLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.uni b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.uni
new file mode 100644
index 0000000000..3e8cdd8d7f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.uni
diff --git a/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.c b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.c
new file mode 100644
index 0000000000..761e45b0de
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.c
@@ -0,0 +1,115 @@
+/** @file
+  Null instance of PCI Host Bridge Library with empty functions.
+
+  Copyright (c) 2016, 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 <PiDxe.h>
+#include <Library/PciHostBridgeLib.h>
+#include <Library/DebugLib.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+CHAR16 *mPciHostBridgeLibAcpiAddressSpaceTypeStr[] = {
+  L"Mem", L"I/O", L"Bus"
+};
+
+/**
+  Return all the root bridge instances in an array.
+
+  @param Count  Return the count of root bridge instances.
+
+  @return All the root bridge instances in an array.
+          The array should be passed into PciHostBridgeFreeRootBridges()
+          when it's not used.
+**/
+PCI_ROOT_BRIDGE *
+EFIAPI
+PciHostBridgeGetRootBridges (
+  UINTN *Count
+  )
+{
+  *Count = 0;
+  return NULL;
+}
+
+/**
+  Free the root bridge instances array returned from PciHostBridgeGetRootBridges().
+
+  @param Bridges The root bridge instances array.
+  @param Count   The count of the array.
+**/
+VOID
+EFIAPI
+PciHostBridgeFreeRootBridges (
+  PCI_ROOT_BRIDGE *Bridges,
+  UINTN           Count
+  )
+{
+  return;
+}
+
+/**
+  Inform the platform that the resource conflict happens.
+
+  @param HostBridgeHandle Handle of the Host Bridge.
+  @param Configuration    Pointer to PCI I/O and PCI memory resource
+                          descriptors. The Configuration contains the resources
+                          for all the root bridges. The resource for each root
+                          bridge is terminated with END descriptor and an
+                          additional END is appended indicating the end of the
+                          entire resources. The resource descriptor field
+                          values follow the description in
+                          EFI_PCI_HOST_BRIDGE_RESOURCE_ALLOCATION_PROTOCOL
+                          .SubmitResources().
+**/
+VOID
+EFIAPI
+PciHostBridgeResourceConflict (
+  EFI_HANDLE                        HostBridgeHandle,
+  VOID                              *Configuration
+  )
+{
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *Descriptor;
+  UINTN                             RootBridgeIndex;
+  DEBUG ((EFI_D_ERROR, "PciHostBridge: Resource conflict happens!\n"));
+
+  RootBridgeIndex = 0;
+  Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *) Configuration;
+  while (Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR) {
+    DEBUG ((EFI_D_ERROR, "RootBridge[%d]:\n", RootBridgeIndex++));
+    for (; Descriptor->Desc == ACPI_ADDRESS_SPACE_DESCRIPTOR; Descriptor++) {
+      ASSERT (Descriptor->ResType <
+              (sizeof (mPciHostBridgeLibAcpiAddressSpaceTypeStr) /
+               sizeof (mPciHostBridgeLibAcpiAddressSpaceTypeStr[0])
+               )
+              );
+      DEBUG ((EFI_D_ERROR, " %s: Length/Alignment = 0x%lx / 0x%lx\n",
+              mPciHostBridgeLibAcpiAddressSpaceTypeStr[Descriptor->ResType],
+              Descriptor->AddrLen, Descriptor->AddrRangeMax
+              ));
+      if (Descriptor->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM) {
+        DEBUG ((EFI_D_ERROR, "     Granularity/SpecificFlag = %ld / %02x%s\n",
+                Descriptor->AddrSpaceGranularity, Descriptor->SpecificFlag,
+                ((Descriptor->SpecificFlag &
+                  EFI_ACPI_MEMORY_RESOURCE_SPECIFIC_FLAG_CACHEABLE_PREFETCHABLE
+                  ) != 0) ? L" (Prefetchable)" : L""
+                ));
+      }
+    }
+    //
+    // Skip the END descriptor for root bridge
+    //
+    ASSERT (Descriptor->Desc == ACPI_END_TAG_DESCRIPTOR);
+    Descriptor = (EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR *)(
+                   (EFI_ACPI_END_TAG_DESCRIPTOR *)Descriptor + 1
+                   );
+  }
+}
diff --git a/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.inf b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.inf
new file mode 100644
index 0000000000..8df14924ad
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.inf
@@ -0,0 +1,38 @@
+## @file
+#  Null instance of PCI Host Bridge Library with empty functions.
+#
+#  Copyright (c) 2016, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PciHostBridgeLibNull
+  MODULE_UNI_FILE                = PciHostBridgeLibNull.uni
+  FILE_GUID                      = A19A6C36-7053-4E2C-8BD0-E8286230E473
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PciHostBridgeLib
+
+#
+# The following information is for reference only and not required by the build
+# tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PciHostBridgeLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.uni b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.uni
new file mode 100644
index 0000000000..7b0fdee324
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.uni
@@ -0,0 +1,20 @@
+// /** @file
+// Null instance of PCI Host Bridge Library with empty functions.
+//
+// Null instance of PCI Host Bridge Library with empty functions.
+//
+// Copyright (c) 2016, 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.
+//
+// **/
+
+
+#string STR_MODULE_ABSTRACT             #language en-US "Null instance of PCI Host Bridge Library with empty functions."
+
+#string STR_MODULE_DESCRIPTION          #language en-US "Null instance of PCI Host Bridge Library with empty functions."
diff --git a/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.c b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.c
new file mode 100644
index 0000000000..6e3dcdd690
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.c
@@ -0,0 +1,312 @@
+/** @file
+
+  This library registers CRC32 guided section handler 
+  to parse CRC32 encapsulation section and extract raw data.
+
+Copyright (c) 2007 - 2014, 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 <PiPei.h>
+#include <Guid/Crc32GuidedSectionExtraction.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+
+///
+/// CRC32 Guided Section header
+///
+typedef struct {
+  EFI_GUID_DEFINED_SECTION  GuidedSectionHeader; ///< EFI guided section header
+  UINT32                    CRC32Checksum;       ///< 32bit CRC check sum
+} CRC32_SECTION_HEADER;
+
+typedef struct {
+  EFI_GUID_DEFINED_SECTION2 GuidedSectionHeader; ///< EFI guided section header
+  UINT32                    CRC32Checksum;       ///< 32bit CRC check sum
+} CRC32_SECTION2_HEADER;
+
+/**
+  This internal function reverses bits for 32bit data.
+
+  @param  Value                 The data to be reversed.
+
+  @return                       Data reversed.
+
+**/
+UINT32
+PeiCrc32GuidedSectionExtractLibReverseBits (
+  UINT32  Value
+  )
+{
+  UINTN   Index;
+  UINT32  NewValue;
+
+  NewValue = 0;
+  for (Index = 0; Index < 32; Index++) {
+    if ((Value & (1 << Index)) != 0) {
+      NewValue = NewValue | (1 << (31 - Index));
+    }
+  }
+
+  return NewValue;
+}
+
+/**
+  Calculate CRC32 for target data.
+
+  @param  Data                  The target data.
+  @param  DataSize              The target data size.
+  @param  CrcOut                The CRC32 for target data.
+
+  @retval EFI_SUCCESS           The CRC32 for target data is calculated successfully.
+  @retval EFI_INVALID_PARAMETER Some parameter is not valid, so the CRC32 is not
+                                calculated.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiCrc32GuidedSectionExtractLibCalculateCrc32 (
+  IN  VOID    *Data,
+  IN  UINTN   DataSize,
+  OUT UINT32  *CrcOut
+  )
+{
+  UINT32  CrcTable[256];
+  UINTN   TableEntry;
+  UINTN   Index;
+  UINT32  Value;
+  UINT32  Crc;
+  UINT8   *Ptr;
+
+  if (Data == NULL || DataSize == 0 || CrcOut == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Initialize CRC32 table.
+  //
+  for (TableEntry = 0; TableEntry < 256; TableEntry++) {
+    Value = PeiCrc32GuidedSectionExtractLibReverseBits ((UINT32) TableEntry);
+    for (Index = 0; Index < 8; Index++) {
+      if ((Value & 0x80000000) != 0) {
+        Value = (Value << 1) ^ 0x04c11db7;
+      } else {
+        Value = Value << 1;
+      }
+    }
+    CrcTable[TableEntry] = PeiCrc32GuidedSectionExtractLibReverseBits (Value);
+  }
+
+  //
+  // Compute CRC
+  //
+  Crc = 0xffffffff;
+  for (Index = 0, Ptr = Data; Index < DataSize; Index++, Ptr++) {
+    Crc = (Crc >> 8) ^ CrcTable[(UINT8) Crc ^ *Ptr];
+  }
+
+  *CrcOut = Crc ^ 0xffffffff;
+  return EFI_SUCCESS;
+}
+
+/**
+
+  GetInfo gets raw data size and attribute of the input guided section.
+  It first checks whether the input guid section is supported. 
+  If not, EFI_INVALID_PARAMETER will return.
+
+  @param InputSection       Buffer containing the input GUIDed section to be processed.
+  @param OutputBufferSize   The size of OutputBuffer.
+  @param ScratchBufferSize  The size of ScratchBuffer.
+  @param SectionAttribute   The attribute of the input guided section.
+
+  @retval EFI_SUCCESS            The size of destination buffer, the size of scratch buffer and 
+                                 the attribute of the input section are successull retrieved.
+  @retval EFI_INVALID_PARAMETER  The GUID in InputSection does not match this instance guid.
+
+**/
+EFI_STATUS
+EFIAPI
+Crc32GuidedSectionGetInfo (
+  IN  CONST VOID  *InputSection,
+  OUT UINT32      *OutputBufferSize,
+  OUT UINT32      *ScratchBufferSize,
+  OUT UINT16      *SectionAttribute
+  )
+{
+  if (IS_SECTION2 (InputSection)) {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // Retrieve the size and attribute of the input section data.
+    //
+    *SectionAttribute  = ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes;
+    *ScratchBufferSize = 0;
+    *OutputBufferSize  = SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+  } else {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // Retrieve the size and attribute of the input section data.
+    //
+    *SectionAttribute  = ((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes;
+    *ScratchBufferSize = 0;
+    *OutputBufferSize  = SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Extraction handler tries to extract raw data from the input guided section.
+  It also does authentication check for 32bit CRC value in the input guided section.
+  It first checks whether the input guid section is supported. 
+  If not, EFI_INVALID_PARAMETER will return.
+
+  @param InputSection    Buffer containing the input GUIDed section to be processed.
+  @param OutputBuffer    Buffer to contain the output raw data allocated by the caller.
+  @param ScratchBuffer   A pointer to a caller-allocated buffer for function internal use.
+  @param AuthenticationStatus A pointer to a caller-allocated UINT32 that indicates the
+                              authentication status of the output buffer.
+
+  @retval EFI_SUCCESS            Section Data and Auth Status is extracted successfully.
+  @retval EFI_INVALID_PARAMETER  The GUID in InputSection does not match this instance guid.
+
+**/
+EFI_STATUS
+EFIAPI
+Crc32GuidedSectionHandler (
+  IN CONST  VOID    *InputSection,
+  OUT       VOID    **OutputBuffer,
+  IN        VOID    *ScratchBuffer,        OPTIONAL
+  OUT       UINT32  *AuthenticationStatus
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      SectionCrc32Checksum;
+  UINT32      Crc32Checksum;
+  UINT32      OutputBufferSize;
+
+  if (IS_SECTION2 (InputSection)) {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION2 *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  
+    //
+    // Get section Crc32 checksum.
+    //
+    SectionCrc32Checksum = ((CRC32_SECTION2_HEADER *) InputSection)->CRC32Checksum;
+    *OutputBuffer      = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+    OutputBufferSize   = SECTION2_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION2 *) InputSection)->DataOffset;
+
+    //
+    // Implicitly CRC32 GUIDed section should have STATUS_VALID bit set
+    //
+    ASSERT (((EFI_GUID_DEFINED_SECTION2 *) InputSection)->Attributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID);
+    *AuthenticationStatus = EFI_AUTH_STATUS_IMAGE_SIGNED;
+  } else {
+    //
+    // Check whether the input guid section is recognized.
+    //
+    if (!CompareGuid (
+        &gEfiCrc32GuidedSectionExtractionGuid,
+        &(((EFI_GUID_DEFINED_SECTION *) InputSection)->SectionDefinitionGuid))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  
+    //
+    // Get section Crc32 checksum.
+    //
+    SectionCrc32Checksum = ((CRC32_SECTION_HEADER *) InputSection)->CRC32Checksum;
+    *OutputBuffer      = (UINT8 *) InputSection + ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+    OutputBufferSize   = SECTION_SIZE (InputSection) - ((EFI_GUID_DEFINED_SECTION *) InputSection)->DataOffset;
+
+    //
+    // Implicitly CRC32 GUIDed section should have STATUS_VALID bit set
+    //
+    ASSERT (((EFI_GUID_DEFINED_SECTION *) InputSection)->Attributes & EFI_GUIDED_SECTION_AUTH_STATUS_VALID);
+    *AuthenticationStatus = EFI_AUTH_STATUS_IMAGE_SIGNED;
+  }
+
+  //
+  // Init Checksum value to Zero.
+  //
+  Crc32Checksum = 0;
+
+  //
+  // Calculate CRC32 Checksum of Image
+  //
+  Status = PeiCrc32GuidedSectionExtractLibCalculateCrc32 (*OutputBuffer, OutputBufferSize, &Crc32Checksum);
+  if (Status == EFI_SUCCESS) {
+    if (Crc32Checksum != SectionCrc32Checksum) {
+      //
+      // If Crc32 checksum is not matched, AUTH tested failed bit is set.
+      //
+      *AuthenticationStatus |= EFI_AUTH_STATUS_TEST_FAILED;
+    }
+  } else {
+    //
+    // If Crc32 checksum is not calculated, AUTH not tested bit is set.
+    //
+    *AuthenticationStatus |= EFI_AUTH_STATUS_NOT_TESTED;
+  }
+
+  //
+  // Temp solution until PeiCore checks AUTH Status.
+  //
+  if ((*AuthenticationStatus & (EFI_AUTH_STATUS_TEST_FAILED | EFI_AUTH_STATUS_NOT_TESTED)) != 0) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register the handler to extract CRC32 guided section.
+
+  @param  FileHandle   The handle of FFS header the loaded driver.
+  @param  PeiServices  The pointer to the PEI services.
+
+  @retval  EFI_SUCCESS           Register successfully.
+  @retval  EFI_OUT_OF_RESOURCES  Not enough memory to register this handler.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiCrc32GuidedSectionExtractLibConstructor (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  return ExtractGuidedSectionRegisterHandlers (
+          &gEfiCrc32GuidedSectionExtractionGuid,
+          Crc32GuidedSectionGetInfo,
+          Crc32GuidedSectionHandler
+          );
+}
diff --git a/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.inf b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.inf
new file mode 100644
index 0000000000..7134810040
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.inf
@@ -0,0 +1,48 @@
+## @file
+#  Pei Crc32 Guided Section Extract library.
+#
+#  This library doesn't produce any library class. The constructor function uses 
+#  ExtractGuidedSectionLib service to register CRC32 guided section handler
+#  that parses CRC32 encapsulation section and extracts raw data.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiCrc32GuidedSectionExtractLib
+  FILE_GUID                      = 1EBE57F5-BE42-45f0-A1F9-FA3DC633910B
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|PEI_CORE PEIM
+  CONSTRUCTOR                    = PeiCrc32GuidedSectionExtractLibConstructor
+  MODULE_UNI_FILE                = PeiCrc32GuidedSectionExtractLib.uni
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  PeiCrc32GuidedSectionExtractLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  ExtractGuidedSectionLib
+  DebugLib
+  BaseMemoryLib
+
+[Guids]
+  gEfiCrc32GuidedSectionExtractionGuid             ## PRODUCES ## UNDEFINED
diff --git a/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.uni b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.uni
new file mode 100644
index 0000000000..408aadb76d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.uni
diff --git a/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.c b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.c
new file mode 100644
index 0000000000..4bcf0030a0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.c
@@ -0,0 +1,78 @@
+/** @file
+  NULL Library class that reads Debug Mask variable and if it exists makes a
+  HOB that contains the debug mask.
+
+  Copyright (c) 2011, Apple, Inc. 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 <PiPei.h>
+
+#include <Library/HobLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PeiServicesLib.h>
+
+#include <Ppi/ReadOnlyVariable2.h>
+#include <Guid/DebugMask.h>
+
+
+/**
+  The constructor reads variable and sets HOB
+  
+  @param  FileHandle   The handle of FFS header the loaded driver.
+  @param  PeiServices  The pointer to the PEI services.
+
+  @retval EFI_SUCCESS  The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiDebugPrintHobLibConstructor (
+  IN EFI_PEI_FILE_HANDLE        FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI *Variable;
+  UINTN                           Size;
+  UINT64                          GlobalErrorLevel;
+  UINT32                          HobErrorLevel;
+  
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiReadOnlyVariable2PpiGuid,
+             0,
+             NULL,
+             (VOID **)&Variable
+             );
+  if (!EFI_ERROR (Status)) {
+    Size = sizeof (GlobalErrorLevel);
+    Status = Variable->GetVariable ( 
+                         Variable, 
+                         DEBUG_MASK_VARIABLE_NAME,
+                         &gEfiGenericVariableGuid,
+                         NULL,
+                         &Size,
+                         &GlobalErrorLevel
+                         );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Build the GUID'ed HOB for DXE
+      //
+      HobErrorLevel = (UINT32)GlobalErrorLevel;
+      BuildGuidDataHob (
+        &gEfiGenericVariableGuid,
+        &HobErrorLevel,
+        sizeof (HobErrorLevel)
+        );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.inf b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.inf
new file mode 100644
index 0000000000..0380dee01f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.inf
@@ -0,0 +1,50 @@
+## @file
+#  NULL Library class that reads Debug Mask variable and if it exists makes a
+#  HOB that contains the debug mask.
+#
+#  Copyright (c) 2011, Apple, Inc. All rights reserved.<BR>
+#  Copyright (c) 2012 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiDebugPrintHobLib
+  MODULE_UNI_FILE                = PeiDebugPrintHobLib.uni
+  FILE_GUID                      = EB0BDD73-DABB-E74B-BF51-62DC1DA521E1
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|PEIM
+  CONSTRUCTOR                    = PeiDebugPrintHobLibConstructor
+
+
+[Sources]
+  PeiDebugPrintHobLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeiServicesLib
+  DebugLib
+
+[Ppis]
+  gEfiPeiReadOnlyVariable2PpiGuid   ## CONSUMES
+
+[Guids]
+  ## SOMETIMES_CONSUMES   ## Variable:L"EFIDebug"
+  ## SOMETIMES_PRODUCES   ## HOB
+  gEfiGenericVariableGuid
+
+[Depex]
+  gEfiPeiReadOnlyVariable2PpiGuid
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.uni b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.uni
new file mode 100644
index 0000000000..686c606c79
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.uni
diff --git a/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/DebugLib.c b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/DebugLib.c
new file mode 100644
index 0000000000..f1d98277a3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/DebugLib.c
@@ -0,0 +1,461 @@
+/** @file
+  Debug Library based on report status code library.
+
+  Note that if the debug message length is larger than the maximum allowable
+  record length, then the debug message will be ignored directly.
+
+  Copyright (c) 2006 - 2015, 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 <PiPei.h>
+
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DebugPrintErrorLevelLib.h>
+
+/**
+  Prints a debug message to the debug output device if the specified error level is enabled.
+
+  If any bit in ErrorLevel is also set in DebugPrintErrorLevelLib function 
+  GetDebugPrintErrorLevel (), then print the message specified by Format and the 
+  associated variable argument list to the debug output device.
+
+  If Format is NULL, then ASSERT().
+
+  If the length of the message string specificed by Format is larger than the maximum allowable
+  record length, then directly return and not print it.
+
+  @param  ErrorLevel  The error level of the debug message.
+  @param  Format      Format string for the debug message to print.
+  @param  ...         Variable argument list whose contents are accessed 
+                      based on the format string specified by Format.
+
+**/
+VOID
+EFIAPI
+DebugPrint (
+  IN  UINTN        ErrorLevel,
+  IN  CONST CHAR8  *Format,
+  ...
+  )
+{
+  UINT64          Buffer[(EFI_STATUS_CODE_DATA_MAX_SIZE / sizeof (UINT64)) + 1];
+  EFI_DEBUG_INFO  *DebugInfo;
+  UINTN           TotalSize;
+  VA_LIST         VaListMarker;
+  BASE_LIST       BaseListMarker;
+  CHAR8           *FormatString;
+  BOOLEAN         Long;
+
+  //
+  // If Format is NULL, then ASSERT().
+  //
+  ASSERT (Format != NULL);
+
+  //
+  // Check driver Debug Level value and global debug level
+  //
+  if ((ErrorLevel & GetDebugPrintErrorLevel ()) == 0) {
+    return;
+  }
+
+  //
+  // Compute the total size of the record.
+  // Note that the passing-in format string and variable parameters will be constructed to 
+  // the following layout:
+  //
+  //         Buffer->|------------------------|
+  //                 |         Padding        | 4 bytes
+  //      DebugInfo->|------------------------|
+  //                 |      EFI_DEBUG_INFO    | sizeof(EFI_DEBUG_INFO)
+  // BaseListMarker->|------------------------|
+  //                 |           ...          |
+  //                 |   variable arguments   | 12 * sizeof (UINT64)
+  //                 |           ...          |
+  //                 |------------------------|
+  //                 |       Format String    |
+  //                 |------------------------|<- (UINT8 *)Buffer + sizeof(Buffer)
+  //
+  TotalSize = 4 + sizeof (EFI_DEBUG_INFO) + 12 * sizeof (UINT64) + AsciiStrSize (Format);
+
+  //
+  // If the TotalSize is larger than the maximum record size, then return
+  //
+  if (TotalSize > sizeof (Buffer)) {
+    return;
+  }
+
+  //
+  // Fill in EFI_DEBUG_INFO
+  //
+  // Here we skip the first 4 bytes of Buffer, because we must ensure BaseListMarker is
+  // 64-bit aligned, otherwise retrieving 64-bit parameter from BaseListMarker will cause
+  // exception on IPF. Buffer starts at 64-bit aligned address, so skipping 4 types (sizeof(EFI_DEBUG_INFO))
+  // just makes address of BaseListMarker, which follows DebugInfo, 64-bit aligned.
+  //
+  DebugInfo             = (EFI_DEBUG_INFO *)(Buffer) + 1;
+  DebugInfo->ErrorLevel = (UINT32)ErrorLevel;
+  BaseListMarker        = (BASE_LIST)(DebugInfo + 1);
+  FormatString          = (CHAR8 *)((UINT64 *)(DebugInfo + 1) + 12);
+
+  //
+  // Copy the Format string into the record
+  //
+  AsciiStrCpyS (FormatString, sizeof(Buffer) - (4 + sizeof(EFI_DEBUG_INFO) + 12 * sizeof(UINT64)), Format);
+
+  //
+  // The first 12 * sizeof (UINT64) bytes following EFI_DEBUG_INFO are for variable arguments
+  // of format in DEBUG string, which is followed by the DEBUG format string.
+  // Here we will process the variable arguments and pack them in this area.
+  //
+  VA_START (VaListMarker, Format);
+  for (; *Format != '\0'; Format++) {
+    //
+    // Only format with prefix % is processed.
+    //
+    if (*Format != '%') {
+      continue;
+    }
+    Long = FALSE;
+    //
+    // Parse Flags and Width
+    //
+    for (Format++; TRUE; Format++) {
+      if (*Format == '.' || *Format == '-' || *Format == '+' || *Format == ' ') {
+        //
+        // These characters in format field are omitted.
+        //
+        continue;
+      }
+      if (*Format >= '0' && *Format <= '9') {
+        //
+        // These characters in format field are omitted.
+        //
+        continue;
+      }
+      if (*Format == 'L' || *Format == 'l') {
+        //
+        // 'L" or "l" in format field means the number being printed is a UINT64
+        //
+        Long = TRUE;
+        continue;
+      }
+      if (*Format == '*') {
+        //
+        // '*' in format field means the precision of the field is specified by
+        // a UINTN argument in the argument list.
+        //
+        BASE_ARG (BaseListMarker, UINTN) = VA_ARG (VaListMarker, UINTN);
+        continue;
+      }
+      if (*Format == '\0') {
+        //
+        // Make no output if Format string terminates unexpectedly when
+        // looking up for flag, width, precision and type. 
+        //
+        Format--;
+      }
+      //
+      // When valid argument type detected or format string terminates unexpectedly,
+      // the inner loop is done.
+      //
+      break;
+    }
+    
+    //
+    // Pack variable arguments into the storage area following EFI_DEBUG_INFO.
+    //
+    if ((*Format == 'p') && (sizeof (VOID *) > 4)) {
+      Long = TRUE;
+    }
+    if (*Format == 'p' || *Format == 'X' || *Format == 'x' || *Format == 'd' || *Format == 'u') {
+      if (Long) {
+        BASE_ARG (BaseListMarker, INT64) = VA_ARG (VaListMarker, INT64);
+      } else {
+        BASE_ARG (BaseListMarker, int) = VA_ARG (VaListMarker, int);
+      }
+    } else if (*Format == 's' || *Format == 'S' || *Format == 'a' || *Format == 'g' || *Format == 't') {
+      BASE_ARG (BaseListMarker, VOID *) = VA_ARG (VaListMarker, VOID *);
+    } else if (*Format == 'c') {
+      BASE_ARG (BaseListMarker, UINTN) = VA_ARG (VaListMarker, UINTN);
+    } else if (*Format == 'r') {
+      BASE_ARG (BaseListMarker, RETURN_STATUS) = VA_ARG (VaListMarker, RETURN_STATUS);
+    }
+
+    //
+    // If the converted BASE_LIST is larger than the 12 * sizeof (UINT64) allocated bytes, then ASSERT()
+    // This indicates that the DEBUG() macro is passing in more argument than can be handled by 
+    // the EFI_DEBUG_INFO record
+    //
+    ASSERT ((CHAR8 *)BaseListMarker <= FormatString);
+
+    //
+    // If the converted BASE_LIST is larger than the 12 * sizeof (UINT64) allocated bytes, then return
+    //
+    if ((CHAR8 *)BaseListMarker > FormatString) {
+      VA_END (VaListMarker);
+      return;
+    }
+  }
+  VA_END (VaListMarker);
+
+  //
+  // Send the DebugInfo record
+  //
+  REPORT_STATUS_CODE_EX (
+    EFI_DEBUG_CODE,
+    (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_DC_UNSPECIFIED),
+    0,
+    NULL,
+    &gEfiStatusCodeDataTypeDebugGuid,
+    DebugInfo,
+    TotalSize
+    );
+}
+
+/**
+  Prints an assert message containing a filename, line number, and description.  
+  This may be followed by a breakpoint or a dead loop.
+
+  Print a message of the form "ASSERT <FileName>(<LineNumber>): <Description>\n"
+  to the debug output device.  If DEBUG_PROPERTY_ASSERT_BREAKPOINT_ENABLED bit of 
+  PcdDebugProperyMask is set then CpuBreakpoint() is called. Otherwise, if 
+  DEBUG_PROPERTY_ASSERT_DEADLOOP_ENABLED bit of PcdDebugProperyMask is set then 
+  CpuDeadLoop() is called.  If neither of these bits are set, then this function 
+  returns immediately after the message is printed to the debug output device.
+  DebugAssert() must actively prevent recursion.  If DebugAssert() is called while
+  processing another DebugAssert(), then DebugAssert() must return immediately.
+
+  If FileName is NULL, then a <FileName> string of "(NULL) Filename" is printed.
+  If Description is NULL, then a <Description> string of "(NULL) Description" is printed.
+
+  @param  FileName     Pointer to the name of the source file that generated the assert condition.
+  @param  LineNumber   The line number in the source file that generated the assert condition
+  @param  Description  Pointer to the description of the assert condition.
+
+**/
+VOID
+EFIAPI
+DebugAssert (
+  IN CONST CHAR8  *FileName,
+  IN UINTN        LineNumber,
+  IN CONST CHAR8  *Description
+  )
+{
+  UINT64                 Buffer[EFI_STATUS_CODE_DATA_MAX_SIZE / sizeof(UINT64)];
+  EFI_DEBUG_ASSERT_DATA  *AssertData;
+  UINTN                  HeaderSize;
+  UINTN                  TotalSize;
+  CHAR8                  *Temp;
+  UINTN                  FileNameSize;
+  UINTN                  DescriptionSize;
+
+  //
+  // Get string size
+  //
+  HeaderSize       = sizeof (EFI_DEBUG_ASSERT_DATA);
+  FileNameSize     = AsciiStrSize (FileName);
+  DescriptionSize  = AsciiStrSize (Description);
+
+  //
+  // Make sure it will all fit in the passed in buffer.
+  //
+  if (HeaderSize + FileNameSize + DescriptionSize > sizeof (Buffer)) {
+    //
+    // FileName + Description is too long to be filled into buffer. 
+    //
+    if (HeaderSize + FileNameSize < sizeof (Buffer)) {
+      //
+      // Description has enough buffer to be truncated. 
+      //
+      DescriptionSize = sizeof (Buffer) - HeaderSize - FileNameSize;
+    } else {
+      //
+      // FileName is too long to be filled into buffer.
+      // FileName will be truncated. Reserved one byte for Description NULL terminator.
+      //
+      DescriptionSize = 1;
+      FileNameSize    = sizeof (Buffer) - HeaderSize - DescriptionSize;
+    }
+  }
+ 
+  //
+  // Fill in EFI_DEBUG_ASSERT_DATA
+  //
+  AssertData = (EFI_DEBUG_ASSERT_DATA *)Buffer;
+  AssertData->LineNumber = (UINT32)LineNumber;
+  TotalSize  = sizeof (EFI_DEBUG_ASSERT_DATA);
+
+  //
+  // Copy Ascii FileName including NULL terminator.
+  //
+  Temp = CopyMem (AssertData + 1, FileName, FileNameSize);
+  Temp[FileNameSize - 1] = 0;
+  TotalSize += FileNameSize;
+
+  //
+  // Copy Ascii Description include NULL terminator.
+  //
+  Temp = CopyMem (Temp + FileNameSize, Description, DescriptionSize);
+  Temp[DescriptionSize - 1] = 0;
+  TotalSize += DescriptionSize;
+
+  REPORT_STATUS_CODE_EX (
+    (EFI_ERROR_CODE | EFI_ERROR_UNRECOVERED),
+    (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_EC_ILLEGAL_SOFTWARE_STATE),
+    0,
+    NULL,
+    NULL,
+    AssertData,
+    TotalSize
+    );
+
+  //
+  // Generate a Breakpoint, DeadLoop, or NOP based on PCD settings
+  //
+  if ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_ASSERT_BREAKPOINT_ENABLED) != 0) {
+    CpuBreakpoint ();
+  } else if ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_ASSERT_DEADLOOP_ENABLED) != 0) {
+    CpuDeadLoop ();
+  }
+}
+
+
+/**
+  Fills a target buffer with PcdDebugClearMemoryValue, and returns the target buffer.
+
+  This function fills Length bytes of Buffer with the value specified by 
+  PcdDebugClearMemoryValue, and returns Buffer.
+
+  If Buffer is NULL, then ASSERT().
+  If Length is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param   Buffer  Pointer to the target buffer to be filled with PcdDebugClearMemoryValue.
+  @param   Length  Number of bytes in Buffer to fill with zeros PcdDebugClearMemoryValue. 
+
+  @return  Buffer  Pointer to the target buffer filled with PcdDebugClearMemoryValue.
+
+**/
+VOID *
+EFIAPI
+DebugClearMemory (
+  OUT VOID  *Buffer,
+  IN UINTN  Length
+  )
+{
+  ASSERT (Buffer != NULL);
+
+  return SetMem (Buffer, Length, PcdGet8 (PcdDebugClearMemoryValue));
+}
+
+
+/**
+  Returns TRUE if ASSERT() macros are enabled.
+
+  This function returns TRUE if the DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED bit of 
+  PcdDebugProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE    The DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED bit of PcdDebugProperyMask is set.
+  @retval  FALSE   The DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED bit of PcdDebugProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+DebugAssertEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED) != 0);
+}
+
+
+/**  
+  Returns TRUE if DEBUG() macros are enabled.
+
+  This function returns TRUE if the DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of 
+  PcdDebugProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE    The DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of PcdDebugProperyMask is set.
+  @retval  FALSE   The DEBUG_PROPERTY_DEBUG_PRINT_ENABLED bit of PcdDebugProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+DebugPrintEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_DEBUG_PRINT_ENABLED) != 0);
+}
+
+
+/**  
+  Returns TRUE if DEBUG_CODE() macros are enabled.
+
+  This function returns TRUE if the DEBUG_PROPERTY_DEBUG_CODE_ENABLED bit of 
+  PcdDebugProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE    The DEBUG_PROPERTY_DEBUG_CODE_ENABLED bit of PcdDebugProperyMask is set.
+  @retval  FALSE   The DEBUG_PROPERTY_DEBUG_CODE_ENABLED bit of PcdDebugProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+DebugCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_DEBUG_CODE_ENABLED) != 0);
+}
+
+
+/**  
+  Returns TRUE if DEBUG_CLEAR_MEMORY() macro is enabled.
+
+  This function returns TRUE if the DEBUG_PROPERTY_CLEAR_MEMORY_ENABLED bit of 
+  PcdDebugProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE    The DEBUG_PROPERTY_CLEAR_MEMORY_ENABLED bit of PcdDebugProperyMask is set.
+  @retval  FALSE   The DEBUG_PROPERTY_CLEAR_MEMORY_ENABLED bit of PcdDebugProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+DebugClearMemoryEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdDebugPropertyMask) & DEBUG_PROPERTY_CLEAR_MEMORY_ENABLED) != 0);
+}
+
+/**
+  Returns TRUE if any one of the bit is set both in ErrorLevel and PcdFixedDebugPrintErrorLevel.
+
+  This function compares the bit mask of ErrorLevel and PcdFixedDebugPrintErrorLevel.
+
+  @retval  TRUE    Current ErrorLevel is supported.
+  @retval  FALSE   Current ErrorLevel is not supported.
+
+**/
+BOOLEAN
+EFIAPI
+DebugPrintLevelEnabled (
+  IN  CONST UINTN        ErrorLevel
+  )
+{
+  return (BOOLEAN) ((ErrorLevel & PcdGet32(PcdFixedDebugPrintErrorLevel)) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.inf b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.inf
new file mode 100644
index 0000000000..55446672d7
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.inf
@@ -0,0 +1,54 @@
+## @file
+#  Debug Library based on report status code library
+#
+#  Debug Library for PEIMs and DXE drivers that sends debug messages to ReportStatusCode
+#  Copyright (c) 2006 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiDxeDebugLibReportStatusCode
+  MODULE_UNI_FILE                = PeiDxeDebugLibReportStatusCode.uni
+  FILE_GUID                      = bda39d3a-451b-4350-8266-81ab10fa0523
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = DebugLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER SMM_CORE PEIM SEC PEI_CORE UEFI_APPLICATION UEFI_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DebugLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  
+[LibraryClasses]
+  PcdLib
+  ReportStatusCodeLib
+  BaseMemoryLib
+  BaseLib
+  DebugPrintErrorLevelLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdDebugClearMemoryValue  ## SOMETIMES_CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdDebugPropertyMask      ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdFixedDebugPrintErrorLevel ## CONSUMES
+
+[Guids]
+  gEfiStatusCodeDataTypeDebugGuid    ## SOMETIMES_CONSUMES ## GUID
+
diff --git a/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.uni b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.uni
new file mode 100644
index 0000000000..fe2abee431
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.uni
diff --git a/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.c b/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.c
new file mode 100644
index 0000000000..cb6a007360
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.c
@@ -0,0 +1,81 @@
+/** @file
+  Implementation of Ipmi Library in PEI Phase for SMS.
+
+  Copyright (c) 2014 - 2015, 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 <PiPei.h>
+#include <Ppi/IpmiPpi.h>
+#include <Library/IpmiLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/DebugLib.h>
+
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+EFI_STATUS
+EFIAPI
+IpmiSubmitCommand (
+  IN     UINT8     NetFunction,
+  IN     UINT8     Command,
+  IN     UINT8     *RequestData,
+  IN     UINT32    RequestDataSize,
+     OUT UINT8     *ResponseData,
+  IN OUT UINT32    *ResponseDataSize
+  )
+{
+  EFI_STATUS   Status;
+  PEI_IPMI_PPI *IpmiPpi;
+
+  Status = PeiServicesLocatePpi(
+            &gPeiIpmiPpiGuid,
+            0,
+            NULL,
+            (VOID **) &IpmiPpi
+            );
+  if (EFI_ERROR (Status)) {
+    //
+    // Ipmi Ppi is not installed. So, IPMI device is not present.
+    //
+    DEBUG ((EFI_D_ERROR, "IpmiSubmitCommand in Pei Phase under SMS Status - %r\n", Status));
+    return EFI_NOT_FOUND;
+  }
+
+  Status = IpmiPpi->IpmiSubmitCommand (
+                            IpmiPpi,
+                            NetFunction,
+                            Command,
+                            RequestData,
+                            RequestDataSize,
+                            ResponseData,
+                            ResponseDataSize
+                            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.inf b/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.inf
new file mode 100644
index 0000000000..e2e2646f91
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.inf
@@ -0,0 +1,41 @@
+## @file
+#  Instance of IPMI Library in PEI phase for SMS.
+#
+#  Copyright (c) 2014 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiIpmiLibIpmiPpi
+  FILE_GUID                      = 43679142-87C4-44AD-AF02-B47F782D6CF3
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = IpmiLib|PEIM PEI_CORE
+
+#
+#  VALID_ARCHITECTURES          = IA32 X64
+#
+
+[Sources]
+  PeiIpmiLibIpmiPpi.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DebugLib
+  BaseMemoryLib
+  PeiServicesLib
+
+[Ppis]
+  gPeiIpmiPpiGuid      ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.c b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.c
new file mode 100644
index 0000000000..0b5a717caa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.c
@@ -0,0 +1,494 @@
+/** @file
+  Performance library instance used in PEI phase.
+
+  This file implements all APIs in Performance Library class in MdePkg. It creates
+  performance logging GUIDed HOB on the first performance logging and then logs the
+  performance data to the GUIDed HOB. Due to the limitation of temporary RAM, the maximum
+  number of performance logging entry is specified by PcdMaxPeiPerformanceLogEntries.  
+
+Copyright (c) 2006 - 2015, 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 <PiPei.h>
+
+#include <Guid/Performance.h>
+
+#include <Library/PerformanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/BaseLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+
+
+/**
+  Gets the GUID HOB for PEI performance.
+
+  This internal function searches for the GUID HOB for PEI performance.
+  If that GUID HOB is not found, it will build a new one.
+  It outputs the data area of that GUID HOB to record performance log.
+
+  @param    PeiPerformanceLog           Pointer to Pointer to PEI performance log header.
+  @param    PeiPerformanceIdArray       Pointer to Pointer to PEI performance identifier array.
+
+**/
+VOID
+InternalGetPerformanceHobLog (
+  OUT PEI_PERFORMANCE_LOG_HEADER    **PeiPerformanceLog,
+  OUT UINT32                        **PeiPerformanceIdArray
+  )
+{
+  EFI_HOB_GUID_TYPE           *GuidHob;
+  UINTN                       PeiPerformanceSize;
+
+  ASSERT (PeiPerformanceLog != NULL);
+  ASSERT (PeiPerformanceIdArray != NULL);
+
+  GuidHob = GetFirstGuidHob (&gPerformanceProtocolGuid);
+
+  if (GuidHob != NULL) {
+    //
+    // PEI Performance HOB was found, then return the existing one.
+    //
+    *PeiPerformanceLog = GET_GUID_HOB_DATA (GuidHob);
+
+    GuidHob = GetFirstGuidHob (&gPerformanceExProtocolGuid);
+    ASSERT (GuidHob != NULL);
+    *PeiPerformanceIdArray = GET_GUID_HOB_DATA (GuidHob);
+  } else {
+    //
+    // PEI Performance HOB was not found, then build one.
+    //
+    PeiPerformanceSize     = sizeof (PEI_PERFORMANCE_LOG_HEADER) +
+                             sizeof (PEI_PERFORMANCE_LOG_ENTRY) * PcdGet8 (PcdMaxPeiPerformanceLogEntries);
+    *PeiPerformanceLog     = BuildGuidHob (&gPerformanceProtocolGuid, PeiPerformanceSize);
+    *PeiPerformanceLog     = ZeroMem (*PeiPerformanceLog, PeiPerformanceSize);
+
+    PeiPerformanceSize     = sizeof (UINT32) * PcdGet8 (PcdMaxPeiPerformanceLogEntries);
+    *PeiPerformanceIdArray = BuildGuidHob (&gPerformanceExProtocolGuid, PeiPerformanceSize);
+    *PeiPerformanceIdArray = ZeroMem (*PeiPerformanceIdArray, PeiPerformanceSize);
+  }
+}
+
+/**
+  Searches in the log array with keyword Handle, Token, Module and Identifier.
+
+  This internal function searches for the log entry in the log array.
+  If there is an entry that exactly matches the given keywords
+  and its end time stamp is zero, then the index of that log entry is returned;
+  otherwise, the the number of log entries in the array is returned.
+
+  @param  PeiPerformanceLog       Pointer to the data structure containing PEI 
+                                  performance data.
+  @param  PeiPerformanceIdArray   Pointer to PEI performance identifier array.
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  Identifier              32-bit identifier.
+
+  @retval The index of log entry in the array.
+
+**/
+UINT32
+InternalSearchForLogEntry (
+  IN PEI_PERFORMANCE_LOG_HEADER *PeiPerformanceLog,
+  IN UINT32                     *PeiPerformanceIdArray,
+  IN CONST VOID                 *Handle,  OPTIONAL
+  IN CONST CHAR8                *Token,   OPTIONAL
+  IN CONST CHAR8                *Module,   OPTIONAL
+  IN UINT32                     Identifier
+  )
+{
+  UINT32                    Index;
+  UINT32                    Index2;
+  UINT32                    NumberOfEntries;
+  PEI_PERFORMANCE_LOG_ENTRY *LogEntryArray;
+
+
+  if (Token == NULL) {
+    Token = "";
+  }
+  if (Module == NULL) {
+    Module = "";
+  }
+  NumberOfEntries = PeiPerformanceLog->NumberOfEntries;
+  LogEntryArray   = (PEI_PERFORMANCE_LOG_ENTRY *) (PeiPerformanceLog + 1);
+
+  Index2 = 0;
+
+  for (Index = 0; Index < NumberOfEntries; Index++) {
+    Index2 = NumberOfEntries - 1 - Index;
+    if (LogEntryArray[Index2].EndTimeStamp == 0 &&
+        (LogEntryArray[Index2].Handle == (EFI_PHYSICAL_ADDRESS) (UINTN) Handle) &&
+        AsciiStrnCmp (LogEntryArray[Index2].Token, Token, PEI_PERFORMANCE_STRING_LENGTH) == 0 &&
+        AsciiStrnCmp (LogEntryArray[Index2].Module, Module, PEI_PERFORMANCE_STRING_LENGTH) == 0 &&
+        (PeiPerformanceIdArray[Index2] == Identifier)) {
+      Index = Index2;
+      break;
+    }
+  }
+  return Index;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, Module and Identifier.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  PEI_PERFORMANCE_LOG_HEADER  *PeiPerformanceLog;
+  UINT32                      *PeiPerformanceIdArray;
+  PEI_PERFORMANCE_LOG_ENTRY   *LogEntryArray;
+  UINT32                      Index;
+
+  InternalGetPerformanceHobLog (&PeiPerformanceLog, &PeiPerformanceIdArray);
+
+  if (PeiPerformanceLog->NumberOfEntries >= PcdGet8 (PcdMaxPeiPerformanceLogEntries)) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  Index                       = PeiPerformanceLog->NumberOfEntries++;
+  LogEntryArray               = (PEI_PERFORMANCE_LOG_ENTRY *) (PeiPerformanceLog + 1);
+  LogEntryArray[Index].Handle = (EFI_PHYSICAL_ADDRESS) (UINTN) Handle;
+
+  if (Token != NULL) {
+    AsciiStrnCpyS (LogEntryArray[Index].Token, PEI_PERFORMANCE_STRING_SIZE, Token, PEI_PERFORMANCE_STRING_LENGTH);
+  }
+  if (Module != NULL) {
+    AsciiStrnCpyS (LogEntryArray[Index].Module, PEI_PERFORMANCE_STRING_SIZE, Module, PEI_PERFORMANCE_STRING_LENGTH);
+  }
+
+  LogEntryArray[Index].EndTimeStamp = 0;
+  PeiPerformanceIdArray[Index] = Identifier;
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+  LogEntryArray[Index].StartTimeStamp = TimeStamp;
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, Module and Identifier.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  PEI_PERFORMANCE_LOG_HEADER  *PeiPerformanceLog;
+  UINT32                      *PeiPerformanceIdArray;
+  PEI_PERFORMANCE_LOG_ENTRY   *LogEntryArray;
+  UINT32                      Index;
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+
+  InternalGetPerformanceHobLog (&PeiPerformanceLog, &PeiPerformanceIdArray);
+  Index             = InternalSearchForLogEntry (PeiPerformanceLog, PeiPerformanceIdArray, Handle, Token, Module, Identifier);
+  if (Index >= PeiPerformanceLog->NumberOfEntries) {
+    return RETURN_NOT_FOUND;
+  }
+  LogEntryArray     = (PEI_PERFORMANCE_LOG_ENTRY *) (PeiPerformanceLog + 1);
+  LogEntryArray[Index].EndTimeStamp = TimeStamp;
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance of entry entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  PEI_PERFORMANCE_LOG_HEADER  *PeiPerformanceLog;
+  UINT32                      *PeiPerformanceIdArray;
+  PEI_PERFORMANCE_LOG_ENTRY   *CurrentLogEntry;
+  PEI_PERFORMANCE_LOG_ENTRY   *LogEntryArray;
+  UINTN                       NumberOfEntries;
+
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  InternalGetPerformanceHobLog (&PeiPerformanceLog, &PeiPerformanceIdArray);
+
+  NumberOfEntries   = (UINTN) (PeiPerformanceLog->NumberOfEntries);
+  LogEntryArray     = (PEI_PERFORMANCE_LOG_ENTRY *) (PeiPerformanceLog + 1);
+  //
+  // Make sure that LogEntryKey is a valid log entry key.
+  //
+  ASSERT (LogEntryKey <= NumberOfEntries);
+
+  if (LogEntryKey == NumberOfEntries) {
+    return 0;
+  }
+
+  CurrentLogEntry = &(LogEntryArray[LogEntryKey]);
+
+  *Handle         = (VOID *) (UINTN) (CurrentLogEntry->Handle);
+  *Token          = CurrentLogEntry->Token;
+  *Module         = CurrentLogEntry->Module;
+  *StartTimeStamp = CurrentLogEntry->StartTimeStamp;
+  *EndTimeStamp   = CurrentLogEntry->EndTimeStamp;
+  *Identifier     = PeiPerformanceIdArray[LogEntryKey++];
+
+  return LogEntryKey;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, and Module.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, and Module.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance of entry entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.inf b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.inf
new file mode 100644
index 0000000000..7a5d2407ac
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.inf
@@ -0,0 +1,62 @@
+## @file 
+#  Performance library instance used in PEI phase.
+#  
+#  This library provides the performance measurement interfaces in PEI phase, it creates
+#  and consumes GUIDed HOB for performance logging. The GUIDed HOB is passed to DXE phase
+#  so that it can be taken over by DxeCorePerformanceLib.  
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiPerformanceLib
+  MODULE_UNI_FILE                = PeiPerformanceLib.uni
+  FILE_GUID                      = F72DE735-B24F-4ef6-897F-70A85D01A047
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PerformanceLib|PEIM PEI_CORE SEC
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  PeiPerformanceLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseMemoryLib
+  PcdLib
+  TimerLib
+  BaseLib
+  HobLib
+  DebugLib
+
+
+[Guids]
+  ## PRODUCES ## HOB
+  ## CONSUMES ## HOB
+  gPerformanceProtocolGuid
+  ## PRODUCES ## HOB
+  ## CONSUMES ## HOB
+  gPerformanceExProtocolGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPerformanceLogEntries ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.uni b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.uni
new file mode 100644
index 0000000000..3be291a988
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.c b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.c
new file mode 100644
index 0000000000..d77c6e544b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.c
@@ -0,0 +1,34 @@
+/** @file
+  Null Recovery Library instance does nothing and returns unsupported status.
+
+  This library instance is no longer used and module using this library
+  class should update to directly locate EFI_PEI_RECOVERY_MODULE_PPI defined
+  in PI 1.2 specification. 
+
+Copyright (c) 2006 - 2008, 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 <PiPei.h>
+#include <Library/RecoveryLib.h>
+
+/**
+  Calling this function causes the system do recovery boot path.
+  
+  @retval EFI_UNSUPPORTED       Recovery is not supported.
+**/
+EFI_STATUS
+EFIAPI
+PeiRecoverFirmware (
+  VOID
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.inf b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.inf
new file mode 100644
index 0000000000..6c565acc40
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.inf
@@ -0,0 +1,39 @@
+## @file 
+#  Null Recovery library instance for PEIM module
+#  This library instance is no longer used and module using this library
+#  class should update to directly locate EFI_PEI_RECOVERY_MODULE_PPI defined
+#  in PI 1.2 specification. 
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiRecoveryLibNull
+  MODULE_UNI_FILE                = PeiRecoveryLibNull.uni
+  FILE_GUID                      = 41789FB9-02AC-4484-BD40-A3147D7EDA25
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = RecoveryLib|PEIM
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  PeiRecoveryLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.uni b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.uni
new file mode 100644
index 0000000000..6c7a2cdb7a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.uni
diff --git a/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.inf b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.inf
new file mode 100644
index 0000000000..f66ce80cd5
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.inf
@@ -0,0 +1,59 @@
+## @file
+#  Instance of Report Status Code Library for PEI Phase.
+#
+#  Instance of Report Status Code Library for PEI Phase. It first tries to report status
+#  code via PEI Status Code Service. If the service is not available, it then tries calling
+#  OEM Hook Status Code Library.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiReportStatusCodeLib
+  MODULE_UNI_FILE                = PeiReportStatusCodeLib.uni
+  FILE_GUID                      = 8c690838-7a22-45c4-aa58-a33e3e515cd4
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ReportStatusCodeLib|SEC PEIM PEI_CORE
+  
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  ReportStatusCodeLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  PeiServicesTablePointerLib
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  OemHookStatusCodeLib
+
+
+[Guids]
+  gEfiStatusCodeSpecificDataGuid                ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiStatusCodeDataTypeDebugGuid               ## SOMETIMES_CONSUMES   ## UNDEFINED
+
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdReportStatusCodePropertyMask  ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.uni b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.uni
new file mode 100644
index 0000000000..53c74d809f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.uni
diff --git a/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/ReportStatusCodeLib.c b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/ReportStatusCodeLib.c
new file mode 100644
index 0000000000..d41d4e981d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiReportStatusCodeLib/ReportStatusCodeLib.c
@@ -0,0 +1,560 @@
+/** @file
+  Instance of Report Status Code Library for PEI Phase.
+
+  Copyright (c) 2006 - 2010, 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 <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/OemHookStatusCodeLib.h>
+#include <Library/PcdLib.h>
+
+//
+// Define the maximum extended data size that is supported in the PEI phase
+//
+#define MAX_EXTENDED_DATA_SIZE  0x200
+
+/**
+  Internal worker function that reports a status code through the PEI Status Code Service or
+  OEM Hook Status Code Library.
+
+  This function first tries to report status code via PEI Status Code Service. If the service
+  is not available, it then tries calling OEM Hook Status Code Library.
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  This is an optional parameter that may be
+                            NULL.
+  @param  Data              Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+
+  @retval EFI_SUCCESS       The status code was reported.
+  @retval EFI_UNSUPPORTED   Status code type is not supported.
+  @retval Others            Failed to report status code.
+
+**/
+EFI_STATUS
+InternalReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN CONST EFI_GUID           *CallerId OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data     OPTIONAL
+  )
+{
+  CONST EFI_PEI_SERVICES  **PeiServices;
+  EFI_STATUS              Status;
+
+  if ((ReportProgressCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      (ReportErrorCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) ||
+      (ReportDebugCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_DEBUG_CODE)) {
+    PeiServices = GetPeiServicesTablePointer ();
+    Status = (*PeiServices)->ReportStatusCode (
+                               PeiServices,
+                               Type,
+                               Value,
+                               Instance,
+                               (EFI_GUID *)CallerId,
+                               Data
+                               );
+    if (Status == EFI_NOT_AVAILABLE_YET) {
+      Status = OemHookStatusCodeInitialize ();
+      if (!EFI_ERROR (Status)) {
+        return OemHookStatusCodeReport (Type, Value, Instance, (EFI_GUID *) CallerId, Data);
+      }
+    }
+    return Status;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Converts a status code to an 8-bit POST code value.
+
+  Converts the status code specified by CodeType and Value to an 8-bit POST code
+  and returns the 8-bit POST code in PostCode.  If CodeType is an
+  EFI_PROGRESS_CODE or CodeType is an EFI_ERROR_CODE, then bits 0..4 of PostCode
+  are set to bits 16..20 of Value, and bits 5..7 of PostCode are set to bits
+  24..26 of Value., and TRUE is returned.  Otherwise, FALSE is returned.
+
+  If PostCode is NULL, then ASSERT().
+
+  @param  CodeType  The type of status code being converted.
+  @param  Value     The status code value being converted.
+  @param  PostCode  A pointer to the 8-bit POST code value to return.
+
+  @retval  TRUE   The status code specified by CodeType and Value was converted
+                  to an 8-bit POST code and returned in  PostCode.
+  @retval  FALSE  The status code specified by CodeType and Value could not be
+                  converted to an 8-bit POST code value.
+
+**/
+BOOLEAN
+EFIAPI
+CodeTypeToPostCode (
+  IN  EFI_STATUS_CODE_TYPE   CodeType,
+  IN  EFI_STATUS_CODE_VALUE  Value,
+  OUT UINT8                  *PostCode
+  )
+{
+  //
+  // If PostCode is NULL, then ASSERT()
+  //
+  ASSERT (PostCode != NULL);
+
+  //
+  // Convert Value to an 8 bit post code
+  //
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE)) {
+    *PostCode  = (UINT8) ((((Value & EFI_STATUS_CODE_CLASS_MASK) >> 24) << 5) |
+                          (((Value & EFI_STATUS_CODE_SUBCLASS_MASK) >> 16) & 0x1f));
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts ASSERT() information from a status code structure.
+
+  Converts the status code specified by CodeType, Value, and Data to the ASSERT()
+  arguments specified by Filename, Description, and LineNumber.  If CodeType is
+  an EFI_ERROR_CODE, and CodeType has a severity of EFI_ERROR_UNRECOVERED, and
+  Value has an operation mask of EFI_SW_EC_ILLEGAL_SOFTWARE_STATE, extract
+  Filename, Description, and LineNumber from the optional data area of the
+  status code buffer specified by Data.  The optional data area of Data contains
+  a Null-terminated ASCII string for the FileName, followed by a Null-terminated
+  ASCII string for the Description, followed by a 32-bit LineNumber.  If the
+  ASSERT() information could be extracted from Data, then return TRUE.
+  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If Filename is NULL, then ASSERT().
+  If Description is NULL, then ASSERT().
+  If LineNumber is NULL, then ASSERT().
+
+  @param  CodeType     The type of status code being converted.
+  @param  Value        The status code value being converted.
+  @param  Data         Pointer to status code data buffer.
+  @param  Filename     Pointer to the source file name that generated the ASSERT().
+  @param  Description  Pointer to the description of the ASSERT().
+  @param  LineNumber   Pointer to source line number that generated the ASSERT().
+
+  @retval  TRUE   The status code specified by CodeType, Value, and Data was
+                  converted ASSERT() arguments specified by Filename, Description,
+                  and LineNumber.
+  @retval  FALSE  The status code specified by CodeType, Value, and Data could
+                  not be converted to ASSERT() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractAssertInfo (
+  IN EFI_STATUS_CODE_TYPE        CodeType,
+  IN EFI_STATUS_CODE_VALUE       Value,
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT CHAR8                      **Filename,
+  OUT CHAR8                      **Description,
+  OUT UINT32                     *LineNumber
+  )
+{
+  EFI_DEBUG_ASSERT_DATA  *AssertData;
+
+  ASSERT (Data        != NULL);
+  ASSERT (Filename    != NULL);
+  ASSERT (Description != NULL);
+  ASSERT (LineNumber  != NULL);
+
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK)      == EFI_ERROR_CODE) &&
+      ((CodeType & EFI_STATUS_CODE_SEVERITY_MASK)  == EFI_ERROR_UNRECOVERED) &&
+      ((Value    & EFI_STATUS_CODE_OPERATION_MASK) == EFI_SW_EC_ILLEGAL_SOFTWARE_STATE)) {
+    AssertData   = (EFI_DEBUG_ASSERT_DATA *)(Data + 1);
+    *Filename    = (CHAR8 *)(AssertData + 1);
+    *Description = *Filename + AsciiStrLen (*Filename) + 1;
+    *LineNumber  = AssertData->LineNumber;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts DEBUG() information from a status code structure.
+
+  Converts the status code specified by Data to the DEBUG() arguments specified
+  by ErrorLevel, Marker, and Format.  If type GUID in Data is
+  EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID, then extract ErrorLevel, Marker, and
+  Format from the optional data area of the status code buffer specified by Data.
+  The optional data area of Data contains a 32-bit ErrorLevel followed by Marker
+  which is 12 UINTN parameters, followed by a Null-terminated ASCII string for
+  the Format.  If the DEBUG() information could be extracted from Data, then
+  return TRUE.  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If ErrorLevel is NULL, then ASSERT().
+  If Marker is NULL, then ASSERT().
+  If Format is NULL, then ASSERT().
+
+  @param  Data        Pointer to status code data buffer.
+  @param  ErrorLevel  Pointer to error level mask for a debug message.
+  @param  Marker      Pointer to the variable argument list associated with Format.
+  @param  Format      Pointer to a Null-terminated ASCII format string of a
+                      debug message.
+
+  @retval  TRUE   The status code specified by Data was converted DEBUG() arguments
+                  specified by ErrorLevel, Marker, and Format.
+  @retval  FALSE  The status code specified by Data could not be converted to
+                  DEBUG() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractDebugInfo (
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT UINT32                     *ErrorLevel,
+  OUT BASE_LIST                  *Marker,
+  OUT CHAR8                      **Format
+  )
+{
+  EFI_DEBUG_INFO  *DebugInfo;
+
+  ASSERT (Data       != NULL);
+  ASSERT (ErrorLevel != NULL);
+  ASSERT (Marker     != NULL);
+  ASSERT (Format     != NULL);
+
+  //
+  // If the GUID type is not EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID then return FALSE
+  //
+  if (!CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeDebugGuid)) {
+    return FALSE;
+  }
+
+  //
+  // Retrieve the debug information from the status code record
+  //
+  DebugInfo = (EFI_DEBUG_INFO *)(Data + 1);
+
+  *ErrorLevel = DebugInfo->ErrorLevel;
+
+  //
+  // The first 12 * sizeof (UINT64) bytes following EFI_DEBUG_INFO are for variable arguments
+  // of format in DEBUG string. Its address is returned in Marker and has to be 64-bit aligned.
+  // It must be noticed that EFI_DEBUG_INFO follows EFI_STATUS_CODE_DATA, whose size is
+  // 20 bytes. The size of EFI_DEBUG_INFO is 4 bytes, so we can ensure that Marker
+  // returned is 64-bit aligned.
+  // 64-bit aligned is a must, otherwise retrieving 64-bit parameter from BASE_LIST will
+  // cause unalignment exception.
+  //
+  *Marker = (BASE_LIST) (DebugInfo + 1);
+  *Format = (CHAR8 *)(((UINT64 *)*Marker) + 12);
+
+  return TRUE;
+}
+
+
+/**
+  Reports a status code.
+
+  Reports the status code specified by the parameters Type and Value.  Status
+  code also require an instance, caller ID, and extended data.  This function
+  passed in a zero instance, NULL extended data, and a caller ID of
+  gEfiCallerIdGuid, which is the GUID for the module.
+
+  ReportStatusCode()must actively prevent recusrsion.  If ReportStatusCode()
+  is called while processing another any other Report Status Code Library function,
+  then ReportStatusCode() must return immediately.
+
+  @param  Type   Status code type.
+  @param  Value  Status code value.
+
+  @retval  EFI_SUCCESS       The status code was reported.
+  @retval  EFI_DEVICE_ERROR  There status code could not be reported due to a
+                             device error.
+  @retval  EFI_UNSUPPORTED   Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value
+  )
+{
+  return InternalReportStatusCode (Type, Value, 0, &gEfiCallerIdGuid, NULL);
+}
+
+
+/**
+  Reports a status code with a Device Path Protocol as the extended data.
+
+  Allocates and fills in the extended data section of a status code with the
+  Device Path Protocol specified by DevicePath.  This function is responsible
+  for allocating a buffer large enough for the standard header and the device
+  path.  The standard header is filled in with a GUID of
+  gEfiStatusCodeSpecificDataGuid.  The status code is reported with a zero
+  instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithDevicePath()must actively prevent recursion.  If
+  ReportStatusCodeWithDevicePath() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithDevicePath()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If DevicePath is NULL, then ASSERT().
+
+  @param  Type        Status code type.
+  @param  Value       Status code value.
+  @param  DevicePath  Pointer to the Device Path Protocol to be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by DevicePath.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithDevicePath (
+  IN EFI_STATUS_CODE_TYPE            Type,
+  IN EFI_STATUS_CODE_VALUE           Value,
+  IN CONST EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  )
+{
+  ASSERT (DevicePath != NULL);
+  //
+  // EFI_UNSUPPORTED is returned for device path is not supported in PEI phase.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Reports a status code with an extended data buffer.
+
+  Allocates and fills in the extended data section of a status code with the
+  extended data specified by ExtendedData and ExtendedDataSize.  ExtendedData
+  is assumed to be one of the data structures specified in Related Definitions.
+  These data structure do not have the standard header, so this function is
+  responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled
+  in with a GUID of  gEfiStatusCodeSpecificDataGuid.  The status code is reported
+  with a zero instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithExtendedData()must actively prevent recursion.  If
+  ReportStatusCodeWithExtendedData() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithExtendedData()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL, then ASSERT().
+  If ExtendedDataSize is 0, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  ExtendedData      Pointer to the extended data buffer to be reported.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer to
+                            be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by ExtendedData and ExtendedDataSize.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithExtendedData (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN CONST VOID             *ExtendedData,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  ASSERT (ExtendedData     != NULL);
+  ASSERT (ExtendedDataSize != 0);
+  return ReportStatusCodeEx (
+           Type,
+           Value,
+           0,
+           NULL,
+           NULL,
+           ExtendedData,
+           ExtendedDataSize
+           );
+}
+
+
+/**
+  Reports a status code with full parameters.
+
+  The function reports a status code.  If ExtendedData is NULL and ExtendedDataSize
+  is 0, then an extended data buffer is not reported.  If ExtendedData is not
+  NULL and ExtendedDataSize is not 0, then an extended data buffer is allocated.
+  ExtendedData is assumed not have the standard status code header, so this function
+  is responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled in
+  with a GUID specified by ExtendedDataGuid.  If ExtendedDataGuid is NULL, then a
+  GUID of gEfiStatusCodeSpecificDatauid is used.  The status code is reported with
+  an instance specified by Instance and a caller ID specified by CallerId.  If
+  CallerId is NULL, then a caller ID of gEfiCallerIdGuid is used.
+
+  ReportStatusCodeEx()must actively prevent recursion.  If ReportStatusCodeEx()
+  is called while processing another any other Report Status Code Library function,
+  then ReportStatusCodeEx() must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL and ExtendedDataSize is not zero, then ASSERT().
+  If ExtendedData is not NULL and ExtendedDataSize is zero, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  If this parameter is NULL, then a caller
+                            ID of gEfiCallerIdGuid is used.
+  @param  ExtendedDataGuid  Pointer to the GUID for the extended data buffer.
+                            If this parameter is NULL, then a the status code
+                            standard header is filled in with
+                            gEfiStatusCodeSpecificDataGuid.
+  @param  ExtendedData      Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer.
+
+  @retval  EFI_SUCCESS           The status code was reported.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate
+                                 the extended data section if it was specified.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeEx (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN UINT32                 Instance,
+  IN CONST EFI_GUID         *CallerId          OPTIONAL,
+  IN CONST EFI_GUID         *ExtendedDataGuid  OPTIONAL,
+  IN CONST VOID             *ExtendedData      OPTIONAL,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  EFI_STATUS_CODE_DATA  *StatusCodeData;
+  UINT64                Buffer[(MAX_EXTENDED_DATA_SIZE / sizeof (UINT64)) + 1];
+
+  //
+  // If ExtendedData is NULL and ExtendedDataSize is not zero, then ASSERT().
+  //
+  ASSERT (!((ExtendedData == NULL) && (ExtendedDataSize != 0)));
+  //
+  // If ExtendedData is not NULL and ExtendedDataSize is zero, then ASSERT().
+  //
+  ASSERT (!((ExtendedData != NULL) && (ExtendedDataSize == 0)));
+
+  if (ExtendedDataSize > (MAX_EXTENDED_DATA_SIZE - sizeof (EFI_STATUS_CODE_DATA))) {
+    //
+    // The local variable Buffer not large enough to hold the extended data associated
+    // with the status code being reported.
+    //
+    DEBUG ((EFI_D_ERROR, "Status code extended data is too large to be reported!\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  StatusCodeData = (EFI_STATUS_CODE_DATA  *) Buffer;
+  StatusCodeData->HeaderSize = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+  StatusCodeData->Size = (UINT16) ExtendedDataSize;
+  if (ExtendedDataGuid == NULL) {
+    ExtendedDataGuid = &gEfiStatusCodeSpecificDataGuid;
+  }
+  CopyGuid (&StatusCodeData->Type, ExtendedDataGuid);
+  if (ExtendedData != NULL) {
+    CopyMem (StatusCodeData + 1, ExtendedData, ExtendedDataSize);
+  }
+  if (CallerId == NULL) {
+    CallerId = &gEfiCallerIdGuid;
+  }
+  return InternalReportStatusCode (Type, Value, Instance, CallerId, StatusCodeData);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_PROGRESS_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportProgressCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_ERROR_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportErrorCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_DEBUG_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportDebugCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.c b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.c
new file mode 100644
index 0000000000..82882b8706
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.c
@@ -0,0 +1,35 @@
+/** @file
+  Null S3 Library instance does nothing and returns unsupported status.
+
+  This library instance is no longer used and module using this library
+  class should update to directly locate EFI_PEI_S3_RESUME_PPI defined
+  in PI 1.2 specification. 
+
+Copyright (c) 2006 - 2008, 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 <PiPei.h>
+#include <Library/S3Lib.h>
+
+/**
+  This function is responsible for calling the S3 resume vector in the ACPI Tables.
+  
+  @retval EFI_SUCESS   Success to restore config from S3.
+  @retval Others       Fail to restore config from S3.
+**/
+EFI_STATUS
+EFIAPI
+AcpiS3ResumeOs (
+  VOID
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.inf b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.inf
new file mode 100644
index 0000000000..f582054870
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.inf
@@ -0,0 +1,40 @@
+## @file
+#  Null S3 library instance for PEIM module.
+#  This library instance is no longer used and module using this library
+#  class should update to directly locate EFI_PEI_S3_RESUME_PPI defined
+#  in PI 1.2 specification. 
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiS3LibNull
+  MODULE_UNI_FILE                = PeiS3LibNull.uni
+  FILE_GUID                      = 018E1925-D6A2-4a2a-8958-817610A15ADF
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = S3Lib|PEIM
+
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  PeiS3LibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.uni b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.uni
new file mode 100644
index 0000000000..ac0ce9f08d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.uni
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptExecute.c b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptExecute.c
new file mode 100644
index 0000000000..45dd581b08
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptExecute.c
@@ -0,0 +1,1792 @@
+/** @file
+  Interpret and execute the S3 data in S3 boot script. 
+
+  Copyright (c) 2006 - 2015, 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 "InternalBootScriptLib.h"
+
+/**
+  Executes an SMBus operation to an SMBus controller. Returns when either the command has been
+  executed or an error is encountered in doing the operation.
+
+  The SmbusExecute() function provides a standard way to execute an operation as defined in the System
+  Management Bus (SMBus) Specification. The resulting transaction will be either that the SMBus
+  slave devices accept this transaction or that this function returns with error.
+
+  @param  SmbusAddress            Address that encodes the SMBUS Slave Address, SMBUS Command, SMBUS Data Length, 
+                                  and PEC.
+  @param  Operation               Signifies which particular SMBus hardware protocol instance that
+                                  it will use to execute the SMBus transactions. This SMBus
+                                  hardware protocol is defined by the SMBus Specification and is
+                                  not related to EFI.
+  @param  Length                  Signifies the number of bytes that this operation will do. The
+                                  maximum number of bytes can be revision specific and operation
+                                  specific. This field will contain the actual number of bytes that
+                                  are executed for this operation. Not all operations require this
+                                  argument.
+  @param  Buffer                  Contains the value of data to execute to the SMBus slave device.
+                                  Not all operations require this argument. The length of this
+                                  buffer is identified by Length.
+
+  @retval EFI_SUCCESS             The last data that was returned from the access matched the poll
+                                  exit criteria.
+  @retval EFI_CRC_ERROR           Checksum is not correct (PEC is incorrect).
+  @retval EFI_TIMEOUT             Timeout expired before the operation was completed. Timeout is
+                                  determined by the SMBus host controller device.
+  @retval EFI_OUT_OF_RESOURCES    The request could not be completed due to a lack of resources.
+  @retval EFI_DEVICE_ERROR        The request was not completed because a failure that was
+                                  reflected in the Host Status Register bit. Device errors are a
+                                  result of a transaction collision, illegal command field,
+                                  unclaimed cycle (host initiated), or bus errors (collisions).
+  @retval EFI_INVALID_PARAMETER   Operation is not defined in EFI_SMBUS_OPERATION.
+  @retval EFI_INVALID_PARAMETER   Length/Buffer is NULL for operations except for EfiSmbusQuickRead
+                                  and EfiSmbusQuickWrite. Length is outside the range of valid
+                                  values.
+  @retval EFI_UNSUPPORTED         The SMBus operation or PEC is not supported.
+  @retval EFI_BUFFER_TOO_SMALL    Buffer is not sufficient for this operation.
+
+**/
+EFI_STATUS
+SmbusExecute (
+  IN     UINTN                    SmbusAddress,
+  IN     EFI_SMBUS_OPERATION      Operation,
+  IN OUT UINTN                    *Length,
+  IN OUT VOID                     *Buffer
+  )
+{
+  EFI_STATUS                      Status;
+  UINT8                           WorkBuffer[MAX_SMBUS_BLOCK_LEN];
+
+  switch (Operation) {
+    case EfiSmbusQuickRead:
+      DEBUG ((EFI_D_INFO, "EfiSmbusQuickRead - 0x%08x\n", SmbusAddress));
+      SmBusQuickRead (SmbusAddress, &Status);
+      break;
+    case EfiSmbusQuickWrite:
+      DEBUG ((EFI_D_INFO, "EfiSmbusQuickWrite - 0x%08x\n", SmbusAddress));
+      SmBusQuickWrite (SmbusAddress, &Status);
+      break;
+    case EfiSmbusReceiveByte:
+      DEBUG ((EFI_D_INFO, "EfiSmbusReceiveByte - 0x%08x\n", SmbusAddress));
+      SmBusReceiveByte (SmbusAddress, &Status);
+      break;
+    case EfiSmbusSendByte:
+      DEBUG ((EFI_D_INFO, "EfiSmbusSendByte - 0x%08x (0x%02x)\n", SmbusAddress, (UINTN)*(UINT8 *) Buffer));
+      SmBusSendByte (SmbusAddress, *(UINT8 *) Buffer, &Status);
+      break;
+    case EfiSmbusReadByte:
+      DEBUG ((EFI_D_INFO, "EfiSmbusReadByte - 0x%08x\n", SmbusAddress));
+      SmBusReadDataByte (SmbusAddress, &Status);
+      break;
+    case EfiSmbusWriteByte:
+      DEBUG ((EFI_D_INFO, "EfiSmbusWriteByte - 0x%08x (0x%02x)\n", SmbusAddress, (UINTN)*(UINT8 *) Buffer));
+      SmBusWriteDataByte (SmbusAddress, *(UINT8 *) Buffer, &Status);
+      break;
+    case EfiSmbusReadWord:
+      DEBUG ((EFI_D_INFO, "EfiSmbusReadWord - 0x%08x\n", SmbusAddress));
+      SmBusReadDataWord (SmbusAddress, &Status);
+      break;
+    case EfiSmbusWriteWord:
+      DEBUG ((EFI_D_INFO, "EfiSmbusWriteWord - 0x%08x (0x%04x)\n", SmbusAddress, (UINTN)*(UINT16 *) Buffer));
+      SmBusWriteDataWord (SmbusAddress, *(UINT16 *) Buffer, &Status);
+      break;
+    case EfiSmbusProcessCall:
+      DEBUG ((EFI_D_INFO, "EfiSmbusProcessCall - 0x%08x (0x%04x)\n", SmbusAddress, (UINTN)*(UINT16 *) Buffer));
+      SmBusProcessCall (SmbusAddress, *(UINT16 *) Buffer, &Status);
+      break;
+    case EfiSmbusReadBlock:
+      DEBUG ((EFI_D_INFO, "EfiSmbusReadBlock - 0x%08x\n", SmbusAddress));
+      SmBusReadBlock (SmbusAddress, WorkBuffer, &Status);
+      break;
+    case EfiSmbusWriteBlock:
+      DEBUG ((EFI_D_INFO, "EfiSmbusWriteBlock - 0x%08x\n", SmbusAddress));
+      SmBusWriteBlock ((SmbusAddress + SMBUS_LIB_ADDRESS (0, 0, (*Length), FALSE)), Buffer, &Status);
+      break;
+    case EfiSmbusBWBRProcessCall:
+      DEBUG ((EFI_D_INFO, "EfiSmbusBWBRProcessCall - 0x%08x\n", SmbusAddress));
+      SmBusBlockProcessCall ((SmbusAddress + SMBUS_LIB_ADDRESS (0, 0, (*Length), FALSE)), Buffer, WorkBuffer, &Status);
+      break;
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+
+  return Status;  
+}
+
+/**
+  Translates boot script width and address stride to MDE library interface.
+
+
+  @param Width          Width of the operation.
+  @param Address        Address of the operation.
+  @param AddressStride  Instride for stepping input buffer.
+  @param BufferStride   Outstride for stepping output buffer.  
+
+  @retval EFI_SUCCESS  Successful translation.
+  @retval EFI_INVALID_PARAMETER Width or Address is invalid.
+**/
+EFI_STATUS
+BuildLoopData (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH      Width,
+  IN  UINT64                    Address,
+  OUT UINTN                     *AddressStride,
+  OUT UINTN                     *BufferStride
+  )
+{
+  UINTN AlignMask;
+
+  if (Width >= S3BootScriptWidthMaximum) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *AddressStride  = (UINT32)(1 << (Width & 0x03));
+  *BufferStride   = *AddressStride;
+
+  AlignMask       = *AddressStride - 1;
+  if ((Address & AlignMask) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Width >= S3BootScriptWidthFifoUint8 && Width <= S3BootScriptWidthFifoUint64) {
+    *AddressStride = 0;
+  }
+
+  if (Width >= S3BootScriptWidthFillUint8 && Width <= S3BootScriptWidthFillUint64) {
+    *BufferStride = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform IO read operation
+  
+  @param[in]  Width   Width of the operation.
+  @param[in]  Address Address of the operation.
+  @param[in]  Count   Count of the number of accesses to perform.
+  @param[out] Buffer  Pointer to the buffer to read from I/O space.  
+
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+                                Address is outside the legal range of I/O ports.  
+                                
+**/
+EFI_STATUS
+ScriptIoRead (
+  IN      S3_BOOT_SCRIPT_LIB_WIDTH Width,
+  IN      UINT64                   Address,
+  IN      UINTN                    Count,
+  OUT     VOID                     *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINTN       BufferStride;
+  PTR         Out;
+
+  Out.Buf = (UINT8 *) Buffer;
+
+  if (Address > MAX_IO_ADDRESS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = BuildLoopData (Width, Address, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  for (; Count > 0; Count--, Address += AddressStride, Out.Buf += BufferStride) {
+    switch (Width) {
+
+    case S3BootScriptWidthUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint8 = IoRead8 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint8 = IoRead8 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint8 = IoRead8 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint16 = IoRead16 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint16 = IoRead16 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint16 = IoRead16 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint32 = IoRead32 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint32 = IoRead32 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint32 = IoRead32 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint64 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint64 = IoRead64 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint64 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint64 = IoRead64 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint64 - 0x%08x\n", (UINTN) Address));
+      *Out.Uint64 = IoRead64 ((UINTN) Address);
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform IO write operation
+  
+  @param[in]  Width Width of the operation.
+  @param[in]  Address Address of the operation.
+  @param[in]  Count Count of the number of accesses to perform.
+  @param[in]  Buffer Pointer to the buffer to write to I/O space.  
+
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+                                Address is outside the legal range of I/O ports.  
+                                
+**/
+EFI_STATUS
+ScriptIoWrite (
+  IN      S3_BOOT_SCRIPT_LIB_WIDTH  Width,
+  IN      UINT64                 Address,
+  IN      UINTN                  Count,
+  IN      VOID                   *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINTN       BufferStride;
+  UINT64      OriginalAddress;
+  PTR         In;
+  PTR         OriginalIn;
+
+  In.Buf = (UINT8 *) Buffer;
+
+  if (Address > MAX_IO_ADDRESS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = BuildLoopData (Width, Address, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  OriginalAddress = Address;
+  OriginalIn.Buf = In.Buf;
+  for (; Count > 0; Count--, Address += AddressStride, In.Buf += BufferStride) {
+    switch (Width) {
+      case S3BootScriptWidthUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x (0x%02x)\n", (UINTN)Address, (UINTN)*In.Uint8));
+        IoWrite8 ((UINTN) Address, *In.Uint8);
+        break;      
+      case S3BootScriptWidthFifoUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x (0x%02x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint8));
+        IoWrite8 ((UINTN) OriginalAddress, *In.Uint8);
+        break;       
+      case S3BootScriptWidthFillUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x (0x%02x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint8));
+        IoWrite8 ((UINTN) Address, *OriginalIn.Uint8);
+        break;
+      case S3BootScriptWidthUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x (0x%04x)\n", (UINTN)Address, (UINTN)*In.Uint16));
+        IoWrite16 ((UINTN) Address, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFifoUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x (0x%04x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint16));
+        IoWrite16 ((UINTN) OriginalAddress, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFillUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x (0x%04x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint16));
+        IoWrite16 ((UINTN) Address, *OriginalIn.Uint16);
+        break;
+      case S3BootScriptWidthUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x (0x%08x)\n", (UINTN)Address, (UINTN)*In.Uint32));
+        IoWrite32 ((UINTN) Address, *In.Uint32);
+        break;      
+      case S3BootScriptWidthFifoUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x (0x%08x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint32));
+        IoWrite32 ((UINTN) OriginalAddress, *In.Uint32);
+        break;
+      case S3BootScriptWidthFillUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x (0x%08x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint32));
+        IoWrite32 ((UINTN) Address, *OriginalIn.Uint32);
+        break;
+      case S3BootScriptWidthUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint64 - 0x%08x (0x%016lx)\n", (UINTN)Address, *In.Uint64));
+        IoWrite64 ((UINTN) Address, *In.Uint64);
+        break;      
+      case S3BootScriptWidthFifoUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint64 - 0x%08x (0x%016lx)\n", (UINTN)OriginalAddress, *In.Uint64));
+        IoWrite64 ((UINTN) OriginalAddress, *In.Uint64);
+        break;      
+      case S3BootScriptWidthFillUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint64 - 0x%08x (0x%016lx)\n", (UINTN)Address, *OriginalIn.Uint64));
+        IoWrite64 ((UINTN) Address, *OriginalIn.Uint64);
+        break;
+      default:
+        return EFI_INVALID_PARAMETER;
+    }
+  }
+  
+
+  return EFI_SUCCESS;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_IO_WRITE OP code.
+  
+  @param Script       Pointer to the node which is to be interpreted.
+
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+                                Address is outside the legal range of I/O ports.  
+                                
+**/
+EFI_STATUS
+BootScriptExecuteIoWrite (
+  IN UINT8                    *Script    
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH   Width;
+  UINT64                     Address;
+  UINTN                      Count;
+  VOID                      *Buffer;
+  EFI_BOOT_SCRIPT_IO_WRITE   IoWrite;
+  
+  CopyMem ((VOID*)&IoWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_IO_WRITE));
+  Width = (S3_BOOT_SCRIPT_LIB_WIDTH) IoWrite.Width;
+  Address = IoWrite.Address;
+  Count = IoWrite.Count;
+  Buffer = Script + sizeof (EFI_BOOT_SCRIPT_IO_WRITE);
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteIoWrite - 0x%08x, 0x%08x, 0x%08x\n", (UINTN)Address, Count, (UINTN)Width));
+  return ScriptIoWrite(Width, Address, Count, Buffer);
+}
+/**
+  Perform memory read operation
+  
+  @param  Width Width of the operation.
+  @param  Address Address of the operation.
+  @param  Count Count of the number of accesses to perform.
+  @param  Buffer Pointer to the buffer read from memory.  
+
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+  @retval EFI_UNSUPPORTED The address range specified by Address, Width, and Count 
+                          is not valid for this EFI System.  
+                                
+**/
+EFI_STATUS
+ScriptMemoryRead (
+  IN       S3_BOOT_SCRIPT_LIB_WIDTH    Width,
+  IN       UINT64                   Address,
+  IN       UINTN                    Count,
+  IN OUT   VOID                     *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINTN       BufferStride;
+  PTR         Out;
+
+  Out.Buf = Buffer;
+
+  Status  = BuildLoopData (Width, Address, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  for (; Count > 0; Count--, Address += AddressStride, Out.Buf += BufferStride) {
+    switch (Width) {
+    case S3BootScriptWidthUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint8 = MmioRead8 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint8 = MmioRead8 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint8 = MmioRead8 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint16 = MmioRead16 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint16 = MmioRead16 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint16 = MmioRead16 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint32 = MmioRead32 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint32 = MmioRead32 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint32 = MmioRead32 ((UINTN) Address);
+      break;
+
+    case S3BootScriptWidthUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint64 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint64 = MmioRead64 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFifoUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint64 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint64 = MmioRead64 ((UINTN) Address);
+      break;
+    case S3BootScriptWidthFillUint64:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint64 - 0x%08x\n", (UINTN)Address));
+      *Out.Uint64 = MmioRead64 ((UINTN) Address);
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+/**
+  Perform memory write operation
+  
+  @param   Width   Width of the operation.
+  @param   Address Address of the operation.
+  @param   Count   Count of the number of accesses to perform.
+  @param   Buffer  Pointer to the buffer write to memory.      
+
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+  @retval EFI_UNSUPPORTED The address range specified by Address, Width, and Count 
+                          is not valid for this EFI System.  
+                                
+**/
+EFI_STATUS
+ScriptMemoryWrite (
+  IN      S3_BOOT_SCRIPT_LIB_WIDTH Width,
+  IN      UINT64                Address,
+  IN      UINTN                 Count,
+  IN OUT  VOID                 *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINT64      OriginalAddress;  
+  UINTN       BufferStride;
+  PTR         In;
+  PTR         OriginalIn;
+
+  In.Buf  = Buffer;
+
+  Status  = BuildLoopData (Width, Address, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  OriginalAddress = Address;
+  OriginalIn.Buf = In.Buf;  
+  for (; Count > 0; Count--, Address += AddressStride, In.Buf += BufferStride) {
+    switch (Width) {
+      case S3BootScriptWidthUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x (0x%02x)\n", (UINTN)Address, (UINTN)*In.Uint8));
+        MmioWrite8 ((UINTN) Address, *In.Uint8);
+        break;      
+      case S3BootScriptWidthFifoUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x (0x%02x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint8));
+        MmioWrite8 ((UINTN) OriginalAddress, *In.Uint8);
+        break;      
+      case S3BootScriptWidthFillUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x (0x%02x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint8));
+        MmioWrite8 ((UINTN) Address, *OriginalIn.Uint8);
+        break;
+      case S3BootScriptWidthUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x (0x%04x)\n", (UINTN)Address, (UINTN)*In.Uint16));
+        MmioWrite16 ((UINTN) Address, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFifoUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x (0x%04x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint16));
+        MmioWrite16 ((UINTN) OriginalAddress, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFillUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x (0x%04x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint16));
+        MmioWrite16 ((UINTN) Address, *OriginalIn.Uint16);
+        break;
+      case S3BootScriptWidthUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x (0x%08x)\n", (UINTN)Address, (UINTN)*In.Uint32));
+        MmioWrite32 ((UINTN) Address, *In.Uint32);
+        break;      
+      case S3BootScriptWidthFifoUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x (0x%08x)\n", (UINTN)OriginalAddress, (UINTN)*In.Uint32));
+        MmioWrite32 ((UINTN) OriginalAddress, *In.Uint32);
+        break;      
+      case S3BootScriptWidthFillUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x (0x%08x)\n", (UINTN)Address, (UINTN)*OriginalIn.Uint32));
+        MmioWrite32 ((UINTN) Address, *OriginalIn.Uint32);
+        break;
+      case S3BootScriptWidthUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint64 - 0x%08x (0x%016lx)\n", (UINTN)Address, *In.Uint64));
+        MmioWrite64 ((UINTN) Address, *In.Uint64);
+        break;      
+      case S3BootScriptWidthFifoUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint64 - 0x%08x (0x%016lx)\n", (UINTN)OriginalAddress, *In.Uint64));
+        MmioWrite64 ((UINTN) OriginalAddress, *In.Uint64);
+        break;      
+      case S3BootScriptWidthFillUint64:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint64 - 0x%08x (0x%016lx)\n", (UINTN)Address, *OriginalIn.Uint64));
+        MmioWrite64 ((UINTN) Address, *OriginalIn.Uint64);
+        break;
+      default:
+        return EFI_UNSUPPORTED;
+    }
+  }
+  return EFI_SUCCESS;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_MEM_WRITE OP code.
+
+  @param[in]  Script Pointer to the node which is to be interpreted.
+  
+  @retval EFI_SUCCESS The data was written to the EFI System.
+  @retval EFI_INVALID_PARAMETER Width is invalid for this EFI System.
+                                Buffer is NULL.
+                                The Buffer is not aligned for the given Width.
+  @retval EFI_UNSUPPORTED The address range specified by Address, Width, and Count 
+                          is not valid for this EFI System.  
+                                
+**/
+EFI_STATUS
+BootScriptExecuteMemoryWrite (
+  IN UINT8             *Script
+  )
+{
+  VOID            *Buffer;
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64           Address;
+  UINTN            Count;
+  EFI_BOOT_SCRIPT_MEM_WRITE  MemWrite;
+  
+  CopyMem((VOID*)&MemWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_MEM_WRITE));
+  Width   = (S3_BOOT_SCRIPT_LIB_WIDTH)MemWrite.Width;
+  Address = MemWrite.Address;
+  Count   = MemWrite.Count;
+  Buffer  = Script + sizeof(EFI_BOOT_SCRIPT_MEM_WRITE);
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteMemoryWrite - 0x%08x, 0x%08x, 0x%08x\n", (UINTN)Address, Count, (UINTN)Width));
+  return ScriptMemoryWrite (Width,Address, Count,  Buffer);
+  
+}  
+/**
+  Performance PCI configuration read operation
+
+  @param  Width   Width of the operation.
+  @param  Address Address of the operation.
+  @param  Count   Count of the number of accesses to perform.
+  @param  Buffer  Pointer to the buffer read from PCI config space
+  
+  @retval EFI_SUCCESS The read succeed.
+  @retval EFI_INVALID_PARAMETER if Width is not defined  
+  @note  A known Limitations in the implementation which is 64bits operations are not supported.
+
+**/
+EFI_STATUS
+ScriptPciCfgRead (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH    Width,
+  IN  UINT64                       Address,
+  IN  UINTN                        Count,
+  OUT VOID                        *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINTN       BufferStride;
+  PTR         Out;
+  UINTN       PciAddress;
+
+  Out.Buf = (UINT8 *) Buffer;
+
+  PciAddress = PCI_ADDRESS_ENCODE (Address);
+
+  Status = BuildLoopData (Width, PciAddress, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  for (; Count > 0; Count--, PciAddress += AddressStride, Out.Buf += BufferStride) {
+    switch (Width) {
+    case S3BootScriptWidthUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x\n", PciAddress));
+      *Out.Uint8 = PciRead8 (PciAddress);
+      break;
+    case S3BootScriptWidthFifoUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x\n", PciAddress));
+      *Out.Uint8 = PciRead8 (PciAddress);
+      break;
+    case S3BootScriptWidthFillUint8:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x\n", PciAddress));
+      *Out.Uint8 = PciRead8 (PciAddress);
+      break;
+
+    case S3BootScriptWidthUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x\n", PciAddress));
+      *Out.Uint16 = PciRead16 (PciAddress);
+      break;
+    case S3BootScriptWidthFifoUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x\n", PciAddress));
+      *Out.Uint16 = PciRead16 (PciAddress);
+      break;
+    case S3BootScriptWidthFillUint16:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x\n", PciAddress));
+      *Out.Uint16 = PciRead16 (PciAddress);
+      break;
+
+    case S3BootScriptWidthUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x\n", PciAddress));
+      *Out.Uint32 = PciRead32 (PciAddress);
+      break;
+    case S3BootScriptWidthFifoUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x\n", PciAddress));
+      *Out.Uint32 = PciRead32 (PciAddress);
+      break;
+    case S3BootScriptWidthFillUint32:
+      DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x\n", PciAddress));
+      *Out.Uint32 = PciRead32 (PciAddress);
+      break;
+
+    default:
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Performance PCI configuration write operation
+
+  @param  Width   Width of the operation.
+  @param  Address Address of the operation.
+  @param  Count   Count of the number of accesses to perform.
+  @param  Buffer  Pointer to the buffer write to PCI config space
+  
+  @retval EFI_SUCCESS The write succeed.
+  @retval EFI_INVALID_PARAMETER if Width is not defined  
+  @note  A known Limitations in the implementation which is 64bits operations are not supported.
+
+**/
+EFI_STATUS
+ScriptPciCfgWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH     Width,
+  IN  UINT64                       Address,
+  IN  UINTN                        Count,
+  IN  VOID                         *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       AddressStride;
+  UINTN       BufferStride;
+  UINTN       OriginalPciAddress;
+  PTR         In;
+  PTR         OriginalIn;
+  UINTN       PciAddress;
+
+  In.Buf = (UINT8 *) Buffer;
+
+  PciAddress = PCI_ADDRESS_ENCODE (Address);
+
+  Status = BuildLoopData (Width, PciAddress, &AddressStride, &BufferStride);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Loop for each iteration and move the data
+  //
+  OriginalPciAddress = PciAddress;
+  OriginalIn.Buf = In.Buf;
+  for (; Count > 0; Count--, PciAddress += AddressStride, In.Buf += BufferStride) {
+    switch (Width) {
+      case S3BootScriptWidthUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint8 - 0x%08x (0x%02x)\n", PciAddress, (UINTN)*In.Uint8));
+        PciWrite8 (PciAddress, *In.Uint8);
+        break;      
+      case S3BootScriptWidthFifoUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint8 - 0x%08x (0x%02x)\n", OriginalPciAddress, (UINTN)*In.Uint8));
+        PciWrite8 (OriginalPciAddress, *In.Uint8);
+        break;      
+      case S3BootScriptWidthFillUint8:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint8 - 0x%08x (0x%02x)\n", PciAddress, (UINTN)*OriginalIn.Uint8));
+        PciWrite8 (PciAddress, *OriginalIn.Uint8);
+        break;
+      case S3BootScriptWidthUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint16 - 0x%08x (0x%04x)\n", PciAddress, (UINTN)*In.Uint16));
+        PciWrite16 (PciAddress, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFifoUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint16 - 0x%08x (0x%04x)\n", OriginalPciAddress, (UINTN)*In.Uint16));
+        PciWrite16 (OriginalPciAddress, *In.Uint16);
+        break;      
+      case S3BootScriptWidthFillUint16:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint16 - 0x%08x (0x%04x)\n", PciAddress, (UINTN)*OriginalIn.Uint16));
+        PciWrite16 (PciAddress, *OriginalIn.Uint16);
+        break;
+      case S3BootScriptWidthUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthUint32 - 0x%08x (0x%08x)\n", PciAddress, (UINTN)*In.Uint32));
+        PciWrite32 (PciAddress, *In.Uint32);
+        break;      
+      case S3BootScriptWidthFifoUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFifoUint32 - 0x%08x (0x%08x)\n", OriginalPciAddress, (UINTN)*In.Uint32));
+        PciWrite32 (OriginalPciAddress, *In.Uint32);
+        break;      
+      case S3BootScriptWidthFillUint32:
+        DEBUG ((EFI_D_INFO, "S3BootScriptWidthFillUint32 - 0x%08x (0x%08x)\n", (UINTN)PciAddress, (UINTN)*OriginalIn.Uint32));
+        PciWrite32 (PciAddress, *OriginalIn.Uint32);
+        break;
+      default:
+        return EFI_INVALID_PARAMETER;
+    }
+  }
+  return EFI_SUCCESS;
+}
+/**
+  Performance PCI configuration 2 read operation
+  
+  @param     Width                      Width of the operation.
+  @param     Segment                    Pci segment number
+  @param     Address                    Address of the operation.
+  @param     Count                      Count of the number of accesses to perform.
+  @param     Buffer                     Pointer to the buffer to read from PCI config space.
+
+  @retval    EFI_SUCCESS                The data was written to the EFI System.
+  @retval    EFI_INVALID_PARAMETER      Width is invalid for this EFI System.
+                                        Buffer is NULL.
+                                        The Buffer is not aligned for the given Width.
+                                        Address is outside the legal range of I/O ports.
+  @note  A known Limitations in the implementation which is the 'Segment' parameter is assumed as 
+         Zero, or else, assert.
+**/
+EFI_STATUS
+ScriptPciCfg2Read (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH    Width,
+  IN  UINT16                   Segment,  
+  IN  UINT64                   Address,
+  IN  UINTN                    Count,
+  OUT VOID                     *Buffer
+  )
+{
+  ASSERT (Segment==0);
+  
+  return ScriptPciCfgRead (Width, Address, Count, Buffer);
+}
+/**
+  Performance PCI configuration 2 write operation
+  
+  @param     Width                      Width of the operation.
+  @param     Segment                    Pci segment number
+  @param     Address                    Address of the operation.
+  @param     Count                      Count of the number of accesses to perform.
+  @param     Buffer                     Pointer to the buffer to write to PCI config space.
+
+  @retval    EFI_SUCCESS                The data was written to the EFI System.
+  @retval    EFI_INVALID_PARAMETER      Width is invalid for this EFI System.
+                                        Buffer is NULL.
+                                        The Buffer is not aligned for the given Width.
+                                        Address is outside the legal range of I/O ports.
+  @note  A known Limitations in the implementation which is the 'Segment' parameter is assumed as 
+         Zero, or else, assert.
+                                
+**/
+EFI_STATUS
+EFIAPI
+ScriptPciCfg2Write (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH    Width,
+  IN  UINT16                   Segment,  
+  IN  UINT64                   Address,
+  IN  UINTN                    Count,
+  IN  VOID                     *Buffer
+  )
+{
+  ASSERT (Segment==0);
+  return ScriptPciCfgWrite (Width, Address, Count, Buffer);
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE OP code.
+  
+  @param  Script        The pointer of typed node in boot script table 
+  
+  @retval EFI_SUCCESS  The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecutePciCfgWrite (
+  IN UINT8                    *Script
+  )
+{
+  VOID                              *Buffer;
+  S3_BOOT_SCRIPT_LIB_WIDTH          Width;
+  UINT64                            Address;
+  UINTN                             Count;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE  PciCfgWrite;
+
+  CopyMem ((VOID*)&PciCfgWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE));
+
+  Width   = (S3_BOOT_SCRIPT_LIB_WIDTH)PciCfgWrite.Width;
+  Address = PciCfgWrite.Address;
+  Count   = PciCfgWrite.Count;
+  Buffer  = Script + sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE);
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecutePciCfgWrite - 0x%08x, 0x%08x, 0x%08x\n", PCI_ADDRESS_ENCODE (Address), Count, (UINTN)Width));
+  return ScriptPciCfgWrite (Width, Address, Count, Buffer);
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_IO_READ_WRITE OP code.
+
+  @param Script   The pointer of typed node in boot script table 
+  @param AndMask  Mask value for 'and' operation
+  @param OrMask   Mask value for 'or' operation
+
+  @retval EFI_SUCCESS    The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteIoReadWrite (
+  IN  UINT8                        *Script,
+  IN  UINT64                       AndMask,
+  IN  UINT64                        OrMask
+  )
+
+{
+  EFI_STATUS  Status;
+  UINT64      Data;
+  EFI_BOOT_SCRIPT_IO_READ_WRITE IoReadWrite;
+  
+  Data = 0;
+  
+  CopyMem((VOID*)&IoReadWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_IO_READ_WRITE));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteIoReadWrite - 0x%08x, 0x%016lx, 0x%016lx\n", (UINTN)IoReadWrite.Address, AndMask, OrMask));
+
+  Status = ScriptIoRead (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) IoReadWrite.Width,
+             IoReadWrite.Address,
+             1,
+             &Data
+             );
+  if (!EFI_ERROR (Status)) {
+    Data = (Data & AndMask) | OrMask;
+    Status = ScriptIoWrite (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) IoReadWrite.Width,
+               IoReadWrite.Address,
+               1,
+               &Data
+               );
+  }
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_MEM_READ_WRITE OP code.
+
+  @param Script    The pointer of typed node in boot script table 
+  @param AndMask   Mask value for 'and' operation
+  @param OrMask    Mask value for 'or' operation
+
+  @retval EFI_SUCCESS The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteMemoryReadWrite (
+  IN UINT8                        *Script,
+  IN UINT64                        AndMask,
+  IN UINT64                        OrMask
+  )
+
+{
+  EFI_STATUS  Status;
+  UINT64      Data;
+  EFI_BOOT_SCRIPT_MEM_READ_WRITE  MemReadWrite;
+  
+  Data = 0;
+  
+  CopyMem((VOID*)&MemReadWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_MEM_READ_WRITE));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteMemoryReadWrite - 0x%08x, 0x%016lx, 0x%016lx\n", (UINTN)MemReadWrite.Address, AndMask, OrMask));
+  
+  Status = ScriptMemoryRead (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) MemReadWrite.Width,
+             MemReadWrite.Address,
+             1,
+             &Data
+             );
+  if (!EFI_ERROR (Status)) {
+    Data = (Data & AndMask) | OrMask;
+    Status = ScriptMemoryWrite (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) MemReadWrite.Width,
+               MemReadWrite.Address,
+               1,
+               &Data
+               );
+  }
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CFG_READ_WRITE OP code.
+
+  @param Script   The pointer of typed node in boot script table 
+  @param AndMask  Mask value for 'and' operation
+  @param OrMask   Mask value for 'or' operation
+
+  @retval EFI_SUCCESS   The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecutePciCfgReadWrite (
+  IN UINT8                     *Script,
+  IN UINT64                    AndMask,
+  IN UINT64                    OrMask
+  )
+
+{
+  EFI_STATUS  Status;
+  UINT64      Data;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE  PciCfgReadWrite;
+
+  Data = 0;
+
+  CopyMem((VOID*)&PciCfgReadWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecutePciCfgReadWrite - 0x%08x, 0x%016lx, 0x%016lx\n", PCI_ADDRESS_ENCODE (PciCfgReadWrite.Address), AndMask, OrMask));
+  
+  Status = ScriptPciCfgRead (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfgReadWrite.Width,
+             PciCfgReadWrite.Address,
+             1,
+             &Data
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Data = (Data & AndMask) | OrMask;
+
+  Status = ScriptPciCfgWrite (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfgReadWrite.Width,
+             PciCfgReadWrite.Address,
+             1,
+             &Data
+             );
+
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_SMBUS_EXECUTE OP code.
+
+  @param Script  The pointer of typed node in boot script table 
+ 
+  @retval EFI_SUCCESS      The operation was executed successfully
+  @retval EFI_UNSUPPORTED  Cannot locate smbus ppi or occur error of script execution
+  @retval Others           Result of script execution 
+**/
+EFI_STATUS
+BootScriptExecuteSmbusExecute (
+  IN UINT8                     *Script
+  )
+{
+  UINTN                    SmBusAddress;
+  UINTN                    DataSize;
+  EFI_BOOT_SCRIPT_SMBUS_EXECUTE SmbusExecuteEntry;
+  
+  CopyMem ((VOID*)&SmbusExecuteEntry, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_SMBUS_EXECUTE ));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteSmbusExecute - 0x%08x, 0x%08x\n", (UINTN)SmbusExecuteEntry.SmBusAddress, (UINTN)SmbusExecuteEntry.Operation));
+
+  SmBusAddress = (UINTN)SmbusExecuteEntry.SmBusAddress;
+  DataSize = (UINTN) SmbusExecuteEntry.DataSize;
+  return SmbusExecute (
+           SmBusAddress,
+           (EFI_SMBUS_OPERATION) SmbusExecuteEntry.Operation,
+           &DataSize,
+           Script + sizeof (EFI_BOOT_SCRIPT_SMBUS_EXECUTE)
+           );
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_STALL OP code.
+
+  @param Script      The pointer of typed node in boot script table 
+  
+  @retval EFI_SUCCESS The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteStall (
+  IN UINT8                 *Script
+  )
+{
+  EFI_BOOT_SCRIPT_STALL    Stall;
+  
+  CopyMem ((VOID*)&Stall, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_STALL));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteStall - 0x%08x\n", (UINTN)Stall.Duration));
+
+  MicroSecondDelay ((UINTN) Stall.Duration);
+  return EFI_SUCCESS;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_DISPATCH OP code.
+  
+  @param Script  The pointer of typed node in boot script table 
+  @retval EFI_SUCCESS  The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteDispatch (
+  IN UINT8                  *Script
+  )
+{
+  EFI_STATUS                Status;
+  DISPATCH_ENTRYPOINT_FUNC  EntryFunc;
+  EFI_BOOT_SCRIPT_DISPATCH  ScriptDispatch;
+  
+  CopyMem ((VOID*)&ScriptDispatch, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_DISPATCH));
+  EntryFunc = (DISPATCH_ENTRYPOINT_FUNC) (UINTN) (ScriptDispatch.EntryPoint);
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteDispatch - 0x%08x\n", (UINTN)ScriptDispatch.EntryPoint));
+
+  Status    = EntryFunc (NULL, NULL);
+
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_DISPATCH_2 OP code.
+
+  @param  Script       The pointer of typed node in boot script table 
+  @retval EFI_SUCCESS  The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteDispatch2 (
+  IN UINT8                  *Script
+  )
+{
+  EFI_STATUS                Status;
+  DISPATCH_ENTRYPOINT_FUNC  EntryFunc;
+  EFI_BOOT_SCRIPT_DISPATCH_2  ScriptDispatch2;
+  
+  CopyMem ((VOID*)&ScriptDispatch2, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_DISPATCH_2));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteDispatch2 - 0x%08x(0x%08x)\n", (UINTN)ScriptDispatch2.EntryPoint, (UINTN)ScriptDispatch2.Context));
+  
+  EntryFunc = (DISPATCH_ENTRYPOINT_FUNC) (UINTN) (ScriptDispatch2.EntryPoint);
+
+  Status    = EntryFunc (NULL, (VOID *) (UINTN) ScriptDispatch2.Context);
+
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_MEM_POLL OP code.
+
+  @param  Script  The pointer of typed node in boot script table 
+  @param  AndMask  Mask value for 'and' operation
+  @param  OrMask   Mask value for 'or' operation
+  
+  @retval EFI_DEVICE_ERROR Data polled from memory does not equal to 
+                           the epecting data within the Loop Times.
+  @retval EFI_SUCCESS      The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteMemPoll (
+  IN UINT8                        *Script,
+  IN UINT64                        AndMask,
+  IN UINT64                        OrMask  
+  )
+{
+  
+  UINT64        Data;
+  UINT64        LoopTimes;
+  EFI_STATUS    Status;
+  EFI_BOOT_SCRIPT_MEM_POLL       MemPoll;
+  
+  CopyMem ((VOID*)&MemPoll, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_MEM_POLL));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteMemPoll - 0x%08x, 0x%016lx, 0x%016lx\n", (UINTN)MemPoll.Address, AndMask, OrMask));
+
+  Data = 0;
+  Status = ScriptMemoryRead (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) MemPoll.Width,
+               MemPoll.Address,
+               1,
+               &Data
+               );
+  if ((!EFI_ERROR (Status)) && (Data & AndMask) == OrMask) {
+    return EFI_SUCCESS;
+  }
+
+  for (LoopTimes = 0; LoopTimes < MemPoll.LoopTimes; LoopTimes++) {
+    MicroSecondDelay ((UINTN)MemPoll.Duration);
+
+    Data = 0;
+    Status = ScriptMemoryRead (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) MemPoll.Width,
+               MemPoll.Address,
+               1,
+               &Data
+               );
+   if ((!EFI_ERROR (Status)) && (Data & AndMask) == OrMask) {
+    return EFI_SUCCESS;
+   }
+  }
+
+  if (LoopTimes < MemPoll.LoopTimes) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+}
+/**
+  Execute the boot script to interpret the Store arbitrary information.
+  This opcode is a no-op on dispatch and is only used for debugging script issues.
+
+  @param Script       The pointer of node in boot script table 
+ 
+**/
+VOID
+BootScriptExecuteInformation (
+  IN UINT8       *Script
+  )
+
+{
+  UINT32                        Index;
+  EFI_BOOT_SCRIPT_INFORMATION   Information;
+  UINT8                         *InformationData;
+
+  CopyMem ((VOID*)&Information, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_INFORMATION));
+
+  InformationData = Script + sizeof (EFI_BOOT_SCRIPT_INFORMATION);
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteInformation - 0x%08x\n", (UINTN) InformationData));
+
+  DEBUG ((EFI_D_INFO, "BootScriptInformation: "));
+  for (Index = 0; Index < Information.InformationLength; Index++) {
+    DEBUG ((EFI_D_INFO, "%02x ", InformationData[Index]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+}
+
+/**
+  Execute the boot script to interpret the Label information. 
+
+  @param Script       The pointer of node in boot script table 
+ 
+**/
+VOID
+BootScriptExecuteLabel (
+  IN UINT8       *Script
+  )
+
+{
+  UINT32                        Index;
+  EFI_BOOT_SCRIPT_INFORMATION   Information;
+  UINT8                         *InformationData;
+
+  CopyMem ((VOID*)&Information, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_INFORMATION));
+
+  InformationData = Script + sizeof (EFI_BOOT_SCRIPT_INFORMATION);
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteLabel - 0x%08x\n", (UINTN) InformationData));
+
+  DEBUG ((EFI_D_INFO, "BootScriptLabel: "));
+  for (Index = 0; Index < Information.InformationLength; Index++) {
+    DEBUG ((EFI_D_INFO, "%02x ", InformationData[Index]));
+  }
+  DEBUG ((EFI_D_INFO, "\n"));
+}
+
+/**
+  calculate the mask value for 'and' and 'or' operation
+  @param ScriptHeader   The pointer of header of node in boot script table 
+  @param AndMask  The Mask value for 'and' operation
+  @param OrMask   The Mask value for 'or' operation
+  @param Script   Pointer to the entry.
+
+**/
+VOID
+CheckAndOrMask (
+  IN   EFI_BOOT_SCRIPT_COMMON_HEADER  *ScriptHeader,
+  OUT UINT64                      *AndMask,
+  OUT UINT64                      *OrMask,
+  IN  UINT8                       *Script
+  )
+{
+  UINT8 *DataPtr;
+  UINTN Size;
+
+  switch (ScriptHeader->OpCode) {
+  case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_IO_READ_WRITE);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_MEM_READ_WRITE);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE);
+    break;
+  case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_MEM_POLL);
+    break;
+  
+  case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_IO_POLL);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE);
+    break;
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL);
+    break;
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE:
+    Size = sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL);
+    break;
+  
+  default:
+    return;
+  }
+  
+  DataPtr = Script + Size;
+
+  switch (ScriptHeader->Width) {
+  case S3BootScriptWidthUint8:
+    *AndMask  = (UINT64) (*(UINT8*) (DataPtr + 1));
+    *OrMask   = (UINT64) (*DataPtr);
+    break;
+
+  case S3BootScriptWidthUint16:
+    *AndMask  = (UINT64) (*(UINT16 *) (DataPtr + 2));
+    *OrMask   = (UINT64) (*(UINT16 *) DataPtr);
+    break;
+
+  case S3BootScriptWidthUint32:
+    *AndMask  = (UINT64) (*(UINT32 *) (DataPtr + 4));
+    *OrMask   = (UINT64) (*(UINT32 *) DataPtr);
+    break;
+
+  case S3BootScriptWidthUint64:
+    *AndMask  = (UINT64) (*(UINT64 *) (DataPtr + 8));
+    *OrMask   = (UINT64) (*(UINT64 *) DataPtr);
+    break;
+
+  default:
+    break;
+  }
+
+  return;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_IO_POLL OP code.
+
+  @param  Script  The pointer of typed node in boot script table 
+  @param  AndMask  Mask value for 'and' operation
+  @param  OrMask   Mask value for 'or' operation
+  
+  @retval EFI_DEVICE_ERROR Data polled from memory does not equal to 
+                           the epecting data within the Loop Times.
+  @retval EFI_SUCCESS      The operation was executed successfully
+**/
+EFI_STATUS
+BootScriptExecuteIoPoll (
+  IN UINT8       *Script,
+  IN UINT64      AndMask,
+  IN UINT64      OrMask
+  )
+{
+  EFI_STATUS    Status;
+  UINT64        Data;
+  UINT64        LoopTimes;
+  EFI_BOOT_SCRIPT_IO_POLL       IoPoll;
+  
+  CopyMem ((VOID*)&IoPoll, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_IO_POLL));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecuteIoPoll - 0x%08x, 0x%016lx, 0x%016lx\n", (UINTN)IoPoll.Address, AndMask, OrMask));
+
+  Data = 0;
+  Status = ScriptIoRead (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) IoPoll.Width,
+             IoPoll.Address,
+             1,
+             &Data
+             );
+  if ((!EFI_ERROR (Status)) && (Data & AndMask) == OrMask) {
+    return EFI_SUCCESS;
+  }
+  for (LoopTimes = 0; LoopTimes < IoPoll.Delay; LoopTimes++) {
+    NanoSecondDelay (100);
+    Data = 0;
+    Status = ScriptIoRead (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) IoPoll.Width,
+               IoPoll.Address,
+               1,
+               &Data
+               );
+    if ((!EFI_ERROR (Status)) &&(Data & AndMask) == OrMask) {
+      return EFI_SUCCESS;
+    } 
+  }
+
+  if (LoopTimes < IoPoll.Delay) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE OP code.
+
+  @param    Script              The pointer of S3 boot script
+
+  @retval   EFI_SUCCESS         The operation was executed successfully
+
+**/
+EFI_STATUS
+BootScriptExecutePciCfg2Write (
+  IN UINT8             *Script
+  )
+{
+  VOID                              *Buffer;
+  S3_BOOT_SCRIPT_LIB_WIDTH          Width;
+  UINT16                            Segment;
+  UINT64                            Address;
+  UINTN                             Count;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE PciCfg2Write;
+  
+  CopyMem ((VOID*)&PciCfg2Write, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE));
+
+  Width   = (S3_BOOT_SCRIPT_LIB_WIDTH)PciCfg2Write.Width;
+  Segment = PciCfg2Write.Segment;
+  Address = PciCfg2Write.Address;
+  Count   = PciCfg2Write.Count;
+  Buffer  = Script + sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE);
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecutePciCfg2Write - 0x%04x, 0x%08x, 0x%08x, 0x%08x\n", Segment, PCI_ADDRESS_ENCODE (Address), Count, (UINTN)Width));
+  return ScriptPciCfg2Write (Width, Segment, Address, Count, Buffer);
+}
+
+
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE OP code.
+  
+  @param     Script                     The pointer of S3 boot script
+  @param     AndMask                    Mask value for 'and' operation
+  @param     OrMask                     Mask value for 'or' operation
+
+  @retval    EFI_SUCCESS                The operation was executed successfully
+
+**/
+EFI_STATUS
+BootScriptExecutePciCfg2ReadWrite (
+  IN UINT8                        *Script,
+  IN UINT64                        AndMask,
+  IN UINT64                        OrMask
+  )
+{
+  UINT64      Data;
+  EFI_STATUS  Status;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE PciCfg2ReadWrite;
+
+  Data = 0;
+
+  CopyMem ((VOID*)&PciCfg2ReadWrite, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE));
+
+  DEBUG ((EFI_D_INFO, "BootScriptExecutePciCfg2ReadWrite - 0x%04x, 0x%08x, 0x%016lx, 0x%016lx\n", PciCfg2ReadWrite.Segment, PCI_ADDRESS_ENCODE (PciCfg2ReadWrite.Address), AndMask, OrMask));
+  
+  Status = ScriptPciCfg2Read (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfg2ReadWrite.Width,
+             PciCfg2ReadWrite.Segment,
+             PciCfg2ReadWrite.Address,
+             1,
+             &Data
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Data = (Data & AndMask) | OrMask;
+  Status = ScriptPciCfg2Write (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfg2ReadWrite.Width,
+             PciCfg2ReadWrite.Segment,
+             PciCfg2ReadWrite.Address,
+             1,
+             &Data
+             );
+  return Status;
+}
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CONFIG_POLL OP code.
+  
+  @param     Script                     The pointer of S3 boot script
+  @param     AndMask                    Mask value for 'and' operation
+  @param     OrMask                     Mask value for 'or' operation
+
+  @retval    EFI_SUCCESS                The operation was executed successfully
+  @retval    EFI_DEVICE_ERROR           Data polled from Pci configuration space does not equal to 
+                                        epecting data within the Loop Times.
+**/
+EFI_STATUS
+BootScriptPciCfgPoll (
+  IN UINT8                         *Script,
+  IN UINT64                        AndMask,
+  IN UINT64                        OrMask  
+  )
+{
+  UINT64        Data;
+  UINT64        LoopTimes;
+  EFI_STATUS    Status;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_POLL PciCfgPoll;
+  CopyMem ((VOID*)&PciCfgPoll, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG_POLL));
+
+  DEBUG ((EFI_D_INFO, "BootScriptPciCfgPoll - 0x%08x, 0x%016lx, 0x%016lx\n", PCI_ADDRESS_ENCODE (PciCfgPoll.Address), AndMask, OrMask));
+  
+  Data = 0;
+  Status = ScriptPciCfgRead (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfgPoll.Width,
+             PciCfgPoll.Address,
+             1,
+             &Data
+             );
+  if ((!EFI_ERROR (Status)) &&(Data & AndMask) == OrMask) {
+    return EFI_SUCCESS;
+  }
+
+  for (LoopTimes = 0; LoopTimes < PciCfgPoll.Delay; LoopTimes++) {
+    NanoSecondDelay (100);
+    Data = 0;
+    Status = ScriptPciCfgRead (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfgPoll.Width,
+               PciCfgPoll.Address,
+               1,
+               &Data
+               );
+    if ((!EFI_ERROR (Status)) &&
+       (Data & AndMask) == OrMask) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  if (LoopTimes < PciCfgPoll.Delay) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+}
+
+/**
+  Interprete the boot script node with EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL OP code.
+  
+  @param     Script                     The pointer of S3 Boot Script
+  @param     AndMask                    Mask value for 'and' operation
+  @param     OrMask                     Mask value for 'or' operation
+
+  @retval    EFI_SUCCESS                The operation was executed successfully
+  @retval    EFI_DEVICE_ERROR           Data polled from Pci configuration space does not equal to 
+                                        epecting data within the Loop Times.
+
+**/
+EFI_STATUS
+BootScriptPciCfg2Poll (
+  IN UINT8                        *Script,
+  IN UINT64                        AndMask,
+  IN UINT64                        OrMask  
+  )
+{
+  EFI_STATUS    Status;
+  UINT64        Data;
+  UINT64        LoopTimes;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL PciCfg2Poll;
+
+  Data = 0;
+  CopyMem ((VOID*)&PciCfg2Poll, (VOID*)Script, sizeof(EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL));
+
+  DEBUG ((EFI_D_INFO, "BootScriptPciCfg2Poll - 0x%04x, 0x%08x, 0x%016lx, 0x%016lx\n", PciCfg2Poll.Segment, PCI_ADDRESS_ENCODE (PciCfg2Poll.Address), AndMask, OrMask));
+ 
+  Status = ScriptPciCfg2Read (
+             (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfg2Poll.Width,
+             PciCfg2Poll.Segment,
+             PciCfg2Poll.Address,
+             1,
+             &Data
+             );
+  if ((!EFI_ERROR (Status)) && (Data & AndMask) == OrMask) {
+    return EFI_SUCCESS;
+  }
+
+  for (LoopTimes = 0; LoopTimes < PciCfg2Poll.Delay; LoopTimes++) {
+    NanoSecondDelay (100);
+
+    Data = 0;
+    Status = ScriptPciCfg2Read (
+               (S3_BOOT_SCRIPT_LIB_WIDTH) PciCfg2Poll.Width,
+               PciCfg2Poll.Segment,               
+               PciCfg2Poll.Address,
+               1,
+               &Data
+               );
+    if ((!EFI_ERROR (Status)) &&  (Data & AndMask) == OrMask) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  if (LoopTimes < PciCfg2Poll.Delay) {
+    return EFI_SUCCESS;
+  } else {
+    return EFI_DEVICE_ERROR;
+  }
+  
+}
+
+/**
+  Executes the S3 boot script table.
+ 
+  @retval RETURN_SUCCESS           The boot script table was executed successfully.
+  @retval RETURN_UNSUPPORTED       Invalid script table or opcode.  
+  
+  @note  A known Limitations in the implementation: When interpreting the opcode  EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE
+         EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE and EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE, the 'Segment' parameter is assumed as 
+         Zero, or else, assert.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptExecute (
+  VOID
+  )
+{
+  EFI_STATUS            Status;
+  UINT8*                Script;
+  UINTN                 StartAddress;
+  UINT32                TableLength;
+  UINT64                AndMask;
+  UINT64                OrMask;
+  EFI_BOOT_SCRIPT_COMMON_HEADER  ScriptHeader;
+  EFI_BOOT_SCRIPT_TABLE_HEADER   TableHeader;
+  Script = mS3BootScriptTablePtr->TableBase;
+  if (Script != 0) {    
+    CopyMem ((VOID*)&TableHeader, Script, sizeof(EFI_BOOT_SCRIPT_TABLE_HEADER));
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_INFO, "S3BootScriptExecute:\n"));
+  if (TableHeader.OpCode != S3_BOOT_SCRIPT_LIB_TABLE_OPCODE) {
+    return EFI_UNSUPPORTED;
+  }
+
+  DEBUG ((EFI_D_INFO, "TableHeader - 0x%08x\n", Script));
+  
+  StartAddress  = (UINTN) Script;
+  TableLength   = TableHeader.TableLength;
+  Script    =    Script + TableHeader.Length;
+  Status        = EFI_SUCCESS;
+  AndMask       = 0;
+  OrMask        = 0;
+
+  DEBUG ((EFI_D_INFO, "TableHeader.Version - 0x%04x\n", (UINTN)TableHeader.Version));
+  DEBUG ((EFI_D_INFO, "TableHeader.TableLength - 0x%08x\n", (UINTN)TableLength));
+
+  while ((UINTN) Script < (UINTN) (StartAddress + TableLength)) {
+    DEBUG ((EFI_D_INFO, "ExecuteBootScript - %08x\n", (UINTN)Script));
+    
+    CopyMem ((VOID*)&ScriptHeader, Script, sizeof(EFI_BOOT_SCRIPT_COMMON_HEADER));
+    switch (ScriptHeader.OpCode) {
+
+    case EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE\n"));
+      Status = BootScriptExecuteMemoryWrite (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecuteMemoryReadWrite (
+                 Script,
+                 AndMask,
+                 OrMask
+                 );
+      break;
+
+    case EFI_BOOT_SCRIPT_IO_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_IO_WRITE_OPCODE\n"));
+      Status = BootScriptExecuteIoWrite (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE\n"));
+      Status = BootScriptExecutePciCfgWrite (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecutePciCfgReadWrite (
+                 Script,
+                 AndMask,
+                 OrMask
+                 );
+      break;
+    case EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE\n"));
+      Status = BootScriptExecutePciCfg2Write (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecutePciCfg2ReadWrite (
+                 Script,
+                 AndMask,
+                 OrMask
+                 );
+      break;
+    case EFI_BOOT_SCRIPT_DISPATCH_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_DISPATCH_OPCODE\n"));
+      Status = BootScriptExecuteDispatch (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE\n"));
+      Status = BootScriptExecuteDispatch2 (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_INFORMATION_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_INFORMATION_OPCODE\n"));
+      BootScriptExecuteInformation (Script);
+      break;    
+
+    case S3_BOOT_SCRIPT_LIB_TERMINATE_OPCODE:
+      DEBUG ((EFI_D_INFO, "S3_BOOT_SCRIPT_LIB_TERMINATE_OPCODE\n"));
+      DEBUG ((EFI_D_INFO, "S3BootScriptDone - %r\n", EFI_SUCCESS));
+      return EFI_SUCCESS;
+
+    case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecuteIoReadWrite (
+                Script,
+                AndMask,
+                OrMask
+                );
+      break;
+
+    case EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE\n"));
+      Status = BootScriptExecuteSmbusExecute (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_STALL_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_STALL_OPCODE\n"));
+      Status = BootScriptExecuteStall (Script);
+      break;
+
+    case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_MEM_POLL_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecuteMemPoll (Script, AndMask, OrMask);
+      
+      break;
+    
+    case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_IO_POLL_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptExecuteIoPoll (Script, AndMask, OrMask);
+      break;
+      
+    case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE:
+      DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE\n"));
+      CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+      Status = BootScriptPciCfgPoll (Script, AndMask, OrMask);
+      break;
+      
+    case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+     DEBUG ((EFI_D_INFO, "EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE\n"));
+     CheckAndOrMask (&ScriptHeader, &AndMask, &OrMask, Script);
+     Status = BootScriptPciCfg2Poll (Script, AndMask, OrMask);
+     break;
+
+    case S3_BOOT_SCRIPT_LIB_LABEL_OPCODE:
+      //
+      // For label
+      //
+      DEBUG ((EFI_D_INFO, "S3_BOOT_SCRIPT_LIB_LABEL_OPCODE\n"));
+      BootScriptExecuteLabel (Script);
+      break;
+    default:
+      DEBUG ((EFI_D_INFO, "S3BootScriptDone - %r\n", EFI_UNSUPPORTED));
+      return EFI_UNSUPPORTED;
+    }
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "S3BootScriptDone - %r\n", Status));
+      return Status;
+    }
+
+    Script  = Script + ScriptHeader.Length;
+  }
+
+  DEBUG ((EFI_D_INFO, "S3BootScriptDone - %r\n", Status));
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptInternalFormat.h b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptInternalFormat.h
new file mode 100644
index 0000000000..99cb407dbd
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptInternalFormat.h
@@ -0,0 +1,188 @@
+/** @file
+  This file declares the internal Framework Boot Script format used by
+  the PI implementation of Script Saver and Executor.
+
+  Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _BOOT_SCRIPT_INTERNAL_FORMAT_H_
+#define _BOOT_SCRIPT_INTERNAL_FORMAT_H_
+
+#pragma pack(1)
+
+//
+// Boot Script Opcode Header Structure Definitions
+//
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+} EFI_BOOT_SCRIPT_GENERIC_HEADER;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT16  Version;
+  UINT32  TableLength;
+  UINT16  Reserved[2];
+} EFI_BOOT_SCRIPT_TABLE_HEADER;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+} EFI_BOOT_SCRIPT_COMMON_HEADER;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT32                Count;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_IO_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_IO_READ_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT32                Count;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_MEM_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_MEM_READ_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT32                Count;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT32                Count;
+  UINT64                Address;
+  UINT16                Segment;
+} EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT64                Address;
+} EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  UINT32                Width;
+  UINT64                Address;
+  UINT16                Segment;
+} EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE;
+
+typedef struct {
+  UINT16                    OpCode;
+  UINT8                     Length;
+  UINT64                    SmBusAddress;
+  UINT32                    Operation;
+  UINT32                    DataSize;
+} EFI_BOOT_SCRIPT_SMBUS_EXECUTE;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT64  Duration;
+} EFI_BOOT_SCRIPT_STALL;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  EFI_PHYSICAL_ADDRESS  EntryPoint;
+} EFI_BOOT_SCRIPT_DISPATCH;
+
+typedef struct {
+  UINT16                OpCode;
+  UINT8                 Length;
+  EFI_PHYSICAL_ADDRESS  EntryPoint;
+  EFI_PHYSICAL_ADDRESS  Context;
+} EFI_BOOT_SCRIPT_DISPATCH_2;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT32  Width;
+  UINT64  Address;
+  UINT64  Duration;
+  UINT64  LoopTimes;
+} EFI_BOOT_SCRIPT_MEM_POLL;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT32  InformationLength;
+// UINT8   InformationData[InformationLength];
+} EFI_BOOT_SCRIPT_INFORMATION;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT32  Width;
+  UINT64  Address;
+  UINT64  Delay;
+} EFI_BOOT_SCRIPT_IO_POLL;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT32  Width;
+  UINT64  Address;
+  UINT64  Delay;
+} EFI_BOOT_SCRIPT_PCI_CONFIG_POLL;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+  UINT32  Width;
+  UINT64  Address;
+  UINT16  Segment;  
+  UINT64  Delay;
+} EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL;
+
+typedef struct {
+  UINT16  OpCode;
+  UINT8   Length;
+} EFI_BOOT_SCRIPT_TERMINATE;
+
+
+#pragma pack()
+
+#define BOOT_SCRIPT_NODE_MAX_LENGTH   1024
+
+#define BOOT_SCRIPT_TABLE_VERSION     0x0001
+
+#endif
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptSave.c b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptSave.c
new file mode 100644
index 0000000000..0459bae4c6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/BootScriptSave.c
@@ -0,0 +1,2358 @@
+/** @file
+  Save the S3 data to S3 boot script.
+
+  Copyright (c) 2006 - 2016, 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 "InternalBootScriptLib.h"
+
+/**
+
+  Data structure usage:
+
+  +------------------------------+<------- PcdS3BootScriptTablePrivateDataPtr
+  | SCRIPT_TABLE_PRIVATE_DATA    |          (mS3BootScriptTablePtr, Before SmmReadyToLock)
+  |    TableBase                 |---      PcdS3BootScriptTablePrivateSmmDataPtr
+  |    TableLength               |--|--     (mS3BootScriptTablePtr = mS3BootScriptTableSmmPtr, After SmmReadyToLock InSmm)
+  |    TableMemoryPageNumber     |--|-|----
+  |    AtRuntime                 |  | |   |
+  |    InSmm                     |  | |   |
+  |    BootTimeScriptLength      |--|-|---|---
+  |    SmmLocked                 |  | |   |  |
+  |    BackFromS3                |  | |   |  |
+  +------------------------------+  | |   |  |
+                                    | |   |  |
+  +------------------------------+<-- |   |  |
+  | EFI_BOOT_SCRIPT_TABLE_HEADER |    |   |  |
+  |    TableLength               |----|-- |  |
+  +------------------------------+    | | |  |
+  |     ......                   |    | | |  |
+  +------------------------------+<---- | |  |
+  | EFI_BOOT_SCRIPT_TERMINATE    |      | |  |
+  +------------------------------+<------ |  |
+                                          |  |
+                                          |  |
+  mBootScriptDataBootTimeGuid LockBox:    |  |
+   Used to restore data after back from S3|  |
+   to handle potential INSERT boot script |  |
+   at runtime.                            |  |
+  +------------------------------+        |  |
+  | Boot Time Boot Script        |        |  |
+  | Before SmmReadyToLock        |        |  |
+  |                              |        |  |
+  |                              |        |  |
+  +------------------------------+        |  |
+  | Boot Time Boot Script        |        |  |
+  | After SmmReadyToLock InSmm   |        |  |
+  |                              |        |  |
+  +------------------------------+<-------|--|
+                                          |  |
+                                          |  |
+  mBootScriptDataGuid LockBox: (IN_PLACE) |  |
+   Used to restore data at S3 resume.     |  |
+  +------------------------------+        |  |
+  | Boot Time Boot Script        |        |  |
+  | Before SmmReadyToLock        |        |  |
+  |                              |        |  |
+  |                              |        |  |
+  +------------------------------+        |  |
+  | Boot Time Boot Script        |        |  |
+  | After SmmReadyToLock InSmm   |        |  |
+  |                              |        |  |
+  +------------------------------+<-------|---
+  | Runtime Boot Script          |        |
+  | After SmmReadyToLock InSmm   |        |
+  +------------------------------+        |
+  |     ......                   |        |
+  +------------------------------+<--------
+
+
+  mBootScriptTableBaseGuid LockBox: (IN_PLACE)
+  +------------------------------+
+  | mS3BootScriptTablePtr->      |
+  |  TableBase                   |
+  +------------------------------+
+
+
+  mBootScriptSmmPrivateDataGuid LockBox: (IN_PLACE)
+   SMM private data with BackFromS3 = TRUE
+   at runtime. S3 will help restore it to
+   tell the Library the system is back from S3.
+  +------------------------------+
+  | SCRIPT_TABLE_PRIVATE_DATA    |
+  |    TableBase                 |
+  |    TableLength               |
+  |    TableMemoryPageNumber     |
+  |    AtRuntime                 |
+  |    InSmm                     |
+  |    BootTimeScriptLength      |
+  |    SmmLocked                 |
+  |    BackFromS3 = TRUE         |
+  +------------------------------+
+
+**/
+
+SCRIPT_TABLE_PRIVATE_DATA        *mS3BootScriptTablePtr;
+
+//
+// Allocate SMM copy because we can not use mS3BootScriptTablePtr after SmmReadyToLock in InSmm.
+//
+SCRIPT_TABLE_PRIVATE_DATA        *mS3BootScriptTableSmmPtr;
+
+EFI_GUID                         mBootScriptDataGuid = {
+  0xaea6b965, 0xdcf5, 0x4311, { 0xb4, 0xb8, 0xf, 0x12, 0x46, 0x44, 0x94, 0xd2 }
+};
+
+EFI_GUID                         mBootScriptDataBootTimeGuid = {
+  0xb5af1d7a, 0xb8cf, 0x4eb3, { 0x89, 0x25, 0xa8, 0x20, 0xe1, 0x6b, 0x68, 0x7d }
+};
+
+EFI_GUID                         mBootScriptTableBaseGuid = {
+  0x1810ab4a, 0x2314, 0x4df6, { 0x81, 0xeb, 0x67, 0xc6, 0xec, 0x5, 0x85, 0x91 }
+};
+
+EFI_GUID                         mBootScriptSmmPrivateDataGuid = {
+  0x627ee2da, 0x3bf9, 0x439b, { 0x92, 0x9f, 0x2e, 0xe, 0x6e, 0x9d, 0xba, 0x62 }
+};
+
+EFI_EVENT                        mEventDxeSmmReadyToLock = NULL;
+VOID                             *mRegistrationSmmExitBootServices = NULL;
+VOID                             *mRegistrationSmmLegacyBoot = NULL;
+VOID                             *mRegistrationSmmReadyToLock = NULL;
+BOOLEAN                          mS3BootScriptTableAllocated = FALSE;
+BOOLEAN                          mS3BootScriptTableSmmAllocated = FALSE;
+EFI_SMM_SYSTEM_TABLE2            *mBootScriptSmst = NULL;
+
+/**
+  This is an internal function to add a terminate node the entry, recalculate the table 
+  length and fill into the table. 
+  
+  @return the base address of the boot script table.   
+ **/
+UINT8*
+S3BootScriptInternalCloseTable (
+  VOID
+  )
+{
+  UINT8                          *S3TableBase;
+  EFI_BOOT_SCRIPT_TERMINATE      ScriptTerminate;
+  EFI_BOOT_SCRIPT_TABLE_HEADER   *ScriptTableInfo;
+  S3TableBase = mS3BootScriptTablePtr->TableBase;
+  
+  if (S3TableBase == NULL) {
+    //
+    // the table is not exist
+    //
+    return S3TableBase;
+  }
+  //
+  // Append the termination entry.
+  //
+  ScriptTerminate.OpCode  = S3_BOOT_SCRIPT_LIB_TERMINATE_OPCODE;
+  ScriptTerminate.Length  = (UINT8) sizeof (EFI_BOOT_SCRIPT_TERMINATE);
+  CopyMem (mS3BootScriptTablePtr->TableBase + mS3BootScriptTablePtr->TableLength, &ScriptTerminate, sizeof (EFI_BOOT_SCRIPT_TERMINATE));
+  //
+  // fill the table length
+  //
+  ScriptTableInfo                = (EFI_BOOT_SCRIPT_TABLE_HEADER*)(mS3BootScriptTablePtr->TableBase);
+  ScriptTableInfo->TableLength = mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE);
+  
+ 
+  
+  return S3TableBase;
+  //
+  // NOTE: Here we did NOT adjust the mS3BootScriptTablePtr->TableLength to 
+  // mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE).
+  // Because maybe after SmmReadyToLock, we still need add entries into the table,
+  // and the entry should be added start before this TERMINATE node.
+  //
+}  
+
+/**
+  This function save boot script data to LockBox.
+
+**/
+VOID
+SaveBootScriptDataToLockBox (
+  VOID
+  )
+{
+  EFI_STATUS            Status;
+
+  //
+  // Save whole memory copy into LockBox.
+  // It will be used to restore data at S3 resume.
+  //
+  Status = SaveLockBox (
+             &mBootScriptDataGuid,
+             (VOID *)mS3BootScriptTablePtr->TableBase,
+             EFI_PAGES_TO_SIZE (mS3BootScriptTablePtr->TableMemoryPageNumber)
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SetLockBoxAttributes (&mBootScriptDataGuid, LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Just need save TableBase.
+  // Do not update other field because they will NOT be used in S3.
+  //
+  Status = SaveLockBox (
+             &mBootScriptTableBaseGuid,
+             (VOID *)&mS3BootScriptTablePtr->TableBase,
+             sizeof(mS3BootScriptTablePtr->TableBase)
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SetLockBoxAttributes (&mBootScriptTableBaseGuid, LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  This is the Event call back function to notify the Library the system is entering
+  SmmLocked phase.
+  
+  @param  Event   Pointer to this event
+  @param  Context Event handler private data 
+ **/
+VOID
+EFIAPI
+S3BootScriptEventCallBack (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS   Status;
+  VOID         *Interface;
+
+  //
+  // Try to locate it because EfiCreateProtocolNotifyEvent will trigger it once when registration.
+  // Just return if it is not found.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiDxeSmmReadyToLockProtocolGuid,
+                  NULL,
+                  &Interface
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Here we should tell the library that we are entering SmmLocked phase.
+  // and the memory page number occupied by the table should not grow anymore.
+  //
+  if (!mS3BootScriptTablePtr->SmmLocked) {
+    //
+    // Before SmmReadyToLock, we need not write the terminate node when adding a node to boot scipt table
+    // or else, that will impact the performance. However, after SmmReadyToLock, we should append terminate
+    // node on every add to boot script table.
+    //
+    S3BootScriptInternalCloseTable ();
+    mS3BootScriptTablePtr->SmmLocked = TRUE;
+
+    //
+    // Save BootScript data to lockbox
+    //
+    SaveBootScriptDataToLockBox ();
+  }
+}
+
+/**
+  This is the Event call back function is triggered in SMM to notify the Library
+  the system is entering SmmLocked phase and set InSmm flag.
+
+  @param  Protocol   Points to the protocol's unique identifier
+  @param  Interface  Points to the interface instance
+  @param  Handle     The handle on which the interface was installed
+
+  @retval EFI_SUCCESS SmmEventCallback runs successfully
+ **/
+EFI_STATUS
+EFIAPI
+S3BootScriptSmmEventCallBack (
+  IN CONST EFI_GUID  *Protocol,
+  IN VOID            *Interface,
+  IN EFI_HANDLE      Handle
+  )
+{
+  //
+  // Check if it is already done
+  //
+  if (mS3BootScriptTablePtr == mS3BootScriptTableSmmPtr) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Last chance to call-out, just make sure SmmLocked is set.
+  //
+  S3BootScriptEventCallBack (NULL, NULL);
+
+  //
+  // Save a SMM copy. If TableBase is NOT null, it means SMM copy has been ready, skip copy mem.
+  //
+  if (mS3BootScriptTableSmmPtr->TableBase == NULL) {
+    CopyMem (mS3BootScriptTableSmmPtr, mS3BootScriptTablePtr, sizeof(*mS3BootScriptTablePtr));
+
+    //
+    // Set InSmm, we allow boot script update when InSmm, but not allow boot script outside SMM.
+    // InSmm will only be checked if SmmLocked is TRUE.
+    //
+    mS3BootScriptTableSmmPtr->InSmm = TRUE;
+  }
+  //
+  // We should not use ACPI Reserved copy, because it is not safe.
+  //
+  mS3BootScriptTablePtr = mS3BootScriptTableSmmPtr;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is to save boot time boot script data to LockBox.
+
+  Because there may be INSERT boot script at runtime in SMM.
+  The boot time copy will be used to restore data after back from S3.
+  Otherwise the data inserted may cause some boot time boot script data lost
+  if only BootScriptData used.
+
+**/
+VOID
+SaveBootTimeDataToLockBox (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+
+  //
+  // ACPI Reserved copy is not safe, restore from BootScriptData LockBox first,
+  // and then save the data to BootScriptDataBootTime LockBox.
+  //
+  Status = RestoreLockBox (
+             &mBootScriptDataGuid,
+             NULL,
+             NULL
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Save BootScriptDataBootTime
+  // It will be used to restore data after back from S3.
+  //
+  Status = SaveLockBox (
+             &mBootScriptDataBootTimeGuid,
+             (VOID *) mS3BootScriptTablePtr->TableBase,
+             mS3BootScriptTablePtr->BootTimeScriptLength
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  This function save boot script SMM private data to LockBox with BackFromS3 = TRUE at runtime.
+  S3 resume will help restore it to tell the Library the system is back from S3.
+
+**/
+VOID
+SaveSmmPriviateDataToLockBoxAtRuntime (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+
+  //
+  // Save boot script SMM private data with BackFromS3 = TRUE.
+  //
+  mS3BootScriptTablePtr->BackFromS3 = TRUE;
+  Status = SaveLockBox (
+             &mBootScriptSmmPrivateDataGuid,
+             (VOID *) mS3BootScriptTablePtr,
+             sizeof (SCRIPT_TABLE_PRIVATE_DATA)
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SetLockBoxAttributes (&mBootScriptSmmPrivateDataGuid, LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Set BackFromS3 flag back to FALSE to indicate that now is not back from S3.
+  //
+  mS3BootScriptTablePtr->BackFromS3 = FALSE;
+}
+
+/**
+  This is the Event call back function is triggered in SMM to notify the Library
+  the system is entering runtime phase.
+
+  @param[in] Protocol   Points to the protocol's unique identifier
+  @param[in] Interface  Points to the interface instance
+  @param[in] Handle     The handle on which the interface was installed
+
+  @retval EFI_SUCCESS SmmAtRuntimeCallBack runs successfully
+ **/
+EFI_STATUS
+EFIAPI
+S3BootScriptSmmAtRuntimeCallBack (
+  IN CONST EFI_GUID     *Protocol,
+  IN VOID               *Interface,
+  IN EFI_HANDLE         Handle
+  )
+{
+  if (!mS3BootScriptTablePtr->AtRuntime) {
+    mS3BootScriptTablePtr->BootTimeScriptLength = (UINT32) (mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE));
+    SaveBootTimeDataToLockBox ();
+
+    mS3BootScriptTablePtr->AtRuntime = TRUE;
+    SaveSmmPriviateDataToLockBoxAtRuntime ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Library Constructor.
+  this function just identify it is a smm driver or non-smm driver linked against 
+  with the library   
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval RETURN_SUCCESS    The constructor always returns RETURN_SUCCESS.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptLibInitialize (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS                      Status;
+  SCRIPT_TABLE_PRIVATE_DATA      *S3TablePtr;
+  SCRIPT_TABLE_PRIVATE_DATA      *S3TableSmmPtr;
+  VOID                           *Registration;
+  EFI_SMM_BASE2_PROTOCOL         *SmmBase2;
+  BOOLEAN                        InSmm;
+  EFI_PHYSICAL_ADDRESS           Buffer;
+
+  S3TablePtr = (SCRIPT_TABLE_PRIVATE_DATA*)(UINTN)PcdGet64(PcdS3BootScriptTablePrivateDataPtr);
+  //
+  // The Boot script private data is not be initialized. create it
+  //
+  if (S3TablePtr == 0) {
+    Buffer = SIZE_4GB - 1;
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiReservedMemoryType,
+                    EFI_SIZE_TO_PAGES(sizeof(SCRIPT_TABLE_PRIVATE_DATA)),
+                    &Buffer
+                    );
+    ASSERT_EFI_ERROR (Status);
+    mS3BootScriptTableAllocated = TRUE;
+    S3TablePtr = (VOID *) (UINTN) Buffer;
+
+    Status = PcdSet64S (PcdS3BootScriptTablePrivateDataPtr, (UINT64) (UINTN)S3TablePtr);
+    ASSERT_EFI_ERROR (Status);
+    ZeroMem (S3TablePtr, sizeof(SCRIPT_TABLE_PRIVATE_DATA));
+    //
+    // Create event to notify the library system enter the SmmLocked phase.
+    //
+    mEventDxeSmmReadyToLock = EfiCreateProtocolNotifyEvent (
+                                &gEfiDxeSmmReadyToLockProtocolGuid,
+                                TPL_CALLBACK,
+                                S3BootScriptEventCallBack,
+                                NULL,
+                                &Registration
+                                );
+    ASSERT (mEventDxeSmmReadyToLock != NULL);
+  }
+  mS3BootScriptTablePtr = S3TablePtr;
+
+  //
+  // Get InSmm, we need to register SmmReadyToLock if this library is linked to SMM driver.
+  //
+  Status = gBS->LocateProtocol (&gEfiSmmBase2ProtocolGuid, NULL, (VOID**) &SmmBase2);
+  if (EFI_ERROR (Status)) {
+    return RETURN_SUCCESS;
+  }
+  Status = SmmBase2->InSmm (SmmBase2, &InSmm);
+  if (EFI_ERROR (Status)) {
+    return RETURN_SUCCESS;
+  }
+  if (!InSmm) {
+    return RETURN_SUCCESS;
+  }
+  //
+  // Good, we are in SMM
+  //
+  Status = SmmBase2->GetSmstLocation (SmmBase2, &mBootScriptSmst);
+  if (EFI_ERROR (Status)) {
+    return RETURN_SUCCESS;
+  }
+
+  S3TableSmmPtr = (SCRIPT_TABLE_PRIVATE_DATA*)(UINTN)PcdGet64(PcdS3BootScriptTablePrivateSmmDataPtr);
+  //
+  // The Boot script private data in SMM is not be initialized. create it
+  //
+  if (S3TableSmmPtr == 0) {
+    Status = mBootScriptSmst->SmmAllocatePool (
+                                EfiRuntimeServicesData,
+                                sizeof(SCRIPT_TABLE_PRIVATE_DATA),
+                                (VOID **) &S3TableSmmPtr
+                                );
+    ASSERT_EFI_ERROR (Status);
+    mS3BootScriptTableSmmAllocated = TRUE;
+
+    Status = PcdSet64S (PcdS3BootScriptTablePrivateSmmDataPtr, (UINT64) (UINTN)S3TableSmmPtr);
+    ASSERT_EFI_ERROR (Status);
+    ZeroMem (S3TableSmmPtr, sizeof(SCRIPT_TABLE_PRIVATE_DATA));
+
+    //
+    // Register SmmExitBootServices and SmmLegacyBoot notification.
+    //
+    Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                                &gEdkiiSmmExitBootServicesProtocolGuid,
+                                S3BootScriptSmmAtRuntimeCallBack,
+                                &mRegistrationSmmExitBootServices
+                                );
+    ASSERT_EFI_ERROR (Status);
+
+    Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                                &gEdkiiSmmLegacyBootProtocolGuid,
+                                S3BootScriptSmmAtRuntimeCallBack,
+                                &mRegistrationSmmLegacyBoot
+                                );
+    ASSERT_EFI_ERROR (Status);
+  }
+  mS3BootScriptTableSmmPtr = S3TableSmmPtr;
+
+  //
+  // Register SmmReadyToLock notification.
+  //
+  Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                              &gEfiSmmReadyToLockProtocolGuid,
+                              S3BootScriptSmmEventCallBack,
+                              &mRegistrationSmmReadyToLock
+                              );
+  ASSERT_EFI_ERROR (Status);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Library Destructor to free the resources allocated by
+  S3BootScriptLibInitialize() and unregister callbacks.
+
+  NOTICE: The destructor doesn't support unloading as a separate action, and it
+  only supports unloading if the containing driver's entry point function fails.
+
+  @param ImageHandle        The firmware allocated handle for the EFI image.
+  @param SystemTable        A pointer to the EFI System Table.
+
+  @retval RETURN_SUCCESS    The destructor always returns RETURN_SUCCESS.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptLibDeinitialize (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+
+  DEBUG ((EFI_D_INFO, "%a() in %a module\n", __FUNCTION__, gEfiCallerBaseName));
+
+  if (mEventDxeSmmReadyToLock != NULL) {
+    //
+    // Close the DxeSmmReadyToLock event.
+    //
+    Status = gBS->CloseEvent (mEventDxeSmmReadyToLock);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (mBootScriptSmst != NULL) {
+    if (mRegistrationSmmExitBootServices != NULL) {
+      //
+      // Unregister SmmExitBootServices notification.
+      //
+      Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                                  &gEdkiiSmmExitBootServicesProtocolGuid,
+                                  NULL,
+                                  &mRegistrationSmmExitBootServices
+                                  );
+      ASSERT_EFI_ERROR (Status);
+    }
+    if (mRegistrationSmmLegacyBoot != NULL) {
+      //
+      // Unregister SmmLegacyBoot notification.
+      //
+      Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                                  &gEdkiiSmmLegacyBootProtocolGuid,
+                                  NULL,
+                                  &mRegistrationSmmLegacyBoot
+                                  );
+      ASSERT_EFI_ERROR (Status);
+    }
+    if (mRegistrationSmmReadyToLock != NULL) {
+      //
+      // Unregister SmmReadyToLock notification.
+      //
+      Status = mBootScriptSmst->SmmRegisterProtocolNotify (
+                                  &gEfiSmmReadyToLockProtocolGuid,
+                                  NULL,
+                                  &mRegistrationSmmReadyToLock
+                                  );
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  //
+  // Free the resources allocated and set PCDs to 0.
+  //
+  if (mS3BootScriptTableAllocated) {
+    Status = gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) mS3BootScriptTablePtr, EFI_SIZE_TO_PAGES(sizeof(SCRIPT_TABLE_PRIVATE_DATA)));
+    ASSERT_EFI_ERROR (Status);
+    Status = PcdSet64S (PcdS3BootScriptTablePrivateDataPtr, 0);
+    ASSERT_EFI_ERROR (Status);
+  }
+  if ((mBootScriptSmst != NULL) && mS3BootScriptTableSmmAllocated) {
+    Status = mBootScriptSmst->SmmFreePool (mS3BootScriptTableSmmPtr);
+    ASSERT_EFI_ERROR (Status);
+    Status = PcdSet64S (PcdS3BootScriptTablePrivateSmmDataPtr, 0);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  To get the start address from which a new boot time s3 boot script entry will write into.
+  If the table is not exist, the functio will first allocate a buffer for the table
+  If the table buffer is not enough for the new entry, in non-smm mode, the funtion will 
+  invoke reallocate to enlarge buffer.
+  
+  @param EntryLength      the new entry length.
+  
+  @retval the address from which the a new s3 boot script entry will write into 
+ **/
+UINT8*
+S3BootScriptGetBootTimeEntryAddAddress (
+  UINT8  EntryLength
+  )
+{
+   EFI_PHYSICAL_ADDRESS              S3TableBase;
+   EFI_PHYSICAL_ADDRESS              NewS3TableBase;
+   UINT8                            *NewEntryPtr;
+   UINT32                            TableLength;
+   UINT16                            PageNumber;
+   EFI_STATUS                        Status;
+   EFI_BOOT_SCRIPT_TABLE_HEADER      *ScriptTableInfo;
+   
+   S3TableBase = (EFI_PHYSICAL_ADDRESS)(UINTN)(mS3BootScriptTablePtr->TableBase);
+   if (S3TableBase == 0) {
+     //
+     // The table is not exist. This is the first to add entry. 
+     // Allocate ACPI script table space under 4G memory.
+     //
+     S3TableBase = 0xffffffff;
+     Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  2 + PcdGet16(PcdS3BootScriptRuntimeTableReservePageNumber),
+                  (EFI_PHYSICAL_ADDRESS*)&S3TableBase
+                  );
+     
+     if (EFI_ERROR(Status)) {
+       ASSERT_EFI_ERROR (Status);
+       return 0;
+     }
+     //
+     // Fill Table Header
+     //
+     ScriptTableInfo              = (EFI_BOOT_SCRIPT_TABLE_HEADER*)(UINTN)S3TableBase;
+     ScriptTableInfo->OpCode      = S3_BOOT_SCRIPT_LIB_TABLE_OPCODE;
+     ScriptTableInfo->Length      = (UINT8) sizeof (EFI_BOOT_SCRIPT_TABLE_HEADER);
+     ScriptTableInfo->Version     = BOOT_SCRIPT_TABLE_VERSION;
+     ScriptTableInfo->TableLength = 0;   // will be calculate at CloseTable
+     mS3BootScriptTablePtr->TableLength = sizeof (EFI_BOOT_SCRIPT_TABLE_HEADER);
+     mS3BootScriptTablePtr->TableBase = (UINT8*)(UINTN)S3TableBase;
+     mS3BootScriptTablePtr->TableMemoryPageNumber = (UINT16)(2 + PcdGet16(PcdS3BootScriptRuntimeTableReservePageNumber));
+   }
+     
+   // Here we do not count the reserved memory for runtime script table.
+   PageNumber = (UINT16) (mS3BootScriptTablePtr->TableMemoryPageNumber - PcdGet16(PcdS3BootScriptRuntimeTableReservePageNumber));
+   TableLength =  mS3BootScriptTablePtr->TableLength;
+   if ((UINTN) EFI_PAGES_TO_SIZE ((UINTN) PageNumber) < (UINTN) (TableLength + EntryLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE))) {
+     // 
+     // The buffer is too small to hold the table, Reallocate the buffer
+     //
+     NewS3TableBase = 0xffffffff;
+     Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  2 + PageNumber + PcdGet16(PcdS3BootScriptRuntimeTableReservePageNumber),
+                  (EFI_PHYSICAL_ADDRESS*)&NewS3TableBase
+                  );
+   
+     if (EFI_ERROR(Status)) {
+       ASSERT_EFI_ERROR (Status);
+       return 0;
+     }
+     
+     CopyMem ((VOID*)(UINTN)NewS3TableBase, (VOID*)(UINTN)S3TableBase, TableLength);
+     gBS->FreePages (S3TableBase, mS3BootScriptTablePtr->TableMemoryPageNumber);
+         
+     mS3BootScriptTablePtr->TableBase = (UINT8*)(UINTN)NewS3TableBase;
+     mS3BootScriptTablePtr->TableMemoryPageNumber =  (UINT16) (2 + PageNumber + PcdGet16(PcdS3BootScriptRuntimeTableReservePageNumber)); 
+   }
+   //
+   // calculate the the start address for the new entry. 
+   //
+   NewEntryPtr = mS3BootScriptTablePtr->TableBase + TableLength;
+   
+   //
+   // update the table lenghth
+   //
+   mS3BootScriptTablePtr->TableLength =  TableLength + EntryLength;
+   
+   //
+   // In the boot time, we will not append the termination entry to the boot script
+   // table until the callers think there is no boot time data that should be added and 
+   // it is caller's responsibility to explicit call the CloseTable. 
+   //
+   //
+  
+   return NewEntryPtr;    
+}
+/**
+  To get the start address from which a new runtime(after SmmReadyToLock) s3 boot script entry will write into.
+  In this case, it should be ensured that there is enough buffer to hold the entry.
+  
+  @param EntryLength      the new entry length.
+  
+  @retval the address from which the a new s3 runtime(after SmmReadyToLock) script entry will write into
+ **/
+UINT8*
+S3BootScriptGetRuntimeEntryAddAddress (
+  UINT8  EntryLength
+  )
+{
+   UINT8     *NewEntryPtr;
+   
+   NewEntryPtr = NULL;   
+   //
+   // Check if the memory range reserved for S3 Boot Script table is large enough to hold the node. 
+   //
+   if ((UINTN) (mS3BootScriptTablePtr->TableLength + EntryLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE)) <= (UINTN) EFI_PAGES_TO_SIZE ((UINTN) (mS3BootScriptTablePtr->TableMemoryPageNumber))) {
+     NewEntryPtr = mS3BootScriptTablePtr->TableBase + mS3BootScriptTablePtr->TableLength;   
+     mS3BootScriptTablePtr->TableLength = mS3BootScriptTablePtr->TableLength + EntryLength;
+     //
+     // Append a terminate node on every insert
+     //
+     S3BootScriptInternalCloseTable ();
+   }
+   return (UINT8*)NewEntryPtr;    
+}
+
+/**
+  This function is to restore boot time boot script data from LockBox.
+
+**/
+VOID
+RestoreBootTimeDataFromLockBox (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         LockBoxLength;
+
+  //
+  // Restore boot time boot script data from LockBox.
+  //
+  LockBoxLength = mS3BootScriptTablePtr->BootTimeScriptLength;
+  Status = RestoreLockBox (
+             &mBootScriptDataBootTimeGuid,
+             (VOID *) mS3BootScriptTablePtr->TableBase,
+             &LockBoxLength
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Update the data to BootScriptData LockBox.
+  //
+  Status = UpdateLockBox (
+             &mBootScriptDataGuid,
+             0,
+             (VOID *) mS3BootScriptTablePtr->TableBase,
+             LockBoxLength
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Update TableLength.
+  //
+  mS3BootScriptTablePtr->TableLength = (UINT32) (mS3BootScriptTablePtr->BootTimeScriptLength - sizeof (EFI_BOOT_SCRIPT_TERMINATE));
+}
+
+/**
+  To get the start address from which a new s3 boot script entry will write into.
+  
+  @param EntryLength      the new entry length.
+  
+  @retval the address from which the a new s3 boot script entry will write into 
+ **/ 
+UINT8* 
+S3BootScriptGetEntryAddAddress (
+  UINT8  EntryLength
+  )
+{
+  UINT8*                         NewEntryPtr;
+
+  if (mS3BootScriptTablePtr->SmmLocked) {
+    //
+    // We need check InSmm, because after SmmReadyToLock, only SMM driver is allowed to write boot script.
+    //
+    if (!mS3BootScriptTablePtr->InSmm) {
+      //
+      // Add DEBUG ERROR, so that we can find it after SmmReadyToLock.
+      // Do not use ASSERT, because we may have test to invoke this interface.
+      //
+      DEBUG ((EFI_D_ERROR, "FATAL ERROR: Set boot script outside SMM after SmmReadyToLock!!!\n"));
+      return NULL;
+    }
+
+    if (mS3BootScriptTablePtr->BackFromS3) {
+      //
+      // Back from S3, restore boot time boot script data from LockBox
+      // and set BackFromS3 flag back to FALSE.
+      //
+      RestoreBootTimeDataFromLockBox ();
+      mS3BootScriptTablePtr->BackFromS3 = FALSE;
+    }
+
+    NewEntryPtr  = S3BootScriptGetRuntimeEntryAddAddress (EntryLength);
+  } else {
+    NewEntryPtr  = S3BootScriptGetBootTimeEntryAddAddress (EntryLength);
+  }  
+  return NewEntryPtr;
+  
+}  
+
+/**
+  Sync BootScript LockBox data.
+
+  @param Script           The address from where the boot script has been added or updated.
+
+**/
+VOID
+SyncBootScript (
+  IN UINT8      *Script
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      ScriptOffset;
+  UINT32      TotalScriptLength;
+
+  if (!mS3BootScriptTablePtr->SmmLocked || !mS3BootScriptTablePtr->InSmm) {
+    //
+    // If it is not after SmmReadyToLock in SMM,
+    // just return.
+    //
+    return ;
+  }
+
+  ScriptOffset = (UINT32) (Script - mS3BootScriptTablePtr->TableBase);
+
+  TotalScriptLength = (UINT32) (mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE));
+
+  //
+  // Update BootScriptData
+  // So in S3 resume, the data can be restored correctly.
+  //
+  Status = UpdateLockBox (
+             &mBootScriptDataGuid,
+             ScriptOffset,
+             (VOID *)((UINTN)mS3BootScriptTablePtr->TableBase + ScriptOffset),
+             TotalScriptLength - ScriptOffset
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Now the length field is updated, need sync to lockbox.
+  // So at S3 resume, the data can be restored correctly.
+  //
+  Status = UpdateLockBox (
+             &mBootScriptDataGuid,
+             OFFSET_OF (EFI_BOOT_SCRIPT_TABLE_HEADER, TableLength),
+             &TotalScriptLength,
+             sizeof (TotalScriptLength)
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/** 
+  This is an function to close the S3 boot script table. The function could only be called in 
+  BOOT time phase. To comply with the Framework spec definition on 
+  EFI_BOOT_SCRIPT_SAVE_PROTOCOL.CloseTable(), this function will fulfill following things:
+  1. Closes the specified boot script table
+  2. It allocates a new memory pool to duplicate all the boot scripts in the specified table. 
+     Once this function is called, the table maintained by the library will be destroyed 
+     after it is copied into the allocated pool.
+  3. Any attempts to add a script record after calling this function will cause a new table 
+     to be created by the library.
+  4. The base address of the allocated pool will be returned in Address. Note that after 
+     using the boot script table, the CALLER is responsible for freeing the pool that is allocated
+     by this function. 
+
+  In Spec PI1.1, this EFI_BOOT_SCRIPT_SAVE_PROTOCOL.CloseTable() is retired. To provides this API for now is 
+  for Framework Spec compatibility.
+  
+  If anyone does call CloseTable() on a real platform, then the caller is responsible for figuring out 
+  how to get the script to run at S3 resume because the boot script maintained by the lib will be 
+  destroyed.
+ 
+  @return the base address of the new copy of the boot script table.   
+  @note this function could only called in boot time phase
+
+**/
+UINT8*
+EFIAPI
+S3BootScriptCloseTable (
+  VOID
+  )
+{
+  UINT8                          *S3TableBase;
+  UINT32                          TableLength;
+  UINT8                          *Buffer;
+  EFI_STATUS                      Status;
+  EFI_BOOT_SCRIPT_TABLE_HEADER      *ScriptTableInfo;
+  
+  S3TableBase =    mS3BootScriptTablePtr->TableBase;    
+  if (S3TableBase == 0) {
+    return 0; 
+  }
+  //
+  // Append the termination record the S3 boot script table
+  //
+  S3BootScriptInternalCloseTable();
+  TableLength = mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE);
+  //
+  // Allocate the buffer and copy the boot script to the buffer. 
+  //
+  Status = gBS->AllocatePool (
+                  EfiBootServicesData,
+                  (UINTN)TableLength,
+                  (VOID **) &Buffer
+                  );
+  if (EFI_ERROR (Status)) {
+        return 0; 
+  }
+  CopyMem (Buffer, S3TableBase, TableLength);
+  
+  //
+  // Destroy the table maintained by the library so that the next write operation 
+  // will write the record to the first entry of the table.
+  //
+  // Fill the table header.
+  ScriptTableInfo                    = (EFI_BOOT_SCRIPT_TABLE_HEADER*)S3TableBase;
+  ScriptTableInfo->OpCode      = S3_BOOT_SCRIPT_LIB_TABLE_OPCODE;
+  ScriptTableInfo->Length      = (UINT8) sizeof (EFI_BOOT_SCRIPT_TABLE_HEADER);
+  ScriptTableInfo->TableLength = 0;   // will be calculate at close the table
+  
+  mS3BootScriptTablePtr->TableLength = sizeof (EFI_BOOT_SCRIPT_TABLE_HEADER);
+  return Buffer;
+}
+/**
+  Save I/O write to boot script 
+
+  @param Width   The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address The base address of the I/O operations.
+  @param Count   The number of I/O operations to perform.
+  @param Buffer  The source buffer from which to write data.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveIoWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  VOID                              *Buffer
+  )
+
+{
+  UINT8                     Length;
+  UINT8                    *Script;
+  UINT8                     WidthInByte;
+  EFI_BOOT_SCRIPT_IO_WRITE  ScriptIoWrite;
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_IO_WRITE) + (WidthInByte * Count));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // save script data
+  //
+  ScriptIoWrite.OpCode  = EFI_BOOT_SCRIPT_IO_WRITE_OPCODE;
+  ScriptIoWrite.Length  = Length;
+  ScriptIoWrite.Width   = Width;
+  ScriptIoWrite.Address = Address;
+  ScriptIoWrite.Count   = (UINT32) Count;
+  CopyMem ((VOID*)Script, (VOID*)&ScriptIoWrite, sizeof(EFI_BOOT_SCRIPT_IO_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_IO_WRITE)), Buffer, WidthInByte * Count);
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Adds a record for an I/O modify operation into a S3 boot script table
+
+  @param Width   The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address The base address of the I/O operations.
+  @param Data    A pointer to the data to be OR-ed.
+  @param DataMask  A pointer to the data mask to be AND-ed with the data read from the register
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveIoReadWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH         Width,
+  IN  UINT64                           Address,
+  IN  VOID                            *Data,
+  IN  VOID                            *DataMask
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_IO_READ_WRITE  ScriptIoReadWrite;
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_IO_READ_WRITE) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptIoReadWrite.OpCode  = EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE;
+  ScriptIoReadWrite.Length  = Length;
+  ScriptIoReadWrite.Width   = Width;
+  ScriptIoReadWrite.Address = Address;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptIoReadWrite, sizeof(EFI_BOOT_SCRIPT_IO_READ_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_IO_READ_WRITE)), Data, WidthInByte);
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_IO_READ_WRITE) + WidthInByte), DataMask, WidthInByte);
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a memory write operation into a specified boot script table.
+
+  @param Width   The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address The base address of the memory operations
+  @param Count   The number of memory operations to perform.
+  @param Buffer  The source buffer from which to write the data.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveMemWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  UINTN                             Count,
+  IN  VOID                              *Buffer
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_MEM_WRITE  ScriptMemWrite;
+  
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_MEM_WRITE) + (WidthInByte * Count));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptMemWrite.OpCode   = EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE;
+  ScriptMemWrite.Length   = Length;
+  ScriptMemWrite.Width    = Width;
+  ScriptMemWrite.Address  = Address;
+  ScriptMemWrite.Count    = (UINT32) Count;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptMemWrite, sizeof(EFI_BOOT_SCRIPT_MEM_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_MEM_WRITE)), Buffer, WidthInByte * Count);
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a memory modify operation into a specified boot script table.
+
+  @param Width     The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address   The base address of the memory operations. Address needs alignment if required
+  @param Data      A pointer to the data to be OR-ed.
+  @param DataMask  A pointer to the data mask to be AND-ed with the data read from the register.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveMemReadWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  VOID                              *Data,
+  IN  VOID                              *DataMask
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_MEM_READ_WRITE  ScriptMemReadWrite;
+ 
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_MEM_READ_WRITE) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  } 
+  //
+  // Build script data
+  //    
+  ScriptMemReadWrite.OpCode   = EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE;
+  ScriptMemReadWrite.Length   = Length;
+  ScriptMemReadWrite.Width    = Width;
+  ScriptMemReadWrite.Address  = Address;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptMemReadWrite , sizeof (EFI_BOOT_SCRIPT_MEM_READ_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_MEM_READ_WRITE)), Data, WidthInByte);
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_MEM_READ_WRITE) + WidthInByte), DataMask, WidthInByte);
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a PCI configuration space write operation into a specified boot script table.
+
+  @param Width     The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address   The address within the PCI configuration space.
+  @param Count     The number of PCI operations to perform.
+  @param Buffer    The source buffer from which to write the data.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePciCfgWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH         Width,
+  IN  UINT64                           Address,
+  IN  UINTN                            Count,
+  IN  VOID                            *Buffer
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE  ScriptPciWrite;
+
+  if (Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE) + (WidthInByte * Count));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  } 
+  //
+  // Build script data
+  //
+  ScriptPciWrite.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE;
+  ScriptPciWrite.Length   = Length;
+  ScriptPciWrite.Width    = Width;
+  ScriptPciWrite.Address  = Address;
+  ScriptPciWrite.Count    = (UINT32) Count;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPciWrite,  sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE)), Buffer, WidthInByte * Count);
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a PCI configuration space modify operation into a specified boot script table.
+
+  @param Width     The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Address   The address within the PCI configuration space.
+  @param Data      A pointer to the data to be OR-ed.The size depends on Width.
+  @param DataMask    A pointer to the data mask to be AND-ed.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN__SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePciCfgReadWrite (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  VOID                              *Data,
+  IN  VOID                              *DataMask
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE  ScriptPciReadWrite;
+
+  if (Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  // 
+  ScriptPciReadWrite.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE;
+  ScriptPciReadWrite.Length   = Length;
+  ScriptPciReadWrite.Width    = Width;
+  ScriptPciReadWrite.Address  = Address;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPciReadWrite, sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE)), Data, WidthInByte);
+  CopyMem (
+    (VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE) + WidthInByte),
+    DataMask,
+    WidthInByte
+    );
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a PCI configuration 2 space write operation into a specified boot script table.
+
+  @param Width     The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Segment   The PCI segment number for Address.
+  @param Address   The address within the PCI configuration space.
+  @param Count     The number of PCI operations to perform.
+  @param Buffer    The source buffer from which to write the data.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is non-zero Segment and 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePciCfg2Write (
+  IN S3_BOOT_SCRIPT_LIB_WIDTH        Width,
+  IN UINT16                          Segment,
+  IN UINT64                          Address,
+  IN UINTN                           Count,
+  IN VOID                           *Buffer
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE  ScriptPciWrite2;
+
+  if (Segment != 0 ||
+      Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE) + (WidthInByte * Count));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptPciWrite2.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE;
+  ScriptPciWrite2.Length   = Length;
+  ScriptPciWrite2.Width    = Width;
+  ScriptPciWrite2.Address  = Address;
+  ScriptPciWrite2.Segment  = Segment;
+  ScriptPciWrite2.Count    = (UINT32)Count;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPciWrite2, sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE)), Buffer, WidthInByte * Count);
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for a PCI configuration 2 space modify operation into a specified boot script table.
+
+  @param Width     The width of the I/O operations.Enumerated in S3_BOOT_SCRIPT_LIB_WIDTH.
+  @param Segment   The PCI segment number for Address.
+  @param Address   The address within the PCI configuration space.
+  @param Data      A pointer to the data to be OR-ed. The size depends on Width.
+  @param DataMask    A pointer to the data mask to be AND-ed.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is non-zero Segment and 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePciCfg2ReadWrite (
+  IN S3_BOOT_SCRIPT_LIB_WIDTH        Width,
+  IN UINT16                          Segment,
+  IN UINT64                          Address,
+  IN VOID                           *Data,
+  IN VOID                           *DataMask
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE  ScriptPciReadWrite2;
+
+  if (Segment != 0 ||
+      Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptPciReadWrite2.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE;
+  ScriptPciReadWrite2.Length   = Length;
+  ScriptPciReadWrite2.Width    = Width;
+  ScriptPciReadWrite2.Segment  = Segment;
+  ScriptPciReadWrite2.Address  = Address;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPciReadWrite2, sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE)), Data, WidthInByte);
+  CopyMem (
+    (VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE) + WidthInByte),
+    DataMask,
+    WidthInByte
+    );
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Checks the parameter of S3BootScriptSaveSmbusExecute().
+
+  This function checks the input parameters of SmbusExecute().  If the input parameters are valid
+  for certain SMBus bus protocol, it will return EFI_SUCCESS; otherwise, it will return certain
+  error code based on the input SMBus bus protocol.
+
+  @param  SmBusAddress            Address that encodes the SMBUS Slave Address, SMBUS Command, SMBUS Data Length, 
+                                  and PEC.
+  @param  Operation               Signifies which particular SMBus hardware protocol instance that
+                                  it will use to execute the SMBus transactions. This SMBus
+                                  hardware protocol is defined by the SMBus Specification and is
+                                  not related to EFI.
+  @param  Length                  Signifies the number of bytes that this operation will do. The
+                                  maximum number of bytes can be revision specific and operation
+                                  specific. This field will contain the actual number of bytes that
+                                  are executed for this operation. Not all operations require this
+                                  argument.
+  @param  Buffer                  Contains the value of data to execute to the SMBus slave device.
+                                  Not all operations require this argument. The length of this
+                                  buffer is identified by Length.
+
+  @retval EFI_SUCCESS             All the parameters are valid for the corresponding SMBus bus
+                                  protocol. 
+  @retval EFI_INVALID_PARAMETER   Operation is not defined in EFI_SMBUS_OPERATION.
+  @retval EFI_INVALID_PARAMETER   Length/Buffer is NULL for operations except for EfiSmbusQuickRead
+                                  and EfiSmbusQuickWrite. Length is outside the range of valid
+                                  values.
+  @retval EFI_UNSUPPORTED         The SMBus operation or PEC is not supported.
+  @retval EFI_BUFFER_TOO_SMALL    Buffer is not sufficient for this operation.
+
+**/
+EFI_STATUS
+CheckParameters (
+  IN     UINTN                    SmBusAddress,
+  IN     EFI_SMBUS_OPERATION      Operation,
+  IN OUT UINTN                    *Length,
+  IN     VOID                     *Buffer
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       RequiredLen;
+  EFI_SMBUS_DEVICE_COMMAND Command;
+  BOOLEAN                  PecCheck;
+ 
+  Command      = SMBUS_LIB_COMMAND (SmBusAddress);
+  PecCheck     = SMBUS_LIB_PEC (SmBusAddress);
+  //
+  // Set default value to be 2:
+  // for SmbusReadWord, SmbusWriteWord and SmbusProcessCall. 
+  //
+  RequiredLen = 2;
+  Status      = EFI_SUCCESS;
+  switch (Operation) {
+    case EfiSmbusQuickRead:
+    case EfiSmbusQuickWrite:
+      if (PecCheck || Command != 0) {
+        return EFI_UNSUPPORTED;
+      }
+      break;
+    case EfiSmbusReceiveByte:
+    case EfiSmbusSendByte:
+      if (Command != 0) {
+        return EFI_UNSUPPORTED;
+      }
+      //
+      // Cascade to check length parameter.
+      //
+    case EfiSmbusReadByte:
+    case EfiSmbusWriteByte:
+      RequiredLen = 1;
+      //
+      // Cascade to check length parameter.
+      //
+    case EfiSmbusReadWord:
+    case EfiSmbusWriteWord:
+    case EfiSmbusProcessCall:
+      if (Buffer == NULL || Length == NULL) {
+        return EFI_INVALID_PARAMETER;
+      } else if (*Length < RequiredLen) {
+        Status = EFI_BUFFER_TOO_SMALL;
+      }
+      *Length = RequiredLen;
+      break;
+    case EfiSmbusReadBlock:
+    case EfiSmbusWriteBlock:
+    case EfiSmbusBWBRProcessCall:
+      if ((Buffer == NULL) || 
+          (Length == NULL) || 
+          (*Length < MIN_SMBUS_BLOCK_LEN) ||
+          (*Length > MAX_SMBUS_BLOCK_LEN)) {
+        return EFI_INVALID_PARAMETER;
+      }
+      break;
+    default:
+      return EFI_INVALID_PARAMETER;
+  }
+  return Status;
+}
+
+/**
+  Adds a record for an SMBus command execution into a specified boot script table.
+
+  @param  SmBusAddress  Address that encodes the SMBUS Slave Address, SMBUS Command, SMBUS Data Length, and PEC.
+  @param Operation      Indicates which particular SMBus protocol it will use to execute the SMBus
+                        transactions.
+  @param Length         A pointer to signify the number of bytes that this operation will do.
+  @param Buffer         Contains the value of data to execute to the SMBUS slave device.
+  
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveSmbusExecute (
+  IN  UINTN                             SmBusAddress, 
+  IN  EFI_SMBUS_OPERATION               Operation,
+  IN  UINTN                             *Length,
+  IN  VOID                              *Buffer
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 BufferLength;
+  UINT8                 DataSize;
+  UINT8                *Script;
+  EFI_BOOT_SCRIPT_SMBUS_EXECUTE  ScriptSmbusExecute;
+
+  if (Length == NULL) {
+    BufferLength = 0;
+  } else {
+    BufferLength = *Length;
+  }
+
+  Status = CheckParameters (SmBusAddress, Operation, &BufferLength, Buffer);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DataSize = (UINT8)(sizeof (EFI_BOOT_SCRIPT_SMBUS_EXECUTE) + BufferLength);
+  
+  Script = S3BootScriptGetEntryAddAddress (DataSize);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptSmbusExecute.OpCode       = EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE;
+  ScriptSmbusExecute.Length       = DataSize;
+  ScriptSmbusExecute.SmBusAddress = (UINT64) SmBusAddress;
+  ScriptSmbusExecute.Operation    = Operation;
+  ScriptSmbusExecute.DataSize     = (UINT32) BufferLength;
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptSmbusExecute, sizeof (EFI_BOOT_SCRIPT_SMBUS_EXECUTE));
+  CopyMem (
+    (VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_SMBUS_EXECUTE)),
+    Buffer,
+    BufferLength
+    );
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for an execution stall on the processor into a specified boot script table.
+
+  @param Duration   Duration in microseconds of the stall
+  
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveStall (
+  IN  UINTN                             Duration
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  EFI_BOOT_SCRIPT_STALL  ScriptStall;
+
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_STALL));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptStall.OpCode    = EFI_BOOT_SCRIPT_STALL_OPCODE;
+  ScriptStall.Length    = Length;
+  ScriptStall.Duration  = Duration;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptStall, sizeof (EFI_BOOT_SCRIPT_STALL));
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for dispatching specified arbitrary code into a specified boot script table.
+
+  @param EntryPoint   Entry point of the code to be dispatched.
+  @param Context      Argument to be passed into the EntryPoint of the code to be dispatched.
+  
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveDispatch2 (
+  IN  VOID                      *EntryPoint,
+  IN  VOID                      *Context
+  )
+{
+  UINT8                 Length;
+  UINT8                 *Script;
+  EFI_BOOT_SCRIPT_DISPATCH_2  ScriptDispatch2;
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_DISPATCH_2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptDispatch2.OpCode     = EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE;
+  ScriptDispatch2.Length     = Length;
+  ScriptDispatch2.EntryPoint = (EFI_PHYSICAL_ADDRESS)(UINTN)EntryPoint;
+  ScriptDispatch2.Context =   (EFI_PHYSICAL_ADDRESS)(UINTN)Context;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptDispatch2, sizeof (EFI_BOOT_SCRIPT_DISPATCH_2));
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+
+}
+/**
+  Adds a record for memory reads of the memory location and continues when the exit criteria is
+  satisfied or after a defined duration.
+
+  Please aware, below interface is different with PI specification, Vol 5:
+  EFI_S3_SAVE_STATE_PROTOCOL.Write() for EFI_BOOT_SCRIPT_MEM_POLL_OPCODE.
+  "Duration" below is microseconds, while "Delay" in PI specification means
+  the number of 100ns units to poll.
+
+  @param Width     The width of the memory operations.
+  @param Address   The base address of the memory operations.
+  @param BitMask   A pointer to the bit mask to be AND-ed with the data read from the register.
+  @param BitValue  A pointer to the data value after to be Masked.
+  @param Duration  Duration in microseconds of the stall.
+  @param LoopTimes The times of the register polling.
+
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveMemPoll (
+  IN  S3_BOOT_SCRIPT_LIB_WIDTH          Width,
+  IN  UINT64                            Address,
+  IN  VOID                              *BitMask,
+  IN  VOID                              *BitValue,
+  IN  UINTN                             Duration,
+  IN  UINTN                             LoopTimes
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  UINT8                 WidthInByte; 
+  EFI_BOOT_SCRIPT_MEM_POLL      ScriptMemPoll; 
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_MEM_POLL) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptMemPoll.OpCode   = EFI_BOOT_SCRIPT_MEM_POLL_OPCODE;
+  ScriptMemPoll.Length   = Length;
+  ScriptMemPoll.Width    = Width;  
+  ScriptMemPoll.Address  = Address;
+  ScriptMemPoll.Duration = Duration;
+  ScriptMemPoll.LoopTimes = LoopTimes;
+
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_MEM_POLL)), BitValue, WidthInByte);
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_MEM_POLL) + WidthInByte), BitMask, WidthInByte);
+  CopyMem ((VOID*)Script, (VOID*)&ScriptMemPoll, sizeof (EFI_BOOT_SCRIPT_MEM_POLL)); 
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Store arbitrary information in the boot script table. This opcode is a no-op on dispatch and is only
+  used for debugging script issues.
+  
+  @param InformationLength   Length of the data in bytes
+  @param Information       Information to be logged in the boot scrpit
+ 
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveInformation (
+  IN  UINT32                                InformationLength, 
+  IN  VOID                                 *Information
+  )
+{
+  UINT8                 Length;
+  UINT8                 *Script;
+  EFI_BOOT_SCRIPT_INFORMATION  ScriptInformation;
+
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_INFORMATION) + InformationLength);
+
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptInformation.OpCode     = EFI_BOOT_SCRIPT_INFORMATION_OPCODE;
+  ScriptInformation.Length     = Length;
+
+
+  ScriptInformation.InformationLength = InformationLength;  
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptInformation, sizeof (EFI_BOOT_SCRIPT_INFORMATION));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_INFORMATION)), (VOID *) Information, (UINTN) InformationLength);
+
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+
+}
+/**
+  Store a string in the boot script table. This opcode is a no-op on dispatch and is only
+  used for debugging script issues.
+  
+  @param String            The string to save to boot script table
+  
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveInformationAsciiString (
+  IN  CONST CHAR8               *String
+  )
+{
+  return S3BootScriptSaveInformation (      
+           (UINT32) AsciiStrLen (String) + 1, 
+           (VOID*) String
+           );
+}
+/**
+  Adds a record for dispatching specified arbitrary code into a specified boot script table.
+
+  @param EntryPoint   Entry point of the code to be dispatched.
+  
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveDispatch (
+  IN  VOID                              *EntryPoint
+  )
+{
+  UINT8                 Length;
+  UINT8                *Script;
+  EFI_BOOT_SCRIPT_DISPATCH  ScriptDispatch;
+  
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_DISPATCH));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }  
+  //
+  // Build script data
+  //
+  ScriptDispatch.OpCode     = EFI_BOOT_SCRIPT_DISPATCH_OPCODE;
+  ScriptDispatch.Length     = Length;
+  ScriptDispatch.EntryPoint = (EFI_PHYSICAL_ADDRESS)(UINTN)EntryPoint;
+  
+  CopyMem ((VOID*)Script, (VOID*)&ScriptDispatch, sizeof (EFI_BOOT_SCRIPT_DISPATCH)); 
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+
+}
+/**
+  Adds a record for I/O reads the I/O location and continues when the exit criteria is satisfied or after a
+  defined duration.
+  
+  @param  Width                 The width of the I/O operations. 
+  @param  Address               The base address of the I/O operations.
+  @param  Data                  The comparison value used for the polling exit criteria.
+  @param  DataMask              Mask used for the polling criteria. The bits in the bytes below Width which are zero
+                                in Data are ignored when polling the memory address.
+  @param  Delay                 The number of 100ns units to poll. Note that timer available may be of poorer
+                                granularity so the delay may be longer.
+
+ @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+ @retval RETURN_SUCCESS          Opcode is added.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSaveIoPoll (
+  IN S3_BOOT_SCRIPT_LIB_WIDTH       Width,
+  IN UINT64                     Address,
+  IN VOID                      *Data,
+  IN VOID                      *DataMask, 
+  IN UINT64                     Delay   
+  )
+{
+  UINT8                 WidthInByte;  
+  UINT8                *Script;
+  UINT8                 Length;
+  EFI_BOOT_SCRIPT_IO_POLL  ScriptIoPoll;
+  
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));  
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_IO_POLL) + (WidthInByte * 2));
+ 
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  } 
+  //
+  // Build script data
+  //
+  ScriptIoPoll.OpCode   = EFI_BOOT_SCRIPT_IO_POLL_OPCODE;
+  ScriptIoPoll.Length   = (UINT8) (sizeof (EFI_BOOT_SCRIPT_IO_POLL) + (WidthInByte * 2));
+  ScriptIoPoll.Width    = Width;  
+  ScriptIoPoll.Address  = Address;
+  ScriptIoPoll.Delay    = Delay;
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptIoPoll, sizeof (EFI_BOOT_SCRIPT_IO_POLL));  
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_IO_POLL)), Data, WidthInByte);
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_IO_POLL) + WidthInByte), DataMask, WidthInByte);
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+
+/**
+  Adds a record for PCI configuration space reads and continues when the exit criteria is satisfied or
+  after a defined duration.
+
+  @param  Width                 The width of the I/O operations. 
+  @param  Address               The address within the PCI configuration space.
+  @param  Data                  The comparison value used for the polling exit criteria.
+  @param  DataMask              Mask used for the polling criteria. The bits in the bytes below Width which are zero
+                                in Data are ignored when polling the memory address
+  @param  Delay                 The number of 100ns units to poll. Note that timer available may be of poorer
+                                granularity so the delay may be longer.
+
+ @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+ @retval RETURN_SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePciPoll (
+   IN S3_BOOT_SCRIPT_LIB_WIDTH   Width,
+   IN UINT64                     Address,
+   IN VOID                      *Data,
+   IN VOID                      *DataMask,
+   IN UINT64                     Delay
+)
+{
+  UINT8                   *Script;
+  UINT8                    WidthInByte;  
+  UINT8                    Length;
+  EFI_BOOT_SCRIPT_PCI_CONFIG_POLL  ScriptPciPoll;
+
+  if (Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptPciPoll.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE;
+  ScriptPciPoll.Length   = (UINT8) (sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL) + (WidthInByte * 2));
+  ScriptPciPoll.Width    = Width;  
+  ScriptPciPoll.Address  = Address;
+  ScriptPciPoll.Delay    = Delay;
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPciPoll, sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL));
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL)), Data, WidthInByte);
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG_POLL) + WidthInByte), DataMask, WidthInByte);
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Adds a record for PCI configuration space reads and continues when the exit criteria is satisfied or
+  after a defined duration.
+
+  @param  Width                 The width of the I/O operations. 
+  @param  Segment               The PCI segment number for Address.
+  @param  Address               The address within the PCI configuration space.
+  @param  Data                  The comparison value used for the polling exit criteria.
+  @param  DataMask              Mask used for the polling criteria. The bits in the bytes below Width which are zero
+                                in Data are ignored when polling the memory address
+  @param  Delay                 The number of 100ns units to poll. Note that timer available may be of poorer
+                                granularity so the delay may be longer.
+
+ @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+ @retval RETURN_SUCCESS           Opcode is added.
+  @note  A known Limitations in the implementation which is non-zero Segment and 64bits operations are not supported.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptSavePci2Poll (
+   IN S3_BOOT_SCRIPT_LIB_WIDTH      Width,
+   IN UINT16                        Segment,
+   IN UINT64                        Address,
+   IN VOID                         *Data,
+   IN VOID                         *DataMask,
+  IN UINT64                         Delay
+)
+{
+  UINT8                    WidthInByte;  
+  UINT8                   *Script;
+  UINT8                    Length;
+  EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL  ScriptPci2Poll;
+
+  if (Segment != 0 ||
+      Width == S3BootScriptWidthUint64 ||
+      Width == S3BootScriptWidthFifoUint64 ||
+      Width == S3BootScriptWidthFillUint64) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  WidthInByte = (UINT8) (0x01 << (Width & 0x03));
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL) + (WidthInByte * 2));
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  } 
+  //
+  // Build script data
+  //
+  ScriptPci2Poll.OpCode   = EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE;
+  ScriptPci2Poll.Length   = (UINT8) (sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL) + (WidthInByte * 2));
+  ScriptPci2Poll.Width    = Width; 
+  ScriptPci2Poll.Segment  = Segment;
+  ScriptPci2Poll.Address  = Address;
+  ScriptPci2Poll.Delay    = Delay;
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptPci2Poll, sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL));
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL)), Data, WidthInByte);
+  CopyMem ((UINT8 *) (Script + sizeof (EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL) + WidthInByte), DataMask, WidthInByte);
+  
+  SyncBootScript (Script);
+
+  return RETURN_SUCCESS;
+}
+/**
+  Do the calculation of start address from which a new s3 boot script entry will write into.
+  
+  @param EntryLength      The new entry length.
+  @param Position         specifies the position in the boot script table where the opcode will be
+                          inserted, either before or after, depending on BeforeOrAfter. 
+  @param BeforeOrAfter    The flag to indicate to insert the nod before or after the position.
+                          This parameter is effective when InsertFlag is TRUE
+  @param Script           return out the position from which the a new s3 boot script entry will write into
+**/
+VOID
+S3BootScriptCalculateInsertAddress (
+  IN  UINT8     EntryLength,
+  IN  VOID     *Position OPTIONAL,
+  IN  BOOLEAN   BeforeOrAfter OPTIONAL,
+  OUT UINT8   **Script   
+  )
+{
+   UINTN                            TableLength;
+   UINT8                            *S3TableBase;
+   UINTN                            PositionOffset; 
+   EFI_BOOT_SCRIPT_COMMON_HEADER     ScriptHeader;
+   //
+   // The entry inserting to table is already added to the end of the table
+   //
+   TableLength =  mS3BootScriptTablePtr->TableLength - EntryLength;
+   S3TableBase = mS3BootScriptTablePtr->TableBase ;
+   // 
+   // calculate the Position offset
+   //
+   if (Position != NULL) {
+     PositionOffset = (UINTN) ((UINT8 *)Position - S3TableBase);
+   
+     //
+     // If the BeforeOrAfter is FALSE, that means to insert the node right after the node.
+     //
+     if (!BeforeOrAfter) {
+        CopyMem ((VOID*)&ScriptHeader, Position, sizeof(EFI_BOOT_SCRIPT_COMMON_HEADER));  
+        PositionOffset += (ScriptHeader.Length);
+     }
+     //     
+     // Insert the node before the adjusted Position
+     //
+     CopyMem (S3TableBase+PositionOffset+EntryLength, S3TableBase+PositionOffset, TableLength - PositionOffset);  
+     //
+     // calculate the the start address for the new entry. 
+     //
+     *Script = S3TableBase + PositionOffset;
+       
+   } else {
+     if (!BeforeOrAfter) {
+       //
+       //  Insert the node to the end of the table
+       //
+       *Script = S3TableBase + TableLength; 
+     } else {
+       // 
+       // Insert the node to the beginning of the table
+       //
+       PositionOffset = (UINTN) sizeof(EFI_BOOT_SCRIPT_TABLE_HEADER);
+       CopyMem (S3TableBase+PositionOffset+EntryLength, S3TableBase+PositionOffset, TableLength - PositionOffset); 
+       *Script = S3TableBase + PositionOffset; 
+     }
+   }       
+}
+/**
+  Move the last boot script entry to the position 
+
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.
+
+  @retval RETURN_OUT_OF_RESOURCES  The table is not available.
+  @retval RETURN_INVALID_PARAMETER The Position is not a valid position in the boot script table.
+  @retval RETURN_SUCCESS           Opcode is inserted.
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptMoveLastOpcode (
+  IN     BOOLEAN                        BeforeOrAfter,
+  IN OUT VOID                         **Position OPTIONAL
+)
+{
+  UINT8*                Script;
+  VOID                  *TempPosition;  
+  UINTN                 StartAddress;
+  UINT32                TableLength;
+  EFI_BOOT_SCRIPT_COMMON_HEADER  ScriptHeader;
+  BOOLEAN               ValidatePosition;
+  UINT8*                LastOpcode;
+  UINT8                 TempBootScriptEntry[BOOT_SCRIPT_NODE_MAX_LENGTH];
+  
+  ValidatePosition = FALSE;
+  TempPosition = (Position == NULL) ? NULL:(*Position);
+
+  //
+  // Check that the script is initialized and synced without adding an entry to the script.
+  //
+  Script = S3BootScriptGetEntryAddAddress (0);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  Script = mS3BootScriptTablePtr->TableBase;
+
+  StartAddress  = (UINTN) Script;
+  TableLength   = mS3BootScriptTablePtr->TableLength;
+  Script        = Script + sizeof(EFI_BOOT_SCRIPT_TABLE_HEADER);
+  LastOpcode    = Script;
+  //
+  // Find the last boot Script Entry which is not the terminate node
+  //
+  while ((UINTN) Script < (UINTN) (StartAddress + TableLength)) {    
+    CopyMem ((VOID*)&ScriptHeader, Script, sizeof(EFI_BOOT_SCRIPT_COMMON_HEADER));   
+    if (TempPosition != NULL && TempPosition == Script) {
+      //
+      // If the position is specified, the position must be pointed to a boot script entry start address. 
+      //
+      ValidatePosition = TRUE;
+    }
+    if (ScriptHeader.OpCode != S3_BOOT_SCRIPT_LIB_TERMINATE_OPCODE) {
+      LastOpcode = Script;
+    } 
+    Script  = Script + ScriptHeader.Length;
+  }
+  //
+  // If the position is specified, but not the start of a boot script entry, it is a invalid input
+  //
+  if (TempPosition != NULL && !ValidatePosition) {
+    return RETURN_INVALID_PARAMETER;
+  }
+  
+  CopyMem ((VOID*)&ScriptHeader, LastOpcode, sizeof(EFI_BOOT_SCRIPT_COMMON_HEADER)); 
+  
+  CopyMem((VOID*)TempBootScriptEntry, LastOpcode, ScriptHeader.Length); 
+  //
+  // Find the right position to write the node in
+  //
+  S3BootScriptCalculateInsertAddress (
+    ScriptHeader.Length,
+    TempPosition,
+    BeforeOrAfter,
+    &Script   
+  );
+  //
+  // Copy the node to Boot script table
+  //
+  CopyMem((VOID*)Script, (VOID*)TempBootScriptEntry, ScriptHeader.Length);
+
+  SyncBootScript (Script);
+
+  //
+  // return out the Position
+  //
+  if (Position != NULL) {
+    *Position = Script;
+  }
+  return RETURN_SUCCESS;
+}
+/**
+  Create a Label node in the boot script table. 
+  
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.  
+  @param InformationLength      Length of the label in bytes
+  @param Information            Label to be logged in the boot scrpit
+ 
+  @retval RETURN_INVALID_PARAMETER The Position is not a valid position in the boot script table.
+  @retval RETURN_OUT_OF_RESOURCES  Not enough memory for the table do operation.
+  @retval RETURN_SUCCESS           Opcode is added.
+
+**/
+RETURN_STATUS
+EFIAPI
+S3BootScriptLabelInternal (
+  IN        BOOLEAN                        BeforeOrAfter,
+  IN OUT    VOID                         **Position OPTIONAL, 
+  IN        UINT32                         InformationLength, 
+  IN CONST  CHAR8                          *Information
+  )
+{
+  UINT8                 Length;
+  UINT8                 *Script;
+  EFI_BOOT_SCRIPT_INFORMATION  ScriptInformation;
+ 
+  Length = (UINT8)(sizeof (EFI_BOOT_SCRIPT_INFORMATION) + InformationLength);
+  
+  Script = S3BootScriptGetEntryAddAddress (Length);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  //
+  // Build script data
+  //
+  ScriptInformation.OpCode     = S3_BOOT_SCRIPT_LIB_LABEL_OPCODE;
+  ScriptInformation.Length     = Length;
+
+
+  ScriptInformation.InformationLength = InformationLength;  
+
+  CopyMem ((VOID*)Script, (VOID*)&ScriptInformation, sizeof (EFI_BOOT_SCRIPT_INFORMATION));
+  CopyMem ((VOID*)(Script + sizeof (EFI_BOOT_SCRIPT_INFORMATION)), (VOID *) Information, (UINTN) InformationLength);
+
+  SyncBootScript (Script);
+
+  return S3BootScriptMoveLastOpcode (BeforeOrAfter, Position);
+
+}
+/**
+  Find a label within the boot script table and, if not present, optionally create it.
+
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE)
+                                or after (FALSE) the position in the boot script table 
+                                specified by Position.
+  @param  CreateIfNotFound      Specifies whether the label will be created if the label 
+                                does not exists (TRUE) or not (FALSE).
+  @param  Position              On entry, specifies the position in the boot script table
+                                where the opcode will be inserted, either before or after,
+                                depending on BeforeOrAfter. On exit, specifies the position
+                                of the inserted opcode in the boot script table.
+  @param  Label                 Points to the label which will be inserted in the boot script table.
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+RETURN_STATUS
+EFIAPI 
+S3BootScriptLabel (
+  IN       BOOLEAN                      BeforeOrAfter,
+  IN       BOOLEAN                      CreateIfNotFound,
+  IN OUT   VOID                       **Position OPTIONAL,
+  IN CONST CHAR8                       *Label
+  )
+{
+  UINT8*                Script;
+  UINTN                 StartAddress;
+  UINT32                TableLength;
+  EFI_BOOT_SCRIPT_COMMON_HEADER  ScriptHeader;
+  EFI_BOOT_SCRIPT_TABLE_HEADER   TableHeader;
+  UINT32                         LabelLength;
+  //
+  // Check NULL Label
+  //
+  if (Label == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Check empty Label
+  //
+  if (Label[0] == '\0') {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Check that the script is initialized and synced without adding an entry to the script.
+  // The code must search for the label first before it knows if a new entry needs
+  // to be added.
+  //
+  Script = S3BootScriptGetEntryAddAddress (0);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Check the header and search for existing label.
+  // 
+  Script = mS3BootScriptTablePtr->TableBase;
+  CopyMem ((VOID*)&TableHeader, Script, sizeof(EFI_BOOT_SCRIPT_TABLE_HEADER));
+  if (TableHeader.OpCode != S3_BOOT_SCRIPT_LIB_TABLE_OPCODE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  StartAddress  = (UINTN) Script;
+  TableLength   = mS3BootScriptTablePtr->TableLength;
+  Script    =     Script + TableHeader.Length;
+  while ((UINTN) Script < (UINTN) (StartAddress + TableLength)) {
+    
+    CopyMem ((VOID*)&ScriptHeader, Script, sizeof(EFI_BOOT_SCRIPT_COMMON_HEADER));   
+    if (ScriptHeader.OpCode == S3_BOOT_SCRIPT_LIB_LABEL_OPCODE) {
+      if (AsciiStrCmp ((CHAR8 *)(UINTN)(Script+sizeof(EFI_BOOT_SCRIPT_INFORMATION)), Label) == 0) {
+        (*Position) = Script; 
+        return EFI_SUCCESS;
+      }
+    } 
+    Script  = Script + ScriptHeader.Length;
+  }
+  if (CreateIfNotFound) {
+    LabelLength = (UINT32)AsciiStrSize(Label);
+    return S3BootScriptLabelInternal (BeforeOrAfter,Position, LabelLength, Label);     
+  } else {
+    return EFI_NOT_FOUND;
+  }   
+}
+
+/**
+  Compare two positions in the boot script table and return their relative position.
+  @param  Position1             The positions in the boot script table to compare
+  @param  Position2             The positions in the boot script table to compare
+  @param  RelativePosition      On return, points to the result of the comparison
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+RETURN_STATUS
+EFIAPI 
+S3BootScriptCompare (
+  IN  UINT8                       *Position1,
+  IN  UINT8                       *Position2,
+  OUT UINTN                       *RelativePosition
+  )
+{
+  UINT8*                    Script;
+  UINT32                    TableLength; 
+
+  if (RelativePosition == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check that the script is initialized and synced without adding an entry to the script.
+  //
+  Script = S3BootScriptGetEntryAddAddress (0);
+  if (Script == NULL) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+  Script = mS3BootScriptTablePtr->TableBase;
+
+  //
+  // mS3BootScriptTablePtr->TableLength does not include the termination node, so add it up
+  //
+  TableLength = mS3BootScriptTablePtr->TableLength + sizeof (EFI_BOOT_SCRIPT_TERMINATE);
+  if (Position1 < Script || Position1 > Script+TableLength) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Position2 < Script || Position2 > Script+TableLength) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *RelativePosition = (Position1 < Position2)?-1:((Position1 == Position2)?0:1);
+  
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.inf b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.inf
new file mode 100644
index 0000000000..de314db479
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.inf
@@ -0,0 +1,74 @@
+## @file
+# DXE S3 boot script Library.
+#
+# Copyright (c) 2006 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DxeS3BootScriptLib
+  MODULE_UNI_FILE                = DxeS3BootScriptLib.uni
+  FILE_GUID                      = 57F9967B-26CD-4262-837A-55B8AA158254
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = S3BootScriptLib|DXE_CORE DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SMM_DRIVER DXE_SAL_DRIVER UEFI_DRIVER UEFI_APPLICATION
+
+
+  CONSTRUCTOR                    = S3BootScriptLibInitialize
+  DESTRUCTOR                     = S3BootScriptLibDeinitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BootScriptSave.c
+  BootScriptExecute.c
+  InternalBootScriptLib.h
+  BootScriptInternalFormat.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  TimerLib
+  DebugLib
+  PcdLib
+  UefiLib
+  SmbusLib
+  PciLib
+  IoLib 
+  LockBoxLib
+
+[Protocols]
+  gEfiSmmBase2ProtocolGuid                      ## SOMETIMES_CONSUMES
+  gEfiDxeSmmReadyToLockProtocolGuid             ## NOTIFY
+  gEfiSmmReadyToLockProtocolGuid                ## NOTIFY
+  gEdkiiSmmExitBootServicesProtocolGuid         ## NOTIFY
+  gEdkiiSmmLegacyBootProtocolGuid               ## NOTIFY
+
+[Pcd]
+  ## CONSUMES
+  ## SOMETIMES_PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptTablePrivateDataPtr
+  ## CONSUMES
+  ## SOMETIMES_PRODUCES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptTablePrivateSmmDataPtr
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptRuntimeTableReservePageNumber   ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.uni b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.uni
new file mode 100644
index 0000000000..775b2f8715
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.uni
diff --git a/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/InternalBootScriptLib.h b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/InternalBootScriptLib.h
new file mode 100644
index 0000000000..e540278483
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiDxeS3BootScriptLib/InternalBootScriptLib.h
@@ -0,0 +1,110 @@
+/** @file
+  Support for S3 boot script lib. This file defined some internal macro and internal 
+  data structure
+ 
+  Copyright (c) 2006 - 2015, 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.
+
+**/
+#ifndef __INTERNAL_BOOT_SCRIPT_LIB__
+#define __INTERNAL_BOOT_SCRIPT_LIB__
+
+#include <PiDxe.h>
+
+#include <Guid/EventGroup.h>
+#include <Protocol/SmmBase2.h>
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <Protocol/SmmReadyToLock.h>
+#include <Protocol/SmmExitBootServices.h>
+#include <Protocol/SmmLegacyBoot.h>
+
+#include <Library/S3BootScriptLib.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PcdLib.h>
+#include <Library/SmbusLib.h>
+#include <Library/IoLib.h>
+#include <Library/PciLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/UefiLib.h>
+#include <Library/LockBoxLib.h>
+
+#include "BootScriptInternalFormat.h"
+
+#define MAX_IO_ADDRESS 0xFFFF
+
+//
+// Macro to convert a UEFI PCI address to a PCI Library PCI address
+//
+#define PCI_ADDRESS_ENCODE(A) (UINTN)PCI_LIB_ADDRESS( \
+        ((((UINTN)(A))& 0xff000000) >> 24), ((((UINTN)(A)) &0x00ff0000) >> 16), ((((UINTN)(A)) & 0xff00) >> 8), ((RShiftU64 ((A), 32) & 0xfff) | ((A)& 0xff)) \
+        )
+        
+
+
+typedef union {
+  UINT8 volatile  *Buf;
+  UINT8 volatile  *Uint8;
+  UINT16 volatile *Uint16;
+  UINT32 volatile *Uint32;
+  UINT64 volatile *Uint64;
+  UINTN volatile   Uint;
+} PTR;
+
+
+// Minimum and maximum length for SMBus bus block protocols defined in SMBus spec 2.0.
+//
+#define MIN_SMBUS_BLOCK_LEN               1
+#define MAX_SMBUS_BLOCK_LEN               32
+
+//
+// The boot script private data.
+//
+typedef struct {
+  UINT8     *TableBase;
+  UINT32    TableLength;            // Record the actual memory length 
+  UINT16    TableMemoryPageNumber;  // Record the page number Allocated for the table 
+  BOOLEAN   InSmm;                  // Record if this library is in SMM.
+  BOOLEAN   AtRuntime;              // Record if current state is after SmmExitBootServices or SmmLegacyBoot.
+  UINT32    BootTimeScriptLength;   // Maintain boot time script length in LockBox after SmmReadyToLock in SMM.
+  BOOLEAN   SmmLocked;              // Record if current state is after SmmReadyToLock
+  BOOLEAN   BackFromS3;             // Indicate that the system is back from S3.
+} SCRIPT_TABLE_PRIVATE_DATA;
+
+typedef
+EFI_STATUS
+(EFIAPI *DISPATCH_ENTRYPOINT_FUNC) (
+  IN EFI_HANDLE ImageHandle,
+  IN VOID       *Context
+  );
+
+extern SCRIPT_TABLE_PRIVATE_DATA       *mS3BootScriptTablePtr;
+
+//
+// Define Opcode for Label which is implementation specific and no standard spec define.
+//
+#define  S3_BOOT_SCRIPT_LIB_LABEL_OPCODE    0xFE
+
+///
+/// The opcode indicate the start of the boot script table.
+///
+#define S3_BOOT_SCRIPT_LIB_TABLE_OPCODE                  0xAA
+///
+/// The opcode indicate the end of the boot script table.
+///
+#define S3_BOOT_SCRIPT_LIB_TERMINATE_OPCODE              0xFF
+
+
+#endif //__INTERNAL_BOOT_SCRIPT_LIB__
+
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/MemoryAllocationLib.c b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/MemoryAllocationLib.c
new file mode 100644
index 0000000000..5e13a3eda2
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/MemoryAllocationLib.c
@@ -0,0 +1,929 @@
+/** @file
+  Support routines for memory allocation routines based on SMM Core internal functions.
+  
+  The PI System Management Mode Core Interface Specification only allows the use
+  of EfiRuntimeServicesCode and EfiRuntimeServicesData memory types for memory 
+  allocations as the SMRAM space should be reserved after BDS phase.  The functions 
+  in the Memory Allocation Library use EfiBootServicesData as the default memory 
+  allocation type.  For this SMM specific instance of the Memory Allocation Library, 
+  EfiRuntimeServicesData is used as the default memory type for all allocations. 
+  In addition, allocation for the Reserved memory types are not supported and will 
+  always return NULL.
+
+  Copyright (c) 2006 - 2015, 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 <PiSmm.h>
+
+#include <Protocol/SmmAccess2.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include "PiSmmCoreMemoryAllocationServices.h"
+
+EFI_SMRAM_DESCRIPTOR  *mSmmCoreMemoryAllocLibSmramRanges    = NULL;
+UINTN                 mSmmCoreMemoryAllocLibSmramRangeCount = 0;
+
+/**
+  Check whether the start address of buffer is within any of the SMRAM ranges.
+
+  @param[in]  Buffer   The pointer to the buffer to be checked.
+
+  @retval     TURE     The buffer is in SMRAM ranges.
+  @retval     FALSE    The buffer is out of SMRAM ranges.
+**/
+BOOLEAN
+EFIAPI
+BufferInSmram (
+  IN VOID *Buffer
+  )
+{
+  UINTN  Index;
+
+  for (Index = 0; Index < mSmmCoreMemoryAllocLibSmramRangeCount; Index ++) {
+    if (((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer >= mSmmCoreMemoryAllocLibSmramRanges[Index].CpuStart) && 
+        ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer < (mSmmCoreMemoryAllocLibSmramRanges[Index].CpuStart + mSmmCoreMemoryAllocLibSmramRanges[Index].PhysicalSize))) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Allocates one or more 4KB pages of a certain memory type.
+
+  Allocates the number of 4KB pages of a certain memory type and returns a pointer to the allocated
+  buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL is returned.
+  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocatePages (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            Pages
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory; 
+
+  if (Pages == 0) {
+    return NULL;
+  }
+
+  Status = SmmAllocatePages (AllocateAnyPages, MemoryType, Pages, &Memory);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  return (VOID *) (UINTN) Memory;
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData.
+
+  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocatePages (
+  IN UINTN  Pages
+  )
+{
+  return InternalAllocatePages (EfiRuntimeServicesData, Pages);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData.
+
+  Allocates the number of 4KB pages of type EfiRuntimeServicesData and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimePages (
+  IN UINTN  Pages
+  )
+{
+  return InternalAllocatePages (EfiRuntimeServicesData, Pages);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiReservedMemoryType.
+
+  Allocates the number of 4KB pages of type EfiReservedMemoryType and returns a pointer to the
+  allocated buffer.  The buffer returned is aligned on a 4KB boundary.  If Pages is 0, then NULL
+  is returned.  If there is not enough memory remaining to satisfy the request, then NULL is
+  returned.
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedPages (
+  IN UINTN  Pages
+  )
+{
+  return NULL;
+}
+
+/**
+  Frees one or more 4KB pages that were previously allocated with one of the page allocation
+  functions in the Memory Allocation Library.
+
+  Frees the number of 4KB pages specified by Pages from the buffer specified by Buffer.  Buffer
+  must have been allocated on a previous call to the page allocation services of the Memory
+  Allocation Library.  If it is not possible to free allocated pages, then this function will
+  perform no actions.
+  
+  If Buffer was not allocated with a page allocation function in the Memory Allocation Library,
+  then ASSERT().
+  If Pages is zero, then ASSERT().
+ 
+  @param  Buffer                Pointer to the buffer of pages to free.
+  @param  Pages                 The number of 4 KB pages to free.
+
+**/
+VOID
+EFIAPI
+FreePages (
+  IN VOID   *Buffer,
+  IN UINTN  Pages
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Pages != 0);
+  if (BufferInSmram (Buffer)) {
+    //
+    // When Buffer is in SMRAM range, it should be allocated by SmmAllocatePages() service.
+    // So, SmmFreePages() service is used to free it.
+    //
+    Status = SmmFreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  } else {
+    //
+    // When Buffer is out of SMRAM range, it should be allocated by gBS->AllocatePages() service.
+    // So, gBS->FreePages() service is used to free it.
+    //
+    Status = gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  }
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates one or more 4KB pages of a certain memory type at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of a certain memory type with an alignment
+  specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is returned.
+  If there is not enough memory at the specified alignment remaining to satisfy the request, then
+  NULL is returned.
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateAlignedPages (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            Pages,
+  IN UINTN            Alignment
+  )
+{
+  EFI_STATUS            Status;
+  EFI_PHYSICAL_ADDRESS  Memory;
+  UINTN                 AlignedMemory;
+  UINTN                 AlignmentMask;
+  UINTN                 UnalignedPages;
+  UINTN                 RealPages;
+
+  //
+  // Alignment must be a power of two or zero.
+  //
+  ASSERT ((Alignment & (Alignment - 1)) == 0);
+ 
+  if (Pages == 0) {
+    return NULL;
+  }
+  if (Alignment > EFI_PAGE_SIZE) {
+    //
+    // Calculate the total number of pages since alignment is larger than page size.
+    //
+    AlignmentMask  = Alignment - 1;
+    RealPages      = Pages + EFI_SIZE_TO_PAGES (Alignment);
+    //
+    // Make sure that Pages plus EFI_SIZE_TO_PAGES (Alignment) does not overflow.
+    //
+    ASSERT (RealPages > Pages);
+ 
+    Status         = SmmAllocatePages (AllocateAnyPages, MemoryType, RealPages, &Memory);
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+    AlignedMemory  = ((UINTN) Memory + AlignmentMask) & ~AlignmentMask;
+    UnalignedPages = EFI_SIZE_TO_PAGES (AlignedMemory - (UINTN) Memory);
+    if (UnalignedPages > 0) {
+      //
+      // Free first unaligned page(s).
+      //
+      Status = SmmFreePages (Memory, UnalignedPages);
+      ASSERT_EFI_ERROR (Status);
+    }
+    Memory         = (EFI_PHYSICAL_ADDRESS) (AlignedMemory + EFI_PAGES_TO_SIZE (Pages));
+    UnalignedPages = RealPages - Pages - UnalignedPages;
+    if (UnalignedPages > 0) {
+      //
+      // Free last unaligned page(s).
+      //
+      Status = SmmFreePages (Memory, UnalignedPages);
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    //
+    // Do not over-allocate pages in this case.
+    //
+    Status = SmmAllocatePages (AllocateAnyPages, MemoryType, Pages, &Memory);
+    if (EFI_ERROR (Status)) {
+      return NULL;
+    }
+    AlignedMemory  = (UINTN) Memory;
+  }
+  return (VOID *) AlignedMemory;
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiRuntimeServicesData with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedPages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return InternalAllocateAlignedPages (EfiRuntimeServicesData, Pages, Alignment);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiRuntimeServicesData at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiRuntimeServicesData with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedRuntimePages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return InternalAllocateAlignedPages (EfiRuntimeServicesData, Pages, Alignment);
+}
+
+/**
+  Allocates one or more 4KB pages of type EfiReservedMemoryType at a specified alignment.
+
+  Allocates the number of 4KB pages specified by Pages of type EfiReservedMemoryType with an
+  alignment specified by Alignment.  The allocated buffer is returned.  If Pages is 0, then NULL is
+  returned.  If there is not enough memory at the specified alignment remaining to satisfy the
+  request, then NULL is returned.
+  
+  If Alignment is not a power of two and Alignment is not zero, then ASSERT().
+  If Pages plus EFI_SIZE_TO_PAGES (Alignment) overflows, then ASSERT().
+
+  @param  Pages                 The number of 4 KB pages to allocate.
+  @param  Alignment             The requested alignment of the allocation.  Must be a power of two.
+                                If Alignment is zero, then byte alignment is used.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateAlignedReservedPages (
+  IN UINTN  Pages,
+  IN UINTN  Alignment
+  )
+{
+  return NULL;
+}
+
+/**
+  Frees one or more 4KB pages that were previously allocated with one of the aligned page
+  allocation functions in the Memory Allocation Library.
+
+  Frees the number of 4KB pages specified by Pages from the buffer specified by Buffer.  Buffer
+  must have been allocated on a previous call to the aligned page allocation services of the Memory
+  Allocation Library.  If it is not possible to free allocated pages, then this function will 
+  perform no actions.
+  
+  If Buffer was not allocated with an aligned page allocation function in the Memory Allocation
+  Library, then ASSERT().
+  If Pages is zero, then ASSERT().
+  
+  @param  Buffer                Pointer to the buffer of pages to free.
+  @param  Pages                 The number of 4 KB pages to free.
+
+**/
+VOID
+EFIAPI
+FreeAlignedPages (
+  IN VOID   *Buffer,
+  IN UINTN  Pages
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Pages != 0);
+  if (BufferInSmram (Buffer)) {
+    //
+    // When Buffer is in SMRAM range, it should be allocated by SmmAllocatePages() service.
+    // So, SmmFreePages() service is used to free it.
+    //
+    Status = SmmFreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  } else {
+    //
+    // When Buffer is out of SMRAM range, it should be allocated by gBS->AllocatePages() service.
+    // So, gBS->FreePages() service is used to free it.
+    //
+    Status = gBS->FreePages ((EFI_PHYSICAL_ADDRESS) (UINTN) Buffer, Pages);
+  }
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Allocates a buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type and returns a
+  pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocatePool (
+  IN EFI_MEMORY_TYPE  MemoryType,  
+  IN UINTN            AllocationSize
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Memory;
+
+  Memory = NULL;
+
+  Status = SmmAllocatePool (MemoryType, AllocationSize, &Memory);
+  if (EFI_ERROR (Status)) {
+    Memory = NULL;
+  }
+  return Memory;
+}
+
+/**
+  Allocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData and returns a
+  pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocatePool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocatePool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData and returns
+  a pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimePool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocatePool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates a buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType and returns
+  a pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return NULL;
+}
+
+/**
+  Allocates and zeros a buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type, clears the buffer
+  with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a valid
+  buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the request,
+  then NULL is returned.
+
+  @param  PoolType              The type of memory to allocate.
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateZeroPool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            AllocationSize
+  ) 
+{
+  VOID  *Memory;
+
+  Memory = InternalAllocatePool (PoolType, AllocationSize);
+  if (Memory != NULL) {
+    Memory = ZeroMem (Memory, AllocationSize);
+  }
+  return Memory;
+}
+
+/**
+  Allocates and zeros a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocateZeroPool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates and zeros a buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimeZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return InternalAllocateZeroPool (EfiRuntimeServicesData, AllocationSize);
+}
+
+/**
+  Allocates and zeros a buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedZeroPool (
+  IN UINTN  AllocationSize
+  )
+{
+  return NULL;
+}
+
+/**
+  Copies a buffer to an allocated buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  PoolType              The type of pool to allocate.
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalAllocateCopyPool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            AllocationSize,
+  IN CONST VOID       *Buffer
+  ) 
+{
+  VOID  *Memory;
+
+  ASSERT (Buffer != NULL);
+  ASSERT (AllocationSize <= (MAX_ADDRESS - (UINTN) Buffer + 1));
+
+  Memory = InternalAllocatePool (PoolType, AllocationSize);
+  if (Memory != NULL) {
+     Memory = CopyMem (Memory, Buffer, AllocationSize);
+  }
+  return Memory;
+} 
+
+/**
+  Copies a buffer to an allocated buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return InternalAllocateCopyPool (EfiRuntimeServicesData, AllocationSize, Buffer);
+}
+
+/**
+  Copies a buffer to an allocated buffer of type EfiRuntimeServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiRuntimeServicesData, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateRuntimeCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return InternalAllocateCopyPool (EfiRuntimeServicesData, AllocationSize, Buffer);
+}
+
+/**
+  Copies a buffer to an allocated buffer of type EfiReservedMemoryType.
+
+  Allocates the number bytes specified by AllocationSize of type EfiReservedMemoryType, copies
+  AllocationSize bytes from Buffer to the newly allocated buffer, and returns a pointer to the
+  allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is returned.  If there
+  is not enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If Buffer is NULL, then ASSERT().
+  If AllocationSize is greater than (MAX_ADDRESS - Buffer + 1), then ASSERT(). 
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+  @param  Buffer                The buffer to copy to the allocated buffer.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+AllocateReservedCopyPool (
+  IN UINTN       AllocationSize,
+  IN CONST VOID  *Buffer
+  )
+{
+  return NULL;
+}
+
+/**
+  Reallocates a buffer of a specified memory type.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of the type
+  specified by PoolType.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  PoolType       The type of pool to allocate.
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalReallocatePool (
+  IN EFI_MEMORY_TYPE  PoolType,  
+  IN UINTN            OldSize,
+  IN UINTN            NewSize,
+  IN VOID             *OldBuffer  OPTIONAL
+  )
+{
+  VOID  *NewBuffer;
+
+  NewBuffer = InternalAllocateZeroPool (PoolType, NewSize);
+  if (NewBuffer != NULL && OldBuffer != NULL) {
+    CopyMem (NewBuffer, OldBuffer, MIN (OldSize, NewSize));
+    FreePool (OldBuffer);
+  }
+  return NewBuffer;
+}
+
+/**
+  Reallocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiRuntimeServicesData.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+  
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocatePool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return InternalReallocatePool (EfiRuntimeServicesData, OldSize, NewSize, OldBuffer);
+}
+
+/**
+  Reallocates a buffer of type EfiRuntimeServicesData.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiRuntimeServicesData.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocateRuntimePool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return InternalReallocatePool (EfiRuntimeServicesData, OldSize, NewSize, OldBuffer);
+}
+
+/**
+  Reallocates a buffer of type EfiReservedMemoryType.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiReservedMemoryType.  If OldBuffer is not NULL, then the smaller of OldSize and 
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and 
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.  
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not 
+  enough memory remaining to satisfy the request, then NULL is returned.
+
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional 
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+EFIAPI
+ReallocateReservedPool (
+  IN UINTN  OldSize,
+  IN UINTN  NewSize,
+  IN VOID   *OldBuffer  OPTIONAL
+  )
+{
+  return NULL;
+}
+
+/**
+  Frees a buffer that was previously allocated with one of the pool allocation functions in the
+  Memory Allocation Library.
+
+  Frees the buffer specified by Buffer.  Buffer must have been allocated on a previous call to the
+  pool allocation services of the Memory Allocation Library.  If it is not possible to free pool
+  resources, then this function will perform no actions.
+  
+  If Buffer was not allocated with a pool allocation function in the Memory Allocation Library,
+  then ASSERT().
+
+  @param  Buffer                Pointer to the buffer to free.
+
+**/
+VOID
+EFIAPI
+FreePool (
+  IN VOID   *Buffer
+  )
+{
+  EFI_STATUS    Status;
+
+  if (BufferInSmram (Buffer)) {
+    //
+    // When Buffer is in SMRAM range, it should be allocated by SmmAllocatePool() service.
+    // So, SmmFreePool() service is used to free it.
+    //
+    Status = SmmFreePool (Buffer);
+  } else {
+    //
+    // When Buffer is out of SMRAM range, it should be allocated by gBS->AllocatePool() service.
+    // So, gBS->FreePool() service is used to free it.
+    //
+    Status = gBS->FreePool (Buffer);
+  }
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  The constructor function calls SmmInitializeMemoryServices to initialize memory in SMRAM.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+PiSmmCoreMemoryAllocationLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  SMM_CORE_PRIVATE_DATA  *SmmCorePrivate;
+  UINTN                  Size;
+
+  SmmCorePrivate = (SMM_CORE_PRIVATE_DATA *)ImageHandle;
+  //
+  // Initialize memory service using free SMRAM
+  //
+  SmmInitializeMemoryServices (SmmCorePrivate->SmramRangeCount, SmmCorePrivate->SmramRanges);
+
+  mSmmCoreMemoryAllocLibSmramRangeCount = SmmCorePrivate->SmramRangeCount;
+  Size = mSmmCoreMemoryAllocLibSmramRangeCount * sizeof (EFI_SMRAM_DESCRIPTOR);
+  mSmmCoreMemoryAllocLibSmramRanges = (EFI_SMRAM_DESCRIPTOR *) AllocatePool (Size);
+  ASSERT (mSmmCoreMemoryAllocLibSmramRanges != NULL);
+  CopyMem (mSmmCoreMemoryAllocLibSmramRanges, SmmCorePrivate->SmramRanges, Size);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.inf b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.inf
new file mode 100644
index 0000000000..e8f7081149
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.inf
@@ -0,0 +1,48 @@
+## @file
+# Memory Allocation Library instance dedicated to SMM Core.
+# The implementation borrows the SMM Core Memory Allocation services as the primitive
+# for memory allocation instead of using SMM System Table servces in an indirect way.
+# It is assumed that this library instance must be linked with SMM Cre in this package. 
+#
+# Copyright (c) 2010 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PiSmmCoreMemoryAllocationLib
+  MODULE_UNI_FILE                = PiSmmCoreMemoryAllocationLib.uni
+  FILE_GUID                      = B618E089-9ABA-4d97-AE80-57B5BCCDA51D
+  MODULE_TYPE                    = SMM_CORE
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  LIBRARY_CLASS                  = MemoryAllocationLib|SMM_CORE
+  CONSTRUCTOR                    = PiSmmCoreMemoryAllocationLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  MemoryAllocationLib.c
+  PiSmmCoreMemoryAllocationServices.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  DebugLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+
+[Protocols]
+  gEfiSmmAccess2ProtocolGuid    ## CONSUMES  
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.uni b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.uni
new file mode 100644
index 0000000000..adfc0381c1
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.uni
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationServices.h b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationServices.h
new file mode 100644
index 0000000000..a2b89acf5d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationServices.h
@@ -0,0 +1,191 @@
+/** @file
+  Contains function prototypes for Memory Services in the SMM Core.
+
+  This header file borrows the PiSmmCore Memory Allocation services as the primitive
+  for memory allocation. 
+
+  Copyright (c) 2008 - 2015, 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.             
+
+**/
+
+#ifndef _PI_SMM_CORE_MEMORY_ALLOCATION_SERVICES_H_
+#define _PI_SMM_CORE_MEMORY_ALLOCATION_SERVICES_H_
+
+//
+// It should be aligned with the definition in PiSmmCore.
+//
+typedef struct {
+  UINTN                           Signature;
+
+  ///
+  /// The ImageHandle passed into the entry point of the SMM IPL.  This ImageHandle
+  /// is used by the SMM Core to fill in the ParentImageHandle field of the Loaded
+  /// Image Protocol for each SMM Driver that is dispatched by the SMM Core.
+  ///
+  EFI_HANDLE                      SmmIplImageHandle;
+
+  ///
+  /// The number of SMRAM ranges passed from the SMM IPL to the SMM Core.  The SMM
+  /// Core uses these ranges of SMRAM to initialize the SMM Core memory manager.
+  ///
+  UINTN                           SmramRangeCount;
+
+  ///
+  /// A table of SMRAM ranges passed from the SMM IPL to the SMM Core.  The SMM
+  /// Core uses these ranges of SMRAM to initialize the SMM Core memory manager.
+  ///
+  EFI_SMRAM_DESCRIPTOR            *SmramRanges;
+
+  ///
+  /// The SMM Foundation Entry Point.  The SMM Core fills in this field when the 
+  /// SMM Core is initialized.  The SMM IPL is responsbile for registering this entry 
+  /// point with the SMM Configuration Protocol.  The SMM Configuration Protocol may 
+  /// not be available at the time the SMM IPL and SMM Core are started, so the SMM IPL
+  /// sets up a protocol notification on the SMM Configuration Protocol and registers 
+  /// the SMM Foundation Entry Point as soon as the SMM Configuration Protocol is 
+  /// available.
+  ///
+  EFI_SMM_ENTRY_POINT             SmmEntryPoint;
+  
+  ///
+  /// Boolean flag set to TRUE while an SMI is being processed by the SMM Core.
+  /// 
+  BOOLEAN                         SmmEntryPointRegistered;
+
+  ///
+  /// Boolean flag set to TRUE while an SMI is being processed by the SMM Core.
+  /// 
+  BOOLEAN                         InSmm;
+
+  ///
+  /// This field is set by the SMM Core then the SMM Core is initialized.  This field is
+  /// used by the SMM Base 2 Protocol and SMM Communication Protocol implementations in
+  /// the SMM IPL.  
+  ///
+  EFI_SMM_SYSTEM_TABLE2           *Smst;
+
+  ///
+  /// This field is used by the SMM Communicatioon Protocol to pass a buffer into 
+  /// a software SMI handler and for the software SMI handler to pass a buffer back to
+  /// the caller of the SMM Communication Protocol.  
+  ///
+  VOID                            *CommunicationBuffer;
+
+  ///
+  /// This field is used by the SMM Communicatioon Protocol to pass the size of a buffer,
+  /// in bytes, into a software SMI handler and for the software SMI handler to pass the 
+  /// size, in bytes, of a buffer back to the caller of the SMM Communication Protocol.
+  ///
+  UINTN                           BufferSize;
+
+  ///
+  /// This field is used by the SMM Communication Protocol to pass the return status from
+  /// a software SMI handler back to the caller of the SMM Communication Protocol.
+  ///
+  EFI_STATUS                      ReturnStatus;
+
+  EFI_PHYSICAL_ADDRESS            PiSmmCoreImageBase;
+  UINT64                          PiSmmCoreImageSize;
+  EFI_PHYSICAL_ADDRESS            PiSmmCoreEntryPoint;
+} SMM_CORE_PRIVATE_DATA;
+
+/**
+  Called to initialize the memory service.
+
+  @param   SmramRangeCount       Number of SMRAM Regions
+  @param   SmramRanges           Pointer to SMRAM Descriptors
+
+**/
+VOID
+SmmInitializeMemoryServices (
+  IN UINTN                 SmramRangeCount,
+  IN EFI_SMRAM_DESCRIPTOR  *SmramRanges
+  );
+
+/**
+  Allocates pages from the memory map.
+
+  @param  Type                   The type of allocation to perform
+  @param  MemoryType             The type of memory to turn the allocated pages
+                                 into
+  @param  NumberOfPages          The number of pages to allocate
+  @param  Memory                 A pointer to receive the base allocated memory
+                                 address
+
+  @retval EFI_INVALID_PARAMETER  Parameters violate checking rules defined in spec.
+  @retval EFI_NOT_FOUND          Could not allocate pages match the requirement.
+  @retval EFI_OUT_OF_RESOURCES   No enough pages to allocate.
+  @retval EFI_SUCCESS            Pages successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePages (
+  IN      EFI_ALLOCATE_TYPE         Type,
+  IN      EFI_MEMORY_TYPE           MemoryType,
+  IN      UINTN                     NumberOfPages,
+  OUT     EFI_PHYSICAL_ADDRESS      *Memory
+  );
+
+/**
+  Frees previous allocated pages.
+
+  @param  Memory                 Base address of memory being freed
+  @param  NumberOfPages          The number of pages to free
+
+  @retval EFI_NOT_FOUND          Could not find the entry that covers the range
+  @retval EFI_INVALID_PARAMETER  Address not aligned
+  @return EFI_SUCCESS            Pages successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePages (
+  IN      EFI_PHYSICAL_ADDRESS      Memory,
+  IN      UINTN                     NumberOfPages
+  );
+
+/**
+  Allocate pool of a particular type.
+
+  @param  PoolType               Type of pool to allocate
+  @param  Size                   The amount of pool to allocate
+  @param  Buffer                 The address to return a pointer to the allocated
+                                 pool
+
+  @retval EFI_INVALID_PARAMETER  PoolType not valid
+  @retval EFI_OUT_OF_RESOURCES   Size exceeds max pool size or allocation failed.
+  @retval EFI_SUCCESS            Pool successfully allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmAllocatePool (
+  IN      EFI_MEMORY_TYPE           PoolType,
+  IN      UINTN                     Size,
+  OUT     VOID                      **Buffer
+  );
+
+/**
+  Frees pool.
+
+  @param  Buffer                 The allocated pool entry to free
+
+  @retval EFI_INVALID_PARAMETER  Buffer is not a valid value.
+  @retval EFI_SUCCESS            Pool successfully freed.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFreePool (
+  IN      VOID                      *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.c b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.c
new file mode 100644
index 0000000000..788fafae35
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.c
@@ -0,0 +1,60 @@
+/** @file
+  SMM Core SMM Services Table Library.
+
+  Copyright (c) 2010 - 2014, 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 <PiSmm.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/DebugLib.h>
+
+EFI_SMM_SYSTEM_TABLE2         *gSmst = NULL;
+extern EFI_SMM_SYSTEM_TABLE2  gSmmCoreSmst;
+
+/**
+  The constructor function caches the pointer of SMM Services Table.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCoreSmmServicesTableLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  gSmst = &gSmmCoreSmst;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function allows the caller to determine if the driver is executing in 
+  System Management Mode(SMM).
+
+  This function returns TRUE if the driver is executing in SMM and FALSE if the 
+  driver is not executing in SMM.
+
+  @retval  TRUE  The driver is executing in System Management Mode (SMM).
+  @retval  FALSE The driver is not executing in System Management Mode (SMM). 
+
+**/
+BOOLEAN
+EFIAPI
+InSmm (
+  VOID
+  )
+{
+  return TRUE;
+}
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.inf b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.inf
new file mode 100644
index 0000000000..80d523c71b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.inf
@@ -0,0 +1,36 @@
+## @file
+# SMM Core SMM Services Table Library.
+#
+# Copyright (c) 2010 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PiSmmCoreSmmServicesTableLib
+  MODULE_UNI_FILE                = PiSmmCoreSmmServicesTableLib.uni
+  FILE_GUID                      = C427146A-2EF2-4af9-A85A-E09EA65EE47D
+  MODULE_TYPE                    = SMM_CORE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SmmServicesTableLib|SMM_CORE
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  CONSTRUCTOR                    = SmmCoreSmmServicesTableLibConstructor
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  PiSmmCoreSmmServicesTableLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
diff --git a/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.uni b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.uni
new file mode 100644
index 0000000000..3e0854c169
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.uni
diff --git a/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManager.c b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManager.c
new file mode 100644
index 0000000000..1390e19097
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManager.c
@@ -0,0 +1,67 @@
+/** @file
+  This file include all platform action which can be customized
+  by IBV/OEM.
+
+Copyright (c) 2012 - 2015, 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 <Library/PlatformBootManagerLib.h>
+
+
+/**
+  Do the platform specific action before the console is connected.
+
+  Such as:
+    Update console variable;
+    Register new Driver#### or Boot####;
+    Signal ReadyToLock event.
+**/
+VOID
+EFIAPI
+PlatformBootManagerBeforeConsole (
+  VOID
+  )
+{
+  return;
+}
+
+/**
+  Do the platform specific action after the console is connected.
+
+  Such as:
+    Dynamically switch output mode;
+    Signal console ready platform customized event;
+    Run diagnostics like memory testing;
+    Connect certain devices;
+    Dispatch aditional option roms.
+**/
+VOID
+EFIAPI
+PlatformBootManagerAfterConsole (
+  VOID
+  )
+{
+  return;
+}
+
+/**
+  This function is called each second during the boot manager waits the timeout.
+
+  @param TimeoutRemain  The remaining timeout.
+**/
+VOID
+EFIAPI
+PlatformBootManagerWaitCallback (
+  UINT16          TimeoutRemain
+  )
+{
+  return;
+}
diff --git a/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.inf b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.inf
new file mode 100644
index 0000000000..fb756ca2ff
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.inf
@@ -0,0 +1,37 @@
+## @file
+#  Include all platform action which can be customized by IBV/OEM.
+#
+#  Copyright (c) 2012 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PlatformBootManagerLib
+  MODULE_UNI_FILE                = PlatformBootManagerLibNull.uni
+  FILE_GUID                      = 95C097CC-8943-4038-BB8A-1C70CF2E9F3C
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PlatformBootManagerLib|DXE_DRIVER
+
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 EBC
+#
+
+[Sources]
+  PlatformBootManager.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.uni b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.uni
new file mode 100644
index 0000000000..329790afb0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.uni
diff --git a/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.c b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.c
new file mode 100644
index 0000000000..00b70c79e6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.c
@@ -0,0 +1,36 @@
+/** @file
+  Null Platform Hook Library instance with dependency on gPeiSerialPortPpiGuid
+
+  Copyright (c) 2010 - 2014, 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 <Base.h>
+#include <Library/PlatformHookLib.h>
+
+/**
+  Performs platform specific initialization required for the CPU to access
+  the hardware associated with a SerialPortLib instance.  This function does
+  not initialize the serial port hardware itself.  Instead, it initializes
+  hardware devices that are required for the CPU to access the serial port
+  hardware.  This function may be called more than once.
+
+  @retval RETURN_SUCCESS       The platform specific initialization succeeded.
+  @retval RETURN_DEVICE_ERROR  The platform specific initialization could not be completed.
+
+**/
+RETURN_STATUS
+EFIAPI
+PlatformHookSerialPortInitialize (
+  VOID
+  )
+{
+  return RETURN_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.inf b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.inf
new file mode 100644
index 0000000000..09074a8a4e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.inf
@@ -0,0 +1,38 @@
+## @file
+#  Null Platform Hook Library instance with dependency on gPeiSerialPortPpiGuid
+#
+#  Copyright (c) 2010 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PlatformHookLibSerialPortPpi
+  FILE_GUID                      = 621734D8-8B5E-4c01-B330-9F89A1081710
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PlatformHookLib
+  MODULE_UNI_FILE                = PlatformHookLibSerialPortPpi.uni
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PlatformHookLibSerialPortPpi.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[Depex.common.PEIM]
+  gPeiSerialPortPpiGuid
diff --git a/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.uni b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.uni
new file mode 100644
index 0000000000..d88a279f8e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.uni
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.h b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.h
new file mode 100644
index 0000000000..5290aae158
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.h
@@ -0,0 +1,108 @@
+/** @file
+  Include file for platform variable cleanup.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _PLAT_VAR_CLEANUP_
+#define _PLAT_VAR_CLEANUP_
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PrintLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/HiiLib.h>
+#include <Library/PlatformVarCleanupLib.h>
+
+#include <Protocol/Variable.h>
+#include <Protocol/VarCheck.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/DevicePath.h>
+
+#include <Guid/EventGroup.h>
+#include <Guid/MdeModuleHii.h>
+#include <Guid/ImageAuthentication.h>
+#include <Guid/VarErrorFlag.h>
+
+#include "PlatVarCleanupHii.h"
+
+//
+// This is the generated IFR binary data for each formset defined in VFR.
+// This data array is ready to be used as input of HiiAddPackages() to
+// create a packagelist (which contains Form packages, String packages, etc).
+//
+extern UINT8 PlatVarCleanupBin[];
+
+//
+// This is the generated String package data for all .UNI files.
+// This data array is ready to be used as input of HiiAddPackages() to
+// create a packagelist (which contains Form packages, String packages, etc).
+//
+extern UINT8 PlatformVarCleanupLibStrings[];
+
+#define USER_VARIABLE_NODE_SIGNATURE SIGNATURE_32 ('U', 'V', 'N', 'S')
+
+typedef struct {
+  UINTN             Signature;
+  LIST_ENTRY        Link;
+  EFI_GUID          Guid;
+  CHAR16            *PromptString;
+  LIST_ENTRY        NameLink;
+} USER_VARIABLE_NODE;
+
+#define USER_VARIABLE_FROM_LINK(a) CR (a, USER_VARIABLE_NODE, Link, USER_VARIABLE_NODE_SIGNATURE)
+
+#define USER_VARIABLE_NAME_NODE_SIGNATURE SIGNATURE_32 ('U', 'V', 'N', 'N')
+
+typedef struct {
+  UINTN             Signature;
+  LIST_ENTRY        Link;
+  CHAR16            *Name;
+  UINTN             DataSize;
+  UINT32            Attributes;
+  UINT16            Index;
+  EFI_QUESTION_ID   QuestionId;
+  CHAR16            *PromptString;
+  CHAR16            *HelpString;
+  BOOLEAN           Deleted;
+} USER_VARIABLE_NAME_NODE;
+
+#define USER_VARIABLE_NAME_FROM_LINK(a) CR (a, USER_VARIABLE_NAME_NODE, Link, USER_VARIABLE_NAME_NODE_SIGNATURE)
+
+#pragma pack(1)
+//
+// HII specific Vendor Device Path definition.
+//
+typedef struct {
+  VENDOR_DEVICE_PATH            VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL      End;
+} HII_VENDOR_DEVICE_PATH;
+#pragma pack()
+
+#define VARIABLE_CLEANUP_HII_PRIVATE_SIGNATURE SIGNATURE_32 ('V', 'C', 'H', 'P')
+
+typedef struct {
+  UINTN                             Signature;
+  EFI_HANDLE                        DriverHandle;
+  EFI_HII_HANDLE                    HiiHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL    ConfigAccess;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL   *ConfigRouting;
+  VARIABLE_CLEANUP_DATA             VariableCleanupData;
+} VARIABLE_CLEANUP_HII_PRIVATE_DATA;
+
+#define VARIABLE_CLEANUP_HII_PRIVATE_FROM_THIS(a) CR (a, VARIABLE_CLEANUP_HII_PRIVATE_DATA, ConfigAccess, VARIABLE_CLEANUP_HII_PRIVATE_SIGNATURE)
+
+#endif
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.vfr b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.vfr
new file mode 100644
index 0000000000..7dfeafa2c4
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanup.vfr
@@ -0,0 +1,41 @@
+/** @file
+ Platform variable cleanup Formset.
+
+Copyright (c) 2015, 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 "PlatVarCleanupHii.h"
+
+formset
+  guid     = VARIABLE_CLEANUP_HII_GUID,
+  title    = STRING_TOKEN(STR_ENTRY_TITLE),
+  help     = STRING_TOKEN(STR_TITLE_HELP),
+
+  varstore VARIABLE_CLEANUP_DATA,
+    varid = VARIABLE_CLEANUP_VARSTORE_ID,
+    name  = VariableCleanup,
+    guid  = VARIABLE_CLEANUP_HII_GUID;
+
+  form formid = FORM_ID_VARIABLE_CLEANUP,
+    title = STRING_TOKEN(STR_TITLE);
+
+    checkbox varid = VARIABLE_CLEANUP_DATA.SelectAll,
+      prompt   = STRING_TOKEN(STR_SELECT_ALL_PROMPT),
+      help     = STRING_TOKEN(STR_SELECT_ALL_HELP),
+      flags    = INTERACTIVE,
+      key      = SELECT_ALL_QUESTION_ID,
+    endcheckbox;
+
+    label LABEL_START;
+    label LABEL_END;
+
+  endform;
+endformset;
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupHii.h b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupHii.h
new file mode 100644
index 0000000000..02af877b44
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupHii.h
@@ -0,0 +1,59 @@
+/** @file
+  Include file for platform variable cleanup HII.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _PLAT_VAR_CLEANUP_HII_
+#define _PLAT_VAR_CLEANUP_HII_
+
+//
+// {24F14D8A-D7A8-4991-91E0-96C3B7DB8456}
+//
+#define VARIABLE_CLEANUP_HII_GUID \
+  { \
+    0x24f14d8a, 0xd7a8, 0x4991, { 0x91, 0xe0, 0x96, 0xc3, 0xb7, 0xdb, 0x84, 0x56 } \
+  }
+
+#define MAX_USER_VARIABLE_COUNT         0x1000
+
+typedef struct {
+  UINT8   SelectAll;
+  //
+  // FALSE is to not delete, TRUE is to delete.
+  //
+  UINT8   UserVariable[MAX_USER_VARIABLE_COUNT];
+} VARIABLE_CLEANUP_DATA;
+
+#define VARIABLE_CLEANUP_VARSTORE_ID    0x8000
+
+//
+// Field offset of structure VARIABLE_CLEANUP_DATA
+//
+#define VAR_OFFSET(Field)               ((UINTN) &(((VARIABLE_CLEANUP_DATA *) 0)->Field))
+#define USER_VARIABLE_VAR_OFFSET        (VAR_OFFSET (UserVariable))
+
+#define FORM_ID_VARIABLE_CLEANUP        0x8000
+
+#define LABEL_START                     0x0000
+#define LABEL_END                       0xFFFF
+
+#define SELECT_ALL_QUESTION_ID          0x7FFD
+#define SAVE_AND_EXIT_QUESTION_ID       0x7FFE
+#define NO_SAVE_AND_EXIT_QUESTION_ID    0x7FFF
+
+//
+// Tool automatic generated Question Id start from 1.
+// In order to avoid to conflict them, the user variable QuestionID offset is defined from 0x8000.
+//
+#define USER_VARIABLE_QUESTION_ID       0x8000
+
+#endif
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupLib.c b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupLib.c
new file mode 100644
index 0000000000..d99174c4b9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatVarCleanupLib.c
@@ -0,0 +1,1250 @@
+/** @file
+  Sample platform variable cleanup library implementation.
+
+Copyright (c) 2015, 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 "PlatVarCleanup.h"
+
+VAR_ERROR_FLAG              mLastVarErrorFlag = VAR_ERROR_FLAG_NO_ERROR;
+EDKII_VAR_CHECK_PROTOCOL    *mVarCheck = NULL;
+
+///
+/// The flag to indicate whether the platform has left the DXE phase of execution.
+///
+BOOLEAN                     mEndOfDxe = FALSE;
+
+LIST_ENTRY                  mUserVariableList = INITIALIZE_LIST_HEAD_VARIABLE (mUserVariableList);
+UINT16                      mUserVariableCount = 0;
+UINT16                      mMarkedUserVariableCount = 0;
+
+EFI_GUID                    mVariableCleanupHiiGuid = VARIABLE_CLEANUP_HII_GUID;
+CHAR16                      mVarStoreName[] = L"VariableCleanup";
+
+HII_VENDOR_DEVICE_PATH      mVarCleanupHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    VARIABLE_CLEANUP_HII_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      (UINT8) (sizeof (EFI_DEVICE_PATH_PROTOCOL)),
+      (UINT8) ((sizeof (EFI_DEVICE_PATH_PROTOCOL)) >> 8)
+    }
+  }
+};
+
+/**
+  Internal get variable error flag.
+
+  @return   Variable error flag.
+
+**/
+VAR_ERROR_FLAG
+InternalGetVarErrorFlag (
+  VOID
+  )
+{
+  EFI_STATUS        Status;
+  UINTN             Size;
+  VAR_ERROR_FLAG    ErrorFlag;
+
+  Size = sizeof (ErrorFlag);
+  Status = gRT->GetVariable (
+                  VAR_ERROR_FLAG_NAME,
+                  &gEdkiiVarErrorFlagGuid,
+                  NULL,
+                  &Size,
+                  &ErrorFlag
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "%s - not found\n", VAR_ERROR_FLAG_NAME));
+    return VAR_ERROR_FLAG_NO_ERROR;
+  }
+  return ErrorFlag;
+}
+
+/**
+  Is user variable?
+
+  @param[in] Name   Pointer to variable name.
+  @param[in] Guid   Pointer to vendor guid.
+
+  @retval TRUE      User variable.
+  @retval FALSE     System variable.
+
+**/
+BOOLEAN
+IsUserVariable (
+  IN CHAR16                     *Name,
+  IN EFI_GUID                   *Guid
+  )
+{
+  EFI_STATUS                    Status;
+  VAR_CHECK_VARIABLE_PROPERTY   Property;
+
+  ZeroMem (&Property, sizeof (Property));
+  Status = mVarCheck->VariablePropertyGet (
+                        Name,
+                        Guid,
+                        &Property
+                        );
+  if (EFI_ERROR (Status)) {
+    //
+    // No property, it is user variable.
+    //
+    DEBUG ((EFI_D_INFO, "PlatformVarCleanup - User variable: %g:%s\n", Guid, Name));
+    return TRUE;
+  }
+
+//  DEBUG ((EFI_D_INFO, "PlatformVarCleanup - Variable Property: %g:%s\n", Guid, Name));
+//  DEBUG ((EFI_D_INFO, "  Revision  - 0x%04x\n", Property.Revision));
+//  DEBUG ((EFI_D_INFO, "  Property  - 0x%04x\n", Property.Property));
+//  DEBUG ((EFI_D_INFO, "  Attribute - 0x%08x\n", Property.Attributes));
+//  DEBUG ((EFI_D_INFO, "  MinSize   - 0x%x\n", Property.MinSize));
+//  DEBUG ((EFI_D_INFO, "  MaxSize   - 0x%x\n", Property.MaxSize));
+
+  return FALSE;
+}
+
+/**
+  Find user variable node by variable GUID.
+
+  @param[in] Guid   Pointer to vendor guid.
+
+  @return Pointer to user variable node.
+
+**/
+USER_VARIABLE_NODE *
+FindUserVariableNodeByGuid (
+  IN EFI_GUID   *Guid
+  )
+{
+  USER_VARIABLE_NODE    *UserVariableNode;
+  LIST_ENTRY            *Link;
+
+  for (Link = mUserVariableList.ForwardLink
+       ;Link != &mUserVariableList
+       ;Link = Link->ForwardLink) {
+    UserVariableNode = USER_VARIABLE_FROM_LINK (Link);
+
+    if (CompareGuid (Guid, &UserVariableNode->Guid)) {
+      //
+      // Found it.
+      //
+      return UserVariableNode;
+    }
+  }
+
+  //
+  // Create new one if not found.
+  //
+  UserVariableNode = AllocateZeroPool (sizeof (*UserVariableNode));
+  ASSERT (UserVariableNode != NULL);
+  UserVariableNode->Signature = USER_VARIABLE_NODE_SIGNATURE;
+  CopyGuid (&UserVariableNode->Guid, Guid);
+  //
+  // (36 chars of "########-####-####-####-############" + 1 space + 1 terminator) * sizeof (CHAR16).
+  //
+  UserVariableNode->PromptString = AllocatePool ((36 + 2) * sizeof (CHAR16));
+  ASSERT (UserVariableNode->PromptString != NULL);
+  UnicodeSPrint (UserVariableNode->PromptString, (36 + 2) * sizeof (CHAR16), L" %g", &UserVariableNode->Guid);
+  InitializeListHead (&UserVariableNode->NameLink);
+  InsertTailList (&mUserVariableList, &UserVariableNode->Link);
+  return UserVariableNode;
+}
+
+/**
+  Create user variable node.
+
+**/
+VOID
+CreateUserVariableNode (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    GetVariableStatus;
+  CHAR16                        *VarName;
+  UINTN                         MaxVarNameSize;
+  UINTN                         VarNameSize;
+  UINTN                         MaxDataSize;
+  UINTN                         DataSize;
+  VOID                          *Data;
+  UINT32                        Attributes;
+  EFI_GUID                      Guid;
+  USER_VARIABLE_NODE            *UserVariableNode;
+  USER_VARIABLE_NAME_NODE       *UserVariableNameNode;
+  UINT16                        Index;
+  UINTN                         StringSize;
+
+  //
+  // Initialize 128 * sizeof (CHAR16) variable name size.
+  //
+  MaxVarNameSize = 128 * sizeof (CHAR16);
+  VarName = AllocateZeroPool (MaxVarNameSize);
+  ASSERT (VarName != NULL);
+
+  //
+  // Initialize 0x1000 variable data size.
+  //
+  MaxDataSize = 0x1000;
+  Data = AllocateZeroPool (MaxDataSize);
+  ASSERT (Data != NULL);
+
+  Index = 0;
+  do {
+    VarNameSize = MaxVarNameSize;
+    Status = gRT->GetNextVariableName (&VarNameSize, VarName, &Guid);
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      VarName = ReallocatePool (MaxVarNameSize, VarNameSize, VarName);
+      ASSERT (VarName != NULL);
+      MaxVarNameSize = VarNameSize;
+      Status = gRT->GetNextVariableName (&VarNameSize, VarName, &Guid);
+    }
+
+    if (!EFI_ERROR (Status)) {
+      if (IsUserVariable (VarName, &Guid)) {
+        DataSize = MaxDataSize;
+        GetVariableStatus = gRT->GetVariable (VarName, &Guid, &Attributes, &DataSize, Data);
+        if (GetVariableStatus == EFI_BUFFER_TOO_SMALL) {
+          Data = ReallocatePool (MaxDataSize, DataSize, Data);
+          ASSERT (Data != NULL);
+          MaxDataSize = DataSize;
+          GetVariableStatus = gRT->GetVariable (VarName, &Guid, &Attributes, &DataSize, Data);
+        }
+        ASSERT_EFI_ERROR (GetVariableStatus);
+
+        if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+          UserVariableNode = FindUserVariableNodeByGuid (&Guid);
+          ASSERT (UserVariableNode != NULL);
+
+          //
+          // Different variables that have same variable GUID share same user variable node.
+          //
+          UserVariableNameNode = AllocateZeroPool (sizeof (*UserVariableNameNode));
+          ASSERT (UserVariableNameNode != NULL);
+          UserVariableNameNode->Signature = USER_VARIABLE_NAME_NODE_SIGNATURE;
+          UserVariableNameNode->Name = AllocateCopyPool (VarNameSize, VarName);
+          UserVariableNameNode->Attributes = Attributes;
+          UserVariableNameNode->DataSize = DataSize;
+          UserVariableNameNode->Index = Index;
+          UserVariableNameNode->QuestionId = (EFI_QUESTION_ID) (USER_VARIABLE_QUESTION_ID + Index);
+          //
+          // 2 space * sizeof (CHAR16) + StrSize.
+          //
+          StringSize = 2 * sizeof (CHAR16) + StrSize (UserVariableNameNode->Name);
+          UserVariableNameNode->PromptString = AllocatePool (StringSize);
+          ASSERT (UserVariableNameNode->PromptString != NULL);
+          UnicodeSPrint (UserVariableNameNode->PromptString, StringSize, L"  %s", UserVariableNameNode->Name);
+          //
+          // (33 chars of "Attribtues = 0x and DataSize = 0x" + 1 terminator + (sizeof (UINT32) + sizeof (UINTN)) * 2) * sizeof (CHAR16).
+          //
+          StringSize = (33 + 1 + (sizeof (UINT32) + sizeof (UINTN)) * 2) * sizeof (CHAR16);
+          UserVariableNameNode->HelpString = AllocatePool (StringSize);
+          ASSERT (UserVariableNameNode->HelpString != NULL);
+          UnicodeSPrint (UserVariableNameNode->HelpString, StringSize, L"Attribtues = 0x%08x and DataSize = 0x%x", UserVariableNameNode->Attributes, UserVariableNameNode->DataSize);
+          UserVariableNameNode->Deleted = FALSE;
+          InsertTailList (&UserVariableNode->NameLink, &UserVariableNameNode->Link);
+          Index++;
+        }
+      }
+    }
+  } while (Status != EFI_NOT_FOUND);
+
+  mUserVariableCount = Index;
+  ASSERT (mUserVariableCount <= MAX_USER_VARIABLE_COUNT);
+  DEBUG ((EFI_D_INFO, "PlatformVarCleanup - User variable count: 0x%04x\n", mUserVariableCount));
+
+  FreePool (VarName);
+  FreePool (Data);
+}
+
+/**
+  Destroy user variable nodes.
+
+**/
+VOID
+DestroyUserVariableNode (
+  VOID
+  )
+{
+  USER_VARIABLE_NODE        *UserVariableNode;
+  LIST_ENTRY                *Link;
+  USER_VARIABLE_NAME_NODE   *UserVariableNameNode;
+  LIST_ENTRY                *NameLink;
+
+  while (mUserVariableList.ForwardLink != &mUserVariableList) {
+    Link = mUserVariableList.ForwardLink;
+    UserVariableNode = USER_VARIABLE_FROM_LINK (Link);
+
+    RemoveEntryList (&UserVariableNode->Link);
+
+    while (UserVariableNode->NameLink.ForwardLink != &UserVariableNode->NameLink) {
+      NameLink = UserVariableNode->NameLink.ForwardLink;
+      UserVariableNameNode = USER_VARIABLE_NAME_FROM_LINK (NameLink);
+
+      RemoveEntryList (&UserVariableNameNode->Link);
+
+      FreePool (UserVariableNameNode->Name);
+      FreePool (UserVariableNameNode->PromptString);
+      FreePool (UserVariableNameNode->HelpString);
+      FreePool (UserVariableNameNode);
+    }
+
+    FreePool (UserVariableNode->PromptString);
+    FreePool (UserVariableNode);
+  }
+}
+
+/**
+  Create a time based data payload by concatenating the EFI_VARIABLE_AUTHENTICATION_2
+  descriptor with the input data. NO authentication is required in this function.
+
+  @param[in, out] DataSize          On input, the size of Data buffer in bytes.
+                                    On output, the size of data returned in Data
+                                    buffer in bytes.
+  @param[in, out] Data              On input, Pointer to data buffer to be wrapped or
+                                    pointer to NULL to wrap an empty payload.
+                                    On output, Pointer to the new payload date buffer allocated from pool,
+                                    it's caller's responsibility to free the memory after using it.
+
+  @retval EFI_SUCCESS               Create time based payload successfully.
+  @retval EFI_OUT_OF_RESOURCES      There are not enough memory resourses to create time based payload.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+  @retval Others                    Unexpected error happens.
+
+**/
+EFI_STATUS
+CreateTimeBasedPayload (
+  IN OUT UINTN      *DataSize,
+  IN OUT UINT8      **Data
+  )
+{
+  EFI_STATUS                        Status;
+  UINT8                             *NewData;
+  UINT8                             *Payload;
+  UINTN                             PayloadSize;
+  EFI_VARIABLE_AUTHENTICATION_2     *DescriptorData;
+  UINTN                             DescriptorSize;
+  EFI_TIME                          Time;
+
+  if (Data == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // At user physical presence, the variable does not need to be signed but the
+  // parameters to the SetVariable() call still need to be prepared as authenticated
+  // variable. So we create EFI_VARIABLE_AUTHENTICATED_2 descriptor without certificate
+  // data in it.
+  //
+  Payload     = *Data;
+  PayloadSize = *DataSize;
+
+  DescriptorSize = OFFSET_OF (EFI_VARIABLE_AUTHENTICATION_2, AuthInfo) + OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData);
+  NewData = (UINT8 *) AllocateZeroPool (DescriptorSize + PayloadSize);
+  if (NewData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if ((Payload != NULL) && (PayloadSize != 0)) {
+    CopyMem (NewData + DescriptorSize, Payload, PayloadSize);
+  }
+
+  DescriptorData = (EFI_VARIABLE_AUTHENTICATION_2 *) (NewData);
+
+  ZeroMem (&Time, sizeof (EFI_TIME));
+  Status = gRT->GetTime (&Time, NULL);
+  if (EFI_ERROR (Status)) {
+    FreePool (NewData);
+    return Status;
+  }
+  Time.Pad1       = 0;
+  Time.Nanosecond = 0;
+  Time.TimeZone   = 0;
+  Time.Daylight   = 0;
+  Time.Pad2       = 0;
+  CopyMem (&DescriptorData->TimeStamp, &Time, sizeof (EFI_TIME));
+
+  DescriptorData->AuthInfo.Hdr.dwLength         = OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData);
+  DescriptorData->AuthInfo.Hdr.wRevision        = 0x0200;
+  DescriptorData->AuthInfo.Hdr.wCertificateType = WIN_CERT_TYPE_EFI_GUID;
+  CopyGuid (&DescriptorData->AuthInfo.CertType, &gEfiCertPkcs7Guid);
+
+  if (Payload != NULL) {
+    FreePool (Payload);
+  }
+
+  *DataSize = DescriptorSize + PayloadSize;
+  *Data     = NewData;
+  return EFI_SUCCESS;
+}
+
+/**
+  Create a counter based data payload by concatenating the EFI_VARIABLE_AUTHENTICATION
+  descriptor with the input data. NO authentication is required in this function.
+
+  @param[in, out] DataSize          On input, the size of Data buffer in bytes.
+                                    On output, the size of data returned in Data
+                                    buffer in bytes.
+  @param[in, out] Data              On input, Pointer to data buffer to be wrapped or
+                                    pointer to NULL to wrap an empty payload.
+                                    On output, Pointer to the new payload date buffer allocated from pool,
+                                    it's caller's responsibility to free the memory after using it.
+
+  @retval EFI_SUCCESS               Create counter based payload successfully.
+  @retval EFI_OUT_OF_RESOURCES      There are not enough memory resourses to create time based payload.
+  @retval EFI_INVALID_PARAMETER     The parameter is invalid.
+  @retval Others                    Unexpected error happens.
+
+**/
+EFI_STATUS
+CreateCounterBasedPayload (
+  IN OUT UINTN      *DataSize,
+  IN OUT UINT8      **Data
+  )
+{
+  EFI_STATUS                        Status;
+  UINT8                             *NewData;
+  UINT8                             *Payload;
+  UINTN                             PayloadSize;
+  EFI_VARIABLE_AUTHENTICATION       *DescriptorData;
+  UINTN                             DescriptorSize;
+  UINT64                            MonotonicCount;
+
+  if (Data == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // At user physical presence, the variable does not need to be signed but the
+  // parameters to the SetVariable() call still need to be prepared as authenticated
+  // variable. So we create EFI_VARIABLE_AUTHENTICATED descriptor without certificate
+  // data in it.
+  //
+  Payload     = *Data;
+  PayloadSize = *DataSize;
+
+  DescriptorSize = (OFFSET_OF (EFI_VARIABLE_AUTHENTICATION, AuthInfo)) + \
+                   (OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData)) + \
+                   sizeof (EFI_CERT_BLOCK_RSA_2048_SHA256);
+  NewData = (UINT8 *) AllocateZeroPool (DescriptorSize + PayloadSize);
+  if (NewData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if ((Payload != NULL) && (PayloadSize != 0)) {
+    CopyMem (NewData + DescriptorSize, Payload, PayloadSize);
+  }
+
+  DescriptorData = (EFI_VARIABLE_AUTHENTICATION *) (NewData);
+
+  Status = gBS->GetNextMonotonicCount (&MonotonicCount);
+  if (EFI_ERROR (Status)) {
+    FreePool (NewData);
+    return Status;
+  }
+  DescriptorData->MonotonicCount = MonotonicCount;
+
+  DescriptorData->AuthInfo.Hdr.dwLength         = OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData) + sizeof (EFI_CERT_BLOCK_RSA_2048_SHA256);
+  DescriptorData->AuthInfo.Hdr.wRevision        = 0x0200;
+  DescriptorData->AuthInfo.Hdr.wCertificateType = WIN_CERT_TYPE_EFI_GUID;
+  CopyGuid (&DescriptorData->AuthInfo.CertType, &gEfiCertTypeRsa2048Sha256Guid);
+
+  if (Payload != NULL) {
+    FreePool (Payload);
+  }
+
+  *DataSize = DescriptorSize + PayloadSize;
+  *Data     = NewData;
+  return EFI_SUCCESS;
+}
+
+/**
+  Delete user variable.
+
+  @param[in] DeleteAll              Delete all user variables.
+  @param[in] VariableCleanupData    Pointer to variable cleanup data.
+
+**/
+VOID
+DeleteUserVariable (
+  IN BOOLEAN                DeleteAll,
+  IN VARIABLE_CLEANUP_DATA  *VariableCleanupData OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+  USER_VARIABLE_NODE        *UserVariableNode;
+  LIST_ENTRY                *Link;
+  USER_VARIABLE_NAME_NODE   *UserVariableNameNode;
+  LIST_ENTRY                *NameLink;
+  UINTN                     DataSize;
+  UINT8                     *Data;
+
+  for (Link = mUserVariableList.ForwardLink
+       ;Link != &mUserVariableList
+       ;Link = Link->ForwardLink) {
+    UserVariableNode = USER_VARIABLE_FROM_LINK (Link);
+
+    for (NameLink = UserVariableNode->NameLink.ForwardLink
+        ;NameLink != &UserVariableNode->NameLink
+        ;NameLink = NameLink->ForwardLink) {
+      UserVariableNameNode = USER_VARIABLE_NAME_FROM_LINK (NameLink);
+
+      if (!UserVariableNameNode->Deleted && (DeleteAll || ((VariableCleanupData != NULL) && (VariableCleanupData->UserVariable[UserVariableNameNode->Index] == TRUE)))) {
+        DEBUG ((EFI_D_INFO, "PlatformVarCleanup - Delete variable: %g:%s\n", &UserVariableNode->Guid, UserVariableNameNode->Name));
+        if ((UserVariableNameNode->Attributes & EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS) != 0) {
+          DataSize = 0;
+          Data = NULL;
+          Status = CreateTimeBasedPayload (&DataSize, &Data);
+          if (!EFI_ERROR (Status)) {
+            Status = gRT->SetVariable (UserVariableNameNode->Name, &UserVariableNode->Guid, UserVariableNameNode->Attributes, DataSize, Data);
+            FreePool (Data);
+          }
+        } else if ((UserVariableNameNode->Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) != 0) {
+          DataSize = 0;
+          Data = NULL;
+          Status = CreateCounterBasedPayload (&DataSize, &Data);
+          if (!EFI_ERROR (Status)) {
+            Status = gRT->SetVariable (UserVariableNameNode->Name, &UserVariableNode->Guid, UserVariableNameNode->Attributes, DataSize, Data);
+            FreePool (Data);
+          }
+        } else {
+          Status = gRT->SetVariable (UserVariableNameNode->Name, &UserVariableNode->Guid, 0, 0, NULL);
+        }
+        if (!EFI_ERROR (Status)) {
+          UserVariableNameNode->Deleted = TRUE;
+        } else {
+          DEBUG ((EFI_D_INFO, "PlatformVarCleanup - Delete variable fail: %g:%s\n", &UserVariableNode->Guid, UserVariableNameNode->Name));
+        }
+      }
+    }
+  }
+}
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+  @param[in]  This          Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Request       A null-terminated Unicode string in <ConfigRequest> format.
+  @param[out] Progress      On return, points to a character in the Request string.
+                            Points to the string's null terminator if request was successful.
+                            Points to the most recent '&' before the first failing name/value
+                            pair (or the beginning of the string if the failure is in the
+                            first name/value pair) if the request was not successful.
+  @param[out] Results       A null-terminated Unicode string in <ConfigAltResp> format which
+                            has all values filled in for the names in the Request string.
+                            String to be allocated by the called function.
+
+  @retval EFI_SUCCESS               The Results is filled with the requested values.
+  @retval EFI_OUT_OF_RESOURCES      Not enough memory to store the results.
+  @retval EFI_INVALID_PARAMETER     Request is illegal syntax, or unknown name.
+  @retval EFI_NOT_FOUND             Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableCleanupHiiExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN  CONST EFI_STRING                          Request,
+  OUT EFI_STRING                                *Progress,
+  OUT EFI_STRING                                *Results
+  )
+{
+  EFI_STATUS                        Status;
+  VARIABLE_CLEANUP_HII_PRIVATE_DATA *Private;
+  UINTN                             BufferSize;
+  EFI_STRING                        ConfigRequestHdr;
+  EFI_STRING                        ConfigRequest;
+  BOOLEAN                           AllocatedRequest;
+  UINTN                             Size;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Request;
+  if ((Request != NULL) && !HiiIsConfigHdrMatch (Request, &mVariableCleanupHiiGuid, mVarStoreName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  ConfigRequestHdr = NULL;
+  ConfigRequest    = NULL;
+  AllocatedRequest = FALSE;
+  Size             = 0;
+
+  Private = VARIABLE_CLEANUP_HII_PRIVATE_FROM_THIS (This);
+  //
+  // Convert buffer data to <ConfigResp> by helper function BlockToConfig().
+  //
+  BufferSize = sizeof (VARIABLE_CLEANUP_DATA);
+  ConfigRequest = Request;
+  if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+    //
+    // Request has no request element, construct full request string.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator.
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&mVariableCleanupHiiGuid, mVarStoreName, Private->HiiHandle);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+  }
+
+  Status = Private->ConfigRouting->BlockToConfig (
+                                     Private->ConfigRouting,
+                                     ConfigRequest,
+                                     (UINT8 *) &Private->VariableCleanupData,
+                                     BufferSize,
+                                     Results,
+                                     Progress
+                                     );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+  }
+  //
+  // Set Progress string to the original request string or the string's null terminator.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+/**
+  Update user variable form.
+
+  @param[in] Private    Points to the VARIABLE_CLEANUP_HII_PRIVATE_DATA.
+
+**/
+VOID
+UpdateUserVariableForm (
+  IN VARIABLE_CLEANUP_HII_PRIVATE_DATA  *Private
+  )
+{
+  EFI_STRING_ID             PromptStringToken;
+  EFI_STRING_ID             HelpStringToken;
+  VOID                      *StartOpCodeHandle;
+  VOID                      *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL        *StartLabel;
+  EFI_IFR_GUID_LABEL        *EndLabel;
+  USER_VARIABLE_NODE        *UserVariableNode;
+  LIST_ENTRY                *Link;
+  USER_VARIABLE_NAME_NODE   *UserVariableNameNode;
+  LIST_ENTRY                *NameLink;
+  BOOLEAN                   Created;
+
+  //
+  // Init OpCode Handle.
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode.
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number = LABEL_START;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode.
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number = LABEL_END;
+
+  HiiUpdateForm (
+    Private->HiiHandle,
+    &mVariableCleanupHiiGuid,
+    FORM_ID_VARIABLE_CLEANUP,
+    StartOpCodeHandle, // LABEL_START
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  for (Link = mUserVariableList.ForwardLink
+      ;Link != &mUserVariableList
+      ;Link = Link->ForwardLink) {
+    UserVariableNode = USER_VARIABLE_FROM_LINK (Link);
+
+    //
+    // Create checkbox opcode for variables in the same variable GUID space.
+    //
+    Created = FALSE;
+    for (NameLink = UserVariableNode->NameLink.ForwardLink
+        ;NameLink != &UserVariableNode->NameLink
+        ;NameLink = NameLink->ForwardLink) {
+      UserVariableNameNode = USER_VARIABLE_NAME_FROM_LINK (NameLink);
+
+      if (!UserVariableNameNode->Deleted) {
+        if (!Created) {
+          //
+          // Create subtitle opcode for variable GUID.
+          //
+          PromptStringToken = HiiSetString (Private->HiiHandle, 0, UserVariableNode->PromptString, NULL);
+          HiiCreateSubTitleOpCode (StartOpCodeHandle, PromptStringToken, 0, 0, 0);
+          Created = TRUE;
+        }
+
+        //
+        // Only create opcode for the non-deleted variables.
+        //
+        PromptStringToken = HiiSetString (Private->HiiHandle, 0, UserVariableNameNode->PromptString, NULL);
+        HelpStringToken = HiiSetString (Private->HiiHandle, 0, UserVariableNameNode->HelpString, NULL);
+        HiiCreateCheckBoxOpCode (
+          StartOpCodeHandle,
+          UserVariableNameNode->QuestionId,
+          VARIABLE_CLEANUP_VARSTORE_ID,
+          (UINT16) (USER_VARIABLE_VAR_OFFSET + UserVariableNameNode->Index),
+          PromptStringToken,
+          HelpStringToken,
+          EFI_IFR_FLAG_CALLBACK,
+          Private->VariableCleanupData.UserVariable[UserVariableNameNode->Index],
+          NULL
+          );
+      }
+    }
+  }
+
+  HiiCreateSubTitleOpCode (
+    StartOpCodeHandle,
+    STRING_TOKEN (STR_NULL_STRING),
+    0,
+    0,
+    0
+    );
+
+  //
+  // Create the "Apply changes" and "Discard changes" tags.
+  //
+  HiiCreateActionOpCode (
+    StartOpCodeHandle,
+    SAVE_AND_EXIT_QUESTION_ID,
+    STRING_TOKEN (STR_SAVE_AND_EXIT),
+    STRING_TOKEN (STR_NULL_STRING),
+    EFI_IFR_FLAG_CALLBACK,
+    0
+    );
+  HiiCreateActionOpCode (
+    StartOpCodeHandle,
+    NO_SAVE_AND_EXIT_QUESTION_ID,
+    STRING_TOKEN (STR_NO_SAVE_AND_EXIT),
+    STRING_TOKEN (STR_NULL_STRING),
+    EFI_IFR_FLAG_CALLBACK,
+    0
+    );
+
+  HiiUpdateForm (
+    Private->HiiHandle,
+    &mVariableCleanupHiiGuid,
+    FORM_ID_VARIABLE_CLEANUP,
+    StartOpCodeHandle, // LABEL_START
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+}
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This                  Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration         A null-terminated Unicode string in
+                                    <ConfigString> format.
+  @param[out] Progress              A pointer to a string filled in with the
+                                    offset of the most recent '&' before the
+                                    first failing name / value pair (or the
+                                    beginn ing of the string if the failure
+                                    is in the first name / value pair) or
+                                    the terminating NULL if all was
+                                    successful.
+
+  @retval EFI_SUCCESS               The results have been distributed or are
+                                    awaiting distribution.
+  @retval EFI_OUT_OF_RESOURCES      Not enough memory to store the
+                                    parts of the results that must be
+                                    stored awaiting possible future
+                                    protocols.
+  @retval EFI_INVALID_PARAMETERS    Passing in a NULL for the
+                                    Results parameter would result
+                                    in this type of error.
+  @retval EFI_NOT_FOUND             Target for the specified routing data
+                                    was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableCleanupHiiRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN  CONST EFI_STRING                          Configuration,
+  OUT EFI_STRING                                *Progress
+  )
+{
+  EFI_STATUS                        Status;
+  VARIABLE_CLEANUP_HII_PRIVATE_DATA *Private;
+  UINTN                             BufferSize;
+
+  if (Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Configuration;
+
+  if (Configuration == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check routing data in <ConfigHdr>.
+  // Note: there is no name for Name/Value storage, only GUID will be checked.
+  //
+  if (!HiiIsConfigHdrMatch (Configuration, &mVariableCleanupHiiGuid, mVarStoreName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = VARIABLE_CLEANUP_HII_PRIVATE_FROM_THIS (This);
+  //
+  // Get Buffer Storage data.
+  //
+  BufferSize = sizeof (VARIABLE_CLEANUP_DATA);
+  //
+  // Convert <ConfigResp> to buffer data by helper function ConfigToBlock().
+  //
+  Status = Private->ConfigRouting->ConfigToBlock (
+                            Private->ConfigRouting,
+                            Configuration,
+                            (UINT8 *) &Private->VariableCleanupData,
+                            &BufferSize,
+                            Progress
+                            );
+  ASSERT_EFI_ERROR (Status);
+
+  DeleteUserVariable (FALSE, &Private->VariableCleanupData);
+  //
+  // For "F10" hotkey to refresh the form.
+  //
+//  UpdateUserVariableForm (Private);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is called to provide results data to the driver.
+  This data consists of a unique key that is used to identify
+  which data is either being passed back or being asked for.
+
+  @param[in]  This                  Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Action                Specifies the type of action taken by the browser.
+  @param[in]  QuestionId            A unique value which is sent to the original
+                                    exporting driver so that it can identify the type
+                                    of data to expect. The format of the data tends to
+                                    vary based on the opcode that generated the callback.
+  @param[in]  Type                  The type of value for the question.
+  @param[in]  Value                 A pointer to the data being sent to the original
+                                    exporting driver.
+  @param[out] ActionRequest         On return, points to the action requested by the
+                                    callback function.
+
+  @retval EFI_SUCCESS               The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES      Not enough storage is available to hold the
+                                    variable and its data.
+  @retval EFI_DEVICE_ERROR          The variable could not be saved.
+  @retval EFI_UNSUPPORTED           The specified Action is not supported by the
+                                    callback.
+**/
+EFI_STATUS
+EFIAPI
+VariableCleanupHiiCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  VARIABLE_CLEANUP_HII_PRIVATE_DATA *Private;
+  VARIABLE_CLEANUP_DATA             *VariableCleanupData;
+
+  Private = VARIABLE_CLEANUP_HII_PRIVATE_FROM_THIS (This);
+
+  if ((Action != EFI_BROWSER_ACTION_CHANGING) && (Action != EFI_BROWSER_ACTION_CHANGED)) {
+    //
+    // All other action return unsupported.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Retrive uncommitted data from Form Browser.
+  //
+  VariableCleanupData = &Private->VariableCleanupData;
+  HiiGetBrowserData (&mVariableCleanupHiiGuid, mVarStoreName, sizeof (VARIABLE_CLEANUP_DATA), (UINT8 *) VariableCleanupData);
+  if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if (Value == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    if ((Value == NULL) || (ActionRequest == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if ((QuestionId >= USER_VARIABLE_QUESTION_ID) && (QuestionId < USER_VARIABLE_QUESTION_ID + MAX_USER_VARIABLE_COUNT)) {
+      if (Value->b){
+        //
+        // Means one user variable checkbox is marked to delete but not press F10 or "Commit Changes and Exit" menu.
+        //
+        mMarkedUserVariableCount++;
+        ASSERT (mMarkedUserVariableCount <= mUserVariableCount);
+        if (mMarkedUserVariableCount == mUserVariableCount) {
+          //
+          // All user variables have been marked, then also mark the SelectAll checkbox.
+          //
+          VariableCleanupData->SelectAll = TRUE;
+        }
+      } else {
+        //
+        // Means one user variable checkbox is unmarked.
+        //
+        mMarkedUserVariableCount--;
+        //
+        // Also unmark the SelectAll checkbox.
+        //
+        VariableCleanupData->SelectAll = FALSE;
+      }
+    } else {
+      switch (QuestionId) {
+        case SELECT_ALL_QUESTION_ID:
+         if (Value->b){
+            //
+            // Means the SelectAll checkbox is marked to delete all user variables but not press F10 or "Commit Changes and Exit" menu.
+            //
+            SetMem (VariableCleanupData->UserVariable, sizeof (VariableCleanupData->UserVariable), TRUE);
+            mMarkedUserVariableCount = mUserVariableCount;
+          } else {
+            //
+            // Means the SelectAll checkbox is unmarked.
+            //
+            SetMem (VariableCleanupData->UserVariable, sizeof (VariableCleanupData->UserVariable), FALSE);
+            mMarkedUserVariableCount = 0;
+          }
+          break;
+        case SAVE_AND_EXIT_QUESTION_ID:
+          DeleteUserVariable (FALSE, VariableCleanupData);
+          *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_SUBMIT_EXIT;
+          break;
+
+        case NO_SAVE_AND_EXIT_QUESTION_ID:
+          //
+          // Restore local maintain data.
+          //
+          *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD_EXIT;
+          break;
+
+        default:
+          break;
+      }
+    }
+  }
+
+  //
+  // Pass changed uncommitted data back to Form Browser.
+  //
+  HiiSetBrowserData (&mVariableCleanupHiiGuid, mVarStoreName, sizeof (VARIABLE_CLEANUP_DATA), (UINT8 *) VariableCleanupData, NULL);
+  return EFI_SUCCESS;
+}
+
+/**
+  Platform variable cleanup.
+
+  @param[in] Flag                   Variable error flag.
+  @param[in] Type                   Variable cleanup type.
+                                    If it is VarCleanupManually, the interface must be called after console connected.
+
+  @retval EFI_SUCCESS               No error or error processed.
+  @retval EFI_UNSUPPORTED           The specified Flag or Type is not supported.
+                                    For example, system error may be not supported to process and Platform should have mechanism to reset system to manufacture mode.
+                                    Another, if system and user variables are wanted to be distinguished to process, the interface must be called after EndOfDxe.
+  @retval EFI_OUT_OF_RESOURCES      Not enough resource to process the error.
+  @retval EFI_INVALID_PARAMETER     The specified Flag or Type is an invalid value.
+  @retval Others                    Other failure occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformVarCleanup (
+  IN VAR_ERROR_FLAG     Flag,
+  IN VAR_CLEANUP_TYPE   Type
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_FORM_BROWSER2_PROTOCOL            *FormBrowser2;
+  VARIABLE_CLEANUP_HII_PRIVATE_DATA     *Private;
+
+  if (!mEndOfDxe) {
+    //
+    // This implementation must be called after EndOfDxe.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((Type >= VarCleanupMax) || ((Flag & ((VAR_ERROR_FLAG) (VAR_ERROR_FLAG_SYSTEM_ERROR & VAR_ERROR_FLAG_USER_ERROR))) == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Flag == VAR_ERROR_FLAG_NO_ERROR) {
+    //
+    // Just return success if no error.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if ((Flag & (~((VAR_ERROR_FLAG) VAR_ERROR_FLAG_SYSTEM_ERROR))) == 0) {
+    //
+    // This sample does not support system variables cleanup.
+    //
+    DEBUG ((EFI_D_ERROR, "NOTICE - VAR_ERROR_FLAG_SYSTEM_ERROR\n"));
+    DEBUG ((EFI_D_ERROR, "Platform should have mechanism to reset system to manufacture mode\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Continue to process VAR_ERROR_FLAG_USER_ERROR.
+  //
+
+  //
+  // Create user variable nodes for the following processing.
+  //
+  CreateUserVariableNode ();
+
+  switch (Type) {
+    case VarCleanupAll:
+      DeleteUserVariable (TRUE, NULL);
+      //
+      // Destroyed the created user variable nodes
+      //
+      DestroyUserVariableNode ();
+      return EFI_SUCCESS;
+      break;
+
+    case VarCleanupManually:
+      //
+      // Locate FormBrowser2 protocol.
+      //
+      Status = gBS->LocateProtocol (&gEfiFormBrowser2ProtocolGuid, NULL, (VOID **) &FormBrowser2);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Private = AllocateZeroPool (sizeof (VARIABLE_CLEANUP_HII_PRIVATE_DATA));
+      if (Private == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      Private->Signature = VARIABLE_CLEANUP_HII_PRIVATE_SIGNATURE;
+      Private->ConfigAccess.ExtractConfig = VariableCleanupHiiExtractConfig;
+      Private->ConfigAccess.RouteConfig   = VariableCleanupHiiRouteConfig;
+      Private->ConfigAccess.Callback      = VariableCleanupHiiCallback;
+
+      Status = gBS->LocateProtocol (
+                      &gEfiHiiConfigRoutingProtocolGuid,
+                      NULL,
+                      (VOID **) &Private->ConfigRouting
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Install Device Path Protocol and Config Access protocol to driver handle.
+      //
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Private->DriverHandle,
+                      &gEfiDevicePathProtocolGuid,
+                      &mVarCleanupHiiVendorDevicePath,
+                      &gEfiHiiConfigAccessProtocolGuid,
+                      &Private->ConfigAccess,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Publish our HII data.
+      //
+      Private->HiiHandle = HiiAddPackages (
+                             &mVariableCleanupHiiGuid,
+                             Private->DriverHandle,
+                             PlatformVarCleanupLibStrings,
+                             PlatVarCleanupBin,
+                             NULL
+                             );
+      if (Private->HiiHandle == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+
+      UpdateUserVariableForm (Private);
+
+      Status = FormBrowser2->SendForm (
+                               FormBrowser2,
+                               &Private->HiiHandle,
+                               1,
+                               NULL,
+                               0,
+                               NULL,
+                               NULL
+                               );
+      break;
+
+    default:
+      return EFI_UNSUPPORTED;
+      break;
+  }
+
+Done:
+  if (Private->DriverHandle != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Private->DriverHandle,
+           &gEfiDevicePathProtocolGuid,
+           &mVarCleanupHiiVendorDevicePath,
+           &gEfiHiiConfigAccessProtocolGuid,
+           &Private->ConfigAccess,
+           NULL
+           );
+  }
+  if (Private->HiiHandle != NULL) {
+    HiiRemovePackages (Private->HiiHandle);
+  }
+
+  FreePool (Private);
+
+  //
+  // Destroyed the created user variable nodes
+  //
+  DestroyUserVariableNode ();
+  return Status;
+}
+
+/**
+  Get last boot variable error flag.
+
+  @return   Last boot variable error flag.
+
+**/
+VAR_ERROR_FLAG
+EFIAPI
+GetLastBootVarErrorFlag (
+  )
+{
+  return mLastVarErrorFlag;
+}
+
+/**
+  Notification function of END_OF_DXE.
+
+  This is a notification function registered on END_OF_DXE event.
+
+  @param[in] Event      Event whose notification function is being invoked.
+  @param[in] Context    Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+PlatformVarCleanupEndOfDxeEvent (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  mEndOfDxe = TRUE;
+}
+
+/**
+  The constructor function caches the pointer to VarCheck protocol and last boot variable error flag.
+
+  The constructor function locates VarCheck protocol from protocol database.
+  It will ASSERT() if that operation fails and it will always return EFI_SUCCESS.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformVarCleanupLibConstructor (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS    Status;
+  EFI_EVENT     Event;
+
+  mLastVarErrorFlag = InternalGetVarErrorFlag ();
+  DEBUG ((EFI_D_INFO, "mLastVarErrorFlag - 0x%02x\n", mLastVarErrorFlag));
+
+  Status = gBS->LocateProtocol (
+                  &gEdkiiVarCheckProtocolGuid,
+                  NULL,
+                  (VOID **) &mVarCheck
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register EFI_END_OF_DXE_EVENT_GROUP_GUID event.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  PlatformVarCleanupEndOfDxeEvent,
+                  NULL,
+                  &gEfiEndOfDxeEventGroupGuid,
+                  &Event
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.inf b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.inf
new file mode 100644
index 0000000000..c56a17584a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.inf
@@ -0,0 +1,73 @@
+## @file
+#  Sample platform variable cleanup library instance.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                   = 0x00010005
+  BASE_NAME                     = PlatformVarCleanupLib
+  MODULE_UNI_FILE               = PlatformVarCleanupLib.uni
+  FILE_GUID                     = 9C9623EB-4EF3-44e0-A931-F3A340D1A0F9
+  MODULE_TYPE                   = DXE_DRIVER
+  VERSION_STRING                = 1.0
+  LIBRARY_CLASS                 = PlatformVarCleanupLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+  CONSTRUCTOR                   = PlatformVarCleanupLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES          = IA32 X64
+#
+
+[Sources.common]
+  PlatVarCleanupLib.c
+  PlatVarCleanup.h
+  PlatVarCleanupHii.h
+  PlatVarCleanup.vfr
+  VfrStrings.uni
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  UefiLib
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  PrintLib
+  MemoryAllocationLib
+  HiiLib
+
+[Guids]
+  gEfiIfrTianoGuid                  ## SOMETIMES_PRODUCES   ## GUID
+  gEdkiiVarErrorFlagGuid            ## CONSUMES             ## Variable:L"VarErrorFlag"
+  gEfiEndOfDxeEventGroupGuid        ## CONSUMES             ## Event
+  gEfiCertPkcs7Guid                 ## SOMETIMES_CONSUMES   ## GUID
+  gEfiCertTypeRsa2048Sha256Guid     ## SOMETIMES_CONSUMES   ## GUID
+
+[Protocols]
+  gEfiVariableArchProtocolGuid      ## CONSUMES
+  gEdkiiVarCheckProtocolGuid        ## CONSUMES
+  gEfiDevicePathProtocolGuid        ## SOMETIMES_PRODUCES
+  gEfiFormBrowser2ProtocolGuid      ## SOMETIMES_CONSUMES
+  gEfiHiiConfigAccessProtocolGuid   ## SOMETIMES_PRODUCES
+  gEfiHiiConfigRoutingProtocolGuid  ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEdkiiVarCheckProtocolGuid AND
+  gEfiVariableArchProtocolGuid
+
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.uni b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.uni
new file mode 100644
index 0000000000..e4a0debafa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.uni
diff --git a/Core/MdeModulePkg/Library/PlatformVarCleanupLib/VfrStrings.uni b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/VfrStrings.uni
new file mode 100644
index 0000000000..56f8e3625f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/PlatformVarCleanupLib/VfrStrings.uni
diff --git a/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/ReportStatusCodeLib.c b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/ReportStatusCodeLib.c
new file mode 100644
index 0000000000..e533af3d33
--- /dev/null
+++ b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/ReportStatusCodeLib.c
@@ -0,0 +1,754 @@
+/** @file
+  API implementation for instance of Report Status Code Library.
+
+  Copyright (c) 2006 - 2012, 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 <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiRuntimeLib.h>
+
+#include <Protocol/StatusCode.h>
+
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+#include <Guid/EventGroup.h>
+
+
+//
+// Define the maximum extended data size that is supported when a status code is reported.
+//
+#define MAX_EXTENDED_DATA_SIZE  0x200
+
+EFI_STATUS_CODE_PROTOCOL  *mReportStatusCodeLibStatusCodeProtocol = NULL;
+EFI_EVENT                 mReportStatusCodeLibVirtualAddressChangeEvent;
+EFI_EVENT                 mReportStatusCodeLibExitBootServicesEvent;
+BOOLEAN                   mHaveExitedBootServices = FALSE;
+
+/**
+  Locate the report status code service.
+
+  Retrieve ReportStatusCode() API of Report Status Code Protocol.
+
+**/
+VOID
+InternalGetReportStatusCode (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  if (mReportStatusCodeLibStatusCodeProtocol != NULL) {
+    return;
+  }
+  
+  if (mHaveExitedBootServices) {
+    return;
+  }
+  
+  //
+  // Check gBS just in case ReportStatusCode is called before gBS is initialized.
+  //
+  if (gBS != NULL && gBS->LocateProtocol != NULL) {
+    Status = gBS->LocateProtocol (&gEfiStatusCodeRuntimeProtocolGuid, NULL, (VOID**) &mReportStatusCodeLibStatusCodeProtocol);
+    if (EFI_ERROR (Status)) {
+      mReportStatusCodeLibStatusCodeProtocol = NULL;
+    }
+  }
+}
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context
+
+**/
+VOID
+EFIAPI
+ReportStatusCodeLibVirtualAddressChange (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  if (mReportStatusCodeLibStatusCodeProtocol == NULL) {
+    return;
+  }
+  EfiConvertPointer (0, (VOID **) &mReportStatusCodeLibStatusCodeProtocol);
+}
+
+/**
+  Notification function of EVT_SIGNAL_EXIT_BOOT_SERVICES.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context
+
+**/
+VOID
+EFIAPI
+ReportStatusCodeLibExitBootServices (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  //
+  // Locate the report status code service before enter runtime.
+  // 
+  InternalGetReportStatusCode ();
+  
+  mHaveExitedBootServices = TRUE;
+}
+
+/**
+  The constructor function of Runtime DXE Report Status Code Lib.
+
+  This function allocates memory for extended status code data, caches
+  the report status code service, and registers events.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeLibConstructor (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Cache the report status code service
+  // 
+  InternalGetReportStatusCode ();
+
+  //
+  // Register notify function for EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE
+  // 
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ReportStatusCodeLibVirtualAddressChange,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mReportStatusCodeLibVirtualAddressChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register notify function for EVT_SIGNAL_EXIT_BOOT_SERVICES
+  // 
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ReportStatusCodeLibExitBootServices,
+                  NULL,
+                  &gEfiEventExitBootServicesGuid,
+                  &mReportStatusCodeLibExitBootServicesEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The destructor function of Runtime DXE Report Status Code Lib.
+  
+  The destructor function frees memory allocated by constructor, and closes related events.
+  It will ASSERT() if that related operation fails and it will always return EFI_SUCCESS. 
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeLibDestructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (gBS != NULL);
+  Status = gBS->CloseEvent (mReportStatusCodeLibVirtualAddressChangeEvent);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->CloseEvent (mReportStatusCodeLibExitBootServicesEvent);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal worker function that reports a status code through the Report Status Code Protocol.
+
+  If status code service is not cached, then this function checks if Report Status Code
+  Protocol is available in system.  If Report Status Code Protocol is not available, then
+  EFI_UNSUPPORTED is returned.  If Report Status Code Protocol is present, then it is
+  cached in mReportStatusCodeLibStatusCodeProtocol. Finally this function reports status
+  code through the Report Status Code Protocol.
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  This is an optional parameter that may be
+                            NULL.
+  @param  Data              Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+
+  @retval EFI_SUCCESS       The status code was reported.
+  @retval EFI_UNSUPPORTED   Report Status Code Protocol is not available.
+  @retval EFI_UNSUPPORTED   Status code type is not supported.
+
+**/
+EFI_STATUS
+InternalReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN CONST EFI_GUID           *CallerId OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data     OPTIONAL
+  )
+{
+  if ((ReportProgressCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      (ReportErrorCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) ||
+      (ReportDebugCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_DEBUG_CODE)) {
+    //
+    // If mReportStatusCodeLibStatusCodeProtocol is NULL, then check if Report Status Code Protocol is available in system.
+    //
+    InternalGetReportStatusCode ();
+    if (mReportStatusCodeLibStatusCodeProtocol == NULL) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // A Report Status Code Protocol is present in system, so pass in all the parameters to the service.
+    //
+    return mReportStatusCodeLibStatusCodeProtocol->ReportStatusCode (Type, Value, Instance, (EFI_GUID *)CallerId, Data);
+  }
+  
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Converts a status code to an 8-bit POST code value.
+
+  Converts the status code specified by CodeType and Value to an 8-bit POST code
+  and returns the 8-bit POST code in PostCode.  If CodeType is an
+  EFI_PROGRESS_CODE or CodeType is an EFI_ERROR_CODE, then bits 0..4 of PostCode
+  are set to bits 16..20 of Value, and bits 5..7 of PostCode are set to bits
+  24..26 of Value., and TRUE is returned.  Otherwise, FALSE is returned.
+
+  If PostCode is NULL, then ASSERT().
+
+  @param  CodeType  The type of status code being converted.
+  @param  Value     The status code value being converted.
+  @param  PostCode  A pointer to the 8-bit POST code value to return.
+
+  @retval  TRUE   The status code specified by CodeType and Value was converted
+                  to an 8-bit POST code and returned in  PostCode.
+  @retval  FALSE  The status code specified by CodeType and Value could not be
+                  converted to an 8-bit POST code value.
+
+**/
+BOOLEAN
+EFIAPI
+CodeTypeToPostCode (
+  IN  EFI_STATUS_CODE_TYPE   CodeType,
+  IN  EFI_STATUS_CODE_VALUE  Value,
+  OUT UINT8                  *PostCode
+  )
+{
+  //
+  // If PostCode is NULL, then ASSERT()
+  //
+  ASSERT (PostCode != NULL);
+
+  //
+  // Convert Value to an 8 bit post code
+  //
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE)       ) {
+    *PostCode  = (UINT8) ((((Value & EFI_STATUS_CODE_CLASS_MASK) >> 24) << 5) |
+                          (((Value & EFI_STATUS_CODE_SUBCLASS_MASK) >> 16) & 0x1f));
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts ASSERT() information from a status code structure.
+
+  Converts the status code specified by CodeType, Value, and Data to the ASSERT()
+  arguments specified by Filename, Description, and LineNumber.  If CodeType is
+  an EFI_ERROR_CODE, and CodeType has a severity of EFI_ERROR_UNRECOVERED, and
+  Value has an operation mask of EFI_SW_EC_ILLEGAL_SOFTWARE_STATE, extract
+  Filename, Description, and LineNumber from the optional data area of the
+  status code buffer specified by Data.  The optional data area of Data contains
+  a Null-terminated ASCII string for the FileName, followed by a Null-terminated
+  ASCII string for the Description, followed by a 32-bit LineNumber.  If the
+  ASSERT() information could be extracted from Data, then return TRUE.
+  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If Filename is NULL, then ASSERT().
+  If Description is NULL, then ASSERT().
+  If LineNumber is NULL, then ASSERT().
+
+  @param  CodeType     The type of status code being converted.
+  @param  Value        The status code value being converted.
+  @param  Data         Pointer to status code data buffer.
+  @param  Filename     Pointer to the source file name that generated the ASSERT().
+  @param  Description  Pointer to the description of the ASSERT().
+  @param  LineNumber   Pointer to source line number that generated the ASSERT().
+
+  @retval  TRUE   The status code specified by CodeType, Value, and Data was
+                  converted ASSERT() arguments specified by Filename, Description,
+                  and LineNumber.
+  @retval  FALSE  The status code specified by CodeType, Value, and Data could
+                  not be converted to ASSERT() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractAssertInfo (
+  IN EFI_STATUS_CODE_TYPE        CodeType,
+  IN EFI_STATUS_CODE_VALUE       Value,
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT CHAR8                      **Filename,
+  OUT CHAR8                      **Description,
+  OUT UINT32                     *LineNumber
+  )
+{
+  EFI_DEBUG_ASSERT_DATA  *AssertData;
+
+  ASSERT (Data        != NULL);
+  ASSERT (Filename    != NULL);
+  ASSERT (Description != NULL);
+  ASSERT (LineNumber  != NULL);
+
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK)      == EFI_ERROR_CODE) &&
+      ((CodeType & EFI_STATUS_CODE_SEVERITY_MASK)  == EFI_ERROR_UNRECOVERED) &&
+      ((Value    & EFI_STATUS_CODE_OPERATION_MASK) == EFI_SW_EC_ILLEGAL_SOFTWARE_STATE)) {
+    AssertData   = (EFI_DEBUG_ASSERT_DATA *)(Data + 1);
+    *Filename    = (CHAR8 *)(AssertData + 1);
+    *Description = *Filename + AsciiStrLen (*Filename) + 1;
+    *LineNumber  = AssertData->LineNumber;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts DEBUG() information from a status code structure.
+
+  Converts the status code specified by Data to the DEBUG() arguments specified
+  by ErrorLevel, Marker, and Format.  If type GUID in Data is
+  EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID, then extract ErrorLevel, Marker, and
+  Format from the optional data area of the status code buffer specified by Data.
+  The optional data area of Data contains a 32-bit ErrorLevel followed by Marker
+  which is 12 UINTN parameters, followed by a Null-terminated ASCII string for
+  the Format.  If the DEBUG() information could be extracted from Data, then
+  return TRUE.  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If ErrorLevel is NULL, then ASSERT().
+  If Marker is NULL, then ASSERT().
+  If Format is NULL, then ASSERT().
+
+  @param  Data        Pointer to status code data buffer.
+  @param  ErrorLevel  Pointer to error level mask for a debug message.
+  @param  Marker      Pointer to the variable argument list associated with Format.
+  @param  Format      Pointer to a Null-terminated ASCII format string of a
+                      debug message.
+
+  @retval  TRUE   The status code specified by Data was converted DEBUG() arguments
+                  specified by ErrorLevel, Marker, and Format.
+  @retval  FALSE  The status code specified by Data could not be converted to
+                  DEBUG() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractDebugInfo (
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT UINT32                     *ErrorLevel,
+  OUT BASE_LIST                  *Marker,
+  OUT CHAR8                      **Format
+  )
+{
+  EFI_DEBUG_INFO  *DebugInfo;
+
+  ASSERT (Data       != NULL);
+  ASSERT (ErrorLevel != NULL);
+  ASSERT (Marker     != NULL);
+  ASSERT (Format     != NULL);
+
+  //
+  // If the GUID type is not EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID then return FALSE
+  //
+  if (!CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeDebugGuid)) {
+    return FALSE;
+  }
+
+  //
+  // Retrieve the debug information from the status code record
+  //
+  DebugInfo = (EFI_DEBUG_INFO *)(Data + 1);
+
+  *ErrorLevel = DebugInfo->ErrorLevel;
+
+  //
+  // The first 12 * sizeof (UINT64) bytes following EFI_DEBUG_INFO are for variable arguments
+  // of format in DEBUG string. Its address is returned in Marker and has to be 64-bit aligned.
+  // It must be noticed that EFI_DEBUG_INFO follows EFI_STATUS_CODE_DATA, whose size is
+  // 20 bytes. The size of EFI_DEBUG_INFO is 4 bytes, so we can ensure that Marker
+  // returned is 64-bit aligned.
+  // 64-bit aligned is a must, otherwise retrieving 64-bit parameter from BASE_LIST will
+  // cause unalignment exception.
+  //
+  *Marker = (BASE_LIST) (DebugInfo + 1);
+  *Format = (CHAR8 *)(((UINT64 *)*Marker) + 12);
+
+  return TRUE;
+}
+
+
+/**
+  Reports a status code.
+
+  Reports the status code specified by the parameters Type and Value.  Status
+  code also require an instance, caller ID, and extended data.  This function
+  passed in a zero instance, NULL extended data, and a caller ID of
+  gEfiCallerIdGuid, which is the GUID for the module.
+
+  ReportStatusCode()must actively prevent recusrsion.  If ReportStatusCode()
+  is called while processing another any other Report Status Code Library function,
+  then ReportStatusCode() must return immediately.
+
+  @param  Type   Status code type.
+  @param  Value  Status code value.
+
+  @retval  EFI_SUCCESS       The status code was reported.
+  @retval  EFI_DEVICE_ERROR  There status code could not be reported due to a
+                             device error.
+  @retval  EFI_UNSUPPORTED   Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value
+  )
+{
+  return InternalReportStatusCode (Type, Value, 0, &gEfiCallerIdGuid, NULL);
+}
+
+
+/**
+  Reports a status code with a Device Path Protocol as the extended data.
+
+  Allocates and fills in the extended data section of a status code with the
+  Device Path Protocol specified by DevicePath.  This function is responsible
+  for allocating a buffer large enough for the standard header and the device
+  path.  The standard header is filled in with a GUID of
+  gEfiStatusCodeSpecificDataGuid.  The status code is reported with a zero
+  instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithDevicePath()must actively prevent recursion.  If
+  ReportStatusCodeWithDevicePath() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithDevicePath()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If DevicePath is NULL, then ASSERT().
+
+  @param  Type        Status code type.
+  @param  Value       Status code value.
+  @param  DevicePath  Pointer to the Device Path Protocol to be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by DevicePath.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithDevicePath (
+  IN EFI_STATUS_CODE_TYPE            Type,
+  IN EFI_STATUS_CODE_VALUE           Value,
+  IN CONST EFI_DEVICE_PATH_PROTOCOL  *DevicePath
+  )
+{
+  ASSERT (DevicePath != NULL);
+  return ReportStatusCodeWithExtendedData (
+           Type,
+           Value,
+           (VOID *)DevicePath,
+           GetDevicePathSize (DevicePath)
+           );
+}
+
+
+/**
+  Reports a status code with an extended data buffer.
+
+  Allocates and fills in the extended data section of a status code with the
+  extended data specified by ExtendedData and ExtendedDataSize.  ExtendedData
+  is assumed to be one of the data structures specified in Related Definitions.
+  These data structure do not have the standard header, so this function is
+  responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled
+  in with a GUID of  gEfiStatusCodeSpecificDataGuid.  The status code is reported
+  with a zero instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithExtendedData()must actively prevent recursion.  If
+  ReportStatusCodeWithExtendedData() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithExtendedData()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL, then ASSERT().
+  If ExtendedDataSize is 0, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  ExtendedData      Pointer to the extended data buffer to be reported.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer to
+                            be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by ExtendedData and ExtendedDataSize.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithExtendedData (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN CONST VOID             *ExtendedData,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  ASSERT (ExtendedData     != NULL);
+  ASSERT (ExtendedDataSize != 0);
+  return ReportStatusCodeEx (
+           Type,
+           Value,
+           0,
+           NULL,
+           NULL,
+           ExtendedData,
+           ExtendedDataSize
+           );
+}
+
+
+/**
+  Reports a status code with full parameters.
+
+  The function reports a status code.  If ExtendedData is NULL and ExtendedDataSize
+  is 0, then an extended data buffer is not reported.  If ExtendedData is not
+  NULL and ExtendedDataSize is not 0, then an extended data buffer is allocated.
+  ExtendedData is assumed not have the standard status code header, so this function
+  is responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled in
+  with a GUID specified by ExtendedDataGuid.  If ExtendedDataGuid is NULL, then a
+  GUID of gEfiStatusCodeSpecificDataGuid is used.  The status code is reported with
+  an instance specified by Instance and a caller ID specified by CallerId.  If
+  CallerId is NULL, then a caller ID of gEfiCallerIdGuid is used.
+
+  ReportStatusCodeEx()must actively prevent recursion. If
+  ReportStatusCodeEx() is called while processing another any
+  other Report Status Code Library function, then
+  ReportStatusCodeEx() must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL and ExtendedDataSize is not zero, then ASSERT().
+  If ExtendedData is not NULL and ExtendedDataSize is zero, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  If this parameter is NULL, then a caller
+                            ID of gEfiCallerIdGuid is used.
+  @param  ExtendedDataGuid  Pointer to the GUID for the extended data buffer.
+                            If this parameter is NULL, then a the status code
+                            standard header is filled in with
+                            gEfiStatusCodeSpecificDataGuid.
+  @param  ExtendedData      Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer.
+
+  @retval  EFI_SUCCESS           The status code was reported.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate
+                                 the extended data section if it was specified.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeEx (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN UINT32                 Instance,
+  IN CONST EFI_GUID         *CallerId          OPTIONAL,
+  IN CONST EFI_GUID         *ExtendedDataGuid  OPTIONAL,
+  IN CONST VOID             *ExtendedData      OPTIONAL,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  EFI_STATUS            Status;
+  EFI_STATUS_CODE_DATA  *StatusCodeData;
+  UINT64                StatusCodeBuffer[(MAX_EXTENDED_DATA_SIZE / sizeof (UINT64)) + 1];
+
+  ASSERT (!((ExtendedData == NULL) && (ExtendedDataSize != 0)));
+  ASSERT (!((ExtendedData != NULL) && (ExtendedDataSize == 0)));
+
+  if (mHaveExitedBootServices) {
+    if (sizeof (EFI_STATUS_CODE_DATA) + ExtendedDataSize > MAX_EXTENDED_DATA_SIZE) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    StatusCodeData = (EFI_STATUS_CODE_DATA *) StatusCodeBuffer;
+  } else  {
+    if (gBS == NULL || gBS->AllocatePool == NULL || gBS->FreePool == NULL) {
+      return EFI_UNSUPPORTED;
+    }
+  
+    //
+    // Allocate space for the Status Code Header and its buffer
+    //
+    StatusCodeData = NULL;
+    gBS->AllocatePool (EfiBootServicesData, sizeof (EFI_STATUS_CODE_DATA) + ExtendedDataSize, (VOID **)&StatusCodeData);
+    if (StatusCodeData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  //
+  // Fill in the extended data header
+  //
+  StatusCodeData->HeaderSize = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+  StatusCodeData->Size = (UINT16) ExtendedDataSize;
+  if (ExtendedDataGuid == NULL) {
+    ExtendedDataGuid = &gEfiStatusCodeSpecificDataGuid;
+  }
+  CopyGuid (&StatusCodeData->Type, ExtendedDataGuid);
+
+  //
+  // Fill in the extended data buffer
+  //
+  if (ExtendedData != NULL) {
+    CopyMem (StatusCodeData + 1, ExtendedData, ExtendedDataSize);
+  }
+
+  //
+  // Report the status code
+  //
+  if (CallerId == NULL) {
+    CallerId = &gEfiCallerIdGuid;
+  }
+  Status = InternalReportStatusCode (Type, Value, Instance, CallerId, StatusCodeData);
+
+  //
+  // Free the allocated buffer
+  //
+  if (!mHaveExitedBootServices) {
+    gBS->FreePool (StatusCodeData);
+  }
+
+  return Status;
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_PROGRESS_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportProgressCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_ERROR_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportErrorCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_DEBUG_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportDebugCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.inf b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.inf
new file mode 100644
index 0000000000..49d9933b58
--- /dev/null
+++ b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.inf
@@ -0,0 +1,59 @@
+## @file
+#  Report status code library instance which supports logging message in DXE & runtime phase.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = RuntimeDxeReportStatusCodeLib
+  MODULE_UNI_FILE                = RuntimeDxeReportStatusCodeLib.uni
+  FILE_GUID                      = 07D25BBB-F832-41bb-BBA0-612E9F033067
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ReportStatusCodeLib|DXE_RUNTIME_DRIVER DXE_SAL_DRIVER
+  CONSTRUCTOR                    = ReportStatusCodeLibConstructor
+  DESTRUCTOR                     = ReportStatusCodeLibDestructor
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  ReportStatusCodeLib.c
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  PcdLib
+  DevicePathLib
+  UefiRuntimeLib
+
+[Guids]
+  gEfiStatusCodeSpecificDataGuid                ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiStatusCodeDataTypeDebugGuid               ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES ## Event
+  gEfiEventExitBootServicesGuid                 ## CONSUMES ## Event
+
+[Protocols]
+  gEfiStatusCodeRuntimeProtocolGuid             ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdReportStatusCodePropertyMask  ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.uni b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.uni
new file mode 100644
index 0000000000..5392a9bf1b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.c b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.c
new file mode 100644
index 0000000000..e59cc28d53
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.c
@@ -0,0 +1,1099 @@
+/** @file
+  Performance library instance used by SMM Core.
+
+  This library provides the performance measurement interfaces and initializes performance
+  logging for the SMM phase.
+  It initializes SMM phase performance logging by publishing the SMM Performance and PerformanceEx Protocol,
+  which is consumed by SmmPerformanceLib to logging performance data in SMM phase.
+
+  This library is mainly used by SMM Core to start performance logging to ensure that
+  SMM Performance and PerformanceEx Protocol are installed at the very beginning of SMM phase.
+
+ Caution: This module requires additional review when modified.
+ This driver will have external input - performance data and communicate buffer in SMM mode.
+ This external input must be validated carefully to avoid security issue like
+ buffer overflow, integer overflow.
+
+ SmmPerformanceHandlerEx(), SmmPerformanceHandler() will receive untrusted input and do basic validation.
+
+Copyright (c) 2011 - 2015, 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 "SmmCorePerformanceLibInternal.h"
+
+//
+// The data structure to hold global performance data.
+//
+GAUGE_DATA_HEADER       *mGaugeData;
+
+//
+// The current maximum number of logging entries. If current number of 
+// entries exceeds this value, it will re-allocate a larger array and
+// migration the old data to the larger array.
+//
+UINT32                  mMaxGaugeRecords;
+
+//
+// The handle to install Performance Protocol instance.
+//
+EFI_HANDLE              mHandle = NULL;
+
+BOOLEAN                 mPerformanceMeasurementEnabled;
+
+SPIN_LOCK               mSmmPerfLock;
+
+//
+// Interfaces for SMM Performance Protocol.
+//
+PERFORMANCE_PROTOCOL mPerformanceInterface = {
+  StartGauge,
+  EndGauge,
+  GetGauge
+};
+
+//
+// Interfaces for SMM PerformanceEx Protocol.
+//
+PERFORMANCE_EX_PROTOCOL mPerformanceExInterface = {
+  StartGaugeEx,
+  EndGaugeEx,
+  GetGaugeEx
+};
+
+/**
+  Searches in the gauge array with keyword Handle, Token, Module and Identfier.
+
+  This internal function searches for the gauge entry in the gauge array.
+  If there is an entry that exactly matches the given keywords
+  and its end time stamp is zero, then the index of that gauge entry is returned;
+  otherwise, the the number of gauge entries in the array is returned.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  Identifier              32-bit identifier.
+
+  @retval The index of gauge entry in the array.
+
+**/
+UINT32
+SmmSearchForGaugeEntry (
+  IN CONST VOID                 *Handle,  OPTIONAL
+  IN CONST CHAR8                *Token,   OPTIONAL
+  IN CONST CHAR8                *Module,   OPTIONAL
+  IN CONST UINT32               Identifier
+  )
+{
+  UINT32                    Index;
+  UINT32                    Index2;
+  UINT32                    NumberOfEntries;
+  GAUGE_DATA_ENTRY_EX       *GaugeEntryExArray;
+
+  if (Token == NULL) {
+    Token = "";
+  }
+  if (Module == NULL) {
+    Module = "";
+  }
+
+  NumberOfEntries = mGaugeData->NumberOfEntries;
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+  Index2 = 0;
+
+  for (Index = 0; Index < NumberOfEntries; Index++) {
+    Index2 = NumberOfEntries - 1 - Index;
+    if (GaugeEntryExArray[Index2].EndTimeStamp == 0 &&
+        (GaugeEntryExArray[Index2].Handle == (EFI_PHYSICAL_ADDRESS) (UINTN) Handle) &&
+        AsciiStrnCmp (GaugeEntryExArray[Index2].Token, Token, SMM_PERFORMANCE_STRING_LENGTH) == 0 &&
+        AsciiStrnCmp (GaugeEntryExArray[Index2].Module, Module, SMM_PERFORMANCE_STRING_LENGTH) == 0 &&
+        (GaugeEntryExArray[Index2].Identifier == Identifier)) {
+      Index = Index2;
+      break;
+    }
+  }
+
+  return Index;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  GAUGE_DATA_ENTRY_EX       *GaugeEntryExArray;
+  UINTN                     GaugeDataSize;
+  GAUGE_DATA_HEADER         *NewGaugeData;
+  UINTN                     OldGaugeDataSize;
+  GAUGE_DATA_HEADER         *OldGaugeData;
+  UINT32                    Index;
+
+  AcquireSpinLock (&mSmmPerfLock);
+
+  Index = mGaugeData->NumberOfEntries;
+  if (Index >= mMaxGaugeRecords) {
+    //
+    // Try to enlarge the scale of gauge array.
+    //
+    OldGaugeData      = mGaugeData;
+    OldGaugeDataSize  = sizeof (GAUGE_DATA_HEADER) + sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords;
+
+    GaugeDataSize     = sizeof (GAUGE_DATA_HEADER) + sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords * 2;
+
+    NewGaugeData = AllocateZeroPool (GaugeDataSize);
+    if (NewGaugeData == NULL) {
+      ReleaseSpinLock (&mSmmPerfLock);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    mGaugeData = NewGaugeData;
+    mMaxGaugeRecords *= 2;
+
+    //
+    // Initialize new data array and migrate old data one.
+    //
+    mGaugeData = CopyMem (mGaugeData, OldGaugeData, OldGaugeDataSize);
+
+    FreePool (OldGaugeData);
+  }
+
+  GaugeEntryExArray               = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+  GaugeEntryExArray[Index].Handle = (EFI_PHYSICAL_ADDRESS) (UINTN) Handle;
+
+  if (Token != NULL) {
+    AsciiStrnCpyS (GaugeEntryExArray[Index].Token, SMM_PERFORMANCE_STRING_SIZE, Token, SMM_PERFORMANCE_STRING_LENGTH);
+  }
+  if (Module != NULL) {
+    AsciiStrnCpyS (GaugeEntryExArray[Index].Module, SMM_PERFORMANCE_STRING_SIZE, Module, SMM_PERFORMANCE_STRING_LENGTH);
+  }
+
+  GaugeEntryExArray[Index].EndTimeStamp = 0;
+  GaugeEntryExArray[Index].Identifier = Identifier;
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+  GaugeEntryExArray[Index].StartTimeStamp = TimeStamp;
+
+  mGaugeData->NumberOfEntries++;
+
+  ReleaseSpinLock (&mSmmPerfLock);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  GAUGE_DATA_ENTRY_EX *GaugeEntryExArray;
+  UINT32              Index;
+
+  AcquireSpinLock (&mSmmPerfLock);
+
+  if (TimeStamp == 0) {
+    TimeStamp = GetPerformanceCounter ();
+  }
+
+  Index = SmmSearchForGaugeEntry (Handle, Token, Module, Identifier);
+  if (Index >= mGaugeData->NumberOfEntries) {
+    ReleaseSpinLock (&mSmmPerfLock);
+    return EFI_NOT_FOUND;
+  }
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+  GaugeEntryExArray[Index].EndTimeStamp = TimeStamp;
+
+  ReleaseSpinLock (&mSmmPerfLock);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGauge and EndGauge of PERFORMANCE_PROTOCOL,
+  and then assign the Identifier with 0.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntryEx stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntryEx        The indirect pointer to the extended gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntryEx is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntryEx is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGaugeEx (
+  IN  UINTN                 LogEntryKey,
+  OUT GAUGE_DATA_ENTRY_EX   **GaugeDataEntryEx
+  )
+{
+  UINTN               NumberOfEntries;
+  GAUGE_DATA_ENTRY_EX *GaugeEntryExArray;
+
+  NumberOfEntries = (UINTN) (mGaugeData->NumberOfEntries);
+  if (LogEntryKey > NumberOfEntries) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (LogEntryKey == NumberOfEntries) {
+    return EFI_NOT_FOUND;
+  }
+
+  GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+  if (GaugeDataEntryEx == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *GaugeDataEntryEx = &GaugeEntryExArray[LogEntryKey];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartGaugeEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndGaugeEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGaugeEx and EndGaugeEx of PERFORMANCE_EX_PROTOCOL,
+  and then eliminate the Identifier.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntry stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntry          The indirect pointer to the gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntry is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntry is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGauge (
+  IN  UINTN               LogEntryKey,
+  OUT GAUGE_DATA_ENTRY    **GaugeDataEntry
+  )
+{
+  EFI_STATUS          Status;
+  GAUGE_DATA_ENTRY_EX *GaugeEntryEx;
+
+  GaugeEntryEx = NULL;
+
+  Status = GetGaugeEx (LogEntryKey, &GaugeEntryEx);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (GaugeDataEntry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *GaugeDataEntry = (GAUGE_DATA_ENTRY *) GaugeEntryEx;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Communication service SMI Handler entry.
+
+  This SMI handler provides services for the performance wrapper driver.
+  
+   Caution: This function may receive untrusted input.
+   Communicate buffer and buffer size are external input, so this function will do basic validation.
+
+  @param[in]     DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param[in]     RegisterContext Points to an optional handler context which was specified when the
+                                 handler was registered.
+  @param[in, out] CommBuffer     A pointer to a collection of data in memory that will
+                                 be conveyed from a non-SMM environment into an SMM environment.
+  @param[in, out] CommBufferSize The size of the CommBuffer.
+
+  @retval EFI_SUCCESS                         The interrupt was handled and quiesced. No other handlers 
+                                              should still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED  The interrupt has been quiesced but other handlers should 
+                                              still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING   The interrupt is still pending and other handlers should still 
+                                              be called.
+  @retval EFI_INTERRUPT_PENDING               The interrupt could not be quiesced.
+**/
+EFI_STATUS
+EFIAPI
+SmmPerformanceHandlerEx (
+  IN     EFI_HANDLE                    DispatchHandle,
+  IN     CONST VOID                   *RegisterContext,
+  IN OUT VOID                          *CommBuffer,
+  IN OUT UINTN                         *CommBufferSize
+  )
+{
+  EFI_STATUS                Status;
+  SMM_PERF_COMMUNICATE_EX   *SmmPerfCommData;
+  GAUGE_DATA_ENTRY_EX       *GaugeEntryExArray;
+  UINT64                    DataSize;
+  UINTN                     Index;
+  GAUGE_DATA_ENTRY_EX       *GaugeDataEx;
+  UINTN                     NumberOfEntries;
+  UINTN                     LogEntryKey;
+  UINTN                     TempCommBufferSize;
+
+  GaugeEntryExArray = NULL;
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if(TempCommBufferSize < sizeof (SMM_PERF_COMMUNICATE_EX)) {
+    return EFI_SUCCESS;
+  }
+
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmmPerformanceHandlerEx: SMM communcation data buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+  
+  SmmPerfCommData = (SMM_PERF_COMMUNICATE_EX *)CommBuffer;
+
+  switch (SmmPerfCommData->Function) {
+    case SMM_PERF_FUNCTION_GET_GAUGE_ENTRY_NUMBER :
+       SmmPerfCommData->NumberOfEntries = mGaugeData->NumberOfEntries;
+       Status = EFI_SUCCESS;
+       break;
+
+    case SMM_PERF_FUNCTION_GET_GAUGE_DATA :
+      GaugeDataEx = SmmPerfCommData->GaugeDataEx;
+      NumberOfEntries = SmmPerfCommData->NumberOfEntries;
+      LogEntryKey = SmmPerfCommData->LogEntryKey;
+       if (GaugeDataEx == NULL || NumberOfEntries == 0 || LogEntryKey > mGaugeData->NumberOfEntries ||
+           NumberOfEntries > mGaugeData->NumberOfEntries || LogEntryKey > (mGaugeData->NumberOfEntries - NumberOfEntries)) {
+         Status = EFI_INVALID_PARAMETER;
+         break;
+       }
+
+       //
+       // Sanity check
+       //
+       DataSize = MultU64x32 (NumberOfEntries, sizeof(GAUGE_DATA_ENTRY_EX));
+       if (!SmmIsBufferOutsideSmmValid ((UINTN) GaugeDataEx, DataSize)) {
+         DEBUG ((EFI_D_ERROR, "SmmPerformanceHandlerEx: SMM Performance Data buffer in SMRAM or overflow!\n"));
+         Status = EFI_ACCESS_DENIED;
+         break;
+       }
+
+       GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+       for (Index = 0; Index < NumberOfEntries; Index++) {
+         CopyMem (
+           (UINT8 *) &GaugeDataEx[Index],
+           (UINT8 *) &GaugeEntryExArray[LogEntryKey++],
+           sizeof (GAUGE_DATA_ENTRY_EX)
+           );
+       }
+       Status = EFI_SUCCESS;
+       break;
+
+    default:
+       Status = EFI_UNSUPPORTED;
+  }
+
+
+  SmmPerfCommData->ReturnStatus = Status;
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Communication service SMI Handler entry.
+
+  This SMI handler provides services for the performance wrapper driver.
+
+  Caution: This function may receive untrusted input.
+  Communicate buffer and buffer size are external input, so this function will do basic validation.
+
+  @param[in]     DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param[in]     RegisterContext Points to an optional handler context which was specified when the
+                                 handler was registered.
+  @param[in, out] CommBuffer     A pointer to a collection of data in memory that will
+                                 be conveyed from a non-SMM environment into an SMM environment.
+  @param[in, out] CommBufferSize The size of the CommBuffer.
+
+  @retval EFI_SUCCESS                         The interrupt was handled and quiesced. No other handlers 
+                                              should still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED  The interrupt has been quiesced but other handlers should 
+                                              still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING   The interrupt is still pending and other handlers should still 
+                                              be called.
+  @retval EFI_INTERRUPT_PENDING               The interrupt could not be quiesced.
+**/
+EFI_STATUS
+EFIAPI
+SmmPerformanceHandler (
+  IN     EFI_HANDLE                    DispatchHandle,
+  IN     CONST VOID                   *RegisterContext,
+  IN OUT VOID                          *CommBuffer,
+  IN OUT UINTN                         *CommBufferSize
+  )
+{
+  EFI_STATUS            Status;
+  SMM_PERF_COMMUNICATE  *SmmPerfCommData;
+  GAUGE_DATA_ENTRY_EX   *GaugeEntryExArray;
+  UINT64                DataSize;
+  UINTN                 Index;
+  GAUGE_DATA_ENTRY      *GaugeData;
+  UINTN                 NumberOfEntries;
+  UINTN                 LogEntryKey;
+  UINTN                 TempCommBufferSize;
+
+  GaugeEntryExArray = NULL;
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if(TempCommBufferSize < sizeof (SMM_PERF_COMMUNICATE)) {
+    return EFI_SUCCESS;
+  }
+
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmmPerformanceHandler: SMM communcation data buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  SmmPerfCommData = (SMM_PERF_COMMUNICATE *)CommBuffer;
+
+  switch (SmmPerfCommData->Function) {
+    case SMM_PERF_FUNCTION_GET_GAUGE_ENTRY_NUMBER :
+       SmmPerfCommData->NumberOfEntries = mGaugeData->NumberOfEntries;
+       Status = EFI_SUCCESS;
+       break;
+
+    case SMM_PERF_FUNCTION_GET_GAUGE_DATA :
+       GaugeData = SmmPerfCommData->GaugeData;
+       NumberOfEntries = SmmPerfCommData->NumberOfEntries;
+       LogEntryKey = SmmPerfCommData->LogEntryKey;
+       if (GaugeData == NULL || NumberOfEntries == 0 || LogEntryKey > mGaugeData->NumberOfEntries ||
+           NumberOfEntries > mGaugeData->NumberOfEntries || LogEntryKey > (mGaugeData->NumberOfEntries - NumberOfEntries)) {
+         Status = EFI_INVALID_PARAMETER;
+         break;
+       }
+
+       //
+       // Sanity check
+       //
+       DataSize = MultU64x32 (NumberOfEntries, sizeof(GAUGE_DATA_ENTRY));
+       if (!SmmIsBufferOutsideSmmValid ((UINTN) GaugeData, DataSize)) {
+         DEBUG ((EFI_D_ERROR, "SmmPerformanceHandler: SMM Performance Data buffer in SMRAM or overflow!\n"));
+         Status = EFI_ACCESS_DENIED;
+         break;
+       }
+
+       GaugeEntryExArray = (GAUGE_DATA_ENTRY_EX *) (mGaugeData + 1);
+
+       for (Index = 0; Index < NumberOfEntries; Index++) {
+         CopyMem (
+           (UINT8 *) &GaugeData[Index],
+           (UINT8 *) &GaugeEntryExArray[LogEntryKey++],
+           sizeof (GAUGE_DATA_ENTRY)
+           );
+       }
+       Status = EFI_SUCCESS;
+       break;
+
+    default:
+       Status = EFI_UNSUPPORTED;
+  }
+
+
+  SmmPerfCommData->ReturnStatus = Status;
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  SmmBase2 protocol notify callback function, when SMST and SMM memory service get initialized 
+  this function is callbacked to initialize the Smm Performance Lib 
+
+  @param  Event    The event of notify protocol.
+  @param  Context  Notify event context.
+
+**/
+VOID
+EFIAPI
+InitializeSmmCorePerformanceLib (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HANDLE                Handle;
+
+  //
+  // Initialize spin lock
+  //
+  InitializeSpinLock (&mSmmPerfLock);
+
+  mMaxGaugeRecords = INIT_SMM_GAUGE_DATA_ENTRIES;
+
+  mGaugeData = AllocateZeroPool (sizeof (GAUGE_DATA_HEADER) + (sizeof (GAUGE_DATA_ENTRY_EX) * mMaxGaugeRecords));
+  ASSERT (mGaugeData != NULL);
+  
+  //
+  // Install the protocol interfaces.
+  //
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &mHandle,
+                    &gSmmPerformanceProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mPerformanceInterface
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &mHandle,
+                    &gSmmPerformanceExProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mPerformanceExInterface
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Register SMM Performance SMI handler
+  ///
+  Handle = NULL;
+  Status = gSmst->SmiHandlerRegister (SmmPerformanceHandler, &gSmmPerformanceProtocolGuid, &Handle);
+  ASSERT_EFI_ERROR (Status);
+  Status = gSmst->SmiHandlerRegister (SmmPerformanceHandlerEx, &gSmmPerformanceExProtocolGuid, &Handle);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  The constructor function initializes the Performance Measurement Enable flag and 
+  registers SmmBase2 protocol notify callback.
+  It will ASSERT() if one of these operations fails and it will always return EFI_SUCCESS.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmCorePerformanceLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   Event;
+  VOID        *Registration;
+
+  mPerformanceMeasurementEnabled =  (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+  if (!mPerformanceMeasurementEnabled) {
+    //
+    // Do not initialize performance infrastructure if not required.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Create the events to do the library init.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  InitializeSmmCorePerformanceLib,
+                  NULL,
+                  &Event
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register for protocol notifications on this event
+  //
+  Status = gBS->RegisterProtocolNotify (
+                  &gEfiSmmBase2ProtocolGuid,
+                  Event,
+                  &Registration
+                  );
+
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return (RETURN_STATUS) StartGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  return (RETURN_STATUS) EndGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  EFI_STATUS           Status;
+  GAUGE_DATA_ENTRY_EX  *GaugeData;
+
+  GaugeData = NULL;
+  
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  Status = GetGaugeEx (LogEntryKey++, &GaugeData);
+
+  //
+  // Make sure that LogEntryKey is a valid log entry key,
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // The LogEntryKey is the last entry (equals to the total entry number).
+    //
+    return 0;
+  }
+
+  ASSERT (GaugeData != NULL);
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+  *Identifier     = GaugeData->Identifier;
+
+  return LogEntryKey;
+}
+
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return mPerformanceMeasurementEnabled;
+}
diff --git a/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.inf b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.inf
new file mode 100644
index 0000000000..160a749390
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.inf
@@ -0,0 +1,73 @@
+## @file
+#  Performance library instance used by SMM Core.
+#  
+#  This library provides the performance measurement interfaces and initializes performance
+#  logging for the SMM phase.
+#  It initializes SMM phase performance logging by publishing the SMM Performance and PerformanceEx Protocol,
+#  which is consumed by SmmPerformanceLib to logging performance data in SMM phase.
+#  This library is mainly used by SMM Core to start performance logging to ensure that
+#  SMM Performance and PerformanceEx Protocol are installed at the very beginning of SMM phase.
+#  
+#  Copyright (c) 2011 - 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmCorePerformanceLib
+  MODULE_UNI_FILE                = SmmCorePerformanceLib.uni
+  FILE_GUID                      = 36290D10-0F47-42c1-BBCE-E191C7928DCF
+  MODULE_TYPE                    = SMM_CORE
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  LIBRARY_CLASS                  = PerformanceLib|SMM_CORE
+
+  CONSTRUCTOR                    = SmmCorePerformanceLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  SmmCorePerformanceLib.c
+  SmmCorePerformanceLibInternal.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  PcdLib
+  TimerLib
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  SynchronizationLib
+  SmmServicesTableLib
+  SmmMemLib
+
+[Protocols]
+  gEfiSmmBase2ProtocolGuid                  ## CONSUMES
+
+[Guids]
+  ## PRODUCES ## UNDEFINED # Install protocol
+  ## CONSUMES ## UNDEFINED # SmiHandlerRegister
+  gSmmPerformanceProtocolGuid
+  ## PRODUCES ## UNDEFINED # Install protocol
+  ## CONSUMES ## UNDEFINED # SmiHandlerRegister
+  gSmmPerformanceExProtocolGuid
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.uni b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.uni
new file mode 100644
index 0000000000..40c74dc5d8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLibInternal.h b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLibInternal.h
new file mode 100644
index 0000000000..8eb30320ee
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLibInternal.h
@@ -0,0 +1,236 @@
+/** @file
+  Master header files for SmmCorePerformanceLib instance.
+
+  This header file holds the prototypes of the SMM Performance and PerformanceEx Protocol published by this
+  library instance at its constructor.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+  
+#ifndef _SMM_CORE_PERFORMANCE_LIB_INTERNAL_H_
+#define _SMM_CORE_PERFORMANCE_LIB_INTERNAL_H_
+
+
+#include <Guid/Performance.h>
+
+#include <Library/SmmServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiLib.h>                   
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/SmmMemLib.h>
+
+#include <Protocol/SmmBase2.h>
+
+//
+// Interface declarations for SMM PerformanceEx Protocol.
+//
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, Module and Identifier.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, Module and Identifier and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndGauge of PERFORMANCE_PROTOCOL.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGaugeEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGauge and EndGauge of PERFORMANCE_PROTOCOL,
+  and then assign the Identifier with 0.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntryEx stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntryEx        The indirect pointer to the extended gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntryEx is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntryEx is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGaugeEx (
+  IN  UINTN                 LogEntryKey,
+  OUT GAUGE_DATA_ENTRY_EX   **GaugeDataEntryEx
+  );
+
+//
+// Interface declarations for SMM Performance Protocol.
+//
+/**
+  Adds a record at the end of the performance measurement log
+  that records the start time of a performance measurement.
+
+  Adds a record to the end of the performance measurement log
+  that contains the Handle, Token, and Module.
+  The end time of the new record must be set to zero.
+  If TimeStamp is not zero, then TimeStamp is used to fill in the start time in the record.
+  If TimeStamp is zero, the start time in the record is filled in with the value
+  read from the current time stamp.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The data was read correctly from the device.
+  @retval EFI_OUT_OF_RESOURCES    There are not enough resources to record the measurement.
+
+**/
+EFI_STATUS
+EFIAPI
+StartGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Searches the performance measurement log from the beginning of the log
+  for the first matching record that contains a zero end time and fills in a valid end time.
+
+  Searches the performance measurement log from the beginning of the log
+  for the first record that matches Handle, Token, and Module and has an end time value of zero.
+  If the record can not be found then return EFI_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then the end time in the record is filled in with the value specified by TimeStamp.
+  If the record is found and TimeStamp is zero, then the end time in the matching record
+  is filled in with the current time stamp value.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval EFI_SUCCESS             The end of  the measurement was recorded.
+  @retval EFI_NOT_FOUND           The specified measurement record could not be found.
+
+**/
+EFI_STATUS
+EFIAPI
+EndGauge (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  );
+
+/**
+  Retrieves a previously logged performance measurement.
+  It can also retrieve the log created by StartGaugeEx and EndGaugeEx of PERFORMANCE_EX_PROTOCOL,
+  and then eliminate the Identifier.
+
+  Retrieves the performance log entry from the performance log specified by LogEntryKey.
+  If it stands for a valid entry, then EFI_SUCCESS is returned and
+  GaugeDataEntry stores the pointer to that entry.
+
+  @param  LogEntryKey             The key for the previous performance measurement log entry.
+                                  If 0, then the first performance measurement log entry is retrieved.
+  @param  GaugeDataEntry          The indirect pointer to the gauge data entry specified by LogEntryKey
+                                  if the retrieval is successful.
+
+  @retval EFI_SUCCESS             The GuageDataEntry is successfully found based on LogEntryKey.
+  @retval EFI_NOT_FOUND           The LogEntryKey is the last entry (equals to the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  The LogEntryKey is not a valid entry (greater than the total entry number).
+  @retval EFI_INVALIDE_PARAMETER  GaugeDataEntry is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetGauge (
+  IN  UINTN               LogEntryKey,
+  OUT GAUGE_DATA_ENTRY    **GaugeDataEntry
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.c b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.c
new file mode 100644
index 0000000000..ef38c8fdf5
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.c
@@ -0,0 +1,52 @@
+/** @file
+  Null instance of SmmCorePlatformHookLibNull.
+
+  Copyright (c) 2011, 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 <Library/SmmCorePlatformHookLib.h>
+
+/**
+  Performs platform specific tasks before invoking registered SMI handlers.
+  
+  This function performs platform specific tasks before invoking registered SMI handlers.
+  
+  @retval EFI_SUCCESS       The platform hook completes successfully.
+  @retval Other values      The paltform hook cannot complete due to some error.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformHookBeforeSmmDispatch (
+  VOID
+  )
+{
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Performs platform specific tasks after invoking registered SMI handlers.
+  
+  This function performs platform specific tasks after invoking registered SMI handlers.
+  
+  @retval EFI_SUCCESS       The platform hook completes successfully.
+  @retval Other values      The paltform hook cannot complete due to some error.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatformHookAfterSmmDispatch (
+  VOID
+  )
+{
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.inf b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.inf
new file mode 100644
index 0000000000..ff7c1d26b3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.inf
@@ -0,0 +1,36 @@
+## @file
+#  SMM Core Platform Hook Null Library instance
+#
+#  Copyright (c) 2011 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmCorePlatformHookLibNull
+  MODULE_UNI_FILE                = SmmCorePlatformHookLibNull.uni
+  FILE_GUID                      = FED6583D-2418-4760-AC96-B5E18F0A6326
+  MODULE_TYPE                    = SMM_CORE
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  LIBRARY_CLASS                  = SmmCorePlatformHookLib|SMM_CORE
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  SmmCorePlatformHookLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.uni b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.uni
new file mode 100644
index 0000000000..40d79a3804
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.uni
diff --git a/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.c b/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.c
new file mode 100644
index 0000000000..70d12a2138
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.c
@@ -0,0 +1,82 @@
+/** @file
+  Implementation of Ipmi Library for SMM.
+
+  Copyright (c) 2009 - 2015, 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 <PiSmm.h>
+#include <Protocol/IpmiProtocol.h>
+#include <Library/IpmiLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/DebugLib.h>
+
+IPMI_PROTOCOL *mIpmiProtocol = NULL;
+
+/**
+  This service enables submitting commands via Ipmi.
+
+  @param[in]         NetFunction       Net function of the command.
+  @param[in]         Command           IPMI Command.
+  @param[in]         RequestData       Command Request Data.
+  @param[in]         RequestDataSize   Size of Command Request Data.
+  @param[out]        ResponseData      Command Response Data. The completion code is the first byte of response data.
+  @param[in, out]    ResponseDataSize  Size of Command Response Data.
+
+  @retval EFI_SUCCESS            The command byte stream was successfully submit to the device and a response was successfully received.
+  @retval EFI_NOT_FOUND          The command was not successfully sent to the device or a response was not successfully received from the device.
+  @retval EFI_NOT_READY          Ipmi Device is not ready for Ipmi command access.
+  @retval EFI_DEVICE_ERROR       Ipmi Device hardware error.
+  @retval EFI_TIMEOUT            The command time out.
+  @retval EFI_UNSUPPORTED        The command was not successfully sent to the device.
+  @retval EFI_OUT_OF_RESOURCES   The resource allcation is out of resource or data size error.
+**/
+EFI_STATUS
+EFIAPI
+IpmiSubmitCommand (
+  IN     UINT8     NetFunction,
+  IN     UINT8     Command,
+  IN     UINT8     *RequestData,
+  IN     UINT32    RequestDataSize,
+     OUT UINT8     *ResponseData,
+  IN OUT UINT32    *ResponseDataSize
+  )
+{
+  EFI_STATUS  Status;
+
+  if (mIpmiProtocol == NULL) {
+    Status = gSmst->SmmLocateProtocol (
+                  &gSmmIpmiProtocolGuid,
+                  NULL,
+                  (VOID **) &mIpmiProtocol
+                  );
+    if (EFI_ERROR (Status)) {
+      //
+      // Smm Ipmi Protocol is not installed. So, IPMI device is not present.
+      //
+      DEBUG ((EFI_D_ERROR, "IpmiSubmitCommand for SMM Status - %r\n", Status));
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  Status = mIpmiProtocol->IpmiSubmitCommand (
+                            mIpmiProtocol,
+                            NetFunction,
+                            Command,
+                            RequestData,
+                            RequestDataSize,
+                            ResponseData,
+                            ResponseDataSize
+                            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.inf b/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.inf
new file mode 100644
index 0000000000..f13e5a433b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.inf
@@ -0,0 +1,40 @@
+## @file
+#  Instance of SMM IPMI Library.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmIpmiLibSmmIpmiProtocol
+  FILE_GUID                      = B422FB70-E835-448D-A921-EBA460E105B6
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = IpmiLib|DXE_SMM_DRIVER SMM_CORE
+
+#
+#  VALID_ARCHITECTURES          = IA32 X64
+#
+
+[Sources]
+  SmmIpmiLibSmmIpmiProtocol.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DebugLib
+  SmmServicesTableLib
+
+[Protocols]
+  gSmmIpmiProtocolGuid      ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.c b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.c
new file mode 100644
index 0000000000..9659f014e9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.c
@@ -0,0 +1,455 @@
+/** @file
+
+Copyright (c) 2010, 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 <PiDxe.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/DebugLib.h>
+#include <Protocol/SmmCommunication.h>
+#include <Guid/SmmLockBox.h>
+
+#include "SmmLockBoxLibPrivate.h"
+
+/**
+  This function will save confidential information to lockbox.
+
+  @param Guid       the guid to identify the confidential information
+  @param Buffer     the address of the confidential information
+  @param Length     the length of the confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0
+  @retval RETURN_ALREADY_STARTED    the requested GUID already exist.
+  @retval RETURN_OUT_OF_RESOURCES   no enough resource to save the information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SaveLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL  *SmmCommunication;
+  EFI_SMM_LOCK_BOX_PARAMETER_SAVE *LockBoxParameterSave;
+  EFI_SMM_COMMUNICATE_HEADER      *CommHeader;
+  UINT8                           CommBuffer[sizeof(EFI_GUID) + sizeof(UINTN) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_SAVE)];
+  UINTN                           CommSize;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib SaveLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) || (Buffer == NULL) || (Length == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get needed resource
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmmCommunicationProtocolGuid,
+                  NULL,
+                  (VOID **)&SmmCommunication
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  CommHeader->MessageLength = sizeof(*LockBoxParameterSave);
+
+  LockBoxParameterSave = (EFI_SMM_LOCK_BOX_PARAMETER_SAVE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  LockBoxParameterSave->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_SAVE;
+  LockBoxParameterSave->Header.DataLength = sizeof(*LockBoxParameterSave);
+  LockBoxParameterSave->Header.ReturnStatus = (UINT64)-1;
+  CopyMem (&LockBoxParameterSave->Guid, Guid, sizeof(*Guid));
+  LockBoxParameterSave->Buffer     = (EFI_PHYSICAL_ADDRESS)(UINTN)Buffer;
+  LockBoxParameterSave->Length     = (UINT64)Length;
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunication->Communicate (
+                               SmmCommunication,
+                               &CommBuffer[0],
+                               &CommSize
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (EFI_STATUS)LockBoxParameterSave->Header.ReturnStatus;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib SaveLockBox - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
+/**
+  This function will set lockbox attributes.
+
+  @param Guid       the guid to identify the confidential information
+  @param Attributes the attributes of the lockbox
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  attributes is invalid.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SetLockBoxAttributes (
+  IN  GUID                        *Guid,
+  IN  UINT64                      Attributes
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL            *SmmCommunication;
+  EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES *LockBoxParameterSetAttributes;
+  EFI_SMM_COMMUNICATE_HEADER                *CommHeader;
+  UINT8                                     CommBuffer[sizeof(EFI_GUID) + sizeof(UINTN) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES)];
+  UINTN                                     CommSize;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib SetLockBoxAttributes - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) ||
+      ((Attributes & ~LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get needed resource
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmmCommunicationProtocolGuid,
+                  NULL,
+                  (VOID **)&SmmCommunication
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  CommHeader->MessageLength = sizeof(*LockBoxParameterSetAttributes);
+
+  LockBoxParameterSetAttributes = (EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  LockBoxParameterSetAttributes->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_SET_ATTRIBUTES;
+  LockBoxParameterSetAttributes->Header.DataLength = sizeof(*LockBoxParameterSetAttributes);
+  LockBoxParameterSetAttributes->Header.ReturnStatus = (UINT64)-1;
+  CopyMem (&LockBoxParameterSetAttributes->Guid, Guid, sizeof(*Guid));
+  LockBoxParameterSetAttributes->Attributes = (UINT64)Attributes;
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunication->Communicate (
+                               SmmCommunication,
+                               &CommBuffer[0],
+                               &CommSize
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (EFI_STATUS)LockBoxParameterSetAttributes->Header.ReturnStatus;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib SetLockBoxAttributes - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
+/**
+  This function will update confidential information to lockbox.
+
+  @param Guid   the guid to identify the original confidential information
+  @param Offset the offset of the original confidential information
+  @param Buffer the address of the updated confidential information
+  @param Length the length of the updated confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_BUFFER_TOO_SMALL   the original buffer to too small to hold new information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+UpdateLockBox (
+  IN  GUID                        *Guid,
+  IN  UINTN                       Offset,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL    *SmmCommunication;
+  EFI_SMM_LOCK_BOX_PARAMETER_UPDATE *LockBoxParameterUpdate;
+  EFI_SMM_COMMUNICATE_HEADER        *CommHeader;
+  UINT8                             CommBuffer[sizeof(EFI_GUID) + sizeof(UINTN) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_UPDATE)];
+  UINTN                             CommSize;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib UpdateLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) || (Buffer == NULL) || (Length == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get needed resource
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmmCommunicationProtocolGuid,
+                  NULL,
+                  (VOID **)&SmmCommunication
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  CommHeader->MessageLength = sizeof(*LockBoxParameterUpdate);
+
+  LockBoxParameterUpdate = (EFI_SMM_LOCK_BOX_PARAMETER_UPDATE *)(UINTN)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  LockBoxParameterUpdate->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_UPDATE;
+  LockBoxParameterUpdate->Header.DataLength = sizeof(*LockBoxParameterUpdate);
+  LockBoxParameterUpdate->Header.ReturnStatus = (UINT64)-1;
+  CopyMem (&LockBoxParameterUpdate->Guid, Guid, sizeof(*Guid));
+  LockBoxParameterUpdate->Offset = (UINT64)Offset;
+  LockBoxParameterUpdate->Buffer = (EFI_PHYSICAL_ADDRESS)(UINTN)Buffer;
+  LockBoxParameterUpdate->Length = (UINT64)Length;
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunication->Communicate (
+                               SmmCommunication,
+                               &CommBuffer[0],
+                               &CommSize
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (EFI_STATUS)LockBoxParameterUpdate->Header.ReturnStatus;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib UpdateLockBox - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
+/**
+  This function will restore confidential information from lockbox.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or one of Buffer and Length is NULL.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_ACCESS_DENIED      not allow to restore to the address
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL     *SmmCommunication;
+  EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *LockBoxParameterRestore;
+  EFI_SMM_COMMUNICATE_HEADER         *CommHeader;
+  UINT8                              CommBuffer[sizeof(EFI_GUID) + sizeof(UINTN) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE)];
+  UINTN                              CommSize;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib RestoreLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) ||
+      ((Buffer == NULL) && (Length != NULL)) ||
+      ((Buffer != NULL) && (Length == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get needed resource
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmmCommunicationProtocolGuid,
+                  NULL,
+                  (VOID **)&SmmCommunication
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  CommHeader->MessageLength = sizeof(*LockBoxParameterRestore);
+
+  LockBoxParameterRestore = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  LockBoxParameterRestore->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_RESTORE;
+  LockBoxParameterRestore->Header.DataLength = sizeof(*LockBoxParameterRestore);
+  LockBoxParameterRestore->Header.ReturnStatus = (UINT64)-1;
+  CopyMem (&LockBoxParameterRestore->Guid, Guid, sizeof(*Guid));
+  LockBoxParameterRestore->Buffer = (EFI_PHYSICAL_ADDRESS)(UINTN)Buffer;
+  if (Length != NULL) {
+    LockBoxParameterRestore->Length = (EFI_PHYSICAL_ADDRESS)*Length;
+  } else {
+    LockBoxParameterRestore->Length = 0;
+  }
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunication->Communicate (
+                               SmmCommunication,
+                               &CommBuffer[0],
+                               &CommSize
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  if (Length != NULL) {
+    *Length = (UINTN)LockBoxParameterRestore->Length;
+  }
+
+  Status = (EFI_STATUS)LockBoxParameterRestore->Header.ReturnStatus;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib RestoreLockBox - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreAllLockBoxInPlace (
+  VOID
+  )
+{
+  EFI_STATUS                                      Status;
+  EFI_SMM_COMMUNICATION_PROTOCOL                  *SmmCommunication;
+  EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *LockBoxParameterRestoreAllInPlace;
+  EFI_SMM_COMMUNICATE_HEADER                      *CommHeader;
+  UINT8                                           CommBuffer[sizeof(EFI_GUID) + sizeof(UINTN) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE)];
+  UINTN                                           CommSize;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib RestoreAllLockBoxInPlace - Enter\n"));
+
+  //
+  // Get needed resource
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSmmCommunicationProtocolGuid,
+                  NULL,
+                  (VOID **)&SmmCommunication
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  CommHeader->MessageLength = sizeof(*LockBoxParameterRestoreAllInPlace);
+
+  LockBoxParameterRestoreAllInPlace = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data)];
+  LockBoxParameterRestoreAllInPlace->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_RESTORE_ALL_IN_PLACE;
+  LockBoxParameterRestoreAllInPlace->Header.DataLength = sizeof(*LockBoxParameterRestoreAllInPlace);
+  LockBoxParameterRestoreAllInPlace->Header.ReturnStatus = (UINT64)-1;
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunication->Communicate (
+                               SmmCommunication,
+                               &CommBuffer[0],
+                               &CommSize
+                               );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (EFI_STATUS)LockBoxParameterRestoreAllInPlace->Header.ReturnStatus;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxDxeLib RestoreAllLockBoxInPlace - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
new file mode 100644
index 0000000000..48cdb9c66a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
@@ -0,0 +1,50 @@
+## @file
+#  DXE LockBox library instance.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmLockBoxDxeLib
+  MODULE_UNI_FILE                = SmmLockBoxDxeLib.uni
+  FILE_GUID                      = 4A0054B4-3CA8-4e1b-9339-9B58D5FBB7D2
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = LockBoxLib|DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_DRIVER UEFI_APPLICATION
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 
+#
+
+[Sources]
+  SmmLockBoxDxeLib.c
+  SmmLockBoxLibPrivate.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+
+[Guids]
+  gEfiSmmLockBoxCommunicationGuid       ## SOMETIMES_CONSUMES ## GUID # Used to do smm communication
+
+[Protocols]
+  gEfiSmmCommunicationProtocolGuid      ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.uni b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.uni
new file mode 100644
index 0000000000..78aa7db639
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxLibPrivate.h b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxLibPrivate.h
new file mode 100644
index 0000000000..31da89af0f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxLibPrivate.h
@@ -0,0 +1,54 @@
+/** @file
+
+Copyright (c) 2010, 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.
+
+**/
+
+#ifndef _SMM_LOCK_BOX_LIB_PRIVATE_H_
+#define _SMM_LOCK_BOX_LIB_PRIVATE_H_
+
+#include <Uefi.h>
+
+#pragma pack(1)
+
+//
+// Below data structure is used for lockbox registration in SMST
+//
+
+#define SMM_LOCK_BOX_SIGNATURE_32 SIGNATURE_64 ('L','O','C','K','B','_','3','2')
+#define SMM_LOCK_BOX_SIGNATURE_64 SIGNATURE_64 ('L','O','C','K','B','_','6','4')
+
+typedef struct {
+  UINT64                   Signature;
+  EFI_PHYSICAL_ADDRESS     LockBoxDataAddress;
+} SMM_LOCK_BOX_CONTEXT;
+
+//
+// Below data structure is used for lockbox management
+//
+
+#define SMM_LOCK_BOX_DATA_SIGNATURE SIGNATURE_64 ('L','O','C','K','B','O','X','D')
+
+typedef struct {
+  UINT64                         Signature;
+  EFI_GUID                       Guid;
+  EFI_PHYSICAL_ADDRESS           Buffer;
+  UINT64                         Length;
+  UINT64                         Attributes;
+  EFI_PHYSICAL_ADDRESS           SmramBuffer;
+  LIST_ENTRY                     Link;
+} SMM_LOCK_BOX_DATA;
+
+#pragma pack()
+
+#endif
+
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.c b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.c
new file mode 100644
index 0000000000..bd3204b195
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.c
@@ -0,0 +1,741 @@
+/** @file
+
+Copyright (c) 2010 - 2011, 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 <PiPei.h>
+#include <PiDxe.h>
+#include <PiSmm.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/HobLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Protocol/SmmCommunication.h>
+#include <Ppi/SmmCommunication.h>
+#include <Ppi/SmmAccess.h>
+#include <Guid/AcpiS3Context.h>
+#include <Guid/SmmLockBox.h>
+
+#include "SmmLockBoxLibPrivate.h"
+
+#if defined (MDE_CPU_IA32)
+typedef struct _LIST_ENTRY64 LIST_ENTRY64;
+struct _LIST_ENTRY64 {
+  LIST_ENTRY64  *ForwardLink;
+  UINT32        Reserved1;
+  LIST_ENTRY64  *BackLink;
+  UINT32        Reserved2;
+};
+
+typedef struct {
+  EFI_TABLE_HEADER    Hdr;
+  UINT64              SmmFirmwareVendor;
+  UINT64              SmmFirmwareRevision;
+  UINT64              SmmInstallConfigurationTable;
+  UINT64              SmmIoMemRead;
+  UINT64              SmmIoMemWrite;
+  UINT64              SmmIoIoRead;
+  UINT64              SmmIoIoWrite;
+  UINT64              SmmAllocatePool;
+  UINT64              SmmFreePool;
+  UINT64              SmmAllocatePages;
+  UINT64              SmmFreePages;
+  UINT64              SmmStartupThisAp;
+  UINT64              CurrentlyExecutingCpu;
+  UINT64              NumberOfCpus;
+  UINT64              CpuSaveStateSize;
+  UINT64              CpuSaveState;
+  UINT64              NumberOfTableEntries;
+  UINT64              SmmConfigurationTable;
+} EFI_SMM_SYSTEM_TABLE2_64;
+
+typedef struct {
+  EFI_GUID                          VendorGuid;
+  UINT64                            VendorTable;
+} EFI_CONFIGURATION_TABLE64;
+#endif
+
+#if defined (MDE_CPU_X64)
+typedef LIST_ENTRY LIST_ENTRY64;
+typedef EFI_SMM_SYSTEM_TABLE2 EFI_SMM_SYSTEM_TABLE2_64;
+typedef EFI_CONFIGURATION_TABLE EFI_CONFIGURATION_TABLE64;
+#endif
+
+/**
+  This function return first node of LinkList queue.
+
+  @param LockBoxQueue  LinkList queue
+
+  @return first node of LinkList queue
+**/
+LIST_ENTRY *
+InternalInitLinkDxe (
+  IN LIST_ENTRY *LinkList
+  )
+{
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    //
+    // 32 PEI + 64 DXE
+    //
+    return (LIST_ENTRY *)(((LIST_ENTRY64 *)LinkList)->ForwardLink);
+  } else {
+    return LinkList->ForwardLink;
+  }
+}
+
+/**
+  This function return next node of LinkList.
+
+  @param Link  LinkList node
+
+  @return next node of LinkList
+**/
+LIST_ENTRY *
+InternalNextLinkDxe (
+  IN LIST_ENTRY *Link
+  )
+{
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    //
+    // 32 PEI + 64 DXE
+    //
+    return (LIST_ENTRY *)(((LIST_ENTRY64 *)Link)->ForwardLink);
+  } else {
+    return Link->ForwardLink;
+  }
+}
+
+/**
+  This function find LockBox by GUID from SMRAM.
+
+  @param LockBoxQueue The LockBox queue in SMRAM
+  @param Guid         The guid to indentify the LockBox
+
+  @return LockBoxData
+**/
+SMM_LOCK_BOX_DATA *
+InternalFindLockBoxByGuidFromSmram (
+  IN LIST_ENTRY *LockBoxQueue,
+  IN EFI_GUID   *Guid
+  )
+{
+  LIST_ENTRY                    *Link;
+  SMM_LOCK_BOX_DATA             *LockBox;
+
+  for (Link = InternalInitLinkDxe (LockBoxQueue);
+       Link != LockBoxQueue;
+       Link = InternalNextLinkDxe (Link)) {
+    LockBox = BASE_CR (
+                Link,
+                SMM_LOCK_BOX_DATA,
+                Link
+                );
+    if (CompareGuid (&LockBox->Guid, Guid)) {
+      return LockBox;
+    }
+  }
+  return NULL;
+}
+
+/**
+  Get VendorTable by VendorGuid in Smst.
+
+  @param Signature  Signature of SMM_S3_RESUME_STATE
+  @param Smst       SMM system table
+  @param VendorGuid vendor guid
+
+  @return vendor table.
+**/
+VOID *
+InternalSmstGetVendorTableByGuid (
+  IN UINT64                                        Signature,
+  IN EFI_SMM_SYSTEM_TABLE2                         *Smst,
+  IN EFI_GUID                                      *VendorGuid
+  )
+{
+  EFI_CONFIGURATION_TABLE                       *SmmConfigurationTable;
+  UINTN                                         NumberOfTableEntries;
+  UINTN                                         Index;
+  EFI_SMM_SYSTEM_TABLE2_64                      *Smst64;
+  EFI_CONFIGURATION_TABLE64                     *SmmConfigurationTable64;
+
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode))) {
+    //
+    // 32 PEI + 64 DXE
+    //
+    Smst64 = (EFI_SMM_SYSTEM_TABLE2_64 *)Smst;
+    SmmConfigurationTable64 = (EFI_CONFIGURATION_TABLE64 *)(UINTN)Smst64->SmmConfigurationTable;
+    NumberOfTableEntries = (UINTN)Smst64->NumberOfTableEntries;
+    for (Index = 0; Index < NumberOfTableEntries; Index++) {
+      if (CompareGuid (&SmmConfigurationTable64[Index].VendorGuid, VendorGuid)) {
+        return (VOID *)(UINTN)SmmConfigurationTable64[Index].VendorTable;
+      }
+    }
+    return NULL;
+  } else {
+    SmmConfigurationTable = Smst->SmmConfigurationTable;
+    NumberOfTableEntries = Smst->NumberOfTableEntries;
+    for (Index = 0; Index < NumberOfTableEntries; Index++) {
+      if (CompareGuid (&SmmConfigurationTable[Index].VendorGuid, VendorGuid)) {
+        return (VOID *)SmmConfigurationTable[Index].VendorTable;
+      }
+    }
+    return NULL;
+  }
+}
+
+/**
+  Get SMM LockBox context.
+
+  @return SMM LockBox context.
+**/
+SMM_LOCK_BOX_CONTEXT *
+InternalGetSmmLockBoxContext (
+  VOID
+  )
+{
+  EFI_SMRAM_DESCRIPTOR                          *SmramDescriptor;
+  SMM_S3_RESUME_STATE                           *SmmS3ResumeState;
+  VOID                                          *GuidHob;
+  SMM_LOCK_BOX_CONTEXT                          *SmmLockBoxContext;
+
+  GuidHob = GetFirstGuidHob (&gEfiAcpiVariableGuid);
+  ASSERT (GuidHob != NULL);
+  SmramDescriptor = (EFI_SMRAM_DESCRIPTOR *) GET_GUID_HOB_DATA (GuidHob);
+  SmmS3ResumeState = (SMM_S3_RESUME_STATE *)(UINTN)SmramDescriptor->CpuStart;
+
+  SmmLockBoxContext = (SMM_LOCK_BOX_CONTEXT *)InternalSmstGetVendorTableByGuid (
+                                                SmmS3ResumeState->Signature,
+                                                (EFI_SMM_SYSTEM_TABLE2 *)(UINTN)SmmS3ResumeState->Smst,
+                                                &gEfiSmmLockBoxCommunicationGuid
+                                                );
+  ASSERT (SmmLockBoxContext != NULL);
+
+  return SmmLockBoxContext;
+}
+
+/**
+  This function will restore confidential information from lockbox in SMRAM directly.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+**/
+EFI_STATUS
+InternalRestoreLockBoxFromSmram (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  )
+{
+  PEI_SMM_ACCESS_PPI             *SmmAccess;
+  UINTN                          Index;
+  EFI_STATUS                     Status;
+  SMM_LOCK_BOX_CONTEXT           *SmmLockBoxContext;
+  LIST_ENTRY                     *LockBoxQueue;
+  SMM_LOCK_BOX_DATA              *LockBox;
+  VOID                           *RestoreBuffer;
+
+  //
+  // Get needed resource
+  //
+  Status = PeiServicesLocatePpi (
+             &gPeiSmmAccessPpiGuid,
+             0,
+             NULL,
+             (VOID **)&SmmAccess
+             );
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; !EFI_ERROR (Status); Index++) {
+      Status = SmmAccess->Open ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);
+    }
+  }
+
+  //
+  // Get LockBox context
+  //
+  SmmLockBoxContext = InternalGetSmmLockBoxContext ();
+  LockBoxQueue = (LIST_ENTRY *)(UINTN)SmmLockBoxContext->LockBoxDataAddress;
+
+  //
+  // We do NOT check Buffer address in SMRAM, because if SMRAM not locked, we trust the caller.
+  //
+
+  //
+  // Restore this, Buffer and Length MUST be both NULL or both non-NULL
+  //
+
+  //
+  // Find LockBox
+  //
+  LockBox = InternalFindLockBoxByGuidFromSmram (LockBoxQueue, Guid);
+  if (LockBox == NULL) {
+    //
+    // Not found
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Set RestoreBuffer
+  //
+  if (Buffer != NULL) {
+    //
+    // restore to new buffer
+    //
+    RestoreBuffer = Buffer;
+  } else {
+    //
+    // restore to original buffer
+    //
+    if ((LockBox->Attributes & LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) == 0) {
+      return EFI_WRITE_PROTECTED;
+    }
+    RestoreBuffer = (VOID *)(UINTN)LockBox->Buffer;
+  }
+
+  //
+  // Set RestoreLength
+  //
+  if (Length != NULL) {
+    if (*Length < (UINTN)LockBox->Length) {
+      //
+      // Input buffer is too small to hold all data.
+      //
+      *Length = (UINTN)LockBox->Length;
+      return EFI_BUFFER_TOO_SMALL;
+    }
+    *Length = (UINTN)LockBox->Length;
+  }
+
+  //
+  // Restore data
+  //
+  CopyMem (RestoreBuffer, (VOID *)(UINTN)LockBox->SmramBuffer, (UINTN)LockBox->Length);
+
+  //
+  // Done
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+**/
+EFI_STATUS
+InternalRestoreAllLockBoxInPlaceFromSmram (
+  VOID
+  )
+{
+  PEI_SMM_ACCESS_PPI             *SmmAccess;
+  UINTN                          Index;
+  EFI_STATUS                     Status;
+  SMM_LOCK_BOX_CONTEXT           *SmmLockBoxContext;
+  LIST_ENTRY                     *LockBoxQueue;
+  SMM_LOCK_BOX_DATA              *LockBox;
+  LIST_ENTRY                     *Link;
+
+  //
+  // Get needed resource
+  //
+  Status = PeiServicesLocatePpi (
+             &gPeiSmmAccessPpiGuid,
+             0,
+             NULL,
+             (VOID **)&SmmAccess
+             );
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; !EFI_ERROR (Status); Index++) {
+      Status = SmmAccess->Open ((EFI_PEI_SERVICES **)GetPeiServicesTablePointer (), SmmAccess, Index);
+    }
+  }
+
+  //
+  // Get LockBox context
+  //
+  SmmLockBoxContext = InternalGetSmmLockBoxContext ();
+  LockBoxQueue = (LIST_ENTRY *)(UINTN)SmmLockBoxContext->LockBoxDataAddress;
+
+  //
+  // We do NOT check Buffer address in SMRAM, because if SMRAM not locked, we trust the caller.
+  //
+
+  //
+  // Restore all, Buffer and Length MUST be NULL
+  //
+  for (Link = InternalInitLinkDxe (LockBoxQueue);
+       Link != LockBoxQueue;
+       Link = InternalNextLinkDxe (Link)) {
+    LockBox = BASE_CR (
+                Link,
+                SMM_LOCK_BOX_DATA,
+                Link
+                );
+    if ((LockBox->Attributes & LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) != 0) {
+      //
+      // Restore data
+      //
+      CopyMem ((VOID *)(UINTN)LockBox->Buffer, (VOID *)(UINTN)LockBox->SmramBuffer, (UINTN)LockBox->Length);
+    }
+  }
+  //
+  // Done
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will save confidential information to lockbox.
+
+  @param Guid       the guid to identify the confidential information
+  @param Buffer     the address of the confidential information
+  @param Length     the length of the confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0
+  @retval RETURN_ALREADY_STARTED    the requested GUID already exist.
+  @retval RETURN_OUT_OF_RESOURCES   no enough resource to save the information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SaveLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  ASSERT (FALSE);
+
+  //
+  // No support to save at PEI phase
+  //
+  return RETURN_UNSUPPORTED;
+}
+
+/**
+  This function will set lockbox attributes.
+
+  @param Guid       the guid to identify the confidential information
+  @param Attributes the attributes of the lockbox
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  attributes is invalid.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SetLockBoxAttributes (
+  IN  GUID                        *Guid,
+  IN  UINT64                      Attributes
+  )
+{
+  ASSERT (FALSE);
+
+  //
+  // No support to save at PEI phase
+  //
+  return RETURN_UNSUPPORTED;
+}
+
+/**
+  This function will update confidential information to lockbox.
+
+  @param Guid   the guid to identify the original confidential information
+  @param Offset the offset of the original confidential information
+  @param Buffer the address of the updated confidential information
+  @param Length the length of the updated confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_BUFFER_TOO_SMALL   the original buffer to too small to hold new information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+UpdateLockBox (
+  IN  GUID                        *Guid,
+  IN  UINTN                       Offset,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  ASSERT (FALSE);
+
+  //
+  // No support to update at PEI phase
+  //
+  return RETURN_UNSUPPORTED;
+}
+
+/**
+  This function will restore confidential information from lockbox.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or one of Buffer and Length is NULL.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_ACCESS_DENIED      not allow to restore to the address
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_PEI_SMM_COMMUNICATION_PPI      *SmmCommunicationPpi;
+  EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *LockBoxParameterRestore;
+  EFI_SMM_COMMUNICATE_HEADER         *CommHeader;
+  UINT8                              CommBuffer[sizeof(EFI_GUID) + sizeof(UINT64) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE)];
+  UINTN                              CommSize;
+  UINT64                             MessageLength;
+
+  //
+  // Please aware that there is UINTN in EFI_SMM_COMMUNICATE_HEADER. It might be UINT64 in DXE, while it is UINT32 in PEI.
+  // typedef struct {
+  //   EFI_GUID  HeaderGuid;
+  //   UINTN     MessageLength;
+  //   UINT8     Data[1];
+  // } EFI_SMM_COMMUNICATE_HEADER;
+  //
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib RestoreLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) ||
+      ((Buffer == NULL) && (Length != NULL)) ||
+      ((Buffer != NULL) && (Length == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get needed resource
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiSmmCommunicationPpiGuid,
+             0,
+             NULL,
+             (VOID **)&SmmCommunicationPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    MessageLength = sizeof(*LockBoxParameterRestore);
+    CopyMem (&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength)], &MessageLength, sizeof(MessageLength));
+  } else {
+    CommHeader->MessageLength = sizeof(*LockBoxParameterRestore);
+  }
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib CommBuffer - %x\n", &CommBuffer[0]));
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    LockBoxParameterRestore = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength) + sizeof(UINT64)];
+  } else {
+    LockBoxParameterRestore = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength) + sizeof(UINTN)];
+  }
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib LockBoxParameterRestore - %x\n", LockBoxParameterRestore));
+  LockBoxParameterRestore->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_RESTORE;
+  LockBoxParameterRestore->Header.DataLength = sizeof(*LockBoxParameterRestore);
+  LockBoxParameterRestore->Header.ReturnStatus = (UINT64)-1;
+  if (Guid != 0) {
+    CopyMem (&LockBoxParameterRestore->Guid, Guid, sizeof(*Guid));
+  } else {
+    ZeroMem (&LockBoxParameterRestore->Guid, sizeof(*Guid));
+  }
+  LockBoxParameterRestore->Buffer = (EFI_PHYSICAL_ADDRESS)(UINTN)Buffer;
+  if (Length != NULL) {
+    LockBoxParameterRestore->Length = (EFI_PHYSICAL_ADDRESS)*Length;
+  } else {
+    LockBoxParameterRestore->Length = 0;
+  }
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunicationPpi->Communicate (
+                                  SmmCommunicationPpi,
+                                  &CommBuffer[0],
+                                  &CommSize
+                                  );
+  if (Status == EFI_NOT_STARTED) {
+    //
+    // Pei SMM communication not ready yet, so we access SMRAM directly
+    //
+    DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib Communicate - (%r)\n", Status));
+    Status = InternalRestoreLockBoxFromSmram (Guid, Buffer, Length);
+    LockBoxParameterRestore->Header.ReturnStatus = (UINT64)Status;
+    if (Length != NULL) {
+      LockBoxParameterRestore->Length = (UINT64)*Length;
+    }
+  }
+  ASSERT_EFI_ERROR (Status);
+
+  if (Length != NULL) {
+    *Length = (UINTN)LockBoxParameterRestore->Length;
+  }
+
+  Status = (EFI_STATUS)LockBoxParameterRestore->Header.ReturnStatus;
+  if (Status != EFI_SUCCESS) {
+    // Need or MAX_BIT, because there might be case that SMM is X64 while PEI is IA32.
+    Status |= MAX_BIT;
+  }
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib RestoreLockBox - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreAllLockBoxInPlace (
+  VOID
+  )
+{
+  EFI_STATUS                                      Status;
+  EFI_PEI_SMM_COMMUNICATION_PPI                   *SmmCommunicationPpi;
+  EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *LockBoxParameterRestoreAllInPlace;
+  EFI_SMM_COMMUNICATE_HEADER                      *CommHeader;
+  UINT8                                           CommBuffer[sizeof(EFI_GUID) + sizeof(UINT64) + sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE)];
+  UINTN                                           CommSize;
+  UINT64                                          MessageLength;
+
+  //
+  // Please aware that there is UINTN in EFI_SMM_COMMUNICATE_HEADER. It might be UINT64 in DXE, while it is UINT32 in PEI.
+  // typedef struct {
+  //   EFI_GUID  HeaderGuid;
+  //   UINTN     MessageLength;
+  //   UINT8     Data[1];
+  // } EFI_SMM_COMMUNICATE_HEADER;
+  //
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib RestoreAllLockBoxInPlace - Enter\n"));
+
+  //
+  // Get needed resource
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiSmmCommunicationPpiGuid,
+             0,
+             NULL,
+             (VOID **)&SmmCommunicationPpi
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Prepare parameter
+  //
+  CommHeader = (EFI_SMM_COMMUNICATE_HEADER *)&CommBuffer[0];
+  CopyMem (&CommHeader->HeaderGuid, &gEfiSmmLockBoxCommunicationGuid, sizeof(gEfiSmmLockBoxCommunicationGuid));
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    MessageLength = sizeof(*LockBoxParameterRestoreAllInPlace);
+    CopyMem (&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength)], &MessageLength, sizeof(MessageLength));
+  } else {
+    CommHeader->MessageLength = sizeof(*LockBoxParameterRestoreAllInPlace);
+  }
+
+  if ((sizeof(UINTN) == sizeof(UINT32)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) ) {
+    LockBoxParameterRestoreAllInPlace = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength) + sizeof(UINT64)];
+  } else {
+    LockBoxParameterRestoreAllInPlace = (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *)&CommBuffer[OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, MessageLength) + sizeof(UINTN)];
+  }
+  LockBoxParameterRestoreAllInPlace->Header.Command    = EFI_SMM_LOCK_BOX_COMMAND_RESTORE_ALL_IN_PLACE;
+  LockBoxParameterRestoreAllInPlace->Header.DataLength = sizeof(*LockBoxParameterRestoreAllInPlace);
+  LockBoxParameterRestoreAllInPlace->Header.ReturnStatus = (UINT64)-1;
+
+  //
+  // Send command
+  //
+  CommSize = sizeof(CommBuffer);
+  Status = SmmCommunicationPpi->Communicate (
+                                  SmmCommunicationPpi,
+                                  &CommBuffer[0],
+                                  &CommSize
+                                  );
+  if (Status == EFI_NOT_STARTED) {
+    //
+    // Pei SMM communication not ready yet, so we access SMRAM directly
+    //
+    DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib Communicate - (%r)\n", Status));
+    Status = InternalRestoreAllLockBoxInPlaceFromSmram ();
+    LockBoxParameterRestoreAllInPlace->Header.ReturnStatus = (UINT64)Status;
+  }
+  ASSERT_EFI_ERROR (Status);
+
+  Status = (EFI_STATUS)LockBoxParameterRestoreAllInPlace->Header.ReturnStatus;
+  if (Status != EFI_SUCCESS) {
+    // Need or MAX_BIT, because there might be case that SMM is X64 while PEI is IA32.
+    Status |= MAX_BIT;
+  }
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxPeiLib RestoreAllLockBoxInPlace - Exit (%r)\n", Status));
+
+  //
+  // Done
+  //
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf
new file mode 100644
index 0000000000..dceff8f0f6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf
@@ -0,0 +1,59 @@
+## @file
+#  PEI LockBox library instance.
+#
+#  Copyright (c) 2010 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmLockBoxPeiLib
+  MODULE_UNI_FILE                = SmmLockBoxPeiLib.uni
+  FILE_GUID                      = 5F5E6140-E7BA-4bd6-B85F-236B5BCD8E1E
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = LockBoxLib|PEIM
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 
+#
+
+[Sources]
+  SmmLockBoxPeiLib.c
+  SmmLockBoxLibPrivate.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeiServicesTablePointerLib
+  PeiServicesLib
+  BaseLib
+  BaseMemoryLib
+  HobLib
+  DebugLib
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode  ## CONSUMES
+
+[Guids]
+  ## SOMETIMES_CONSUMES ## UNDEFINED # Used to do smm communication
+  ## SOMETIMES_CONSUMES ## UNDEFINED # SmmSystemTable
+  gEfiSmmLockBoxCommunicationGuid
+  gEfiAcpiVariableGuid                  ## SOMETIMES_CONSUMES ## HOB
+
+[Ppis]
+  gEfiPeiSmmCommunicationPpiGuid        ## CONSUMES
+  gPeiSmmAccessPpiGuid                  ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.uni b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.uni
new file mode 100644
index 0000000000..d4302d2543
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.c b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.c
new file mode 100644
index 0000000000..4960df7555
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.c
@@ -0,0 +1,591 @@
+/** @file
+
+Copyright (c) 2010 - 2016, 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 <PiSmm.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/DebugLib.h>
+#include <Guid/SmmLockBox.h>
+
+#include "SmmLockBoxLibPrivate.h"
+
+/**
+  We need handle this library carefully. Only one library instance will construct the environment.
+  Below 2 global variable can only be used in constructor. They should NOT be used in any other library functions.
+**/
+SMM_LOCK_BOX_CONTEXT mSmmLockBoxContext;
+LIST_ENTRY           mLockBoxQueue = INITIALIZE_LIST_HEAD_VARIABLE (mLockBoxQueue);
+
+BOOLEAN              mSmmConfigurationTableInstalled = FALSE;
+
+/**
+  This function return SmmLockBox context from SMST.
+
+  @return SmmLockBox context from SMST.
+**/
+SMM_LOCK_BOX_CONTEXT *
+InternalGetSmmLockBoxContext (
+  VOID
+  )
+{
+  UINTN   Index;
+
+  //
+  // Check if gEfiSmmLockBoxCommunicationGuid is installed by someone
+  //
+  for (Index = 0; Index < gSmst->NumberOfTableEntries; Index++) {
+    if (CompareGuid (&gSmst->SmmConfigurationTable[Index].VendorGuid, &gEfiSmmLockBoxCommunicationGuid)) {
+      //
+      // Found. That means some other library instance is already run.
+      // No need to install again, just return.
+      //
+      return (SMM_LOCK_BOX_CONTEXT *)gSmst->SmmConfigurationTable[Index].VendorTable;
+    }
+  }
+
+  //
+  // Not found.
+  //
+  return NULL;
+}
+
+/**
+  Constructor for SmmLockBox library.
+  This is used to set SmmLockBox context, which will be used in PEI phase in S3 boot path later.
+
+  @param[in] ImageHandle  Image handle of this driver.
+  @param[in] SystemTable  A Pointer to the EFI System Table.
+
+  @retval EFI_SUCEESS     
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+EFIAPI
+SmmLockBoxSmmConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS           Status;
+  SMM_LOCK_BOX_CONTEXT *SmmLockBoxContext;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxSmmConstructor - Enter\n"));
+
+  //
+  // Check if gEfiSmmLockBoxCommunicationGuid is installed by someone
+  //
+  SmmLockBoxContext = InternalGetSmmLockBoxContext ();
+  if (SmmLockBoxContext != NULL) {
+    //
+    // Find it. That means some other library instance is already run.
+    // No need to install again, just return.
+    //
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxContext - already installed\n"));
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxSmmConstructor - Exit\n"));
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If no one install this, it means this is first instance. Install it.
+  //
+  if (sizeof(UINTN) == sizeof(UINT64)) {
+    mSmmLockBoxContext.Signature = SMM_LOCK_BOX_SIGNATURE_64;
+  } else {
+    mSmmLockBoxContext.Signature = SMM_LOCK_BOX_SIGNATURE_32;
+  }
+  mSmmLockBoxContext.LockBoxDataAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)&mLockBoxQueue;
+
+  Status = gSmst->SmmInstallConfigurationTable (
+                    gSmst,
+                    &gEfiSmmLockBoxCommunicationGuid,
+                    &mSmmLockBoxContext,
+                    sizeof(mSmmLockBoxContext)
+                    );
+  ASSERT_EFI_ERROR (Status);
+  mSmmConfigurationTableInstalled = TRUE;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxContext - %x\n", (UINTN)&mSmmLockBoxContext));
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib LockBoxDataAddress - %x\n", (UINTN)&mLockBoxQueue));
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxSmmConstructor - Exit\n"));
+
+  return Status;
+}
+
+/**
+  Destructor for SmmLockBox library.
+  This is used to uninstall SmmLockBoxCommunication configuration table
+  if it has been installed in Constructor.
+
+  @param[in] ImageHandle    Image handle of this driver.
+  @param[in] SystemTable    A Pointer to the EFI System Table.
+
+  @retval EFI_SUCEESS       The destructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLockBoxSmmDestructor (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SmmLockBoxSmmDestructor in %a module\n", gEfiCallerBaseName));
+
+  if (mSmmConfigurationTableInstalled) {
+    Status = gSmst->SmmInstallConfigurationTable (
+                      gSmst,
+                      &gEfiSmmLockBoxCommunicationGuid,
+                      NULL,
+                      0
+                      );
+    ASSERT_EFI_ERROR (Status);
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib uninstall SmmLockBoxCommunication configuration table\n"));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function return SmmLockBox queue address.
+
+  @return SmmLockBox queue address.
+**/
+LIST_ENTRY *
+InternalGetLockBoxQueue (
+  VOID
+  )
+{
+  SMM_LOCK_BOX_CONTEXT        *SmmLockBoxContext;
+
+  SmmLockBoxContext = InternalGetSmmLockBoxContext ();
+  ASSERT (SmmLockBoxContext != NULL);
+  if (SmmLockBoxContext == NULL) {
+    return NULL;
+  }
+  return (LIST_ENTRY *)(UINTN)SmmLockBoxContext->LockBoxDataAddress;
+}
+
+/**
+  This function find LockBox by GUID.
+
+  @param Guid The guid to indentify the LockBox
+
+  @return LockBoxData
+**/
+SMM_LOCK_BOX_DATA *
+InternalFindLockBoxByGuid (
+  IN EFI_GUID   *Guid
+  )
+{
+  LIST_ENTRY                    *Link;
+  SMM_LOCK_BOX_DATA             *LockBox;
+  LIST_ENTRY                    *LockBoxQueue;
+
+  LockBoxQueue = InternalGetLockBoxQueue ();
+  ASSERT (LockBoxQueue != NULL);
+
+  for (Link = LockBoxQueue->ForwardLink;
+       Link != LockBoxQueue;
+       Link = Link->ForwardLink) {
+    LockBox = BASE_CR (
+                Link,
+                SMM_LOCK_BOX_DATA,
+                Link
+                );
+    if (CompareGuid (&LockBox->Guid, Guid)) {
+      return LockBox;
+    }
+  }
+  return NULL;
+}
+
+/**
+  This function will save confidential information to lockbox.
+
+  @param Guid       the guid to identify the confidential information
+  @param Buffer     the address of the confidential information
+  @param Length     the length of the confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0
+  @retval RETURN_ALREADY_STARTED    the requested GUID already exist.
+  @retval RETURN_OUT_OF_RESOURCES   no enough resource to save the information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SaveLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  SMM_LOCK_BOX_DATA           *LockBox;
+  EFI_PHYSICAL_ADDRESS        SmramBuffer;
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *LockBoxQueue;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) || (Buffer == NULL) || (Length == 0)) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Exit (%r)\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find LockBox
+  //
+  LockBox = InternalFindLockBoxByGuid (Guid);
+  if (LockBox != NULL) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Exit (%r)\n", EFI_ALREADY_STARTED));
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Allocate SMRAM buffer
+  //
+  Status = gSmst->SmmAllocatePages (
+                    AllocateAnyPages,
+                    EfiRuntimeServicesData,
+                    EFI_SIZE_TO_PAGES (Length),
+                    &SmramBuffer
+                    );
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Exit (%r)\n", EFI_OUT_OF_RESOURCES));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Allocate LockBox
+  //
+  Status = gSmst->SmmAllocatePool (
+                    EfiRuntimeServicesData,
+                    sizeof(*LockBox),
+                    (VOID **)&LockBox
+                    );
+  ASSERT_EFI_ERROR (Status);
+  if (EFI_ERROR (Status)) {
+    gSmst->SmmFreePages (SmramBuffer, EFI_SIZE_TO_PAGES (Length));
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Exit (%r)\n", EFI_OUT_OF_RESOURCES));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Save data
+  //
+  CopyMem ((VOID *)(UINTN)SmramBuffer, (VOID *)(UINTN)Buffer, Length);
+
+  //
+  // Insert LockBox to queue
+  //
+  LockBox->Signature   = SMM_LOCK_BOX_DATA_SIGNATURE;
+  CopyMem (&LockBox->Guid, Guid, sizeof(EFI_GUID));
+  LockBox->Buffer      = (EFI_PHYSICAL_ADDRESS)(UINTN)Buffer;
+  LockBox->Length      = (UINT64)Length;
+  LockBox->Attributes  = 0;
+  LockBox->SmramBuffer = SmramBuffer;
+
+  DEBUG ((
+    EFI_D_INFO,
+    "LockBoxGuid - %g, SmramBuffer - 0x%lx, Length - 0x%lx\n",
+    &LockBox->Guid,
+    LockBox->SmramBuffer,
+    LockBox->Length
+    ));
+
+  LockBoxQueue = InternalGetLockBoxQueue ();
+  ASSERT (LockBoxQueue != NULL);
+  InsertTailList (LockBoxQueue, &LockBox->Link);
+
+  //
+  // Done
+  //
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SaveLockBox - Exit (%r)\n", EFI_SUCCESS));
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will set lockbox attributes.
+
+  @param Guid       the guid to identify the confidential information
+  @param Attributes the attributes of the lockbox
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  attributes is invalid.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+SetLockBoxAttributes (
+  IN  GUID                        *Guid,
+  IN  UINT64                      Attributes
+  )
+{
+  SMM_LOCK_BOX_DATA              *LockBox;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SetLockBoxAttributes - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) ||
+      ((Attributes & ~LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) != 0)) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SetLockBoxAttributes - Exit (%r)\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find LockBox
+  //
+  LockBox = InternalFindLockBoxByGuid (Guid);
+  if (LockBox == NULL) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SetLockBoxAttributes - Exit (%r)\n", EFI_NOT_FOUND));
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Update data
+  //
+  LockBox->Attributes = Attributes;
+
+  //
+  // Done
+  //
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib SetLockBoxAttributes - Exit (%r)\n", EFI_SUCCESS));
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will update confidential information to lockbox.
+
+  @param Guid   the guid to identify the original confidential information
+  @param Offset the offset of the original confidential information
+  @param Buffer the address of the updated confidential information
+  @param Length the length of the updated confidential information
+
+  @retval RETURN_SUCCESS            the information is saved successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or Buffer is NULL, or Length is 0.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_BUFFER_TOO_SMALL   the original buffer to too small to hold new information.
+  @retval RETURN_ACCESS_DENIED      it is too late to invoke this interface
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+UpdateLockBox (
+  IN  GUID                        *Guid,
+  IN  UINTN                       Offset,
+  IN  VOID                        *Buffer,
+  IN  UINTN                       Length
+  )
+{
+  SMM_LOCK_BOX_DATA             *LockBox;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib UpdateLockBox - Enter\n"));
+
+  //
+  // Basic check
+  //
+  if ((Guid == NULL) || (Buffer == NULL) || (Length == 0)) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib UpdateLockBox - Exit (%r)\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find LockBox
+  //
+  LockBox = InternalFindLockBoxByGuid (Guid);
+  if (LockBox == NULL) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib UpdateLockBox - Exit (%r)\n", EFI_NOT_FOUND));
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Update data
+  //
+  if (LockBox->Length < Offset + Length) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib UpdateLockBox - Exit (%r)\n", EFI_BUFFER_TOO_SMALL));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  ASSERT ((UINTN)LockBox->SmramBuffer <= (MAX_ADDRESS - Offset));
+  CopyMem ((VOID *)((UINTN)LockBox->SmramBuffer + Offset), Buffer, Length);
+
+  //
+  // Done
+  //
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib UpdateLockBox - Exit (%r)\n", EFI_SUCCESS));
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will restore confidential information from lockbox.
+
+  @param Guid   the guid to identify the confidential information
+  @param Buffer the address of the restored confidential information
+                NULL means restored to original address, Length MUST be NULL at same time.
+  @param Length the length of the restored confidential information
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_INVALID_PARAMETER  the Guid is NULL, or one of Buffer and Length is NULL.
+  @retval RETURN_WRITE_PROTECTED    Buffer and Length are NULL, but the LockBox has no 
+                                    LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE attribute.
+  @retval RETURN_BUFFER_TOO_SMALL   the Length is too small to hold the confidential information.
+  @retval RETURN_NOT_FOUND          the requested GUID not found.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_ACCESS_DENIED      not allow to restore to the address
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreLockBox (
+  IN  GUID                        *Guid,
+  IN  VOID                        *Buffer, OPTIONAL
+  IN  OUT UINTN                   *Length  OPTIONAL
+  )
+{
+  SMM_LOCK_BOX_DATA              *LockBox;
+  VOID                           *RestoreBuffer;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Enter\n"));
+
+  //
+  // Restore this, Buffer and Length MUST be both NULL or both non-NULL
+  //
+  if ((Guid == NULL) ||
+      ((Buffer == NULL) && (Length != NULL)) ||
+      ((Buffer != NULL) && (Length == NULL))) {
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Exit (%r)\n", EFI_INVALID_PARAMETER));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find LockBox
+  //
+  LockBox = InternalFindLockBoxByGuid (Guid);
+  if (LockBox == NULL) {
+    //
+    // Not found
+    //
+    DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Exit (%r)\n", EFI_NOT_FOUND));
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Set RestoreBuffer
+  //
+  if (Buffer != NULL) {
+    //
+    // restore to new buffer
+    //
+    RestoreBuffer = Buffer;
+  } else {
+    //
+    // restore to original buffer
+    //
+    if ((LockBox->Attributes & LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) == 0) {
+      DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Exit (%r)\n", EFI_WRITE_PROTECTED));
+      return EFI_WRITE_PROTECTED;
+    }
+    RestoreBuffer = (VOID *)(UINTN)LockBox->Buffer;
+  }
+
+  //
+  // Set RestoreLength
+  //
+  if (Length != NULL) {
+    if (*Length < (UINTN)LockBox->Length) {
+      //
+      // Input buffer is too small to hold all data.
+      //
+      *Length = (UINTN)LockBox->Length;
+      DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Exit (%r)\n", EFI_BUFFER_TOO_SMALL));
+      return EFI_BUFFER_TOO_SMALL;
+    }
+    *Length = (UINTN)LockBox->Length;
+  }
+
+  //
+  // Restore data
+  //
+  CopyMem (RestoreBuffer, (VOID *)(UINTN)LockBox->SmramBuffer, (UINTN)LockBox->Length);
+
+  //
+  // Done
+  //
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreLockBox - Exit (%r)\n", EFI_SUCCESS));
+  return EFI_SUCCESS;
+}
+
+/**
+  This function will restore confidential information from all lockbox which have RestoreInPlace attribute.
+
+  @retval RETURN_SUCCESS            the information is restored successfully.
+  @retval RETURN_NOT_STARTED        it is too early to invoke this interface
+  @retval RETURN_UNSUPPORTED        the service is not supported by implementaion.
+**/
+RETURN_STATUS
+EFIAPI
+RestoreAllLockBoxInPlace (
+  VOID
+  )
+{
+  SMM_LOCK_BOX_DATA             *LockBox;
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *LockBoxQueue;
+
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreAllLockBoxInPlace - Enter\n"));
+
+  LockBoxQueue = InternalGetLockBoxQueue ();
+  ASSERT (LockBoxQueue != NULL);
+
+  //
+  // Restore all, Buffer and Length MUST be NULL
+  //
+  for (Link = LockBoxQueue->ForwardLink;
+       Link != LockBoxQueue;
+       Link = Link->ForwardLink) {
+    LockBox = BASE_CR (
+                Link,
+                SMM_LOCK_BOX_DATA,
+                Link
+                );
+    if ((LockBox->Attributes & LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE) != 0) {
+      //
+      // Restore data
+      //
+      CopyMem ((VOID *)(UINTN)LockBox->Buffer, (VOID *)(UINTN)LockBox->SmramBuffer, (UINTN)LockBox->Length);
+    }
+  }
+  //
+  // Done
+  //
+  DEBUG ((EFI_D_INFO, "SmmLockBoxSmmLib RestoreAllLockBoxInPlace - Exit (%r)\n", EFI_SUCCESS));
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.inf b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.inf
new file mode 100644
index 0000000000..eb7ba0bb2e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.inf
@@ -0,0 +1,50 @@
+## @file
+#  SMM LockBox library instance.
+#
+#  Copyright (c) 2010 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmLockBoxSmmLib
+  MODULE_UNI_FILE                = SmmLockBoxSmmLib.uni
+  FILE_GUID                      = E04894D6-290D-4171-A362-0ACFD939F3C8
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = LockBoxLib|DXE_SMM_DRIVER
+  CONSTRUCTOR                    = SmmLockBoxSmmConstructor
+  DESTRUCTOR                     = SmmLockBoxSmmDestructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 
+#
+
+[Sources]
+  SmmLockBoxSmmLib.c
+  SmmLockBoxLibPrivate.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SmmServicesTableLib
+  BaseLib
+  DebugLib
+
+[Guids]
+  ## SOMETIMES_CONSUMES   ## UNDEFINED # SmmSystemTable
+  ## SOMETIMES_PRODUCES   ## UNDEFINED # SmmSystemTable
+  gEfiSmmLockBoxCommunicationGuid
diff --git a/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.uni b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.uni
new file mode 100644
index 0000000000..a3fef6620b
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.c b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.c
new file mode 100644
index 0000000000..c01d16d1eb
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.c
@@ -0,0 +1,451 @@
+/** @file
+  Performance Library used in SMM phase.
+
+  This library instance provides infrastructure for SMM drivers to log performance
+  data. It consumes SMM PerformanceEx or Performance Protocol published by SmmCorePerformanceLib
+  to log performance data. If both SMM PerformanceEx and Performance Protocol are not available, it does not log any
+  performance information.
+
+  Copyright (c) 2011 - 2012, 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 <Guid/Performance.h>
+
+#include <Library/PerformanceLib.h>
+#include <Library/DebugLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+
+//
+// The cached SMM Performance Protocol and SMM PerformanceEx Protocol interface.
+//
+PERFORMANCE_PROTOCOL        *mPerformance = NULL;
+PERFORMANCE_EX_PROTOCOL     *mPerformanceEx = NULL;
+BOOLEAN                     mPerformanceMeasurementEnabled;
+
+/**
+  The constructor function initializes the Performance Measurement Enable flag
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmPerformanceLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  
+  mPerformanceMeasurementEnabled =  (BOOLEAN) ((PcdGet8(PcdPerformanceLibraryPropertyMask) & PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED) != 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The function caches the pointers to SMM PerformanceEx protocol and Performance Protocol.
+
+  The function locates SMM PerformanceEx protocol and Performance Protocol from protocol database.
+
+  @retval EFI_SUCCESS     SMM PerformanceEx protocol or Performance Protocol is successfully located.
+  @retval EFI_NOT_FOUND   Both SMM PerformanceEx protocol and Performance Protocol are not located to log performance.
+
+**/
+EFI_STATUS
+GetPerformanceProtocol (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  PERFORMANCE_PROTOCOL      *Performance;
+  PERFORMANCE_EX_PROTOCOL   *PerformanceEx;
+
+  if (mPerformanceEx != NULL || mPerformance != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gSmst->SmmLocateProtocol (&gSmmPerformanceExProtocolGuid, NULL, (VOID **) &PerformanceEx);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (PerformanceEx != NULL);
+    //
+    // Cache PerformanceEx Protocol.
+    //
+    mPerformanceEx = PerformanceEx;
+    return EFI_SUCCESS;
+  }
+
+  Status = gSmst->SmmLocateProtocol (&gSmmPerformanceProtocolGuid, NULL, (VOID **) &Performance);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (Performance != NULL);
+    //
+    // Cache performance protocol.
+    //
+    mPerformance = Performance;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, Module and Identifier.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the created record
+                                  is same as the one created by StartPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->StartGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->StartGauge (Handle, Token, Module, TimeStamp);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return (RETURN_STATUS) Status;
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, Module and Identifier.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+  @param  Identifier              32-bit identifier. If the value is 0, the found record
+                                  is same as the one found by EndPerformanceMeasurement.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurementEx (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp,
+  IN UINT32       Identifier
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return RETURN_NOT_FOUND;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->EndGaugeEx (Handle, Token, Module, TimeStamp, Identifier);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->EndGauge (Handle, Token, Module, TimeStamp);
+  } else {
+    ASSERT (FALSE);
+  }
+
+  return (RETURN_STATUS) Status;
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurement and EndPerformanceMeasurement,
+  and then assign the Identifier with 0.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, EndTimeStamp and Identifier.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+  If Identifier is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+  @param  Identifier              Pointer to the 32-bit identifier that was recorded.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurementEx (
+  IN  UINTN       LogEntryKey, 
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp,
+  OUT UINT32      *Identifier
+  )
+{
+  EFI_STATUS            Status;
+  GAUGE_DATA_ENTRY_EX   *GaugeData;
+
+  GaugeData = NULL;
+
+  ASSERT (Handle != NULL);
+  ASSERT (Token != NULL);
+  ASSERT (Module != NULL);
+  ASSERT (StartTimeStamp != NULL);
+  ASSERT (EndTimeStamp != NULL);
+  ASSERT (Identifier != NULL);
+
+  Status = GetPerformanceProtocol ();
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  if (mPerformanceEx != NULL) {
+    Status = mPerformanceEx->GetGaugeEx (LogEntryKey++, &GaugeData);
+  } else if (mPerformance != NULL) {
+    Status = mPerformance->GetGauge (LogEntryKey++, (GAUGE_DATA_ENTRY **) &GaugeData);
+  } else {
+    ASSERT (FALSE);
+    return 0;
+  }
+
+  //
+  // Make sure that LogEntryKey is a valid log entry key,
+  //
+  ASSERT (Status != EFI_INVALID_PARAMETER);
+
+  if (EFI_ERROR (Status)) {
+    //
+    // The LogEntryKey is the last entry (equals to the total entry number).
+    //
+    return 0;
+  }
+
+  ASSERT (GaugeData != NULL);
+
+  *Handle         = (VOID *) (UINTN) GaugeData->Handle;
+  *Token          = GaugeData->Token;
+  *Module         = GaugeData->Module;
+  *StartTimeStamp = GaugeData->StartTimeStamp;
+  *EndTimeStamp   = GaugeData->EndTimeStamp;
+  if (mPerformanceEx != NULL) {
+    *Identifier   = GaugeData->Identifier;
+  } else {
+    *Identifier   = 0;
+  }
+
+  return LogEntryKey;
+}
+
+/**
+  Creates a record for the beginning of a performance measurement.
+
+  Creates a record that contains the Handle, Token, and Module.
+  If TimeStamp is not zero, then TimeStamp is added to the record as the start time.
+  If TimeStamp is zero, then this function reads the current time stamp
+  and adds that time stamp value to the record as the start time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The start of the measurement was recorded.
+  @retval RETURN_OUT_OF_RESOURCES There are not enough resources to record the measurement.
+
+**/
+RETURN_STATUS
+EFIAPI
+StartPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return StartPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Fills in the end time of a performance measurement.
+
+  Looks up the record that matches Handle, Token, and Module.
+  If the record can not be found then return RETURN_NOT_FOUND.
+  If the record is found and TimeStamp is not zero,
+  then TimeStamp is added to the record as the end time.
+  If the record is found and TimeStamp is zero, then this function reads
+  the current time stamp and adds that time stamp value to the record as the end time.
+
+  @param  Handle                  Pointer to environment specific context used
+                                  to identify the component being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string
+                                  that identifies the component being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string
+                                  that identifies the module being measured.
+  @param  TimeStamp               64-bit time stamp.
+
+  @retval RETURN_SUCCESS          The end of  the measurement was recorded.
+  @retval RETURN_NOT_FOUND        The specified measurement record could not be found.
+
+**/
+RETURN_STATUS
+EFIAPI
+EndPerformanceMeasurement (
+  IN CONST VOID   *Handle,  OPTIONAL
+  IN CONST CHAR8  *Token,   OPTIONAL
+  IN CONST CHAR8  *Module,  OPTIONAL
+  IN UINT64       TimeStamp
+  )
+{
+  return EndPerformanceMeasurementEx (Handle, Token, Module, TimeStamp, 0);
+}
+
+/**
+  Attempts to retrieve a performance measurement log entry from the performance measurement log.
+  It can also retrieve the log created by StartPerformanceMeasurementEx and EndPerformanceMeasurementEx,
+  and then eliminate the Identifier.
+
+  Attempts to retrieve the performance log entry specified by LogEntryKey.  If LogEntryKey is
+  zero on entry, then an attempt is made to retrieve the first entry from the performance log,
+  and the key for the second entry in the log is returned.  If the performance log is empty,
+  then no entry is retrieved and zero is returned.  If LogEntryKey is not zero, then the performance
+  log entry associated with LogEntryKey is retrieved, and the key for the next entry in the log is
+  returned.  If LogEntryKey is the key for the last entry in the log, then the last log entry is
+  retrieved and an implementation specific non-zero key value that specifies the end of the performance
+  log is returned.  If LogEntryKey is equal this implementation specific non-zero key value, then no entry
+  is retrieved and zero is returned.  In the cases where a performance log entry can be returned,
+  the log entry is returned in Handle, Token, Module, StartTimeStamp, and EndTimeStamp.
+  If LogEntryKey is not a valid log entry key for the performance measurement log, then ASSERT().
+  If Handle is NULL, then ASSERT().
+  If Token is NULL, then ASSERT().
+  If Module is NULL, then ASSERT().
+  If StartTimeStamp is NULL, then ASSERT().
+  If EndTimeStamp is NULL, then ASSERT().
+
+  @param  LogEntryKey             On entry, the key of the performance measurement log entry to retrieve.
+                                  0, then the first performance measurement log entry is retrieved.
+                                  On exit, the key of the next performance log entry.
+  @param  Handle                  Pointer to environment specific context used to identify the component
+                                  being measured.
+  @param  Token                   Pointer to a Null-terminated ASCII string that identifies the component
+                                  being measured.
+  @param  Module                  Pointer to a Null-terminated ASCII string that identifies the module
+                                  being measured.
+  @param  StartTimeStamp          Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was started.
+  @param  EndTimeStamp            Pointer to the 64-bit time stamp that was recorded when the measurement
+                                  was ended.
+
+  @return The key for the next performance log entry (in general case).
+
+**/
+UINTN
+EFIAPI
+GetPerformanceMeasurement (
+  IN  UINTN       LogEntryKey,
+  OUT CONST VOID  **Handle,
+  OUT CONST CHAR8 **Token,
+  OUT CONST CHAR8 **Module,
+  OUT UINT64      *StartTimeStamp,
+  OUT UINT64      *EndTimeStamp
+  )
+{
+  UINT32 Identifier;
+  return GetPerformanceMeasurementEx (LogEntryKey, Handle, Token, Module, StartTimeStamp, EndTimeStamp, &Identifier);
+}
+
+/**
+  Returns TRUE if the performance measurement macros are enabled.
+
+  This function returns TRUE if the PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+  PcdPerformanceLibraryPropertyMask is set.  Otherwise FALSE is returned.
+
+  @retval TRUE                    The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is set.
+  @retval FALSE                   The PERFORMANCE_LIBRARY_PROPERTY_MEASUREMENT_ENABLED bit of
+                                  PcdPerformanceLibraryPropertyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+PerformanceMeasurementEnabled (
+  VOID
+  )
+{
+  return mPerformanceMeasurementEnabled;
+}
diff --git a/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.inf b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.inf
new file mode 100644
index 0000000000..c3d01a1e51
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.inf
@@ -0,0 +1,57 @@
+## @file
+#  Performance library instance used in SMM phase.
+#  
+#  This library instance provides infrastructure for SMM drivers to log performance
+#  data. It consumes SMM PerformanceEx or Performance Protocol published by SmmCorePerformanceLib
+#  to log performance data. If both SMM PerformanceEx and Performance Protocol are not available,
+#  it does not log any performance information.
+#  
+#  Copyright (c) 2011 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmPerformanceLib
+  MODULE_UNI_FILE                = SmmPerformanceLib.uni
+  FILE_GUID                      = 1EDD13E6-D0CD-4432-A692-FF65C9B4F039
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = PerformanceLib|DXE_SMM_DRIVER
+
+  CONSTRUCTOR                    = SmmPerformanceLibConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  SmmPerformanceLib.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  PcdLib
+  SmmServicesTableLib
+  DebugLib
+  BaseMemoryLib
+
+[Guids]
+  gSmmPerformanceProtocolGuid           ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+  gSmmPerformanceExProtocolGuid         ## SOMETIMES_CONSUMES   ## UNDEFINED # Locate protocol
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdPerformanceLibraryPropertyMask    ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.uni b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.uni
new file mode 100644
index 0000000000..aa804d269e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.uni
diff --git a/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/ReportStatusCodeLib.c b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/ReportStatusCodeLib.c
new file mode 100644
index 0000000000..5394fb9e97
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/ReportStatusCodeLib.c
@@ -0,0 +1,545 @@
+/** @file
+  Report Status Code Library for SMM Phase.
+
+  Copyright (c) 2009 - 2010, 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 <Library/ReportStatusCodeLib.h>
+#include <Library/DebugLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+#include <Protocol/SmmStatusCode.h>
+
+EFI_SMM_REPORT_STATUS_CODE     mReportStatusCode = NULL;
+EFI_SMM_STATUS_CODE_PROTOCOL   *mStatusCodeProtocol = NULL;
+
+
+/**
+  Locate the report status code service.
+
+  @return   Function pointer to the report status code service.
+            NULL is returned if no status code service is available.
+
+**/
+EFI_SMM_REPORT_STATUS_CODE
+InternalGetReportStatusCode (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+
+  Status = gSmst->SmmLocateProtocol (&gEfiSmmStatusCodeProtocolGuid, NULL, (VOID**)&mStatusCodeProtocol);
+  if (!EFI_ERROR (Status) && mStatusCodeProtocol != NULL) {
+    return mStatusCodeProtocol->ReportStatusCode;
+  }
+  return NULL;
+}
+
+/**
+  Internal worker function that reports a status code through the status code service.
+
+  If status code service is not cached, then this function checks if status code service is
+  available in system.  If status code service is not available, then EFI_UNSUPPORTED is
+  returned.  If status code service is present, then it is cached in mReportStatusCode.
+  Finally this function reports status code through the status code service.
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  This is an optional parameter that may be
+                            NULL.
+  @param  Data              Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+
+  @retval EFI_SUCCESS       The status code was reported.
+  @retval EFI_UNSUPPORTED   Status code service is not available.
+  @retval EFI_UNSUPPORTED   Status code type is not supported.
+
+**/
+EFI_STATUS
+InternalReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN CONST EFI_GUID           *CallerId OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data     OPTIONAL
+  )
+{
+  if ((ReportProgressCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      (ReportErrorCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) ||
+      (ReportDebugCodeEnabled() && ((Type) & EFI_STATUS_CODE_TYPE_MASK) == EFI_DEBUG_CODE)) {
+    //
+    // If mReportStatusCode is NULL, then check if status code service is available in system.
+    //
+    if (mReportStatusCode == NULL) {
+      mReportStatusCode = InternalGetReportStatusCode ();
+      if (mReportStatusCode == NULL) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  
+    //
+    // A status code service is present in system, so pass in all the parameters to the service.
+    //
+    return (*mReportStatusCode) (mStatusCodeProtocol, Type, Value, Instance, (EFI_GUID *)CallerId, Data);
+  }
+  
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Converts a status code to an 8-bit POST code value.
+
+  Converts the status code specified by CodeType and Value to an 8-bit POST code
+  and returns the 8-bit POST code in PostCode.  If CodeType is an
+  EFI_PROGRESS_CODE or CodeType is an EFI_ERROR_CODE, then bits 0..4 of PostCode
+  are set to bits 16..20 of Value, and bits 5..7 of PostCode are set to bits
+  24..26 of Value., and TRUE is returned.  Otherwise, FALSE is returned.
+
+  If PostCode is NULL, then ASSERT().
+
+  @param  CodeType  The type of status code being converted.
+  @param  Value     The status code value being converted.
+  @param  PostCode  A pointer to the 8-bit POST code value to return.
+
+  @retval  TRUE   The status code specified by CodeType and Value was converted
+                  to an 8-bit POST code and returned in  PostCode.
+  @retval  FALSE  The status code specified by CodeType and Value could not be
+                  converted to an 8-bit POST code value.
+
+**/
+BOOLEAN
+EFIAPI
+CodeTypeToPostCode (
+  IN  EFI_STATUS_CODE_TYPE   CodeType,
+  IN  EFI_STATUS_CODE_VALUE  Value,
+  OUT UINT8                  *PostCode
+  )
+{
+  //
+  // If PostCode is NULL, then ASSERT()
+  //
+  ASSERT (PostCode != NULL);
+
+  //
+  // Convert Value to an 8 bit post code
+  //
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) ||
+      ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE)       ) {
+    *PostCode  = (UINT8) ((((Value & EFI_STATUS_CODE_CLASS_MASK) >> 24) << 5) |
+                          (((Value & EFI_STATUS_CODE_SUBCLASS_MASK) >> 16) & 0x1f));
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts ASSERT() information from a status code structure.
+
+  Converts the status code specified by CodeType, Value, and Data to the ASSERT()
+  arguments specified by Filename, Description, and LineNumber.  If CodeType is
+  an EFI_ERROR_CODE, and CodeType has a severity of EFI_ERROR_UNRECOVERED, and
+  Value has an operation mask of EFI_SW_EC_ILLEGAL_SOFTWARE_STATE, extract
+  Filename, Description, and LineNumber from the optional data area of the
+  status code buffer specified by Data.  The optional data area of Data contains
+  a Null-terminated ASCII string for the FileName, followed by a Null-terminated
+  ASCII string for the Description, followed by a 32-bit LineNumber.  If the
+  ASSERT() information could be extracted from Data, then return TRUE.
+  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If Filename is NULL, then ASSERT().
+  If Description is NULL, then ASSERT().
+  If LineNumber is NULL, then ASSERT().
+
+  @param  CodeType     The type of status code being converted.
+  @param  Value        The status code value being converted.
+  @param  Data         Pointer to status code data buffer.
+  @param  Filename     Pointer to the source file name that generated the ASSERT().
+  @param  Description  Pointer to the description of the ASSERT().
+  @param  LineNumber   Pointer to source line number that generated the ASSERT().
+
+  @retval  TRUE   The status code specified by CodeType, Value, and Data was
+                  converted ASSERT() arguments specified by Filename, Description,
+                  and LineNumber.
+  @retval  FALSE  The status code specified by CodeType, Value, and Data could
+                  not be converted to ASSERT() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractAssertInfo (
+  IN EFI_STATUS_CODE_TYPE        CodeType,
+  IN EFI_STATUS_CODE_VALUE       Value,
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT CHAR8                      **Filename,
+  OUT CHAR8                      **Description,
+  OUT UINT32                     *LineNumber
+  )
+{
+  EFI_DEBUG_ASSERT_DATA  *AssertData;
+
+  ASSERT (Data        != NULL);
+  ASSERT (Filename    != NULL);
+  ASSERT (Description != NULL);
+  ASSERT (LineNumber  != NULL);
+
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK)      == EFI_ERROR_CODE) &&
+      ((CodeType & EFI_STATUS_CODE_SEVERITY_MASK)  == EFI_ERROR_UNRECOVERED) &&
+      ((Value    & EFI_STATUS_CODE_OPERATION_MASK) == EFI_SW_EC_ILLEGAL_SOFTWARE_STATE)) {
+    AssertData   = (EFI_DEBUG_ASSERT_DATA *)(Data + 1);
+    *Filename    = (CHAR8 *)(AssertData + 1);
+    *Description = *Filename + AsciiStrLen (*Filename) + 1;
+    *LineNumber  = AssertData->LineNumber;
+    return TRUE;
+  }
+  return FALSE;
+}
+
+
+/**
+  Extracts DEBUG() information from a status code structure.
+
+  Converts the status code specified by Data to the DEBUG() arguments specified
+  by ErrorLevel, Marker, and Format.  If type GUID in Data is
+  EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID, then extract ErrorLevel, Marker, and
+  Format from the optional data area of the status code buffer specified by Data.
+  The optional data area of Data contains a 32-bit ErrorLevel followed by Marker
+  which is 12 UINTN parameters, followed by a Null-terminated ASCII string for
+  the Format.  If the DEBUG() information could be extracted from Data, then
+  return TRUE.  Otherwise, FALSE is returned.
+
+  If Data is NULL, then ASSERT().
+  If ErrorLevel is NULL, then ASSERT().
+  If Marker is NULL, then ASSERT().
+  If Format is NULL, then ASSERT().
+
+  @param  Data        Pointer to status code data buffer.
+  @param  ErrorLevel  Pointer to error level mask for a debug message.
+  @param  Marker      Pointer to the variable argument list associated with Format.
+  @param  Format      Pointer to a Null-terminated ASCII format string of a
+                      debug message.
+
+  @retval  TRUE   The status code specified by Data was converted DEBUG() arguments
+                  specified by ErrorLevel, Marker, and Format.
+  @retval  FALSE  The status code specified by Data could not be converted to
+                  DEBUG() arguments.
+
+**/
+BOOLEAN
+EFIAPI
+ReportStatusCodeExtractDebugInfo (
+  IN CONST EFI_STATUS_CODE_DATA  *Data,
+  OUT UINT32                     *ErrorLevel,
+  OUT BASE_LIST                  *Marker,
+  OUT CHAR8                      **Format
+  )
+{
+  EFI_DEBUG_INFO  *DebugInfo;
+
+  ASSERT (Data       != NULL);
+  ASSERT (ErrorLevel != NULL);
+  ASSERT (Marker     != NULL);
+  ASSERT (Format     != NULL);
+
+  //
+  // If the GUID type is not EFI_STATUS_CODE_DATA_TYPE_DEBUG_GUID then return FALSE
+  //
+  if (!CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeDebugGuid)) {
+    return FALSE;
+  }
+
+  //
+  // Retrieve the debug information from the status code record
+  //
+  DebugInfo = (EFI_DEBUG_INFO *)(Data + 1);
+
+  *ErrorLevel = DebugInfo->ErrorLevel;
+
+  //
+  // The first 12 * sizeof (UINT64) bytes following EFI_DEBUG_INFO are for variable arguments
+  // of format in DEBUG string. Its address is returned in Marker and has to be 64-bit aligned.
+  // It must be noticed that EFI_DEBUG_INFO follows EFI_STATUS_CODE_DATA, whose size is
+  // 20 bytes. The size of EFI_DEBUG_INFO is 4 bytes, so we can ensure that Marker
+  // returned is 64-bit aligned.
+  // 64-bit aligned is a must, otherwise retrieving 64-bit parameter from BASE_LIST will
+  // cause unalignment exception.
+  //
+  *Marker = (BASE_LIST) (DebugInfo + 1);
+  *Format = (CHAR8 *)(((UINT64 *)*Marker) + 12);
+
+  return TRUE;
+}
+
+
+/**
+  Reports a status code.
+
+  Reports the status code specified by the parameters Type and Value.  Status
+  code also require an instance, caller ID, and extended data.  This function
+  passed in a zero instance, NULL extended data, and a caller ID of
+  gEfiCallerIdGuid, which is the GUID for the module.
+
+  ReportStatusCode()must actively prevent recusrsion.  If ReportStatusCode()
+  is called while processing another any other Report Status Code Library function,
+  then ReportStatusCode() must return immediately.
+
+  @param  Type   Status code type.
+  @param  Value  Status code value.
+
+  @retval  EFI_SUCCESS       The status code was reported.
+  @retval  EFI_DEVICE_ERROR  There status code could not be reported due to a
+                             device error.
+  @retval  EFI_UNSUPPORTED   Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCode (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value
+  )
+{
+  return InternalReportStatusCode (Type, Value, 0, &gEfiCallerIdGuid, NULL);
+}
+
+
+/**
+  Reports a status code with an extended data buffer.
+
+  Allocates and fills in the extended data section of a status code with the
+  extended data specified by ExtendedData and ExtendedDataSize.  ExtendedData
+  is assumed to be one of the data structures specified in Related Definitions.
+  These data structure do not have the standard header, so this function is
+  responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled
+  in with a GUID of  gEfiStatusCodeSpecificDataGuid.  The status code is reported
+  with a zero instance and a caller ID of gEfiCallerIdGuid.
+
+  ReportStatusCodeWithExtendedData()must actively prevent recursion.  If
+  ReportStatusCodeWithExtendedData() is called while processing another any other
+  Report Status Code Library function, then ReportStatusCodeWithExtendedData()
+  must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL, then ASSERT().
+  If ExtendedDataSize is 0, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  ExtendedData      Pointer to the extended data buffer to be reported.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer to
+                            be reported.
+
+  @retval  EFI_SUCCESS           The status code was reported with the extended
+                                 data specified by ExtendedData and ExtendedDataSize.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate the
+                                 extended data section.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeWithExtendedData (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN CONST VOID             *ExtendedData,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  ASSERT (ExtendedData     != NULL);
+  ASSERT (ExtendedDataSize != 0);
+  return ReportStatusCodeEx (
+           Type,
+           Value,
+           0,
+           NULL,
+           NULL,
+           ExtendedData,
+           ExtendedDataSize
+           );
+}
+
+
+/**
+  Reports a status code with full parameters.
+
+  The function reports a status code.  If ExtendedData is NULL and ExtendedDataSize
+  is 0, then an extended data buffer is not reported.  If ExtendedData is not
+  NULL and ExtendedDataSize is not 0, then an extended data buffer is allocated.
+  ExtendedData is assumed not have the standard status code header, so this function
+  is responsible for allocating a buffer large enough for the standard header and
+  the extended data passed into this function.  The standard header is filled in
+  with a GUID specified by ExtendedDataGuid.  If ExtendedDataGuid is NULL, then a
+  GUID of gEfiStatusCodeSpecificDataGuid is used.  The status code is reported with
+  an instance specified by Instance and a caller ID specified by CallerId.  If
+  CallerId is NULL, then a caller ID of gEfiCallerIdGuid is used.
+
+  ReportStatusCodeEx()must actively prevent recursion. If
+  ReportStatusCodeEx() is called while processing another any
+  other Report Status Code Library function, then
+  ReportStatusCodeEx() must return EFI_DEVICE_ERROR immediately.
+
+  If ExtendedData is NULL and ExtendedDataSize is not zero, then ASSERT().
+  If ExtendedData is not NULL and ExtendedDataSize is zero, then ASSERT().
+
+  @param  Type              Status code type.
+  @param  Value             Status code value.
+  @param  Instance          Status code instance number.
+  @param  CallerId          Pointer to a GUID that identifies the caller of this
+                            function.  If this parameter is NULL, then a caller
+                            ID of gEfiCallerIdGuid is used.
+  @param  ExtendedDataGuid  Pointer to the GUID for the extended data buffer.
+                            If this parameter is NULL, then a the status code
+                            standard header is filled in with
+                            gEfiStatusCodeSpecificDataGuid.
+  @param  ExtendedData      Pointer to the extended data buffer.  This is an
+                            optional parameter that may be NULL.
+  @param  ExtendedDataSize  The size, in bytes, of the extended data buffer.
+
+  @retval  EFI_SUCCESS           The status code was reported.
+  @retval  EFI_OUT_OF_RESOURCES  There were not enough resources to allocate
+                                 the extended data section if it was specified.
+  @retval  EFI_UNSUPPORTED       Report status code is not supported
+
+**/
+EFI_STATUS
+EFIAPI
+ReportStatusCodeEx (
+  IN EFI_STATUS_CODE_TYPE   Type,
+  IN EFI_STATUS_CODE_VALUE  Value,
+  IN UINT32                 Instance,
+  IN CONST EFI_GUID         *CallerId          OPTIONAL,
+  IN CONST EFI_GUID         *ExtendedDataGuid  OPTIONAL,
+  IN CONST VOID             *ExtendedData      OPTIONAL,
+  IN UINTN                  ExtendedDataSize
+  )
+{
+  EFI_STATUS            Status;
+  EFI_STATUS_CODE_DATA  *StatusCodeData;
+
+  ASSERT (!((ExtendedData == NULL) && (ExtendedDataSize != 0)));
+  ASSERT (!((ExtendedData != NULL) && (ExtendedDataSize == 0)));
+
+  //
+  // Allocate space for the Status Code Header and its buffer
+  //
+  StatusCodeData = AllocatePool (sizeof (EFI_STATUS_CODE_DATA) + ExtendedDataSize);
+  if (StatusCodeData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Fill in the extended data header
+  //
+  StatusCodeData->HeaderSize = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+  StatusCodeData->Size = (UINT16) ExtendedDataSize;
+  if (ExtendedDataGuid == NULL) {
+    ExtendedDataGuid = &gEfiStatusCodeSpecificDataGuid;
+  }
+  CopyGuid (&StatusCodeData->Type, ExtendedDataGuid);
+
+  //
+  // Fill in the extended data buffer
+  //
+  if (ExtendedData != NULL) {
+    CopyMem (StatusCodeData + 1, ExtendedData, ExtendedDataSize);
+  }
+
+  //
+  // Report the status code
+  //
+  if (CallerId == NULL) {
+    CallerId = &gEfiCallerIdGuid;
+  }
+  Status = InternalReportStatusCode (Type, Value, Instance, CallerId, StatusCodeData);
+
+  //
+  // Free the allocated buffer
+  //
+  FreePool (StatusCodeData);
+
+  return Status;
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_PROGRESS_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportProgressCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_PROGRESS_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_ERROR_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportErrorCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_ERROR_CODE_ENABLED) != 0);
+}
+
+
+/**
+  Returns TRUE if status codes of type EFI_DEBUG_CODE are enabled
+
+  This function returns TRUE if the REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED
+  bit of PcdReportStatusCodeProperyMask is set.  Otherwise FALSE is returned.
+
+  @retval  TRUE   The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is set.
+  @retval  FALSE  The REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED bit of
+                  PcdReportStatusCodeProperyMask is clear.
+
+**/
+BOOLEAN
+EFIAPI
+ReportDebugCodeEnabled (
+  VOID
+  )
+{
+  return (BOOLEAN) ((PcdGet8 (PcdReportStatusCodePropertyMask) & REPORT_STATUS_CODE_PROPERTY_DEBUG_CODE_ENABLED) != 0);
+}
diff --git a/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.inf b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.inf
new file mode 100644
index 0000000000..15799299fc
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.inf
@@ -0,0 +1,56 @@
+## @file
+#  SMM report status code library.
+#
+#  Retrieve status code and report status code in SMM phase.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmReportStatusCodeLib
+  MODULE_UNI_FILE                = SmmReportStatusCodeLib.uni
+  FILE_GUID                      = 67089D19-B3D6-4d9e-A0EB-FEDC1F83A1EE
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  LIBRARY_CLASS                  = ReportStatusCodeLib|DXE_SMM_DRIVER SMM_CORE
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  ReportStatusCodeLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  BaseMemoryLib
+  SmmServicesTableLib
+  DebugLib
+  MemoryAllocationLib
+
+[Guids]
+  gEfiStatusCodeSpecificDataGuid                ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiStatusCodeDataTypeDebugGuid               ## SOMETIMES_CONSUMES   ## UNDEFINED
+
+[Protocols]
+  gEfiSmmStatusCodeProtocolGuid                 ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdReportStatusCodePropertyMask  ## CONSUMES
diff --git a/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.uni b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.uni
new file mode 100644
index 0000000000..99cfab569d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.uni
diff --git a/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.c b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.c
new file mode 100644
index 0000000000..8a0377b7a4
--- /dev/null
+++ b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.c
@@ -0,0 +1,45 @@
+/** @file
+  This library is used by other modules to measure data to TPM.
+
+Copyright (c) 2015, 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.
+
+**/
+
+/**
+  Tpm measure and log data, and extend the measurement result into a specific PCR.
+
+  @param[in]  PcrIndex         PCR Index.
+  @param[in]  EventType        Event type.
+  @param[in]  EventLog         Measurement event log.
+  @param[in]  LogLen           Event log length in bytes.
+  @param[in]  HashData         The start of the data buffer to be hashed, extended.
+  @param[in]  HashDataLen      The length, in bytes, of the buffer referenced by HashData
+
+  @retval EFI_SUCCESS               Operation completed successfully.
+  @retval EFI_UNSUPPORTED       TPM device not available.
+  @retval EFI_OUT_OF_RESOURCES  Out of memory.
+  @retval EFI_DEVICE_ERROR      The operation was unsuccessful.
+**/
+EFI_STATUS
+EFIAPI
+TpmMeasureAndLogData (
+  IN UINT32             PcrIndex,
+  IN UINT32             EventType,
+  IN VOID               *EventLog,
+  IN UINT32             LogLen,
+  IN VOID               *HashData,
+  IN UINT64             HashDataLen
+  )
+{
+  //
+  // Do nothing, just return EFI_SUCCESS.
+  //
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.inf b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.inf
new file mode 100644
index 0000000000..fef783a4f9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.inf
@@ -0,0 +1,34 @@
+## @file
+#  Provides NULL TPM measurement function.
+#
+# Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = TpmMeasurementLibNull
+  FILE_GUID                      = 6DFD6E9F-9278-48D8-8F45-B6CFF2C2B69C
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = TpmMeasurementLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+  MODULE_UNI_FILE                = TpmMeasurementLibNull.uni
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF
+#
+
+[Sources]
+  TpmMeasurementLibNull.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
diff --git a/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.uni b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.uni
new file mode 100644
index 0000000000..71a2b1f392
--- /dev/null
+++ b/Core/MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.uni
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmBoot.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmBoot.c
new file mode 100644
index 0000000000..52f2d038f3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmBoot.c
@@ -0,0 +1,2351 @@
+/** @file
+  Library functions which relates with booting.
+
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+#define VENDOR_IDENTIFICATION_OFFSET     3
+#define VENDOR_IDENTIFICATION_LENGTH     8
+#define PRODUCT_IDENTIFICATION_OFFSET    11
+#define PRODUCT_IDENTIFICATION_LENGTH    16
+
+CONST UINT16 mBmUsbLangId    = 0x0409; // English
+CHAR16       mBmUefiPrefix[] = L"UEFI ";
+
+EFI_BOOT_MANAGER_REFRESH_LEGACY_BOOT_OPTION  mBmRefreshLegacyBootOption = NULL;
+EFI_BOOT_MANAGER_LEGACY_BOOT                 mBmLegacyBoot              = NULL;
+
+LIST_ENTRY mPlatformBootDescriptionHandlers = INITIALIZE_LIST_HEAD_VARIABLE (mPlatformBootDescriptionHandlers);
+
+///
+/// This GUID is used for an EFI Variable that stores the front device pathes
+/// for a partial device path that starts with the HD node.
+///
+EFI_GUID mBmHardDriveBootVariableGuid = { 0xfab7e9e1, 0x39dd, 0x4f2b, { 0x84, 0x08, 0xe2, 0x0e, 0x90, 0x6c, 0xb6, 0xde } };
+EFI_GUID mBmAutoCreateBootOptionGuid  = { 0x8108ac4e, 0x9f11, 0x4d59, { 0x85, 0x0e, 0xe2, 0x1a, 0x52, 0x2c, 0x59, 0xb2 } };
+
+/**
+  The function registers the legacy boot support capabilities.
+
+  @param RefreshLegacyBootOption The function pointer to create all the legacy boot options.
+  @param LegacyBoot              The function pointer to boot the legacy boot option.
+**/
+VOID
+EFIAPI
+EfiBootManagerRegisterLegacyBootSupport (
+  EFI_BOOT_MANAGER_REFRESH_LEGACY_BOOT_OPTION   RefreshLegacyBootOption,
+  EFI_BOOT_MANAGER_LEGACY_BOOT                  LegacyBoot
+  )
+{
+  mBmRefreshLegacyBootOption = RefreshLegacyBootOption;
+  mBmLegacyBoot              = LegacyBoot;
+}
+
+/**
+  Return TRUE when the boot option is auto-created instead of manually added.
+
+  @param BootOption Pointer to the boot option to check.
+
+  @retval TRUE  The boot option is auto-created.
+  @retval FALSE The boot option is manually added.
+**/
+BOOLEAN
+BmIsAutoCreateBootOption (
+  EFI_BOOT_MANAGER_LOAD_OPTION    *BootOption
+  )
+{
+  if ((BootOption->OptionalDataSize == sizeof (EFI_GUID)) &&
+      CompareGuid ((EFI_GUID *) BootOption->OptionalData, &mBmAutoCreateBootOptionGuid)
+      ) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  For a bootable Device path, return its boot type.
+
+  @param  DevicePath                   The bootable device Path to check
+
+  @retval AcpiFloppyBoot               If given device path contains ACPI_DEVICE_PATH type device path node
+                                       which HID is floppy device.
+  @retval MessageAtapiBoot             If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_ATAPI_DP.
+  @retval MessageSataBoot              If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_SATA_DP.
+  @retval MessageScsiBoot              If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_SCSI_DP.
+  @retval MessageUsbBoot               If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_USB_DP.
+  @retval MessageNetworkBoot           If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_MAC_ADDR_DP, MSG_VLAN_DP,
+                                       MSG_IPv4_DP or MSG_IPv6_DP.
+  @retval MessageHttpBoot              If given device path contains MESSAGING_DEVICE_PATH type device path node
+                                       and its last device path node's subtype is MSG_URI_DP.
+  @retval UnsupportedBoot              If tiven device path doesn't match the above condition, it's not supported.
+
+**/
+BM_BOOT_TYPE
+BmDevicePathType (
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL      *Node;
+  EFI_DEVICE_PATH_PROTOCOL      *NextNode;
+
+  ASSERT (DevicePath != NULL);
+
+  for (Node = DevicePath; !IsDevicePathEndType (Node); Node = NextDevicePathNode (Node)) {
+    switch (DevicePathType (Node)) {
+
+      case ACPI_DEVICE_PATH:
+        if (EISA_ID_TO_NUM (((ACPI_HID_DEVICE_PATH *) Node)->HID) == 0x0604) {
+          return BmAcpiFloppyBoot;
+        }
+        break;
+
+      case HARDWARE_DEVICE_PATH:
+        if (DevicePathSubType (Node) == HW_CONTROLLER_DP) {
+          return BmHardwareDeviceBoot;
+        }
+        break;
+
+      case MESSAGING_DEVICE_PATH:
+        //
+        // Skip LUN device node
+        //
+        NextNode = Node;
+        do {
+          NextNode = NextDevicePathNode (NextNode);
+        } while (
+            (DevicePathType (NextNode) == MESSAGING_DEVICE_PATH) &&
+            (DevicePathSubType(NextNode) == MSG_DEVICE_LOGICAL_UNIT_DP)
+            );
+
+        //
+        // If the device path not only point to driver device, it is not a messaging device path,
+        //
+        if (!IsDevicePathEndType (NextNode)) {
+          continue;
+        }
+
+        switch (DevicePathSubType (Node)) {
+        case MSG_ATAPI_DP:
+          return BmMessageAtapiBoot;
+          break;
+
+        case MSG_SATA_DP:
+          return BmMessageSataBoot;
+          break;
+
+        case MSG_USB_DP:
+          return BmMessageUsbBoot;
+          break;
+
+        case MSG_SCSI_DP:
+          return BmMessageScsiBoot;
+          break;
+
+        case MSG_MAC_ADDR_DP:
+        case MSG_VLAN_DP:
+        case MSG_IPv4_DP:
+        case MSG_IPv6_DP:
+          return BmMessageNetworkBoot;
+          break;
+
+        case MSG_URI_DP:
+          return BmMessageHttpBoot;
+          break;
+        }
+    }
+  }
+
+  return BmMiscBoot;
+}
+
+/**
+  Find the boot option in the NV storage and return the option number.
+
+  @param OptionToFind  Boot option to be checked.
+
+  @return   The option number of the found boot option.
+
+**/
+UINTN
+BmFindBootOptionInVariable (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION             *OptionToFind
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION BootOption;
+  UINTN                        OptionNumber;
+  CHAR16                       OptionName[BM_OPTION_NAME_LEN];
+  EFI_BOOT_MANAGER_LOAD_OPTION *BootOptions;
+  UINTN                        BootOptionCount;
+  UINTN                        Index;
+  
+  OptionNumber = LoadOptionNumberUnassigned;
+
+  //
+  // Try to match the variable exactly if the option number is assigned
+  //
+  if (OptionToFind->OptionNumber != LoadOptionNumberUnassigned) {
+    UnicodeSPrint (
+      OptionName, sizeof (OptionName), L"%s%04x",
+      mBmLoadOptionName[OptionToFind->OptionType], OptionToFind->OptionNumber
+      );
+    Status = EfiBootManagerVariableToLoadOption (OptionName, &BootOption);
+
+    if (!EFI_ERROR (Status)) {
+      ASSERT (OptionToFind->OptionNumber == BootOption.OptionNumber);
+      if ((OptionToFind->Attributes == BootOption.Attributes) &&
+          (StrCmp (OptionToFind->Description, BootOption.Description) == 0) &&
+          (CompareMem (OptionToFind->FilePath, BootOption.FilePath, GetDevicePathSize (OptionToFind->FilePath)) == 0) &&
+          (OptionToFind->OptionalDataSize == BootOption.OptionalDataSize) &&
+          (CompareMem (OptionToFind->OptionalData, BootOption.OptionalData, OptionToFind->OptionalDataSize) == 0)
+         ) {
+        OptionNumber = OptionToFind->OptionNumber;
+      }
+      EfiBootManagerFreeLoadOption (&BootOption);
+    }
+  }
+
+  //
+  // The option number assigned is either incorrect or unassigned.
+  //
+  if (OptionNumber == LoadOptionNumberUnassigned) {
+    BootOptions = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+    Index = BmFindLoadOption (OptionToFind, BootOptions, BootOptionCount);
+    if (Index != -1) {
+      OptionNumber = BootOptions[Index].OptionNumber;
+    }
+
+    EfiBootManagerFreeLoadOptions (BootOptions, BootOptionCount);
+  }
+
+  return OptionNumber;
+}
+
+/**
+  Get the file buffer using a Memory Mapped Device Path.
+
+  FV address may change across reboot. This routine promises the FV file device path is right.
+
+  @param  DevicePath   The Memory Mapped Device Path to get the file buffer.
+  @param  FullPath     Receive the updated FV Device Path pointint to the file.
+  @param  FileSize     Receive the file buffer size.
+
+  @return  The file buffer.
+**/
+VOID *
+BmGetFileBufferByMemmapFv (
+  IN EFI_DEVICE_PATH_PROTOCOL      *DevicePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL     **FullPath,
+  OUT UINTN                        *FileSize
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  EFI_DEVICE_PATH_PROTOCOL      *FvFileNode;
+  EFI_HANDLE                    FvHandle;
+  EFI_LOADED_IMAGE_PROTOCOL     *LoadedImage;
+  UINT32                        AuthenticationStatus;
+  UINTN                         FvHandleCount;
+  EFI_HANDLE                    *FvHandles;
+  EFI_DEVICE_PATH_PROTOCOL      *NewDevicePath;
+  VOID                          *FileBuffer;
+  
+  FvFileNode = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiFirmwareVolume2ProtocolGuid, &FvFileNode, &FvHandle);
+  if (!EFI_ERROR (Status)) {
+    FileBuffer = GetFileBufferByFilePath (TRUE, DevicePath, FileSize, &AuthenticationStatus);
+    if (FileBuffer != NULL) {
+      *FullPath = DuplicateDevicePath (DevicePath);
+    }
+    return FileBuffer;
+  }
+
+  FvFileNode = NextDevicePathNode (DevicePath);
+
+  //
+  // Firstly find the FV file in current FV
+  //
+  gBS->HandleProtocol (
+         gImageHandle,
+         &gEfiLoadedImageProtocolGuid,
+         (VOID **) &LoadedImage
+         );
+  NewDevicePath = AppendDevicePathNode (DevicePathFromHandle (LoadedImage->DeviceHandle), FvFileNode);
+  FileBuffer = BmGetFileBufferByMemmapFv (NewDevicePath, FullPath, FileSize);
+  FreePool (NewDevicePath);
+
+  if (FileBuffer != NULL) {
+    return FileBuffer;
+  }
+
+  //
+  // Secondly find the FV file in all other FVs
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiFirmwareVolume2ProtocolGuid,
+         NULL,
+         &FvHandleCount,
+         &FvHandles
+         );
+  for (Index = 0; (Index < FvHandleCount) && (FileBuffer == NULL); Index++) {
+    if (FvHandles[Index] == LoadedImage->DeviceHandle) {
+      //
+      // Skip current FV
+      //
+      continue;
+    }
+    NewDevicePath = AppendDevicePathNode (DevicePathFromHandle (FvHandles[Index]), FvFileNode);
+    FileBuffer = BmGetFileBufferByMemmapFv (NewDevicePath, FullPath, FileSize);
+    FreePool (NewDevicePath);
+  }
+  
+  if (FvHandles != NULL) {
+    FreePool (FvHandles);
+  }
+  return FileBuffer;
+}
+
+/**
+  Check if it's a Memory Mapped FV Device Path.
+  
+  The function doesn't garentee the device path points to existing FV file.
+
+  @param  DevicePath     Input device path.
+
+  @retval TRUE   The device path is a Memory Mapped FV Device Path.
+  @retval FALSE  The device path is NOT a Memory Mapped FV Device Path.
+**/
+BOOLEAN
+BmIsMemmapFvFilePath (
+  IN EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL   *FileNode;
+
+  if ((DevicePathType (DevicePath) == HARDWARE_DEVICE_PATH) && (DevicePathSubType (DevicePath) == HW_MEMMAP_DP)) {
+    FileNode = NextDevicePathNode (DevicePath);
+    if ((DevicePathType (FileNode) == MEDIA_DEVICE_PATH) && (DevicePathSubType (FileNode) == MEDIA_PIWG_FW_FILE_DP)) {
+      return IsDevicePathEnd (NextDevicePathNode (FileNode));
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether a USB device match the specified USB Class device path. This
+  function follows "Load Option Processing" behavior in UEFI specification.
+
+  @param UsbIo       USB I/O protocol associated with the USB device.
+  @param UsbClass    The USB Class device path to match.
+
+  @retval TRUE       The USB device match the USB Class device path.
+  @retval FALSE      The USB device does not match the USB Class device path.
+
+**/
+BOOLEAN
+BmMatchUsbClass (
+  IN EFI_USB_IO_PROTOCOL        *UsbIo,
+  IN USB_CLASS_DEVICE_PATH      *UsbClass
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_USB_DEVICE_DESCRIPTOR     DevDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR  IfDesc;
+  UINT8                         DeviceClass;
+  UINT8                         DeviceSubClass;
+  UINT8                         DeviceProtocol;
+
+  if ((DevicePathType (UsbClass) != MESSAGING_DEVICE_PATH) ||
+      (DevicePathSubType (UsbClass) != MSG_USB_CLASS_DP)){
+    return FALSE;
+  }
+
+  //
+  // Check Vendor Id and Product Id.
+  //
+  Status = UsbIo->UsbGetDeviceDescriptor (UsbIo, &DevDesc);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if ((UsbClass->VendorId != 0xffff) &&
+      (UsbClass->VendorId != DevDesc.IdVendor)) {
+    return FALSE;
+  }
+
+  if ((UsbClass->ProductId != 0xffff) &&
+      (UsbClass->ProductId != DevDesc.IdProduct)) {
+    return FALSE;
+  }
+
+  DeviceClass    = DevDesc.DeviceClass;
+  DeviceSubClass = DevDesc.DeviceSubClass;
+  DeviceProtocol = DevDesc.DeviceProtocol;
+  if (DeviceClass == 0) {
+    //
+    // If Class in Device Descriptor is set to 0, use the Class, SubClass and
+    // Protocol in Interface Descriptor instead.
+    //
+    Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &IfDesc);
+    if (EFI_ERROR (Status)) {
+      return FALSE;
+    }
+
+    DeviceClass    = IfDesc.InterfaceClass;
+    DeviceSubClass = IfDesc.InterfaceSubClass;
+    DeviceProtocol = IfDesc.InterfaceProtocol;
+  }
+
+  //
+  // Check Class, SubClass and Protocol.
+  //
+  if ((UsbClass->DeviceClass != 0xff) &&
+      (UsbClass->DeviceClass != DeviceClass)) {
+    return FALSE;
+  }
+
+  if ((UsbClass->DeviceSubClass != 0xff) &&
+      (UsbClass->DeviceSubClass != DeviceSubClass)) {
+    return FALSE;
+  }
+
+  if ((UsbClass->DeviceProtocol != 0xff) &&
+      (UsbClass->DeviceProtocol != DeviceProtocol)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Eliminate the extra spaces in the Str to one space.
+
+  @param    Str     Input string info.
+**/
+VOID
+BmEliminateExtraSpaces (
+  IN CHAR16                    *Str
+  )
+{
+  UINTN                        Index;
+  UINTN                        ActualIndex;
+
+  for (Index = 0, ActualIndex = 0; Str[Index] != L'\0'; Index++) {
+    if ((Str[Index] != L' ') || ((ActualIndex > 0) && (Str[ActualIndex - 1] != L' '))) {
+      Str[ActualIndex++] = Str[Index];
+    }
+  }
+  Str[ActualIndex] = L'\0';
+}
+
+/**
+  Try to get the controller's ATA/ATAPI description.
+
+  @param Handle                Controller handle.
+
+  @return  The description string.
+**/
+CHAR16 *
+BmGetDescriptionFromDiskInfo (
+  IN EFI_HANDLE                Handle
+  )
+{
+  UINTN                        Index;
+  EFI_STATUS                   Status;
+  EFI_DISK_INFO_PROTOCOL       *DiskInfo;
+  UINT32                       BufferSize;
+  EFI_ATAPI_IDENTIFY_DATA      IdentifyData;
+  EFI_SCSI_INQUIRY_DATA        InquiryData;
+  CHAR16                       *Description;
+  UINTN                        Length;
+  CONST UINTN                  ModelNameLength    = 40;
+  CONST UINTN                  SerialNumberLength = 20;
+  CHAR8                        *StrPtr;
+  UINT8                        Temp;
+
+  Description  = NULL;
+
+  Status = gBS->HandleProtocol (
+                  Handle,
+                  &gEfiDiskInfoProtocolGuid,
+                  (VOID **) &DiskInfo
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  if (CompareGuid (&DiskInfo->Interface, &gEfiDiskInfoAhciInterfaceGuid) || 
+      CompareGuid (&DiskInfo->Interface, &gEfiDiskInfoIdeInterfaceGuid)) {
+    BufferSize   = sizeof (EFI_ATAPI_IDENTIFY_DATA);
+    Status = DiskInfo->Identify (
+                         DiskInfo,
+                         &IdentifyData,
+                         &BufferSize
+                         );
+    if (!EFI_ERROR (Status)) {
+      Description = AllocateZeroPool ((ModelNameLength + SerialNumberLength + 2) * sizeof (CHAR16));
+      ASSERT (Description != NULL);
+      for (Index = 0; Index + 1 < ModelNameLength; Index += 2) {
+        Description[Index]     = (CHAR16) IdentifyData.ModelName[Index + 1];
+        Description[Index + 1] = (CHAR16) IdentifyData.ModelName[Index];
+      }
+
+      Length = Index;
+      Description[Length++] = L' ';
+
+      for (Index = 0; Index + 1 < SerialNumberLength; Index += 2) {
+        Description[Length + Index]     = (CHAR16) IdentifyData.SerialNo[Index + 1];
+        Description[Length + Index + 1] = (CHAR16) IdentifyData.SerialNo[Index];
+      }
+      Length += Index;
+      Description[Length++] = L'\0';
+      ASSERT (Length == ModelNameLength + SerialNumberLength + 2);
+
+      BmEliminateExtraSpaces (Description);
+    }
+  } else if (CompareGuid (&DiskInfo->Interface, &gEfiDiskInfoScsiInterfaceGuid)) {
+    BufferSize   = sizeof (EFI_SCSI_INQUIRY_DATA);
+    Status = DiskInfo->Inquiry (
+                         DiskInfo,
+                         &InquiryData,
+                         &BufferSize
+                         );
+    if (!EFI_ERROR (Status)) {
+      Description = AllocateZeroPool ((VENDOR_IDENTIFICATION_LENGTH + PRODUCT_IDENTIFICATION_LENGTH + 2) * sizeof (CHAR16));
+      ASSERT (Description != NULL);
+
+      //
+      // Per SCSI spec, EFI_SCSI_INQUIRY_DATA.Reserved_5_95[3 - 10] save the Verdor identification
+      // EFI_SCSI_INQUIRY_DATA.Reserved_5_95[11 - 26] save the product identification, 
+      // Here combine the vendor identification and product identification to the description.
+      //
+      StrPtr = (CHAR8 *) (&InquiryData.Reserved_5_95[VENDOR_IDENTIFICATION_OFFSET]);
+      Temp = StrPtr[VENDOR_IDENTIFICATION_LENGTH];
+      StrPtr[VENDOR_IDENTIFICATION_LENGTH] = '\0';
+      AsciiStrToUnicodeStrS (StrPtr, Description, VENDOR_IDENTIFICATION_LENGTH + 1);
+      StrPtr[VENDOR_IDENTIFICATION_LENGTH] = Temp;
+
+      //
+      // Add one space at the middle of vendor information and product information.
+      //
+      Description[VENDOR_IDENTIFICATION_LENGTH] = L' ';
+
+      StrPtr = (CHAR8 *) (&InquiryData.Reserved_5_95[PRODUCT_IDENTIFICATION_OFFSET]);
+      StrPtr[PRODUCT_IDENTIFICATION_LENGTH] = '\0';
+      AsciiStrToUnicodeStrS (StrPtr, Description + VENDOR_IDENTIFICATION_LENGTH + 1, PRODUCT_IDENTIFICATION_LENGTH + 1);
+
+      BmEliminateExtraSpaces (Description);
+    }
+  }
+
+  return Description;
+}
+
+/**
+  Try to get the controller's USB description.
+
+  @param Handle                Controller handle.
+
+  @return  The description string.
+**/
+CHAR16 *
+BmGetUsbDescription (
+  IN EFI_HANDLE                Handle
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_USB_IO_PROTOCOL          *UsbIo;
+  CHAR16                       NullChar;
+  CHAR16                       *Manufacturer;
+  CHAR16                       *Product;
+  CHAR16                       *SerialNumber;
+  CHAR16                       *Description;
+  EFI_USB_DEVICE_DESCRIPTOR    DevDesc;
+  UINTN                        DescMaxSize;
+
+  Status = gBS->HandleProtocol (
+                  Handle,
+                  &gEfiUsbIoProtocolGuid,
+                  (VOID **) &UsbIo
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  NullChar = L'\0';
+
+  Status = UsbIo->UsbGetDeviceDescriptor (UsbIo, &DevDesc);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  Status = UsbIo->UsbGetStringDescriptor (
+                    UsbIo,
+                    mBmUsbLangId,
+                    DevDesc.StrManufacturer,
+                    &Manufacturer
+                    );
+  if (EFI_ERROR (Status)) {
+    Manufacturer = &NullChar;
+  }
+  
+  Status = UsbIo->UsbGetStringDescriptor (
+                    UsbIo,
+                    mBmUsbLangId,
+                    DevDesc.StrProduct,
+                    &Product
+                    );
+  if (EFI_ERROR (Status)) {
+    Product = &NullChar;
+  }
+  
+  Status = UsbIo->UsbGetStringDescriptor (
+                    UsbIo,
+                    mBmUsbLangId,
+                    DevDesc.StrSerialNumber,
+                    &SerialNumber
+                    );
+  if (EFI_ERROR (Status)) {
+    SerialNumber = &NullChar;
+  }
+
+  if ((Manufacturer == &NullChar) &&
+      (Product == &NullChar) &&
+      (SerialNumber == &NullChar)
+      ) {
+    return NULL;
+  }
+
+  DescMaxSize = StrSize (Manufacturer) + StrSize (Product) + StrSize (SerialNumber);
+  Description = AllocateZeroPool (DescMaxSize);
+  ASSERT (Description != NULL);
+  StrCatS (Description, DescMaxSize/sizeof(CHAR16), Manufacturer);
+  StrCatS (Description, DescMaxSize/sizeof(CHAR16), L" ");
+
+  StrCatS (Description, DescMaxSize/sizeof(CHAR16), Product);  
+  StrCatS (Description, DescMaxSize/sizeof(CHAR16), L" ");
+
+  StrCatS (Description, DescMaxSize/sizeof(CHAR16), SerialNumber);
+
+  if (Manufacturer != &NullChar) {
+    FreePool (Manufacturer);
+  }
+  if (Product != &NullChar) {
+    FreePool (Product);
+  }
+  if (SerialNumber != &NullChar) {
+    FreePool (SerialNumber);
+  }
+
+  BmEliminateExtraSpaces (Description);
+
+  return Description;
+}
+
+/**
+  Return the boot description for the controller based on the type.
+
+  @param Handle                Controller handle.
+
+  @return  The description string.
+**/
+CHAR16 *
+BmGetMiscDescription (
+  IN EFI_HANDLE                  Handle
+  )
+{
+  EFI_STATUS                      Status;
+  CHAR16                          *Description;
+  EFI_BLOCK_IO_PROTOCOL           *BlockIo;
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *Fs;
+
+  switch (BmDevicePathType (DevicePathFromHandle (Handle))) {
+  case BmAcpiFloppyBoot:
+    Description = L"Floppy";
+    break;
+
+  case BmMessageAtapiBoot:
+  case BmMessageSataBoot:
+    Status = gBS->HandleProtocol (Handle, &gEfiBlockIoProtocolGuid, (VOID **) &BlockIo);
+    ASSERT_EFI_ERROR (Status);
+    //
+    // Assume a removable SATA device should be the DVD/CD device
+    //
+    Description = BlockIo->Media->RemovableMedia ? L"DVD/CDROM" : L"Hard Drive";
+    break;
+
+  case BmMessageUsbBoot:
+    Description = L"USB Device";
+    break;
+
+  case BmMessageScsiBoot:
+    Description = L"SCSI Device";
+    break;
+
+  case BmHardwareDeviceBoot:
+    Status = gBS->HandleProtocol (Handle, &gEfiBlockIoProtocolGuid, (VOID **) &BlockIo);
+    if (!EFI_ERROR (Status)) {
+      Description = BlockIo->Media->RemovableMedia ? L"Removable Disk" : L"Hard Drive";
+    } else {
+      Description = L"Misc Device";
+    }
+    break;
+
+  case BmMessageNetworkBoot:
+    Description = L"Network";
+    break;
+
+  case BmMessageHttpBoot:
+    Description = L"Http";
+    break;
+
+  default:
+    Status = gBS->HandleProtocol (Handle, &gEfiSimpleFileSystemProtocolGuid, (VOID **) &Fs);
+    if (!EFI_ERROR (Status)) {
+      Description = L"Non-Block Boot Device";
+    } else {
+      Description = L"Misc Device";
+    }
+    break;
+  }
+
+  return AllocateCopyPool (StrSize (Description), Description);
+}
+
+/**
+  Register the platform provided boot description handler.
+
+  @param Handler  The platform provided boot description handler
+
+  @retval EFI_SUCCESS          The handler was registered successfully.
+  @retval EFI_ALREADY_STARTED  The handler was already registered.
+  @retval EFI_OUT_OF_RESOURCES There is not enough resource to perform the registration.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerRegisterBootDescriptionHandler (
+  IN EFI_BOOT_MANAGER_BOOT_DESCRIPTION_HANDLER  Handler
+  )
+{
+  LIST_ENTRY                                    *Link;
+  BM_BOOT_DESCRIPTION_ENTRY                    *Entry;
+
+  for ( Link = GetFirstNode (&mPlatformBootDescriptionHandlers)
+      ; !IsNull (&mPlatformBootDescriptionHandlers, Link)
+      ; Link = GetNextNode (&mPlatformBootDescriptionHandlers, Link)
+      ) {
+    Entry = CR (Link, BM_BOOT_DESCRIPTION_ENTRY, Link, BM_BOOT_DESCRIPTION_ENTRY_SIGNATURE);
+    if (Entry->Handler == Handler) {
+      return EFI_ALREADY_STARTED;
+    }
+  }
+
+  Entry = AllocatePool (sizeof (BM_BOOT_DESCRIPTION_ENTRY));
+  if (Entry == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Entry->Signature = BM_BOOT_DESCRIPTION_ENTRY_SIGNATURE;
+  Entry->Handler   = Handler;
+  InsertTailList (&mPlatformBootDescriptionHandlers, &Entry->Link);
+  return EFI_SUCCESS;
+}
+
+BM_GET_BOOT_DESCRIPTION mBmBootDescriptionHandlers[] = {
+  BmGetUsbDescription,
+  BmGetDescriptionFromDiskInfo,
+  BmGetMiscDescription
+};
+
+/**
+  Return the boot description for the controller.
+
+  @param Handle                Controller handle.
+
+  @return  The description string.
+**/
+CHAR16 *
+BmGetBootDescription (
+  IN EFI_HANDLE                  Handle
+  )
+{
+  LIST_ENTRY                     *Link;
+  BM_BOOT_DESCRIPTION_ENTRY      *Entry;
+  CHAR16                         *Description;
+  CHAR16                         *DefaultDescription;
+  CHAR16                         *Temp;
+  UINTN                          Index;
+
+  //
+  // Firstly get the default boot description
+  //
+  DefaultDescription = NULL;
+  for (Index = 0; Index < sizeof (mBmBootDescriptionHandlers) / sizeof (mBmBootDescriptionHandlers[0]); Index++) {
+    DefaultDescription = mBmBootDescriptionHandlers[Index] (Handle);
+    if (DefaultDescription != NULL) {
+      //
+      // Avoid description confusion between UEFI & Legacy boot option by adding "UEFI " prefix
+      // ONLY for core provided boot description handler.
+      //
+      Temp = AllocatePool (StrSize (DefaultDescription) + sizeof (mBmUefiPrefix)); 
+      ASSERT (Temp != NULL);
+      StrCpyS ( Temp, 
+                (StrSize (DefaultDescription) + sizeof (mBmUefiPrefix))/sizeof(CHAR16), 
+                mBmUefiPrefix
+                );
+      StrCatS ( Temp, 
+                (StrSize (DefaultDescription) + sizeof (mBmUefiPrefix))/sizeof(CHAR16), 
+                DefaultDescription
+                );
+      FreePool (DefaultDescription);
+      DefaultDescription = Temp;
+      break;
+    }
+  }
+  ASSERT (DefaultDescription != NULL);
+
+  //
+  // Secondly query platform for the better boot description
+  //
+  for ( Link = GetFirstNode (&mPlatformBootDescriptionHandlers)
+      ; !IsNull (&mPlatformBootDescriptionHandlers, Link)
+      ; Link = GetNextNode (&mPlatformBootDescriptionHandlers, Link)
+      ) {
+    Entry = CR (Link, BM_BOOT_DESCRIPTION_ENTRY, Link, BM_BOOT_DESCRIPTION_ENTRY_SIGNATURE);
+    Description = Entry->Handler (Handle, DefaultDescription);
+    if (Description != NULL) {
+      FreePool (DefaultDescription);
+      return Description;
+    }
+  }
+
+  return DefaultDescription;
+}
+
+/**
+  Check whether a USB device match the specified USB WWID device path. This
+  function follows "Load Option Processing" behavior in UEFI specification.
+
+  @param UsbIo       USB I/O protocol associated with the USB device.
+  @param UsbWwid     The USB WWID device path to match.
+
+  @retval TRUE       The USB device match the USB WWID device path.
+  @retval FALSE      The USB device does not match the USB WWID device path.
+
+**/
+BOOLEAN
+BmMatchUsbWwid (
+  IN EFI_USB_IO_PROTOCOL        *UsbIo,
+  IN USB_WWID_DEVICE_PATH       *UsbWwid
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_USB_DEVICE_DESCRIPTOR    DevDesc;
+  EFI_USB_INTERFACE_DESCRIPTOR IfDesc;
+  UINT16                       *LangIdTable;
+  UINT16                       TableSize;
+  UINT16                       Index;
+  CHAR16                       *CompareStr;
+  UINTN                        CompareLen;
+  CHAR16                       *SerialNumberStr;
+  UINTN                        Length;
+
+  if ((DevicePathType (UsbWwid) != MESSAGING_DEVICE_PATH) ||
+      (DevicePathSubType (UsbWwid) != MSG_USB_WWID_DP)) {
+    return FALSE;
+  }
+
+  //
+  // Check Vendor Id and Product Id.
+  //
+  Status = UsbIo->UsbGetDeviceDescriptor (UsbIo, &DevDesc);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  if ((DevDesc.IdVendor != UsbWwid->VendorId) ||
+      (DevDesc.IdProduct != UsbWwid->ProductId)) {
+    return FALSE;
+  }
+
+  //
+  // Check Interface Number.
+  //
+  Status = UsbIo->UsbGetInterfaceDescriptor (UsbIo, &IfDesc);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  if (IfDesc.InterfaceNumber != UsbWwid->InterfaceNumber) {
+    return FALSE;
+  }
+
+  //
+  // Check Serial Number.
+  //
+  if (DevDesc.StrSerialNumber == 0) {
+    return FALSE;
+  }
+
+  //
+  // Get all supported languages.
+  //
+  TableSize = 0;
+  LangIdTable = NULL;
+  Status = UsbIo->UsbGetSupportedLanguages (UsbIo, &LangIdTable, &TableSize);
+  if (EFI_ERROR (Status) || (TableSize == 0) || (LangIdTable == NULL)) {
+    return FALSE;
+  }
+
+  //
+  // Serial number in USB WWID device path is the last 64-or-less UTF-16 characters.
+  //
+  CompareStr = (CHAR16 *) (UINTN) (UsbWwid + 1);
+  CompareLen = (DevicePathNodeLength (UsbWwid) - sizeof (USB_WWID_DEVICE_PATH)) / sizeof (CHAR16);
+  if (CompareStr[CompareLen - 1] == L'\0') {
+    CompareLen--;
+  }
+
+  //
+  // Compare serial number in each supported language.
+  //
+  for (Index = 0; Index < TableSize / sizeof (UINT16); Index++) {
+    SerialNumberStr = NULL;
+    Status = UsbIo->UsbGetStringDescriptor (
+                      UsbIo,
+                      LangIdTable[Index],
+                      DevDesc.StrSerialNumber,
+                      &SerialNumberStr
+                      );
+    if (EFI_ERROR (Status) || (SerialNumberStr == NULL)) {
+      continue;
+    }
+
+    Length = StrLen (SerialNumberStr);
+    if ((Length >= CompareLen) &&
+        (CompareMem (SerialNumberStr + Length - CompareLen, CompareStr, CompareLen * sizeof (CHAR16)) == 0)) {
+      FreePool (SerialNumberStr);
+      return TRUE;
+    }
+
+    FreePool (SerialNumberStr);
+  }
+
+  return FALSE;
+}
+
+/**
+  Find a USB device which match the specified short-form device path start with 
+  USB Class or USB WWID device path. If ParentDevicePath is NULL, this function
+  will search in all USB devices of the platform. If ParentDevicePath is not NULL,
+  this function will only search in its child devices.
+
+  @param DevicePath           The device path that contains USB Class or USB WWID device path.
+  @param ParentDevicePathSize The length of the device path before the USB Class or 
+                              USB WWID device path.
+  @param UsbIoHandleCount     A pointer to the count of the returned USB IO handles.
+
+  @retval NULL       The matched USB IO handles cannot be found.
+  @retval other      The matched USB IO handles.
+
+**/
+EFI_HANDLE *
+BmFindUsbDevice (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN  UINTN                     ParentDevicePathSize,
+  OUT UINTN                     *UsbIoHandleCount
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HANDLE                *UsbIoHandles;
+  EFI_DEVICE_PATH_PROTOCOL  *UsbIoDevicePath;
+  EFI_USB_IO_PROTOCOL       *UsbIo;
+  UINTN                     Index;
+  BOOLEAN                   Matched;
+
+  ASSERT (UsbIoHandleCount != NULL);  
+
+  //
+  // Get all UsbIo Handles.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiUsbIoProtocolGuid,
+                  NULL,
+                  UsbIoHandleCount,
+                  &UsbIoHandles
+                  );
+  if (EFI_ERROR (Status)) {
+    *UsbIoHandleCount = 0;
+    UsbIoHandles      = NULL;
+  }
+
+  for (Index = 0; Index < *UsbIoHandleCount; ) {
+    //
+    // Get the Usb IO interface.
+    //
+    Status = gBS->HandleProtocol(
+                    UsbIoHandles[Index],
+                    &gEfiUsbIoProtocolGuid,
+                    (VOID **) &UsbIo
+                    );
+    UsbIoDevicePath = DevicePathFromHandle (UsbIoHandles[Index]);
+    Matched         = FALSE;
+    if (!EFI_ERROR (Status) && (UsbIoDevicePath != NULL)) {
+
+      //
+      // Compare starting part of UsbIoHandle's device path with ParentDevicePath.
+      //
+      if (CompareMem (UsbIoDevicePath, DevicePath, ParentDevicePathSize) == 0) {
+        if (BmMatchUsbClass (UsbIo, (USB_CLASS_DEVICE_PATH *) ((UINTN) DevicePath + ParentDevicePathSize)) ||
+            BmMatchUsbWwid (UsbIo, (USB_WWID_DEVICE_PATH *) ((UINTN) DevicePath + ParentDevicePathSize))) {
+          Matched = TRUE;
+        }
+      }
+    }
+
+    if (!Matched) {
+      (*UsbIoHandleCount) --;
+      CopyMem (&UsbIoHandles[Index], &UsbIoHandles[Index + 1], (*UsbIoHandleCount - Index) * sizeof (EFI_HANDLE));
+    } else {
+      Index++;
+    }
+  }
+
+  return UsbIoHandles;
+}
+
+/**
+  Expand USB Class or USB WWID device path node to be full device path of a USB
+  device in platform.
+
+  This function support following 4 cases:
+  1) Boot Option device path starts with a USB Class or USB WWID device path,
+     and there is no Media FilePath device path in the end.
+     In this case, it will follow Removable Media Boot Behavior.
+  2) Boot Option device path starts with a USB Class or USB WWID device path,
+     and ended with Media FilePath device path.
+  3) Boot Option device path starts with a full device path to a USB Host Controller,
+     contains a USB Class or USB WWID device path node, while not ended with Media
+     FilePath device path. In this case, it will follow Removable Media Boot Behavior.
+  4) Boot Option device path starts with a full device path to a USB Host Controller,
+     contains a USB Class or USB WWID device path node, and ended with Media
+     FilePath device path.
+
+  @param FilePath      The device path pointing to a load option.
+                       It could be a short-form device path.
+  @param FullPath      Return the full device path of the load option after
+                       short-form device path expanding.
+                       Caller is responsible to free it.
+  @param FileSize      Return the load option size.
+  @param ShortformNode Pointer to the USB short-form device path node in the FilePath buffer.
+
+  @return The load option buffer. Caller is responsible to free the memory.
+**/
+VOID *
+BmExpandUsbDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *FilePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL  **FullPath,
+  OUT UINTN                     *FileSize,
+  IN EFI_DEVICE_PATH_PROTOCOL   *ShortformNode
+  )
+{
+  UINTN                             ParentDevicePathSize;
+  EFI_DEVICE_PATH_PROTOCOL          *RemainingDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL          *FullDevicePath;
+  EFI_HANDLE                        *Handles;
+  UINTN                             HandleCount;
+  UINTN                             Index;
+  VOID                              *FileBuffer;
+
+  ParentDevicePathSize = (UINTN) ShortformNode - (UINTN) FilePath;
+  RemainingDevicePath = NextDevicePathNode (ShortformNode);
+  FileBuffer = NULL;
+  Handles = BmFindUsbDevice (FilePath, ParentDevicePathSize, &HandleCount);
+
+  for (Index = 0; (Index < HandleCount) && (FileBuffer == NULL); Index++) {
+    FullDevicePath = AppendDevicePath (DevicePathFromHandle (Handles[Index]), RemainingDevicePath);
+    FileBuffer = BmGetLoadOptionBuffer (FullDevicePath, FullPath, FileSize);
+    FreePool (FullDevicePath);
+  }
+
+  if (Handles != NULL) {
+    FreePool (Handles);
+  }
+
+  return FileBuffer;
+}
+
+/**
+  Save the partition DevicePath to the CachedDevicePath as the first instance.
+
+  @param CachedDevicePath  The device path cache.
+  @param DevicePath        The partition device path to be cached.
+**/
+VOID
+BmCachePartitionDevicePath (
+  IN OUT EFI_DEVICE_PATH_PROTOCOL **CachedDevicePath,
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL        *TempDevicePath;
+  UINTN                           Count;
+  
+  if (BmMatchDevicePaths (*CachedDevicePath, DevicePath)) {
+    TempDevicePath = *CachedDevicePath;
+    *CachedDevicePath = BmDelPartMatchInstance (*CachedDevicePath, DevicePath);
+    FreePool (TempDevicePath);
+  }
+
+  if (*CachedDevicePath == NULL) {
+    *CachedDevicePath = DuplicateDevicePath (DevicePath);
+    return;
+  }
+
+  TempDevicePath = *CachedDevicePath;
+  *CachedDevicePath = AppendDevicePathInstance (DevicePath, *CachedDevicePath);
+  if (TempDevicePath != NULL) {
+    FreePool (TempDevicePath);
+  }
+
+  //
+  // Here limit the device path instance number to 12, which is max number for a system support 3 IDE controller
+  // If the user try to boot many OS in different HDs or partitions, in theory, the 'HDDP' variable maybe become larger and larger.
+  //
+  Count = 0;
+  TempDevicePath = *CachedDevicePath;
+  while (!IsDevicePathEnd (TempDevicePath)) {
+    TempDevicePath = NextDevicePathNode (TempDevicePath);
+    //
+    // Parse one instance
+    //
+    while (!IsDevicePathEndType (TempDevicePath)) {
+      TempDevicePath = NextDevicePathNode (TempDevicePath);
+    }
+    Count++;
+    //
+    // If the CachedDevicePath variable contain too much instance, only remain 12 instances.
+    //
+    if (Count == 12) {
+      SetDevicePathEndNode (TempDevicePath);
+      break;
+    }
+  }
+}
+
+/**
+  Expand a device path that starts with a hard drive media device path node to be a
+  full device path that includes the full hardware path to the device. We need
+  to do this so it can be booted. As an optimization the front match (the part point
+  to the partition node. E.g. ACPI() /PCI()/ATA()/Partition() ) is saved in a variable
+  so a connect all is not required on every boot. All successful history device path
+  which point to partition node (the front part) will be saved.
+
+  @param FilePath      The device path pointing to a load option.
+                       It could be a short-form device path.
+  @param FullPath      Return the full device path of the load option after
+                       short-form device path expanding.
+                       Caller is responsible to free it.
+  @param FileSize      Return the load option size.
+
+  @return The load option buffer. Caller is responsible to free the memory.
+**/
+VOID *
+BmExpandPartitionDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *FilePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL  **FullPath,
+  OUT UINTN                     *FileSize
+  )
+{
+  EFI_STATUS                Status;
+  UINTN                     BlockIoHandleCount;
+  EFI_HANDLE                *BlockIoBuffer;
+  VOID                      *FileBuffer;
+  EFI_DEVICE_PATH_PROTOCOL  *BlockIoDevicePath;
+  UINTN                     Index;
+  EFI_DEVICE_PATH_PROTOCOL  *CachedDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *TempNewDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *TempDevicePath;
+  UINTN                     CachedDevicePathSize;
+  BOOLEAN                   NeedAdjust;
+  EFI_DEVICE_PATH_PROTOCOL  *Instance;
+  UINTN                     Size;
+
+  FileBuffer = NULL;
+  //
+  // Check if there is prestore 'HDDP' variable.
+  // If exist, search the front path which point to partition node in the variable instants.
+  // If fail to find or 'HDDP' not exist, reconnect all and search in all system
+  //
+  GetVariable2 (L"HDDP", &mBmHardDriveBootVariableGuid, (VOID **) &CachedDevicePath, &CachedDevicePathSize);
+
+  //
+  // Delete the invalid 'HDDP' variable.
+  //
+  if ((CachedDevicePath != NULL) && !IsDevicePathValid (CachedDevicePath, CachedDevicePathSize)) {
+    FreePool (CachedDevicePath);
+    CachedDevicePath = NULL;
+    Status = gRT->SetVariable (
+                    L"HDDP",
+                    &mBmHardDriveBootVariableGuid,
+                    0,
+                    0,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (CachedDevicePath != NULL) {
+    TempNewDevicePath = CachedDevicePath;
+    NeedAdjust = FALSE;
+    do {
+      //
+      // Check every instance of the variable
+      // First, check whether the instance contain the partition node, which is needed for distinguishing  multi
+      // partial partition boot option. Second, check whether the instance could be connected.
+      //
+      Instance  = GetNextDevicePathInstance (&TempNewDevicePath, &Size);
+      if (BmMatchPartitionDevicePathNode (Instance, (HARDDRIVE_DEVICE_PATH *) FilePath)) {
+        //
+        // Connect the device path instance, the device path point to hard drive media device path node
+        // e.g. ACPI() /PCI()/ATA()/Partition()
+        //
+        Status = EfiBootManagerConnectDevicePath (Instance, NULL);
+        if (!EFI_ERROR (Status)) {
+          TempDevicePath = AppendDevicePath (Instance, NextDevicePathNode (FilePath));
+          FileBuffer = BmGetLoadOptionBuffer (TempDevicePath, FullPath, FileSize);
+          FreePool (TempDevicePath);
+
+          if (FileBuffer != NULL) {
+            //
+            // Adjust the 'HDDP' instances sequence if the matched one is not first one.
+            //
+            if (NeedAdjust) {
+              BmCachePartitionDevicePath (&CachedDevicePath, Instance);
+              //
+              // Save the matching Device Path so we don't need to do a connect all next time
+              // Failing to save only impacts performance next time expanding the short-form device path
+              //
+              Status = gRT->SetVariable (
+                L"HDDP",
+                &mBmHardDriveBootVariableGuid,
+                EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                GetDevicePathSize (CachedDevicePath),
+                CachedDevicePath
+                );
+            }
+
+            FreePool (Instance);
+            FreePool (CachedDevicePath);
+            return FileBuffer;
+          }
+        }
+      }
+      //
+      // Come here means the first instance is not matched
+      //
+      NeedAdjust = TRUE;
+      FreePool(Instance);
+    } while (TempNewDevicePath != NULL);
+  }
+
+  //
+  // If we get here we fail to find or 'HDDP' not exist, and now we need
+  // to search all devices in the system for a matched partition
+  //
+  EfiBootManagerConnectAll ();
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiBlockIoProtocolGuid, NULL, &BlockIoHandleCount, &BlockIoBuffer);
+  if (EFI_ERROR (Status)) {
+    BlockIoHandleCount = 0;
+    BlockIoBuffer      = NULL;
+  }
+  //
+  // Loop through all the device handles that support the BLOCK_IO Protocol
+  //
+  for (Index = 0; Index < BlockIoHandleCount; Index++) {
+    BlockIoDevicePath = DevicePathFromHandle (BlockIoBuffer[Index]);
+    if (BlockIoDevicePath == NULL) {
+      continue;
+    }
+
+    if (BmMatchPartitionDevicePathNode (BlockIoDevicePath, (HARDDRIVE_DEVICE_PATH *) FilePath)) {
+      //
+      // Find the matched partition device path
+      //
+      TempDevicePath = AppendDevicePath (BlockIoDevicePath, NextDevicePathNode (FilePath));
+      FileBuffer = BmGetLoadOptionBuffer (TempDevicePath, FullPath, FileSize);
+      FreePool (TempDevicePath);
+
+      if (FileBuffer != NULL) {
+        BmCachePartitionDevicePath (&CachedDevicePath, BlockIoDevicePath);
+
+        //
+        // Save the matching Device Path so we don't need to do a connect all next time
+        // Failing to save only impacts performance next time expanding the short-form device path
+        //
+        Status = gRT->SetVariable (
+                        L"HDDP",
+                        &mBmHardDriveBootVariableGuid,
+                        EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                        GetDevicePathSize (CachedDevicePath),
+                        CachedDevicePath
+                        );
+
+        break;
+      }
+    }
+  }
+
+  if (CachedDevicePath != NULL) {
+    FreePool (CachedDevicePath);
+  }
+  if (BlockIoBuffer != NULL) {
+    FreePool (BlockIoBuffer);
+  }
+  return FileBuffer;
+}
+
+/**
+  Expand the media device path which points to a BlockIo or SimpleFileSystem instance
+  by appending EFI_REMOVABLE_MEDIA_FILE_NAME.
+
+  @param DevicePath  The media device path pointing to a BlockIo or SimpleFileSystem instance.
+  @param FullPath    Return the full device path pointing to the load option.
+  @param FileSize    Return the size of the load option.
+
+  @return  The load option buffer.
+**/
+VOID *
+BmExpandMediaDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL        *DevicePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL        **FullPath,
+  OUT UINTN                           *FileSize
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HANDLE                          Handle;
+  EFI_BLOCK_IO_PROTOCOL               *BlockIo;
+  VOID                                *Buffer;
+  EFI_DEVICE_PATH_PROTOCOL            *TempDevicePath;
+  UINTN                               Size;
+  UINTN                               TempSize;
+  EFI_HANDLE                          *SimpleFileSystemHandles;
+  UINTN                               NumberSimpleFileSystemHandles;
+  UINTN                               Index;
+  VOID                                *FileBuffer;
+  UINT32                              AuthenticationStatus;
+
+  //
+  // Check whether the device is connected
+  //
+  TempDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiSimpleFileSystemProtocolGuid, &TempDevicePath, &Handle);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (IsDevicePathEnd (TempDevicePath));
+
+    TempDevicePath = FileDevicePath (Handle, EFI_REMOVABLE_MEDIA_FILE_NAME);
+    FileBuffer = GetFileBufferByFilePath (TRUE, TempDevicePath, FileSize, &AuthenticationStatus);
+    if (FileBuffer == NULL) {
+      FreePool (TempDevicePath);
+      TempDevicePath = NULL;
+    }
+    *FullPath = TempDevicePath;
+    return FileBuffer;
+  }
+
+  //
+  // For device boot option only pointing to the removable device handle, 
+  // should make sure all its children handles (its child partion or media handles) are created and connected. 
+  //
+  gBS->ConnectController (Handle, NULL, NULL, TRUE);
+
+  //
+  // Issue a dummy read to the device to check for media change.
+  // When the removable media is changed, any Block IO read/write will
+  // cause the BlockIo protocol be reinstalled and EFI_MEDIA_CHANGED is
+  // returned. After the Block IO protocol is reinstalled, subsequent
+  // Block IO read/write will success.
+  //
+  Status = gBS->LocateDevicePath (&gEfiBlockIoProtocolGuid, &TempDevicePath, &Handle);
+  ASSERT_EFI_ERROR (Status);
+  Status = gBS->HandleProtocol (Handle, &gEfiBlockIoProtocolGuid, (VOID **) &BlockIo);
+  ASSERT_EFI_ERROR (Status);
+  Buffer = AllocatePool (BlockIo->Media->BlockSize);
+  if (Buffer != NULL) {
+    BlockIo->ReadBlocks (
+      BlockIo,
+      BlockIo->Media->MediaId,
+      0,
+      BlockIo->Media->BlockSize,
+      Buffer
+      );
+    FreePool (Buffer);
+  }
+
+  //
+  // Detect the the default boot file from removable Media
+  //
+  FileBuffer = NULL;
+  *FullPath = NULL;
+  Size = GetDevicePathSize (DevicePath) - END_DEVICE_PATH_LENGTH;
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiSimpleFileSystemProtocolGuid,
+         NULL,
+         &NumberSimpleFileSystemHandles,
+         &SimpleFileSystemHandles
+         );
+  for (Index = 0; Index < NumberSimpleFileSystemHandles; Index++) {
+    //
+    // Get the device path size of SimpleFileSystem handle
+    //
+    TempDevicePath = DevicePathFromHandle (SimpleFileSystemHandles[Index]);
+    TempSize = GetDevicePathSize (TempDevicePath) - END_DEVICE_PATH_LENGTH;
+    //
+    // Check whether the device path of boot option is part of the SimpleFileSystem handle's device path
+    //
+    if ((Size <= TempSize) && (CompareMem (TempDevicePath, DevicePath, Size) == 0)) {
+      TempDevicePath = FileDevicePath (SimpleFileSystemHandles[Index], EFI_REMOVABLE_MEDIA_FILE_NAME);
+      FileBuffer = GetFileBufferByFilePath (TRUE, TempDevicePath, FileSize, &AuthenticationStatus);
+      if (FileBuffer != NULL) {
+        *FullPath = TempDevicePath;
+        break;
+      }
+      FreePool (TempDevicePath);
+    }
+  }
+
+  if (SimpleFileSystemHandles != NULL) {
+    FreePool (SimpleFileSystemHandles);
+  }
+
+  return FileBuffer;
+}
+
+/**
+  Get the load option by its device path.
+
+  @param FilePath  The device path pointing to a load option.
+                   It could be a short-form device path.
+  @param FullPath  Return the full device path of the load option after
+                   short-form device path expanding.
+                   Caller is responsible to free it.
+  @param FileSize  Return the load option size.
+
+  @return The load option buffer. Caller is responsible to free the memory.
+**/
+VOID *
+BmGetLoadOptionBuffer (
+  IN  EFI_DEVICE_PATH_PROTOCOL          *FilePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL          **FullPath,
+  OUT UINTN                             *FileSize
+  )
+{
+  EFI_HANDLE                      Handle;
+  VOID                            *FileBuffer;
+  UINT32                          AuthenticationStatus;
+  EFI_DEVICE_PATH_PROTOCOL        *Node;
+  EFI_STATUS                      Status;
+
+  ASSERT ((FilePath != NULL) && (FullPath != NULL) && (FileSize != NULL));
+
+  EfiBootManagerConnectDevicePath (FilePath, NULL);
+
+  *FullPath  = NULL;
+  *FileSize  = 0;
+  FileBuffer = NULL;
+
+  //
+  // Boot from media device by adding a default file name \EFI\BOOT\BOOT{machine type short-name}.EFI
+  //
+  Node = FilePath;
+  Status = gBS->LocateDevicePath (&gEfiSimpleFileSystemProtocolGuid, &Node, &Handle);
+  if (EFI_ERROR (Status)) {
+    Status = gBS->LocateDevicePath (&gEfiBlockIoProtocolGuid, &Node, &Handle);
+  }
+
+  if (!EFI_ERROR (Status) && IsDevicePathEnd (Node)) {
+    return BmExpandMediaDevicePath (FilePath, FullPath, FileSize);
+  }
+
+  //
+  // Expand the short-form device path to full device path
+  //
+  if ((DevicePathType (FilePath) == MEDIA_DEVICE_PATH) &&
+      (DevicePathSubType (FilePath) == MEDIA_HARDDRIVE_DP)) {
+    //
+    // Expand the Harddrive device path
+    //
+    return BmExpandPartitionDevicePath (FilePath, FullPath, FileSize);
+  } else {
+    for (Node = FilePath; !IsDevicePathEnd (Node); Node = NextDevicePathNode (Node)) {
+      if ((DevicePathType (Node) == MESSAGING_DEVICE_PATH) &&
+          ((DevicePathSubType (Node) == MSG_USB_CLASS_DP) || (DevicePathSubType (Node) == MSG_USB_WWID_DP))) {
+        break;
+      }
+    }
+
+    if (!IsDevicePathEnd (Node)) {
+      //
+      // Expand the USB WWID/Class device path
+      //
+      FileBuffer = BmExpandUsbDevicePath (FilePath, FullPath, FileSize, Node);
+      if ((FileBuffer == NULL) && (FilePath == Node)) {
+        //
+        // Boot Option device path starts with USB Class or USB WWID device path.
+        // For Boot Option device path which doesn't begin with the USB Class or
+        // USB WWID device path, it's not needed to connect again here.
+        //
+        BmConnectUsbShortFormDevicePath (FilePath);
+        FileBuffer = BmExpandUsbDevicePath (FilePath, FullPath, FileSize, Node);
+      }
+      return FileBuffer;
+    }
+  }
+
+  //
+  // Fix up the boot option path if it points to a FV in memory map style of device path
+  //
+  if (BmIsMemmapFvFilePath (FilePath)) {
+    return BmGetFileBufferByMemmapFv (FilePath, FullPath, FileSize);
+  }
+
+  //
+  // Directly reads the load option when it doesn't reside in simple file system instance (LoadFile/LoadFile2),
+  //   or it directly points to a file in simple file system instance.
+  //
+  Node   = FilePath;
+  Status = gBS->LocateDevicePath (&gEfiLoadFileProtocolGuid, &Node, &Handle);
+  FileBuffer = GetFileBufferByFilePath (TRUE, FilePath, FileSize, &AuthenticationStatus);
+  if (FileBuffer != NULL) {
+    if (EFI_ERROR (Status)) {
+      *FullPath = DuplicateDevicePath (FilePath);
+    } else {
+      //
+      // LoadFile () may cause the device path of the Handle be updated.
+      //
+      *FullPath = AppendDevicePath (DevicePathFromHandle (Handle), Node);
+    }
+  }
+
+  return FileBuffer;
+}
+
+/**
+  Attempt to boot the EFI boot option. This routine sets L"BootCurent" and
+  also signals the EFI ready to boot event. If the device path for the option
+  starts with a BBS device path a legacy boot is attempted via the registered 
+  gLegacyBoot function. Short form device paths are also supported via this 
+  rountine. A device path starting with MEDIA_HARDDRIVE_DP, MSG_USB_WWID_DP,
+  MSG_USB_CLASS_DP gets expaned out to find the first device that matches.
+  If the BootOption Device Path fails the removable media boot algorithm 
+  is attempted (\EFI\BOOTIA32.EFI, \EFI\BOOTX64.EFI,... only one file type 
+  is tried per processor type)
+
+  @param  BootOption    Boot Option to try and boot.
+                        On return, BootOption->Status contains the boot status.
+                        EFI_SUCCESS     BootOption was booted
+                        EFI_UNSUPPORTED A BBS device path was found with no valid callback
+                                        registered via EfiBootManagerInitialize().
+                        EFI_NOT_FOUND   The BootOption was not found on the system
+                        !EFI_SUCCESS    BootOption failed with this error status
+
+**/
+VOID
+EFIAPI
+EfiBootManagerBoot (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION             *BootOption
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HANDLE                ImageHandle;
+  EFI_LOADED_IMAGE_PROTOCOL *ImageInfo;
+  UINT16                    Uint16;
+  UINTN                     OptionNumber;
+  UINTN                     OriginalOptionNumber;
+  EFI_DEVICE_PATH_PROTOCOL  *FilePath;
+  EFI_DEVICE_PATH_PROTOCOL  *Node;
+  EFI_HANDLE                FvHandle;
+  VOID                      *FileBuffer;
+  UINTN                     FileSize;
+  EFI_BOOT_LOGO_PROTOCOL    *BootLogo;
+  EFI_EVENT                 LegacyBootEvent;
+
+  if (BootOption == NULL) {
+    return;
+  }
+
+  if (BootOption->FilePath == NULL || BootOption->OptionType != LoadOptionTypeBoot) {
+    BootOption->Status = EFI_INVALID_PARAMETER;
+    return;
+  }
+
+  //
+  // 1. Create Boot#### for a temporary boot if there is no match Boot#### (i.e. a boot by selected a EFI Shell using "Boot From File")
+  //
+  OptionNumber = BmFindBootOptionInVariable (BootOption);
+  if (OptionNumber == LoadOptionNumberUnassigned) {
+    Status = BmGetFreeOptionNumber (LoadOptionTypeBoot, &Uint16);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Save the BootOption->OptionNumber to restore later
+      //
+      OptionNumber             = Uint16;
+      OriginalOptionNumber     = BootOption->OptionNumber;
+      BootOption->OptionNumber = OptionNumber;
+      Status = EfiBootManagerLoadOptionToVariable (BootOption);
+      BootOption->OptionNumber = OriginalOptionNumber;
+    }
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "[Bds] Failed to create Boot#### for a temporary boot - %r!\n", Status));
+      BootOption->Status = Status;
+      return ;
+    }
+  }
+
+  //
+  // 2. Set BootCurrent
+  //
+  Uint16 = (UINT16) OptionNumber;
+  BmSetVariableAndReportStatusCodeOnError (
+    L"BootCurrent",
+    &gEfiGlobalVariableGuid,
+    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+    sizeof (UINT16),
+    &Uint16
+    );
+
+  //
+  // 3. Signal the EVT_SIGNAL_READY_TO_BOOT event when we are about to load and execute
+  //    the boot option.
+  //
+  Node   = BootOption->FilePath;
+  Status = gBS->LocateDevicePath (&gEfiFirmwareVolume2ProtocolGuid, &Node, &FvHandle);
+  if (!EFI_ERROR (Status) && CompareGuid (
+        EfiGetNameGuidFromFwVolDevicePathNode ((CONST MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) Node),
+        PcdGetPtr (PcdBootManagerMenuFile)
+        )) {
+    DEBUG ((EFI_D_INFO, "[Bds] Booting Boot Manager Menu.\n"));
+    BmStopHotkeyService (NULL, NULL);
+  } else {
+    EfiSignalEventReadyToBoot();
+    //
+    // Report Status Code to indicate ReadyToBoot was signalled
+    //
+    REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_PC_READY_TO_BOOT_EVENT));
+    //
+    // 4. Repair system through DriverHealth protocol
+    //
+    BmRepairAllControllers ();
+  }
+
+  PERF_START_EX (gImageHandle, "BdsAttempt", NULL, 0, (UINT32) OptionNumber);
+
+  //
+  // 5. Load EFI boot option to ImageHandle
+  //
+  ImageHandle = NULL;
+  if (DevicePathType (BootOption->FilePath) != BBS_DEVICE_PATH) {
+    Status     = EFI_NOT_FOUND;
+    FileBuffer = BmGetLoadOptionBuffer (BootOption->FilePath, &FilePath, &FileSize);
+    DEBUG_CODE (
+      if (FileBuffer != NULL && CompareMem (BootOption->FilePath, FilePath, GetDevicePathSize (FilePath)) != 0) {
+        DEBUG ((EFI_D_INFO, "[Bds] DevicePath expand: "));
+        BmPrintDp (BootOption->FilePath);
+        DEBUG ((EFI_D_INFO, " -> "));
+        BmPrintDp (FilePath);
+        DEBUG ((EFI_D_INFO, "\n"));
+      }
+    );
+    if (BmIsLoadOptionPeHeaderValid (BootOption->OptionType, FileBuffer, FileSize)) {
+      REPORT_STATUS_CODE (EFI_PROGRESS_CODE, PcdGet32 (PcdProgressCodeOsLoaderLoad));
+      Status = gBS->LoadImage (
+                      TRUE,
+                      gImageHandle,
+                      FilePath,
+                      FileBuffer,
+                      FileSize,
+                      &ImageHandle
+                      );
+    }
+    if (FileBuffer != NULL) {
+      FreePool (FileBuffer);
+    }
+    if (FilePath != NULL) {
+      FreePool (FilePath);
+    }
+
+    if (EFI_ERROR (Status)) {
+      //
+      // Report Status Code to indicate that the failure to load boot option
+      //
+      REPORT_STATUS_CODE (
+        EFI_ERROR_CODE | EFI_ERROR_MINOR,
+        (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_EC_BOOT_OPTION_LOAD_ERROR)
+        );
+      BootOption->Status = Status;
+      return;
+    }
+  }
+
+  //
+  // 6. Adjust the different type memory page number just before booting
+  //    and save the updated info into the variable for next boot to use
+  //
+  BmSetMemoryTypeInformationVariable (
+    (BOOLEAN) ((BootOption->Attributes & LOAD_OPTION_CATEGORY) == LOAD_OPTION_CATEGORY_BOOT)
+    );
+
+  DEBUG_CODE_BEGIN();
+    if (BootOption->Description == NULL) {
+      DEBUG ((DEBUG_INFO | DEBUG_LOAD, "[Bds]Booting from unknown device path\n"));
+    } else {
+      DEBUG ((DEBUG_INFO | DEBUG_LOAD, "[Bds]Booting %s\n", BootOption->Description));
+    }
+  DEBUG_CODE_END();
+
+  //
+  // Check to see if we should legacy BOOT. If yes then do the legacy boot
+  // Write boot to OS performance data for Legacy boot
+  //
+  if ((DevicePathType (BootOption->FilePath) == BBS_DEVICE_PATH) && (DevicePathSubType (BootOption->FilePath) == BBS_BBS_DP)) {
+    if (mBmLegacyBoot != NULL) {
+      //
+      // Write boot to OS performance data for legacy boot.
+      //
+      PERF_CODE (
+        //
+        // Create an event to be signalled when Legacy Boot occurs to write performance data.
+        //
+        Status = EfiCreateEventLegacyBootEx(
+                   TPL_NOTIFY,
+                   BmWriteBootToOsPerformanceData,
+                   NULL, 
+                   &LegacyBootEvent
+                   );
+        ASSERT_EFI_ERROR (Status);
+      );
+
+      mBmLegacyBoot (BootOption);
+    } else {
+      BootOption->Status = EFI_UNSUPPORTED;
+    }
+
+    PERF_END_EX (gImageHandle, "BdsAttempt", NULL, 0, (UINT32) OptionNumber);
+    return;
+  }
+ 
+  //
+  // Provide the image with its load options
+  //
+  Status = gBS->HandleProtocol (ImageHandle, &gEfiLoadedImageProtocolGuid, (VOID **) &ImageInfo);
+  ASSERT_EFI_ERROR (Status);
+
+  if (!BmIsAutoCreateBootOption (BootOption)) {
+    ImageInfo->LoadOptionsSize = BootOption->OptionalDataSize;
+    ImageInfo->LoadOptions     = BootOption->OptionalData;
+  }
+
+  //
+  // Clean to NULL because the image is loaded directly from the firmwares boot manager.
+  //
+  ImageInfo->ParentHandle = NULL;
+
+  //
+  // Before calling the image, enable the Watchdog Timer for 5 minutes period
+  //
+  gBS->SetWatchdogTimer (5 * 60, 0x0000, 0x00, NULL);
+
+  //
+  // Write boot to OS performance data for UEFI boot
+  //
+  PERF_CODE (
+    BmWriteBootToOsPerformanceData (NULL, NULL);
+  );
+
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, PcdGet32 (PcdProgressCodeOsLoaderStart));
+
+  Status = gBS->StartImage (ImageHandle, &BootOption->ExitDataSize, &BootOption->ExitData);
+  DEBUG ((DEBUG_INFO | DEBUG_LOAD, "Image Return Status = %r\n", Status));
+  BootOption->Status = Status;
+  if (EFI_ERROR (Status)) {
+    //
+    // Report Status Code to indicate that boot failure
+    //
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_EC_BOOT_OPTION_FAILED)
+      );
+  }
+  PERF_END_EX (gImageHandle, "BdsAttempt", NULL, 0, (UINT32) OptionNumber);
+
+  //
+  // Clear the Watchdog Timer after the image returns
+  //
+  gBS->SetWatchdogTimer (0x0000, 0x0000, 0x0000, NULL);
+
+  //
+  // Set Logo status invalid after trying one boot option
+  //
+  BootLogo = NULL;
+  Status = gBS->LocateProtocol (&gEfiBootLogoProtocolGuid, NULL, (VOID **) &BootLogo);
+  if (!EFI_ERROR (Status) && (BootLogo != NULL)) {
+    Status = BootLogo->SetBootLogo (BootLogo, NULL, 0, 0, 0, 0);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Clear Boot Current
+  //
+  Status = gRT->SetVariable (
+                  L"BootCurrent",
+                  &gEfiGlobalVariableGuid,
+                  0,
+                  0,
+                  NULL
+                  );
+  //
+  // Deleting variable with current variable implementation shouldn't fail.
+  // When BootXXXX (e.g.: BootManagerMenu) boots BootYYYY, exiting BootYYYY causes BootCurrent deleted,
+  // exiting BootXXXX causes deleting BootCurrent returns EFI_NOT_FOUND.
+  //
+  ASSERT (Status == EFI_SUCCESS || Status == EFI_NOT_FOUND);
+}
+
+/**
+  Check whether there is a instance in BlockIoDevicePath, which contain multi device path
+  instances, has the same partition node with HardDriveDevicePath device path
+
+  @param  BlockIoDevicePath      Multi device path instances which need to check
+  @param  HardDriveDevicePath    A device path which starts with a hard drive media
+                                 device path.
+
+  @retval TRUE                   There is a matched device path instance.
+  @retval FALSE                  There is no matched device path instance.
+
+**/
+BOOLEAN
+BmMatchPartitionDevicePathNode (
+  IN  EFI_DEVICE_PATH_PROTOCOL   *BlockIoDevicePath,
+  IN  HARDDRIVE_DEVICE_PATH      *HardDriveDevicePath
+  )
+{
+  HARDDRIVE_DEVICE_PATH     *Node;
+
+  if ((BlockIoDevicePath == NULL) || (HardDriveDevicePath == NULL)) {
+    return FALSE;
+  }
+
+  //
+  // find the partition device path node
+  //
+  while (!IsDevicePathEnd (BlockIoDevicePath)) {
+    if ((DevicePathType (BlockIoDevicePath) == MEDIA_DEVICE_PATH) &&
+        (DevicePathSubType (BlockIoDevicePath) == MEDIA_HARDDRIVE_DP)
+        ) {
+      break;
+    }
+
+    BlockIoDevicePath = NextDevicePathNode (BlockIoDevicePath);
+  }
+
+  if (IsDevicePathEnd (BlockIoDevicePath)) {
+    return FALSE;
+  }
+
+  //
+  // See if the harddrive device path in blockio matches the orig Hard Drive Node
+  //
+  Node = (HARDDRIVE_DEVICE_PATH *) BlockIoDevicePath;
+
+  //
+  // Match Signature and PartitionNumber.
+  // Unused bytes in Signature are initiaized with zeros.
+  //
+  return (BOOLEAN) (
+    (Node->PartitionNumber == HardDriveDevicePath->PartitionNumber) &&
+    (Node->MBRType == HardDriveDevicePath->MBRType) &&
+    (Node->SignatureType == HardDriveDevicePath->SignatureType) &&
+    (CompareMem (Node->Signature, HardDriveDevicePath->Signature, sizeof (Node->Signature)) == 0)
+    );
+}
+
+/**
+  Enumerate all boot option descriptions and append " 2"/" 3"/... to make
+  unique description.
+
+  @param BootOptions            Array of boot options.
+  @param BootOptionCount        Count of boot options.
+**/
+VOID
+BmMakeBootOptionDescriptionUnique (
+  EFI_BOOT_MANAGER_LOAD_OPTION         *BootOptions,
+  UINTN                                BootOptionCount
+  )
+{
+  UINTN                                Base;
+  UINTN                                Index;
+  UINTN                                DescriptionSize;
+  UINTN                                MaxSuffixSize;
+  BOOLEAN                              *Visited;
+  UINTN                                MatchCount;
+
+  if (BootOptionCount == 0) {
+    return;
+  }
+
+  //
+  // Calculate the maximum buffer size for the number suffix.
+  // The initial sizeof (CHAR16) is for the blank space before the number.
+  //
+  MaxSuffixSize = sizeof (CHAR16);
+  for (Index = BootOptionCount; Index != 0; Index = Index / 10) {
+    MaxSuffixSize += sizeof (CHAR16);
+  }
+
+  Visited = AllocateZeroPool (sizeof (BOOLEAN) * BootOptionCount);
+  ASSERT (Visited != NULL);
+
+  for (Base = 0; Base < BootOptionCount; Base++) {
+    if (!Visited[Base]) {
+      MatchCount      = 1;
+      Visited[Base]   = TRUE;
+      DescriptionSize = StrSize (BootOptions[Base].Description);
+      for (Index = Base + 1; Index < BootOptionCount; Index++) {
+        if (!Visited[Index] && StrCmp (BootOptions[Base].Description, BootOptions[Index].Description) == 0) {
+          Visited[Index] = TRUE;
+          MatchCount++;
+          FreePool (BootOptions[Index].Description);
+          BootOptions[Index].Description = AllocatePool (DescriptionSize + MaxSuffixSize);
+          UnicodeSPrint (
+            BootOptions[Index].Description, DescriptionSize + MaxSuffixSize,
+            L"%s %d",
+            BootOptions[Base].Description, MatchCount
+            );
+        }
+      }
+    }
+  }
+
+  FreePool (Visited);
+}
+
+/**
+  Emuerate all possible bootable medias in the following order:
+  1. Removable BlockIo            - The boot option only points to the removable media
+                                    device, like USB key, DVD, Floppy etc.
+  2. Fixed BlockIo                - The boot option only points to a Fixed blockIo device,
+                                    like HardDisk.
+  3. Non-BlockIo SimpleFileSystem - The boot option points to a device supporting
+                                    SimpleFileSystem Protocol, but not supporting BlockIo
+                                    protocol.
+  4. LoadFile                     - The boot option points to the media supporting 
+                                    LoadFile protocol.
+  Reference: UEFI Spec chapter 3.3 Boot Option Variables Default Boot Behavior
+
+  @param BootOptionCount   Return the boot option count which has been found.
+
+  @retval   Pointer to the boot option array.
+**/
+EFI_BOOT_MANAGER_LOAD_OPTION *
+BmEnumerateBootOptions (
+  UINTN                                 *BootOptionCount
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION          *BootOptions;
+  UINTN                                 HandleCount;
+  EFI_HANDLE                            *Handles;
+  EFI_BLOCK_IO_PROTOCOL                 *BlkIo;
+  UINTN                                 Removable;
+  UINTN                                 Index;
+  CHAR16                                *Description;
+
+  ASSERT (BootOptionCount != NULL);
+
+  *BootOptionCount = 0;
+  BootOptions      = NULL;
+
+  //
+  // Parse removable block io followed by fixed block io
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiBlockIoProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+
+  for (Removable = 0; Removable < 2; Removable++) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      Status = gBS->HandleProtocol (
+                      Handles[Index],
+                      &gEfiBlockIoProtocolGuid,
+                      (VOID **) &BlkIo
+                      );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Skip the logical partitions
+      //
+      if (BlkIo->Media->LogicalPartition) {
+        continue;
+      }
+
+      //
+      // Skip the fixed block io then the removable block io
+      //
+      if (BlkIo->Media->RemovableMedia == ((Removable == 0) ? FALSE : TRUE)) {
+        continue;
+      }
+
+      Description = BmGetBootDescription (Handles[Index]);
+      BootOptions = ReallocatePool (
+                      sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                      sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                      BootOptions
+                      );
+      ASSERT (BootOptions != NULL);
+
+      Status = EfiBootManagerInitializeLoadOption (
+                 &BootOptions[(*BootOptionCount)++],
+                 LoadOptionNumberUnassigned,
+                 LoadOptionTypeBoot,
+                 LOAD_OPTION_ACTIVE,
+                 Description,
+                 DevicePathFromHandle (Handles[Index]),
+                 NULL,
+                 0
+                 );
+      ASSERT_EFI_ERROR (Status);
+
+      FreePool (Description);
+    }
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  //
+  // Parse simple file system not based on block io
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiSimpleFileSystemProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (
+                    Handles[Index],
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlkIo
+                    );
+     if (!EFI_ERROR (Status)) {
+      //
+      //  Skip if the file system handle supports a BlkIo protocol, which we've handled in above
+      //
+      continue;
+    }
+    Description = BmGetBootDescription (Handles[Index]);
+    BootOptions = ReallocatePool (
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                    BootOptions
+                    );
+    ASSERT (BootOptions != NULL);
+
+    Status = EfiBootManagerInitializeLoadOption (
+               &BootOptions[(*BootOptionCount)++],
+               LoadOptionNumberUnassigned,
+               LoadOptionTypeBoot,
+               LOAD_OPTION_ACTIVE,
+               Description,
+               DevicePathFromHandle (Handles[Index]),
+               NULL,
+               0
+               );
+    ASSERT_EFI_ERROR (Status);
+    FreePool (Description);
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  //
+  // Parse load file, assuming UEFI Network boot option
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiLoadFileProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+  for (Index = 0; Index < HandleCount; Index++) {
+
+    Description = BmGetBootDescription (Handles[Index]);
+    BootOptions = ReallocatePool (
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                    BootOptions
+                    );
+    ASSERT (BootOptions != NULL);
+
+    Status = EfiBootManagerInitializeLoadOption (
+               &BootOptions[(*BootOptionCount)++],
+               LoadOptionNumberUnassigned,
+               LoadOptionTypeBoot,
+               LOAD_OPTION_ACTIVE,
+               Description,
+               DevicePathFromHandle (Handles[Index]),
+               NULL,
+               0
+               );
+    ASSERT_EFI_ERROR (Status);
+    FreePool (Description);
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  BmMakeBootOptionDescriptionUnique (BootOptions, *BootOptionCount);
+  return BootOptions;
+}
+
+/**
+  The function enumerates all boot options, creates them and registers them in the BootOrder variable.
+**/
+VOID
+EFIAPI
+EfiBootManagerRefreshAllBootOption (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION  *NvBootOptions;
+  UINTN                         NvBootOptionCount;
+  EFI_BOOT_MANAGER_LOAD_OPTION  *BootOptions;
+  UINTN                         BootOptionCount;
+  UINTN                         Index;
+
+  //
+  // Optionally refresh the legacy boot option
+  //
+  if (mBmRefreshLegacyBootOption != NULL) {
+    mBmRefreshLegacyBootOption ();
+  }
+
+  BootOptions   = BmEnumerateBootOptions (&BootOptionCount);
+  NvBootOptions = EfiBootManagerGetLoadOptions (&NvBootOptionCount, LoadOptionTypeBoot);
+
+  //
+  // Mark the boot option as added by BDS by setting OptionalData to a special GUID
+  //
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    BootOptions[Index].OptionalData     = AllocateCopyPool (sizeof (EFI_GUID), &mBmAutoCreateBootOptionGuid);
+    BootOptions[Index].OptionalDataSize = sizeof (EFI_GUID);
+  }
+
+  //
+  // Remove invalid EFI boot options from NV
+  //
+  for (Index = 0; Index < NvBootOptionCount; Index++) {
+    if (((DevicePathType (NvBootOptions[Index].FilePath) != BBS_DEVICE_PATH) || 
+         (DevicePathSubType (NvBootOptions[Index].FilePath) != BBS_BBS_DP)
+        ) && BmIsAutoCreateBootOption (&NvBootOptions[Index])
+       ) {
+      //
+      // Only check those added by BDS
+      // so that the boot options added by end-user or OS installer won't be deleted
+      //
+      if (BmFindLoadOption (&NvBootOptions[Index], BootOptions, BootOptionCount) == (UINTN) -1) {
+        Status = EfiBootManagerDeleteLoadOptionVariable (NvBootOptions[Index].OptionNumber, LoadOptionTypeBoot);
+        //
+        // Deleting variable with current variable implementation shouldn't fail.
+        //
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+  }
+
+  //
+  // Add new EFI boot options to NV
+  //
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    if (BmFindLoadOption (&BootOptions[Index], NvBootOptions, NvBootOptionCount) == (UINTN) -1) {
+      EfiBootManagerAddLoadOptionVariable (&BootOptions[Index], (UINTN) -1);
+      //
+      // Try best to add the boot options so continue upon failure.
+      //
+    }
+  }
+
+  EfiBootManagerFreeLoadOptions (BootOptions,   BootOptionCount);
+  EfiBootManagerFreeLoadOptions (NvBootOptions, NvBootOptionCount);
+}
+
+/**
+  This function is called to create the boot option for the Boot Manager Menu.
+
+  The Boot Manager Menu is shown after successfully booting a boot option.
+  Assume the BootManagerMenuFile is in the same FV as the module links to this library.
+
+  @param  BootOption    Return the boot option of the Boot Manager Menu
+
+  @retval EFI_SUCCESS   Successfully register the Boot Manager Menu.
+  @retval Status        Return status of gRT->SetVariable (). BootOption still points
+                        to the Boot Manager Menu even the Status is not EFI_SUCCESS.
+**/
+EFI_STATUS
+BmRegisterBootManagerMenu (
+  OUT EFI_BOOT_MANAGER_LOAD_OPTION   *BootOption
+  )
+{
+  EFI_STATUS                         Status;
+  CHAR16                             *Description;
+  UINTN                              DescriptionLength;
+  EFI_DEVICE_PATH_PROTOCOL           *DevicePath;
+  EFI_LOADED_IMAGE_PROTOCOL          *LoadedImage;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH  FileNode;
+
+  Status = GetSectionFromFv (
+             PcdGetPtr (PcdBootManagerMenuFile),
+             EFI_SECTION_USER_INTERFACE,
+             0,
+             (VOID **) &Description,
+             &DescriptionLength
+             );
+  if (EFI_ERROR (Status)) {
+    Description = NULL;
+  }
+
+  EfiInitializeFwVolDevicepathNode (&FileNode, PcdGetPtr (PcdBootManagerMenuFile));
+  Status = gBS->HandleProtocol (
+                  gImageHandle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID **) &LoadedImage
+                  );
+  ASSERT_EFI_ERROR (Status);
+  DevicePath = AppendDevicePathNode (
+                 DevicePathFromHandle (LoadedImage->DeviceHandle),
+                 (EFI_DEVICE_PATH_PROTOCOL *) &FileNode
+                 );
+  ASSERT (DevicePath != NULL);
+
+  Status = EfiBootManagerInitializeLoadOption (
+             BootOption,
+             LoadOptionNumberUnassigned,
+             LoadOptionTypeBoot,
+             LOAD_OPTION_CATEGORY_APP | LOAD_OPTION_ACTIVE | LOAD_OPTION_HIDDEN,
+             (Description != NULL) ? Description : L"Boot Manager Menu",
+             DevicePath,
+             NULL,
+             0
+             );
+  ASSERT_EFI_ERROR (Status);
+  FreePool (DevicePath);
+  if (Description != NULL) {
+    FreePool (Description);
+  }
+
+  DEBUG_CODE (
+    EFI_BOOT_MANAGER_LOAD_OPTION    *BootOptions;
+    UINTN                           BootOptionCount;
+
+    BootOptions = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+    ASSERT (BmFindLoadOption (BootOption, BootOptions, BootOptionCount) == -1);
+    EfiBootManagerFreeLoadOptions (BootOptions, BootOptionCount);
+    );
+
+  return EfiBootManagerAddLoadOptionVariable (BootOption, 0);
+}
+
+/**
+  Return the boot option corresponding to the Boot Manager Menu.
+  It may automatically create one if the boot option hasn't been created yet.
+  
+  @param BootOption    Return the Boot Manager Menu.
+
+  @retval EFI_SUCCESS   The Boot Manager Menu is successfully returned.
+  @retval Status        Return status of gRT->SetVariable (). BootOption still points
+                        to the Boot Manager Menu even the Status is not EFI_SUCCESS.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerGetBootManagerMenu (
+  EFI_BOOT_MANAGER_LOAD_OPTION *BootOption
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        BootOptionCount;
+  EFI_BOOT_MANAGER_LOAD_OPTION *BootOptions;
+  UINTN                        Index;
+  EFI_DEVICE_PATH_PROTOCOL     *Node;
+  EFI_HANDLE                   FvHandle;
+  
+  BootOptions = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    Node   = BootOptions[Index].FilePath;
+    Status = gBS->LocateDevicePath (&gEfiFirmwareVolume2ProtocolGuid, &Node, &FvHandle);
+    if (!EFI_ERROR (Status)) {
+      if (CompareGuid (
+            EfiGetNameGuidFromFwVolDevicePathNode ((CONST MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) Node),
+            PcdGetPtr (PcdBootManagerMenuFile)
+            )
+          ) {        
+        Status = EfiBootManagerInitializeLoadOption (
+                   BootOption,
+                   BootOptions[Index].OptionNumber,
+                   BootOptions[Index].OptionType,
+                   BootOptions[Index].Attributes,
+                   BootOptions[Index].Description,
+                   BootOptions[Index].FilePath,
+                   BootOptions[Index].OptionalData,
+                   BootOptions[Index].OptionalDataSize
+                   );
+        ASSERT_EFI_ERROR (Status);
+        break;
+      }
+    }
+  }
+
+  EfiBootManagerFreeLoadOptions (BootOptions, BootOptionCount);
+
+  //
+  // Automatically create the Boot#### for Boot Manager Menu when not found.
+  //
+  if (Index == BootOptionCount) {
+    return BmRegisterBootManagerMenu (BootOption);
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConnect.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConnect.c
new file mode 100644
index 0000000000..b1c94ad9d9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConnect.c
@@ -0,0 +1,321 @@
+/** @file
+  Library functions which relate with connecting the device.
+
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+/**
+  Connect all the drivers to all the controllers.
+
+  This function makes sure all the current system drivers manage the correspoinding
+  controllers if have. And at the same time, makes sure all the system controllers
+  have driver to manage it if have.
+**/
+VOID
+BmConnectAllDriversToAllControllers (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       HandleCount;
+  EFI_HANDLE  *HandleBuffer;
+  UINTN       Index;
+
+  do {
+    //
+    // Connect All EFI 1.10 drivers following EFI 1.10 algorithm
+    //
+    gBS->LocateHandleBuffer (
+           AllHandles,
+           NULL,
+           NULL,
+           &HandleCount,
+           &HandleBuffer
+           );
+
+    for (Index = 0; Index < HandleCount; Index++) {
+      gBS->ConnectController (HandleBuffer[Index], NULL, NULL, TRUE);
+    }
+
+    if (HandleBuffer != NULL) {
+      FreePool (HandleBuffer);
+    }
+
+    //
+    // Check to see if it's possible to dispatch an more DXE drivers.
+    // The above code may have made new DXE drivers show up.
+    // If any new driver is dispatched (Status == EFI_SUCCESS) and we will try
+    // the connect again.
+    //
+    Status = gDS->Dispatch ();
+
+  } while (!EFI_ERROR (Status));
+}
+
+/**
+  This function will connect all the system driver to controller
+  first, and then special connect the default console, this make
+  sure all the system controller available and the platform default
+  console connected.
+
+**/
+VOID
+EFIAPI
+EfiBootManagerConnectAll (
+  VOID
+  )
+{
+  //
+  // Connect the platform console first
+  //
+  EfiBootManagerConnectAllDefaultConsoles ();
+
+  //
+  // Generic way to connect all the drivers
+  //
+  BmConnectAllDriversToAllControllers ();
+
+  //
+  // Here we have the assumption that we have already had
+  // platform default console
+  //
+  EfiBootManagerConnectAllDefaultConsoles ();
+}
+
+/**
+  This function will create all handles associate with every device
+  path node. If the handle associate with one device path node can not
+  be created successfully, then still give chance to do the dispatch,
+  which load the missing drivers if possible.
+
+  @param  DevicePathToConnect   The device path which will be connected, it can be
+                                a multi-instance device path
+  @param  MatchingHandle        Return the controller handle closest to the DevicePathToConnect
+
+  @retval EFI_SUCCESS            All handles associate with every device path node
+                                 have been created.
+  @retval EFI_OUT_OF_RESOURCES   There is no resource to create new handles.
+  @retval EFI_NOT_FOUND          Create the handle associate with one device path
+                                 node failed.
+  @retval EFI_SECURITY_VIOLATION The user has no permission to start UEFI device 
+                                 drivers on the DevicePath.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectDevicePath (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *DevicePathToConnect,
+  OUT EFI_HANDLE                *MatchingHandle          OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath;
+  EFI_HANDLE                Handle;
+  EFI_HANDLE                PreviousHandle;
+  EFI_TPL                   CurrentTpl;
+
+  if (DevicePathToConnect == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CurrentTpl = EfiGetCurrentTpl ();
+  //
+  // Start the real work of connect with RemainingDevicePath
+  //
+  PreviousHandle = NULL;
+  do {
+    //
+    // Find the handle that best matches the Device Path. If it is only a
+    // partial match the remaining part of the device path is returned in
+    // RemainingDevicePath.
+    //
+    RemainingDevicePath = DevicePathToConnect;
+    Status              = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &Handle);
+    if (!EFI_ERROR (Status)) {
+      if (Handle == PreviousHandle) {
+        //
+        // If no forward progress is made try invoking the Dispatcher.
+        // A new FV may have been added to the system an new drivers
+        // may now be found.
+        // Status == EFI_SUCCESS means a driver was dispatched
+        // Status == EFI_NOT_FOUND means no new drivers were dispatched
+        //
+        if (CurrentTpl == TPL_APPLICATION) {
+          Status = gDS->Dispatch ();
+        } else {
+          //
+          // Always return EFI_NOT_FOUND here
+          // to prevent dead loop when control handle is found but connection failded case
+          //
+          Status = EFI_NOT_FOUND;
+        }
+      }
+
+
+      if (!EFI_ERROR (Status)) {
+        PreviousHandle = Handle;
+        //
+        // Connect all drivers that apply to Handle and RemainingDevicePath,
+        // the Recursive flag is FALSE so only one level will be expanded.
+        //
+        // If ConnectController fails to find a driver, then still give the chance to 
+        // do dispatch, because partial RemainingDevicePath may be in the new FV
+        //
+        // 1. If the connect fail, RemainingDevicepath and handle will not
+        //    change, so next time will do the dispatch, then dispatch's status
+        //    will take effect
+        // 2. If the connect success, the RemainingDevicepath and handle will
+        //    change, then avoid the dispatch, we have chance to continue the
+        //    next connection
+        //
+        Status = gBS->ConnectController (Handle, NULL, RemainingDevicePath, FALSE);
+        if (Status == EFI_NOT_FOUND) {
+          Status = EFI_SUCCESS;
+        }
+        if (MatchingHandle != NULL) {
+          *MatchingHandle = Handle;
+        }
+      }
+    }
+    //
+    // Loop until RemainingDevicePath is an empty device path
+    //
+  } while (!EFI_ERROR (Status) && !IsDevicePathEnd (RemainingDevicePath));
+
+  ASSERT (EFI_ERROR (Status) || IsDevicePathEnd (RemainingDevicePath));
+
+  return Status;
+}
+
+/**
+  This function will disconnect all current system handles. 
+  
+  gBS->DisconnectController() is invoked for each handle exists in system handle buffer.
+  If handle is a bus type handle, all childrens also are disconnected recursively by
+  gBS->DisconnectController().
+**/
+VOID
+EFIAPI
+EfiBootManagerDisconnectAll (
+  VOID
+  )
+{
+  UINTN       HandleCount;
+  EFI_HANDLE  *HandleBuffer;
+  UINTN       Index;
+
+  //
+  // Disconnect all
+  //
+  gBS->LocateHandleBuffer (
+         AllHandles,
+         NULL,
+         NULL,
+         &HandleCount,
+         &HandleBuffer
+         );
+  for (Index = 0; Index < HandleCount; Index++) {
+    gBS->DisconnectController (HandleBuffer[Index], NULL, NULL);
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool (HandleBuffer);
+  }
+}
+
+/**
+  Connect the specific Usb device which match the short form device path,
+  and whose bus is determined by Host Controller (Uhci or Ehci).
+
+  @param  DevicePath             A short-form device path that starts with the first
+                                 element being a USB WWID or a USB Class device
+                                 path
+
+  @return EFI_INVALID_PARAMETER  DevicePath is NULL pointer.
+                                 DevicePath is not a USB device path.
+
+  @return EFI_SUCCESS            Success to connect USB device
+  @return EFI_NOT_FOUND          Fail to find handle for USB controller to connect.
+
+**/
+EFI_STATUS
+BmConnectUsbShortFormDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL   *DevicePath
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_HANDLE                            *Handles;
+  UINTN                                 HandleCount;
+  UINTN                                 Index;
+  EFI_PCI_IO_PROTOCOL                   *PciIo;
+  UINT8                                 Class[3];
+  BOOLEAN                               AtLeastOneConnected;
+
+  //
+  // Check the passed in parameters
+  //
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DevicePathType (DevicePath) != MESSAGING_DEVICE_PATH) ||
+      ((DevicePathSubType (DevicePath) != MSG_USB_CLASS_DP) && (DevicePathSubType (DevicePath) != MSG_USB_WWID_DP))
+     ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find the usb host controller firstly, then connect with the remaining device path
+  //
+  AtLeastOneConnected = FALSE;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiPciIoProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &Handles
+                  );
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      Status = gBS->HandleProtocol (
+                      Handles[Index],
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo
+                      );
+      if (!EFI_ERROR (Status)) {
+        //
+        // Check whether the Pci device is the wanted usb host controller
+        //
+        Status = PciIo->Pci.Read (PciIo, EfiPciIoWidthUint8, 0x09, 3, &Class);
+        if (!EFI_ERROR (Status) &&
+            ((PCI_CLASS_SERIAL == Class[2]) && (PCI_CLASS_SERIAL_USB == Class[1]))
+           ) {
+          Status = gBS->ConnectController (
+                          Handles[Index],
+                          NULL,
+                          DevicePath,
+                          FALSE
+                          );
+          if (!EFI_ERROR(Status)) {
+            AtLeastOneConnected = TRUE;
+          }
+        }
+      }
+    }
+
+    if (Handles != NULL) {
+      FreePool (Handles);
+    }
+  }
+
+  return AtLeastOneConnected ? EFI_SUCCESS : EFI_NOT_FOUND;
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConsole.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConsole.c
new file mode 100644
index 0000000000..86b4fac424
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmConsole.c
@@ -0,0 +1,764 @@
+/** @file
+  Library functions which contain all the code to connect console device.
+
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+CHAR16       *mConVarName[] = {
+  L"ConIn",
+  L"ConOut",
+  L"ErrOut",
+  L"ConInDev",
+  L"ConOutDev",
+  L"ErrOutDev"
+};
+
+/**
+  Search out the video controller.
+
+  @return  PCI device path of the video controller.
+**/
+EFI_HANDLE
+BmGetVideoController (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  UINTN                     RootBridgeHandleCount;
+  EFI_HANDLE                *RootBridgeHandleBuffer;
+  UINTN                     HandleCount;
+  EFI_HANDLE                *HandleBuffer;
+  UINTN                     RootBridgeIndex;
+  UINTN                     Index;
+  EFI_HANDLE                VideoController;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  PCI_TYPE00                Pci;
+
+  //
+  // Make all the PCI_IO protocols show up
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiPciRootBridgeIoProtocolGuid,
+                  NULL,
+                  &RootBridgeHandleCount,
+                  &RootBridgeHandleBuffer
+                  );
+  if (EFI_ERROR (Status) || (RootBridgeHandleCount == 0)) {
+    return NULL;
+  }
+
+  VideoController = NULL;
+  for (RootBridgeIndex = 0; RootBridgeIndex < RootBridgeHandleCount; RootBridgeIndex++) {
+    gBS->ConnectController (RootBridgeHandleBuffer[RootBridgeIndex], NULL, NULL, FALSE);
+
+    //
+    // Start to check all the pci io to find the first video controller
+    //
+    Status = gBS->LocateHandleBuffer (
+                    ByProtocol,
+                    &gEfiPciIoProtocolGuid,
+                    NULL,
+                    &HandleCount,
+                    &HandleBuffer
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    for (Index = 0; Index < HandleCount; Index++) {
+      Status = gBS->HandleProtocol (HandleBuffer[Index], &gEfiPciIoProtocolGuid, (VOID **) &PciIo);
+      if (!EFI_ERROR (Status)) {
+        //
+        // Check for all video controller
+        //
+        Status = PciIo->Pci.Read (
+                          PciIo,
+                          EfiPciIoWidthUint32,
+                          0,
+                          sizeof (Pci) / sizeof (UINT32),
+                          &Pci
+                          );
+        if (!EFI_ERROR (Status) && IS_PCI_VGA (&Pci)) {
+          // TODO: use IS_PCI_DISPLAY??
+          VideoController = HandleBuffer[Index];
+          break;
+        }
+      }
+    }
+    FreePool (HandleBuffer);
+
+    if (VideoController != NULL) {
+      break;
+    }
+  }
+  FreePool (RootBridgeHandleBuffer);
+  
+  return VideoController;
+}
+
+/**
+  Query all the children of VideoController and return the device paths of all the 
+  children that support GraphicsOutput protocol.
+
+  @param VideoController       PCI handle of video controller.
+
+  @return  Device paths of all the children that support GraphicsOutput protocol.
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+EFIAPI
+EfiBootManagerGetGopDevicePath (
+  IN  EFI_HANDLE                       VideoController
+  )
+{
+  UINTN                                Index;
+  EFI_STATUS                           Status;
+  EFI_GUID                             **ProtocolBuffer;
+  UINTN                                ProtocolBufferCount;
+  UINTN                                ProtocolIndex;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY  *OpenInfoBuffer;
+  UINTN                                EntryCount;
+  EFI_DEVICE_PATH_PROTOCOL             *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL             *Next;
+  EFI_DEVICE_PATH_PROTOCOL             *Previous;
+  EFI_DEVICE_PATH_PROTOCOL             *TempDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL             *GopPool;
+  EFI_DEVICE_PATH_PROTOCOL             *ReturnDevicePath;
+
+
+  Status = gBS->ProtocolsPerHandle (
+                  VideoController,
+                  &ProtocolBuffer,
+                  &ProtocolBufferCount
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  GopPool = NULL;
+
+  for (ProtocolIndex = 0; ProtocolIndex < ProtocolBufferCount; ProtocolIndex++) {
+    Status = gBS->OpenProtocolInformation (
+                    VideoController,
+                    ProtocolBuffer[ProtocolIndex],
+                    &OpenInfoBuffer,
+                    &EntryCount
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    for (Index = 0; Index < EntryCount; Index++) {
+      //
+      // Query all the children
+      //
+      if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+        Status = gBS->OpenProtocol (
+                        OpenInfoBuffer[Index].ControllerHandle,
+                        &gEfiDevicePathProtocolGuid,
+                        (VOID **) &DevicePath,
+                        NULL,
+                        NULL,
+                        EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                        );
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+
+        Previous = NULL;
+        for (Next = DevicePath; !IsDevicePathEnd (Next); Next = NextDevicePathNode (Next)) {
+          Previous = Next;
+        }
+        ASSERT (Previous != NULL);
+
+        if (DevicePathType (Previous) == ACPI_DEVICE_PATH && DevicePathSubType (Previous) == ACPI_ADR_DP) {
+          Status = gBS->OpenProtocol (
+                          OpenInfoBuffer[Index].ControllerHandle,
+                          &gEfiGraphicsOutputProtocolGuid,
+                          NULL,
+                          NULL,
+                          NULL,
+                          EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                          );
+          if (!EFI_ERROR (Status)) {
+            //
+            // Append the device path to GOP pool when there is GOP protocol installed.
+            //
+            TempDevicePath = GopPool;
+            GopPool = AppendDevicePathInstance (GopPool, DevicePath);
+            gBS->FreePool (TempDevicePath);
+          }
+        }
+
+        if (DevicePathType (Previous) == HARDWARE_DEVICE_PATH && DevicePathSubType (Previous) == HW_CONTROLLER_DP) {
+          //
+          // Recursively look for GOP child in this frame buffer handle
+          //
+          DEBUG ((EFI_D_INFO, "[Bds] Looking for GOP child deeper ... \n"));
+          TempDevicePath = GopPool;
+          ReturnDevicePath = EfiBootManagerGetGopDevicePath (OpenInfoBuffer[Index].ControllerHandle);
+          GopPool = AppendDevicePathInstance (GopPool, ReturnDevicePath);
+          gBS->FreePool (ReturnDevicePath);
+          gBS->FreePool (TempDevicePath);
+        }
+      }
+    }
+
+    FreePool (OpenInfoBuffer);
+  }
+
+  FreePool (ProtocolBuffer);
+
+  return GopPool;
+}
+
+/**
+  Connect the platform active active video controller.
+
+  @param VideoController       PCI handle of video controller.
+
+  @retval EFI_NOT_FOUND There is no active video controller.
+  @retval EFI_SUCCESS   The video controller is connected.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectVideoController (
+  EFI_HANDLE                 VideoController  OPTIONAL
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL   *Gop;
+  
+  if (VideoController == NULL) {
+    //
+    // Get the platform vga device
+    //
+    VideoController = BmGetVideoController ();
+  }
+ 
+  if (VideoController == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Try to connect the PCI device path, so that GOP dirver could start on this 
+  // device and create child handles with GraphicsOutput Protocol installed
+  // on them, then we get device paths of these child handles and select 
+  // them as possible console device.
+  //
+  gBS->ConnectController (VideoController, NULL, NULL, FALSE);
+
+  Gop = EfiBootManagerGetGopDevicePath (VideoController);
+  if (Gop == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  EfiBootManagerUpdateConsoleVariable (ConOut, Gop, NULL);
+  FreePool (Gop);
+
+  //
+  // Necessary for ConPlatform and ConSplitter driver to start up again after ConOut is updated.
+  //
+  return gBS->ConnectController (VideoController, NULL, NULL, TRUE);
+}
+
+/**
+  Fill console handle in System Table if there are no valid console handle in.
+
+  Firstly, check the validation of console handle in System Table. If it is invalid,
+  update it by the first console device handle from EFI console variable. 
+
+  @param  VarName            The name of the EFI console variable.
+  @param  ConsoleGuid        Specified Console protocol GUID.
+  @param  ConsoleHandle      On IN,  console handle in System Table to be checked. 
+                             On OUT, new console handle in system table.
+  @param  ProtocolInterface  On IN,  console protocol on console handle in System Table to be checked. 
+                             On OUT, new console protocol on new console handle in system table.
+
+  @retval TRUE               System Table has been updated.
+  @retval FALSE              System Table hasn't been updated.
+
+**/
+BOOLEAN 
+BmUpdateSystemTableConsole (
+  IN     CHAR16                   *VarName,
+  IN     EFI_GUID                 *ConsoleGuid,
+  IN OUT EFI_HANDLE               *ConsoleHandle,
+  IN OUT VOID                     **ProtocolInterface
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           DevicePathSize;
+  EFI_DEVICE_PATH_PROTOCOL        *FullDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL        *VarConsole;
+  EFI_DEVICE_PATH_PROTOCOL        *Instance;
+  VOID                            *Interface;
+  EFI_HANDLE                      NewHandle;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *TextOut;
+
+  ASSERT (VarName != NULL);
+  ASSERT (ConsoleHandle != NULL);
+  ASSERT (ConsoleGuid != NULL);
+  ASSERT (ProtocolInterface != NULL);
+
+  if (*ConsoleHandle != NULL) {
+    Status = gBS->HandleProtocol (
+                   *ConsoleHandle,
+                   ConsoleGuid,
+                   &Interface
+                   );
+    if (Status == EFI_SUCCESS && Interface == *ProtocolInterface) {
+      //
+      // If ConsoleHandle is valid and console protocol on this handle also
+      // also matched, just return.
+      //
+      return FALSE;
+    }
+  }
+  
+  //
+  // Get all possible consoles device path from EFI variable
+  //
+  GetEfiGlobalVariable2 (VarName, (VOID **) &VarConsole, NULL);
+  if (VarConsole == NULL) {
+    //
+    // If there is no any console device, just return.
+    //
+    return FALSE;
+  }
+
+  FullDevicePath = VarConsole;
+
+  do {
+    //
+    // Check every instance of the console variable
+    //
+    Instance  = GetNextDevicePathInstance (&VarConsole, &DevicePathSize);
+    if (Instance == NULL) {
+      DEBUG ((EFI_D_ERROR, "[Bds] No valid console instance is found for %s!\n", VarName));
+      // We should not ASSERT when all the console devices are removed.
+      // ASSERT_EFI_ERROR (EFI_NOT_FOUND);
+      FreePool (FullDevicePath);
+      return FALSE;
+    }
+    
+    //
+    // Find console device handle by device path instance
+    //
+    Status = gBS->LocateDevicePath (
+                    ConsoleGuid,
+                    &Instance,
+                    &NewHandle
+                    );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Get the console protocol on this console device handle
+      //
+      Status = gBS->HandleProtocol (
+                      NewHandle,
+                      ConsoleGuid,
+                      &Interface
+                      );
+      if (!EFI_ERROR (Status)) {
+        //
+        // Update new console handle in System Table.
+        //
+        *ConsoleHandle     = NewHandle;
+        *ProtocolInterface = Interface;
+        if (CompareGuid (ConsoleGuid, &gEfiSimpleTextOutProtocolGuid)) {
+          //
+          // If it is console out device, set console mode 80x25 if current mode is invalid.
+          //
+          TextOut = (EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL *) Interface;
+          if (TextOut->Mode->Mode == -1) {
+            TextOut->SetMode (TextOut, 0);
+          }
+        }
+        return TRUE;
+      }
+    }
+
+  } while (Instance != NULL);
+
+  //
+  // No any available console devcie found.
+  //
+  return FALSE;
+}
+
+/**
+  This function updates the console variable based on ConVarName. It can
+  add or remove one specific console device path from the variable
+
+  @param  ConsoleType              ConIn, ConOut, ErrOut, ConInDev, ConOutDev or ErrOutDev.
+  @param  CustomizedConDevicePath  The console device path to be added to
+                                   the console variable. Cannot be multi-instance.
+  @param  ExclusiveDevicePath      The console device path to be removed
+                                   from the console variable. Cannot be multi-instance.
+
+  @retval EFI_UNSUPPORTED          The added device path is the same as a removed one.
+  @retval EFI_SUCCESS              Successfully added or removed the device path from the
+                                   console variable.
+  @retval others                   Return status of RT->SetVariable().
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerUpdateConsoleVariable (
+  IN  CONSOLE_TYPE              ConsoleType,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *CustomizedConDevicePath,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *ExclusiveDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *VarConsole;
+  EFI_DEVICE_PATH_PROTOCOL  *NewDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *TempNewDevicePath;
+
+  if (ConsoleType >= sizeof (mConVarName) / sizeof (mConVarName[0])) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Notes: check the device path point, here should check
+  // with compare memory
+  //
+  if (CustomizedConDevicePath == ExclusiveDevicePath) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Delete the ExclusiveDevicePath from current default console
+  //
+  GetEfiGlobalVariable2 (mConVarName[ConsoleType], (VOID **) &VarConsole, NULL);
+  //
+  // Initialize NewDevicePath
+  //
+  NewDevicePath = VarConsole;
+
+  //
+  // If ExclusiveDevicePath is even the part of the instance in VarConsole, delete it.
+  // In the end, NewDevicePath is the final device path.
+  //
+  if (ExclusiveDevicePath != NULL && VarConsole != NULL) {
+      NewDevicePath = BmDelPartMatchInstance (VarConsole, ExclusiveDevicePath);
+  }
+  //
+  // Try to append customized device path to NewDevicePath.
+  //
+  if (CustomizedConDevicePath != NULL) {
+    if (!BmMatchDevicePaths (NewDevicePath, CustomizedConDevicePath)) {
+      //
+      // Check if there is part of CustomizedConDevicePath in NewDevicePath, delete it.
+      //
+      NewDevicePath = BmDelPartMatchInstance (NewDevicePath, CustomizedConDevicePath);
+      //
+      // In the first check, the default console variable will be _ModuleEntryPoint,
+      // just append current customized device path
+      //
+      TempNewDevicePath = NewDevicePath;
+      NewDevicePath = AppendDevicePathInstance (NewDevicePath, CustomizedConDevicePath);
+      if (TempNewDevicePath != NULL) {
+        FreePool(TempNewDevicePath);
+      }
+    }
+  }
+
+  //
+  // Finally, Update the variable of the default console by NewDevicePath
+  //
+  Status = gRT->SetVariable (
+                  mConVarName[ConsoleType],
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS
+                                                  | ((ConsoleType < ConInDev) ? EFI_VARIABLE_NON_VOLATILE : 0),
+                  GetDevicePathSize (NewDevicePath),
+                  NewDevicePath
+                  );
+
+  if (VarConsole == NewDevicePath) {
+    if (VarConsole != NULL) {
+      FreePool(VarConsole);
+    }
+  } else {
+    if (VarConsole != NULL) {
+      FreePool(VarConsole);
+    }
+    if (NewDevicePath != NULL) {
+      FreePool(NewDevicePath);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Connect the console device base on the variable ConsoleType.
+
+  @param  ConsoleType              ConIn, ConOut or ErrOut.
+
+  @retval EFI_NOT_FOUND            There is not any console devices connected
+                                   success
+  @retval EFI_SUCCESS              Success connect any one instance of the console
+                                   device path base on the variable ConVarName.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectConsoleVariable (
+  IN  CONSOLE_TYPE              ConsoleType
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *StartDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *Instance;
+  EFI_DEVICE_PATH_PROTOCOL  *Next;
+  EFI_DEVICE_PATH_PROTOCOL  *CopyOfDevicePath;
+  UINTN                     Size;
+  BOOLEAN                   DeviceExist;
+  EFI_HANDLE                Handle;
+
+  if ((ConsoleType != ConIn) && (ConsoleType != ConOut) && (ConsoleType != ErrOut)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status      = EFI_SUCCESS;
+  DeviceExist = FALSE;
+  Handle      = NULL;
+
+  //
+  // Check if the console variable exist
+  //
+  GetEfiGlobalVariable2 (mConVarName[ConsoleType], (VOID **) &StartDevicePath, NULL);
+  if (StartDevicePath == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  CopyOfDevicePath = StartDevicePath;
+  do {
+    //
+    // Check every instance of the console variable
+    //
+    Instance  = GetNextDevicePathInstance (&CopyOfDevicePath, &Size);
+    if (Instance == NULL) {
+      FreePool (StartDevicePath);
+      return EFI_UNSUPPORTED;
+    }
+    
+    Next      = Instance;
+    while (!IsDevicePathEndType (Next)) {
+      Next = NextDevicePathNode (Next);
+    }
+
+    SetDevicePathEndNode (Next);
+    //
+    // Connect the USB console
+    // USB console device path is a short-form device path that 
+    //  starts with the first element being a USB WWID
+    //  or a USB Class device path
+    //
+    if ((DevicePathType (Instance) == MESSAGING_DEVICE_PATH) &&
+        ((DevicePathSubType (Instance) == MSG_USB_CLASS_DP) || (DevicePathSubType (Instance) == MSG_USB_WWID_DP))
+       ) {
+      Status = BmConnectUsbShortFormDevicePath (Instance);
+      if (!EFI_ERROR (Status)) {
+        DeviceExist = TRUE;
+      }
+    } else {
+      for (Next = Instance; !IsDevicePathEnd (Next); Next = NextDevicePathNode (Next)) {
+        if (DevicePathType (Next) == ACPI_DEVICE_PATH && DevicePathSubType (Next) == ACPI_ADR_DP) {
+          break;
+        } else if (DevicePathType (Next) == HARDWARE_DEVICE_PATH && 
+                   DevicePathSubType (Next) == HW_CONTROLLER_DP &&
+                   DevicePathType (NextDevicePathNode (Next)) == ACPI_DEVICE_PATH &&
+                   DevicePathSubType (NextDevicePathNode (Next)) == ACPI_ADR_DP
+                   ) {
+          break;
+        }
+      }
+      if (!IsDevicePathEnd (Next)) {
+        //
+        // For GOP device path, start the video driver with NULL remaining device path
+        //
+        SetDevicePathEndNode (Next);
+        Status = EfiBootManagerConnectDevicePath (Instance, &Handle);
+        if (!EFI_ERROR (Status)) {
+          gBS->ConnectController (Handle, NULL, NULL, TRUE);
+        }
+      } else {
+        Status = EfiBootManagerConnectDevicePath (Instance, NULL);
+      }
+      if (EFI_ERROR (Status)) {
+        //
+        // Delete the instance from the console varialbe
+        //
+        EfiBootManagerUpdateConsoleVariable (ConsoleType, NULL, Instance);
+      } else {
+        DeviceExist = TRUE;
+      }
+    }
+    FreePool(Instance);
+  } while (CopyOfDevicePath != NULL);
+
+  FreePool (StartDevicePath);
+
+  if (!DeviceExist) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function will search every input/output device in current system,
+  and make every input/output device as potential console device.
+**/
+VOID
+EFIAPI
+EfiBootManagerConnectAllConsoles (
+  VOID
+  )
+{
+  UINTN                     Index;
+  EFI_DEVICE_PATH_PROTOCOL  *ConDevicePath;
+  UINTN                     HandleCount;
+  EFI_HANDLE                *HandleBuffer;
+
+  Index         = 0;
+  HandleCount   = 0;
+  HandleBuffer  = NULL;
+  ConDevicePath = NULL;
+
+  //
+  // Update all the console variables
+  //
+  gBS->LocateHandleBuffer (
+          ByProtocol,
+          &gEfiSimpleTextInProtocolGuid,
+          NULL,
+          &HandleCount,
+          &HandleBuffer
+          );
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    gBS->HandleProtocol (
+            HandleBuffer[Index],
+            &gEfiDevicePathProtocolGuid,
+            (VOID **) &ConDevicePath
+            );
+    EfiBootManagerUpdateConsoleVariable (ConIn, ConDevicePath, NULL);
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool(HandleBuffer);
+    HandleBuffer = NULL;
+  }
+
+  gBS->LocateHandleBuffer (
+          ByProtocol,
+          &gEfiSimpleTextOutProtocolGuid,
+          NULL,
+          &HandleCount,
+          &HandleBuffer
+          );
+  for (Index = 0; Index < HandleCount; Index++) {
+    gBS->HandleProtocol (
+            HandleBuffer[Index],
+            &gEfiDevicePathProtocolGuid,
+            (VOID **) &ConDevicePath
+            );
+    EfiBootManagerUpdateConsoleVariable (ConOut, ConDevicePath, NULL);
+    EfiBootManagerUpdateConsoleVariable (ErrOut, ConDevicePath, NULL);
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool(HandleBuffer);
+  }
+
+  //
+  // Connect all console variables
+  //
+  EfiBootManagerConnectAllDefaultConsoles ();
+}
+
+
+/**
+  This function will connect all the console devices base on the console
+  device variable ConIn, ConOut and ErrOut.
+
+  @retval EFI_DEVICE_ERROR         All the consoles were not connected due to an error.
+  @retval EFI_SUCCESS              Success connect any one instance of the console
+                                   device path base on the variable ConVarName.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerConnectAllDefaultConsoles (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  BOOLEAN                   OneConnected;
+  BOOLEAN                   SystemTableUpdated;
+
+  OneConnected = FALSE;
+
+  Status = EfiBootManagerConnectConsoleVariable (ConOut);
+  if (!EFI_ERROR (Status)) {
+    OneConnected = TRUE;
+  }
+  PERF_START (NULL, "ConOutReady", "BDS", 1);
+  PERF_END   (NULL, "ConOutReady", "BDS", 0);
+
+  
+  Status = EfiBootManagerConnectConsoleVariable (ConIn);
+  if (!EFI_ERROR (Status)) {
+    OneConnected = TRUE;
+  }
+  PERF_START (NULL, "ConInReady", "BDS", 1);
+  PERF_END   (NULL, "ConInReady", "BDS", 0);
+
+  Status = EfiBootManagerConnectConsoleVariable (ErrOut);
+  if (!EFI_ERROR (Status)) {
+    OneConnected = TRUE;
+  }
+  PERF_START (NULL, "ErrOutReady", "BDS", 1);
+  PERF_END   (NULL, "ErrOutReady", "BDS", 0);
+
+  SystemTableUpdated = FALSE;
+  //
+  // Fill console handles in System Table if no console device assignd.
+  //
+  if (BmUpdateSystemTableConsole (L"ConIn", &gEfiSimpleTextInProtocolGuid, &gST->ConsoleInHandle, (VOID **) &gST->ConIn)) {
+    SystemTableUpdated = TRUE;
+  }
+  if (BmUpdateSystemTableConsole (L"ConOut", &gEfiSimpleTextOutProtocolGuid, &gST->ConsoleOutHandle, (VOID **) &gST->ConOut)) {
+    SystemTableUpdated = TRUE;
+  }
+  if (BmUpdateSystemTableConsole (L"ErrOut", &gEfiSimpleTextOutProtocolGuid, &gST->StandardErrorHandle, (VOID **) &gST->StdErr)) {
+    SystemTableUpdated = TRUE;
+  }
+
+  if (SystemTableUpdated) {
+    //
+    // Update the CRC32 in the EFI System Table header
+    //
+    gST->Hdr.CRC32 = 0;
+    gBS->CalculateCrc32 (
+          (UINT8 *) &gST->Hdr,
+          gST->Hdr.HeaderSize,
+          &gST->Hdr.CRC32
+          );
+  }
+
+  return OneConnected ? EFI_SUCCESS : EFI_DEVICE_ERROR;
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmDriverHealth.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmDriverHealth.c
new file mode 100644
index 0000000000..bffbedaf84
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmDriverHealth.c
@@ -0,0 +1,576 @@
+/** @file
+  Library functions which relates with driver health.
+
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+  CHAR16 *mBmHealthStatusText[] = {
+    L"Healthy",
+    L"Repair Required",
+    L"Configuration Required",
+    L"Failed",
+    L"Reconnect Required",
+    L"Reboot Required"
+    };
+
+/**
+  Return the controller name.
+
+  @param DriverHealthHandle  The handle on which the Driver Health protocol instance is retrieved.
+  @param ControllerHandle    The handle of a controller that the driver specified by DriverBindingHandle is managing.
+                             This handle specifies the controller whose name is to be returned.
+  @param ChildHandle         The handle of the child controller to retrieve the name of. This is an
+                             optional parameter that may be NULL. It will be NULL for device drivers.
+                             It will also be NULL for bus drivers that attempt to retrieve the name
+                             of the bus controller. It will not be NULL for a bus driver that attempts
+                             to retrieve the name of a child controller.
+
+  @return A pointer to the Unicode string to return. This Unicode string is the name of the controller
+          specified by ControllerHandle and ChildHandle.
+**/
+CHAR16 *
+BmGetControllerName (
+  IN  EFI_HANDLE                   DriverHealthHandle,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           *ControllerName;
+  CHAR8                            *LanguageVariable;
+  CHAR8                            *BestLanguage;
+  BOOLEAN                          Iso639Language;
+  EFI_COMPONENT_NAME_PROTOCOL      *ComponentName;
+
+  ControllerName = NULL;
+
+  //
+  // Locate Component Name (2) protocol on the driver binging handle.
+  //
+  Iso639Language = FALSE;
+  Status = gBS->HandleProtocol (
+                 DriverHealthHandle,
+                 &gEfiComponentName2ProtocolGuid,
+                 (VOID **) &ComponentName
+                 );
+  if (EFI_ERROR (Status)) {
+    Status = gBS->HandleProtocol (
+                    DriverHealthHandle,
+                    &gEfiComponentNameProtocolGuid,
+                    (VOID **) &ComponentName
+                    );
+    if (!EFI_ERROR (Status)) {
+      Iso639Language = TRUE;
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    GetEfiGlobalVariable2 (Iso639Language ? L"Lang" : L"PlatformLang", (VOID**)&LanguageVariable, NULL);
+    BestLanguage     = GetBestLanguage(
+                         ComponentName->SupportedLanguages,
+                         Iso639Language,
+                         (LanguageVariable != NULL) ? LanguageVariable : "",
+                         Iso639Language ? "eng" : "en-US",
+                         NULL
+                         );
+    if (LanguageVariable != NULL) {
+      FreePool (LanguageVariable);
+    }
+
+    Status = ComponentName->GetControllerName (
+                              ComponentName,
+                              ControllerHandle, 
+                              ChildHandle,
+                              BestLanguage,
+                              &ControllerName
+                              );
+  }
+
+  if (!EFI_ERROR (Status)) {
+    return AllocateCopyPool (StrSize (ControllerName), ControllerName);
+  } else {
+    return ConvertDevicePathToText (
+             DevicePathFromHandle (ChildHandle != NULL ? ChildHandle : ControllerHandle),
+             FALSE,
+             FALSE
+             );
+  }
+}
+
+/**
+  Display a set of messages returned by the GetHealthStatus () service of the EFI Driver Health Protocol
+
+  @param DriverHealthInfo  Pointer to the Driver Health information entry.
+**/
+VOID
+BmDisplayMessages (
+  IN  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO *DriverHealthInfo
+  )
+{
+  UINTN                            Index;
+  EFI_STRING                       String;
+  CHAR16                           *ControllerName;
+
+  if (DriverHealthInfo->MessageList == NULL ||
+      DriverHealthInfo->MessageList[0].HiiHandle == NULL) {
+    return;
+  }
+
+  ControllerName = BmGetControllerName (
+                     DriverHealthInfo->DriverHealthHandle,
+                     DriverHealthInfo->ControllerHandle, 
+                     DriverHealthInfo->ChildHandle
+                     );
+
+  DEBUG ((EFI_D_INFO, "Controller: %s\n", ControllerName));
+  Print (L"Controller: %s\n", ControllerName);
+  for (Index = 0; DriverHealthInfo->MessageList[Index].HiiHandle != NULL; Index++) {
+    String = HiiGetString (
+               DriverHealthInfo->MessageList[Index].HiiHandle,
+               DriverHealthInfo->MessageList[Index].StringId,
+               NULL
+               );
+    if (String != NULL) {
+      Print (L"  %s\n", String);
+      DEBUG ((EFI_D_INFO, "  %s\n", String));
+      FreePool (String);
+    }
+  }
+
+  if (ControllerName != NULL) {
+    FreePool (ControllerName);
+  }
+}
+
+/**
+  The repair notify function.
+  @param Value  A value between 0 and Limit that identifies the current progress
+                of the repair operation.
+  @param Limit  The maximum value of Value for the current repair operation.
+                If Limit is 0, then the completion progress is indeterminate.
+                For example, a driver that wants to specify progress in percent
+                would use a Limit value of 100.
+
+  @retval EFI_SUCCESS  Successfully return from the notify function.
+**/
+EFI_STATUS
+EFIAPI
+BmRepairNotify (
+  IN UINTN        Value,
+  IN UINTN        Limit
+  )
+{
+  DEBUG ((EFI_D_INFO, "[BDS]RepairNotify: %d/%d\n", Value, Limit));
+  Print (L"[BDS]RepairNotify: %d/%d\n", Value, Limit);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Collect the Driver Health status of a single controller.
+  
+  @param DriverHealthInfo        A pointer to the array containing all of the platform driver health information.
+  @param Count                   Return the updated array count.
+  @param DriverHealthHandle      The handle on which the Driver Health protocol instance is retrieved.
+  @param ControllerHandle        The handle of the controller..
+  @param ChildHandle             The handle of the child controller to retrieve the health
+                                 status on.  This is an optional parameter that may be NULL.
+
+  @retval Status                 The status returned from GetHealthStatus.
+  @retval EFI_ABORTED            The health status is healthy so no further query is needed.
+
+**/
+EFI_STATUS
+BmGetSingleControllerHealthStatus (
+  IN OUT EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO **DriverHealthInfo,
+  IN OUT UINTN                               *Count,
+  IN  EFI_HANDLE                             DriverHealthHandle,
+  IN  EFI_HANDLE                             ControllerHandle,  OPTIONAL
+  IN  EFI_HANDLE                             ChildHandle        OPTIONAL
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_DRIVER_HEALTH_PROTOCOL     *DriverHealth;
+  EFI_DRIVER_HEALTH_HII_MESSAGE  *MessageList;
+  EFI_HII_HANDLE                 FormHiiHandle;
+  EFI_DRIVER_HEALTH_STATUS       HealthStatus;
+
+  ASSERT (DriverHealthHandle != NULL);
+  //
+  // Retrieve the Driver Health Protocol from DriverHandle
+  //
+  Status = gBS->HandleProtocol (
+                  DriverHealthHandle,
+                  &gEfiDriverHealthProtocolGuid,
+                  (VOID **) &DriverHealth
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+
+  if (ControllerHandle == NULL) {
+    //
+    // If ControllerHandle is NULL, the return the cumulative health status of the driver
+    //
+    Status = DriverHealth->GetHealthStatus (DriverHealth, NULL, NULL, &HealthStatus, NULL, NULL);
+    if (!EFI_ERROR (Status) && HealthStatus == EfiDriverHealthStatusHealthy) {
+      *DriverHealthInfo = ReallocatePool (
+                            (*Count)     * sizeof (EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO),
+                            (*Count + 1) * sizeof (EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO),
+                            *DriverHealthInfo
+                            );
+      ASSERT (*DriverHealthInfo != NULL);
+
+      (*DriverHealthInfo)[*Count].DriverHealthHandle = DriverHealthHandle;
+      (*DriverHealthInfo)[*Count].DriverHealth       = DriverHealth;
+      (*DriverHealthInfo)[*Count].HealthStatus       = HealthStatus;
+
+      *Count = *Count + 1;
+
+      Status = EFI_ABORTED;
+    }
+    return Status;
+  }
+
+  MessageList   = NULL;
+  FormHiiHandle = NULL;
+
+  //
+  // Collect the health status with the optional HII message list
+  //
+  Status = DriverHealth->GetHealthStatus (DriverHealth, ControllerHandle, ChildHandle, &HealthStatus, &MessageList, &FormHiiHandle);
+  if (!EFI_ERROR (Status)) {
+    *DriverHealthInfo = ReallocatePool (
+                          (*Count)     * sizeof (EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO),
+                          (*Count + 1) * sizeof (EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO),
+                          *DriverHealthInfo
+                          );
+    ASSERT (*DriverHealthInfo != NULL);
+    (*DriverHealthInfo)[*Count].DriverHealth       = DriverHealth;
+    (*DriverHealthInfo)[*Count].DriverHealthHandle = DriverHealthHandle;
+    (*DriverHealthInfo)[*Count].ControllerHandle   = ControllerHandle;
+    (*DriverHealthInfo)[*Count].ChildHandle        = ChildHandle;
+    (*DriverHealthInfo)[*Count].HiiHandle          = FormHiiHandle;
+    (*DriverHealthInfo)[*Count].MessageList        = MessageList;
+    (*DriverHealthInfo)[*Count].HealthStatus       = HealthStatus;
+
+    *Count = *Count + 1;
+  }
+
+  return Status;
+}
+
+/**
+  Return all the Driver Health information.
+
+  When the cumulative health status of all the controllers managed by the
+  driver who produces the EFI_DRIVER_HEALTH_PROTOCOL is healthy, only one
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO entry is created for such
+  EFI_DRIVER_HEALTH_PROTOCOL instance.
+  Otherwise, every controller creates one EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO
+  entry. Additionally every child controller creates one
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO entry if the driver is a bus driver.
+
+  @param Count      Return the count of the Driver Health information.
+
+  @retval NULL      No Driver Health information is returned.
+  @retval !NULL     Pointer to the Driver Health information array.
+**/
+EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO *
+EFIAPI
+EfiBootManagerGetDriverHealthInfo (
+  UINTN                      *Count
+  )
+{
+  EFI_STATUS                 Status;
+  UINTN                      NumHandles;
+  EFI_HANDLE                 *DriverHealthHandles;
+  UINTN                      DriverHealthIndex;
+  EFI_HANDLE                 *Handles;
+  UINTN                      HandleCount;
+  UINTN                      ControllerIndex;
+  UINTN                      ChildIndex;
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO  *DriverHealthInfo;
+
+  //
+  // Initialize local variables
+  //
+  *Count                  = 0;
+  DriverHealthInfo        = NULL;
+  Handles                 = NULL;
+  DriverHealthHandles     = NULL;
+  NumHandles              = 0;
+  HandleCount             = 0;
+
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDriverHealthProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &DriverHealthHandles
+                  );
+
+  if (Status == EFI_NOT_FOUND || NumHandles == 0) {
+    //
+    // If there are no Driver Health Protocols handles, then return EFI_NOT_FOUND
+    //
+    return NULL;
+  }
+
+  ASSERT_EFI_ERROR (Status);
+  ASSERT (DriverHealthHandles != NULL);
+
+  //
+  // Check the health status of all controllers in the platform
+  // Start by looping through all the Driver Health Protocol handles in the handle database
+  //
+  for (DriverHealthIndex = 0; DriverHealthIndex < NumHandles; DriverHealthIndex++) {
+    //
+    // Get the cumulative health status of the driver
+    //
+    Status = BmGetSingleControllerHealthStatus (&DriverHealthInfo, Count, DriverHealthHandles[DriverHealthIndex], NULL, NULL);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // See if the list of all handles in the handle database has been retrieved
+    //
+    //
+    if (Handles == NULL) {
+      //
+      // Retrieve the list of all handles from the handle database
+      //
+      Status = gBS->LocateHandleBuffer (
+        AllHandles,
+        NULL,
+        NULL,
+        &HandleCount,
+        &Handles
+        );
+      ASSERT_EFI_ERROR (Status);
+    }
+    //
+    // Loop through all the controller handles in the handle database
+    //
+    for (ControllerIndex = 0; ControllerIndex < HandleCount; ControllerIndex++) {
+      Status = BmGetSingleControllerHealthStatus (&DriverHealthInfo, Count, DriverHealthHandles[DriverHealthIndex], Handles[ControllerIndex], NULL);
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Loop through all the child handles in the handle database
+      //
+      for (ChildIndex = 0; ChildIndex < HandleCount; ChildIndex++) {
+        Status = BmGetSingleControllerHealthStatus (&DriverHealthInfo, Count, DriverHealthHandles[DriverHealthIndex], Handles[ControllerIndex], Handles[ChildIndex]);
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+      }
+    }
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (Handles != NULL) {
+    FreePool (Handles);
+  }
+  if (DriverHealthHandles != NULL) {
+    FreePool (DriverHealthHandles);
+  }
+
+  return DriverHealthInfo;
+}
+
+/**
+  Free the Driver Health information array.
+
+  @param DriverHealthInfo       Pointer to array of the Driver Health information.
+  @param Count                  Count of the array.
+
+  @retval EFI_SUCCESS           The array is freed.
+  @retval EFI_INVALID_PARAMETER The array is NULL.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeDriverHealthInfo (
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO *DriverHealthInfo,
+  UINTN                               Count
+  )
+{
+  UINTN                               Index;
+
+  for (Index = 0; Index < Count; Index++) {
+    if (DriverHealthInfo[Index].MessageList != NULL) {
+      FreePool (DriverHealthInfo[Index].MessageList);
+    }
+  }
+  return gBS->FreePool (DriverHealthInfo);
+}
+
+/**
+  Repair all the controllers according to the Driver Health status queried.
+**/
+VOID
+BmRepairAllControllers (
+  VOID
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO *DriverHealthInfo;
+  EFI_DRIVER_HEALTH_STATUS            HealthStatus;
+  UINTN                               Count;
+  UINTN                               Index;
+  BOOLEAN                             RepairRequired;
+  BOOLEAN                             ConfigurationRequired;
+  BOOLEAN                             ReconnectRequired;
+  BOOLEAN                             RebootRequired;
+  EFI_HII_HANDLE                      *HiiHandles;
+  EFI_FORM_BROWSER2_PROTOCOL          *FormBrowser2;
+
+  //
+  // Configure PcdDriverHealthConfigureForm to ZeroGuid to disable driver health check.
+  //
+  if (CompareGuid (PcdGetPtr (PcdDriverHealthConfigureForm), &gZeroGuid)) {
+    return;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiFormBrowser2ProtocolGuid, NULL, (VOID **) &FormBrowser2);
+  ASSERT_EFI_ERROR (Status);
+
+  do {
+    RepairRequired        = FALSE;
+    ConfigurationRequired = FALSE;
+
+    //
+    // Deal with Repair Required
+    //
+    DriverHealthInfo = EfiBootManagerGetDriverHealthInfo (&Count);
+    for (Index = 0; Index < Count; Index++) {
+      if (DriverHealthInfo[Index].HealthStatus == EfiDriverHealthStatusConfigurationRequired) {
+        ConfigurationRequired = TRUE;
+      }
+      
+      if (DriverHealthInfo[Index].HealthStatus == EfiDriverHealthStatusRepairRequired) {
+        RepairRequired        = TRUE;
+
+        BmDisplayMessages (&DriverHealthInfo[Index]);
+
+        Status = DriverHealthInfo[Index].DriverHealth->Repair (
+                                                         DriverHealthInfo[Index].DriverHealth,
+                                                         DriverHealthInfo[Index].ControllerHandle,
+                                                         DriverHealthInfo[Index].ChildHandle,
+                                                         BmRepairNotify
+                                                         );
+        if (!EFI_ERROR (Status) && !ConfigurationRequired) {
+          Status = DriverHealthInfo[Index].DriverHealth->GetHealthStatus (
+                                                           DriverHealthInfo[Index].DriverHealth,
+                                                           DriverHealthInfo[Index].ControllerHandle,
+                                                           DriverHealthInfo[Index].ChildHandle,
+                                                           &HealthStatus,
+                                                           NULL,
+                                                           NULL
+                                                           );
+          if (!EFI_ERROR (Status) && (HealthStatus == EfiDriverHealthStatusConfigurationRequired)) {
+            ConfigurationRequired = TRUE;
+          }
+        }
+      }
+    }
+
+    if (ConfigurationRequired) {
+      HiiHandles = HiiGetHiiHandles (NULL);
+      if (HiiHandles != NULL) {
+        for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+          Status = FormBrowser2->SendForm (
+                                   FormBrowser2,
+                                   &HiiHandles[Index],
+                                   1,
+                                   PcdGetPtr (PcdDriverHealthConfigureForm),
+                                   0,
+                                   NULL,
+                                   NULL
+                                   );
+          if (!EFI_ERROR (Status)) {
+            break;
+          }
+        }
+        FreePool (HiiHandles);
+      }
+    }
+  
+    EfiBootManagerFreeDriverHealthInfo (DriverHealthInfo, Count);
+  } while (RepairRequired || ConfigurationRequired);
+
+  RebootRequired    = FALSE;
+  ReconnectRequired = FALSE;
+  DriverHealthInfo  = EfiBootManagerGetDriverHealthInfo (&Count);
+  for (Index = 0; Index < Count; Index++) {
+
+    BmDisplayMessages (&DriverHealthInfo[Index]);
+
+    if (DriverHealthInfo[Index].HealthStatus == EfiDriverHealthStatusReconnectRequired) {
+      Status = gBS->DisconnectController (DriverHealthInfo[Index].ControllerHandle, NULL, NULL);
+      if (EFI_ERROR (Status)) {
+        //
+        // Disconnect failed. Need to promote reconnect to a reboot.
+        //
+        RebootRequired    = TRUE;
+      } else {
+        gBS->ConnectController (DriverHealthInfo[Index].ControllerHandle, NULL, NULL, TRUE);
+        ReconnectRequired = TRUE;
+      }
+    }
+
+    if (DriverHealthInfo[Index].HealthStatus == EfiDriverHealthStatusRebootRequired) {
+      RebootRequired      = TRUE;
+    }
+  }
+  EfiBootManagerFreeDriverHealthInfo (DriverHealthInfo, Count);
+
+
+  if (ReconnectRequired) {
+    BmRepairAllControllers ();
+  }
+
+  DEBUG_CODE (
+    CHAR16 *ControllerName;
+
+    DriverHealthInfo = EfiBootManagerGetDriverHealthInfo (&Count);
+    for (Index = 0; Index < Count; Index++) {
+      ControllerName = BmGetControllerName (
+                         DriverHealthInfo[Index].DriverHealthHandle,
+                         DriverHealthInfo[Index].ControllerHandle,
+                         DriverHealthInfo[Index].ChildHandle
+                         );
+      DEBUG ((
+        EFI_D_INFO,
+        "%02d: %s - %s\n",
+        Index,
+        ControllerName,
+        mBmHealthStatusText[DriverHealthInfo[Index].HealthStatus]
+        ));
+      if (ControllerName != NULL) {
+        FreePool (ControllerName);
+      }
+    }
+    EfiBootManagerFreeDriverHealthInfo (DriverHealthInfo, Count);
+    );
+
+  if (RebootRequired) {
+    DEBUG ((EFI_D_INFO, "[BDS] One of the Driver Health instances requires rebooting.\n"));
+    gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+  }
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmHotkey.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmHotkey.c
new file mode 100644
index 0000000000..8d398fb4c6
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmHotkey.c
@@ -0,0 +1,1101 @@
+/** @file
+  Hotkey library functions.
+
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+//
+// Lock for linked list
+//
+EFI_LOCK                     mBmHotkeyLock            = EFI_INITIALIZE_LOCK_VARIABLE (TPL_NOTIFY);
+LIST_ENTRY                   mBmHotkeyList            = INITIALIZE_LIST_HEAD_VARIABLE (mBmHotkeyList);
+EFI_EVENT                    mBmHotkeyTriggered       = NULL;
+BOOLEAN                      mBmHotkeyServiceStarted  = FALSE;
+UINTN                        mBmHotkeySupportCount    = 0;
+
+//
+// Set OptionNumber as unassigned value to indicate the option isn't initialized
+//
+EFI_BOOT_MANAGER_LOAD_OPTION mBmHotkeyBootOption      = { LoadOptionNumberUnassigned };
+
+EFI_BOOT_MANAGER_KEY_OPTION  *mBmContinueKeyOption    = NULL;
+VOID                         *mBmTxtInExRegistration  = NULL;
+
+/**
+
+  Check whether the input key option is valid.
+
+  @param   KeyOption               Input key option info.
+
+  @retval  TRUE               Input key option is valid.
+  @retval  FALSE              Input key option is not valid.
+**/
+BOOLEAN
+BmIsKeyOptionValid (
+  IN EFI_BOOT_MANAGER_KEY_OPTION     *KeyOption
+)
+{
+  UINT16   OptionName[sizeof (L"Boot####")];
+  UINT8    *BootOption;
+  UINTN    BootOptionSize;
+  UINT32   Crc;
+
+  //
+  // Check whether corresponding Boot Option exist
+  //
+  UnicodeSPrint (OptionName, sizeof (OptionName), L"Boot%04x", KeyOption->BootOption);
+  GetEfiGlobalVariable2 (OptionName, (VOID **) &BootOption, &BootOptionSize);
+
+  if (BootOption == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Check CRC for Boot Option
+  //
+  gBS->CalculateCrc32 (BootOption, BootOptionSize, &Crc);
+  FreePool (BootOption);
+
+  return (BOOLEAN) (KeyOption->BootOptionCrc == Crc);
+}
+
+/**
+
+  Check whether the input variable is an key option variable.
+
+  @param   Name               Input variable name.
+  @param   Guid               Input variable guid.
+  @param   OptionNumber       The option number of this key option variable.
+
+  @retval  TRUE               Input variable is a key option variable.
+  @retval  FALSE              Input variable is not a key option variable.
+**/
+BOOLEAN
+BmIsKeyOptionVariable (
+  CHAR16        *Name,
+  EFI_GUID      *Guid,
+  UINT16        *OptionNumber
+  )
+{
+  UINTN         Index;
+  
+  if (!CompareGuid (Guid, &gEfiGlobalVariableGuid) ||
+      (StrSize (Name) != sizeof (L"Key####")) ||
+      (StrnCmp (Name, L"Key", 3) != 0)
+     ) {
+    return FALSE;
+  }
+
+  *OptionNumber = 0;
+  for (Index = 3; Index < 7; Index++) {
+    if ((Name[Index] >= L'0') && (Name[Index] <= L'9')) {
+      *OptionNumber = *OptionNumber * 16 + Name[Index] - L'0';
+    } else if ((Name[Index] >= L'A') && (Name[Index] <= L'F')) {
+      *OptionNumber = *OptionNumber * 16 + Name[Index] - L'A' + 10;
+    } else {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  Return the buffer size of the EFI_BOOT_MANAGER_KEY_OPTION data.
+
+  @param   KeyOption            The input key option info.
+
+  @retval  The buffer size of the key option data.
+**/
+UINTN
+BmSizeOfKeyOption (
+  EFI_BOOT_MANAGER_KEY_OPTION  *KeyOption
+  )
+{
+  return OFFSET_OF (EFI_BOOT_MANAGER_KEY_OPTION, Keys)
+    + KeyOption->KeyData.Options.InputKeyCount * sizeof (EFI_INPUT_KEY);
+}
+
+/**
+  Return the array of key options.
+
+  @param Count  Return the number of key options.
+
+  @retval NULL  No key option.
+  @retval Other Pointer to the key options.
+**/
+EFI_BOOT_MANAGER_KEY_OPTION *
+BmGetKeyOptions (
+  OUT UINTN     *Count
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       Index;
+  CHAR16                      *Name;
+  EFI_GUID                    Guid;
+  UINTN                       NameSize;
+  UINTN                       NewNameSize;
+  EFI_BOOT_MANAGER_KEY_OPTION *KeyOptions;
+  EFI_BOOT_MANAGER_KEY_OPTION *KeyOption;
+  UINT16                      OptionNumber;
+
+  if (Count == NULL) {
+    return NULL;
+  }
+
+  *Count     = 0;
+  KeyOptions = NULL;
+
+  NameSize = sizeof (CHAR16);
+  Name     = AllocateZeroPool (NameSize);
+  ASSERT (Name != NULL);
+  while (TRUE) {
+    NewNameSize = NameSize;
+    Status = gRT->GetNextVariableName (&NewNameSize, Name, &Guid);
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      Name = ReallocatePool (NameSize, NewNameSize, Name);
+      ASSERT (Name != NULL);
+      Status = gRT->GetNextVariableName (&NewNameSize, Name, &Guid);
+      NameSize = NewNameSize;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      break;
+    }
+    ASSERT_EFI_ERROR (Status);
+
+    if (BmIsKeyOptionVariable (Name ,&Guid, &OptionNumber)) {
+      GetEfiGlobalVariable2 (Name, (VOID**) &KeyOption, NULL);
+      ASSERT (KeyOption != NULL);
+      if (BmIsKeyOptionValid (KeyOption)) {
+        KeyOptions = ReallocatePool (
+                       *Count * sizeof (EFI_BOOT_MANAGER_KEY_OPTION),
+                       (*Count + 1) * sizeof (EFI_BOOT_MANAGER_KEY_OPTION),
+                       KeyOptions
+                       );
+        ASSERT (KeyOptions != NULL);
+        //
+        // Insert the key option in order
+        //
+        for (Index = 0; Index < *Count; Index++) {
+          if (OptionNumber < KeyOptions[Index].OptionNumber) {
+            break;
+          }
+        }
+        CopyMem (&KeyOptions[Index + 1], &KeyOptions[Index], (*Count - Index) * sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+        CopyMem (&KeyOptions[Index], KeyOption, BmSizeOfKeyOption (KeyOption));
+        KeyOptions[Index].OptionNumber = OptionNumber;
+        (*Count)++;
+      }
+      FreePool (KeyOption);
+    }
+  }
+
+  FreePool (Name);
+
+  return KeyOptions;
+}
+
+/**
+  Callback function for event.
+  
+  @param    Event          Event for this callback function.
+  @param    Context        Context pass to this function.
+**/
+VOID
+EFIAPI
+BmEmptyFunction (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+}
+
+/**
+  Check whether the bit is set in the value.
+
+  @param   Value            The value need to be check.
+  @param   Bit              The bit filed need to be check.
+
+  @retval  TRUE             The bit is set.
+  @retval  FALSE            The bit is not set.
+**/
+BOOLEAN
+BmBitSet (
+  IN UINT32   Value,
+  IN UINT32   Bit
+  )
+{
+  return (BOOLEAN) ((Value & Bit) != 0);
+}
+
+/**
+  Initialize the KeyData and Key[] in the EFI_BOOT_MANAGER_KEY_OPTION.
+
+  @param  Modifier   Input key info.
+  @param  Args       Va_list info.
+  @param  KeyOption  Key info which need to update.
+
+  @retval  EFI_SUCCESS             Succeed to initialize the KeyData and Key[].
+  @return  EFI_INVALID_PARAMETER   Input parameter error.
+**/
+EFI_STATUS
+BmInitializeKeyFields (
+  IN UINT32                       Modifier,
+  IN VA_LIST                      Args,
+  OUT EFI_BOOT_MANAGER_KEY_OPTION *KeyOption
+  )
+{
+  EFI_INPUT_KEY                   *Key;
+
+  if (KeyOption == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Key = NULL;
+  while (KeyOption->KeyData.Options.InputKeyCount < sizeof (KeyOption->Keys) / sizeof (KeyOption->Keys[0])) {
+    Key = VA_ARG (Args, EFI_INPUT_KEY *);
+    if (Key == NULL) {
+      break;
+    }
+    CopyMem (
+      &KeyOption->Keys[KeyOption->KeyData.Options.InputKeyCount],
+      Key,
+      sizeof (EFI_INPUT_KEY)
+      );
+    KeyOption->KeyData.Options.InputKeyCount++;
+  }
+
+  if (Key != NULL) {
+    //
+    // Too many keys
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Modifier & ~(EFI_BOOT_MANAGER_SHIFT_PRESSED
+                 | EFI_BOOT_MANAGER_CONTROL_PRESSED
+                 | EFI_BOOT_MANAGER_ALT_PRESSED
+                 | EFI_BOOT_MANAGER_LOGO_PRESSED
+                 | EFI_BOOT_MANAGER_MENU_KEY_PRESSED
+                 | EFI_BOOT_MANAGER_SYS_REQ_PRESSED
+                 )) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_SHIFT_PRESSED)) {
+    KeyOption->KeyData.Options.ShiftPressed = 1;
+  }
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_CONTROL_PRESSED)) {
+    KeyOption->KeyData.Options.ControlPressed = 1;
+  }
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_ALT_PRESSED)) {
+    KeyOption->KeyData.Options.AltPressed = 1;
+  }
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_LOGO_PRESSED)) {
+    KeyOption->KeyData.Options.LogoPressed = 1;
+  }
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_MENU_KEY_PRESSED)) {
+    KeyOption->KeyData.Options.MenuPressed = 1;
+  }
+  if (BmBitSet (Modifier, EFI_BOOT_MANAGER_SYS_REQ_PRESSED)) {
+    KeyOption->KeyData.Options.SysReqPressed = 1;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Try to boot the boot option triggered by hot key.
+**/
+VOID
+EFIAPI
+EfiBootManagerHotkeyBoot (
+  VOID
+  )
+{
+  if (mBmHotkeyBootOption.OptionNumber != LoadOptionNumberUnassigned) {
+    EfiBootManagerBoot (&mBmHotkeyBootOption);
+    EfiBootManagerFreeLoadOption (&mBmHotkeyBootOption);
+    mBmHotkeyBootOption.OptionNumber = LoadOptionNumberUnassigned;
+  }
+}
+
+/**
+  This is the common notification function for HotKeys, it will be registered
+  with SimpleTextInEx protocol interface - RegisterKeyNotify() of ConIn handle.
+
+  @param KeyData         A pointer to a buffer that is filled in with the keystroke
+                         information for the key that was pressed.
+
+  @retval  EFI_SUCCESS   KeyData is successfully processed.
+  @return  EFI_NOT_FOUND Fail to find boot option variable.
+**/
+EFI_STATUS
+EFIAPI
+BmHotkeyCallback (
+  IN EFI_KEY_DATA     *KeyData
+)
+{
+  LIST_ENTRY                    *Link;
+  BM_HOTKEY                     *Hotkey;
+  CHAR16                        OptionName[sizeof ("Boot####")];
+  EFI_STATUS                    Status;
+  EFI_KEY_DATA                  *HotkeyData;
+
+  if (mBmHotkeyBootOption.OptionNumber != LoadOptionNumberUnassigned) {
+    //
+    // Do not process sequential hotkey stroke until the current boot option returns
+    //
+    return EFI_SUCCESS;
+  }
+
+  DEBUG ((EFI_D_INFO, "[Bds]BmHotkeyCallback: %04x:%04x\n", KeyData->Key.ScanCode, KeyData->Key.UnicodeChar));
+
+  EfiAcquireLock (&mBmHotkeyLock);
+  for ( Link = GetFirstNode (&mBmHotkeyList)
+      ; !IsNull (&mBmHotkeyList, Link)
+      ; Link = GetNextNode (&mBmHotkeyList, Link)
+      ) {
+    Hotkey = BM_HOTKEY_FROM_LINK (Link);
+
+    //
+    // Is this Key Stroke we are waiting for?
+    //
+    ASSERT (Hotkey->WaitingKey < (sizeof (Hotkey->KeyData) / sizeof (Hotkey->KeyData[0])));
+    HotkeyData = &Hotkey->KeyData[Hotkey->WaitingKey];
+    if ((KeyData->Key.ScanCode == HotkeyData->Key.ScanCode) &&
+        (KeyData->Key.UnicodeChar == HotkeyData->Key.UnicodeChar) &&
+        (((KeyData->KeyState.KeyShiftState & EFI_SHIFT_STATE_VALID) != 0) ? 
+          (KeyData->KeyState.KeyShiftState == HotkeyData->KeyState.KeyShiftState) : TRUE
+        )
+       ) {
+
+      //
+      // Receive an expecting key stroke, transit to next waiting state
+      //
+      Hotkey->WaitingKey++;
+
+      if (Hotkey->WaitingKey == Hotkey->CodeCount) {
+        //
+        // Reset to initial waiting state
+        //
+        Hotkey->WaitingKey = 0;
+        //
+        // Received the whole key stroke sequence
+        //
+        Status = gBS->SignalEvent (mBmHotkeyTriggered);
+        ASSERT_EFI_ERROR (Status);
+
+        if (!Hotkey->IsContinue) {
+          //
+          // Launch its BootOption
+          //
+          UnicodeSPrint (OptionName, sizeof (OptionName), L"Boot%04x", Hotkey->BootOption);
+          Status = EfiBootManagerVariableToLoadOption (OptionName, &mBmHotkeyBootOption);
+          DEBUG ((EFI_D_INFO, "[Bds]Hotkey for %s pressed - %r\n", OptionName, Status));
+          if (EFI_ERROR (Status)) {
+            mBmHotkeyBootOption.OptionNumber = LoadOptionNumberUnassigned;
+          }
+        } else {
+          DEBUG ((EFI_D_INFO, "[Bds]Continue key pressed!\n"));
+        }
+      }
+    } else {
+      //
+      // Receive an unexpected key stroke, reset to initial waiting state
+      //
+      Hotkey->WaitingKey = 0;
+    }
+
+  }
+  EfiReleaseLock (&mBmHotkeyLock);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unregister hotkey notify list.
+
+  @param    Hotkey                Hotkey list.
+
+  @retval   EFI_SUCCESS           Unregister hotkey notify success.
+  @retval   Others                Unregister hotkey notify failed.
+**/
+EFI_STATUS
+BmUnregisterHotkeyNotify (
+  IN BM_HOTKEY                          *Hotkey
+  )
+{
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  UINTN                                 KeyIndex;
+  EFI_HANDLE                            *Handles;
+  UINTN                                 HandleCount;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL     *TxtInEx;
+  VOID                                  *NotifyHandle;
+
+  gBS->LocateHandleBuffer (
+          ByProtocol,
+          &gEfiSimpleTextInputExProtocolGuid,
+          NULL,
+          &HandleCount,
+          &Handles
+          );
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (Handles[Index], &gEfiSimpleTextInputExProtocolGuid, (VOID **) &TxtInEx);
+    ASSERT_EFI_ERROR (Status);
+    for (KeyIndex = 0; KeyIndex < Hotkey->CodeCount; KeyIndex++) {
+      Status = TxtInEx->RegisterKeyNotify (
+                          TxtInEx,
+                          &Hotkey->KeyData[KeyIndex],
+                          BmHotkeyCallback,
+                          &NotifyHandle
+                          );
+      if (!EFI_ERROR (Status)) {
+        Status = TxtInEx->UnregisterKeyNotify (TxtInEx, NotifyHandle);
+        DEBUG ((EFI_D_INFO, "[Bds]UnregisterKeyNotify: %04x/%04x %r\n", Hotkey->KeyData[KeyIndex].Key.ScanCode, Hotkey->KeyData[KeyIndex].Key.UnicodeChar, Status));
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register hotkey notify list.
+
+  @param    TxtInEx               Pointer to EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL protocol.
+  @param    Hotkey                Hotkey list.
+
+  @retval   EFI_SUCCESS           Register hotkey notify success.
+  @retval   Others                Register hotkey notify failed.
+**/
+EFI_STATUS
+BmRegisterHotkeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *TxtInEx,
+  IN BM_HOTKEY                          *Hotkey
+  )
+{
+  EFI_STATUS                            Status;
+  UINTN                                 Index;
+  VOID                                  *NotifyHandle;
+
+  for (Index = 0; Index < Hotkey->CodeCount; Index++) {
+    Status = TxtInEx->RegisterKeyNotify (
+                        TxtInEx,
+                        &Hotkey->KeyData[Index],
+                        BmHotkeyCallback,
+                        &NotifyHandle
+                        );
+    DEBUG ((
+      EFI_D_INFO,
+      "[Bds]RegisterKeyNotify: %04x/%04x %08x/%02x %r\n",
+      Hotkey->KeyData[Index].Key.ScanCode,
+      Hotkey->KeyData[Index].Key.UnicodeChar,
+      Hotkey->KeyData[Index].KeyState.KeyShiftState,
+      Hotkey->KeyData[Index].KeyState.KeyToggleState,
+      Status
+      ));
+    if (EFI_ERROR (Status)) {
+      //
+      // some of the hotkey registry failed
+      // do not unregister all in case we have both CTRL-ALT-P and CTRL-ALT-P-R
+      //
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate key shift state base on the input key option info.
+
+  @param    Depth                 Which key is checked.
+  @param    KeyOption             Input key option info.
+  @param    KeyShiftState         Input key shift state.
+  @param    KeyShiftStates        Return possible key shift state array.
+  @param    KeyShiftStateCount    Possible key shift state count.
+**/
+VOID
+BmGenerateKeyShiftState (
+  IN UINTN                             Depth,
+  IN EFI_BOOT_MANAGER_KEY_OPTION       *KeyOption,
+  IN UINT32                            KeyShiftState,
+  IN UINT32                            *KeyShiftStates,
+  IN UINTN                             *KeyShiftStateCount
+  )
+{
+  switch (Depth) {
+  case 0:
+    if (KeyOption->KeyData.Options.ShiftPressed) {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_RIGHT_SHIFT_PRESSED, KeyShiftStates, KeyShiftStateCount);
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_LEFT_SHIFT_PRESSED,  KeyShiftStates, KeyShiftStateCount);
+    } else {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState, KeyShiftStates, KeyShiftStateCount);
+    }
+    break;
+
+  case 1:
+    if (KeyOption->KeyData.Options.ControlPressed) {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_RIGHT_CONTROL_PRESSED, KeyShiftStates, KeyShiftStateCount);
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_LEFT_CONTROL_PRESSED,  KeyShiftStates, KeyShiftStateCount);
+    } else {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState, KeyShiftStates, KeyShiftStateCount);
+    }
+    break;
+
+  case 2:
+    if (KeyOption->KeyData.Options.AltPressed) {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_RIGHT_ALT_PRESSED, KeyShiftStates, KeyShiftStateCount);
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_LEFT_ALT_PRESSED,  KeyShiftStates, KeyShiftStateCount);
+    } else {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState, KeyShiftStates, KeyShiftStateCount);
+    }
+    break;
+  case  3:
+    if (KeyOption->KeyData.Options.LogoPressed) {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_RIGHT_LOGO_PRESSED, KeyShiftStates, KeyShiftStateCount);
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState | EFI_LEFT_LOGO_PRESSED,  KeyShiftStates, KeyShiftStateCount);
+    } else {
+      BmGenerateKeyShiftState (Depth + 1, KeyOption, KeyShiftState, KeyShiftStates, KeyShiftStateCount);
+    }
+    break;
+  case 4:
+    if (KeyOption->KeyData.Options.MenuPressed) {
+      KeyShiftState |= EFI_MENU_KEY_PRESSED;
+    }
+    if (KeyOption->KeyData.Options.SysReqPressed) {
+      KeyShiftState |= EFI_SYS_REQ_PRESSED;
+    }
+    KeyShiftStates[*KeyShiftStateCount] = KeyShiftState;
+    (*KeyShiftStateCount)++;
+    break;
+  }
+}
+
+/**
+  Add it to hot key database, register it to existing TxtInEx.
+  New TxtInEx will be automatically registered with all the hot key in dababase
+
+  @param    KeyOption  Input key option info.
+**/
+EFI_STATUS
+BmProcessKeyOption (
+  IN EFI_BOOT_MANAGER_KEY_OPTION       *KeyOption
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL    *TxtInEx;
+  EFI_HANDLE                           *Handles;
+  UINTN                                HandleCount;
+  UINTN                                HandleIndex;
+  UINTN                                Index;
+  BM_HOTKEY                            *Hotkey;
+  UINTN                                KeyIndex;
+  //
+  // 16 is enough to enumerate all the possible combination of LEFT_XXX and RIGHT_XXX
+  //
+  UINT32                               KeyShiftStates[16];
+  UINTN                                KeyShiftStateCount;
+
+  if (KeyOption->KeyData.Options.InputKeyCount > mBmHotkeySupportCount) {
+    return EFI_UNSUPPORTED;
+  }
+
+  KeyShiftStateCount = 0;
+  BmGenerateKeyShiftState (0, KeyOption, EFI_SHIFT_STATE_VALID, KeyShiftStates, &KeyShiftStateCount);
+  ASSERT (KeyShiftStateCount <= sizeof (KeyShiftStates) / sizeof (KeyShiftStates[0]));
+
+  EfiAcquireLock (&mBmHotkeyLock);
+
+  for (Index = 0; Index < KeyShiftStateCount; Index++) {
+    Hotkey = AllocateZeroPool (sizeof (BM_HOTKEY));
+    ASSERT (Hotkey != NULL);
+
+    Hotkey->Signature  = BM_HOTKEY_SIGNATURE;
+    Hotkey->BootOption = KeyOption->BootOption;
+    Hotkey->IsContinue = (BOOLEAN) (KeyOption == mBmContinueKeyOption);
+    Hotkey->CodeCount  = (UINT8) KeyOption->KeyData.Options.InputKeyCount;
+
+    for (KeyIndex = 0; KeyIndex < Hotkey->CodeCount; KeyIndex++) {
+      CopyMem (&Hotkey->KeyData[KeyIndex].Key, &KeyOption->Keys[KeyIndex], sizeof (EFI_INPUT_KEY));
+      Hotkey->KeyData[KeyIndex].KeyState.KeyShiftState = KeyShiftStates[Index];
+    }
+    InsertTailList (&mBmHotkeyList, &Hotkey->Link);
+
+    gBS->LocateHandleBuffer (
+            ByProtocol,
+            &gEfiSimpleTextInputExProtocolGuid,
+            NULL,
+            &HandleCount,
+            &Handles
+            );
+    for (HandleIndex = 0; HandleIndex < HandleCount; HandleIndex++) {
+      Status = gBS->HandleProtocol (Handles[HandleIndex], &gEfiSimpleTextInputExProtocolGuid, (VOID **) &TxtInEx);
+      ASSERT_EFI_ERROR (Status);
+      BmRegisterHotkeyNotify (TxtInEx, Hotkey);
+    }
+  }
+
+  EfiReleaseLock (&mBmHotkeyLock);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Callback function for SimpleTextInEx protocol install events
+
+  @param Event           the event that is signaled.
+  @param Context         not used here.
+
+**/
+VOID
+EFIAPI
+BmTxtInExCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  EFI_STATUS                         Status;
+  UINTN                              BufferSize;
+  EFI_HANDLE                         Handle;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *TxtInEx;
+  LIST_ENTRY                         *Link;
+
+  while (TRUE) {
+    BufferSize = sizeof (EFI_HANDLE);
+    Status = gBS->LocateHandle (
+                    ByRegisterNotify,
+                    NULL,
+                    mBmTxtInExRegistration,
+                    &BufferSize,
+                    &Handle
+                    );
+    if (EFI_ERROR (Status)) {
+      //
+      // If no more notification events exist
+      //
+      return ;
+    }
+
+    Status = gBS->HandleProtocol (
+                    Handle,
+                    &gEfiSimpleTextInputExProtocolGuid,
+                    (VOID **) &TxtInEx
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Register the hot key notification for the existing items in the list
+    //
+    EfiAcquireLock (&mBmHotkeyLock);
+    for (Link = GetFirstNode (&mBmHotkeyList); !IsNull (&mBmHotkeyList, Link); Link = GetNextNode (&mBmHotkeyList, Link)) {
+      BmRegisterHotkeyNotify (TxtInEx, BM_HOTKEY_FROM_LINK (Link));
+    }
+    EfiReleaseLock (&mBmHotkeyLock);
+  }
+}
+
+/**
+  Free the key options returned from BmGetKeyOptions.
+
+  @param KeyOptions     Pointer to the key options.
+  @param KeyOptionCount Number of the key options.
+
+  @retval EFI_SUCCESS   The key options are freed.
+  @retval EFI_NOT_FOUND KeyOptions is NULL.
+**/
+EFI_STATUS
+BmFreeKeyOptions (
+  IN EFI_BOOT_MANAGER_KEY_OPTION    *KeyOptions,
+  IN UINTN                          KeyOptionCount
+  )
+{
+  if (KeyOptions != NULL) {
+    FreePool (KeyOptions);
+    return EFI_SUCCESS;
+  } else {
+    return EFI_NOT_FOUND;
+  }
+}
+
+/**
+  Register the key option to exit the waiting of the Boot Manager timeout.
+  Platform should ensure that the continue key option isn't conflict with
+  other boot key options.
+
+  @param Modifier     Key shift state.
+  @param  ...         Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS         Successfully register the continue key option.
+  @retval EFI_ALREADY_STARTED The continue key option is already registered.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerRegisterContinueKeyOption (
+  IN UINT32           Modifier,
+  ...
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_BOOT_MANAGER_KEY_OPTION  KeyOption;
+  VA_LIST                      Args;
+  
+  if (mBmContinueKeyOption != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  ZeroMem (&KeyOption, sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+  VA_START (Args, Modifier);
+  Status = BmInitializeKeyFields (Modifier, Args, &KeyOption);
+  VA_END (Args);
+
+  if (!EFI_ERROR (Status)) {
+    mBmContinueKeyOption = AllocateCopyPool (sizeof (EFI_BOOT_MANAGER_KEY_OPTION), &KeyOption);
+    ASSERT (mBmContinueKeyOption != NULL);
+    if (mBmHotkeyServiceStarted) {
+      BmProcessKeyOption (mBmContinueKeyOption);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Stop the hotkey processing.
+  
+  @param    Event          Event pointer related to hotkey service.
+  @param    Context        Context pass to this function.
+**/
+VOID
+EFIAPI
+BmStopHotkeyService (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  LIST_ENTRY            *Link;
+  BM_HOTKEY             *Hotkey;
+
+  DEBUG ((EFI_D_INFO, "[Bds]Stop Hotkey Service!\n"));
+  gBS->CloseEvent (Event);
+
+  EfiAcquireLock (&mBmHotkeyLock);
+  for (Link = GetFirstNode (&mBmHotkeyList); !IsNull (&mBmHotkeyList, Link); ) {
+    Hotkey = BM_HOTKEY_FROM_LINK (Link);
+    BmUnregisterHotkeyNotify (Hotkey);
+    Link   = RemoveEntryList (Link);
+    FreePool (Hotkey);
+  }
+  EfiReleaseLock (&mBmHotkeyLock);
+}
+
+/**
+  Start the hot key service so that the key press can trigger the boot option.
+
+  @param HotkeyTriggered  Return the waitable event and it will be signaled 
+                          when a valid hot key is pressed.
+
+  @retval EFI_SUCCESS     The hot key service is started.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerStartHotkeyService (
+  IN EFI_EVENT                 *HotkeyTriggered
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_BOOT_MANAGER_KEY_OPTION  *KeyOptions;
+  UINTN                        KeyOptionCount;
+  UINTN                        Index;
+  EFI_EVENT                    Event;
+  UINT32                       *BootOptionSupport;
+
+  Status = GetEfiGlobalVariable2 (EFI_BOOT_OPTION_SUPPORT_VARIABLE_NAME, (VOID **) &BootOptionSupport, NULL);
+  ASSERT (BootOptionSupport != NULL);
+
+  if ((*BootOptionSupport & EFI_BOOT_OPTION_SUPPORT_KEY)  != 0) {
+    mBmHotkeySupportCount = ((*BootOptionSupport & EFI_BOOT_OPTION_SUPPORT_COUNT) >> LowBitSet32 (EFI_BOOT_OPTION_SUPPORT_COUNT));
+  }
+  FreePool (BootOptionSupport);
+
+  if (mBmHotkeySupportCount == 0) {
+    DEBUG ((EFI_D_INFO, "Bds: BootOptionSupport NV variable forbids starting the hotkey service.\n"));
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_CALLBACK,
+                  BmEmptyFunction,
+                  NULL,
+                  &mBmHotkeyTriggered
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  if (HotkeyTriggered != NULL) {
+    *HotkeyTriggered = mBmHotkeyTriggered;
+  }
+
+  KeyOptions = BmGetKeyOptions (&KeyOptionCount);
+  for (Index = 0; Index < KeyOptionCount; Index ++) {
+    BmProcessKeyOption (&KeyOptions[Index]);
+  }
+  BmFreeKeyOptions (KeyOptions, KeyOptionCount);
+
+  if (mBmContinueKeyOption != NULL) {
+    BmProcessKeyOption (mBmContinueKeyOption);
+  }
+
+  EfiCreateProtocolNotifyEvent (
+    &gEfiSimpleTextInputExProtocolGuid,
+    TPL_CALLBACK,
+    BmTxtInExCallback,
+    NULL,
+    &mBmTxtInExRegistration
+    );
+
+  Status = EfiCreateEventReadyToBootEx (
+             TPL_CALLBACK,
+             BmStopHotkeyService,
+             NULL,
+             &Event
+             );
+  ASSERT_EFI_ERROR (Status);
+
+
+  mBmHotkeyServiceStarted = TRUE;
+  return Status;
+}
+
+/**
+  Add the key option.
+  It adds the key option variable and the key option takes affect immediately.
+
+  @param AddedOption      Return the added key option.
+  @param BootOptionNumber The boot option number for the key option.
+  @param Modifier         Key shift state.
+  @param ...              Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS         The key option is added.
+  @retval EFI_ALREADY_STARTED The hot key is already used by certain key option.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerAddKeyOptionVariable (
+  OUT EFI_BOOT_MANAGER_KEY_OPTION *AddedOption,   OPTIONAL
+  IN UINT16                       BootOptionNumber,
+  IN UINT32                       Modifier,
+  ...
+  )
+{
+  EFI_STATUS                     Status;
+  VA_LIST                        Args;
+  VOID                           *BootOption;
+  UINTN                          BootOptionSize;
+  CHAR16                         BootOptionName[sizeof (L"Boot####")];
+  EFI_BOOT_MANAGER_KEY_OPTION    KeyOption;
+  EFI_BOOT_MANAGER_KEY_OPTION    *KeyOptions;
+  UINTN                          KeyOptionCount;
+  UINTN                          Index;
+  UINTN                          KeyOptionNumber;
+  CHAR16                         KeyOptionName[sizeof (L"Key####")];
+
+  UnicodeSPrint (BootOptionName, sizeof (BootOptionName), L"Boot%04x", BootOptionNumber);
+  GetEfiGlobalVariable2 (BootOptionName, &BootOption, &BootOptionSize);
+
+  if (BootOption == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  ZeroMem (&KeyOption, sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+  KeyOption.BootOption = BootOptionNumber;
+  Status = gBS->CalculateCrc32 (BootOption, BootOptionSize, &KeyOption.BootOptionCrc);
+  ASSERT_EFI_ERROR (Status);
+  FreePool (BootOption);
+
+  VA_START (Args, Modifier);
+  Status = BmInitializeKeyFields (Modifier, Args, &KeyOption);
+  VA_END (Args);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  KeyOptionNumber = LoadOptionNumberUnassigned;
+  //
+  // Check if the hot key sequence was defined already
+  //
+  KeyOptions = BmGetKeyOptions (&KeyOptionCount);
+  for (Index = 0; Index < KeyOptionCount; Index++) {
+    if ((KeyOptions[Index].KeyData.PackedValue == KeyOption.KeyData.PackedValue) &&
+      (CompareMem (KeyOptions[Index].Keys, KeyOption.Keys, KeyOption.KeyData.Options.InputKeyCount * sizeof (EFI_INPUT_KEY)) == 0)) {
+      break;
+    }
+
+    if ((KeyOptionNumber == LoadOptionNumberUnassigned) &&
+        (KeyOptions[Index].OptionNumber > Index)
+       ){
+      KeyOptionNumber = Index;
+    }
+  }
+  BmFreeKeyOptions (KeyOptions, KeyOptionCount);
+
+  if (Index < KeyOptionCount) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  if (KeyOptionNumber == LoadOptionNumberUnassigned) {
+    KeyOptionNumber = KeyOptionCount;
+  }
+
+  UnicodeSPrint (KeyOptionName, sizeof (KeyOptionName), L"Key%04x", KeyOptionNumber);
+
+  Status = gRT->SetVariable (
+                  KeyOptionName,
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                  BmSizeOfKeyOption (&KeyOption),
+                  &KeyOption
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Return the Key Option in case needed by caller
+    //
+    if (AddedOption != NULL) {
+      CopyMem (AddedOption, &KeyOption, sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+    }
+
+    //
+    // Register the newly added hot key
+    // Calling this function before EfiBootManagerStartHotkeyService doesn't
+    // need to call BmProcessKeyOption
+    //
+    if (mBmHotkeyServiceStarted) {
+      BmProcessKeyOption (&KeyOption);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Delete the Key Option variable and unregister the hot key
+
+  @param DeletedOption  Return the deleted key options.
+  @param Modifier       Key shift state.
+  @param ...            Parameter list of pointer of EFI_INPUT_KEY.
+
+  @retval EFI_SUCCESS   The key option is deleted.
+  @retval EFI_NOT_FOUND The key option cannot be found.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerDeleteKeyOptionVariable (
+  IN EFI_BOOT_MANAGER_KEY_OPTION *DeletedOption, OPTIONAL
+  IN UINT32                      Modifier,
+  ...
+  )
+{
+  EFI_STATUS                     Status;
+  UINTN                          Index;
+  VA_LIST                        Args;
+  EFI_BOOT_MANAGER_KEY_OPTION    KeyOption;
+  EFI_BOOT_MANAGER_KEY_OPTION    *KeyOptions;
+  UINTN                          KeyOptionCount;
+  LIST_ENTRY                     *Link;
+  BM_HOTKEY                      *Hotkey;
+  UINT32                         ShiftState;
+  BOOLEAN                        Match;
+  CHAR16                         KeyOptionName[sizeof (L"Key####")];
+
+  ZeroMem (&KeyOption, sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+  VA_START (Args, Modifier);
+  Status = BmInitializeKeyFields (Modifier, Args, &KeyOption);
+  VA_END (Args);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  EfiAcquireLock (&mBmHotkeyLock);
+  //
+  // Delete the key option from active hot key list
+  // Could have multiple entries when modifier isn't 0 because we map the ShiftPressed to RIGHT_SHIFT and RIGHT_SHIFT
+  //
+  for (Link = GetFirstNode (&mBmHotkeyList); !IsNull (&mBmHotkeyList, Link); ) {
+    Hotkey = BM_HOTKEY_FROM_LINK (Link);
+    Match  = (BOOLEAN) (Hotkey->CodeCount == KeyOption.KeyData.Options.InputKeyCount);
+
+    for (Index = 0; Match && (Index < Hotkey->CodeCount); Index++) {
+      ShiftState = Hotkey->KeyData[Index].KeyState.KeyShiftState;
+      if (
+        (BmBitSet (ShiftState, EFI_RIGHT_SHIFT_PRESSED | EFI_LEFT_SHIFT_PRESSED) != KeyOption.KeyData.Options.ShiftPressed) ||
+        (BmBitSet (ShiftState, EFI_RIGHT_CONTROL_PRESSED | EFI_LEFT_CONTROL_PRESSED) != KeyOption.KeyData.Options.ControlPressed) ||
+        (BmBitSet (ShiftState, EFI_RIGHT_ALT_PRESSED | EFI_LEFT_ALT_PRESSED) != KeyOption.KeyData.Options.AltPressed) ||
+        (BmBitSet (ShiftState, EFI_RIGHT_LOGO_PRESSED | EFI_LEFT_LOGO_PRESSED) != KeyOption.KeyData.Options.LogoPressed) ||
+        (BmBitSet (ShiftState, EFI_MENU_KEY_PRESSED) != KeyOption.KeyData.Options.MenuPressed) ||
+        (BmBitSet (ShiftState, EFI_SYS_REQ_PRESSED) != KeyOption.KeyData.Options.SysReqPressed) ||
+        (CompareMem (&Hotkey->KeyData[Index].Key, &KeyOption.Keys[Index], sizeof (EFI_INPUT_KEY)) != 0)
+        ) {
+        //
+        // Break when any field doesn't match
+        //
+        Match = FALSE;
+        break;
+      }
+    }
+
+    if (Match) {
+      Link = RemoveEntryList (Link);
+      FreePool (Hotkey);
+    } else {
+      Link = GetNextNode (&mBmHotkeyList, Link);
+    }
+  }
+
+  //
+  // Delete the key option from the variable
+  //
+  Status     = EFI_NOT_FOUND;
+  KeyOptions = BmGetKeyOptions (&KeyOptionCount);
+  for (Index = 0; Index < KeyOptionCount; Index++) {
+    if ((KeyOptions[Index].KeyData.PackedValue == KeyOption.KeyData.PackedValue) &&
+        (CompareMem (
+           KeyOptions[Index].Keys, KeyOption.Keys,
+           KeyOption.KeyData.Options.InputKeyCount * sizeof (EFI_INPUT_KEY)) == 0)
+       ) {
+      UnicodeSPrint (KeyOptionName, sizeof (KeyOptionName), L"Key%04x", KeyOptions[Index].OptionNumber);
+      Status = gRT->SetVariable (
+                 KeyOptionName,
+                 &gEfiGlobalVariableGuid,
+                 EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                 0,
+                 NULL
+                 );
+      //
+      // Return the deleted key option in case needed by caller
+      //
+      if (DeletedOption != NULL) {
+        CopyMem (DeletedOption, &KeyOptions[Index], sizeof (EFI_BOOT_MANAGER_KEY_OPTION));
+      }
+      break;
+    }
+  }
+  BmFreeKeyOptions (KeyOptions, KeyOptionCount);
+
+  EfiReleaseLock (&mBmHotkeyLock);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmLoadOption.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmLoadOption.c
new file mode 100644
index 0000000000..6b9690a37c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmLoadOption.c
@@ -0,0 +1,1253 @@
+/** @file
+  Load option library functions which relate with creating and processing load options.
+
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+  CHAR16 *mBmLoadOptionName[] = {
+    L"Driver",
+    L"SysPrep",
+    L"Boot"
+  };
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+  CHAR16 *mBmLoadOptionOrderName[] = {
+    EFI_DRIVER_ORDER_VARIABLE_NAME,
+    EFI_SYS_PREP_ORDER_VARIABLE_NAME,
+    EFI_BOOT_ORDER_VARIABLE_NAME
+  };
+
+/**
+  Call Visitor function for each variable in variable storage.
+
+  @param Visitor  Visitor function.
+  @param Context  The context passed to Visitor function.
+**/
+VOID
+BmForEachVariable (
+  VARIABLE_VISITOR            Visitor,
+  VOID                        *Context
+  )
+{
+  EFI_STATUS                  Status;
+  CHAR16                      *Name;
+  EFI_GUID                    Guid;
+  UINTN                       NameSize;
+  UINTN                       NewNameSize;
+
+  NameSize = sizeof (CHAR16);
+  Name = AllocateZeroPool (NameSize);
+  ASSERT (Name != NULL);
+  while (TRUE) {
+    NewNameSize = NameSize;
+    Status = gRT->GetNextVariableName (&NewNameSize, Name, &Guid);
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      Name = ReallocatePool (NameSize, NewNameSize, Name);
+      ASSERT (Name != NULL);
+      Status = gRT->GetNextVariableName (&NewNameSize, Name, &Guid);
+      NameSize = NewNameSize;
+    }
+
+    if (Status == EFI_NOT_FOUND) {
+      break;
+    }
+    ASSERT_EFI_ERROR (Status);
+
+    Visitor (Name, &Guid, Context);
+  }
+
+  FreePool (Name);
+}
+
+/**
+  Get the Option Number that wasn't used.
+
+  @param  LoadOptionType      The load option type.
+  @param  FreeOptionNumber    Return the minimal free option number.
+
+  @retval EFI_SUCCESS           The option number is found and will be returned.
+  @retval EFI_OUT_OF_RESOURCES  There is no free option number that can be used.
+  @retval EFI_INVALID_PARAMETER FreeOptionNumber is NULL
+
+**/
+EFI_STATUS
+BmGetFreeOptionNumber (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE LoadOptionType,
+  OUT UINT16                            *FreeOptionNumber
+  )
+{
+  
+  UINTN         OptionNumber;
+  UINTN         Index;
+  UINT16        *OptionOrder;
+  UINTN         OptionOrderSize;
+  UINT16        *BootNext;
+
+  ASSERT (FreeOptionNumber != NULL);
+  ASSERT (LoadOptionType == LoadOptionTypeDriver || 
+          LoadOptionType == LoadOptionTypeBoot ||
+          LoadOptionType == LoadOptionTypeSysPrep);
+
+  GetEfiGlobalVariable2 (mBmLoadOptionOrderName[LoadOptionType], (VOID **) &OptionOrder, &OptionOrderSize);
+  ASSERT ((OptionOrder != NULL && OptionOrderSize != 0) || (OptionOrder == NULL && OptionOrderSize == 0));
+
+  BootNext = NULL;
+  if (LoadOptionType == LoadOptionTypeBoot) {
+    GetEfiGlobalVariable2 (L"BootNext", (VOID**) &BootNext, NULL);
+  }
+
+  for (OptionNumber = 0; 
+       OptionNumber < OptionOrderSize / sizeof (UINT16)
+                    + ((BootNext != NULL) ? 1 : 0); 
+       OptionNumber++
+       ) {
+    //
+    // Search in OptionOrder whether the OptionNumber exists
+    //
+    for (Index = 0; Index < OptionOrderSize / sizeof (UINT16); Index++) {
+      if (OptionNumber == OptionOrder[Index]) {
+        break;
+      }
+    }
+
+    //
+    // We didn't find it in the ****Order array and it doesn't equal to BootNext 
+    // Otherwise, OptionNumber equals to OptionOrderSize / sizeof (UINT16) + 1
+    //
+    if ((Index == OptionOrderSize / sizeof (UINT16)) && 
+        ((BootNext == NULL) || (OptionNumber != *BootNext))
+        ) {
+      break;
+    }
+  }
+  if (OptionOrder != NULL) {
+    FreePool (OptionOrder);
+  }
+
+  if (BootNext != NULL) {
+    FreePool (BootNext);
+  }
+
+  //
+  // When BootOrder & BootNext conver all numbers in the range [0 ... 0xffff],
+  //   OptionNumber equals to 0x10000 which is not valid.
+  //
+  ASSERT (OptionNumber <= 0x10000);
+  if (OptionNumber == 0x10000) {
+    return EFI_OUT_OF_RESOURCES;
+  } else {
+    *FreeOptionNumber = (UINT16) OptionNumber;
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Create the Boot####, Driver####, SysPrep####, variable from the load option.
+  
+  @param  LoadOption      Pointer to the load option.
+
+  @retval EFI_SUCCESS     The variable was created.
+  @retval Others          Error status returned by RT->SetVariable.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerLoadOptionToVariable (
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION     *Option
+  )
+{
+  UINTN                            VariableSize;
+  UINT8                            *Variable;
+  UINT8                            *Ptr;
+  CHAR16                           OptionName[BM_OPTION_NAME_LEN];
+  CHAR16                           *Description;
+  CHAR16                           NullChar;
+  UINT32                           VariableAttributes;
+
+  if ((Option->OptionNumber == LoadOptionNumberUnassigned) ||
+      (Option->FilePath == NULL) ||
+      ((UINT32) Option->OptionType >= LoadOptionTypeMax)
+     ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Convert NULL description to empty description
+  //
+  NullChar    = L'\0';
+  Description = Option->Description;
+  if (Description == NULL) {
+    Description = &NullChar;
+  }
+
+  /*
+  UINT32                      Attributes;
+  UINT16                      FilePathListLength;
+  CHAR16                      Description[];
+  EFI_DEVICE_PATH_PROTOCOL    FilePathList[];
+  UINT8                       OptionalData[];
+TODO: FilePathList[] IS:
+A packed array of UEFI device paths.  The first element of the 
+array is a device path that describes the device and location of the 
+Image for this load option.  The FilePathList[0] is specific 
+to the device type.  Other device paths may optionally exist in the 
+FilePathList, but their usage is OSV specific. Each element 
+in the array is variable length, and ends at the device path end 
+structure.
+  */
+  VariableSize = sizeof (Option->Attributes)
+               + sizeof (UINT16)
+               + StrSize (Description)
+               + GetDevicePathSize (Option->FilePath)
+               + Option->OptionalDataSize;
+
+  Variable     = AllocatePool (VariableSize);
+  ASSERT (Variable != NULL);
+
+  Ptr             = Variable;
+  WriteUnaligned32 ((UINT32 *) Ptr, Option->Attributes);
+  Ptr            += sizeof (Option->Attributes);
+
+  WriteUnaligned16 ((UINT16 *) Ptr, (UINT16) GetDevicePathSize (Option->FilePath));
+  Ptr            += sizeof (UINT16);
+
+  CopyMem (Ptr, Description, StrSize (Description));
+  Ptr            += StrSize (Description);
+
+  CopyMem (Ptr, Option->FilePath, GetDevicePathSize (Option->FilePath));
+  Ptr            += GetDevicePathSize (Option->FilePath);
+
+  CopyMem (Ptr, Option->OptionalData, Option->OptionalDataSize);
+
+  UnicodeSPrint (OptionName, sizeof (OptionName), L"%s%04x", mBmLoadOptionName[Option->OptionType], Option->OptionNumber);
+
+  VariableAttributes = EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE;
+
+  return gRT->SetVariable (
+                OptionName,
+                &gEfiGlobalVariableGuid,
+                VariableAttributes,
+                VariableSize,
+                Variable
+                );
+}
+
+/**
+  Update order variable .
+
+  @param  OptionOrderName     Order variable name which need to be updated.
+  @param  OptionNumber        Option number for the new option.
+  @param  Position            Position of the new load option to put in the ****Order variable.
+
+  @retval EFI_SUCCESS           The boot#### or driver#### have been successfully registered.
+  @retval EFI_ALREADY_STARTED   The option number of Option is being used already.
+  @retval EFI_STATUS            Return the status of gRT->SetVariable ().
+
+**/
+EFI_STATUS
+BmAddOptionNumberToOrderVariable (
+  IN CHAR16               *OptionOrderName,
+  IN UINT16               OptionNumber,
+  IN UINTN                Position
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  UINT16                  *OptionOrder;
+  UINT16                  *NewOptionOrder;
+  UINTN                   OptionOrderSize;
+  //
+  // Update the option order variable
+  //
+  GetEfiGlobalVariable2 (OptionOrderName, (VOID **) &OptionOrder, &OptionOrderSize);
+  ASSERT ((OptionOrder != NULL && OptionOrderSize != 0) || (OptionOrder == NULL && OptionOrderSize == 0));
+
+  Status = EFI_SUCCESS;
+  for (Index = 0; Index < OptionOrderSize / sizeof (UINT16); Index++) {
+    if (OptionOrder[Index] == OptionNumber) {
+      Status = EFI_ALREADY_STARTED;
+      break;
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    Position       = MIN (Position, OptionOrderSize / sizeof (UINT16));
+
+    NewOptionOrder = AllocatePool (OptionOrderSize + sizeof (UINT16));
+    ASSERT (NewOptionOrder != NULL);
+    if (OptionOrderSize != 0) {
+      CopyMem (NewOptionOrder, OptionOrder, Position * sizeof (UINT16));
+      CopyMem (&NewOptionOrder[Position + 1], &OptionOrder[Position], OptionOrderSize - Position * sizeof (UINT16));
+    }
+    NewOptionOrder[Position] = OptionNumber;
+
+    Status = gRT->SetVariable (
+                    OptionOrderName,
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    OptionOrderSize + sizeof (UINT16),
+                    NewOptionOrder
+                    );
+    FreePool (NewOptionOrder);
+  }
+
+  if (OptionOrder != NULL) {
+    FreePool (OptionOrder);
+  }
+
+  return Status;
+}
+
+/**
+  This function will register the new Boot####, Driver#### or SysPrep#### option.
+  After the *#### is updated, the *Order will also be updated.
+
+  @param  Option            Pointer to load option to add.
+  @param  Position          Position of the new load option to put in the ****Order variable.
+
+  @retval EFI_SUCCESS           The *#### have been successfully registered.
+  @retval EFI_INVALID_PARAMETER The option number exceeds 0xFFFF.
+  @retval EFI_ALREADY_STARTED   The option number of Option is being used already.
+                                Note: this API only adds new load option, no replacement support.
+  @retval EFI_OUT_OF_RESOURCES  There is no free option number that can be used when the
+                                option number specified in the Option is LoadOptionNumberUnassigned.
+  @retval EFI_STATUS            Return the status of gRT->SetVariable ().
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerAddLoadOptionVariable (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION *Option,
+  IN UINTN                        Position
+  )
+{
+  EFI_STATUS                      Status;
+  UINT16                          OptionNumber;
+
+  if (Option == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Option->OptionType != LoadOptionTypeDriver && 
+      Option->OptionType != LoadOptionTypeSysPrep &&
+      Option->OptionType != LoadOptionTypeBoot
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get the free option number if the option number is unassigned
+  //
+  if (Option->OptionNumber == LoadOptionNumberUnassigned) {
+    Status = BmGetFreeOptionNumber (Option->OptionType, &OptionNumber);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Option->OptionNumber = OptionNumber;
+  }
+
+  if (Option->OptionNumber >= LoadOptionNumberMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = BmAddOptionNumberToOrderVariable (mBmLoadOptionOrderName[Option->OptionType], (UINT16) Option->OptionNumber, Position);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Save the Boot#### or Driver#### variable
+    //
+    Status = EfiBootManagerLoadOptionToVariable (Option);
+    if (EFI_ERROR (Status)) {
+      //
+      // Remove the #### from *Order variable when the Driver####/SysPrep####/Boot#### cannot be saved.
+      //
+      EfiBootManagerDeleteLoadOptionVariable (Option->OptionNumber, Option->OptionType);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Sort the load option. The DriverOrder or BootOrder will be re-created to 
+  reflect the new order.
+
+  @param OptionType             Load option type
+  @param CompareFunction        The comparator
+**/
+VOID
+EFIAPI
+EfiBootManagerSortLoadOptionVariable (
+  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE        OptionType,
+  SORT_COMPARE                             CompareFunction
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION   *LoadOption;
+  UINTN                          LoadOptionCount;
+  UINTN                          Index;
+  UINT16                         *OptionOrder;
+
+  LoadOption = EfiBootManagerGetLoadOptions (&LoadOptionCount, OptionType);
+
+  //
+  // Insertion sort algorithm
+  //
+  PerformQuickSort (
+    LoadOption,
+    LoadOptionCount,
+    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION),
+    CompareFunction
+    );
+
+  //
+  // Create new ****Order variable
+  //
+  OptionOrder = AllocatePool (LoadOptionCount * sizeof (UINT16));
+  ASSERT (OptionOrder != NULL);
+  for (Index = 0; Index < LoadOptionCount; Index++) {
+    OptionOrder[Index] = (UINT16) LoadOption[Index].OptionNumber;
+  }
+
+  Status = gRT->SetVariable (
+                  mBmLoadOptionOrderName[OptionType],
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                  LoadOptionCount * sizeof (UINT16),
+                  OptionOrder
+                  );
+  //
+  // Changing the *Order content without increasing its size with current variable implementation shouldn't fail.
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  FreePool (OptionOrder);
+  EfiBootManagerFreeLoadOptions (LoadOption, LoadOptionCount);
+}
+
+/**
+  Initialize a load option.
+
+  @param Option           Pointer to the load option to be initialized.
+  @param OptionNumber     Option number of the load option.
+  @param OptionType       Type of the load option.
+  @param Attributes       Attributes of the load option.
+  @param Description      Description of the load option.
+  @param FilePath         Device path of the load option.
+  @param OptionalData     Optional data of the load option.
+  @param OptionalDataSize Size of the optional data of the load option.
+
+  @retval EFI_SUCCESS           The load option was initialized successfully.
+  @retval EFI_INVALID_PARAMETER Option, Description or FilePath is NULL.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerInitializeLoadOption (
+  IN OUT EFI_BOOT_MANAGER_LOAD_OPTION   *Option,
+  IN  UINTN                             OptionNumber,
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE OptionType,
+  IN  UINT32                            Attributes,
+  IN  CHAR16                            *Description,
+  IN  EFI_DEVICE_PATH_PROTOCOL          *FilePath,
+  IN  UINT8                             *OptionalData,   OPTIONAL
+  IN  UINT32                            OptionalDataSize
+  )
+{
+  if ((Option == NULL) || (Description == NULL) || (FilePath == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((OptionalData != NULL) && (OptionalDataSize == 0)) ||
+      ((OptionalData == NULL) && (OptionalDataSize != 0))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((UINT32) OptionType >= LoadOptionTypeMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (Option, sizeof (EFI_BOOT_MANAGER_LOAD_OPTION));
+  Option->OptionNumber       = OptionNumber;
+  Option->OptionType         = OptionType;
+  Option->Attributes         = Attributes;
+  Option->Description        = AllocateCopyPool (StrSize (Description), Description);
+  Option->FilePath           = DuplicateDevicePath (FilePath);
+  if (OptionalData != NULL) {
+    Option->OptionalData     = AllocateCopyPool (OptionalDataSize, OptionalData);
+    Option->OptionalDataSize = OptionalDataSize;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Return the index of the load option in the load option array.
+
+  The function consider two load options are equal when the 
+  OptionType, Attributes, Description, FilePath and OptionalData are equal.
+
+  @param Key    Pointer to the load option to be found.
+  @param Array  Pointer to the array of load options to be found.
+  @param Count  Number of entries in the Array.
+
+  @retval -1          Key wasn't found in the Array.
+  @retval 0 ~ Count-1 The index of the Key in the Array.
+**/
+INTN
+BmFindLoadOption (
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION *Key,
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION *Array,
+  IN UINTN                              Count
+  )
+{
+  UINTN                             Index;
+
+  for (Index = 0; Index < Count; Index++) {
+    if ((Key->OptionType == Array[Index].OptionType) &&
+        (Key->Attributes == Array[Index].Attributes) &&
+        (StrCmp (Key->Description, Array[Index].Description) == 0) &&
+        (CompareMem (Key->FilePath, Array[Index].FilePath, GetDevicePathSize (Key->FilePath)) == 0) &&
+        (Key->OptionalDataSize == Array[Index].OptionalDataSize) &&
+        (CompareMem (Key->OptionalData, Array[Index].OptionalData, Key->OptionalDataSize) == 0)) {
+      return (INTN) Index;
+    }
+  }
+
+  return -1;
+}
+
+/**
+  Delete the load option.
+
+  @param  OptionNumber        Indicate the option number of load option
+  @param  OptionType          Indicate the type of load option
+
+  @retval EFI_INVALID_PARAMETER OptionType or OptionNumber is invalid.
+  @retval EFI_NOT_FOUND         The load option cannot be found
+  @retval EFI_SUCCESS           The load option was deleted
+  @retval others                Status of RT->SetVariable()
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerDeleteLoadOptionVariable (
+  IN UINTN                              OptionNumber,
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE  OptionType
+  )
+{
+  UINT16                            *OptionOrder;
+  UINTN                             OptionOrderSize;
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+
+  if (((UINT32) OptionType >= LoadOptionTypeMax) || (OptionNumber >= LoadOptionNumberMax)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EFI_NOT_FOUND;
+
+  if (OptionType == LoadOptionTypeDriver || OptionType == LoadOptionTypeSysPrep || OptionType == LoadOptionTypeBoot) {
+    //
+    // If the associated *Order exists, just remove the reference in *Order.
+    //
+    GetEfiGlobalVariable2 (mBmLoadOptionOrderName[OptionType], (VOID **) &OptionOrder, &OptionOrderSize);
+    ASSERT ((OptionOrder != NULL && OptionOrderSize != 0) || (OptionOrder == NULL && OptionOrderSize == 0));
+
+    for (Index = 0; Index < OptionOrderSize / sizeof (UINT16); Index++) {
+      if (OptionOrder[Index] == OptionNumber) {
+        OptionOrderSize -= sizeof (UINT16);
+        CopyMem (&OptionOrder[Index], &OptionOrder[Index + 1], OptionOrderSize - Index * sizeof (UINT16));
+        Status = gRT->SetVariable (
+          mBmLoadOptionOrderName[OptionType],
+          &gEfiGlobalVariableGuid,
+          EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+          OptionOrderSize,
+          OptionOrder
+          );
+        break;
+      }
+    }
+    if (OptionOrder != NULL) {
+      FreePool (OptionOrder);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Convert a single character to number.
+  It assumes the input Char is in the scope of L'0' ~ L'9' and L'A' ~ L'F'
+
+  @param    Char   The input char which need to convert to int.
+**/
+UINTN
+BmCharToUint (
+  IN CHAR16                           Char
+  )
+{
+  if ((Char >= L'0') && (Char <= L'9')) {
+    return (UINTN) (Char - L'0');
+  }
+
+  if ((Char >= L'A') && (Char <= L'F')) {
+    return (UINTN) (Char - L'A' + 0xA);
+  }
+
+  ASSERT (FALSE);
+  return (UINTN) -1;
+}
+
+/**
+  Returns the size of a device path in bytes.
+
+  This function returns the size, in bytes, of the device path data structure 
+  specified by DevicePath including the end of device path node. If DevicePath 
+  is NULL, then 0 is returned. If the length of the device path is bigger than
+  MaxSize, also return 0 to indicate this is an invalidate device path.
+
+  @param  DevicePath         A pointer to a device path data structure.
+  @param  MaxSize            Max valid device path size. If big than this size, 
+                             return error.
+  
+  @retval 0                  An invalid device path.
+  @retval Others             The size of a device path in bytes.
+
+**/
+UINTN
+BmGetDevicePathSizeEx (
+  IN CONST EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN UINTN                           MaxSize
+  )
+{
+  UINTN  Size;
+  UINTN  NodeSize;
+
+  if (DevicePath == NULL) {
+    return 0;
+  }
+
+  //
+  // Search for the end of the device path structure
+  //
+  Size = 0;
+  while (!IsDevicePathEnd (DevicePath)) {
+    NodeSize = DevicePathNodeLength (DevicePath);
+    if (NodeSize == 0) {
+      return 0;
+    }
+    Size += NodeSize;
+    if (Size > MaxSize) {
+      return 0;
+    }
+    DevicePath = NextDevicePathNode (DevicePath);
+  }
+  Size += DevicePathNodeLength (DevicePath);
+  if (Size > MaxSize) {
+    return 0;
+  }
+
+  return Size;
+}
+
+/**
+  Returns the length of a Null-terminated Unicode string. If the length is 
+  bigger than MaxStringLen, return length 0 to indicate that this is an 
+  invalidate string.
+
+  This function returns the number of Unicode characters in the Null-terminated
+  Unicode string specified by String. 
+
+  If String is NULL, then ASSERT().
+  If String is not aligned on a 16-bit boundary, then ASSERT().
+
+  @param  String           A pointer to a Null-terminated Unicode string.
+  @param  MaxStringLen     Max string len in this string.
+
+  @retval 0                An invalid string.
+  @retval Others           The length of String.
+
+**/
+UINTN
+BmStrSizeEx (
+  IN      CONST CHAR16              *String,
+  IN      UINTN                     MaxStringLen
+  )
+{
+  UINTN                             Length;
+
+  ASSERT (String != NULL && MaxStringLen != 0);
+  ASSERT (((UINTN) String & BIT0) == 0);
+
+  for (Length = 0; *String != L'\0' && MaxStringLen != Length; String++, Length+=2);
+
+  if (*String != L'\0' && MaxStringLen == Length) {
+    return 0;
+  }
+
+  return Length + 2;
+}
+
+/**
+  Validate the Boot####, Driver####, SysPrep#### variable (VendorGuid/Name)
+
+  @param  Variable              The variable data.
+  @param  VariableSize          The variable size.
+
+  @retval TRUE                  The variable data is correct.
+  @retval FALSE                 The variable data is corrupted.
+
+**/
+BOOLEAN 
+BmValidateOption (
+  UINT8                     *Variable,
+  UINTN                     VariableSize
+  )
+{
+  UINT16                    FilePathSize;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINTN                     DescriptionSize;
+
+  if (VariableSize <= sizeof (UINT16) + sizeof (UINT32)) {
+    return FALSE;
+  }
+
+  //
+  // Skip the option attribute
+  //
+  Variable += sizeof (UINT32);
+
+  //
+  // Get the option's device path size
+  //
+  FilePathSize = ReadUnaligned16 ((UINT16 *) Variable);
+  Variable += sizeof (UINT16);
+
+  //
+  // Get the option's description string size
+  //
+  DescriptionSize = BmStrSizeEx ((CHAR16 *) Variable, VariableSize - sizeof (UINT16) - sizeof (UINT32));
+  Variable += DescriptionSize;
+
+  //
+  // Get the option's device path
+  //
+  DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) Variable;
+
+  //
+  // Validation boot option variable.
+  //
+  if ((FilePathSize == 0) || (DescriptionSize == 0)) {
+    return FALSE;
+  }
+
+  if (sizeof (UINT32) + sizeof (UINT16) + DescriptionSize + FilePathSize > VariableSize) {
+    return FALSE;
+  }
+
+  return (BOOLEAN) (BmGetDevicePathSizeEx (DevicePath, FilePathSize) != 0);
+}
+
+/**
+  Check whether the VariableName is a valid load option variable name
+  and return the load option type and option number.
+
+  @param VariableName The name of the load option variable.
+  @param OptionType   Return the load option type.
+  @param OptionNumber Return the load option number.
+
+  @retval TRUE  The variable name is valid; The load option type and
+                load option number is returned.
+  @retval FALSE The variable name is NOT valid.
+**/
+BOOLEAN
+BmIsValidLoadOptionVariableName (
+  IN CHAR16                             *VariableName,
+  OUT EFI_BOOT_MANAGER_LOAD_OPTION_TYPE *OptionType,
+  OUT UINT16                            *OptionNumber
+  )
+{
+  UINTN                             VariableNameLen;
+  UINTN                             Index;
+  UINTN                             Uint;
+
+  VariableNameLen = StrLen (VariableName);
+
+  if (VariableNameLen <= 4) {
+    return FALSE;
+  }
+
+  for (Index = 0; Index < sizeof (mBmLoadOptionName) / sizeof (mBmLoadOptionName[0]); Index++) {
+    if ((VariableNameLen - 4 == StrLen (mBmLoadOptionName[Index])) &&
+        (StrnCmp (VariableName, mBmLoadOptionName[Index], VariableNameLen - 4) == 0)
+        ) {
+      break;
+    }
+  }
+
+  if (Index == sizeof (mBmLoadOptionName) / sizeof (mBmLoadOptionName[0])) {
+    return FALSE;
+  }
+
+  *OptionType = (EFI_BOOT_MANAGER_LOAD_OPTION_TYPE) Index;
+  *OptionNumber = 0;
+  for (Index = VariableNameLen - 4; Index < VariableNameLen; Index++) {
+    Uint = BmCharToUint (VariableName[Index]);
+    if (Uint == -1) {
+      break;
+    } else {
+      *OptionNumber = (UINT16) Uint + *OptionNumber * 0x10;
+    }
+  }
+
+  return (BOOLEAN) (Index == VariableNameLen);
+}
+
+/**
+  Build the Boot#### or Driver#### option from the VariableName.
+
+  @param  VariableName          Variable name of the load option
+  @param  VendorGuid            Variable GUID of the load option
+  @param  Option                Return the load option.
+
+  @retval EFI_SUCCESS     Get the option just been created
+  @retval EFI_NOT_FOUND   Failed to get the new option
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerVariableToLoadOptionEx (
+  IN CHAR16                           *VariableName,
+  IN EFI_GUID                         *VendorGuid,
+  IN OUT EFI_BOOT_MANAGER_LOAD_OPTION *Option  
+  )
+{
+  EFI_STATUS                         Status;
+  UINT32                             Attribute;
+  UINT16                             FilePathSize;
+  UINT8                              *Variable;
+  UINT8                              *VariablePtr;
+  UINTN                              VariableSize;
+  EFI_DEVICE_PATH_PROTOCOL           *FilePath;
+  UINT8                              *OptionalData;
+  UINT32                             OptionalDataSize;
+  CHAR16                             *Description;
+  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE  OptionType;
+  UINT16                             OptionNumber;
+
+  if ((VariableName == NULL) || (Option == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!BmIsValidLoadOptionVariableName (VariableName, &OptionType, &OptionNumber)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Read the variable
+  //
+  GetVariable2 (VariableName, VendorGuid, (VOID **) &Variable, &VariableSize);
+  if (Variable == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Validate *#### variable data.
+  //
+  if (!BmValidateOption(Variable, VariableSize)) {
+    FreePool (Variable);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get the option attribute
+  //
+  VariablePtr = Variable;
+  Attribute = ReadUnaligned32 ((UINT32 *) VariablePtr);
+  VariablePtr += sizeof (UINT32);
+
+  //
+  // Get the option's device path size
+  //
+  FilePathSize = ReadUnaligned16 ((UINT16 *) VariablePtr);
+  VariablePtr += sizeof (UINT16);
+
+  //
+  // Get the option's description string
+  //
+  Description = (CHAR16 *) VariablePtr;
+
+  //
+  // Get the option's description string size
+  //
+  VariablePtr += StrSize ((CHAR16 *) VariablePtr);
+
+  //
+  // Get the option's device path
+  //
+  FilePath = (EFI_DEVICE_PATH_PROTOCOL *) VariablePtr;
+  VariablePtr += FilePathSize;
+
+  OptionalDataSize = (UINT32) (VariableSize - (UINTN) (VariablePtr - Variable));
+  if (OptionalDataSize == 0) {
+    OptionalData = NULL;
+  } else {
+    OptionalData = VariablePtr;
+  }
+
+  Status = EfiBootManagerInitializeLoadOption (
+             Option,
+             OptionNumber,
+             OptionType,
+             Attribute,
+             Description,
+             FilePath,
+             OptionalData,
+             OptionalDataSize
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  CopyGuid (&Option->VendorGuid, VendorGuid);
+
+  FreePool (Variable);
+  return Status;
+}
+
+/**
+Build the Boot#### or Driver#### option from the VariableName.
+
+@param  VariableName          EFI Variable name indicate if it is Boot#### or Driver####
+@param  Option                Return the Boot#### or Driver#### option.
+
+@retval EFI_SUCCESS     Get the option just been created
+@retval EFI_NOT_FOUND   Failed to get the new option
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerVariableToLoadOption (
+  IN  CHAR16                          *VariableName,
+  IN OUT EFI_BOOT_MANAGER_LOAD_OPTION *Option
+  )
+{
+  return EfiBootManagerVariableToLoadOptionEx (VariableName, &gEfiGlobalVariableGuid, Option);
+}
+
+/**
+  Returns an array of load options based on the EFI variable
+  L"BootOrder"/L"DriverOrder" and the L"Boot####"/L"Driver####" variables impled by it.
+  #### is the hex value of the UINT16 in each BootOrder/DriverOrder entry. 
+
+  @param  LoadOptionCount   Returns number of entries in the array.
+  @param  LoadOptionType    The type of the load option.
+
+  @retval NULL  No load options exist.
+  @retval !NULL Array of load option entries.
+
+**/
+EFI_BOOT_MANAGER_LOAD_OPTION *
+EFIAPI
+EfiBootManagerGetLoadOptions (
+  OUT UINTN                             *OptionCount,
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE  LoadOptionType
+  )
+{
+  EFI_STATUS                   Status;
+  UINT16                       *OptionOrder;
+  UINTN                        OptionOrderSize;
+  UINTN                        Index;
+  UINTN                        OptionIndex;
+  EFI_BOOT_MANAGER_LOAD_OPTION *Options;
+  CHAR16                       OptionName[BM_OPTION_NAME_LEN];
+  UINT16                       OptionNumber;
+
+  *OptionCount = 0;
+
+  if (LoadOptionType == LoadOptionTypeDriver || LoadOptionType == LoadOptionTypeSysPrep || LoadOptionType == LoadOptionTypeBoot) {
+    //
+    // Read the BootOrder, or DriverOrder variable.
+    //
+    GetEfiGlobalVariable2 (mBmLoadOptionOrderName[LoadOptionType], (VOID **) &OptionOrder, &OptionOrderSize);
+    if (OptionOrder == NULL) {
+      return NULL;
+    }
+
+    *OptionCount = OptionOrderSize / sizeof (UINT16);
+
+    Options = AllocatePool (*OptionCount * sizeof (EFI_BOOT_MANAGER_LOAD_OPTION));
+    ASSERT (Options != NULL);
+
+    OptionIndex = 0;
+    for (Index = 0; Index < *OptionCount; Index++) {
+      OptionNumber = OptionOrder[Index];
+      UnicodeSPrint (OptionName, sizeof (OptionName), L"%s%04x", mBmLoadOptionName[LoadOptionType], OptionNumber);
+
+      Status = EfiBootManagerVariableToLoadOption (OptionName, &Options[OptionIndex]);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "[Bds] %s doesn't exist - Update ****Order variable to remove the reference!!", OptionName));
+        EfiBootManagerDeleteLoadOptionVariable (OptionNumber, LoadOptionType);
+      } else {
+        ASSERT (Options[OptionIndex].OptionNumber == OptionNumber);
+        OptionIndex++;
+      }
+    }
+
+    if (OptionOrder != NULL) {
+      FreePool (OptionOrder);
+    }
+
+    if (OptionIndex < *OptionCount) {
+      Options = ReallocatePool (*OptionCount * sizeof (EFI_BOOT_MANAGER_LOAD_OPTION), OptionIndex * sizeof (EFI_BOOT_MANAGER_LOAD_OPTION), Options);
+      ASSERT (Options != NULL);
+      *OptionCount = OptionIndex;
+    }
+
+  } else {
+    return NULL;
+  }
+
+  return Options;
+}
+
+/**
+  Free an EFI_BOOT_MANGER_LOAD_OPTION entry that was allocate by the library.
+
+  @param  LoadOption   Pointer to boot option to Free.
+
+  @return EFI_SUCCESS   BootOption was freed 
+  @return EFI_NOT_FOUND BootOption == NULL 
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeLoadOption (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *LoadOption
+  )
+{
+  if (LoadOption == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (LoadOption->Description != NULL) {
+    FreePool (LoadOption->Description);
+  }
+  if (LoadOption->FilePath != NULL) {
+    FreePool (LoadOption->FilePath);
+  }
+  if (LoadOption->OptionalData != NULL) {
+    FreePool (LoadOption->OptionalData);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Free an EFI_BOOT_MANGER_LOAD_OPTION array that was allocated by 
+  EfiBootManagerGetLoadOptions().
+
+  @param  Option       Pointer to boot option array to free.
+  @param  OptionCount  Number of array entries in BootOption
+
+  @return EFI_SUCCESS   BootOption was freed 
+  @return EFI_NOT_FOUND BootOption == NULL 
+
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerFreeLoadOptions (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION  *Option,
+  IN  UINTN                         OptionCount
+  )
+{
+  UINTN   Index;
+
+  if (Option == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  for (Index = 0;Index < OptionCount; Index++) {
+    EfiBootManagerFreeLoadOption (&Option[Index]);
+  }
+
+  FreePool (Option);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Return whether the PE header of the load option is valid or not.
+
+  @param[in] Type       The load option type.
+  @param[in] FileBuffer The PE file buffer of the load option.
+  @param[in] FileSize   The size of the load option file.
+
+  @retval TRUE  The PE header of the load option is valid.
+  @retval FALSE The PE header of the load option is not valid.
+**/
+BOOLEAN
+BmIsLoadOptionPeHeaderValid (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE Type,
+  IN VOID                              *FileBuffer,
+  IN UINTN                             FileSize
+  )
+{
+  EFI_IMAGE_DOS_HEADER              *DosHeader;
+  EFI_IMAGE_OPTIONAL_HEADER_UNION   *PeHeader;
+  EFI_IMAGE_OPTIONAL_HEADER32       *OptionalHeader;
+  UINT16                            Subsystem;
+
+  if (FileBuffer == NULL || FileSize == 0) {
+    return FALSE;
+  }
+
+  //
+  // Read dos header
+  //
+  DosHeader = (EFI_IMAGE_DOS_HEADER *) FileBuffer;
+  if (FileSize >= sizeof (EFI_IMAGE_DOS_HEADER) &&
+      FileSize > DosHeader->e_lfanew && DosHeader->e_magic == EFI_IMAGE_DOS_SIGNATURE
+      ) {
+    //
+    // Read and check PE signature
+    //
+    PeHeader = (EFI_IMAGE_OPTIONAL_HEADER_UNION *) ((UINT8 *) FileBuffer + DosHeader->e_lfanew);
+    if (FileSize >= DosHeader->e_lfanew + sizeof (EFI_IMAGE_OPTIONAL_HEADER_UNION) &&
+        PeHeader->Pe32.Signature == EFI_IMAGE_NT_SIGNATURE
+        ) {
+      //
+      // Check PE32 or PE32+ magic, and machine type
+      //
+      OptionalHeader = (EFI_IMAGE_OPTIONAL_HEADER32 *) &PeHeader->Pe32.OptionalHeader;
+      if ((OptionalHeader->Magic == EFI_IMAGE_NT_OPTIONAL_HDR32_MAGIC || 
+           OptionalHeader->Magic == EFI_IMAGE_NT_OPTIONAL_HDR64_MAGIC) &&
+          EFI_IMAGE_MACHINE_TYPE_SUPPORTED (PeHeader->Pe32.FileHeader.Machine)
+          ) {
+        //
+        // Check the Subsystem:
+        //   Driver#### must be of type BootServiceDriver or RuntimeDriver
+        //   SysPrep####, Boot####, OsRecovery####, PlatformRecovery#### must be of type Application
+        //
+        Subsystem = OptionalHeader->Subsystem;
+        if ((Type == LoadOptionTypeDriver && Subsystem == EFI_IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER) ||
+            (Type == LoadOptionTypeDriver && Subsystem == EFI_IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER) ||
+            (Type == LoadOptionTypeSysPrep && Subsystem == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION) ||
+            (Type == LoadOptionTypeBoot && Subsystem == EFI_IMAGE_SUBSYSTEM_EFI_APPLICATION)
+            ) {
+          return TRUE;
+        }
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Process (load and execute) the load option.
+
+  @param LoadOption  Pointer to the load option.
+
+  @retval EFI_INVALID_PARAMETER  The load option type is invalid, 
+                                 or the load option file path doesn't point to a valid file.
+  @retval EFI_UNSUPPORTED        The load option type is of LoadOptionTypeBoot.
+  @retval EFI_SUCCESS            The load option is inactive, or successfully loaded and executed.
+**/
+EFI_STATUS
+EFIAPI
+EfiBootManagerProcessLoadOption (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION       *LoadOption
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *FilePath;
+  EFI_HANDLE                        ImageHandle;
+  EFI_LOADED_IMAGE_PROTOCOL         *ImageInfo;
+  VOID                              *FileBuffer;
+  UINTN                             FileSize;
+
+  if ((UINT32) LoadOption->OptionType >= LoadOptionTypeMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (LoadOption->OptionType == LoadOptionTypeBoot) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // If a load option is not marked as LOAD_OPTION_ACTIVE,
+  // the boot manager will not automatically load the option.
+  //
+  if ((LoadOption->Attributes & LOAD_OPTION_ACTIVE) == 0) {
+    return EFI_SUCCESS;
+  }
+
+  Status = EFI_INVALID_PARAMETER;
+
+  //
+  // Load and start the load option.
+  //
+  DEBUG ((
+    DEBUG_INFO | DEBUG_LOAD, "Process Load Option (%s%04x) ...\n",
+    mBmLoadOptionName[LoadOption->OptionType], LoadOption->OptionNumber
+    ));
+  ImageHandle = NULL;
+  FileBuffer = BmGetLoadOptionBuffer (LoadOption->FilePath, &FilePath, &FileSize);
+  DEBUG_CODE (
+    if (FileBuffer != NULL && CompareMem (LoadOption->FilePath, FilePath, GetDevicePathSize (FilePath)) != 0) {
+      DEBUG ((EFI_D_INFO, "[Bds] DevicePath expand: "));
+      BmPrintDp (LoadOption->FilePath);
+      DEBUG ((EFI_D_INFO, " -> "));
+      BmPrintDp (FilePath);
+      DEBUG ((EFI_D_INFO, "\n"));
+    }
+  );
+  if (BmIsLoadOptionPeHeaderValid (LoadOption->OptionType, FileBuffer, FileSize)) {
+    Status = gBS->LoadImage (
+                    FALSE,
+                    gImageHandle,
+                    FilePath,
+                    FileBuffer,
+                    FileSize,
+                    &ImageHandle
+                    );
+  }
+  if (FilePath != NULL) {
+    FreePool (FilePath);
+  }
+  if (FileBuffer != NULL) {
+    FreePool (FileBuffer);
+  }
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->HandleProtocol (ImageHandle, &gEfiLoadedImageProtocolGuid, (VOID **) &ImageInfo);
+    ASSERT_EFI_ERROR (Status);
+
+    ImageInfo->LoadOptionsSize = LoadOption->OptionalDataSize;
+    ImageInfo->LoadOptions = LoadOption->OptionalData;
+    //
+    // Before calling the image, enable the Watchdog Timer for the 5-minute period
+    //
+    gBS->SetWatchdogTimer (5 * 60, 0, 0, NULL);
+
+    LoadOption->Status = gBS->StartImage (ImageHandle, &LoadOption->ExitDataSize, &LoadOption->ExitData);
+    DEBUG ((
+      DEBUG_INFO | DEBUG_LOAD, "Load Option (%s%04x) Return Status = %r\n",
+      mBmLoadOptionName[LoadOption->OptionType], LoadOption->OptionNumber, LoadOption->Status
+      ));
+
+    //
+    // Clear the Watchdog Timer after the image returns
+    //
+    gBS->SetWatchdogTimer (0, 0, 0, NULL);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmMisc.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmMisc.c
new file mode 100644
index 0000000000..e675904857
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmMisc.c
@@ -0,0 +1,391 @@
+/** @file
+  Misc library functions.
+
+Copyright (c) 2011 - 2015, 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 "InternalBm.h"
+
+/**
+  Delete the instance in Multi which matches partly with Single instance
+
+  @param  Multi                 A pointer to a multi-instance device path data
+                                structure.
+  @param  Single                A pointer to a single-instance device path data
+                                structure.
+
+  @return This function will remove the device path instances in Multi which partly
+          match with the Single, and return the result device path. If there is no
+          remaining device path as a result, this function will return NULL.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+BmDelPartMatchInstance (
+  IN     EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN     EFI_DEVICE_PATH_PROTOCOL  *Single
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *Instance;
+  EFI_DEVICE_PATH_PROTOCOL  *NewDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *TempNewDevicePath;
+  UINTN                     InstanceSize;
+  UINTN                     SingleDpSize;
+
+  NewDevicePath     = NULL;
+  TempNewDevicePath = NULL;
+
+  if (Multi == NULL || Single == NULL) {
+    return Multi;
+  }
+
+  Instance        = GetNextDevicePathInstance (&Multi, &InstanceSize);
+  SingleDpSize    = GetDevicePathSize (Single) - END_DEVICE_PATH_LENGTH;
+  InstanceSize   -= END_DEVICE_PATH_LENGTH;
+
+  while (Instance != NULL) {
+
+    if (CompareMem (Instance, Single, MIN (SingleDpSize, InstanceSize)) != 0) {
+      //
+      // Append the device path instance which does not match with Single
+      //
+      TempNewDevicePath = NewDevicePath;
+      NewDevicePath = AppendDevicePathInstance (NewDevicePath, Instance);
+      if (TempNewDevicePath != NULL) {
+        FreePool(TempNewDevicePath);
+      }
+    }
+    FreePool(Instance);
+    Instance      = GetNextDevicePathInstance (&Multi, &InstanceSize);
+    InstanceSize -= END_DEVICE_PATH_LENGTH;
+  }
+
+  return NewDevicePath;
+}
+
+/**
+  Function compares a device path data structure to that of all the nodes of a
+  second device path instance.
+
+  @param  Multi                 A pointer to a multi-instance device path data
+                                structure.
+  @param  Single                A pointer to a single-instance device path data
+                                structure.
+
+  @retval TRUE                  If the Single device path is contained within Multi device path.
+  @retval FALSE                 The Single device path is not match within Multi device path.
+
+**/
+BOOLEAN
+BmMatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathInst;
+  UINTN                     Size;
+
+  if (Multi == NULL || Single  == NULL) {
+    return FALSE;
+  }
+
+  DevicePath     = Multi;
+  DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+
+  //
+  // Search for the match of 'Single' in 'Multi'
+  //
+  while (DevicePathInst != NULL) {
+    //
+    // If the single device path is found in multiple device paths,
+    // return success
+    //
+    if (CompareMem (Single, DevicePathInst, Size) == 0) {
+      FreePool (DevicePathInst);
+      return TRUE;
+    }
+
+    FreePool (DevicePathInst);
+    DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+  }
+
+  return FALSE;
+}
+
+/**
+  This routine adjust the memory information for different memory type and 
+  save them into the variables for next boot. It resets the system when
+  memory information is updated and the current boot option belongs to
+  boot category instead of application category.
+
+  @param Boot  TRUE if current boot option belongs to boot category instead of
+               application category.
+**/
+VOID
+BmSetMemoryTypeInformationVariable (
+  IN BOOLEAN                    Boot
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_MEMORY_TYPE_INFORMATION  *PreviousMemoryTypeInformation;
+  EFI_MEMORY_TYPE_INFORMATION  *CurrentMemoryTypeInformation;
+  UINTN                        VariableSize;
+  UINTN                        Index;
+  UINTN                        Index1;
+  UINT32                       Previous;
+  UINT32                       Current;
+  UINT32                       Next;
+  EFI_HOB_GUID_TYPE            *GuidHob;
+  BOOLEAN                      MemoryTypeInformationModified;
+  BOOLEAN                      MemoryTypeInformationVariableExists;
+  EFI_BOOT_MODE                BootMode;
+
+  MemoryTypeInformationModified       = FALSE;
+  MemoryTypeInformationVariableExists = FALSE;
+
+
+  BootMode = GetBootModeHob ();
+  //
+  // In BOOT_IN_RECOVERY_MODE, Variable region is not reliable.
+  //
+  if (BootMode == BOOT_IN_RECOVERY_MODE) {
+    return;
+  }
+
+  //
+  // Only check the the Memory Type Information variable in the boot mode 
+  // other than BOOT_WITH_DEFAULT_SETTINGS because the Memory Type
+  // Information is not valid in this boot mode.
+  //
+  if (BootMode != BOOT_WITH_DEFAULT_SETTINGS) {
+    VariableSize = 0;
+    Status = gRT->GetVariable (
+                    EFI_MEMORY_TYPE_INFORMATION_VARIABLE_NAME,
+                    &gEfiMemoryTypeInformationGuid,
+                    NULL, 
+                    &VariableSize, 
+                    NULL
+                    );
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      MemoryTypeInformationVariableExists = TRUE;
+    }
+  }
+
+  //
+  // Retrieve the current memory usage statistics.  If they are not found, then
+  // no adjustments can be made to the Memory Type Information variable.
+  //
+  Status = EfiGetSystemConfigurationTable (
+             &gEfiMemoryTypeInformationGuid,
+             (VOID **) &CurrentMemoryTypeInformation
+             );
+  if (EFI_ERROR (Status) || CurrentMemoryTypeInformation == NULL) {
+    return;
+  }
+
+  //
+  // Get the Memory Type Information settings from Hob if they exist,
+  // PEI is responsible for getting them from variable and build a Hob to save them.
+  // If the previous Memory Type Information is not available, then set defaults
+  //
+  GuidHob = GetFirstGuidHob (&gEfiMemoryTypeInformationGuid);
+  if (GuidHob == NULL) {
+    //
+    // If Platform has not built Memory Type Info into the Hob, just return.
+    //
+    return;
+  }
+  PreviousMemoryTypeInformation = GET_GUID_HOB_DATA (GuidHob);
+  VariableSize = GET_GUID_HOB_DATA_SIZE (GuidHob);
+
+  //
+  // Use a heuristic to adjust the Memory Type Information for the next boot
+  //
+  DEBUG ((EFI_D_INFO, "Memory  Previous  Current    Next   \n"));
+  DEBUG ((EFI_D_INFO, " Type    Pages     Pages     Pages  \n"));
+  DEBUG ((EFI_D_INFO, "======  ========  ========  ========\n"));
+
+  for (Index = 0; PreviousMemoryTypeInformation[Index].Type != EfiMaxMemoryType; Index++) {
+
+    for (Index1 = 0; CurrentMemoryTypeInformation[Index1].Type != EfiMaxMemoryType; Index1++) {
+      if (PreviousMemoryTypeInformation[Index].Type == CurrentMemoryTypeInformation[Index1].Type) {
+        break;
+      }
+    }
+    if (CurrentMemoryTypeInformation[Index1].Type == EfiMaxMemoryType) {
+      continue;
+    }
+
+    //
+    // Previous is the number of pages pre-allocated
+    // Current is the number of pages actually needed
+    //
+    Previous = PreviousMemoryTypeInformation[Index].NumberOfPages;
+    Current  = CurrentMemoryTypeInformation[Index1].NumberOfPages;
+    Next     = Previous;
+
+    //
+    // Inconsistent Memory Reserved across bootings may lead to S4 fail
+    // Write next varible to 125% * current when the pre-allocated memory is:
+    //  1. More than 150% of needed memory and boot mode is BOOT_WITH_DEFAULT_SETTING
+    //  2. Less than the needed memory
+    //
+    if ((Current + (Current >> 1)) < Previous) {
+      if (BootMode == BOOT_WITH_DEFAULT_SETTINGS) {
+        Next = Current + (Current >> 2);
+      }
+    } else if (Current > Previous) {
+      Next = Current + (Current >> 2);
+    }
+    if (Next > 0 && Next < 4) {
+      Next = 4;
+    }
+
+    if (Next != Previous) {
+      PreviousMemoryTypeInformation[Index].NumberOfPages = Next;
+      MemoryTypeInformationModified = TRUE;
+    }
+
+    DEBUG ((EFI_D_INFO, "  %02x    %08x  %08x  %08x\n", PreviousMemoryTypeInformation[Index].Type, Previous, Current, Next));
+  }
+
+  //
+  // If any changes were made to the Memory Type Information settings, then set the new variable value;
+  // Or create the variable in first boot.
+  //
+  if (MemoryTypeInformationModified || !MemoryTypeInformationVariableExists) {
+    Status = BmSetVariableAndReportStatusCodeOnError (
+               EFI_MEMORY_TYPE_INFORMATION_VARIABLE_NAME,
+               &gEfiMemoryTypeInformationGuid,
+               EFI_VARIABLE_NON_VOLATILE  | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+               VariableSize,
+               PreviousMemoryTypeInformation
+               );
+
+    if (!EFI_ERROR (Status)) {
+      //
+      // If the Memory Type Information settings have been modified and the boot option belongs to boot category,
+      // then reset the platform so the new Memory Type Information setting will be used to guarantee that an S4
+      // entry/resume cycle will not fail.
+      //
+      if (MemoryTypeInformationModified && Boot && PcdGetBool (PcdResetOnMemoryTypeInformationChange)) {
+        DEBUG ((EFI_D_INFO, "Memory Type Information settings change. Warm Reset!!!\n"));
+        gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+      }
+    } else {
+      DEBUG ((EFI_D_ERROR, "Memory Type Information settings cannot be saved. OS S4 may fail!\n"));
+    }
+  }
+}
+
+/**
+  Set the variable and report the error through status code upon failure.
+
+  @param  VariableName           A Null-terminated string that is the name of the vendor's variable.
+                                 Each VariableName is unique for each VendorGuid. VariableName must
+                                 contain 1 or more characters. If VariableName is an empty string,
+                                 then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             Attributes bitmask to set for the variable.
+  @param  DataSize               The size in bytes of the Data buffer. Unless the EFI_VARIABLE_APPEND_WRITE, 
+                                 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS, or 
+                                 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS attribute is set, a size of zero 
+                                 causes the variable to be deleted. When the EFI_VARIABLE_APPEND_WRITE attribute is 
+                                 set, then a SetVariable() call with a DataSize of zero will not cause any change to 
+                                 the variable value (the timestamp associated with the variable may be updated however 
+                                 even if no new data value is provided,see the description of the 
+                                 EFI_VARIABLE_AUTHENTICATION_2 descriptor below. In this case the DataSize will not 
+                                 be zero since the EFI_VARIABLE_AUTHENTICATION_2 descriptor will be populated). 
+  @param  Data                   The contents for the variable.
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits, name, and GUID was supplied, or the
+                                 DataSize exceeds the maximum allowed.
+  @retval EFI_INVALID_PARAMETER  VariableName is an empty string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be retrieved due to a hardware error.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only.
+  @retval EFI_WRITE_PROTECTED    The variable in question cannot be deleted.
+  @retval EFI_SECURITY_VIOLATION The variable could not be written due to EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS 
+                                 or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS being set, but the AuthInfo 
+                                 does NOT pass the validation check carried out by the firmware.
+
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+**/
+EFI_STATUS
+BmSetVariableAndReportStatusCodeOnError (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  EFI_STATUS                 Status;
+  EDKII_SET_VARIABLE_STATUS  *SetVariableStatus;
+  UINTN                      NameSize;
+
+  Status = gRT->SetVariable (
+                  VariableName,
+                  VendorGuid,
+                  Attributes,
+                  DataSize,
+                  Data
+                  );
+  if (EFI_ERROR (Status)) {
+    NameSize = StrSize (VariableName);
+    SetVariableStatus = AllocatePool (sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + DataSize);
+    if (SetVariableStatus != NULL) {
+      CopyGuid (&SetVariableStatus->Guid, VendorGuid);
+      SetVariableStatus->NameSize   = NameSize;
+      SetVariableStatus->DataSize   = DataSize;
+      SetVariableStatus->SetStatus  = Status;
+      SetVariableStatus->Attributes = Attributes;
+      CopyMem (SetVariableStatus + 1,                          VariableName, NameSize);
+      CopyMem (((UINT8 *) (SetVariableStatus + 1)) + NameSize, Data,         DataSize);
+
+      REPORT_STATUS_CODE_EX (
+        EFI_ERROR_CODE,
+        PcdGet32 (PcdErrorCodeSetVariable),
+        0,
+        NULL,
+        &gEdkiiStatusCodeDataTypeVariableGuid,
+        SetVariableStatus,
+        sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + DataSize
+        );
+
+      FreePool (SetVariableStatus);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Print the device path info.
+
+  @param DevicePath           The device path need to print.
+**/
+VOID
+BmPrintDp (
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath
+  )
+{
+  CHAR16                              *Str;
+
+  Str = ConvertDevicePathToText (DevicePath, FALSE, FALSE);
+  DEBUG ((EFI_D_INFO, "%s", Str));
+  if (Str != NULL) {
+    FreePool (Str);
+  }
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/BmPerformance.c b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmPerformance.c
new file mode 100644
index 0000000000..0abd019440
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/BmPerformance.c
@@ -0,0 +1,317 @@
+/** @file
+  This file include the file which can help to get the system
+  performance, all the function will only include if the performance
+  switch is set.
+
+Copyright (c) 2004 - 2015, 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 "InternalBm.h"
+
+PERF_HEADER               mBmPerfHeader;
+PERF_DATA                 mBmPerfData;
+EFI_PHYSICAL_ADDRESS      mBmAcpiLowMemoryBase = 0x0FFFFFFFFULL;
+
+/**
+  Get the short verion of PDB file name to be
+  used in performance data logging.
+
+  @param PdbFileName     The long PDB file name.
+  @param GaugeString     The output string to be logged by performance logger.
+  @param StringSize      The buffer size of GaugeString in bytes.
+
+**/
+VOID
+BmGetShortPdbFileName (
+  IN  CONST CHAR8  *PdbFileName,
+  OUT       CHAR8  *GaugeString,
+  IN        UINTN   StringSize
+  )
+{
+  UINTN Index;
+  UINTN Index1;
+  UINTN StartIndex;
+  UINTN EndIndex;
+
+  if (PdbFileName == NULL) {
+    AsciiStrCpyS (GaugeString, StringSize, " ");
+  } else {
+    StartIndex = 0;
+    for (EndIndex = 0; PdbFileName[EndIndex] != 0; EndIndex++)
+      ;
+
+    for (Index = 0; PdbFileName[Index] != 0; Index++) {
+      if (PdbFileName[Index] == '\\') {
+        StartIndex = Index + 1;
+      }
+
+      if (PdbFileName[Index] == '.') {
+        EndIndex = Index;
+      }
+    }
+
+    Index1 = 0;
+    for (Index = StartIndex; Index < EndIndex; Index++) {
+      GaugeString[Index1] = PdbFileName[Index];
+      Index1++;
+      if (Index1 == StringSize - 1) {
+        break;
+      }
+    }
+
+    GaugeString[Index1] = 0;
+  }
+
+  return ;
+}
+
+/**
+  Get the name from the Driver handle, which can be a handle with
+  EFI_LOADED_IMAGE_PROTOCOL or EFI_DRIVER_BINDING_PROTOCOL installed.
+  This name can be used in performance data logging.
+
+  @param Handle          Driver handle.
+  @param GaugeString     The output string to be logged by performance logger.
+  @param StringSize      The buffer size of GaugeString in bytes.
+
+**/
+VOID
+BmGetNameFromHandle (
+  IN  EFI_HANDLE     Handle,
+  OUT CHAR8          *GaugeString,
+  IN  UINTN          StringSize
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_LOADED_IMAGE_PROTOCOL   *Image;
+  CHAR8                       *PdbFileName;
+  EFI_DRIVER_BINDING_PROTOCOL *DriverBinding;
+
+  AsciiStrCpyS (GaugeString, StringSize, " ");
+
+  //
+  // Get handle name from image protocol
+  //
+  Status = gBS->HandleProtocol (
+                  Handle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID **) &Image
+                  );
+
+  if (EFI_ERROR (Status)) {
+    Status = gBS->OpenProtocol (
+                    Handle,
+                    &gEfiDriverBindingProtocolGuid,
+                    (VOID **) &DriverBinding,
+                    NULL,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return ;
+    }
+    //
+    // Get handle name from image protocol
+    //
+    Status = gBS->HandleProtocol (
+                    DriverBinding->ImageHandle,
+                    &gEfiLoadedImageProtocolGuid,
+                    (VOID **) &Image
+                    );
+  }
+
+  PdbFileName = PeCoffLoaderGetPdbPointer (Image->ImageBase);
+
+  if (PdbFileName != NULL) {
+    BmGetShortPdbFileName (PdbFileName, GaugeString, StringSize);
+  }
+
+  return ;
+}
+
+/**
+
+  Writes performance data of booting into the allocated memory.
+  OS can process these records.
+
+  @param  Event                 The triggered event.
+  @param  Context               Context for this event.
+
+**/
+VOID
+EFIAPI
+BmWriteBootToOsPerformanceData (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    LimitCount;
+  EFI_HANDLE                *Handles;
+  UINTN                     NoHandles;
+  CHAR8                     GaugeString[PERF_TOKEN_SIZE];
+  UINT8                     *Ptr;
+  UINT32                    Index;
+  UINT64                    Ticker;
+  UINT64                    Freq;
+  UINT32                    Duration;
+  UINTN                     LogEntryKey;
+  CONST VOID                *Handle;
+  CONST CHAR8               *Token;
+  CONST CHAR8               *Module;
+  UINT64                    StartTicker;
+  UINT64                    EndTicker;
+  UINT64                    StartValue;
+  UINT64                    EndValue;
+  BOOLEAN                   CountUp;
+  UINTN                     VarSize;
+  BOOLEAN                   Found;
+
+  //
+  // Record the performance data for End of BDS
+  //
+  PERF_END(NULL, "BDS", NULL, 0);
+
+  //
+  // Retrieve time stamp count as early as possible
+  //
+  Ticker  = GetPerformanceCounter ();
+
+  Freq    = GetPerformanceCounterProperties (&StartValue, &EndValue);
+  
+  Freq    = DivU64x32 (Freq, 1000);
+
+  mBmPerfHeader.CpuFreq = Freq;
+
+  //
+  // Record BDS raw performance data
+  //
+  if (EndValue >= StartValue) {
+    mBmPerfHeader.BDSRaw = Ticker - StartValue;
+    CountUp            = TRUE;
+  } else {
+    mBmPerfHeader.BDSRaw = StartValue - Ticker;
+    CountUp            = FALSE;
+  }
+
+  //
+  // Reset the entry count
+  //
+  mBmPerfHeader.Count = 0;
+
+  if (mBmAcpiLowMemoryBase == 0x0FFFFFFFF) {
+    VarSize = sizeof (EFI_PHYSICAL_ADDRESS);
+    Status = gRT->GetVariable (
+                    L"PerfDataMemAddr",
+                    &gPerformanceProtocolGuid,
+                    NULL,
+                    &VarSize,
+                    &mBmAcpiLowMemoryBase
+                    );
+    if (EFI_ERROR (Status)) {
+      //
+      // Fail to get the variable, return.
+      //
+      return;
+    }
+  }
+
+  //
+  // Put Detailed performance data into memory
+  //
+  Handles = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  AllHandles,
+                  NULL,
+                  NULL,
+                  &NoHandles,
+                  &Handles
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  Ptr        = (UINT8 *) ((UINT32) mBmAcpiLowMemoryBase + sizeof (PERF_HEADER));
+  LimitCount = (UINT32) (PERF_DATA_MAX_LENGTH - sizeof (PERF_HEADER)) / sizeof (PERF_DATA);
+
+  //
+  // Get performance data
+  //
+  LogEntryKey = 0;
+  while ((LogEntryKey = GetPerformanceMeasurement (
+                          LogEntryKey,
+                          &Handle,
+                          &Token,
+                          &Module,
+                          &StartTicker,
+                          &EndTicker)) != 0) {
+    if (EndTicker != 0) {
+      if (StartTicker == 1) {
+        StartTicker = StartValue;
+      }
+      if (EndTicker == 1) {
+        EndTicker = StartValue;
+      }
+      Ticker = CountUp ? (EndTicker - StartTicker) : (StartTicker - EndTicker);
+
+      Duration = (UINT32) DivU64x32 (Ticker, (UINT32) Freq);
+      if (Duration == 0) {
+        continue;
+      }
+
+      ZeroMem (&mBmPerfData, sizeof (PERF_DATA));
+
+      mBmPerfData.Duration = Duration;
+
+      //
+      // See if the Handle is in the handle buffer
+      //
+      Found = FALSE;
+      for (Index = 0; Index < NoHandles; Index++) {
+        if (Handle == Handles[Index]) {
+          BmGetNameFromHandle (Handles[Index], GaugeString, PERF_TOKEN_SIZE);
+          AsciiStrCpyS (mBmPerfData.Token, PERF_TOKEN_SIZE, GaugeString);
+          Found = TRUE;
+          break;
+        }
+      }
+
+      if (!Found) {
+        AsciiStrnCpyS (mBmPerfData.Token, PERF_TOKEN_SIZE, Token, PERF_TOKEN_LENGTH);
+      }
+
+      CopyMem (Ptr, &mBmPerfData, sizeof (PERF_DATA));
+      Ptr += sizeof (PERF_DATA);
+
+      mBmPerfHeader.Count++;
+      if (mBmPerfHeader.Count == LimitCount) {
+        goto Done;
+      }
+    }
+  }
+
+Done:
+
+  FreePool (Handles);
+
+  mBmPerfHeader.Signiture = PERFORMANCE_SIGNATURE;
+
+  //
+  // Put performance data to Reserved memory
+  //
+  CopyMem (
+    (UINTN *) (UINTN) mBmAcpiLowMemoryBase,
+    &mBmPerfHeader,
+    sizeof (PERF_HEADER)
+    );
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/InternalBm.h b/Core/MdeModulePkg/Library/UefiBootManagerLib/InternalBm.h
new file mode 100644
index 0000000000..c808ed2ffe
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/InternalBm.h
@@ -0,0 +1,454 @@
+/** @file
+  BDS library definition, include the file and data structure
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _INTERNAL_BM_H_
+#define _INTERNAL_BM_H_
+
+#include <PiDxe.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/PeImage.h>
+#include <IndustryStandard/Atapi.h>
+#include <IndustryStandard/Scsi.h>
+
+#include <Protocol/PciRootBridgeIo.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/SimpleFileSystem.h>
+#include <Protocol/LoadFile.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/SimpleTextInEx.h>
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/SimpleNetwork.h>
+#include <Protocol/FirmwareVolume2.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/UsbIo.h>
+#include <Protocol/DiskInfo.h>
+#include <Protocol/IdeControllerInit.h>
+#include <Protocol/BootLogo.h>
+#include <Protocol/DriverHealth.h>
+#include <Protocol/FormBrowser2.h>
+
+#include <Guid/ZeroGuid.h>
+#include <Guid/MemoryTypeInformation.h>
+#include <Guid/FileInfo.h>
+#include <Guid/GlobalVariable.h>
+#include <Guid/Performance.h>
+#include <Guid/StatusCodeDataTypeVariable.h>
+
+#include <Library/PrintLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/HobLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/UefiBootManagerLib.h>
+#include <Library/TimerLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/CapsuleLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/HiiLib.h>
+
+#if !defined (EFI_REMOVABLE_MEDIA_FILE_NAME)
+    #if defined (MDE_CPU_EBC)
+        //
+        // Uefi specification only defines the default boot file name for IA32, X64
+        // and IPF processor, so need define boot file name for EBC architecture here.
+        //
+        #define EFI_REMOVABLE_MEDIA_FILE_NAME L"\\EFI\\BOOT\\BOOTEBC.EFI"
+    #else
+        #error "Can not determine the default boot file name for unknown processor type!"
+    #endif
+#endif
+
+typedef enum {
+  BmAcpiFloppyBoot,
+  BmHardwareDeviceBoot,
+  BmMessageAtapiBoot,
+  BmMessageSataBoot,
+  BmMessageUsbBoot,
+  BmMessageScsiBoot,
+  BmMessageNetworkBoot,
+  BmMessageHttpBoot,
+  BmMiscBoot
+} BM_BOOT_TYPE;
+
+typedef
+CHAR16 *
+(* BM_GET_BOOT_DESCRIPTION) (
+  IN EFI_HANDLE          Handle
+  );
+
+#define BM_OPTION_NAME_LEN                          sizeof ("SysPrep####")
+extern CHAR16  *mBmLoadOptionName[];
+
+typedef
+VOID
+(*VARIABLE_VISITOR) (
+  CHAR16                *Name,
+  EFI_GUID              *Guid,
+  VOID                  *Context
+  );
+
+/**
+  Call Visitor function for each variable in variable storage.
+
+  @param Visitor   Visitor function.
+  @param Context   The context passed to Visitor function.
+**/
+VOID
+ForEachVariable (
+  VARIABLE_VISITOR            Visitor,
+  VOID                        *Context
+  );
+
+#define BM_BOOT_DESCRIPTION_ENTRY_SIGNATURE SIGNATURE_32 ('b', 'm', 'd', 'h')
+typedef struct {
+  UINT32                                    Signature;
+  LIST_ENTRY                                Link;
+  EFI_BOOT_MANAGER_BOOT_DESCRIPTION_HANDLER Handler;
+} BM_BOOT_DESCRIPTION_ENTRY;
+
+/**
+  Repair all the controllers according to the Driver Health status queried.
+**/
+VOID
+BmRepairAllControllers (
+  VOID
+  );
+
+#define BM_HOTKEY_SIGNATURE SIGNATURE_32 ('b', 'm', 'h', 'k')
+typedef struct {
+  UINT32                    Signature;
+  LIST_ENTRY                Link;
+
+  BOOLEAN                   IsContinue;
+  UINT16                    BootOption;
+  UINT8                     CodeCount;
+  UINT8                     WaitingKey;
+  EFI_KEY_DATA              KeyData[3];
+} BM_HOTKEY;
+
+#define BM_HOTKEY_FROM_LINK(a) CR (a, BM_HOTKEY, Link, BM_HOTKEY_SIGNATURE)
+
+/**
+  Get the image file buffer data and buffer size by its device path. 
+
+  @param FilePath  On input, a pointer to an allocated buffer containing the device
+                   path of the file.
+                   On output the pointer could be NULL when the function fails to
+                   load the boot option, or could point to an allocated buffer containing
+                   the device path of the file.
+                   It could be updated by either short-form device path expanding,
+                   or default boot file path appending.
+                   Caller is responsible to free it when it's non-NULL.
+  @param FileSize  A pointer to the size of the file buffer.
+
+  @retval NULL   File is NULL, or FileSize is NULL. Or, the file can't be found.
+  @retval other  The file buffer. The caller is responsible to free the memory.
+**/
+VOID *
+BmLoadEfiBootOption (
+  IN OUT EFI_DEVICE_PATH_PROTOCOL **FilePath,
+  OUT    UINTN                    *FileSize
+  );
+
+/**
+  Get the Option Number that wasn't used.
+
+  @param  LoadOptionType      Load option type.
+  @param  FreeOptionNumber    To receive the minimal free option number.
+
+  @retval EFI_SUCCESS           The option number is found
+  @retval EFI_OUT_OF_RESOURCES  There is no free option number that can be used.
+  @retval EFI_INVALID_PARAMETER FreeOptionNumber is NULL
+
+**/
+EFI_STATUS
+BmGetFreeOptionNumber (
+  IN  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE LoadOptionType,
+  OUT UINT16                            *FreeOptionNumber
+  );
+
+/**
+
+  Writes performance data of booting into the allocated memory.
+  OS can process these records.
+
+  @param  Event                 The triggered event.
+  @param  Context               Context for this event.
+
+**/
+VOID
+EFIAPI
+BmWriteBootToOsPerformanceData (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+
+/**
+  Get the headers (dos, image, optional header) from an image
+
+  @param  Device                SimpleFileSystem device handle
+  @param  FileName              File name for the image
+  @param  DosHeader             Pointer to dos header
+  @param  Hdr                   The buffer in which to return the PE32, PE32+, or TE header.
+
+  @retval EFI_SUCCESS           Successfully get the machine type.
+  @retval EFI_NOT_FOUND         The file is not found.
+  @retval EFI_LOAD_ERROR        File is not a valid image file.
+
+**/
+EFI_STATUS
+BmGetImageHeader (
+  IN  EFI_HANDLE                  Device,
+  IN  CHAR16                      *FileName,
+  OUT EFI_IMAGE_DOS_HEADER        *DosHeader,
+  OUT EFI_IMAGE_OPTIONAL_HEADER_PTR_UNION   Hdr
+  );
+
+/**
+  This routine adjust the memory information for different memory type and 
+  save them into the variables for next boot. It resets the system when
+  memory information is updated and the current boot option belongs to
+  boot category instead of application category.
+
+  @param Boot  TRUE if current boot option belongs to boot category instead of
+               application category.
+**/
+VOID
+BmSetMemoryTypeInformationVariable (
+  IN BOOLEAN                    Boot
+  );
+
+/**
+  Check whether there is a instance in BlockIoDevicePath, which contain multi device path
+  instances, has the same partition node with HardDriveDevicePath device path
+
+  @param  BlockIoDevicePath      Multi device path instances which need to check
+  @param  HardDriveDevicePath    A device path which starts with a hard drive media
+                                 device path.
+
+  @retval TRUE                   There is a matched device path instance.
+  @retval FALSE                  There is no matched device path instance.
+
+**/
+BOOLEAN
+BmMatchPartitionDevicePathNode (
+  IN  EFI_DEVICE_PATH_PROTOCOL   *BlockIoDevicePath,
+  IN  HARDDRIVE_DEVICE_PATH      *HardDriveDevicePath
+  );
+
+/**
+  Connect the specific Usb device which match the short form device path.
+
+  @param  DevicePath             A short-form device path that starts with the first
+                                 element being a USB WWID or a USB Class device
+                                 path
+
+  @return EFI_INVALID_PARAMETER  DevicePath is NULL pointer.
+                                 DevicePath is not a USB device path.
+
+  @return EFI_SUCCESS            Success to connect USB device
+  @return EFI_NOT_FOUND          Fail to find handle for USB controller to connect.
+
+**/
+EFI_STATUS
+BmConnectUsbShortFormDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL   *DevicePath
+  );
+
+/**
+  Stop the hotkey processing.
+  
+  @param    Event          Event pointer related to hotkey service. 
+  @param    Context        Context pass to this function. 
+**/
+VOID
+EFIAPI
+BmStopHotkeyService (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  );
+
+/**
+  Set the variable and report the error through status code upon failure.
+
+  @param  VariableName           A Null-terminated string that is the name of the vendor's variable.
+                                 Each VariableName is unique for each VendorGuid. VariableName must
+                                 contain 1 or more characters. If VariableName is an empty string,
+                                 then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             Attributes bitmask to set for the variable.
+  @param  DataSize               The size in bytes of the Data buffer. Unless the EFI_VARIABLE_APPEND_WRITE, 
+                                 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS, or 
+                                 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS attribute is set, a size of zero 
+                                 causes the variable to be deleted. When the EFI_VARIABLE_APPEND_WRITE attribute is 
+                                 set, then a SetVariable() call with a DataSize of zero will not cause any change to 
+                                 the variable value (the timestamp associated with the variable may be updated however 
+                                 even if no new data value is provided,see the description of the 
+                                 EFI_VARIABLE_AUTHENTICATION_2 descriptor below. In this case the DataSize will not 
+                                 be zero since the EFI_VARIABLE_AUTHENTICATION_2 descriptor will be populated). 
+  @param  Data                   The contents for the variable.
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits, name, and GUID was supplied, or the
+                                 DataSize exceeds the maximum allowed.
+  @retval EFI_INVALID_PARAMETER  VariableName is an empty string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be retrieved due to a hardware error.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only.
+  @retval EFI_WRITE_PROTECTED    The variable in question cannot be deleted.
+  @retval EFI_SECURITY_VIOLATION The variable could not be written due to EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS 
+                                 or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS being set, but the AuthInfo 
+                                 does NOT pass the validation check carried out by the firmware.
+
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+**/
+EFI_STATUS
+BmSetVariableAndReportStatusCodeOnError (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  );
+
+/**
+  Get the load option by its device path.
+
+  @param FilePath  The device path pointing to a load option.
+                   It could be a short-form device path.
+  @param FullPath  Return the full device path of the load option after
+                   short-form device path expanding.
+                   Caller is responsible to free it.
+  @param FileSize  Return the load option size.
+
+  @return The load option buffer. Caller is responsible to free the memory.
+**/
+VOID *
+BmGetLoadOptionBuffer (
+  IN  EFI_DEVICE_PATH_PROTOCOL          *FilePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL          **FullPath,
+  OUT UINTN                             *FileSize
+  );
+
+/**
+  Return whether the PE header of the load option is valid or not.
+
+  @param[in] Type       The load option type.
+  @param[in] FileBuffer The PE file buffer of the load option.
+  @param[in] FileSize   The size of the load option file.
+
+  @retval TRUE  The PE header of the load option is valid.
+  @retval FALSE The PE header of the load option is not valid.
+**/
+BOOLEAN
+BmIsLoadOptionPeHeaderValid (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION_TYPE Type,
+  IN VOID                              *FileBuffer,
+  IN UINTN                             FileSize
+  );
+
+/**
+  Function compares a device path data structure to that of all the nodes of a
+  second device path instance.
+
+  @param  Multi                 A pointer to a multi-instance device path data
+                                structure.
+  @param  Single                A pointer to a single-instance device path data
+                                structure.
+
+  @retval TRUE                  If the Single device path is contained within Multi device path.
+  @retval FALSE                 The Single device path is not match within Multi device path.
+
+**/
+BOOLEAN
+BmMatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single
+  );
+
+/**
+  Delete the instance in Multi which matches partly with Single instance
+
+  @param  Multi                 A pointer to a multi-instance device path data
+                                structure.
+  @param  Single                A pointer to a single-instance device path data
+                                structure.
+
+  @return This function will remove the device path instances in Multi which partly
+          match with the Single, and return the result device path. If there is no
+          remaining device path as a result, this function will return NULL.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+BmDelPartMatchInstance (
+  IN     EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN     EFI_DEVICE_PATH_PROTOCOL  *Single
+  );
+
+
+/**
+  Return the index of the load option in the load option array.
+
+  The function consider two load options are equal when the 
+  OptionType, Attributes, Description, FilePath and OptionalData are equal.
+
+  @param Key    Pointer to the load option to be found.
+  @param Array  Pointer to the array of load options to be found.
+  @param Count  Number of entries in the Array.
+
+  @retval -1          Key wasn't found in the Array.
+  @retval 0 ~ Count-1 The index of the Key in the Array.
+**/
+INTN
+BmFindLoadOption (
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION *Key,
+  IN CONST EFI_BOOT_MANAGER_LOAD_OPTION *Array,
+  IN UINTN                              Count
+  );
+
+/**
+  Repair all the controllers according to the Driver Health status queried.
+**/
+VOID
+BmRepairAllControllers (
+  VOID
+  );
+
+/**
+  Print the device path info.
+
+  @param DevicePath           The device path need to print.
+**/
+VOID
+BmPrintDp (
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath
+  );
+
+#endif // _INTERNAL_BM_H_
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.inf b/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.inf
new file mode 100644
index 0000000000..a2c6441dbe
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.inf
@@ -0,0 +1,117 @@
+## @file
+#  Define and produce general Boot Manager related interfaces.
+#
+#  The implementation provides richful library functions supporting load option
+#  manipulation, hotkey registration, UEFI boot, connect/disconnect, console
+#  manipulation, driver health checking and etc.
+#
+#  Copyright (c) 2007 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UefiBootManagerLib
+  MODULE_UNI_FILE                = UefiBootManagerLib.uni
+  FILE_GUID                      = 8D4752BC-595E-49a2-B4AF-F3F57B601DE9
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = UefiBootManagerLib|DXE_DRIVER DXE_RUNTIME_DRIVER UEFI_APPLICATION UEFI_DRIVER 
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BmPerformance.c
+  BmConnect.c
+  BmMisc.c
+  BmConsole.c
+  BmBoot.c
+  BmLoadOption.c
+  BmHotkey.c
+  BmDriverHealth.c
+  InternalBm.h
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  HobLib
+  PcdLib
+  BaseLib
+  UefiLib
+  TimerLib
+  DebugLib
+  PrintLib
+  BaseMemoryLib
+  DevicePathLib
+  PerformanceLib
+  PeCoffGetEntryPointLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  DxeServicesTableLib
+  MemoryAllocationLib
+  DxeServicesLib
+  ReportStatusCodeLib
+  PerformanceLib
+  HiiLib
+  SortLib
+
+[Guids]
+  gEfiMemoryTypeInformationGuid                 ## CONSUMES ## GUID (The identifier of memory type information type in system table)
+                                                ## CONSUMES ## GUID HOB (The hob holding memory type information)
+  gEfiGlobalVariableGuid                        ## SOMETIMES_PRODUCES ## Variable:L"BootCurrent" (The boot option of current boot)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"BootXX" (Boot option variable)
+                                                ## CONSUMES           ## Variable:L"Timeout" (The time out value in second of showing progress bar)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"BootOrder" (The boot option array)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"DriverOrder" (The driver order list)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConIn" (The device path of console in device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConOut" (The device path of console out device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ErrOut" (The device path of error out device)
+  gEfiFileInfoGuid                              ## CONSUMES ## GUID
+  gPerformanceProtocolGuid                      ## SOMETIMES_PRODUCES ## Variable:L"PerfDataMemAddr" (The ACPI address of performance data)
+  gEdkiiStatusCodeDataTypeVariableGuid          ## SOMETIMES_CONSUMES ## GUID
+  gEfiDiskInfoAhciInterfaceGuid                 ## SOMETIMES_CONSUMES ## GUID
+  gEfiDiskInfoIdeInterfaceGuid                  ## SOMETIMES_CONSUMES ## GUID
+  gEfiDiskInfoScsiInterfaceGuid                 ## SOMETIMES_CONSUMES ## GUID
+  gZeroGuid                                     ## CONSUMES ## GUID
+
+[Protocols]
+  gEfiPciRootBridgeIoProtocolGuid               ## CONSUMES
+  gEfiSimpleFileSystemProtocolGuid              ## CONSUMES
+  gEfiLoadFileProtocolGuid                      ## CONSUMES
+  gEfiSimpleTextOutProtocolGuid                 ## CONSUMES
+  gEfiPciIoProtocolGuid                         ## CONSUMES
+  gEfiLoadedImageProtocolGuid                   ## CONSUMES
+  gEfiSimpleNetworkProtocolGuid                 ## CONSUMES
+  gEfiSimpleTextInProtocolGuid                  ## CONSUMES
+  gEfiBlockIoProtocolGuid                       ## CONSUMES
+  gEfiFirmwareVolume2ProtocolGuid               ## CONSUMES
+  gEfiDevicePathProtocolGuid                    ## CONSUMES
+  gEfiBootLogoProtocolGuid                      ## CONSUMES
+  gEfiSimpleTextInputExProtocolGuid             ## CONSUMES
+  gEfiGraphicsOutputProtocolGuid                ## SOMETIMES_CONSUMES
+  gEfiUsbIoProtocolGuid                         ## SOMETIMES_CONSUMES
+  gEfiDiskInfoProtocolGuid                      ## SOMETIMES_CONSUMES
+  gEfiDriverHealthProtocolGuid                  ## SOMETIMES_CONSUMES
+  gEfiFormBrowser2ProtocolGuid                  ## SOMETIMES_CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdResetOnMemoryTypeInformationChange      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderLoad                ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderStart               ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdErrorCodeSetVariable                    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBootManagerMenuFile                     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDriverHealthConfigureForm               ## SOMETIMES_CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.uni b/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.uni
new file mode 100644
index 0000000000..961fdcada2
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.uni
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/HiiLanguage.c b/Core/MdeModulePkg/Library/UefiHiiLib/HiiLanguage.c
new file mode 100644
index 0000000000..2519afa7d4
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/HiiLanguage.c
@@ -0,0 +1,96 @@
+/** @file
+  Language related HII Library implementation.
+
+  Copyright (c) 2006 - 2008, 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 "InternalHiiLib.h"
+
+/**
+  Retrieves a pointer to the a Null-terminated ASCII string containing the list 
+  of languages that an HII handle in the HII Database supports.  The returned 
+  string is allocated using AllocatePool().  The caller is responsible for freeing
+  the returned string using FreePool().  The format of the returned string follows
+  the language format assumed the HII Database.
+  
+  If HiiHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+
+  @retval NULL   HiiHandle is not registered in the HII database
+  @retval NULL   There are not enough resources available to retrieve the suported 
+                 languages.
+  @retval NULL   The list of suported languages could not be retrieved.
+  @retval Other  A pointer to the Null-terminated ASCII string of supported languages.
+
+**/
+CHAR8 *
+EFIAPI
+HiiGetSupportedLanguages (
+  IN EFI_HII_HANDLE           HiiHandle
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       LanguageSize;
+  CHAR8       TempSupportedLanguages;
+  CHAR8       *SupportedLanguages;
+
+  ASSERT (HiiHandle != NULL);
+
+  //
+  // Retrieve the size required for the supported languages buffer.
+  //
+  LanguageSize = 0;
+  Status = gHiiString->GetLanguages (gHiiString, HiiHandle, &TempSupportedLanguages, &LanguageSize);
+
+  //
+  // If GetLanguages() returns EFI_SUCCESS for a zero size, 
+  // then there are no supported languages registered for HiiHandle.  If GetLanguages() 
+  // returns an error other than EFI_BUFFER_TOO_SMALL, then HiiHandle is not present
+  // in the HII Database
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    //
+    // Return NULL if the size can not be retrieved, or if HiiHandle is not in the HII Database
+    //
+    return NULL;
+  }
+
+  //
+  // Allocate the supported languages buffer.
+  //
+  SupportedLanguages = AllocateZeroPool (LanguageSize);
+  if (SupportedLanguages == NULL) {
+    //
+    // Return NULL if allocation fails.
+    //
+    return NULL;
+  }
+
+  //
+  // Retrieve the supported languages string
+  //
+  Status = gHiiString->GetLanguages (gHiiString, HiiHandle, SupportedLanguages, &LanguageSize);
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the buffer and return NULL if the supported languages can not be retrieved.
+    //
+    FreePool (SupportedLanguages);
+    return NULL;
+  }
+
+  //
+  // Return the Null-terminated ASCII string of supported languages
+  //
+  return SupportedLanguages;
+}
+
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/HiiLib.c b/Core/MdeModulePkg/Library/UefiHiiLib/HiiLib.c
new file mode 100644
index 0000000000..66d72acf27
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/HiiLib.c
@@ -0,0 +1,4231 @@
+/** @file
+  HII Library implementation that uses DXE protocols and services.
+
+  Copyright (c) 2006 - 2015, 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 "InternalHiiLib.h"
+
+#define GUID_CONFIG_STRING_TYPE 0x00
+#define NAME_CONFIG_STRING_TYPE 0x01
+#define PATH_CONFIG_STRING_TYPE 0x02
+
+#define ACTION_SET_DEFAUTL_VALUE 0x01
+#define ACTION_VALIDATE_SETTING  0x02
+
+#define HII_LIB_DEFAULT_VARSTORE_SIZE  0x200
+
+typedef struct {
+  LIST_ENTRY          Entry;      // Link to Block array
+  UINT16              Offset;
+  UINT16              Width;
+  UINT8               OpCode;
+  UINT8               Scope;
+} IFR_BLOCK_DATA;
+
+typedef struct {
+  EFI_VARSTORE_ID     VarStoreId;
+  UINT16              Size;
+} IFR_VARSTORAGE_DATA;
+
+//
+// <ConfigHdr> Template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR16 mConfigHdrTemplate[] = L"GUID=00000000000000000000000000000000&NAME=0000&PATH=00";
+
+EFI_FORM_BROWSER2_PROTOCOL  *mUefiFormBrowser2 = NULL;
+
+//
+// Template used to mark the end of a list of packages 
+//
+GLOBAL_REMOVE_IF_UNREFERENCED CONST EFI_HII_PACKAGE_HEADER  mEndOfPakageList = {
+  sizeof (EFI_HII_PACKAGE_HEADER),
+  EFI_HII_PACKAGE_END
+};
+
+/**
+  Extract Hii package list GUID for given HII handle.
+
+  If HiiHandle could not be found in the HII database, then ASSERT.
+  If Guid is NULL, then ASSERT.
+
+  @param  Handle              Hii handle
+  @param  Guid                Package list GUID
+
+  @retval EFI_SUCCESS         Successfully extract GUID from Hii database.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalHiiExtractGuidFromHiiHandle (
+  IN      EFI_HII_HANDLE      Handle,
+  OUT     EFI_GUID            *Guid
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        BufferSize;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+
+  ASSERT (Guid != NULL);
+  ASSERT (Handle != NULL);
+
+  //
+  // Get HII PackageList
+  //
+  BufferSize = 0;
+  HiiPackageList = NULL;
+
+  Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  ASSERT (Status != EFI_NOT_FOUND);
+  
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    HiiPackageList = AllocatePool (BufferSize);
+    ASSERT (HiiPackageList != NULL);
+
+    Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  }
+  if (EFI_ERROR (Status)) {
+    FreePool (HiiPackageList);
+    return Status;
+  }
+
+  //
+  // Extract GUID
+  //
+  CopyGuid (Guid, &HiiPackageList->PackageListGuid);
+
+  FreePool (HiiPackageList);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Registers a list of packages in the HII Database and returns the HII Handle
+  associated with that registration.  If an HII Handle has already been registered
+  with the same PackageListGuid and DeviceHandle, then NULL is returned.  If there
+  are not enough resources to perform the registration, then NULL is returned.
+  If an empty list of packages is passed in, then NULL is returned.  If the size of
+  the list of package is 0, then NULL is returned.
+
+  The variable arguments are pointers which point to package header that defined 
+  by UEFI VFR compiler and StringGather tool.
+
+  #pragma pack (push, 1)
+  typedef struct {
+    UINT32                  BinaryLength;
+    EFI_HII_PACKAGE_HEADER  PackageHeader;
+  } EDKII_AUTOGEN_PACKAGES_HEADER;
+  #pragma pack (pop)
+  
+  @param[in]  PackageListGuid  The GUID of the package list.
+  @param[in]  DeviceHandle     If not NULL, the Device Handle on which 
+                               an instance of DEVICE_PATH_PROTOCOL is installed.
+                               This Device Handle uniquely defines the device that 
+                               the added packages are associated with.
+  @param[in]  ...              The variable argument list that contains pointers 
+                               to packages terminated by a NULL.
+
+  @retval NULL   A HII Handle has already been registered in the HII Database with
+                 the same PackageListGuid and DeviceHandle.
+  @retval NULL   The HII Handle could not be created.
+  @retval NULL   An empty list of packages was passed in.
+  @retval NULL   All packages are empty.
+  @retval Other  The HII Handle associated with the newly registered package list.
+
+**/
+EFI_HII_HANDLE
+EFIAPI
+HiiAddPackages (
+  IN CONST EFI_GUID    *PackageListGuid,
+  IN       EFI_HANDLE  DeviceHandle  OPTIONAL,
+  ...
+  )
+{
+  EFI_STATUS                   Status;
+  VA_LIST                      Args;
+  UINT32                       *Package;
+  EFI_HII_PACKAGE_LIST_HEADER  *PackageListHeader;
+  EFI_HII_HANDLE               HiiHandle;
+  UINT32                       Length;
+  UINT8                        *Data;
+
+  ASSERT (PackageListGuid != NULL);
+
+  //
+  // Calculate the length of all the packages in the variable argument list
+  //
+  for (Length = 0, VA_START (Args, DeviceHandle); (Package = VA_ARG (Args, UINT32 *)) != NULL; ) {
+    Length += (ReadUnaligned32 (Package) - sizeof (UINT32));
+  }
+  VA_END (Args);
+
+  //
+  // If there are no packages in the variable argument list or all the packages 
+  // are empty, then return a NULL HII Handle
+  //
+  if (Length == 0) {
+    return NULL;
+  }
+
+  //
+  // Add the length of the Package List Header and the terminating Package Header 
+  //
+  Length += sizeof (EFI_HII_PACKAGE_LIST_HEADER) + sizeof (EFI_HII_PACKAGE_HEADER);
+
+  //
+  // Allocate the storage for the entire Package List
+  //
+  PackageListHeader = AllocateZeroPool (Length);
+
+  //
+  // If the Package List can not be allocated, then return a NULL HII Handle
+  //
+  if (PackageListHeader == NULL) {
+    return NULL;
+  }
+
+  //
+  // Fill in the GUID and Length of the Package List Header
+  //
+  CopyGuid (&PackageListHeader->PackageListGuid, PackageListGuid);
+  PackageListHeader->PackageLength = Length;
+
+  //
+  // Initialize a pointer to the beginning if the Package List data
+  //
+  Data = (UINT8 *)(PackageListHeader + 1);
+
+  //
+  // Copy the data from each package in the variable argument list
+  //
+  for (VA_START (Args, DeviceHandle); (Package = VA_ARG (Args, UINT32 *)) != NULL; ) {
+    Length = ReadUnaligned32 (Package) - sizeof (UINT32);
+    CopyMem (Data, Package + 1, Length);
+    Data += Length;
+  }
+  VA_END (Args);
+
+  //
+  // Append a package of type EFI_HII_PACKAGE_END to mark the end of the package list
+  //
+  CopyMem (Data, &mEndOfPakageList, sizeof (mEndOfPakageList));
+
+  //
+  // Register the package list with the HII Database
+  //
+  Status = gHiiDatabase->NewPackageList (
+                           gHiiDatabase, 
+                           PackageListHeader, 
+                           DeviceHandle, 
+                           &HiiHandle
+                           );
+  if (EFI_ERROR (Status)) {
+    HiiHandle = NULL;
+  }
+
+  //
+  // Free the allocated package list
+  //
+  FreePool (PackageListHeader);
+
+  //
+  // Return the new HII Handle
+  //
+  return HiiHandle;
+}
+
+/**
+  Removes a package list from the HII database.
+
+  If HiiHandle is NULL, then ASSERT.
+  If HiiHandle is not a valid EFI_HII_HANDLE in the HII database, then ASSERT.
+
+  @param[in]  HiiHandle   The handle that was previously registered in the HII database
+
+**/
+VOID
+EFIAPI
+HiiRemovePackages (
+  IN      EFI_HII_HANDLE      HiiHandle
+  )
+{
+  EFI_STATUS Status;
+
+  ASSERT (HiiHandle != NULL);
+  Status = gHiiDatabase->RemovePackageList (gHiiDatabase, HiiHandle);
+  ASSERT_EFI_ERROR (Status);
+}
+
+
+/**
+  Retrieves the array of all the HII Handles or the HII handles of a specific
+  package list GUID in the HII Database.
+  This array is terminated with a NULL HII Handle.
+  This function allocates the returned array using AllocatePool().
+  The caller is responsible for freeing the array with FreePool().
+
+  @param[in]  PackageListGuid  An optional parameter that is used to request 
+                               HII Handles associated with a specific
+                               Package List GUID.  If this parameter is NULL,
+                               then all the HII Handles in the HII Database
+                               are returned.  If this parameter is not NULL,
+                               then zero or more HII Handles associated with 
+                               PackageListGuid are returned.
+
+  @retval NULL   No HII handles were found in the HII database
+  @retval NULL   The array of HII Handles could not be retrieved
+  @retval Other  A pointer to the NULL terminated array of HII Handles
+
+**/
+EFI_HII_HANDLE *
+EFIAPI
+HiiGetHiiHandles (
+  IN CONST EFI_GUID  *PackageListGuid  OPTIONAL
+  )
+{
+  EFI_STATUS      Status;
+  UINTN           HandleBufferLength;
+  EFI_HII_HANDLE  TempHiiHandleBuffer;
+  EFI_HII_HANDLE  *HiiHandleBuffer;
+  EFI_GUID        Guid;
+  UINTN           Index1;
+  UINTN           Index2;
+
+  //
+  // Retrieve the size required for the buffer of all HII handles.
+  //
+  HandleBufferLength = 0;
+  Status = gHiiDatabase->ListPackageLists (
+                           gHiiDatabase,
+                           EFI_HII_PACKAGE_TYPE_ALL,
+                           NULL,
+                           &HandleBufferLength,
+                           &TempHiiHandleBuffer
+                           );
+
+  //
+  // If ListPackageLists() returns EFI_SUCCESS for a zero size, 
+  // then there are no HII handles in the HII database.  If ListPackageLists() 
+  // returns an error other than EFI_BUFFER_TOO_SMALL, then there are no HII 
+  // handles in the HII database.
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    //
+    // Return NULL if the size can not be retrieved, or if there are no HII 
+    // handles in the HII Database
+    //
+    return NULL;
+  }
+
+  //
+  // Allocate the array of HII handles to hold all the HII Handles and a NULL terminator
+  //
+  HiiHandleBuffer = AllocateZeroPool (HandleBufferLength + sizeof (EFI_HII_HANDLE));
+  if (HiiHandleBuffer == NULL) {
+    //
+    // Return NULL if allocation fails.
+    //
+    return NULL;
+  }
+
+  //
+  // Retrieve the array of HII Handles in the HII Database
+  //
+  Status = gHiiDatabase->ListPackageLists (
+                           gHiiDatabase,
+                           EFI_HII_PACKAGE_TYPE_ALL,
+                           NULL,
+                           &HandleBufferLength,
+                           HiiHandleBuffer
+                           );
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the buffer and return NULL if the HII handles can not be retrieved.
+    //
+    FreePool (HiiHandleBuffer);
+    return NULL;
+  }
+
+  if (PackageListGuid == NULL) {
+    //
+    // Return the NULL terminated array of HII handles in the HII Database
+    //
+    return HiiHandleBuffer;
+  } else {
+    for (Index1 = 0, Index2 = 0; HiiHandleBuffer[Index1] != NULL; Index1++) {
+      Status = InternalHiiExtractGuidFromHiiHandle (HiiHandleBuffer[Index1], &Guid);
+      ASSERT_EFI_ERROR (Status);
+      if (CompareGuid (&Guid, PackageListGuid)) {
+        HiiHandleBuffer[Index2++] = HiiHandleBuffer[Index1];       
+      }
+    }
+    if (Index2 > 0) {
+      HiiHandleBuffer[Index2] = NULL;
+      return HiiHandleBuffer;
+    } else {
+      FreePool (HiiHandleBuffer);
+      return NULL;
+    }
+  }
+}
+
+/**
+  Converts all hex dtring characters in range ['A'..'F'] to ['a'..'f'] for 
+  hex digits that appear between a '=' and a '&' in a config string.
+
+  If ConfigString is NULL, then ASSERT().
+
+  @param[in] ConfigString  Pointer to a Null-terminated Unicode string.
+
+  @return  Pointer to the Null-terminated Unicode result string.
+
+**/
+EFI_STRING
+EFIAPI
+InternalHiiLowerConfigString (
+  IN EFI_STRING  ConfigString
+  )
+{
+  EFI_STRING  String;
+  BOOLEAN     Lower;
+
+  ASSERT (ConfigString != NULL);
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  for (String = ConfigString, Lower = FALSE; *String != L'\0'; String++) {
+    if (*String == L'=') {
+      Lower = TRUE;
+    } else if (*String == L'&') {
+      Lower = FALSE;
+    } else if (Lower && *String >= L'A' && *String <= L'F') {
+      *String = (CHAR16) (*String - L'A' + L'a');
+    }
+  }
+
+  return ConfigString;
+}
+
+/**
+  Uses the BlockToConfig() service of the Config Routing Protocol to 
+  convert <ConfigRequest> and a buffer to a <ConfigResp>
+
+  If ConfigRequest is NULL, then ASSERT().
+  If Block is NULL, then ASSERT().
+
+  @param[in] ConfigRequest  Pointer to a Null-terminated Unicode string.
+  @param[in] Block          Pointer to a block of data.
+  @param[in] BlockSize      The zie, in bytes, of Block.
+
+  @retval NULL   The <ConfigResp> string could not be generated.
+  @retval Other  Pointer to the Null-terminated Unicode <ConfigResp> string.
+
+**/
+EFI_STRING
+EFIAPI
+InternalHiiBlockToConfig (
+  IN CONST EFI_STRING  ConfigRequest,
+  IN CONST UINT8       *Block,
+  IN UINTN             BlockSize
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STRING  ConfigResp;
+  CHAR16      *Progress;
+
+  ASSERT (ConfigRequest != NULL);
+  ASSERT (Block != NULL);
+
+  //
+  // Convert <ConfigRequest> to <ConfigResp>
+  //
+  Status = gHiiConfigRouting->BlockToConfig (
+                                gHiiConfigRouting,
+                                ConfigRequest,
+                                Block,
+                                BlockSize,
+                                &ConfigResp,
+                                &Progress
+                                );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  return ConfigResp;
+}
+
+/**
+  Uses the BrowserCallback() service of the Form Browser Protocol to retrieve 
+  or set uncommitted data.  If sata i being retrieved, then the buffer is 
+  allocated using AllocatePool().  The caller is then responsible for freeing 
+  the buffer using FreePool().
+
+  @param[in]  VariableGuid    Pointer to an EFI_GUID structure.  This is an optional 
+                              parameter that may be NULL.
+  @param[in]  VariableName    Pointer to a Null-terminated Unicode string.  This 
+                              is an optional parameter that may be NULL.
+  @param[in]  SetResultsData  If not NULL, then this parameter specified the buffer
+                              of uncommited data to set.  If this parameter is NULL,
+                              then the caller is requesting to get the uncommited data
+                              from the Form Browser.
+
+  @retval NULL   The uncommitted data could not be retrieved.
+  @retval Other  A pointer to a buffer containing the uncommitted data.
+
+**/
+EFI_STRING
+EFIAPI
+InternalHiiBrowserCallback (
+  IN CONST EFI_GUID    *VariableGuid,  OPTIONAL
+  IN CONST CHAR16      *VariableName,  OPTIONAL
+  IN CONST EFI_STRING  SetResultsData  OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       ResultsDataSize;
+  EFI_STRING  ResultsData;
+  CHAR16      TempResultsData;
+
+  //
+  // Locate protocols
+  //
+  if (mUefiFormBrowser2 == NULL) {
+    Status = gBS->LocateProtocol (&gEfiFormBrowser2ProtocolGuid, NULL, (VOID **) &mUefiFormBrowser2);
+    if (EFI_ERROR (Status) || mUefiFormBrowser2 == NULL) {
+      return NULL;
+    }
+  }
+
+  ResultsDataSize = 0;
+
+  if (SetResultsData != NULL) {
+    //
+    // Request to to set data in the uncommitted browser state information
+    //
+    ResultsData = SetResultsData;
+  } else {
+    //
+    // Retrieve the length of the buffer required ResultsData from the Browser Callback
+    //
+    Status = mUefiFormBrowser2->BrowserCallback (
+                              mUefiFormBrowser2,
+                              &ResultsDataSize,
+                              &TempResultsData,
+                              TRUE,
+                              VariableGuid,
+                              VariableName
+                              );
+    
+    if (!EFI_ERROR (Status)) {
+      //
+      // No Resluts Data, only allocate one char for '\0'
+      //
+      ResultsData = AllocateZeroPool (sizeof (CHAR16));
+      return ResultsData;
+    }
+
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      return NULL;
+    }
+
+    //
+    // Allocate the ResultsData buffer
+    //
+    ResultsData = AllocateZeroPool (ResultsDataSize);
+    if (ResultsData == NULL) {
+      return NULL;
+    }
+  }
+
+  //
+  // Retrieve or set the ResultsData from the Browser Callback
+  //
+  Status = mUefiFormBrowser2->BrowserCallback (
+                            mUefiFormBrowser2,
+                            &ResultsDataSize,
+                            ResultsData,
+                            (BOOLEAN)(SetResultsData == NULL),
+                            VariableGuid,
+                            VariableName
+                            );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  return ResultsData;
+}
+
+/**
+  Allocates and returns a Null-terminated Unicode <ConfigHdr> string using routing 
+  information that includes a GUID, an optional Unicode string name, and a device
+  path.  The string returned is allocated with AllocatePool().  The caller is 
+  responsible for freeing the allocated string with FreePool().
+  
+  The format of a <ConfigHdr> is as follows:
+
+    GUID=<HexCh>32&NAME=<Char>NameLength&PATH=<HexChar>DevicePathSize<Null>
+
+  @param[in]  Guid          Pointer to an EFI_GUID that is the routing information
+                            GUID.  Each of the 16 bytes in Guid is converted to 
+                            a 2 Unicode character hexidecimal string.  This is 
+                            an optional parameter that may be NULL.
+  @param[in]  Name          Pointer to a Null-terminated Unicode string that is 
+                            the routing information NAME.  This is an optional 
+                            parameter that may be NULL.  Each 16-bit Unicode 
+                            character in Name is converted to a 4 character Unicode 
+                            hexidecimal string.                        
+  @param[in]  DriverHandle  The driver handle which supports a Device Path Protocol
+                            that is the routing information PATH.  Each byte of
+                            the Device Path associated with DriverHandle is converted
+                            to a 2 Unicode character hexidecimal string.
+
+  @retval NULL   DriverHandle does not support the Device Path Protocol.
+  @retval Other  A pointer to the Null-terminate Unicode <ConfigHdr> string
+
+**/
+EFI_STRING
+EFIAPI
+HiiConstructConfigHdr (
+  IN CONST EFI_GUID  *Guid,  OPTIONAL
+  IN CONST CHAR16    *Name,  OPTIONAL
+  IN EFI_HANDLE      DriverHandle
+  )
+{
+  UINTN                     NameLength;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINTN                     DevicePathSize;
+  CHAR16                    *String;
+  CHAR16                    *ReturnString;
+  UINTN                     Index;
+  UINT8                     *Buffer;
+  UINTN                     MaxLen;
+
+  //
+  // Compute the length of Name in Unicode characters.  
+  // If Name is NULL, then the length is 0.
+  //
+  NameLength = 0;
+  if (Name != NULL) {
+    NameLength = StrLen (Name);
+  }
+
+  DevicePath = NULL;
+  DevicePathSize = 0;
+  //
+  // Retrieve DevicePath Protocol associated with DriverHandle
+  //
+  if (DriverHandle != NULL) {
+    DevicePath = DevicePathFromHandle (DriverHandle);
+    if (DevicePath == NULL) {
+      return NULL;
+    }
+    //
+    // Compute the size of the device path in bytes
+    //
+    DevicePathSize = GetDevicePathSize (DevicePath);
+  }
+
+  //
+  // GUID=<HexCh>32&NAME=<Char>NameLength&PATH=<HexChar>DevicePathSize <Null>
+  // | 5 | sizeof (EFI_GUID) * 2 | 6 | NameStrLen*4 | 6 | DevicePathSize * 2 | 1 |
+  //
+  MaxLen = 5 + sizeof (EFI_GUID) * 2 + 6 + NameLength * 4 + 6 + DevicePathSize * 2 + 1;
+  String = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  if (String == NULL) {
+    return NULL;
+  }
+
+  //
+  // Start with L"GUID="
+  //
+  StrCpyS (String, MaxLen, L"GUID=");
+  ReturnString = String;
+  String += StrLen (String);
+
+  if (Guid != NULL) {
+    //
+    // Append Guid converted to <HexCh>32
+    //
+    for (Index = 0, Buffer = (UINT8 *)Guid; Index < sizeof (EFI_GUID); Index++) {
+      String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(Buffer++), 2);
+    }
+  }
+  
+  //
+  // Append L"&NAME="
+  //
+  StrCatS (ReturnString, MaxLen, L"&NAME=");
+  String += StrLen (String);
+
+  if (Name != NULL) {
+    //
+    // Append Name converted to <Char>NameLength
+    //
+    for (; *Name != L'\0'; Name++) {
+      String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *Name, 4);
+    }
+  }
+
+  //
+  // Append L"&PATH="
+  //
+  StrCatS (ReturnString, MaxLen, L"&PATH=");
+  String += StrLen (String);
+
+  //
+  // Append the device path associated with DriverHandle converted to <HexChar>DevicePathSize
+  //
+  for (Index = 0, Buffer = (UINT8 *)DevicePath; Index < DevicePathSize; Index++) {
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(Buffer++), 2);
+  }
+
+  //
+  // Null terminate the Unicode string
+  //
+  *String = L'\0';
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  return InternalHiiLowerConfigString (ReturnString);
+}
+
+/**
+  Convert the hex UNICODE encoding string of UEFI GUID, NAME or device path 
+  to binary buffer from <ConfigHdr>.
+
+  This is a internal function.
+
+  @param  String                 UEFI configuration string.
+  @param  Flag                   Flag specifies what type buffer will be retrieved.
+  @param  Buffer                 Binary of Guid, Name or Device path.
+
+  @retval EFI_INVALID_PARAMETER  Any incoming parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Lake of resources to store neccesary structures.
+  @retval EFI_SUCCESS            The buffer data is retrieved and translated to
+                                 binary format.
+
+**/
+EFI_STATUS
+InternalHiiGetBufferFromString (
+  IN  EFI_STRING                 String,
+  IN  UINT8                      Flag,
+  OUT UINT8                      **Buffer
+  )
+{
+  UINTN      Length;
+  EFI_STRING ConfigHdr;
+  CHAR16     *StringPtr;
+  UINT8      *DataBuffer;
+  CHAR16     TemStr[5];
+  UINTN      Index;
+  UINT8      DigitUint8;
+
+  if (String == NULL || Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  DataBuffer = NULL;
+  StringPtr  = NULL;
+  ConfigHdr  = String;
+  //
+  // The content between 'GUID', 'NAME', 'PATH' of <ConfigHdr> and '&' of next element
+  // or '\0' (end of configuration string) is the UNICODE %02x bytes encoding string.
+  //
+  for (Length = 0; *String != 0 && *String != L'&'; String++, Length++);
+
+  switch (Flag) {
+  case GUID_CONFIG_STRING_TYPE:
+  case PATH_CONFIG_STRING_TYPE:
+    //
+    // The data in <ConfigHdr> is encoded as hex UNICODE %02x bytes in the same order
+    // as the device path and Guid resides in RAM memory.
+    // Translate the data into binary.
+    //
+    DataBuffer = (UINT8 *) AllocateZeroPool ((Length + 1) / 2);
+    if (DataBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Convert binary byte one by one
+    //
+    ZeroMem (TemStr, sizeof (TemStr));
+    for (Index = 0; Index < Length; Index ++) {
+      TemStr[0] = ConfigHdr[Index];
+      DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+      if ((Index & 1) == 0) {
+        DataBuffer [Index/2] = DigitUint8;
+      } else {
+        DataBuffer [Index/2] = (UINT8) ((DataBuffer [Index/2] << 4) + DigitUint8);
+      }
+    }
+    
+    *Buffer = DataBuffer;
+    break;
+
+  case NAME_CONFIG_STRING_TYPE:
+    //
+    // Convert Config String to Unicode String, e.g. "0041004200430044" => "ABCD"
+    // 
+
+    //
+    // Add the tailling char L'\0'
+    //
+    DataBuffer = (UINT8 *) AllocateZeroPool ((Length/4 + 1) * sizeof (CHAR16));
+    if (DataBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Convert character one by one
+    //
+    StringPtr = (CHAR16 *) DataBuffer;
+    ZeroMem (TemStr, sizeof (TemStr));
+    for (Index = 0; Index < Length; Index += 4) {
+      StrnCpyS (TemStr, sizeof (TemStr) / sizeof (CHAR16), ConfigHdr + Index, 4);
+      StringPtr[Index/4] = (CHAR16) StrHexToUint64 (TemStr);
+    }
+    //
+    // Add tailing L'\0' character
+    //
+    StringPtr[Index/4] = L'\0';
+
+    *Buffer = DataBuffer;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function checks VarOffset and VarWidth is in the block range.
+
+  @param  BlockArray         The block array is to be checked. 
+  @param  VarOffset          Offset of var to the structure
+  @param  VarWidth           Width of var.
+  
+  @retval TRUE   This Var is in the block range.
+  @retval FALSE  This Var is not in the block range.
+**/
+BOOLEAN
+BlockArrayCheck (
+  IN IFR_BLOCK_DATA  *BlockArray,
+  IN UINT16          VarOffset,
+  IN UINT16          VarWidth
+  )
+{
+  LIST_ENTRY          *Link;
+  IFR_BLOCK_DATA      *BlockData;
+  
+  //
+  // No Request Block array, all vars are got.
+  //
+  if (BlockArray == NULL) {
+    return TRUE;
+  }
+  
+  //
+  // Check the input var is in the request block range.
+  //
+  for (Link = BlockArray->Entry.ForwardLink; Link != &BlockArray->Entry; Link = Link->ForwardLink) {
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    if ((VarOffset >= BlockData->Offset) && ((VarOffset + VarWidth) <= (BlockData->Offset + BlockData->Width))) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the value of <Number> in <BlockConfig> format, i.e. the value of OFFSET
+  or WIDTH or VALUE.
+  <BlockConfig> ::= 'OFFSET='<Number>&'WIDTH='<Number>&'VALUE'=<Number>
+
+  @param  ValueString            String in <BlockConfig> format and points to the
+                                 first character of <Number>.
+  @param  ValueData              The output value. Caller takes the responsibility
+                                 to free memory.
+  @param  ValueLength            Length of the <Number>, in characters.
+
+  @retval EFI_OUT_OF_RESOURCES   Insufficient resources to store neccessary
+                                 structures.
+  @retval EFI_SUCCESS            Value of <Number> is outputted in Number
+                                 successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalHiiGetValueOfNumber (
+  IN  EFI_STRING           ValueString,
+  OUT UINT8                **ValueData,
+  OUT UINTN                *ValueLength
+  )
+{
+  EFI_STRING               StringPtr;
+  UINTN                    Length;
+  UINT8                    *Buf;
+  UINT8                    DigitUint8;
+  UINTN                    Index;
+  CHAR16                   TemStr[2];
+
+  ASSERT (ValueString != NULL && ValueData != NULL && ValueLength != NULL);
+  ASSERT (*ValueString != L'\0');
+
+  //
+  // Get the length of value string
+  //
+  StringPtr = ValueString;
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr++;
+  }
+  Length = StringPtr - ValueString;
+  
+  //
+  // Allocate buffer to store the value
+  //
+  Buf = (UINT8 *) AllocateZeroPool ((Length + 1) / 2);
+  if (Buf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Convert character one by one to the value buffer
+  //
+  ZeroMem (TemStr, sizeof (TemStr));
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = ValueString[Length - Index - 1];
+    DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+    if ((Index & 1) == 0) {
+      Buf [Index/2] = DigitUint8;
+    } else {
+      Buf [Index/2] = (UINT8) ((DigitUint8 << 4) + Buf [Index/2]);
+    }
+  }
+  
+  //
+  // Set the converted value and string length.
+  //
+  *ValueData    = Buf;
+  *ValueLength  = Length;
+  return EFI_SUCCESS;
+}
+
+/**
+  Get value from config request resp string.
+
+  @param ConfigElement           ConfigResp string contains the current setting.
+  @param VarName                 The variable name which need to get value.
+  @param VarValue                The return value.
+  
+  @retval EFI_SUCCESS            Get the value for the VarName
+  @retval EFI_OUT_OF_RESOURCES   The memory is not enough.
+**/
+EFI_STATUS
+GetValueFromRequest (
+  IN CHAR16                       *ConfigElement,
+  IN CHAR16                       *VarName,
+  OUT UINT64                      *VarValue
+  )
+{
+  UINT8                        *TmpBuffer;
+  CHAR16                       *StringPtr;
+  UINTN                        Length;
+  EFI_STATUS                   Status;
+
+  //
+  // Find VarName related string.
+  //
+  StringPtr = StrStr (ConfigElement, VarName);
+  ASSERT (StringPtr != NULL);
+
+  //
+  // Skip the "VarName=" string
+  //
+  StringPtr += StrLen (VarName) + 1;
+
+  //
+  // Get Offset
+  //
+  Status = InternalHiiGetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  *VarValue = 0;
+  CopyMem (VarValue, TmpBuffer, (((Length + 1) / 2) < sizeof (UINT64)) ? ((Length + 1) / 2) : sizeof (UINT64));
+
+  FreePool (TmpBuffer);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This internal function parses IFR data to validate current setting.
+
+  Base on the NameValueType, if it is TRUE, RequestElement and HiiHandle is valid;
+  else the VarBuffer and CurrentBlockArray is valid.
+
+  @param HiiPackageList     Point to Hii package list.
+  @param PackageListLength  The length of the pacakge.
+  @param VarGuid            Guid of the buffer storage.
+  @param VarName            Name of the buffer storage.
+  @param VarBuffer          The data buffer for the storage.
+  @param CurrentBlockArray  The block array from the config Requst string.
+  @param RequestElement     The config string for this storage.
+  @param HiiHandle          The HiiHandle for this formset.
+  @param NameValueType      Whether current storage is name/value varstore or not.
+  
+  @retval EFI_SUCCESS            The current setting is valid.
+  @retval EFI_OUT_OF_RESOURCES   The memory is not enough.
+  @retval EFI_INVALID_PARAMETER  The config string or the Hii package is invalid.
+**/
+EFI_STATUS
+ValidateQuestionFromVfr (
+  IN EFI_HII_PACKAGE_LIST_HEADER   *HiiPackageList,
+  IN UINTN                         PackageListLength,
+  IN EFI_GUID                      *VarGuid,
+  IN CHAR16                        *VarName,
+  IN UINT8                         *VarBuffer,
+  IN IFR_BLOCK_DATA                *CurrentBlockArray,
+  IN CHAR16                        *RequestElement,
+  IN EFI_HII_HANDLE                HiiHandle,
+  IN BOOLEAN                       NameValueType
+  )
+{
+  IFR_BLOCK_DATA               VarBlockData;
+  UINT16                       Offset;
+  UINT16                       Width;
+  UINT64                       VarValue;
+  EFI_IFR_TYPE_VALUE           TmpValue;
+  EFI_STATUS                   Status;
+  EFI_HII_PACKAGE_HEADER       PacakgeHeader;
+  UINT32                       PackageOffset;
+  UINT8                        *PackageData;
+  UINTN                        IfrOffset;
+  EFI_IFR_OP_HEADER            *IfrOpHdr;
+  EFI_IFR_VARSTORE             *IfrVarStore;
+  EFI_IFR_VARSTORE_NAME_VALUE  *IfrNameValueStore;
+  EFI_IFR_VARSTORE_EFI         *IfrEfiVarStore;
+  IFR_VARSTORAGE_DATA          VarStoreData;
+  EFI_IFR_ONE_OF               *IfrOneOf;
+  EFI_IFR_NUMERIC              *IfrNumeric;
+  EFI_IFR_ONE_OF_OPTION        *IfrOneOfOption;
+  EFI_IFR_CHECKBOX             *IfrCheckBox;
+  EFI_IFR_STRING               *IfrString;
+  CHAR8                        *VarStoreName;
+  UINTN                        Index;
+  CHAR16                       *QuestionName;
+  CHAR16                       *StringPtr;
+
+  //
+  // Initialize the local variables.
+  //
+  Index             = 0;
+  VarStoreName      = NULL;
+  Status            = EFI_SUCCESS;
+  VarValue          = 0;
+  IfrVarStore       = NULL;
+  IfrNameValueStore = NULL;
+  IfrEfiVarStore    = NULL;
+  ZeroMem (&VarStoreData, sizeof (IFR_VARSTORAGE_DATA));
+  ZeroMem (&VarBlockData, sizeof (VarBlockData));
+
+  //
+  // Check IFR value is in block data, then Validate Value
+  //
+  PackageOffset = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  while (PackageOffset < PackageListLength) {
+    CopyMem (&PacakgeHeader, (UINT8 *) HiiPackageList + PackageOffset, sizeof (PacakgeHeader));
+
+    //
+    // Parse IFR opcode from the form package.
+    //
+    if (PacakgeHeader.Type == EFI_HII_PACKAGE_FORMS) {
+      IfrOffset   = sizeof (PacakgeHeader);
+      PackageData = (UINT8 *) HiiPackageList + PackageOffset;
+      while (IfrOffset < PacakgeHeader.Length) {
+        IfrOpHdr = (EFI_IFR_OP_HEADER *) (PackageData + IfrOffset);
+        //
+        // Validate current setting to the value built in IFR opcode
+        //
+        switch (IfrOpHdr->OpCode) {
+        case EFI_IFR_VARSTORE_OP:
+          //
+          // VarStoreId has been found. No further found.
+          //
+          if (VarStoreData.VarStoreId != 0) {
+            break;
+          }
+          //
+          // Find the matched VarStoreId to the input VarGuid and VarName
+          //
+          IfrVarStore = (EFI_IFR_VARSTORE *) IfrOpHdr;
+          if (CompareGuid ((EFI_GUID *) (VOID *) &IfrVarStore->Guid, VarGuid)) {
+            VarStoreName = (CHAR8 *) IfrVarStore->Name;
+            for (Index = 0; VarStoreName[Index] != 0; Index ++) {
+              if ((CHAR16) VarStoreName[Index] != VarName[Index]) {
+                break;
+              }
+            }
+            //
+            // The matched VarStore is found.
+            //
+            if ((VarStoreName[Index] != 0) || (VarName[Index] != 0)) {
+              IfrVarStore = NULL;
+            }
+          } else {
+            IfrVarStore = NULL;
+          }
+
+          if (IfrVarStore != NULL) {
+            VarStoreData.VarStoreId = IfrVarStore->VarStoreId;
+            VarStoreData.Size       = IfrVarStore->Size;
+          }
+          break;
+        case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+          //
+          // VarStoreId has been found. No further found.
+          //
+          if (VarStoreData.VarStoreId != 0) {
+            break;
+          }
+          //
+          // Find the matched VarStoreId to the input VarGuid
+          //
+          IfrNameValueStore = (EFI_IFR_VARSTORE_NAME_VALUE *) IfrOpHdr;
+          if (!CompareGuid ((EFI_GUID *) (VOID *) &IfrNameValueStore->Guid, VarGuid)) {
+            IfrNameValueStore = NULL;
+          }
+
+          if (IfrNameValueStore != NULL) {
+            VarStoreData.VarStoreId = IfrNameValueStore->VarStoreId;
+          }
+          break;
+        case EFI_IFR_VARSTORE_EFI_OP:
+          //
+          // VarStore is found. Don't need to search any more.
+          //
+          if (VarStoreData.VarStoreId != 0) {
+            break;
+          }
+
+          IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpHdr;
+
+          //
+          // If the length is small than the structure, this is from old efi 
+          // varstore definition. Old efi varstore get config directly from 
+          // GetVariable function.
+          //
+          if (IfrOpHdr->Length < sizeof (EFI_IFR_VARSTORE_EFI)) {
+            break;
+          }
+
+          if (CompareGuid ((EFI_GUID *) (VOID *) &IfrEfiVarStore->Guid, VarGuid)) {
+            VarStoreName = (CHAR8 *) IfrEfiVarStore->Name;
+            for (Index = 0; VarStoreName[Index] != 0; Index ++) {
+              if ((CHAR16) VarStoreName[Index] != VarName[Index]) {
+                break;
+              }
+            }
+            //
+            // The matched VarStore is found.
+            //
+            if ((VarStoreName[Index] != 0) || (VarName[Index] != 0)) {
+              IfrEfiVarStore = NULL;
+            }
+          } else {
+            IfrEfiVarStore = NULL;
+          }
+
+          if (IfrEfiVarStore != NULL) {
+            //
+            // Find the matched VarStore
+            //
+            VarStoreData.VarStoreId = IfrEfiVarStore->VarStoreId;
+            VarStoreData.Size       = IfrEfiVarStore->Size;
+          }
+          break;
+        case EFI_IFR_FORM_OP:
+        case EFI_IFR_FORM_MAP_OP:
+          //
+          // Check the matched VarStoreId is found.
+          //
+          if (VarStoreData.VarStoreId == 0) {
+            return EFI_SUCCESS;
+          }
+          break;
+        case EFI_IFR_ONE_OF_OP:
+          //
+          // Check whether current value is the one of option.
+          //
+
+          //
+          // OneOf question is not in IFR Form. This IFR form is not valid. 
+          //
+          if (VarStoreData.VarStoreId == 0) {
+            return EFI_INVALID_PARAMETER;
+          }
+          // 
+          // Check whether this question is for the requested varstore.
+          //
+          IfrOneOf = (EFI_IFR_ONE_OF *) IfrOpHdr;
+          if (IfrOneOf->Question.VarStoreId != VarStoreData.VarStoreId) {
+            break;
+          }
+
+          if (NameValueType) {
+            QuestionName = HiiGetString (HiiHandle, IfrOneOf->Question.VarStoreInfo.VarName, NULL);
+            ASSERT (QuestionName != NULL);
+
+            if (StrStr (RequestElement, QuestionName) == NULL) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+
+            Status = GetValueFromRequest (RequestElement, QuestionName, &VarValue);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+          } else {
+            //
+            // Get Offset by Question header and Width by DataType Flags
+            //
+            Offset = IfrOneOf->Question.VarStoreInfo.VarOffset;
+            Width  = (UINT16) (1 << (IfrOneOf->Flags & EFI_IFR_NUMERIC_SIZE));
+            //
+            // Check whether this question is in current block array.
+            //
+            if (!BlockArrayCheck (CurrentBlockArray, Offset, Width)) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            //
+            // Check this var question is in the var storage 
+            //
+            if ((Offset + Width) > VarStoreData.Size) {
+              //
+              // This question exceeds the var store size. 
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+
+            //
+            // Get the current value for oneof opcode
+            //
+            VarValue = 0;
+            CopyMem (&VarValue, VarBuffer +  Offset, Width);
+          }
+          //
+          // Set Block Data, to be checked in the following Oneof option opcode.
+          //
+          VarBlockData.OpCode     = IfrOpHdr->OpCode;
+          VarBlockData.Scope      = IfrOpHdr->Scope;
+          break;
+        case EFI_IFR_NUMERIC_OP:
+          //
+          // Check the current value is in the numeric range.
+          //
+
+          //
+          // Numeric question is not in IFR Form. This IFR form is not valid. 
+          //
+          if (VarStoreData.VarStoreId == 0) {
+            return EFI_INVALID_PARAMETER;
+          }
+          //
+          // Check whether this question is for the requested varstore.
+          //
+          IfrNumeric = (EFI_IFR_NUMERIC *) IfrOpHdr;
+          if (IfrNumeric->Question.VarStoreId != VarStoreData.VarStoreId) {
+            break;
+          }
+
+          if (NameValueType) {
+            QuestionName = HiiGetString (HiiHandle, IfrNumeric->Question.VarStoreInfo.VarName, NULL);
+            ASSERT (QuestionName != NULL);
+
+            if (StrStr (RequestElement, QuestionName) == NULL) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            
+            Status = GetValueFromRequest (RequestElement, QuestionName, &VarValue);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+          } else {
+            //
+            // Get Offset by Question header and Width by DataType Flags
+            //
+            Offset = IfrNumeric->Question.VarStoreInfo.VarOffset;
+            Width  = (UINT16) (1 << (IfrNumeric->Flags & EFI_IFR_NUMERIC_SIZE));
+            //
+            // Check whether this question is in current block array.
+            //
+            if (!BlockArrayCheck (CurrentBlockArray, Offset, Width)) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            //
+            // Check this var question is in the var storage 
+            //
+            if ((Offset + Width) > VarStoreData.Size) {
+              //
+              // This question exceeds the var store size. 
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+
+            //
+            // Check the current value is in the numeric range.
+            //
+            VarValue = 0;
+            CopyMem (&VarValue, VarBuffer +  Offset, Width);
+          }
+          if ((IfrNumeric->Flags & EFI_IFR_DISPLAY) == 0) {
+            switch (IfrNumeric->Flags & EFI_IFR_NUMERIC_SIZE) {
+            case EFI_IFR_NUMERIC_SIZE_1:
+              if ((INT8) VarValue < (INT8) IfrNumeric->data.u8.MinValue || (INT8) VarValue > (INT8) IfrNumeric->data.u8.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_2:
+              if ((INT16) VarValue < (INT16) IfrNumeric->data.u16.MinValue || (INT16) VarValue > (INT16) IfrNumeric->data.u16.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_4:
+              if ((INT32) VarValue < (INT32) IfrNumeric->data.u32.MinValue || (INT32) VarValue > (INT32) IfrNumeric->data.u32.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_8:
+              if ((INT64) VarValue < (INT64) IfrNumeric->data.u64.MinValue || (INT64) VarValue > (INT64) IfrNumeric->data.u64.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            }
+          } else {
+            switch (IfrNumeric->Flags & EFI_IFR_NUMERIC_SIZE) {
+            case EFI_IFR_NUMERIC_SIZE_1:
+              if ((UINT8) VarValue < IfrNumeric->data.u8.MinValue || (UINT8) VarValue > IfrNumeric->data.u8.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_2:
+              if ((UINT16) VarValue < IfrNumeric->data.u16.MinValue || (UINT16) VarValue > IfrNumeric->data.u16.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_4:
+              if ((UINT32) VarValue < IfrNumeric->data.u32.MinValue || (UINT32) VarValue > IfrNumeric->data.u32.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            case EFI_IFR_NUMERIC_SIZE_8:
+              if ((UINT64) VarValue < IfrNumeric->data.u64.MinValue || (UINT64) VarValue > IfrNumeric->data.u64.MaxValue) {
+                //
+                // Not in the valid range.
+                //
+                return EFI_INVALID_PARAMETER;
+              }
+              break;
+            }
+          }
+          break;
+        case EFI_IFR_CHECKBOX_OP:
+          //
+          // Check value is BOOLEAN type, only 0 and 1 is valid.
+          //
+
+          //
+          // CheckBox question is not in IFR Form. This IFR form is not valid. 
+          //
+          if (VarStoreData.VarStoreId == 0) {
+            return EFI_INVALID_PARAMETER;
+          }
+
+          //
+          // Check whether this question is for the requested varstore.
+          //
+          IfrCheckBox = (EFI_IFR_CHECKBOX *) IfrOpHdr;
+          if (IfrCheckBox->Question.VarStoreId != VarStoreData.VarStoreId) {
+            break;
+          }
+
+          if (NameValueType) {
+            QuestionName = HiiGetString (HiiHandle, IfrCheckBox->Question.VarStoreInfo.VarName, NULL);
+            ASSERT (QuestionName != NULL);
+
+            if (StrStr (RequestElement, QuestionName) == NULL) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            
+            Status = GetValueFromRequest (RequestElement, QuestionName, &VarValue);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+          } else {
+            //
+            // Get Offset by Question header
+            //
+            Offset = IfrCheckBox->Question.VarStoreInfo.VarOffset;
+            Width  = (UINT16) sizeof (BOOLEAN);
+            //
+            // Check whether this question is in current block array.
+            //
+            if (!BlockArrayCheck (CurrentBlockArray, Offset, Width)) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            //
+            // Check this var question is in the var storage 
+            //
+            if ((Offset + Width) > VarStoreData.Size) {
+              //
+              // This question exceeds the var store size. 
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+            //
+            // Check the current value is in the numeric range.
+            //
+            VarValue = 0;
+            CopyMem (&VarValue, VarBuffer +  Offset, Width);
+          }
+          //
+          // Boolean type, only 1 and 0 is valid.
+          //
+          if (VarValue > 1) {
+            return EFI_INVALID_PARAMETER;
+          }
+          break;
+        case EFI_IFR_STRING_OP:
+          //
+          // Check current string length is less than maxsize
+          //
+
+          //
+          // CheckBox question is not in IFR Form. This IFR form is not valid. 
+          //
+          if (VarStoreData.VarStoreId == 0) {
+            return EFI_INVALID_PARAMETER;
+          }
+
+          //
+          // Check whether this question is for the requested varstore.
+          //
+          IfrString = (EFI_IFR_STRING *) IfrOpHdr;
+          if (IfrString->Question.VarStoreId != VarStoreData.VarStoreId) {
+            break;
+          }
+          //
+          // Get Width by OneOf Flags
+          //
+          Width  = (UINT16) (IfrString->MaxSize * sizeof (UINT16));
+          if (NameValueType) {
+            QuestionName = HiiGetString (HiiHandle, IfrString->Question.VarStoreInfo.VarName, NULL);
+            ASSERT (QuestionName != NULL);
+
+            StringPtr = StrStr (RequestElement, QuestionName);
+            if (StringPtr == NULL) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+
+            //
+            // Skip the "=".
+            // 
+            StringPtr += 1;
+            
+            //
+            // Check current string length is less than maxsize
+            //
+            if (StrSize (StringPtr) > Width) {
+              return EFI_INVALID_PARAMETER;
+            }
+          } else {
+            //
+            // Get Offset/Width by Question header and OneOf Flags
+            //
+            Offset = IfrString->Question.VarStoreInfo.VarOffset;
+            //
+            // Check whether this question is in current block array.
+            //
+            if (!BlockArrayCheck (CurrentBlockArray, Offset, Width)) {
+              //
+              // This question is not in the current configuration string. Skip it.
+              //
+              break;
+            }
+            //
+            // Check this var question is in the var storage 
+            //
+            if ((Offset + Width) > VarStoreData.Size) {
+              //
+              // This question exceeds the var store size. 
+              //
+              return EFI_INVALID_PARAMETER;
+            }
+
+            //
+            // Check current string length is less than maxsize
+            //
+            if (StrSize ((CHAR16 *) (VarBuffer + Offset)) > Width) {
+              return EFI_INVALID_PARAMETER;
+            }
+          }
+          break;
+        case EFI_IFR_ONE_OF_OPTION_OP:
+          //
+          // Opcode Scope is zero. This one of option is not to be checked. 
+          //
+          if (VarBlockData.Scope == 0) {
+            break;
+          }
+
+          //
+          // Only check for OneOf and OrderList opcode
+          //
+          IfrOneOfOption = (EFI_IFR_ONE_OF_OPTION *) IfrOpHdr;
+          if (VarBlockData.OpCode == EFI_IFR_ONE_OF_OP) {
+            //
+            // Check current value is the value of one of option.
+            //
+            ASSERT (IfrOneOfOption->Type <= EFI_IFR_TYPE_NUM_SIZE_64);
+            ZeroMem (&TmpValue, sizeof (EFI_IFR_TYPE_VALUE));
+            CopyMem (&TmpValue, &IfrOneOfOption->Value, IfrOneOfOption->Header.Length - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+            if (VarValue == TmpValue.u64) {
+              //
+              // The value is one of option value.
+              // Set OpCode to Zero, don't need check again.
+              //
+              VarBlockData.OpCode = 0;
+            }
+          }
+          break;
+        case EFI_IFR_END_OP:
+          //
+          // Decrease opcode scope for the validated opcode
+          //
+          if (VarBlockData.Scope > 0) {
+            VarBlockData.Scope --;
+          }
+
+          //
+          // OneOf value doesn't belong to one of option value. 
+          //
+          if ((VarBlockData.Scope == 0) && (VarBlockData.OpCode == EFI_IFR_ONE_OF_OP)) {
+            return EFI_INVALID_PARAMETER;
+          }
+          break;
+        default:
+          //
+          // Increase Scope for the validated opcode
+          //
+          if (VarBlockData.Scope > 0) {
+            VarBlockData.Scope = (UINT8) (VarBlockData.Scope + IfrOpHdr->Scope);
+          }
+          break;
+        }
+        //
+        // Go to the next opcode
+        //
+        IfrOffset += IfrOpHdr->Length;
+      }
+      //
+      // Only one form is in a package list.
+      //
+      break;
+    }
+
+    //
+    // Go to next package.
+    //
+    PackageOffset += PacakgeHeader.Length;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This internal function parses IFR data to validate current setting.
+
+  @param ConfigElement         ConfigResp element string contains the current setting.
+  @param CurrentBlockArray     Current block array.
+  @param VarBuffer             Data buffer for this varstore.
+  
+  @retval EFI_SUCCESS            The current setting is valid.
+  @retval EFI_OUT_OF_RESOURCES   The memory is not enough.
+  @retval EFI_INVALID_PARAMETER  The config string or the Hii package is invalid.
+**/
+EFI_STATUS
+GetBlockDataInfo (
+  IN  CHAR16                        *ConfigElement,
+  OUT IFR_BLOCK_DATA                **CurrentBlockArray,
+  OUT UINT8                         **VarBuffer
+  )
+{
+  IFR_BLOCK_DATA               *BlockData;
+  IFR_BLOCK_DATA               *NewBlockData;
+  EFI_STRING                   StringPtr;
+  UINTN                        Length;
+  UINT8                        *TmpBuffer;
+  UINT16                       Offset;
+  UINT16                       Width;
+  LIST_ENTRY                   *Link;
+  UINTN                        MaxBufferSize;
+  EFI_STATUS                   Status;
+  IFR_BLOCK_DATA               *BlockArray;
+  UINT8                        *DataBuffer;
+  
+  //
+  // Initialize the local variables.
+  //
+  Status        = EFI_SUCCESS;
+  BlockData     = NULL;
+  NewBlockData  = NULL;
+  TmpBuffer     = NULL;
+  BlockArray    = NULL;
+  MaxBufferSize = HII_LIB_DEFAULT_VARSTORE_SIZE;
+  DataBuffer     = AllocateZeroPool (MaxBufferSize);
+  if (DataBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Init BlockArray
+  //
+  BlockArray = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+  if (BlockArray == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  InitializeListHead (&BlockArray->Entry);
+
+  StringPtr = StrStr (ConfigElement, L"&OFFSET=");
+  ASSERT (StringPtr != NULL);
+
+  //
+  // Parse each <RequestElement> if exists
+  // Only <BlockName> format is supported by this help function.
+  // <BlockName> ::= &'OFFSET='<Number>&'WIDTH='<Number>
+  //
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"&OFFSET=", StrLen (L"&OFFSET=")) == 0) {
+    //
+    // Skip the &OFFSET= string
+    // 
+    StringPtr += StrLen (L"&OFFSET=");
+
+    //
+    // Get Offset
+    //
+    Status = InternalHiiGetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    Offset = 0;
+    CopyMem (
+      &Offset,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINT16)) ? ((Length + 1) / 2) : sizeof (UINT16)
+      );
+    FreePool (TmpBuffer);
+    TmpBuffer = NULL;
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&WIDTH=");
+
+    //
+    // Get Width
+    //
+    Status = InternalHiiGetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    Width = 0;
+    CopyMem (
+      &Width,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINT16)) ? ((Length + 1) / 2) : sizeof (UINT16)
+      );
+    FreePool (TmpBuffer);
+    TmpBuffer = NULL;
+
+    StringPtr += Length;
+    if (*StringPtr != 0 && *StringPtr != L'&') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    if (StrnCmp (StringPtr, L"&VALUE=", StrLen (L"&VALUE=")) != 0) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&VALUE=");
+
+    //
+    // Get Value
+    //
+    Status = InternalHiiGetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    StringPtr += Length;
+    if (*StringPtr != 0 && *StringPtr != L'&') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    //
+    // Check whether VarBuffer is enough
+    //
+    if ((UINTN) (Offset + Width) > MaxBufferSize) {
+      DataBuffer = ReallocatePool (
+                    MaxBufferSize,
+                    Offset + Width + HII_LIB_DEFAULT_VARSTORE_SIZE,
+                    DataBuffer
+                    );
+      if (DataBuffer == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      MaxBufferSize = Offset + Width + HII_LIB_DEFAULT_VARSTORE_SIZE;
+    }
+
+    //
+    // Update the Block with configuration info
+    //
+    CopyMem (DataBuffer + Offset, TmpBuffer, Width);
+    FreePool (TmpBuffer);
+    TmpBuffer = NULL;
+
+    //
+    // Set new Block Data
+    //
+    NewBlockData = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+    if (NewBlockData == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    NewBlockData->Offset = Offset;
+    NewBlockData->Width  = Width;
+
+    //
+    // Insert the new block data into the block data array.
+    //
+    for (Link = BlockArray->Entry.ForwardLink; Link != &BlockArray->Entry; Link = Link->ForwardLink) {
+      BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+      if (NewBlockData->Offset == BlockData->Offset) {
+        if (NewBlockData->Width > BlockData->Width) {
+          BlockData->Width = NewBlockData->Width;
+        }
+        FreePool (NewBlockData);
+        break;
+      } else if (NewBlockData->Offset < BlockData->Offset) {
+        //
+        // Insert new block data as the previous one of this link.
+        //
+        InsertTailList (Link, &NewBlockData->Entry);
+        break;
+      }
+    }
+
+    //
+    // Insert new block data into the array tail.
+    //
+    if (Link == &BlockArray->Entry) {
+      InsertTailList (Link, &NewBlockData->Entry);
+    }
+
+    //
+    // If '\0', parsing is finished. 
+    //
+    if (*StringPtr == 0) {
+      break;
+    }
+    //
+    // Go to next ConfigBlock 
+    //
+  }
+
+  //
+  // Merge the aligned block data into the single block data.
+  //
+  Link = BlockArray->Entry.ForwardLink;
+  while ((Link != &BlockArray->Entry) && (Link->ForwardLink != &BlockArray->Entry)) {
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    NewBlockData = BASE_CR (Link->ForwardLink, IFR_BLOCK_DATA, Entry);
+    if ((NewBlockData->Offset >= BlockData->Offset) && (NewBlockData->Offset <= (BlockData->Offset + BlockData->Width))) {
+      if ((NewBlockData->Offset + NewBlockData->Width) > (BlockData->Offset + BlockData->Width)) {
+        BlockData->Width = (UINT16) (NewBlockData->Offset + NewBlockData->Width - BlockData->Offset);
+      }
+      RemoveEntryList (Link->ForwardLink);
+      FreePool (NewBlockData);
+      continue;
+    }
+    Link = Link->ForwardLink;
+  }
+
+  *VarBuffer         = DataBuffer;
+  *CurrentBlockArray = BlockArray;
+  return EFI_SUCCESS;
+
+Done:
+  if (DataBuffer != NULL) {
+    FreePool (DataBuffer);
+  }
+  
+  if (BlockArray != NULL) {
+    //
+    // Free Link Array CurrentBlockArray
+    //
+    while (!IsListEmpty (&BlockArray->Entry)) {
+      BlockData = BASE_CR (BlockArray->Entry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      FreePool (BlockData);
+    }
+    FreePool (BlockArray);
+  }
+
+  return Status;
+}
+
+/**
+  This internal function parses IFR data to validate current setting.
+
+  @param ConfigResp         ConfigResp string contains the current setting.
+  @param HiiPackageList     Point to Hii package list.
+  @param PackageListLength  The length of the pacakge.
+  @param VarGuid            Guid of the buffer storage.
+  @param VarName            Name of the buffer storage.
+  @param HiiHandle          The HiiHandle for this package.
+  
+  @retval EFI_SUCCESS            The current setting is valid.
+  @retval EFI_OUT_OF_RESOURCES   The memory is not enough.
+  @retval EFI_INVALID_PARAMETER  The config string or the Hii package is invalid.
+**/
+EFI_STATUS
+EFIAPI
+InternalHiiValidateCurrentSetting (
+  IN EFI_STRING                    ConfigResp,
+  IN EFI_HII_PACKAGE_LIST_HEADER   *HiiPackageList,
+  IN UINTN                         PackageListLength,
+  IN EFI_GUID                      *VarGuid,
+  IN CHAR16                        *VarName,
+  IN EFI_HII_HANDLE                HiiHandle
+  )
+{
+  CHAR16              *StringPtr;
+  EFI_STATUS          Status;
+  IFR_BLOCK_DATA      *CurrentBlockArray;
+  IFR_BLOCK_DATA      *BlockData;
+  UINT8               *VarBuffer;
+  BOOLEAN             NameValueType;
+
+  CurrentBlockArray = NULL;
+  VarBuffer         = NULL;
+  StringPtr         = NULL;
+  Status            = EFI_SUCCESS;
+
+  //
+  // If StringPtr != NULL, get the request elements.
+  //
+  if (StrStr (ConfigResp, L"&OFFSET=") != NULL) {
+    Status = GetBlockDataInfo(ConfigResp, &CurrentBlockArray, &VarBuffer);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    NameValueType = FALSE;
+  } else {
+    //
+    // Skip header part.
+    //
+    StringPtr = StrStr (ConfigResp, L"PATH=");
+    ASSERT (StringPtr != NULL);
+
+    if (StrStr (StringPtr, L"&") != NULL) {
+      NameValueType = TRUE;
+    } else {
+      //
+      // Not found Request element, return success.
+      //
+      return EFI_SUCCESS;
+    }
+  }
+
+  Status = ValidateQuestionFromVfr(
+                          HiiPackageList,
+                          PackageListLength,
+                          VarGuid,
+                          VarName,
+                          VarBuffer,
+                          CurrentBlockArray,
+                          ConfigResp,
+                          HiiHandle,
+                          NameValueType
+                          );
+
+  if (VarBuffer != NULL) {
+    FreePool (VarBuffer);
+  }
+  
+  if (CurrentBlockArray != NULL) {
+    //
+    // Free Link Array CurrentBlockArray
+    //
+    while (!IsListEmpty (&CurrentBlockArray->Entry)) {
+      BlockData = BASE_CR (CurrentBlockArray->Entry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      FreePool (BlockData);
+    }
+    FreePool (CurrentBlockArray);
+  }
+
+  return Status;
+}
+
+/**
+  Check whether the ConfigRequest string has the request elements.
+  For EFI_HII_VARSTORE_BUFFER type, the request has "&OFFSET=****&WIDTH=****..." format.
+  For EFI_HII_VARSTORE_NAME_VALUE type, the request has "&NAME1**&NAME2..." format.
+
+  @param  ConfigRequest      The input config request string.
+
+  @retval  TRUE              The input include config request elements.
+  @retval  FALSE             The input string not includes.
+                                 
+**/
+BOOLEAN
+GetElementsFromRequest (
+  IN EFI_STRING    ConfigRequest
+  )
+{
+  EFI_STRING   TmpRequest;
+
+  TmpRequest = StrStr (ConfigRequest, L"PATH=");
+  ASSERT (TmpRequest != NULL);
+
+  if ((StrStr (TmpRequest, L"&OFFSET=") != NULL) || (StrStr (TmpRequest, L"&") != NULL)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  This function parses the input ConfigRequest string and its matched IFR code
+  string for setting default value and validating current setting.
+
+  1. For setting default action, Reset the default value specified by DefaultId 
+  to the driver configuration got by Request string.
+  2. For validating current setting, Validate the current configuration 
+  by parsing HII form IFR opcode.
+
+  NULL request string support depends on the ExportConfig interface of
+  HiiConfigRouting protocol in UEFI specification.
+  
+  @param Request    A null-terminated Unicode string in 
+                    <MultiConfigRequest> format. It can be NULL.
+                    If it is NULL, all current configuration for the
+                    entirety of the current HII database will be validated.
+                    If it is NULL, all configuration for the
+                    entirety of the current HII database will be reset.
+  @param DefaultId  Specifies the type of defaults to retrieve only for setting default action.
+  @param ActionType Action supports setting defaults and validate current setting.
+  
+  @retval TURE    Action runs successfully.
+  @retval FALSE   Action is not valid or Action can't be executed successfully..
+**/
+BOOLEAN
+EFIAPI
+InternalHiiIfrValueAction (
+  IN CONST EFI_STRING Request,  OPTIONAL
+  IN UINT16           DefaultId,
+  IN UINT8            ActionType
+  )
+{
+  EFI_STRING     ConfigAltResp;
+  EFI_STRING     ConfigAltHdr;
+  EFI_STRING     ConfigResp;
+  EFI_STRING     Progress;
+  EFI_STRING     StringPtr;
+  EFI_STRING     StringHdr;
+  EFI_STATUS     Status;
+  EFI_HANDLE     DriverHandle;
+  EFI_HANDLE     TempDriverHandle;
+  EFI_HII_HANDLE *HiiHandleBuffer;
+  EFI_HII_HANDLE HiiHandle;
+  UINT32         Index;
+  EFI_GUID       *VarGuid;
+  EFI_STRING     VarName;
+
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINTN                        PackageListLength;
+  UINTN                        MaxLen;
+  EFI_DEVICE_PATH_PROTOCOL     *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL     *TempDevicePath;
+
+  ConfigAltResp = NULL;
+  ConfigResp    = NULL;
+  VarGuid       = NULL;
+  VarName       = NULL;
+  DevicePath    = NULL;
+  ConfigAltHdr  = NULL;
+  HiiHandleBuffer  = NULL;
+  Index            = 0;
+  TempDriverHandle = NULL;
+  HiiHandle        = NULL;
+  HiiPackageList   = NULL;
+  
+  //
+  // Only support set default and validate setting action.
+  //
+  if ((ActionType != ACTION_SET_DEFAUTL_VALUE) && (ActionType != ACTION_VALIDATE_SETTING)) {
+    return FALSE;
+  }
+
+  //
+  // Get the full requested value and deault value string.
+  //
+  if (Request != NULL) {
+    Status = gHiiConfigRouting->ExtractConfig (
+                                  gHiiConfigRouting,
+                                  Request,
+                                  &Progress,
+                                  &ConfigAltResp
+                                );
+  } else {
+    Status = gHiiConfigRouting->ExportConfig (
+                                  gHiiConfigRouting,
+                                  &ConfigAltResp
+                                );
+  }
+  
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  
+  StringPtr = ConfigAltResp;
+  
+  while (StringPtr != L'\0') {
+    //
+    // 1. Find <ConfigHdr> GUID=...&NAME=...&PATH=...
+    //
+    StringHdr = StringPtr;
+
+    //
+    // Get Guid value
+    //
+    if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"GUID=");
+    Status = InternalHiiGetBufferFromString (StringPtr, GUID_CONFIG_STRING_TYPE, (UINT8 **) &VarGuid);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // Get Name value VarName
+    //
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&NAME=", StrLen (L"&NAME=")) != 0) {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&NAME=");
+    Status = InternalHiiGetBufferFromString (StringPtr, NAME_CONFIG_STRING_TYPE, (UINT8 **) &VarName);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    
+    //
+    // Get Path value DevicePath
+    //
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&PATH=", StrLen (L"&PATH=")) != 0) {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&PATH=");
+    Status = InternalHiiGetBufferFromString (StringPtr, PATH_CONFIG_STRING_TYPE, (UINT8 **) &DevicePath);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // Get the Driver handle by the got device path.
+    //
+    TempDevicePath = DevicePath;
+    Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &TempDevicePath, &DriverHandle);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    
+    //
+    // Find the matched Hii Handle for the found Driver handle
+    //
+    HiiHandleBuffer = HiiGetHiiHandles (NULL);
+    if (HiiHandleBuffer == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+
+    for (Index = 0; HiiHandleBuffer[Index] != NULL; Index ++) {
+      gHiiDatabase->GetPackageListHandle (gHiiDatabase, HiiHandleBuffer[Index], &TempDriverHandle);
+      if (TempDriverHandle == DriverHandle) {
+        break;
+      }
+    }
+
+    HiiHandle = HiiHandleBuffer[Index];
+    FreePool (HiiHandleBuffer);
+
+    if (HiiHandle == NULL) {
+      //
+      // This request string has no its Hii package.
+      // Its default value and validating can't execute by parsing IFR data.
+      // Directly jump into the next ConfigAltResp string for another pair Guid, Name, and Path.   
+      //
+      Status = EFI_SUCCESS;
+      goto NextConfigAltResp;
+    }
+
+    //
+    // 2. Get HiiPackage by HiiHandle
+    //
+    PackageListLength  = 0;
+    HiiPackageList     = NULL;
+    Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, HiiHandle, &PackageListLength, HiiPackageList);
+  
+    //
+    // The return status should always be EFI_BUFFER_TOO_SMALL as input buffer's size is 0.
+    //
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+  
+    HiiPackageList = AllocatePool (PackageListLength);
+    if (HiiPackageList == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+  
+    //
+    // Get PackageList on HiiHandle
+    //
+    Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, HiiHandle, &PackageListLength, HiiPackageList);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    
+    //
+    // 3. Call ConfigRouting GetAltCfg(ConfigRoute, <ConfigResponse>, Guid, Name, DevicePath, AltCfgId, AltCfgResp)
+    //    Get the default configuration string according to the default ID.
+    //
+    Status = gHiiConfigRouting->GetAltConfig (
+                                  gHiiConfigRouting,
+                                  ConfigAltResp,
+                                  VarGuid,
+                                  VarName,
+                                  DevicePath,
+                                  (ActionType == ACTION_SET_DEFAUTL_VALUE) ? &DefaultId:NULL,  // it can be NULL to get the current setting.
+                                  &ConfigResp
+                                );
+    
+    //
+    // The required setting can't be found. So, it is not required to be validated and set.
+    //
+    if (EFI_ERROR (Status)) {
+      Status = EFI_SUCCESS;
+      goto NextConfigAltResp;
+    }
+    //
+    // Only the ConfigHdr is found. Not any block data is found. No data is required to be validated and set.
+    //
+    if (!GetElementsFromRequest (ConfigResp)) {
+      goto NextConfigAltResp;
+    }
+    
+    //
+    // 4. Set the default configuration information or Validate current setting by parse IFR code.
+    //    Current Setting is in ConfigResp, will be set into buffer, then check it again.
+    //
+    if (ActionType == ACTION_SET_DEFAUTL_VALUE) {
+      //
+      // Set the default configuration information.
+      //
+      Status = gHiiConfigRouting->RouteConfig (gHiiConfigRouting, ConfigResp, &Progress);
+    } else {
+      //
+      // Current Setting is in ConfigResp, will be set into buffer, then check it again.
+      //
+      Status = InternalHiiValidateCurrentSetting (ConfigResp, HiiPackageList, PackageListLength, VarGuid, VarName, HiiHandle);
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+NextConfigAltResp:
+    //
+    // Free the allocated pacakge buffer and the got ConfigResp string.
+    //
+    if (HiiPackageList != NULL) {
+      FreePool (HiiPackageList);
+      HiiPackageList = NULL;
+    }
+    
+    if (ConfigResp != NULL) {
+      FreePool (ConfigResp);
+      ConfigResp = NULL;
+    }
+
+    //
+    // Free the allocated buffer.
+    //
+    FreePool (VarGuid);
+    VarGuid = NULL;
+  
+    FreePool (VarName);
+    VarName = NULL;
+  
+    FreePool (DevicePath);
+    DevicePath = NULL;
+
+    //
+    // 5. Jump to next ConfigAltResp for another Guid, Name, Path.
+    //
+
+    //
+    // Get and Skip ConfigHdr
+    //
+    while (*StringPtr != L'\0' && *StringPtr != L'&') {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      break;
+    }
+        
+    //
+    // Construct ConfigAltHdr string  "&<ConfigHdr>&ALTCFG=\0" 
+    //                               | 1 | StrLen (ConfigHdr) | 8 | 1 |
+    //
+    MaxLen = 1 + StringPtr - StringHdr + 8 + 1;
+    ConfigAltHdr = AllocateZeroPool ( MaxLen * sizeof (CHAR16));
+    if (ConfigAltHdr == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    StrCpyS (ConfigAltHdr, MaxLen, L"&");
+    StrnCatS (ConfigAltHdr, MaxLen, StringHdr, StringPtr - StringHdr);
+    StrCatS (ConfigAltHdr, MaxLen, L"&ALTCFG=");
+    
+    //
+    // Skip all AltResp (AltConfigHdr ConfigBody) for the same ConfigHdr
+    //
+    while ((StringHdr = StrStr (StringPtr, ConfigAltHdr)) != NULL) {
+      StringPtr = StringHdr + StrLen (ConfigAltHdr);
+      if (*StringPtr == L'\0') {
+        break;
+      }
+    }
+    
+    //
+    // Free the allocated ConfigAltHdr string
+    //
+    FreePool (ConfigAltHdr);
+    if (*StringPtr == L'\0') {
+      break;
+    }
+    
+    //
+    // Find &GUID as the next ConfigHdr
+    //
+    StringPtr = StrStr (StringPtr, L"&GUID");
+    if (StringPtr == NULL) {
+      break;
+    }
+
+    //
+    // Skip char '&'
+    //
+    StringPtr ++;
+  }
+  
+Done:
+  if (VarGuid != NULL) {
+    FreePool (VarGuid);
+  }
+
+  if (VarName != NULL) {
+    FreePool (VarName);
+  }
+
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+
+  if (ConfigResp != NULL) {
+    FreePool (ConfigResp);
+  }
+
+  if (ConfigAltResp != NULL) {
+    FreePool (ConfigAltResp);
+  }
+ 
+  if (HiiPackageList != NULL) {
+    FreePool (HiiPackageList);
+  }
+  
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Validate the current configuration by parsing HII form IFR opcode.
+
+  NULL request string support depends on the ExportConfig interface of
+  HiiConfigRouting protocol in UEFI specification.
+  
+  @param  Request   A null-terminated Unicode string in 
+                    <MultiConfigRequest> format. It can be NULL.
+                    If it is NULL, all current configuration for the
+                    entirety of the current HII database will be validated.
+  
+  @retval TRUE    Current configuration is valid.
+  @retval FALSE   Current configuration is invalid.
+**/
+BOOLEAN
+EFIAPI                               
+HiiValidateSettings (
+  IN CONST EFI_STRING Request  OPTIONAL
+  )
+{
+  return InternalHiiIfrValueAction (Request, 0, ACTION_VALIDATE_SETTING);
+}
+
+/**
+  Reset the default value specified by DefaultId to the driver
+  configuration got by Request string. 
+
+  NULL request string support depends on the ExportConfig interface of
+  HiiConfigRouting protocol in UEFI specification.
+  
+  @param Request    A null-terminated Unicode string in 
+                    <MultiConfigRequest> format. It can be NULL.
+                    If it is NULL, all configuration for the
+                    entirety of the current HII database will be reset.
+  @param DefaultId  Specifies the type of defaults to retrieve.
+  
+  @retval TURE    The default value is set successfully.
+  @retval FALSE   The default value can't be found and set.
+**/
+BOOLEAN
+EFIAPI
+HiiSetToDefaults (
+  IN CONST EFI_STRING Request,  OPTIONAL
+  IN UINT16        DefaultId
+  )
+{
+  return InternalHiiIfrValueAction (Request, DefaultId, ACTION_SET_DEFAUTL_VALUE);
+}
+
+/**
+  Determines if two values in config strings match.
+
+  Compares the substring between StartSearchString and StopSearchString in 
+  FirstString to the substring between StartSearchString and StopSearchString 
+  in SecondString.  If the two substrings match, then TRUE is returned.  If the
+  two substrings do not match, then FALSE is returned.
+
+  If FirstString is NULL, then ASSERT().
+  If SecondString is NULL, then ASSERT().
+  If StartSearchString is NULL, then ASSERT().
+  If StopSearchString is NULL, then ASSERT().
+
+  @param FirstString        Pointer to the first Null-terminated Unicode string.
+  @param SecondString       Pointer to the second Null-terminated Unicode string.
+  @param StartSearchString  Pointer to the Null-terminated Unicode string that 
+                            marks the start of the value string to compare.
+  @param StopSearchString   Pointer to the Null-terminated Unicode string that 
+                            marks the end of the value string to compare.
+
+  @retval FALSE             StartSearchString is not present in FirstString. 
+  @retval FALSE             StartSearchString is not present in SecondString.
+  @retval FALSE             StopSearchString is not present in FirstString. 
+  @retval FALSE             StopSearchString is not present in SecondString.
+  @retval FALSE             The length of the substring in FirstString is not the 
+                            same length as the substring in SecondString.
+  @retval FALSE             The value string in FirstString does not matche the 
+                            value string in SecondString.
+  @retval TRUE              The value string in FirstString matches the value 
+                            string in SecondString.
+
+**/
+BOOLEAN
+EFIAPI
+InternalHiiCompareSubString (
+  IN CHAR16  *FirstString,
+  IN CHAR16  *SecondString,
+  IN CHAR16  *StartSearchString,
+  IN CHAR16  *StopSearchString
+  )
+{
+  CHAR16  *EndFirstString;
+  CHAR16  *EndSecondString;
+
+  ASSERT (FirstString != NULL);
+  ASSERT (SecondString != NULL);
+  ASSERT (StartSearchString != NULL);
+  ASSERT (StopSearchString != NULL);
+
+  FirstString = StrStr (FirstString, StartSearchString);
+  if (FirstString == NULL) {
+    return FALSE;
+  }
+
+  SecondString = StrStr (SecondString, StartSearchString);
+  if (SecondString == NULL) {
+    return FALSE;
+  }
+
+  EndFirstString = StrStr (FirstString, StopSearchString);
+  if (EndFirstString == NULL) {
+    return FALSE;
+  }
+
+  EndSecondString = StrStr (SecondString, StopSearchString);
+  if (EndSecondString == NULL) {
+    return FALSE;
+  }
+
+  if ((EndFirstString - FirstString) != (EndSecondString - SecondString)) {
+    return FALSE;
+  }
+
+  return (BOOLEAN)(StrnCmp (FirstString, SecondString, EndFirstString - FirstString) == 0);
+}
+
+/**
+  Determines if the routing data specified by GUID and NAME match a <ConfigHdr>.
+
+  If ConfigHdr is NULL, then ASSERT().
+
+  @param[in] ConfigHdr  Either <ConfigRequest> or <ConfigResp>.
+  @param[in] Guid       GUID of the storage.
+  @param[in] Name       NAME of the storage.
+
+  @retval TRUE   Routing information matches <ConfigHdr>.
+  @retval FALSE  Routing information does not match <ConfigHdr>.
+
+**/
+BOOLEAN
+EFIAPI
+HiiIsConfigHdrMatch (
+  IN CONST EFI_STRING  ConfigHdr,
+  IN CONST EFI_GUID    *Guid,     OPTIONAL
+  IN CONST CHAR16      *Name      OPTIONAL
+  )
+{
+  EFI_STRING  CompareConfigHdr;
+  BOOLEAN     Result;
+
+  ASSERT (ConfigHdr != NULL);
+
+  //
+  // Use Guid and Name to generate a <ConfigHdr> string
+  //
+  CompareConfigHdr = HiiConstructConfigHdr (Guid, Name, NULL);
+  if (CompareConfigHdr == NULL) {
+    return FALSE;
+  }
+
+  Result = TRUE;
+  if (Guid != NULL) {
+    //
+    // Compare GUID value strings
+    //
+    Result = InternalHiiCompareSubString (ConfigHdr, CompareConfigHdr, L"GUID=", L"&NAME=");
+  }
+
+  if (Result && Name != NULL) {
+    //
+    // Compare NAME value strings
+    //
+    Result = InternalHiiCompareSubString (ConfigHdr, CompareConfigHdr, L"&NAME=", L"&PATH=");
+  }
+
+  //
+  // Free the <ConfigHdr> string
+  //
+  FreePool (CompareConfigHdr);
+
+  return Result;
+}
+
+/**
+  Retrieves uncommitted data from the Form Browser and converts it to a binary
+  buffer.
+
+  @param[in]  VariableGuid  Pointer to an EFI_GUID structure.  This is an optional 
+                            parameter that may be NULL.
+  @param[in]  VariableName  Pointer to a Null-terminated Unicode string.  This 
+                            is an optional parameter that may be NULL.
+  @param[in]  BufferSize    Length in bytes of buffer to hold retrieved data. 
+  @param[out] Buffer        Buffer of data to be updated.
+
+  @retval FALSE  The uncommitted data could not be retrieved.
+  @retval TRUE   The uncommitted data was retrieved.
+
+**/
+BOOLEAN
+EFIAPI
+HiiGetBrowserData (
+  IN CONST EFI_GUID  *VariableGuid,  OPTIONAL
+  IN CONST CHAR16    *VariableName,  OPTIONAL
+  IN UINTN           BufferSize,
+  OUT UINT8          *Buffer
+  )
+{
+  EFI_STRING  ResultsData;
+  UINTN       Size;
+  EFI_STRING  ConfigResp;
+  EFI_STATUS  Status;
+  CHAR16      *Progress;
+
+  //
+  // Retrieve the results data from the Browser Callback
+  //
+  ResultsData = InternalHiiBrowserCallback (VariableGuid, VariableName, NULL);
+  if (ResultsData == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Construct <ConfigResp> mConfigHdrTemplate L'&' ResultsData L'\0'
+  //
+  Size = (StrLen (mConfigHdrTemplate) + 1) * sizeof (CHAR16);
+  Size = Size + (StrLen (ResultsData) + 1) * sizeof (CHAR16);
+  ConfigResp = AllocateZeroPool (Size);
+  UnicodeSPrint (ConfigResp, Size, L"%s&%s", mConfigHdrTemplate, ResultsData);
+  
+  //
+  // Free the allocated buffer
+  //
+  FreePool (ResultsData);
+  if (ConfigResp == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Convert <ConfigResp> to a buffer
+  //
+  Status = gHiiConfigRouting->ConfigToBlock (
+                                gHiiConfigRouting,
+                                ConfigResp,
+                                Buffer,
+                                &BufferSize,
+                                &Progress
+                                );
+  //
+  // Free the allocated buffer
+  //
+  FreePool (ConfigResp);
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Updates uncommitted data in the Form Browser.
+
+  If Buffer is NULL, then ASSERT().
+
+  @param[in]  VariableGuid    Pointer to an EFI_GUID structure.  This is an optional
+                              parameter that may be NULL.
+  @param[in]  VariableName    Pointer to a Null-terminated Unicode string.  This
+                              is an optional parameter that may be NULL.
+  @param[in]  BufferSize      Length, in bytes, of Buffer.
+  @param[in]  Buffer          Buffer of data to commit.
+  @param[in]  RequestElement  An optional field to specify which part of the
+                              buffer data will be send back to Browser. If NULL,
+                              the whole buffer of data will be committed to
+                              Browser. 
+                              <RequestElement> ::= &OFFSET=<Number>&WIDTH=<Number>*
+
+  @retval FALSE  The uncommitted data could not be updated.
+  @retval TRUE   The uncommitted data was updated.
+
+**/
+BOOLEAN
+EFIAPI
+HiiSetBrowserData (
+  IN CONST EFI_GUID  *VariableGuid, OPTIONAL
+  IN CONST CHAR16    *VariableName, OPTIONAL
+  IN UINTN           BufferSize,
+  IN CONST UINT8     *Buffer,
+  IN CONST CHAR16    *RequestElement  OPTIONAL
+  )
+{
+  UINTN       Size;
+  EFI_STRING  ConfigRequest;
+  EFI_STRING  ConfigResp;
+  EFI_STRING  ResultsData;
+
+  ASSERT (Buffer != NULL);
+
+  //
+  // Construct <ConfigRequest>
+  //
+  if (RequestElement == NULL) {
+    //
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template 
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    Size = (StrLen (mConfigHdrTemplate) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", mConfigHdrTemplate, (UINT64)BufferSize);
+  } else {
+    //
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template 
+    // followed by <RequestElement> followed by a Null-terminator
+    //
+    Size = StrLen (mConfigHdrTemplate) * sizeof (CHAR16);
+    Size = Size + (StrLen (RequestElement) + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    UnicodeSPrint (ConfigRequest, Size, L"%s%s", mConfigHdrTemplate, RequestElement);
+  }
+  if (ConfigRequest == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Convert <ConfigRequest> to <ConfigResp>
+  //
+  ConfigResp = InternalHiiBlockToConfig (ConfigRequest, Buffer, BufferSize);
+  FreePool (ConfigRequest);
+  if (ConfigResp == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Set data in the uncommitted browser state information
+  //
+  ResultsData = InternalHiiBrowserCallback (VariableGuid, VariableName, ConfigResp + StrLen(mConfigHdrTemplate) + 1);
+  FreePool (ConfigResp);
+
+  return (BOOLEAN)(ResultsData != NULL);
+}
+
+/////////////////////////////////////////
+/////////////////////////////////////////
+/// IFR Functions
+/////////////////////////////////////////
+/////////////////////////////////////////
+
+#define HII_LIB_OPCODE_ALLOCATION_SIZE  0x200
+
+typedef struct {
+  UINT8  *Buffer;
+  UINTN  BufferSize;
+  UINTN  Position;
+} HII_LIB_OPCODE_BUFFER;
+
+///
+/// Lookup table that converts EFI_IFR_TYPE_X enum values to a width in bytes
+///
+GLOBAL_REMOVE_IF_UNREFERENCED CONST UINT8 mHiiDefaultTypeToWidth[] = {
+  1, // EFI_IFR_TYPE_NUM_SIZE_8
+  2, // EFI_IFR_TYPE_NUM_SIZE_16
+  4, // EFI_IFR_TYPE_NUM_SIZE_32
+  8, // EFI_IFR_TYPE_NUM_SIZE_64
+  1, // EFI_IFR_TYPE_BOOLEAN
+  3, // EFI_IFR_TYPE_TIME
+  4, // EFI_IFR_TYPE_DATE
+  2  // EFI_IFR_TYPE_STRING
+};
+
+/**
+  Allocates and returns a new OpCode Handle.  OpCode Handles must be freed with 
+  HiiFreeOpCodeHandle().
+
+  @retval NULL   There are not enough resources to allocate a new OpCode Handle.
+  @retval Other  A new OpCode handle.
+
+**/
+VOID *
+EFIAPI
+HiiAllocateOpCodeHandle (
+  VOID
+  )
+{
+  HII_LIB_OPCODE_BUFFER  *OpCodeBuffer;
+
+  OpCodeBuffer = (HII_LIB_OPCODE_BUFFER *)AllocatePool (sizeof (HII_LIB_OPCODE_BUFFER));
+  if (OpCodeBuffer == NULL) {
+    return NULL;
+  }
+  OpCodeBuffer->Buffer = (UINT8 *)AllocatePool (HII_LIB_OPCODE_ALLOCATION_SIZE);
+  if (OpCodeBuffer->Buffer == NULL) {
+    FreePool (OpCodeBuffer);
+    return NULL;
+  }
+  OpCodeBuffer->BufferSize = HII_LIB_OPCODE_ALLOCATION_SIZE;
+  OpCodeBuffer->Position = 0;
+  return (VOID *)OpCodeBuffer;
+}
+
+/**
+  Frees an OpCode Handle that was previously allocated with HiiAllocateOpCodeHandle().
+  When an OpCode Handle is freed, all of the opcodes associated with the OpCode
+  Handle are also freed.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+
+**/
+VOID
+EFIAPI
+HiiFreeOpCodeHandle (
+  VOID  *OpCodeHandle
+  )
+{
+  HII_LIB_OPCODE_BUFFER  *OpCodeBuffer;
+
+  ASSERT (OpCodeHandle != NULL);
+
+  OpCodeBuffer = (HII_LIB_OPCODE_BUFFER *)OpCodeHandle;
+  if (OpCodeBuffer->Buffer != NULL) {
+    FreePool (OpCodeBuffer->Buffer);
+  }
+  FreePool (OpCodeBuffer);
+}
+
+/**
+  Internal function gets the current position of opcode buffer.
+  
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+
+  @return Current position of opcode buffer.
+**/
+UINTN
+EFIAPI
+InternalHiiOpCodeHandlePosition (
+  IN VOID  *OpCodeHandle
+  )
+{
+  return ((HII_LIB_OPCODE_BUFFER  *)OpCodeHandle)->Position;
+}
+
+/**
+  Internal function gets the start pointer of opcode buffer.
+  
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+
+  @return Pointer to the opcode buffer base.
+**/
+UINT8 *
+EFIAPI
+InternalHiiOpCodeHandleBuffer (
+  IN VOID  *OpCodeHandle
+  )
+{
+  return ((HII_LIB_OPCODE_BUFFER  *)OpCodeHandle)->Buffer;
+}
+
+/**
+  Internal function reserves the enough buffer for current opcode.
+  When the buffer is not enough, Opcode buffer will be extended.
+  
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+  @param[in]  Size           Size of current opcode.
+
+  @return Pointer to the current opcode.
+**/
+UINT8 *
+EFIAPI
+InternalHiiGrowOpCodeHandle (
+  IN VOID   *OpCodeHandle,
+  IN UINTN  Size
+  )
+{
+  HII_LIB_OPCODE_BUFFER  *OpCodeBuffer;
+  UINT8                  *Buffer;
+
+  ASSERT (OpCodeHandle != NULL);
+
+  OpCodeBuffer = (HII_LIB_OPCODE_BUFFER *)OpCodeHandle;
+  if (OpCodeBuffer->Position + Size > OpCodeBuffer->BufferSize) {
+    Buffer = ReallocatePool (
+              OpCodeBuffer->BufferSize, 
+              OpCodeBuffer->BufferSize + (Size + HII_LIB_OPCODE_ALLOCATION_SIZE),
+              OpCodeBuffer->Buffer
+              );
+    ASSERT (Buffer != NULL);
+    OpCodeBuffer->Buffer = Buffer;
+    OpCodeBuffer->BufferSize += (Size + HII_LIB_OPCODE_ALLOCATION_SIZE);
+  }
+  Buffer = OpCodeBuffer->Buffer + OpCodeBuffer->Position;
+  OpCodeBuffer->Position += Size;
+  return Buffer;
+}
+
+/**
+  Internal function creates opcode based on the template opcode.
+  
+  @param[in]  OpCodeHandle    Handle to the buffer of opcodes.
+  @param[in]  OpCodeTemplate  Pointer to the template buffer of opcode.
+  @param[in]  OpCode          OpCode IFR value.
+  @param[in]  OpCodeSize      Size of opcode.
+  @param[in]  ExtensionSize   Size of extended opcode.
+  @param[in]  Scope           Scope bit of opcode.
+
+  @return Pointer to the current opcode with opcode data.
+**/
+UINT8 *
+EFIAPI
+InternalHiiCreateOpCodeExtended (
+  IN VOID   *OpCodeHandle,
+  IN VOID   *OpCodeTemplate,
+  IN UINT8  OpCode,
+  IN UINTN  OpCodeSize,
+  IN UINTN  ExtensionSize,
+  IN UINT8  Scope
+  )
+{
+  EFI_IFR_OP_HEADER  *Header;
+  UINT8              *Buffer;
+
+  ASSERT (OpCodeTemplate != NULL);
+  ASSERT ((OpCodeSize + ExtensionSize) <= 0x7F);
+
+  Header = (EFI_IFR_OP_HEADER *)OpCodeTemplate;
+  Header->OpCode = OpCode;
+  Header->Scope  = Scope;
+  Header->Length = (UINT8)(OpCodeSize + ExtensionSize);
+  Buffer = InternalHiiGrowOpCodeHandle (OpCodeHandle, Header->Length);
+  return (UINT8 *)CopyMem (Buffer, Header, OpCodeSize);
+}
+
+/**
+  Internal function creates opcode based on the template opcode for the normal opcode.
+  
+  @param[in]  OpCodeHandle    Handle to the buffer of opcodes.
+  @param[in]  OpCodeTemplate  Pointer to the template buffer of opcode.
+  @param[in]  OpCode          OpCode IFR value.
+  @param[in]  OpCodeSize      Size of opcode.
+
+  @return Pointer to the current opcode with opcode data.
+**/
+UINT8 *
+EFIAPI
+InternalHiiCreateOpCode (
+  IN VOID   *OpCodeHandle,
+  IN VOID   *OpCodeTemplate,
+  IN UINT8  OpCode,
+  IN UINTN  OpCodeSize
+  )
+{
+  return InternalHiiCreateOpCodeExtended (OpCodeHandle, OpCodeTemplate, OpCode, OpCodeSize, 0, 0);
+}
+
+/**
+  Append raw opcodes to an OpCodeHandle.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If RawBuffer is NULL, then ASSERT();
+
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+  @param[in]  RawBuffer      Buffer of opcodes to append.
+  @param[in]  RawBufferSize  The size, in bytes, of Buffer.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the appended opcodes.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateRawOpCodes (
+  IN VOID   *OpCodeHandle,
+  IN UINT8  *RawBuffer,
+  IN UINTN  RawBufferSize
+  )
+{
+  UINT8  *Buffer;
+
+  ASSERT (RawBuffer != NULL);
+
+  Buffer = InternalHiiGrowOpCodeHandle (OpCodeHandle, RawBufferSize);
+  return (UINT8 *)CopyMem (Buffer, RawBuffer, RawBufferSize);
+}
+
+/**
+  Append opcodes from one OpCode Handle to another OpCode handle.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If RawOpCodeHandle is NULL, then ASSERT();
+
+  @param[in]  OpCodeHandle     Handle to the buffer of opcodes.
+  @param[in]  RawOpCodeHandle  Handle to the buffer of opcodes.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the appended opcodes.
+
+**/
+UINT8 *
+EFIAPI
+InternalHiiAppendOpCodes (
+  IN VOID  *OpCodeHandle,
+  IN VOID  *RawOpCodeHandle
+  )
+{
+  HII_LIB_OPCODE_BUFFER  *RawOpCodeBuffer;
+
+  ASSERT (RawOpCodeHandle != NULL);
+
+  RawOpCodeBuffer = (HII_LIB_OPCODE_BUFFER *)RawOpCodeHandle;
+  return HiiCreateRawOpCodes (OpCodeHandle, RawOpCodeBuffer->Buffer, RawOpCodeBuffer->Position);
+}
+
+/**
+  Create EFI_IFR_END_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateEndOpCode (
+  IN VOID  *OpCodeHandle
+  )
+{
+  EFI_IFR_END  OpCode;
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_END_OP, sizeof (OpCode));
+}
+
+/**
+  Create EFI_IFR_ONE_OF_OPTION_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Type is invalid, then ASSERT().
+  If Flags is invalid, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  StringId      StringId for the option
+  @param[in]  Flags         Flags for the option
+  @param[in]  Type          Type for the option
+  @param[in]  Value         Value for the option
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOneOfOptionOpCode (
+  IN VOID    *OpCodeHandle,
+  IN UINT16  StringId,
+  IN UINT8   Flags,
+  IN UINT8   Type,
+  IN UINT64  Value
+  )
+{
+  EFI_IFR_ONE_OF_OPTION  OpCode;
+
+  ASSERT (Type < EFI_IFR_TYPE_OTHER);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Option = StringId;
+  OpCode.Flags  = (UINT8) (Flags & (EFI_IFR_OPTION_DEFAULT | EFI_IFR_OPTION_DEFAULT_MFG));
+  OpCode.Type   = Type;
+  CopyMem (&OpCode.Value, &Value, mHiiDefaultTypeToWidth[Type]);
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_ONE_OF_OPTION_OP, OFFSET_OF(EFI_IFR_ONE_OF_OPTION, Value) + mHiiDefaultTypeToWidth[Type]);
+}
+
+/**
+  Create EFI_IFR_DEFAULT_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Type is invalid, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  DefaultId     DefaultId for the default
+  @param[in]  Type          Type for the default
+  @param[in]  Value         Value for the default
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateDefaultOpCode (
+  IN VOID    *OpCodeHandle,
+  IN UINT16  DefaultId,
+  IN UINT8   Type,
+  IN UINT64  Value
+  )
+{
+  EFI_IFR_DEFAULT  OpCode;
+
+  ASSERT (Type < EFI_IFR_TYPE_OTHER);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Type      = Type;
+  OpCode.DefaultId = DefaultId;
+  CopyMem (&OpCode.Value, &Value, mHiiDefaultTypeToWidth[Type]);
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_DEFAULT_OP, OFFSET_OF(EFI_IFR_DEFAULT, Value) + mHiiDefaultTypeToWidth[Type]);
+}
+
+/**
+  Create EFI_IFR_GUID opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If Guid is NULL, then ASSERT().
+  If OpCodeSize < sizeof (EFI_IFR_GUID), then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  Guid          Pointer to EFI_GUID of this guided opcode.
+  @param[in]  GuidOpCode    Pointer to an EFI_IFR_GUID opcode.  This is an 
+                            optional parameter that may be NULL.  If this
+                            parameter is NULL, then the GUID extension 
+                            region of the created opcode is filled with zeros.
+                            If this parameter is not NULL, then the GUID 
+                            extension region of GuidData will be copied to 
+                            the GUID extension region of the created opcode.
+  @param[in]  OpCodeSize    The size, in bytes, of created opcode.  This value 
+                            must be >= sizeof(EFI_IFR_GUID).
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGuidOpCode (
+  IN VOID            *OpCodeHandle,
+  IN CONST EFI_GUID  *Guid,
+  IN CONST VOID      *GuidOpCode,    OPTIONAL
+  IN UINTN           OpCodeSize
+  )
+{
+  EFI_IFR_GUID  OpCode;
+  EFI_IFR_GUID  *OpCodePointer;
+
+  ASSERT (Guid != NULL);
+  ASSERT (OpCodeSize >= sizeof (OpCode));
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  CopyGuid ((EFI_GUID *)(VOID *)&OpCode.Guid, Guid);
+
+  OpCodePointer = (EFI_IFR_GUID *)InternalHiiCreateOpCodeExtended (
+                                    OpCodeHandle, 
+                                    &OpCode,
+                                    EFI_IFR_GUID_OP,
+                                    sizeof (OpCode),
+                                    OpCodeSize - sizeof (OpCode),
+                                    0
+                                    );
+  if (OpCodePointer != NULL && GuidOpCode != NULL) {
+    CopyMem (OpCodePointer + 1, (EFI_IFR_GUID *)GuidOpCode + 1, OpCodeSize - sizeof (OpCode));
+  }
+  return (UINT8 *)OpCodePointer;
+}
+
+/**
+  Create EFI_IFR_ACTION_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  QuestionId      Question ID
+  @param[in]  Prompt          String ID for Prompt
+  @param[in]  Help            String ID for Help
+  @param[in]  QuestionFlags   Flags in Question Header
+  @param[in]  QuestionConfig  String ID for configuration
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateActionOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_STRING_ID    QuestionConfig
+  )
+{
+  EFI_IFR_ACTION  OpCode;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.QuestionId    = QuestionId;
+  OpCode.Question.Header.Prompt = Prompt;
+  OpCode.Question.Header.Help   = Help;
+  OpCode.Question.Flags         = QuestionFlags;
+  OpCode.QuestionConfig         = QuestionConfig;
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_ACTION_OP, sizeof (OpCode));
+}
+
+/**
+  Create EFI_IFR_SUBTITLE_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in Flags, then ASSERT().
+  If Scope > 1, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  Prompt      String ID for Prompt
+  @param[in]  Help        String ID for Help
+  @param[in]  Flags       Subtitle opcode flags
+  @param[in]  Scope       1 if this opcpde is the beginning of a new scope.
+                          0 if this opcode is within the current scope.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateSubTitleOpCode (
+  IN VOID           *OpCodeHandle,
+  IN EFI_STRING_ID  Prompt,
+  IN EFI_STRING_ID  Help,
+  IN UINT8          Flags,
+  IN UINT8          Scope
+  )
+{
+  EFI_IFR_SUBTITLE  OpCode;
+
+  ASSERT (Scope <= 1);
+  ASSERT ((Flags & (~(EFI_IFR_FLAGS_HORIZONTAL))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Statement.Prompt = Prompt;
+  OpCode.Statement.Help   = Help;
+  OpCode.Flags            = Flags;
+
+  return InternalHiiCreateOpCodeExtended (
+           OpCodeHandle, 
+           &OpCode,
+           EFI_IFR_SUBTITLE_OP, 
+           sizeof (OpCode), 
+           0, 
+           Scope
+           );
+}
+
+/**
+  Create EFI_IFR_REF_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle   Handle to the buffer of opcodes.
+  @param[in]  FormId         Destination Form ID
+  @param[in]  Prompt         String ID for Prompt
+  @param[in]  Help           String ID for Help
+  @param[in]  QuestionFlags  Flags in Question Header
+  @param[in]  QuestionId     Question ID
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGotoOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_FORM_ID      FormId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_QUESTION_ID  QuestionId
+  )
+{
+  EFI_IFR_REF  OpCode;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt = Prompt;
+  OpCode.Question.Header.Help   = Help;
+  OpCode.Question.QuestionId    = QuestionId;
+  OpCode.Question.Flags         = QuestionFlags;
+  OpCode.FormId                 = FormId;
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_REF_OP, sizeof (OpCode));
+}
+
+/**
+  Create EFI_IFR_REF_OP, EFI_IFR_REF2_OP, EFI_IFR_REF3_OP and EFI_IFR_REF4_OP opcode.
+
+  When RefDevicePath is not zero, EFI_IFR_REF4 opcode will be created. 
+  When RefDevicePath is zero and RefFormSetId is not NULL, EFI_IFR_REF3 opcode will be created.
+  When RefDevicePath is zero, RefFormSetId is NULL and RefQuestionId is not zero, EFI_IFR_REF2 opcode will be created.
+  When RefDevicePath is zero, RefFormSetId is NULL and RefQuestionId is zero, EFI_IFR_REF opcode will be created.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle   The handle to the buffer of opcodes.
+  @param[in]  RefFormId      The Destination Form ID.
+  @param[in]  Prompt         The string ID for Prompt.
+  @param[in]  Help           The string ID for Help.
+  @param[in]  QuestionFlags  The flags in Question Header
+  @param[in]  QuestionId     Question ID.
+  @param[in]  RefQuestionId  The question on the form to which this link is referring. 
+                             If its value is zero, then the link refers to the top of the form.
+  @param[in]  RefFormSetId   The form set to which this link is referring. If its value is NULL, and RefDevicePath is 
+                             zero, then the link is to the current form set.
+  @param[in]  RefDevicePath  The string identifier that specifies the string containing the text representation of 
+                             the device path to which the form set containing the form specified by FormId.
+                             If its value is zero, then the link refers to the current page.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateGotoExOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_FORM_ID      RefFormId,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_QUESTION_ID  RefQuestionId,
+  IN EFI_GUID         *RefFormSetId,    OPTIONAL
+  IN EFI_STRING_ID    RefDevicePath
+  )
+{
+  EFI_IFR_REF4  OpCode;
+  UINTN         OpCodeSize;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt = Prompt;
+  OpCode.Question.Header.Help   = Help;
+  OpCode.Question.QuestionId    = QuestionId;
+  OpCode.Question.Flags         = QuestionFlags;
+  OpCode.FormId                 = RefFormId;
+  OpCode.QuestionId             = RefQuestionId;
+  OpCode.DevicePath             = RefDevicePath;
+  if (RefFormSetId != NULL) {
+    CopyMem (&OpCode.FormSetId, RefFormSetId, sizeof (OpCode.FormSetId));
+  }
+
+  //
+  // Cacluate OpCodeSize based on the input Ref value.
+  // Try to use the small OpCode to save size.
+  //
+  OpCodeSize = sizeof (EFI_IFR_REF);
+  if (RefDevicePath != 0) {
+    OpCodeSize = sizeof (EFI_IFR_REF4);
+  } else if (RefFormSetId != NULL) {
+    OpCodeSize = sizeof (EFI_IFR_REF3);
+  } else if (RefQuestionId != 0) {
+    OpCodeSize = sizeof (EFI_IFR_REF2);
+  }
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_REF_OP, OpCodeSize);
+}
+
+/**
+  Create EFI_IFR_CHECKBOX_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in CheckBoxFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  CheckBoxFlags         Flags for checkbox opcode
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateCheckBoxOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            CheckBoxFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_CHECKBOX  OpCode;
+  UINTN             Position;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.Flags                           = CheckBoxFlags;
+
+  if (DefaultsOpCodeHandle == NULL) {
+    return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_CHECKBOX_OP, sizeof (OpCode));
+  }
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_CHECKBOX_OP, sizeof (OpCode), 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_NUMERIC_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in NumericFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  NumericFlags          Flags for numeric opcode
+  @param[in]  Minimum               Numeric minimum value
+  @param[in]  Maximum               Numeric maximum value
+  @param[in]  Step                  Numeric step for edit
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateNumericOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            NumericFlags,
+  IN UINT64           Minimum,
+  IN UINT64           Maximum,
+  IN UINT64           Step,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_NUMERIC  OpCode;
+  UINTN            Position;
+  UINTN            Length;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  Length  = 0;
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.Flags                           = NumericFlags;
+
+  switch (NumericFlags & EFI_IFR_NUMERIC_SIZE) {
+  case EFI_IFR_NUMERIC_SIZE_1:
+    OpCode.data.u8.MinValue = (UINT8)Minimum;
+    OpCode.data.u8.MaxValue = (UINT8)Maximum;
+    OpCode.data.u8.Step     = (UINT8)Step;
+    Length                  = 3;
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_2:
+    OpCode.data.u16.MinValue = (UINT16)Minimum;
+    OpCode.data.u16.MaxValue = (UINT16)Maximum;
+    OpCode.data.u16.Step     = (UINT16)Step;
+    Length                   = 6;
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_4:
+    OpCode.data.u32.MinValue = (UINT32)Minimum;
+    OpCode.data.u32.MaxValue = (UINT32)Maximum;
+    OpCode.data.u32.Step     = (UINT32)Step;
+    Length                   = 12;
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_8:
+    OpCode.data.u64.MinValue = Minimum;
+    OpCode.data.u64.MaxValue = Maximum;
+    OpCode.data.u64.Step     = Step;
+    Length                   = 24;
+    break;
+  }
+
+  Length += OFFSET_OF (EFI_IFR_NUMERIC, data);
+
+  if (DefaultsOpCodeHandle == NULL) {
+    return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_NUMERIC_OP, Length);
+  }
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_NUMERIC_OP, Length, 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_STRING_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in StringFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  StringFlags           Flags for string opcode
+  @param[in]  MinSize               String minimum length
+  @param[in]  MaxSize               String maximum length
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateStringOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            StringFlags,
+  IN UINT8            MinSize,
+  IN UINT8            MaxSize,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_STRING  OpCode;
+  UINTN           Position;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.MinSize                         = MinSize;
+  OpCode.MaxSize                         = MaxSize;
+  OpCode.Flags                           = (UINT8) (StringFlags & EFI_IFR_STRING_MULTI_LINE);
+
+  if (DefaultsOpCodeHandle == NULL) {
+    return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_STRING_OP, sizeof (OpCode));
+  }
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_STRING_OP, sizeof (OpCode), 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_ONE_OF_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in OneOfFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  OneOfFlags            Flags for oneof opcode
+  @param[in]  OptionsOpCodeHandle   Handle for a buffer of ONE_OF_OPTION opcodes.
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOneOfOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            OneOfFlags,
+  IN VOID             *OptionsOpCodeHandle,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_ONE_OF  OpCode;
+  UINTN           Position;
+  UINTN           Length;
+
+  ASSERT (OptionsOpCodeHandle != NULL);
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED | EFI_IFR_FLAG_OPTIONS_ONLY))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.Flags                           = OneOfFlags;
+
+  Length  = OFFSET_OF (EFI_IFR_ONE_OF, data);
+  Length += (1 << (OneOfFlags & EFI_IFR_NUMERIC_SIZE)) * 3;
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_ONE_OF_OP, Length, 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, OptionsOpCodeHandle);
+  if (DefaultsOpCodeHandle != NULL) {
+    InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  }
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_ORDERED_LIST_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in OrderedListFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  OrderedListFlags      Flags for ordered list opcode
+  @param[in]  DataType              Type for option value
+  @param[in]  MaxContainers         Maximum count for options in this ordered list
+  @param[in]  OptionsOpCodeHandle   Handle for a buffer of ONE_OF_OPTION opcodes.
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateOrderedListOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,
+  IN UINT16           VarOffset,
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            OrderedListFlags,
+  IN UINT8            DataType,
+  IN UINT8            MaxContainers,
+  IN VOID             *OptionsOpCodeHandle,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_ORDERED_LIST  OpCode;
+  UINTN                 Position;
+
+  ASSERT (OptionsOpCodeHandle != NULL);
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED | EFI_IFR_FLAG_OPTIONS_ONLY))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.MaxContainers                   = MaxContainers;
+  OpCode.Flags                           = OrderedListFlags;
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_ORDERED_LIST_OP, sizeof (OpCode), 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, OptionsOpCodeHandle);
+  if (DefaultsOpCodeHandle != NULL) {
+    InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  }
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_TEXT_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  OpCodeHandle  Handle to the buffer of opcodes.
+  @param[in]  Prompt        String ID for Prompt.
+  @param[in]  Help          String ID for Help.
+  @param[in]  TextTwo       String ID for TextTwo.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateTextOpCode (
+  IN VOID           *OpCodeHandle,
+  IN EFI_STRING_ID  Prompt,
+  IN EFI_STRING_ID  Help,
+  IN EFI_STRING_ID  TextTwo
+  )
+{
+  EFI_IFR_TEXT  OpCode;
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Statement.Prompt = Prompt;
+  OpCode.Statement.Help   = Help;
+  OpCode.TextTwo          = TextTwo;
+
+  return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_TEXT_OP, sizeof (OpCode));
+}
+
+/**
+  Create EFI_IFR_DATE_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in DateFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID, optional. If DateFlags is not
+                                    QF_DATE_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair, optional. If DateFlags is not
+                                    QF_DATE_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  DateFlags             Flags for date opcode
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateDateOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,   OPTIONAL
+  IN UINT16           VarOffset,    OPTIONAL
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            DateFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_DATE    OpCode;
+  UINTN           Position;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+  ASSERT ((DateFlags & (~(EFI_QF_DATE_YEAR_SUPPRESS | EFI_QF_DATE_MONTH_SUPPRESS | EFI_QF_DATE_DAY_SUPPRESS | EFI_QF_DATE_STORAGE))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.Flags                           = DateFlags;
+
+  if (DefaultsOpCodeHandle == NULL) {
+    return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_DATE_OP, sizeof (OpCode));
+  }
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_DATE_OP, sizeof (OpCode), 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  Create EFI_IFR_TIME_OP opcode.
+
+  If OpCodeHandle is NULL, then ASSERT().
+  If any reserved bits are set in QuestionFlags, then ASSERT().
+  If any reserved bits are set in TimeFlags, then ASSERT().
+
+  @param[in]  OpCodeHandle          Handle to the buffer of opcodes.
+  @param[in]  QuestionId            Question ID
+  @param[in]  VarStoreId            Storage ID, optional. If TimeFlags is not
+                                    QF_TIME_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  VarOffset             Offset in Storage or String ID of the name (VarName)
+                                    for this name/value pair, optional. If TimeFlags is not
+                                    QF_TIME_STORAGE_NORMAL, this parameter is ignored.
+  @param[in]  Prompt                String ID for Prompt
+  @param[in]  Help                  String ID for Help
+  @param[in]  QuestionFlags         Flags in Question Header
+  @param[in]  TimeFlags             Flags for time opcode
+  @param[in]  DefaultsOpCodeHandle  Handle for a buffer of DEFAULT opcodes.  This
+                                    is an optional parameter that may be NULL.
+
+  @retval NULL   There is not enough space left in Buffer to add the opcode.
+  @retval Other  A pointer to the created opcode.
+
+**/
+UINT8 *
+EFIAPI
+HiiCreateTimeOpCode (
+  IN VOID             *OpCodeHandle,
+  IN EFI_QUESTION_ID  QuestionId,
+  IN EFI_VARSTORE_ID  VarStoreId,   OPTIONAL
+  IN UINT16           VarOffset,    OPTIONAL
+  IN EFI_STRING_ID    Prompt,
+  IN EFI_STRING_ID    Help,
+  IN UINT8            QuestionFlags,
+  IN UINT8            TimeFlags,
+  IN VOID             *DefaultsOpCodeHandle  OPTIONAL
+  )
+{
+  EFI_IFR_TIME    OpCode;
+  UINTN           Position;
+
+  ASSERT ((QuestionFlags & (~(EFI_IFR_FLAG_READ_ONLY | EFI_IFR_FLAG_CALLBACK | EFI_IFR_FLAG_RESET_REQUIRED))) == 0);
+  ASSERT ((TimeFlags & (~(QF_TIME_HOUR_SUPPRESS | QF_TIME_MINUTE_SUPPRESS | QF_TIME_SECOND_SUPPRESS | QF_TIME_STORAGE))) == 0);
+
+  ZeroMem (&OpCode, sizeof (OpCode));
+  OpCode.Question.Header.Prompt          = Prompt;
+  OpCode.Question.Header.Help            = Help;
+  OpCode.Question.QuestionId             = QuestionId;
+  OpCode.Question.VarStoreId             = VarStoreId;
+  OpCode.Question.VarStoreInfo.VarOffset = VarOffset;
+  OpCode.Question.Flags                  = QuestionFlags;
+  OpCode.Flags                           = TimeFlags;
+
+  if (DefaultsOpCodeHandle == NULL) {
+    return InternalHiiCreateOpCode (OpCodeHandle, &OpCode, EFI_IFR_TIME_OP, sizeof (OpCode));
+  }
+
+  Position = InternalHiiOpCodeHandlePosition (OpCodeHandle);
+  InternalHiiCreateOpCodeExtended (OpCodeHandle, &OpCode, EFI_IFR_TIME_OP, sizeof (OpCode), 0, 1);
+  InternalHiiAppendOpCodes (OpCodeHandle, DefaultsOpCodeHandle);
+  HiiCreateEndOpCode (OpCodeHandle);
+  return InternalHiiOpCodeHandleBuffer (OpCodeHandle) + Position;
+}
+
+/**
+  This is the internal worker function to update the data in
+  a form specified by FormSetGuid, FormId and Label.
+
+  @param[in] FormSetGuid       The optional Formset GUID.
+  @param[in] FormId            The Form ID.
+  @param[in] Package           The package header.
+  @param[in] OpCodeBufferStart An OpCode buffer that contains the set of IFR 
+                               opcodes to be inserted or replaced in the form.
+  @param[in] OpCodeBufferEnd   An OpCcode buffer that contains the IFR opcode
+                               that marks the end of a replace operation in the form.
+  @param[out] TempPackage      The resultant package.
+
+  @retval EFI_SUCCESS    The function completes successfully.
+  @retval EFI_NOT_FOUND  The updated opcode or endopcode is not found.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalHiiUpdateFormPackageData (
+  IN  EFI_GUID               *FormSetGuid, OPTIONAL
+  IN  EFI_FORM_ID            FormId,
+  IN  EFI_HII_PACKAGE_HEADER *Package,
+  IN  HII_LIB_OPCODE_BUFFER  *OpCodeBufferStart,
+  IN  HII_LIB_OPCODE_BUFFER  *OpCodeBufferEnd,    OPTIONAL
+  OUT EFI_HII_PACKAGE_HEADER *TempPackage
+  )
+{
+  UINTN                     AddSize;
+  UINT8                     *BufferPos;
+  EFI_HII_PACKAGE_HEADER    PackageHeader;
+  UINTN                     Offset;
+  EFI_IFR_OP_HEADER         *IfrOpHdr;
+  EFI_IFR_OP_HEADER         *UpdateIfrOpHdr;
+  BOOLEAN                   GetFormSet;
+  BOOLEAN                   GetForm;
+  BOOLEAN                   Updated;
+  UINTN                     UpdatePackageLength;
+
+  CopyMem (TempPackage, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+  UpdatePackageLength = sizeof (EFI_HII_PACKAGE_HEADER);
+  BufferPos           = (UINT8 *) (TempPackage + 1);
+
+  CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+  IfrOpHdr   = (EFI_IFR_OP_HEADER *)((UINT8 *) Package + sizeof (EFI_HII_PACKAGE_HEADER));
+  Offset     = sizeof (EFI_HII_PACKAGE_HEADER);
+  GetFormSet = (BOOLEAN) ((FormSetGuid == NULL) ? TRUE : FALSE);
+  GetForm    = FALSE;
+  Updated    = FALSE;
+
+  while (Offset < PackageHeader.Length) {
+    CopyMem (BufferPos, IfrOpHdr, IfrOpHdr->Length);
+    BufferPos           += IfrOpHdr->Length;
+    UpdatePackageLength += IfrOpHdr->Length;
+    
+    //
+    // Find the matched FormSet and Form
+    //
+    if ((IfrOpHdr->OpCode == EFI_IFR_FORM_SET_OP) && (FormSetGuid != NULL)) {
+      if (CompareGuid((GUID *)(VOID *)&((EFI_IFR_FORM_SET *) IfrOpHdr)->Guid, FormSetGuid)) {
+        GetFormSet = TRUE;
+      } else {
+        GetFormSet = FALSE;
+      }
+    } else if (IfrOpHdr->OpCode == EFI_IFR_FORM_OP || IfrOpHdr->OpCode == EFI_IFR_FORM_MAP_OP) {
+      if (CompareMem (&((EFI_IFR_FORM *) IfrOpHdr)->FormId, &FormId, sizeof (EFI_FORM_ID)) == 0) {
+        GetForm = TRUE;
+      } else {
+        GetForm = FALSE;
+      }
+    }
+    
+    //
+    // The matched Form is found, and Update data in this form
+    //
+    if (GetFormSet && GetForm) {
+      UpdateIfrOpHdr = (EFI_IFR_OP_HEADER *) OpCodeBufferStart->Buffer;
+      if ((UpdateIfrOpHdr->Length == IfrOpHdr->Length) && \
+          (CompareMem (IfrOpHdr, UpdateIfrOpHdr, UpdateIfrOpHdr->Length) == 0)) {
+        //
+        // Remove the original data when End OpCode buffer exist.
+        //
+        if (OpCodeBufferEnd != NULL) {
+          Offset        += IfrOpHdr->Length;
+          IfrOpHdr       = (EFI_IFR_OP_HEADER *) ((UINT8 *) (IfrOpHdr) + IfrOpHdr->Length);
+          UpdateIfrOpHdr = (EFI_IFR_OP_HEADER *) OpCodeBufferEnd->Buffer;
+          while (Offset < PackageHeader.Length) {
+            //
+            // Search the matched end opcode
+            //
+            if ((UpdateIfrOpHdr->Length == IfrOpHdr->Length) && \
+                (CompareMem (IfrOpHdr, UpdateIfrOpHdr, UpdateIfrOpHdr->Length) == 0)) {
+              break;
+            }
+            //
+            // Go to the next Op-Code
+            //
+            Offset        += IfrOpHdr->Length;
+            IfrOpHdr       = (EFI_IFR_OP_HEADER *) ((UINT8 *) (IfrOpHdr) + IfrOpHdr->Length);
+          }
+          
+          if (Offset >= PackageHeader.Length) {
+            //
+            // The end opcode is not found.
+            //
+            return EFI_NOT_FOUND;
+          }
+        }
+
+        //
+        // Insert the updated data
+        //
+        AddSize = ((EFI_IFR_OP_HEADER *) OpCodeBufferStart->Buffer)->Length;
+        CopyMem (BufferPos, OpCodeBufferStart->Buffer + AddSize, OpCodeBufferStart->Position - AddSize);
+        BufferPos           += OpCodeBufferStart->Position - AddSize;
+        UpdatePackageLength += OpCodeBufferStart->Position - AddSize;
+
+        if (OpCodeBufferEnd != NULL) {
+          //
+          // Add the end opcode
+          //
+          CopyMem (BufferPos, IfrOpHdr, IfrOpHdr->Length);
+          BufferPos           += IfrOpHdr->Length;
+          UpdatePackageLength += IfrOpHdr->Length;
+        }
+
+        //
+        // Copy the left package data.
+        //
+        Offset += IfrOpHdr->Length;
+        CopyMem (BufferPos, (UINT8 *) Package + Offset, PackageHeader.Length - Offset);
+        UpdatePackageLength += PackageHeader.Length - Offset;
+
+        //
+        // Set update flag
+        //
+        Updated = TRUE;
+        break;
+      }
+    }
+
+    //
+    // Go to the next Op-Code
+    //
+    Offset   += IfrOpHdr->Length;
+    IfrOpHdr = (EFI_IFR_OP_HEADER *) ((CHAR8 *) (IfrOpHdr) + IfrOpHdr->Length);
+  }
+  
+  if (!Updated) {
+    //
+    // The updated opcode buffer is not found.
+    //
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Update the package length.
+  //
+  PackageHeader.Length = (UINT32) UpdatePackageLength;
+  CopyMem (TempPackage, &PackageHeader, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function updates a form that has previously been registered with the HII 
+  Database.  This function will perform at most one update operation.
+    
+  The form to update is specified by Handle, FormSetGuid, and FormId.  Binary 
+  comparisons of IFR opcodes are performed from the beginning of the form being 
+  updated until an IFR opcode is found that exactly matches the first IFR opcode 
+  specified by StartOpCodeHandle.  The following rules are used to determine if
+  an insert, replace, or delete operation is performed.
+  
+  1) If no matches are found, then NULL is returned.  
+  2) If a match is found, and EndOpCodeHandle is NULL, then all of the IFR opcodes
+     from StartOpCodeHandle except the first opcode are inserted immediately after 
+     the matching IFR opcode in the form to be updated.
+  3) If a match is found, and EndOpCodeHandle is not NULL, then a search is made 
+     from the matching IFR opcode until an IFR opcode exactly matches the first 
+     IFR opcode specified by EndOpCodeHandle.  If no match is found for the first
+     IFR opcode specified by EndOpCodeHandle, then NULL is returned.  If a match
+     is found, then all of the IFR opcodes between the start match and the end 
+     match are deleted from the form being updated and all of the IFR opcodes
+     from StartOpCodeHandle except the first opcode are inserted immediately after 
+     the matching start IFR opcode.  If StartOpCcodeHandle only contains one
+     IFR instruction, then the result of this operation will delete all of the IFR
+     opcodes between the start end matches.
+
+  If HiiHandle is NULL, then ASSERT().
+  If StartOpCodeHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle          The HII Handle of the form to update.
+  @param[in]  FormSetGuid        The Formset GUID of the form to update.  This
+                                 is an optional parameter that may be NULL.
+                                 If it is NULL, all FormSet will be updated.
+  @param[in]  FormId             The ID of the form to update.
+  @param[in]  StartOpCodeHandle  An OpCode Handle that contains the set of IFR 
+                                 opcodes to be inserted or replaced in the form.
+                                 The first IFR instruction in StartOpCodeHandle 
+                                 is used to find matching IFR opcode in the 
+                                 form. 
+  @param[in]  EndOpCodeHandle    An OpCcode Handle that contains the IFR opcode
+                                 that marks the end of a replace operation in
+                                 the form.  This is an optional parameter that
+                                 may be NULL.  If it is NULL, then an the IFR
+                                 opcodes specified by StartOpCodeHandle are 
+                                 inserted into the form.
+  
+  @retval EFI_OUT_OF_RESOURCES   No enough memory resource is allocated.
+  @retval EFI_NOT_FOUND          The following cases will return EFI_NOT_FOUND.
+                                 1) The form specified by HiiHandle, FormSetGuid, 
+                                 and FormId could not be found in the HII Database.
+                                 2) No IFR opcodes in the target form match the first
+                                 IFR opcode in StartOpCodeHandle.
+                                 3) EndOpCOde is not NULL, and no IFR opcodes in the 
+                                 target form following a matching start opcode match 
+                                 the first IFR opcode in EndOpCodeHandle.
+  @retval EFI_SUCCESS            The matched form is updated by StartOpcode.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUpdateForm (
+  IN EFI_HII_HANDLE  HiiHandle,           
+  IN EFI_GUID        *FormSetGuid,        OPTIONAL
+  IN EFI_FORM_ID     FormId,
+  IN VOID            *StartOpCodeHandle,
+  IN VOID            *EndOpCodeHandle     OPTIONAL
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINT32                       PackageListLength;  
+  UINT32                       Offset;
+  EFI_HII_PACKAGE_LIST_HEADER  *UpdatePackageList;
+  UINTN                        BufferSize;
+  UINT8                        *UpdateBufferPos;
+  EFI_HII_PACKAGE_HEADER       *Package;
+  EFI_HII_PACKAGE_HEADER       *TempPacakge;
+  EFI_HII_PACKAGE_HEADER       PackageHeader;
+  BOOLEAN                      Updated;
+  HII_LIB_OPCODE_BUFFER        *OpCodeBufferStart;
+  HII_LIB_OPCODE_BUFFER        *OpCodeBufferEnd;
+  
+  //
+  // Input update data can't be NULL.
+  //
+  ASSERT (HiiHandle != NULL);
+  ASSERT (StartOpCodeHandle != NULL);
+  UpdatePackageList = NULL;
+  TempPacakge       = NULL;
+  HiiPackageList    = NULL;
+  
+  //
+  // Retrieve buffer data from Opcode Handle
+  //
+  OpCodeBufferStart = (HII_LIB_OPCODE_BUFFER *) StartOpCodeHandle;
+  OpCodeBufferEnd   = (HII_LIB_OPCODE_BUFFER *) EndOpCodeHandle;
+  
+  //
+  // Get the original package list
+  //
+  BufferSize = 0;
+  HiiPackageList   = NULL;
+  Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, HiiHandle, &BufferSize, HiiPackageList);
+  //
+  // The return status should always be EFI_BUFFER_TOO_SMALL as input buffer's size is 0.
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  HiiPackageList = AllocatePool (BufferSize);
+  if (HiiPackageList == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Finish;
+  }
+
+  Status = gHiiDatabase->ExportPackageLists (gHiiDatabase, HiiHandle, &BufferSize, HiiPackageList);
+  if (EFI_ERROR (Status)) {
+    goto Finish;
+  }
+
+  //
+  // Calculate and allocate space for retrieval of IFR data
+  //
+  BufferSize += OpCodeBufferStart->Position;
+  UpdatePackageList = AllocateZeroPool (BufferSize);
+  if (UpdatePackageList == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Finish;
+  }
+  
+  //
+  // Allocate temp buffer to store the temp updated package buffer
+  //
+  TempPacakge = AllocateZeroPool (BufferSize);
+  if (TempPacakge == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Finish;
+  }
+
+  UpdateBufferPos = (UINT8 *) UpdatePackageList;
+
+  //
+  // Copy the package list header
+  //
+  CopyMem (UpdateBufferPos, HiiPackageList, sizeof (EFI_HII_PACKAGE_LIST_HEADER));
+  UpdateBufferPos += sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  
+  //
+  // Go through each package to find the matched package and update one by one
+  //
+  Updated = FALSE;
+  Offset  = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  PackageListLength = ReadUnaligned32 (&HiiPackageList->PackageLength);
+  while (Offset < PackageListLength) {
+    Package = (EFI_HII_PACKAGE_HEADER *) (((UINT8 *) HiiPackageList) + Offset);
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+    Offset += Package->Length;
+
+    if (Package->Type == EFI_HII_PACKAGE_FORMS) {
+      //
+      // Check this package is the matched package.
+      //
+      Status = InternalHiiUpdateFormPackageData (FormSetGuid, FormId, Package, OpCodeBufferStart, OpCodeBufferEnd, TempPacakge);
+      //
+      // The matched package is found. Its package buffer will be updated by the input new data.
+      //
+      if (!EFI_ERROR(Status)) {
+        //
+        // Set Update Flag
+        //        
+        Updated = TRUE;
+        //
+        // Add updated package buffer
+        //
+        Package = TempPacakge;
+      }
+    }
+
+    //
+    // Add pacakge buffer
+    //
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+    CopyMem (UpdateBufferPos, Package, PackageHeader.Length);
+    UpdateBufferPos += PackageHeader.Length;
+  }
+  
+  if (Updated) {
+    //
+    // Update package list length
+    //
+    BufferSize = UpdateBufferPos - (UINT8 *) UpdatePackageList;
+    WriteUnaligned32 (&UpdatePackageList->PackageLength, (UINT32) BufferSize);
+    
+    //
+    // Update Package to show form
+    //
+    Status = gHiiDatabase->UpdatePackageList (gHiiDatabase, HiiHandle, UpdatePackageList);
+  } else {
+    //
+    // Not matched form is found and updated.
+    //
+    Status = EFI_NOT_FOUND;
+  }
+
+Finish:
+  if (HiiPackageList != NULL) {
+    FreePool (HiiPackageList);
+  }
+  
+  if (UpdatePackageList != NULL) {
+    FreePool (UpdatePackageList);
+  }
+  
+  if (TempPacakge != NULL) {
+    FreePool (TempPacakge);
+  }
+
+  return Status; 
+}
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/HiiString.c b/Core/MdeModulePkg/Library/UefiHiiLib/HiiString.c
new file mode 100644
index 0000000000..bfae3f8fc0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/HiiString.c
@@ -0,0 +1,355 @@
+/** @file
+  HII Library implementation that uses DXE protocols and services.
+
+  Copyright (c) 2006 - 2015, 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 "InternalHiiLib.h"
+
+/**
+  This function create a new string in String Package or updates an existing 
+  string in a String Package.  If StringId is 0, then a new string is added to
+  a String Package.  If StringId is not zero, then a string in String Package is
+  updated.  If SupportedLanguages is NULL, then the string is added or updated
+  for all the languages that the String Package supports.  If SupportedLanguages
+  is not NULL, then the string is added or updated for the set of languages 
+  specified by SupportedLanguages.
+    
+  If HiiHandle is NULL, then ASSERT().
+  If String is NULL, then ASSERT().
+
+  @param[in]  HiiHandle           A handle that was previously registered in the 
+                                  HII Database.
+  @param[in]  StringId            If zero, then a new string is created in the 
+                                  String Package associated with HiiHandle.  If 
+                                  non-zero, then the string specified by StringId 
+                                  is updated in the String Package  associated 
+                                  with HiiHandle. 
+  @param[in]  String              A pointer to the Null-terminated Unicode string 
+                                  to add or update in the String Package associated 
+                                  with HiiHandle.
+  @param[in]  SupportedLanguages  A pointer to a Null-terminated ASCII string of 
+                                  language codes.  If this parameter is NULL, then 
+                                  String is added or updated in the String Package 
+                                  associated with HiiHandle for all the languages 
+                                  that the String Package supports.  If this 
+                                  parameter is not NULL, then then String is added 
+                                  or updated in the String Package associated with 
+                                  HiiHandle for the set oflanguages specified by 
+                                  SupportedLanguages.  The format of 
+                                  SupportedLanguages must follow the language 
+                                  format assumed the HII Database.
+
+  @retval 0      The string could not be added or updated in the String Package.
+  @retval Other  The EFI_STRING_ID of the newly added or updated string.
+
+**/
+EFI_STRING_ID
+EFIAPI
+HiiSetString (
+  IN EFI_HII_HANDLE    HiiHandle,
+  IN EFI_STRING_ID     StringId,            OPTIONAL
+  IN CONST EFI_STRING  String,
+  IN CONST CHAR8       *SupportedLanguages  OPTIONAL
+  )
+{
+  EFI_STATUS     Status;
+  CHAR8          *AllocatedLanguages;
+  CHAR8          *Supported;
+  CHAR8          *Language;
+
+  ASSERT (HiiHandle != NULL);
+  ASSERT (String != NULL);
+
+  if (SupportedLanguages == NULL) {
+    //
+    // Retrieve the languages that the package specified by HiiHandle supports
+    //
+    AllocatedLanguages = HiiGetSupportedLanguages (HiiHandle);
+  } else {
+    //
+    // Allocate a copy of the SupportLanguages string that passed in
+    //
+    AllocatedLanguages = AllocateCopyPool (AsciiStrSize (SupportedLanguages), SupportedLanguages);
+  }
+
+  //
+  // If there are not enough resources for the supported languages string, then return a StringId of 0
+  //
+  if (AllocatedLanguages == NULL) {
+    return (EFI_STRING_ID)(0);
+  }
+
+  Status = EFI_INVALID_PARAMETER;
+  //
+  // Loop through each language that the string supports
+  //
+  for (Supported = AllocatedLanguages; *Supported != '\0'; ) {
+    //
+    // Cache a pointer to the beginning of the current language in the list of languages
+    //
+    Language = Supported;
+
+    //
+    // Search for the next language seperator and replace it with a Null-terminator
+    //
+    for (; *Supported != 0 && *Supported != ';'; Supported++);
+    if (*Supported != 0) {
+      *(Supported++) = '\0';
+    }
+    
+    if ((SupportedLanguages == NULL) && AsciiStrnCmp (Language, UEFI_CONFIG_LANG, AsciiStrLen (UEFI_CONFIG_LANG)) == 0) {
+      //
+      // Skip string package used for keyword protocol.
+      //
+      continue;
+    }
+
+    //
+    // If StringId is 0, then call NewString().  Otherwise, call SetString()
+    //
+    if (StringId == (EFI_STRING_ID)(0)) {
+      Status = gHiiString->NewString (gHiiString, HiiHandle, &StringId, Language, NULL, String, NULL);
+    } else {
+      Status = gHiiString->SetString (gHiiString, HiiHandle, StringId, Language, String, NULL);
+    }
+
+    //
+    // If there was an error, then break out of the loop and return a StringId of 0
+    //
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  //
+  // Free the buffer of supported languages
+  //
+  FreePool (AllocatedLanguages);
+
+  if (EFI_ERROR (Status)) {
+    return (EFI_STRING_ID)(0);
+  } else {
+    return StringId;
+  }
+}
+
+
+/**
+  Retrieves a string from a string package names by GUID in a specific language.  
+  If the language is not specified, then a string from a string package in the 
+  current platform  language is retrieved.  If the string can not be retrieved 
+  using the specified language or the current platform language, then the string 
+  is retrieved from the string package in the first language the string package 
+  supports.  The returned string is allocated using AllocatePool().  The caller 
+  is responsible for freeing the allocated buffer using FreePool().
+  
+  If PackageListGuid is NULL, then ASSERT().
+  If StringId is 0, then ASSERT.
+
+  @param[in]  PackageListGuid  The GUID of a package list that was previously 
+                               registered in the HII Database.
+  @param[in]  StringId         The identifier of the string to retrieved from the 
+                               string package associated with PackageListGuid.
+  @param[in]  Language         The language of the string to retrieve.  If this 
+                               parameter is NULL, then the current platform 
+                               language is used.  The format of Language must 
+                               follow the language format assumed the HII Database.
+
+  @retval NULL   The package list specified by PackageListGuid is not present in the
+                 HII Database.
+  @retval NULL   The string specified by StringId is not present in the string package.
+  @retval Other  The string was returned.
+
+**/
+EFI_STRING
+EFIAPI
+HiiGetPackageString (
+  IN CONST EFI_GUID  *PackageListGuid,
+  IN EFI_STRING_ID   StringId,
+  IN CONST CHAR8     *Language  OPTIONAL
+  )
+{
+  EFI_HANDLE  *HiiHandleBuffer;
+  EFI_HANDLE  HiiHandle;
+
+  ASSERT (PackageListGuid != NULL);
+
+  HiiHandleBuffer = HiiGetHiiHandles (PackageListGuid);
+  if (HiiHandleBuffer == NULL) {
+    return NULL;
+  }
+
+  HiiHandle = HiiHandleBuffer[0];
+  FreePool (HiiHandleBuffer);
+
+  return HiiGetString (HiiHandle, StringId, Language);
+}
+
+/**
+  Retrieves a string from a string package in a specific language.  If the language
+  is not specified, then a string from a string package in the current platform 
+  language is retrieved.  If the string can not be retrieved using the specified 
+  language or the current platform language, then the string is retrieved from 
+  the string package in the first language the string package supports.  The 
+  returned string is allocated using AllocatePool().  The caller is responsible 
+  for freeing the allocated buffer using FreePool().
+  
+  If HiiHandle is NULL, then ASSERT().
+  If StringId is 0, then ASSET.
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+  @param[in]  StringId   The identifier of the string to retrieved from the string 
+                         package associated with HiiHandle.
+  @param[in]  Language   The language of the string to retrieve.  If this parameter 
+                         is NULL, then the current platform language is used.  The 
+                         format of Language must follow the language format assumed 
+                         the HII Database.
+
+  @retval NULL   The string specified by StringId is not present in the string package.
+  @retval Other  The string was returned.
+
+**/
+EFI_STRING
+EFIAPI
+HiiGetString (
+  IN EFI_HII_HANDLE  HiiHandle,
+  IN EFI_STRING_ID   StringId,
+  IN CONST CHAR8     *Language  OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       StringSize;
+  CHAR16      TempString;
+  EFI_STRING  String;
+  CHAR8       *SupportedLanguages;
+  CHAR8       *PlatformLanguage;
+  CHAR8       *BestLanguage;
+
+  ASSERT (HiiHandle != NULL);
+  ASSERT (StringId != 0);
+
+  //
+  // Initialize all allocated buffers to NULL
+  // 
+  SupportedLanguages = NULL;
+  PlatformLanguage   = NULL;
+  BestLanguage       = NULL;
+  String             = NULL;
+
+  //
+  // Get the languages that the package specified by HiiHandle supports
+  //
+  SupportedLanguages = HiiGetSupportedLanguages (HiiHandle);
+  if (SupportedLanguages == NULL) {
+    goto Error;
+  }
+
+  //
+  // Get the current platform language setting
+  //
+  GetEfiGlobalVariable2 (L"PlatformLang", (VOID**)&PlatformLanguage, NULL);
+
+  //
+  // If Languag is NULL, then set it to an empty string, so it will be 
+  // skipped by GetBestLanguage()
+  //
+  if (Language == NULL) {
+    Language = "";
+  }
+
+  //
+  // Get the best matching language from SupportedLanguages
+  //
+  BestLanguage = GetBestLanguage (
+                   SupportedLanguages, 
+                   FALSE,                                             // RFC 4646 mode
+                   Language,                                          // Highest priority 
+                   PlatformLanguage != NULL ? PlatformLanguage : "",  // Next highest priority
+                   SupportedLanguages,                                // Lowest priority 
+                   NULL
+                   );
+  if (BestLanguage == NULL) {
+    goto Error;
+  }
+
+  //
+  // Retrieve the size of the string in the string package for the BestLanguage
+  //
+  StringSize = 0;
+  Status = gHiiString->GetString (
+                         gHiiString,
+                         BestLanguage,
+                         HiiHandle,
+                         StringId,
+                         &TempString,
+                         &StringSize,
+                         NULL
+                         );
+  //
+  // If GetString() returns EFI_SUCCESS for a zero size, 
+  // then there are no supported languages registered for HiiHandle.  If GetString() 
+  // returns an error other than EFI_BUFFER_TOO_SMALL, then HiiHandle is not present
+  // in the HII Database
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    goto Error;
+  }
+
+  //
+  // Allocate a buffer for the return string
+  //
+  String = AllocateZeroPool (StringSize);
+  if (String == NULL) {
+    goto Error;
+  }
+
+  //
+  // Retrieve the string from the string package
+  //
+  Status = gHiiString->GetString (
+                         gHiiString,
+                         BestLanguage,
+                         HiiHandle,
+                         StringId,
+                         String,
+                         &StringSize,
+                         NULL
+                         );
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the buffer and return NULL if the supported languages can not be retrieved.
+    //
+    FreePool (String);
+    String = NULL;
+  }
+
+Error:
+  //
+  // Free allocated buffers
+  //
+  if (SupportedLanguages != NULL) {
+    FreePool (SupportedLanguages);
+  }
+  if (PlatformLanguage != NULL) {
+    FreePool (PlatformLanguage);
+  }
+  if (BestLanguage != NULL) {
+    FreePool (BestLanguage);
+  }
+
+  //
+  // Return the Null-terminated Unicode string
+  //
+  return String;
+}
+
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/InternalHiiLib.h b/Core/MdeModulePkg/Library/UefiHiiLib/InternalHiiLib.h
new file mode 100644
index 0000000000..9bf7696ea3
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/InternalHiiLib.h
@@ -0,0 +1,34 @@
+/** @file
+  Internal include file for the HII Library instance.
+
+  Copyright (c) 2007, 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.             
+
+**/
+
+#ifndef __INTERNAL_HII_LIB_H__
+#define __INTERNAL_HII_LIB_H__
+
+#include <Uefi.h>
+
+#include <Protocol/DevicePath.h>
+#include <Protocol/FormBrowser2.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/HiiLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiHiiServicesLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiLib.h>
+
+#endif
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf b/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf
new file mode 100644
index 0000000000..62f435a089
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf
@@ -0,0 +1,53 @@
+## @file
+#  HII Library implementation using UEFI HII protocols and services.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UefiHiiLib
+  MODULE_UNI_FILE                = UefiHiiLib.uni
+  FILE_GUID                      = 3143687A-7C80-404e-B5FE-2D88980E1B1C
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = HiiLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  HiiLib.c
+  HiiString.c
+  HiiLanguage.c
+  InternalHiiLib.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  DevicePathLib
+  UefiLib
+  UefiHiiServicesLib
+  PrintLib
+
+[Protocols]
+  gEfiFormBrowser2ProtocolGuid ## SOMETIMES_CONSUMES
+  gEfiDevicePathProtocolGuid   ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.uni b/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.uni
new file mode 100644
index 0000000000..7385c4db9d
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.uni
diff --git a/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.c b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.c
new file mode 100644
index 0000000000..bd3a125603
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.c
@@ -0,0 +1,113 @@
+/** @file
+  This library retrieves pointers to the UEFI HII Protocol instances in the 
+  library's constructor.  All of the UEFI HII related protocols are optional, 
+  so the consumers of this library class must verify that the global variable 
+  pointers are not NULL before use.   
+
+  Copyright (c) 2006 - 2009, 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 <Uefi.h>
+
+#include <Library/UefiHiiServicesLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+
+#include <Protocol/HiiFont.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/HiiImage.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiConfigRouting.h>
+
+///
+/// Pointer to the UEFI HII Font Protocol
+///
+EFI_HII_FONT_PROTOCOL  *gHiiFont = NULL;
+
+///
+/// Pointer to the UEFI HII String Protocol
+///
+EFI_HII_STRING_PROTOCOL  *gHiiString = NULL;
+
+///
+/// Pointer to the UEFI HII Image Protocol
+///
+EFI_HII_IMAGE_PROTOCOL  *gHiiImage = NULL;
+
+///
+/// Pointer to the UEFI HII Database Protocol
+///
+EFI_HII_DATABASE_PROTOCOL  *gHiiDatabase = NULL;
+
+///
+/// Pointer to the UEFI HII Config Rounting Protocol
+///
+EFI_HII_CONFIG_ROUTING_PROTOCOL  *gHiiConfigRouting = NULL;
+
+/**
+  The constructor function retrieves pointers to the UEFI HII protocol instances
+    
+  The constructor function retrieves pointers to the four UEFI HII protocols from the 
+  handle database.  These include the UEFI HII Font Protocol, the UEFI HII String 
+  Protocol, the UEFI HII Image Protocol, the UEFI HII Database Protocol, and the 
+  UEFI HII Config Routing Protocol.  This function always return EFI_SUCCESS.
+  All of these protocols are optional if the platform does not support configuration
+  and the UEFI HII Image Protocol and the UEFI HII Font Protocol are optional if 
+  the platform does not support a graphical console.  As a result, the consumers
+  of this library much check the protocol pointers againt NULL before using them,
+  or use dependency expressions to guarantee that some of them are present before
+  assuming they are not NULL.
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+UefiHiiServicesLibConstructor (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS Status;
+
+  //
+  // Retrieve the pointer to the UEFI HII String Protocol 
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiStringProtocolGuid, NULL, (VOID **) &gHiiString);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Retrieve the pointer to the UEFI HII Database Protocol 
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiDatabaseProtocolGuid, NULL, (VOID **) &gHiiDatabase);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Retrieve the pointer to the UEFI HII Config Routing Protocol 
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiConfigRoutingProtocolGuid, NULL, (VOID **) &gHiiConfigRouting);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Retrieve the pointer to the optional UEFI HII Font Protocol 
+  //
+  gBS->LocateProtocol (&gEfiHiiFontProtocolGuid, NULL, (VOID **) &gHiiFont);
+
+  //
+  // Retrieve the pointer to the optional UEFI HII Image Protocol 
+  //
+  gBS->LocateProtocol (&gEfiHiiImageProtocolGuid, NULL, (VOID **) &gHiiImage);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.inf b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.inf
new file mode 100644
index 0000000000..a009e9a715
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.inf
@@ -0,0 +1,67 @@
+## @file
+# UEFI HII Services Library implementation.
+# 
+# Copyright (c) 2007 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UefiHiiServicesLib
+  MODULE_UNI_FILE                = UefiHiiServicesLib.uni
+  FILE_GUID                      = 894DC1B6-07A3-4a9d-8CDD-333580B3D4B1
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = UefiHiiServicesLib|DXE_DRIVER DXE_RUNTIME_DRIVER DXE_SAL_DRIVER DXE_SMM_DRIVER UEFI_APPLICATION UEFI_DRIVER 
+
+  CONSTRUCTOR                    = UefiHiiServicesLibConstructor
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UefiHiiServicesLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  DebugLib
+  
+[Protocols]
+  gEfiHiiFontProtocolGuid             ## SOMETIMES_CONSUMES 
+  gEfiHiiStringProtocolGuid           ## CONSUMES
+  gEfiHiiImageProtocolGuid            ## SOMETIMES_CONSUMES
+  gEfiHiiDatabaseProtocolGuid         ## CONSUMES
+  gEfiHiiConfigRoutingProtocolGuid    ## CONSUMES
+
+[Depex.common.DXE_DRIVER]
+  gEfiHiiStringProtocolGuid        AND
+  gEfiHiiDatabaseProtocolGuid      AND
+  gEfiHiiConfigRoutingProtocolGuid
+
+[Depex.common.DXE_RUNTIME_DRIVER]
+  gEfiHiiStringProtocolGuid        AND
+  gEfiHiiDatabaseProtocolGuid      AND
+  gEfiHiiConfigRoutingProtocolGuid
+
+[Depex.common.DXE_SAL_DRIVER]
+  gEfiHiiStringProtocolGuid        AND
+  gEfiHiiDatabaseProtocolGuid      AND
+  gEfiHiiConfigRoutingProtocolGuid
+
+[Depex.common.DXE_SMM_DRIVER]
+  gEfiHiiStringProtocolGuid        AND
+  gEfiHiiDatabaseProtocolGuid      AND
+  gEfiHiiConfigRoutingProtocolGuid
diff --git a/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.uni b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.uni
new file mode 100644
index 0000000000..f7ad669b8e
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.uni
diff --git a/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.c b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.c
new file mode 100644
index 0000000000..8a45cd0d9f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.c
@@ -0,0 +1,322 @@
+/** @file
+  Library used for sorting routines.
+
+  Copyright (c) 2009 - 2014, 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 <Uefi.h>
+
+#include <Protocol/UnicodeCollation.h>
+#include <Protocol/DevicePath.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/SortLib.h>
+#include <Library/DevicePathLib.h>
+
+STATIC EFI_UNICODE_COLLATION_PROTOCOL   *mUnicodeCollation = NULL;
+
+#define USL_FREE_NON_NULL(Pointer)  \
+{                                     \
+  if ((Pointer) != NULL) {            \
+  FreePool((Pointer));                \
+  (Pointer) = NULL;                   \
+  }                                   \
+}
+
+/**
+  Worker function for QuickSorting.  This function is identical to PerformQuickSort,
+  except that is uses the pre-allocated buffer so the in place sorting does not need to
+  allocate and free buffers constantly.
+
+  Each element must be equal sized.
+
+  if BufferToSort is NULL, then ASSERT.
+  if CompareFunction is NULL, then ASSERT.
+  if Buffer is NULL, then ASSERT.
+
+  if Count is < 2 then perform no action.
+  if Size is < 1 then perform no action.
+
+  @param[in, out] BufferToSort   on call a Buffer of (possibly sorted) elements
+                                 on return a buffer of sorted elements
+  @param[in] Count               the number of elements in the buffer to sort
+  @param[in] ElementSize         Size of an element in bytes
+  @param[in] CompareFunction     The function to call to perform the comparison
+                                 of any 2 elements
+  @param[in] Buffer              Buffer of size ElementSize for use in swapping
+**/
+VOID
+EFIAPI
+QuickSortWorker (
+  IN OUT VOID                           *BufferToSort,
+  IN CONST UINTN                        Count,
+  IN CONST UINTN                        ElementSize,
+  IN       SORT_COMPARE                 CompareFunction,
+  IN VOID                               *Buffer
+  )
+{
+  VOID        *Pivot;
+  UINTN       LoopCount;
+  UINTN       NextSwapLocation;
+
+  ASSERT(BufferToSort     != NULL);
+  ASSERT(CompareFunction  != NULL);
+  ASSERT(Buffer  != NULL);
+
+  if ( Count < 2
+    || ElementSize  < 1
+   ){
+    return;
+  }
+
+  NextSwapLocation = 0;
+
+  //
+  // pick a pivot (we choose last element)
+  //
+  Pivot = ((UINT8*)BufferToSort+((Count-1)*ElementSize));
+
+  //
+  // Now get the pivot such that all on "left" are below it
+  // and everything "right" are above it
+  //
+  for ( LoopCount = 0
+      ; LoopCount < Count -1
+      ; LoopCount++
+     ){
+    //
+    // if the element is less than the pivot
+    //
+    if (CompareFunction((VOID*)((UINT8*)BufferToSort+((LoopCount)*ElementSize)),Pivot) <= 0){
+      //
+      // swap
+      //
+      CopyMem (Buffer, (UINT8*)BufferToSort+(NextSwapLocation*ElementSize), ElementSize);
+      CopyMem ((UINT8*)BufferToSort+(NextSwapLocation*ElementSize), (UINT8*)BufferToSort+((LoopCount)*ElementSize), ElementSize);
+      CopyMem ((UINT8*)BufferToSort+((LoopCount)*ElementSize), Buffer, ElementSize);
+
+      //
+      // increment NextSwapLocation
+      //
+      NextSwapLocation++;
+    }
+  }
+  //
+  // swap pivot to it's final position (NextSwapLocaiton)
+  //
+  CopyMem (Buffer, Pivot, ElementSize);
+  CopyMem (Pivot, (UINT8*)BufferToSort+(NextSwapLocation*ElementSize), ElementSize);
+  CopyMem ((UINT8*)BufferToSort+(NextSwapLocation*ElementSize), Buffer, ElementSize);
+
+  //
+  // Now recurse on 2 paritial lists.  neither of these will have the 'pivot' element
+  // IE list is sorted left half, pivot element, sorted right half...
+  //
+  if (NextSwapLocation >= 2) {
+    QuickSortWorker(
+      BufferToSort,
+      NextSwapLocation,
+      ElementSize,
+      CompareFunction,
+      Buffer);
+  }
+
+  if ((Count - NextSwapLocation - 1) >= 2) {
+    QuickSortWorker(
+      (UINT8 *)BufferToSort + (NextSwapLocation+1) * ElementSize,
+      Count - NextSwapLocation - 1,
+      ElementSize,
+      CompareFunction,
+      Buffer);
+  }
+
+  return;
+}
+/**
+  Function to perform a Quick Sort alogrithm on a buffer of comparable elements.
+
+  Each element must be equal sized.
+
+  if BufferToSort is NULL, then ASSERT.
+  if CompareFunction is NULL, then ASSERT.
+
+  if Count is < 2 then perform no action.
+  if Size is < 1 then perform no action.
+
+  @param[in, out] BufferToSort   on call a Buffer of (possibly sorted) elements
+                                 on return a buffer of sorted elements
+  @param[in] Count               the number of elements in the buffer to sort
+  @param[in] ElementSize         Size of an element in bytes
+  @param[in] CompareFunction     The function to call to perform the comparison
+                                 of any 2 elements
+**/
+VOID
+EFIAPI
+PerformQuickSort (
+  IN OUT VOID                           *BufferToSort,
+  IN CONST UINTN                        Count,
+  IN CONST UINTN                        ElementSize,
+  IN       SORT_COMPARE                 CompareFunction
+  )
+{
+  VOID  *Buffer;
+
+  ASSERT(BufferToSort     != NULL);
+  ASSERT(CompareFunction  != NULL);
+
+  Buffer = AllocateZeroPool(ElementSize);
+  ASSERT(Buffer != NULL);
+
+  QuickSortWorker(
+    BufferToSort,
+    Count,
+    ElementSize,
+    CompareFunction,
+    Buffer);
+
+  FreePool(Buffer);
+  return;
+}
+
+/**
+  Function to compare 2 device paths for use in QuickSort.
+
+  @param[in] Buffer1            pointer to Device Path poiner to compare
+  @param[in] Buffer2            pointer to second DevicePath pointer to compare
+
+  @retval 0                     Buffer1 equal to Buffer2
+  @retval <0                    Buffer1 is less than Buffer2
+  @retval >0                    Buffer1 is greater than Buffer2
+**/
+INTN
+EFIAPI
+DevicePathCompare (
+  IN  CONST VOID             *Buffer1,
+  IN  CONST VOID             *Buffer2
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath1;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath2;
+  CHAR16                    *TextPath1;
+  CHAR16                    *TextPath2;
+  EFI_STATUS                Status;
+  INTN                      RetVal;
+
+  DevicePath1 = *(EFI_DEVICE_PATH_PROTOCOL**)Buffer1;
+  DevicePath2 = *(EFI_DEVICE_PATH_PROTOCOL**)Buffer2;
+
+  if (DevicePath1 == NULL) {
+    if (DevicePath2 == NULL) {
+      return 0;
+    }
+
+    return -1;
+  }
+
+  if (DevicePath2 == NULL) {
+    return 1;
+  }
+
+  if (mUnicodeCollation == NULL) {
+    Status = gBS->LocateProtocol(
+      &gEfiUnicodeCollation2ProtocolGuid,
+      NULL,
+      (VOID**)&mUnicodeCollation);
+
+    ASSERT_EFI_ERROR(Status);
+  }
+
+  TextPath1 = ConvertDevicePathToText(
+    DevicePath1,
+    FALSE,
+    FALSE);
+
+  TextPath2 = ConvertDevicePathToText(
+    DevicePath2,
+    FALSE,
+    FALSE);
+
+  if (TextPath1 == NULL) {
+    RetVal = -1;
+  } else if (TextPath2 == NULL) {
+    RetVal = 1;
+  } else {
+    RetVal = mUnicodeCollation->StriColl(
+      mUnicodeCollation,
+      TextPath1,
+      TextPath2);
+  }
+
+  USL_FREE_NON_NULL(TextPath1);
+  USL_FREE_NON_NULL(TextPath2);
+
+  return (RetVal);
+}
+
+/**
+  Function to compare 2 strings without regard to case of the characters.
+
+  @param[in] Buffer1            Pointer to String to compare.
+  @param[in] Buffer2            Pointer to second String to compare.
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @retval <0                    Buffer1 is less than Buffer2.
+  @retval >0                    Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringNoCaseCompare (
+  IN  CONST VOID             *Buffer1,
+  IN  CONST VOID             *Buffer2
+  )
+{
+  EFI_STATUS                Status;
+  if (mUnicodeCollation == NULL) {
+    Status = gBS->LocateProtocol(
+      &gEfiUnicodeCollation2ProtocolGuid,
+      NULL,
+      (VOID**)&mUnicodeCollation);
+
+    ASSERT_EFI_ERROR(Status);
+  }
+
+  return (mUnicodeCollation->StriColl(
+    mUnicodeCollation,
+    *(CHAR16**)Buffer1,
+    *(CHAR16**)Buffer2));
+}
+
+
+/**
+  Function to compare 2 strings.
+
+  @param[in] Buffer1            Pointer to String to compare (CHAR16**).
+  @param[in] Buffer2            Pointer to second String to compare (CHAR16**).
+
+  @retval 0                     Buffer1 equal to Buffer2.
+  @retval <0                    Buffer1 is less than Buffer2.
+  @retval >0                    Buffer1 is greater than Buffer2.
+**/
+INTN
+EFIAPI
+StringCompare (
+  IN  CONST VOID                *Buffer1,
+  IN  CONST VOID                *Buffer2
+  )
+{
+  return (StrCmp(
+    *(CHAR16**)Buffer1,
+    *(CHAR16**)Buffer2));
+}
diff --git a/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.inf b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.inf
new file mode 100644
index 0000000000..32c02b012f
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.inf
@@ -0,0 +1,47 @@
+##  @file
+#   Library used for sorting routines.
+#
+#  Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010006
+  BASE_NAME                      = UefiSortLib
+  MODULE_UNI_FILE                = UefiSortLib.uni
+  FILE_GUID                      = 4264A823-45A3-42db-B92C-AA078555CBD3
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = SortLib|UEFI_APPLICATION UEFI_DRIVER UEFI_DRIVER DXE_RUNTIME_DRIVER DXE_DRIVER
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources.common]
+  UefiSortLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  UefiBootServicesTableLib
+  DevicePathLib
+
+[Protocols]
+  gEfiUnicodeCollation2ProtocolGuid                       ## CONSUMES
+  gEfiDevicePathProtocolGuid                              ## CONSUMES
+
diff --git a/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.uni b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.uni
new file mode 100644
index 0000000000..2163301c42
--- /dev/null
+++ b/Core/MdeModulePkg/Library/UefiSortLib/UefiSortLib.uni
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/InternalVarCheckStructure.h b/Core/MdeModulePkg/Library/VarCheckHiiLib/InternalVarCheckStructure.h
new file mode 100644
index 0000000000..a9faed48d9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/InternalVarCheckStructure.h
@@ -0,0 +1,82 @@
+/** @file
+  Internal structure for Var Check Hii.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VAR_CHECK_STRUCTURE_H_
+#define _VAR_CHECK_STRUCTURE_H_
+
+//
+// Alignment for Hii Variable and Question header.
+//
+#define HEADER_ALIGNMENT  4
+#define HEADER_ALIGN(Header)  (((UINTN) (Header) + HEADER_ALIGNMENT - 1) & (~(HEADER_ALIGNMENT - 1)))
+
+#pragma pack (1)
+
+#define VAR_CHECK_HII_REVISION  0x0001
+
+typedef struct {
+  UINT16            Revision;
+  UINT16            HeaderLength;
+  UINT32            Length; // Length include this header
+  UINT8             OpCode;
+  UINT8             Reserved;
+  UINT16            Size;
+  UINT32            Attributes;
+  EFI_GUID          Guid;
+//CHAR16              Name[];
+} VAR_CHECK_HII_VARIABLE_HEADER;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+} VAR_CHECK_HII_QUESTION_HEADER;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+//UINTx               Data[]; // x = UINT8/UINT16/UINT32/UINT64;
+} VAR_CHECK_HII_QUESTION_ONEOF;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+} VAR_CHECK_HII_QUESTION_CHECKBOX;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+//UINTx               Minimum; // x = UINT8/UINT16/UINT32/UINT64;
+//UINTx               Maximum; // x = UINT8/UINT16/UINT32/UINT64;
+} VAR_CHECK_HII_QUESTION_NUMERIC;
+
+typedef struct {
+  UINT8             OpCode;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+  UINT8             MaxContainers;
+//UINTx               Data[]; // x = UINT8/UINT16/UINT32/UINT64;
+} VAR_CHECK_HII_QUESTION_ORDEREDLIST;
+
+#pragma pack ()
+
+#endif
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHii.h b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHii.h
new file mode 100644
index 0000000000..d770785490
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHii.h
@@ -0,0 +1,63 @@
+/** @file
+  Include file for Var Check Hii handler and bin.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VAR_CHECK_HII_H_
+#define _VAR_CHECK_HII_H_
+
+#include <Library/VarCheckLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Guid/ZeroGuid.h>
+
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/FirmwareVolume2.h>
+#include <Protocol/FirmwareVolumeBlock.h>
+
+#include "InternalVarCheckStructure.h"
+#include "VarCheckHiiGen.h"
+
+//#define DUMP_VAR_CHECK_HII
+//#define DUMP_HII_DATA
+
+typedef struct {
+  UINT8     HiiOpCode;
+  CHAR8     *HiiOpCodeStr;
+} VAR_CHECK_HII_OPCODE_STRING;
+
+typedef struct {
+  UINT8     PackageType;
+  CHAR8     *PackageTypeStr;
+} VAR_CHECK_HII_PACKAGE_TYPE_STRING;
+
+/**
+  Dump Var Check HII.
+
+  @param[in] VarCheckHiiBin     Pointer to VarCheckHiiBin.
+  @param[in] VarCheckHiiBinSize VarCheckHiiBin size.
+
+**/
+VOID
+DumpVarCheckHii (
+  IN VOID   *VarCheckHiiBin,
+  IN UINTN  VarCheckHiiBinSize
+  );
+
+extern VAR_CHECK_HII_VARIABLE_HEADER    *mVarCheckHiiBin;
+extern UINTN                            mVarCheckHiiBinSize;
+
+#endif
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.c b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.c
new file mode 100644
index 0000000000..cab92967aa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.c
@@ -0,0 +1,1483 @@
+/** @file
+  Var Check Hii bin generation.
+
+Copyright (c) 2015 - 2016, 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 "VarCheckHiiGen.h"
+
+LIST_ENTRY mVarCheckHiiList = INITIALIZE_LIST_HEAD_VARIABLE (mVarCheckHiiList);
+
+#define VAR_CHECK_HII_VARIABLE_NODE_SIGNATURE   SIGNATURE_32 ('V', 'C', 'H', 'V')
+
+typedef struct {
+  UINTN                         Signature;
+  LIST_ENTRY                    Link;
+  VAR_CHECK_HII_VARIABLE_HEADER *HiiVariable;
+  EFI_VARSTORE_ID               VarStoreId;
+
+  VAR_CHECK_HII_QUESTION_HEADER **HiiQuestionArray;
+} VAR_CHECK_HII_VARIABLE_NODE;
+
+#define VAR_CHECK_HII_VARIABLE_FROM_LINK(a) CR (a, VAR_CHECK_HII_VARIABLE_NODE, Link, VAR_CHECK_HII_VARIABLE_NODE_SIGNATURE)
+
+CHAR16 *mVarName = NULL;
+UINTN  mMaxVarNameSize = 0;
+
+#ifdef DUMP_HII_DATA
+GLOBAL_REMOVE_IF_UNREFERENCED VAR_CHECK_HII_OPCODE_STRING   mIfrOpCodeStringTable[] = {
+  {EFI_IFR_VARSTORE_OP,             "EFI_IFR_VARSTORE_OP"},
+  {EFI_IFR_VARSTORE_EFI_OP,         "EFI_IFR_VARSTORE_EFI_OP"},
+  {EFI_IFR_ONE_OF_OP,               "EFI_IFR_ONE_OF_OP"},
+  {EFI_IFR_CHECKBOX_OP,             "EFI_IFR_CHECKBOX_OP"},
+  {EFI_IFR_NUMERIC_OP,              "EFI_IFR_NUMERIC_OP"},
+  {EFI_IFR_ORDERED_LIST_OP,         "EFI_IFR_ORDERED_LIST_OP"},
+  {EFI_IFR_ONE_OF_OPTION_OP,        "EFI_IFR_ONE_OF_OPTION_OP"},
+};
+
+/**
+  Ifr opcode to string.
+
+  @param[in] IfrOpCode  Ifr OpCode.
+
+  @return Pointer to string.
+
+**/
+CHAR8 *
+IfrOpCodeToStr (
+  IN UINT8  IfrOpCode
+  )
+{
+  UINTN  Index;
+  for (Index = 0; Index < sizeof (mIfrOpCodeStringTable) / sizeof (mIfrOpCodeStringTable[0]); Index++) {
+    if (mIfrOpCodeStringTable[Index].HiiOpCode == IfrOpCode) {
+      return mIfrOpCodeStringTable[Index].HiiOpCodeStr;
+    }
+  }
+
+  return "<UnknownIfrOpCode>";
+}
+
+GLOBAL_REMOVE_IF_UNREFERENCED VAR_CHECK_HII_PACKAGE_TYPE_STRING  mPackageTypeStringTable[] = {
+  {EFI_HII_PACKAGE_TYPE_ALL,            "EFI_HII_PACKAGE_TYPE_ALL"},
+  {EFI_HII_PACKAGE_TYPE_GUID,           "EFI_HII_PACKAGE_TYPE_GUID"},
+  {EFI_HII_PACKAGE_FORMS,               "EFI_HII_PACKAGE_FORMS"},
+  {EFI_HII_PACKAGE_STRINGS,             "EFI_HII_PACKAGE_STRINGS"},
+  {EFI_HII_PACKAGE_FONTS,               "EFI_HII_PACKAGE_FONTS"},
+  {EFI_HII_PACKAGE_IMAGES,              "EFI_HII_PACKAGE_IMAGES"},
+  {EFI_HII_PACKAGE_SIMPLE_FONTS,        "EFI_HII_PACKAGE_SIMPLE_FONTS"},
+  {EFI_HII_PACKAGE_DEVICE_PATH,         "EFI_HII_PACKAGE_DEVICE_PATH"},
+  {EFI_HII_PACKAGE_KEYBOARD_LAYOUT,     "EFI_HII_PACKAGE_KEYBOARD_LAYOUT"},
+  {EFI_HII_PACKAGE_ANIMATIONS,          "EFI_HII_PACKAGE_ANIMATIONS"},
+  {EFI_HII_PACKAGE_END,                 "EFI_HII_PACKAGE_END"},
+  {EFI_HII_PACKAGE_TYPE_SYSTEM_BEGIN,   "EFI_HII_PACKAGE_TYPE_SYSTEM_BEGIN"},
+  {EFI_HII_PACKAGE_TYPE_SYSTEM_END,     "EFI_HII_PACKAGE_TYPE_SYSTEM_END"},
+};
+
+/**
+  Hii Package type to string.
+
+  @param[in] PackageType    Package Type
+
+  @return Pointer to string.
+
+**/
+CHAR8 *
+HiiPackageTypeToStr (
+  IN UINT8  PackageType
+  )
+{
+  UINTN     Index;
+  for (Index = 0; Index < sizeof (mPackageTypeStringTable) / sizeof (mPackageTypeStringTable[0]); Index++) {
+    if (mPackageTypeStringTable[Index].PackageType == PackageType) {
+      return mPackageTypeStringTable[Index].PackageTypeStr;
+    }
+  }
+
+  return "<UnknownPackageType>";
+}
+
+/**
+  Dump Hii Package.
+
+  @param[in] HiiPackage         Pointer to Hii Package.
+
+**/
+VOID
+DumpHiiPackage (
+  IN VOID       *HiiPackage
+  )
+{
+  EFI_HII_PACKAGE_HEADER        *HiiPackageHeader;
+  EFI_IFR_OP_HEADER             *IfrOpCodeHeader;
+  EFI_IFR_VARSTORE              *IfrVarStore;
+  EFI_IFR_VARSTORE_EFI          *IfrEfiVarStore;
+
+  HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) HiiPackage;
+
+  DEBUG ((EFI_D_INFO, "  HiiPackageHeader->Type   - 0x%02x (%a)\n", HiiPackageHeader->Type, HiiPackageTypeToStr ((UINT8) HiiPackageHeader->Type)));
+  DEBUG ((EFI_D_INFO, "  HiiPackageHeader->Length - 0x%06x\n", HiiPackageHeader->Length));
+
+  switch (HiiPackageHeader->Type) {
+    case EFI_HII_PACKAGE_FORMS:
+      IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) (HiiPackageHeader + 1);
+
+      while ((UINTN) IfrOpCodeHeader < ((UINTN) HiiPackageHeader + HiiPackageHeader->Length)) {
+        switch (IfrOpCodeHeader->OpCode) {
+          case EFI_IFR_VARSTORE_OP:
+            IfrVarStore = (EFI_IFR_VARSTORE *) IfrOpCodeHeader;
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->OpCode - 0x%02x (%a)\n", IfrOpCodeHeader->OpCode, IfrOpCodeToStr (IfrOpCodeHeader->OpCode)));
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Length - 0x%02x\n", IfrOpCodeHeader->Length));
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Scope  - 0x%02x\n", IfrOpCodeHeader->Scope));
+            DEBUG ((EFI_D_INFO, "      Guid       - %g\n", &IfrVarStore->Guid));
+            DEBUG ((EFI_D_INFO, "      VarStoreId - 0x%04x\n", IfrVarStore->VarStoreId));
+            DEBUG ((EFI_D_INFO, "      Size       - 0x%04x\n", IfrVarStore->Size));
+            DEBUG ((EFI_D_INFO, "      Name       - %a\n", IfrVarStore->Name));
+            break;
+
+          case EFI_IFR_VARSTORE_EFI_OP:
+            IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpCodeHeader;
+            if (IfrEfiVarStore->Header.Length >= sizeof (EFI_IFR_VARSTORE_EFI)) {
+              DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->OpCode - 0x%02x (%a)\n", IfrOpCodeHeader->OpCode, IfrOpCodeToStr (IfrOpCodeHeader->OpCode)));
+              DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Length - 0x02%x\n", IfrOpCodeHeader->Length));
+              DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Scope  - 0x02%x\n", IfrOpCodeHeader->Scope));
+              DEBUG ((EFI_D_INFO, "      Guid       - %g\n", &IfrEfiVarStore->Guid));
+              DEBUG ((EFI_D_INFO, "      VarStoreId - 0x%04x\n", IfrEfiVarStore->VarStoreId));
+              DEBUG ((EFI_D_INFO, "      Size       - 0x%04x\n", IfrEfiVarStore->Size));
+              DEBUG ((EFI_D_INFO, "      Attributes - 0x%08x\n", IfrEfiVarStore->Attributes));
+              DEBUG ((EFI_D_INFO, "      Name       - %a\n", IfrEfiVarStore->Name));
+            }
+            break;
+
+          case EFI_IFR_ONE_OF_OP:
+          case EFI_IFR_CHECKBOX_OP:
+          case EFI_IFR_NUMERIC_OP:
+          case EFI_IFR_ORDERED_LIST_OP:
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->OpCode - 0x%02x (%a)\n", IfrOpCodeHeader->OpCode, IfrOpCodeToStr (IfrOpCodeHeader->OpCode)));
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Length - 0x02%x\n", IfrOpCodeHeader->Length));
+            DEBUG ((EFI_D_INFO, "    IfrOpCodeHeader->Scope  - 0x02%x\n", IfrOpCodeHeader->Scope));
+            DEBUG ((EFI_D_INFO, "      Prompt       - 0x%04x\n", ((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.Header.Prompt));
+            DEBUG ((EFI_D_INFO, "      Help         - 0x%04x\n", ((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.Header.Help));
+            DEBUG ((EFI_D_INFO, "      QuestionId   - 0x%04x\n", ((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.QuestionId));
+            DEBUG ((EFI_D_INFO, "      VarStoreId   - 0x%04x\n", ((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.VarStoreId));
+            DEBUG ((EFI_D_INFO, "      VarStoreInfo - 0x%04x\n", ((EFI_IFR_ONE_OF * )IfrOpCodeHeader)->Question.VarStoreInfo.VarOffset));
+            {
+              EFI_IFR_ONE_OF            *IfrOneOf;
+              EFI_IFR_CHECKBOX          *IfrCheckBox;
+              EFI_IFR_NUMERIC           *IfrNumeric;
+              EFI_IFR_ORDERED_LIST      *IfrOrderedList;
+
+              switch (IfrOpCodeHeader->OpCode) {
+                case EFI_IFR_ONE_OF_OP:
+                  IfrOneOf = (EFI_IFR_ONE_OF *) IfrOpCodeHeader;
+                  DEBUG ((EFI_D_INFO, "      Flags         - 0x%02x\n", IfrOneOf->Flags));
+                  switch (IfrOneOf->Flags & EFI_IFR_NUMERIC_SIZE) {
+                  case EFI_IFR_NUMERIC_SIZE_1:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%02x\n", IfrOneOf->data.u8.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%02x\n", IfrOneOf->data.u8.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%02x\n", IfrOneOf->data.u8.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_2:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%04x\n", IfrOneOf->data.u16.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%04x\n", IfrOneOf->data.u16.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%04x\n", IfrOneOf->data.u16.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_4:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%08x\n", IfrOneOf->data.u32.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%08x\n", IfrOneOf->data.u32.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%08x\n", IfrOneOf->data.u32.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_8:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%016lx\n", IfrOneOf->data.u64.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%016lx\n", IfrOneOf->data.u64.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%016lx\n", IfrOneOf->data.u64.Step));
+                    break;
+                  }
+                  break;
+                case EFI_IFR_CHECKBOX_OP:
+                  IfrCheckBox = (EFI_IFR_CHECKBOX *) IfrOpCodeHeader;
+                  DEBUG ((EFI_D_INFO, "      Flags         - 0x%02x\n", IfrCheckBox->Flags));
+                  break;
+                case EFI_IFR_NUMERIC_OP:
+                  IfrNumeric = (EFI_IFR_NUMERIC *) IfrOpCodeHeader;
+                  DEBUG ((EFI_D_INFO, "      Flags         - 0x%02x\n", IfrNumeric->Flags));
+                  switch (IfrNumeric->Flags & EFI_IFR_NUMERIC_SIZE) {
+                  case EFI_IFR_NUMERIC_SIZE_1:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%02x\n", IfrNumeric->data.u8.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%02x\n", IfrNumeric->data.u8.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%02x\n", IfrNumeric->data.u8.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_2:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%04x\n", IfrNumeric->data.u16.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%04x\n", IfrNumeric->data.u16.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%04x\n", IfrNumeric->data.u16.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_4:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%08x\n", IfrNumeric->data.u32.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%08x\n", IfrNumeric->data.u32.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%08x\n", IfrNumeric->data.u32.Step));
+                    break;
+                  case EFI_IFR_NUMERIC_SIZE_8:
+                    DEBUG ((EFI_D_INFO, "      MinValue      - 0x%016lx\n", IfrNumeric->data.u64.MinValue));
+                    DEBUG ((EFI_D_INFO, "      MaxValue      - 0x%016lx\n", IfrNumeric->data.u64.MaxValue));
+                    DEBUG ((EFI_D_INFO, "      Step          - 0x%016lx\n", IfrNumeric->data.u64.Step));
+                    break;
+                  }
+                  break;
+                case EFI_IFR_ORDERED_LIST_OP:
+                  IfrOrderedList = (EFI_IFR_ORDERED_LIST *) IfrOpCodeHeader;
+                  DEBUG ((EFI_D_INFO, "      MaxContainers - 0x%02x\n", IfrOrderedList->MaxContainers));
+                  DEBUG ((EFI_D_INFO, "      Flags         - 0x%02x\n", IfrOrderedList->Flags));
+                  break;
+                default:
+                  break;
+              }
+
+              if (IfrOpCodeHeader->Scope != 0) {
+                UINTN                       Scope;
+                EFI_IFR_ONE_OF_OPTION       *IfrOneOfOption;
+
+                IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+                Scope = 1;
+                while (Scope != 0) {
+                  switch (IfrOpCodeHeader->OpCode) {
+                    case EFI_IFR_ONE_OF_OPTION_OP:
+                      IfrOneOfOption = (EFI_IFR_ONE_OF_OPTION *)IfrOpCodeHeader;
+                      DEBUG ((EFI_D_INFO, "!!!!    IfrOpCodeHeader->OpCode - 0x%02x (%a)\n", (UINTN)IfrOpCodeHeader->OpCode, IfrOpCodeToStr (IfrOpCodeHeader->OpCode)));
+                      DEBUG ((EFI_D_INFO, "!!!!    IfrOpCodeHeader->Scope  - 0x%02x\n", IfrOpCodeHeader->Scope));
+                      DEBUG ((EFI_D_INFO, "!!!!      Option                - 0x%04x\n", IfrOneOfOption->Option));
+                      DEBUG ((EFI_D_INFO, "!!!!      Flags                 - 0x%02x\n", IfrOneOfOption->Flags));
+                      DEBUG ((EFI_D_INFO, "!!!!      Type                  - 0x%02x\n", IfrOneOfOption->Type));
+                      switch (IfrOneOfOption->Type) {
+                        case EFI_IFR_TYPE_NUM_SIZE_8:
+                          DEBUG ((EFI_D_INFO, "!!!!      Value                 - 0x%02x\n", IfrOneOfOption->Value.u8));
+                          break;
+                        case EFI_IFR_TYPE_NUM_SIZE_16:
+                          DEBUG ((EFI_D_INFO, "!!!!      Value                 - 0x%04x\n", IfrOneOfOption->Value.u16));
+                          break;
+                        case EFI_IFR_TYPE_NUM_SIZE_32:
+                          DEBUG ((EFI_D_INFO, "!!!!      Value                 - 0x%08x\n", IfrOneOfOption->Value.u32));
+                          break;
+                        case EFI_IFR_TYPE_NUM_SIZE_64:
+                          DEBUG ((EFI_D_INFO, "!!!!      Value                 - 0x%016lx\n", IfrOneOfOption->Value.u64));
+                          break;
+                        case EFI_IFR_TYPE_BOOLEAN:
+                          DEBUG ((EFI_D_INFO, "!!!!      Value                 - 0x%02x\n", IfrOneOfOption->Value.b));
+                          break;
+                        default:
+                          break;
+                      }
+                      break;
+                  }
+
+                  if (IfrOpCodeHeader->OpCode == EFI_IFR_END_OP) {
+                    ASSERT (Scope > 0);
+                    Scope--;
+                    if (Scope == 0) {
+                      break;
+                    }
+                  } else if (IfrOpCodeHeader->Scope != 0) {
+                    Scope++;
+                  }
+                  IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+                }
+              }
+            }
+          default:
+            break;
+        }
+        IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+      }
+      break;
+    default:
+      break;
+  }
+}
+
+/**
+  Dump Hii Database.
+
+  @param[in] HiiDatabase        Pointer to Hii Database.
+  @param[in] HiiDatabaseSize    Hii Database size.
+
+**/
+VOID
+DumpHiiDatabase (
+  IN VOID       *HiiDatabase,
+  IN UINTN      HiiDatabaseSize
+  )
+{
+  EFI_HII_PACKAGE_LIST_HEADER   *HiiPackageListHeader;
+  EFI_HII_PACKAGE_HEADER        *HiiPackageHeader;
+
+  DEBUG ((EFI_D_INFO, "HiiDatabaseSize - 0x%x\n", HiiDatabaseSize));
+  HiiPackageListHeader = (EFI_HII_PACKAGE_LIST_HEADER *) HiiDatabase;
+
+  while ((UINTN) HiiPackageListHeader < ((UINTN) HiiDatabase + HiiDatabaseSize)) {
+    DEBUG ((EFI_D_INFO, "HiiPackageListHeader->PackageListGuid - %g\n", &HiiPackageListHeader->PackageListGuid));
+    DEBUG ((EFI_D_INFO, "HiiPackageListHeader->PackageLength   - 0x%x\n", (UINTN)HiiPackageListHeader->PackageLength));
+    HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *)(HiiPackageListHeader + 1);
+
+    while ((UINTN) HiiPackageHeader < (UINTN) HiiPackageListHeader + HiiPackageListHeader->PackageLength) {
+
+      DumpHiiPackage (HiiPackageHeader);
+
+      HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) ((UINTN) HiiPackageHeader + HiiPackageHeader->Length);
+    }
+
+    HiiPackageListHeader = (EFI_HII_PACKAGE_LIST_HEADER *) ((UINTN) HiiPackageListHeader + HiiPackageListHeader->PackageLength);
+  }
+
+  return ;
+}
+#endif
+
+/**
+  Allocates a buffer of a certain pool type.
+
+  Allocates the number bytes specified by AllocationSize of a certain pool type and returns a
+  pointer to the allocated buffer.  If AllocationSize is 0, then a valid buffer of 0 size is
+  returned.  If there is not enough memory remaining to satisfy the request, then NULL is returned.
+
+  @param  MemoryType            The type of memory to allocate.
+  @param  AllocationSize        The number of bytes to allocate.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalVarCheckAllocatePool (
+  IN EFI_MEMORY_TYPE  MemoryType,
+  IN UINTN            AllocationSize
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Memory;
+
+  Status = gBS->AllocatePool (MemoryType, AllocationSize, &Memory);
+  if (EFI_ERROR (Status)) {
+    Memory = NULL;
+  }
+  return Memory;
+}
+
+/**
+  Allocates and zeros a buffer of type EfiBootServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiBootServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalVarCheckAllocateZeroPool (
+  IN UINTN            AllocationSize
+  )
+{
+  VOID  *Memory;
+
+  Memory = InternalVarCheckAllocatePool (EfiBootServicesData, AllocationSize);
+  if (Memory != NULL) {
+    Memory = ZeroMem (Memory, AllocationSize);
+  }
+  return Memory;
+}
+
+/**
+  Frees a buffer that was previously allocated with one of the pool allocation functions in the
+  Memory Allocation Library.
+
+  Frees the buffer specified by Buffer.  Buffer must have been allocated on a previous call to the
+  pool allocation services of the Memory Allocation Library.  If it is not possible to free pool
+  resources, then this function will perform no actions.
+
+  If Buffer was not allocated with a pool allocation function in the Memory Allocation Library,
+  then ASSERT().
+
+  @param  Buffer                The pointer to the buffer to free.
+
+**/
+VOID
+EFIAPI
+InternalVarCheckFreePool (
+  IN VOID   *Buffer
+  )
+{
+  EFI_STATUS    Status;
+
+  Status = gBS->FreePool (Buffer);
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Reallocates a buffer of type EfiBootServicesData.
+
+  Allocates and zeros the number bytes specified by NewSize from memory of type
+  EfiBootServicesData.  If OldBuffer is not NULL, then the smaller of OldSize and
+  NewSize bytes are copied from OldBuffer to the newly allocated buffer, and
+  OldBuffer is freed.  A pointer to the newly allocated buffer is returned.
+  If NewSize is 0, then a valid buffer of 0 size is  returned.  If there is not
+  enough memory remaining to satisfy the request, then NULL is returned.
+
+  If the allocation of the new buffer is successful and the smaller of NewSize and OldSize
+  is greater than (MAX_ADDRESS - OldBuffer + 1), then ASSERT().
+
+  @param  OldSize        The size, in bytes, of OldBuffer.
+  @param  NewSize        The size, in bytes, of the buffer to reallocate.
+  @param  OldBuffer      The buffer to copy to the allocated buffer.  This is an optional
+                         parameter that may be NULL.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalVarCheckReallocatePool (
+  IN UINTN            OldSize,
+  IN UINTN            NewSize,
+  IN VOID             *OldBuffer  OPTIONAL
+  )
+{
+  VOID  *NewBuffer;
+
+  NewBuffer = InternalVarCheckAllocateZeroPool (NewSize);
+  if (NewBuffer != NULL && OldBuffer != NULL) {
+    CopyMem (NewBuffer, OldBuffer, MIN (OldSize, NewSize));
+    InternalVarCheckFreePool (OldBuffer);
+  }
+  return NewBuffer;
+}
+
+/**
+  Merge Hii Question.
+
+  @param[in, out] HiiVariableNode   Pointer to Hii Variable node.
+  @param[in]      HiiQuestion       Pointer to Hii Question.
+  @param[in]      FromFv            Hii Question from FV.
+
+**/
+VOID
+MergeHiiQuestion (
+  IN OUT VAR_CHECK_HII_VARIABLE_NODE    *HiiVariableNode,
+  IN VAR_CHECK_HII_QUESTION_HEADER      *HiiQuestion,
+  IN BOOLEAN                            FromFv
+  )
+{
+  VAR_CHECK_HII_QUESTION_HEADER     *HiiQuestion1;
+  VAR_CHECK_HII_QUESTION_HEADER     *HiiQuestion2;
+  VAR_CHECK_HII_QUESTION_HEADER     *NewHiiQuestion;
+  UINT8                             NewLength;
+  UINT64                            Minimum1;
+  UINT64                            Maximum1;
+  UINT64                            OneValue1;
+  UINT64                            Minimum2;
+  UINT64                            Maximum2;
+  UINT64                            OneValue2;
+  UINT8                             *Ptr;
+  UINT8                             *Ptr1;
+  UINT8                             *Ptr2;
+
+  //
+  // Hii Question from Hii Database has high priority.
+  // Do not to merge Hii Question from Fv to Hii Question from Hii Database.
+  //
+  if (FromFv) {
+    InternalVarCheckFreePool (HiiQuestion);
+    return;
+  }
+
+  HiiQuestion1 = HiiVariableNode->HiiQuestionArray[HiiQuestion->VarOffset];
+  HiiQuestion2 = HiiQuestion;
+
+  ASSERT ((HiiQuestion1->OpCode == HiiQuestion2->OpCode) && (HiiQuestion1->StorageWidth == HiiQuestion2->StorageWidth));
+
+  switch (HiiQuestion1->OpCode) {
+    case EFI_IFR_ONE_OF_OP:
+      DEBUG ((EFI_D_INFO, "MergeHiiQuestion - EFI_IFR_ONE_OF_OP VarOffset = 0x%04x\n", HiiQuestion1->VarOffset));
+      //
+      // Get the length of Hii Question 1.
+      //
+      NewLength = HiiQuestion1->Length;
+
+      //
+      // Check if the one of options in Hii Question 2 have been in Hii Question 1.
+      //
+      Ptr2 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion2 + 1);
+      while ((UINTN) Ptr2 < (UINTN) HiiQuestion2 + HiiQuestion2->Length) {
+        OneValue2 = 0;
+        CopyMem (&OneValue2, Ptr2, HiiQuestion2->StorageWidth);
+
+        Ptr1 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion1 + 1);
+        while ((UINTN) Ptr1 < (UINTN) HiiQuestion1 + HiiQuestion1->Length) {
+          OneValue1 = 0;
+          CopyMem (&OneValue1, Ptr1, HiiQuestion1->StorageWidth);
+          if (OneValue2 == OneValue1) {
+            //
+            // Match
+            //
+            break;
+          }
+          Ptr1 += HiiQuestion1->StorageWidth;
+        }
+        if ((UINTN) Ptr1 >= ((UINTN) HiiQuestion1 + HiiQuestion1->Length)) {
+          //
+          // No match
+          //
+          NewLength = (UINT8) (NewLength + HiiQuestion1->StorageWidth);
+        }
+        Ptr2 += HiiQuestion2->StorageWidth;
+      }
+
+      if (NewLength > HiiQuestion1->Length) {
+        //
+        // Merge the one of options of Hii Question 2 and Hii Question 1.
+        //
+        NewHiiQuestion = InternalVarCheckAllocateZeroPool (NewLength);
+        ASSERT (NewHiiQuestion != NULL);
+        CopyMem (NewHiiQuestion, HiiQuestion1, HiiQuestion1->Length);
+        //
+        // Use the new length.
+        //
+        NewHiiQuestion->Length = NewLength;
+        Ptr = (UINT8 *) NewHiiQuestion + HiiQuestion1->Length;
+
+        Ptr2 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion2 + 1);
+        while ((UINTN) Ptr2 < (UINTN) HiiQuestion2 + HiiQuestion2->Length) {
+          OneValue2 = 0;
+          CopyMem (&OneValue2, Ptr2, HiiQuestion2->StorageWidth);
+
+          Ptr1 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion1 + 1);
+          while ((UINTN) Ptr1 < (UINTN) HiiQuestion1 + HiiQuestion1->Length) {
+            OneValue1 = 0;
+            CopyMem (&OneValue1, Ptr1, HiiQuestion1->StorageWidth);
+            if (OneValue2 == OneValue1) {
+              //
+              // Match
+              //
+              break;
+            }
+            Ptr1 += HiiQuestion1->StorageWidth;
+          }
+          if ((UINTN) Ptr1 >= ((UINTN) HiiQuestion1 + HiiQuestion1->Length)) {
+            //
+            // No match
+            //
+            CopyMem (Ptr, &OneValue2, HiiQuestion1->StorageWidth);
+            Ptr += HiiQuestion1->StorageWidth;
+          }
+          Ptr2 += HiiQuestion2->StorageWidth;
+        }
+
+        HiiVariableNode->HiiQuestionArray[HiiQuestion1->VarOffset] = NewHiiQuestion;
+        InternalVarCheckFreePool (HiiQuestion1);
+      }
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      DEBUG ((EFI_D_INFO, "MergeHiiQuestion - EFI_IFR_CHECKBOX_OP VarOffset = 0x%04x\n", HiiQuestion1->VarOffset));
+      break;
+
+    case EFI_IFR_NUMERIC_OP:
+      DEBUG ((EFI_D_INFO, "MergeHiiQuestion - EFI_IFR_NUMERIC_OP VarOffset = 0x%04x\n", HiiQuestion1->VarOffset));
+      //
+      // Get minimum and maximum of Hii Question 1.
+      //
+      Minimum1 = 0;
+      Maximum1 = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_NUMERIC *) HiiQuestion1 + 1);
+      CopyMem (&Minimum1, Ptr, HiiQuestion1->StorageWidth);
+      Ptr += HiiQuestion1->StorageWidth;
+      CopyMem (&Maximum1, Ptr, HiiQuestion1->StorageWidth);
+
+      //
+      // Get minimum and maximum of Hii Question 2.
+      //
+      Minimum2 = 0;
+      Maximum2 = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_NUMERIC *) HiiQuestion2 + 1);
+      CopyMem (&Minimum2, Ptr, HiiQuestion2->StorageWidth);
+      Ptr += HiiQuestion2->StorageWidth;
+      CopyMem (&Maximum2, Ptr, HiiQuestion2->StorageWidth);
+
+      //
+      // Update minimum.
+      //
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_NUMERIC *) HiiQuestion1 + 1);
+      if (Minimum2 < Minimum1) {
+        Minimum1 = Minimum2;
+        CopyMem (Ptr, &Minimum1, HiiQuestion1->StorageWidth);
+      }
+      //
+      // Update maximum.
+      //
+      Ptr += HiiQuestion1->StorageWidth;
+      if (Maximum2 > Maximum1) {
+        Maximum1 = Maximum2;
+        CopyMem (Ptr, &Maximum1, HiiQuestion1->StorageWidth);
+      }
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      DEBUG ((EFI_D_INFO, "MergeHiiQuestion - EFI_IFR_ORDERED_LIST_OP VarOffset = 0x%04x\n", HiiQuestion1->VarOffset));
+      //
+      // Get the length of Hii Question 1.
+      //
+      NewLength = HiiQuestion1->Length;
+
+      //
+      // Check if the one of options in Hii Question 2 have been in Hii Question 1.
+      //
+      Ptr2 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion2 + 1);
+      while ((UINTN) Ptr2 < (UINTN) HiiQuestion2 + HiiQuestion2->Length) {
+        OneValue2 = 0;
+        CopyMem (&OneValue2, Ptr2, HiiQuestion2->StorageWidth);
+
+        Ptr1 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion1 + 1);
+        while ((UINTN) Ptr1 < (UINTN) HiiQuestion1 + HiiQuestion1->Length) {
+          OneValue1 = 0;
+          CopyMem (&OneValue1, Ptr1, HiiQuestion1->StorageWidth);
+          if (OneValue2 == OneValue1) {
+            //
+            // Match
+            //
+            break;
+          }
+          Ptr1 += HiiQuestion1->StorageWidth;
+        }
+        if ((UINTN) Ptr1 >= ((UINTN) HiiQuestion1 + HiiQuestion1->Length)) {
+          //
+          // No match
+          //
+          NewLength = (UINT8) (NewLength + HiiQuestion1->StorageWidth);
+        }
+        Ptr2 += HiiQuestion2->StorageWidth;
+      }
+
+      if (NewLength > HiiQuestion1->Length) {
+        //
+        // Merge the one of options of Hii Question 2 and Hii Question 1.
+        //
+        NewHiiQuestion = InternalVarCheckAllocateZeroPool (NewLength);
+        ASSERT (NewHiiQuestion != NULL);
+        CopyMem (NewHiiQuestion, HiiQuestion1, HiiQuestion1->Length);
+        //
+        // Use the new length.
+        //
+        NewHiiQuestion->Length = NewLength;
+        Ptr = (UINT8 *) NewHiiQuestion + HiiQuestion1->Length;
+
+        Ptr2 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion2 + 1);
+        while ((UINTN) Ptr2 < (UINTN) HiiQuestion2 + HiiQuestion2->Length) {
+          OneValue2 = 0;
+          CopyMem (&OneValue2, Ptr2, HiiQuestion2->StorageWidth);
+
+          Ptr1 = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion1 + 1);
+          while ((UINTN) Ptr1 < (UINTN) HiiQuestion1 + HiiQuestion1->Length) {
+            OneValue1 = 0;
+            CopyMem (&OneValue1, Ptr1, HiiQuestion1->StorageWidth);
+            if (OneValue2 == OneValue1) {
+              //
+              // Match
+              //
+              break;
+            }
+            Ptr1 += HiiQuestion1->StorageWidth;
+          }
+          if ((UINTN) Ptr1 >= ((UINTN) HiiQuestion1 + HiiQuestion1->Length)) {
+            //
+            // No match
+            //
+            CopyMem (Ptr, &OneValue2, HiiQuestion1->StorageWidth);
+            Ptr += HiiQuestion1->StorageWidth;
+          }
+          Ptr2 += HiiQuestion2->StorageWidth;
+        }
+
+        HiiVariableNode->HiiQuestionArray[HiiQuestion1->VarOffset] = NewHiiQuestion;
+        InternalVarCheckFreePool (HiiQuestion1);
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      return;
+      break;
+  }
+
+  //
+  //
+  // Hii Question 2 has been merged with Hii Question 1.
+  //
+  InternalVarCheckFreePool (HiiQuestion2);
+}
+
+/**
+  Get OneOf option data.
+
+  @param[in]  IfrOpCodeHeader   Pointer to Ifr OpCode header.
+  @param[out] Count             Pointer to option count.
+  @param[out] Width             Pointer to option width.
+  @param[out] OptionBuffer      Pointer to option buffer.
+
+**/
+VOID
+GetOneOfOption (
+  IN  EFI_IFR_OP_HEADER     *IfrOpCodeHeader,
+  OUT UINTN                 *Count,
+  OUT UINT8                 *Width,
+  OUT VOID                  *OptionBuffer OPTIONAL
+  )
+{
+  UINTN                     Scope;
+  EFI_IFR_ONE_OF_OPTION     *IfrOneOfOption;
+
+  //
+  // Assume all OPTION has same Width.
+  //
+  *Count = 0;
+
+  if (IfrOpCodeHeader->Scope != 0) {
+    //
+    // Nested OpCode.
+    //
+    Scope = 1;
+    IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+    while (Scope != 0) {
+      switch (IfrOpCodeHeader->OpCode) {
+        case EFI_IFR_ONE_OF_OPTION_OP:
+          IfrOneOfOption = (EFI_IFR_ONE_OF_OPTION *) IfrOpCodeHeader;
+          switch (IfrOneOfOption->Type) {
+            case EFI_IFR_TYPE_NUM_SIZE_8:
+              *Count = *Count + 1;
+              *Width = sizeof (UINT8);
+              if (OptionBuffer != NULL) {
+                CopyMem (OptionBuffer, &IfrOneOfOption->Value.u8, sizeof (UINT8));
+                OptionBuffer = (UINT8 *) OptionBuffer + 1;
+              }
+              break;
+            case EFI_IFR_TYPE_NUM_SIZE_16:
+              *Count = *Count + 1;
+              *Width = sizeof (UINT16);
+              if (OptionBuffer != NULL) {
+                CopyMem (OptionBuffer, &IfrOneOfOption->Value.u16, sizeof (UINT16));
+                OptionBuffer = (UINT16 *) OptionBuffer + 1;
+              }
+              break;
+            case EFI_IFR_TYPE_NUM_SIZE_32:
+              *Count = *Count + 1;
+              *Width = sizeof (UINT32);
+              if (OptionBuffer != NULL) {
+                CopyMem (OptionBuffer, &IfrOneOfOption->Value.u32, sizeof (UINT32));
+                OptionBuffer = (UINT32 *) OptionBuffer + 1;
+              }
+              break;
+            case EFI_IFR_TYPE_NUM_SIZE_64:
+              *Count = *Count + 1;
+              *Width = sizeof (UINT64);
+              if (OptionBuffer != NULL) {
+                CopyMem (OptionBuffer, &IfrOneOfOption->Value.u64, sizeof (UINT64));
+                OptionBuffer = (UINT64 *) OptionBuffer + 1;
+              }
+              break;
+            case EFI_IFR_TYPE_BOOLEAN:
+              *Count = *Count + 1;
+              *Width = sizeof (BOOLEAN);
+              if (OptionBuffer != NULL) {
+                CopyMem (OptionBuffer, &IfrOneOfOption->Value.b, sizeof (BOOLEAN));
+                OptionBuffer = (BOOLEAN *) OptionBuffer + 1;
+              }
+              break;
+            default:
+              break;
+          }
+          break;
+      }
+
+      //
+      // Until End OpCode.
+      //
+      if (IfrOpCodeHeader->OpCode == EFI_IFR_END_OP) {
+        ASSERT (Scope > 0);
+        Scope--;
+        if (Scope == 0) {
+          break;
+        }
+      } else if (IfrOpCodeHeader->Scope != 0) {
+        //
+        // Nested OpCode.
+        //
+        Scope++;
+      }
+      IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+    }
+  }
+
+  return ;
+}
+
+/**
+  Parse Hii Question Oneof.
+
+  @param[in] IfrOpCodeHeader    Pointer to Ifr OpCode header.
+
+  return Pointer to Hii Question.
+
+**/
+VAR_CHECK_HII_QUESTION_HEADER *
+ParseHiiQuestionOneOf (
+  IN EFI_IFR_OP_HEADER  *IfrOpCodeHeader
+  )
+{
+  EFI_IFR_ONE_OF                *IfrOneOf;
+  VAR_CHECK_HII_QUESTION_ONEOF  *OneOf;
+  UINTN                         Length;
+  UINT8                         Width;
+  UINTN                         OptionCount;
+  UINT8                         OptionWidth;
+
+  IfrOneOf = (EFI_IFR_ONE_OF *) IfrOpCodeHeader;
+
+  Width = (UINT8) (1 << (IfrOneOf->Flags & EFI_IFR_NUMERIC_SIZE));
+
+  GetOneOfOption (IfrOpCodeHeader, &OptionCount, &OptionWidth, NULL);
+  ASSERT (Width == OptionWidth);
+
+  Length = sizeof (*OneOf) + OptionCount * Width;
+
+  OneOf = InternalVarCheckAllocateZeroPool (Length);
+  ASSERT (OneOf != NULL);
+  OneOf->OpCode       = EFI_IFR_ONE_OF_OP;
+  OneOf->Length       = (UINT8) Length;
+  OneOf->VarOffset    = IfrOneOf->Question.VarStoreInfo.VarOffset;
+  OneOf->StorageWidth = Width;
+
+  GetOneOfOption (IfrOpCodeHeader, &OptionCount, &OptionWidth, OneOf + 1);
+
+  return (VAR_CHECK_HII_QUESTION_HEADER *) OneOf;
+}
+
+/**
+  Parse Hii Question CheckBox.
+
+  @param[in] IfrOpCodeHeader    Pointer to Ifr OpCode header.
+
+  return Pointer to Hii Question.
+
+**/
+VAR_CHECK_HII_QUESTION_HEADER *
+ParseHiiQuestionCheckBox (
+  IN EFI_IFR_OP_HEADER  *IfrOpCodeHeader
+  )
+{
+  EFI_IFR_CHECKBOX                  *IfrCheckBox;
+  VAR_CHECK_HII_QUESTION_CHECKBOX   *CheckBox;
+
+  IfrCheckBox = (EFI_IFR_CHECKBOX *) IfrOpCodeHeader;
+
+  CheckBox = InternalVarCheckAllocateZeroPool (sizeof (*CheckBox));
+  ASSERT (CheckBox != NULL);
+  CheckBox->OpCode       = EFI_IFR_CHECKBOX_OP;
+  CheckBox->Length       = (UINT8) sizeof (*CheckBox);;
+  CheckBox->VarOffset    = IfrCheckBox->Question.VarStoreInfo.VarOffset;
+  CheckBox->StorageWidth = (UINT8) sizeof (BOOLEAN);
+
+  return (VAR_CHECK_HII_QUESTION_HEADER *) CheckBox;
+}
+
+/**
+  Parse Hii Question Numeric.
+
+  @param[in] IfrOpCodeHeader    Pointer to Ifr OpCode header.
+
+  return Pointer to Hii Question.
+
+**/
+VAR_CHECK_HII_QUESTION_HEADER *
+ParseHiiQuestionNumeric (
+  IN EFI_IFR_OP_HEADER  *IfrOpCodeHeader
+  )
+{
+  EFI_IFR_NUMERIC                   *IfrNumeric;
+  VAR_CHECK_HII_QUESTION_NUMERIC    *Numeric;
+  UINT8                             Width;
+
+  IfrNumeric = (EFI_IFR_NUMERIC *) IfrOpCodeHeader;
+
+  Numeric = InternalVarCheckAllocateZeroPool (sizeof (VAR_CHECK_HII_QUESTION_NUMERIC) + 2 * sizeof (UINT64));
+  ASSERT (Numeric != NULL);
+
+  Width = (UINT8) (1 << (IfrNumeric->Flags & EFI_IFR_NUMERIC_SIZE));
+
+  Numeric->OpCode       = EFI_IFR_NUMERIC_OP;
+  Numeric->Length       = (UINT8) (sizeof (VAR_CHECK_HII_QUESTION_NUMERIC) + 2 * Width);
+  Numeric->VarOffset    = IfrNumeric->Question.VarStoreInfo.VarOffset;
+  Numeric->StorageWidth = Width;
+
+  CopyMem (Numeric + 1, &IfrNumeric->data, Width * 2);
+
+  return (VAR_CHECK_HII_QUESTION_HEADER *) Numeric;
+}
+
+/**
+  Parse Hii Question OrderedList.
+
+  @param[in] IfrOpCodeHeader    Pointer to Ifr OpCode header.
+
+  return Pointer to Hii Question.
+
+**/
+VAR_CHECK_HII_QUESTION_HEADER *
+ParseHiiQuestionOrderedList (
+  IN EFI_IFR_OP_HEADER  *IfrOpCodeHeader
+  )
+{
+  EFI_IFR_ORDERED_LIST                  *IfrOrderedList;
+  VAR_CHECK_HII_QUESTION_ORDEREDLIST    *OrderedList;
+  UINTN                                 Length;
+  UINTN                                 OptionCount;
+  UINT8                                 OptionWidth;
+
+  IfrOrderedList = (EFI_IFR_ORDERED_LIST *) IfrOpCodeHeader;
+
+  GetOneOfOption (IfrOpCodeHeader, &OptionCount, &OptionWidth, NULL);
+
+  Length = sizeof (*OrderedList) + OptionCount * OptionWidth;
+
+  OrderedList = InternalVarCheckAllocateZeroPool (Length);
+  ASSERT (OrderedList != NULL);
+  OrderedList->OpCode        = EFI_IFR_ORDERED_LIST_OP;
+  OrderedList->Length        = (UINT8) Length;
+  OrderedList->VarOffset     = IfrOrderedList->Question.VarStoreInfo.VarOffset;
+  OrderedList->StorageWidth  = OptionWidth;
+  OrderedList->MaxContainers = IfrOrderedList->MaxContainers;
+
+  GetOneOfOption (IfrOpCodeHeader, &OptionCount, &OptionWidth, OrderedList + 1);
+
+  return (VAR_CHECK_HII_QUESTION_HEADER *) OrderedList;
+}
+
+/**
+  Parse and create Hii Question node.
+
+  @param[in] HiiVariableNode    Pointer to Hii Variable node.
+  @param[in] IfrOpCodeHeader    Pointer to Ifr OpCode header.
+  @param[in] FromFv             Hii Question from FV.
+
+**/
+VOID
+ParseHiiQuestion (
+  IN VAR_CHECK_HII_VARIABLE_NODE    *HiiVariableNode,
+  IN  EFI_IFR_OP_HEADER             *IfrOpCodeHeader,
+  IN BOOLEAN                        FromFv
+  )
+{
+  VAR_CHECK_HII_QUESTION_HEADER *HiiQuestion;
+
+  switch (IfrOpCodeHeader->OpCode) {
+    case EFI_IFR_ONE_OF_OP:
+      HiiQuestion = ParseHiiQuestionOneOf (IfrOpCodeHeader);
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      HiiQuestion = ParseHiiQuestionCheckBox (IfrOpCodeHeader);
+      break;
+
+    case EFI_IFR_NUMERIC_OP:
+      HiiQuestion = ParseHiiQuestionNumeric (IfrOpCodeHeader);
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      HiiQuestion = ParseHiiQuestionOrderedList (IfrOpCodeHeader);
+      break;
+
+    default:
+      ASSERT (FALSE);
+      return;
+      break;
+  }
+
+  if (HiiVariableNode->HiiQuestionArray[HiiQuestion->VarOffset] != NULL) {
+    MergeHiiQuestion (HiiVariableNode, HiiQuestion, FromFv);
+  } else {
+    HiiVariableNode->HiiQuestionArray[HiiQuestion->VarOffset] = HiiQuestion;
+  }
+}
+
+/**
+  Find Hii variable node by name and GUID.
+
+  @param[in] Name   Pointer to variable name.
+  @param[in] Guid   Pointer to vendor GUID.
+
+  @return Pointer to Hii Variable node.
+
+**/
+VAR_CHECK_HII_VARIABLE_NODE *
+FindHiiVariableNode (
+  IN CHAR16     *Name,
+  IN EFI_GUID   *Guid
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  LIST_ENTRY                    *Link;
+
+  for (Link = mVarCheckHiiList.ForwardLink
+      ;Link != &mVarCheckHiiList
+      ;Link = Link->ForwardLink) {
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (Link);
+
+    if ((StrCmp (Name, (CHAR16 *) (HiiVariableNode->HiiVariable + 1)) == 0) &&
+        CompareGuid (Guid, &HiiVariableNode->HiiVariable->Guid)) {
+      return HiiVariableNode;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Find Hii variable node by var store id.
+
+  @param[in] VarStoreId         Var store id.
+
+  @return Pointer to Hii Variable node.
+
+**/
+VAR_CHECK_HII_VARIABLE_NODE *
+FindHiiVariableNodeByVarStoreId (
+  IN EFI_VARSTORE_ID            VarStoreId
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  LIST_ENTRY                    *Link;
+
+  if (VarStoreId == 0) {
+    //
+    // The variable store identifier, which is unique within the current form set.
+    // A value of zero is invalid.
+    //
+    return NULL;
+  }
+
+  for (Link = mVarCheckHiiList.ForwardLink
+      ;Link != &mVarCheckHiiList
+      ;Link = Link->ForwardLink) {
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (Link);
+    //
+    // The variable store identifier, which is unique within the current form set.
+    //
+    if (VarStoreId == HiiVariableNode->VarStoreId) {
+      return HiiVariableNode;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Destroy var store id in the Hii Variable node after parsing one Hii Package.
+
+**/
+VOID
+DestroyVarStoreId (
+  VOID
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  LIST_ENTRY                    *Link;
+
+  for (Link = mVarCheckHiiList.ForwardLink
+      ;Link != &mVarCheckHiiList
+      ;Link = Link->ForwardLink) {
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (Link);
+    //
+    // The variable store identifier, which is unique within the current form set.
+    // A value of zero is invalid.
+    //
+    HiiVariableNode->VarStoreId = 0;
+  }
+}
+
+/**
+  Create Hii Variable node.
+
+  @param[in] IfrEfiVarStore  Pointer to EFI VARSTORE.
+
+**/
+VOID
+CreateHiiVariableNode (
+  IN EFI_IFR_VARSTORE_EFI   *IfrEfiVarStore
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  VAR_CHECK_HII_VARIABLE_HEADER *HiiVariable;
+  UINTN                         HeaderLength;
+  CHAR16                        *VarName;
+  UINTN                         VarNameSize;
+
+  //
+  // Get variable name.
+  //
+  VarNameSize = AsciiStrSize ((CHAR8 *) IfrEfiVarStore->Name) * sizeof (CHAR16);
+  if (VarNameSize > mMaxVarNameSize) {
+    mVarName = InternalVarCheckReallocatePool (mMaxVarNameSize, VarNameSize, mVarName);
+    ASSERT (mVarName != NULL);
+    mMaxVarNameSize = VarNameSize;
+  }
+  AsciiStrToUnicodeStrS ((CHAR8 *) IfrEfiVarStore->Name, mVarName, mMaxVarNameSize / sizeof (CHAR16));
+  VarName = mVarName;
+
+  HiiVariableNode = FindHiiVariableNode (
+                      VarName,
+                      &IfrEfiVarStore->Guid
+                      );
+  if (HiiVariableNode == NULL) {
+    //
+    // Not found, then create new.
+    //
+    HeaderLength = sizeof (*HiiVariable) + VarNameSize;
+    HiiVariable = InternalVarCheckAllocateZeroPool (HeaderLength);
+    ASSERT (HiiVariable != NULL);
+    HiiVariable->Revision = VAR_CHECK_HII_REVISION;
+    HiiVariable->OpCode = EFI_IFR_VARSTORE_EFI_OP;
+    HiiVariable->HeaderLength = (UINT16) HeaderLength;
+    HiiVariable->Size = IfrEfiVarStore->Size;
+    HiiVariable->Attributes = IfrEfiVarStore->Attributes;
+    CopyGuid (&HiiVariable->Guid, &IfrEfiVarStore->Guid);
+    StrCpyS ((CHAR16 *) (HiiVariable + 1), VarNameSize / sizeof (CHAR16), VarName);
+
+    HiiVariableNode = InternalVarCheckAllocateZeroPool (sizeof (*HiiVariableNode));
+    ASSERT (HiiVariableNode != NULL);
+    HiiVariableNode->Signature = VAR_CHECK_HII_VARIABLE_NODE_SIGNATURE;
+    HiiVariableNode->HiiVariable = HiiVariable;
+    //
+    // The variable store identifier, which is unique within the current form set.
+    //
+    HiiVariableNode->VarStoreId = IfrEfiVarStore->VarStoreId;
+    HiiVariableNode->HiiQuestionArray = InternalVarCheckAllocateZeroPool (IfrEfiVarStore->Size * sizeof (VAR_CHECK_HII_QUESTION_HEADER *));
+
+    InsertTailList (&mVarCheckHiiList, &HiiVariableNode->Link);
+  } else {
+    HiiVariableNode->VarStoreId = IfrEfiVarStore->VarStoreId;
+  }
+}
+
+/**
+  Parse and create Hii Variable node list.
+
+  @param[in] HiiPackage         Pointer to Hii Package.
+
+**/
+VOID
+ParseHiiVariable (
+  IN VOID       *HiiPackage
+  )
+{
+  EFI_HII_PACKAGE_HEADER    *HiiPackageHeader;
+  EFI_IFR_OP_HEADER         *IfrOpCodeHeader;
+  EFI_IFR_VARSTORE_EFI      *IfrEfiVarStore;
+
+  HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) HiiPackage;
+
+  switch (HiiPackageHeader->Type) {
+    case EFI_HII_PACKAGE_FORMS:
+      IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) (HiiPackageHeader + 1);
+
+      while ((UINTN) IfrOpCodeHeader < (UINTN) HiiPackageHeader + HiiPackageHeader->Length) {
+        switch (IfrOpCodeHeader->OpCode) {
+          case EFI_IFR_VARSTORE_EFI_OP:
+            //
+            // Come to EFI VARSTORE in Form Package.
+            //
+            IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpCodeHeader;
+            if ((IfrEfiVarStore->Header.Length >= sizeof (EFI_IFR_VARSTORE_EFI)) &&
+                ((IfrEfiVarStore->Attributes & EFI_VARIABLE_NON_VOLATILE) != 0)) {
+              //
+              // Only create node list for Hii Variable with NV attribute.
+              //
+              CreateHiiVariableNode (IfrEfiVarStore);
+            }
+            break;
+
+          default:
+            break;
+        }
+      IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+      }
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  Var Check Parse Hii Package.
+
+  @param[in] HiiPackage         Pointer to Hii Package.
+  @param[in] FromFv             Hii Package from FV.
+
+**/
+VOID
+VarCheckParseHiiPackage (
+  IN VOID       *HiiPackage,
+  IN BOOLEAN    FromFv
+  )
+{
+  EFI_HII_PACKAGE_HEADER        *HiiPackageHeader;
+  EFI_IFR_OP_HEADER             *IfrOpCodeHeader;
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+
+  //
+  // Parse and create Hii Variable node list for this Hii Package.
+  //
+  ParseHiiVariable (HiiPackage);
+
+  HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) HiiPackage;
+
+  switch (HiiPackageHeader->Type) {
+    case EFI_HII_PACKAGE_FORMS:
+      IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) (HiiPackageHeader + 1);
+
+      while ((UINTN) IfrOpCodeHeader < (UINTN) HiiPackageHeader + HiiPackageHeader->Length) {
+        switch (IfrOpCodeHeader->OpCode) {
+          case EFI_IFR_ONE_OF_OP:
+          case EFI_IFR_CHECKBOX_OP:
+          case EFI_IFR_NUMERIC_OP:
+          case EFI_IFR_ORDERED_LIST_OP:
+            HiiVariableNode = FindHiiVariableNodeByVarStoreId (((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.VarStoreId);
+            if ((HiiVariableNode == NULL) ||
+                //
+                // No related Hii Variable node found.
+                //
+                ((((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.Header.Prompt == 0) && (((EFI_IFR_ONE_OF *) IfrOpCodeHeader)->Question.Header.Help == 0))) {
+                //
+                // meanless IFR item introduced by ECP.
+                //
+            } else {
+              //
+              // Normal IFR
+              //
+              ParseHiiQuestion (HiiVariableNode, IfrOpCodeHeader, FromFv);
+            }
+          default:
+            break;
+        }
+        IfrOpCodeHeader = (EFI_IFR_OP_HEADER *) ((UINTN) IfrOpCodeHeader + IfrOpCodeHeader->Length);
+      }
+      break;
+
+    default:
+      break;
+  }
+  DestroyVarStoreId ();
+}
+
+/**
+  Var Check Parse Hii Database.
+
+  @param[in] HiiDatabase        Pointer to Hii Database.
+  @param[in] HiiDatabaseSize    Hii Database size.
+
+**/
+VOID
+VarCheckParseHiiDatabase (
+  IN VOID       *HiiDatabase,
+  IN UINTN      HiiDatabaseSize
+  )
+{
+  EFI_HII_PACKAGE_LIST_HEADER   *HiiPackageListHeader;
+  EFI_HII_PACKAGE_HEADER        *HiiPackageHeader;
+
+  HiiPackageListHeader = (EFI_HII_PACKAGE_LIST_HEADER *) HiiDatabase;
+
+  while ((UINTN) HiiPackageListHeader < ((UINTN) HiiDatabase + HiiDatabaseSize)) {
+    HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) (HiiPackageListHeader + 1);
+
+    while ((UINTN) HiiPackageHeader < ((UINTN) HiiPackageListHeader + HiiPackageListHeader->PackageLength)) {
+      //
+      // Parse Hii Pacakge.
+      //
+      VarCheckParseHiiPackage (HiiPackageHeader, FALSE);
+
+      HiiPackageHeader = (EFI_HII_PACKAGE_HEADER *) ((UINTN) HiiPackageHeader + HiiPackageHeader->Length);
+    }
+
+    HiiPackageListHeader = (EFI_HII_PACKAGE_LIST_HEADER *) ((UINTN) HiiPackageListHeader + HiiPackageListHeader->PackageLength);
+  }
+}
+
+/**
+  Destroy Hii Variable node.
+
+**/
+VOID
+DestroyHiiVariableNode (
+  VOID
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  LIST_ENTRY                    *HiiVariableLink;
+  UINTN                         Index;
+
+  while (mVarCheckHiiList.ForwardLink != &mVarCheckHiiList) {
+    HiiVariableLink = mVarCheckHiiList.ForwardLink;
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (HiiVariableLink);
+
+    RemoveEntryList (&HiiVariableNode->Link);
+
+    //
+    // Free the allocated buffer.
+    //
+    for (Index = 0; Index < HiiVariableNode->HiiVariable->Size; Index++) {
+      if (HiiVariableNode->HiiQuestionArray[Index] != NULL) {
+        InternalVarCheckFreePool (HiiVariableNode->HiiQuestionArray[Index]);
+      }
+    }
+    InternalVarCheckFreePool (HiiVariableNode->HiiQuestionArray);
+    InternalVarCheckFreePool (HiiVariableNode->HiiVariable);
+    InternalVarCheckFreePool (HiiVariableNode);
+  }
+}
+
+/**
+  Build VarCheckHiiBin.
+
+  @param[out] Size      Pointer to VarCheckHii size.
+
+  @return Pointer to VarCheckHiiBin.
+
+**/
+VOID *
+BuildVarCheckHiiBin (
+  OUT UINTN  *Size
+  )
+{
+  VAR_CHECK_HII_VARIABLE_NODE   *HiiVariableNode;
+  LIST_ENTRY                    *HiiVariableLink;
+  UINTN                         Index;
+  VOID                          *Data;
+  UINT8                         *Ptr;
+  UINT32                        BinSize;
+  UINT32                        HiiVariableLength;
+
+  //
+  // Get Size
+  //
+  BinSize = 0;
+
+  for (HiiVariableLink = mVarCheckHiiList.ForwardLink
+      ;HiiVariableLink != &mVarCheckHiiList
+      ;HiiVariableLink = HiiVariableLink->ForwardLink) {
+    //
+    // For Hii Variable header align.
+    //
+    BinSize = (UINT32) HEADER_ALIGN (BinSize);
+
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (HiiVariableLink);
+    HiiVariableLength = HiiVariableNode->HiiVariable->HeaderLength;
+
+    for (Index = 0; Index < HiiVariableNode->HiiVariable->Size; Index++) {
+      if (HiiVariableNode->HiiQuestionArray[Index] != NULL) {
+        //
+        // For Hii Question header align.
+        //
+        HiiVariableLength = (UINT32) HEADER_ALIGN (HiiVariableLength);
+        HiiVariableLength += HiiVariableNode->HiiQuestionArray[Index]->Length;
+      }
+    }
+
+    HiiVariableNode->HiiVariable->Length = HiiVariableLength;
+    BinSize += HiiVariableLength;
+  }
+
+  DEBUG ((EFI_D_INFO, "VarCheckHiiBin - size = 0x%x\n", BinSize));
+  if (BinSize == 0) {
+    *Size = BinSize;
+    return NULL;
+  }
+
+  //
+  // AllocatePages () and AllocatePool () from gBS are used for the process of VarCheckHiiBin generation.
+  // Only here AllocateRuntimeZeroPool () from MemoryAllocateLib is used for runtime access
+  // in SetVariable check handler.
+  //
+  Data = AllocateRuntimeZeroPool (BinSize);
+  ASSERT (Data != NULL);
+  DEBUG ((EFI_D_INFO, "VarCheckHiiBin - built at 0x%x\n", Data));
+
+  //
+  // Gen Data
+  //
+  Ptr = Data;
+  for (HiiVariableLink = mVarCheckHiiList.ForwardLink
+      ;HiiVariableLink != &mVarCheckHiiList
+      ;HiiVariableLink = HiiVariableLink->ForwardLink) {
+    //
+    // For Hii Variable header align.
+    //
+    Ptr = (UINT8 *) HEADER_ALIGN (Ptr);
+
+    HiiVariableNode = VAR_CHECK_HII_VARIABLE_FROM_LINK (HiiVariableLink);
+    CopyMem (Ptr, HiiVariableNode->HiiVariable, HiiVariableNode->HiiVariable->HeaderLength);
+    Ptr += HiiVariableNode->HiiVariable->HeaderLength;
+
+    for (Index = 0; Index < HiiVariableNode->HiiVariable->Size; Index++) {
+      if (HiiVariableNode->HiiQuestionArray[Index] != NULL) {
+        //
+        // For Hii Question header align.
+        //
+        Ptr = (UINT8 *) HEADER_ALIGN (Ptr);
+        CopyMem (Ptr, HiiVariableNode->HiiQuestionArray[Index], HiiVariableNode->HiiQuestionArray[Index]->Length);
+        Ptr += HiiVariableNode->HiiQuestionArray[Index]->Length;
+      }
+    }
+  }
+
+  *Size = BinSize;
+  return Data;
+}
+
+/**
+  Generate VarCheckHiiBin from Hii Database and FV.
+
+**/
+VOID
+EFIAPI
+VarCheckHiiGen (
+  VOID
+  )
+{
+  VarCheckHiiGenFromHiiDatabase ();
+  VarCheckHiiGenFromFv ();
+
+  mVarCheckHiiBin = BuildVarCheckHiiBin (&mVarCheckHiiBinSize);
+  if (mVarCheckHiiBin == NULL) {
+    DEBUG ((EFI_D_INFO, "[VarCheckHii] This driver could be removed from *.dsc and *.fdf\n"));
+    return;
+  }
+
+  DestroyHiiVariableNode ();
+  if (mVarName != NULL) {
+    InternalVarCheckFreePool (mVarName);
+  }
+
+#ifdef DUMP_VAR_CHECK_HII
+  DEBUG_CODE (
+    DumpVarCheckHii (mVarCheckHiiBin, mVarCheckHiiBinSize);
+  );
+#endif
+}
+
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.h b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.h
new file mode 100644
index 0000000000..f81be2ea88
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGen.h
@@ -0,0 +1,136 @@
+/** @file
+  Include file for Var Check Hii bin generation.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VAR_CHECK_HII_GEN_H_
+#define _VAR_CHECK_HII_GEN_H_
+
+#include "VarCheckHii.h"
+
+/**
+  Dump Hii Package.
+
+  @param[in] HiiPackage         Pointer to Hii Package.
+
+**/
+VOID
+DumpHiiPackage (
+  IN VOID       *HiiPackage
+  );
+
+/**
+  Dump Hii Database.
+
+  @param[in] HiiDatabase        Pointer to Hii Database.
+  @param[in] HiiDatabaseSize    Hii Database size.
+
+**/
+VOID
+DumpHiiDatabase (
+  IN VOID       *HiiDatabase,
+  IN UINTN      HiiDatabaseSize
+  );
+
+/**
+  Allocates and zeros a buffer of type EfiBootServicesData.
+
+  Allocates the number bytes specified by AllocationSize of type EfiBootServicesData, clears the
+  buffer with zeros, and returns a pointer to the allocated buffer.  If AllocationSize is 0, then a
+  valid buffer of 0 size is returned.  If there is not enough memory remaining to satisfy the
+  request, then NULL is returned.
+
+  @param  AllocationSize        The number of bytes to allocate and zero.
+
+  @return A pointer to the allocated buffer or NULL if allocation fails.
+
+**/
+VOID *
+InternalVarCheckAllocateZeroPool (
+  IN UINTN            AllocationSize
+  );
+
+/**
+  Frees a buffer that was previously allocated with one of the pool allocation functions in the
+  Memory Allocation Library.
+
+  Frees the buffer specified by Buffer.  Buffer must have been allocated on a previous call to the
+  pool allocation services of the Memory Allocation Library.  If it is not possible to free pool
+  resources, then this function will perform no actions.
+
+  If Buffer was not allocated with a pool allocation function in the Memory Allocation Library,
+  then ASSERT().
+
+  @param  Buffer                The pointer to the buffer to free.
+
+**/
+VOID
+EFIAPI
+InternalVarCheckFreePool (
+  IN VOID   *Buffer
+  );
+
+/**
+  Var Check Parse Hii Package.
+
+  @param[in] HiiPackage         Pointer to Hii Package.
+  @param[in] FromFv             Hii Package from FV.
+
+**/
+VOID
+VarCheckParseHiiPackage (
+  IN VOID       *HiiPackage,
+  IN BOOLEAN    FromFv
+  );
+
+/**
+  Var Check Parse Hii Database.
+
+  @param[in] HiiDatabase        Pointer to Hii Database.
+  @param[in] HiiDatabaseSize    Hii Database size.
+
+**/
+VOID
+VarCheckParseHiiDatabase (
+  IN VOID       *HiiDatabase,
+  IN UINTN      HiiDatabaseSize
+  );
+
+/**
+  Generate from FV.
+
+**/
+VOID
+VarCheckHiiGenFromFv (
+  VOID
+  );
+
+/**
+  Generate from Hii Database.
+
+**/
+VOID
+VarCheckHiiGenFromHiiDatabase (
+  VOID
+  );
+
+/**
+  Generate VarCheckHiiBin from Hii Database and FV.
+
+**/
+VOID
+EFIAPI
+VarCheckHiiGen (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromFv.c b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromFv.c
new file mode 100644
index 0000000000..21fc80e89c
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromFv.c
@@ -0,0 +1,443 @@
+/** @file
+  Var Check Hii generation from FV.
+
+Copyright (c) 2015, 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 "VarCheckHiiGen.h"
+
+// {d0bc7cb4-6a47-495f-aa11-710746da06a2}
+#define EFI_VFR_ATTRACT_GUID \
+{ 0xd0bc7cb4, 0x6a47, 0x495f, { 0xaa, 0x11, 0x71, 0x7, 0x46, 0xda, 0x6, 0xa2 } }
+
+EFI_GUID  gVfrArrayAttractGuid  = EFI_VFR_ATTRACT_GUID;
+
+#define ALL_FF_GUID \
+{ 0xFFFFFFFF, 0xFFFF, 0xFFFF, { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF } }
+
+EFI_GUID mAllFfGuid             = ALL_FF_GUID;
+
+#define VAR_CHECK_VFR_DRIVER_INFO_SIGNATURE     SIGNATURE_32 ('V', 'D', 'R', 'I')
+
+typedef struct {
+  UINTN             Signature;
+  LIST_ENTRY        Link;
+  EFI_GUID          *DriverGuid;
+} VAR_CHECK_VFR_DRIVER_INFO;
+
+LIST_ENTRY                      mVfrDriverList = INITIALIZE_LIST_HEAD_VARIABLE (mVfrDriverList);
+
+#define VAR_CHECK_VFR_DRIVER_INFO_FROM_LINK(a)  CR (a, VAR_CHECK_VFR_DRIVER_INFO, Link, VAR_CHECK_VFR_DRIVER_INFO_SIGNATURE)
+
+#define MAX_MATCH_GUID_NUM      100
+
+/**
+  Get the address by Guid.
+
+  Parse the FFS and find the GUID address.
+  There may be multiple Guids matching the searched Guid.
+
+  @param Ffs                Pointer to the FFS.
+  @param Guid               Guid to find.
+  @param Length             The length of FFS.
+  @param Offset             Pointer to pointer to the offset.
+  @param NumOfMatchingGuid  The number of matching Guid.
+
+  @retval EFI_SUCCESS       One or multiple Guids matching the searched Guid.
+  @retval EFI_NOT_FOUND     No Guid matching the searched Guid.
+
+**/
+EFI_STATUS
+GetAddressByGuid (
+  IN  VOID          *Ffs,
+  IN  EFI_GUID      *Guid,
+  IN  UINTN         Length,
+  OUT UINTN         **Offset,
+  OUT UINT8         *NumOfMatchingGuid
+  )
+{
+  UINTN     LoopControl;
+  BOOLEAN   Found;
+
+  if((Ffs == NULL) || (Guid == NULL) || (Length == 0)){
+    return EFI_NOT_FOUND;
+  }
+
+  if (NumOfMatchingGuid != NULL) {
+    *NumOfMatchingGuid = 0;
+  }
+
+  Found = FALSE;
+  for (LoopControl = 0; LoopControl < Length; LoopControl++) {
+    if (CompareGuid (Guid, (EFI_GUID *) ((UINT8 *) Ffs + LoopControl))) {
+      Found = TRUE;
+      //
+      // If NumOfMatchGuid or Offset are NULL, means user only want
+      // to check whether current FFS includes this Guid or not.
+      //
+      if ((NumOfMatchingGuid != NULL) && (Offset != NULL)) {
+        if (*NumOfMatchingGuid == 0) {
+          *Offset = InternalVarCheckAllocateZeroPool (sizeof (UINTN) * MAX_MATCH_GUID_NUM);
+          ASSERT (*Offset != NULL);
+        }
+        *(*Offset + *NumOfMatchingGuid) = LoopControl + sizeof (EFI_GUID);
+        (*NumOfMatchingGuid)++;
+      } else {
+        break;
+      }
+    }
+  }
+
+  return (Found ? EFI_SUCCESS : EFI_NOT_FOUND);
+}
+
+/**
+  Search the VfrBin Base address.
+
+  According to the known GUID gVfrArrayAttractGuid to get the base address from FFS.
+
+  @param Ffs                    Pointer to the FFS.
+  @param EfiAddr                Pointer to the EFI in FFS
+  @param Length                 The length of FFS.
+  @param Offset                 Pointer to pointer to the Addr (Offset).
+  @param NumOfMatchingOffset    The number of Addr (Offset).
+
+  @retval EFI_SUCCESS           Get the address successfully.
+  @retval EFI_NOT_FOUND         No VfrBin found.
+
+**/
+EFI_STATUS
+SearchVfrBinInFfs (
+   IN  VOID      *Ffs,
+   IN  VOID      *EfiAddr,
+   IN  UINTN     Length,
+   OUT UINTN     **Offset,
+   OUT UINT8     *NumOfMatchingOffset
+  )
+{
+  UINTN        Index;
+  EFI_STATUS   Status;
+  UINTN        VirOffValue;
+
+  if ((Ffs == NULL) || (Offset == NULL)) {
+    return EFI_NOT_FOUND;
+  }
+  Status = GetAddressByGuid (
+             Ffs,
+             &gVfrArrayAttractGuid,
+             Length,
+             Offset,
+             NumOfMatchingOffset
+             );
+  if (Status != EFI_SUCCESS) {
+    return Status;
+  }
+
+  for (Index = 0; Index < *NumOfMatchingOffset; Index++) {
+    //
+    // Got the virOffset after the GUID
+    //
+    VirOffValue = *(UINTN *) ((UINTN) Ffs + *(*Offset + Index));
+    //
+    // Transfer the offset to the VA address. One modules may own multiple VfrBin address.
+    //
+    *(*Offset + Index) = (UINTN) EfiAddr + VirOffValue;
+  }
+
+  return Status;
+}
+
+/**
+  Parse FFS.
+
+  @param[in] Fv2            Pointer to Fv2 protocol.
+  @param[in] DriverGuid     Pointer to driver GUID.
+
+  @return Found the driver in the FV or not.
+
+**/
+BOOLEAN
+ParseFfs (
+  IN EFI_FIRMWARE_VOLUME2_PROTOCOL  *Fv2,
+  IN EFI_GUID                       *DriverGuid
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_FV_FILETYPE               FoundType;
+  EFI_FV_FILE_ATTRIBUTES        FileAttributes;
+  UINT32                        AuthenticationStatus;
+  UINTN                         Size;
+  VOID                          *Buffer;
+  UINTN                         SectionSize;
+  VOID                          *SectionBuffer;
+  UINTN                         VfrBinIndex;
+  UINT8                         NumberofMatchingVfrBin;
+  UINTN                         *VfrBinBaseAddress;
+
+  Status = Fv2->ReadFile (
+                  Fv2,
+                  DriverGuid,
+                  NULL,
+                  &Size,
+                  &FoundType,
+                  &FileAttributes,
+                  &AuthenticationStatus
+                  );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  Buffer = NULL;
+  Status = Fv2->ReadSection (
+                  Fv2,
+                  DriverGuid,
+                  EFI_SECTION_RAW,
+                  0, // Instance
+                  &Buffer,
+                  &Size,
+                  &AuthenticationStatus
+                  );
+   if (!EFI_ERROR (Status)) {
+     Status = SearchVfrBinInFfs (Buffer, 0, Size, &VfrBinBaseAddress, &NumberofMatchingVfrBin);
+     if (!EFI_ERROR (Status)) {
+        SectionBuffer = NULL;
+        Status = Fv2->ReadSection (
+                        Fv2,
+                        DriverGuid,
+                        EFI_SECTION_PE32,
+                        0, // Instance
+                        &SectionBuffer,
+                        &SectionSize,
+                        &AuthenticationStatus
+                        );
+        if (!EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_INFO, "FfsNameGuid - %g\n", DriverGuid));
+          DEBUG ((EFI_D_INFO, "NumberofMatchingVfrBin - 0x%02x\n", NumberofMatchingVfrBin));
+
+          for (VfrBinIndex = 0; VfrBinIndex < NumberofMatchingVfrBin; VfrBinIndex++) {
+#ifdef DUMP_HII_DATA
+            DEBUG_CODE (
+              DumpHiiPackage ((UINT8 *) (UINTN) SectionBuffer + VfrBinBaseAddress[VfrBinIndex] + sizeof (UINT32));
+              );
+#endif
+            VarCheckParseHiiPackage ((UINT8 *) (UINTN) SectionBuffer + VfrBinBaseAddress[VfrBinIndex] + sizeof (UINT32), TRUE);
+          }
+
+          FreePool (SectionBuffer);
+        }
+
+        InternalVarCheckFreePool (VfrBinBaseAddress);
+      }
+
+      FreePool (Buffer);
+    }
+
+   return TRUE;
+}
+
+/**
+  Parse FVs.
+
+  @param[in] ScanAll    Scan all modules in all FVs or not.
+
+**/
+VOID
+ParseFv (
+  IN BOOLEAN    ScanAll
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    *HandleBuffer;
+  UINTN                         HandleCount;
+  UINTN                         Index;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *Fv2;
+  VOID                          *Key;
+  EFI_FV_FILETYPE               FileType;
+  EFI_GUID                      NameGuid;
+  EFI_FV_FILE_ATTRIBUTES        FileAttributes;
+  UINTN                         Size;
+  UINTN                         FfsIndex;
+  VAR_CHECK_VFR_DRIVER_INFO     *VfrDriverInfo;
+  LIST_ENTRY                    *VfrDriverLink;
+
+  HandleBuffer = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Search all FVs
+  //
+  for (Index = 0; Index < HandleCount; Index++) {
+    DEBUG ((EFI_D_INFO, "FvIndex - %x\n", Index));
+    Status = gBS->HandleProtocol (
+                    HandleBuffer[Index],
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    (VOID **) &Fv2
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    DEBUG_CODE (
+      EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL   *Fvb2;
+      EFI_PHYSICAL_ADDRESS                  FvAddress;
+      UINT64                                FvSize;
+
+      Status = gBS->HandleProtocol (
+                      HandleBuffer[Index],
+                      &gEfiFirmwareVolumeBlock2ProtocolGuid,
+                      (VOID **) &Fvb2
+                      );
+      ASSERT_EFI_ERROR (Status);
+      Status = Fvb2->GetPhysicalAddress (Fvb2, &FvAddress);
+      if (!EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_INFO, "FvAddress - 0x%08x\n", FvAddress));
+        FvSize = ((EFI_FIRMWARE_VOLUME_HEADER *) (UINTN) FvAddress)->FvLength;
+        DEBUG ((EFI_D_INFO, "FvSize    - 0x%08x\n", FvSize));
+      }
+    );
+
+    if (ScanAll) {
+      //
+      // Need to parse all modules in all FVs.
+      //
+      Key = InternalVarCheckAllocateZeroPool (Fv2->KeySize);
+      ASSERT (Key != NULL);
+
+      for (FfsIndex = 0; ; FfsIndex++) {
+        FileType = EFI_FV_FILETYPE_ALL;
+        Status = Fv2->GetNextFile (
+                        Fv2,
+                        Key,
+                        &FileType,
+                        &NameGuid,
+                        &FileAttributes,
+                        &Size
+                      );
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+
+        ParseFfs (Fv2, &NameGuid);
+      }
+
+      InternalVarCheckFreePool (Key);
+    } else {
+      //
+      // Only parse drivers in the VFR drivers list.
+      //
+      VfrDriverLink = mVfrDriverList.ForwardLink;
+      while (VfrDriverLink != &mVfrDriverList) {
+        VfrDriverInfo = VAR_CHECK_VFR_DRIVER_INFO_FROM_LINK (VfrDriverLink);
+        VfrDriverLink = VfrDriverLink->ForwardLink;
+        if (ParseFfs (Fv2, VfrDriverInfo->DriverGuid)) {
+          //
+          // Found the driver in the FV.
+          //
+          RemoveEntryList (&VfrDriverInfo->Link);
+          InternalVarCheckFreePool (VfrDriverInfo);
+        }
+      }
+    }
+  }
+
+  FreePool (HandleBuffer);
+}
+
+/**
+  Create Vfr Driver List.
+
+  @param[in] DriverGuidArray    Driver Guid Array
+
+**/
+VOID
+CreateVfrDriverList (
+  IN EFI_GUID   *DriverGuidArray
+  )
+{
+  UINTN                         Index;
+  VAR_CHECK_VFR_DRIVER_INFO     *VfrDriverInfo;
+
+  for (Index = 0; !CompareGuid (&DriverGuidArray[Index], &gZeroGuid); Index++) {
+     DEBUG ((EFI_D_INFO, "CreateVfrDriverList: %g\n", &DriverGuidArray[Index]));
+     VfrDriverInfo = InternalVarCheckAllocateZeroPool (sizeof (*VfrDriverInfo));
+     ASSERT (VfrDriverInfo != NULL);
+     VfrDriverInfo->Signature = VAR_CHECK_VFR_DRIVER_INFO_SIGNATURE;
+     VfrDriverInfo->DriverGuid = &DriverGuidArray[Index];
+     InsertTailList (&mVfrDriverList, &VfrDriverInfo->Link);
+  }
+}
+
+/**
+  Destroy Vfr Driver List.
+
+**/
+VOID
+DestroyVfrDriverList (
+  VOID
+  )
+{
+  VAR_CHECK_VFR_DRIVER_INFO     *VfrDriverInfo;
+  LIST_ENTRY                    *VfrDriverLink;
+
+  while (mVfrDriverList.ForwardLink != &mVfrDriverList) {
+    VfrDriverLink = mVfrDriverList.ForwardLink;
+    VfrDriverInfo = VAR_CHECK_VFR_DRIVER_INFO_FROM_LINK (VfrDriverLink);
+    RemoveEntryList (&VfrDriverInfo->Link);
+    InternalVarCheckFreePool (VfrDriverInfo);
+  }
+}
+
+/**
+  Generate from FV.
+
+**/
+VOID
+VarCheckHiiGenFromFv (
+  VOID
+  )
+{
+  EFI_GUID      *DriverGuidArray;
+  BOOLEAN       ScanAll;
+
+  DEBUG ((EFI_D_INFO, "VarCheckHiiGenDxeFromFv\n"));
+
+  //
+  // Get vfr driver guid array from PCD.
+  //
+  DriverGuidArray = (EFI_GUID *) PcdGetPtr (PcdVarCheckVfrDriverGuidArray);
+
+  if (CompareGuid (&DriverGuidArray[0], &gZeroGuid)) {
+    //
+    // No VFR driver will be parsed from FVs.
+    //
+    return;
+  }
+
+  if (CompareGuid (&DriverGuidArray[0], &mAllFfGuid)) {
+    ScanAll = TRUE;
+  } else {
+    ScanAll = FALSE;
+    CreateVfrDriverList (DriverGuidArray);
+  }
+
+  ParseFv (ScanAll);
+
+  if (!ScanAll) {
+    DestroyVfrDriverList ();
+  }
+}
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromHii.c b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromHii.c
new file mode 100644
index 0000000000..41cde34af7
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiGenFromHii.c
@@ -0,0 +1,73 @@
+/** @file
+  Var Check Hii generation from Hii Database.
+
+Copyright (c) 2015, 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 "VarCheckHiiGen.h"
+
+/**
+  Generate from Hii Database.
+
+**/
+VOID
+VarCheckHiiGenFromHiiDatabase (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         BufferSize;
+  VOID                          *Buffer;
+  EFI_PHYSICAL_ADDRESS          BufferAddress;
+  EFI_HII_DATABASE_PROTOCOL     *HiiDatabase;
+
+  //
+  // Locate HII Database protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiDatabaseProtocolGuid, NULL, (VOID **) &HiiDatabase);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Call first time with zero buffer length.
+  // Should fail with EFI_BUFFER_TOO_SMALL.
+  //
+  BufferSize = 0;
+  Buffer = NULL;
+  Status = HiiDatabase->ExportPackageLists (HiiDatabase, 0, &BufferSize, Buffer);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // Allocate buffer to hold the HII Database.
+    //
+    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData, EFI_SIZE_TO_PAGES (BufferSize), &BufferAddress);
+    ASSERT_EFI_ERROR (Status);
+    Buffer = (VOID *) (UINTN) BufferAddress;
+
+    //
+    // Export HII Database into the buffer.
+    //
+    Status = HiiDatabase->ExportPackageLists (HiiDatabase, 0, &BufferSize, Buffer);
+    ASSERT_EFI_ERROR (Status);
+
+    DEBUG ((EFI_D_INFO, "VarCheckHiiGenDxeFromHii - HII Database exported at 0x%x, size = 0x%x\n", Buffer, BufferSize));
+
+#ifdef DUMP_HII_DATA
+    DEBUG_CODE (
+      DumpHiiDatabase (Buffer, BufferSize);
+      );
+#endif
+
+    VarCheckParseHiiDatabase (Buffer, BufferSize);
+
+    gBS->FreePages (BufferAddress, EFI_SIZE_TO_PAGES (BufferSize));
+  }
+}
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf
new file mode 100644
index 0000000000..24d24c5dd5
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf
@@ -0,0 +1,58 @@
+## @file
+#  NULL class library to register var check HII handler.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VarCheckHiiLib
+  MODULE_UNI_FILE                = VarCheckHiiLib.uni
+  FILE_GUID                      = A34FBDD0-05D3-4AF7-A720-560E91AC8CDF
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|DXE_RUNTIME_DRIVER DXE_SMM_DRIVER
+  CONSTRUCTOR                    = VarCheckHiiLibNullClassConstructor
+
+[Sources]
+  VarCheckHiiLibNullClass.c
+  VarCheckHii.h
+  VarCheckHiiGenFromFv.c
+  VarCheckHiiGenFromHii.c
+  VarCheckHiiGen.c
+  VarCheckHiiGen.h
+  InternalVarCheckStructure.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  PcdLib
+  VarCheckLib
+
+[Guids]
+  gZeroGuid                                 ## SOMETIMES_CONSUMES   ## GUID
+
+[Protocols]
+  gEfiFirmwareVolume2ProtocolGuid           ## SOMETIMES_CONSUMES
+  gEfiFirmwareVolumeBlock2ProtocolGuid      ## SOMETIMES_CONSUMES
+  gEfiHiiDatabaseProtocolGuid               ## SOMETIMES_CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVarCheckVfrDriverGuidArray   ## SOMETIMES_CONSUMES
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.uni b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.uni
new file mode 100644
index 0000000000..e4a0debafa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.uni
diff --git a/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLibNullClass.c b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLibNullClass.c
new file mode 100644
index 0000000000..3afa7962aa
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLibNullClass.c
@@ -0,0 +1,539 @@
+/** @file
+  Var Check Hii handler.
+
+Copyright (c) 2015, 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 "VarCheckHii.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 mVarCheckHiiHex[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
+
+/**
+  Dump some hexadecimal data.
+
+  @param[in] Indent     How many spaces to indent the output.
+  @param[in] Offset     The offset of the dump.
+  @param[in] DataSize   The size in bytes of UserData.
+  @param[in] UserData   The data to dump.
+
+**/
+VOID
+VarCheckHiiInternalDumpHex (
+  IN UINTN        Indent,
+  IN UINTN        Offset,
+  IN UINTN        DataSize,
+  IN VOID         *UserData
+  )
+{
+  UINT8 *Data;
+
+  CHAR8 Val[50];
+
+  CHAR8 Str[20];
+
+  UINT8 TempByte;
+  UINTN Size;
+  UINTN Index;
+
+  Data = UserData;
+  while (DataSize != 0) {
+    Size = 16;
+    if (Size > DataSize) {
+      Size = DataSize;
+    }
+
+    for (Index = 0; Index < Size; Index += 1) {
+      TempByte            = Data[Index];
+      Val[Index * 3 + 0]  = mVarCheckHiiHex[TempByte >> 4];
+      Val[Index * 3 + 1]  = mVarCheckHiiHex[TempByte & 0xF];
+      Val[Index * 3 + 2]  = (CHAR8) ((Index == 7) ? '-' : ' ');
+      Str[Index]          = (CHAR8) ((TempByte < ' ' || TempByte > 'z') ? '.' : TempByte);
+    }
+
+    Val[Index * 3]  = 0;
+    Str[Index]      = 0;
+    DEBUG ((EFI_D_INFO, "%*a%08X: %-48a *%a*\r\n", Indent, "", Offset, Val, Str));
+
+    Data += Size;
+    Offset += Size;
+    DataSize -= Size;
+  }
+}
+
+/**
+  Var Check Hii Question.
+
+  @param[in] HiiQuestion    Pointer to Hii Question
+  @param[in] Data           Data pointer.
+  @param[in] DataSize       Size of Data to set.
+
+  @retval TRUE  Check pass
+  @retval FALSE Check fail.
+
+**/
+BOOLEAN
+VarCheckHiiQuestion (
+  IN VAR_CHECK_HII_QUESTION_HEADER  *HiiQuestion,
+  IN VOID                           *Data,
+  IN UINTN                          DataSize
+  )
+{
+  UINT64   OneData;
+  UINT64   Minimum;
+  UINT64   Maximum;
+  UINT64   OneValue;
+  UINT8    *Ptr;
+  UINT8    Index;
+  UINT8    MaxContainers;
+
+  if ((UINTN) (HiiQuestion->VarOffset + HiiQuestion->StorageWidth) > DataSize) {
+    DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: (VarOffset(0x%04x) + StorageWidth(0x%02x)) > Size(0x%x)\n", HiiQuestion->VarOffset, HiiQuestion->StorageWidth, DataSize));
+    return FALSE;
+  }
+
+  OneData = 0;
+  CopyMem (&OneData, (UINT8 *) Data + HiiQuestion->VarOffset, HiiQuestion->StorageWidth);
+
+  switch (HiiQuestion->OpCode) {
+    case EFI_IFR_ONE_OF_OP:
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion + 1);
+      while ((UINTN) Ptr < (UINTN) HiiQuestion + HiiQuestion->Length) {
+        OneValue = 0;
+        CopyMem (&OneValue, Ptr, HiiQuestion->StorageWidth);
+        if (OneData == OneValue) {
+          //
+          // Match
+          //
+          break;
+        }
+        Ptr += HiiQuestion->StorageWidth;
+      }
+      if ((UINTN) Ptr >= ((UINTN) HiiQuestion + HiiQuestion->Length)) {
+        //
+        // No match
+        //
+        DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: OneOf mismatch (0x%lx)\n", OneData));
+        DEBUG_CODE (VarCheckHiiInternalDumpHex (2, 0, HiiQuestion->Length, (UINT8 *) HiiQuestion););
+        return FALSE;
+      }
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      if ((OneData != 0) && (OneData != 1)) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: CheckBox mismatch (0x%lx)\n", OneData));
+        DEBUG_CODE (VarCheckHiiInternalDumpHex (2, 0, HiiQuestion->Length, (UINT8 *) HiiQuestion););
+        return FALSE;
+      }
+      break;
+
+    case EFI_IFR_NUMERIC_OP:
+      Minimum = 0;
+      Maximum = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_NUMERIC *) HiiQuestion + 1);
+      CopyMem (&Minimum, Ptr, HiiQuestion->StorageWidth);
+      Ptr += HiiQuestion->StorageWidth;
+      CopyMem (&Maximum, Ptr, HiiQuestion->StorageWidth);
+      Ptr += HiiQuestion->StorageWidth;
+
+      //
+      // No need to check Step, because it is ONLY for UI.
+      //
+      if ((OneData < Minimum) || (OneData > Maximum)) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: Numeric mismatch (0x%lx)\n", OneData));
+        DEBUG_CODE (VarCheckHiiInternalDumpHex (2, 0, HiiQuestion->Length, (UINT8 *) HiiQuestion););
+        return FALSE;
+      }
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      MaxContainers = ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion)->MaxContainers;
+      if ((UINTN) (HiiQuestion->VarOffset + HiiQuestion->StorageWidth * MaxContainers) > DataSize) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: (VarOffset(0x%04x) + StorageWidth(0x%02x) * MaxContainers(0x%02x)) > Size(0x%x)\n", HiiQuestion->VarOffset, HiiQuestion->StorageWidth, MaxContainers, DataSize));
+        return FALSE;
+      }
+      for (Index = 0; Index < MaxContainers; Index++) {
+        OneData = 0;
+        CopyMem (&OneData, (UINT8 *) Data + HiiQuestion->VarOffset + HiiQuestion->StorageWidth * Index, HiiQuestion->StorageWidth);
+        if (OneData == 0) {
+          //
+          // The value of 0 is used to determine if a particular "slot" in the array is empty.
+          //
+          continue;
+        }
+
+        Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion + 1);
+        while ((UINTN) Ptr < ((UINTN) HiiQuestion + HiiQuestion->Length)) {
+          OneValue = 0;
+          CopyMem (&OneValue, Ptr, HiiQuestion->StorageWidth);
+          if (OneData == OneValue) {
+            //
+            // Match
+            //
+            break;
+          }
+          Ptr += HiiQuestion->StorageWidth;
+        }
+        if ((UINTN) Ptr >= ((UINTN) HiiQuestion + HiiQuestion->Length)) {
+          //
+          // No match
+          //
+          DEBUG ((EFI_D_INFO, "VarCheckHiiQuestion fail: OrderedList mismatch\n"));
+          DEBUG_CODE (VarCheckHiiInternalDumpHex (2, 0, HiiQuestion->StorageWidth * MaxContainers, (UINT8 *) Data + HiiQuestion->VarOffset););
+          DEBUG_CODE (VarCheckHiiInternalDumpHex (2, 0, HiiQuestion->Length, (UINT8 *) HiiQuestion););
+          return FALSE;
+        }
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+
+  return TRUE;
+}
+
+VAR_CHECK_HII_VARIABLE_HEADER   *mVarCheckHiiBin = NULL;
+UINTN                           mVarCheckHiiBinSize = 0;
+
+/**
+  SetVariable check handler HII.
+
+  @param[in] VariableName       Name of Variable to set.
+  @param[in] VendorGuid         Variable vendor GUID.
+  @param[in] Attributes         Attribute value of the variable.
+  @param[in] DataSize           Size of Data to set.
+  @param[in] Data               Data pointer.
+
+  @retval EFI_SUCCESS               The SetVariable check result was success.
+  @retval EFI_SECURITY_VIOLATION    Check fail.
+
+**/
+EFI_STATUS
+EFIAPI
+SetVariableCheckHandlerHii (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  VAR_CHECK_HII_VARIABLE_HEADER     *HiiVariable;
+  VAR_CHECK_HII_QUESTION_HEADER     *HiiQuestion;
+
+  if (mVarCheckHiiBin == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  if ((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0)) || (Attributes == 0)) {
+    //
+    // Do not check delete variable.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // For Hii Variable header align.
+  //
+  HiiVariable = (VAR_CHECK_HII_VARIABLE_HEADER *) HEADER_ALIGN (mVarCheckHiiBin);
+  while ((UINTN) HiiVariable < ((UINTN) mVarCheckHiiBin + mVarCheckHiiBinSize)) {
+    if ((StrCmp ((CHAR16 *) (HiiVariable + 1), VariableName) == 0) &&
+        (CompareGuid (&HiiVariable->Guid, VendorGuid))) {
+      //
+      // Found the Hii Variable that could be used to do check.
+      //
+      DEBUG ((EFI_D_INFO, "VarCheckHiiVariable - %s:%g with Attributes = 0x%08x Size = 0x%x\n", VariableName, VendorGuid, Attributes, DataSize));
+      if (HiiVariable->Attributes != Attributes) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiVariable fail for Attributes - 0x%08x\n", HiiVariable->Attributes));
+        return EFI_SECURITY_VIOLATION;
+      }
+
+      if (DataSize == 0) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiVariable - CHECK PASS with DataSize == 0 !\n"));
+        return EFI_SUCCESS;
+      }
+
+      if (HiiVariable->Size != DataSize) {
+        DEBUG ((EFI_D_INFO, "VarCheckHiiVariable fail for Size - 0x%x\n", HiiVariable->Size));
+        return EFI_SECURITY_VIOLATION;
+      }
+
+      //
+      // Do the check.
+      // For Hii Question header align.
+      //
+      HiiQuestion = (VAR_CHECK_HII_QUESTION_HEADER *) HEADER_ALIGN (((UINTN) HiiVariable + HiiVariable->HeaderLength));
+      while ((UINTN) HiiQuestion < ((UINTN) HiiVariable + HiiVariable->Length)) {
+        if (!VarCheckHiiQuestion (HiiQuestion, Data, DataSize)) {
+          return EFI_SECURITY_VIOLATION;
+        }
+        //
+        // For Hii Question header align.
+        //
+        HiiQuestion = (VAR_CHECK_HII_QUESTION_HEADER *) HEADER_ALIGN (((UINTN) HiiQuestion + HiiQuestion->Length));
+      }
+
+      DEBUG ((EFI_D_INFO, "VarCheckHiiVariable - ALL CHECK PASS!\n"));
+      return EFI_SUCCESS;
+    }
+    //
+    // For Hii Variable header align.
+    //
+    HiiVariable = (VAR_CHECK_HII_VARIABLE_HEADER *) HEADER_ALIGN (((UINTN) HiiVariable + HiiVariable->Length));
+  }
+
+  // Not found, so pass.
+  return EFI_SUCCESS;
+}
+
+#ifdef DUMP_VAR_CHECK_HII
+GLOBAL_REMOVE_IF_UNREFERENCED VAR_CHECK_HII_OPCODE_STRING   mHiiOpCodeStringTable[] = {
+  {EFI_IFR_VARSTORE_EFI_OP,     "EfiVarStore"},
+  {EFI_IFR_ONE_OF_OP,           "OneOf"},
+  {EFI_IFR_CHECKBOX_OP,         "CheckBox"},
+  {EFI_IFR_NUMERIC_OP,          "Numeric"},
+  {EFI_IFR_ORDERED_LIST_OP,     "OrderedList"},
+};
+
+/**
+  HII opcode to string.
+
+  @param[in] HiiOpCode  Hii OpCode.
+
+  @return Pointer to string.
+
+**/
+CHAR8 *
+HiiOpCodeToStr (
+  IN UINT8  HiiOpCode
+  )
+{
+  UINTN     Index;
+  for (Index = 0; Index < sizeof (mHiiOpCodeStringTable) / sizeof (mHiiOpCodeStringTable[0]); Index++) {
+    if (mHiiOpCodeStringTable[Index].HiiOpCode == HiiOpCode) {
+      return mHiiOpCodeStringTable[Index].HiiOpCodeStr;
+    }
+  }
+
+  return "<UnknownHiiOpCode>";
+}
+
+/**
+  Dump Hii Question.
+
+  @param[in] HiiQuestion    Pointer to Hii Question.
+
+**/
+VOID
+DumpHiiQuestion (
+  IN VAR_CHECK_HII_QUESTION_HEADER  *HiiQuestion
+  )
+{
+  UINT64    Minimum;
+  UINT64    Maximum;
+  UINT64    OneValue;
+  UINT8     *Ptr;
+
+  DEBUG ((EFI_D_INFO, "  VAR_CHECK_HII_QUESTION_HEADER\n"));
+  DEBUG ((EFI_D_INFO, "    OpCode        - 0x%02x (%a)\n", HiiQuestion->OpCode, HiiOpCodeToStr (HiiQuestion->OpCode)));
+  DEBUG ((EFI_D_INFO, "    Length        - 0x%02x\n", HiiQuestion->Length));
+  DEBUG ((EFI_D_INFO, "    VarOffset     - 0x%04x\n", HiiQuestion->VarOffset));
+  DEBUG ((EFI_D_INFO, "    StorageWidth  - 0x%02x\n", HiiQuestion->StorageWidth));
+
+  switch (HiiQuestion->OpCode) {
+    case EFI_IFR_ONE_OF_OP:
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ONEOF *) HiiQuestion + 1);
+      while ((UINTN) Ptr < ((UINTN) HiiQuestion + HiiQuestion->Length)) {
+        OneValue = 0;
+        CopyMem (&OneValue, Ptr, HiiQuestion->StorageWidth);
+        switch (HiiQuestion->StorageWidth) {
+          case sizeof (UINT8):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%02x\n", OneValue));
+            break;
+          case sizeof (UINT16):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%04x\n", OneValue));
+            break;
+          case sizeof (UINT32):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%08x\n", OneValue));
+            break;
+          case sizeof (UINT64):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%016lx\n", OneValue));
+            break;
+          default:
+            ASSERT (FALSE);
+            break;
+        }
+        Ptr += HiiQuestion->StorageWidth;
+      }
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      break;
+
+    case EFI_IFR_NUMERIC_OP:
+      Minimum = 0;
+      Maximum = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_NUMERIC *) HiiQuestion + 1);
+      CopyMem (&Minimum, Ptr, HiiQuestion->StorageWidth);
+      Ptr += HiiQuestion->StorageWidth;
+      CopyMem (&Maximum, Ptr, HiiQuestion->StorageWidth);
+      Ptr += HiiQuestion->StorageWidth;
+
+      switch (HiiQuestion->StorageWidth) {
+        case sizeof (UINT8):
+          DEBUG ((EFI_D_INFO, "    Minimum       - 0x%02x\n", Minimum));
+          DEBUG ((EFI_D_INFO, "    Maximum       - 0x%02x\n", Maximum));
+          break;
+        case sizeof (UINT16):
+          DEBUG ((EFI_D_INFO, "    Minimum       - 0x%04x\n", Minimum));
+          DEBUG ((EFI_D_INFO, "    Maximum       - 0x%04x\n", Maximum));
+          break;
+        case sizeof (UINT32):
+          DEBUG ((EFI_D_INFO, "    Minimum       - 0x%08x\n", Minimum));
+          DEBUG ((EFI_D_INFO, "    Maximum       - 0x%08x\n", Maximum));
+          break;
+        case sizeof (UINT64):
+          DEBUG ((EFI_D_INFO, "    Minimum       - 0x%016lx\n", Minimum));
+          DEBUG ((EFI_D_INFO, "    Maximum       - 0x%016lx\n", Maximum));
+          break;
+        default:
+          ASSERT (FALSE);
+          break;
+      }
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      DEBUG ((EFI_D_INFO, "    MaxContainers - 0x%02x\n", ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion)->MaxContainers));
+      Ptr = (UINT8 *) ((VAR_CHECK_HII_QUESTION_ORDEREDLIST *) HiiQuestion + 1);
+      while ((UINTN) Ptr < ((UINTN) HiiQuestion + HiiQuestion->Length)) {
+        OneValue = 0;
+        CopyMem (&OneValue, Ptr, HiiQuestion->StorageWidth);
+        switch (HiiQuestion->StorageWidth) {
+          case sizeof (UINT8):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%02x\n", OneValue));
+            break;
+          case sizeof (UINT16):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%04x\n", OneValue));
+            break;
+          case sizeof (UINT32):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%08x\n", OneValue));
+            break;
+          case sizeof (UINT64):
+            DEBUG ((EFI_D_INFO, "    OneOfOption   - 0x%016lx\n", OneValue));
+            break;
+          default:
+            ASSERT (FALSE);
+            break;
+        }
+        Ptr += HiiQuestion->StorageWidth;
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Dump Hii Variable.
+
+  @param[in] HiiVariable    Pointer to Hii Variable.
+
+**/
+VOID
+DumpHiiVariable (
+  IN VAR_CHECK_HII_VARIABLE_HEADER  *HiiVariable
+  )
+{
+  VAR_CHECK_HII_QUESTION_HEADER *HiiQuestion;
+
+  DEBUG ((EFI_D_INFO, "VAR_CHECK_HII_VARIABLE_HEADER\n"));
+  DEBUG ((EFI_D_INFO, "  Revision        - 0x%04x\n", HiiVariable->Revision));
+  DEBUG ((EFI_D_INFO, "  HeaderLength    - 0x%04x\n", HiiVariable->HeaderLength));
+  DEBUG ((EFI_D_INFO, "  Length          - 0x%08x\n", HiiVariable->Length));
+  DEBUG ((EFI_D_INFO, "  OpCode          - 0x%02x (%a)\n", HiiVariable->OpCode, HiiOpCodeToStr (HiiVariable->OpCode)));
+  DEBUG ((EFI_D_INFO, "  Size            - 0x%04x\n", HiiVariable->Size));
+  DEBUG ((EFI_D_INFO, "  Attributes      - 0x%08x\n", HiiVariable->Attributes));
+  DEBUG ((EFI_D_INFO, "  Guid            - %g\n", &HiiVariable->Guid));
+  DEBUG ((EFI_D_INFO, "  Name            - %s\n", HiiVariable + 1));
+
+  //
+  // For Hii Question header align.
+  //
+  HiiQuestion = (VAR_CHECK_HII_QUESTION_HEADER *) HEADER_ALIGN (((UINTN) HiiVariable + HiiVariable->HeaderLength));
+  while ((UINTN) HiiQuestion < ((UINTN) HiiVariable + HiiVariable->Length)) {
+    //
+    // Dump Hii Question related to the Hii Variable.
+    //
+    DumpHiiQuestion (HiiQuestion);
+    //
+    // For Hii Question header align.
+    //
+    HiiQuestion = (VAR_CHECK_HII_QUESTION_HEADER *) HEADER_ALIGN (((UINTN) HiiQuestion + HiiQuestion->Length));
+  }
+}
+
+/**
+  Dump Var Check HII.
+
+  @param[in] VarCheckHiiBin     Pointer to VarCheckHiiBin.
+  @param[in] VarCheckHiiBinSize VarCheckHiiBin size.
+
+**/
+VOID
+DumpVarCheckHii (
+  IN VOID   *VarCheckHiiBin,
+  IN UINTN  VarCheckHiiBinSize
+  )
+{
+  VAR_CHECK_HII_VARIABLE_HEADER     *HiiVariable;
+
+  DEBUG ((EFI_D_INFO, "DumpVarCheckHii\n"));
+
+  //
+  // For Hii Variable header align.
+  //
+  HiiVariable = (VAR_CHECK_HII_VARIABLE_HEADER *) HEADER_ALIGN (VarCheckHiiBin);
+  while ((UINTN) HiiVariable < ((UINTN) VarCheckHiiBin + VarCheckHiiBinSize)) {
+    DumpHiiVariable (HiiVariable);
+    //
+    // For Hii Variable header align.
+    //
+    HiiVariable = (VAR_CHECK_HII_VARIABLE_HEADER *) HEADER_ALIGN (((UINTN) HiiVariable + HiiVariable->Length));
+  }
+}
+#endif
+
+/**
+  Constructor function of VarCheckHiiLib to register var check HII handler.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The constructor executed correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckHiiLibNullClassConstructor (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  VarCheckLibRegisterEndOfDxeCallback (VarCheckHiiGen);
+  VarCheckLibRegisterAddressPointer ((VOID **) &mVarCheckHiiBin);
+  VarCheckLibRegisterSetVariableCheckHandler (SetVariableCheckHandlerHii);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.c b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.c
new file mode 100644
index 0000000000..60c141a6b9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.c
@@ -0,0 +1,663 @@
+/** @file
+  Implementation functions and structures for var check services.
+
+Copyright (c) 2015, 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 <Library/VarCheckLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Guid/GlobalVariable.h>
+#include <Guid/HardwareErrorVariable.h>
+
+BOOLEAN mVarCheckLibEndOfDxe    = FALSE;
+
+#define VAR_CHECK_TABLE_SIZE    0x8
+
+UINTN                                   mVarCheckLibEndOfDxeCallbackCount = 0;
+UINTN                                   mVarCheckLibEndOfDxeCallbackMaxCount = 0;
+VAR_CHECK_END_OF_DXE_CALLBACK           *mVarCheckLibEndOfDxeCallback = NULL;
+
+UINTN                                   mVarCheckLibAddressPointerCount = 0;
+UINTN                                   mVarCheckLibAddressPointerMaxCount = 0;
+VOID                                    ***mVarCheckLibAddressPointer = NULL;
+
+UINTN                                   mNumberOfVarCheckHandler = 0;
+UINTN                                   mMaxNumberOfVarCheckHandler = 0;
+VAR_CHECK_SET_VARIABLE_CHECK_HANDLER    *mVarCheckHandlerTable = NULL;
+
+typedef struct {
+  EFI_GUID                      Guid;
+  VAR_CHECK_VARIABLE_PROPERTY   VariableProperty;
+  //CHAR16                        *Name;
+} VAR_CHECK_VARIABLE_ENTRY;
+
+UINTN                                   mNumberOfVarCheckVariable = 0;
+UINTN                                   mMaxNumberOfVarCheckVariable = 0;
+VARIABLE_ENTRY_PROPERTY                 **mVarCheckVariableTable = NULL;
+
+//
+// Handle variables with wildcard name specially.
+//
+VARIABLE_ENTRY_PROPERTY mVarCheckVariableWithWildcardName[] = {
+  {
+    &gEfiGlobalVariableGuid,
+    L"Boot####",
+    {
+      0
+    },
+  },
+  {
+    &gEfiGlobalVariableGuid,
+    L"Driver####",
+    {
+      0
+    },
+  },
+  {
+    &gEfiGlobalVariableGuid,
+    L"SysPrep####",
+    {
+      0
+    },
+  },
+  {
+    &gEfiGlobalVariableGuid,
+    L"Key####",
+    {
+      0
+    },
+  },
+  {
+    &gEfiHardwareErrorVariableGuid,
+    L"HwErrRec####",
+    {
+      0
+    },
+  },
+};
+
+/**
+  Check if a Unicode character is a hexadecimal character.
+
+  This function checks if a Unicode character is a
+  hexadecimal character.  The valid hexadecimal character is
+  L'0' to L'9', L'a' to L'f', or L'A' to L'F'.
+
+
+  @param[in] Char       The character to check against.
+
+  @retval TRUE          If the Char is a hexadecmial character.
+  @retval FALSE         If the Char is not a hexadecmial character.
+
+**/
+BOOLEAN
+EFIAPI
+VarCheckInternalIsHexaDecimalDigitCharacter (
+  IN CHAR16             Char
+  )
+{
+  return (BOOLEAN) ((Char >= L'0' && Char <= L'9') || (Char >= L'A' && Char <= L'F') || (Char >= L'a' && Char <= L'f'));
+}
+
+/**
+  Variable property get with wildcard name.
+
+  @param[in] VariableName       Pointer to variable name.
+  @param[in] VendorGuid         Pointer to variable vendor GUID.
+  @param[in] WildcardMatch      Try wildcard match or not.
+
+  @return Pointer to variable property.
+
+**/
+VAR_CHECK_VARIABLE_PROPERTY *
+VariablePropertyGetWithWildcardName (
+  IN CHAR16                         *VariableName,
+  IN EFI_GUID                       *VendorGuid,
+  IN BOOLEAN                        WildcardMatch
+  )
+{
+  UINTN     Index;
+  UINTN     NameLength;
+
+  NameLength = StrLen (VariableName) - 4;
+  for (Index = 0; Index < sizeof (mVarCheckVariableWithWildcardName)/sizeof (mVarCheckVariableWithWildcardName[0]); Index++) {
+    if (CompareGuid (mVarCheckVariableWithWildcardName[Index].Guid, VendorGuid)){
+      if (WildcardMatch) {
+        if ((StrLen (VariableName) == StrLen (mVarCheckVariableWithWildcardName[Index].Name)) &&
+            (StrnCmp (VariableName, mVarCheckVariableWithWildcardName[Index].Name, NameLength) == 0) &&
+            VarCheckInternalIsHexaDecimalDigitCharacter (VariableName[NameLength]) &&
+            VarCheckInternalIsHexaDecimalDigitCharacter (VariableName[NameLength + 1]) &&
+            VarCheckInternalIsHexaDecimalDigitCharacter (VariableName[NameLength + 2]) &&
+            VarCheckInternalIsHexaDecimalDigitCharacter (VariableName[NameLength + 3])) {
+          return &mVarCheckVariableWithWildcardName[Index].VariableProperty;
+        }
+      }
+      if (StrCmp (mVarCheckVariableWithWildcardName[Index].Name, VariableName) == 0) {
+        return  &mVarCheckVariableWithWildcardName[Index].VariableProperty;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Variable property get function.
+
+  @param[in] Name           Pointer to the variable name.
+  @param[in] Guid           Pointer to the vendor GUID.
+  @param[in] WildcardMatch  Try wildcard match or not.
+
+  @return Pointer to the property of variable specified by the Name and Guid.
+
+**/
+VAR_CHECK_VARIABLE_PROPERTY *
+VariablePropertyGetFunction (
+  IN CHAR16                 *Name,
+  IN EFI_GUID               *Guid,
+  IN BOOLEAN                WildcardMatch
+  )
+{
+  UINTN                     Index;
+  VAR_CHECK_VARIABLE_ENTRY  *Entry;
+  CHAR16                    *VariableName;
+
+  for (Index = 0; Index < mNumberOfVarCheckVariable; Index++) {
+    Entry = (VAR_CHECK_VARIABLE_ENTRY *) mVarCheckVariableTable[Index];
+    VariableName = (CHAR16 *) ((UINTN) Entry + sizeof (*Entry));
+    if (CompareGuid (&Entry->Guid, Guid) && (StrCmp (VariableName, Name) == 0)) {
+      return &Entry->VariableProperty;
+    }
+  }
+
+  return VariablePropertyGetWithWildcardName (Name, Guid, WildcardMatch);
+}
+
+/**
+  Var check add table entry.
+
+  @param[in, out] Table         Pointer to table buffer.
+  @param[in, out] MaxNumber     Pointer to maximum number of entry in the table.
+  @param[in, out] CurrentNumber Pointer to current number of entry in the table.
+  @param[in]      Entry         Entry will be added to the table.
+
+  @retval EFI_SUCCESS           Reallocate memory successfully.
+  @retval EFI_OUT_OF_RESOURCES  No enough memory to allocate.
+
+**/
+EFI_STATUS
+VarCheckAddTableEntry (
+  IN OUT UINTN      **Table,
+  IN OUT UINTN      *MaxNumber,
+  IN OUT UINTN      *CurrentNumber,
+  IN UINTN          Entry
+  )
+{
+  UINTN     *TempTable;
+
+  //
+  // Check whether the table is enough to store new entry.
+  //
+  if (*CurrentNumber == *MaxNumber) {
+    //
+    // Reallocate memory for the table.
+    //
+    TempTable = ReallocateRuntimePool (
+                  *MaxNumber * sizeof (UINTN),
+                  (*MaxNumber + VAR_CHECK_TABLE_SIZE) * sizeof (UINTN),
+                  *Table
+                  );
+
+    //
+    // No enough resource to allocate.
+    //
+    if (TempTable == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    *Table = TempTable;
+    //
+    // Increase max number.
+    //
+    *MaxNumber += VAR_CHECK_TABLE_SIZE;
+  }
+
+  //
+  // Add entry to the table.
+  //
+  (*Table)[*CurrentNumber] = Entry;
+  (*CurrentNumber)++;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register END_OF_DXE callback.
+  The callback will be invoked by VarCheckLibInitializeAtEndOfDxe().
+
+  @param[in] Callback           END_OF_DXE callback.
+
+  @retval EFI_SUCCESS           The callback was registered successfully.
+  @retval EFI_INVALID_PARAMETER Callback is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the callback register request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterEndOfDxeCallback (
+  IN VAR_CHECK_END_OF_DXE_CALLBACK  Callback
+  )
+{
+  EFI_STATUS    Status;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mVarCheckLibEndOfDxe) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  Status = VarCheckAddTableEntry (
+           (UINTN **) &mVarCheckLibEndOfDxeCallback,
+           &mVarCheckLibEndOfDxeCallbackMaxCount,
+           &mVarCheckLibEndOfDxeCallbackCount,
+           (UINTN) Callback
+           );
+
+  DEBUG ((EFI_D_INFO, "VarCheckLibRegisterEndOfDxeCallback - 0x%x %r\n", Callback, Status));
+
+  return Status;
+}
+
+/**
+  Var check initialize at END_OF_DXE.
+
+  This function needs to be called at END_OF_DXE.
+  Address pointers may be returned,
+  and caller needs to ConvertPointer() for the pointers.
+
+  @param[in, out] AddressPointerCount   Output pointer to address pointer count.
+
+  @return Address pointer buffer, NULL if input AddressPointerCount is NULL.
+
+**/
+VOID ***
+EFIAPI
+VarCheckLibInitializeAtEndOfDxe (
+  IN OUT UINTN                  *AddressPointerCount OPTIONAL
+  )
+{
+  VOID                          *TempTable;
+  UINTN                         TotalCount;
+  UINTN                         Index;
+
+  for (Index = 0; Index < mVarCheckLibEndOfDxeCallbackCount; Index++) {
+    //
+    // Invoke the callback registered by VarCheckLibRegisterEndOfDxeCallback().
+    //
+    mVarCheckLibEndOfDxeCallback[Index] ();
+  }
+  if (mVarCheckLibEndOfDxeCallback != NULL) {
+    //
+    // Free the callback buffer.
+    //
+    mVarCheckLibEndOfDxeCallbackCount = 0;
+    mVarCheckLibEndOfDxeCallbackMaxCount = 0;
+    FreePool ((VOID *) mVarCheckLibEndOfDxeCallback);
+    mVarCheckLibEndOfDxeCallback = NULL;
+  }
+
+  mVarCheckLibEndOfDxe = TRUE;
+
+  if (AddressPointerCount == NULL) {
+    if (mVarCheckLibAddressPointer != NULL) {
+      //
+      // Free the address pointer buffer.
+      //
+      mVarCheckLibAddressPointerCount = 0;
+      mVarCheckLibAddressPointerMaxCount = 0;
+      FreePool ((VOID *) mVarCheckLibAddressPointer);
+      mVarCheckLibAddressPointer = NULL;
+    }
+    return NULL;
+  }
+
+  //
+  // Get the total count needed.
+  // Also cover VarCheckHandler and the entries, and VarCheckVariable and the entries.
+  //
+  TotalCount = mVarCheckLibAddressPointerCount + (mNumberOfVarCheckHandler + 1) + (mNumberOfVarCheckVariable + 1);
+  TempTable = ReallocateRuntimePool (
+                mVarCheckLibAddressPointerMaxCount * sizeof (VOID **),
+                TotalCount * sizeof (VOID **),
+                (VOID *) mVarCheckLibAddressPointer
+                );
+
+  if (TempTable != NULL) {
+    mVarCheckLibAddressPointer = (VOID ***) TempTable;
+
+    //
+    // Cover VarCheckHandler and the entries.
+    //
+    mVarCheckLibAddressPointer[mVarCheckLibAddressPointerCount++] = (VOID **) &mVarCheckHandlerTable;
+    for (Index = 0; Index < mNumberOfVarCheckHandler; Index++) {
+      mVarCheckLibAddressPointer[mVarCheckLibAddressPointerCount++] = (VOID **) &mVarCheckHandlerTable[Index];
+    }
+
+    //
+    // Cover VarCheckVariable and the entries.
+    //
+    mVarCheckLibAddressPointer[mVarCheckLibAddressPointerCount++] = (VOID **) &mVarCheckVariableTable;
+    for (Index = 0; Index < mNumberOfVarCheckVariable; Index++) {
+      mVarCheckLibAddressPointer[mVarCheckLibAddressPointerCount++] = (VOID **) &mVarCheckVariableTable[Index];
+    }
+
+    ASSERT (mVarCheckLibAddressPointerCount == TotalCount);
+    mVarCheckLibAddressPointerMaxCount = mVarCheckLibAddressPointerCount;
+  }
+
+  *AddressPointerCount = mVarCheckLibAddressPointerCount;
+  return mVarCheckLibAddressPointer;
+}
+
+/**
+  Register address pointer.
+  The AddressPointer may be returned by VarCheckLibInitializeAtEndOfDxe().
+
+  @param[in] AddressPointer     Address pointer.
+
+  @retval EFI_SUCCESS           The address pointer was registered successfully.
+  @retval EFI_INVALID_PARAMETER AddressPointer is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the address pointer register request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterAddressPointer (
+  IN VOID                       **AddressPointer
+  )
+{
+  EFI_STATUS    Status;
+
+  if (AddressPointer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mVarCheckLibEndOfDxe) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  Status = VarCheckAddTableEntry(
+           (UINTN **) &mVarCheckLibAddressPointer,
+           &mVarCheckLibAddressPointerMaxCount,
+           &mVarCheckLibAddressPointerCount,
+           (UINTN) AddressPointer
+           );
+
+  DEBUG ((EFI_D_INFO, "VarCheckLibRegisterAddressPointer - 0x%x %r\n", AddressPointer, Status));
+
+  return Status;
+}
+
+/**
+  Register SetVariable check handler.
+
+  @param[in] Handler            Pointer to check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibRegisterSetVariableCheckHandler (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  )
+{
+  EFI_STATUS    Status;
+
+  if (Handler == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mVarCheckLibEndOfDxe) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  Status =  VarCheckAddTableEntry(
+             (UINTN **) &mVarCheckHandlerTable,
+             &mMaxNumberOfVarCheckHandler,
+             &mNumberOfVarCheckHandler,
+             (UINTN) Handler
+             );
+
+  DEBUG ((EFI_D_INFO, "VarCheckLibRegisterSetVariableCheckHandler - 0x%x %r\n", Handler, Status));
+
+  return Status;
+}
+
+/**
+  Variable property set.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibVariablePropertySet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  )
+{
+  EFI_STATUS                    Status;
+  VAR_CHECK_VARIABLE_ENTRY      *Entry;
+  CHAR16                        *VariableName;
+  VAR_CHECK_VARIABLE_PROPERTY   *Property;
+
+  if (Name == NULL || Name[0] == 0 || Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty->Revision != VAR_CHECK_VARIABLE_PROPERTY_REVISION) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (mVarCheckLibEndOfDxe) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Get the pointer of property data for set.
+  //
+  Property = VariablePropertyGetFunction (Name, Guid, FALSE);
+  if (Property != NULL) {
+    CopyMem (Property, VariableProperty, sizeof (*VariableProperty));
+  } else {
+    Entry = AllocateRuntimeZeroPool (sizeof (*Entry) + StrSize (Name));
+    if (Entry == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    VariableName = (CHAR16 *) ((UINTN) Entry + sizeof (*Entry));
+    StrCpyS (VariableName, StrSize (Name)/sizeof (CHAR16), Name);
+    CopyGuid (&Entry->Guid, Guid);
+    CopyMem (&Entry->VariableProperty, VariableProperty, sizeof (*VariableProperty));
+
+    Status = VarCheckAddTableEntry(
+               (UINTN **) &mVarCheckVariableTable,
+               &mMaxNumberOfVarCheckVariable,
+               &mNumberOfVarCheckVariable,
+               (UINTN) Entry
+               );
+
+    if (EFI_ERROR (Status)) {
+      FreePool (Entry);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibVariablePropertyGet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  )
+{
+  VAR_CHECK_VARIABLE_PROPERTY   *Property;
+
+  if (Name == NULL || Name[0] == 0 || Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Property = VariablePropertyGetFunction (Name, Guid, TRUE);
+  //
+  // Also check the property revision before using the property data.
+  // There is no property set to this variable(wildcard name)
+  // if the revision is not VAR_CHECK_VARIABLE_PROPERTY_REVISION.
+  //
+  if ((Property != NULL) && (Property->Revision == VAR_CHECK_VARIABLE_PROPERTY_REVISION)) {
+    CopyMem (VariableProperty, Property, sizeof (*VariableProperty));
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  SetVariable check.
+
+  @param[in] VariableName       Name of Variable to set.
+  @param[in] VendorGuid         Variable vendor GUID.
+  @param[in] Attributes         Attribute value of the variable.
+  @param[in] DataSize           Size of Data to set.
+  @param[in] Data               Data pointer.
+  @param[in] RequestSource      Request source.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER An invalid combination of attribute bits, name, GUID,
+                                DataSize and Data value was supplied.
+  @retval EFI_WRITE_PROTECTED   The variable in question is read-only.
+  @retval Others                The other return status from check handler.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckLibSetVariableCheck (
+  IN CHAR16                     *VariableName,
+  IN EFI_GUID                   *VendorGuid,
+  IN UINT32                     Attributes,
+  IN UINTN                      DataSize,
+  IN VOID                       *Data,
+  IN VAR_CHECK_REQUEST_SOURCE   RequestSource
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  VAR_CHECK_VARIABLE_PROPERTY   *Property;
+
+  if (!mVarCheckLibEndOfDxe) {
+    //
+    // Only do check after End Of Dxe.
+    //
+    return EFI_SUCCESS;
+  }
+
+  Property = VariablePropertyGetFunction (VariableName, VendorGuid, TRUE);
+  //
+  // Also check the property revision before using the property data.
+  // There is no property set to this variable(wildcard name)
+  // if the revision is not VAR_CHECK_VARIABLE_PROPERTY_REVISION.
+  //
+  if ((Property != NULL) && (Property->Revision == VAR_CHECK_VARIABLE_PROPERTY_REVISION)) {
+    if ((RequestSource != VarCheckFromTrusted) && ((Property->Property & VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY) != 0)) {
+      DEBUG ((EFI_D_INFO, "Variable Check ReadOnly variable fail %r - %g:%s\n", EFI_WRITE_PROTECTED, VendorGuid, VariableName));
+      return EFI_WRITE_PROTECTED;
+    }
+    if (!((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0)) || (Attributes == 0))) {
+      //
+      // Not to delete variable.
+      //
+      if ((Property->Attributes != 0) && ((Attributes & (~EFI_VARIABLE_APPEND_WRITE)) != Property->Attributes)) {
+        DEBUG ((EFI_D_INFO, "Variable Check Attributes(0x%08x to 0x%08x) fail %r - %g:%s\n", Property->Attributes, Attributes, EFI_INVALID_PARAMETER, VendorGuid, VariableName));
+        return EFI_INVALID_PARAMETER;
+      }
+      if (DataSize != 0) {
+        if ((DataSize < Property->MinSize) || (DataSize > Property->MaxSize)) {
+          DEBUG ((EFI_D_INFO, "Variable Check DataSize fail(0x%x not in 0x%x - 0x%x) %r - %g:%s\n", DataSize, Property->MinSize, Property->MaxSize, EFI_INVALID_PARAMETER, VendorGuid, VariableName));
+          return EFI_INVALID_PARAMETER;
+        }
+      }
+    }
+  }
+
+  for (Index = 0; Index < mNumberOfVarCheckHandler; Index++) {
+    Status = mVarCheckHandlerTable[Index] (
+               VariableName,
+               VendorGuid,
+               Attributes,
+               DataSize,
+               Data
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "Variable Check handler fail %r - %g:%s\n", Status, VendorGuid, VariableName));
+      return Status;
+    }
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.inf b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.inf
new file mode 100644
index 0000000000..099f83dd6a
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.inf
@@ -0,0 +1,51 @@
+## @file
+#  Provides variable check services and database management.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VarCheckLib
+  MODULE_UNI_FILE                = VarCheckLib.uni
+  FILE_GUID                      = 63E12D08-0C5D-47F8-95E4-09F89D7506C5
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = VarCheckLib|DXE_RUNTIME_DRIVER DXE_SMM_DRIVER
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  VarCheckLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  MemoryAllocationLib
+
+[Guids]
+  ## SOMETIMES_CONSUMES  ## Variable:L"Boot####"
+  ## SOMETIMES_CONSUMES  ## Variable:L"Driver####"
+  ## SOMETIMES_CONSUMES  ## Variable:L"SysPrep####"
+  ## SOMETIMES_CONSUMES  ## Variable:L"Key####"
+  gEfiGlobalVariableGuid
+  gEfiHardwareErrorVariableGuid  ## SOMETIMES_CONSUMES   ## Variable:L"HwErrRec####"
diff --git a/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.uni b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.uni
new file mode 100644
index 0000000000..d9761a1103
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckLib/VarCheckLib.uni
diff --git a/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf
new file mode 100644
index 0000000000..b9d235be60
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf
@@ -0,0 +1,65 @@
+## @file
+#  NULL class library to register var check PCD handler.
+#
+#  In platform *.fdf, the example build rule for the driver this library linked to.
+#    [Rule.Common.DXE_RUNTIME_DRIVER.VARCHECKPCD]
+#      FILE DRIVER = $(NAMED_GUID) {
+#        RAW          BIN                     $(WORKSPACE)/$(OUTPUT_DIRECTORY)/$(TARGET)_$(TOOL_CHAIN_TAG)/FV/PcdVarCheck.bin
+#        DXE_DEPEX    DXE_DEPEX Optional      $(INF_OUTPUT)/$(MODULE_NAME).depex
+#        PE32         PE32                    $(INF_OUTPUT)/$(MODULE_NAME).efi
+#        UI           STRING="$(MODULE_NAME)" Optional
+#        VERSION      STRING="$(INF_VERSION)" Optional BUILD_NUM=$(BUILD_NUMBER)
+#      }
+#
+#    or
+#
+#    [Rule.Common.DXE_SMM_DRIVER.VARCHECKPCD]
+#      FILE SMM = $(NAMED_GUID) {
+#        RAW          BIN                     $(WORKSPACE)/$(OUTPUT_DIRECTORY)/$(TARGET)_$(TOOL_CHAIN_TAG)/FV/PcdVarCheck.bin
+#        DXE_DEPEX    DXE_DEPEX Optional      $(INF_OUTPUT)/$(MODULE_NAME).depex
+#        PE32         PE32                    $(INF_OUTPUT)/$(MODULE_NAME).efi
+#        UI           STRING="$(MODULE_NAME)" Optional
+#        VERSION      STRING="$(INF_VERSION)" Optional BUILD_NUM=$(BUILD_NUMBER)
+#      }
+#
+#  In platform *.dsc, also need add one line below to enable PcdVarCheck.bin generation by BaseTools.
+#    PCD_VAR_CHECK_GENERATION            = TRUE
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VarCheckPcdLib
+  MODULE_UNI_FILE                = VarCheckPcdLib.uni
+  FILE_GUID                      = D4FA5311-5F1F-4B1E-9AC3-90C4DFC029F1
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|DXE_RUNTIME_DRIVER DXE_SMM_DRIVER
+  CONSTRUCTOR                    = VarCheckPcdLibNullClassConstructor
+
+[Sources]
+  VarCheckPcdLibNullClass.c
+  VarCheckPcdStructure.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  BaseMemoryLib
+  DxeServicesLib
+  MemoryAllocationLib
+  VarCheckLib
diff --git a/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.uni b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.uni
new file mode 100644
index 0000000000..4a3d932be9
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.uni
diff --git a/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLibNullClass.c b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLibNullClass.c
new file mode 100644
index 0000000000..72b0363b19
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLibNullClass.c
@@ -0,0 +1,474 @@
+/** @file
+  Var Check PCD handler.
+
+Copyright (c) 2015, 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 <Library/VarCheckLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DxeServicesLib.h>
+
+#include "VarCheckPcdStructure.h"
+
+//#define DUMP_VAR_CHECK_PCD
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8 mVarCheckPcdHex[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
+
+/**
+  Dump some hexadecimal data.
+
+  @param[in] Indent     How many spaces to indent the output.
+  @param[in] Offset     The offset of the dump.
+  @param[in] DataSize   The size in bytes of UserData.
+  @param[in] UserData   The data to dump.
+
+**/
+VOID
+VarCheckPcdInternalDumpHex (
+  IN UINTN        Indent,
+  IN UINTN        Offset,
+  IN UINTN        DataSize,
+  IN VOID         *UserData
+  )
+{
+  UINT8 *Data;
+
+  CHAR8 Val[50];
+
+  CHAR8 Str[20];
+
+  UINT8 TempByte;
+  UINTN Size;
+  UINTN Index;
+
+  Data = UserData;
+  while (DataSize != 0) {
+    Size = 16;
+    if (Size > DataSize) {
+      Size = DataSize;
+    }
+
+    for (Index = 0; Index < Size; Index += 1) {
+      TempByte            = Data[Index];
+      Val[Index * 3 + 0]  = mVarCheckPcdHex[TempByte >> 4];
+      Val[Index * 3 + 1]  = mVarCheckPcdHex[TempByte & 0xF];
+      Val[Index * 3 + 2]  = (CHAR8) ((Index == 7) ? '-' : ' ');
+      Str[Index]          = (CHAR8) ((TempByte < ' ' || TempByte > 'z') ? '.' : TempByte);
+    }
+
+    Val[Index * 3]  = 0;
+    Str[Index]      = 0;
+    DEBUG ((EFI_D_INFO, "%*a%08X: %-48a *%a*\r\n", Indent, "", Offset, Val, Str));
+
+    Data += Size;
+    Offset += Size;
+    DataSize -= Size;
+  }
+}
+
+/**
+  Var Check Pcd ValidData.
+
+  @param[in] PcdValidData   Pointer to Pcd ValidData
+  @param[in] Data           Data pointer.
+  @param[in] DataSize       Size of Data to set.
+
+  @retval TRUE  Check pass
+  @retval FALSE Check fail.
+
+**/
+BOOLEAN
+VarCheckPcdValidData (
+  IN VAR_CHECK_PCD_VALID_DATA_HEADER    *PcdValidData,
+  IN VOID                               *Data,
+  IN UINTN                              DataSize
+  )
+{
+  UINT64   OneData;
+  UINT64   Minimum;
+  UINT64   Maximum;
+  UINT64   OneValue;
+  UINT8    *Ptr;
+
+  OneData = 0;
+  CopyMem (&OneData, (UINT8 *) Data + PcdValidData->VarOffset, PcdValidData->StorageWidth);
+
+  switch (PcdValidData->Type) {
+    case VarCheckPcdValidList:
+      Ptr = (UINT8 *) ((VAR_CHECK_PCD_VALID_LIST *) PcdValidData + 1);
+      while ((UINTN) Ptr < (UINTN) PcdValidData + PcdValidData->Length) {
+        OneValue = 0;
+        CopyMem (&OneValue, Ptr, PcdValidData->StorageWidth);
+        if (OneData == OneValue) {
+          //
+          // Match
+          //
+          break;
+        }
+        Ptr += PcdValidData->StorageWidth;
+      }
+      if ((UINTN) Ptr >= ((UINTN) PcdValidData + PcdValidData->Length)) {
+        //
+        // No match
+        //
+        DEBUG ((EFI_D_INFO, "VarCheckPcdValidData fail: ValidList mismatch (0x%lx)\n", OneData));
+        DEBUG_CODE (VarCheckPcdInternalDumpHex (2, 0, PcdValidData->Length, (UINT8 *) PcdValidData););
+        return FALSE;
+      }
+      break;
+
+    case VarCheckPcdValidRange:
+      Minimum = 0;
+      Maximum = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_PCD_VALID_RANGE *) PcdValidData + 1);
+      while ((UINTN) Ptr < (UINTN) PcdValidData + PcdValidData->Length) {
+        CopyMem (&Minimum, Ptr, PcdValidData->StorageWidth);
+        Ptr += PcdValidData->StorageWidth;
+        CopyMem (&Maximum, Ptr, PcdValidData->StorageWidth);
+        Ptr += PcdValidData->StorageWidth;
+
+        if ((OneData >= Minimum) && (OneData <= Maximum)) {
+          return TRUE;
+        }
+      }
+      DEBUG ((EFI_D_INFO, "VarCheckPcdValidData fail: ValidRange mismatch (0x%lx)\n", OneData));
+      DEBUG_CODE (VarCheckPcdInternalDumpHex (2, 0, PcdValidData->Length, (UINT8 *) PcdValidData););
+      return FALSE;
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+
+  return TRUE;
+}
+
+VAR_CHECK_PCD_VARIABLE_HEADER   *mVarCheckPcdBin = NULL;
+UINTN                           mVarCheckPcdBinSize = 0;
+
+/**
+  SetVariable check handler PCD.
+
+  @param[in] VariableName       Name of Variable to set.
+  @param[in] VendorGuid         Variable vendor GUID.
+  @param[in] Attributes         Attribute value of the variable.
+  @param[in] DataSize           Size of Data to set.
+  @param[in] Data               Data pointer.
+
+  @retval EFI_SUCCESS               The SetVariable check result was success.
+  @retval EFI_SECURITY_VIOLATION    Check fail.
+
+**/
+EFI_STATUS
+EFIAPI
+SetVariableCheckHandlerPcd (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  VAR_CHECK_PCD_VARIABLE_HEADER     *PcdVariable;
+  VAR_CHECK_PCD_VALID_DATA_HEADER   *PcdValidData;
+
+  if (mVarCheckPcdBin == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  if ((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0)) || (Attributes == 0)) {
+    //
+    // Do not check delete variable.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // For Pcd Variable header align.
+  //
+  PcdVariable = (VAR_CHECK_PCD_VARIABLE_HEADER *) HEADER_ALIGN (mVarCheckPcdBin);
+  while ((UINTN) PcdVariable < ((UINTN) mVarCheckPcdBin + mVarCheckPcdBinSize)) {
+    if ((StrCmp ((CHAR16 *) (PcdVariable + 1), VariableName) == 0) &&
+        (CompareGuid (&PcdVariable->Guid, VendorGuid))) {
+      //
+      // Found the Pcd Variable that could be used to do check.
+      //
+      DEBUG ((EFI_D_INFO, "VarCheckPcdVariable - %s:%g with Attributes = 0x%08x Size = 0x%x\n", VariableName, VendorGuid, Attributes, DataSize));
+      if ((PcdVariable->Attributes != 0) && PcdVariable->Attributes != Attributes) {
+        DEBUG ((EFI_D_INFO, "VarCheckPcdVariable fail for Attributes - 0x%08x\n", PcdVariable->Attributes));
+        return EFI_SECURITY_VIOLATION;
+      }
+
+      if (DataSize == 0) {
+        DEBUG ((EFI_D_INFO, "VarCheckPcdVariable - CHECK PASS with DataSize == 0 !\n"));
+        return EFI_SUCCESS;
+      }
+
+      //
+      // Do the check.
+      // For Pcd ValidData header align.
+      //
+      PcdValidData = (VAR_CHECK_PCD_VALID_DATA_HEADER *) HEADER_ALIGN (((UINTN) PcdVariable + PcdVariable->HeaderLength));
+      while ((UINTN) PcdValidData < ((UINTN) PcdVariable + PcdVariable->Length)) {
+        if (((UINTN) PcdValidData->VarOffset + PcdValidData->StorageWidth) <= DataSize) {
+          if (!VarCheckPcdValidData (PcdValidData, Data, DataSize)) {
+            return EFI_SECURITY_VIOLATION;
+          }
+        }
+        //
+        // For Pcd ValidData header align.
+        //
+        PcdValidData = (VAR_CHECK_PCD_VALID_DATA_HEADER *) HEADER_ALIGN (((UINTN) PcdValidData + PcdValidData->Length));
+      }
+
+      DEBUG ((EFI_D_INFO, "VarCheckPcdVariable - ALL CHECK PASS!\n"));
+      return EFI_SUCCESS;
+    }
+    //
+    // For Pcd Variable header align.
+    //
+    PcdVariable = (VAR_CHECK_PCD_VARIABLE_HEADER *) HEADER_ALIGN (((UINTN) PcdVariable + PcdVariable->Length));
+  }
+
+  // Not found, so pass.
+  return EFI_SUCCESS;
+}
+
+#ifdef DUMP_VAR_CHECK_PCD
+/**
+  Dump Pcd ValidData.
+
+  @param[in] PcdValidData    Pointer to Pcd ValidData.
+
+**/
+VOID
+DumpPcdValidData (
+  IN VAR_CHECK_PCD_VALID_DATA_HEADER    *PcdValidData
+  )
+{
+  UINT64    Minimum;
+  UINT64    Maximum;
+  UINT64    OneValue;
+  UINT8     *Ptr;
+
+  DEBUG ((EFI_D_INFO, "  VAR_CHECK_PCD_VALID_DATA_HEADER\n"));
+  DEBUG ((EFI_D_INFO, "    Type          - 0x%02x\n", PcdValidData->Type));
+  DEBUG ((EFI_D_INFO, "    Length        - 0x%02x\n", PcdValidData->Length));
+  DEBUG ((EFI_D_INFO, "    VarOffset     - 0x%04x\n", PcdValidData->VarOffset));
+  DEBUG ((EFI_D_INFO, "    StorageWidth  - 0x%02x\n", PcdValidData->StorageWidth));
+
+  switch (PcdValidData->Type) {
+    case VarCheckPcdValidList:
+      Ptr = (UINT8 *) ((VAR_CHECK_PCD_VALID_LIST *) PcdValidData + 1);
+      while ((UINTN) Ptr < ((UINTN) PcdValidData + PcdValidData->Length)) {
+        OneValue = 0;
+        CopyMem (&OneValue, Ptr, PcdValidData->StorageWidth);
+        switch (PcdValidData->StorageWidth) {
+          case sizeof (UINT8):
+            DEBUG ((EFI_D_INFO, "    ValidList   - 0x%02x\n", OneValue));
+            break;
+          case sizeof (UINT16):
+            DEBUG ((EFI_D_INFO, "    ValidList   - 0x%04x\n", OneValue));
+            break;
+          case sizeof (UINT32):
+            DEBUG ((EFI_D_INFO, "    ValidList   - 0x%08x\n", OneValue));
+            break;
+          case sizeof (UINT64):
+            DEBUG ((EFI_D_INFO, "    ValidList   - 0x%016lx\n", OneValue));
+            break;
+          default:
+            ASSERT (FALSE);
+            break;
+        }
+        Ptr += PcdValidData->StorageWidth;
+      }
+      break;
+
+    case VarCheckPcdValidRange:
+      Minimum = 0;
+      Maximum = 0;
+      Ptr = (UINT8 *) ((VAR_CHECK_PCD_VALID_RANGE *) PcdValidData + 1);
+      while ((UINTN) Ptr < (UINTN) PcdValidData + PcdValidData->Length) {
+        CopyMem (&Minimum, Ptr, PcdValidData->StorageWidth);
+        Ptr += PcdValidData->StorageWidth;
+        CopyMem (&Maximum, Ptr, PcdValidData->StorageWidth);
+        Ptr += PcdValidData->StorageWidth;
+
+        switch (PcdValidData->StorageWidth) {
+          case sizeof (UINT8):
+            DEBUG ((EFI_D_INFO, "    Minimum       - 0x%02x\n", Minimum));
+            DEBUG ((EFI_D_INFO, "    Maximum       - 0x%02x\n", Maximum));
+            break;
+          case sizeof (UINT16):
+            DEBUG ((EFI_D_INFO, "    Minimum       - 0x%04x\n", Minimum));
+            DEBUG ((EFI_D_INFO, "    Maximum       - 0x%04x\n", Maximum));
+            break;
+          case sizeof (UINT32):
+            DEBUG ((EFI_D_INFO, "    Minimum       - 0x%08x\n", Minimum));
+            DEBUG ((EFI_D_INFO, "    Maximum       - 0x%08x\n", Maximum));
+            break;
+          case sizeof (UINT64):
+            DEBUG ((EFI_D_INFO, "    Minimum       - 0x%016lx\n", Minimum));
+            DEBUG ((EFI_D_INFO, "    Maximum       - 0x%016lx\n", Maximum));
+            break;
+          default:
+            ASSERT (FALSE);
+            break;
+        }
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+}
+
+/**
+  Dump Pcd Variable.
+
+  @param[in] PcdVariable    Pointer to Pcd Variable.
+
+**/
+VOID
+DumpPcdVariable (
+  IN VAR_CHECK_PCD_VARIABLE_HEADER  *PcdVariable
+  )
+{
+  VAR_CHECK_PCD_VALID_DATA_HEADER *PcdValidData;
+
+  DEBUG ((EFI_D_INFO, "VAR_CHECK_PCD_VARIABLE_HEADER\n"));
+  DEBUG ((EFI_D_INFO, "  Revision        - 0x%04x\n", PcdVariable->Revision));
+  DEBUG ((EFI_D_INFO, "  HeaderLength    - 0x%04x\n", PcdVariable->HeaderLength));
+  DEBUG ((EFI_D_INFO, "  Length          - 0x%08x\n", PcdVariable->Length));
+  DEBUG ((EFI_D_INFO, "  Type            - 0x%02x\n", PcdVariable->Type));
+  DEBUG ((EFI_D_INFO, "  Attributes      - 0x%08x\n", PcdVariable->Attributes));
+  DEBUG ((EFI_D_INFO, "  Guid            - %g\n", &PcdVariable->Guid));
+  DEBUG ((EFI_D_INFO, "  Name            - %s\n", PcdVariable + 1));
+
+  //
+  // For Pcd ValidData header align.
+  //
+  PcdValidData = (VAR_CHECK_PCD_VALID_DATA_HEADER *) HEADER_ALIGN (((UINTN) PcdVariable + PcdVariable->HeaderLength));
+  while ((UINTN) PcdValidData < ((UINTN) PcdVariable + PcdVariable->Length)) {
+    //
+    // Dump Pcd ValidData related to the Pcd Variable.
+    //
+    DumpPcdValidData (PcdValidData);
+    //
+    // For Pcd ValidData header align.
+    //
+    PcdValidData = (VAR_CHECK_PCD_VALID_DATA_HEADER *) HEADER_ALIGN (((UINTN) PcdValidData + PcdValidData->Length));
+  }
+}
+
+/**
+  Dump Var Check PCD.
+
+  @param[in] VarCheckPcdBin     Pointer to VarCheckPcdBin.
+  @param[in] VarCheckPcdBinSize VarCheckPcdBin size.
+
+**/
+VOID
+DumpVarCheckPcd (
+  IN VOID   *VarCheckPcdBin,
+  IN UINTN  VarCheckPcdBinSize
+  )
+{
+  VAR_CHECK_PCD_VARIABLE_HEADER     *PcdVariable;
+
+  DEBUG ((EFI_D_INFO, "DumpVarCheckPcd\n"));
+
+  //
+  // For Pcd Variable header align.
+  //
+  PcdVariable = (VAR_CHECK_PCD_VARIABLE_HEADER *) HEADER_ALIGN (VarCheckPcdBin);
+  while ((UINTN) PcdVariable < ((UINTN) VarCheckPcdBin + VarCheckPcdBinSize)) {
+    DumpPcdVariable (PcdVariable);
+    //
+    // For Pcd Variable header align.
+    //
+    PcdVariable = (VAR_CHECK_PCD_VARIABLE_HEADER *) HEADER_ALIGN (((UINTN) PcdVariable + PcdVariable->Length));
+  }
+}
+#endif
+
+/**
+  Locate VarCheckPcdBin.
+
+**/
+VOID
+EFIAPI
+LocateVarCheckPcdBin (
+  VOID
+  )
+{
+  EFI_STATUS                        Status;
+  VAR_CHECK_PCD_VARIABLE_HEADER     *VarCheckPcdBin;
+  UINTN                             VarCheckPcdBinSize;
+
+  //
+  // Search the VarCheckPcdBin from the first RAW section of current FFS.
+  //
+  Status = GetSectionFromFfs (
+             EFI_SECTION_RAW,
+             0,
+             (VOID **) &VarCheckPcdBin,
+             &VarCheckPcdBinSize
+             );
+  if (!EFI_ERROR (Status)) {
+    //
+    // AllocateRuntimeZeroPool () from MemoryAllocateLib is used for runtime access
+    // in SetVariable check handler.
+    //
+    mVarCheckPcdBin = AllocateRuntimeCopyPool (VarCheckPcdBinSize, VarCheckPcdBin);
+    ASSERT (mVarCheckPcdBin != NULL);
+    mVarCheckPcdBinSize = VarCheckPcdBinSize;
+    FreePool (VarCheckPcdBin);
+
+    DEBUG ((EFI_D_INFO, "VarCheckPcdBin - at 0x%x size = 0x%x\n", mVarCheckPcdBin, mVarCheckPcdBinSize));
+
+#ifdef DUMP_VAR_CHECK_PCD
+    DEBUG_CODE (
+      DumpVarCheckPcd (mVarCheckPcdBin, mVarCheckPcdBinSize);
+    );
+#endif
+  } else {
+    DEBUG ((EFI_D_INFO, "[VarCheckPcd] No VarCheckPcdBin found at the first RAW section\n"));
+  }
+}
+
+/**
+  Constructor function of VarCheckPcdLib to register var check PCD handler.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The constructor executed correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckPcdLibNullClassConstructor (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  LocateVarCheckPcdBin ();
+  VarCheckLibRegisterAddressPointer ((VOID **) &mVarCheckPcdBin);
+  VarCheckLibRegisterSetVariableCheckHandler (SetVariableCheckHandlerPcd);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdStructure.h b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdStructure.h
new file mode 100644
index 0000000000..8180bc5cfd
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdStructure.h
@@ -0,0 +1,76 @@
+/** @file
+  Internal structure for Var Check Pcd.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _VAR_CHECK_STRUCTURE_H_
+#define _VAR_CHECK_STRUCTURE_H_
+
+//
+// Alignment for PCD Variable and check data header.
+//
+#define HEADER_ALIGNMENT  4
+#define HEADER_ALIGN(Header)  (((UINTN) (Header) + HEADER_ALIGNMENT - 1) & (~(HEADER_ALIGNMENT - 1)))
+
+#pragma pack (1)
+
+#define VAR_CHECK_PCD_REVISION  0x0001
+
+typedef enum {
+  VarCheckPcdVariableHeader,
+  VarCheckPcdValidList,
+  VarCheckPcdValidRange,
+  VarCheckPcdCheckTypeMax,
+} VAR_CHECK_PCD_CHECK_TYPE;
+
+typedef struct {
+  UINT16            Revision;
+  UINT16            HeaderLength;
+  UINT32            Length; // Length include this header
+  UINT8             Type;
+  UINT8             Reserved[3];
+  UINT32            Attributes;
+  EFI_GUID          Guid;
+//CHAR16              Name[];
+} VAR_CHECK_PCD_VARIABLE_HEADER;
+
+typedef struct {
+  UINT8             Type;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+} VAR_CHECK_PCD_VALID_DATA_HEADER;
+
+typedef struct {
+  UINT8             Type;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+//UINTx               Data[]; // x = UINT8/UINT16/UINT32/UINT64;
+} VAR_CHECK_PCD_VALID_LIST;
+
+//typedef struct {
+//  UINTx             Minimum; // x = UINT8/UINT16/UINT32/UINT64
+//  UINTx             Maximum; // x = UINT8/UINT16/UINT32/UINT64
+//} VAR_CHECK_PCD_VALID_RANGE_DATA;
+
+typedef struct {
+  UINT8             Type;
+  UINT8             Length; // Length include this header
+  UINT16            VarOffset;
+  UINT8             StorageWidth;
+//  VAR_CHECK_PCD_VALID_RANGE_DATA    ValidRange[];
+} VAR_CHECK_PCD_VALID_RANGE;
+
+#pragma pack ()
+
+#endif
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf
new file mode 100644
index 0000000000..77ef210fb2
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf
@@ -0,0 +1,88 @@
+## @file
+#  NULL class library to register var check handler and variable property set for UEFI defined variables.
+#
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VarCheckUefiLib
+  MODULE_UNI_FILE                = VarCheckUefiLib.uni
+  FILE_GUID                      = AC24A4C7-F845-4665-90E5-6431D6E28DC0
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NULL|DXE_RUNTIME_DRIVER DXE_SMM_DRIVER
+  CONSTRUCTOR                    = VarCheckUefiLibNullClassConstructor
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  VarCheckUefiLibNullClass.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  VarCheckLib
+
+[Guids]
+  ## CONSUMES   ## Variable:L"LangCodes"
+  ## CONSUMES   ## Variable:L"Lang"
+  ## CONSUMES   ## Variable:L"Timeout"
+  ## CONSUMES   ## Variable:L"PlatformLangCodes"
+  ## CONSUMES   ## Variable:L"PlatformLang"
+  ## CONSUMES   ## Variable:L"ConIn"
+  ## CONSUMES   ## Variable:L"ConOut"
+  ## CONSUMES   ## Variable:L"ErrOut"
+  ## CONSUMES   ## Variable:L"ConInDev"
+  ## CONSUMES   ## Variable:L"ConOutDev"
+  ## CONSUMES   ## Variable:L"ErrOutDev"
+  ## CONSUMES   ## Variable:L"BootOrder"
+  ## CONSUMES   ## Variable:L"BootNext"
+  ## CONSUMES   ## Variable:L"BootCurrent"
+  ## CONSUMES   ## Variable:L"BootOptionSupport"
+  ## CONSUMES   ## Variable:L"DriverOrder"
+  ## CONSUMES   ## Variable:L"SysPrepOrder"
+  ## CONSUMES   ## Variable:L"HwErrRecSupport"
+  ## CONSUMES   ## Variable:L"SetupMode"
+  ## CONSUMES   ## Variable:L"PK"
+  ## CONSUMES   ## Variable:L"KEK"
+  ## CONSUMES   ## Variable:L"SignatureSupport"
+  ## CONSUMES   ## Variable:L"SecureBoot"
+  ## CONSUMES   ## Variable:L"KEKDefault"
+  ## CONSUMES   ## Variable:L"PKDefault"
+  ## CONSUMES   ## Variable:L"dbDefault"
+  ## CONSUMES   ## Variable:L"dbxDefault"
+  ## CONSUMES   ## Variable:L"dbtDefault"
+  ## CONSUMES   ## Variable:L"OsIndicationsSupported"
+  ## CONSUMES   ## Variable:L"OsIndications"
+  ## CONSUMES   ## Variable:L"VendorKeys"
+  ## CONSUMES   ## Variable:L"Boot####"
+  ## CONSUMES   ## Variable:L"Driver####"
+  ## CONSUMES   ## Variable:L"SysPrep####"
+  ## CONSUMES   ## Variable:L"Key####"
+  gEfiGlobalVariableGuid
+  ## CONSUMES   ## Variable:L"DB"
+  ## CONSUMES   ## Variable:L"DBX"
+  ## CONSUMES   ## Variable:L"DBT"
+  gEfiImageSecurityDatabaseGuid
+  gEfiHardwareErrorVariableGuid  ## CONSUMES    ## Variable:L"HwErrRec####"
diff --git a/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.uni b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.uni
new file mode 100644
index 0000000000..a18cbfa5a8
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.uni
diff --git a/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLibNullClass.c b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLibNullClass.c
new file mode 100644
index 0000000000..15144bd4c0
--- /dev/null
+++ b/Core/MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLibNullClass.c
@@ -0,0 +1,930 @@
+/** @file
+  Implementation functions and structures for var check uefi library.
+
+Copyright (c) 2015, 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 <Library/VarCheckLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <Guid/VariableFormat.h>
+#include <Guid/GlobalVariable.h>
+#include <Guid/HardwareErrorVariable.h>
+#include <Guid/ImageAuthentication.h>
+
+typedef
+EFI_STATUS
+(EFIAPI *INTERNAL_VAR_CHECK_FUNCTION) (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *Propery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  );
+
+typedef struct {
+  CHAR16                        *Name;
+  VAR_CHECK_VARIABLE_PROPERTY   VariableProperty;
+  INTERNAL_VAR_CHECK_FUNCTION   CheckFunction;
+} UEFI_DEFINED_VARIABLE_ENTRY;
+
+/**
+  Internal check for load option.
+
+  @param[in] VariablePropery    Pointer to variable property.
+  @param[in] DataSize           Data size.
+  @param[in] Data               Pointer to data buffer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER The data buffer is not a valid load option.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalVarCheckLoadOption (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariablePropery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  )
+{
+  UINT16                    FilePathListLength;
+  CHAR16                    *Description;
+  EFI_DEVICE_PATH_PROTOCOL  *FilePathList;
+
+  FilePathListLength = *((UINT16 *) ((UINTN) Data + sizeof (UINT32)));
+
+  //
+  // Check Description
+  //
+  Description = (CHAR16 *) ((UINTN) Data + sizeof (UINT32) + sizeof (UINT16));
+  while (Description < (CHAR16 *) ((UINTN) Data + DataSize)) {
+    if (*Description == L'\0') {
+      break;
+    }
+    Description++;
+  }
+  if ((UINTN) Description >= ((UINTN) Data + DataSize)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  Description++;
+
+  //
+  // Check FilePathList
+  //
+  FilePathList = (EFI_DEVICE_PATH_PROTOCOL *) Description;
+  if ((UINTN) FilePathList > (MAX_ADDRESS - FilePathListLength)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (((UINTN) FilePathList + FilePathListLength) > ((UINTN) Data + DataSize)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (FilePathListLength < sizeof (EFI_DEVICE_PATH_PROTOCOL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (!IsDevicePathValid (FilePathList, FilePathListLength)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal check for key option.
+
+  @param[in] VariablePropery    Pointer to variable property.
+  @param[in] DataSize           Data size.
+  @param[in] Data               Pointer to data buffer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER The data buffer is not a valid key option.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalVarCheckKeyOption (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariablePropery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  )
+{
+  if (((DataSize - sizeof (EFI_KEY_OPTION)) % sizeof (EFI_INPUT_KEY)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal check for device path.
+
+  @param[in] VariablePropery    Pointer to variable property.
+  @param[in] DataSize           Data size.
+  @param[in] Data               Pointer to data buffer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER The data buffer is not a valid device path.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalVarCheckDevicePath (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariablePropery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  )
+{
+  if (!IsDevicePathValid ((EFI_DEVICE_PATH_PROTOCOL *) Data, DataSize)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal check for ASCII string.
+
+  @param[in] VariablePropery    Pointer to variable property.
+  @param[in] DataSize           Data size.
+  @param[in] Data               Pointer to data buffer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER The data buffer is not a Null-terminated ASCII string.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalVarCheckAsciiString (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariablePropery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  )
+{
+  CHAR8     *String;
+  UINTN     Index;
+
+  String = (CHAR8 *) Data;
+  if (String[DataSize - 1] == '\0') {
+    return EFI_SUCCESS;
+  } else {
+    for (Index = 1; Index < DataSize && (String[DataSize - 1 - Index] != '\0'); Index++);
+    if (Index == DataSize) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Internal check for size array.
+
+  @param[in] VariablePropery    Pointer to variable property.
+  @param[in] DataSize           Data size.
+  @param[in] Data               Pointer to data buffer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER The DataSize is not size array.
+
+**/
+EFI_STATUS
+EFIAPI
+InternalVarCheckSizeArray (
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariablePropery,
+  IN UINTN                          DataSize,
+  IN VOID                           *Data
+  )
+{
+  if ((DataSize % VariablePropery->MinSize) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+//
+// To prevent name collisions with possible future globally defined variables,
+// other internal firmware data variables that are not defined here must be
+// saved with a unique VendorGuid other than EFI_GLOBAL_VARIABLE or
+// any other GUID defined by the UEFI Specification. Implementations must
+// only permit the creation of variables with a UEFI Specification-defined
+// VendorGuid when these variables are documented in the UEFI Specification.
+//
+UEFI_DEFINED_VARIABLE_ENTRY mGlobalVariableList[] = {
+  {
+    EFI_LANG_CODES_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    InternalVarCheckAsciiString
+  },
+  {
+    EFI_LANG_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    InternalVarCheckAsciiString
+  },
+  {
+    EFI_TIME_OUT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      sizeof (UINT16)
+    },
+    NULL
+  },
+  {
+    EFI_PLATFORM_LANG_CODES_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    InternalVarCheckAsciiString
+  },
+  {
+    EFI_PLATFORM_LANG_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    InternalVarCheckAsciiString
+  },
+  {
+    EFI_CON_IN_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_CON_OUT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_ERR_OUT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_CON_IN_DEV_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_CON_OUT_DEV_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_ERR_OUT_DEV_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (EFI_DEVICE_PATH_PROTOCOL),
+      MAX_UINTN
+    },
+    InternalVarCheckDevicePath
+  },
+  {
+    EFI_BOOT_ORDER_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckSizeArray
+  },
+  {
+    EFI_BOOT_NEXT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      sizeof (UINT16)
+    },
+    NULL
+  },
+  {
+    EFI_BOOT_CURRENT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT16),
+      sizeof (UINT16)
+    },
+    NULL
+  },
+  {
+    EFI_BOOT_OPTION_SUPPORT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT32),
+      sizeof (UINT32)
+    },
+    NULL
+  },
+  {
+    EFI_DRIVER_ORDER_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckSizeArray
+  },
+  {
+    EFI_SYS_PREP_ORDER_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckSizeArray
+  },
+  {
+    EFI_HW_ERR_REC_SUPPORT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT16),
+      sizeof (UINT16)
+    },
+    NULL
+  },
+  {
+    EFI_SETUP_MODE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT8),
+      sizeof (UINT8)
+    },
+    NULL
+  },
+  {
+    EFI_KEY_EXCHANGE_KEY_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT_AT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_PLATFORM_KEY_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT_AT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_SIGNATURE_SUPPORT_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (EFI_GUID),
+      MAX_UINTN
+    },
+    InternalVarCheckSizeArray
+  },
+  {
+    EFI_SECURE_BOOT_MODE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT8),
+      sizeof (UINT8)
+    },
+    NULL
+  },
+  {
+    EFI_KEK_DEFAULT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_PK_DEFAULT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_DB_DEFAULT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_DBX_DEFAULT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_DBT_DEFAULT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_OS_INDICATIONS_SUPPORT_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT64),
+      sizeof (UINT64)
+    },
+    NULL
+  },
+  {
+    EFI_OS_INDICATIONS_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT64),
+      sizeof (UINT64)
+    },
+    NULL
+  },
+  {
+    EFI_VENDOR_KEYS_VARIABLE_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_BS_RT,
+      sizeof (UINT8),
+      sizeof (UINT8)
+    },
+    NULL
+  },
+};
+
+UEFI_DEFINED_VARIABLE_ENTRY mGlobalVariableList2[] = {
+  {
+    L"Boot####",
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT32) + sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckLoadOption
+  },
+  {
+    L"Driver####",
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT32) + sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckLoadOption
+  },
+  {
+    L"SysPrep####",
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (UINT32) + sizeof (UINT16),
+      MAX_UINTN
+    },
+    InternalVarCheckLoadOption
+  },
+  {
+    L"Key####",
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (EFI_KEY_OPTION),
+      sizeof (EFI_KEY_OPTION) + 3 * sizeof (EFI_INPUT_KEY)
+    },
+    InternalVarCheckKeyOption
+  },
+};
+
+//
+// EFI_IMAGE_SECURITY_DATABASE_GUID
+//
+UEFI_DEFINED_VARIABLE_ENTRY mImageSecurityVariableList[] = {
+  {
+    EFI_IMAGE_SECURITY_DATABASE,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT_AT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_IMAGE_SECURITY_DATABASE1,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT_AT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+  {
+    EFI_IMAGE_SECURITY_DATABASE2,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      0,
+      VARIABLE_ATTRIBUTE_NV_BS_RT_AT,
+      1,
+      MAX_UINTN
+    },
+    NULL
+  },
+};
+
+//
+// EFI_HARDWARE_ERROR_VARIABLE
+//
+UEFI_DEFINED_VARIABLE_ENTRY mHwErrRecVariable = {
+  L"HwErrRec####",
+  {
+    VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+    0,
+    VARIABLE_ATTRIBUTE_NV_BS_RT_HR,
+    1,
+    MAX_UINTN
+  },
+  NULL
+};
+
+EFI_GUID *mUefiDefinedGuid[] = {
+  &gEfiGlobalVariableGuid,
+  &gEfiImageSecurityDatabaseGuid,
+  &gEfiHardwareErrorVariableGuid
+};
+
+/**
+  Check if a Unicode character is a hexadecimal character.
+
+  This function checks if a Unicode character is a
+  hexadecimal character.  The valid hexadecimal character is
+  L'0' to L'9', L'a' to L'f', or L'A' to L'F'.
+
+
+  @param[in] Char       The character to check against.
+
+  @retval TRUE          If the Char is a hexadecmial character.
+  @retval FALSE         If the Char is not a hexadecmial character.
+
+**/
+BOOLEAN
+EFIAPI
+VarCheckUefiIsHexaDecimalDigitCharacter (
+  IN CHAR16             Char
+  )
+{
+  return (BOOLEAN) ((Char >= L'0' && Char <= L'9') || (Char >= L'A' && Char <= L'F') || (Char >= L'a' && Char <= L'f'));
+}
+
+/**
+
+  This code checks if variable is hardware error record variable or not.
+
+  According to UEFI spec, hardware error record variable should use the EFI_HARDWARE_ERROR_VARIABLE VendorGuid
+  and have the L"HwErrRec####" name convention, #### is a printed hex value and no 0x or h is included in the hex value.
+
+  @param[in] VariableName   Pointer to variable name.
+  @param[in] VendorGuid     Variable Vendor Guid.
+
+  @retval TRUE              Variable is hardware error record variable.
+  @retval FALSE             Variable is not hardware error record variable.
+
+**/
+BOOLEAN
+EFIAPI
+IsHwErrRecVariable (
+  IN CHAR16             *VariableName,
+  IN EFI_GUID           *VendorGuid
+  )
+{
+  if (!CompareGuid (VendorGuid, &gEfiHardwareErrorVariableGuid) ||
+      (StrLen (VariableName) != StrLen (L"HwErrRec####")) ||
+      (StrnCmp(VariableName, L"HwErrRec", StrLen (L"HwErrRec")) != 0) ||
+      !VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[0x8]) ||
+      !VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[0x9]) ||
+      !VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[0xA]) ||
+      !VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[0xB])) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Get UEFI defined var check function.
+
+  @param[in]  VariableName      Pointer to variable name.
+  @param[in]  VendorGuid        Pointer to variable vendor GUID.
+  @param[out] VariableProperty  Pointer to variable property.
+
+  @return Internal var check function, NULL if no specific check function.
+
+**/
+INTERNAL_VAR_CHECK_FUNCTION
+GetUefiDefinedVarCheckFunction (
+  IN CHAR16                         *VariableName,
+  IN EFI_GUID                       *VendorGuid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   **VariableProperty
+  )
+{
+  UINTN     Index;
+  UINTN     NameLength;
+
+  if (CompareGuid (VendorGuid, &gEfiGlobalVariableGuid)) {
+    //
+    // Try list 1, exactly match.
+    //
+    for (Index = 0; Index < sizeof (mGlobalVariableList)/sizeof (mGlobalVariableList[0]); Index++) {
+      if (StrCmp (mGlobalVariableList[Index].Name, VariableName) == 0) {
+        *VariableProperty = &(mGlobalVariableList[Index].VariableProperty);
+        return mGlobalVariableList[Index].CheckFunction;
+      }
+    }
+
+    //
+    // Try list 2.
+    //
+    NameLength = StrLen (VariableName) - 4;
+    for (Index = 0; Index < sizeof (mGlobalVariableList2)/sizeof (mGlobalVariableList2[0]); Index++) {
+      if ((StrLen (VariableName) == StrLen (mGlobalVariableList2[Index].Name)) &&
+          (StrnCmp (VariableName, mGlobalVariableList2[Index].Name, NameLength) == 0) &&
+          VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[NameLength]) &&
+          VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[NameLength + 1]) &&
+          VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[NameLength + 2]) &&
+          VarCheckUefiIsHexaDecimalDigitCharacter (VariableName[NameLength + 3])) {
+        *VariableProperty = &(mGlobalVariableList2[Index].VariableProperty);
+        return mGlobalVariableList2[Index].CheckFunction;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  SetVariable check handler UEFI defined.
+
+  @param[in] VariableName       Name of Variable to set.
+  @param[in] VendorGuid         Variable vendor GUID.
+  @param[in] Attributes         Attribute value of the variable.
+  @param[in] DataSize           Size of Data to set.
+  @param[in] Data               Data pointer.
+
+  @retval EFI_SUCCESS           The SetVariable check result was success.
+  @retval EFI_INVALID_PARAMETER An invalid combination of attribute bits, name, GUID,
+                                DataSize and Data value was supplied.
+  @retval EFI_WRITE_PROTECTED   The variable in question is read-only.
+
+**/
+EFI_STATUS
+EFIAPI
+SetVariableCheckHandlerUefiDefined (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  VAR_CHECK_VARIABLE_PROPERTY   Property;
+  VAR_CHECK_VARIABLE_PROPERTY   *VarCheckProperty;
+  INTERNAL_VAR_CHECK_FUNCTION   VarCheckFunction;
+
+  if ((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0)) || (Attributes == 0)) {
+    //
+    // Do not check delete variable.
+    //
+    return EFI_SUCCESS;
+  }
+
+  if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+    if (!IsHwErrRecVariable (VariableName, VendorGuid)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  for (Index = 0; Index < sizeof (mUefiDefinedGuid)/sizeof (mUefiDefinedGuid[0]); Index++) {
+    if (CompareGuid (VendorGuid, mUefiDefinedGuid[Index])) {
+      if (VarCheckLibVariablePropertyGet (VariableName, VendorGuid, &Property) == EFI_NOT_FOUND) {
+        //
+        // To prevent name collisions with possible future globally defined variables,
+        // other internal firmware data variables that are not defined here must be
+        // saved with a unique VendorGuid other than EFI_GLOBAL_VARIABLE or
+        // any other GUID defined by the UEFI Specification. Implementations must
+        // only permit the creation of variables with a UEFI Specification-defined
+        // VendorGuid when these variables are documented in the UEFI Specification.
+        //
+        DEBUG ((EFI_D_INFO, "UEFI Variable Check fail %r - %s not in %g namespace\n", EFI_INVALID_PARAMETER, VariableName, VendorGuid));
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+  }
+
+  if (DataSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  VarCheckProperty = NULL;
+  VarCheckFunction = GetUefiDefinedVarCheckFunction (VariableName, VendorGuid, &VarCheckProperty);
+  if (VarCheckFunction != NULL) {
+    Status = VarCheckFunction (
+               VarCheckProperty,
+               DataSize,
+               Data
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "UEFI Variable Check function fail %r - %g:%s\n", Status, VendorGuid, VariableName));
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Variable property set for UEFI defined variables.
+
+**/
+VOID
+VariablePropertySetUefiDefined (
+  VOID
+  )
+{
+  UINTN     Index;
+
+  //
+  // EFI_GLOBAL_VARIABLE
+  //
+  for (Index = 0; Index < sizeof (mGlobalVariableList)/sizeof (mGlobalVariableList[0]); Index++) {
+    VarCheckLibVariablePropertySet (
+      mGlobalVariableList[Index].Name,
+      &gEfiGlobalVariableGuid,
+      &mGlobalVariableList[Index].VariableProperty
+      );
+  }
+  for (Index = 0; Index < sizeof (mGlobalVariableList2)/sizeof (mGlobalVariableList2[0]); Index++) {
+    VarCheckLibVariablePropertySet (
+      mGlobalVariableList2[Index].Name,
+      &gEfiGlobalVariableGuid,
+      &mGlobalVariableList2[Index].VariableProperty
+      );
+  }
+
+  //
+  // EFI_IMAGE_SECURITY_DATABASE_GUID
+  //
+  for (Index = 0; Index < sizeof (mImageSecurityVariableList)/sizeof (mImageSecurityVariableList[0]); Index++) {
+    VarCheckLibVariablePropertySet (
+      mImageSecurityVariableList[Index].Name,
+      &gEfiImageSecurityDatabaseGuid,
+      &mImageSecurityVariableList[Index].VariableProperty
+      );
+  }
+
+  //
+  // EFI_HARDWARE_ERROR_VARIABLE
+  //
+  VarCheckLibVariablePropertySet (
+    mHwErrRecVariable.Name,
+    &gEfiHardwareErrorVariableGuid,
+    &mHwErrRecVariable.VariableProperty
+    );
+}
+
+/**
+  Constructor function of VarCheckUefiLib to set property and
+  register SetVariable check handler for UEFI defined variables.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The constructor executed correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckUefiLibNullClassConstructor (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  VariablePropertySetUefiDefined ();
+  VarCheckLibRegisterSetVariableCheckHandler (SetVariableCheckHandlerUefiDefined);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/License.txt b/Core/MdeModulePkg/License.txt
new file mode 100644
index 0000000000..be68999be6
--- /dev/null
+++ b/Core/MdeModulePkg/License.txt
@@ -0,0 +1,25 @@
+Copyright (c) 2012, Intel Corporation. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+* Redistributions of source code must retain the above copyright
+  notice, this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in
+  the documentation and/or other materials provided with the
+  distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/Core/MdeModulePkg/Logo/Logo.bmp b/Core/MdeModulePkg/Logo/Logo.bmp
new file mode 100644
index 0000000000..3e85229e17
--- /dev/null
+++ b/Core/MdeModulePkg/Logo/Logo.bmp
diff --git a/Core/MdeModulePkg/Logo/Logo.inf b/Core/MdeModulePkg/Logo/Logo.inf
new file mode 100644
index 0000000000..e2e61c82c5
--- /dev/null
+++ b/Core/MdeModulePkg/Logo/Logo.inf
@@ -0,0 +1,34 @@
+## @file
+#  The default logo bitmap picture shown on setup screen, which is corresponding to gEfiDefaultBmpLogoGuid.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Logo
+  MODULE_UNI_FILE                = Logo.uni
+  FILE_GUID                      = 7BB28B99-61BB-11D5-9A5D-0090273FC14D
+  MODULE_TYPE                    = USER_DEFINED
+  VERSION_STRING                 = 1.0
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+
+[Binaries]
+  BIN|Logo.bmp|*
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  LogoExtra.uni
diff --git a/Core/MdeModulePkg/Logo/Logo.uni b/Core/MdeModulePkg/Logo/Logo.uni
new file mode 100644
index 0000000000..aa15048a31
--- /dev/null
+++ b/Core/MdeModulePkg/Logo/Logo.uni
diff --git a/Core/MdeModulePkg/Logo/LogoExtra.uni b/Core/MdeModulePkg/Logo/LogoExtra.uni
new file mode 100644
index 0000000000..8b27ec593a
--- /dev/null
+++ b/Core/MdeModulePkg/Logo/LogoExtra.uni
diff --git a/Core/MdeModulePkg/MdeModulePkg.dec b/Core/MdeModulePkg/MdeModulePkg.dec
new file mode 100644
index 0000000000..a1f6bb6897
--- /dev/null
+++ b/Core/MdeModulePkg/MdeModulePkg.dec
@@ -0,0 +1,1501 @@
+## @file  MdeModulePkg.dec
+# This package provides the modules that conform to UEFI/PI Industry standards.
+# It also provides the definitions(including PPIs/PROTOCOLs/GUIDs and library classes)
+# and libraries instances, which are used for those modules.
+#
+# Copyright (c) 2007 - 2016, 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 that 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.
+#
+##
+
+
+[Defines]
+  DEC_SPECIFICATION              = 0x00010005
+  PACKAGE_NAME                   = MdeModulePkg
+  PACKAGE_UNI_FILE               = MdeModulePkg.uni
+  PACKAGE_GUID                   = BA0D78D6-2CAF-414b-BD4D-B6762A894288
+  PACKAGE_VERSION                = 0.96
+
+[Includes]
+  Include
+
+
+[LibraryClasses]
+  ##  @libraryclass  IpIo layer upon EFI IP4 Protocol.
+  #   This library is only intended to be used by UEFI network stack modules.
+  IpIoLib|Include/Library/IpIoLib.h
+
+  ##  @libraryclass  Basic function for UEFI network stack.
+  #   This library is only intended to be used by UEFI network stack modules.
+  NetLib|Include/Library/NetLib.h
+
+  ##  @libraryclass  The helper routines to access UDP service.
+  #   This library is only intended to be used by UEFI network stack modules.
+  UdpIoLib|Include/Library/UdpIoLib.h
+
+  ##  @libraryclass  The helper routines to access TCP service.
+  #   This library is only intended to be used by UEFI network stack modules.
+  TcpIoLib|Include/Library/TcpIoLib.h
+
+  ##  @libraryclass  The helper routines to access HTTP service.
+  #   This library is only intended to be used by UEFI network stack modules.
+  HttpLib|Include/Library/HttpLib.h
+
+  ##  @libraryclass  Defines a set of methods to reset whole system.
+  ResetSystemLib|Include/Library/ResetSystemLib.h
+
+  ##  @libraryclass  Defines a set of methods related do S3 mode.
+  #   This library class is no longer used and modules using this library should
+  #   directly locate EFI_PEI_S3_RESUME_PPI defined in PI 1.2 specification.
+  S3Lib|Include/Library/S3Lib.h
+
+  ##  @libraryclass  Defines a set of methods related recovery mode.
+  #   This library class is no longer used and modules using this library should
+  #   directly locate EFI_PEI_RECOVERY_MODULE_PPI defined in PI 1.2 specification.
+  RecoveryLib|Include/Library/RecoveryLib.h
+
+  ##  @libraryclass  Provides HII related functions.
+  HiiLib|Include/Library/HiiLib.h
+
+  ##  @libraryclass  Defines a set of interfaces on how to process capusle image update.
+  CapsuleLib|Include/Library/CapsuleLib.h
+
+  ##  @libraryclass  Library for Deferred Procedure Calls.
+  DpcLib|Include/Library/DpcLib.h
+
+  ##  @libraryclass    Provides global variables that are pointers
+  #   to the UEFI HII related protocols.
+  #
+  UefiHiiServicesLib|Include/Library/UefiHiiServicesLib.h
+
+  ##  @libraryclass    Provides a set of interfaces to abstract the policy of security measurement.
+  #
+  SecurityManagementLib|Include/Library/SecurityManagementLib.h
+
+  ##  @libraryclass  OEM status code libary is used to report status code to OEM device.
+  #
+  OemHookStatusCodeLib|Include/Library/OemHookStatusCodeLib.h
+
+  ##  @libraryclass  Debug Agent is used to provide soft debug capability.
+  #
+  DebugAgentLib|Include/Library/DebugAgentLib.h
+
+  ##  @libraryclass  Provide platform specific hooks.
+  #
+  PlatformHookLib|Include/Library/PlatformHookLib.h
+
+  ##  @libraryclass  Provide platform specific hooks for SMM core.
+  #
+  SmmCorePlatformHookLib|Include/Library/SmmCorePlatformHookLib.h
+
+  ##  @libraryclass  Provide capability to maintain the data integrity cross S3 phase.
+  #
+  LockBoxLib|Include/Library/LockBoxLib.h
+
+  ##  @libraryclass  Provide the CPU exception handler.
+  #
+  CpuExceptionHandlerLib|Include/Library/CpuExceptionHandlerLib.h
+
+  ##  @libraryclass    Provides platform specific display interface.
+  #
+  CustomizedDisplayLib|Include/Library/CustomizedDisplayLib.h
+
+  ## @libraryclass   Provides sorting functions
+  SortLib|Include/Library/SortLib.h
+
+  ## @libraryclass   Provides core boot manager functions
+  UefiBootManagerLib|Include/Library/UefiBootManagerLib.h
+
+  ## @libraryclass   Provides core boot manager functions
+  PlatformBootManagerLib|Include/Library/PlatformBootManagerLib.h
+
+  ## @libraryclass  Provides common interfaces about TPM measurement for other modules.
+  #
+  TpmMeasurementLib|Include/Library/TpmMeasurementLib.h
+
+  ## @libraryclass  Provides authenticated variable services.
+  #
+  AuthVariableLib|Include/Library/AuthVariableLib.h
+
+  ## @libraryclass  Provides variable check services and database management.
+  #
+  VarCheckLib|Include/Library/VarCheckLib.h
+
+  ## @libraryclass  Provides services to get variable error flag and do platform variable cleanup.
+  #
+  PlatformVarCleanupLib|Include/Library/PlatformVarCleanupLib.h
+
+  ## @libraryclass  Provides interfaces about Ipmi submit generic commond.
+  #
+  IpmiLib|Include/Library/IpmiLib.h
+
+  ## @libraryclass  Provides interfaces for platform to return root bridge information to PciHostBridgeDxe driver.
+  #
+  PciHostBridgeLib|Include/Library/PciHostBridgeLib.h
+
+[Guids]
+  ## MdeModule package token space guid
+  # Include/Guid/MdeModulePkgTokenSpace.h
+  gEfiMdeModulePkgTokenSpaceGuid       = { 0xA1AFF049, 0xFDEB, 0x442a, { 0xB3, 0x20, 0x13, 0xAB, 0x4C, 0xB7, 0x2B, 0xBC }}
+
+  ## Hob guid for Pcd DataBase
+  #  Include/Guid/PcdDataBaseHobGuid.h
+  gPcdDataBaseHobGuid            = { 0xEA296D92, 0x0B69, 0x423C, { 0x8C, 0x28, 0x33, 0xB4, 0xE0, 0xA9, 0x12, 0x68 }}
+
+  ## Guid for PCD DataBase signature.
+  # Include/Guid/PcdDataBaseSignatureGuid.h
+  gPcdDataBaseSignatureGuid = { 0x3c7d193c, 0x682c, 0x4c14, { 0xa6, 0x8f, 0x55, 0x2d, 0xea, 0x4f, 0x43, 0x7e }}
+
+  ## Guid for EDKII implementation GUIDed opcodes
+  #  Include/Guid/MdeModuleHii.h
+  gEfiIfrTianoGuid      = { 0xf0b1735, 0x87a0, 0x4193, {0xb2, 0x66, 0x53, 0x8c, 0x38, 0xaf, 0x48, 0xce }}
+
+  ## Guid for Framework vfr GUIDed opcodes.
+  #  Include/Guid/MdeModuleHii.h
+  gEfiIfrFrameworkGuid  = { 0x31ca5d1a, 0xd511, 0x4931, { 0xb7, 0x82, 0xae, 0x6b, 0x2b, 0x17, 0x8c, 0xd7 }}
+
+  ## Guid to specify the System Non Volatile FV
+  #  Include/Guid/SystemNvDataGuid.h
+  gEfiSystemNvDataFvGuid         = { 0xFFF12B8D, 0x7696, 0x4C8B, { 0xA9, 0x85, 0x27, 0x47, 0x07, 0x5B, 0x4F, 0x50 }}
+
+  ## GUID used as the signature of FTW working block header.
+  #  Include/Guid/SystemNvDataGuid.h
+  gEdkiiWorkingBlockSignatureGuid   = { 0x9e58292b, 0x7c68, 0x497d, { 0xa0, 0xce, 0x65,  0x0, 0xfd, 0x9f, 0x1b, 0x95 }}
+
+  ## GUID used to build FTW last write data hob and install PPI to inform the check for FTW last write data has been done.
+  #  Include/Guid/FaultTolerantWrite.h
+  gEdkiiFaultTolerantWriteGuid      = { 0x1d3e9cb8, 0x43af, 0x490b, { 0x83,  0xa, 0x35, 0x16, 0xaa, 0x53, 0x20, 0x47 }}
+
+  ## Guid specify the device is the console out device.
+  #  Include/Guid/ConsoleOutDevice.h
+  gEfiConsoleOutDeviceGuid       = { 0xD3B36F2C, 0xD551, 0x11D4, { 0x9A, 0x46, 0x00, 0x90, 0x27, 0x3F, 0xC1, 0x4D }}
+
+  ## Guid specify the device is the console in device.
+  #  Include/Guid/ConsoleInDevice.h
+  gEfiConsoleInDeviceGuid        = { 0xD3B36F2B, 0xD551, 0x11D4, { 0x9A, 0x46, 0x00, 0x90, 0x27, 0x3F, 0xC1, 0x4D }}
+
+  ## Hob and Variable guid specify the platform memory type information.
+  #  Include/Guid/MemoryTypeInformation.h
+  gEfiMemoryTypeInformationGuid  = { 0x4C19049F, 0x4137, 0x4DD3, { 0x9C, 0x10, 0x8B, 0x97, 0xA8, 0x3F, 0xFD, 0xFA }}
+
+  ## Capsule update hob and variable guid
+  #  Include/Guid/CapsuleVendor.h
+  gEfiCapsuleVendorGuid          = { 0x711C703F, 0xC285, 0x4B10, { 0xA3, 0xB0, 0x36, 0xEC, 0xBD, 0x3C, 0x8B, 0xE2 }}
+
+  ## Guid specifiy the device is the StdErr device.
+  #  Include/Guid/StandardErrorDevice.h
+  gEfiStandardErrorDeviceGuid    = { 0xD3B36F2D, 0xD551, 0x11D4, { 0x9A, 0x46, 0x00, 0x90, 0x27, 0x3F, 0xC1, 0x4D }}
+
+  ## Guid acted as variable store header's signature and to specify the variable list entries put in the EFI system table.
+  #  Include/Guid/VariableFormat.h
+  gEfiVariableGuid           = { 0xddcf3616, 0x3275, 0x4164, { 0x98, 0xb6, 0xfe, 0x85, 0x70, 0x7f, 0xfe, 0x7d }}
+
+  ## Guid acted as the authenticated variable store header's signature, and to specify the variable list entries put in the EFI system table.
+  #  Include/Guid/AuthenticatedVariableFormat.h
+  gEfiAuthenticatedVariableGuid = { 0xaaf32c78, 0x947b, 0x439a, { 0xa1, 0x80, 0x2e, 0x14, 0x4e, 0xc3, 0x77, 0x92 } }
+
+  #  Include/Guid/VariableIndexTable.h
+  gEfiVariableIndexTableGuid  = { 0x8cfdb8c8, 0xd6b2, 0x40f3, { 0x8e, 0x97, 0x02, 0x30, 0x7c, 0xc9, 0x8b, 0x7c }}
+
+  ## Guid is defined for SMM variable module to notify SMM variable wrapper module when variable write service was ready.
+  #  Include/Guid/SmmVariableCommon.h
+  gSmmVariableWriteGuid  = { 0x93ba1826, 0xdffb, 0x45dd, { 0x82, 0xa7, 0xe7, 0xdc, 0xaa, 0x3b, 0xbd, 0xf3 }}
+
+  ## Performance protocol guid that also acts as the performance HOB guid and performance variable GUID
+  #  Include/Guid/Performance.h
+  gPerformanceProtocolGuid       = { 0x76B6BDFA, 0x2ACD, 0x4462, { 0x9E, 0x3F, 0xCB, 0x58, 0xC9, 0x69, 0xD9, 0x37 } }
+  gSmmPerformanceProtocolGuid    = { 0xf866226a, 0xeaa5, 0x4f5a, { 0xa9, 0xa,  0x6c, 0xfb, 0xa5, 0x7c, 0x58, 0x8e } }
+  gPerformanceExProtocolGuid     = { 0x1ea81bec, 0xf01a, 0x4d98, { 0xa2, 0x1,  0x4a, 0x61, 0xce, 0x2f, 0xc0, 0x22 } }
+  gSmmPerformanceExProtocolGuid  = { 0x931fc048, 0xc71d, 0x4455, { 0x89, 0x30, 0x47, 0x6,  0x30, 0xe3, 0xe,  0xe5 } }
+
+  ## Guid is defined for CRC32 encapsulation scheme.
+  #  Include/Guid/Crc32GuidedSectionExtraction.h
+  gEfiCrc32GuidedSectionExtractionGuid = { 0xFC1BCDB0, 0x7D31, 0x49aa, {0x93, 0x6A, 0xA4, 0x60, 0x0D, 0x9D, 0xD0, 0x83 } }
+
+  ## Include/Guid/StatusCodeCallbackGuid.h
+  gStatusCodeCallbackGuid   = {0xe701458c, 0x4900, 0x4ca5, {0xb7, 0x72, 0x3d, 0x37, 0x94, 0x9f, 0x79, 0x27}}
+
+  ## GUID identifies status code records HOB that originate from the PEI status code
+  #  Include/Guid/MemoryStatusCodeRecord.h
+  gMemoryStatusCodeRecordGuid     = { 0x060CC026, 0x4C0D, 0x4DDA, { 0x8F, 0x41, 0x59, 0x5F, 0xEF, 0x00, 0xA5, 0x02 }}
+
+  ## GUID used to pass DEBUG() macro information through the Status Code Protocol and Status Code PPI
+  #  Include/Guid/StatusCodeDataTypeDebug.h
+  gEfiStatusCodeDataTypeDebugGuid  = { 0x9A4E9246, 0xD553, 0x11D5, { 0x87, 0xE2, 0x00, 0x06, 0x29, 0x45, 0xC3, 0xB9 }}
+
+  ##  A configuration Table Guid for Load module at fixed address
+  #  Include/Guid/LoadModuleAtFixedAddress.h
+  gLoadFixedAddressConfigurationTableGuid  = { 0x2CA88B53,0xD296,0x4080, { 0xA4,0xA5,0xCA,0xD9,0xBA,0xE2,0x4B,0x9 } }
+
+  ## GUID used to store the global debug mask value into an EFI Variable
+  # Include/Guid/DebugMask.h
+  gEfiGenericVariableGuid = { 0x59d1c24f, 0x50f1, 0x401a, {0xb1, 0x01, 0xf3, 0x3e, 0x0d, 0xae, 0xd4, 0x43} }
+
+  ##  Event for the DXE Core to signal idle events
+  #  Include/Guid/EventIdle.h
+  gIdleLoopEventGuid  = { 0x3c8d294c, 0x5fc3, 0x4451, { 0xbb, 0x31, 0xc4, 0xc0, 0x32, 0x29, 0x5e, 0x6c } }
+
+  ## Include/Guid/RecoveryDevice.h
+  gRecoveryOnFatUsbDiskGuid          = { 0x0FFBCE19, 0x324C, 0x4690, { 0xA0, 0x09, 0x98, 0xC6, 0xAE, 0x2E, 0xB1, 0x86 }}
+
+  ## Include/Guid/RecoveryDevice.h
+  gRecoveryOnFatIdeDiskGuid          = { 0xB38573B6, 0x6200, 0x4AC5, { 0xB5, 0x1D, 0x82, 0xE6, 0x59, 0x38, 0xD7, 0x83 }}
+
+  ## Include/Guid/RecoveryDevice.h
+  gRecoveryOnFatFloppyDiskGuid       = { 0x2E3D2E75, 0x9B2E, 0x412D, { 0xB4, 0xB1, 0x70, 0x41, 0x6B, 0x87, 0x00, 0xFF }}
+
+  ## Include/Guid/RecoveryDevice.h
+  gRecoveryOnDataCdGuid              = { 0x5CAC0099, 0x0DC9, 0x48E5, { 0x80, 0x68, 0xBB, 0x95, 0xF5, 0x40, 0x0A, 0x9F }}
+
+  ## Include/Guid/SmmLockBox.h
+  gEfiSmmLockBoxCommunicationGuid    = { 0x2a3cfebd, 0x27e8, 0x4d0a, { 0x8b, 0x79, 0xd6, 0x88, 0xc2, 0xa3, 0xe1, 0xc0 }}
+
+  ## Include/Guid/AcpiS3Context.h
+  gEfiAcpiVariableGuid               = { 0xAF9FFD67, 0xEC10, 0x488A, { 0x9D, 0xFC, 0x6C, 0xBF, 0x5E, 0xE2, 0x2C, 0x2E }}
+
+  ## Include/Guid/AcpiS3Context.h
+  gEfiAcpiS3ContextGuid              = { 0xef98d3a, 0x3e33, 0x497a, { 0xa4, 0x1, 0x77, 0xbe, 0x3e, 0xb7, 0x4f, 0x38 }}
+
+  ## Include/Guid/BootScriptExecutorVariable.h
+  gEfiBootScriptExecutorVariableGuid = { 0x3079818c, 0x46d4, 0x4a73, { 0xae, 0xf3, 0xe3, 0xe4, 0x6c, 0xf1, 0xee, 0xdb }}
+  gEfiBootScriptExecutorContextGuid  = { 0x79cb58c4, 0xac51, 0x442f, { 0xaf, 0xd7, 0x98, 0xe4, 0x7d, 0x2e, 0x99, 0x8 }}
+
+  ## Include/Guid/UsbKeyBoardLayout.h
+  gUsbKeyboardLayoutPackageGuid      = { 0xc0f3b43, 0x44de, 0x4907, { 0xb4, 0x78, 0x22, 0x5f, 0x6f, 0x62, 0x89, 0xdc }}
+  gUsbKeyboardLayoutKeyGuid          = { 0x3a4d7a7c, 0x18a, 0x4b42, { 0x81, 0xb3, 0xdc, 0x10, 0xe3, 0xb5, 0x91, 0xbd }}
+
+  ## Include/Guid/HiiResourceSampleHii.h
+  gHiiResourceSamleFormSetGuid       = { 0x4f4ef7f0, 0xaa29, 0x4ce9, { 0xba, 0x41, 0x64, 0x3e, 0x1, 0x23, 0xa9, 0x9f }}
+
+  ## Include/Guid/DriverSampleHii.h
+  gDriverSampleFormSetGuid           = { 0xA04A27f4, 0xDF00, 0x4D42, { 0xB5, 0x52, 0x39, 0x51, 0x13, 0x02, 0x11, 0x3D }}
+  gDriverSampleInventoryGuid         = { 0xb3f56470, 0x6141, 0x4621, { 0x8f, 0x19, 0x70, 0x4e, 0x57, 0x7a, 0xa9, 0xe8 }}
+  gEfiIfrRefreshIdOpGuid             = { 0xF5E655D9, 0x02A6, 0x46f2, { 0x9E, 0x76, 0xB8, 0xBE, 0x8E, 0x60, 0xAB, 0x22 }}
+
+  ## Include/Guid/PlatDriOverrideHii.h
+  gPlatformOverridesManagerGuid      = { 0x8614567d, 0x35be, 0x4415, { 0x8d, 0x88, 0xbd, 0x7d, 0xc, 0x9c, 0x70, 0xc0 }}
+
+  ## Include/Guid/Ip4Config2Hii.h
+  gIp4Config2NvDataGuid              = { 0x9b942747, 0x154e, 0x4d29, { 0xa4, 0x36, 0xbf, 0x71, 0x0, 0xc8, 0xb5, 0x3b }}
+
+  ## Include/Guid/VlanConfigHii.h
+  gVlanConfigFormSetGuid             = { 0xd79df6b0, 0xef44, 0x43bd, { 0x97, 0x97, 0x43, 0xe9, 0x3b, 0xcf, 0x5f, 0xa8 }}
+
+  ## Include/Guid/Ip4IScsiConfigHii.h
+  gIp4IScsiConfigGuid                = { 0x6456ed61, 0x3579, 0x41c9, { 0x8a, 0x26, 0x0a, 0x0b, 0xd6, 0x2b, 0x78, 0xfc }}
+  gIScsiCHAPAuthInfoGuid             = { 0x786ec0ac, 0x65ae, 0x4d1b, { 0xb1, 0x37, 0xd, 0x11, 0xa, 0x48, 0x37, 0x97 }}
+
+  ## Include/Guid/ZeroGuid.h
+  gZeroGuid                          = { 0x0, 0x0, 0x0, {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}}
+
+  ## Include/Guid/MtcVendor.h
+  gMtcVendorGuid                     = { 0xeb704011, 0x1402, 0x11d3, { 0x8e, 0x77, 0x0, 0xa0, 0xc9, 0x69, 0x72, 0x3b }}
+
+  ## Guid for Firmware Performance Data Table (FPDT) implementation.
+  #  Include/Guid/FirmwarePerformance.h
+  gEfiFirmwarePerformanceGuid        = { 0xc095791a, 0x3001, 0x47b2, { 0x80, 0xc9, 0xea, 0xc7, 0x31, 0x9f, 0x2f, 0xa4 }}
+  gFirmwarePerformanceS3PointerGuid  = { 0xdc65adc,  0xa973, 0x4130, { 0x8d, 0xf0, 0x2a, 0xdb, 0xeb, 0x9e, 0x4a, 0x31 }}
+
+  ## Include/Guid/ExitBootServiceFailed.h
+  gEventExitBootServicesFailedGuid   = { 0x4f6c5507, 0x232f, 0x4787, { 0xb9, 0x5e, 0x72, 0xf8, 0x62, 0x49, 0xc, 0xb1 } }
+
+  ## Include/Guid/ConnectConInEvent.h
+  gConnectConInEventGuid             = { 0xdb4e8151, 0x57ed, 0x4bed, { 0x88, 0x33, 0x67, 0x51, 0xb5, 0xd1, 0xa8, 0xd7 }}
+
+  ## Include/Guid/StatusCodeDataTypeVariable.h
+  gEdkiiStatusCodeDataTypeVariableGuid = { 0xf6ee6dbb, 0xd67f, 0x4ea0, { 0x8b, 0x96, 0x6a, 0x71, 0xb1, 0x9d, 0x84, 0xad }}
+
+  ## Include/Guid/MemoryProfile.h
+  gEdkiiMemoryProfileGuid              = { 0x821c9a09, 0x541a, 0x40f6, { 0x9f, 0x43, 0xa, 0xd1, 0x93, 0xa1, 0x2c, 0xfe }}
+
+  ## Include/Protocol/VarErrorFlag.h
+  gEdkiiVarErrorFlagGuid               = { 0x4b37fe8, 0xf6ae, 0x480b, { 0xbd, 0xd5, 0x37, 0xd9, 0x8c, 0x5e, 0x89, 0xaa } }
+
+  ## GUID indicates the LZMA custom compress/decompress algorithm.
+  #  Include/Guid/LzmaDecompress.h
+  gLzmaCustomDecompressGuid      = { 0xEE4E5898, 0x3914, 0x4259, { 0x9D, 0x6E, 0xDC, 0x7B, 0xD7, 0x94, 0x03, 0xCF }}
+  gLzmaF86CustomDecompressGuid     = { 0xD42AE6BD, 0x1352, 0x4bfb, { 0x90, 0x9A, 0xCA, 0x72, 0xA6, 0xEA, 0xE8, 0x89 }}
+
+  ## Include/Guid/TtyTerm.h
+  gEfiTtyTermGuid                = { 0x7d916d80, 0x5bb1, 0x458c, {0xa4, 0x8f, 0xe2, 0x5f, 0xdd, 0x51, 0xef, 0x94 }}
+
+  ## Include/Guid/HiiBootMaintenanceFormset.h
+  gEfiIfrBootMaintenanceGuid      = { 0xb2dedc91, 0xd59f, 0x48d2, { 0x89, 0x8a, 0x12, 0x49, 0xc, 0x74, 0xa4, 0xe0 }}
+
+  ## Include/Guid/PiSmmCommunicationRegionTable.h
+  gEdkiiPiSmmCommunicationRegionTableGuid = { 0x4e28ca50, 0xd582, 0x44ac, {0xa1, 0x1f, 0xe3, 0xd5, 0x65, 0x26, 0xdb, 0x34}}
+
+[Ppis]
+  ## Include/Ppi/AtaController.h
+  gPeiAtaControllerPpiGuid       = { 0xa45e60d1, 0xc719, 0x44aa, { 0xb0, 0x7a, 0xaa, 0x77, 0x7f, 0x85, 0x90, 0x6d }}
+
+  ## Include/Ppi/UsbHostController.h
+  gPeiUsbHostControllerPpiGuid   = { 0x652B38A9, 0x77F4, 0x453F, { 0x89, 0xD5, 0xE7, 0xBD, 0xC3, 0x52, 0xFC, 0x53 }}
+
+  ## Include/Ppi/Usb2HostController.h
+  gPeiUsb2HostControllerPpiGuid  = { 0xfedd6305, 0xe2d7, 0x4ed5, { 0x9f, 0xaa, 0xda, 0x8, 0xe, 0x33, 0x6c, 0x22   }}
+
+  ## Include/Ppi/UsbController.h
+  gPeiUsbControllerPpiGuid       = { 0x3BC1F6DE, 0x693E, 0x4547, { 0xA3, 0x00, 0x21, 0x82, 0x3C, 0xA4, 0x20, 0xB2 }}
+
+  ## Include/Ppi/UsbIo.h
+  gPeiUsbIoPpiGuid               = { 0x7C29785C, 0x66B9, 0x49FC, { 0xB7, 0x97, 0x1C, 0xA5, 0x55, 0x0E, 0xF2, 0x83 }}
+
+  ## Include/Ppi/SecPerformance.h
+  gPeiSecPerformancePpiGuid      = { 0x0ecc666b, 0x4662, 0x47f9, { 0x9d, 0xd5, 0xd0, 0x96, 0xff, 0x7d, 0xa4, 0x9e }}
+
+  ## Include/Ppi/SmmCommunication.h
+  gEfiPeiSmmCommunicationPpiGuid   =  { 0xae933e1c, 0xcc47, 0x4e38, { 0x8f, 0xe, 0xe2, 0xf6, 0x1d, 0x26, 0x5, 0xdf }}
+
+  ## Include/Ppi/SmmAccess.h
+  gPeiSmmAccessPpiGuid          =  { 0x268f33a9, 0xcccd, 0x48be, { 0x88, 0x17, 0x86, 0x5, 0x3a, 0xc3, 0x2e, 0xd6 }}
+
+  ## Include/Ppi/SmmControl.h
+  gPeiSmmControlPpiGuid         =  { 0x61c68702, 0x4d7e, 0x4f43, { 0x8d, 0xef, 0xa7, 0x43, 0x5, 0xce, 0x74, 0xc5 }}
+
+  ## Include/Ppi/PostBootScriptTable.h
+  gPeiPostScriptTablePpiGuid    =  { 0x88c9d306, 0x900, 0x4eb5, { 0x82, 0x60, 0x3e, 0x2d, 0xbe, 0xda, 0x1f, 0x89}}
+
+  ## Include/Ppi/SerialPortPei.h
+  gPeiSerialPortPpiGuid         =  { 0x490e9d85, 0x8aef, 0x4193, { 0x8e, 0x56, 0xf7, 0x34, 0xa9, 0xff, 0xac, 0x8b}}
+
+  ## Include/Ppi/UfsHostController.h
+  gEdkiiPeiUfsHostControllerPpiGuid  =  { 0xdc54b283, 0x1a77, 0x4cd6, { 0x83, 0xbb, 0xfd, 0xda, 0x46, 0x9a, 0x2e, 0xc6 }}
+
+  ## Include/Ppi/IpmiPpi.h
+  gPeiIpmiPpiGuid               =  { 0xa9731431, 0xd968, 0x4277, { 0xb7, 0x52, 0xa3, 0xa9, 0xa6, 0xae, 0x18, 0x98 }}
+
+[Protocols]
+  ## Load File protocol provides capability to load and unload EFI image into memory and execute it.
+  #  Include/Protocol/LoadPe32Image.h
+  #  This protocol is deprecated. Native EDKII module should NOT use this protocol to load/unload image.
+  #  If developer need implement such functionality, they should use BasePeCoffLib.
+  gEfiLoadPeImageProtocolGuid    = { 0x5CB5C776, 0x60D5, 0x45EE, { 0x88, 0x3C, 0x45, 0x27, 0x08, 0xCD, 0x74, 0x3F }}
+
+  ## Print protocol defines basic print functions to print the format unicode and ascii string.
+  # Include/Protocol/Print2.h
+  gEfiPrint2ProtocolGuid          = { 0xf05976ef, 0x83f1, 0x4f3d, { 0x86, 0x19, 0xf7, 0x59, 0x5d, 0x41, 0xe5, 0x38 } }
+
+  ## This protocol defines the generic memory test interfaces in Dxe phase.
+  # Include/Protocol/GenericMemoryTest.h
+  gEfiGenericMemTestProtocolGuid = { 0x309DE7F1, 0x7F5E, 0x4ACE, { 0xB4, 0x9C, 0x53, 0x1B, 0xE5, 0xAA, 0x95, 0xEF }}
+
+  ## Include/Protocol/Dpc.h
+  gEfiDpcProtocolGuid            = {0x480f8ae9, 0xc46, 0x4aa9,  { 0xbc, 0x89, 0xdb, 0x9f, 0xba, 0x61, 0x98, 0x6 }}
+
+  ## Fault Tolerant Write protocol provides boot-time service to do fault tolerant write capability for block devices.
+  #  Include/Protocol/FaultTolerantWrite.h
+  gEfiFaultTolerantWriteProtocolGuid = { 0x3EBD9E82, 0x2C78, 0x4DE6, { 0x97, 0x86, 0x8D, 0x4B, 0xFC, 0xB7, 0xC8, 0x81 }}
+
+  ## This protocol provides boot-time service to do fault tolerant write capability for block devices in SMM environment.
+  #  Include/Protocol/SmmFaultTolerantWrite.h
+  gEfiSmmFaultTolerantWriteProtocolGuid = { 0x3868fc3b, 0x7e45, 0x43a7, { 0x90, 0x6c, 0x4b, 0xa4, 0x7d, 0xe1, 0x75, 0x4d }}
+
+  ## This protocol is used to abstract the swap operation of boot block and backup block of boot FV.
+  #  Include/Protocol/SwapAddressRange.h
+  gEfiSwapAddressRangeProtocolGuid = { 0x1259F60D, 0xB754, 0x468E, { 0xA7, 0x89, 0x4D, 0xB8, 0x5D, 0x55, 0xE8, 0x7E }}
+
+  ## This protocol is used to abstract the swap operation of boot block and backup block of boot FV in SMM environment.
+  #  Include/Protocol/SmmSwapAddressRange.h
+  gEfiSmmSwapAddressRangeProtocolGuid = { 0x67c4f112, 0x3385, 0x4e55, { 0x9c, 0x5b, 0xc0, 0x5b, 0x71, 0x7c, 0x42, 0x28 }}
+
+  ## This protocol is intended for use as a means to store data in the EFI SMM environment.
+  #  Include/Protocol/SmmVariableProtocol.h
+  gEfiSmmVariableProtocolGuid = { 0xed32d533, 0x99e6, 0x4209, { 0x9c, 0xc0, 0x2d, 0x72, 0xcd, 0xd9, 0x98, 0xa7 }}
+
+  ## This protocol is intended for use as a means to mark a variable read-only after the event EFI_END_OF_DXE_EVENT_GUID is signaled.
+  #  Include/Protocol/VariableLock.h
+  gEdkiiVariableLockProtocolGuid = { 0xcd3d0a05, 0x9e24, 0x437c, { 0xa8, 0x91, 0x1e, 0xe0, 0x53, 0xdb, 0x76, 0x38 }}
+
+  ## Include/Protocol/VarCheck.h
+  gEdkiiVarCheckProtocolGuid     = { 0xaf23b340, 0x97b4, 0x4685, { 0x8d, 0x4f, 0xa3, 0xf2, 0x81, 0x69, 0xb2, 0x1d } }
+
+  ## Include/Protocol/SmmVarCheck.h
+  gEdkiiSmmVarCheckProtocolGuid  = { 0xb0d8f3c1, 0xb7de, 0x4c11, { 0xbc, 0x89, 0x2f, 0xb5, 0x62, 0xc8, 0xc4, 0x11 } }
+
+  ## This protocol is similar with DXE FVB protocol and used in the UEFI SMM evvironment.
+  #  Include/Protocol/SmmFirmwareVolumeBlock.h
+  gEfiSmmFirmwareVolumeBlockProtocolGuid = { 0xd326d041, 0xbd31, 0x4c01, { 0xb5, 0xa8, 0x62, 0x8b, 0xe8, 0x7f, 0x6, 0x53 }}
+
+  ## This protocol allows the error level mask for DEBUG() macros to be adjusted for DXE Phase modules
+  # Include/Guid/DebugMask.h
+  gEfiDebugMaskProtocolGuid = { 0x4c8a2451, 0xc207, 0x405b, {0x96, 0x94, 0x99, 0xea, 0x13, 0x25, 0x13, 0x41} }
+
+  ## Include/Protocol/LockBox.h
+  gEfiLockBoxProtocolGuid        = { 0xbd445d79, 0xb7ad, 0x4f04, { 0x9a, 0xd8, 0x29, 0xbd, 0x20, 0x40, 0xeb, 0x3c }}
+
+  ## Include/Protocol/FormBrowserEx.h
+  gEfiFormBrowserExProtocolGuid = { 0x1f73b18d, 0x4630, 0x43c1, { 0xa1, 0xde, 0x6f, 0x80, 0x85, 0x5d, 0x7d, 0xa4 } }
+
+  ## Include/Protocol/EbcVmTest.h
+  gEfiEbcVmTestProtocolGuid = { 0xAAEACCFD, 0xF27B, 0x4C17, { 0xB6, 0x10, 0x75, 0xCA, 0x1F, 0x2D, 0xFB, 0x52 } }
+
+  ## Include/Protocol/EbcSimpleDebugger.h
+  gEfiEbcSimpleDebuggerProtocolGuid = { 0x2a72d11e, 0x7376, 0x40f6, { 0x9c, 0x68, 0x23, 0xfa, 0x2f, 0xe3, 0x63, 0xf1 } }
+
+  ## Include/Protocol/BootLogo.h
+  gEfiBootLogoProtocolGuid = { 0xcdea2bd3, 0xfc25, 0x4c1c, { 0xb9, 0x7c, 0xb3, 0x11, 0x86, 0x6, 0x49, 0x90 } }
+
+  ## Include/Protocol/DisplayProtocol.h
+  gEdkiiFormDisplayEngineProtocolGuid = { 0x9bbe29e9, 0xfda1, 0x41ec, { 0xad, 0x52, 0x45, 0x22, 0x13, 0x74, 0x2d, 0x2e } }
+
+  ## Include/Protocol/FormBrowserEx2.h
+  gEdkiiFormBrowserEx2ProtocolGuid = { 0xa770c357, 0xb693, 0x4e6d, { 0xa6, 0xcf, 0xd2, 0x1c, 0x72, 0x8e, 0x55, 0xb } }
+
+  ## Include/Protocol/UfsHostController.h
+  gEdkiiUfsHostControllerProtocolGuid = { 0xebc01af5, 0x7a9, 0x489e, { 0xb7, 0xce, 0xdc, 0x8, 0x9e, 0x45, 0x9b, 0x2f } }
+
+  ## Include/Protocol/EsrtManagement.h
+  gEsrtManagementProtocolGuid         = { 0xa340c064, 0x723c, 0x4a9c, { 0xa4, 0xdd, 0xd5, 0xb4, 0x7a, 0x26, 0xfb, 0xb0 }}
+
+  ## Include/Protocol/SmmExitBootServices.h
+  gEdkiiSmmExitBootServicesProtocolGuid = { 0x296eb418, 0xc4c8, 0x4e05, { 0xab, 0x59, 0x39, 0xe8, 0xaf, 0x56, 0xf0, 0xa } }
+
+  ## Include/Protocol/SmmLegacyBoot.h
+  gEdkiiSmmLegacyBootProtocolGuid = { 0x85a8ab57, 0x644, 0x4110, { 0x85, 0xf, 0x98, 0x13, 0x22, 0x4, 0x70, 0x70 } }
+
+  ## Include/Protocol/SmmReadyToBoot.h
+  gEdkiiSmmReadyToBootProtocolGuid = { 0x6e057ecf, 0xfa99, 0x4f39, { 0x95, 0xbc, 0x59, 0xf9, 0x92, 0x1d, 0x17, 0xe4 } }
+
+  ## Include/Protocol/IpmiProtocol.h
+  gIpmiProtocolGuid    = { 0xdbc6381f, 0x5554, 0x4d14, { 0x8f, 0xfd, 0x76, 0xd7, 0x87, 0xb8, 0xac, 0xbf } }
+  gSmmIpmiProtocolGuid = { 0x5169af60, 0x8c5a, 0x4243, { 0xb3, 0xe9, 0x56, 0xc5, 0x6d, 0x18, 0xee, 0x26 } }
+
+#
+# [Error.gEfiMdeModulePkgTokenSpaceGuid]
+#   0x80000001 | Invalid value provided.
+#   0x80000002 | Reserved bits must be set to zero.
+#   0x80000003 | Incorrect progress code provided.
+#   0x80000004 | Invalid foreground color specified.
+#   0x80000005 | Invalid background color specified.
+#   0x80000006 | Incorrect error code provided.
+#
+
+[PcdsFeatureFlag]
+  ## Indicates if the platform can support update capsule across a system reset.<BR><BR>
+  #   TRUE  - Supports update capsule across a system reset.<BR>
+  #   FALSE - Does not support update capsule across a system reset.<BR>
+  # @Prompt Enable update capsule across a system reset.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSupportUpdateCapsuleReset|FALSE|BOOLEAN|0x0001001d
+
+  ## Indicates if all PCD PPI services will be enabled.<BR><BR>
+  #   TRUE  - All PCD PPI services will be produced.<BR>
+  #   FALSE - Minimal PCD PPI services (only GetService) will be produced.<BR>
+  # @Prompt Enable full PEI PCD services.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiFullPcdDatabaseEnable|TRUE|BOOLEAN|0x00010020
+
+  ## Indicates if the Device Path To Text Protocol should be produced by the platform.
+  #  It can be disabled to save size.<BR><BR>
+  #   TRUE  - Device Path To Text Protocol will be produced.<BR>
+  #   FALSE - Device Path To Text Protocol will not be produced.<BR>
+  # @Prompt Enable Device Path to Text support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathToText|TRUE|BOOLEAN|0x00010037
+
+  ## Indicates if the Device Path From Text Protocol should be produced by the platform.
+  #  It can be disabled to save size.<BR><BR>
+  #   TRUE  - Device Path From Text Protocol will be produced.<BR>
+  #   FALSE - Device Path From Text Protocol will not be produced.<BR>
+  # @Prompt Enable Device Path From Text support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathFromText|TRUE|BOOLEAN|0x00010038
+
+  ## Indicates if the statistics about variable usage will be collected. This information is
+  #  stored as a vendor configuration table into the EFI system table.
+  #  Set this PCD to TRUE to use VariableInfo application in MdeModulePkg\Application directory to get
+  #  variable usage info. VariableInfo application will not output information if not set to TRUE.<BR><BR>
+  #   TRUE  - Statistics about variable usage will be collected.<BR>
+  #   FALSE - Statistics about variable usage will not be collected.<BR>
+  # @Prompt Enable variable statistics collection.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableCollectStatistics|FALSE|BOOLEAN|0x0001003f
+
+  ## Indicates if Unicode Collation Protocol will be installed.<BR><BR>
+  #   TRUE  - Installs Unicode Collation Protocol.<BR>
+  #   FALSE - Does not install Unicode Collation Protocol.<BR>
+  # @Prompt Enable Unicode Collation support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollationSupport|TRUE|BOOLEAN|0x00010040
+
+  ## Indicates if Unicode Collation 2 Protocol will be installed.<BR><BR>
+  #   TRUE  - Installs Unicode Collation 2 Protocol.<BR>
+  #   FALSE - Does not install Unicode Collation 2 Protocol.<BR>
+  # @Prompt Enable Unicode Collation 2 support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollation2Support|TRUE|BOOLEAN|0x00010041
+
+  ## Indicates if Graphics Output Protocol will be installed on virtual handle created by ConsplitterDxe.
+  #  It could be set FALSE to save size.<BR><BR>
+  #   TRUE  - Installs Graphics Output Protocol on virtual handle created by ConsplitterDxe.<BR>
+  #   FALSE - Does not install Graphics Output Protocol on virtual handle created by ConsplitterDxe.<BR>
+  # @Prompt Enable ConOut GOP support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutGopSupport|TRUE|BOOLEAN|0x00010042
+
+  ## Indicates if UGA Draw Protocol will be installed on virtual handle created by ConsplitterDxe.
+  #  It could be set FALSE to save size.<BR><BR>
+  #   TRUE  - Installs UGA Draw Protocol on virtual handle created by ConsplitterDxe.<BR>
+  #   FALSE - Does not install UGA Draw Protocol on virtual handle created by ConsplitterDxe.<BR>
+  # @Prompt Enable ConOut UGA support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutUgaSupport|TRUE|BOOLEAN|0x00010043
+
+  ## Indicates PeiCore will first search TE section from the PEIM to load the image, or PE32 section, when PeiCore dispatches a PEI module.
+  #  This PCD is used to tune PEI phase performance to reduce the search image time.
+  #  It can be set according to the generated image section type.<BR><BR>
+  #   TRUE  - PeiCore will first search TE section from PEIM to load the image, if TE section is not found, then PeiCore will search PE section.<BR>
+  #   FALSE - PeiCore will first search PE section from PEIM to load the image.<BR>
+  # @Prompt PeiCore search TE section first.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreImageLoaderSearchTeSectionFirst|TRUE|BOOLEAN|0x00010044
+
+  ## Indicates if to turn off the support of legacy usb. So legacy usb device driver can not make use of SMI
+  #  interrupt to access usb device in the case of absence of usb stack.
+  #  DUET platform requires the token to be TRUE.<BR><BR>
+  #   TRUE  - Turn off usb legacy support.<BR>
+  #   FALSE - Does not turn off usb legacy support.<BR>
+  # @Prompt Turn off USB legacy support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTurnOffUsbLegacySupport|FALSE|BOOLEAN|0x00010047
+
+  ## Indicates if HiiImageProtocol will be installed.
+  #  FALSE is for size reduction.<BR><BR>
+  #   TRUE  - Installs HiiImageProtocol.<BR>
+  #   FALSE - Does not install HiiImageProtocol.<BR>
+  # @Prompt Enable HII image support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSupportHiiImageProtocol|TRUE|BOOLEAN|0x00010100
+
+  ## Indicates if USB KeyBoard Driver disables the default keyboard layout.
+  #  The default keyboard layout serves as the backup when no keyboard layout can be retrieved
+  #  from HII database.<BR><BR>
+  #   TRUE  - USB KeyBoard Driver will disable the default keyboard layout.<BR>
+  #   FALSE - USB KeyBoard Driver will not disable the default keyboard layout.<BR>
+  # @Prompt Disable default keyboard layout in USB KeyBoard Driver.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDisableDefaultKeyboardLayoutInUsbKbDriver|FALSE|BOOLEAN|0x00010200
+
+  ## Indicates if backward compatibility to Framework HII and Framework FvHob is supported.<BR><BR>
+  #   TRUE  - Setup Browser supports GUID opcodes generated from Framework HII VFR file by VFR compiler.
+  #           the PeiCore will handle the framework FvHob and install FvInfo PPI for it.<BR>
+  #   FALSE - Setup Browser doesn't support GUID opcodes generated from Framework HII VFR file by VFR compiler.
+  #           the PeiCore will not handle the framework FvHob and install FvInfo PPI for it.<BR>
+  # @Prompt Enable framework backward compatibility support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrameworkCompatibilitySupport|FALSE|BOOLEAN|0x00012009
+
+  ## Indicates if HelloWorld Application will print the verbose information.
+  #  This PCD is a sample to explain FeatureFlag PCD usage.<BR><BR>
+  #   TRUE  - HelloWorld Application will print the verbose information.<BR>
+  #   FALSE - HelloWorld Application will not print the verbose information.<BR>
+  # @Prompt Enable HelloWorld print.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintEnable|TRUE|BOOLEAN|0x0001200a
+
+  ## Indicates if FULL FTW protocol services (total six APIs) will be produced.<BR><BR>
+  #   TRUE  - Produces FULL FTW protocol services (total six APIs).<BR>
+  #   FALSE - Only FTW Write service is available.<BR>
+  # @Prompt Enable FULL FTW services.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFullFtwServiceEnable|TRUE|BOOLEAN|0x0001200b
+
+  ## Indicates if DXE IPL supports the UEFI decompression algorithm.<BR><BR>
+  #   TRUE  - DXE IPL will support UEFI decompression.<BR>
+  #   FALSE - DXE IPL will not support UEFI decompression to save space.<BR>
+  # @Prompt Enable UEFI decompression support in DXE IPL.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSupportUefiDecompress|TRUE|BOOLEAN|0x0001200c
+
+  ## Indicates if PciBus driver supports the hot plug device.<BR><BR>
+  #   TRUE  - PciBus driver supports the hot plug device.<BR>
+  #   FALSE - PciBus driver doesn't support the hot plug device.<BR>
+  # @Prompt Enable PciBus hot plug device support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBusHotplugDeviceSupport|TRUE|BOOLEAN|0x0001003d
+
+  ## Indicates if the PciBus driver probes non-standard, such as 2K/1K/512, granularity for PCI to PCI bridge I/O window.<BR><BR>
+  #   TRUE  - PciBus driver probes non-standard granularity for PCI to PCI bridge I/O window.<BR>
+  #   FALSE - PciBus driver doesn't probe non-standard granularity for PCI to PCI bridge I/O window.<BR>
+  # @Prompt Enable PCI bridge IO alignment probe.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciBridgeIoAlignmentProbe|FALSE|BOOLEAN|0x0001004e
+
+  ## Indicates if StatusCode is reported via Serial port.<BR><BR>
+  #   TRUE  - Reports StatusCode via Serial port.<BR>
+  #   FALSE - Does not report StatusCode via Serial port.<BR>
+  # @Prompt Enable StatusCode via Serial port.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseSerial|TRUE|BOOLEAN|0x00010022
+
+  ## Indicates if StatusCode is stored in memory.
+  #  The memory is boot time memory in PEI Phase and is runtime memory in DXE Phase.<BR><BR>
+  #   TRUE  - Stores StatusCode in memory.<BR>
+  #   FALSE - Does not store StatusCode in memory.<BR>
+  # @Prompt Enable StatusCode via memory.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory|FALSE|BOOLEAN|0x00010023
+
+  ## Indicates if PEI phase StatusCode will be replayed in DXE phase.<BR><BR>
+  #   TRUE  - Replays PEI phase StatusCode in DXE phased.<BR>
+  #   FALSE - Does not replay PEI phase StatusCode in DXE phase.<BR>
+  # @Prompt Enable PEI StatusCode replay in DXE phase
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeReplayIn|FALSE|BOOLEAN|0x0001002d
+
+  ## Indicates if ACPI SDT protocol will be installed.<BR><BR>
+  #   TRUE  - Installs ACPI SDT protocol.<BR>
+  #   FALSE - Does not install ACPI SDT protocol.<BR>
+  # @Prompt Enable ACPI SDT support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdInstallAcpiSdtProtocol|FALSE|BOOLEAN|0x0001004d
+
+  ## Indicates if the unaligned I/O, MMIO, and PCI Configuration cycles through the PCI I/O Protocol are enabled.
+  #  The default value for this PCD is false to disable support for unaligned PCI I/O Protocol requests.<BR><BR>
+  #   TRUE  - Enables the unaligned I/O, MMIO, and PCI Configuration cycles through the PCI I/O Protocol.<BR>
+  #   FALSE - Disables the unaligned I/O, MMIO, and PCI Configuration cycles through the PCI I/O Protocol.<BR>
+  # @Prompt Enable unaligned PCI I/O support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnalignedPciIoEnable|FALSE|BOOLEAN|0x0001003e
+
+  ## Indicates if TEXT statement is always set to GrayOut statement in HII Form Browser.<BR><BR>
+  #   TRUE  - TEXT statement will always be set to GrayOut.<BR>
+  #   FALSE - TEXT statement will be set to GrayOut only when GrayOut condition is TRUE.<BR>
+  # @Prompt Always GrayOut TEXT statement.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserGrayOutTextStatement|FALSE|BOOLEAN|0x0001004f
+
+  ## Indicates if unselectable menu should be gray out in HII Form Browser.<BR><BR>
+  #   TRUE  - The unselectable menu will be set to GrayOut.<BR>
+  #   FALSE - The menu will be show as normal menu entry even if it is not selectable.<BR>
+  # @Prompt GrayOut read only menu.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowerGrayOutReadOnlyMenu|FALSE|BOOLEAN|0x00010070
+
+  ## Indicates if recovery from IDE disk will be supported.<BR><BR>
+  #   TRUE  - Supports recovery from IDE disk.<BR>
+  #   FALSE - Does not support recovery from IDE disk.<BR>
+  # @Prompt Enable recovery on IDE disk.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdRecoveryOnIdeDisk|TRUE|BOOLEAN|0x00010060
+
+  ## Indicates if recovery from FAT floppy disk will be supported.<BR><BR>
+  #   TRUE  - Supports recovery from FAT floppy disk.<BR>
+  #   FALSE - Does not support recovery from FAT floppy disk.<BR>
+  # @Prompt Enable recovery on FAT floppy disk.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdRecoveryOnFatFloppyDisk|TRUE|BOOLEAN|0x00010061
+
+  ## Indicates if recovery from data CD will be supported.<BR><BR>
+  #   TRUE  - Supports recovery from data CD.<BR>
+  #   FALSE - Does not support recovery from data CD.<BR>
+  # @Prompt Enable recovery on data CD.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdRecoveryOnDataCD|TRUE|BOOLEAN|0x00010062
+
+  ## Indicates if recovery from FAT USB disk will be supported.<BR><BR>
+  #   TRUE  - Supports recovery from USB disk.<BR>
+  #   FALSE - Does not support recovery from USB disk.<BR>
+  # @Prompt Enable recovery on FAT USB disk.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdRecoveryOnFatUsbDisk|TRUE|BOOLEAN|0x00010063
+
+  ## Indicates if S3 performance data will be supported in ACPI FPDT table.<BR><BR>
+  #   TRUE  - S3 performance data will be supported in ACPI FPDT table.<BR>
+  #   FALSE - S3 performance data will not be supported in ACPI FPDT table.<BR>
+  # @Prompt Enable S3 performance data support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwarePerformanceDataTableS3Support|TRUE|BOOLEAN|0x00010064
+
+  ## Indicates if Serial device uses half hand shake.<BR><BR>
+  #   TRUE  - Serial device uses half hand shake.<BR>
+  #   FALSE - Serial device doesn't use half hand shake.<BR>
+  # @Prompt Enable Serial device Half Hand Shake
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseHalfHandshake|FALSE|BOOLEAN|0x00010073
+
+[PcdsFeatureFlag.IA32, PcdsFeatureFlag.X64]
+  ## Indicates if DxeIpl should switch to long mode to enter DXE phase.
+  #  It is assumed that 64-bit DxeCore is built in firmware if it is true; otherwise 32-bit DxeCore
+  #  is built in firmware.<BR><BR>
+  #   TRUE  - DxeIpl will load a 64-bit DxeCore and switch to long mode to hand over to DxeCore.<BR>
+  #   FALSE - DxeIpl will load a 32-bit DxeCore and perform stack switch to hand over to DxeCore.<BR>
+  # @Prompt DxeIpl switch to long mode.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode|TRUE|BOOLEAN|0x0001003b
+
+  ## Indicates if DxeIpl should rebuild page tables. This flag only
+  #  makes sense in the case where the DxeIpl and the DxeCore are both X64.<BR><BR>
+  #   TRUE  - DxeIpl will rebuild page tables.<BR>
+  #   FALSE - DxeIpl will not rebuild page tables.<BR>
+  # @Prompt DxeIpl rebuild page tables.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplBuildPageTables|TRUE|BOOLEAN|0x0001003c
+
+[PcdsFixedAtBuild]
+  ## Flag of enabling/disabling the feature of Loading Module at Fixed Address.<BR><BR>
+  #  0xFFFFFFFFFFFFFFFF: Enable the feature as fixed offset to TOLM.<BR>
+  #  0:                  Disable the feature.<BR>
+  #  Other Value:        Enable the feature as fixed absolute address, and the value is the top memory address.<BR>
+  # @Prompt Enable LMFA feature.
+  # @Expression 0x80000001 | (gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable == 0xFFFFFFFFFFFFFFFF || gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable <= 0x0FFFFFFFFFFFFFFF)
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadModuleAtFixAddressEnable|0|UINT64|0x30001015
+
+  ## Progress Code for OS Loader LoadImage start.<BR><BR>
+  #  PROGRESS_CODE_OS_LOADER_LOAD   = (EFI_SOFTWARE_DXE_BS_DRIVER | (EFI_OEM_SPECIFIC | 0x00000000)) = 0x03058000<BR>
+  # @Prompt Progress Code for OS Loader LoadImage start.
+  # @ValidList  0x80000003 | 0x03058000
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderLoad|0x03058000|UINT32|0x30001030
+
+  ## Progress Code for OS Loader StartImage start.<BR><BR>
+  #  PROGRESS_CODE_OS_LOADER_START  = (EFI_SOFTWARE_DXE_BS_DRIVER | (EFI_OEM_SPECIFIC | 0x00000001)) = 0x03058001<BR>
+  # @Prompt Progress Code for OS Loader StartImage start.
+  # @ValidList  0x80000003 | 0x03058001
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderStart|0x03058001|UINT32|0x30001031
+
+  ## Progress Code for S3 Suspend start.<BR><BR>
+  #  PROGRESS_CODE_S3_SUSPEND_START = (EFI_SOFTWARE_SMM_DRIVER | (EFI_OEM_SPECIFIC | 0x00000000))    = 0x03078000<BR>
+  # @Prompt Progress Code for S3 Suspend start.
+  # @ValidList  0x80000003 | 0x03078000
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeS3SuspendStart|0x03078000|UINT32|0x30001032
+
+  ## Progress Code for S3 Suspend end.<BR><BR>
+  #  PROGRESS_CODE_S3_SUSPEND_END   = (EFI_SOFTWARE_SMM_DRIVER | (EFI_OEM_SPECIFIC | 0x00000001))    = 0x03078001<BR>
+  # @Prompt Progress Code for S3 Suspend end.
+  # @ValidList  0x80000003 | 0x03078001
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeS3SuspendEnd|0x03078001|UINT32|0x30001033
+
+  ## Error Code for SetVariable failure.<BR><BR>
+  #  EDKII_ERROR_CODE_SET_VARIABLE  = (EFI_SOFTWARE_DXE_BS_DRIVER | (EFI_OEM_SPECIFIC | 0x00000002)) = 0x03058002<BR>
+  # @Prompt Error Code for SetVariable failure.
+  # @ValidList  0x80000006 | 0x03058002
+  gEfiMdeModulePkgTokenSpaceGuid.PcdErrorCodeSetVariable|0x03058002|UINT32|0x30001040
+
+[PcdsFixedAtBuild, PcdsPatchableInModule]
+  ## Dynamic type PCD can be registered callback function for Pcd setting action.
+  #  PcdMaxPeiPcdCallBackNumberPerPcdEntry indicates the maximum number of callback function
+  #  for a dynamic PCD used in PEI phase.
+  # @Prompt Max PEI PCD callback number per PCD entry.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPcdCallBackNumberPerPcdEntry|0x08|UINT32|0x0001000f
+
+  ## VPD type PCD  allows a developer to point to an absolute physical address PcdVpdBaseAddress
+  #  to store PCD value.
+  # @Prompt VPD base address.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVpdBaseAddress|0x0|UINT32|0x00010010
+
+  ## Maximum number of FV is supported by PeiCore's dispatching.
+  # @Prompt Maximum number of FV supported by PeiCore.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxFvSupported|6|UINT32|0x00010030
+
+  ## Maximum File count in every FV is supported by PeiCore's dispatching.
+  #  PeiCore supported File type includes PEIM, Combined PEIM and FV.
+  # @Prompt Maximum File count per FV supported by PeiCore.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPeimPerFv|32|UINT32|0x00010031
+
+  ## Maximum stack size for PeiCore.
+  # @Prompt Maximum stack size for PeiCore.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPeiStackSize|0x20000|UINT32|0x00010032
+
+  ## Maximum PPI count is supported by PeiCore's PPI database.
+  # @Prompt Maximum PPI count supported by PeiCore.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiCoreMaxPpiSupported|64|UINT32|0x00010033
+
+  ## The maximum size of a single non-HwErr type variable.
+  # @Prompt Maximum variable size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxVariableSize|0x400|UINT32|0x30000003
+
+  ## The maximum size of a single authenticated variable.
+  # The value is 0 as default for compatibility that maximum authenticated variable size is specified by PcdMaxVariableSize.
+  # @Prompt Maximum authenticated variable size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxAuthVariableSize|0x00|UINT32|0x30000009
+
+  ## The maximum size of single hardware error record variable.<BR><BR>
+  # In IA32/X64 platforms, this value should be larger than 1KB.<BR>
+  # In IA64 platforms, this value should be larger than 128KB.<BR>
+  # @Prompt Maximum HwErr variable size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxHardwareErrorVariableSize|0x8000|UINT32|0x30000004
+
+  ## The size of reserved HwErr variable space. Note that this value must be less than (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER)).
+  # In EdkII implementation, HwErr type variable is stored with common non-volatile variables in the same NV region.
+  # so the platform integrator should ensure this value is less than (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER)).
+  # this value is used to guarantee the space of HwErr type variable and not populated by common variable.
+  # @Prompt HwErr variable storage size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHwErrStorageSize|0x0000|UINT32|0x30000006
+
+  ## The size of maximum user NV variable space.<BR><BR>
+  # Note that this value must be less than (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER) - PcdHwErrStorageSize).<BR>
+  # If the value is 0, it means user variable share the same NV storage with system variable,
+  # this is designed to keep the compatibility for the platform that does not allocate special region for user variable.<BR>
+  # If the value is non-0, the below 4 types of variables will be regarded as System Variable after EndOfDxe, their property could be got by VarCheck protocol,
+  # otherwise the variable will be regarded as user variable.<BR>
+  #  1) UEFI defined variables (gEfiGlobalVariableGuid and gEfiImageSecurityDatabaseGuid(auth variable) variables at least).<BR>
+  #  2) Variables managed by Variable driver internally.<BR>
+  #  3) Variables need to be locked, they MUST be set by VariableLock protocol.<BR>
+  #  4) Important variables during platform boot, their property SHOULD be set by VarCheck protocol.<BR>
+  # The PCD is used to guarantee the space of system variable and not populated by user variable.<BR>
+  # @Prompt Maximum user NV variable space size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxUserNvVariableSpaceSize|0x00|UINT32|0x00000009
+
+  ## The size of NV variable space reserved at UEFI boottime.<BR><BR>
+  # Note that this value must be less than (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER) - PcdHwErrStorageSize).<BR>
+  # In EdkII implementation, variable driver can reserved some NV storage region for boottime settings.
+  # So at UEFI runtime, the variable service consumer can not exhaust full NV storage region.<BR>
+  # Then the common NV variable space size at boottime will be
+  #  (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER) - PcdHwErrStorageSize),<BR>
+  # and the common NV variable space size at runtime will be
+  #  (PcdFlashNvStorageVariableSize - EFI_FIRMWARE_VOLUME_HEADER.HeaderLength - sizeof (VARIABLE_STORE_HEADER) - PcdHwErrStorageSize) - PcdBoottimeReservedNvVariableSpaceSize.<BR>
+  # @Prompt Boottime reserved NV variable space size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBoottimeReservedNvVariableSpaceSize|0x00|UINT32|0x30000007
+
+  ## Reclaim variable space at EndOfDxe.<BR><BR>
+  # The value is FALSE as default for compatibility that variable driver tries to reclaim variable space at ReadyToBoot event.<BR>
+  # If the value is set to TRUE, variable driver tries to reclaim variable space at EndOfDxe event.<BR>
+  # @Prompt Reclaim variable space at EndOfDxe.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdReclaimVariableSpaceAtEndOfDxe|FALSE|BOOLEAN|0x30000008
+
+  ## The size of volatile buffer. This buffer is used to store VOLATILE attribute variables.
+  # @Prompt Variable storage size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableStoreSize|0x10000|UINT32|0x30000005
+
+  ## FFS filename to find the ACPI tables.
+  # @Prompt FFS name of ACPI tables storage.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiTableStorageFile|{ 0x25, 0x4e, 0x37, 0x7e, 0x01, 0x8e, 0xee, 0x4f, 0x87, 0xf2, 0x39, 0xc, 0x23, 0xc6, 0x6, 0xcd }|VOID*|0x30000016
+
+  ## FFS filename to find the capsule coalesce image.
+  # @Prompt FFS name of capsule coalesce image.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdCapsuleCoalesceFile|{ 0xA6, 0xE4, 0xFD, 0xF7, 0x4C, 0x29, 0x3c, 0x49, 0xB5, 0x0F, 0x97, 0x34, 0x55, 0x3B, 0xB7, 0x57 }|VOID*|0x30000017
+
+  ## Maximum number of performance log entries during PEI phase.
+  # @Prompt Maximum number of PEI performance log entries.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPerformanceLogEntries|40|UINT8|0x0001002f
+
+  ## RTC Update Timeout Value(microsecond).
+  # @Prompt RTC Update Timeout Value.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdRealTimeClockUpdateTimeout|100000|UINT32|0x00010034
+
+  ## Indicates the 16550 serial port registers are in MMIO space, or in I/O space. Default is I/O space.<BR><BR>
+  #   TRUE  - 16550 serial port registers are in MMIO space.<BR>
+  #   FALSE - 16550 serial port registers are in I/O space.<BR>
+  # @Prompt Serial port registers use MMIO.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseMmio|FALSE|BOOLEAN|0x00020000
+
+  ## Indicates if the 16550 serial port hardware flow control will be enabled. Default is FALSE.<BR><BR>
+  #   TRUE  - 16550 serial port hardware flow control will be enabled.<BR>
+  #   FALSE - 16550 serial port hardware flow control will be disabled.<BR>
+  # @Prompt Enable serial port hardware flow control.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialUseHardwareFlowControl|FALSE|BOOLEAN|0x00020001
+
+  ## Indicates if the 16550 serial Tx operations will be blocked if DSR is not asserted (no cable). Default is FALSE.
+  #  This PCD is ignored if PcdSerialUseHardwareFlowControl is FALSE.<BR><BR>
+  #   TRUE  - 16550 serial Tx operations will be blocked if DSR is not asserted.<BR>
+  #   FALSE - 16550 serial Tx operations will not be blocked if DSR is not asserted.<BR>
+  # @Prompt Enable serial port cable detetion.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialDetectCable|FALSE|BOOLEAN|0x00020006
+
+  ## Base address of 16550 serial port registers in MMIO or I/O space. Default is 0x3F8.
+  # @Prompt Base address of serial port registers.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialRegisterBase|0x03F8|UINT64|0x00020002
+
+  ## Baud rate for the 16550 serial port.  Default is 115200 baud.
+  # @Prompt Baud rate for serial port.
+  # @ValidList  0x80000001 | 921600, 460800, 230400, 115200, 57600, 38400, 19200, 9600, 7200, 4800, 3600, 2400, 2000, 1800, 1200, 600, 300, 150, 134, 110, 75, 50
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialBaudRate|115200|UINT32|0x00020003
+
+  ## Line Control Register (LCR) for the 16550 serial port. This encodes data bits, parity, and stop bits.<BR><BR>
+  #    BIT1..BIT0 - Data bits.  00b = 5 bits, 01b = 6 bits, 10b = 7 bits, 11b = 8 bits<BR>
+  #    BIT2       - Stop Bits.  0 = 1 stop bit.  1 = 1.5 stop bits if 5 data bits selected, otherwise 2 stop bits.<BR>
+  #    BIT5..BIT3 - Parity.  xx0b = No Parity, 001b = Odd Parity, 011b = Even Parity, 101b = Mark Parity, 111b=Stick Parity<BR>
+  #    BIT7..BIT6 - Reserved.  Must be 0.<BR>
+  #
+  #  Default is No Parity, 8 Data Bits, 1 Stop Bit.<BR>
+  # @Prompt Serial port Line Control settings.
+  # @Expression 0x80000002 | (gEfiMdeModulePkgTokenSpaceGuid.PcdSerialLineControl & 0xC0) == 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialLineControl|0x03|UINT8|0x00020004
+
+  ## FIFO Control Register (FCR) for the 16550 serial port.<BR><BR>
+  #    BIT0       - FIFO Enable.  0 = Disable FIFOs.  1 = Enable FIFOs.<BR>
+  #    BIT1       - Clear receive FIFO.  1 = Clear FIFO.<BR>
+  #    BIT2       - Clear transmit FIFO.  1 = Clear FIFO.<BR>
+  #    BIT4..BIT3 - Reserved.  Must be 0.<BR>
+  #    BIT5       - Enable 64-byte FIFO.  0 = Disable 64-byte FIFO.  1 = Enable 64-byte FIFO<BR>
+  #    BIT7..BIT6 - Reserved.  Must be 0.<BR>
+  #
+  #  Default is to enable and clear all FIFOs.<BR>
+  # @Prompt Serial port FIFO Control settings.
+  # @Expression 0x80000002 | (gEfiMdeModulePkgTokenSpaceGuid.PcdSerialFifoControl & 0xD8) == 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialFifoControl|0x07|UINT8|0x00020005
+
+  ## This setting can override the default TFTP block size. A value of 0 computes
+  # the default from MTU information. A non-zero value will be used as block size
+  # in bytes.
+  # @Prompt TFTP block size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTftpBlockSize|0x0|UINT64|0x30001026
+
+  ## Maximum address that the DXE Core will allocate the EFI_SYSTEM_TABLE_POINTER
+  #  structure. The default value for this PCD is 0, which means that the DXE Core
+  #  will allocate the buffer from the EFI_SYSTEM_TABLE_POINTER structure on a 4MB
+  #  boundary as close to the top of memory as feasible.  If this PCD is set to a
+  #  value other than 0, then the DXE Core will first attempt to allocate the
+  #  EFI_SYSTEM_TABLE_POINTER structure on a 4MB boundary below the address specified
+  #  by this PCD, and if that allocation fails, retry the allocation on a 4MB
+  #  boundary as close to the top of memory as feasible.
+  # @Prompt Maximum Efi System Table Pointer address.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxEfiSystemTablePointerAddress|0x0|UINT64|0x30001027
+
+  ## Indicates if to shadow PEIM on S3 boot path after memory is ready.<BR><BR>
+  #   TRUE  - Shadow PEIM on S3 boot path after memory is ready.<BR>
+  #   FALSE - Not shadow PEIM on S3 boot path after memory is ready.<BR>
+  # @Prompt Shadow Peim On S3 Boot.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdShadowPeimOnS3Boot|FALSE|BOOLEAN|0x30001028
+
+  ## Indicates if to shadow PEIM and PeiCore after memory is ready.<BR><BR>
+  #  This PCD is used on other boot path except for S3 boot.
+  #   TRUE  - Shadow PEIM and PeiCore after memory is ready.<BR>
+  #   FALSE - Not shadow PEIM after memory is ready.<BR>
+  # @Prompt Shadow Peim and PeiCore on boot
+  gEfiMdeModulePkgTokenSpaceGuid.PcdShadowPeimOnBoot|TRUE|BOOLEAN|0x30001029
+
+  ## The mask is used to control memory profile behavior.<BR><BR>
+  #  BIT0 - Enable UEFI memory profile.<BR>
+  #  BIT1 - Enable SMRAM profile.<BR>
+  # @Prompt Memory Profile Property.
+  # @Expression  0x80000002 | (gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfilePropertyMask & 0xFC) == 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfilePropertyMask|0x0|UINT8|0x30001041
+
+  ## This flag is to control which memory types of alloc info will be recorded by DxeCore & SmmCore.<BR><BR>
+  # For SmmCore, only EfiRuntimeServicesCode and EfiRuntimeServicesData are valid.<BR>
+  #
+  # Below is bit mask for this PCD: (Order is same as UEFI spec)<BR>
+  #  EfiReservedMemoryType          0x0001<BR>
+  #  EfiLoaderCode                  0x0002<BR>
+  #  EfiLoaderData                  0x0004<BR>
+  #  EfiBootServicesCode            0x0008<BR>
+  #  EfiBootServicesData            0x0010<BR>
+  #  EfiRuntimeServicesCode         0x0020<BR>
+  #  EfiRuntimeServicesData         0x0040<BR>
+  #  EfiConventionalMemory          0x0080<BR>
+  #  EfiUnusableMemory              0x0100<BR>
+  #  EfiACPIReclaimMemory           0x0200<BR>
+  #  EfiACPIMemoryNVS               0x0400<BR>
+  #  EfiMemoryMappedIO              0x0800<BR>
+  #  EfiMemoryMappedIOPortSpace     0x1000<BR>
+  #  EfiPalCode                     0x2000<BR>
+  #  EfiPersistentMemory            0x4000<BR>
+  #  OEM Reserved       0x4000000000000000<BR>
+  #  OS Reserved        0x8000000000000000<BR>
+  #
+  # e.g. Reserved+ACPINvs+ACPIReclaim+RuntimeCode+RuntimeData are needed, 0x661 should be used.<BR>
+  #
+  # @Prompt Memory profile memory type.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfileMemoryType|0x0|UINT64|0x30001042
+
+  ## PCI Serial Device Info. It is an array of Device, Function, and Power Management
+  #  information that describes the path that contains zero or more PCI to PCI briges
+  #  followed by a PCI serial device.  Each array entry is 4-bytes in length.  The
+  #  first byte is the PCI Device Number, then second byte is the PCI Function Number,
+  #  and the last two bytes are the offset to the PCI power management capabilities
+  #  register used to manage the D0-D3 states.  If a PCI power management capabilities
+  #  register is not present, then the last two bytes in the offset is set to 0.  The
+  #  array is terminated by an array entry with a PCI Device Number of 0xFF.  For a
+  #  non-PCI fixed address serial device, such as an ISA serial device, the value is 0xFF.
+  # @Prompt Pci Serial Device Info
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialPciDeviceInfo|{0xFF}|VOID*|0x00010067
+
+  ## PCI Serial Parameters. It is an array of VendorID, DeviceID, ClockRate, Offset,
+  #  BarIndex, RegisterStride, ReceiveFifoDepth, TransmitFifoDepth information that
+  #  describes the parameters of special PCI serial devices.
+  #  Each array entry is 24-byte in length. The array is terminated
+  #  by an array entry with a PCI Vendor ID of 0xFFFF. If a platform only contains a
+  #  standard 16550 PCI serial device whose class code is 7/0/2, the value is 0xFFFF.
+  #  The C style structure is defined as below:<BR>
+  #  typedef struct {<BR>
+  #    UINT16  VendorId;          ///< Vendor ID to match the PCI device. The value 0xFFFF terminates the list of entries.<BR>
+  #    UINT16  DeviceId;          ///< Device ID to match the PCI device.<BR>
+  #    UINT32  ClockRate;         ///< UART clock rate. Set to 0 for default clock rate of 1843200 Hz.<BR>
+  #    UINT64  Offset;            ///< The byte offset into to the BAR.<BR>
+  #    UINT8   BarIndex;          ///< Which BAR to get the UART base address.<BR>
+  #    UINT8   RegisterStride;    ///< UART register stride in bytes. Set to 0 for default register stride of 1 byte.<BR>
+  #    UINT16  ReceiveFifoDepth;  ///< UART receive FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.<BR>
+  #    UINT16  TransmitFifoDepth; ///< UART transmit FIFO depth in bytes. Set to 0 for a default FIFO depth of 16 bytes.<BR>
+  #    UINT8   Reserved[2];<BR>
+  #  } PCI_SERIAL_PARAMETER;<BR>
+  #  It contains zero or more instances of the above structure.<BR>
+  #  For example, if a PCI device contains two UARTs, PcdPciSerialParameters needs
+  #  to contain  two instances of the above structure, with the VendorId and DeviceId
+  #  equals to the Device ID and Vendor ID of the device; If the PCI device uses the
+  #  first two BARs to support two UARTs, BarIndex of first instance equals to 0 and
+  #  BarIndex of second one equals to 1; If the PCI device uses the first BAR to
+  #  support both UARTs, BarIndex of both instance equals to 0, Offset of first
+  #  instance equals to 0 and Offset of second one equals to a value bigger than or
+  #  equal to 8.<BR>
+  #  For certain UART whose register needs to be accessed in DWORD aligned address,
+  #  RegisterStride equals to 4.
+  # @Prompt Pci Serial Parameters
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciSerialParameters|{0xFF, 0xFF}|VOID*|0x00010071
+
+  ## Serial Port Extended Transmit FIFO Size.  The default is 64 bytes.
+  # @Prompt Serial Port Extended Transmit FIFO Size in Bytes
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialExtendedTxFifoSize|64|UINT32|0x00010068
+
+  ## This PCD points to the file name GUID of the BootManagerMenuApp
+  #  Platform can customize the PCD to point to different application for Boot Manager Menu
+  # @Prompt Boot Manager Menu File
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBootManagerMenuFile|{ 0xdc, 0x5b, 0xc2, 0xee, 0xf2, 0x67, 0x95, 0x4d, 0xb1, 0xd5, 0xf8, 0x1b, 0x20, 0x39, 0xd1, 0x1d }|VOID*|0x0001006b
+
+  ## This PCD points to the formset GUID of the driver health management form
+  #  The form will be popped up by BDS core when there are Configuration Required driver health intances.
+  #  Platform can customize the PCD to point to different formset.
+  # @Prompt Driver Health Management Form
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDriverHealthConfigureForm|{ 0xf4, 0xd9, 0x96, 0x42, 0xfc, 0xf6, 0xde, 0x4d, 0x86, 0x85, 0x8c, 0xe2, 0xd7, 0x9d, 0x90, 0xf0 }|VOID*|0x0001006c
+
+  ## The number of bytes between registers in serial device.  The default is 1 byte.
+  # @Prompt Serial Port Register Stride in Bytes
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialRegisterStride|1|UINT32|0x0001006d
+
+  ## This PCD to include the driver guid of VFR drivers for VarCheckHiiBin generation.<BR><BR>
+  # Default is gZeroGuid that means no VFR driver will be parsed for VarCheckHiiBin generation.<BR>
+  # If it is set to an all FFs GUID, it means all modules in all FVs will be parsed for VarCheckHiiBin generation.<BR>
+  # @Prompt Driver guid array of VFR drivers for VarCheckHiiBin generation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVarCheckVfrDriverGuidArray|{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }|VOID*|0x3000103A
+
+[PcdsPatchableInModule, PcdsDynamic, PcdsDynamicEx]
+  ## This PCD defines the Console output row. The default value is 25 according to UEFI spec.
+  #  This PCD could be set to 0 then console output would be at max column and max row.
+  # @Prompt Console output row.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutRow|25|UINT32|0x40000006
+
+  ## This PCD defines the Console output column. The default value is 80 according to UEFI spec.
+  #  This PCD could be set to 0 then console output would be at max column and max row.
+  # @Prompt Console output column.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutColumn|80|UINT32|0x40000007
+
+  ## This PCD defines the video horizontal resolution.
+  #  If this PCD is set to 0 then video resolution would be at highest resolution.
+  # @Prompt Video horizontal resolution.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoHorizontalResolution|800|UINT32|0x40000009
+
+  ## This PCD defines the video vertical resolution.
+  #  If this PCD is set to 0 then video resolution would be at highest resolution.
+  # @Prompt Video vertical resolution.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoVerticalResolution|600|UINT32|0x4000000a
+
+  #  The 4 PCDs below are used to specify the video resolution and text mode of text setup.
+  #  To make text setup work in this resolution, PcdVideoHorizontalResolution, PcdVideoVerticalResolution,
+  #  PcdConOutColumn and PcdConOutRow should be created as PcdsDynamic or PcdsDynamicEx in platform DSC file.
+  #  Then BDS setup will update these PCDs defined in MdeModulePkg.dec and reconnect console drivers
+  #  (GraphicsConsole, Terminal, Consplitter) to make the video resolution and text mode work
+  #  for text setup.
+
+  ## Specify the video horizontal resolution of text setup.
+  # @Prompt Video Horizontal Resolution of Text Setup
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoHorizontalResolution|800|UINT32|0x4000000b
+
+  ## Specify the video vertical resolution of text setup.
+  # @Prompt Video Vertical Resolution of Text Setup
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupVideoVerticalResolution|600|UINT32|0x4000000c
+
+  ## Specify the console output column of text setup.
+  # @Prompt Console Output Column of Text Setup
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutColumn|80|UINT32|0x4000000d
+
+  ## Specify the console output row of text setup.
+  # @Prompt Console Output Row of Text Setup
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetupConOutRow|25|UINT32|0x4000000e
+
+[PcdsFixedAtBuild, PcdsPatchableInModule, PcdsDynamic, PcdsDynamicEx]
+  ## UART clock frequency is for the baud rate configuration.
+  # @Prompt Serial Port Clock Rate.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSerialClockRate|1843200|UINT32|0x00010066
+
+  ## This PCD points to the front page formset GUID
+  #  Compare the FormsetGuid or ClassGuid with this PCD value can detect whether in front page
+  # @Prompt Front Page Formset.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrontPageFormSetGuid|{ 0xbc, 0x30, 0x0c, 0x9e,0x06, 0x3f, 0xa6, 0x4b, 0x82, 0x88, 0x9, 0x17, 0x9b, 0x85, 0x5d, 0xbe }|VOID*|0x0001006e
+
+  ## Base address of the NV variable range in flash device.
+  # @Prompt Base address of flash NV variable range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase|0x0|UINT32|0x30000001
+
+  ## Size of the NV variable range. Note that this value should less than or equal to PcdFlashNvStorageFtwSpareSize.
+  #  The root cause is that variable driver will use FTW protocol to reclaim variable region.
+  #  If the length of variable region is larger than FTW spare size, it means the whole variable region can not
+  #  be reflushed through the manner of fault tolerant write.
+  # @Prompt Size of flash NV variable range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableSize|0x0|UINT32|0x30000002
+
+  ## Base address of the FTW spare block range in flash device. Note that this value should be block size aligned.
+  # @Prompt Base address of flash FTW spare block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase|0x0|UINT32|0x30000013
+
+  ## Size of the FTW spare block range. Note that this value should larger than PcdFlashNvStorageVariableSize and block size aligned.
+  # The root cause is that variable driver will use FTW protocol to reclaim variable region.
+  # If the length of variable region is larger than FTW spare size, it means the whole variable region can not
+  # be reflushed through the manner of fault tolerant write.
+  # @Prompt Size of flash FTW spare block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareSize|0x0|UINT32|0x30000014
+
+  ## Base address of the FTW working block range in flash device.
+  # If PcdFlashNvStorageFtwWorkingSize is larger than one block size, this value should be block size aligned.
+  # @Prompt Base address of flash FTW working block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase|0x0|UINT32|0x30000010
+
+  ## Size of the FTW working block range.
+  # If the value is less than one block size, the work space range should not span blocks.
+  # If the value is larger than one block size, it should be block size aligned.
+  # @Prompt Size of flash FTW working block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingSize|0x0|UINT32|0x30000011
+
+  ## 64-bit Base address of the NV variable range in flash device.
+  # @Prompt 64-bit Base address of flash NV variable range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase64|0x0|UINT64|0x80000001
+
+  ## 64-bit Base address of the FTW spare block range in flash device. Note that this value should be block size aligned.
+  # @Prompt 64-bit Base address of flash FTW spare block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase64|0x0|UINT64|0x80000013
+
+  ## 64-bit Base address of the FTW working block range in flash device.
+  # If PcdFlashNvStorageFtwWorkingSize is larger than one block size, this value should be block size aligned.
+  # @Prompt 64-bit Base address of flash FTW working block range.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase64|0x0|UINT64|0x80000010
+
+  ## This PCD defines a reserved memory range for the EMU Variable driver's NV Variable Store.
+  #  The range is valid if non-zero. The memory range size must be PcdVariableStoreSize.
+  # @Prompt Reserved memory range for EMU variable NV storage.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdEmuVariableNvStoreReserved|0|UINT64|0x40000008
+
+  ## This PCD defines the times to print hello world string.
+  #  This PCD is a sample to explain UINT32 PCD usage.
+  # @Prompt HellowWorld print times.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintTimes|1|UINT32|0x40000005
+
+  ## This PCD defines the HelloWorld print string.
+  #  This PCD is a sample to explain String typed PCD usage.
+  # @Prompt HelloWorld print string.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHelloWorldPrintString|L"UEFI Hello World!\n"|VOID*|0x40000004
+
+  ## Indicates the maximum size of the capsule image with a reset flag that the platform can support.
+  #  The default max size is 100MB (0x6400000) for more than one large capsule images.
+  # @Prompt Max size of populated capsule.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizePopulateCapsule|0x6400000|UINT32|0x0001001e
+
+  ## Indicates the maximum size of the capsule image without a reset flag that the platform can support.
+  #  The default max size is 10MB (0xa00000) for the casule image without reset flag setting.
+  # @Prompt Max size of non-populated capsule.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizeNonPopulateCapsule|0xa00000|UINT32|0x0001001f
+
+  ## Null-terminated Unicode string of the firmware vendor name that is the default name filled into the EFI System Table.
+  # @Prompt Firmware vendor.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareVendor|L"EDK II"|VOID*|0x00010050
+
+  ## Firmware revision that is the default revision filled into the EFI System Table.
+  # @Prompt Firmware revision.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareRevision|0x00010000|UINT32|0x00010051
+
+  ## Null-terminated Unicode string that describes the firmware version.
+  # @Prompt Firmware version string.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareVersionString|L""|VOID*|0x00010052
+
+  ## Null-terminated Unicode string that contains the date the firmware was released
+  # @Prompt Firmware release data string.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareReleaseDateString|L""|VOID*|0x00010053
+
+  ## PcdStatusCodeMemorySize is used when PcdStatusCodeUseMemory is set to true.
+  #  (PcdStatusCodeMemorySize * KBytes) is the total taken memory size.<BR><BR>
+  #  The default value in PeiPhase is 1 KBytes.<BR>
+  #  The default value in DxePhase is 128 KBytes.<BR>
+  # @Prompt StatusCode memory size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeMemorySize|1|UINT16|0x00010054
+
+  ## Indicates if to reset system when memory type information changes.<BR><BR>
+  #   TRUE  - Resets system when memory type information changes.<BR>
+  #   FALSE - Does not reset system when memory type information changes.<BR>
+  # @Prompt Reset on memory type information change.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdResetOnMemoryTypeInformationChange|TRUE|BOOLEAN|0x00010056
+
+  ## Specify the foreground color for Subtile text in HII Form Browser. The default value is EFI_BLUE.
+  #  Only following values defined in UEFI specification are valid:<BR><BR>
+  #  0x00 (EFI_BLACK)<BR>
+  #  0x01 (EFI_BLUE)<BR>
+  #  0x02 (EFI_GREEN)<BR>
+  #  0x03 (EFI_CYAN)<BR>
+  #  0x04 (EFI_RED)<BR>
+  #  0x05 (EFI_MAGENTA)<BR>
+  #  0x06 (EFI_BROWN)<BR>
+  #  0x07 (EFI_LIGHTGRAY)<BR>
+  #  0x08 (EFI_DARKGRAY)<BR>
+  #  0x09 (EFI_LIGHTBLUE)<BR>
+  #  0x0A (EFI_LIGHTGREEN)<BR>
+  #  0x0B (EFI_LIGHTCYAN)<BR>
+  #  0x0C (EFI_LIGHTRED)<BR>
+  #  0x0D (EFI_LIGHTMAGENTA)<BR>
+  #  0x0E (EFI_YELLOW)<BR>
+  #  0x0F (EFI_WHITE)<BR>
+  # @Prompt Foreground color for browser subtile.
+  # @ValidRange 0x80000004 | 0x00 - 0x0F
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserSubtitleTextColor|0x01|UINT8|0x00010057
+
+  ## Specify the foreground color for prompt and Question value text in HII Form Browser. The default value is EFI_BLACK.
+  #  Only following values defined in UEFI specification are valid:<BR><BR>
+  #  0x00 (EFI_BLACK)<BR>
+  #  0x01 (EFI_BLUE)<BR>
+  #  0x02 (EFI_GREEN)<BR>
+  #  0x03 (EFI_CYAN)<BR>
+  #  0x04 (EFI_RED)<BR>
+  #  0x05 (EFI_MAGENTA)<BR>
+  #  0x06 (EFI_BROWN)<BR>
+  #  0x07 (EFI_LIGHTGRAY)<BR>
+  #  0x08 (EFI_DARKGRAY)<BR>
+  #  0x09 (EFI_LIGHTBLUE)<BR>
+  #  0x0A (EFI_LIGHTGREEN)<BR>
+  #  0x0B (EFI_LIGHTCYAN)<BR>
+  #  0x0C (EFI_LIGHTRED)<BR>
+  #  0x0D (EFI_LIGHTMAGENTA)<BR>
+  #  0x0E (EFI_YELLOW)<BR>
+  #  0x0F (EFI_WHITE)<BR>
+  # @Prompt Foreground color for browser field.
+  # @ValidRange 0x80000004 | 0x00 - 0x0F
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldTextColor|0x00|UINT8|0x00010058
+
+  ## Specify the foreground color for highlighted prompt and Question value text in HII Form Browser.
+  #  The default value is EFI_LIGHTGRAY. Only following values defined in UEFI specification are valid:<BR><BR>
+  #  0x00 (EFI_BLACK)<BR>
+  #  0x01 (EFI_BLUE)<BR>
+  #  0x02 (EFI_GREEN)<BR>
+  #  0x03 (EFI_CYAN)<BR>
+  #  0x04 (EFI_RED)<BR>
+  #  0x05 (EFI_MAGENTA)<BR>
+  #  0x06 (EFI_BROWN)<BR>
+  #  0x07 (EFI_LIGHTGRAY)<BR>
+  #  0x08 (EFI_DARKGRAY)<BR>
+  #  0x09 (EFI_LIGHTBLUE)<BR>
+  #  0x0A (EFI_LIGHTGREEN)<BR>
+  #  0x0B (EFI_LIGHTCYAN)<BR>
+  #  0x0C (EFI_LIGHTRED)<BR>
+  #  0x0D (EFI_LIGHTMAGENTA)<BR>
+  #  0x0E (EFI_YELLOW)<BR>
+  #  0x0F (EFI_WHITE)<BR>
+  # @Prompt Foreground color for highlighted browser field.
+  # @ValidRange 0x80000004 | 0x00 - 0x0F
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldTextHighlightColor|0x07|UINT8|0x00010059
+
+  ## Specify the background color for highlighted prompt and Question value text in HII Form Browser.
+  #  The default value is EFI_BACKGROUND_BLACK. Only following values defined in UEFI specification are valid:<BR><BR>
+  #  0x00 (EFI_BACKGROUND_BLACK)<BR>
+  #  0x10 (EFI_BACKGROUND_BLUE)<BR>
+  #  0x20 (EFI_BACKGROUND_GREEN)<BR>
+  #  0x30 (EFI_BACKGROUND_CYAN)<BR>
+  #  0x40 (EFI_BACKGROUND_RED)<BR>
+  #  0x50 (EFI_BACKGROUND_MAGENTA)<BR>
+  #  0x60 (EFI_BACKGROUND_BROWN)<BR>
+  #  0x70 (EFI_BACKGROUND_LIGHTGRAY)<BR>
+  # @Prompt Background color for highlighted browser field.
+  # @ValidList  0x80000005 | 0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserFieldBackgroundHighlightColor|0x00|UINT8|0x0001005A
+
+  ## Time in second to delay for SATA devices to spin-up for recovery.
+  # @Prompt SATA spin-up delay time in second for recovery path.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSataSpinUpDelayInSecForRecoveryPath|15|UINT16|0x0001005B
+
+  ## This PCD is used to specify memory size with page number for a pre-allocated ACPI reserved memory
+  #  to hold runtime(after SmmReadyToLock) created S3 boot script entries. The default page number is 2.
+  #  When changing the value of this PCD, the platform developer should make sure the memory size is
+  #  large enough to hold the S3 boot script node created in runtime(after SmmReadyToLock) phase.
+  # @Prompt Reserved page number for S3 Boot Script Runtime Table.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptRuntimeTableReservePageNumber|0x2|UINT16|0x0001005C
+
+  ## The PCD is used to specify the stack size when capsule IA32 PEI transfers to long mode in PEI phase.
+  #  The default size is 32K. When changing the value of this PCD, the platform developer should
+  #  make sure the memory size is large enough to meet capsule PEI requirement in capsule update path.
+  # @Prompt Stack size for CapsulePei transfer to long mode.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdCapsulePeiLongModeStackSize|0x8000|UINT32|0x0001005D
+
+  ## Indicates if 1G page table will be enabled.<BR><BR>
+  #   TRUE  - 1G page table will be enabled.<BR>
+  #   FALSE - 1G page table will not be enabled.<BR>
+  # @Prompt Enable 1G page table support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable|FALSE|BOOLEAN|0x0001005E
+
+  ## Indicates if the Single Root I/O virtualization is supported.<BR><BR>
+  #   TRUE  - Single Root I/O virtualization is supported.<BR>
+  #   FALSE - Single Root I/O virtualization is not supported.<BR>
+  # @Prompt Enable SRIOV support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSupport|TRUE|BOOLEAN|0x10000044
+
+  ## Indicates if the Alternative Routing-ID is supported.<BR><BR>
+  #   TRUE  - Alternative Routing-ID is supported.<BR>
+  #   FALSE - Alternative Routing-ID is not supported.<BR>
+  # @Prompt Enable ARI support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAriSupport|TRUE|BOOLEAN|0x10000045
+
+  ## Indicates if the Multi Root I/O virtualization is supported.<BR><BR>
+  #   TRUE  - Multi Root I/O virtualization is supported.<BR>
+  #   FALSE - Multi Root I/O virtualization is not supported.<BR>
+  # @Prompt Enable MRIOV support.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMrIovSupport|FALSE|BOOLEAN|0x10000046
+
+  ## Single root I/O virtualization virtual function memory BAR alignment.<BR><BR>
+  #  BITN set indicates 2 of n+12 power<BR>
+  #  BIT0 set indicates 4KB alignment<BR>
+  #  BIT1 set indicates 8KB alignment<BR>
+  # @Prompt SRIOV system page size.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSrIovSystemPageSize|0x1|UINT32|0x10000047
+
+  ## SMBIOS version.
+  # @Prompt SMBIOS version.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosVersion|0x0300|UINT16|0x00010055
+
+  ## SMBIOS Docrev field in SMBIOS 3.0 (64-bit) Entry Point Structure.
+  # @Prompt SMBIOS Docrev field in SMBIOS 3.0 (64-bit) Entry Point Structure.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosDocRev|0x0|UINT8|0x0001006A
+
+  ## SMBIOS produce method.
+  #  BIT0 set indicates 32-bit entry point and table are produced.<BR>
+  #  BIT1 set indicates 64-bit entry point and table are produced.<BR>
+  # @Prompt The policy to produce SMBIOS entry point and table.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosEntryPointProvideMethod|0x3|UINT32|0x00010069
+
+  ## This PCD specifies the additional pad size in FPDT Basic Boot Performance Table for
+  #  the extension FPDT boot records received after ReadyToBoot and before ExitBootService.
+  # @Prompt Pad size for extension FPDT boot records.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdExtFpdtBootRecordPadSize|0x0|UINT32|0x0001005F
+
+  ## Indicates if ConIn device are connected on demand.<BR><BR>
+  #   TRUE  - ConIn device are not connected during BDS and ReadKeyStroke/ReadKeyStrokeEx produced
+  #           by Consplitter should be called before any real key read operation.<BR>
+  #   FALSE - ConIn device may be connected normally during BDS.<BR>
+  # @Prompt ConIn connect on demand.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConInConnectOnDemand|FALSE|BOOLEAN|0x10000060
+
+  ## Indicates if the S.M.A.R.T feature of attached ATA hard disks will be enabled.<BR><BR>
+  #   TRUE  - S.M.A.R.T feature of attached ATA hard disks will be enabled.<BR>
+  #   FALSE - S.M.A.R.T feature of attached ATA hard disks will be default status.<BR>
+  # @Prompt Enable ATA S.M.A.R.T feature.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAtaSmartEnable|TRUE|BOOLEAN|0x00010065
+
+  ## Indicates if full PCI enumeration is disabled.<BR><BR>
+  #   TRUE  - Full PCI enumeration is disabled.<BR>
+  #   FALSE - Full PCI enumeration is not disabled.<BR>
+  # @Prompt Disable full PCI enumeration.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPciDisableBusEnumeration|FALSE|BOOLEAN|0x10000048
+
+  ## Disk I/O - Number of Data Buffer block.
+  # Define the size in block of the pre-allocated buffer. It provide better
+  # performance for large Disk I/O requests.
+  # @Prompt Disk I/O - Number of Data Buffer block.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDiskIoDataBufferBlockNum|64|UINT32|0x30001039
+
+  ## This PCD specifies the PCI-based UFS host controller mmio base address.
+  # Define the mmio base address of the pci-based UFS host controller. If there are multiple UFS
+  # host controllers, their mmio base addresses are calculated one by one from this base address.
+  # @Prompt Mmio base address of pci-based UFS host controller.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUfsPciHostControllerMmioBase|0xd0000000|UINT32|0x10000061
+
+  ## Specify Max ESRT cache entry number supported for FMP instances
+  #
+  # @Prompt Max FMP ESRT entry number to be synced & cached in repository.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxFmpEsrtCacheNum|32|UINT32|0x0000006b
+
+  ## Specify Max ESRT cache entry number supported for Non FMP instances
+  #
+  # @Prompt Max Non-FMP ESRT entry number to be cached in repository.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxNonFmpEsrtCacheNum|32|UINT32|0x0000006c
+
+  ## Specify of Capsule Flag defined by CapsuleGuid to request system reboot after capsule process
+  #
+  # @Prompt Flag to request system reboot after processing capsule.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSystemRebootAfterCapsuleProcessFlag|0x0001|UINT16|0x0000006d
+
+  ## Publish PropertiesTable or not.
+  #
+  # If this PCD is TRUE, DxeCore publishs PropertiesTable.
+  # DxeCore evaluates if all runtime drivers has 4K aligned PE sections. If all
+  # PE sections in runtime drivers are 4K aligned, DxeCore sets BIT0 in
+  # PropertiesTable. Or DxeCore clears BIT0 in PropertiesTable.
+  # If this PCD is FALSE, DxeCore does not publish PropertiesTable.
+  #
+  # If PropertiesTable has BIT0 set, DxeCore uses below policy in UEFI memory map:
+  #   1) Use EfiRuntimeServicesCode for runtime driver PE image code section and
+  #      use EfiRuntimeServicesData for runtime driver PE image header and other section.
+  #   2) Set EfiRuntimeServicesCode to be EFI_MEMORY_RO.
+  #   3) Set EfiRuntimeServicesData to be EFI_MEMORY_XP.
+  #   4) Set EfiMemoryMappedIO and EfiMemoryMappedIOPortSpace to be EFI_MEMORY_XP.
+  #
+  # NOTE: Platform need gurantee this PCD is set correctly. Platform should set
+  # this PCD to be TURE if and only if all runtime driver has seperated Code/Data
+  # section. If PE code/data sections are merged, the result is unpredictable.
+  #
+  # UEFI 2.6 specification does not recommend to use this BIT0 attribute.
+  #
+  # @Prompt Publish UEFI PropertiesTable.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPropertiesTableEnable|FALSE|BOOLEAN|0x0000006e
+
+  ## Default OEM ID for ACPI table creation, its length must be 0x6 bytes to follow ACPI specification.
+  # @Prompt Default OEM ID for ACPI table creation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemId|"INTEL "|VOID*|0x30001034
+
+  ## Default OEM Table ID for ACPI table creation, it is "EDK2    ".
+  #  Accroding to ACPI specification, this field is particularly useful when
+  #  defining a definition block to distinguish definition block functions.
+  #  The OEM assigns each dissimilar table a new OEM Table ID.
+  #  This PCD is ignored for definition block.
+  # @Prompt Default OEM Table ID for ACPI table creation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemTableId|0x20202020324B4445|UINT64|0x30001035
+
+  ## Default OEM Revision for ACPI table creation.
+  #  Accroding to ACPI specification, for LoadTable() opcode, the OS can also
+  #  check the OEM Table ID and Revision ID against a database for a newer
+  #  revision Definition Block of the same OEM Table ID and load it instead.
+  #  This PCD is ignored for definition block.
+  # @Prompt Default OEM Revision for ACPI table creation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision|0x00000002|UINT32|0x30001036
+
+  ## Default Creator ID for ACPI table creation.
+  #  Accroding to ACPI specification, for tables containing Definition Blocks,
+  #  this is the ID for the ASL Compiler.
+  #  This PCD is ignored for definition block.
+  # @Prompt Default Creator ID for ACPI table creation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorId|0x20202020|UINT32|0x30001037
+
+  ## Default Creator Revision for ACPI table creation.
+  #  Accroding to ACPI specification, for tables containing Definition Blocks,
+  #  this is the revision for the ASL Compiler.
+  #  This PCD is ignored for definition block.
+  # @Prompt Default Creator Revision for ACPI table creation.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorRevision|0x01000013|UINT32|0x30001038
+
+  ## Indicates if to set NX for stack.<BR><BR>
+  #  For the DxeIpl and the DxeCore are both X64, set NX for stack feature also require PcdDxeIplBuildPageTables be TRUE.<BR>
+  #  For the DxeIpl and the DxeCore are both IA32 (PcdDxeIplSwitchToLongMode is FALSE), set NX for stack feature also require
+  #  IA32 PAE is supported and Execute Disable Bit is available.<BR>
+  #   TRUE  - to set NX for stack.<BR>
+  #   FALSE - Not to set NX for stack.<BR>
+  # @Prompt Set NX for stack.
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSetNxForStack|FALSE|BOOLEAN|0x0001006f
+
+[PcdsPatchableInModule]
+  ## Specify memory size with page number for PEI code when
+  #  Loading Module at Fixed Address feature is enabled.
+  #  The value will be set by the build tool.
+  # @Prompt LMFA PEI code page number.
+  # @ValidList  0x80000001 | 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressPeiCodePageNumber|0|UINT32|0x00000029
+
+  ## Specify memory size with page number for DXE boot time code when
+  #  Loading Module at Fixed Address feature is enabled.
+  #  The value will be set by the build tool.
+  # @Prompt LMFA DXE boot code page number.
+  # @ValidList  0x80000001 | 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressBootTimeCodePageNumber|0|UINT32|0x0000002a
+
+  ## Specify memory size with page number for DXE runtime code when
+  #  Loading Module at Fixed Address feature is enabled.
+  #  The value will be set by the build tool.
+  # @Prompt LMFA DXE runtime code page number.
+  # @ValidList  0x80000001 | 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressRuntimeCodePageNumber|0|UINT32|0x0000002b
+
+  ## Specify memory size with page number for SMM code when
+  #  Loading Module at Fixed Address feature is enabled.
+  #  The value will be set by the build tool.
+  # @Prompt LMFA SMM code page number.
+  # @ValidList  0x80000001 | 0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdLoadFixAddressSmmCodePageNumber|0|UINT32|0x0000002c
+
+[PcdsDynamic, PcdsDynamicEx]
+  ## This dynamic PCD hold an address to point to private data structure used in DxeS3BootScriptLib library
+  #  instance which records the S3 boot script table start address, length, etc. To introduce this PCD is
+  #  only for DxeS3BootScriptLib instance implementation purpose. The platform developer should make sure the
+  #  default value is set to Zero. And the PCD is assumed ONLY to be accessed in DxeS3BootScriptLib Library.
+  # @Prompt S3 Boot Script Table Private Data pointer.
+  # @ValidList  0x80000001 | 0x0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptTablePrivateDataPtr|0x0|UINT64|0x00030000
+
+  ## This dynamic PCD hold an address to point to private data structure SMM copy used in DxeS3BootScriptLib library
+  #  instance which records the S3 boot script table start address, length, etc. To introduce this PCD is
+  #  only for DxeS3BootScriptLib instance implementation purpose. The platform developer should make sure the
+  #  default value is set to Zero. And the PCD is assumed ONLY to be accessed in DxeS3BootScriptLib Library.
+  # @Prompt S3 Boot Script Table Private Smm Data pointer.
+  # @ValidList  0x80000001 | 0x0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdS3BootScriptTablePrivateSmmDataPtr|0x0|UINT64|0x00030001
+
+  ## This dynamic PCD hold an address to point to the memory of page table. The page table establishes a 1:1
+  #  Virtual to Physical mapping according to the processor physical address bits.
+  # @Prompt Identify Mapping Page Table pointer.
+  # @ValidList  0x80000001 | 0x0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdIdentifyMappingPageTablePtr|0x0|UINT64|0x00030002
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  MdeModulePkgExtra.uni
diff --git a/Core/MdeModulePkg/MdeModulePkg.dsc b/Core/MdeModulePkg/MdeModulePkg.dsc
new file mode 100644
index 0000000000..30e546da1d
--- /dev/null
+++ b/Core/MdeModulePkg/MdeModulePkg.dsc
@@ -0,0 +1,429 @@
+## @file
+# EFI/PI Reference Module Package for All Architectures
+#
+# (C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+# Copyright (c) 2007 - 2016, 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.
+#
+##
+
+[Defines]
+  PLATFORM_NAME                  = MdeModule
+  PLATFORM_GUID                  = 587CE499-6CBE-43cd-94E2-186218569478
+  PLATFORM_VERSION               = 0.96
+  DSC_SPECIFICATION              = 0x00010005
+  OUTPUT_DIRECTORY               = Build/MdeModule
+  SUPPORTED_ARCHITECTURES        = IA32|IPF|X64|EBC|ARM|AARCH64
+  BUILD_TARGETS                  = DEBUG|RELEASE
+  SKUID_IDENTIFIER               = DEFAULT
+
+[LibraryClasses]
+  #
+  # Entry point
+  #
+  PeiCoreEntryPoint|MdePkg/Library/PeiCoreEntryPoint/PeiCoreEntryPoint.inf
+  PeimEntryPoint|MdePkg/Library/PeimEntryPoint/PeimEntryPoint.inf
+  DxeCoreEntryPoint|MdePkg/Library/DxeCoreEntryPoint/DxeCoreEntryPoint.inf
+  UefiDriverEntryPoint|MdePkg/Library/UefiDriverEntryPoint/UefiDriverEntryPoint.inf
+  UefiApplicationEntryPoint|MdePkg/Library/UefiApplicationEntryPoint/UefiApplicationEntryPoint.inf
+  #
+  # Basic
+  #
+  BaseLib|MdePkg/Library/BaseLib/BaseLib.inf
+  BaseMemoryLib|MdePkg/Library/BaseMemoryLib/BaseMemoryLib.inf
+  SynchronizationLib|MdePkg/Library/BaseSynchronizationLib/BaseSynchronizationLib.inf
+  PrintLib|MdePkg/Library/BasePrintLib/BasePrintLib.inf
+  IoLib|MdePkg/Library/BaseIoLibIntrinsic/BaseIoLibIntrinsic.inf
+  PciLib|MdePkg/Library/BasePciLibCf8/BasePciLibCf8.inf
+  PciCf8Lib|MdePkg/Library/BasePciCf8Lib/BasePciCf8Lib.inf
+  PciSegmentLib|MdePkg/Library/BasePciSegmentLibPci/BasePciSegmentLibPci.inf
+  CacheMaintenanceLib|MdePkg/Library/BaseCacheMaintenanceLib/BaseCacheMaintenanceLib.inf
+  PeCoffLib|MdePkg/Library/BasePeCoffLib/BasePeCoffLib.inf
+  PeCoffGetEntryPointLib|MdePkg/Library/BasePeCoffGetEntryPointLib/BasePeCoffGetEntryPointLib.inf
+  SortLib|MdeModulePkg/Library/BaseSortLib/BaseSortLib.inf
+  #
+  # UEFI & PI
+  #
+  UefiBootServicesTableLib|MdePkg/Library/UefiBootServicesTableLib/UefiBootServicesTableLib.inf
+  UefiRuntimeServicesTableLib|MdePkg/Library/UefiRuntimeServicesTableLib/UefiRuntimeServicesTableLib.inf
+  UefiRuntimeLib|MdePkg/Library/UefiRuntimeLib/UefiRuntimeLib.inf
+  UefiLib|MdePkg/Library/UefiLib/UefiLib.inf
+  UefiHiiServicesLib|MdeModulePkg/Library/UefiHiiServicesLib/UefiHiiServicesLib.inf
+  HiiLib|MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf
+  DevicePathLib|MdePkg/Library/UefiDevicePathLib/UefiDevicePathLib.inf
+  UefiDecompressLib|MdePkg/Library/BaseUefiDecompressLib/BaseUefiDecompressLib.inf
+  PeiServicesTablePointerLib|MdePkg/Library/PeiServicesTablePointerLib/PeiServicesTablePointerLib.inf
+  PeiServicesLib|MdePkg/Library/PeiServicesLib/PeiServicesLib.inf
+  DxeServicesLib|MdePkg/Library/DxeServicesLib/DxeServicesLib.inf
+  DxeServicesTableLib|MdePkg/Library/DxeServicesTableLib/DxeServicesTableLib.inf
+  UefiBootManagerLib|MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.inf
+  #
+  # Generic Modules
+  #
+  UefiUsbLib|MdePkg/Library/UefiUsbLib/UefiUsbLib.inf
+  UefiScsiLib|MdePkg/Library/UefiScsiLib/UefiScsiLib.inf
+  NetLib|MdeModulePkg/Library/DxeNetLib/DxeNetLib.inf
+  IpIoLib|MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.inf
+  UdpIoLib|MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.inf
+  TcpIoLib|MdeModulePkg/Library/DxeTcpIoLib/DxeTcpIoLib.inf
+  DpcLib|MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.inf
+  SecurityManagementLib|MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.inf
+  TimerLib|MdePkg/Library/BaseTimerLibNullTemplate/BaseTimerLibNullTemplate.inf
+  SerialPortLib|MdePkg/Library/BaseSerialPortLibNull/BaseSerialPortLibNull.inf
+  CapsuleLib|MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.inf
+  PcdLib|MdePkg/Library/BasePcdLibNull/BasePcdLibNull.inf
+  PalLib|MdePkg/Library/BasePalLibNull/BasePalLibNull.inf
+  CustomizedDisplayLib|MdeModulePkg/Library/CustomizedDisplayLib/CustomizedDisplayLib.inf
+  #
+  # Misc
+  #
+  DebugLib|MdePkg/Library/BaseDebugLibNull/BaseDebugLibNull.inf
+  DebugPrintErrorLevelLib|MdePkg/Library/BaseDebugPrintErrorLevelLib/BaseDebugPrintErrorLevelLib.inf  
+  ReportStatusCodeLib|MdePkg/Library/BaseReportStatusCodeLibNull/BaseReportStatusCodeLibNull.inf  
+  PeCoffExtraActionLib|MdePkg/Library/BasePeCoffExtraActionLibNull/BasePeCoffExtraActionLibNull.inf
+  PerformanceLib|MdePkg/Library/BasePerformanceLibNull/BasePerformanceLibNull.inf
+  DebugAgentLib|MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf
+  PlatformHookLib|MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.inf
+  ResetSystemLib|MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.inf
+  SmbusLib|MdePkg/Library/DxeSmbusLib/DxeSmbusLib.inf
+  S3BootScriptLib|MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.inf
+  CpuExceptionHandlerLib|MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.inf
+  PlatformBootManagerLib|MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.inf
+  PciHostBridgeLib|MdeModulePkg/Library/PciHostBridgeLibNull/PciHostBridgeLibNull.inf
+  TpmMeasurementLib|MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.inf
+  AuthVariableLib|MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.inf
+  VarCheckLib|MdeModulePkg/Library/VarCheckLib/VarCheckLib.inf
+
+[LibraryClasses.EBC.PEIM]
+  IoLib|MdePkg/Library/PeiIoLibCpuIo/PeiIoLibCpuIo.inf
+
+[LibraryClasses.common.PEI_CORE]
+  HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf
+  MemoryAllocationLib|MdePkg/Library/PeiMemoryAllocationLib/PeiMemoryAllocationLib.inf
+
+[LibraryClasses.common.PEIM]
+  HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf
+  MemoryAllocationLib|MdePkg/Library/PeiMemoryAllocationLib/PeiMemoryAllocationLib.inf
+  ExtractGuidedSectionLib|MdePkg/Library/PeiExtractGuidedSectionLib/PeiExtractGuidedSectionLib.inf
+  LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf
+
+[LibraryClasses.common.DXE_CORE]
+  HobLib|MdePkg/Library/DxeCoreHobLib/DxeCoreHobLib.inf
+  MemoryAllocationLib|MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.inf
+  ExtractGuidedSectionLib|MdePkg/Library/DxeExtractGuidedSectionLib/DxeExtractGuidedSectionLib.inf
+
+[LibraryClasses.common.DXE_DRIVER]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
+  MemoryAllocationLib|MdePkg/Library/UefiMemoryAllocationLib/UefiMemoryAllocationLib.inf
+  ExtractGuidedSectionLib|MdePkg/Library/DxeExtractGuidedSectionLib/DxeExtractGuidedSectionLib.inf
+
+[LibraryClasses.common.DXE_RUNTIME_DRIVER]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  MemoryAllocationLib|MdePkg/Library/UefiMemoryAllocationLib/UefiMemoryAllocationLib.inf
+  DebugLib|MdePkg/Library/UefiDebugLibConOut/UefiDebugLibConOut.inf
+  LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
+
+[LibraryClasses.common.SMM_CORE]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  MemoryAllocationLib|MdeModulePkg/Library/PiSmmCoreMemoryAllocationLib/PiSmmCoreMemoryAllocationLib.inf
+  SmmServicesTableLib|MdeModulePkg/Library/PiSmmCoreSmmServicesTableLib/PiSmmCoreSmmServicesTableLib.inf
+  SmmCorePlatformHookLib|MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.inf  
+  SmmMemLib|MdePkg/Library/SmmMemLib/SmmMemLib.inf
+
+[LibraryClasses.common.DXE_SMM_DRIVER]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  DebugLib|MdePkg/Library/UefiDebugLibConOut/UefiDebugLibConOut.inf
+  MemoryAllocationLib|MdePkg/Library/SmmMemoryAllocationLib/SmmMemoryAllocationLib.inf
+  SmmServicesTableLib|MdePkg/Library/SmmServicesTableLib/SmmServicesTableLib.inf
+  LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.inf
+  SmmMemLib|MdePkg/Library/SmmMemLib/SmmMemLib.inf
+
+[LibraryClasses.common.UEFI_DRIVER]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  MemoryAllocationLib|MdePkg/Library/UefiMemoryAllocationLib/UefiMemoryAllocationLib.inf
+  DebugLib|MdePkg/Library/UefiDebugLibConOut/UefiDebugLibConOut.inf
+  LockBoxLib|MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
+
+[LibraryClasses.common.UEFI_APPLICATION]
+  HobLib|MdePkg/Library/DxeHobLib/DxeHobLib.inf
+  MemoryAllocationLib|MdePkg/Library/UefiMemoryAllocationLib/UefiMemoryAllocationLib.inf
+  DebugLib|MdePkg/Library/UefiDebugLibStdErr/UefiDebugLibStdErr.inf
+
+[LibraryClasses.ARM, LibraryClasses.AARCH64]
+  #
+  # It is not possible to prevent ARM compiler calls to generic intrinsic functions.
+  # This library provides the instrinsic functions generated by a given compiler.
+  # [LibraryClasses.ARM] and NULL mean link this library into all ARM images.
+  #
+  NULL|ArmPkg/Library/CompilerIntrinsicsLib/CompilerIntrinsicsLib.inf
+
+[LibraryClasses.EBC]
+  LockBoxLib|MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.inf
+
+[PcdsFeatureFlag]
+  gEfiMdePkgTokenSpaceGuid.PcdDriverDiagnostics2Disable|TRUE
+  gEfiMdePkgTokenSpaceGuid.PcdComponentName2Disable|TRUE
+  gEfiMdeModulePkgTokenSpaceGuid.PcdInstallAcpiSdtProtocol|TRUE
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathFromText|FALSE
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathToText|FALSE
+
+[PcdsFixedAtBuild]
+  gEfiMdePkgTokenSpaceGuid.PcdDebugPropertyMask|0x0f
+  gEfiMdePkgTokenSpaceGuid.PcdReportStatusCodePropertyMask|0x06
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizeNonPopulateCapsule|0x0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizePopulateCapsule|0x0
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPerformanceLogEntries|28
+
+[PcdsFixedAtBuild.IPF]
+  gEfiMdePkgTokenSpaceGuid.PcdIoBlockBaseAddressForIpf|0x0ffffc000000
+
+###################################################################################################
+#
+# Components Section - list of the modules and components that will be processed by compilation
+#                      tools and the EDK II tools to generate PE32/PE32+/Coff image files.
+#
+# Note: The EDK II DSC file is not used to specify how compiled binary images get placed
+#       into firmware volume images. This section is just a list of modules to compile from
+#       source into UEFI-compliant binaries.
+#       It is the FDF file that contains information on combining binary files into firmware
+#       volume images, whose concept is beyond UEFI and is described in PI specification.
+#       Binary modules do not need to be listed in this section, as they should be
+#       specified in the FDF file. For example: Shell binary (Shell_Full.efi), FAT binary (Fat.efi),
+#       Logo (Logo.bmp), and etc.
+#       There may also be modules listed in this section that are not required in the FDF file,
+#       When a module listed here is excluded from FDF file, then UEFI-compliant binary will be
+#       generated for it, but the binary will not be put into any firmware volume.
+#
+###################################################################################################
+
+[Components]
+  MdeModulePkg/Application/HelloWorld/HelloWorld.inf
+  MdeModulePkg/Application/MemoryProfileInfo/MemoryProfileInfo.inf
+
+  MdeModulePkg/Bus/Pci/PciHostBridgeDxe/PciHostBridgeDxe.inf
+  MdeModulePkg/Bus/Pci/PciSioSerialDxe/PciSioSerialDxe.inf
+  MdeModulePkg/Bus/Pci/PciBusDxe/PciBusDxe.inf
+  MdeModulePkg/Bus/Pci/IncompatiblePciDeviceSupportDxe/IncompatiblePciDeviceSupportDxe.inf
+  MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressDxe.inf
+  MdeModulePkg/Bus/Pci/UfsPciHcDxe/UfsPciHcDxe.inf
+  MdeModulePkg/Bus/Ufs/UfsPassThruDxe/UfsPassThruDxe.inf
+  MdeModulePkg/Bus/Pci/UfsPciHcPei/UfsPciHcPei.inf
+  MdeModulePkg/Bus/Ufs/UfsBlockIoPei/UfsBlockIoPei.inf
+  MdeModulePkg/Bus/Pci/XhciDxe/XhciDxe.inf
+  MdeModulePkg/Bus/Pci/EhciDxe/EhciDxe.inf
+  MdeModulePkg/Bus/Pci/UhciDxe/UhciDxe.inf
+  MdeModulePkg/Bus/Pci/UhciPei/UhciPei.inf
+  MdeModulePkg/Bus/Pci/EhciPei/EhciPei.inf
+  MdeModulePkg/Bus/Pci/XhciPei/XhciPei.inf
+  MdeModulePkg/Bus/Pci/IdeBusPei/IdeBusPei.inf
+  MdeModulePkg/Bus/Usb/UsbBusPei/UsbBusPei.inf
+  MdeModulePkg/Bus/Usb/UsbBotPei/UsbBotPei.inf
+  MdeModulePkg/Bus/Ata/AtaBusDxe/AtaBusDxe.inf
+  MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AtaAtapiPassThru.inf
+  MdeModulePkg/Bus/Scsi/ScsiBusDxe/ScsiBusDxe.inf
+  MdeModulePkg/Bus/Scsi/ScsiDiskDxe/ScsiDiskDxe.inf
+  MdeModulePkg/Bus/Usb/UsbBusDxe/UsbBusDxe.inf
+  MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
+  MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
+  MdeModulePkg/Bus/Usb/UsbMouseAbsolutePointerDxe/UsbMouseAbsolutePointerDxe.inf
+  MdeModulePkg/Bus/Usb/UsbMouseDxe/UsbMouseDxe.inf
+  MdeModulePkg/Bus/I2c/I2cDxe/I2cBusDxe.inf
+  MdeModulePkg/Bus/I2c/I2cDxe/I2cHostDxe.inf
+  MdeModulePkg/Bus/I2c/I2cDxe/I2cDxe.inf
+  MdeModulePkg/Bus/Isa/IsaBusDxe/IsaBusDxe.inf
+
+  MdeModulePkg/Core/Dxe/DxeMain.inf {
+    <LibraryClasses>
+      NULL|MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf
+  }
+  MdeModulePkg/Core/DxeIplPeim/DxeIpl.inf
+  MdeModulePkg/Core/Pei/PeiMain.inf
+  MdeModulePkg/Core/RuntimeDxe/RuntimeDxe.inf
+
+  MdeModulePkg/Library/DxeCapsuleLibNull/DxeCapsuleLibNull.inf
+  MdeModulePkg/Library/DxeCoreMemoryAllocationLib/DxeCoreMemoryAllocationLib.inf
+  MdeModulePkg/Library/DxeCorePerformanceLib/DxeCorePerformanceLib.inf
+  MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf
+  MdeModulePkg/Library/DxeDpcLib/DxeDpcLib.inf
+  MdeModulePkg/Library/DxeIpIoLib/DxeIpIoLib.inf
+  MdeModulePkg/Library/DxeNetLib/DxeNetLib.inf
+  MdeModulePkg/Library/DxePerformanceLib/DxePerformanceLib.inf
+  MdeModulePkg/Library/DxeUdpIoLib/DxeUdpIoLib.inf
+  MdeModulePkg/Library/DxePrintLibPrint2Protocol/DxePrintLibPrint2Protocol.inf
+  MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.inf
+  MdeModulePkg/Library/PeiPerformanceLib/PeiPerformanceLib.inf
+  MdeModulePkg/Library/PeiRecoveryLibNull/PeiRecoveryLibNull.inf
+  MdeModulePkg/Library/PeiS3LibNull/PeiS3LibNull.inf
+  MdeModulePkg/Library/UefiHiiLib/UefiHiiLib.inf
+  MdeModulePkg/Library/BaseResetSystemLibNull/BaseResetSystemLibNull.inf
+  MdeModulePkg/Library/DxeSecurityManagementLib/DxeSecurityManagementLib.inf
+  MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.inf
+  MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.inf
+  MdeModulePkg/Library/DxeReportStatusCodeLib/DxeReportStatusCodeLib.inf
+  MdeModulePkg/Library/RuntimeDxeReportStatusCodeLib/RuntimeDxeReportStatusCodeLib.inf
+  MdeModulePkg/Library/BaseSerialPortLib16550/BaseSerialPortLib16550.inf
+  MdeModulePkg/Library/BasePlatformHookLibNull/BasePlatformHookLibNull.inf
+  MdeModulePkg/Library/DxeDebugPrintErrorLevelLib/DxeDebugPrintErrorLevelLib.inf
+  MdeModulePkg/Library/PiDxeS3BootScriptLib/DxeS3BootScriptLib.inf
+  MdeModulePkg/Library/PeiDebugPrintHobLib/PeiDebugPrintHobLib.inf
+  MdeModulePkg/Library/CpuExceptionHandlerLibNull/CpuExceptionHandlerLibNull.inf
+  MdeModulePkg/Library/PlatformHookLibSerialPortPpi/PlatformHookLibSerialPortPpi.inf
+  MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaCustomDecompressLib.inf
+  MdeModulePkg/Library/PeiDxeDebugLibReportStatusCode/PeiDxeDebugLibReportStatusCode.inf
+  MdeModulePkg/Library/UefiBootManagerLib/UefiBootManagerLib.inf
+  MdeModulePkg/Library/PlatformBootManagerLibNull/PlatformBootManagerLibNull.inf
+  MdeModulePkg/Library/TpmMeasurementLibNull/TpmMeasurementLibNull.inf
+  MdeModulePkg/Library/AuthVariableLibNull/AuthVariableLibNull.inf
+  MdeModulePkg/Library/VarCheckLib/VarCheckLib.inf
+  MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf
+  MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf
+  MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf
+  MdeModulePkg/Library/PlatformVarCleanupLib/PlatformVarCleanupLib.inf
+  MdeModulePkg/Library/BaseIpmiLibNull/BaseIpmiLibNull.inf
+  MdeModulePkg/Library/DxeIpmiLibIpmiProtocol/DxeIpmiLibIpmiProtocol.inf
+  MdeModulePkg/Library/PeiIpmiLibIpmiPpi/PeiIpmiLibIpmiPpi.inf
+  MdeModulePkg/Library/SmmIpmiLibSmmIpmiProtocol/SmmIpmiLibSmmIpmiProtocol.inf
+
+  MdeModulePkg/Universal/BdsDxe/BdsDxe.inf
+  MdeModulePkg/Application/BootManagerMenuApp/BootManagerMenuApp.inf
+  MdeModulePkg/Application/UiApp/UiApp.inf
+  MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.inf
+  MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.inf
+  MdeModulePkg/Universal/CapsulePei/CapsulePei.inf
+  MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.inf
+  MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.inf
+  MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.inf
+  MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.inf
+  MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.inf
+  MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.inf
+  MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.inf
+  MdeModulePkg/Universal/PrintDxe/PrintDxe.inf
+  MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.inf
+  MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.inf
+  MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.inf
+  MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.inf
+  MdeModulePkg/Universal/DriverSampleDxe/DriverSampleDxe.inf
+  MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseDxe.inf
+  MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.inf
+  MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.inf
+  MdeModulePkg/Universal/Metronome/Metronome.inf
+  MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.inf
+  MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.inf
+  MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.inf
+  MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.inf
+
+  MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.inf
+  MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.inf
+  MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.inf
+  MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.inf
+  MdeModulePkg/Universal/Network/IScsiDxe/IScsiDxe.inf
+  MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.inf
+  MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.inf
+  MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.inf
+  MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.inf
+  MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.inf
+  MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.inf
+
+  MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.inf
+  MdeModulePkg/Universal/PCD/Dxe/Pcd.inf
+  MdeModulePkg/Universal/PCD/Pei/Pcd.inf
+  MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatformDriOverrideDxe.inf
+
+  MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.inf
+  MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.inf
+
+  MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.inf
+  MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserDxe.inf
+  MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineDxe.inf
+  MdeModulePkg/Application/VariableInfo/VariableInfo.inf
+  MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.inf
+  MdeModulePkg/Universal/Variable/Pei/VariablePei.inf
+  MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.inf
+  MdeModulePkg/Universal/TimestampDxe/TimestampDxe.inf
+  MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.inf
+
+  MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf
+  MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf
+  MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleDxe.inf
+  MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.inf
+
+  MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.inf
+  MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.inf
+
+  MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.inf {
+    <LibraryClasses>
+      LockBoxLib|MdeModulePkg/Library/LockBoxNullLib/LockBoxNullLib.inf
+  }
+  MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.inf
+  MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.inf
+  MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.inf {
+    <LibraryClasses>
+      NULL|MdeModulePkg/Library/DxeCrc32GuidedSectionExtractLib/DxeCrc32GuidedSectionExtractLib.inf
+  }
+  MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.inf {
+    <LibraryClasses>
+      NULL|MdeModulePkg/Library/PeiCrc32GuidedSectionExtractLib/PeiCrc32GuidedSectionExtractLib.inf
+  }
+
+  MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemDxe.inf
+  MdeModulePkg/Universal/EsrtDxe/EsrtDxe.inf
+  
+  MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.inf
+
+  MdeModulePkg/Universal/SerialDxe/SerialDxe.inf
+
+[Components.IA32, Components.X64, Components.IPF]  
+  MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxeBcDxe.inf
+  MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.inf
+  MdeModulePkg/Universal/EbcDxe/EbcDxe.inf
+
+[Components.IA32, Components.X64, Components.Ebc]
+  MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.inf {
+    <LibraryClasses>
+      NULL|MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf
+      NULL|MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf
+      NULL|MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf
+  }
+  MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.inf
+  
+[Components.IA32, Components.X64]
+  MdeModulePkg/Core/PiSmmCore/PiSmmIpl.inf
+  MdeModulePkg/Core/PiSmmCore/PiSmmCore.inf
+  MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.inf {
+    <LibraryClasses>
+      NULL|MdeModulePkg/Library/VarCheckUefiLib/VarCheckUefiLib.inf
+      NULL|MdeModulePkg/Library/VarCheckHiiLib/VarCheckHiiLib.inf
+      NULL|MdeModulePkg/Library/VarCheckPcdLib/VarCheckPcdLib.inf
+  }
+  MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.inf
+  MdeModulePkg/Library/SmmReportStatusCodeLib/SmmReportStatusCodeLib.inf
+  MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.inf
+  MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.inf
+  MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.inf
+  MdeModulePkg/Library/SmmCorePerformanceLib/SmmCorePerformanceLib.inf
+  MdeModulePkg/Library/SmmPerformanceLib/SmmPerformanceLib.inf
+  MdeModulePkg/Library/DxeSmmPerformanceLib/DxeSmmPerformanceLib.inf
+  MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxPeiLib.inf
+  MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxDxeLib.inf
+  MdeModulePkg/Library/SmmLockBoxLib/SmmLockBoxSmmLib.inf
+  MdeModulePkg/Library/SmmCorePlatformHookLibNull/SmmCorePlatformHookLibNull.inf
+  MdeModulePkg/Library/LzmaCustomDecompressLib/LzmaArchCustomDecompressLib.inf
+  MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
+  MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.inf
+  MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.inf
+  MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.inf
+  MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.inf
+  MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.inf
+  MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.inf
+
+[Components.X64]
+  MdeModulePkg/Universal/CapsulePei/CapsuleX64.inf
+
diff --git a/Core/MdeModulePkg/MdeModulePkg.uni b/Core/MdeModulePkg/MdeModulePkg.uni
new file mode 100644
index 0000000000..ea385ff178
--- /dev/null
+++ b/Core/MdeModulePkg/MdeModulePkg.uni
diff --git a/Core/MdeModulePkg/MdeModulePkgExtra.uni b/Core/MdeModulePkg/MdeModulePkgExtra.uni
new file mode 100644
index 0000000000..cbee5edbf1
--- /dev/null
+++ b/Core/MdeModulePkg/MdeModulePkgExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.c b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.c
new file mode 100644
index 0000000000..44bdd94fa8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.c
@@ -0,0 +1,260 @@
+/** @file
+  Sample ACPI Platform Driver
+
+  Copyright (c) 2008 - 2011, 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 <PiDxe.h>
+
+#include <Protocol/AcpiTable.h>
+#include <Protocol/FirmwareVolume2.h>
+
+#include <Library/BaseLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+
+#include <IndustryStandard/Acpi.h>
+
+/**
+  Locate the first instance of a protocol.  If the protocol requested is an
+  FV protocol, then it will return the first FV that contains the ACPI table
+  storage file.
+
+  @param  Instance      Return pointer to the first instance of the protocol
+
+  @return EFI_SUCCESS           The function completed successfully.
+  @return EFI_NOT_FOUND         The protocol could not be located.
+  @return EFI_OUT_OF_RESOURCES  There are not enough resources to find the protocol.
+
+**/
+EFI_STATUS
+LocateFvInstanceWithTables (
+  OUT EFI_FIRMWARE_VOLUME2_PROTOCOL **Instance
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    *HandleBuffer;
+  UINTN                         NumberOfHandles;
+  EFI_FV_FILETYPE               FileType;
+  UINT32                        FvStatus;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  UINTN                         Size;
+  UINTN                         Index;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *FvInstance;
+
+  FvStatus = 0;
+
+  //
+  // Locate protocol.
+  //
+  Status = gBS->LocateHandleBuffer (
+                   ByProtocol,
+                   &gEfiFirmwareVolume2ProtocolGuid,
+                   NULL,
+                   &NumberOfHandles,
+                   &HandleBuffer
+                   );
+  if (EFI_ERROR (Status)) {
+    //
+    // Defined errors at this time are not found and out of resources.
+    //
+    return Status;
+  }
+
+
+
+  //
+  // Looking for FV with ACPI storage file
+  //
+
+  for (Index = 0; Index < NumberOfHandles; Index++) {
+    //
+    // Get the protocol on this handle
+    // This should not fail because of LocateHandleBuffer
+    //
+    Status = gBS->HandleProtocol (
+                     HandleBuffer[Index],
+                     &gEfiFirmwareVolume2ProtocolGuid,
+                     (VOID**) &FvInstance
+                     );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // See if it has the ACPI storage file
+    //
+    Status = FvInstance->ReadFile (
+                           FvInstance,
+                           (EFI_GUID*)PcdGetPtr (PcdAcpiTableStorageFile),
+                           NULL,
+                           &Size,
+                           &FileType,
+                           &Attributes,
+                           &FvStatus
+                           );
+
+    //
+    // If we found it, then we are done
+    //
+    if (Status == EFI_SUCCESS) {
+      *Instance = FvInstance;
+      break;
+    }
+  }
+
+  //
+  // Our exit status is determined by the success of the previous operations
+  // If the protocol was found, Instance already points to it.
+  //
+
+  //
+  // Free any allocated buffers
+  //
+  gBS->FreePool (HandleBuffer);
+
+  return Status;
+}
+
+
+/**
+  This function calculates and updates an UINT8 checksum.
+
+  @param  Buffer          Pointer to buffer to checksum
+  @param  Size            Number of bytes to checksum
+
+**/
+VOID
+AcpiPlatformChecksum (
+  IN UINT8      *Buffer,
+  IN UINTN      Size
+  )
+{
+  UINTN ChecksumOffset;
+
+  ChecksumOffset = OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER, Checksum);
+
+  //
+  // Set checksum to 0 first
+  //
+  Buffer[ChecksumOffset] = 0;
+
+  //
+  // Update checksum value
+  //
+  Buffer[ChecksumOffset] = CalculateCheckSum8(Buffer, Size);
+}
+
+
+/**
+  Entrypoint of Acpi Platform driver.
+
+  @param  ImageHandle
+  @param  SystemTable
+
+  @return EFI_SUCCESS
+  @return EFI_LOAD_ERROR
+  @return EFI_OUT_OF_RESOURCES
+
+**/
+EFI_STATUS
+EFIAPI
+AcpiPlatformEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_ACPI_TABLE_PROTOCOL        *AcpiTable;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL  *FwVol;
+  INTN                           Instance;
+  EFI_ACPI_COMMON_HEADER         *CurrentTable;
+  UINTN                          TableHandle;
+  UINT32                         FvStatus;
+  UINTN                          TableSize;
+  UINTN                          Size;
+
+  Instance     = 0;
+  CurrentTable = NULL;
+  TableHandle  = 0;
+
+  //
+  // Find the AcpiTable protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID**)&AcpiTable);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  //
+  // Locate the firmware volume protocol
+  //
+  Status = LocateFvInstanceWithTables (&FwVol);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Read tables from the storage file.
+  //
+  while (Status == EFI_SUCCESS) {
+
+    Status = FwVol->ReadSection (
+                      FwVol,
+                      (EFI_GUID*)PcdGetPtr (PcdAcpiTableStorageFile),
+                      EFI_SECTION_RAW,
+                      Instance,
+                      (VOID**) &CurrentTable,
+                      &Size,
+                      &FvStatus
+                      );
+    if (!EFI_ERROR(Status)) {
+      //
+      // Add the table
+      //
+      TableHandle = 0;
+
+      TableSize = ((EFI_ACPI_DESCRIPTION_HEADER *) CurrentTable)->Length;
+      ASSERT (Size >= TableSize);
+
+      //
+      // Checksum ACPI table
+      //
+      AcpiPlatformChecksum ((UINT8*)CurrentTable, TableSize);
+
+      //
+      // Install ACPI table
+      //
+      Status = AcpiTable->InstallAcpiTable (
+                            AcpiTable,
+                            CurrentTable,
+                            TableSize,
+                            &TableHandle
+                            );
+
+      //
+      // Free memory allocated by ReadSection
+      //
+      gBS->FreePool (CurrentTable);
+
+      if (EFI_ERROR(Status)) {
+        return EFI_ABORTED;
+      }
+
+      //
+      // Increment the instance
+      //
+      Instance++;
+      CurrentTable = NULL;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.uni b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.uni
new file mode 100644
index 0000000000..e67102d47b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatform.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf
new file mode 100644
index 0000000000..34b1600171
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf
@@ -0,0 +1,56 @@
+## @file
+#  Sample ACPI Platform Driver
+#
+#  Copyright (c) 2008 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AcpiPlatform
+  MODULE_UNI_FILE                = AcpiPlatform.uni
+  FILE_GUID                      = cb933912-df8f-4305-b1f9-7b44fa11395c
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = AcpiPlatformEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  AcpiPlatform.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiLib
+  DxeServicesLib
+  PcdLib
+  BaseMemoryLib
+  DebugLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+
+[Protocols]
+  gEfiAcpiTableProtocolGuid                     ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiTableStorageFile    ## CONSUMES
+
+[Depex]
+  gEfiAcpiTableProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AcpiPlatformExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformExtra.uni b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformExtra.uni
new file mode 100644
index 0000000000..613bba3117
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.c
new file mode 100644
index 0000000000..57fdc7844e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.c
@@ -0,0 +1,1118 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2015, 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.
+
+**/
+
+//
+// Includes
+//
+#include "AcpiTable.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+EFI_ACPI_SDT_PROTOCOL  mAcpiSdtProtocolTemplate = {
+  EFI_ACPI_TABLE_VERSION_NONE | EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0,
+  GetAcpiTable2,
+  RegisterNotify,
+  Open,
+  OpenSdt,
+  Close,
+  GetChild,
+  GetOption,
+  SetOption,
+  FindPath
+};
+
+/**
+  This function returns ACPI Table instance.
+
+  @return AcpiTableInstance
+**/
+EFI_ACPI_TABLE_INSTANCE *
+SdtGetAcpiTableInstance (
+  VOID
+  )
+{
+  return mPrivateData;
+}
+
+/**
+  This function finds the table specified by the buffer.
+
+  @param[in]  Buffer      Table buffer to find.
+
+  @return ACPI table list.
+**/
+EFI_ACPI_TABLE_LIST *
+FindTableByBuffer (
+  IN VOID  *Buffer
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  LIST_ENTRY                *CurrentLink;
+  EFI_ACPI_TABLE_LIST       *CurrentTableList;
+  LIST_ENTRY                *StartLink;
+
+  //
+  // Get the instance of the ACPI Table
+  //
+  AcpiTableInstance = SdtGetAcpiTableInstance ();
+
+  //
+  // Find the notify
+  //
+  StartLink   = &AcpiTableInstance->TableList;
+  CurrentLink = StartLink->ForwardLink;
+
+  while (CurrentLink != StartLink) {
+    CurrentTableList = EFI_ACPI_TABLE_LIST_FROM_LINK (CurrentLink);
+    if (((UINTN)CurrentTableList->PageAddress <= (UINTN)Buffer) &&
+        ((UINTN)CurrentTableList->PageAddress + EFI_PAGES_TO_SIZE(CurrentTableList->NumberOfPages) > (UINTN)Buffer)) {
+      //
+      // Good! Found Table.
+      //
+      return CurrentTableList;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return NULL;
+}
+
+/**
+  This function updates AML table checksum.
+  It will search the ACPI table installed by ACPI_TABLE protocol.
+
+  @param[in]  Buffer        A piece of AML code buffer pointer.
+
+  @retval EFI_SUCCESS       The table holds the AML buffer is found, and checksum is updated.
+  @retval EFI_NOT_FOUND     The table holds the AML buffer is not found.
+**/
+EFI_STATUS
+SdtUpdateAmlChecksum (
+  IN VOID  *Buffer
+  )
+{
+  EFI_ACPI_TABLE_LIST       *CurrentTableList;
+
+  CurrentTableList = FindTableByBuffer (Buffer);
+  if (CurrentTableList == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  AcpiPlatformChecksum (
+    (VOID *)CurrentTableList->Table,
+    CurrentTableList->Table->Length,
+    OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER, Checksum)
+    );
+  return EFI_SUCCESS;
+}
+
+/**
+  This function finds MAX AML buffer size.
+  It will search the ACPI table installed by ACPI_TABLE protocol.
+
+  @param[in]  Buffer        A piece of AML code buffer pointer.
+  @param[out] MaxSize       On return it holds the MAX size of buffer.
+
+  @retval EFI_SUCCESS       The table holds the AML buffer is found, and MAX size if returned.
+  @retval EFI_NOT_FOUND     The table holds the AML buffer is not found.
+**/
+EFI_STATUS
+SdtGetMaxAmlBufferSize (
+  IN  VOID  *Buffer,
+  OUT UINTN *MaxSize
+  )
+{
+  EFI_ACPI_TABLE_LIST       *CurrentTableList;
+
+  CurrentTableList = FindTableByBuffer (Buffer);
+  if (CurrentTableList == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *MaxSize = (UINTN)CurrentTableList->Table + CurrentTableList->Table->Length - (UINTN)Buffer;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function invokes ACPI notification.
+
+  @param[in]  AcpiTableInstance          Instance to AcpiTable
+  @param[in]  Version                    Version(s) to set.
+  @param[in]  Handle                     Handle of the table.
+**/
+VOID
+SdtNotifyAcpiList (
+  IN EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION    Version,
+  IN UINTN                     Handle
+  )
+{
+  EFI_ACPI_NOTIFY_LIST      *CurrentNotifyList;
+  LIST_ENTRY                *CurrentLink;
+  LIST_ENTRY                *StartLink;
+  EFI_ACPI_TABLE_LIST       *Table;
+  EFI_STATUS                Status;
+
+  //
+  // We should not use Table buffer, because it is user input buffer.
+  //
+  Status = FindTableByHandle (
+             Handle,
+             &AcpiTableInstance->TableList,
+             &Table
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Find the notify
+  //
+  StartLink   = &AcpiTableInstance->NotifyList;
+  CurrentLink = StartLink->ForwardLink;
+
+  while (CurrentLink != StartLink) {
+    CurrentNotifyList = EFI_ACPI_NOTIFY_LIST_FROM_LINK (CurrentLink);
+
+    //
+    // Inovke notification
+    //
+    CurrentNotifyList->Notification ((EFI_ACPI_SDT_HEADER *)Table->Table, Version, Handle);
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return ;
+}
+
+/**
+  Returns a requested ACPI table.
+  
+  The GetAcpiTable() function returns a pointer to a buffer containing the ACPI table associated
+  with the Index that was input. The following structures are not considered elements in the list of
+  ACPI tables:
+  - Root System Description Pointer (RSD_PTR)
+  - Root System Description Table (RSDT)
+  - Extended System Description Table (XSDT)
+  Version is updated with a bit map containing all the versions of ACPI of which the table is a
+  member. For tables installed via the EFI_ACPI_TABLE_PROTOCOL.InstallAcpiTable() interface,
+  the function returns the value of EFI_ACPI_STD_PROTOCOL.AcpiVersion.
+  
+  @param[in]    Index       The zero-based index of the table to retrieve.
+  @param[out]   Table       Pointer for returning the table buffer.
+  @param[out]   Version     On return, updated with the ACPI versions to which this table belongs. Type
+                            EFI_ACPI_TABLE_VERSION is defined in "Related Definitions" in the
+                            EFI_ACPI_SDT_PROTOCOL.    
+  @param[out]   TableKey    On return, points to the table key for the specified ACPI system definition table.
+                            This is identical to the table key used in the EFI_ACPI_TABLE_PROTOCOL.
+                            The TableKey can be passed to EFI_ACPI_TABLE_PROTOCOL.UninstallAcpiTable()
+                            to uninstall the table.
+  @retval EFI_SUCCESS       The function completed successfully.
+  @retval EFI_NOT_FOUND     The requested index is too large and a table was not found.                                  
+**/  
+EFI_STATUS
+EFIAPI
+GetAcpiTable2 (
+  IN  UINTN                               Index,
+  OUT EFI_ACPI_SDT_HEADER                 **Table,
+  OUT EFI_ACPI_TABLE_VERSION              *Version,
+  OUT UINTN                               *TableKey
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  UINTN                     TableIndex;
+  LIST_ENTRY                *CurrentLink;
+  LIST_ENTRY                *StartLink;
+  EFI_ACPI_TABLE_LIST       *CurrentTable;
+
+  ASSERT (Table != NULL);
+  ASSERT (Version != NULL);
+  ASSERT (TableKey != NULL);
+
+  //
+  // Get the instance of the ACPI Table
+  //
+  AcpiTableInstance = SdtGetAcpiTableInstance ();
+
+  //
+  // Find the table
+  //
+  StartLink   = &AcpiTableInstance->TableList;
+  CurrentLink = StartLink->ForwardLink;
+  TableIndex = 0;
+
+  while (CurrentLink != StartLink) {
+    if (TableIndex == Index) {
+      break;
+    }
+    //
+    // Next one
+    //
+    CurrentLink = CurrentLink->ForwardLink;
+    TableIndex ++;
+  }
+
+  if ((TableIndex != Index) || (CurrentLink == StartLink)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Get handle and version
+  //
+  CurrentTable  = EFI_ACPI_TABLE_LIST_FROM_LINK (CurrentLink);
+  *TableKey     = CurrentTable->Handle;
+  *Version      = CurrentTable->Version;
+  *Table        = (EFI_ACPI_SDT_HEADER *)CurrentTable->Table;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register a callback when an ACPI table is installed.
+  
+  This function registers a function which will be called whenever a new ACPI table is installed.
+  
+  @param[in]  Notification               Points to the callback function to be registered
+**/
+VOID
+SdtRegisterNotify (
+  IN EFI_ACPI_NOTIFICATION_FN   Notification
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  EFI_ACPI_NOTIFY_LIST      *CurrentNotifyList;
+
+  //
+  // Get the instance of the ACPI Table
+  //
+  AcpiTableInstance = SdtGetAcpiTableInstance ();
+
+  //
+  // Create a new list entry
+  //
+  CurrentNotifyList = AllocatePool (sizeof (EFI_ACPI_NOTIFY_LIST));
+  ASSERT (CurrentNotifyList != NULL);
+
+  //
+  // Initialize the table contents
+  //
+  CurrentNotifyList->Signature    = EFI_ACPI_NOTIFY_LIST_SIGNATURE;
+  CurrentNotifyList->Notification = Notification;
+
+  //
+  // Add the table to the current list of tables
+  //
+  InsertTailList (&AcpiTableInstance->NotifyList, &CurrentNotifyList->Link);
+
+  return ;
+}
+
+/**
+  Unregister a callback when an ACPI table is installed.
+  
+  This function unregisters a function which will be called whenever a new ACPI table is installed.
+  
+  @param[in]  Notification               Points to the callback function to be unregistered.
+  
+  @retval EFI_SUCCESS           Callback successfully unregistered.
+  @retval EFI_INVALID_PARAMETER Notification does not match a known registration function.                        
+**/
+EFI_STATUS
+SdtUnregisterNotify (
+  IN EFI_ACPI_NOTIFICATION_FN   Notification
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  EFI_ACPI_NOTIFY_LIST      *CurrentNotifyList;
+  LIST_ENTRY                *CurrentLink;
+  LIST_ENTRY                *StartLink;
+
+  //
+  // Get the instance of the ACPI Table
+  //
+  AcpiTableInstance = SdtGetAcpiTableInstance ();
+
+  //
+  // Find the notify
+  //
+  StartLink   = &AcpiTableInstance->NotifyList;
+  CurrentLink = StartLink->ForwardLink;
+
+  while (CurrentLink != StartLink) {
+    CurrentNotifyList = EFI_ACPI_NOTIFY_LIST_FROM_LINK (CurrentLink);
+    if (CurrentNotifyList->Notification == Notification) {
+      //
+      // Good! Found notification.
+      //
+      // Remove it from list and free the node.
+      //
+      RemoveEntryList (&(CurrentNotifyList->Link));
+      FreePool (CurrentNotifyList);
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  //
+  // Not found!
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Register or unregister a callback when an ACPI table is installed.
+  
+  This function registers or unregisters a function which will be called whenever a new ACPI table is
+  installed.
+  
+  @param[in]    Register        If TRUE, then the specified function will be registered. If FALSE, then the specified
+                                function will be unregistered.
+  @param[in]    Notification    Points to the callback function to be registered or unregistered.
+  
+  @retval EFI_SUCCESS           Callback successfully registered or unregistered.
+  @retval EFI_INVALID_PARAMETER Notification is NULL
+  @retval EFI_INVALID_PARAMETER Register is FALSE and Notification does not match a known registration function.                        
+**/
+EFI_STATUS
+EFIAPI
+RegisterNotify (
+  IN BOOLEAN                    Register,
+  IN EFI_ACPI_NOTIFICATION_FN   Notification
+  )
+{
+  //
+  // Check for invalid input parameters
+  //
+  if (Notification == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Register) {
+    //
+    // Register a new notify
+    //
+    SdtRegisterNotify (Notification);
+    return EFI_SUCCESS;
+  } else {
+    //
+    // Unregister an old notify
+    //
+    return SdtUnregisterNotify (Notification);
+  }
+}
+
+/**
+  Create a handle for the first ACPI opcode in an ACPI system description table.
+  
+  @param[in]    TableKey    The table key for the ACPI table, as returned by GetTable().
+  @param[out]   Handle      On return, points to the newly created ACPI handle.
+
+  @retval EFI_SUCCESS       Handle created successfully.
+  @retval EFI_NOT_FOUND     TableKey does not refer to a valid ACPI table.  
+**/
+EFI_STATUS
+SdtOpenSdtTable (
+  IN    UINTN           TableKey,
+  OUT   EFI_ACPI_HANDLE *Handle
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  EFI_STATUS                Status;
+  EFI_ACPI_TABLE_LIST       *Table;
+  EFI_AML_HANDLE            *AmlHandle;
+
+  //
+  // Get the instance of the ACPI Table
+  //
+  AcpiTableInstance = SdtGetAcpiTableInstance ();
+
+  //
+  // Find the table
+  //
+  Status = FindTableByHandle (
+             TableKey,
+             &AcpiTableInstance->TableList,
+             &Table
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  AmlHandle = AllocatePool (sizeof(*AmlHandle));
+  ASSERT (AmlHandle != NULL);
+  AmlHandle->Signature       = EFI_AML_ROOT_HANDLE_SIGNATURE;
+  AmlHandle->Buffer          = (VOID *)((UINTN)Table->Table + sizeof(EFI_ACPI_SDT_HEADER));
+  AmlHandle->Size            = Table->Table->Length - sizeof(EFI_ACPI_SDT_HEADER);
+  AmlHandle->AmlByteEncoding = NULL;
+  AmlHandle->Modified        = FALSE;
+
+  //
+  // return the ACPI handle
+  //
+  *Handle = (EFI_ACPI_HANDLE)AmlHandle;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create a handle for the first ACPI opcode in an ACPI system description table.
+  
+  @param[in]    TableKey    The table key for the ACPI table, as returned by GetTable().
+  @param[out]   Handle      On return, points to the newly created ACPI handle.
+
+  @retval EFI_SUCCESS       Handle created successfully.
+  @retval EFI_NOT_FOUND     TableKey does not refer to a valid ACPI table.  
+**/
+EFI_STATUS
+EFIAPI
+OpenSdt (
+  IN    UINTN           TableKey,
+  OUT   EFI_ACPI_HANDLE *Handle
+  )
+{
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return SdtOpenSdtTable (TableKey, Handle);
+}
+
+/**
+  Create a handle from an ACPI opcode
+  
+  @param[in]  Buffer                 Points to the ACPI opcode.
+  @param[in]  BufferSize             Max buffer size.
+  @param[out] Handle                 Upon return, holds the handle.
+  
+  @retval   EFI_SUCCESS             Success
+  @retval   EFI_INVALID_PARAMETER   Buffer is NULL or Handle is NULL or Buffer points to an
+                                    invalid opcode.
+  
+**/
+EFI_STATUS
+SdtOpenEx (
+  IN    VOID            *Buffer,
+  IN    UINTN           BufferSize,
+  OUT   EFI_ACPI_HANDLE *Handle 
+  )
+{
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  EFI_AML_HANDLE      *AmlHandle;
+
+  AmlByteEncoding = AmlSearchByOpByte (Buffer);
+  if (AmlByteEncoding == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Do not open NameString as handle
+  //
+  if ((AmlByteEncoding->Attribute & AML_IS_NAME_CHAR) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Good, find it
+  //
+  AmlHandle = AllocatePool (sizeof(*AmlHandle));
+  ASSERT (AmlHandle != NULL);
+  
+  AmlHandle->Signature       = EFI_AML_HANDLE_SIGNATURE;
+  AmlHandle->Buffer          = Buffer;
+  AmlHandle->AmlByteEncoding = AmlByteEncoding;
+  AmlHandle->Modified        = FALSE;
+
+  AmlHandle->Size = AmlGetObjectSize (AmlByteEncoding, Buffer, BufferSize);
+  if (AmlHandle->Size == 0) {
+    FreePool (AmlHandle);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Handle = (EFI_ACPI_HANDLE)AmlHandle;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create a handle from an ACPI opcode
+  
+  @param[in]  Buffer                 Points to the ACPI opcode.
+  @param[out] Handle                 Upon return, holds the handle.
+  
+  @retval   EFI_SUCCESS             Success
+  @retval   EFI_INVALID_PARAMETER   Buffer is NULL or Handle is NULL or Buffer points to an
+                                    invalid opcode.
+  
+**/
+EFI_STATUS
+EFIAPI
+Open (
+  IN    VOID            *Buffer,
+  OUT   EFI_ACPI_HANDLE *Handle 
+  )
+{
+  EFI_STATUS          Status;
+  UINTN               MaxSize;
+
+  MaxSize = 0;
+
+  //
+  // Check for invalid input parameters
+  //
+  if (Buffer == NULL || Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = SdtGetMaxAmlBufferSize (Buffer, &MaxSize);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return SdtOpenEx (Buffer, MaxSize, Handle);
+}
+
+/**
+  Close an ACPI handle.
+  
+  @param[in] Handle Returns the handle.
+  
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.  
+**/
+EFI_STATUS
+EFIAPI
+Close (
+  IN EFI_ACPI_HANDLE Handle
+  )
+{
+  EFI_AML_HANDLE      *AmlHandle;
+  EFI_STATUS          Status;
+
+  //
+  // Check for invalid input parameters
+  //
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlHandle = (EFI_AML_HANDLE *)Handle;
+  if ((AmlHandle->Signature != EFI_AML_ROOT_HANDLE_SIGNATURE) &&
+      (AmlHandle->Signature != EFI_AML_HANDLE_SIGNATURE)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Update Checksum only if modified
+  //
+  if (AmlHandle->Modified) {
+    Status = SdtUpdateAmlChecksum (AmlHandle->Buffer);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  FreePool (AmlHandle);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieve information about an ACPI object.
+  
+  @param[in]    Handle      ACPI object handle.
+  @param[in]    Index       Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                            in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[out]   DataType    Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                            for the specified index.
+  @param[out]   Data        Upon return, points to the pointer to the data.
+  @param[out]   DataSize    Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+EFIAPI
+GetOption (
+  IN        EFI_ACPI_HANDLE     Handle,
+  IN        UINTN               Index,
+  OUT       EFI_ACPI_DATA_TYPE  *DataType,
+  OUT CONST VOID                **Data,
+  OUT       UINTN               *DataSize
+  )
+{
+  EFI_AML_HANDLE      *AmlHandle;
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  EFI_STATUS          Status;
+
+  ASSERT (DataType != NULL);
+  ASSERT (Data != NULL);
+  ASSERT (DataSize != NULL);
+
+  //
+  // Check for invalid input parameters
+  //
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlHandle = (EFI_AML_HANDLE *)Handle;
+  //
+  // Do not check EFI_AML_ROOT_HANDLE_SIGNATURE because there is no option for Root handle
+  //
+  if (AmlHandle->Signature != EFI_AML_HANDLE_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlByteEncoding = AmlHandle->AmlByteEncoding;
+  if (Index > AmlByteEncoding->MaxIndex) {
+    *DataType = EFI_ACPI_DATA_TYPE_NONE;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Parse option
+  //
+  Status = AmlParseOptionHandleCommon (AmlHandle, (AML_OP_PARSE_INDEX)Index, DataType, (VOID **)Data, DataSize);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Change information about an ACPI object.
+  
+  @param[in]  Handle    ACPI object handle.
+  @param[in]  Index     Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                        in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[in]  Data      Points to the data.
+  @param[in]  DataSize  The size of the Data.
+
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.
+  @retval EFI_BAD_BUFFER_SIZE   Data cannot be accommodated in the space occupied by
+                                the option.
+
+**/
+EFI_STATUS
+EFIAPI
+SetOption (
+  IN        EFI_ACPI_HANDLE Handle,
+  IN        UINTN           Index,
+  IN CONST  VOID            *Data,
+  IN        UINTN           DataSize
+  )
+{
+  EFI_AML_HANDLE      *AmlHandle;
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  EFI_STATUS          Status;
+  EFI_ACPI_DATA_TYPE  DataType;
+  VOID                *OrgData;
+  UINTN               OrgDataSize;
+
+  ASSERT (Data != NULL);
+
+  //
+  // Check for invalid input parameters
+  //
+  if (Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlHandle = (EFI_AML_HANDLE *)Handle;
+  //
+  // Do not check EFI_AML_ROOT_HANDLE_SIGNATURE because there is no option for Root handle
+  //
+  if (AmlHandle->Signature != EFI_AML_HANDLE_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  AmlByteEncoding = AmlHandle->AmlByteEncoding;
+
+  if (Index > AmlByteEncoding->MaxIndex) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Parse option
+  //
+  Status = AmlParseOptionHandleCommon (AmlHandle, (AML_OP_PARSE_INDEX)Index, &DataType, &OrgData, &OrgDataSize);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (DataType == EFI_ACPI_DATA_TYPE_NONE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataSize > OrgDataSize) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // Update
+  //
+  CopyMem (OrgData, Data, DataSize);
+  AmlHandle->Modified = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Return the child ACPI objects.
+  
+  @param[in]        ParentHandle    Parent handle.
+  @param[in, out]   Handle          On entry, points to the previously returned handle or NULL to start with the first
+                                    handle. On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+EFIAPI
+GetChild (
+  IN EFI_ACPI_HANDLE        ParentHandle,
+  IN OUT EFI_ACPI_HANDLE    *Handle
+  )
+{
+  EFI_AML_HANDLE      *AmlParentHandle;
+  EFI_AML_HANDLE      *AmlHandle;
+  VOID                *Buffer;
+  EFI_STATUS          Status;
+
+  ASSERT (Handle != NULL);
+
+  //
+  // Check for invalid input parameters
+  //
+  if (ParentHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlHandle       = *Handle;
+  if ((AmlHandle != NULL) && (AmlHandle->Signature != EFI_AML_HANDLE_SIGNATURE)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlParentHandle = (EFI_AML_HANDLE *)ParentHandle;
+  if (AmlParentHandle->Signature == EFI_AML_ROOT_HANDLE_SIGNATURE) {
+    //
+    // Root handle
+    //
+    Status = AmlGetChildFromRoot (AmlParentHandle, AmlHandle, &Buffer);
+  } else if (AmlParentHandle->Signature == EFI_AML_HANDLE_SIGNATURE) {
+    //
+    // Non-root handle
+    //
+    Status = AmlGetChildFromNonRoot (AmlParentHandle, AmlHandle, &Buffer);
+  } else {
+    //
+    // Invalid
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Buffer == NULL) {
+    *Handle = NULL;
+    return EFI_SUCCESS;
+  }
+  return SdtOpenEx (Buffer, (UINTN)AmlParentHandle->Buffer + AmlParentHandle->Size - (UINTN)Buffer, Handle);
+}
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI path
+  
+  @param[in]    HandleIn    Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AmlPath     Points to the AML path.
+  @param[out]   HandleOut   On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn is NULL or does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+SdtFindPathFromNonRoot (
+  IN    EFI_ACPI_HANDLE HandleIn,
+  IN    UINT8           *AmlPath,
+  OUT   EFI_ACPI_HANDLE *HandleOut
+  )
+{
+  EFI_AML_HANDLE      *AmlHandle;
+  VOID                *Buffer;
+  EFI_STATUS          Status;
+
+  Buffer = NULL;
+  AmlHandle = (EFI_AML_HANDLE *)HandleIn;
+
+  //
+  // For non-root handle, we need search from THIS node instead of ROOT.
+  //
+  Status = AmlFindPath (AmlHandle, AmlPath, &Buffer, FALSE);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Buffer == NULL) {
+    *HandleOut = NULL;
+    return EFI_SUCCESS;
+  }
+  return SdtOpenEx (Buffer, (UINTN)AmlHandle->Buffer + AmlHandle->Size - (UINTN)Buffer, HandleOut);
+}
+
+/**
+  Duplicate AML handle.
+  
+  @param[in]    AmlHandle   Handle to be duplicated.
+                            
+  @return Duplicated AML handle.
+**/
+EFI_AML_HANDLE *
+SdtDuplicateHandle (
+  IN EFI_AML_HANDLE      *AmlHandle
+  )
+{
+  EFI_AML_HANDLE  *DstAmlHandle;
+
+  DstAmlHandle = AllocatePool (sizeof(*DstAmlHandle));
+  ASSERT (DstAmlHandle != NULL);
+  CopyMem (DstAmlHandle, (VOID *)AmlHandle, sizeof(*DstAmlHandle));
+
+  return DstAmlHandle;
+}
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI path
+  
+  @param[in]    HandleIn    Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AmlPath     Points to the AML path.
+  @param[out]   HandleOut   On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn is NULL or does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+SdtFindPathFromRoot (
+  IN    EFI_ACPI_HANDLE HandleIn,
+  IN    UINT8           *AmlPath,
+  OUT   EFI_ACPI_HANDLE *HandleOut
+  )
+{
+  EFI_ACPI_HANDLE     ChildHandle;
+  EFI_AML_HANDLE      *AmlHandle;
+  EFI_STATUS          Status;
+  VOID                *Buffer;
+
+  Buffer = NULL;
+  AmlHandle = (EFI_AML_HANDLE *)HandleIn;
+
+  //
+  // Handle case that AcpiPath is Root
+  //
+  if (AmlIsRootPath (AmlPath)) {
+    //
+    // Duplicate RootHandle
+    //
+    *HandleOut = (EFI_ACPI_HANDLE)SdtDuplicateHandle (AmlHandle);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Let children find it.
+  //
+  ChildHandle = NULL;
+  while (TRUE) {
+    Status = GetChild (HandleIn, &ChildHandle);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (ChildHandle == NULL) {
+      //
+      // Not found
+      //
+      *HandleOut = NULL;
+      return EFI_SUCCESS;
+    }
+
+    //
+    // More child
+    //
+    AmlHandle = (EFI_AML_HANDLE *)ChildHandle;
+    Status = AmlFindPath (AmlHandle, AmlPath, &Buffer, TRUE);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (Buffer != NULL) {
+      //
+      // Great! Find it, open
+      //
+      Status = SdtOpenEx (Buffer, (UINTN)AmlHandle->Buffer + AmlHandle->Size - (UINTN)Buffer, HandleOut);
+      if (!EFI_ERROR (Status))  {
+        return EFI_SUCCESS;
+      }
+      //
+      // Not success, try next one
+      //
+    }
+  }
+
+  //
+  // Should not run here
+  //
+}
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI path
+  
+  @param[in]    HandleIn    Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AcpiPath    Points to the ACPI path, which conforms to the ACPI encoded path format.
+  @param[out]   HandleOut   On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn is NULL or does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+EFIAPI
+FindPath (
+  IN    EFI_ACPI_HANDLE HandleIn,
+  IN    VOID            *AcpiPath,
+  OUT   EFI_ACPI_HANDLE *HandleOut
+  )
+{
+  EFI_AML_HANDLE      *AmlHandle;
+  EFI_STATUS          Status;
+  UINT8               *AmlPath;
+
+  //
+  // Check for invalid input parameters
+  //
+  if (HandleIn == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AmlHandle = (EFI_AML_HANDLE *)HandleIn;
+  
+  //
+  // Convert ASL path to AML path
+  //
+  AmlPath = AmlNameFromAslName (AcpiPath);
+  if (AmlPath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG_CODE_BEGIN ();
+  DEBUG ((EFI_D_ERROR, "AcpiSdt: FindPath - "));
+  AmlPrintNameString (AmlPath);
+  DEBUG ((EFI_D_ERROR, "\n"));
+  DEBUG_CODE_END ();
+
+  if (AmlHandle->Signature == EFI_AML_ROOT_HANDLE_SIGNATURE) {
+    //
+    // Root Handle
+    //
+    Status = SdtFindPathFromRoot (HandleIn, AmlPath, HandleOut);
+  } else if (AmlHandle->Signature == EFI_AML_HANDLE_SIGNATURE) {
+    //
+    // Non-Root handle
+    //
+    Status = SdtFindPathFromNonRoot (HandleIn, AmlPath, HandleOut);
+  } else {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+  FreePool (AmlPath);
+
+  return Status;
+}
+
+/**
+  ExitPmAuth Protocol notification event handler.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+**/
+VOID
+EFIAPI
+ExitPmAuthNotification (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS Status;
+  VOID       *DxeSmmReadyToLock;
+
+  //
+  // Add more check to locate protocol after got event, because
+  // the library will signal this event immediately once it is register
+  // just in case it is already installed.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiDxeSmmReadyToLockProtocolGuid,
+                  NULL,
+                  &DxeSmmReadyToLock
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Uninstall ACPI SDT protocol, so that we can make sure no one update ACPI table from API level.
+  //
+  Status = gBS->UninstallProtocolInterface (
+                  mHandle,
+                  &gEfiAcpiSdtProtocolGuid,
+                  &mPrivateData->AcpiSdtProtocol
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Close event, so it will not be invoked again.
+  //
+  gBS->CloseEvent (Event);
+
+  return ;
+}
+
+/**
+  This function initializes AcpiSdt protocol in ACPI table instance.
+
+  @param[in]  AcpiTableInstance       Instance to construct
+**/
+VOID
+SdtAcpiTableAcpiSdtConstructor (
+  IN EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance
+  )
+{
+  VOID *Registration;
+
+  InitializeListHead (&AcpiTableInstance->NotifyList);
+  CopyMem (&AcpiTableInstance->AcpiSdtProtocol, &mAcpiSdtProtocolTemplate, sizeof(mAcpiSdtProtocolTemplate));
+
+  //
+  // Register event for ExitPmAuth, so that we can uninstall ACPI SDT protocol after ExitPmAuth.
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEfiDxeSmmReadyToLockProtocolGuid,
+    TPL_CALLBACK,
+    ExitPmAuthNotification,
+    NULL,
+    &Registration
+    );
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.h b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.h
new file mode 100644
index 0000000000..f11bdee9ac
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiSdt.h
@@ -0,0 +1,586 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2015, 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.
+
+**/
+
+#ifndef _ACPI_SDT_H_
+#define _ACPI_SDT_H_
+
+//
+// Privacy data structure
+//
+
+//
+// ACPI Notify Linked List Signature.
+//
+#define EFI_ACPI_NOTIFY_LIST_SIGNATURE SIGNATURE_32 ('E', 'A', 'N', 'L')
+
+//
+// ACPI Notify List Entry definition.
+//
+//  Signature must be set to EFI_ACPI_NOTIFY_LIST_SIGNATURE
+//  Link is the linked list data.
+//  Notification is the callback function.
+//
+typedef struct {
+  UINT32                   Signature;
+  LIST_ENTRY               Link;
+  EFI_ACPI_NOTIFICATION_FN Notification;
+} EFI_ACPI_NOTIFY_LIST;
+
+//
+// Containment record for ACPI Notify linked list.
+//
+#define EFI_ACPI_NOTIFY_LIST_FROM_LINK(_link)  CR (_link, EFI_ACPI_NOTIFY_LIST, Link, EFI_ACPI_NOTIFY_LIST_SIGNATURE)
+
+typedef struct _AML_BYTE_ENCODING AML_BYTE_ENCODING;
+typedef struct _EFI_AML_NODE_LIST EFI_AML_NODE_LIST;
+
+//
+// AML Node Linked List Signature.
+//
+#define EFI_AML_NODE_LIST_SIGNATURE SIGNATURE_32 ('E', 'A', 'M', 'L')
+
+//
+// AML Node Linked List Entry definition.
+//
+//  Signature must be set to EFI_AML_NODE_LIST_SIGNATURE
+//  Link is the linked list data.
+//  Name is the ACPI node name.
+//         This is listed for PATH finding.
+//  Buffer is the ACPI node buffer pointer, the first/second bytes are opcode.
+//         This buffer should not be freed.
+//  Size is the total size of this ACPI node buffer.
+//  Children is the children linked list of this node.
+//
+#define AML_NAME_SEG_SIZE  4
+
+struct _EFI_AML_NODE_LIST {
+  UINT32                  Signature;
+  UINT8                   Name[AML_NAME_SEG_SIZE];
+  UINT8                   *Buffer;
+  UINTN                   Size;
+  LIST_ENTRY              Link;
+  LIST_ENTRY              Children;
+  EFI_AML_NODE_LIST       *Parent;
+  AML_BYTE_ENCODING       *AmlByteEncoding;
+};
+
+//
+// Containment record for AML Node linked list.
+//
+#define EFI_AML_NODE_LIST_FROM_LINK(_link)  CR (_link, EFI_AML_NODE_LIST, Link, EFI_AML_NODE_LIST_SIGNATURE)
+
+//
+// AML Handle Signature.
+//
+#define EFI_AML_HANDLE_SIGNATURE SIGNATURE_32 ('E', 'A', 'H', 'S')
+#define EFI_AML_ROOT_HANDLE_SIGNATURE SIGNATURE_32 ('E', 'A', 'R', 'H')
+
+//
+// AML Handle Entry definition.
+//
+//  Signature must be set to EFI_AML_HANDLE_SIGNATURE or EFI_AML_ROOT_HANDLE_SIGNATURE
+//  Buffer is the ACPI node buffer pointer, the first/second bytes are opcode.
+//         This buffer should not be freed.
+//  Size is the total size of this ACPI node buffer.
+//
+typedef struct {
+  UINT32                  Signature;
+  UINT8                   *Buffer;
+  UINTN                   Size;
+  AML_BYTE_ENCODING       *AmlByteEncoding;
+  BOOLEAN                 Modified;
+} EFI_AML_HANDLE;
+
+typedef UINT32 AML_OP_PARSE_INDEX;
+
+#define AML_OP_PARSE_INDEX_GET_OPCODE     0
+#define AML_OP_PARSE_INDEX_GET_TERM1      1
+#define AML_OP_PARSE_INDEX_GET_TERM2      2
+#define AML_OP_PARSE_INDEX_GET_TERM3      3
+#define AML_OP_PARSE_INDEX_GET_TERM4      4
+#define AML_OP_PARSE_INDEX_GET_TERM5      5
+#define AML_OP_PARSE_INDEX_GET_TERM6      6
+#define AML_OP_PARSE_INDEX_GET_SIZE       (AML_OP_PARSE_INDEX)-1
+
+typedef UINT32 AML_OP_PARSE_FORMAT;
+#define AML_NONE         0
+#define AML_OPCODE       1
+#define AML_UINT8        2
+#define AML_UINT16       3
+#define AML_UINT32       4
+#define AML_UINT64       5
+#define AML_NAME         6
+#define AML_STRING       7
+#define AML_OBJECT       8
+
+typedef UINT32 AML_OP_ATTRIBUTE;
+#define AML_HAS_PKG_LENGTH       0x1     // It is ACPI attribute - if OpCode has PkgLength
+#define AML_IS_NAME_CHAR         0x2     // It is ACPI attribute - if this is NameChar
+#define AML_HAS_CHILD_OBJ        0x4     // it is ACPI attribute - if OpCode has Child Object.
+#define AML_IN_NAMESPACE         0x10000 // It is UEFI SDT attribute - if OpCode will be in NameSpace
+                                         // NOTE; Not all OBJECT will be in NameSpace
+                                         // For example, BankField | CreateBitField | CreateByteField | CreateDWordField | 
+                                         //   CreateField | CreateQWordField | CreateWordField | Field | IndexField.
+
+struct _AML_BYTE_ENCODING {
+  UINT8                      OpCode;
+  UINT8                      SubOpCode;
+  AML_OP_PARSE_INDEX         MaxIndex;
+  AML_OP_PARSE_FORMAT        Format[6];
+  AML_OP_ATTRIBUTE           Attribute;
+};
+
+//
+// AcpiSdt protocol declaration
+//
+
+/**
+  Returns a requested ACPI table.
+  
+  The GetAcpiTable() function returns a pointer to a buffer containing the ACPI table associated
+  with the Index that was input. The following structures are not considered elements in the list of
+  ACPI tables:
+  - Root System Description Pointer (RSD_PTR)
+  - Root System Description Table (RSDT)
+  - Extended System Description Table (XSDT)
+  Version is updated with a bit map containing all the versions of ACPI of which the table is a
+  member. For tables installed via the EFI_ACPI_TABLE_PROTOCOL.InstallAcpiTable() interface,
+  the function returns the value of EFI_ACPI_STD_PROTOCOL.AcpiVersion.
+  
+  @param[in]    Index       The zero-based index of the table to retrieve.
+  @param[out]   Table       Pointer for returning the table buffer.
+  @param[out]   Version     On return, updated with the ACPI versions to which this table belongs. Type
+                            EFI_ACPI_TABLE_VERSION is defined in "Related Definitions" in the
+                            EFI_ACPI_SDT_PROTOCOL.    
+  @param[out]   TableKey    On return, points to the table key for the specified ACPI system definition table.
+                            This is identical to the table key used in the EFI_ACPI_TABLE_PROTOCOL.
+                            The TableKey can be passed to EFI_ACPI_TABLE_PROTOCOL.UninstallAcpiTable()
+                            to uninstall the table.
+                            
+  @retval EFI_SUCCESS       The function completed successfully.
+  @retval EFI_NOT_FOUND     The requested index is too large and a table was not found.                                  
+**/  
+EFI_STATUS
+EFIAPI
+GetAcpiTable2 (
+  IN  UINTN                               Index,
+  OUT EFI_ACPI_SDT_HEADER                 **Table,
+  OUT EFI_ACPI_TABLE_VERSION              *Version,
+  OUT UINTN                               *TableKey
+  );
+
+/**
+  Register or unregister a callback when an ACPI table is installed.
+  
+  This function registers or unregisters a function which will be called whenever a new ACPI table is
+  installed.
+  
+  @param[in]    Register        If TRUE, then the specified function will be registered. If FALSE, then the specified
+                                function will be unregistered.
+  @param[in]    Notification    Points to the callback function to be registered or unregistered.
+  
+  @retval EFI_SUCCESS           Callback successfully registered or unregistered.
+  @retval EFI_INVALID_PARAMETER Notification is NULL
+  @retval EFI_INVALID_PARAMETER Register is FALSE and Notification does not match a known registration function.                        
+**/
+EFI_STATUS
+EFIAPI
+RegisterNotify (
+  IN BOOLEAN                    Register,
+  IN EFI_ACPI_NOTIFICATION_FN   Notification
+  );
+
+/**
+  Create a handle for the first ACPI opcode in an ACPI system description table.
+  
+  @param[in]    TableKey    The table key for the ACPI table, as returned by GetTable().
+  @param[out]   Handle      On return, points to the newly created ACPI handle.
+
+  @retval EFI_SUCCESS       Handle created successfully.
+  @retval EFI_NOT_FOUND     TableKey does not refer to a valid ACPI table.  
+**/
+EFI_STATUS
+EFIAPI
+OpenSdt (
+  IN    UINTN           TableKey,
+  OUT   EFI_ACPI_HANDLE *Handle
+  );
+
+/**
+  Create a handle from an ACPI opcode
+  
+  @param[in]  Buffer                 Points to the ACPI opcode.
+  @param[out] Handle                 Upon return, holds the handle.
+  
+  @retval   EFI_SUCCESS             Success
+  @retval   EFI_INVALID_PARAMETER   Buffer is NULL or Handle is NULL or Buffer points to an
+                                    invalid opcode.
+  
+**/
+EFI_STATUS
+EFIAPI
+Open (
+  IN    VOID            *Buffer,
+  OUT   EFI_ACPI_HANDLE *Handle 
+  );
+
+/**
+  Close an ACPI handle.
+  
+  @param[in] Handle Returns the handle.
+  
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.  
+**/
+EFI_STATUS
+EFIAPI
+Close (
+  IN EFI_ACPI_HANDLE Handle
+  );
+
+/**
+  Retrieve information about an ACPI object.
+  
+  @param[in]    Handle      ACPI object handle.
+  @param[in]    Index       Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                            in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[out]   DataType    Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                            for the specified index.
+  @param[out]   Data        Upon return, points to the pointer to the data.
+  @param[out]   DataSize    Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+EFIAPI
+GetOption (
+  IN        EFI_ACPI_HANDLE     Handle,
+  IN        UINTN               Index,
+  OUT       EFI_ACPI_DATA_TYPE  *DataType,
+  OUT CONST VOID                **Data,
+  OUT       UINTN               *DataSize
+  );
+
+/**
+  Change information about an ACPI object.
+  
+  @param[in]  Handle    ACPI object handle.
+  @param[in]  Index     Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                        in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[in]  Data      Points to the data.
+  @param[in]  DataSize  The size of the Data.
+
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER Handle is NULL or does not refer to a valid ACPI object.
+  @retval EFI_BAD_BUFFER_SIZE   Data cannot be accommodated in the space occupied by
+                                the option.
+
+**/
+EFI_STATUS
+EFIAPI
+SetOption (
+  IN        EFI_ACPI_HANDLE Handle,
+  IN        UINTN           Index,
+  IN CONST  VOID            *Data,
+  IN        UINTN           DataSize
+  );
+
+/**
+  Return the child ACPI objects.
+  
+  @param[in]        ParentHandle    Parent handle.
+  @param[in, out]   Handle          On entry, points to the previously returned handle or NULL to start with the first
+                                    handle. On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+EFIAPI
+GetChild (
+  IN EFI_ACPI_HANDLE        ParentHandle,
+  IN OUT EFI_ACPI_HANDLE    *Handle
+  );
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI path
+  
+  @param[in]    HandleIn    Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AcpiPath    Points to the ACPI path, which conforms to the ACPI encoded path format.
+  @param[out]   HandleOut   On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn is NULL or does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+EFIAPI
+FindPath (
+  IN    EFI_ACPI_HANDLE HandleIn,
+  IN    VOID            *AcpiPath,
+  OUT   EFI_ACPI_HANDLE *HandleOut
+  );
+
+//
+// ACPI SDT function
+//
+
+/**
+  Create a handle from an ACPI opcode
+  
+  @param[in]  Buffer                 Points to the ACPI opcode.
+  @param[in]  BufferSize             Max buffer size.
+  @param[out] Handle                 Upon return, holds the handle.
+  
+  @retval   EFI_SUCCESS             Success
+  @retval   EFI_INVALID_PARAMETER   Buffer is NULL or Handle is NULL or Buffer points to an
+                                    invalid opcode.
+  
+**/
+EFI_STATUS
+SdtOpenEx (
+  IN    VOID            *Buffer,
+  IN    UINTN           BufferSize,
+  OUT   EFI_ACPI_HANDLE *Handle 
+  );
+
+//
+// AML support function
+//
+
+/**
+  Get AML NameString size.
+
+  @param[in]    Buffer     AML NameString.
+  @param[out]   BufferSize AML NameString size 
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Buffer does not refer to a valid AML NameString.
+**/
+EFI_STATUS
+AmlGetNameStringSize (
+  IN  UINT8              *Buffer,
+  OUT UINTN              *BufferSize
+  );
+
+/**
+  This function retuns package length from the buffer.
+
+  @param[in]  Buffer    AML buffer
+  @param[out] PkgLength The total length of package.
+
+  @return The byte data count to present the package length.
+**/
+UINTN
+AmlGetPkgLength (
+  IN UINT8              *Buffer,
+  OUT UINTN             *PkgLength
+  );
+
+/**
+  This function returns AcpiDataType according to AmlType.
+
+  @param[in]  AmlType        AML Type.
+
+  @return AcpiDataType
+**/
+EFI_ACPI_DATA_TYPE
+AmlTypeToAcpiType (
+  IN AML_OP_PARSE_FORMAT  AmlType
+  );
+
+/**
+  This function returns AmlByteEncoding according to OpCode Byte.
+
+  @param[in]  OpByteBuffer        OpCode byte buffer.
+
+  @return AmlByteEncoding
+**/
+AML_BYTE_ENCODING *
+AmlSearchByOpByte (
+  IN UINT8                *OpByteBuffer
+  );
+
+/**
+  Return object size.
+  
+  @param[in]    AmlByteEncoding      AML Byte Encoding.
+  @param[in]    Buffer               AML object buffer.
+  @param[in]    MaxBufferSize        AML object buffer MAX size. The parser can not parse any data exceed this region.
+  
+  @return       Size of the object.
+**/
+UINTN
+AmlGetObjectSize (
+  IN AML_BYTE_ENCODING   *AmlByteEncoding,
+  IN UINT8               *Buffer,
+  IN UINTN               MaxBufferSize
+  );
+
+/**
+  Return object name.
+  
+  @param[in]    AmlHandle            AML handle.
+  
+  @return       Name of the object.
+**/
+CHAR8 *
+AmlGetObjectName (
+  IN EFI_AML_HANDLE      *AmlHandle
+  );
+
+/**
+  Retrieve information according to AmlHandle
+  
+  @param[in]    AmlHandle            AML handle.
+  @param[in]    Index                Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                                     in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[out]   DataType             Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                                     for the specified index.
+  @param[out]   Data                 Upon return, points to the pointer to the data.
+  @param[out]   DataSize             Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AmlHandle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlParseOptionHandleCommon (
+  IN EFI_AML_HANDLE      *AmlHandle,
+  IN AML_OP_PARSE_INDEX  Index,
+  OUT EFI_ACPI_DATA_TYPE *DataType,
+  OUT VOID               **Data,
+  OUT UINTN              *DataSize
+  );
+
+/**
+  Return offset of last option.
+  
+  @param[in]    AmlHandle            AML Handle.
+  @param[out]   Buffer               Upon return, points to the offset after last option.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AmlHandle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlGetOffsetAfterLastOption (
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT UINT8                 **Buffer
+  );
+
+/**
+  Return the child ACPI objects from Root Handle.
+  
+  @param[in]        AmlParentHandle Parent handle. It is Root Handle.
+  @param[in]        AmlHandle       The previously returned handle or NULL to start with the first handle.
+  @param[out]       Buffer          On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromRoot (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  );
+
+/**
+  Return the child ACPI objects from Non-Root Handle.
+  
+  @param[in]        AmlParentHandle Parent handle. It is Non-Root Handle.
+  @param[in]        AmlHandle       The previously returned handle or NULL to start with the first handle.
+  @param[out]       Buffer          On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromNonRoot (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  );
+
+/**
+  Return AML name according to ASL name.
+  The caller need free the AmlName returned.
+
+  @param[in]    AslPath     ASL name.
+
+  @return AmlName
+**/
+UINT8 *
+AmlNameFromAslName (
+  IN UINT8 *AslPath
+  );
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI AML path
+  
+  @param[in]    AmlHandle   Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AmlPath     Points to the ACPI AML path.
+  @param[out]   Buffer      On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+  @param[in]    FromRoot    TRUE means to find AML path from \ (Root) Node.
+                            FALSE means to find AML path from this Node (The HandleIn).
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+AmlFindPath (
+  IN    EFI_AML_HANDLE  *AmlHandle,
+  IN    UINT8           *AmlPath,
+  OUT   VOID            **Buffer,
+  IN    BOOLEAN         FromRoot
+  );
+
+/**
+  Print AML NameString.
+
+  @param[in] Buffer AML NameString.
+**/
+VOID
+AmlPrintNameString (
+  IN UINT8              *Buffer
+  );
+
+/**
+  Print AML NameSeg.
+
+  @param[in] Buffer AML NameSeg.
+**/
+VOID
+AmlPrintNameSeg (
+  IN UINT8              *Buffer
+  );
+
+/**
+  Check if it is AML Root name
+
+  @param[in]    Buffer AML path.
+  
+  @retval       TRUE  AML path is root.
+  @retval       FALSE AML path is not root.
+**/
+BOOLEAN
+AmlIsRootPath (
+  IN UINT8              *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.c
new file mode 100644
index 0000000000..f39de844e0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.c
@@ -0,0 +1,90 @@
+/** @file
+  ACPI Table Protocol Driver
+
+  Copyright (c) 2006 - 2010, 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.
+
+**/
+
+//
+// Includes
+//
+#include "AcpiTable.h"
+
+//
+// Handle to install ACPI Table Protocol
+//
+EFI_HANDLE    mHandle = NULL;
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_ACPI_TABLE_INSTANCE   *mPrivateData = NULL;
+
+/**
+  Entry point of the ACPI table driver.
+  Creates and initializes an instance of the ACPI Table
+  Protocol and installs it on a new handle.
+
+  @param  ImageHandle   A handle for the image that is initializing this driver.
+  @param  SystemTable   A pointer to the EFI system table.
+
+  @return EFI_SUCCESS           Driver initialized successfully.
+  @return EFI_LOAD_ERROR        Failed to Initialize or has been loaded.
+  @return EFI_OUT_OF_RESOURCES  Could not allocate needed resources.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAcpiTableDxe (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+  EFI_ACPI_TABLE_INSTANCE   *PrivateData;
+
+  //
+  // Initialize our protocol
+  //
+  PrivateData = AllocateZeroPool (sizeof (EFI_ACPI_TABLE_INSTANCE));
+  ASSERT (PrivateData);
+  PrivateData->Signature = EFI_ACPI_TABLE_SIGNATURE;
+
+  //
+  // Call all constructors per produced protocols
+  //
+  Status = AcpiTableAcpiTableConstructor (PrivateData);
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (PrivateData);
+    return EFI_LOAD_ERROR;
+  }
+
+  //
+  // Install ACPI Table protocol
+  //
+  if (FeaturePcdGet (PcdInstallAcpiSdtProtocol)) {
+    mPrivateData = PrivateData;
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mHandle,
+                    &gEfiAcpiTableProtocolGuid,
+                    &PrivateData->AcpiTableProtocol,
+                    &gEfiAcpiSdtProtocolGuid,
+                    &mPrivateData->AcpiSdtProtocol,
+                    NULL
+                    );
+  } else {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mHandle,
+                    &gEfiAcpiTableProtocolGuid,
+                    &PrivateData->AcpiTableProtocol,
+                    NULL
+                    );
+  }
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.h b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.h
new file mode 100644
index 0000000000..ebedefb906
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTable.h
@@ -0,0 +1,241 @@
+/** @file
+  ACPI Table Protocol Driver
+
+  Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _ACPI_TABLE_H_
+#define _ACPI_TABLE_H_
+
+
+#include <PiDxe.h>
+
+#include <Protocol/AcpiTable.h>
+#include <Guid/Acpi.h>
+#include <Protocol/AcpiSystemDescriptionTable.h>
+#include <Protocol/DxeSmmReadyToLock.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/PcdLib.h>
+
+//
+// Statements that include other files
+//
+#include <IndustryStandard/Acpi.h>
+
+#include "AcpiSdt.h"
+
+//
+// Great than or equal to 2.0.
+//
+#define ACPI_TABLE_VERSION_GTE_2_0 (EFI_ACPI_TABLE_VERSION_2_0  | \
+                                    EFI_ACPI_TABLE_VERSION_3_0  | \
+                                    EFI_ACPI_TABLE_VERSION_4_0  | \
+                                    EFI_ACPI_TABLE_VERSION_5_0)
+
+//
+// Private Driver Data
+//
+//
+// ACPI Table Linked List Signature.
+//
+#define EFI_ACPI_TABLE_LIST_SIGNATURE SIGNATURE_32 ('E', 'A', 'T', 'L')
+
+//
+// ACPI Table Linked List Entry definition.
+//
+//  Signature must be set to EFI_ACPI_TABLE_LIST_SIGNATURE
+//  Link is the linked list data.
+//  Version is the versions of the ACPI tables that this table belongs in.
+//  Table is a pointer to the table.
+//  PageAddress is the address of the pages allocated for the table.
+//  NumberOfPages is the number of pages allocated at PageAddress.
+//  Handle is used to identify a particular table.
+//
+typedef struct {
+  UINT32                  Signature;
+  LIST_ENTRY              Link;
+  EFI_ACPI_TABLE_VERSION  Version;
+  EFI_ACPI_COMMON_HEADER  *Table;
+  EFI_PHYSICAL_ADDRESS    PageAddress;
+  UINTN                   NumberOfPages;
+  UINTN                   Handle;
+} EFI_ACPI_TABLE_LIST;
+
+//
+// Containment record for ACPI Table linked list.
+//
+#define EFI_ACPI_TABLE_LIST_FROM_LINK(_link)  CR (_link, EFI_ACPI_TABLE_LIST, Link, EFI_ACPI_TABLE_LIST_SIGNATURE)
+
+//
+// The maximum number of tables this driver supports
+//
+#define EFI_ACPI_MAX_NUM_TABLES 20
+
+//
+// Protocol private structure definition
+//
+//
+// ACPI support protocol instance signature definition.
+//
+#define EFI_ACPI_TABLE_SIGNATURE  SIGNATURE_32 ('S', 'T', 'A', 'E')
+
+//
+// ACPI support protocol instance data structure
+//
+typedef struct {
+  UINTN                                         Signature;
+  EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER  *Rsdp1;                 // Pointer to RSD_PTR structure
+  EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER  *Rsdp3;                 // Pointer to RSD_PTR structure
+  EFI_ACPI_DESCRIPTION_HEADER                   *Rsdt1;                 // Pointer to RSDT table header
+  EFI_ACPI_DESCRIPTION_HEADER                   *Rsdt3;                 // Pointer to RSDT table header
+  EFI_ACPI_DESCRIPTION_HEADER                   *Xsdt;                  // Pointer to XSDT table header
+  EFI_ACPI_1_0_FIXED_ACPI_DESCRIPTION_TABLE     *Fadt1;                 // Pointer to FADT table header
+  EFI_ACPI_3_0_FIXED_ACPI_DESCRIPTION_TABLE     *Fadt3;                 // Pointer to FADT table header
+  EFI_ACPI_1_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs1;                 // Pointer to FACS table header
+  EFI_ACPI_3_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs3;                 // Pointer to FACS table header
+  EFI_ACPI_DESCRIPTION_HEADER                   *Dsdt1;                 // Pointer to DSDT table header
+  EFI_ACPI_DESCRIPTION_HEADER                   *Dsdt3;                 // Pointer to DSDT table header
+  LIST_ENTRY                                    TableList;
+  UINTN                                         NumberOfTableEntries1;  // Number of ACPI 1.0 tables
+  UINTN                                         NumberOfTableEntries3;  // Number of ACPI 3.0 tables
+  UINTN                                         CurrentHandle;
+  EFI_ACPI_TABLE_PROTOCOL                       AcpiTableProtocol;
+  EFI_ACPI_SDT_PROTOCOL                         AcpiSdtProtocol;
+  LIST_ENTRY                                    NotifyList;
+} EFI_ACPI_TABLE_INSTANCE;
+
+//
+// ACPI table protocol instance containing record macro
+//
+#define EFI_ACPI_TABLE_INSTANCE_FROM_THIS(a) \
+  CR (a, \
+      EFI_ACPI_TABLE_INSTANCE, \
+      AcpiTableProtocol, \
+      EFI_ACPI_TABLE_SIGNATURE \
+      )
+
+//
+// Protocol Constructor functions
+//
+
+/**
+  Constructor for the ACPI support protocol.  Initializes instance
+  data.
+
+  @param  AcpiTableInstance       Instance to construct
+
+  @return EFI_SUCCESS             Instance initialized.
+  @return EFI_OUT_OF_RESOURCES    Unable to allocate required resources.
+
+**/
+EFI_STATUS
+AcpiTableAcpiTableConstructor (
+  EFI_ACPI_TABLE_INSTANCE                 *AcpiTableInstance
+  );
+
+
+/**
+  Entry point of the ACPI table driver.
+  Creates and initializes an instance of the ACPI Table 
+  Protocol and installs it on a new handle.
+
+  @param  ImageHandle   A handle for the image that is initializing this driver
+  @param  SystemTable   A pointer to the EFI system table
+
+  @return EFI_SUCCESS           Driver initialized successfully
+  @return EFI_LOAD_ERROR        Failed to Initialize or has been loaded 
+  @return EFI_OUT_OF_RESOURCES  Could not allocate needed resources
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeAcpiTableDxe (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  );
+
+/**
+
+  This function finds the table specified by the handle and returns a pointer to it.
+  If the handle is not found, EFI_NOT_FOUND is returned and the contents of Table are
+  undefined.
+
+  @param[in]  Handle      Table to find.
+  @param[in]  TableList   Table list to search
+  @param[out] Table       Pointer to table found. 
+
+  @retval EFI_SUCCESS              The function completed successfully.
+  @retval EFI_NOT_FOUND            No table found matching the handle specified.
+
+**/
+EFI_STATUS
+FindTableByHandle (
+  IN UINTN                                Handle,
+  IN LIST_ENTRY                           *TableList,
+  OUT EFI_ACPI_TABLE_LIST                 **Table
+  );
+
+/**
+
+  This function calculates and updates an UINT8 checksum.
+
+  @param[in]  Buffer          Pointer to buffer to checksum
+  @param[in]  Size            Number of bytes to checksum
+  @param[in]  ChecksumOffset  Offset to place the checksum result in
+
+  @retval EFI_SUCCESS             The function completed successfully.
+
+**/
+EFI_STATUS
+AcpiPlatformChecksum (
+  IN VOID       *Buffer,
+  IN UINTN      Size,
+  IN UINTN      ChecksumOffset
+  );
+
+/**
+  This function invokes ACPI notification.
+
+  @param[in]  AcpiTableInstance          Instance to AcpiTable
+  @param[in]  Version                    Version(s) to set.
+  @param[in]  Handle                     Handle of the table.
+**/
+VOID
+SdtNotifyAcpiList (
+  IN EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION    Version,
+  IN UINTN                     Handle
+  );
+
+/**
+  This function initializes AcpiSdt protocol in ACPI table instance.
+
+  @param[in]  AcpiTableInstance       Instance to construct
+**/
+VOID
+SdtAcpiTableAcpiSdtConstructor (
+  IN EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance
+  );
+
+//
+// export PrivateData symbol, because we need that in AcpiSdtProtol implementation
+//
+extern EFI_HANDLE                mHandle;
+extern EFI_ACPI_TABLE_INSTANCE   *mPrivateData;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf
new file mode 100644
index 0000000000..e9cd728dbf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf
@@ -0,0 +1,82 @@
+## @file
+#  ACPI Table Protocol Driver
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = AcpiTableDxe
+  MODULE_UNI_FILE                = AcpiTableDxe.uni
+  FILE_GUID                      = 9622E42C-8E38-4a08-9E8F-54F784652F6B
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeAcpiTableDxe
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  AcpiTableProtocol.c
+  AcpiTable.h
+  AcpiTable.c
+  AcpiSdt.h
+  AcpiSdt.c
+  Aml.c
+  AmlString.c
+  AmlOption.c
+  AmlChild.c
+  AmlNamespace.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  UefiLib
+  DebugLib
+  BaseLib
+  PcdLib
+
+[Guids]
+  gEfiAcpi10TableGuid                           ## PRODUCES ## SystemTable
+  gEfiAcpiTableGuid                             ## PRODUCES ## SystemTable
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdInstallAcpiSdtProtocol  ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemId            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemTableId       ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorId        ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorRevision  ## CONSUMES
+
+[Protocols]
+  gEfiAcpiTableProtocolGuid                     ## PRODUCES
+  gEfiAcpiSdtProtocolGuid                       ## PRODUCES
+  ## NOTIFY
+  ## SOMETIMES_CONSUMES
+  gEfiDxeSmmReadyToLockProtocolGuid
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  AcpiTableDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.uni b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.uni
new file mode 100644
index 0000000000..8e6af22a80
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxeExtra.uni b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxeExtra.uni
new file mode 100644
index 0000000000..27355aa983
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableProtocol.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableProtocol.c
new file mode 100644
index 0000000000..c6abf1bf0c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableProtocol.c
@@ -0,0 +1,1752 @@
+/** @file
+  ACPI Table Protocol Implementation
+
+  Copyright (c) 2006 - 2015, 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.
+
+**/
+
+//
+// Includes
+//
+#include "AcpiTable.h"
+//
+// The maximum number of tables that pre-allocated. 
+//
+UINTN         mEfiAcpiMaxNumTables = EFI_ACPI_MAX_NUM_TABLES; 
+
+/**
+  This function adds an ACPI table to the table list.  It will detect FACS and
+  allocate the correct type of memory and properly align the table.
+
+  @param  AcpiTableInstance         Instance of the protocol.
+  @param  Table                     Table to add.
+  @param  Checksum                  Does the table require checksumming.
+  @param  Version                   The version of the list to add the table to.
+  @param  Handle                    Pointer for returning the handle.
+
+  @return EFI_SUCCESS               The function completed successfully.
+  @return EFI_OUT_OF_RESOURCES      Could not allocate a required resource.
+  @return EFI_ABORTED               The table is a duplicate of a table that is required
+                                    to be unique.
+
+**/
+EFI_STATUS
+AddTableToList (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN VOID                                 *Table,
+  IN BOOLEAN                              Checksum,
+  IN EFI_ACPI_TABLE_VERSION               Version,
+  OUT UINTN                               *Handle
+  );
+
+/**
+  This function finds and removes the table specified by the handle.
+
+  @param  AcpiTableInstance  Instance of the protocol.
+  @param  Version            Bitmask of which versions to remove.
+  @param  Handle             Table to remove.
+
+  @return EFI_SUCCESS    The function completed successfully.
+  @return EFI_ABORTED    An error occurred.
+  @return EFI_NOT_FOUND  Handle not found in table list.
+
+**/
+EFI_STATUS
+RemoveTableFromList (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION               Version,
+  IN UINTN                                Handle
+  );
+
+/**
+  This function calculates and updates an UINT8 checksum.
+
+  @param  Buffer          Pointer to buffer to checksum
+  @param  Size            Number of bytes to checksum
+  @param  ChecksumOffset  Offset to place the checksum result in
+
+  @return EFI_SUCCESS             The function completed successfully.
+**/
+EFI_STATUS
+AcpiPlatformChecksum (
+  IN VOID       *Buffer,
+  IN UINTN      Size,
+  IN UINTN      ChecksumOffset
+  );
+
+/**
+  Checksum all versions of the common tables, RSDP, RSDT, XSDT.
+
+  @param  AcpiTableInstance  Protocol instance private data.
+
+  @return EFI_SUCCESS        The function completed successfully.
+
+**/
+EFI_STATUS
+ChecksumCommonTables (
+  IN OUT EFI_ACPI_TABLE_INSTANCE          *AcpiTableInstance
+  );
+
+//
+// Protocol function implementations.
+//
+
+/**
+  This function publishes the specified versions of the ACPI tables by
+  installing EFI configuration table entries for them.  Any combination of
+  table versions can be published.
+
+  @param  AcpiTableInstance  Instance of the protocol.
+  @param  Version            Version(s) to publish.
+
+  @return EFI_SUCCESS  The function completed successfully.
+  @return EFI_ABORTED  The function could not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+PublishTables (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION               Version
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    *CurrentRsdtEntry;
+  VOID                      *CurrentXsdtEntry;
+  UINT64                    Buffer64;
+
+  //
+  // Reorder tables as some operating systems don't seem to find the
+  // FADT correctly if it is not in the first few entries
+  //
+
+  //
+  // Add FADT as the first entry
+  //
+  if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+    CurrentRsdtEntry  = (UINT32 *) ((UINT8 *) AcpiTableInstance->Rsdt1 + sizeof (EFI_ACPI_DESCRIPTION_HEADER));
+    *CurrentRsdtEntry = (UINT32) (UINTN) AcpiTableInstance->Fadt1;
+  }
+  if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+    CurrentRsdtEntry  = (UINT32 *) ((UINT8 *) AcpiTableInstance->Rsdt3 + sizeof (EFI_ACPI_DESCRIPTION_HEADER));
+    *CurrentRsdtEntry = (UINT32) (UINTN) AcpiTableInstance->Fadt3;
+    CurrentXsdtEntry  = (VOID *) ((UINT8 *) AcpiTableInstance->Xsdt + sizeof (EFI_ACPI_DESCRIPTION_HEADER));
+    //
+    // Add entry to XSDT, XSDT expects 64 bit pointers, but
+    // the table pointers in XSDT are not aligned on 8 byte boundary.
+    //
+    Buffer64 = (UINT64) (UINTN) AcpiTableInstance->Fadt3;
+    CopyMem (
+      CurrentXsdtEntry,
+      &Buffer64,
+      sizeof (UINT64)
+      );
+  }
+
+  //
+  // Do checksum again because Dsdt/Xsdt is updated.
+  //
+  ChecksumCommonTables (AcpiTableInstance);
+
+  //
+  // Add the RSD_PTR to the system table and store that we have installed the
+  // tables.
+  //
+  if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+    Status = gBS->InstallConfigurationTable (&gEfiAcpi10TableGuid, AcpiTableInstance->Rsdp1);
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+  }
+
+  if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+    Status = gBS->InstallConfigurationTable (&gEfiAcpiTableGuid, AcpiTableInstance->Rsdp3);
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Installs an ACPI table into the RSDT/XSDT.
+  Note that the ACPI table should be checksumed before installing it.
+  Otherwise it will assert.
+
+  @param  This                 Protocol instance pointer.
+  @param  AcpiTableBuffer      A pointer to a buffer containing the ACPI table to be installed.
+  @param  AcpiTableBufferSize  Specifies the size, in bytes, of the AcpiTableBuffer buffer.
+  @param  TableKey             Reurns a key to refer to the ACPI table.
+
+  @return EFI_SUCCESS            The table was successfully inserted.
+  @return EFI_INVALID_PARAMETER  Either AcpiTableBuffer is NULL, TableKey is NULL, or AcpiTableBufferSize 
+                                 and the size field embedded in the ACPI table pointed to by AcpiTableBuffer
+                                 are not in sync.
+  @return EFI_OUT_OF_RESOURCES   Insufficient resources exist to complete the request.
+  @retval EFI_ACCESS_DENIED      The table signature matches a table already
+                                 present in the system and platform policy
+                                 does not allow duplicate tables of this type.
+
+**/
+EFI_STATUS
+EFIAPI
+InstallAcpiTable (
+  IN   EFI_ACPI_TABLE_PROTOCOL                    *This,
+  IN   VOID                                       *AcpiTableBuffer,
+  IN   UINTN                                      AcpiTableBufferSize,
+  OUT  UINTN                                      *TableKey
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  EFI_STATUS                Status;
+  VOID                      *AcpiTableBufferConst;
+
+  //
+  // Check for invalid input parameters
+  //
+  if ((AcpiTableBuffer == NULL) || (TableKey == NULL)
+     || (((EFI_ACPI_DESCRIPTION_HEADER *) AcpiTableBuffer)->Length != AcpiTableBufferSize)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get the instance of the ACPI table protocol
+  //
+  AcpiTableInstance = EFI_ACPI_TABLE_INSTANCE_FROM_THIS (This);
+
+  //
+  // Install the ACPI table
+  //
+  AcpiTableBufferConst = AllocateCopyPool (AcpiTableBufferSize,AcpiTableBuffer);
+  *TableKey = 0;
+  Status = AddTableToList (
+             AcpiTableInstance,
+             AcpiTableBufferConst,
+             TRUE,
+             EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0,
+             TableKey
+             );
+  if (!EFI_ERROR (Status)) {
+    Status = PublishTables (
+               AcpiTableInstance,
+               EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0
+               );
+  }
+  FreePool (AcpiTableBufferConst);
+  
+  //
+  // Add a new table successfully, notify registed callback
+  //
+  if (FeaturePcdGet (PcdInstallAcpiSdtProtocol)) {
+    if (!EFI_ERROR (Status)) {
+      SdtNotifyAcpiList (
+        AcpiTableInstance,
+        EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0,
+        *TableKey
+        );
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Removes an ACPI table from the RSDT/XSDT.
+
+  @param  This      Protocol instance pointer.
+  @param  TableKey  Specifies the table to uninstall.  The key was returned from InstallAcpiTable().
+
+  @return EFI_SUCCESS    The table was successfully uninstalled.
+  @return EFI_NOT_FOUND  TableKey does not refer to a valid key for a table entry.
+
+**/
+EFI_STATUS
+EFIAPI
+UninstallAcpiTable (
+  IN  EFI_ACPI_TABLE_PROTOCOL                    *This,
+  IN  UINTN                                      TableKey
+  )
+{
+  EFI_ACPI_TABLE_INSTANCE   *AcpiTableInstance;
+  EFI_STATUS                Status;
+
+  //
+  // Get the instance of the ACPI table protocol
+  //
+  AcpiTableInstance = EFI_ACPI_TABLE_INSTANCE_FROM_THIS (This);
+
+  //
+  // Uninstall the ACPI table
+  //
+  Status = RemoveTableFromList (
+             AcpiTableInstance,
+             EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0,
+             TableKey
+             );
+  if (!EFI_ERROR (Status)) {
+    Status = PublishTables (
+               AcpiTableInstance,
+               EFI_ACPI_TABLE_VERSION_1_0B | ACPI_TABLE_VERSION_GTE_2_0
+               );
+  }
+
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  If the number of APCI tables exceeds the preallocated max table number, enlarge the table buffer.
+
+  @param  AcpiTableInstance       ACPI table protocol instance data structure.
+
+  @return EFI_SUCCESS             reallocate the table beffer successfully.
+  @return EFI_OUT_OF_RESOURCES    Unable to allocate required resources.
+
+**/
+EFI_STATUS
+ReallocateAcpiTableBuffer (
+  IN EFI_ACPI_TABLE_INSTANCE                   *AcpiTableInstance
+  )
+{
+  UINTN                    NewMaxTableNumber;
+  UINTN                    TotalSize;
+  UINT8                    *Pointer;
+  EFI_PHYSICAL_ADDRESS     PageAddress;
+  EFI_ACPI_TABLE_INSTANCE  TempPrivateData;
+  EFI_STATUS               Status;
+  UINT64                   CurrentData;
+   
+  CopyMem (&TempPrivateData, AcpiTableInstance, sizeof (EFI_ACPI_TABLE_INSTANCE)); 
+  //
+  // Enlarge the max table number from mEfiAcpiMaxNumTables to mEfiAcpiMaxNumTables + EFI_ACPI_MAX_NUM_TABLES
+  //
+  NewMaxTableNumber = mEfiAcpiMaxNumTables + EFI_ACPI_MAX_NUM_TABLES;
+  //
+  // Create RSDT, XSDT structures and allocate buffers.
+  //
+  TotalSize = sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 1.0 RSDT
+              NewMaxTableNumber * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 RSDT
+              NewMaxTableNumber * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 XSDT
+              NewMaxTableNumber * sizeof (UINT64);
+
+  //
+  // Allocate memory in the lower 32 bit of address range for
+  // compatibility with ACPI 1.0 OS.
+  //
+  // This is done because ACPI 1.0 pointers are 32 bit values.
+  // ACPI 2.0 OS and all 64 bit OS must use the 64 bit ACPI table addresses.
+  // There is no architectural reason these should be below 4GB, it is purely
+  // for convenience of implementation that we force memory below 4GB.
+  //
+  PageAddress = 0xFFFFFFFF;
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiACPIReclaimMemory,
+                  EFI_SIZE_TO_PAGES (TotalSize),
+                  &PageAddress
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Pointer = (UINT8 *) (UINTN) PageAddress;
+  ZeroMem (Pointer, TotalSize);
+  
+  AcpiTableInstance->Rsdt1 = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+  Pointer += (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + NewMaxTableNumber * sizeof (UINT32));
+  AcpiTableInstance->Rsdt3 = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+  Pointer += (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + NewMaxTableNumber * sizeof (UINT32));
+  AcpiTableInstance->Xsdt = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+
+  //
+  // Update RSDP to point to the new Rsdt and Xsdt address.
+  //
+  AcpiTableInstance->Rsdp1->RsdtAddress = (UINT32) (UINTN) AcpiTableInstance->Rsdt1;
+  AcpiTableInstance->Rsdp3->RsdtAddress = (UINT32) (UINTN) AcpiTableInstance->Rsdt3;
+  CurrentData = (UINT64) (UINTN) AcpiTableInstance->Xsdt;
+  CopyMem (&AcpiTableInstance->Rsdp3->XsdtAddress, &CurrentData, sizeof (UINT64));
+
+  //
+  // copy the original Rsdt1, Rsdt3 and Xsdt structure to new buffer 
+  //
+  CopyMem (AcpiTableInstance->Rsdt1, TempPrivateData.Rsdt1, (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + mEfiAcpiMaxNumTables * sizeof (UINT32))); 
+  CopyMem (AcpiTableInstance->Rsdt3, TempPrivateData.Rsdt3, (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + mEfiAcpiMaxNumTables * sizeof (UINT32))); 
+  CopyMem (AcpiTableInstance->Xsdt, TempPrivateData.Xsdt, (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + mEfiAcpiMaxNumTables * sizeof (UINT64)));
+  
+  //
+  // Calculate orignal ACPI table buffer size
+  //
+  TotalSize = sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 1.0 RSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 RSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 XSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT64);
+  gBS->FreePages ((EFI_PHYSICAL_ADDRESS)(UINTN)TempPrivateData.Rsdt1, EFI_SIZE_TO_PAGES (TotalSize));
+  
+  //
+  // Update the Max ACPI table number
+  // 
+  mEfiAcpiMaxNumTables = NewMaxTableNumber;
+  return EFI_SUCCESS;
+}
+/**
+  This function adds an ACPI table to the table list.  It will detect FACS and
+  allocate the correct type of memory and properly align the table.
+
+  @param  AcpiTableInstance         Instance of the protocol.
+  @param  Table                     Table to add.
+  @param  Checksum                  Does the table require checksumming.
+  @param  Version                   The version of the list to add the table to.
+  @param  Handle                    Pointer for returning the handle.
+
+  @return EFI_SUCCESS               The function completed successfully.
+  @return EFI_OUT_OF_RESOURCES      Could not allocate a required resource.
+  @retval EFI_ACCESS_DENIED         The table signature matches a table already
+                                    present in the system and platform policy
+                                    does not allow duplicate tables of this type.
+
+**/
+EFI_STATUS
+AddTableToList (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN VOID                                 *Table,
+  IN BOOLEAN                              Checksum,
+  IN EFI_ACPI_TABLE_VERSION               Version,
+  OUT UINTN                               *Handle
+  )
+{
+  EFI_STATUS          Status;
+  EFI_ACPI_TABLE_LIST *CurrentTableList;
+  UINT32              CurrentTableSignature;
+  UINT32              CurrentTableSize;
+  UINT32              *CurrentRsdtEntry;
+  VOID                *CurrentXsdtEntry;
+  UINT64              Buffer64;
+  BOOLEAN             AddToRsdt;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (AcpiTableInstance);
+  ASSERT (Table);
+  ASSERT (Handle);
+
+  //
+  // Init locals
+  //
+  AddToRsdt = TRUE;
+
+  //
+  // Create a new list entry
+  //
+  CurrentTableList = AllocatePool (sizeof (EFI_ACPI_TABLE_LIST));
+  ASSERT (CurrentTableList);
+
+  //
+  // Determine table type and size
+  //
+  CurrentTableSignature = ((EFI_ACPI_COMMON_HEADER *) Table)->Signature;
+  CurrentTableSize      = ((EFI_ACPI_COMMON_HEADER *) Table)->Length;
+
+  //
+  // Allocate a buffer for the table.  All tables are allocated in the lower 32 bits of address space
+  // for backwards compatibility with ACPI 1.0 OS.
+  //
+  // This is done because ACPI 1.0 pointers are 32 bit values.
+  // ACPI 2.0 OS and all 64 bit OS must use the 64 bit ACPI table addresses.
+  // There is no architectural reason these should be below 4GB, it is purely
+  // for convenience of implementation that we force memory below 4GB.
+  //
+  CurrentTableList->PageAddress   = 0xFFFFFFFF;
+  CurrentTableList->NumberOfPages = EFI_SIZE_TO_PAGES (CurrentTableSize);
+
+  //
+  // Allocation memory type depends on the type of the table
+  //
+  if ((CurrentTableSignature == EFI_ACPI_2_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) || 
+      (CurrentTableSignature == EFI_ACPI_4_0_UEFI_ACPI_DATA_TABLE_SIGNATURE)) {
+    //
+    // Allocate memory for the FACS.  This structure must be aligned
+    // on a 64 byte boundary and must be ACPI NVS memory.
+    // Using AllocatePages should ensure that it is always aligned.
+    // Do not change signature for new ACPI version because they are same.
+    //
+    // UEFI table also need to be in ACPI NVS memory, because some data field
+    // could be updated by OS present agent. For example, BufferPtrAddress in
+    // SMM communication ACPI table.
+    //
+    ASSERT ((EFI_PAGE_SIZE % 64) == 0);
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiACPIMemoryNVS,
+                    CurrentTableList->NumberOfPages,
+                    &CurrentTableList->PageAddress
+                    );
+  } else {
+    //
+    // All other tables are ACPI reclaim memory, no alignment requirements.
+    //
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiACPIReclaimMemory,
+                    CurrentTableList->NumberOfPages,
+                    &CurrentTableList->PageAddress
+                    );
+  }
+  //
+  // Check return value from memory alloc.
+  //
+  if (EFI_ERROR (Status)) {
+    gBS->FreePool (CurrentTableList);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Update the table pointer with the allocated memory start
+  //
+  CurrentTableList->Table = (EFI_ACPI_COMMON_HEADER *) (UINTN) CurrentTableList->PageAddress;
+
+  //
+  // Initialize the table contents
+  //
+  CurrentTableList->Signature = EFI_ACPI_TABLE_LIST_SIGNATURE;
+  CopyMem (CurrentTableList->Table, Table, CurrentTableSize);
+  CurrentTableList->Handle  = AcpiTableInstance->CurrentHandle++;
+  *Handle                   = CurrentTableList->Handle;
+  CurrentTableList->Version = Version;
+
+  //
+  // Update internal pointers if this is a required table.  If it is a required
+  // table and a table of that type already exists, return an error.
+  //
+  // Calculate the checksum if the table is not FACS.
+  //
+  switch (CurrentTableSignature) {
+
+  case EFI_ACPI_1_0_FIXED_ACPI_DESCRIPTION_TABLE_SIGNATURE:
+    //
+    // We don't add the FADT in the standard way because some
+    // OS expect the FADT to be early in the table list.
+    // So we always add it as the first element in the list.
+    //
+    AddToRsdt = FALSE;
+
+    //
+    // Check that the table has not been previously added.
+    //
+    if (((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0 && AcpiTableInstance->Fadt1 != NULL) ||
+        ((Version & ACPI_TABLE_VERSION_GTE_2_0)  != 0 && AcpiTableInstance->Fadt3 != NULL)
+        ) {
+      gBS->FreePages (CurrentTableList->PageAddress, CurrentTableList->NumberOfPages);
+      gBS->FreePool (CurrentTableList);
+      return EFI_ACCESS_DENIED;
+    }
+    //
+    // Add the table to the appropriate table version
+    //
+    if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Fadt1 = (EFI_ACPI_1_0_FIXED_ACPI_DESCRIPTION_TABLE *) CurrentTableList->Table;
+
+      //
+      // Update pointers in FADT.  If tables don't exist this will put NULL pointers there.
+      //
+      AcpiTableInstance->Fadt1->FirmwareCtrl  = (UINT32) (UINTN) AcpiTableInstance->Facs1;
+      AcpiTableInstance->Fadt1->Dsdt          = (UINT32) (UINTN) AcpiTableInstance->Dsdt1;
+
+      //
+      // RSDP OEM information is updated to match the FADT OEM information
+      //
+      CopyMem (
+        &AcpiTableInstance->Rsdp1->OemId,
+        &AcpiTableInstance->Fadt1->Header.OemId,
+        6
+        );
+
+      //
+      // RSDT OEM information is updated to match the FADT OEM information.
+      //
+      CopyMem (
+        &AcpiTableInstance->Rsdt1->OemId,
+        &AcpiTableInstance->Fadt1->Header.OemId,
+        6
+        );
+
+      CopyMem (
+        &AcpiTableInstance->Rsdt1->OemTableId,
+        &AcpiTableInstance->Fadt1->Header.OemTableId,
+        sizeof (UINT64)
+        );
+      AcpiTableInstance->Rsdt1->OemRevision = AcpiTableInstance->Fadt1->Header.OemRevision;
+    }
+
+    if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Fadt3 = (EFI_ACPI_3_0_FIXED_ACPI_DESCRIPTION_TABLE *) CurrentTableList->Table;
+
+      //
+      // Update pointers in FADT.  If tables don't exist this will put NULL pointers there.
+      // Note: If the FIRMWARE_CTRL is non-zero, then X_FIRMWARE_CTRL must be zero, and 
+      // vice-versa.
+      //
+      if ((UINT64)(UINTN)AcpiTableInstance->Facs3 < BASE_4GB) {
+        AcpiTableInstance->Fadt3->FirmwareCtrl  = (UINT32) (UINTN) AcpiTableInstance->Facs3;
+        ZeroMem (&AcpiTableInstance->Fadt3->XFirmwareCtrl, sizeof (UINT64));
+      } else {
+        Buffer64 = (UINT64) (UINTN) AcpiTableInstance->Facs3;
+        CopyMem (
+          &AcpiTableInstance->Fadt3->XFirmwareCtrl,
+          &Buffer64,
+          sizeof (UINT64)
+          );
+        AcpiTableInstance->Fadt3->FirmwareCtrl = 0;
+      }
+      AcpiTableInstance->Fadt3->Dsdt  = (UINT32) (UINTN) AcpiTableInstance->Dsdt3;
+      Buffer64                          = (UINT64) (UINTN) AcpiTableInstance->Dsdt3;
+      CopyMem (
+        &AcpiTableInstance->Fadt3->XDsdt,
+        &Buffer64,
+        sizeof (UINT64)
+        );
+
+      //
+      // RSDP OEM information is updated to match the FADT OEM information
+      //
+      CopyMem (
+        &AcpiTableInstance->Rsdp3->OemId,
+        &AcpiTableInstance->Fadt3->Header.OemId,
+        6
+        );
+      
+      //
+      // RSDT OEM information is updated to match FADT OEM information.
+      //
+      CopyMem (
+        &AcpiTableInstance->Rsdt3->OemId,
+        &AcpiTableInstance->Fadt3->Header.OemId,
+        6
+        );
+      CopyMem (
+        &AcpiTableInstance->Rsdt3->OemTableId,
+        &AcpiTableInstance->Fadt3->Header.OemTableId,
+        sizeof (UINT64)
+        );
+      AcpiTableInstance->Rsdt3->OemRevision = AcpiTableInstance->Fadt3->Header.OemRevision;
+      
+      //
+      // XSDT OEM information is updated to match FADT OEM information.
+      //
+      CopyMem (
+        &AcpiTableInstance->Xsdt->OemId,
+        &AcpiTableInstance->Fadt3->Header.OemId,
+        6
+        );
+      CopyMem (
+        &AcpiTableInstance->Xsdt->OemTableId,
+        &AcpiTableInstance->Fadt3->Header.OemTableId,
+        sizeof (UINT64)
+        );
+      AcpiTableInstance->Xsdt->OemRevision = AcpiTableInstance->Fadt3->Header.OemRevision;
+    }    
+    //
+    // Checksum the table
+    //
+    if (Checksum) {
+      AcpiPlatformChecksum (
+        CurrentTableList->Table,
+        CurrentTableList->Table->Length,
+        OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+        Checksum)
+        );
+    }
+    break;
+
+  case EFI_ACPI_1_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE:
+    //
+    // Check that the table has not been previously added.
+    //
+    if (((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0 && AcpiTableInstance->Facs1 != NULL) ||
+        ((Version & ACPI_TABLE_VERSION_GTE_2_0)  != 0 && AcpiTableInstance->Facs3 != NULL)
+        ) {
+      gBS->FreePages (CurrentTableList->PageAddress, CurrentTableList->NumberOfPages);
+      gBS->FreePool (CurrentTableList);
+      return EFI_ACCESS_DENIED;
+    }
+    //
+    // FACS is referenced by FADT and is not part of RSDT
+    //
+    AddToRsdt = FALSE;
+
+    //
+    // Add the table to the appropriate table version
+    //
+    if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Facs1 = (EFI_ACPI_1_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) CurrentTableList->Table;
+
+      //
+      // If FADT already exists, update table pointers.
+      //
+      if (AcpiTableInstance->Fadt1 != NULL) {
+        AcpiTableInstance->Fadt1->FirmwareCtrl = (UINT32) (UINTN) AcpiTableInstance->Facs1;
+
+        //
+        // Checksum FADT table
+        //
+        AcpiPlatformChecksum (
+          AcpiTableInstance->Fadt1,
+          AcpiTableInstance->Fadt1->Header.Length,
+          OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+          Checksum)
+          );
+      }
+    }
+
+    if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Facs3 = (EFI_ACPI_3_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) CurrentTableList->Table;
+
+      //
+      // If FADT already exists, update table pointers.
+      //
+      if (AcpiTableInstance->Fadt3 != NULL) {
+        //
+        // Note: If the FIRMWARE_CTRL is non-zero, then X_FIRMWARE_CTRL must be zero, and 
+        // vice-versa.
+        //
+        if ((UINT64)(UINTN)AcpiTableInstance->Facs3 < BASE_4GB) {
+          AcpiTableInstance->Fadt3->FirmwareCtrl  = (UINT32) (UINTN) AcpiTableInstance->Facs3;
+          ZeroMem (&AcpiTableInstance->Fadt3->XFirmwareCtrl, sizeof (UINT64));
+        } else {
+          Buffer64 = (UINT64) (UINTN) AcpiTableInstance->Facs3;
+          CopyMem (
+            &AcpiTableInstance->Fadt3->XFirmwareCtrl,
+            &Buffer64,
+            sizeof (UINT64)
+            );
+          AcpiTableInstance->Fadt3->FirmwareCtrl = 0;
+        }
+
+        //
+        // Checksum FADT table
+        //
+        AcpiPlatformChecksum (
+          AcpiTableInstance->Fadt3,
+          AcpiTableInstance->Fadt3->Header.Length,
+          OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+          Checksum)
+          );
+      }
+    }
+
+    break;
+
+  case EFI_ACPI_1_0_DIFFERENTIATED_SYSTEM_DESCRIPTION_TABLE_SIGNATURE:
+    //
+    // Check that the table has not been previously added.
+    //
+    if (((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0 && AcpiTableInstance->Dsdt1 != NULL) ||
+        ((Version & ACPI_TABLE_VERSION_GTE_2_0)  != 0 && AcpiTableInstance->Dsdt3 != NULL)
+        ) {
+      gBS->FreePages (CurrentTableList->PageAddress, CurrentTableList->NumberOfPages);
+      gBS->FreePool (CurrentTableList);
+      return EFI_ACCESS_DENIED;
+    }
+    //
+    // DSDT is referenced by FADT and is not part of RSDT
+    //
+    AddToRsdt = FALSE;
+
+    //
+    // Add the table to the appropriate table version
+    //
+    if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Dsdt1 = (EFI_ACPI_DESCRIPTION_HEADER *) CurrentTableList->Table;
+
+      //
+      // If FADT already exists, update table pointers.
+      //
+      if (AcpiTableInstance->Fadt1 != NULL) {
+        AcpiTableInstance->Fadt1->Dsdt = (UINT32) (UINTN) AcpiTableInstance->Dsdt1;
+
+        //
+        // Checksum FADT table
+        //
+        AcpiPlatformChecksum (
+          AcpiTableInstance->Fadt1,
+          AcpiTableInstance->Fadt1->Header.Length,
+          OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+          Checksum)
+          );
+      }
+    }
+    
+    if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+      //
+      // Save a pointer to the table
+      //
+      AcpiTableInstance->Dsdt3 = (EFI_ACPI_DESCRIPTION_HEADER *) CurrentTableList->Table;
+
+      //
+      // If FADT already exists, update table pointers.
+      //
+      if (AcpiTableInstance->Fadt3 != NULL) {
+        AcpiTableInstance->Fadt3->Dsdt  = (UINT32) (UINTN) AcpiTableInstance->Dsdt3;
+        Buffer64                          = (UINT64) (UINTN) AcpiTableInstance->Dsdt3;
+        CopyMem (
+          &AcpiTableInstance->Fadt3->XDsdt,
+          &Buffer64,
+          sizeof (UINT64)
+          );
+
+        //
+        // Checksum FADT table
+        //
+        AcpiPlatformChecksum (
+          AcpiTableInstance->Fadt3,
+          AcpiTableInstance->Fadt3->Header.Length,
+          OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+          Checksum)
+          );
+      }
+    }        
+    //
+    // Checksum the table
+    //
+    if (Checksum) {
+      AcpiPlatformChecksum (
+        CurrentTableList->Table,
+        CurrentTableList->Table->Length,
+        OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+        Checksum)
+        );
+    }
+    break;
+
+  default:
+    //
+    // Checksum the table
+    //
+    if (Checksum) {
+      AcpiPlatformChecksum (
+        CurrentTableList->Table,
+        CurrentTableList->Table->Length,
+        OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+        Checksum)
+        );
+    }
+    break;
+  }
+  //
+  // Add the table to the current list of tables
+  //
+  InsertTailList (&AcpiTableInstance->TableList, &CurrentTableList->Link);
+
+  //
+  // Add the table to RSDT and/or XSDT table entry lists.
+  //
+  //
+  // Add to ACPI 1.0b table tree
+  //
+  if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+    if (AddToRsdt) {
+      //
+      // If the table number exceed the gEfiAcpiMaxNumTables, enlarge the table buffer
+      //
+      if (AcpiTableInstance->NumberOfTableEntries1 >= mEfiAcpiMaxNumTables) {
+        Status = ReallocateAcpiTableBuffer (AcpiTableInstance);
+        ASSERT_EFI_ERROR (Status);
+      }
+      CurrentRsdtEntry = (UINT32 *)
+        (
+          (UINT8 *) AcpiTableInstance->Rsdt1 +
+          sizeof (EFI_ACPI_DESCRIPTION_HEADER) +
+          AcpiTableInstance->NumberOfTableEntries1 *
+          sizeof (UINT32)
+        );
+
+      //
+      // Add entry to the RSDT unless its the FACS or DSDT
+      //
+      *CurrentRsdtEntry = (UINT32) (UINTN) CurrentTableList->Table;
+
+      //
+      // Update RSDT length
+      //
+      AcpiTableInstance->Rsdt1->Length = AcpiTableInstance->Rsdt1->Length + sizeof (UINT32);
+
+      AcpiTableInstance->NumberOfTableEntries1++;
+    }
+  }
+  //
+  // Add to ACPI 2.0/3.0  table tree
+  //
+  if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+     if (AddToRsdt) {
+       //
+       // If the table number exceed the gEfiAcpiMaxNumTables, enlarge the table buffer
+       //
+       if (AcpiTableInstance->NumberOfTableEntries3 >= mEfiAcpiMaxNumTables) {
+         Status = ReallocateAcpiTableBuffer (AcpiTableInstance);
+         ASSERT_EFI_ERROR (Status);
+       }
+       //
+       // At this time, it is assumed that RSDT and XSDT maintain parallel lists of tables.
+       // If it becomes necessary to maintain separate table lists, changes will be required.
+       //
+       CurrentRsdtEntry = (UINT32 *)
+         (
+           (UINT8 *) AcpiTableInstance->Rsdt3 +
+           sizeof (EFI_ACPI_DESCRIPTION_HEADER) +
+           AcpiTableInstance->NumberOfTableEntries3 *
+           sizeof (UINT32)
+         );
+
+       //
+       // This pointer must not be directly dereferenced as the XSDT entries may not
+       // be 64 bit aligned resulting in a possible fault.  Use CopyMem to update.
+       //
+       CurrentXsdtEntry = (VOID *)
+         (
+           (UINT8 *) AcpiTableInstance->Xsdt +
+           sizeof (EFI_ACPI_DESCRIPTION_HEADER) +
+           AcpiTableInstance->NumberOfTableEntries3 *
+           sizeof (UINT64)
+         );
+
+       //
+       // Add entry to the RSDT
+       //
+       *CurrentRsdtEntry = (UINT32) (UINTN) CurrentTableList->Table;
+
+       //
+       // Update RSDT length
+       //
+       AcpiTableInstance->Rsdt3->Length = AcpiTableInstance->Rsdt3->Length + sizeof (UINT32);
+
+       //
+       // Add entry to XSDT, XSDT expects 64 bit pointers, but
+       // the table pointers in XSDT are not aligned on 8 byte boundary.
+       //
+       Buffer64 = (UINT64) (UINTN) CurrentTableList->Table;
+       CopyMem (
+         CurrentXsdtEntry,
+         &Buffer64,
+         sizeof (UINT64)
+         );
+
+       //
+       // Update length
+       //
+       AcpiTableInstance->Xsdt->Length = AcpiTableInstance->Xsdt->Length + sizeof (UINT64);
+
+       AcpiTableInstance->NumberOfTableEntries3++;
+    }
+  }
+
+  ChecksumCommonTables (AcpiTableInstance);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function finds the table specified by the handle and returns a pointer to it.
+  If the handle is not found, EFI_NOT_FOUND is returned and the contents of Table are
+  undefined.
+
+  @param  Handle      Table to find.
+  @param  TableList   Table list to search
+  @param  Table       Pointer to table found. 
+
+  @return EFI_SUCCESS    The function completed successfully.
+  @return EFI_NOT_FOUND  No table found matching the handle specified.
+
+**/
+EFI_STATUS
+FindTableByHandle (
+  IN UINTN                                Handle,
+  IN LIST_ENTRY                       *TableList,
+  OUT EFI_ACPI_TABLE_LIST                 **Table
+  )
+{
+  LIST_ENTRY      *CurrentLink;
+  EFI_ACPI_TABLE_LIST *CurrentTable;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (Table);
+
+  //
+  // Find the table
+  //
+  CurrentLink = TableList->ForwardLink;
+
+  while (CurrentLink != TableList) {
+    CurrentTable = EFI_ACPI_TABLE_LIST_FROM_LINK (CurrentLink);
+    if (CurrentTable->Handle == Handle) {
+      //
+      // Found handle, so return this table.
+      //
+      *Table = CurrentTable;
+      return EFI_SUCCESS;
+    }
+
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+  //
+  // Table not found
+  //
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function removes a basic table from the RSDT and/or XSDT.
+  For Acpi 1.0 tables, pass in the Rsdt.
+  For Acpi 2.0 tables, pass in both Rsdt and Xsdt.
+
+  @param  Table                 Pointer to table found. 
+  @param  NumberOfTableEntries  Current number of table entries in the RSDT/XSDT
+  @param  Rsdt                  Pointer to the RSDT to remove from
+  @param  Xsdt                  Pointer to the Xsdt to remove from
+
+  @return EFI_SUCCESS            The function completed successfully.
+  @return EFI_INVALID_PARAMETER  The table was not found in both Rsdt and Xsdt.
+
+**/
+EFI_STATUS
+RemoveTableFromRsdt (
+  IN OUT EFI_ACPI_TABLE_LIST              * Table,
+  IN OUT UINTN                            *NumberOfTableEntries,
+  IN OUT EFI_ACPI_DESCRIPTION_HEADER      * Rsdt,
+  IN OUT EFI_ACPI_DESCRIPTION_HEADER      * Xsdt OPTIONAL
+  )
+{
+  UINT32  *CurrentRsdtEntry;
+  VOID    *CurrentXsdtEntry;
+  UINT64  CurrentTablePointer64;
+  UINTN   Index;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (Table);
+  ASSERT (NumberOfTableEntries);
+  ASSERT (Rsdt);
+
+  //
+  // Find the table entry in the RSDT and XSDT
+  //
+  for (Index = 0; Index < *NumberOfTableEntries; Index++) {
+    //
+    // At this time, it is assumed that RSDT and XSDT maintain parallel lists of tables.
+    // If it becomes necessary to maintain separate table lists, changes will be required.
+    //
+    CurrentRsdtEntry = (UINT32 *) ((UINT8 *) Rsdt + sizeof (EFI_ACPI_DESCRIPTION_HEADER) + Index * sizeof (UINT32));
+    if (Xsdt != NULL) {
+      //
+      // This pointer must not be directly dereferenced as the XSDT entries may not
+      // be 64 bit aligned resulting in a possible fault.  Use CopyMem to update.
+      //
+      CurrentXsdtEntry = (VOID *) ((UINT8 *) Xsdt + sizeof (EFI_ACPI_DESCRIPTION_HEADER) + Index * sizeof (UINT64));
+
+      //
+      // Read the entry value out of the XSDT
+      //
+      CopyMem (&CurrentTablePointer64, CurrentXsdtEntry, sizeof (UINT64));
+    } else {
+      //
+      // Initialize to NULL
+      //
+      CurrentXsdtEntry      = 0;
+      CurrentTablePointer64 = 0;
+    }
+    //
+    // Check if we have found the corresponding entry in both RSDT and XSDT
+    //
+    if (*CurrentRsdtEntry == (UINT32) (UINTN) Table->Table &&
+        ((Xsdt == NULL) || CurrentTablePointer64 == (UINT64) (UINTN) Table->Table)
+        ) {
+      //
+      // Found entry, so copy all following entries and shrink table
+      // We actually copy all + 1 to copy the initialized value of memory over
+      // the last entry.
+      //
+      CopyMem (CurrentRsdtEntry, CurrentRsdtEntry + 1, (*NumberOfTableEntries - Index) * sizeof (UINT32));
+      Rsdt->Length = Rsdt->Length - sizeof (UINT32);
+      if (Xsdt != NULL) {
+        CopyMem (CurrentXsdtEntry, ((UINT64 *) CurrentXsdtEntry) + 1, (*NumberOfTableEntries - Index) * sizeof (UINT64));
+        Xsdt->Length = Xsdt->Length - sizeof (UINT64);
+      }
+      break;
+    } else if (Index + 1 == *NumberOfTableEntries) {
+      //
+      // At the last entry, and table not found
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  //
+  // Checksum the tables
+  //
+  AcpiPlatformChecksum (
+    Rsdt,
+    Rsdt->Length,
+    OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+    Checksum)
+    );
+
+  if (Xsdt != NULL) {
+    AcpiPlatformChecksum (
+      Xsdt,
+      Xsdt->Length,
+      OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+      Checksum)
+      );
+  }
+  //
+  // Decrement the number of tables
+  //
+  (*NumberOfTableEntries)--;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function removes a table and frees any associated memory.
+
+  @param  AcpiTableInstance  Instance of the protocol.
+  @param  Version            Version(s) to delete.
+  @param  Table              Pointer to table found.
+
+  @return EFI_SUCCESS  The function completed successfully.
+
+**/
+EFI_STATUS
+DeleteTable (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION               Version,
+  IN OUT EFI_ACPI_TABLE_LIST              *Table
+  )
+{
+  UINT32  CurrentTableSignature;
+  BOOLEAN RemoveFromRsdt;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (AcpiTableInstance);
+  ASSERT (Table);
+
+  //
+  // Init locals
+  //
+  RemoveFromRsdt        = TRUE;
+  //
+  // Check for Table->Table
+  //
+  ASSERT (Table->Table != NULL);
+  CurrentTableSignature = ((EFI_ACPI_COMMON_HEADER *) Table->Table)->Signature;
+
+  //
+  // Basic tasks to accomplish delete are:
+  //   Determine removal requirements (in RSDT/XSDT or not)
+  //   Remove entry from RSDT/XSDT
+  //   Remove any table references to the table
+  //   If no one is using the table
+  //      Free the table (removing pointers from private data and tables)
+  //      Remove from list
+  //      Free list structure
+  //
+  //
+  // Determine if this table is in the RSDT or XSDT
+  //
+  if ((CurrentTableSignature == EFI_ACPI_1_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) ||
+      (CurrentTableSignature == EFI_ACPI_2_0_DIFFERENTIATED_SYSTEM_DESCRIPTION_TABLE_SIGNATURE) ||
+      (CurrentTableSignature == EFI_ACPI_3_0_DIFFERENTIATED_SYSTEM_DESCRIPTION_TABLE_SIGNATURE)
+      ) {
+    RemoveFromRsdt = FALSE;
+  }
+  //
+  // We don't remove the FADT in the standard way because some
+  // OS expect the FADT to be early in the table list.
+  // So we always put it as the first element in the list.
+  //
+  if (CurrentTableSignature == EFI_ACPI_1_0_FIXED_ACPI_DESCRIPTION_TABLE_SIGNATURE) {
+    RemoveFromRsdt = FALSE;
+  }
+
+  //
+  // Remove the table from RSDT and XSDT
+  //
+  if (Table->Table != NULL) {
+    //
+    // This is a basic table, remove it from any lists and the Rsdt and/or Xsdt
+    //
+    if (Version & EFI_ACPI_TABLE_VERSION_NONE & Table->Version) {
+      //
+      // Remove this version from the table
+      //
+      Table->Version = Table->Version &~EFI_ACPI_TABLE_VERSION_NONE;
+    }
+
+    if (Version & EFI_ACPI_TABLE_VERSION_1_0B & Table->Version) {
+      //
+      // Remove this version from the table
+      //
+      Table->Version = Table->Version &~EFI_ACPI_TABLE_VERSION_1_0B;
+
+      //
+      // Remove from Rsdt.  We don't care about the return value because it is
+      // acceptable for the table to not exist in Rsdt.
+      // We didn't add some tables so we don't remove them.
+      //
+      if (RemoveFromRsdt) {
+        RemoveTableFromRsdt (
+          Table,
+          &AcpiTableInstance->NumberOfTableEntries1,
+          AcpiTableInstance->Rsdt1,
+          NULL
+          );
+      }
+    }
+
+    if (Version & ACPI_TABLE_VERSION_GTE_2_0 & Table->Version) {
+      //
+      // Remove this version from the table
+      //
+      Table->Version = Table->Version &~(Version & ACPI_TABLE_VERSION_GTE_2_0);
+      
+      //
+      // Remove from Rsdt and Xsdt.  We don't care about the return value
+      // because it is acceptable for the table to not exist in Rsdt/Xsdt.
+      // We didn't add some tables so we don't remove them.
+      //
+      if (RemoveFromRsdt) {
+        RemoveTableFromRsdt (
+          Table,
+          &AcpiTableInstance->NumberOfTableEntries3,
+          AcpiTableInstance->Rsdt3,
+          AcpiTableInstance->Xsdt
+          );
+      }
+    }    
+    //
+    // Free the table, clean up any dependent tables and our private data pointers.
+    //
+    switch (Table->Table->Signature) {
+
+    case EFI_ACPI_3_0_FIXED_ACPI_DESCRIPTION_TABLE_SIGNATURE:
+      if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+        AcpiTableInstance->Fadt1 = NULL;
+      }
+
+      if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+        AcpiTableInstance->Fadt3 = NULL;
+      }
+      break;
+
+    case EFI_ACPI_3_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE:
+      if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+        AcpiTableInstance->Facs1 = NULL;
+
+        //
+        // Update FADT table pointers
+        //
+        if (AcpiTableInstance->Fadt1 != NULL) {
+          AcpiTableInstance->Fadt1->FirmwareCtrl = 0;
+
+          //
+          // Checksum table
+          //
+          AcpiPlatformChecksum (
+            AcpiTableInstance->Fadt1,
+            AcpiTableInstance->Fadt1->Header.Length,
+            OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+            Checksum)
+            );
+        }
+      }
+
+      if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+        AcpiTableInstance->Facs3 = NULL;
+
+        //
+        // Update FADT table pointers
+        //
+        if (AcpiTableInstance->Fadt3 != NULL) {
+          AcpiTableInstance->Fadt3->FirmwareCtrl = 0;
+          ZeroMem (&AcpiTableInstance->Fadt3->XFirmwareCtrl, sizeof (UINT64));
+
+          //
+          // Checksum table
+          //
+          AcpiPlatformChecksum (
+            AcpiTableInstance->Fadt3,
+            AcpiTableInstance->Fadt3->Header.Length,
+            OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+            Checksum)
+            );
+        }
+      }    
+      break;
+
+    case EFI_ACPI_3_0_DIFFERENTIATED_SYSTEM_DESCRIPTION_TABLE_SIGNATURE:
+      if ((Version & EFI_ACPI_TABLE_VERSION_1_0B) != 0) {
+        AcpiTableInstance->Dsdt1 = NULL;
+
+        //
+        // Update FADT table pointers
+        //
+        if (AcpiTableInstance->Fadt1 != NULL) {
+          AcpiTableInstance->Fadt1->Dsdt = 0;
+
+          //
+          // Checksum table
+          //
+          AcpiPlatformChecksum (
+            AcpiTableInstance->Fadt1,
+            AcpiTableInstance->Fadt1->Header.Length,
+            OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+            Checksum)
+            );
+        }
+      }
+
+      
+      if ((Version & ACPI_TABLE_VERSION_GTE_2_0) != 0) {
+        AcpiTableInstance->Dsdt3 = NULL;
+
+        //
+        // Update FADT table pointers
+        //
+        if (AcpiTableInstance->Fadt3 != NULL) {
+          AcpiTableInstance->Fadt3->Dsdt = 0;
+          ZeroMem (&AcpiTableInstance->Fadt3->XDsdt, sizeof (UINT64));
+
+          //
+          // Checksum table
+          //
+          AcpiPlatformChecksum (
+            AcpiTableInstance->Fadt3,
+            AcpiTableInstance->Fadt3->Header.Length,
+            OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+            Checksum)
+            );
+        }
+      }
+      break;
+
+    default:
+      //
+      // Do nothing
+      //
+      break;
+    }
+  }
+  //
+  // If no version is using this table anymore, remove and free list entry.
+  //
+  if (Table->Version == 0) {
+    //
+    // Free the Table
+    //
+    gBS->FreePages (Table->PageAddress, Table->NumberOfPages);
+    RemoveEntryList (&(Table->Link));
+    gBS->FreePool (Table);
+  }
+  //
+  // Done
+  //
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function finds and removes the table specified by the handle.
+
+  @param  AcpiTableInstance  Instance of the protocol.
+  @param  Version            Bitmask of which versions to remove.
+  @param  Handle             Table to remove.
+
+  @return EFI_SUCCESS    The function completed successfully.
+  @return EFI_ABORTED    An error occurred.
+  @return EFI_NOT_FOUND  Handle not found in table list.
+
+**/
+EFI_STATUS
+RemoveTableFromList (
+  IN EFI_ACPI_TABLE_INSTANCE              *AcpiTableInstance,
+  IN EFI_ACPI_TABLE_VERSION               Version,
+  IN UINTN                                Handle
+  )
+{
+  EFI_ACPI_TABLE_LIST *Table;
+  EFI_STATUS          Status;
+
+  Table = (EFI_ACPI_TABLE_LIST*) NULL;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (AcpiTableInstance);
+
+  //
+  // Find the table
+  //
+  Status = FindTableByHandle (
+            Handle,
+            &AcpiTableInstance->TableList,
+            &Table
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Remove the table
+  //
+  Status = DeleteTable (AcpiTableInstance, Version, Table);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Completed successfully
+  //
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function calculates and updates an UINT8 checksum.
+
+  @param  Buffer          Pointer to buffer to checksum
+  @param  Size            Number of bytes to checksum
+  @param  ChecksumOffset  Offset to place the checksum result in
+
+  @return EFI_SUCCESS             The function completed successfully.
+
+**/
+EFI_STATUS
+AcpiPlatformChecksum (
+  IN VOID       *Buffer,
+  IN UINTN      Size,
+  IN UINTN      ChecksumOffset
+  )
+{
+  UINT8 Sum;
+  UINT8 *Ptr;
+
+  Sum = 0;
+  //
+  // Initialize pointer
+  //
+  Ptr = Buffer;
+
+  //
+  // set checksum to 0 first
+  //
+  Ptr[ChecksumOffset] = 0;
+
+  //
+  // add all content of buffer
+  //
+  while ((Size--) != 0) {
+    Sum = (UINT8) (Sum + (*Ptr++));
+  }
+  //
+  // set checksum
+  //
+  Ptr                 = Buffer;
+  Ptr[ChecksumOffset] = (UINT8) (0xff - Sum + 1);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Checksum all versions of the common tables, RSDP, RSDT, XSDT.
+
+  @param  AcpiTableInstance  Protocol instance private data.
+
+  @return EFI_SUCCESS        The function completed successfully.
+
+**/
+EFI_STATUS
+ChecksumCommonTables (
+  IN OUT EFI_ACPI_TABLE_INSTANCE                   *AcpiTableInstance
+  )
+{
+  //
+  // RSDP ACPI 1.0 checksum for 1.0 table.  This is only the first 20 bytes of the structure
+  //
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Rsdp1,
+    sizeof (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER),
+    OFFSET_OF (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER,
+    Checksum)
+    );
+
+  //
+  // RSDP ACPI 1.0 checksum for 2.0/3.0 table.  This is only the first 20 bytes of the structure
+  //
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Rsdp3,
+    sizeof (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER),
+    OFFSET_OF (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER,
+    Checksum)
+    );
+
+  //
+  // RSDP ACPI 2.0/3.0 checksum, this is the entire table
+  //
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Rsdp3,
+    sizeof (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER),
+    OFFSET_OF (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER,
+    ExtendedChecksum)
+    );
+
+  //
+  // RSDT checksums
+  //
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Rsdt1,
+    AcpiTableInstance->Rsdt1->Length,
+    OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+    Checksum)
+    );
+
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Rsdt3,
+    AcpiTableInstance->Rsdt3->Length,
+    OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+    Checksum)
+    );
+
+  //
+  // XSDT checksum
+  //
+  AcpiPlatformChecksum (
+    AcpiTableInstance->Xsdt,
+    AcpiTableInstance->Xsdt->Length,
+    OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER,
+    Checksum)
+    );
+  
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Constructor for the ACPI table protocol.  Initializes instance
+  data.
+
+  @param  AcpiTableInstance   Instance to construct
+
+  @return EFI_SUCCESS             Instance initialized.
+  @return EFI_OUT_OF_RESOURCES    Unable to allocate required resources.
+
+**/
+EFI_STATUS
+AcpiTableAcpiTableConstructor (
+  EFI_ACPI_TABLE_INSTANCE                   *AcpiTableInstance
+  )
+{
+  EFI_STATUS            Status;
+  UINT64                CurrentData;
+  UINTN                 TotalSize;
+  UINTN                 RsdpTableSize;
+  UINT8                 *Pointer;
+  EFI_PHYSICAL_ADDRESS  PageAddress;
+
+  //
+  // Check for invalid input parameters
+  //
+  ASSERT (AcpiTableInstance);
+
+  InitializeListHead (&AcpiTableInstance->TableList);
+  AcpiTableInstance->CurrentHandle              = 1;
+
+  AcpiTableInstance->AcpiTableProtocol.InstallAcpiTable   = InstallAcpiTable;
+  AcpiTableInstance->AcpiTableProtocol.UninstallAcpiTable = UninstallAcpiTable;
+
+  if (FeaturePcdGet (PcdInstallAcpiSdtProtocol)) {
+    SdtAcpiTableAcpiSdtConstructor (AcpiTableInstance);
+  }
+
+  //
+  // Create RSDP table
+  //
+  RsdpTableSize = sizeof (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER) +
+                  sizeof (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER);
+
+  PageAddress = 0xFFFFFFFF;
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiACPIReclaimMemory,
+                  EFI_SIZE_TO_PAGES (RsdpTableSize),
+                  &PageAddress
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Pointer = (UINT8 *) (UINTN) PageAddress;
+  ZeroMem (Pointer, RsdpTableSize);
+
+  AcpiTableInstance->Rsdp1 = (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER *) Pointer;
+  Pointer += sizeof (EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER);
+  AcpiTableInstance->Rsdp3 = (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER *) Pointer;
+
+  //
+  // Create RSDT, XSDT structures
+  //
+  TotalSize = sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 1.0 RSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 RSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT32) +
+              sizeof (EFI_ACPI_DESCRIPTION_HEADER) +         // for ACPI 2.0/3.0 XSDT
+              mEfiAcpiMaxNumTables * sizeof (UINT64);
+
+  //
+  // Allocate memory in the lower 32 bit of address range for
+  // compatibility with ACPI 1.0 OS.
+  //
+  // This is done because ACPI 1.0 pointers are 32 bit values.
+  // ACPI 2.0 OS and all 64 bit OS must use the 64 bit ACPI table addresses.
+  // There is no architectural reason these should be below 4GB, it is purely
+  // for convenience of implementation that we force memory below 4GB.
+  //
+  PageAddress = 0xFFFFFFFF;
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiACPIReclaimMemory,
+                  EFI_SIZE_TO_PAGES (TotalSize),
+                  &PageAddress
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->FreePages ((EFI_PHYSICAL_ADDRESS)(UINTN)AcpiTableInstance->Rsdp1, EFI_SIZE_TO_PAGES (RsdpTableSize));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Pointer = (UINT8 *) (UINTN) PageAddress;
+  ZeroMem (Pointer, TotalSize);
+
+  AcpiTableInstance->Rsdt1 = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+  Pointer += (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + EFI_ACPI_MAX_NUM_TABLES * sizeof (UINT32));
+  AcpiTableInstance->Rsdt3 = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+  Pointer += (sizeof (EFI_ACPI_DESCRIPTION_HEADER) + EFI_ACPI_MAX_NUM_TABLES * sizeof (UINT32));
+  AcpiTableInstance->Xsdt = (EFI_ACPI_DESCRIPTION_HEADER *) Pointer;
+
+  //
+  // Initialize RSDP
+  //
+  CurrentData = EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_POINTER_SIGNATURE;
+  CopyMem (&AcpiTableInstance->Rsdp1->Signature, &CurrentData, sizeof (UINT64));
+  CopyMem (AcpiTableInstance->Rsdp1->OemId, PcdGetPtr (PcdAcpiDefaultOemId), sizeof (AcpiTableInstance->Rsdp1->OemId));
+  AcpiTableInstance->Rsdp1->Reserved    = EFI_ACPI_RESERVED_BYTE;
+  AcpiTableInstance->Rsdp1->RsdtAddress = (UINT32) (UINTN) AcpiTableInstance->Rsdt1;
+
+  CurrentData = EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER_SIGNATURE;
+  CopyMem (&AcpiTableInstance->Rsdp3->Signature, &CurrentData, sizeof (UINT64));
+  CopyMem (AcpiTableInstance->Rsdp3->OemId, PcdGetPtr (PcdAcpiDefaultOemId), sizeof (AcpiTableInstance->Rsdp3->OemId));
+  AcpiTableInstance->Rsdp3->Revision    = EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER_REVISION;
+  AcpiTableInstance->Rsdp3->RsdtAddress = (UINT32) (UINTN) AcpiTableInstance->Rsdt3;
+  AcpiTableInstance->Rsdp3->Length      = sizeof (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER);
+  CurrentData = (UINT64) (UINTN) AcpiTableInstance->Xsdt;
+  CopyMem (&AcpiTableInstance->Rsdp3->XsdtAddress, &CurrentData, sizeof (UINT64));
+  SetMem (AcpiTableInstance->Rsdp3->Reserved, 3, EFI_ACPI_RESERVED_BYTE);
+
+  //
+  // Initialize Rsdt
+  //
+  // Note that we "reserve" one entry for the FADT so it can always be
+  // at the beginning of the list of tables.  Some OS don't seem
+  // to find it correctly if it is too far down the list.
+  //
+  AcpiTableInstance->Rsdt1->Signature = EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_TABLE_SIGNATURE;
+  AcpiTableInstance->Rsdt1->Length    = sizeof (EFI_ACPI_DESCRIPTION_HEADER);
+  AcpiTableInstance->Rsdt1->Revision  = EFI_ACPI_1_0_ROOT_SYSTEM_DESCRIPTION_TABLE_REVISION;
+  CopyMem (AcpiTableInstance->Rsdt1->OemId, PcdGetPtr (PcdAcpiDefaultOemId), sizeof (AcpiTableInstance->Rsdt1->OemId));
+  CurrentData = PcdGet64 (PcdAcpiDefaultOemTableId);
+  CopyMem (&AcpiTableInstance->Rsdt1->OemTableId, &CurrentData, sizeof (UINT64));
+  AcpiTableInstance->Rsdt1->OemRevision     = PcdGet32 (PcdAcpiDefaultOemRevision);
+  AcpiTableInstance->Rsdt1->CreatorId       = PcdGet32 (PcdAcpiDefaultCreatorId);
+  AcpiTableInstance->Rsdt1->CreatorRevision = PcdGet32 (PcdAcpiDefaultCreatorRevision);
+  //
+  // We always reserve first one for FADT
+  //
+  AcpiTableInstance->NumberOfTableEntries1  = 1;
+  AcpiTableInstance->Rsdt1->Length          = AcpiTableInstance->Rsdt1->Length + sizeof(UINT32);
+
+  AcpiTableInstance->Rsdt3->Signature       = EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_TABLE_SIGNATURE;
+  AcpiTableInstance->Rsdt3->Length          = sizeof (EFI_ACPI_DESCRIPTION_HEADER);
+  AcpiTableInstance->Rsdt3->Revision        = EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_TABLE_REVISION;
+  CopyMem (AcpiTableInstance->Rsdt3->OemId, PcdGetPtr (PcdAcpiDefaultOemId), sizeof (AcpiTableInstance->Rsdt3->OemId));
+  CurrentData = PcdGet64 (PcdAcpiDefaultOemTableId);
+  CopyMem (&AcpiTableInstance->Rsdt3->OemTableId, &CurrentData, sizeof (UINT64));
+  AcpiTableInstance->Rsdt3->OemRevision     = PcdGet32 (PcdAcpiDefaultOemRevision);
+  AcpiTableInstance->Rsdt3->CreatorId       = PcdGet32 (PcdAcpiDefaultCreatorId);
+  AcpiTableInstance->Rsdt3->CreatorRevision = PcdGet32 (PcdAcpiDefaultCreatorRevision);
+  //
+  // We always reserve first one for FADT
+  //
+  AcpiTableInstance->NumberOfTableEntries3  = 1;
+  AcpiTableInstance->Rsdt3->Length          = AcpiTableInstance->Rsdt3->Length + sizeof(UINT32);
+
+  //
+  // Initialize Xsdt
+  //
+  AcpiTableInstance->Xsdt->Signature  = EFI_ACPI_3_0_EXTENDED_SYSTEM_DESCRIPTION_TABLE_SIGNATURE;
+  AcpiTableInstance->Xsdt->Length     = sizeof (EFI_ACPI_DESCRIPTION_HEADER);
+  AcpiTableInstance->Xsdt->Revision   = EFI_ACPI_3_0_EXTENDED_SYSTEM_DESCRIPTION_TABLE_REVISION;
+  CopyMem (AcpiTableInstance->Xsdt->OemId, PcdGetPtr (PcdAcpiDefaultOemId), sizeof (AcpiTableInstance->Xsdt->OemId));
+  CurrentData = PcdGet64 (PcdAcpiDefaultOemTableId);
+  CopyMem (&AcpiTableInstance->Xsdt->OemTableId, &CurrentData, sizeof (UINT64));
+  AcpiTableInstance->Xsdt->OemRevision      = PcdGet32 (PcdAcpiDefaultOemRevision);
+  AcpiTableInstance->Xsdt->CreatorId        = PcdGet32 (PcdAcpiDefaultCreatorId);
+  AcpiTableInstance->Xsdt->CreatorRevision  = PcdGet32 (PcdAcpiDefaultCreatorRevision);
+  //
+  // We always reserve first one for FADT
+  //
+  AcpiTableInstance->Xsdt->Length           = AcpiTableInstance->Xsdt->Length + sizeof(UINT64);
+
+  ChecksumCommonTables (AcpiTableInstance);
+
+  //
+  // Completed successfully
+  //
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/Aml.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/Aml.c
new file mode 100644
index 0000000000..30f71bdda4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/Aml.c
@@ -0,0 +1,302 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2014, 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 "AcpiTable.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+AML_BYTE_ENCODING mAmlByteEncoding[] = {
+  //                             OpCode                      SubOpCode              Num 1           2           3           4           5           6           Attribute
+/* ZeroOp - 0x00 */              {AML_ZERO_OP,               0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* OneOp  - 0x01 */              {AML_ONE_OP,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* AliasOp - 0x06 */             {AML_ALIAS_OP,              0,                      2, {AML_NAME,   AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* NameOp - 0x08 */              {AML_NAME_OP,               0,                      2, {AML_NAME,   AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* BytePrefix - 0x0A */          {AML_BYTE_PREFIX,           0,                      1, {AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* WordPrefix - 0x0B */          {AML_WORD_PREFIX,           0,                      1, {AML_UINT16, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* DWordPrefix - 0x0C */         {AML_DWORD_PREFIX,          0,                      1, {AML_UINT32, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* StringPrefix - 0x0D */        {AML_STRING_PREFIX,         0,                      1, {AML_STRING, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* QWordPrefix - 0x0E */         {AML_QWORD_PREFIX,          0,                      1, {AML_UINT64, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ScopeOp - 0x10 */             {AML_SCOPE_OP,              0,                      1, {AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* BufferOp - 0x11 */            {AML_BUFFER_OP,             0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH},
+/* PackageOp - 0x12 */           {AML_PACKAGE_OP,            0,                      1, {AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ},
+/* VarPackageOp - 0x13 */        {AML_VAR_PACKAGE_OP,        0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ},
+/* MethodOp - 0x14 */            {AML_METHOD_OP,             0,                      2, {AML_NAME,   AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* DualNamePrefix - 0x2F */      {AML_DUAL_NAME_PREFIX,      0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* MultiNamePrefix - 0x2F */     {AML_MULTI_NAME_PREFIX,     0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x41 */            {'A',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x42 */            {'B',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x43 */            {'C',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x44 */            {'D',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x45 */            {'E',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x46 */            {'F',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x47 */            {'G',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x48 */            {'H',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x49 */            {'I',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4A */            {'J',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4B */            {'K',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4C */            {'L',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4D */            {'M',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4E */            {'N',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x4F */            {'O',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x50 */            {'P',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x51 */            {'Q',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x52 */            {'R',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x53 */            {'S',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x54 */            {'T',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x55 */            {'U',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x56 */            {'V',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x57 */            {'W',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x58 */            {'X',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x59 */            {'Y',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x5A */            {'Z',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* MutexOp - 0x5B 0x01 */        {AML_EXT_OP,                AML_EXT_MUTEX_OP,       2, {AML_NAME,   AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* EventOp - 0x5B 0x02 */        {AML_EXT_OP,                AML_EXT_EVENT_OP,       1, {AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* CondRefOfOp - 0x5B 0x12 */    {AML_EXT_OP,                AML_EXT_COND_REF_OF_OP, 2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CreateFieldOp - 0x5B 0x13 */  {AML_EXT_OP,                AML_EXT_CREATE_FIELD_OP,4, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE},   0},
+/* LoadTableOp - 0x5B 0x1F */    {AML_EXT_OP,                AML_EXT_LOAD_TABLE_OP,  6, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_OBJECT}, 0},
+/* LoadOp - 0x5B 0x20 */         {AML_EXT_OP,                AML_EXT_LOAD_OP,        2, {AML_NAME,   AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* StallOp - 0x5B 0x21 */        {AML_EXT_OP,                AML_EXT_STALL_OP,       1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* SleepOp - 0x5B 0x22 */        {AML_EXT_OP,                AML_EXT_SLEEP_OP,       1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* AcquireOp - 0x5B 0x23 */      {AML_EXT_OP,                AML_EXT_ACQUIRE_OP,     2, {AML_OBJECT, AML_UINT16, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* SignalOp - 0x5B 0x24 */       {AML_EXT_OP,                AML_EXT_SIGNAL_OP,      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* WaitOp - 0x5B 0x25 */         {AML_EXT_OP,                AML_EXT_WAIT_OP,        2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ResetOp - 0x5B 0x26 */        {AML_EXT_OP,                AML_EXT_RESET_OP,       1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ReleaseOp - 0x5B 0x27 */      {AML_EXT_OP,                AML_EXT_RELEASE_OP,     1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* FromBCDOp - 0x5B 0x28 */      {AML_EXT_OP,                AML_EXT_FROM_BCD_OP,    2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToBCDOp - 0x5B 0x29 */        {AML_EXT_OP,                AML_EXT_TO_BCD_OP,      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* UnloadOp - 0x5B 0x2A */       {AML_EXT_OP,                AML_EXT_UNLOAD_OP,      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* RevisionOp - 0x5B 0x30 */     {AML_EXT_OP,                AML_EXT_REVISION_OP,    0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* DebugOp - 0x5B 0x31 */        {AML_EXT_OP,                AML_EXT_DEBUG_OP,       0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* FatalOp - 0x5B 0x32 */        {AML_EXT_OP,                AML_EXT_FATAL_OP,       3, {AML_UINT8,  AML_UINT32, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* TimerOp - 0x5B 0x33 */        {AML_EXT_OP,                AML_EXT_TIMER_OP,       0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* OpRegionOp - 0x5B 0x80 */     {AML_EXT_OP,                AML_EXT_REGION_OP,      4, {AML_NAME,   AML_UINT8,  AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* FieldOp - 0x5B 0x81 */        {AML_EXT_OP,                AML_EXT_FIELD_OP,       2, {AML_NAME,   AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH},
+/* DeviceOp - 0x5B 0x82 */       {AML_EXT_OP,                AML_EXT_DEVICE_OP,      1, {AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* ProcessorOp - 0x5B 0x83 */    {AML_EXT_OP,                AML_EXT_PROCESSOR_OP,   4, {AML_NAME,   AML_UINT8,  AML_UINT32, AML_UINT8,  AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* PowerResOp - 0x5B 0x84 */     {AML_EXT_OP,                AML_EXT_POWER_RES_OP,   3, {AML_NAME,   AML_UINT8,  AML_UINT16, AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* ThermalZoneOp - 0x5B 0x85 */  {AML_EXT_OP,                AML_EXT_THERMAL_ZONE_OP,1, {AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ | AML_IN_NAMESPACE},
+/* IndexFieldOp - 0x5B 0x86 */   {AML_EXT_OP,                AML_EXT_INDEX_FIELD_OP, 3, {AML_NAME,   AML_NAME,   AML_UINT8,  AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH},
+/* BankFieldOp - 0x5B 0x87 */    {AML_EXT_OP,                AML_EXT_BANK_FIELD_OP,  4, {AML_NAME,   AML_NAME,   AML_OBJECT, AML_UINT8,  AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH},
+/* DataRegionOp - 0x5B 0x88 */   {AML_EXT_OP,                AML_EXT_DATA_REGION_OP, 4, {AML_NAME,   AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE},   AML_IN_NAMESPACE},
+/* RootChar - 0x5C */            {AML_ROOT_CHAR,             0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* ParentPrefixChar - 0x5E */    {AML_PARENT_PREFIX_CHAR,    0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* NameChar - 0x5F */            {'_',                       0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_IS_NAME_CHAR},
+/* Local0Op - 0x60 */            {AML_LOCAL0,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local1Op - 0x61 */            {AML_LOCAL1,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local2Op - 0x62 */            {AML_LOCAL2,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local3Op - 0x63 */            {AML_LOCAL3,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local4Op - 0x64 */            {AML_LOCAL4,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local5Op - 0x65 */            {AML_LOCAL5,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local6Op - 0x66 */            {AML_LOCAL6,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Local7Op - 0x67 */            {AML_LOCAL7,                0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg0Op - 0x68 */              {AML_ARG0,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg1Op - 0x69 */              {AML_ARG1,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg2Op - 0x6A */              {AML_ARG2,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg3Op - 0x6B */              {AML_ARG3,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg4Op - 0x6C */              {AML_ARG4,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg5Op - 0x6D */              {AML_ARG5,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* Arg6Op - 0x6E */              {AML_ARG6,                  0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* StoreOp - 0x70 */             {AML_STORE_OP,              0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* RefOfOp - 0x71 */             {AML_REF_OF_OP,             0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* AddOp - 0x72 */               {AML_ADD_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ConcatOp - 0x73 */            {AML_CONCAT_OP,             0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* SubtractOp - 0x74 */          {AML_SUBTRACT_OP,           0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* IncrementOp - 0x75 */         {AML_INCREMENT_OP,          0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* DecrementOp - 0x76 */         {AML_DECREMENT_OP,          0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* MultiplyOp - 0x77 */          {AML_MULTIPLY_OP,           0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* DivideOp - 0x78 */            {AML_DIVIDE_OP,             0,                      4, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE},   0},
+/* ShiftLeftOp - 0x79 */         {AML_SHIFT_LEFT_OP,         0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ShiftRightOp - 0x7A */        {AML_SHIFT_RIGHT_OP,        0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* AndOp - 0x7B */               {AML_AND_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* NAndOp - 0x7C */              {AML_NAND_OP,               0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* OrOp - 0x7D */                {AML_OR_OP,                 0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* NorOp - 0x7E */               {AML_NOR_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* XOrOp - 0x7F */               {AML_XOR_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* NotOp - 0x80 */               {AML_NOT_OP,                0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* FindSetLeftBitOp - 0x81 */    {AML_FIND_SET_LEFT_BIT_OP,  0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* FindSetRightBitOp - 0x82 */   {AML_FIND_SET_RIGHT_BIT_OP, 0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* DerefOfOp - 0x83 */           {AML_DEREF_OF_OP,           0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ConcatResOp - 0x84 */         {AML_CONCAT_RES_OP,         0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ModOp - 0x85 */               {AML_MOD_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* NotifyOp - 0x86 */            {AML_NOTIFY_OP,             0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* SizeOfOp - 0x87 */            {AML_SIZE_OF_OP,            0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* IndexOp - 0x88 */             {AML_INDEX_OP,              0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* MatchOp - 0x89 */             {AML_MATCH_OP,              0,                      6, {AML_OBJECT, AML_UINT8,  AML_OBJECT, AML_UINT8,  AML_OBJECT, AML_OBJECT}, 0},
+/* CreateDWordFieldOp - 0x8A */  {AML_CREATE_DWORD_FIELD_OP, 0,                      3, {AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CreateWordFieldOp - 0x8B */   {AML_CREATE_WORD_FIELD_OP,  0,                      3, {AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CreateByteFieldOp - 0x8C */   {AML_CREATE_BYTE_FIELD_OP,  0,                      3, {AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CreateBitFieldOp - 0x8D */    {AML_CREATE_BIT_FIELD_OP,   0,                      3, {AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ObjectTypeOp - 0x8E */        {AML_OBJECT_TYPE_OP,        0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CreateQWordFieldOp - 0x8F */  {AML_CREATE_QWORD_FIELD_OP, 0,                      3, {AML_OBJECT, AML_OBJECT, AML_NAME,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LAndOp - 0x90 */              {AML_LAND_OP,               0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LOrOp - 0x91 */               {AML_LOR_OP,                0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LNotOp - 0x92 */              {AML_LNOT_OP,               0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LEqualOp - 0x93 */            {AML_LEQUAL_OP,             0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LGreaterOp - 0x94 */          {AML_LGREATER_OP,           0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* LLessOp - 0x95 */             {AML_LLESS_OP,              0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToBufferOp - 0x96 */          {AML_TO_BUFFER_OP,          0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToDecimalStringOp - 0x97 */   {AML_TO_DEC_STRING_OP,      0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToHexStringOp - 0x98 */       {AML_TO_HEX_STRING_OP,      0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToIntegerOp - 0x99 */         {AML_TO_INTEGER_OP,         0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ToStringOp - 0x9C */          {AML_TO_STRING_OP,          0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* CopyObjectOp - 0x9D */        {AML_COPY_OBJECT_OP,        0,                      2, {AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* MidOp - 0x9E */               {AML_MID_OP,                0,                      3, {AML_OBJECT, AML_OBJECT, AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ContinueOp - 0x9F */          {AML_CONTINUE_OP,           0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* IfOp - 0xA0 */                {AML_IF_OP,                 0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ},
+/* ElseOp - 0xA1 */              {AML_ELSE_OP,               0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ},
+/* WhileOp - 0xA2 */             {AML_WHILE_OP,              0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   AML_HAS_PKG_LENGTH | AML_HAS_CHILD_OBJ},
+/* NoopOp - 0xA3 */              {AML_NOOP_OP,               0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* ReturnOp - 0xA4 */            {AML_RETURN_OP,             0,                      1, {AML_OBJECT, AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* BreakOp - 0xA5 */             {AML_BREAK_OP,              0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* BreakPointOp - 0xCC */        {AML_BREAK_POINT_OP,        0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+/* OnesOp - 0xFF */              {AML_ONES_OP,               0,                      0, {AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE,   AML_NONE},   0},
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED
+EFI_ACPI_DATA_TYPE  mAmlTypeToAcpiType[] = {
+  EFI_ACPI_DATA_TYPE_NONE,             // AML_NONE
+  EFI_ACPI_DATA_TYPE_OPCODE,           // AML_OPCODE
+  EFI_ACPI_DATA_TYPE_UINT,             // AML_UINT8
+  EFI_ACPI_DATA_TYPE_UINT,             // AML_UINT16
+  EFI_ACPI_DATA_TYPE_UINT,             // AML_UINT32
+  EFI_ACPI_DATA_TYPE_UINT,             // AML_UINT64
+  EFI_ACPI_DATA_TYPE_NAME_STRING,      // AML_NAME
+  EFI_ACPI_DATA_TYPE_STRING,           // AML_STRING
+  EFI_ACPI_DATA_TYPE_CHILD             // AML_OBJECT
+};
+
+/**
+  This function returns AmlByteEncoding according to OpCode Byte.
+
+  @param[in]  OpByteBuffer        OpCode byte buffer.
+
+  @return AmlByteEncoding
+**/
+AML_BYTE_ENCODING *
+AmlSearchByOpByte (
+  IN UINT8                *OpByteBuffer
+  )
+{
+  UINT8       OpCode;
+  UINT8       SubOpCode;
+  UINTN       Index;
+
+  //
+  // Get OpCode and SubOpCode
+  //
+  OpCode = OpByteBuffer[0];
+  if (OpCode == AML_EXT_OP) {
+    SubOpCode = OpByteBuffer[1];
+  } else {
+    SubOpCode = 0;
+  }
+
+  //
+  // Search the table
+  //
+  for (Index = 0; Index < sizeof(mAmlByteEncoding)/sizeof(mAmlByteEncoding[0]); Index++) {
+    if ((mAmlByteEncoding[Index].OpCode == OpCode) && (mAmlByteEncoding[Index].SubOpCode == SubOpCode)) {
+      return &mAmlByteEncoding[Index];
+    }
+  }
+  
+  return NULL;
+}
+
+/**
+  This function returns AcpiDataType according to AmlType.
+
+  @param[in]  AmlType        AML Type.
+
+  @return AcpiDataType
+**/
+EFI_ACPI_DATA_TYPE
+AmlTypeToAcpiType (
+  IN AML_OP_PARSE_FORMAT  AmlType
+  )
+{
+  if (AmlType >= sizeof(mAmlTypeToAcpiType)/sizeof(mAmlTypeToAcpiType[0])) {
+    ASSERT(FALSE);
+    return EFI_ACPI_DATA_TYPE_NONE;
+  }
+  return mAmlTypeToAcpiType [AmlType];
+}
+
+/**
+  This function retuns package length from the buffer.
+
+  @param[in]  Buffer    AML buffer
+  @param[out] PkgLength The total length of package.
+
+  @return The byte data count to present the package length.
+**/
+UINTN
+AmlGetPkgLength (
+  IN UINT8              *Buffer,
+  OUT UINTN             *PkgLength
+  )
+{
+  UINT8             LeadByte;
+  UINT8             ByteCount;
+  UINTN             RealLength;
+  UINTN             Offset;
+  
+  //
+  // <bit 7-6: ByteData count that follows (0-3)>
+  // <bit 5-4: Only used if PkgLength < 63>
+  // <bit 3-0: Least significant package length nybble>
+  //
+  // Note: The high 2 bits of the first byte reveal how many follow bytes are in the
+  // If the PkgLength has only one byte, bit 0 through 5 are used to encode the
+  // package length (in other words, values 0-63). If the package length value is more than
+  // 63, more than one byte must be used for the encoding in which case bit 4 and 5 of the
+  // PkgLeadByte are reserved and must be zero. If the multiple bytes encoding is used,
+  // bits 0-3 of the PkgLeadByte become the least significant 4 bits of the resulting
+  // package length value. The next ByteData will become the next least significant 8 bits
+  // of the resulting value and so on, up to 3 ByteData bytes. Thus, the maximum package
+  // length is 2**28.
+  // 
+  
+  LeadByte = *Buffer;
+  ByteCount = (UINT8)((LeadByte >> 6) & 0x03);
+  Offset = ByteCount + 1;
+  RealLength = 0;
+
+  switch (ByteCount) {
+  case 0:
+    RealLength = (UINT32)LeadByte;
+    break;
+  case 1:
+    RealLength = *(Buffer + 1);
+    RealLength = (RealLength << 4) | (LeadByte & 0xF);
+    break;
+  case 2:
+    RealLength = *(Buffer + 1);
+    RealLength |= (UINTN)((*(Buffer + 2)) << 8);
+    RealLength = (RealLength << 4) | (LeadByte & 0xF);
+    break;
+  case 3:
+    RealLength = *(Buffer + 1);
+    RealLength |= (UINTN)((*(Buffer + 2)) << 8);
+    RealLength |= (UINTN)((*(Buffer + 3)) << 16);
+    RealLength = (RealLength << 4) | (LeadByte & 0xF);
+    break;
+  default:
+    ASSERT (0);
+    break;
+  }
+  
+  *PkgLength = RealLength;
+  return Offset;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlChild.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlChild.c
new file mode 100644
index 0000000000..c7e8af96d3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlChild.c
@@ -0,0 +1,280 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2014, 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 "AcpiTable.h"
+
+/**
+  Return the child objects buffer from AML Handle's buffer.
+  
+  @param[in]        AmlParentHandle Parent handle.
+  @param[in]        CurrentBuffer   The current child buffer.
+  @param[out]       Buffer          On return, points to the next returned child buffer or NULL if there are no
+                                    child buffer.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     AmlParentHandle does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromObjectBuffer (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN UINT8                  *CurrentBuffer,
+  OUT VOID                  **Buffer
+  )
+{
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  UINTN               DataSize;
+
+  //
+  // Root is considered as SCOPE, which has TermList.
+  // We need return only Object in TermList.
+  //
+  while ((UINTN)CurrentBuffer < (UINTN)(AmlParentHandle->Buffer + AmlParentHandle->Size)) {
+    AmlByteEncoding = AmlSearchByOpByte (CurrentBuffer);
+    if (AmlByteEncoding == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // NOTE: We need return everything, because user might need parse the returned object.
+    //
+    if ((AmlByteEncoding->Attribute & AML_IS_NAME_CHAR) == 0) {
+      *Buffer = CurrentBuffer;
+      return EFI_SUCCESS;
+    }
+
+    DataSize = AmlGetObjectSize (
+                 AmlByteEncoding,
+                 CurrentBuffer,
+                 (UINTN)AmlParentHandle->Buffer + AmlParentHandle->Size - (UINTN)CurrentBuffer
+                 );
+    if (DataSize == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+    CurrentBuffer += DataSize;
+  }
+
+  //
+  // No more
+  //
+  *Buffer = NULL;
+  return EFI_SUCCESS;
+}
+
+/**
+  Return the child ACPI objects from Root Handle.
+  
+  @param[in]        AmlParentHandle Parent handle. It is Root Handle.
+  @param[in]        AmlHandle       The previously returned handle or NULL to start with the first handle.
+  @param[out]       Buffer          On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromRoot (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  )
+{
+  UINT8               *CurrentBuffer;
+
+  if (AmlHandle == NULL) {
+    //
+    // First One
+    //
+    CurrentBuffer = (VOID *)AmlParentHandle->Buffer;
+  } else {
+    CurrentBuffer = (VOID *)(AmlHandle->Buffer + AmlHandle->Size);
+  }
+
+  return AmlGetChildFromObjectBuffer (AmlParentHandle, CurrentBuffer, Buffer);
+}
+
+/**
+  Return the child objects buffer from AML Handle's option list.
+  
+  @param[in]        AmlParentHandle Parent handle.
+  @param[in]        AmlHandle       The current child handle.
+  @param[out]       Buffer          On return, points to the next returned child buffer or NULL if there are no
+                                    child buffer.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     AmlParentHandle does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromOptionList (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  )
+{
+  EFI_ACPI_DATA_TYPE  DataType;
+  VOID                *Data;
+  UINTN               DataSize;
+  AML_OP_PARSE_INDEX  Index;
+  EFI_STATUS          Status;
+  AML_OP_PARSE_INDEX  MaxTerm;
+
+  Index = AML_OP_PARSE_INDEX_GET_TERM1;
+  MaxTerm = AmlParentHandle->AmlByteEncoding->MaxIndex;
+  while (Index <= MaxTerm) {
+    Status = AmlParseOptionHandleCommon (
+               AmlParentHandle,
+               (AML_OP_PARSE_INDEX)Index,
+               &DataType,
+               (VOID **)&Data,
+               &DataSize
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if (DataType == EFI_ACPI_DATA_TYPE_NONE) {
+      //
+      // Not found
+      //
+      break;
+    }
+
+    //
+    // Find it, and Check Data
+    //
+    if ((DataType == EFI_ACPI_DATA_TYPE_CHILD) &&
+        ((UINTN)AmlHandle->Buffer < (UINTN)Data)) {
+      //
+      // Buffer < Data means current node is next one
+      //
+      *Buffer = Data;
+      return EFI_SUCCESS;
+    }
+    //
+    // Not Child
+    //
+    Index ++;
+  }
+
+  *Buffer = NULL;
+  return EFI_SUCCESS;
+}
+
+/**
+  Return the child objects buffer from AML Handle's object child list.
+  
+  @param[in]        AmlParentHandle Parent handle.
+  @param[in]        AmlHandle       The current child handle.
+  @param[out]       Buffer          On return, points to the next returned child buffer or NULL if there are no
+                                    child buffer.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     AmlParentHandle does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromObjectChildList (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  )
+{
+  EFI_STATUS          Status;
+  UINT8               *CurrentBuffer;
+
+  CurrentBuffer = NULL;
+
+  if ((AmlParentHandle->AmlByteEncoding->Attribute & AML_HAS_CHILD_OBJ) == 0) {
+    //
+    // No ObjectList
+    //
+    *Buffer = NULL;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Do we need add node within METHOD?
+  // Yes, just add Object is OK. But we need filter NameString for METHOD invoke.
+  //
+
+  //
+  // Now, we get the last node.
+  //
+  Status = AmlGetOffsetAfterLastOption (AmlParentHandle, &CurrentBuffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Go through all the reset buffer.
+  //
+  if ((UINTN)AmlHandle->Buffer < (UINTN)CurrentBuffer) {
+    //
+    // Buffer < Data means next node is first object
+    //
+  } else if ((UINTN)AmlHandle->Buffer + AmlHandle->Size < (UINTN)AmlParentHandle->Buffer + AmlParentHandle->Size) {
+    //
+    // There is still more node
+    //
+    CurrentBuffer = AmlHandle->Buffer + AmlHandle->Size;
+  } else {
+    //
+    // No more data
+    //
+    *Buffer = NULL;
+    return EFI_SUCCESS;
+  }
+
+  return AmlGetChildFromObjectBuffer (AmlParentHandle, CurrentBuffer, Buffer);
+}
+
+/**
+  Return the child ACPI objects from Non-Root Handle.
+  
+  @param[in]        AmlParentHandle Parent handle. It is Non-Root Handle.
+  @param[in]        AmlHandle       The previously returned handle or NULL to start with the first handle.
+  @param[out]       Buffer          On return, points to the next returned ACPI handle or NULL if there are no
+                                    child objects.
+
+  @retval EFI_SUCCESS               Success
+  @retval EFI_INVALID_PARAMETER     ParentHandle is NULL or does not refer to a valid ACPI object.                                
+**/
+EFI_STATUS
+AmlGetChildFromNonRoot (
+  IN EFI_AML_HANDLE         *AmlParentHandle,
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT VOID                  **Buffer
+  )
+{
+  EFI_STATUS          Status;
+
+  if (AmlHandle == NULL) {
+    //
+    // NULL means first one
+    //
+    AmlHandle = AmlParentHandle;
+  }
+
+  //
+  // 1. Get Option
+  //
+  Status = AmlGetChildFromOptionList (AmlParentHandle, AmlHandle, Buffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*Buffer != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // 2. search ObjectList
+  //
+  return AmlGetChildFromObjectChildList (AmlParentHandle, AmlHandle, Buffer);
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlNamespace.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlNamespace.c
new file mode 100644
index 0000000000..03b7394c4d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlNamespace.c
@@ -0,0 +1,614 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2014, 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 "AcpiTable.h"
+
+/**
+  Construct node list according to the AML handle.
+  
+  @param[in]    AmlHandle            AML handle.
+  @param[in]    AmlRootNodeList      AML root node list.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AML handle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlConstructNodeList (
+  IN EFI_AML_HANDLE      *AmlHandle,
+  IN EFI_AML_NODE_LIST   *AmlRootNodeList,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList
+  );
+
+/**
+  Create AML Node.
+  
+  @param[in]    NameSeg              AML NameSeg.
+  @param[in]    Parent               AML parent node list.
+  @param[in]    AmlByteEncoding      AML Byte Encoding.
+  
+  @return       AML Node.
+**/
+EFI_AML_NODE_LIST *
+AmlCreateNode (
+  IN UINT8              *NameSeg,
+  IN EFI_AML_NODE_LIST  *Parent,
+  IN AML_BYTE_ENCODING  *AmlByteEncoding
+  )
+{
+  EFI_AML_NODE_LIST      *AmlNodeList;
+
+  AmlNodeList = AllocatePool (sizeof(*AmlNodeList));
+  ASSERT (AmlNodeList != NULL);
+
+  AmlNodeList->Signature = EFI_AML_NODE_LIST_SIGNATURE;
+  CopyMem (AmlNodeList->Name, NameSeg, AML_NAME_SEG_SIZE);
+  AmlNodeList->Buffer    = NULL;
+  AmlNodeList->Size      = 0;
+  InitializeListHead (&AmlNodeList->Link);
+  InitializeListHead (&AmlNodeList->Children);
+  AmlNodeList->Parent = Parent;
+  AmlNodeList->AmlByteEncoding = AmlByteEncoding;
+
+  return AmlNodeList;
+}
+
+/**
+  Find the AML NameSeg in the children of AmlParentNodeList.
+  
+  @param[in]    NameSeg              AML NameSeg.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  @param[in]    Create               TRUE means to create node if not found.
+  
+  @return       AmlChildNode whoes name is same as NameSeg.
+**/
+EFI_AML_NODE_LIST *
+AmlFindNodeInThis (
+  IN UINT8               *NameSeg,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList,
+  IN BOOLEAN             Create
+  )
+{
+  EFI_AML_NODE_LIST      *CurrentAmlNodeList;
+  LIST_ENTRY             *CurrentLink;
+  LIST_ENTRY             *StartLink;
+  EFI_AML_NODE_LIST      *AmlNodeList;
+
+  StartLink   = &AmlParentNodeList->Children;
+  CurrentLink = StartLink->ForwardLink;
+
+  while (CurrentLink != StartLink) {
+    CurrentAmlNodeList = EFI_AML_NODE_LIST_FROM_LINK (CurrentLink);
+    //
+    // AML name is same as the one stored
+    //
+    if (CompareMem (CurrentAmlNodeList->Name, NameSeg, AML_NAME_SEG_SIZE) == 0) {
+      //
+      // Good! Found it
+      //
+      return CurrentAmlNodeList;
+    }
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  //
+  // Not found
+  //
+  if (!Create) {
+    return NULL;
+  }
+
+  //
+  // Create new node with NULL buffer - it means namespace not be returned.
+  //
+  AmlNodeList = AmlCreateNode (NameSeg, AmlParentNodeList, NULL);
+  InsertTailList (&AmlParentNodeList->Children, &AmlNodeList->Link);
+
+  return AmlNodeList;
+}
+
+/**
+  Find the AML NameString in the children of AmlParentNodeList or AmlRootNodeList.
+  
+  @param[in]    NameString           AML NameString.
+  @param[in]    AmlRootNodeList      AML root node list.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  @param[in]    Create               TRUE means to create node if not found.
+  
+  @return       AmlChildNode whoes name is same as NameSeg.
+**/
+EFI_AML_NODE_LIST *
+AmlFindNodeInTheTree (
+  IN UINT8               *NameString,
+  IN EFI_AML_NODE_LIST   *AmlRootNodeList,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList,
+  IN BOOLEAN             Create
+  )
+{
+  UINT8               *Buffer;
+  EFI_AML_NODE_LIST   *AmlNodeList;
+  EFI_AML_NODE_LIST   *AmlCurrentNodeList;
+  UINT8               Index;
+  UINT8               SegCount;
+
+  Buffer = NameString;
+
+  //
+  // Handle root or parent prefix
+  //
+  if (*Buffer == AML_ROOT_CHAR) {
+    AmlCurrentNodeList = AmlRootNodeList;
+    Buffer += 1;
+  } else if (*Buffer == AML_PARENT_PREFIX_CHAR) {
+    AmlCurrentNodeList = AmlParentNodeList;
+    do {
+      if (AmlCurrentNodeList->Parent != NULL) {
+        AmlCurrentNodeList = AmlCurrentNodeList->Parent;
+      } else {
+        //
+        // Only root has no parent
+        //
+        ASSERT (AmlCurrentNodeList == AmlRootNodeList);
+      }
+      Buffer += 1;
+    } while (*Buffer == AML_PARENT_PREFIX_CHAR);
+  } else {
+    AmlCurrentNodeList = AmlParentNodeList;
+  }
+  
+  //
+  // Handle name segment
+  //
+  if (*Buffer == AML_DUAL_NAME_PREFIX) {
+    Buffer += 1;
+    SegCount = 2;
+  } else if (*Buffer == AML_MULTI_NAME_PREFIX) {
+    Buffer += 1;
+    SegCount = *Buffer;
+    Buffer += 1;
+  } else if (*Buffer == 0) {
+    //
+    // NULL name, only for Root
+    //
+    ASSERT (AmlCurrentNodeList == AmlRootNodeList);
+    return AmlCurrentNodeList;
+  } else {
+    SegCount = 1;
+  }
+
+  //
+  // Handle NamePath
+  //
+  Index = 0;
+  do {
+    AmlNodeList = AmlFindNodeInThis (Buffer, AmlCurrentNodeList, Create);
+    if (AmlNodeList == NULL) {
+      return NULL;
+    }
+    AmlCurrentNodeList = AmlNodeList;
+    Buffer += AML_NAME_SEG_SIZE;
+    Index ++;
+  } while (Index < SegCount);
+
+  return AmlNodeList;
+}
+
+/**
+  Insert the NameString to the AmlNodeList.
+  
+  @param[in]    NameString           AML NameString.
+  @param[in]    Buffer               Buffer for the Node.
+  @param[in]    Size                 Size for the Node.
+  @param[in]    AmlRootNodeList      AML root node list.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  
+  @return       AmlChildNode whoes name is NameString.
+**/
+EFI_AML_NODE_LIST *
+AmlInsertNodeToTree (
+  IN UINT8               *NameString,
+  IN VOID                *Buffer,
+  IN UINTN               Size,
+  IN EFI_AML_NODE_LIST   *AmlRootNodeList,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList
+  )
+{
+  EFI_AML_NODE_LIST   *AmlNodeList;
+
+  AmlNodeList = AmlFindNodeInTheTree (
+                  NameString,
+                  AmlRootNodeList,
+                  AmlParentNodeList,
+                  TRUE  // Find and Create
+                  );
+  ASSERT (AmlNodeList != NULL);
+  if (AmlNodeList == NULL) {
+    return NULL;
+  }
+
+  //
+  // Check buffer
+  //
+  if (AmlNodeList->Buffer == NULL) {
+    //
+    // NULL means new added one or SCOPE_OP
+    //
+    if (*(UINT8 *)Buffer != AML_SCOPE_OP) {
+      //
+      // We need check if new one is SCOPE_OP, because SCOPE_OP just means namespace, not a real device.
+      // We should not return SCOPE_OP.
+      //
+      AmlNodeList->Buffer = Buffer;
+      AmlNodeList->Size   = Size;
+      AmlNodeList->AmlByteEncoding = AmlSearchByOpByte (Buffer);
+    }
+    return AmlNodeList;
+  }
+
+  //
+  // Already added
+  //
+  if (*(UINT8 *)Buffer == AML_SCOPE_OP) {
+    //
+    // The new one is SCOPE_OP, OK just return;
+    //
+    return AmlNodeList;
+  }
+
+  //
+  // Oops!!!, There must be something wrong.
+  //
+  DEBUG ((EFI_D_ERROR, "AML: Override Happen - %a!\n", NameString));
+  DEBUG ((EFI_D_ERROR, "AML: Existing Node - %x\n", AmlNodeList->Buffer));
+  DEBUG ((EFI_D_ERROR, "AML: New Buffer - %x\n", Buffer));
+
+  return NULL;
+}
+
+/**
+  Construct child node list according to the AML handle.
+  
+  @param[in]    AmlHandle            AML handle.
+  @param[in]    AmlRootNodeList      AML root node list.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AML handle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlConstructNodeListForChild (
+  IN EFI_AML_HANDLE      *AmlHandle,
+  IN EFI_AML_NODE_LIST   *AmlRootNodeList,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList
+  )
+{
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  UINT8               *Buffer;
+  UINTN               BufferSize;
+  UINT8               *CurrentBuffer;
+  EFI_AML_HANDLE      *AmlChildHandle;
+  EFI_STATUS          Status;
+
+  CurrentBuffer   = NULL;
+  AmlChildHandle  = NULL;
+  AmlByteEncoding = AmlHandle->AmlByteEncoding;
+  Buffer          = AmlHandle->Buffer;
+  BufferSize      = AmlHandle->Size;
+
+  //
+  // Check if we need recursively add node
+  //
+  if ((AmlByteEncoding->Attribute & AML_HAS_CHILD_OBJ) == 0) {
+    //
+    // No more node need to be added
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Do we need add node within METHOD?
+  // Yes, just add Object is OK. But we need filter NameString for METHOD invoke.
+  //
+
+  //
+  // Now, we get the last node.
+  //
+  Status = AmlGetOffsetAfterLastOption (AmlHandle, &CurrentBuffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Go through all the reset buffer.
+  //
+  while ((UINTN)CurrentBuffer < (UINTN)Buffer + BufferSize) {
+    //
+    // Find the child node.
+    //
+    Status = SdtOpenEx (CurrentBuffer, (UINTN)Buffer + BufferSize - (UINTN)CurrentBuffer, (EFI_ACPI_HANDLE *)&AmlChildHandle);
+    if (EFI_ERROR (Status)) {
+      //
+      // No child found, break now.
+      //
+      break;
+    }
+
+    //
+    // Good, find the child. Construct node recursively
+    //
+    Status = AmlConstructNodeList (
+               AmlChildHandle,
+               AmlRootNodeList,
+               AmlParentNodeList
+               );
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    //
+    // Parse next one
+    //
+    CurrentBuffer += AmlChildHandle->Size;
+
+    Close ((EFI_ACPI_HANDLE)AmlChildHandle);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Construct node list according to the AML handle.
+  
+  @param[in]    AmlHandle            AML handle.
+  @param[in]    AmlRootNodeList      AML root node list.
+  @param[in]    AmlParentNodeList    AML parent node list.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AML handle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlConstructNodeList (
+  IN EFI_AML_HANDLE      *AmlHandle,
+  IN EFI_AML_NODE_LIST   *AmlRootNodeList,
+  IN EFI_AML_NODE_LIST   *AmlParentNodeList
+  )
+{
+  VOID                *NameString;
+  EFI_AML_NODE_LIST   *AmlNodeList;
+
+  //
+  // 1. Check if there is need to construct node for this OpCode.
+  //
+  if ((AmlHandle->AmlByteEncoding->Attribute & AML_IN_NAMESPACE) == 0) {
+    //
+    // No need to construct node, so we just skip this OpCode.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // 2. Now, we need construct node for this OpCode.
+  //
+  NameString = AmlGetObjectName (AmlHandle);
+  if (NameString == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Now, we need to insert node to the node list.
+  // NOTE: The name here could be AML NameString. So the callee need parse it.
+  //
+  AmlNodeList = AmlInsertNodeToTree (NameString, AmlHandle->Buffer, AmlHandle->Size, AmlRootNodeList, AmlParentNodeList);
+  ASSERT (AmlNodeList != NULL);
+
+  //
+  // 3. Ok, we need to parse the object list to see if there are more node to be added.
+  //
+  return AmlConstructNodeListForChild (AmlHandle, AmlRootNodeList, AmlNodeList);
+}
+
+/**
+  Destruct node list
+  
+  @param[in]    AmlParentNodeList    AML parent node list.
+**/
+VOID
+AmlDestructNodeList (
+  IN EFI_AML_NODE_LIST *AmlParentNodeList
+  )
+{
+  EFI_AML_NODE_LIST      *CurrentAmlNodeList;
+  LIST_ENTRY             *CurrentLink;
+  LIST_ENTRY             *StartLink;
+
+  //
+  // Get the children link
+  //
+  StartLink   = &AmlParentNodeList->Children;
+  CurrentLink = StartLink->ForwardLink;
+
+  //
+  // Go through all the children
+  //
+  while (CurrentLink != StartLink) {
+    //
+    // Destruct the child's list recursively
+    //
+    CurrentAmlNodeList = EFI_AML_NODE_LIST_FROM_LINK (CurrentLink);
+    CurrentLink = CurrentLink->ForwardLink;
+
+    //
+    // Remove this child from list and free the node
+    //
+    RemoveEntryList (&(CurrentAmlNodeList->Link));
+
+    AmlDestructNodeList (CurrentAmlNodeList);
+  }
+
+  //
+  // Done.
+  //
+  FreePool (AmlParentNodeList);
+  return ;
+}
+
+/**
+  Dump node list
+  
+  @param[in]    AmlParentNodeList    AML parent node list.
+  @param[in]    Level                Output debug level.
+**/
+VOID
+AmlDumpNodeInfo (
+  IN EFI_AML_NODE_LIST *AmlParentNodeList,
+  IN UINTN             Level
+  )
+{
+  EFI_AML_NODE_LIST      *CurrentAmlNodeList;
+  volatile LIST_ENTRY    *CurrentLink;
+  UINTN                  Index;
+
+  CurrentLink = AmlParentNodeList->Children.ForwardLink;
+
+  if (Level == 0) {
+    DEBUG ((EFI_D_ERROR, "\\"));
+  } else {
+    for (Index = 0; Index < Level; Index++) {
+      DEBUG ((EFI_D_ERROR, "    "));
+    }
+    AmlPrintNameSeg (AmlParentNodeList->Name);
+  }
+  DEBUG ((EFI_D_ERROR, "\n"));
+
+  while (CurrentLink != &AmlParentNodeList->Children) {
+    CurrentAmlNodeList = EFI_AML_NODE_LIST_FROM_LINK (CurrentLink);
+    AmlDumpNodeInfo (CurrentAmlNodeList, Level + 1);
+    CurrentLink = CurrentLink->ForwardLink;
+  }
+
+  return ;
+}
+
+/**
+  Returns the handle of the ACPI object representing the specified ACPI AML path
+  
+  @param[in]    AmlHandle   Points to the handle of the object representing the starting point for the path search.
+  @param[in]    AmlPath     Points to the ACPI AML path.
+  @param[out]   Buffer      On return, points to the ACPI object which represents AcpiPath, relative to
+                            HandleIn.
+  @param[in]    FromRoot    TRUE means to find AML path from \ (Root) Node.
+                            FALSE means to find AML path from this Node (The HandleIn).
+                            
+  @retval EFI_SUCCESS           Success
+  @retval EFI_INVALID_PARAMETER HandleIn does not refer to a valid ACPI object.                            
+**/
+EFI_STATUS
+AmlFindPath (
+  IN    EFI_AML_HANDLE  *AmlHandle,
+  IN    UINT8           *AmlPath,
+  OUT   VOID            **Buffer,
+  IN    BOOLEAN         FromRoot
+  )
+{
+  EFI_AML_NODE_LIST   *AmlRootNodeList;
+  EFI_STATUS          Status;
+  EFI_AML_NODE_LIST   *AmlNodeList;
+  UINT8               RootNameSeg[AML_NAME_SEG_SIZE];
+  EFI_AML_NODE_LIST   *CurrentAmlNodeList;
+  LIST_ENTRY          *CurrentLink;
+
+  //
+  // 1. create tree
+  //
+
+  //
+  // Create root handle
+  //
+  RootNameSeg[0] = AML_ROOT_CHAR;
+  RootNameSeg[1] = 0;
+  AmlRootNodeList = AmlCreateNode (RootNameSeg, NULL, AmlHandle->AmlByteEncoding);
+
+  Status = AmlConstructNodeList (
+             AmlHandle,
+             AmlRootNodeList, // Root
+             AmlRootNodeList  // Parent
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG_CODE_BEGIN ();
+  DEBUG ((EFI_D_ERROR, "AcpiSdt: NameSpace:\n"));
+  AmlDumpNodeInfo (AmlRootNodeList, 0);
+  DEBUG_CODE_END ();
+
+  //
+  // 2. Search the node in the tree
+  //
+  if (FromRoot) {
+    //
+    // Search from Root
+    //
+    CurrentAmlNodeList = AmlRootNodeList;
+  } else {
+    //
+    // Search from this node, NOT ROOT.
+    // Since we insert node to ROOT one by one, we just get the first node and search from it.
+    //
+    CurrentLink = AmlRootNodeList->Children.ForwardLink;
+    if (CurrentLink != &AmlRootNodeList->Children) {
+      //
+      // First node
+      //
+      CurrentAmlNodeList = EFI_AML_NODE_LIST_FROM_LINK (CurrentLink);
+    } else {
+      //
+      // No child
+      //
+      CurrentAmlNodeList = NULL;
+    }
+  }
+
+  //
+  // Search
+  //
+  if (CurrentAmlNodeList != NULL) {
+    DEBUG_CODE_BEGIN ();
+    DEBUG ((EFI_D_ERROR, "AcpiSdt: Search from: \\"));
+    AmlPrintNameSeg (CurrentAmlNodeList->Name);
+    DEBUG ((EFI_D_ERROR, "\n"));
+    DEBUG_CODE_END ();
+    AmlNodeList = AmlFindNodeInTheTree (
+                    AmlPath,
+                    AmlRootNodeList,    // Root
+                    CurrentAmlNodeList, // Parent
+                    FALSE
+                    );
+  } else {
+    AmlNodeList = NULL;
+  }
+
+  *Buffer = NULL;
+  Status = EFI_SUCCESS;
+  if (AmlNodeList != NULL && AmlNodeList->Buffer != NULL) {
+    *Buffer = AmlNodeList->Buffer;
+  }
+
+  //
+  // 3. free the tree
+  //
+  AmlDestructNodeList (AmlRootNodeList);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlOption.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlOption.c
new file mode 100644
index 0000000000..93bbe480e8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlOption.c
@@ -0,0 +1,452 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010, 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 "AcpiTable.h"
+
+/**
+  Retrieve option term according to AmlByteEncoding and Buffer.
+  
+  @param[in]    AmlByteEncoding      AML Byte Encoding.
+  @param[in]    Buffer               AML buffer.
+  @param[in]    MaxBufferSize        AML buffer MAX size. The parser can not parse any data exceed this region.
+  @param[in]    TermIndex            Index of the data to retrieve from the object.
+  @param[out]   DataType             Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                                     for the specified index.
+  @param[out]   Data                 Upon return, points to the pointer to the data.
+  @param[out]   DataSize             Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Buffer does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlParseOptionTerm (
+  IN AML_BYTE_ENCODING   *AmlByteEncoding,
+  IN UINT8               *Buffer,
+  IN UINTN               MaxBufferSize,
+  IN AML_OP_PARSE_INDEX  TermIndex,
+  OUT EFI_ACPI_DATA_TYPE *DataType,
+  OUT VOID               **Data,
+  OUT UINTN              *DataSize
+  )
+{
+  AML_BYTE_ENCODING   *ChildAmlByteEncoding;
+  EFI_STATUS          Status;
+
+  if (DataType != NULL) {
+    *DataType = AmlTypeToAcpiType (AmlByteEncoding->Format[TermIndex - 1]);
+  }
+  if (Data != NULL) {
+    *Data = Buffer;
+  }
+  //
+  // Parse term according to AML type
+  //
+  switch (AmlByteEncoding->Format[TermIndex - 1]) {
+  case AML_UINT8:
+    *DataSize = sizeof(UINT8);
+    break;
+  case AML_UINT16:
+    *DataSize = sizeof(UINT16);
+    break;
+  case AML_UINT32:
+    *DataSize = sizeof(UINT32);
+    break;
+  case AML_UINT64:
+    *DataSize = sizeof(UINT64);
+    break;
+  case AML_STRING:
+    *DataSize = AsciiStrSize((CHAR8 *)Buffer);
+    break;
+  case AML_NAME:
+    Status = AmlGetNameStringSize (Buffer, DataSize);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    break;
+  case AML_OBJECT:
+    ChildAmlByteEncoding = AmlSearchByOpByte (Buffer);
+    if (ChildAmlByteEncoding == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // NOTE: We need override DataType here, if there is a case the AML_OBJECT is AML_NAME.
+    // We need convert type from EFI_ACPI_DATA_TYPE_CHILD to EFI_ACPI_DATA_TYPE_NAME_STRING.
+    // We should not return CHILD because there is NO OpCode for NameString.
+    //
+    if ((ChildAmlByteEncoding->Attribute & AML_IS_NAME_CHAR) != 0) {
+      if (DataType != NULL) {
+        *DataType = AmlTypeToAcpiType (AML_NAME);
+      }
+      Status = AmlGetNameStringSize (Buffer, DataSize);
+      if (EFI_ERROR (Status)) {
+        return EFI_INVALID_PARAMETER;
+      }
+      break;
+    }
+
+    //
+    // It is real AML_OBJECT
+    //
+    *DataSize = AmlGetObjectSize (
+                     ChildAmlByteEncoding,
+                     Buffer,
+                     MaxBufferSize
+                     );
+    if (*DataSize == 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+    break;
+  case AML_NONE:
+    //
+    // No term
+    //
+  case AML_OPCODE:
+  default:
+    ASSERT (FALSE);
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*DataSize > MaxBufferSize) {
+    return EFI_INVALID_PARAMETER;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieve information according to AmlByteEncoding and Buffer.
+  
+  @param[in]    AmlByteEncoding      AML Byte Encoding.
+  @param[in]    Buffer               AML buffer.
+  @param[in]    MaxBufferSize        AML buffer MAX size. The parser can not parse any data exceed this region.
+  @param[in]    Index                Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                                     in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[out]   DataType             Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                                     for the specified index.
+  @param[out]   Data                 Upon return, points to the pointer to the data.
+  @param[out]   DataSize             Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Buffer does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlParseOptionCommon (
+  IN AML_BYTE_ENCODING   *AmlByteEncoding,
+  IN UINT8               *Buffer,
+  IN UINTN               MaxBufferSize,
+  IN AML_OP_PARSE_INDEX  Index,
+  OUT EFI_ACPI_DATA_TYPE *DataType,
+  OUT VOID               **Data,
+  OUT UINTN              *DataSize
+  )
+{
+  UINT8               *CurrentBuffer;
+  UINTN               PkgLength;
+  UINTN               OpLength;
+  UINTN               PkgOffset;
+  AML_OP_PARSE_INDEX  TermIndex;
+  EFI_STATUS          Status;
+
+  ASSERT ((Index <= AmlByteEncoding->MaxIndex) || (Index == AML_OP_PARSE_INDEX_GET_SIZE));
+
+  //
+  // 0. Check if this is NAME string.
+  //
+  if ((AmlByteEncoding->Attribute & AML_IS_NAME_CHAR) != 0) {
+    //
+    // Only allow GET_SIZE
+    //
+    if (Index != AML_OP_PARSE_INDEX_GET_SIZE) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // return NameString size
+    //
+    Status = AmlGetNameStringSize (Buffer, DataSize);
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if (*DataSize > MaxBufferSize) {
+      return EFI_INVALID_PARAMETER;
+    }
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Not NAME string, start parsing
+  //
+  CurrentBuffer = Buffer;
+
+  //
+  // 1. Get OpCode
+  //
+  if (Index != AML_OP_PARSE_INDEX_GET_SIZE) {
+    *DataType = EFI_ACPI_DATA_TYPE_OPCODE;
+    *Data = (VOID *)CurrentBuffer;
+  }
+  if (*CurrentBuffer == AML_EXT_OP) {
+    OpLength = 2;
+  } else {
+    OpLength = 1;
+  }
+  *DataSize = OpLength;
+  if (Index == AML_OP_PARSE_INDEX_GET_OPCODE) {
+    return EFI_SUCCESS;
+  }
+  if (OpLength > MaxBufferSize) {
+    return EFI_INVALID_PARAMETER;
+  }
+  CurrentBuffer += OpLength;
+
+  //
+  // 2. Skip PkgLength field, if have
+  //
+  if ((AmlByteEncoding->Attribute & AML_HAS_PKG_LENGTH) != 0) {
+    PkgOffset = AmlGetPkgLength(CurrentBuffer, &PkgLength);
+    //
+    // Override MaxBufferSize if it is valid PkgLength
+    //
+    if (OpLength + PkgLength > MaxBufferSize) {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      MaxBufferSize = OpLength + PkgLength;
+    }
+  } else {
+    PkgOffset = 0;
+    PkgLength = 0;
+  }
+  CurrentBuffer += PkgOffset;
+
+  //
+  // 3. Get Term one by one.
+  //
+  TermIndex = AML_OP_PARSE_INDEX_GET_TERM1;
+  while ((Index >= TermIndex) && (TermIndex <= AmlByteEncoding->MaxIndex) && ((UINTN)CurrentBuffer < (UINTN)Buffer + MaxBufferSize)) {
+    Status = AmlParseOptionTerm (
+               AmlByteEncoding,
+               CurrentBuffer,
+               (UINTN)Buffer + MaxBufferSize - (UINTN)CurrentBuffer,
+               TermIndex,
+               DataType,
+               Data,
+               DataSize
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (Index == TermIndex) {
+      //
+      // Done
+      //
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Parse next one
+    //
+    CurrentBuffer += *DataSize;
+    TermIndex ++;
+  }
+
+  //
+  // Finish all options, but no option found.
+  //
+  if ((UINTN)CurrentBuffer > (UINTN)Buffer + MaxBufferSize) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((UINTN)CurrentBuffer == (UINTN)Buffer + MaxBufferSize) {
+    if (Index != AML_OP_PARSE_INDEX_GET_SIZE) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // 4. Finish parsing all node, return size
+  //
+  ASSERT (Index == AML_OP_PARSE_INDEX_GET_SIZE);
+  if ((AmlByteEncoding->Attribute & AML_HAS_PKG_LENGTH) != 0) {
+    *DataSize = OpLength + PkgLength;
+  } else {
+    *DataSize = (UINTN)CurrentBuffer - (UINTN)Buffer;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Return object size.
+  
+  @param[in]    AmlByteEncoding      AML Byte Encoding.
+  @param[in]    Buffer               AML object buffer.
+  @param[in]    MaxBufferSize        AML object buffer MAX size. The parser can not parse any data exceed this region.
+  
+  @return       Size of the object.
+**/
+UINTN
+AmlGetObjectSize (
+  IN AML_BYTE_ENCODING   *AmlByteEncoding,
+  IN UINT8               *Buffer,
+  IN UINTN               MaxBufferSize
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        DataSize;
+
+  Status = AmlParseOptionCommon (
+               AmlByteEncoding,
+               Buffer,
+               MaxBufferSize,
+               AML_OP_PARSE_INDEX_GET_SIZE,
+               NULL,
+               NULL,
+               &DataSize
+               );
+  if (EFI_ERROR (Status)) {
+    return 0;
+  } else {
+    return DataSize;
+  }
+}
+
+/**
+  Return object name.
+  
+  @param[in]    AmlHandle            AML handle.
+  
+  @return       Name of the object.
+**/
+CHAR8 *
+AmlGetObjectName (
+  IN EFI_AML_HANDLE      *AmlHandle
+  )
+{
+  AML_BYTE_ENCODING   *AmlByteEncoding;
+  VOID                *NameString;
+  UINTN               NameSize;
+  AML_OP_PARSE_INDEX  TermIndex;
+  EFI_STATUS          Status;
+  EFI_ACPI_DATA_TYPE  DataType;
+
+  AmlByteEncoding = AmlHandle->AmlByteEncoding;
+
+  ASSERT ((AmlByteEncoding->Attribute & AML_IN_NAMESPACE) != 0);
+
+  //
+  // Find out Last Name index, accroding to OpCode table.
+  // The last name will be the node name by design.
+  //
+  TermIndex = AmlByteEncoding->MaxIndex;
+  for (TermIndex = AmlByteEncoding->MaxIndex; TermIndex > 0; TermIndex--) {
+    if (AmlByteEncoding->Format[TermIndex - 1] == AML_NAME) {
+      break;
+    }
+  }
+  ASSERT (TermIndex != 0);
+
+  //
+  // Get Name for this node.
+  //
+  Status = AmlParseOptionHandleCommon (
+             AmlHandle,
+             TermIndex,
+             &DataType,
+             &NameString,
+             &NameSize
+             );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  ASSERT (DataType == EFI_ACPI_DATA_TYPE_NAME_STRING);
+
+  return NameString;
+}
+
+/**
+  Return offset of last option.
+  
+  @param[in]    AmlHandle            AML Handle.
+  @param[out]   Buffer               Upon return, points to the offset after last option.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AmlHandle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlGetOffsetAfterLastOption (
+  IN EFI_AML_HANDLE         *AmlHandle,
+  OUT UINT8                 **Buffer
+  )
+{
+  EFI_ACPI_DATA_TYPE  DataType;
+  VOID                *Data;
+  UINTN               DataSize;
+  EFI_STATUS          Status;
+
+  Status = AmlParseOptionHandleCommon (
+             AmlHandle,
+             AmlHandle->AmlByteEncoding->MaxIndex,
+             &DataType,
+             &Data,
+             &DataSize
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // We need to parse the rest buffer after last node.
+  //
+  *Buffer = (UINT8 *)((UINTN)Data + DataSize);
+
+  //
+  // We need skip PkgLength if no Option
+  //
+  if (DataType == EFI_ACPI_DATA_TYPE_OPCODE) {
+    *Buffer += AmlGetPkgLength (*Buffer, &DataSize);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieve information according to AmlHandle
+  
+  @param[in]    AmlHandle            AML handle.
+  @param[in]    Index                Index of the data to retrieve from the object. In general, indexes read from left-to-right
+                                     in the ACPI encoding, with index 0 always being the ACPI opcode.
+  @param[out]   DataType             Points to the returned data type or EFI_ACPI_DATA_TYPE_NONE if no data exists
+                                     for the specified index.
+  @param[out]   Data                 Upon return, points to the pointer to the data.
+  @param[out]   DataSize             Upon return, points to the size of Data.
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER AmlHandle does not refer to a valid ACPI object.
+**/
+EFI_STATUS
+AmlParseOptionHandleCommon (
+  IN EFI_AML_HANDLE      *AmlHandle,
+  IN AML_OP_PARSE_INDEX  Index,
+  OUT EFI_ACPI_DATA_TYPE *DataType,
+  OUT VOID               **Data,
+  OUT UINTN              *DataSize
+  )
+{
+  return AmlParseOptionCommon (
+           AmlHandle->AmlByteEncoding,
+           AmlHandle->Buffer,
+           AmlHandle->Size,
+           Index,
+           DataType,
+           Data,
+           DataSize
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlString.c b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlString.c
new file mode 100644
index 0000000000..9f47b48835
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/AcpiTableDxe/AmlString.c
@@ -0,0 +1,545 @@
+/** @file
+  ACPI Sdt Protocol Driver
+
+  Copyright (c) 2010 - 2011, 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 "AcpiTable.h"
+
+/**
+  Check if it is AML Root name
+
+  @param[in]    Buffer AML path.
+  
+  @retval       TRUE  AML path is root.
+  @retval       FALSE AML path is not root.
+**/
+BOOLEAN
+AmlIsRootPath (
+  IN UINT8              *Buffer
+  )
+{
+  if ((Buffer[0] == AML_ROOT_CHAR) && (Buffer[1] == 0)) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Check if it is AML LeadName.
+
+  @param[in]    Ch   Char.
+  
+  @retval       TRUE  Char is AML LeadName.
+  @retval       FALSE Char is not AML LeadName.
+**/
+BOOLEAN
+AmlIsLeadName (
+  IN CHAR8 Ch
+  )
+{
+  if ((Ch == '_') || (Ch >= 'A' && Ch <= 'Z')) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Check if it is AML Name.
+
+  @param[in]    Ch   Char.
+  
+  @retval       TRUE  Char is AML Name.
+  @retval       FALSE Char is not AML Name.
+**/
+BOOLEAN
+AmlIsName (
+  IN CHAR8 Ch
+  )
+{
+  if (AmlIsLeadName (Ch) || (Ch >= '0' && Ch <= '9')) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Return is buffer is AML NameSeg.
+
+  @param[in]    Buffer     AML NameSement.
+  
+  @retval       TRUE       It is AML NameSegment.
+  @retval       FALSE      It is not AML NameSegment.
+**/
+BOOLEAN
+AmlIsNameSeg (
+  IN  UINT8              *Buffer
+  )
+{
+  UINTN  Index;
+  if (!AmlIsLeadName (Buffer[0])) {
+    return FALSE;
+  }
+  for (Index = 1; Index < AML_NAME_SEG_SIZE; Index++) {
+    if (!AmlIsName (Buffer[Index])) {
+      return FALSE;
+    }
+  }
+  return TRUE;
+}
+
+/**
+  Get AML NameString size.
+
+  @param[in]    Buffer     AML NameString.
+  @param[out]   BufferSize AML NameString size 
+  
+  @retval       EFI_SUCCESS           Success.
+  @retval       EFI_INVALID_PARAMETER Buffer does not refer to a valid AML NameString.
+**/
+EFI_STATUS
+AmlGetNameStringSize (
+  IN  UINT8              *Buffer,
+  OUT UINTN              *BufferSize
+  )
+{
+  UINTN                 SegCount;
+  UINTN                 Length;
+  UINTN                 Index;
+
+  Length = 0;
+
+  //
+  // Parse root or parent prefix
+  //
+  if (*Buffer == AML_ROOT_CHAR) {
+    Buffer ++;
+    Length ++;
+  } else if (*Buffer == AML_PARENT_PREFIX_CHAR) {
+    do {
+      Buffer ++;
+      Length ++;
+    } while (*Buffer == AML_PARENT_PREFIX_CHAR);
+  }
+
+  //
+  // Parse name segment
+  //
+  if (*Buffer == AML_DUAL_NAME_PREFIX) {
+    Buffer ++;
+    Length ++;
+    SegCount = 2;
+  } else if (*Buffer == AML_MULTI_NAME_PREFIX) {
+    Buffer ++;
+    Length ++;
+    SegCount = *Buffer;
+    Buffer ++;
+    Length ++;
+  } else if (*Buffer == 0) {
+    //
+    // NULL Name, only for Root
+    //
+    SegCount = 0;
+    Buffer --;
+    if ((Length == 1) && (*Buffer == AML_ROOT_CHAR)) {
+      *BufferSize = 2;
+      return EFI_SUCCESS;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    // NameSeg
+    //
+    SegCount = 1;
+  }
+
+  Index = 0;
+  do {
+    if (!AmlIsNameSeg (Buffer)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    Buffer += AML_NAME_SEG_SIZE;
+    Length += AML_NAME_SEG_SIZE;
+    Index ++;
+  } while (Index < SegCount);
+
+  *BufferSize = Length;
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if it is ASL LeadName.
+
+  @param[in]    Ch   Char.
+  
+  @retval       TRUE  Char is ASL LeadName.
+  @retval       FALSE Char is not ASL LeadName.
+**/
+BOOLEAN
+AmlIsAslLeadName (
+  IN CHAR8 Ch
+  )
+{
+  if (AmlIsLeadName (Ch) || (Ch >= 'a' && Ch <= 'z')) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Check if it is ASL Name.
+
+  @param[in]    Ch   Char.
+  
+  @retval       TRUE  Char is ASL Name.
+  @retval       FALSE Char is not ASL Name.
+**/
+BOOLEAN
+AmlIsAslName (
+  IN CHAR8 Ch
+  )
+{
+  if (AmlIsAslLeadName (Ch) || (Ch >= '0' && Ch <= '9')) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Get ASL NameString size.
+
+  @param[in]    Buffer   ASL NameString.
+  
+  @return       ASL NameString size.
+**/
+UINTN
+AmlGetAslNameSegLength (
+  IN UINT8 *Buffer
+  )
+{
+  UINTN Length;
+  UINTN Index;
+
+  if (*Buffer == 0) {
+    return 0;
+  }
+  
+  Length = 0;
+  //
+  // 1st
+  //
+  if (AmlIsAslLeadName (*Buffer)) {
+    Length ++;
+    Buffer ++;
+  }
+  if ((*Buffer == 0) || (*Buffer == '.')) {
+    return Length;
+  }
+  //
+  // 2, 3, 4 name char
+  //
+  for (Index = 0; Index < 3; Index++) {
+    if (AmlIsAslName (*Buffer)) {
+      Length ++;
+      Buffer ++;
+    }
+    if ((*Buffer == 0) || (*Buffer == '.')) {
+      return Length;
+    }
+  }
+
+  //
+  // Invalid ASL name
+  //
+  return 0;
+}
+
+/**
+  Get ASL NameString size.
+
+  @param[in]    Buffer   ASL NameString.
+  @param[out]   Root     On return, points to Root char number.
+  @param[out]   Parent   On return, points to Parent char number.
+  @param[out]   SegCount On return, points to Segment count.
+  
+  @return       ASL NameString size.
+**/
+UINTN
+AmlGetAslNameStringSize (
+  IN UINT8              *Buffer,
+  OUT UINTN             *Root,
+  OUT UINTN             *Parent,
+  OUT UINTN             *SegCount
+  )
+{
+  UINTN NameLength;
+  UINTN TotalLength;
+
+  *Root   = 0;
+  *Parent = 0;
+  *SegCount = 0;
+  TotalLength = 0;
+  NameLength = 0;
+  if (*Buffer == AML_ROOT_CHAR) {
+    *Root = 1;
+    Buffer ++;
+  } else if (*Buffer == AML_PARENT_PREFIX_CHAR) {
+    do {
+      Buffer ++;
+      (*Parent) ++;
+    } while (*Buffer == AML_PARENT_PREFIX_CHAR);
+  }
+
+  //
+  // Now parse name
+  //
+  while (*Buffer != 0) {
+    NameLength = AmlGetAslNameSegLength (Buffer);
+    if ((NameLength == 0) || (NameLength > AML_NAME_SEG_SIZE)) {
+      return 0;
+    }
+    (*SegCount) ++;
+    Buffer += NameLength;
+    if (*Buffer == 0) {
+      break;
+    }
+    Buffer ++;
+  }
+
+  //
+  // Check SegCoount
+  //
+  if (*SegCount > 0xFF) {
+    return 0;
+  }
+
+  //
+  // Calculate total length
+  //
+  TotalLength = *Root + *Parent + (*SegCount) * AML_NAME_SEG_SIZE;
+  if (*SegCount > 2) {
+    TotalLength += 2;
+  } else if (*SegCount == 2) {
+    TotalLength += 1;
+  }
+
+  //
+  // Add NULL char
+  //
+  TotalLength ++;
+
+  return TotalLength;
+}
+
+/**
+  Copy mem, and cast all the char in dest to be upper case.
+
+  @param[in]    DstBuffer   Destination buffer.
+  @param[in]    SrcBuffer   Source buffer.
+  @param[in]    Length      Buffer length.
+**/
+VOID
+AmlUpperCaseCopyMem (
+  IN UINT8 *DstBuffer,
+  IN UINT8 *SrcBuffer,
+  IN UINTN Length
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < Length; Index++) {
+    if (SrcBuffer[Index] >= 'a' && SrcBuffer[Index] <= 'z') {
+      DstBuffer[Index] = (UINT8)(SrcBuffer[Index] - 'a' + 'A');
+    } else {
+      DstBuffer[Index] = SrcBuffer[Index];
+    }
+  }
+}
+
+/**
+  Return AML name according to ASL name.
+  The caller need free the AmlName returned.
+
+  @param[in]    AslPath     ASL name.
+
+  @return AmlName
+**/
+UINT8 *
+AmlNameFromAslName (
+  IN UINT8 *AslPath
+  )
+{
+  UINTN Root;
+  UINTN Parent;
+  UINTN SegCount;
+  UINTN TotalLength;
+  UINTN NameLength;
+  UINT8 *Buffer;
+  UINT8 *AmlPath;
+  UINT8 *AmlBuffer;
+
+  TotalLength = AmlGetAslNameStringSize (AslPath, &Root, &Parent, &SegCount);
+  if (TotalLength == 0) {
+    return NULL;
+  }
+
+  AmlPath = AllocatePool (TotalLength);
+  ASSERT (AmlPath != NULL);
+
+  AmlBuffer = AmlPath;
+  Buffer = AslPath;
+
+  //
+  // Handle Root and Parent
+  //
+  if (Root == 1) {
+    *AmlBuffer = AML_ROOT_CHAR;
+    AmlBuffer ++;
+    Buffer ++;
+  } else if (Parent > 0) {
+    SetMem (AmlBuffer, Parent, AML_PARENT_PREFIX_CHAR);
+    AmlBuffer += Parent;
+    Buffer += Parent;
+  }
+
+  //
+  // Handle SegCount
+  //
+  if (SegCount > 2) {
+    *AmlBuffer = AML_MULTI_NAME_PREFIX;
+    AmlBuffer ++;
+    *AmlBuffer = (UINT8)SegCount;
+    AmlBuffer ++;
+  } else if (SegCount == 2) {
+    *AmlBuffer = AML_DUAL_NAME_PREFIX;
+    AmlBuffer ++;
+  }
+
+  //
+  // Now to name
+  //
+  while (*Buffer != 0) {
+    NameLength = AmlGetAslNameSegLength (Buffer);
+    ASSERT ((NameLength != 0) && (NameLength <= AML_NAME_SEG_SIZE));
+    AmlUpperCaseCopyMem (AmlBuffer, Buffer, NameLength);
+    SetMem (AmlBuffer + NameLength, AML_NAME_SEG_SIZE - NameLength, AML_NAME_CHAR__);
+    Buffer += NameLength;
+    AmlBuffer += AML_NAME_SEG_SIZE;
+    if (*Buffer == 0) {
+      break;
+    }
+    Buffer ++;
+  }
+
+  //
+  // Add NULL
+  //
+  AmlPath[TotalLength - 1] = 0;
+
+  return AmlPath;
+}
+
+/**
+  Print AML NameSeg.
+
+  @param[in] Buffer AML NameSeg.
+**/
+VOID
+AmlPrintNameSeg (
+  IN UINT8              *Buffer
+  )
+{
+  DEBUG ((EFI_D_ERROR, "%c", Buffer[0]));
+  if ((Buffer[1] == '_') && (Buffer[2] == '_') && (Buffer[3] == '_')) {
+    return ;
+  }
+  DEBUG ((EFI_D_ERROR, "%c", Buffer[1]));
+  if ((Buffer[2] == '_') && (Buffer[3] == '_')) {
+    return ;
+  }
+  DEBUG ((EFI_D_ERROR, "%c", Buffer[2]));
+  if (Buffer[3] == '_') {
+    return ;
+  }
+  DEBUG ((EFI_D_ERROR, "%c", Buffer[3]));
+  return ;
+}
+
+/**
+  Print AML NameString.
+
+  @param[in] Buffer AML NameString.
+**/
+VOID
+AmlPrintNameString (
+  IN UINT8              *Buffer
+  )
+{
+  UINT8                 SegCount;
+  UINT8                 Index;
+
+  if (*Buffer == AML_ROOT_CHAR) {
+    //
+    // RootChar
+    //
+    Buffer ++;
+    DEBUG ((EFI_D_ERROR, "\\"));
+  } else if (*Buffer == AML_PARENT_PREFIX_CHAR) {
+    //
+    // ParentPrefixChar
+    //
+    do {
+      Buffer ++;
+      DEBUG ((EFI_D_ERROR, "^"));
+    } while (*Buffer == AML_PARENT_PREFIX_CHAR);
+  }
+
+  if (*Buffer == AML_DUAL_NAME_PREFIX) {
+    //
+    // DualName
+    //
+    Buffer ++;
+    SegCount = 2;
+  } else if (*Buffer == AML_MULTI_NAME_PREFIX) {
+    //
+    // MultiName
+    //
+    Buffer ++;
+    SegCount = *Buffer;
+    Buffer ++;
+  } else if (*Buffer == 0) {
+    //
+    // NULL Name
+    //
+    return ;
+  } else {
+    //
+    // NameSeg
+    //
+    SegCount = 1;
+  }
+  
+  AmlPrintNameSeg (Buffer);
+  Buffer += AML_NAME_SEG_SIZE;
+  for (Index = 0; Index < SegCount - 1; Index++) {
+    DEBUG ((EFI_D_ERROR, "."));
+    AmlPrintNameSeg (Buffer);
+    Buffer += AML_NAME_SEG_SIZE;
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.c b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.c
new file mode 100644
index 0000000000..804ffa5a6b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.c
@@ -0,0 +1,500 @@
+/** @file
+  This module install ACPI Boot Graphics Resource Table (BGRT).
+
+  Copyright (c) 2011 - 2013, 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 <Uefi.h>
+
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/Bmp.h>
+
+#include <Protocol/AcpiTable.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/BootLogo.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+
+//
+// Module globals.
+//
+EFI_EVENT  mBootGraphicsReadyToBootEvent;
+UINTN      mBootGraphicsResourceTableKey = 0;
+
+EFI_HANDLE                     mBootLogoHandle = NULL;
+BOOLEAN                        mIsLogoValid = FALSE;
+EFI_GRAPHICS_OUTPUT_BLT_PIXEL  *mLogoBltBuffer = NULL;
+UINTN                          mLogoDestX = 0;
+UINTN                          mLogoDestY = 0;
+UINTN                          mLogoWidth = 0;
+UINTN                          mLogoHeight = 0;
+
+BMP_IMAGE_HEADER  mBmpImageHeaderTemplate = {
+  'B',    // CharB
+  'M',    // CharM
+  0,      // Size will be updated at runtime
+  {0, 0}, // Reserved
+  sizeof (BMP_IMAGE_HEADER), // ImageOffset
+  sizeof (BMP_IMAGE_HEADER) - OFFSET_OF (BMP_IMAGE_HEADER, HeaderSize), // HeaderSize
+  0,      // PixelWidth will be updated at runtime
+  0,      // PixelHeight will be updated at runtime
+  1,      // Planes
+  24,     // BitPerPixel
+  0,      // CompressionType
+  0,      // ImageSize will be updated at runtime
+  0,      // XPixelsPerMeter
+  0,      // YPixelsPerMeter
+  0,      // NumberOfColors
+  0       // ImportantColors
+};
+
+BOOLEAN  mAcpiBgrtInstalled = FALSE;
+BOOLEAN  mAcpiBgrtStatusChanged = FALSE;
+BOOLEAN  mAcpiBgrtBufferChanged = FALSE;
+
+EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE mBootGraphicsResourceTableTemplate = {
+  {
+    EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE_SIGNATURE,
+    sizeof (EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE),
+    EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE_REVISION,     // Revision
+    0x00,  // Checksum will be updated at runtime
+    //
+    // It is expected that these values will be updated at EntryPoint.
+    //
+    {0x00},     // OEM ID is a 6 bytes long field
+    0x00,       // OEM Table ID(8 bytes long)
+    0x00,       // OEM Revision
+    0x00,       // Creator ID
+    0x00,       // Creator Revision
+  },
+  EFI_ACPI_5_0_BGRT_VERSION,         // Version
+  EFI_ACPI_5_0_BGRT_STATUS_VALID,    // Status
+  EFI_ACPI_5_0_BGRT_IMAGE_TYPE_BMP,  // Image Type
+  0,                                 // Image Address
+  0,                                 // Image Offset X
+  0                                  // Image Offset Y
+};
+
+/**
+  Update information of logo image drawn on screen.
+
+  @param  This           The pointer to the Boot Logo protocol instance.
+  @param  BltBuffer      The BLT buffer for logo drawn on screen. If BltBuffer
+                         is set to NULL, it indicates that logo image is no
+                         longer on the screen.
+  @param  DestinationX   X coordinate of destination for the BltBuffer.
+  @param  DestinationY   Y coordinate of destination for the BltBuffer.
+  @param  Width          Width of rectangle in BltBuffer in pixels.
+  @param  Height         Hight of rectangle in BltBuffer in pixels.
+
+  @retval EFI_SUCCESS             The boot logo information was updated.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_OUT_OF_RESOURCES    The logo information was not updated due to
+                                  insufficient memory resources.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBootLogo (
+  IN EFI_BOOT_LOGO_PROTOCOL            *This,
+  IN EFI_GRAPHICS_OUTPUT_BLT_PIXEL     *BltBuffer       OPTIONAL,
+  IN UINTN                             DestinationX,
+  IN UINTN                             DestinationY,
+  IN UINTN                             Width,
+  IN UINTN                             Height
+  );
+
+EFI_BOOT_LOGO_PROTOCOL  mBootLogoProtocolTemplate = { SetBootLogo };
+
+/**
+  Update information of logo image drawn on screen.
+
+  @param  This           The pointer to the Boot Logo protocol instance.
+  @param  BltBuffer      The BLT buffer for logo drawn on screen. If BltBuffer
+                         is set to NULL, it indicates that logo image is no
+                         longer on the screen.
+  @param  DestinationX   X coordinate of destination for the BltBuffer.
+  @param  DestinationY   Y coordinate of destination for the BltBuffer.
+  @param  Width          Width of rectangle in BltBuffer in pixels.
+  @param  Height         Hight of rectangle in BltBuffer in pixels.
+
+  @retval EFI_SUCCESS             The boot logo information was updated.
+  @retval EFI_INVALID_PARAMETER   One of the parameters has an invalid value.
+  @retval EFI_OUT_OF_RESOURCES    The logo information was not updated due to
+                                  insufficient memory resources.
+
+**/
+EFI_STATUS
+EFIAPI
+SetBootLogo (
+  IN EFI_BOOT_LOGO_PROTOCOL            *This,
+  IN EFI_GRAPHICS_OUTPUT_BLT_PIXEL     *BltBuffer       OPTIONAL,
+  IN UINTN                             DestinationX,
+  IN UINTN                             DestinationY,
+  IN UINTN                             Width,
+  IN UINTN                             Height
+  )
+{
+  UINT64                        BufferSize;
+
+  if (BltBuffer == NULL) {
+    mIsLogoValid = FALSE;
+    mAcpiBgrtStatusChanged = TRUE;
+    return EFI_SUCCESS;
+  }
+
+  if (Width == 0 || Height == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  mAcpiBgrtBufferChanged = TRUE;
+  if (mLogoBltBuffer != NULL) {
+    FreePool (mLogoBltBuffer);
+    mLogoBltBuffer = NULL;
+  }
+  
+  //
+  // Ensure the Height * Width doesn't overflow
+  //
+  if (Height > DivU64x64Remainder ((UINTN) ~0, Width, NULL)) {
+    return EFI_UNSUPPORTED;
+  }
+  BufferSize = MultU64x64 (Width, Height);
+  
+  //
+  // Ensure the BufferSize * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL) doesn't overflow
+  //
+  if (BufferSize > DivU64x32 ((UINTN) ~0, sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  mLogoBltBuffer = AllocateCopyPool (
+                     (UINTN)BufferSize * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL),
+                     BltBuffer
+                     );
+  if (mLogoBltBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  mLogoDestX = DestinationX;
+  mLogoDestY = DestinationY;
+  mLogoWidth = Width;
+  mLogoHeight = Height;
+  mIsLogoValid = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function calculates and updates an UINT8 checksum.
+
+  @param[in]  Buffer          Pointer to buffer to checksum.
+  @param[in]  Size            Number of bytes to checksum.
+
+**/
+VOID
+BgrtAcpiTableChecksum (
+  IN UINT8      *Buffer,
+  IN UINTN      Size
+  )
+{
+  UINTN ChecksumOffset;
+
+  ChecksumOffset = OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER, Checksum);
+
+  //
+  // Set checksum to 0 first.
+  //
+  Buffer[ChecksumOffset] = 0;
+
+  //
+  // Update checksum value.
+  //
+  Buffer[ChecksumOffset] = CalculateCheckSum8 (Buffer, Size);
+}
+
+/**
+  Allocate EfiBootServicesData below 4G memory address.
+
+  This function allocates EfiBootServicesData below 4G memory address.
+
+  @param[in]  Size   Size of memory to allocate.
+
+  @return Allocated address for output.
+
+**/
+VOID *
+BgrtAllocateBsDataMemoryBelow4G (
+  IN UINTN       Size
+  )
+{
+  UINTN                 Pages;
+  EFI_PHYSICAL_ADDRESS  Address;
+  EFI_STATUS            Status;
+  VOID                  *Buffer;
+
+  Pages   = EFI_SIZE_TO_PAGES (Size);
+  Address = 0xffffffff;
+
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiBootServicesData,
+                  Pages,
+                  &Address
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Buffer = (VOID *) (UINTN) Address;
+  ZeroMem (Buffer, Size);
+
+  return Buffer;
+}
+
+/**
+  Install Boot Graphics Resource Table to ACPI table.
+
+  @return Status code.
+
+**/
+EFI_STATUS
+InstallBootGraphicsResourceTable (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_ACPI_TABLE_PROTOCOL       *AcpiTableProtocol;
+  UINT8                         *ImageBuffer;
+  UINTN                         PaddingSize;
+  UINTN                         BmpSize;
+  UINTN                         OrigBmpSize;
+  UINT8                         *Image;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL *BltPixel;
+  UINTN                         Col;
+  UINTN                         Row;
+
+  //
+  // Get ACPI Table protocol.
+  //
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID **) &AcpiTableProtocol);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check whether Boot Graphics Resource Table is already installed.
+  //
+  if (mAcpiBgrtInstalled) {
+    if (!mAcpiBgrtStatusChanged && !mAcpiBgrtBufferChanged) {
+      //
+      // Nothing has changed
+      //
+      return EFI_SUCCESS;
+    } else {
+      //
+      // If BGRT data change happens. Uninstall Orignal AcpiTable first
+      //
+      Status = AcpiTableProtocol->UninstallAcpiTable (
+                                    AcpiTableProtocol,
+                                    mBootGraphicsResourceTableKey
+                                    );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      } 
+    }
+  } else {
+    //
+    // Check whether Logo exist.
+    //
+    if ( mLogoBltBuffer == NULL) {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  if (mAcpiBgrtBufferChanged) {
+    //
+    // reserve original BGRT buffer size
+    //
+    OrigBmpSize = mBmpImageHeaderTemplate.ImageSize + sizeof (BMP_IMAGE_HEADER);
+    //
+    // Free orignal BMP memory 
+    // 
+    if (mBootGraphicsResourceTableTemplate.ImageAddress) {
+      gBS->FreePages(mBootGraphicsResourceTableTemplate.ImageAddress, EFI_SIZE_TO_PAGES(OrigBmpSize));
+    }
+
+    //
+    // Allocate memory for BMP file.
+    //
+    PaddingSize = mLogoWidth & 0x3;
+
+    //
+    // First check mLogoWidth * 3 + PaddingSize doesn't overflow
+    //
+    if (mLogoWidth > (((UINT32) ~0) - PaddingSize) / 3 ) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Second check (mLogoWidth * 3 + PaddingSize) * mLogoHeight + sizeof (BMP_IMAGE_HEADER) doesn't overflow
+    //
+    if (mLogoHeight > (((UINT32) ~0) - sizeof (BMP_IMAGE_HEADER)) / (mLogoWidth * 3 + PaddingSize)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // The image should be stored in EfiBootServicesData, allowing the system to reclaim the memory
+    //
+    BmpSize = (mLogoWidth * 3 + PaddingSize) * mLogoHeight + sizeof (BMP_IMAGE_HEADER);
+    ImageBuffer = BgrtAllocateBsDataMemoryBelow4G (BmpSize);
+    if (ImageBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    mBmpImageHeaderTemplate.Size = (UINT32) BmpSize;
+    mBmpImageHeaderTemplate.ImageSize = (UINT32) BmpSize - sizeof (BMP_IMAGE_HEADER);
+    mBmpImageHeaderTemplate.PixelWidth = (UINT32) mLogoWidth;
+    mBmpImageHeaderTemplate.PixelHeight = (UINT32) mLogoHeight;
+    CopyMem (ImageBuffer, &mBmpImageHeaderTemplate, sizeof (BMP_IMAGE_HEADER));
+    
+    //
+    // Convert BLT buffer to BMP file.
+    //
+    Image = ImageBuffer + sizeof (BMP_IMAGE_HEADER);
+    for (Row = 0; Row < mLogoHeight; Row++) {
+    BltPixel = &mLogoBltBuffer[(mLogoHeight - Row - 1) * mLogoWidth];
+
+    for (Col = 0; Col < mLogoWidth; Col++) {
+      *Image++ = BltPixel->Blue;
+      *Image++ = BltPixel->Green;
+      *Image++ = BltPixel->Red;
+      BltPixel++;
+    }
+
+      //
+      // Padding for 4 byte alignment.
+      //
+      Image += PaddingSize;
+    }
+    FreePool (mLogoBltBuffer);
+    mLogoBltBuffer = NULL;
+
+    mBootGraphicsResourceTableTemplate.ImageAddress = (UINT64) (UINTN) ImageBuffer;
+    mBootGraphicsResourceTableTemplate.ImageOffsetX = (UINT32) mLogoDestX;
+    mBootGraphicsResourceTableTemplate.ImageOffsetY = (UINT32) mLogoDestY;
+  }
+
+  mBootGraphicsResourceTableTemplate.Status = (UINT8) (mIsLogoValid ? EFI_ACPI_5_0_BGRT_STATUS_VALID : EFI_ACPI_5_0_BGRT_STATUS_INVALID);
+
+  //
+  // Update Checksum.
+  //
+  BgrtAcpiTableChecksum ((UINT8 *) &mBootGraphicsResourceTableTemplate, sizeof (EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE));
+
+  //
+  // Publish Boot Graphics Resource Table.
+  //
+  Status = AcpiTableProtocol->InstallAcpiTable (
+                                AcpiTableProtocol,
+                                &mBootGraphicsResourceTableTemplate,
+                                sizeof (EFI_ACPI_5_0_BOOT_GRAPHICS_RESOURCE_TABLE),
+                                &mBootGraphicsResourceTableKey
+                                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  mAcpiBgrtInstalled = TRUE;
+  mAcpiBgrtStatusChanged = FALSE;
+  mAcpiBgrtBufferChanged = FALSE;
+  
+  return Status;
+}
+
+/**
+  Notify function for event group EFI_EVENT_GROUP_READY_TO_BOOT. This is used to
+  install the Boot Graphics Resource Table.
+
+  @param[in]  Event   The Event that is being processed.
+  @param[in]  Context The Event Context.
+
+**/
+VOID
+EFIAPI
+BgrtReadyToBootEventNotify (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  InstallBootGraphicsResourceTable ();
+}
+
+/**
+  The module Entry Point of the Boot Graphics Resource Table DXE driver.
+
+  @param[in]  ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+BootGraphicsDxeEntryPoint (
+  IN EFI_HANDLE          ImageHandle,
+  IN EFI_SYSTEM_TABLE    *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINT64      OemTableId;
+
+  CopyMem (
+    mBootGraphicsResourceTableTemplate.Header.OemId,
+    PcdGetPtr (PcdAcpiDefaultOemId),
+    sizeof (mBootGraphicsResourceTableTemplate.Header.OemId)
+    );
+  OemTableId = PcdGet64 (PcdAcpiDefaultOemTableId);
+  CopyMem (&mBootGraphicsResourceTableTemplate.Header.OemTableId, &OemTableId, sizeof (UINT64));
+  mBootGraphicsResourceTableTemplate.Header.OemRevision      = PcdGet32 (PcdAcpiDefaultOemRevision);
+  mBootGraphicsResourceTableTemplate.Header.CreatorId        = PcdGet32 (PcdAcpiDefaultCreatorId);
+  mBootGraphicsResourceTableTemplate.Header.CreatorRevision  = PcdGet32 (PcdAcpiDefaultCreatorRevision);
+
+  //
+  // Install Boot Logo protocol.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mBootLogoHandle,
+                  &gEfiBootLogoProtocolGuid,
+                  &mBootLogoProtocolTemplate,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register notify function to install BGRT on ReadyToBoot Event.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  BgrtReadyToBootEventNotify,
+                  NULL,
+                  &gEfiEventReadyToBootGuid,
+                  &mBootGraphicsReadyToBootEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.inf b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.inf
new file mode 100644
index 0000000000..9fd1da448c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.inf
@@ -0,0 +1,62 @@
+## @file
+#  This module install ACPI Boot Graphics Resource Table (BGRT).
+#
+#  Copyright (c) 2011 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BootGraphicsResourceTableDxe
+  MODULE_UNI_FILE                = BootGraphicsResourceTableDxe.uni
+  FILE_GUID                      = B8E62775-BB0A-43f0-A843-5BE8B14F8CCD
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = BootGraphicsDxeEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BootGraphicsResourceTableDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  DebugLib
+  PcdLib
+
+[Protocols]
+  gEfiAcpiTableProtocolGuid                     ## CONSUMES
+  gEfiBootLogoProtocolGuid                      ## PRODUCES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemId            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemTableId       ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorId        ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorRevision  ## CONSUMES
+
+[Guids]
+  gEfiEventReadyToBootGuid                      ## CONSUMES ## Event
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  BootGraphicsResourceTableDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.uni b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.uni
new file mode 100644
index 0000000000..c167cedcdc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxeExtra.uni b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxeExtra.uni
new file mode 100644
index 0000000000..9a3921d580
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
new file mode 100644
index 0000000000..c761f609b9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.inf
@@ -0,0 +1,90 @@
+## @file
+# Boot Script Executor Module
+#
+# This is a standalone Boot Script Executor. Standalone means it does not
+# depends on any PEI or DXE service.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BootScriptExecutorDxe
+  MODULE_UNI_FILE                = BootScriptExecutorDxe.uni
+  FILE_GUID                      = FA20568B-548B-4b2b-81EF-1BA08D4A3CEC
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = BootScriptExecutorEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  ScriptExecute.h
+  ScriptExecute.c
+
+[Sources.X64]
+  X64/SetIdtEntry.c  
+  X64/S3Asm.asm
+  X64/S3Asm.S
+  
+[Sources.Ia32]
+  IA32/SetIdtEntry.c   
+  IA32/S3Asm.asm
+  IA32/S3Asm.S
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PcdLib
+  BaseMemoryLib
+  UefiDriverEntryPoint
+  BaseLib
+  S3BootScriptLib
+  PeCoffLib
+  DxeServicesLib
+  UefiBootServicesTableLib
+  CacheMaintenanceLib
+  UefiLib
+  DebugAgentLib
+  LockBoxLib
+  CpuExceptionHandlerLib
+  DevicePathLib
+
+[Guids]
+  gEfiBootScriptExecutorVariableGuid    ## PRODUCES ## UNDEFINED # SaveLockBox
+  gEfiBootScriptExecutorContextGuid     ## PRODUCES ## UNDEFINED # SaveLockBox
+  gEdkiiMemoryProfileGuid               ## SOMETIMES_CONSUMES ## GUID # Locate protocol
+
+[Protocols]
+  ## NOTIFY
+  ## CONSUMES
+  gEfiDxeSmmReadyToLockProtocolGuid
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode  ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable              ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMemoryProfilePropertyMask   ## CONSUMES
+
+[Depex]
+  gEfiLockBoxProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  BootScriptExecutorDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.uni b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.uni
new file mode 100644
index 0000000000..21bf53cc6a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxeExtra.uni b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxeExtra.uni
new file mode 100644
index 0000000000..135270d842
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/BootScriptExecutorDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.S b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.S
new file mode 100644
index 0000000000..21516d5ad9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.S
@@ -0,0 +1,66 @@
+## @file
+#
+# Copyright (c) 2010, 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.
+#
+##
+
+#-----------------------------------------
+#VOID
+#AsmTransferControl (
+#  IN   UINT32           S3WakingVector,
+#  IN   UINT32           AcpiLowMemoryBase
+#  );
+#-----------------------------------------
+
+ASM_GLOBAL ASM_PFX(AsmTransferControl)
+ASM_PFX(AsmTransferControl):
+    # S3WakingVector    :DWORD
+    # AcpiLowMemoryBase :DWORD
+    pushl %ebp
+    movl  %esp,%ebp
+    leal  LABLE, %eax
+    pushl $0x28             # CS
+    pushl %eax
+    movl  8(%ebp),%ecx
+    shrdl $20,%ecx,%ebx
+    andl  $0xf,%ecx
+    movw  %cx,%bx
+    movl  %ebx, jmp_addr
+    lret
+LABLE: 
+    .byte 0xb8,0x30,0       # mov ax, 30h as selector
+    movw  %ax,%ds
+    movw  %ax,%es
+    movw  %ax,%fs
+    movw  %ax,%gs
+    movw  %ax,%ss
+    movl  %cr0, %eax        # Get control register 0  
+    .byte 0x66
+    .byte 0x83,0xe0,0xfe    # and    eax, 0fffffffeh  ; Clear PE bit (bit #0)
+    .byte 0xf,0x22,0xc0     # mov    cr0, eax         ; Activate real mode
+    .byte 0xea              # jmp far @jmp_addr
+jmp_addr: 
+    .long    0
+
+ASM_GLOBAL ASM_PFX(AsmTransferControl32)
+ASM_PFX(AsmTransferControl32):
+    jmp ASM_PFX(AsmTransferControl)
+
+# dummy
+ASM_GLOBAL ASM_PFX(AsmTransferControl16)
+ASM_PFX(AsmTransferControl16):
+ASM_GLOBAL ASM_PFX(AsmFixAddress16)
+ASM_PFX(AsmFixAddress16):
+    .long    0
+ASM_GLOBAL ASM_PFX(AsmJmpAddr32)
+ASM_PFX(AsmJmpAddr32):
+    .long    0
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.asm b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.asm
new file mode 100644
index 0000000000..710546e114
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/S3Asm.asm
@@ -0,0 +1,71 @@
+;; @file
+;   This is the assembly code for transferring to control to OS S3 waking vector
+;   for IA32 platform
+;
+; Copyright (c) 2006, 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.
+;
+;;
+    .586P
+    .model  flat,C
+    .code
+
+EXTERNDEF   AsmFixAddress16:DWORD
+EXTERNDEF   AsmJmpAddr32:DWORD
+   
+;-----------------------------------------
+;VOID
+;AsmTransferControl (
+;  IN   UINT32           S3WakingVector,
+;  IN   UINT32           AcpiLowMemoryBase
+;  );
+;-----------------------------------------
+   
+AsmTransferControl  PROC
+    ; S3WakingVector    :DWORD
+    ; AcpiLowMemoryBase :DWORD
+    push  ebp
+    mov   ebp, esp    
+    lea   eax, @F
+    push  28h               ; CS
+    push  eax
+    mov   ecx, [ebp + 8]
+    shrd  ebx, ecx, 20
+    and   ecx, 0fh          
+    mov   bx, cx          
+    mov   @jmp_addr, ebx
+    retf
+@@:
+    DB    0b8h, 30h, 0      ; mov ax, 30h as selector
+    mov   ds, ax
+    mov   es, ax
+    mov   fs, ax
+    mov   gs, ax
+    mov   ss, ax
+    mov   eax, cr0          ; Get control register 0  
+    DB    66h
+    DB    83h, 0e0h, 0feh   ; and    eax, 0fffffffeh  ; Clear PE bit (bit #0)
+    DB    0fh, 22h, 0c0h    ; mov    cr0, eax         ; Activate real mode
+    DB    0eah              ; jmp far @jmp_addr
+@jmp_addr DD  ?
+
+AsmTransferControl  ENDP
+
+AsmTransferControl32  PROC
+  jmp AsmTransferControl
+AsmTransferControl32  ENDP
+
+; dummy
+AsmTransferControl16  PROC
+AsmFixAddress16  DD ?
+AsmJmpAddr32 DD  ?
+AsmTransferControl16  ENDP
+
+    END
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c
new file mode 100644
index 0000000000..63b06ba5d6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/IA32/SetIdtEntry.c
@@ -0,0 +1,62 @@
+/** @file
+  Set a IDT entry for debug purpose
+
+  Set a IDT entry for interrupt vector 3 for debug purpose for IA32 platform
+
+Copyright (c) 2006 - 2013, 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 "ScriptExecute.h"
+
+/**
+  Set a IDT entry for interrupt vector 3 for debug purpose.
+
+  @param  AcpiS3Context  a pointer to a structure of ACPI_S3_CONTEXT
+
+**/
+VOID
+SetIdtEntry (
+  IN ACPI_S3_CONTEXT     *AcpiS3Context
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR                      *IdtEntry;
+  IA32_DESCRIPTOR                               *IdtDescriptor;
+  UINTN                                         S3DebugBuffer;
+  EFI_STATUS                                    Status;
+
+  //
+  // Restore IDT for debug
+  //
+  IdtDescriptor = (IA32_DESCRIPTOR *) (UINTN) (AcpiS3Context->IdtrProfile);
+  AsmWriteIdtr (IdtDescriptor);
+
+  //
+  // Setup the default CPU exception handlers
+  //
+  Status = InitializeCpuExceptionHandlers (NULL);
+  ASSERT_EFI_ERROR (Status);
+
+  DEBUG_CODE (
+    //
+    // Update IDT entry INT3 if the instruction is valid in it
+    //
+    S3DebugBuffer = (UINTN) (AcpiS3Context->S3DebugBufferAddress);
+    if (*(UINTN *)S3DebugBuffer != (UINTN) -1) {
+      IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *)(IdtDescriptor->Base + (3 * sizeof (IA32_IDT_GATE_DESCRIPTOR)));
+      IdtEntry->Bits.OffsetLow      = (UINT16)S3DebugBuffer;
+      IdtEntry->Bits.Selector       = (UINT16)AsmReadCs ();
+      IdtEntry->Bits.Reserved_0     = 0;
+      IdtEntry->Bits.GateType       = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+      IdtEntry->Bits.OffsetHigh     = (UINT16)(S3DebugBuffer >> 16);
+    }
+  );
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.c b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.c
new file mode 100644
index 0000000000..f8b2782244
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.c
@@ -0,0 +1,478 @@
+/** @file
+  This is the code for Boot Script Executer module.
+
+  This driver is dispatched by Dxe core and the driver will reload itself to ACPI reserved memory
+  in the entry point. The functionality is to interpret and restore the S3 boot script
+
+Copyright (c) 2006 - 2015, 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 "ScriptExecute.h"
+
+EFI_GUID              mBootScriptExecutorImageGuid = {
+  0x9a8d3433, 0x9fe8, 0x42b6, { 0x87, 0xb, 0x1e, 0x31, 0xc8, 0x4e, 0xbe, 0x3b }
+};
+
+BOOLEAN               mPage1GSupport = FALSE;
+
+/**
+  Entry function of Boot script exector. This function will be executed in
+  S3 boot path.
+  This function should not return, because it is invoked by switch stack.
+
+  @param  AcpiS3Context    a pointer to a structure of ACPI_S3_CONTEXT
+  @param  PeiS3ResumeState a pointer to a structure of PEI_S3_RESUME_STATE
+
+  @retval EFI_INVALID_PARAMETER - OS waking vector not found
+  @retval EFI_UNSUPPORTED - something wrong when we resume to OS
+**/
+EFI_STATUS
+EFIAPI
+S3BootScriptExecutorEntryFunction (
+  IN ACPI_S3_CONTEXT       *AcpiS3Context,
+  IN PEI_S3_RESUME_STATE   *PeiS3ResumeState
+  )
+{
+  EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs;
+  EFI_STATUS                                    Status;
+  UINTN                                         TempStackTop;
+  UINTN                                         TempStack[0x10];
+  UINTN                                         AsmTransferControl16Address;
+  IA32_DESCRIPTOR                               IdtDescriptor;
+
+  //
+  // Disable interrupt of Debug timer, since new IDT table cannot handle it.
+  //
+  SaveAndSetDebugTimerInterrupt (FALSE);
+
+  AsmReadIdtr (&IdtDescriptor);
+  //
+  // Restore IDT for debug
+  //
+  SetIdtEntry (AcpiS3Context);
+
+  //
+  // Initialize Debug Agent to support source level debug in S3 path, it will disable interrupt and Debug Timer.
+  //
+  InitializeDebugAgent (DEBUG_AGENT_INIT_S3, (VOID *)&IdtDescriptor, NULL);
+
+  //
+  // Because not install BootScriptExecute PPI(used just in this module), So just pass NULL
+  // for that parameter.
+  //
+  Status = S3BootScriptExecute ();
+  
+  //
+  // If invalid script table or opcode in S3 boot script table.
+  //
+  ASSERT_EFI_ERROR (Status);
+  
+  if (EFI_ERROR (Status)) {
+    CpuDeadLoop ();
+    return Status;
+  }
+
+  AsmWbinvd ();
+
+  //
+  // Get ACPI Table Address
+  //
+  Facs = (EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable));
+
+  //
+  // We need turn back to S3Resume - install boot script done ppi and report status code on S3resume.
+  //
+  if (PeiS3ResumeState != 0) {
+    //
+    // Need report status back to S3ResumePeim. 
+    // If boot script execution is failed, S3ResumePeim wil report the error status code.
+    //
+    PeiS3ResumeState->ReturnStatus = (UINT64)(UINTN)Status;
+    if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+      //
+      // X64 S3 Resume
+      //
+      DEBUG ((EFI_D_ERROR, "Call AsmDisablePaging64() to return to S3 Resume in PEI Phase\n"));
+      PeiS3ResumeState->AsmTransferControl = (EFI_PHYSICAL_ADDRESS)(UINTN)AsmTransferControl32;
+
+      if ((Facs != NULL) &&
+          (Facs->Signature == EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) &&
+          (Facs->FirmwareWakingVector != 0) ) {
+        //
+        // more step needed - because relative address is handled differently between X64 and IA32.
+        //
+        AsmTransferControl16Address = (UINTN)AsmTransferControl16;
+        AsmFixAddress16 = (UINT32)AsmTransferControl16Address;
+        AsmJmpAddr32 = (UINT32)((Facs->FirmwareWakingVector & 0xF) | ((Facs->FirmwareWakingVector & 0xFFFF0) << 12));
+      }
+
+      AsmDisablePaging64 (
+        PeiS3ResumeState->ReturnCs,
+        (UINT32)PeiS3ResumeState->ReturnEntryPoint,
+        (UINT32)(UINTN)AcpiS3Context,
+        (UINT32)(UINTN)PeiS3ResumeState,
+        (UINT32)PeiS3ResumeState->ReturnStackPointer
+        );
+    } else {
+      //
+      // IA32 S3 Resume
+      //
+      DEBUG ((EFI_D_ERROR, "Call SwitchStack() to return to S3 Resume in PEI Phase\n"));
+      PeiS3ResumeState->AsmTransferControl = (EFI_PHYSICAL_ADDRESS)(UINTN)AsmTransferControl;
+
+      SwitchStack (
+        (SWITCH_STACK_ENTRY_POINT)(UINTN)PeiS3ResumeState->ReturnEntryPoint,
+        (VOID *)(UINTN)AcpiS3Context,
+        (VOID *)(UINTN)PeiS3ResumeState,
+        (VOID *)(UINTN)PeiS3ResumeState->ReturnStackPointer
+        );
+    }
+
+    //
+    // Never run to here
+    //
+    CpuDeadLoop();
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // S3ResumePeim does not provide a way to jump back to itself, so resume to OS here directly
+  //
+  if (Facs->XFirmwareWakingVector != 0) {
+    //
+    // Switch to native waking vector
+    //
+    TempStackTop = (UINTN)&TempStack + sizeof(TempStack);
+    if ((Facs->Version == EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_VERSION) &&
+        ((Facs->Flags & EFI_ACPI_4_0_64BIT_WAKE_SUPPORTED_F) != 0) &&
+        ((Facs->Flags & EFI_ACPI_4_0_OSPM_64BIT_WAKE__F) != 0)) {
+      //
+      // X64 long mode waking vector
+      //
+      DEBUG (( EFI_D_ERROR, "Transfer to 64bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));
+      if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+        SwitchStack (
+          (SWITCH_STACK_ENTRY_POINT)(UINTN)Facs->XFirmwareWakingVector,
+          NULL,
+          NULL,
+          (VOID *)(UINTN)TempStackTop
+          );
+      } else {
+        // Unsupported for 32bit DXE, 64bit OS vector
+        DEBUG (( EFI_D_ERROR, "Unsupported for 32bit DXE transfer to 64bit OS waking vector!\r\n"));
+        ASSERT (FALSE);
+      }
+    } else {
+      //
+      // IA32 protected mode waking vector (Page disabled)
+      //
+      DEBUG (( EFI_D_ERROR, "Transfer to 32bit OS waking vector - %x\r\n", (UINTN)Facs->XFirmwareWakingVector));
+      if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+        AsmDisablePaging64 (
+          0x10,
+          (UINT32)Facs->XFirmwareWakingVector,
+          0,
+          0,
+          (UINT32)TempStackTop
+          );
+      } else {
+        SwitchStack (
+          (SWITCH_STACK_ENTRY_POINT)(UINTN)Facs->XFirmwareWakingVector,
+          NULL,
+          NULL,
+          (VOID *)(UINTN)TempStackTop
+          );
+      }
+    }
+  } else {
+    //
+    // 16bit Realmode waking vector
+    //
+    DEBUG (( EFI_D_ERROR, "Transfer to 16bit OS waking vector - %x\r\n", (UINTN)Facs->FirmwareWakingVector));
+    AsmTransferControl (Facs->FirmwareWakingVector, 0x0);
+  }
+
+  //
+  // Never run to here
+  //
+  CpuDeadLoop();
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Register image to memory profile.
+
+  @param FileName       File name of the image.
+  @param ImageBase      Image base address.
+  @param ImageSize      Image size.
+  @param FileType       File type of the image.
+
+**/
+VOID
+RegisterMemoryProfileImage (
+  IN EFI_GUID                       *FileName,
+  IN PHYSICAL_ADDRESS               ImageBase,
+  IN UINT64                         ImageSize,
+  IN EFI_FV_FILETYPE                FileType
+  )
+{
+  EFI_STATUS                        Status;
+  EDKII_MEMORY_PROFILE_PROTOCOL     *ProfileProtocol;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *FilePath;
+  UINT8                             TempBuffer[sizeof (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH) + sizeof (EFI_DEVICE_PATH_PROTOCOL)];
+
+  if ((PcdGet8 (PcdMemoryProfilePropertyMask) & BIT0) != 0) {
+
+    FilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *)TempBuffer;
+    Status = gBS->LocateProtocol (&gEdkiiMemoryProfileGuid, NULL, (VOID **) &ProfileProtocol);
+    if (!EFI_ERROR (Status)) {
+      EfiInitializeFwVolDevicepathNode (FilePath, FileName);
+      SetDevicePathEndNode (FilePath + 1);
+
+      Status = ProfileProtocol->RegisterImage (
+                                  ProfileProtocol,
+                                  (EFI_DEVICE_PATH_PROTOCOL *) FilePath,
+                                  ImageBase,
+                                  ImageSize,
+                                  FileType
+                                  );
+    }
+  }
+}
+
+/**
+  This is the Event notification function to reload BootScriptExecutor image
+  to RESERVED mem and save it to LockBox.
+  
+  @param    Event   Pointer to this event
+  @param    Context Event handler private data 
+ **/
+VOID
+EFIAPI
+ReadyToLockEventNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS                                    Status;
+  VOID                                          *Interface;
+  UINT8                                         *Buffer;
+  UINTN                                         BufferSize;
+  EFI_HANDLE                                    NewImageHandle;
+  UINTN                                         Pages;
+  EFI_PHYSICAL_ADDRESS                          FfsBuffer;
+  PE_COFF_LOADER_IMAGE_CONTEXT                  ImageContext;
+
+  Status = gBS->LocateProtocol (&gEfiDxeSmmReadyToLockProtocolGuid, NULL, &Interface);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // A workaround: Here we install a dummy handle
+  //
+  NewImageHandle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &NewImageHandle,
+                  &gEfiCallerIdGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Reload BootScriptExecutor image itself to RESERVED mem
+  //
+  Status = GetSectionFromAnyFv  (
+             &gEfiCallerIdGuid,
+             EFI_SECTION_PE32,
+             0,
+             (VOID **) &Buffer,
+             &BufferSize
+             );
+  ASSERT_EFI_ERROR (Status);
+  ImageContext.Handle    = Buffer;
+  ImageContext.ImageRead = PeCoffLoaderImageReadFromMemory;
+  //
+  // Get information about the image being loaded
+  //
+  Status = PeCoffLoaderGetImageInfo (&ImageContext);
+  ASSERT_EFI_ERROR (Status);
+  if (ImageContext.SectionAlignment > EFI_PAGE_SIZE) {
+    Pages = EFI_SIZE_TO_PAGES ((UINTN) (ImageContext.ImageSize + ImageContext.SectionAlignment));
+  } else {
+    Pages = EFI_SIZE_TO_PAGES ((UINTN) ImageContext.ImageSize);
+  }
+  FfsBuffer = 0xFFFFFFFF;
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  Pages,
+                  &FfsBuffer
+                  );
+  ASSERT_EFI_ERROR (Status);
+  ImageContext.ImageAddress = (PHYSICAL_ADDRESS)(UINTN)FfsBuffer;
+  //
+  // Align buffer on section boundry
+  //
+  ImageContext.ImageAddress += ImageContext.SectionAlignment - 1;
+  ImageContext.ImageAddress &= ~((EFI_PHYSICAL_ADDRESS)(ImageContext.SectionAlignment - 1));
+  //
+  // Load the image to our new buffer
+  //
+  Status = PeCoffLoaderLoadImage (&ImageContext);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Relocate the image in our new buffer
+  //
+  Status = PeCoffLoaderRelocateImage (&ImageContext);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Free the buffer allocated by ReadSection since the image has been relocated in the new buffer
+  //
+  gBS->FreePool (Buffer);
+
+  //
+  // Flush the instruction cache so the image data is written before we execute it
+  //
+  InvalidateInstructionCacheRange ((VOID *)(UINTN)ImageContext.ImageAddress, (UINTN)ImageContext.ImageSize);
+
+  RegisterMemoryProfileImage (
+    &gEfiCallerIdGuid,
+    ImageContext.ImageAddress,
+    ImageContext.ImageSize,
+    EFI_FV_FILETYPE_DRIVER
+    );
+
+  Status = ((EFI_IMAGE_ENTRY_POINT)(UINTN)(ImageContext.EntryPoint)) (NewImageHandle, gST);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Additional step for BootScript integrity
+  // Save BootScriptExecutor image
+  //
+  Status = SaveLockBox (
+             &mBootScriptExecutorImageGuid,
+             (VOID *)(UINTN)ImageContext.ImageAddress,
+             (UINTN)ImageContext.ImageSize
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SetLockBoxAttributes (&mBootScriptExecutorImageGuid, LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE);
+  ASSERT_EFI_ERROR (Status);
+
+  gBS->CloseEvent (Event);
+}
+
+/**
+  Entrypoint of Boot script exector driver, this function will be executed in
+  normal boot phase and invoked by DXE dispatch.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+**/
+EFI_STATUS
+EFIAPI
+BootScriptExecutorEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  UINTN                                         BufferSize;
+  UINTN                                         Pages;
+  BOOT_SCRIPT_EXECUTOR_VARIABLE                 *EfiBootScriptExecutorVariable;
+  EFI_PHYSICAL_ADDRESS                          BootScriptExecutorBuffer;
+  EFI_STATUS                                    Status;
+  VOID                                          *DevicePath;
+  EFI_EVENT                                     ReadyToLockEvent;
+  VOID                                          *Registration;
+  UINT32                                        RegEax;
+  UINT32                                        RegEdx;
+
+  //
+  // Test if the gEfiCallerIdGuid of this image is already installed. if not, the entry
+  // point is loaded by DXE code which is the first time loaded. or else, it is already
+  // be reloaded be itself.This is a work-around
+  //
+  Status = gBS->LocateProtocol (&gEfiCallerIdGuid, NULL, &DevicePath);
+  if (EFI_ERROR (Status)) {
+      //
+      // Create ReadyToLock event to reload BootScriptExecutor image
+      // to RESERVED mem and save it to LockBox.
+      //
+      ReadyToLockEvent = EfiCreateProtocolNotifyEvent  (
+                           &gEfiDxeSmmReadyToLockProtocolGuid,
+                           TPL_NOTIFY,
+                           ReadyToLockEventNotify,
+                           NULL,
+                           &Registration
+                           );
+      ASSERT (ReadyToLockEvent != NULL);
+    } else {
+      //
+      // the entry point is invoked after reloading. following code only run in RESERVED mem
+      //
+      if (PcdGetBool(PcdUse1GPageTable)) {
+        AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+        if (RegEax >= 0x80000001) {
+          AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+          if ((RegEdx & BIT26) != 0) {
+            mPage1GSupport = TRUE;
+          }
+        }
+      }
+
+      BufferSize = sizeof (BOOT_SCRIPT_EXECUTOR_VARIABLE);
+
+      BootScriptExecutorBuffer = 0xFFFFFFFF;
+      Pages = EFI_SIZE_TO_PAGES(BufferSize);
+      Status = gBS->AllocatePages (
+                      AllocateMaxAddress,
+                      EfiReservedMemoryType,
+                      Pages,
+                      &BootScriptExecutorBuffer
+                      );
+      ASSERT_EFI_ERROR (Status);
+
+      EfiBootScriptExecutorVariable = (BOOT_SCRIPT_EXECUTOR_VARIABLE *)(UINTN)BootScriptExecutorBuffer;
+      EfiBootScriptExecutorVariable->BootScriptExecutorEntrypoint = (UINTN) S3BootScriptExecutorEntryFunction ;
+
+      Status = SaveLockBox (
+                 &gEfiBootScriptExecutorVariableGuid,
+                 &BootScriptExecutorBuffer,
+                 sizeof(BootScriptExecutorBuffer)
+                 );
+      ASSERT_EFI_ERROR (Status);
+
+      //
+      // Additional step for BootScript integrity
+      // Save BootScriptExecutor context
+      //
+      Status = SaveLockBox (
+                 &gEfiBootScriptExecutorContextGuid,
+                 EfiBootScriptExecutorVariable,
+                 sizeof(*EfiBootScriptExecutorVariable)
+                 );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = SetLockBoxAttributes (&gEfiBootScriptExecutorContextGuid, LOCK_BOX_ATTRIBUTE_RESTORE_IN_PLACE);
+      ASSERT_EFI_ERROR (Status);
+    }
+
+    return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.h b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.h
new file mode 100644
index 0000000000..772347a57e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/ScriptExecute.h
@@ -0,0 +1,91 @@
+/** @file
+  The header file for Boot Script Executer module.
+  
+  This driver is dispatched by Dxe core and the driver will reload itself to ACPI reserved memory 
+  in the entry point. The functionality is to interpret and restore the S3 boot script 
+  
+Copyright (c) 2006 - 2014, 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.
+
+**/
+#ifndef _BOOT_SCRIPT_EXECUTOR_H_
+#define _BOOT_SCRIPT_EXECUTOR_H_
+
+#include <PiDxe.h>
+
+#include <Library/BaseLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/S3BootScriptLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/DxeServicesLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/PcdLib.h>
+#include <Library/CacheMaintenanceLib.h>
+#include <Library/TimerLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DebugAgentLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/CpuExceptionHandlerLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <Guid/AcpiS3Context.h>
+#include <Guid/BootScriptExecutorVariable.h>
+#include <Guid/MemoryProfile.h>
+
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <IndustryStandard/Acpi.h>
+/**
+  a ASM function to transfer control to OS.
+  
+  @param  S3WakingVector  The S3 waking up vector saved in ACPI Facs table
+  @param  AcpiLowMemoryBase a buffer under 1M which could be used during the transfer             
+**/
+VOID
+AsmTransferControl (
+  IN   UINT32           S3WakingVector,
+  IN   UINT32           AcpiLowMemoryBase
+  );
+/**
+  a 32bit ASM function to transfer control to OS.
+  
+  @param  S3WakingVector  The S3 waking up vector saved in ACPI Facs table
+  @param  AcpiLowMemoryBase a buffer under 1M which could be used during the transfer             
+**/
+VOID
+AsmTransferControl32 (
+  IN   UINT32           S3WakingVector,
+  IN   UINT32           AcpiLowMemoryBase
+  );
+/**
+  a 16bit ASM function to transfer control to OS.
+**/
+VOID
+AsmTransferControl16 (
+  VOID
+  );
+/**
+  Set a IDT entry for interrupt vector 3 for debug purpose.  
+  
+  @param  AcpiS3Context  a pointer to a structure of ACPI_S3_CONTEXT  
+              
+**/
+VOID
+SetIdtEntry ( 
+  IN ACPI_S3_CONTEXT     *AcpiS3Context
+  );
+
+extern UINT32 AsmFixAddress16;
+extern UINT32 AsmJmpAddr32;
+extern BOOLEAN mPage1GSupport;
+
+#endif //_BOOT_SCRIPT_EXECUTOR_H_
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S
new file mode 100644
index 0000000000..e59fd048b0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.S
@@ -0,0 +1,130 @@
+## @file
+#   This is the assembly code for transferring to control to OS S3 waking vector
+#   for X64 platform
+#
+# Copyright (c) 2006 - 2013, 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.
+#
+##
+
+ASM_GLOBAL ASM_PFX(AsmTransferControl)
+ASM_PFX(AsmTransferControl):
+    # rcx S3WakingVector    :DWORD
+    # rdx AcpiLowMemoryBase :DWORD
+    lea   _AsmTransferControl_al_0000(%rip), %eax 
+    movq  $0x2800000000, %r8 
+    orq   %r8, %rax
+    pushq %rax
+    shrd  $20, %ecx, %ebx
+    andl  $0x0f, %ecx 
+    movw  %cx, %bx
+    movl  %ebx, jmp_addr(%rip) 
+    lret
+_AsmTransferControl_al_0000:
+    .byte    0x0b8, 0x30, 0      # mov ax, 30h as selector
+    movl  %eax, %ds
+    movl  %eax, %es
+    movl  %eax, %fs
+    movl  %eax, %gs
+    movl  %eax, %ss
+    movq  %cr0, %rax
+    movq  %cr4, %rbx
+    .byte    0x66
+    andl  $0x7ffffffe, %eax 
+    andb  $0xdf, %bl 
+    movq  %rax, %cr0
+    .byte    0x66
+    movl  $0x0c0000080, %ecx 
+    rdmsr
+    andb  $0xfe, %ah 
+    wrmsr
+    movq  %rbx, %cr4
+    .byte    0x0ea              # jmp far jmp_addr
+jmp_addr:
+    .long    0
+
+ASM_GLOBAL ASM_PFX(AsmTransferControl32)
+ASM_PFX(AsmTransferControl32):
+    # S3WakingVector    :DWORD
+    # AcpiLowMemoryBase :DWORD
+    pushq %rbp
+    movl  %esp,%ebp
+    .byte 0x8d, 0x05        #  lea   eax, AsmTransferControl16
+ASM_GLOBAL ASM_PFX(AsmFixAddress16)
+ASM_PFX(AsmFixAddress16):
+    .long    0
+    pushq $0x28             # CS
+    pushq %rax
+    lret
+
+ASM_GLOBAL ASM_PFX(AsmTransferControl16)
+ASM_PFX(AsmTransferControl16):
+    .byte 0xb8,0x30,0       # mov ax, 30h as selector
+    movw  %ax,%ds
+    movw  %ax,%es
+    movw  %ax,%fs
+    movw  %ax,%gs
+    movw  %ax,%ss
+    movq  %cr0, %rax        # Get control register 0  
+    .byte 0x66
+    .byte 0x83,0xe0,0xfe    # and    eax, 0fffffffeh  ; Clear PE bit (bit #0)
+    .byte 0xf,0x22,0xc0     # mov    cr0, eax         ; Activate real mode
+    .byte 0xea              # jmp far AsmJmpAddr32
+ASM_GLOBAL ASM_PFX(AsmJmpAddr32)
+ASM_PFX(AsmJmpAddr32):
+    .long    0
+
+ASM_GLOBAL ASM_PFX(PageFaultHandlerHook)
+ASM_PFX(PageFaultHandlerHook):
+    pushq    %rax                         # save all volatile registers
+    pushq    %rcx
+    pushq    %rdx
+    pushq    %r8
+    pushq    %r9
+    pushq    %r10
+    pushq    %r11
+    # save volatile fp registers
+    addq     $-0x68, %rsp
+    stmxcsr  0x60(%rsp)
+    movdqa   %xmm0, 0x0(%rsp) 
+    movdqa   %xmm1, 0x10(%rsp) 
+    movdqa   %xmm2, 0x20(%rsp) 
+    movdqa   %xmm3, 0x30(%rsp) 
+    movdqa   %xmm4, 0x40(%rsp) 
+    movdqa   %xmm5, 0x50(%rsp) 
+
+    addq     $-0x20, %rsp
+    call     ASM_PFX(PageFaultHandler)
+    addq     $0x20, %rsp
+
+    # load volatile fp registers
+    ldmxcsr  0x60(%rsp)
+    movdqa   0x0(%rsp), %xmm0
+    movdqa   0x10(%rsp), %xmm1
+    movdqa   0x20(%rsp), %xmm2
+    movdqa   0x30(%rsp), %xmm3
+    movdqa   0x40(%rsp), %xmm4
+    movdqa   0x50(%rsp), %xmm5
+    addq     $0x68, %rsp
+
+    testb    %al, %al
+
+    popq     %r11
+    popq     %r10
+    popq     %r9
+    popq     %r8
+    popq     %rdx
+    popq     %rcx
+    popq     %rax                         # restore all volatile registers
+    jnz      L1
+    jmpq     *ASM_PFX(mOriginalHandler)(%rip)
+L1:
+    addq     $0x08, %rsp                  # skip error code for PF
+    iretq
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm
new file mode 100644
index 0000000000..0b7432daf7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/S3Asm.asm
@@ -0,0 +1,135 @@
+;; @file
+;   This is the assembly code for transferring to control to OS S3 waking vector
+;   for X64 platform
+;
+; Copyright (c) 2006 - 2012, 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.
+;
+;;
+
+EXTERN mOriginalHandler:QWORD
+EXTERN PageFaultHandler:PROC
+
+    .code
+    
+EXTERNDEF   AsmFixAddress16:DWORD
+EXTERNDEF   AsmJmpAddr32:DWORD
+    
+AsmTransferControl  PROC
+    ; rcx S3WakingVector    :DWORD
+    ; rdx AcpiLowMemoryBase :DWORD
+    lea   eax, @F
+    mov   r8, 2800000000h
+    or    rax, r8
+    push  rax
+    shrd  ebx, ecx, 20
+    and   ecx, 0fh          
+    mov   bx, cx          
+    mov   @jmp_addr, ebx
+    retf
+@@:
+    DB    0b8h, 30h, 0      ; mov ax, 30h as selector
+    mov   ds, eax
+    mov   es, eax
+    mov   fs, eax
+    mov   gs, eax
+    mov   ss, eax
+    mov   rax, cr0
+    mov   rbx, cr4        
+    DB    66h
+    and   eax, ((NOT 080000001h) AND 0ffffffffh)
+    and   bl, NOT (1 SHL 5)
+    mov   cr0, rax
+    DB    66h
+    mov   ecx, 0c0000080h
+    rdmsr
+    and   ah, NOT 1
+    wrmsr
+    mov   cr4, rbx
+    DB    0eah              ; jmp far @jmp_addr
+@jmp_addr DD  ?
+AsmTransferControl  ENDP
+
+AsmTransferControl32  PROC
+    ; S3WakingVector    :DWORD
+    ; AcpiLowMemoryBase :DWORD
+    push  rbp
+    mov   ebp, esp    
+    DB    8dh, 05h          ;  lea   eax, AsmTransferControl16
+AsmFixAddress16  DD ?
+    push  28h               ; CS
+    push  rax
+    retf
+AsmTransferControl32  ENDP
+
+AsmTransferControl16  PROC
+    DB    0b8h, 30h, 0      ; mov ax, 30h as selector
+    mov   ds, ax
+    mov   es, ax
+    mov   fs, ax
+    mov   gs, ax
+    mov   ss, ax
+    mov   rax, cr0          ; Get control register 0  
+    DB    66h
+    DB    83h, 0e0h, 0feh   ; and    eax, 0fffffffeh  ; Clear PE bit (bit #0)
+    DB    0fh, 22h, 0c0h    ; mov    cr0, eax         ; Activate real mode
+    DB    0eah              ; jmp far AsmJmpAddr32
+AsmJmpAddr32 DD  ?
+AsmTransferControl16  ENDP
+
+PageFaultHandlerHook PROC
+    push    rax                         ; save all volatile registers
+    push    rcx
+    push    rdx
+    push    r8
+    push    r9
+    push    r10
+    push    r11
+    ; save volatile fp registers
+    add     rsp, -68h
+    stmxcsr [rsp + 60h]
+    movdqa  [rsp + 0h], xmm0
+    movdqa  [rsp + 10h], xmm1
+    movdqa  [rsp + 20h], xmm2
+    movdqa  [rsp + 30h], xmm3
+    movdqa  [rsp + 40h], xmm4
+    movdqa  [rsp + 50h], xmm5
+
+    add     rsp, -20h
+    call    PageFaultHandler
+    add     rsp, 20h
+    
+    ; load volatile fp registers
+    ldmxcsr [rsp + 60h]
+    movdqa  xmm0,  [rsp + 0h]
+    movdqa  xmm1,  [rsp + 10h]
+    movdqa  xmm2,  [rsp + 20h]
+    movdqa  xmm3,  [rsp + 30h]
+    movdqa  xmm4,  [rsp + 40h]
+    movdqa  xmm5,  [rsp + 50h]
+    add     rsp, 68h
+
+    test    al, al
+    
+    pop     r11
+    pop     r10
+    pop     r9
+    pop     r8
+    pop     rdx
+    pop     rcx
+    pop     rax                         ; restore all volatile registers
+    jnz     @F
+    jmp     mOriginalHandler
+@@:
+    add     rsp, 08h                    ; skip error code for PF
+    iretq
+PageFaultHandlerHook ENDP
+
+    END
diff --git a/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c
new file mode 100644
index 0000000000..6674560597
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/BootScriptExecutorDxe/X64/SetIdtEntry.c
@@ -0,0 +1,264 @@
+/** @file
+  Set a IDT entry for debug purpose
+
+  Set a IDT entry for interrupt vector 3 for debug purpose for x64 platform
+
+Copyright (c) 2006 - 2015, 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 "ScriptExecute.h"
+
+//
+// 8 extra pages for PF handler.
+//
+#define EXTRA_PAGE_TABLE_PAGES      8
+
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+#define IA32_PG_PS                  BIT7
+
+UINT64                              mPhyMask;
+VOID                                *mOriginalHandler;
+UINTN                               mPageFaultBuffer;
+UINTN                               mPageFaultIndex = 0;
+//
+// Store the uplink information for each page being used.
+//
+UINT64                              *mPageFaultUplink[EXTRA_PAGE_TABLE_PAGES];
+
+/**
+  Page fault handler.
+
+**/
+VOID
+EFIAPI
+PageFaultHandlerHook (
+  VOID
+  );
+
+/**
+  Hook IDT with our page fault handler so that the on-demand paging works on page fault.
+
+  @param  IdtEntry  a pointer to IDT entry
+
+**/
+VOID
+HookPageFaultHandler (
+  IN IA32_IDT_GATE_DESCRIPTOR                   *IdtEntry
+  )
+{
+  UINT32         RegEax;
+  UINT8          PhysicalAddressBits;
+  UINTN          PageFaultHandlerHookAddress;
+
+  AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= 0x80000008) {
+    AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
+    PhysicalAddressBits = (UINT8) RegEax;
+  } else {
+    PhysicalAddressBits = 36;
+  }
+  mPhyMask = LShiftU64 (1, PhysicalAddressBits) - 1;
+  mPhyMask &= (1ull << 48) - SIZE_4KB;
+
+  //
+  // Set Page Fault entry to catch >4G access
+  //
+  PageFaultHandlerHookAddress = (UINTN)PageFaultHandlerHook;
+  mOriginalHandler = (VOID *)(UINTN)(LShiftU64 (IdtEntry->Bits.OffsetUpper, 32) + IdtEntry->Bits.OffsetLow + (IdtEntry->Bits.OffsetHigh << 16));
+  IdtEntry->Bits.OffsetLow      = (UINT16)PageFaultHandlerHookAddress;
+  IdtEntry->Bits.Selector       = (UINT16)AsmReadCs ();
+  IdtEntry->Bits.Reserved_0     = 0;
+  IdtEntry->Bits.GateType       = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+  IdtEntry->Bits.OffsetHigh     = (UINT16)(PageFaultHandlerHookAddress >> 16);
+  IdtEntry->Bits.OffsetUpper    = (UINT32)(PageFaultHandlerHookAddress >> 32);
+  IdtEntry->Bits.Reserved_1     = 0;
+
+  if (mPage1GSupport) {
+    mPageFaultBuffer = (UINTN)(AsmReadCr3 () & mPhyMask) + EFI_PAGES_TO_SIZE(2);
+  }else {
+    mPageFaultBuffer = (UINTN)(AsmReadCr3 () & mPhyMask) + EFI_PAGES_TO_SIZE(6);
+  }
+  ZeroMem (mPageFaultUplink, sizeof (mPageFaultUplink));
+}
+
+/**
+  The function will check if current waking vector is long mode.
+
+  @param  AcpiS3Context                 a pointer to a structure of ACPI_S3_CONTEXT
+
+  @retval TRUE   Current context need long mode waking vector.
+  @retval FALSE  Current context need not long mode waking vector.
+**/
+BOOLEAN
+IsLongModeWakingVector (
+  IN ACPI_S3_CONTEXT                *AcpiS3Context
+  )
+{
+  EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE  *Facs;
+
+  Facs = (EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE *) ((UINTN) (AcpiS3Context->AcpiFacsTable));
+  if ((Facs == NULL) ||
+      (Facs->Signature != EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_SIGNATURE) ||
+      ((Facs->FirmwareWakingVector == 0) && (Facs->XFirmwareWakingVector == 0)) ) {
+    // Something wrong with FACS
+    return FALSE;
+  }
+  if (Facs->XFirmwareWakingVector != 0) {
+    if ((Facs->Version == EFI_ACPI_4_0_FIRMWARE_ACPI_CONTROL_STRUCTURE_VERSION) &&
+        ((Facs->Flags & EFI_ACPI_4_0_64BIT_WAKE_SUPPORTED_F) != 0) &&
+        ((Facs->Flags & EFI_ACPI_4_0_OSPM_64BIT_WAKE__F) != 0)) {
+      // Both BIOS and OS wants 64bit vector
+      if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+        return TRUE;
+      }
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Set a IDT entry for interrupt vector 3 for debug purpose.
+
+  @param  AcpiS3Context  a pointer to a structure of ACPI_S3_CONTEXT
+
+**/
+VOID
+SetIdtEntry (
+  IN ACPI_S3_CONTEXT     *AcpiS3Context
+  )
+{
+  IA32_IDT_GATE_DESCRIPTOR                      *IdtEntry;
+  IA32_DESCRIPTOR                               *IdtDescriptor;
+  UINTN                                         S3DebugBuffer;
+  EFI_STATUS                                    Status;
+
+  //
+  // Restore IDT for debug
+  //
+  IdtDescriptor = (IA32_DESCRIPTOR *) (UINTN) (AcpiS3Context->IdtrProfile);
+  AsmWriteIdtr (IdtDescriptor);
+
+  //
+  // Setup the default CPU exception handlers
+  //
+  Status = InitializeCpuExceptionHandlers (NULL);
+  ASSERT_EFI_ERROR (Status);
+
+  DEBUG_CODE (
+    //
+    // Update IDT entry INT3 if the instruction is valid in it
+    //
+    S3DebugBuffer = (UINTN) (AcpiS3Context->S3DebugBufferAddress);
+    if (*(UINTN *)S3DebugBuffer != (UINTN) -1) {
+      IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *)(IdtDescriptor->Base + (3 * sizeof (IA32_IDT_GATE_DESCRIPTOR)));
+      IdtEntry->Bits.OffsetLow      = (UINT16)S3DebugBuffer;
+      IdtEntry->Bits.Selector       = (UINT16)AsmReadCs ();
+      IdtEntry->Bits.Reserved_0     = 0;
+      IdtEntry->Bits.GateType       = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+      IdtEntry->Bits.OffsetHigh     = (UINT16)(S3DebugBuffer >> 16);
+      IdtEntry->Bits.OffsetUpper    = (UINT32)(S3DebugBuffer >> 32);
+      IdtEntry->Bits.Reserved_1     = 0;
+    }
+  );
+
+  //
+  // If both BIOS and OS wants long mode waking vector,
+  // S3ResumePei should have established 1:1 Virtual to Physical identity mapping page table,
+  // no need to hook page fault handler.
+  //
+  if (!IsLongModeWakingVector (AcpiS3Context)) {
+    IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *)(IdtDescriptor->Base + (14 * sizeof (IA32_IDT_GATE_DESCRIPTOR)));
+    HookPageFaultHandler (IdtEntry);
+  }
+}
+
+/**
+  Acquire page for page fault.
+
+  @param[in, out] Uplink        Pointer to up page table entry.
+
+**/
+VOID
+AcquirePage (
+  IN OUT UINT64                 *Uplink
+  )
+{
+  UINTN             Address;
+
+  Address = mPageFaultBuffer + EFI_PAGES_TO_SIZE (mPageFaultIndex);
+  ZeroMem ((VOID *) Address, EFI_PAGES_TO_SIZE (1));
+
+  //
+  // Cut the previous uplink if it exists and wasn't overwritten.
+  //
+  if ((mPageFaultUplink[mPageFaultIndex] != NULL) && ((*mPageFaultUplink[mPageFaultIndex] & mPhyMask) == Address)) {
+    *mPageFaultUplink[mPageFaultIndex] = 0;
+  }
+
+  //
+  // Link & Record the current uplink.
+  //
+  *Uplink = Address | IA32_PG_P | IA32_PG_RW;
+  mPageFaultUplink[mPageFaultIndex] = Uplink;
+
+  mPageFaultIndex = (mPageFaultIndex + 1) % EXTRA_PAGE_TABLE_PAGES;
+}
+
+/**
+  The page fault handler that on-demand read >4G memory/MMIO.
+  
+  @retval TRUE     The page fault is correctly handled.
+  @retval FALSE    The page fault is not handled and is passed through to original handler.
+
+**/
+BOOLEAN
+EFIAPI
+PageFaultHandler (
+  VOID
+  )
+{
+  UINT64         *PageTable;
+  UINT64         PFAddress;
+  UINTN          PTIndex;
+
+  PFAddress = AsmReadCr2 ();
+  DEBUG ((EFI_D_ERROR, "BootScript - PageFaultHandler: Cr2 - %lx\n", PFAddress));
+
+  if (PFAddress >= mPhyMask + SIZE_4KB) {
+    return FALSE;
+  }
+  PFAddress &= mPhyMask;
+
+  PageTable = (UINT64*)(UINTN)(AsmReadCr3 () & mPhyMask);
+
+  PTIndex = BitFieldRead64 (PFAddress, 39, 47);
+  // PML4E
+  if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+    AcquirePage (&PageTable[PTIndex]);
+  }
+  PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & mPhyMask);
+  PTIndex = BitFieldRead64 (PFAddress, 30, 38);
+  // PDPTE
+  if (mPage1GSupport) {
+    PageTable[PTIndex] = (PFAddress & ~((1ull << 30) - 1)) | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  } else {
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      AcquirePage (&PageTable[PTIndex]);
+    }
+    PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & mPhyMask);
+    PTIndex = BitFieldRead64 (PFAddress, 21, 29);
+    // PD
+    PageTable[PTIndex] = (PFAddress & ~((1ull << 21) - 1)) | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  }
+
+  return TRUE;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.c b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.c
new file mode 100644
index 0000000000..6f6ea073aa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.c
@@ -0,0 +1,909 @@
+/** @file
+  This module install ACPI Firmware Performance Data Table (FPDT).
+
+  This module register report status code listener to collect performance data
+  for Firmware Basic Boot Performance Record and other boot performance records, 
+  and install FPDT to ACPI table.
+
+  Copyright (c) 2011 - 2016, 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 <PiDxe.h>
+
+#include <Protocol/ReportStatusCodeHandler.h>
+#include <Protocol/AcpiTable.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/LockBox.h>
+#include <Protocol/Variable.h>
+
+#include <Guid/Acpi.h>
+#include <Guid/FirmwarePerformance.h>
+#include <Guid/EventGroup.h>
+#include <Guid/EventLegacyBios.h>
+#include <Guid/PiSmmCommunicationRegionTable.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/UefiLib.h>
+
+#define EXTENSION_RECORD_SIZE     0x10000
+#define SMM_BOOT_RECORD_COMM_SIZE OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data) + sizeof(SMM_BOOT_RECORD_COMMUNICATE)
+
+EFI_RSC_HANDLER_PROTOCOL    *mRscHandlerProtocol = NULL;
+
+BOOLEAN                     mLockBoxReady = FALSE;
+EFI_EVENT                   mReadyToBootEvent;
+EFI_EVENT                   mLegacyBootEvent;
+EFI_EVENT                   mExitBootServicesEvent;
+UINTN                       mFirmwarePerformanceTableTemplateKey  = 0;
+UINT32                      mBootRecordSize = 0;
+UINT32                      mBootRecordMaxSize = 0;
+UINT8                       *mBootRecordBuffer = NULL;
+BOOLEAN                     mDxeCoreReportStatusCodeEnable = FALSE;
+
+BOOT_PERFORMANCE_TABLE                      *mAcpiBootPerformanceTable = NULL;
+S3_PERFORMANCE_TABLE                        *mAcpiS3PerformanceTable   = NULL;
+
+FIRMWARE_PERFORMANCE_TABLE  mFirmwarePerformanceTableTemplate = {
+  {
+    EFI_ACPI_5_0_FIRMWARE_PERFORMANCE_DATA_TABLE_SIGNATURE,
+    sizeof (FIRMWARE_PERFORMANCE_TABLE),
+    EFI_ACPI_5_0_FIRMWARE_PERFORMANCE_DATA_TABLE_REVISION,    // Revision
+    0x00, // Checksum will be updated at runtime
+    //
+    // It is expected that these values will be updated at EntryPoint.
+    //
+    {0x00},     // OEM ID is a 6 bytes long field
+    0x00,       // OEM Table ID(8 bytes long)
+    0x00,       // OEM Revision
+    0x00,       // Creator ID
+    0x00,       // Creator Revision
+  },
+  //
+  // Firmware Basic Boot Performance Table Pointer Record.
+  //
+  {
+    {
+      EFI_ACPI_5_0_FPDT_RECORD_TYPE_FIRMWARE_BASIC_BOOT_POINTER ,       // Type
+      sizeof (EFI_ACPI_5_0_FPDT_BOOT_PERFORMANCE_TABLE_POINTER_RECORD), // Length
+      EFI_ACPI_5_0_FPDT_RECORD_REVISION_FIRMWARE_BASIC_BOOT_POINTER     // Revision
+    },
+    0,  // Reserved
+    0   // BootPerformanceTablePointer will be updated at runtime.
+  },
+  //
+  // S3 Performance Table Pointer Record.
+  //
+  {
+    {
+      EFI_ACPI_5_0_FPDT_RECORD_TYPE_S3_PERFORMANCE_TABLE_POINTER,     // Type
+      sizeof (EFI_ACPI_5_0_FPDT_S3_PERFORMANCE_TABLE_POINTER_RECORD), // Length
+      EFI_ACPI_5_0_FPDT_RECORD_REVISION_S3_PERFORMANCE_TABLE_POINTER  // Revision
+    },
+    0,  // Reserved
+    0   // S3PerformanceTablePointer will be updated at runtime.
+  }
+};
+
+BOOT_PERFORMANCE_TABLE mBootPerformanceTableTemplate = {
+  {
+    EFI_ACPI_5_0_FPDT_BOOT_PERFORMANCE_TABLE_SIGNATURE,
+    sizeof (BOOT_PERFORMANCE_TABLE)
+  },
+  {
+    {
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_TYPE_FIRMWARE_BASIC_BOOT,    // Type
+      sizeof (EFI_ACPI_5_0_FPDT_FIRMWARE_BASIC_BOOT_RECORD),        // Length
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_REVISION_FIRMWARE_BASIC_BOOT // Revision
+    },
+    0,  // Reserved
+    //
+    // These values will be updated at runtime.
+    //
+    0,  // ResetEnd
+    0,  // OsLoaderLoadImageStart
+    0,  // OsLoaderStartImageStart
+    0,  // ExitBootServicesEntry
+    0   // ExitBootServicesExit
+  }
+};
+
+S3_PERFORMANCE_TABLE        mS3PerformanceTableTemplate = {
+  {
+    EFI_ACPI_5_0_FPDT_S3_PERFORMANCE_TABLE_SIGNATURE,
+    sizeof (S3_PERFORMANCE_TABLE)
+  },
+  {
+    {
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_TYPE_S3_RESUME,     // Type
+      sizeof (EFI_ACPI_5_0_FPDT_S3_RESUME_RECORD),         // Length
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_REVISION_S3_RESUME  // Revision
+    },
+    //
+    // These values will be updated by Firmware Performance PEIM.
+    //
+    0,  // ResumeCount
+    0,  // FullResume
+    0   // AverageResume
+  },
+  {
+    {
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_TYPE_S3_SUSPEND,    // Type
+      sizeof (EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD),        // Length
+      EFI_ACPI_5_0_FPDT_RUNTIME_RECORD_REVISION_S3_SUSPEND // Revision
+    },
+    //
+    // These values will be updated bye Firmware Performance SMM driver.
+    //
+    0,  // SuspendStart
+    0   // SuspendEnd
+  }
+};
+
+/**
+  This function calculates and updates an UINT8 checksum.
+
+  @param[in]  Buffer          Pointer to buffer to checksum
+  @param[in]  Size            Number of bytes to checksum
+
+**/
+VOID
+FpdtAcpiTableChecksum (
+  IN UINT8      *Buffer,
+  IN UINTN      Size
+  )
+{
+  UINTN ChecksumOffset;
+
+  ChecksumOffset = OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER, Checksum);
+
+  //
+  // Set checksum to 0 first.
+  //
+  Buffer[ChecksumOffset] = 0;
+
+  //
+  // Update checksum value.
+  //
+  Buffer[ChecksumOffset] = CalculateCheckSum8 (Buffer, Size);
+}
+
+/**
+  Allocate EfiReservedMemoryType below 4G memory address.
+
+  This function allocates EfiReservedMemoryType below 4G memory address.
+
+  @param[in]  Size   Size of memory to allocate.
+
+  @return Allocated address for output.
+
+**/
+VOID *
+FpdtAllocateReservedMemoryBelow4G (
+  IN UINTN       Size
+  )
+{
+  UINTN                 Pages;
+  EFI_PHYSICAL_ADDRESS  Address;
+  EFI_STATUS            Status;
+  VOID                  *Buffer;
+
+  Buffer  = NULL;
+  Pages   = EFI_SIZE_TO_PAGES (Size);
+  Address = 0xffffffff;
+
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  Pages,
+                  &Address
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  if (!EFI_ERROR (Status)) {
+    Buffer = (VOID *) (UINTN) Address;
+    ZeroMem (Buffer, Size);
+  }
+
+  return Buffer;
+}
+
+/**
+  Callback function upon VariableArchProtocol and LockBoxProtocol
+  to allocate S3 performance table memory and save the pointer to LockBox.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+**/
+VOID
+EFIAPI
+FpdtAllocateS3PerformanceTableMemory (
+  IN  EFI_EVENT                             Event,
+  IN  VOID                                  *Context
+  )
+{
+  EFI_STATUS                    Status;
+  VOID                          *Interface;
+  FIRMWARE_PERFORMANCE_VARIABLE PerformanceVariable;
+  UINTN                         Size;
+  EFI_PHYSICAL_ADDRESS          S3PerformanceTablePointer;
+
+  if (mLockBoxReady && (mAcpiS3PerformanceTable != NULL)) {
+    //
+    // The memory for S3 performance table should have been ready,
+    // and the pointer should have been saved to LockBox, just return.
+    //
+    return;
+  }
+
+  if (!mLockBoxReady) {
+    Status = gBS->LocateProtocol (&gEfiLockBoxProtocolGuid, NULL, &Interface);
+    if (!EFI_ERROR (Status)) {
+      //
+      // LockBox services has been ready.
+      //
+      mLockBoxReady = TRUE;
+    }
+  }
+
+  if (mAcpiS3PerformanceTable == NULL) {
+    Status = gBS->LocateProtocol (&gEfiVariableArchProtocolGuid, NULL, &Interface);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Try to allocate the same runtime buffer as last time boot.
+      //
+      ZeroMem (&PerformanceVariable, sizeof (PerformanceVariable));
+      Size = sizeof (PerformanceVariable);
+      Status = gRT->GetVariable (
+                      EFI_FIRMWARE_PERFORMANCE_VARIABLE_NAME,
+                      &gEfiFirmwarePerformanceGuid,
+                      NULL,
+                      &Size,
+                      &PerformanceVariable
+                      );
+      if (!EFI_ERROR (Status)) {
+        Status = gBS->AllocatePages (
+                        AllocateAddress,
+                        EfiReservedMemoryType,
+                        EFI_SIZE_TO_PAGES (sizeof (S3_PERFORMANCE_TABLE)),
+                        &PerformanceVariable.S3PerformanceTablePointer
+                        );
+        if (!EFI_ERROR (Status)) {
+          mAcpiS3PerformanceTable = (S3_PERFORMANCE_TABLE *) (UINTN) PerformanceVariable.S3PerformanceTablePointer;
+        }
+      }
+      if (mAcpiS3PerformanceTable == NULL) {
+        //
+        // Fail to allocate at specified address, continue to allocate at any address.
+        //
+        mAcpiS3PerformanceTable = (S3_PERFORMANCE_TABLE *) FpdtAllocateReservedMemoryBelow4G (sizeof (S3_PERFORMANCE_TABLE));
+      }
+      DEBUG ((EFI_D_INFO, "FPDT: ACPI S3 Performance Table address = 0x%x\n", mAcpiS3PerformanceTable));
+      if (mAcpiS3PerformanceTable != NULL) {
+        CopyMem (mAcpiS3PerformanceTable, &mS3PerformanceTableTemplate, sizeof (mS3PerformanceTableTemplate));
+      }
+    }
+  }
+
+  if (mLockBoxReady && (mAcpiS3PerformanceTable != NULL)) {
+    //
+    // If LockBox services has been ready and memory for FPDT S3 performance table has been allocated,
+    // save the pointer to LockBox for use in S3 resume.
+    //
+    S3PerformanceTablePointer = (EFI_PHYSICAL_ADDRESS) (UINTN) mAcpiS3PerformanceTable;
+    Status = SaveLockBox (
+               &gFirmwarePerformanceS3PointerGuid,
+               &S3PerformanceTablePointer,
+               sizeof (EFI_PHYSICAL_ADDRESS)
+               );
+    ASSERT_EFI_ERROR (Status);
+  }
+}
+
+/**
+  Install ACPI Firmware Performance Data Table (FPDT).
+
+  @return Status code.
+
+**/
+EFI_STATUS
+InstallFirmwarePerformanceDataTable (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_ACPI_TABLE_PROTOCOL       *AcpiTableProtocol;
+  UINTN                         Size;
+  UINT8                         *SmmBootRecordCommBuffer;
+  EFI_SMM_COMMUNICATE_HEADER    *SmmCommBufferHeader;
+  SMM_BOOT_RECORD_COMMUNICATE   *SmmCommData;
+  UINTN                         CommSize;
+  UINTN                         BootPerformanceDataSize;
+  UINT8                         *BootPerformanceData; 
+  EFI_SMM_COMMUNICATION_PROTOCOL  *Communication;
+  FIRMWARE_PERFORMANCE_VARIABLE PerformanceVariable;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE *SmmCommRegionTable;
+  EFI_MEMORY_DESCRIPTOR         *SmmCommMemRegion;
+  UINTN                         Index;
+  VOID                          *SmmBootRecordData;
+  UINTN                         SmmBootRecordDataSize;
+  UINTN                         ReservedMemSize;
+
+  //
+  // Get AcpiTable Protocol.
+  //
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID **) &AcpiTableProtocol);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Collect boot records from SMM drivers.
+  //
+  SmmBootRecordCommBuffer = NULL;
+  SmmCommData             = NULL;
+  SmmBootRecordData       = NULL;
+  SmmBootRecordDataSize   = 0;
+  ReservedMemSize         = 0;
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &Communication);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Initialize communicate buffer 
+    // Get the prepared Reserved Memory Range
+    //
+    Status = EfiGetSystemConfigurationTable (
+              &gEdkiiPiSmmCommunicationRegionTableGuid, 
+              (VOID **) &SmmCommRegionTable
+              );
+    if (!EFI_ERROR (Status)) {
+      ASSERT (SmmCommRegionTable != NULL);
+      SmmCommMemRegion = (EFI_MEMORY_DESCRIPTOR *) (SmmCommRegionTable + 1);
+      for (Index = 0; Index < SmmCommRegionTable->NumberOfEntries; Index ++) {
+        if (SmmCommMemRegion->Type == EfiConventionalMemory) {
+          break;
+        }
+        SmmCommMemRegion = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) SmmCommMemRegion + SmmCommRegionTable->DescriptorSize);
+      }
+      ASSERT (Index < SmmCommRegionTable->NumberOfEntries);
+      ASSERT (SmmCommMemRegion->PhysicalStart > 0);
+      ASSERT (SmmCommMemRegion->NumberOfPages > 0);
+      ReservedMemSize = (UINTN) SmmCommMemRegion->NumberOfPages * EFI_PAGE_SIZE;
+    
+      //
+      // Check enough reserved memory space
+      //
+      if (ReservedMemSize > SMM_BOOT_RECORD_COMM_SIZE) {
+        SmmBootRecordCommBuffer = (VOID *) (UINTN) SmmCommMemRegion->PhysicalStart;
+        SmmCommBufferHeader = (EFI_SMM_COMMUNICATE_HEADER*)SmmBootRecordCommBuffer;
+        SmmCommData = (SMM_BOOT_RECORD_COMMUNICATE*)SmmCommBufferHeader->Data;
+        ZeroMem((UINT8*)SmmCommData, sizeof(SMM_BOOT_RECORD_COMMUNICATE));
+    
+        CopyGuid (&SmmCommBufferHeader->HeaderGuid, &gEfiFirmwarePerformanceGuid);
+        SmmCommBufferHeader->MessageLength = sizeof(SMM_BOOT_RECORD_COMMUNICATE);
+        CommSize = SMM_BOOT_RECORD_COMM_SIZE;
+      
+        //
+        // Get the size of boot records.
+        //
+        SmmCommData->Function       = SMM_FPDT_FUNCTION_GET_BOOT_RECORD_SIZE;
+        SmmCommData->BootRecordData = NULL;
+        Status = Communication->Communicate (Communication, SmmBootRecordCommBuffer, &CommSize);
+        ASSERT_EFI_ERROR (Status);
+      
+        if (!EFI_ERROR (SmmCommData->ReturnStatus) && SmmCommData->BootRecordSize != 0) {
+          //
+          // Get all boot records
+          //
+          SmmCommData->Function       = SMM_FPDT_FUNCTION_GET_BOOT_RECORD_DATA_BY_OFFSET;
+          SmmBootRecordDataSize       = SmmCommData->BootRecordSize;
+          SmmBootRecordData           = AllocateZeroPool(SmmBootRecordDataSize);
+          ASSERT (SmmBootRecordData  != NULL);
+          SmmCommData->BootRecordOffset = 0;
+          SmmCommData->BootRecordData   = (VOID *) ((UINTN) SmmCommMemRegion->PhysicalStart + SMM_BOOT_RECORD_COMM_SIZE);
+          SmmCommData->BootRecordSize   = ReservedMemSize - SMM_BOOT_RECORD_COMM_SIZE;
+          while (SmmCommData->BootRecordOffset < SmmBootRecordDataSize) {
+            Status = Communication->Communicate (Communication, SmmBootRecordCommBuffer, &CommSize);
+            ASSERT_EFI_ERROR (Status);
+            ASSERT_EFI_ERROR(SmmCommData->ReturnStatus);
+            CopyMem ((UINT8 *) SmmBootRecordData + SmmCommData->BootRecordOffset, SmmCommData->BootRecordData, SmmCommData->BootRecordSize);
+            SmmCommData->BootRecordOffset = SmmCommData->BootRecordOffset + SmmCommData->BootRecordSize;
+          }
+        }
+      }
+    }
+  }
+
+  //
+  // Prepare memory for Boot Performance table.
+  // Boot Performance table includes BasicBoot record, and one or more appended Boot Records. 
+  //
+  BootPerformanceDataSize = sizeof (BOOT_PERFORMANCE_TABLE) + mBootRecordSize + PcdGet32 (PcdExtFpdtBootRecordPadSize);
+  if (SmmCommData != NULL) {
+    BootPerformanceDataSize += SmmBootRecordDataSize;
+  }
+
+  //
+  // Try to allocate the same runtime buffer as last time boot.
+  //
+  ZeroMem (&PerformanceVariable, sizeof (PerformanceVariable));
+  Size = sizeof (PerformanceVariable);
+  Status = gRT->GetVariable (
+                  EFI_FIRMWARE_PERFORMANCE_VARIABLE_NAME,
+                  &gEfiFirmwarePerformanceGuid,
+                  NULL,
+                  &Size,
+                  &PerformanceVariable
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->AllocatePages (
+                    AllocateAddress,
+                    EfiReservedMemoryType,
+                    EFI_SIZE_TO_PAGES (BootPerformanceDataSize),
+                    &PerformanceVariable.BootPerformanceTablePointer
+                    );
+    if (!EFI_ERROR (Status)) {
+      mAcpiBootPerformanceTable = (BOOT_PERFORMANCE_TABLE *) (UINTN) PerformanceVariable.BootPerformanceTablePointer;
+    }
+  }
+
+  if (mAcpiBootPerformanceTable == NULL) {
+    //
+    // Fail to allocate at specified address, continue to allocate at any address.
+    //
+    mAcpiBootPerformanceTable = (BOOT_PERFORMANCE_TABLE *) FpdtAllocateReservedMemoryBelow4G (BootPerformanceDataSize);
+  }
+  DEBUG ((EFI_D_INFO, "FPDT: ACPI Boot Performance Table address = 0x%x\n", mAcpiBootPerformanceTable));
+
+  if (mAcpiBootPerformanceTable == NULL) {
+    if (SmmCommData != NULL && SmmBootRecordData != NULL) {
+      FreePool (SmmBootRecordData);
+    }
+    if (mAcpiS3PerformanceTable != NULL) {
+      FreePages (mAcpiS3PerformanceTable, EFI_SIZE_TO_PAGES (sizeof (S3_PERFORMANCE_TABLE)));
+      mAcpiS3PerformanceTable = NULL;
+    }
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Prepare Boot Performance Table.
+  //
+  BootPerformanceData = (UINT8 *) mAcpiBootPerformanceTable;
+  //
+  // Fill Basic Boot record to Boot Performance Table.
+  //
+  CopyMem (mAcpiBootPerformanceTable, &mBootPerformanceTableTemplate, sizeof (mBootPerformanceTableTemplate));
+  BootPerformanceData = BootPerformanceData + mAcpiBootPerformanceTable->Header.Length;
+  //
+  // Fill Boot records from boot drivers.
+  //
+  CopyMem (BootPerformanceData, mBootRecordBuffer, mBootRecordSize);
+  mAcpiBootPerformanceTable->Header.Length += mBootRecordSize;
+  BootPerformanceData = BootPerformanceData + mBootRecordSize;
+  if (SmmCommData != NULL && SmmBootRecordData != NULL) {
+    //
+    // Fill Boot records from SMM drivers.
+    //
+    CopyMem (BootPerformanceData, SmmBootRecordData, SmmBootRecordDataSize);
+    FreePool (SmmBootRecordData);
+    mAcpiBootPerformanceTable->Header.Length = (UINT32) (mAcpiBootPerformanceTable->Header.Length + SmmBootRecordDataSize);
+    BootPerformanceData = BootPerformanceData + SmmBootRecordDataSize;
+  }
+
+  //
+  // Save Boot Performance Table address to Variable for use in S4 resume.
+  //
+  PerformanceVariable.BootPerformanceTablePointer = (EFI_PHYSICAL_ADDRESS) (UINTN) mAcpiBootPerformanceTable;
+  //
+  // Update Boot Performance Table Pointer in template.
+  //
+  mFirmwarePerformanceTableTemplate.BootPointerRecord.BootPerformanceTablePointer = (UINT64) (UINTN) mAcpiBootPerformanceTable;
+
+  //
+  // Save S3 Performance Table address to Variable for use in S4 resume.
+  //
+  PerformanceVariable.S3PerformanceTablePointer = (EFI_PHYSICAL_ADDRESS) (UINTN) mAcpiS3PerformanceTable;
+  //
+  // Update S3 Performance Table Pointer in template.
+  //
+  mFirmwarePerformanceTableTemplate.S3PointerRecord.S3PerformanceTablePointer = (UINT64) (UINTN) mAcpiS3PerformanceTable;
+  //
+  // Save Runtime Performance Table pointers to Variable.
+  // Don't check SetVariable return status. It doesn't impact FPDT table generation. 
+  //
+  gRT->SetVariable (
+        EFI_FIRMWARE_PERFORMANCE_VARIABLE_NAME,
+        &gEfiFirmwarePerformanceGuid,
+        EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+        sizeof (PerformanceVariable),
+        &PerformanceVariable
+        );
+
+  //
+  // Publish Firmware Performance Data Table.
+  //
+  FpdtAcpiTableChecksum ((UINT8 *) &mFirmwarePerformanceTableTemplate, mFirmwarePerformanceTableTemplate.Header.Length);
+  Status = AcpiTableProtocol->InstallAcpiTable (
+                                AcpiTableProtocol,
+                                &mFirmwarePerformanceTableTemplate,
+                                mFirmwarePerformanceTableTemplate.Header.Length,
+                                &mFirmwarePerformanceTableTemplateKey
+                                );
+  if (EFI_ERROR (Status)) {
+    FreePages (mAcpiBootPerformanceTable, EFI_SIZE_TO_PAGES (BootPerformanceDataSize));
+    if (mAcpiS3PerformanceTable != NULL) {
+      FreePages (mAcpiS3PerformanceTable, EFI_SIZE_TO_PAGES (sizeof (S3_PERFORMANCE_TABLE)));
+    }
+    mAcpiBootPerformanceTable = NULL;
+    mAcpiS3PerformanceTable = NULL;
+    return Status;
+  }
+  
+  //
+  // Free temp Boot record, and update Boot Record to point to Basic Boot performance table.
+  //
+  if (mBootRecordBuffer != NULL) {
+    FreePool (mBootRecordBuffer);
+  }
+  mBootRecordBuffer  = (UINT8 *) mAcpiBootPerformanceTable;
+  mBootRecordSize    = mAcpiBootPerformanceTable->Header.Length;
+  mBootRecordMaxSize = mBootRecordSize + PcdGet32 (PcdExtFpdtBootRecordPadSize);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Notify function for event group EFI_EVENT_GROUP_READY_TO_BOOT. This is used to
+  install the Firmware Performance Data Table.
+
+  @param[in]  Event   The Event that is being processed.
+  @param[in]  Context The Event Context.
+
+**/
+VOID
+EFIAPI
+FpdtReadyToBootEventNotify (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  if (mAcpiBootPerformanceTable == NULL) {
+    //
+    // ACPI Firmware Performance Data Table not installed yet, install it now.
+    //
+    InstallFirmwarePerformanceDataTable ();
+  }
+}
+
+/**
+  Report status code listener of FPDT. This is used to collect performance data
+  for OsLoaderLoadImageStart and OsLoaderStartImageStart in FPDT.
+
+  @param[in]  CodeType            Indicates the type of status code being reported.
+  @param[in]  Value               Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param[in]  Instance            The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param[in]  CallerId            This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param[in]  Data                This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS             Status code is what we expected.
+  @retval EFI_UNSUPPORTED         Status code not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FpdtStatusCodeListenerDxe (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check whether status code is what we are interested in.
+  //
+  if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) != EFI_PROGRESS_CODE) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  if (Value == (EFI_SOFTWARE_DXE_CORE | EFI_SW_DXE_CORE_PC_HANDOFF_TO_NEXT)) {
+    //
+    // DxeCore ReportStatusCode Enable so that the capability can be supported.
+    //
+    mDxeCoreReportStatusCodeEnable = TRUE;
+  }
+
+  Status = EFI_SUCCESS;
+  if (Value == PcdGet32 (PcdProgressCodeOsLoaderLoad)) {
+    //
+    // Progress code for OS Loader LoadImage.
+    //
+    if (mAcpiBootPerformanceTable == NULL) {
+      return Status;
+    }
+
+    //
+    // Update OS Loader LoadImage Start for UEFI boot.
+    //
+    mAcpiBootPerformanceTable->BasicBoot.OsLoaderLoadImageStart = GetTimeInNanoSecond (GetPerformanceCounter ());
+  } else if (Value == PcdGet32 (PcdProgressCodeOsLoaderStart)) {
+    //
+    // Progress code for OS Loader StartImage.
+    //
+    if (mAcpiBootPerformanceTable == NULL) {
+      return Status;
+    }
+
+    //
+    // Update OS Loader StartImage Start for UEFI boot.
+    //
+    mAcpiBootPerformanceTable->BasicBoot.OsLoaderStartImageStart = GetTimeInNanoSecond (GetPerformanceCounter ());
+  } else if (Value == (EFI_SOFTWARE_EFI_BOOT_SERVICE | EFI_SW_BS_PC_EXIT_BOOT_SERVICES)) {
+    //
+    // Unregister boot time report status code listener.
+    //
+    mRscHandlerProtocol->Unregister (FpdtStatusCodeListenerDxe);
+
+    //
+    // Progress code for ExitBootServices.
+    //
+    if (mAcpiBootPerformanceTable == NULL) {
+      return Status;
+    }
+
+    //
+    // Update ExitBootServicesExit for UEFI boot.
+    //
+    mAcpiBootPerformanceTable->BasicBoot.ExitBootServicesExit = GetTimeInNanoSecond (GetPerformanceCounter ());
+  } else if (Value == (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_PC_LEGACY_BOOT_EVENT)) {
+    if (mAcpiBootPerformanceTable == NULL) {
+      //
+      // Firmware Performance Data Table not installed, do nothing.
+      //
+      return Status;
+    }
+
+    //
+    // Update Firmware Basic Boot Performance Record for legacy boot.
+    //
+    mAcpiBootPerformanceTable->BasicBoot.OsLoaderStartImageStart = GetTimeInNanoSecond (GetPerformanceCounter ());
+
+    //
+    // Dump FPDT Boot Performance record.
+    //
+    DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - ResetEnd                = %ld\n", mAcpiBootPerformanceTable->BasicBoot.ResetEnd));
+    DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - OsLoaderLoadImageStart  = 0\n"));
+    DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - OsLoaderStartImageStart = %ld\n", mAcpiBootPerformanceTable->BasicBoot.OsLoaderStartImageStart));
+    DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - ExitBootServicesEntry   = 0\n"));
+    DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - ExitBootServicesExit    = 0\n"));
+  } else if (Data != NULL && CompareGuid (&Data->Type, &gEfiFirmwarePerformanceGuid)) {
+    //
+    // Append one or more Boot records
+    //
+    if (mAcpiBootPerformanceTable == NULL) {
+      //
+      // Append Boot records before FPDT ACPI table is installed. 
+      //
+      if (mBootRecordSize + Data->Size > mBootRecordMaxSize) {
+        mBootRecordBuffer = ReallocatePool (mBootRecordSize, mBootRecordSize + Data->Size + EXTENSION_RECORD_SIZE, mBootRecordBuffer);
+        ASSERT (mBootRecordBuffer != NULL);
+        mBootRecordMaxSize = mBootRecordSize + Data->Size + EXTENSION_RECORD_SIZE;
+      }
+      //
+      // Save boot record into the temp memory space.
+      //
+      CopyMem (mBootRecordBuffer + mBootRecordSize, Data + 1, Data->Size);
+      mBootRecordSize += Data->Size;
+    } else {
+      //
+      // Append Boot records after FPDT ACPI table is installed. 
+      //
+      if (mBootRecordSize + Data->Size > mBootRecordMaxSize) {
+        //
+        // No enough space to save boot record.
+        //
+        Status = EFI_OUT_OF_RESOURCES;
+      } else {
+        //
+        // Save boot record into BootPerformance table
+        //
+        CopyMem (mBootRecordBuffer + mBootRecordSize, Data + 1, Data->Size);
+        mBootRecordSize += Data->Size;
+        mAcpiBootPerformanceTable->Header.Length = mBootRecordSize;
+      }
+    }
+  } else {
+    //
+    // Ignore else progress code.
+    //
+    Status = EFI_UNSUPPORTED;
+  }
+
+  return Status;
+}
+
+
+/**
+  Notify function for event EVT_SIGNAL_EXIT_BOOT_SERVICES. This is used to record
+  performance data for ExitBootServicesEntry in FPDT.
+
+  @param[in]  Event   The Event that is being processed.
+  @param[in]  Context The Event Context.
+
+**/
+VOID
+EFIAPI
+FpdtExitBootServicesEventNotify (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  if (!mDxeCoreReportStatusCodeEnable) {
+    //
+    // When DxeCore Report Status Code is disabled, 
+    // Unregister boot time report status code listener at ExitBootService Event.
+    //
+    mRscHandlerProtocol->Unregister (FpdtStatusCodeListenerDxe);
+  }
+
+  if (mAcpiBootPerformanceTable == NULL) {
+    //
+    // Firmware Performance Data Table not installed, do nothing.
+    //
+    return ;
+  }
+
+  //
+  // Update Firmware Basic Boot Performance Record for UEFI boot.
+  //
+  mAcpiBootPerformanceTable->BasicBoot.ExitBootServicesEntry = GetTimeInNanoSecond (GetPerformanceCounter ());
+
+  //
+  // Dump FPDT Boot Performance record.
+  //
+  DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - ResetEnd                = %ld\n", mAcpiBootPerformanceTable->BasicBoot.ResetEnd));
+  DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - OsLoaderLoadImageStart  = %ld\n", mAcpiBootPerformanceTable->BasicBoot.OsLoaderLoadImageStart));
+  DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - OsLoaderStartImageStart = %ld\n", mAcpiBootPerformanceTable->BasicBoot.OsLoaderStartImageStart));
+  DEBUG ((EFI_D_INFO, "FPDT: Boot Performance - ExitBootServicesEntry   = %ld\n", mAcpiBootPerformanceTable->BasicBoot.ExitBootServicesEntry));
+  //
+  // ExitBootServicesExit will be updated later, so don't dump it here.
+  //
+}
+
+/**
+  The module Entry Point of the Firmware Performance Data Table DXE driver.
+
+  @param[in]  ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+FirmwarePerformanceDxeEntryPoint (
+  IN EFI_HANDLE          ImageHandle,
+  IN EFI_SYSTEM_TABLE    *SystemTable
+  )
+{
+  EFI_STATUS               Status;
+  EFI_HOB_GUID_TYPE        *GuidHob;
+  FIRMWARE_SEC_PERFORMANCE *Performance;
+  VOID                     *Registration;
+  UINT64                   OemTableId;
+
+  CopyMem (
+    mFirmwarePerformanceTableTemplate.Header.OemId,
+    PcdGetPtr (PcdAcpiDefaultOemId),
+    sizeof (mFirmwarePerformanceTableTemplate.Header.OemId)
+    );
+  OemTableId = PcdGet64 (PcdAcpiDefaultOemTableId);
+  CopyMem (&mFirmwarePerformanceTableTemplate.Header.OemTableId, &OemTableId, sizeof (UINT64));
+  mFirmwarePerformanceTableTemplate.Header.OemRevision      = PcdGet32 (PcdAcpiDefaultOemRevision);
+  mFirmwarePerformanceTableTemplate.Header.CreatorId        = PcdGet32 (PcdAcpiDefaultCreatorId);
+  mFirmwarePerformanceTableTemplate.Header.CreatorRevision  = PcdGet32 (PcdAcpiDefaultCreatorRevision);
+
+  //
+  // Get Report Status Code Handler Protocol.
+  //
+  Status = gBS->LocateProtocol (&gEfiRscHandlerProtocolGuid, NULL, (VOID **) &mRscHandlerProtocol);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register report status code listener for OS Loader load and start.
+  //
+  Status = mRscHandlerProtocol->Register (FpdtStatusCodeListenerDxe, TPL_HIGH_LEVEL);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register the notify function to update FPDT on ExitBootServices Event.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  FpdtExitBootServicesEventNotify,
+                  NULL,
+                  &gEfiEventExitBootServicesGuid,
+                  &mExitBootServicesEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Create ready to boot event to install ACPI FPDT table.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  FpdtReadyToBootEventNotify,
+                  NULL,
+                  &gEfiEventReadyToBootGuid,
+                  &mReadyToBootEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Retrieve GUID HOB data that contains the ResetEnd.
+  //
+  GuidHob = GetFirstGuidHob (&gEfiFirmwarePerformanceGuid);
+  if (GuidHob != NULL) {
+    Performance = (FIRMWARE_SEC_PERFORMANCE *) GET_GUID_HOB_DATA (GuidHob);
+    mBootPerformanceTableTemplate.BasicBoot.ResetEnd = Performance->ResetEnd;
+  } else {
+    //
+    // SEC Performance Data Hob not found, ResetEnd in ACPI FPDT table will be 0.
+    //
+    DEBUG ((EFI_D_ERROR, "FPDT: WARNING: SEC Performance Data Hob not found, ResetEnd will be set to 0!\n"));
+  }
+
+  if (FeaturePcdGet (PcdFirmwarePerformanceDataTableS3Support)) {
+    //
+    // Register callback function upon VariableArchProtocol and LockBoxProtocol
+    // to allocate S3 performance table memory and save the pointer to LockBox.
+    //
+    EfiCreateProtocolNotifyEvent (
+      &gEfiVariableArchProtocolGuid,
+      TPL_CALLBACK,
+      FpdtAllocateS3PerformanceTableMemory,
+      NULL,
+      &Registration
+      );
+    EfiCreateProtocolNotifyEvent (
+      &gEfiLockBoxProtocolGuid,
+      TPL_CALLBACK,
+      FpdtAllocateS3PerformanceTableMemory,
+      NULL,
+      &Registration
+      );
+  } else {
+    //
+    // Exclude S3 Performance Table Pointer from FPDT table template.
+    //
+    mFirmwarePerformanceTableTemplate.Header.Length -= sizeof (EFI_ACPI_5_0_FPDT_S3_PERFORMANCE_TABLE_POINTER_RECORD);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.inf b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.inf
new file mode 100644
index 0000000000..49d8420ba4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.inf
@@ -0,0 +1,92 @@
+## @file
+#  This module installs ACPI Firmware Performance Data Table (FPDT).
+#
+#  This module registers report status code listener to collect performance data
+#  for Firmware Basic Boot Performance Record and other boot performance records, 
+#  and install FPDT to ACPI table.
+#
+#  Copyright (c) 2011 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FirmwarePerformanceDxe
+  MODULE_UNI_FILE                = FirmwarePerformanceDxe.uni
+  FILE_GUID                      = 00160F8D-2B35-4df2-BBE0-B272A8D631F0
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = FirmwarePerformanceDxeEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FirmwarePerformanceDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  BaseLib
+  DebugLib
+  TimerLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  PcdLib
+  HobLib
+  LockBoxLib
+  UefiLib
+
+[Protocols]
+  gEfiAcpiTableProtocolGuid                     ## CONSUMES
+  gEfiRscHandlerProtocolGuid                    ## CONSUMES
+  gEfiSmmCommunicationProtocolGuid              ## SOMETIMES_CONSUMES
+  gEfiVariableArchProtocolGuid                  ## CONSUMES
+  gEfiLockBoxProtocolGuid                       ## CONSUMES
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## CONSUMES             ## Event
+  gEfiEventReadyToBootGuid                      ## CONSUMES             ## Event
+  gEfiEventLegacyBootGuid                       ## SOMETIMES_CONSUMES   ## Event
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_CONSUMES   ## Variable:L"FirmwarePerformance"
+  ## PRODUCES             ## Variable:L"FirmwarePerformance"
+  ## SOMETIMES_CONSUMES ## UNDEFINED # Used to do smm communication
+  ## SOMETIMES_CONSUMES ## UNDEFINED # StatusCode Data
+  gEfiFirmwarePerformanceGuid
+  gFirmwarePerformanceS3PointerGuid             ## PRODUCES ## UNDEFINED # SaveLockBox
+  gEdkiiPiSmmCommunicationRegionTableGuid       ## SOMETIMES_CONSUMES    ## SystemTable
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderLoad    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeOsLoaderStart   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdExtFpdtBootRecordPadSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemId            ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemTableId       ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorId        ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorRevision  ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwarePerformanceDataTableS3Support   ## CONSUMES
+
+[Depex]
+  gEfiRscHandlerProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FirmwarePerformanceDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.uni
new file mode 100644
index 0000000000..9d25f6f98a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxeExtra.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxeExtra.uni
new file mode 100644
index 0000000000..a39550b9df
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableDxe/FirmwarePerformanceDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.c b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.c
new file mode 100644
index 0000000000..396462e7aa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.c
@@ -0,0 +1,207 @@
+/** @file
+  This module updates S3 Resume Performance Record in ACPI Firmware Performance
+  Data Table in S3 resume boot mode. In normal boot mode, this module consumes
+  SecPerformance PPI produced by SEC phase and build Hob to convey the SEC
+  performance data to DXE phase.
+
+  This module register report status code listener to collect performance data
+  for S3 Resume Performance Record on S3 resume boot path.
+
+  Copyright (c) 2011 - 2015, 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 <PiPei.h>
+
+#include <Ppi/ReportStatusCodeHandler.h>
+#include <Ppi/SecPerformance.h>
+
+#include <Guid/FirmwarePerformance.h>
+
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/HobLib.h>
+#include <Library/PcdLib.h>
+
+/**
+  Report status code listener for PEI. This is used to record the performance
+  data for S3 FullResume in FPDT.
+
+  @param[in]  PeiServices         An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param[in]  CodeType            Indicates the type of status code being reported.
+  @param[in]  Value               Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param[in]  Instance            The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param[in]  CallerId            This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param[in]  Data                This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS             Status code is what we expected.
+  @retval EFI_UNSUPPORTED         Status code not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FpdtStatusCodeListenerPei (
+  IN CONST  EFI_PEI_SERVICES        **PeiServices,
+  IN        EFI_STATUS_CODE_TYPE    CodeType,
+  IN        EFI_STATUS_CODE_VALUE   Value,
+  IN        UINT32                  Instance,
+  IN CONST  EFI_GUID                *CallerId,
+  IN CONST  EFI_STATUS_CODE_DATA    *Data
+  )
+{
+  EFI_STATUS                           Status;
+  UINT64                               CurrentTime;
+  UINTN                                VarSize;
+  EFI_PHYSICAL_ADDRESS                 S3PerformanceTablePointer;
+  S3_PERFORMANCE_TABLE                 *AcpiS3PerformanceTable;
+  EFI_ACPI_5_0_FPDT_S3_RESUME_RECORD   *AcpiS3ResumeRecord;
+  UINT64                               S3ResumeTotal;
+  EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD  S3SuspendRecord;
+  EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD  *AcpiS3SuspendRecord;
+
+  //
+  // Check whether status code is what we are interested in.
+  //
+  if (((CodeType & EFI_STATUS_CODE_TYPE_MASK) != EFI_PROGRESS_CODE) ||
+  	  (Value != (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_PC_OS_WAKE))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Retrieve current time as early as possible.
+  //
+  CurrentTime = GetTimeInNanoSecond (GetPerformanceCounter ());
+
+  //
+  // Update S3 Resume Performance Record.
+  //
+  S3PerformanceTablePointer = 0;
+  VarSize = sizeof (EFI_PHYSICAL_ADDRESS);
+  Status = RestoreLockBox (&gFirmwarePerformanceS3PointerGuid, &S3PerformanceTablePointer, &VarSize);
+  ASSERT_EFI_ERROR (Status);
+
+  AcpiS3PerformanceTable = (S3_PERFORMANCE_TABLE *) (UINTN) S3PerformanceTablePointer;
+  ASSERT (AcpiS3PerformanceTable != NULL);
+  if (AcpiS3PerformanceTable->Header.Signature != EFI_ACPI_5_0_FPDT_S3_PERFORMANCE_TABLE_SIGNATURE) {
+    DEBUG ((EFI_D_ERROR, "FPDT S3 performance data in ACPI memory get corrupted\n"));
+    return EFI_ABORTED;
+  }
+  AcpiS3ResumeRecord = &AcpiS3PerformanceTable->S3Resume;
+  AcpiS3ResumeRecord->FullResume = CurrentTime;
+  //
+  // Calculate average S3 resume time.
+  //
+  S3ResumeTotal = MultU64x32 (AcpiS3ResumeRecord->AverageResume, AcpiS3ResumeRecord->ResumeCount);
+  AcpiS3ResumeRecord->ResumeCount++;
+  AcpiS3ResumeRecord->AverageResume = DivU64x32 (S3ResumeTotal + AcpiS3ResumeRecord->FullResume, AcpiS3ResumeRecord->ResumeCount);
+
+  DEBUG ((EFI_D_INFO, "FPDT: S3 Resume Performance - ResumeCount   = %d\n", AcpiS3ResumeRecord->ResumeCount));
+  DEBUG ((EFI_D_INFO, "FPDT: S3 Resume Performance - FullResume    = %ld\n", AcpiS3ResumeRecord->FullResume));
+  DEBUG ((EFI_D_INFO, "FPDT: S3 Resume Performance - AverageResume = %ld\n", AcpiS3ResumeRecord->AverageResume));
+
+  //
+  // Update S3 Suspend Performance Record.
+  //
+  AcpiS3SuspendRecord = &AcpiS3PerformanceTable->S3Suspend;
+  VarSize = sizeof (EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD);
+  ZeroMem (&S3SuspendRecord, sizeof (EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD));
+  Status = RestoreLockBox (
+             &gEfiFirmwarePerformanceGuid,
+             &S3SuspendRecord,
+             &VarSize
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  AcpiS3SuspendRecord->SuspendStart = S3SuspendRecord.SuspendStart;
+  AcpiS3SuspendRecord->SuspendEnd   = S3SuspendRecord.SuspendEnd;
+
+  DEBUG ((EFI_D_INFO, "FPDT: S3 Suspend Performance - SuspendStart = %ld\n", AcpiS3SuspendRecord->SuspendStart));
+  DEBUG ((EFI_D_INFO, "FPDT: S3 Suspend Performance - SuspendEnd   = %ld\n", AcpiS3SuspendRecord->SuspendEnd));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Main entry for Firmware Performance Data Table PEIM.
+
+  This routine is to register report status code listener for FPDT.
+
+  @param[in]  FileHandle              Handle of the file being invoked.
+  @param[in]  PeiServices             Pointer to PEI Services table.
+
+  @retval EFI_SUCCESS Report status code listener is registered successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+FirmwarePerformancePeiEntryPoint (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS               Status;
+  EFI_PEI_RSC_HANDLER_PPI  *RscHandler;
+  PEI_SEC_PERFORMANCE_PPI  *SecPerf;
+  FIRMWARE_SEC_PERFORMANCE Performance;
+
+  if (FeaturePcdGet (PcdFirmwarePerformanceDataTableS3Support)) {
+    //
+    // S3 resume - register status code listener for OS wake vector.
+    //
+    Status = PeiServicesLocatePpi (
+               &gEfiPeiRscHandlerPpiGuid,
+               0,
+               NULL,
+               (VOID **) &RscHandler
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    Status = RscHandler->Register (FpdtStatusCodeListenerPei);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Normal boot - build Hob for SEC performance data.
+  //
+  Status = PeiServicesLocatePpi (
+             &gPeiSecPerformancePpiGuid,
+             0,
+             NULL,
+             (VOID **) &SecPerf
+             );
+  if (!EFI_ERROR (Status)) {
+    Status = SecPerf->GetPerformance (PeiServices, SecPerf, &Performance);
+  }
+  if (!EFI_ERROR (Status)) {
+    BuildGuidDataHob (
+      &gEfiFirmwarePerformanceGuid,
+      &Performance,
+      sizeof (FIRMWARE_SEC_PERFORMANCE)
+    );
+    DEBUG ((EFI_D_INFO, "FPDT: SEC Performance Hob ResetEnd = %ld\n", Performance.ResetEnd));
+  } else {
+    //
+    // SEC performance PPI is not installed or fail to get performance data
+    // from SEC Performance PPI.
+    //
+    DEBUG ((EFI_D_ERROR, "FPDT: WARNING: SEC Performance PPI not installed or failed!\n"));
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.inf b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.inf
new file mode 100644
index 0000000000..25049f8323
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.inf
@@ -0,0 +1,76 @@
+## @file
+#  Firmware Performance Pei Module.
+#
+#  In S3 resume boot mode, it updates S3 Resume Performance Record in ACPI Firmware Performance Data Table.
+#  In normal boot mode, it consumes SecPerformance PPI produced by SEC phase
+#  and build Hob to convey the SEC performance data to DXE phase.
+#
+#  This module register report status code listener to collect performance data
+#  for S3 Resume Performance Record on S3 resume boot path.
+#
+#  Copyright (c) 2011 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FirmwarePerformancePei
+  MODULE_UNI_FILE                = FirmwarePerformancePei.uni
+  FILE_GUID                      = ADF01BF6-47D6-495d-B95B-687777807214
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = FirmwarePerformancePeiEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FirmwarePerformancePei.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeimEntryPoint
+  PeiServicesLib
+  BaseLib
+  DebugLib
+  HobLib
+  TimerLib
+  BaseMemoryLib
+  LockBoxLib
+  PcdLib
+
+[Ppis]
+  gEfiPeiRscHandlerPpiGuid                      ## CONSUMES
+  gPeiSecPerformancePpiGuid                     ## SOMETIMES_CONSUMES
+
+[Guids]
+  ## SOMETIMES_CONSUMES   ## UNDEFINED # RestoreLockBox
+  ## SOMETIMES_PRODUCES   ## HOB
+  ## SOMETIMES_CONSUMES   ## Variable:L"FirmwarePerformance"
+  gEfiFirmwarePerformanceGuid
+  gFirmwarePerformanceS3PointerGuid             ## SOMETIMES_CONSUMES ## UNDEFINED # RestoreLockBox
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwarePerformanceDataTableS3Support   ## CONSUMES
+
+[Depex]
+  gEfiPeiMasterBootModePpiGuid AND gEfiPeiRscHandlerPpiGuid
+
+# [BootMode]
+# S3_RESUME             ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FirmwarePerformancePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.uni
new file mode 100644
index 0000000000..fdb9152f6d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePei.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePeiExtra.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePeiExtra.uni
new file mode 100644
index 0000000000..3412a0ed1a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTablePei/FirmwarePerformancePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.c b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.c
new file mode 100644
index 0000000000..c750331374
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.c
@@ -0,0 +1,336 @@
+/** @file
+  This module collects performance data for SMM driver boot records and S3 Suspend Performance Record.
+
+  This module registers report status code listener to collect performance data
+  for SMM driver boot records and S3 Suspend Performance Record.
+
+  Caution: This module requires additional review when modified.
+  This driver will have external input - communicate buffer in SMM mode.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  FpdtSmiHandler() will receive untrusted input and do basic validation.
+
+  Copyright (c) 2011 - 2016, 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 <PiSmm.h>
+
+#include <Protocol/SmmReportStatusCodeHandler.h>
+
+#include <Guid/FirmwarePerformance.h>
+
+#include <Library/SmmServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/TimerLib.h>
+#include <Library/LockBoxLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/SmmMemLib.h>
+
+#define EXTENSION_RECORD_SIZE     0x1000
+
+EFI_SMM_RSC_HANDLER_PROTOCOL  *mRscHandlerProtocol    = NULL;
+UINT64                        mSuspendStartTime       = 0;
+BOOLEAN                       mS3SuspendLockBoxSaved  = FALSE;
+UINT32                        mBootRecordSize = 0;
+UINT32                        mBootRecordMaxSize = 0;
+UINT8                         *mBootRecordBuffer = NULL;
+
+SPIN_LOCK                     mSmmFpdtLock;
+BOOLEAN                       mSmramIsOutOfResource = FALSE;
+
+/**
+  Report status code listener for SMM. This is used to record the performance
+  data for S3 Suspend Start and S3 Suspend End in FPDT.
+
+  @param[in]  CodeType            Indicates the type of status code being reported.
+  @param[in]  Value               Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param[in]  Instance            The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param[in]  CallerId            This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param[in]  Data                This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS             Status code is what we expected.
+  @retval EFI_UNSUPPORTED         Status code not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+FpdtStatusCodeListenerSmm (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data
+  )
+{
+  EFI_STATUS                           Status;
+  UINT64                               CurrentTime;
+  EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD  S3SuspendRecord;
+  UINT8                                *NewRecordBuffer;
+
+  //
+  // Check whether status code is what we are interested in.
+  //
+  if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) != EFI_PROGRESS_CODE) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // Collect one or more Boot records in boot time
+  //
+  if (Data != NULL && CompareGuid (&Data->Type, &gEfiFirmwarePerformanceGuid)) {
+    AcquireSpinLock (&mSmmFpdtLock);
+    
+    if (mBootRecordSize + Data->Size > mBootRecordMaxSize) {
+      //
+      // Try to allocate big SMRAM data to store Boot record. 
+      //
+      if (mSmramIsOutOfResource) {
+        ReleaseSpinLock (&mSmmFpdtLock);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      NewRecordBuffer = ReallocatePool (mBootRecordSize, mBootRecordSize + Data->Size + EXTENSION_RECORD_SIZE, mBootRecordBuffer); 
+      if (NewRecordBuffer == NULL) {
+        ReleaseSpinLock (&mSmmFpdtLock);
+        mSmramIsOutOfResource = TRUE;
+        return EFI_OUT_OF_RESOURCES;
+      }
+      mBootRecordBuffer  = NewRecordBuffer;
+      mBootRecordMaxSize = mBootRecordSize + Data->Size + EXTENSION_RECORD_SIZE;
+    }
+    //
+    // Save boot record into the temp memory space.
+    //
+    CopyMem (mBootRecordBuffer + mBootRecordSize, Data + 1, Data->Size);
+    mBootRecordSize += Data->Size;
+    
+    ReleaseSpinLock (&mSmmFpdtLock);
+    return EFI_SUCCESS;
+  }
+
+  if ((Value != PcdGet32 (PcdProgressCodeS3SuspendStart)) &&
+      (Value != PcdGet32 (PcdProgressCodeS3SuspendEnd))) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Retrieve current time.
+  //
+  CurrentTime = GetTimeInNanoSecond (GetPerformanceCounter ());
+
+  if (Value == PcdGet32 (PcdProgressCodeS3SuspendStart)) {
+    //
+    // S3 Suspend started, record the performance data and return.
+    //
+    mSuspendStartTime = CurrentTime;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // We are going to S3 sleep, record S3 Suspend End performance data.
+  //
+  S3SuspendRecord.SuspendStart = mSuspendStartTime;
+  S3SuspendRecord.SuspendEnd   = CurrentTime;
+
+  //
+  // Save S3 suspend performance data to lock box, it will be used by Firmware Performance PEIM.
+  //
+  if (!mS3SuspendLockBoxSaved) {
+    Status = SaveLockBox (
+               &gEfiFirmwarePerformanceGuid,
+               &S3SuspendRecord,
+               sizeof (EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD)
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    mS3SuspendLockBoxSaved = TRUE;
+  } else {
+    Status = UpdateLockBox (
+               &gEfiFirmwarePerformanceGuid,
+               0,
+               &S3SuspendRecord,
+               sizeof (EFI_ACPI_5_0_FPDT_S3_SUSPEND_RECORD)
+               );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Communication service SMI Handler entry.
+
+  This SMI handler provides services for report SMM boot records. 
+
+  Caution: This function may receive untrusted input.
+  Communicate buffer and buffer size are external input, so this function will do basic validation.
+
+  @param[in]     DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param[in]     RegisterContext Points to an optional handler context which was specified when the
+                                 handler was registered.
+  @param[in, out] CommBuffer     A pointer to a collection of data in memory that will
+                                 be conveyed from a non-SMM environment into an SMM environment.
+  @param[in, out] CommBufferSize The size of the CommBuffer.
+
+  @retval EFI_SUCCESS                         The interrupt was handled and quiesced. No other handlers 
+                                              should still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED  The interrupt has been quiesced but other handlers should 
+                                              still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING   The interrupt is still pending and other handlers should still 
+                                              be called.
+  @retval EFI_INTERRUPT_PENDING               The interrupt could not be quiesced.
+
+**/
+EFI_STATUS
+EFIAPI
+FpdtSmiHandler (
+  IN     EFI_HANDLE                   DispatchHandle,
+  IN     CONST VOID                   *RegisterContext,
+  IN OUT VOID                         *CommBuffer,
+  IN OUT UINTN                        *CommBufferSize
+  )
+{
+  EFI_STATUS                   Status;
+  SMM_BOOT_RECORD_COMMUNICATE  *SmmCommData;
+  UINTN                        BootRecordOffset;
+  UINTN                        BootRecordSize;
+  VOID                         *BootRecordData;
+  UINTN                        TempCommBufferSize;
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if(TempCommBufferSize < sizeof (SMM_BOOT_RECORD_COMMUNICATE)) {
+    return EFI_SUCCESS;
+  }
+  
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "FpdtSmiHandler: SMM communication data buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  SmmCommData = (SMM_BOOT_RECORD_COMMUNICATE*)CommBuffer;
+
+  Status = EFI_SUCCESS;
+
+  switch (SmmCommData->Function) {
+    case SMM_FPDT_FUNCTION_GET_BOOT_RECORD_SIZE :
+      SmmCommData->BootRecordSize = mBootRecordSize;
+      break;
+
+    case SMM_FPDT_FUNCTION_GET_BOOT_RECORD_DATA :
+      Status = EFI_UNSUPPORTED;
+      break;
+
+    case SMM_FPDT_FUNCTION_GET_BOOT_RECORD_DATA_BY_OFFSET :
+      BootRecordOffset = SmmCommData->BootRecordOffset;
+      BootRecordData   = SmmCommData->BootRecordData;
+      BootRecordSize   = SmmCommData->BootRecordSize;
+      if (BootRecordData == NULL || BootRecordOffset >= mBootRecordSize) {
+        Status = EFI_INVALID_PARAMETER;
+        break;
+      }
+      
+      //
+      // Sanity check
+      //
+      if (BootRecordSize > mBootRecordSize - BootRecordOffset) {
+        BootRecordSize = mBootRecordSize - BootRecordOffset;
+      }
+      SmmCommData->BootRecordSize = BootRecordSize;
+      if (!SmmIsBufferOutsideSmmValid ((UINTN)BootRecordData, BootRecordSize)) {
+        DEBUG ((EFI_D_ERROR, "FpdtSmiHandler: SMM Data buffer in SMRAM or overflow!\n"));
+        Status = EFI_ACCESS_DENIED;
+        break;
+      }
+      
+      CopyMem (
+       (UINT8*)BootRecordData, 
+       mBootRecordBuffer + BootRecordOffset, 
+       BootRecordSize
+       );
+      break;
+
+    default:
+      Status = EFI_UNSUPPORTED;
+  }
+
+  SmmCommData->ReturnStatus = Status;
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  The module Entry Point of the Firmware Performance Data Table SMM driver.
+
+  @param[in]  ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+FirmwarePerformanceSmmEntryPoint (
+  IN EFI_HANDLE          ImageHandle,
+  IN EFI_SYSTEM_TABLE    *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HANDLE                Handle;
+
+  //
+  // Initialize spin lock
+  //
+  InitializeSpinLock (&mSmmFpdtLock); 
+   
+  //
+  // Get SMM Report Status Code Handler Protocol.
+  //
+  Status = gSmst->SmmLocateProtocol (
+                    &gEfiSmmRscHandlerProtocolGuid,
+                    NULL,
+                    (VOID **) &mRscHandlerProtocol
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register report status code listener for BootRecords and S3 Suspend Start and End.
+  //
+  Status = mRscHandlerProtocol->Register (FpdtStatusCodeListenerSmm);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register SMI handler.
+  //
+  Handle = NULL;
+  Status = gSmst->SmiHandlerRegister (FpdtSmiHandler, &gEfiFirmwarePerformanceGuid, &Handle);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.inf b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.inf
new file mode 100644
index 0000000000..724e7bcd1d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.inf
@@ -0,0 +1,72 @@
+## @file
+#  This module collects performance data for SMM driver boot records and S3 Suspend Performance Record.
+#
+#  This module registers report status code listener to collect performance data
+#  for SMM boot performance records and S3 Suspend Performance Record.
+#
+#  Copyright (c) 2011 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FirmwarePerformanceSmm
+  MODULE_UNI_FILE                = FirmwarePerformanceSmm.uni
+  FILE_GUID                      = 044310AB-77FD-402a-AF1A-87D4120E7329
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = FirmwarePerformanceSmmEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  FirmwarePerformanceSmm.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  SmmServicesTableLib
+  BaseLib
+  DebugLib
+  TimerLib
+  LockBoxLib
+  PcdLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  SynchronizationLib
+  SmmMemLib
+
+[Protocols]
+  gEfiSmmRscHandlerProtocolGuid                 ## CONSUMES
+
+[Guids]
+  ## SOMETIMES_PRODUCES   ## UNDEFINED # SaveLockBox
+  ## PRODUCES             ## UNDEFINED # SmiHandlerRegister
+  ## SOMETIMES_CONSUMES   ## UNDEFINED # StatusCode Data
+  gEfiFirmwarePerformanceGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeS3SuspendStart  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdProgressCodeS3SuspendEnd    ## CONSUMES
+
+[Depex]
+  gEfiSmmRscHandlerProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FirmwarePerformanceSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.uni
new file mode 100644
index 0000000000..5a7e09301f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmm.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmmExtra.uni b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmmExtra.uni
new file mode 100644
index 0000000000..60acf9799e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/FirmwarePerformanceDataTableSmm/FirmwarePerformanceSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/InternalS3SaveState.h b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/InternalS3SaveState.h
new file mode 100644
index 0000000000..f5ca60611e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/InternalS3SaveState.h
@@ -0,0 +1,161 @@
+/** @file
+  Internal header file for S3 Boot Script Saver state driver.
+
+  Copyright (c) 2006 - 2010, 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.
+
+**/
+#ifndef _INTERNAL_S3_SAVE_STATE_H_
+#define _INTERNAL_S3_SAVE_STATE_H_
+#include <PiDxe.h>
+
+#include <Protocol/S3SaveState.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/S3BootScriptLib.h>
+#include <Library/SmbusLib.h>
+#include <IndustryStandard/SmBus.h>
+/**
+  Adds a record into S3 boot script table.
+
+  This function is used to store a boot script record into a given boot
+  script table. If the table specified by TableName is nonexistent in the 
+  system, a new table will automatically be created and then the script record 
+  will be added into the new table. This function is responsible for allocating 
+  necessary memory for the script.
+
+  This function has a variable parameter list. The exact parameter list depends on 
+  the OpCode that is passed into the function. If an unsupported OpCode or illegal 
+  parameter list is passed in, this function returns EFI_INVALID_PARAMETER.
+  If there are not enough resources available for storing more scripts, this function returns
+  EFI_OUT_OF_RESOURCES.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptWrite (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       UINT16                           OpCode,
+  ...
+  );
+/**
+  Insert a record into a specified Framework boot script table.
+
+  This function is used to store an OpCode to be replayed as part of the S3 resume boot path. It is
+  assumed this protocol has platform specific mechanism to store the OpCode set and replay them
+  during the S3 resume.
+  The opcode is inserted before or after the specified position in the boot script table. If Position is
+  NULL then that position is after the last opcode in the table (BeforeOrAfter is FALSE) or before
+  the first opcode in the table (BeforeOrAfter is TRUE). The position which is pointed to by
+  Position upon return can be used for subsequent insertions.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptInsert (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL    *This,
+  IN       BOOLEAN                          BeforeOrAfter,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION     *Position OPTIONAL,
+  IN       UINT16                           OpCode,
+  ...
+  );
+/**
+  Find a label within the boot script table and, if not present, optionally create it.
+
+  If the label Label is already exists in the boot script table, then no new label is created, the
+  position of the Label is returned in *Position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is TRUE, then it will be
+  created before or after the specified position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is FALSE, then
+  EFI_NOT_FOUND is returned.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the label is stored before (TRUE) or after (FALSE) the position in
+                                the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new label is inserted at the beginning of the table (if TRUE) or end of
+                                the table (if FALSE).
+  @param  CreateIfNotFound      Specifies whether the label will be created if the label does not exists (TRUE) or not
+                                (FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the label will be inserted,
+                                either before or after, depending on BeforeOrAfter. On exit, specifies the position
+                                of the inserted label in the boot script table.
+  @param  Label                 Points to the label which will be inserted in the boot script table.
+
+  @retval EFI_SUCCESS           The label already exists or was inserted.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  
+**/
+EFI_STATUS
+EFIAPI
+BootScriptLabel (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL           *This,
+  IN       BOOLEAN                               BeforeOrAfter,
+  IN       BOOLEAN                               CreateIfNotFound,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION          *Position OPTIONAL,
+  IN CONST CHAR8                                *Label
+  );
+/**
+  Compare two positions in the boot script table and return their relative position.
+  
+  This function compares two positions in the boot script table and returns their relative positions. If
+  Position1 is before Position2, then -1 is returned. If Position1 is equal to Position2,
+  then 0 is returned. If Position1 is after Position2, then 1 is returned.
+  
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  Position1             The positions in the boot script table to compare
+  @param  Position2             The positions in the boot script table to compare
+  @param  RelativePosition      On return, points to the result of the comparison
+
+  @retval EFI_SUCCESS           The operation succeeded. 
+  @retval EFI_INVALID_PARAMETER The Position1 or Position2 is not a valid position in the boot script table.
+
+**/
+EFI_STATUS
+EFIAPI 
+BootScriptCompare (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position1,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position2,
+  OUT      UINTN                           *RelativePosition
+  );
+  
+#endif //_INTERNAL_S3_SAVE_STATE_H_
diff --git a/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveState.c b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveState.c
new file mode 100644
index 0000000000..60cd9b1b3b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveState.c
@@ -0,0 +1,920 @@
+/** @file
+  Implementation for S3 Boot Script Saver state driver.
+
+  Copyright (c) 2006 - 2013, 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 "InternalS3SaveState.h"
+
+EFI_HANDLE                    mHandle = NULL;
+EFI_S3_SAVE_STATE_PROTOCOL    mS3SaveState = {
+  BootScriptWrite,
+  BootScriptInsert,
+  BootScriptLabel,
+  BootScriptCompare
+  };
+/**
+  Internal function to add IO write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteIoWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+  
+  return S3BootScriptSaveIoWrite (Width, Address, Count, Buffer);
+}
+/**
+  Internal function to add IO read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteIoReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+  
+  return S3BootScriptSaveIoReadWrite (Width, Address, Data, DataMask);
+}
+
+/**
+  Internal function to add memory write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSaveMemWrite (Width, Address, Count, Buffer);
+}
+
+/**
+  Internal function to add memory read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+  
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSaveMemReadWrite (Width, Address, Data, DataMask);
+}
+
+/**
+  Internal function to add PciCfg write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfgWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSavePciCfgWrite (Width, Address, Count, Buffer);
+}
+
+/**
+  Internal function to PciCfg read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfgReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSavePciCfgReadWrite (Width, Address, Data, DataMask);
+}
+/**
+  Internal function to add PciCfg2 write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfg2Write (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+  UINT16                Segment;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+  Segment     = VA_ARG (Marker, UINT16);
+
+  return S3BootScriptSavePciCfg2Write (Width, Segment, Address, Count, Buffer);
+}
+
+/**
+  Internal function to PciCfg2 read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfg2ReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT16                Segment;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Segment     = VA_ARG (Marker, UINT16);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+ 
+  return S3BootScriptSavePciCfg2ReadWrite (Width, Segment, Address, Data, DataMask);
+}
+/**
+  Internal function to add smbus excute opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteSmbusExecute (
+  IN VA_LIST                       Marker
+  )
+{
+  EFI_SMBUS_DEVICE_ADDRESS  SlaveAddress;
+  EFI_SMBUS_DEVICE_COMMAND  Command;
+  EFI_SMBUS_OPERATION       Operation;
+  BOOLEAN                   PecCheck;
+  VOID                     *Buffer;
+  UINTN                    *DataSize;
+  UINTN                     SmBusAddress;
+  
+  SlaveAddress.SmbusDeviceAddress = VA_ARG (Marker, UINTN);
+  Command                         = VA_ARG (Marker, EFI_SMBUS_DEVICE_COMMAND);
+  Operation                       = VA_ARG (Marker, EFI_SMBUS_OPERATION);
+  PecCheck                        = VA_ARG (Marker, BOOLEAN);
+  SmBusAddress                    = SMBUS_LIB_ADDRESS (SlaveAddress.SmbusDeviceAddress,Command,0,PecCheck);
+  DataSize                        = VA_ARG (Marker, UINTN *);    
+  Buffer                          = VA_ARG (Marker, VOID *);
+ 
+  return S3BootScriptSaveSmbusExecute (SmBusAddress, Operation, DataSize, Buffer);
+}
+/**
+  Internal function to add stall opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteStall (
+  IN VA_LIST                       Marker
+  )
+{
+  UINT32                Duration;
+
+  Duration    = VA_ARG (Marker, UINT32);
+
+  return S3BootScriptSaveStall (Duration);
+}
+
+/**
+  Internal function to add Save jmp address according to DISPATCH_OPCODE. 
+  We ignore "Context" parameter
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteDispatch (
+  IN VA_LIST                       Marker
+  )
+{
+  VOID        *EntryPoint;
+
+  EntryPoint = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  return S3BootScriptSaveDispatch (EntryPoint);
+}
+
+/**
+  Internal function to add memory pool operation to the table. 
+ 
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemPoll (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH   Width;                                    
+  UINT64                     Address;                                    
+  VOID                      *Data;                                    
+  VOID                      *DataMask;                                  
+  UINTN                     Delay;                                   
+  UINTN                     LoopTimes;
+  UINT32                    Remainder;
+
+  Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);                  
+  Address  = VA_ARG (Marker, UINT64);                                    
+  Data     = VA_ARG (Marker, VOID *);                                    
+  DataMask = VA_ARG (Marker, VOID *);                                    
+  Delay    = (UINTN)VA_ARG (Marker, UINT64);                            
+  //
+  // According to the spec, the interval between 2 polls is 100ns,
+  // but the unit of Duration for S3BootScriptSaveMemPoll() is microsecond(1000ns).
+  // Duration * 1000ns * LoopTimes = Delay * 100ns
+  // Duration will be minimum 1(microsecond) to be minimum deviation,
+  // so LoopTimes = Delay / 10.
+  //
+  LoopTimes = (UINTN) DivU64x32Remainder (
+                Delay,
+                10,
+                &Remainder
+                );
+  if (Remainder != 0) {
+    //
+    // If Remainder is not zero, LoopTimes will be rounded up by 1.
+    //
+    LoopTimes +=1;
+  }
+  return S3BootScriptSaveMemPoll (Width, Address, DataMask, Data, 1, LoopTimes);
+
+}
+
+/**
+  Internal function to add Save jmp address according to DISPATCH_OPCODE2. 
+  The "Context" parameter is not ignored.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteDispatch2 (
+  IN VA_LIST                       Marker
+  )
+{
+  VOID                  *EntryPoint;
+  VOID                  *Context;  
+
+  EntryPoint = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  Context    = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+
+  return S3BootScriptSaveDispatch2 (EntryPoint, Context);
+}
+/**
+  Internal function to add INFORAMTION opcode node to the table
+  list.
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWriteInformation (
+  IN VA_LIST                       Marker
+  )
+{
+  UINT32                InformationLength;
+  EFI_PHYSICAL_ADDRESS  Information;  
+
+  InformationLength = VA_ARG (Marker, UINT32);
+  Information = VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  return S3BootScriptSaveInformation (InformationLength, (VOID*)(UINTN)Information);
+}
+/**
+  Internal function to add IO poll opcode node  to the table
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWriteIoPoll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH     Width;
+   UINT64                     Address;    
+   VOID                      *Data;    
+   VOID                      *DataMask;   
+   UINT64                     Delay;   
+   
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSaveIoPoll (Width, Address, Data, DataMask, Delay);
+}
+/**
+  Internal function to add PCI config poll opcode node to the table
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWritePciConfigPoll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH   Width;
+   UINT64                     Address;
+   VOID                      *Data;
+   VOID                      *DataMask;
+   UINT64                     Delay;
+
+   
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSavePciPoll (Width, Address, Data, DataMask, Delay);
+}
+/**
+  Internal function to add PCI config 2 poll opcode node to the table
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWritePciConfig2Poll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH      Width;
+   UINT16                        Segment;
+   UINT64                        Address;
+   VOID                         *Data;
+   VOID                         *DataMask;
+   UINT64                        Delay;
+  
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Segment  = VA_ARG (Marker, UINT16);      
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSavePci2Poll (Width, Segment, Address, Data, DataMask, Delay);
+}
+
+
+/**
+  Adds a record into S3 boot script table.
+
+  This function is used to store a boot script record into a given boot
+  script table. If the table specified by TableName is nonexistent in the 
+  system, a new table will automatically be created and then the script record 
+  will be added into the new table. This function is responsible for allocating 
+  necessary memory for the script.
+
+  This function has a variable parameter list. The exact parameter list depends on 
+  the OpCode that is passed into the function. If an unsupported OpCode or illegal 
+  parameter list is passed in, this function returns EFI_INVALID_PARAMETER.
+  If there are not enough resources available for storing more scripts, this function returns
+  EFI_OUT_OF_RESOURCES.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+                                
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptWrite (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       UINT16                           OpCode,
+  ...
+  )
+{
+  EFI_STATUS                Status;
+  VA_LIST                   Marker;  
+  //
+  // Build script according to opcode
+  //
+  switch (OpCode) {
+
+  case EFI_BOOT_SCRIPT_IO_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemWrite (Marker);
+    VA_END (Marker); 
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteSmbusExecute (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_STALL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteStall (Marker);
+    VA_END (Marker);
+  
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch2 (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_INFORMATION_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteInformation (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemPoll (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2Write (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2ReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:  
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE: 
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfigPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfig2Poll (Marker);
+    VA_END (Marker);
+    break;    
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  return Status;
+}
+/**
+  Insert a record into a specified Framework boot script table.
+
+  This function is used to store an OpCode to be replayed as part of the S3 resume boot path. It is
+  assumed this protocol has platform specific mechanism to store the OpCode set and replay them
+  during the S3 resume.
+  The opcode is inserted before or after the specified position in the boot script table. If Position is
+  NULL then that position is after the last opcode in the table (BeforeOrAfter is FALSE) or before
+  the first opcode in the table (BeforeOrAfter is TRUE). The position which is pointed to by
+  Position upon return can be used for subsequent insertions.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptInsert (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL     *This,
+  IN       BOOLEAN                        BeforeOrAfter,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION    *Position OPTIONAL,
+  IN       UINT16                         OpCode,
+  ...
+  )
+{
+  EFI_STATUS                Status;
+  VA_LIST                   Marker;  
+  //
+  // Build script according to opcode
+  //
+  switch (OpCode) {
+
+  case EFI_BOOT_SCRIPT_IO_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemWrite (Marker);
+    VA_END (Marker); 
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteSmbusExecute (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_STALL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteStall (Marker);
+    VA_END (Marker);
+  
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch2 (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_INFORMATION_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteInformation (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemPoll (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2Write (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2ReadWrite (Marker);
+    VA_END (Marker);
+    break;
+    
+  case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:  
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE: 
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfigPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfig2Poll (Marker);
+    VA_END (Marker);
+    break;    
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+  
+  if (!EFI_ERROR (Status)) {
+   Status = S3BootScriptMoveLastOpcode (BeforeOrAfter, Position);
+  }
+  return Status;
+}
+/**
+  Find a label within the boot script table and, if not present, optionally create it.
+
+  If the label Label is already exists in the boot script table, then no new label is created, the
+  position of the Label is returned in *Position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is TRUE, then it will be
+  created before or after the specified position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is FALSE, then
+  EFI_NOT_FOUND is returned.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the label is stored before (TRUE) or after (FALSE) the position in
+                                the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new label is inserted at the beginning of the table (if TRUE) or end of
+                                the table (if FALSE).
+  @param  CreateIfNotFound      Specifies whether the label will be created if the label does not exists (TRUE) or not
+                                (FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the label will be inserted,
+                                either before or after, depending on BeforeOrAfter. On exit, specifies the position
+                                of the inserted label in the boot script table.
+  @param  Label                 Points to the label which will be inserted in the boot script table.
+
+  @retval EFI_SUCCESS           The label already exists or was inserted.
+  @retval EFI_INVALID_PARAMETER The Label is NULL or points to an empty string.
+  @retval EFI_INVALID_PARAMETER The Position is not a valid position in the boot script table.
+  
+**/
+EFI_STATUS
+EFIAPI
+BootScriptLabel (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL           *This,
+  IN       BOOLEAN                              BeforeOrAfter,
+  IN       BOOLEAN                              CreateIfNotFound,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION          *Position OPTIONAL,
+  IN CONST CHAR8                                *Label
+  )
+{
+	return S3BootScriptLabel (BeforeOrAfter, CreateIfNotFound, Position, Label);
+}
+/**
+  Compare two positions in the boot script table and return their relative position.
+  
+  This function compares two positions in the boot script table and returns their relative positions. If
+  Position1 is before Position2, then -1 is returned. If Position1 is equal to Position2,
+  then 0 is returned. If Position1 is after Position2, then 1 is returned.
+  
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  Position1             The positions in the boot script table to compare
+  @param  Position2             The positions in the boot script table to compare
+  @param  RelativePosition      On return, points to the result of the comparison
+
+  @retval EFI_SUCCESS           The operation succeeded. 
+  @retval EFI_INVALID_PARAMETER The Position1 or Position2 is not a valid position in the boot script table.
+  @retval EFI_INVALID_PARAMETER The RelativePosition is NULL.
+
+**/
+EFI_STATUS
+EFIAPI 
+BootScriptCompare (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position1,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position2,
+  OUT      UINTN                           *RelativePosition
+  )
+{
+	return S3BootScriptCompare (Position1, Position2, RelativePosition);
+}
+/**
+  This routine is entry point of ScriptSave driver.
+
+  @param  ImageHandle           Handle for this drivers loaded image protocol.
+  @param  SystemTable           EFI system table.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough resource
+  @retval EFI_SUCCESS           Succesfully installed the ScriptSave driver.
+  @retval other                 Errors occured.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeS3SaveState (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+
+
+  return  gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiS3SaveStateProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mS3SaveState
+                  );
+
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.inf b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.inf
new file mode 100644
index 0000000000..2f021ed90a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.inf
@@ -0,0 +1,59 @@
+## @file
+# S3 Boot Script Save State driver.
+#
+# It will install S3 Save State protocol to store or record various IO operations to be replayed during an S3 resume.
+#
+# Copyright (c) 2009 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = S3SaveStateDxe
+  MODULE_UNI_FILE                = S3SaveStateDxe.uni
+  FILE_GUID                      = BDCE85BB-FBAA-4f4e-9264-501A2C249581
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeS3SaveState
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  S3SaveState.c
+  InternalS3SaveState.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  BaseLib
+  S3BootScriptLib
+
+[Protocols]
+  gEfiS3SaveStateProtocolGuid                ## PRODUCES
+
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  S3SaveStateDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.uni b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.uni
new file mode 100644
index 0000000000..d27e16b2aa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxeExtra.uni b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxeExtra.uni
new file mode 100644
index 0000000000..0007d0ca71
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/S3SaveStateDxe/S3SaveStateDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/InternalSmmSaveState.h b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/InternalSmmSaveState.h
new file mode 100644
index 0000000000..d6263fd727
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/InternalSmmSaveState.h
@@ -0,0 +1,161 @@
+/** @file
+  Internal header file for SMM S3 Boot Script Saver state driver.
+
+  Copyright (c) 2006 - 2010, 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.
+
+**/
+#ifndef _INTERNAL_SMM_S3_SAVE_STATE_H_
+#define _INTERNAL_SMM_S3_SAVE_STATE_H_
+#include <PiDxe.h>
+
+#include <Protocol/S3SmmSaveState.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/S3BootScriptLib.h>
+#include <Library/PcdLib.h>
+#include <Library/SmbusLib.h>
+#include <IndustryStandard/SmBus.h>
+/**
+  Adds a record into S3 boot script table.
+
+  This function is used to store a boot script record into a given boot
+  script table. If the table specified by TableName is nonexistent in the 
+  system, a new table will automatically be created and then the script record 
+  will be added into the new table. This function is responsible for allocating 
+  necessary memory for the script.
+
+  This function has a variable parameter list. The exact parameter list depends on 
+  the OpCode that is passed into the function. If an unsupported OpCode or illegal 
+  parameter list is passed in, this function returns EFI_INVALID_PARAMETER.
+  If there are not enough resources available for storing more scripts, this function returns
+  EFI_OUT_OF_RESOURCES.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptWrite (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       UINT16                           OpCode,
+  ...
+  );
+/**
+  Insert a record into a specified Framework boot script table.
+
+  This function is used to store an OpCode to be replayed as part of the S3 resume boot path. It is
+  assumed this protocol has platform specific mechanism to store the OpCode set and replay them
+  during the S3 resume.
+  The opcode is inserted before or after the specified position in the boot script table. If Position is
+  NULL then that position is after the last opcode in the table (BeforeOrAfter is FALSE) or before
+  the first opcode in the table (BeforeOrAfter is TRUE). The position which is pointed to by
+  Position upon return can be used for subsequent insertions.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptInsert (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       BOOLEAN                          BeforeOrAfter,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION     *Position OPTIONAL,
+  IN       UINT16                           OpCode,
+  ...
+  );
+/**
+  Find a label within the boot script table and, if not present, optionally create it.
+
+  If the label Label is already exists in the boot script table, then no new label is created, the
+  position of the Label is returned in *Position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is TRUE, then it will be
+  created before or after the specified position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is FALSE, then
+  EFI_NOT_FOUND is returned.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the label is stored before (TRUE) or after (FALSE) the position in
+                                the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new label is inserted at the beginning of the table (if TRUE) or end of
+                                the table (if FALSE).
+  @param  CreateIfNotFound      Specifies whether the label will be created if the label does not exists (TRUE) or not
+                                (FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the label will be inserted,
+                                either before or after, depending on BeforeOrAfter. On exit, specifies the position
+                                of the inserted label in the boot script table.
+  @param  Label                 Points to the label which will be inserted in the boot script table.
+
+  @retval EFI_SUCCESS           The label already exists or was inserted.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  
+**/
+EFI_STATUS
+EFIAPI
+BootScriptLabel (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL           *This,
+  IN       BOOLEAN                               BeforeOrAfter,
+  IN       BOOLEAN                               CreateIfNotFound,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION          *Position OPTIONAL,
+  IN CONST CHAR8                                *Label
+  );
+/**
+  Compare two positions in the boot script table and return their relative position.
+  
+  This function compares two positions in the boot script table and returns their relative positions. If
+  Position1 is before Position2, then -1 is returned. If Position1 is equal to Position2,
+  then 0 is returned. If Position1 is after Position2, then 1 is returned.
+  
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  Position1             The positions in the boot script table to compare
+  @param  Position2             The positions in the boot script table to compare
+  @param  RelativePosition      On return, points to the result of the comparison
+
+  @retval EFI_SUCCESS           The operation succeeded. 
+  @retval EFI_INVALID_PARAMETER The Position1 or Position2 is not a valid position in the boot script table.
+
+**/
+EFI_STATUS
+EFIAPI 
+BootScriptCompare (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position1,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position2,
+  OUT      UINTN                           *RelativePosition
+  );
+  
+#endif //_INTERNAL_SMM_S3_SAVE_STATE_H_
diff --git a/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.c b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.c
new file mode 100644
index 0000000000..e4227282c9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.c
@@ -0,0 +1,917 @@
+/** @file
+  Implementation for S3 SMM Boot Script Saver state driver.
+
+  Copyright (c) 2010 - 2013, 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 "InternalSmmSaveState.h"
+
+EFI_S3_SMM_SAVE_STATE_PROTOCOL    mS3SmmSaveState = {
+   BootScriptWrite,
+   BootScriptInsert,
+   BootScriptLabel,
+   BootScriptCompare
+  };
+/**
+  Internal function to add IO write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteIoWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+  
+  return S3BootScriptSaveIoWrite (Width, Address, Count, Buffer);
+}
+/**
+  Internal function to add IO read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteIoReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+  
+  return S3BootScriptSaveIoReadWrite (Width, Address, Data, DataMask);
+}
+
+/**
+  Internal function to add memory write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSaveMemWrite (Width, Address, Count, Buffer);
+}
+
+/**
+  Internal function to add memory read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+  
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSaveMemReadWrite (Width, Address, Data, DataMask);
+}
+
+/**
+  Internal function to add PciCfg write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfgWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSavePciCfgWrite (Width, Address, Count, Buffer);
+}
+
+/**
+  Internal function to PciCfg read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfgReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+
+  return S3BootScriptSavePciCfgReadWrite (Width, Address, Data, DataMask);
+}
+/**
+  Internal function to add PciCfg2 write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfg2Write (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT64                Address;
+  UINTN                 Count;
+  UINT8                 *Buffer;
+  UINT16                Segment;
+
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Count       = VA_ARG (Marker, UINTN);
+  Buffer      = VA_ARG (Marker, UINT8 *);
+  Segment     = VA_ARG (Marker, UINT16);
+
+  return S3BootScriptSavePciCfg2Write (Width, Segment, Address, Count, Buffer);
+}
+
+/**
+  Internal function to PciCfg2 read/write opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWritePciCfg2ReadWrite (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH Width;
+  UINT16                Segment;
+  UINT64                Address;
+  UINT8                 *Data;
+  UINT8                 *DataMask;
+ 
+  Width       = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);
+  Address     = VA_ARG (Marker, UINT64);
+  Segment     = VA_ARG (Marker, UINT16);
+  Data        = VA_ARG (Marker, UINT8 *);
+  DataMask    = VA_ARG (Marker, UINT8 *);
+ 
+  return S3BootScriptSavePciCfg2ReadWrite (Width, Segment, Address, Data, DataMask);
+}
+/**
+  Internal function to add smbus excute opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteSmbusExecute (
+  IN VA_LIST                       Marker
+  )
+{
+  EFI_SMBUS_DEVICE_ADDRESS  SlaveAddress;
+  EFI_SMBUS_DEVICE_COMMAND  Command;
+  EFI_SMBUS_OPERATION       Operation;
+  BOOLEAN                   PecCheck;
+  VOID                     *Buffer;
+  UINTN                    *DataSize;
+  UINTN                     SmBusAddress;
+  
+  SlaveAddress.SmbusDeviceAddress = VA_ARG (Marker, UINTN);
+  Command                         = VA_ARG (Marker, EFI_SMBUS_DEVICE_COMMAND);
+  Operation                       = VA_ARG (Marker, EFI_SMBUS_OPERATION);
+  PecCheck                        = VA_ARG (Marker, BOOLEAN);
+  SmBusAddress                    = SMBUS_LIB_ADDRESS (SlaveAddress.SmbusDeviceAddress,Command,0,PecCheck);
+  DataSize                        = VA_ARG (Marker, UINTN *);    
+  Buffer                          = VA_ARG (Marker, VOID *);
+ 
+  return S3BootScriptSaveSmbusExecute (SmBusAddress, Operation, DataSize, Buffer);
+}
+/**
+  Internal function to add stall opcode to the table.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteStall (
+  IN VA_LIST                       Marker
+  )
+{
+  UINT32                Duration;
+
+  Duration    = VA_ARG (Marker, UINT32);
+
+  return S3BootScriptSaveStall (Duration);
+}
+
+/**
+  Internal function to add Save jmp address according to DISPATCH_OPCODE. 
+  We ignore "Context" parameter
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteDispatch (
+  IN VA_LIST                       Marker
+  )
+{
+  VOID        *EntryPoint;
+
+  EntryPoint = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  return S3BootScriptSaveDispatch (EntryPoint);
+}
+
+/**
+  Internal function to add memory pool operation to the table. 
+ 
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteMemPoll (
+  IN VA_LIST                       Marker
+  )
+{
+  S3_BOOT_SCRIPT_LIB_WIDTH   Width;                                   
+  UINT64                     Address;                                    
+  VOID                      *Data;                                       
+  VOID                      *DataMask;                                   
+  UINTN                      Delay;                                       
+  UINTN                      LoopTimes;
+  UINT32                     Remainder;
+
+  Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);                  
+  Address  = VA_ARG (Marker, UINT64);                                    
+  Data     = VA_ARG (Marker, VOID *);                                    
+  DataMask = VA_ARG (Marker, VOID *);                                    
+  Delay    = (UINTN)VA_ARG (Marker, UINT64);                            
+  //
+  // According to the spec, the interval between 2 polls is 100ns,
+  // but the unit of Duration for S3BootScriptSaveMemPoll() is microsecond(1000ns).
+  // Duration * 1000ns * LoopTimes = Delay * 100ns
+  // Duration will be minimum 1(microsecond) to be minimum deviation,
+  // so LoopTimes = Delay / 10.
+  //
+  LoopTimes = (UINTN) DivU64x32Remainder (
+                Delay,
+                10,
+                &Remainder
+                );
+  if (Remainder != 0) {
+    //
+    // If Remainder is not zero, LoopTimes will be rounded up by 1.
+    //
+    LoopTimes +=1;
+  }
+  return S3BootScriptSaveMemPoll (Width, Address, DataMask, Data, 1, LoopTimes); 
+
+}
+
+/**
+  Internal function to add Save jmp address according to DISPATCH_OPCODE2. 
+  The "Context" parameter is not ignored.
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enough resource to do operation.
+  @retval EFI_SUCCESS           Opcode is added.
+
+**/
+EFI_STATUS
+BootScriptWriteDispatch2 (
+  IN VA_LIST                       Marker
+  )
+{
+  VOID                  *EntryPoint;
+  VOID                  *Context;  
+
+  EntryPoint = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  Context    = (VOID*)(UINTN)VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+
+  return S3BootScriptSaveDispatch2 (EntryPoint, Context);
+}
+/**
+  Internal function to add INFORAMTION opcode node to the table
+  list.
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWriteInformation (
+  IN VA_LIST                       Marker
+  )
+{
+  UINT32                InformationLength;
+  EFI_PHYSICAL_ADDRESS  Information;  
+
+  InformationLength = VA_ARG (Marker, UINT32);
+  Information = VA_ARG (Marker, EFI_PHYSICAL_ADDRESS);
+  return S3BootScriptSaveInformation (InformationLength, (VOID*)(UINTN)Information);
+}
+/**
+  Internal function to add IO poll opcode node  to the table
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWriteIoPoll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH     Width;  
+   UINT64                     Address;    
+   VOID                      *Data;       
+   VOID                      *DataMask;   
+   UINT64                     Delay;       
+   
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSaveIoPoll (Width, Address, Data, DataMask, Delay);
+}
+/**
+  Internal function to add PCI config poll opcode node to the table
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWritePciConfigPoll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH   Width;
+   UINT64                     Address;
+   VOID                      *Data;
+   VOID                      *DataMask;
+   UINT64                     Delay;
+
+   
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSavePciPoll (Width, Address, Data, DataMask, Delay);
+}
+/**
+  Internal function to add PCI config 2 poll opcode node to the table
+
+  @param  Marker                The variable argument list to get the opcode
+                                and associated attributes.
+
+  @retval EFI_OUT_OF_RESOURCES  Not enought resource to complete the operations.
+  @retval EFI_SUCCESS           The opcode entry is added to the  table
+                                successfully.
+**/
+EFI_STATUS
+BootScriptWritePciConfig2Poll (
+  IN VA_LIST                       Marker
+  )
+{
+   S3_BOOT_SCRIPT_LIB_WIDTH      Width;
+   UINT16                        Segment;
+   UINT64                        Address;
+   VOID                         *Data;
+   VOID                         *DataMask;
+   UINT64                        Delay;
+  
+   Width    = VA_ARG (Marker, S3_BOOT_SCRIPT_LIB_WIDTH);  
+   Segment  = VA_ARG (Marker, UINT16);      
+   Address  = VA_ARG (Marker, UINT64);                    
+   Data     = VA_ARG (Marker, VOID *);                   
+   DataMask = VA_ARG (Marker, VOID *);                   
+   Delay    = (UINT64)VA_ARG (Marker, UINT64);             
+   
+   return S3BootScriptSavePci2Poll (Width, Segment, Address, Data, DataMask, Delay);
+}
+
+/**
+  Adds a record into S3 boot script table.
+
+  This function is used to store a boot script record into a given boot
+  script table. If the table specified by TableName is nonexistent in the 
+  system, a new table will automatically be created and then the script record 
+  will be added into the new table. This function is responsible for allocating 
+  necessary memory for the script.
+
+  This function has a variable parameter list. The exact parameter list depends on 
+  the OpCode that is passed into the function. If an unsupported OpCode or illegal 
+  parameter list is passed in, this function returns EFI_INVALID_PARAMETER.
+  If there are not enough resources available for storing more scripts, this function returns
+  EFI_OUT_OF_RESOURCES.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+  
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The parameter is illegal or the given boot script is not supported.
+                                If the opcode is unknow or not supported because of the PCD 
+                                Feature Flags.
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptWrite (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL         *This,
+  IN UINT16                                    OpCode,
+  ...
+  )
+{
+  EFI_STATUS                Status;
+  VA_LIST                   Marker;  
+  //
+  // Build script according to opcode
+  //
+  switch (OpCode) {
+
+  case EFI_BOOT_SCRIPT_IO_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemWrite (Marker);
+    VA_END (Marker); 
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteSmbusExecute (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_STALL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteStall (Marker);
+    VA_END (Marker);
+  
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch2 (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_INFORMATION_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteInformation (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemPoll (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2Write (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2ReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:  
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE: 
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfigPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfig2Poll (Marker);
+    VA_END (Marker);
+    break;    
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  return Status;
+}
+/**
+  Insert a record into a specified Framework boot script table.
+
+  This function is used to store an OpCode to be replayed as part of the S3 resume boot path. It is
+  assumed this protocol has platform specific mechanism to store the OpCode set and replay them
+  during the S3 resume.
+  The opcode is inserted before or after the specified position in the boot script table. If Position is
+  NULL then that position is after the last opcode in the table (BeforeOrAfter is FALSE) or before
+  the first opcode in the table (BeforeOrAfter is TRUE). The position which is pointed to by
+  Position upon return can be used for subsequent insertions.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the opcode is stored before (TRUE) or after (FALSE) the position
+                                in the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new opcode is inserted at the beginning of the table (if TRUE) or end
+                                of the table (if FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the opcode will be
+                                inserted, either before or after, depending on BeforeOrAfter. On exit, specifies
+                                the position of the inserted opcode in the boot script table.
+  @param  OpCode                The operation code (opcode) number.
+  @param  ...                   Argument list that is specific to each opcode. 
+
+  @retval EFI_SUCCESS           The operation succeeded. A record was added into the
+                                specified script table.
+  @retval EFI_INVALID_PARAMETER The Opcode is an invalid opcode value or the Position is not a valid position in the boot script table..
+  @retval EFI_OUT_OF_RESOURCES  There is insufficient memory to store the boot script.
+
+**/
+EFI_STATUS
+EFIAPI
+BootScriptInsert (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL    *This,
+  IN       BOOLEAN                          BeforeOrAfter,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION     *Position OPTIONAL,
+  IN       UINT16                           OpCode,
+  ...
+  )
+{
+  EFI_STATUS                Status;
+  VA_LIST                   Marker;  
+  //
+  // Build script according to opcode
+  //
+  switch (OpCode) {
+
+  case EFI_BOOT_SCRIPT_IO_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_IO_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemWrite (Marker);
+    VA_END (Marker); 
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfgReadWrite (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_SMBUS_EXECUTE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteSmbusExecute (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_STALL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteStall (Marker);
+    VA_END (Marker);
+  
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_DISPATCH_2_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteDispatch2 (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_INFORMATION_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteInformation (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_MEM_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteMemPoll (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2Write (Marker);
+    VA_END (Marker);
+    break;
+
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_READ_WRITE_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciCfg2ReadWrite (Marker);
+    VA_END (Marker);
+    break;
+    
+  case EFI_BOOT_SCRIPT_IO_POLL_OPCODE:  
+    VA_START (Marker, OpCode);
+    Status = BootScriptWriteIoPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG_POLL_OPCODE: 
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfigPoll (Marker);
+    VA_END (Marker);
+    break;    
+  
+  case EFI_BOOT_SCRIPT_PCI_CONFIG2_POLL_OPCODE:
+    VA_START (Marker, OpCode);
+    Status = BootScriptWritePciConfig2Poll (Marker);
+    VA_END (Marker);
+    break;    
+
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+  
+  if (!EFI_ERROR (Status)) {
+   Status = S3BootScriptMoveLastOpcode (BeforeOrAfter, Position);
+  }
+  return Status;
+}
+/**
+  Find a label within the boot script table and, if not present, optionally create it.
+
+  If the label Label is already exists in the boot script table, then no new label is created, the
+  position of the Label is returned in *Position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is TRUE, then it will be
+  created before or after the specified position and EFI_SUCCESS is returned.
+  If the label Label does not already exist and CreateIfNotFound is FALSE, then
+  EFI_NOT_FOUND is returned.
+
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  BeforeOrAfter         Specifies whether the label is stored before (TRUE) or after (FALSE) the position in
+                                the boot script table specified by Position. If Position is NULL or points to
+                                NULL then the new label is inserted at the beginning of the table (if TRUE) or end of
+                                the table (if FALSE).
+  @param  CreateIfNotFound      Specifies whether the label will be created if the label does not exists (TRUE) or not
+                                (FALSE).
+  @param  Position              On entry, specifies the position in the boot script table where the label will be inserted,
+                                either before or after, depending on BeforeOrAfter. On exit, specifies the position
+                                of the inserted label in the boot script table.
+  @param  Label                 Points to the label which will be inserted in the boot script table.
+
+  @retval EFI_SUCCESS           The label already exists or was inserted.
+  @retval EFI_INVALID_PARAMETER The Label is NULL or points to an empty string.
+  @retval EFI_INVALID_PARAMETER The Position is not a valid position in the boot script table.
+  
+**/
+EFI_STATUS
+EFIAPI
+BootScriptLabel (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL           *This,
+  IN       BOOLEAN                               BeforeOrAfter,
+  IN       BOOLEAN                               CreateIfNotFound,
+  IN OUT   EFI_S3_BOOT_SCRIPT_POSITION          *Position OPTIONAL,
+  IN CONST CHAR8                                *Label
+  )
+{
+	return S3BootScriptLabel (BeforeOrAfter, CreateIfNotFound, Position, Label);
+}
+/**
+  Compare two positions in the boot script table and return their relative position.
+  
+  This function compares two positions in the boot script table and returns their relative positions. If
+  Position1 is before Position2, then -1 is returned. If Position1 is equal to Position2,
+  then 0 is returned. If Position1 is after Position2, then 1 is returned.
+  
+  @param  This                  A pointer to the EFI_S3_SAVE_STATE_PROTOCOL instance.
+  @param  Position1             The positions in the boot script table to compare
+  @param  Position2             The positions in the boot script table to compare
+  @param  RelativePosition      On return, points to the result of the comparison
+
+  @retval EFI_SUCCESS           The operation succeeded. 
+  @retval EFI_INVALID_PARAMETER The Position1 or Position2 is not a valid position in the boot script table.
+  @retval EFI_INVALID_PARAMETER The RelativePosition is NULL.
+
+**/
+EFI_STATUS
+EFIAPI 
+BootScriptCompare (
+  IN CONST EFI_S3_SAVE_STATE_PROTOCOL      *This,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position1,
+  IN       EFI_S3_BOOT_SCRIPT_POSITION      Position2,
+  OUT      UINTN                           *RelativePosition
+  )
+{
+	return S3BootScriptCompare (Position1, Position2, RelativePosition);
+}
+/**
+  This routine is entry point of ScriptSave driver.
+
+  @param  ImageHandle           Handle for this drivers loaded image protocol.
+  @param  SystemTable           EFI system table.
+
+  @retval EFI_OUT_OF_RESOURCES  No enough resource
+  @retval EFI_SUCCESS           Succesfully installed the ScriptSave driver.
+  @retval other                 Errors occured.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeSmmS3SaveState (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_HANDLE   Handle;
+  
+
+  Handle  = NULL;
+  return  gSmst->SmmInstallProtocolInterface (
+                   &Handle,
+                   &gEfiS3SmmSaveStateProtocolGuid,
+                   EFI_NATIVE_INTERFACE,
+                   &mS3SmmSaveState
+                   );
+}
diff --git a/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.inf b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.inf
new file mode 100644
index 0000000000..be9a5619fe
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.inf
@@ -0,0 +1,58 @@
+## @file
+# S3 SMM Boot Script Save State driver.
+#
+# It will install S3 SMM Save State protocol to store or record various IO operations to be replayed during an S3 resume.
+#
+# Copyright (c) 2009 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmS3SaveState
+  MODULE_UNI_FILE                = SmmS3SaveState.uni
+  FILE_GUID                      = 2D59F041-53A4-40d0-A6CD-844DC0DFEF17
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  
+  ENTRY_POINT                    = InitializeSmmS3SaveState
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  SmmS3SaveState.c
+  InternalSmmSaveState.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SmmServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  BaseLib
+  S3BootScriptLib
+
+[Protocols]
+  gEfiS3SmmSaveStateProtocolGuid                ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmmS3SaveStateExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.uni b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.uni
new file mode 100644
index 0000000000..ab1b4901b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveState.uni
diff --git a/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveStateExtra.uni b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveStateExtra.uni
new file mode 100644
index 0000000000..11a636c144
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Acpi/SmmS3SaveState/SmmS3SaveStateExtra.uni
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/Bds.h b/Core/MdeModulePkg/Universal/BdsDxe/Bds.h
new file mode 100644
index 0000000000..2171d14791
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/Bds.h
@@ -0,0 +1,106 @@
+/** @file
+  Head file for BDS Architectural Protocol implementation
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _BDS_MODULE_H_
+#define _BDS_MODULE_H_
+
+#include <Uefi.h>
+#include <Guid/GlobalVariable.h>
+#include <Guid/ConnectConInEvent.h>
+#include <Guid/Performance.h>
+#include <Guid/StatusCodeDataTypeVariable.h>
+
+#include <Protocol/Bds.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/VariableLock.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/LoadFile.h>
+#include <Protocol/SimpleFileSystem.h>
+
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PerformanceLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PrintLib.h>
+
+#include <Library/UefiBootManagerLib.h>
+#include <Library/PlatformBootManagerLib.h>
+
+/**
+
+  Service routine for BdsInstance->Entry(). Devices are connected, the
+  consoles are initialized, and the boot options are tried.
+
+  @param This            Protocol Instance structure.
+
+**/
+VOID
+EFIAPI
+BdsEntry (
+  IN  EFI_BDS_ARCH_PROTOCOL *This
+  );
+
+/**
+  Set the variable and report the error through status code upon failure.
+
+  @param  VariableName           A Null-terminated string that is the name of the vendor's variable.
+                                 Each VariableName is unique for each VendorGuid. VariableName must
+                                 contain 1 or more characters. If VariableName is an empty string,
+                                 then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             Attributes bitmask to set for the variable.
+  @param  DataSize               The size in bytes of the Data buffer. Unless the EFI_VARIABLE_APPEND_WRITE, 
+                                 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS, or 
+                                 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS attribute is set, a size of zero 
+                                 causes the variable to be deleted. When the EFI_VARIABLE_APPEND_WRITE attribute is 
+                                 set, then a SetVariable() call with a DataSize of zero will not cause any change to 
+                                 the variable value (the timestamp associated with the variable may be updated however 
+                                 even if no new data value is provided,see the description of the 
+                                 EFI_VARIABLE_AUTHENTICATION_2 descriptor below. In this case the DataSize will not 
+                                 be zero since the EFI_VARIABLE_AUTHENTICATION_2 descriptor will be populated). 
+  @param  Data                   The contents for the variable.
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits, name, and GUID was supplied, or the
+                                 DataSize exceeds the maximum allowed.
+  @retval EFI_INVALID_PARAMETER  VariableName is an empty string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be retrieved due to a hardware error.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only.
+  @retval EFI_WRITE_PROTECTED    The variable in question cannot be deleted.
+  @retval EFI_SECURITY_VIOLATION The variable could not be written due to EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS 
+                                 or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS being set, but the AuthInfo 
+                                 does NOT pass the validation check carried out by the firmware.
+
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+**/
+EFI_STATUS
+BdsDxeSetVariableAndReportStatusCodeOnError (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.inf b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.inf
new file mode 100644
index 0000000000..12d61e79c9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.inf
@@ -0,0 +1,105 @@
+## @file
+#  BdsDxe module is core driver for BDS phase.
+#
+#  When DxeCore dispatching all DXE driver, this module will produce architecture protocol 
+#  gEfiBdsArchProtocolGuid. After DxeCore finish dispatching, DxeCore will invoke Entry
+#  interface of protocol gEfiBdsArchProtocolGuid, then BDS phase is entered.
+#  
+#  Copyright (c) 2008 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BdsDxe
+  MODULE_UNI_FILE                = BdsDxe.uni
+  FILE_GUID                      = 6D33944A-EC75-4855-A54D-809C75241F6C
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0 
+  ENTRY_POINT                    = BdsInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Language.h
+  Bds.h
+  HwErrRecSupport.c
+  HwErrRecSupport.h
+  Language.c
+  BdsEntry.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  BaseLib
+  MemoryAllocationLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  ReportStatusCodeLib
+  UefiLib
+  BaseMemoryLib
+  DebugLib
+  UefiBootManagerLib
+  PlatformBootManagerLib
+  PcdLib
+
+[Guids]
+  gEfiGlobalVariableGuid                        ## SOMETIMES_PRODUCES ## Variable:L"BootNext" (The number of next boot option)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"Boot####" (Boot option variable)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"PlatformLang" (Platform supported languange in Rfc4646 format)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"Lang" (Platform supported languange in Iso639 format)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"Key####" (Hotkey option variable)
+                                                ## PRODUCES           ## Variable:L"HwErrRecSupport" (The level of platform supported hardware Error Record Persistence)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"BootOptionSupport" (The feature supported in boot option menu, value could be: EFI_BOOT_OPTION_SUPPORT_KEY, EFI_BOOT_OPTION_SUPPORT_APP
+                                                ## SOMETIMES_PRODUCES (not PcdUefiVariableDefaultLangDeprecate) ## Variable:L"LangCodes" (Value of PcdUefiVariableDefaultLangCodes)
+                                                ## PRODUCES           ## Variable:L"PlatformLangCodes" (Value of PcdUefiVariableDefaultPlatformLangCodes)
+                                                ## PRODUCES           ## Variable:L"Timeout" (The time out value in second of showing progress bar)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"BootOrder" (The boot option array)
+                                                ## SOMETIMES_PRODUCES ## Variable:L"DriverOrder" (The driver order list)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConIn" (The device path of console in device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ConOut" (The device path of console out device)
+                                                ## SOMETIMES_CONSUMES ## Variable:L"ErrOut" (The device path of error out device)
+  gConnectConInEventGuid                        ## SOMETIMES_CONSUMES ## Event
+  gEdkiiStatusCodeDataTypeVariableGuid          ## SOMETIMES_CONSUMES ## GUID
+
+[Protocols]
+  gEfiBdsArchProtocolGuid                       ## PRODUCES
+  gEfiSimpleTextInputExProtocolGuid             ## CONSUMES
+  gEdkiiVariableLockProtocolGuid                ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLangDeprecate    ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLangCodes            ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLang                 ## SOMETIMES_CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLangCodes    ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLang         ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdHardwareErrorRecordLevel                ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdPlatformBootTimeOut                     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareVendor                    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFirmwareRevision                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConInConnectOnDemand              ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdErrorCodeSetVariable              ## SOMETIMES_CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  BdsDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.uni b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.uni
new file mode 100644
index 0000000000..dcaa13ac22
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxe.uni
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/BdsDxeExtra.uni b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxeExtra.uni
new file mode 100644
index 0000000000..170a4f45bb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/BdsDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/BdsEntry.c b/Core/MdeModulePkg/Universal/BdsDxe/BdsEntry.c
new file mode 100644
index 0000000000..c8898925e6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/BdsEntry.c
@@ -0,0 +1,1230 @@
+/** @file
+  This module produce main entry for BDS phase - BdsEntry.
+  When this module was dispatched by DxeCore, gEfiBdsArchProtocolGuid will be installed
+  which contains interface of BdsEntry.
+  After DxeCore finish DXE phase, gEfiBdsArchProtocolGuid->BdsEntry will be invoked
+  to enter BDS phase.
+
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2004 - 2015, 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 "Bds.h"
+#include "Language.h"
+#include "HwErrRecSupport.h"
+
+#define SET_BOOT_OPTION_SUPPORT_KEY_COUNT(a, c) {  \
+      (a) = ((a) & ~EFI_BOOT_OPTION_SUPPORT_COUNT) | (((c) << LowBitSet32 (EFI_BOOT_OPTION_SUPPORT_COUNT)) & EFI_BOOT_OPTION_SUPPORT_COUNT); \
+      }
+
+///
+/// BDS arch protocol instance initial value.
+///
+EFI_BDS_ARCH_PROTOCOL  gBds = {
+  BdsEntry
+};
+
+//
+// gConnectConInEvent - Event which is signaled when ConIn connection is required
+//
+EFI_EVENT      gConnectConInEvent = NULL;
+
+///
+/// The read-only variables defined in UEFI Spec.
+///
+CHAR16  *mReadOnlyVariables[] = {
+  EFI_PLATFORM_LANG_CODES_VARIABLE_NAME,
+  EFI_LANG_CODES_VARIABLE_NAME,
+  EFI_BOOT_OPTION_SUPPORT_VARIABLE_NAME,
+  EFI_HW_ERR_REC_SUPPORT_VARIABLE_NAME,
+  EFI_OS_INDICATIONS_SUPPORT_VARIABLE_NAME
+  };
+
+CHAR16 *mBdsLoadOptionName[] = {
+  L"Driver",
+  L"SysPrep",
+  L"Boot"
+};
+
+CHAR16  mRecoveryBoot[] = L"Recovery Boot";
+/**
+  Event to Connect ConIn.
+
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               Pointer to the notification function's context,
+                                which is implementation-dependent.
+
+**/
+VOID
+EFIAPI
+BdsDxeOnConnectConInCallBack (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  EFI_STATUS Status;
+
+  //
+  // When Osloader call ReadKeyStroke to signal this event
+  // no driver dependency is assumed existing. So use a non-dispatch version
+  //
+  Status = EfiBootManagerConnectConsoleVariable (ConIn);
+  if (EFI_ERROR (Status)) {
+    //
+    // Should not enter this case, if enter, the keyboard will not work.
+    // May need platfrom policy to connect keyboard.
+    //
+    DEBUG ((EFI_D_WARN, "[Bds] Connect ConIn failed - %r!!!\n", Status));
+  }
+}
+
+/**
+
+  Install Boot Device Selection Protocol
+
+  @param ImageHandle     The image handle.
+  @param SystemTable     The system table.
+
+  @retval  EFI_SUCEESS  BDS has finished initializing.
+                        Return the dispatcher and recall BDS.Entry
+  @retval  Other        Return status from AllocatePool() or gBS->InstallProtocolInterface
+
+**/
+EFI_STATUS
+EFIAPI
+BdsInitialize (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  Handle;
+  //
+  // Install protocol interface
+  //
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiBdsArchProtocolGuid, &gBds,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Emuerate all possible bootable medias in the following order:
+  1. Removable BlockIo            - The boot option only points to the removable media
+                                    device, like USB key, DVD, Floppy etc.
+  2. Fixed BlockIo                - The boot option only points to a Fixed blockIo device,
+                                    like HardDisk.
+  3. Non-BlockIo SimpleFileSystem - The boot option points to a device supporting
+                                    SimpleFileSystem Protocol, but not supporting BlockIo
+                                    protocol.
+  4. LoadFile                     - The boot option points to the media supporting 
+                                    LoadFile protocol.
+  Reference: UEFI Spec chapter 3.3 Boot Option Variables Default Boot Behavior
+
+  @param BootOptionCount   Return the boot option count which has been found.
+
+  @retval   Pointer to the boot option array.
+**/
+EFI_BOOT_MANAGER_LOAD_OPTION *
+BdsEnumerateBootOptions (
+  UINTN                                 *BootOptionCount
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_BOOT_MANAGER_LOAD_OPTION          *BootOptions;
+  UINTN                                 HandleCount;
+  EFI_HANDLE                            *Handles;
+  EFI_BLOCK_IO_PROTOCOL                 *BlkIo;
+  UINTN                                 Removable;
+  UINTN                                 Index;
+
+  ASSERT (BootOptionCount != NULL);
+
+  *BootOptionCount = 0;
+  BootOptions      = NULL;
+
+  //
+  // Parse removable block io followed by fixed block io
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiBlockIoProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+
+  for (Removable = 0; Removable < 2; Removable++) {
+    for (Index = 0; Index < HandleCount; Index++) {
+      Status = gBS->HandleProtocol (
+                      Handles[Index],
+                      &gEfiBlockIoProtocolGuid,
+                      (VOID **) &BlkIo
+                      );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+
+      //
+      // Skip the logical partitions
+      //
+      if (BlkIo->Media->LogicalPartition) {
+        continue;
+      }
+
+      //
+      // Skip the fixed block io then the removable block io
+      //
+      if (BlkIo->Media->RemovableMedia == ((Removable == 0) ? FALSE : TRUE)) {
+        continue;
+      }
+
+      BootOptions = ReallocatePool (
+                      sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                      sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                      BootOptions
+                      );
+      ASSERT (BootOptions != NULL);
+
+      Status = EfiBootManagerInitializeLoadOption (
+                 &BootOptions[(*BootOptionCount)++],
+                 LoadOptionNumberUnassigned,
+                 LoadOptionTypeBoot,
+                 LOAD_OPTION_ACTIVE,
+                 mRecoveryBoot,
+                 DevicePathFromHandle (Handles[Index]),
+                 NULL,
+                 0
+                 );
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  //
+  // Parse simple file system not based on block io
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiSimpleFileSystemProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (
+                    Handles[Index],
+                    &gEfiBlockIoProtocolGuid,
+                    (VOID **) &BlkIo
+                    );
+     if (!EFI_ERROR (Status)) {
+      //
+      //  Skip if the file system handle supports a BlkIo protocol, which we've handled in above
+      //
+      continue;
+    }
+    BootOptions = ReallocatePool (
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                    BootOptions
+                    );
+    ASSERT (BootOptions != NULL);
+
+    Status = EfiBootManagerInitializeLoadOption (
+               &BootOptions[(*BootOptionCount)++],
+               LoadOptionNumberUnassigned,
+               LoadOptionTypeBoot,
+               LOAD_OPTION_ACTIVE,
+               mRecoveryBoot,
+               DevicePathFromHandle (Handles[Index]),
+               NULL,
+               0
+               );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  //
+  // Parse load file, assuming UEFI Network boot option
+  //
+  gBS->LocateHandleBuffer (
+         ByProtocol,
+         &gEfiLoadFileProtocolGuid,
+         NULL,
+         &HandleCount,
+         &Handles
+         );
+  for (Index = 0; Index < HandleCount; Index++) {
+
+    BootOptions = ReallocatePool (
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount),
+                    sizeof (EFI_BOOT_MANAGER_LOAD_OPTION) * (*BootOptionCount + 1),
+                    BootOptions
+                    );
+    ASSERT (BootOptions != NULL);
+
+    Status = EfiBootManagerInitializeLoadOption (
+               &BootOptions[(*BootOptionCount)++],
+               LoadOptionNumberUnassigned,
+               LoadOptionTypeBoot,
+               LOAD_OPTION_ACTIVE,
+               mRecoveryBoot,
+               DevicePathFromHandle (Handles[Index]),
+               NULL,
+               0
+               );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (HandleCount != 0) {
+    FreePool (Handles);
+  }
+
+  return BootOptions;
+}
+
+/**
+  Function waits for a given event to fire, or for an optional timeout to expire.
+
+  @param   Event              The event to wait for
+  @param   Timeout            An optional timeout value in 100 ns units.
+
+  @retval  EFI_SUCCESS      Event fired before Timeout expired.
+  @retval  EFI_TIME_OUT     Timout expired before Event fired..
+
+**/
+EFI_STATUS
+BdsWaitForSingleEvent (
+  IN  EFI_EVENT                  Event,
+  IN  UINT64                     Timeout       OPTIONAL
+  )
+{
+  UINTN       Index;
+  EFI_STATUS  Status;
+  EFI_EVENT   TimerEvent;
+  EFI_EVENT   WaitList[2];
+
+  if (Timeout != 0) {
+    //
+    // Create a timer event
+    //
+    Status = gBS->CreateEvent (EVT_TIMER, 0, NULL, NULL, &TimerEvent);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Set the timer event
+      //
+      gBS->SetTimer (
+             TimerEvent,
+             TimerRelative,
+             Timeout
+             );
+
+      //
+      // Wait for the original event or the timer
+      //
+      WaitList[0] = Event;
+      WaitList[1] = TimerEvent;
+      Status      = gBS->WaitForEvent (2, WaitList, &Index);
+      ASSERT_EFI_ERROR (Status);
+      gBS->CloseEvent (TimerEvent);
+
+      //
+      // If the timer expired, change the return to timed out
+      //
+      if (Index == 1) {
+        Status = EFI_TIMEOUT;
+      }
+    }
+  } else {
+    //
+    // No timeout... just wait on the event
+    //
+    Status = gBS->WaitForEvent (1, &Event, &Index);
+    ASSERT (!EFI_ERROR (Status));
+    ASSERT (Index == 0);
+  }
+
+  return Status;
+}
+
+/**
+  The function reads user inputs.
+
+**/
+VOID
+BdsReadKeys (
+  VOID
+  )
+{
+  EFI_STATUS         Status;
+  EFI_INPUT_KEY      Key;
+
+  if (PcdGetBool (PcdConInConnectOnDemand)) {
+    return;
+  }
+
+  while (gST->ConIn != NULL) {
+    
+    Status = gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+    
+    if (EFI_ERROR (Status)) {
+      //
+      // No more keys.
+      //
+      break;
+    }
+  }
+}
+
+/**
+  The function waits for the boot manager timeout expires or hotkey is pressed.
+
+  It calls PlatformBootManagerWaitCallback each second.
+
+  @param     HotkeyTriggered   Input hotkey event.
+**/
+VOID
+BdsWait (
+  IN EFI_EVENT      HotkeyTriggered
+  )
+{
+  EFI_STATUS            Status;
+  UINT16                TimeoutRemain;
+
+  DEBUG ((EFI_D_INFO, "[Bds]BdsWait ...Zzzzzzzzzzzz...\n"));
+
+  TimeoutRemain = PcdGet16 (PcdPlatformBootTimeOut);
+  while (TimeoutRemain != 0) {
+    DEBUG ((EFI_D_INFO, "[Bds]BdsWait(%d)..Zzzz...\n", (UINTN) TimeoutRemain));
+    PlatformBootManagerWaitCallback (TimeoutRemain);
+
+    BdsReadKeys (); // BUGBUG: Only reading can signal HotkeyTriggered
+                    //         Can be removed after all keyboard drivers invoke callback in timer callback.
+
+    if (HotkeyTriggered != NULL) {
+      Status = BdsWaitForSingleEvent (HotkeyTriggered, EFI_TIMER_PERIOD_SECONDS (1));
+      if (!EFI_ERROR (Status)) {
+        break;
+      }
+    } else {
+      gBS->Stall (1000000);
+    }
+
+    //
+    // 0xffff means waiting forever
+    // BDS with no hotkey provided and 0xffff as timeout will "hang" in the loop
+    //
+    if (TimeoutRemain != 0xffff) {
+      TimeoutRemain--;
+    }
+  }
+  DEBUG ((EFI_D_INFO, "[Bds]Exit the waiting!\n"));
+}
+
+/**
+  Attempt to boot each boot option in the BootOptions array.
+
+  @param BootOptions       Input boot option array.
+  @param BootOptionCount   Input boot option count.
+
+  @retval TRUE  Successfully boot one of the boot options.
+  @retval FALSE Failed boot any of the boot options.
+**/
+BOOLEAN
+BootAllBootOptions (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION    *BootOptions,
+  IN UINTN                           BootOptionCount
+  )
+{
+  UINTN                              Index;
+
+  //
+  // Attempt boot each boot option
+  //
+  for (Index = 0; Index < BootOptionCount; Index++) {
+    //
+    // According to EFI Specification, if a load option is not marked
+    // as LOAD_OPTION_ACTIVE, the boot manager will not automatically
+    // load the option.
+    //
+    if ((BootOptions[Index].Attributes & LOAD_OPTION_ACTIVE) == 0) {
+      continue;
+    }
+
+    //
+    // Boot#### load options with LOAD_OPTION_CATEGORY_APP are executables which are not
+    // part of the normal boot processing. Boot options with reserved category values will be
+    // ignored by the boot manager.
+    //
+    if ((BootOptions[Index].Attributes & LOAD_OPTION_CATEGORY) != LOAD_OPTION_CATEGORY_BOOT) {
+      continue;
+    }
+
+    //
+    // All the driver options should have been processed since
+    // now boot will be performed.
+    //
+    EfiBootManagerBoot (&BootOptions[Index]);
+
+    //
+    // Successful boot breaks the loop, otherwise tries next boot option
+    //
+    if (BootOptions[Index].Status == EFI_SUCCESS) {
+      break;
+    }
+  }
+
+  return (BOOLEAN) (Index < BootOptionCount);
+}
+
+/**
+  This function attempts to boot per the boot order specified by platform policy.
+
+  If the boot via Boot#### returns with a status of EFI_SUCCESS the boot manager will stop 
+  processing the BootOrder variable and present a boot manager menu to the user. If a boot via 
+  Boot#### returns a status other than EFI_SUCCESS, the boot has failed and the next Boot####
+  in the BootOrder variable will be tried until all possibilities are exhausted.
+                                  -- Chapter 3.1.1 Boot Manager Programming, the 4th paragraph
+**/
+VOID
+DefaultBootBehavior (
+  VOID
+  )
+{
+  UINTN                        BootOptionCount;
+  EFI_BOOT_MANAGER_LOAD_OPTION *BootOptions;
+  EFI_BOOT_MANAGER_LOAD_OPTION BootManagerMenu;
+
+  EfiBootManagerGetBootManagerMenu (&BootManagerMenu);
+  //
+  // BootManagerMenu always contains the correct information even the above function returns failure.
+  //
+
+  BootOptions = EfiBootManagerGetLoadOptions (&BootOptionCount, LoadOptionTypeBoot);
+
+  if (BootAllBootOptions (BootOptions, BootOptionCount)) {
+    //
+    // Follow generic rule, Call BdsDxeOnConnectConInCallBack to connect ConIn before enter UI
+    //
+    if (PcdGetBool (PcdConInConnectOnDemand)) {
+      BdsDxeOnConnectConInCallBack (NULL, NULL);
+    }
+
+    //
+    // Show the Boot Manager Menu after successful boot
+    //
+    EfiBootManagerBoot (&BootManagerMenu);
+  } else {
+    EfiBootManagerFreeLoadOptions (BootOptions, BootOptionCount);
+    //
+    // Re-scan all EFI boot options in case all the boot#### are deleted or failed to boot
+    //
+    // If no valid boot options exist, the boot manager will enumerate all removable media
+    // devices followed by all fixed media devices. The order within each group is undefined.
+    // These new default boot options are not saved to non volatile storage.The boot manger
+    // will then attempt toboot from each boot option.
+    //                            -- Chapter 3.3 Boot Manager Programming, the 2nd paragraph
+    //
+    EfiBootManagerConnectAll ();
+    BootOptions = BdsEnumerateBootOptions (&BootOptionCount);
+
+    if (!BootAllBootOptions (BootOptions, BootOptionCount)) {
+      DEBUG ((EFI_D_ERROR, "[Bds]No bootable device!\n"));
+      EfiBootManagerBoot (&BootManagerMenu);
+    }
+  }
+
+  EfiBootManagerFreeLoadOption (&BootManagerMenu);
+  EfiBootManagerFreeLoadOptions (BootOptions, BootOptionCount);
+}
+
+/**
+  The function will load and start every Driver####/SysPrep####.
+
+  @param  LoadOptions        Load option array.
+  @param  LoadOptionCount    Load option count.
+
+**/
+VOID
+ProcessLoadOptions (
+  IN EFI_BOOT_MANAGER_LOAD_OPTION       *LoadOptions,
+  IN UINTN                              LoadOptionCount
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+  BOOLEAN                           ReconnectAll;
+  EFI_BOOT_MANAGER_LOAD_OPTION_TYPE LoadOptionType;
+
+  ReconnectAll = FALSE;
+  LoadOptionType = LoadOptionTypeMax;
+
+  //
+  // Process the driver option
+  //
+  for (Index = 0; Index < LoadOptionCount; Index++) {
+    //
+    // All the load options in the array should be of the same type.
+    //
+    if (LoadOptionType == LoadOptionTypeMax) {
+      LoadOptionType = LoadOptions[Index].OptionType;
+    }
+    ASSERT (LoadOptionType == LoadOptions[Index].OptionType);
+    ASSERT (LoadOptionType == LoadOptionTypeDriver || LoadOptionType == LoadOptionTypeSysPrep);
+
+    Status = EfiBootManagerProcessLoadOption (&LoadOptions[Index]);
+
+    if (!EFI_ERROR (Status) && ((LoadOptions[Index].Attributes & LOAD_OPTION_FORCE_RECONNECT) != 0)) {
+      ReconnectAll = TRUE;
+    }
+  }
+
+  //
+  // If a driver load option is marked as LOAD_OPTION_FORCE_RECONNECT,
+  // then all of the EFI drivers in the system will be disconnected and
+  // reconnected after the last driver load option is processed.
+  //
+  if (ReconnectAll && LoadOptionType == LoadOptionTypeDriver) {
+    EfiBootManagerDisconnectAll ();
+    EfiBootManagerConnectAll ();
+  }
+}
+
+/**
+
+  Validate input console variable data. 
+
+  If found the device path is not a valid device path, remove the variable.
+  
+  @param VariableName             Input console variable name.
+
+**/
+VOID
+BdsFormalizeConsoleVariable (
+  IN  CHAR16          *VariableName
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINTN                     VariableSize;
+  EFI_STATUS                Status;
+
+  GetEfiGlobalVariable2 (VariableName, (VOID **) &DevicePath, &VariableSize);
+  if ((DevicePath != NULL) && !IsDevicePathValid (DevicePath, VariableSize)) { 
+    Status = gRT->SetVariable (
+                    VariableName,
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    0,
+                    NULL
+                    );
+    //
+    // Deleting variable with current variable implementation shouldn't fail.
+    //
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+}
+
+/**
+  Formalize OsIndication related variables. 
+  
+  For OsIndicationsSupported, Create a BS/RT/UINT64 variable to report caps 
+  Delete OsIndications variable if it is not NV/BS/RT UINT64.
+  
+  Item 3 is used to solve case when OS corrupts OsIndications. Here simply delete this NV variable.
+
+**/
+VOID 
+BdsFormalizeOSIndicationVariable (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  UINT64     OsIndicationSupport;
+  UINT64     OsIndication;
+  UINTN      DataSize;
+  UINT32     Attributes;
+
+  //
+  // OS indicater support variable
+  //
+  OsIndicationSupport = EFI_OS_INDICATIONS_BOOT_TO_FW_UI;
+  Status = gRT->SetVariable (
+                  L"OsIndicationsSupported",
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                  sizeof(UINT64),
+                  &OsIndicationSupport
+                  );
+  //
+  // Platform needs to make sure setting volatile variable before calling 3rd party code shouldn't fail.
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // If OsIndications is invalid, remove it.
+  // Invalid case
+  //   1. Data size != UINT64
+  //   2. OsIndication value inconsistence
+  //   3. OsIndication attribute inconsistence
+  //
+  OsIndication = 0;
+  Attributes = 0;
+  DataSize = sizeof(UINT64);
+  Status = gRT->GetVariable (
+                  L"OsIndications",
+                  &gEfiGlobalVariableGuid,
+                  &Attributes,
+                  &DataSize,
+                  &OsIndication
+                  );
+  if (Status == EFI_NOT_FOUND) {
+    return;
+  }
+
+  if ((DataSize != sizeof (OsIndication)) ||
+      ((OsIndication & ~OsIndicationSupport) != 0) ||
+      (Attributes != (EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE))
+     ){
+
+    DEBUG ((EFI_D_ERROR, "[Bds] Unformalized OsIndications variable exists. Delete it\n"));
+    Status = gRT->SetVariable (
+                    L"OsIndications",
+                    &gEfiGlobalVariableGuid,
+                    0,
+                    0,
+                    NULL
+                    );
+    //
+    // Deleting variable with current variable implementation shouldn't fail.
+    //
+    ASSERT_EFI_ERROR(Status);
+  }
+}
+
+/**
+
+  Validate variables. 
+
+**/
+VOID 
+BdsFormalizeEfiGlobalVariable (
+  VOID
+  )
+{
+  //
+  // Validate Console variable.
+  //
+  BdsFormalizeConsoleVariable (L"ConIn");
+  BdsFormalizeConsoleVariable (L"ConOut");
+  BdsFormalizeConsoleVariable (L"ErrOut");
+
+  //
+  // Validate OSIndication related variable.
+  //
+  BdsFormalizeOSIndicationVariable ();
+}
+
+/**
+
+  Allocate a block of memory that will contain performance data to OS.
+
+**/
+VOID
+BdsAllocateMemoryForPerformanceData (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_PHYSICAL_ADDRESS          AcpiLowMemoryBase;
+  EDKII_VARIABLE_LOCK_PROTOCOL  *VariableLock;
+
+  AcpiLowMemoryBase = 0x0FFFFFFFFULL;
+
+  //
+  // Allocate a block of memory that will contain performance data to OS.
+  //
+  Status = gBS->AllocatePages (
+                  AllocateMaxAddress,
+                  EfiReservedMemoryType,
+                  EFI_SIZE_TO_PAGES (PERF_DATA_MAX_LENGTH),
+                  &AcpiLowMemoryBase
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Save the pointer to variable for use in S3 resume.
+    //
+    Status = BdsDxeSetVariableAndReportStatusCodeOnError (
+               L"PerfDataMemAddr",
+               &gPerformanceProtocolGuid,
+               EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+               sizeof (EFI_PHYSICAL_ADDRESS),
+               &AcpiLowMemoryBase
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "[Bds] PerfDataMemAddr (%08x) cannot be saved to NV storage.\n", AcpiLowMemoryBase));
+    }
+    //
+    // Mark L"PerfDataMemAddr" variable to read-only if the Variable Lock protocol exists
+    // Still lock it even the variable cannot be saved to prevent it's set by 3rd party code.
+    //
+    Status = gBS->LocateProtocol (&gEdkiiVariableLockProtocolGuid, NULL, (VOID **) &VariableLock);
+    if (!EFI_ERROR (Status)) {
+      Status = VariableLock->RequestToLock (VariableLock, L"PerfDataMemAddr", &gPerformanceProtocolGuid);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+}
+
+/**
+
+  Service routine for BdsInstance->Entry(). Devices are connected, the
+  consoles are initialized, and the boot options are tried.
+
+  @param This             Protocol Instance structure.
+
+**/
+VOID
+EFIAPI
+BdsEntry (
+  IN EFI_BDS_ARCH_PROTOCOL  *This
+  )
+{
+  EFI_BOOT_MANAGER_LOAD_OPTION    *LoadOptions;
+  UINTN                           LoadOptionCount;
+  CHAR16                          *FirmwareVendor;
+  EFI_EVENT                       HotkeyTriggered;
+  UINT64                          OsIndication;
+  UINTN                           DataSize;
+  EFI_STATUS                      Status;
+  UINT32                          BootOptionSupport;
+  UINT16                          BootTimeOut;
+  EDKII_VARIABLE_LOCK_PROTOCOL    *VariableLock;
+  UINTN                           Index;
+  EFI_BOOT_MANAGER_LOAD_OPTION    BootOption;
+  UINT16                          *BootNext;
+  CHAR16                          BootNextVariableName[sizeof ("Boot####")];
+  EFI_BOOT_MANAGER_LOAD_OPTION    BootManagerMenu;
+  BOOLEAN                         BootFwUi;
+
+  HotkeyTriggered = NULL;
+  Status          = EFI_SUCCESS;
+
+  //
+  // Insert the performance probe
+  //
+  PERF_END (NULL, "DXE", NULL, 0);
+  PERF_START (NULL, "BDS", NULL, 0);
+  DEBUG ((EFI_D_INFO, "[Bds] Entry...\n"));
+
+  PERF_CODE (
+    BdsAllocateMemoryForPerformanceData ();
+  );
+
+  //
+  // Fill in FirmwareVendor and FirmwareRevision from PCDs
+  //
+  FirmwareVendor = (CHAR16 *) PcdGetPtr (PcdFirmwareVendor);
+  gST->FirmwareVendor = AllocateRuntimeCopyPool (StrSize (FirmwareVendor), FirmwareVendor);
+  ASSERT (gST->FirmwareVendor != NULL);
+  gST->FirmwareRevision = PcdGet32 (PcdFirmwareRevision);
+
+  //
+  // Fixup Tasble CRC after we updated Firmware Vendor and Revision
+  //
+  gST->Hdr.CRC32 = 0;
+  gBS->CalculateCrc32 ((VOID *) gST, sizeof (EFI_SYSTEM_TABLE), &gST->Hdr.CRC32);
+
+  //
+  // Validate Variable.
+  //
+  BdsFormalizeEfiGlobalVariable ();
+
+  //
+  // Mark the read-only variables if the Variable Lock protocol exists
+  //
+  Status = gBS->LocateProtocol (&gEdkiiVariableLockProtocolGuid, NULL, (VOID **) &VariableLock);
+  DEBUG ((EFI_D_INFO, "[BdsDxe] Locate Variable Lock protocol - %r\n", Status));
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < sizeof (mReadOnlyVariables) / sizeof (mReadOnlyVariables[0]); Index++) {
+      Status = VariableLock->RequestToLock (VariableLock, mReadOnlyVariables[Index], &gEfiGlobalVariableGuid);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  InitializeHwErrRecSupport ();
+
+  //
+  // Initialize L"Timeout" EFI global variable.
+  //
+  BootTimeOut = PcdGet16 (PcdPlatformBootTimeOut);
+  if (BootTimeOut != 0xFFFF) {
+    //
+    // If time out value equal 0xFFFF, no need set to 0xFFFF to variable area because UEFI specification
+    // define same behavior between no value or 0xFFFF value for L"Timeout".
+    //
+    BdsDxeSetVariableAndReportStatusCodeOnError (
+      EFI_TIME_OUT_VARIABLE_NAME,
+      &gEfiGlobalVariableGuid,
+      EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+      sizeof (UINT16),
+      &BootTimeOut
+      );
+  }
+
+  //
+  // Initialize L"BootOptionSupport" EFI global variable.
+  // Lazy-ConIn implictly disables BDS hotkey.
+  //
+  BootOptionSupport = EFI_BOOT_OPTION_SUPPORT_APP | EFI_BOOT_OPTION_SUPPORT_SYSPREP;
+  if (!PcdGetBool (PcdConInConnectOnDemand)) {
+    BootOptionSupport |= EFI_BOOT_OPTION_SUPPORT_KEY;
+    SET_BOOT_OPTION_SUPPORT_KEY_COUNT (BootOptionSupport, 3);
+  }
+  Status = gRT->SetVariable (
+                  EFI_BOOT_OPTION_SUPPORT_VARIABLE_NAME,
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                  sizeof (BootOptionSupport),
+                  &BootOptionSupport
+                  );
+  //
+  // Platform needs to make sure setting volatile variable before calling 3rd party code shouldn't fail.
+  //
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Cache and remove the "BootNext" NV variable.
+  //
+  GetEfiGlobalVariable2 (EFI_BOOT_NEXT_VARIABLE_NAME, (VOID **) &BootNext, &DataSize);
+  if (DataSize != sizeof (UINT16)) {
+    if (BootNext != NULL) {
+      FreePool (BootNext);
+    }
+    BootNext = NULL;
+  }
+  Status = gRT->SetVariable (
+                  EFI_BOOT_NEXT_VARIABLE_NAME,
+                  &gEfiGlobalVariableGuid,
+                  0,
+                  0,
+                  NULL
+                  );
+  //
+  // Deleting NV variable shouldn't fail unless it doesn't exist.
+  //
+  ASSERT (Status == EFI_SUCCESS || Status == EFI_NOT_FOUND);
+
+  //
+  // Initialize the platform language variables
+  //
+  InitializeLanguage (TRUE);
+
+  //
+  // Report Status Code to indicate connecting drivers will happen
+  //
+  REPORT_STATUS_CODE (
+    EFI_PROGRESS_CODE,
+    (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_DXE_BS_PC_BEGIN_CONNECTING_DRIVERS)
+    );
+
+  //
+  // Do the platform init, can be customized by OEM/IBV
+  // Possible things that can be done in PlatformBootManagerBeforeConsole:
+  // > Update console variable: 1. include hot-plug devices; 2. Clear ConIn and add SOL for AMT
+  // > Register new Driver#### or Boot####
+  // > Register new Key####: e.g.: F12 
+  // > Signal ReadyToLock event
+  // > Authentication action: 1. connect Auth devices; 2. Identify auto logon user.
+  //
+  PERF_START (NULL, "PlatformBootManagerBeforeConsole", "BDS", 0);
+  PlatformBootManagerBeforeConsole ();
+  PERF_END   (NULL, "PlatformBootManagerBeforeConsole", "BDS", 0);
+
+  //
+  // Initialize hotkey service
+  //
+  EfiBootManagerStartHotkeyService (&HotkeyTriggered);
+
+  //
+  // Execute Driver Options
+  //
+  LoadOptions = EfiBootManagerGetLoadOptions (&LoadOptionCount, LoadOptionTypeDriver);
+  ProcessLoadOptions (LoadOptions, LoadOptionCount);
+  EfiBootManagerFreeLoadOptions (LoadOptions, LoadOptionCount);
+
+  //
+  // Connect consoles
+  //
+  PERF_START (NULL, "EfiBootManagerConnectAllDefaultConsoles", "BDS", 0);
+  if (PcdGetBool (PcdConInConnectOnDemand)) {
+    EfiBootManagerConnectConsoleVariable (ConOut);
+    EfiBootManagerConnectConsoleVariable (ErrOut);
+
+    //
+    // Initialize ConnectConIn event
+    //
+    Status = gBS->CreateEventEx (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    BdsDxeOnConnectConInCallBack,
+                    NULL,
+                    &gConnectConInEventGuid,
+                    &gConnectConInEvent
+                    );
+    if (EFI_ERROR (Status)) {
+      gConnectConInEvent = NULL;
+    }
+  } else {
+    EfiBootManagerConnectAllDefaultConsoles ();
+  }
+  PERF_END   (NULL, "EfiBootManagerConnectAllDefaultConsoles", "BDS", 0);
+
+  //
+  // Do the platform specific action after the console is ready
+  // Possible things that can be done in PlatformBootManagerAfterConsole:
+  // > Console post action:
+  //   > Dynamically switch output mode from 100x31 to 80x25 for certain senarino
+  //   > Signal console ready platform customized event
+  // > Run diagnostics like memory testing
+  // > Connect certain devices
+  // > Dispatch aditional option roms
+  // > Special boot: e.g.: USB boot, enter UI
+  // 
+  PERF_START (NULL, "PlatformBootManagerAfterConsole", "BDS", 0);
+  PlatformBootManagerAfterConsole ();
+  PERF_END   (NULL, "PlatformBootManagerAfterConsole", "BDS", 0);
+
+  DEBUG_CODE (
+    EFI_BOOT_MANAGER_LOAD_OPTION_TYPE LoadOptionType;
+    DEBUG ((EFI_D_INFO, "[Bds]=============Begin Load Options Dumping ...=============\n"));
+    for (LoadOptionType = 0; LoadOptionType < LoadOptionTypeMax; LoadOptionType++) {
+      DEBUG ((
+        EFI_D_INFO, "  %s Options:\n",
+        mBdsLoadOptionName[LoadOptionType]
+        ));
+      LoadOptions = EfiBootManagerGetLoadOptions (&LoadOptionCount, LoadOptionType);
+      for (Index = 0; Index < LoadOptionCount; Index++) {
+        DEBUG ((
+          EFI_D_INFO, "    %s%04x: %s \t\t 0x%04x\n",
+          mBdsLoadOptionName[LoadOptionType],
+          LoadOptions[Index].OptionNumber,
+          LoadOptions[Index].Description,
+          LoadOptions[Index].Attributes
+          ));
+      }
+      EfiBootManagerFreeLoadOptions (LoadOptions, LoadOptionCount);
+    }
+    DEBUG ((EFI_D_INFO, "[Bds]=============End Load Options Dumping=============\n"));
+  );
+
+  //
+  // Boot to Boot Manager Menu when EFI_OS_INDICATIONS_BOOT_TO_FW_UI is set. Skip HotkeyBoot
+  //
+  DataSize = sizeof (UINT64);
+  Status = gRT->GetVariable (
+                  L"OsIndications",
+                  &gEfiGlobalVariableGuid,
+                  NULL,
+                  &DataSize,
+                  &OsIndication
+                  );
+  if (EFI_ERROR (Status)) {
+    OsIndication = 0;
+  }
+
+  BootFwUi = (BOOLEAN) ((OsIndication & EFI_OS_INDICATIONS_BOOT_TO_FW_UI) != 0);
+  //
+  // Clear EFI_OS_INDICATIONS_BOOT_TO_FW_UI to acknowledge OS
+  // 
+  if (BootFwUi) {
+    OsIndication &= ~((UINT64) EFI_OS_INDICATIONS_BOOT_TO_FW_UI);
+    Status = gRT->SetVariable (
+               L"OsIndications",
+               &gEfiGlobalVariableGuid,
+               EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+               sizeof(UINT64),
+               &OsIndication
+               );
+    //
+    // Changing the content without increasing its size with current variable implementation shouldn't fail.
+    //
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Launch Boot Manager Menu directly when EFI_OS_INDICATIONS_BOOT_TO_FW_UI is set. Skip HotkeyBoot
+  //
+  if (BootFwUi) {
+    //
+    // Follow generic rule, Call BdsDxeOnConnectConInCallBack to connect ConIn before enter UI
+    //
+    if (PcdGetBool (PcdConInConnectOnDemand)) {
+      BdsDxeOnConnectConInCallBack (NULL, NULL);
+    }
+
+    //
+    // Directly enter the setup page.
+    // BootManagerMenu always contains the correct information even call fails.
+    //
+    EfiBootManagerGetBootManagerMenu (&BootManagerMenu);
+    EfiBootManagerBoot (&BootManagerMenu);
+    EfiBootManagerFreeLoadOption (&BootManagerMenu);
+  }
+
+  //
+  // Execute SysPrep####
+  //
+  LoadOptions = EfiBootManagerGetLoadOptions (&LoadOptionCount, LoadOptionTypeSysPrep);
+  ProcessLoadOptions (LoadOptions, LoadOptionCount);
+  EfiBootManagerFreeLoadOptions (LoadOptions, LoadOptionCount);
+
+  //
+  // Execute Key####
+  //
+  PERF_START (NULL, "BdsWait", "BDS", 0);
+  BdsWait (HotkeyTriggered);
+  PERF_END   (NULL, "BdsWait", "BDS", 0);
+
+  //
+  // BdsReadKeys() be removed after all keyboard drivers invoke callback in timer callback.
+  //
+  BdsReadKeys ();
+
+  EfiBootManagerHotkeyBoot ();
+
+  //
+  // Boot to "BootNext"
+  //
+  if (BootNext != NULL) {
+    UnicodeSPrint (BootNextVariableName, sizeof (BootNextVariableName), L"Boot%04x", *BootNext);
+    Status = EfiBootManagerVariableToLoadOption (BootNextVariableName, &BootOption);
+    if (!EFI_ERROR (Status)) {
+      EfiBootManagerBoot (&BootOption);
+      EfiBootManagerFreeLoadOption (&BootOption);
+      if (BootOption.Status == EFI_SUCCESS) {
+        //
+        // Boot to Boot Manager Menu upon EFI_SUCCESS
+        //
+        EfiBootManagerGetBootManagerMenu (&BootOption);
+        EfiBootManagerBoot (&BootOption);
+        EfiBootManagerFreeLoadOption (&BootOption);
+      }
+    }
+  }
+
+  while (TRUE) {
+    //
+    // BDS select the boot device to load OS
+    // Try next upon boot failure
+    // Show Boot Manager Menu upon boot success
+    //
+    DefaultBootBehavior ();
+  }
+}
+
+/**
+  Set the variable and report the error through status code upon failure.
+
+  @param  VariableName           A Null-terminated string that is the name of the vendor's variable.
+                                 Each VariableName is unique for each VendorGuid. VariableName must
+                                 contain 1 or more characters. If VariableName is an empty string,
+                                 then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             Attributes bitmask to set for the variable.
+  @param  DataSize               The size in bytes of the Data buffer. Unless the EFI_VARIABLE_APPEND_WRITE, 
+                                 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS, or 
+                                 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS attribute is set, a size of zero 
+                                 causes the variable to be deleted. When the EFI_VARIABLE_APPEND_WRITE attribute is 
+                                 set, then a SetVariable() call with a DataSize of zero will not cause any change to 
+                                 the variable value (the timestamp associated with the variable may be updated however 
+                                 even if no new data value is provided,see the description of the 
+                                 EFI_VARIABLE_AUTHENTICATION_2 descriptor below. In this case the DataSize will not 
+                                 be zero since the EFI_VARIABLE_AUTHENTICATION_2 descriptor will be populated). 
+  @param  Data                   The contents for the variable.
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits, name, and GUID was supplied, or the
+                                 DataSize exceeds the maximum allowed.
+  @retval EFI_INVALID_PARAMETER  VariableName is an empty string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be retrieved due to a hardware error.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only.
+  @retval EFI_WRITE_PROTECTED    The variable in question cannot be deleted.
+  @retval EFI_SECURITY_VIOLATION The variable could not be written due to EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS 
+                                 or EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACESS being set, but the AuthInfo 
+                                 does NOT pass the validation check carried out by the firmware.
+
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+**/
+EFI_STATUS
+BdsDxeSetVariableAndReportStatusCodeOnError (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  EFI_STATUS                 Status;
+  EDKII_SET_VARIABLE_STATUS  *SetVariableStatus;
+  UINTN                      NameSize;
+
+  Status = gRT->SetVariable (
+                  VariableName,
+                  VendorGuid,
+                  Attributes,
+                  DataSize,
+                  Data
+                  );
+  if (EFI_ERROR (Status)) {
+    NameSize = StrSize (VariableName);
+    SetVariableStatus = AllocatePool (sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + DataSize);
+    if (SetVariableStatus != NULL) {
+      CopyGuid (&SetVariableStatus->Guid, VendorGuid);
+      SetVariableStatus->NameSize   = NameSize;
+      SetVariableStatus->DataSize   = DataSize;
+      SetVariableStatus->SetStatus  = Status;
+      SetVariableStatus->Attributes = Attributes;
+      CopyMem (SetVariableStatus + 1,                          VariableName, NameSize);
+      CopyMem (((UINT8 *) (SetVariableStatus + 1)) + NameSize, Data,         DataSize);
+
+      REPORT_STATUS_CODE_EX (
+        EFI_ERROR_CODE,
+        PcdGet32 (PcdErrorCodeSetVariable),
+        0,
+        NULL,
+        &gEdkiiStatusCodeDataTypeVariableGuid,
+        SetVariableStatus,
+        sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + DataSize
+        );
+
+      FreePool (SetVariableStatus);
+    }
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.c b/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.c
new file mode 100644
index 0000000000..87e39c3c8d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.c
@@ -0,0 +1,48 @@
+/** @file
+  Set the level of support for Hardware Error Record Persistence that is
+  implemented by the platform.
+
+Copyright (c) 2007 - 2015, 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 "HwErrRecSupport.h"
+
+/**
+  Set the HwErrRecSupport variable contains a binary UINT16 that supplies the
+  level of support for Hardware Error Record Persistence that is implemented
+  by the platform.
+
+**/
+VOID
+InitializeHwErrRecSupport (
+  VOID
+  )
+{
+  EFI_STATUS    Status;
+  UINT16        HardwareErrorRecordLevel;
+  
+  HardwareErrorRecordLevel = PcdGet16 (PcdHardwareErrorRecordLevel);
+  
+  if (HardwareErrorRecordLevel != 0) {
+    //
+    // If level value equal 0, no need set to 0 to variable area because UEFI specification
+    // define same behavior between no value or 0 value for L"HwErrRecSupport".
+    //
+    Status = gRT->SetVariable (
+                    L"HwErrRecSupport",
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    sizeof (UINT16),
+                    &HardwareErrorRecordLevel
+                    );
+    ASSERT_EFI_ERROR(Status);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.h b/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.h
new file mode 100644
index 0000000000..2ac05d5566
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/HwErrRecSupport.h
@@ -0,0 +1,32 @@
+/** @file
+  Set the level of support for Hardware Error Record Persistence that is
+  implemented by the platform.
+
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef _HW_ERR_REC_SUPPORT_H_
+#define _HW_ERR_REC_SUPPORT_H_
+
+#include "Bds.h"
+
+/**
+  Set the HwErrRecSupport variable contains a binary UINT16 that supplies the
+  level of support for Hardware Error Record Persistence that is implemented
+  by the platform.
+
+**/
+VOID
+InitializeHwErrRecSupport (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/Language.c b/Core/MdeModulePkg/Universal/BdsDxe/Language.c
new file mode 100644
index 0000000000..09127316fb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/Language.c
@@ -0,0 +1,202 @@
+/** @file
+  Language settings
+
+Copyright (c) 2004 - 2015, 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 "Bds.h"
+#define ISO_639_2_ENTRY_SIZE 3
+
+/**
+  Check if lang is in supported language codes according to language string.
+
+  This code is used to check if lang is in in supported language codes. It can handle
+  RFC4646 and ISO639 language tags.
+  In ISO639 language tags, take 3-characters as a delimitation to find matched string.
+  In RFC4646 language tags, take semicolon as a delimitation to find matched string.
+
+  For example:
+    SupportedLang  = "engfraengfra"
+    Iso639Language = TRUE
+    Lang           = "eng", the return value is "TRUE", or
+    Lang           = "chs", the return value is "FALSE".
+  Another example:
+    SupportedLang  = "en;fr;en-US;fr-FR"
+    Iso639Language = FALSE
+    Lang           = "en", the return value is "TRUE", or
+    Lang           = "zh", the return value is "FALSE".
+
+  @param  SupportedLang               Platform supported language codes.
+  @param  Lang                        Configured language.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646.
+
+  @retval TRUE  lang is in supported language codes.
+  @retval FALSE lang is not in supported language codes.
+
+**/
+BOOLEAN
+IsLangInSupportedLangCodes(
+  IN  CHAR8            *SupportedLang,
+  IN  CHAR8            *Lang,
+  IN  BOOLEAN          Iso639Language
+  ) 
+{
+  UINTN    Index;
+  UINTN    CompareLength;
+  UINTN    LanguageLength;
+
+  if (Iso639Language) {
+    CompareLength = ISO_639_2_ENTRY_SIZE;
+    for (Index = 0; Index < AsciiStrLen (SupportedLang); Index += CompareLength) {
+      if (AsciiStrnCmp (Lang, SupportedLang + Index, CompareLength) == 0) {
+        //
+        // Successfully find the Lang string in SupportedLang string.
+        //
+        return TRUE;
+      }
+    }
+    return FALSE;
+  } else {
+    //
+    // Compare RFC4646 language code
+    //
+    for (LanguageLength = 0; Lang[LanguageLength] != '\0'; LanguageLength++);
+
+    for (; *SupportedLang != '\0'; SupportedLang += CompareLength) {
+      //
+      // Skip ';' characters in SupportedLang
+      //
+      for (; *SupportedLang != '\0' && *SupportedLang == ';'; SupportedLang++);
+      //
+      // Determine the length of the next language code in SupportedLang
+      //
+      for (CompareLength = 0; SupportedLang[CompareLength] != '\0' && SupportedLang[CompareLength] != ';'; CompareLength++);
+      
+      if ((CompareLength == LanguageLength) && 
+          (AsciiStrnCmp (Lang, SupportedLang, CompareLength) == 0)) {
+        //
+        // Successfully find the Lang string in SupportedLang string.
+        //
+        return TRUE;
+      }
+    }
+    return FALSE;
+  }
+}
+
+/**
+  Initialize Lang or PlatformLang variable, if Lang or PlatformLang variable is not found,
+  or it has been set to an unsupported value(not one of platform supported language codes),
+  set the default language code to it.
+
+  @param  LangName                    Language name, L"Lang" or L"PlatformLang".
+  @param  SupportedLang               Platform supported language codes.
+  @param  DefaultLang                 Default language code.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646,
+                                      TRUE for L"Lang" LangName or FALSE for L"PlatformLang" LangName.
+
+**/
+VOID
+InitializeLangVariable (
+  IN CHAR16     *LangName,
+  IN CHAR8      *SupportedLang,
+  IN CHAR8      *DefaultLang,     
+  IN BOOLEAN    Iso639Language
+  )
+{
+  CHAR8       *Lang;
+
+  //
+  // Find current Lang or PlatformLang from EFI Variable.
+  //
+  GetEfiGlobalVariable2 (LangName, (VOID **) &Lang, NULL);
+
+  //
+  // If Lang or PlatformLang variable is not found,
+  // or it has been set to an unsupported value(not one of the supported language codes),
+  // set the default language code to it.
+  //
+  if ((Lang == NULL) || !IsLangInSupportedLangCodes (SupportedLang, Lang, Iso639Language)) {
+    //
+    // The default language code should be one of the supported language codes.
+    //
+    ASSERT (IsLangInSupportedLangCodes (SupportedLang, DefaultLang, Iso639Language));
+    BdsDxeSetVariableAndReportStatusCodeOnError (
+      LangName,
+      &gEfiGlobalVariableGuid,
+      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+      AsciiStrSize (DefaultLang),
+      DefaultLang
+      );
+  }
+
+  if (Lang != NULL) {
+    FreePool (Lang);
+  }
+}
+
+/**
+  Determine the current language that will be used
+  based on language related EFI Variables.
+
+  @param LangCodesSettingRequired - If required to set LangCodes variable
+
+**/
+VOID
+InitializeLanguage (
+  BOOLEAN LangCodesSettingRequired
+  )
+{
+  EFI_STATUS  Status;
+  CHAR8       *LangCodes;
+  CHAR8       *PlatformLangCodes;
+
+  LangCodes = (CHAR8 *)PcdGetPtr (PcdUefiVariableDefaultLangCodes);
+  PlatformLangCodes = (CHAR8 *)PcdGetPtr (PcdUefiVariableDefaultPlatformLangCodes);
+  if (LangCodesSettingRequired) {
+    if (!FeaturePcdGet (PcdUefiVariableDefaultLangDeprecate)) {
+      //
+      // UEFI 2.1 depricated this variable so we support turning it off
+      //
+      Status = gRT->SetVariable (
+                      L"LangCodes",
+                      &gEfiGlobalVariableGuid,
+                      EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                      AsciiStrSize (LangCodes),
+                      LangCodes
+                      );
+      //
+      // Platform needs to make sure setting volatile variable before calling 3rd party code shouldn't fail.
+      //
+      ASSERT_EFI_ERROR(Status);
+    }
+
+    Status = gRT->SetVariable (
+                    L"PlatformLangCodes",
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                    AsciiStrSize (PlatformLangCodes),
+                    PlatformLangCodes
+                    );
+    //
+    // Platform needs to make sure setting volatile variable before calling 3rd party code shouldn't fail.
+    //
+    ASSERT_EFI_ERROR(Status);
+  }
+
+  if (!FeaturePcdGet (PcdUefiVariableDefaultLangDeprecate)) {
+    //
+    // UEFI 2.1 depricated this variable so we support turning it off
+    //
+    InitializeLangVariable (L"Lang", LangCodes, (CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultLang), TRUE);
+  }
+  InitializeLangVariable (L"PlatformLang", PlatformLangCodes, (CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultPlatformLang), FALSE);
+}
diff --git a/Core/MdeModulePkg/Universal/BdsDxe/Language.h b/Core/MdeModulePkg/Universal/BdsDxe/Language.h
new file mode 100644
index 0000000000..3d5f34f561
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BdsDxe/Language.h
@@ -0,0 +1,30 @@
+/** @file
+  Language setting
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _LANGUAGE_H_
+#define _LANGUAGE_H_
+
+/**
+  Determine the current language that will be used
+  based on language related EFI Variables.
+
+  @param LangCodesSettingRequired If required to set LangCode variable
+
+**/
+VOID
+InitializeLanguage (
+  BOOLEAN LangCodesSettingRequired
+  );
+
+#endif // _LANGUAGE_H_
diff --git a/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.c b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.c
new file mode 100644
index 0000000000..412ecbda55
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.c
@@ -0,0 +1,287 @@
+/** @file
+  This module produces Boot Manager Policy protocol.
+
+Copyright (c) 2015, 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 <Uefi.h>
+#include <Protocol/BootManagerPolicy.h>
+#include <Protocol/ManagedNetwork.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiBootManagerLib.h>
+
+CHAR16 mNetworkDeviceList[] = L"_NDL";
+
+/**
+  Connect all the system drivers to controllers and create the network device list in NV storage.
+
+  @retval EFI_SUCCESS      Network devices are connected.
+  @retval EFI_DEVICE_ERROR No network device is connected.
+
+**/
+EFI_STATUS
+ConnectAllAndCreateNetworkDeviceList (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_HANDLE                      *Handles;
+  UINTN                           HandleCount;
+  EFI_DEVICE_PATH_PROTOCOL        *SingleDevice;
+  EFI_DEVICE_PATH_PROTOCOL        *Devices;
+  EFI_DEVICE_PATH_PROTOCOL        *TempDevicePath;
+
+  EfiBootManagerConnectAll ();
+
+  Status = gBS->LocateHandleBuffer (ByProtocol, &gEfiManagedNetworkServiceBindingProtocolGuid, NULL, &HandleCount, &Handles);
+  if (EFI_ERROR (Status)) {
+    Handles = NULL;
+    HandleCount = 0;
+  }
+
+  Devices = NULL;
+  while (HandleCount-- != 0) {
+    Status = gBS->HandleProtocol (Handles[HandleCount], &gEfiDevicePathProtocolGuid, (VOID **) &SingleDevice);
+    if (EFI_ERROR (Status) || (SingleDevice == NULL)) {
+      continue;
+    }
+    TempDevicePath = Devices;
+    Devices = AppendDevicePathInstance (Devices, SingleDevice);
+    if (TempDevicePath != NULL) {
+      FreePool (TempDevicePath);
+    }
+  }
+
+  if (Devices != NULL) {
+    Status = gRT->SetVariable (
+                    mNetworkDeviceList,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    GetDevicePathSize (Devices),
+                    Devices
+                    );
+    //
+    // Fails to save the network device list to NV storage is not a fatal error.
+    // Only impact is performance.
+    //
+    FreePool (Devices);
+  }
+
+  return (Devices == NULL) ? EFI_DEVICE_ERROR : EFI_SUCCESS;
+}
+
+/**
+  Connect the network devices.
+
+  @retval EFI_SUCCESS      At least one network device was connected.
+  @retval EFI_DEVICE_ERROR Network devices were not connected due to an error.
+**/
+EFI_STATUS
+ConnectNetwork (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  BOOLEAN                       OneConnected;
+  EFI_DEVICE_PATH_PROTOCOL      *Devices;
+  EFI_DEVICE_PATH_PROTOCOL      *TempDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL      *SingleDevice;
+  UINTN                         Size;
+
+  OneConnected = FALSE;
+  GetVariable2 (mNetworkDeviceList, &gEfiCallerIdGuid, (VOID **) &Devices, NULL);
+  TempDevicePath = Devices;
+  while (TempDevicePath != NULL) {
+    SingleDevice = GetNextDevicePathInstance (&TempDevicePath, &Size);
+    Status = EfiBootManagerConnectDevicePath (SingleDevice, NULL);
+    if (!EFI_ERROR (Status)) {
+      OneConnected = TRUE;
+    }
+    FreePool (SingleDevice);
+  }
+  if (Devices != NULL) {
+    FreePool (Devices);
+  }
+
+  if (OneConnected) {
+    return EFI_SUCCESS;
+  } else {
+    //
+    // Cached network devices list doesn't exist or is NOT valid.
+    //
+    return ConnectAllAndCreateNetworkDeviceList ();
+  }
+}
+
+/**
+  Connect a device path following the platforms EFI Boot Manager policy.
+
+  The ConnectDevicePath() function allows the caller to connect a DevicePath using the
+  same policy as the EFI Boot Manger.
+
+  @param[in] This       A pointer to the EFI_BOOT_MANAGER_POLICY_PROTOCOL instance.
+  @param[in] DevicePath Points to the start of the EFI device path to connect.
+                        If DevicePath is NULL then all the controllers in the
+                        system will be connected using the platforms EFI Boot
+                        Manager policy.
+  @param[in] Recursive  If TRUE, then ConnectController() is called recursively
+                        until the entire tree of controllers below the 
+                        controller specified by DevicePath have been created.
+                        If FALSE, then the tree of controllers is only expanded
+                        one level. If DevicePath is NULL then Recursive is ignored.
+
+  @retval EFI_SUCCESS            The DevicePath was connected.
+  @retval EFI_NOT_FOUND          The DevicePath was not found.
+  @retval EFI_NOT_FOUND          No driver was connected to DevicePath.
+  @retval EFI_SECURITY_VIOLATION The user has no permission to start UEFI device 
+                                 drivers on the DevicePath.
+  @retval EFI_UNSUPPORTED        The current TPL is not TPL_APPLICATION.
+**/
+EFI_STATUS
+EFIAPI
+BootManagerPolicyConnectDevicePath (
+  IN EFI_BOOT_MANAGER_POLICY_PROTOCOL *This,
+  IN EFI_DEVICE_PATH                  *DevicePath,
+  IN BOOLEAN                          Recursive
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HANDLE                          Controller;
+
+  if (EfiGetCurrentTpl () != TPL_APPLICATION) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (DevicePath == NULL) {
+    EfiBootManagerConnectAll ();
+    return EFI_SUCCESS;
+  }
+
+  if (Recursive) {
+    Status = EfiBootManagerConnectDevicePath (DevicePath, NULL);
+  } else {
+    Status = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &DevicePath, &Controller);
+    if (!EFI_ERROR (Status)) {
+      Status = gBS->ConnectController (Controller, NULL, DevicePath, FALSE);
+    }
+  }
+  return Status;
+}
+/**
+  Connect a class of devices using the platform Boot Manager policy.
+
+  The ConnectDeviceClass() function allows the caller to request that the Boot
+  Manager connect a class of devices.
+
+  If Class is EFI_BOOT_MANAGER_POLICY_CONSOLE_GUID then the Boot Manager will
+  use platform policy to connect consoles. Some platforms may restrict the 
+  number of consoles connected as they attempt to fast boot, and calling
+  ConnectDeviceClass() with a Class value of EFI_BOOT_MANAGER_POLICY_CONSOLE_GUID
+  must connect the set of consoles that follow the Boot Manager platform policy,
+  and the EFI_SIMPLE_TEXT_INPUT_PROTOCOL, EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL, and
+  the EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL are produced on the connected handles.
+  The Boot Manager may restrict which consoles get connect due to platform policy,
+  for example a security policy may require that a given console is not connected.
+
+  If Class is EFI_BOOT_MANAGER_POLICY_NETWORK_GUID then the Boot Manager will
+  connect the protocols the platforms supports for UEFI general purpose network
+  applications on one or more handles. If more than one network controller is
+  available a platform will connect, one, many, or all of the networks based
+  on platform policy. Connecting UEFI networking protocols, like EFI_DHCP4_PROTOCOL,
+  does not establish connections on the network. The UEFI general purpose network
+  application that called ConnectDeviceClass() may need to use the published
+  protocols to establish the network connection. The Boot Manager can optionally
+  have a policy to establish a network connection.
+  
+  If Class is EFI_BOOT_MANAGER_POLICY_CONNECT_ALL_GUID then the Boot Manager
+  will connect all UEFI drivers using the UEFI Boot Service
+  EFI_BOOT_SERVICES.ConnectController(). If the Boot Manager has policy
+  associated with connect all UEFI drivers this policy will be used.
+
+  A platform can also define platform specific Class values as a properly generated
+  EFI_GUID would never conflict with this specification.
+
+  @param[in] This  A pointer to the EFI_BOOT_MANAGER_POLICY_PROTOCOL instance.
+  @param[in] Class A pointer to an EFI_GUID that represents a class of devices
+                   that will be connected using the Boot Mangers platform policy.
+
+  @retval EFI_SUCCESS      At least one devices of the Class was connected.
+  @retval EFI_DEVICE_ERROR Devices were not connected due to an error.
+  @retval EFI_NOT_FOUND    The Class is not supported by the platform.
+  @retval EFI_UNSUPPORTED  The current TPL is not TPL_APPLICATION.
+**/        
+EFI_STATUS
+EFIAPI
+BootManagerPolicyConnectDeviceClass (
+  IN EFI_BOOT_MANAGER_POLICY_PROTOCOL *This,
+  IN EFI_GUID                         *Class
+  )
+{
+  if (EfiGetCurrentTpl () != TPL_APPLICATION) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (CompareGuid (Class, &gEfiBootManagerPolicyConnectAllGuid)) {
+    ConnectAllAndCreateNetworkDeviceList ();
+    return EFI_SUCCESS;
+  }
+
+  if (CompareGuid (Class, &gEfiBootManagerPolicyConsoleGuid)) {
+    return EfiBootManagerConnectAllDefaultConsoles ();
+  }
+
+  if (CompareGuid (Class, &gEfiBootManagerPolicyNetworkGuid)) {
+    return ConnectNetwork ();
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+EFI_BOOT_MANAGER_POLICY_PROTOCOL  mBootManagerPolicy = {
+  EFI_BOOT_MANAGER_POLICY_PROTOCOL_REVISION,
+  BootManagerPolicyConnectDevicePath,
+  BootManagerPolicyConnectDeviceClass
+};
+
+/**
+  Install Boot Manager Policy Protocol.
+
+  @param ImageHandle    The image handle.
+  @param SystemTable    The system table.
+
+  @retval  EFI_SUCEESS  The Boot Manager Policy protocol is successfully installed.
+  @retval  Other        Return status from gBS->InstallMultipleProtocolInterfaces().
+
+**/
+EFI_STATUS
+EFIAPI
+BootManagerPolicyInitialize (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  )
+{
+  EFI_HANDLE                               Handle;
+
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiBootManagerPolicyProtocolGuid);
+
+  Handle = NULL;
+  return gBS->InstallMultipleProtocolInterfaces (
+                &Handle,
+                &gEfiBootManagerPolicyProtocolGuid, &mBootManagerPolicy,
+                NULL
+                );
+}
diff --git a/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.inf b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.inf
new file mode 100644
index 0000000000..488f421341
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.inf
@@ -0,0 +1,62 @@
+## @file
+#  This module produces Boot Manager Policy protocol.
+#  
+#  Copyright (c) 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = BootManagerPolicyDxe
+  MODULE_UNI_FILE                = BootManagerPolicyDxe.uni
+  FILE_GUID                      = E622443C-284E-4b47-A984-FD66B482DAC0
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0 
+  ENTRY_POINT                    = BootManagerPolicyInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  BootManagerPolicyDxe.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  DevicePathLib
+  DebugLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  UefiBootManagerLib
+
+[Guids]
+  gEfiBootManagerPolicyConnectAllGuid           ## CONSUMES ## GUID
+  gEfiBootManagerPolicyNetworkGuid              ## CONSUMES ## GUID
+  gEfiBootManagerPolicyConsoleGuid              ## CONSUMES ## GUID
+
+[Protocols]
+  gEfiManagedNetworkServiceBindingProtocolGuid  ## CONSUMES
+  gEfiBootManagerPolicyProtocolGuid             ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  BootManagerPolicyDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.uni b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.uni
new file mode 100644
index 0000000000..008eee0efa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxe.uni
diff --git a/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxeExtra.uni b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxeExtra.uni
new file mode 100644
index 0000000000..8cdbde0a50
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/BootManagerPolicyDxe/BootManagerPolicyDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/Capsule.h b/Core/MdeModulePkg/Universal/CapsulePei/Capsule.h
new file mode 100644
index 0000000000..68d8e42745
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/Capsule.h
@@ -0,0 +1,130 @@
+/** @file
+
+Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef _CAPSULE_PEIM_H_
+#define _CAPSULE_PEIM_H_
+
+#include <PiPei.h>
+#include <Uefi/UefiSpec.h>
+
+#include <Ppi/Capsule.h>
+#include <Ppi/LoadFile.h>
+#include <Ppi/ReadOnlyVariable2.h>
+#include <Guid/CapsuleVendor.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/HobLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/PrintLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/PcdLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/DebugAgentLib.h>
+#include <IndustryStandard/PeImage.h>
+#include "Common/CommonHeader.h"
+
+#ifdef MDE_CPU_IA32 
+
+#pragma pack(1)
+
+//
+// Page-Map Level-4 Offset (PML4) and
+// Page-Directory-Pointer Offset (PDPE) entries 4K & 2MB
+//
+
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Reserved:1;               // Reserved
+    UINT64  MustBeZero:2;             // Must Be Zero
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PageTableBaseAddress:40;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // No Execute bit
+  } Bits;
+  UINT64    Uint64;
+} PAGE_MAP_AND_DIRECTORY_POINTER;
+
+//
+// Page Table Entry 2MB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1:1;                // Must be 1 
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PAT:1;                    //
+    UINT64  MustBeZero:8;             // Must be zero;
+    UINT64  PageTableBaseAddress:31;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_ENTRY;
+
+//
+// Page Table Entry 1GB
+//
+typedef union {
+  struct {
+    UINT64  Present:1;                // 0 = Not present in memory, 1 = Present in memory
+    UINT64  ReadWrite:1;              // 0 = Read-Only, 1= Read/Write
+    UINT64  UserSupervisor:1;         // 0 = Supervisor, 1=User
+    UINT64  WriteThrough:1;           // 0 = Write-Back caching, 1=Write-Through caching
+    UINT64  CacheDisabled:1;          // 0 = Cached, 1=Non-Cached
+    UINT64  Accessed:1;               // 0 = Not accessed, 1 = Accessed (set by CPU)
+    UINT64  Dirty:1;                  // 0 = Not Dirty, 1 = written by processor on access to page
+    UINT64  MustBe1:1;                // Must be 1 
+    UINT64  Global:1;                 // 0 = Not global page, 1 = global page TLB not cleared on CR3 write
+    UINT64  Available:3;              // Available for use by system software
+    UINT64  PAT:1;                    //
+    UINT64  MustBeZero:17;            // Must be zero;
+    UINT64  PageTableBaseAddress:22;  // Page Table Base Address
+    UINT64  AvabilableHigh:11;        // Available for use by system software
+    UINT64  Nx:1;                     // 0 = Execute Code, 1 = No Code Execution
+  } Bits;
+  UINT64    Uint64;
+} PAGE_TABLE_1G_ENTRY;
+
+#pragma pack()
+
+typedef
+EFI_STATUS
+(*COALESCE_ENTRY) (
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR    *BlockList,
+  IN OUT VOID                        **MemoryBase,
+  IN OUT UINTN                       *MemorySize
+  );
+
+#endif
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.inf b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.inf
new file mode 100644
index 0000000000..d7aa37186f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.inf
@@ -0,0 +1,96 @@
+## @file
+# Capsule update PEIM supports EFI and UEFI.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - capsule image.
+#  This external input must be validated carefully to avoid security issue like
+#  buffer overflow, integer overflow.
+#
+# Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CapsulePei
+  MODULE_UNI_FILE                = CapsulePei.uni
+  FILE_GUID                      = C779F6D8-7113-4AA1-9648-EB1633C7D53B
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = CapsuleMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UefiCapsule.c
+  Capsule.h
+  Common/CapsuleCoalesce.c  
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  HobLib
+  BaseMemoryLib
+  PeiServicesLib
+  PeimEntryPoint
+  DebugLib
+  PeiServicesTablePointerLib
+  PrintLib
+  ReportStatusCodeLib
+
+[LibraryClasses.IA32]
+  PeCoffGetEntryPointLib
+  PcdLib
+  DebugAgentLib
+
+[Guids]
+  ## SOMETIMES_CONSUMES   ## Variable:L"CapsuleUpdateData"
+  ## SOMETIMES_CONSUMES   ## Variable:L"CapsuleLongModeBuffer"
+  gEfiCapsuleVendorGuid
+
+[Ppis]
+  gEfiPeiReadOnlyVariable2PpiGuid               ## CONSUMES
+  gEfiPeiCapsulePpiGuid                         ## PRODUCES
+  
+[Ppis.IA32]  
+  gEfiPeiLoadFilePpiGuid                        ## SOMETIMES_CONSUMES
+
+[Pcd.IA32]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdCapsuleCoalesceFile     ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable          ## SOMETIMES_CONSUMES
+
+[FeaturePcd.IA32]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode  ## CONSUMES
+
+[Depex]
+  gEfiPeiReadOnlyVariable2PpiGuid
+
+# [BootMode]
+# FLASH_UPDATE              ## SOMETIMES_CONSUMES
+
+# [Hob.IA32]
+# UNDEFINED                 ## SOMETIMES_CONSUMES # CPU
+
+# [Hob]
+# UNDEFINED                 ## SOMETIMES_PRODUCES # UEFI_CAPSULE
+
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  CapsulePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.uni b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.uni
new file mode 100644
index 0000000000..c082315258
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePei.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsulePeiExtra.uni b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePeiExtra.uni
new file mode 100644
index 0000000000..871be9b513
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsulePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.inf b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.inf
new file mode 100644
index 0000000000..1630dfc383
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.inf
@@ -0,0 +1,59 @@
+## @file
+# CapsuleX64 module handles >4GB capsule blocks.
+#
+# The X64 entrypoint to process capsule in long mode.
+# This module is built as X64.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - capsule image.
+#  This external input must be validated carefully to avoid security issue like
+#  buffer overflow, integer overflow.
+#
+# Copyright (c) 2011 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CapsuleX64
+  MODULE_UNI_FILE                = CapsuleX64.uni
+  FILE_GUID                      = F7FDE4A6-294C-493c-B50F-9734553BB757
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = X64
+#
+
+[Sources]
+  X64/X64Entry.c
+  X64/PageFaultHandler.asm
+  X64/PageFaultHandler.S
+  Common/CapsuleCoalesce.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  CpuExceptionHandlerLib
+  DebugAgentLib
+
+[Depex]
+  FALSE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  CapsuleX64Extra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.uni b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.uni
new file mode 100644
index 0000000000..10d3c4c5ce
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64Extra.uni b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64Extra.uni
new file mode 100644
index 0000000000..88d5e2057c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/CapsuleX64Extra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/Common/CapsuleCoalesce.c b/Core/MdeModulePkg/Universal/CapsulePei/Common/CapsuleCoalesce.c
new file mode 100644
index 0000000000..d789fbbc75
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/Common/CapsuleCoalesce.c
@@ -0,0 +1,1254 @@
+/** @file
+  The logic to process capsule.
+
+  Caution: This module requires additional review when modified.
+  This driver will have external input - capsule image.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  CapsuleDataCoalesce() will do basic validation before coalesce capsule data
+  into memory.
+
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2011 - 2014, 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 <Uefi.h>
+#include <PiPei.h>
+
+#include <Guid/CapsuleVendor.h>
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PrintLib.h>
+#include <Library/BaseLib.h>
+
+#include "CommonHeader.h"
+
+#define MIN_COALESCE_ADDR                     (1024 * 1024)
+
+/**
+  Given a pointer to the capsule block list, info on the available system
+  memory, and the size of a buffer, find a free block of memory where a
+  buffer of the given size can be copied to safely.
+
+  @param BlockList   Pointer to head of capsule block descriptors
+  @param MemBase     Pointer to the base of memory in which we want to find free space
+  @param MemSize     The size of the block of memory pointed to by MemBase
+  @param DataSize    How big a free block we want to find
+
+  @return A pointer to a memory block of at least DataSize that lies somewhere 
+          between MemBase and (MemBase + MemSize). The memory pointed to does not
+          contain any of the capsule block descriptors or capsule blocks pointed to
+          by the BlockList.
+
+**/
+UINT8 *
+FindFreeMem (
+  EFI_CAPSULE_BLOCK_DESCRIPTOR     *BlockList,
+  UINT8                            *MemBase,
+  UINTN                             MemSize,
+  UINTN                             DataSize
+  );
+
+/**
+  Check the integrity of the capsule descriptors.
+
+  @param BlockList    Pointer to the capsule descriptors
+
+  @retval NULL           BlockList is not valid.
+  @retval LastBlockDesc  Last one Block in BlockList
+
+**/
+EFI_CAPSULE_BLOCK_DESCRIPTOR *
+ValidateCapsuleIntegrity (
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR    *BlockList
+  );
+
+/**
+  The capsule block descriptors may be fragmented and spread all over memory.
+  To simplify the coalescing of capsule blocks, first coalesce all the
+  capsule block descriptors low in memory.
+
+  The descriptors passed in can be fragmented throughout memory. Here
+  they are relocated into memory to turn them into a contiguous (null
+  terminated) array.
+
+  @param PeiServices    pointer to PEI services table
+  @param BlockList      pointer to the capsule block descriptors
+  @param NumDescriptors number of capsule data block descriptors, whose Length is non-zero.
+  @param MemBase        base of system memory in which we can work
+  @param MemSize        size of the system memory pointed to by MemBase
+
+  @retval NULL    could not relocate the descriptors
+  @retval Pointer to the base of the successfully-relocated block descriptors. 
+
+**/
+EFI_CAPSULE_BLOCK_DESCRIPTOR *
+RelocateBlockDescriptors (
+  IN EFI_PEI_SERVICES                  **PeiServices,
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR      *BlockList,
+  IN UINTN                              NumDescriptors,
+  IN UINT8                             *MemBase,
+  IN UINTN                             MemSize
+  );
+
+/**
+  Check every capsule header.
+
+  @param CapsuleHeader   The pointer to EFI_CAPSULE_HEADER
+
+  @retval FALSE  Capsule is OK
+  @retval TRUE   Capsule is corrupted 
+
+**/
+BOOLEAN
+IsCapsuleCorrupted (
+  IN EFI_CAPSULE_HEADER       *CapsuleHeader
+  );
+
+/**
+  Determine if two buffers overlap in memory.
+
+  @param Buff1   pointer to first buffer
+  @param Size1   size of Buff1
+  @param Buff2   pointer to second buffer
+  @param Size2   size of Buff2
+
+  @retval TRUE    Buffers overlap in memory.
+  @retval FALSE   Buffer doesn't overlap.
+
+**/
+BOOLEAN
+IsOverlapped (
+  UINT8     *Buff1,
+  UINTN     Size1,
+  UINT8     *Buff2,
+  UINTN     Size2
+  );
+
+/**
+  Given a pointer to a capsule block descriptor, traverse the list to figure
+  out how many legitimate descriptors there are, and how big the capsule it
+  refers to is.
+
+  @param Desc            Pointer to the capsule block descriptors
+  @param NumDescriptors  Optional pointer to where to return the number of capsule data descriptors, whose Length is non-zero.
+  @param CapsuleSize     Optional pointer to where to return the capsule image size
+  @param CapsuleNumber   Optional pointer to where to return the number of capsule
+
+  @retval EFI_NOT_FOUND   No descriptors containing data in the list
+  @retval EFI_SUCCESS     Return data is valid
+
+**/
+EFI_STATUS
+GetCapsuleInfo (
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR   *Desc,
+  IN OUT UINTN                      *NumDescriptors OPTIONAL,
+  IN OUT UINTN                      *CapsuleSize OPTIONAL,
+  IN OUT UINTN                      *CapsuleNumber OPTIONAL
+  );
+
+/**
+  Given a pointer to the capsule block list, info on the available system
+  memory, and the size of a buffer, find a free block of memory where a
+  buffer of the given size can be copied to safely.
+
+  @param BlockList   Pointer to head of capsule block descriptors
+  @param MemBase     Pointer to the base of memory in which we want to find free space
+  @param MemSize     The size of the block of memory pointed to by MemBase
+  @param DataSize    How big a free block we want to find
+
+  @return A pointer to a memory block of at least DataSize that lies somewhere 
+          between MemBase and (MemBase + MemSize). The memory pointed to does not
+          contain any of the capsule block descriptors or capsule blocks pointed to
+          by the BlockList.
+
+**/
+UINT8 *
+FindFreeMem (
+  EFI_CAPSULE_BLOCK_DESCRIPTOR      *BlockList,
+  UINT8                             *MemBase,
+  UINTN                             MemSize,
+  UINTN                             DataSize
+  )
+{
+  UINTN                           Size;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR    *CurrDesc;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR    *TempDesc;
+  UINT8                           *MemEnd;
+  BOOLEAN                         Failed;
+
+  //
+  // Need at least enough to copy the data to at the end of the buffer, so
+  // say the end is less the data size for easy comparisons here.
+  //
+  MemEnd    = MemBase + MemSize - DataSize;
+  CurrDesc  = BlockList;
+  //
+  // Go through all the descriptor blocks and see if any obstruct the range
+  //
+  while (CurrDesc != NULL) {
+    //
+    // Get the size of this block list and see if it's in the way
+    //
+    Failed    = FALSE;
+    TempDesc  = CurrDesc;
+    Size      = sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+    while (TempDesc->Length != 0) {
+      Size += sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+      TempDesc++;
+    }
+
+    if (IsOverlapped (MemBase, DataSize, (UINT8 *) CurrDesc, Size)) {
+      //
+      // Set our new base to the end of this block list and start all over
+      //
+      MemBase   = (UINT8 *) CurrDesc + Size;
+      CurrDesc  = BlockList;
+      if (MemBase > MemEnd) {
+        return NULL;
+      }
+
+      Failed = TRUE;
+    }
+    //
+    // Now go through all the blocks and make sure none are in the way
+    //
+    while ((CurrDesc->Length != 0) && (!Failed)) {
+      if (IsOverlapped (MemBase, DataSize, (UINT8 *) (UINTN) CurrDesc->Union.DataBlock, (UINTN) CurrDesc->Length)) {
+        //
+        // Set our new base to the end of this block and start all over
+        //
+        Failed    = TRUE;
+        MemBase   = (UINT8 *) ((UINTN) CurrDesc->Union.DataBlock) + CurrDesc->Length;
+        CurrDesc  = BlockList;
+        if (MemBase > MemEnd) {
+          return NULL;
+        }
+      }
+      CurrDesc++;
+    }
+    //
+    // Normal continuation -- jump to next block descriptor list
+    //
+    if (!Failed) {
+      CurrDesc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) CurrDesc->Union.ContinuationPointer;
+    }
+  }
+  return MemBase;
+}
+
+/**
+  Check the integrity of the capsule descriptors.
+
+  @param BlockList    Pointer to the capsule descriptors
+
+  @retval NULL           BlockList is not valid.
+  @retval LastBlockDesc  Last one Block in BlockList
+
+**/
+EFI_CAPSULE_BLOCK_DESCRIPTOR *
+ValidateCapsuleIntegrity (
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR    *BlockList
+  )
+{
+  EFI_CAPSULE_HEADER             *CapsuleHeader;
+  UINT64                         CapsuleSize;
+  UINTN                          CapsuleCount;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *Ptr;
+
+  DEBUG ((EFI_D_INFO, "ValidateCapsuleIntegrity\n"));
+
+  //
+  // Go through the list to look for inconsistencies. Check for:
+  //   * misaligned block descriptors.
+  //   * The first capsule header guid
+  //   * The first capsule header flag
+  //   * The first capsule header HeaderSize
+  //   * Length > MAX_ADDRESS
+  //   * ContinuationPointer > MAX_ADDRESS
+  //   * DataBlock + Length > MAX_ADDRESS
+  //
+  CapsuleSize  = 0;
+  CapsuleCount = 0;
+  Ptr = BlockList;
+
+  DEBUG ((EFI_D_INFO, "Ptr - 0x%x\n", Ptr));
+  DEBUG ((EFI_D_INFO, "Ptr->Length - 0x%x\n", Ptr->Length));
+  DEBUG ((EFI_D_INFO, "Ptr->Union - 0x%x\n", Ptr->Union.ContinuationPointer));
+  while ((Ptr->Length != 0) || (Ptr->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+    //
+    // Make sure the descriptor is aligned at UINT64 in memory
+    //
+    if ((UINTN) Ptr & (sizeof(UINT64) - 1)) {
+      DEBUG ((EFI_D_ERROR, "ERROR: BlockList address failed alignment check\n"));
+      return NULL;
+    }
+    //
+    // Sanity Check
+    //
+    if (Ptr->Length > MAX_ADDRESS) {
+      DEBUG ((EFI_D_ERROR, "ERROR: Ptr->Length(0x%lx) > MAX_ADDRESS\n", Ptr->Length));
+      return NULL;
+    }
+
+    if (Ptr->Length == 0) {
+      //
+      // Sanity Check
+      //
+      if (Ptr->Union.ContinuationPointer > MAX_ADDRESS) {
+        DEBUG ((EFI_D_ERROR, "ERROR: Ptr->Union.ContinuationPointer(0x%lx) > MAX_ADDRESS\n", Ptr->Union.ContinuationPointer));
+        return NULL;
+      }
+      //
+      // Descriptor points to another list of block descriptors somewhere
+      // else.
+      //
+      Ptr = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) Ptr->Union.ContinuationPointer;
+      DEBUG ((EFI_D_INFO, "Ptr(C) - 0x%x\n", Ptr));
+      DEBUG ((EFI_D_INFO, "Ptr->Length - 0x%x\n", Ptr->Length));
+      DEBUG ((EFI_D_INFO, "Ptr->Union - 0x%x\n", Ptr->Union.ContinuationPointer));
+    } else {
+      //
+      // Sanity Check
+      //
+      if (Ptr->Union.DataBlock > (MAX_ADDRESS - (UINTN)Ptr->Length)) {
+        DEBUG ((EFI_D_ERROR, "ERROR: Ptr->Union.DataBlock(0x%lx) > (MAX_ADDRESS - (UINTN)Ptr->Length(0x%lx))\n", Ptr->Union.DataBlock, Ptr->Length));
+        return NULL;
+      }
+
+      //
+      //To enhance the reliability of check-up, the first capsule's header is checked here.
+      //More reliabilities check-up will do later.
+      //
+      if (CapsuleSize == 0) {
+        //
+        //Move to the first capsule to check its header.
+        //
+        CapsuleHeader = (EFI_CAPSULE_HEADER*)((UINTN)Ptr->Union.DataBlock);
+        //
+        // Sanity check
+        //
+        if (Ptr->Length < sizeof(EFI_CAPSULE_HEADER)) {
+          DEBUG ((EFI_D_ERROR, "ERROR: Ptr->Length(0x%lx) < sizeof(EFI_CAPSULE_HEADER)\n", Ptr->Length));
+          return NULL;
+        }
+        //
+        // Make sure HeaderSize field is valid
+        //
+        if (CapsuleHeader->HeaderSize > CapsuleHeader->CapsuleImageSize) {
+          DEBUG ((EFI_D_ERROR, "ERROR: CapsuleHeader->HeaderSize(0x%x) > CapsuleHeader->CapsuleImageSize(0x%x)\n", CapsuleHeader->HeaderSize, CapsuleHeader->CapsuleImageSize));
+          return NULL;
+        }
+        if (IsCapsuleCorrupted (CapsuleHeader)) {
+          return NULL;
+        }
+        CapsuleCount ++;
+        CapsuleSize = CapsuleHeader->CapsuleImageSize;
+      }
+
+      if (CapsuleSize >= Ptr->Length) {
+        CapsuleSize = CapsuleSize - Ptr->Length;
+      } else {
+        DEBUG ((EFI_D_ERROR, "ERROR: CapsuleSize(0x%lx) < Ptr->Length(0x%lx)\n", CapsuleSize, Ptr->Length));
+        //
+        // Sanity check
+        //
+        return NULL;
+      }
+
+      //
+      // Move to next BLOCK descriptor
+      //
+      Ptr++;
+      DEBUG ((EFI_D_INFO, "Ptr(B) - 0x%x\n", Ptr));
+      DEBUG ((EFI_D_INFO, "Ptr->Length - 0x%x\n", Ptr->Length));
+      DEBUG ((EFI_D_INFO, "Ptr->Union - 0x%x\n", Ptr->Union.ContinuationPointer));
+    }
+  }
+
+  if (CapsuleCount == 0) {
+    //
+    // No any capsule is found in BlockList
+    //
+    DEBUG ((EFI_D_ERROR, "ERROR: CapsuleCount(0x%x) == 0\n", CapsuleCount));
+    return NULL;
+  }
+
+  if (CapsuleSize != 0) {
+    //
+    // Capsule data is incomplete.
+    //
+    DEBUG ((EFI_D_ERROR, "ERROR: CapsuleSize(0x%lx) != 0\n", CapsuleSize));
+    return NULL;
+  }
+
+  return Ptr;
+}
+
+/**
+  The capsule block descriptors may be fragmented and spread all over memory.
+  To simplify the coalescing of capsule blocks, first coalesce all the
+  capsule block descriptors low in memory.
+
+  The descriptors passed in can be fragmented throughout memory. Here
+  they are relocated into memory to turn them into a contiguous (null
+  terminated) array.
+
+  @param PeiServices    pointer to PEI services table
+  @param BlockList      pointer to the capsule block descriptors
+  @param NumDescriptors number of capsule data block descriptors, whose Length is non-zero.
+  @param MemBase        base of system memory in which we can work
+  @param MemSize        size of the system memory pointed to by MemBase
+
+  @retval NULL    could not relocate the descriptors
+  @retval Pointer to the base of the successfully-relocated block descriptors. 
+
+**/
+EFI_CAPSULE_BLOCK_DESCRIPTOR  *
+RelocateBlockDescriptors (
+  IN EFI_PEI_SERVICES                   **PeiServices,
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR       *BlockList,
+  IN UINTN                              NumDescriptors,
+  IN UINT8                              *MemBase,
+  IN UINTN                              MemSize
+  )
+{
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *NewBlockList;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *CurrBlockDescHead;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *TempBlockDesc;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *PrevBlockDescTail;
+  UINTN                          BufferSize;
+  UINT8                          *RelocBuffer;
+  UINTN                          BlockListSize;
+
+  //
+  // Get the info on the blocks and descriptors. Since we're going to move
+  // the descriptors low in memory, adjust the base/size values accordingly here.
+  // NumDescriptors is the number of legit data descriptors, so add one for
+  // a terminator. (Already done by caller, no check is needed.)
+  //
+
+  BufferSize    = NumDescriptors * sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+  NewBlockList  = (EFI_CAPSULE_BLOCK_DESCRIPTOR *) MemBase;
+  if (MemSize < BufferSize) {
+    return NULL;
+  }
+
+  MemSize -= BufferSize;
+  MemBase += BufferSize;
+  //
+  // Go through all the blocks and make sure none are in the way
+  //
+  TempBlockDesc = BlockList;
+  while (TempBlockDesc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL) {
+    if (TempBlockDesc->Length == 0) {
+      //
+      // Next block of descriptors
+      //
+      TempBlockDesc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) TempBlockDesc->Union.ContinuationPointer;
+    } else {
+      //
+      // If the capsule data pointed to by this descriptor is in the way,
+      // move it.
+      //
+      if (IsOverlapped (
+            (UINT8 *) NewBlockList,
+            BufferSize,
+            (UINT8 *) (UINTN) TempBlockDesc->Union.DataBlock,
+            (UINTN) TempBlockDesc->Length
+            )) {
+        //
+        // Relocate the block
+        //
+        RelocBuffer = FindFreeMem (BlockList, MemBase, MemSize, (UINTN) TempBlockDesc->Length);
+        if (RelocBuffer == NULL) {
+          return NULL;
+        }
+
+        CopyMem ((VOID *) RelocBuffer, (VOID *) (UINTN) TempBlockDesc->Union.DataBlock, (UINTN) TempBlockDesc->Length);
+        DEBUG ((EFI_D_INFO, "Capsule relocate descriptors from/to/size  0x%lX 0x%lX 0x%lX\n", TempBlockDesc->Union.DataBlock, (UINT64)(UINTN)RelocBuffer, TempBlockDesc->Length));
+        TempBlockDesc->Union.DataBlock = (EFI_PHYSICAL_ADDRESS) (UINTN) RelocBuffer;
+      }
+      TempBlockDesc++;
+    }
+  }
+  //
+  // Now go through all the block descriptors to make sure that they're not
+  // in the memory region we want to copy them to.
+  //
+  CurrBlockDescHead = BlockList;
+  PrevBlockDescTail = NULL;
+  while ((CurrBlockDescHead != NULL) && (CurrBlockDescHead->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+    //
+    // Get the size of this list then see if it overlaps our low region
+    //
+    TempBlockDesc = CurrBlockDescHead;
+    BlockListSize = sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+    while (TempBlockDesc->Length != 0) {
+      BlockListSize += sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+      TempBlockDesc++;
+    }
+
+    if (IsOverlapped (
+          (UINT8 *) NewBlockList,
+          BufferSize,
+          (UINT8 *) CurrBlockDescHead,
+          BlockListSize
+          )) {
+      //
+      // Overlaps, so move it out of the way
+      //
+      RelocBuffer = FindFreeMem (BlockList, MemBase, MemSize, BlockListSize);
+      if (RelocBuffer == NULL) {
+        return NULL;
+      }
+      CopyMem ((VOID *) RelocBuffer, (VOID *) CurrBlockDescHead, BlockListSize);
+      DEBUG ((EFI_D_INFO, "Capsule reloc descriptor block #2\n"));
+      //
+      // Point the previous block's next point to this copied version. If
+      // the tail pointer is null, then this is the first descriptor block.
+      //
+      if (PrevBlockDescTail == NULL) {
+        BlockList = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) RelocBuffer;
+      } else {
+        PrevBlockDescTail->Union.DataBlock = (EFI_PHYSICAL_ADDRESS) (UINTN) RelocBuffer;
+      }
+    }
+    //
+    // Save our new tail and jump to the next block list
+    //
+    PrevBlockDescTail = TempBlockDesc;
+    CurrBlockDescHead = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) TempBlockDesc->Union.ContinuationPointer;
+  }
+  //
+  // Cleared out low memory. Now copy the descriptors down there.
+  //
+  TempBlockDesc     = BlockList;
+  CurrBlockDescHead = NewBlockList;
+  while ((TempBlockDesc != NULL) && (TempBlockDesc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+    if (TempBlockDesc->Length != 0) {
+      CurrBlockDescHead->Union.DataBlock = TempBlockDesc->Union.DataBlock;
+      CurrBlockDescHead->Length = TempBlockDesc->Length;
+      CurrBlockDescHead++;
+      TempBlockDesc++;
+    } else {
+      TempBlockDesc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) TempBlockDesc->Union.ContinuationPointer;
+    }
+  }
+  //
+  // Null terminate
+  //
+  CurrBlockDescHead->Union.ContinuationPointer   = (EFI_PHYSICAL_ADDRESS) (UINTN) NULL;
+  CurrBlockDescHead->Length = 0;
+  return NewBlockList;
+}
+
+/**
+  Determine if two buffers overlap in memory.
+
+  @param Buff1   pointer to first buffer
+  @param Size1   size of Buff1
+  @param Buff2   pointer to second buffer
+  @param Size2   size of Buff2
+
+  @retval TRUE    Buffers overlap in memory.
+  @retval FALSE   Buffer doesn't overlap.
+
+**/
+BOOLEAN
+IsOverlapped (
+  UINT8     *Buff1,
+  UINTN     Size1,
+  UINT8     *Buff2,
+  UINTN     Size2
+  )
+{
+  //
+  // If buff1's end is less than the start of buff2, then it's ok.
+  // Also, if buff1's start is beyond buff2's end, then it's ok.
+  //
+  if (((Buff1 + Size1) <= Buff2) || (Buff1 >= (Buff2 + Size2))) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Given a pointer to a capsule block descriptor, traverse the list to figure
+  out how many legitimate descriptors there are, and how big the capsule it
+  refers to is.
+
+  @param Desc            Pointer to the capsule block descriptors
+  @param NumDescriptors  Optional pointer to where to return the number of capsule data descriptors, whose Length is non-zero.
+  @param CapsuleSize     Optional pointer to where to return the capsule image size
+  @param CapsuleNumber   Optional pointer to where to return the number of capsule
+
+  @retval EFI_NOT_FOUND   No descriptors containing data in the list
+  @retval EFI_SUCCESS     Return data is valid
+
+**/
+EFI_STATUS
+GetCapsuleInfo (
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR   *Desc,
+  IN OUT UINTN                      *NumDescriptors OPTIONAL,
+  IN OUT UINTN                      *CapsuleSize OPTIONAL,
+  IN OUT UINTN                      *CapsuleNumber OPTIONAL
+  )
+{
+  UINTN                          Count;
+  UINTN                          Size;
+  UINTN                          Number;
+  UINTN                          ThisCapsuleImageSize;
+  EFI_CAPSULE_HEADER             *CapsuleHeader;
+
+  DEBUG ((EFI_D_INFO, "GetCapsuleInfo enter\n"));
+
+  ASSERT (Desc != NULL);
+
+  Count = 0;
+  Size  = 0;
+  Number = 0;
+  ThisCapsuleImageSize = 0;
+
+  while (Desc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL) {
+    if (Desc->Length == 0) {
+      //
+      // Descriptor points to another list of block descriptors somewhere
+      //
+      Desc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) Desc->Union.ContinuationPointer;
+    } else {
+      //
+      // Sanity Check
+      // It is needed, because ValidateCapsuleIntegrity() only validate one individual capsule Size.
+      // While here we need check all capsules size.
+      //
+      if (Desc->Length >= (MAX_ADDRESS - Size)) {
+        DEBUG ((EFI_D_ERROR, "ERROR: Desc->Length(0x%lx) >= (MAX_ADDRESS - Size(0x%x))\n", Desc->Length, Size));
+        return EFI_OUT_OF_RESOURCES;
+      }
+      Size += (UINTN) Desc->Length;
+      Count++;
+
+      //
+      // See if this is first capsule's header
+      //
+      if (ThisCapsuleImageSize == 0) {
+        CapsuleHeader = (EFI_CAPSULE_HEADER*)((UINTN)Desc->Union.DataBlock);
+        //
+        // This has been checked in ValidateCapsuleIntegrity()
+        //
+        Number ++;
+        ThisCapsuleImageSize = CapsuleHeader->CapsuleImageSize;
+      }
+
+      //
+      // This has been checked in ValidateCapsuleIntegrity()
+      //
+      ASSERT (ThisCapsuleImageSize >= Desc->Length);
+      ThisCapsuleImageSize = (UINTN)(ThisCapsuleImageSize - Desc->Length);
+
+      //
+      // Move to next
+      //
+      Desc++;
+    }
+  }
+  //
+  // If no descriptors, then fail
+  //
+  if (Count == 0) {
+    DEBUG ((EFI_D_ERROR, "ERROR: Count == 0\n"));
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // checked in ValidateCapsuleIntegrity()
+  //
+  ASSERT (ThisCapsuleImageSize == 0);
+
+  if (NumDescriptors != NULL) {
+    *NumDescriptors = Count;
+  }
+
+  if (CapsuleSize != NULL) {
+    *CapsuleSize = Size;
+  }
+
+  if (CapsuleNumber != NULL) {
+    *CapsuleNumber = Number;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check every capsule header.
+
+  @param CapsuleHeader   The pointer to EFI_CAPSULE_HEADER
+
+  @retval FALSE  Capsule is OK
+  @retval TRUE   Capsule is corrupted 
+
+**/
+BOOLEAN
+IsCapsuleCorrupted (
+  IN EFI_CAPSULE_HEADER       *CapsuleHeader
+  )
+{
+  //
+  //A capsule to be updated across a system reset should contain CAPSULE_FLAGS_PERSIST_ACROSS_RESET.
+  //
+  if ((CapsuleHeader->Flags & CAPSULE_FLAGS_PERSIST_ACROSS_RESET) == 0) {
+    return TRUE;
+  }
+  //
+  //Make sure the flags combination is supported by the platform.
+  //
+  if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE)) == CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) {
+    return TRUE;
+  }
+  if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_INITIATE_RESET)) == CAPSULE_FLAGS_INITIATE_RESET) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Try to verify the integrity of a capsule test pattern before the
+  capsule gets coalesced. This can be useful in narrowing down
+  where capsule data corruption occurs.
+
+  The test pattern mode fills in memory with a counting UINT32 value. 
+  If the capsule is not divided up in a multiple of 4-byte blocks, then
+  things get messy doing the check. Therefore there are some cases
+  here where we just give up and skip the pre-coalesce check.
+
+  @param PeiServices  PEI services table
+  @param Desc         Pointer to capsule descriptors
+**/
+VOID
+CapsuleTestPatternPreCoalesce (
+  IN EFI_PEI_SERVICES              **PeiServices,
+  IN EFI_CAPSULE_BLOCK_DESCRIPTOR  *Desc
+  )
+{
+  UINT32  *TestPtr;
+  UINT32  TestCounter;
+  UINT32  TestSize;
+
+  DEBUG ((EFI_D_INFO, "CapsuleTestPatternPreCoalesce\n"));
+
+  //
+  // Find first data descriptor
+  //
+  while ((Desc->Length == 0) && (Desc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+    Desc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) Desc->Union.ContinuationPointer;
+  }
+
+  if (Desc->Union.ContinuationPointer == 0) {
+    return ;
+  }
+  //
+  // First one better be long enough to at least hold the test signature
+  //
+  if (Desc->Length < sizeof (UINT32)) {
+    DEBUG ((EFI_D_INFO, "Capsule test pattern pre-coalesce punted #1\n"));
+    return ;
+  }
+
+  TestPtr = (UINT32 *) (UINTN) Desc->Union.DataBlock;
+  //
+  // 0x54534554 "TEST"
+  //
+  if (*TestPtr != 0x54534554) {
+    return ;
+  }
+
+  TestCounter = 0;
+  TestSize    = (UINT32) Desc->Length - 2 * sizeof (UINT32);
+  //
+  // Skip over the signature and the size fields in the pattern data header
+  //
+  TestPtr += 2;
+  while (1) {
+    if ((TestSize & 0x03) != 0) {
+      DEBUG ((EFI_D_INFO, "Capsule test pattern pre-coalesce punted #2\n"));
+      return ;
+    }
+
+    while (TestSize > 0) {
+      if (*TestPtr != TestCounter) {
+        DEBUG ((EFI_D_INFO, "Capsule test pattern pre-coalesce failed data corruption check\n"));
+        return ;
+      }
+
+      TestSize -= sizeof (UINT32);
+      TestCounter++;
+      TestPtr++;
+    }
+    Desc++;
+    while ((Desc->Length == 0) && (Desc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+      Desc = (EFI_CAPSULE_BLOCK_DESCRIPTOR  *) (UINTN) Desc->Union.ContinuationPointer;
+    }
+
+    if (Desc->Union.ContinuationPointer == (EFI_PHYSICAL_ADDRESS) (UINTN) NULL) {
+      return ;
+    }
+    TestSize = (UINT32) Desc->Length;
+    TestPtr  = (UINT32 *) (UINTN) Desc->Union.DataBlock;
+  }
+}
+
+/**
+  Checks for the presence of capsule descriptors.
+  Get capsule descriptors from variable CapsuleUpdateData, CapsuleUpdateData1, CapsuleUpdateData2...
+
+  @param BlockListBuffer            Pointer to the buffer of capsule descriptors variables
+  @param BlockDescriptorList        Pointer to the capsule descriptors list
+
+  @retval EFI_SUCCESS               a valid capsule is present
+  @retval EFI_NOT_FOUND             if a valid capsule is not present
+**/
+EFI_STATUS
+BuildCapsuleDescriptors (
+  IN  EFI_PHYSICAL_ADDRESS            *BlockListBuffer,
+  OUT EFI_CAPSULE_BLOCK_DESCRIPTOR    **BlockDescriptorList 
+  )
+{
+  UINTN                            Index;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR     *LastBlock;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR     *TempBlock;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR     *HeadBlock;
+
+  DEBUG ((EFI_D_INFO, "BuildCapsuleDescriptors enter\n"));
+
+  LastBlock         = NULL;
+  HeadBlock         = NULL;
+  TempBlock         = NULL;
+  Index             = 0;
+
+  while (BlockListBuffer[Index] != 0) {
+    //
+    // Test integrity of descriptors.
+    //
+    if (BlockListBuffer[Index] < MAX_ADDRESS) {
+      TempBlock = ValidateCapsuleIntegrity ((EFI_CAPSULE_BLOCK_DESCRIPTOR *)(UINTN)BlockListBuffer[Index]);
+      if (TempBlock != NULL) {
+        if (LastBlock == NULL) {
+          LastBlock = TempBlock;
+
+          //
+          // Return the base of the block descriptors
+          //
+          HeadBlock = (EFI_CAPSULE_BLOCK_DESCRIPTOR *)(UINTN)BlockListBuffer[Index];
+        } else {
+          //
+          // Combine the different BlockList into single BlockList.
+          //
+          LastBlock->Union.DataBlock = (EFI_PHYSICAL_ADDRESS)(UINTN)BlockListBuffer[Index];
+          LastBlock->Length          = 0;
+          LastBlock                  = TempBlock;
+        }
+      }
+    } else {
+      DEBUG ((EFI_D_ERROR, "ERROR: BlockListBuffer[Index](0x%lx) < MAX_ADDRESS\n", BlockListBuffer[Index]));
+    }
+    Index ++;
+  }
+  
+  if (HeadBlock != NULL) {
+    *BlockDescriptorList = HeadBlock;
+    return EFI_SUCCESS;
+  }
+  return EFI_NOT_FOUND;
+}
+
+/**
+  The function to coalesce a fragmented capsule in memory.
+
+  Memory Map for coalesced capsule:
+  MemBase +   ---->+---------------------------+<-----------+
+  MemSize          | ------------------------- |            |
+                   | |  Capsule [Num-1]      | |            |
+                   | ------------------------- |            |
+                   | |  ................     | |            |
+                   | ------------------------- |            |
+                   | |  Capsule [1]          | |            |
+                   | ------------------------- |            |
+                   | |  Capsule [0]          | |            |
+                   | ------------------------- |            |
+                   |    Capsule Image          |            |   
+CapsuleImageBase-->+---------------------------+
+                   | ------------------------- |            |
+                   | |  CapsuleOffset[Num-1] | |            |
+                   | ------------------------- |            |
+                   | |  ................     | |        CapsuleSize
+                   | ------------------------- |            |
+                   | |  CapsuleOffset[1]     | |            |
+                   | ------------------------- |            |
+                   | |  CapsuleOffset[0]     | |            |
+                   |---------------------------|            |
+                   | |  CapsuleNumber        | |            |
+                   | ------------------------- |            |
+                   | |  CapsuleAllImageSize  | |            |
+                   | ------------------------- |            |
+                   |    PrivateData            |            |
+     DestPtr  ---->+---------------------------+<-----------+
+                   |                           |            |
+                   |     FreeMem               |        FreeMemSize
+                   |                           |            |
+   FreeMemBase --->+---------------------------+<-----------+
+                   |    Terminator             |
+                   +---------------------------+
+                   |    BlockDescriptor n      |
+                   +---------------------------+
+                   |    .................      |
+                   +---------------------------+
+                   |    BlockDescriptor 1      |
+                   +---------------------------+
+                   |    BlockDescriptor 0      |
+                   +---------------------------+
+                   |    PrivateDataDesc 0      |
+      MemBase ---->+---------------------------+<----- BlockList
+
+  Caution: This function may receive untrusted input.
+  The capsule data is external input, so this routine will do basic validation before
+  coalesce capsule data into memory.
+
+  @param PeiServices        General purpose services available to every PEIM.
+  @param BlockListBuffer    Point to the buffer of Capsule Descriptor Variables.
+  @param MemoryBase         Pointer to the base of a block of memory that we can walk
+                            all over while trying to coalesce our buffers.
+                            On output, this variable will hold the base address of
+                            a coalesced capsule.
+  @param MemorySize         Size of the memory region pointed to by MemoryBase.
+                            On output, this variable will contain the size of the
+                            coalesced capsule.
+
+  @retval EFI_NOT_FOUND     If we could not find the capsule descriptors.
+
+  @retval EFI_BUFFER_TOO_SMALL
+                            If we could not coalesce the capsule in the memory
+                            region provided to us.
+
+  @retval EFI_SUCCESS       Processed the capsule successfully.
+**/
+EFI_STATUS
+EFIAPI
+CapsuleDataCoalesce (
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN EFI_PHYSICAL_ADDRESS            *BlockListBuffer,
+  IN OUT VOID                        **MemoryBase,
+  IN OUT UINTN                       *MemorySize
+  )
+{
+  VOID                           *NewCapsuleBase;
+  VOID                           *CapsuleImageBase;
+  UINTN                          CapsuleIndex;
+  UINT8                          *FreeMemBase;
+  UINT8                          *DestPtr;
+  UINTN                          DestLength;
+  UINT8                          *RelocPtr;
+  UINTN                          CapsuleTimes; 
+  UINT64                         SizeLeft; 
+  UINT64                         CapsuleImageSize; 
+  UINTN                          CapsuleSize;
+  UINTN                          CapsuleNumber;
+  UINTN                          DescriptorsSize;
+  UINTN                          FreeMemSize;
+  UINTN                          NumDescriptors;
+  BOOLEAN                        CapsuleBeginFlag;
+  EFI_STATUS                     Status;
+  EFI_CAPSULE_HEADER             *CapsuleHeader;
+  EFI_CAPSULE_PEIM_PRIVATE_DATA  PrivateData;
+  EFI_CAPSULE_PEIM_PRIVATE_DATA  *PrivateDataPtr;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *BlockList;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *CurrentBlockDesc;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   *TempBlockDesc;
+  EFI_CAPSULE_BLOCK_DESCRIPTOR   PrivateDataDesc[2];
+
+  DEBUG ((EFI_D_INFO, "CapsuleDataCoalesce enter\n"));
+
+  CapsuleIndex     = 0;
+  SizeLeft         = 0;
+  CapsuleTimes     = 0;
+  CapsuleImageSize = 0;
+  PrivateDataPtr   = NULL;
+  CapsuleHeader    = NULL;
+  CapsuleBeginFlag = TRUE;
+  CapsuleSize      = 0;
+  NumDescriptors   = 0;
+
+  //
+  // Build capsule descriptors list
+  //
+  Status = BuildCapsuleDescriptors (BlockListBuffer, &BlockList);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DEBUG_CODE (
+    CapsuleTestPatternPreCoalesce (PeiServices, BlockList);
+  );
+
+  //
+  // Get the size of our descriptors and the capsule size. GetCapsuleInfo()
+  // returns the number of descriptors that actually point to data, so add
+  // one for a terminator. Do that below.
+  //
+  Status = GetCapsuleInfo (BlockList, &NumDescriptors, &CapsuleSize, &CapsuleNumber);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  DEBUG ((EFI_D_INFO, "CapsuleSize - 0x%x\n", CapsuleSize));
+  DEBUG ((EFI_D_INFO, "CapsuleNumber - 0x%x\n", CapsuleNumber));
+  DEBUG ((EFI_D_INFO, "NumDescriptors - 0x%x\n", NumDescriptors));
+  if ((CapsuleSize == 0) || (NumDescriptors == 0) || (CapsuleNumber == 0)) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (CapsuleNumber - 1 >= (MAX_ADDRESS - (sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA)  + sizeof(UINT64))) / sizeof(UINT64)) {
+    DEBUG ((EFI_D_ERROR, "ERROR: CapsuleNumber - 0x%x\n", CapsuleNumber));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Initialize our local copy of private data. When we're done, we'll create a
+  // descriptor for it as well so that it can be put into free memory without
+  // trashing anything.
+  //
+  PrivateData.Signature           = EFI_CAPSULE_PEIM_PRIVATE_DATA_SIGNATURE;
+  PrivateData.CapsuleAllImageSize = (UINT64) CapsuleSize;
+  PrivateData.CapsuleNumber       = (UINT64) CapsuleNumber;
+  PrivateData.CapsuleOffset[0]    = 0;
+  //
+  // NOTE: Only data in sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA) is valid, CapsuleOffset field is unitialized at this moment.
+  // The code sets partial length here for Descriptor.Length check, but later it will use full length to reserve those PrivateData region.
+  //
+  PrivateDataDesc[0].Union.DataBlock  = (EFI_PHYSICAL_ADDRESS) (UINTN) &PrivateData;
+  PrivateDataDesc[0].Length           = sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA);
+  PrivateDataDesc[1].Union.DataBlock  = (EFI_PHYSICAL_ADDRESS) (UINTN) BlockList;
+  PrivateDataDesc[1].Length           = 0;
+  //
+  // Add PrivateDataDesc[0] in beginning beginning, as it is new descriptor. PrivateDataDesc[1] is NOT needed.
+  // In addition, one NULL terminator is added in the end. See RelocateBlockDescriptors().
+  //
+  NumDescriptors  += 2;
+  //
+  // Sandity check
+  //
+  if (CapsuleSize >= (MAX_ADDRESS - (sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64) + sizeof(UINT64)))) {
+    DEBUG ((EFI_D_ERROR, "ERROR: CapsuleSize - 0x%x\n", CapsuleSize));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  //
+  // Need add sizeof(UINT64) for PrivateData alignment
+  //
+  CapsuleSize     += sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64) + sizeof(UINT64);
+  BlockList        = PrivateDataDesc;
+  //
+  // Sandity check
+  //
+  if (NumDescriptors >= (MAX_ADDRESS / sizeof(EFI_CAPSULE_BLOCK_DESCRIPTOR))) {
+    DEBUG ((EFI_D_ERROR, "ERROR: NumDescriptors - 0x%x\n", NumDescriptors));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  DescriptorsSize  = NumDescriptors * sizeof (EFI_CAPSULE_BLOCK_DESCRIPTOR);
+  //
+  // Sandity check
+  //
+  if (DescriptorsSize >= (MAX_ADDRESS - CapsuleSize)) {
+    DEBUG ((EFI_D_ERROR, "ERROR: DescriptorsSize - 0x%lx, CapsuleSize - 0x%lx\n", (UINT64)DescriptorsSize, (UINT64)CapsuleSize));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Don't go below some min address. If the base is below it,
+  // then move it up and adjust the size accordingly.
+  //
+  DEBUG ((EFI_D_INFO, "Capsule Memory range from 0x%8X to 0x%8X\n", (UINTN) *MemoryBase, (UINTN)*MemoryBase + *MemorySize));
+  if ((UINTN)*MemoryBase < (UINTN) MIN_COALESCE_ADDR) {
+    if (((UINTN)*MemoryBase + *MemorySize) < (UINTN) MIN_COALESCE_ADDR) {
+      DEBUG ((EFI_D_ERROR, "ERROR: *MemoryBase + *MemorySize - 0x%x\n", (UINTN)*MemoryBase + *MemorySize));
+      return EFI_BUFFER_TOO_SMALL;
+    } else {
+      *MemorySize = *MemorySize - ((UINTN) MIN_COALESCE_ADDR - (UINTN) *MemoryBase);
+      *MemoryBase = (VOID *) (UINTN) MIN_COALESCE_ADDR;
+    }
+  }
+
+  if (*MemorySize <= (CapsuleSize + DescriptorsSize)) {
+    DEBUG ((EFI_D_ERROR, "ERROR: CapsuleSize + DescriptorsSize - 0x%x\n", CapsuleSize + DescriptorsSize));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  FreeMemBase = *MemoryBase;
+  FreeMemSize = *MemorySize;
+  DEBUG ((EFI_D_INFO, "Capsule Free Memory from 0x%8X to 0x%8X\n", (UINTN) FreeMemBase, (UINTN) FreeMemBase + FreeMemSize));
+
+  //
+  // Relocate all the block descriptors to low memory to make further
+  // processing easier.
+  //
+  BlockList = RelocateBlockDescriptors (PeiServices, BlockList, NumDescriptors, FreeMemBase, FreeMemSize);
+  if (BlockList == NULL) {
+    //
+    // Not enough room to relocate the descriptors
+    //
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Take the top of memory for the capsule. UINT64 align up.
+  //
+  DestPtr         = FreeMemBase + FreeMemSize - CapsuleSize;
+  DestPtr         = (UINT8 *) (((UINTN)DestPtr + sizeof (UINT64) - 1) & ~(sizeof (UINT64) - 1));
+  FreeMemBase     = (UINT8 *) BlockList + DescriptorsSize;
+  FreeMemSize     = (UINTN) DestPtr - (UINTN) FreeMemBase;
+  NewCapsuleBase  = (VOID *) DestPtr;
+  CapsuleImageBase = (UINT8 *)NewCapsuleBase + sizeof(EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64);
+
+  PrivateDataPtr = (EFI_CAPSULE_PEIM_PRIVATE_DATA *) NewCapsuleBase;
+
+  //
+  // Move all the blocks to the top (high) of memory.
+  // Relocate all the obstructing blocks. Note that the block descriptors
+  // were coalesced when they were relocated, so we can just ++ the pointer.
+  //
+  CurrentBlockDesc = BlockList;
+  while ((CurrentBlockDesc->Length != 0) || (CurrentBlockDesc->Union.ContinuationPointer != (EFI_PHYSICAL_ADDRESS) (UINTN) NULL)) {
+    if (CapsuleTimes == 0) {
+      //
+      // The first entry is the block descriptor for EFI_CAPSULE_PEIM_PRIVATE_DATA.
+      // CapsuleOffset field is uninitialized at this time. No need copy it, but need to reserve for future use.
+      //
+      ASSERT (CurrentBlockDesc->Union.DataBlock == (UINT64)(UINTN)&PrivateData);
+      DestLength = sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64);
+    } else {
+      DestLength = (UINTN)CurrentBlockDesc->Length;
+    }
+    //
+    // See if any of the remaining capsule blocks are in the way
+    //
+    TempBlockDesc = CurrentBlockDesc;
+    while (TempBlockDesc->Length != 0) {
+      //
+      // Is this block in the way of where we want to copy the current descriptor to?
+      //
+      if (IsOverlapped (
+            (UINT8 *) DestPtr,
+            (UINTN) DestLength,
+            (UINT8 *) (UINTN) TempBlockDesc->Union.DataBlock,
+            (UINTN) TempBlockDesc->Length
+            )) {
+        //
+        // Relocate the block
+        //
+        RelocPtr = FindFreeMem (BlockList, FreeMemBase, FreeMemSize, (UINTN) TempBlockDesc->Length);
+        if (RelocPtr == NULL) {
+          return EFI_BUFFER_TOO_SMALL;
+        }
+
+        CopyMem ((VOID *) RelocPtr, (VOID *) (UINTN) TempBlockDesc->Union.DataBlock, (UINTN) TempBlockDesc->Length);
+        DEBUG ((EFI_D_INFO, "Capsule reloc data block from 0x%8X to 0x%8X with size 0x%8X\n",
+                (UINTN) TempBlockDesc->Union.DataBlock, (UINTN) RelocPtr, (UINTN) TempBlockDesc->Length));
+
+        TempBlockDesc->Union.DataBlock = (EFI_PHYSICAL_ADDRESS) (UINTN) RelocPtr;
+      }
+      //
+      // Next descriptor
+      //
+      TempBlockDesc++;
+    }
+    //
+    // Ok, we made it through. Copy the block.
+    // we just support greping one capsule from the lists of block descs list.
+    //
+    CapsuleTimes ++;
+    //
+    //Skip the first block descriptor that filled with EFI_CAPSULE_PEIM_PRIVATE_DATA
+    //
+    if (CapsuleTimes > 1) {
+      //
+      //For every capsule entry point, check its header to determine whether to relocate it.
+      //If it is invalid, skip it and move on to the next capsule. If it is valid, relocate it.
+      //
+      if (CapsuleBeginFlag) {
+        CapsuleBeginFlag  = FALSE;
+        CapsuleHeader     = (EFI_CAPSULE_HEADER*)(UINTN)CurrentBlockDesc->Union.DataBlock;
+        SizeLeft          = CapsuleHeader->CapsuleImageSize;
+
+        //
+        // No more check here is needed, because IsCapsuleCorrupted() already in ValidateCapsuleIntegrity()
+        //
+        ASSERT (CapsuleIndex < CapsuleNumber);
+
+        //
+        // Relocate this capsule
+        //
+        CapsuleImageSize += SizeLeft;
+        //
+        // Cache the begin offset of this capsule
+        //
+        ASSERT (PrivateDataPtr->Signature == EFI_CAPSULE_PEIM_PRIVATE_DATA_SIGNATURE);
+        ASSERT ((UINTN)DestPtr >= (UINTN)CapsuleImageBase);
+        PrivateDataPtr->CapsuleOffset[CapsuleIndex++] = (UINT64)((UINTN)DestPtr - (UINTN)CapsuleImageBase);
+      }
+
+      //
+      // Below ASSERT is checked in ValidateCapsuleIntegrity()
+      //
+      ASSERT (CurrentBlockDesc->Length <= SizeLeft);
+
+      CopyMem ((VOID *) DestPtr, (VOID *) (UINTN) (CurrentBlockDesc->Union.DataBlock), (UINTN)CurrentBlockDesc->Length);
+      DEBUG ((EFI_D_INFO, "Capsule coalesce block no.0x%lX from 0x%lX to 0x%lX with size 0x%lX\n",(UINT64)CapsuleTimes,
+             CurrentBlockDesc->Union.DataBlock, (UINT64)(UINTN)DestPtr, CurrentBlockDesc->Length));
+      DestPtr += CurrentBlockDesc->Length;
+      SizeLeft -= CurrentBlockDesc->Length;
+
+      if (SizeLeft == 0) {
+        //
+        //Here is the end of the current capsule image.
+        //
+        CapsuleBeginFlag = TRUE; 
+      }
+    } else {
+      //
+      // The first entry is the block descriptor for EFI_CAPSULE_PEIM_PRIVATE_DATA.
+      // CapsuleOffset field is uninitialized at this time. No need copy it, but need to reserve for future use.
+      //
+      ASSERT (CurrentBlockDesc->Length == sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA));
+      ASSERT ((UINTN)DestPtr == (UINTN)NewCapsuleBase);
+      CopyMem ((VOID *) DestPtr, (VOID *) (UINTN) CurrentBlockDesc->Union.DataBlock, (UINTN) CurrentBlockDesc->Length);
+      DestPtr += sizeof (EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64);
+    }
+    //
+    //Walk through the block descriptor list.
+    //
+    CurrentBlockDesc++;
+  }
+  //
+  // We return the base of memory we want reserved, and the size.
+  // The memory peim should handle it appropriately from there.
+  //
+  *MemorySize = (UINTN) CapsuleSize;
+  *MemoryBase = (VOID *) NewCapsuleBase;
+
+  ASSERT (PrivateDataPtr->Signature == EFI_CAPSULE_PEIM_PRIVATE_DATA_SIGNATURE);
+  ASSERT (PrivateDataPtr->CapsuleAllImageSize == CapsuleImageSize);
+  ASSERT (PrivateDataPtr->CapsuleNumber == CapsuleIndex);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/Common/CommonHeader.h b/Core/MdeModulePkg/Universal/CapsulePei/Common/CommonHeader.h
new file mode 100644
index 0000000000..6210d2133e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/Common/CommonHeader.h
@@ -0,0 +1,102 @@
+/** @file
+  Common header file.
+
+Copyright (c) 2011 - 2015, 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.
+
+**/
+
+#ifndef _CAPSULE_COMMON_HEADER_
+#define _CAPSULE_COMMON_HEADER_
+
+//
+// 8 extra pages for PF handler.
+//
+#define EXTRA_PAGE_TABLE_PAGES      8
+
+//
+// This capsule PEIM puts its private data at the start of the
+// coalesced capsule. Here's the structure definition.
+//
+#define EFI_CAPSULE_PEIM_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('C', 'a', 'p', 'P')
+
+#pragma pack(1)
+typedef struct {
+  UINT64  Signature;
+  UINT64  CapsuleAllImageSize;
+  UINT64  CapsuleNumber;
+  UINT64  CapsuleOffset[1];
+} EFI_CAPSULE_PEIM_PRIVATE_DATA;
+#pragma pack()
+
+#define CAPSULE_TEST_SIGNATURE SIGNATURE_32('T', 'E', 'S', 'T')
+
+#if defined (MDE_CPU_IA32) || defined (MDE_CPU_X64)
+#pragma pack(1)
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  EntryPoint;
+  EFI_PHYSICAL_ADDRESS  StackBufferBase;
+  UINT64                StackBufferLength;
+  EFI_PHYSICAL_ADDRESS  JumpBuffer;
+  EFI_PHYSICAL_ADDRESS  BlockListAddr;
+  EFI_PHYSICAL_ADDRESS  MemoryBase64Ptr;
+  EFI_PHYSICAL_ADDRESS  MemorySize64Ptr;
+  BOOLEAN               Page1GSupport;
+} SWITCH_32_TO_64_CONTEXT;
+
+typedef struct {
+  UINT16                ReturnCs;
+  EFI_PHYSICAL_ADDRESS  ReturnEntryPoint;
+  UINT64                ReturnStatus;
+  //
+  // NOTICE:
+  // Be careful about the Base field of IA32_DESCRIPTOR
+  // that is UINTN type.
+  // To extend new field for this structure, add it to
+  // right before this Gdtr field.
+  //
+  IA32_DESCRIPTOR       Gdtr;
+} SWITCH_64_TO_32_CONTEXT;
+#pragma pack()
+#endif
+
+/**
+  The function to coalesce a fragmented capsule in memory.
+
+  @param PeiServices        General purpose services available to every PEIM.
+  @param BlockListBuffer    Point to the buffer of Capsule Descriptor Variables.
+  @param MemoryBase         Pointer to the base of a block of memory that we can walk
+                            all over while trying to coalesce our buffers.
+                            On output, this variable will hold the base address of
+                            a coalesced capsule.
+  @param MemorySize         Size of the memory region pointed to by MemoryBase.
+                            On output, this variable will contain the size of the
+                            coalesced capsule.
+
+  @retval EFI_NOT_FOUND     if we can't determine the boot mode
+                            if the boot mode is not flash-update
+                            if we could not find the capsule descriptors
+
+  @retval EFI_BUFFER_TOO_SMALL
+                            if we could not coalesce the capsule in the memory
+                            region provided to us
+
+  @retval EFI_SUCCESS       if there's no capsule, or if we processed the
+                            capsule successfully.
+**/
+EFI_STATUS
+EFIAPI
+CapsuleDataCoalesce (
+  IN EFI_PEI_SERVICES                **PeiServices,
+  IN IN EFI_PHYSICAL_ADDRESS         *BlockListBuffer,
+  IN OUT VOID                        **MemoryBase,
+  IN OUT UINTN                       *MemorySize
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/UefiCapsule.c b/Core/MdeModulePkg/Universal/CapsulePei/UefiCapsule.c
new file mode 100644
index 0000000000..61fafc7fe0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/UefiCapsule.c
@@ -0,0 +1,1063 @@
+/** @file
+  Capsule update PEIM for UEFI2.0
+
+Copyright (c) 2006 - 2015, 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 "Capsule.h"
+
+#ifdef MDE_CPU_IA32
+//
+// Global Descriptor Table (GDT)
+//
+GLOBAL_REMOVE_IF_UNREFERENCED IA32_SEGMENT_DESCRIPTOR mGdtEntries[] = {
+/* selector { Global Segment Descriptor                              } */
+/* 0x00 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //null descriptor
+/* 0x08 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear data segment descriptor
+/* 0x10 */  {{0xffff, 0,  0,  0xf,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //linear code segment descriptor
+/* 0x18 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
+/* 0x20 */  {{0xffff, 0,  0,  0xb,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system code segment descriptor
+/* 0x28 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
+/* 0x30 */  {{0xffff, 0,  0,  0x3,  1,  0,  1,  0xf,  0,  0, 1,  1,  0}}, //system data segment descriptor
+/* 0x38 */  {{0xffff, 0,  0,  0xb,  1,  0,  1,  0xf,  0,  1, 0,  1,  0}}, //system code segment descriptor
+/* 0x40 */  {{0,      0,  0,  0,    0,  0,  0,  0,    0,  0, 0,  0,  0}}, //spare segment descriptor
+};
+
+//
+// IA32 Gdt register
+//
+GLOBAL_REMOVE_IF_UNREFERENCED CONST IA32_DESCRIPTOR mGdt = {
+  sizeof (mGdtEntries) - 1,
+  (UINTN) mGdtEntries
+  };
+
+/**
+  The function will check if 1G page is supported.
+
+  @retval TRUE   1G page is supported.
+  @retval FALSE  1G page is not supported.
+
+**/
+BOOLEAN
+IsPage1GSupport (
+  VOID
+  )
+{
+  UINT32                                        RegEax;
+  UINT32                                        RegEdx;
+  BOOLEAN                                       Page1GSupport;
+
+  Page1GSupport = FALSE;
+  if (PcdGetBool(PcdUse1GPageTable)) {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000001) {
+      AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+      if ((RegEdx & BIT26) != 0) {
+        Page1GSupport = TRUE;
+      }
+    }
+  }
+
+  return Page1GSupport;
+}
+
+/**
+  Calculate the total size of page table.
+
+  @param[in] Page1GSupport      1G page support or not.
+
+  @return The size of page table.
+
+**/
+UINTN
+CalculatePageTableSize (
+  IN BOOLEAN                                    Page1GSupport
+  )
+{
+  UINTN                                         ExtraPageTablePages;
+  UINTN                                         TotalPagesNum;
+  UINT8                                         PhysicalAddressBits;
+  UINT32                                        NumberOfPml4EntriesNeeded;
+  UINT32                                        NumberOfPdpEntriesNeeded;
+
+  //
+  // Create 4G page table by default,
+  // and let PF handler to handle > 4G request.
+  //
+  PhysicalAddressBits = 32;
+  ExtraPageTablePages = EXTRA_PAGE_TABLE_PAGES;
+
+  //
+  // Calculate the table entries needed.
+  //
+  if (PhysicalAddressBits <= 39 ) {
+    NumberOfPml4EntriesNeeded = 1;
+    NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 30));
+  } else {
+    NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 39));
+    NumberOfPdpEntriesNeeded = 512;
+  }
+
+  if (!Page1GSupport) {
+    TotalPagesNum = (NumberOfPdpEntriesNeeded + 1) * NumberOfPml4EntriesNeeded + 1;
+  } else {
+    TotalPagesNum = NumberOfPml4EntriesNeeded + 1;
+  }
+  TotalPagesNum += ExtraPageTablePages;
+
+  return EFI_PAGES_TO_SIZE (TotalPagesNum);
+}
+
+/**
+  Allocates and fills in the Page Directory and Page Table Entries to
+  establish a 4G page table.
+
+  @param[in] PageTablesAddress  The base address of page table.
+  @param[in] Page1GSupport      1G page support or not.
+
+**/
+VOID
+Create4GPageTables (
+  IN EFI_PHYSICAL_ADDRESS   PageTablesAddress,
+  IN BOOLEAN                Page1GSupport
+  )
+{  
+  UINT8                                         PhysicalAddressBits;
+  EFI_PHYSICAL_ADDRESS                          PageAddress;
+  UINTN                                         IndexOfPml4Entries;
+  UINTN                                         IndexOfPdpEntries;
+  UINTN                                         IndexOfPageDirectoryEntries;
+  UINT32                                        NumberOfPml4EntriesNeeded;
+  UINT32                                        NumberOfPdpEntriesNeeded;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageMapLevel4Entry;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageMap;
+  PAGE_MAP_AND_DIRECTORY_POINTER                *PageDirectoryPointerEntry;
+  PAGE_TABLE_ENTRY                              *PageDirectoryEntry;
+  UINTN                                         BigPageAddress;
+  PAGE_TABLE_1G_ENTRY                           *PageDirectory1GEntry;
+
+  //
+  // Create 4G page table by default,
+  // and let PF handler to handle > 4G request.
+  //
+  PhysicalAddressBits = 32;
+
+  //
+  // Calculate the table entries needed.
+  //
+  if (PhysicalAddressBits <= 39 ) {
+    NumberOfPml4EntriesNeeded = 1;
+    NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 30));
+  } else {
+    NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 39));
+    NumberOfPdpEntriesNeeded = 512;
+  }
+
+  //
+  // Pre-allocate big pages to avoid later allocations. 
+  //
+  BigPageAddress = (UINTN) PageTablesAddress;
+
+  //
+  // By architecture only one PageMapLevel4 exists - so lets allocate storage for it.
+  //
+  PageMap         = (VOID *) BigPageAddress;
+  BigPageAddress += SIZE_4KB;
+
+  PageMapLevel4Entry = PageMap;
+  PageAddress        = 0;
+  for (IndexOfPml4Entries = 0; IndexOfPml4Entries < NumberOfPml4EntriesNeeded; IndexOfPml4Entries++, PageMapLevel4Entry++) {
+    //
+    // Each PML4 entry points to a page of Page Directory Pointer entires.
+    // So lets allocate space for them and fill them in in the IndexOfPdpEntries loop.
+    //
+    PageDirectoryPointerEntry = (VOID *) BigPageAddress;
+    BigPageAddress += SIZE_4KB;
+
+    //
+    // Make a PML4 Entry
+    //
+    PageMapLevel4Entry->Uint64 = (UINT64)(UINTN)PageDirectoryPointerEntry;
+    PageMapLevel4Entry->Bits.ReadWrite = 1;
+    PageMapLevel4Entry->Bits.Present = 1;
+
+    if (Page1GSupport) {
+      PageDirectory1GEntry = (VOID *) PageDirectoryPointerEntry;
+    
+      for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectory1GEntry++, PageAddress += SIZE_1GB) {
+        //
+        // Fill in the Page Directory entries
+        //
+        PageDirectory1GEntry->Uint64 = (UINT64)PageAddress;
+        PageDirectory1GEntry->Bits.ReadWrite = 1;
+        PageDirectory1GEntry->Bits.Present = 1;
+        PageDirectory1GEntry->Bits.MustBe1 = 1;
+      }
+    } else {
+      for (IndexOfPdpEntries = 0; IndexOfPdpEntries < NumberOfPdpEntriesNeeded; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+        //
+        // Each Directory Pointer entries points to a page of Page Directory entires.
+        // So allocate space for them and fill them in in the IndexOfPageDirectoryEntries loop.
+        //       
+        PageDirectoryEntry = (VOID *) BigPageAddress;
+        BigPageAddress += SIZE_4KB;
+
+        //
+        // Fill in a Page Directory Pointer Entries
+        //
+        PageDirectoryPointerEntry->Uint64 = (UINT64)(UINTN)PageDirectoryEntry;
+        PageDirectoryPointerEntry->Bits.ReadWrite = 1;
+        PageDirectoryPointerEntry->Bits.Present = 1;
+
+        for (IndexOfPageDirectoryEntries = 0; IndexOfPageDirectoryEntries < 512; IndexOfPageDirectoryEntries++, PageDirectoryEntry++, PageAddress += SIZE_2MB) {
+          //
+          // Fill in the Page Directory entries
+          //
+          PageDirectoryEntry->Uint64 = (UINT64)PageAddress;
+          PageDirectoryEntry->Bits.ReadWrite = 1;
+          PageDirectoryEntry->Bits.Present = 1;
+          PageDirectoryEntry->Bits.MustBe1 = 1;
+        }
+      }
+
+      for (; IndexOfPdpEntries < 512; IndexOfPdpEntries++, PageDirectoryPointerEntry++) {
+        ZeroMem (
+          PageDirectoryPointerEntry,
+          sizeof(PAGE_MAP_AND_DIRECTORY_POINTER)
+          );
+      }
+    }
+  }
+
+  //
+  // For the PML4 entries we are not using fill in a null entry.
+  //
+  for (; IndexOfPml4Entries < 512; IndexOfPml4Entries++, PageMapLevel4Entry++) {
+    ZeroMem (
+      PageMapLevel4Entry,
+      sizeof (PAGE_MAP_AND_DIRECTORY_POINTER)
+      );
+  }
+}
+
+/**
+  Return function from long mode to 32-bit mode.
+
+  @param  EntrypointContext  Context for mode switching
+  @param  ReturnContext      Context for mode switching
+
+**/
+VOID
+ReturnFunction (
+  SWITCH_32_TO_64_CONTEXT  *EntrypointContext,
+  SWITCH_64_TO_32_CONTEXT  *ReturnContext
+  )
+{
+  //
+  // Restore original GDT
+  //
+  AsmWriteGdtr (&ReturnContext->Gdtr);
+
+  //
+  // return to original caller
+  //
+  LongJump ((BASE_LIBRARY_JUMP_BUFFER  *)(UINTN)EntrypointContext->JumpBuffer, 1);
+
+  //
+  // never be here
+  //
+  ASSERT (FALSE);
+}
+
+/**
+  Thunk function from 32-bit protection mode to long mode.
+
+  @param  PageTableAddress  Page table base address
+  @param  Context           Context for mode switching
+  @param  ReturnContext     Context for mode switching
+
+  @retval EFI_SUCCESS  Function successfully executed.
+
+**/
+EFI_STATUS
+Thunk32To64 (
+  EFI_PHYSICAL_ADDRESS          PageTableAddress,
+  SWITCH_32_TO_64_CONTEXT       *Context,
+  SWITCH_64_TO_32_CONTEXT       *ReturnContext
+  )
+{
+  UINTN                       SetJumpFlag;
+  EFI_STATUS                  Status;
+
+  //
+  // Save return address, LongJump will return here then
+  //
+  SetJumpFlag = SetJump ((BASE_LIBRARY_JUMP_BUFFER  *) (UINTN) Context->JumpBuffer);
+
+  if (SetJumpFlag == 0) {
+
+    //
+    // Build 4G Page Tables.
+    //
+    Create4GPageTables (PageTableAddress, Context->Page1GSupport);
+
+    //
+    // Create 64-bit GDT
+    //
+    AsmWriteGdtr (&mGdt);
+
+    //
+    // Write CR3
+    //
+    AsmWriteCr3 ((UINTN) PageTableAddress);
+
+    //
+    // Disable interrupt of Debug timer, since the IDT table cannot work in long mode
+    //
+    SaveAndSetDebugTimerInterrupt (FALSE);
+    //
+    // Transfer to long mode
+    //
+    AsmEnablePaging64 (
+       0x38,
+      (UINT64) Context->EntryPoint,
+      (UINT64)(UINTN) Context,
+      (UINT64)(UINTN) ReturnContext,
+      Context->StackBufferBase + Context->StackBufferLength
+      );
+  }
+
+  //
+  // Convert to 32-bit Status and return
+  //
+  Status = EFI_SUCCESS;
+  if ((UINTN) ReturnContext->ReturnStatus != 0) {
+    Status = ENCODE_ERROR ((UINTN) ReturnContext->ReturnStatus);
+  }
+  
+  return Status;
+}
+
+/**
+  If in 32 bit protection mode, and coalesce image is of X64, switch to long mode.
+
+  @param  LongModeBuffer            The context of long mode.
+  @param  CoalesceEntry             Entry of coalesce image.
+  @param  BlockListAddr             Address of block list.
+  @param  MemoryBase                Base of memory range.
+  @param  MemorySize                Size of memory range.
+
+  @retval EFI_SUCCESS               Successfully switched to long mode and execute coalesce.
+  @retval Others                    Failed to execute coalesce in long mode.
+
+**/
+EFI_STATUS
+ModeSwitch (
+  IN EFI_CAPSULE_LONG_MODE_BUFFER   *LongModeBuffer,
+  IN COALESCE_ENTRY                 CoalesceEntry,
+  IN EFI_PHYSICAL_ADDRESS           BlockListAddr,
+  IN OUT VOID                       **MemoryBase,
+  IN OUT UINTN                      *MemorySize
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_PHYSICAL_ADDRESS                 MemoryBase64;
+  UINT64                               MemorySize64;
+  EFI_PHYSICAL_ADDRESS                 MemoryEnd64;
+  SWITCH_32_TO_64_CONTEXT              Context;
+  SWITCH_64_TO_32_CONTEXT              ReturnContext;
+  BASE_LIBRARY_JUMP_BUFFER             JumpBuffer;
+  EFI_PHYSICAL_ADDRESS                 ReservedRangeBase;
+  EFI_PHYSICAL_ADDRESS                 ReservedRangeEnd;
+  BOOLEAN                              Page1GSupport;
+
+  ZeroMem (&Context, sizeof (SWITCH_32_TO_64_CONTEXT));
+  ZeroMem (&ReturnContext, sizeof (SWITCH_64_TO_32_CONTEXT));
+  
+  MemoryBase64  = (UINT64) (UINTN) *MemoryBase;
+  MemorySize64  = (UINT64) (UINTN) *MemorySize;
+  MemoryEnd64   = MemoryBase64 + MemorySize64;
+
+  Page1GSupport = IsPage1GSupport ();
+
+  //
+  // Merge memory range reserved for stack and page table  
+  //
+  if (LongModeBuffer->StackBaseAddress < LongModeBuffer->PageTableAddress) {
+    ReservedRangeBase = LongModeBuffer->StackBaseAddress;
+    ReservedRangeEnd  = LongModeBuffer->PageTableAddress + CalculatePageTableSize (Page1GSupport);
+  } else {
+    ReservedRangeBase = LongModeBuffer->PageTableAddress;
+    ReservedRangeEnd  = LongModeBuffer->StackBaseAddress + LongModeBuffer->StackSize;
+  }
+
+  //
+  // Check if memory range reserved is overlap with MemoryBase ~ MemoryBase + MemorySize.
+  // If they are overlapped, get a larger range to process capsule data.
+  //
+  if (ReservedRangeBase <= MemoryBase64) {
+    if (ReservedRangeEnd < MemoryEnd64) {
+      MemoryBase64 = ReservedRangeEnd;
+    } else {
+      DEBUG ((EFI_D_ERROR, "Memory is not enough to process capsule!\n"));
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else if (ReservedRangeBase < MemoryEnd64) {
+    if (ReservedRangeEnd < MemoryEnd64   &&
+        ReservedRangeBase - MemoryBase64 < MemoryEnd64 - ReservedRangeEnd) {
+      MemoryBase64 = ReservedRangeEnd;
+    } else {
+      MemorySize64 = (UINT64)(UINTN)(ReservedRangeBase - MemoryBase64);
+    }
+  }
+
+  //
+  // Initialize context jumping to 64-bit enviroment
+  //
+  Context.JumpBuffer            = (EFI_PHYSICAL_ADDRESS)(UINTN)&JumpBuffer;
+  Context.StackBufferBase       = LongModeBuffer->StackBaseAddress;
+  Context.StackBufferLength     = LongModeBuffer->StackSize;
+  Context.EntryPoint            = (EFI_PHYSICAL_ADDRESS)(UINTN)CoalesceEntry;
+  Context.BlockListAddr         = BlockListAddr;
+  Context.MemoryBase64Ptr       = (EFI_PHYSICAL_ADDRESS)(UINTN)&MemoryBase64;
+  Context.MemorySize64Ptr       = (EFI_PHYSICAL_ADDRESS)(UINTN)&MemorySize64;
+  Context.Page1GSupport         = Page1GSupport;
+
+  //
+  // Prepare data for return back
+  //
+  ReturnContext.ReturnCs           = 0x10;
+  ReturnContext.ReturnEntryPoint   = (EFI_PHYSICAL_ADDRESS)(UINTN)ReturnFunction;
+  //
+  // Will save the return status of processing capsule
+  //
+  ReturnContext.ReturnStatus       = 0;
+  
+  //
+  // Save original GDT
+  //
+  AsmReadGdtr ((IA32_DESCRIPTOR *)&ReturnContext.Gdtr);
+  
+  Status = Thunk32To64 (LongModeBuffer->PageTableAddress, &Context, &ReturnContext);
+  
+  if (!EFI_ERROR (Status)) {
+    *MemoryBase = (VOID *) (UINTN) MemoryBase64;
+    *MemorySize = (UINTN) MemorySize64;
+  }
+
+  return Status;
+
+}
+
+/**
+  Locates the coalesce image entry point, and detects its machine type.
+
+  @param CoalesceImageEntryPoint   Pointer to coalesce image entry point for output.
+  @param CoalesceImageMachineType  Pointer to machine type of coalesce image.
+
+  @retval EFI_SUCCESS     Coalesce image successfully located.
+  @retval Others          Failed to locate the coalesce image.
+
+**/
+EFI_STATUS
+FindCapsuleCoalesceImage (
+  OUT EFI_PHYSICAL_ADDRESS    *CoalesceImageEntryPoint,
+  OUT UINT16                  *CoalesceImageMachineType
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                Instance;
+  EFI_PEI_LOAD_FILE_PPI                *LoadFile;
+  EFI_PEI_FV_HANDLE                    VolumeHandle;
+  EFI_PEI_FILE_HANDLE                  FileHandle;
+  EFI_PHYSICAL_ADDRESS                 CoalesceImageAddress;
+  UINT64                               CoalesceImageSize;
+  UINT32                               AuthenticationState;
+
+  Instance = 0;
+
+  while (TRUE) {
+    Status = PeiServicesFfsFindNextVolume (Instance++, &VolumeHandle);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = PeiServicesFfsFindFileByName (PcdGetPtr(PcdCapsuleCoalesceFile), VolumeHandle, &FileHandle);
+    if (!EFI_ERROR (Status)) {
+      Status = PeiServicesLocatePpi (&gEfiPeiLoadFilePpiGuid, 0, NULL, (VOID **) &LoadFile);
+      ASSERT_EFI_ERROR (Status);
+
+      Status = LoadFile->LoadFile (
+                           LoadFile,
+                           FileHandle,
+                           &CoalesceImageAddress,
+                           &CoalesceImageSize,
+                           CoalesceImageEntryPoint,
+                           &AuthenticationState
+                           );
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "Unable to find PE32 section in CapsuleX64 image ffs %r!\n", Status));
+        return Status;
+      }
+      *CoalesceImageMachineType = PeCoffLoaderGetMachineType ((VOID *) (UINTN) CoalesceImageAddress);
+      break;
+    } else {
+      continue;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Gets the reserved long mode buffer.
+
+  @param  LongModeBuffer  Pointer to the long mode buffer for output.
+
+  @retval EFI_SUCCESS     Long mode buffer successfully retrieved.
+  @retval Others          Variable storing long mode buffer not found.
+
+**/
+EFI_STATUS
+GetLongModeContext (
+  OUT EFI_CAPSULE_LONG_MODE_BUFFER *LongModeBuffer
+  )
+{
+  EFI_STATUS   Status;
+  UINTN        Size;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI *PPIVariableServices;
+
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiReadOnlyVariable2PpiGuid,
+             0,
+             NULL,
+             (VOID **) &PPIVariableServices
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Size = sizeof (EFI_CAPSULE_LONG_MODE_BUFFER);
+  Status = PPIVariableServices->GetVariable (
+                                  PPIVariableServices,
+                                  EFI_CAPSULE_LONG_MODE_BUFFER_NAME,
+                                  &gEfiCapsuleVendorGuid,
+                                  NULL,
+                                  &Size,
+                                  LongModeBuffer
+                                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG (( EFI_D_ERROR, "Error Get LongModeBuffer variable %r!\n", Status));
+  }
+  return Status;
+}
+#endif
+
+/**
+  Checks for the presence of capsule descriptors.
+  Get capsule descriptors from variable CapsuleUpdateData, CapsuleUpdateData1, CapsuleUpdateData2...
+  and save to DescriptorBuffer.
+
+  @param DescriptorBuffer        Pointer to the capsule descriptors
+
+  @retval EFI_SUCCESS     a valid capsule is present
+  @retval EFI_NOT_FOUND   if a valid capsule is not present
+**/
+EFI_STATUS
+GetCapsuleDescriptors (
+  IN EFI_PHYSICAL_ADDRESS     *DescriptorBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  UINTN                            Size;
+  UINTN                            Index;
+  UINTN                            TempIndex;
+  UINTN                            ValidIndex;
+  BOOLEAN                          Flag;
+  CHAR16                           CapsuleVarName[30];
+  CHAR16                           *TempVarName;
+  EFI_PHYSICAL_ADDRESS             CapsuleDataPtr64;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI  *PPIVariableServices;
+
+  Index             = 0;
+  TempVarName       = NULL;
+  CapsuleVarName[0] = 0;
+  ValidIndex        = 0;
+  CapsuleDataPtr64  = 0;
+  
+  Status = PeiServicesLocatePpi (
+              &gEfiPeiReadOnlyVariable2PpiGuid,
+              0,
+              NULL,
+              (VOID **) &PPIVariableServices
+              );
+  if (Status == EFI_SUCCESS) {
+    StrCpyS (CapsuleVarName, sizeof(CapsuleVarName)/sizeof(CHAR16), EFI_CAPSULE_VARIABLE_NAME);
+    TempVarName = CapsuleVarName + StrLen (CapsuleVarName);
+    Size = sizeof (CapsuleDataPtr64);
+    while (1) {
+      if (Index == 0) {
+        //
+        // For the first Capsule Image
+        //
+        Status = PPIVariableServices->GetVariable (
+                                        PPIVariableServices,
+                                        CapsuleVarName,
+                                        &gEfiCapsuleVendorGuid,
+                                        NULL,
+                                        &Size,
+                                        (VOID *) &CapsuleDataPtr64
+                                        );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((EFI_D_ERROR, "Capsule -- capsule variable not set\n"));
+          return EFI_NOT_FOUND;
+        }
+        //
+        // We have a chicken/egg situation where the memory init code needs to
+        // know the boot mode prior to initializing memory. For this case, our
+        // validate function will fail. We can detect if this is the case if blocklist
+        // pointer is null. In that case, return success since we know that the
+        // variable is set.
+        //
+        if (DescriptorBuffer == NULL) {
+          return EFI_SUCCESS;
+        }
+      } else {
+        UnicodeValueToString (TempVarName, 0, Index, 0);
+        Status = PPIVariableServices->GetVariable (
+                                        PPIVariableServices,
+                                        CapsuleVarName,
+                                        &gEfiCapsuleVendorGuid,
+                                        NULL,
+                                        &Size,
+                                        (VOID *) &CapsuleDataPtr64
+                                        );
+        if (EFI_ERROR (Status)) {
+          break;
+        }
+        
+        //
+        // If this BlockList has been linked before, skip this variable
+        //
+        Flag = FALSE;
+        for (TempIndex = 0; TempIndex < ValidIndex; TempIndex++) {
+          if (DescriptorBuffer[TempIndex] == CapsuleDataPtr64)  {
+            Flag = TRUE;
+            break;
+          }
+        }
+        if (Flag) {
+          Index ++;
+          continue;
+        }
+      }
+      
+      //
+      // Cache BlockList which has been processed
+      //
+      DescriptorBuffer[ValidIndex++] = CapsuleDataPtr64;
+      Index ++;
+    }
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Capsule PPI service to coalesce a fragmented capsule in memory.
+
+  @param PeiServices  General purpose services available to every PEIM.
+  @param MemoryBase   Pointer to the base of a block of memory that we can walk
+                      all over while trying to coalesce our buffers.
+                      On output, this variable will hold the base address of
+                      a coalesced capsule.
+  @param MemorySize   Size of the memory region pointed to by MemoryBase.
+                      On output, this variable will contain the size of the
+                      coalesced capsule.
+
+  @retval EFI_NOT_FOUND   if we can't determine the boot mode
+                          if the boot mode is not flash-update
+                          if we could not find the capsule descriptors
+
+  @retval EFI_BUFFER_TOO_SMALL
+                          if we could not coalesce the capsule in the memory
+                          region provided to us
+
+  @retval EFI_SUCCESS     if there's no capsule, or if we processed the
+                          capsule successfully.
+**/
+EFI_STATUS
+EFIAPI
+CapsuleCoalesce (
+  IN     EFI_PEI_SERVICES            **PeiServices,
+  IN OUT VOID                        **MemoryBase,
+  IN OUT UINTN                       *MemorySize
+  )
+{
+  UINTN                                Index;
+  UINTN                                Size;
+  UINTN                                VariableCount;
+  CHAR16                               CapsuleVarName[30];
+  CHAR16                               *TempVarName;
+  EFI_PHYSICAL_ADDRESS                 CapsuleDataPtr64;  
+  EFI_STATUS                           Status;
+  EFI_BOOT_MODE                        BootMode;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI      *PPIVariableServices;
+  EFI_PHYSICAL_ADDRESS                 *VariableArrayAddress;
+#ifdef MDE_CPU_IA32
+  UINT16                               CoalesceImageMachineType;
+  EFI_PHYSICAL_ADDRESS                 CoalesceImageEntryPoint;
+  COALESCE_ENTRY                       CoalesceEntry;
+  EFI_CAPSULE_LONG_MODE_BUFFER         LongModeBuffer;
+#endif
+
+  Index                   = 0;
+  VariableCount           = 0;
+  CapsuleVarName[0]       = 0;
+  CapsuleDataPtr64        = 0;
+
+  //
+  // Someone should have already ascertained the boot mode. If it's not
+  // capsule update, then return normally.
+  //
+  Status = PeiServicesGetBootMode (&BootMode);
+  if (EFI_ERROR (Status) || (BootMode != BOOT_ON_FLASH_UPDATE)) {
+    DEBUG ((EFI_D_ERROR, "Boot mode is not correct for capsule update path.\n"));    
+    Status = EFI_NOT_FOUND;
+    goto Done;
+  }
+  
+  //
+  // User may set the same ScatterGatherList with several different variables,
+  // so cache all ScatterGatherList for check later.
+  //
+  Status = PeiServicesLocatePpi (
+              &gEfiPeiReadOnlyVariable2PpiGuid,
+              0,
+              NULL,
+              (VOID **) &PPIVariableServices
+              );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  Size = sizeof (CapsuleDataPtr64);
+  StrCpyS (CapsuleVarName, sizeof(CapsuleVarName)/sizeof(CHAR16), EFI_CAPSULE_VARIABLE_NAME);
+  TempVarName = CapsuleVarName + StrLen (CapsuleVarName);
+  while (TRUE) {
+    if (Index > 0) {
+      UnicodeValueToString (TempVarName, 0, Index, 0);
+    }
+    Status = PPIVariableServices->GetVariable (
+                                    PPIVariableServices,
+                                    CapsuleVarName,
+                                    &gEfiCapsuleVendorGuid,
+                                    NULL,
+                                    &Size,
+                                    (VOID *) &CapsuleDataPtr64
+                                    );
+    if (EFI_ERROR (Status)) {
+      //
+      // There is no capsule variables, quit
+      //
+      DEBUG ((EFI_D_INFO,"Capsule variable Index = %d\n", Index));
+      break;
+    }
+    VariableCount++;
+    Index++;
+  }
+  
+  DEBUG ((EFI_D_INFO,"Capsule variable count = %d\n", VariableCount));
+  
+  //
+  // The last entry is the end flag.
+  //
+  Status = PeiServicesAllocatePool (
+             (VariableCount + 1) * sizeof (EFI_PHYSICAL_ADDRESS),
+             (VOID **)&VariableArrayAddress
+             );
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "AllocatePages Failed!, Status = %x\n", Status));
+    goto Done;
+  }
+  
+  ZeroMem (VariableArrayAddress, (VariableCount + 1) * sizeof (EFI_PHYSICAL_ADDRESS));
+  
+  //
+  // Find out if we actually have a capsule.
+  // GetCapsuleDescriptors depends on variable PPI, so it should run in 32-bit environment.
+  //
+  Status = GetCapsuleDescriptors (VariableArrayAddress);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Fail to find capsule variables.\n"));
+    goto Done;
+  }
+
+#ifdef MDE_CPU_IA32
+  if (FeaturePcdGet (PcdDxeIplSwitchToLongMode)) {
+    //
+    // Switch to 64-bit mode to process capsule data when:
+    // 1. When DXE phase is 64-bit
+    // 2. When the buffer for 64-bit transition exists
+    // 3. When Capsule X64 image is built in BIOS image
+    // In 64-bit mode, we can process capsule data above 4GB.
+    //
+    CoalesceImageEntryPoint = 0;
+    Status = GetLongModeContext (&LongModeBuffer);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Fail to find the variable for long mode context!\n"));
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+    
+    Status = FindCapsuleCoalesceImage (&CoalesceImageEntryPoint, &CoalesceImageMachineType);
+    if ((EFI_ERROR (Status)) || (CoalesceImageMachineType != EFI_IMAGE_MACHINE_X64)) {
+      DEBUG ((EFI_D_ERROR, "Fail to find CapsuleX64 module in FV!\n"));
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+    ASSERT (CoalesceImageEntryPoint != 0);
+    CoalesceEntry = (COALESCE_ENTRY) (UINTN) CoalesceImageEntryPoint;
+    Status = ModeSwitch (&LongModeBuffer, CoalesceEntry, (EFI_PHYSICAL_ADDRESS)(UINTN)VariableArrayAddress, MemoryBase, MemorySize);
+  } else {
+    //
+    // Capsule is processed in IA32 mode.
+    //
+    Status = CapsuleDataCoalesce (PeiServices, (EFI_PHYSICAL_ADDRESS *)(UINTN)VariableArrayAddress, MemoryBase, MemorySize);
+  }
+#else
+  //
+  // Process capsule directly.
+  //
+  Status = CapsuleDataCoalesce (PeiServices, (EFI_PHYSICAL_ADDRESS *)(UINTN)VariableArrayAddress, MemoryBase, MemorySize);
+#endif
+  
+  DEBUG ((EFI_D_INFO, "Capsule Coalesce Status = %r!\n", Status));
+
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    DEBUG ((EFI_D_ERROR, "There is not enough memory to process capsule!\n"));
+  }
+  
+  if (Status == EFI_NOT_FOUND) {
+    DEBUG ((EFI_D_ERROR, "Fail to parse capsule descriptor in memory!\n"));
+    REPORT_STATUS_CODE (
+      EFI_ERROR_CODE | EFI_ERROR_MAJOR,
+      (EFI_SOFTWARE_PEI_MODULE | EFI_SW_PEI_EC_INVALID_CAPSULE_DESCRIPTOR)
+      );
+  }
+
+Done:
+  return Status;
+}
+
+/**
+  Determine if we're in capsule update boot mode.
+
+  @param PeiServices  PEI services table
+
+  @retval EFI_SUCCESS   if we have a capsule available
+  @retval EFI_NOT_FOUND no capsule detected
+
+**/
+EFI_STATUS
+EFIAPI
+CheckCapsuleUpdate (
+  IN EFI_PEI_SERVICES           **PeiServices
+  )
+{
+  EFI_STATUS  Status;
+  Status = GetCapsuleDescriptors (NULL);
+  return Status;
+}
+/**
+  This function will look at a capsule and determine if it's a test pattern. 
+  If it is, then it will verify it and emit an error message if corruption is detected.
+  
+  @param PeiServices   Standard pei services pointer
+  @param CapsuleBase   Base address of coalesced capsule, which is preceeded
+                       by private data. Very implementation specific.
+
+  @retval TRUE    Capsule image is the test image
+  @retval FALSE   Capsule image is not the test image.
+
+**/
+BOOLEAN
+CapsuleTestPattern (
+  IN EFI_PEI_SERVICES                 **PeiServices,
+  IN VOID                             *CapsuleBase
+  )
+{
+  UINT32  *TestPtr;
+  UINT32  TestCounter;
+  UINT32  TestSize;
+  BOOLEAN RetValue;
+
+  RetValue = FALSE;
+
+  //
+  // Look at the capsule data and determine if it's a test pattern. If it
+  // is, then test it now.
+  //
+  TestPtr = (UINT32 *) CapsuleBase;
+  //
+  // 0x54534554 "TEST"
+  //
+  if (*TestPtr == 0x54534554) {
+    RetValue = TRUE;
+    DEBUG ((EFI_D_INFO, "Capsule test pattern mode activated...\n"));
+    TestSize = TestPtr[1] / sizeof (UINT32);
+    //
+    // Skip over the signature and the size fields in the pattern data header
+    //
+    TestPtr += 2;
+    TestCounter = 0;
+    while (TestSize > 0) {
+      if (*TestPtr != TestCounter) {
+        DEBUG ((EFI_D_INFO, "Capsule test pattern mode FAILED: BaseAddr/FailAddr 0x%X 0x%X\n", (UINT32)(UINTN)(EFI_CAPSULE_PEIM_PRIVATE_DATA *)CapsuleBase, (UINT32)(UINTN)TestPtr));
+        return TRUE;
+      }
+
+      TestPtr++;
+      TestCounter++;
+      TestSize--;
+    }
+
+    DEBUG ((EFI_D_INFO, "Capsule test pattern mode SUCCESS\n"));
+  }
+
+  return RetValue;
+}
+
+/**
+  Capsule PPI service that gets called after memory is available. The
+  capsule coalesce function, which must be called first, returns a base
+  address and size, which can be anything actually. Once the memory init
+  PEIM has discovered memory, then it should call this function and pass in
+  the base address and size returned by the coalesce function. Then this
+  function can create a capsule HOB and return.
+
+  @param PeiServices   standard pei services pointer
+  @param CapsuleBase   address returned by the capsule coalesce function. Most
+                       likely this will actually be a pointer to private data.
+  @param CapsuleSize   value returned by the capsule coalesce function.
+
+  @retval EFI_VOLUME_CORRUPTED  CapsuleBase does not appear to point to a
+                                coalesced capsule
+  @retval EFI_SUCCESS           if all goes well.
+**/
+EFI_STATUS
+EFIAPI
+CreateState (
+  IN EFI_PEI_SERVICES                 **PeiServices,
+  IN VOID                             *CapsuleBase,
+  IN UINTN                            CapsuleSize
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_CAPSULE_PEIM_PRIVATE_DATA *PrivateData;
+  UINTN                         Size;
+  EFI_PHYSICAL_ADDRESS          NewBuffer;
+  UINTN                         CapsuleNumber;
+  UINT32                        Index;
+  EFI_PHYSICAL_ADDRESS          BaseAddress;
+  UINT64                        Length;
+ 
+  PrivateData    = (EFI_CAPSULE_PEIM_PRIVATE_DATA *) CapsuleBase;
+  if (PrivateData->Signature != EFI_CAPSULE_PEIM_PRIVATE_DATA_SIGNATURE) {
+    return EFI_VOLUME_CORRUPTED;
+  }
+  if (PrivateData->CapsuleAllImageSize >= MAX_ADDRESS) {
+    DEBUG ((EFI_D_ERROR, "CapsuleAllImageSize too big - 0x%lx\n", PrivateData->CapsuleAllImageSize));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  if (PrivateData->CapsuleNumber >= MAX_ADDRESS) {
+    DEBUG ((EFI_D_ERROR, "CapsuleNumber too big - 0x%lx\n", PrivateData->CapsuleNumber));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Capsule Number and Capsule Offset is in the tail of Capsule data.
+  //
+  Size          = (UINTN)PrivateData->CapsuleAllImageSize;
+  CapsuleNumber = (UINTN)PrivateData->CapsuleNumber;
+  //
+  // Allocate the memory so that it gets preserved into DXE
+  //
+  Status = PeiServicesAllocatePages (
+             EfiRuntimeServicesData,
+             EFI_SIZE_TO_PAGES (Size),
+             &NewBuffer
+             );
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "AllocatePages Failed!\n"));
+    return Status;
+  }
+  //
+  // Copy to our new buffer for DXE
+  //
+  DEBUG ((EFI_D_INFO, "Capsule copy from 0x%8X to 0x%8X with size 0x%8X\n", (UINTN)((UINT8 *)PrivateData + sizeof(EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64)), (UINTN) NewBuffer, Size));
+  CopyMem ((VOID *) (UINTN) NewBuffer, (VOID *) (UINTN) ((UINT8 *)PrivateData + sizeof(EFI_CAPSULE_PEIM_PRIVATE_DATA) + (CapsuleNumber - 1) * sizeof(UINT64)), Size);
+  //
+  // Check for test data pattern. If it is the test pattern, then we'll
+  // test it ans still create the HOB so that it can be used to verify
+  // that capsules don't get corrupted all the way into BDS. BDS will
+  // still try to turn it into a firmware volume, but will think it's
+  // corrupted so nothing will happen.
+  //
+  DEBUG_CODE (
+    CapsuleTestPattern (PeiServices, (VOID *) (UINTN) NewBuffer);
+  );
+
+  //
+  // Build the UEFI Capsule Hob for each capsule image.
+  //
+  for (Index = 0; Index < CapsuleNumber; Index ++) {
+    BaseAddress = NewBuffer + PrivateData->CapsuleOffset[Index];
+    Length      = ((EFI_CAPSULE_HEADER *)((UINTN) BaseAddress))->CapsuleImageSize;
+
+    BuildCvHob (BaseAddress, Length);
+  }
+  
+  return EFI_SUCCESS;
+}
+
+CONST EFI_PEI_CAPSULE_PPI        mCapsulePpi = {
+  CapsuleCoalesce,
+  CheckCapsuleUpdate,
+  CreateState
+};
+
+CONST EFI_PEI_PPI_DESCRIPTOR mUefiPpiListCapsule = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiCapsulePpiGuid,
+  (EFI_PEI_CAPSULE_PPI *) &mCapsulePpi
+};
+
+/**
+  Entry point function for the PEIM
+
+  @param FileHandle      Handle of the file being invoked.
+  @param PeiServices     Describes the list of possible PEI Services.
+
+  @return EFI_SUCCESS    If we installed our PPI
+
+**/
+EFI_STATUS
+EFIAPI
+CapsuleMain (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  //
+  // Just produce our PPI
+  //
+  return PeiServicesInstallPpi (&mUefiPpiListCapsule);
+}
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.S b/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.S
new file mode 100644
index 0000000000..9e17cc39ee
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.S
@@ -0,0 +1,81 @@
+## @file
+#   This is the assembly code for page fault handler hook.
+#
+# Copyright (c) 2015, 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.
+#
+##
+
+ASM_GLOBAL ASM_PFX(PageFaultHandlerHook)
+ASM_PFX(PageFaultHandlerHook):
+    addq     $-0x10, %rsp
+    # save rax
+    movq     %rax, 0x08(%rsp)
+
+    # pushq    %rax                         # save all volatile registers
+    pushq    %rcx
+    pushq    %rdx
+    pushq    %r8
+    pushq    %r9
+    pushq    %r10
+    pushq    %r11
+    # save volatile fp registers
+    # 68h + 08h(for alignment)
+    addq     $-0x70, %rsp
+    stmxcsr  0x60(%rsp)
+    movdqa   %xmm0, 0x0(%rsp) 
+    movdqa   %xmm1, 0x10(%rsp) 
+    movdqa   %xmm2, 0x20(%rsp) 
+    movdqa   %xmm3, 0x30(%rsp) 
+    movdqa   %xmm4, 0x40(%rsp) 
+    movdqa   %xmm5, 0x50(%rsp) 
+
+    addq     $-0x20, %rsp
+    call     ASM_PFX(PageFaultHandler)
+    addq     $0x20, %rsp
+
+    # load volatile fp registers
+    ldmxcsr  0x60(%rsp)
+    movdqa   0x0(%rsp), %xmm0
+    movdqa   0x10(%rsp), %xmm1
+    movdqa   0x20(%rsp), %xmm2
+    movdqa   0x30(%rsp), %xmm3
+    movdqa   0x40(%rsp), %xmm4
+    movdqa   0x50(%rsp), %xmm5
+    addq     $0x70, %rsp
+
+    popq     %r11
+    popq     %r10
+    popq     %r9
+    popq     %r8
+    popq     %rdx
+    popq     %rcx
+    # popq     %rax                         # restore all volatile registers
+
+    addq     $0x10, %rsp
+
+    # rax returned from PageFaultHandler is NULL or OriginalHandler address
+    # NULL if the page fault is handled by PageFaultHandler
+    # OriginalHandler address if the page fault is not handled by PageFaultHandler
+    testq    %rax, %rax
+
+    # save OriginalHandler address
+    movq     %rax, -0x10(%rsp)
+    # restore rax
+    movq     -0x08(%rsp), %rax
+
+    jz       L1
+
+    # jump to OriginalHandler
+    jmpq     *-0x10(%rsp)
+
+L1:
+    addq     $0x08, %rsp                  # skip error code for PF
+    iretq
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.asm b/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.asm
new file mode 100644
index 0000000000..2f1eab72ef
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/X64/PageFaultHandler.asm
@@ -0,0 +1,87 @@
+;; @file
+;   This is the assembly code for page fault handler hook.
+;
+; Copyright (c) 2015, 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.
+;
+;;
+
+EXTERN PageFaultHandler:PROC
+
+    .code
+
+PageFaultHandlerHook PROC
+    add     rsp, -10h
+    ; save rax
+    mov     [rsp + 08h], rax
+
+    ;push    rax                         ; save all volatile registers
+    push    rcx
+    push    rdx
+    push    r8
+    push    r9
+    push    r10
+    push    r11
+    ; save volatile fp registers
+    ; 68h + 08h(for alignment)
+    add     rsp, -70h
+    stmxcsr [rsp + 60h]
+    movdqa  [rsp + 0h], xmm0
+    movdqa  [rsp + 10h], xmm1
+    movdqa  [rsp + 20h], xmm2
+    movdqa  [rsp + 30h], xmm3
+    movdqa  [rsp + 40h], xmm4
+    movdqa  [rsp + 50h], xmm5
+
+    add     rsp, -20h
+    call    PageFaultHandler
+    add     rsp, 20h
+
+    ; load volatile fp registers
+    ldmxcsr [rsp + 60h]
+    movdqa  xmm0,  [rsp + 0h]
+    movdqa  xmm1,  [rsp + 10h]
+    movdqa  xmm2,  [rsp + 20h]
+    movdqa  xmm3,  [rsp + 30h]
+    movdqa  xmm4,  [rsp + 40h]
+    movdqa  xmm5,  [rsp + 50h]
+    add     rsp, 70h
+
+    pop     r11
+    pop     r10
+    pop     r9
+    pop     r8
+    pop     rdx
+    pop     rcx
+    ;pop     rax                         ; restore all volatile registers
+
+    add     rsp, 10h
+
+    ; rax returned from PageFaultHandler is NULL or OriginalHandler address
+    ; NULL if the page fault is handled by PageFaultHandler
+    ; OriginalHandler address if the page fault is not handled by PageFaultHandler
+    test    rax, rax
+
+    ; save OriginalHandler address
+    mov     [rsp - 10h], rax
+    ; restore rax
+    mov     rax, [rsp - 08h]
+
+    jz      @F
+
+    ; jump to OriginalHandler
+    jmp     qword ptr [rsp - 10h]
+
+@@:
+    add     rsp, 08h                    ; skip error code for PF
+    iretq
+PageFaultHandlerHook ENDP
+
+    END
diff --git a/Core/MdeModulePkg/Universal/CapsulePei/X64/X64Entry.c b/Core/MdeModulePkg/Universal/CapsulePei/X64/X64Entry.c
new file mode 100644
index 0000000000..670e2c7d5f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsulePei/X64/X64Entry.c
@@ -0,0 +1,292 @@
+/** @file
+  The X64 entrypoint is used to process capsule in long mode.
+
+Copyright (c) 2011 - 2015, 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 <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/CpuExceptionHandlerLib.h>
+#include <Library/DebugAgentLib.h>
+#include "CommonHeader.h"
+
+#define EXCEPTION_VECTOR_NUMBER     0x22
+
+#define IA32_PG_P                   BIT0
+#define IA32_PG_RW                  BIT1
+#define IA32_PG_PS                  BIT7
+
+typedef struct _PAGE_FAULT_CONTEXT {
+  BOOLEAN                       Page1GSupport;
+  UINT64                        PhyMask;
+  UINTN                         PageFaultBuffer;
+  UINTN                         PageFaultIndex;
+  //
+  // Store the uplink information for each page being used.
+  //
+  UINT64                        *PageFaultUplink[EXTRA_PAGE_TABLE_PAGES];
+  VOID                          *OriginalHandler;
+} PAGE_FAULT_CONTEXT;
+
+typedef struct _PAGE_FAULT_IDT_TABLE {
+  PAGE_FAULT_CONTEXT            PageFaultContext;
+  IA32_IDT_GATE_DESCRIPTOR      IdtEntryTable[EXCEPTION_VECTOR_NUMBER];
+} PAGE_FAULT_IDT_TABLE;
+
+/**
+  Page fault handler.
+
+**/
+VOID
+EFIAPI
+PageFaultHandlerHook (
+  VOID
+  );
+
+/**
+  Hook IDT with our page fault handler so that the on-demand paging works on page fault.
+
+  @param[in, out] IdtEntry          Pointer to IDT entry.
+  @param[in, out] PageFaultContext  Pointer to page fault context.
+
+**/
+VOID
+HookPageFaultHandler (
+  IN OUT IA32_IDT_GATE_DESCRIPTOR   *IdtEntry,
+  IN OUT PAGE_FAULT_CONTEXT         *PageFaultContext
+  )
+{
+  UINT32            RegEax;
+  UINT8             PhysicalAddressBits;
+  UINTN             PageFaultHandlerHookAddress;
+
+  AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+  if (RegEax >= 0x80000008) {
+    AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);
+    PhysicalAddressBits = (UINT8) RegEax;
+  } else {
+    PhysicalAddressBits = 36;
+  }
+  PageFaultContext->PhyMask = LShiftU64 (1, PhysicalAddressBits) - 1;
+  PageFaultContext->PhyMask &= (1ull << 48) - SIZE_4KB;
+
+  //
+  // Set Page Fault entry to catch >4G access
+  //
+  PageFaultHandlerHookAddress       = (UINTN)PageFaultHandlerHook;
+  PageFaultContext->OriginalHandler = (VOID *)(UINTN)(LShiftU64 (IdtEntry->Bits.OffsetUpper, 32) + IdtEntry->Bits.OffsetLow + (IdtEntry->Bits.OffsetHigh << 16));
+  IdtEntry->Bits.OffsetLow          = (UINT16)PageFaultHandlerHookAddress;
+  IdtEntry->Bits.Selector           = (UINT16)AsmReadCs ();
+  IdtEntry->Bits.Reserved_0         = 0;
+  IdtEntry->Bits.GateType           = IA32_IDT_GATE_TYPE_INTERRUPT_32;
+  IdtEntry->Bits.OffsetHigh         = (UINT16)(PageFaultHandlerHookAddress >> 16);
+  IdtEntry->Bits.OffsetUpper        = (UINT32)(PageFaultHandlerHookAddress >> 32);
+  IdtEntry->Bits.Reserved_1         = 0;
+
+  if (PageFaultContext->Page1GSupport) {
+    PageFaultContext->PageFaultBuffer = (UINTN)(AsmReadCr3 () & PageFaultContext->PhyMask) + EFI_PAGES_TO_SIZE(2);
+  }else {
+    PageFaultContext->PageFaultBuffer = (UINTN)(AsmReadCr3 () & PageFaultContext->PhyMask) + EFI_PAGES_TO_SIZE(6);
+  }
+  PageFaultContext->PageFaultIndex = 0;
+  ZeroMem (PageFaultContext->PageFaultUplink, sizeof (PageFaultContext->PageFaultUplink));
+}
+
+/**
+  Acquire page for page fault.
+
+  @param[in, out] PageFaultContext  Pointer to page fault context.
+  @param[in, out] Uplink            Pointer to up page table entry.
+
+**/
+VOID
+AcquirePage (
+  IN OUT PAGE_FAULT_CONTEXT     *PageFaultContext,
+  IN OUT UINT64                 *Uplink
+  )
+{
+  UINTN             Address;
+
+  Address = PageFaultContext->PageFaultBuffer + EFI_PAGES_TO_SIZE (PageFaultContext->PageFaultIndex);
+  ZeroMem ((VOID *) Address, EFI_PAGES_TO_SIZE (1));
+
+  //
+  // Cut the previous uplink if it exists and wasn't overwritten.
+  //
+  if ((PageFaultContext->PageFaultUplink[PageFaultContext->PageFaultIndex] != NULL) && ((*PageFaultContext->PageFaultUplink[PageFaultContext->PageFaultIndex] & PageFaultContext->PhyMask) == Address)) {
+    *PageFaultContext->PageFaultUplink[PageFaultContext->PageFaultIndex] = 0;
+  }
+
+  //
+  // Link & Record the current uplink.
+  //
+  *Uplink = Address | IA32_PG_P | IA32_PG_RW;
+  PageFaultContext->PageFaultUplink[PageFaultContext->PageFaultIndex] = Uplink;
+
+  PageFaultContext->PageFaultIndex = (PageFaultContext->PageFaultIndex + 1) % EXTRA_PAGE_TABLE_PAGES;
+}
+
+/**
+  The page fault handler that on-demand read >4G memory/MMIO.
+
+  @retval NULL              The page fault is correctly handled.
+  @retval OriginalHandler   The page fault is not handled and is passed through to original handler.
+
+**/
+VOID *
+EFIAPI
+PageFaultHandler (
+  VOID
+  )
+{
+  IA32_DESCRIPTOR           Idtr;
+  PAGE_FAULT_CONTEXT        *PageFaultContext;
+  UINT64                    PhyMask;
+  UINT64                    *PageTable;
+  UINT64                    PFAddress;
+  UINTN                     PTIndex;
+
+  //
+  // Get the IDT Descriptor.
+  //
+  AsmReadIdtr ((IA32_DESCRIPTOR *) &Idtr); 
+  //
+  // Then get page fault context by IDT Descriptor.
+  //
+  PageFaultContext = (PAGE_FAULT_CONTEXT *) (UINTN) (Idtr.Base - sizeof (PAGE_FAULT_CONTEXT));
+  PhyMask = PageFaultContext->PhyMask;
+
+  PFAddress = AsmReadCr2 ();
+  DEBUG ((EFI_D_ERROR, "CapsuleX64 - PageFaultHandler: Cr2 - %lx\n", PFAddress));
+
+  if (PFAddress >= PhyMask + SIZE_4KB) {
+    return PageFaultContext->OriginalHandler;
+  }
+  PFAddress &= PhyMask;
+
+  PageTable = (UINT64*)(UINTN)(AsmReadCr3 () & PhyMask);
+
+  PTIndex = BitFieldRead64 (PFAddress, 39, 47);
+  // PML4E
+  if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+    AcquirePage (PageFaultContext, &PageTable[PTIndex]);
+  }
+  PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & PhyMask);
+  PTIndex = BitFieldRead64 (PFAddress, 30, 38);
+  // PDPTE
+  if (PageFaultContext->Page1GSupport) {
+    PageTable[PTIndex] = (PFAddress & ~((1ull << 30) - 1)) | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  } else {
+    if ((PageTable[PTIndex] & IA32_PG_P) == 0) {
+      AcquirePage (PageFaultContext, &PageTable[PTIndex]);
+    }
+    PageTable = (UINT64*)(UINTN)(PageTable[PTIndex] & PhyMask);
+    PTIndex = BitFieldRead64 (PFAddress, 21, 29);
+    // PD
+    PageTable[PTIndex] = (PFAddress & ~((1ull << 21) - 1)) | IA32_PG_P | IA32_PG_RW | IA32_PG_PS;
+  }
+
+  return NULL;
+}
+
+
+/**
+  The X64 entrypoint is used to process capsule in long mode then
+  return to 32-bit protected mode.
+
+  @param  EntrypointContext   Pointer to the context of long mode.
+  @param  ReturnContext       Pointer to the context of 32-bit protected mode.
+
+  @retval This function should never return actually.
+
+**/
+EFI_STATUS
+EFIAPI
+_ModuleEntryPoint (
+  SWITCH_32_TO_64_CONTEXT       *EntrypointContext,
+  SWITCH_64_TO_32_CONTEXT       *ReturnContext
+)
+{
+  EFI_STATUS                    Status;
+  IA32_DESCRIPTOR               Ia32Idtr;
+  IA32_DESCRIPTOR               X64Idtr;
+  PAGE_FAULT_IDT_TABLE          PageFaultIdtTable;
+  IA32_IDT_GATE_DESCRIPTOR      *IdtEntry;
+
+  //
+  // Save the IA32 IDT Descriptor
+  //
+  AsmReadIdtr ((IA32_DESCRIPTOR *) &Ia32Idtr); 
+
+  //
+  // Setup X64 IDT table
+  //
+  ZeroMem (PageFaultIdtTable.IdtEntryTable, sizeof (IA32_IDT_GATE_DESCRIPTOR) * EXCEPTION_VECTOR_NUMBER);
+  X64Idtr.Base = (UINTN) PageFaultIdtTable.IdtEntryTable;
+  X64Idtr.Limit = (UINT16) (sizeof (IA32_IDT_GATE_DESCRIPTOR) * EXCEPTION_VECTOR_NUMBER - 1);
+  AsmWriteIdtr ((IA32_DESCRIPTOR *) &X64Idtr);  
+
+  //
+  // Setup the default CPU exception handlers
+  //
+  Status = InitializeCpuExceptionHandlers (NULL);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Hook page fault handler to handle >4G request.
+  //
+  PageFaultIdtTable.PageFaultContext.Page1GSupport = EntrypointContext->Page1GSupport;
+  IdtEntry = (IA32_IDT_GATE_DESCRIPTOR *) (X64Idtr.Base + (14 * sizeof (IA32_IDT_GATE_DESCRIPTOR)));
+  HookPageFaultHandler (IdtEntry, &(PageFaultIdtTable.PageFaultContext));
+
+  //
+  // Initialize Debug Agent to support source level debug
+  //
+  InitializeDebugAgent (DEBUG_AGENT_INIT_THUNK_PEI_IA32TOX64, (VOID *) &Ia32Idtr, NULL);
+
+  //
+  // Call CapsuleDataCoalesce to process capsule.
+  //
+  Status = CapsuleDataCoalesce (
+             NULL,
+             (EFI_PHYSICAL_ADDRESS *) (UINTN) EntrypointContext->BlockListAddr,
+             (VOID **) (UINTN) EntrypointContext->MemoryBase64Ptr,
+             (UINTN *) (UINTN) EntrypointContext->MemorySize64Ptr
+             );
+  
+  ReturnContext->ReturnStatus = Status;
+
+  //
+  // Disable interrupt of Debug timer, since the new IDT table cannot work in long mode
+  //
+  SaveAndSetDebugTimerInterrupt (FALSE);
+  //
+  // Restore IA32 IDT table
+  //
+  AsmWriteIdtr ((IA32_DESCRIPTOR *) &Ia32Idtr);  
+  
+  //
+  // Finish to coalesce capsule, and return to 32-bit mode.
+  //
+  AsmDisablePaging64 (
+    ReturnContext->ReturnCs,
+    (UINT32) ReturnContext->ReturnEntryPoint,
+    (UINT32) (UINTN) EntrypointContext,
+    (UINT32) (UINTN) ReturnContext,
+    (UINT32) (EntrypointContext->StackBufferBase + EntrypointContext->StackBufferLength)
+    );  
+  
+  //
+  // Should never be here.
+  //
+  ASSERT (FALSE);
+  return EFI_SUCCESS;
+}
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.inf b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.inf
new file mode 100644
index 0000000000..7a7a3e3706
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.inf
@@ -0,0 +1,97 @@
+## @file
+#  Capsule Runtime Driver produces two UEFI capsule runtime services: (UpdateCapsule, QueryCapsuleCapabilities).
+#
+#  It installs the Capsule Architectural Protocol defined in PI1.0a to signify 
+#  the capsule runtime services are ready.
+#  
+#  Copyright (c) 2006 - 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CapsuleRuntimeDxe
+  MODULE_UNI_FILE                = CapsuleRuntimeDxe.uni
+  FILE_GUID                      = 42857F0A-13F2-4B21-8A23-53D3F714B840
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = CapsuleServiceInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  CapsuleService.c
+
+[Sources.Ia32, Sources.IPF, Sources.EBC, Sources.ARM, Sources.AARCH64]
+  SaveLongModeContext.c
+
+[Sources.X64]
+  X64/SaveLongModeContext.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  PcdLib
+  DebugLib
+  UefiRuntimeServicesTableLib
+  UefiDriverEntryPoint
+  CapsuleLib
+  UefiRuntimeLib
+  BaseLib
+  PrintLib
+  BaseMemoryLib
+
+[LibraryClasses.X64]
+  UefiLib
+  BaseMemoryLib
+
+[Guids]
+  ## SOMETIMES_PRODUCES   ## Variable:L"CapsuleUpdateData" # (Process across reset capsule image) for capsule updated data
+  ## SOMETIMES_PRODUCES   ## Variable:L"CapsuleLongModeBuffer" # The long mode buffer used by IA32 Capsule PEIM to call X64 CapsuleCoalesce code to handle >4GB capsule blocks
+  gEfiCapsuleVendorGuid
+  gEfiFmpCapsuleGuid                            ## SOMETIMES_CONSUMES   ## GUID # FMP capsule GUID
+
+[Protocols]
+  gEfiCapsuleArchProtocolGuid                   ## PRODUCES
+
+[Protocols.X64]
+  ## UNDEFINED ## NOTIFY
+  ## SOMETIMES_CONSUMES
+  gEdkiiVariableLockProtocolGuid
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSupportUpdateCapsuleReset   ## CONSUMES
+
+[FeaturePcd.X64]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDxeIplSwitchToLongMode      ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizeNonPopulateCapsule   ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxSizePopulateCapsule || gEfiMdeModulePkgTokenSpaceGuid.PcdSupportUpdateCapsuleReset ## SOMETIMES_CONSUMES # Populate Image requires reset support.
+
+[Pcd.X64]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdCapsulePeiLongModeStackSize   ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUse1GPageTable                ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiVariableWriteArchProtocolGuid             # Depends on variable write functionality to produce capsule data variable
+
+# [Hob.X64]
+# UNDEFINED                 ## SOMETIMES_CONSUMES # CPU
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  CapsuleRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.uni b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.uni
new file mode 100644
index 0000000000..8bd8739994
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..a9d83883eb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleService.c b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleService.c
new file mode 100644
index 0000000000..ed8820a8f7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/CapsuleService.c
@@ -0,0 +1,408 @@
+/** @file
+  Capsule Runtime Driver produces two UEFI capsule runtime services.
+  (UpdateCapsule, QueryCapsuleCapabilities)
+  It installs the Capsule Architectural Protocol defined in PI1.0a to signify 
+  the capsule runtime services are ready.
+
+Copyright (c) 2006 - 2015, 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 <Uefi.h>
+
+#include <Protocol/Capsule.h>
+#include <Guid/CapsuleVendor.h>
+#include <Guid/FmpCapsule.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/CapsuleLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PrintLib.h>
+#include <Library/BaseMemoryLib.h>
+//
+// Handle for the installation of Capsule Architecture Protocol.
+//
+EFI_HANDLE  mNewHandle = NULL;
+
+//
+// The times of calling UpdateCapsule ()
+//
+UINTN       mTimes      = 0;
+
+UINT32      mMaxSizePopulateCapsule     = 0;
+UINT32      mMaxSizeNonPopulateCapsule  = 0;
+
+/**
+  Create the variable to save the base address of page table and stack
+  for transferring into long mode in IA32 PEI.
+**/
+VOID
+SaveLongModeContext (
+  VOID
+  );
+
+/**
+  Passes capsules to the firmware with both virtual and physical mapping. Depending on the intended
+  consumption, the firmware may process the capsule immediately. If the payload should persist
+  across a system reset, the reset value returned from EFI_QueryCapsuleCapabilities must
+  be passed into ResetSystem() and will cause the capsule to be processed by the firmware as
+  part of the reset process.
+
+  @param  CapsuleHeaderArray    Virtual pointer to an array of virtual pointers to the capsules
+                                being passed into update capsule.
+  @param  CapsuleCount          Number of pointers to EFI_CAPSULE_HEADER in
+                                CaspuleHeaderArray.
+  @param  ScatterGatherList     Physical pointer to a set of
+                                EFI_CAPSULE_BLOCK_DESCRIPTOR that describes the
+                                location in physical memory of a set of capsules.
+
+  @retval EFI_SUCCESS           Valid capsule was passed. If
+                                CAPSULE_FLAGS_PERSIT_ACROSS_RESET is not set, the
+                                capsule has been successfully processed by the firmware.
+  @retval EFI_DEVICE_ERROR      The capsule update was started, but failed due to a device error.
+  @retval EFI_INVALID_PARAMETER CapsuleSize is NULL, or an incompatible set of flags were
+                                set in the capsule header.
+  @retval EFI_INVALID_PARAMETER CapsuleCount is Zero.
+  @retval EFI_INVALID_PARAMETER For across reset capsule image, ScatterGatherList is NULL.
+  @retval EFI_UNSUPPORTED       CapsuleImage is not recognized by the firmware.
+  @retval EFI_OUT_OF_RESOURCES  When ExitBootServices() has been previously called this error indicates the capsule 
+                                is compatible with this platform but is not capable of being submitted or processed 
+                                in runtime. The caller may resubmit the capsule prior to ExitBootServices().
+  @retval EFI_OUT_OF_RESOURCES  When ExitBootServices() has not been previously called then this error indicates 
+                                the capsule is compatible with this platform but there are insufficient resources to process.
+
+**/
+EFI_STATUS
+EFIAPI
+UpdateCapsule (
+  IN EFI_CAPSULE_HEADER      **CapsuleHeaderArray,
+  IN UINTN                   CapsuleCount,
+  IN EFI_PHYSICAL_ADDRESS    ScatterGatherList OPTIONAL
+  )
+{
+  UINTN                     ArrayNumber;
+  EFI_STATUS                Status;
+  EFI_CAPSULE_HEADER        *CapsuleHeader;
+  BOOLEAN                   NeedReset;
+  BOOLEAN                   InitiateReset;
+  CHAR16                    CapsuleVarName[30];
+  CHAR16                    *TempVarName;  
+  
+  //
+  // Capsule Count can't be less than one.
+  //
+  if (CapsuleCount < 1) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NeedReset         = FALSE;
+  InitiateReset     = FALSE;
+  CapsuleHeader     = NULL;
+  CapsuleVarName[0] = 0;
+
+  for (ArrayNumber = 0; ArrayNumber < CapsuleCount; ArrayNumber++) {
+    //
+    // A capsule which has the CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE flag must have
+    // CAPSULE_FLAGS_PERSIST_ACROSS_RESET set in its header as well.
+    //
+    CapsuleHeader = CapsuleHeaderArray[ArrayNumber];
+    if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE)) == CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // A capsule which has the CAPSULE_FLAGS_INITIATE_RESET flag must have
+    // CAPSULE_FLAGS_PERSIST_ACROSS_RESET set in its header as well.
+    //
+    if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_INITIATE_RESET)) == CAPSULE_FLAGS_INITIATE_RESET) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Check FMP capsule flag 
+    //
+    if (CompareGuid(&CapsuleHeader->CapsuleGuid, &gEfiFmpCapsuleGuid)
+     && (CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) != 0 ) {
+       return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Check Capsule image without populate flag by firmware support capsule function  
+    //
+    if ((CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) == 0) {
+      Status = SupportCapsuleImage (CapsuleHeader);
+      if (EFI_ERROR(Status)) {
+        return Status;
+      }
+    }
+  }
+
+  //
+  // Walk through all capsules, record whether there is a capsule needs reset
+  // or initiate reset. And then process capsules which has no reset flag directly.
+  //
+  for (ArrayNumber = 0; ArrayNumber < CapsuleCount ; ArrayNumber++) {
+    CapsuleHeader = CapsuleHeaderArray[ArrayNumber];
+    //
+    // Here should be in the boot-time for non-reset capsule image
+    // Platform specific update for the non-reset capsule image.
+    //
+    if ((CapsuleHeader->Flags & CAPSULE_FLAGS_PERSIST_ACROSS_RESET) == 0) {
+      if (EfiAtRuntime ()) { 
+        Status = EFI_OUT_OF_RESOURCES;
+      } else {
+        Status = ProcessCapsuleImage(CapsuleHeader);
+      }
+      if (EFI_ERROR(Status)) {
+        return Status;
+      }
+    } else {
+      NeedReset = TRUE;
+      if ((CapsuleHeader->Flags & CAPSULE_FLAGS_INITIATE_RESET) != 0) {
+        InitiateReset = TRUE;
+      }
+    }
+  }
+  
+  //
+  // After launching all capsules who has no reset flag, if no more capsules claims
+  // for a system reset just return.
+  //
+  if (!NeedReset) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // ScatterGatherList is only referenced if the capsules are defined to persist across
+  // system reset. 
+  //
+  if (ScatterGatherList == (EFI_PHYSICAL_ADDRESS) (UINTN) NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check if the platform supports update capsule across a system reset
+  //
+  if (!FeaturePcdGet(PcdSupportUpdateCapsuleReset)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Construct variable name CapsuleUpdateData, CapsuleUpdateData1, CapsuleUpdateData2...
+  // if user calls UpdateCapsule multiple times.
+  //
+  StrCpyS (CapsuleVarName, sizeof(CapsuleVarName)/sizeof(CHAR16), EFI_CAPSULE_VARIABLE_NAME);
+  TempVarName = CapsuleVarName + StrLen (CapsuleVarName);
+  if (mTimes > 0) {
+    UnicodeValueToString (TempVarName, 0, mTimes, 0);
+  }
+
+  //
+  // ScatterGatherList is only referenced if the capsules are defined to persist across
+  // system reset. Set its value into NV storage to let pre-boot driver to pick it up 
+  // after coming through a system reset.
+  //
+  Status = EfiSetVariable (
+             CapsuleVarName,
+             &gEfiCapsuleVendorGuid,
+             EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+             sizeof (UINTN),
+             (VOID *) &ScatterGatherList
+             );
+  if (!EFI_ERROR (Status)) {
+     //
+     // Variable has been set successfully, increase variable index.
+     //
+     mTimes++;
+     if(InitiateReset) {
+       //
+       // Firmware that encounters a capsule which has the CAPSULE_FLAGS_INITIATE_RESET Flag set in its header
+       // will initiate a reset of the platform which is compatible with the passed-in capsule request and will 
+       // not return back to the caller.
+       //
+       EfiResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+     }
+  }
+  return Status;
+}
+
+/**
+  Returns if the capsule can be supported via UpdateCapsule().
+
+  @param  CapsuleHeaderArray    Virtual pointer to an array of virtual pointers to the capsules
+                                being passed into update capsule.
+  @param  CapsuleCount          Number of pointers to EFI_CAPSULE_HEADER in
+                                CaspuleHeaderArray.
+  @param  MaxiumCapsuleSize     On output the maximum size that UpdateCapsule() can
+                                support as an argument to UpdateCapsule() via
+                                CapsuleHeaderArray and ScatterGatherList.
+  @param  ResetType             Returns the type of reset required for the capsule update.
+
+  @retval EFI_SUCCESS           Valid answer returned.
+  @retval EFI_UNSUPPORTED       The capsule image is not supported on this platform, and
+                                MaximumCapsuleSize and ResetType are undefined.
+  @retval EFI_INVALID_PARAMETER MaximumCapsuleSize is NULL, or ResetTyep is NULL,
+                                Or CapsuleCount is Zero, or CapsuleImage is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+QueryCapsuleCapabilities (
+  IN  EFI_CAPSULE_HEADER   **CapsuleHeaderArray,
+  IN  UINTN                CapsuleCount,
+  OUT UINT64               *MaxiumCapsuleSize,
+  OUT EFI_RESET_TYPE       *ResetType
+  )
+{
+  EFI_STATUS                Status;
+  UINTN                     ArrayNumber;
+  EFI_CAPSULE_HEADER        *CapsuleHeader;
+  BOOLEAN                   NeedReset;
+
+  //
+  // Capsule Count can't be less than one.
+  //
+  if (CapsuleCount < 1) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Check whether input parameter is valid
+  //
+  if ((MaxiumCapsuleSize == NULL) ||(ResetType == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CapsuleHeader = NULL;
+  NeedReset     = FALSE;
+
+  for (ArrayNumber = 0; ArrayNumber < CapsuleCount; ArrayNumber++) {
+    CapsuleHeader = CapsuleHeaderArray[ArrayNumber];
+    //
+    // A capsule which has the CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE flag must have
+    // CAPSULE_FLAGS_PERSIST_ACROSS_RESET set in its header as well.
+    //
+    if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE)) == CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // A capsule which has the CAPSULE_FLAGS_INITIATE_RESET flag must have
+    // CAPSULE_FLAGS_PERSIST_ACROSS_RESET set in its header as well.
+    //
+    if ((CapsuleHeader->Flags & (CAPSULE_FLAGS_PERSIST_ACROSS_RESET | CAPSULE_FLAGS_INITIATE_RESET)) == CAPSULE_FLAGS_INITIATE_RESET) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Check FMP capsule flag 
+    //
+    if (CompareGuid(&CapsuleHeader->CapsuleGuid, &gEfiFmpCapsuleGuid)
+     && (CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) != 0 ) {
+       return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Check Capsule image without populate flag is supported by firmware
+    //
+    if ((CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) == 0) {
+      Status = SupportCapsuleImage (CapsuleHeader);
+      if (EFI_ERROR(Status)) {
+        return Status;
+      }
+    }
+  }
+
+  //
+  // Find out whether there is any capsule defined to persist across system reset. 
+  //
+  for (ArrayNumber = 0; ArrayNumber < CapsuleCount ; ArrayNumber++) {
+    CapsuleHeader = CapsuleHeaderArray[ArrayNumber];
+    if ((CapsuleHeader->Flags & CAPSULE_FLAGS_PERSIST_ACROSS_RESET) != 0) {
+      NeedReset = TRUE;
+      break;
+    }
+  }
+
+  if (NeedReset) {
+    //
+    //Check if the platform supports update capsule across a system reset
+    //
+    if (!FeaturePcdGet(PcdSupportUpdateCapsuleReset)) {
+      return EFI_UNSUPPORTED;
+    }
+    *ResetType = EfiResetWarm;
+    *MaxiumCapsuleSize = (UINT64) mMaxSizePopulateCapsule;
+  } else {
+    //
+    // For non-reset capsule image.
+    //
+    *ResetType = EfiResetCold;
+    *MaxiumCapsuleSize = (UINT64) mMaxSizeNonPopulateCapsule;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+
+  This code installs UEFI capsule runtime service.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.  
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    UEFI Capsule Runtime Services are installed successfully. 
+
+**/
+EFI_STATUS
+EFIAPI
+CapsuleServiceInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  mMaxSizePopulateCapsule = PcdGet32(PcdMaxSizePopulateCapsule);
+  mMaxSizeNonPopulateCapsule = PcdGet32(PcdMaxSizeNonPopulateCapsule);
+
+  //
+  // When PEI phase is IA32, DXE phase is X64, it is possible that capsule data are 
+  // put above 4GB, so capsule PEI will transfer to long mode to get capsule data.
+  // The page table and stack is used to transfer processor mode from IA32 to long mode.
+  // Create the base address of page table and stack, and save them into variable.
+  // This is not needed when capsule with reset type is not supported.
+  //
+  SaveLongModeContext ();
+    
+  //
+  // Install capsule runtime services into UEFI runtime service tables.
+  //
+  gRT->UpdateCapsule                    = UpdateCapsule;
+  gRT->QueryCapsuleCapabilities         = QueryCapsuleCapabilities;
+
+  //
+  // Install the Capsule Architectural Protocol on a new handle
+  // to signify the capsule runtime services are ready.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mNewHandle,
+                  &gEfiCapsuleArchProtocolGuid,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/SaveLongModeContext.c b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/SaveLongModeContext.c
new file mode 100644
index 0000000000..72cea7956b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/SaveLongModeContext.c
@@ -0,0 +1,27 @@
+/** @file
+  Create the NULL function to pass build in IA32/IPF/ARM/EBC.
+
+Copyright (c) 2011, 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 <Uefi.h>
+
+/**
+  Only when PEI is IA32 and DXE is X64, we need transfer to long mode in PEI
+  in order to process capsule data above 4GB. So create a NULL function here for
+  other cases.
+**/
+VOID
+SaveLongModeContext (
+  VOID
+  )
+{
+}
diff --git a/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/X64/SaveLongModeContext.c b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/X64/SaveLongModeContext.c
new file mode 100644
index 0000000000..7e0dd5cf13
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/CapsuleRuntimeDxe/X64/SaveLongModeContext.c
@@ -0,0 +1,215 @@
+/** @file
+  Create the variable to save the base address of page table and stack
+  for transferring into long mode in IA32 capsule PEI.
+
+Copyright (c) 2011 - 2015, 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 <Uefi.h>
+
+#include <Protocol/Capsule.h>
+#include <Protocol/DxeSmmReadyToLock.h>
+#include <Protocol/VariableLock.h>
+
+#include <Guid/CapsuleVendor.h>
+#include <Guid/AcpiS3Context.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+
+//
+// 8 extra pages for PF handler.
+//
+#define EXTRA_PAGE_TABLE_PAGES   8
+
+/**
+  Allocate EfiReservedMemoryType below 4G memory address.
+
+  This function allocates EfiReservedMemoryType below 4G memory address.
+
+  @param  Size      Size of memory to allocate.
+  
+  @return Allocated Address for output.
+
+**/
+VOID*
+AllocateReservedMemoryBelow4G (
+  IN   UINTN   Size
+  )
+{
+  UINTN                 Pages;
+  EFI_PHYSICAL_ADDRESS  Address;
+  EFI_STATUS            Status;
+  VOID*                 Buffer;
+
+  Pages = EFI_SIZE_TO_PAGES (Size);
+  Address = 0xffffffff;
+
+  Status  = gBS->AllocatePages (
+                   AllocateMaxAddress,
+                   EfiReservedMemoryType,
+                   Pages,
+                   &Address
+                   );
+  ASSERT_EFI_ERROR (Status);
+
+  Buffer = (VOID *) (UINTN) Address;
+  ZeroMem (Buffer, Size);
+
+  return Buffer;
+}
+
+/**
+  Register callback function upon VariableLockProtocol
+  to lock EFI_CAPSULE_LONG_MODE_BUFFER_NAME variable to avoid malicious code to update it.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+**/
+VOID
+EFIAPI
+VariableLockCapsuleLongModeBufferVariable (
+  IN  EFI_EVENT                             Event,
+  IN  VOID                                  *Context
+  )
+{
+  EFI_STATUS                    Status;
+  EDKII_VARIABLE_LOCK_PROTOCOL  *VariableLock;
+  //
+  // Mark EFI_CAPSULE_LONG_MODE_BUFFER_NAME variable to read-only if the Variable Lock protocol exists
+  //
+  Status = gBS->LocateProtocol (&gEdkiiVariableLockProtocolGuid, NULL, (VOID **) &VariableLock);
+  if (!EFI_ERROR (Status)) {
+    Status = VariableLock->RequestToLock (VariableLock, EFI_CAPSULE_LONG_MODE_BUFFER_NAME, &gEfiCapsuleVendorGuid);
+    ASSERT_EFI_ERROR (Status);
+  }
+}
+
+/**
+  1. Allocate Reserved memory for capsule PEIM to establish a 1:1 Virtual to Physical mapping.
+  2. Allocate Reserved memroy as a stack for capsule PEIM to transfer from 32-bit mdoe to 64-bit mode.
+
+**/
+VOID
+EFIAPI
+PrepareContextForCapsulePei (
+  VOID
+  )
+{
+  UINTN                                         ExtraPageTablePages;
+  UINT32                                        RegEax;
+  UINT32                                        RegEdx;
+  UINTN                                         TotalPagesNum;
+  UINT8                                         PhysicalAddressBits;
+  UINT32                                        NumberOfPml4EntriesNeeded;
+  UINT32                                        NumberOfPdpEntriesNeeded;
+  BOOLEAN                                       Page1GSupport;
+  EFI_CAPSULE_LONG_MODE_BUFFER                  LongModeBuffer;
+  EFI_STATUS                                    Status;
+  VOID                                          *Registration;
+
+  //
+  // Calculate the size of page table, allocate the memory.
+  //
+  Page1GSupport = FALSE;
+  if (PcdGetBool(PcdUse1GPageTable)) {
+    AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);
+    if (RegEax >= 0x80000001) {
+      AsmCpuid (0x80000001, NULL, NULL, NULL, &RegEdx);
+      if ((RegEdx & BIT26) != 0) {
+        Page1GSupport = TRUE;
+      }
+    }
+  }
+
+  //
+  // Create 4G page table by default,
+  // and let PF handler to handle > 4G request.
+  //
+  PhysicalAddressBits = 32;
+  ExtraPageTablePages = EXTRA_PAGE_TABLE_PAGES;
+
+  //
+  // Calculate the table entries needed.
+  //
+  if (PhysicalAddressBits <= 39 ) {
+    NumberOfPml4EntriesNeeded = 1;
+    NumberOfPdpEntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 30));
+  } else {
+    NumberOfPml4EntriesNeeded = (UINT32)LShiftU64 (1, (PhysicalAddressBits - 39));
+    NumberOfPdpEntriesNeeded = 512;
+  }
+
+  if (!Page1GSupport) {
+    TotalPagesNum = (NumberOfPdpEntriesNeeded + 1) * NumberOfPml4EntriesNeeded + 1;
+  } else {
+    TotalPagesNum = NumberOfPml4EntriesNeeded + 1;
+  }
+  TotalPagesNum += ExtraPageTablePages;
+  DEBUG ((EFI_D_ERROR, "CapsuleRuntimeDxe X64 TotalPagesNum - 0x%x pages\n", TotalPagesNum));
+
+  LongModeBuffer.PageTableAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)AllocateReservedMemoryBelow4G (EFI_PAGES_TO_SIZE (TotalPagesNum));
+  ASSERT (LongModeBuffer.PageTableAddress != 0);
+  
+  //
+  // Allocate stack
+  //
+  LongModeBuffer.StackSize        = PcdGet32 (PcdCapsulePeiLongModeStackSize);
+  LongModeBuffer.StackBaseAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)AllocateReservedMemoryBelow4G (PcdGet32 (PcdCapsulePeiLongModeStackSize));
+  ASSERT (LongModeBuffer.StackBaseAddress != 0);  
+  
+  Status = gRT->SetVariable (
+                  EFI_CAPSULE_LONG_MODE_BUFFER_NAME,
+                  &gEfiCapsuleVendorGuid,
+                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  sizeof (EFI_CAPSULE_LONG_MODE_BUFFER),
+                  &LongModeBuffer
+                  );
+  if (!EFI_ERROR (Status)) {
+      //
+      // Register callback function upon VariableLockProtocol
+      // to lock EFI_CAPSULE_LONG_MODE_BUFFER_NAME variable to avoid malicious code to update it.
+      //
+      EfiCreateProtocolNotifyEvent (
+        &gEdkiiVariableLockProtocolGuid,
+        TPL_CALLBACK,
+        VariableLockCapsuleLongModeBufferVariable,
+        NULL,
+        &Registration
+        );    
+  } else {
+      DEBUG ((EFI_D_ERROR, "FATAL ERROR: CapsuleLongModeBuffer cannot be saved: %r. Capsule in PEI may fail!\n", Status));
+      gBS->FreePages (LongModeBuffer.StackBaseAddress, EFI_SIZE_TO_PAGES (LongModeBuffer.StackSize));
+  }
+}
+
+/**
+  Create the variable to save the base address of page table and stack
+  for transferring into long mode in IA32 capsule PEI.
+**/
+VOID
+SaveLongModeContext (
+  VOID
+  )
+{
+  if ((FeaturePcdGet(PcdSupportUpdateCapsuleReset)) && (FeaturePcdGet (PcdDxeIplSwitchToLongMode))) {
+    //
+    // Allocate memory for Capsule IA32 PEIM, it will create page table to transfer to long mode to access capsule above 4GB.
+    //
+    PrepareContextForCapsulePei ();
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ComponentName.c
new file mode 100644
index 0000000000..ac74d0bc82
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ComponentName.c
@@ -0,0 +1,167 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for ConPlatform driver.
+
+Copyright (c) 2006 - 2011, 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 "ConPlatform.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gConPlatformComponentName = {
+  ConPlatformComponentNameGetDriverName,
+  ConPlatformComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConPlatformComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConPlatformComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConPlatformComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConPlatformDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Platform Console Management Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConPlatformDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gConPlatformComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.c b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.c
new file mode 100644
index 0000000000..795123d106
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.c
@@ -0,0 +1,1154 @@
+/** @file
+  Console Platform DXE Driver, install Console Device Guids and update Console
+  Environment Variables.
+
+Copyright (c) 2006 - 2013, 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 "ConPlatform.h"
+
+
+EFI_DRIVER_BINDING_PROTOCOL gConPlatformTextInDriverBinding = {
+  ConPlatformTextInDriverBindingSupported,
+  ConPlatformTextInDriverBindingStart,
+  ConPlatformTextInDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_DRIVER_BINDING_PROTOCOL gConPlatformTextOutDriverBinding = {
+  ConPlatformTextOutDriverBindingSupported,
+  ConPlatformTextOutDriverBindingStart,
+  ConPlatformTextOutDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Entrypoint of this module.
+
+  This function is the entrypoint of this module. It installs Driver Binding
+  Protocols together with Component Name Protocols.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeConPlatform(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConPlatformTextInDriverBinding,
+             ImageHandle,
+             &gConPlatformComponentName,
+             &gConPlatformComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConPlatformTextOutDriverBinding,
+             NULL,
+             &gConPlatformComponentName,
+             &gConPlatformComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Test to see if EFI_SIMPLE_TEXT_INPUT_PROTOCOL is supported on ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  )
+{
+  return ConPlatformDriverBindingSupported (
+           This,
+           ControllerHandle,
+           &gEfiSimpleTextInProtocolGuid
+           );
+}
+
+
+/**
+  Test to see if EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL is supported on ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  )
+{
+  return ConPlatformDriverBindingSupported (
+           This,
+           ControllerHandle,
+           &gEfiSimpleTextOutProtocolGuid
+           );
+}
+
+
+/**
+  Test to see if the specified protocol is supported on ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  ProtocolGuid        The specfic protocol.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+ConPlatformDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_GUID                     *ProtocolGuid
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Interface;
+
+  //
+  // Test to see if this is a physical device by checking if
+  // it has a Device Path Protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Test to see if this device supports the specified Protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  ProtocolGuid,
+                  (VOID **) &Interface,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+         ControllerHandle,
+         ProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on the device for console input.
+
+  Start this driver on ControllerHandle by opening Simple Text Input Protocol,
+  reading Device Path, and installing Console In Devcice GUID on ControllerHandle.
+
+  If this devcie is not one hot-plug devce, append its device path into the 
+  console environment variables ConInDev.
+  
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL      *RemainingDevicePath
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_DEVICE_PATH_PROTOCOL       *DevicePath;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL *TextIn;
+  BOOLEAN                        IsInConInVariable;
+
+  //
+  // Get the Device Path Protocol so the environment variables can be updated
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Open the Simple Text Input Protocol BY_DRIVER
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextInProtocolGuid,
+                  (VOID **) &TextIn,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Check if the device path is in ConIn Variable
+  //
+  IsInConInVariable = FALSE;
+  Status = ConPlatformUpdateDeviceVariable (
+             L"ConIn",
+             DevicePath,
+             Check
+             );
+  if (!EFI_ERROR (Status)) {
+    IsInConInVariable = TRUE;
+  }
+
+  //
+  // Check the device path, if it is a hot plug device,
+  // do not put the device path into ConInDev, and install
+  // gEfiConsoleInDeviceGuid to the device handle directly.
+  // The policy is, make hot plug device plug in and play immediately.
+  //
+  if (IsHotPlugDevice (DevicePath)) {
+    gBS->InstallMultipleProtocolInterfaces (
+           &ControllerHandle,
+           &gEfiConsoleInDeviceGuid,
+           NULL,
+           NULL
+           );
+    //
+    // Append the device path to ConInDev only if it is in ConIn variable.
+    //
+    if (IsInConInVariable) {
+      ConPlatformUpdateDeviceVariable (
+        L"ConInDev",
+        DevicePath,
+        Append
+        );
+    }
+  } else {
+    //
+    // If it is not a hot-plug device, append the device path to the
+    // ConInDev environment variable
+    //
+    ConPlatformUpdateDeviceVariable (
+      L"ConInDev",
+      DevicePath,
+      Append
+      );
+
+    //
+    // If the device path is an instance in the ConIn environment variable,
+    // then install EfiConsoleInDeviceGuid onto ControllerHandle
+    //
+    if (IsInConInVariable) {
+      gBS->InstallMultipleProtocolInterfaces (
+             &ControllerHandle,
+             &gEfiConsoleInDeviceGuid,
+             NULL,
+             NULL
+             );
+    } else {
+      gBS->CloseProtocol (
+             ControllerHandle,
+             &gEfiSimpleTextInProtocolGuid,
+             This->DriverBindingHandle,
+             ControllerHandle
+             );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on the device for console output and stardard error output.
+
+  Start this driver on ControllerHandle by opening Simple Text Output Protocol,
+  reading Device Path, and installing Console Out Devcic GUID, Standard Error
+  Device GUID on ControllerHandle.
+
+  If this devcie is not one hot-plug devce, append its device path into the 
+  console environment variables ConOutDev, ErrOutDev.
+  
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL      *RemainingDevicePath
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_DEVICE_PATH_PROTOCOL         *DevicePath;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+  BOOLEAN                          NeedClose;
+  BOOLEAN                          IsInConOutVariable;
+  BOOLEAN                          IsInErrOutVariable;
+
+  NeedClose = TRUE;
+
+  //
+  // Get the Device Path Protocol so the environment variables can be updated
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Open the Simple Text Output Protocol BY_DRIVER
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  (VOID **) &TextOut,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Check if the device path is in ConOut & ErrOut Variable
+  //
+  IsInConOutVariable = FALSE;
+  Status = ConPlatformUpdateDeviceVariable (
+             L"ConOut",
+             DevicePath,
+             Check
+             );
+  if (!EFI_ERROR (Status)) {
+    IsInConOutVariable = TRUE;
+  }
+
+  IsInErrOutVariable = FALSE;
+  Status = ConPlatformUpdateDeviceVariable (
+             L"ErrOut",
+             DevicePath,
+             Check
+             );
+  if (!EFI_ERROR (Status)) {
+    IsInErrOutVariable = TRUE;
+  }
+
+  //
+  // Check the device path, if it is a hot plug device,
+  // do not put the device path into ConOutDev and ErrOutDev,
+  // and install gEfiConsoleOutDeviceGuid to the device handle directly.
+  // The policy is, make hot plug device plug in and play immediately.
+  //
+  if (IsHotPlugDevice (DevicePath)) {
+    gBS->InstallMultipleProtocolInterfaces (
+           &ControllerHandle,
+           &gEfiConsoleOutDeviceGuid,
+           NULL,
+           NULL
+           );
+    //
+    // Append the device path to ConOutDev only if it is in ConOut variable.
+    //
+    if (IsInConOutVariable) {
+      ConPlatformUpdateDeviceVariable (
+        L"ConOutDev",
+        DevicePath,
+        Append
+        );
+    }
+    //
+    // Append the device path to ErrOutDev only if it is in ErrOut variable.
+    //
+    if (IsInErrOutVariable) {
+      ConPlatformUpdateDeviceVariable (
+        L"ErrOutDev",
+        DevicePath,
+        Append
+        );
+    }
+  } else {
+    //
+    // If it is not a hot-plug device, append the device path to 
+    // the ConOutDev and ErrOutDev environment variable.
+    // For GOP device path, append the sibling device path as well.
+    //
+    if (!ConPlatformUpdateGopCandidate (DevicePath)) {
+      ConPlatformUpdateDeviceVariable (
+        L"ConOutDev",
+        DevicePath,
+        Append
+        );
+      //
+      // Then append the device path to the ErrOutDev environment variable
+      //
+      ConPlatformUpdateDeviceVariable (
+        L"ErrOutDev",
+        DevicePath,
+        Append
+        );
+    }
+
+    //
+    // If the device path is an instance in the ConOut environment variable,
+    // then install EfiConsoleOutDeviceGuid onto ControllerHandle
+    //
+    if (IsInConOutVariable) {
+      NeedClose = FALSE;
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &ControllerHandle,
+                      &gEfiConsoleOutDeviceGuid,
+                      NULL,
+                      NULL
+                      );
+    }
+    //
+    // If the device path is an instance in the ErrOut environment variable,
+    // then install EfiStandardErrorDeviceGuid onto ControllerHandle
+    //
+    if (IsInErrOutVariable) {
+      NeedClose = FALSE;
+      gBS->InstallMultipleProtocolInterfaces (
+             &ControllerHandle,
+             &gEfiStandardErrorDeviceGuid,
+             NULL,
+             NULL
+             );
+    }
+
+    if (NeedClose) {
+      gBS->CloseProtocol (
+             ControllerHandle,
+             &gEfiSimpleTextOutProtocolGuid,
+             This->DriverBindingHandle,
+             ControllerHandle
+             );
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop this driver on ControllerHandle by removing Console In Devcice GUID 
+  and closing the Simple Text Input protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  //
+  // Get the Device Path Protocol firstly
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  //
+  // If there is device path on ControllerHandle
+  //
+  if (!EFI_ERROR (Status)) {
+    //
+    // Remove DevicePath from ConInDev if exists.
+    //
+    ConPlatformUpdateDeviceVariable (
+      L"ConInDev",
+      DevicePath,
+      Delete
+      );
+  }
+
+  //
+  // Uninstall the Console Device GUIDs from Controller Handle
+  //
+  ConPlatformUnInstallProtocol (
+    This,
+    ControllerHandle,
+    &gEfiConsoleInDeviceGuid
+    );
+
+  //
+  // Close the Simple Text Input Protocol
+  //
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiSimpleTextInProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Stop this driver on ControllerHandle by removing Console Out Devcice GUID 
+  and closing the Simple Text Output protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  //
+  // Get the Device Path Protocol firstly
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Remove DevicePath from ConOutDev and ErrOutDev if exists.
+    //
+    ConPlatformUpdateDeviceVariable (
+      L"ConOutDev",
+      DevicePath,
+      Delete
+      );
+    ConPlatformUpdateDeviceVariable (
+      L"ErrOutDev",
+      DevicePath,
+      Delete
+      );
+  }
+
+  //
+  // Uninstall the Console Device GUIDs from Controller Handle
+  //
+  ConPlatformUnInstallProtocol (
+    This,
+    ControllerHandle,
+    &gEfiConsoleOutDeviceGuid
+    );
+
+  ConPlatformUnInstallProtocol (
+    This,
+    ControllerHandle,
+    &gEfiStandardErrorDeviceGuid
+    );
+
+  //
+  // Close the Simple Text Output Protocol
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        &gEfiSimpleTextOutProtocolGuid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Uninstall the specified protocol.
+
+  @param This            Protocol instance pointer.
+  @param Handle          Handle of device to uninstall protocol on.
+  @param ProtocolGuid    The specified protocol need to be uninstalled.
+
+**/
+VOID
+ConPlatformUnInstallProtocol (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_GUID                     *ProtocolGuid
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  ProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Handle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Handle,
+           ProtocolGuid,
+           NULL,
+           NULL
+           );
+  }
+
+  return ;
+}
+
+/**
+  Get the necessary size of buffer and read the variable.
+
+  First get the necessary size of buffer. Then read the
+  EFI variable (Name) and return a dynamically allocated
+  buffer. On failure return NULL.
+
+  @param  Name             String part of EFI variable name
+
+  @return Dynamically allocated memory that contains a copy of the EFI variable.
+          Caller is repsoncible freeing the buffer. Return NULL means Variable 
+          was not read.
+
+**/
+VOID *
+ConPlatformGetVariable (
+  IN  CHAR16    *Name
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Buffer;
+  UINTN       BufferSize;
+
+  BufferSize  = 0;
+  Buffer      = NULL;
+
+  //
+  // Test to see if the variable exists.  If it doesn't, return NULL.
+  //
+  Status = gRT->GetVariable (
+                  Name,
+                  &gEfiGlobalVariableGuid,
+                  NULL,
+                  &BufferSize,
+                  Buffer
+                  );
+
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // Allocate the buffer to return
+    //
+    Buffer = AllocatePool (BufferSize);
+    if (Buffer == NULL) {
+      return NULL;
+    }
+    //
+    // Read variable into the allocated buffer.
+    //
+    Status = gRT->GetVariable (
+                    Name,
+                    &gEfiGlobalVariableGuid,
+                    NULL,
+                    &BufferSize,
+                    Buffer
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (Buffer);
+      //
+      // To make sure Buffer is NULL if any error occurs.
+      //
+      Buffer = NULL;
+    }
+  }
+
+  return Buffer;
+}
+
+/**
+  Function returns TRUE when the two input device paths point to the two
+  GOP child handles that have the same parent.
+
+  @param Left    A pointer to a device path data structure.
+  @param Right   A pointer to a device path data structure.
+
+  @retval TRUE  Left and Right share the same parent.
+  @retval FALSE Left and Right don't share the same parent or either of them is not
+                a GOP device path.
+**/
+BOOLEAN
+IsGopSibling (
+  IN EFI_DEVICE_PATH_PROTOCOL  *Left,
+  IN EFI_DEVICE_PATH_PROTOCOL  *Right
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *NodeLeft;
+  EFI_DEVICE_PATH_PROTOCOL  *NodeRight;
+
+  for (NodeLeft = Left; !IsDevicePathEndType (NodeLeft); NodeLeft = NextDevicePathNode (NodeLeft)) {
+    if ((DevicePathType (NodeLeft) == ACPI_DEVICE_PATH && DevicePathSubType (NodeLeft) == ACPI_ADR_DP) ||
+        (DevicePathType (NodeLeft) == HARDWARE_DEVICE_PATH && DevicePathSubType (NodeLeft) == HW_CONTROLLER_DP &&
+         DevicePathType (NextDevicePathNode (NodeLeft)) == ACPI_DEVICE_PATH && DevicePathSubType (NextDevicePathNode (NodeLeft)) == ACPI_ADR_DP)) {
+      break;
+    }
+  }
+
+  if (IsDevicePathEndType (NodeLeft)) {
+    return FALSE;
+  }
+
+  for (NodeRight = Right; !IsDevicePathEndType (NodeRight); NodeRight = NextDevicePathNode (NodeRight)) {
+    if ((DevicePathType (NodeRight) == ACPI_DEVICE_PATH && DevicePathSubType (NodeRight) == ACPI_ADR_DP) ||
+        (DevicePathType (NodeRight) == HARDWARE_DEVICE_PATH && DevicePathSubType (NodeRight) == HW_CONTROLLER_DP &&
+         DevicePathType (NextDevicePathNode (NodeRight)) == ACPI_DEVICE_PATH && DevicePathSubType (NextDevicePathNode (NodeRight)) == ACPI_ADR_DP)) {
+      break;
+    }
+  }
+
+  if (IsDevicePathEndType (NodeRight)) {
+    return FALSE;
+  }
+
+  if (((UINTN) NodeLeft - (UINTN) Left) != ((UINTN) NodeRight - (UINTN) Right)) {
+    return FALSE;
+  }
+
+  return (BOOLEAN) (CompareMem (Left, Right, (UINTN) NodeLeft - (UINTN) Left) == 0);
+}
+
+/**
+  Function compares a device path data structure to that of all the nodes of a
+  second device path instance.
+
+  @param Multi           A pointer to a multi-instance device path data structure.
+  @param Single          A pointer to a single-instance device path data structure.
+  @param NewDevicePath   If Delete is TRUE, this parameter must not be null, and it
+                         points to the remaining device path data structure.
+                         (remaining device path = Multi - Single.)
+  @param Delete          If TRUE, means removing Single from Multi.
+                         If FALSE, the routine just check whether Single matches
+                         with any instance in Multi.
+
+  @retval EFI_SUCCESS           If the Single is contained within Multi.
+  @retval EFI_NOT_FOUND         If the Single is not contained within Multi.
+  @retval EFI_INVALID_PARAMETER Multi is NULL.
+  @retval EFI_INVALID_PARAMETER Single is NULL.
+  @retval EFI_INVALID_PARAMETER NewDevicePath is NULL when Delete is TRUE.
+
+**/
+EFI_STATUS
+ConPlatformMatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single,
+  OUT EFI_DEVICE_PATH_PROTOCOL  **NewDevicePath OPTIONAL,
+  IN  BOOLEAN                   Delete
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *TempDevicePath1;
+  EFI_DEVICE_PATH_PROTOCOL  *TempDevicePath2;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathInst;
+  UINTN                     Size;
+
+  //
+  // The passed in DevicePath should not be NULL
+  //
+  if ((Multi == NULL) || (Single == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If performing Delete operation, the NewDevicePath must not be NULL.
+  //
+  if (Delete) {
+    if (NewDevicePath == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  TempDevicePath1 = NULL;
+
+  DevicePath      = Multi;
+  DevicePathInst  = GetNextDevicePathInstance (&DevicePath, &Size);
+
+  //
+  // Search for the match of 'Single' in 'Multi'
+  //
+  while (DevicePathInst != NULL) {
+    if ((CompareMem (Single, DevicePathInst, Size) == 0) || IsGopSibling (Single, DevicePathInst)) {
+      if (!Delete) {
+        //
+        // If Delete is FALSE, return EFI_SUCCESS if Single is found in Multi.
+        //
+        FreePool (DevicePathInst);
+        return EFI_SUCCESS;
+      }
+    } else {
+      if (Delete) {
+        //
+        // If the node of Multi does not match Single, then added it back to the result.
+        // That is, the node matching Single will be dropped and deleted from result.
+        //
+        TempDevicePath2 = AppendDevicePathInstance (
+                            TempDevicePath1,
+                            DevicePathInst
+                            );
+        if (TempDevicePath1 != NULL) {
+          FreePool (TempDevicePath1);
+        }
+        TempDevicePath1 = TempDevicePath2;
+      }
+    }
+
+    FreePool (DevicePathInst);
+    DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+  }
+
+  if (Delete) {
+    //
+    // Return the new device path data structure with specified node deleted.
+    //
+    *NewDevicePath = TempDevicePath1;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Update console environment variables. 
+
+  @param  VariableName    Console environment variables, ConOutDev, ConInDev
+                          ErrOutDev, ConIn ,ConOut or ErrOut.
+  @param  DevicePath      Console devcie's device path.
+  @param  Operation       Variable operations, including APPEND, CHECK and DELETE.
+
+  @retval EFI_SUCCESS           Variable operates successfully.
+  @retval EFI_OUT_OF_RESOURCES  If variable cannot be appended.
+  @retval other                 Variable updating failed.
+
+**/
+EFI_STATUS
+ConPlatformUpdateDeviceVariable (
+  IN  CHAR16                    *VariableName,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN  CONPLATFORM_VAR_OPERATION Operation
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *VariableDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *NewVariableDevicePath;
+
+  VariableDevicePath    = NULL;
+  NewVariableDevicePath = NULL;
+
+  //
+  // Get Variable according to variable name.
+  // The memory for Variable is allocated within ConPlatformGetVarible(),
+  // it is the caller's responsibility to free the memory before return.
+  //
+  VariableDevicePath = ConPlatformGetVariable (VariableName);
+
+  if (Operation != Delete) {
+    //
+    // Match specified DevicePath in Console Variable.
+    // 
+    Status = ConPlatformMatchDevicePaths (
+               VariableDevicePath,
+               DevicePath,
+               NULL,
+               FALSE
+               );
+
+    if ((Operation == Check) || (!EFI_ERROR (Status))) {
+      //
+      // Branch here includes 2 cases:
+      // 1. Operation is CHECK, simply return Status.
+      // 2. Operation is APPEND, and device path already exists in variable, also return.
+      //
+      if (VariableDevicePath != NULL) {
+        FreePool (VariableDevicePath);
+      }
+
+      return Status;
+    }
+    //
+    // We reach here to append a device path that does not exist in variable.
+    //
+    Status = EFI_SUCCESS;
+    NewVariableDevicePath = AppendDevicePathInstance (
+                              VariableDevicePath,
+                              DevicePath
+                              );
+    if (NewVariableDevicePath == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+    }
+
+  } else {
+    //
+    // We reach here to remove DevicePath from the environment variable that
+    // is a multi-instance device path.
+    //
+    Status = ConPlatformMatchDevicePaths (
+               VariableDevicePath,
+               DevicePath,
+               &NewVariableDevicePath,
+               TRUE
+               );
+  }
+
+  if (VariableDevicePath != NULL) {
+    FreePool (VariableDevicePath);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (NewVariableDevicePath != NULL) {
+    //
+    // Update Console Environment Variable.
+    //
+    Status = gRT->SetVariable (
+                    VariableName,
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                    GetDevicePathSize (NewVariableDevicePath),
+                    NewVariableDevicePath
+                    );
+
+    FreePool (NewVariableDevicePath);
+  }
+
+  return Status;
+}
+
+/**
+  Check if the device supports hot-plug through its device path.
+
+  This function could be updated to check more types of Hot Plug devices.
+  Currently, it checks USB and PCCard device.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a hot-plug device
+  @retval FALSE                 The devcie is not a hot-plug device.
+
+**/
+BOOLEAN
+IsHotPlugDevice (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL     *CheckDevicePath;
+
+  CheckDevicePath = DevicePath;
+  while (!IsDevicePathEnd (CheckDevicePath)) {
+    //
+    // Check device whether is hot plug device or not throught Device Path
+    // 
+    if ((DevicePathType (CheckDevicePath) == MESSAGING_DEVICE_PATH) &&
+        (DevicePathSubType (CheckDevicePath) == MSG_USB_DP ||
+         DevicePathSubType (CheckDevicePath) == MSG_USB_CLASS_DP ||
+         DevicePathSubType (CheckDevicePath) == MSG_USB_WWID_DP)) {
+      //
+      // If Device is USB device
+      //
+      return TRUE;
+    }
+    if ((DevicePathType (CheckDevicePath) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType (CheckDevicePath) == HW_PCCARD_DP)) {
+      //
+      // If Device is PCCard
+      //
+      return TRUE;
+    }
+  
+    CheckDevicePath = NextDevicePathNode (CheckDevicePath);
+  }
+
+  return FALSE;
+}
+
+/**
+  Update ConOutDev and ErrOutDev variables to add the device path of
+  GOP controller itself and the sibling controllers.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a GOP device.
+  @retval FALSE                 The devcie is not a GOP device.
+
+**/
+BOOLEAN
+ConPlatformUpdateGopCandidate (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_HANDLE                           PciHandle;
+  EFI_HANDLE                           GopHandle;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY  *OpenInfoBuffer;
+  UINTN                                EntryCount;
+  UINTN                                Index;
+  EFI_DEVICE_PATH_PROTOCOL             *ChildDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL             *TempDevicePath;
+
+  //
+  // Check whether it's a GOP device.
+  //
+  TempDevicePath = DevicePath;
+  Status = gBS->LocateDevicePath (&gEfiGraphicsOutputProtocolGuid, &TempDevicePath, &GopHandle);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Get the parent PciIo handle in order to find all the children
+  //
+  Status = gBS->LocateDevicePath (&gEfiPciIoProtocolGuid, &DevicePath, &PciHandle);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  Status = gBS->OpenProtocolInformation (
+                  PciHandle,
+                  &gEfiPciIoProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  for (Index = 0; Index < EntryCount; Index++) {
+    //
+    // Query all the children created by the GOP driver
+    //
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      Status = gBS->OpenProtocol (
+                      OpenInfoBuffer[Index].ControllerHandle,
+                      &gEfiDevicePathProtocolGuid,
+                      (VOID **) &ChildDevicePath,
+                      NULL,
+                      NULL,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (!EFI_ERROR (Status)) {
+        //
+        // Append the device path to ConOutDev and ErrOutDev
+        //
+        ConPlatformUpdateDeviceVariable (L"ConOutDev", ChildDevicePath, Append);
+        ConPlatformUpdateDeviceVariable (L"ErrOutDev", ChildDevicePath, Append);
+      }
+    }
+  }
+  FreePool (OpenInfoBuffer);
+
+  return TRUE;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.h b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.h
new file mode 100644
index 0000000000..4e59e1bbd7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatform.h
@@ -0,0 +1,442 @@
+/** @file
+  Header file for Console Platfrom DXE Driver.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _CON_PLATFORM_H_
+#define _CON_PLATFORM_H_
+
+#include <Uefi.h>
+
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/GraphicsOutput.h>
+
+#include <Guid/GlobalVariable.h>
+#include <Guid/ConsoleInDevice.h>
+#include <Guid/StandardErrorDevice.h>
+#include <Guid/ConsoleOutDevice.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+//
+// Driver Binding Externs
+//
+extern EFI_DRIVER_BINDING_PROTOCOL  gConPlatformTextInDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConPlatformComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConPlatformComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gConPlatformTextOutDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConPlatformComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConPlatformComponentName2;
+
+
+typedef enum {
+  Check,
+  Append,
+  Delete
+} CONPLATFORM_VAR_OPERATION;
+
+/**
+  Test to see if specific protocol could be supported on the ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  ProtocolGuid        The specfic protocol.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+ConPlatformDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_GUID                     *ProtocolGuid
+  );
+
+/**
+  Test to see if EFI_SIMPLE_TEXT_INPUT_PROTOCOL is supported on ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  );
+
+/**
+  Test to see if EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL is supported on ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  );
+
+/**
+  Start this driver on the device for console input.
+
+  Start this driver on ControllerHandle by opening Simple Text Input Protocol,
+  reading Device Path, and installing Console In Devcice GUID on ControllerHandle.
+
+  If this devcie is not one hot-plug devce, append its device path into the 
+  console environment variables ConInDev.
+  
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Handle,
+  IN  EFI_DEVICE_PATH_PROTOCOL      *RemainingDevicePath
+  );
+
+/**
+  Start this driver on the device for console output and stardard error output.
+
+  Start this driver on ControllerHandle by opening Simple Text Output Protocol,
+  reading Device Path, and installing Console Out Devcic GUID, Standard Error
+  Device GUID on ControllerHandle.
+
+  If this devcie is not one hot-plug devce, append its device path into the 
+  console environment variables ConOutDev, StdErrDev.
+  
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Handle,
+  IN  EFI_DEVICE_PATH_PROTOCOL      *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle by removing Console In Devcice GUID 
+  and closing the Simple Text Input protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextInDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Stop this driver on ControllerHandle by removing Console Out Devcice GUID 
+  and closing the Simple Text Output protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformTextOutDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Uninstall the specified protocol.
+
+  @param This            Protocol instance pointer.
+  @param Handle          Handle of device to uninstall protocol on.
+  @param ProtocolGuid    The specified protocol need to be uninstalled.
+
+**/
+VOID
+ConPlatformUnInstallProtocol (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_GUID                     *ProtocolGuid
+  );
+
+/**
+  Read the EFI variable (Name) and return a dynamically allocated
+  buffer, and the size of the buffer. On failure return NULL.
+
+  @param  Name             String part of EFI variable name
+
+  @return Dynamically allocated memory that contains a copy of the EFI variable.
+          Caller is repsoncible freeing the buffer. Return NULL means Variable 
+          was not read.
+
+**/
+VOID *
+ConPlatformGetVariable (
+  IN  CHAR16              *Name
+  );
+
+/**
+  Function compares a device path data structure to that of all the nodes of a
+  second device path instance.
+
+
+  @param Multi           A pointer to a multi-instance device path data structure.
+  @param Single          A pointer to a single-instance device path data structure.
+  @param NewDevicePath   If Delete is TRUE, this parameter must not be null, and it
+                         points to the remaining device path data structure.
+                         (remaining device path = Multi - Single.)
+  @param Delete          If TRUE, means removing Single from Multi.
+                         If FALSE, the routine just check whether Single matches
+                         with any instance in Multi.
+
+  @retval EFI_SUCCESS           If the Single is contained within Multi.
+  @retval EFI_NOT_FOUND         If the Single is not contained within Multi.
+  @retval EFI_INVALID_PARAMETER Multi is NULL.
+  @retval EFI_INVALID_PARAMETER Single is NULL.
+  @retval EFI_INVALID_PARAMETER NewDevicePath is NULL when Delete is TRUE.
+
+**/
+EFI_STATUS
+ConPlatformMatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single,
+  OUT EFI_DEVICE_PATH_PROTOCOL  **NewDevicePath OPTIONAL,
+  IN  BOOLEAN                   Delete
+  );
+
+/**
+  Update console environment variables.
+
+  @param  VariableName    Console environment variables, ConOutDev, ConInDev
+                          StdErrDev, ConIn or ConOut.
+  @param  DevicePath      Console devcie's device path.
+  @param  Operation       Variable operations, including APPEND, CHECK and DELETE.
+
+  @retval EFI_SUCCESS           Variable operates successfully.
+  @retval EFI_OUT_OF_RESOURCES  If variable cannot be appended.
+  @retval other                 Variable updating failed.
+
+**/
+EFI_STATUS
+ConPlatformUpdateDeviceVariable (
+  IN  CHAR16                    *VariableName,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN  CONPLATFORM_VAR_OPERATION Operation
+  );
+
+/**
+  Check if the device supports hot-plug through its device path.
+
+  This function could be updated to check more types of Hot Plug devices.
+  Currently, it checks USB and PCCard device.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a hot-plug device
+  @retval FALSE                 The devcie is not a hot-plug device.
+
+**/
+BOOLEAN
+IsHotPlugDevice (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConPlatformComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Update ConOutDev and ErrOutDev variables to add the device path of
+  GOP controller itself and the sibling controllers.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a GOP device.
+  @retval FALSE                 The devcie is not a GOP device.
+
+**/
+BOOLEAN
+ConPlatformUpdateGopCandidate (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.inf b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.inf
new file mode 100644
index 0000000000..de66b468e4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.inf
@@ -0,0 +1,98 @@
+## @file
+# Platform console driver manages console devices.
+#
+# Console Platfrom DXE Driver that specifies whether device can be used as console
+# input/output device or error output device and update global variables accordingly.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ConPlatformDxe
+  MODULE_UNI_FILE                = ConPlatformDxe.uni
+  FILE_GUID                      = 51ccf399-4fdf-4e55-a45b-e123f84d456a
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeConPlatform
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gConPlatformTextInDriverBinding
+#  COMPONENT_NAME                =  gConPlatformComponentName
+#  COMPONENT_NAME2               =  gConPlatformComponentName2
+#  DRIVER_BINDING                =  gConPlatformTextOutDriverBinding
+#  COMPONENT_NAME                =  gConPlatformComponentName
+#  COMPONENT_NAME2               =  gConPlatformComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  ConPlatform.h
+  ConPlatform.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  DevicePathLib
+  UefiRuntimeServicesTableLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Guids]
+  #
+  # This is the VendorGuid of all architecturally defined variables in UEFI spec.
+  #
+  ## SOMETIMES_CONSUMES ## Variable:L"ConIn"
+  ## SOMETIMES_CONSUMES ## Variable:L"ConOut"
+  ## SOMETIMES_CONSUMES ## Variable:L"ErrOut"
+  ## SOMETIMES_PRODUCES ## Variable:L"ConInDev"
+  ## SOMETIMES_PRODUCES ## Variable:L"ConOutDev"
+  ## SOMETIMES_PRODUCES ## Variable:L"ErrOutDev"
+  gEfiGlobalVariableGuid
+  #
+  # This GUID is used to specify the device is the standard error device.
+  # If the device is a standard error device, this GUID as the protocol GUID will be installed
+  # onto this device handle.
+  #
+  gEfiStandardErrorDeviceGuid                   ## SOMETIMES_PRODUCES ## UNDEFINED # protocol GUID installed on device handle
+  #
+  # This GUID is used to specify the device is the console output device.
+  # If the device is a console output device, this GUID as the protocol GUID will be installed
+  # onto this device handle.
+  #
+  gEfiConsoleOutDeviceGuid                      ## SOMETIMES_PRODUCES ## UNDEFINED # protocol GUID installed on device handle
+  #
+  # This GUID is used to specify the device is the console input device.
+  # If the device is a console input device, this GUID as the protocol GUID will be installed
+  # onto this device handle.
+  #
+  gEfiConsoleInDeviceGuid                       ## SOMETIMES_PRODUCES ## UNDEFINED # protocol GUID installed on device handle
+
+[Protocols]
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimpleTextInProtocolGuid                  ## TO_START
+  gEfiSimpleTextOutProtocolGuid                 ## TO_START
+  gEfiPciIoProtocolGuid                         ## SOMETIMES_CONSUMES
+  gEfiGraphicsOutputProtocolGuid                ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ConPlatformDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.uni b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.uni
new file mode 100644
index 0000000000..b707a6d110
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxeExtra.uni b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxeExtra.uni
new file mode 100644
index 0000000000..1e1cc7ec6c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConPlatformDxe/ConPlatformDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ComponentName.c
new file mode 100644
index 0000000000..9ad1726599
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ComponentName.c
@@ -0,0 +1,776 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for ConSplitter driver.
+
+Copyright (c) 2006 - 2011, 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 "ConSplitter.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gConSplitterConInComponentName = {
+  ConSplitterComponentNameGetDriverName,
+  ConSplitterConInComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConSplitterConInComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConSplitterComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConSplitterConInComponentNameGetControllerName,
+  "en"
+};
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gConSplitterSimplePointerComponentName = {
+  ConSplitterComponentNameGetDriverName,
+  ConSplitterSimplePointerComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConSplitterSimplePointerComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConSplitterComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConSplitterSimplePointerComponentNameGetControllerName,
+  "en"
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL gConSplitterAbsolutePointerComponentName = {
+  ConSplitterComponentNameGetDriverName,
+  ConSplitterAbsolutePointerComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConSplitterAbsolutePointerComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConSplitterComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConSplitterAbsolutePointerComponentNameGetControllerName,
+  "en"
+};
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gConSplitterConOutComponentName = {
+  ConSplitterComponentNameGetDriverName,
+  ConSplitterConOutComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConSplitterConOutComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConSplitterComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConSplitterConOutComponentNameGetControllerName,
+  "en"
+};
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gConSplitterStdErrComponentName = {
+  ConSplitterComponentNameGetDriverName,
+  ConSplitterStdErrComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gConSplitterStdErrComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ConSplitterComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ConSplitterStdErrComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterDriverNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"Console Splitter Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterConInControllerNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"Primary Console Input Device"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterSimplePointerControllerNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"Primary Simple Pointer Device"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterAbsolutePointerControllerNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *)L"Primary Absolute Pointer Device"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterConOutControllerNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"Primary Console Output Device"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mConSplitterStdErrControllerNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"Primary Standard Error Device"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterDriverNameTable,
+           DriverName,
+           (BOOLEAN)((This == &gConSplitterConInComponentName) || 
+                     (This == &gConSplitterSimplePointerComponentName) ||
+                     (This == &gConSplitterAbsolutePointerComponentName) ||
+                     (This == &gConSplitterConOutComponentName) ||
+                     (This == &gConSplitterStdErrComponentName))
+           );
+}
+
+/**
+  Tests whether a controller handle is being managed by a specific driver and
+  the child handle is a child device of the controller.
+
+  @param  ControllerHandle     A handle for a controller to test.
+  @param  DriverBindingHandle  Specifies the driver binding handle for the
+                               driver.
+  @param  ProtocolGuid         Specifies the protocol that the driver specified
+                               by DriverBindingHandle opens in its Start()
+                               function.
+  @param  ChildHandle          A child handle to test.
+  @param  ConsumsedGuid        Supplies the protocol that the child controller
+                               opens on its parent controller.
+
+  @retval EFI_SUCCESS          ControllerHandle is managed by the driver
+                               specifed by DriverBindingHandle and ChildHandle
+                               is a child of the ControllerHandle.
+  @retval EFI_UNSUPPORTED      ControllerHandle is not managed by the driver
+                               specifed by DriverBindingHandle.
+  @retval EFI_UNSUPPORTED      ChildHandle is not a child of the
+                               ControllerHandle.
+
+**/
+EFI_STATUS
+ConSplitterTestControllerHandles (
+  IN  CONST EFI_HANDLE       ControllerHandle,
+  IN  CONST EFI_HANDLE       DriverBindingHandle,
+  IN  CONST EFI_GUID         *ProtocolGuid,
+  IN  EFI_HANDLE             ChildHandle,
+  IN  CONST EFI_GUID         *ConsumsedGuid
+  )
+{
+  EFI_STATUS                 Status;
+
+  //
+  // here ChildHandle is not an Optional parameter.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Tests whether a controller handle is being managed by a specific driver.
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             DriverBindingHandle,
+             ProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Tests whether a child handle is a child device of the controller.
+  //
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             ConsumsedGuid
+             );
+
+  return Status;
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                     Status;
+
+  Status = ConSplitterTestControllerHandles (
+             ControllerHandle,
+             gConSplitterConInDriverBinding.DriverBindingHandle,
+             &gEfiConsoleInDeviceGuid,
+             ChildHandle,
+             &gEfiConsoleInDeviceGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterConInControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gConSplitterConInComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                  Status;
+
+  Status = ConSplitterTestControllerHandles (
+             ControllerHandle,
+             gConSplitterSimplePointerDriverBinding.DriverBindingHandle,
+             &gEfiSimplePointerProtocolGuid,
+             ChildHandle,
+             &gEfiSimplePointerProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterSimplePointerControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gConSplitterSimplePointerComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by an EFI Driver.
+
+  @param  This                   A pointer to the EFI_COMPONENT_NAME_PROTOCOL
+                                 instance.
+  @param  ControllerHandle       The handle of a controller that the driver
+                                 specified by This is managing.  This handle
+                                 specifies the controller whose name is to be
+                                 returned.
+  @param  ChildHandle            The handle of the child controller to retrieve the
+                                 name of.  This is an optional parameter that may
+                                 be NULL.  It will be NULL for device drivers.  It
+                                 will also be NULL for a bus drivers that wish to
+                                 retrieve the name of the bus controller.  It will
+                                 not be NULL for a bus driver that wishes to
+                                 retrieve the name of a child controller.
+  @param  Language               A pointer to RFC4646 language identifier. This is
+                                 the language of the controller name that that the
+                                 caller is requesting, and it must match one of the
+                                 languages specified in SupportedLanguages.  The
+                                 number of languages supported by a driver is up to
+                                 the driver writer.
+  @param  ControllerName         A pointer to the Unicode string to return.  This
+                                 Unicode string is the name of the controller
+                                 specified by ControllerHandle and ChildHandle in
+                                 the language specified by Language from the point
+                                 of view of the driver specified by This.
+
+  @retval EFI_SUCCESS            The Unicode string for the user readable name in
+                                 the language specified by Language for the driver
+                                 specified by This was returned in DriverName.
+  @retval EFI_INVALID_PARAMETER  ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER  ChildHandle is not NULL and it is not a valid
+                                 EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER  Language is NULL.
+  @retval EFI_INVALID_PARAMETER  ControllerName is NULL.
+  @retval EFI_UNSUPPORTED        The driver specified by This is not currently
+                                 managing the controller specified by
+                                 ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED        The driver specified by This does not support the
+                                 language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+
+  Status = ConSplitterTestControllerHandles (
+             ControllerHandle,
+             gConSplitterAbsolutePointerDriverBinding.DriverBindingHandle,
+             &gEfiAbsolutePointerProtocolGuid,
+             ChildHandle,
+             &gEfiAbsolutePointerProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterAbsolutePointerControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gConSplitterAbsolutePointerComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                       Status;
+
+  Status = ConSplitterTestControllerHandles (
+             ControllerHandle,
+             gConSplitterConOutDriverBinding.DriverBindingHandle,
+             &gEfiConsoleOutDeviceGuid,
+             ChildHandle,
+             &gEfiConsoleOutDeviceGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterConOutControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gConSplitterConOutComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                       Status;
+
+  Status = ConSplitterTestControllerHandles (
+             ControllerHandle,
+             gConSplitterStdErrDriverBinding.DriverBindingHandle,
+             &gEfiStandardErrorDeviceGuid,
+             ChildHandle,
+             &gEfiStandardErrorDeviceGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mConSplitterStdErrControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gConSplitterStdErrComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.c b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.c
new file mode 100644
index 0000000000..dae97b09a2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.c
@@ -0,0 +1,4778 @@
+/** @file
+  Console Splitter Driver. Any Handle that attatched console I/O protocols
+  (Console In device, Console Out device, Console Error device, Simple Pointer
+  protocol, Absolute Pointer protocol) can be bound by this driver.
+
+  So far it works like any other driver by opening a SimpleTextIn and/or
+  SimpleTextOut protocol with EFI_OPEN_PROTOCOL_BY_DRIVER attributes. The big
+  difference is this driver does not layer a protocol on the passed in
+  handle, or construct a child handle like a standard device or bus driver.
+  This driver produces three virtual handles as children, one for console input
+  splitter, one for console output splitter and one for error output splitter.
+  These 3 virtual handles would be installed on gST.
+
+  Each virtual handle, that supports the Console I/O protocol, will be produced
+  in the driver entry point. The virtual handle are added on driver entry and
+  never removed. Such design ensures sytem function well during none console
+  device situation.
+
+Copyright (c) 2006 - 2015, 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 "ConSplitter.h"
+
+//
+// Identify if ConIn is connected in PcdConInConnectOnDemand enabled mode. 
+// default not connect
+//
+BOOLEAN  mConInIsConnect = FALSE;
+
+//
+// Text In Splitter Private Data template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED TEXT_IN_SPLITTER_PRIVATE_DATA  mConIn = {
+  TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE,
+  (EFI_HANDLE) NULL,
+
+  {
+    ConSplitterTextInReset,
+    ConSplitterTextInReadKeyStroke,
+    (EFI_EVENT) NULL
+  },
+  0,
+  (EFI_SIMPLE_TEXT_INPUT_PROTOCOL **) NULL,
+  0,
+
+  {
+    ConSplitterTextInResetEx,
+    ConSplitterTextInReadKeyStrokeEx,
+    (EFI_EVENT) NULL,
+    ConSplitterTextInSetState,
+    ConSplitterTextInRegisterKeyNotify,
+    ConSplitterTextInUnregisterKeyNotify
+  },
+  0,
+  (EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL **) NULL,
+  0,
+  {
+    (LIST_ENTRY *) NULL,
+    (LIST_ENTRY *) NULL
+  },
+
+  {
+    ConSplitterSimplePointerReset,
+    ConSplitterSimplePointerGetState,
+    (EFI_EVENT) NULL,
+    (EFI_SIMPLE_POINTER_MODE *) NULL
+  },
+  {
+    0x10000,
+    0x10000,
+    0x10000,
+    TRUE,
+    TRUE
+  },
+  0,
+  (EFI_SIMPLE_POINTER_PROTOCOL **) NULL,
+  0,
+
+  {
+    ConSplitterAbsolutePointerReset,
+    ConSplitterAbsolutePointerGetState,
+    (EFI_EVENT) NULL,
+    (EFI_ABSOLUTE_POINTER_MODE *) NULL
+  },
+  {
+    0,       // AbsoluteMinX
+    0,       // AbsoluteMinY
+    0,       // AbsoluteMinZ
+    0x10000, // AbsoluteMaxX
+    0x10000, // AbsoluteMaxY
+    0x10000, // AbsoluteMaxZ
+    0        // Attributes
+  },
+  0,
+  (EFI_ABSOLUTE_POINTER_PROTOCOL **) NULL,
+  0,
+  FALSE,
+
+  FALSE,
+  FALSE
+};
+
+
+//
+// Uga Draw Protocol Private Data template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UGA_DRAW_PROTOCOL mUgaDrawProtocolTemplate = {
+  ConSplitterUgaDrawGetMode,
+  ConSplitterUgaDrawSetMode,
+  ConSplitterUgaDrawBlt
+};
+
+//
+// Graphics Output Protocol Private Data template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_GRAPHICS_OUTPUT_PROTOCOL mGraphicsOutputProtocolTemplate = {
+  ConSplitterGraphicsOutputQueryMode,
+  ConSplitterGraphicsOutputSetMode,
+  ConSplitterGraphicsOutputBlt,
+  NULL
+};
+
+
+//
+// Text Out Splitter Private Data template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED TEXT_OUT_SPLITTER_PRIVATE_DATA mConOut = {
+  TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE,
+  (EFI_HANDLE) NULL,
+  {
+    ConSplitterTextOutReset,
+    ConSplitterTextOutOutputString,
+    ConSplitterTextOutTestString,
+    ConSplitterTextOutQueryMode,
+    ConSplitterTextOutSetMode,
+    ConSplitterTextOutSetAttribute,
+    ConSplitterTextOutClearScreen,
+    ConSplitterTextOutSetCursorPosition,
+    ConSplitterTextOutEnableCursor,
+    (EFI_SIMPLE_TEXT_OUTPUT_MODE *) NULL
+  },
+  {
+    1,
+    0,
+    0,
+    0,
+    0,
+    FALSE,
+  },
+
+  {
+    NULL,
+    NULL,
+    NULL
+  },
+  0,
+  0,
+  0,
+  0,
+
+  {
+    NULL,
+    NULL,
+    NULL,
+    NULL
+  },
+  (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *) NULL,
+  0,
+  0,
+
+  0,
+  (TEXT_OUT_AND_GOP_DATA *) NULL,
+  0,
+  (TEXT_OUT_SPLITTER_QUERY_DATA *) NULL,
+  0,
+  (INT32 *) NULL
+};
+
+//
+// Standard Error Text Out Splitter Data Template
+//
+GLOBAL_REMOVE_IF_UNREFERENCED TEXT_OUT_SPLITTER_PRIVATE_DATA mStdErr = {
+  TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE,
+  (EFI_HANDLE) NULL,
+  {
+    ConSplitterTextOutReset,
+    ConSplitterTextOutOutputString,
+    ConSplitterTextOutTestString,
+    ConSplitterTextOutQueryMode,
+    ConSplitterTextOutSetMode,
+    ConSplitterTextOutSetAttribute,
+    ConSplitterTextOutClearScreen,
+    ConSplitterTextOutSetCursorPosition,
+    ConSplitterTextOutEnableCursor,
+    (EFI_SIMPLE_TEXT_OUTPUT_MODE *) NULL
+  },
+  {
+    1,
+    0,
+    0,
+    0,
+    0,
+    FALSE,
+  },
+
+  {
+    NULL,
+    NULL,
+    NULL
+  },
+  0,
+  0,
+  0,
+  0,
+
+  {
+    NULL,
+    NULL,
+    NULL,
+    NULL
+  },
+  (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *) NULL,
+  0,
+  0,
+
+  0,
+  (TEXT_OUT_AND_GOP_DATA *) NULL,
+  0,
+  (TEXT_OUT_SPLITTER_QUERY_DATA *) NULL,
+  0,
+  (INT32 *) NULL
+};
+
+//
+// Driver binding instance for Console Input Device
+//
+EFI_DRIVER_BINDING_PROTOCOL           gConSplitterConInDriverBinding = {
+  ConSplitterConInDriverBindingSupported,
+  ConSplitterConInDriverBindingStart,
+  ConSplitterConInDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+//
+// Driver binding instance for Console Out device
+//
+EFI_DRIVER_BINDING_PROTOCOL           gConSplitterConOutDriverBinding = {
+  ConSplitterConOutDriverBindingSupported,
+  ConSplitterConOutDriverBindingStart,
+  ConSplitterConOutDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+//
+// Driver binding instance for Standard Error device
+//
+EFI_DRIVER_BINDING_PROTOCOL           gConSplitterStdErrDriverBinding = {
+  ConSplitterStdErrDriverBindingSupported,
+  ConSplitterStdErrDriverBindingStart,
+  ConSplitterStdErrDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+//
+// Driver binding instance for Simple Pointer protocol
+//
+EFI_DRIVER_BINDING_PROTOCOL           gConSplitterSimplePointerDriverBinding = {
+  ConSplitterSimplePointerDriverBindingSupported,
+  ConSplitterSimplePointerDriverBindingStart,
+  ConSplitterSimplePointerDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+//
+// Driver binding instance for Absolute Pointer protocol
+//
+EFI_DRIVER_BINDING_PROTOCOL           gConSplitterAbsolutePointerDriverBinding = {
+  ConSplitterAbsolutePointerDriverBindingSupported,
+  ConSplitterAbsolutePointerDriverBindingStart,
+  ConSplitterAbsolutePointerDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  The Entry Point for module ConSplitter. The user code starts with this function.
+
+  Installs driver module protocols and. Creates virtual device handles for ConIn,
+  ConOut, and StdErr. Installs Simple Text In protocol, Simple Text In Ex protocol,
+  Simple Pointer protocol, Absolute Pointer protocol on those virtual handlers.
+  Installs Graphics Output protocol and/or UGA Draw protocol if needed.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterDriverEntry(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConSplitterConInDriverBinding,
+             ImageHandle,
+             &gConSplitterConInComponentName,
+             &gConSplitterConInComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConSplitterSimplePointerDriverBinding,
+             NULL,
+             &gConSplitterSimplePointerComponentName,
+             &gConSplitterSimplePointerComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConSplitterAbsolutePointerDriverBinding,
+             NULL,
+             &gConSplitterAbsolutePointerComponentName,
+             &gConSplitterAbsolutePointerComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConSplitterConOutDriverBinding,
+             NULL,
+             &gConSplitterConOutComponentName,
+             &gConSplitterConOutComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gConSplitterStdErrDriverBinding,
+             NULL,
+             &gConSplitterStdErrComponentName,
+             &gConSplitterStdErrComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Either Graphics Output protocol or UGA Draw protocol must be supported.
+  //
+  ASSERT (FeaturePcdGet (PcdConOutGopSupport) ||
+          FeaturePcdGet (PcdConOutUgaSupport));
+
+  //
+  // The driver creates virtual handles for ConIn, ConOut, StdErr.
+  // The virtual handles will always exist even if no console exist in the
+  // system. This is need to support hotplug devices like USB.
+  //
+  //
+  // Create virtual device handle for ConIn Splitter
+  //
+  Status = ConSplitterTextInConstructor (&mConIn);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mConIn.VirtualHandle,
+                    &gEfiSimpleTextInProtocolGuid,
+                    &mConIn.TextIn,
+                    &gEfiSimpleTextInputExProtocolGuid,
+                    &mConIn.TextInEx,
+                    &gEfiSimplePointerProtocolGuid,
+                    &mConIn.SimplePointer,
+                    &gEfiAbsolutePointerProtocolGuid,
+                    &mConIn.AbsolutePointer,
+                    NULL
+                    );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update the EFI System Table with new virtual console
+      // and update the pointer to Simple Text Input protocol.
+      //
+      gST->ConsoleInHandle  = mConIn.VirtualHandle;
+      gST->ConIn            = &mConIn.TextIn;
+    }
+  }
+  //
+  // Create virtual device handle for ConOut Splitter
+  //
+  Status = ConSplitterTextOutConstructor (&mConOut);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mConOut.VirtualHandle,
+                    &gEfiSimpleTextOutProtocolGuid,
+                    &mConOut.TextOut,
+                    NULL
+                    );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update the EFI System Table with new virtual console
+      // and Update the pointer to Text Output protocol.
+      //
+      gST->ConsoleOutHandle = mConOut.VirtualHandle;
+      gST->ConOut           = &mConOut.TextOut;
+    }
+
+  }
+
+  //
+  // Create virtual device handle for StdErr Splitter
+  //
+  Status = ConSplitterTextOutConstructor (&mStdErr);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mStdErr.VirtualHandle,
+                    &gEfiSimpleTextOutProtocolGuid,
+                    &mStdErr.TextOut,
+                    NULL
+                    );
+    if (!EFI_ERROR (Status)) {  
+      //
+      // Update the EFI System Table with new virtual console
+      // and update the pointer to Text Output protocol.
+      //
+      gST->StandardErrorHandle  = mStdErr.VirtualHandle;
+      gST->StdErr               = &mStdErr.TextOut;
+    }
+  }
+  
+  //
+  // Update the CRC32 in the EFI System Table header
+  //
+  gST->Hdr.CRC32 = 0;
+  gBS->CalculateCrc32 (
+        (UINT8 *) &gST->Hdr,
+        gST->Hdr.HeaderSize,
+        &gST->Hdr.CRC32
+        );
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Construct console input devices' private data.
+
+  @param  ConInPrivate             A pointer to the TEXT_IN_SPLITTER_PRIVATE_DATA
+                                   structure.
+
+  @retval EFI_OUT_OF_RESOURCES     Out of resources.
+  @retval EFI_SUCCESS              Text Input Devcie's private data has been constructed.
+  @retval other                    Failed to construct private data.
+
+**/
+EFI_STATUS
+ConSplitterTextInConstructor (
+  TEXT_IN_SPLITTER_PRIVATE_DATA       *ConInPrivate
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Allocate buffer for Simple Text Input device
+  //
+  Status = ConSplitterGrowBuffer (
+            sizeof (EFI_SIMPLE_TEXT_INPUT_PROTOCOL *),
+            &ConInPrivate->TextInListCount,
+            (VOID **) &ConInPrivate->TextInList
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Create Event to wait for a key
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  ConSplitterTextInWaitForKey,
+                  ConInPrivate,
+                  &ConInPrivate->TextIn.WaitForKey
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Allocate buffer for Simple Text Input Ex device
+  //
+  Status = ConSplitterGrowBuffer (
+             sizeof (EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *),
+             &ConInPrivate->TextInExListCount,
+             (VOID **) &ConInPrivate->TextInExList
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Create Event to wait for a key Ex
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  ConSplitterTextInWaitForKey,
+                  ConInPrivate,
+                  &ConInPrivate->TextInEx.WaitForKeyEx
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  InitializeListHead (&ConInPrivate->NotifyList);
+
+  ConInPrivate->AbsolutePointer.Mode = &ConInPrivate->AbsolutePointerMode;
+  //
+  // Allocate buffer for Absolute Pointer device
+  //
+  Status = ConSplitterGrowBuffer (
+            sizeof (EFI_ABSOLUTE_POINTER_PROTOCOL *),
+            &ConInPrivate->AbsolutePointerListCount,
+            (VOID **) &ConInPrivate->AbsolutePointerList
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Create Event to wait for device input for Absolute pointer device
+  //
+  Status = gBS->CreateEvent (
+            EVT_NOTIFY_WAIT,
+            TPL_NOTIFY,
+            ConSplitterAbsolutePointerWaitForInput,
+            ConInPrivate,
+            &ConInPrivate->AbsolutePointer.WaitForInput
+        );
+  ASSERT_EFI_ERROR (Status);
+
+  ConInPrivate->SimplePointer.Mode = &ConInPrivate->SimplePointerMode;
+  //
+  // Allocate buffer for Simple Pointer device
+  //
+  Status = ConSplitterGrowBuffer (
+            sizeof (EFI_SIMPLE_POINTER_PROTOCOL *),
+            &ConInPrivate->PointerListCount,
+            (VOID **) &ConInPrivate->PointerList
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Create Event to wait for device input for Simple pointer device
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_WAIT,
+                  TPL_NOTIFY,
+                  ConSplitterSimplePointerWaitForInput,
+                  ConInPrivate,
+                  &ConInPrivate->SimplePointer.WaitForInput
+                  );
+  ASSERT_EFI_ERROR (Status);
+  //
+  // Create Event to signal ConIn connection request
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  ConSplitterEmptyCallbackFunction,
+                  NULL,
+                  &gConnectConInEventGuid,
+                  &ConInPrivate->ConnectConInEvent
+                  );
+
+  return Status;
+}
+
+/**
+  Construct console output devices' private data.
+
+  @param  ConOutPrivate            A pointer to the TEXT_OUT_SPLITTER_PRIVATE_DATA
+                                   structure.
+
+  @retval EFI_OUT_OF_RESOURCES     Out of resources.
+  @retval EFI_SUCCESS              Text Input Devcie's private data has been constructed.
+
+**/
+EFI_STATUS
+ConSplitterTextOutConstructor (
+  TEXT_OUT_SPLITTER_PRIVATE_DATA      *ConOutPrivate
+  )
+{
+  EFI_STATUS  Status;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  *Info;
+
+  //
+  // Copy protocols template
+  //
+  if (FeaturePcdGet (PcdConOutUgaSupport)) {
+    CopyMem (&ConOutPrivate->UgaDraw, &mUgaDrawProtocolTemplate, sizeof (EFI_UGA_DRAW_PROTOCOL));
+  }
+  if (FeaturePcdGet (PcdConOutGopSupport)) {
+    CopyMem (&ConOutPrivate->GraphicsOutput, &mGraphicsOutputProtocolTemplate, sizeof (EFI_GRAPHICS_OUTPUT_PROTOCOL));
+  }
+
+  //
+  // Initilize console output splitter's private data.
+  //
+  ConOutPrivate->TextOut.Mode = &ConOutPrivate->TextOutMode;
+
+  //
+  // When new console device is added, the new mode will be set later,
+  // so put current mode back to init state.
+  //
+  ConOutPrivate->TextOutMode.Mode = 0xFF;
+  //
+  // Allocate buffer for Console Out device
+  //
+  Status = ConSplitterGrowBuffer (
+            sizeof (TEXT_OUT_AND_GOP_DATA),
+            &ConOutPrivate->TextOutListCount,
+            (VOID **) &ConOutPrivate->TextOutList
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Allocate buffer for Text Out query data
+  //
+  Status = ConSplitterGrowBuffer (
+            sizeof (TEXT_OUT_SPLITTER_QUERY_DATA),
+            &ConOutPrivate->TextOutQueryDataCount,
+            (VOID **) &ConOutPrivate->TextOutQueryData
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Setup the default console to 80 x 25 and mode to 0
+  //
+  ConOutPrivate->TextOutQueryData[0].Columns  = 80;
+  ConOutPrivate->TextOutQueryData[0].Rows     = 25;
+  TextOutSetMode (ConOutPrivate, 0);
+
+
+  if (FeaturePcdGet (PcdConOutUgaSupport)) {
+    //
+    // Setup the UgaDraw to 800 x 600 x 32 bits per pixel, 60Hz.
+    //
+    ConSplitterUgaDrawSetMode (&ConOutPrivate->UgaDraw, 800, 600, 32, 60);
+  }
+  if (FeaturePcdGet (PcdConOutGopSupport)) {
+    //
+    // Setup resource for mode information in Graphics Output Protocol interface
+    //
+    if ((ConOutPrivate->GraphicsOutput.Mode = AllocateZeroPool (sizeof (EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE))) == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    if ((ConOutPrivate->GraphicsOutput.Mode->Info = AllocateZeroPool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION))) == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Setup the DevNullGraphicsOutput to 800 x 600 x 32 bits per pixel
+    // DevNull will be updated to user-defined mode after driver has started.
+    //
+    if ((ConOutPrivate->GraphicsOutputModeBuffer = AllocateZeroPool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION))) == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    Info = &ConOutPrivate->GraphicsOutputModeBuffer[0];
+    Info->Version = 0;
+    Info->HorizontalResolution = 800;
+    Info->VerticalResolution = 600;
+    Info->PixelFormat = PixelBltOnly;
+    Info->PixelsPerScanLine = 800;
+    CopyMem (ConOutPrivate->GraphicsOutput.Mode->Info, Info, sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+    ConOutPrivate->GraphicsOutput.Mode->SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
+
+    //
+    // Initialize the following items, theset items remain unchanged in GraphicsOutput->SetMode()
+    // GraphicsOutputMode->FrameBufferBase, GraphicsOutputMode->FrameBufferSize
+    //
+    ConOutPrivate->GraphicsOutput.Mode->FrameBufferBase = (EFI_PHYSICAL_ADDRESS) (UINTN) NULL;
+    ConOutPrivate->GraphicsOutput.Mode->FrameBufferSize = 0;
+
+    ConOutPrivate->GraphicsOutput.Mode->MaxMode = 1;
+    //
+    // Initial current mode to unknown state, and then set to mode 0
+    //
+    ConOutPrivate->GraphicsOutput.Mode->Mode = 0xffff;
+    ConOutPrivate->GraphicsOutput.SetMode (&ConOutPrivate->GraphicsOutput, 0);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Test to see if the specified protocol could be supported on the specified device.
+
+  @param  This                Driver Binding protocol pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  Guid                The specified protocol.
+
+  @retval EFI_SUCCESS         The specified protocol is supported on this device.
+  @retval EFI_UNSUPPORTED     The specified protocol attempts to be installed on virtul handle.
+  @retval other               Failed to open specified protocol on this device.
+
+**/
+EFI_STATUS
+ConSplitterSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_GUID                        *Guid
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Instance;
+
+  //
+  // Make sure the Console Splitter does not attempt to attach to itself
+  //
+  if (ControllerHandle == mConIn.VirtualHandle  ||
+      ControllerHandle == mConOut.VirtualHandle ||
+      ControllerHandle == mStdErr.VirtualHandle
+      ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check to see whether the specific protocol could be opened BY_DRIVER
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  Guid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->CloseProtocol (
+        ControllerHandle,
+        Guid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test to see if Console In Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  return ConSplitterSupported (
+          This,
+          ControllerHandle,
+          &gEfiConsoleInDeviceGuid
+          );
+}
+
+/**
+  Test to see if Simple Pointer protocol could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  return ConSplitterSupported (
+          This,
+          ControllerHandle,
+          &gEfiSimplePointerProtocolGuid
+          );
+}
+
+/**
+  Test to see if Absolute Pointer protocol could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  return ConSplitterSupported (
+          This,
+          ControllerHandle,
+          &gEfiAbsolutePointerProtocolGuid
+          );
+}
+
+
+/**
+  Test to see if Console Out Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  return ConSplitterSupported (
+          This,
+          ControllerHandle,
+          &gEfiConsoleOutDeviceGuid
+          );
+}
+
+/**
+  Test to see if Standard Error Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  return ConSplitterSupported (
+          This,
+          ControllerHandle,
+          &gEfiStandardErrorDeviceGuid
+          );
+}
+
+
+/**
+  Start ConSplitter on devcie handle by opening Console Device Guid on device handle
+  and the console virtual handle. And Get the console interface on controller handle.
+
+  @param  This                      Driver Binding protocol instance pointer.
+  @param  ControllerHandle          Handle of device.
+  @param  ConSplitterVirtualHandle  Console virtual Handle.
+  @param  DeviceGuid                The specified Console Device, such as ConInDev,
+                                    ConOutDev.
+  @param  InterfaceGuid             The specified protocol to be opened.
+  @param  Interface                 Protocol interface returned.
+
+  @retval EFI_SUCCESS               This driver supports this device.
+  @retval other                     Failed to open the specified Console Device Guid
+                                    or specified protocol.
+
+**/
+EFI_STATUS
+ConSplitterStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_HANDLE                      ConSplitterVirtualHandle,
+  IN  EFI_GUID                        *DeviceGuid,
+  IN  EFI_GUID                        *InterfaceGuid,
+  OUT VOID                            **Interface
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *Instance;
+
+  //
+  // Check to see whether the ControllerHandle has the DeviceGuid on it.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  DeviceGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the Parent Handle for the child.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  DeviceGuid,
+                  &Instance,
+                  This->DriverBindingHandle,
+                  ConSplitterVirtualHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Err;
+  }
+
+  //
+  // Open InterfaceGuid on the virtul handle.
+  //
+  Status =  gBS->OpenProtocol (
+                ControllerHandle,
+                InterfaceGuid,
+                Interface,
+                This->DriverBindingHandle,
+                ConSplitterVirtualHandle,
+                EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                );
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // close the DeviceGuid on ConSplitter VirtualHandle.
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        DeviceGuid,
+        This->DriverBindingHandle,
+        ConSplitterVirtualHandle
+        );
+
+Err:
+  //
+  // close the DeviceGuid on ControllerHandle.
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        DeviceGuid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+
+  return Status;
+}
+
+
+/**
+  Start Console In Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Console In Consplitter is added to ControllerHandle.
+  @retval other                Console In Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL      *TextIn;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL   *TextInEx;
+
+  //
+  // Start ConSplitter on ControllerHandle, and create the virtual
+  // agrogated console device on first call Start for a SimpleTextIn handle.
+  //
+  Status = ConSplitterStart (
+            This,
+            ControllerHandle,
+            mConIn.VirtualHandle,
+            &gEfiConsoleInDeviceGuid,
+            &gEfiSimpleTextInProtocolGuid,
+            (VOID **) &TextIn
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Add this device into Text In devices list.
+  //
+  Status = ConSplitterTextInAddDevice (&mConIn, TextIn);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  (VOID **) &TextInEx,
+                  This->DriverBindingHandle,
+                  mConIn.VirtualHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // If Simple Text Input Ex protocol exists,
+    // add this device into Text In Ex devices list.
+    //
+    Status = ConSplitterTextInExAddDevice (&mConIn, TextInEx);
+  }
+
+  return Status;
+}
+
+
+/**
+  Start Simple Pointer Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Simple Pointer Consplitter is added to ControllerHandle.
+  @retval other                Simple Pointer Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointer;
+
+  //
+  // Start ConSplitter on ControllerHandle, and create the virtual
+  // agrogated console device on first call Start for a SimplePointer handle.
+  //
+  Status = ConSplitterStart (
+            This,
+            ControllerHandle,
+            mConIn.VirtualHandle,
+            &gEfiSimplePointerProtocolGuid,
+            &gEfiSimplePointerProtocolGuid,
+            (VOID **) &SimplePointer
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Add this devcie into Simple Pointer devices list.
+  //
+  return ConSplitterSimplePointerAddDevice (&mConIn, SimplePointer);
+}
+
+
+/**
+  Start Absolute Pointer Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Absolute Pointer Consplitter is added to ControllerHandle.
+  @retval other                Absolute Pointer Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer;
+
+  //
+  // Start ConSplitter on ControllerHandle, and create the virtual
+  // agrogated console device on first call Start for a AbsolutePointer handle.
+  //
+  Status = ConSplitterStart (
+             This,
+             ControllerHandle,
+             mConIn.VirtualHandle,
+             &gEfiAbsolutePointerProtocolGuid,
+             &gEfiAbsolutePointerProtocolGuid,
+             (VOID **) &AbsolutePointer
+             );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Add this devcie into Absolute Pointer devices list.
+  //
+  return ConSplitterAbsolutePointerAddDevice (&mConIn, AbsolutePointer);
+}
+
+
+/**
+  Start Console Out Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Console Out Consplitter is added to ControllerHandle.
+  @retval other                Console Out Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL      *TextOut;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL         *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL                *UgaDraw;
+  UINTN                                SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Info;
+
+  //
+  // Start ConSplitter on ControllerHandle, and create the virtual
+  // agrogated console device on first call Start for a ConsoleOut handle.
+  //
+  Status = ConSplitterStart (
+            This,
+            ControllerHandle,
+            mConOut.VirtualHandle,
+            &gEfiConsoleOutDeviceGuid,
+            &gEfiSimpleTextOutProtocolGuid,
+            (VOID **) &TextOut
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  GraphicsOutput = NULL;
+  UgaDraw        = NULL;
+  //
+  // Try to Open Graphics Output protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiGraphicsOutputProtocolGuid,
+                  (VOID **) &GraphicsOutput,
+                  This->DriverBindingHandle,
+                  mConOut.VirtualHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status) && FeaturePcdGet (PcdUgaConsumeSupport)) {
+    //
+    // Open UGA DRAW protocol
+    //
+    gBS->OpenProtocol (
+           ControllerHandle,
+           &gEfiUgaDrawProtocolGuid,
+           (VOID **) &UgaDraw,
+           This->DriverBindingHandle,
+           mConOut.VirtualHandle,
+           EFI_OPEN_PROTOCOL_GET_PROTOCOL
+           );
+  }
+
+  //
+  // When new console device is added, the new mode will be set later,
+  // so put current mode back to init state.
+  //
+  mConOut.TextOutMode.Mode = 0xFF;
+
+  //
+  // If both ConOut and StdErr incorporate the same Text Out device,
+  // their MaxMode and QueryData should be the intersection of both.
+  //
+  Status = ConSplitterTextOutAddDevice (&mConOut, TextOut, GraphicsOutput, UgaDraw);
+  ConSplitterTextOutSetAttribute (&mConOut.TextOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+
+  if (FeaturePcdGet (PcdConOutUgaSupport)) {
+    //
+    // Get the UGA mode data of ConOut from the current mode
+    //
+    if (GraphicsOutput != NULL) {
+      Status = GraphicsOutput->QueryMode (GraphicsOutput, GraphicsOutput->Mode->Mode, &SizeOfInfo, &Info);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      ASSERT ( SizeOfInfo <= sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+
+      mConOut.UgaHorizontalResolution = Info->HorizontalResolution;
+      mConOut.UgaVerticalResolution   = Info->VerticalResolution;
+      mConOut.UgaColorDepth           = 32;
+      mConOut.UgaRefreshRate          = 60;
+
+      FreePool (Info);
+
+    } else if (UgaDraw != NULL) {
+      Status = UgaDraw->GetMode (
+                 UgaDraw,
+                 &mConOut.UgaHorizontalResolution,
+                 &mConOut.UgaVerticalResolution,
+                 &mConOut.UgaColorDepth,
+                 &mConOut.UgaRefreshRate
+                 );
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Start Standard Error Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Standard Error Consplitter is added to ControllerHandle.
+  @retval other                Standard Error Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+
+  //
+  // Start ConSplitter on ControllerHandle, and create the virtual
+  // agrogated console device on first call Start for a StandardError handle.
+  //
+  Status = ConSplitterStart (
+            This,
+            ControllerHandle,
+            mStdErr.VirtualHandle,
+            &gEfiStandardErrorDeviceGuid,
+            &gEfiSimpleTextOutProtocolGuid,
+            (VOID **) &TextOut
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // When new console device is added, the new mode will be set later,
+  // so put current mode back to init state.
+  //
+  mStdErr.TextOutMode.Mode = 0xFF;
+
+  //
+  // If both ConOut and StdErr incorporate the same Text Out device,
+  // their MaxMode and QueryData should be the intersection of both.
+  //
+  Status = ConSplitterTextOutAddDevice (&mStdErr, TextOut, NULL, NULL);
+  ConSplitterTextOutSetAttribute (&mStdErr.TextOut, EFI_TEXT_ATTR (EFI_MAGENTA, EFI_BLACK));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return Status;
+}
+
+
+/**
+  Stop ConSplitter on device handle by closing Console Device Guid on device handle
+  and the console virtual handle.
+
+  @param  This                      Protocol instance pointer.
+  @param  ControllerHandle          Handle of device.
+  @param  ConSplitterVirtualHandle  Console virtual Handle.
+  @param  DeviceGuid                The specified Console Device, such as ConInDev,
+                                    ConOutDev.
+  @param  InterfaceGuid             The specified protocol to be opened.
+  @param  Interface                 Protocol interface returned.
+
+  @retval EFI_SUCCESS               Stop ConSplitter on ControllerHandle successfully.
+  @retval other                     Failed to Stop ConSplitter on ControllerHandle.
+
+**/
+EFI_STATUS
+ConSplitterStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_HANDLE                      ConSplitterVirtualHandle,
+  IN  EFI_GUID                        *DeviceGuid,
+  IN  EFI_GUID                        *InterfaceGuid,
+  IN  VOID                            **Interface
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  InterfaceGuid,
+                  Interface,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // close the protocol refered.
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        DeviceGuid,
+        This->DriverBindingHandle,
+        ConSplitterVirtualHandle
+        );
+
+  gBS->CloseProtocol (
+        ControllerHandle,
+        DeviceGuid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Stop Console In ConSplitter on ControllerHandle by closing Console In Devcice GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL    *TextIn;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *TextInEx;
+
+  if (NumberOfChildren == 0) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleTextInputExProtocolGuid,
+                  (VOID **) &TextInEx,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // If Simple Text Input Ex protocol exists,
+    // remove device from Text Input Ex devices list.
+    //
+    Status = ConSplitterTextInExDeleteDevice (&mConIn, TextInEx);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Close Simple Text In protocol on controller handle and virtual handle.
+  //
+  Status = ConSplitterStop (
+            This,
+            ControllerHandle,
+            mConIn.VirtualHandle,
+            &gEfiConsoleInDeviceGuid,
+            &gEfiSimpleTextInProtocolGuid,
+            (VOID **) &TextIn
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Remove device from Text Input devices list.
+  //
+  return ConSplitterTextInDeleteDevice (&mConIn, TextIn);
+}
+
+
+/**
+  Stop Simple Pointer protocol ConSplitter on ControllerHandle by closing
+  Simple Pointer protocol.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_POINTER_PROTOCOL *SimplePointer;
+
+  if (NumberOfChildren == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Close Simple Pointer protocol on controller handle and virtual handle.
+  //
+  Status = ConSplitterStop (
+            This,
+            ControllerHandle,
+            mConIn.VirtualHandle,
+            &gEfiSimplePointerProtocolGuid,
+            &gEfiSimplePointerProtocolGuid,
+            (VOID **) &SimplePointer
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Remove this device from Simple Pointer device list.
+  //
+  return ConSplitterSimplePointerDeleteDevice (&mConIn, SimplePointer);
+}
+
+
+/**
+  Stop Absolute Pointer protocol ConSplitter on ControllerHandle by closing
+  Absolute Pointer protocol.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer;
+
+  if (NumberOfChildren == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Close Absolute Pointer protocol on controller handle and virtual handle.
+  //
+  Status = ConSplitterStop (
+             This,
+             ControllerHandle,
+             mConIn.VirtualHandle,
+             &gEfiAbsolutePointerProtocolGuid,
+             &gEfiAbsolutePointerProtocolGuid,
+             (VOID **) &AbsolutePointer
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Remove this device from Absolute Pointer device list.
+  //
+  return ConSplitterAbsolutePointerDeleteDevice (&mConIn, AbsolutePointer);
+}
+
+
+/**
+  Stop Console Out ConSplitter on device handle by closing Console Out Devcice GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+
+  if (NumberOfChildren == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Close Absolute Pointer protocol on controller handle and virtual handle.
+  //
+  Status = ConSplitterStop (
+            This,
+            ControllerHandle,
+            mConOut.VirtualHandle,
+            &gEfiConsoleOutDeviceGuid,
+            &gEfiSimpleTextOutProtocolGuid,
+            (VOID **) &TextOut
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Remove this device from Text Out device list.
+  //
+  return ConSplitterTextOutDeleteDevice (&mConOut, TextOut);
+}
+
+
+/**
+  Stop Standard Error ConSplitter on ControllerHandle by closing Standard Error GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+
+  if (NumberOfChildren == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Close Standard Error Device on controller handle and virtual handle.
+  //
+  Status = ConSplitterStop (
+            This,
+            ControllerHandle,
+            mStdErr.VirtualHandle,
+            &gEfiStandardErrorDeviceGuid,
+            &gEfiSimpleTextOutProtocolGuid,
+            (VOID **) &TextOut
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Delete this console error out device's data structures.
+  //
+  return ConSplitterTextOutDeleteDevice (&mStdErr, TextOut);
+}
+
+
+/**
+  Take the passed in Buffer of size ElementSize and grow the buffer
+  by CONSOLE_SPLITTER_ALLOC_UNIT * ElementSize bytes.
+  Copy the current data in Buffer to the new version of Buffer and
+  free the old version of buffer.
+
+  @param  ElementSize              Size of element in array.
+  @param  Count                    Current number of elements in array.
+  @param  Buffer                   Bigger version of passed in Buffer with all the
+                                   data.
+
+  @retval EFI_SUCCESS              Buffer size has grown.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterGrowBuffer (
+  IN      UINTN                       ElementSize,
+  IN OUT  UINTN                       *Count,
+  IN OUT  VOID                        **Buffer
+  )
+{
+  VOID  *Ptr;
+
+  //
+  // grow the buffer to new buffer size,
+  // copy the old buffer's content to the new-size buffer,
+  // then free the old buffer.
+  //
+  Ptr = ReallocatePool (
+          ElementSize * (*Count),
+          ElementSize * ((*Count) + CONSOLE_SPLITTER_ALLOC_UNIT),
+          *Buffer
+          );
+  if (Ptr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  *Count += CONSOLE_SPLITTER_ALLOC_UNIT;
+  *Buffer = Ptr;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Add Text Input Device in Consplitter Text Input list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextIn                   Simple Text Input protocol pointer.
+
+  @retval EFI_SUCCESS              Text Input Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextInAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *TextIn
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // If the Text In List is full, enlarge it by calling ConSplitterGrowBuffer().
+  //
+  if (Private->CurrentNumberOfConsoles >= Private->TextInListCount) {
+    Status = ConSplitterGrowBuffer (
+              sizeof (EFI_SIMPLE_TEXT_INPUT_PROTOCOL *),
+              &Private->TextInListCount,
+              (VOID **) &Private->TextInList
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+  //
+  // Add the new text-in device data structure into the Text In List.
+  //
+  Private->TextInList[Private->CurrentNumberOfConsoles] = TextIn;
+  Private->CurrentNumberOfConsoles++;
+
+  //
+  // Extra CheckEvent added to reduce the double CheckEvent().
+  //
+  gBS->CheckEvent (TextIn->WaitForKey);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove Text Input Device from Consplitter Text Input list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextIn                   Simple Text protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Text Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Text Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextInDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *TextIn
+  )
+{
+  UINTN Index;
+  //
+  // Remove the specified text-in device data structure from the Text In List,
+  // and rearrange the remaining data structures in the Text In List.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    if (Private->TextInList[Index] == TextIn) {
+      for (; Index < Private->CurrentNumberOfConsoles - 1; Index++) {
+        Private->TextInList[Index] = Private->TextInList[Index + 1];
+      }
+
+      Private->CurrentNumberOfConsoles--;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Add Text Input Ex Device in Consplitter Text Input Ex list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextInEx                 Simple Text Input Ex Input protocol pointer.
+
+  @retval EFI_SUCCESS              Text Input Ex Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextInExAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA         *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL     *TextInEx
+  )
+{
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *Link;
+  TEXT_IN_EX_SPLITTER_NOTIFY  *CurrentNotify;
+  UINTN                       TextInExListCount;
+
+  //
+  // Enlarge the NotifyHandleList and the TextInExList
+  //
+  if (Private->CurrentNumberOfExConsoles >= Private->TextInExListCount) {
+    for (Link = Private->NotifyList.ForwardLink; Link != &Private->NotifyList; Link = Link->ForwardLink) {
+      CurrentNotify     = TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS (Link);
+      TextInExListCount = Private->TextInExListCount;
+
+      Status = ConSplitterGrowBuffer (
+                 sizeof (EFI_HANDLE),
+                 &TextInExListCount,
+                 (VOID **) &CurrentNotify->NotifyHandleList
+                 );
+      if (EFI_ERROR (Status)) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+    Status = ConSplitterGrowBuffer (
+              sizeof (EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *),
+              &Private->TextInExListCount,
+              (VOID **) &Private->TextInExList
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  //
+  // Register the key notify in the new text-in device
+  //
+  for (Link = Private->NotifyList.ForwardLink; Link != &Private->NotifyList; Link = Link->ForwardLink) {
+    CurrentNotify = TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS (Link);
+    Status = TextInEx->RegisterKeyNotify (
+                         TextInEx,
+                         &CurrentNotify->KeyData,
+                         CurrentNotify->KeyNotificationFn,
+                         &CurrentNotify->NotifyHandleList[Private->CurrentNumberOfExConsoles]
+                         );
+    if (EFI_ERROR (Status)) {
+      for (Link = Link->BackLink; Link != &Private->NotifyList; Link = Link->BackLink) {
+        CurrentNotify = TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS (Link);
+        TextInEx->UnregisterKeyNotify (
+                    TextInEx,
+                    CurrentNotify->NotifyHandleList[Private->CurrentNumberOfExConsoles]
+                    );
+      }
+      return Status;
+    }
+  }
+
+  //
+  // Add the new text-in device data structure into the Text Input Ex List.
+  //
+  Private->TextInExList[Private->CurrentNumberOfExConsoles] = TextInEx;
+  Private->CurrentNumberOfExConsoles++;
+
+  //
+  // Extra CheckEvent added to reduce the double CheckEvent().
+  //
+  gBS->CheckEvent (TextInEx->WaitForKeyEx);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove Text Ex Device from Consplitter Text Input Ex list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextInEx                 Simple Text Ex protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Text Input Ex Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Text Input Ex Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextInExDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA         *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL     *TextInEx
+  )
+{
+  UINTN Index;
+  //
+  // Remove the specified text-in device data structure from the Text Input Ex List,
+  // and rearrange the remaining data structures in the Text In List.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfExConsoles; Index++) {
+    if (Private->TextInExList[Index] == TextInEx) {
+      for (; Index < Private->CurrentNumberOfExConsoles - 1; Index++) {
+        Private->TextInExList[Index] = Private->TextInExList[Index + 1];
+      }
+
+      Private->CurrentNumberOfExConsoles--;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Add Simple Pointer Device in Consplitter Simple Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  SimplePointer            Simple Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Pointer Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterSimplePointerAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *SimplePointer
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // If the Simple Pointer List is full, enlarge it by calling ConSplitterGrowBuffer().
+  //
+  if (Private->CurrentNumberOfPointers >= Private->PointerListCount) {
+    Status = ConSplitterGrowBuffer (
+              sizeof (EFI_SIMPLE_POINTER_PROTOCOL *),
+              &Private->PointerListCount,
+              (VOID **) &Private->PointerList
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+  //
+  // Add the new text-in device data structure into the Simple Pointer List.
+  //
+  Private->PointerList[Private->CurrentNumberOfPointers] = SimplePointer;
+  Private->CurrentNumberOfPointers++;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove Simple Pointer Device from Consplitter Simple Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  SimplePointer            Simple Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Pointer Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Pointer Device found.
+
+**/
+EFI_STATUS
+ConSplitterSimplePointerDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *SimplePointer
+  )
+{
+  UINTN Index;
+  //
+  // Remove the specified text-in device data structure from the Simple Pointer List,
+  // and rearrange the remaining data structures in the Text In List.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfPointers; Index++) {
+    if (Private->PointerList[Index] == SimplePointer) {
+      for (; Index < Private->CurrentNumberOfPointers - 1; Index++) {
+        Private->PointerList[Index] = Private->PointerList[Index + 1];
+      }
+
+      Private->CurrentNumberOfPointers--;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Add Absolute Pointer Device in Consplitter Absolute Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  AbsolutePointer          Absolute Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Absolute Pointer Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterAbsolutePointerAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // If the Absolute Pointer List is full, enlarge it by calling ConSplitterGrowBuffer().
+  //
+  if (Private->CurrentNumberOfAbsolutePointers >= Private->AbsolutePointerListCount) {
+    Status = ConSplitterGrowBuffer (
+              sizeof (EFI_ABSOLUTE_POINTER_PROTOCOL *),
+              &Private->AbsolutePointerListCount,
+              (VOID **) &Private->AbsolutePointerList
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+  //
+  // Add the new text-in device data structure into the Absolute Pointer List.
+  //
+  Private->AbsolutePointerList[Private->CurrentNumberOfAbsolutePointers] = AbsolutePointer;
+  Private->CurrentNumberOfAbsolutePointers++;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove Absolute Pointer Device from Consplitter Absolute Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  AbsolutePointer          Absolute Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Absolute Pointer Device removed successfully.
+  @retval EFI_NOT_FOUND            No Absolute Pointer Device found.
+
+**/
+EFI_STATUS
+ConSplitterAbsolutePointerDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer
+  )
+{
+  UINTN Index;
+  //
+  // Remove the specified text-in device data structure from the Absolute Pointer List,
+  // and rearrange the remaining data structures from the Absolute Pointer List.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfAbsolutePointers; Index++) {
+    if (Private->AbsolutePointerList[Index] == AbsolutePointer) {
+      for (; Index < Private->CurrentNumberOfAbsolutePointers - 1; Index++) {
+        Private->AbsolutePointerList[Index] = Private->AbsolutePointerList[Index + 1];
+      }
+
+      Private->CurrentNumberOfAbsolutePointers--;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Reallocate Text Out mode map.
+
+  Allocate new buffer and copy original buffer into the new buffer.
+
+  @param  Private                  Consplitter Text Out pointer.
+
+  @retval EFI_SUCCESS              Buffer size has grown
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterGrowMapTable (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private
+  )
+{
+  UINTN Size;
+  UINTN NewSize;
+  UINTN TotalSize;
+  INT32 *TextOutModeMap;
+  INT32 *OldTextOutModeMap;
+  INT32 *SrcAddress;
+  INT32 Index;
+  UINTN OldStepSize;
+  UINTN NewStepSize;
+
+  NewSize           = Private->TextOutListCount * sizeof (INT32);
+  OldTextOutModeMap = Private->TextOutModeMap;
+  TotalSize         = NewSize * (Private->TextOutQueryDataCount);
+
+  //
+  // Allocate new buffer for Text Out List.
+  //
+  TextOutModeMap    = AllocatePool (TotalSize);
+  if (TextOutModeMap == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SetMem (TextOutModeMap, TotalSize, 0xFF);
+  Private->TextOutModeMap = TextOutModeMap;
+
+  //
+  // If TextOutList has been enlarged, need to realloc the mode map table
+  // The mode map table is regarded as a two dimension array.
+  //
+  //                         Old                    New
+  //  0   ---------> TextOutListCount ----> TextOutListCount
+  //  |   -------------------------------------------
+  //  |  |                    |                      |
+  //  |  |                    |                      |
+  //  |  |                    |                      |
+  //  |  |                    |                      |
+  //  |  |                    |                      |
+  // \/  |                    |                      |
+  //      -------------------------------------------
+  // QueryDataCount
+  //
+  if (OldTextOutModeMap != NULL) {
+
+    Size        = Private->CurrentNumberOfConsoles * sizeof (INT32);
+    Index       = 0;
+    SrcAddress  = OldTextOutModeMap;
+    NewStepSize = NewSize / sizeof(INT32);    
+    // If Private->CurrentNumberOfConsoles is not zero and OldTextOutModeMap
+    // is not NULL, it indicates that the original TextOutModeMap is not enough
+    // for the new console devices and has been enlarged by CONSOLE_SPLITTER_ALLOC_UNIT columns.
+    //
+    OldStepSize = NewStepSize - CONSOLE_SPLITTER_ALLOC_UNIT;
+
+    //
+    // Copy the old data to the new one
+    //
+    while (Index < Private->TextOutMode.MaxMode) {
+      CopyMem (TextOutModeMap, SrcAddress, Size);
+      //
+      // Go to next row of new TextOutModeMap.
+      //
+      TextOutModeMap += NewStepSize;
+      //
+      // Go to next row of old TextOutModeMap.
+      //
+      SrcAddress += OldStepSize;
+      Index++;
+    }
+    //
+    // Free the old buffer
+    //
+    FreePool (OldTextOutModeMap);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Add new device's output mode to console splitter's mode list.
+
+  @param  Private               Text Out Splitter pointer
+  @param  TextOut               Simple Text Output protocol pointer.
+
+  @retval EFI_SUCCESS           Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES  Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterAddOutputMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut
+  )
+{
+  EFI_STATUS  Status;
+  INT32       MaxMode;
+  INT32       Mode;
+  UINTN       Index;
+
+  MaxMode                       = TextOut->Mode->MaxMode;
+  Private->TextOutMode.MaxMode  = MaxMode;
+
+  //
+  // Grow the buffer if query data buffer is not large enough to
+  // hold all the mode supported by the first console.
+  //
+  while (MaxMode > (INT32) Private->TextOutQueryDataCount) {
+    Status = ConSplitterGrowBuffer (
+              sizeof (TEXT_OUT_SPLITTER_QUERY_DATA),
+              &Private->TextOutQueryDataCount,
+              (VOID **) &Private->TextOutQueryData
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+  //
+  // Allocate buffer for the output mode map
+  //
+  Status = ConSplitterGrowMapTable (Private);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // As the first textout device, directly add the mode in to QueryData
+  // and at the same time record the mapping between QueryData and TextOut.
+  //
+  Mode  = 0;
+  Index = 0;
+  while (Mode < MaxMode) {
+    Status = TextOut->QueryMode (
+                  TextOut,
+                  Mode,
+                  &Private->TextOutQueryData[Mode].Columns,
+                  &Private->TextOutQueryData[Mode].Rows
+                  );
+    //
+    // If mode 1 (80x50) is not supported, make sure mode 1 in TextOutQueryData
+    // is clear to 0x0.
+    //
+    if ((EFI_ERROR(Status)) && (Mode == 1)) {
+      Private->TextOutQueryData[Mode].Columns = 0;
+      Private->TextOutQueryData[Mode].Rows = 0;
+    }
+    Private->TextOutModeMap[Index] = Mode;
+    Mode++;
+    Index += Private->TextOutListCount;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reconstruct TextOutModeMap to get intersection of modes.
+
+  This routine reconstruct TextOutModeMap to get the intersection
+  of modes for all console out devices. Because EFI/UEFI spec require
+  mode 0 is 80x25, mode 1 is 80x50, this routine will not check the
+  intersection for mode 0 and mode 1.
+
+  @param TextOutModeMap  Current text out mode map, begin with the mode 80x25
+  @param NewlyAddedMap   New text out mode map, begin with the mode 80x25
+  @param MapStepSize     Mode step size for one console device
+  @param NewMapStepSize  New Mode step size for one console device
+  @param MaxMode         IN: Current max text mode, OUT: Updated max text mode.
+  @param CurrentMode     IN: Current text mode,     OUT: Updated current text mode.
+
+**/
+VOID
+ConSplitterGetIntersection (
+  IN     INT32                        *TextOutModeMap,
+  IN     INT32                        *NewlyAddedMap,
+  IN     UINTN                        MapStepSize,
+  IN     UINTN                        NewMapStepSize,
+  IN OUT INT32                        *MaxMode,
+  IN OUT INT32                        *CurrentMode
+  )
+{
+  INT32 Index;
+  INT32 *CurrentMapEntry;
+  INT32 *NextMapEntry;
+  INT32 *NewMapEntry;
+  INT32 CurrentMaxMode;
+  INT32 Mode;
+
+  //
+  // According to EFI/UEFI spec, mode 0 and mode 1 have been reserved
+  // for 80x25 and 80x50 in Simple Text Out protocol, so don't make intersection
+  // for mode 0 and mode 1, mode number starts from 2.
+  //
+  Index           = 2;
+  CurrentMapEntry = &TextOutModeMap[MapStepSize * 2];
+  NextMapEntry    = CurrentMapEntry;
+  NewMapEntry     = &NewlyAddedMap[NewMapStepSize * 2];
+
+  CurrentMaxMode  = *MaxMode;
+  Mode            = *CurrentMode;
+
+  while (Index < CurrentMaxMode) {
+    if (*NewMapEntry == -1) {
+      //
+      // This mode is not supported any more. Remove it. Special care
+      // must be taken as this remove will also affect current mode;
+      //
+      if (Index == *CurrentMode) {
+        Mode = -1;
+      } else if (Index < *CurrentMode) {
+        Mode--;
+      }
+      (*MaxMode)--;
+    } else {
+      if (CurrentMapEntry != NextMapEntry) {
+        CopyMem (NextMapEntry, CurrentMapEntry, MapStepSize * sizeof (INT32));
+      }
+
+      NextMapEntry += MapStepSize;
+    }
+
+    CurrentMapEntry += MapStepSize;
+    NewMapEntry     += NewMapStepSize;
+    Index++;
+  }
+
+  *CurrentMode = Mode;
+
+  return ;
+}
+
+/**
+  Sync the device's output mode to console splitter's mode list.
+
+  @param  Private               Text Out Splitter pointer.
+  @param  TextOut               Simple Text Output protocol pointer.
+
+**/
+VOID
+ConSplitterSyncOutputMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut
+  )
+{
+  INT32                         CurrentMaxMode;
+  INT32                         Mode;
+  INT32                         Index;
+  INT32                         *TextOutModeMap;
+  INT32                         *MapTable;
+  INT32                         QueryMode;
+  TEXT_OUT_SPLITTER_QUERY_DATA  *TextOutQueryData;
+  UINTN                         Rows;
+  UINTN                         Columns;
+  UINTN                         StepSize;
+  EFI_STATUS                    Status;
+
+  //
+  // Must make sure that current mode won't change even if mode number changes
+  //
+  CurrentMaxMode    = Private->TextOutMode.MaxMode;
+  TextOutModeMap    = Private->TextOutModeMap;
+  StepSize          = Private->TextOutListCount;
+  TextOutQueryData  = Private->TextOutQueryData;
+
+  //
+  // Query all the mode that the newly added TextOut supports
+  //
+  Mode      = 0;
+  MapTable  = TextOutModeMap + Private->CurrentNumberOfConsoles;
+  while (Mode < TextOut->Mode->MaxMode) {
+    Status = TextOut->QueryMode (TextOut, Mode, &Columns, &Rows);
+
+    if (EFI_ERROR(Status)) {
+      if (Mode == 1) {
+        //
+        // If mode 1 (80x50) is not supported, make sure mode 1 in TextOutQueryData
+        // is clear to 0x0.
+        //
+        MapTable[StepSize] = Mode;
+        TextOutQueryData[Mode].Columns = 0;
+        TextOutQueryData[Mode].Rows = 0;
+      }
+      Mode++;
+      continue;
+    }
+    //
+    // Search the intersection map and QueryData database to see if they intersects
+    //
+    Index = 0;
+    while (Index < CurrentMaxMode) {
+      QueryMode = *(TextOutModeMap + Index * StepSize);
+      if ((TextOutQueryData[QueryMode].Rows == Rows) && (TextOutQueryData[QueryMode].Columns == Columns)) {
+        MapTable[Index * StepSize] = Mode;
+        break;
+      }
+      Index++;
+    }
+    Mode++;
+  }
+  //
+  // Now search the TextOutModeMap table to find the intersection of supported
+  // mode between ConSplitter and the newly added device.
+  //
+  ConSplitterGetIntersection (
+    TextOutModeMap,
+    MapTable,
+    StepSize,
+    StepSize,
+    &Private->TextOutMode.MaxMode,
+    &Private->TextOutMode.Mode
+    );
+
+  return ;
+}
+
+
+/**
+  Sync output device between ConOut and StdErr output.
+
+  @retval EFI_SUCCESS              Sync implemented successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterGetIntersectionBetweenConOutAndStrErr (
+  VOID
+  )
+{
+  UINTN                         ConOutNumOfConsoles;
+  UINTN                         StdErrNumOfConsoles;
+  TEXT_OUT_AND_GOP_DATA         *ConOutTextOutList;
+  TEXT_OUT_AND_GOP_DATA         *StdErrTextOutList;
+  UINTN                         Indexi;
+  UINTN                         Indexj;
+  UINTN                         ConOutRows;
+  UINTN                         ConOutColumns;
+  UINTN                         StdErrRows;
+  UINTN                         StdErrColumns;
+  INT32                         ConOutMaxMode;
+  INT32                         StdErrMaxMode;
+  INT32                         ConOutMode;
+  INT32                         StdErrMode;
+  INT32                         Mode;
+  INT32                         Index;
+  INT32                         *ConOutModeMap;
+  INT32                         *StdErrModeMap;
+  INT32                         *ConOutMapTable;
+  INT32                         *StdErrMapTable;
+  TEXT_OUT_SPLITTER_QUERY_DATA  *ConOutQueryData;
+  TEXT_OUT_SPLITTER_QUERY_DATA  *StdErrQueryData;
+  UINTN                         ConOutStepSize;
+  UINTN                         StdErrStepSize;
+  BOOLEAN                       FoundTheSameTextOut;
+  UINTN                         ConOutMapTableSize;
+  UINTN                         StdErrMapTableSize;
+
+  ConOutNumOfConsoles = mConOut.CurrentNumberOfConsoles;
+  StdErrNumOfConsoles = mStdErr.CurrentNumberOfConsoles;
+  ConOutTextOutList   = mConOut.TextOutList;
+  StdErrTextOutList   = mStdErr.TextOutList;
+
+  Indexi              = 0;
+  FoundTheSameTextOut = FALSE;
+  while ((Indexi < ConOutNumOfConsoles) && (!FoundTheSameTextOut)) {
+    Indexj = 0;
+    while (Indexj < StdErrNumOfConsoles) {
+      if (ConOutTextOutList->TextOut == StdErrTextOutList->TextOut) {
+        FoundTheSameTextOut = TRUE;
+        break;
+      }
+
+      Indexj++;
+      StdErrTextOutList++;
+    }
+
+    Indexi++;
+    ConOutTextOutList++;
+  }
+
+  if (!FoundTheSameTextOut) {
+    return EFI_SUCCESS;
+  }
+  //
+  // Must make sure that current mode won't change even if mode number changes
+  //
+  ConOutMaxMode     = mConOut.TextOutMode.MaxMode;
+  ConOutModeMap     = mConOut.TextOutModeMap;
+  ConOutStepSize    = mConOut.TextOutListCount;
+  ConOutQueryData   = mConOut.TextOutQueryData;
+
+  StdErrMaxMode     = mStdErr.TextOutMode.MaxMode;
+  StdErrModeMap     = mStdErr.TextOutModeMap;
+  StdErrStepSize    = mStdErr.TextOutListCount;
+  StdErrQueryData   = mStdErr.TextOutQueryData;
+
+  //
+  // Allocate the map table and set the map table's index to -1.
+  //
+  ConOutMapTableSize  = ConOutMaxMode * sizeof (INT32);
+  ConOutMapTable      = AllocateZeroPool (ConOutMapTableSize);
+  if (ConOutMapTable == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SetMem (ConOutMapTable, ConOutMapTableSize, 0xFF);
+
+  StdErrMapTableSize  = StdErrMaxMode * sizeof (INT32);
+  StdErrMapTable      = AllocateZeroPool (StdErrMapTableSize);
+  if (StdErrMapTable == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SetMem (StdErrMapTable, StdErrMapTableSize, 0xFF);
+
+  //
+  // Find the intersection of the two set of modes. If they actually intersect, the
+  // correponding entry in the map table is set to 1.
+  //
+  Mode = 0;
+  while (Mode < ConOutMaxMode) {
+    //
+    // Search the intersection map and QueryData database to see if they intersect
+    //
+    Index = 0;
+    ConOutMode    = *(ConOutModeMap + Mode * ConOutStepSize);
+    ConOutRows    = ConOutQueryData[ConOutMode].Rows;
+    ConOutColumns = ConOutQueryData[ConOutMode].Columns;
+    while (Index < StdErrMaxMode) {
+      StdErrMode    = *(StdErrModeMap + Index * StdErrStepSize);
+      StdErrRows    = StdErrQueryData[StdErrMode].Rows;
+      StdErrColumns = StdErrQueryData[StdErrMode].Columns;
+      if ((StdErrRows == ConOutRows) && (StdErrColumns == ConOutColumns)) {
+        ConOutMapTable[Mode]  = 1;
+        StdErrMapTable[Index] = 1;
+        break;
+      }
+
+      Index++;
+    }
+
+    Mode++;
+  }
+  //
+  // Now search the TextOutModeMap table to find the intersection of supported
+  // mode between ConSplitter and the newly added device.
+  //
+  ConSplitterGetIntersection (
+    ConOutModeMap,
+    ConOutMapTable,
+    mConOut.TextOutListCount,
+    1,
+    &(mConOut.TextOutMode.MaxMode),
+    &(mConOut.TextOutMode.Mode)
+    );
+
+  if (mConOut.TextOutMode.Mode < 0) {
+    mConOut.TextOut.SetMode (&(mConOut.TextOut), 0);
+  }
+
+  ConSplitterGetIntersection (
+    StdErrModeMap,
+    StdErrMapTable,
+    mStdErr.TextOutListCount,
+    1,
+    &(mStdErr.TextOutMode.MaxMode),
+    &(mStdErr.TextOutMode.Mode)
+    );
+
+  if (mStdErr.TextOutMode.Mode < 0) {
+    mStdErr.TextOut.SetMode (&(mStdErr.TextOut), 0);
+  }
+
+  FreePool (ConOutMapTable);
+  FreePool (StdErrMapTable);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Add Grahpics Output modes into Consplitter Text Out list.
+
+  @param  Private               Text Out Splitter pointer.
+  @param  GraphicsOutput        Graphics Output protocol pointer.
+  @param  UgaDraw               UGA Draw protocol pointer.
+
+  @retval EFI_SUCCESS           Output mode added successfully.
+  @retval other                 Failed to add output mode.
+
+**/
+EFI_STATUS
+ConSplitterAddGraphicsOutputMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private,
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput,
+  IN  EFI_UGA_DRAW_PROTOCOL           *UgaDraw
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                Index;
+  UINTN                                CurrentIndex;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Mode;
+  UINTN                                SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Info;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE    *CurrentGraphicsOutputMode;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *ModeBuffer;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *MatchedMode;
+  UINTN                                NumberIndex;
+  BOOLEAN                              Match;
+  BOOLEAN                              AlreadyExist;
+  UINT32                               UgaHorizontalResolution;
+  UINT32                               UgaVerticalResolution;
+  UINT32                               UgaColorDepth;
+  UINT32                               UgaRefreshRate;
+
+  ASSERT (GraphicsOutput != NULL || UgaDraw != NULL);
+
+  CurrentGraphicsOutputMode = Private->GraphicsOutput.Mode;
+
+  Index        = 0;
+  CurrentIndex = 0;
+  Status       = EFI_SUCCESS;
+
+  if (Private->CurrentNumberOfUgaDraw != 0) {
+    //
+    // If any UGA device has already been added, then there is no need to
+    // calculate intersection of display mode of different GOP/UGA device,
+    // since only one display mode will be exported (i.e. user-defined mode)
+    //
+    goto Done;
+  }
+
+  if (GraphicsOutput != NULL) {
+    if (Private->CurrentNumberOfGraphicsOutput == 0) {
+        //
+        // This is the first Graphics Output device added
+        //
+        CurrentGraphicsOutputMode->MaxMode = GraphicsOutput->Mode->MaxMode;
+        CurrentGraphicsOutputMode->Mode = GraphicsOutput->Mode->Mode;
+        CopyMem (CurrentGraphicsOutputMode->Info, GraphicsOutput->Mode->Info, GraphicsOutput->Mode->SizeOfInfo);
+        CurrentGraphicsOutputMode->SizeOfInfo = GraphicsOutput->Mode->SizeOfInfo;
+        CurrentGraphicsOutputMode->FrameBufferBase = GraphicsOutput->Mode->FrameBufferBase;
+        CurrentGraphicsOutputMode->FrameBufferSize = GraphicsOutput->Mode->FrameBufferSize;
+
+        //
+        // Allocate resource for the private mode buffer
+        //
+        ModeBuffer = AllocatePool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION) * GraphicsOutput->Mode->MaxMode);
+        if (ModeBuffer == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+        FreePool (Private->GraphicsOutputModeBuffer);
+        Private->GraphicsOutputModeBuffer = ModeBuffer;
+
+        //
+        // Store all supported display modes to the private mode buffer
+        //
+        Mode = ModeBuffer;
+        for (Index = 0; Index < GraphicsOutput->Mode->MaxMode; Index++) {
+          //
+          // The Info buffer would be allocated by callee
+          //
+          Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) Index, &SizeOfInfo, &Info);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+          ASSERT ( SizeOfInfo <= sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+          CopyMem (Mode, Info, SizeOfInfo);
+          Mode++;
+          FreePool (Info);
+        }
+    } else {
+      //
+      // Check intersection of display mode
+      //
+      ModeBuffer = AllocatePool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION) * CurrentGraphicsOutputMode->MaxMode);
+      if (ModeBuffer == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      MatchedMode = ModeBuffer;
+      Mode = &Private->GraphicsOutputModeBuffer[0];
+      for (Index = 0; Index < CurrentGraphicsOutputMode->MaxMode; Index++) {
+        Match = FALSE;
+
+        for (NumberIndex = 0; NumberIndex < GraphicsOutput->Mode->MaxMode; NumberIndex++) {
+          //
+          // The Info buffer would be allocated by callee
+          //
+          Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) NumberIndex, &SizeOfInfo, &Info);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+          if ((Info->HorizontalResolution == Mode->HorizontalResolution) &&
+              (Info->VerticalResolution == Mode->VerticalResolution)) {
+            //
+            // If GOP device supports one mode in current mode buffer,
+            // it will be added into matched mode buffer
+            //
+            Match = TRUE;
+            FreePool (Info);
+            break;
+          }
+          FreePool (Info);
+        }
+
+        if (Match) {
+          AlreadyExist = FALSE;
+
+          //
+          // Check if GOP mode has been in the mode buffer, ModeBuffer = MatchedMode at begin.
+          //
+          for (Info = ModeBuffer; Info < MatchedMode; Info++) {
+            if ((Info->HorizontalResolution == Mode->HorizontalResolution) &&
+                (Info->VerticalResolution == Mode->VerticalResolution)) {
+              AlreadyExist = TRUE;
+              break;
+            }
+          }
+
+          if (!AlreadyExist) {
+            CopyMem (MatchedMode, Mode, sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+
+            //
+            // Physical frame buffer is no longer available, change PixelFormat to PixelBltOnly
+            //
+            MatchedMode->Version = 0;
+            MatchedMode->PixelFormat = PixelBltOnly;
+            ZeroMem (&MatchedMode->PixelInformation, sizeof (EFI_PIXEL_BITMASK));
+
+            MatchedMode++;
+          }
+        }
+
+        Mode++;
+      }
+
+      //
+      // Drop the old mode buffer, assign it to a new one
+      //
+      FreePool (Private->GraphicsOutputModeBuffer);
+      Private->GraphicsOutputModeBuffer = ModeBuffer;
+
+      //
+      // Physical frame buffer is no longer available when there are more than one physical GOP devices
+      //
+      CurrentGraphicsOutputMode->MaxMode = (UINT32) (((UINTN) MatchedMode - (UINTN) ModeBuffer) / sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+      CurrentGraphicsOutputMode->Info->PixelFormat = PixelBltOnly;
+      ZeroMem (&CurrentGraphicsOutputMode->Info->PixelInformation, sizeof (EFI_PIXEL_BITMASK));
+      CurrentGraphicsOutputMode->SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
+      CurrentGraphicsOutputMode->FrameBufferBase = (EFI_PHYSICAL_ADDRESS) (UINTN) NULL;
+      CurrentGraphicsOutputMode->FrameBufferSize = 0;
+    }
+
+    //
+    // Graphics console driver can ensure the same mode for all GOP devices
+    //
+    for (Index = 0; Index < CurrentGraphicsOutputMode->MaxMode; Index++) {
+      Mode = &Private->GraphicsOutputModeBuffer[Index];
+      if ((Mode->HorizontalResolution == GraphicsOutput->Mode->Info->HorizontalResolution) &&
+         (Mode->VerticalResolution == GraphicsOutput->Mode->Info->VerticalResolution)) {
+        CurrentIndex = Index;
+        break;
+      }
+    }
+    if (Index >= CurrentGraphicsOutputMode->MaxMode) {
+      //
+      // if user defined mode is not found, set to default mode 800x600
+      //
+      for (Index = 0; Index < CurrentGraphicsOutputMode->MaxMode; Index++) {
+        Mode = &Private->GraphicsOutputModeBuffer[Index];
+        if ((Mode->HorizontalResolution == 800) && (Mode->VerticalResolution == 600)) {
+          CurrentIndex = Index;
+          break;
+        }
+      }
+    }
+  } else if (UgaDraw != NULL) {
+    //
+    // Graphics console driver can ensure the same mode for all GOP devices
+    // so we can get the current mode from this video device
+    //
+    UgaDraw->GetMode (
+               UgaDraw,
+               &UgaHorizontalResolution,
+               &UgaVerticalResolution,
+               &UgaColorDepth,
+               &UgaRefreshRate
+               );
+
+    CurrentGraphicsOutputMode->MaxMode = 1;
+    Info = CurrentGraphicsOutputMode->Info;
+    Info->Version = 0;
+    Info->HorizontalResolution                 = UgaHorizontalResolution;
+    Info->VerticalResolution                   = UgaVerticalResolution;
+    Info->PixelFormat                          = PixelBltOnly;
+    Info->PixelsPerScanLine                    = UgaHorizontalResolution;
+    CurrentGraphicsOutputMode->SizeOfInfo      = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
+    CurrentGraphicsOutputMode->FrameBufferBase = (EFI_PHYSICAL_ADDRESS) (UINTN) NULL;
+    CurrentGraphicsOutputMode->FrameBufferSize = 0;
+
+    //
+    // Update the private mode buffer
+    //
+    CopyMem (&Private->GraphicsOutputModeBuffer[0], Info, sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+
+    //
+    // Only mode 0 is available to be set
+    //
+    CurrentIndex = 0;
+  }
+
+Done:
+
+  if (GraphicsOutput != NULL) {
+    Private->CurrentNumberOfGraphicsOutput++;
+  }
+  if (UgaDraw != NULL) {
+    Private->CurrentNumberOfUgaDraw++;
+  }
+
+  //
+  // Force GraphicsOutput mode to be set,
+  //
+  
+  Mode = &Private->GraphicsOutputModeBuffer[CurrentIndex];
+  if ((GraphicsOutput != NULL) &&
+      (Mode->HorizontalResolution == CurrentGraphicsOutputMode->Info->HorizontalResolution) &&
+      (Mode->VerticalResolution == CurrentGraphicsOutputMode->Info->VerticalResolution)) {
+    CurrentGraphicsOutputMode->Mode = (UINT32) CurrentIndex;
+    if ((Mode->HorizontalResolution != GraphicsOutput->Mode->Info->HorizontalResolution) ||
+        (Mode->VerticalResolution != GraphicsOutput->Mode->Info->VerticalResolution)) {
+      //
+      // If all existing video device has been set to common mode, only set new GOP device to
+      // the common mode
+      //
+      for (NumberIndex = 0; NumberIndex < GraphicsOutput->Mode->MaxMode; NumberIndex ++) {
+        Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) NumberIndex, &SizeOfInfo, &Info);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((Info->HorizontalResolution == Mode->HorizontalResolution) && (Info->VerticalResolution == Mode->VerticalResolution)) {
+          FreePool (Info);
+          break;
+        }
+        FreePool (Info);
+      }
+      Status = GraphicsOutput->SetMode (GraphicsOutput, (UINT32) NumberIndex);
+    }
+  } else {
+    //
+    // Current mode number may need update now, so set it to an invalid mode number
+    //
+    CurrentGraphicsOutputMode->Mode = 0xffff;
+    //
+    // Graphics console can ensure all GOP devices have the same mode which can be taken as current mode.
+    //
+    Status = Private->GraphicsOutput.SetMode (&Private->GraphicsOutput, (UINT32) CurrentIndex);
+    if (EFI_ERROR(Status)) {
+      //
+      // If user defined mode is not valid for display device, set to the default mode 800x600.
+      //
+      (Private->GraphicsOutputModeBuffer[0]).HorizontalResolution = 800;
+      (Private->GraphicsOutputModeBuffer[0]).VerticalResolution   = 600;
+      Status = Private->GraphicsOutput.SetMode (&Private->GraphicsOutput, 0);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Set the current console out mode.
+
+  This routine will get the current console mode information (column, row)
+  from ConsoleOutMode variable and set it; if the variable does not exist,
+  set to user defined console mode.
+
+  @param  Private            Consplitter Text Out pointer.
+
+**/
+VOID
+ConsplitterSetConsoleOutMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private
+  )
+{
+  UINTN                            Col;
+  UINTN                            Row;
+  UINTN                            Mode;
+  UINTN                            PreferMode;
+  UINTN                            BaseMode;
+  UINTN                            MaxMode;
+  EFI_STATUS                       Status;
+  CONSOLE_OUT_MODE                 ModeInfo;
+  CONSOLE_OUT_MODE                 MaxModeInfo;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+
+  PreferMode   = 0xFF;
+  BaseMode     = 0xFF;
+  TextOut      = &Private->TextOut;
+  MaxMode      = (UINTN) (TextOut->Mode->MaxMode);
+
+  MaxModeInfo.Column = 0;
+  MaxModeInfo.Row    = 0; 
+  ModeInfo.Column    = PcdGet32 (PcdConOutColumn);
+  ModeInfo.Row       = PcdGet32 (PcdConOutRow);
+
+  //
+  // To find the prefer mode and basic mode from Text Out mode list
+  //
+  for (Mode = 0; Mode < MaxMode; Mode++) {
+    Status = TextOut->QueryMode (TextOut, Mode, &Col, &Row);
+    if (!EFI_ERROR(Status)) {
+      if ((ModeInfo.Column != 0) && (ModeInfo.Row != 0)) {
+        //
+        // Use user defined column and row
+        //
+        if (Col == ModeInfo.Column && Row == ModeInfo.Row) {
+          PreferMode = Mode;
+        }
+      } else {
+        //
+        // If user sets PcdConOutColumn or PcdConOutRow to 0,
+        // find and set the highest text mode.
+        //
+        if ((Col >= MaxModeInfo.Column) && (Row >= MaxModeInfo.Row)) {
+          MaxModeInfo.Column  = Col;
+          MaxModeInfo.Row     = Row;
+          PreferMode          = Mode;
+        }
+      }
+      if (Col == 80 && Row == 25) {
+        BaseMode = Mode;
+      }
+    }
+  }
+
+  //
+  // Set prefer mode to Text Out devices.
+  //
+  Status = TextOut->SetMode (TextOut, PreferMode);
+  if (EFI_ERROR(Status)) {
+    //
+    // if current mode setting is failed, default 80x25 mode will be set.
+    //
+    Status = TextOut->SetMode (TextOut, BaseMode);
+    ASSERT(!EFI_ERROR(Status));
+
+    Status = PcdSet32S (PcdConOutColumn, 80);
+    ASSERT(!EFI_ERROR(Status));
+    Status = PcdSet32S (PcdConOutRow, 25);
+    ASSERT(!EFI_ERROR(Status));
+  }
+
+  return ;
+}
+
+
+/**
+  Add Text Output Device in Consplitter Text Output list.
+
+  @param  Private                  Text Out Splitter pointer.
+  @param  TextOut                  Simple Text Output protocol pointer.
+  @param  GraphicsOutput           Graphics Output protocol pointer.
+  @param  UgaDraw                  UGA Draw protocol pointer.
+
+  @retval EFI_SUCCESS              Text Output Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextOutAddDevice (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut,
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL       *GraphicsOutput,
+  IN  EFI_UGA_DRAW_PROTOCOL              *UgaDraw
+  )
+{
+  EFI_STATUS                           Status;
+  UINTN                                CurrentNumOfConsoles;
+  INT32                                MaxMode;
+  UINT32                               UgaHorizontalResolution;
+  UINT32                               UgaVerticalResolution;
+  UINT32                               UgaColorDepth;
+  UINT32                               UgaRefreshRate;
+  TEXT_OUT_AND_GOP_DATA                *TextAndGop;
+  UINTN                                SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Info;
+  EFI_STATUS                           DeviceStatus;
+
+  Status                = EFI_SUCCESS;
+  CurrentNumOfConsoles  = Private->CurrentNumberOfConsoles;
+
+  //
+  // If the Text Out List is full, enlarge it by calling ConSplitterGrowBuffer().
+  //
+  while (CurrentNumOfConsoles >= Private->TextOutListCount) {
+    Status = ConSplitterGrowBuffer (
+              sizeof (TEXT_OUT_AND_GOP_DATA),
+              &Private->TextOutListCount,
+              (VOID **) &Private->TextOutList
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Also need to reallocate the TextOutModeMap table
+    //
+    Status = ConSplitterGrowMapTable (Private);
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  TextAndGop          = &Private->TextOutList[CurrentNumOfConsoles];
+
+  TextAndGop->TextOut        = TextOut;
+  TextAndGop->GraphicsOutput = GraphicsOutput;
+  TextAndGop->UgaDraw        = UgaDraw;
+
+  if (CurrentNumOfConsoles == 0) {
+    //
+    // Add the first device's output mode to console splitter's mode list
+    //
+    Status = ConSplitterAddOutputMode (Private, TextOut);
+  } else {
+    ConSplitterSyncOutputMode (Private, TextOut);
+  }
+
+  Private->CurrentNumberOfConsoles++;
+
+  //
+  // Scan both TextOutList, for the intersection TextOut device
+  // maybe both ConOut and StdErr incorporate the same Text Out
+  // device in them, thus the output of both should be synced.
+  //
+  ConSplitterGetIntersectionBetweenConOutAndStrErr ();
+
+  MaxMode     = Private->TextOutMode.MaxMode;
+  ASSERT (MaxMode >= 1);
+
+  DeviceStatus = EFI_DEVICE_ERROR;
+  Status       = EFI_DEVICE_ERROR;
+  
+  //
+  // This device display mode will be added into Graphics Ouput modes.
+  //
+  if ((GraphicsOutput != NULL) || (UgaDraw != NULL)) {
+    DeviceStatus = ConSplitterAddGraphicsOutputMode (Private, GraphicsOutput, UgaDraw);
+  }
+
+  if (FeaturePcdGet (PcdConOutUgaSupport)) {
+    //
+    // If UGA is produced by Consplitter
+    //
+    if (GraphicsOutput != NULL) {
+      Status = GraphicsOutput->QueryMode (GraphicsOutput, GraphicsOutput->Mode->Mode, &SizeOfInfo, &Info);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      ASSERT ( SizeOfInfo <= sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+
+      UgaHorizontalResolution = Info->HorizontalResolution;
+      UgaVerticalResolution   = Info->VerticalResolution;
+
+      FreePool (Info);
+
+    } else if (UgaDraw != NULL) {
+      Status = UgaDraw->GetMode (
+                    UgaDraw,
+                    &UgaHorizontalResolution,
+                    &UgaVerticalResolution,
+                    &UgaColorDepth,
+                    &UgaRefreshRate
+                    );
+      if (!EFI_ERROR (Status) && EFI_ERROR (DeviceStatus)) {
+        //
+        // if GetMode is successfully and UGA device hasn't been set, set it
+        //
+        Status = ConSplitterUgaDrawSetMode (
+                    &Private->UgaDraw,
+                    UgaHorizontalResolution,
+                    UgaVerticalResolution,
+                    UgaColorDepth,
+                    UgaRefreshRate
+                    );
+      }
+      //
+      // If GetMode/SetMode is failed, set to 800x600 mode
+      //
+      if(EFI_ERROR (Status)) {
+        Status = ConSplitterUgaDrawSetMode (
+                    &Private->UgaDraw,
+                    800,
+                    600,
+                    32,
+                    60
+                    );
+      }
+    }
+  }
+
+  if (((!EFI_ERROR (DeviceStatus)) || (!EFI_ERROR (Status))) &&
+      ((Private->CurrentNumberOfGraphicsOutput + Private->CurrentNumberOfUgaDraw) == 1)) {
+    if (!FeaturePcdGet (PcdConOutGopSupport)) {
+      //
+      // If Graphics Outpurt protocol not supported, UGA Draw protocol is installed
+      // on the virtual handle.
+      //
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mConOut.VirtualHandle,
+                      &gEfiUgaDrawProtocolGuid,
+                      &mConOut.UgaDraw,
+                      NULL
+                      );
+    } else if (!FeaturePcdGet (PcdConOutUgaSupport)) {
+      //
+      // If UGA Draw protocol not supported, Graphics Output Protocol is installed
+      // on virtual handle.
+      //
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mConOut.VirtualHandle,
+                      &gEfiGraphicsOutputProtocolGuid,
+                      &mConOut.GraphicsOutput,
+                      NULL
+                      );
+    } else {
+      //
+      // Boot Graphics Output protocol and UGA Draw protocol are supported,
+      // both they will be installed on virtual handle.
+      //
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mConOut.VirtualHandle,
+                      &gEfiGraphicsOutputProtocolGuid,
+                      &mConOut.GraphicsOutput,
+                      &gEfiUgaDrawProtocolGuid,
+                      &mConOut.UgaDraw,
+                      NULL
+                      );
+    }
+  }
+
+  //
+  // After adding new console device, all existing console devices should be
+  // synced to the current shared mode.
+  //
+  ConsplitterSetConsoleOutMode (Private);
+
+  return Status;
+}
+
+
+/**
+  Remove Text Out Device in Consplitter Text Out list.
+
+  @param  Private                  Text Out Splitter pointer.
+  @param  TextOut                  Simple Text Output Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Text Out Device removed successfully.
+  @retval EFI_NOT_FOUND            No Text Out Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextOutDeleteDevice (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut
+  )
+{
+  INT32                 Index;
+  UINTN                 CurrentNumOfConsoles;
+  TEXT_OUT_AND_GOP_DATA *TextOutList;
+  EFI_STATUS            Status;
+
+  //
+  // Remove the specified text-out device data structure from the Text out List,
+  // and rearrange the remaining data structures in the Text out List.
+  //
+  CurrentNumOfConsoles  = Private->CurrentNumberOfConsoles;
+  Index                 = (INT32) CurrentNumOfConsoles - 1;
+  TextOutList           = Private->TextOutList;
+  while (Index >= 0) {
+    if (TextOutList->TextOut == TextOut) {
+      if (TextOutList->UgaDraw != NULL) {
+        Private->CurrentNumberOfUgaDraw--;
+      }
+      if (TextOutList->GraphicsOutput != NULL) {
+        Private->CurrentNumberOfGraphicsOutput--;
+      }
+      CopyMem (TextOutList, TextOutList + 1, sizeof (TEXT_OUT_AND_GOP_DATA) * Index);
+      CurrentNumOfConsoles--;
+      break;
+    }
+
+    Index--;
+    TextOutList++;
+  }
+  //
+  // The specified TextOut is not managed by the ConSplitter driver
+  //
+  if (Index < 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if ((Private->CurrentNumberOfGraphicsOutput == 0) && (Private->CurrentNumberOfUgaDraw == 0)) {
+    //
+    // If there is not any physical GOP and UGA device in system,
+    // Consplitter GOP or UGA protocol will be uninstalled
+    //
+    if (!FeaturePcdGet (PcdConOutGopSupport)) {
+      Status = gBS->UninstallProtocolInterface (
+                      Private->VirtualHandle,
+                      &gEfiUgaDrawProtocolGuid,
+                      &Private->UgaDraw
+                      );      
+    } else if (!FeaturePcdGet (PcdConOutUgaSupport)) {
+      Status = gBS->UninstallProtocolInterface (
+                      Private->VirtualHandle,
+                      &gEfiGraphicsOutputProtocolGuid,
+                      &Private->GraphicsOutput
+                      );
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+             Private->VirtualHandle,
+             &gEfiUgaDrawProtocolGuid,
+             &Private->UgaDraw,
+             &gEfiGraphicsOutputProtocolGuid,
+             &Private->GraphicsOutput,
+             NULL
+             );
+    }
+  }
+
+  if (CurrentNumOfConsoles == 0) {
+    //
+    // If the number of consoles is zero, reset all parameters
+    //
+    Private->CurrentNumberOfConsoles      = 0;
+    Private->TextOutMode.MaxMode          = 1;
+    Private->TextOutQueryData[0].Columns  = 80;
+    Private->TextOutQueryData[0].Rows     = 25;
+    TextOutSetMode (Private, 0);
+
+    return EFI_SUCCESS;
+  }
+  //
+  // Max Mode is realy an intersection of the QueryMode command to all
+  // devices. So we must copy the QueryMode of the first device to
+  // QueryData.
+  //
+  ZeroMem (
+    Private->TextOutQueryData,
+    Private->TextOutQueryDataCount * sizeof (TEXT_OUT_SPLITTER_QUERY_DATA)
+    );
+
+  FreePool (Private->TextOutModeMap);
+  Private->TextOutModeMap = NULL;
+  TextOutList             = Private->TextOutList;
+
+  //
+  // Add the first TextOut to the QueryData array and ModeMap table
+  //
+  Status = ConSplitterAddOutputMode (Private, TextOutList->TextOut);
+
+  //
+  // Now add one by one
+  //
+  Index = 1;
+  Private->CurrentNumberOfConsoles = 1;
+  TextOutList++;
+  while ((UINTN) Index < CurrentNumOfConsoles) {
+    ConSplitterSyncOutputMode (Private, TextOutList->TextOut);
+    Index++;
+    Private->CurrentNumberOfConsoles++;
+    TextOutList++;
+  }
+
+  ConSplitterGetIntersectionBetweenConOutAndStrErr ();
+
+  return Status;
+}
+
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  IN  BOOLEAN                         ExtendedVerification
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    ReturnStatus;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private                       = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  Private->KeyEventSignalState  = FALSE;
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextInList[Index]->Reset (
+                                          Private->TextInList[Index],
+                                          ExtendedVerification
+                                          );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  Private                  Protocol instance pointer.
+  @param  Key                      Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInPrivateReadKeyStroke (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  OUT EFI_INPUT_KEY                   *Key
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         Index;
+  EFI_INPUT_KEY CurrentKey;
+
+  Key->UnicodeChar  = 0;
+  Key->ScanCode     = SCAN_NULL;
+
+  //
+  // if no physical console input device exists, return EFI_NOT_READY;
+  // if any physical console input device has key input,
+  // return the key and EFI_SUCCESS.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextInList[Index]->ReadKeyStroke (
+                                          Private->TextInList[Index],
+                                          &CurrentKey
+                                          );
+    if (!EFI_ERROR (Status)) {
+      *Key = CurrentKey;
+      return Status;
+    }
+  }
+
+  return EFI_NOT_READY;
+}
+
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  Key                      Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  OUT EFI_INPUT_KEY                   *Key
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+
+  Private = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  Private->KeyEventSignalState = FALSE;
+
+  //
+  // Signal ConnectConIn event on first call in Lazy ConIn mode
+  //
+  if (!mConInIsConnect && PcdGetBool (PcdConInConnectOnDemand)) {
+    DEBUG ((EFI_D_INFO, "Connect ConIn in first ReadKeyStoke in Lazy ConIn mode.\n"));    
+    gBS->SignalEvent (Private->ConnectConInEvent);
+    mConInIsConnect = TRUE;
+  }
+
+  return ConSplitterTextInPrivateReadKeyStroke (Private, Key);
+}
+
+
+/**
+  This event aggregates all the events of the ConIn devices in the spliter.
+
+  If any events of physical ConIn devices are signaled, signal the ConIn
+  spliter event. This will cause the calling code to call
+  ConSplitterTextInReadKeyStroke ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterTextInWaitForKey (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  )
+{
+  EFI_STATUS                    Status;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private = (TEXT_IN_SPLITTER_PRIVATE_DATA *) Context;
+
+  if (Private->KeyEventSignalState) {
+    //
+    // If KeyEventSignalState is flagged before, and not cleared by Reset() or ReadKeyStroke()
+    //
+    gBS->SignalEvent (Event);
+    return ;
+  }
+
+  //
+  // If any physical console input device has key input, signal the event.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = gBS->CheckEvent (Private->TextInList[Index]->WaitForKey);
+    if (!EFI_ERROR (Status)) {
+      gBS->SignalEvent (Event);
+      Private->KeyEventSignalState = TRUE;
+    }
+  }
+}
+
+
+
+/**
+  Test if the key has been registered on input device.
+
+  @param  RegsiteredData           A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   registered.
+  @param  InputData                A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval TRUE                     Key be pressed matches a registered key.
+  @retval FLASE                    Match failed.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  )
+{
+  ASSERT (RegsiteredData != NULL && InputData != NULL);
+
+  if ((RegsiteredData->Key.ScanCode    != InputData->Key.ScanCode) ||
+      (RegsiteredData->Key.UnicodeChar != InputData->Key.UnicodeChar)) {
+    return FALSE;
+  }
+
+  //
+  // Assume KeyShiftState/KeyToggleState = 0 in Registered key data means these state could be ignored.
+  //
+  if (RegsiteredData->KeyState.KeyShiftState != 0 &&
+      RegsiteredData->KeyState.KeyShiftState != InputData->KeyState.KeyShiftState) {
+    return FALSE;
+  }
+  if (RegsiteredData->KeyState.KeyToggleState != 0 &&
+      RegsiteredData->KeyState.KeyToggleState != InputData->KeyState.KeyToggleState) {
+    return FALSE;
+  }
+
+  return TRUE;
+
+}
+
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    ReturnStatus;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private                       = TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  Private->KeyEventSignalState  = FALSE;
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfExConsoles; Index++) {
+    Status = Private->TextInExList[Index]->Reset (
+                                             Private->TextInExList[Index],
+                                             ExtendedVerification
+                                             );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  return ReturnStatus;
+
+}
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keystroke information was not returned due
+                                   to hardware errors.
+  @retval EFI_INVALID_PARAMETER    KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  EFI_KEY_DATA                  CurrentKeyData;
+
+
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  Private->KeyEventSignalState = FALSE;
+
+  KeyData->Key.UnicodeChar  = 0;
+  KeyData->Key.ScanCode     = SCAN_NULL;
+
+  //
+  // Signal ConnectConIn event on first call in Lazy ConIn mode
+  //
+  if (!mConInIsConnect && PcdGetBool (PcdConInConnectOnDemand)) {
+    DEBUG ((EFI_D_INFO, "Connect ConIn in first ReadKeyStoke in Lazy ConIn mode.\n"));    
+    gBS->SignalEvent (Private->ConnectConInEvent);
+    mConInIsConnect = TRUE;
+  }
+
+  //
+  // if no physical console input device exists, return EFI_NOT_READY;
+  // if any physical console input device has key input,
+  // return the key and EFI_SUCCESS.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfExConsoles; Index++) {
+    Status = Private->TextInExList[Index]->ReadKeyStrokeEx (
+                                          Private->TextInExList[Index],
+                                          &CurrentKeyData
+                                          );
+    if (!EFI_ERROR (Status)) {
+      CopyMem (KeyData, &CurrentKeyData, sizeof (CurrentKeyData));
+      return Status;
+    }
+  }
+
+  return EFI_NOT_READY;
+}
+
+
+/**
+  Set certain state for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyToggleState           A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                   state for the input device.
+
+  @retval EFI_SUCCESS              The device state was set successfully.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not have the setting adjusted.
+  @retval EFI_UNSUPPORTED          The device does not have the ability to set its
+                                   state.
+  @retval EFI_INVALID_PARAMETER    KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+
+  if (KeyToggleState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // if no physical console input device exists, return EFI_SUCCESS;
+  // otherwise return the status of setting state of physical console input device
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfExConsoles; Index++) {
+    Status = Private->TextInExList[Index]->SetState (
+                                             Private->TextInExList[Index],
+                                             KeyToggleState
+                                             );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke information data for the key that was
+                                   pressed.
+  @param  KeyNotificationFunction  Points to the function to be called when the key
+                                   sequence is typed specified by KeyData.
+  @param  NotifyHandle             Points to the unique handle assigned to the
+                                   registered notification.
+
+  @retval EFI_SUCCESS              The notification function was registered
+                                   successfully.
+  @retval EFI_OUT_OF_RESOURCES     Unable to allocate resources for necesssary data
+                                   structures.
+  @retval EFI_INVALID_PARAMETER    KeyData or KeyNotificationFunction or NotifyHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInRegisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_DATA                       *KeyData,
+  IN EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                              **NotifyHandle
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  EFI_STATUS                    Status;
+  UINTN                         Index;
+  TEXT_IN_EX_SPLITTER_NOTIFY    *NewNotify;
+  LIST_ENTRY                    *Link;
+  TEXT_IN_EX_SPLITTER_NOTIFY    *CurrentNotify;
+
+
+  if (KeyData == NULL || NotifyHandle == NULL || KeyNotificationFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Return EFI_SUCCESS if the (KeyData, NotificationFunction) is already registered.
+  //
+  for (Link = Private->NotifyList.ForwardLink; Link != &Private->NotifyList; Link = Link->ForwardLink) {
+    CurrentNotify = TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS (Link);
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      if (CurrentNotify->KeyNotificationFn == KeyNotificationFunction) {
+        *NotifyHandle = CurrentNotify;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Allocate resource to save the notification function
+  //
+  NewNotify = (TEXT_IN_EX_SPLITTER_NOTIFY *) AllocateZeroPool (sizeof (TEXT_IN_EX_SPLITTER_NOTIFY));
+  if (NewNotify == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  NewNotify->NotifyHandleList = (EFI_HANDLE *) AllocateZeroPool (sizeof (EFI_HANDLE) *  Private->TextInExListCount);
+  if (NewNotify->NotifyHandleList == NULL) {
+    gBS->FreePool (NewNotify);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  NewNotify->Signature         = TEXT_IN_EX_SPLITTER_NOTIFY_SIGNATURE;
+  NewNotify->KeyNotificationFn = KeyNotificationFunction;
+  CopyMem (&NewNotify->KeyData, KeyData, sizeof (EFI_KEY_DATA));
+
+  //
+  // Return the wrong status of registering key notify of
+  // physical console input device if meet problems
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfExConsoles; Index++) {
+    Status = Private->TextInExList[Index]->RegisterKeyNotify (
+                                             Private->TextInExList[Index],
+                                             KeyData,
+                                             KeyNotificationFunction,
+                                             &NewNotify->NotifyHandleList[Index]
+                                             );
+    if (EFI_ERROR (Status)) {
+      //
+      // Un-register the key notify on all physical console input devices
+      //
+      while (Index-- != 0) {
+        Private->TextInExList[Index]->UnregisterKeyNotify (
+                                        Private->TextInExList[Index],
+                                        NewNotify->NotifyHandleList[Index]
+                                        );
+      }
+      gBS->FreePool (NewNotify->NotifyHandleList);
+      gBS->FreePool (NewNotify);
+      return Status;
+    }
+  }
+
+  InsertTailList (&mConIn.NotifyList, &NewNotify->NotifyEntry);
+
+  *NotifyHandle                = NewNotify;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                     Protocol instance pointer.
+  @param  NotificationHandle       The handle of the notification function being
+                                   unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered
+                                   successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+  TEXT_IN_EX_SPLITTER_NOTIFY    *CurrentNotify;
+  LIST_ENTRY                    *Link;
+
+  if (NotificationHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  for (Link = Private->NotifyList.ForwardLink; Link != &Private->NotifyList; Link = Link->ForwardLink) {
+    CurrentNotify = TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS (Link);
+    if (CurrentNotify == NotificationHandle) {
+      for (Index = 0; Index < Private->CurrentNumberOfExConsoles; Index++) {
+        Private->TextInExList[Index]->UnregisterKeyNotify (
+                                        Private->TextInExList[Index],
+                                        CurrentNotify->NotifyHandleList[Index]
+                                        );
+      }
+      RemoveEntryList (&CurrentNotify->NotifyEntry);
+
+      gBS->FreePool (CurrentNotify->NotifyHandleList);
+      gBS->FreePool (CurrentNotify);
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // NotificationHandle is not found in database
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerReset (
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *This,
+  IN  BOOLEAN                         ExtendedVerification
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    ReturnStatus;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private                         = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_SIMPLE_POINTER_THIS (This);
+
+  Private->InputEventSignalState  = FALSE;
+
+  if (Private->CurrentNumberOfPointers == 0) {
+    return EFI_SUCCESS;
+  }
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfPointers; Index++) {
+    Status = Private->PointerList[Index]->Reset (
+                                            Private->PointerList[Index],
+                                            ExtendedVerification
+                                            );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  Private                  Protocol instance pointer.
+  @param  State                    The state information of simple pointer device.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerPrivateGetState (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN OUT EFI_SIMPLE_POINTER_STATE     *State
+  )
+{
+  EFI_STATUS                Status;
+  EFI_STATUS                ReturnStatus;
+  UINTN                     Index;
+  EFI_SIMPLE_POINTER_STATE  CurrentState;
+
+  State->RelativeMovementX  = 0;
+  State->RelativeMovementY  = 0;
+  State->RelativeMovementZ  = 0;
+  State->LeftButton         = FALSE;
+  State->RightButton        = FALSE;
+
+  //
+  // if no physical console input device exists, return EFI_NOT_READY;
+  // if any physical console input device has key input,
+  // return the key and EFI_SUCCESS.
+  //
+  ReturnStatus = EFI_NOT_READY;
+  for (Index = 0; Index < Private->CurrentNumberOfPointers; Index++) {
+
+    Status = Private->PointerList[Index]->GetState (
+                                            Private->PointerList[Index],
+                                            &CurrentState
+                                            );
+    if (!EFI_ERROR (Status)) {
+      if (ReturnStatus == EFI_NOT_READY) {
+        ReturnStatus = EFI_SUCCESS;
+      }
+
+      if (CurrentState.LeftButton) {
+        State->LeftButton = TRUE;
+      }
+
+      if (CurrentState.RightButton) {
+        State->RightButton = TRUE;
+      }
+
+      if (CurrentState.RelativeMovementX != 0 && Private->PointerList[Index]->Mode->ResolutionX != 0) {
+        State->RelativeMovementX += (CurrentState.RelativeMovementX * (INT32) Private->SimplePointerMode.ResolutionX) / (INT32) Private->PointerList[Index]->Mode->ResolutionX;
+      }
+
+      if (CurrentState.RelativeMovementY != 0 && Private->PointerList[Index]->Mode->ResolutionY != 0) {
+        State->RelativeMovementY += (CurrentState.RelativeMovementY * (INT32) Private->SimplePointerMode.ResolutionY) / (INT32) Private->PointerList[Index]->Mode->ResolutionY;
+      }
+
+      if (CurrentState.RelativeMovementZ != 0 && Private->PointerList[Index]->Mode->ResolutionZ != 0) {
+        State->RelativeMovementZ += (CurrentState.RelativeMovementZ * (INT32) Private->SimplePointerMode.ResolutionZ) / (INT32) Private->PointerList[Index]->Mode->ResolutionZ;
+      }
+    } else if (Status == EFI_DEVICE_ERROR) {
+      ReturnStatus = EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     A pointer to protocol instance.
+  @param  State                    A pointer to state information on the pointer device
+
+  @retval EFI_SUCCESS              The keystroke information was returned in State.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerGetState (
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *This,
+  IN OUT EFI_SIMPLE_POINTER_STATE     *State
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+
+  Private = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_SIMPLE_POINTER_THIS (This);
+
+  Private->InputEventSignalState = FALSE;
+
+  return ConSplitterSimplePointerPrivateGetState (Private, State);
+}
+
+
+/**
+  This event agregates all the events of the ConIn devices in the spliter.
+  If any events of physical ConIn devices are signaled, signal the ConIn
+  spliter event. This will cause the calling code to call
+  ConSplitterTextInReadKeyStroke ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterSimplePointerWaitForInput (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  )
+{
+  EFI_STATUS                    Status;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private = (TEXT_IN_SPLITTER_PRIVATE_DATA *) Context;
+
+  //
+  // if InputEventSignalState is flagged before, and not cleared by Reset() or ReadKeyStroke()
+  //
+  if (Private->InputEventSignalState) {
+    gBS->SignalEvent (Event);
+    return ;
+  }
+  //
+  // if any physical console input device has key input, signal the event.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfPointers; Index++) {
+    Status = gBS->CheckEvent (Private->PointerList[Index]->WaitForInput);
+    if (!EFI_ERROR (Status)) {
+      gBS->SignalEvent (Event);
+      Private->InputEventSignalState = TRUE;
+    }
+  }
+}
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL   *This,
+  IN BOOLEAN                         ExtendedVerification
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_STATUS                    ReturnStatus;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_ABSOLUTE_POINTER_THIS (This);
+
+  Private->AbsoluteInputEventSignalState = FALSE;
+
+  if (Private->CurrentNumberOfAbsolutePointers == 0) {
+    return EFI_SUCCESS;
+  }
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfAbsolutePointers; Index++) {
+    Status = Private->AbsolutePointerList[Index]->Reset (
+                                                    Private->AbsolutePointerList[Index],
+                                                    ExtendedVerification
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                     Protocol instance pointer.
+  @param  State                    A pointer to the state information on the
+                                   pointer device.
+
+  @retval EFI_SUCCESS              The state of the pointer device was returned in
+                                   State..
+  @retval EFI_NOT_READY            The state of the pointer device has not changed
+                                   since the last call to GetState().
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to
+                                   retrieve the pointer device's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerGetState (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL   *This,
+  IN OUT EFI_ABSOLUTE_POINTER_STATE  *State
+  )
+{
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  EFI_STATUS                    Status;
+  EFI_STATUS                    ReturnStatus;
+  UINTN                         Index;
+  EFI_ABSOLUTE_POINTER_STATE    CurrentState;
+
+
+  Private = TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_ABSOLUTE_POINTER_THIS (This);
+
+  Private->AbsoluteInputEventSignalState = FALSE;
+
+  State->CurrentX                        = 0;
+  State->CurrentY                        = 0;
+  State->CurrentZ                        = 0;
+  State->ActiveButtons                   = 0;
+
+  //
+  // if no physical pointer device exists, return EFI_NOT_READY;
+  // if any physical pointer device has changed state,
+  // return the state and EFI_SUCCESS.
+  //
+  ReturnStatus = EFI_NOT_READY;
+  for (Index = 0; Index < Private->CurrentNumberOfAbsolutePointers; Index++) {
+
+    Status = Private->AbsolutePointerList[Index]->GetState (
+                                                    Private->AbsolutePointerList[Index],
+                                                    &CurrentState
+                                                    );
+    if (!EFI_ERROR (Status)) {
+      if (ReturnStatus == EFI_NOT_READY) {
+        ReturnStatus = EFI_SUCCESS;
+      }
+
+      State->ActiveButtons = CurrentState.ActiveButtons;
+
+      if (!(Private->AbsolutePointerMode.AbsoluteMinX == 0 && Private->AbsolutePointerMode.AbsoluteMaxX == 0)) {
+        State->CurrentX = CurrentState.CurrentX;
+      }
+      if (!(Private->AbsolutePointerMode.AbsoluteMinY == 0 && Private->AbsolutePointerMode.AbsoluteMaxY == 0)) {
+        State->CurrentY = CurrentState.CurrentY;
+      }
+      if (!(Private->AbsolutePointerMode.AbsoluteMinZ == 0 && Private->AbsolutePointerMode.AbsoluteMaxZ == 0)) {
+        State->CurrentZ = CurrentState.CurrentZ;
+      }
+
+    } else if (Status == EFI_DEVICE_ERROR) {
+      ReturnStatus = EFI_DEVICE_ERROR;
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  This event agregates all the events of the pointer devices in the splitter.
+  If any events of physical pointer devices are signaled, signal the pointer
+  splitter event. This will cause the calling code to call
+  ConSplitterAbsolutePointerGetState ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterAbsolutePointerWaitForInput (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  )
+{
+  EFI_STATUS                    Status;
+  TEXT_IN_SPLITTER_PRIVATE_DATA *Private;
+  UINTN                         Index;
+
+  Private = (TEXT_IN_SPLITTER_PRIVATE_DATA *) Context;
+
+  //
+  // if AbsoluteInputEventSignalState is flagged before,
+  // and not cleared by Reset() or GetState(), signal it
+  //
+  if (Private->AbsoluteInputEventSignalState) {
+    gBS->SignalEvent (Event);
+    return ;
+  }
+  //
+  // if any physical console input device has key input, signal the event.
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfAbsolutePointers; Index++) {
+    Status = gBS->CheckEvent (Private->AbsolutePointerList[Index]->WaitForInput);
+    if (!EFI_ERROR (Status)) {
+      gBS->SignalEvent (Event);
+      Private->AbsoluteInputEventSignalState = TRUE;
+    }
+  }
+}
+
+
+/**
+  Reset the text output device hardware and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform more exhaustive verfication
+                                   operation of the device during reset.
+
+  @retval EFI_SUCCESS              The text output device was reset.
+  @retval EFI_DEVICE_ERROR         The text output device is not functioning
+                                   correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            ExtendedVerification
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->Reset (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    ExtendedVerification
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  This->SetAttribute (This, EFI_TEXT_ATTR (This->Mode->Attribute & 0x0F, EFI_BLACK));
+
+  //
+  // reset all mode parameters
+  //
+  TextOutSetMode (Private, 0);
+
+  return ReturnStatus;
+}
+
+
+/**
+  Write a Unicode string to the output device.
+
+  @param  This                     Protocol instance pointer.
+  @param  WString                  The NULL-terminated Unicode string to be
+                                   displayed on the output device(s). All output
+                                   devices must also support the Unicode drawing
+                                   defined in this file.
+
+  @retval EFI_SUCCESS              The string was output to the device.
+  @retval EFI_DEVICE_ERROR         The device reported an error while attempting to
+                                   output the text.
+  @retval EFI_UNSUPPORTED          The output device's mode is not currently in a
+                                   defined text mode.
+  @retval EFI_WARN_UNKNOWN_GLYPH   This warning code indicates that some of the
+                                   characters in the Unicode string could not be
+                                   rendered and were skipped.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutOutputString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  CHAR16                             *WString
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+  UINTN                           MaxColumn;
+  UINTN                           MaxRow;
+
+  This->SetAttribute (This, This->Mode->Attribute);
+
+  Private         = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->OutputString (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    WString
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  if (Private->CurrentNumberOfConsoles > 0) {
+    Private->TextOutMode.CursorColumn = Private->TextOutList[0].TextOut->Mode->CursorColumn;
+    Private->TextOutMode.CursorRow    = Private->TextOutList[0].TextOut->Mode->CursorRow;
+  } else {
+    //
+    // When there is no real console devices in system, 
+    // update cursor position for the virtual device in consplitter.
+    //
+    Private->TextOut.QueryMode (
+                       &Private->TextOut,
+                       Private->TextOutMode.Mode,
+                       &MaxColumn,
+                       &MaxRow
+                       );    
+    for (; *WString != CHAR_NULL; WString++) {
+      switch (*WString) {
+      case CHAR_BACKSPACE:
+        if (Private->TextOutMode.CursorColumn == 0 && Private->TextOutMode.CursorRow > 0) {
+          Private->TextOutMode.CursorRow--;
+          Private->TextOutMode.CursorColumn = (INT32) (MaxColumn - 1);          
+        } else if (Private->TextOutMode.CursorColumn > 0) {
+          Private->TextOutMode.CursorColumn--;
+        }
+        break;
+      
+      case CHAR_LINEFEED:
+        if (Private->TextOutMode.CursorRow < (INT32) (MaxRow - 1)) {
+          Private->TextOutMode.CursorRow++;
+        }
+        break;
+      
+      case CHAR_CARRIAGE_RETURN:
+        Private->TextOutMode.CursorColumn = 0;
+        break;
+      
+      default:
+        if (Private->TextOutMode.CursorColumn < (INT32) (MaxColumn - 1)) {
+          Private->TextOutMode.CursorColumn++;
+        } else {
+          Private->TextOutMode.CursorColumn = 0;
+          if (Private->TextOutMode.CursorRow < (INT32) (MaxRow - 1)) {
+            Private->TextOutMode.CursorRow++;
+          }
+        }
+        break;
+      }
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Verifies that all characters in a Unicode string can be output to the
+  target device.
+
+  @param  This                     Protocol instance pointer.
+  @param  WString                  The NULL-terminated Unicode string to be
+                                   examined for the output device(s).
+
+  @retval EFI_SUCCESS              The device(s) are capable of rendering the
+                                   output string.
+  @retval EFI_UNSUPPORTED          Some of the characters in the Unicode string
+                                   cannot be rendered by one or more of the output
+                                   devices mapped by the EFI handle.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  CHAR16                             *WString
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->TestString (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    WString
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+  //
+  // There is no DevNullTextOutTestString () since a Unicode buffer would
+  // always return EFI_SUCCESS.
+  // ReturnStatus will be EFI_SUCCESS if no consoles are present
+  //
+  return ReturnStatus;
+}
+
+
+/**
+  Returns information for an available text mode that the output device(s)
+  supports.
+
+  @param  This                     Protocol instance pointer.
+  @param  ModeNumber               The mode number to return information on.
+  @param  Columns                  Returns the columns of the text output device
+                                   for the requested ModeNumber.
+  @param  Rows                     Returns the rows of the text output device
+                                   for the requested ModeNumber.
+
+  @retval EFI_SUCCESS              The requested mode information was returned.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The mode number was not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              ModeNumber,
+  OUT UINTN                              *Columns,
+  OUT UINTN                              *Rows
+  )
+{
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           CurrentMode;
+  INT32                           *TextOutModeMap;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Check whether param ModeNumber is valid.
+  // ModeNumber should be within range 0 ~ MaxMode - 1.
+  //
+  if ( (ModeNumber > (UINTN)(((UINT32)-1)>>1)) ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((INT32) ModeNumber >= This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // We get the available mode from mode intersection map if it's available
+  //
+  if (Private->TextOutModeMap != NULL) {
+    TextOutModeMap = Private->TextOutModeMap + Private->TextOutListCount * ModeNumber;
+    CurrentMode    = (UINTN)(*TextOutModeMap);
+    *Columns       = Private->TextOutQueryData[CurrentMode].Columns;
+    *Rows          = Private->TextOutQueryData[CurrentMode].Rows;
+  } else {
+    *Columns  = Private->TextOutQueryData[ModeNumber].Columns;
+    *Rows     = Private->TextOutQueryData[ModeNumber].Rows;
+  }
+
+  if (*Columns <= 0 && *Rows <= 0) {
+    return EFI_UNSUPPORTED;
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Sets the output device(s) to a specified mode.
+
+  @param  This                     Protocol instance pointer.
+  @param  ModeNumber               The mode number to set.
+
+  @retval EFI_SUCCESS              The requested text mode was set.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The mode number was not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              ModeNumber
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  INT32                           *TextOutModeMap;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Check whether param ModeNumber is valid.
+  // ModeNumber should be within range 0 ~ MaxMode - 1.
+  //
+  if ( (ModeNumber > (UINTN)(((UINT32)-1)>>1)) ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((INT32) ModeNumber >= This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // If the mode is being set to the curent mode, then just clear the screen and return.
+  //
+  if (Private->TextOutMode.Mode == (INT32) ModeNumber) {
+    return ConSplitterTextOutClearScreen (This);
+  }
+  //
+  // return the worst status met
+  //
+  TextOutModeMap = Private->TextOutModeMap + Private->TextOutListCount * ModeNumber;
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->SetMode (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    TextOutModeMap[Index]
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  //
+  // Set mode parameter to specified mode number
+  //
+  TextOutSetMode (Private, ModeNumber);
+
+  return ReturnStatus;
+}
+
+
+/**
+  Sets the background and foreground colors for the OutputString () and
+  ClearScreen () functions.
+
+  @param  This                     Protocol instance pointer.
+  @param  Attribute                The attribute to set. Bits 0..3 are the
+                                   foreground color, and bits 4..6 are the
+                                   background color. All other bits are undefined
+                                   and must be zero. The valid Attributes are
+                                   defined in this file.
+
+  @retval EFI_SUCCESS              The attribute was set.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The attribute requested is not defined.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              Attribute
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Check whether param Attribute is valid.
+  //
+  if ((Attribute | 0x7F) != 0x7F) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->SetAttribute (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    Attribute
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  Private->TextOutMode.Attribute = (INT32) Attribute;
+
+  return ReturnStatus;
+}
+
+
+/**
+  Clears the output device(s) display to the currently selected background
+  color.
+
+  @param  This                     Protocol instance pointer.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->ClearScreen (Private->TextOutList[Index].TextOut);
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  //
+  // No need to do extra check here as whether (Column, Row) is valid has
+  // been checked in ConSplitterTextOutSetCursorPosition. And (0, 0) should
+  // always be supported.
+  //
+  Private->TextOutMode.CursorColumn = 0;
+  Private->TextOutMode.CursorRow    = 0;
+  Private->TextOutMode.CursorVisible = TRUE;
+
+  return ReturnStatus;
+}
+
+
+/**
+  Sets the current coordinates of the cursor position
+
+  @param  This                     Protocol instance pointer.
+  @param  Column                   The column position to set the cursor to. Must be
+                                   greater than or equal to zero and less than the
+                                   number of columns by QueryMode ().
+  @param  Row                      The row position to set the cursor to. Must be
+                                   greater than or equal to zero and less than the
+                                   number of rows by QueryMode ().
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode,
+                                   or the cursor position is invalid for the
+                                   current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              Column,
+  IN  UINTN                              Row
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+  UINTN                           MaxColumn;
+  UINTN                           MaxRow;
+  INT32                           *TextOutModeMap;
+  INT32                           ModeNumber;
+  INT32                           CurrentMode;
+
+  Private   = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+  TextOutModeMap  = NULL;
+  ModeNumber      = Private->TextOutMode.Mode;
+
+  //
+  // Get current MaxColumn and MaxRow from intersection map
+  //
+  if (Private->TextOutModeMap != NULL) {
+    TextOutModeMap = Private->TextOutModeMap + Private->TextOutListCount * ModeNumber;
+    CurrentMode    = *TextOutModeMap;
+  } else {
+    CurrentMode = ModeNumber;
+  }
+
+  MaxColumn = Private->TextOutQueryData[CurrentMode].Columns;
+  MaxRow    = Private->TextOutQueryData[CurrentMode].Rows;
+
+  if (Column >= MaxColumn || Row >= MaxRow) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->SetCursorPosition (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    Column,
+                                                    Row
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  //
+  // No need to do extra check here as whether (Column, Row) is valid has
+  // been checked in ConSplitterTextOutSetCursorPosition. And (0, 0) should
+  // always be supported.
+  //
+  Private->TextOutMode.CursorColumn = (INT32) Column;
+  Private->TextOutMode.CursorRow    = (INT32) Row;
+
+  return ReturnStatus;
+}
+
+
+/**
+  Makes the cursor visible or invisible
+
+  @param  This                     Protocol instance pointer.
+  @param  Visible                  If TRUE, the cursor is set to be visible. If
+                                   FALSE, the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request, or the device does not support
+                                   changing the cursor mode.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            Visible
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+
+  Private = TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0, ReturnStatus = EFI_SUCCESS; Index < Private->CurrentNumberOfConsoles; Index++) {
+    Status = Private->TextOutList[Index].TextOut->EnableCursor (
+                                                    Private->TextOutList[Index].TextOut,
+                                                    Visible
+                                                    );
+    if (EFI_ERROR (Status)) {
+      ReturnStatus = Status;
+    }
+  }
+
+  Private->TextOutMode.CursorVisible = Visible;
+
+  return ReturnStatus;
+}
+
+
+/**
+  An empty function to pass error checking of CreateEventEx ().
+
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               Pointer to the notification function's context,
+                                which is implementation-dependent.
+
+**/
+VOID
+EFIAPI
+ConSplitterEmptyCallbackFunction (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+}
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.h b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.h
new file mode 100644
index 0000000000..e32abbaea1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitter.h
@@ -0,0 +1,2004 @@
+/** @file
+  Private data structures for the Console Splitter driver
+
+Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef _CON_SPLITTER_H_
+#define _CON_SPLITTER_H_
+
+#include <Uefi.h>
+#include <PiDxe.h>
+
+#include <Protocol/DevicePath.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/SimplePointer.h>
+#include <Protocol/AbsolutePointer.h>
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/SimpleTextInEx.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/UgaDraw.h>
+
+#include <Guid/ConsoleInDevice.h>
+#include <Guid/StandardErrorDevice.h>
+#include <Guid/ConsoleOutDevice.h>
+#include <Guid/ConnectConInEvent.h>
+
+#include <Library/PcdLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+
+//
+// Driver Binding Externs
+//
+extern EFI_DRIVER_BINDING_PROTOCOL  gConSplitterConInDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConSplitterConInComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConSplitterConInComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gConSplitterSimplePointerDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConSplitterSimplePointerComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConSplitterSimplePointerComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gConSplitterAbsolutePointerDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConSplitterAbsolutePointerComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConSplitterAbsolutePointerComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gConSplitterConOutDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConSplitterConOutComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConSplitterConOutComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL  gConSplitterStdErrDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gConSplitterStdErrComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gConSplitterStdErrComponentName2;
+
+
+//
+// These definitions were in the old Hii protocol, but are not in the new UEFI
+// version. So they are defined locally.
+//
+#define UNICODE_NARROW_CHAR   0xFFF0
+#define UNICODE_WIDE_CHAR     0xFFF1
+
+
+//
+// Private Data Structures
+//
+#define CONSOLE_SPLITTER_ALLOC_UNIT  32
+
+
+typedef struct {
+  UINTN   Column;
+  UINTN   Row;
+} CONSOLE_OUT_MODE;
+
+typedef struct {
+  UINTN   Columns;
+  UINTN   Rows;
+} TEXT_OUT_SPLITTER_QUERY_DATA;
+
+
+#define TEXT_IN_EX_SPLITTER_NOTIFY_SIGNATURE    SIGNATURE_32 ('T', 'i', 'S', 'n')
+
+//
+// Private data for Text In Ex Splitter Notify
+//
+typedef struct _TEXT_IN_EX_SPLITTER_NOTIFY {
+  UINTN                                 Signature;
+  VOID                                  **NotifyHandleList;
+  EFI_KEY_DATA                          KeyData;
+  EFI_KEY_NOTIFY_FUNCTION               KeyNotificationFn;
+  LIST_ENTRY                            NotifyEntry;
+} TEXT_IN_EX_SPLITTER_NOTIFY;
+
+#define TEXT_IN_EX_SPLITTER_NOTIFY_FROM_THIS(a)  \
+  CR ((a),                                       \
+      TEXT_IN_EX_SPLITTER_NOTIFY,                \
+      NotifyEntry,                               \
+      TEXT_IN_EX_SPLITTER_NOTIFY_SIGNATURE       \
+      )
+
+#define TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('T', 'i', 'S', 'p')
+
+//
+// Private data for the Console In splitter
+//
+typedef struct {
+  UINT64                             Signature;
+  EFI_HANDLE                         VirtualHandle;
+
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL     TextIn;
+  UINTN                              CurrentNumberOfConsoles;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL     **TextInList;
+  UINTN                              TextInListCount;
+
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  TextInEx;
+  UINTN                              CurrentNumberOfExConsoles;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  **TextInExList;
+  UINTN                              TextInExListCount;
+  LIST_ENTRY                         NotifyList;
+
+
+  EFI_SIMPLE_POINTER_PROTOCOL        SimplePointer;
+  EFI_SIMPLE_POINTER_MODE            SimplePointerMode;
+  UINTN                              CurrentNumberOfPointers;
+  EFI_SIMPLE_POINTER_PROTOCOL        **PointerList;
+  UINTN                              PointerListCount;
+
+  EFI_ABSOLUTE_POINTER_PROTOCOL      AbsolutePointer;
+  EFI_ABSOLUTE_POINTER_MODE          AbsolutePointerMode;
+  UINTN                              CurrentNumberOfAbsolutePointers;
+  EFI_ABSOLUTE_POINTER_PROTOCOL      **AbsolutePointerList;
+  UINTN                              AbsolutePointerListCount;
+  BOOLEAN                            AbsoluteInputEventSignalState;
+
+  BOOLEAN                            KeyEventSignalState;
+  BOOLEAN                            InputEventSignalState;
+  EFI_EVENT                          ConnectConInEvent;
+} TEXT_IN_SPLITTER_PRIVATE_DATA;
+
+#define TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_THIS(a)  \
+  CR ((a),                                          \
+      TEXT_IN_SPLITTER_PRIVATE_DATA,                \
+      TextIn,                                       \
+      TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE       \
+      )
+
+#define TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_SIMPLE_POINTER_THIS(a) \
+  CR ((a),                                                        \
+      TEXT_IN_SPLITTER_PRIVATE_DATA,                              \
+      SimplePointer,                                              \
+      TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE                     \
+      )
+#define TEXT_IN_EX_SPLITTER_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a,                                              \
+      TEXT_IN_SPLITTER_PRIVATE_DATA,                  \
+      TextInEx,                                       \
+      TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE         \
+      )
+
+#define TEXT_IN_SPLITTER_PRIVATE_DATA_FROM_ABSOLUTE_POINTER_THIS(a) \
+  CR (a,                                                            \
+      TEXT_IN_SPLITTER_PRIVATE_DATA,                                \
+      AbsolutePointer,                                              \
+      TEXT_IN_SPLITTER_PRIVATE_DATA_SIGNATURE                       \
+      )
+
+
+#define TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE  SIGNATURE_32 ('T', 'o', 'S', 'p')
+
+typedef struct {
+  EFI_GRAPHICS_OUTPUT_PROTOCOL     *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL            *UgaDraw;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *TextOut;
+} TEXT_OUT_AND_GOP_DATA;
+
+//
+// Private data for the Console Out splitter
+//
+typedef struct {
+  UINT64                                Signature;
+  EFI_HANDLE                            VirtualHandle;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL       TextOut;
+  EFI_SIMPLE_TEXT_OUTPUT_MODE           TextOutMode;
+
+  EFI_UGA_DRAW_PROTOCOL                 UgaDraw;
+  UINT32                                UgaHorizontalResolution;
+  UINT32                                UgaVerticalResolution;
+  UINT32                                UgaColorDepth;
+  UINT32                                UgaRefreshRate;
+
+  EFI_GRAPHICS_OUTPUT_PROTOCOL          GraphicsOutput;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  *GraphicsOutputModeBuffer;
+  UINTN                                 CurrentNumberOfGraphicsOutput;
+  UINTN                                 CurrentNumberOfUgaDraw;
+
+  UINTN                                 CurrentNumberOfConsoles;
+  TEXT_OUT_AND_GOP_DATA                 *TextOutList;
+  UINTN                                 TextOutListCount;
+  TEXT_OUT_SPLITTER_QUERY_DATA          *TextOutQueryData;
+  UINTN                                 TextOutQueryDataCount;
+  INT32                                 *TextOutModeMap;
+
+} TEXT_OUT_SPLITTER_PRIVATE_DATA;
+
+#define TEXT_OUT_SPLITTER_PRIVATE_DATA_FROM_THIS(a) \
+  CR ((a),                                          \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA,               \
+      TextOut,                                      \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE      \
+      )
+
+#define GRAPHICS_OUTPUT_SPLITTER_PRIVATE_DATA_FROM_THIS(a)  \
+  CR ((a),                                                  \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA,                       \
+      GraphicsOutput,                                       \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE              \
+      )
+
+#define UGA_DRAW_SPLITTER_PRIVATE_DATA_FROM_THIS(a) \
+  CR ((a),                                          \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA,               \
+      UgaDraw,                                      \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE      \
+      )
+
+#define CONSOLE_CONTROL_SPLITTER_PRIVATE_DATA_FROM_THIS(a)  \
+  CR ((a),                                                  \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA,                       \
+      ConsoleControl,                                       \
+      TEXT_OUT_SPLITTER_PRIVATE_DATA_SIGNATURE              \
+      )
+
+//
+// Function Prototypes
+//
+
+/**
+  The user Entry Point for module ConSplitter. The user code starts with this function.
+
+  Installs driver module protocols and. Creates virtual device handles for ConIn,
+  ConOut, and StdErr. Installs Simple Text In protocol, Simple Text In Ex protocol,
+  Simple Pointer protocol, Absolute Pointer protocol on those virtual handlers.
+  Installs Graphics Output protocol and/or UGA Draw protocol if needed.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterDriverEntry (
+  IN EFI_HANDLE                       ImageHandle,
+  IN EFI_SYSTEM_TABLE                 *SystemTable
+  );
+
+/**
+  Construct console input devices' private data.
+
+  @param  ConInPrivate             A pointer to the TEXT_IN_SPLITTER_PRIVATE_DATA
+                                   structure.
+
+  @retval EFI_OUT_OF_RESOURCES     Out of resources.
+  @retval EFI_SUCCESS              Text Input Devcie's private data has been constructed.
+  @retval other                    Failed to construct private data.
+
+**/
+EFI_STATUS
+ConSplitterTextInConstructor (
+  TEXT_IN_SPLITTER_PRIVATE_DATA       *ConInPrivate
+  );
+
+/**
+  Construct console output devices' private data.
+
+  @param  ConOutPrivate            A pointer to the TEXT_OUT_SPLITTER_PRIVATE_DATA
+                                   structure.
+
+  @retval EFI_OUT_OF_RESOURCES     Out of resources.
+  @retval EFI_SUCCESS              Text Input Devcie's private data has been constructed.
+
+**/
+EFI_STATUS
+ConSplitterTextOutConstructor (
+  TEXT_OUT_SPLITTER_PRIVATE_DATA      *ConOutPrivate
+  );
+
+
+/**
+  Test to see if Console In Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Test to see if Simple Pointer protocol could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Test to see if Console Out Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Test to see if Standard Error Device could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Start Console In Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Console In Consplitter is added to ControllerHandle.
+  @retval other                Console In Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Start Simple Pointer Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Simple Pointer Consplitter is added to ControllerHandle.
+  @retval other                Simple Pointer Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Start Console Out Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Console Out Consplitter is added to ControllerHandle.
+  @retval other                Console Out Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Start Standard Error Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Standard Error Consplitter is added to ControllerHandle.
+  @retval other                Standard Error Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Stop Console In ConSplitter on ControllerHandle by closing Console In Devcice GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Stop Simple Pointer protocol ConSplitter on ControllerHandle by closing
+  Simple Pointer protocol.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Stop Console Out ConSplitter on device handle by closing Console Out Devcice GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Stop Standard Error ConSplitter on ControllerHandle by closing Standard Error GUID.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+
+/**
+  Test to see if Absolute Pointer protocol could be supported on the Controller.
+
+  @param  This                Driver Binding protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Start Absolute Pointer Consplitter on device handle.
+
+  @param  This                 Driver Binding protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          Absolute Pointer Consplitter is added to ControllerHandle.
+  @retval other                Absolute Pointer Consplitter does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath
+  );
+
+/**
+  Stop Absolute Pointer protocol ConSplitter on ControllerHandle by closing
+  Absolute Pointer protocol.
+
+  @param  This              Driver Binding protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN  EFI_HANDLE                      ControllerHandle,
+  IN  UINTN                           NumberOfChildren,
+  IN  EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  Add Absolute Pointer Device in Consplitter Absolute Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  AbsolutePointer          Absolute Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Absolute Pointer Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterAbsolutePointerAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer
+  );
+
+/**
+  Remove Absolute Pointer Device from Consplitter Absolute Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  AbsolutePointer          Absolute Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Absolute Pointer Device removed successfully.
+  @retval EFI_NOT_FOUND            No Absolute Pointer Device found.
+
+**/
+EFI_STATUS
+ConSplitterAbsolutePointerDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_ABSOLUTE_POINTER_PROTOCOL     *AbsolutePointer
+  );
+
+//
+// Absolute Pointer protocol interfaces
+//
+
+
+/**
+  Resets the pointer device hardware.
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerReset (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL   *This,
+  IN BOOLEAN                         ExtendedVerification
+  );
+
+
+/**
+  Retrieves the current state of a pointer device.
+
+  @param  This                     Protocol instance pointer.
+  @param  State                    A pointer to the state information on the
+                                   pointer device.
+
+  @retval EFI_SUCCESS              The state of the pointer device was returned in
+                                   State..
+  @retval EFI_NOT_READY            The state of the pointer device has not changed
+                                   since the last call to GetState().
+  @retval EFI_DEVICE_ERROR         A device error occurred while attempting to
+                                   retrieve the pointer device's current state.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerGetState (
+  IN EFI_ABSOLUTE_POINTER_PROTOCOL   *This,
+  IN OUT EFI_ABSOLUTE_POINTER_STATE  *State
+  );
+
+/**
+  This event agregates all the events of the pointer devices in the splitter.
+
+  If any events of physical pointer devices are signaled, signal the pointer
+  splitter event. This will cause the calling code to call
+  ConSplitterAbsolutePointerGetState ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterAbsolutePointerWaitForInput (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConInComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by an EFI Driver.
+
+  @param  This                   A pointer to the EFI_COMPONENT_NAME_PROTOCOL
+                                 instance.
+  @param  ControllerHandle       The handle of a controller that the driver
+                                 specified by This is managing.  This handle
+                                 specifies the controller whose name is to be
+                                 returned.
+  @param  ChildHandle            The handle of the child controller to retrieve the
+                                 name of.  This is an optional parameter that may
+                                 be NULL.  It will be NULL for device drivers.  It
+                                 will also be NULL for a bus drivers that wish to
+                                 retrieve the name of the bus controller.  It will
+                                 not be NULL for a bus driver that wishes to
+                                 retrieve the name of a child controller.
+  @param  Language               A pointer to RFC4646 language identifier. This is
+                                 the language of the controller name that that the
+                                 caller is requesting, and it must match one of the
+                                 languages specified in SupportedLanguages.  The
+                                 number of languages supported by a driver is up to
+                                 the driver writer.
+  @param  ControllerName         A pointer to the Unicode string to return.  This
+                                 Unicode string is the name of the controller
+                                 specified by ControllerHandle and ChildHandle in
+                                 the language specified by Language from the point
+                                 of view of the driver specified by This.
+
+  @retval EFI_SUCCESS            The Unicode string for the user readable name in
+                                 the language specified by Language for the driver
+                                 specified by This was returned in DriverName.
+  @retval EFI_INVALID_PARAMETER  ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER  ChildHandle is not NULL and it is not a valid
+                                 EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER  Language is NULL.
+  @retval EFI_INVALID_PARAMETER  ControllerName is NULL.
+  @retval EFI_UNSUPPORTED        The driver specified by This is not currently
+                                 managing the controller specified by
+                                 ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED        The driver specified by This does not support the
+                                 language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterAbsolutePointerComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                    *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterConOutComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterStdErrComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// TextIn Constructor/Destructor functions
+//
+
+/**
+  Add Text Input Device in Consplitter Text Input list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextIn                   Simple Text Input protocol pointer.
+
+  @retval EFI_SUCCESS              Text Input Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextInAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *TextIn
+  );
+
+/**
+  Remove Text Input Device from Consplitter Text Input list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextIn                   Simple Text protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Text Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Text Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextInDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *TextIn
+  );
+
+//
+// SimplePointer Constuctor/Destructor functions
+//
+
+/**
+  Add Simple Pointer Device in Consplitter Simple Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  SimplePointer            Simple Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Pointer Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterSimplePointerAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *SimplePointer
+  );
+
+/**
+  Remove Simple Pointer Device from Consplitter Simple Pointer list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  SimplePointer            Simple Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Pointer Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Pointer Device found.
+
+**/
+EFI_STATUS
+ConSplitterSimplePointerDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *SimplePointer
+  );
+
+//
+// TextOut Constuctor/Destructor functions
+//
+
+/**
+  Add Text Output Device in Consplitter Text Output list.
+
+  @param  Private                  Text Out Splitter pointer.
+  @param  TextOut                  Simple Text Output protocol pointer.
+  @param  GraphicsOutput           Graphics Output protocol pointer.
+  @param  UgaDraw                  UGA Draw protocol pointer.
+
+  @retval EFI_SUCCESS              Text Output Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextOutAddDevice (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut,
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL       *GraphicsOutput,
+  IN  EFI_UGA_DRAW_PROTOCOL              *UgaDraw
+  );
+
+/**
+  Remove Text Out Device in Consplitter Text Out list.
+
+  @param  Private                  Text Out Splitter pointer.
+  @param  TextOut                  Simple Text Output Pointer protocol pointer.
+
+  @retval EFI_SUCCESS              Text Out Device removed successfully.
+  @retval EFI_NOT_FOUND            No Text Out Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextOutDeleteDevice (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA     *Private,
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *TextOut
+  );
+
+//
+// TextIn I/O Functions
+//
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  IN  BOOLEAN                         ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  Key                      Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  OUT EFI_INPUT_KEY                   *Key
+  );
+
+/**
+  Add Text Input Ex Device in Consplitter Text Input Ex list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextInEx                 Simple Text Input Ex Input protocol pointer.
+
+  @retval EFI_SUCCESS              Text Input Ex Device added successfully.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterTextInExAddDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA         *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL     *TextInEx
+  );
+
+/**
+  Remove Text Ex Device from Consplitter Text Input Ex list.
+
+  @param  Private                  Text In Splitter pointer.
+  @param  TextInEx                 Simple Text Ex protocol pointer.
+
+  @retval EFI_SUCCESS              Simple Text Input Ex Device removed successfully.
+  @retval EFI_NOT_FOUND            No Simple Text Input Ex Device found.
+
+**/
+EFI_STATUS
+ConSplitterTextInExDeleteDevice (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA         *Private,
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL     *TextInEx
+  );
+
+//
+// Simple Text Input Ex protocol function prototypes
+//
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  );
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keystroke information was not returned due
+                                   to hardware errors.
+  @retval EFI_INVALID_PARAMETER    KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  );
+
+
+/**
+  Set certain state for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyToggleState           A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                   state for the input device.
+
+  @retval EFI_SUCCESS              The device state was set successfully.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not have the setting adjusted.
+  @retval EFI_UNSUPPORTED          The device does not have the ability to set its
+                                   state.
+  @retval EFI_INVALID_PARAMETER    KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  );
+
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke information data for the key that was
+                                   pressed.
+  @param  KeyNotificationFunction  Points to the function to be called when the key
+                                   sequence is typed specified by KeyData.
+  @param  NotifyHandle             Points to the unique handle assigned to the
+                                   registered notification.
+
+  @retval EFI_SUCCESS              The notification function was registered
+                                   successfully.
+  @retval EFI_OUT_OF_RESOURCES     Unable to allocate resources for necesssary data
+                                   structures.
+  @retval EFI_INVALID_PARAMETER    KeyData or KeyNotificationFunction or NotifyHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInRegisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_DATA                       *KeyData,
+  IN EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                              **NotifyHandle
+  );
+
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                     Protocol instance pointer.
+  @param  NotificationHandle       The handle of the notification function being
+                                   unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered
+                                   successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid.
+  @retval EFI_NOT_FOUND            Can not find the matching entry in database.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  );
+
+/**
+  This event aggregates all the events of the ConIn devices in the spliter.
+
+  If any events of physical ConIn devices are signaled, signal the ConIn
+  spliter event. This will cause the calling code to call
+  ConSplitterTextInReadKeyStroke ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterTextInWaitForKey (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  );
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  Private                  Protocol instance pointer.
+  @param  Key                      Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextInPrivateReadKeyStroke (
+  IN  TEXT_IN_SPLITTER_PRIVATE_DATA   *Private,
+  OUT EFI_INPUT_KEY                   *Key
+  );
+
+/**
+  Reset the input device and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerReset (
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *This,
+  IN  BOOLEAN                         ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existance of a keystroke via WaitForEvent () call.
+
+  @param  This                     A pointer to protocol instance.
+  @param  State                    A pointer to state information on the pointer device
+
+  @retval EFI_SUCCESS              The keystroke information was returned in State.
+  @retval EFI_NOT_READY            There was no keystroke data availiable.
+  @retval EFI_DEVICE_ERROR         The keydtroke information was not returned due
+                                   to hardware errors.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterSimplePointerGetState (
+  IN  EFI_SIMPLE_POINTER_PROTOCOL     *This,
+  IN OUT EFI_SIMPLE_POINTER_STATE     *State
+  );
+
+/**
+  This event agregates all the events of the ConIn devices in the spliter.
+  If any events of physical ConIn devices are signaled, signal the ConIn
+  spliter event. This will cause the calling code to call
+  ConSplitterTextInReadKeyStroke ().
+
+  @param  Event                    The Event assoicated with callback.
+  @param  Context                  Context registered when Event was created.
+
+**/
+VOID
+EFIAPI
+ConSplitterSimplePointerWaitForInput (
+  IN  EFI_EVENT                       Event,
+  IN  VOID                            *Context
+  );
+
+//
+// TextOut I/O Functions
+//
+
+/**
+  Reset the text output device hardware and optionaly run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform more exhaustive verfication
+                                   operation of the device during reset.
+
+  @retval EFI_SUCCESS              The text output device was reset.
+  @retval EFI_DEVICE_ERROR         The text output device is not functioning
+                                   correctly and could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Write a Unicode string to the output device.
+
+  @param  This                     Protocol instance pointer.
+  @param  WString                  The NULL-terminated Unicode string to be
+                                   displayed on the output device(s). All output
+                                   devices must also support the Unicode drawing
+                                   defined in this file.
+
+  @retval EFI_SUCCESS              The string was output to the device.
+  @retval EFI_DEVICE_ERROR         The device reported an error while attempting to
+                                   output the text.
+  @retval EFI_UNSUPPORTED          The output device's mode is not currently in a
+                                   defined text mode.
+  @retval EFI_WARN_UNKNOWN_GLYPH   This warning code indicates that some of the
+                                   characters in the Unicode string could not be
+                                   rendered and were skipped.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutOutputString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  CHAR16                             *WString
+  );
+
+/**
+  Verifies that all characters in a Unicode string can be output to the
+  target device.
+
+  @param  This                     Protocol instance pointer.
+  @param  WString                  The NULL-terminated Unicode string to be
+                                   examined for the output device(s).
+
+  @retval EFI_SUCCESS              The device(s) are capable of rendering the
+                                   output string.
+  @retval EFI_UNSUPPORTED          Some of the characters in the Unicode string
+                                   cannot be rendered by one or more of the output
+                                   devices mapped by the EFI handle.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  CHAR16                             *WString
+  );
+
+/**
+  Returns information for an available text mode that the output device(s)
+  supports.
+
+  @param  This                     Protocol instance pointer.
+  @param  ModeNumber               The mode number to return information on.
+  @param  Columns                  Returns the columns of the text output device
+                                   for the requested ModeNumber.
+  @param  Rows                     Returns the rows of the text output device
+                                   for the requested ModeNumber.
+
+  @retval EFI_SUCCESS              The requested mode information was returned.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The mode number was not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              ModeNumber,
+  OUT UINTN                              *Columns,
+  OUT UINTN                              *Rows
+  );
+
+/**
+  Sets the output device(s) to a specified mode.
+
+  @param  This                     Protocol instance pointer.
+  @param  ModeNumber               The mode number to set.
+
+  @retval EFI_SUCCESS              The requested text mode was set.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The mode number was not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              ModeNumber
+  );
+
+/**
+  Sets the background and foreground colors for the OutputString () and
+  ClearScreen () functions.
+
+  @param  This                     Protocol instance pointer.
+  @param  Attribute                The attribute to set. Bits 0..3 are the
+                                   foreground color, and bits 4..6 are the
+                                   background color. All other bits are undefined
+                                   and must be zero. The valid Attributes are
+                                   defined in this file.
+
+  @retval EFI_SUCCESS              The attribute was set.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The attribute requested is not defined.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              Attribute
+  );
+
+/**
+  Clears the output device(s) display to the currently selected background
+  color.
+
+  @param  This                     Protocol instance pointer.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This
+  );
+
+/**
+  Sets the current coordinates of the cursor position
+
+  @param  This                     Protocol instance pointer.
+  @param  Column                   The column position to set the cursor to. Must be
+                                   greater than or equal to zero and less than the
+                                   number of columns by QueryMode ().
+  @param  Row                      The row position to set the cursor to. Must be
+                                   greater than or equal to zero and less than the
+                                   number of rows by QueryMode ().
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode,
+                                   or the cursor position is invalid for the
+                                   current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  UINTN                              Column,
+  IN  UINTN                              Row
+  );
+
+
+/**
+  Makes the cursor visible or invisible
+
+  @param  This                     Protocol instance pointer.
+  @param  Visible                  If TRUE, the cursor is set to be visible. If
+                                   FALSE, the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_DEVICE_ERROR         The device had an error and could not complete
+                                   the request, or the device does not support
+                                   changing the cursor mode.
+  @retval EFI_UNSUPPORTED          The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterTextOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            Visible
+  );
+
+/**
+  Take the passed in Buffer of size ElementSize and grow the buffer
+  by CONSOLE_SPLITTER_ALLOC_UNIT * ElementSize bytes.
+  Copy the current data in Buffer to the new version of Buffer and
+  free the old version of buffer.
+
+  @param  ElementSize              Size of element in array.
+  @param  Count                    Current number of elements in array.
+  @param  Buffer                   Bigger version of passed in Buffer with all the
+                                   data.
+
+  @retval EFI_SUCCESS              Buffer size has grown.
+  @retval EFI_OUT_OF_RESOURCES     Could not grow the buffer size.
+
+**/
+EFI_STATUS
+ConSplitterGrowBuffer (
+  IN      UINTN                       ElementSize,
+  IN OUT  UINTN                       *Count,
+  IN OUT  VOID                        **Buffer
+  );
+
+/**
+  Returns information for an available graphics mode that the graphics device
+  and the set of active video output devices supports.
+
+  @param  This                  The EFI_GRAPHICS_OUTPUT_PROTOCOL instance.
+  @param  ModeNumber            The mode number to return information on.
+  @param  SizeOfInfo            A pointer to the size, in bytes, of the Info buffer.
+  @param  Info                  A pointer to callee allocated buffer that returns information about ModeNumber.
+
+  @retval EFI_SUCCESS           Mode information returned.
+  @retval EFI_BUFFER_TOO_SMALL  The Info buffer was too small.
+  @retval EFI_DEVICE_ERROR      A hardware error occurred trying to retrieve the video mode.
+  @retval EFI_INVALID_PARAMETER One of the input args was NULL.
+  @retval EFI_OUT_OF_RESOURCES  No resource available.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputQueryMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL          *This,
+  IN  UINT32                                ModeNumber,
+  OUT UINTN                                 *SizeOfInfo,
+  OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  **Info
+  );
+
+/**
+  Set the video device into the specified mode and clears the visible portions of
+  the output display to black.
+
+  @param  This                  The EFI_GRAPHICS_OUTPUT_PROTOCOL instance.
+  @param  ModeNumber            Abstraction that defines the current video mode.
+
+  @retval EFI_SUCCESS           The graphics mode specified by ModeNumber was selected.
+  @retval EFI_DEVICE_ERROR      The device had an error and could not complete the request.
+  @retval EFI_UNSUPPORTED       ModeNumber is not supported by this device.
+  @retval EFI_OUT_OF_RESOURCES  No resource available.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputSetMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL * This,
+  IN  UINT32                       ModeNumber
+  );
+
+/**
+  The following table defines actions for BltOperations.
+
+  EfiBltVideoFill - Write data from the  BltBuffer pixel (SourceX, SourceY)
+  directly to every pixel of the video display rectangle
+  (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height).
+  Only one pixel will be used from the BltBuffer. Delta is NOT used.
+  EfiBltVideoToBltBuffer - Read data from the video display rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) and place it in
+  the BltBuffer rectangle (DestinationX, DestinationY )
+  (DestinationX + Width, DestinationY + Height). If DestinationX or
+  DestinationY is not zero then Delta must be set to the length in bytes
+  of a row in the BltBuffer.
+  EfiBltBufferToVideo - Write data from the  BltBuffer rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) directly to the
+  video display rectangle (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height). If SourceX or SourceY is
+  not zero then Delta must be set to the length in bytes of a row in the
+  BltBuffer.
+  EfiBltVideoToVideo - Copy from the video display rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) .
+  to the video display rectangle (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height).
+  The BltBuffer and Delta  are not used in this mode.
+
+  @param  This                    Protocol instance pointer.
+  @param  BltBuffer               Buffer containing data to blit into video buffer.
+                                  This buffer has a size of
+                                  Width*Height*sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+  @param  BltOperation            Operation to perform on BlitBuffer and video
+                                  memory
+  @param  SourceX                 X coordinate of source for the BltBuffer.
+  @param  SourceY                 Y coordinate of source for the BltBuffer.
+  @param  DestinationX            X coordinate of destination for the BltBuffer.
+  @param  DestinationY            Y coordinate of destination for the BltBuffer.
+  @param  Width                   Width of rectangle in BltBuffer in pixels.
+  @param  Height                  Hight of rectangle in BltBuffer in pixels.
+  @param  Delta                   OPTIONAL.
+
+  @retval EFI_SUCCESS             The Blt operation completed.
+  @retval EFI_INVALID_PARAMETER   BltOperation is not valid.
+  @retval EFI_DEVICE_ERROR        A hardware error occured writting to the video
+                                  buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputBlt (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL                  *This,
+  IN  EFI_GRAPHICS_OUTPUT_BLT_PIXEL                 *BltBuffer, OPTIONAL
+  IN  EFI_GRAPHICS_OUTPUT_BLT_OPERATION             BltOperation,
+  IN  UINTN                                         SourceX,
+  IN  UINTN                                         SourceY,
+  IN  UINTN                                         DestinationX,
+  IN  UINTN                                         DestinationY,
+  IN  UINTN                                         Width,
+  IN  UINTN                                         Height,
+  IN  UINTN                                         Delta         OPTIONAL
+  );
+
+
+/**
+  Return the current video mode information.
+
+  @param  This                  The EFI_UGA_DRAW_PROTOCOL instance.
+  @param  HorizontalResolution  The size of video screen in pixels in the X dimension.
+  @param  VerticalResolution    The size of video screen in pixels in the Y dimension.
+  @param  ColorDepth            Number of bits per pixel, currently defined to be 32.
+  @param  RefreshRate           The refresh rate of the monitor in Hertz.
+
+  @retval EFI_SUCCESS           Mode information returned.
+  @retval EFI_NOT_STARTED       Video display is not initialized. Call SetMode ()
+  @retval EFI_INVALID_PARAMETER One of the input args was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawGetMode (
+  IN  EFI_UGA_DRAW_PROTOCOL           *This,
+  OUT UINT32                          *HorizontalResolution,
+  OUT UINT32                          *VerticalResolution,
+  OUT UINT32                          *ColorDepth,
+  OUT UINT32                          *RefreshRate
+  );
+
+/**
+  Set the current video mode information.
+
+  @param  This                 The EFI_UGA_DRAW_PROTOCOL instance.
+  @param  HorizontalResolution The size of video screen in pixels in the X dimension.
+  @param  VerticalResolution   The size of video screen in pixels in the Y dimension.
+  @param  ColorDepth           Number of bits per pixel, currently defined to be 32.
+  @param  RefreshRate          The refresh rate of the monitor in Hertz.
+
+  @retval EFI_SUCCESS          Mode information returned.
+  @retval EFI_NOT_STARTED      Video display is not initialized. Call SetMode ()
+  @retval EFI_OUT_OF_RESOURCES Out of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawSetMode (
+  IN  EFI_UGA_DRAW_PROTOCOL           *This,
+  IN UINT32                           HorizontalResolution,
+  IN UINT32                           VerticalResolution,
+  IN UINT32                           ColorDepth,
+  IN UINT32                           RefreshRate
+  );
+
+/**
+  Blt a rectangle of pixels on the graphics screen.
+
+  The following table defines actions for BltOperations.
+
+  EfiUgaVideoFill:
+    Write data from the  BltBuffer pixel (SourceX, SourceY)
+    directly to every pixel of the video display rectangle
+    (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height).
+    Only one pixel will be used from the BltBuffer. Delta is NOT used.
+  EfiUgaVideoToBltBuffer:
+    Read data from the video display rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) and place it in
+    the BltBuffer rectangle (DestinationX, DestinationY )
+    (DestinationX + Width, DestinationY + Height). If DestinationX or
+    DestinationY is not zero then Delta must be set to the length in bytes
+    of a row in the BltBuffer.
+  EfiUgaBltBufferToVideo:
+    Write data from the  BltBuffer rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) directly to the
+    video display rectangle (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height). If SourceX or SourceY is
+    not zero then Delta must be set to the length in bytes of a row in the
+    BltBuffer.
+  EfiUgaVideoToVideo:
+    Copy from the video display rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) .
+    to the video display rectangle (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height).
+    The BltBuffer and Delta  are not used in this mode.
+
+  @param  This           Protocol instance pointer.
+  @param  BltBuffer      Buffer containing data to blit into video buffer. This
+                         buffer has a size of Width*Height*sizeof(EFI_UGA_PIXEL)
+  @param  BltOperation   Operation to perform on BlitBuffer and video memory
+  @param  SourceX        X coordinate of source for the BltBuffer.
+  @param  SourceY        Y coordinate of source for the BltBuffer.
+  @param  DestinationX   X coordinate of destination for the BltBuffer.
+  @param  DestinationY   Y coordinate of destination for the BltBuffer.
+  @param  Width          Width of rectangle in BltBuffer in pixels.
+  @param  Height         Hight of rectangle in BltBuffer in pixels.
+  @param  Delta          OPTIONAL
+
+  @retval EFI_SUCCESS            The Blt operation completed.
+  @retval EFI_INVALID_PARAMETER  BltOperation is not valid.
+  @retval EFI_DEVICE_ERROR       A hardware error occured writting to the video buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawBlt (
+  IN  EFI_UGA_DRAW_PROTOCOL                         *This,
+  IN  EFI_UGA_PIXEL                                 *BltBuffer, OPTIONAL
+  IN  EFI_UGA_BLT_OPERATION                         BltOperation,
+  IN  UINTN                                         SourceX,
+  IN  UINTN                                         SourceY,
+  IN  UINTN                                         DestinationX,
+  IN  UINTN                                         DestinationY,
+  IN  UINTN                                         Width,
+  IN  UINTN                                         Height,
+  IN  UINTN                                         Delta         OPTIONAL
+  );
+
+/**
+  Sets the output device(s) to a specified mode.
+
+  @param  Private                 Text Out Splitter pointer.
+  @param  ModeNumber              The mode number to set.
+
+**/
+VOID
+TextOutSetMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private,
+  IN  UINTN                           ModeNumber
+  );
+
+/**
+  An empty function to pass error checking of CreateEventEx ().
+
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               Pointer to the notification function's context,
+                                which is implementation-dependent.
+
+**/
+VOID
+EFIAPI
+ConSplitterEmptyCallbackFunction (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.inf b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.inf
new file mode 100644
index 0000000000..ff10a50b50
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.inf
@@ -0,0 +1,119 @@
+## @file
+# This driver provides multi console supports.
+#
+# This driver acts as a virtual console, takes over the console I/O control from selected
+# standard console devices, and transmits console I/O to related console device drivers.
+# Consplitter could install Graphics Output protocol and/or UGA Draw protocol in system
+# table according PCD settings(PcdConOutGopSupport, and PcdConOutUgaSupport). It always
+# consumes Graphics Output protocol which is produced by display device, and consumes UGA Draw
+# protocol which is produced by display device according to PcdUgaConsumeSupport value.
+# Note: If only UGA Draw protocol is installed in system, PcdUgaConsumeSupport should be
+# set to TRUE.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ConSplitterDxe
+  MODULE_UNI_FILE                = ConSplitterDxe.uni
+  FILE_GUID                      = 408edcec-cf6d-477c-a5a8-b4844e3de281
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = ConSplitterDriverEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gConSplitterConInDriverBinding
+#  COMPONENT_NAME                =  gConSplitterConInComponentName
+#  COMPONENT_NAME2               =  gConSplitterConInComponentName2
+#  DRIVER_BINDING                =  gConSplitterSimplePointerDriverBinding
+#  COMPONENT_NAME                =  gConSplitterSimplePointerComponentName
+#  COMPONENT_NAME2               =  gConSplitterSimplePointerComponentName2
+#  DRIVER_BINDING                =  gConSplitterConOutDriverBinding
+#  COMPONENT_NAME                =  gConSplitterConOutComponentName
+#  COMPONENT_NAME2               =  gConSplitterConOutComponentName2
+#  DRIVER_BINDING                =  gConSplitterStdErrDriverBinding
+#  COMPONENT_NAME                =  gConSplitterStdErrComponentName
+#  COMPONENT_NAME2               =  gConSplitterStdErrComponentName2
+#
+
+[Sources]
+  ConSplitterGraphics.c
+  ComponentName.c
+  ConSplitter.h
+  ConSplitter.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  PcdLib
+
+[Guids]
+  gEfiConsoleInDeviceGuid                       ## SOMETIMES_CONSUMES ## UNDEFINED # protocol GUID installed on device handle
+  gEfiStandardErrorDeviceGuid                   ## SOMETIMES_CONSUMES ## UNDEFINED # protocol GUID installed on device handle
+  gEfiConsoleOutDeviceGuid                      ## SOMETIMES_CONSUMES ## UNDEFINED # protocol GUID installed on device handle
+  ## SOMETIMES_PRODUCES ## Event
+  ## SOMETIMES_CONSUMES ## Event
+  gConnectConInEventGuid
+
+[Protocols]
+  ## PRODUCES
+  ## TO_START
+  gEfiSimplePointerProtocolGuid
+  ## PRODUCES
+  ## TO_START
+  gEfiAbsolutePointerProtocolGuid
+  ## PRODUCES
+  ## TO_START
+  gEfiSimpleTextInProtocolGuid
+  ## PRODUCES
+  ## TO_START
+  gEfiSimpleTextInputExProtocolGuid
+  ## PRODUCES
+  ## TO_START 
+  gEfiSimpleTextOutProtocolGuid
+  ## SOMETIMES_PRODUCES
+  ## SOMETIMES_CONSUMES
+  gEfiGraphicsOutputProtocolGuid
+  ## SOMETIMES_PRODUCES
+  ## SOMETIMES_CONSUMES
+  gEfiUgaDrawProtocolGuid
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutGopSupport   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutUgaSupport   ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUgaConsumeSupport        ## CONSUMES
+
+[Pcd]
+  ## SOMETIMES_PRODUCES
+  ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutRow
+  ## SOMETIMES_PRODUCES
+  ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConOutColumn
+  gEfiMdeModulePkgTokenSpaceGuid.PcdConInConnectOnDemand  ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ConSplitterDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.uni b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.uni
new file mode 100644
index 0000000000..cb8037d71b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxeExtra.uni b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxeExtra.uni
new file mode 100644
index 0000000000..86515ff49d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterGraphics.c b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterGraphics.c
new file mode 100644
index 0000000000..b7cdd7af39
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/ConSplitterDxe/ConSplitterGraphics.c
@@ -0,0 +1,628 @@
+/** @file
+  Support for Graphics output spliter.
+  
+Copyright (c) 2006 - 2011, 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 "ConSplitter.h"
+
+
+CHAR16 mCrLfString[3] = { CHAR_CARRIAGE_RETURN, CHAR_LINEFEED, CHAR_NULL };
+
+/**
+  Returns information for an available graphics mode that the graphics device
+  and the set of active video output devices supports.
+
+  @param  This                  The EFI_GRAPHICS_OUTPUT_PROTOCOL instance.
+  @param  ModeNumber            The mode number to return information on.
+  @param  SizeOfInfo            A pointer to the size, in bytes, of the Info buffer.
+  @param  Info                  A pointer to callee allocated buffer that returns information about ModeNumber.
+
+  @retval EFI_SUCCESS           Mode information returned.
+  @retval EFI_BUFFER_TOO_SMALL  The Info buffer was too small.
+  @retval EFI_DEVICE_ERROR      A hardware error occurred trying to retrieve the video mode.
+  @retval EFI_INVALID_PARAMETER One of the input args was NULL.
+  @retval EFI_OUT_OF_RESOURCES  No resource available.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputQueryMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL          *This,
+  IN  UINT32                                ModeNumber,
+  OUT UINTN                                 *SizeOfInfo,
+  OUT EFI_GRAPHICS_OUTPUT_MODE_INFORMATION  **Info
+  )
+{
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput;
+  EFI_STATUS                      Status;
+  UINTN                           Index;
+
+  if (This == NULL || Info == NULL || SizeOfInfo == NULL || ModeNumber >= This->Mode->MaxMode) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // retrieve private data
+  //
+  Private = GRAPHICS_OUTPUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  GraphicsOutput = NULL;
+  
+  if (Private->CurrentNumberOfGraphicsOutput == 1) {
+    //
+    // Find the only one GraphicsOutput.
+    //
+    for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+      GraphicsOutput = Private->TextOutList[Index].GraphicsOutput;
+      if (GraphicsOutput != NULL) {
+        break;
+      }
+    }
+  }
+  
+  if (GraphicsOutput != NULL) {
+    //
+    // If only one physical GOP device exist, return its information.
+    //
+    Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) ModeNumber, SizeOfInfo, Info);
+    return Status;
+  } else {
+    //
+    // If 2 more phyiscal GOP device exist or GOP protocol does not exist, 
+    // return GOP information (PixelFormat is PixelBltOnly) created in ConSplitterAddGraphicsOutputMode ().
+    //
+    *Info = AllocatePool (sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION));
+    if (*Info == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    *SizeOfInfo = sizeof (EFI_GRAPHICS_OUTPUT_MODE_INFORMATION);
+    CopyMem (*Info, &Private->GraphicsOutputModeBuffer[ModeNumber], *SizeOfInfo);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Set the video device into the specified mode and clears the visible portions of
+  the output display to black.
+
+  @param  This                  The EFI_GRAPHICS_OUTPUT_PROTOCOL instance.
+  @param  ModeNumber            Abstraction that defines the current video mode.
+
+  @retval EFI_SUCCESS           The graphics mode specified by ModeNumber was selected.
+  @retval EFI_DEVICE_ERROR      The device had an error and could not complete the request.
+  @retval EFI_UNSUPPORTED       ModeNumber is not supported by this device.
+  @retval EFI_OUT_OF_RESOURCES  No resource available.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputSetMode (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL * This,
+  IN  UINT32                       ModeNumber
+  )
+{
+  EFI_STATUS                             Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA         *Private;
+  UINTN                                  Index;
+  EFI_STATUS                             ReturnStatus;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION   *Mode;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL           *GraphicsOutput;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL           *PhysicalGraphicsOutput;
+  UINTN                                  NumberIndex;
+  UINTN                                  SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION   *Info;
+  EFI_UGA_DRAW_PROTOCOL                  *UgaDraw;
+
+  if (ModeNumber >= This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Private = GRAPHICS_OUTPUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+  Mode = &Private->GraphicsOutputModeBuffer[ModeNumber];
+
+  ReturnStatus = EFI_SUCCESS;
+  GraphicsOutput = NULL;
+  PhysicalGraphicsOutput = NULL;
+  //
+  // return the worst status met
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    GraphicsOutput = Private->TextOutList[Index].GraphicsOutput;
+    if (GraphicsOutput != NULL) {
+      PhysicalGraphicsOutput = GraphicsOutput;
+      //
+      // Find corresponding ModeNumber of this GraphicsOutput instance
+      //
+      for (NumberIndex = 0; NumberIndex < GraphicsOutput->Mode->MaxMode; NumberIndex ++) {
+        Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) NumberIndex, &SizeOfInfo, &Info);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((Info->HorizontalResolution == Mode->HorizontalResolution) && (Info->VerticalResolution == Mode->VerticalResolution)) {
+          FreePool (Info);
+          break;
+        }
+        FreePool (Info);
+      }
+
+      Status = GraphicsOutput->SetMode (GraphicsOutput, (UINT32) NumberIndex);
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      }
+    } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+      UgaDraw = Private->TextOutList[Index].UgaDraw;
+      if (UgaDraw != NULL) {
+        Status = UgaDraw->SetMode (
+                            UgaDraw,
+                            Mode->HorizontalResolution,
+                            Mode->VerticalResolution,
+                            32,
+                            60
+                            );
+        if (EFI_ERROR (Status)) {
+          ReturnStatus = Status;
+        }
+      }
+    }
+  }
+
+  This->Mode->Mode = ModeNumber;
+
+  if ((Private->CurrentNumberOfGraphicsOutput == 1) && (PhysicalGraphicsOutput != NULL)) {
+    //
+    // If only one physical GOP device exist, copy physical information to consplitter.
+    //
+    CopyMem (This->Mode->Info, PhysicalGraphicsOutput->Mode->Info, PhysicalGraphicsOutput->Mode->SizeOfInfo);
+    This->Mode->SizeOfInfo = PhysicalGraphicsOutput->Mode->SizeOfInfo;
+    This->Mode->FrameBufferBase = PhysicalGraphicsOutput->Mode->FrameBufferBase;
+    This->Mode->FrameBufferSize = PhysicalGraphicsOutput->Mode->FrameBufferSize;
+  } else {
+    //
+    // If 2 more phyiscal GOP device exist or GOP protocol does not exist, 
+    // return GOP information (PixelFormat is PixelBltOnly) created in ConSplitterAddGraphicsOutputMode ().
+    //
+    CopyMem (This->Mode->Info, &Private->GraphicsOutputModeBuffer[ModeNumber], This->Mode->SizeOfInfo);
+  }
+
+  return ReturnStatus;
+}
+
+
+
+/**
+  The following table defines actions for BltOperations.
+
+  EfiBltVideoFill - Write data from the  BltBuffer pixel (SourceX, SourceY)
+  directly to every pixel of the video display rectangle
+  (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height).
+  Only one pixel will be used from the BltBuffer. Delta is NOT used.
+  EfiBltVideoToBltBuffer - Read data from the video display rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) and place it in
+  the BltBuffer rectangle (DestinationX, DestinationY )
+  (DestinationX + Width, DestinationY + Height). If DestinationX or
+  DestinationY is not zero then Delta must be set to the length in bytes
+  of a row in the BltBuffer.
+  EfiBltBufferToVideo - Write data from the  BltBuffer rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) directly to the
+  video display rectangle (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height). If SourceX or SourceY is
+  not zero then Delta must be set to the length in bytes of a row in the
+  BltBuffer.
+  EfiBltVideoToVideo - Copy from the video display rectangle
+  (SourceX, SourceY) (SourceX + Width, SourceY + Height) .
+  to the video display rectangle (DestinationX, DestinationY)
+  (DestinationX + Width, DestinationY + Height).
+  The BltBuffer and Delta  are not used in this mode.
+
+  @param  This                    Protocol instance pointer.
+  @param  BltBuffer               Buffer containing data to blit into video buffer.
+                                  This buffer has a size of
+                                  Width*Height*sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+  @param  BltOperation            Operation to perform on BlitBuffer and video
+                                  memory
+  @param  SourceX                 X coordinate of source for the BltBuffer.
+  @param  SourceY                 Y coordinate of source for the BltBuffer.
+  @param  DestinationX            X coordinate of destination for the BltBuffer.
+  @param  DestinationY            Y coordinate of destination for the BltBuffer.
+  @param  Width                   Width of rectangle in BltBuffer in pixels.
+  @param  Height                  Hight of rectangle in BltBuffer in pixels.
+  @param  Delta                   OPTIONAL.
+
+  @retval EFI_SUCCESS             The Blt operation completed.
+  @retval EFI_INVALID_PARAMETER   BltOperation is not valid.
+  @retval EFI_DEVICE_ERROR        A hardware error occured writting to the video
+                                  buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterGraphicsOutputBlt (
+  IN  EFI_GRAPHICS_OUTPUT_PROTOCOL                  *This,
+  IN  EFI_GRAPHICS_OUTPUT_BLT_PIXEL                 *BltBuffer, OPTIONAL
+  IN  EFI_GRAPHICS_OUTPUT_BLT_OPERATION             BltOperation,
+  IN  UINTN                                         SourceX,
+  IN  UINTN                                         SourceY,
+  IN  UINTN                                         DestinationX,
+  IN  UINTN                                         DestinationY,
+  IN  UINTN                                         Width,
+  IN  UINTN                                         Height,
+  IN  UINTN                                         Delta         OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_STATUS                      ReturnStatus;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL           *UgaDraw;
+
+  if (This == NULL || ((UINTN) BltOperation) >= EfiGraphicsOutputBltOperationMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Private = GRAPHICS_OUTPUT_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  ReturnStatus = EFI_SUCCESS;
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    GraphicsOutput = Private->TextOutList[Index].GraphicsOutput;
+    if (GraphicsOutput != NULL) {
+      Status = GraphicsOutput->Blt (
+                              GraphicsOutput,
+                              BltBuffer,
+                              BltOperation,
+                              SourceX,
+                              SourceY,
+                              DestinationX,
+                              DestinationY,
+                              Width,
+                              Height,
+                              Delta
+                              );
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      } else if (BltOperation == EfiBltVideoToBltBuffer) {
+        //
+        // Only need to read the data into buffer one time
+        //
+        return EFI_SUCCESS;
+      }
+    }
+
+    UgaDraw = Private->TextOutList[Index].UgaDraw;
+    if (UgaDraw != NULL && FeaturePcdGet (PcdUgaConsumeSupport)) {
+      Status = UgaDraw->Blt (
+                              UgaDraw,
+                              (EFI_UGA_PIXEL *) BltBuffer,
+                              (EFI_UGA_BLT_OPERATION) BltOperation,
+                              SourceX,
+                              SourceY,
+                              DestinationX,
+                              DestinationY,
+                              Width,
+                              Height,
+                              Delta
+                              );
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      } else if (BltOperation == EfiBltVideoToBltBuffer) {
+        //
+        // Only need to read the data into buffer one time
+        //
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return ReturnStatus;
+}
+
+/**
+  Return the current video mode information.
+
+  @param  This                  The EFI_UGA_DRAW_PROTOCOL instance.
+  @param  HorizontalResolution  The size of video screen in pixels in the X dimension.
+  @param  VerticalResolution    The size of video screen in pixels in the Y dimension.
+  @param  ColorDepth            Number of bits per pixel, currently defined to be 32.
+  @param  RefreshRate           The refresh rate of the monitor in Hertz.
+
+  @retval EFI_SUCCESS           Mode information returned.
+  @retval EFI_NOT_STARTED       Video display is not initialized. Call SetMode ()
+  @retval EFI_INVALID_PARAMETER One of the input args was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawGetMode (
+  IN  EFI_UGA_DRAW_PROTOCOL           *This,
+  OUT UINT32                          *HorizontalResolution,
+  OUT UINT32                          *VerticalResolution,
+  OUT UINT32                          *ColorDepth,
+  OUT UINT32                          *RefreshRate
+  )
+{
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+
+  if ((HorizontalResolution == NULL) ||
+      (VerticalResolution   == NULL) ||
+      (RefreshRate          == NULL) ||
+      (ColorDepth           == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // retrieve private data
+  //
+  Private               = UGA_DRAW_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  *HorizontalResolution = Private->UgaHorizontalResolution;
+  *VerticalResolution   = Private->UgaVerticalResolution;
+  *ColorDepth           = Private->UgaColorDepth;
+  *RefreshRate          = Private->UgaRefreshRate;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Set the current video mode information.
+
+  @param  This                 The EFI_UGA_DRAW_PROTOCOL instance.
+  @param  HorizontalResolution The size of video screen in pixels in the X dimension.
+  @param  VerticalResolution   The size of video screen in pixels in the Y dimension.
+  @param  ColorDepth           Number of bits per pixel, currently defined to be 32.
+  @param  RefreshRate          The refresh rate of the monitor in Hertz.
+
+  @retval EFI_SUCCESS          Mode information returned.
+  @retval EFI_NOT_STARTED      Video display is not initialized. Call SetMode ()
+  @retval EFI_OUT_OF_RESOURCES Out of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawSetMode (
+  IN  EFI_UGA_DRAW_PROTOCOL           *This,
+  IN UINT32                           HorizontalResolution,
+  IN UINT32                           VerticalResolution,
+  IN UINT32                           ColorDepth,
+  IN UINT32                           RefreshRate
+  )
+{
+  EFI_STATUS                             Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA         *Private;
+  UINTN                                  Index;
+  EFI_STATUS                             ReturnStatus;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL           *GraphicsOutput;
+  UINTN                                  NumberIndex;
+  UINTN                                  SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION   *Info;
+  EFI_UGA_DRAW_PROTOCOL                  *UgaDraw;
+
+  Private = UGA_DRAW_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  ReturnStatus = EFI_SUCCESS;
+
+  //
+  // Update the Mode data
+  //
+  Private->UgaHorizontalResolution  = HorizontalResolution;
+  Private->UgaVerticalResolution    = VerticalResolution;
+  Private->UgaColorDepth            = ColorDepth;
+  Private->UgaRefreshRate           = RefreshRate;
+
+  //
+  // return the worst status met
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+
+    GraphicsOutput = Private->TextOutList[Index].GraphicsOutput;
+    if (GraphicsOutput != NULL) {
+      //
+      // Find corresponding ModeNumber of this GraphicsOutput instance
+      //
+      for (NumberIndex = 0; NumberIndex < GraphicsOutput->Mode->MaxMode; NumberIndex ++) {
+        Status = GraphicsOutput->QueryMode (GraphicsOutput, (UINT32) NumberIndex, &SizeOfInfo, &Info);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        if ((Info->HorizontalResolution == HorizontalResolution) && (Info->VerticalResolution == VerticalResolution)) {
+          FreePool (Info);
+          break;
+        }
+        FreePool (Info);
+      }
+
+      Status = GraphicsOutput->SetMode (GraphicsOutput, (UINT32) NumberIndex);
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      }
+    } else if (FeaturePcdGet (PcdUgaConsumeSupport)){
+      UgaDraw = Private->TextOutList[Index].UgaDraw;
+      if (UgaDraw != NULL) {
+        Status = UgaDraw->SetMode (
+                          UgaDraw,
+                          HorizontalResolution,
+                          VerticalResolution,
+                          ColorDepth,
+                          RefreshRate
+                          );
+        if (EFI_ERROR (Status)) {
+          ReturnStatus = Status;
+        }
+      }
+    }
+  }
+
+  return ReturnStatus;
+}
+
+
+/**
+  Blt a rectangle of pixels on the graphics screen.
+
+  The following table defines actions for BltOperations.
+
+  EfiUgaVideoFill:
+    Write data from the  BltBuffer pixel (SourceX, SourceY)
+    directly to every pixel of the video display rectangle
+    (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height).
+    Only one pixel will be used from the BltBuffer. Delta is NOT used.
+  EfiUgaVideoToBltBuffer:
+    Read data from the video display rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) and place it in
+    the BltBuffer rectangle (DestinationX, DestinationY )
+    (DestinationX + Width, DestinationY + Height). If DestinationX or
+    DestinationY is not zero then Delta must be set to the length in bytes
+    of a row in the BltBuffer.
+  EfiUgaBltBufferToVideo:
+    Write data from the  BltBuffer rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) directly to the
+    video display rectangle (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height). If SourceX or SourceY is
+    not zero then Delta must be set to the length in bytes of a row in the
+    BltBuffer.
+  EfiUgaVideoToVideo:
+    Copy from the video display rectangle
+    (SourceX, SourceY) (SourceX + Width, SourceY + Height) .
+    to the video display rectangle (DestinationX, DestinationY)
+    (DestinationX + Width, DestinationY + Height).
+    The BltBuffer and Delta  are not used in this mode.
+
+  @param  This           Protocol instance pointer.
+  @param  BltBuffer      Buffer containing data to blit into video buffer. This
+                         buffer has a size of Width*Height*sizeof(EFI_UGA_PIXEL)
+  @param  BltOperation   Operation to perform on BlitBuffer and video memory
+  @param  SourceX        X coordinate of source for the BltBuffer.
+  @param  SourceY        Y coordinate of source for the BltBuffer.
+  @param  DestinationX   X coordinate of destination for the BltBuffer.
+  @param  DestinationY   Y coordinate of destination for the BltBuffer.
+  @param  Width          Width of rectangle in BltBuffer in pixels.
+  @param  Height         Hight of rectangle in BltBuffer in pixels.
+  @param  Delta          OPTIONAL
+
+  @retval EFI_SUCCESS            The Blt operation completed.
+  @retval EFI_INVALID_PARAMETER  BltOperation is not valid.
+  @retval EFI_DEVICE_ERROR       A hardware error occured writting to the video buffer.
+
+**/
+EFI_STATUS
+EFIAPI
+ConSplitterUgaDrawBlt (
+  IN  EFI_UGA_DRAW_PROTOCOL                         *This,
+  IN  EFI_UGA_PIXEL                                 *BltBuffer, OPTIONAL
+  IN  EFI_UGA_BLT_OPERATION                         BltOperation,
+  IN  UINTN                                         SourceX,
+  IN  UINTN                                         SourceY,
+  IN  UINTN                                         DestinationX,
+  IN  UINTN                                         DestinationY,
+  IN  UINTN                                         Width,
+  IN  UINTN                                         Height,
+  IN  UINTN                                         Delta         OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private;
+  UINTN                           Index;
+  EFI_STATUS                      ReturnStatus;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput;
+
+  Private = UGA_DRAW_SPLITTER_PRIVATE_DATA_FROM_THIS (This);
+
+  ReturnStatus = EFI_SUCCESS;
+  //
+  // return the worst status met
+  //
+  for (Index = 0; Index < Private->CurrentNumberOfConsoles; Index++) {
+    GraphicsOutput = Private->TextOutList[Index].GraphicsOutput;
+    if (GraphicsOutput != NULL) {
+      Status = GraphicsOutput->Blt (
+                              GraphicsOutput,
+                              (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) BltBuffer,
+                              (EFI_GRAPHICS_OUTPUT_BLT_OPERATION) BltOperation,
+                              SourceX,
+                              SourceY,
+                              DestinationX,
+                              DestinationY,
+                              Width,
+                              Height,
+                              Delta
+                              );
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      } else if (BltOperation == EfiUgaVideoToBltBuffer) {
+        //
+        // Only need to read the data into buffer one time
+        //
+        return EFI_SUCCESS;
+      }
+    }
+
+    if (Private->TextOutList[Index].UgaDraw != NULL && FeaturePcdGet (PcdUgaConsumeSupport)) {
+      Status = Private->TextOutList[Index].UgaDraw->Blt (
+                                                      Private->TextOutList[Index].UgaDraw,
+                                                      BltBuffer,
+                                                      BltOperation,
+                                                      SourceX,
+                                                      SourceY,
+                                                      DestinationX,
+                                                      DestinationY,
+                                                      Width,
+                                                      Height,
+                                                      Delta
+                                                      );
+      if (EFI_ERROR (Status)) {
+        ReturnStatus = Status;
+      } else if (BltOperation == EfiUgaVideoToBltBuffer) {
+        //
+        // Only need to read the data into buffer one time
+        //
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return ReturnStatus;
+}
+
+/**
+  Sets the output device(s) to a specified mode.
+
+  @param  Private                 Text Out Splitter pointer.
+  @param  ModeNumber              The mode number to set.
+
+**/
+VOID
+TextOutSetMode (
+  IN  TEXT_OUT_SPLITTER_PRIVATE_DATA  *Private,
+  IN  UINTN                           ModeNumber
+  )
+{
+  //
+  // No need to do extra check here as whether (Column, Row) is valid has
+  // been checked in ConSplitterTextOutSetCursorPosition. And (0, 0) should
+  // always be supported.
+  //
+  Private->TextOutMode.Mode          = (INT32) ModeNumber;
+  Private->TextOutMode.CursorColumn  = 0;
+  Private->TextOutMode.CursorRow     = 0;
+  Private->TextOutMode.CursorVisible = TRUE;
+
+  return;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/ComponentName.c
new file mode 100644
index 0000000000..13be3a1b67
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/ComponentName.c
@@ -0,0 +1,182 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for GraphicsConsole driver.
+
+Copyright (c) 2006 - 2012, 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 "GraphicsConsole.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gGraphicsConsoleComponentName = {
+  GraphicsConsoleComponentNameGetDriverName,
+  GraphicsConsoleComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gGraphicsConsoleComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) GraphicsConsoleComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) GraphicsConsoleComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mGraphicsConsoleDriverNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *)L"Graphics Console Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mGraphicsConsoleDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gGraphicsConsoleComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.c b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.c
new file mode 100644
index 0000000000..74218c8b4e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.c
@@ -0,0 +1,2118 @@
+/** @file
+  This is the main routine for initializing the Graphics Console support routines.
+
+Copyright (c) 2006 - 2014, 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 "GraphicsConsole.h"
+
+//
+// Graphics Console Device Private Data template
+//
+GRAPHICS_CONSOLE_DEV    mGraphicsConsoleDevTemplate = {
+  GRAPHICS_CONSOLE_DEV_SIGNATURE,
+  (EFI_GRAPHICS_OUTPUT_PROTOCOL *) NULL,
+  (EFI_UGA_DRAW_PROTOCOL *) NULL,
+  {
+    GraphicsConsoleConOutReset,
+    GraphicsConsoleConOutOutputString,
+    GraphicsConsoleConOutTestString,
+    GraphicsConsoleConOutQueryMode,
+    GraphicsConsoleConOutSetMode,
+    GraphicsConsoleConOutSetAttribute,
+    GraphicsConsoleConOutClearScreen,
+    GraphicsConsoleConOutSetCursorPosition,
+    GraphicsConsoleConOutEnableCursor,
+    (EFI_SIMPLE_TEXT_OUTPUT_MODE *) NULL
+  },
+  {
+    0,
+    -1,
+    EFI_TEXT_ATTR(EFI_LIGHTGRAY, EFI_BLACK),
+    0,
+    0,
+    TRUE
+  },
+  (GRAPHICS_CONSOLE_MODE_DATA *) NULL,
+  (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) NULL
+};
+
+GRAPHICS_CONSOLE_MODE_DATA mGraphicsConsoleModeData[] = {
+  {100, 31},
+  //
+  // New modes can be added here.
+  // The last entry is specific for full screen mode.
+  //
+  {0, 0}
+};
+
+EFI_HII_DATABASE_PROTOCOL   *mHiiDatabase;
+EFI_HII_FONT_PROTOCOL       *mHiiFont;
+EFI_HII_HANDLE              mHiiHandle;
+VOID                        *mHiiRegistration;
+
+EFI_GUID             mFontPackageListGuid = {0xf5f219d3, 0x7006, 0x4648, {0xac, 0x8d, 0xd6, 0x1d, 0xfb, 0x7b, 0xc6, 0xad}};
+
+CHAR16               mCrLfString[3] = { CHAR_CARRIAGE_RETURN, CHAR_LINEFEED, CHAR_NULL };
+
+EFI_GRAPHICS_OUTPUT_BLT_PIXEL        mGraphicsEfiColors[16] = {
+  //
+  // B    G    R   reserved
+  //
+  {0x00, 0x00, 0x00, 0x00},  // BLACK
+  {0x98, 0x00, 0x00, 0x00},  // LIGHTBLUE
+  {0x00, 0x98, 0x00, 0x00},  // LIGHGREEN
+  {0x98, 0x98, 0x00, 0x00},  // LIGHCYAN
+  {0x00, 0x00, 0x98, 0x00},  // LIGHRED
+  {0x98, 0x00, 0x98, 0x00},  // MAGENTA
+  {0x00, 0x98, 0x98, 0x00},  // BROWN
+  {0x98, 0x98, 0x98, 0x00},  // LIGHTGRAY
+  {0x30, 0x30, 0x30, 0x00},  // DARKGRAY - BRIGHT BLACK
+  {0xff, 0x00, 0x00, 0x00},  // BLUE
+  {0x00, 0xff, 0x00, 0x00},  // LIME
+  {0xff, 0xff, 0x00, 0x00},  // CYAN
+  {0x00, 0x00, 0xff, 0x00},  // RED
+  {0xff, 0x00, 0xff, 0x00},  // FUCHSIA
+  {0x00, 0xff, 0xff, 0x00},  // YELLOW
+  {0xff, 0xff, 0xff, 0x00}   // WHITE
+};
+
+EFI_NARROW_GLYPH     mCursorGlyph = {
+  0x0000,
+  0x00,
+  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF }
+};
+
+CHAR16       SpaceStr[] = { NARROW_CHAR, ' ', 0 };
+
+EFI_DRIVER_BINDING_PROTOCOL gGraphicsConsoleDriverBinding = {
+  GraphicsConsoleControllerDriverSupported,
+  GraphicsConsoleControllerDriverStart,
+  GraphicsConsoleControllerDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Test to see if Graphics Console could be supported on the Controller.
+
+  Graphics Console could be supported if Graphics Output Protocol or UGA Draw
+  Protocol exists on the Controller. (UGA Draw Protocol could be skipped
+  if PcdUgaConsumeSupport is set to FALSE.)
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL        *UgaDraw;
+  EFI_DEVICE_PATH_PROTOCOL     *DevicePath;
+
+  GraphicsOutput = NULL;
+  UgaDraw        = NULL;
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiGraphicsOutputProtocolGuid,
+                  (VOID **) &GraphicsOutput,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status) && FeaturePcdGet (PcdUgaConsumeSupport)) {
+    //
+    // Open Graphics Output Protocol failed, try to open UGA Draw Protocol
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUgaDrawProtocolGuid,
+                    (VOID **) &UgaDraw,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // We need to ensure that we do not layer on top of a virtual handle.
+  // We need to ensure that the handles produced by the conspliter do not
+  // get used.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  } else {
+    goto Error;
+  }
+
+  //
+  // Does Hii Exist?  If not, we aren't ready to run
+  //
+  Status = EfiLocateHiiProtocol ();
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+Error:
+  if (GraphicsOutput != NULL) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiGraphicsOutputProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiUgaDrawProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+  }
+  return Status;
+}
+
+/**
+  Initialize all the text modes which the graphics console supports.
+
+  It returns information for available text modes that the graphics can support.
+
+  @param[in]  HorizontalResolution     The size of video screen in pixels in the X dimension.
+  @param[in]  VerticalResolution       The size of video screen in pixels in the Y dimension.
+  @param[in]  GopModeNumber            The graphics mode number which graphis console is based on.
+  @param[out] TextModeCount            The total number of text modes that graphics console supports.
+  @param[out] TextModeData             The buffer to the text modes column and row information.
+                                       Caller is responsible to free it when it's non-NULL.
+
+  @retval EFI_SUCCESS                  The supporting mode information is returned.
+  @retval EFI_INVALID_PARAMETER        The parameters are invalid.
+
+**/
+EFI_STATUS
+InitializeGraphicsConsoleTextMode (
+  IN UINT32                        HorizontalResolution,
+  IN UINT32                        VerticalResolution,
+  IN UINT32                        GopModeNumber,
+  OUT UINTN                        *TextModeCount,
+  OUT GRAPHICS_CONSOLE_MODE_DATA   **TextModeData
+  )
+{
+  UINTN                       Index;
+  UINTN                       Count;
+  GRAPHICS_CONSOLE_MODE_DATA  *ModeBuffer;
+  GRAPHICS_CONSOLE_MODE_DATA  *NewModeBuffer;
+  UINTN                       ValidCount;
+  UINTN                       ValidIndex;
+  UINTN                       MaxColumns;
+  UINTN                       MaxRows;  
+  
+  if ((TextModeCount == NULL) || (TextModeData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Count = sizeof (mGraphicsConsoleModeData) / sizeof (GRAPHICS_CONSOLE_MODE_DATA);
+
+  //
+  // Compute the maximum number of text Rows and Columns that this current graphics mode can support.
+  // To make graphics console work well, MaxColumns and MaxRows should not be zero.
+  //
+  MaxColumns = HorizontalResolution / EFI_GLYPH_WIDTH;
+  MaxRows    = VerticalResolution / EFI_GLYPH_HEIGHT;
+
+  //
+  // According to UEFI spec, all output devices support at least 80x25 text mode.
+  //
+  ASSERT ((MaxColumns >= 80) && (MaxRows >= 25));
+
+  //
+  // Add full screen mode to the last entry.
+  //
+  mGraphicsConsoleModeData[Count - 1].Columns = MaxColumns;
+  mGraphicsConsoleModeData[Count - 1].Rows    = MaxRows;
+
+  //
+  // Get defined mode buffer pointer.
+  //
+  ModeBuffer = mGraphicsConsoleModeData;
+
+  //
+  // Here we make sure that the final mode exposed does not include the duplicated modes,
+  // and does not include the invalid modes which exceed the max column and row.
+  // Reserve 2 modes for 80x25, 80x50 of graphics console.
+  //
+  NewModeBuffer = AllocateZeroPool (sizeof (GRAPHICS_CONSOLE_MODE_DATA) * (Count + 2));
+  ASSERT (NewModeBuffer != NULL);
+
+  //
+  // Mode 0 and mode 1 is for 80x25, 80x50 according to UEFI spec.
+  //
+  ValidCount = 0;  
+
+  NewModeBuffer[ValidCount].Columns       = 80;
+  NewModeBuffer[ValidCount].Rows          = 25;
+  NewModeBuffer[ValidCount].GopWidth      = HorizontalResolution;
+  NewModeBuffer[ValidCount].GopHeight     = VerticalResolution;
+  NewModeBuffer[ValidCount].GopModeNumber = GopModeNumber;
+  NewModeBuffer[ValidCount].DeltaX        = (HorizontalResolution - (NewModeBuffer[ValidCount].Columns * EFI_GLYPH_WIDTH)) >> 1;
+  NewModeBuffer[ValidCount].DeltaY        = (VerticalResolution - (NewModeBuffer[ValidCount].Rows * EFI_GLYPH_HEIGHT)) >> 1;      
+  ValidCount++;
+
+  if ((MaxColumns >= 80) && (MaxRows >= 50)) {
+    NewModeBuffer[ValidCount].Columns = 80;
+    NewModeBuffer[ValidCount].Rows    = 50;
+    NewModeBuffer[ValidCount].DeltaX  = (HorizontalResolution - (80 * EFI_GLYPH_WIDTH)) >> 1;
+    NewModeBuffer[ValidCount].DeltaY  = (VerticalResolution - (50 * EFI_GLYPH_HEIGHT)) >> 1;    
+  }
+  NewModeBuffer[ValidCount].GopWidth      = HorizontalResolution;
+  NewModeBuffer[ValidCount].GopHeight     = VerticalResolution;
+  NewModeBuffer[ValidCount].GopModeNumber = GopModeNumber;
+  ValidCount++;
+  
+  //
+  // Start from mode 2 to put the valid mode other than 80x25 and 80x50 in the output mode buffer.
+  //
+  for (Index = 0; Index < Count; Index++) {
+    if ((ModeBuffer[Index].Columns == 0) || (ModeBuffer[Index].Rows == 0) ||
+        (ModeBuffer[Index].Columns > MaxColumns) || (ModeBuffer[Index].Rows > MaxRows)) {
+      //
+      // Skip the pre-defined mode which is invalid or exceeds the max column and row.
+      //
+      continue;
+    }
+    for (ValidIndex = 0; ValidIndex < ValidCount; ValidIndex++) {
+      if ((ModeBuffer[Index].Columns == NewModeBuffer[ValidIndex].Columns) &&
+          (ModeBuffer[Index].Rows == NewModeBuffer[ValidIndex].Rows)) {
+        //
+        // Skip the duplicated mode.
+        //
+        break;
+      }
+    }
+    if (ValidIndex == ValidCount) {
+      NewModeBuffer[ValidCount].Columns       = ModeBuffer[Index].Columns;
+      NewModeBuffer[ValidCount].Rows          = ModeBuffer[Index].Rows;
+      NewModeBuffer[ValidCount].GopWidth      = HorizontalResolution;
+      NewModeBuffer[ValidCount].GopHeight     = VerticalResolution;
+      NewModeBuffer[ValidCount].GopModeNumber = GopModeNumber;
+      NewModeBuffer[ValidCount].DeltaX        = (HorizontalResolution - (NewModeBuffer[ValidCount].Columns * EFI_GLYPH_WIDTH)) >> 1;
+      NewModeBuffer[ValidCount].DeltaY        = (VerticalResolution - (NewModeBuffer[ValidCount].Rows * EFI_GLYPH_HEIGHT)) >> 1;
+      ValidCount++;
+    }
+  }
+ 
+  DEBUG_CODE (
+    for (Index = 0; Index < ValidCount; Index++) {
+      DEBUG ((EFI_D_INFO, "Graphics - Mode %d, Column = %d, Row = %d\n", 
+                           Index, NewModeBuffer[Index].Columns, NewModeBuffer[Index].Rows));  
+    }
+  );
+  
+  //
+  // Return valid mode count and mode information buffer.
+  //
+  *TextModeCount = ValidCount;
+  *TextModeData  = NewModeBuffer;
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on Controller by opening Graphics Output protocol or
+  UGA Draw protocol, and installing Simple Text Out protocol on Controller.
+  (UGA Draw protocol could be skipped if PcdUgaConsumeSupport is set to FALSE.)
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to Controller.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                           Status;
+  GRAPHICS_CONSOLE_DEV                 *Private;
+  UINT32                               HorizontalResolution;
+  UINT32                               VerticalResolution;
+  UINT32                               ColorDepth;
+  UINT32                               RefreshRate;
+  UINT32                               ModeIndex;
+  UINTN                                MaxMode;
+  UINT32                               ModeNumber;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE    *Mode;
+  UINTN                                SizeOfInfo;  
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Info;
+  
+  ModeNumber = 0;
+
+  //
+  // Initialize the Graphics Console device instance
+  //
+  Private = AllocateCopyPool (
+              sizeof (GRAPHICS_CONSOLE_DEV),
+              &mGraphicsConsoleDevTemplate
+              );
+  if (Private == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->SimpleTextOutput.Mode = &(Private->SimpleTextOutputMode);
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiGraphicsOutputProtocolGuid,
+                  (VOID **) &Private->GraphicsOutput,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR(Status) && FeaturePcdGet (PcdUgaConsumeSupport)) {
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiUgaDrawProtocolGuid,
+                    (VOID **) &Private->UgaDraw,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Error;
+  }
+
+  HorizontalResolution  = PcdGet32 (PcdVideoHorizontalResolution);
+  VerticalResolution    = PcdGet32 (PcdVideoVerticalResolution);
+
+  if (Private->GraphicsOutput != NULL) {
+    //
+    // The console is build on top of Graphics Output Protocol, find the mode number
+    // for the user-defined mode; if there are multiple video devices,
+    // graphic console driver will set all the video devices to the same mode.
+    //
+    if ((HorizontalResolution == 0x0) || (VerticalResolution == 0x0)) {
+      //
+      // Find the highest resolution which GOP supports.
+      //    
+      MaxMode = Private->GraphicsOutput->Mode->MaxMode;
+      
+      for (ModeIndex = 0; ModeIndex < MaxMode; ModeIndex++) {
+        Status = Private->GraphicsOutput->QueryMode (
+                           Private->GraphicsOutput,
+                           ModeIndex,
+                           &SizeOfInfo,
+                           &Info
+                           );
+        if (!EFI_ERROR (Status)) {
+          if ((Info->HorizontalResolution > HorizontalResolution) ||
+              ((Info->HorizontalResolution == HorizontalResolution) && (Info->VerticalResolution > VerticalResolution))) {
+            HorizontalResolution = Info->HorizontalResolution;
+            VerticalResolution   = Info->VerticalResolution;
+            ModeNumber           = ModeIndex;
+          }
+          FreePool (Info);
+        }
+      }
+      if ((HorizontalResolution == 0x0) || (VerticalResolution == 0x0)) {
+        Status = EFI_UNSUPPORTED;
+        goto Error;
+      }
+    } else {
+      //
+      // Use user-defined resolution
+      //
+      Status = CheckModeSupported (
+                   Private->GraphicsOutput,
+                   HorizontalResolution,
+                   VerticalResolution,
+                   &ModeNumber
+                   );
+      if (EFI_ERROR (Status)) {
+        //
+        // if not supporting current mode, try 800x600 which is required by UEFI/EFI spec
+        //
+        Status = CheckModeSupported (
+                     Private->GraphicsOutput,
+                     800,
+                     600,
+                     &ModeNumber
+                     );
+        Mode = Private->GraphicsOutput->Mode;
+        if (EFI_ERROR (Status) && Mode->MaxMode != 0) {
+          //
+          // Set default mode failed or device don't support default mode, then get the current mode information
+          //
+          HorizontalResolution = Mode->Info->HorizontalResolution;
+          VerticalResolution = Mode->Info->VerticalResolution;
+          ModeNumber = Mode->Mode;
+        }
+      }
+    }
+    if (ModeNumber != Private->GraphicsOutput->Mode->Mode) {
+      //
+      // Current graphics mode is not set or is not set to the mode which we has found,
+      // set the new graphic mode.
+      //
+      Status = Private->GraphicsOutput->SetMode (Private->GraphicsOutput, ModeNumber);
+      if (EFI_ERROR (Status)) {
+        //
+        // The mode set operation failed
+        //
+        goto Error;
+      }
+    }
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    //
+    // At first try to set user-defined resolution
+    //
+    ColorDepth            = 32;
+    RefreshRate           = 60;
+    Status = Private->UgaDraw->SetMode (
+                                Private->UgaDraw,
+                                HorizontalResolution,
+                                VerticalResolution,
+                                ColorDepth,
+                                RefreshRate
+                                );
+    if (EFI_ERROR (Status)) {
+      //
+      // Try to set 800*600 which is required by UEFI/EFI spec
+      //
+      Status = Private->UgaDraw->SetMode (
+                                  Private->UgaDraw,
+                                  800,
+                                  600,
+                                  ColorDepth,
+                                  RefreshRate
+                                  );
+      if (EFI_ERROR (Status)) {
+        Status = Private->UgaDraw->GetMode (
+                                    Private->UgaDraw,
+                                    &HorizontalResolution,
+                                    &VerticalResolution,
+                                    &ColorDepth,
+                                    &RefreshRate
+                                    );
+        if (EFI_ERROR (Status)) {
+          goto Error;
+        }
+      }
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "GraphicsConsole video resolution %d x %d\n", HorizontalResolution, VerticalResolution));
+
+  //
+  // Initialize the mode which GraphicsConsole supports.
+  //
+  Status = InitializeGraphicsConsoleTextMode (
+             HorizontalResolution,
+             VerticalResolution,
+             ModeNumber,
+             &MaxMode,
+             &Private->ModeData
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto Error;
+  }
+
+  //
+  // Update the maximum number of modes
+  //
+  Private->SimpleTextOutputMode.MaxMode = (INT32) MaxMode;
+
+  DEBUG_CODE_BEGIN ();
+    Status = GraphicsConsoleConOutSetMode (&Private->SimpleTextOutput, 0);
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+    Status = GraphicsConsoleConOutOutputString (&Private->SimpleTextOutput, (CHAR16 *)L"Graphics Console Started\n\r");
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }  
+  DEBUG_CODE_END ();
+
+  //
+  // Install protocol interfaces for the Graphics Console device.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  &Private->SimpleTextOutput,
+                  NULL
+                  );
+
+Error:
+  if (EFI_ERROR (Status)) {
+    //
+    // Close the GOP and UGA Draw Protocol
+    //
+    if (Private->GraphicsOutput != NULL) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiGraphicsOutputProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+      gBS->CloseProtocol (
+             Controller,
+             &gEfiUgaDrawProtocolGuid,
+             This->DriverBindingHandle,
+             Controller
+             );
+    }
+
+    if (Private->LineBuffer != NULL) {
+      FreePool (Private->LineBuffer);
+    }
+
+    if (Private->ModeData != NULL) {
+      FreePool (Private->ModeData);
+    }
+
+    //
+    // Free private data
+    //
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+/**
+  Stop this driver on Controller by removing Simple Text Out protocol
+  and closing the Graphics Output Protocol or UGA Draw protocol on Controller.
+  (UGA Draw protocol could be skipped if PcdUgaConsumeSupport is set to FALSE.)
+
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed Controller.
+  @retval EFI_NOT_STARTED   Simple Text Out protocol could not be found the
+                            Controller.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *SimpleTextOutput;
+  GRAPHICS_CONSOLE_DEV             *Private;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  (VOID **) &SimpleTextOutput,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_STARTED;
+  }
+
+  Private = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (SimpleTextOutput);
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  &Private->SimpleTextOutput
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Close the GOP or UGA IO Protocol
+    //
+    if (Private->GraphicsOutput != NULL) {
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiGraphicsOutputProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiUgaDrawProtocolGuid,
+            This->DriverBindingHandle,
+            Controller
+            );
+    }
+
+    if (Private->LineBuffer != NULL) {
+      FreePool (Private->LineBuffer);
+    }
+
+    if (Private->ModeData != NULL) {
+      FreePool (Private->ModeData);
+    }
+
+    //
+    // Free our instance data
+    //
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+/**
+  Check if the current specific mode supported the user defined resolution
+  for the Graphics Console device based on Graphics Output Protocol.
+
+  If yes, set the graphic devcice's current mode to this specific mode.
+
+  @param  GraphicsOutput        Graphics Output Protocol instance pointer.
+  @param  HorizontalResolution  User defined horizontal resolution
+  @param  VerticalResolution    User defined vertical resolution.
+  @param  CurrentModeNumber     Current specific mode to be check.
+
+  @retval EFI_SUCCESS       The mode is supported.
+  @retval EFI_UNSUPPORTED   The specific mode is out of range of graphics
+                            device supported.
+  @retval other             The specific mode does not support user defined
+                            resolution or failed to set the current mode to the
+                            specific mode on graphics device.
+
+**/
+EFI_STATUS
+CheckModeSupported (
+  EFI_GRAPHICS_OUTPUT_PROTOCOL  *GraphicsOutput,
+  IN  UINT32                    HorizontalResolution,
+  IN  UINT32                    VerticalResolution,
+  OUT UINT32                    *CurrentModeNumber
+  )
+{
+  UINT32     ModeNumber;
+  EFI_STATUS Status;
+  UINTN      SizeOfInfo;
+  EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *Info;
+  UINT32     MaxMode;
+
+  Status  = EFI_SUCCESS;
+  MaxMode = GraphicsOutput->Mode->MaxMode;
+
+  for (ModeNumber = 0; ModeNumber < MaxMode; ModeNumber++) {
+    Status = GraphicsOutput->QueryMode (
+                       GraphicsOutput,
+                       ModeNumber,
+                       &SizeOfInfo,
+                       &Info
+                       );
+    if (!EFI_ERROR (Status)) {
+      if ((Info->HorizontalResolution == HorizontalResolution) &&
+          (Info->VerticalResolution == VerticalResolution)) {
+        if ((GraphicsOutput->Mode->Info->HorizontalResolution == HorizontalResolution) &&
+            (GraphicsOutput->Mode->Info->VerticalResolution == VerticalResolution)) {
+          //
+          // If video device has been set to this mode, we do not need to SetMode again
+          //
+          FreePool (Info);
+          break;
+        } else {
+          Status = GraphicsOutput->SetMode (GraphicsOutput, ModeNumber);
+          if (!EFI_ERROR (Status)) {
+            FreePool (Info);
+            break;
+          }
+        }
+      }
+      FreePool (Info);
+    }
+  }
+
+  if (ModeNumber == GraphicsOutput->Mode->MaxMode) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  *CurrentModeNumber = ModeNumber;
+  return Status;
+}
+
+
+/**
+  Locate HII Database protocol and HII Font protocol.
+
+  @retval  EFI_SUCCESS     HII Database protocol and HII Font protocol
+                           are located successfully.
+  @return  other           Failed to locate HII Database protocol or
+                           HII Font protocol.
+
+**/
+EFI_STATUS
+EfiLocateHiiProtocol (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->LocateProtocol (&gEfiHiiDatabaseProtocolGuid, NULL, (VOID **) &mHiiDatabase);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiHiiFontProtocolGuid, NULL, (VOID **) &mHiiFont);
+  return Status;
+}
+
+//
+// Body of the STO functions
+//
+
+/**
+  Reset the text output device hardware and optionally run diagnostics.
+
+  Implements SIMPLE_TEXT_OUTPUT.Reset().
+  If ExtendeVerification is TRUE, then perform dependent Graphics Console
+  device reset, and set display mode to mode 0.
+  If ExtendedVerification is FALSE, only set display mode to mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ExtendedVerification  Indicates that the driver may perform a more
+                                exhaustive verification operation of the device
+                                during reset.
+
+  @retval EFI_SUCCESS          The text output device was reset.
+  @retval EFI_DEVICE_ERROR     The text output device is not functioning correctly and
+                               could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          ExtendedVerification
+  )
+{
+  EFI_STATUS    Status;
+  Status = This->SetMode (This, 0);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = This->SetAttribute (This, EFI_TEXT_ATTR (This->Mode->Attribute & 0x0F, EFI_BACKGROUND_BLACK));
+  return Status;
+}
+
+
+/**
+  Write a Unicode string to the output device.
+
+  Implements SIMPLE_TEXT_OUTPUT.OutputString().
+  The Unicode string will be converted to Glyphs and will be
+  sent to the Graphics Console.
+
+  @param  This                    Protocol instance pointer.
+  @param  WString                 The NULL-terminated Unicode string to be displayed
+                                  on the output device(s). All output devices must
+                                  also support the Unicode drawing defined in this file.
+
+  @retval EFI_SUCCESS             The string was output to the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting to output
+                                  the text.
+  @retval EFI_UNSUPPORTED         The output device's mode is not currently in a
+                                  defined text mode.
+  @retval EFI_WARN_UNKNOWN_GLYPH  This warning code indicates that some of the
+                                  characters in the Unicode string could not be
+                                  rendered and were skipped.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutOutputString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  )
+{
+  GRAPHICS_CONSOLE_DEV  *Private;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL   *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL *UgaDraw;
+  INTN                  Mode;
+  UINTN                 MaxColumn;
+  UINTN                 MaxRow;
+  UINTN                 Width;
+  UINTN                 Height;
+  UINTN                 Delta;
+  EFI_STATUS            Status;
+  BOOLEAN               Warning;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL  Foreground;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL  Background;
+  UINTN                 DeltaX;
+  UINTN                 DeltaY;
+  UINTN                 Count;
+  UINTN                 Index;
+  INT32                 OriginAttribute;
+  EFI_TPL               OldTpl;
+
+  if (This->Mode->Mode == -1) {
+    //
+    // If current mode is not valid, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = EFI_SUCCESS;
+  
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  //
+  // Current mode
+  //
+  Mode      = This->Mode->Mode;
+  Private   = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  GraphicsOutput = Private->GraphicsOutput;
+  UgaDraw   = Private->UgaDraw;
+
+  MaxColumn = Private->ModeData[Mode].Columns;
+  MaxRow    = Private->ModeData[Mode].Rows;
+  DeltaX    = (UINTN) Private->ModeData[Mode].DeltaX;
+  DeltaY    = (UINTN) Private->ModeData[Mode].DeltaY;
+  Width     = MaxColumn * EFI_GLYPH_WIDTH;
+  Height    = (MaxRow - 1) * EFI_GLYPH_HEIGHT;
+  Delta     = Width * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL);
+
+  //
+  // The Attributes won't change when during the time OutputString is called
+  //
+  GetTextColors (This, &Foreground, &Background);
+
+  FlushCursor (This);
+
+  Warning = FALSE;
+
+  //
+  // Backup attribute
+  //
+  OriginAttribute = This->Mode->Attribute;
+
+  while (*WString != L'\0') {
+
+    if (*WString == CHAR_BACKSPACE) {
+      //
+      // If the cursor is at the left edge of the display, then move the cursor
+      // one row up.
+      //
+      if (This->Mode->CursorColumn == 0 && This->Mode->CursorRow > 0) {
+        This->Mode->CursorRow--;
+        This->Mode->CursorColumn = (INT32) (MaxColumn - 1);
+        This->OutputString (This, SpaceStr);
+        FlushCursor (This);
+        This->Mode->CursorRow--;
+        This->Mode->CursorColumn = (INT32) (MaxColumn - 1);
+      } else if (This->Mode->CursorColumn > 0) {
+        //
+        // If the cursor is not at the left edge of the display, then move the cursor
+        // left one column.
+        //
+        This->Mode->CursorColumn--;
+        This->OutputString (This, SpaceStr);
+        FlushCursor (This);
+        This->Mode->CursorColumn--;
+      }
+
+      WString++;
+
+    } else if (*WString == CHAR_LINEFEED) {
+      //
+      // If the cursor is at the bottom of the display, then scroll the display one
+      // row, and do not update the cursor position. Otherwise, move the cursor
+      // down one row.
+      //
+      if (This->Mode->CursorRow == (INT32) (MaxRow - 1)) {
+        if (GraphicsOutput != NULL) {
+          //
+          // Scroll Screen Up One Row
+          //
+          GraphicsOutput->Blt (
+                    GraphicsOutput,
+                    NULL,
+                    EfiBltVideoToVideo,
+                    DeltaX,
+                    DeltaY + EFI_GLYPH_HEIGHT,
+                    DeltaX,
+                    DeltaY,
+                    Width,
+                    Height,
+                    Delta
+                    );
+
+          //
+          // Print Blank Line at last line
+          //
+          GraphicsOutput->Blt (
+                    GraphicsOutput,
+                    &Background,
+                    EfiBltVideoFill,
+                    0,
+                    0,
+                    DeltaX,
+                    DeltaY + Height,
+                    Width,
+                    EFI_GLYPH_HEIGHT,
+                    Delta
+                    );
+        } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+          //
+          // Scroll Screen Up One Row
+          //
+          UgaDraw->Blt (
+                    UgaDraw,
+                    NULL,
+                    EfiUgaVideoToVideo,
+                    DeltaX,
+                    DeltaY + EFI_GLYPH_HEIGHT,
+                    DeltaX,
+                    DeltaY,
+                    Width,
+                    Height,
+                    Delta
+                    );
+
+          //
+          // Print Blank Line at last line
+          //
+          UgaDraw->Blt (
+                    UgaDraw,
+                    (EFI_UGA_PIXEL *) (UINTN) &Background,
+                    EfiUgaVideoFill,
+                    0,
+                    0,
+                    DeltaX,
+                    DeltaY + Height,
+                    Width,
+                    EFI_GLYPH_HEIGHT,
+                    Delta
+                    );
+        }
+      } else {
+        This->Mode->CursorRow++;
+      }
+
+      WString++;
+
+    } else if (*WString == CHAR_CARRIAGE_RETURN) {
+      //
+      // Move the cursor to the beginning of the current row.
+      //
+      This->Mode->CursorColumn = 0;
+      WString++;
+
+    } else if (*WString == WIDE_CHAR) {
+
+      This->Mode->Attribute |= EFI_WIDE_ATTRIBUTE;
+      WString++;
+
+    } else if (*WString == NARROW_CHAR) {
+
+      This->Mode->Attribute &= (~ (UINT32) EFI_WIDE_ATTRIBUTE);
+      WString++;
+
+    } else {
+      //
+      // Print the character at the current cursor position and move the cursor
+      // right one column. If this moves the cursor past the right edge of the
+      // display, then the line should wrap to the beginning of the next line. This
+      // is equivalent to inserting a CR and an LF. Note that if the cursor is at the
+      // bottom of the display, and the line wraps, then the display will be scrolled
+      // one line.
+      // If wide char is going to be displayed, need to display one character at a time
+      // Or, need to know the display length of a certain string.
+      //
+      // Index is used to determine how many character width units (wide = 2, narrow = 1)
+      // Count is used to determine how many characters are used regardless of their attributes
+      //
+      for (Count = 0, Index = 0; (This->Mode->CursorColumn + Index) < MaxColumn; Count++, Index++) {
+        if (WString[Count] == CHAR_NULL ||
+            WString[Count] == CHAR_BACKSPACE ||
+            WString[Count] == CHAR_LINEFEED ||
+            WString[Count] == CHAR_CARRIAGE_RETURN ||
+            WString[Count] == WIDE_CHAR ||
+            WString[Count] == NARROW_CHAR) {
+          break;
+        }
+        //
+        // Is the wide attribute on?
+        //
+        if ((This->Mode->Attribute & EFI_WIDE_ATTRIBUTE) != 0) {
+          //
+          // If wide, add one more width unit than normal since we are going to increment at the end of the for loop
+          //
+          Index++;
+          //
+          // This is the end-case where if we are at column 79 and about to print a wide character
+          // We should prevent this from happening because we will wrap inappropriately.  We should
+          // not print this character until the next line.
+          //
+          if ((This->Mode->CursorColumn + Index + 1) > MaxColumn) {
+            Index++;
+            break;
+          }
+        }
+      }
+
+      Status = DrawUnicodeWeightAtCursorN (This, WString, Count);
+      if (EFI_ERROR (Status)) {
+        Warning = TRUE;
+      }
+      //
+      // At the end of line, output carriage return and line feed
+      //
+      WString += Count;
+      This->Mode->CursorColumn += (INT32) Index;
+      if (This->Mode->CursorColumn > (INT32) MaxColumn) {
+        This->Mode->CursorColumn -= 2;
+        This->OutputString (This, SpaceStr);
+      }
+
+      if (This->Mode->CursorColumn >= (INT32) MaxColumn) {
+        FlushCursor (This);
+        This->OutputString (This, mCrLfString);
+        FlushCursor (This);
+      }
+    }
+  }
+
+  This->Mode->Attribute = OriginAttribute;
+
+  FlushCursor (This);
+
+  if (Warning) {
+    Status = EFI_WARN_UNKNOWN_GLYPH;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+
+}
+
+/**
+  Verifies that all characters in a Unicode string can be output to the
+  target device.
+
+  Implements SIMPLE_TEXT_OUTPUT.TestString().
+  If one of the characters in the *Wstring is neither valid valid Unicode
+  drawing characters, not ASCII code, then this function will return
+  EFI_UNSUPPORTED
+
+  @param  This    Protocol instance pointer.
+  @param  WString The NULL-terminated Unicode string to be examined for the output
+                  device(s).
+
+  @retval EFI_SUCCESS      The device(s) are capable of rendering the output string.
+  @retval EFI_UNSUPPORTED  Some of the characters in the Unicode string cannot be
+                           rendered by one or more of the output devices mapped
+                           by the EFI handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  )
+{
+  EFI_STATUS            Status;
+  UINT16                Count;
+
+  EFI_IMAGE_OUTPUT      *Blt;
+
+  Blt   = NULL;
+  Count = 0;
+
+  while (WString[Count] != 0) {
+    Status = mHiiFont->GetGlyph (
+                         mHiiFont,
+                         WString[Count],
+                         NULL,
+                         &Blt,
+                         NULL
+                         );
+    if (Blt != NULL) {
+      FreePool (Blt);
+      Blt = NULL;
+    }
+    Count++;
+
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Returns information for an available text mode that the output device(s)
+  supports
+
+  Implements SIMPLE_TEXT_OUTPUT.QueryMode().
+  It returnes information for an available text mode that the Graphics Console supports.
+  In this driver,we only support text mode 80x25, which is defined as mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ModeNumber            The mode number to return information on.
+  @param  Columns               The returned columns of the requested mode.
+  @param  Rows                  The returned rows of the requested mode.
+
+  @retval EFI_SUCCESS           The requested mode information is returned.
+  @retval EFI_UNSUPPORTED       The mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber,
+  OUT UINTN                            *Columns,
+  OUT UINTN                            *Rows
+  )
+{
+  GRAPHICS_CONSOLE_DEV  *Private;
+  EFI_STATUS            Status;
+  EFI_TPL               OldTpl;
+
+  if (ModeNumber >= (UINTN) This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+  Status = EFI_SUCCESS;
+
+  Private   = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+
+  *Columns  = Private->ModeData[ModeNumber].Columns;
+  *Rows     = Private->ModeData[ModeNumber].Rows;
+
+  if (*Columns <= 0 || *Rows <= 0) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+
+  }
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets the output device(s) to a specified mode.
+
+  Implements SIMPLE_TEXT_OUTPUT.SetMode().
+  Set the Graphics Console to a specified mode. In this driver, we only support mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ModeNumber            The text mode to set.
+
+  @retval EFI_SUCCESS           The requested text mode is set.
+  @retval EFI_DEVICE_ERROR      The requested text mode cannot be set because of
+                                Graphics Console device error.
+  @retval EFI_UNSUPPORTED       The text mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber
+  )
+{
+  EFI_STATUS                      Status;
+  GRAPHICS_CONSOLE_DEV            *Private;
+  GRAPHICS_CONSOLE_MODE_DATA      *ModeData;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *NewLineBuffer;
+  UINT32                          HorizontalResolution;
+  UINT32                          VerticalResolution;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL    *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL           *UgaDraw;
+  UINT32                          ColorDepth;
+  UINT32                          RefreshRate;
+  EFI_TPL                         OldTpl;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Private   = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  GraphicsOutput = Private->GraphicsOutput;
+  UgaDraw   = Private->UgaDraw;
+
+  //
+  // Make sure the requested mode number is supported
+  //
+  if (ModeNumber >= (UINTN) This->Mode->MaxMode) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+  
+  ModeData  = &(Private->ModeData[ModeNumber]);
+
+  if (ModeData->Columns <= 0 && ModeData->Rows <= 0) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  //
+  // If the mode has been set at least one other time, then LineBuffer will not be NULL
+  //
+  if (Private->LineBuffer != NULL) {
+    //
+    // If the new mode is the same as the old mode, then just return EFI_SUCCESS
+    //
+    if ((INT32) ModeNumber == This->Mode->Mode) {
+      //
+      // Clear the current text window on the current graphics console
+      //
+      This->ClearScreen (This);
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+    //
+    // Otherwise, the size of the text console and/or the GOP/UGA mode will be changed,
+    // so erase the cursor, and free the LineBuffer for the current mode
+    //
+    FlushCursor (This);
+
+    FreePool (Private->LineBuffer);
+  }
+
+  //
+  // Attempt to allocate a line buffer for the requested mode number
+  //
+  NewLineBuffer = AllocatePool (sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL) * ModeData->Columns * EFI_GLYPH_WIDTH * EFI_GLYPH_HEIGHT);
+
+  if (NewLineBuffer == NULL) {
+    //
+    // The new line buffer could not be allocated, so return an error.
+    // No changes to the state of the current console have been made, so the current console is still valid
+    //
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Assign the current line buffer to the newly allocated line buffer
+  //
+  Private->LineBuffer = NewLineBuffer;
+
+  if (GraphicsOutput != NULL) {
+    if (ModeData->GopModeNumber != GraphicsOutput->Mode->Mode) {
+      //
+      // Either no graphics mode is currently set, or it is set to the wrong resolution, so set the new graphics mode
+      //
+      Status = GraphicsOutput->SetMode (GraphicsOutput, ModeData->GopModeNumber);
+      if (EFI_ERROR (Status)) {
+        //
+        // The mode set operation failed
+        //
+        goto Done;
+      }
+    } else {
+      //
+      // The current graphics mode is correct, so simply clear the entire display
+      //
+      Status = GraphicsOutput->Blt (
+                          GraphicsOutput,
+                          &mGraphicsEfiColors[0],
+                          EfiBltVideoFill,
+                          0,
+                          0,
+                          0,
+                          0,
+                          ModeData->GopWidth,
+                          ModeData->GopHeight,
+                          0
+                          );
+    }
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    //
+    // Get the current UGA Draw mode information
+    //
+    Status = UgaDraw->GetMode (
+                        UgaDraw,
+                        &HorizontalResolution,
+                        &VerticalResolution,
+                        &ColorDepth,
+                        &RefreshRate
+                        );
+    if (EFI_ERROR (Status) || HorizontalResolution != ModeData->GopWidth || VerticalResolution != ModeData->GopHeight) {
+      //
+      // Either no graphics mode is currently set, or it is set to the wrong resolution, so set the new graphics mode
+      //
+      Status = UgaDraw->SetMode (
+                          UgaDraw,
+                          ModeData->GopWidth,
+                          ModeData->GopHeight,
+                          32,
+                          60
+                          );
+      if (EFI_ERROR (Status)) {
+        //
+        // The mode set operation failed
+        //
+        goto Done;
+      }
+    } else {
+      //
+      // The current graphics mode is correct, so simply clear the entire display
+      //
+      Status = UgaDraw->Blt (
+                          UgaDraw,
+                          (EFI_UGA_PIXEL *) (UINTN) &mGraphicsEfiColors[0],
+                          EfiUgaVideoFill,
+                          0,
+                          0,
+                          0,
+                          0,
+                          ModeData->GopWidth,
+                          ModeData->GopHeight,
+                          0
+                          );
+    }
+  }
+
+  //
+  // The new mode is valid, so commit the mode change
+  //
+  This->Mode->Mode = (INT32) ModeNumber;
+
+  //
+  // Move the text cursor to the upper left hand corner of the display and flush it
+  //
+  This->Mode->CursorColumn  = 0;
+  This->Mode->CursorRow     = 0;
+
+  FlushCursor (This);  
+
+  Status = EFI_SUCCESS;
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Sets the background and foreground colors for the OutputString () and
+  ClearScreen () functions.
+
+  Implements SIMPLE_TEXT_OUTPUT.SetAttribute().
+
+  @param  This                  Protocol instance pointer.
+  @param  Attribute             The attribute to set. Bits 0..3 are the foreground
+                                color, and bits 4..6 are the background color.
+                                All other bits are undefined and must be zero.
+
+  @retval EFI_SUCCESS           The requested attribute is set.
+  @retval EFI_DEVICE_ERROR      The requested attribute cannot be set due to Graphics Console port error.
+  @retval EFI_UNSUPPORTED       The attribute requested is not defined.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Attribute
+  )
+{
+  EFI_TPL               OldTpl;
+
+  if ((Attribute | 0x7F) != 0x7F) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((INT32) Attribute == This->Mode->Attribute) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  FlushCursor (This);
+
+  This->Mode->Attribute = (INT32) Attribute;
+
+  FlushCursor (This);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Clears the output device(s) display to the currently selected background
+  color.
+
+  Implements SIMPLE_TEXT_OUTPUT.ClearScreen().
+
+  @param  This                  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS      The operation completed successfully.
+  @retval  EFI_DEVICE_ERROR The device had an error and could not complete the request.
+  @retval  EFI_UNSUPPORTED  The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  )
+{
+  EFI_STATUS                    Status;
+  GRAPHICS_CONSOLE_DEV          *Private;
+  GRAPHICS_CONSOLE_MODE_DATA    *ModeData;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL  *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL         *UgaDraw;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL Foreground;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL Background;
+  EFI_TPL                       OldTpl;
+  
+  if (This->Mode->Mode == -1) {
+    //
+    // If current mode is not valid, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Private   = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  GraphicsOutput = Private->GraphicsOutput;
+  UgaDraw   = Private->UgaDraw;
+  ModeData  = &(Private->ModeData[This->Mode->Mode]);
+
+  GetTextColors (This, &Foreground, &Background);
+  if (GraphicsOutput != NULL) {
+    Status = GraphicsOutput->Blt (
+                        GraphicsOutput,
+                        &Background,
+                        EfiBltVideoFill,
+                        0,
+                        0,
+                        0,
+                        0,
+                        ModeData->GopWidth,
+                        ModeData->GopHeight,
+                        0
+                        );
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    Status = UgaDraw->Blt (
+                        UgaDraw,
+                        (EFI_UGA_PIXEL *) (UINTN) &Background,
+                        EfiUgaVideoFill,
+                        0,
+                        0,
+                        0,
+                        0,
+                        ModeData->GopWidth,
+                        ModeData->GopHeight,
+                        0
+                        );
+  } else {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  This->Mode->CursorColumn  = 0;
+  This->Mode->CursorRow     = 0;
+
+  FlushCursor (This);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Sets the current coordinates of the cursor position.
+
+  Implements SIMPLE_TEXT_OUTPUT.SetCursorPosition().
+
+  @param  This        Protocol instance pointer.
+  @param  Column      The position to set the cursor to. Must be greater than or
+                      equal to zero and less than the number of columns and rows
+                      by QueryMode ().
+  @param  Row         The position to set the cursor to. Must be greater than or
+                      equal to zero and less than the number of columns and rows
+                      by QueryMode ().
+
+  @retval EFI_SUCCESS      The operation completed successfully.
+  @retval EFI_DEVICE_ERROR The device had an error and could not complete the request.
+  @retval EFI_UNSUPPORTED  The output device is not in a valid text mode, or the
+                           cursor position is invalid for the current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Column,
+  IN  UINTN                            Row
+  )
+{
+  GRAPHICS_CONSOLE_DEV        *Private;
+  GRAPHICS_CONSOLE_MODE_DATA  *ModeData;
+  EFI_STATUS                  Status;
+  EFI_TPL                     OldTpl;
+
+  if (This->Mode->Mode == -1) {
+    //
+    // If current mode is not valid, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = EFI_SUCCESS;
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  Private   = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  ModeData  = &(Private->ModeData[This->Mode->Mode]);
+
+  if ((Column >= ModeData->Columns) || (Row >= ModeData->Rows)) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  if ((This->Mode->CursorColumn == (INT32) Column) && (This->Mode->CursorRow == (INT32) Row)) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  FlushCursor (This);
+
+  This->Mode->CursorColumn  = (INT32) Column;
+  This->Mode->CursorRow     = (INT32) Row;
+
+  FlushCursor (This);
+
+Done:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Makes the cursor visible or invisible.
+
+  Implements SIMPLE_TEXT_OUTPUT.EnableCursor().
+
+  @param  This                  Protocol instance pointer.
+  @param  Visible               If TRUE, the cursor is set to be visible, If FALSE,
+                                the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_UNSUPPORTED       The output device's mode is not currently in a
+                                defined text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          Visible
+  )
+{
+  EFI_TPL               OldTpl;
+
+  if (This->Mode->Mode == -1) {
+    //
+    // If current mode is not valid, return error.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  FlushCursor (This);
+
+  This->Mode->CursorVisible = Visible;
+
+  FlushCursor (This);
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+/**
+  Gets Graphics Console devcie's foreground color and background color.
+
+  @param  This                  Protocol instance pointer.
+  @param  Foreground            Returned text foreground color.
+  @param  Background            Returned text background color.
+
+  @retval EFI_SUCCESS           It returned always.
+
+**/
+EFI_STATUS
+GetTextColors (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL    *Foreground,
+  OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL    *Background
+  )
+{
+  INTN  Attribute;
+
+  Attribute   = This->Mode->Attribute & 0x7F;
+
+  *Foreground = mGraphicsEfiColors[Attribute & 0x0f];
+  *Background = mGraphicsEfiColors[Attribute >> 4];
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Draw Unicode string on the Graphics Console device's screen.
+
+  @param  This                  Protocol instance pointer.
+  @param  UnicodeWeight         One Unicode string to be displayed.
+  @param  Count                 The count of Unicode string.
+
+  @retval EFI_OUT_OF_RESOURCES  If no memory resource to use.
+  @retval EFI_UNSUPPORTED       If no Graphics Output protocol and UGA Draw
+                                protocol exist.
+  @retval EFI_SUCCESS           Drawing Unicode string implemented successfully.
+
+**/
+EFI_STATUS
+DrawUnicodeWeightAtCursorN (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *UnicodeWeight,
+  IN  UINTN                            Count
+  )
+{
+  EFI_STATUS                        Status;
+  GRAPHICS_CONSOLE_DEV              *Private;
+  EFI_IMAGE_OUTPUT                  *Blt;
+  EFI_STRING                        String;
+  EFI_FONT_DISPLAY_INFO             *FontInfo;
+  EFI_UGA_DRAW_PROTOCOL             *UgaDraw;
+  EFI_HII_ROW_INFO                  *RowInfoArray;
+  UINTN                             RowInfoArraySize;
+
+  Private = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  Blt = (EFI_IMAGE_OUTPUT *) AllocateZeroPool (sizeof (EFI_IMAGE_OUTPUT));
+  if (Blt == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Blt->Width        = (UINT16) (Private->ModeData[This->Mode->Mode].GopWidth);
+  Blt->Height       = (UINT16) (Private->ModeData[This->Mode->Mode].GopHeight);
+
+  String = AllocateCopyPool ((Count + 1) * sizeof (CHAR16), UnicodeWeight);
+  if (String == NULL) {
+    FreePool (Blt);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Set the end character
+  //
+  *(String + Count) = L'\0';
+
+  FontInfo = (EFI_FONT_DISPLAY_INFO *) AllocateZeroPool (sizeof (EFI_FONT_DISPLAY_INFO));
+  if (FontInfo == NULL) {
+    FreePool (Blt);
+    FreePool (String);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Get current foreground and background colors.
+  //
+  GetTextColors (This, &FontInfo->ForegroundColor, &FontInfo->BackgroundColor);
+
+  if (Private->GraphicsOutput != NULL) {
+    //
+    // If Graphics Output protocol exists, using HII Font protocol to draw.
+    //
+    Blt->Image.Screen = Private->GraphicsOutput;
+
+    Status = mHiiFont->StringToImage (
+                         mHiiFont,
+                         EFI_HII_IGNORE_IF_NO_GLYPH | EFI_HII_DIRECT_TO_SCREEN | EFI_HII_IGNORE_LINE_BREAK,
+                         String,
+                         FontInfo,
+                         &Blt,
+                         This->Mode->CursorColumn * EFI_GLYPH_WIDTH + Private->ModeData[This->Mode->Mode].DeltaX,
+                         This->Mode->CursorRow * EFI_GLYPH_HEIGHT + Private->ModeData[This->Mode->Mode].DeltaY,
+                         NULL,
+                         NULL,
+                         NULL
+                         );
+
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    //
+    // If Graphics Output protocol cannot be found and PcdUgaConsumeSupport enabled,
+    // using UGA Draw protocol to draw.
+    //
+    ASSERT (Private->UgaDraw!= NULL);
+
+    UgaDraw = Private->UgaDraw;
+
+    Blt->Image.Bitmap = AllocateZeroPool (Blt->Width * Blt->Height * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+    if (Blt->Image.Bitmap == NULL) {
+      FreePool (Blt);
+      FreePool (String);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    RowInfoArray = NULL;
+    //
+    //  StringToImage only support blt'ing image to device using GOP protocol. If GOP is not supported in this platform,
+    //  we ask StringToImage to print the string to blt buffer, then blt to device using UgaDraw.
+    //
+    Status = mHiiFont->StringToImage (
+                          mHiiFont,
+                          EFI_HII_IGNORE_IF_NO_GLYPH | EFI_HII_IGNORE_LINE_BREAK,
+                          String,
+                          FontInfo,
+                          &Blt,
+                          This->Mode->CursorColumn * EFI_GLYPH_WIDTH + Private->ModeData[This->Mode->Mode].DeltaX,
+                          This->Mode->CursorRow * EFI_GLYPH_HEIGHT + Private->ModeData[This->Mode->Mode].DeltaY,
+                          &RowInfoArray,
+                          &RowInfoArraySize,
+                          NULL
+                          );
+
+    if (!EFI_ERROR (Status)) {
+      //
+      // Line breaks are handled by caller of DrawUnicodeWeightAtCursorN, so the updated parameter RowInfoArraySize by StringToImage will
+      // always be 1 or 0 (if there is no valid Unicode Char can be printed). ASSERT here to make sure.
+      //
+      ASSERT (RowInfoArraySize <= 1);
+
+      Status = UgaDraw->Blt (
+                          UgaDraw,
+                          (EFI_UGA_PIXEL *) Blt->Image.Bitmap,
+                          EfiUgaBltBufferToVideo,
+                          This->Mode->CursorColumn * EFI_GLYPH_WIDTH  + Private->ModeData[This->Mode->Mode].DeltaX,
+                          (This->Mode->CursorRow) * EFI_GLYPH_HEIGHT + Private->ModeData[This->Mode->Mode].DeltaY,
+                          This->Mode->CursorColumn * EFI_GLYPH_WIDTH  + Private->ModeData[This->Mode->Mode].DeltaX,
+                          (This->Mode->CursorRow) * EFI_GLYPH_HEIGHT + Private->ModeData[This->Mode->Mode].DeltaY,
+                          RowInfoArray[0].LineWidth,
+                          RowInfoArray[0].LineHeight,
+                          Blt->Width * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+                          );
+    }
+
+    FreePool (RowInfoArray);
+    FreePool (Blt->Image.Bitmap);
+  } else {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  if (Blt != NULL) {
+    FreePool (Blt);
+  }
+  if (String != NULL) {
+    FreePool (String);
+  }
+  if (FontInfo != NULL) {
+    FreePool (FontInfo);
+  }
+  return Status;
+}
+
+/**
+  Flush the cursor on the screen.
+  
+  If CursorVisible is FALSE, nothing to do and return directly.
+  If CursorVisible is TRUE, 
+     i) If the cursor shows on screen, it will be erased.
+    ii) If the cursor does not show on screen, it will be shown. 
+
+  @param  This                  Protocol instance pointer.
+
+  @retval EFI_SUCCESS           The cursor is erased successfully.
+
+**/
+EFI_STATUS
+FlushCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  )
+{
+  GRAPHICS_CONSOLE_DEV                *Private;
+  EFI_SIMPLE_TEXT_OUTPUT_MODE         *CurrentMode;
+  INTN                                GlyphX;
+  INTN                                GlyphY;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL        *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL               *UgaDraw;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL_UNION Foreground;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL_UNION Background;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL_UNION BltChar[EFI_GLYPH_HEIGHT][EFI_GLYPH_WIDTH];
+  UINTN                               PosX;
+  UINTN                               PosY;
+
+  CurrentMode = This->Mode;
+
+  if (!CurrentMode->CursorVisible) {
+    return EFI_SUCCESS;
+  }
+
+  Private = GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS (This);
+  GraphicsOutput = Private->GraphicsOutput;
+  UgaDraw = Private->UgaDraw;
+
+  //
+  // In this driver, only narrow character was supported.
+  //
+  //
+  // Blt a character to the screen
+  //
+  GlyphX  = (CurrentMode->CursorColumn * EFI_GLYPH_WIDTH) + Private->ModeData[CurrentMode->Mode].DeltaX;
+  GlyphY  = (CurrentMode->CursorRow * EFI_GLYPH_HEIGHT) + Private->ModeData[CurrentMode->Mode].DeltaY;
+  if (GraphicsOutput != NULL) {
+    GraphicsOutput->Blt (
+              GraphicsOutput,
+              (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) BltChar,
+              EfiBltVideoToBltBuffer,
+              GlyphX,
+              GlyphY,
+              0,
+              0,
+              EFI_GLYPH_WIDTH,
+              EFI_GLYPH_HEIGHT,
+              EFI_GLYPH_WIDTH * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+              );
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    UgaDraw->Blt (
+              UgaDraw,
+              (EFI_UGA_PIXEL *) (UINTN) BltChar,
+              EfiUgaVideoToBltBuffer,
+              GlyphX,
+              GlyphY,
+              0,
+              0,
+              EFI_GLYPH_WIDTH,
+              EFI_GLYPH_HEIGHT,
+              EFI_GLYPH_WIDTH * sizeof (EFI_UGA_PIXEL)
+              );
+  }
+
+  GetTextColors (This, &Foreground.Pixel, &Background.Pixel);
+
+  //
+  // Convert Monochrome bitmap of the Glyph to BltBuffer structure
+  //
+  for (PosY = 0; PosY < EFI_GLYPH_HEIGHT; PosY++) {
+    for (PosX = 0; PosX < EFI_GLYPH_WIDTH; PosX++) {
+      if ((mCursorGlyph.GlyphCol1[PosY] & (BIT0 << PosX)) != 0) {
+        BltChar[PosY][EFI_GLYPH_WIDTH - PosX - 1].Raw ^= Foreground.Raw;
+      }
+    }
+  }
+
+  if (GraphicsOutput != NULL) {
+    GraphicsOutput->Blt (
+              GraphicsOutput,
+              (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) BltChar,
+              EfiBltBufferToVideo,
+              0,
+              0,
+              GlyphX,
+              GlyphY,
+              EFI_GLYPH_WIDTH,
+              EFI_GLYPH_HEIGHT,
+              EFI_GLYPH_WIDTH * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+              );
+  } else if (FeaturePcdGet (PcdUgaConsumeSupport)) {
+    UgaDraw->Blt (
+              UgaDraw,
+              (EFI_UGA_PIXEL *) (UINTN) BltChar,
+              EfiUgaBltBufferToVideo,
+              0,
+              0,
+              GlyphX,
+              GlyphY,
+              EFI_GLYPH_WIDTH,
+              EFI_GLYPH_HEIGHT,
+              EFI_GLYPH_WIDTH * sizeof (EFI_UGA_PIXEL)
+              );
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  HII Database Protocol notification event handler.
+
+  Register font package when HII Database Protocol has been installed.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+**/
+VOID
+EFIAPI
+RegisterFontPackage (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  )
+{
+  EFI_STATUS                           Status;
+  EFI_HII_SIMPLE_FONT_PACKAGE_HDR      *SimplifiedFont;
+  UINT32                               PackageLength;
+  UINT8                                *Package;
+  UINT8                                *Location;
+  EFI_HII_DATABASE_PROTOCOL            *HiiDatabase;
+
+  //
+  // Locate HII Database Protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &HiiDatabase
+                  );
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  //
+  // Add 4 bytes to the header for entire length for HiiAddPackages use only.
+  //
+  //    +--------------------------------+ <-- Package
+  //    |                                |
+  //    |    PackageLength(4 bytes)      |
+  //    |                                |
+  //    |--------------------------------| <-- SimplifiedFont
+  //    |                                |
+  //    |EFI_HII_SIMPLE_FONT_PACKAGE_HDR |
+  //    |                                |
+  //    |--------------------------------| <-- Location
+  //    |                                |
+  //    |     gUsStdNarrowGlyphData      |
+  //    |                                |
+  //    +--------------------------------+
+
+  PackageLength   = sizeof (EFI_HII_SIMPLE_FONT_PACKAGE_HDR) + mNarrowFontSize + 4;
+  Package = AllocateZeroPool (PackageLength);
+  ASSERT (Package != NULL);
+
+  WriteUnaligned32((UINT32 *) Package,PackageLength);
+  SimplifiedFont = (EFI_HII_SIMPLE_FONT_PACKAGE_HDR *) (Package + 4);
+  SimplifiedFont->Header.Length        = (UINT32) (PackageLength - 4);
+  SimplifiedFont->Header.Type          = EFI_HII_PACKAGE_SIMPLE_FONTS;
+  SimplifiedFont->NumberOfNarrowGlyphs = (UINT16) (mNarrowFontSize / sizeof (EFI_NARROW_GLYPH));
+
+  Location = (UINT8 *) (&SimplifiedFont->NumberOfWideGlyphs + 1);
+  CopyMem (Location, gUsStdNarrowGlyphData, mNarrowFontSize);
+
+  //
+  // Add this simplified font package to a package list then install it.
+  //
+  mHiiHandle = HiiAddPackages (
+                 &mFontPackageListGuid,
+                 NULL,
+                 Package,
+                 NULL
+                 );
+  ASSERT (mHiiHandle != NULL);
+  FreePool (Package);
+}
+
+/**
+  The user Entry Point for module GraphicsConsole. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval  EFI_SUCCESS       The entry point is executed successfully.
+  @return  other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeGraphicsConsole (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Register notify function on HII Database Protocol to add font package.
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEfiHiiDatabaseProtocolGuid,
+    TPL_CALLBACK,
+    RegisterFontPackage,
+    NULL,
+    &mHiiRegistration
+    );
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gGraphicsConsoleDriverBinding,
+             ImageHandle,
+             &gGraphicsConsoleComponentName,
+             &gGraphicsConsoleComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.h b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.h
new file mode 100644
index 0000000000..0db6f04c11
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsole.h
@@ -0,0 +1,600 @@
+/** @file
+  Header file for GraphicsConsole driver.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _GRAPHICS_CONSOLE_H_
+#define _GRAPHICS_CONSOLE_H_
+
+#include <Uefi.h>
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/GraphicsOutput.h>
+#include <Protocol/UgaDraw.h>
+#include <Protocol/DevicePath.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/HiiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PcdLib.h>
+
+#include <Guid/MdeModuleHii.h>
+
+#include <Protocol/HiiFont.h>
+#include <Protocol/HiiDatabase.h>
+
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gGraphicsConsoleComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gGraphicsConsoleComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL   gGraphicsConsoleDriverBinding;
+extern EFI_NARROW_GLYPH              gUsStdNarrowGlyphData[];
+
+extern UINT32 mNarrowFontSize;
+
+typedef union {
+  EFI_NARROW_GLYPH  NarrowGlyph;
+  EFI_WIDE_GLYPH    WideGlyph;
+} GLYPH_UNION;
+
+//
+// Device Structure
+//
+#define GRAPHICS_CONSOLE_DEV_SIGNATURE  SIGNATURE_32 ('g', 's', 't', 'o')
+
+typedef struct {
+  UINTN   Columns;
+  UINTN   Rows;
+  INTN    DeltaX;
+  INTN    DeltaY;
+  UINT32  GopWidth;
+  UINT32  GopHeight;
+  UINT32  GopModeNumber;
+} GRAPHICS_CONSOLE_MODE_DATA;
+
+typedef struct {
+  UINTN                            Signature;
+  EFI_GRAPHICS_OUTPUT_PROTOCOL     *GraphicsOutput;
+  EFI_UGA_DRAW_PROTOCOL            *UgaDraw;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  SimpleTextOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_MODE      SimpleTextOutputMode;
+  GRAPHICS_CONSOLE_MODE_DATA       *ModeData;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL    *LineBuffer;
+} GRAPHICS_CONSOLE_DEV;
+
+#define GRAPHICS_CONSOLE_CON_OUT_DEV_FROM_THIS(a) \
+  CR (a, GRAPHICS_CONSOLE_DEV, SimpleTextOutput, GRAPHICS_CONSOLE_DEV_SIGNATURE)
+
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Reset the text output device hardware and optionally run diagnostics.
+  
+  Implements SIMPLE_TEXT_OUTPUT.Reset().
+  If ExtendeVerification is TRUE, then perform dependent Graphics Console
+  device reset, and set display mode to mode 0.
+  If ExtendedVerification is FALSE, only set display mode to mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ExtendedVerification  Indicates that the driver may perform a more
+                                exhaustive verification operation of the device
+                                during reset.
+
+  @retval EFI_SUCCESS          The text output device was reset.
+  @retval EFI_DEVICE_ERROR     The text output device is not functioning correctly and
+                               could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Write a Unicode string to the output device.
+
+  Implements SIMPLE_TEXT_OUTPUT.OutputString(). 
+  The Unicode string will be converted to Glyphs and will be
+  sent to the Graphics Console.
+
+  @param  This                    Protocol instance pointer.
+  @param  WString                 The NULL-terminated Unicode string to be displayed
+                                  on the output device(s). All output devices must
+                                  also support the Unicode drawing defined in this file.
+
+  @retval EFI_SUCCESS             The string was output to the device.
+  @retval EFI_DEVICE_ERROR        The device reported an error while attempting to output
+                                  the text.
+  @retval EFI_UNSUPPORTED         The output device's mode is not currently in a
+                                  defined text mode.
+  @retval EFI_WARN_UNKNOWN_GLYPH  This warning code indicates that some of the
+                                  characters in the Unicode string could not be
+                                  rendered and were skipped.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutOutputString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  );
+
+/**
+  Verifies that all characters in a Unicode string can be output to the 
+  target device.
+
+  Implements SIMPLE_TEXT_OUTPUT.TestString().
+  If one of the characters in the *Wstring is neither valid valid Unicode
+  drawing characters, not ASCII code, then this function will return
+  EFI_UNSUPPORTED
+
+  @param  This    Protocol instance pointer.
+  @param  WString The NULL-terminated Unicode string to be examined for the output
+                  device(s).
+
+  @retval EFI_SUCCESS      The device(s) are capable of rendering the output string.
+  @retval EFI_UNSUPPORTED  Some of the characters in the Unicode string cannot be
+                           rendered by one or more of the output devices mapped
+                           by the EFI handle.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  );
+
+/**
+  Returns information for an available text mode that the output device(s)
+  supports
+
+  Implements SIMPLE_TEXT_OUTPUT.QueryMode().
+  It returnes information for an available text mode that the Graphics Console supports.
+  In this driver,we only support text mode 80x25, which is defined as mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ModeNumber            The mode number to return information on.
+  @param  Columns               The returned columns of the requested mode.
+  @param  Rows                  The returned rows of the requested mode.
+
+  @retval EFI_SUCCESS           The requested mode information is returned.
+  @retval EFI_UNSUPPORTED       The mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber,
+  OUT UINTN                            *Columns,
+  OUT UINTN                            *Rows
+  );
+
+
+/**
+  Sets the output device(s) to a specified mode.
+  
+  Implements SIMPLE_TEXT_OUTPUT.SetMode().
+  Set the Graphics Console to a specified mode. In this driver, we only support mode 0.
+
+  @param  This                  Protocol instance pointer.
+  @param  ModeNumber            The text mode to set.
+
+  @retval EFI_SUCCESS           The requested text mode is set.
+  @retval EFI_DEVICE_ERROR      The requested text mode cannot be set because of 
+                                Graphics Console device error.
+  @retval EFI_UNSUPPORTED       The text mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber
+  );
+
+/**
+  Sets the background and foreground colors for the OutputString () and
+  ClearScreen () functions.
+
+  Implements SIMPLE_TEXT_OUTPUT.SetAttribute().
+
+  @param  This                  Protocol instance pointer.
+  @param  Attribute             The attribute to set. Bits 0..3 are the foreground
+                                color, and bits 4..6 are the background color. 
+                                All other bits are undefined and must be zero.
+
+  @retval EFI_SUCCESS           The requested attribute is set.
+  @retval EFI_DEVICE_ERROR      The requested attribute cannot be set due to Graphics Console port error.
+  @retval EFI_UNSUPPORTED       The attribute requested is not defined.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Attribute
+  );
+
+/**
+  Clears the output device(s) display to the currently selected background 
+  color.
+
+  Implements SIMPLE_TEXT_OUTPUT.ClearScreen().
+
+  @param  This                  Protocol instance pointer.
+
+  @retval  EFI_SUCCESS      The operation completed successfully.
+  @retval  EFI_DEVICE_ERROR The device had an error and could not complete the request.
+  @retval  EFI_UNSUPPORTED  The output device is not in a valid text mode.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  );
+
+/**
+  Sets the current coordinates of the cursor position.
+
+  Implements SIMPLE_TEXT_OUTPUT.SetCursorPosition().
+
+  @param  This        Protocol instance pointer.
+  @param  Column      The position to set the cursor to. Must be greater than or
+                      equal to zero and less than the number of columns and rows
+                      by QueryMode ().
+  @param  Row         The position to set the cursor to. Must be greater than or
+                      equal to zero and less than the number of columns and rows
+                      by QueryMode ().
+
+  @retval EFI_SUCCESS      The operation completed successfully.
+  @retval EFI_DEVICE_ERROR The device had an error and could not complete the request.
+  @retval EFI_UNSUPPORTED  The output device is not in a valid text mode, or the
+                           cursor position is invalid for the current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Column,
+  IN  UINTN                            Row
+  );
+
+
+/**
+  Makes the cursor visible or invisible.
+
+  Implements SIMPLE_TEXT_OUTPUT.EnableCursor().
+
+  @param  This                  Protocol instance pointer.
+  @param  Visible               If TRUE, the cursor is set to be visible, If FALSE,
+                                the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleConOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          Visible
+  );
+
+/**
+  Test to see if Graphics Console could be supported on the Controller.
+
+  Graphics Console could be supported if Graphics Output Protocol or UGA Draw
+  Protocol exists on the Controller. (UGA Draw Protocol could be skipped
+  if PcdUgaConsumeSupport is set to FALSE.)
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+
+/**
+  Start this driver on Controller by opening Graphics Output protocol or 
+  UGA Draw protocol, and installing Simple Text Out protocol on Controller.
+  (UGA Draw protocol could be skipped if PcdUgaConsumeSupport is set to FALSE.)
+  
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to Controller.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on Controller by removing Simple Text Out protocol 
+  and closing the Graphics Output Protocol or UGA Draw protocol on Controller.
+  (UGA Draw protocol could be skipped if PcdUgaConsumeSupport is set to FALSE.)
+  
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed Controller.
+  @retval EFI_NOT_STARTED   Simple Text Out protocol could not be found the 
+                            Controller.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+GraphicsConsoleControllerDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+
+/**
+  Locate HII Database protocol and HII Font protocol.
+
+  @retval  EFI_SUCCESS     HII Database protocol and HII Font protocol 
+                           are located successfully.
+  @return  other           Failed to locate HII Database protocol or 
+                           HII Font protocol.
+
+**/
+EFI_STATUS
+EfiLocateHiiProtocol (
+  VOID
+  );
+
+
+/**
+  Gets Graphics Console devcie's foreground color and background color.
+
+  @param  This                  Protocol instance pointer.
+  @param  Foreground            Returned text foreground color.
+  @param  Background            Returned text background color.
+
+  @retval EFI_SUCCESS           It returned always.
+
+**/
+EFI_STATUS
+GetTextColors (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL    *Foreground,
+  OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL    *Background
+  );
+
+/**
+  Draw Unicode string on the Graphics Console device's screen.
+
+  @param  This                  Protocol instance pointer.
+  @param  UnicodeWeight         One Unicode string to be displayed.
+  @param  Count                 The count of Unicode string.
+
+  @retval EFI_OUT_OF_RESOURCES  If no memory resource to use.
+  @retval EFI_UNSUPPORTED       If no Graphics Output protocol and UGA Draw
+                                protocol exist.
+  @retval EFI_SUCCESS           Drawing Unicode string implemented successfully.
+
+**/
+EFI_STATUS
+DrawUnicodeWeightAtCursorN (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *UnicodeWeight,
+  IN  UINTN                            Count
+  );
+
+/**
+  Flush the cursor on the screen.
+  
+  If CursorVisible is FALSE, nothing to do and return directly.
+  If CursorVisible is TRUE, 
+     i) If the cursor shows on screen, it will be erased.
+    ii) If the cursor does not show on screen, it will be shown. 
+
+  @param  This                  Protocol instance pointer.
+
+  @retval EFI_SUCCESS           The cursor is erased successfully.
+
+**/
+EFI_STATUS
+FlushCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  );
+
+/**
+  Check if the current specific mode supported the user defined resolution
+  for the Graphics Console device based on Graphics Output Protocol.
+
+  If yes, set the graphic device's current mode to this specific mode.
+  
+  @param  GraphicsOutput        Graphics Output Protocol instance pointer.
+  @param  HorizontalResolution  User defined horizontal resolution
+  @param  VerticalResolution    User defined vertical resolution.
+  @param  CurrentModeNumber     Current specific mode to be check.
+
+  @retval EFI_SUCCESS       The mode is supported.
+  @retval EFI_UNSUPPORTED   The specific mode is out of range of graphics 
+                            device supported.
+  @retval other             The specific mode does not support user defined 
+                            resolution or failed to set the current mode to the 
+                            specific mode on graphics device.
+
+**/
+EFI_STATUS
+CheckModeSupported (
+  EFI_GRAPHICS_OUTPUT_PROTOCOL  *GraphicsOutput,
+  IN  UINT32  HorizontalResolution,
+  IN  UINT32  VerticalResolution,
+  OUT UINT32  *CurrentModeNumber
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.inf b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.inf
new file mode 100644
index 0000000000..ccf44979d5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.inf
@@ -0,0 +1,75 @@
+## @file
+#  Console support on graphic devices.
+#
+#  This driver will install Simple Text Output protocol by consuming Graphices Output
+#  protocol or UGA Draw protocol on graphic devices.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = GraphicsConsoleDxe
+  MODULE_UNI_FILE                = GraphicsConsoleDxe.uni
+  FILE_GUID                      = CCCB0C28-4B24-11d5-9A5A-0090273FC14D
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeGraphicsConsole
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gGraphicsConsoleDriverBinding
+#  COMPONENT_NAME                =  gGraphicsConsoleComponentName
+#  COMPONENT_NAME2               =  gGraphicsConsoleComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  LaffStd.c
+  GraphicsConsole.c
+  GraphicsConsole.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  HiiLib
+  PcdLib
+
+[Protocols]
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiSimpleTextOutProtocolGuid                 ## BY_START
+  gEfiGraphicsOutputProtocolGuid                ## TO_START
+  gEfiUgaDrawProtocolGuid                       ## TO_START
+  gEfiHiiFontProtocolGuid                       ## TO_START
+  ## TO_START
+  ## NOTIFY
+  gEfiHiiDatabaseProtocolGuid
+
+[FeaturePcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUgaConsumeSupport ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoHorizontalResolution ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVideoVerticalResolution   ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  GraphicsConsoleDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.uni b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.uni
new file mode 100644
index 0000000000..549c03beb3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxeExtra.uni b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxeExtra.uni
new file mode 100644
index 0000000000..49dfb5c8da
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/GraphicsConsoleDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/LaffStd.c b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/LaffStd.c
new file mode 100644
index 0000000000..ef9692df80
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/GraphicsConsoleDxe/LaffStd.c
@@ -0,0 +1,277 @@
+/** @file
+  Narrow font Data definition for GraphicsConsole driver.
+
+Copyright (c) 2006 - 2008, 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 "GraphicsConsole.h"
+
+
+
+EFI_NARROW_GLYPH  gUsStdNarrowGlyphData[] = {
+  //
+  // Unicode glyphs from 0x20 to 0x7e are the same as ASCII characters 0x20 to 0x7e
+  //
+  { 0x0020, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x0021, 0x00, {0x00,0x00,0x00,0x18,0x3C,0x3C,0x3C,0x18,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x0022, 0x00, {0x00,0x00,0x00,0x6C,0x6C,0x6C,0x28,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x0023, 0x00, {0x00,0x00,0x00,0x00,0x6C,0x6C,0x6C,0xFE,0x6C,0x6C,0x6C,0xFE,0x6C,0x6C,0x6C,0x00,0x00,0x00,0x00}},
+  { 0x0024, 0x00, {0x00,0x00,0x18,0x18,0x7C,0xC6,0xC6,0x60,0x38,0x0C,0x06,0xC6,0xC6,0x7C,0x18,0x18,0x00,0x00,0x00}},
+  { 0x0025, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0x0C,0x0C,0x18,0x18,0x30,0x30,0x60,0x60,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x0026, 0x00, {0x00,0x00,0x00,0x78,0xCC,0xCC,0xCC,0x78,0x76,0xDC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x0027, 0x00, {0x00,0x00,0x18,0x18,0x18,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x0028, 0x00, {0x00,0x00,0x00,0x06,0x0C,0x0C,0x18,0x18,0x18,0x18,0x18,0x18,0x0C,0x0C,0x06,0x00,0x00,0x00,0x00}},
+  { 0x0029, 0x00, {0x00,0x00,0x00,0xC0,0x60,0x60,0x30,0x30,0x30,0x30,0x30,0x30,0x60,0x60,0xC0,0x00,0x00,0x00,0x00}},
+  { 0x002a, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x6C,0x38,0xFE,0x38,0x6C,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x002b, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x7E,0x18,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x002c, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x18,0x30,0x00,0x00,0x00,0x00}},
+  { 0x002d, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x002e, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x00,0x00}},
+  { 0x002f, 0x00, {0x00,0x00,0x00,0x06,0x06,0x0C,0x0C,0x18,0x18,0x30,0x30,0x60,0x60,0xC0,0xC0,0x00,0x00,0x00,0x00}},
+  { 0x0030, 0x00, {0x00,0x00,0x00,0x38,0x6C,0xC6,0xC6,0xC6,0xD6,0xD6,0xC6,0xC6,0xC6,0x6C,0x38,0x00,0x00,0x00,0x00}},
+  { 0x0031, 0x00, {0x00,0x00,0x00,0x18,0x38,0x78,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x7E,0x00,0x00,0x00,0x00}},
+  { 0x0032, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0x06,0x06,0x06,0x0C,0x18,0x30,0x60,0xC0,0xC2,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x0033, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0x06,0x06,0x06,0x3C,0x06,0x06,0x06,0x06,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0034, 0x00, {0x00,0x00,0x00,0x1C,0x1C,0x3C,0x3C,0x6C,0x6C,0xCC,0xFE,0x0C,0x0C,0x0C,0x1E,0x00,0x00,0x00,0x00}},
+  { 0x0035, 0x00, {0x00,0x00,0x00,0xFE,0xC0,0xC0,0xC0,0xC0,0xFC,0x06,0x06,0x06,0x06,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0036, 0x00, {0x00,0x00,0x00,0x3C,0x60,0xC0,0xC0,0xC0,0xFC,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0037, 0x00, {0x00,0x00,0x00,0xFE,0xC6,0x06,0x06,0x06,0x0C,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x0038, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0039, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0x7E,0x06,0x06,0x06,0x06,0x0C,0x78,0x00,0x00,0x00,0x00}},
+  { 0x003a, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x00,0x00}},
+  { 0x003b, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x18,0x18,0x30,0x00,0x00,0x00,0x00}},
+  { 0x003c, 0x00, {0x00,0x00,0x00,0x00,0x06,0x0C,0x18,0x30,0x60,0xC0,0x60,0x30,0x18,0x0C,0x06,0x00,0x00,0x00,0x00}},
+  { 0x003d, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x00,0x00,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x003e, 0x00, {0x00,0x00,0x00,0x00,0xC0,0x60,0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x60,0xC0,0x00,0x00,0x00,0x00}},
+  { 0x003f, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0x0C,0x0C,0x18,0x18,0x18,0x00,0x00,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x0040, 0x00, {0x00,0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xDE,0xDE,0xDE,0xDC,0xC0,0xC0,0x7E,0x00,0x00,0x00,0x00}},
+
+  { 0x0041, 0x00, {0x00,0x00,0x00,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+
+  { 0x0042, 0x00, {0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x66,0x7C,0x66,0x66,0x66,0x66,0x66,0xFC,0x00,0x00,0x00,0x00}},
+  { 0x0043, 0x00, {0x00,0x00,0x00,0x3C,0x66,0xC2,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC2,0x66,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x0044, 0x00, {0x00,0x00,0x00,0xF8,0x6C,0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x6C,0xF8,0x00,0x00,0x00,0x00}},
+  { 0x0045, 0x00, {0x00,0x00,0x00,0xFE,0x66,0x62,0x60,0x68,0x78,0x68,0x60,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x0046, 0x00, {0x00,0x00,0x00,0xFE,0x66,0x62,0x60,0x64,0x7C,0x64,0x60,0x60,0x60,0x60,0xF0,0x00,0x00,0x00,0x00}},
+  { 0x0047, 0x00, {0x00,0x00,0x00,0x3C,0x66,0xC2,0xC0,0xC0,0xC0,0xDE,0xC6,0xC6,0xC6,0x66,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x0048, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x0049, 0x00, {0x00,0x00,0x00,0xFC,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0xFC,0x00,0x00,0x00,0x00}},
+  { 0x004a, 0x00, {0x00,0x00,0x00,0x1E,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0xCC,0xCC,0x78,0x00,0x00,0x00,0x00}},
+  { 0x004b, 0x00, {0x00,0x00,0x00,0xE6,0x66,0x6C,0x6C,0x78,0x70,0x78,0x6C,0x6C,0x66,0x66,0xE6,0x00,0x00,0x00,0x00}},
+  { 0x004c, 0x00, {0x00,0x00,0x00,0xF0,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x004d, 0x00, {0x00,0x00,0x00,0xC6,0xEE,0xEE,0xFE,0xFE,0xD6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x004e, 0x00, {0x00,0x00,0x00,0xC6,0xE6,0xF6,0xF6,0xF6,0xDE,0xCE,0xCE,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x004f, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0050, 0x00, {0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x66,0x66,0x7C,0x60,0x60,0x60,0x60,0xF0,0x00,0x00,0x00,0x00}},
+  { 0x0051, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xD6,0xD6,0x7C,0x1C,0x0E,0x00,0x00}},
+  { 0x0052, 0x00, {0x00,0x00,0x00,0xFC,0x66,0x66,0x66,0x66,0x7C,0x78,0x6C,0x6C,0x66,0x66,0xE6,0x00,0x00,0x00,0x00}},
+  { 0x0053, 0x00, {0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0x60,0x38,0x0C,0x06,0x06,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0054, 0x00, {0x00,0x00,0x00,0xFC,0xFC,0xB4,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x78,0x00,0x00,0x00,0x00}},
+  { 0x0055, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0056, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x6C,0x38,0x10,0x00,0x00,0x00,0x00}},
+  { 0x0057, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xD6,0xD6,0xD6,0xFE,0x6C,0x6C,0x00,0x00,0x00,0x00}},
+  { 0x0058, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0xC6,0x6C,0x6C,0x38,0x6C,0x6C,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x0059, 0x00, {0x00,0x00,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0x78,0x30,0x30,0x30,0x30,0x30,0x78,0x00,0x00,0x00,0x00}},
+  { 0x005a, 0x00, {0x00,0x00,0x00,0xFE,0xC6,0x86,0x0C,0x0C,0x18,0x30,0x60,0xC0,0xC2,0xC6,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x005b, 0x00, {0x00,0x00,0x00,0x1E,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1E,0x00,0x00,0x00,0x00}},
+  { 0x005c, 0x00, {0x00,0x00,0x00,0xC0,0xC0,0x60,0x60,0x30,0x30,0x18,0x18,0x0C,0x0C,0x06,0x06,0x00,0x00,0x00,0x00}},
+  { 0x005d, 0x00, {0x00,0x00,0x00,0xF0,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30,0xF0,0x00,0x00,0x00,0x00}},
+  { 0x005e, 0x00, {0x00,0x00,0x10,0x38,0x6C,0xC6,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x005f, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x0060, 0x00, {0x00,0x30,0x30,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x0061, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x0062, 0x00, {0x00,0x00,0x00,0xE0,0x60,0x60,0x60,0x7C,0x66,0x66,0x66,0x66,0x66,0x66,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0063, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC0,0xC0,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0064, 0x00, {0x00,0x00,0x00,0x1C,0x0C,0x0C,0x0C,0x3C,0x6C,0xCC,0xCC,0xCC,0xCC,0xCC,0x7E,0x00,0x00,0x00,0x00}},
+  { 0x0065, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0066, 0x00, {0x00,0x00,0x00,0x1E,0x33,0x30,0x30,0x30,0x78,0x30,0x30,0x30,0x30,0x30,0x78,0x00,0x00,0x00,0x00}},
+  { 0x0067, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x76,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x7C,0x0C,0xCC,0x78,0x00}},
+  { 0x0068, 0x00, {0x00,0x00,0x00,0xE0,0x60,0x60,0x60,0x7C,0x76,0x66,0x66,0x66,0x66,0x66,0xE6,0x00,0x00,0x00,0x00}},
+  { 0x0069, 0x00, {0x00,0x00,0x00,0x00,0x18,0x18,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x006a, 0x00, {0x00,0x00,0x00,0x0C,0x0C,0x0C,0x00,0x1C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x6C,0x38,0x00}},
+  { 0x006b, 0x00, {0x00,0x00,0x00,0xE0,0x60,0x60,0x66,0x6C,0x78,0x70,0x78,0x6C,0x6C,0x66,0xE6,0x00,0x00,0x00,0x00}},
+  { 0x006c, 0x00, {0x00,0x00,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x006d, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xEC,0xEE,0xFE,0xD6,0xD6,0xD6,0xD6,0xD6,0x00,0x00,0x00,0x00}},
+  { 0x006e, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x00,0x00,0x00,0x00}},
+  { 0x006f, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0070, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x66,0x66,0x66,0x66,0x66,0x66,0x7C,0x60,0x60,0xF0,0x00}},
+  { 0x0071, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x76,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x7C,0x0C,0x0C,0x1E,0x00}},
+  { 0x0072, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDC,0x66,0x60,0x60,0x60,0x60,0x60,0xF0,0x00,0x00,0x00,0x00}},
+  { 0x0073, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC0,0x7C,0x06,0x06,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x0074, 0x00, {0x00,0x00,0x00,0x10,0x30,0x30,0x30,0xFC,0x30,0x30,0x30,0x30,0x30,0x36,0x1C,0x00,0x00,0x00,0x00}},
+  { 0x0075, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x0076, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x78,0x30,0x00,0x00,0x00,0x00}},
+  { 0x0077, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0xC6,0xC6,0xD6,0xD6,0xFE,0xEE,0x6C,0x00,0x00,0x00,0x00}},
+  { 0x0078, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0x6C,0x38,0x38,0x6C,0x6C,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x0079, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7E,0x06,0x0C,0xF8,0x00}},
+  { 0x007a, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x86,0x0C,0x18,0x30,0x60,0xC0,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x007b, 0x00, {0x00,0x00,0x00,0x0E,0x18,0x18,0x18,0x18,0x30,0x18,0x18,0x18,0x18,0x18,0x0E,0x00,0x00,0x00,0x00}},
+  { 0x007c, 0x00, {0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x007d, 0x00, {0x00,0x00,0x00,0xE0,0x30,0x30,0x30,0x30,0x18,0x30,0x30,0x30,0x30,0x30,0xE0,0x00,0x00,0x00,0x00}},
+  { 0x007e, 0x00, {0x00,0x00,0x00,0x76,0xDC,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+
+  { 0x00a0, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00a1, 0x00, {0x00,0x00,0x00,0x18,0x18,0x00,0x18,0x18,0x18,0x18,0x18,0x3C,0x3C,0x3C,0x18,0x00,0x00,0x00,0x00}},
+  { 0x00a2, 0x00, {0x00,0x00,0x00,0x00,0x18,0x18,0x7C,0xC6,0xC0,0xC0,0xC0,0xC6,0x7C,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x00a3, 0x00, {0x00,0x00,0x00,0x38,0x6C,0x64,0x60,0x60,0xF0,0x60,0x60,0x60,0x60,0xE6,0xFC,0x00,0x00,0x00,0x00}},
+  { 0x00a4, 0x00, {0x00,0x00,0x18,0x00,0x00,0x00,0xC6,0x7C,0xC6,0xC6,0xC6,0xC6,0x7C,0xC6,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00a5, 0x00, {0x00,0x00,0x00,0x66,0x66,0x66,0x3C,0x18,0x7E,0x18,0x7E,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x00a6, 0x00, {0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00}},
+  { 0x00a7, 0x00, {0x00,0x00,0x18,0x7C,0xC6,0x60,0x38,0x6C,0xC6,0xC6,0x6C,0x38,0x0C,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00a8, 0x00, {0x00,0x00,0x00,0xC6,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00a9, 0x00, {0x00,0x00,0x00,0x00,0x7C,0x82,0x9A,0xA2,0xA2,0xA2,0x9A,0x82,0x7C,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00aa, 0x00, {0x00,0x00,0x00,0x00,0x3C,0x6C,0x6C,0x6C,0x3E,0x00,0x7E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00ab, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x36,0x6C,0xD8,0x6C,0x36,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00ac, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x06,0x06,0x06,0x06,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00ad, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00ae, 0x00, {0x00,0x00,0x00,0x00,0x7C,0x82,0xB2,0xAA,0xAA,0xB2,0xAA,0xAA,0x82,0x7C,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00af, 0x00, {0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b0, 0x00, {0x00,0x00,0x00,0x38,0x6C,0x6C,0x38,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b1, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x7E,0x18,0x18,0x00,0x00,0x7E,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b2, 0x00, {0x00,0x00,0x00,0x3C,0x66,0x0C,0x18,0x32,0x7E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b3, 0x00, {0x00,0x00,0x00,0x7C,0x06,0x3C,0x06,0x06,0x7C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b4, 0x00, {0x00,0x00,0x00,0x0C,0x18,0x30,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b5, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x7C,0x60,0x60,0xC0,0x00}},
+  { 0x00b6, 0x00, {0x00,0x00,0x00,0x7F,0xDB,0xDB,0xDB,0xDB,0x7B,0x1B,0x1B,0x1B,0x1B,0x1B,0x1B,0x00,0x00,0x00,0x00}},
+  { 0x00b7, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00b8, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x0C,0x78,0x00,0x00,0x00}},
+  { 0x00b9, 0x00, {0x00,0x00,0x00,0x18,0x38,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00ba, 0x00, {0x00,0x00,0x00,0x00,0x38,0x6C,0x6C,0x6C,0x38,0x00,0x7C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00bb, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8,0x6C,0x36,0x6C,0xD8,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00bc, 0x00, {0x00,0x00,0x00,0x60,0xE0,0x62,0x66,0x6C,0x18,0x30,0x66,0xCE,0x9A,0x3F,0x06,0x06,0x00,0x00,0x00}},
+  { 0x00bd, 0x00, {0x00,0x00,0x00,0x60,0xE0,0x62,0x66,0x6C,0x18,0x30,0x60,0xDC,0x86,0x0C,0x18,0x3E,0x00,0x00,0x00}},
+  { 0x00be, 0x00, {0x00,0x00,0x00,0xE0,0x30,0x62,0x36,0xEC,0x18,0x30,0x66,0xCE,0x9A,0x3F,0x06,0x06,0x00,0x00,0x00}},
+  { 0x00bf, 0x00, {0x00,0x00,0x00,0x00,0x30,0x30,0x00,0x30,0x30,0x60,0x60,0xC0,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00c0, 0x00, {0x60,0x30,0x18,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c1, 0x00, {0x18,0x30,0x60,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c2, 0x00, {0x10,0x38,0x6C,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c3, 0x00, {0x76,0xDC,0x00,0x00,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c4, 0x00, {0xCC,0xCC,0x00,0x00,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c5, 0x00, {0x38,0x6C,0x38,0x00,0x10,0x38,0x6C,0xC6,0xC6,0xC6,0xFE,0xC6,0xC6,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00c6, 0x00, {0x00,0x00,0x00,0x00,0x3E,0x6C,0xCC,0xCC,0xCC,0xFE,0xCC,0xCC,0xCC,0xCC,0xCE,0x00,0x00,0x00,0x00}},
+  { 0x00c7, 0x00, {0x00,0x00,0x00,0x3C,0x66,0xC2,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC2,0x66,0x3C,0x18,0x70,0x00,0x00}},
+  { 0x00c8, 0x00, {0x60,0x30,0x18,0x00,0xFE,0x66,0x62,0x60,0x68,0x78,0x68,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x00c9, 0x00, {0x18,0x30,0x60,0x00,0xFE,0x66,0x62,0x60,0x68,0x78,0x68,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x00ca, 0x00, {0x10,0x38,0x6C,0x00,0xFE,0x66,0x62,0x60,0x68,0x78,0x68,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x00cb, 0x00, {0xCC,0xCC,0x00,0x00,0xFE,0x66,0x62,0x60,0x68,0x78,0x68,0x60,0x62,0x66,0xFE,0x00,0x00,0x00,0x00}},
+  { 0x00cc, 0x00, {0x60,0x30,0x00,0x00,0x3C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00cd, 0x00, {0x18,0x30,0x00,0x00,0x3C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00ce, 0x00, {0x10,0x38,0x6C,0x00,0x3C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00cf, 0x00, {0xCC,0xCC,0x00,0x00,0x3C,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00d0, 0x00, {0x00,0x00,0x00,0xF8,0x6C,0x66,0x66,0x66,0xF6,0x66,0x66,0x66,0x66,0x6C,0xF8,0x00,0x00,0x00,0x00}},
+  { 0x00d1, 0x00, {0x76,0xDC,0x00,0x00,0xC6,0xE6,0xE6,0xF6,0xF6,0xDE,0xDE,0xCE,0xCE,0xC6,0xC6,0x00,0x00,0x00,0x00}},
+  { 0x00d2, 0x00, {0x60,0x30,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00d3, 0x00, {0x18,0x30,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00d4, 0x00, {0x10,0x38,0x6C,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00d5, 0x00, {0x76,0xDC,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00d6, 0x00, {0xCC,0xCC,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00d7, 0x00, {0x10,0x28,0x00,0x00,0x00,0x00,0x00,0xC6,0x6C,0x38,0x38,0x6C,0x6C,0xC6,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00d8, 0x00, {0x00,0x00,0x00,0x7C,0xCE,0xCE,0xDE,0xD6,0xD6,0xD6,0xD6,0xF6,0xE6,0xE6,0x7C,0x40,0x00,0x00,0x00}},
+  { 0x00d9, 0x00, {0x60,0x30,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00da, 0x00, {0x18,0x30,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00db, 0x00, {0x10,0x38,0x6C,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00dc, 0x00, {0xCC,0xCC,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00dd, 0x00, {0x18,0x30,0x00,0x00,0x66,0x66,0x66,0x66,0x66,0x3C,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00de, 0x00, {0x00,0x00,0x10,0x00,0xF0,0x60,0x60,0x7C,0x66,0x66,0x66,0x66,0x7C,0x60,0xF0,0x00,0x00,0x00,0x00}},
+  { 0x00df, 0x00, {0x00,0x00,0x00,0x78,0xCC,0xCC,0xCC,0xCC,0xD8,0xCC,0xC6,0xC6,0xC6,0xC6,0xCC,0x00,0x00,0x00,0x00}},
+  { 0x00e0, 0x00, {0x00,0x30,0x30,0x60,0x30,0x18,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e1, 0x00, {0x00,0x00,0x00,0x18,0x30,0x60,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e2, 0x00, {0x00,0x00,0x00,0x10,0x38,0x6C,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e3, 0x00, {0x00,0x00,0x00,0x00,0x76,0xDC,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e4, 0x00, {0x00,0x00,0x00,0xCC,0xCC,0x00,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e5, 0x00, {0x00,0x00,0x00,0x38,0x6C,0x38,0x00,0x78,0x0C,0x0C,0x7C,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00e6, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xEC,0x36,0x36,0x7E,0xD8,0xD8,0xD8,0x6E,0x00,0x00,0x00,0x00}},
+  { 0x00e7, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xC6,0xC0,0xC0,0xC0,0xC0,0xC6,0x7C,0x18,0x70,0x00,0x00}},
+  { 0x00e8, 0x00, {0x00,0x00,0x00,0x60,0x30,0x18,0x00,0x7C,0xC6,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00e9, 0x00, {0x00,0x00,0x00,0x0C,0x18,0x30,0x00,0x7C,0xC6,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00ea, 0x00, {0x00,0x00,0x00,0x10,0x38,0x6C,0x00,0x7C,0xC6,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00eb, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0x00,0x00,0x7C,0xC6,0xC6,0xFE,0xC0,0xC0,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00ec, 0x00, {0x00,0x00,0x00,0x60,0x30,0x18,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00ed, 0x00, {0x00,0x00,0x00,0x0C,0x18,0x30,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00ee, 0x00, {0x00,0x00,0x00,0x18,0x3C,0x66,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00ef, 0x00, {0x00,0x00,0x00,0x66,0x66,0x00,0x00,0x38,0x18,0x18,0x18,0x18,0x18,0x18,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00f0, 0x00, {0x00,0x00,0x00,0x34,0x18,0x2C,0x0C,0x06,0x3E,0x66,0x66,0x66,0x66,0x66,0x3C,0x00,0x00,0x00,0x00}},
+  { 0x00f1, 0x00, {0x00,0x00,0x00,0x00,0x76,0xDC,0x00,0xDC,0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x00,0x00,0x00,0x00}},
+  { 0x00f2, 0x00, {0x00,0x00,0x00,0x60,0x30,0x18,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f3, 0x00, {0x00,0x00,0x00,0x18,0x30,0x60,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f4, 0x00, {0x00,0x00,0x00,0x10,0x38,0x6C,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f5, 0x00, {0x00,0x00,0x00,0x00,0x76,0xDC,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f6, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0x00,0x00,0x7C,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f7, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x00,0x7E,0x00,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { 0x00f8, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0xCE,0xDE,0xD6,0xF6,0xE6,0xC6,0x7C,0x00,0x00,0x00,0x00}},
+  { 0x00f9, 0x00, {0x00,0x00,0x00,0x60,0x30,0x18,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00fa, 0x00, {0x00,0x00,0x00,0x18,0x30,0x60,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00fb, 0x00, {0x00,0x00,0x00,0x30,0x78,0xCC,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00fc, 0x00, {0x00,0x00,0x00,0xCC,0xCC,0x00,0x00,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0xCC,0x76,0x00,0x00,0x00,0x00}},
+  { 0x00fd, 0x00, {0x00,0x00,0x00,0x0C,0x18,0x30,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7E,0x06,0x0C,0xF8,0x00}},
+  { 0x00fe, 0x00, {0x00,0x00,0x00,0xE0,0x60,0x60,0x60,0x7C,0x66,0x66,0x66,0x66,0x66,0x66,0x7C,0x60,0x60,0xF0,0x00}},
+  { 0x00ff, 0x00, {0x00,0x00,0x00,0xC6,0xC6,0x00,0x00,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0xC6,0x7E,0x06,0x0C,0x78,0x00}},
+
+  { (CHAR16)BOXDRAW_HORIZONTAL,                 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_VERTICAL,                   0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_RIGHT,                 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_LEFT,                  0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_UP_RIGHT,                   0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_UP_LEFT,                    0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_VERTICAL_RIGHT,             0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_VERTICAL_LEFT,              0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xF8,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_HORIZONTAL,            0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_UP_HORIZONTAL,              0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_VERTICAL_HORIZONTAL,        0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xFF,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOUBLE_HORIZONTAL,          0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_DOUBLE_VERTICAL,            0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOWN_RIGHT_DOUBLE,          0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x18,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_DOUBLE_RIGHT,          0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_DOWN_RIGHT,          0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x30,0x37,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOWN_LEFT_DOUBLE,           0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF8,0x18,0xF8,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_DOUBLE_LEFT,           0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_DOWN_LEFT,           0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0x06,0xF6,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_UP_RIGHT_DOUBLE,            0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1F,0x18,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_UP_DOUBLE_RIGHT,            0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_DOUBLE_UP_RIGHT,            0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x37,0x30,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_UP_LEFT_DOUBLE,             0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xF8,0x18,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_UP_DOUBLE_LEFT,             0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_DOUBLE_UP_LEFT,             0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xF6,0x06,0xFE,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_VERTICAL_RIGHT_DOUBLE,      0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x1F,0x18,0x1F,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_VERTICAL_DOUBLE_RIGHT,      0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x37,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_VERTICAL_RIGHT,      0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x37,0x30,0x37,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_VERTICAL_LEFT_DOUBLE,       0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xF8,0x18,0xF8,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_VERTICAL_DOUBLE_LEFT,       0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xF6,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_VERTICAL_LEFT,       0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xF6,0x06,0xF6,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOWN_HORIZONTAL_DOUBLE,     0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0xFF,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_DOWN_DOUBLE_HORIZONTAL,     0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_DOWN_HORIZONTAL,     0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0xF7,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_UP_HORIZONTAL_DOUBLE,       0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xFF,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_UP_DOUBLE_HORIZONTAL,       0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_DOUBLE_UP_HORIZONTAL,       0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xF7,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)BOXDRAW_VERTICAL_HORIZONTAL_DOUBLE, 0x00, {0x18,0x18,0x18,0x18,0x18,0x18,0x18,0xFF,0x18,0xFF,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}},
+  { (CHAR16)BOXDRAW_VERTICAL_DOUBLE_HORIZONTAL, 0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xFF,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+  { (CHAR16)BOXDRAW_DOUBLE_VERTICAL_HORIZONTAL, 0x00, {0x36,0x36,0x36,0x36,0x36,0x36,0x36,0xF7,0x00,0xF7,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36}},
+
+  { (CHAR16)BLOCKELEMENT_FULL_BLOCK,            0x00, {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}},
+  { (CHAR16)BLOCKELEMENT_LIGHT_SHADE,           0x00, {0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22,0x88,0x22}},
+
+  { (CHAR16)GEOMETRICSHAPE_RIGHT_TRIANGLE,      0x00, {0x00,0x00,0x00,0x00,0x00,0xC0,0xE0,0xF0,0xF8,0xFE,0xF8,0xF0,0xE0,0xC0,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)GEOMETRICSHAPE_LEFT_TRIANGLE,       0x00, {0x00,0x00,0x00,0x00,0x00,0x06,0x0E,0x1E,0x3E,0xFE,0x3E,0x1E,0x0E,0x06,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)GEOMETRICSHAPE_UP_TRIANGLE,         0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x38,0x38,0x7C,0x7C,0xFE,0xFE,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)GEOMETRICSHAPE_DOWN_TRIANGLE,       0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0xFE,0xFE,0x7C,0x7C,0x38,0x38,0x10,0x00,0x00,0x00,0x00,0x00,0x00}},
+
+  { (CHAR16)ARROW_UP,                           0x00, {0x00,0x00,0x00,0x18,0x3C,0x7E,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)ARROW_DOWN,                         0x00, {0x00,0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x7E,0x3C,0x18,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)ARROW_LEFT,                         0x00, {0x00,0x00,0x00,0x00,0x00,0x20,0x60,0x60,0xFE,0xFE,0x60,0x60,0x20,0x00,0x00,0x00,0x00,0x00,0x00}},
+  { (CHAR16)ARROW_RIGHT,                        0x00, {0x00,0x00,0x00,0x00,0x00,0x08,0x0C,0x0C,0xFE,0xFE,0x0C,0x0C,0x08,0x00,0x00,0x00,0x00,0x00,0x00}},
+
+  { 0x0000, 0x00, {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}} //EOL
+};
+
+// Get available Unicode glyphs narrow fonts(8*19 pixels) size.
+UINT32 mNarrowFontSize =  sizeof (gUsStdNarrowGlyphData);
+
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/Ansi.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Ansi.c
new file mode 100644
index 0000000000..15147c1f2c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Ansi.c
@@ -0,0 +1,79 @@
+/** @file
+  Implementation of translation upon PC ANSI.
+
+Copyright (c) 2006 - 2008, 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 "Terminal.h"
+
+/**
+  Translate all raw data in the Raw FIFO into unicode, and insert
+  them into Unicode FIFO.
+
+  @param TerminalDevice          The terminal device.
+
+**/
+VOID
+AnsiRawDataToUnicode (
+  IN  TERMINAL_DEV    *TerminalDevice
+  )
+{
+  UINT8 RawData;
+
+  //
+  // pop the raw data out from the raw fifo,
+  // and translate it into unicode, then push
+  // the unicode into unicode fifo, until the raw fifo is empty.
+  //
+  while (!IsRawFiFoEmpty (TerminalDevice) && !IsUnicodeFiFoFull (TerminalDevice)) {
+
+    RawFiFoRemoveOneKey (TerminalDevice, &RawData);
+
+    UnicodeFiFoInsertOneKey (TerminalDevice, (UINT16) RawData);
+  }
+}
+
+/**
+  Check if input string is valid Ascii string, valid EFI control characters
+  or valid text graphics.
+
+  @param  TerminalDevice          The terminal device.
+  @param  WString                 The input string.          
+ 
+  @retval EFI_UNSUPPORTED         If not all input characters are valid.
+  @retval EFI_SUCCESS             If all input characters are valid.
+
+**/
+EFI_STATUS
+AnsiTestString (
+  IN  TERMINAL_DEV    *TerminalDevice,
+  IN  CHAR16          *WString
+  )
+{
+  CHAR8 GraphicChar;
+
+  //
+  // support three kind of character:
+  // valid ascii, valid efi control char, valid text graphics.
+  //
+  for (; *WString != CHAR_NULL; WString++) {
+
+    if ( !(TerminalIsValidAscii (*WString) ||
+        TerminalIsValidEfiCntlChar (*WString) ||
+        TerminalIsValidTextGraphics (*WString, &GraphicChar, NULL) )) {
+
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/ComponentName.c
new file mode 100644
index 0000000000..41812bb04f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/ComponentName.c
@@ -0,0 +1,237 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for Terminal driver.
+
+Copyright (c) 2006 - 2011, 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 "Terminal.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gTerminalComponentName = {
+  TerminalComponentNameGetDriverName,
+  TerminalComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gTerminalComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) TerminalComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) TerminalComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mTerminalDriverNameTable[] = {
+  {
+    "eng;en",
+   (CHAR16 *) L"Serial Terminal Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mTerminalDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gTerminalComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *SimpleTextOutput;
+  TERMINAL_DEV                     *TerminalDevice;
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gTerminalDriverBinding.DriverBindingHandle,
+             &gEfiSerialIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // This is a bus driver, so ChildHandle can not be NULL.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiSerialIoProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get our context back
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiSimpleTextOutProtocolGuid,
+                  (VOID **) &SimpleTextOutput,
+                  gTerminalDriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (SimpleTextOutput);
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           TerminalDevice->ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gTerminalComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.c
new file mode 100644
index 0000000000..6fde3b2c7a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.c
@@ -0,0 +1,1786 @@
+/** @file
+  Produces Simple Text Input Protocol, Simple Text Input Extended Protocol and
+  Simple Text Output Protocol upon Serial IO Protocol.
+
+Copyright (c) 2006 - 2014, 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 "Terminal.h"
+
+//
+// Globals
+//
+EFI_DRIVER_BINDING_PROTOCOL gTerminalDriverBinding = {
+  TerminalDriverBindingSupported,
+  TerminalDriverBindingStart,
+  TerminalDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+
+EFI_GUID  *gTerminalType[] = {
+  &gEfiPcAnsiGuid,
+  &gEfiVT100Guid,
+  &gEfiVT100PlusGuid,
+  &gEfiVTUTF8Guid,
+  &gEfiTtyTermGuid
+};
+
+
+TERMINAL_DEV  mTerminalDevTemplate = {
+  TERMINAL_DEV_SIGNATURE,
+  NULL,
+  0,
+  NULL,
+  NULL,
+  {   // SimpleTextInput
+    TerminalConInReset,
+    TerminalConInReadKeyStroke,
+    NULL
+  },
+  {   // SimpleTextOutput
+    TerminalConOutReset,
+    TerminalConOutOutputString,
+    TerminalConOutTestString,
+    TerminalConOutQueryMode,
+    TerminalConOutSetMode,
+    TerminalConOutSetAttribute,
+    TerminalConOutClearScreen,
+    TerminalConOutSetCursorPosition,
+    TerminalConOutEnableCursor,
+    NULL
+  },
+  {   // SimpleTextOutputMode
+    1,                                           // MaxMode
+    0,                                           // Mode
+    EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK),    // Attribute
+    0,                                           // CursorColumn
+    0,                                           // CursorRow
+    TRUE                                         // CursorVisible
+  },
+  NULL, // TerminalConsoleModeData
+  0,  // SerialInTimeOut
+
+  NULL, // RawFifo
+  NULL, // UnicodeFiFo
+  NULL, // EfiKeyFiFo
+
+  NULL, // ControllerNameTable
+  NULL, // TimerEvent
+  NULL, // TwoSecondTimeOut
+  INPUT_STATE_DEFAULT,
+  RESET_STATE_DEFAULT,
+  {
+      0,
+      0,
+      0
+  },
+  0,
+  FALSE,
+  {   // SimpleTextInputEx
+    TerminalConInResetEx,
+    TerminalConInReadKeyStrokeEx,
+    NULL,
+    TerminalConInSetState,
+    TerminalConInRegisterKeyNotify,
+    TerminalConInUnregisterKeyNotify,
+  },
+  {   // NotifyList
+    NULL,
+    NULL,
+  }
+};
+
+TERMINAL_CONSOLE_MODE_DATA mTerminalConsoleModeData[] = {
+  {100, 31},
+  //
+  // New modes can be added here.
+  //
+};
+
+/**
+  Test to see if this driver supports Controller.
+
+  @param  This                Protocol instance pointer.
+  @param  Controller          Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_SERIAL_IO_PROTOCOL    *SerialIo;
+  VENDOR_DEVICE_PATH        *Node;
+
+  //
+  // If remaining device path is not NULL, then make sure it is a
+  // device path that describes a terminal communications protocol.
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node,
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      Node = (VENDOR_DEVICE_PATH *) RemainingDevicePath;
+
+      if (Node->Header.Type != MESSAGING_DEVICE_PATH ||
+          Node->Header.SubType != MSG_VENDOR_DP ||
+          DevicePathNodeLength(&Node->Header) != sizeof(VENDOR_DEVICE_PATH)) {
+
+        return EFI_UNSUPPORTED;
+
+      }
+      //
+      // only supports PC ANSI, VT100, VT100+, VT-UTF8, and TtyTerm terminal types
+      //
+      if (!CompareGuid (&Node->Guid, &gEfiPcAnsiGuid) &&
+          !CompareGuid (&Node->Guid, &gEfiVT100Guid) &&
+          !CompareGuid (&Node->Guid, &gEfiVT100PlusGuid) &&
+          !CompareGuid (&Node->Guid, &gEfiVTUTF8Guid) &&
+          !CompareGuid (&Node->Guid, &gEfiTtyTermGuid)) {
+
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  // The Controller must support the Serial I/O Protocol.
+  // This driver is a bus driver with at most 1 child device, so it is
+  // ok for it to be already started.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSerialIoProtocolGuid,
+                  (VOID **) &SerialIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiSerialIoProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+/**
+  Build the terminal device path for the child device according to the
+  terminal type.
+
+  @param  ParentDevicePath         Parent device path.
+  @param  RemainingDevicePath      A specific child device.
+
+  @return The child device path built.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL*
+EFIAPI
+BuildTerminalDevpath  (
+  IN EFI_DEVICE_PATH_PROTOCOL       *ParentDevicePath,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL          *TerminalDevicePath;
+  UINT8                             TerminalType;
+  VENDOR_DEVICE_PATH                *Node;
+  EFI_STATUS                        Status;
+
+  TerminalDevicePath = NULL;
+
+  //
+  // Use the RemainingDevicePath to determine the terminal type
+  //
+  Node = (VENDOR_DEVICE_PATH *) RemainingDevicePath;
+  if (Node == NULL) {
+    TerminalType = PcdGet8 (PcdDefaultTerminalType);
+
+  } else if (CompareGuid (&Node->Guid, &gEfiPcAnsiGuid)) {
+
+    TerminalType = PCANSITYPE;
+
+  } else if (CompareGuid (&Node->Guid, &gEfiVT100Guid)) {
+
+    TerminalType = VT100TYPE;
+
+  } else if (CompareGuid (&Node->Guid, &gEfiVT100PlusGuid)) {
+
+    TerminalType = VT100PLUSTYPE;
+
+  } else if (CompareGuid (&Node->Guid, &gEfiVTUTF8Guid)) {
+
+    TerminalType = VTUTF8TYPE;
+
+  } else if (CompareGuid (&Node->Guid, &gEfiTtyTermGuid)) {
+
+    TerminalType = TTYTERMTYPE;
+
+  } else {
+    return NULL;
+  }
+
+  //
+  // Build the device path for the child device
+  //
+  Status = SetTerminalDevicePath (
+            TerminalType,
+            ParentDevicePath,
+            &TerminalDevicePath
+            );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  return TerminalDevicePath;
+}
+
+/**
+  Compare a device path data structure to that of all the nodes of a
+  second device path instance.
+
+  @param  Multi          A pointer to a multi-instance device path data structure.
+  @param  Single         A pointer to a single-instance device path data structure.
+
+  @retval TRUE           If the Single is contained within Multi.
+  @retval FALSE          The Single is not match within Multi.
+
+**/
+BOOLEAN
+MatchDevicePaths (
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Multi,
+  IN  EFI_DEVICE_PATH_PROTOCOL  *Single
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathInst;
+  UINTN                     Size;
+
+  DevicePath      = Multi;
+  DevicePathInst  = GetNextDevicePathInstance (&DevicePath, &Size);
+  //
+  // Search for the match of 'Single' in 'Multi'
+  //
+  while (DevicePathInst != NULL) {
+    //
+    // If the single device path is found in multiple device paths,
+    // return success
+    //
+    if (CompareMem (Single, DevicePathInst, Size) == 0) {
+      FreePool (DevicePathInst);
+      return TRUE;
+    }
+
+    FreePool (DevicePathInst);
+    DevicePathInst = GetNextDevicePathInstance (&DevicePath, &Size);
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether the terminal device path is in the global variable.
+
+  @param  VariableName          Pointer to one global variable.
+  @param  TerminalDevicePath    Pointer to the terminal device's device path.
+
+  @retval TRUE                  The devcie is in the global variable.
+  @retval FALSE                 The devcie is not in the global variable.
+
+**/
+BOOLEAN
+IsTerminalInConsoleVariable (
+  IN CHAR16                    *VariableName,
+  IN EFI_DEVICE_PATH_PROTOCOL  *TerminalDevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *Variable;
+  BOOLEAN                   ReturnFlag;
+
+  //
+  // Get global variable and its size according to the name given.
+  //
+  GetEfiGlobalVariable2 (VariableName, (VOID**)&Variable, NULL);
+  if (Variable == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Check whether the terminal device path is one of the variable instances.
+  //
+  ReturnFlag = MatchDevicePaths (Variable, TerminalDevicePath);
+
+  FreePool (Variable);
+
+  return ReturnFlag;
+}
+
+/**
+  Free notify functions list.
+
+  @param  ListHead               The list head
+
+  @retval EFI_SUCCESS            Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER  ListHead is NULL.
+
+**/
+EFI_STATUS
+TerminalFreeNotifyList (
+  IN OUT LIST_ENTRY           *ListHead
+  )
+{
+  TERMINAL_CONSOLE_IN_EX_NOTIFY *NotifyNode;
+
+  if (ListHead == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  while (!IsListEmpty (ListHead)) {
+    NotifyNode = CR (
+                   ListHead->ForwardLink,
+                   TERMINAL_CONSOLE_IN_EX_NOTIFY,
+                   NotifyEntry,
+                   TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                   );
+    RemoveEntryList (ListHead->ForwardLink);
+    FreePool (NotifyNode);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize all the text modes which the terminal console supports.
+
+  It returns information for available text modes that the terminal can support.
+
+  @param[out] TextModeCount      The total number of text modes that terminal console supports.
+  @param[out] TextModeData       The buffer to the text modes column and row information.
+                                 Caller is responsible to free it when it's non-NULL.
+
+  @retval EFI_SUCCESS            The supporting mode information is returned.
+  @retval EFI_INVALID_PARAMETER  The parameters are invalid.
+
+**/
+EFI_STATUS
+InitializeTerminalConsoleTextMode (
+  OUT UINTN                         *TextModeCount,
+  OUT TERMINAL_CONSOLE_MODE_DATA    **TextModeData
+  )
+{
+  UINTN                       Index;
+  UINTN                       Count;
+  TERMINAL_CONSOLE_MODE_DATA  *ModeBuffer;
+  TERMINAL_CONSOLE_MODE_DATA  *NewModeBuffer;
+  UINTN                       ValidCount;
+  UINTN                       ValidIndex;
+  
+  if ((TextModeCount == NULL) || (TextModeData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Count = sizeof (mTerminalConsoleModeData) / sizeof (TERMINAL_CONSOLE_MODE_DATA);
+  
+  //
+  // Get defined mode buffer pointer.
+  //
+  ModeBuffer = mTerminalConsoleModeData;
+    
+  //
+  // Here we make sure that the final mode exposed does not include the duplicated modes,
+  // and does not include the invalid modes which exceed the max column and row.
+  // Reserve 2 modes for 80x25, 80x50 of terminal console.
+  //
+  NewModeBuffer = AllocateZeroPool (sizeof (TERMINAL_CONSOLE_MODE_DATA) * (Count + 2));
+  ASSERT (NewModeBuffer != NULL);
+
+  //
+  // Mode 0 and mode 1 is for 80x25, 80x50 according to UEFI spec.
+  //
+  ValidCount = 0;  
+
+  NewModeBuffer[ValidCount].Columns = 80;
+  NewModeBuffer[ValidCount].Rows    = 25;
+  ValidCount++;
+
+  NewModeBuffer[ValidCount].Columns = 80;
+  NewModeBuffer[ValidCount].Rows    = 50;
+  ValidCount++;
+  
+  //
+  // Start from mode 2 to put the valid mode other than 80x25 and 80x50 in the output mode buffer.
+  //
+  for (Index = 0; Index < Count; Index++) {
+    if ((ModeBuffer[Index].Columns == 0) || (ModeBuffer[Index].Rows == 0)) {
+      //
+      // Skip the pre-defined mode which is invalid.
+      //
+      continue;
+    }
+    for (ValidIndex = 0; ValidIndex < ValidCount; ValidIndex++) {
+      if ((ModeBuffer[Index].Columns == NewModeBuffer[ValidIndex].Columns) &&
+          (ModeBuffer[Index].Rows == NewModeBuffer[ValidIndex].Rows)) {
+        //
+        // Skip the duplicated mode.
+        //
+        break;
+      }
+    }
+    if (ValidIndex == ValidCount) {
+      NewModeBuffer[ValidCount].Columns = ModeBuffer[Index].Columns;
+      NewModeBuffer[ValidCount].Rows    = ModeBuffer[Index].Rows;
+      ValidCount++;
+    }
+  }
+ 
+  DEBUG_CODE (
+    for (Index = 0; Index < ValidCount; Index++) {
+      DEBUG ((EFI_D_INFO, "Terminal - Mode %d, Column = %d, Row = %d\n", 
+                           Index, NewModeBuffer[Index].Columns, NewModeBuffer[Index].Rows));  
+    }
+  );
+  
+  //
+  // Return valid mode count and mode information buffer.
+  //
+  *TextModeCount = ValidCount;
+  *TextModeData  = NewModeBuffer;
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on Controller by opening a Serial IO protocol,
+  reading Device Path, and creating a child handle with a Simple Text In,
+  Simple Text In Ex and Simple Text Out protocol, and device path protocol.
+  And store Console Device Environment Variables.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to Controller.
+  @retval EFI_ALREADY_STARTED  This driver is already running on Controller.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_SERIAL_IO_PROTOCOL              *SerialIo;
+  EFI_DEVICE_PATH_PROTOCOL            *ParentDevicePath;
+  VENDOR_DEVICE_PATH                  *Node;
+  EFI_SERIAL_IO_MODE                  *Mode;
+  UINTN                               SerialInTimeOut;
+  TERMINAL_DEV                        *TerminalDevice;
+  UINT8                               TerminalType;
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY *OpenInfoBuffer;
+  UINTN                               EntryCount;
+  UINTN                               Index;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL     *SimpleTextOutput;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL      *SimpleTextInput;
+  BOOLEAN                             ConInSelected;
+  BOOLEAN                             ConOutSelected;
+  BOOLEAN                             NullRemaining;
+  BOOLEAN                             SimTxtInInstalled;
+  BOOLEAN                             SimTxtOutInstalled;
+  BOOLEAN                             FirstEnter;
+  UINTN                               ModeCount;
+
+  TerminalDevice     = NULL;
+  ConInSelected      = FALSE;
+  ConOutSelected     = FALSE;
+  NullRemaining      = FALSE;
+  SimTxtInInstalled  = FALSE;
+  SimTxtOutInstalled = FALSE;
+  FirstEnter         = FALSE;
+  //
+  // Get the Device Path Protocol to build the device path of the child device
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  //
+  // Open the Serial I/O Protocol BY_DRIVER.  It might already be started.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSerialIoProtocolGuid,
+                  (VOID **) &SerialIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    return Status;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    //
+    // the serial I/O protocol never be opened before, it is the first
+    // time to start the serial Io controller
+    //
+    FirstEnter = TRUE;
+  }
+
+  //
+  // Serial I/O is not already open by this driver, then tag the handle
+  // with the Terminal Driver GUID and update the ConInDev, ConOutDev, and
+  // StdErrDev variables with the list of possible terminal types on this
+  // serial port.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiCallerIdGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Controller,
+                    &gEfiCallerIdGuid,
+                    DuplicateDevicePath (ParentDevicePath),
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+
+    if (!IsHotPlugDevice (ParentDevicePath)) {
+      //
+      // if the serial device is a hot plug device, do not update the
+      // ConInDev, ConOutDev, and StdErrDev variables.
+      //
+      TerminalUpdateConsoleDevVariable (L"ConInDev", ParentDevicePath);
+      TerminalUpdateConsoleDevVariable (L"ConOutDev", ParentDevicePath);
+      TerminalUpdateConsoleDevVariable (L"ErrOutDev", ParentDevicePath);
+    }
+  }
+
+  //
+  // Check the requirement for the SimpleTxtIn and SimpleTxtOut protocols
+  //
+  // Simple In/Out Protocol will not be installed onto the handle if the
+  // device path to the handle is not present in the ConIn/ConOut
+  // environment variable. But If RemainingDevicePath is NULL, then always
+  // produce both Simple In and Simple Text Output Protocols. This is required
+  // for the connect all sequences to make sure all possible consoles are
+  // produced no matter what the current values of ConIn, ConOut, or StdErr are.
+  //
+  if (RemainingDevicePath == NULL) {
+    NullRemaining = TRUE;
+  }
+
+  DevicePath = BuildTerminalDevpath (ParentDevicePath, RemainingDevicePath);
+  if (DevicePath != NULL) {
+    ConInSelected  = IsTerminalInConsoleVariable (L"ConIn", DevicePath);
+    ConOutSelected = IsTerminalInConsoleVariable (L"ConOut", DevicePath);
+    FreePool (DevicePath);
+  } else {
+    goto Error;
+  }
+  //
+  // Not create the child terminal handle if both Simple In/In Ex and
+  // Simple text Out protocols are not required to be published
+  //
+  if ((!ConInSelected)&&(!ConOutSelected)&&(!NullRemaining)) {
+    goto Error;
+  }
+
+  //
+  // create the child terminal handle during first entry
+  //
+  if (FirstEnter) {
+    //
+    // First enther the start funciton
+    //
+    FirstEnter = FALSE;
+    //
+    // Make sure a child handle does not already exist.  This driver can only
+    // produce one child per serial port.
+    //
+    Status = gBS->OpenProtocolInformation (
+                    Controller,
+                    &gEfiSerialIoProtocolGuid,
+                    &OpenInfoBuffer,
+                    &EntryCount
+                    );
+    if (!EFI_ERROR (Status)) {
+      Status = EFI_SUCCESS;
+      for (Index = 0; Index < EntryCount; Index++) {
+        if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+          Status = EFI_ALREADY_STARTED;
+        }
+      }
+
+      FreePool (OpenInfoBuffer);
+      if (EFI_ERROR (Status)) {
+          goto Error;
+      }
+    }
+
+    //
+    // If RemainingDevicePath is NULL, use default terminal type
+    //
+    if (RemainingDevicePath == NULL) {
+      TerminalType = PcdGet8 (PcdDefaultTerminalType);
+      //
+      // Must be between PCANSITYPE (0) and TTYTERMTYPE (4)
+      //
+      ASSERT (TerminalType <= TTYTERMTYPE);
+    } else if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // Use the RemainingDevicePath to determine the terminal type
+      //
+      Node = (VENDOR_DEVICE_PATH *)RemainingDevicePath;
+      if (CompareGuid (&Node->Guid, &gEfiPcAnsiGuid)) {
+        TerminalType = PCANSITYPE;
+      } else if (CompareGuid (&Node->Guid, &gEfiVT100Guid)) {
+        TerminalType = VT100TYPE;
+      } else if (CompareGuid (&Node->Guid, &gEfiVT100PlusGuid)) {
+        TerminalType = VT100PLUSTYPE;
+      } else if (CompareGuid (&Node->Guid, &gEfiVTUTF8Guid)) {
+        TerminalType = VTUTF8TYPE;
+      } else if (CompareGuid (&Node->Guid, &gEfiTtyTermGuid)) {
+        TerminalType = TTYTERMTYPE;
+      } else {
+        goto Error;
+      }
+    } else {
+      //
+      // If RemainingDevicePath is the End of Device Path Node,
+      // skip enumerate any device and return EFI_SUCESSS
+      //
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Initialize the Terminal Dev
+    //
+    TerminalDevice = AllocateCopyPool (sizeof (TERMINAL_DEV), &mTerminalDevTemplate);
+    if (TerminalDevice == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Error;
+    }
+
+    TerminalDevice->TerminalType  = TerminalType;
+    TerminalDevice->SerialIo      = SerialIo;
+
+    InitializeListHead (&TerminalDevice->NotifyList);
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_WAIT,
+                    TPL_NOTIFY,
+                    TerminalConInWaitForKeyEx,
+                    TerminalDevice,
+                    &TerminalDevice->SimpleInputEx.WaitForKeyEx
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_WAIT,
+                    TPL_NOTIFY,
+                    TerminalConInWaitForKey,
+                    TerminalDevice,
+                    &TerminalDevice->SimpleInput.WaitForKey
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+    //
+    // Allocates and initializes the FIFO buffer to be zero, used for accommodating
+    // the pre-read pending characters.
+    //
+    TerminalDevice->RawFiFo     = AllocateZeroPool (sizeof (RAW_DATA_FIFO));
+    if (TerminalDevice->RawFiFo == NULL) {
+      goto Error;
+    }
+    TerminalDevice->UnicodeFiFo = AllocateZeroPool (sizeof (UNICODE_FIFO));
+    if (TerminalDevice->UnicodeFiFo == NULL) {
+      goto Error;
+    }
+    TerminalDevice->EfiKeyFiFo  = AllocateZeroPool (sizeof (EFI_KEY_FIFO));
+    if (TerminalDevice->EfiKeyFiFo == NULL) {
+      goto Error;
+    }
+
+    //
+    // Set the timeout value of serial buffer for
+    // keystroke response performance issue
+    //
+    Mode            = TerminalDevice->SerialIo->Mode;
+
+    SerialInTimeOut = 0;
+    if (Mode->BaudRate != 0) {
+      SerialInTimeOut = (1 + Mode->DataBits + Mode->StopBits) * 2 * 1000000 / (UINTN) Mode->BaudRate;
+    }
+
+    Status = TerminalDevice->SerialIo->SetAttributes (
+                                        TerminalDevice->SerialIo,
+                                        Mode->BaudRate,
+                                        Mode->ReceiveFifoDepth,
+                                        (UINT32) SerialInTimeOut,
+                                        (EFI_PARITY_TYPE) (Mode->Parity),
+                                        (UINT8) Mode->DataBits,
+                                        (EFI_STOP_BITS_TYPE) (Mode->StopBits)
+                                        );
+    if (EFI_ERROR (Status)) {
+      //
+      // if set attributes operation fails, invalidate
+      // the value of SerialInTimeOut,thus make it
+      // inconsistent with the default timeout value
+      // of serial buffer. This will invoke the recalculation
+      // in the readkeystroke routine.
+      //
+      TerminalDevice->SerialInTimeOut = 0;
+    } else {
+      TerminalDevice->SerialInTimeOut = SerialInTimeOut;
+    }
+    //
+    // Set Simple Text Output Protocol from template.
+    //
+    SimpleTextOutput = CopyMem (
+                         &TerminalDevice->SimpleTextOutput,
+                         &mTerminalDevTemplate.SimpleTextOutput,
+                         sizeof (mTerminalDevTemplate.SimpleTextOutput)
+                         );
+    SimpleTextOutput->Mode = &TerminalDevice->SimpleTextOutputMode;
+    
+    Status = InitializeTerminalConsoleTextMode (&ModeCount, &TerminalDevice->TerminalConsoleModeData);
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+    TerminalDevice->SimpleTextOutputMode.MaxMode = (INT32) ModeCount;
+    
+    //
+    // For terminal devices, cursor is always visible
+    //
+    TerminalDevice->SimpleTextOutputMode.CursorVisible = TRUE;
+    Status = TerminalConOutSetAttribute (
+               SimpleTextOutput,
+               EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK)
+               );
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    //
+    // Build the component name for the child device
+    //
+    TerminalDevice->ControllerNameTable = NULL;
+    switch (TerminalDevice->TerminalType) {
+    case PCANSITYPE:
+      AddUnicodeString2 (
+        "eng",
+        gTerminalComponentName.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"PC-ANSI Serial Console",
+        TRUE
+        );
+      AddUnicodeString2 (
+        "en",
+        gTerminalComponentName2.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"PC-ANSI Serial Console",
+        FALSE
+        );
+
+      break;
+
+    case VT100TYPE:
+      AddUnicodeString2 (
+        "eng",
+        gTerminalComponentName.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-100 Serial Console",
+        TRUE
+        );
+      AddUnicodeString2 (
+        "en",
+        gTerminalComponentName2.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-100 Serial Console",
+        FALSE
+        );
+
+      break;
+
+    case VT100PLUSTYPE:
+      AddUnicodeString2 (
+        "eng",
+        gTerminalComponentName.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-100+ Serial Console",
+        TRUE
+        );
+      AddUnicodeString2 (
+        "en",
+        gTerminalComponentName2.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-100+ Serial Console",
+        FALSE
+        );
+
+      break;
+
+    case VTUTF8TYPE:
+      AddUnicodeString2 (
+        "eng",
+        gTerminalComponentName.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-UTF8 Serial Console",
+        TRUE
+        );
+      AddUnicodeString2 (
+        "en",
+        gTerminalComponentName2.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"VT-UTF8 Serial Console",
+        FALSE
+        );
+
+      break;
+
+    case TTYTERMTYPE:
+      AddUnicodeString2 (
+        "eng",
+        gTerminalComponentName.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"Tty Terminal Serial Console",
+        TRUE
+        );
+      AddUnicodeString2 (
+        "en",
+        gTerminalComponentName2.SupportedLanguages,
+        &TerminalDevice->ControllerNameTable,
+        (CHAR16 *)L"Tty Terminal Serial Console",
+        FALSE
+        );
+
+      break;
+    }
+
+    //
+    // Build the device path for the child device
+    //
+    Status = SetTerminalDevicePath (
+              TerminalDevice->TerminalType,
+              ParentDevicePath,
+              &TerminalDevice->DevicePath
+              );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+
+    Status = TerminalConOutReset (SimpleTextOutput, FALSE);
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = TerminalConOutSetMode (SimpleTextOutput, 0);
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = TerminalConOutEnableCursor (SimpleTextOutput, TRUE);
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = gBS->CreateEvent (
+                    EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    TerminalConInTimerHandler,
+                    TerminalDevice,
+                    &TerminalDevice->TimerEvent
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    Status = gBS->SetTimer (
+                    TerminalDevice->TimerEvent,
+                    TimerPeriodic,
+                    KEYBOARD_TIMER_INTERVAL
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    Status = gBS->CreateEvent (
+                    EVT_TIMER,
+                    TPL_CALLBACK,
+                    NULL,
+                    NULL,
+                    &TerminalDevice->TwoSecondTimeOut
+                    );
+    ASSERT_EFI_ERROR (Status);
+
+    Status = gBS->InstallProtocolInterface (
+                    &TerminalDevice->Handle,
+                    &gEfiDevicePathProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    TerminalDevice->DevicePath
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+
+    //
+    // Register the Parent-Child relationship via
+    // EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiSerialIoProtocolGuid,
+                    (VOID **) &TerminalDevice->SerialIo,
+                    This->DriverBindingHandle,
+                    TerminalDevice->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+  }
+
+  //
+  // Find the child handle, and get its TerminalDevice private data
+  //
+  Status = gBS->OpenProtocolInformation (
+                  Controller,
+                  &gEfiSerialIoProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = EFI_NOT_FOUND;
+    ASSERT (OpenInfoBuffer != NULL);
+    for (Index = 0; Index < EntryCount; Index++) {
+      if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+        //
+        // Find the child terminal handle.
+        // Test whether the SimpleTxtIn and SimpleTxtOut have been published
+        //
+        Status = gBS->OpenProtocol (
+                        OpenInfoBuffer[Index].ControllerHandle,
+                        &gEfiSimpleTextInProtocolGuid,
+                        (VOID **) &SimpleTextInput,
+                        This->DriverBindingHandle,
+                        OpenInfoBuffer[Index].ControllerHandle,
+                        EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                        );
+        if (!EFI_ERROR (Status)) {
+          SimTxtInInstalled = TRUE;
+          TerminalDevice = TERMINAL_CON_IN_DEV_FROM_THIS (SimpleTextInput);
+        }
+
+        Status = gBS->OpenProtocol (
+                        OpenInfoBuffer[Index].ControllerHandle,
+                        &gEfiSimpleTextOutProtocolGuid,
+                        (VOID **) &SimpleTextOutput,
+                        This->DriverBindingHandle,
+                        OpenInfoBuffer[Index].ControllerHandle,
+                        EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                        );
+        if (!EFI_ERROR (Status)) {
+          SimTxtOutInstalled = TRUE;
+          TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (SimpleTextOutput);
+        }
+        Status = EFI_SUCCESS;
+        break;
+      }
+    }
+
+    FreePool (OpenInfoBuffer);
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+  } else {
+    goto ReportError;
+  }
+
+  ASSERT (TerminalDevice != NULL);
+  //
+  // Only do the reset if the device path is in the Conout variable
+  //
+  if (ConInSelected && !SimTxtInInstalled) {
+    Status = TerminalDevice->SimpleInput.Reset (
+                                          &TerminalDevice->SimpleInput,
+                                          FALSE
+                                          );
+    if (EFI_ERROR (Status)) {
+      //
+      // Need to report Error Code first
+      //
+      goto ReportError;
+    }
+  }
+
+  //
+  // Only output the configure string to remote terminal if the device path
+  // is in the Conout variable
+  //
+  if (ConOutSelected && !SimTxtOutInstalled) {
+    Status = TerminalDevice->SimpleTextOutput.SetAttribute (
+                                                        &TerminalDevice->SimpleTextOutput,
+                                                        EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK)
+                                                        );
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = TerminalDevice->SimpleTextOutput.Reset (
+                                                &TerminalDevice->SimpleTextOutput,
+                                                FALSE
+                                                );
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = TerminalDevice->SimpleTextOutput.SetMode (
+                                                &TerminalDevice->SimpleTextOutput,
+                                                0
+                                                );
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+
+    Status = TerminalDevice->SimpleTextOutput.EnableCursor (
+                                                &TerminalDevice->SimpleTextOutput,
+                                                TRUE
+                                                );
+    if (EFI_ERROR (Status)) {
+      goto ReportError;
+    }
+  }
+
+  //
+  // Simple In/Out Protocol will not be installed onto the handle if the
+  // device path to the handle is not present in the ConIn/ConOut
+  // environment variable. But If RemainingDevicePath is NULL, then always
+  // produce both Simple In and Simple Text Output Protocols. This is required
+  // for the connect all sequences to make sure all possible consoles are
+  // produced no matter what the current values of ConIn, ConOut, or StdErr are.
+  //
+  if (!SimTxtInInstalled && (ConInSelected || NullRemaining)) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &TerminalDevice->Handle,
+                    &gEfiSimpleTextInProtocolGuid,
+                    &TerminalDevice->SimpleInput,
+                    &gEfiSimpleTextInputExProtocolGuid,
+                    &TerminalDevice->SimpleInputEx,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+  }
+
+  if (!SimTxtOutInstalled && (ConOutSelected || NullRemaining)) {
+    Status = gBS->InstallProtocolInterface (
+                    &TerminalDevice->Handle,
+                    &gEfiSimpleTextOutProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &TerminalDevice->SimpleTextOutput
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Error;
+    }
+  }
+
+  return EFI_SUCCESS;
+
+ReportError:
+  //
+  // Report error code before exiting
+  //
+  DevicePath = ParentDevicePath;
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_ERROR_CODE | EFI_ERROR_MINOR,
+    (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_EC_CONTROLLER_ERROR),
+    DevicePath
+    );
+
+Error:
+  //
+  // Use the Stop() function to free all resources allocated in Start()
+  //
+  if (TerminalDevice != NULL) {
+
+    if (TerminalDevice->Handle != NULL) {
+      This->Stop (This, Controller, 1, &TerminalDevice->Handle);
+    } else {
+
+      if (TerminalDevice->TwoSecondTimeOut != NULL) {
+        gBS->CloseEvent (TerminalDevice->TwoSecondTimeOut);
+      }
+
+      if (TerminalDevice->TimerEvent != NULL) {
+        gBS->CloseEvent (TerminalDevice->TimerEvent);
+      }
+
+      if (TerminalDevice->SimpleInput.WaitForKey != NULL) {
+        gBS->CloseEvent (TerminalDevice->SimpleInput.WaitForKey);
+      }
+
+      if (TerminalDevice->SimpleInputEx.WaitForKeyEx != NULL) {
+        gBS->CloseEvent (TerminalDevice->SimpleInputEx.WaitForKeyEx);
+      }
+
+      TerminalFreeNotifyList (&TerminalDevice->NotifyList);
+
+      if (TerminalDevice->RawFiFo != NULL) {
+        FreePool (TerminalDevice->RawFiFo);
+      }
+      if (TerminalDevice->UnicodeFiFo != NULL) {
+        FreePool (TerminalDevice->UnicodeFiFo);
+      }
+      if (TerminalDevice->EfiKeyFiFo != NULL) {
+        FreePool (TerminalDevice->EfiKeyFiFo);
+      }
+
+      if (TerminalDevice->ControllerNameTable != NULL) {
+        FreeUnicodeStringTable (TerminalDevice->ControllerNameTable);
+      }
+
+      if (TerminalDevice->DevicePath != NULL) {
+        FreePool (TerminalDevice->DevicePath);
+      }
+
+      if (TerminalDevice->TerminalConsoleModeData != NULL) {
+        FreePool (TerminalDevice->TerminalConsoleModeData);
+      }
+
+      FreePool (TerminalDevice);
+    }
+  }
+
+  This->Stop (This, Controller, 0, NULL);
+
+  return Status;
+}
+
+/**
+  Stop this driver on Controller by closing Simple Text In, Simple Text
+  In Ex, Simple Text Out protocol, and removing parent device path from
+  Console Device Environment Variables.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed Controller.
+  @retval other             This driver could not be removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    Controller,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                       Status;
+  UINTN                            Index;
+  BOOLEAN                          AllChildrenStopped;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *SimpleTextOutput;
+  TERMINAL_DEV                     *TerminalDevice;
+  EFI_DEVICE_PATH_PROTOCOL         *ParentDevicePath;
+  EFI_SERIAL_IO_PROTOCOL           *SerialIo;
+  EFI_DEVICE_PATH_PROTOCOL         *DevicePath;
+
+  Status = gBS->HandleProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Complete all outstanding transactions to Controller.
+  // Don't allow any new transaction to Controller to be started.
+  //
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    Status = gBS->OpenProtocol (
+                    Controller,
+                    &gEfiCallerIdGuid,
+                    (VOID **) &ParentDevicePath,
+                    This->DriverBindingHandle,
+                    Controller,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Remove Parent Device Path from
+      // the Console Device Environment Variables
+      //
+      TerminalRemoveConsoleDevVariable (L"ConInDev", ParentDevicePath);
+      TerminalRemoveConsoleDevVariable (L"ConOutDev", ParentDevicePath);
+      TerminalRemoveConsoleDevVariable (L"ErrOutDev", ParentDevicePath);
+
+      //
+      // Uninstall the Terminal Driver's GUID Tag from the Serial controller
+      //
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Controller,
+                      &gEfiCallerIdGuid,
+                      ParentDevicePath,
+                      NULL
+                      );
+
+      //
+      // Free the ParentDevicePath that was duplicated in Start()
+      //
+      if (!EFI_ERROR (Status)) {
+        FreePool (ParentDevicePath);
+      }
+    }
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiSerialIoProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[Index],
+                    &gEfiSimpleTextOutProtocolGuid,
+                    (VOID **) &SimpleTextOutput,
+                    This->DriverBindingHandle,
+                    ChildHandleBuffer[Index],
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (!EFI_ERROR (Status)) {
+
+      TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (SimpleTextOutput);
+
+      gBS->CloseProtocol (
+            Controller,
+            &gEfiSerialIoProtocolGuid,
+            This->DriverBindingHandle,
+            ChildHandleBuffer[Index]
+            );
+
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ChildHandleBuffer[Index],
+                      &gEfiSimpleTextInProtocolGuid,
+                      &TerminalDevice->SimpleInput,
+                      &gEfiSimpleTextInputExProtocolGuid,
+                      &TerminalDevice->SimpleInputEx,
+                      &gEfiSimpleTextOutProtocolGuid,
+                      &TerminalDevice->SimpleTextOutput,
+                      &gEfiDevicePathProtocolGuid,
+                      TerminalDevice->DevicePath,
+                      NULL
+                      );
+      if (EFI_ERROR (Status)) {
+        gBS->OpenProtocol (
+              Controller,
+              &gEfiSerialIoProtocolGuid,
+              (VOID **) &SerialIo,
+              This->DriverBindingHandle,
+              ChildHandleBuffer[Index],
+              EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+              );
+      } else {
+
+        if (TerminalDevice->ControllerNameTable != NULL) {
+          FreeUnicodeStringTable (TerminalDevice->ControllerNameTable);
+        }
+
+        gBS->CloseEvent (TerminalDevice->TimerEvent);
+        gBS->CloseEvent (TerminalDevice->TwoSecondTimeOut);
+        gBS->CloseEvent (TerminalDevice->SimpleInput.WaitForKey);
+        gBS->CloseEvent (TerminalDevice->SimpleInputEx.WaitForKeyEx);
+        TerminalFreeNotifyList (&TerminalDevice->NotifyList);
+        FreePool (TerminalDevice->DevicePath);
+        if (TerminalDevice->TerminalConsoleModeData != NULL) {
+          FreePool (TerminalDevice->TerminalConsoleModeData);
+        }
+        FreePool (TerminalDevice);
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update terminal device path in Console Device Environment Variables.
+
+  @param  VariableName           The Console Device Environment Variable.
+  @param  ParentDevicePath       The terminal device path to be updated.
+
+**/
+VOID
+TerminalUpdateConsoleDevVariable (
+  IN CHAR16                    *VariableName,
+  IN EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  UINTN                     NameSize;
+  UINTN                     VariableSize;
+  UINT8                     TerminalType;
+  EFI_DEVICE_PATH_PROTOCOL  *Variable;
+  EFI_DEVICE_PATH_PROTOCOL  *NewVariable;
+  EFI_DEVICE_PATH_PROTOCOL  *TempDevicePath;
+  EDKII_SET_VARIABLE_STATUS *SetVariableStatus;
+
+  //
+  // Get global variable and its size according to the name given.
+  //
+  GetEfiGlobalVariable2 (VariableName, (VOID**)&Variable, NULL);
+  if (Variable == NULL) {
+    return;
+  }
+
+  //
+  // Append terminal device path onto the variable.
+  //
+  for (TerminalType = PCANSITYPE; TerminalType <= TTYTERMTYPE; TerminalType++) {
+    SetTerminalDevicePath (TerminalType, ParentDevicePath, &TempDevicePath);
+    NewVariable = AppendDevicePathInstance (Variable, TempDevicePath);
+    ASSERT (NewVariable != NULL);
+    if (Variable != NULL) {
+      FreePool (Variable);
+    }
+
+    if (TempDevicePath != NULL) {
+      FreePool (TempDevicePath);
+    }
+
+    Variable = NewVariable;
+  }
+
+  VariableSize = GetDevicePathSize (Variable);
+
+  Status = gRT->SetVariable (
+                  VariableName,
+                  &gEfiGlobalVariableGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                  VariableSize,
+                  Variable
+                  );
+
+  if (EFI_ERROR (Status)) {
+    NameSize = StrSize (VariableName);
+    SetVariableStatus = AllocatePool (sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + VariableSize);
+    if (SetVariableStatus != NULL) {
+      CopyGuid (&SetVariableStatus->Guid, &gEfiGlobalVariableGuid);
+      SetVariableStatus->NameSize   = NameSize;
+      SetVariableStatus->DataSize   = VariableSize;
+      SetVariableStatus->SetStatus  = Status;
+      SetVariableStatus->Attributes = EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS;
+      CopyMem (SetVariableStatus + 1,                          VariableName, NameSize);
+      CopyMem (((UINT8 *) (SetVariableStatus + 1)) + NameSize, Variable,     VariableSize);
+
+      REPORT_STATUS_CODE_EX (
+        EFI_ERROR_CODE,
+        PcdGet32 (PcdErrorCodeSetVariable),
+        0,
+        NULL,
+        &gEdkiiStatusCodeDataTypeVariableGuid,
+        SetVariableStatus,
+        sizeof (EDKII_SET_VARIABLE_STATUS) + NameSize + VariableSize
+        );
+
+      FreePool (SetVariableStatus);
+    }
+  }
+
+  FreePool (Variable);
+
+  return ;
+}
+
+
+/**
+  Remove terminal device path from Console Device Environment Variables.
+
+  @param  VariableName           Console Device Environment Variables.
+  @param  ParentDevicePath       The terminal device path to be updated.
+
+**/
+VOID
+TerminalRemoveConsoleDevVariable (
+  IN CHAR16                    *VariableName,
+  IN EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  BOOLEAN                   FoundOne;
+  BOOLEAN                   Match;
+  UINTN                     VariableSize;
+  UINTN                     InstanceSize;
+  UINT8                     TerminalType;
+  EFI_DEVICE_PATH_PROTOCOL  *Instance;
+  EFI_DEVICE_PATH_PROTOCOL  *Variable;
+  EFI_DEVICE_PATH_PROTOCOL  *OriginalVariable;
+  EFI_DEVICE_PATH_PROTOCOL  *NewVariable;
+  EFI_DEVICE_PATH_PROTOCOL  *SavedNewVariable;
+  EFI_DEVICE_PATH_PROTOCOL  *TempDevicePath;
+
+  Instance  = NULL;
+
+  //
+  // Get global variable and its size according to the name given.
+  //
+  GetEfiGlobalVariable2 (VariableName, (VOID**)&Variable, NULL);
+  if (Variable == NULL) {
+    return ;
+  }
+
+  FoundOne          = FALSE;
+  OriginalVariable  = Variable;
+  NewVariable       = NULL;
+
+  //
+  // Get first device path instance from Variable
+  //
+  Instance = GetNextDevicePathInstance (&Variable, &InstanceSize);
+  if (Instance == NULL) {
+    FreePool (OriginalVariable);
+    return ;
+  }
+  //
+  // Loop through all the device path instances of Variable
+  //
+  do {
+    //
+    // Loop through all the terminal types that this driver supports
+    //
+    Match = FALSE;
+    for (TerminalType = PCANSITYPE; TerminalType <= TTYTERMTYPE; TerminalType++) {
+
+      SetTerminalDevicePath (TerminalType, ParentDevicePath, &TempDevicePath);
+
+      //
+      // Compare the generated device path to the current device path instance
+      //
+      if (TempDevicePath != NULL) {
+        if (CompareMem (Instance, TempDevicePath, InstanceSize) == 0) {
+          Match     = TRUE;
+          FoundOne  = TRUE;
+        }
+
+        FreePool (TempDevicePath);
+      }
+    }
+    //
+    // If a match was not found, then keep the current device path instance
+    //
+    if (!Match) {
+      SavedNewVariable  = NewVariable;
+      NewVariable       = AppendDevicePathInstance (NewVariable, Instance);
+      if (SavedNewVariable != NULL) {
+        FreePool (SavedNewVariable);
+      }
+    }
+    //
+    // Get next device path instance from Variable
+    //
+    FreePool (Instance);
+    Instance = GetNextDevicePathInstance (&Variable, &InstanceSize);
+  } while (Instance != NULL);
+
+  FreePool (OriginalVariable);
+
+  if (FoundOne) {
+    VariableSize = GetDevicePathSize (NewVariable);
+
+    Status = gRT->SetVariable (
+                    VariableName,
+                    &gEfiGlobalVariableGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+                    VariableSize,
+                    NewVariable
+                    );
+    //
+    // Shrinking variable with existing variable driver implementation shouldn't fail.
+    //
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  if (NewVariable != NULL) {
+    FreePool (NewVariable);
+  }
+
+  return ;
+}
+
+/**
+  Build terminal device path according to terminal type.
+
+  @param  TerminalType           The terminal type is PC ANSI, VT100, VT100+ or VT-UTF8.
+  @param  ParentDevicePath       Parent device path.
+  @param  TerminalDevicePath     Returned terminal device path, if building successfully.
+
+  @retval EFI_UNSUPPORTED        Terminal does not belong to the supported type.
+  @retval EFI_OUT_OF_RESOURCES   Generate terminal device path failed.
+  @retval EFI_SUCCESS            Build terminal device path successfully.
+
+**/
+EFI_STATUS
+SetTerminalDevicePath (
+  IN  UINT8                       TerminalType,
+  IN  EFI_DEVICE_PATH_PROTOCOL    *ParentDevicePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL    **TerminalDevicePath
+  )
+{
+  VENDOR_DEVICE_PATH  Node;
+
+  *TerminalDevicePath = NULL;
+  Node.Header.Type    = MESSAGING_DEVICE_PATH;
+  Node.Header.SubType = MSG_VENDOR_DP;
+
+  //
+  // Generate terminal device path node according to terminal type.
+  //
+  switch (TerminalType) {
+
+  case PCANSITYPE:
+    CopyGuid (&Node.Guid, &gEfiPcAnsiGuid);
+    break;
+
+  case VT100TYPE:
+    CopyGuid (&Node.Guid, &gEfiVT100Guid);
+    break;
+
+  case VT100PLUSTYPE:
+    CopyGuid (&Node.Guid, &gEfiVT100PlusGuid);
+    break;
+
+  case VTUTF8TYPE:
+    CopyGuid (&Node.Guid, &gEfiVTUTF8Guid);
+    break;
+
+  case TTYTERMTYPE:
+    CopyGuid (&Node.Guid, &gEfiTtyTermGuid);
+    break;
+
+  default:
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get VENDOR_DEVCIE_PATH size and put into Node.Header
+  //
+  SetDevicePathNodeLength (
+    &Node.Header,
+    sizeof (VENDOR_DEVICE_PATH)
+    );
+
+  //
+  // Append the terminal node onto parent device path
+  // to generate a complete terminal device path.
+  //
+  *TerminalDevicePath = AppendDevicePathNode (
+                          ParentDevicePath,
+                          (EFI_DEVICE_PATH_PROTOCOL *) &Node
+                          );
+  if (*TerminalDevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user Entry Point for module Terminal. The user code starts with this function.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeTerminal(
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gTerminalDriverBinding,
+             ImageHandle,
+             &gTerminalComponentName,
+             &gTerminalComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Check if the device supports hot-plug through its device path.
+
+  This function could be updated to check more types of Hot Plug devices.
+  Currently, it checks USB and PCCard device.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a hot-plug device
+  @retval FALSE                 The devcie is not a hot-plug device.
+
+**/
+BOOLEAN
+IsHotPlugDevice (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL     *CheckDevicePath;
+
+  CheckDevicePath = DevicePath;
+  while (!IsDevicePathEnd (CheckDevicePath)) {
+    //
+    // Check device whether is hot plug device or not throught Device Path
+    //
+    if ((DevicePathType (CheckDevicePath) == MESSAGING_DEVICE_PATH) &&
+        (DevicePathSubType (CheckDevicePath) == MSG_USB_DP ||
+         DevicePathSubType (CheckDevicePath) == MSG_USB_CLASS_DP ||
+         DevicePathSubType (CheckDevicePath) == MSG_USB_WWID_DP)) {
+      //
+      // If Device is USB device
+      //
+      return TRUE;
+    }
+    if ((DevicePathType (CheckDevicePath) == HARDWARE_DEVICE_PATH) &&
+        (DevicePathSubType (CheckDevicePath) == HW_PCCARD_DP)) {
+      //
+      // If Device is PCCard
+      //
+      return TRUE;
+    }
+
+    CheckDevicePath = NextDevicePathNode (CheckDevicePath);
+  }
+
+  return FALSE;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.h b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.h
new file mode 100644
index 0000000000..269d2aeb5a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Terminal.h
@@ -0,0 +1,1361 @@
+/** @file
+  Header file for Terminal driver.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _TERMINAL_H_
+#define _TERMINAL_H_
+
+
+#include <Uefi.h>
+
+#include <Guid/GlobalVariable.h>
+#include <Guid/PcAnsi.h>
+#include <Guid/TtyTerm.h>
+#include <Guid/StatusCodeDataTypeVariable.h>
+
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/SerialIo.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/SimpleTextInEx.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseLib.h>
+
+
+#define RAW_FIFO_MAX_NUMBER 256
+#define FIFO_MAX_NUMBER     128
+
+typedef struct {
+  UINT8 Head;
+  UINT8 Tail;
+  UINT8 Data[RAW_FIFO_MAX_NUMBER + 1];
+} RAW_DATA_FIFO;
+
+typedef struct {
+  UINT8   Head;
+  UINT8   Tail;
+  UINT16  Data[FIFO_MAX_NUMBER + 1];
+} UNICODE_FIFO;
+
+typedef struct {
+  UINT8         Head;
+  UINT8         Tail;
+  EFI_INPUT_KEY Data[FIFO_MAX_NUMBER + 1];
+} EFI_KEY_FIFO;
+
+typedef struct {
+  UINTN   Columns;
+  UINTN   Rows;
+} TERMINAL_CONSOLE_MODE_DATA;
+
+#define KEYBOARD_TIMER_INTERVAL         200000  // 0.02s
+
+#define TERMINAL_DEV_SIGNATURE  SIGNATURE_32 ('t', 'm', 'n', 'l')
+
+#define TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE SIGNATURE_32 ('t', 'm', 'e', 'n')
+
+typedef struct _TERMINAL_CONSOLE_IN_EX_NOTIFY {
+  UINTN                                 Signature;
+  EFI_KEY_DATA                          KeyData;
+  EFI_KEY_NOTIFY_FUNCTION               KeyNotificationFn;
+  LIST_ENTRY                            NotifyEntry;
+} TERMINAL_CONSOLE_IN_EX_NOTIFY;
+typedef struct {
+  UINTN                               Signature;
+  EFI_HANDLE                          Handle;
+  UINT8                               TerminalType;
+  EFI_SERIAL_IO_PROTOCOL              *SerialIo;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  EFI_SIMPLE_TEXT_INPUT_PROTOCOL      SimpleInput;
+  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL     SimpleTextOutput;
+  EFI_SIMPLE_TEXT_OUTPUT_MODE         SimpleTextOutputMode;
+  TERMINAL_CONSOLE_MODE_DATA          *TerminalConsoleModeData;
+  UINTN                               SerialInTimeOut;
+  RAW_DATA_FIFO                       *RawFiFo;
+  UNICODE_FIFO                        *UnicodeFiFo;
+  EFI_KEY_FIFO                        *EfiKeyFiFo;
+  EFI_UNICODE_STRING_TABLE            *ControllerNameTable;
+  EFI_EVENT                           TimerEvent;
+  EFI_EVENT                           TwoSecondTimeOut;
+  UINT32                              InputState;
+  UINT32                              ResetState;
+  UINT16                              TtyEscapeStr[3];
+  INTN                                TtyEscapeIndex;
+
+  //
+  // Esc could not be output to the screen by user,
+  // but the terminal driver need to output it to
+  // the terminal emulation software to send control sequence.
+  // This boolean is used by the terminal driver only
+  // to indicate whether the Esc could be sent or not.
+  //
+  BOOLEAN                             OutputEscChar;
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL   SimpleInputEx;
+  LIST_ENTRY                          NotifyList;
+} TERMINAL_DEV;
+
+#define INPUT_STATE_DEFAULT               0x00
+#define INPUT_STATE_ESC                   0x01
+#define INPUT_STATE_CSI                   0x02
+#define INPUT_STATE_LEFTOPENBRACKET       0x04
+#define INPUT_STATE_O                     0x08
+#define INPUT_STATE_2                     0x10
+#define INPUT_STATE_LEFTOPENBRACKET_2     0x20
+
+#define RESET_STATE_DEFAULT               0x00
+#define RESET_STATE_ESC_R                 0x01
+#define RESET_STATE_ESC_R_ESC_R           0x02
+
+#define TERMINAL_CON_IN_DEV_FROM_THIS(a)  CR (a, TERMINAL_DEV, SimpleInput, TERMINAL_DEV_SIGNATURE)
+#define TERMINAL_CON_OUT_DEV_FROM_THIS(a) CR (a, TERMINAL_DEV, SimpleTextOutput, TERMINAL_DEV_SIGNATURE)
+#define TERMINAL_CON_IN_EX_DEV_FROM_THIS(a)  CR (a, TERMINAL_DEV, SimpleInputEx, TERMINAL_DEV_SIGNATURE)
+
+typedef union {
+  UINT8 Utf8_1;
+  UINT8 Utf8_2[2];
+  UINT8 Utf8_3[3];
+} UTF8_CHAR;
+
+#define PCANSITYPE                0
+#define VT100TYPE                 1
+#define VT100PLUSTYPE             2
+#define VTUTF8TYPE                3
+#define TTYTERMTYPE               4
+
+#define LEFTOPENBRACKET           0x5b  // '['
+#define ACAP                      0x41
+#define BCAP                      0x42
+#define CCAP                      0x43
+#define DCAP                      0x44
+
+#define BACKSPACE                 8
+#define ESC                       27
+#define CSI                       0x9B
+#define DEL                       127
+#define BRIGHT_CONTROL_OFFSET     2
+#define FOREGROUND_CONTROL_OFFSET 6
+#define BACKGROUND_CONTROL_OFFSET 11
+#define ROW_OFFSET                2
+#define COLUMN_OFFSET             5
+
+typedef struct {
+  UINT16  Unicode;
+  CHAR8   PcAnsi;
+  CHAR8   Ascii;
+} UNICODE_TO_CHAR;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gTerminalDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gTerminalComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gTerminalComponentName2;
+
+/**
+  The user Entry Point for module Terminal. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeTerminal (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_INPUT_PROTOCOL.Reset().
+  This driver only perform dependent serial device reset regardless of
+  the value of ExtendeVerification
+
+  @param  This                     Indicates the calling context.
+  @param  ExtendedVerification     Skip by this driver.
+
+  @retval EFI_SUCCESS              The reset operation succeeds.
+  @retval EFI_DEVICE_ERROR         The dependent serial port reset fails.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL    *This,
+  IN  BOOLEAN                           ExtendedVerification
+  );
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_INPUT_PROTOCOL.ReadKeyStroke().
+
+  @param  This                Indicates the calling context.
+  @param  Key                 A pointer to a buffer that is filled in with the
+                              keystroke information for the key that was sent
+                              from terminal.
+
+  @retval EFI_SUCCESS         The keystroke information is returned successfully.
+  @retval EFI_NOT_READY       There is no keystroke data available.
+  @retval EFI_DEVICE_ERROR    The dependent serial device encounters error.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  OUT EFI_INPUT_KEY                   *Key
+  );
+
+/**
+  Check if the key already has been registered.
+
+  @param  RegsiteredData           A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   registered.
+  @param  InputData                A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval TRUE                     Key be pressed matches a registered key.
+  @retval FALSE                    Match failed.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  );
+
+/**
+  Event notification function for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx event
+  Signal the event if there is key available
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+
+**/
+VOID
+EFIAPI
+TerminalConInWaitForKeyEx (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  );
+
+//
+// Simple Text Input Ex protocol prototypes
+//
+
+/**
+  Reset the input device and optionally run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existence of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR         The keystroke information was not returned due
+                                   to hardware errors.
+  @retval EFI_INVALID_PARAMETER    KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  );
+
+/**
+  Set certain state for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyToggleState           A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                   state for the input device.
+
+  @retval EFI_SUCCESS              The device state was set successfully.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not have the setting adjusted.
+  @retval EFI_UNSUPPORTED          The device does not have the ability to set its
+                                   state.
+  @retval EFI_INVALID_PARAMETER    KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  );
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke information data for the key that was
+                                   pressed.
+  @param  KeyNotificationFunction  Points to the function to be called when the key
+                                   sequence is typed specified by KeyData.
+  @param  NotifyHandle             Points to the unique handle assigned to the
+                                   registered notification.
+
+  @retval EFI_SUCCESS              The notification function was registered
+                                   successfully.
+  @retval EFI_OUT_OF_RESOURCES     Unable to allocate resources for necesssary data
+                                   structures.
+  @retval EFI_INVALID_PARAMETER    KeyData or NotifyHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInRegisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_DATA                       *KeyData,
+  IN EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                              **NotifyHandle
+  );
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                     Protocol instance pointer.
+  @param  NotificationHandle       The handle of the notification function being
+                                   unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered
+                                   successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid.
+  @retval EFI_NOT_FOUND            Can not find the matching entry in database.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  );
+
+/**
+  Event notification function for EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey event
+  Signal the event if there is key available
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+
+**/
+VOID
+EFIAPI
+TerminalConInWaitForKey (
+  IN  EFI_EVENT     Event,
+  IN  VOID          *Context
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.Reset().
+  If ExtendeVerification is TRUE, then perform dependent serial device reset,
+  and set display mode to mode 0.
+  If ExtendedVerification is FALSE, only set display mode to mode 0.
+
+  @param  This                  Indicates the calling context.
+  @param  ExtendedVerification  Indicates that the driver may perform a more
+                                exhaustive verification operation of the device
+                                during reset.
+
+  @retval EFI_SUCCESS           The reset operation succeeds.
+  @retval EFI_DEVICE_ERROR      The terminal is not functioning correctly or the serial port reset fails.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL    *This,
+  IN  BOOLEAN                            ExtendedVerification
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.OutputString().
+  The Unicode string will be converted to terminal expressible data stream
+  and send to terminal via serial port.
+
+  @param  This                    Indicates the calling context.
+  @param  WString                 The Null-terminated Unicode string to be displayed
+                                  on the terminal screen.
+
+  @retval EFI_SUCCESS             The string is output successfully.
+  @retval EFI_DEVICE_ERROR        The serial port fails to send the string out.
+  @retval EFI_WARN_UNKNOWN_GLYPH  Indicates that some of the characters in the Unicode string could not
+                                  be rendered and are skipped.
+  @retval EFI_UNSUPPORTED         If current display mode is out of range.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutOutputString (
+  IN   EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                            *WString
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.TestString().
+  If one of the characters in the *Wstring is
+  neither valid Unicode drawing characters,
+  not ASCII code, then this function will return
+  EFI_UNSUPPORTED.
+
+  @param  This              Indicates the calling context.
+  @param  WString           The Null-terminated Unicode string to be tested.
+
+  @retval EFI_SUCCESS       The terminal is capable of rendering the output string.
+  @retval EFI_UNSUPPORTED   Some of the characters in the Unicode string cannot be rendered.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.QueryMode().
+  It returns information for an available text mode
+  that the terminal supports.
+  In this driver, we support text mode 80x25 (mode 0),
+  80x50 (mode 1), 100x31 (mode 2).
+
+  @param This        Indicates the calling context.
+  @param ModeNumber  The mode number to return information on.
+  @param Columns     The returned columns of the requested mode.
+  @param Rows        The returned rows of the requested mode.
+
+  @retval EFI_SUCCESS       The requested mode information is returned.
+  @retval EFI_UNSUPPORTED   The mode number is not valid.
+  @retval EFI_DEVICE_ERROR
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber,
+  OUT UINTN                            *Columns,
+  OUT UINTN                            *Rows
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUT.SetMode().
+  Set the terminal to a specified display mode.
+  In this driver, we only support mode 0.
+
+  @param This          Indicates the calling context.
+  @param ModeNumber    The text mode to set.
+
+  @retval EFI_SUCCESS       The requested text mode is set.
+  @retval EFI_DEVICE_ERROR  The requested text mode cannot be set
+                            because of serial device error.
+  @retval EFI_UNSUPPORTED   The text mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetAttribute().
+
+  @param This        Indicates the calling context.
+  @param Attribute   The attribute to set. Only bit0..6 are valid, all other bits
+                     are undefined and must be zero.
+
+  @retval EFI_SUCCESS        The requested attribute is set.
+  @retval EFI_DEVICE_ERROR   The requested attribute cannot be set due to serial port error.
+  @retval EFI_UNSUPPORTED    The attribute requested is not defined by EFI spec.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Attribute
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.ClearScreen().
+  It clears the ANSI terminal's display to the
+  currently selected background color.
+
+  @param This     Indicates the calling context.
+
+  @retval EFI_SUCCESS       The operation completed successfully.
+  @retval EFI_DEVICE_ERROR  The terminal screen cannot be cleared due to serial port error.
+  @retval EFI_UNSUPPORTED   The terminal is not in a valid display mode.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  );
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetCursorPosition().
+
+  @param This      Indicates the calling context.
+  @param Column    The row to set cursor to.
+  @param Row       The column to set cursor to.
+
+  @retval EFI_SUCCESS       The operation completed successfully.
+  @retval EFI_DEVICE_ERROR  The request fails due to serial port error.
+  @retval EFI_UNSUPPORTED   The terminal is not in a valid text mode, or the cursor position
+                            is invalid for current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Column,
+  IN  UINTN                            Row
+  );
+
+/**
+  Implements SIMPLE_TEXT_OUTPUT.EnableCursor().
+  In this driver, the cursor cannot be hidden.
+
+  @param This      Indicates the calling context.
+  @param Visible   If TRUE, the cursor is set to be visible,
+                   If FALSE, the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS      The request is valid.
+  @retval EFI_UNSUPPORTED  The terminal does not support cursor hidden.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          Visible
+  );
+
+/**
+  Test to see if this driver supports Controller.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start this driver on Controller by opening a Serial IO protocol,
+  reading Device Path, and creating a child handle with a Simple Text In,
+  Simple Text In Ex and Simple Text Out protocol, and device path protocol.
+  And store Console Device Environment Variables.
+
+  @param  This                 Protocol instance pointer.
+  @param  Controller           Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to Controller.
+  @retval EFI_ALREADY_STARTED  This driver is already running on Controller.
+  @retval other                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+
+/**
+  Stop this driver on Controller by closing Simple Text In, Simple Text
+  In Ex, Simple Text Out protocol, and removing parent device path from
+  Console Device Environment Variables.
+
+  @param  This              Protocol instance pointer.
+  @param  Controller        Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed Controller.
+  @retval other             This driver could not be removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+/**
+  Free notify functions list.
+
+  @param  ListHead               The list head
+
+  @retval EFI_SUCCESS            Free the notify list successfully.
+  @retval EFI_INVALID_PARAMETER  ListHead is NULL.
+
+**/
+EFI_STATUS
+TerminalFreeNotifyList (
+  IN OUT LIST_ENTRY           *ListHead
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// internal functions
+//
+
+/**
+  Check for a pending key in the Efi Key FIFO or Serial device buffer.
+
+  @param  This                     Indicates the calling context.
+
+  @retval EFI_SUCCESS              There is key pending.
+  @retval EFI_NOT_READY            There is no key pending.
+  @retval EFI_DEVICE_ERROR         If Serial IO is not attached to serial device.
+
+**/
+EFI_STATUS
+TerminalConInCheckForKey (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This
+  );
+
+/**
+  Update terminal device path in Console Device Environment Variables.
+
+  @param  VariableName           The Console Device Environment Variable.
+  @param  ParentDevicePath       The terminal device path to be updated.
+
+  @return None.
+
+**/
+VOID
+TerminalUpdateConsoleDevVariable (
+  IN CHAR16                    *VariableName,
+  IN EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath
+  );
+
+/**
+  Remove console device variable.
+
+  @param  VariableName           A pointer to the variable name.
+  @param  ParentDevicePath       A pointer to the parent device path.
+
+**/
+VOID
+TerminalRemoveConsoleDevVariable (
+  IN CHAR16                    *VariableName,
+  IN EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath
+  );
+
+/**
+  Build termial device path according to terminal type.
+
+  @param  TerminalType           The terminal type is PC ANSI, VT100, VT100+ or VT-UTF8.
+  @param  ParentDevicePath       Parent device path.
+  @param  TerminalDevicePath     Returned terminal device path, if building successfully.
+
+  @retval EFI_UNSUPPORTED        Terminal does not belong to the supported type.
+  @retval EFI_OUT_OF_RESOURCES   Generate terminal device path failed.
+  @retval EFI_SUCCESS            Build terminal device path successfully.
+
+**/
+EFI_STATUS
+SetTerminalDevicePath (
+  IN  UINT8                       TerminalType,
+  IN  EFI_DEVICE_PATH_PROTOCOL    *ParentDevicePath,
+  OUT EFI_DEVICE_PATH_PROTOCOL    **TerminalDevicePath
+  );
+
+/**
+  Get one key out of serial buffer.
+
+  @param  SerialIo           Serial I/O protocl attached to the serial device.
+  @param  Input              The fetched key.
+
+  @retval EFI_NOT_READY      If serial buffer is empty.
+  @retval EFI_DEVICE_ERROR   If reading serial buffer encounter error.
+  @retval EFI_SUCCESS        If reading serial buffer successfully, put
+                             the fetched key to the parameter output.
+
+**/
+EFI_STATUS
+GetOneKeyFromSerial (
+  EFI_SERIAL_IO_PROTOCOL  *SerialIo,
+  UINT8                   *Input
+  );
+
+/**
+  Insert one byte raw data into the Raw Data FIFO.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Input                The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Raw Data buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+RawFiFoInsertOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  UINT8         Input
+  );
+
+/**
+  Remove one pre-fetched key out of the Raw Data FIFO.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Raw Data FIFO buffer is empty before remove operation.
+
+**/
+BOOLEAN
+RawFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  UINT8         *Output
+  );
+
+/**
+  Clarify whether Raw Data FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Raw Data FIFO buffer is empty.
+  @retval FALSE                If Raw Data FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsRawFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Clarify whether Raw Data FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Raw Data FIFO buffer is full.
+  @retval FALSE                If Raw Data FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsRawFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Insert one pre-fetched key into the FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Key                  The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If FIFO buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+EfiKeyFiFoInsertOneKey (
+  TERMINAL_DEV      *TerminalDevice,
+  EFI_INPUT_KEY     *Key
+  );
+
+/**
+  Remove one pre-fetched key out of the FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If FIFO buffer is empty before remove operation.
+
+**/
+BOOLEAN
+EfiKeyFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  EFI_INPUT_KEY *Output
+  );
+
+/**
+  Clarify whether FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If FIFO buffer is empty.
+  @retval FALSE                If FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsEfiKeyFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Clarify whether FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If FIFO buffer is full.
+  @retval FALSE                If FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsEfiKeyFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Insert one pre-fetched key into the Unicode FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Input                The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Unicode FIFO buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+UnicodeFiFoInsertOneKey (
+  TERMINAL_DEV      *TerminalDevice,
+  UINT16            Input
+  );
+
+/**
+  Remove one pre-fetched key out of the Unicode FIFO buffer.
+  The caller should guarantee that Unicode FIFO buffer is not empty 
+  by IsUnicodeFiFoEmpty ().
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+**/
+VOID
+UnicodeFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  UINT16        *Output
+  );
+
+/**
+  Clarify whether Unicode FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Unicode FIFO buffer is empty.
+  @retval FALSE                If Unicode FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsUnicodeFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Clarify whether Unicode FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Unicode FIFO buffer is full.
+  @retval FALSE                If Unicode FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsUnicodeFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  );
+
+/**
+  Count Unicode FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @return The count in bytes of Unicode FIFO.
+
+**/
+UINT8
+UnicodeFiFoGetKeyCount (
+  TERMINAL_DEV    *TerminalDevice
+  );
+
+/**
+  Translate raw data into Unicode (according to different encode), and
+  translate Unicode into key information. (according to different standard).
+
+  @param  TerminalDevice       Terminal driver private structure.
+
+**/
+VOID
+TranslateRawDataToEfiKey (
+  IN  TERMINAL_DEV      *TerminalDevice
+  );
+
+//
+// internal functions for PC ANSI
+//
+
+/**
+  Translate all raw data in the Raw FIFI into unicode, and insert
+  them into Unicode FIFO.
+
+  @param TerminalDevice          The terminal device.
+
+**/
+VOID
+AnsiRawDataToUnicode (
+  IN  TERMINAL_DEV    *TerminalDevice
+  );
+
+/**
+  Converts a stream of Unicode characters from a terminal input device into EFI Keys that
+  can be read through the Simple Input Protocol.
+
+  The table below shows the keyboard input mappings that this function supports.
+  If the ESC sequence listed in one of the columns is presented, then it is translated
+  into the coorespoding EFI Scan Code.  If a matching sequence is not found, then the raw
+  key strokes are converted into EFI Keys.
+
+  2 seconds are allowed for an ESC sequence to be completed.  If the ESC sequence is not
+  completed in 2 seconds, then the raw key strokes of the partial ESC sequence are
+  converted into EFI Keys.
+  There is one special input sequence that will force the system to reset.
+  This is ESC R ESC r ESC R.
+
+  Symbols used in table below
+  ===========================
+    ESC = 0x1B
+    CSI = 0x9B
+    DEL = 0x7f
+    ^   = CTRL
+  +=========+======+===========+==========+==========+
+  |         | EFI  | UEFI 2.0  |          |          |
+  |         | Scan |           |  VT100+  |          |
+  |   KEY   | Code |  PC ANSI  |  VTUTF8  |   VT100  |
+  +=========+======+===========+==========+==========+
+  | NULL    | 0x00 |           |          |          |
+  | UP      | 0x01 | ESC [ A   | ESC [ A  | ESC [ A  |
+  | DOWN    | 0x02 | ESC [ B   | ESC [ B  | ESC [ B  |
+  | RIGHT   | 0x03 | ESC [ C   | ESC [ C  | ESC [ C  |
+  | LEFT    | 0x04 | ESC [ D   | ESC [ D  | ESC [ D  |
+  | HOME    | 0x05 | ESC [ H   | ESC h    | ESC [ H  |
+  | END     | 0x06 | ESC [ F   | ESC k    | ESC [ K  |
+  | INSERT  | 0x07 | ESC [ @   | ESC +    | ESC [ @  |
+  |         |      | ESC [ L   |          | ESC [ L  |
+  | DELETE  | 0x08 | ESC [ X   | ESC -    | ESC [ P  |
+  | PG UP   | 0x09 | ESC [ I   | ESC ?    | ESC [ V  |
+  |         |      |           |          | ESC [ ?  |
+  | PG DOWN | 0x0A | ESC [ G   | ESC /    | ESC [ U  |
+  |         |      |           |          | ESC [ /  |
+  | F1      | 0x0B | ESC [ M   | ESC 1    | ESC O P  |
+  | F2      | 0x0C | ESC [ N   | ESC 2    | ESC O Q  |
+  | F3      | 0x0D | ESC [ O   | ESC 3    | ESC O w  |
+  | F4      | 0x0E | ESC [ P   | ESC 4    | ESC O x  |
+  | F5      | 0x0F | ESC [ Q   | ESC 5    | ESC O t  |
+  | F6      | 0x10 | ESC [ R   | ESC 6    | ESC O u  |
+  | F7      | 0x11 | ESC [ S   | ESC 7    | ESC O q  |
+  | F8      | 0x12 | ESC [ T   | ESC 8    | ESC O r  |
+  | F9      | 0x13 | ESC [ U   | ESC 9    | ESC O p  |
+  | F10     | 0x14 | ESC [ V   | ESC 0    | ESC O M  |
+  | Escape  | 0x17 | ESC       | ESC      | ESC      |
+  | F11     | 0x15 |           | ESC !    |          |
+  | F12     | 0x16 |           | ESC @    |          |
+  +=========+======+===========+==========+==========+
+
+  Special Mappings
+  ================
+  ESC R ESC r ESC R = Reset System
+
+
+  @param TerminalDevice   The terminal device to use to translate raw input into EFI Keys
+
+**/
+VOID
+UnicodeToEfiKey (
+  IN  TERMINAL_DEV    *TerminalDevice
+  );
+
+/**
+  Check if input string is valid Ascii string, valid EFI control characters
+  or valid text graphics.
+
+  @param  TerminalDevice          The terminal device.
+  @param  WString                 The input string.
+
+  @retval EFI_UNSUPPORTED         If not all input characters are valid.
+  @retval EFI_SUCCESS             If all input characters are valid.
+
+**/
+EFI_STATUS
+AnsiTestString (
+  IN  TERMINAL_DEV    *TerminalDevice,
+  IN  CHAR16          *WString
+  );
+
+//
+// internal functions for VTUTF8
+//
+
+/**
+  Translate all VT-UTF8 characters in the Raw FIFI into unicode characters,
+  and insert them into Unicode FIFO.
+
+  @param VtUtf8Device          The terminal device.
+
+**/
+VOID
+VTUTF8RawDataToUnicode (
+  IN  TERMINAL_DEV    *VtUtf8Device
+  );
+
+/**
+  Check if input string is valid VT-UTF8 string.
+
+  @param  TerminalDevice          The terminal device.
+  @param  WString                 The input string.
+
+  @retval EFI_SUCCESS             If all input characters are valid.
+
+**/
+EFI_STATUS
+VTUTF8TestString (
+  IN  TERMINAL_DEV    *TerminalDevice,
+  IN  CHAR16          *WString
+  );
+
+/**
+  Translate one Unicode character into VT-UTF8 characters.
+
+  UTF8 Encoding Table
+  Bits per Character | Unicode Character Range | Unicode Binary  Encoding |	UTF8 Binary Encoding
+        0-7	         |     0x0000 - 0x007F	    |     00000000 0xxxxxxx	   |   0xxxxxxx
+        8-11 	       |     0x0080 - 0x07FF	    |     00000xxx xxxxxxxx 	  |   110xxxxx 10xxxxxx
+       12-16	        |     0x0800 - 0xFFFF	    |     xxxxxxxx xxxxxxxx	   |   1110xxxx 10xxxxxx 10xxxxxx
+
+
+  @param  Unicode          Unicode character need translating.
+  @param  Utf8Char         Return VT-UTF8 character set.
+  @param  ValidBytes       The count of valid VT-UTF8 characters. If
+                           ValidBytes is zero, no valid VT-UTF8 returned.
+
+**/
+VOID
+UnicodeToUtf8 (
+  IN  CHAR16      Unicode,
+  OUT UTF8_CHAR   *Utf8Char,
+  OUT UINT8       *ValidBytes
+  );
+
+/**
+  Get one valid VT-UTF8 characters set from Raw Data FIFO.
+
+  @param  Utf8Device          The terminal device.
+  @param  Utf8Char            Returned valid VT-UTF8 characters set.
+  @param  ValidBytes          The count of returned VT-VTF8 characters.
+                              If ValidBytes is zero, no valid VT-UTF8 returned.
+
+**/
+VOID
+GetOneValidUtf8Char (
+  IN  TERMINAL_DEV      *Utf8Device,
+  OUT UTF8_CHAR         *Utf8Char,
+  OUT UINT8             *ValidBytes
+  );
+
+/**
+  Translate VT-UTF8 characters into one Unicode character.
+
+  UTF8 Encoding Table
+  Bits per Character | Unicode Character Range | Unicode Binary  Encoding |	UTF8 Binary Encoding
+        0-7	         |     0x0000 - 0x007F	    |     00000000 0xxxxxxx	   |   0xxxxxxx
+        8-11 	       |     0x0080 - 0x07FF	    |     00000xxx xxxxxxxx 	  |   110xxxxx 10xxxxxx
+       12-16	        |     0x0800 - 0xFFFF	    |     xxxxxxxx xxxxxxxx	   |   1110xxxx 10xxxxxx 10xxxxxx
+
+
+  @param  Utf8Char         VT-UTF8 character set needs translating.
+  @param  ValidBytes       The count of valid VT-UTF8 characters.
+  @param  UnicodeChar      Returned unicode character.
+
+**/
+VOID
+Utf8ToUnicode (
+  IN  UTF8_CHAR       Utf8Char,
+  IN  UINT8           ValidBytes,
+  OUT CHAR16          *UnicodeChar
+  );
+
+//
+// functions for boxdraw unicode
+//
+
+/**
+  Detects if a Unicode char is for Box Drawing text graphics.
+
+  @param  Graphic      Unicode char to test.
+  @param  PcAnsi       Optional pointer to return PCANSI equivalent of
+                       Graphic.
+  @param  Ascii        Optional pointer to return ASCII equivalent of
+                       Graphic.
+
+  @retval TRUE         If Graphic is a supported Unicode Box Drawing character.
+
+**/
+BOOLEAN
+TerminalIsValidTextGraphics (
+  IN  CHAR16  Graphic,
+  OUT CHAR8   *PcAnsi, OPTIONAL
+  OUT CHAR8   *Ascii OPTIONAL
+  );
+
+/**
+  Detects if a valid ASCII char.
+
+  @param  Ascii        An ASCII character.
+
+  @retval TRUE         If it is a valid ASCII character.
+  @retval FALSE        If it is not a valid ASCII character.
+
+**/
+BOOLEAN
+TerminalIsValidAscii (
+  IN  CHAR16  Ascii
+  );
+
+/**
+  Detects if a valid EFI control character.
+
+  @param  CharC        An input EFI Control character.
+
+  @retval TRUE         If it is a valid EFI control character.
+  @retval FALSE        If it is not a valid EFI control character.
+
+**/
+BOOLEAN
+TerminalIsValidEfiCntlChar (
+  IN  CHAR16  CharC
+  );
+
+/**
+  Check if the device supports hot-plug through its device path.
+
+  This function could be updated to check more types of Hot Plug devices.
+  Currently, it checks USB and PCCard device.
+
+  @param  DevicePath            Pointer to device's device path.
+
+  @retval TRUE                  The devcie is a hot-plug device
+  @retval FALSE                 The devcie is not a hot-plug device.
+
+**/
+BOOLEAN
+IsHotPlugDevice (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  );
+
+/**
+  Timer handler to poll the key from serial.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+TerminalConInTimerHandler (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConIn.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConIn.c
new file mode 100644
index 0000000000..3be877b466
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConIn.c
@@ -0,0 +1,1671 @@
+/** @file
+  Implementation for EFI_SIMPLE_TEXT_INPUT_PROTOCOL protocol.
+
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2015, 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 "Terminal.h"
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existence of a keystroke via WaitForEvent () call.
+
+  @param  TerminalDevice           Terminal driver private structure
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data available.
+  @retval EFI_INVALID_PARAMETER    KeyData is NULL.
+
+**/
+EFI_STATUS
+ReadKeyStrokeWorker (
+  IN  TERMINAL_DEV *TerminalDevice,
+  OUT EFI_KEY_DATA *KeyData
+  )
+{
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!EfiKeyFiFoRemoveOneKey (TerminalDevice, &KeyData->Key)) {
+    return EFI_NOT_READY;
+  }
+
+  KeyData->KeyState.KeyShiftState  = 0;
+  KeyData->KeyState.KeyToggleState = 0;
+
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Implements EFI_SIMPLE_TEXT_INPUT_PROTOCOL.Reset().
+  This driver only perform dependent serial device reset regardless of
+  the value of ExtendeVerification
+
+  @param  This                     Indicates the calling context.
+  @param  ExtendedVerification     Skip by this driver.
+
+  @retval EFI_SUCCESS              The reset operation succeeds.
+  @retval EFI_DEVICE_ERROR         The dependent serial port reset fails.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReset (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  IN  BOOLEAN                         ExtendedVerification
+  )
+{
+  EFI_STATUS    Status;
+  TERMINAL_DEV  *TerminalDevice;
+
+  TerminalDevice = TERMINAL_CON_IN_DEV_FROM_THIS (This);
+
+  //
+  // Report progress code here
+  //
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_PROGRESS_CODE,
+    (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_PC_RESET),
+    TerminalDevice->DevicePath
+    );
+
+  Status = TerminalDevice->SerialIo->Reset (TerminalDevice->SerialIo);
+
+  //
+  // Make all the internal buffer empty for keys
+  //
+  TerminalDevice->RawFiFo->Head     = TerminalDevice->RawFiFo->Tail;
+  TerminalDevice->UnicodeFiFo->Head = TerminalDevice->UnicodeFiFo->Tail;
+  TerminalDevice->EfiKeyFiFo->Head  = TerminalDevice->EfiKeyFiFo->Tail;
+
+  if (EFI_ERROR (Status)) {
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_ERROR_CODE | EFI_ERROR_MINOR,
+      (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_EC_CONTROLLER_ERROR),
+      TerminalDevice->DevicePath
+      );
+  }
+
+  return Status;
+}
+
+/**
+  Implements EFI_SIMPLE_TEXT_INPUT_PROTOCOL.ReadKeyStroke().
+
+  @param  This                Indicates the calling context.
+  @param  Key                 A pointer to a buffer that is filled in with the
+                              keystroke information for the key that was sent
+                              from terminal.
+
+  @retval EFI_SUCCESS         The keystroke information is returned successfully.
+  @retval EFI_NOT_READY       There is no keystroke data available.
+  @retval EFI_DEVICE_ERROR    The dependent serial device encounters error.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReadKeyStroke (
+  IN  EFI_SIMPLE_TEXT_INPUT_PROTOCOL  *This,
+  OUT EFI_INPUT_KEY                   *Key
+  )
+{
+  TERMINAL_DEV  *TerminalDevice;
+  EFI_STATUS    Status;
+  EFI_KEY_DATA  KeyData;
+
+  //
+  //  get TERMINAL_DEV from "This" parameter.
+  //
+  TerminalDevice  = TERMINAL_CON_IN_DEV_FROM_THIS (This);
+
+  Status = ReadKeyStrokeWorker (TerminalDevice, &KeyData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (Key, &KeyData.Key, sizeof (EFI_INPUT_KEY));
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Check if the key already has been registered.
+
+  If both RegsiteredData and InputData is NULL, then ASSERT().
+
+  @param  RegsiteredData           A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   registered.
+  @param  InputData                A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval TRUE                     Key be pressed matches a registered key.
+  @retval FALSE                    Match failed.
+
+**/
+BOOLEAN
+IsKeyRegistered (
+  IN EFI_KEY_DATA  *RegsiteredData,
+  IN EFI_KEY_DATA  *InputData
+  )
+{
+  ASSERT (RegsiteredData != NULL && InputData != NULL);
+
+  if ((RegsiteredData->Key.ScanCode    != InputData->Key.ScanCode) ||
+      (RegsiteredData->Key.UnicodeChar != InputData->Key.UnicodeChar)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+
+/**
+  Event notification function for EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL.WaitForKeyEx event
+  Signal the event if there is key available
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+
+**/
+VOID
+EFIAPI
+TerminalConInWaitForKeyEx (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  )
+{
+  TerminalConInWaitForKey (Event, Context);
+}
+
+//
+// Simple Text Input Ex protocol functions
+//
+
+/**
+  Reset the input device and optionally run diagnostics
+
+  @param  This                     Protocol instance pointer.
+  @param  ExtendedVerification     Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS              The device was reset.
+  @retval EFI_DEVICE_ERROR         The device is not functioning properly and could
+                                   not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInResetEx (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN BOOLEAN                            ExtendedVerification
+  )
+{
+  EFI_STATUS              Status;
+  TERMINAL_DEV            *TerminalDevice;
+
+  TerminalDevice = TERMINAL_CON_IN_EX_DEV_FROM_THIS (This);
+
+  Status = TerminalDevice->SimpleInput.Reset (&TerminalDevice->SimpleInput, ExtendedVerification);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Reads the next keystroke from the input device. The WaitForKey Event can
+  be used to test for existence of a keystroke via WaitForEvent () call.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke state data for the key that was
+                                   pressed.
+
+  @retval EFI_SUCCESS              The keystroke information was returned.
+  @retval EFI_NOT_READY            There was no keystroke data available.
+  @retval EFI_DEVICE_ERROR         The keystroke information was not returned due
+                                   to hardware errors.
+  @retval EFI_INVALID_PARAMETER    KeyData is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInReadKeyStrokeEx (
+  IN  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *This,
+  OUT EFI_KEY_DATA                      *KeyData
+  )
+{
+  TERMINAL_DEV                    *TerminalDevice;
+
+  if (KeyData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TerminalDevice = TERMINAL_CON_IN_EX_DEV_FROM_THIS (This);
+
+  return ReadKeyStrokeWorker (TerminalDevice, KeyData);
+
+}
+
+
+/**
+  Set certain state for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyToggleState           A pointer to the EFI_KEY_TOGGLE_STATE to set the
+                                   state for the input device.
+
+  @retval EFI_SUCCESS              The device state was set successfully.
+  @retval EFI_DEVICE_ERROR         The device is not functioning correctly and
+                                   could not have the setting adjusted.
+  @retval EFI_UNSUPPORTED          The device does not have the ability to set its
+                                   state.
+  @retval EFI_INVALID_PARAMETER    KeyToggleState is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInSetState (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_TOGGLE_STATE               *KeyToggleState
+  )
+{
+  if (KeyToggleState == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*KeyToggleState & EFI_TOGGLE_STATE_VALID) != EFI_TOGGLE_STATE_VALID) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Register a notification function for a particular keystroke for the input device.
+
+  @param  This                     Protocol instance pointer.
+  @param  KeyData                  A pointer to a buffer that is filled in with the
+                                   keystroke information data for the key that was
+                                   pressed.
+  @param  KeyNotificationFunction  Points to the function to be called when the key
+                                   sequence is typed specified by KeyData.
+  @param  NotifyHandle             Points to the unique handle assigned to the
+                                   registered notification.
+
+  @retval EFI_SUCCESS              The notification function was registered
+                                   successfully.
+  @retval EFI_OUT_OF_RESOURCES     Unable to allocate resources for necessary data
+                                   structures.
+  @retval EFI_INVALID_PARAMETER    KeyData or NotifyHandle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInRegisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN EFI_KEY_DATA                       *KeyData,
+  IN EFI_KEY_NOTIFY_FUNCTION            KeyNotificationFunction,
+  OUT VOID                              **NotifyHandle
+  )
+{
+  TERMINAL_DEV                    *TerminalDevice;
+  TERMINAL_CONSOLE_IN_EX_NOTIFY   *NewNotify;
+  LIST_ENTRY                      *Link;
+  LIST_ENTRY                      *NotifyList;
+  TERMINAL_CONSOLE_IN_EX_NOTIFY   *CurrentNotify;
+
+  if (KeyData == NULL || NotifyHandle == NULL || KeyNotificationFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TerminalDevice = TERMINAL_CON_IN_EX_DEV_FROM_THIS (This);
+
+  //
+  // Return EFI_SUCCESS if the (KeyData, NotificationFunction) is already registered.
+  //
+  NotifyList = &TerminalDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList); !IsNull (NotifyList,Link); Link = GetNextNode (NotifyList,Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (IsKeyRegistered (&CurrentNotify->KeyData, KeyData)) {
+      if (CurrentNotify->KeyNotificationFn == KeyNotificationFunction) {
+        *NotifyHandle = CurrentNotify;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  //
+  // Allocate resource to save the notification function
+  //
+  NewNotify = (TERMINAL_CONSOLE_IN_EX_NOTIFY *) AllocateZeroPool (sizeof (TERMINAL_CONSOLE_IN_EX_NOTIFY));
+  if (NewNotify == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewNotify->Signature         = TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE;
+  NewNotify->KeyNotificationFn = KeyNotificationFunction;
+  CopyMem (&NewNotify->KeyData, KeyData, sizeof (EFI_KEY_DATA));
+  InsertTailList (&TerminalDevice->NotifyList, &NewNotify->NotifyEntry);
+
+  *NotifyHandle                = NewNotify;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove a registered notification function from a particular keystroke.
+
+  @param  This                     Protocol instance pointer.
+  @param  NotificationHandle       The handle of the notification function being
+                                   unregistered.
+
+  @retval EFI_SUCCESS              The notification function was unregistered
+                                   successfully.
+  @retval EFI_INVALID_PARAMETER    The NotificationHandle is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConInUnregisterKeyNotify (
+  IN EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL  *This,
+  IN VOID                               *NotificationHandle
+  )
+{
+  TERMINAL_DEV                    *TerminalDevice;
+  LIST_ENTRY                      *Link;
+  TERMINAL_CONSOLE_IN_EX_NOTIFY   *CurrentNotify;
+  LIST_ENTRY                      *NotifyList;
+
+  if (NotificationHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TerminalDevice = TERMINAL_CON_IN_EX_DEV_FROM_THIS (This);
+
+  NotifyList = &TerminalDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList); !IsNull (NotifyList,Link); Link = GetNextNode (NotifyList,Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (CurrentNotify == NotificationHandle) {
+      //
+      // Remove the notification function from NotifyList and free resources
+      //
+      RemoveEntryList (&CurrentNotify->NotifyEntry);
+
+      gBS->FreePool (CurrentNotify);
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Can not find the matching entry in database.
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Translate raw data into Unicode (according to different encode), and
+  translate Unicode into key information. (according to different standard).
+
+  @param  TerminalDevice       Terminal driver private structure.
+
+**/
+VOID
+TranslateRawDataToEfiKey (
+  IN  TERMINAL_DEV      *TerminalDevice
+  )
+{
+  switch (TerminalDevice->TerminalType) {
+
+  case PCANSITYPE:
+  case VT100TYPE:
+  case VT100PLUSTYPE:
+  case TTYTERMTYPE:
+    AnsiRawDataToUnicode (TerminalDevice);
+    UnicodeToEfiKey (TerminalDevice);
+    break;
+
+  case VTUTF8TYPE:
+    //
+    // Process all the raw data in the RawFIFO,
+    // put the processed key into UnicodeFIFO.
+    //
+    VTUTF8RawDataToUnicode (TerminalDevice);
+
+    //
+    // Translate all the Unicode data in the UnicodeFIFO to Efi key,
+    // then put into EfiKeyFIFO.
+    //
+    UnicodeToEfiKey (TerminalDevice);
+
+    break;
+  }
+}
+
+/**
+  Event notification function for EFI_SIMPLE_TEXT_INPUT_PROTOCOL.WaitForKey event
+  Signal the event if there is key available
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+
+**/
+VOID
+EFIAPI
+TerminalConInWaitForKey (
+  IN  EFI_EVENT       Event,
+  IN  VOID            *Context
+  )
+{
+  //
+  // Someone is waiting on the keystroke event, if there's
+  // a key pending, signal the event
+  //
+  if (!IsEfiKeyFiFoEmpty ((TERMINAL_DEV *) Context)) {
+
+    gBS->SignalEvent (Event);
+  }
+}
+
+/**
+  Timer handler to poll the key from serial.
+
+  @param  Event                    Indicates the event that invoke this function.
+  @param  Context                  Indicates the calling context.
+**/
+VOID
+EFIAPI
+TerminalConInTimerHandler (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  EFI_STATUS              Status;
+  TERMINAL_DEV            *TerminalDevice;
+  UINT32                  Control;
+  UINT8                   Input;
+  EFI_SERIAL_IO_MODE      *Mode;
+  EFI_SERIAL_IO_PROTOCOL  *SerialIo;
+  UINTN                   SerialInTimeOut;
+
+  TerminalDevice  = (TERMINAL_DEV *) Context;
+
+  SerialIo        = TerminalDevice->SerialIo;
+  if (SerialIo == NULL) {
+    return ;
+  }
+  //
+  //  if current timeout value for serial device is not identical with
+  //  the value saved in TERMINAL_DEV structure, then recalculate the
+  //  timeout value again and set serial attribute according to this value.
+  //
+  Mode = SerialIo->Mode;
+  if (Mode->Timeout != TerminalDevice->SerialInTimeOut) {
+
+    SerialInTimeOut = 0;
+    if (Mode->BaudRate != 0) {
+      //
+      // According to BAUD rate to calculate the timeout value.
+      //
+      SerialInTimeOut = (1 + Mode->DataBits + Mode->StopBits) * 2 * 1000000 / (UINTN) Mode->BaudRate;
+    }
+
+    Status = SerialIo->SetAttributes (
+                        SerialIo,
+                        Mode->BaudRate,
+                        Mode->ReceiveFifoDepth,
+                        (UINT32) SerialInTimeOut,
+                        (EFI_PARITY_TYPE) (Mode->Parity),
+                        (UINT8) Mode->DataBits,
+                        (EFI_STOP_BITS_TYPE) (Mode->StopBits)
+                        );
+
+    if (EFI_ERROR (Status)) {
+      TerminalDevice->SerialInTimeOut = 0;
+    } else {
+      TerminalDevice->SerialInTimeOut = SerialInTimeOut;
+    }
+  }
+  //
+  // Check whether serial buffer is empty.
+  // Skip the key transfer loop only if the SerialIo protocol instance
+  // successfully reports EFI_SERIAL_INPUT_BUFFER_EMPTY.
+  //
+  Status = SerialIo->GetControl (SerialIo, &Control);
+  if (EFI_ERROR (Status) || ((Control & EFI_SERIAL_INPUT_BUFFER_EMPTY) == 0)) {
+    //
+    // Fetch all the keys in the serial buffer,
+    // and insert the byte stream into RawFIFO.
+    //
+    while (!IsRawFiFoFull (TerminalDevice)) {
+
+      Status = GetOneKeyFromSerial (TerminalDevice->SerialIo, &Input);
+
+      if (EFI_ERROR (Status)) {
+        if (Status == EFI_DEVICE_ERROR) {
+          REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+            EFI_ERROR_CODE | EFI_ERROR_MINOR,
+            (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_EC_INPUT_ERROR),
+            TerminalDevice->DevicePath
+            );
+        }
+        break;
+      }
+
+      RawFiFoInsertOneKey (TerminalDevice, Input);
+    }
+  }
+
+  //
+  // Translate all the raw data in RawFIFO into EFI Key,
+  // according to different terminal type supported.
+  //
+  TranslateRawDataToEfiKey (TerminalDevice);
+}
+
+/**
+  Get one key out of serial buffer.
+
+  @param  SerialIo           Serial I/O protocol attached to the serial device.
+  @param  Output             The fetched key.
+
+  @retval EFI_NOT_READY      If serial buffer is empty.
+  @retval EFI_DEVICE_ERROR   If reading serial buffer encounter error.
+  @retval EFI_SUCCESS        If reading serial buffer successfully, put
+                             the fetched key to the parameter output.
+
+**/
+EFI_STATUS
+GetOneKeyFromSerial (
+  EFI_SERIAL_IO_PROTOCOL  *SerialIo,
+  UINT8                   *Output
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Size;
+
+  Size    = 1;
+  *Output = 0;
+
+  //
+  // Read one key from serial I/O device.
+  //
+  Status  = SerialIo->Read (SerialIo, &Size, Output);
+
+  if (EFI_ERROR (Status)) {
+
+    if (Status == EFI_TIMEOUT) {
+      return EFI_NOT_READY;
+    }
+
+    return EFI_DEVICE_ERROR;
+
+  }
+
+  if (*Output == 0) {
+    return EFI_NOT_READY;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Insert one byte raw data into the Raw Data FIFO.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Input                The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Raw Data buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+RawFiFoInsertOneKey (
+  TERMINAL_DEV      *TerminalDevice,
+  UINT8             Input
+  )
+{
+  UINT8 Tail;
+
+  Tail = TerminalDevice->RawFiFo->Tail;
+
+  if (IsRawFiFoFull (TerminalDevice)) {
+    //
+    // Raw FIFO is full
+    //
+    return FALSE;
+  }
+
+  TerminalDevice->RawFiFo->Data[Tail]  = Input;
+
+  TerminalDevice->RawFiFo->Tail        = (UINT8) ((Tail + 1) % (RAW_FIFO_MAX_NUMBER + 1));
+
+  return TRUE;
+}
+
+/**
+  Remove one pre-fetched key out of the Raw Data FIFO.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Raw Data FIFO buffer is empty before remove operation.
+
+**/
+BOOLEAN
+RawFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  UINT8         *Output
+  )
+{
+  UINT8 Head;
+
+  Head = TerminalDevice->RawFiFo->Head;
+
+  if (IsRawFiFoEmpty (TerminalDevice)) {
+    //
+    //  FIFO is empty
+    //
+    *Output = 0;
+    return FALSE;
+  }
+
+  *Output                       = TerminalDevice->RawFiFo->Data[Head];
+
+  TerminalDevice->RawFiFo->Head  = (UINT8) ((Head + 1) % (RAW_FIFO_MAX_NUMBER + 1));
+
+  return TRUE;
+}
+
+/**
+  Clarify whether Raw Data FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Raw Data FIFO buffer is empty.
+  @retval FALSE                If Raw Data FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsRawFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  if (TerminalDevice->RawFiFo->Head == TerminalDevice->RawFiFo->Tail) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Clarify whether Raw Data FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Raw Data FIFO buffer is full.
+  @retval FALSE                If Raw Data FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsRawFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  UINT8 Tail;
+  UINT8 Head;
+
+  Tail  = TerminalDevice->RawFiFo->Tail;
+  Head  = TerminalDevice->RawFiFo->Head;
+
+  if (((Tail + 1) % (RAW_FIFO_MAX_NUMBER + 1)) == Head) {
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Insert one pre-fetched key into the FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Key                  The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If FIFO buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+EfiKeyFiFoInsertOneKey (
+  TERMINAL_DEV                    *TerminalDevice,
+  EFI_INPUT_KEY                   *Key
+  )
+{
+  UINT8                           Tail;
+  LIST_ENTRY                      *Link;
+  LIST_ENTRY                      *NotifyList;
+  TERMINAL_CONSOLE_IN_EX_NOTIFY   *CurrentNotify;
+  EFI_KEY_DATA                    KeyData;
+
+  Tail = TerminalDevice->EfiKeyFiFo->Tail;
+
+  CopyMem (&KeyData.Key, Key, sizeof (EFI_INPUT_KEY));
+  KeyData.KeyState.KeyShiftState  = 0;
+  KeyData.KeyState.KeyToggleState = 0;
+
+  //
+  // Invoke notification functions if exist
+  //
+  NotifyList = &TerminalDevice->NotifyList;
+  for (Link = GetFirstNode (NotifyList); !IsNull (NotifyList,Link); Link = GetNextNode (NotifyList,Link)) {
+    CurrentNotify = CR (
+                      Link,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY,
+                      NotifyEntry,
+                      TERMINAL_CONSOLE_IN_EX_NOTIFY_SIGNATURE
+                      );
+    if (IsKeyRegistered (&CurrentNotify->KeyData, &KeyData)) {
+      CurrentNotify->KeyNotificationFn (&KeyData);
+    }
+  }
+  if (IsEfiKeyFiFoFull (TerminalDevice)) {
+    //
+    // Efi Key FIFO is full
+    //
+    return FALSE;
+  }
+
+  CopyMem (&TerminalDevice->EfiKeyFiFo->Data[Tail], Key, sizeof (EFI_INPUT_KEY));
+
+  TerminalDevice->EfiKeyFiFo->Tail = (UINT8) ((Tail + 1) % (FIFO_MAX_NUMBER + 1));
+
+  return TRUE;
+}
+
+/**
+  Remove one pre-fetched key out of the FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If FIFO buffer is empty before remove operation.
+
+**/
+BOOLEAN
+EfiKeyFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  EFI_INPUT_KEY *Output
+  )
+{
+  UINT8 Head;
+
+  Head = TerminalDevice->EfiKeyFiFo->Head;
+  ASSERT (Head < FIFO_MAX_NUMBER + 1);
+
+  if (IsEfiKeyFiFoEmpty (TerminalDevice)) {
+    //
+    //  FIFO is empty
+    //
+    Output->ScanCode    = SCAN_NULL;
+    Output->UnicodeChar = 0;
+    return FALSE;
+  }
+
+  CopyMem (Output, &TerminalDevice->EfiKeyFiFo->Data[Head], sizeof (EFI_INPUT_KEY));
+
+  TerminalDevice->EfiKeyFiFo->Head = (UINT8) ((Head + 1) % (FIFO_MAX_NUMBER + 1));
+
+  return TRUE;
+}
+
+/**
+  Clarify whether FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If FIFO buffer is empty.
+  @retval FALSE                If FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsEfiKeyFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  if (TerminalDevice->EfiKeyFiFo->Head == TerminalDevice->EfiKeyFiFo->Tail) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Clarify whether FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If FIFO buffer is full.
+  @retval FALSE                If FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsEfiKeyFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  UINT8 Tail;
+  UINT8 Head;
+
+  Tail  = TerminalDevice->EfiKeyFiFo->Tail;
+  Head  = TerminalDevice->EfiKeyFiFo->Head;
+
+  if (((Tail + 1) % (FIFO_MAX_NUMBER + 1)) == Head) {
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Insert one pre-fetched key into the Unicode FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Input                The key will be input.
+
+  @retval TRUE                 If insert successfully.
+  @retval FALSE                If Unicode FIFO buffer is full before key insertion,
+                               and the key is lost.
+
+**/
+BOOLEAN
+UnicodeFiFoInsertOneKey (
+  TERMINAL_DEV      *TerminalDevice,
+  UINT16            Input
+  )
+{
+  UINT8 Tail;
+
+  Tail = TerminalDevice->UnicodeFiFo->Tail;
+  ASSERT (Tail < FIFO_MAX_NUMBER + 1);
+
+
+  if (IsUnicodeFiFoFull (TerminalDevice)) {
+    //
+    // Unicode FIFO is full
+    //
+    return FALSE;
+  }
+
+  TerminalDevice->UnicodeFiFo->Data[Tail]  = Input;
+
+  TerminalDevice->UnicodeFiFo->Tail        = (UINT8) ((Tail + 1) % (FIFO_MAX_NUMBER + 1));
+
+  return TRUE;
+}
+
+/**
+  Remove one pre-fetched key out of the Unicode FIFO buffer.
+  The caller should guarantee that Unicode FIFO buffer is not empty 
+  by IsUnicodeFiFoEmpty ().
+
+  @param  TerminalDevice       Terminal driver private structure.
+  @param  Output               The key will be removed.
+
+**/
+VOID
+UnicodeFiFoRemoveOneKey (
+  TERMINAL_DEV  *TerminalDevice,
+  UINT16        *Output
+  )
+{
+  UINT8 Head;
+
+  Head = TerminalDevice->UnicodeFiFo->Head;
+  ASSERT (Head < FIFO_MAX_NUMBER + 1);
+
+  *Output = TerminalDevice->UnicodeFiFo->Data[Head];
+
+  TerminalDevice->UnicodeFiFo->Head = (UINT8) ((Head + 1) % (FIFO_MAX_NUMBER + 1));
+}
+
+/**
+  Clarify whether Unicode FIFO buffer is empty.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Unicode FIFO buffer is empty.
+  @retval FALSE                If Unicode FIFO buffer is not empty.
+
+**/
+BOOLEAN
+IsUnicodeFiFoEmpty (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  if (TerminalDevice->UnicodeFiFo->Head == TerminalDevice->UnicodeFiFo->Tail) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Clarify whether Unicode FIFO buffer is full.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @retval TRUE                 If Unicode FIFO buffer is full.
+  @retval FALSE                If Unicode FIFO buffer is not full.
+
+**/
+BOOLEAN
+IsUnicodeFiFoFull (
+  TERMINAL_DEV  *TerminalDevice
+  )
+{
+  UINT8 Tail;
+  UINT8 Head;
+
+  Tail  = TerminalDevice->UnicodeFiFo->Tail;
+  Head  = TerminalDevice->UnicodeFiFo->Head;
+
+  if (((Tail + 1) % (FIFO_MAX_NUMBER + 1)) == Head) {
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Count Unicode FIFO buffer.
+
+  @param  TerminalDevice       Terminal driver private structure
+
+  @return The count in bytes of Unicode FIFO.
+
+**/
+UINT8
+UnicodeFiFoGetKeyCount (
+  TERMINAL_DEV    *TerminalDevice
+  )
+{
+  UINT8 Tail;
+  UINT8 Head;
+
+  Tail  = TerminalDevice->UnicodeFiFo->Tail;
+  Head  = TerminalDevice->UnicodeFiFo->Head;
+
+  if (Tail >= Head) {
+    return (UINT8) (Tail - Head);
+  } else {
+    return (UINT8) (Tail + FIFO_MAX_NUMBER + 1 - Head);
+  }
+}
+
+/**
+  Update the Unicode characters from a terminal input device into EFI Keys FIFO.
+
+  @param TerminalDevice   The terminal device to use to translate raw input into EFI Keys
+
+**/
+VOID
+UnicodeToEfiKeyFlushState (
+  IN  TERMINAL_DEV    *TerminalDevice
+  )
+{
+  EFI_INPUT_KEY Key;
+  UINT32        InputState;
+
+  InputState = TerminalDevice->InputState;
+
+  if (IsEfiKeyFiFoFull (TerminalDevice)) {
+    return;
+  }
+
+  if ((InputState & INPUT_STATE_ESC) != 0) {
+    Key.ScanCode    = SCAN_ESC;
+    Key.UnicodeChar = 0;
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+
+  if ((InputState & INPUT_STATE_CSI) != 0) {
+    Key.ScanCode    = SCAN_NULL;
+    Key.UnicodeChar = CSI;
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+
+  if ((InputState & INPUT_STATE_LEFTOPENBRACKET) != 0) {
+    Key.ScanCode    = SCAN_NULL;
+    Key.UnicodeChar = LEFTOPENBRACKET;
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+
+  if ((InputState & INPUT_STATE_O) != 0) {
+    Key.ScanCode    = SCAN_NULL;
+    Key.UnicodeChar = 'O';
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+
+  if ((InputState & INPUT_STATE_2) != 0) {
+    Key.ScanCode    = SCAN_NULL;
+    Key.UnicodeChar = '2';
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+
+  //
+  // Cancel the timer.
+  //
+  gBS->SetTimer (
+        TerminalDevice->TwoSecondTimeOut,
+        TimerCancel,
+        0
+        );
+
+  TerminalDevice->InputState = INPUT_STATE_DEFAULT;
+}
+
+
+/**
+  Converts a stream of Unicode characters from a terminal input device into EFI Keys that
+  can be read through the Simple Input Protocol.
+
+  The table below shows the keyboard input mappings that this function supports.
+  If the ESC sequence listed in one of the columns is presented, then it is translated
+  into the corresponding EFI Scan Code.  If a matching sequence is not found, then the raw
+  key strokes are converted into EFI Keys.
+
+  2 seconds are allowed for an ESC sequence to be completed.  If the ESC sequence is not
+  completed in 2 seconds, then the raw key strokes of the partial ESC sequence are
+  converted into EFI Keys.
+  There is one special input sequence that will force the system to reset.
+  This is ESC R ESC r ESC R.
+
+  Note: current implementation support terminal types include: PC ANSI, VT100+/VTUTF8, VT100. 
+        The table below is not same with UEFI Spec 2.3 Appendix B Table 201(not support ANSI X3.64 /
+        DEC VT200-500 and extra support PC ANSI, VT100)since UEFI Table 201 is just an example.
+        
+  Symbols used in table below
+  ===========================
+    ESC = 0x1B
+    CSI = 0x9B
+    DEL = 0x7f
+    ^   = CTRL
+
+  +=========+======+===========+==========+==========+
+  |         | EFI  | UEFI 2.0  |          |          |
+  |         | Scan |           |  VT100+  |          |
+  |   KEY   | Code |  PC ANSI  |  VTUTF8  |   VT100  |
+  +=========+======+===========+==========+==========+
+  | NULL    | 0x00 |           |          |          |
+  | UP      | 0x01 | ESC [ A   | ESC [ A  | ESC [ A  |
+  | DOWN    | 0x02 | ESC [ B   | ESC [ B  | ESC [ B  |
+  | RIGHT   | 0x03 | ESC [ C   | ESC [ C  | ESC [ C  |
+  | LEFT    | 0x04 | ESC [ D   | ESC [ D  | ESC [ D  |
+  | HOME    | 0x05 | ESC [ H   | ESC h    | ESC [ H  |
+  | END     | 0x06 | ESC [ F   | ESC k    | ESC [ K  |
+  | INSERT  | 0x07 | ESC [ @   | ESC +    | ESC [ @  |
+  |         |      | ESC [ L   |          | ESC [ L  |
+  | DELETE  | 0x08 | ESC [ X   | ESC -    | ESC [ P  |
+  | PG UP   | 0x09 | ESC [ I   | ESC ?    | ESC [ V  |
+  |         |      |           |          | ESC [ ?  |
+  | PG DOWN | 0x0A | ESC [ G   | ESC /    | ESC [ U  |
+  |         |      |           |          | ESC [ /  |
+  | F1      | 0x0B | ESC [ M   | ESC 1    | ESC O P  |
+  | F2      | 0x0C | ESC [ N   | ESC 2    | ESC O Q  |
+  | F3      | 0x0D | ESC [ O   | ESC 3    | ESC O w  |
+  | F4      | 0x0E | ESC [ P   | ESC 4    | ESC O x  |
+  | F5      | 0x0F | ESC [ Q   | ESC 5    | ESC O t  |
+  | F6      | 0x10 | ESC [ R   | ESC 6    | ESC O u  |
+  | F7      | 0x11 | ESC [ S   | ESC 7    | ESC O q  |
+  | F8      | 0x12 | ESC [ T   | ESC 8    | ESC O r  |
+  | F9      | 0x13 | ESC [ U   | ESC 9    | ESC O p  |
+  | F10     | 0x14 | ESC [ V   | ESC 0    | ESC O M  |
+  | Escape  | 0x17 | ESC       | ESC      | ESC      |
+  | F11     | 0x15 |           | ESC !    |          |
+  | F12     | 0x16 |           | ESC @    |          |
+  +=========+======+===========+==========+==========+
+
+  Special Mappings
+  ================
+  ESC R ESC r ESC R = Reset System
+
+  @param TerminalDevice   The terminal device to use to translate raw input into EFI Keys
+
+**/
+VOID
+UnicodeToEfiKey (
+  IN  TERMINAL_DEV    *TerminalDevice
+  )
+{
+  EFI_STATUS          Status;
+  EFI_STATUS          TimerStatus;
+  UINT16              UnicodeChar;
+  EFI_INPUT_KEY       Key;
+  BOOLEAN             SetDefaultResetState;
+
+  TimerStatus = gBS->CheckEvent (TerminalDevice->TwoSecondTimeOut);
+
+  if (!EFI_ERROR (TimerStatus)) {
+    UnicodeToEfiKeyFlushState (TerminalDevice);
+    TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+  }
+
+  while (!IsUnicodeFiFoEmpty (TerminalDevice) && !IsEfiKeyFiFoFull (TerminalDevice)) {
+
+    if (TerminalDevice->InputState != INPUT_STATE_DEFAULT) {
+      //
+      // Check to see if the 2 seconds timer has expired
+      //
+      TimerStatus = gBS->CheckEvent (TerminalDevice->TwoSecondTimeOut);
+      if (!EFI_ERROR (TimerStatus)) {
+        UnicodeToEfiKeyFlushState (TerminalDevice);
+        TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+      }
+    }
+
+    //
+    // Fetch one Unicode character from the Unicode FIFO
+    //
+    UnicodeFiFoRemoveOneKey (TerminalDevice, &UnicodeChar);
+
+    SetDefaultResetState = TRUE;
+
+    switch (TerminalDevice->InputState) {
+    case INPUT_STATE_DEFAULT:
+
+      break;
+
+    case INPUT_STATE_ESC:
+
+      if (UnicodeChar == LEFTOPENBRACKET) {
+        TerminalDevice->InputState |= INPUT_STATE_LEFTOPENBRACKET;
+        TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+        continue;
+      }
+
+      if (UnicodeChar == 'O' && (TerminalDevice->TerminalType == VT100TYPE ||
+                                 TerminalDevice->TerminalType == TTYTERMTYPE)) {
+        TerminalDevice->InputState |= INPUT_STATE_O;
+        TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+        continue;
+      }
+
+      Key.ScanCode = SCAN_NULL;
+
+      if (TerminalDevice->TerminalType == VT100PLUSTYPE ||
+          TerminalDevice->TerminalType == VTUTF8TYPE) {
+        switch (UnicodeChar) {
+        case '1':
+          Key.ScanCode = SCAN_F1;
+          break;
+        case '2':
+          Key.ScanCode = SCAN_F2;
+          break;
+        case '3':
+          Key.ScanCode = SCAN_F3;
+          break;
+        case '4':
+          Key.ScanCode = SCAN_F4;
+          break;
+        case '5':
+          Key.ScanCode = SCAN_F5;
+          break;
+        case '6':
+          Key.ScanCode = SCAN_F6;
+          break;
+        case '7':
+          Key.ScanCode = SCAN_F7;
+          break;
+        case '8':
+          Key.ScanCode = SCAN_F8;
+          break;
+        case '9':
+          Key.ScanCode = SCAN_F9;
+          break;
+        case '0':
+          Key.ScanCode = SCAN_F10;
+          break;
+        case '!':
+          Key.ScanCode = SCAN_F11;
+          break;
+        case '@':
+          Key.ScanCode = SCAN_F12;
+          break;
+        case 'h':
+          Key.ScanCode = SCAN_HOME;
+          break;
+        case 'k':
+          Key.ScanCode = SCAN_END;
+          break;
+        case '+':
+          Key.ScanCode = SCAN_INSERT;
+          break;
+        case '-':
+          Key.ScanCode = SCAN_DELETE;
+          break;
+        case '/':
+          Key.ScanCode = SCAN_PAGE_DOWN;
+          break;
+        case '?':
+          Key.ScanCode = SCAN_PAGE_UP;
+          break;
+        default :
+          break;
+        }
+      }
+
+      switch (UnicodeChar) {
+      case 'R':
+        if (TerminalDevice->ResetState == RESET_STATE_DEFAULT) {
+          TerminalDevice->ResetState = RESET_STATE_ESC_R;
+          SetDefaultResetState = FALSE;
+        } else if (TerminalDevice->ResetState == RESET_STATE_ESC_R_ESC_R) {
+          gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+        }
+        Key.ScanCode = SCAN_NULL;
+        break;
+      case 'r':
+        if (TerminalDevice->ResetState == RESET_STATE_ESC_R) {
+          TerminalDevice->ResetState = RESET_STATE_ESC_R_ESC_R;
+          SetDefaultResetState = FALSE;
+        }
+        Key.ScanCode = SCAN_NULL;
+        break;
+      default :
+        break;
+      }
+
+      if (SetDefaultResetState) {
+        TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+      }
+
+      if (Key.ScanCode != SCAN_NULL) {
+        Key.UnicodeChar = 0;
+        EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+        TerminalDevice->InputState = INPUT_STATE_DEFAULT;
+        UnicodeToEfiKeyFlushState (TerminalDevice);
+        continue;
+      }
+
+      UnicodeToEfiKeyFlushState (TerminalDevice);
+
+      break;
+
+    case INPUT_STATE_ESC | INPUT_STATE_O:
+
+      TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+
+      Key.ScanCode = SCAN_NULL;
+
+      if (TerminalDevice->TerminalType == VT100TYPE) {
+        switch (UnicodeChar) {
+        case 'P':
+          Key.ScanCode = SCAN_F1;
+          break;
+        case 'Q':
+          Key.ScanCode = SCAN_F2;
+          break;
+        case 'w':
+          Key.ScanCode = SCAN_F3;
+          break;
+        case 'x':
+          Key.ScanCode = SCAN_F4;
+          break;
+        case 't':
+          Key.ScanCode = SCAN_F5;
+          break;
+        case 'u':
+          Key.ScanCode = SCAN_F6;
+          break;
+        case 'q':
+          Key.ScanCode = SCAN_F7;
+          break;
+        case 'r':
+          Key.ScanCode = SCAN_F8;
+          break;
+        case 'p':
+          Key.ScanCode = SCAN_F9;
+          break;
+        case 'M':
+          Key.ScanCode = SCAN_F10;
+          break;
+        default :
+          break;
+        }
+      } else if (TerminalDevice->TerminalType == TTYTERMTYPE) {
+        /* Also accept VT100 escape codes for F1-F4 for TTY term */
+        switch (UnicodeChar) {
+        case 'P':
+          Key.ScanCode = SCAN_F1;
+          break;
+        case 'Q':
+          Key.ScanCode = SCAN_F2;
+          break;
+        case 'R':
+          Key.ScanCode = SCAN_F3;
+          break;
+        case 'S':
+          Key.ScanCode = SCAN_F4;
+          break;
+        }
+      }
+
+      if (Key.ScanCode != SCAN_NULL) {
+        Key.UnicodeChar = 0;
+        EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+        TerminalDevice->InputState = INPUT_STATE_DEFAULT;
+        UnicodeToEfiKeyFlushState (TerminalDevice);
+        continue;
+      }
+
+      UnicodeToEfiKeyFlushState (TerminalDevice);
+
+      break;
+
+    case INPUT_STATE_ESC | INPUT_STATE_LEFTOPENBRACKET:
+
+      TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+
+      Key.ScanCode = SCAN_NULL;
+
+      if (TerminalDevice->TerminalType == PCANSITYPE    ||
+          TerminalDevice->TerminalType == VT100TYPE     ||
+          TerminalDevice->TerminalType == VT100PLUSTYPE ||
+          TerminalDevice->TerminalType == VTUTF8TYPE    ||
+          TerminalDevice->TerminalType == TTYTERMTYPE) {
+        switch (UnicodeChar) {
+        case 'A':
+          Key.ScanCode = SCAN_UP;
+          break;
+        case 'B':
+          Key.ScanCode = SCAN_DOWN;
+          break;
+        case 'C':
+          Key.ScanCode = SCAN_RIGHT;
+          break;
+        case 'D':
+          Key.ScanCode = SCAN_LEFT;
+          break;
+        case 'H':
+          if (TerminalDevice->TerminalType == PCANSITYPE ||
+              TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_HOME;
+          }
+          break;
+        case 'F':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_END;
+          }
+          break;
+        case 'K':
+          if (TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_END;
+          }
+          break;
+        case 'L':
+        case '@':
+          if (TerminalDevice->TerminalType == PCANSITYPE ||
+              TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_INSERT;
+          }
+          break;
+        case 'X':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_DELETE;
+          }
+          break;
+        case 'P':
+          if (TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_DELETE;
+          } else if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F4;
+          }
+          break;
+        case 'I':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_PAGE_UP;
+          }
+          break;
+        case 'V':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F10;
+          }
+          break;
+        case '?':
+          if (TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_PAGE_UP;
+          }
+          break;
+        case 'G':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_PAGE_DOWN;
+          }
+          break;
+        case 'U':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F9;
+          }
+          break;
+        case '/':
+          if (TerminalDevice->TerminalType == VT100TYPE) {
+            Key.ScanCode = SCAN_PAGE_DOWN;
+          }
+          break;
+        case 'M':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F1;
+          }
+          break;
+        case 'N':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F2;
+          }
+          break;
+        case 'O':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F3;
+          }
+          break;
+        case 'Q':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F5;
+          }
+          break;
+        case 'R':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F6;
+          }
+          break;
+        case 'S':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F7;
+          }
+          break;
+        case 'T':
+          if (TerminalDevice->TerminalType == PCANSITYPE) {
+            Key.ScanCode = SCAN_F8;
+          }
+          break;
+        default :
+          break;
+        }
+      }
+
+      /*
+       * The VT220 escape codes that the TTY terminal accepts all have
+       * numeric codes, and there are no ambiguous prefixes shared with
+       * other terminal types.
+       */
+      if (TerminalDevice->TerminalType == TTYTERMTYPE &&
+          Key.ScanCode == SCAN_NULL &&
+          UnicodeChar >= '0' &&
+          UnicodeChar <= '9') {
+        TerminalDevice->TtyEscapeStr[0] = UnicodeChar;
+        TerminalDevice->TtyEscapeIndex = 1;
+        TerminalDevice->InputState |= INPUT_STATE_LEFTOPENBRACKET_2;
+        continue;
+      }
+
+      if (Key.ScanCode != SCAN_NULL) {
+        Key.UnicodeChar = 0;
+        EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+        TerminalDevice->InputState = INPUT_STATE_DEFAULT;
+        UnicodeToEfiKeyFlushState (TerminalDevice);
+        continue;
+      }
+
+      UnicodeToEfiKeyFlushState (TerminalDevice);
+
+      break;
+
+
+    case INPUT_STATE_ESC | INPUT_STATE_LEFTOPENBRACKET | INPUT_STATE_LEFTOPENBRACKET_2:
+      /*
+       * Here we handle the VT220 escape codes that we accept.  This
+       * state is only used by the TTY terminal type.
+       */
+      Key.ScanCode = SCAN_NULL;
+      if (TerminalDevice->TerminalType == TTYTERMTYPE) {
+
+        if (UnicodeChar == '~' && TerminalDevice->TtyEscapeIndex <= 2) {
+          UINT16 EscCode;
+          TerminalDevice->TtyEscapeStr[TerminalDevice->TtyEscapeIndex] = 0; /* Terminate string */
+          EscCode = (UINT16) StrDecimalToUintn(TerminalDevice->TtyEscapeStr);
+          switch (EscCode) {
+          case 3:
+              Key.ScanCode = SCAN_DELETE;
+              break;
+          case 11:
+          case 12:
+          case 13:
+          case 14:
+          case 15:
+            Key.ScanCode = SCAN_F1 + EscCode - 11;
+            break;
+          case 17:
+          case 18:
+          case 19:
+          case 20:
+          case 21:
+            Key.ScanCode = SCAN_F6 + EscCode - 17;
+            break;
+          case 23:
+          case 24:
+            Key.ScanCode = SCAN_F11 + EscCode - 23;
+            break;
+          default:
+            break;
+          }
+        } else if (TerminalDevice->TtyEscapeIndex == 1){
+          /* 2 character escape code   */
+          TerminalDevice->TtyEscapeStr[TerminalDevice->TtyEscapeIndex++] = UnicodeChar;
+          continue;
+        }
+        else {
+          DEBUG ((EFI_D_ERROR, "Unexpected state in escape2\n"));
+        }
+      }
+      TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+
+      if (Key.ScanCode != SCAN_NULL) {
+        Key.UnicodeChar = 0;
+        EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+        TerminalDevice->InputState = INPUT_STATE_DEFAULT;
+        UnicodeToEfiKeyFlushState (TerminalDevice);
+        continue;
+      }
+
+      UnicodeToEfiKeyFlushState (TerminalDevice);
+      break;
+
+    default:
+      //
+      // Invalid state. This should never happen.
+      //
+      ASSERT (FALSE);
+
+      UnicodeToEfiKeyFlushState (TerminalDevice);
+
+      break;
+    }
+
+    if (UnicodeChar == ESC) {
+      TerminalDevice->InputState = INPUT_STATE_ESC;
+    }
+
+    if (UnicodeChar == CSI) {
+      TerminalDevice->InputState = INPUT_STATE_CSI;
+    }
+
+    if (TerminalDevice->InputState != INPUT_STATE_DEFAULT) {
+      Status = gBS->SetTimer(
+                      TerminalDevice->TwoSecondTimeOut,
+                      TimerRelative,
+                      (UINT64)20000000
+                      );
+      ASSERT_EFI_ERROR (Status);
+      continue;
+    }
+
+    if (SetDefaultResetState) {
+      TerminalDevice->ResetState = RESET_STATE_DEFAULT;
+    }
+
+    if (UnicodeChar == DEL) {
+      if (TerminalDevice->TerminalType == TTYTERMTYPE) {
+        Key.ScanCode    = SCAN_NULL;
+        Key.UnicodeChar = CHAR_BACKSPACE;
+      }
+      else {
+        Key.ScanCode    = SCAN_DELETE;
+        Key.UnicodeChar = 0;
+      }
+    } else {
+      Key.ScanCode    = SCAN_NULL;
+      Key.UnicodeChar = UnicodeChar;
+    }
+
+    EfiKeyFiFoInsertOneKey (TerminalDevice, &Key);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConOut.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConOut.c
new file mode 100644
index 0000000000..9fa952ad2a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalConOut.c
@@ -0,0 +1,908 @@
+/** @file
+  Implementation for EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL protocol.
+
+Copyright (c) 2006 - 2010, 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 "Terminal.h"
+
+//
+// This list is used to define the valid extend chars.
+// It also provides a mapping from Unicode to PCANSI or
+// ASCII. The ASCII mapping we just made up.
+//
+//
+UNICODE_TO_CHAR  UnicodeToPcAnsiOrAscii[] = {
+  { BOXDRAW_HORIZONTAL,                 0xc4, L'-' },
+  { BOXDRAW_VERTICAL,                   0xb3, L'|' },
+  { BOXDRAW_DOWN_RIGHT,                 0xda, L'/' },
+  { BOXDRAW_DOWN_LEFT,                  0xbf, L'\\' },
+  { BOXDRAW_UP_RIGHT,                   0xc0, L'\\' },
+  { BOXDRAW_UP_LEFT,                    0xd9, L'/' },
+  { BOXDRAW_VERTICAL_RIGHT,             0xc3, L'|' },
+  { BOXDRAW_VERTICAL_LEFT,              0xb4, L'|' },
+  { BOXDRAW_DOWN_HORIZONTAL,            0xc2, L'+' },
+  { BOXDRAW_UP_HORIZONTAL,              0xc1, L'+' },
+  { BOXDRAW_VERTICAL_HORIZONTAL,        0xc5, L'+' },
+  { BOXDRAW_DOUBLE_HORIZONTAL,          0xcd, L'-' },
+  { BOXDRAW_DOUBLE_VERTICAL,            0xba, L'|' },
+  { BOXDRAW_DOWN_RIGHT_DOUBLE,          0xd5, L'/' },
+  { BOXDRAW_DOWN_DOUBLE_RIGHT,          0xd6, L'/' },
+  { BOXDRAW_DOUBLE_DOWN_RIGHT,          0xc9, L'/' },
+  { BOXDRAW_DOWN_LEFT_DOUBLE,           0xb8, L'\\' },
+  { BOXDRAW_DOWN_DOUBLE_LEFT,           0xb7, L'\\' },
+  { BOXDRAW_DOUBLE_DOWN_LEFT,           0xbb, L'\\' },
+  { BOXDRAW_UP_RIGHT_DOUBLE,            0xd4, L'\\' },
+  { BOXDRAW_UP_DOUBLE_RIGHT,            0xd3, L'\\' },
+  { BOXDRAW_DOUBLE_UP_RIGHT,            0xc8, L'\\' },
+  { BOXDRAW_UP_LEFT_DOUBLE,             0xbe, L'/' },
+  { BOXDRAW_UP_DOUBLE_LEFT,             0xbd, L'/' },
+  { BOXDRAW_DOUBLE_UP_LEFT,             0xbc, L'/' },
+  { BOXDRAW_VERTICAL_RIGHT_DOUBLE,      0xc6, L'|' },
+  { BOXDRAW_VERTICAL_DOUBLE_RIGHT,      0xc7, L'|' },
+  { BOXDRAW_DOUBLE_VERTICAL_RIGHT,      0xcc, L'|' },
+  { BOXDRAW_VERTICAL_LEFT_DOUBLE,       0xb5, L'|' },
+  { BOXDRAW_VERTICAL_DOUBLE_LEFT,       0xb6, L'|' },
+  { BOXDRAW_DOUBLE_VERTICAL_LEFT,       0xb9, L'|' },
+  { BOXDRAW_DOWN_HORIZONTAL_DOUBLE,     0xd1, L'+' },
+  { BOXDRAW_DOWN_DOUBLE_HORIZONTAL,     0xd2, L'+' },
+  { BOXDRAW_DOUBLE_DOWN_HORIZONTAL,     0xcb, L'+' },
+  { BOXDRAW_UP_HORIZONTAL_DOUBLE,       0xcf, L'+' },
+  { BOXDRAW_UP_DOUBLE_HORIZONTAL,       0xd0, L'+' },
+  { BOXDRAW_DOUBLE_UP_HORIZONTAL,       0xca, L'+' },
+  { BOXDRAW_VERTICAL_HORIZONTAL_DOUBLE, 0xd8, L'+' },
+  { BOXDRAW_VERTICAL_DOUBLE_HORIZONTAL, 0xd7, L'+' },
+  { BOXDRAW_DOUBLE_VERTICAL_HORIZONTAL, 0xce, L'+' },
+
+  { BLOCKELEMENT_FULL_BLOCK,            0xdb, L'*' },
+  { BLOCKELEMENT_LIGHT_SHADE,           0xb0, L'+' },
+
+  { GEOMETRICSHAPE_UP_TRIANGLE,         0x1e, L'^' },
+  { GEOMETRICSHAPE_RIGHT_TRIANGLE,      0x10, L'>' },
+  { GEOMETRICSHAPE_DOWN_TRIANGLE,       0x1f, L'v' },
+  { GEOMETRICSHAPE_LEFT_TRIANGLE,       0x11, L'<' },
+
+  { ARROW_LEFT,                         0x3c, L'<' },
+  { ARROW_UP,                           0x18, L'^' },
+  { ARROW_RIGHT,                        0x3e, L'>' },
+  { ARROW_DOWN,                         0x19, L'v' },
+
+  { 0x0000,                             0x00, L'\0' }
+};
+
+CHAR16 mSetModeString[]            = { ESC, '[', '=', '3', 'h', 0 };
+CHAR16 mSetAttributeString[]       = { ESC, '[', '0', 'm', ESC, '[', '4', '0', 'm', ESC, '[', '4', '0', 'm', 0 };
+CHAR16 mClearScreenString[]        = { ESC, '[', '2', 'J', 0 };
+CHAR16 mSetCursorPositionString[]  = { ESC, '[', '0', '0', ';', '0', '0', 'H', 0 };
+
+//
+// Body of the ConOut functions
+//
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.Reset().
+
+  If ExtendeVerification is TRUE, then perform dependent serial device reset,
+  and set display mode to mode 0.
+  If ExtendedVerification is FALSE, only set display mode to mode 0.
+
+  @param  This                  Indicates the calling context.
+  @param  ExtendedVerification  Indicates that the driver may perform a more
+                                exhaustive verification operation of the device
+                                during reset.
+
+  @retval EFI_SUCCESS           The reset operation succeeds.
+  @retval EFI_DEVICE_ERROR      The terminal is not functioning correctly or the serial port reset fails.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutReset (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          ExtendedVerification
+  )
+{
+  EFI_STATUS    Status;
+  TERMINAL_DEV  *TerminalDevice;
+
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  //
+  // Perform a more exhaustive reset by resetting the serial port.
+  //
+  if (ExtendedVerification) {
+    //
+    // Report progress code here
+    //
+    REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+      EFI_PROGRESS_CODE,
+      (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_PC_RESET),
+      TerminalDevice->DevicePath
+      );
+
+    Status = TerminalDevice->SerialIo->Reset (TerminalDevice->SerialIo);
+    if (EFI_ERROR (Status)) {
+      //
+      // Report error code here
+      //
+      REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+        EFI_ERROR_CODE | EFI_ERROR_MINOR,
+        (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_EC_CONTROLLER_ERROR),
+        TerminalDevice->DevicePath
+        );
+
+      return Status;
+    }
+  }
+
+  This->SetAttribute (This, EFI_TEXT_ATTR (This->Mode->Attribute & 0x0F, EFI_BLACK));
+
+  Status = This->SetMode (This, 0);
+
+  return Status;
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.OutputString().
+
+  The Unicode string will be converted to terminal expressible data stream
+  and send to terminal via serial port.
+
+  @param  This                    Indicates the calling context.
+  @param  WString                 The Null-terminated Unicode string to be displayed
+                                  on the terminal screen.
+
+  @retval EFI_SUCCESS             The string is output successfully.
+  @retval EFI_DEVICE_ERROR        The serial port fails to send the string out.
+  @retval EFI_WARN_UNKNOWN_GLYPH  Indicates that some of the characters in the Unicode string could not
+                                  be rendered and are skipped.
+  @retval EFI_UNSUPPORTED         If current display mode is out of range.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutOutputString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  )
+{
+  TERMINAL_DEV                *TerminalDevice;
+  EFI_SIMPLE_TEXT_OUTPUT_MODE *Mode;
+  UINTN                       MaxColumn;
+  UINTN                       MaxRow;
+  UINTN                       Length;
+  UTF8_CHAR                   Utf8Char;
+  CHAR8                       GraphicChar;
+  CHAR8                       AsciiChar;
+  EFI_STATUS                  Status;
+  UINT8                       ValidBytes;
+  //
+  //  flag used to indicate whether condition happens which will cause
+  //  return EFI_WARN_UNKNOWN_GLYPH
+  //
+  BOOLEAN                     Warning;
+
+  ValidBytes  = 0;
+  Warning     = FALSE;
+  AsciiChar   = 0;
+
+  //
+  //  get Terminal device data structure pointer.
+  //
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  //
+  //  Get current display mode
+  //
+  Mode = This->Mode;
+
+  if (Mode->Mode >= Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  This->QueryMode (
+          This,
+          Mode->Mode,
+          &MaxColumn,
+          &MaxRow
+          );
+
+  for (; *WString != CHAR_NULL; WString++) {
+
+    switch (TerminalDevice->TerminalType) {
+
+    case PCANSITYPE:
+    case VT100TYPE:
+    case VT100PLUSTYPE:
+    case TTYTERMTYPE:
+
+      if (!TerminalIsValidTextGraphics (*WString, &GraphicChar, &AsciiChar)) {
+        //
+        // If it's not a graphic character convert Unicode to ASCII.
+        //
+        GraphicChar = (CHAR8) *WString;
+
+        if (!(TerminalIsValidAscii (GraphicChar) || TerminalIsValidEfiCntlChar (GraphicChar))) {
+          //
+          // when this driver use the OutputString to output control string,
+          // TerminalDevice->OutputEscChar is set to let the Esc char
+          // to be output to the terminal emulation software.
+          //
+          if ((GraphicChar == 27) && TerminalDevice->OutputEscChar) {
+            GraphicChar = 27;
+          } else {
+            GraphicChar = '?';
+            Warning     = TRUE;
+          }
+        }
+
+        AsciiChar = GraphicChar;
+
+      }
+
+      if (TerminalDevice->TerminalType != PCANSITYPE) {
+        GraphicChar = AsciiChar;
+      }
+
+      Length = 1;
+
+      Status = TerminalDevice->SerialIo->Write (
+                                          TerminalDevice->SerialIo,
+                                          &Length,
+                                          &GraphicChar
+                                          );
+
+      if (EFI_ERROR (Status)) {
+        goto OutputError;
+      }
+
+      break;
+
+    case VTUTF8TYPE:
+      UnicodeToUtf8 (*WString, &Utf8Char, &ValidBytes);
+      Length = ValidBytes;
+      Status = TerminalDevice->SerialIo->Write (
+                                          TerminalDevice->SerialIo,
+                                          &Length,
+                                          (UINT8 *) &Utf8Char
+                                          );
+      if (EFI_ERROR (Status)) {
+        goto OutputError;
+      }
+      break;
+    }
+    //
+    //  Update cursor position.
+    //
+    switch (*WString) {
+
+    case CHAR_BACKSPACE:
+      if (Mode->CursorColumn > 0) {
+        Mode->CursorColumn--;
+      }
+      break;
+
+    case CHAR_LINEFEED:
+      if (Mode->CursorRow < (INT32) (MaxRow - 1)) {
+        Mode->CursorRow++;
+      }
+      break;
+
+    case CHAR_CARRIAGE_RETURN:
+      Mode->CursorColumn = 0;
+      break;
+
+    default:
+      if (Mode->CursorColumn < (INT32) (MaxColumn - 1)) {
+
+        Mode->CursorColumn++;
+
+      } else {
+
+        Mode->CursorColumn = 0;
+        if (Mode->CursorRow < (INT32) (MaxRow - 1)) {
+          Mode->CursorRow++;
+        }
+
+      }
+      break;
+
+    };
+
+  }
+
+  if (Warning) {
+    return EFI_WARN_UNKNOWN_GLYPH;
+  }
+
+  return EFI_SUCCESS;
+
+OutputError:
+  REPORT_STATUS_CODE_WITH_DEVICE_PATH (
+    EFI_ERROR_CODE | EFI_ERROR_MINOR,
+    (EFI_PERIPHERAL_REMOTE_CONSOLE | EFI_P_EC_OUTPUT_ERROR),
+    TerminalDevice->DevicePath
+    );
+
+  return EFI_DEVICE_ERROR;
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.TestString().
+
+  If one of the characters in the *Wstring is
+  neither valid Unicode drawing characters,
+  not ASCII code, then this function will return
+  EFI_UNSUPPORTED.
+
+  @param  This              Indicates the calling context.
+  @param  WString           The Null-terminated Unicode string to be tested.
+
+  @retval EFI_SUCCESS       The terminal is capable of rendering the output string.
+  @retval EFI_UNSUPPORTED   Some of the characters in the Unicode string cannot be rendered.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutTestString (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  CHAR16                           *WString
+  )
+{
+  TERMINAL_DEV  *TerminalDevice;
+  EFI_STATUS    Status;
+
+  //
+  //  get Terminal device data structure pointer.
+  //
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  switch (TerminalDevice->TerminalType) {
+
+  case PCANSITYPE:
+  case VT100TYPE:
+  case VT100PLUSTYPE:
+  case TTYTERMTYPE:
+    Status = AnsiTestString (TerminalDevice, WString);
+    break;
+
+  case VTUTF8TYPE:
+    Status = VTUTF8TestString (TerminalDevice, WString);
+    break;
+
+  default:
+    Status = EFI_UNSUPPORTED;
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.QueryMode().
+
+  It returns information for an available text mode
+  that the terminal supports.
+
+  @param This        Indicates the calling context.
+  @param ModeNumber  The mode number to return information on.
+  @param Columns     The returned columns of the requested mode.
+  @param Rows        The returned rows of the requested mode.
+
+  @retval EFI_SUCCESS       The requested mode information is returned.
+  @retval EFI_UNSUPPORTED   The mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutQueryMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber,
+  OUT UINTN                            *Columns,
+  OUT UINTN                            *Rows
+  )
+{
+  TERMINAL_DEV  *TerminalDevice;
+
+  if (ModeNumber >= (UINTN) This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Terminal device data structure pointer.
+  //
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);  
+  *Columns = TerminalDevice->TerminalConsoleModeData[ModeNumber].Columns;
+  *Rows    = TerminalDevice->TerminalConsoleModeData[ModeNumber].Rows;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUT.SetMode().
+
+  Set the terminal to a specified display mode.
+  In this driver, we only support mode 0.
+
+  @param This          Indicates the calling context.
+  @param ModeNumber    The text mode to set.
+
+  @retval EFI_SUCCESS       The requested text mode is set.
+  @retval EFI_DEVICE_ERROR  The requested text mode cannot be set 
+                            because of serial device error.
+  @retval EFI_UNSUPPORTED   The text mode number is not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetMode (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            ModeNumber
+  )
+{
+  EFI_STATUS    Status;
+  TERMINAL_DEV  *TerminalDevice;
+
+  //
+  //  get Terminal device data structure pointer.
+  //
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  if (ModeNumber >= (UINTN) This->Mode->MaxMode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Set the current mode
+  //
+  This->Mode->Mode = (INT32) ModeNumber;
+
+  This->ClearScreen (This);
+
+  TerminalDevice->OutputEscChar = TRUE;
+  Status                        = This->OutputString (This, mSetModeString);
+  TerminalDevice->OutputEscChar = FALSE;
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  This->Mode->Mode  = (INT32) ModeNumber;
+
+  Status            = This->ClearScreen (This);
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetAttribute().
+
+  @param This        Indicates the calling context.
+  @param Attribute   The attribute to set. Only bit0..6 are valid, all other bits
+                     are undefined and must be zero.
+
+  @retval EFI_SUCCESS        The requested attribute is set.
+  @retval EFI_DEVICE_ERROR   The requested attribute cannot be set due to serial port error.
+  @retval EFI_UNSUPPORTED    The attribute requested is not defined by EFI spec.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetAttribute (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Attribute
+  )
+{
+  UINT8         ForegroundControl;
+  UINT8         BackgroundControl;
+  UINT8         BrightControl;
+  INT32         SavedColumn;
+  INT32         SavedRow;
+  EFI_STATUS    Status;
+  TERMINAL_DEV  *TerminalDevice;
+
+  SavedColumn = 0;
+  SavedRow    = 0;
+
+  //
+  //  get Terminal device data structure pointer.
+  //
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  //
+  //  only the bit0..6 of the Attribute is valid
+  //
+  if ((Attribute | 0x7f) != 0x7f) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Skip outputting the command string for the same attribute
+  // It improves the terminal performance significantly
+  //
+  if (This->Mode->Attribute == (INT32) Attribute) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  //  convert Attribute value to terminal emulator
+  //  understandable foreground color
+  //
+  switch (Attribute & 0x07) {
+
+  case EFI_BLACK:
+    ForegroundControl = 30;
+    break;
+
+  case EFI_BLUE:
+    ForegroundControl = 34;
+    break;
+
+  case EFI_GREEN:
+    ForegroundControl = 32;
+    break;
+
+  case EFI_CYAN:
+    ForegroundControl = 36;
+    break;
+
+  case EFI_RED:
+    ForegroundControl = 31;
+    break;
+
+  case EFI_MAGENTA:
+    ForegroundControl = 35;
+    break;
+
+  case EFI_BROWN:
+    ForegroundControl = 33;
+    break;
+
+  default:
+
+  case EFI_LIGHTGRAY:
+    ForegroundControl = 37;
+    break;
+
+  }
+  //
+  //  bit4 of the Attribute indicates bright control
+  //  of terminal emulator.
+  //
+  BrightControl = (UINT8) ((Attribute >> 3) & 1);
+
+  //
+  //  convert Attribute value to terminal emulator
+  //  understandable background color.
+  //
+  switch ((Attribute >> 4) & 0x07) {
+
+  case EFI_BLACK:
+    BackgroundControl = 40;
+    break;
+
+  case EFI_BLUE:
+    BackgroundControl = 44;
+    break;
+
+  case EFI_GREEN:
+    BackgroundControl = 42;
+    break;
+
+  case EFI_CYAN:
+    BackgroundControl = 46;
+    break;
+
+  case EFI_RED:
+    BackgroundControl = 41;
+    break;
+
+  case EFI_MAGENTA:
+    BackgroundControl = 45;
+    break;
+
+  case EFI_BROWN:
+    BackgroundControl = 43;
+    break;
+
+  default:
+
+  case EFI_LIGHTGRAY:
+    BackgroundControl = 47;
+    break;
+  }
+  //
+  // terminal emulator's control sequence to set attributes
+  //
+  mSetAttributeString[BRIGHT_CONTROL_OFFSET]          = (CHAR16) ('0' + BrightControl);
+  mSetAttributeString[FOREGROUND_CONTROL_OFFSET + 0]  = (CHAR16) ('0' + (ForegroundControl / 10));
+  mSetAttributeString[FOREGROUND_CONTROL_OFFSET + 1]  = (CHAR16) ('0' + (ForegroundControl % 10));
+  mSetAttributeString[BACKGROUND_CONTROL_OFFSET + 0]  = (CHAR16) ('0' + (BackgroundControl / 10));
+  mSetAttributeString[BACKGROUND_CONTROL_OFFSET + 1]  = (CHAR16) ('0' + (BackgroundControl % 10));
+
+  //
+  // save current column and row
+  // for future scrolling back use.
+  //
+  SavedColumn                   = This->Mode->CursorColumn;
+  SavedRow                      = This->Mode->CursorRow;
+
+  TerminalDevice->OutputEscChar = TRUE;
+  Status                        = This->OutputString (This, mSetAttributeString);
+  TerminalDevice->OutputEscChar = FALSE;
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  //  scroll back to saved cursor position.
+  //
+  This->Mode->CursorColumn  = SavedColumn;
+  This->Mode->CursorRow     = SavedRow;
+
+  This->Mode->Attribute     = (INT32) Attribute;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.ClearScreen().
+  It clears the ANSI terminal's display to the
+  currently selected background color.
+
+  @param This     Indicates the calling context.
+
+  @retval EFI_SUCCESS       The operation completed successfully.
+  @retval EFI_DEVICE_ERROR  The terminal screen cannot be cleared due to serial port error.
+  @retval EFI_UNSUPPORTED   The terminal is not in a valid display mode.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutClearScreen (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This
+  )
+{
+  EFI_STATUS    Status;
+  TERMINAL_DEV  *TerminalDevice;
+
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  //
+  //  control sequence for clear screen request
+  //
+  TerminalDevice->OutputEscChar = TRUE;
+  Status                        = This->OutputString (This, mClearScreenString);
+  TerminalDevice->OutputEscChar = FALSE;
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Status = This->SetCursorPosition (This, 0, 0);
+
+  return Status;
+}
+
+
+/**
+  Implements EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.SetCursorPosition().
+
+  @param This      Indicates the calling context.
+  @param Column    The row to set cursor to.
+  @param Row       The column to set cursor to.
+
+  @retval EFI_SUCCESS       The operation completed successfully.
+  @retval EFI_DEVICE_ERROR  The request fails due to serial port error.
+  @retval EFI_UNSUPPORTED   The terminal is not in a valid text mode, or the cursor position
+                            is invalid for current mode.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutSetCursorPosition (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  UINTN                            Column,
+  IN  UINTN                            Row
+  )
+{
+  EFI_SIMPLE_TEXT_OUTPUT_MODE *Mode;
+  UINTN                       MaxColumn;
+  UINTN                       MaxRow;
+  EFI_STATUS                  Status;
+  TERMINAL_DEV                *TerminalDevice;
+
+  TerminalDevice = TERMINAL_CON_OUT_DEV_FROM_THIS (This);
+
+  //
+  //  get current mode
+  //
+  Mode = This->Mode;
+
+  //
+  //  get geometry of current mode
+  //
+  Status = This->QueryMode (
+                  This,
+                  Mode->Mode,
+                  &MaxColumn,
+                  &MaxRow
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Column >= MaxColumn || Row >= MaxRow) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // control sequence to move the cursor
+  //
+  mSetCursorPositionString[ROW_OFFSET + 0]    = (CHAR16) ('0' + ((Row + 1) / 10));
+  mSetCursorPositionString[ROW_OFFSET + 1]    = (CHAR16) ('0' + ((Row + 1) % 10));
+  mSetCursorPositionString[COLUMN_OFFSET + 0] = (CHAR16) ('0' + ((Column + 1) / 10));
+  mSetCursorPositionString[COLUMN_OFFSET + 1] = (CHAR16) ('0' + ((Column + 1) % 10));
+
+  TerminalDevice->OutputEscChar               = TRUE;
+  Status = This->OutputString (This, mSetCursorPositionString);
+  TerminalDevice->OutputEscChar = FALSE;
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  //  update current cursor position
+  //  in the Mode data structure.
+  //
+  Mode->CursorColumn  = (INT32) Column;
+  Mode->CursorRow     = (INT32) Row;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Implements SIMPLE_TEXT_OUTPUT.EnableCursor().
+
+  In this driver, the cursor cannot be hidden.
+
+  @param This      Indicates the calling context.
+  @param Visible   If TRUE, the cursor is set to be visible,
+                   If FALSE, the cursor is set to be invisible.
+
+  @retval EFI_SUCCESS      The request is valid.
+  @retval EFI_UNSUPPORTED  The terminal does not support cursor hidden.
+
+**/
+EFI_STATUS
+EFIAPI
+TerminalConOutEnableCursor (
+  IN  EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL  *This,
+  IN  BOOLEAN                          Visible
+  )
+{
+  if (!Visible) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Detects if a Unicode char is for Box Drawing text graphics.
+
+  @param  Graphic      Unicode char to test.
+  @param  PcAnsi       Optional pointer to return PCANSI equivalent of
+                       Graphic.
+  @param  Ascii        Optional pointer to return ASCII equivalent of
+                       Graphic.
+
+  @retval TRUE         If Graphic is a supported Unicode Box Drawing character.
+
+**/
+BOOLEAN
+TerminalIsValidTextGraphics (
+  IN  CHAR16  Graphic,
+  OUT CHAR8   *PcAnsi, OPTIONAL
+  OUT CHAR8   *Ascii OPTIONAL
+  )
+{
+  UNICODE_TO_CHAR *Table;
+
+  if ((((Graphic & 0xff00) != 0x2500) && ((Graphic & 0xff00) != 0x2100))) {
+    //
+    // Unicode drawing code charts are all in the 0x25xx range,
+    //  arrows are 0x21xx
+    //
+    return FALSE;
+  }
+
+  for (Table = UnicodeToPcAnsiOrAscii; Table->Unicode != 0x0000; Table++) {
+    if (Graphic == Table->Unicode) {
+      if (PcAnsi != NULL) {
+        *PcAnsi = Table->PcAnsi;
+      }
+
+      if (Ascii != NULL) {
+        *Ascii = Table->Ascii;
+      }
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Detects if a valid ASCII char.
+
+  @param  Ascii        An ASCII character.
+                       
+  @retval TRUE         If it is a valid ASCII character.
+  @retval FALSE        If it is not a valid ASCII character.
+
+**/
+BOOLEAN
+TerminalIsValidAscii (
+  IN  CHAR16  Ascii
+  )
+{
+  //
+  // valid ascii code lies in the extent of 0x20 ~ 0x7f
+  //
+  if ((Ascii >= 0x20) && (Ascii <= 0x7f)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Detects if a valid EFI control character.
+
+  @param  CharC        An input EFI Control character.
+                       
+  @retval TRUE         If it is a valid EFI control character.
+  @retval FALSE        If it is not a valid EFI control character.
+
+**/
+BOOLEAN
+TerminalIsValidEfiCntlChar (
+  IN  CHAR16  CharC
+  )
+{
+  //
+  // only support four control characters.
+  //
+  if (CharC == CHAR_NULL ||
+      CharC == CHAR_BACKSPACE ||
+      CharC == CHAR_LINEFEED ||
+      CharC == CHAR_CARRIAGE_RETURN ||
+      CharC == CHAR_TAB
+      ) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.inf b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.inf
new file mode 100644
index 0000000000..0780296798
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.inf
@@ -0,0 +1,99 @@
+## @file
+#  Terminal module installs Simple Text Input(ex)/Out protocols for serial devices.
+#
+#  This module will install Simple Text Input (Ex) protocol and Simple Test Output
+#  protocols based on Serial I/O protocol for serial devices including hotplug serial
+#  devices.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = TerminalDxe
+  MODULE_UNI_FILE                = TerminalDxe.uni
+  FILE_GUID                      = 9E863906-A40F-4875-977F-5B93FF237FC6
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeTerminal
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gTerminalDriverBinding
+#  COMPONENT_NAME                =  gTerminalComponentName
+#  COMPONENT_NAME2               =  gTerminalComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  Vtutf8.c
+  Ansi.c
+  TerminalConOut.c
+  TerminalConIn.c
+  Terminal.c
+  Terminal.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiRuntimeServicesTableLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  ReportStatusCodeLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  PcdLib
+  BaseLib
+
+[Guids]
+  ## SOMETIMES_PRODUCES ## Variable:L"ConInDev"
+  ## SOMETIMES_CONSUMES ## Variable:L"ConInDev"
+  ## SOMETIMES_PRODUCES ## Variable:L"ConOutDev"
+  ## SOMETIMES_CONSUMES ## Variable:L"ConOutDev"
+  ## SOMETIMES_PRODUCES ## Variable:L"ErrOutDev"
+  ## SOMETIMES_CONSUMES ## Variable:L"ErrOutDev"
+  gEfiGlobalVariableGuid
+  gEfiVTUTF8Guid                                ## SOMETIMES_CONSUMES ## GUID # used with a Vendor-Defined Messaging Device Path
+  gEfiVT100Guid                                 ## SOMETIMES_CONSUMES ## GUID # used with a Vendor-Defined Messaging Device Path
+  gEfiVT100PlusGuid                             ## SOMETIMES_CONSUMES ## GUID # used with a Vendor-Defined Messaging Device Path
+  gEfiPcAnsiGuid                                ## SOMETIMES_CONSUMES ## GUID # used with a Vendor-Defined Messaging Device Path
+  gEfiTtyTermGuid                               ## SOMETIMES_CONSUMES ## GUID # used with a Vendor-Defined Messaging Device Path
+  gEdkiiStatusCodeDataTypeVariableGuid          ## SOMETIMES_CONSUMES ## GUID
+
+[Protocols]
+  gEfiSerialIoProtocolGuid                      ## TO_START
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiSimpleTextInProtocolGuid                  ## BY_START
+  gEfiSimpleTextInputExProtocolGuid             ## BY_START
+  gEfiSimpleTextOutProtocolGuid                 ## BY_START
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdDefaultTerminalType           ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdErrorCodeSetVariable    ## CONSUMES
+
+# [Event]
+# # Relative timer event set by UnicodeToEfiKey(), used to be one 2 seconds input timeout.
+# EVENT_TYPE_RELATIVE_TIMER                   ## CONSUMES
+# # Period timer event to invoke TerminalConInTimerHandler(), period value is KEYBOARD_TIMER_INTERVAL and used to poll the key from serial
+# EVENT_TYPE_PERIODIC_TIMER                   ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  TerminalDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.uni b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.uni
new file mode 100644
index 0000000000..667ec097de
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxeExtra.uni b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxeExtra.uni
new file mode 100644
index 0000000000..c26e92b060
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/TerminalDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Console/TerminalDxe/Vtutf8.c b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Vtutf8.c
new file mode 100644
index 0000000000..586e8ab769
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Console/TerminalDxe/Vtutf8.c
@@ -0,0 +1,328 @@
+/** @file
+  Implementation of translation upon VT-UTF8.
+
+Copyright (c) 2006 - 2010, 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 "Terminal.h"
+
+/**
+  Translate all VT-UTF8 characters in the Raw FIFI into unicode characters,
+  and insert them into Unicode FIFO.
+
+  @param TerminalDevice          The terminal device.
+
+**/
+VOID
+VTUTF8RawDataToUnicode (
+  IN  TERMINAL_DEV    *TerminalDevice
+  )
+{
+  UTF8_CHAR Utf8Char;
+  UINT8     ValidBytes;
+  UINT16    UnicodeChar;
+
+  ValidBytes = 0;
+  //
+  // pop the raw data out from the raw fifo,
+  // and translate it into unicode, then push
+  // the unicode into unicode fifo, until the raw fifo is empty.
+  //
+  while (!IsRawFiFoEmpty (TerminalDevice) && !IsUnicodeFiFoFull (TerminalDevice)) {
+
+    GetOneValidUtf8Char (TerminalDevice, &Utf8Char, &ValidBytes);
+
+    if (ValidBytes < 1 || ValidBytes > 3) {
+      continue;
+    }
+
+    Utf8ToUnicode (Utf8Char, ValidBytes, (CHAR16 *) &UnicodeChar);
+
+    UnicodeFiFoInsertOneKey (TerminalDevice, UnicodeChar);
+  }
+}
+
+/**
+  Get one valid VT-UTF8 characters set from Raw Data FIFO.
+
+  @param  Utf8Device          The terminal device.
+  @param  Utf8Char            Returned valid VT-UTF8 characters set.
+  @param  ValidBytes          The count of returned VT-VTF8 characters.
+                              If ValidBytes is zero, no valid VT-UTF8 returned.
+
+**/
+VOID
+GetOneValidUtf8Char (
+  IN  TERMINAL_DEV      *Utf8Device,
+  OUT UTF8_CHAR         *Utf8Char,
+  OUT UINT8             *ValidBytes
+  )
+{
+  UINT8   Temp;
+  UINT8   Index;
+  BOOLEAN FetchFlag;
+
+  Temp      = 0;
+  Index     = 0;
+  FetchFlag = TRUE;
+
+  //
+  // if no valid Utf8 char is found in the RawFiFo,
+  // then *ValidBytes will be zero.
+  //
+  *ValidBytes = 0;
+
+  while (!IsRawFiFoEmpty (Utf8Device)) {
+
+    RawFiFoRemoveOneKey (Utf8Device, &Temp);
+
+    switch (*ValidBytes) {
+
+    case 0:
+      if ((Temp & 0x80) == 0) {
+        //
+        // one-byte utf8 char
+        //
+        *ValidBytes       = 1;
+
+        Utf8Char->Utf8_1  = Temp;
+
+        FetchFlag         = FALSE;
+
+      } else if ((Temp & 0xe0) == 0xc0) {
+        //
+        // two-byte utf8 char
+        //
+        *ValidBytes         = 2;
+
+        Utf8Char->Utf8_2[1] = Temp;
+
+      } else if ((Temp & 0xf0) == 0xe0) {
+        //
+        // three-byte utf8 char
+        //
+        *ValidBytes         = 3;
+
+        Utf8Char->Utf8_3[2] = Temp;
+
+        Index++;
+
+      } else {
+        //
+        // reset *ValidBytes to zero, let valid utf8 char search restart
+        //
+        *ValidBytes = 0;
+      }
+
+      break;
+
+    case 2:
+      //
+      // two-byte utf8 char go on
+      //
+      if ((Temp & 0xc0) == 0x80) {
+
+        Utf8Char->Utf8_2[0] = Temp;
+
+        FetchFlag           = FALSE;
+
+      } else {
+
+        *ValidBytes = 0;
+      }
+      break;
+
+    case 3:
+      //
+      // three-byte utf8 char go on
+      //
+      if ((Temp & 0xc0) == 0x80) {
+        if (Index == 1) {
+          Utf8Char->Utf8_3[1] = Temp;
+          Index++;
+        } else {
+          Utf8Char->Utf8_3[0] = Temp;
+          FetchFlag = FALSE;
+        }
+      } else {
+        //
+        // reset *ValidBytes and Index to zero, let valid utf8 char search restart
+        //
+        *ValidBytes = 0;
+        Index       = 0;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    if (!FetchFlag) {
+      break;
+    }
+  }
+
+  return ;
+}
+
+/**
+  Translate VT-UTF8 characters into one Unicode character.
+
+  UTF8 Encoding Table
+  Bits per Character | Unicode Character Range | Unicode Binary  Encoding |	UTF8 Binary Encoding
+        0-7	         |     0x0000 - 0x007F	   |     00000000 0xxxxxxx	  |   0xxxxxxx
+        8-11 	       |     0x0080 - 0x07FF	   |     00000xxx xxxxxxxx 	  |   110xxxxx 10xxxxxx
+       12-16	       |     0x0800 - 0xFFFF	   |     xxxxxxxx xxxxxxxx	  |   1110xxxx 10xxxxxx 10xxxxxx
+
+
+  @param  Utf8Char         VT-UTF8 character set needs translating.
+  @param  ValidBytes       The count of valid VT-UTF8 characters.
+  @param  UnicodeChar      Returned unicode character.
+
+**/
+VOID
+Utf8ToUnicode (
+  IN  UTF8_CHAR       Utf8Char,
+  IN  UINT8           ValidBytes,
+  OUT CHAR16          *UnicodeChar
+  )
+{
+  UINT8 UnicodeByte0;
+  UINT8 UnicodeByte1;
+  UINT8 Byte0;
+  UINT8 Byte1;
+  UINT8 Byte2;
+
+  *UnicodeChar = 0;
+
+  //
+  // translate utf8 code to unicode, in terminal standard,
+  // up to 3 bytes utf8 code is supported.
+  //
+  switch (ValidBytes) {
+  case 1:
+    //
+    // one-byte utf8 code
+    //
+    *UnicodeChar = (UINT16) Utf8Char.Utf8_1;
+    break;
+
+  case 2:
+    //
+    // two-byte utf8 code
+    //
+    Byte0         = Utf8Char.Utf8_2[0];
+    Byte1         = Utf8Char.Utf8_2[1];
+
+    UnicodeByte0  = (UINT8) ((Byte1 << 6) | (Byte0 & 0x3f));
+    UnicodeByte1  = (UINT8) ((Byte1 >> 2) & 0x07);
+    *UnicodeChar  = (UINT16) (UnicodeByte0 | (UnicodeByte1 << 8));
+    break;
+
+  case 3:
+    //
+    // three-byte utf8 code
+    //
+    Byte0         = Utf8Char.Utf8_3[0];
+    Byte1         = Utf8Char.Utf8_3[1];
+    Byte2         = Utf8Char.Utf8_3[2];
+
+    UnicodeByte0  = (UINT8) ((Byte1 << 6) | (Byte0 & 0x3f));
+    UnicodeByte1  = (UINT8) ((Byte2 << 4) | ((Byte1 >> 2) & 0x0f));
+    *UnicodeChar  = (UINT16) (UnicodeByte0 | (UnicodeByte1 << 8));
+
+  default:
+    break;
+  }
+
+  return ;
+}
+
+/**
+  Translate one Unicode character into VT-UTF8 characters.
+
+  UTF8 Encoding Table
+  Bits per Character | Unicode Character Range | Unicode Binary  Encoding |	UTF8 Binary Encoding
+        0-7	         |     0x0000 - 0x007F	   |     00000000 0xxxxxxx	  |   0xxxxxxx
+        8-11 	       |     0x0080 - 0x07FF	   |     00000xxx xxxxxxxx 	  |   110xxxxx 10xxxxxx
+       12-16	       |     0x0800 - 0xFFFF	   |     xxxxxxxx xxxxxxxx	  |   1110xxxx 10xxxxxx 10xxxxxx
+
+
+  @param  Unicode          Unicode character need translating.
+  @param  Utf8Char         Return VT-UTF8 character set.
+  @param  ValidBytes       The count of valid VT-UTF8 characters. If
+                           ValidBytes is zero, no valid VT-UTF8 returned.
+
+**/
+VOID
+UnicodeToUtf8 (
+  IN  CHAR16      Unicode,
+  OUT UTF8_CHAR   *Utf8Char,
+  OUT UINT8       *ValidBytes
+  )
+{
+  UINT8 UnicodeByte0;
+  UINT8 UnicodeByte1;
+  //
+  // translate unicode to utf8 code
+  //
+  UnicodeByte0  = (UINT8) Unicode;
+  UnicodeByte1  = (UINT8) (Unicode >> 8);
+
+  if (Unicode < 0x0080) {
+
+    Utf8Char->Utf8_1  = (UINT8) (UnicodeByte0 & 0x7f);
+    *ValidBytes       = 1;
+
+  } else if (Unicode < 0x0800) {
+    //
+    // byte sequence: high -> low
+    //                Utf8_2[0], Utf8_2[1]
+    //
+    Utf8Char->Utf8_2[1] = (UINT8) ((UnicodeByte0 & 0x3f) + 0x80);
+    Utf8Char->Utf8_2[0] = (UINT8) ((((UnicodeByte1 << 2) + (UnicodeByte0 >> 6)) & 0x1f) + 0xc0);
+
+    *ValidBytes         = 2;
+
+  } else {
+    //
+    // byte sequence: high -> low
+    //                Utf8_3[0], Utf8_3[1], Utf8_3[2]
+    //
+    Utf8Char->Utf8_3[2] = (UINT8) ((UnicodeByte0 & 0x3f) + 0x80);
+    Utf8Char->Utf8_3[1] = (UINT8) ((((UnicodeByte1 << 2) + (UnicodeByte0 >> 6)) & 0x3f) + 0x80);
+    Utf8Char->Utf8_3[0] = (UINT8) (((UnicodeByte1 >> 4) & 0x0f) + 0xe0);
+
+    *ValidBytes         = 3;
+  }
+}
+
+
+/**
+  Check if input string is valid VT-UTF8 string.
+
+  @param  TerminalDevice          The terminal device.
+  @param  WString                 The input string.
+
+  @retval EFI_SUCCESS             If all input characters are valid.
+
+**/
+EFI_STATUS
+VTUTF8TestString (
+  IN  TERMINAL_DEV    *TerminalDevice,
+  IN  CHAR16          *WString
+  )
+{
+  //
+  // to utf8, all kind of characters are supported.
+  //
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/ComponentName.c b/Core/MdeModulePkg/Universal/DebugPortDxe/ComponentName.c
new file mode 100644
index 0000000000..f8eee09b80
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/ComponentName.c
@@ -0,0 +1,182 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for DebugPort driver.
+
+Copyright (c) 2006 - 2011, 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 "DebugPort.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gDebugPortComponentName = {
+  DebugPortComponentNameGetDriverName,
+  DebugPortComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gDebugPortComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) DebugPortComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) DebugPortComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mDebugPortDriverNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *) L"DebugPort Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mDebugPortDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gDebugPortComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.c b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.c
new file mode 100644
index 0000000000..298b6b2da3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.c
@@ -0,0 +1,745 @@
+/** @file
+  Top level C file for debugport driver.  Contains initialization function.
+  This driver layers on top of SerialIo.
+  ALL CODE IN THE SERIALIO STACK MUST BE RE-ENTRANT AND CALLABLE FROM
+  INTERRUPT CONTEXT
+
+Copyright (c) 2006 - 2014, 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 "DebugPort.h"
+
+//
+// Globals
+//
+EFI_DRIVER_BINDING_PROTOCOL gDebugPortDriverBinding = {
+  DebugPortSupported,
+  DebugPortStart,
+  DebugPortStop,
+  DEBUGPORT_DRIVER_VERSION,
+  NULL,
+  NULL
+};
+
+DEBUGPORT_DEVICE mDebugPortDevice = {
+  DEBUGPORT_DEVICE_SIGNATURE,
+  (EFI_HANDLE) 0,
+  (EFI_HANDLE) 0,
+  (EFI_DEVICE_PATH_PROTOCOL *) NULL,
+  {
+    DebugPortReset,
+    DebugPortWrite,
+    DebugPortRead,
+    DebugPortPoll
+  },
+  (EFI_HANDLE) 0,
+  (EFI_SERIAL_IO_PROTOCOL *) NULL,
+  DEBUGPORT_UART_DEFAULT_BAUDRATE,
+  DEBUGPORT_UART_DEFAULT_FIFO_DEPTH,
+  DEBUGPORT_UART_DEFAULT_TIMEOUT,
+  (EFI_PARITY_TYPE) DEBUGPORT_UART_DEFAULT_PARITY,
+  DEBUGPORT_UART_DEFAULT_DATA_BITS,
+  (EFI_STOP_BITS_TYPE) DEBUGPORT_UART_DEFAULT_STOP_BITS
+};
+
+/**
+  Local worker function to obtain device path information from DebugPort variable.
+
+  Records requested settings in DebugPort device structure.
+
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+GetDebugPortVariable (
+  VOID
+  )
+{
+  UINTN                     DataSize;
+  EFI_DEVICE_PATH_PROTOCOL  *DebugPortVariable;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+
+  GetVariable2 (EFI_DEBUGPORT_VARIABLE_NAME, &gEfiDebugPortVariableGuid, (VOID **) &DebugPortVariable, &DataSize);
+  if (DebugPortVariable == NULL) {
+    return NULL;
+  }
+
+  DevicePath = DebugPortVariable;
+  while (!IsDevicePathEnd (DevicePath) && !IS_UART_DEVICEPATH (DevicePath)) {
+    DevicePath = NextDevicePathNode (DevicePath);
+  }
+
+  if (IsDevicePathEnd (DevicePath)) {
+    FreePool (DebugPortVariable);
+    return NULL;
+  } else {
+    CopyMem (
+      &mDebugPortDevice.BaudRate,
+      &((UART_DEVICE_PATH *) DevicePath)->BaudRate,
+      sizeof (((UART_DEVICE_PATH *) DevicePath)->BaudRate)
+      );
+    mDebugPortDevice.ReceiveFifoDepth = DEBUGPORT_UART_DEFAULT_FIFO_DEPTH;
+    mDebugPortDevice.Timeout          = DEBUGPORT_UART_DEFAULT_TIMEOUT;
+    CopyMem (
+      &mDebugPortDevice.Parity,
+      &((UART_DEVICE_PATH *) DevicePath)->Parity,
+      sizeof (((UART_DEVICE_PATH *) DevicePath)->Parity)
+      );
+    CopyMem (
+      &mDebugPortDevice.DataBits,
+      &((UART_DEVICE_PATH *) DevicePath)->DataBits,
+      sizeof (((UART_DEVICE_PATH *) DevicePath)->DataBits)
+      );
+    CopyMem (
+      &mDebugPortDevice.StopBits,
+      &((UART_DEVICE_PATH *) DevicePath)->StopBits,
+      sizeof (((UART_DEVICE_PATH *) DevicePath)->StopBits)
+      );
+    return DebugPortVariable;
+  }
+}
+
+/**
+  Debug Port Driver entry point.
+
+  Reads DebugPort variable to determine what device and settings to use as the
+  debug port.  Binds exclusively to SerialIo. Reverts to defaults if no variable
+  is found.
+
+  @param[in] ImageHandle       The firmware allocated handle for the EFI image.
+  @param[in] SystemTable       A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS          The entry point is executed successfully.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval other                Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDebugPortDriver (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  EFI_STATUS    Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gDebugPortDriverBinding,
+             ImageHandle,
+             &gDebugPortComponentName,
+             &gDebugPortComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Checks to see if there's not already a DebugPort interface somewhere.
+
+  If there's a DEBUGPORT variable, the device path must match exactly.  If there's
+  no DEBUGPORT variable, then device path is not checked and does not matter.
+  Checks to see that there's a serial io interface on the controller handle
+  that can be bound BY_DRIVER | EXCLUSIVE.
+  If all these tests succeed, then we return EFI_SUCCESS, else, EFI_UNSUPPORTED
+  or other error returned by OpenProtocol.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to test.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      Debug Port device is not supported.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval others               Some error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DebugPortVariable;
+  EFI_SERIAL_IO_PROTOCOL    *SerialIo;
+  EFI_DEBUGPORT_PROTOCOL    *DebugPortInterface;
+  EFI_HANDLE                TempHandle;
+
+  //
+  // Check to see that there's not a debugport protocol already published,
+  // since only one standard UART serial port could be supported by this driver.
+  //
+  if (gBS->LocateProtocol (&gEfiDebugPortProtocolGuid, NULL, (VOID **) &DebugPortInterface) != EFI_NOT_FOUND) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Read DebugPort variable to determine debug port selection and parameters
+  //
+  DebugPortVariable = GetDebugPortVariable ();
+
+  if (DebugPortVariable != NULL) {
+    //
+    // There's a DEBUGPORT variable, so do LocateDevicePath and check to see if
+    // the closest matching handle matches the controller handle, and if it does,
+    // check to see that the remaining device path has the DebugPort GUIDed messaging
+    // device path only.  Otherwise, it's a mismatch and EFI_UNSUPPORTED is returned.
+    //
+    DevicePath = DebugPortVariable;
+    Status = gBS->LocateDevicePath (
+                    &gEfiSerialIoProtocolGuid,
+                    &DevicePath,
+                    &TempHandle
+                    );
+
+    if (Status == EFI_SUCCESS && TempHandle != ControllerHandle) {
+      Status = EFI_UNSUPPORTED;
+    }
+
+    if (Status == EFI_SUCCESS &&
+        (DevicePath->Type != MESSAGING_DEVICE_PATH ||
+         DevicePath->SubType != MSG_VENDOR_DP ||
+         *((UINT16 *) DevicePath->Length) != sizeof (DEBUGPORT_DEVICE_PATH))) {
+
+      Status = EFI_UNSUPPORTED;
+    }
+
+    if (Status == EFI_SUCCESS && !CompareGuid (&gEfiDebugPortDevicePathGuid, (GUID *) (DevicePath + 1))) {
+      Status = EFI_UNSUPPORTED;
+    }
+
+    FreePool (DebugPortVariable);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSerialIoProtocolGuid,
+                  (VOID **) &SerialIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->CloseProtocol (
+                  ControllerHandle,
+                  &gEfiSerialIoProtocolGuid,
+                  This->DriverBindingHandle,
+                  ControllerHandle
+                  );
+
+  return Status;
+}
+
+/**
+  Binds exclusively to serial io on the controller handle, Produces DebugPort
+  protocol and DevicePath on new handle.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval others               Some error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  DEBUGPORT_DEVICE_PATH     DebugPortDP;
+  EFI_DEVICE_PATH_PROTOCOL  EndDP;
+  EFI_DEVICE_PATH_PROTOCOL  *Dp1;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSerialIoProtocolGuid,
+                  (VOID **) &mDebugPortDevice.SerialIoBinding,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  mDebugPortDevice.SerialIoDeviceHandle = ControllerHandle;
+
+  //
+  // Initialize the Serial Io interface...
+  //
+  Status = mDebugPortDevice.SerialIoBinding->SetAttributes (
+                                                mDebugPortDevice.SerialIoBinding,
+                                                mDebugPortDevice.BaudRate,
+                                                mDebugPortDevice.ReceiveFifoDepth,
+                                                mDebugPortDevice.Timeout,
+                                                mDebugPortDevice.Parity,
+                                                mDebugPortDevice.DataBits,
+                                                mDebugPortDevice.StopBits
+                                                );
+  if (EFI_ERROR (Status)) {
+    mDebugPortDevice.BaudRate          = 0;
+    mDebugPortDevice.Parity            = DefaultParity;
+    mDebugPortDevice.DataBits          = 0;
+    mDebugPortDevice.StopBits          = DefaultStopBits;
+    mDebugPortDevice.ReceiveFifoDepth  = 0;
+    Status = mDebugPortDevice.SerialIoBinding->SetAttributes (
+                                                  mDebugPortDevice.SerialIoBinding,
+                                                  mDebugPortDevice.BaudRate,
+                                                  mDebugPortDevice.ReceiveFifoDepth,
+                                                  mDebugPortDevice.Timeout,
+                                                  mDebugPortDevice.Parity,
+                                                  mDebugPortDevice.DataBits,
+                                                  mDebugPortDevice.StopBits
+                                                  );
+    if (EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+            ControllerHandle,
+            &gEfiSerialIoProtocolGuid,
+            This->DriverBindingHandle,
+            ControllerHandle
+            );
+      return Status;
+    }
+  }
+
+  mDebugPortDevice.SerialIoBinding->Reset (mDebugPortDevice.SerialIoBinding);
+
+  //
+  // Create device path instance for DebugPort
+  //
+  DebugPortDP.Header.Type     = MESSAGING_DEVICE_PATH;
+  DebugPortDP.Header.SubType  = MSG_VENDOR_DP;
+  SetDevicePathNodeLength (&(DebugPortDP.Header), sizeof (DebugPortDP));
+  CopyGuid (&DebugPortDP.Guid, &gEfiDebugPortDevicePathGuid);
+
+  Dp1 = DevicePathFromHandle (ControllerHandle);
+  if (Dp1 == NULL) {
+    Dp1 = &EndDP;
+    SetDevicePathEndNode (Dp1);
+  }
+
+  mDebugPortDevice.DebugPortDevicePath = AppendDevicePathNode (Dp1, (EFI_DEVICE_PATH_PROTOCOL *) &DebugPortDP);
+  if (mDebugPortDevice.DebugPortDevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Publish DebugPort and Device Path protocols
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mDebugPortDevice.DebugPortDeviceHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  mDebugPortDevice.DebugPortDevicePath,
+                  &gEfiDebugPortProtocolGuid,
+                  &mDebugPortDevice.DebugPortInterface,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiSerialIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    return Status;
+  }
+  //
+  // Connect debugport child to serial io
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSerialIoProtocolGuid,
+                  (VOID **) &mDebugPortDevice.SerialIoBinding,
+                  This->DriverBindingHandle,
+                  mDebugPortDevice.DebugPortDeviceHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiSerialIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Stop this driver on ControllerHandle by removing Serial IO protocol on
+  the ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     ControllerHandle,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS  Status;
+
+  if (NumberOfChildren == 0) {
+    //
+    // Close the bus driver
+    //
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiSerialIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    mDebugPortDevice.SerialIoBinding = NULL;
+
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    FreePool (mDebugPortDevice.DebugPortDevicePath);
+
+    return EFI_SUCCESS;
+  } else {
+    //
+    // Disconnect SerialIo child handle
+    //
+    Status = gBS->CloseProtocol (
+                    mDebugPortDevice.SerialIoDeviceHandle,
+                    &gEfiSerialIoProtocolGuid,
+                    This->DriverBindingHandle,
+                    mDebugPortDevice.DebugPortDeviceHandle
+                    );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Unpublish our protocols (DevicePath, DebugPort)
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    mDebugPortDevice.DebugPortDeviceHandle,
+                    &gEfiDevicePathProtocolGuid,
+                    mDebugPortDevice.DebugPortDevicePath,
+                    &gEfiDebugPortProtocolGuid,
+                    &mDebugPortDevice.DebugPortInterface,
+                    NULL
+                    );
+
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+            ControllerHandle,
+            &gEfiSerialIoProtocolGuid,
+            (VOID **) &mDebugPortDevice.SerialIoBinding,
+            This->DriverBindingHandle,
+            mDebugPortDevice.DebugPortDeviceHandle,
+            EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+            );
+    } else {
+      mDebugPortDevice.DebugPortDeviceHandle = NULL;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:GetControl to flush buffer.
+  We cannot call SerialIo:SetAttributes because it uses pool services, which use
+  locks, which affect TPL, so it's not interrupt context safe or re-entrant.
+  SerialIo:Reset() calls SetAttributes, so it can't be used either.
+
+  The port itself should be fine since it was set up during initialization.
+
+  @param  This              Protocol instance pointer.
+
+  @return EFI_SUCCESS       Always.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortReset (
+  IN EFI_DEBUGPORT_PROTOCOL   *This
+  )
+{
+  UINTN             BufferSize;
+  UINTN             BitBucket;
+
+  while (This->Poll (This) == EFI_SUCCESS) {
+    BufferSize = 1;
+    This->Read (This, 0, &BufferSize, &BitBucket);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Read() after setting
+  if it's different than the last SerialIo access.
+
+  @param  This                Pointer to DebugPort protocol.
+  @param  Timeout             Timeout value.
+  @param  BufferSize          On input, the size of Buffer.
+                              On output, the amount of data actually written.
+  @param  Buffer              Pointer to buffer to read.
+
+  @retval EFI_SUCCESS
+  @retval others
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortRead (
+  IN EFI_DEBUGPORT_PROTOCOL   *This,
+  IN UINT32                   Timeout,
+  IN OUT UINTN                *BufferSize,
+  IN VOID                     *Buffer
+  )
+{
+  DEBUGPORT_DEVICE  *DebugPortDevice;
+  UINTN             LocalBufferSize;
+  EFI_STATUS        Status;
+  UINT8             *BufferPtr;
+
+  DebugPortDevice = DEBUGPORT_DEVICE_FROM_THIS (This);
+  BufferPtr       = Buffer;
+  LocalBufferSize = *BufferSize;
+
+  do {
+    Status = DebugPortDevice->SerialIoBinding->Read (
+                                                DebugPortDevice->SerialIoBinding,
+                                                &LocalBufferSize,
+                                                BufferPtr
+                                                );
+    if (Status == EFI_TIMEOUT) {
+      if (Timeout > DEBUGPORT_UART_DEFAULT_TIMEOUT) {
+        Timeout -= DEBUGPORT_UART_DEFAULT_TIMEOUT;
+      } else {
+        Timeout = 0;
+      }
+    } else if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    BufferPtr += LocalBufferSize;
+    LocalBufferSize = *BufferSize - (BufferPtr - (UINT8 *) Buffer);
+  } while (LocalBufferSize != 0 && Timeout > 0);
+
+  *BufferSize = (UINTN) (BufferPtr - (UINT8 *) Buffer);
+
+  return Status;
+}
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Write() Writes 8 bytes at
+  a time and does a GetControl between 8 byte writes to help insure reads are
+  interspersed This is poor-man's flow control.
+
+  @param  This                Pointer to DebugPort protocol.
+  @param  Timeout             Timeout value.
+  @param  BufferSize          On input, the size of Buffer.
+                              On output, the amount of data actually written.
+  @param  Buffer              Pointer to buffer to read.
+
+  @retval EFI_SUCCESS         The data was written.
+  @retval others              Fails when writting datas to debug port device.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortWrite (
+  IN EFI_DEBUGPORT_PROTOCOL   *This,
+  IN UINT32                   Timeout,
+  IN OUT UINTN                *BufferSize,
+  OUT VOID                    *Buffer
+  )
+{
+  DEBUGPORT_DEVICE  *DebugPortDevice;
+  UINTN             Position;
+  UINTN             WriteSize;
+  EFI_STATUS        Status;
+  UINT32            SerialControl;
+
+  Status          = EFI_SUCCESS;
+  DebugPortDevice = DEBUGPORT_DEVICE_FROM_THIS (This);
+
+  WriteSize       = 8;
+  for (Position = 0; Position < *BufferSize && !EFI_ERROR (Status); Position += WriteSize) {
+    DebugPortDevice->SerialIoBinding->GetControl (
+                                        DebugPortDevice->SerialIoBinding,
+                                        &SerialControl
+                                        );
+    if (*BufferSize - Position < 8) {
+      WriteSize = *BufferSize - Position;
+    }
+
+    Status = DebugPortDevice->SerialIoBinding->Write (
+                                                DebugPortDevice->SerialIoBinding,
+                                                &WriteSize,
+                                                &((UINT8 *) Buffer)[Position]
+                                                );
+  }
+
+  *BufferSize = Position;
+  return Status;
+}
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Write() after setting
+  if it's different than the last SerialIo access.
+
+  @param  This                Pointer to DebugPort protocol.
+
+  @retval EFI_SUCCESS         At least 1 character is ready to be read from
+                              the DebugPort interface.
+  @retval EFI_NOT_READY       There are no characters ready to read from the
+                              DebugPort interface
+  @retval EFI_DEVICE_ERROR    A hardware failure occured... (from SerialIo)
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortPoll (
+  IN EFI_DEBUGPORT_PROTOCOL   *This
+  )
+{
+  EFI_STATUS        Status;
+  UINT32            SerialControl;
+  DEBUGPORT_DEVICE  *DebugPortDevice;
+
+  DebugPortDevice = DEBUGPORT_DEVICE_FROM_THIS (This);
+
+  Status = DebugPortDevice->SerialIoBinding->GetControl (
+                                              DebugPortDevice->SerialIoBinding,
+                                              &SerialControl
+                                              );
+
+  if (!EFI_ERROR (Status)) {
+    if ((SerialControl & EFI_SERIAL_INPUT_BUFFER_EMPTY) != 0) {
+      Status = EFI_NOT_READY;
+    } else {
+      Status = EFI_SUCCESS;
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Unload function that is registered in the LoadImage protocol.  It un-installs
+  protocols produced and deallocates pool used by the driver.  Called by the core
+  when unloading the driver.
+
+  @param  ImageHandle
+
+  @retval EFI_SUCCESS     Unload Debug Port driver successfully.
+  @retval EFI_ABORTED     Serial IO is still binding.
+
+**/
+EFI_STATUS
+EFIAPI
+ImageUnloadHandler (
+  EFI_HANDLE ImageHandle
+  )
+{
+  EFI_STATUS  Status;
+  VOID        *ComponentName;
+  VOID        *ComponentName2;
+
+  if (mDebugPortDevice.SerialIoBinding != NULL) {
+    return EFI_ABORTED;
+  }
+
+  //
+  // Driver is stopped already.
+  //
+  Status = gBS->HandleProtocol (ImageHandle, &gEfiComponentNameProtocolGuid, &ComponentName);
+  if (EFI_ERROR (Status)) {
+    ComponentName = NULL;
+  }
+
+  Status = gBS->HandleProtocol (ImageHandle, &gEfiComponentName2ProtocolGuid, &ComponentName2);
+  if (EFI_ERROR (Status)) {
+    ComponentName2 = NULL;
+  }
+
+  if (ComponentName == NULL) {
+    if (ComponentName2 == NULL) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ImageHandle,
+                      &gEfiDriverBindingProtocolGuid,  &gDebugPortDriverBinding,
+                      NULL
+                      );
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ImageHandle,
+                      &gEfiDriverBindingProtocolGuid,  &gDebugPortDriverBinding,
+                      &gEfiComponentName2ProtocolGuid, ComponentName2,
+                      NULL
+                      );
+    }
+  } else {
+    if (ComponentName2 == NULL) {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ImageHandle,
+                      &gEfiDriverBindingProtocolGuid,  &gDebugPortDriverBinding,
+                      &gEfiComponentNameProtocolGuid,  ComponentName,
+                      NULL
+                      );
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      ImageHandle,
+                      &gEfiDriverBindingProtocolGuid,  &gDebugPortDriverBinding,
+                      &gEfiComponentNameProtocolGuid,  ComponentName,
+                      &gEfiComponentName2ProtocolGuid, ComponentName2,
+                      NULL
+                      );
+    }
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.h b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.h
new file mode 100644
index 0000000000..3ce702dfa4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPort.h
@@ -0,0 +1,396 @@
+/** @file
+  Definitions and prototypes for DebugPort driver.
+
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef __DEBUGPORT_H__
+#define __DEBUGPORT_H__
+
+
+#include <Uefi.h>
+
+#include <Protocol/DevicePath.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/SerialIo.h>
+#include <Protocol/DebugPort.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+
+//
+// Driver Binding Externs
+//
+extern EFI_DRIVER_BINDING_PROTOCOL  gDebugPortDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL  gDebugPortComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gDebugPortComponentName2;
+
+//
+// local type definitions
+//
+#define DEBUGPORT_DEVICE_SIGNATURE  SIGNATURE_32 ('D', 'B', 'G', 'P')
+
+//
+// Device structure used by driver
+//
+typedef struct {
+  UINT32                      Signature;
+  EFI_HANDLE                  DriverBindingHandle;
+  EFI_HANDLE                  DebugPortDeviceHandle;
+
+  EFI_DEVICE_PATH_PROTOCOL    *DebugPortDevicePath;
+  EFI_DEBUGPORT_PROTOCOL      DebugPortInterface;
+
+  EFI_HANDLE                  SerialIoDeviceHandle;
+  EFI_SERIAL_IO_PROTOCOL      *SerialIoBinding;
+  UINT64                      BaudRate;
+  UINT32                      ReceiveFifoDepth;
+  UINT32                      Timeout;
+  EFI_PARITY_TYPE             Parity;
+  UINT8                       DataBits;
+  EFI_STOP_BITS_TYPE          StopBits;
+} DEBUGPORT_DEVICE;
+
+#define DEBUGPORT_DEVICE_FROM_THIS(a)     CR (a, DEBUGPORT_DEVICE, DebugPortInterface, DEBUGPORT_DEVICE_SIGNATURE)
+
+#define EFI_ACPI_PC_COMPORT_HID           EISA_PNP_ID (0x0500)
+#define EFI_ACPI_16550UART_HID            EISA_PNP_ID (0x0501)
+
+#define DEBUGPORT_UART_DEFAULT_BAUDRATE   115200
+#define DEBUGPORT_UART_DEFAULT_PARITY     0
+#define DEBUGPORT_UART_DEFAULT_FIFO_DEPTH 16
+#define DEBUGPORT_UART_DEFAULT_TIMEOUT    50000 ///< 5 ms
+#define DEBUGPORT_UART_DEFAULT_DATA_BITS  8
+#define DEBUGPORT_UART_DEFAULT_STOP_BITS  1
+
+#define DEBUGPORT_DRIVER_VERSION          1
+
+#define IS_UART_DEVICEPATH(dp)           (DevicePathType (dp) == MESSAGING_DEVICE_PATH && DevicePathSubType (dp) == MSG_UART_DP)
+
+/**
+  Debug Port Driver entry point. 
+
+  Reads DebugPort variable to determine what device and settings to use as the
+  debug port.  Binds exclusively to SerialIo. Reverts to defaults if no variable
+  is found.
+
+  @param[in] ImageHandle       The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS          The entry point is executed successfully.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval other                Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDebugPortDriver (
+  IN EFI_HANDLE                     ImageHandle,
+  IN EFI_SYSTEM_TABLE               *SystemTable
+  );
+
+/**
+  Checks to see if there's not already a DebugPort interface somewhere. 
+
+  If there's a DEBUGPORT variable, the device path must match exactly.  If there's
+  no DEBUGPORT variable, then device path is not checked and does not matter.
+  Checks to see that there's a serial io interface on the controller handle
+  that can be bound BY_DRIVER | EXCLUSIVE.
+  If all these tests succeed, then we return EFI_SUCCESS, else, EFI_UNSUPPORTED
+  or other error returned by OpenProtocol.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to test.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver supports this device.
+  @retval EFI_UNSUPPORTED      Debug Port device is not supported.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval others               Some error occurs.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Binds exclusively to serial io on the controller handle, Produces DebugPort
+  protocol and DevicePath on new handle.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES Fails to allocate memory for device.
+  @retval others               Some error occurs.                
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle by removing Serial IO protocol on
+  the ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle.
+  @retval other             This driver was not removed from this device.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:GetControl to flush buffer.
+  We cannot call SerialIo:SetAttributes because it uses pool services, which use
+  locks, which affect TPL, so it's not interrupt context safe or re-entrant.
+  SerialIo:Reset() calls SetAttributes, so it can't be used either.
+
+  The port itself should be fine since it was set up during initialization.
+
+  @param  This              Protocol instance pointer.   
+
+  @return EFI_SUCCESS       Always.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortReset (
+  IN EFI_DEBUGPORT_PROTOCOL         *This
+  );
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Read() after setting
+  if it's different than the last SerialIo access.
+
+  @param  This                Pointer to DebugPort protocol.
+  @param  Timeout             Timeout value.
+  @param  BufferSize          On input, the size of Buffer.
+                              On output, the amount of data actually written.
+  @param  Buffer              Pointer to buffer to read.
+
+  @retval EFI_SUCCESS         
+  @retval others              
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortRead (
+  IN EFI_DEBUGPORT_PROTOCOL         *This,
+  IN UINT32                         Timeout,
+  IN OUT UINTN                      *BufferSize,
+  IN VOID                           *Buffer
+  );
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Write() Writes 8 bytes at
+  a time and does a GetControl between 8 byte writes to help insure reads are
+  interspersed This is poor-man's flow control.
+
+  @param  This                Pointer to DebugPort protocol.
+  @param  Timeout             Timeout value.
+  @param  BufferSize          On input, the size of Buffer.
+                              On output, the amount of data actually written.
+  @param  Buffer              Pointer to buffer to read.
+
+  @retval EFI_SUCCESS         The data was written.
+  @retval others              Fails when writting datas to debug port device.
+
+**/
+EFI_STATUS
+EFIAPI
+DebugPortWrite (
+  IN EFI_DEBUGPORT_PROTOCOL         *This,
+  IN UINT32                         Timeout,
+  IN OUT UINTN                      *BufferSize,
+  OUT VOID                          *Buffer
+  );
+
+/**
+  DebugPort protocol member function.  Calls SerialIo:Write() after setting
+  if it's different than the last SerialIo access.
+
+  @param  This                Pointer to DebugPort protocol.
+
+  @retval EFI_SUCCESS         At least 1 character is ready to be read from
+                              the DebugPort interface.
+  @retval EFI_NOT_READY       There are no characters ready to read from the
+                              DebugPort interface
+  @retval EFI_DEVICE_ERROR    A hardware failure occured... (from SerialIo)
+
+**/ 
+EFI_STATUS
+EFIAPI
+DebugPortPoll (
+  IN EFI_DEBUGPORT_PROTOCOL         *This
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.inf b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.inf
new file mode 100644
index 0000000000..b727cda3cf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.inf
@@ -0,0 +1,71 @@
+## @file
+# This driver produces Debug Port protocol to be used by debug agent to communicate with the remote debug host.
+#
+# This driver binds exclusively to a standard UART serial port on the controller handle,
+# and initializes serial Io interface, publishs Debug Port and Device Path Protocol.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DebugPortDxe
+  MODULE_UNI_FILE                = DebugPortDxe.uni
+  FILE_GUID                      = 73E9457A-CEA1-4917-9A9C-9F1F0F0FD322
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeDebugPortDriver
+  UNLOAD_IMAGE                   = ImageUnloadHandler
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gDebugPortDriverBinding
+#  COMPONENT_NAME                =  gDebugPortComponentName
+#  COMPONENT_NAME2               =  gDebugPortComponentName2
+#  Variable Guid C Name: gEfiDebugPortProtocolGuid Variable Name: L"DEBUGPORT"
+#
+#
+
+[Sources]
+  ComponentName.c
+  DebugPort.c
+  DebugPort.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  DevicePathLib
+  UefiRuntimeServicesTableLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Guids]
+  gEfiDebugPortVariableGuid                     ## SOMETIMES_CONSUMES   ## Variable:L"DEBUGPORT"
+  gEfiDebugPortDevicePathGuid                   ## SOMETIMES_CONSUMES   ## UNDEFINED # Device path
+
+[Protocols]
+  gEfiSerialIoProtocolGuid                      ## TO_START
+  gEfiDevicePathProtocolGuid                    ## BY_START
+  gEfiDebugPortProtocolGuid                     ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DebugPortDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.uni b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.uni
new file mode 100644
index 0000000000..e263649452
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxe.uni
diff --git a/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxeExtra.uni b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxeExtra.uni
new file mode 100644
index 0000000000..597a2cf137
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugPortDxe/DebugPortDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupport.c b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupport.c
new file mode 100644
index 0000000000..9a8f86de10
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupport.c
@@ -0,0 +1,133 @@
+/** @file
+  Top level C file for debug support driver.  Contains initialization function.
+
+Copyright (c) 2006 - 2009, 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 "PlDebugSupport.h"
+
+EFI_DEBUG_SUPPORT_PROTOCOL  mDebugSupportProtocolInterface = {
+  EFI_ISA,
+  GetMaximumProcessorIndex,
+  RegisterPeriodicCallback,
+  RegisterExceptionCallback,
+  InvalidateInstructionCache
+};
+
+
+/**
+  Debug Support Driver entry point. 
+
+  Checks to see if there's not already a Debug Support protocol installed for 
+  the selected processor before installing it.
+
+  @param[in] ImageHandle       The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS          The entry point is executed successfully.
+  @retval EFI_ALREADY_STARTED  Debug Support protocol is installed already.
+  @retval other                Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDebugSupportDriver (
+  IN EFI_HANDLE               ImageHandle,
+  IN EFI_SYSTEM_TABLE         *SystemTable
+  )
+{
+  EFI_LOADED_IMAGE_PROTOCOL   *LoadedImageProtocolPtr;
+  EFI_STATUS                  Status;
+  EFI_HANDLE                  Handle;
+  EFI_HANDLE                  *HandlePtr;
+  UINTN                       NumHandles;
+  EFI_DEBUG_SUPPORT_PROTOCOL  *DebugSupportProtocolPtr;
+
+  //
+  // First check to see that the debug support protocol for this processor
+  // type is not already installed
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDebugSupportProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &HandlePtr
+                  );
+
+  if (Status != EFI_NOT_FOUND) {
+    do {
+      NumHandles--;
+      Status = gBS->OpenProtocol (
+                      HandlePtr[NumHandles],
+                      &gEfiDebugSupportProtocolGuid,
+                      (VOID **) &DebugSupportProtocolPtr,
+                      ImageHandle,
+                      NULL,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if ((Status == EFI_SUCCESS) && (DebugSupportProtocolPtr->Isa == EFI_ISA)) {
+        //
+        // a Debug Support protocol has been installed for this processor
+        //
+        FreePool (HandlePtr);
+        Status = EFI_ALREADY_STARTED;
+        goto ErrExit;
+      }
+    } while (NumHandles > 0);
+    FreePool (HandlePtr);
+  }
+
+  //
+  // Get our image information and install platform specific unload handler
+  //
+  Status = gBS->OpenProtocol (
+                  ImageHandle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID **) &LoadedImageProtocolPtr,
+                  ImageHandle,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  ASSERT (!EFI_ERROR (Status));
+  if (Status != EFI_SUCCESS) {
+    goto ErrExit;
+  }
+
+  LoadedImageProtocolPtr->Unload = PlUnloadDebugSupportDriver;
+
+  //
+  // Call hook for processor specific initialization 
+  //
+  Status = PlInitializeDebugSupportDriver ();
+  ASSERT (!EFI_ERROR (Status));
+  if (Status != EFI_SUCCESS) {
+    goto ErrExit;
+  }
+
+  //
+  // Install Debug Support protocol to new handle
+  //
+  Handle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &Handle,
+                  &gEfiDebugSupportProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mDebugSupportProtocolInterface
+                  );
+  ASSERT (!EFI_ERROR (Status));
+  if (Status != EFI_SUCCESS) {
+    goto ErrExit;
+  }
+
+ErrExit:
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.inf b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.inf
new file mode 100644
index 0000000000..f1faa3d2c9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.inf
@@ -0,0 +1,87 @@
+## @file
+# This driver installs Debug Support protocol for the selected processor.
+#
+# This driver provides the capabilities for debug-agent to gain control of the machine
+# when certain types of events occur, i.e. breakpoint, processor execptions, etc. It also
+# provides debug-agent to periodically gain control during operation of the machine to
+# check for asynchronous commands form the host.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DebugSupportDxe
+  MODULE_UNI_FILE                = DebugSupportDxe.uni
+  FILE_GUID                      = 911D584C-35F7-4955-BEF9-B452769DDC3A
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeDebugSupportDriver
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF
+#
+
+[Sources]
+  DebugSupport.c
+
+[Sources.Ia32]
+  Ia32/DebugSupport.h
+  Ia32/PlDebugSupport.c
+  Ia32/PlDebugSupport.h
+  Ia32/PlDebugSupportIa32.c
+  Ia32/AsmFuncs.S
+  Ia32/AsmFuncs.asm
+
+[Sources.X64]
+  Ia32/DebugSupport.h
+  Ia32/PlDebugSupport.c
+  X64/PlDebugSupport.h
+  X64/PlDebugSupportX64.c
+  X64/AsmFuncs.S
+  X64/AsmFuncs.asm
+
+[Sources.IPF]
+  Ipf/PlDebugSupport.h
+  Ipf/PlDebugSupport.c
+  Ipf/Ds64Macros.i
+  Ipf/Common.i
+  Ipf/AsmFuncs.s
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[LibraryClasses.IA32, LibraryClasses.X64]
+  BaseLib
+
+[Protocols]
+  gEfiLoadedImageProtocolGuid                   ## CONSUMES
+  gEfiDebugSupportProtocolGuid                  ## PRODUCES
+
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DebugSupportDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.uni b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.uni
new file mode 100644
index 0000000000..be3ca60bfb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxe.uni
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxeExtra.uni b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxeExtra.uni
new file mode 100644
index 0000000000..5e49ee0140
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/DebugSupportDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.S b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.S
new file mode 100644
index 0000000000..fd1a96b76f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.S
@@ -0,0 +1,407 @@
+#/**@file
+# Low leve IA32 specific debug support functions.
+#
+# Copyright (c) 2006 - 2011, 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.
+#
+#**/
+
+ASM_GLOBAL ASM_PFX(OrigVector)
+ASM_GLOBAL ASM_PFX(InterruptEntryStub)
+ASM_GLOBAL ASM_PFX(StubSize)
+ASM_GLOBAL ASM_PFX(CommonIdtEntry)
+ASM_GLOBAL ASM_PFX(FxStorSupport)
+
+ASM_PFX(StubSize):       .long   ASM_PFX(InterruptEntryStubEnd) - ASM_PFX(InterruptEntryStub)
+ASM_PFX(AppEsp):         .long   0x11111111 # ?
+ASM_PFX(DebugEsp):       .long   0x22222222 # ?
+ASM_PFX(ExtraPush):      .long   0x33333333 # ?
+ASM_PFX(ExceptData):     .long   0x44444444 # ?
+ASM_PFX(Eflags):         .long   0x55555555 # ?
+ASM_PFX(OrigVector):     .long   0x66666666 # ?
+
+#------------------------------------------------------------------------------
+# BOOLEAN
+# FxStorSupport (
+#   void
+#   )
+#
+# Abstract: Returns TRUE if FxStor instructions are supported
+#
+ASM_GLOBAL ASM_PFX(FxStorSupport)
+ASM_PFX(FxStorSupport):
+#
+# cpuid corrupts ebx which must be preserved per the C calling convention
+#
+        push   %ebx
+        mov    $0x1,%eax
+        cpuid
+        mov    %edx,%eax
+        and    $0x1000000,%eax
+        shr    $0x18,%eax
+        pop    %ebx
+        ret
+#------------------------------------------------------------------------------
+# void
+# Vect2Desc (
+#   DESCRIPTOR * DestDesc,
+#   void (*Vector) (void)
+#   )
+#
+# Abstract: Encodes an IDT descriptor with the given physical address
+#
+
+ASM_GLOBAL ASM_PFX(Vect2Desc)
+ASM_PFX(Vect2Desc):
+        push   %ebp
+        mov    %esp,%ebp
+        mov    0xc(%ebp),%eax
+        mov    0x8(%ebp),%ecx
+        mov    %ax,(%ecx)
+        movw   $0x20,0x2(%ecx)
+        movw   $0x8e00,0x4(%ecx)
+        shr    $0x10,%eax
+        mov    %ax,0x6(%ecx)
+        leave
+        ret
+
+ASM_GLOBAL ASM_PFX(InterruptEntryStub)
+ASM_PFX(InterruptEntryStub):
+        mov    %esp,0x0                    # save stack top
+        mov    $0x0,%esp                   # switch to debugger stack
+        push   $0x0                        # push vector number - will be modified before installed
+        jmp    ASM_PFX(CommonIdtEntry)     # jump CommonIdtEntry
+ASM_GLOBAL ASM_PFX(InterruptEntryStubEnd)
+ASM_PFX(InterruptEntryStubEnd):
+
+#------------------------------------------------------------------------------
+# CommonIdtEntry
+#
+# Abstract: This code is not a function, but is the common part for all IDT
+#               vectors.
+#
+ASM_GLOBAL ASM_PFX(CommonIdtEntry)
+ASM_PFX(CommonIdtEntry):
+##
+## At this point, the stub has saved the current application stack esp into AppEsp
+## and switched stacks to the debug stack, where it pushed the vector number
+##
+## The application stack looks like this:
+##
+##              ...
+##              (last application stack entry)
+##              eflags from interrupted task
+##              CS from interrupted task
+##              EIP from interrupted task
+##              Error code <-------------------- Only present for some exeption types
+##
+##
+
+
+## The stub switched us to the debug stack and pushed the interrupt number.
+##
+## Next, construct the context record.  It will be build on the debug stack by
+## pushing the registers in the correct order so as to create the context structure
+## on the debug stack.  The context record must be built from the end back to the
+## beginning because the stack grows down...
+#
+## For reference, the context record looks like this:
+##
+## typedef
+## struct {
+##   UINT32             ExceptionData;
+##   FX_SAVE_STATE_IA32 FxSaveState;
+##   UINT32             Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+##   UINT32             Cr0, Cr2, Cr3, Cr4;
+##   UINT32             EFlags;
+##   UINT32             Ldtr, Tr;
+##   UINT32             Gdtr[2], Idtr[2];
+##   UINT32             Eip;
+##   UINT32             Gs, Fs, Es, Ds, Cs, Ss;
+##   UINT32             Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+## } SYSTEM_CONTEXT_IA32;  // 32 bit system context record
+
+## UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+        pusha
+## Save interrupt state eflags register...
+        pushf
+        pop    %eax
+## We need to determine if any extra data was pushed by the exception, and if so, save it
+## To do this, we check the exception number pushed by the stub, and cache the
+## result in a variable since we'll need this again.
+        mov    %eax,0x0
+        cmpl   $0x8,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x20)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0xa,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x35)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0xb,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x4a)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0xc,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x5f)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0xd,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x74)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0xe,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x89)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        cmpl   $0x11,0x0
+        jne    ASM_PFX(CommonIdtEntry+0x9e)
+        movl   $0x1,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xa8)
+        movl   $0x0,0x0
+## If there's some extra data, save it also, and modify the saved AppEsp to effectively
+## pop this value off the application's stack.
+
+        cmpl   $0x1,0x0
+        jne    ASM_PFX(CommonIdtEntry+0xc8)
+        mov    0x0,%eax
+        mov    (%eax),%ebx
+        mov    %ebx,0x0
+        add    $0x4,%eax
+        mov    %eax,0x0
+        jmp    ASM_PFX(CommonIdtEntry+0xd2)
+        movl   $0x0,0x0
+## The "pushad" above pushed the debug stack esp.  Since what we're actually doing
+## is building the context record on the debug stack, we need to save the pushed
+## debug ESP, and replace it with the application's last stack entry...
+        mov    0xc(%esp),%eax
+        mov    %eax,0x0
+        mov    0x0,%eax
+        add    $0xc,%eax
+        # application stack has eflags, cs, & eip, so
+        # last actual application stack entry is
+        # 12 bytes into the application stack.
+        mov    %eax,0xc(%esp)
+## continue building context record
+## UINT32  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+        mov    %ss,%eax
+        push   %eax
+        
+        # CS from application is one entry back in application stack
+        mov    0x0,%eax
+        movzwl 0x4(%eax),%eax
+        push   %eax
+        mov    %ds,%eax
+        push   %eax
+        mov    %es,%eax
+        push   %eax
+        mov    %fs,%eax
+        push   %eax
+        mov    %gs,%eax
+        push   %eax
+
+## UINT32  Eip;
+        # Eip from application is on top of application stack
+        mov    0x0,%eax
+        pushl  (%eax)
+
+## UINT32  Gdtr[2], Idtr[2];
+        push   $0x0
+        push   $0x0
+        sidtl  (%esp)
+        push   $0x0
+        push   $0x0
+        sgdtl  (%esp)
+
+## UINT32  Ldtr, Tr;
+        xor    %eax,%eax
+        str    %eax
+        push   %eax
+        sldt   %eax
+        push   %eax
+
+## UINT32  EFlags;
+## Eflags from application is two entries back in application stack
+        mov    0x0,%eax
+        pushl  0x8(%eax)
+
+## UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+## insure FXSAVE/FXRSTOR is enabled in CR4...
+## ... while we're at it, make sure DE is also enabled...
+        mov    %cr4,%eax
+        or     $0x208,%eax
+        mov    %eax,%cr4
+        push   %eax
+        mov    %cr3,%eax
+        push   %eax
+        mov    %cr2,%eax
+        push   %eax
+        push   $0x0
+        mov    %cr0,%eax
+        push   %eax
+
+## UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+        mov    %db7,%eax
+        push   %eax
+
+## clear Dr7 while executing debugger itself
+        xor    %eax,%eax
+        mov    %eax,%db7
+        mov    %db6,%eax
+        push   %eax
+
+## insure all status bits in dr6 are clear...
+        xor    %eax,%eax
+        mov    %eax,%db6
+        mov    %db3,%eax
+        push   %eax
+        mov    %db2,%eax
+        push   %eax
+        mov    %db1,%eax
+        push   %eax
+        mov    %db0,%eax
+        push   %eax
+
+## FX_SAVE_STATE_IA32 FxSaveState;
+        sub    $0x200,%esp
+        mov    %esp,%edi
+        # IMPORTANT!! The debug stack has been carefully constructed to
+        # insure that esp and edi are 16 byte aligned when we get here.
+        # They MUST be.  If they are not, a GP fault will occur.
+        fxsave (%edi)
+
+## UEFI calling convention for IA32 requires that Direction flag in EFLAGs is clear
+        cld
+                
+## UINT32  ExceptionData;
+        mov    0x0,%eax
+        push   %eax
+
+# call to C code which will in turn call registered handler
+# pass in the vector number
+        mov    %esp,%eax
+        push   %eax
+        mov    0x0,%eax
+        push   %eax
+        call   ASM_PFX(CommonIdtEntry+0x184)
+        add    $0x8,%esp
+
+# restore context...
+## UINT32  ExceptionData;
+        add    $0x4,%esp
+
+## FX_SAVE_STATE_IA32 FxSaveState;
+        mov    %esp,%esi
+        fxrstor (%esi)
+        add    $0x200,%esp
+
+## UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+        pop    %eax
+        mov    %eax,%db0
+        pop    %eax
+        mov    %eax,%db1
+        pop    %eax
+        mov    %eax,%db2
+        pop    %eax
+        mov    %eax,%db3
+
+## skip restore of dr6.  We cleared dr6 during the context save.
+        add    $0x4,%esp
+        pop    %eax
+        mov    %eax,%db7
+
+## UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+        pop    %eax
+        mov    %eax,%cr0
+        add    $0x4,%esp
+        pop    %eax
+        mov    %eax,%cr2
+        pop    %eax
+        mov    %eax,%cr3
+        pop    %eax
+        mov    %eax,%cr4
+
+## UINT32  EFlags;
+        mov    0x0,%eax
+        popl   0x8(%eax)
+
+## UINT32  Ldtr, Tr;
+## UINT32  Gdtr[2], Idtr[2];
+## Best not let anyone mess with these particular registers...
+        add    $0x18,%esp
+
+## UINT32  Eip;
+        popl   (%eax)
+
+## UINT32  SegGs, SegFs, SegEs, SegDs, SegCs, SegSs;
+## NOTE - modified segment registers could hang the debugger...  We
+##        could attempt to insulate ourselves against this possibility,
+##        but that poses risks as well.
+##
+
+        pop    %gs
+        pop    %fs
+        pop    %es
+        pop    %ds
+        popl   0x4(%eax)
+        pop    %ss
+        mov    0xc(%esp),%ebx
+
+## The next stuff to restore is the general purpose registers that were pushed
+## using the "pushad" instruction.
+##
+## The value of ESP as stored in the context record is the application ESP
+## including the 3 entries on the application stack caused by the exception
+## itself. It may have been modified by the debug agent, so we need to
+## determine if we need to relocate the application stack.
+
+        mov    0x0,%eax          # move the potentially modified AppEsp into ebx
+        add    $0xc,%eax
+        cmp    %eax,%ebx
+        je     ASM_PFX(CommonIdtEntry+0x202)
+        mov    0x0,%eax
+        mov    (%eax),%ecx       # EIP
+        mov    %ecx,(%ebx)
+        mov    0x4(%eax),%ecx    # CS
+        mov    %ecx,0x4(%ebx)
+        mov    0x8(%eax),%ecx    # EFLAGS
+        mov    %ecx,0x8(%ebx)
+	
+        mov    %ebx,%eax         # modify the saved AppEsp to the new AppEsp
+        mov    %eax,0x0
+        mov    0x0,%eax          # restore the DebugEsp on the debug stack
+	                               # so our "popad" will not cause a stack switch
+        mov    %eax,0xc(%esp)    
+        cmpl   $0x68,0x0
+        jne    PhonyIretd+0xd
+## Restore eflags so when we chain, the flags will be exactly as if we were never here.
+## We gin up the stack to do an iretd so we can get ALL the flags.
+        mov    0x0,%eax
+        mov    0x8(%eax),%ebx
+        and    $0xfffffcff,%ebx  # special handling for IF and TF
+        push   %ebx
+        push   %cs
+        push   $0x0
+        iret
+
+PhonyIretd:
+## UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+        popa
+
+## Switch back to application stack
+        mov    0x0,%esp
+        jmp    *0x0
+## Jump to original handler
+## UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+        popa
+## Switch back to application stack
+        mov    0x0,%esp
+
+## We're outa here...
+        iret
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.asm b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.asm
new file mode 100644
index 0000000000..32cbc31654
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/AsmFuncs.asm
@@ -0,0 +1,509 @@
+;/** @file
+;  Low leve IA32 specific debug support functions.
+;
+;  Copyright (c) 2006 - 2011, 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.
+;
+;**/
+
+.586p
+.MODEL          FLAT, C
+
+EXCPT32_DIVIDE_ERROR     EQU    0
+EXCPT32_DEBUG            EQU    1
+EXCPT32_NMI              EQU    2
+EXCPT32_BREAKPOINT       EQU    3
+EXCPT32_OVERFLOW         EQU    4
+EXCPT32_BOUND            EQU    5
+EXCPT32_INVALID_OPCODE   EQU    6
+EXCPT32_DOUBLE_FAULT     EQU    8
+EXCPT32_INVALID_TSS      EQU   10
+EXCPT32_SEG_NOT_PRESENT  EQU   11
+EXCPT32_STACK_FAULT      EQU   12
+EXCPT32_GP_FAULT         EQU   13
+EXCPT32_PAGE_FAULT       EQU   14
+EXCPT32_FP_ERROR         EQU   16
+EXCPT32_ALIGNMENT_CHECK  EQU   17
+EXCPT32_MACHINE_CHECK    EQU   18
+EXCPT32_SIMD             EQU   19
+
+FXSTOR_FLAG              EQU   01000000h         ; bit cpuid 24 of feature flags
+
+;; The FXSTOR and FXRSTOR commands are used for saving and restoring the x87,
+;; MMX, SSE, SSE2, etc registers.  The initialization of the debugsupport driver
+;; MUST check the CPUID feature flags to see that these instructions are available
+;; and fail to init if they are not.
+
+;; fxstor [edi]
+FXSTOR_EDI               MACRO
+                         db 0fh, 0aeh, 00000111y ; mod = 00, reg/op = 000, r/m = 111 = [edi]
+ENDM
+
+;; fxrstor [esi]
+FXRSTOR_ESI              MACRO
+                         db 0fh, 0aeh, 00001110y ; mod = 00, reg/op = 001, r/m = 110 = [esi]
+ENDM
+.DATA
+
+public          OrigVector, InterruptEntryStub, StubSize, CommonIdtEntry, FxStorSupport
+
+StubSize        dd      InterruptEntryStubEnd - InterruptEntryStub
+AppEsp          dd      11111111h ; ?
+DebugEsp        dd      22222222h ; ?
+ExtraPush       dd      33333333h ; ?
+ExceptData      dd      44444444h ; ?
+Eflags          dd      55555555h ; ?
+OrigVector      dd      66666666h ; ?
+
+;; The declarations below define the memory region that will be used for the debug stack.
+;; The context record will be built by pushing register values onto this stack.
+;; It is imparitive that alignment be carefully managed, since the FXSTOR and
+;; FXRSTOR instructions will GP fault if their memory operand is not 16 byte aligned.
+;;
+;; The stub will switch stacks from the application stack to the debuger stack
+;; and pushes the exception number.
+;;
+;; Then we building the context record on the stack. Since the stack grows down,
+;; we push the fields of the context record from the back to the front.  There
+;; are 132 bytes of stack used prior allocating the 512 bytes of stack to be
+;; used as the memory buffer for the fxstor instruction. Therefore address of
+;; the buffer used for the FXSTOR instruction is &Eax - 132 - 512, which
+;; must be 16 byte aligned.
+;;
+;; We carefully locate the stack to make this happen.
+;;
+;; For reference, the context structure looks like this:
+;;      struct {
+;;        UINT32             ExceptionData;
+;;        FX_SAVE_STATE_IA32 FxSaveState;    // 512 bytes, must be 16 byte aligned
+;;        UINT32             Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+;;        UINT32             Cr0, Cr1, Cr2, Cr3, Cr4;
+;;        UINT32             EFlags;
+;;        UINT32             Ldtr, Tr;
+;;        UINT32             Gdtr[2], Idtr[2];
+;;        UINT32             Eip;
+;;        UINT32             Gs, Fs, Es, Ds, Cs, Ss;
+;;        UINT32             Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+;;      } SYSTEM_CONTEXT_IA32;  // 32 bit system context record
+
+
+align           16
+DebugStackEnd   db      "DbgStkEnd >>>>>>"      ;; 16 byte long string - must be 16 bytes to preserve alignment
+                dd      1ffdh dup (000000000h)  ;; 32K should be enough stack
+                                                ;;   This allocation is coocked to insure
+                                                ;;   that the the buffer for the FXSTORE instruction
+                                                ;;   will be 16 byte aligned also.
+                                                ;;
+ExceptionNumber dd      ?                       ;; first entry will be the vector number pushed by the stub
+
+DebugStackBegin db      "<<<< DbgStkBegin"      ;; initial debug ESP == DebugStackBegin, set in stub
+
+.CODE
+
+externdef InterruptDistrubutionHub:near
+
+;------------------------------------------------------------------------------
+; BOOLEAN
+; FxStorSupport (
+;   void
+;   )
+;
+; Abstract: Returns TRUE if FxStor instructions are supported
+;
+FxStorSupport   PROC    C PUBLIC
+
+;
+; cpuid corrupts ebx which must be preserved per the C calling convention
+;
+                push    ebx
+                mov     eax, 1
+                cpuid
+                mov     eax, edx
+                and     eax, FXSTOR_FLAG
+                shr     eax, 24
+                pop     ebx
+                ret
+FxStorSupport   ENDP
+
+
+
+;------------------------------------------------------------------------------
+; void
+; Vect2Desc (
+;   DESCRIPTOR * DestDesc,
+;   void (*Vector) (void)
+;   )
+;
+; Abstract: Encodes an IDT descriptor with the given physical address
+;
+Vect2Desc       PROC    C PUBLIC DestPtr:DWORD, Vector:DWORD
+
+                mov     eax, Vector
+                mov     ecx, DestPtr
+                mov     word ptr [ecx], ax                  ; write bits 15..0 of offset
+                mov     dx, cs
+                mov     word ptr [ecx+2], dx                ; SYS_CODE_SEL from GDT
+                mov     word ptr [ecx+4], 0e00h OR 8000h    ; type = 386 interrupt gate, present
+                shr     eax, 16
+                mov     word ptr [ecx+6], ax                ; write bits 31..16 of offset
+
+                ret
+
+Vect2Desc       ENDP
+
+
+
+;------------------------------------------------------------------------------
+; InterruptEntryStub
+;
+; Abstract: This code is not a function, but is a small piece of code that is
+;               copied and fixed up once for each IDT entry that is hooked.
+;
+InterruptEntryStub::
+                mov     AppEsp, esp                  ; save stack top
+                mov     esp, offset DebugStackBegin  ; switch to debugger stack
+                push    0                            ; push vector number - will be modified before installed
+                db      0e9h                         ; jump rel32
+                dd      0                            ; fixed up to relative address of CommonIdtEntry
+InterruptEntryStubEnd:
+
+
+
+;------------------------------------------------------------------------------
+; CommonIdtEntry
+;
+; Abstract: This code is not a function, but is the common part for all IDT
+;               vectors.
+;
+CommonIdtEntry::
+;;
+;; At this point, the stub has saved the current application stack esp into AppEsp
+;; and switched stacks to the debug stack, where it pushed the vector number
+;;
+;; The application stack looks like this:
+;;
+;;              ...
+;;              (last application stack entry)
+;;              eflags from interrupted task
+;;              CS from interrupted task
+;;              EIP from interrupted task
+;;              Error code <-------------------- Only present for some exeption types
+;;
+;;
+
+
+;; The stub switched us to the debug stack and pushed the interrupt number.
+;;
+;; Next, construct the context record.  It will be build on the debug stack by
+;; pushing the registers in the correct order so as to create the context structure
+;; on the debug stack.  The context record must be built from the end back to the
+;; beginning because the stack grows down...
+;
+;; For reference, the context record looks like this:
+;;
+;; typedef
+;; struct {
+;;   UINT32             ExceptionData;
+;;   FX_SAVE_STATE_IA32 FxSaveState;
+;;   UINT32             Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+;;   UINT32             Cr0, Cr2, Cr3, Cr4;
+;;   UINT32             EFlags;
+;;   UINT32             Ldtr, Tr;
+;;   UINT32             Gdtr[2], Idtr[2];
+;;   UINT32             Eip;
+;;   UINT32             Gs, Fs, Es, Ds, Cs, Ss;
+;;   UINT32             Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+;; } SYSTEM_CONTEXT_IA32;  // 32 bit system context record
+
+;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+                pushad
+
+;; Save interrupt state eflags register...
+                pushfd
+                pop     eax
+                mov     dword ptr Eflags, eax
+
+;; We need to determine if any extra data was pushed by the exception, and if so, save it
+;; To do this, we check the exception number pushed by the stub, and cache the
+;; result in a variable since we'll need this again.
+                .IF     ExceptionNumber == EXCPT32_DOUBLE_FAULT
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_INVALID_TSS
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_SEG_NOT_PRESENT
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_STACK_FAULT
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_GP_FAULT
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_PAGE_FAULT
+                mov     ExtraPush, 1
+                .ELSEIF ExceptionNumber == EXCPT32_ALIGNMENT_CHECK
+                mov     ExtraPush, 1
+                .ELSE
+                mov     ExtraPush, 0
+                .ENDIF
+
+;; If there's some extra data, save it also, and modify the saved AppEsp to effectively
+;; pop this value off the application's stack.
+                .IF     ExtraPush == 1
+                mov     eax, AppEsp
+                mov     ebx, [eax]
+                mov     ExceptData, ebx
+                add     eax, 4
+                mov     AppEsp, eax
+                .ELSE
+                mov     ExceptData, 0
+                .ENDIF
+
+;; The "pushad" above pushed the debug stack esp.  Since what we're actually doing
+;; is building the context record on the debug stack, we need to save the pushed
+;; debug ESP, and replace it with the application's last stack entry...
+                mov     eax, [esp + 12]
+                mov     DebugEsp, eax
+                mov     eax, AppEsp
+                add     eax, 12
+                ; application stack has eflags, cs, & eip, so
+                ; last actual application stack entry is
+                ; 12 bytes into the application stack.
+                mov     [esp + 12], eax
+
+;; continue building context record
+;; UINT32  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+                mov     eax, ss
+                push    eax
+
+                ; CS from application is one entry back in application stack
+                mov     eax, AppEsp
+                movzx   eax, word ptr [eax + 4]
+                push    eax
+
+                mov     eax, ds
+                push    eax
+                mov     eax, es
+                push    eax
+                mov     eax, fs
+                push    eax
+                mov     eax, gs
+                push    eax
+
+;; UINT32  Eip;
+                ; Eip from application is on top of application stack
+                mov     eax, AppEsp
+                push    dword ptr [eax]
+
+;; UINT32  Gdtr[2], Idtr[2];
+                push    0
+                push    0
+                sidt    fword ptr [esp]
+                push    0
+                push    0
+                sgdt    fword ptr [esp]
+
+;; UINT32  Ldtr, Tr;
+                xor     eax, eax
+                str     ax
+                push    eax
+                sldt    ax
+                push    eax
+
+;; UINT32  EFlags;
+;; Eflags from application is two entries back in application stack
+                mov     eax, AppEsp
+                push    dword ptr [eax + 8]
+
+;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+;; insure FXSAVE/FXRSTOR is enabled in CR4...
+;; ... while we're at it, make sure DE is also enabled...
+                mov     eax, cr4
+                or      eax, 208h
+                mov     cr4, eax
+                push    eax
+                mov     eax, cr3
+                push    eax
+                mov     eax, cr2
+                push    eax
+                push    0
+                mov     eax, cr0
+                push    eax
+
+;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                mov     eax, dr7
+                push    eax
+;; clear Dr7 while executing debugger itself
+                xor     eax, eax
+                mov     dr7, eax
+
+                mov     eax, dr6
+                push    eax
+;; insure all status bits in dr6 are clear...
+                xor     eax, eax
+                mov     dr6, eax
+
+                mov     eax, dr3
+                push    eax
+                mov     eax, dr2
+                push    eax
+                mov     eax, dr1
+                push    eax
+                mov     eax, dr0
+                push    eax
+
+;; FX_SAVE_STATE_IA32 FxSaveState;
+                sub     esp, 512
+                mov     edi, esp
+                ; IMPORTANT!! The debug stack has been carefully constructed to
+                ; insure that esp and edi are 16 byte aligned when we get here.
+                ; They MUST be.  If they are not, a GP fault will occur.
+                FXSTOR_EDI
+
+;; UEFI calling convention for IA32 requires that Direction flag in EFLAGs is clear
+                cld
+
+;; UINT32  ExceptionData;
+                mov     eax, ExceptData
+                push    eax
+
+; call to C code which will in turn call registered handler
+; pass in the vector number
+                mov     eax, esp
+                push    eax
+                mov     eax, ExceptionNumber
+                push    eax
+                call    InterruptDistrubutionHub
+                add     esp, 8
+
+; restore context...
+;; UINT32  ExceptionData;
+                add     esp, 4
+
+;; FX_SAVE_STATE_IA32 FxSaveState;
+                mov     esi, esp
+                FXRSTOR_ESI
+                add     esp, 512
+
+;; UINT32  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                pop     eax
+                mov     dr0, eax
+                pop     eax
+                mov     dr1, eax
+                pop     eax
+                mov     dr2, eax
+                pop     eax
+                mov     dr3, eax
+;; skip restore of dr6.  We cleared dr6 during the context save.
+                add     esp, 4
+                pop     eax
+                mov     dr7, eax
+
+;; UINT32  Cr0, Cr1, Cr2, Cr3, Cr4;
+                pop     eax
+                mov     cr0, eax
+                add     esp, 4
+                pop     eax
+                mov     cr2, eax
+                pop     eax
+                mov     cr3, eax
+                pop     eax
+                mov     cr4, eax
+
+;; UINT32  EFlags;
+                mov     eax, AppEsp
+                pop     dword ptr [eax + 8]
+
+;; UINT32  Ldtr, Tr;
+;; UINT32  Gdtr[2], Idtr[2];
+;; Best not let anyone mess with these particular registers...
+                add     esp, 24
+
+;; UINT32  Eip;
+                pop     dword ptr [eax]
+
+;; UINT32  SegGs, SegFs, SegEs, SegDs, SegCs, SegSs;
+;; NOTE - modified segment registers could hang the debugger...  We
+;;        could attempt to insulate ourselves against this possibility,
+;;        but that poses risks as well.
+;;
+
+                pop     gs
+                pop     fs
+                pop     es
+                pop     ds
+                pop     [eax + 4]
+                pop     ss
+
+;; The next stuff to restore is the general purpose registers that were pushed
+;; using the "pushad" instruction.
+;;
+;; The value of ESP as stored in the context record is the application ESP
+;; including the 3 entries on the application stack caused by the exception
+;; itself. It may have been modified by the debug agent, so we need to
+;; determine if we need to relocate the application stack.
+
+                mov     ebx, [esp + 12]  ; move the potentially modified AppEsp into ebx
+                mov     eax, AppEsp
+                add     eax, 12
+                cmp     ebx, eax
+                je      NoAppStackMove
+
+                mov     eax, AppEsp
+                mov     ecx, [eax]       ; EIP
+                mov     [ebx], ecx
+
+                mov     ecx, [eax + 4]   ; CS
+                mov     [ebx + 4], ecx
+
+                mov     ecx, [eax + 8]   ; EFLAGS
+                mov     [ebx + 8], ecx
+
+                mov     eax, ebx         ; modify the saved AppEsp to the new AppEsp
+                mov     AppEsp, eax
+NoAppStackMove:
+                mov     eax, DebugEsp    ; restore the DebugEsp on the debug stack
+                                         ; so our "popad" will not cause a stack switch
+                mov     [esp + 12], eax
+
+                cmp     ExceptionNumber, 068h
+                jne     NoChain
+
+Chain:
+
+;; Restore eflags so when we chain, the flags will be exactly as if we were never here.
+;; We gin up the stack to do an iretd so we can get ALL the flags.
+                mov     eax, AppEsp
+                mov     ebx, [eax + 8]
+                and     ebx, NOT 300h ; special handling for IF and TF
+                push    ebx
+                push    cs
+                push    PhonyIretd
+                iretd
+PhonyIretd:
+
+;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+                popad
+
+;; Switch back to application stack
+                mov     esp, AppEsp
+
+;; Jump to original handler
+                jmp     OrigVector
+
+NoChain:
+;; UINT32  Edi, Esi, Ebp, Esp, Ebx, Edx, Ecx, Eax;
+                popad
+
+;; Switch back to application stack
+                mov     esp, AppEsp
+
+;; We're outa here...
+                iretd
+END
+
+
+
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/DebugSupport.h b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/DebugSupport.h
new file mode 100644
index 0000000000..7f7d8e5ba6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/DebugSupport.h
@@ -0,0 +1,298 @@
+/** @file
+  Generic debug support macros, typedefs and prototypes for IA32/x64.
+
+Copyright (c) 2006 - 2010, 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.             
+
+**/
+
+#ifndef _DEBUG_SUPPORT_H_
+#define _DEBUG_SUPPORT_H_
+
+#include <Uefi.h>
+
+#include <Protocol/DebugSupport.h>
+#include <Protocol/LoadedImage.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+
+#define NUM_IDT_ENTRIES                 0x78
+#define SYSTEM_TIMER_VECTOR             0x68
+
+typedef
+VOID
+(*DEBUG_PROC) (
+  VOID
+  );
+
+typedef
+VOID
+(EFIAPI *CALLBACK_FUNC) (
+  );
+
+typedef struct {
+  IA32_IDT_GATE_DESCRIPTOR  OrigDesc;
+  DEBUG_PROC                OrigVector;
+  IA32_IDT_GATE_DESCRIPTOR  NewDesc;
+  DEBUG_PROC                StubEntry;
+  CALLBACK_FUNC             RegisteredCallback;
+} IDT_ENTRY;
+
+extern UINT8                     InterruptEntryStub[];
+extern UINT32                    StubSize;
+extern VOID                      (*OrigVector) (VOID);
+extern IDT_ENTRY                 *IdtEntryTable;
+extern IA32_IDT_GATE_DESCRIPTOR  NullDesc;
+
+/**
+  Generic IDT entry.
+
+**/
+VOID
+CommonIdtEntry (
+  VOID
+  );
+
+/**
+  Check whether FXSTOR is supported
+
+  @retval TRUE   FXSTOR is supported.
+  @retval FALSE  FXSTOR is not supported.
+
+**/
+BOOLEAN
+FxStorSupport (
+  VOID
+  );
+
+/**
+  Encodes an IDT descriptor with the given physical address.
+
+  @param  DestDesc    The IDT descriptor address.
+  @param  Vecotr      The interrupt vector entry.
+
+**/
+VOID
+Vect2Desc (
+  IA32_IDT_GATE_DESCRIPTOR * DestDesc,
+  VOID (*Vector) (VOID)
+  );
+
+/**
+  Initializes driver's handler registration database. 
+  
+  This code executes in boot services context
+  Must be public because it's referenced from DebugSupport.c
+
+  @retval  EFI_UNSUPPORTED      If IA32 processor does not support FXSTOR/FXRSTOR instructions,
+                                the context save will fail, so these processor's are not supported.
+  @retval  EFI_OUT_OF_RESOURCES Fails to allocate memory.
+  @retval  EFI_SUCCESS          Initializes successfully.
+
+**/
+EFI_STATUS
+PlInitializeDebugSupportDriver (
+  VOID
+  );
+
+/**
+  This is the callback that is written to the LoadedImage protocol instance
+  on the image handle. It uninstalls all registered handlers and frees all entry
+  stub memory.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+
+  @retval EFI_SUCCESS    Always.
+
+**/
+EFI_STATUS
+EFIAPI
+PlUnloadDebugSupportDriver (
+  IN EFI_HANDLE                       ImageHandle
+  );
+
+/**
+  Returns the maximum value that may be used for the ProcessorIndex parameter in
+  RegisterPeriodicCallback() and RegisterExceptionCallback().                   
+    
+  Hard coded to support only 1 processor for now.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the maximum supported
+                                processor index is returned. Always 0 returned.                                     
+                                
+  @retval EFI_SUCCESS           Always returned with **MaxProcessorIndex set to 0.
+
+**/
+EFI_STATUS
+EFIAPI
+GetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  OUT UINTN                           *MaxProcessorIndex
+  );
+
+/**
+  Registers a function to be called back periodically in interrupt context.
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  PeriodicCallback      A pointer to a function of type PERIODIC_CALLBACK that is the main
+                                periodic entry point of the debug agent.
+                                
+  @retval EFI_SUCCESS           The function completed successfully.  
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.                
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback                               
+                                function. 
+**/
+EFI_STATUS
+EFIAPI
+RegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  IN UINTN                            ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK            PeriodicCallback
+  );
+
+/**
+  Registers a function to be called when a given processor exception occurs.
+
+  This code executes in boot services context.
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  ExceptionCallback     A pointer to a function of type EXCEPTION_CALLBACK that is called
+                                when the processor exception specified by ExceptionType occurs.  
+  @param  ExceptionType         Specifies which processor exception to hook.                       
+                                
+  @retval EFI_SUCCESS           The function completed successfully.  
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.                
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback                               
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  IN UINTN                            ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK           ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE               ExceptionType
+  );
+
+/**
+  Invalidates processor instruction cache for a memory range. Subsequent execution in this range
+  causes a fresh memory fetch to retrieve code to be executed.                                  
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor's instruction cache is to be invalidated.
+  @param  Start                 Specifies the physical base of the memory range to be invalidated.                                
+  @param  Length                Specifies the minimum number of bytes in the processor's instruction
+                                cache to invalidate.                                                 
+                                
+  @retval EFI_SUCCESS           Always returned.
+
+**/
+EFI_STATUS
+EFIAPI
+InvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  IN UINTN                            ProcessorIndex,
+  IN VOID                             *Start,
+  IN UINT64                           Length
+  );
+
+/**
+  Allocate pool for a new IDT entry stub.
+
+  Copy the generic stub into the new buffer and fixup the vector number
+  and jump target address.
+
+  @param  ExceptionType   This is the exception type that the new stub will be created
+                          for.
+  @param  Stub            On successful exit, *Stub contains the newly allocated entry stub.
+
+**/
+VOID
+CreateEntryStub (
+  IN EFI_EXCEPTION_TYPE     ExceptionType,
+  OUT VOID                  **Stub
+  );
+
+/**
+  Get Interrupt Handle from IDT Gate Descriptor.
+
+  @param  IdtGateDecriptor  IDT Gate Descriptor.
+
+  @return Interrupt Handle stored in IDT Gate Descriptor.
+
+**/
+UINTN
+GetInterruptHandleFromIdt (
+  IN IA32_IDT_GATE_DESCRIPTOR  *IdtGateDecriptor
+  );
+
+/**
+  This is the main worker function that manages the state of the interrupt
+  handlers.  It both installs and uninstalls interrupt handlers based on the
+  value of NewCallback.  If NewCallback is NULL, then uninstall is indicated.
+  If NewCallback is non-NULL, then install is indicated.
+
+  @param  NewCallback   If non-NULL, NewCallback specifies the new handler to register.
+                        If NULL, specifies that the previously registered handler should
+                        be uninstalled.
+  @param  ExceptionType Indicates which entry to manage.
+
+  @retval EFI_SUCCESS            Process is ok.
+  @retval EFI_INVALID_PARAMETER  Requested uninstalling a handler from a vector that has
+                                 no handler registered for it
+  @retval EFI_ALREADY_STARTED    Requested install to a vector that already has a handler registered.
+  @retval others                 Possible return values are passed through from UnHookEntry and HookEntry.
+
+**/
+EFI_STATUS
+ManageIdtEntryTable (
+  CALLBACK_FUNC      NewCallback,
+  EFI_EXCEPTION_TYPE ExceptionType
+  );
+
+/**
+  Creates a nes entry stub.  Then saves the current IDT entry and replaces it
+  with an interrupt gate for the new entry point.  The IdtEntryTable is updated
+  with the new registered function.
+
+  This code executes in boot services context.  The stub entry executes in interrupt
+  context.
+
+  @param  ExceptionType      Specifies which vector to hook.
+  @param  NewCallback        A pointer to the new function to be registered.
+
+**/
+VOID
+HookEntry (
+  IN EFI_EXCEPTION_TYPE            ExceptionType,
+  IN CALLBACK_FUNC                 NewCallback
+  );
+
+/**
+  Undoes HookEntry. This code executes in boot services context.
+
+  @param  ExceptionType   Specifies which entry to unhook
+
+**/
+VOID
+UnhookEntry (
+  IN EFI_EXCEPTION_TYPE           ExceptionType
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.c b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.c
new file mode 100644
index 0000000000..f0c529a41b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.c
@@ -0,0 +1,373 @@
+/** @file
+  IA32/x64 generic functions to support Debug Support protocol.
+
+Copyright (c) 2006 - 2010, 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 "DebugSupport.h"
+
+//
+// This the global main table to keep track of the interrupts
+//
+IDT_ENTRY                 *IdtEntryTable  = NULL;
+
+/**
+  Read IDT Gate Descriptor from IDT Table.
+
+  @param  Vector            Specifies vector number.
+  @param  IdtGateDescriptor Pointer to IDT Gate Descriptor read from IDT Table.
+
+**/
+VOID
+ReadIdtGateDescriptor (
+  IN  EFI_EXCEPTION_TYPE        Vector,
+  OUT IA32_IDT_GATE_DESCRIPTOR  *IdtGateDescriptor
+  )
+{
+ IA32_DESCRIPTOR            IdtrValue;
+ IA32_IDT_GATE_DESCRIPTOR   *IdtTable;
+
+ AsmReadIdtr (&IdtrValue);
+ IdtTable = (IA32_IDT_GATE_DESCRIPTOR *) IdtrValue.Base;
+
+ CopyMem ((VOID *) IdtGateDescriptor, (VOID *) &(IdtTable)[Vector], sizeof (IA32_IDT_GATE_DESCRIPTOR));
+}
+
+/**
+  Write IDT Gate Descriptor into IDT Table.
+
+  @param  Vector            Specifies vector number.
+  @param  IdtGateDescriptor Pointer to IDT Gate Descriptor written into IDT Table.
+
+**/
+VOID
+WriteIdtGateDescriptor (
+  EFI_EXCEPTION_TYPE        Vector,
+  IA32_IDT_GATE_DESCRIPTOR  *IdtGateDescriptor
+  )
+{
+ IA32_DESCRIPTOR            IdtrValue;
+ IA32_IDT_GATE_DESCRIPTOR   *IdtTable;
+
+ AsmReadIdtr (&IdtrValue);
+ IdtTable = (IA32_IDT_GATE_DESCRIPTOR *) IdtrValue.Base;
+
+ CopyMem ((VOID *) &(IdtTable)[Vector], (VOID *) IdtGateDescriptor, sizeof (IA32_IDT_GATE_DESCRIPTOR));
+}
+
+/**
+  Creates a nes entry stub.  Then saves the current IDT entry and replaces it
+  with an interrupt gate for the new entry point.  The IdtEntryTable is updated
+  with the new registered function.
+
+  This code executes in boot services context.  The stub entry executes in interrupt
+  context.
+
+  @param  ExceptionType      Specifies which vector to hook.
+  @param  NewCallback        A pointer to the new function to be registered.
+
+**/
+VOID
+HookEntry (
+  IN EFI_EXCEPTION_TYPE            ExceptionType,
+  IN CALLBACK_FUNC                 NewCallback
+  )
+{
+  BOOLEAN     OldIntFlagState;
+
+  CreateEntryStub (ExceptionType, (VOID **) &IdtEntryTable[ExceptionType].StubEntry);
+                          
+  //
+  // Disables CPU interrupts and returns the previous interrupt state
+  //
+  OldIntFlagState = SaveAndDisableInterrupts ();
+
+  //
+  // gets IDT Gate descriptor by index
+  //
+  ReadIdtGateDescriptor (ExceptionType, &(IdtEntryTable[ExceptionType].OrigDesc));
+  //
+  // stores orignal interrupt handle 
+  //
+  IdtEntryTable[ExceptionType].OrigVector = (DEBUG_PROC) GetInterruptHandleFromIdt (&(IdtEntryTable[ExceptionType].OrigDesc));
+
+  // 
+  // encodes new IDT Gate descriptor by stub entry 
+  //
+  Vect2Desc (&IdtEntryTable[ExceptionType].NewDesc, IdtEntryTable[ExceptionType].StubEntry);
+  //
+  // stores NewCallback
+  //
+  IdtEntryTable[ExceptionType].RegisteredCallback = NewCallback;
+
+  //
+  // writes back new IDT Gate descriptor
+  //
+  WriteIdtGateDescriptor (ExceptionType, &(IdtEntryTable[ExceptionType].NewDesc));
+
+  //
+  // restore interrupt state
+  //
+  SetInterruptState (OldIntFlagState);
+
+  return ;
+}
+
+/**
+  Undoes HookEntry. This code executes in boot services context.
+
+  @param  ExceptionType   Specifies which entry to unhook
+
+**/
+VOID
+UnhookEntry (
+  IN EFI_EXCEPTION_TYPE           ExceptionType
+  )
+{
+  BOOLEAN     OldIntFlagState;
+
+  //
+  // Disables CPU interrupts and returns the previous interrupt state
+  //
+  OldIntFlagState = SaveAndDisableInterrupts ();
+
+  //
+  // restore the default IDT Date Descriptor
+  //
+  WriteIdtGateDescriptor (ExceptionType, &(IdtEntryTable[ExceptionType].OrigDesc));
+
+  //
+  // restore interrupt state
+  //
+  SetInterruptState (OldIntFlagState);
+
+  return ;
+}
+
+/**
+  Returns the maximum value that may be used for the ProcessorIndex parameter in
+  RegisterPeriodicCallback() and RegisterExceptionCallback().                   
+    
+  Hard coded to support only 1 processor for now.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the maximum supported
+                                processor index is returned. Always 0 returned.                                     
+                                
+  @retval EFI_SUCCESS           Always returned with **MaxProcessorIndex set to 0.
+
+**/
+EFI_STATUS
+EFIAPI
+GetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  OUT UINTN                           *MaxProcessorIndex
+  )
+{
+  *MaxProcessorIndex = 0;
+  return EFI_SUCCESS;
+}
+
+/**
+  Registers a function to be called back periodically in interrupt context.
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  PeriodicCallback      A pointer to a function of type PERIODIC_CALLBACK that is the main
+                                periodic entry point of the debug agent.
+                                
+  @retval EFI_SUCCESS           The function completed successfully.  
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.                
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback                               
+                                function. 
+**/
+EFI_STATUS
+EFIAPI
+RegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL *This,
+  IN UINTN                      ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK      PeriodicCallback
+  )
+{
+  return ManageIdtEntryTable (PeriodicCallback, SYSTEM_TIMER_VECTOR);
+}
+
+/**
+  Registers a function to be called when a given processor exception occurs.
+
+  This code executes in boot services context.
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  ExceptionCallback     A pointer to a function of type EXCEPTION_CALLBACK that is called
+                                when the processor exception specified by ExceptionType occurs.  
+  @param  ExceptionType         Specifies which processor exception to hook.                       
+                                
+  @retval EFI_SUCCESS           The function completed successfully.  
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.                
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback                               
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL *This,
+  IN UINTN                      ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK     ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE         ExceptionType
+  )
+{
+  return ManageIdtEntryTable (ExceptionCallback, ExceptionType);
+}
+
+
+/**
+  Invalidates processor instruction cache for a memory range. Subsequent execution in this range
+  causes a fresh memory fetch to retrieve code to be executed.                                  
+    
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor's instruction cache is to be invalidated.
+  @param  Start                 Specifies the physical base of the memory range to be invalidated.                                
+  @param  Length                Specifies the minimum number of bytes in the processor's instruction
+                                cache to invalidate.                                                 
+                                
+  @retval EFI_SUCCESS           Always returned.
+
+**/
+EFI_STATUS
+EFIAPI
+InvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL       *This,
+  IN UINTN                            ProcessorIndex,
+  IN VOID                             *Start,
+  IN UINT64                           Length
+  )
+{
+  AsmWbinvd ();
+  return EFI_SUCCESS;
+}
+
+/**
+  Common piece of code that invokes the registered handlers.
+
+  This code executes in exception context so no efi calls are allowed.
+  This code is called from assembly file.
+
+  @param  ExceptionType     Exception type
+  @param  ContextRecord     System context
+
+**/
+VOID
+InterruptDistrubutionHub (
+  EFI_EXCEPTION_TYPE      ExceptionType,
+  EFI_SYSTEM_CONTEXT_IA32 *ContextRecord
+  )
+{
+  if (IdtEntryTable[ExceptionType].RegisteredCallback != NULL) {
+    if (ExceptionType != SYSTEM_TIMER_VECTOR) {
+      IdtEntryTable[ExceptionType].RegisteredCallback (ExceptionType, ContextRecord);
+    } else {
+      OrigVector = IdtEntryTable[ExceptionType].OrigVector;
+      IdtEntryTable[ExceptionType].RegisteredCallback (ContextRecord);
+    }
+  }
+}
+
+/**
+  This is the callback that is written to the Loaded Image protocol instance
+  on the image handle. It uninstalls all registered handlers and frees all entry
+  stub memory.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+
+  @retval EFI_SUCCESS    Always.
+
+**/
+EFI_STATUS
+EFIAPI
+PlUnloadDebugSupportDriver (
+  IN EFI_HANDLE ImageHandle
+  )
+{
+  EFI_EXCEPTION_TYPE  ExceptionType;
+
+  for (ExceptionType = 0; ExceptionType < NUM_IDT_ENTRIES; ExceptionType++) {
+    ManageIdtEntryTable (NULL, ExceptionType);
+    //
+    // Free space for each Interrupt Stub precedure.
+    //
+    if (IdtEntryTable[ExceptionType].StubEntry != NULL) {
+      FreePool ((VOID *)(UINTN)IdtEntryTable[ExceptionType].StubEntry);
+    }
+  }
+
+  FreePool (IdtEntryTable);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initializes driver's handler registration database. 
+  
+  This code executes in boot services context.
+  Must be public because it's referenced from DebugSupport.c
+
+  @retval  EFI_UNSUPPORTED      If IA32/x64 processor does not support FXSTOR/FXRSTOR instructions,
+                                the context save will fail, so these processors are not supported.
+  @retval  EFI_OUT_OF_RESOURCES Fails to allocate memory.
+  @retval  EFI_SUCCESS          Initializes successfully.
+
+**/
+EFI_STATUS
+PlInitializeDebugSupportDriver (
+  VOID
+  )
+{
+  EFI_EXCEPTION_TYPE  ExceptionType;
+
+  //
+  // Check whether FxStor instructions are supported.
+  //
+  if (!FxStorSupport ()) {
+    return EFI_UNSUPPORTED;
+  }
+
+  IdtEntryTable = AllocateZeroPool (sizeof (IDT_ENTRY) * NUM_IDT_ENTRIES);
+  if (IdtEntryTable == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (ExceptionType = 0; ExceptionType < NUM_IDT_ENTRIES; ExceptionType ++) {
+    IdtEntryTable[ExceptionType].StubEntry = (DEBUG_PROC) (UINTN) AllocatePool (StubSize);
+    if (IdtEntryTable[ExceptionType].StubEntry == NULL) {
+      goto ErrorCleanup;
+    }
+    
+    //
+    // Copy Interrupt stub code.
+    //
+    CopyMem ((VOID *)(UINTN)IdtEntryTable[ExceptionType].StubEntry, InterruptEntryStub, StubSize);
+  }
+  return EFI_SUCCESS;
+
+ErrorCleanup:
+
+  for (ExceptionType = 0; ExceptionType < NUM_IDT_ENTRIES; ExceptionType++) {
+    if (IdtEntryTable[ExceptionType].StubEntry != NULL) {
+      FreePool ((VOID *)(UINTN)IdtEntryTable[ExceptionType].StubEntry);
+    }
+  }
+  FreePool (IdtEntryTable);
+
+  return EFI_OUT_OF_RESOURCES;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.h b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.h
new file mode 100644
index 0000000000..0a1577fab3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupport.h
@@ -0,0 +1,22 @@
+/** @file
+  IA32 specific debug support macros, typedefs and prototypes.
+
+Copyright (c) 2006 - 2008, 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.             
+
+**/
+
+#ifndef _PLDEBUG_SUPPORT_H_
+#define _PLDEBUG_SUPPORT_H_
+
+#include "Ia32/DebugSupport.h"
+
+#define EFI_ISA                         IsaIa32
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupportIa32.c b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupportIa32.c
new file mode 100644
index 0000000000..23222737f8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ia32/PlDebugSupportIa32.c
@@ -0,0 +1,145 @@
+/** @file
+  IA32 specific functions to support Debug Support protocol.
+
+Copyright (c) 2008 - 2010, 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 "PlDebugSupport.h"
+
+IA32_IDT_GATE_DESCRIPTOR  NullDesc = {{0}};
+
+/**
+  Get Interrupt Handle from IDT Gate Descriptor.
+
+  @param  IdtGateDescriptor  IDT Gate Descriptor.
+
+  @return Interrupt Handle stored in IDT Gate Descriptor.
+
+**/
+UINTN
+GetInterruptHandleFromIdt (
+  IN IA32_IDT_GATE_DESCRIPTOR  *IdtGateDescriptor
+  )
+{
+  UINTN      InterruptHandle;
+
+  //
+  // InterruptHandle  0-15 : OffsetLow
+  // InterruptHandle 16-31 : OffsetHigh
+  //
+  ((UINT16 *) &InterruptHandle)[0] = (UINT16) IdtGateDescriptor->Bits.OffsetLow;
+  ((UINT16 *) &InterruptHandle)[1] = (UINT16) IdtGateDescriptor->Bits.OffsetHigh;
+
+  return InterruptHandle;
+}
+
+/**
+  Allocate pool for a new IDT entry stub.
+
+  Copy the generic stub into the new buffer and fixup the vector number
+  and jump target address.
+
+  @param  ExceptionType   This is the exception type that the new stub will be created
+                          for.
+  @param  Stub            On successful exit, *Stub contains the newly allocated entry stub.
+
+**/
+VOID
+CreateEntryStub (
+  IN EFI_EXCEPTION_TYPE     ExceptionType,
+  OUT VOID                  **Stub
+  )
+{
+  UINT8       *StubCopy;
+
+  StubCopy = *Stub;
+
+  //
+  // Fixup the stub code for this vector
+  //
+
+  // The stub code looks like this:
+  //
+  //    00000000  89 25 00000004 R  mov     AppEsp, esp             ; save stack top
+  //    00000006  BC 00008014 R     mov     esp, offset DbgStkBot   ; switch to debugger stack
+  //    0000000B  6A 00             push    0                       ; push vector number - will be modified before installed
+  //    0000000D  E9                db      0e9h                    ; jump rel32
+  //    0000000E  00000000          dd      0                       ; fixed up to relative address of CommonIdtEntry
+  //
+
+  //
+  // poke in the exception type so the second push pushes the exception type
+  //
+  StubCopy[0x0c] = (UINT8) ExceptionType;
+
+  //
+  // fixup the jump target to point to the common entry
+  //
+  *(UINT32 *) &StubCopy[0x0e] = (UINT32) CommonIdtEntry - (UINT32) &StubCopy[StubSize];
+
+  return ;
+}
+
+/**
+  This is the main worker function that manages the state of the interrupt
+  handlers.  It both installs and uninstalls interrupt handlers based on the
+  value of NewCallback.  If NewCallback is NULL, then uninstall is indicated.
+  If NewCallback is non-NULL, then install is indicated.
+
+  @param  NewCallback   If non-NULL, NewCallback specifies the new handler to register.
+                        If NULL, specifies that the previously registered handler should
+                        be uninstalled.
+  @param  ExceptionType Indicates which entry to manage.
+
+  @retval EFI_SUCCESS            Installing or Uninstalling operation is ok.
+  @retval EFI_INVALID_PARAMETER  Requested uninstalling a handler from a vector that has
+                                 no handler registered for it
+  @retval EFI_ALREADY_STARTED    Requested install to a vector that already has a handler registered.
+
+**/
+EFI_STATUS
+ManageIdtEntryTable (
+  CALLBACK_FUNC      NewCallback,
+  EFI_EXCEPTION_TYPE ExceptionType
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EFI_SUCCESS;
+
+  if (CompareMem (&IdtEntryTable[ExceptionType].NewDesc, &NullDesc, sizeof (IA32_IDT_GATE_DESCRIPTOR)) != 0) {
+    //
+    // we've already installed to this vector
+    //
+    if (NewCallback != NULL) {
+      //
+      // if the input handler is non-null, error
+      //
+      Status = EFI_ALREADY_STARTED;
+    } else {
+      UnhookEntry (ExceptionType);
+    }
+  } else {
+    //
+    // no user handler installed on this vector
+    //
+    if (NewCallback == NULL) {
+      //
+      // if the input handler is null, error
+      //
+      Status = EFI_INVALID_PARAMETER;
+    } else {
+      HookEntry (ExceptionType, NewCallback);
+    }
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/AsmFuncs.s b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/AsmFuncs.s
new file mode 100644
index 0000000000..db75fc088e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/AsmFuncs.s
@@ -0,0 +1,1382 @@
+/// @file
+///  Low level IPF routines used by the debug support driver
+///
+/// Copyright (c) 2006 - 2008, 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 "Common.i"
+#include "Ds64Macros.i"
+
+ASM_GLOBAL PatchSaveBuffer
+ASM_GLOBAL IpfContextBuf
+ASM_GLOBAL CommonHandler
+ASM_GLOBAL ExternalInterruptCount
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      InstructionCacheFlush
+//
+//  Description:
+//      Flushes instruction cache for specified number of bytes
+//
+        ASM_GLOBAL InstructionCacheFlush
+        .proc   InstructionCacheFlush
+        .align 32
+InstructionCacheFlush::
+ {      .mii
+        alloc   r3=2, 0, 0, 0
+        cmp4.leu p0,p6=32, r33;;
+        (p6)    mov r33=32;;
+ }
+ {      .mii
+        nop.m    0
+        zxt4    r29=r33;;
+        dep.z   r30=r29, 0, 5;;
+ }
+ {      .mii
+        cmp4.eq p0,p7=r0, r30
+        shr.u   r28=r29, 5;;
+        (p7)    adds    r28=1, r28;;
+ }
+ {      .mii
+        nop.m    0
+        shl r27=r28, 5;;
+        zxt4    r26=r27;;
+ }
+ {      .mfb
+        add r31=r26, r32
+        nop.f    0
+        nop.b    0
+ }
+LoopBack:   // $L143:
+ {      .mii
+        fc   r32
+        adds    r32=32, r32;;
+        cmp.ltu p14,p15=r32, r31
+ }
+ {      .mfb
+        nop.m    0
+        nop.f    0
+        //(p14) br.cond.dptk.few $L143#;;
+        (p14)   br.cond.dptk.few LoopBack;;
+ }
+ {      .mmi
+        sync.i;;
+        srlz.i
+        nop.i   0;;
+ }
+ {      .mfb
+        nop.m    0
+        nop.f    0
+        br.ret.sptk.few b0;;
+ }
+        .endp   InstructionCacheFlush
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      ChainHandler
+//
+//  Description:
+//      Chains an interrupt handler
+//
+//      The purpose of this function is to enable chaining of the external interrupt.
+//      Since there's no clean SAL abstraction for doing this, we must do it
+//      surreptitiously.
+//
+//      The reserved IVT entry at offset 0x3400 is coopted for use by this handler.
+//      According to Itanium architecture, it is reserved.  Strictly speaking, this is
+//      not safe, as we're cheating and violating the Itanium architecture.  However,
+//      as long as we're the only ones cheating, we should be OK.  Without hooks in
+//      the SAL to enable IVT management, there aren't many good options.
+//
+//      The strategy is to replace the first bundle of the external interrupt handler
+//      with our own that will branch into a piece of code we've supplied and located
+//      in the reserved IVT entry.  Only the first bundle of the external interrupt
+//      IVT entry is modified.
+//
+//      The original bundle is moved and relocated to space
+//      allocated within the reserved IVT entry.  The next bundle following is
+//      is generated to go a hard coded branch back to the second bundle of the
+//      external interrupt IVT entry just in case the first bundle had no branch.
+//
+//      Our new code will execute our handler, and then fall through to the
+//      original bundle after restoring all context appropriately.
+//
+//      The following is a representation of what the IVT memory map looks like with
+//      our chained handler installed:
+//
+//
+//
+//
+//
+//      This IVT entry is      Failsafe bundle
+//      reserved by the
+//      Itanium architecture   Original bundle 0
+//      and is used for
+//      for locating our
+//      handler and the
+//      original bundle        Patch code...
+//      zero of the ext
+//      interrupt handler
+//
+//      RSVD    (3400)         Unused
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//      EXT_INT (3000)         Bundle 0               Bundle zero - This one is
+//                                modified, all other bundles
+//                                                       in the EXT_INT entry are
+//                                                       untouched.
+//
+//
+//       Arguments:
+//
+//       Returns:
+//
+//       Notes:
+//
+//
+        ASM_GLOBAL ChainHandler
+        .proc ChainHandler
+ChainHandler:
+
+        NESTED_SETUP( 0,2+3,3,0 )
+
+        mov         r8=1                           // r8 = success
+        mov         r2=cr.iva;;
+//
+// NOTE: There's a potential hazard here in that we're simply stealing a bunch of
+// bundles (memory) from the IVT and assuming there's no catastrophic side effect.
+//
+// First, save IVT area we're taking over with the patch so we can restore it later
+//
+        addl        out0=PATCH_ENTRY_OFFSET, r2    // out0 = source buffer
+        movl        out1=PatchSaveBuffer           // out1 = destination buffer
+        mov         out2=0x40;;                    // out2 = number of bundles to copy... save entire IDT entry
+        br.call.sptk.few    b0 = CopyBundles
+
+// Next, copy the patch code into the IVT
+        movl        out0=PatchCode                 // out0 = source buffer of patch code
+        addl        out1=PATCH_OFFSET, r2          // out1 = destination buffer - in IVT
+        mov         out2=PATCH_CODE_SIZE;;       
+        shr         out2=out2, 4;;                 // out2 = number of bundles to copy
+        br.call.sptk.few    b0 = CopyBundles
+
+
+// copy original bundle 0 from the external interrupt handler to the
+// appropriate place in the reserved IVT interrupt slot
+        addl        out0=EXT_INT_ENTRY_OFFSET, r2  // out0 = source buffer
+        addl        out1=RELOCATED_EXT_INT, r2     // out1 = destination buffer - in reserved IVT
+        mov         out2=1;;                       // out2 = copy 1 bundle
+        br.call.sptk.few    b0 = CopyBundles
+
+// Now relocate it there because it very likely had a branch instruction that
+// that must now be fixed up.
+        addl        out0=RELOCATED_EXT_INT, r2     // out0 = new runtime address of bundle - in reserved IVT
+        addl        out1=EXT_INT_ENTRY_OFFSET, r2;;// out1 = IP address of previous location
+        mov         out2=out0;;                    // out2 = IP address of new location
+        br.call.sptk.few    b0 = RelocateBundle
+
+// Now copy into the failsafe branch into the next bundle just in case
+// the original ext int bundle 0 bundle did not contain a branch instruction
+        movl        out0=FailsafeBranch            // out0 = source buffer
+        addl        out1=FAILSAFE_BRANCH_OFFSET, r2  // out1 = destination buffer - in reserved IVT
+        mov         out2=1;;                       // out2 = copy 1 bundle
+        br.call.sptk.few    b0 = CopyBundles
+
+// Last, copy in our replacement for the external interrupt IVT entry bundle 0
+        movl        out0=PatchCodeNewBun0          // out0 = source buffer - our replacement bundle 0
+        addl        out1=EXT_INT_ENTRY_OFFSET, r2  // out1 = destination buffer - bundle 0 of External interrupt entry
+        mov         out2=1;;                       // out2 = copy 1 bundle
+        br.call.sptk.few    b0 = CopyBundles
+
+ChainHandlerDone:
+        NESTED_RETURN
+
+        .endp ChainHandler
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      UnchainHandler
+//
+//  Description:
+//      Unchains an interrupt handler
+//
+//  Arguments:
+//
+//  Returns:
+//
+//  Notes:
+//
+//
+        ASM_GLOBAL UnchainHandler
+        .proc UnchainHandler
+
+UnchainHandler:
+
+        NESTED_SETUP( 0,2+3,3,0 )
+
+        mov         r8=1                        // r8 = success
+        mov         r2=cr.iva;;                 // r2 = interrupt vector address
+
+// First copy original Ext Int bundle 0 back to it's proper home...
+        addl        out0=RELOCATED_EXT_INT, r2     // out0 = source - in reserved IVT
+        addl        out1=EXT_INT_ENTRY_OFFSET, r2  // out1 = destination buffer - first bundle of Ext Int entry
+        mov         out2=1;;                       // out2 = copy 1 bundle
+        br.call.sptk.few    b0 = CopyBundles
+
+// Now, relocate it again...
+        addl        out0=EXT_INT_ENTRY_OFFSET, r2  // out1 = New runtime address
+        addl        out1=RELOCATED_EXT_INT, r2;;   // out0 = IP address of previous location
+        mov         out2=out0;;                    // out2 = IP address of new location
+        br.call.sptk.few    b0 = RelocateBundle
+
+// Last, restore the patch area
+        movl        out0=PatchSaveBuffer           // out0 = source buffer
+        addl        out1=PATCH_ENTRY_OFFSET, r2    // out1 = destination buffer
+        mov         out2=0x40;;                    // out2 = number of bundles to copy... save entire IDT entry
+        br.call.sptk.few    b0 = CopyBundles
+
+UnchainHandlerDone:
+        NESTED_RETURN
+
+        .endp UnchainHandler
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      CopyBundles
+//
+//  Description:
+//      Copies instruction bundles - flushes icache as necessary
+//
+//  Arguments:
+//      in0 - Bundle source
+//      in1 - Bundle destination
+//      in2 - Bundle count
+//
+//  Returns:
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+        .proc   CopyBundles
+
+CopyBundles:
+
+        NESTED_SETUP(3,2+1,0,0)
+
+        shl         in2=in2, 1;;                // in2 = count of 8 byte blocks to copy
+
+CopyBundlesLoop:
+
+        cmp.eq      p14, p15 = 0, in2;;         // Check if done
+(p14)   br.sptk.few CopyBundlesDone;;
+
+        ld8         loc2=[in0], 0x8;;           // loc2 = source bytes
+        st8         [in1]=loc2;;                // [in1] = destination bytes
+        fc          in1;;                       // Flush instruction cache
+        sync.i;;                                // Ensure local and remote data/inst caches in sync
+        srlz.i;;                                // Ensure sync has been observed
+        add         in1=0x8, in1;;              // in1 = next destination
+        add         in2=-1, in2;;               // in2 = decrement 8 bytes blocks to copy
+        br.sptk.few CopyBundlesLoop;;
+
+CopyBundlesDone:
+        NESTED_RETURN
+
+        .endp   CopyBundles
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      RelocateBundle
+//
+//  Description:
+//      Relocates an instruction bundle by updating any ip-relative branch instructions.
+//
+//  Arguments:
+//      in0 - Runtime address of bundle
+//      in1 - IP address of previous location of bundle
+//      in2 - IP address of new location of bundle
+//
+//  Returns:
+//      in0 - 1 if successful or 0 if unsuccessful
+//
+//  Notes:
+//      This routine examines all slots in the given bundle that are destined for the
+//      branch execution unit.  If any of these slots contain an IP-relative branch
+//      namely instructions B1, B2, B3, or B6, the slot is fixed-up with a new relative
+//      address.  Errors can occur if a branch cannot be reached.
+//
+        .proc   RelocateBundle
+
+RelocateBundle:
+
+        NESTED_SETUP(3,2+4,3,0)
+
+        mov         loc2=SLOT0                  // loc2 = slot index
+        mov         loc5=in0;;                  // loc5 = runtime address of bundle
+        mov         in0=1;;                     // in0 = success
+
+RelocateBundleNextSlot:
+
+        cmp.ge      p14, p15 = SLOT2, loc2;;    // Check if maximum slot
+(p15)   br.sptk.few RelocateBundleDone
+
+        mov         out0=loc5;;                 // out0 = runtime address of bundle
+        br.call.sptk.few    b0 = GetTemplate
+        mov         loc3=out0;;                 // loc3 = instruction template
+        mov         out0=loc5                   // out0 = runtime address of bundle
+        mov         out1=loc2;;                 // out1 = instruction slot number
+        br.call.sptk.few    b0 = GetSlot
+        mov         loc4=out0;;                 // loc4 = instruction encoding
+        mov         out0=loc4                   // out0 = instuction encoding
+        mov         out1=loc2                   // out1 = instruction slot number
+        mov         out2=loc3;;                 // out2 = instruction template
+        br.call.sptk.few    b0 = IsSlotBranch
+        cmp.eq      p14, p15 = 1, out0;;        // Check if branch slot
+(p15)   add         loc2=1,loc2                 // Increment slot
+(p15)   br.sptk.few RelocateBundleNextSlot
+        mov         out0=loc4                   // out0 = instuction encoding
+        mov         out1=in1                    // out1 = IP address of previous location
+        mov         out2=in2;;                  // out2 = IP address of new location
+        br.call.sptk.few    b0 = RelocateSlot
+        cmp.eq      p14, p15 = 1, out1;;        // Check if relocated slot
+(p15)   mov         in0=0                       // in0 = failure
+(p15)   br.sptk.few RelocateBundleDone
+        mov         out2=out0;;                 // out2 = instruction encoding
+        mov         out0=loc5                   // out0 = runtime address of bundle
+        mov         out1=loc2;;                 // out1 = instruction slot number
+        br.call.sptk.few    b0 = SetSlot
+        add         loc2=1,loc2;;               // Increment slot
+        br.sptk.few RelocateBundleNextSlot
+
+RelocateBundleDone:
+        NESTED_RETURN
+
+        .endp   RelocateBundle
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      RelocateSlot
+//
+//  Description:
+//      Relocates an instruction bundle by updating any ip-relative branch instructions.
+//
+//  Arguments:
+//      in0 - Instruction encoding (41-bits, right justified)
+//      in1 - IP address of previous location of bundle
+//      in2 - IP address of new location of bundle
+//
+//  Returns:
+//      in0 - Instruction encoding (41-bits, right justified)
+//      in1 - 1 if successful otherwise 0
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+        .proc   RelocateSlot
+
+RelocateSlot:
+        NESTED_SETUP(3,2+5,0,0)
+        extr.u      loc2=in0, 37, 4;;           // loc2 = instruction opcode
+        cmp.eq      p14, p15 = 4, loc2;;        // IP-relative branch (B1) or
+                                                // IP-relative counted branch (B2)
+(p15)   cmp.eq      p14, p15 = 5, loc2;;        // IP-relative call (B3)
+(p15)   cmp.eq      p14, p15 = 7, loc2;;        // IP-relative predict (B6)
+(p15)   mov         in1=1                       // Instruction did not need to be reencoded
+(p15)   br.sptk.few RelocateSlotDone
+        tbit.nz     p14, p15 = in0, 36;;        // put relative offset sign bit in p14
+        extr.u      loc2=in0, 13, 20;;          // loc2 = relative offset in instruction
+(p14)   movl        loc3=0xfffffffffff00000;;   // extend sign
+(p14)   or          loc2=loc2, loc3;;
+        shl         loc2=loc2,4;;               // convert to byte offset instead of bundle offset
+        add         loc3=loc2, in1;;            // loc3 = physical address of branch target
+(p14)   sub         loc2=r0,loc2;;              // flip sign in loc2 if offset is negative
+        sub         loc4=loc3,in2;;             // loc4 = relative offset from new ip to branch target
+        cmp.lt      p15, p14 = 0, loc4;;        // get new sign bit
+(p14)   sub         loc5=r0,loc4                // get absolute value of offset
+(p15)   mov         loc5=loc4;;
+        movl        loc6=0x0FFFFFF;;            // maximum offset in bytes for ip-rel branch
+        cmp.gt      p14, p15 = loc5, loc6;;     // check to see we're not out of range for an ip-relative branch
+(p14)   br.sptk.few RelocateSlotError
+        cmp.lt      p15, p14 = 0, loc4;;        // store sign in p14 again
+(p14)   dep         in0=-1,in0,36,1              // store sign bit in instruction
+(p15)   dep         in0=0,in0,36,1
+        shr         loc4=loc4, 4;;              // convert back to bundle offset
+        dep         in0=loc4,in0,13,16;;        // put first 16 bits of new offset into instruction
+        shr         loc4=loc4,16;;
+        dep         in0=loc4,in0,13+16,4        // put last 4 bits of new offset into instruction
+        mov         in1=1;;                     // in1 = success
+        br.sptk.few RelocateSlotDone;;
+
+RelocateSlotError:
+        mov         in1=0;;                     // in1 = failure
+
+RelocateSlotDone:
+        NESTED_RETURN
+
+        .endp   RelocateSlot
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      IsSlotBranch
+//
+//  Description:
+//      Determines if the given instruction is a branch instruction.
+//
+//  Arguments:
+//      in0 - Instruction encoding (41-bits, right justified)
+//      in1 - Instruction slot number
+//      in2 - Bundle template
+//
+//  Returns:
+//      in0 - 1 if branch or 0 if not branch
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+//      IsSlotBranch recognizes all branch instructions by looking at the provided template.
+//      The instruction encoding is only passed to this routine for future expansion.
+//
+        .proc   IsSlotBranch
+
+IsSlotBranch:
+
+        NESTED_SETUP (3,2+0,0,0)
+
+        mov         in0=1;;                     // in0 = 1 which destroys the instruction
+        andcm       in2=in2,in0;;               // in2 = even template to reduce compares
+        mov         in0=0;;                     // in0 = not a branch
+        cmp.eq      p14, p15 = 0x16, in2;;      // Template 0x16 is BBB
+(p14)   br.sptk.few IsSlotBranchTrue
+        cmp.eq      p14, p15 = SLOT0, in1;;     // Slot 0 has no other possiblities
+(p14)   br.sptk.few IsSlotBranchDone
+        cmp.eq      p14, p15 = 0x12, in2;;      // Template 0x12 is MBB
+(p14)   br.sptk.few IsSlotBranchTrue
+        cmp.eq      p14, p15 = SLOT1, in1;;     // Slot 1 has no other possiblities
+(p14)   br.sptk.few IsSlotBranchDone
+        cmp.eq      p14, p15 = 0x10, in2;;      // Template 0x10 is MIB
+(p14)   br.sptk.few IsSlotBranchTrue
+        cmp.eq      p14, p15 = 0x18, in2;;      // Template 0x18 is MMB
+(p14)   br.sptk.few IsSlotBranchTrue
+        cmp.eq      p14, p15 = 0x1C, in2;;      // Template 0x1C is MFB
+(p14)   br.sptk.few IsSlotBranchTrue
+        br.sptk.few IsSlotBranchDone
+
+IsSlotBranchTrue:
+        mov         in0=1;;                     // in0 = branch
+
+IsSlotBranchDone:
+        NESTED_RETURN
+
+        .endp   IsSlotBranch
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      GetTemplate
+//
+//  Description:
+//      Retrieves the instruction template for an instruction bundle
+//
+//  Arguments:
+//      in0 - Runtime address of bundle
+//
+//  Returns:
+//      in0 - Instruction template (5-bits, right-justified)
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+        .proc   GetTemplate
+
+GetTemplate:
+
+        NESTED_SETUP (1,2+2,0,0)
+
+        ld8     loc2=[in0], 0x8             // loc2 = first 8 bytes of branch bundle
+        movl    loc3=MASK_0_4;;             // loc3 = template mask
+        and     loc2=loc2,loc3;;            // loc2 = template, right justified
+        mov     in0=loc2;;                  // in0 = template, right justified
+
+        NESTED_RETURN
+
+        .endp   GetTemplate
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      GetSlot
+//
+//  Description:
+//      Gets the instruction encoding for an instruction slot and bundle
+//
+//  Arguments:
+//      in0 - Runtime address of bundle
+//      in1 - Instruction slot (either 0, 1, or 2)
+//
+//  Returns:
+//      in0 - Instruction encoding (41-bits, right justified)
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+//      Slot0 - [in0 + 0x8] Bits 45-5
+//      Slot1 - [in0 + 0x8] Bits 63-46 and [in0] Bits 22-0
+//      Slot2 - [in0] Bits 63-23
+//
+        .proc   GetSlot
+
+GetSlot:
+        NESTED_SETUP (2,2+3,0,0)
+
+        ld8     loc2=[in0], 0x8;;           // loc2 = first 8 bytes of branch bundle
+        ld8     loc3=[in0];;                // loc3 = second 8 bytes of branch bundle
+        cmp.eq  p14, p15 = 2, in1;;         // check if slot 2 specified
+ (p14)  br.cond.sptk.few    GetSlot2;;      // get slot 2
+        cmp.eq  p14, p15 = 1, in1;;         // check if slot 1 specified
+ (p14)  br.cond.sptk.few    GetSlot1;;      // get slot 1
+
+GetSlot0:
+        extr.u  in0=loc2, 5, 45             // in0 = extracted slot 0
+        br.sptk.few GetSlotDone;;
+
+GetSlot1:
+        extr.u  in0=loc2, 46, 18            // in0 = bits 63-46 of loc2 right-justified
+        extr.u  loc4=loc3, 0, 23;;          // loc4 = bits 22-0 of loc3 right-justified
+        dep     in0=loc4, in0, 18, 15;;
+        shr.u   loc4=loc4,15;;
+        dep     in0=loc4, in0, 33, 8;;      // in0 = extracted slot 1
+        br.sptk.few GetSlotDone;;
+
+GetSlot2:
+        extr.u  in0=loc3, 23, 41;;          // in0 = extracted slot 2
+
+GetSlotDone:
+        NESTED_RETURN
+
+        .endp   GetSlot
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      SetSlot
+//
+//  Description:
+//      Sets the instruction encoding for an instruction slot and bundle
+//
+//  Arguments:
+//      in0 - Runtime address of bundle
+//      in1 - Instruction slot (either 0, 1, or 2)
+//      in2 - Instruction encoding (41-bits, right justified)
+//
+//  Returns:
+//
+//  Notes:
+//      This procedure is a leaf routine
+//
+        .proc       SetSlot
+
+SetSlot:
+        NESTED_SETUP (3,2+3,0,0)
+
+        ld8     loc2=[in0], 0x8;;           // loc2 = first 8 bytes of bundle
+        ld8     loc3=[in0];;                // loc3 = second 8 bytes of bundle
+        cmp.eq  p14, p15 = 2, in1;;         // check if slot 2 specified
+ (p14)  br.cond.sptk.few    SetSlot2;;      // set slot 2
+        cmp.eq  p14, p15 = 1, in1;;         // check if slot 1 specified
+ (p14)  br.cond.sptk.few    SetSlot1;;      // set slot 1
+
+SetSlot0:
+        dep     loc2=0, loc2, 5, 41;;       // remove old instruction from slot 0
+        shl     loc4=in2, 5;;               // loc4 = new instruction ready to be inserted
+        or      loc2=loc2, loc4;;           // loc2 = updated first 8 bytes of bundle
+        add     loc4=0x8,in0;;              // loc4 = address to store first 8 bytes of bundle
+        st8     [loc4]=loc2                 // [loc4] = updated bundle
+        br.sptk.few SetSlotDone;;
+        ;;
+
+SetSlot1:
+        dep     loc2=0, loc2, 46, 18        // remove old instruction from slot 1
+        dep     loc3=0, loc3, 0, 23;;
+        shl     loc4=in2, 46;;              // loc4 = partial instruction ready to be inserted
+        or      loc2=loc2, loc4;;           // loc2 = updated first 8 bytes of bundle
+        add     loc4=0x8,in0;;              // loc4 = address to store first 8 bytes of bundle
+        st8     [loc4]=loc2;;               // [loc4] = updated bundle
+        shr.u   loc4=in2, 18;;              // loc4 = partial instruction ready to be inserted
+        or      loc3=loc3, loc4;;           // loc3 = updated second 8 bytes of bundle
+        st8     [in0]=loc3;;                // [in0] = updated bundle
+        br.sptk.few SetSlotDone;;
+
+SetSlot2:
+        dep     loc3=0, loc3, 23, 41;;      // remove old instruction from slot 2
+        shl     loc4=in2, 23;;              // loc4 = instruction ready to be inserted
+        or      loc3=loc3, loc4;;           // loc3 = updated second 8 bytes of bundle
+        st8     [in0]=loc3;;                // [in0] = updated bundle
+
+SetSlotDone:
+
+        NESTED_RETURN
+        .endp       SetSlot
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      GetIva
+//
+//  Description:
+//      C callable function to obtain the current value of IVA
+//
+//  Returns:
+//      Current value if IVA
+
+        ASM_GLOBAL     GetIva
+        .proc       GetIva
+GetIva:
+        mov         r8=cr2;;
+        br.ret.sptk.many    b0
+
+        .endp       GetIva
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      ProgramInterruptFlags
+//
+//  Description:
+//      C callable function to enable/disable interrupts
+//
+//  Returns:
+//      Previous state of psr.ic
+//
+        ASM_GLOBAL     ProgramInterruptFlags
+        .proc       ProgramInterruptFlags
+ProgramInterruptFlags:
+        alloc       loc0=1,2,0,0;;
+        mov         loc0=psr
+        mov         loc1=0x6000;;
+        and         r8=loc0, loc1           // obtain current psr.ic and psr.i state
+        and         in0=in0, loc1           // insure no extra bits set in input
+        andcm       loc0=loc0,loc1;;        // clear original psr.i and psr.ic
+        or          loc0=loc0,in0;;         // OR in new psr.ic value
+        mov         psr.l=loc0;;            // write new psr
+        srlz.d
+        br.ret.sptk.many    b0              // return
+
+        .endp       ProgramInterruptFlags
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      SpillContext
+//
+//  Description:
+//      Saves system context to context record.
+//
+//  Arguments:
+//          in0 = 512 byte aligned context record address
+//          in1 = original B0
+//          in2 = original ar.bsp
+//          in3 = original ar.bspstore
+//          in4 = original ar.rnat
+//          in5 = original ar.pfs
+//
+//  Notes:
+//      loc0 - scratch
+//      loc1 - scratch
+//      loc2 - temporary application unat storage
+//      loc3 - temporary exception handler unat storage
+
+        .proc       SpillContext
+
+SpillContext:
+        alloc       loc0=6,4,0,0;;          // alloc 6 input, 4 locals, 0 outs
+        mov         loc2=ar.unat;;          // save application context unat (spilled later)
+        mov         ar.unat=r0;;            // set UNAT=0
+        st8.spill   [in0]=r0,8;;
+        st8.spill   [in0]=r1,8;;            // save R1 - R31
+        st8.spill   [in0]=r2,8;;
+        st8.spill   [in0]=r3,8;;
+        st8.spill   [in0]=r4,8;;
+        st8.spill   [in0]=r5,8;;
+        st8.spill   [in0]=r6,8;;
+        st8.spill   [in0]=r7,8;;
+        st8.spill   [in0]=r8,8;;
+        st8.spill   [in0]=r9,8;;
+        st8.spill   [in0]=r10,8;;
+        st8.spill   [in0]=r11,8;;
+        st8.spill   [in0]=r12,8;;
+        st8.spill   [in0]=r13,8;;
+        st8.spill   [in0]=r14,8;;
+        st8.spill   [in0]=r15,8;;
+        st8.spill   [in0]=r16,8;;
+        st8.spill   [in0]=r17,8;;
+        st8.spill   [in0]=r18,8;;
+        st8.spill   [in0]=r19,8;;
+        st8.spill   [in0]=r20,8;;
+        st8.spill   [in0]=r21,8;;
+        st8.spill   [in0]=r22,8;;
+        st8.spill   [in0]=r23,8;;
+        st8.spill   [in0]=r24,8;;
+        st8.spill   [in0]=r25,8;;
+        st8.spill   [in0]=r26,8;;
+        st8.spill   [in0]=r27,8;;
+        st8.spill   [in0]=r28,8;;
+        st8.spill   [in0]=r29,8;;
+        st8.spill   [in0]=r30,8;;
+        st8.spill   [in0]=r31,8;;
+        mov         loc3=ar.unat;;          // save debugger context unat (spilled later)
+        stf.spill   [in0]=f2,16;;           // save f2 - f31
+        stf.spill   [in0]=f3,16;;
+        stf.spill   [in0]=f4,16;;
+        stf.spill   [in0]=f5,16;;
+        stf.spill   [in0]=f6,16;;
+        stf.spill   [in0]=f7,16;;
+        stf.spill   [in0]=f8,16;;
+        stf.spill   [in0]=f9,16;;
+        stf.spill   [in0]=f10,16;;
+        stf.spill   [in0]=f11,16;;
+        stf.spill   [in0]=f12,16;;
+        stf.spill   [in0]=f13,16;;
+        stf.spill   [in0]=f14,16;;
+        stf.spill   [in0]=f15,16;;
+        stf.spill   [in0]=f16,16;;
+        stf.spill   [in0]=f17,16;;
+        stf.spill   [in0]=f18,16;;
+        stf.spill   [in0]=f19,16;;
+        stf.spill   [in0]=f20,16;;
+        stf.spill   [in0]=f21,16;;
+        stf.spill   [in0]=f22,16;;
+        stf.spill   [in0]=f23,16;;
+        stf.spill   [in0]=f24,16;;
+        stf.spill   [in0]=f25,16;;
+        stf.spill   [in0]=f26,16;;
+        stf.spill   [in0]=f27,16;;
+        stf.spill   [in0]=f28,16;;
+        stf.spill   [in0]=f29,16;;
+        stf.spill   [in0]=f30,16;;
+        stf.spill   [in0]=f31,16;;
+        mov         loc0=pr;;               // save predicates
+        st8.spill   [in0]=loc0,8;;
+        st8.spill   [in0]=in1,8;;           // save b0 - b7... in1 already equals saved b0
+        mov         loc0=b1;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b2;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b3;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b4;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b5;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b6;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=b7;;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.rsc;;           // save ar.rsc
+        st8.spill   [in0]=loc0,8;;
+        st8.spill   [in0]=in2,8;;           // save ar.bsp (in2)
+        st8.spill   [in0]=in3,8;;           // save ar.bspstore (in3)
+        st8.spill   [in0]=in4,8;;           // save ar.rnat (in4)
+        mov         loc0=ar.fcr;;           // save ar.fcr (ar21 - IA32 floating-point control register)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.eflag;;         // save ar.eflag (ar24)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.csd;;           // save ar.csd (ar25 - ia32 CS descriptor)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.ssd;;           // save ar.ssd (ar26 - ia32 ss descriptor)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.cflg;;          // save ar.cflg (ar27 - ia32 cr0 and cr4)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.fsr;;           // save ar.fsr (ar28 - ia32 floating-point status register)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.fir;;           // save ar.fir (ar29 - ia32 floating-point instruction register)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.fdr;;           // save ar.fdr (ar30 - ia32 floating-point data register)
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.ccv;;           // save ar.ccv
+        st8.spill   [in0]=loc0,8;;
+        st8.spill   [in0]=loc2,8;;          // save ar.unat (saved to loc2 earlier)
+        mov         loc0=ar.fpsr;;          // save floating point status register
+        st8.spill   [in0]=loc0,8;;
+        st8.spill   [in0]=in5,8;;           // save ar.pfs
+        mov         loc0=ar.lc;;            // save ar.lc
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ar.ec;;            // save ar.ec
+        st8.spill   [in0]=loc0,8;;
+
+        // save control registers
+        mov         loc0=cr.dcr;;           // save dcr
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.itm;;           // save itm
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.iva;;           // save iva
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.pta;;           // save pta
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.ipsr;;          // save ipsr
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.isr;;           // save isr
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.iip;;           // save iip
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.ifa;;           // save ifa
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.itir;;          // save itir
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.iipa;;          // save iipa
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.ifs;;           // save ifs
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.iim;;           // save iim
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=cr.iha;;           // save iha
+        st8.spill   [in0]=loc0,8;;
+
+        // save debug registers
+        mov         loc0=dbr[r0];;          // save dbr0 - dbr7
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=1;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=2;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=3;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=4;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=5;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=6;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=7;;
+        mov         loc0=dbr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        mov         loc0=ibr[r0];;          // save ibr0 - ibr7
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=1;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=2;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=3;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=4;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=5;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=6;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        movl        loc1=7;;
+        mov         loc0=ibr[loc1];;
+        st8.spill   [in0]=loc0,8;;
+        st8.spill   [in0]=loc3;;
+
+        br.ret.sptk.few     b0
+
+        .endp       SpillContext
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      FillContext
+//
+//  Description:
+//      Restores register context from context record.
+//
+//  Arguments:
+//          in0 = address of last element 512 byte aligned context record address
+//          in1 = modified B0
+//          in2 = modified ar.bsp
+//          in3 = modified ar.bspstore
+//          in4 = modified ar.rnat
+//          in5 = modified ar.pfs
+//
+//  Notes:
+//      loc0 - scratch
+//      loc1 - scratch
+//      loc2 - temporary application unat storage
+//      loc3 - temporary exception handler unat storage
+
+        .proc       FillContext
+FillContext:
+        alloc       loc0=6,4,0,0;;          // alloc 6 inputs, 4 locals, 0 outs
+        ld8.fill    loc3=[in0],-8;;         // int_nat (nat bits for R1-31)
+        movl        loc1=7;;                // ibr7
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=6;;                // ibr6
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=5;;                // ibr5
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=4;;                // ibr4
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=3;;                // ibr3
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=2;;                // ibr2
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        movl        loc1=1;;                // ibr1
+        ld8.fill    loc0=[in0],-8;;
+        mov         ibr[loc1]=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ibr0
+        mov         ibr[r0]=loc0;;
+        movl        loc1=7;;                // dbr7
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=6;;                // dbr6
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=5;;                // dbr5
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=4;;                // dbr4
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=3;;                // dbr3
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=2;;                // dbr2
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        movl        loc1=1;;                // dbr1
+        ld8.fill    loc0=[in0],-8;;
+        mov         dbr[loc1]=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // dbr0
+        mov         dbr[r0]=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // iha
+        mov         cr.iha=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // iim
+        mov         cr.iim=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ifs
+        mov         cr.ifs=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // iipa
+        mov         cr.iipa=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // itir
+        mov         cr.itir=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ifa
+        mov         cr.ifa=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // iip
+        mov         cr.iip=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // isr
+        mov         cr.isr=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ipsr
+        mov         cr.ipsr=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // pta
+        mov         cr.pta=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // iva
+        mov         cr.iva=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // itm
+        mov         cr.itm=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // dcr
+        mov         cr.dcr=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ec
+        mov         ar.ec=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // lc
+        mov         ar.lc=loc0;;
+        ld8.fill    in5=[in0],-8;;          // ar.pfs
+        ld8.fill    loc0=[in0],-8;;         // ar.fpsr
+        mov         ar.fpsr=loc0;;
+        ld8.fill    loc2=[in0],-8;;         // ar.unat - restored later...
+        ld8.fill    loc0=[in0],-8;;         // ar.ccv
+        mov         ar.ccv=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.fdr
+        mov         ar.fdr=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.fir
+        mov         ar.fir=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.fsr
+        mov         ar.fsr=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.cflg
+        mov         ar.cflg=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.ssd
+        mov         ar.ssd=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.csd
+        mov         ar.csd=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.eflag
+        mov         ar.eflag=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // ar.fcr
+        mov         ar.fcr=loc0;;
+        ld8.fill    in4=[in0],-8;;          // ar.rnat
+        ld8.fill    in3=[in0],-8;;          // bspstore
+        ld8.fill    in2=[in0],-8;;          // bsp
+        ld8.fill    loc0=[in0],-8;;         // ar.rsc
+        mov         ar.rsc=loc0;;
+        ld8.fill    loc0=[in0],-8;;         // B7 - B0
+        mov         b7=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b6=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b5=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b4=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b3=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b2=loc0;;
+        ld8.fill    loc0=[in0],-8;;
+        mov         b1=loc0;;
+        ld8.fill    in1=[in0],-8;;          // b0 is temporarily stored in in1
+        ld8.fill    loc0=[in0],-16;;        // predicates
+        mov         pr=loc0;;
+        ldf.fill    f31=[in0],-16;;
+        ldf.fill    f30=[in0],-16;;
+        ldf.fill    f29=[in0],-16;;
+        ldf.fill    f28=[in0],-16;;
+        ldf.fill    f27=[in0],-16;;
+        ldf.fill    f26=[in0],-16;;
+        ldf.fill    f25=[in0],-16;;
+        ldf.fill    f24=[in0],-16;;
+        ldf.fill    f23=[in0],-16;;
+        ldf.fill    f22=[in0],-16;;
+        ldf.fill    f21=[in0],-16;;
+        ldf.fill    f20=[in0],-16;;
+        ldf.fill    f19=[in0],-16;;
+        ldf.fill    f18=[in0],-16;;
+        ldf.fill    f17=[in0],-16;;
+        ldf.fill    f16=[in0],-16;;
+        ldf.fill    f15=[in0],-16;;
+        ldf.fill    f14=[in0],-16;;
+        ldf.fill    f13=[in0],-16;;
+        ldf.fill    f12=[in0],-16;;
+        ldf.fill    f11=[in0],-16;;
+        ldf.fill    f10=[in0],-16;;
+        ldf.fill    f9=[in0],-16;;
+        ldf.fill    f8=[in0],-16;;
+        ldf.fill    f7=[in0],-16;;
+        ldf.fill    f6=[in0],-16;;
+        ldf.fill    f5=[in0],-16;;
+        ldf.fill    f4=[in0],-16;;
+        ldf.fill    f3=[in0],-16;;
+        ldf.fill    f2=[in0],-8;;
+        mov         ar.unat=loc3;;          // restore unat (int_nat) before fill of general registers
+        ld8.fill    r31=[in0],-8;;
+        ld8.fill    r30=[in0],-8;;
+        ld8.fill    r29=[in0],-8;;
+        ld8.fill    r28=[in0],-8;;
+        ld8.fill    r27=[in0],-8;;
+        ld8.fill    r26=[in0],-8;;
+        ld8.fill    r25=[in0],-8;;
+        ld8.fill    r24=[in0],-8;;
+        ld8.fill    r23=[in0],-8;;
+        ld8.fill    r22=[in0],-8;;
+        ld8.fill    r21=[in0],-8;;
+        ld8.fill    r20=[in0],-8;;
+        ld8.fill    r19=[in0],-8;;
+        ld8.fill    r18=[in0],-8;;
+        ld8.fill    r17=[in0],-8;;
+        ld8.fill    r16=[in0],-8;;
+        ld8.fill    r15=[in0],-8;;
+        ld8.fill    r14=[in0],-8;;
+        ld8.fill    r13=[in0],-8;;
+        ld8.fill    r12=[in0],-8;;
+        ld8.fill    r11=[in0],-8;;
+        ld8.fill    r10=[in0],-8;;
+        ld8.fill    r9=[in0],-8;;
+        ld8.fill    r8=[in0],-8;;
+        ld8.fill    r7=[in0],-8;;
+        ld8.fill    r6=[in0],-8;;
+        ld8.fill    r5=[in0],-8;;
+        ld8.fill    r4=[in0],-8;;
+        ld8.fill    r3=[in0],-8;;
+        ld8.fill    r2=[in0],-8;;
+        ld8.fill    r1=[in0],-8;;
+        mov         ar.unat=loc2;;          // restore application context unat
+
+        br.ret.sptk.many    b0
+
+        .endp       FillContext
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      HookHandler
+//
+//  Description:
+//      Common branch target from hooked IVT entries.  Runs in interrupt context.
+//      Responsible for saving and restoring context and calling common C
+//      handler.  Banked registers running on bank 0 at entry.
+//
+//  Arguments:
+//      All arguments are passed in banked registers:
+//          B0_REG = Original B0
+//          SCRATCH_REG1 = IVT entry index
+//
+//  Returns:
+//      Returns via rfi
+//
+//  Notes:
+//      loc0 - scratch
+//      loc1 - scratch
+//      loc2 - vector number / mask
+//      loc3 - 16 byte aligned context record address
+//      loc4 - temporary storage of last address in context record
+
+HookHandler:
+        flushrs;;                               // Synch RSE with backing store
+        mov         SCRATCH_REG2=ar.bsp         // save interrupted context bsp
+        mov         SCRATCH_REG3=ar.bspstore    // save interrupted context bspstore
+        mov         SCRATCH_REG4=ar.rnat        // save interrupted context rnat
+        mov         SCRATCH_REG6=cr.ifs;;       // save IFS in case we need to chain...
+        cover;;                                 // creates new frame, moves old
+                                                //   CFM to IFS.
+        alloc       SCRATCH_REG5=0,5,6,0        // alloc 5 locals, 6 outs
+        ;;
+        // save banked registers to locals
+        mov         out1=B0_REG                 // out1 = Original B0
+        mov         out2=SCRATCH_REG2           // out2 = original ar.bsp
+        mov         out3=SCRATCH_REG3           // out3 = original ar.bspstore
+        mov         out4=SCRATCH_REG4           // out4 = original ar.rnat
+        mov         out5=SCRATCH_REG5           // out5 = original ar.pfs
+        mov         loc2=SCRATCH_REG1;;         // loc2 = vector number + chain flag
+        bsw.1;;                                 // switch banked registers to bank 1
+        srlz.d                                  // explicit serialize required
+                                                // now fill in context record structure
+        movl        loc3=IpfContextBuf          // Insure context record is aligned
+        add         loc0=-0x200,r0;;            // mask the lower 9 bits (align on 512 byte boundary)
+        and         loc3=loc3,loc0;;
+        add         loc3=0x200,loc3;;           // move to next 512 byte boundary
+                                                // loc3 now contains the 512 byte aligned context record
+                                                // spill register context into context record
+        mov         out0=loc3;;                 // Context record base in out0
+                                                // original B0 in out1 already
+                                                // original ar.bsp in out2 already
+                                                // original ar.bspstore in out3 already
+        br.call.sptk.few b0=SpillContext;;      // spill context
+        mov         loc4=out0                   // save modified address
+
+    // At this point, the context has been saved to the context record and we're
+    // ready to call the C part of the handler...
+
+        movl        loc0=CommonHandler;;        // obtain address of plabel
+        ld8         loc1=[loc0];;               // get entry point of CommonHandler
+        mov         b6=loc1;;                   // put it in a branch register
+        adds        loc1= 8, loc0;;             // index to GP in plabel
+        ld8         r1=[loc1];;                 // set up gp for C call
+        mov         loc1=0xfffff;;              // mask off so only vector bits are present
+        and         out0=loc2,loc1;;            // pass vector number (exception type)
+        mov         out1=loc3;;                 // pass context record address
+        br.call.sptk.few b0=b6;;                // call C handler
+
+    // We've returned from the C call, so restore the context and either rfi
+    // back to interrupted thread, or chain into the SAL if this was an external interrupt
+        mov         out0=loc4;;                 // pass address of last element in context record
+        br.call.sptk.few b0=FillContext;;       // Fill context
+        mov         b0=out1                     // fill in b0
+        mov         ar.rnat=out4
+        mov         ar.pfs=out5
+
+  // Loadrs is necessary because the debugger may have changed some values in
+  // the backing store.  The processor, however may not be aware that the
+  // stacked registers need to be reloaded from the backing store.  Therefore,
+  // we explicitly cause the RSE to refresh the stacked register's contents
+  // from the backing store.
+        mov         loc0=ar.rsc                 // get RSC value
+        mov         loc1=ar.rsc                 // save it so we can restore it
+        movl        loc3=0xffffffffc000ffff;;   // create mask for clearing RSC.loadrs
+        and         loc0=loc0,loc3;;            // create value for RSC with RSC.loadrs==0
+        mov         ar.rsc=loc0;;               // modify RSC
+        loadrs;;                                // invalidate register stack
+        mov         ar.rsc=loc1;;               // restore original RSC
+
+        bsw.0;;                                 // switch banked registers back to bank 0
+        srlz.d;;                                // explicit serialize required
+        mov         PR_REG=pr                   // save predicates - to be restored after chaining decision
+        mov         B0_REG=b0                   // save b0 - required by chain code
+        mov         loc2=EXCPT_EXTERNAL_INTERRUPT;;
+        cmp.eq      p7,p0=SCRATCH_REG1,loc2;;   // check to see if this is the timer tick
+  (p7)  br.cond.dpnt.few    DO_CHAIN;;
+
+NO_CHAIN:
+        mov         pr=PR_REG;;
+        rfi;;                                   // we're outa here.
+
+DO_CHAIN:
+        mov         pr=PR_REG
+        mov         SCRATCH_REG1=cr.iva
+        mov         SCRATCH_REG2=PATCH_RETURN_OFFSET;;
+        add         SCRATCH_REG1=SCRATCH_REG1, SCRATCH_REG2;;
+        mov         b0=SCRATCH_REG1;;
+        br.cond.sptk.few  b0;;
+
+EndHookHandler:
+
+
+/////////////////////////////////////////////
+//
+//  Name:
+//      HookStub
+//
+//  Description:
+//      HookStub will be copied from it's loaded location into the IVT when
+//      an IVT entry is hooked.  The IVT entry does an indirect jump via B0 to
+//      HookHandler, which in turn calls into the default C handler, which calls
+//      the user-installed C handler.  The calls return and HookHandler executes
+//      an rfi.
+//
+//  Notes:
+//      Saves B0 to B0_REG
+//      Saves IVT index to SCRATCH_REG1 (immediate value is fixed up when code is copied
+//          to the IVT entry.
+
+        ASM_GLOBAL HookStub
+        .proc   HookStub
+HookStub:
+
+        mov         B0_REG=b0
+        movl        SCRATCH_REG1=HookHandler;;
+        mov         b0=SCRATCH_REG1;;
+        mov         SCRATCH_REG1=0;;// immediate value is fixed up during install of handler to be the vector number
+        br.cond.sptk.few b0
+
+        .endp       HookStub
+
+
+/////////////////////////////////////////////
+// The following code is moved into IVT entry 14 (offset 3400) which is reserved
+// in the Itanium architecture.  The patch code is located at the end of the
+// IVT entry.
+
+PatchCode:
+        mov       SCRATCH_REG0=psr
+        mov       SCRATCH_REG6=cr.ipsr
+        mov       PR_REG=pr
+        mov       B0_REG=b0;;
+
+        // turn off any virtual translations
+        movl      SCRATCH_REG1 = ~( MASK(PSR_DT,1) | MASK(PSR_RT,1));;
+        and       SCRATCH_REG1 = SCRATCH_REG0, SCRATCH_REG1;;
+        mov       psr.l = SCRATCH_REG1;;
+        srlz.d
+        tbit.z    p14, p15 = SCRATCH_REG6, PSR_IS;;   // Check to see if we were
+                                                      // interrupted from IA32
+                                                      // context.  If so, bail out
+                                                      // and chain to SAL immediately
+ (p15)  br.cond.sptk.few Stub_IVT_Passthru;;
+        // we only want to take 1 out of 32 external interrupts to minimize the
+        // impact to system performance.  Check our interrupt count and bail
+        // out if we're not up to 32
+        movl      SCRATCH_REG1=ExternalInterruptCount;;
+        ld8       SCRATCH_REG2=[SCRATCH_REG1];;       // ExternalInterruptCount
+        tbit.z    p14, p15 = SCRATCH_REG2, 5;;        // bit 5 set?
+ (p14)  add       SCRATCH_REG2=1, SCRATCH_REG2;;      // No?  Then increment
+                                                      // ExternalInterruptCount
+                                                      // and Chain to SAL
+                                                      // immediately
+ (p14)  st8       [SCRATCH_REG1]=SCRATCH_REG2;;
+ (p14)  br.cond.sptk.few Stub_IVT_Passthru;;
+ (p15)  mov       SCRATCH_REG2=0;;                    // Yes?  Then reset
+                                                        // ExternalInterruptCount
+                                                        // and branch to
+                                                        // HookHandler
+ (p15)  st8       [SCRATCH_REG1]=SCRATCH_REG2;;
+        mov       pr=PR_REG
+        movl      SCRATCH_REG1=HookHandler;;          // SCRATCH_REG1 = entrypoint of HookHandler
+        mov       b0=SCRATCH_REG1;;                   // b0 = entrypoint of HookHandler
+        mov       SCRATCH_REG1=EXCPT_EXTERNAL_INTERRUPT;;
+        br.sptk.few b0;;                                // branch to HookHandler
+
+PatchCodeRet:
+        // fake-up an rfi to get RSE back to being coherent and insure psr has
+        // original contents when interrupt occured, then exit to SAL
+        // at this point:
+        //      cr.ifs has been modified by previous "cover"
+        //      SCRATCH_REG6 has original cr.ifs
+
+        mov       SCRATCH_REG5=cr.ipsr
+        mov       SCRATCH_REG4=cr.iip;;
+        mov       cr.ipsr=SCRATCH_REG0
+        mov       SCRATCH_REG1=ip;;
+        add       SCRATCH_REG1=0x30, SCRATCH_REG1;;
+        mov       cr.iip=SCRATCH_REG1;;
+        rfi;;                       // rfi to next instruction
+
+Stub_RfiTarget:
+        mov       cr.ifs=SCRATCH_REG6
+        mov       cr.ipsr=SCRATCH_REG5
+        mov       cr.iip=SCRATCH_REG4;;
+
+Stub_IVT_Passthru:
+        mov       pr=PR_REG                         // pr = saved predicate registers
+        mov       b0=B0_REG;;                       // b0 = saved b0
+EndPatchCode:
+
+
+/////////////////////////////////////////////
+// The following bundle is moved into IVT entry 14 (offset 0x3400) which is reserved
+// in the Itanium architecture.  This bundle will be the last bundle and will
+// be located at offset 0x37F0 in the IVT.
+
+FailsafeBranch:
+{
+        .mib
+        nop.m     0
+        nop.i     0
+        br.sptk.few -(FAILSAFE_BRANCH_OFFSET - EXT_INT_ENTRY_OFFSET - 0x10)
+}
+
+
+/////////////////////////////////////////////
+// The following bundle is moved into IVT entry 13 (offset 0x3000) which is the
+// external interrupt.  It branches to the patch code.
+
+PatchCodeNewBun0:
+{
+        .mib
+        nop.m     0
+        nop.i     0
+        br.cond.sptk.few PATCH_BRANCH
+}
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Common.i b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Common.i
new file mode 100644
index 0000000000..a11f780125
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Common.i
@@ -0,0 +1,29 @@
+/// @file
+///  This is set of useful macros.
+///
+/// Copyright (c) 2006, 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.
+///
+/// Module Name: Common.i
+///
+///
+
+
+#define NESTED_SETUP(i,l,o,r)               \
+         alloc loc1=ar##.##pfs,i,l,o,r ;    \
+         mov loc0=b0 ;;
+
+
+#define NESTED_RETURN                       \
+         mov b0=loc0 ;                      \
+         mov ar##.##pfs=loc1 ;;             \
+         br##.##ret##.##dpnt  b0 ;;
+
+#define MASK(bp,value)  (value << bp)
+
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Ds64Macros.i b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Ds64Macros.i
new file mode 100644
index 0000000000..8ce97f32c2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/Ds64Macros.i
@@ -0,0 +1,78 @@
+/// @file
+///  This is set of macros used in calculating offsets in the IVT.
+///
+/// Copyright (c) 2006 - 2008, 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.
+///
+///
+
+
+#define EXCPT_EXTERNAL_INTERRUPT 12
+#define MASK_0_4        0x000000000000001F  // mask bits 0 through 4
+#define SLOT0           0
+#define SLOT1           1
+#define SLOT2           2
+
+#define PSR_DT          17
+#define PSR_TB          26
+#define PSR_RT          27
+#define PSR_IS          34
+#define PSR_IT          36
+#define PSR_IC          13
+#define PSR_I           14
+#define PSR_SS          40
+#define PSR_BN          44
+#define PSR_RI_MASK     0x60000000000
+
+#define EXCPT_EXTERNAL_INTERRUPT 12
+
+#define SCRATCH_REG0    r23
+#define SCRATCH_REG1    r24
+#define SCRATCH_REG2    r25
+#define SCRATCH_REG3    r26
+#define SCRATCH_REG4    r27
+#define SCRATCH_REG5    r28
+#define SCRATCH_REG6    r29
+#define PR_REG          r30
+#define B0_REG          r31
+
+
+// EXT_INT_OFFSET is the offset of the external interrupt entry in the IVT
+#define EXT_INT_ENTRY_OFFSET    0x03000
+
+// PATCH_ENTRY_OFFSET is the offset into the IVT of the entry that is coopted (stolen)
+// for use by the handler.  The entire entry is restored when the handler is
+// unloaded.
+#define PATCH_ENTRY_OFFSET      0x03400
+
+// PATCH_CODE_SIZE is the size of patch code
+#define PATCH_CODE_SIZE       (EndPatchCode - PatchCode)
+
+// A hard coded branch back into the external interrupt IVT entry's second bundle
+// is put here, just in case the original bundle zero did not have a branch
+// This is the last bundle in the reserved IVT entry
+#define FAILSAFE_BRANCH_OFFSET  (PATCH_ENTRY_OFFSET + 0x400 - 0x10)
+
+// the original external interrupt IVT entry bundle zero is copied and relocated
+// here... also in the reserved IVT entry
+// This is the second-to-last bundle in the reserved IVT entry
+#define RELOCATED_EXT_INT       (PATCH_ENTRY_OFFSET + 0x400 - 0x20)
+
+// The patch is actually stored at the end of IVT:PATCH_ENTRY.  The PATCH_OFFSET
+// is the offset into IVT where the patch is actually stored.  It is carefully
+// located so that when we run out of patch code, the next bundle is the
+// relocated bundle 0 from the original external interrupt handler
+#define PATCH_OFFSET            (PATCH_ENTRY_OFFSET + 0x400 - ( EndPatchCode - PatchCode ) - 0x20)
+
+#define PATCH_RETURN_OFFSET     (PATCH_ENTRY_OFFSET + 0x400 - ( EndPatchCode - PatchCodeRet ) - 0x20)
+
+// PATCH_BRANCH is used only in the new bundle that is placed at the beginning
+// of the external interrupt IVT entry.
+#define PATCH_BRANCH            (PATCH_OFFSET - EXT_INT_ENTRY_OFFSET)
+
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.c b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.c
new file mode 100644
index 0000000000..44f59e8ec8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.c
@@ -0,0 +1,467 @@
+/** @file
+  IPF specific functions to support Debug Support protocol.
+
+Copyright (c) 2006 - 2010, 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 "PlDebugSupport.h"
+
+BOOLEAN  mInHandler = FALSE;
+
+//
+// number of bundles to swap in ivt
+//
+#define NUM_BUNDLES_IN_STUB 5
+#define NUM_IVT_ENTRIES     64
+
+typedef struct {
+  BUNDLE         OrigBundles[NUM_BUNDLES_IN_STUB];
+  CALLBACK_FUNC  RegisteredCallback;
+} IVT_ENTRY;
+
+IVT_ENTRY IvtEntryTable[NUM_IVT_ENTRIES];
+
+//
+// IPF context record is overallocated by 512 bytes to guarantee a 512 byte alignment exists
+// within the buffer and still have a large enough buffer to hold a whole IPF context record.
+//
+UINT8     IpfContextBuf[sizeof (EFI_SYSTEM_CONTEXT_IPF) + 512];
+
+//
+// The PatchSaveBuffer is used to store the original bundles from the IVT where it is patched
+// with the common handler.
+//
+UINT8     PatchSaveBuffer[0x400];
+UINTN     ExternalInterruptCount;
+
+
+/**
+  IPF specific DebugSupport driver initialization.
+
+  Must be public because it's referenced from DebugSupport.c
+
+  @retval  EFI_SUCCESS     Always.
+
+**/
+EFI_STATUS
+PlInitializeDebugSupportDriver (
+  VOID
+  )
+{
+  ZeroMem (IvtEntryTable, sizeof (IvtEntryTable));
+  ExternalInterruptCount = 0;
+  return EFI_SUCCESS;
+}
+
+/**
+  Unload handler that is called during UnloadImage() - deallocates pool memory
+  used by the driver.
+
+  Must be public because it's referenced from DebugSuport.c
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+
+  @retval EFI_SUCCESS    Always.
+
+**/
+EFI_STATUS
+EFIAPI
+PlUnloadDebugSupportDriver (
+  IN EFI_HANDLE       ImageHandle
+  )
+{
+  EFI_EXCEPTION_TYPE  ExceptionType;
+
+  for (ExceptionType = 0; ExceptionType < NUM_IVT_ENTRIES; ExceptionType++) {
+    ManageIvtEntryTable (ExceptionType, NULL, NULL);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  C routine that is called for all registered exceptions.  This is the main
+  exception dispatcher.
+
+  Must be public because it's referenced from AsmFuncs.s.
+
+  @param  ExceptionType        Specifies which processor exception.
+  @param  Context              System Context.
+**/
+VOID
+CommonHandler (
+  IN EFI_EXCEPTION_TYPE ExceptionType,
+  IN EFI_SYSTEM_CONTEXT Context
+  )
+{
+  DEBUG_CODE_BEGIN ();
+    if (mInHandler) {
+      DEBUG ((EFI_D_INFO, "ERROR: Re-entered debugger!\n"
+                                    "       ExceptionType == %X\n"
+                                    "       Context       == %X\n"
+                                    "       Context.SystemContextIpf->CrIip  == %LX\n"
+                                    "       Context.SystemContextIpf->CrIpsr == %LX\n"
+                                    "       mInHandler     == %X\n",
+                                    (INT32)ExceptionType,
+                                    Context,
+                                    Context.SystemContextIpf->CrIip,
+                                    Context.SystemContextIpf->CrIpsr,
+                                    mInHandler));
+    }
+  DEBUG_CODE_END ();
+
+  ASSERT (!mInHandler);
+  mInHandler = TRUE;
+  if (IvtEntryTable[ExceptionType].RegisteredCallback != NULL) {
+    if (ExceptionType != EXCEPT_IPF_EXTERNAL_INTERRUPT) {
+      IvtEntryTable[ExceptionType].RegisteredCallback (ExceptionType, Context.SystemContextIpf);
+    } else {
+      IvtEntryTable[ExceptionType].RegisteredCallback (Context.SystemContextIpf);
+    }
+  } else {
+    ASSERT (0);
+  }
+
+  mInHandler = FALSE;
+}
+
+/**
+  Given an integer number, return the physical address of the entry point in the IFT.
+
+  @param  HandlerIndex       Index of the Handler
+  @param  EntryPoint         IFT Entrypoint
+
+**/
+VOID
+GetHandlerEntryPoint (
+  UINTN   HandlerIndex,
+  VOID    **EntryPoint
+  )
+{
+  UINT8 *TempPtr;
+
+  //
+  // get base address of IVT
+  //
+  TempPtr = GetIva ();
+
+  if (HandlerIndex < 20) {
+    //
+    // first 20 provide 64 bundles per vector
+    //
+    TempPtr += 0x400 * HandlerIndex;
+  } else {
+    //
+    // the rest provide 16 bundles per vector
+    //
+    TempPtr += 0x5000 + 0x100 * (HandlerIndex - 20);
+  }
+
+  *EntryPoint = (VOID *) TempPtr;
+}
+
+/**
+  This is the worker function that uninstalls and removes all handlers.
+
+  @param  ExceptionType     Specifies which processor exception.
+  @param  NewBundles        New Boundles.
+  @param  NewCallback       A pointer to the new function to be registered.
+
+  @retval EFI_ALEADY_STARTED Ivt already hooked.
+  @retval EFI_SUCCESS        Successfully uninstalled.
+
+**/
+EFI_STATUS
+ManageIvtEntryTable (
+  IN  EFI_EXCEPTION_TYPE           ExceptionType,
+  IN  BUNDLE                       NewBundles[NUM_BUNDLES_IN_STUB],
+  IN  CALLBACK_FUNC                NewCallback
+  )
+{
+  BUNDLE  *B0Ptr;
+  UINT64  InterruptFlags;
+  EFI_TPL OldTpl;
+
+  //
+  // Get address of bundle 0
+  //
+  GetHandlerEntryPoint (ExceptionType, (VOID **) &B0Ptr);
+
+  if (IvtEntryTable[ExceptionType].RegisteredCallback != NULL) {
+    //
+    // we've already installed to this vector
+    //
+    if (NewCallback != NULL) {
+      //
+      // if the input handler is non-null, error
+      //
+      return EFI_ALREADY_STARTED;
+    } else {
+      //
+      // else remove the previously installed handler
+      //
+      OldTpl          = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+      InterruptFlags  = ProgramInterruptFlags (DISABLE_INTERRUPTS);
+      if (ExceptionType == EXCEPT_IPF_EXTERNAL_INTERRUPT) {
+        UnchainExternalInterrupt ();
+      } else {
+        UnhookEntry (ExceptionType);
+      }
+
+      ProgramInterruptFlags (InterruptFlags);
+      gBS->RestoreTPL (OldTpl);
+      //
+      // re-init IvtEntryTable
+      //
+      ZeroMem (&IvtEntryTable[ExceptionType], sizeof (IVT_ENTRY));
+    }
+  } else {
+    //
+    // no user handler installed on this vector
+    //
+    if (NewCallback != NULL) {
+      OldTpl          = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+      InterruptFlags  = ProgramInterruptFlags (DISABLE_INTERRUPTS);
+      if (ExceptionType == EXCEPT_IPF_EXTERNAL_INTERRUPT) {
+        ChainExternalInterrupt (NewCallback);
+      } else {
+        HookEntry (ExceptionType, NewBundles, NewCallback);
+      }
+
+      ProgramInterruptFlags (InterruptFlags);
+      gBS->RestoreTPL (OldTpl);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Saves original IVT contents and inserts a few new bundles which are fixed up
+  to store the ExceptionType and then call the common handler.
+
+  @param  ExceptionType      Specifies which processor exception.
+  @param  NewBundles         New Boundles.
+  @param  NewCallback        A pointer to the new function to be hooked.
+
+**/
+VOID
+HookEntry (
+  IN  EFI_EXCEPTION_TYPE  ExceptionType,
+  IN  BUNDLE              NewBundles[4],
+  IN  CALLBACK_FUNC       NewCallback
+  )
+{
+  BUNDLE  *FixupBundle;
+  BUNDLE  *B0Ptr;
+
+  //
+  // Get address of bundle 0
+  //
+  GetHandlerEntryPoint (ExceptionType, (VOID **) &B0Ptr);
+
+  //
+  // copy original bundles from IVT to IvtEntryTable so we can restore them later
+  //
+  CopyMem (
+    IvtEntryTable[ExceptionType].OrigBundles,
+    B0Ptr,
+    sizeof (BUNDLE) * NUM_BUNDLES_IN_STUB
+    );
+  //
+  // insert new B0
+  //
+  CopyMem (B0Ptr, NewBundles, sizeof (BUNDLE) * NUM_BUNDLES_IN_STUB);
+
+  //
+  // fixup IVT entry so it stores its index and whether or not to chain...
+  //
+  FixupBundle = B0Ptr + 2;
+  FixupBundle->High |= ExceptionType << 36;
+
+  InstructionCacheFlush (B0Ptr, 5);
+  IvtEntryTable[ExceptionType].RegisteredCallback = NewCallback;
+}
+
+/**
+  Restores original IVT contents when unregistering a callback function.
+
+  @param  ExceptionType     Specifies which processor exception.
+
+**/
+VOID
+UnhookEntry (
+  IN  EFI_EXCEPTION_TYPE  ExceptionType
+  )
+{
+  BUNDLE  *B0Ptr;
+
+  //
+  // Get address of bundle 0
+  //
+  GetHandlerEntryPoint (ExceptionType, (VOID **) &B0Ptr);
+  //
+  // restore original bundles in IVT
+  //
+  CopyMem (
+    B0Ptr,
+    IvtEntryTable[ExceptionType].OrigBundles,
+    sizeof (BUNDLE) * NUM_BUNDLES_IN_STUB
+    );
+  InstructionCacheFlush (B0Ptr, 5);
+}
+
+/**
+  Sets up cache flush and calls assembly function to chain external interrupt.
+
+  Records new callback in IvtEntryTable.
+
+  @param  NewCallback     A pointer to the interrupt handle.
+
+**/
+VOID
+ChainExternalInterrupt (
+  IN  CALLBACK_FUNC  NewCallback
+  )
+{
+  VOID  *Start;
+
+  Start = (VOID *) ((UINT8 *) GetIva () + 0x400 * EXCEPT_IPF_EXTERNAL_INTERRUPT + 0x400);
+  IvtEntryTable[EXCEPT_IPF_EXTERNAL_INTERRUPT].RegisteredCallback = NewCallback;
+  ChainHandler ();
+  InstructionCacheFlush (Start, 0x400);
+}
+
+/**
+  Sets up cache flush and calls assembly function to restore external interrupt.
+  Removes registered callback from IvtEntryTable.
+
+**/
+VOID
+UnchainExternalInterrupt (
+  VOID
+  )
+{
+  VOID  *Start;
+
+  Start = (VOID *) ((UINT8 *) GetIva () + 0x400 * EXCEPT_IPF_EXTERNAL_INTERRUPT + 0x400);
+  UnchainHandler ();
+  InstructionCacheFlush (Start, 0x400);
+  IvtEntryTable[EXCEPT_IPF_EXTERNAL_INTERRUPT].RegisteredCallback = NULL;
+}
+
+/**
+  Returns the maximum value that may be used for the ProcessorIndex parameter in
+  RegisterPeriodicCallback() and RegisterExceptionCallback().
+
+  Hard coded to support only 1 processor for now.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the maximum supported
+                                processor index is returned. Always 0 returned.
+
+  @retval EFI_SUCCESS           Always returned with **MaxProcessorIndex set to 0.
+
+**/
+EFI_STATUS
+EFIAPI
+GetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL    *This,
+  OUT UINTN                        *MaxProcessorIndex
+  )
+{
+  *MaxProcessorIndex = 0;
+  return (EFI_SUCCESS);
+}
+
+/**
+  Registers a function to be called back periodically in interrupt context.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  PeriodicCallback      A pointer to a function of type PERIODIC_CALLBACK that is the main
+                                periodic entry point of the debug agent.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL     *This,
+  IN UINTN                          ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK          PeriodicCallback
+  )
+{
+  return ManageIvtEntryTable (EXCEPT_IPF_EXTERNAL_INTERRUPT, NULL, PeriodicCallback);
+}
+
+/**
+  Registers a function to be called when a given processor exception occurs.
+
+  This code executes in boot services context.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  ExceptionCallback     A pointer to a function of type EXCEPTION_CALLBACK that is called
+                                when the processor exception specified by ExceptionType occurs.
+  @param  ExceptionType         Specifies which processor exception to hook.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL    *This,
+  IN UINTN                         ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK        ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE            ExceptionType
+  )
+{
+  return ManageIvtEntryTable (
+          ExceptionType,
+          (BUNDLE *) ((EFI_PLABEL *) HookStub)->EntryPoint,
+          ExceptionCallback
+          );
+}
+
+/**
+  Invalidates processor instruction cache for a memory range. Subsequent execution in this range
+  causes a fresh memory fetch to retrieve code to be executed.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor's instruction cache is to be invalidated.
+  @param  Start                 Specifies the physical base of the memory range to be invalidated.
+  @param  Length                Specifies the minimum number of bytes in the processor's instruction
+                                cache to invalidate.
+
+  @retval EFI_SUCCESS           Always returned.
+
+**/
+EFI_STATUS
+EFIAPI
+InvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL    *This,
+  IN UINTN                         ProcessorIndex,
+  IN VOID                          *Start,
+  IN UINTN                         Length
+  )
+{
+  InstructionCacheFlush (Start, Length);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.h b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.h
new file mode 100644
index 0000000000..0cf29cadfb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/Ipf/PlDebugSupport.h
@@ -0,0 +1,324 @@
+/** @file
+  IPF specific types, macros, and definitions for Debug Support Driver.
+
+Copyright (c) 2004 - 2010, 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.
+
+**/
+
+#ifndef _PLDEBUG_SUPPORT_H_
+#define _PLDEBUG_SUPPORT_H_
+
+#include <Uefi.h>
+
+#include <Protocol/DebugSupport.h>
+#include <Protocol/LoadedImage.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#define DISABLE_INTERRUPTS  0UL
+
+#define EFI_ISA IsaIpf
+
+typedef struct {
+  UINT64  Low;
+  UINT64  High;
+} BUNDLE;
+
+typedef
+VOID
+(*CALLBACK_FUNC) (
+  );
+
+/**
+  IPF specific DebugSupport driver initialization.
+
+  Must be public because it's referenced from DebugSupport.c
+
+  @retval  EFI_SUCCESS     Always.
+
+**/
+EFI_STATUS
+PlInitializeDebugSupportDriver (
+  VOID
+  );
+
+/**
+  Unload handler that is called during UnloadImage() - deallocates pool memory
+  used by the driver.
+
+  Must be public because it's referenced from DebugSuport.c
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+
+  @retval EFI_SUCCESS    Always.
+
+**/
+EFI_STATUS
+EFIAPI
+PlUnloadDebugSupportDriver (
+  IN EFI_HANDLE                   ImageHandle
+  );
+
+/**
+  C callable function to obtain the current value of IVA.
+
+  @return Current value of IVA.
+
+**/
+VOID  *
+GetIva (
+  VOID
+  );
+
+/**
+  C callable function that HookStub will be copied from it's loaded location into the IVT when
+  an IVT entry is hooked.
+
+**/
+VOID
+HookStub (
+  VOID
+  );
+
+/**
+  C callable function to chain an interrupt handler.
+
+**/
+VOID
+ChainHandler (
+  VOID
+  );
+
+/**
+  C callable function to unchain an interrupt handler.
+
+**/
+VOID
+UnchainHandler (
+  VOID
+  );
+
+/**
+  C callable function to enable/disable interrupts.
+
+  @param  NewInterruptState   New Interrupt State.
+
+  @return Previous state of psr.ic.
+
+**/
+UINT64
+ProgramInterruptFlags (
+  IN UINT64                       NewInterruptState
+  );
+
+/**
+  Flushes instruction cache for specified number of bytes.
+
+  @param  StartAddress     Cache Start Address.
+  @param  SizeInBytes      Cache Size.
+
+**/
+VOID
+InstructionCacheFlush (
+  IN VOID    *StartAddress,
+  IN UINTN   SizeInBytes
+  );
+
+/**
+  Returns the maximum value that may be used for the ProcessorIndex parameter in
+  RegisterPeriodicCallback() and RegisterExceptionCallback().
+
+  Hard coded to support only 1 processor for now.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the maximum supported
+                                processor index is returned. Always 0 returned.
+
+  @retval EFI_SUCCESS           Always returned with **MaxProcessorIndex set to 0.
+
+**/
+EFI_STATUS
+EFIAPI
+GetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL   *This,
+  OUT UINTN                       *MaxProcessorIndex
+  );
+
+/**
+  Registers a function to be called back periodically in interrupt context.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  PeriodicCallback      A pointer to a function of type PERIODIC_CALLBACK that is the main
+                                periodic entry point of the debug agent.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL   *This,
+  IN UINTN                        ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK        PeriodicCallback
+  );
+
+/**
+  Registers a function to be called when a given processor exception occurs.
+
+  This code executes in boot services context.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor the callback function applies to.
+  @param  ExceptionCallback     A pointer to a function of type EXCEPTION_CALLBACK that is called
+                                when the processor exception specified by ExceptionType occurs.  
+  @param  ExceptionType         Specifies which processor exception to hook.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a callback
+                                function was previously registered.
+  @retval EFI_OUT_OF_RESOURCES  System has insufficient memory resources to register new callback
+                                function.
+**/
+EFI_STATUS
+EFIAPI
+RegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL   *This,
+  IN UINTN                        ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK       ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE           ExceptionType
+  );
+
+/**
+  Invalidates processor instruction cache for a memory range. Subsequent execution in this range
+  causes a fresh memory fetch to retrieve code to be executed.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL instance.
+  @param  ProcessorIndex        Specifies which processor's instruction cache is to be invalidated.
+  @param  Start                 Specifies the physical base of the memory range to be invalidated.
+  @param  Length                Specifies the minimum number of bytes in the processor's instruction
+                                cache to invalidate.
+
+  @retval EFI_SUCCESS           Always returned.
+
+**/
+EFI_STATUS
+EFIAPI
+InvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL   *This,
+  IN UINTN                        ProcessorIndex,
+  IN VOID                         *Start,
+  IN UINTN                        Length
+  );
+
+/**
+  C routine that is called for all registered exceptions.  This is the main
+  exception dispatcher.
+
+  Must be public because it's referenced from AsmFuncs.s.
+
+  @param  ExceptionType        Specifies which processor exception.
+  @param  Context              System Context.
+**/
+VOID
+CommonHandler (
+  IN EFI_EXCEPTION_TYPE ExceptionType,
+  IN EFI_SYSTEM_CONTEXT Context
+  );
+
+/**
+  This is the worker function that uninstalls and removes all handlers.
+
+  @param  ExceptionType     Specifies which processor exception.
+  @param  NewBundles        New Boundles.
+  @param  NewCallback       A pointer to the new function to be registered.
+
+  @retval EFI_ALEADY_STARTED Ivt already hooked.
+  @retval EFI_SUCCESS        Successfully uninstalled.
+
+**/
+EFI_STATUS
+ManageIvtEntryTable (
+  IN  EFI_EXCEPTION_TYPE    ExceptionType,
+  IN  BUNDLE                NewBundles[4],
+  IN  CALLBACK_FUNC         NewCallback
+  );
+
+/**
+  Saves original IVT contents and inserts a few new bundles which are fixed up
+  to store the ExceptionType and then call the common handler.
+
+  @param  ExceptionType      Specifies which processor exception.
+  @param  NewBundles         New Boundles.
+  @param  NewCallback        A pointer to the new function to be hooked.
+
+**/
+VOID
+HookEntry (
+  IN  EFI_EXCEPTION_TYPE    ExceptionType,
+  IN  BUNDLE                NewBundles[4],
+  IN  CALLBACK_FUNC         NewCallback
+  );
+
+/**
+  Restores original IVT contents when unregistering a callback function.
+
+  @param  ExceptionType     Specifies which processor exception.
+
+**/
+VOID
+UnhookEntry (
+  IN  EFI_EXCEPTION_TYPE    ExceptionType
+  );
+
+/**
+  Sets up cache flush and calls assembly function to chain external interrupt.
+
+  Records new callback in IvtEntryTable.
+
+  @param  NewCallback     A pointer to the interrupt handle.
+
+**/
+VOID
+ChainExternalInterrupt (
+  IN  CALLBACK_FUNC         NewCallback
+  );
+
+/**
+  Sets up cache flush and calls assembly function to restore external interrupt.
+  Removes registered callback from IvtEntryTable.
+
+**/
+VOID
+UnchainExternalInterrupt (
+  VOID
+  );
+
+/**
+  Given an integer number, return the physical address of the entry point in the IFT.
+
+  @param  HandlerIndex       Index of the Handler
+  @param  EntryPoint         IFT Entrypoint
+
+**/
+VOID
+GetHandlerEntryPoint (
+  UINTN                     HandlerIndex,
+  VOID                      **EntryPoint
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.S b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.S
new file mode 100644
index 0000000000..7f0919ee1b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.S
@@ -0,0 +1,551 @@
+///**@file
+// Low leve x64 specific debug support functions.
+//
+// Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+// Portions copyright (c) 2008 - 2009, Apple Inc. 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.
+//
+//**/
+
+ASM_GLOBAL ASM_PFX(OrigVector)
+ASM_GLOBAL ASM_PFX(InterruptEntryStub)
+ASM_GLOBAL ASM_PFX(StubSize)
+ASM_GLOBAL ASM_PFX(CommonIdtEntry)
+ASM_GLOBAL ASM_PFX(FxStorSupport)
+
+.data
+
+ASM_PFX(StubSize):         .long   ASM_PFX(InterruptEntryStubEnd) - ASM_PFX(InterruptEntryStub)
+ASM_PFX(AppRsp):           .long   0x11111111 # ?
+                           .long   0x11111111 # ?
+ASM_PFX(DebugRsp):         .long   0x22222222 # ?
+                           .long   0x22222222 # ?
+ASM_PFX(ExtraPush):        .long   0x33333333 # ?
+                           .long   0x33333333 # ?
+ASM_PFX(ExceptData):       .long   0x44444444 # ?
+                           .long   0x44444444 # ?
+ASM_PFX(Rflags):           .long   0x55555555 # ?
+                           .long   0x55555555 # ?
+ASM_PFX(OrigVector):       .long   0x66666666 # ?
+                           .long   0x66666666 # ?
+
+// The declarations below define the memory region that will be used for the debug stack.
+// The context record will be built by pushing register values onto this stack.
+// It is imparitive that alignment be carefully managed, since the FXSTOR and
+// FXRSTOR instructions will GP fault if their memory operand is not 16 byte aligned.
+//
+// The stub will switch stacks from the application stack to the debuger stack
+// and pushes the exception number.
+//
+// Then we building the context record on the stack. Since the stack grows down,
+// we push the fields of the context record from the back to the front.  There
+// are 336 bytes of stack used prior allocating the 512 bytes of stack to be
+// used as the memory buffer for the fxstor instruction. Therefore address of
+// the buffer used for the FXSTOR instruction is &Eax - 336 - 512, which
+// must be 16 byte aligned.
+//
+// We carefully locate the stack to make this happen.
+//
+// For reference, the context structure looks like this:
+//      struct {
+//        UINT64            ExceptionData;
+//        FX_SAVE_STATE_X64 FxSaveState;    // 512 bytes, must be 16 byte aligned
+//        UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+//        UINT64            Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+//        UINT64            RFlags;
+//        UINT64            Ldtr, Tr;
+//        UINT64            Gdtr[2], Idtr[2];
+//        UINT64            Rip;
+//        UINT64            Gs, Fs, Es, Ds, Cs, Ss;
+//        UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+//        UINT64            R8, R9, R10, R11, R12, R13, R14, R15;
+//      } SYSTEM_CONTEXT_X64;  // 64 bit system context record
+
+.p2align 4
+DebugStackEnd : .ascii     "DbgStkEnd >>>>>>"    # 16 byte long string - must be 16 bytes to preserve alignment
+                .fill 0x1ffc, 4, 0x00000000
+                                                 # 32K should be enough stack
+                                                 #   This allocation is coocked to insure
+                                                 #   that the the buffer for the FXSTORE instruction
+                                                 #   will be 16 byte aligned also.
+                                                 #
+ASM_PFX(ExceptionNumber):  .long   0x77777777    # first entry will be the vector number pushed by the stub
+                           .long   0x77777777    # ?
+
+DebugStackBegin : .ascii    "<<<< DbgStkBegin"   # initial debug ESP == DebugStackBegin, set in stub
+
+
+.text
+
+//------------------------------------------------------------------------------
+// BOOLEAN
+// FxStorSupport (
+//   void
+//   )
+//
+// Abstract: Returns TRUE if FxStor instructions are supported
+//
+ASM_GLOBAL ASM_PFX(FxStorSupport)
+ASM_PFX(FxStorSupport):
+//
+// cpuid corrupts rbx which must be preserved per the C calling convention
+//
+                pushq   %rbx
+                movq    $1, %rax
+                cpuid
+                movl    %edx, %eax
+                andq    $0x01000000, %rax
+                shrq    $24, %rax
+                popq    %rbx
+                ret
+//------------------------------------------------------------------------------
+// void
+// Vect2Desc (
+//   IA32_IDT_GATE_DESCRIPTOR * DestDesc,  // rcx
+//   void (*Vector) (void)   // rdx
+//   )
+//
+// Abstract: Encodes an IDT descriptor with the given physical address
+//
+ASM_GLOBAL ASM_PFX(Vect2Desc)
+ASM_PFX(Vect2Desc):
+                movq    %rdx, %rax
+                movw    %ax, (%rcx)                  # write bits 15..0 of offset
+                movw    %cs, %dx
+                movw    %dx, 2(%rcx)                 # SYS_CODE_SEL from GDT
+                movw    $(0x0e00 | 0x8000), 4(%rcx)  # type = 386 interrupt gate, present
+                shrq    $16, %rax
+                movw    %ax, 6(%rcx)                 # write bits 31..16 of offset
+                shrq    $16, %rax
+                movl    %eax, 8(%rcx)                # write bits 63..32 of offset
+
+                ret
+
+//------------------------------------------------------------------------------
+// InterruptEntryStub
+//
+// Abstract: This code is not a function, but is a small piece of code that is
+//               copied and fixed up once for each IDT entry that is hooked.
+//
+ASM_GLOBAL ASM_PFX(InterruptEntryStub)
+ASM_PFX(InterruptEntryStub):
+
+                pushq  $0                       # push vector number - will be modified before installed
+                jmp     ASM_PFX(CommonIdtEntry)
+
+ASM_GLOBAL ASM_PFX(InterruptEntryStubEnd)
+ASM_PFX(InterruptEntryStubEnd):
+
+//------------------------------------------------------------------------------
+// CommonIdtEntry
+//
+// Abstract: This code is not a function, but is the common part for all IDT
+//               vectors.
+//
+ASM_GLOBAL ASM_PFX(CommonIdtEntry)
+//
+// At this point, the stub has saved the current application stack esp into AppRsp
+// and switched stacks to the debug stack, where it pushed the vector number
+//
+// The application stack looks like this:
+//
+//              ...
+//              (last application stack entry)
+//              [16 bytes alignment, do not care it]
+//              SS from interrupted task
+//              RSP from interrupted task
+//              rflags from interrupted task
+//              CS from interrupted task
+//              RIP from interrupted task
+//              Error code <-------------------- Only present for some exeption types
+//
+//              Vector Number <----------------- pushed in our IDT Entry
+//
+
+
+// The stub switched us to the debug stack and pushed the interrupt number.
+//
+// Next, construct the context record.  It will be build on the debug stack by
+// pushing the registers in the correct order so as to create the context structure
+// on the debug stack.  The context record must be built from the end back to the
+// beginning because the stack grows down...
+//
+// For reference, the context record looks like this:
+//
+// typedef
+// struct {
+//   UINT64            ExceptionData;
+//   FX_SAVE_STATE_X64 FxSaveState;
+//   UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+//   UINT64            Cr0, Cr2, Cr3, Cr4, Cr8;
+//   UINT64            RFlags;
+//   UINT64            Ldtr, Tr;
+//   UINT64            Gdtr[2], Idtr[2];
+//   UINT64            Rip;
+//   UINT64            Gs, Fs, Es, Ds, Cs, Ss;
+//   UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+//   UINT64            R8, R9, R10, R11, R12, R13, R14, R15;
+// } SYSTEM_CONTEXT_X64;  // 64
+ASM_PFX(CommonIdtEntry):
+// NOTE: we save rsp here to prevent compiler put rip reference cause error AppRsp
+                pushq   %rax
+                movq    (8)(%rsp), %rax                  # save vector number
+                movq    %rax, ASM_PFX(ExceptionNumber)(%rip)   # save vector number
+                popq    %rax
+                addq    $8, %rsp                         # pop vector number
+                movq    %rsp, ASM_PFX(AppRsp)(%rip)      # save stack top
+                movq    DebugStackBegin(%rip), %rsp      # switch to debugger stack
+                subq    $8, %rsp                         # leave space for vector number
+// UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+// UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                pushq    %r15
+                pushq    %r14
+                pushq    %r13
+                pushq    %r12
+                pushq    %r11
+                pushq    %r10
+                pushq    %r9
+                pushq    %r8
+                pushq    %rax
+                pushq    %rcx
+                pushq    %rdx
+                pushq    %rbx
+                pushq    %rsp
+                pushq    %rbp
+                pushq    %rsi
+                pushq    %rdi
+// Save interrupt state rflags register...
+                pushfq
+                popq    %rax
+                movq    %rax, ASM_PFX(Rflags)(%rip)
+// We need to determine if any extra data was pushed by the exception, and if so, save it
+// To do this, we check the exception number pushed by the stub, and cache the
+// result in a variable since we'll need this again.
+                cmpl    $0, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $10, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $11, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $12, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $13, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $14, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                cmpl    $17, ASM_PFX(ExceptionNumber)(%rip)
+                jz      ExtraPushOne
+                movl    $0, ASM_PFX(ExtraPush)(%rip)
+                movl    $0, ASM_PFX(ExceptData)(%rip)
+                jmp     ExtraPushDone
+ExtraPushOne:
+                movl  $1, ASM_PFX(ExtraPush)(%rip)
+
+// If there's some extra data, save it also, and modify the saved AppRsp to effectively
+// pop this value off the application's stack.
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    (%rax), %rbx
+                movq    %rbx, ASM_PFX(ExceptData)(%rip)
+                addq    $8, %rax
+                movq    %rax, ASM_PFX(AppRsp)(%rip)
+
+ExtraPushDone:
+
+// The "push" above pushed the debug stack rsp.  Since what we're actually doing
+// is building the context record on the debug stack, we need to save the pushed
+// debug RSP, and replace it with the application's last stack entry...
+                movq    24(%rsp), %rax
+                movq    %rax, ASM_PFX(DebugRsp)(%rip)
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    24(%rax), %rax
+                # application stack has ss, rsp, rflags, cs, & rip, so
+                # last actual application stack entry is saved at offset
+                # 24 bytes from stack top.
+                movq    %rax, 24(%rsp)
+
+// continue building context record
+// UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+                mov     %ss, %rax
+                pushq   %rax
+                # CS from application is one entry back in application stack
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movzxw  8(%rax), %rax
+                pushq   %rax
+
+                mov     %ds, %rax
+                pushq   %rax
+                mov     %es, %rax
+                pushq   %rax
+                mov     %fs, %rax
+                pushq   %rax
+                mov     %gs, %rax
+                pushq   %rax
+// UINT64  Rip;
+                # Rip from application is on top of application stack
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                pushq   (%rax)
+// UINT64  Gdtr[2], Idtr[2];
+                push    $0
+                push    $0
+                sidtq   (%rsp)
+                push    $0
+                push    $0
+                sgdtq   (%rsp)
+
+// UINT64  Ldtr, Tr;
+                xorq    %rax, %rax
+                str     %ax
+                pushq   %rax
+                sldt    %ax
+                pushq   %rax
+
+// UINT64  RFlags;
+// Rflags from application is two entries back in application stack
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                pushq   16(%rax)
+// UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+// insure FXSAVE/FXRSTOR is enabled in CR4...
+// ... while we're at it, make sure DE is also enabled...
+                movq    %cr8, %rax
+                pushq   %rax
+                movq    %cr4, %rax
+                orq     $0x208, %rax
+                movq    %rax, %cr4
+                pushq   %rax
+                movq    %cr3, %rax
+                pushq   %rax
+                movq    %cr2, %rax
+                pushq   %rax
+                push    $0
+                movq    %cr0, %rax
+                pushq   %rax
+// UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                movq    %dr7, %rax
+                pushq   %rax
+// clear Dr7 while executing debugger itself
+                xorq    %rax, %rax
+                movq    %rax, %dr7
+
+                movq    %dr6, %rax
+                pushq   %rax
+// insure all status bits in dr6 are clear...
+                xorq    %rax, %rax
+                movq    %rax, %dr6
+
+                movq    %dr3, %rax
+                pushq   %rax
+                movq    %dr2, %rax
+                pushq   %rax
+                movq    %dr1, %rax
+                pushq   %rax
+                movq    %dr0, %rax
+                pushq   %rax
+
+// FX_SAVE_STATE_X64 FxSaveState;
+                subq    $512, %rsp
+                movq    %rsp, %rdi
+                # IMPORTANT!! The debug stack has been carefully constructed to
+                # insure that rsp and rdi are 16 byte aligned when we get here.
+                # They MUST be.  If they are not, a GP fault will occur.
+
+                #     FXSTOR_RDI
+                fxsave      (%rdi)
+
+// UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear
+                cld
+                
+// UINT64  ExceptionData;
+                movq    ASM_PFX(ExceptData)(%rip),  %rax
+                pushq   %rax
+
+// call to C code which will in turn call registered handler
+// pass in the vector number
+                movq    %rsp, %rdx
+                movq    ASM_PFX(ExceptionNumber)(%rip), %rcx
+                subq    $40, %rsp
+                call    ASM_PFX(InterruptDistrubutionHub)
+                addq    $40, %rsp
+// restore context...
+// UINT64  ExceptionData;
+                addq    $8, %rsp
+
+// FX_SAVE_STATE_X64 FxSaveState;
+                movq    %rsp, %rsi
+
+                #    FXRSTOR_RSI
+                fxrstor     (%rsi)
+
+                addq    $512, %rsp
+
+// UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                popq    %rax
+                movq    %rax, %dr0
+                popq    %rax
+                movq    %rax, %dr1
+                popq    %rax
+                movq    %rax, %dr2
+                popq    %rax
+                movq    %rax, %dr3
+
+// skip restore of dr6.  We cleared dr6 during the context save.
+                addq    $8, %rsp
+                popq    %rax
+                movq    %rax, %dr7
+
+// UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+                popq    %rax
+                movq    %rax, %cr0
+                addq    $8, %rsp
+                popq    %rax
+                movq    %rax, %cr2
+                popq    %rax
+                movq    %rax, %cr3
+                popq    %rax
+                movq    %rax, %cr4
+                popq    %rax
+                movq    %rax, %cr8
+// UINT64  RFlags;
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                popq    16(%rax)
+// UINT64  Ldtr, Tr;
+// UINT64  Gdtr[2], Idtr[2];
+// Best not let anyone mess with these particular registers...
+                addq    $48, %rsp
+// UINT64  Rip;
+                popq    (%rax)
+
+// UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+// NOTE - modified segment registers could hang the debugger...  We
+//        could attempt to insulate ourselves against this possibility,
+//        but that poses risks as well.
+//
+
+                popq    %rax
+                # mov     %rax, %gs
+                popq    %rax
+                # mov     %rax, %fs
+                popq    %rax
+                mov     %rax, %es
+                popq    %rax
+                mov     %rax, %ds
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                popq    8(%rax)
+                popq    %rax
+                mov     %rax, %ss
+## The next stuff to restore is the general purpose registers that were pushed
+## using the "push" instruction.
+##
+## The value of RSP as stored in the context record is the application RSP
+## including the 5 entries on the application stack caused by the exception
+## itself. It may have been modified by the debug agent, so we need to
+## determine if we need to relocate the application stack.
+
+                movq    24(%rsp), %rbx  # move the potentially modified AppRsp into rbx
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    24(%rax), %rax
+                cmpq    %rax, %rbx
+                je      NoAppStackMove
+
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    (%rax), %rcx     # RIP
+                movq    %rcx, (%rbx)
+
+                movq    8(%rax), %rcx    # CS
+                movq    %rcx, 8(%rbx)
+
+                movq    16(%rax), %rcx   # RFLAGS
+                movq    %rcx, 16(%rbx)
+
+                movq    24(%rax), %rcx   # RSP
+                movq    %rcx, 24(%rbx)
+
+                movq    32(%rax), %rcx   # SS
+                movq    %rcx, 32(%rbx)
+
+                movq    %rbx, %rax       # modify the saved AppRsp to the new AppRsp
+                movq    %rax, ASM_PFX(AppRsp)(%rip)
+NoAppStackMove:
+                movq    ASM_PFX(DebugRsp)(%rip), %rax   # restore the DebugRsp on the debug stack
+                                         # so our "pop" will not cause a stack switch
+                movq    %rax, 24(%rsp)
+
+                cmpl    $0x068, ASM_PFX(ExceptionNumber)(%rip)
+                jne     NoChain
+
+Chain:
+
+// Restore rflags so when we chain, the flags will be exactly as if we were never here.
+// We gin up the stack to do an iretq so we can get ALL the flags.
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    40(%rax), %rbx
+                pushq   %rbx
+                mov     %ss, %rax
+                pushq   %rax
+                movq    %rsp, %rax
+                addq    $16, %rax
+                pushq   %rax
+                movq    ASM_PFX(AppRsp)(%rip), %rax
+                movq    16(%rax), %rbx
+                andq    $0xfffffffffffffcff, %rbx  # special handling for IF and TF
+                pushq   %rbx
+                mov     %cs, %rax
+                pushq   %rax
+                movq    PhonyIretq(%rip), %rax
+                pushq   %rax
+                iretq
+PhonyIretq:
+
+// UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+// UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                popq    %rdi
+                popq    %rsi
+                popq    %rbp
+                popq    %rsp
+                popq    %rbx
+                popq    %rdx
+                popq    %rcx
+                popq    %rax
+                popq    %r8
+                popq    %r9
+                popq    %r10
+                popq    %r11
+                popq    %r12
+                popq    %r13
+                popq    %r14
+                popq    %r15
+
+// Switch back to application stack
+                movq    ASM_PFX(AppRsp)(%rip), %rsp
+// Jump to original handler
+                jmp     ASM_PFX(OrigVector)
+NoChain:
+// UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+// UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                popq    %rdi
+                popq    %rsi
+                popq    %rbp
+                popq    %rsp
+                popq    %rbx
+                popq    %rdx
+                popq    %rcx
+                popq    %rax
+                popq    %r8
+                popq    %r9
+                popq    %r10
+                popq    %r11
+                popq    %r12
+                popq    %r13
+                popq    %r14
+                popq    %r15
+
+// Switch back to application stack
+                movq    ASM_PFX(AppRsp)(%rip), %rsp
+
+// We're outa here...
+                iret
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.asm b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.asm
new file mode 100644
index 0000000000..bce49ef762
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/AsmFuncs.asm
@@ -0,0 +1,596 @@
+;/** @file
+;  Low level x64 routines used by the debug support driver.
+;
+;  Copyright (c) 2007 - 2011, 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.
+;
+;**/
+
+EXCPT64_DIVIDE_ERROR     EQU    0
+EXCPT64_DEBUG            EQU    1
+EXCPT64_NMI              EQU    2
+EXCPT64_BREAKPOINT       EQU    3
+EXCPT64_OVERFLOW         EQU    4
+EXCPT64_BOUND            EQU    5
+EXCPT64_INVALID_OPCODE   EQU    6
+EXCPT64_DOUBLE_FAULT     EQU    8
+EXCPT64_INVALID_TSS      EQU   10
+EXCPT64_SEG_NOT_PRESENT  EQU   11
+EXCPT64_STACK_FAULT      EQU   12
+EXCPT64_GP_FAULT         EQU   13
+EXCPT64_PAGE_FAULT       EQU   14
+EXCPT64_FP_ERROR         EQU   16
+EXCPT64_ALIGNMENT_CHECK  EQU   17
+EXCPT64_MACHINE_CHECK    EQU   18
+EXCPT64_SIMD             EQU   19
+
+FXSTOR_FLAG              EQU   01000000h         ; bit cpuid 24 of feature flags
+
+;; The FXSTOR and FXRSTOR commands are used for saving and restoring the x87,
+;; MMX, SSE, SSE2, etc registers.  The initialization of the debugsupport driver
+;; MUST check the CPUID feature flags to see that these instructions are available
+;; and fail to init if they are not.
+
+;; fxstor [rdi]
+FXSTOR_RDI               MACRO
+                         db 0fh, 0aeh, 00000111y ; mod = 00, reg/op = 000, r/m = 111 = [rdi]
+ENDM
+
+;; fxrstor [rsi]
+FXRSTOR_RSI              MACRO
+                         db 0fh, 0aeh, 00001110y ; mod = 00, reg/op = 001, r/m = 110 = [rsi]
+ENDM
+
+data SEGMENT
+
+public          OrigVector, InterruptEntryStub, StubSize, CommonIdtEntry, FxStorSupport
+
+StubSize        dd      InterruptEntryStubEnd - InterruptEntryStub
+AppRsp          dq      1111111111111111h ; ?
+DebugRsp        dq      2222222222222222h ; ?
+ExtraPush       dq      3333333333333333h ; ?
+ExceptData      dq      4444444444444444h ; ?
+Rflags          dq      5555555555555555h ; ?
+OrigVector      dq      6666666666666666h ; ?
+
+;; The declarations below define the memory region that will be used for the debug stack.
+;; The context record will be built by pushing register values onto this stack.
+;; It is imparitive that alignment be carefully managed, since the FXSTOR and
+;; FXRSTOR instructions will GP fault if their memory operand is not 16 byte aligned.
+;;
+;; The stub will switch stacks from the application stack to the debuger stack
+;; and pushes the exception number.
+;;
+;; Then we building the context record on the stack. Since the stack grows down,
+;; we push the fields of the context record from the back to the front.  There
+;; are 336 bytes of stack used prior allocating the 512 bytes of stack to be
+;; used as the memory buffer for the fxstor instruction. Therefore address of
+;; the buffer used for the FXSTOR instruction is &Eax - 336 - 512, which
+;; must be 16 byte aligned.
+;;
+;; We carefully locate the stack to make this happen.
+;;
+;; For reference, the context structure looks like this:
+;;      struct {
+;;        UINT64            ExceptionData;
+;;        FX_SAVE_STATE_X64 FxSaveState;    // 512 bytes, must be 16 byte aligned
+;;        UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+;;        UINT64            Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+;;        UINT64            RFlags;
+;;        UINT64            Ldtr, Tr;
+;;        UINT64            Gdtr[2], Idtr[2];
+;;        UINT64            Rip;
+;;        UINT64            Gs, Fs, Es, Ds, Cs, Ss;
+;;        UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;;        UINT64            R8, R9, R10, R11, R12, R13, R14, R15;
+;;      } SYSTEM_CONTEXT_X64;  // 64 bit system context record
+
+align           16
+DebugStackEnd   db      "DbgStkEnd >>>>>>"      ;; 16 byte long string - must be 16 bytes to preserve alignment
+                dd      1ffch dup (000000000h)  ;; 32K should be enough stack
+                                                ;;   This allocation is coocked to insure
+                                                ;;   that the the buffer for the FXSTORE instruction
+                                                ;;   will be 16 byte aligned also.
+                                                ;;
+ExceptionNumber dq      ?                       ;; first entry will be the vector number pushed by the stub
+
+DebugStackBegin db      "<<<< DbgStkBegin"      ;; initial debug ESP == DebugStackBegin, set in stub
+
+data ENDS
+
+text SEGMENT
+
+externdef InterruptDistrubutionHub:near
+
+;------------------------------------------------------------------------------
+; BOOLEAN
+; FxStorSupport (
+;   void
+;   )
+;
+; Abstract: Returns TRUE if FxStor instructions are supported
+;
+FxStorSupport   PROC    PUBLIC
+
+;
+; cpuid corrupts rbx which must be preserved per the C calling convention
+;
+                push    rbx
+                mov     rax, 1
+                cpuid
+                mov     eax, edx
+                and     rax, FXSTOR_FLAG
+                shr     rax, 24
+                pop     rbx
+                ret
+FxStorSupport   ENDP
+
+;------------------------------------------------------------------------------
+; void
+; Vect2Desc (
+;   IA32_IDT_GATE_DESCRIPTOR * DestDesc,  // rcx
+;   void (*Vector) (void)   // rdx
+;   )
+;
+; Abstract: Encodes an IDT descriptor with the given physical address
+;
+Vect2Desc       PROC    PUBLIC
+
+                mov     rax, rdx
+                mov     word ptr [rcx], ax                  ; write bits 15..0 of offset
+                mov     dx, cs
+                mov     word ptr [rcx+2], dx                ; SYS_CODE_SEL from GDT
+                mov     word ptr [rcx+4], 0e00h OR 8000h    ; type = 386 interrupt gate, present
+                shr     rax, 16
+                mov     word ptr [rcx+6], ax                ; write bits 31..16 of offset
+                shr     rax, 16
+                mov     dword ptr [rcx+8], eax              ; write bits 63..32 of offset
+
+                ret
+
+Vect2Desc       ENDP
+
+
+
+;------------------------------------------------------------------------------
+; InterruptEntryStub
+;
+; Abstract: This code is not a function, but is a small piece of code that is
+;               copied and fixed up once for each IDT entry that is hooked.
+;
+InterruptEntryStub::
+                push    0                       ; push vector number - will be modified before installed
+                db      0e9h                    ; jump rel32
+                dd      0                       ; fixed up to relative address of CommonIdtEntry
+InterruptEntryStubEnd:
+
+
+
+;------------------------------------------------------------------------------
+; CommonIdtEntry
+;
+; Abstract: This code is not a function, but is the common part for all IDT
+;               vectors.
+;
+CommonIdtEntry::
+;;
+;; At this point, the stub has saved the current application stack esp into AppRsp
+;; and switched stacks to the debug stack, where it pushed the vector number
+;;
+;; The application stack looks like this:
+;;
+;;              ...
+;;              (last application stack entry)
+;;              [16 bytes alignment, do not care it]
+;;              SS from interrupted task
+;;              RSP from interrupted task
+;;              rflags from interrupted task
+;;              CS from interrupted task
+;;              RIP from interrupted task
+;;              Error code <-------------------- Only present for some exeption types
+;;
+;;              Vector Number <----------------- pushed in our IDT Entry
+;;
+
+
+;; The stub switched us to the debug stack and pushed the interrupt number.
+;;
+;; Next, construct the context record.  It will be build on the debug stack by
+;; pushing the registers in the correct order so as to create the context structure
+;; on the debug stack.  The context record must be built from the end back to the
+;; beginning because the stack grows down...
+;
+;; For reference, the context record looks like this:
+;;
+;; typedef
+;; struct {
+;;   UINT64            ExceptionData;
+;;   FX_SAVE_STATE_X64 FxSaveState;
+;;   UINT64            Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+;;   UINT64            Cr0, Cr2, Cr3, Cr4, Cr8;
+;;   UINT64            RFlags;
+;;   UINT64            Ldtr, Tr;
+;;   UINT64            Gdtr[2], Idtr[2];
+;;   UINT64            Rip;
+;;   UINT64            Gs, Fs, Es, Ds, Cs, Ss;
+;;   UINT64            Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;;   UINT64            R8, R9, R10, R11, R12, R13, R14, R15;
+;; } SYSTEM_CONTEXT_X64;  // 64 bit system context record
+
+;; NOTE: we save rsp here to prevent compiler put rip reference cause error AppRsp
+                push    rax
+                mov     rax, qword ptr [rsp][8]          ; save vector number
+                mov     ExceptionNumber, rax             ; save vector number
+                pop     rax
+                add     rsp, 8                           ; pop vector number
+                mov     AppRsp, rsp                      ; save stack top
+                mov     rsp, offset DebugStackBegin      ; switch to debugger stack
+                sub     rsp, 8                           ; leave space for vector number
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                push    r15
+                push    r14
+                push    r13
+                push    r12
+                push    r11
+                push    r10
+                push    r9
+                push    r8
+                push    rax
+                push    rcx
+                push    rdx
+                push    rbx
+                push    rsp
+                push    rbp
+                push    rsi
+                push    rdi
+
+;; Save interrupt state rflags register...
+                pushfq
+                pop     rax
+                mov     qword ptr Rflags, rax
+
+;; We need to determine if any extra data was pushed by the exception, and if so, save it
+;; To do this, we check the exception number pushed by the stub, and cache the
+;; result in a variable since we'll need this again.
+                cmp     ExceptionNumber, EXCPT64_DOUBLE_FAULT
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_INVALID_TSS
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_SEG_NOT_PRESENT
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_STACK_FAULT
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_GP_FAULT
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_PAGE_FAULT
+                jz      ExtraPushOne
+                cmp     ExceptionNumber, EXCPT64_ALIGNMENT_CHECK
+                jz      ExtraPushOne
+                mov     ExtraPush, 0
+                mov     ExceptData, 0
+                jmp     ExtraPushDone
+ExtraPushOne:
+                mov     ExtraPush, 1
+
+;; If there's some extra data, save it also, and modify the saved AppRsp to effectively
+;; pop this value off the application's stack.
+                mov     rax, AppRsp
+                mov     rbx, [rax]
+                mov     ExceptData, rbx
+                add     rax, 8
+                mov     AppRsp, rax
+
+ExtraPushDone:
+
+;; The "push" above pushed the debug stack rsp.  Since what we're actually doing
+;; is building the context record on the debug stack, we need to save the pushed
+;; debug RSP, and replace it with the application's last stack entry...
+                mov     rax, [rsp + 24]
+                mov     DebugRsp, rax
+                mov     rax, AppRsp
+                mov     rax, QWORD PTR [rax + 24]
+                ; application stack has ss, rsp, rflags, cs, & rip, so
+                ; last actual application stack entry is saved at offset
+                ; 24 bytes from stack top.
+                mov     [rsp + 24], rax
+
+;; continue building context record
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;  insure high 16 bits of each is zero
+                mov     rax, ss
+                push    rax
+
+                ; CS from application is one entry back in application stack
+                mov     rax, AppRsp
+                movzx   rax, word ptr [rax + 8]
+                push    rax
+
+                mov     rax, ds
+                push    rax
+                mov     rax, es
+                push    rax
+                mov     rax, fs
+                push    rax
+                mov     rax, gs
+                push    rax
+
+;; UINT64  Rip;
+                ; Rip from application is on top of application stack
+                mov     rax, AppRsp
+                push    qword ptr [rax]
+
+;; UINT64  Gdtr[2], Idtr[2];
+                push    0
+                push    0
+                sidt    fword ptr [rsp]
+                push    0
+                push    0
+                sgdt    fword ptr [rsp]
+
+;; UINT64  Ldtr, Tr;
+                xor     rax, rax
+                str     ax
+                push    rax
+                sldt    ax
+                push    rax
+
+;; UINT64  RFlags;
+;; Rflags from application is two entries back in application stack
+                mov     rax, AppRsp
+                push    qword ptr [rax + 16]
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+;; insure FXSAVE/FXRSTOR is enabled in CR4...
+;; ... while we're at it, make sure DE is also enabled...
+                mov     rax, cr8
+                push    rax
+                mov     rax, cr4
+                or      rax, 208h
+                mov     cr4, rax
+                push    rax
+                mov     rax, cr3
+                push    rax
+                mov     rax, cr2
+                push    rax
+                push    0
+                mov     rax, cr0
+                push    rax
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                mov     rax, dr7
+                push    rax
+;; clear Dr7 while executing debugger itself
+                xor     rax, rax
+                mov     dr7, rax
+
+                mov     rax, dr6
+                push    rax
+;; insure all status bits in dr6 are clear...
+                xor     rax, rax
+                mov     dr6, rax
+
+                mov     rax, dr3
+                push    rax
+                mov     rax, dr2
+                push    rax
+                mov     rax, dr1
+                push    rax
+                mov     rax, dr0
+                push    rax
+
+;; FX_SAVE_STATE_X64 FxSaveState;
+                sub     rsp, 512
+                mov     rdi, rsp
+                ; IMPORTANT!! The debug stack has been carefully constructed to
+                ; insure that rsp and rdi are 16 byte aligned when we get here.
+                ; They MUST be.  If they are not, a GP fault will occur.
+                FXSTOR_RDI
+
+;; UEFI calling convention for x64 requires that Direction flag in EFLAGs is clear
+                cld
+
+;; UINT64  ExceptionData;
+                mov     rax, ExceptData
+                push    rax
+
+; call to C code which will in turn call registered handler
+; pass in the vector number
+                mov     rdx, rsp
+                mov     rcx, ExceptionNumber
+                sub     rsp, 40
+                call    InterruptDistrubutionHub
+                add     rsp, 40
+
+; restore context...
+;; UINT64  ExceptionData;
+                add     rsp, 8
+
+;; FX_SAVE_STATE_X64 FxSaveState;
+                mov     rsi, rsp
+                FXRSTOR_RSI
+                add     rsp, 512
+
+;; UINT64  Dr0, Dr1, Dr2, Dr3, Dr6, Dr7;
+                pop     rax
+                mov     dr0, rax
+                pop     rax
+                mov     dr1, rax
+                pop     rax
+                mov     dr2, rax
+                pop     rax
+                mov     dr3, rax
+;; skip restore of dr6.  We cleared dr6 during the context save.
+                add     rsp, 8
+                pop     rax
+                mov     dr7, rax
+
+;; UINT64  Cr0, Cr1, Cr2, Cr3, Cr4, Cr8;
+                pop     rax
+                mov     cr0, rax
+                add     rsp, 8
+                pop     rax
+                mov     cr2, rax
+                pop     rax
+                mov     cr3, rax
+                pop     rax
+                mov     cr4, rax
+                pop     rax
+                mov     cr8, rax
+
+;; UINT64  RFlags;
+                mov     rax, AppRsp
+                pop     qword ptr [rax + 16]
+
+;; UINT64  Ldtr, Tr;
+;; UINT64  Gdtr[2], Idtr[2];
+;; Best not let anyone mess with these particular registers...
+                add     rsp, 48
+
+;; UINT64  Rip;
+                pop     qword ptr [rax]
+
+;; UINT64  Gs, Fs, Es, Ds, Cs, Ss;
+;; NOTE - modified segment registers could hang the debugger...  We
+;;        could attempt to insulate ourselves against this possibility,
+;;        but that poses risks as well.
+;;
+
+                pop     rax
+                ; mov     gs, rax
+                pop     rax
+                ; mov     fs, rax
+                pop     rax
+                mov     es, rax
+                pop     rax
+                mov     ds, rax
+                mov     rax, AppRsp
+                pop     qword ptr [rax + 8]
+                pop     rax
+                mov     ss, rax
+
+;; The next stuff to restore is the general purpose registers that were pushed
+;; using the "push" instruction.
+;;
+;; The value of RSP as stored in the context record is the application RSP
+;; including the 5 entries on the application stack caused by the exception
+;; itself. It may have been modified by the debug agent, so we need to
+;; determine if we need to relocate the application stack.
+
+                mov     rbx, [rsp + 24]  ; move the potentially modified AppRsp into rbx
+                mov     rax, AppRsp
+                mov     rax, QWORD PTR [rax + 24]
+                cmp     rbx, rax
+                je      NoAppStackMove
+
+                mov     rax, AppRsp
+                mov     rcx, [rax]       ; RIP
+                mov     [rbx], rcx
+
+                mov     rcx, [rax + 8]   ; CS
+                mov     [rbx + 8], rcx
+
+                mov     rcx, [rax + 16]  ; RFLAGS
+                mov     [rbx + 16], rcx
+
+                mov     rcx, [rax + 24]  ; RSP
+                mov     [rbx + 24], rcx
+
+                mov     rcx, [rax + 32]  ; SS
+                mov     [rbx + 32], rcx
+
+                mov     rax, rbx         ; modify the saved AppRsp to the new AppRsp
+                mov     AppRsp, rax
+NoAppStackMove:
+                mov     rax, DebugRsp    ; restore the DebugRsp on the debug stack
+                                         ; so our "pop" will not cause a stack switch
+                mov     [rsp + 24], rax
+
+                cmp     ExceptionNumber, 068h
+                jne     NoChain
+
+Chain:
+
+;; Restore rflags so when we chain, the flags will be exactly as if we were never here.
+;; We gin up the stack to do an iretq so we can get ALL the flags.
+                mov     rax, AppRsp
+                mov     rbx, [rax + 40]
+                push    rbx
+                mov     rax, ss
+                push    rax
+                mov     rax, rsp
+                add     rax, 16
+                push    rax
+                mov     rax, AppRsp
+                mov     rbx, [rax + 16]
+                and     rbx, NOT 300h ; special handling for IF and TF
+                push    rbx
+                mov     rax, cs
+                push    rax
+                mov     rax, offset PhonyIretq
+                push    rax
+                iretq
+PhonyIretq:
+
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                pop     rdi
+                pop     rsi
+                pop     rbp
+                pop     rsp
+                pop     rbx
+                pop     rdx
+                pop     rcx
+                pop     rax
+                pop     r8
+                pop     r9
+                pop     r10
+                pop     r11
+                pop     r12
+                pop     r13
+                pop     r14
+                pop     r15
+
+;; Switch back to application stack
+                mov     rsp, AppRsp
+
+;; Jump to original handler
+                jmp     OrigVector
+
+NoChain:
+;; UINT64  Rdi, Rsi, Rbp, Rsp, Rbx, Rdx, Rcx, Rax;
+;; UINT64  R8, R9, R10, R11, R12, R13, R14, R15;
+                pop     rdi
+                pop     rsi
+                pop     rbp
+                pop     rsp
+                pop     rbx
+                pop     rdx
+                pop     rcx
+                pop     rax
+                pop     r8
+                pop     r9
+                pop     r10
+                pop     r11
+                pop     r12
+                pop     r13
+                pop     r14
+                pop     r15
+
+;; Switch back to application stack
+                mov     rsp, AppRsp
+
+;; We're outa here...
+                iretq
+text ENDS
+
+END
+
+
+
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupport.h b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupport.h
new file mode 100644
index 0000000000..044e5d9d07
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupport.h
@@ -0,0 +1,22 @@
+/** @file
+  X64 specific debug support macros.
+
+Copyright (c) 2006 - 2008, 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.             
+
+**/
+
+#ifndef _PLDEBUG_SUPPORT_H_
+#define _PLDEBUG_SUPPORT_H_
+
+#include "Ia32/DebugSupport.h"
+
+#define EFI_ISA                         IsaX64
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupportX64.c b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupportX64.c
new file mode 100644
index 0000000000..fa8869d287
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DebugSupportDxe/X64/PlDebugSupportX64.c
@@ -0,0 +1,146 @@
+/** @file
+  X64 specific functions to support Debug Support protocol.
+
+Copyright (c) 2008 - 2010, 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 "PlDebugSupport.h"
+
+IA32_IDT_GATE_DESCRIPTOR  NullDesc = {{0,0}};
+
+/**
+  Get Interrupt Handle from IDT Gate Descriptor.
+
+  @param  IdtGateDecriptor  IDT Gate Descriptor.
+
+  @return Interrupt Handle stored in IDT Gate Descriptor.
+
+**/
+UINTN
+GetInterruptHandleFromIdt (
+  IN IA32_IDT_GATE_DESCRIPTOR  *IdtGateDecriptor
+  )
+{
+  UINTN      InterruptHandle;
+
+  //
+  // InterruptHandle  0-15 : OffsetLow
+  // InterruptHandle 16-31 : OffsetHigh
+  // InterruptHandle 32-63 : OffsetUpper
+  //
+  InterruptHandle = ((UINTN) IdtGateDecriptor->Bits.OffsetLow) |
+                    (((UINTN) IdtGateDecriptor->Bits.OffsetHigh)  << 16) |
+                    (((UINTN) IdtGateDecriptor->Bits.OffsetUpper) << 32) ;
+
+  return InterruptHandle;
+}
+
+/**
+  Allocate pool for a new IDT entry stub.
+
+  Copy the generic stub into the new buffer and fixup the vector number
+  and jump target address.
+
+  @param  ExceptionType   This is the exception type that the new stub will be created
+                          for.
+  @param  Stub            On successful exit, *Stub contains the newly allocated entry stub.
+
+**/
+VOID
+CreateEntryStub (
+  IN EFI_EXCEPTION_TYPE     ExceptionType,
+  OUT VOID                  **Stub
+  )
+{
+  UINT8       *StubCopy;
+
+  StubCopy = *Stub;
+
+  //
+  // Fixup the stub code for this vector
+  //
+
+  // The stub code looks like this:
+  //
+  //    00000000  6A 00               push    0                       ; push vector number - will be modified before installed
+  //    00000002  E9                  db      0e9h                    ; jump rel32
+  //    00000003  00000000            dd      0                       ; fixed up to relative address of CommonIdtEntry
+  //
+
+  //
+  // poke in the exception type so the second push pushes the exception type
+  //
+  StubCopy[0x1] = (UINT8) ExceptionType;
+
+  //
+  // fixup the jump target to point to the common entry
+  //
+  *(UINT32 *) &StubCopy[0x3] = (UINT32)((UINTN) CommonIdtEntry - (UINTN) &StubCopy[StubSize]);
+
+  return;
+}
+
+/**
+  This is the main worker function that manages the state of the interrupt
+  handlers.  It both installs and uninstalls interrupt handlers based on the
+  value of NewCallback.  If NewCallback is NULL, then uninstall is indicated.
+  If NewCallback is non-NULL, then install is indicated.
+
+  @param  NewCallback   If non-NULL, NewCallback specifies the new handler to register.
+                        If NULL, specifies that the previously registered handler should
+                        be uninstalled.
+  @param  ExceptionType Indicates which entry to manage.
+
+  @retval EFI_SUCCESS            Process is ok.
+  @retval EFI_INVALID_PARAMETER  Requested uninstalling a handler from a vector that has
+                                 no handler registered for it
+  @retval EFI_ALREADY_STARTED    Requested install to a vector that already has a handler registered.
+  @retval others                 Possible return values are passed through from UnHookEntry and HookEntry.
+
+**/
+EFI_STATUS
+ManageIdtEntryTable (
+  CALLBACK_FUNC      NewCallback,
+  EFI_EXCEPTION_TYPE ExceptionType
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EFI_SUCCESS;
+
+  if (CompareMem (&IdtEntryTable[ExceptionType].NewDesc, &NullDesc, sizeof (IA32_IDT_GATE_DESCRIPTOR)) != 0) {
+    //
+    // we've already installed to this vector
+    //
+    if (NewCallback != NULL) {
+      //
+      // if the input handler is non-null, error
+      //
+      Status = EFI_ALREADY_STARTED;
+    } else {
+      UnhookEntry (ExceptionType);
+    }
+  } else {
+    //
+    // no user handler installed on this vector
+    //
+    if (NewCallback == NULL) {
+      //
+      // if the input handler is null, error
+      //
+      Status = EFI_INVALID_PARAMETER;
+    } else {
+      HookEntry (ExceptionType, NewCallback);
+    }
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePath.c b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePath.c
new file mode 100644
index 0000000000..70f03bd27a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePath.c
@@ -0,0 +1,105 @@
+/** @file
+  Device Path Driver to produce DevPathUtilities Protocol, DevPathFromText Protocol
+  and DevPathToText Protocol.
+
+Copyright (c) 2006 - 2013, 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 <Uefi.h>
+#include <Protocol/DevicePathUtilities.h>
+#include <Protocol/DevicePathToText.h>
+#include <Protocol/DevicePathFromText.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST EFI_DEVICE_PATH_UTILITIES_PROTOCOL mDevicePathUtilities = {
+  GetDevicePathSize,
+  DuplicateDevicePath,
+  AppendDevicePath,
+  AppendDevicePathNode,
+  AppendDevicePathInstance,
+  GetNextDevicePathInstance,
+  IsDevicePathMultiInstance,
+  CreateDeviceNode
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST EFI_DEVICE_PATH_TO_TEXT_PROTOCOL   mDevicePathToText = {
+  ConvertDeviceNodeToText,
+  ConvertDevicePathToText
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST EFI_DEVICE_PATH_FROM_TEXT_PROTOCOL mDevicePathFromText = {
+  ConvertTextToDeviceNode,
+  ConvertTextToDevicePath
+};
+
+/**
+  The user Entry Point for DevicePath module.
+
+  This is the entry point for DevicePath module. It installs the UEFI Device Path Utility Protocol and
+  optionally the Device Path to Text and Device Path from Text protocols based on feature flags.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval Others            Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+DevicePathEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_HANDLE  Handle;
+
+  Handle = NULL;
+  Status = EFI_UNSUPPORTED;
+  if (FeaturePcdGet (PcdDevicePathSupportDevicePathToText)) {
+    if (FeaturePcdGet (PcdDevicePathSupportDevicePathFromText)) {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Handle,
+                      &gEfiDevicePathUtilitiesProtocolGuid, &mDevicePathUtilities,
+                      &gEfiDevicePathToTextProtocolGuid,    &mDevicePathToText,
+                      &gEfiDevicePathFromTextProtocolGuid,  &mDevicePathFromText,
+                      NULL
+                      );
+    } else {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Handle,
+                      &gEfiDevicePathUtilitiesProtocolGuid, &mDevicePathUtilities,
+                      &gEfiDevicePathToTextProtocolGuid,    &mDevicePathToText,
+                      NULL
+                      );
+    }
+  } else {
+    if (FeaturePcdGet (PcdDevicePathSupportDevicePathFromText)) {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Handle,
+                      &gEfiDevicePathUtilitiesProtocolGuid, &mDevicePathUtilities,
+                      &gEfiDevicePathFromTextProtocolGuid,  &mDevicePathFromText,
+                      NULL
+                      );
+    } else {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &Handle,
+                      &gEfiDevicePathUtilitiesProtocolGuid, &mDevicePathUtilities,
+                      NULL
+                      );
+    }
+  }
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.inf b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.inf
new file mode 100644
index 0000000000..ba6ed6b6d8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.inf
@@ -0,0 +1,60 @@
+## @file
+#  Device path driver that produces three UEFI device path protocols.
+#
+#  This driver produces Device Path Utilities protocol and optionally
+#  DevicePahtToText and DevicePathFromText protocols based on feature flags
+#  PcdDevicePathSupportDevicePathToText & PcdDevicePathSupportDevicePathFromText
+#  respectively.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DevicePathDxe
+  MODULE_UNI_FILE                = DevicePathDxe.uni
+  FILE_GUID                      = 9B680FCE-AD6B-4F3A-B60B-F59899003443
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = DevicePathEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DevicePath.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  
+[Protocols]
+  gEfiDevicePathToTextProtocolGuid   | gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathFromText ## SOMETIMES_PRODUCES
+  gEfiDevicePathFromTextProtocolGuid | gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathToText ## SOMETIMES_PRODUCES
+  gEfiDevicePathUtilitiesProtocolGuid ## PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathFromText ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDevicePathSupportDevicePathToText   ## CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DevicePathDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.uni b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.uni
new file mode 100644
index 0000000000..187e2a8577
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxe.uni
diff --git a/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxeExtra.uni b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxeExtra.uni
new file mode 100644
index 0000000000..bb94010999
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DevicePathDxe/DevicePathDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.inf b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.inf
new file mode 100644
index 0000000000..e9d45e5b86
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.inf
@@ -0,0 +1,75 @@
+## @file
+# PeiCdExpress recovery module.
+#
+# This module reads data from CDROM device by all installed block IO ppi and 
+# finds whether there is Recovery data in the device. If it finds recovery
+# data, it will install Device Recovery Module PPI.
+#
+# Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = CdExpressPei
+  MODULE_UNI_FILE                = CdExpressPei.uni
+  FILE_GUID                      = 31e147a6-d39a-4147-9da3-befd4d523243
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = CdExpressPeimEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PeiCdExpress.c
+  PeiCdExpress.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  PeimEntryPoint
+  DebugLib
+  PeiServicesTablePointerLib
+  PeiServicesLib
+  MemoryAllocationLib
+  PcdLib
+
+[Guids]
+  gRecoveryOnDataCdGuid                         ## CONSUMES   ## UNDEFINED # Indicate the recovery device type
+
+
+[Ppis]
+  ## NOTIFY
+  ## CONSUMES
+  gEfiPeiVirtualBlockIoPpiGuid
+  ## NOTIFY
+  ## CONSUMES
+  gEfiPeiVirtualBlockIo2PpiGuid
+  gEfiPeiDeviceRecoveryModulePpiGuid            ## PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrameworkCompatibilitySupport   ## CONSUMES
+
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid AND gEfiPeiBootInRecoveryModePpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  CdExpressPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.uni b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.uni
new file mode 100644
index 0000000000..9ef1b035d7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPei.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPeiExtra.uni b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPeiExtra.uni
new file mode 100644
index 0000000000..b76d2a6e79
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/CdExpressPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.c b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.c
new file mode 100644
index 0000000000..f78118a6a8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.c
@@ -0,0 +1,717 @@
+/** @file
+  Source file for CD recovery PEIM
+
+Copyright (c) 2006 - 2015, 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 "PeiCdExpress.h"
+
+PEI_CD_EXPRESS_PRIVATE_DATA *mPrivateData = NULL;
+
+/**
+  Installs the Device Recovery Module PPI, Initialize BlockIo Ppi 
+  installation notification
+
+  @param  FileHandle            The file handle of the image.
+  @param  PeiServices           General purpose services available to every PEIM.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough system memory.
+
+**/
+EFI_STATUS
+EFIAPI
+CdExpressPeimEntry (
+  IN EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES    **PeiServices
+  )
+{
+  EFI_STATUS                  Status;
+  PEI_CD_EXPRESS_PRIVATE_DATA *PrivateData;
+
+  if (!EFI_ERROR (PeiServicesRegisterForShadow (FileHandle))) {
+    return EFI_SUCCESS;
+  }
+
+  PrivateData = AllocatePages (EFI_SIZE_TO_PAGES (sizeof (*PrivateData)));
+  if (PrivateData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize Private Data (to zero, as is required by subsequent operations)
+  //
+  ZeroMem (PrivateData, sizeof (*PrivateData));
+  PrivateData->Signature    = PEI_CD_EXPRESS_PRIVATE_DATA_SIGNATURE;
+
+  PrivateData->BlockBuffer  = AllocatePages (EFI_SIZE_TO_PAGES (PEI_CD_BLOCK_SIZE));
+  if (PrivateData->BlockBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  PrivateData->CapsuleCount = 0;
+  Status = UpdateBlocksAndVolumes (PrivateData, TRUE);
+  Status = UpdateBlocksAndVolumes (PrivateData, FALSE);
+
+  //
+  // Installs Ppi
+  //
+  PrivateData->DeviceRecoveryPpi.GetNumberRecoveryCapsules  = GetNumberRecoveryCapsules;
+  PrivateData->DeviceRecoveryPpi.GetRecoveryCapsuleInfo     = GetRecoveryCapsuleInfo;
+  PrivateData->DeviceRecoveryPpi.LoadRecoveryCapsule        = LoadRecoveryCapsule;
+
+  PrivateData->PpiDescriptor.Flags = (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST);
+  PrivateData->PpiDescriptor.Guid  = &gEfiPeiDeviceRecoveryModulePpiGuid;
+  PrivateData->PpiDescriptor.Ppi   = &PrivateData->DeviceRecoveryPpi;
+
+  Status = PeiServicesInstallPpi (&PrivateData->PpiDescriptor);
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // PrivateData is allocated now, set it to the module variable
+  //
+  mPrivateData = PrivateData;
+
+  //
+  // Installs Block Io Ppi notification function
+  //
+  PrivateData->NotifyDescriptor.Flags =
+    (
+      EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK
+    );
+  PrivateData->NotifyDescriptor.Guid    = &gEfiPeiVirtualBlockIoPpiGuid;
+  PrivateData->NotifyDescriptor.Notify  = BlockIoNotifyEntry;
+
+  PrivateData->NotifyDescriptor2.Flags =
+    (
+      EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK |
+      EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST
+    );
+  PrivateData->NotifyDescriptor2.Guid    = &gEfiPeiVirtualBlockIo2PpiGuid;
+  PrivateData->NotifyDescriptor2.Notify  = BlockIoNotifyEntry;
+
+  return PeiServicesNotifyPpi (&PrivateData->NotifyDescriptor);
+
+}
+
+/**
+  BlockIo installation notification function. 
+  
+  This function finds out all the current Block IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices            Indirect reference to the PEI Services Table.
+  @param  NotifyDescriptor       Address of the notification descriptor data structure.
+  @param  Ppi                    Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS            The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BlockIoNotifyEntry (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyDescriptor,
+  IN VOID                       *Ppi
+  )
+{
+  if (CompareGuid (NotifyDescriptor->Guid, &gEfiPeiVirtualBlockIo2PpiGuid)) {
+    UpdateBlocksAndVolumes (mPrivateData, TRUE);
+  } else {
+    UpdateBlocksAndVolumes (mPrivateData, FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Finds out all the current Block IO PPIs in the system and add them into private data.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+  @param BlockIo2                       Boolean to show whether using BlockIo2 or BlockIo.
+
+  @retval EFI_SUCCESS                   The blocks and volumes are updated successfully.
+
+**/
+EFI_STATUS
+UpdateBlocksAndVolumes (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA     *PrivateData,
+  IN     BOOLEAN                         BlockIo2
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_PEI_PPI_DESCRIPTOR          *TempPpiDescriptor;
+  UINTN                           BlockIoPpiInstance;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   *BlockIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  *BlockIo2Ppi;
+  UINTN                           NumberBlockDevices;
+  UINTN                           IndexBlockDevice;
+  EFI_PEI_BLOCK_IO_MEDIA          Media;
+  EFI_PEI_BLOCK_IO2_MEDIA         Media2;
+  EFI_PEI_SERVICES                **PeiServices;
+
+  IndexBlockDevice = 0;
+  BlockIo2Ppi      = NULL;
+  BlockIoPpi       = NULL;
+  //
+  // Find out all Block Io Ppi instances within the system
+  // Assuming all device Block Io Peims are dispatched already
+  //
+  for (BlockIoPpiInstance = 0; BlockIoPpiInstance < PEI_CD_EXPRESS_MAX_BLOCK_IO_PPI; BlockIoPpiInstance++) {
+    if (BlockIo2) {
+      Status = PeiServicesLocatePpi (
+                                &gEfiPeiVirtualBlockIo2PpiGuid,
+                                BlockIoPpiInstance,
+                                &TempPpiDescriptor,
+                                (VOID **) &BlockIo2Ppi
+                                );
+    } else {
+      Status = PeiServicesLocatePpi (
+                                &gEfiPeiVirtualBlockIoPpiGuid,
+                                BlockIoPpiInstance,
+                                &TempPpiDescriptor,
+                                (VOID **) &BlockIoPpi
+                                );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Done with all Block Io Ppis
+      //
+      break;
+    }
+
+    PeiServices = (EFI_PEI_SERVICES  **) GetPeiServicesTablePointer ();
+    if (BlockIo2) {
+      Status = BlockIo2Ppi->GetNumberOfBlockDevices (
+                              PeiServices,
+                              BlockIo2Ppi,
+                              &NumberBlockDevices
+                              );
+    } else {
+      Status = BlockIoPpi->GetNumberOfBlockDevices (
+                            PeiServices,
+                            BlockIoPpi,
+                            &NumberBlockDevices
+                            );
+    }
+    if (EFI_ERROR (Status) || (NumberBlockDevices == 0)) {
+      continue;
+    }
+    //
+    // Just retrieve the first block, should emulate all blocks.
+    //
+    for (IndexBlockDevice = 1; IndexBlockDevice <= NumberBlockDevices && PrivateData->CapsuleCount < PEI_CD_EXPRESS_MAX_CAPSULE_NUMBER; IndexBlockDevice ++) {
+      if (BlockIo2) {
+        Status = BlockIo2Ppi->GetBlockDeviceMediaInfo (
+                                PeiServices,
+                                BlockIo2Ppi,
+                                IndexBlockDevice,
+                                &Media2
+                                );
+        if (EFI_ERROR (Status) ||
+            !Media2.MediaPresent ||
+             ((Media2.InterfaceType != MSG_ATAPI_DP) && (Media2.InterfaceType != MSG_USB_DP)) ||
+            (Media2.BlockSize != PEI_CD_BLOCK_SIZE)
+            ) {
+          continue;
+        }
+        DEBUG ((EFI_D_INFO, "PeiCdExpress InterfaceType is %d\n", Media2.InterfaceType));
+        DEBUG ((EFI_D_INFO, "PeiCdExpress MediaPresent is %d\n", Media2.MediaPresent));
+        DEBUG ((EFI_D_INFO, "PeiCdExpress BlockSize is  0x%x\n", Media2.BlockSize));
+      } else {
+        Status = BlockIoPpi->GetBlockDeviceMediaInfo (
+                              PeiServices,
+                              BlockIoPpi,
+                              IndexBlockDevice,
+                              &Media
+                              );
+        if (EFI_ERROR (Status) ||
+            !Media.MediaPresent ||
+             ((Media.DeviceType != IdeCDROM) && (Media.DeviceType != UsbMassStorage)) ||
+            (Media.BlockSize != PEI_CD_BLOCK_SIZE)
+            ) {
+          continue;
+        }
+        DEBUG ((EFI_D_INFO, "PeiCdExpress DeviceType is %d\n", Media.DeviceType));
+        DEBUG ((EFI_D_INFO, "PeiCdExpress MediaPresent is %d\n", Media.MediaPresent));
+        DEBUG ((EFI_D_INFO, "PeiCdExpress BlockSize is  0x%x\n", Media.BlockSize));
+      }
+
+      DEBUG ((EFI_D_INFO, "PeiCdExpress Status is %d\n", Status));
+
+      DEBUG ((EFI_D_INFO, "IndexBlockDevice is %d\n", IndexBlockDevice));
+      PrivateData->CapsuleData[PrivateData->CapsuleCount].IndexBlock = IndexBlockDevice;
+      if (BlockIo2) {
+        PrivateData->CapsuleData[PrivateData->CapsuleCount].BlockIo2 = BlockIo2Ppi;
+      } else {
+        PrivateData->CapsuleData[PrivateData->CapsuleCount].BlockIo  = BlockIoPpi;
+      }
+      Status = FindRecoveryCapsules (PrivateData);
+      DEBUG ((EFI_D_INFO, "Status is %d\n", Status));
+
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+  
+      PrivateData->CapsuleCount++;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Finds out the recovery capsule in the current volume.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+
+  @retval EFI_SUCCESS                   The recovery capsule is successfully found in the volume.
+  @retval EFI_NOT_FOUND                 The recovery capsule is not found in the volume.
+
+**/
+EFI_STATUS
+EFIAPI
+FindRecoveryCapsules (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA            *PrivateData
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           Lba;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   *BlockIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  *BlockIo2Ppi;
+  UINTN                           BufferSize;
+  UINT8                           *Buffer;
+  UINT8                           Type;
+  UINT8                           *StandardID;
+  UINT32                          RootDirLBA;
+  PEI_CD_EXPRESS_DIR_FILE_RECORD  *RoorDirRecord;
+  UINTN                           VolumeSpaceSize;
+  BOOLEAN                         StartOfVolume;
+  UINTN                           OriginalLBA;
+  UINTN                           IndexBlockDevice;
+
+  Buffer      = PrivateData->BlockBuffer;
+  BufferSize  = PEI_CD_BLOCK_SIZE;
+
+  Lba         = 16;
+  //
+  // The volume descriptor starts on Lba 16
+  //
+  IndexBlockDevice = PrivateData->CapsuleData[PrivateData->CapsuleCount].IndexBlock;
+  BlockIoPpi       = PrivateData->CapsuleData[PrivateData->CapsuleCount].BlockIo;
+  BlockIo2Ppi      = PrivateData->CapsuleData[PrivateData->CapsuleCount].BlockIo2;
+
+  VolumeSpaceSize = 0;
+  StartOfVolume   = TRUE;
+  OriginalLBA     = 16;
+
+  while (TRUE) {
+    SetMem (Buffer, BufferSize, 0);
+    if (BlockIo2Ppi != NULL) {
+      Status = BlockIo2Ppi->ReadBlocks (
+                            (EFI_PEI_SERVICES **) GetPeiServicesTablePointer (),
+                            BlockIo2Ppi,
+                            IndexBlockDevice,
+                            Lba,
+                            BufferSize,
+                            Buffer
+                            );
+    } else {
+      Status = BlockIoPpi->ReadBlocks (
+                            (EFI_PEI_SERVICES **) GetPeiServicesTablePointer (),
+                            BlockIoPpi,
+                            IndexBlockDevice,
+                            Lba,
+                            BufferSize,
+                            Buffer
+                            );
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    StandardID = (UINT8 *) (Buffer + PEI_CD_EXPRESS_STANDARD_ID_OFFSET);
+    if (!StringCmp (StandardID, (UINT8 *) PEI_CD_STANDARD_ID, PEI_CD_EXPRESS_STANDARD_ID_SIZE, TRUE)) {
+      break;
+    }
+
+    if (StartOfVolume) {
+      OriginalLBA   = Lba;
+      StartOfVolume = FALSE;
+    }
+
+    Type = *(UINT8 *) (Buffer + PEI_CD_EXPRESS_VOLUME_TYPE_OFFSET);
+    if (Type == PEI_CD_EXPRESS_VOLUME_TYPE_TERMINATOR) {
+      if (VolumeSpaceSize == 0) {
+        break;
+      } else {
+        Lba             = (OriginalLBA + VolumeSpaceSize);
+        VolumeSpaceSize = 0;
+        StartOfVolume   = TRUE;
+        continue;
+      }
+    }
+
+    if (Type != PEI_CD_EXPRESS_VOLUME_TYPE_PRIMARY) {
+      Lba++;
+      continue;
+    }
+
+    VolumeSpaceSize = *(UINT32 *) (Buffer + PEI_CD_EXPRESS_VOLUME_SPACE_OFFSET);
+
+    RoorDirRecord   = (PEI_CD_EXPRESS_DIR_FILE_RECORD *) (Buffer + PEI_CD_EXPRESS_ROOT_DIR_RECORD_OFFSET);
+    RootDirLBA      = RoorDirRecord->LocationOfExtent[0];
+
+    Status          = RetrieveCapsuleFileFromRoot (PrivateData, BlockIoPpi, BlockIo2Ppi, IndexBlockDevice, RootDirLBA);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Just look for the first primary descriptor
+      //
+      return EFI_SUCCESS;
+    }
+
+    Lba++;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Retrieves the recovery capsule in root directory of the current volume.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+  @param BlockIoPpi                     The Block IO PPI used to access the volume.
+  @param BlockIo2Ppi                    The Block IO 2 PPI used to access the volume.
+  @param IndexBlockDevice               The index of current block device.
+  @param Lba                            The starting logic block address to retrieve capsule.
+
+  @retval EFI_SUCCESS                   The recovery capsule is successfully found in the volume.
+  @retval EFI_NOT_FOUND                 The recovery capsule is not found in the volume.
+  @retval Others                        
+
+**/
+EFI_STATUS
+EFIAPI
+RetrieveCapsuleFileFromRoot (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA        *PrivateData,
+  IN EFI_PEI_RECOVERY_BLOCK_IO_PPI          *BlockIoPpi,
+  IN EFI_PEI_RECOVERY_BLOCK_IO2_PPI         *BlockIo2Ppi,
+  IN UINTN                                  IndexBlockDevice,
+  IN UINT32                                 Lba
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           BufferSize;
+  UINT8                           *Buffer;
+  PEI_CD_EXPRESS_DIR_FILE_RECORD  *FileRecord;
+  UINTN                           Index;
+
+  Buffer      = PrivateData->BlockBuffer;
+  BufferSize  = PEI_CD_BLOCK_SIZE;
+
+  SetMem (Buffer, BufferSize, 0);
+
+  if (BlockIo2Ppi != NULL) {
+    Status = BlockIo2Ppi->ReadBlocks (
+                          (EFI_PEI_SERVICES **) GetPeiServicesTablePointer (),
+                          BlockIo2Ppi,
+                          IndexBlockDevice,
+                          Lba,
+                          BufferSize,
+                          Buffer
+                          );
+  } else {
+    Status = BlockIoPpi->ReadBlocks (
+                          (EFI_PEI_SERVICES **) GetPeiServicesTablePointer (),
+                          BlockIoPpi,
+                          IndexBlockDevice,
+                          Lba,
+                          BufferSize,
+                          Buffer
+                          );
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (1) {
+    FileRecord = (PEI_CD_EXPRESS_DIR_FILE_RECORD *) Buffer;
+
+    if (FileRecord->Length == 0) {
+      break;
+    }
+    //
+    // Not intend to check other flag now
+    //
+    if ((FileRecord->Flag & PEI_CD_EXPRESS_DIR_FILE_REC_FLAG_ISDIR) != 0) {
+      Buffer += FileRecord->Length;
+      continue;
+    }
+
+    for (Index = 0; Index < FileRecord->FileIDLength; Index++) {
+      if (FileRecord->FileID[Index] == ';') {
+        break;
+      }
+    }
+
+    if (Index != (sizeof (PEI_RECOVERY_FILE_NAME) - 1)) {
+      Buffer += FileRecord->Length;
+      continue;
+    }
+
+    if (!StringCmp (FileRecord->FileID, (UINT8 *) PEI_RECOVERY_FILE_NAME, sizeof (PEI_RECOVERY_FILE_NAME) - 1, FALSE)) {
+      Buffer += FileRecord->Length;
+      continue;
+    }
+
+    PrivateData->CapsuleData[PrivateData->CapsuleCount].CapsuleStartLBA = FileRecord->LocationOfExtent[0];
+    PrivateData->CapsuleData[PrivateData->CapsuleCount].CapsuleSize =
+      (
+        FileRecord->DataLength[0] /
+        PEI_CD_BLOCK_SIZE +
+        1
+      ) *
+      PEI_CD_BLOCK_SIZE;
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Returns the number of DXE capsules residing on the device.
+
+  This function searches for DXE capsules from the associated device and returns
+  the number and maximum size in bytes of the capsules discovered. Entry 1 is 
+  assumed to be the highest load priority and entry N is assumed to be the lowest 
+  priority.
+
+  @param[in]  PeiServices              General-purpose services that are available 
+                                       to every PEIM
+  @param[in]  This                     Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI
+                                       instance.
+  @param[out] NumberRecoveryCapsules   Pointer to a caller-allocated UINTN. On 
+                                       output, *NumberRecoveryCapsules contains 
+                                       the number of recovery capsule images 
+                                       available for retrieval from this PEIM 
+                                       instance.
+
+  @retval EFI_SUCCESS        One or more capsules were discovered.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNumberRecoveryCapsules (
+  IN EFI_PEI_SERVICES                               **PeiServices,
+  IN EFI_PEI_DEVICE_RECOVERY_MODULE_PPI             *This,
+  OUT UINTN                                         *NumberRecoveryCapsules
+  )
+{
+  PEI_CD_EXPRESS_PRIVATE_DATA *PrivateData;
+
+  PrivateData = PEI_CD_EXPRESS_PRIVATE_DATA_FROM_THIS (This);
+  UpdateBlocksAndVolumes (PrivateData, TRUE);
+  UpdateBlocksAndVolumes (PrivateData, FALSE);
+  *NumberRecoveryCapsules = PrivateData->CapsuleCount;
+
+  if (*NumberRecoveryCapsules == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns the size and type of the requested recovery capsule.
+
+  This function gets the size and type of the capsule specified by CapsuleInstance.
+
+  @param[in]  PeiServices       General-purpose services that are available to every PEIM
+  @param[in]  This              Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI 
+                                instance.
+  @param[in]  CapsuleInstance   Specifies for which capsule instance to retrieve 
+                                the information.  This parameter must be between 
+                                one and the value returned by GetNumberRecoveryCapsules() 
+                                in NumberRecoveryCapsules.
+  @param[out] Size              A pointer to a caller-allocated UINTN in which 
+                                the size of the requested recovery module is 
+                                returned.
+  @param[out] CapsuleType       A pointer to a caller-allocated EFI_GUID in which 
+                                the type of the requested recovery capsule is 
+                                returned.  The semantic meaning of the value 
+                                returned is defined by the implementation.
+
+  @retval EFI_SUCCESS        One or more capsules were discovered.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+GetRecoveryCapsuleInfo (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_DEVICE_RECOVERY_MODULE_PPI            *This,
+  IN  UINTN                                         CapsuleInstance,
+  OUT UINTN                                         *Size,
+  OUT EFI_GUID                                      *CapsuleType
+  )
+{
+  PEI_CD_EXPRESS_PRIVATE_DATA *PrivateData;
+  UINTN                       NumberRecoveryCapsules;
+  EFI_STATUS                  Status;
+
+  Status = GetNumberRecoveryCapsules (PeiServices, This, &NumberRecoveryCapsules);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    CapsuleInstance = CapsuleInstance + 1;
+  }
+
+  if ((CapsuleInstance == 0) || (CapsuleInstance > NumberRecoveryCapsules)) {
+    return EFI_NOT_FOUND;
+  }
+
+  PrivateData = PEI_CD_EXPRESS_PRIVATE_DATA_FROM_THIS (This);
+
+  *Size = PrivateData->CapsuleData[CapsuleInstance - 1].CapsuleSize;
+  CopyMem (
+    CapsuleType,
+    &gRecoveryOnDataCdGuid,
+    sizeof (EFI_GUID)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Loads a DXE capsule from some media into memory.
+
+  This function, by whatever mechanism, retrieves a DXE capsule from some device
+  and loads it into memory. Note that the published interface is device neutral.
+
+  @param[in]     PeiServices       General-purpose services that are available 
+                                   to every PEIM
+  @param[in]     This              Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI
+                                   instance.
+  @param[in]     CapsuleInstance   Specifies which capsule instance to retrieve.
+  @param[out]    Buffer            Specifies a caller-allocated buffer in which 
+                                   the requested recovery capsule will be returned.
+
+  @retval EFI_SUCCESS        The capsule was loaded correctly.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A requested recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadRecoveryCapsule (
+  IN EFI_PEI_SERVICES                             **PeiServices,
+  IN EFI_PEI_DEVICE_RECOVERY_MODULE_PPI           *This,
+  IN UINTN                                        CapsuleInstance,
+  OUT VOID                                        *Buffer
+  )
+{
+  EFI_STATUS                      Status;
+  PEI_CD_EXPRESS_PRIVATE_DATA     *PrivateData;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   *BlockIoPpi;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  *BlockIo2Ppi;
+  UINTN                           NumberRecoveryCapsules;
+
+  Status = GetNumberRecoveryCapsules (PeiServices, This, &NumberRecoveryCapsules);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    CapsuleInstance = CapsuleInstance + 1;
+  }
+
+  if ((CapsuleInstance == 0) || (CapsuleInstance > NumberRecoveryCapsules)) {
+    return EFI_NOT_FOUND;
+  }
+
+  PrivateData = PEI_CD_EXPRESS_PRIVATE_DATA_FROM_THIS (This);
+  BlockIoPpi  = PrivateData->CapsuleData[CapsuleInstance - 1].BlockIo;
+  BlockIo2Ppi = PrivateData->CapsuleData[CapsuleInstance - 1].BlockIo2;
+
+  if (BlockIo2Ppi != NULL) {
+    Status = BlockIo2Ppi->ReadBlocks (
+                          PeiServices,
+                          BlockIo2Ppi,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].IndexBlock,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].CapsuleStartLBA,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].CapsuleSize,
+                          Buffer
+                          );
+  } else {
+    Status = BlockIoPpi->ReadBlocks (
+                          PeiServices,
+                          BlockIoPpi,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].IndexBlock,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].CapsuleStartLBA,
+                          PrivateData->CapsuleData[CapsuleInstance - 1].CapsuleSize,
+                          Buffer
+                          );
+  }
+  return Status;
+}
+
+/**
+  This function compares two ASCII strings in case sensitive/insensitive way.
+
+  @param  Source1           The first string.
+  @param  Source2           The second string.
+  @param  Size              The maximum comparison length.
+  @param  CaseSensitive     Flag to indicate whether the comparison is case sensitive.
+
+  @retval TRUE              The two strings are the same.
+  @retval FALSE             The two string are not the same.
+
+**/
+BOOLEAN
+StringCmp (
+  IN UINT8      *Source1,
+  IN UINT8      *Source2,
+  IN UINTN      Size,
+  IN BOOLEAN    CaseSensitive
+  )
+{
+  UINTN Index;
+  UINT8 Dif;
+
+  for (Index = 0; Index < Size; Index++) {
+    if (Source1[Index] == Source2[Index]) {
+      continue;
+    }
+
+    if (!CaseSensitive) {
+      Dif = (UINT8) ((Source1[Index] > Source2[Index]) ? (Source1[Index] - Source2[Index]) : (Source2[Index] - Source1[Index]));
+      if (Dif == ('a' - 'A')) {
+        continue;
+      }
+    }
+
+    return FALSE;
+  }
+
+  return TRUE;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.h b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.h
new file mode 100644
index 0000000000..5ca26722a7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/CdExpressPei/PeiCdExpress.h
@@ -0,0 +1,303 @@
+/** @file
+  Header file for CD recovery PEIM
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_CD_EXPRESS_H_
+#define _PEI_CD_EXPRESS_H_
+
+
+#include <PiPei.h>
+
+#include <Ppi/BlockIo.h>
+#include <Ppi/BlockIo2.h>
+#include <Guid/RecoveryDevice.h>
+#include <Ppi/DeviceRecoveryModule.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+
+#pragma pack(1)
+
+#define PEI_CD_EXPRESS_MAX_BLOCK_IO_PPI   8
+#define PEI_CD_EXPRESS_MAX_CAPSULE_NUMBER 16
+
+#define PEI_CD_BLOCK_SIZE                 0x800
+#define PEI_MEMMORY_PAGE_SIZE             0x1000
+
+//
+// Recovery file name (in root directory)
+//
+#define PEI_RECOVERY_FILE_NAME  "FVMAIN.FV"
+
+//
+// Following are defined according to ISO-9660 specification
+//
+#define PEI_CD_STANDARD_ID                      "CD001"
+#define PEI_CD_EXPRESS_STANDARD_ID_SIZE         5
+
+#define PEI_CD_EXPRESS_VOLUME_TYPE_OFFSET       0
+#define PEI_CD_EXPRESS_STANDARD_ID_OFFSET       1
+#define PEI_CD_EXPRESS_VOLUME_SPACE_OFFSET      80
+#define PEI_CD_EXPRESS_ROOT_DIR_RECORD_OFFSET   156
+
+#define PEI_CD_EXPRESS_VOLUME_TYPE_PRIMARY      1
+#define PEI_CD_EXPRESS_VOLUME_TYPE_TERMINATOR   255
+
+#define PEI_CD_EXPRESS_DIR_FILE_REC_FLAG_ISDIR  0x02
+
+typedef struct {
+  UINTN                           CapsuleStartLBA;
+  UINTN                           CapsuleSize;
+  UINTN                           IndexBlock;
+  EFI_PEI_RECOVERY_BLOCK_IO_PPI   *BlockIo;
+  EFI_PEI_RECOVERY_BLOCK_IO2_PPI  *BlockIo2;
+} PEI_CD_EXPRESS_CAPSULE_DATA;
+
+#define PEI_CD_EXPRESS_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('p', 'c', 'd', 'e')
+
+typedef struct {
+
+  UINTN                                 Signature;
+  EFI_PEI_DEVICE_RECOVERY_MODULE_PPI    DeviceRecoveryPpi;
+  EFI_PEI_PPI_DESCRIPTOR                PpiDescriptor;
+  EFI_PEI_NOTIFY_DESCRIPTOR             NotifyDescriptor;
+  EFI_PEI_NOTIFY_DESCRIPTOR             NotifyDescriptor2;
+
+  UINT8                                 *BlockBuffer;
+  UINTN                                 CapsuleCount;
+  PEI_CD_EXPRESS_CAPSULE_DATA           CapsuleData[PEI_CD_EXPRESS_MAX_CAPSULE_NUMBER];
+
+} PEI_CD_EXPRESS_PRIVATE_DATA;
+
+#define PEI_CD_EXPRESS_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+          PEI_CD_EXPRESS_PRIVATE_DATA, \
+          DeviceRecoveryPpi, \
+          PEI_CD_EXPRESS_PRIVATE_DATA_SIGNATURE \
+      )
+
+typedef struct {
+  UINT8   Length;
+  UINT8   ExtendedAttributeRecordLength;
+  UINT32  LocationOfExtent[2];
+  UINT32  DataLength[2];
+  UINT8   DateTime[7];
+  UINT8   Flag;
+  UINT8   FileUnitSize;
+  UINT8   InterleaveGapSize;
+  UINT32  VolumeSequenceNumber;
+  UINT8   FileIDLength;
+  UINT8   FileID[1];
+} PEI_CD_EXPRESS_DIR_FILE_RECORD;
+
+/**
+  BlockIo installation notification function. 
+  
+  This function finds out all the current Block IO PPIs in the system and add them
+  into private data.
+
+  @param  PeiServices            Indirect reference to the PEI Services Table.
+  @param  NotifyDescriptor       Address of the notification descriptor data structure.
+  @param  Ppi                    Address of the PPI that was installed.
+
+  @retval EFI_SUCCESS            The function completes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+BlockIoNotifyEntry (
+  IN EFI_PEI_SERVICES           **PeiServices,
+  IN EFI_PEI_NOTIFY_DESCRIPTOR  *NotifyDescriptor,
+  IN VOID                       *Ppi
+  );
+
+/**
+  Finds out all the current Block IO PPIs in the system and add them into private data.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+  @param BlockIo2                       Boolean to show whether using BlockIo2 or BlockIo.
+
+  @retval EFI_SUCCESS                   The blocks and volumes are updated successfully.
+
+**/
+EFI_STATUS
+UpdateBlocksAndVolumes (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA     *PrivateData,
+  IN     BOOLEAN                         BlockIo2
+  );
+
+/**
+  Returns the number of DXE capsules residing on the device.
+
+  This function searches for DXE capsules from the associated device and returns
+  the number and maximum size in bytes of the capsules discovered. Entry 1 is 
+  assumed to be the highest load priority and entry N is assumed to be the lowest 
+  priority.
+
+  @param[in]  PeiServices              General-purpose services that are available 
+                                       to every PEIM
+  @param[in]  This                     Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI
+                                       instance.
+  @param[out] NumberRecoveryCapsules   Pointer to a caller-allocated UINTN. On 
+                                       output, *NumberRecoveryCapsules contains 
+                                       the number of recovery capsule images 
+                                       available for retrieval from this PEIM 
+                                       instance.
+
+  @retval EFI_SUCCESS        One or more capsules were discovered.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNumberRecoveryCapsules (
+  IN EFI_PEI_SERVICES                               **PeiServices,
+  IN EFI_PEI_DEVICE_RECOVERY_MODULE_PPI             *This,
+  OUT UINTN                                         *NumberRecoveryCapsules
+  );
+
+/**
+  Returns the size and type of the requested recovery capsule.
+
+  This function gets the size and type of the capsule specified by CapsuleInstance.
+
+  @param[in]  PeiServices       General-purpose services that are available to every PEIM
+  @param[in]  This              Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI 
+                                instance.
+  @param[in]  CapsuleInstance   Specifies for which capsule instance to retrieve 
+                                the information.  This parameter must be between 
+                                one and the value returned by GetNumberRecoveryCapsules() 
+                                in NumberRecoveryCapsules.
+  @param[out] Size              A pointer to a caller-allocated UINTN in which 
+                                the size of the requested recovery module is 
+                                returned.
+  @param[out] CapsuleType       A pointer to a caller-allocated EFI_GUID in which 
+                                the type of the requested recovery capsule is 
+                                returned.  The semantic meaning of the value 
+                                returned is defined by the implementation.
+
+  @retval EFI_SUCCESS        One or more capsules were discovered.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+GetRecoveryCapsuleInfo (
+  IN  EFI_PEI_SERVICES                              **PeiServices,
+  IN  EFI_PEI_DEVICE_RECOVERY_MODULE_PPI            *This,
+  IN  UINTN                                         CapsuleInstance,
+  OUT UINTN                                         *Size,
+  OUT EFI_GUID                                      *CapsuleType
+  );
+
+/**
+  Loads a DXE capsule from some media into memory.
+
+  This function, by whatever mechanism, retrieves a DXE capsule from some device
+  and loads it into memory. Note that the published interface is device neutral.
+
+  @param[in]     PeiServices       General-purpose services that are available 
+                                   to every PEIM
+  @param[in]     This              Indicates the EFI_PEI_DEVICE_RECOVERY_MODULE_PPI
+                                   instance.
+  @param[in]     CapsuleInstance   Specifies which capsule instance to retrieve.
+  @param[out]    Buffer            Specifies a caller-allocated buffer in which 
+                                   the requested recovery capsule will be returned.
+
+  @retval EFI_SUCCESS        The capsule was loaded correctly.
+  @retval EFI_DEVICE_ERROR   A device error occurred.
+  @retval EFI_NOT_FOUND      A requested recovery DXE capsule cannot be found.
+
+**/
+EFI_STATUS
+EFIAPI
+LoadRecoveryCapsule (
+  IN EFI_PEI_SERVICES                             **PeiServices,
+  IN EFI_PEI_DEVICE_RECOVERY_MODULE_PPI           *This,
+  IN UINTN                                        CapsuleInstance,
+  OUT VOID                                        *Buffer
+  );
+
+/**
+  Finds out the recovery capsule in the current volume.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+
+  @retval EFI_SUCCESS                   The recovery capsule is successfully found in the volume.
+  @retval EFI_NOT_FOUND                 The recovery capsule is not found in the volume.
+
+**/
+EFI_STATUS
+EFIAPI
+FindRecoveryCapsules (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA            *PrivateData
+  );
+
+/**
+  Retrieves the recovery capsule in root directory of the current volume.
+
+  @param PrivateData                    The private data structure that contains recovery module information.
+  @param BlockIoPpi                     The Block IO PPI used to access the volume.
+  @param BlockIo2Ppi                    The Block IO 2 PPI used to access the volume.
+  @param IndexBlockDevice               The index of current block device.
+  @param Lba                            The starting logic block address to retrieve capsule.
+
+  @retval EFI_SUCCESS                   The recovery capsule is successfully found in the volume.
+  @retval EFI_NOT_FOUND                 The recovery capsule is not found in the volume.
+  @retval Others                        
+
+**/
+EFI_STATUS
+EFIAPI
+RetrieveCapsuleFileFromRoot (
+  IN OUT PEI_CD_EXPRESS_PRIVATE_DATA        *PrivateData,
+  IN EFI_PEI_RECOVERY_BLOCK_IO_PPI          *BlockIoPpi,
+  IN EFI_PEI_RECOVERY_BLOCK_IO2_PPI         *BlockIo2Ppi,
+  IN UINTN                                  IndexBlockDevice,
+  IN UINT32                                 Lba
+  );
+
+
+/**
+  This function compares two ASCII strings in case sensitive/insensitive way.
+
+  @param  Source1           The first string.
+  @param  Source2           The second string.
+  @param  Size              The maximum comparison length.
+  @param  CaseSensitive     Flag to indicate whether the comparison is case sensitive.
+
+  @retval TRUE              The two strings are the same.
+  @retval FALSE             The two string are not the same.
+
+**/
+BOOLEAN
+StringCmp (
+  IN UINT8      *Source1,
+  IN UINT8      *Source2,
+  IN UINTN      Size,
+  IN BOOLEAN    CaseSensitive
+  );
+
+#pragma pack()
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/ComponentName.c
new file mode 100644
index 0000000000..8c3db5e725
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/ComponentName.c
@@ -0,0 +1,189 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for DiskIo driver.
+
+Copyright (c) 2006 - 2011, 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 "DiskIo.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gDiskIoComponentName = {
+  DiskIoComponentNameGetDriverName,
+  DiskIoComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gDiskIoComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) DiskIoComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) DiskIoComponentNameGetControllerName,
+  "en"
+};
+
+//
+// Driver name table for DiskIo module.
+// It is shared by the implementation of ComponentName & ComponentName2 Protocol.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mDiskIoDriverNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *)L"Generic Disk I/O Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mDiskIoDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gDiskIoComponentName)
+           );
+}
+
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.c b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.c
new file mode 100644
index 0000000000..97f2d508a2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.c
@@ -0,0 +1,1280 @@
+/** @file
+  DiskIo driver that lays on every BlockIo protocol in the system.
+  DiskIo converts a block oriented device to a byte oriented device.
+
+  Disk access may have to handle unaligned request about sector boundaries.
+  There are three cases:
+    UnderRun - The first byte is not on a sector boundary or the read request is
+               less than a sector in length.
+    Aligned  - A read of N contiguous sectors.
+    OverRun  - The last byte is not on a sector boundary.
+
+Copyright (c) 2006 - 2014, 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 "DiskIo.h"
+
+//
+// Driver binding protocol implementation for DiskIo driver.
+//
+EFI_DRIVER_BINDING_PROTOCOL gDiskIoDriverBinding = {
+  DiskIoDriverBindingSupported,
+  DiskIoDriverBindingStart,
+  DiskIoDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+//
+// Template for DiskIo private data structure.
+// The pointer to BlockIo protocol interface is assigned dynamically.
+//
+DISK_IO_PRIVATE_DATA        gDiskIoPrivateDataTemplate = {
+  DISK_IO_PRIVATE_DATA_SIGNATURE,
+  {
+    EFI_DISK_IO_PROTOCOL_REVISION,
+    DiskIoReadDisk,
+    DiskIoWriteDisk
+  },
+  {
+    EFI_DISK_IO2_PROTOCOL_REVISION,
+    DiskIo2Cancel,
+    DiskIo2ReadDiskEx,
+    DiskIo2WriteDiskEx,
+    DiskIo2FlushDiskEx
+  }
+};
+
+/**
+  Test to see if this driver supports ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  EFI_BLOCK_IO_PROTOCOL *BlockIo;
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test.
+  //
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiBlockIoProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Start this driver on ControllerHandle by opening a Block IO protocol and
+  installing a Disk IO protocol on ControllerHandle.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  DISK_IO_PRIVATE_DATA  *Instance;
+  EFI_TPL               OldTpl;
+
+  Instance = NULL;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Connect to the Block IO and Block IO2 interface on ControllerHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &gDiskIoPrivateDataTemplate.BlockIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit1;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIo2ProtocolGuid,
+                  (VOID **) &gDiskIoPrivateDataTemplate.BlockIo2,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    gDiskIoPrivateDataTemplate.BlockIo2 = NULL;
+  }
+  
+  //
+  // Initialize the Disk IO device instance.
+  //
+  Instance = AllocateCopyPool (sizeof (DISK_IO_PRIVATE_DATA), &gDiskIoPrivateDataTemplate);
+  if (Instance == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+  
+  //
+  // The BlockSize and IoAlign of BlockIo and BlockIo2 should equal.
+  //
+  ASSERT ((Instance->BlockIo2 == NULL) ||
+          ((Instance->BlockIo->Media->IoAlign == Instance->BlockIo2->Media->IoAlign) && 
+           (Instance->BlockIo->Media->BlockSize == Instance->BlockIo2->Media->BlockSize)
+          ));
+  
+  InitializeListHead (&Instance->TaskQueue);
+  EfiInitializeLock (&Instance->TaskQueueLock, TPL_NOTIFY);
+  Instance->SharedWorkingBuffer = AllocateAlignedPages (
+                                    EFI_SIZE_TO_PAGES (PcdGet32 (PcdDiskIoDataBufferBlockNum) * Instance->BlockIo->Media->BlockSize),
+                                    Instance->BlockIo->Media->IoAlign
+                                    );
+  if (Instance->SharedWorkingBuffer == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  //
+  // Install protocol interfaces for the Disk IO device.
+  //
+  if (Instance->BlockIo2 != NULL) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &ControllerHandle,
+                    &gEfiDiskIoProtocolGuid,  &Instance->DiskIo,
+                    &gEfiDiskIo2ProtocolGuid, &Instance->DiskIo2,
+                    NULL
+                    );
+  } else {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &ControllerHandle,
+                    &gEfiDiskIoProtocolGuid,  &Instance->DiskIo,
+                    NULL
+                    );
+  }
+
+ErrorExit:
+  if (EFI_ERROR (Status)) {
+    if (Instance != NULL && Instance->SharedWorkingBuffer != NULL) {
+      FreeAlignedPages (
+        Instance->SharedWorkingBuffer,
+        EFI_SIZE_TO_PAGES (PcdGet32 (PcdDiskIoDataBufferBlockNum) * Instance->BlockIo->Media->BlockSize)
+        );
+    }
+
+    if (Instance != NULL) {
+      FreePool (Instance);
+    }
+
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiBlockIoProtocolGuid,
+           This->DriverBindingHandle,
+           ControllerHandle
+           );
+  }
+
+ErrorExit1:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle by removing Disk IO protocol and closing
+  the Block IO protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     ControllerHandle,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS            Status;
+  EFI_DISK_IO_PROTOCOL  *DiskIo;
+  EFI_DISK_IO2_PROTOCOL *DiskIo2;
+  DISK_IO_PRIVATE_DATA  *Instance;
+  BOOLEAN               AllTaskDone;
+
+  //
+  // Get our context back.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  (VOID **) &DiskIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIo2ProtocolGuid,
+                  (VOID **) &DiskIo2,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DiskIo2 = NULL;
+  }
+  
+  Instance = DISK_IO_PRIVATE_DATA_FROM_DISK_IO (DiskIo);
+
+  if (DiskIo2 != NULL) {
+    //
+    // Call BlockIo2::Reset() to terminate any in-flight non-blocking I/O requests
+    //
+    ASSERT (Instance->BlockIo2 != NULL);
+    Status = Instance->BlockIo2->Reset (Instance->BlockIo2, FALSE);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ControllerHandle,
+                    &gEfiDiskIoProtocolGuid,  &Instance->DiskIo,
+                    &gEfiDiskIo2ProtocolGuid, &Instance->DiskIo2,
+                    NULL
+                    );
+  } else {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    ControllerHandle,
+                    &gEfiDiskIoProtocolGuid,  &Instance->DiskIo,
+                    NULL
+                    );
+  }
+  if (!EFI_ERROR (Status)) {
+    
+    do {
+      EfiAcquireLock (&Instance->TaskQueueLock);
+      AllTaskDone = IsListEmpty (&Instance->TaskQueue);
+      EfiReleaseLock (&Instance->TaskQueueLock);
+    } while (!AllTaskDone);
+
+    FreeAlignedPages (
+      Instance->SharedWorkingBuffer,
+      EFI_SIZE_TO_PAGES (PcdGet32 (PcdDiskIoDataBufferBlockNum) * Instance->BlockIo->Media->BlockSize)
+      );
+
+    Status = gBS->CloseProtocol (
+                    ControllerHandle,
+                    &gEfiBlockIoProtocolGuid,
+                    This->DriverBindingHandle,
+                    ControllerHandle
+                    );
+    ASSERT_EFI_ERROR (Status);
+    if (DiskIo2 != NULL) {
+      Status = gBS->CloseProtocol (
+                      ControllerHandle,
+                      &gEfiBlockIo2ProtocolGuid,
+                      This->DriverBindingHandle,
+                      ControllerHandle
+                      );
+      ASSERT_EFI_ERROR (Status);
+    }
+    
+    FreePool (Instance);
+  }
+
+  return Status;
+}
+
+
+/**
+  Destroy the sub task.
+
+  @param Instance     Pointer to the DISK_IO_PRIVATE_DATA.
+  @param Subtask      Subtask.
+
+  @return LIST_ENTRY *  Pointer to the next link of subtask.
+**/
+LIST_ENTRY *
+DiskIoDestroySubtask (
+  IN DISK_IO_PRIVATE_DATA     *Instance,
+  IN DISK_IO_SUBTASK          *Subtask
+  )
+{
+  LIST_ENTRY               *Link;
+
+  if (Subtask->Task != NULL) {
+    EfiAcquireLock (&Subtask->Task->SubtasksLock);
+  }
+  Link = RemoveEntryList (&Subtask->Link);
+  if (Subtask->Task != NULL) {
+    EfiReleaseLock (&Subtask->Task->SubtasksLock);
+  }
+
+  if (!Subtask->Blocking) {
+    if (Subtask->WorkingBuffer != NULL) {
+      FreeAlignedPages (
+        Subtask->WorkingBuffer, 
+        Subtask->Length < Instance->BlockIo->Media->BlockSize
+        ? EFI_SIZE_TO_PAGES (Instance->BlockIo->Media->BlockSize)
+        : EFI_SIZE_TO_PAGES (Subtask->Length)
+        );
+    }
+    if (Subtask->BlockIo2Token.Event != NULL) {
+      gBS->CloseEvent (Subtask->BlockIo2Token.Event);
+    }
+  }
+  FreePool (Subtask);
+
+  return Link;
+}
+
+/**
+  The callback for the BlockIo2 ReadBlocksEx/WriteBlocksEx.
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               The pointer to the notification function's context,
+                                which points to the DISK_IO_SUBTASK instance.
+**/
+VOID
+EFIAPI
+DiskIo2OnReadWriteComplete (
+  IN EFI_EVENT            Event,
+  IN VOID                 *Context
+  )
+{
+  DISK_IO_SUBTASK       *Subtask;
+  DISK_IO2_TASK         *Task;
+  EFI_STATUS            TransactionStatus;
+  DISK_IO_PRIVATE_DATA  *Instance;
+
+  Subtask           = (DISK_IO_SUBTASK *) Context;
+  TransactionStatus = Subtask->BlockIo2Token.TransactionStatus;
+  Task              = Subtask->Task;
+  Instance          = Task->Instance;
+
+  ASSERT (Subtask->Signature  == DISK_IO_SUBTASK_SIGNATURE);
+  ASSERT (Instance->Signature == DISK_IO_PRIVATE_DATA_SIGNATURE);
+  ASSERT (Task->Signature     == DISK_IO2_TASK_SIGNATURE);
+
+  if ((Subtask->WorkingBuffer != NULL) && !EFI_ERROR (TransactionStatus) && 
+      (Task->Token != NULL) && !Subtask->Write
+     ) {
+    CopyMem (Subtask->Buffer, Subtask->WorkingBuffer + Subtask->Offset, Subtask->Length);
+  }
+
+  DiskIoDestroySubtask (Instance, Subtask);
+
+  if (EFI_ERROR (TransactionStatus) || IsListEmpty (&Task->Subtasks)) {
+    if (Task->Token != NULL) {
+      //
+      // Signal error status once the subtask is failed.
+      // Or signal the last status once the last subtask is finished.
+      //
+      Task->Token->TransactionStatus = TransactionStatus;
+      gBS->SignalEvent (Task->Token->Event);
+
+      //
+      // Mark token to NULL indicating the Task is a dead task.
+      //
+      Task->Token = NULL;
+    }
+  }
+}
+
+/**
+  Create the subtask.
+
+  @param Write         TRUE: Write request; FALSE: Read request.
+  @param Lba           The starting logical block address to read from on the device.
+  @param Offset        The starting byte offset to read from the LBA.
+  @param Length        The number of bytes to read from the device.
+  @param WorkingBuffer The aligned buffer to hold the data for reading or writing.
+  @param Buffer        The buffer to hold the data for reading or writing.
+  @param Blocking      TRUE: Blocking request; FALSE: Non-blocking request.
+
+  @return A pointer to the created subtask.
+**/
+DISK_IO_SUBTASK *
+DiskIoCreateSubtask (
+  IN BOOLEAN          Write,
+  IN UINT64           Lba,
+  IN UINT32           Offset,
+  IN UINTN            Length,
+  IN VOID             *WorkingBuffer,  OPTIONAL
+  IN VOID             *Buffer,
+  IN BOOLEAN          Blocking
+  )
+{
+  DISK_IO_SUBTASK       *Subtask;
+  EFI_STATUS            Status;
+
+  Subtask = AllocateZeroPool (sizeof (DISK_IO_SUBTASK));
+  if (Subtask == NULL) {
+    return NULL;
+  }
+  Subtask->Signature     = DISK_IO_SUBTASK_SIGNATURE;
+  Subtask->Write         = Write;
+  Subtask->Lba           = Lba;
+  Subtask->Offset        = Offset;
+  Subtask->Length        = Length;
+  Subtask->WorkingBuffer = WorkingBuffer;
+  Subtask->Buffer        = Buffer;
+  Subtask->Blocking      = Blocking;
+  if (!Blocking) {
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    DiskIo2OnReadWriteComplete,
+                    Subtask,
+                    &Subtask->BlockIo2Token.Event
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (Subtask);
+      return NULL;
+    }
+  }
+  DEBUG ((
+    EFI_D_BLKIO, 
+    "  %c:Lba/Offset/Length/WorkingBuffer/Buffer = %016lx/%08x/%08x/%08x/%08x\n",
+    Write ? 'W': 'R', Lba, Offset, Length, WorkingBuffer, Buffer
+    ));
+
+  return Subtask;
+}
+
+/**
+  Create the subtask list.
+
+  @param Instance            Pointer to the DISK_IO_PRIVATE_DATA.
+  @param Write               TRUE: Write request; FALSE: Read request.
+  @param Offset              The starting byte offset to read from the device.
+  @param BufferSize          The size in bytes of Buffer. The number of bytes to read from the device.
+  @param Buffer              A pointer to the buffer for the data.
+  @param Blocking            TRUE: Blocking request; FALSE: Non-blocking request.
+  @param SharedWorkingBuffer The aligned buffer to hold the data for reading or writing.
+  @param Subtasks            The subtask list header.
+
+  @retval TRUE  The subtask list is created successfully.
+  @retval FALSE The subtask list is not created.
+**/
+BOOLEAN
+DiskIoCreateSubtaskList (
+  IN DISK_IO_PRIVATE_DATA  *Instance,
+  IN BOOLEAN               Write,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  IN VOID                  *Buffer,
+  IN BOOLEAN               Blocking,
+  IN VOID                  *SharedWorkingBuffer,
+  IN OUT LIST_ENTRY        *Subtasks
+  )
+{
+  UINT32                BlockSize;
+  UINT32                IoAlign;
+  UINT64                Lba;
+  UINT64                OverRunLba;
+  UINT32                UnderRun;
+  UINT32                OverRun;
+  UINT8                 *BufferPtr;
+  UINTN                 Length;
+  UINTN                 DataBufferSize;
+  DISK_IO_SUBTASK       *Subtask;
+  VOID                  *WorkingBuffer;
+  LIST_ENTRY            *Link;
+
+  DEBUG ((EFI_D_BLKIO, "DiskIo: Create subtasks for task: Offset/BufferSize/Buffer = %016lx/%08x/%08x\n", Offset, BufferSize, Buffer));
+
+  BlockSize = Instance->BlockIo->Media->BlockSize;
+  IoAlign   = Instance->BlockIo->Media->IoAlign;
+  if (IoAlign == 0) {
+    IoAlign = 1;
+  }
+  
+  Lba       = DivU64x32Remainder (Offset, BlockSize, &UnderRun);
+  BufferPtr = (UINT8 *) Buffer;
+
+  //
+  // Special handling for zero BufferSize
+  //
+  if (BufferSize == 0) {
+    Subtask = DiskIoCreateSubtask (Write, Lba, UnderRun, 0, NULL, BufferPtr, Blocking);
+    if (Subtask == NULL) {
+      goto Done;
+    }
+    InsertTailList (Subtasks, &Subtask->Link);
+    return TRUE;
+  }
+
+  if (UnderRun != 0) {
+    Length = MIN (BlockSize - UnderRun, BufferSize);
+    if (Blocking) {
+      WorkingBuffer = SharedWorkingBuffer;
+    } else {
+      WorkingBuffer = AllocateAlignedPages (EFI_SIZE_TO_PAGES (BlockSize), IoAlign);
+      if (WorkingBuffer == NULL) {
+        goto Done;
+      }
+    }
+    if (Write) {
+      //
+      // A half write operation can be splitted to a blocking block-read and half write operation
+      // This can simplify the sub task processing logic
+      //
+      Subtask = DiskIoCreateSubtask (FALSE, Lba, 0, BlockSize, NULL, WorkingBuffer, TRUE);
+      if (Subtask == NULL) {
+        goto Done;
+      }
+      InsertTailList (Subtasks, &Subtask->Link);
+    }
+
+    Subtask = DiskIoCreateSubtask (Write, Lba, UnderRun, Length, WorkingBuffer, BufferPtr, Blocking);
+    if (Subtask == NULL) {
+      goto Done;
+    }
+    InsertTailList (Subtasks, &Subtask->Link);
+  
+    BufferPtr  += Length;
+    Offset     += Length;
+    BufferSize -= Length;
+    Lba ++;
+  }
+
+  OverRunLba  = Lba + DivU64x32Remainder (BufferSize, BlockSize, &OverRun);
+  BufferSize -= OverRun;
+
+  if (OverRun != 0) {
+    if (Blocking) {
+      WorkingBuffer = SharedWorkingBuffer;
+    } else {
+      WorkingBuffer = AllocateAlignedPages (EFI_SIZE_TO_PAGES (BlockSize), IoAlign);
+      if (WorkingBuffer == NULL) {
+        goto Done;
+      }
+    }
+    if (Write) {
+      //
+      // A half write operation can be splitted to a blocking block-read and half write operation
+      // This can simplify the sub task processing logic
+      //
+      Subtask = DiskIoCreateSubtask (FALSE, OverRunLba, 0, BlockSize, NULL, WorkingBuffer, TRUE);
+      if (Subtask == NULL) {
+        goto Done;
+      }
+      InsertTailList (Subtasks, &Subtask->Link);
+    }
+
+    Subtask = DiskIoCreateSubtask (Write, OverRunLba, 0, OverRun, WorkingBuffer, BufferPtr + BufferSize, Blocking);
+    if (Subtask == NULL) {
+      goto Done;
+    }
+    InsertTailList (Subtasks, &Subtask->Link);
+  }
+  
+  if (OverRunLba > Lba) {
+    //
+    // If the DiskIo maps directly to a BlockIo device do the read.
+    //
+    if (ALIGN_POINTER (BufferPtr, IoAlign) == BufferPtr) {
+      Subtask = DiskIoCreateSubtask (Write, Lba, 0, BufferSize, NULL, BufferPtr, Blocking);
+      if (Subtask == NULL) {
+        goto Done;
+      }
+      InsertTailList (Subtasks, &Subtask->Link);
+
+      BufferPtr  += BufferSize;
+      Offset     += BufferSize;
+      BufferSize -= BufferSize;
+
+    } else {
+      if (Blocking) {
+        //
+        // Use the allocated buffer instead of the original buffer
+        // to avoid alignment issue.
+        //
+        for (; Lba < OverRunLba; Lba += PcdGet32 (PcdDiskIoDataBufferBlockNum)) {
+          DataBufferSize = MIN (BufferSize, PcdGet32 (PcdDiskIoDataBufferBlockNum) * BlockSize);
+
+          Subtask = DiskIoCreateSubtask (Write, Lba, 0, DataBufferSize, SharedWorkingBuffer, BufferPtr, Blocking);
+          if (Subtask == NULL) {
+            goto Done;
+          }
+          InsertTailList (Subtasks, &Subtask->Link);
+
+          BufferPtr  += DataBufferSize;
+          Offset     += DataBufferSize;
+          BufferSize -= DataBufferSize;
+        }
+      } else {
+        WorkingBuffer = AllocateAlignedPages (EFI_SIZE_TO_PAGES (BufferSize), IoAlign);
+        if (WorkingBuffer == NULL) {
+          //
+          // If there is not enough memory, downgrade to blocking access
+          //
+          DEBUG ((EFI_D_VERBOSE, "DiskIo: No enough memory so downgrade to blocking access\n"));
+          if (!DiskIoCreateSubtaskList (Instance, Write, Offset, BufferSize, BufferPtr, TRUE, SharedWorkingBuffer, Subtasks)) {
+            goto Done;
+          }
+        } else {
+          Subtask = DiskIoCreateSubtask (Write, Lba, 0, BufferSize, WorkingBuffer, BufferPtr, Blocking);
+          if (Subtask == NULL) {
+            goto Done;
+          }
+          InsertTailList (Subtasks, &Subtask->Link);
+        }
+
+        BufferPtr  += BufferSize;
+        Offset     += BufferSize;
+        BufferSize -= BufferSize;
+      }
+    }
+  }
+
+  ASSERT (BufferSize == 0);
+
+  return TRUE;
+
+Done:
+  //
+  // Remove all the subtasks.
+  //
+  for (Link = GetFirstNode (Subtasks); !IsNull (Subtasks, Link); ) {
+    Subtask = CR (Link, DISK_IO_SUBTASK, Link, DISK_IO_SUBTASK_SIGNATURE);
+    Link = DiskIoDestroySubtask (Instance, Subtask);
+  }
+  return FALSE;
+}
+
+/**
+  Terminate outstanding asynchronous requests to a device.
+
+  @param This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS           All outstanding requests were successfully terminated.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the cancel
+                                operation.
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2Cancel (
+  IN EFI_DISK_IO2_PROTOCOL *This
+  )
+{
+  DISK_IO_PRIVATE_DATA  *Instance;
+  DISK_IO2_TASK         *Task;
+  LIST_ENTRY            *Link;
+  
+  Instance = DISK_IO_PRIVATE_DATA_FROM_DISK_IO2 (This);
+
+  EfiAcquireLock (&Instance->TaskQueueLock);
+
+  for (Link = GetFirstNode (&Instance->TaskQueue)
+    ; !IsNull (&Instance->TaskQueue, Link)
+    ; Link = GetNextNode (&Instance->TaskQueue, Link)
+    ) {
+    Task = CR (Link, DISK_IO2_TASK, Link, DISK_IO2_TASK_SIGNATURE);
+
+    if (Task->Token != NULL) {
+      Task->Token->TransactionStatus = EFI_ABORTED;
+      gBS->SignalEvent (Task->Token->Event);
+      //
+      // Set Token to NULL so that the further BlockIo2 responses will be ignored
+      //
+      Task->Token = NULL;
+    }
+  }
+
+  EfiReleaseLock (&Instance->TaskQueueLock);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove the completed tasks from Instance->TaskQueue. Completed tasks are those who don't have any subtasks.
+
+  @param Instance    Pointer to the DISK_IO_PRIVATE_DATA.
+
+  @retval TRUE       The Instance->TaskQueue is empty after the completed tasks are removed.
+  @retval FALSE      The Instance->TaskQueue is not empty after the completed tasks are removed.
+**/
+BOOLEAN
+DiskIo2RemoveCompletedTask (
+  IN DISK_IO_PRIVATE_DATA     *Instance
+  )
+{
+  BOOLEAN                     QueueEmpty;
+  LIST_ENTRY                  *Link;
+  DISK_IO2_TASK               *Task;
+
+  QueueEmpty = TRUE;
+
+  EfiAcquireLock (&Instance->TaskQueueLock);
+  for (Link = GetFirstNode (&Instance->TaskQueue); !IsNull (&Instance->TaskQueue, Link); ) {
+    Task = CR (Link, DISK_IO2_TASK, Link, DISK_IO2_TASK_SIGNATURE);
+    if (IsListEmpty (&Task->Subtasks)) {
+      Link = RemoveEntryList (&Task->Link);
+      ASSERT (Task->Token == NULL);
+      FreePool (Task);
+    } else {
+      Link = GetNextNode (&Instance->TaskQueue, Link);
+      QueueEmpty = FALSE;
+    }
+  }
+  EfiReleaseLock (&Instance->TaskQueueLock);
+
+  return QueueEmpty;
+}
+
+/**
+  Common routine to access the disk.
+
+  @param Instance    Pointer to the DISK_IO_PRIVATE_DATA.
+  @param Write       TRUE: Write operation; FALSE: Read operation.
+  @param MediaId     ID of the medium to access.
+  @param Offset      The starting byte offset on the logical block I/O device to access.
+  @param Token       A pointer to the token associated with the transaction.
+                     If this field is NULL, synchronous/blocking IO is performed.
+  @param  BufferSize            The size in bytes of Buffer. The number of bytes to read from the device.
+  @param  Buffer                A pointer to the destination buffer for the data.
+                                The caller is responsible either having implicit or explicit ownership of the buffer. 
+**/
+EFI_STATUS
+DiskIo2ReadWriteDisk (
+  IN DISK_IO_PRIVATE_DATA     *Instance,
+  IN BOOLEAN                  Write,
+  IN UINT32                   MediaId,
+  IN UINT64                   Offset,
+  IN EFI_DISK_IO2_TOKEN       *Token,
+  IN UINTN                    BufferSize,
+  IN UINT8                    *Buffer
+  )
+{
+  EFI_STATUS             Status;
+  EFI_BLOCK_IO_PROTOCOL  *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL *BlockIo2;
+  EFI_BLOCK_IO_MEDIA     *Media;
+  LIST_ENTRY             *Link;
+  LIST_ENTRY             *NextLink;
+  LIST_ENTRY             Subtasks;
+  DISK_IO_SUBTASK        *Subtask;
+  DISK_IO2_TASK          *Task;
+  EFI_TPL                OldTpl;
+  BOOLEAN                Blocking;
+  BOOLEAN                SubtaskBlocking;
+  LIST_ENTRY             *SubtasksPtr;
+
+  Task      = NULL;
+  BlockIo   = Instance->BlockIo;
+  BlockIo2  = Instance->BlockIo2;
+  Media     = BlockIo->Media;
+  Status    = EFI_SUCCESS;
+  Blocking  = (BOOLEAN) ((Token == NULL) || (Token->Event == NULL));
+
+  if (!(Media->MediaPresent)) {
+    return EFI_NO_MEDIA;
+  }
+
+  if (Media->MediaId != MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+  
+  if (Write && Media->ReadOnly) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  if (Blocking) {
+    //
+    // Wait till pending async task is completed.
+    //
+    while (!DiskIo2RemoveCompletedTask (Instance));
+
+    SubtasksPtr = &Subtasks;
+  } else {
+    DiskIo2RemoveCompletedTask (Instance);
+    Task = AllocatePool (sizeof (DISK_IO2_TASK));
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    EfiAcquireLock (&Instance->TaskQueueLock);
+    InsertTailList (&Instance->TaskQueue, &Task->Link);
+    EfiReleaseLock (&Instance->TaskQueueLock);
+
+    Task->Signature = DISK_IO2_TASK_SIGNATURE;
+    Task->Instance  = Instance;
+    Task->Token     = Token;
+    EfiInitializeLock (&Task->SubtasksLock, TPL_NOTIFY);
+
+    SubtasksPtr = &Task->Subtasks;
+  }
+
+  InitializeListHead (SubtasksPtr);
+  if (!DiskIoCreateSubtaskList (Instance, Write, Offset, BufferSize, Buffer, Blocking, Instance->SharedWorkingBuffer, SubtasksPtr)) {
+    if (Task != NULL) {
+      FreePool (Task);
+    }
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ASSERT (!IsListEmpty (SubtasksPtr));
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  for ( Link = GetFirstNode (SubtasksPtr), NextLink = GetNextNode (SubtasksPtr, Link)
+      ; !IsNull (SubtasksPtr, Link)
+      ; Link = NextLink, NextLink = GetNextNode (SubtasksPtr, NextLink)
+      ) {
+    Subtask         = CR (Link, DISK_IO_SUBTASK, Link, DISK_IO_SUBTASK_SIGNATURE);
+    Subtask->Task   = Task;
+    SubtaskBlocking = Subtask->Blocking;
+
+    ASSERT ((Subtask->Length % Media->BlockSize == 0) || (Subtask->Length < Media->BlockSize));
+
+    if (Subtask->Write) {
+      //
+      // Write
+      //
+      if (Subtask->WorkingBuffer != NULL) {
+        //
+        // A sub task before this one should be a block read operation, causing the WorkingBuffer filled with the entire one block data.
+        //
+        CopyMem (Subtask->WorkingBuffer + Subtask->Offset, Subtask->Buffer, Subtask->Length);
+      }
+
+      if (SubtaskBlocking) {
+        Status = BlockIo->WriteBlocks (
+                            BlockIo,
+                            MediaId,
+                            Subtask->Lba,
+                            (Subtask->Length % Media->BlockSize == 0) ? Subtask->Length : Media->BlockSize,
+                            (Subtask->WorkingBuffer != NULL) ? Subtask->WorkingBuffer : Subtask->Buffer
+                            );
+      } else {
+        Status = BlockIo2->WriteBlocksEx (
+                             BlockIo2,
+                             MediaId,
+                             Subtask->Lba,
+                             &Subtask->BlockIo2Token,
+                             (Subtask->Length % Media->BlockSize == 0) ? Subtask->Length : Media->BlockSize,
+                             (Subtask->WorkingBuffer != NULL) ? Subtask->WorkingBuffer : Subtask->Buffer
+                             );
+      }
+
+    } else {
+      //
+      // Read
+      //
+      if (SubtaskBlocking) {
+        Status = BlockIo->ReadBlocks (
+                            BlockIo,
+                            MediaId,
+                            Subtask->Lba,
+                            (Subtask->Length % Media->BlockSize == 0) ? Subtask->Length : Media->BlockSize,
+                            (Subtask->WorkingBuffer != NULL) ? Subtask->WorkingBuffer : Subtask->Buffer
+                            );
+        if (!EFI_ERROR (Status) && (Subtask->WorkingBuffer != NULL)) {
+          CopyMem (Subtask->Buffer, Subtask->WorkingBuffer + Subtask->Offset, Subtask->Length);
+        }
+      } else {
+        Status = BlockIo2->ReadBlocksEx (
+                             BlockIo2,
+                             MediaId,
+                             Subtask->Lba,
+                             &Subtask->BlockIo2Token,
+                             (Subtask->Length % Media->BlockSize == 0) ? Subtask->Length : Media->BlockSize,
+                             (Subtask->WorkingBuffer != NULL) ? Subtask->WorkingBuffer : Subtask->Buffer
+                             );
+      }
+    }
+
+    if (SubtaskBlocking || EFI_ERROR (Status)) {
+      //
+      // Make sure the subtask list only contains non-blocking subtasks.
+      // Remove failed non-blocking subtasks as well because the callback won't be called.
+      //
+      DiskIoDestroySubtask (Instance, Subtask);
+    }
+
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+  
+  gBS->RaiseTPL (TPL_NOTIFY);
+
+  //
+  // Remove all the remaining subtasks when failure.
+  // We shouldn't remove all the tasks because the non-blocking requests have been submitted and cannot be canceled.
+  //
+  if (EFI_ERROR (Status)) {
+    while (!IsNull (SubtasksPtr, NextLink)) {
+      Subtask = CR (NextLink, DISK_IO_SUBTASK, Link, DISK_IO_SUBTASK_SIGNATURE);
+      NextLink = DiskIoDestroySubtask (Instance, Subtask);
+    }
+  }
+
+  //
+  // It's possible that the non-blocking subtasks finish before raising TPL to NOTIFY,
+  // so the subtasks list might be empty at this point.
+  //
+  if (!Blocking && IsListEmpty (SubtasksPtr)) {
+    EfiAcquireLock (&Instance->TaskQueueLock);
+    RemoveEntryList (&Task->Link);
+    EfiReleaseLock (&Instance->TaskQueueLock);
+
+    if (!EFI_ERROR (Status) && (Task->Token != NULL)) {
+      //
+      // Task->Token should be set to NULL by the DiskIo2OnReadWriteComplete
+      // It it's not, that means the non-blocking request was downgraded to blocking request.
+      //
+      DEBUG ((EFI_D_VERBOSE, "DiskIo: Non-blocking request was downgraded to blocking request, signal event directly.\n"));
+      Task->Token->TransactionStatus = Status;
+      gBS->SignalEvent (Task->Token->Event);
+    }
+
+    FreePool (Task);
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Reads a specified number of bytes from a device.
+
+  @param This                   Indicates a pointer to the calling context.
+  @param MediaId                ID of the medium to be read.
+  @param Offset                 The starting byte offset on the logical block I/O device to read from.
+  @param Token                  A pointer to the token associated with the transaction.
+                                If this field is NULL, synchronous/blocking IO is performed.
+  @param  BufferSize            The size in bytes of Buffer. The number of bytes to read from the device.
+  @param  Buffer                A pointer to the destination buffer for the data.
+                                The caller is responsible either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was read correctly from the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId is not for the current medium.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that are not valid for the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2ReadDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       Offset,
+  IN OUT EFI_DISK_IO2_TOKEN       *Token,
+  IN UINTN                        BufferSize,
+  OUT VOID                        *Buffer
+  )
+{
+  return DiskIo2ReadWriteDisk (
+           DISK_IO_PRIVATE_DATA_FROM_DISK_IO2 (This),
+           FALSE, MediaId, Offset, Token, BufferSize, (UINT8 *) Buffer
+           );
+}
+
+/**
+  Writes a specified number of bytes to a device.
+
+  @param This        Indicates a pointer to the calling context.
+  @param MediaId     ID of the medium to be written.
+  @param Offset      The starting byte offset on the logical block I/O device to write to.
+  @param Token       A pointer to the token associated with the transaction.
+                     If this field is NULL, synchronous/blocking IO is performed.
+  @param BufferSize  The size in bytes of Buffer. The number of bytes to write to the device.
+  @param Buffer      A pointer to the buffer containing the data to be written.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was written correctly to the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write operation.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId is not for the current medium.
+  @retval EFI_INVALID_PARAMETER The write request contains device addresses that are not valid for the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2WriteDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       Offset,
+  IN OUT EFI_DISK_IO2_TOKEN       *Token,
+  IN UINTN                        BufferSize,
+  IN VOID                         *Buffer
+  )
+{
+  return DiskIo2ReadWriteDisk (
+           DISK_IO_PRIVATE_DATA_FROM_DISK_IO2 (This),
+           TRUE, MediaId, Offset, Token, BufferSize, (UINT8 *) Buffer
+           );
+}
+
+/**
+  The callback for the BlockIo2 FlushBlocksEx.
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               The pointer to the notification function's context,
+                                which points to the DISK_IO2_FLUSH_TASK instance.
+**/
+VOID
+EFIAPI
+DiskIo2OnFlushComplete (
+  IN EFI_EVENT                 Event,
+  IN VOID                      *Context
+  )
+{
+  DISK_IO2_FLUSH_TASK             *Task;
+
+  gBS->CloseEvent (Event);
+
+  Task = (DISK_IO2_FLUSH_TASK *) Context;
+  ASSERT (Task->Signature == DISK_IO2_FLUSH_TASK_SIGNATURE);
+  Task->Token->TransactionStatus = Task->BlockIo2Token.TransactionStatus;
+  gBS->SignalEvent (Task->Token->Event);
+
+  FreePool (Task);
+}
+
+/**
+  Flushes all modified data to the physical device.
+
+  @param This        Indicates a pointer to the calling context.
+  @param Token       A pointer to the token associated with the transaction.
+                     If this field is NULL, synchronous/blocking IO is performed.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was flushed successfully to the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write operation.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2FlushDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN OUT EFI_DISK_IO2_TOKEN       *Token
+  )
+{
+  EFI_STATUS                      Status;
+  DISK_IO2_FLUSH_TASK             *Task;
+  DISK_IO_PRIVATE_DATA            *Private;
+
+  Private = DISK_IO_PRIVATE_DATA_FROM_DISK_IO2 (This);
+
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Task = AllocatePool (sizeof (DISK_IO2_FLUSH_TASK));
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    DiskIo2OnFlushComplete,
+                    Task,
+                    &Task->BlockIo2Token.Event
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (Task);
+      return Status;
+    }
+    Task->Signature = DISK_IO2_FLUSH_TASK_SIGNATURE;
+    Task->Token     = Token;
+    Status = Private->BlockIo2->FlushBlocksEx (Private->BlockIo2, &Task->BlockIo2Token);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseEvent (Task->BlockIo2Token.Event);
+      FreePool (Task);
+    }
+  } else {
+    Status = Private->BlockIo2->FlushBlocksEx (Private->BlockIo2, NULL);
+  }
+
+  return Status;
+}
+
+/**
+  Read BufferSize bytes from Offset into Buffer.
+  Reads may support reads that are not aligned on
+  sector boundaries. There are three cases:
+    UnderRun - The first byte is not on a sector boundary or the read request is
+               less than a sector in length.
+    Aligned  - A read of N contiguous sectors.
+    OverRun  - The last byte is not on a sector boundary.
+
+  @param  This                  Protocol instance pointer.
+  @param  MediaId               Id of the media, changes every time the media is replaced.
+  @param  Offset                The starting byte offset to read from
+  @param  BufferSize            Size of Buffer
+  @param  Buffer                Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that are not
+                                valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoReadDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  OUT VOID                 *Buffer
+  )
+{
+  return DiskIo2ReadWriteDisk (
+           DISK_IO_PRIVATE_DATA_FROM_DISK_IO (This),
+           FALSE, MediaId, Offset, NULL, BufferSize, (UINT8 *) Buffer
+           );
+}
+
+
+/**
+  Writes BufferSize bytes from Buffer into Offset.
+  Writes may require a read modify write to support writes that are not
+  aligned on sector boundaries. There are three cases:
+    UnderRun - The first byte is not on a sector boundary or the write request
+               is less than a sector in length. Read modify write is required.
+    Aligned  - A write of N contiguous sectors.
+    OverRun  - The last byte is not on a sector boundary. Read modified write
+               required.
+
+  @param  This       Protocol instance pointer.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Offset     The starting byte offset to read from
+  @param  BufferSize Size of Buffer
+  @param  Buffer     Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The write request contains device addresses that are not
+                                 valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoWriteDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  IN VOID                  *Buffer
+  )
+{
+  return DiskIo2ReadWriteDisk (
+           DISK_IO_PRIVATE_DATA_FROM_DISK_IO (This),
+           TRUE, MediaId, Offset, NULL, BufferSize, (UINT8 *) Buffer
+           );
+}
+
+/**
+  The user Entry Point for module DiskIo. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDiskIo (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gDiskIoDriverBinding,
+             ImageHandle,
+             &gDiskIoComponentName,
+             &gDiskIoComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.h b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.h
new file mode 100644
index 0000000000..7591e469a1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIo.h
@@ -0,0 +1,473 @@
+/** @file
+  Master header file for DiskIo driver. It includes the module private defininitions.
+
+Copyright (c) 2006 - 2013, 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.
+
+**/
+
+#ifndef _DISK_IO_H_
+#define _DISK_IO_H_
+
+#include <Uefi.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Protocol/DiskIo2.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/DiskIo.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#define DISK_IO_PRIVATE_DATA_SIGNATURE  SIGNATURE_32 ('d', 's', 'k', 'I')
+typedef struct {
+  UINT32                          Signature;
+
+  EFI_DISK_IO_PROTOCOL            DiskIo;
+  EFI_DISK_IO2_PROTOCOL           DiskIo2;
+  EFI_BLOCK_IO_PROTOCOL           *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL          *BlockIo2;
+
+  UINT8                           *SharedWorkingBuffer;
+
+  EFI_LOCK                        TaskQueueLock;
+  LIST_ENTRY                      TaskQueue;
+} DISK_IO_PRIVATE_DATA;
+#define DISK_IO_PRIVATE_DATA_FROM_DISK_IO(a)  CR (a, DISK_IO_PRIVATE_DATA, DiskIo,  DISK_IO_PRIVATE_DATA_SIGNATURE)
+#define DISK_IO_PRIVATE_DATA_FROM_DISK_IO2(a) CR (a, DISK_IO_PRIVATE_DATA, DiskIo2, DISK_IO_PRIVATE_DATA_SIGNATURE)
+
+#define DISK_IO2_TASK_SIGNATURE   SIGNATURE_32 ('d', 'i', 'a', 't')
+typedef struct {
+  UINT32                          Signature;
+  LIST_ENTRY                      Link;     /// < link to other task
+  EFI_LOCK                        SubtasksLock;
+  LIST_ENTRY                      Subtasks; /// < header of subtasks
+  EFI_DISK_IO2_TOKEN              *Token;
+  DISK_IO_PRIVATE_DATA            *Instance;
+} DISK_IO2_TASK;
+
+#define DISK_IO2_FLUSH_TASK_SIGNATURE SIGNATURE_32 ('d', 'i', 'f', 't')
+typedef struct {
+  UINT32                          Signature;
+  EFI_BLOCK_IO2_TOKEN             BlockIo2Token;
+  EFI_DISK_IO2_TOKEN              *Token;
+} DISK_IO2_FLUSH_TASK;
+
+#define DISK_IO_SUBTASK_SIGNATURE SIGNATURE_32 ('d', 'i', 's', 't')
+typedef struct {
+  //
+  // UnderRun:  Offset != 0, Length < BlockSize
+  // OverRun:   Offset == 0, Length < BlockSize
+  // Middle:    Offset is block aligned, Length is multiple of block size
+  //
+  UINT32                          Signature;
+  LIST_ENTRY                      Link;
+  BOOLEAN                         Write;
+  UINT64                          Lba;
+  UINT32                          Offset;
+  UINTN                           Length;
+  UINT8                           *WorkingBuffer; /// < NULL indicates using "Buffer" directly
+  UINT8                           *Buffer;
+  BOOLEAN                         Blocking;
+
+  //
+  // Following fields are for DiskIo2
+  //
+  DISK_IO2_TASK                   *Task;
+  EFI_BLOCK_IO2_TOKEN             BlockIo2Token;
+} DISK_IO_SUBTASK;
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gDiskIoDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gDiskIoComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gDiskIoComponentName2;
+
+//
+// Prototypes
+// Driver model protocol interface
+//
+/**
+  Test to see if this driver supports ControllerHandle. 
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle by opening a Block IO protocol and
+  installing a Disk IO protocol on ControllerHandle.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle by removing Disk IO protocol and closing
+  the Block IO protocol on ControllerHandle.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     ControllerHandle,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  );
+
+//
+// Disk I/O Protocol Interface
+//
+/**
+  Read BufferSize bytes from Offset into Buffer.
+  Reads may support reads that are not aligned on
+  sector boundaries. There are three cases:
+    UnderRun - The first byte is not on a sector boundary or the read request is
+               less than a sector in length.
+    Aligned  - A read of N contiguous sectors.
+    OverRun  - The last byte is not on a sector boundary.
+
+  @param  This                  Protocol instance pointer.
+  @param  MediaId               Id of the media, changes every time the media is replaced.
+  @param  Offset                The starting byte offset to read from
+  @param  BufferSize            Size of Buffer
+  @param  Buffer                Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that are not
+                                valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoReadDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  OUT VOID                 *Buffer
+  );
+
+/**
+  Writes BufferSize bytes from Buffer into Offset.
+  Writes may require a read modify write to support writes that are not
+  aligned on sector boundaries. There are three cases:
+    UnderRun - The first byte is not on a sector boundary or the write request
+               is less than a sector in length. Read modify write is required.
+    Aligned  - A write of N contiguous sectors.
+    OverRun  - The last byte is not on a sector boundary. Read modified write
+               required.
+
+  @param  This       Protocol instance pointer.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Offset     The starting byte offset to read from
+  @param  BufferSize Size of Buffer
+  @param  Buffer     Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The write request contains device addresses that are not
+                                 valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoWriteDisk (
+  IN EFI_DISK_IO_PROTOCOL  *This,
+  IN UINT32                MediaId,
+  IN UINT64                Offset,
+  IN UINTN                 BufferSize,
+  IN VOID                  *Buffer
+  );
+
+
+/**
+  Terminate outstanding asynchronous requests to a device.
+
+  @param This                   Indicates a pointer to the calling context.
+
+  @retval EFI_SUCCESS           All outstanding requests were successfully terminated.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the cancel
+                                operation.
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2Cancel (
+  IN EFI_DISK_IO2_PROTOCOL *This
+  );
+
+/**
+  Reads a specified number of bytes from a device.
+
+  @param This                   Indicates a pointer to the calling context.
+  @param MediaId                ID of the medium to be read.
+  @param Offset                 The starting byte offset on the logical block I/O device to read from.
+  @param Token                  A pointer to the token associated with the transaction.
+                                If this field is NULL, synchronous/blocking IO is performed.
+  @param  BufferSize            The size in bytes of Buffer. The number of bytes to read from the device.
+  @param  Buffer                A pointer to the destination buffer for the data.
+                                The caller is responsible either having implicit or explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was read correctly from the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId is not for the current medium.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that are not valid for the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2ReadDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       Offset,
+  IN OUT EFI_DISK_IO2_TOKEN       *Token,
+  IN UINTN                        BufferSize,
+  OUT VOID                        *Buffer
+  );
+
+/**
+  Writes a specified number of bytes to a device.
+
+  @param This        Indicates a pointer to the calling context.
+  @param MediaId     ID of the medium to be written.
+  @param Offset      The starting byte offset on the logical block I/O device to write to.
+  @param Token       A pointer to the token associated with the transaction.
+                     If this field is NULL, synchronous/blocking IO is performed.
+  @param BufferSize  The size in bytes of Buffer. The number of bytes to write to the device.
+  @param Buffer      A pointer to the buffer containing the data to be written.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was written correctly to the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write operation.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId is not for the current medium.
+  @retval EFI_INVALID_PARAMETER The write request contains device addresses that are not valid for the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2WriteDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       Offset,
+  IN EFI_DISK_IO2_TOKEN           *Token,
+  IN UINTN                        BufferSize,
+  IN VOID                         *Buffer
+  );
+
+/**
+  Flushes all modified data to the physical device.
+
+  @param This        Indicates a pointer to the calling context.
+  @param Token       A pointer to the token associated with the transaction.
+                     If this field is NULL, synchronous/blocking IO is performed.
+
+  @retval EFI_SUCCESS           If Event is NULL (blocking I/O): The data was flushed successfully to the device.
+                                If Event is not NULL (asynchronous I/O): The request was successfully queued for processing.
+                                                                         Event will be signaled upon completion.
+  @retval EFI_WRITE_PROTECTED   The device cannot be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write operation.
+  @retval EFI_NO_MEDIA          There is no medium in the device.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+**/
+EFI_STATUS
+EFIAPI
+DiskIo2FlushDiskEx (
+  IN EFI_DISK_IO2_PROTOCOL        *This,
+  IN OUT EFI_DISK_IO2_TOKEN       *Token
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DiskIoComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.inf b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.inf
new file mode 100644
index 0000000000..cee27d5b11
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.inf
@@ -0,0 +1,71 @@
+## @file
+#  Module that lays Disk I/O protocol on every Block I/O protocol.
+#
+#  This module produces Disk I/O protocol to abstract the block accesses
+#  of the Block I/O protocol to a more general offset-length protocol
+#  to provide byte-oriented access to block media. It adds this protocol
+#  to any Block I/O interface that appears in the system that does not
+#  already have a Disk I/O protocol. File systems and other disk access
+#  code utilize the Disk I/O protocol.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DiskIoDxe
+  MODULE_UNI_FILE                = DiskIoDxe.uni
+  FILE_GUID                      = 6B38F7B4-AD98-40e9-9093-ACA2B5A253C4
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeDiskIo
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gDiskIoDriverBinding
+#  COMPONENT_NAME                =  gDiskIoComponentName
+#  COMPONENT_NAME2               =  gDiskIoComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  DiskIo.h
+  DiskIo.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  PcdLib
+
+[Protocols]
+  gEfiDiskIoProtocolGuid                        ## BY_START
+  gEfiDiskIo2ProtocolGuid                       ## BY_START
+  gEfiBlockIoProtocolGuid                       ## TO_START
+  gEfiBlockIo2ProtocolGuid                      ## TO_START
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDiskIoDataBufferBlockNum    ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DiskIoDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.uni b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.uni
new file mode 100644
index 0000000000..9fd078ac70
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxeExtra.uni b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxeExtra.uni
new file mode 100644
index 0000000000..7a2a1b84e3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/DiskIoDxe/DiskIoDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ComponentName.c
new file mode 100644
index 0000000000..c917cd79e6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ComponentName.c
@@ -0,0 +1,188 @@
+/** @file
+  UEFI Component Name protocol for Partition driver.
+
+Copyright (c) 2006 - 2011, 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 "Partition.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gPartitionComponentName = {
+  PartitionComponentNameGetDriverName,
+  PartitionComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gPartitionComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) PartitionComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) PartitionComponentNameGetControllerName,
+  "en"
+};
+
+//
+// Driver name table for Partition module.
+// It is shared by the implementation of ComponentName & ComponentName2 Protocol.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mPartitionDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Partition Driver(MBR/GPT/El Torito)"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mPartitionDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gPartitionComponentName)
+           );
+}
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ElTorito.c b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ElTorito.c
new file mode 100644
index 0000000000..22384499c5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/ElTorito.c
@@ -0,0 +1,274 @@
+/** @file
+  Decode an El Torito formatted CD-ROM
+
+Copyright (c) 2006 - 2015, 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 "Partition.h"
+
+
+/**
+  Install child handles if the Handle supports El Torito format.
+
+  @param[in]  This        Calling context.
+  @param[in]  Handle      Parent Handle.
+  @param[in]  DiskIo      Parent DiskIo interface.
+  @param[in]  DiskIo2     Parent DiskIo2 interface.
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  BlockIo2    Parent BlockIo2 interface.
+  @param[in]  DevicePath  Parent Device Path
+
+
+  @retval EFI_SUCCESS         Child handle(s) was added.
+  @retval EFI_MEDIA_CHANGED   Media changed Detected.
+  @retval other               no child handle was added.
+
+**/
+EFI_STATUS
+PartitionInstallElToritoChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_STATUS              Status;
+  UINT64                  VolDescriptorOffset;
+  UINT32                  Lba2KB;
+  EFI_BLOCK_IO_MEDIA      *Media;
+  CDROM_VOLUME_DESCRIPTOR *VolDescriptor;
+  ELTORITO_CATALOG        *Catalog;
+  UINTN                   Check;
+  UINTN                   Index;
+  UINTN                   BootEntry;
+  UINTN                   MaxIndex;
+  UINT16                  *CheckBuffer;
+  CDROM_DEVICE_PATH       CdDev;
+  UINT32                  SubBlockSize;
+  UINT32                  SectorCount;
+  EFI_STATUS              Found;
+  UINT32                  VolSpaceSize;
+
+  Found         = EFI_NOT_FOUND;
+  Media         = BlockIo->Media;
+
+  VolSpaceSize  = 0;
+
+  //
+  // CD_ROM has the fixed block size as 2048 bytes (SIZE_2KB)
+  //
+
+  // If the ISO image has been copied onto a different storage media
+  // then the block size might be different (eg: USB).
+  // Ensure 2048 (SIZE_2KB) is a multiple of block size
+  if (((SIZE_2KB % Media->BlockSize) != 0) || (Media->BlockSize > SIZE_2KB)) {
+    return EFI_NOT_FOUND;
+  }
+
+  VolDescriptor = AllocatePool ((UINTN)SIZE_2KB);
+
+  if (VolDescriptor == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Catalog = (ELTORITO_CATALOG *) VolDescriptor;
+
+  //
+  // Loop: handle one volume descriptor per time
+  //       The ISO-9660 volume descriptor starts at 32k on the media
+  //
+  for (VolDescriptorOffset = SIZE_32KB;
+       VolDescriptorOffset <= MultU64x32 (Media->LastBlock, Media->BlockSize);
+       VolDescriptorOffset += SIZE_2KB) {
+    Status = DiskIo->ReadDisk (
+                       DiskIo,
+                       Media->MediaId,
+                       VolDescriptorOffset,
+                       SIZE_2KB,
+                       VolDescriptor
+                       );
+    if (EFI_ERROR (Status)) {
+      Found = Status;
+      break;
+    }
+    //
+    // Check for valid volume descriptor signature
+    //
+    if (VolDescriptor->Unknown.Type == CDVOL_TYPE_END ||
+        CompareMem (VolDescriptor->Unknown.Id, CDVOL_ID, sizeof (VolDescriptor->Unknown.Id)) != 0
+        ) {
+      //
+      // end of Volume descriptor list
+      //
+      break;
+    }
+    //
+    // Read the Volume Space Size from Primary Volume Descriptor 81-88 byte,
+    // the 32-bit numerical values is stored in Both-byte orders
+    //
+    if (VolDescriptor->PrimaryVolume.Type == CDVOL_TYPE_CODED) {
+      VolSpaceSize = VolDescriptor->PrimaryVolume.VolSpaceSize[0];
+    }
+    //
+    // Is it an El Torito volume descriptor?
+    //
+    if (CompareMem (VolDescriptor->BootRecordVolume.SystemId, CDVOL_ELTORITO_ID, sizeof (CDVOL_ELTORITO_ID) - 1) != 0) {
+      continue;
+    }
+    //
+    // Read in the boot El Torito boot catalog
+    // The LBA unit used by El Torito boot catalog is 2KB unit
+    //
+    Lba2KB = UNPACK_INT32 (VolDescriptor->BootRecordVolume.EltCatalog);
+    // Ensure the LBA (in 2KB unit) fits into our media
+    if (Lba2KB * (SIZE_2KB / Media->BlockSize) > Media->LastBlock) {
+      continue;
+    }
+
+    Status = DiskIo->ReadDisk (
+                       DiskIo,
+                       Media->MediaId,
+                       MultU64x32 (Lba2KB, SIZE_2KB),
+                       SIZE_2KB,
+                       Catalog
+                       );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "EltCheckDevice: error reading catalog %r\n", Status));
+      continue;
+    }
+    //
+    // We don't care too much about the Catalog header's contents, but we do want
+    // to make sure it looks like a Catalog header
+    //
+    if (Catalog->Catalog.Indicator != ELTORITO_ID_CATALOG || Catalog->Catalog.Id55AA != 0xAA55) {
+      DEBUG ((EFI_D_ERROR, "EltCheckBootCatalog: El Torito boot catalog header IDs not correct\n"));
+      continue;
+    }
+
+    Check       = 0;
+    CheckBuffer = (UINT16 *) Catalog;
+    for (Index = 0; Index < sizeof (ELTORITO_CATALOG) / sizeof (UINT16); Index += 1) {
+      Check += CheckBuffer[Index];
+    }
+
+    if ((Check & 0xFFFF) != 0) {
+      DEBUG ((EFI_D_ERROR, "EltCheckBootCatalog: El Torito boot catalog header checksum failed\n"));
+      continue;
+    }
+
+    MaxIndex = Media->BlockSize / sizeof (ELTORITO_CATALOG);
+    for (Index = 1, BootEntry = 1; Index < MaxIndex; Index += 1) {
+      //
+      // Next entry
+      //
+      Catalog += 1;
+
+      //
+      // Check this entry
+      //
+      if (Catalog->Boot.Indicator != ELTORITO_ID_SECTION_BOOTABLE || Catalog->Boot.Lba == 0) {
+        continue;
+      }
+
+      SubBlockSize  = 512;
+      SectorCount   = Catalog->Boot.SectorCount * (SIZE_2KB / Media->BlockSize);
+
+      switch (Catalog->Boot.MediaType) {
+
+      case ELTORITO_NO_EMULATION:
+        SubBlockSize = Media->BlockSize;
+        break;
+
+      case ELTORITO_HARD_DISK:
+        break;
+
+      case ELTORITO_12_DISKETTE:
+        SectorCount = 0x50 * 0x02 * 0x0F;
+        break;
+
+      case ELTORITO_14_DISKETTE:
+        SectorCount = 0x50 * 0x02 * 0x12;
+        break;
+
+      case ELTORITO_28_DISKETTE:
+        SectorCount = 0x50 * 0x02 * 0x24;
+        break;
+
+      default:
+        DEBUG ((EFI_D_INIT, "EltCheckDevice: unsupported El Torito boot media type %x\n", Catalog->Boot.MediaType));
+        SectorCount   = 0;
+        SubBlockSize  = Media->BlockSize;
+        break;
+      }
+      //
+      // Create child device handle
+      //
+      CdDev.Header.Type     = MEDIA_DEVICE_PATH;
+      CdDev.Header.SubType  = MEDIA_CDROM_DP;
+      SetDevicePathNodeLength (&CdDev.Header, sizeof (CdDev));
+
+      if (Index == 1) {
+        //
+        // This is the initial/default entry
+        //
+        BootEntry = 0;
+      }
+
+      CdDev.BootEntry = (UINT32) BootEntry;
+      BootEntry++;
+      CdDev.PartitionStart = Catalog->Boot.Lba * (SIZE_2KB / Media->BlockSize);
+      if (SectorCount < 2) {
+        //
+        // When the SectorCount < 2, set the Partition as the whole CD.
+        //
+        if (VolSpaceSize > (Media->LastBlock + 1)) {
+          CdDev.PartitionSize = (UINT32)(Media->LastBlock - Catalog->Boot.Lba + 1);
+        } else {
+          CdDev.PartitionSize = (UINT32)(VolSpaceSize - Catalog->Boot.Lba);
+        }
+      } else {
+        CdDev.PartitionSize = DivU64x32 (
+                                MultU64x32 (
+                                  SectorCount,
+                                  SubBlockSize
+                                  ) + Media->BlockSize - 1,
+                                Media->BlockSize
+                                );
+      }
+
+      Status = PartitionInstallChildHandle (
+                This,
+                Handle,
+                DiskIo,
+                DiskIo2,
+                BlockIo,
+                BlockIo2,
+                DevicePath,
+                (EFI_DEVICE_PATH_PROTOCOL *) &CdDev,
+                Catalog->Boot.Lba * (SIZE_2KB / Media->BlockSize),
+                MultU64x32 (Catalog->Boot.Lba + CdDev.PartitionSize - 1, SIZE_2KB / Media->BlockSize),
+                SubBlockSize,
+                FALSE
+                );
+      if (!EFI_ERROR (Status)) {
+        Found = EFI_SUCCESS;
+      }
+    }
+  }
+
+  FreePool (VolDescriptor);
+
+  return Found;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Gpt.c b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Gpt.c
new file mode 100644
index 0000000000..35860515c1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Gpt.c
@@ -0,0 +1,871 @@
+/** @file
+  Decode a hard disk partitioned with the GPT scheme in the UEFI 2.0
+  specification.
+
+  Caution: This file requires additional review when modified.
+  This driver will have external input - disk partition.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  PartitionInstallGptChildHandles() routine will read disk partition content and
+  do basic validation before PartitionInstallChildHandle().
+
+  PartitionValidGptTable(), PartitionCheckGptEntry() routine will accept disk
+  partition content and validate the GPT table and GPT entry.
+
+Copyright (c) 2006 - 2013, 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 "Partition.h"
+
+/**
+  Install child handles if the Handle supports GPT partition structure.
+
+  Caution: This function may receive untrusted input.
+  The GPT partition table header is external input, so this routine
+  will do basic validation for GPT partition table header before return.
+
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  DiskIo      Disk Io protocol.
+  @param[in]  Lba         The starting Lba of the Partition Table
+  @param[out] PartHeader  Stores the partition table that is read
+
+  @retval TRUE      The partition table is valid
+  @retval FALSE     The partition table is not valid
+
+**/
+BOOLEAN
+PartitionValidGptTable (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_LBA                     Lba,
+  OUT EFI_PARTITION_TABLE_HEADER  *PartHeader
+  );
+
+/**
+  Check if the CRC field in the Partition table header is valid
+  for Partition entry array.
+
+  @param[in]  BlockIo     Parent BlockIo interface
+  @param[in]  DiskIo      Disk Io Protocol.
+  @param[in]  PartHeader  Partition table header structure
+
+  @retval TRUE      the CRC is valid
+  @retval FALSE     the CRC is invalid
+
+**/
+BOOLEAN
+PartitionCheckGptEntryArrayCRC (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader
+  );
+
+
+/**
+  Restore Partition Table to its alternate place
+  (Primary -> Backup or Backup -> Primary).
+
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  DiskIo      Disk Io Protocol.
+  @param[in]  PartHeader  Partition table header structure.
+
+  @retval TRUE      Restoring succeeds
+  @retval FALSE     Restoring failed
+
+**/
+BOOLEAN
+PartitionRestoreGptTable (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader
+  );
+
+
+/**
+  This routine will check GPT partition entry and return entry status.
+
+  Caution: This function may receive untrusted input.
+  The GPT partition entry is external input, so this routine
+  will do basic validation for GPT partition entry and report status.
+
+  @param[in]    PartHeader    Partition table header structure
+  @param[in]    PartEntry     The partition entry array
+  @param[out]   PEntryStatus  the partition entry status array 
+                              recording the status of each partition
+
+**/
+VOID
+PartitionCheckGptEntry (
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader,
+  IN  EFI_PARTITION_ENTRY         *PartEntry,
+  OUT EFI_PARTITION_ENTRY_STATUS  *PEntryStatus
+  );
+
+
+/**
+  Checks the CRC32 value in the table header.
+
+  @param  MaxSize   Max Size limit
+  @param  Size      The size of the table
+  @param  Hdr       Table to check
+
+  @return TRUE    CRC Valid
+  @return FALSE   CRC Invalid
+
+**/
+BOOLEAN
+PartitionCheckCrcAltSize (
+  IN UINTN                 MaxSize,
+  IN UINTN                 Size,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  );
+
+
+/**
+  Checks the CRC32 value in the table header.
+
+  @param  MaxSize   Max Size limit
+  @param  Hdr       Table to check
+
+  @return TRUE      CRC Valid
+  @return FALSE     CRC Invalid
+
+**/
+BOOLEAN
+PartitionCheckCrc (
+  IN UINTN                 MaxSize,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  );
+
+
+/**
+  Updates the CRC32 value in the table header.
+
+  @param  Size   The size of the table
+  @param  Hdr    Table to update
+
+**/
+VOID
+PartitionSetCrcAltSize (
+  IN UINTN                 Size,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  );
+
+
+/**
+  Updates the CRC32 value in the table header.
+
+  @param  Hdr    Table to update
+
+**/
+VOID
+PartitionSetCrc (
+  IN OUT EFI_TABLE_HEADER *Hdr
+  );
+
+/**
+  Install child handles if the Handle supports GPT partition structure.
+
+  Caution: This function may receive untrusted input.
+  The GPT partition table is external input, so this routine
+  will do basic validation for GPT partition table before install
+  child handle for each GPT partition.
+
+  @param[in]  This       Calling context.
+  @param[in]  Handle     Parent Handle.
+  @param[in]  DiskIo     Parent DiskIo interface.
+  @param[in]  DiskIo2    Parent DiskIo2 interface.
+  @param[in]  BlockIo    Parent BlockIo interface.
+  @param[in]  BlockIo2   Parent BlockIo2 interface.
+  @param[in]  DevicePath Parent Device Path.
+
+  @retval EFI_SUCCESS           Valid GPT disk.
+  @retval EFI_MEDIA_CHANGED     Media changed Detected.
+  @retval other                 Not a valid GPT disk.
+
+**/
+EFI_STATUS
+PartitionInstallGptChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_STATUS                  Status;
+  UINT32                      BlockSize;
+  EFI_LBA                     LastBlock;
+  MASTER_BOOT_RECORD          *ProtectiveMbr;
+  EFI_PARTITION_TABLE_HEADER  *PrimaryHeader;
+  EFI_PARTITION_TABLE_HEADER  *BackupHeader;
+  EFI_PARTITION_ENTRY         *PartEntry;
+  EFI_PARTITION_ENTRY         *Entry;
+  EFI_PARTITION_ENTRY_STATUS  *PEntryStatus;
+  UINTN                       Index;
+  EFI_STATUS                  GptValidStatus;
+  HARDDRIVE_DEVICE_PATH       HdDev;
+  UINT32                      MediaId;
+
+  ProtectiveMbr = NULL;
+  PrimaryHeader = NULL;
+  BackupHeader  = NULL;
+  PartEntry     = NULL;
+  PEntryStatus  = NULL;
+
+  BlockSize     = BlockIo->Media->BlockSize;
+  LastBlock     = BlockIo->Media->LastBlock;
+  MediaId       = BlockIo->Media->MediaId;
+
+  DEBUG ((EFI_D_INFO, " BlockSize : %d \n", BlockSize));
+  DEBUG ((EFI_D_INFO, " LastBlock : %lx \n", LastBlock));
+
+  GptValidStatus = EFI_NOT_FOUND;
+
+  //
+  // Allocate a buffer for the Protective MBR
+  //
+  ProtectiveMbr = AllocatePool (BlockSize);
+  if (ProtectiveMbr == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Read the Protective MBR from LBA #0
+  //
+  Status = DiskIo->ReadDisk (
+                     DiskIo,
+                     MediaId,
+                     0,
+                     BlockSize,
+                     ProtectiveMbr
+                     );
+  if (EFI_ERROR (Status)) {
+    GptValidStatus = Status;
+    goto Done;
+  }
+
+  //
+  // Verify that the Protective MBR is valid
+  //
+  for (Index = 0; Index < MAX_MBR_PARTITIONS; Index++) {
+    if (ProtectiveMbr->Partition[Index].BootIndicator == 0x00 &&
+        ProtectiveMbr->Partition[Index].OSIndicator == PMBR_GPT_PARTITION &&
+        UNPACK_UINT32 (ProtectiveMbr->Partition[Index].StartingLBA) == 1
+        ) {
+      break;
+    }
+  }
+  if (Index == MAX_MBR_PARTITIONS) {
+    goto Done;
+  }
+
+  //
+  // Allocate the GPT structures
+  //
+  PrimaryHeader = AllocateZeroPool (sizeof (EFI_PARTITION_TABLE_HEADER));
+  if (PrimaryHeader == NULL) {
+    goto Done;
+  }
+
+  BackupHeader = AllocateZeroPool (sizeof (EFI_PARTITION_TABLE_HEADER));
+  if (BackupHeader == NULL) {
+    goto Done;
+  }
+
+  //
+  // Check primary and backup partition tables
+  //
+  if (!PartitionValidGptTable (BlockIo, DiskIo, PRIMARY_PART_HEADER_LBA, PrimaryHeader)) {
+    DEBUG ((EFI_D_INFO, " Not Valid primary partition table\n"));
+
+    if (!PartitionValidGptTable (BlockIo, DiskIo, LastBlock, BackupHeader)) {
+      DEBUG ((EFI_D_INFO, " Not Valid backup partition table\n"));
+      goto Done;
+    } else {
+      DEBUG ((EFI_D_INFO, " Valid backup partition table\n"));
+      DEBUG ((EFI_D_INFO, " Restore primary partition table by the backup\n"));
+      if (!PartitionRestoreGptTable (BlockIo, DiskIo, BackupHeader)) {
+        DEBUG ((EFI_D_INFO, " Restore primary partition table error\n"));
+      }
+
+      if (PartitionValidGptTable (BlockIo, DiskIo, BackupHeader->AlternateLBA, PrimaryHeader)) {
+        DEBUG ((EFI_D_INFO, " Restore backup partition table success\n"));
+      }
+    }
+  } else if (!PartitionValidGptTable (BlockIo, DiskIo, PrimaryHeader->AlternateLBA, BackupHeader)) {
+    DEBUG ((EFI_D_INFO, " Valid primary and !Valid backup partition table\n"));
+    DEBUG ((EFI_D_INFO, " Restore backup partition table by the primary\n"));
+    if (!PartitionRestoreGptTable (BlockIo, DiskIo, PrimaryHeader)) {
+      DEBUG ((EFI_D_INFO, " Restore backup partition table error\n"));
+    }
+
+    if (PartitionValidGptTable (BlockIo, DiskIo, PrimaryHeader->AlternateLBA, BackupHeader)) {
+      DEBUG ((EFI_D_INFO, " Restore backup partition table success\n"));
+    }
+
+  }
+
+  DEBUG ((EFI_D_INFO, " Valid primary and Valid backup partition table\n"));
+
+  //
+  // Read the EFI Partition Entries
+  //
+  PartEntry = AllocatePool (PrimaryHeader->NumberOfPartitionEntries * PrimaryHeader->SizeOfPartitionEntry);
+  if (PartEntry == NULL) {
+    DEBUG ((EFI_D_ERROR, "Allocate pool error\n"));
+    goto Done;
+  }
+
+  Status = DiskIo->ReadDisk (
+                     DiskIo,
+                     MediaId,
+                     MultU64x32(PrimaryHeader->PartitionEntryLBA, BlockSize),
+                     PrimaryHeader->NumberOfPartitionEntries * (PrimaryHeader->SizeOfPartitionEntry),
+                     PartEntry
+                     );
+  if (EFI_ERROR (Status)) {
+    GptValidStatus = Status;
+    DEBUG ((EFI_D_ERROR, " Partition Entry ReadDisk error\n"));
+    goto Done;
+  }
+
+  DEBUG ((EFI_D_INFO, " Partition entries read block success\n"));
+
+  DEBUG ((EFI_D_INFO, " Number of partition entries: %d\n", PrimaryHeader->NumberOfPartitionEntries));
+
+  PEntryStatus = AllocateZeroPool (PrimaryHeader->NumberOfPartitionEntries * sizeof (EFI_PARTITION_ENTRY_STATUS));
+  if (PEntryStatus == NULL) {
+    DEBUG ((EFI_D_ERROR, "Allocate pool error\n"));
+    goto Done;
+  }
+
+  //
+  // Check the integrity of partition entries
+  //
+  PartitionCheckGptEntry (PrimaryHeader, PartEntry, PEntryStatus);
+
+  //
+  // If we got this far the GPT layout of the disk is valid and we should return true
+  //
+  GptValidStatus = EFI_SUCCESS;
+
+  //
+  // Create child device handles
+  //
+  for (Index = 0; Index < PrimaryHeader->NumberOfPartitionEntries; Index++) {
+    Entry = (EFI_PARTITION_ENTRY *) ((UINT8 *) PartEntry + Index * PrimaryHeader->SizeOfPartitionEntry);
+    if (CompareGuid (&Entry->PartitionTypeGUID, &gEfiPartTypeUnusedGuid) ||
+        PEntryStatus[Index].OutOfRange ||
+        PEntryStatus[Index].Overlap ||
+        PEntryStatus[Index].OsSpecific
+        ) {
+      //
+      // Don't use null EFI Partition Entries, Invalid Partition Entries or OS specific
+      // partition Entries
+      //
+      continue;
+    }
+
+    ZeroMem (&HdDev, sizeof (HdDev));
+    HdDev.Header.Type     = MEDIA_DEVICE_PATH;
+    HdDev.Header.SubType  = MEDIA_HARDDRIVE_DP;
+    SetDevicePathNodeLength (&HdDev.Header, sizeof (HdDev));
+
+    HdDev.PartitionNumber = (UINT32) Index + 1;
+    HdDev.MBRType         = MBR_TYPE_EFI_PARTITION_TABLE_HEADER;
+    HdDev.SignatureType   = SIGNATURE_TYPE_GUID;
+    HdDev.PartitionStart  = Entry->StartingLBA;
+    HdDev.PartitionSize   = Entry->EndingLBA - Entry->StartingLBA + 1;
+    CopyMem (HdDev.Signature, &Entry->UniquePartitionGUID, sizeof (EFI_GUID));
+
+    DEBUG ((EFI_D_INFO, " Index : %d\n", (UINT32) Index));
+    DEBUG ((EFI_D_INFO, " Start LBA : %lx\n", (UINT64) HdDev.PartitionStart));
+    DEBUG ((EFI_D_INFO, " End LBA : %lx\n", (UINT64) Entry->EndingLBA));
+    DEBUG ((EFI_D_INFO, " Partition size: %lx\n", (UINT64) HdDev.PartitionSize));
+    DEBUG ((EFI_D_INFO, " Start : %lx", MultU64x32 (Entry->StartingLBA, BlockSize)));
+    DEBUG ((EFI_D_INFO, " End : %lx\n", MultU64x32 (Entry->EndingLBA, BlockSize)));
+
+    Status = PartitionInstallChildHandle (
+               This,
+               Handle,
+               DiskIo,
+               DiskIo2,
+               BlockIo,
+               BlockIo2,
+               DevicePath,
+               (EFI_DEVICE_PATH_PROTOCOL *) &HdDev,
+               Entry->StartingLBA,
+               Entry->EndingLBA,
+               BlockSize,
+               CompareGuid(&Entry->PartitionTypeGUID, &gEfiPartTypeSystemPartGuid)
+               );
+  }
+
+  DEBUG ((EFI_D_INFO, "Prepare to Free Pool\n"));
+
+Done:
+  if (ProtectiveMbr != NULL) {
+    FreePool (ProtectiveMbr);
+  }
+  if (PrimaryHeader != NULL) {
+    FreePool (PrimaryHeader);
+  }
+  if (BackupHeader != NULL) {
+    FreePool (BackupHeader);
+  }
+  if (PartEntry != NULL) {
+    FreePool (PartEntry);
+  }
+  if (PEntryStatus != NULL) {
+    FreePool (PEntryStatus);
+  }
+
+  return GptValidStatus;
+}
+
+/**
+  This routine will read GPT partition table header and return it.
+
+  Caution: This function may receive untrusted input.
+  The GPT partition table header is external input, so this routine
+  will do basic validation for GPT partition table header before return.
+
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  DiskIo      Disk Io protocol.
+  @param[in]  Lba         The starting Lba of the Partition Table
+  @param[out] PartHeader  Stores the partition table that is read
+
+  @retval TRUE      The partition table is valid
+  @retval FALSE     The partition table is not valid
+
+**/
+BOOLEAN
+PartitionValidGptTable (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_LBA                     Lba,
+  OUT EFI_PARTITION_TABLE_HEADER  *PartHeader
+  )
+{
+  EFI_STATUS                  Status;
+  UINT32                      BlockSize;
+  EFI_PARTITION_TABLE_HEADER  *PartHdr;
+  UINT32                      MediaId;
+
+  BlockSize = BlockIo->Media->BlockSize;
+  MediaId   = BlockIo->Media->MediaId;
+  PartHdr   = AllocateZeroPool (BlockSize);
+
+  if (PartHdr == NULL) {
+    DEBUG ((EFI_D_ERROR, "Allocate pool error\n"));
+    return FALSE;
+  }
+  //
+  // Read the EFI Partition Table Header
+  //
+  Status = DiskIo->ReadDisk (
+                     DiskIo,
+                     MediaId,
+                     MultU64x32 (Lba, BlockSize),
+                     BlockSize,
+                     PartHdr
+                     );
+  if (EFI_ERROR (Status)) {
+    FreePool (PartHdr);
+    return FALSE;
+  }
+
+  if ((PartHdr->Header.Signature != EFI_PTAB_HEADER_ID) ||
+      !PartitionCheckCrc (BlockSize, &PartHdr->Header) ||
+      PartHdr->MyLBA != Lba ||
+      (PartHdr->SizeOfPartitionEntry < sizeof (EFI_PARTITION_ENTRY))
+      ) {
+    DEBUG ((EFI_D_INFO, "Invalid efi partition table header\n"));
+    FreePool (PartHdr);
+    return FALSE;
+  }
+
+  //
+  // Ensure the NumberOfPartitionEntries * SizeOfPartitionEntry doesn't overflow.
+  //
+  if (PartHdr->NumberOfPartitionEntries > DivU64x32 (MAX_UINTN, PartHdr->SizeOfPartitionEntry)) {
+    FreePool (PartHdr);
+    return FALSE;
+  }
+
+  CopyMem (PartHeader, PartHdr, sizeof (EFI_PARTITION_TABLE_HEADER));
+  if (!PartitionCheckGptEntryArrayCRC (BlockIo, DiskIo, PartHeader)) {
+    FreePool (PartHdr);
+    return FALSE;
+  }
+
+  DEBUG ((EFI_D_INFO, " Valid efi partition table header\n"));
+  FreePool (PartHdr);
+  return TRUE;
+}
+
+/**
+  Check if the CRC field in the Partition table header is valid
+  for Partition entry array.
+
+  @param[in]  BlockIo     Parent BlockIo interface
+  @param[in]  DiskIo      Disk Io Protocol.
+  @param[in]  PartHeader  Partition table header structure
+
+  @retval TRUE      the CRC is valid
+  @retval FALSE     the CRC is invalid
+
+**/
+BOOLEAN
+PartitionCheckGptEntryArrayCRC (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       *Ptr;
+  UINT32      Crc;
+  UINTN       Size;
+
+  //
+  // Read the EFI Partition Entries
+  //
+  Ptr = AllocatePool (PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry);
+  if (Ptr == NULL) {
+    DEBUG ((EFI_D_ERROR, " Allocate pool error\n"));
+    return FALSE;
+  }
+
+  Status = DiskIo->ReadDisk (
+                    DiskIo,
+                    BlockIo->Media->MediaId,
+                    MultU64x32(PartHeader->PartitionEntryLBA, BlockIo->Media->BlockSize),
+                    PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry,
+                    Ptr
+                    );
+  if (EFI_ERROR (Status)) {
+    FreePool (Ptr);
+    return FALSE;
+  }
+
+  Size    = PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry;
+
+  Status  = gBS->CalculateCrc32 (Ptr, Size, &Crc);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "CheckPEntryArrayCRC: Crc calculation failed\n"));
+    FreePool (Ptr);
+    return FALSE;
+  }
+
+  FreePool (Ptr);
+
+  return (BOOLEAN) (PartHeader->PartitionEntryArrayCRC32 == Crc);
+}
+
+
+/**
+  Restore Partition Table to its alternate place
+  (Primary -> Backup or Backup -> Primary).
+
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  DiskIo      Disk Io Protocol.
+  @param[in]  PartHeader  Partition table header structure.
+
+  @retval TRUE      Restoring succeeds
+  @retval FALSE     Restoring failed
+
+**/
+BOOLEAN
+PartitionRestoreGptTable (
+  IN  EFI_BLOCK_IO_PROTOCOL       *BlockIo,
+  IN  EFI_DISK_IO_PROTOCOL        *DiskIo,
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader
+  )
+{
+  EFI_STATUS                  Status;
+  UINTN                       BlockSize;
+  EFI_PARTITION_TABLE_HEADER  *PartHdr;
+  EFI_LBA                     PEntryLBA;
+  UINT8                       *Ptr;
+  UINT32                      MediaId;
+
+  PartHdr   = NULL;
+  Ptr       = NULL;
+
+  BlockSize = BlockIo->Media->BlockSize;
+  MediaId   = BlockIo->Media->MediaId;
+
+  PartHdr   = AllocateZeroPool (BlockSize);
+
+  if (PartHdr == NULL) {
+    DEBUG ((EFI_D_ERROR, "Allocate pool error\n"));
+    return FALSE;
+  }
+
+  PEntryLBA = (PartHeader->MyLBA == PRIMARY_PART_HEADER_LBA) ? \
+                             (PartHeader->LastUsableLBA + 1) : \
+                             (PRIMARY_PART_HEADER_LBA + 1);
+
+  CopyMem (PartHdr, PartHeader, sizeof (EFI_PARTITION_TABLE_HEADER));
+
+  PartHdr->MyLBA              = PartHeader->AlternateLBA;
+  PartHdr->AlternateLBA       = PartHeader->MyLBA;
+  PartHdr->PartitionEntryLBA  = PEntryLBA;
+  PartitionSetCrc ((EFI_TABLE_HEADER *) PartHdr);
+
+  Status = DiskIo->WriteDisk (
+                     DiskIo,
+                     MediaId,
+                     MultU64x32 (PartHdr->MyLBA, (UINT32) BlockSize),
+                     BlockSize,
+                     PartHdr
+                     );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Ptr = AllocatePool (PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry);
+  if (Ptr == NULL) {
+    DEBUG ((EFI_D_ERROR, " Allocate pool error\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  Status = DiskIo->ReadDisk (
+                    DiskIo,
+                    MediaId,
+                    MultU64x32(PartHeader->PartitionEntryLBA, (UINT32) BlockSize),
+                    PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry,
+                    Ptr
+                    );
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = DiskIo->WriteDisk (
+                    DiskIo,
+                    MediaId,
+                    MultU64x32(PEntryLBA, (UINT32) BlockSize),
+                    PartHeader->NumberOfPartitionEntries * PartHeader->SizeOfPartitionEntry,
+                    Ptr
+                    );
+
+Done:
+  FreePool (PartHdr);
+
+  if (Ptr != NULL) {
+    FreePool (Ptr);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  This routine will check GPT partition entry and return entry status.
+
+  Caution: This function may receive untrusted input.
+  The GPT partition entry is external input, so this routine
+  will do basic validation for GPT partition entry and report status.
+
+  @param[in]    PartHeader    Partition table header structure
+  @param[in]    PartEntry     The partition entry array
+  @param[out]   PEntryStatus  the partition entry status array 
+                              recording the status of each partition
+
+**/
+VOID
+PartitionCheckGptEntry (
+  IN  EFI_PARTITION_TABLE_HEADER  *PartHeader,
+  IN  EFI_PARTITION_ENTRY         *PartEntry,
+  OUT EFI_PARTITION_ENTRY_STATUS  *PEntryStatus
+  )
+{
+  EFI_LBA              StartingLBA;
+  EFI_LBA              EndingLBA;
+  EFI_PARTITION_ENTRY  *Entry;
+  UINTN                Index1;
+  UINTN                Index2;
+
+  DEBUG ((EFI_D_INFO, " start check partition entries\n"));
+  for (Index1 = 0; Index1 < PartHeader->NumberOfPartitionEntries; Index1++) {
+    Entry = (EFI_PARTITION_ENTRY *) ((UINT8 *) PartEntry + Index1 * PartHeader->SizeOfPartitionEntry);
+    if (CompareGuid (&Entry->PartitionTypeGUID, &gEfiPartTypeUnusedGuid)) {
+      continue;
+    }
+
+    StartingLBA = Entry->StartingLBA;
+    EndingLBA   = Entry->EndingLBA;
+    if (StartingLBA > EndingLBA ||
+        StartingLBA < PartHeader->FirstUsableLBA ||
+        StartingLBA > PartHeader->LastUsableLBA ||
+        EndingLBA < PartHeader->FirstUsableLBA ||
+        EndingLBA > PartHeader->LastUsableLBA
+        ) {
+      PEntryStatus[Index1].OutOfRange = TRUE;
+      continue;
+    }
+
+    if ((Entry->Attributes & BIT1) != 0) {
+      //
+      // If Bit 1 is set, this indicate that this is an OS specific GUID partition. 
+      //
+      PEntryStatus[Index1].OsSpecific = TRUE;
+    }
+
+    for (Index2 = Index1 + 1; Index2 < PartHeader->NumberOfPartitionEntries; Index2++) {
+      Entry = (EFI_PARTITION_ENTRY *) ((UINT8 *) PartEntry + Index2 * PartHeader->SizeOfPartitionEntry);
+      if (CompareGuid (&Entry->PartitionTypeGUID, &gEfiPartTypeUnusedGuid)) {
+        continue;
+      }
+
+      if (Entry->EndingLBA >= StartingLBA && Entry->StartingLBA <= EndingLBA) {
+        //
+        // This region overlaps with the Index1'th region
+        //
+        PEntryStatus[Index1].Overlap  = TRUE;
+        PEntryStatus[Index2].Overlap  = TRUE;
+        continue;
+      }
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, " End check partition entries\n"));
+}
+
+
+/**
+  Updates the CRC32 value in the table header.
+
+  @param  Hdr    Table to update
+
+**/
+VOID
+PartitionSetCrc (
+  IN OUT EFI_TABLE_HEADER *Hdr
+  )
+{
+  PartitionSetCrcAltSize (Hdr->HeaderSize, Hdr);
+}
+
+
+/**
+  Updates the CRC32 value in the table header.
+
+  @param  Size   The size of the table
+  @param  Hdr    Table to update
+
+**/
+VOID
+PartitionSetCrcAltSize (
+  IN UINTN                 Size,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  )
+{
+  UINT32  Crc;
+
+  Hdr->CRC32 = 0;
+  gBS->CalculateCrc32 ((UINT8 *) Hdr, Size, &Crc);
+  Hdr->CRC32 = Crc;
+}
+
+
+/**
+  Checks the CRC32 value in the table header.
+
+  @param  MaxSize   Max Size limit
+  @param  Hdr       Table to check
+
+  @return TRUE      CRC Valid
+  @return FALSE     CRC Invalid
+
+**/
+BOOLEAN
+PartitionCheckCrc (
+  IN UINTN                 MaxSize,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  )
+{
+  return PartitionCheckCrcAltSize (MaxSize, Hdr->HeaderSize, Hdr);
+}
+
+
+/**
+  Checks the CRC32 value in the table header.
+
+  @param  MaxSize   Max Size limit
+  @param  Size      The size of the table
+  @param  Hdr       Table to check
+
+  @return TRUE    CRC Valid
+  @return FALSE   CRC Invalid
+
+**/
+BOOLEAN
+PartitionCheckCrcAltSize (
+  IN UINTN                 MaxSize,
+  IN UINTN                 Size,
+  IN OUT EFI_TABLE_HEADER  *Hdr
+  )
+{
+  UINT32      Crc;
+  UINT32      OrgCrc;
+  EFI_STATUS  Status;
+
+  Crc = 0;
+
+  if (Size == 0) {
+    //
+    // If header size is 0 CRC will pass so return FALSE here
+    //
+    return FALSE;
+  }
+
+  if ((MaxSize != 0) && (Size > MaxSize)) {
+    DEBUG ((EFI_D_ERROR, "CheckCrc32: Size > MaxSize\n"));
+    return FALSE;
+  }
+  //
+  // clear old crc from header
+  //
+  OrgCrc      = Hdr->CRC32;
+  Hdr->CRC32  = 0;
+
+  Status      = gBS->CalculateCrc32 ((UINT8 *) Hdr, Size, &Crc);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "CheckCrc32: Crc calculation failed\n"));
+    return FALSE;
+  }
+  //
+  // set results
+  //
+  Hdr->CRC32 = Crc;
+
+  //
+  // return status
+  //
+  DEBUG_CODE_BEGIN ();
+    if (OrgCrc != Crc) {
+      DEBUG ((EFI_D_ERROR, "CheckCrc32: Crc check failed\n"));
+    }
+  DEBUG_CODE_END ();
+
+  return (BOOLEAN) (OrgCrc == Crc);
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Mbr.c b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Mbr.c
new file mode 100644
index 0000000000..4b0159c4fa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Mbr.c
@@ -0,0 +1,331 @@
+/** @file
+  Decode a hard disk partitioned with the legacy MBR found on most PC's
+
+  MBR - Master Boot Record is in the first sector of a partitioned hard disk.
+        The MBR supports four partitions per disk. The MBR also contains legacy
+        code that is not run on an EFI system. The legacy code reads the 
+        first sector of the active partition into memory and 
+
+  BPB - BIOS Parameter Block is in the first sector of a FAT file system. 
+        The BPB contains information about the FAT file system. The BPB is 
+        always on the first sector of a media. The first sector also contains
+        the legacy boot strap code.
+
+Copyright (c) 2014, Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2013, 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 "Partition.h"
+
+/**
+  Test to see if the Mbr buffer is a valid MBR.
+
+  @param  Mbr         Parent Handle.
+  @param  LastLba     Last Lba address on the device.
+   
+  @retval TRUE        Mbr is a Valid MBR.
+  @retval FALSE       Mbr is not a Valid MBR.
+
+**/
+BOOLEAN
+PartitionValidMbr (
+  IN  MASTER_BOOT_RECORD      *Mbr,
+  IN  EFI_LBA                 LastLba
+  )
+{
+  UINT32  StartingLBA;
+  UINT32  EndingLBA;
+  UINT32  NewEndingLBA;
+  INTN    Index1;
+  INTN    Index2;
+  BOOLEAN MbrValid;
+
+  if (Mbr->Signature != MBR_SIGNATURE) {
+    return FALSE;
+  }
+  //
+  // The BPB also has this signature, so it can not be used alone.
+  //
+  MbrValid = FALSE;
+  for (Index1 = 0; Index1 < MAX_MBR_PARTITIONS; Index1++) {
+    if (Mbr->Partition[Index1].OSIndicator == 0x00 || UNPACK_UINT32 (Mbr->Partition[Index1].SizeInLBA) == 0) {
+      continue;
+    }
+
+    MbrValid    = TRUE;
+    StartingLBA = UNPACK_UINT32 (Mbr->Partition[Index1].StartingLBA);
+    EndingLBA   = StartingLBA + UNPACK_UINT32 (Mbr->Partition[Index1].SizeInLBA) - 1;
+    if (EndingLBA > LastLba) {
+      //
+      // Compatibility Errata:
+      //  Some systems try to hide drive space with their INT 13h driver
+      //  This does not hide space from the OS driver. This means the MBR
+      //  that gets created from DOS is smaller than the MBR created from
+      //  a real OS (NT & Win98). This leads to BlockIo->LastBlock being
+      //  wrong on some systems FDISKed by the OS.
+      //
+      // return FALSE since no block devices on a system are implemented
+      // with INT 13h
+      //
+    
+      DEBUG((EFI_D_INFO, "PartitionValidMbr: Bad MBR partition size EndingLBA(%1x) > LastLBA(%1x)\n", EndingLBA, LastLba));
+
+      return FALSE;
+    }
+
+    for (Index2 = Index1 + 1; Index2 < MAX_MBR_PARTITIONS; Index2++) {
+      if (Mbr->Partition[Index2].OSIndicator == 0x00 || UNPACK_UINT32 (Mbr->Partition[Index2].SizeInLBA) == 0) {
+        continue;
+      }
+
+      NewEndingLBA = UNPACK_UINT32 (Mbr->Partition[Index2].StartingLBA) + UNPACK_UINT32 (Mbr->Partition[Index2].SizeInLBA) - 1;
+      if (NewEndingLBA >= StartingLBA && UNPACK_UINT32 (Mbr->Partition[Index2].StartingLBA) <= EndingLBA) {
+        //
+        // This region overlaps with the Index1'th region
+        //
+        return FALSE;
+      }
+    }
+  }
+  //
+  // None of the regions overlapped so MBR is O.K.
+  //
+  return MbrValid;
+}
+
+
+/**
+  Install child handles if the Handle supports MBR format.
+
+  @param[in]  This              Calling context.
+  @param[in]  Handle            Parent Handle.
+  @param[in]  DiskIo            Parent DiskIo interface.
+  @param[in]  DiskIo2           Parent DiskIo2 interface.
+  @param[in]  BlockIo           Parent BlockIo interface.
+  @param[in]  BlockIo2          Parent BlockIo2 interface.
+  @param[in]  DevicePath        Parent Device Path.
+   
+  @retval EFI_SUCCESS       A child handle was added.
+  @retval EFI_MEDIA_CHANGED Media change was detected.
+  @retval Others            MBR partition was not found.
+
+**/
+EFI_STATUS
+PartitionInstallMbrChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  )
+{
+  EFI_STATUS                Status;
+  MASTER_BOOT_RECORD        *Mbr;
+  UINT32                    ExtMbrStartingLba;
+  UINTN                     Index;
+  HARDDRIVE_DEVICE_PATH     HdDev;
+  HARDDRIVE_DEVICE_PATH     ParentHdDev;
+  EFI_STATUS                Found;
+  UINT32                    PartitionNumber;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePathNode;
+  EFI_DEVICE_PATH_PROTOCOL  *LastDevicePathNode;
+  UINT32                    BlockSize;
+  UINT32                    MediaId;
+  EFI_LBA                   LastBlock;
+
+  Found           = EFI_NOT_FOUND;
+
+  BlockSize = BlockIo->Media->BlockSize;
+  MediaId   = BlockIo->Media->MediaId;
+  LastBlock = BlockIo->Media->LastBlock;
+
+  Mbr = AllocatePool (BlockSize);
+  if (Mbr == NULL) {
+    return Found;
+  }
+
+  Status = DiskIo->ReadDisk (
+                     DiskIo,
+                     MediaId,
+                     0,
+                     BlockSize,
+                     Mbr
+                     );
+  if (EFI_ERROR (Status)) {
+    Found = Status;
+    goto Done;
+  }
+  if (!PartitionValidMbr (Mbr, LastBlock)) {
+    goto Done;
+  }
+  //
+  // We have a valid mbr - add each partition
+  //
+  //
+  // Get starting and ending LBA of the parent block device.
+  //
+  LastDevicePathNode = NULL;
+  ZeroMem (&ParentHdDev, sizeof (ParentHdDev));
+  DevicePathNode = DevicePath;
+  while (!IsDevicePathEnd (DevicePathNode)) {
+    LastDevicePathNode  = DevicePathNode;
+    DevicePathNode      = NextDevicePathNode (DevicePathNode);
+  }
+
+  if (LastDevicePathNode != NULL) {
+    if (DevicePathType (LastDevicePathNode) == MEDIA_DEVICE_PATH &&
+        DevicePathSubType (LastDevicePathNode) == MEDIA_HARDDRIVE_DP
+        ) {
+      CopyMem (&ParentHdDev, LastDevicePathNode, sizeof (ParentHdDev));
+    } else {
+      LastDevicePathNode = NULL;
+    }
+  }
+
+  PartitionNumber = 1;
+
+  ZeroMem (&HdDev, sizeof (HdDev));
+  HdDev.Header.Type     = MEDIA_DEVICE_PATH;
+  HdDev.Header.SubType  = MEDIA_HARDDRIVE_DP;
+  SetDevicePathNodeLength (&HdDev.Header, sizeof (HdDev));
+  HdDev.MBRType         = MBR_TYPE_PCAT;
+  HdDev.SignatureType   = SIGNATURE_TYPE_MBR;
+
+  if (LastDevicePathNode == NULL) {
+    //
+    // This is a MBR, add each partition
+    //
+    for (Index = 0; Index < MAX_MBR_PARTITIONS; Index++) {
+      if (Mbr->Partition[Index].OSIndicator == 0x00 || UNPACK_UINT32 (Mbr->Partition[Index].SizeInLBA) == 0) {
+        //
+        // Don't use null MBR entries
+        //
+        continue;
+      }
+
+      if (Mbr->Partition[Index].OSIndicator == PMBR_GPT_PARTITION) {
+        //
+        // This is the guard MBR for the GPT. If you ever see a GPT disk with zero partitions you can get here.
+        //  We can not produce an MBR BlockIo for this device as the MBR spans the GPT headers. So formating 
+        //  this BlockIo would corrupt the GPT structures and require a recovery that would corrupt the format
+        //  that corrupted the GPT partition. 
+        //
+        continue;
+      }
+
+      HdDev.PartitionNumber = PartitionNumber ++;
+      HdDev.PartitionStart  = UNPACK_UINT32 (Mbr->Partition[Index].StartingLBA);
+      HdDev.PartitionSize   = UNPACK_UINT32 (Mbr->Partition[Index].SizeInLBA);
+      CopyMem (HdDev.Signature, &(Mbr->UniqueMbrSignature[0]), sizeof (Mbr->UniqueMbrSignature));
+
+      Status = PartitionInstallChildHandle (
+                This,
+                Handle,
+                DiskIo,
+                DiskIo2,
+                BlockIo,
+                BlockIo2,
+                DevicePath,
+                (EFI_DEVICE_PATH_PROTOCOL *) &HdDev,
+                HdDev.PartitionStart,
+                HdDev.PartitionStart + HdDev.PartitionSize - 1,
+                MBR_SIZE,
+                (BOOLEAN) (Mbr->Partition[Index].OSIndicator == EFI_PARTITION)
+                );
+
+      if (!EFI_ERROR (Status)) {
+        Found = EFI_SUCCESS;
+      }
+    }
+  } else {
+    //
+    // It's an extended partition. Follow the extended partition
+    // chain to get all the logical drives
+    //
+    ExtMbrStartingLba = 0;
+
+    do {
+
+      Status = DiskIo->ReadDisk (
+                         DiskIo,
+                         MediaId,
+                         MultU64x32 (ExtMbrStartingLba, BlockSize),
+                         BlockSize,
+                         Mbr
+                         );
+      if (EFI_ERROR (Status)) {
+        Found = Status;
+        goto Done;
+      }
+
+      if (UNPACK_UINT32 (Mbr->Partition[0].SizeInLBA) == 0) {
+        break;
+      }
+
+      if ((Mbr->Partition[0].OSIndicator == EXTENDED_DOS_PARTITION) ||
+          (Mbr->Partition[0].OSIndicator == EXTENDED_WINDOWS_PARTITION)) {
+        ExtMbrStartingLba = UNPACK_UINT32 (Mbr->Partition[0].StartingLBA);
+        continue;
+      }
+      HdDev.PartitionNumber = PartitionNumber ++;
+      HdDev.PartitionStart  = UNPACK_UINT32 (Mbr->Partition[0].StartingLBA) + ExtMbrStartingLba + ParentHdDev.PartitionStart;
+      HdDev.PartitionSize   = UNPACK_UINT32 (Mbr->Partition[0].SizeInLBA);
+      if ((HdDev.PartitionStart + HdDev.PartitionSize - 1 >= ParentHdDev.PartitionStart + ParentHdDev.PartitionSize) ||
+          (HdDev.PartitionStart <= ParentHdDev.PartitionStart)) {
+        break;
+      }
+
+      //
+      // The signature in EBR(Extended Boot Record) should always be 0.
+      //
+      *((UINT32 *) &HdDev.Signature[0]) = 0;
+
+      Status = PartitionInstallChildHandle (
+                 This,
+                 Handle,
+                 DiskIo,
+                 DiskIo2,
+                 BlockIo,
+                 BlockIo2,
+                 DevicePath,
+                 (EFI_DEVICE_PATH_PROTOCOL *) &HdDev,
+                 HdDev.PartitionStart - ParentHdDev.PartitionStart,
+                 HdDev.PartitionStart - ParentHdDev.PartitionStart + HdDev.PartitionSize - 1,
+                 MBR_SIZE,
+                 (BOOLEAN) (Mbr->Partition[0].OSIndicator == EFI_PARTITION)
+                 );
+      if (!EFI_ERROR (Status)) {
+        Found = EFI_SUCCESS;
+      }
+
+      if ((Mbr->Partition[1].OSIndicator != EXTENDED_DOS_PARTITION) &&
+          (Mbr->Partition[1].OSIndicator != EXTENDED_WINDOWS_PARTITION)
+          ) {
+        break;
+      }
+
+      ExtMbrStartingLba = UNPACK_UINT32 (Mbr->Partition[1].StartingLBA);
+      //
+      // Don't allow partition to be self referencing
+      //
+      if (ExtMbrStartingLba == 0) {
+        break;
+      }
+    } while (ExtMbrStartingLba  < ParentHdDev.PartitionSize);
+  }
+
+Done:
+  FreePool (Mbr);
+
+  return Found;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.c b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.c
new file mode 100644
index 0000000000..1c53bf0233
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.c
@@ -0,0 +1,1339 @@
+/** @file
+  Partition driver that produces logical BlockIo devices from a physical
+  BlockIo device. The logical BlockIo devices are based on the format
+  of the raw block devices media. Currently "El Torito CD-ROM", Legacy
+  MBR, and GPT partition schemes are supported.
+
+Copyright (c) 2006 - 2014, 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 "Partition.h"
+
+//
+// Partition Driver Global Variables.
+//
+EFI_DRIVER_BINDING_PROTOCOL gPartitionDriverBinding = {
+  PartitionDriverBindingSupported,
+  PartitionDriverBindingStart,
+  PartitionDriverBindingStop,
+  //
+  // Grub4Dos copies the BPB of the first partition to the MBR. If the 
+  // DriverBindingStart() of the Fat driver gets run before that of Partition 
+  // driver only the first partition can be recognized.
+  // Let the driver binding version of Partition driver be higher than that of
+  // Fat driver to make sure the DriverBindingStart() of the Partition driver
+  // gets run before that of Fat driver so that all the partitions can be recognized.
+  //
+  0xb,
+  NULL,
+  NULL
+};
+
+//
+// Prioritized function list to detect partition table. 
+//
+PARTITION_DETECT_ROUTINE mPartitionDetectRoutineTable[] = {
+  PartitionInstallGptChildHandles,
+  PartitionInstallElToritoChildHandles,
+  PartitionInstallMbrChildHandles,
+  NULL
+};
+
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a BlockIo and DiskIo protocol or a BlockIo2 protocol can be
+  supported.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test.
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_DISK_IO_PROTOCOL      *DiskIo;
+  EFI_DEV_PATH              *Node;
+
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, go on checking other conditions
+    //
+    if (!IsDevicePathEnd (RemainingDevicePath)) {
+      //
+      // If RemainingDevicePath isn't the End of Device Path Node,
+      // check its validation
+      //
+      Node = (EFI_DEV_PATH *) RemainingDevicePath;
+      if (Node->DevPath.Type != MEDIA_DEVICE_PATH ||
+        Node->DevPath.SubType != MEDIA_HARDDRIVE_DP ||
+        DevicePathNodeLength (&Node->DevPath) != sizeof (HARDDRIVE_DEVICE_PATH)) {
+        return EFI_UNSUPPORTED;
+      }
+    }
+  }
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  (VOID **) &DiskIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Close the I/O Abstraction(s) used to perform the supported test
+  //
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiDiskIoProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  //
+  // Open the EFI Device Path protocol needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (Status == EFI_ALREADY_STARTED) {
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close protocol, don't use device path protocol in the Support() function
+  //
+  gBS->CloseProtocol (
+        ControllerHandle,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        ControllerHandle
+        );
+
+  //
+  // Open the IO Abstraction(s) needed to perform the supported test
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+/**
+  Start this driver on ControllerHandle by opening a Block IO or a Block IO2
+  or both, and Disk IO protocol, reading Device Path, and creating a child
+  handle with a Disk IO and device path protocol.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  EFI_STATUS                Status;
+  EFI_STATUS                OpenStatus;
+  EFI_BLOCK_IO_PROTOCOL     *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL    *BlockIo2;
+  EFI_DISK_IO_PROTOCOL      *DiskIo;
+  EFI_DISK_IO2_PROTOCOL     *DiskIo2;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  PARTITION_DETECT_ROUTINE  *Routine;
+  BOOLEAN                   MediaPresent;
+  EFI_TPL                   OldTpl;
+
+  BlockIo2 = NULL;
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK); 
+  //
+  // Check RemainingDevicePath validation
+  //
+  if (RemainingDevicePath != NULL) {
+    //
+    // Check if RemainingDevicePath is the End of Device Path Node, 
+    // if yes, return EFI_SUCCESS
+    //
+    if (IsDevicePathEnd (RemainingDevicePath)) {
+      Status = EFI_SUCCESS;
+      goto Exit;
+    }
+  }
+
+  //
+  // Try to open BlockIO and BlockIO2. If BlockIO would be opened, continue,
+  // otherwise, return error.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIoProtocolGuid,
+                  (VOID **) &BlockIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiBlockIo2ProtocolGuid,
+                  (VOID **) &BlockIo2,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    BlockIo2 = NULL;
+  }
+
+  //
+  // Get the Device Path Protocol on ControllerHandle's handle.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    goto Exit;
+  }
+
+  //
+  // Get the DiskIo and DiskIo2.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  (VOID **) &DiskIo,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    goto Exit;
+  }
+
+  OpenStatus = Status;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDiskIo2ProtocolGuid,
+                  (VOID **) &DiskIo2,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    DiskIo2 = NULL;
+  }
+
+  //
+  // Try to read blocks when there's media or it is removable physical partition.
+  //
+  Status       = EFI_UNSUPPORTED;
+  MediaPresent = BlockIo->Media->MediaPresent;
+  if (BlockIo->Media->MediaPresent ||
+      (BlockIo->Media->RemovableMedia && !BlockIo->Media->LogicalPartition)) {
+    //
+    // Try for GPT, then El Torito, and then legacy MBR partition types. If the
+    // media supports a given partition type install child handles to represent
+    // the partitions described by the media.
+    //
+    Routine = &mPartitionDetectRoutineTable[0];
+    while (*Routine != NULL) {
+      Status = (*Routine) (
+                   This,
+                   ControllerHandle,
+                   DiskIo,
+                   DiskIo2,
+                   BlockIo,
+                   BlockIo2,
+                   ParentDevicePath
+                   );
+      if (!EFI_ERROR (Status) || Status == EFI_MEDIA_CHANGED || Status == EFI_NO_MEDIA) {
+        break;
+      }
+      Routine++;
+    }
+  }
+  //
+  // In the case that the driver is already started (OpenStatus == EFI_ALREADY_STARTED),
+  // the DevicePathProtocol and the DiskIoProtocol are not actually opened by the
+  // driver. So don't try to close them. Otherwise, we will break the dependency
+  // between the controller and the driver set up before.
+  //
+  // In the case that when the media changes on a device it will Reinstall the 
+  // BlockIo interaface. This will cause a call to our Stop(), and a subsequent
+  // reentrant call to our Start() successfully. We should leave the device open
+  // when this happen. The "media change" case includes either the status is
+  // EFI_MEDIA_CHANGED or it is a "media" to "no media" change. 
+  //  
+  if (EFI_ERROR (Status)          &&
+      !EFI_ERROR (OpenStatus)     &&
+      Status != EFI_MEDIA_CHANGED &&
+      !(MediaPresent && Status == EFI_NO_MEDIA)) {
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDiskIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    //
+    // Close Parent DiskIo2 if has.
+    //    
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiDiskIo2ProtocolGuid,
+           This->DriverBindingHandle,
+           ControllerHandle
+           );
+
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+  }
+
+Exit:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle. Support stopping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   Index;
+  EFI_BLOCK_IO_PROTOCOL   *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL  *BlockIo2;
+  BOOLEAN                 AllChildrenStopped;
+  PARTITION_PRIVATE_DATA  *Private;
+  EFI_DISK_IO_PROTOCOL    *DiskIo;
+
+  BlockIo  = NULL;
+  BlockIo2 = NULL;
+  Private = NULL;
+
+  if (NumberOfChildren == 0) {
+    //
+    // In the case of re-entry of the PartitionDriverBindingStop, the
+    // NumberOfChildren may not reflect the actual number of children on the
+    // bus driver. Hence, additional check is needed here.
+    //
+    if (HasChildren (ControllerHandle)) {
+      DEBUG((EFI_D_ERROR, "PartitionDriverBindingStop: Still has child.\n"));
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // Close the bus driver
+    //
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDiskIoProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    //
+    // Close Parent BlockIO2 if has.
+    //    
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiDiskIo2ProtocolGuid,
+           This->DriverBindingHandle,
+           ControllerHandle
+           );
+
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    return EFI_SUCCESS;
+  }
+
+  AllChildrenStopped = TRUE;
+  for (Index = 0; Index < NumberOfChildren; Index++) {
+    gBS->OpenProtocol (
+           ChildHandleBuffer[Index],
+           &gEfiBlockIoProtocolGuid,
+           (VOID **) &BlockIo,
+           This->DriverBindingHandle,
+           ControllerHandle,
+           EFI_OPEN_PROTOCOL_GET_PROTOCOL
+           );
+    //
+    // Try to locate BlockIo2.
+    //
+    gBS->OpenProtocol (
+           ChildHandleBuffer[Index],
+           &gEfiBlockIo2ProtocolGuid,
+           (VOID **) &BlockIo2,
+           This->DriverBindingHandle,
+           ControllerHandle,
+           EFI_OPEN_PROTOCOL_GET_PROTOCOL
+           ); 
+
+
+    Private = PARTITION_DEVICE_FROM_BLOCK_IO_THIS (BlockIo);
+    if (Private->InStop) {
+      //
+      // If the child handle is going to be stopped again during the re-entry
+      // of DriverBindingStop, just do nothing.
+      //
+      break;
+    }
+    Private->InStop = TRUE;
+
+    BlockIo->FlushBlocks (BlockIo);
+
+    if (BlockIo2 != NULL) {
+      Status = BlockIo2->FlushBlocksEx (BlockIo2, NULL);
+      DEBUG((EFI_D_ERROR, "PartitionDriverBindingStop: FlushBlocksEx returned with %r\n", Status));
+    } else {
+      Status = EFI_SUCCESS;
+    }
+
+    gBS->CloseProtocol (
+           ControllerHandle,
+           &gEfiDiskIoProtocolGuid,
+           This->DriverBindingHandle,
+           ChildHandleBuffer[Index]
+           );
+    //
+    // All Software protocols have be freed from the handle so remove it.
+    // Remove the BlockIo Protocol if has.
+    // Remove the BlockIo2 Protocol if has.
+    //
+    if (BlockIo2 != NULL) {
+      //
+      // Some device drivers might re-install the BlockIO(2) protocols for a
+      // media change condition. Therefore, if the FlushBlocksEx returned with
+      // EFI_MEDIA_CHANGED, just let the BindingStop fail to avoid potential
+      // reference of already stopped child handle.
+      //
+      if (Status != EFI_MEDIA_CHANGED) {
+        Status = gBS->UninstallMultipleProtocolInterfaces (
+                         ChildHandleBuffer[Index],
+                         &gEfiDevicePathProtocolGuid,
+                         Private->DevicePath,
+                         &gEfiBlockIoProtocolGuid,
+                         &Private->BlockIo,
+                         &gEfiBlockIo2ProtocolGuid,
+                         &Private->BlockIo2,
+                         Private->EspGuid,
+                         NULL,
+                         NULL
+                         );
+      }
+    } else {
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                       ChildHandleBuffer[Index],
+                       &gEfiDevicePathProtocolGuid,
+                       Private->DevicePath,
+                       &gEfiBlockIoProtocolGuid,
+                       &Private->BlockIo,
+                       Private->EspGuid,
+                       NULL,
+                       NULL
+                       );
+    }
+
+    if (EFI_ERROR (Status)) {
+      Private->InStop = FALSE;
+      gBS->OpenProtocol (
+             ControllerHandle,
+             &gEfiDiskIoProtocolGuid,
+             (VOID **) &DiskIo,
+             This->DriverBindingHandle,
+             ChildHandleBuffer[Index],
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+    } else {
+      FreePool (Private->DevicePath);
+      FreePool (Private);
+    }
+
+    if (EFI_ERROR (Status)) {
+      AllChildrenStopped = FALSE;
+      if (Status == EFI_MEDIA_CHANGED) {
+        break;
+      }
+    }
+  }
+
+  if (!AllChildrenStopped) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset the Block Device.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionReset (
+  IN EFI_BLOCK_IO_PROTOCOL  *This,
+  IN BOOLEAN                ExtendedVerification
+  )
+{
+  PARTITION_PRIVATE_DATA  *Private;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO_THIS (This);
+
+  return Private->ParentBlockIo->Reset (
+                                  Private->ParentBlockIo,
+                                  ExtendedVerification
+                                  );
+}
+
+/**
+  Probe the media status and return EFI_NO_MEDIA or EFI_MEDIA_CHANGED
+  for no media or media change case. Otherwise DefaultStatus is returned.
+
+  @param DiskIo             Pointer to the DiskIo instance.
+  @param MediaId            Id of the media, changes every time the media is replaced.
+  @param DefaultStatus      The default status to return when it's not the no media
+                            or media change case.
+
+  @retval EFI_NO_MEDIA      There is no media.
+  @retval EFI_MEDIA_CHANGED The media was changed.
+  @retval others            The default status to return.
+**/
+EFI_STATUS
+ProbeMediaStatus (
+  IN EFI_DISK_IO_PROTOCOL    *DiskIo,
+  IN UINT32                  MediaId,
+  IN EFI_STATUS              DefaultStatus
+  )
+{
+  EFI_STATUS                 Status;
+  UINT8                      Buffer[1];
+
+  //
+  // Read 1 byte from offset 0 to check if the MediaId is still valid.
+  // The reading operation is synchronious thus it is not worth it to
+  // allocate a buffer from the pool. The destination buffer for the
+  // data is in the stack.
+  //
+  Status = DiskIo->ReadDisk (DiskIo, MediaId, 0, 1, (VOID*)Buffer);
+  if ((Status == EFI_NO_MEDIA) || (Status == EFI_MEDIA_CHANGED)) {
+    return Status;
+  }
+  return DefaultStatus;
+}
+
+/**
+  Read by using the Disk IO protocol on the parent device. Lba addresses
+  must be converted to byte offsets.
+
+  @param  This       Protocol instance pointer.
+  @param  MediaId    Id of the media, changes every time the media is replaced.
+  @param  Lba        The starting Logical Block Address to read from
+  @param  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param  Buffer     Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that are not
+                                valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionReadBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This,
+  IN UINT32                 MediaId,
+  IN EFI_LBA                Lba,
+  IN UINTN                  BufferSize,
+  OUT VOID                  *Buffer
+  )
+{
+  PARTITION_PRIVATE_DATA  *Private;
+  UINT64                  Offset;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO_THIS (This);
+
+  if (BufferSize % Private->BlockSize != 0) {
+    return ProbeMediaStatus (Private->DiskIo, MediaId, EFI_BAD_BUFFER_SIZE);
+  }
+
+  Offset = MultU64x32 (Lba, Private->BlockSize) + Private->Start;
+  if (Offset + BufferSize > Private->End) {
+    return ProbeMediaStatus (Private->DiskIo, MediaId, EFI_INVALID_PARAMETER);
+  }
+  //
+  // Because some kinds of partition have different block size from their parent
+  // device, we call the Disk IO protocol on the parent device, not the Block IO
+  // protocol
+  //
+  return Private->DiskIo->ReadDisk (Private->DiskIo, MediaId, Offset, BufferSize, Buffer);
+}
+
+/**
+  Write by using the Disk IO protocol on the parent device. Lba addresses
+  must be converted to byte offsets.
+
+  @param[in]  This       Protocol instance pointer.
+  @param[in]  MediaId    Id of the media, changes every time the media is replaced.
+  @param[in]  Lba        The starting Logical Block Address to read from
+  @param[in]  BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]  Buffer     Buffer containing data to be written to device.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains a LBA that is not
+                                valid for the device.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionWriteBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This,
+  IN UINT32                 MediaId,
+  IN EFI_LBA                Lba,
+  IN UINTN                  BufferSize,
+  IN VOID                  *Buffer
+  )
+{
+  PARTITION_PRIVATE_DATA  *Private;
+  UINT64                  Offset;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO_THIS (This);
+
+  if (BufferSize % Private->BlockSize != 0) {
+    return ProbeMediaStatus (Private->DiskIo, MediaId, EFI_BAD_BUFFER_SIZE);
+  }
+
+  Offset = MultU64x32 (Lba, Private->BlockSize) + Private->Start;
+  if (Offset + BufferSize > Private->End) {
+    return ProbeMediaStatus (Private->DiskIo, MediaId, EFI_INVALID_PARAMETER);
+  }
+  //
+  // Because some kinds of partition have different block size from their parent
+  // device, we call the Disk IO protocol on the parent device, not the Block IO
+  // protocol
+  //
+  return Private->DiskIo->WriteDisk (Private->DiskIo, MediaId, Offset, BufferSize, Buffer);
+}
+
+
+/**
+  Flush the parent Block Device.
+
+  @param  This              Protocol instance pointer.
+
+  @retval EFI_SUCCESS       All outstanding data was written to the device
+  @retval EFI_DEVICE_ERROR  The device reported an error while writting back the data
+  @retval EFI_NO_MEDIA      There is no media in the device.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  )
+{
+  PARTITION_PRIVATE_DATA  *Private;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO_THIS (This);
+
+  return Private->ParentBlockIo->FlushBlocks (Private->ParentBlockIo);
+}
+
+/**
+  Probe the media status and return EFI_NO_MEDIA or EFI_MEDIA_CHANGED
+  for no media or media change case. Otherwise DefaultStatus is returned.
+
+  @param DiskIo2            Pointer to the DiskIo2 instance.
+  @param MediaId            Id of the media, changes every time the media is replaced.
+  @param DefaultStatus      The default status to return when it's not the no media
+                            or media change case.
+
+  @retval EFI_NO_MEDIA      There is no media.
+  @retval EFI_MEDIA_CHANGED The media was changed.
+  @retval others            The default status to return.
+**/
+EFI_STATUS
+ProbeMediaStatusEx (
+  IN EFI_DISK_IO2_PROTOCOL   *DiskIo2,
+  IN UINT32                  MediaId,
+  IN EFI_STATUS              DefaultStatus
+  )
+{
+  EFI_STATUS                 Status;
+
+  //
+  // Read 1 byte from offset 0 but passing NULL as buffer pointer
+  //
+  Status = DiskIo2->ReadDiskEx (DiskIo2, MediaId, 0, NULL, 1, NULL);
+  if ((Status == EFI_NO_MEDIA) || (Status == EFI_MEDIA_CHANGED)) {
+    return Status;
+  }
+  return DefaultStatus;
+}
+
+/**
+  Reset the Block Device throught Block I/O2 protocol.
+
+  @param  This                 Protocol instance pointer.
+  @param  ExtendedVerification Driver may perform diagnostics on reset.
+
+  @retval EFI_SUCCESS          The device was reset.
+  @retval EFI_DEVICE_ERROR     The device is not functioning properly and could
+                               not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionResetEx (
+  IN EFI_BLOCK_IO2_PROTOCOL *This,
+  IN BOOLEAN                ExtendedVerification
+  )
+{
+  PARTITION_PRIVATE_DATA  *Private;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO2_THIS (This);
+
+  return Private->ParentBlockIo2->Reset (
+                                    Private->ParentBlockIo2,
+                                    ExtendedVerification
+                                    );
+}
+
+/**
+  The general callback for the DiskIo2 interfaces.
+  @param  Event                 Event whose notification function is being invoked.
+  @param  Context               The pointer to the notification function's context,
+                                which points to the PARTITION_ACCESS_TASK instance.
+**/
+VOID
+EFIAPI
+PartitionOnAccessComplete (
+  IN EFI_EVENT                 Event,
+  IN VOID                      *Context
+  )
+{
+  PARTITION_ACCESS_TASK   *Task;
+
+  Task = (PARTITION_ACCESS_TASK *) Context;
+
+  gBS->CloseEvent (Event);
+
+  Task->BlockIo2Token->TransactionStatus = Task->DiskIo2Token.TransactionStatus;
+  gBS->SignalEvent (Task->BlockIo2Token->Event);
+
+  FreePool (Task);
+}
+
+/**
+  Create a new PARTITION_ACCESS_TASK instance.
+
+  @param  Token  Pointer to the EFI_BLOCK_IO2_TOKEN.
+
+  @return Pointer to the created PARTITION_ACCESS_TASK instance or NULL upon failure.
+**/
+PARTITION_ACCESS_TASK *
+PartitionCreateAccessTask (
+  IN EFI_BLOCK_IO2_TOKEN    *Token
+  )
+{
+  EFI_STATUS                Status;
+  PARTITION_ACCESS_TASK     *Task;
+
+  Task = AllocatePool (sizeof (*Task));
+  if (Task == NULL) {
+    return NULL;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  PartitionOnAccessComplete,
+                  Task,
+                  &Task->DiskIo2Token.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Task);
+    return NULL;
+  }
+
+  Task->BlockIo2Token = Token;
+
+  return Task;
+}
+
+/**
+  Read BufferSize bytes from Lba into Buffer.
+  
+  This function reads the requested number of blocks from the device. All the
+  blocks are read, or an error is returned.
+  If EFI_DEVICE_ERROR, EFI_NO_MEDIA,_or EFI_MEDIA_CHANGED is returned and
+  non-blocking I/O is being used, the Event associated with this request will
+  not be signaled.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    Id of the media, changes every time the media is 
+                              replaced.
+  @param[in]       Lba        The starting Logical Block Address to read from.
+  @param[in, out]  Token	    A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.  
+  @param[out]      Buffer     A pointer to the destination buffer for the data. The 
+                              caller is responsible for either having implicit or 
+                              explicit ownership of the buffer.
+
+  @retval EFI_SUCCESS           The read request was queued if Token->Event is
+                                not NULL.The data was read correctly from the
+                                device if the Token->Event is NULL.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHANGED     The MediaId is not for the current media.
+  @retval EFI_BAD_BUFFER_SIZE   The BufferSize parameter is not a multiple of the
+                                intrinsic block size of the device.
+  @retval EFI_INVALID_PARAMETER The read request contains LBAs that are not valid, 
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+**/
+EFI_STATUS
+EFIAPI
+PartitionReadBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  OUT    VOID                   *Buffer
+  )
+{
+  EFI_STATUS              Status;
+  PARTITION_PRIVATE_DATA  *Private;
+  UINT64                  Offset;
+  PARTITION_ACCESS_TASK   *Task;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO2_THIS (This);
+
+  if (BufferSize % Private->BlockSize != 0) {
+    return ProbeMediaStatusEx (Private->DiskIo2, MediaId, EFI_BAD_BUFFER_SIZE);
+  }
+
+  Offset = MultU64x32 (Lba, Private->BlockSize) + Private->Start;
+  if (Offset + BufferSize > Private->End) {
+    return ProbeMediaStatusEx (Private->DiskIo2, MediaId, EFI_INVALID_PARAMETER);
+  }
+
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Task = PartitionCreateAccessTask (Token);
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = Private->DiskIo2->ReadDiskEx (Private->DiskIo2, MediaId, Offset, &Task->DiskIo2Token, BufferSize, Buffer);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseEvent (Task->DiskIo2Token.Event);
+      FreePool (Task);
+    }
+  } else {
+    Status = Private->DiskIo2->ReadDiskEx (Private->DiskIo2, MediaId, Offset, NULL, BufferSize, Buffer);
+  }
+
+  return Status;
+}
+
+/**
+  Write BufferSize bytes from Lba into Buffer.
+
+  This function writes the requested number of blocks to the device. All blocks
+  are written, or an error is returned.If EFI_DEVICE_ERROR, EFI_NO_MEDIA,
+  EFI_WRITE_PROTECTED or EFI_MEDIA_CHANGED is returned and non-blocking I/O is
+  being used, the Event associated with this request will not be signaled.
+
+  @param[in]       This       Indicates a pointer to the calling context.
+  @param[in]       MediaId    The media ID that the write request is for.
+  @param[in]       Lba        The starting logical block address to be written. The
+                              caller is responsible for writing to only legitimate
+                              locations.
+  @param[in, out]  Token      A pointer to the token associated with the transaction.
+  @param[in]       BufferSize Size of Buffer, must be a multiple of device block size.
+  @param[in]       Buffer     A pointer to the source buffer for the data.
+
+  @retval EFI_SUCCESS           The write request was queued if Event is not NULL.
+                                The data was written correctly to the device if
+                                the Event is NULL.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing the write.
+  @retval EFI_BAD_BUFFER_SIZE   The Buffer was not a multiple of the block size of the device.
+  @retval EFI_INVALID_PARAMETER The write request contains LBAs that are not valid, 
+                                or the buffer is not on proper alignment.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack
+                                of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionWriteBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN     UINT32                 MediaId,
+  IN     EFI_LBA                Lba,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token,
+  IN     UINTN                  BufferSize,
+  IN     VOID                   *Buffer
+  )
+{
+  EFI_STATUS              Status;
+  PARTITION_PRIVATE_DATA  *Private;
+  UINT64                  Offset;
+  PARTITION_ACCESS_TASK   *Task;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO2_THIS (This);
+
+  if (BufferSize % Private->BlockSize != 0) {
+    return ProbeMediaStatusEx (Private->DiskIo2, MediaId, EFI_BAD_BUFFER_SIZE);
+  }
+
+  Offset = MultU64x32 (Lba, Private->BlockSize) + Private->Start;
+  if (Offset + BufferSize > Private->End) {
+    return ProbeMediaStatusEx (Private->DiskIo2, MediaId, EFI_INVALID_PARAMETER);
+  }
+  
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Task = PartitionCreateAccessTask (Token);
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status =  Private->DiskIo2->WriteDiskEx (Private->DiskIo2, MediaId, Offset, &Task->DiskIo2Token, BufferSize, Buffer);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseEvent (Task->DiskIo2Token.Event);
+      FreePool (Task);
+    }
+  } else {
+    Status = Private->DiskIo2->WriteDiskEx (Private->DiskIo2, MediaId, Offset, NULL, BufferSize, Buffer);
+  }
+  return Status;
+}
+
+/**
+  Flush the Block Device.
+ 
+  If EFI_DEVICE_ERROR, EFI_NO_MEDIA,_EFI_WRITE_PROTECTED or EFI_MEDIA_CHANGED
+  is returned and non-blocking I/O is being used, the Event associated with
+  this request will not be signaled.  
+
+  @param[in]      This     Indicates a pointer to the calling context.
+  @param[in, out] Token    A pointer to the token associated with the transaction
+
+  @retval EFI_SUCCESS          The flush request was queued if Event is not NULL.
+                               All outstanding data was written correctly to the
+                               device if the Event is NULL.
+  @retval EFI_DEVICE_ERROR     The device reported an error while writting back
+                               the data.
+  @retval EFI_WRITE_PROTECTED  The device cannot be written to.
+  @retval EFI_NO_MEDIA         There is no media in the device.
+  @retval EFI_MEDIA_CHANGED    The MediaId is not for the current media.
+  @retval EFI_OUT_OF_RESOURCES The request could not be completed due to a lack
+                               of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionFlushBlocksEx (
+  IN     EFI_BLOCK_IO2_PROTOCOL *This,
+  IN OUT EFI_BLOCK_IO2_TOKEN    *Token
+  )
+{
+  EFI_STATUS              Status;
+  PARTITION_PRIVATE_DATA  *Private;
+  PARTITION_ACCESS_TASK   *Task;
+
+  Private = PARTITION_DEVICE_FROM_BLOCK_IO2_THIS (This);
+
+  if ((Token != NULL) && (Token->Event != NULL)) {
+    Task = PartitionCreateAccessTask (Token);
+    if (Task == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = Private->DiskIo2->FlushDiskEx (Private->DiskIo2, &Task->DiskIo2Token);
+    if (EFI_ERROR (Status)) {
+      gBS->CloseEvent (Task->DiskIo2Token.Event);
+      FreePool (Task);
+    }
+  } else {
+    Status = Private->DiskIo2->FlushDiskEx (Private->DiskIo2, NULL);
+  }
+  return Status;
+}
+
+
+/**
+  Create a child handle for a logical block device that represents the
+  bytes Start to End of the Parent Block IO device.
+
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ParentHandle      Parent Handle for new child.
+  @param[in]  ParentDiskIo      Parent DiskIo interface.
+  @param[in]  ParentDiskIo2     Parent DiskIo2 interface.
+  @param[in]  ParentBlockIo     Parent BlockIo interface.
+  @param[in]  ParentBlockIo2    Parent BlockIo2 interface.
+  @param[in]  ParentDevicePath  Parent Device Path.
+  @param[in]  DevicePathNode    Child Device Path node.
+  @param[in]  Start             Start Block.
+  @param[in]  End               End Block.
+  @param[in]  BlockSize         Child block size.
+  @param[in]  InstallEspGuid    Flag to install EFI System Partition GUID on handle.
+
+  @retval EFI_SUCCESS       A child handle was added.
+  @retval other             A child handle was not added.
+
+**/
+EFI_STATUS
+PartitionInstallChildHandle (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ParentHandle,
+  IN  EFI_DISK_IO_PROTOCOL         *ParentDiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *ParentDiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *ParentBlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *ParentBlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *ParentDevicePath,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePathNode,
+  IN  EFI_LBA                      Start,
+  IN  EFI_LBA                      End,
+  IN  UINT32                       BlockSize,
+  IN  BOOLEAN                      InstallEspGuid
+  )
+{
+  EFI_STATUS              Status;
+  PARTITION_PRIVATE_DATA  *Private;
+
+  Status  = EFI_SUCCESS;
+  Private = AllocateZeroPool (sizeof (PARTITION_PRIVATE_DATA));
+  if (Private == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Signature        = PARTITION_PRIVATE_DATA_SIGNATURE;
+
+  Private->Start            = MultU64x32 (Start, ParentBlockIo->Media->BlockSize);
+  Private->End              = MultU64x32 (End + 1, ParentBlockIo->Media->BlockSize);
+
+  Private->BlockSize        = BlockSize;
+  Private->ParentBlockIo    = ParentBlockIo;
+  Private->ParentBlockIo2   = ParentBlockIo2;
+  Private->DiskIo           = ParentDiskIo;
+  Private->DiskIo2          = ParentDiskIo2;
+
+  //
+  // Set the BlockIO into Private Data.
+  //
+  Private->BlockIo.Revision = ParentBlockIo->Revision;
+  
+  Private->BlockIo.Media    = &Private->Media;
+  CopyMem (Private->BlockIo.Media, ParentBlockIo->Media, sizeof (EFI_BLOCK_IO_MEDIA));
+
+  Private->BlockIo.Reset        = PartitionReset;
+  Private->BlockIo.ReadBlocks   = PartitionReadBlocks;
+  Private->BlockIo.WriteBlocks  = PartitionWriteBlocks;
+  Private->BlockIo.FlushBlocks  = PartitionFlushBlocks;
+
+  //
+  // Set the BlockIO2 into Private Data.
+  //
+  if (Private->DiskIo2 != NULL) {
+    ASSERT (Private->ParentBlockIo2 != NULL);
+    Private->BlockIo2.Media    = &Private->Media2;
+    CopyMem (Private->BlockIo2.Media, ParentBlockIo2->Media, sizeof (EFI_BLOCK_IO_MEDIA));
+
+    Private->BlockIo2.Reset          = PartitionResetEx;
+    Private->BlockIo2.ReadBlocksEx   = PartitionReadBlocksEx;
+    Private->BlockIo2.WriteBlocksEx  = PartitionWriteBlocksEx;
+    Private->BlockIo2.FlushBlocksEx  = PartitionFlushBlocksEx; 
+  }
+
+  Private->Media.IoAlign   = 0;
+  Private->Media.LogicalPartition = TRUE;
+  Private->Media.LastBlock = DivU64x32 (
+                               MultU64x32 (
+                                 End - Start + 1,
+                                 ParentBlockIo->Media->BlockSize
+                                 ),
+                                BlockSize
+                               ) - 1;
+
+  Private->Media.BlockSize = (UINT32) BlockSize;
+
+  Private->Media2.IoAlign   = 0;
+  Private->Media2.LogicalPartition = TRUE;
+  Private->Media2.LastBlock = Private->Media.LastBlock;
+  Private->Media2.BlockSize = (UINT32) BlockSize;
+
+  //
+  // Per UEFI Spec, LowestAlignedLba, LogicalBlocksPerPhysicalBlock and OptimalTransferLengthGranularity must be 0
+  //  for logical partitions.
+  //
+  if (Private->BlockIo.Revision >= EFI_BLOCK_IO_PROTOCOL_REVISION2) {
+    Private->Media.LowestAlignedLba               = 0;
+    Private->Media.LogicalBlocksPerPhysicalBlock  = 0;
+    Private->Media2.LowestAlignedLba              = 0;
+    Private->Media2.LogicalBlocksPerPhysicalBlock = 0;
+    if (Private->BlockIo.Revision >= EFI_BLOCK_IO_PROTOCOL_REVISION3) {
+      Private->Media.OptimalTransferLengthGranularity  = 0;
+      Private->Media2.OptimalTransferLengthGranularity = 0;
+    }
+  }
+
+  Private->DevicePath     = AppendDevicePathNode (ParentDevicePath, DevicePathNode);
+
+  if (Private->DevicePath == NULL) {
+    FreePool (Private);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (InstallEspGuid) {
+    Private->EspGuid = &gEfiPartTypeSystemPartGuid;
+  } else {
+    //
+    // If NULL InstallMultipleProtocolInterfaces will ignore it.
+    //
+    Private->EspGuid = NULL;
+  }
+
+  //
+  // Create the new handle. 
+  //
+  Private->Handle = NULL;
+  if (Private->DiskIo2 != NULL) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Private->Handle,
+                    &gEfiDevicePathProtocolGuid,
+                    Private->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &Private->BlockIo,
+                    &gEfiBlockIo2ProtocolGuid,
+                    &Private->BlockIo2,
+                    Private->EspGuid,
+                    NULL,
+                    NULL
+                    );
+  } else {    
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Private->Handle,
+                    &gEfiDevicePathProtocolGuid,
+                    Private->DevicePath,
+                    &gEfiBlockIoProtocolGuid,
+                    &Private->BlockIo,
+                    Private->EspGuid,
+                    NULL,
+                    NULL
+                    );
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Open the Parent Handle for the child
+    //
+    Status = gBS->OpenProtocol (
+                    ParentHandle,
+                    &gEfiDiskIoProtocolGuid,
+                    (VOID **) &ParentDiskIo,
+                    This->DriverBindingHandle,
+                    Private->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+  } else {
+    FreePool (Private->DevicePath);
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+
+/**
+  The user Entry Point for module Partition. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePartition (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gPartitionDriverBinding,
+             ImageHandle,
+             &gPartitionComponentName,
+             &gPartitionComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+
+  return Status;
+}
+
+
+/**
+  Test to see if there is any child on ControllerHandle.
+
+  @param[in]  ControllerHandle    Handle of device to test.
+
+  @retval TRUE                    There are children on the ControllerHandle.
+  @retval FALSE                   No child is on the ControllerHandle.
+
+**/
+BOOLEAN
+HasChildren (
+  IN EFI_HANDLE           ControllerHandle
+  )
+{
+  EFI_OPEN_PROTOCOL_INFORMATION_ENTRY  *OpenInfoBuffer;
+  UINTN                                EntryCount;
+  EFI_STATUS                           Status;
+  UINTN                                Index;
+
+  Status = gBS->OpenProtocolInformation (
+                  ControllerHandle,
+                  &gEfiDiskIoProtocolGuid,
+                  &OpenInfoBuffer,
+                  &EntryCount
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  for (Index = 0; Index < EntryCount; Index++) {
+    if ((OpenInfoBuffer[Index].Attributes & EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) != 0) {
+      break;
+    }
+  }
+  FreePool (OpenInfoBuffer);
+
+  return (BOOLEAN) (Index < EntryCount);
+}
+
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.h b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.h
new file mode 100644
index 0000000000..7cb19882cb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/Partition.h
@@ -0,0 +1,458 @@
+/** @file
+  Partition driver that produces logical BlockIo devices from a physical 
+  BlockIo device. The logical BlockIo devices are based on the format
+  of the raw block devices media. Currently "El Torito CD-ROM", Legacy 
+  MBR, and GPT partition schemes are supported.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _PARTITION_H_ 
+#define _PARTITION_H_ 
+
+#include <Uefi.h>
+#include <Protocol/BlockIo.h>
+#include <Protocol/BlockIo2.h>
+#include <Guid/Gpt.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/DiskIo.h>
+#include <Protocol/DiskIo2.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+
+#include <IndustryStandard/Mbr.h>
+#include <IndustryStandard/ElTorito.h>
+
+
+//
+// Partition private data
+//
+#define PARTITION_PRIVATE_DATA_SIGNATURE  SIGNATURE_32 ('P', 'a', 'r', 't')
+typedef struct {
+  UINT64                    Signature;
+
+  EFI_HANDLE                Handle;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_BLOCK_IO_PROTOCOL     BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL    BlockIo2;
+  EFI_BLOCK_IO_MEDIA        Media;
+  EFI_BLOCK_IO_MEDIA        Media2;//For BlockIO2
+
+  EFI_DISK_IO_PROTOCOL      *DiskIo;
+  EFI_DISK_IO2_PROTOCOL     *DiskIo2;
+  EFI_BLOCK_IO_PROTOCOL     *ParentBlockIo;
+  EFI_BLOCK_IO2_PROTOCOL    *ParentBlockIo2;
+  UINT64                    Start;
+  UINT64                    End;
+  UINT32                    BlockSize;
+  BOOLEAN                   InStop;
+
+  EFI_GUID                  *EspGuid;
+
+} PARTITION_PRIVATE_DATA;
+
+typedef struct {
+  EFI_DISK_IO2_TOKEN           DiskIo2Token;
+  EFI_BLOCK_IO2_TOKEN          *BlockIo2Token;
+} PARTITION_ACCESS_TASK;
+
+#define PARTITION_DEVICE_FROM_BLOCK_IO_THIS(a)  CR (a, PARTITION_PRIVATE_DATA, BlockIo, PARTITION_PRIVATE_DATA_SIGNATURE)
+#define PARTITION_DEVICE_FROM_BLOCK_IO2_THIS(a) CR (a, PARTITION_PRIVATE_DATA, BlockIo2, PARTITION_PRIVATE_DATA_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL   gPartitionDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gPartitionComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gPartitionComponentName2;
+
+//
+// Extract INT32 from char array
+//
+#define UNPACK_INT32(a) (INT32)( (((UINT8 *) a)[0] <<  0) |    \
+                                 (((UINT8 *) a)[1] <<  8) |    \
+                                 (((UINT8 *) a)[2] << 16) |    \
+                                 (((UINT8 *) a)[3] << 24) )
+
+//
+// Extract UINT32 from char array
+//
+#define UNPACK_UINT32(a) (UINT32)( (((UINT8 *) a)[0] <<  0) |    \
+                                   (((UINT8 *) a)[1] <<  8) |    \
+                                   (((UINT8 *) a)[2] << 16) |    \
+                                   (((UINT8 *) a)[3] << 24) )
+
+
+//
+// GPT Partition Entry Status
+//
+typedef struct {
+  BOOLEAN OutOfRange;
+  BOOLEAN Overlap;
+  BOOLEAN OsSpecific;
+} EFI_PARTITION_ENTRY_STATUS;
+
+//
+// Function Prototypes
+//
+/**
+  Test to see if this driver supports ControllerHandle. Any ControllerHandle
+  than contains a BlockIo and DiskIo protocol can be supported.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Start this driver on ControllerHandle by opening a Block IO and Disk IO
+  protocol, reading Device Path, and creating a child handle with a
+  Disk IO and device path protocol.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop this driver on ControllerHandle. Support stopping any child handles
+  created by this driver.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionDriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  UINTN                         NumberOfChildren,
+  IN  EFI_HANDLE                    *ChildHandleBuffer
+  );
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is not a valid EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PartitionComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+/**
+  Create a child handle for a logical block device that represents the
+  bytes Start to End of the Parent Block IO device.
+
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ParentHandle      Parent Handle for new child.
+  @param[in]  ParentDiskIo      Parent DiskIo interface.
+  @param[in]  ParentDiskIo2     Parent DiskIo2 interface.
+  @param[in]  ParentBlockIo     Parent BlockIo interface.
+  @param[in]  ParentBlockIo2    Parent BlockIo2 interface.
+  @param[in]  ParentDevicePath  Parent Device Path.
+  @param[in]  DevicePathNode    Child Device Path node.
+  @param[in]  Start             Start Block.
+  @param[in]  End               End Block.
+  @param[in]  BlockSize         Child block size.
+  @param[in]  InstallEspGuid    Flag to install EFI System Partition GUID on handle.
+
+  @retval EFI_SUCCESS       A child handle was added.
+  @retval other             A child handle was not added.
+
+**/
+EFI_STATUS
+PartitionInstallChildHandle (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ParentHandle,
+  IN  EFI_DISK_IO_PROTOCOL         *ParentDiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *ParentDiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *ParentBlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *ParentBlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *ParentDevicePath,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePathNode,
+  IN  EFI_LBA                      Start,
+  IN  EFI_LBA                      End,
+  IN  UINT32                       BlockSize,
+  IN  BOOLEAN                      InstallEspGuid
+  );
+
+/**
+  Test to see if there is any child on ControllerHandle.
+
+  @param[in]  ControllerHandle    Handle of device to test.
+
+  @retval TRUE                    There are children on the ControllerHandle.
+  @retval FALSE                   No child is on the ControllerHandle.
+
+**/
+BOOLEAN
+HasChildren (
+  IN EFI_HANDLE           ControllerHandle
+  );
+
+/**
+  Install child handles if the Handle supports GPT partition structure.
+
+  @param[in]  This       Calling context.
+  @param[in]  Handle     Parent Handle.
+  @param[in]  DiskIo     Parent DiskIo interface.
+  @param[in]  DiskIo2    Parent DiskIo2 interface.
+  @param[in]  BlockIo    Parent BlockIo interface.
+  @param[in]  BlockIo2   Parent BlockIo2 interface.
+  @param[in]  DevicePath Parent Device Path.
+
+  @retval EFI_SUCCESS           Valid GPT disk.
+  @retval EFI_MEDIA_CHANGED     Media changed Detected.
+  @retval EFI_INVALID_PARAMETER If both BlockIo and BlockIo2 are NULL;
+  @retval other                 Not a valid GPT disk.
+
+**/
+EFI_STATUS
+PartitionInstallGptChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  );
+
+/**
+  Install child handles if the Handle supports El Torito format.
+
+  @param[in]  This        Calling context.
+  @param[in]  Handle      Parent Handle.
+  @param[in]  DiskIo      Parent DiskIo interface.
+  @param[in]  DiskIo2     Parent DiskIo2 interface.
+  @param[in]  BlockIo     Parent BlockIo interface.
+  @param[in]  BlockIo2    Parent BlockIo2 interface.
+  @param[in]  DevicePath  Parent Device Path
+
+
+  @retval EFI_SUCCESS         Child handle(s) was added.
+  @retval EFI_MEDIA_CHANGED   Media changed Detected.
+  @retval other               no child handle was added.
+
+**/
+EFI_STATUS
+PartitionInstallElToritoChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  );
+
+/**
+  Install child handles if the Handle supports MBR format.
+
+  @param[in]  This              Calling context.
+  @param[in]  Handle            Parent Handle.
+  @param[in]  DiskIo            Parent DiskIo interface.
+  @param[in]  DiskIo2           Parent DiskIo2 interface.
+  @param[in]  BlockIo           Parent BlockIo interface.
+  @param[in]  BlockIo2          Parent BlockIo2 interface.
+  @param[in]  DevicePath        Parent Device Path.
+   
+  @retval EFI_SUCCESS       A child handle was added.
+  @retval EFI_MEDIA_CHANGED Media change was detected.
+  @retval Others            MBR partition was not found.
+
+**/
+EFI_STATUS
+PartitionInstallMbrChildHandles (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  );
+
+typedef
+EFI_STATUS
+(*PARTITION_DETECT_ROUTINE) (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   Handle,
+  IN  EFI_DISK_IO_PROTOCOL         *DiskIo,
+  IN  EFI_DISK_IO2_PROTOCOL        *DiskIo2,
+  IN  EFI_BLOCK_IO_PROTOCOL        *BlockIo,
+  IN  EFI_BLOCK_IO2_PROTOCOL       *BlockIo2,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *DevicePath
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.inf b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.inf
new file mode 100644
index 0000000000..680626378f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.inf
@@ -0,0 +1,89 @@
+## @file
+#  Modules that produces the logic Block I/O protocol for every partition via the physical Block I/O.
+#
+#  This module produces the logical Block I/O device that represents
+#  the bytes from Start to End of the Parent Block I/O device.
+#  The partition of physical BlockIo device supported is one of legacy MBR, GPT,
+#  and "El Torito" partitions.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - disk partition.
+#  This external input must be validated carefully to avoid security issue like
+#  buffer overflow, integer overflow.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PartitionDxe
+  MODULE_UNI_FILE                = PartitionDxe.uni
+  FILE_GUID                      = 1FA1F39E-FEFF-4aae-BD7B-38A070A3B609
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePartition
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                = gPartitionDriverBinding
+#  COMPONENT_NAME                = gPartitionComponentName
+#  COMPONENT_NAME2               = gPartitionComponentName2
+#
+
+[Sources]
+  ComponentName.c
+  Mbr.c
+  Gpt.c
+  ElTorito.c
+  Partition.c
+  Partition.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  DevicePathLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiLib
+  BaseLib
+  UefiDriverEntryPoint
+  DebugLib
+
+
+[Guids]
+  gEfiPartTypeUnusedGuid                        ## SOMETIMES_CONSUMES ## GUID
+  ## SOMETIMES_CONSUMES ## GUID
+  ## SOMETIMES_CONSUMES ## GUID # Install protocol
+  gEfiPartTypeSystemPartGuid
+
+
+[Protocols]
+  ## BY_START
+  ## TO_START
+  gEfiBlockIoProtocolGuid
+  ## BY_START
+  ## TO_START
+  gEfiBlockIo2ProtocolGuid
+  ## BY_START
+  ## TO_START
+  gEfiDevicePathProtocolGuid
+  gEfiDiskIoProtocolGuid                        ## TO_START
+  gEfiDiskIo2ProtocolGuid                       ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PartitionDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.uni b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.uni
new file mode 100644
index 0000000000..53e439843d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxeExtra.uni b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxeExtra.uni
new file mode 100644
index 0000000000..a9b13695a9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/PartitionDxe/PartitionDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.inf b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.inf
new file mode 100644
index 0000000000..46c834c648
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.inf
@@ -0,0 +1,60 @@
+## @file
+#  English module that provides Unicode Collation supports.
+#  
+#  This driver installs Unicode ISO 639-2 Collation and
+#  RFC 4646 Unicode Collation 2 protocols based on feature flags
+#  PcdUnicodeCollationSupport & PcdUnicodeCollation2Support respectively.
+#  It allows code running in the boot services environment to perform lexical
+#  comparison functions on Unicode strings for English languages.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EnglishDxe
+  MODULE_UNI_FILE                = EnglishDxe.uni
+  FILE_GUID                      = CD3BAFB6-50FB-4fe8-8E4E-AB74D2C1A600
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeUnicodeCollationEng
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  UnicodeCollationEng.c
+  UnicodeCollationEng.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  PcdLib
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollationSupport     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollation2Support    ## CONSUMES
+
+[Protocols]
+  gEfiUnicodeCollationProtocolGuid | gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollationSupport      ## SOMETIMES_PRODUCES
+  gEfiUnicodeCollation2ProtocolGuid | gEfiMdeModulePkgTokenSpaceGuid.PcdUnicodeCollation2Support    ## PRODUCES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EnglishDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.uni b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.uni
new file mode 100644
index 0000000000..4ca63be4d1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxeExtra.uni b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxeExtra.uni
new file mode 100644
index 0000000000..1ec5c4c3f9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/EnglishDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.c b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.c
new file mode 100644
index 0000000000..a37f5c923c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.c
@@ -0,0 +1,473 @@
+/** @file
+  Driver to implement English version of Unicode Collation Protocol.
+
+Copyright (c) 2006 - 2011, 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 "UnicodeCollationEng.h"
+
+CHAR8 mEngUpperMap[MAP_TABLE_SIZE];
+CHAR8 mEngLowerMap[MAP_TABLE_SIZE];
+CHAR8 mEngInfoMap[MAP_TABLE_SIZE];
+
+CHAR8 mOtherChars[] = {
+  '0',
+  '1',
+  '2',
+  '3',
+  '4',
+  '5',
+  '6',
+  '7',
+  '8',
+  '9',
+  '\\',
+  '.',
+  '_',
+  '^',
+  '$',
+  '~',
+  '!',
+  '#',
+  '%',
+  '&',
+  '-',
+  '{',
+  '}',
+  '(',
+  ')',
+  '@',
+  '`',
+  '\'',
+  '\0'
+};
+
+EFI_HANDLE  mHandle = NULL;
+
+//
+// EFI Unicode Collation Protocol supporting ISO 639-2 language code
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_COLLATION_PROTOCOL  UnicodeEng = {
+  EngStriColl,
+  EngMetaiMatch,
+  EngStrLwr,
+  EngStrUpr,
+  EngFatToStr,
+  EngStrToFat,
+  "eng"
+};
+
+//
+// EFI Unicode Collation2 Protocol supporting RFC 4646 language code
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_COLLATION_PROTOCOL  Unicode2Eng = {
+  EngStriColl,
+  EngMetaiMatch,
+  EngStrLwr,
+  EngStrUpr,
+  EngFatToStr,
+  EngStrToFat,
+  "en"
+};
+
+/**
+  The user Entry Point for English module.
+ 
+  This function initializes unicode character mapping and then installs Unicode
+  Collation & Unicode Collation 2 Protocols based on the feature flags.  
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.  
+  @param  SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUnicodeCollationEng (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  UINTN       Index2;
+
+  //
+  // Initialize mapping tables for the supported languages
+  //
+  for (Index = 0; Index < MAP_TABLE_SIZE; Index++) {
+    mEngUpperMap[Index] = (CHAR8) Index;
+    mEngLowerMap[Index] = (CHAR8) Index;
+    mEngInfoMap[Index]  = 0;
+
+    if ((Index >= 'a' && Index <= 'z') || (Index >= 0xe0 && Index <= 0xf6) || (Index >= 0xf8 && Index <= 0xfe)) {
+
+      Index2                = Index - 0x20;
+      mEngUpperMap[Index]   = (CHAR8) Index2;
+      mEngLowerMap[Index2]  = (CHAR8) Index;
+
+      mEngInfoMap[Index] |= CHAR_FAT_VALID;
+      mEngInfoMap[Index2] |= CHAR_FAT_VALID;
+    }
+  }
+
+  for (Index = 0; mOtherChars[Index] != 0; Index++) {
+    Index2 = mOtherChars[Index];
+    mEngInfoMap[Index2] |= CHAR_FAT_VALID;
+  }
+
+  if (FeaturePcdGet (PcdUnicodeCollation2Support)) {
+    if (FeaturePcdGet (PcdUnicodeCollationSupport)) {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mHandle,
+                      &gEfiUnicodeCollationProtocolGuid,
+                      &UnicodeEng,
+                      &gEfiUnicodeCollation2ProtocolGuid,
+                      &Unicode2Eng,
+                      NULL
+                      );
+      ASSERT_EFI_ERROR (Status);
+    } else {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mHandle,
+                      &gEfiUnicodeCollation2ProtocolGuid,
+                      &Unicode2Eng,
+                      NULL
+                      );
+      ASSERT_EFI_ERROR (Status);
+    }
+  } else {
+    if (FeaturePcdGet (PcdUnicodeCollationSupport)) {
+      Status = gBS->InstallMultipleProtocolInterfaces (
+                      &mHandle,
+                      &gEfiUnicodeCollationProtocolGuid,
+                      &UnicodeEng,
+                      NULL
+                      );
+      ASSERT_EFI_ERROR (Status);
+    } else {
+      //
+      // This module must support to produce at least one of Unicode Collation Protocol
+      // and Unicode Collation 2 Protocol.
+      //
+      ASSERT (FALSE);
+      Status = EFI_UNSUPPORTED;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Performs a case-insensitive comparison of two Null-terminated strings.
+
+  @param  This Protocol instance pointer.
+  @param  Str1 A pointer to a Null-terminated string.
+  @param  Str2 A pointer to a Null-terminated string.
+
+  @retval 0   Str1 is equivalent to Str2
+  @retval > 0 Str1 is lexically greater than Str2
+  @retval < 0 Str1 is lexically less than Str2
+
+**/
+INTN
+EFIAPI
+EngStriColl (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *Str1,
+  IN CHAR16                           *Str2
+  )
+{
+  while (*Str1 != 0) {
+    if (TO_UPPER (*Str1) != TO_UPPER (*Str2)) {
+      break;
+    }
+
+    Str1 += 1;
+    Str2 += 1;
+  }
+
+  return TO_UPPER (*Str1) - TO_UPPER (*Str2);
+}
+
+
+/**
+  Converts all the characters in a Null-terminated string to 
+  lower case characters.
+
+  @param  This   Protocol instance pointer.
+  @param  Str    A pointer to a Null-terminated string.
+
+**/
+VOID
+EFIAPI
+EngStrLwr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN OUT CHAR16                       *Str
+  )
+{
+  while (*Str != 0) {
+    *Str = TO_LOWER (*Str);
+    Str += 1;
+  }
+}
+
+
+/**
+  Converts all the characters in a Null-terminated string to upper
+  case characters.
+
+  @param  This   Protocol instance pointer.
+  @param  Str    A pointer to a Null-terminated string.
+
+**/
+VOID
+EFIAPI
+EngStrUpr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN OUT CHAR16                       *Str
+  )
+{
+  while (*Str != 0) {
+    *Str = TO_UPPER (*Str);
+    Str += 1;
+  }
+}
+
+/**
+  Performs a case-insensitive comparison of a Null-terminated
+  pattern string and a Null-terminated string.
+
+  @param  This    Protocol instance pointer.
+  @param  String  A pointer to a Null-terminated string.
+  @param  Pattern A pointer to a Null-terminated pattern string.
+
+  @retval TRUE    Pattern was found in String.
+  @retval FALSE   Pattern was not found in String.
+
+**/
+BOOLEAN
+EFIAPI
+EngMetaiMatch (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *String,
+  IN CHAR16                           *Pattern
+  )
+{
+  CHAR16  CharC;
+  CHAR16  CharP;
+  CHAR16  Index3;
+
+  for (;;) {
+    CharP = *Pattern;
+    Pattern += 1;
+
+    switch (CharP) {
+    case 0:
+      //
+      // End of pattern.  If end of string, TRUE match
+      //
+      if (*String != 0) {
+        return FALSE;
+      } else {
+        return TRUE;
+      }
+
+    case '*':
+      //
+      // Match zero or more chars
+      //
+      while (*String != 0) {
+        if (EngMetaiMatch (This, String, Pattern)) {
+          return TRUE;
+        }
+
+        String += 1;
+      }
+
+      return EngMetaiMatch (This, String, Pattern);
+
+    case '?':
+      //
+      // Match any one char
+      //
+      if (*String == 0) {
+        return FALSE;
+      }
+
+      String += 1;
+      break;
+
+    case '[':
+      //
+      // Match char set
+      //
+      CharC = *String;
+      if (CharC == 0) {
+        //
+        // syntax problem
+        //
+        return FALSE;
+      }
+
+      Index3  = 0;
+      CharP   = *Pattern++;
+      while (CharP != 0) {
+        if (CharP == ']') {
+          return FALSE;
+        }
+
+        if (CharP == '-') {
+          //
+          // if range of chars, get high range
+          //
+          CharP = *Pattern;
+          if (CharP == 0 || CharP == ']') {
+            //
+            // syntax problem
+            //
+            return FALSE;
+          }
+
+          if (TO_UPPER (CharC) >= TO_UPPER (Index3) && TO_UPPER (CharC) <= TO_UPPER (CharP)) {
+            //
+            // if in range, it's a match
+            //
+            break;
+          }
+        }
+
+        Index3 = CharP;
+        if (TO_UPPER (CharC) == TO_UPPER (CharP)) {
+          //
+          // if char matches
+          //
+          break;
+        }
+
+        CharP = *Pattern++;
+      }
+      //
+      // skip to end of match char set
+      //
+      while ((CharP != 0) && (CharP != ']')) {
+        CharP = *Pattern;
+        Pattern += 1;
+      }
+
+      String += 1;
+      break;
+
+    default:
+      CharC = *String;
+      if (TO_UPPER (CharC) != TO_UPPER (CharP)) {
+        return FALSE;
+      }
+
+      String += 1;
+      break;
+    }
+  }
+}
+
+
+/**
+  Converts an 8.3 FAT file name in an OEM character set to a Null-terminated string.
+
+  @param  This    Protocol instance pointer.
+  @param  FatSize The size of the string Fat in bytes.
+  @param  Fat     A pointer to a Null-terminated string that contains an 8.3 file
+                  name using an 8-bit OEM character set.
+  @param  String  A pointer to a Null-terminated string. The string must
+                  be preallocated to hold FatSize characters.
+
+**/
+VOID
+EFIAPI
+EngFatToStr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN UINTN                            FatSize,
+  IN CHAR8                            *Fat,
+  OUT CHAR16                          *String
+  )
+{
+  //
+  // No DBCS issues, just expand and add null terminate to end of string
+  //
+  while ((*Fat != 0) && (FatSize != 0)) {
+    *String = *Fat;
+    String += 1;
+    Fat += 1;
+    FatSize -= 1;
+  }
+
+  *String = 0;
+}
+
+
+/**
+  Converts a Null-terminated string to legal characters in a FAT 
+  filename using an OEM character set. 
+
+  @param  This    Protocol instance pointer.
+  @param  String  A pointer to a Null-terminated string. The string must
+                  be preallocated to hold FatSize characters.
+  @param  FatSize The size of the string Fat in bytes.
+  @param  Fat     A pointer to a Null-terminated string that contains an 8.3 file
+                  name using an OEM character set.
+
+  @retval TRUE    Fat is a Long File Name
+  @retval FALSE   Fat is an 8.3 file name
+
+**/
+BOOLEAN
+EFIAPI
+EngStrToFat (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *String,
+  IN UINTN                            FatSize,
+  OUT CHAR8                           *Fat
+  )
+{
+  BOOLEAN SpecialCharExist;
+
+  SpecialCharExist = FALSE;
+  while ((*String != 0) && (FatSize != 0)) {
+    //
+    // Skip '.' or ' ' when making a fat name
+    //
+    if (*String != '.' && *String != ' ') {
+      //
+      // If this is a valid fat char, move it.
+      // Otherwise, move a '_' and flag the fact that the name needs a long file name.
+      //
+      if (*String < MAP_TABLE_SIZE && ((mEngInfoMap[*String] & CHAR_FAT_VALID) != 0)) {
+        *Fat = mEngUpperMap[*String];
+      } else {
+        *Fat              = '_';
+        SpecialCharExist  = TRUE;
+      }
+
+      Fat += 1;
+      FatSize -= 1;
+    }
+
+    String += 1;
+  }
+  //
+  // Do not terminate that fat string
+  //
+  return SpecialCharExist;
+}
diff --git a/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.h b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.h
new file mode 100644
index 0000000000..ac6dfc4033
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Disk/UnicodeCollation/EnglishDxe/UnicodeCollationEng.h
@@ -0,0 +1,187 @@
+/** @file
+  Head file for Unicode Collation Protocol (English)
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _UNICODE_COLLATION_ENG_H_
+#define _UNICODE_COLLATION_ENG_H_
+
+
+
+#include <Uefi.h>
+
+#include <Protocol/UnicodeCollation.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/PcdLib.h>
+
+//
+// Bit mask to indicate the validity of character in FAT file name.
+//
+#define CHAR_FAT_VALID  0x01
+
+//
+// Maximum FAT table size.
+//
+#define MAP_TABLE_SIZE  0x100
+
+//
+// Macro to map character a to upper case.
+//
+#define TO_UPPER(a)      (CHAR16) ((a) <= 0xFF ? mEngUpperMap[a] : (a))
+
+//
+// Macro to map character a to lower case.
+//
+#define TO_LOWER(a)      (CHAR16) ((a) <= 0xFF ? mEngLowerMap[a] : (a))
+
+//
+// Prototypes
+//
+/**
+  Performs a case-insensitive comparison of two Null-terminated strings.
+
+  @param  This Protocol instance pointer.
+  @param  Str1 A pointer to a Null-terminated string.
+  @param  Str2 A pointer to a Null-terminated string.
+
+  @retval 0   Str1 is equivalent to Str2
+  @retval > 0 Str1 is lexically greater than Str2
+  @retval < 0 Str1 is lexically less than Str2
+
+**/
+INTN
+EFIAPI
+EngStriColl (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *Str1,
+  IN CHAR16                           *Str2
+  );
+
+/**
+  Performs a case-insensitive comparison of a Null-terminated
+  pattern string and a Null-terminated string.
+
+  @param  This    Protocol instance pointer.
+  @param  String  A pointer to a Null-terminated string.
+  @param  Pattern A pointer to a Null-terminated pattern string.
+
+  @retval TRUE    Pattern was found in String.
+  @retval FALSE   Pattern was not found in String.
+
+**/
+BOOLEAN
+EFIAPI
+EngMetaiMatch (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *String,
+  IN CHAR16                           *Pattern
+  );
+
+/**
+  Converts all the characters in a Null-terminated string to 
+  lower case characters.
+
+  @param  This   Protocol instance pointer.
+  @param  Str    A pointer to a Null-terminated string.
+
+**/
+VOID
+EFIAPI
+EngStrLwr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN OUT CHAR16                       *Str
+  );
+
+/**
+  Converts all the characters in a Null-terminated string to upper
+  case characters.
+
+  @param  This   Protocol instance pointer.
+  @param  Str    A pointer to a Null-terminated string.
+
+**/
+VOID
+EFIAPI
+EngStrUpr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN OUT CHAR16                       *Str
+  );
+
+/**
+  Converts an 8.3 FAT file name in an OEM character set to a Null-terminated string.
+
+  @param  This    Protocol instance pointer.
+  @param  FatSize The size of the string Fat in bytes.
+  @param  Fat     A pointer to a Null-terminated string that contains an 8.3 file
+                  name using an 8-bit OEM character set.
+  @param  String  A pointer to a Null-terminated string. The string must
+                  be preallocated to hold FatSize characters.
+
+**/
+VOID
+EFIAPI
+EngFatToStr (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN UINTN                            FatSize,
+  IN CHAR8                            *Fat,
+  OUT CHAR16                          *String
+  );
+
+/**
+  Converts a Null-terminated string to legal characters in a FAT 
+  filename using an OEM character set. 
+
+  @param  This    Protocol instance pointer.
+  @param  String  A pointer to a Null-terminated string. The string must
+                  be preallocated to hold FatSize characters.
+  @param  FatSize The size of the string Fat in bytes.
+  @param  Fat     A pointer to a Null-terminated string that contains an 8.3 file
+                  name using an OEM character set.
+
+  @retval TRUE    Fat is a Long File Name
+  @retval FALSE   Fat is an 8.3 file name
+
+**/
+BOOLEAN
+EFIAPI
+EngStrToFat (
+  IN EFI_UNICODE_COLLATION_PROTOCOL   *This,
+  IN CHAR16                           *String,
+  IN UINTN                            FatSize,
+  OUT CHAR8                           *Fat
+  );
+
+/**
+  The user Entry Point for English module.
+ 
+  This function initializes unicode character mapping and then installs Unicode
+  Collation & Unicode Collation 2 Protocols based on the feature flags.  
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.  
+  @param  SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeUnicodeCollationEng (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngine.uni b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngine.uni
new file mode 100644
index 0000000000..9f9d7cc790
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngine.uni
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineDxe.inf b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineDxe.inf
new file mode 100644
index 0000000000..bf5e8e541c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineDxe.inf
@@ -0,0 +1,66 @@
+## @file
+# The DXE driver produces FORM DISPLAY ENGIEN protocol.
+#
+# Copyright (c) 2007 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DisplayEngine
+  MODULE_UNI_FILE                = DisplayEngine.uni
+  FILE_GUID                      = E660EA85-058E-4b55-A54B-F02F83A24707
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeDisplayEngine
+  UNLOAD_IMAGE                   = UnloadDisplayEngine
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FormDisplayStr.uni
+  FormDisplay.c
+  FormDisplay.h
+  ProcessOptions.c
+  InputHandler.c
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  BaseMemoryLib
+  BaseLib
+  PrintLib
+  HiiLib
+  MemoryAllocationLib
+  CustomizedDisplayLib
+
+[Protocols]
+  gEdkiiFormDisplayEngineProtocolGuid   ## PRODUCES
+  gEdkiiFormBrowserEx2ProtocolGuid      ## CONSUMES
+
+[Depex]
+  gEfiHiiDatabaseProtocolGuid AND gEfiHiiConfigRoutingProtocolGuid AND gEdkiiFormBrowserEx2ProtocolGuid
+  
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowserGrayOutTextStatement     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBrowerGrayOutReadOnlyMenu       ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DisplayEngineExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineExtra.uni b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineExtra.uni
new file mode 100644
index 0000000000..31b4ae2f1c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.c b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.c
new file mode 100644
index 0000000000..a391442d16
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.c
@@ -0,0 +1,4143 @@
+/** @file
+Entry and initialization module for the browser.
+
+Copyright (c) 2007 - 2015, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2014, Hewlett-Packard Development Company, L.P.<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 "FormDisplay.h"
+
+//
+// Search table for UiDisplayMenu()
+//
+SCAN_CODE_TO_SCREEN_OPERATION     gScanCodeToOperation[] = {
+  {
+    SCAN_UP,
+    UiUp,
+  },
+  {
+    SCAN_DOWN,
+    UiDown,
+  },
+  {
+    SCAN_PAGE_UP,
+    UiPageUp,
+  },
+  {
+    SCAN_PAGE_DOWN,
+    UiPageDown,
+  },
+  {
+    SCAN_ESC,
+    UiReset,
+  },
+  {
+    SCAN_LEFT,
+    UiLeft,
+  },
+  {
+    SCAN_RIGHT,
+    UiRight,
+  }
+};
+
+UINTN mScanCodeNumber = sizeof (gScanCodeToOperation) / sizeof (gScanCodeToOperation[0]);
+
+SCREEN_OPERATION_T0_CONTROL_FLAG  gScreenOperationToControlFlag[] = {
+  {
+    UiNoOperation,
+    CfUiNoOperation,
+  },
+  {
+    UiSelect,
+    CfUiSelect,
+  },
+  {
+    UiUp,
+    CfUiUp,
+  },
+  {
+    UiDown,
+    CfUiDown,
+  },
+  {
+    UiLeft,
+    CfUiLeft,
+  },
+  {
+    UiRight,
+    CfUiRight,
+  },
+  {
+    UiReset,
+    CfUiReset,
+  },
+  {
+    UiPageUp,
+    CfUiPageUp,
+  },
+  {
+    UiPageDown,
+    CfUiPageDown
+  }, 
+  {
+    UiHotKey,
+    CfUiHotKey
+  }
+};
+
+EFI_GUID  gDisplayEngineGuid = {
+  0xE38C1029, 0xE38F, 0x45b9, {0x8F, 0x0D, 0xE2, 0xE6, 0x0B, 0xC9, 0xB2, 0x62}
+};
+
+BOOLEAN                       gMisMatch;
+EFI_SCREEN_DESCRIPTOR         gStatementDimensions;
+BOOLEAN                       mStatementLayoutIsChanged = TRUE;
+USER_INPUT                    *gUserInput;
+FORM_DISPLAY_ENGINE_FORM      *gFormData;
+EFI_HII_HANDLE                gHiiHandle;
+UINT16                        gDirection;
+LIST_ENTRY                    gMenuOption;
+DISPLAY_HIGHLIGHT_MENU_INFO   gHighligthMenuInfo = {0};
+BOOLEAN                       mIsFirstForm = TRUE;
+FORM_ENTRY_INFO               gOldFormEntry = {0};
+
+//
+// Browser Global Strings
+//
+CHAR16            *gReconnectConfirmChanges;
+CHAR16            *gReconnectFail;
+CHAR16            *gReconnectRequired;
+CHAR16            *gChangesOpt;
+CHAR16            *gFormNotFound;
+CHAR16            *gNoSubmitIf;
+CHAR16            *gBrowserError;
+CHAR16            *gSaveFailed;
+CHAR16            *gNoSubmitIfFailed;
+CHAR16            *gSaveProcess;
+CHAR16            *gSaveNoSubmitProcess;
+CHAR16            *gDiscardChange;
+CHAR16            *gJumpToFormSet;
+CHAR16            *gCheckError;
+CHAR16            *gPromptForData;
+CHAR16            *gPromptForPassword;
+CHAR16            *gPromptForNewPassword;
+CHAR16            *gConfirmPassword;
+CHAR16            *gConfirmError;
+CHAR16            *gPassowordInvalid;
+CHAR16            *gPressEnter;
+CHAR16            *gEmptyString;
+CHAR16            *gMiniString;
+CHAR16            *gOptionMismatch;
+CHAR16            *gFormSuppress;
+CHAR16            *gProtocolNotFound;
+CHAR16            *gConfirmDefaultMsg;
+CHAR16            *gConfirmSubmitMsg;
+CHAR16            *gConfirmDiscardMsg;
+CHAR16            *gConfirmResetMsg;
+CHAR16            *gConfirmExitMsg;
+CHAR16            *gConfirmSubmitMsg2nd;
+CHAR16            *gConfirmDefaultMsg2nd;
+CHAR16            *gConfirmResetMsg2nd;
+CHAR16            *gConfirmExitMsg2nd;
+CHAR16            *gConfirmOpt;
+CHAR16            *gConfirmOptYes;
+CHAR16            *gConfirmOptNo;
+CHAR16            *gConfirmMsgConnect;
+CHAR16            *gConfirmMsgEnd;
+CHAR16            gModalSkipColumn;
+CHAR16            gPromptBlockWidth;
+CHAR16            gOptionBlockWidth;
+CHAR16            gHelpBlockWidth;
+CHAR16            *mUnknownString;
+
+FORM_DISPLAY_DRIVER_PRIVATE_DATA  mPrivateData = {
+  FORM_DISPLAY_DRIVER_SIGNATURE,
+  NULL,
+  {
+    FormDisplay,
+    DriverClearDisplayPage,
+    ConfirmDataChange
+  }
+};
+
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The package list in the HII database to search for
+                                 the specified string.
+
+  @return The output string.
+
+**/
+CHAR16 *
+GetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  )
+{
+  EFI_STRING  String;
+
+  String = HiiGetString (HiiHandle, Token, NULL);
+  if (String == NULL) {
+    String = AllocateCopyPool (StrSize (mUnknownString), mUnknownString);
+    ASSERT (String != NULL);
+  }
+
+  return (CHAR16 *) String;
+}
+
+
+/**
+  Initialize the HII String Token to the correct values.
+
+**/
+VOID
+InitializeDisplayStrings (
+  VOID
+  )
+{
+  gReconnectConfirmChanges = GetToken (STRING_TOKEN (RECONNECT_CONFIRM_CHANGES), gHiiHandle);
+  mUnknownString        = GetToken (STRING_TOKEN (UNKNOWN_STRING), gHiiHandle);
+  gSaveFailed           = GetToken (STRING_TOKEN (SAVE_FAILED), gHiiHandle);
+  gNoSubmitIfFailed     = GetToken (STRING_TOKEN (NO_SUBMIT_IF_CHECK_FAILED), gHiiHandle);
+  gReconnectFail        = GetToken (STRING_TOKEN (RECONNECT_FAILED), gHiiHandle);
+  gReconnectRequired    = GetToken (STRING_TOKEN (RECONNECT_REQUIRED), gHiiHandle);
+  gChangesOpt           = GetToken (STRING_TOKEN (RECONNECT_CHANGES_OPTIONS), gHiiHandle);
+  gSaveProcess          = GetToken (STRING_TOKEN (DISCARD_OR_JUMP), gHiiHandle);
+  gSaveNoSubmitProcess  = GetToken (STRING_TOKEN (DISCARD_OR_CHECK), gHiiHandle);
+  gDiscardChange        = GetToken (STRING_TOKEN (DISCARD_OR_JUMP_DISCARD), gHiiHandle);
+  gJumpToFormSet        = GetToken (STRING_TOKEN (DISCARD_OR_JUMP_JUMP), gHiiHandle);
+  gCheckError           = GetToken (STRING_TOKEN (DISCARD_OR_CHECK_CHECK), gHiiHandle);
+  gPromptForData        = GetToken (STRING_TOKEN (PROMPT_FOR_DATA), gHiiHandle);
+  gPromptForPassword    = GetToken (STRING_TOKEN (PROMPT_FOR_PASSWORD), gHiiHandle);
+  gPromptForNewPassword = GetToken (STRING_TOKEN (PROMPT_FOR_NEW_PASSWORD), gHiiHandle);
+  gConfirmPassword      = GetToken (STRING_TOKEN (CONFIRM_PASSWORD), gHiiHandle);
+  gConfirmError         = GetToken (STRING_TOKEN (CONFIRM_ERROR), gHiiHandle);
+  gPassowordInvalid     = GetToken (STRING_TOKEN (PASSWORD_INVALID), gHiiHandle);
+  gPressEnter           = GetToken (STRING_TOKEN (PRESS_ENTER), gHiiHandle);
+  gEmptyString          = GetToken (STRING_TOKEN (EMPTY_STRING), gHiiHandle);
+  gMiniString           = GetToken (STRING_TOKEN (MINI_STRING), gHiiHandle);
+  gOptionMismatch       = GetToken (STRING_TOKEN (OPTION_MISMATCH), gHiiHandle);
+  gFormSuppress         = GetToken (STRING_TOKEN (FORM_SUPPRESSED), gHiiHandle);
+  gProtocolNotFound     = GetToken (STRING_TOKEN (PROTOCOL_NOT_FOUND), gHiiHandle);
+  gFormNotFound         = GetToken (STRING_TOKEN (STATUS_BROWSER_FORM_NOT_FOUND), gHiiHandle);
+  gNoSubmitIf           = GetToken (STRING_TOKEN (STATUS_BROWSER_NO_SUBMIT_IF), gHiiHandle);
+  gBrowserError         = GetToken (STRING_TOKEN (STATUS_BROWSER_ERROR), gHiiHandle);
+  gConfirmDefaultMsg    = GetToken (STRING_TOKEN (CONFIRM_DEFAULT_MESSAGE), gHiiHandle);
+  gConfirmDiscardMsg    = GetToken (STRING_TOKEN (CONFIRM_DISCARD_MESSAGE), gHiiHandle);
+  gConfirmSubmitMsg     = GetToken (STRING_TOKEN (CONFIRM_SUBMIT_MESSAGE), gHiiHandle);
+  gConfirmResetMsg      = GetToken (STRING_TOKEN (CONFIRM_RESET_MESSAGE), gHiiHandle);
+  gConfirmExitMsg       = GetToken (STRING_TOKEN (CONFIRM_EXIT_MESSAGE), gHiiHandle);
+  gConfirmDefaultMsg2nd = GetToken (STRING_TOKEN (CONFIRM_DEFAULT_MESSAGE_2ND), gHiiHandle);
+  gConfirmSubmitMsg2nd  = GetToken (STRING_TOKEN (CONFIRM_SUBMIT_MESSAGE_2ND), gHiiHandle);
+  gConfirmResetMsg2nd   = GetToken (STRING_TOKEN (CONFIRM_RESET_MESSAGE_2ND), gHiiHandle);
+  gConfirmExitMsg2nd    = GetToken (STRING_TOKEN (CONFIRM_EXIT_MESSAGE_2ND), gHiiHandle);
+  gConfirmOpt           = GetToken (STRING_TOKEN (CONFIRM_OPTION), gHiiHandle);
+  gConfirmOptYes        = GetToken (STRING_TOKEN (CONFIRM_OPTION_YES), gHiiHandle);
+  gConfirmOptNo         = GetToken (STRING_TOKEN (CONFIRM_OPTION_NO), gHiiHandle);
+  gConfirmMsgConnect    = GetToken (STRING_TOKEN (CONFIRM_OPTION_CONNECT), gHiiHandle);
+  gConfirmMsgEnd        = GetToken (STRING_TOKEN (CONFIRM_OPTION_END), gHiiHandle);
+}
+
+/**
+  Free up the resource allocated for all strings required
+  by Setup Browser.
+
+**/
+VOID
+FreeDisplayStrings (
+  VOID
+  )
+{
+  FreePool (mUnknownString);
+  FreePool (gEmptyString);
+  FreePool (gSaveFailed);
+  FreePool (gNoSubmitIfFailed);
+  FreePool (gReconnectFail);
+  FreePool (gReconnectRequired);
+  FreePool (gChangesOpt);
+  FreePool (gReconnectConfirmChanges);
+  FreePool (gSaveProcess);
+  FreePool (gSaveNoSubmitProcess);
+  FreePool (gDiscardChange);
+  FreePool (gJumpToFormSet);
+  FreePool (gCheckError);
+  FreePool (gPromptForData);
+  FreePool (gPromptForPassword);
+  FreePool (gPromptForNewPassword);
+  FreePool (gConfirmPassword);
+  FreePool (gConfirmError);
+  FreePool (gPassowordInvalid);
+  FreePool (gPressEnter);
+  FreePool (gMiniString);
+  FreePool (gOptionMismatch);
+  FreePool (gFormSuppress);
+  FreePool (gProtocolNotFound);
+  FreePool (gBrowserError);
+  FreePool (gNoSubmitIf);
+  FreePool (gFormNotFound);
+  FreePool (gConfirmDefaultMsg);
+  FreePool (gConfirmSubmitMsg);
+  FreePool (gConfirmDiscardMsg);
+  FreePool (gConfirmResetMsg);
+  FreePool (gConfirmExitMsg);
+  FreePool (gConfirmDefaultMsg2nd);
+  FreePool (gConfirmSubmitMsg2nd);
+  FreePool (gConfirmResetMsg2nd);
+  FreePool (gConfirmExitMsg2nd);
+  FreePool (gConfirmOpt);
+  FreePool (gConfirmOptYes);
+  FreePool (gConfirmOptNo);
+  FreePool (gConfirmMsgConnect);
+  FreePool (gConfirmMsgEnd);
+}
+
+/**
+  Get prompt string id from the opcode data buffer.
+
+  @param  OpCode                 The input opcode buffer.
+
+  @return The prompt string id.
+
+**/
+EFI_STRING_ID
+GetPrompt (
+  IN EFI_IFR_OP_HEADER     *OpCode
+  )
+{
+  EFI_IFR_STATEMENT_HEADER  *Header;
+
+  if (OpCode->Length <= sizeof (EFI_IFR_OP_HEADER)) {
+    return 0;
+  }
+
+  Header = (EFI_IFR_STATEMENT_HEADER  *) (OpCode + 1);
+
+  return Header->Prompt;
+}
+
+/**
+  Get the supported width for a particular op-code
+
+  @param  MenuOption             The menu option.
+  @param  AdjustWidth            The width which is saved for the space.
+
+  @return Returns the number of CHAR16 characters that is support.
+
+**/
+UINT16
+GetWidth (
+  IN  UI_MENU_OPTION     *MenuOption,
+  OUT UINT16             *AdjustWidth
+  )
+{
+  CHAR16                        *String;
+  UINTN                         Size;
+  EFI_IFR_TEXT                  *TestOp;
+  UINT16                        ReturnWidth;
+  FORM_DISPLAY_ENGINE_STATEMENT *Statement;
+
+  Statement = MenuOption->ThisTag;
+
+  //
+  // For modal form, clean the entire row.
+  //
+  if ((gFormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+    if (AdjustWidth  != NULL) {
+      *AdjustWidth = LEFT_SKIPPED_COLUMNS;
+    }
+    return (UINT16)(gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn - 2 * (gModalSkipColumn + LEFT_SKIPPED_COLUMNS));
+  }
+
+  Size = 0;
+
+  //
+  // See if the second text parameter is really NULL
+  //
+  if (Statement->OpCode->OpCode == EFI_IFR_TEXT_OP) {
+    TestOp = (EFI_IFR_TEXT *) Statement->OpCode;
+    if (TestOp->TextTwo != 0) {
+      String = GetToken (TestOp->TextTwo, gFormData->HiiHandle);
+      Size   = StrLen (String);
+      FreePool (String);
+    }
+  }
+
+  if ((Statement->OpCode->OpCode == EFI_IFR_SUBTITLE_OP) ||
+      (Statement->OpCode->OpCode == EFI_IFR_REF_OP) ||
+      (Statement->OpCode->OpCode == EFI_IFR_PASSWORD_OP) ||
+      (Statement->OpCode->OpCode == EFI_IFR_ACTION_OP) ||
+      (Statement->OpCode->OpCode == EFI_IFR_RESET_BUTTON_OP) ||
+      //
+      // Allow a wide display if text op-code and no secondary text op-code
+      //
+      ((Statement->OpCode->OpCode == EFI_IFR_TEXT_OP) && (Size == 0))
+      ) {
+    
+    //
+    // Return the space width.
+    // 
+    if (AdjustWidth != NULL) {
+      *AdjustWidth = 2;
+    }
+    //
+    // Keep consistent with current behavior.
+    //
+    ReturnWidth = (UINT16) (gPromptBlockWidth + gOptionBlockWidth - 2);
+  } else {
+    if (AdjustWidth != NULL) {
+      *AdjustWidth = 1;
+    }
+
+    ReturnWidth =  (UINT16) (gPromptBlockWidth - 1);
+  }
+
+  //
+  // For nest in statement, should the subtitle indent.
+  //
+  if (MenuOption->NestInStatement) {
+    ReturnWidth -= SUBTITLE_INDENT;
+  }
+
+  return ReturnWidth;
+}
+
+/**
+  Will copy LineWidth amount of a string in the OutputString buffer and return the
+  number of CHAR16 characters that were copied into the OutputString buffer.
+  The output string format is:
+    Glyph Info + String info + '\0'.
+
+  In the code, it deals \r,\n,\r\n same as \n\r, also it not process the \r or \g.
+
+  @param  InputString            String description for this option.
+  @param  LineWidth              Width of the desired string to extract in CHAR16
+                                 characters
+  @param  GlyphWidth             The glyph width of the begin of the char in the string.
+  @param  Index                  Where in InputString to start the copy process
+  @param  OutputString           Buffer to copy the string into
+
+  @return Returns the number of CHAR16 characters that were copied into the OutputString 
+  buffer, include extra glyph info and '\0' info.
+
+**/
+UINT16
+GetLineByWidth (
+  IN      CHAR16                      *InputString,
+  IN      UINT16                      LineWidth,
+  IN OUT  UINT16                      *GlyphWidth,
+  IN OUT  UINTN                       *Index,
+  OUT     CHAR16                      **OutputString
+  )
+{
+  UINT16          StrOffset;
+  UINT16          GlyphOffset;
+  UINT16          OriginalGlyphWidth;
+  BOOLEAN         ReturnFlag;
+  UINT16          LastSpaceOffset;
+  UINT16          LastGlyphWidth;
+
+  if (InputString == NULL || Index == NULL || OutputString == NULL) {
+    return 0;
+  }
+
+  if (LineWidth == 0 || *GlyphWidth == 0) {
+    return 0;
+  }
+
+  //
+  // Save original glyph width.
+  //
+  OriginalGlyphWidth = *GlyphWidth;
+  LastGlyphWidth     = OriginalGlyphWidth;
+  ReturnFlag         = FALSE;
+  LastSpaceOffset    = 0;
+
+  //
+  // NARROW_CHAR can not be printed in screen, so if a line only contain  the two CHARs: 'NARROW_CHAR + CHAR_CARRIAGE_RETURN' , it is a empty line  in Screen.
+  // To avoid displaying this  empty line in screen,  just skip  the two CHARs here.
+  //
+  if ((InputString[*Index] == NARROW_CHAR) && (InputString[*Index + 1] == CHAR_CARRIAGE_RETURN)) {
+    *Index = *Index + 2;
+  }
+
+  //
+  // Fast-forward the string and see if there is a carriage-return in the string
+  //
+  for (StrOffset = 0, GlyphOffset = 0; GlyphOffset <= LineWidth; StrOffset++) {
+    switch (InputString[*Index + StrOffset]) {
+      case NARROW_CHAR:
+        *GlyphWidth = 1;
+        break;
+
+      case WIDE_CHAR:
+        *GlyphWidth = 2;
+        break;
+
+      case CHAR_CARRIAGE_RETURN:
+      case CHAR_LINEFEED:
+      case CHAR_NULL:
+        ReturnFlag = TRUE;
+        break;
+
+      default:
+        GlyphOffset = GlyphOffset + *GlyphWidth;
+
+        //
+        // Record the last space info in this line. Will be used in rewind.
+        //
+        if ((InputString[*Index + StrOffset] == CHAR_SPACE) && (GlyphOffset <= LineWidth)) {
+          LastSpaceOffset = StrOffset;
+          LastGlyphWidth  = *GlyphWidth;
+        }
+        break;
+    }
+
+    if (ReturnFlag) {
+      break;
+    }
+  } 
+
+  //
+  // Rewind the string from the maximum size until we see a space to break the line
+  //
+  if (GlyphOffset > LineWidth) {
+    //
+    // Rewind the string to last space char in this line.
+    //
+    if (LastSpaceOffset != 0) {
+      StrOffset   = LastSpaceOffset;
+      *GlyphWidth = LastGlyphWidth;
+    } else {
+      //
+      // Roll back to last char in the line width.
+      //
+      StrOffset--;
+    }
+  }
+
+  //
+  // The CHAR_NULL has process last time, this time just return 0 to stand for the end.
+  //
+  if (StrOffset == 0 && (InputString[*Index + StrOffset] == CHAR_NULL)) {
+    return 0;
+  }
+
+  //
+  // Need extra glyph info and '\0' info, so +2.
+  //
+  *OutputString = AllocateZeroPool (((UINTN) (StrOffset + 2) * sizeof(CHAR16)));
+  if (*OutputString == NULL) {
+    return 0;
+  }
+
+  //
+  // Save the glyph info at the begin of the string, will used by Print function.
+  //
+  if (OriginalGlyphWidth == 1) {
+    *(*OutputString) = NARROW_CHAR;
+  } else  {
+    *(*OutputString) = WIDE_CHAR;
+  }
+
+  CopyMem ((*OutputString) + 1, &InputString[*Index], StrOffset * sizeof(CHAR16));
+
+  if (InputString[*Index + StrOffset] == CHAR_SPACE) {
+    //
+    // Skip the space info at the begin of next line.
+    //  
+    *Index = (UINT16) (*Index + StrOffset + 1);
+  } else if (InputString[*Index + StrOffset] == CHAR_LINEFEED) {
+    //
+    // Skip the /n or /n/r info.
+    //
+    if (InputString[*Index + StrOffset + 1] == CHAR_CARRIAGE_RETURN) {
+      *Index = (UINT16) (*Index + StrOffset + 2);
+    } else {
+      *Index = (UINT16) (*Index + StrOffset + 1);
+    }
+  } else if (InputString[*Index + StrOffset] == CHAR_CARRIAGE_RETURN) {
+    //
+    // Skip the /r or /r/n info.
+    //  
+    if (InputString[*Index + StrOffset + 1] == CHAR_LINEFEED) {
+      *Index = (UINT16) (*Index + StrOffset + 2);
+    } else {
+      *Index = (UINT16) (*Index + StrOffset + 1);
+    }
+  } else {
+    *Index = (UINT16) (*Index + StrOffset);
+  }
+
+  //
+  // Include extra glyph info and '\0' info, so +2.
+  //
+  return StrOffset + 2;
+}
+
+/**
+  Add one menu option by specified description and context.
+
+  @param  Statement              Statement of this Menu Option.
+  @param  MenuItemCount          The index for this Option in the Menu.
+  @param  NestIn                 Whether this statement is nest in another statement.
+
+**/
+VOID
+UiAddMenuOption (
+  IN FORM_DISPLAY_ENGINE_STATEMENT   *Statement,
+  IN UINT16                          *MenuItemCount,
+  IN BOOLEAN                         NestIn
+  )
+{
+  UI_MENU_OPTION   *MenuOption;
+  UINTN            Index;
+  UINTN            Count;
+  CHAR16           *String;
+  UINT16           NumberOfLines;
+  UINT16           GlyphWidth;
+  UINT16           Width;
+  UINTN            ArrayEntry;
+  CHAR16           *OutputString;
+  EFI_STRING_ID    PromptId;
+
+  NumberOfLines = 1;
+  ArrayEntry    = 0;
+  GlyphWidth    = 1;
+  Count         = 1;
+  MenuOption    = NULL;
+
+  PromptId = GetPrompt (Statement->OpCode);
+  ASSERT (PromptId != 0);
+
+  String = GetToken (PromptId, gFormData->HiiHandle);
+  ASSERT (String != NULL);
+
+  if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP || Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+    Count = 3;
+  }
+
+  for (Index = 0; Index < Count; Index++) {
+    MenuOption = AllocateZeroPool (sizeof (UI_MENU_OPTION));
+    ASSERT (MenuOption);
+
+    MenuOption->Signature   = UI_MENU_OPTION_SIGNATURE;
+    MenuOption->Description = String;
+    MenuOption->Handle      = gFormData->HiiHandle;
+    MenuOption->ThisTag     = Statement;
+    MenuOption->NestInStatement = NestIn;
+    MenuOption->EntryNumber = *MenuItemCount;
+
+    MenuOption->Sequence = Index;
+
+    if ((Statement->Attribute & HII_DISPLAY_GRAYOUT) != 0) {
+      MenuOption->GrayOut = TRUE;
+    } else {
+      MenuOption->GrayOut = FALSE;
+    }
+
+    if ((Statement->Attribute & HII_DISPLAY_LOCK) != 0 || (gFormData->Attribute & HII_DISPLAY_LOCK) != 0) {
+      MenuOption->GrayOut = TRUE;
+    }
+
+    //
+    // If the form or the question has the lock attribute, deal same as grayout.
+    //
+    if ((gFormData->Attribute & HII_DISPLAY_LOCK) != 0 || (Statement->Attribute & HII_DISPLAY_LOCK) != 0) {
+      MenuOption->GrayOut = TRUE;
+    }
+
+    switch (Statement->OpCode->OpCode) {
+    case EFI_IFR_ORDERED_LIST_OP:
+    case EFI_IFR_ONE_OF_OP:
+    case EFI_IFR_NUMERIC_OP:
+    case EFI_IFR_TIME_OP:
+    case EFI_IFR_DATE_OP:
+    case EFI_IFR_CHECKBOX_OP:
+    case EFI_IFR_PASSWORD_OP:
+    case EFI_IFR_STRING_OP:
+      //
+      // User could change the value of these items
+      //
+      MenuOption->IsQuestion = TRUE;
+      break;
+    case EFI_IFR_TEXT_OP:
+      if (FeaturePcdGet (PcdBrowserGrayOutTextStatement)) {
+        //
+        // Initializing GrayOut option as TRUE for Text setup options 
+        // so that those options will be Gray in colour and un selectable.
+        //
+        MenuOption->GrayOut = TRUE;
+      }
+      break;
+    default:
+      MenuOption->IsQuestion = FALSE;
+      break;
+    }
+
+    if ((Statement->Attribute & HII_DISPLAY_READONLY) != 0) {
+      MenuOption->ReadOnly = TRUE;
+      if (FeaturePcdGet (PcdBrowerGrayOutReadOnlyMenu)) {
+        MenuOption->GrayOut = TRUE;
+      }
+    }
+
+    if (Index == 0 && 
+      (Statement->OpCode->OpCode != EFI_IFR_DATE_OP) && 
+      (Statement->OpCode->OpCode != EFI_IFR_TIME_OP)) {
+      Width  = GetWidth (MenuOption, NULL);
+      for (; GetLineByWidth (String, Width, &GlyphWidth,&ArrayEntry, &OutputString) != 0x0000;) {
+        //
+        // If there is more string to process print on the next row and increment the Skip value
+        //
+        if (StrLen (&String[ArrayEntry]) != 0) {
+          NumberOfLines++;
+        }
+        FreePool (OutputString);
+      }
+    } else {
+      //
+      // Add three MenuOptions for Date/Time
+      // Data format :      [01/02/2004]      [11:22:33]
+      // Line number :        0  0    1         0  0  1
+      //    
+      NumberOfLines = 0;
+    }
+
+    if (Index == 2) {
+      //
+      // Override LineNumber for the MenuOption in Date/Time sequence
+      //
+      MenuOption->Skip = 1;
+    } else {
+      MenuOption->Skip = NumberOfLines;
+    }
+
+    InsertTailList (&gMenuOption, &MenuOption->Link);
+  }
+
+  (*MenuItemCount)++;
+}
+
+/**
+  Create the menu list base on the form data info.
+
+**/
+VOID
+ConvertStatementToMenu (
+  VOID
+  )
+{
+  UINT16                        MenuItemCount;
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *NestLink;
+  FORM_DISPLAY_ENGINE_STATEMENT *Statement;
+  FORM_DISPLAY_ENGINE_STATEMENT *NestStatement;
+
+  MenuItemCount = 0;
+  InitializeListHead (&gMenuOption);
+
+  Link = GetFirstNode (&gFormData->StatementListHead);
+  while (!IsNull (&gFormData->StatementListHead, Link)) {
+    Statement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&gFormData->StatementListHead, Link);
+
+    //
+    // Skip the opcode not recognized by Display core.
+    //
+    if (Statement->OpCode->OpCode == EFI_IFR_GUID_OP) {
+      continue;
+    }
+
+    UiAddMenuOption (Statement, &MenuItemCount, FALSE);
+
+    //
+    // Check the statement nest in this host statement.
+    //
+    NestLink = GetFirstNode (&Statement->NestStatementList);
+    while (!IsNull (&Statement->NestStatementList, NestLink)) {
+      NestStatement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (NestLink);
+      NestLink = GetNextNode (&Statement->NestStatementList, NestLink);
+
+      //
+      // Skip the opcode not recognized by Display core.
+      //
+      if (NestStatement->OpCode->OpCode == EFI_IFR_GUID_OP) {
+        continue;
+      }
+
+      UiAddMenuOption (NestStatement, &MenuItemCount, TRUE);
+    }
+  }
+}
+
+/**
+  Count the storage space of a Unicode string.
+
+  This function handles the Unicode string with NARROW_CHAR
+  and WIDE_CHAR control characters. NARROW_HCAR and WIDE_CHAR
+  does not count in the resultant output. If a WIDE_CHAR is
+  hit, then 2 Unicode character will consume an output storage
+  space with size of CHAR16 till a NARROW_CHAR is hit.
+
+  If String is NULL, then ASSERT ().
+
+  @param String          The input string to be counted.
+
+  @return Storage space for the input string.
+
+**/
+UINTN
+GetStringWidth (
+  IN CHAR16               *String
+  )
+{
+  UINTN Index;
+  UINTN Count;
+  UINTN IncrementValue;
+
+  ASSERT (String != NULL);
+  if (String == NULL) {
+    return 0;
+  }
+
+  Index           = 0;
+  Count           = 0;
+  IncrementValue  = 1;
+
+  do {
+    //
+    // Advance to the null-terminator or to the first width directive
+    //
+    for (;
+         (String[Index] != NARROW_CHAR) && (String[Index] != WIDE_CHAR) && (String[Index] != 0);
+         Index++, Count = Count + IncrementValue
+        )
+      ;
+
+    //
+    // We hit the null-terminator, we now have a count
+    //
+    if (String[Index] == 0) {
+      break;
+    }
+    //
+    // We encountered a narrow directive - strip it from the size calculation since it doesn't get printed
+    // and also set the flag that determines what we increment by.(if narrow, increment by 1, if wide increment by 2)
+    //
+    if (String[Index] == NARROW_CHAR) {
+      //
+      // Skip to the next character
+      //
+      Index++;
+      IncrementValue = 1;
+    } else {
+      //
+      // Skip to the next character
+      //
+      Index++;
+      IncrementValue = 2;
+    }
+  } while (String[Index] != 0);
+
+  //
+  // Increment by one to include the null-terminator in the size
+  //
+  Count++;
+
+  return Count * sizeof (CHAR16);
+}
+
+/**
+  Base on the input option string to update the skip value for a menu option.
+
+  @param  MenuOption             The MenuOption to be checked.
+  @param  OptionString           The input option string.
+
+**/
+VOID
+UpdateSkipInfoForMenu (
+  IN UI_MENU_OPTION               *MenuOption,
+  IN CHAR16                       *OptionString
+  )
+{
+  UINTN   Index;
+  UINT16  Width;
+  UINTN   Row;
+  CHAR16  *OutputString;
+  UINT16  GlyphWidth;
+
+  Width         = (UINT16) gOptionBlockWidth;
+  GlyphWidth    = 1;
+  Row           = 1;
+
+  for (Index = 0; GetLineByWidth (OptionString, Width, &GlyphWidth, &Index, &OutputString) != 0x0000;) {
+    if (StrLen (&OptionString[Index]) != 0) {
+      Row++;
+    }
+
+    FreePool (OutputString);
+  }
+
+  if ((Row > MenuOption->Skip) && 
+      (MenuOption->ThisTag->OpCode->OpCode != EFI_IFR_DATE_OP) && 
+      (MenuOption->ThisTag->OpCode->OpCode != EFI_IFR_TIME_OP)) {
+    MenuOption->Skip = Row;
+  }
+}
+
+/**
+  Update display lines for a Menu Option.
+
+  @param  MenuOption             The MenuOption to be checked.
+
+**/
+VOID
+UpdateOptionSkipLines (
+  IN UI_MENU_OPTION               *MenuOption
+  )
+{
+  CHAR16  *OptionString;
+
+  OptionString  = NULL;
+
+  ProcessOptions (MenuOption, FALSE, &OptionString, TRUE);
+  if (OptionString != NULL) {
+    UpdateSkipInfoForMenu (MenuOption, OptionString);
+
+    FreePool (OptionString);
+  }
+
+  if ((MenuOption->ThisTag->OpCode->OpCode  == EFI_IFR_TEXT_OP) && (((EFI_IFR_TEXT*)MenuOption->ThisTag->OpCode)->TextTwo != 0)) {
+    OptionString   = GetToken (((EFI_IFR_TEXT*)MenuOption->ThisTag->OpCode)->TextTwo, gFormData->HiiHandle);
+
+    if (OptionString != NULL) {
+      UpdateSkipInfoForMenu (MenuOption, OptionString);
+
+      FreePool (OptionString);
+    }
+  }
+}
+
+/**
+  Check whether this Menu Option could be print.
+
+  Check Prompt string, option string or text two string not NULL.
+
+  This is an internal function.
+
+  @param  MenuOption             The MenuOption to be checked.
+
+  @retval TRUE                   This Menu Option is printable.
+  @retval FALSE                  This Menu Option could not be printable.
+
+**/
+BOOLEAN
+PrintableMenu (
+  UI_MENU_OPTION   *MenuOption
+  )
+{
+  EFI_STATUS    Status;
+  EFI_STRING    OptionString;
+
+  OptionString = NULL;
+
+  if (MenuOption->Description[0] != '\0') {
+    return TRUE;
+  }
+
+  Status = ProcessOptions (MenuOption, FALSE, &OptionString, FALSE);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  if (OptionString != NULL && OptionString[0] != '\0') {
+    FreePool (OptionString);
+    return TRUE;
+  }
+
+  if ((MenuOption->ThisTag->OpCode->OpCode  == EFI_IFR_TEXT_OP) && (((EFI_IFR_TEXT*)MenuOption->ThisTag->OpCode)->TextTwo != 0)) {
+    OptionString   = GetToken (((EFI_IFR_TEXT*)MenuOption->ThisTag->OpCode)->TextTwo, gFormData->HiiHandle);
+    ASSERT (OptionString != NULL);
+    if (OptionString[0] != '\0'){
+      FreePool (OptionString);
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether this Menu Option could be highlighted.
+
+  This is an internal function.
+
+  @param  MenuOption             The MenuOption to be checked.
+
+  @retval TRUE                   This Menu Option is selectable.
+  @retval FALSE                  This Menu Option could not be selected.
+
+**/
+BOOLEAN
+IsSelectable (
+  UI_MENU_OPTION   *MenuOption
+  )
+{
+  if ((MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_SUBTITLE_OP) ||
+      MenuOption->GrayOut || MenuOption->ReadOnly || !PrintableMenu (MenuOption)) {
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/**
+  Move to next selectable statement.
+
+  This is an internal function.
+
+  @param  GoUp                   The navigation direction. TRUE: up, FALSE: down.
+  @param  CurrentPosition        Current position.
+  @param  GapToTop               Gap position to top or bottom.
+  @param  FindInForm             Whether find menu in current form or beyond.
+
+  @return The row distance from current MenuOption to next selectable MenuOption.
+
+  @retval -1       Reach the begin of the menu, still can't find the selectable menu.
+  @retval Value    Find the selectable menu, maybe the truly selectable, maybe the 
+                   first menu showing beyond current form or last menu showing in 
+                   current form.
+                   The value is the line number between the new selected menu and the
+                   current select menu, not include the new selected menu.
+
+**/
+INTN
+MoveToNextStatement (
+  IN     BOOLEAN                   GoUp,
+  IN OUT LIST_ENTRY                **CurrentPosition,
+  IN     UINTN                     GapToTop,
+  IN     BOOLEAN                   FindInForm
+  )
+{
+  INTN             Distance;
+  LIST_ENTRY       *Pos;
+  UI_MENU_OPTION   *NextMenuOption;
+  UI_MENU_OPTION   *PreMenuOption;
+
+  Distance      = 0;
+  Pos           = *CurrentPosition;
+
+  if (Pos == &gMenuOption) {
+    return -1;
+  }
+
+  PreMenuOption = MENU_OPTION_FROM_LINK (Pos);
+
+  while (TRUE) {
+    NextMenuOption = MENU_OPTION_FROM_LINK (Pos);
+    //
+    // NextMenuOption->Row == 0 means this menu has not calculate
+    // the NextMenuOption->Skip value yet, just calculate here.
+    //
+    if (NextMenuOption->Row == 0) {
+      UpdateOptionSkipLines (NextMenuOption);
+    }
+
+    if (IsSelectable (NextMenuOption)) {
+      break;
+    }
+
+    //
+    // In this case, still can't find the selectable menu,
+    // return the first one beyond the showing form.
+    //
+    if ((UINTN) Distance + NextMenuOption->Skip > GapToTop) {
+      if (FindInForm) {
+        NextMenuOption = PreMenuOption;
+      }
+      break;
+    }
+
+    Distance += NextMenuOption->Skip;
+
+    //
+    // Arrive at begin of the menu list.
+    //
+    if ((GoUp ? Pos->BackLink : Pos->ForwardLink) == &gMenuOption) {
+      Distance = -1;
+      break;
+    }
+
+    Pos = (GoUp ? Pos->BackLink : Pos->ForwardLink);
+    PreMenuOption = NextMenuOption;
+  }
+
+  *CurrentPosition = &NextMenuOption->Link;
+  return Distance;
+}
+
+
+/**
+  Process option string for date/time opcode.
+
+  @param  MenuOption              Menu option point to date/time.
+  @param  OptionString            Option string input for process.
+  @param  AddOptCol               Whether need to update MenuOption->OptCol. 
+
+**/
+VOID 
+ProcessStringForDateTime (
+  UI_MENU_OPTION                  *MenuOption,
+  CHAR16                          *OptionString,
+  BOOLEAN                         AddOptCol
+  )
+{
+  UINTN Index;
+  UINTN Count;
+  FORM_DISPLAY_ENGINE_STATEMENT          *Statement;
+  EFI_IFR_DATE                           *Date;
+  EFI_IFR_TIME                           *Time;
+
+  ASSERT (MenuOption != NULL && OptionString != NULL);
+  
+  Statement = MenuOption->ThisTag;
+  Date      = NULL;
+  Time      = NULL;
+  if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP) {
+    Date = (EFI_IFR_DATE *) Statement->OpCode;
+  } else if (Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+    Time = (EFI_IFR_TIME *) Statement->OpCode;
+  }
+  
+  //
+  // If leading spaces on OptionString - remove the spaces
+  //
+  for (Index = 0; OptionString[Index] == L' '; Index++) {
+    //
+    // Base on the blockspace to get the option column info.
+    //
+    if (AddOptCol) {
+      MenuOption->OptCol++;
+    }
+  }
+  
+  for (Count = 0; OptionString[Index] != CHAR_NULL; Index++) {
+    OptionString[Count] = OptionString[Index];
+    Count++;
+  }
+  OptionString[Count] = CHAR_NULL;
+  
+  //
+  // Enable to suppress field in the opcode base on the flag.
+  //
+  if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP) {
+    //
+    // OptionString format is: <**:  **: ****>
+    //                        |month|day|year|
+    //                          4     3    5
+    //
+    if ((Date->Flags & EFI_QF_DATE_MONTH_SUPPRESS) && (MenuOption->Sequence == 0)) {
+      //
+      // At this point, only "<**:" in the optionstring. 
+      // Clean the day's ** field, after clean, the format is "<  :"
+      //
+      SetUnicodeMem (&OptionString[1], 2, L' ');
+    } else if ((Date->Flags & EFI_QF_DATE_DAY_SUPPRESS) && (MenuOption->Sequence == 1)) {
+      //
+      // At this point, only "**:" in the optionstring. 
+      // Clean the month's "**" field, after clean, the format is "  :"
+      //                
+      SetUnicodeMem (&OptionString[0], 2, L' ');
+    } else if ((Date->Flags & EFI_QF_DATE_YEAR_SUPPRESS) && (MenuOption->Sequence == 2)) {
+      //
+      // At this point, only "****>" in the optionstring. 
+      // Clean the year's "****" field, after clean, the format is "  >"
+      //                
+      SetUnicodeMem (&OptionString[0], 4, L' ');
+    }
+  } else if (Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+    //
+    // OptionString format is: <**:  **:    **>
+    //                        |hour|minute|second|
+    //                          4     3      3
+    //
+    if ((Time->Flags & QF_TIME_HOUR_SUPPRESS) && (MenuOption->Sequence == 0)) {
+      //
+      // At this point, only "<**:" in the optionstring. 
+      // Clean the hour's ** field, after clean, the format is "<  :"
+      //
+      SetUnicodeMem (&OptionString[1], 2, L' ');
+    } else if ((Time->Flags & QF_TIME_MINUTE_SUPPRESS) && (MenuOption->Sequence == 1)) {
+      //
+      // At this point, only "**:" in the optionstring. 
+      // Clean the minute's "**" field, after clean, the format is "  :"
+      //                
+      SetUnicodeMem (&OptionString[0], 2, L' ');
+    } else if ((Time->Flags & QF_TIME_SECOND_SUPPRESS) && (MenuOption->Sequence == 2)) {
+      //
+      // At this point, only "**>" in the optionstring. 
+      // Clean the second's "**" field, after clean, the format is "  >"
+      //                
+      SetUnicodeMem (&OptionString[0], 2, L' ');
+    }
+  }
+}
+
+
+/**
+  Adjust Data and Time position accordingly.
+  Data format :      [01/02/2004]      [11:22:33]
+  Line number :        0  0    1         0  0  1
+
+  This is an internal function.
+
+  @param  DirectionUp            the up or down direction. False is down. True is
+                                 up.
+  @param  CurrentPosition        Current position. On return: Point to the last
+                                 Option (Year or Second) if up; Point to the first
+                                 Option (Month or Hour) if down.
+
+  @return Return line number to pad. It is possible that we stand on a zero-advance
+  @return data or time opcode, so pad one line when we judge if we are going to scroll outside.
+
+**/
+UINTN
+AdjustDateAndTimePosition (
+  IN     BOOLEAN                     DirectionUp,
+  IN OUT LIST_ENTRY                  **CurrentPosition
+  )
+{
+  UINTN           Count;
+  LIST_ENTRY      *NewPosition;
+  UI_MENU_OPTION  *MenuOption;
+  UINTN           PadLineNumber;
+
+  PadLineNumber = 0;
+  NewPosition   = *CurrentPosition;
+  MenuOption    = MENU_OPTION_FROM_LINK (NewPosition);
+
+  if ((MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_DATE_OP) ||
+      (MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+    //
+    // Calculate the distance from current position to the last Date/Time MenuOption
+    //
+    Count = 0;
+    while (MenuOption->Skip == 0) {
+      Count++;
+      NewPosition   = NewPosition->ForwardLink;
+      MenuOption    = MENU_OPTION_FROM_LINK (NewPosition);
+      PadLineNumber = 1;
+    }
+
+    NewPosition = *CurrentPosition;
+    if (DirectionUp) {
+      //
+      // Since the behavior of hitting the up arrow on a Date/Time MenuOption is intended
+      // to be one that back to the previous set of MenuOptions, we need to advance to the first
+      // Date/Time MenuOption and leave the remaining logic in CfUiUp intact so the appropriate
+      // checking can be done.
+      //
+      while (Count++ < 2) {
+        NewPosition = NewPosition->BackLink;
+      }
+    } else {
+      //
+      // Since the behavior of hitting the down arrow on a Date/Time MenuOption is intended
+      // to be one that progresses to the next set of MenuOptions, we need to advance to the last
+      // Date/Time MenuOption and leave the remaining logic in CfUiDown intact so the appropriate
+      // checking can be done.
+      //
+      while (Count-- > 0) {
+        NewPosition = NewPosition->ForwardLink;
+      }
+    }
+
+    *CurrentPosition = NewPosition;
+  }
+
+  return PadLineNumber;
+}
+
+/**
+  Get step info from numeric opcode.
+  
+  @param[in] OpCode     The input numeric op code.
+
+  @return step info for this opcode.
+**/
+UINT64
+GetFieldFromNum (
+  IN  EFI_IFR_OP_HEADER     *OpCode
+  )
+{
+  EFI_IFR_NUMERIC       *NumericOp;
+  UINT64                Step;
+
+  NumericOp = (EFI_IFR_NUMERIC *) OpCode;
+  
+  switch (NumericOp->Flags & EFI_IFR_NUMERIC_SIZE) {
+  case EFI_IFR_NUMERIC_SIZE_1:
+    Step    = NumericOp->data.u8.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_2:
+    Step    = NumericOp->data.u16.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_4:
+    Step    = NumericOp->data.u32.Step;
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_8:
+    Step    = NumericOp->data.u64.Step;
+    break;
+  
+  default:
+    Step = 0;
+    break;
+  }
+
+  return Step;
+}
+
+/**
+  Find the registered HotKey based on KeyData.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key.
+
+  @return The registered HotKey context. If no found, NULL will return.
+**/
+BROWSER_HOT_KEY *
+GetHotKeyFromRegisterList (
+  IN EFI_INPUT_KEY *KeyData
+  )
+{
+  LIST_ENTRY       *Link;
+  BROWSER_HOT_KEY  *HotKey;
+
+  Link = GetFirstNode (&gFormData->HotKeyListHead);
+  while (!IsNull (&gFormData->HotKeyListHead, Link)) {
+    HotKey = BROWSER_HOT_KEY_FROM_LINK (Link);
+    
+    if (HotKey->KeyData->ScanCode == KeyData->ScanCode) {
+      return HotKey;
+    }
+
+    Link = GetNextNode (&gFormData->HotKeyListHead, Link);
+  }
+  
+  return NULL;
+}
+
+
+/**
+  Determine if the menu is the last menu that can be selected.
+
+  This is an internal function.
+
+  @param  Direction              The scroll direction. False is down. True is up.
+  @param  CurrentPos             The current focus.
+
+  @return FALSE -- the menu isn't the last menu that can be selected.
+  @return TRUE  -- the menu is the last menu that can be selected.
+
+**/
+BOOLEAN
+ValueIsScroll (
+  IN  BOOLEAN                     Direction,
+  IN  LIST_ENTRY                  *CurrentPos
+  )
+{
+  LIST_ENTRY      *Temp;
+
+  Temp = Direction ? CurrentPos->BackLink : CurrentPos->ForwardLink;
+
+  if (Temp == &gMenuOption) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Wait for a given event to fire, or for an optional timeout to expire.
+
+  @param  Event                  The event to wait for
+
+  @retval UI_EVENT_TYPE          The type of the event which is trigged.
+
+**/
+UI_EVENT_TYPE
+UiWaitForEvent (
+  IN EFI_EVENT                Event
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  UINTN       EventNum;
+  UINT64      Timeout;
+  EFI_EVENT   TimerEvent;
+  EFI_EVENT   WaitList[3];
+  UI_EVENT_TYPE  EventType;
+
+  TimerEvent = NULL;
+  Timeout    = FormExitTimeout(gFormData);
+
+  if (Timeout != 0) {
+    Status = gBS->CreateEvent (EVT_TIMER, 0, NULL, NULL, &TimerEvent);
+
+    //
+    // Set the timer event
+    //
+    gBS->SetTimer (
+          TimerEvent,
+          TimerRelative,
+          Timeout
+          );
+  }
+  
+  WaitList[0] = Event;
+  EventNum    = 1;
+  if (gFormData->FormRefreshEvent != NULL) {
+    WaitList[EventNum] = gFormData->FormRefreshEvent;
+    EventNum ++;
+  } 
+
+  if (Timeout != 0) {
+    WaitList[EventNum] = TimerEvent;
+    EventNum ++;
+  }
+
+  Status = gBS->WaitForEvent (EventNum, WaitList, &Index);
+  ASSERT_EFI_ERROR (Status);
+
+  switch (Index) {
+  case 0:
+   EventType = UIEventKey;
+   break;
+
+  case 1:
+    if (gFormData->FormRefreshEvent != NULL) {
+      EventType = UIEventDriver;
+    } else {
+      ASSERT (Timeout != 0 && EventNum == 2);
+      EventType = UIEventTimeOut;
+    }
+    break;
+
+  default:
+    ASSERT (Index == 2 && EventNum == 3);
+    EventType = UIEventTimeOut;
+    break;
+  }
+
+  if (Timeout != 0) {
+    gBS->CloseEvent (TimerEvent);
+  }
+  
+  return EventType;
+}
+
+/**
+  Get question id info from the input opcode header.
+
+  @param  OpCode                 The input opcode header pointer.
+
+  @retval                        The question id for this opcode.
+
+**/
+EFI_QUESTION_ID
+GetQuestionIdInfo (
+  IN   EFI_IFR_OP_HEADER     *OpCode
+  )
+{
+  EFI_IFR_QUESTION_HEADER   *QuestionHeader;
+
+  if (OpCode->Length < sizeof (EFI_IFR_OP_HEADER) + sizeof (EFI_IFR_QUESTION_HEADER)) {
+    return 0;
+  }
+
+  QuestionHeader = (EFI_IFR_QUESTION_HEADER *)((UINT8 *) OpCode + sizeof(EFI_IFR_OP_HEADER));
+
+  return QuestionHeader->QuestionId;
+}
+
+
+/**
+  Find the top of screen menu base on the current menu.
+
+  @param  CurPos                 Current input menu.
+  @param  Rows                   Totol screen rows.
+  @param  SkipValue              SkipValue for this new form.
+
+  @retval TopOfScreen            Top of screen menu for the new form.
+
+**/
+LIST_ENTRY *
+FindTopOfScreenMenu (
+  IN  LIST_ENTRY                      *CurPos,
+  IN  UINTN                           Rows,
+  OUT UINTN                           *SkipValue
+  )
+{
+  LIST_ENTRY        *Link;
+  LIST_ENTRY        *TopOfScreen;
+  UI_MENU_OPTION    *PreviousMenuOption;
+
+  Link = CurPos;
+  PreviousMenuOption = NULL;
+
+  while (Link->BackLink != &gMenuOption) {
+    Link = Link->BackLink;
+    PreviousMenuOption = MENU_OPTION_FROM_LINK (Link);
+    if (PreviousMenuOption->Row == 0) {
+      UpdateOptionSkipLines (PreviousMenuOption);
+    }
+    if (Rows <= PreviousMenuOption->Skip) {
+      break;
+    }
+    Rows = Rows - PreviousMenuOption->Skip;
+  }
+
+  if (Link->BackLink == &gMenuOption) {
+    TopOfScreen = gMenuOption.ForwardLink;
+    if (PreviousMenuOption != NULL && Rows < PreviousMenuOption->Skip) {
+      *SkipValue = PreviousMenuOption->Skip - Rows;
+    } else {
+      *SkipValue = 0;
+    }
+  } else {
+    TopOfScreen = Link;
+    *SkipValue = PreviousMenuOption->Skip - Rows;
+  }
+
+  return TopOfScreen;
+}
+
+/**
+  Get the index info for this opcode.
+
+  @param  OpCode      The input opcode for the statement.
+
+  @retval  The index of this statement.
+
+**/
+UINTN
+GetIndexInfoForOpcode (
+  IN EFI_IFR_OP_HEADER  *OpCode
+  )
+{
+  LIST_ENTRY                      *NewPos;
+  UI_MENU_OPTION                  *MenuOption;
+  UINTN                           Index;
+
+  NewPos = gMenuOption.ForwardLink;
+  Index  = 0;
+
+  for (NewPos = gMenuOption.ForwardLink; NewPos != &gMenuOption; NewPos = NewPos->ForwardLink){
+    MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+
+    if (CompareMem (MenuOption->ThisTag->OpCode, OpCode, OpCode->Length) == 0) {
+      if (MenuOption->ThisTag->OpCode == OpCode) {
+        return Index;
+      }
+
+      Index ++;
+    }
+  }
+
+  return Index;
+}
+
+/**
+  Is this the saved highlight statement.
+
+  @param  HighLightedStatement      The input highlight statement.
+
+  @retval  TRUE   This is the highlight statement.
+  @retval  FALSE  This is not the highlight statement.
+
+**/
+BOOLEAN 
+IsSavedHighlightStatement (
+  IN FORM_DISPLAY_ENGINE_STATEMENT  *HighLightedStatement
+  )
+{
+  if ((gFormData->HiiHandle == gHighligthMenuInfo.HiiHandle) &&
+      (gFormData->FormId == gHighligthMenuInfo.FormId)) {
+    if (gHighligthMenuInfo.HLTQuestionId != 0) {
+      return (BOOLEAN) (gHighligthMenuInfo.HLTQuestionId == GetQuestionIdInfo (HighLightedStatement->OpCode));
+    } else {
+      if (CompareMem (gHighligthMenuInfo.HLTOpCode, HighLightedStatement->OpCode, gHighligthMenuInfo.HLTOpCode->Length) == 0) {
+        if (gHighligthMenuInfo.HLTIndex == 0 || gHighligthMenuInfo.HLTIndex == GetIndexInfoForOpcode(HighLightedStatement->OpCode)) {
+          return TRUE;
+        } else {
+          return FALSE;
+        }
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Is this the highlight menu.
+
+  @param  MenuOption      The input Menu option.
+
+  @retval  TRUE   This is the highlight menu option.
+  @retval  FALSE  This is not the highlight menu option.
+
+**/
+BOOLEAN
+IsHighLightMenuOption (
+  IN UI_MENU_OPTION     *MenuOption
+  )
+{
+  if (gHighligthMenuInfo.HLTQuestionId != 0) {
+    if (GetQuestionIdInfo(MenuOption->ThisTag->OpCode) == gHighligthMenuInfo.HLTQuestionId) {
+      return (BOOLEAN) (MenuOption->Sequence == gHighligthMenuInfo.HLTSequence);
+    }
+  } else {
+    if(CompareMem (gHighligthMenuInfo.HLTOpCode, MenuOption->ThisTag->OpCode, gHighligthMenuInfo.HLTOpCode->Length) == 0) {
+      if (gHighligthMenuInfo.HLTIndex == 0 || gHighligthMenuInfo.HLTIndex == GetIndexInfoForOpcode(MenuOption->ThisTag->OpCode)) {
+        return (BOOLEAN) (MenuOption->Sequence == gHighligthMenuInfo.HLTSequence);
+      } else {
+        return FALSE;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Find the highlight menu.
+
+  If the input is NULL, base on the record highlight info in
+  gHighligthMenuInfo to find the last highlight menu.
+
+  @param  HighLightedStatement      The input highlight statement.
+
+  @retval  The highlight menu index.
+
+**/
+LIST_ENTRY *
+FindHighLightMenuOption (
+ IN FORM_DISPLAY_ENGINE_STATEMENT  *HighLightedStatement
+ )
+{
+  LIST_ENTRY                      *NewPos;
+  UI_MENU_OPTION                  *MenuOption;
+
+  NewPos = gMenuOption.ForwardLink;
+  MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+
+  if (HighLightedStatement != NULL) {
+    while (MenuOption->ThisTag != HighLightedStatement) {
+      NewPos     = NewPos->ForwardLink;
+      if (NewPos == &gMenuOption) {
+        //
+        // Not Found it, break
+        //
+        break;
+      }
+      MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+    }
+
+    //
+    // Must find the highlight statement.
+    //
+    ASSERT (NewPos != &gMenuOption);
+
+  } else {
+    while (!IsHighLightMenuOption (MenuOption)) {
+      NewPos     = NewPos->ForwardLink;
+      if (NewPos == &gMenuOption) {
+        //
+        // Not Found it, break
+        //
+        break;
+      }
+      MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+    }
+
+    //
+    // Highlight statement has disappear (suppressed/disableed)
+    //
+    if (NewPos == &gMenuOption) {
+      NewPos = NULL;
+    }
+  }
+
+  return NewPos;
+}
+
+/**
+  Is this the Top of screen menu.
+
+  @param  MenuOption      The input Menu option.
+
+  @retval  TRUE   This is the Top of screen menu option.
+  @retval  FALSE  This is not the Top of screen menu option.
+
+**/
+BOOLEAN
+IsTopOfScreeMenuOption (
+  IN UI_MENU_OPTION     *MenuOption
+  )
+{
+  if (gHighligthMenuInfo.TOSQuestionId != 0) {
+    return (BOOLEAN) (GetQuestionIdInfo(MenuOption->ThisTag->OpCode) == gHighligthMenuInfo.TOSQuestionId);
+  } 
+
+  if(CompareMem (gHighligthMenuInfo.TOSOpCode, MenuOption->ThisTag->OpCode, gHighligthMenuInfo.TOSOpCode->Length) == 0) {
+    if (gHighligthMenuInfo.TOSIndex == 0 || gHighligthMenuInfo.TOSIndex == GetIndexInfoForOpcode(MenuOption->ThisTag->OpCode)) {
+      return TRUE;
+    } else {
+      return FALSE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Find the Top of screen menu.
+
+  If the input is NULL, base on the record highlight info in
+  gHighligthMenuInfo to find the last highlight menu.
+
+  @param  HighLightedStatement      The input highlight statement.
+
+  @retval  The highlight menu index.
+
+**/
+LIST_ENTRY *
+FindTopOfScreenMenuOption (
+ VOID
+ )
+{
+  LIST_ENTRY                      *NewPos;
+  UI_MENU_OPTION                  *MenuOption;
+
+  NewPos = gMenuOption.ForwardLink;
+  MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+
+  while (!IsTopOfScreeMenuOption(MenuOption)) {
+    NewPos     = NewPos->ForwardLink;
+    if (NewPos == &gMenuOption) {
+      //
+      // Not Found it, break
+      //
+      break;
+    }
+    MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+  }
+
+  //
+  // Last time top of screen menu has disappeared.
+  //
+  if (NewPos == &gMenuOption) {
+    NewPos = NULL;
+  }
+
+  return NewPos;
+}
+
+/**
+  Find the first menu which will be show at the top.
+
+  @param  FormData               The data info for this form.
+  @param  TopOfScreen            The link_entry pointer to top menu.
+  @param  HighlightMenu          The menu which will be highlight.
+  @param  SkipValue              The skip value for the top menu.
+
+**/
+VOID
+FindTopMenu (
+  IN  FORM_DISPLAY_ENGINE_FORM  *FormData,
+  OUT LIST_ENTRY                **TopOfScreen,
+  OUT LIST_ENTRY                **HighlightMenu,
+  OUT UINTN                     *SkipValue
+  )
+{
+  UINTN                           TopRow;
+  UINTN                           BottomRow;
+  UI_MENU_OPTION                  *MenuOption;
+  UINTN                           TmpValue;
+
+  TopRow    = gStatementDimensions.TopRow    + SCROLL_ARROW_HEIGHT;
+  BottomRow = gStatementDimensions.BottomRow - SCROLL_ARROW_HEIGHT;
+  //
+  // When option mismatch happens,there exist two cases,one is reenter the form, just like the if case below,
+  // and the other is exit current form and enter last form, it can be covered by the else case.
+  //
+  if (gMisMatch && gFormData->HiiHandle == gHighligthMenuInfo.HiiHandle && gFormData->FormId == gHighligthMenuInfo.FormId) {
+    //
+    // Reenter caused by option mismatch or auto exit caused by refresh form(refresh interval/guid), 
+    // base on the record highlight info to find the highlight menu.
+    //
+
+    *HighlightMenu = FindHighLightMenuOption(NULL);
+    if (*HighlightMenu != NULL) {
+      //
+      // Update skip info for this highlight menu.
+      //
+      MenuOption = MENU_OPTION_FROM_LINK (*HighlightMenu);
+      UpdateOptionSkipLines (MenuOption);
+
+      //
+      // Found the last time highlight menu.
+      //
+      *TopOfScreen = FindTopOfScreenMenuOption();
+      if (*TopOfScreen != NULL) {
+        //
+        // Found the last time selectable top of screen menu.
+        //
+        AdjustDateAndTimePosition(TRUE, TopOfScreen);
+        MenuOption = MENU_OPTION_FROM_LINK (*TopOfScreen);
+        UpdateOptionSkipLines (MenuOption);
+
+        *SkipValue = gHighligthMenuInfo.SkipValue;
+      } else {
+        //
+        // Not found last time top of screen menu, so base on current highlight menu
+        // to find the new top of screen menu.
+        // Make the current highlight menu at the bottom of the form to calculate the
+        // top of screen menu.
+        //
+        if (MenuOption->Skip >= BottomRow - TopRow) {
+          *TopOfScreen = *HighlightMenu;
+          TmpValue     = 0;
+        } else {
+          *TopOfScreen = FindTopOfScreenMenu(*HighlightMenu, BottomRow - TopRow - MenuOption->Skip, &TmpValue);
+        }
+
+        *SkipValue   = TmpValue;
+      }
+    } else {
+      //
+      // Last time highlight menu has disappear, find the first highlightable menu as the defalut one.
+      //
+      *HighlightMenu = gMenuOption.ForwardLink;
+      if (!IsListEmpty (&gMenuOption)) {
+        MoveToNextStatement (FALSE, HighlightMenu, BottomRow - TopRow, TRUE);
+      }
+      *TopOfScreen   = gMenuOption.ForwardLink;
+      *SkipValue = 0;
+    }
+
+  } else if (FormData->HighLightedStatement != NULL) {
+    if (IsSavedHighlightStatement (FormData->HighLightedStatement)) {
+      //
+      // Input highlight menu is same as last time highlight menu.
+      // Base on last time highlight menu to set the top of screen menu and highlight menu.
+      //
+      *HighlightMenu = FindHighLightMenuOption(NULL);
+      ASSERT (*HighlightMenu != NULL);
+
+      //
+      // Update skip info for this highlight menu.
+      //
+      MenuOption = MENU_OPTION_FROM_LINK (*HighlightMenu);
+      UpdateOptionSkipLines (MenuOption);
+      
+      *TopOfScreen = FindTopOfScreenMenuOption();
+      if (*TopOfScreen == NULL) {
+        //
+        // Not found last time top of screen menu, so base on current highlight menu
+        // to find the new top of screen menu.
+        // Make the current highlight menu at the bottom of the form to calculate the
+        // top of screen menu.
+        //
+        if (MenuOption->Skip >= BottomRow - TopRow) {
+          *TopOfScreen = *HighlightMenu;
+          TmpValue     = 0;
+        } else {
+          *TopOfScreen = FindTopOfScreenMenu(*HighlightMenu, BottomRow - TopRow - MenuOption->Skip, &TmpValue);
+        }
+
+        *SkipValue   = TmpValue;
+      } else {
+        AdjustDateAndTimePosition(TRUE, TopOfScreen);
+        MenuOption = MENU_OPTION_FROM_LINK (*TopOfScreen);
+        UpdateOptionSkipLines (MenuOption);
+
+        *SkipValue = gHighligthMenuInfo.SkipValue;
+      }
+      AdjustDateAndTimePosition(TRUE, TopOfScreen);
+    } else {
+      //
+      // Input highlight menu is not save as last time highlight menu.
+      //
+      *HighlightMenu = FindHighLightMenuOption(FormData->HighLightedStatement);
+      MenuOption = MENU_OPTION_FROM_LINK (*HighlightMenu);
+      UpdateOptionSkipLines (MenuOption);
+
+      //
+      // Make the current highlight menu at the bottom of the form to calculate the
+      // top of screen menu.
+      //
+      if (MenuOption->Skip >= BottomRow - TopRow) {
+        *TopOfScreen = *HighlightMenu;
+        TmpValue     = 0;
+      } else {
+        *TopOfScreen = FindTopOfScreenMenu(*HighlightMenu, BottomRow - TopRow - MenuOption->Skip, &TmpValue);
+      }
+
+      *SkipValue   = TmpValue;
+    }
+    AdjustDateAndTimePosition(TRUE, TopOfScreen);
+  } else {
+    //
+    // If not has input highlight statement, just return the first one in this form.
+    //
+    *TopOfScreen   = gMenuOption.ForwardLink;
+    *HighlightMenu = gMenuOption.ForwardLink;
+    if (!IsListEmpty (&gMenuOption)) {
+      MoveToNextStatement (FALSE, HighlightMenu, BottomRow - TopRow, TRUE);
+    }
+    *SkipValue     = 0;
+  }
+
+  gMisMatch = FALSE;
+
+  //
+  // First enter to show the menu, update highlight info.
+  //
+  UpdateHighlightMenuInfo (*HighlightMenu, *TopOfScreen, *SkipValue);
+}
+
+/**
+  Record the highlight menu and top of screen menu info.
+
+  @param  Highlight               The menu opton which is highlight.
+  @param  TopOfScreen             The menu opton which is at the top of the form.
+  @param  SkipValue               The skip line info for the top of screen menu.
+
+**/
+VOID
+UpdateHighlightMenuInfo (
+  IN  LIST_ENTRY                      *Highlight,
+  IN  LIST_ENTRY                      *TopOfScreen,
+  IN  UINTN                           SkipValue
+  )
+{
+  UI_MENU_OPTION                  *MenuOption;
+  FORM_DISPLAY_ENGINE_STATEMENT   *Statement;
+
+  gHighligthMenuInfo.HiiHandle  = gFormData->HiiHandle;
+  gHighligthMenuInfo.FormId     = gFormData->FormId;
+  gHighligthMenuInfo.SkipValue  = (UINT16)SkipValue;
+
+  if (!IsListEmpty (&gMenuOption)) {
+    MenuOption = MENU_OPTION_FROM_LINK (Highlight);
+    Statement  = MenuOption->ThisTag;
+
+    gUserInput->SelectedStatement = Statement;
+
+    gHighligthMenuInfo.HLTSequence   = MenuOption->Sequence;
+    gHighligthMenuInfo.HLTQuestionId = GetQuestionIdInfo(Statement->OpCode);
+    if (gHighligthMenuInfo.HLTQuestionId == 0) {
+      //
+      // if question id == 0, save the opcode buffer..
+      //
+      if (gHighligthMenuInfo.HLTOpCode != NULL) {
+        FreePool (gHighligthMenuInfo.HLTOpCode);
+      }
+      gHighligthMenuInfo.HLTOpCode = AllocateCopyPool (Statement->OpCode->Length, Statement->OpCode);
+      ASSERT (gHighligthMenuInfo.HLTOpCode != NULL);
+
+      gHighligthMenuInfo.HLTIndex = GetIndexInfoForOpcode(Statement->OpCode);
+    }
+
+    MenuOption = MENU_OPTION_FROM_LINK (TopOfScreen);
+    Statement  = MenuOption->ThisTag;
+
+    gHighligthMenuInfo.TOSQuestionId = GetQuestionIdInfo(Statement->OpCode);
+    if (gHighligthMenuInfo.TOSQuestionId == 0) {
+      //
+      // if question id == 0, save the opcode buffer..
+      //
+      if (gHighligthMenuInfo.TOSOpCode != NULL) {
+        FreePool (gHighligthMenuInfo.TOSOpCode);
+      }
+      gHighligthMenuInfo.TOSOpCode = AllocateCopyPool (Statement->OpCode->Length, Statement->OpCode);
+      ASSERT (gHighligthMenuInfo.TOSOpCode != NULL);
+
+      gHighligthMenuInfo.TOSIndex = GetIndexInfoForOpcode(Statement->OpCode);
+    }
+  } else {
+    gUserInput->SelectedStatement    = NULL;
+
+    gHighligthMenuInfo.HLTSequence   = 0;
+    gHighligthMenuInfo.HLTQuestionId = 0;
+    if (gHighligthMenuInfo.HLTOpCode != NULL) {
+      FreePool (gHighligthMenuInfo.HLTOpCode);
+    }
+    gHighligthMenuInfo.HLTOpCode     = NULL;
+    gHighligthMenuInfo.HLTIndex      = 0;
+
+    gHighligthMenuInfo.TOSQuestionId = 0;
+    if (gHighligthMenuInfo.TOSOpCode != NULL) {
+      FreePool (gHighligthMenuInfo.TOSOpCode);
+    }
+    gHighligthMenuInfo.TOSOpCode     = NULL;
+    gHighligthMenuInfo.TOSIndex      = 0;
+  }
+}
+
+/**
+  Update attribut for this menu.
+
+  @param  MenuOption               The menu opton which this attribut used to.
+  @param  Highlight                Whether this menu will be highlight.
+
+**/
+VOID
+SetDisplayAttribute (
+  IN UI_MENU_OPTION                  *MenuOption,
+  IN BOOLEAN                         Highlight
+  )
+{
+  FORM_DISPLAY_ENGINE_STATEMENT   *Statement;
+  
+  Statement = MenuOption->ThisTag;
+
+  if (Highlight) {
+    gST->ConOut->SetAttribute (gST->ConOut, GetHighlightTextColor ());
+    return;
+  }
+
+  if (MenuOption->GrayOut) {
+    gST->ConOut->SetAttribute (gST->ConOut, GetGrayedTextColor ());
+  } else {
+    if (Statement->OpCode->OpCode == EFI_IFR_SUBTITLE_OP) {
+      gST->ConOut->SetAttribute (gST->ConOut, GetSubTitleTextColor ());
+    } else {
+      gST->ConOut->SetAttribute (gST->ConOut, GetFieldTextColor ());
+    }
+  }
+}
+
+/**
+  Print string for this menu option.
+
+  @param  MenuOption               The menu opton which this attribut used to.
+  @param  Col                      The column that this string will be print at.
+  @param  Row                      The row that this string will be print at.
+  @param  String                   The string which need to print.
+  @param  Width                    The width need to print, if string is less than the
+                                   width, the block space will be used.
+  @param  Highlight                Whether this menu will be highlight.
+
+**/
+VOID
+DisplayMenuString (
+  IN UI_MENU_OPTION         *MenuOption,
+  IN UINTN                  Col,
+  IN UINTN                  Row,
+  IN CHAR16                 *String,
+  IN UINTN                  Width,
+  IN BOOLEAN                Highlight
+  )
+{
+  UINTN            Length;
+
+  //
+  // Print string with normal color.
+  //
+  if (!Highlight) {
+    PrintStringAtWithWidth (Col, Row, String, Width);
+    return;
+  }
+  
+  //
+  // Print the highlight menu string.
+  // First print the highlight string.
+  // 
+  SetDisplayAttribute(MenuOption, TRUE);
+  Length = PrintStringAt (Col, Row, String);
+
+  //
+  // Second, clean the empty after the string.
+  //
+  SetDisplayAttribute(MenuOption, FALSE);
+  PrintStringAtWithWidth (Col + Length, Row, L"", Width - Length);
+}
+
+/**
+  Check whether this menu can has option string.
+
+  @param  MenuOption               The menu opton which this attribut used to.
+
+  @retval TRUE                     This menu option can have option string.
+  @retval FALSE                    This menu option can't have option string.
+
+**/
+BOOLEAN 
+HasOptionString (
+  IN UI_MENU_OPTION                  *MenuOption
+  )
+{
+  FORM_DISPLAY_ENGINE_STATEMENT   *Statement;
+  CHAR16                          *String;
+  UINTN                           Size;
+  EFI_IFR_TEXT                    *TestOp;
+
+  Size = 0;
+  Statement = MenuOption->ThisTag;
+
+  //
+  // See if the second text parameter is really NULL
+  //
+  if (Statement->OpCode->OpCode == EFI_IFR_TEXT_OP) {
+    TestOp = (EFI_IFR_TEXT *) Statement->OpCode;
+    if (TestOp->TextTwo != 0) {
+      String = GetToken (TestOp->TextTwo, gFormData->HiiHandle);
+      Size   = StrLen (String);
+      FreePool (String);
+    }
+  }
+
+  if ((Statement->OpCode->OpCode == EFI_IFR_SUBTITLE_OP) ||
+    (Statement->OpCode->OpCode == EFI_IFR_REF_OP) ||
+    (Statement->OpCode->OpCode == EFI_IFR_PASSWORD_OP) ||
+    (Statement->OpCode->OpCode == EFI_IFR_ACTION_OP) ||
+    (Statement->OpCode->OpCode == EFI_IFR_RESET_BUTTON_OP) ||
+    //
+    // Allow a wide display if text op-code and no secondary text op-code
+    //
+    ((Statement->OpCode->OpCode == EFI_IFR_TEXT_OP) && (Size == 0))
+    ) {
+
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Double confirm with user about the action.
+
+  @param  Action               The user input action.
+
+  @retval TRUE                 User confirm with the input or not need user confirm.
+  @retval FALSE                User want ignore this input.
+
+**/
+BOOLEAN
+FxConfirmPopup (
+  IN UINT32   Action
+  )
+{
+  EFI_INPUT_KEY                   Key;
+  CHAR16                          *CfmStr;
+  UINTN                           CfmStrLen;
+  UINT32                          CheckFlags;
+  BOOLEAN                         RetVal;
+  UINTN                           CatLen;
+  UINTN                           MaxLen;
+
+  CfmStrLen = 0;
+  CatLen    = StrLen (gConfirmMsgConnect);
+
+  //
+  // Below action need extra popup dialog to confirm.
+  // 
+  CheckFlags = BROWSER_ACTION_DISCARD | 
+               BROWSER_ACTION_DEFAULT |
+               BROWSER_ACTION_SUBMIT |
+               BROWSER_ACTION_RESET |
+               BROWSER_ACTION_EXIT;
+
+  //
+  // Not need to confirm with user, just return TRUE.
+  //
+  if ((Action & CheckFlags) == 0) {
+    return TRUE;
+  }
+
+  if ((Action & BROWSER_ACTION_DISCARD) == BROWSER_ACTION_DISCARD) {
+    CfmStrLen += StrLen (gConfirmDiscardMsg);
+  } 
+
+  if ((Action & BROWSER_ACTION_DEFAULT) == BROWSER_ACTION_DEFAULT) {
+    if (CfmStrLen != 0) {
+      CfmStrLen += CatLen;
+    } 
+
+    CfmStrLen += StrLen (gConfirmDefaultMsg);
+  }
+
+  if ((Action & BROWSER_ACTION_SUBMIT)  == BROWSER_ACTION_SUBMIT) {
+    if (CfmStrLen != 0) {
+      CfmStrLen += CatLen;
+    } 
+
+    CfmStrLen += StrLen (gConfirmSubmitMsg);
+  }
+
+  if ((Action & BROWSER_ACTION_RESET)  == BROWSER_ACTION_RESET) {
+    if (CfmStrLen != 0) {
+      CfmStrLen += CatLen;
+    } 
+
+    CfmStrLen += StrLen (gConfirmResetMsg);
+  }
+
+  if ((Action & BROWSER_ACTION_EXIT)  == BROWSER_ACTION_EXIT) {
+    if (CfmStrLen != 0) {
+      CfmStrLen += CatLen;
+    } 
+
+    CfmStrLen += StrLen (gConfirmExitMsg);
+  }
+
+  //
+  // Allocate buffer to save the string.
+  // String + "?" + "\0"
+  //
+  MaxLen = CfmStrLen + 1 + 1;
+  CfmStr = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  ASSERT (CfmStr != NULL);
+
+  if ((Action & BROWSER_ACTION_DISCARD) == BROWSER_ACTION_DISCARD) {
+    StrCpyS (CfmStr, MaxLen, gConfirmDiscardMsg);
+  }
+
+  if ((Action & BROWSER_ACTION_DEFAULT) == BROWSER_ACTION_DEFAULT) {
+    if (CfmStr[0] != 0) {
+      StrCatS (CfmStr, MaxLen, gConfirmMsgConnect);
+      StrCatS (CfmStr, MaxLen, gConfirmDefaultMsg2nd);
+    } else {
+      StrCpyS (CfmStr, MaxLen, gConfirmDefaultMsg);
+    }
+  }
+
+  if ((Action & BROWSER_ACTION_SUBMIT)  == BROWSER_ACTION_SUBMIT) {
+    if (CfmStr[0] != 0) {
+      StrCatS (CfmStr, MaxLen, gConfirmMsgConnect);
+      StrCatS (CfmStr, MaxLen, gConfirmSubmitMsg2nd);
+    } else {
+      StrCpyS (CfmStr, MaxLen, gConfirmSubmitMsg);
+    }
+  }
+
+  if ((Action & BROWSER_ACTION_RESET)  == BROWSER_ACTION_RESET) {
+    if (CfmStr[0] != 0) {
+      StrCatS (CfmStr, MaxLen, gConfirmMsgConnect);
+      StrCatS (CfmStr, MaxLen, gConfirmResetMsg2nd);
+    } else {
+      StrCpyS (CfmStr, MaxLen, gConfirmResetMsg);
+    }
+  }
+
+  if ((Action & BROWSER_ACTION_EXIT)  == BROWSER_ACTION_EXIT) {
+    if (CfmStr[0] != 0) {
+      StrCatS (CfmStr, MaxLen, gConfirmMsgConnect);
+      StrCatS (CfmStr, MaxLen, gConfirmExitMsg2nd);
+    } else {
+      StrCpyS (CfmStr, MaxLen, gConfirmExitMsg);
+    }
+  }
+
+  StrCatS (CfmStr, MaxLen, gConfirmMsgEnd);
+
+  do {
+    CreateDialog (&Key, gEmptyString, CfmStr, gConfirmOpt, gEmptyString, NULL);
+  } while (((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (gConfirmOptYes[0] | UPPER_LOWER_CASE_OFFSET)) &&
+           ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (gConfirmOptNo[0] | UPPER_LOWER_CASE_OFFSET)) &&
+           (Key.ScanCode != SCAN_ESC));
+
+  if ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) == (gConfirmOptYes[0] | UPPER_LOWER_CASE_OFFSET)) {
+    RetVal = TRUE;
+  } else {
+    RetVal = FALSE;
+  }
+
+  FreePool (CfmStr);
+
+  return RetVal;
+}
+
+/**
+  Print string for this menu option.
+
+  @param  MenuOption               The menu opton which this attribut used to.
+  @param  SkipWidth                The skip width between the left to the start of the prompt.
+  @param  BeginCol                 The begin column for one menu.
+  @param  SkipLine                 The skip line for this menu. 
+  @param  BottomRow                The bottom row for this form.
+  @param  Highlight                Whether this menu will be highlight.
+  @param  UpdateCol                Whether need to update the column info for Date/Time.
+
+  @retval EFI_SUCESSS              Process the user selection success.
+
+**/
+EFI_STATUS
+DisplayOneMenu (
+  IN UI_MENU_OPTION                  *MenuOption,
+  IN UINTN                           SkipWidth,
+  IN UINTN                           BeginCol,
+  IN UINTN                           SkipLine,
+  IN UINTN                           BottomRow,
+  IN BOOLEAN                         Highlight,
+  IN BOOLEAN                         UpdateCol
+  )
+{
+  FORM_DISPLAY_ENGINE_STATEMENT   *Statement;
+  UINTN                           Index;
+  UINT16                          Width;
+  UINT16                          PromptWidth;
+  CHAR16                          *StringPtr;
+  CHAR16                          *OptionString;
+  CHAR16                          *OutputString;
+  UINT16                          GlyphWidth;
+  UINTN                           Temp;
+  UINTN                           Temp2;
+  UINTN                           Temp3;
+  EFI_STATUS                      Status;
+  UINTN                           Row;
+  BOOLEAN                         IsProcessingFirstRow;
+  UINTN                           Col;
+  UINTN                           PromptLineNum;
+  UINTN                           OptionLineNum;
+  CHAR16                          AdjustValue;
+  UINTN                           MaxRow;
+
+  Statement = MenuOption->ThisTag;
+  Temp      = SkipLine;
+  Temp2     = SkipLine;
+  Temp3     = SkipLine;
+  AdjustValue   = 0;
+  PromptLineNum = 0;
+  OptionLineNum = 0;
+  MaxRow        = 0;
+  IsProcessingFirstRow = TRUE;
+
+  //
+  // Set default color.
+  //
+  SetDisplayAttribute (MenuOption, FALSE);
+
+  //
+  // 1. Paint the option string.
+  //
+  Status = ProcessOptions (MenuOption, FALSE, &OptionString, FALSE);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (OptionString != NULL) {
+    if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP || Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+      //
+      // Adjust option string for date/time opcode.
+      //
+      ProcessStringForDateTime(MenuOption, OptionString, UpdateCol);
+    }
+  
+    Width       = (UINT16) gOptionBlockWidth - 1;
+    Row         = MenuOption->Row;
+    GlyphWidth  = 1;
+    OptionLineNum = 0;
+  
+    for (Index = 0; GetLineByWidth (OptionString, Width, &GlyphWidth, &Index, &OutputString) != 0x0000;) {
+      if (((Temp2 == 0)) && (Row <= BottomRow)) {
+        if (Statement->OpCode->OpCode == EFI_IFR_DATE_OP || Statement->OpCode->OpCode == EFI_IFR_TIME_OP) {
+          //
+          // For date/time question, it has three menu options for this qustion.
+          // The first/second menu options with the skip value is 0. the last one
+          // with skip value is 1.
+          //
+          if (MenuOption->Skip != 0) {
+            //
+            // For date/ time, print the last past (year for date and second for time)
+            // - 7 means skip [##/##/ for date and [##:##: for time.
+            //
+            DisplayMenuString (MenuOption,MenuOption->OptCol, Row, OutputString, Width + 1 - 7, Highlight);
+          } else {
+            //
+            // For date/ time, print the first and second past (year for date and second for time)
+            // The OutputString has a NARROW_CHAR or WIDE_CHAR at the begin of the string, 
+            // so need to - 1 to remove it, otherwise, it will clean 1 extr char follow it.
+            DisplayMenuString (MenuOption, MenuOption->OptCol, Row, OutputString, StrLen (OutputString) - 1, Highlight);
+          }
+        } else {
+          DisplayMenuString (MenuOption, MenuOption->OptCol, Row, OutputString, Width + 1, Highlight);
+        }
+        OptionLineNum++;
+      }
+      
+      //
+      // If there is more string to process print on the next row and increment the Skip value
+      //
+      if (StrLen (&OptionString[Index]) != 0) {
+        if (Temp2 == 0) {
+          Row++;
+          //
+          // Since the Number of lines for this menu entry may or may not be reflected accurately
+          // since the prompt might be 1 lines and option might be many, and vice versa, we need to do
+          // some testing to ensure we are keeping this in-sync.
+          //
+          // If the difference in rows is greater than or equal to the skip value, increase the skip value
+          //
+          if ((Row - MenuOption->Row) >= MenuOption->Skip) {
+            MenuOption->Skip++;
+          }
+        }
+      }
+  
+      FreePool (OutputString);
+      if (Temp2 != 0) {
+        Temp2--;
+      }
+    }
+  
+    Highlight = FALSE;
+
+    FreePool (OptionString);
+  }
+
+  //
+  // 2. Paint the description.
+  //
+  PromptWidth   = GetWidth (MenuOption, &AdjustValue);
+  Row           = MenuOption->Row;
+  GlyphWidth    = 1;
+  PromptLineNum = 0;
+
+  if (MenuOption->Description == NULL || MenuOption->Description[0] == '\0') {
+    PrintStringAtWithWidth (BeginCol, Row, L"", PromptWidth + AdjustValue + SkipWidth);
+    PromptLineNum++;
+  } else {
+    for (Index = 0; GetLineByWidth (MenuOption->Description, PromptWidth, &GlyphWidth, &Index, &OutputString) != 0x0000;) {      
+      if ((Temp == 0) && (Row <= BottomRow)) { 
+        //
+        // 1.Clean the start LEFT_SKIPPED_COLUMNS 
+        //
+        PrintStringAtWithWidth (BeginCol, Row, L"", SkipWidth);
+        
+        if (Statement->OpCode->OpCode == EFI_IFR_REF_OP && MenuOption->Col >= 2 && IsProcessingFirstRow) {
+          //
+          // Print Arrow for Goto button.
+          //
+          PrintCharAt (
+            MenuOption->Col - 2,
+            Row,
+            GEOMETRICSHAPE_RIGHT_TRIANGLE
+            );
+          IsProcessingFirstRow = FALSE;
+        }
+        DisplayMenuString (MenuOption, MenuOption->Col, Row, OutputString, PromptWidth + AdjustValue, Highlight);
+        PromptLineNum ++;
+      }
+      //
+      // If there is more string to process print on the next row and increment the Skip value
+      //
+      if (StrLen (&MenuOption->Description[Index]) != 0) {
+        if (Temp == 0) {
+          Row++;
+        }
+      }
+
+      FreePool (OutputString);
+      if (Temp != 0) {
+        Temp--;
+      }
+    }
+
+    Highlight = FALSE;
+  }
+
+
+  //
+  // 3. If this is a text op with secondary text information
+  //
+  if ((Statement->OpCode->OpCode  == EFI_IFR_TEXT_OP) && (((EFI_IFR_TEXT*)Statement->OpCode)->TextTwo != 0)) {
+    StringPtr   = GetToken (((EFI_IFR_TEXT*)Statement->OpCode)->TextTwo, gFormData->HiiHandle);
+  
+    Width       = (UINT16) gOptionBlockWidth - 1;
+    Row         = MenuOption->Row;
+    GlyphWidth  = 1;
+    OptionLineNum = 0;
+
+    for (Index = 0; GetLineByWidth (StringPtr, Width, &GlyphWidth, &Index, &OutputString) != 0x0000;) { 
+      if ((Temp3 == 0) && (Row <= BottomRow)) {
+        DisplayMenuString (MenuOption, MenuOption->OptCol, Row, OutputString, Width + 1, Highlight);
+        OptionLineNum++;
+      }
+      //
+      // If there is more string to process print on the next row and increment the Skip value
+      //
+      if (StrLen (&StringPtr[Index]) != 0) {
+        if (Temp3 == 0) {
+          Row++;
+          //
+          // If the rows for text two is greater than or equal to the skip value, increase the skip value
+          //
+          if ((Row - MenuOption->Row) >= MenuOption->Skip) {
+            MenuOption->Skip++;
+          }
+        }
+      }
+  
+      FreePool (OutputString);
+      if (Temp3 != 0) {
+        Temp3--;
+      }
+    }
+
+    FreePool (StringPtr);
+  }
+
+  //
+  // 4.Line number for Option string and prompt string are not equal.
+  //  Clean the column whose line number is less.
+  //
+  if (HasOptionString(MenuOption) && (OptionLineNum != PromptLineNum)) {
+    Col    =  OptionLineNum < PromptLineNum ? MenuOption->OptCol : BeginCol;
+    Row    = (OptionLineNum < PromptLineNum ? OptionLineNum : PromptLineNum) + MenuOption->Row;
+    Width  = (UINT16) (OptionLineNum < PromptLineNum ? gOptionBlockWidth : PromptWidth + AdjustValue + SkipWidth);
+    MaxRow = (OptionLineNum < PromptLineNum ? PromptLineNum : OptionLineNum) + MenuOption->Row - 1;
+    
+    while (Row <= MaxRow) {
+      DisplayMenuString (MenuOption, Col, Row++, L"", Width, FALSE);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Display menu and wait for user to select one menu option, then return it.
+  If AutoBoot is enabled, then if user doesn't select any option,
+  after period of time, it will automatically return the first menu option.
+
+  @param  FormData               The current form data info.
+
+  @retval EFI_SUCESSS            Process the user selection success.
+  @retval EFI_NOT_FOUND          Process option string for orderedlist/Oneof fail.
+
+**/
+EFI_STATUS
+UiDisplayMenu (
+  IN  FORM_DISPLAY_ENGINE_FORM  *FormData
+  )
+{
+  UINTN                           SkipValue;
+  INTN                            Difference;
+  UINTN                           DistanceValue;
+  UINTN                           Row;
+  UINTN                           Col;
+  UINTN                           Temp;
+  UINTN                           Temp2;
+  UINTN                           TopRow;
+  UINTN                           BottomRow;
+  UINTN                           Index;
+  CHAR16                          *StringPtr;
+  CHAR16                          *StringRightPtr;
+  CHAR16                          *StringErrorPtr;
+  CHAR16                          *OptionString;
+  CHAR16                          *HelpString;
+  CHAR16                          *HelpHeaderString;
+  CHAR16                          *HelpBottomString;
+  BOOLEAN                         NewLine;
+  BOOLEAN                         Repaint;
+  BOOLEAN                         UpArrow;
+  BOOLEAN                         DownArrow;
+  EFI_STATUS                      Status;
+  EFI_INPUT_KEY                   Key;
+  LIST_ENTRY                      *Link;
+  LIST_ENTRY                      *NewPos;
+  LIST_ENTRY                      *TopOfScreen;
+  LIST_ENTRY                      *SavedListEntry;
+  UI_MENU_OPTION                  *MenuOption;
+  UI_MENU_OPTION                  *NextMenuOption;
+  UI_MENU_OPTION                  *SavedMenuOption;
+  UI_CONTROL_FLAG                 ControlFlag;
+  UI_SCREEN_OPERATION             ScreenOperation;
+  FORM_DISPLAY_ENGINE_STATEMENT   *Statement;
+  BROWSER_HOT_KEY                 *HotKey;
+  UINTN                           HelpPageIndex;
+  UINTN                           HelpPageCount;
+  UINTN                           RowCount;
+  UINTN                           HelpLine;
+  UINTN                           HelpHeaderLine;
+  UINTN                           HelpBottomLine;
+  BOOLEAN                         MultiHelpPage;
+  UINT16                          EachLineWidth;
+  UINT16                          HeaderLineWidth;
+  UINT16                          BottomLineWidth;
+  EFI_STRING_ID                   HelpInfo;
+  UI_EVENT_TYPE                   EventType;
+  BOOLEAN                         SkipHighLight;
+  EFI_HII_VALUE                   *StatementValue;
+
+  EventType           = UIEventNone;
+  Status              = EFI_SUCCESS;
+  HelpString          = NULL;
+  HelpHeaderString    = NULL;
+  HelpBottomString    = NULL;
+  OptionString        = NULL;
+  ScreenOperation     = UiNoOperation;
+  NewLine             = TRUE;
+  HelpPageCount       = 0;
+  HelpLine            = 0;
+  RowCount            = 0;
+  HelpBottomLine      = 0;
+  HelpHeaderLine      = 0;
+  HelpPageIndex       = 0;
+  MultiHelpPage       = FALSE;
+  EachLineWidth       = 0;
+  HeaderLineWidth     = 0;
+  BottomLineWidth     = 0;
+  UpArrow             = FALSE;
+  DownArrow           = FALSE;
+  SkipValue           = 0;
+  SkipHighLight       = FALSE;
+
+  NextMenuOption      = NULL;
+  SavedMenuOption     = NULL;
+  HotKey              = NULL;
+  Repaint             = TRUE;
+  MenuOption          = NULL;
+  gModalSkipColumn    = (CHAR16) (gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn) / 6;
+
+  ZeroMem (&Key, sizeof (EFI_INPUT_KEY));
+
+  TopRow    = gStatementDimensions.TopRow    + SCROLL_ARROW_HEIGHT;
+  BottomRow = gStatementDimensions.BottomRow - SCROLL_ARROW_HEIGHT - 1;
+
+  Row = TopRow;
+  if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+    Col = gStatementDimensions.LeftColumn + LEFT_SKIPPED_COLUMNS + gModalSkipColumn;
+  } else {
+    Col = gStatementDimensions.LeftColumn + LEFT_SKIPPED_COLUMNS;
+  }
+
+  FindTopMenu(FormData, &TopOfScreen, &NewPos, &SkipValue);
+  if (!IsListEmpty (&gMenuOption)) {
+    NextMenuOption = MENU_OPTION_FROM_LINK (NewPos);
+    gUserInput->SelectedStatement = NextMenuOption->ThisTag;
+  }
+
+  gST->ConOut->EnableCursor (gST->ConOut, FALSE);
+
+  ControlFlag = CfInitialization;
+  while (TRUE) {
+    switch (ControlFlag) {
+    case CfInitialization:
+      if ((gOldFormEntry.HiiHandle != FormData->HiiHandle) || 
+          (!CompareGuid (&gOldFormEntry.FormSetGuid, &FormData->FormSetGuid))) {
+        //
+        // Clear Statement range if different formset is painted.
+        //
+        ClearLines (
+          gStatementDimensions.LeftColumn,
+          gStatementDimensions.RightColumn,
+          TopRow - SCROLL_ARROW_HEIGHT,
+          BottomRow + SCROLL_ARROW_HEIGHT,
+          GetFieldTextColor ()
+          );
+
+      }
+      ControlFlag = CfRepaint;
+      break;
+
+    case CfRepaint:
+      ControlFlag = CfRefreshHighLight;
+
+      if (Repaint) {
+        //
+        // Display menu
+        //
+        DownArrow       = FALSE;
+        UpArrow         = FALSE;
+        Row             = TopRow;
+
+        gST->ConOut->SetAttribute (gST->ConOut, GetFieldTextColor ());
+        
+        //
+        // 1. Check whether need to print the arrow up.
+        //
+        if (!ValueIsScroll (TRUE, TopOfScreen)) {
+          UpArrow = TRUE;
+        }
+
+        if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+          PrintStringAtWithWidth(gStatementDimensions.LeftColumn + gModalSkipColumn, TopRow - 1, L"", gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn - 2 * gModalSkipColumn);
+        } else {
+          PrintStringAtWithWidth(gStatementDimensions.LeftColumn, TopRow - 1, L"", gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn);
+        }
+        if (UpArrow) {
+          gST->ConOut->SetAttribute (gST->ConOut, GetArrowColor ());
+          PrintCharAt (
+            gStatementDimensions.LeftColumn + gPromptBlockWidth + gOptionBlockWidth + 1,
+            TopRow - SCROLL_ARROW_HEIGHT,
+            ARROW_UP
+            );
+          gST->ConOut->SetAttribute (gST->ConOut, GetFieldTextColor ());
+        }
+
+        //
+        // 2.Paint the menu.
+        //
+        for (Link = TopOfScreen; Link != &gMenuOption; Link = Link->ForwardLink) {
+          MenuOption          = MENU_OPTION_FROM_LINK (Link);
+          MenuOption->Row     = Row;
+          MenuOption->Col     = Col;
+          if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+            MenuOption->OptCol  = gStatementDimensions.LeftColumn + LEFT_SKIPPED_COLUMNS + gPromptBlockWidth + gModalSkipColumn;
+          } else {
+            MenuOption->OptCol  = gStatementDimensions.LeftColumn + LEFT_SKIPPED_COLUMNS + gPromptBlockWidth;
+          }
+
+          if (MenuOption->NestInStatement) {
+            MenuOption->Col += SUBTITLE_INDENT;
+          }
+
+          //
+          // Save the highlight menu, will be used in CfRefreshHighLight case.
+          //
+          if (Link == NewPos) {
+            SavedMenuOption = MenuOption;
+            SkipHighLight   = TRUE;
+          }
+
+          if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+            Status = DisplayOneMenu (MenuOption, 
+                            MenuOption->Col - gStatementDimensions.LeftColumn,
+                            gStatementDimensions.LeftColumn + gModalSkipColumn, 
+                            Link == TopOfScreen ? SkipValue : 0, 
+                            BottomRow,
+                            (BOOLEAN) ((Link == NewPos) && IsSelectable(MenuOption)),
+                            TRUE
+                            );
+          } else {
+            Status = DisplayOneMenu (MenuOption, 
+                            MenuOption->Col - gStatementDimensions.LeftColumn,
+                            gStatementDimensions.LeftColumn, 
+                            Link == TopOfScreen ? SkipValue : 0, 
+                            BottomRow,
+                            (BOOLEAN) ((Link == NewPos) && IsSelectable(MenuOption)),
+                            TRUE
+                            );
+          }
+
+          if (EFI_ERROR (Status)) {
+            if (gMisMatch) {
+              return EFI_SUCCESS;
+            } else {
+              return Status;
+            }
+          }
+          //
+          // 3. Update the row info which will be used by next menu.
+          //
+          if (Link == TopOfScreen) {
+            Row += MenuOption->Skip - SkipValue;
+          } else {
+            Row += MenuOption->Skip;
+          }
+
+          if (Row > BottomRow) {
+            if (!ValueIsScroll (FALSE, Link)) {
+              DownArrow = TRUE;
+            }
+
+            Row = BottomRow + 1;
+            break;
+          }
+        }
+
+        //
+        // 3. Menus in this form may not cover all form, clean the remain field.
+        //
+        while (Row <= BottomRow) {
+          if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+            PrintStringAtWithWidth(gStatementDimensions.LeftColumn + gModalSkipColumn, Row++, L"", gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn - 2 * gModalSkipColumn);
+          } else {
+            PrintStringAtWithWidth(gStatementDimensions.LeftColumn, Row++, L"", gStatementDimensions.RightColumn - gHelpBlockWidth - gStatementDimensions.LeftColumn);
+          }
+        }
+
+        //
+        // 4. Print the down arrow row.
+        //
+        if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+          PrintStringAtWithWidth(gStatementDimensions.LeftColumn + gModalSkipColumn, BottomRow + 1, L"", gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn - 2 *  + gModalSkipColumn);
+        } else {
+          PrintStringAtWithWidth(gStatementDimensions.LeftColumn, BottomRow + 1, L"", gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn);
+        }
+        if (DownArrow) {
+          gST->ConOut->SetAttribute (gST->ConOut, GetArrowColor ());
+          PrintCharAt (
+            gStatementDimensions.LeftColumn + gPromptBlockWidth + gOptionBlockWidth + 1,
+            BottomRow + SCROLL_ARROW_HEIGHT,
+            ARROW_DOWN
+            );
+          gST->ConOut->SetAttribute (gST->ConOut, GetFieldTextColor ());
+        }
+
+        MenuOption = NULL;
+      }
+      break;
+
+    case CfRefreshHighLight:
+
+      //
+      // MenuOption: Last menu option that need to remove hilight
+      //             MenuOption is set to NULL in Repaint
+      // NewPos:     Current menu option that need to hilight
+      //
+      ControlFlag = CfUpdateHelpString;
+
+      UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+
+      if (SkipHighLight) {
+        SkipHighLight = FALSE;
+        MenuOption    = SavedMenuOption;
+        RefreshKeyHelp(gFormData, SavedMenuOption->ThisTag, FALSE);
+        break;
+      }
+
+      if (IsListEmpty (&gMenuOption)) {
+        //
+        // No menu option, just update the hotkey filed.
+        //
+        RefreshKeyHelp(gFormData, NULL, FALSE);
+        break;
+      }
+
+      if (MenuOption != NULL && TopOfScreen == &MenuOption->Link) {
+        Temp = SkipValue;
+      } else {
+        Temp = 0;
+      }
+      if (NewPos == TopOfScreen) {
+        Temp2 = SkipValue;
+      } else {
+        Temp2 = 0;
+      }
+
+      if (NewPos != NULL && (MenuOption == NULL || NewPos != &MenuOption->Link)) {
+        if (MenuOption != NULL) {
+          //
+          // Remove the old highlight menu.
+          //
+          Status = DisplayOneMenu (MenuOption, 
+                          MenuOption->Col - gStatementDimensions.LeftColumn,
+                          gStatementDimensions.LeftColumn, 
+                          Temp, 
+                          BottomRow,
+                          FALSE,
+                          FALSE
+                          );
+        }
+
+        //
+        // This is the current selected statement
+        //
+        MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+        RefreshKeyHelp(gFormData, MenuOption->ThisTag, FALSE);
+
+        if (!IsSelectable (MenuOption)) {
+          break;
+        }
+
+        Status = DisplayOneMenu (MenuOption, 
+                        MenuOption->Col - gStatementDimensions.LeftColumn,
+                        gStatementDimensions.LeftColumn, 
+                        Temp2, 
+                        BottomRow,
+                        TRUE,
+                        FALSE
+                        );
+      }
+      break;
+
+    case CfUpdateHelpString:
+      ControlFlag = CfPrepareToReadKey;
+      if ((FormData->Attribute & HII_DISPLAY_MODAL) != 0) {
+        break;
+      }
+
+      //
+      // NewLine means only update highlight menu (remove old highlight and highlith
+      // the new one), not need to full repain the form.
+      //
+      if (Repaint || NewLine) {
+        if (IsListEmpty (&gMenuOption)) {
+          //
+          // Don't print anything if no mwnu option.
+          //
+          StringPtr = GetToken (STRING_TOKEN (EMPTY_STRING), gHiiHandle);
+        } else {
+          //
+          // Don't print anything if it is a NULL help token
+          //
+          ASSERT(MenuOption != NULL);
+          HelpInfo = ((EFI_IFR_STATEMENT_HEADER *) ((CHAR8 *)MenuOption->ThisTag->OpCode + sizeof (EFI_IFR_OP_HEADER)))->Help;
+          Statement = MenuOption->ThisTag;
+          StatementValue = &Statement->CurrentValue;
+          if (HelpInfo == 0 || !IsSelectable (MenuOption)) {
+            if ((Statement->OpCode->OpCode == EFI_IFR_DATE_OP && StatementValue->Value.date.Month== 0xff)||(Statement->OpCode->OpCode == EFI_IFR_TIME_OP && StatementValue->Value.time.Hour == 0xff)){
+              StringPtr = GetToken (STRING_TOKEN (GET_TIME_FAIL), gHiiHandle);
+            } else {
+              StringPtr = GetToken (STRING_TOKEN (EMPTY_STRING), gHiiHandle);
+            }
+          } else {
+            if ((Statement->OpCode->OpCode == EFI_IFR_DATE_OP && StatementValue->Value.date.Month== 0xff)||(Statement->OpCode->OpCode == EFI_IFR_TIME_OP && StatementValue->Value.time.Hour == 0xff)){
+              StringRightPtr = GetToken (HelpInfo, gFormData->HiiHandle);
+              StringErrorPtr = GetToken (STRING_TOKEN (GET_TIME_FAIL), gHiiHandle);
+              StringPtr = AllocateZeroPool ((StrLen (StringRightPtr) + StrLen (StringErrorPtr)+ 1 ) * sizeof (CHAR16));
+              StrCpyS (StringPtr, StrLen (StringRightPtr) + StrLen (StringErrorPtr) + 1, StringRightPtr);
+              StrCatS (StringPtr, StrLen (StringRightPtr) + StrLen (StringErrorPtr) + 1, StringErrorPtr);
+              FreePool (StringRightPtr);
+              FreePool (StringErrorPtr);
+            } else {
+              StringPtr = GetToken (HelpInfo, gFormData->HiiHandle);
+            }
+          }
+        }
+
+        RowCount      = BottomRow - TopRow + 1;
+        HelpPageIndex = 0;
+        //
+        // 1.Calculate how many line the help string need to print.
+        //
+        if (HelpString != NULL) {
+          FreePool (HelpString);
+          HelpString = NULL;
+        }
+        HelpLine = ProcessHelpString (StringPtr, &HelpString, &EachLineWidth, RowCount);
+        FreePool (StringPtr);
+
+        if (HelpLine > RowCount) {
+          MultiHelpPage   = TRUE;
+          StringPtr       = GetToken (STRING_TOKEN(ADJUST_HELP_PAGE_UP), gHiiHandle);
+          if (HelpHeaderString != NULL) {
+            FreePool (HelpHeaderString);
+            HelpHeaderString = NULL;
+          }
+          HelpHeaderLine  = ProcessHelpString (StringPtr, &HelpHeaderString, &HeaderLineWidth, 0);
+          FreePool (StringPtr);
+          StringPtr       = GetToken (STRING_TOKEN(ADJUST_HELP_PAGE_DOWN), gHiiHandle);
+          if (HelpBottomString != NULL) {
+            FreePool (HelpBottomString);
+            HelpBottomString = NULL;
+          }
+          HelpBottomLine  = ProcessHelpString (StringPtr, &HelpBottomString, &BottomLineWidth, 0);
+          FreePool (StringPtr);
+          //
+          // Calculate the help page count.
+          //
+          if (HelpLine > 2 * RowCount - 2) {
+            HelpPageCount = (HelpLine - RowCount + 1) / (RowCount - 2) + 1;
+            if ((HelpLine - RowCount + 1) % (RowCount - 2) != 0) {
+              HelpPageCount += 1;
+            }
+          } else {
+            HelpPageCount = 2;
+          }
+        } else {
+          MultiHelpPage = FALSE;
+        }
+      }
+
+      //
+      // Check whether need to show the 'More(U/u)' at the begin.
+      // Base on current direct info, here shows aligned to the right side of the column.
+      // If the direction is multi line and aligned to right side may have problem, so 
+      // add ASSERT code here.
+      //
+      if (HelpPageIndex > 0) {
+        gST->ConOut->SetAttribute (gST->ConOut, GetInfoTextColor ());
+        for (Index = 0; Index < HelpHeaderLine; Index++) {
+          ASSERT (HelpHeaderLine == 1);
+          ASSERT (GetStringWidth (HelpHeaderString) / 2 < (UINTN) (gHelpBlockWidth - 1));
+          PrintStringAtWithWidth (
+            gStatementDimensions.RightColumn - gHelpBlockWidth,
+            Index + TopRow,
+            gEmptyString,
+            gHelpBlockWidth
+            );
+          PrintStringAt (
+            gStatementDimensions.RightColumn - GetStringWidth (HelpHeaderString) / 2 - 1,
+            Index + TopRow,
+            &HelpHeaderString[Index * HeaderLineWidth]
+            );
+        }
+      }
+
+      gST->ConOut->SetAttribute (gST->ConOut, GetHelpTextColor ());
+      //
+      // Print the help string info.
+      //
+      if (!MultiHelpPage) {
+        for (Index = 0; Index < HelpLine; Index++) {
+          PrintStringAtWithWidth (
+            gStatementDimensions.RightColumn - gHelpBlockWidth,
+            Index + TopRow,
+            &HelpString[Index * EachLineWidth],
+            gHelpBlockWidth
+            );
+        }
+        for (; Index < RowCount; Index ++) {
+          PrintStringAtWithWidth (
+            gStatementDimensions.RightColumn - gHelpBlockWidth,
+            Index + TopRow,
+            gEmptyString,
+            gHelpBlockWidth
+            );
+        }
+        gST->ConOut->SetCursorPosition(gST->ConOut, gStatementDimensions.RightColumn-1, BottomRow);
+      } else  {
+        if (HelpPageIndex == 0) {
+          for (Index = 0; Index < RowCount - HelpBottomLine; Index++) {
+            PrintStringAtWithWidth (
+              gStatementDimensions.RightColumn - gHelpBlockWidth,
+              Index + TopRow,
+              &HelpString[Index * EachLineWidth],
+              gHelpBlockWidth
+              );
+          }
+        } else {
+          for (Index = 0; (Index < RowCount - HelpBottomLine - HelpHeaderLine) && 
+              (Index + HelpPageIndex * (RowCount - 2) + 1 < HelpLine); Index++) {
+            PrintStringAtWithWidth (
+              gStatementDimensions.RightColumn - gHelpBlockWidth,
+              Index + TopRow + HelpHeaderLine,
+              &HelpString[(Index + HelpPageIndex * (RowCount - 2) + 1)* EachLineWidth],
+              gHelpBlockWidth
+              );
+          }
+          if (HelpPageIndex == HelpPageCount - 1) {
+            for (; Index < RowCount - HelpHeaderLine; Index ++) {
+              PrintStringAtWithWidth (
+                gStatementDimensions.RightColumn - gHelpBlockWidth,
+                Index + TopRow + HelpHeaderLine,
+                gEmptyString,
+                gHelpBlockWidth
+                );
+            }
+            gST->ConOut->SetCursorPosition(gST->ConOut, gStatementDimensions.RightColumn-1, BottomRow);
+          }
+        } 
+      }
+
+      //
+      // Check whether need to print the 'More(D/d)' at the bottom.
+      // Base on current direct info, here shows aligned to the right side of the column.
+      // If the direction is multi line and aligned to right side may have problem, so 
+      // add ASSERT code here.
+      //
+      if (HelpPageIndex < HelpPageCount - 1 && MultiHelpPage) {
+        gST->ConOut->SetAttribute (gST->ConOut, GetInfoTextColor ());
+        for (Index = 0; Index < HelpBottomLine; Index++) {
+          ASSERT (HelpBottomLine == 1);
+          ASSERT (GetStringWidth (HelpBottomString) / 2 < (UINTN) (gHelpBlockWidth - 1)); 
+          PrintStringAtWithWidth (
+            gStatementDimensions.RightColumn - gHelpBlockWidth,
+            BottomRow + Index - HelpBottomLine + 1,
+            gEmptyString,
+            gHelpBlockWidth
+            );
+          PrintStringAt (
+            gStatementDimensions.RightColumn - GetStringWidth (HelpBottomString) / 2 - 1,
+            BottomRow + Index - HelpBottomLine + 1,
+            &HelpBottomString[Index * BottomLineWidth]
+            );
+        }
+      }
+      //
+      // Reset this flag every time we finish using it.
+      //
+      Repaint = FALSE;
+      NewLine = FALSE;
+      break;
+
+    case CfPrepareToReadKey:
+      ControlFlag = CfReadKey;
+      ScreenOperation = UiNoOperation;
+      break;
+
+    case CfReadKey:
+      ControlFlag = CfScreenOperation;
+
+      //
+      // Wait for user's selection
+      //
+      while (TRUE) {
+        Status = gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+        if (!EFI_ERROR (Status)) {
+          EventType = UIEventKey;
+          break;
+        }
+
+        //
+        // If we encounter error, continue to read another key in.
+        //
+        if (Status != EFI_NOT_READY) {
+          continue;
+        }
+        
+        EventType = UiWaitForEvent(gST->ConIn->WaitForKey);
+        if (EventType == UIEventKey) {
+          gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+        }
+        break;
+      }
+
+      if (EventType == UIEventDriver) {
+        gMisMatch = TRUE;
+        gUserInput->Action = BROWSER_ACTION_NONE;
+        ControlFlag = CfExit;
+        break;
+      }
+      
+      if (EventType == UIEventTimeOut) {
+        gUserInput->Action = BROWSER_ACTION_FORM_EXIT;
+        ControlFlag = CfExit;
+        break;
+      }
+
+      switch (Key.UnicodeChar) {
+      case CHAR_CARRIAGE_RETURN:
+        if(MenuOption == NULL || MenuOption->GrayOut || MenuOption->ReadOnly) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+
+        ScreenOperation = UiSelect;
+        gDirection      = 0;
+        break;
+
+      //
+      // We will push the adjustment of these numeric values directly to the input handler
+      //  NOTE: we won't handle manual input numeric
+      //
+      case '+':
+      case '-':
+        //
+        // If the screen has no menu items, and the user didn't select UiReset
+        // ignore the selection and go back to reading keys.
+        //
+        ASSERT(MenuOption != NULL);
+        if(IsListEmpty (&gMenuOption) || MenuOption->GrayOut || MenuOption->ReadOnly) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+
+        Statement = MenuOption->ThisTag;
+        if ((Statement->OpCode->OpCode == EFI_IFR_DATE_OP)
+          || (Statement->OpCode->OpCode == EFI_IFR_TIME_OP)
+          || ((Statement->OpCode->OpCode == EFI_IFR_NUMERIC_OP) && (GetFieldFromNum(Statement->OpCode) != 0))
+        ){
+          if (Key.UnicodeChar == '+') {
+            gDirection = SCAN_RIGHT;
+          } else {
+            gDirection = SCAN_LEFT;
+          }
+          
+          Status = ProcessOptions (MenuOption, TRUE, &OptionString, TRUE);
+          if (OptionString != NULL) {
+            FreePool (OptionString);
+          }
+          if (EFI_ERROR (Status)) {
+            //
+            // Repaint to clear possible error prompt pop-up
+            //
+            Repaint = TRUE;
+            NewLine = TRUE;
+          } else {
+            ControlFlag = CfExit;
+          }
+        }
+        break;
+
+      case '^':
+        ScreenOperation = UiUp;
+        break;
+
+      case 'V':
+      case 'v':
+        ScreenOperation = UiDown;
+        break;
+
+      case ' ':
+        if(IsListEmpty (&gMenuOption)) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+        
+        ASSERT(MenuOption != NULL);
+        if (MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_CHECKBOX_OP && !MenuOption->GrayOut && !MenuOption->ReadOnly) {
+          ScreenOperation = UiSelect;
+        }
+        break;
+
+      case 'D':
+      case 'd':
+        if (!MultiHelpPage) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+        ControlFlag    = CfUpdateHelpString;
+        HelpPageIndex  = HelpPageIndex < HelpPageCount - 1 ? HelpPageIndex + 1 : HelpPageCount - 1;
+        break;
+
+      case 'U':
+      case 'u':
+        if (!MultiHelpPage) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+        ControlFlag    = CfUpdateHelpString;
+        HelpPageIndex  = HelpPageIndex > 0 ? HelpPageIndex - 1 : 0;
+        break;
+
+      case CHAR_NULL:
+        for (Index = 0; Index < mScanCodeNumber; Index++) {
+          if (Key.ScanCode == gScanCodeToOperation[Index].ScanCode) {
+            ScreenOperation = gScanCodeToOperation[Index].ScreenOperation;
+            break;
+          }
+        }
+        
+        if (((FormData->Attribute & HII_DISPLAY_MODAL) != 0) && (Key.ScanCode == SCAN_ESC || Index == mScanCodeNumber)) {
+          //
+          // ModalForm has no ESC key and Hot Key.
+          //
+          ControlFlag = CfReadKey;
+        } else if (Index == mScanCodeNumber) {
+          //
+          // Check whether Key matches the registered hot key.
+          //
+          HotKey = NULL;
+          HotKey = GetHotKeyFromRegisterList (&Key);
+          if (HotKey != NULL) {
+            ScreenOperation = UiHotKey;
+          }
+        }
+        break;
+      }
+      break;
+
+    case CfScreenOperation:
+      if ((ScreenOperation != UiReset) && (ScreenOperation != UiHotKey)) {
+        //
+        // If the screen has no menu items, and the user didn't select UiReset or UiHotKey
+        // ignore the selection and go back to reading keys.
+        //
+        if (IsListEmpty (&gMenuOption)) {
+          ControlFlag = CfReadKey;
+          break;
+        }
+      }
+
+      for (Index = 0;
+           Index < sizeof (gScreenOperationToControlFlag) / sizeof (gScreenOperationToControlFlag[0]);
+           Index++
+          ) {
+        if (ScreenOperation == gScreenOperationToControlFlag[Index].ScreenOperation) {
+          ControlFlag = gScreenOperationToControlFlag[Index].ControlFlag;
+          break;
+        }
+      }
+      break;
+
+    case CfUiSelect:
+      ControlFlag = CfRepaint;
+
+      ASSERT(MenuOption != NULL);
+      Statement = MenuOption->ThisTag;
+      if (Statement->OpCode->OpCode == EFI_IFR_TEXT_OP) {
+        break;
+      }
+
+      switch (Statement->OpCode->OpCode) {
+      case EFI_IFR_REF_OP:
+      case EFI_IFR_ACTION_OP:
+      case EFI_IFR_RESET_BUTTON_OP:
+        ControlFlag = CfExit;
+        break;
+
+      default:
+        //
+        // Editable Questions: oneof, ordered list, checkbox, numeric, string, password
+        //
+        RefreshKeyHelp (gFormData, Statement, TRUE);
+        Status = ProcessOptions (MenuOption, TRUE, &OptionString, TRUE);
+        
+        if (OptionString != NULL) {
+          FreePool (OptionString);
+        }
+        
+        if (EFI_ERROR (Status)) {
+          Repaint = TRUE;
+          NewLine = TRUE;
+          RefreshKeyHelp (gFormData, Statement, FALSE);
+          break;
+        } else {
+          ControlFlag = CfExit;
+          break;
+        }
+      }
+      break;
+
+    case CfUiReset:
+      //
+      // We come here when someone press ESC
+      // If the policy is not exit front page when user press ESC, process here.
+      //
+      if (!FormExitPolicy()) {
+        Repaint     = TRUE;
+        NewLine     = TRUE;
+        ControlFlag = CfRepaint;
+        break;
+      }
+
+      gUserInput->Action = BROWSER_ACTION_FORM_EXIT;
+      ControlFlag = CfExit;
+      break;
+
+    case CfUiHotKey:
+      ControlFlag = CfRepaint;
+
+      ASSERT (HotKey != NULL);
+
+      if (FxConfirmPopup(HotKey->Action)) {
+        gUserInput->Action = HotKey->Action;
+        if ((HotKey->Action & BROWSER_ACTION_DEFAULT) == BROWSER_ACTION_DEFAULT) {
+          gUserInput->DefaultId = HotKey->DefaultId;
+        }
+        ControlFlag = CfExit;
+      } else {
+        Repaint     = TRUE;
+        NewLine     = TRUE;
+        ControlFlag = CfRepaint;
+      }
+
+      break;
+
+    case CfUiLeft:
+      ControlFlag = CfRepaint;
+      ASSERT(MenuOption != NULL);
+      if ((MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_DATE_OP) || (MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+        if (MenuOption->Sequence != 0) {
+          //
+          // In the middle or tail of the Date/Time op-code set, go left.
+          //
+          ASSERT(NewPos != NULL);
+          NewPos = NewPos->BackLink;
+        }
+      }
+      break;
+
+    case CfUiRight:
+      ControlFlag = CfRepaint;
+      ASSERT(MenuOption != NULL);
+      if ((MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_DATE_OP) || (MenuOption->ThisTag->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+        if (MenuOption->Sequence != 2) {
+          //
+          // In the middle or tail of the Date/Time op-code set, go left.
+          //
+          ASSERT(NewPos != NULL);
+          NewPos = NewPos->ForwardLink;
+        }
+      }
+      break;
+
+    case CfUiUp:
+      ControlFlag = CfRepaint;
+      NewLine     = TRUE;
+
+      SavedListEntry = NewPos;
+      ASSERT(NewPos != NULL);
+
+      MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+      ASSERT (MenuOption != NULL);
+
+      //
+      // Adjust Date/Time position before we advance forward.
+      //
+      AdjustDateAndTimePosition (TRUE, &NewPos);
+
+      NewPos     = NewPos->BackLink;
+      //
+      // Find next selectable menu or the first menu beyond current form.
+      //
+      Difference = MoveToNextStatement (TRUE, &NewPos, MenuOption->Row - TopRow, FALSE);
+      if (Difference < 0) {
+        //
+        // We hit the begining MenuOption that can be focused
+        // so we simply scroll to the top.
+        //
+        Repaint     = TRUE;
+        if (TopOfScreen != gMenuOption.ForwardLink || SkipValue != 0) {
+          TopOfScreen = gMenuOption.ForwardLink;
+          NewPos      = SavedListEntry;
+          SkipValue = 0;
+        } else {
+          //
+          // Scroll up to the last page when we have arrived at top page.
+          //
+          TopOfScreen = FindTopOfScreenMenu (gMenuOption.BackLink, BottomRow - TopRow, &SkipValue);
+          NewPos = gMenuOption.BackLink;
+          MoveToNextStatement (TRUE, &NewPos, BottomRow - TopRow, TRUE);
+        }
+      } else {
+        NextMenuOption = MENU_OPTION_FROM_LINK (NewPos);
+
+        if (MenuOption->Row < TopRow + Difference + NextMenuOption->Skip) {
+          //
+          // Previous focus MenuOption is above the TopOfScreen, so we need to scroll
+          //
+          TopOfScreen = NewPos;
+          Repaint     = TRUE;
+          SkipValue   = 0;
+        }
+
+        //
+        // Check whether new highlight menu is selectable, if not, keep highlight on the old one.
+        //
+        // BottomRow - TopRow + 1 means the total rows current forms supported.
+        // Difference + NextMenuOption->Skip + 1 means the distance between last highlight menu 
+        // and new top menu. New top menu will all shows in next form, but last highlight menu 
+        // may only shows 1 line. + 1 at right part means at least need to keep 1 line for the 
+        // last highlight menu.
+        // 
+        if (!IsSelectable(NextMenuOption) && IsSelectable(MenuOption) && 
+            (BottomRow - TopRow + 1 >= Difference + NextMenuOption->Skip + 1)) {
+          NewPos = SavedListEntry;
+        }
+      }
+
+      UpdateStatusBar (INPUT_ERROR, FALSE);
+
+      //
+      // If we encounter a Date/Time op-code set, rewind to the first op-code of the set.
+      //
+      AdjustDateAndTimePosition (TRUE, &TopOfScreen);
+      AdjustDateAndTimePosition (TRUE, &NewPos);
+
+      UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+      break;
+
+    case CfUiPageUp:
+      //
+      // SkipValue means lines is skipped when show the top menu option.
+      //
+      ControlFlag = CfRepaint;
+      NewLine     = TRUE;
+      Repaint     = TRUE;
+
+      Link      = TopOfScreen;
+      //
+      // First minus the menu of the top screen, it's value is SkipValue.
+      //
+      if (SkipValue >= BottomRow - TopRow + 1) {
+        //
+        // SkipValue > (BottomRow - TopRow + 1) means current menu has more than one
+        // form of options to be show, so just update the SkipValue to show the next
+        // parts of options.
+        //
+        SkipValue -= BottomRow - TopRow + 1;
+        NewPos     = TopOfScreen;
+        break;
+      } else {
+        Index     = (BottomRow + 1) - SkipValue - TopRow;
+      }
+      
+      TopOfScreen = FindTopOfScreenMenu(TopOfScreen, Index, &SkipValue);
+      NewPos = TopOfScreen;
+      MoveToNextStatement (FALSE, &NewPos, BottomRow - TopRow, FALSE);
+      
+      UpdateStatusBar (INPUT_ERROR, FALSE);
+
+      //
+      // If we encounter a Date/Time op-code set, rewind to the first op-code of the set.
+      // Don't do this when we are already in the first page.
+      //
+      AdjustDateAndTimePosition (TRUE, &TopOfScreen);
+      AdjustDateAndTimePosition (TRUE, &NewPos);
+
+      UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+      break;
+
+    case CfUiPageDown:
+      //
+      // SkipValue means lines is skipped when show the top menu option.
+      //
+      ControlFlag = CfRepaint;
+      NewLine     = TRUE;
+      Repaint     = TRUE;
+
+      Link    = TopOfScreen;
+      NextMenuOption = MENU_OPTION_FROM_LINK (Link);
+      Index = TopRow + NextMenuOption->Skip - SkipValue;
+      //
+      // Count to the menu option which will show at the top of the next form.
+      //
+      while ((Index <= BottomRow + 1) && (Link->ForwardLink != &gMenuOption)) {
+        Link           = Link->ForwardLink;
+        NextMenuOption = MENU_OPTION_FROM_LINK (Link);
+        Index = Index + NextMenuOption->Skip;
+      }
+
+      if ((Link->ForwardLink == &gMenuOption) && (Index <= BottomRow + 1)) {
+        //
+        // Highlight on the last menu which can be highlight.
+        //
+        Repaint = FALSE;
+        MoveToNextStatement (TRUE, &Link, Index - TopRow, TRUE);
+      } else {
+        //
+        // Calculate the skip line for top of screen menu.
+        //
+        if (Link == TopOfScreen) {
+          //
+          // The top of screen menu option occupies the entire form.
+          //
+          SkipValue += BottomRow - TopRow + 1;
+        } else {
+          SkipValue = NextMenuOption->Skip - (Index - (BottomRow + 1));
+        }
+        TopOfScreen = Link;
+        MenuOption = NULL;
+        //
+        // Move to the Next selectable menu.
+        //
+        MoveToNextStatement (FALSE, &Link, BottomRow - TopRow, TRUE);
+      }
+
+      //
+      // Save the menu as the next highlight menu.
+      //
+      NewPos  = Link;
+
+      UpdateStatusBar (INPUT_ERROR, FALSE);
+
+      //
+      // If we encounter a Date/Time op-code set, rewind to the first op-code of the set.
+      // Don't do this when we are already in the last page.
+      //
+      AdjustDateAndTimePosition (TRUE, &TopOfScreen);
+      AdjustDateAndTimePosition (TRUE, &NewPos);
+
+      UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+      break;
+
+    case CfUiDown:
+      //
+      // SkipValue means lines is skipped when show the top menu option.
+      // NewPos  points to the menu which is highlighted now.
+      //
+      ControlFlag = CfRepaint;
+      NewLine     = TRUE;
+
+      if (NewPos == TopOfScreen) {
+        Temp2 = SkipValue;
+      } else {
+        Temp2 = 0;
+      }
+
+      SavedListEntry = NewPos;
+      //
+      // Since the behavior of hitting the down arrow on a Date/Time op-code is intended
+      // to be one that progresses to the next set of op-codes, we need to advance to the last
+      // Date/Time op-code and leave the remaining logic in UiDown intact so the appropriate
+      // checking can be done.  The only other logic we need to introduce is that if a Date/Time
+      // op-code is the last entry in the menu, we need to rewind back to the first op-code of
+      // the Date/Time op-code.
+      //
+      AdjustDateAndTimePosition (FALSE, &NewPos);
+
+      MenuOption = MENU_OPTION_FROM_LINK (NewPos);
+      NewPos     = NewPos->ForwardLink;
+      //
+      // Find the next selectable menu.
+      //
+      if (MenuOption->Row + MenuOption->Skip - Temp2 > BottomRow + 1) {
+        if (gMenuOption.ForwardLink == NewPos || &gMenuOption == NewPos) {
+          Difference = -1;
+        } else {
+          Difference = 0;
+        }
+      } else {
+        Difference = MoveToNextStatement (FALSE, &NewPos, BottomRow + 1 - (MenuOption->Row + MenuOption->Skip - Temp2), FALSE);
+      }
+      if (Difference < 0) {
+        //
+        // Scroll to the first page.
+        //
+        if (TopOfScreen != gMenuOption.ForwardLink || SkipValue != 0) { 
+          TopOfScreen = gMenuOption.ForwardLink;
+          Repaint     = TRUE;
+          MenuOption  = NULL;
+        } else {
+          MenuOption = MENU_OPTION_FROM_LINK (SavedListEntry);
+        }
+        NewPos        = gMenuOption.ForwardLink;
+        MoveToNextStatement (FALSE, &NewPos, BottomRow - TopRow, TRUE);
+
+        SkipValue = 0;
+        //
+        // If we are at the end of the list and sitting on a Date/Time op, rewind to the head.
+        //
+        AdjustDateAndTimePosition (TRUE, &TopOfScreen);
+        AdjustDateAndTimePosition (TRUE, &NewPos);
+
+        UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+        break;
+      }        
+
+      //
+      // Get next selected menu info.
+      //
+      AdjustDateAndTimePosition (FALSE, &NewPos);
+      NextMenuOption  = MENU_OPTION_FROM_LINK (NewPos);
+      if (NextMenuOption->Row == 0) {
+        UpdateOptionSkipLines (NextMenuOption);
+      }
+
+      //
+      // Calculate new highlight menu end row.
+      //
+      Temp = (MenuOption->Row + MenuOption->Skip - Temp2) + Difference + NextMenuOption->Skip - 1;
+      if (Temp > BottomRow) {
+        //
+        // Get the top screen menu info.
+        //
+        AdjustDateAndTimePosition (FALSE, &TopOfScreen);
+        SavedMenuOption = MENU_OPTION_FROM_LINK (TopOfScreen);
+
+        //
+        // Current Top screen menu occupy (SavedMenuOption->Skip - SkipValue) rows.
+        // Full shows the new selected menu need to skip (Temp - BottomRow - 1) rows.
+        //
+        if ((Temp - BottomRow) >= (SavedMenuOption->Skip - SkipValue)) {
+          //
+          // Skip the top op-code
+          //
+          TopOfScreen   = TopOfScreen->ForwardLink;
+          DistanceValue = (Temp - BottomRow) - (SavedMenuOption->Skip - SkipValue);
+
+          SavedMenuOption = MENU_OPTION_FROM_LINK (TopOfScreen);
+
+          //
+          // If we have a remainder, skip that many more op-codes until we drain the remainder
+          // Special case is the selected highlight menu has more than one form of menus.
+          //
+          while (DistanceValue >= SavedMenuOption->Skip && TopOfScreen != NewPos) {
+            //
+            // Since the Difference is greater than or equal to this op-code's skip value, skip it
+            //
+            DistanceValue   = DistanceValue - (INTN) SavedMenuOption->Skip;
+            TopOfScreen     = TopOfScreen->ForwardLink;
+            SavedMenuOption = MENU_OPTION_FROM_LINK (TopOfScreen);
+          }
+          //
+          // Since we will act on this op-code in the next routine, and increment the
+          // SkipValue, set the skips to one less than what is required.
+          //
+          if (TopOfScreen != NewPos) {
+            SkipValue = DistanceValue;
+          } else {
+            SkipValue = 0;
+          }
+        } else {
+          //
+          // Since we will act on this op-code in the next routine, and increment the
+          // SkipValue, set the skips to one less than what is required.
+          //
+          SkipValue += Temp - BottomRow;
+        }
+        Repaint       = TRUE;
+      } else if (!IsSelectable (NextMenuOption)) {
+        //
+        // Continue to go down until scroll to next page or the selectable option is found.
+        //
+        ScreenOperation = UiDown;
+        ControlFlag     = CfScreenOperation;
+        break;
+      }
+
+      MenuOption = MENU_OPTION_FROM_LINK (SavedListEntry);
+
+      //
+      // Check whether new highlight menu is selectable, if not, keep highlight on the old one.
+      //
+      // BottomRow - TopRow + 1 means the total rows current forms supported.
+      // Difference + NextMenuOption->Skip + 1 means the distance between last highlight menu 
+      // and new top menu. New top menu will all shows in next form, but last highlight menu 
+      // may only shows 1 line. + 1 at right part means at least need to keep 1 line for the 
+      // last highlight menu.
+      // 
+      if (!IsSelectable (NextMenuOption) && IsSelectable (MenuOption) && 
+         (BottomRow - TopRow + 1 >= Difference + NextMenuOption->Skip + 1)) {
+        NewPos = SavedListEntry;
+      }
+
+      UpdateStatusBar (INPUT_ERROR, FALSE);
+
+      //
+      // If we are at the end of the list and sitting on a Date/Time op, rewind to the head.
+      //
+      AdjustDateAndTimePosition (TRUE, &TopOfScreen);
+      AdjustDateAndTimePosition (TRUE, &NewPos);
+
+      UpdateHighlightMenuInfo(NewPos, TopOfScreen, SkipValue);
+      break;
+
+    case CfUiNoOperation:
+      ControlFlag = CfRepaint;
+      break;
+
+    case CfExit:
+      gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+      if (HelpString != NULL) {
+        FreePool (HelpString);
+      }
+      if (HelpHeaderString != NULL) {
+        FreePool (HelpHeaderString);
+      }
+      if (HelpBottomString != NULL) {
+        FreePool (HelpBottomString);
+      }
+      return EFI_SUCCESS;
+
+    default:
+      break;
+    }
+  }
+}
+
+/**
+
+  Base on the browser status info to show an pop up message.
+
+**/
+VOID
+BrowserStatusProcess (
+  VOID
+  )
+{
+  CHAR16             *ErrorInfo;
+  EFI_INPUT_KEY      Key;
+  EFI_EVENT          WaitList[2];
+  EFI_EVENT          RefreshIntervalEvent;
+  EFI_EVENT          TimeOutEvent;
+  UINT8              TimeOut;
+  EFI_STATUS         Status;
+  UINTN              Index;
+  WARNING_IF_CONTEXT EventContext;
+  EFI_IFR_OP_HEADER  *OpCodeBuf;
+  EFI_STRING_ID      StringToken;
+  CHAR16             DiscardChange;
+  CHAR16             JumpToFormSet;
+  CHAR16             *PrintString;
+
+  if (gFormData->BrowserStatus == BROWSER_SUCCESS) {
+    return;
+  }
+
+  StringToken          = 0;
+  TimeOutEvent         = NULL;
+  RefreshIntervalEvent = NULL;
+  OpCodeBuf            = NULL;
+  if (gFormData->HighLightedStatement != NULL) {
+    OpCodeBuf = gFormData->HighLightedStatement->OpCode;
+  }
+
+  if (gFormData->BrowserStatus == (BROWSER_WARNING_IF)) {
+    ASSERT (OpCodeBuf != NULL && OpCodeBuf->OpCode == EFI_IFR_WARNING_IF_OP);
+
+    TimeOut     = ((EFI_IFR_WARNING_IF *) OpCodeBuf)->TimeOut;
+    StringToken = ((EFI_IFR_WARNING_IF *) OpCodeBuf)->Warning;
+  } else {
+    TimeOut = 0;
+    if ((gFormData->BrowserStatus == (BROWSER_NO_SUBMIT_IF)) &&
+        (OpCodeBuf != NULL && OpCodeBuf->OpCode == EFI_IFR_NO_SUBMIT_IF_OP)) {
+      StringToken = ((EFI_IFR_NO_SUBMIT_IF *) OpCodeBuf)->Error;
+    } else if ((gFormData->BrowserStatus == (BROWSER_INCONSISTENT_IF)) &&
+               (OpCodeBuf != NULL && OpCodeBuf->OpCode == EFI_IFR_INCONSISTENT_IF_OP)) {
+      StringToken = ((EFI_IFR_INCONSISTENT_IF *) OpCodeBuf)->Error;
+    }
+  }
+
+  if (StringToken != 0) {
+    ErrorInfo = GetToken (StringToken, gFormData->HiiHandle);
+  } else if (gFormData->ErrorString != NULL) {
+    //
+    // Only used to compatible with old setup browser.
+    // Not use this field in new browser core.
+    //
+    ErrorInfo = gFormData->ErrorString;
+  } else {
+    switch (gFormData->BrowserStatus) {
+    case BROWSER_SUBMIT_FAIL:
+      ErrorInfo = gSaveFailed;
+      break;
+
+    case BROWSER_FORM_NOT_FOUND:
+      ErrorInfo = gFormNotFound;
+      break;
+
+    case BROWSER_FORM_SUPPRESS:
+      ErrorInfo = gFormSuppress;
+      break;
+
+    case BROWSER_PROTOCOL_NOT_FOUND:
+      ErrorInfo = gProtocolNotFound;
+      break;
+
+    case BROWSER_SUBMIT_FAIL_NO_SUBMIT_IF:
+      ErrorInfo = gNoSubmitIfFailed;
+      break;
+
+    case BROWSER_RECONNECT_FAIL:
+      ErrorInfo = gReconnectFail;
+      break;
+
+    case BROWSER_RECONNECT_SAVE_CHANGES:
+      ErrorInfo = gReconnectConfirmChanges;
+      break;
+
+    case BROWSER_RECONNECT_REQUIRED:
+      ErrorInfo = gReconnectRequired;
+      break;
+
+    default:
+      ErrorInfo = gBrowserError;
+      break;
+    }
+  }
+
+  switch (gFormData->BrowserStatus) {
+  case BROWSER_SUBMIT_FAIL:
+  case BROWSER_SUBMIT_FAIL_NO_SUBMIT_IF:
+  case BROWSER_RECONNECT_SAVE_CHANGES:
+    ASSERT (gUserInput != NULL);
+    if (gFormData->BrowserStatus == (BROWSER_SUBMIT_FAIL)) {
+      PrintString = gSaveProcess;
+      JumpToFormSet = gJumpToFormSet[0];
+      DiscardChange = gDiscardChange[0];
+    } else if (gFormData->BrowserStatus == (BROWSER_RECONNECT_SAVE_CHANGES)){
+      PrintString = gChangesOpt;
+      JumpToFormSet = gConfirmOptYes[0];
+      DiscardChange = gConfirmOptNo[0];
+    } else {
+      PrintString = gSaveNoSubmitProcess;
+      JumpToFormSet = gCheckError[0];
+      DiscardChange = gDiscardChange[0];
+    }
+
+    do {
+      CreateDialog (&Key, gEmptyString, ErrorInfo, PrintString, gEmptyString, NULL);
+    } while (((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (DiscardChange | UPPER_LOWER_CASE_OFFSET)) &&
+             ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) != (JumpToFormSet | UPPER_LOWER_CASE_OFFSET)));
+
+    if ((Key.UnicodeChar | UPPER_LOWER_CASE_OFFSET) == (DiscardChange | UPPER_LOWER_CASE_OFFSET)) {
+      gUserInput->Action = BROWSER_ACTION_DISCARD;
+    } else {
+      gUserInput->Action = BROWSER_ACTION_GOTO;
+    }
+    break;
+
+  default:
+    if (TimeOut == 0) {
+      do {
+        CreateDialog (&Key, gEmptyString, ErrorInfo, gPressEnter, gEmptyString, NULL);
+      } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+    } else {
+      Status = gBS->CreateEvent (EVT_NOTIFY_WAIT, TPL_CALLBACK, EmptyEventProcess, NULL, &TimeOutEvent);
+      ASSERT_EFI_ERROR (Status);
+
+      EventContext.SyncEvent = TimeOutEvent;
+      EventContext.TimeOut   = &TimeOut;
+      EventContext.ErrorInfo = ErrorInfo;
+
+      Status = gBS->CreateEvent (EVT_TIMER | EVT_NOTIFY_SIGNAL, TPL_CALLBACK, RefreshTimeOutProcess, &EventContext, &RefreshIntervalEvent);
+      ASSERT_EFI_ERROR (Status);
+
+      //
+      // Show the dialog first to avoid long time not reaction.
+      //
+      gBS->SignalEvent (RefreshIntervalEvent);
+    
+      Status = gBS->SetTimer (RefreshIntervalEvent, TimerPeriodic, ONE_SECOND);
+      ASSERT_EFI_ERROR (Status);
+
+      while (TRUE) {
+        Status = gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+        if (!EFI_ERROR (Status) && Key.UnicodeChar == CHAR_CARRIAGE_RETURN) {
+          break;
+        }
+
+        if (Status != EFI_NOT_READY) {
+          continue;
+        }
+
+        WaitList[0] = TimeOutEvent;
+        WaitList[1] = gST->ConIn->WaitForKey;
+
+        Status = gBS->WaitForEvent (2, WaitList, &Index);
+        ASSERT_EFI_ERROR (Status);
+
+        if (Index == 0) {
+          //
+          // Timeout occur, close the hoot time out event.
+          //
+          break;
+        }
+      }
+
+      gBS->CloseEvent (TimeOutEvent);
+      gBS->CloseEvent (RefreshIntervalEvent);
+    }
+    break;
+  }
+
+  if (StringToken != 0) {
+    FreePool (ErrorInfo);
+  }
+}
+
+/**
+  Display one form, and return user input.
+  
+  @param FormData                Form Data to be shown.
+  @param UserInputData           User input data.
+  
+  @retval EFI_SUCCESS            1.Form Data is shown, and user input is got.
+                                 2.Error info has show and return.
+  @retval EFI_INVALID_PARAMETER  The input screen dimension is not valid
+  @retval EFI_NOT_FOUND          New form data has some error.
+**/
+EFI_STATUS
+EFIAPI 
+FormDisplay (
+  IN  FORM_DISPLAY_ENGINE_FORM  *FormData,
+  OUT USER_INPUT                *UserInputData
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (FormData != NULL);
+  if (FormData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  gUserInput = UserInputData;
+  gFormData  = FormData;
+
+  //
+  // Process the status info first.
+  //
+  BrowserStatusProcess();
+  if (gFormData->BrowserStatus != BROWSER_SUCCESS) {
+    //
+    // gFormData->BrowserStatus != BROWSER_SUCCESS, means only need to print the error info, return here.
+    //
+    return EFI_SUCCESS;
+  }
+
+  Status = DisplayPageFrame (FormData, &gStatementDimensions);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Global Widths should be initialized before any MenuOption creation
+  // or the GetWidth() used in UiAddMenuOption() will return incorrect value.
+  //
+  //
+  //  Left                                              right
+  //   |<-.->|<-.........->|<- .........->|<-...........->|
+  //     Skip    Prompt         Option         Help 
+  //
+  gOptionBlockWidth = (CHAR16) ((gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn) / 3) + 1;
+  gHelpBlockWidth   = (CHAR16) (gOptionBlockWidth - 1 - LEFT_SKIPPED_COLUMNS);
+  gPromptBlockWidth = (CHAR16) (gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn - 2 * (gOptionBlockWidth - 1) - 1);
+
+  ConvertStatementToMenu();
+
+  //
+  // Check whether layout is changed.
+  //
+  if (mIsFirstForm 
+      || (gOldFormEntry.HiiHandle != FormData->HiiHandle)
+      || (!CompareGuid (&gOldFormEntry.FormSetGuid, &FormData->FormSetGuid))
+      || (gOldFormEntry.FormId != FormData->FormId)) {
+    mStatementLayoutIsChanged = TRUE;
+  } else {
+    mStatementLayoutIsChanged = FALSE;
+  }
+
+  Status = UiDisplayMenu(FormData);
+  
+  //
+  // Backup last form info.
+  //
+  mIsFirstForm            = FALSE;
+  gOldFormEntry.HiiHandle = FormData->HiiHandle;
+  CopyGuid (&gOldFormEntry.FormSetGuid, &FormData->FormSetGuid);
+  gOldFormEntry.FormId    = FormData->FormId;
+
+  return Status;
+}
+
+/**
+  Clear Screen to the initial state.
+**/
+VOID
+EFIAPI 
+DriverClearDisplayPage (
+  VOID
+  )
+{
+  ClearDisplayPage ();
+  mIsFirstForm = TRUE;
+}
+
+/**
+  Set Buffer to Value for Size bytes.
+
+  @param  Buffer                 Memory to set.
+  @param  Size                   Number of bytes to set
+  @param  Value                  Value of the set operation.
+
+**/
+VOID
+SetUnicodeMem (
+  IN VOID   *Buffer,
+  IN UINTN  Size,
+  IN CHAR16 Value
+  )
+{
+  CHAR16  *Ptr;
+
+  Ptr = Buffer;
+  while ((Size--)  != 0) {
+    *(Ptr++) = Value;
+  }
+}
+
+/**
+  Initialize Setup Browser driver.
+
+  @param ImageHandle     The image handle.
+  @param SystemTable     The system table.
+
+  @retval EFI_SUCCESS    The Setup Browser module is initialized correctly..
+  @return Other value if failed to initialize the Setup Browser module.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDisplayEngine (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_INPUT_KEY                       HotKey;
+  EFI_STRING                          NewString;
+  EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL *FormBrowserEx2;
+
+  //
+  // Publish our HII data
+  //
+  gHiiHandle = HiiAddPackages (
+                 &gDisplayEngineGuid,
+                 ImageHandle,
+                 DisplayEngineStrings,
+                 NULL
+                 );
+  ASSERT (gHiiHandle != NULL);
+
+  //
+  // Install Form Display protocol
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mPrivateData.Handle,
+                  &gEdkiiFormDisplayEngineProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mPrivateData.FromDisplayProt
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  InitializeDisplayStrings();
+  
+  ZeroMem (&gHighligthMenuInfo, sizeof (gHighligthMenuInfo));
+  ZeroMem (&gOldFormEntry, sizeof (gOldFormEntry));
+
+  //
+  // Use BrowserEx2 protocol to register HotKey.
+  // 
+  Status = gBS->LocateProtocol (&gEdkiiFormBrowserEx2ProtocolGuid, NULL, (VOID **) &FormBrowserEx2);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Register the default HotKey F9 and F10 again.
+    //
+    HotKey.UnicodeChar = CHAR_NULL;
+    HotKey.ScanCode   = SCAN_F10;
+    NewString         = HiiGetString (gHiiHandle, STRING_TOKEN (FUNCTION_TEN_STRING), NULL);
+    ASSERT (NewString != NULL);
+    FormBrowserEx2->RegisterHotKey (&HotKey, BROWSER_ACTION_SUBMIT, 0, NewString);
+
+    HotKey.ScanCode   = SCAN_F9;
+    NewString         = HiiGetString (gHiiHandle, STRING_TOKEN (FUNCTION_NINE_STRING), NULL);
+    ASSERT (NewString != NULL);
+    FormBrowserEx2->RegisterHotKey (&HotKey, BROWSER_ACTION_DEFAULT, EFI_HII_DEFAULT_CLASS_STANDARD, NewString);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This is the default unload handle for display core drivers.
+
+  @param[in]  ImageHandle       The drivers' driver image.
+
+  @retval EFI_SUCCESS           The image is unloaded.
+  @retval Others                Failed to unload the image.
+
+**/
+EFI_STATUS
+EFIAPI
+UnloadDisplayEngine (
+  IN EFI_HANDLE             ImageHandle
+  )
+{
+  HiiRemovePackages(gHiiHandle);
+
+  FreeDisplayStrings ();
+
+  if (gHighligthMenuInfo.HLTOpCode != NULL) {
+    FreePool (gHighligthMenuInfo.HLTOpCode);
+  }
+
+  if (gHighligthMenuInfo.TOSOpCode != NULL) {
+    FreePool (gHighligthMenuInfo.TOSOpCode);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.h b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.h
new file mode 100644
index 0000000000..063e94c6bc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplay.h
@@ -0,0 +1,652 @@
+/** @file
+  FormDiplay protocol to show Form
+
+Copyright (c) 2013 - 2014, 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 that 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.
+
+**/
+
+#ifndef __FORM_DISPLAY_H__
+#define __FORM_DISPLAY_H__
+
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/BaseLib.h>
+#include <Library/HiiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PrintLib.h>
+#include <Library/CustomizedDisplayLib.h>
+
+#include <Protocol/FormBrowserEx2.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/DisplayProtocol.h>
+
+#include <Guid/MdeModuleHii.h>
+
+//
+// This is the generated header file which includes whatever needs to be exported (strings + IFR)
+//
+extern UINT8  DisplayEngineStrings[];
+extern EFI_SCREEN_DESCRIPTOR         gStatementDimensions;
+extern USER_INPUT                    *gUserInput;
+extern FORM_DISPLAY_ENGINE_FORM      *gFormData;
+extern EFI_HII_HANDLE                gHiiHandle;
+extern UINT16                        gDirection;
+extern LIST_ENTRY                    gMenuOption;
+
+//
+// Browser Global Strings
+//
+extern CHAR16            *gSaveFailed;
+extern CHAR16            *gPromptForData;
+extern CHAR16            *gPromptForPassword;
+extern CHAR16            *gPromptForNewPassword;
+extern CHAR16            *gConfirmPassword;
+extern CHAR16            *gConfirmError;
+extern CHAR16            *gPassowordInvalid;
+extern CHAR16            *gPressEnter;
+extern CHAR16            *gEmptyString;
+extern CHAR16            *gMiniString;
+extern CHAR16            *gOptionMismatch;
+extern CHAR16            *gFormSuppress;
+extern CHAR16            *gProtocolNotFound;
+
+extern CHAR16            gPromptBlockWidth;
+extern CHAR16            gOptionBlockWidth;
+extern CHAR16            gHelpBlockWidth;
+extern CHAR16            *mUnknownString;
+extern BOOLEAN           gMisMatch;
+
+//
+// Screen definitions
+//
+
+#define LEFT_SKIPPED_COLUMNS          3
+#define SCROLL_ARROW_HEIGHT           1
+#define POPUP_PAD_SPACE_COUNT         5
+#define POPUP_FRAME_WIDTH             2
+
+#define UPPER_LOWER_CASE_OFFSET       0x20
+
+//
+// Display definitions
+//
+#define LEFT_ONEOF_DELIMITER      L'<'
+#define RIGHT_ONEOF_DELIMITER     L'>'
+
+#define LEFT_NUMERIC_DELIMITER    L'['
+#define RIGHT_NUMERIC_DELIMITER   L']'
+
+#define LEFT_CHECKBOX_DELIMITER   L'['
+#define RIGHT_CHECKBOX_DELIMITER  L']'
+
+#define CHECK_ON                  L'X'
+#define CHECK_OFF                 L' '
+
+#define TIME_SEPARATOR            L':'
+#define DATE_SEPARATOR            L'/'
+
+#define SUBTITLE_INDENT  2
+
+//
+// This is the Input Error Message
+//
+#define INPUT_ERROR 1
+
+//
+// This is the NV RAM update required Message
+//
+#define NV_UPDATE_REQUIRED  2
+//
+// Time definitions
+//
+#define ONE_SECOND  10000000
+
+//
+// It take 23 characters including the NULL to print a 64 bits number with "[" and "]".
+// pow(2, 64) = [18446744073709551616]
+// with extra '-' flat, set the width to 24.
+//
+#define MAX_NUMERIC_INPUT_WIDTH 24
+
+#define EFI_HII_EXPRESSION_INCONSISTENT_IF   0
+#define EFI_HII_EXPRESSION_NO_SUBMIT_IF      1
+#define EFI_HII_EXPRESSION_GRAY_OUT_IF       2
+#define EFI_HII_EXPRESSION_SUPPRESS_IF       3
+#define EFI_HII_EXPRESSION_DISABLE_IF        4
+
+//
+// Character definitions
+//
+#define CHAR_SPACE              0x0020
+
+#define FORM_DISPLAY_DRIVER_SIGNATURE SIGNATURE_32 ('F', 'D', 'D', 'V')
+typedef struct {
+  UINT32                             Signature;
+
+  EFI_HANDLE                         Handle;
+
+  //
+  // Produced protocol
+  //
+  EDKII_FORM_DISPLAY_ENGINE_PROTOCOL FromDisplayProt;
+} FORM_DISPLAY_DRIVER_PRIVATE_DATA;
+
+
+typedef enum {
+  UiNoOperation,
+  UiSelect,
+  UiUp,
+  UiDown,
+  UiLeft,
+  UiRight,
+  UiReset,
+  UiPrevious,
+  UiPageUp,
+  UiPageDown,
+  UiHotKey,
+  UiMaxOperation
+} UI_SCREEN_OPERATION;
+
+typedef enum {
+  CfInitialization,
+  CfCheckSelection,
+  CfRepaint,
+  CfRefreshHighLight,
+  CfUpdateHelpString,
+  CfPrepareToReadKey,
+  CfReadKey,
+  CfScreenOperation,
+  CfUiSelect,
+  CfUiReset,
+  CfUiLeft,
+  CfUiRight,
+  CfUiUp,
+  CfUiPageUp,
+  CfUiPageDown,
+  CfUiDown,
+  CfUiNoOperation,
+  CfExit,
+  CfUiHotKey,
+  CfMaxControlFlag
+} UI_CONTROL_FLAG;
+
+typedef enum {
+  UIEventNone,
+  UIEventKey,
+  UIEventTimeOut,
+  UIEventDriver
+} UI_EVENT_TYPE;
+
+typedef struct {
+  UINT16              ScanCode;
+  UI_SCREEN_OPERATION ScreenOperation;
+} SCAN_CODE_TO_SCREEN_OPERATION;
+
+typedef struct {
+  UI_SCREEN_OPERATION ScreenOperation;
+  UI_CONTROL_FLAG     ControlFlag;
+} SCREEN_OPERATION_T0_CONTROL_FLAG;
+
+typedef struct {
+  EFI_HII_HANDLE     HiiHandle;
+  UINT16             FormId;
+  
+  //
+  // Info for the highlight question.
+  // HLT means highlight
+  //
+  // If one statement has questionid, save questionid info to find the question.
+  // If one statement not has questionid info, save the opcode info to find the 
+  // statement. If more than one statement has same opcode in one form(just like
+  // empty subtitle info may has more than one info one form), also use Index 
+  // info to find the statement.
+  //
+  EFI_QUESTION_ID    HLTQuestionId;
+  EFI_IFR_OP_HEADER  *HLTOpCode;
+  UINTN              HLTIndex;
+  UINTN              HLTSequence;
+  
+  //
+  // Info for the top of screen question.
+  // TOS means Top Of Screen
+  //
+  EFI_QUESTION_ID    TOSQuestionId;
+  EFI_IFR_OP_HEADER  *TOSOpCode;
+  UINTN              TOSIndex;
+
+  UINT16             SkipValue;
+} DISPLAY_HIGHLIGHT_MENU_INFO;
+
+typedef struct {
+  EFI_EVENT   SyncEvent;
+  UINT8       *TimeOut;
+  CHAR16      *ErrorInfo;
+} WARNING_IF_CONTEXT;
+
+#define UI_MENU_OPTION_SIGNATURE  SIGNATURE_32 ('u', 'i', 'm', 'm')
+
+typedef struct {
+  UINTN                   Signature;
+  LIST_ENTRY              Link;
+
+  EFI_HII_HANDLE          Handle;
+  FORM_DISPLAY_ENGINE_STATEMENT  *ThisTag;
+  UINT16                  EntryNumber;
+
+  UINTN                   Row;
+  UINTN                   Col;
+  UINTN                   OptCol;
+  CHAR16                  *Description;
+  UINTN                   Skip;           // Number of lines
+
+  //
+  // Display item sequence for date/time
+  //  Date:      Month/Day/Year
+  //  Sequence:  0     1   2
+  //
+  //  Time:      Hour : Minute : Second
+  //  Sequence:  0      1        2
+  //
+  //
+  UINTN                   Sequence;
+
+  BOOLEAN                 GrayOut;
+  BOOLEAN                 ReadOnly;
+
+  //
+  // Whether user could change value of this item
+  //
+  BOOLEAN                 IsQuestion;
+  BOOLEAN                 NestInStatement;
+} UI_MENU_OPTION;
+
+#define MENU_OPTION_FROM_LINK(a)  CR (a, UI_MENU_OPTION, Link, UI_MENU_OPTION_SIGNATURE)
+
+/**
+  Print Question Value according to it's storage width and display attributes.
+
+  @param  Question               The Question to be printed.
+  @param  FormattedNumber        Buffer for output string.
+  @param  BufferSize             The FormattedNumber buffer size in bytes.
+
+  @retval EFI_SUCCESS            Print success.
+  @retval EFI_BUFFER_TOO_SMALL   Buffer size is not enough for formatted number.
+
+**/
+EFI_STATUS
+PrintFormattedNumber (
+  IN FORM_DISPLAY_ENGINE_STATEMENT   *Question,
+  IN OUT CHAR16               *FormattedNumber,
+  IN UINTN                    BufferSize
+  );
+
+/**
+  Set value of a data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+  @param  Value                  The value to be set.
+
+**/
+VOID
+SetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index,
+  IN UINT64                   Value
+  );
+
+/**
+  Return data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+
+  @retval Value                  The data to be returned
+
+**/
+UINT64
+GetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index
+  );
+  
+/**
+  Search an Option of a Question by its value.
+
+  @param  Question               The Question
+  @param  OptionValue            Value for Option to be searched.
+
+  @retval Pointer                Pointer to the found Option.
+  @retval NULL                   Option not found.
+
+**/
+DISPLAY_QUESTION_OPTION *
+ValueToOption (
+  IN FORM_DISPLAY_ENGINE_STATEMENT   *Question,
+  IN EFI_HII_VALUE                   *OptionValue
+  );
+
+/**
+  Compare two Hii value.
+
+  @param  Value1                 Expression value to compare on left-hand.
+  @param  Value2                 Expression value to compare on right-hand.
+  @param  Result                 Return value after compare.
+                                 retval 0                      Two operators equal.
+                                 return Positive value if Value1 is greater than Value2.
+                                 retval Negative value if Value1 is less than Value2.
+  @param  HiiHandle              Only required for string compare.
+
+  @retval other                  Could not perform compare on two values.
+  @retval EFI_SUCCESS            Compare the value success.
+
+**/
+EFI_STATUS
+CompareHiiValue (
+  IN  EFI_HII_VALUE   *Value1,
+  IN  EFI_HII_VALUE   *Value2,
+  OUT INTN            *Result,
+  IN  EFI_HII_HANDLE  HiiHandle OPTIONAL
+  );
+
+/**
+  Draw a pop up windows based on the dimension, number of lines and
+  strings specified.
+
+  @param RequestedWidth  The width of the pop-up.
+  @param NumberOfLines   The number of lines.
+  @param ...             A series of text strings that displayed in the pop-up.
+
+**/
+VOID
+EFIAPI
+CreateMultiStringPopUp (
+  IN  UINTN                       RequestedWidth,
+  IN  UINTN                       NumberOfLines,
+  ...
+  );
+
+/**
+  Will copy LineWidth amount of a string in the OutputString buffer and return the
+  number of CHAR16 characters that were copied into the OutputString buffer.
+  The output string format is:
+    Glyph Info + String info + '\0'.
+
+  In the code, it deals \r,\n,\r\n same as \n\r, also it not process the \r or \g.
+
+  @param  InputString            String description for this option.
+  @param  LineWidth              Width of the desired string to extract in CHAR16
+                                 characters
+  @param  GlyphWidth             The glyph width of the begin of the char in the string.
+  @param  Index                  Where in InputString to start the copy process
+  @param  OutputString           Buffer to copy the string into
+
+  @return Returns the number of CHAR16 characters that were copied into the OutputString 
+  buffer, include extra glyph info and '\0' info.
+
+**/
+UINT16
+GetLineByWidth (
+  IN      CHAR16                      *InputString,
+  IN      UINT16                      LineWidth,
+  IN OUT  UINT16                      *GlyphWidth,
+  IN OUT  UINTN                       *Index,
+  OUT     CHAR16                      **OutputString
+  );
+
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The Hii handle for this string package.
+
+  @return The output string.
+
+**/
+CHAR16 *
+GetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  );
+  
+/**
+  Count the storage space of a Unicode string.
+
+  This function handles the Unicode string with NARROW_CHAR
+  and WIDE_CHAR control characters. NARROW_HCAR and WIDE_CHAR
+  does not count in the resultant output. If a WIDE_CHAR is
+  hit, then 2 Unicode character will consume an output storage
+  space with size of CHAR16 till a NARROW_CHAR is hit.
+
+  If String is NULL, then ASSERT ().
+
+  @param String          The input string to be counted.
+
+  @return Storage space for the input string.
+
+**/
+UINTN
+GetStringWidth (
+  IN CHAR16               *String
+  );
+
+/**
+  This routine reads a numeric value from the user input.
+
+  @param  MenuOption        Pointer to the current input menu.
+
+  @retval EFI_SUCCESS       If numerical input is read successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+GetNumericInput (
+  IN  UI_MENU_OPTION              *MenuOption
+  );
+
+/**
+  Get string or password input from user.
+
+  @param  MenuOption        Pointer to the current input menu.
+  @param  Prompt            The prompt string shown on popup window.
+  @param  StringPtr         Old user input and destination for use input string.
+
+  @retval EFI_SUCCESS       If string input is read successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+ReadString (
+  IN     UI_MENU_OPTION              *MenuOption,
+  IN     CHAR16                      *Prompt,
+  IN OUT CHAR16                      *StringPtr
+  );
+
+/**
+  Draw a pop up windows based on the dimension, number of lines and
+  strings specified.
+
+  @param RequestedWidth  The width of the pop-up.
+  @param NumberOfLines   The number of lines.
+  @param Marker          The variable argument list for the list of string to be printed.
+
+**/
+VOID
+CreateSharedPopUp (
+  IN  UINTN                       RequestedWidth,
+  IN  UINTN                       NumberOfLines,
+  IN  VA_LIST                     Marker
+  );
+  
+/**
+  Wait for a key to be pressed by user.
+
+  @param Key         The key which is pressed by user.
+
+  @retval EFI_SUCCESS The function always completed successfully.
+
+**/
+EFI_STATUS
+WaitForKeyStroke (
+  OUT  EFI_INPUT_KEY           *Key
+  );
+
+/**
+  Get selection for OneOf and OrderedList (Left/Right will be ignored).
+
+  @param  MenuOption        Pointer to the current input menu.
+
+  @retval EFI_SUCCESS       If Option input is processed successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+GetSelectionInputPopUp (
+  IN  UI_MENU_OPTION              *MenuOption
+  );
+
+/**
+  Process the help string: Split StringPtr to several lines of strings stored in
+  FormattedString and the glyph width of each line cannot exceed gHelpBlockWidth.
+
+  @param  StringPtr              The entire help string.
+  @param  FormattedString        The oupput formatted string.
+  @param  EachLineWidth          The max string length of each line in the formatted string.
+  @param  RowCount               TRUE: if Question is selected.
+
+**/
+UINTN
+ProcessHelpString (
+  IN  CHAR16  *StringPtr,
+  OUT CHAR16  **FormattedString,
+  OUT UINT16  *EachLineWidth,
+  IN  UINTN   RowCount
+  );
+
+/**
+  Process a Question's Option (whether selected or un-selected).
+
+  @param  MenuOption             The MenuOption for this Question.
+  @param  Selected               TRUE: if Question is selected.
+  @param  OptionString           Pointer of the Option String to be displayed.
+  @param  SkipErrorValue         Whether need to return when value without option for it.
+
+  @retval EFI_SUCCESS            Question Option process success.
+  @retval Other                  Question Option process fail.
+
+**/
+EFI_STATUS
+ProcessOptions (
+  IN  UI_MENU_OPTION              *MenuOption,
+  IN  BOOLEAN                     Selected,
+  OUT CHAR16                      **OptionString,
+  IN  BOOLEAN                     SkipErrorValue
+  );
+
+/**
+  Set Buffer to Value for Size bytes.
+
+  @param  Buffer                 Memory to set.
+  @param  Size                   Number of bytes to set
+  @param  Value                  Value of the set operation.
+
+**/
+VOID
+SetUnicodeMem (
+  IN VOID   *Buffer,
+  IN UINTN  Size,
+  IN CHAR16 Value
+  );
+
+/**
+  Display one form, and return user input.
+  
+  @param FormData                Form Data to be shown.
+  @param UserInputData           User input data.
+  
+  @retval EFI_SUCCESS            Form Data is shown, and user input is got.
+**/
+EFI_STATUS
+EFIAPI 
+FormDisplay (
+  IN  FORM_DISPLAY_ENGINE_FORM  *FormData,
+  OUT USER_INPUT                *UserInputData
+  );
+
+/**
+  Clear Screen to the initial state.
+**/
+VOID
+EFIAPI 
+DriverClearDisplayPage (
+  VOID
+  );
+
+/**
+  Exit Display and Clear Screen to the original state.
+
+**/
+VOID
+EFIAPI 
+ExitDisplay (
+  VOID
+  );
+
+/**
+  Process nothing.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+EmptyEventProcess (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  );
+
+/**
+  Process for the refresh interval statement.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+RefreshTimeOutProcess (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  );
+
+/**
+  Record the highlight menu and top of screen menu info.
+
+  @param  Highlight               The menu opton which is highlight.
+  @param  TopOfScreen             The menu opton which is at the top of the form.
+  @param  SkipValue               The skip line info for the top of screen menu.
+
+**/
+VOID
+UpdateHighlightMenuInfo (
+  IN  LIST_ENTRY                      *Highlight,
+  IN  LIST_ENTRY                      *TopOfScreen,
+  IN  UINTN                           SkipValue
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplayStr.uni b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplayStr.uni
new file mode 100644
index 0000000000..ff94518b06
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/FormDisplayStr.uni
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/InputHandler.c b/Core/MdeModulePkg/Universal/DisplayEngineDxe/InputHandler.c
new file mode 100644
index 0000000000..bbbbdaa8c1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/InputHandler.c
@@ -0,0 +1,1673 @@
+/** @file
+Implementation for handling user input from the User Interfaces.
+
+Copyright (c) 2004 - 2015, 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 "FormDisplay.h"
+
+/**
+  Get maximum and minimum info from this opcode.
+
+  @param  OpCode            Pointer to the current input opcode.
+  @param  Minimum           The minimum size info for this opcode.
+  @param  Maximum           The maximum size info for this opcode.
+
+**/
+VOID
+GetFieldFromOp (
+  IN   EFI_IFR_OP_HEADER       *OpCode,
+  OUT  UINTN                   *Minimum,
+  OUT  UINTN                   *Maximum
+  )
+{
+  EFI_IFR_STRING    *StringOp;
+  EFI_IFR_PASSWORD  *PasswordOp;
+  if (OpCode->OpCode == EFI_IFR_STRING_OP) {
+    StringOp = (EFI_IFR_STRING *) OpCode;
+    *Minimum = StringOp->MinSize;
+    *Maximum = StringOp->MaxSize;    
+  } else if (OpCode->OpCode == EFI_IFR_PASSWORD_OP) {
+    PasswordOp = (EFI_IFR_PASSWORD *) OpCode;
+    *Minimum = PasswordOp->MinSize;
+    *Maximum = PasswordOp->MaxSize;       
+  } else {
+    *Minimum = 0;
+    *Maximum = 0;       
+  }
+}
+
+/**
+  Get string or password input from user.
+
+  @param  MenuOption        Pointer to the current input menu.
+  @param  Prompt            The prompt string shown on popup window.
+  @param  StringPtr         Old user input and destination for use input string.
+
+  @retval EFI_SUCCESS       If string input is read successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+ReadString (
+  IN     UI_MENU_OPTION              *MenuOption,
+  IN     CHAR16                      *Prompt,
+  IN OUT CHAR16                      *StringPtr
+  )
+{
+  EFI_STATUS              Status;
+  EFI_INPUT_KEY           Key;
+  CHAR16                  NullCharacter;
+  UINTN                   ScreenSize;
+  CHAR16                  Space[2];
+  CHAR16                  KeyPad[2];
+  CHAR16                  *TempString;
+  CHAR16                  *BufferedString;
+  UINTN                   Index;
+  UINTN                   Index2;
+  UINTN                   Count;
+  UINTN                   Start;
+  UINTN                   Top;
+  UINTN                   DimensionsWidth;
+  UINTN                   DimensionsHeight;
+  UINTN                   CurrentCursor;
+  BOOLEAN                 CursorVisible;
+  UINTN                   Minimum;
+  UINTN                   Maximum;
+  FORM_DISPLAY_ENGINE_STATEMENT  *Question;
+  BOOLEAN                 IsPassword;
+  UINTN                   MaxLen;
+
+  DimensionsWidth  = gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn;
+  DimensionsHeight = gStatementDimensions.BottomRow - gStatementDimensions.TopRow;
+
+  NullCharacter    = CHAR_NULL;
+  ScreenSize       = GetStringWidth (Prompt) / sizeof (CHAR16);
+  Space[0]         = L' ';
+  Space[1]         = CHAR_NULL;
+
+  Question         = MenuOption->ThisTag;
+  GetFieldFromOp(Question->OpCode, &Minimum, &Maximum);
+
+  if (Question->OpCode->OpCode == EFI_IFR_PASSWORD_OP) {
+    IsPassword = TRUE;
+  } else {
+    IsPassword = FALSE;
+  }
+
+  MaxLen = Maximum + 1;
+  TempString = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  ASSERT (TempString);
+
+  if (ScreenSize < (Maximum + 1)) {
+    ScreenSize = Maximum + 1;
+  }
+
+  if ((ScreenSize + 2) > DimensionsWidth) {
+    ScreenSize = DimensionsWidth - 2;
+  }
+
+  BufferedString = AllocateZeroPool (ScreenSize * 2);
+  ASSERT (BufferedString);
+
+  Start = (DimensionsWidth - ScreenSize - 2) / 2 + gStatementDimensions.LeftColumn + 1;
+  Top   = ((DimensionsHeight - 6) / 2) + gStatementDimensions.TopRow - 1;
+
+  //
+  // Display prompt for string
+  //
+  // CreateDialog (NULL, "", Prompt, Space, "", NULL);
+  CreateMultiStringPopUp (ScreenSize, 4, &NullCharacter, Prompt, Space, &NullCharacter);
+  gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_BLACK, EFI_LIGHTGRAY));
+
+  CursorVisible = gST->ConOut->Mode->CursorVisible;
+  gST->ConOut->EnableCursor (gST->ConOut, TRUE);
+
+  CurrentCursor = GetStringWidth (StringPtr) / 2 - 1;
+  if (CurrentCursor != 0) {
+    //
+    // Show the string which has beed saved before.
+    //
+    SetUnicodeMem (BufferedString, ScreenSize - 1, L' ');
+    PrintStringAt (Start + 1, Top + 3, BufferedString);
+
+    if ((GetStringWidth (StringPtr) / 2) > (DimensionsWidth - 2)) {
+      Index = (GetStringWidth (StringPtr) / 2) - DimensionsWidth + 2;
+    } else {
+      Index = 0;
+    }
+
+    if (IsPassword) {
+      gST->ConOut->SetCursorPosition (gST->ConOut, Start + 1, Top + 3);
+    }
+
+    for (Count = 0; Index + 1 < GetStringWidth (StringPtr) / 2; Index++, Count++) {
+      BufferedString[Count] = StringPtr[Index];
+
+      if (IsPassword) {
+        PrintCharAt ((UINTN)-1, (UINTN)-1, L'*');
+      }
+    }
+
+    if (!IsPassword) {
+      PrintStringAt (Start + 1, Top + 3, BufferedString);
+    }
+    
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+    gST->ConOut->SetCursorPosition (gST->ConOut, Start + GetStringWidth (StringPtr) / 2, Top + 3);
+  }
+  
+  do {
+    Status = WaitForKeyStroke (&Key);
+    ASSERT_EFI_ERROR (Status);
+
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_BLACK, EFI_LIGHTGRAY));
+    switch (Key.UnicodeChar) {
+    case CHAR_NULL:
+      switch (Key.ScanCode) {
+      case SCAN_LEFT:
+        if (CurrentCursor > 0) {
+          CurrentCursor--;
+        }
+        break;
+
+      case SCAN_RIGHT:
+        if (CurrentCursor < (GetStringWidth (StringPtr) / 2 - 1)) {
+          CurrentCursor++;
+        }
+        break;
+
+      case SCAN_ESC:
+        FreePool (TempString);
+        FreePool (BufferedString);
+        gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+        gST->ConOut->EnableCursor (gST->ConOut, CursorVisible);
+        return EFI_DEVICE_ERROR;
+
+       case SCAN_DELETE:
+        for (Index = CurrentCursor; StringPtr[Index] != CHAR_NULL; Index++) {
+          StringPtr[Index] = StringPtr[Index + 1];
+          PrintCharAt (Start + Index + 1, Top + 3, IsPassword && StringPtr[Index] != CHAR_NULL? L'*' : StringPtr[Index]);
+        }
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case CHAR_CARRIAGE_RETURN:
+      if (GetStringWidth (StringPtr) >= ((Minimum + 1) * sizeof (CHAR16))) {
+
+        FreePool (TempString);
+        FreePool (BufferedString);
+        gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+        gST->ConOut->EnableCursor (gST->ConOut, CursorVisible);
+        return EFI_SUCCESS;
+      } else {
+        //
+        // Simply create a popup to tell the user that they had typed in too few characters.
+        // To save code space, we can then treat this as an error and return back to the menu.
+        //
+        do {
+          CreateDialog (&Key, &NullCharacter, gMiniString, gPressEnter, &NullCharacter, NULL);
+        } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+        FreePool (TempString);
+        FreePool (BufferedString);
+        gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+        gST->ConOut->EnableCursor (gST->ConOut, CursorVisible);
+        return EFI_DEVICE_ERROR;
+      }
+
+      break;
+
+    case CHAR_BACKSPACE:
+      if (StringPtr[0] != CHAR_NULL && CurrentCursor != 0) {
+        for (Index = 0; Index < CurrentCursor - 1; Index++) {
+          TempString[Index] = StringPtr[Index];
+        }
+        Count = GetStringWidth (StringPtr) / 2 - 1;
+        if (Count >= CurrentCursor) {
+          for (Index = CurrentCursor - 1, Index2 = CurrentCursor; Index2 < Count; Index++, Index2++) {
+            TempString[Index] = StringPtr[Index2];
+          }
+          TempString[Index] = CHAR_NULL;
+        }
+        //
+        // Effectively truncate string by 1 character
+        //
+        StrCpyS (StringPtr, MaxLen, TempString);
+        CurrentCursor --;
+      }
+
+    default:
+      //
+      // If it is the beginning of the string, don't worry about checking maximum limits
+      //
+      if ((StringPtr[0] == CHAR_NULL) && (Key.UnicodeChar != CHAR_BACKSPACE)) {
+        StrnCpyS (StringPtr, MaxLen, &Key.UnicodeChar, 1);
+        CurrentCursor++;
+      } else if ((GetStringWidth (StringPtr) < ((Maximum + 1) * sizeof (CHAR16))) && (Key.UnicodeChar != CHAR_BACKSPACE)) {
+        KeyPad[0] = Key.UnicodeChar;
+        KeyPad[1] = CHAR_NULL;
+        Count = GetStringWidth (StringPtr) / 2 - 1;
+        if (CurrentCursor < Count) {
+          for (Index = 0; Index < CurrentCursor; Index++) {
+            TempString[Index] = StringPtr[Index];
+          }
+		  TempString[Index] = CHAR_NULL;
+          StrCatS (TempString, MaxLen, KeyPad);
+          StrCatS (TempString, MaxLen, StringPtr + CurrentCursor);
+          StrCpyS (StringPtr, MaxLen, TempString);
+        } else {
+          StrCatS (StringPtr, MaxLen, KeyPad);
+        }
+        CurrentCursor++;
+      }
+
+      //
+      // If the width of the input string is now larger than the screen, we nee to
+      // adjust the index to start printing portions of the string
+      //
+      SetUnicodeMem (BufferedString, ScreenSize - 1, L' ');
+      PrintStringAt (Start + 1, Top + 3, BufferedString);
+
+      if ((GetStringWidth (StringPtr) / 2) > (DimensionsWidth - 2)) {
+        Index = (GetStringWidth (StringPtr) / 2) - DimensionsWidth + 2;
+      } else {
+        Index = 0;
+      }
+
+      if (IsPassword) {
+        gST->ConOut->SetCursorPosition (gST->ConOut, Start + 1, Top + 3);
+      }
+
+      for (Count = 0; Index + 1 < GetStringWidth (StringPtr) / 2; Index++, Count++) {
+        BufferedString[Count] = StringPtr[Index];
+
+        if (IsPassword) {
+          PrintCharAt ((UINTN)-1, (UINTN)-1, L'*');
+        }
+      }
+
+      if (!IsPassword) {
+        PrintStringAt (Start + 1, Top + 3, BufferedString);
+      }
+      break;
+    }
+
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+    gST->ConOut->SetCursorPosition (gST->ConOut, Start + CurrentCursor + 1, Top + 3);
+  } while (TRUE);
+
+}
+
+/**
+  Adjust the value to the correct one. Rules follow the sample:
+  like:  Year change:  2012.02.29 -> 2013.02.29 -> 2013.02.01
+         Month change: 2013.03.29 -> 2013.02.29 -> 2013.02.28
+
+  @param  QuestionValue     Pointer to current question.
+  @param  Sequence          The sequence of the field in the question.
+**/
+VOID
+AdjustQuestionValue (
+  IN  EFI_HII_VALUE           *QuestionValue,
+  IN  UINT8                   Sequence
+  )
+{
+  UINT8     Month;
+  UINT16    Year;
+  UINT8     Maximum;
+  UINT8     Minimum;
+
+  Month   = QuestionValue->Value.date.Month;
+  Year    = QuestionValue->Value.date.Year;
+  Minimum = 1;
+
+  switch (Month) {
+  case 2:
+    if ((Year % 4) == 0 && ((Year % 100) != 0 || (Year % 400) == 0)) {
+      Maximum = 29;
+    } else {
+      Maximum = 28;
+    }
+    break;
+  case 4:
+  case 6:
+  case 9:
+  case 11:
+    Maximum = 30;
+    break;
+  default:
+    Maximum = 31;
+    break;
+  }
+
+  //
+  // Change the month area.
+  //
+  if (Sequence == 0) {
+    if (QuestionValue->Value.date.Day > Maximum) {
+      QuestionValue->Value.date.Day = Maximum;
+    }
+  }
+  
+  //
+  // Change the Year area.
+  //
+  if (Sequence == 2) {
+    if (QuestionValue->Value.date.Day > Maximum) {
+      QuestionValue->Value.date.Day = Minimum;
+    }
+  }
+}
+
+/**
+  Get field info from numeric opcode.
+
+  @param  OpCode            Pointer to the current input opcode.
+  @param  IntInput          Whether question shows with EFI_IFR_DISPLAY_INT_DEC type.
+  @param  QuestionValue     Input question value, with EFI_HII_VALUE type.
+  @param  Value             Return question value, always return UINT64 type.
+  @param  Minimum           The minimum size info for this opcode.
+  @param  Maximum           The maximum size info for this opcode.
+  @param  Step              The step size info for this opcode.
+  @param  StorageWidth      The storage width info for this opcode.
+
+**/
+VOID
+GetValueFromNum (
+  IN  EFI_IFR_OP_HEADER     *OpCode,
+  IN  BOOLEAN               IntInput,
+  IN  EFI_HII_VALUE         *QuestionValue,
+  OUT UINT64                *Value,
+  OUT UINT64                *Minimum,
+  OUT UINT64                *Maximum,
+  OUT UINT64                *Step,
+  OUT UINT16                *StorageWidth
+)
+{
+  EFI_IFR_NUMERIC       *NumericOp;
+
+  NumericOp = (EFI_IFR_NUMERIC *) OpCode;
+  
+  switch (NumericOp->Flags & EFI_IFR_NUMERIC_SIZE) {
+  case EFI_IFR_NUMERIC_SIZE_1:
+    if (IntInput) {
+      *Minimum = (INT64) (INT8) NumericOp->data.u8.MinValue;
+      *Maximum = (INT64) (INT8) NumericOp->data.u8.MaxValue;
+      *Value   = (INT64) (INT8) QuestionValue->Value.u8;
+    } else {
+      *Minimum = NumericOp->data.u8.MinValue;
+      *Maximum = NumericOp->data.u8.MaxValue;
+      *Value   = QuestionValue->Value.u8;
+    }
+    *Step    = NumericOp->data.u8.Step;
+    *StorageWidth = (UINT16) sizeof (UINT8);
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_2:
+    if (IntInput) {
+      *Minimum = (INT64) (INT16) NumericOp->data.u16.MinValue;
+      *Maximum = (INT64) (INT16) NumericOp->data.u16.MaxValue;
+      *Value   = (INT64) (INT16) QuestionValue->Value.u16;
+    } else {
+      *Minimum = NumericOp->data.u16.MinValue;
+      *Maximum = NumericOp->data.u16.MaxValue;
+      *Value   = QuestionValue->Value.u16;
+    }
+    *Step    = NumericOp->data.u16.Step;
+    *StorageWidth = (UINT16) sizeof (UINT16);
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_4:
+    if (IntInput) {
+      *Minimum = (INT64) (INT32) NumericOp->data.u32.MinValue;
+      *Maximum = (INT64) (INT32) NumericOp->data.u32.MaxValue;
+      *Value   = (INT64) (INT32) QuestionValue->Value.u32;
+    } else {
+      *Minimum = NumericOp->data.u32.MinValue;
+      *Maximum = NumericOp->data.u32.MaxValue;
+      *Value   = QuestionValue->Value.u32;
+    }
+    *Step    = NumericOp->data.u32.Step;
+    *StorageWidth = (UINT16) sizeof (UINT32);
+    break;
+  
+  case EFI_IFR_NUMERIC_SIZE_8:
+    if (IntInput) {
+      *Minimum = (INT64) NumericOp->data.u64.MinValue;
+      *Maximum = (INT64) NumericOp->data.u64.MaxValue;
+      *Value   = (INT64) QuestionValue->Value.u64;
+    } else {
+      *Minimum = NumericOp->data.u64.MinValue;
+      *Maximum = NumericOp->data.u64.MaxValue;
+      *Value   = QuestionValue->Value.u64;
+    }
+    *Step    = NumericOp->data.u64.Step;
+    *StorageWidth = (UINT16) sizeof (UINT64);
+    break;
+  
+  default:
+    break;
+  }
+
+  if (*Maximum == 0) {
+    *Maximum = (UINT64) -1;
+  }
+}
+
+/**
+  This routine reads a numeric value from the user input.
+
+  @param  MenuOption        Pointer to the current input menu.
+
+  @retval EFI_SUCCESS       If numerical input is read successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+GetNumericInput (
+  IN  UI_MENU_OPTION              *MenuOption
+  )
+{
+  UINTN                   Column;
+  UINTN                   Row;
+  CHAR16                  InputText[MAX_NUMERIC_INPUT_WIDTH];
+  CHAR16                  FormattedNumber[MAX_NUMERIC_INPUT_WIDTH - 1];
+  UINT64                  PreviousNumber[MAX_NUMERIC_INPUT_WIDTH - 3];
+  UINTN                   Count;
+  UINTN                   Loop;
+  BOOLEAN                 ManualInput;
+  BOOLEAN                 HexInput;
+  BOOLEAN                 IntInput;
+  BOOLEAN                 Negative;
+  BOOLEAN                 ValidateFail;
+  BOOLEAN                 DateOrTime;
+  UINTN                   InputWidth;
+  UINT64                  EditValue;
+  UINT64                  Step;
+  UINT64                  Minimum;
+  UINT64                  Maximum;
+  UINTN                   EraseLen;
+  UINT8                   Digital;
+  EFI_INPUT_KEY           Key;
+  EFI_HII_VALUE           *QuestionValue;
+  FORM_DISPLAY_ENGINE_STATEMENT  *Question;
+  EFI_IFR_NUMERIC                *NumericOp;
+  UINT16                         StorageWidth;
+
+  Column            = MenuOption->OptCol;
+  Row               = MenuOption->Row;
+  PreviousNumber[0] = 0;
+  Count             = 0;
+  InputWidth        = 0;
+  Digital           = 0;
+  StorageWidth      = 0;
+  Minimum           = 0;
+  Maximum           = 0;
+  NumericOp         = NULL;
+  IntInput          = FALSE;
+  HexInput          = FALSE;
+  Negative          = FALSE;
+  ValidateFail      = FALSE;
+
+  Question      = MenuOption->ThisTag;
+  QuestionValue = &Question->CurrentValue;
+
+  //
+  // Only two case, user can enter to this function: Enter and +/- case.
+  // In Enter case, gDirection = 0; in +/- case, gDirection = SCAN_LEFT/SCAN_WRIGHT
+  //
+  ManualInput        = (BOOLEAN)(gDirection == 0 ? TRUE : FALSE);
+
+  if ((Question->OpCode->OpCode == EFI_IFR_DATE_OP) || (Question->OpCode->OpCode == EFI_IFR_TIME_OP)) {
+    DateOrTime = TRUE;
+  } else {
+    DateOrTime = FALSE;
+  }
+
+  //
+  // Prepare Value to be edit
+  //
+  EraseLen = 0;
+  EditValue = 0;
+  if (Question->OpCode->OpCode == EFI_IFR_DATE_OP) {
+    Step = 1;
+    Minimum = 1;
+
+    switch (MenuOption->Sequence) {
+    case 0:
+      Maximum = 12;
+      EraseLen = 4;
+      EditValue = QuestionValue->Value.date.Month;
+      break;
+
+    case 1:
+      switch (QuestionValue->Value.date.Month) {
+      case 2:
+        if ((QuestionValue->Value.date.Year % 4) == 0  && 
+            ((QuestionValue->Value.date.Year % 100) != 0 || 
+            (QuestionValue->Value.date.Year % 400) == 0)) {
+          Maximum = 29;
+        } else {
+          Maximum = 28;
+        }
+        break;
+      case 4:
+      case 6:
+      case 9:
+      case 11:
+        Maximum = 30;
+        break;
+      default:
+        Maximum = 31;
+        break;
+      } 
+
+      EraseLen = 3;
+      EditValue = QuestionValue->Value.date.Day;
+      break;
+
+    case 2:
+      Maximum = 0xffff;
+      EraseLen = 5;
+      EditValue = QuestionValue->Value.date.Year;
+      break;
+
+    default:
+      break;
+    }
+  } else if (Question->OpCode->OpCode == EFI_IFR_TIME_OP) {
+    Step = 1;
+    Minimum = 0;
+
+    switch (MenuOption->Sequence) {
+    case 0:
+      Maximum = 23;
+      EraseLen = 4;
+      EditValue = QuestionValue->Value.time.Hour;
+      break;
+
+    case 1:
+      Maximum = 59;
+      EraseLen = 3;
+      EditValue = QuestionValue->Value.time.Minute;
+      break;
+
+    case 2:
+      Maximum = 59;
+      EraseLen = 3;
+      EditValue = QuestionValue->Value.time.Second;
+      break;
+
+    default:
+      break;
+    }
+  } else {
+    ASSERT (Question->OpCode->OpCode == EFI_IFR_NUMERIC_OP);
+    NumericOp = (EFI_IFR_NUMERIC *) Question->OpCode;
+    GetValueFromNum(Question->OpCode, (NumericOp->Flags & EFI_IFR_DISPLAY) == 0, QuestionValue, &EditValue, &Minimum, &Maximum, &Step, &StorageWidth);
+    EraseLen  = gOptionBlockWidth;
+  }
+
+  if ((Question->OpCode->OpCode == EFI_IFR_NUMERIC_OP) && (NumericOp != NULL)) {
+    if ((NumericOp->Flags & EFI_IFR_DISPLAY) == EFI_IFR_DISPLAY_UINT_HEX){
+      HexInput = TRUE;
+    } else if ((NumericOp->Flags & EFI_IFR_DISPLAY) == 0){
+      //
+      // Display with EFI_IFR_DISPLAY_INT_DEC type. Support negative number.
+      //
+      IntInput = TRUE;
+    }
+  }
+
+  //
+  // Enter from "Enter" input, clear the old word showing.
+  //
+  if (ManualInput) {
+    if (Question->OpCode->OpCode == EFI_IFR_NUMERIC_OP) {
+      if (HexInput) {
+        InputWidth = StorageWidth * 2;
+      } else {
+        switch (StorageWidth) {
+        case 1:
+          InputWidth = 3;
+          break;
+
+        case 2:
+          InputWidth = 5;
+          break;
+
+        case 4:
+          InputWidth = 10;
+          break;
+
+        case 8:
+          InputWidth = 20;
+          break;
+
+        default:
+          InputWidth = 0;
+          break;
+        }
+
+        if (IntInput) {
+          //
+          // Support an extra '-' for negative number.
+          //
+          InputWidth += 1;
+        }
+      }
+
+      InputText[0] = LEFT_NUMERIC_DELIMITER;
+      SetUnicodeMem (InputText + 1, InputWidth, L' ');
+      ASSERT (InputWidth + 2 < MAX_NUMERIC_INPUT_WIDTH);
+      InputText[InputWidth + 1] = RIGHT_NUMERIC_DELIMITER;
+      InputText[InputWidth + 2] = L'\0';
+
+      PrintStringAt (Column, Row, InputText);
+      Column++;
+    }
+
+    if (Question->OpCode->OpCode == EFI_IFR_DATE_OP) {
+      if (MenuOption->Sequence == 2) {
+        InputWidth = 4;
+      } else {
+        InputWidth = 2;
+      }
+
+      if (MenuOption->Sequence == 0) {
+        InputText[0] = LEFT_NUMERIC_DELIMITER;
+        SetUnicodeMem (InputText + 1, InputWidth, L' ');
+      } else {
+        SetUnicodeMem (InputText, InputWidth, L' ');
+      }
+
+      if (MenuOption->Sequence == 2) {
+        InputText[InputWidth + 1] = RIGHT_NUMERIC_DELIMITER;
+      } else {
+        InputText[InputWidth + 1] = DATE_SEPARATOR;
+      }
+      InputText[InputWidth + 2] = L'\0';
+
+      PrintStringAt (Column, Row, InputText);
+      if (MenuOption->Sequence == 0) {
+        Column++;
+      }
+    }
+
+    if (Question->OpCode->OpCode == EFI_IFR_TIME_OP) {
+      InputWidth = 2;
+
+      if (MenuOption->Sequence == 0) {
+        InputText[0] = LEFT_NUMERIC_DELIMITER;
+        SetUnicodeMem (InputText + 1, InputWidth, L' ');
+      } else {
+        SetUnicodeMem (InputText, InputWidth, L' ');
+      }
+
+      if (MenuOption->Sequence == 2) {
+        InputText[InputWidth + 1] = RIGHT_NUMERIC_DELIMITER;
+      } else {
+        InputText[InputWidth + 1] = TIME_SEPARATOR;
+      }
+      InputText[InputWidth + 2] = L'\0';
+
+      PrintStringAt (Column, Row, InputText);
+      if (MenuOption->Sequence == 0) {
+        Column++;
+      }
+    }
+  }
+
+  //
+  // First time we enter this handler, we need to check to see if
+  // we were passed an increment or decrement directive
+  //
+  do {
+    Key.UnicodeChar = CHAR_NULL;
+    if (gDirection != 0) {
+      Key.ScanCode  = gDirection;
+      gDirection    = 0;
+      goto TheKey2;
+    }
+
+    WaitForKeyStroke (&Key);
+
+TheKey2:
+    switch (Key.UnicodeChar) {
+
+    case '+':
+    case '-':
+      if (ManualInput && IntInput) {
+        //
+        // In Manual input mode, check whether input the negative flag.
+        //
+        if (Key.UnicodeChar == '-') {
+          if (Negative) {
+            break;
+          }
+          Negative = TRUE;
+          PrintCharAt (Column++, Row, Key.UnicodeChar);
+        }
+      } else {
+        if (Key.UnicodeChar == '+') {
+          Key.ScanCode = SCAN_RIGHT;
+        } else {
+          Key.ScanCode = SCAN_LEFT;
+        }
+        Key.UnicodeChar = CHAR_NULL;
+        goto TheKey2;
+      }
+      break;
+
+    case CHAR_NULL:
+      switch (Key.ScanCode) {
+      case SCAN_LEFT:
+      case SCAN_RIGHT:
+        if (DateOrTime && !ManualInput) {
+          //
+          // By setting this value, we will return back to the caller.
+          // We need to do this since an auto-refresh will destroy the adjustment
+          // based on what the real-time-clock is showing.  So we always commit
+          // upon changing the value.
+          //
+          gDirection = SCAN_DOWN;
+        }
+
+        if ((Step != 0) && !ManualInput) {
+          if (Key.ScanCode == SCAN_LEFT) {
+            if (IntInput) {
+              if ((INT64) EditValue >= (INT64) Minimum + (INT64) Step) {
+                EditValue = EditValue - Step;
+              } else if ((INT64) EditValue > (INT64) Minimum){
+                EditValue = Minimum;
+              } else {
+                EditValue = Maximum;
+              }
+            } else {
+              if (EditValue >= Minimum + Step) {
+                EditValue = EditValue - Step;
+              } else if (EditValue > Minimum){
+                EditValue = Minimum;
+              } else {
+                EditValue = Maximum;
+              }
+            }
+          } else if (Key.ScanCode == SCAN_RIGHT) {
+            if (IntInput) {
+              if ((INT64) EditValue + (INT64) Step <= (INT64) Maximum) {
+                EditValue = EditValue + Step;
+              } else if ((INT64) EditValue < (INT64) Maximum) {
+                EditValue = Maximum;
+              } else {
+                EditValue = Minimum;
+              }
+            } else {
+              if (EditValue + Step <= Maximum) {
+                EditValue = EditValue + Step;
+              } else if (EditValue < Maximum) {
+                EditValue = Maximum;
+              } else {
+                EditValue = Minimum;
+              }
+            }
+          }
+
+          ZeroMem (FormattedNumber, 21 * sizeof (CHAR16));
+          if (Question->OpCode->OpCode == EFI_IFR_DATE_OP) {
+            if (MenuOption->Sequence == 2) {
+              //
+              // Year
+              //
+              UnicodeSPrint (FormattedNumber, 21 * sizeof (CHAR16), L"%04d", (UINT16) EditValue);
+            } else {
+              //
+              // Month/Day
+              //
+              UnicodeSPrint (FormattedNumber, 21 * sizeof (CHAR16), L"%02d", (UINT8) EditValue);
+            }
+
+            if (MenuOption->Sequence == 0) {
+              ASSERT (EraseLen >= 2);
+              FormattedNumber[EraseLen - 2] = DATE_SEPARATOR;
+            } else if (MenuOption->Sequence == 1) {
+              ASSERT (EraseLen >= 1);
+              FormattedNumber[EraseLen - 1] = DATE_SEPARATOR;
+            }
+          } else if (Question->OpCode->OpCode == EFI_IFR_TIME_OP) {
+            UnicodeSPrint (FormattedNumber, 21 * sizeof (CHAR16), L"%02d", (UINT8) EditValue);
+
+            if (MenuOption->Sequence == 0) {
+              ASSERT (EraseLen >= 2);
+              FormattedNumber[EraseLen - 2] = TIME_SEPARATOR;
+            } else if (MenuOption->Sequence == 1) {
+              ASSERT (EraseLen >= 1);
+              FormattedNumber[EraseLen - 1] = TIME_SEPARATOR;
+            }
+          } else {
+            QuestionValue->Value.u64 = EditValue;
+            PrintFormattedNumber (Question, FormattedNumber, 21 * sizeof (CHAR16));
+          }
+
+          gST->ConOut->SetAttribute (gST->ConOut, GetFieldTextColor ());
+          for (Loop = 0; Loop < EraseLen; Loop++) {
+            PrintStringAt (MenuOption->OptCol + Loop, MenuOption->Row, L" ");
+          }
+          gST->ConOut->SetAttribute (gST->ConOut, GetHighlightTextColor ());
+
+          if (MenuOption->Sequence == 0) {
+            PrintCharAt (MenuOption->OptCol, Row, LEFT_NUMERIC_DELIMITER);
+            Column = MenuOption->OptCol + 1;
+          }
+
+          PrintStringAt (Column, Row, FormattedNumber);
+
+          if (!DateOrTime || MenuOption->Sequence == 2) {
+            PrintCharAt ((UINTN)-1, (UINTN)-1, RIGHT_NUMERIC_DELIMITER);
+          }
+        }
+
+        goto EnterCarriageReturn;
+        break;
+
+      case SCAN_UP:
+      case SCAN_DOWN:
+        goto EnterCarriageReturn;
+
+      case SCAN_ESC:
+        return EFI_DEVICE_ERROR;
+
+      default:
+        break;
+      }
+
+      break;
+
+EnterCarriageReturn:
+
+    case CHAR_CARRIAGE_RETURN:
+      //
+      // Validate input value with Minimum value.
+      //
+      ValidateFail = FALSE;
+      if (IntInput) {
+        //
+        // After user input Enter, need to check whether the input value.
+        // If input a negative value, should compare with maximum value. 
+        // else compare with the minimum value.
+        //
+        if (Negative) {
+          ValidateFail = (INT64) EditValue > (INT64) Maximum ? TRUE : FALSE;
+        } else {
+          ValidateFail = (INT64) EditValue < (INT64) Minimum ? TRUE : FALSE;
+        }
+
+        if (ValidateFail) {
+          UpdateStatusBar (INPUT_ERROR, TRUE);
+          break;
+        }
+      } else if (EditValue < Minimum) {
+        UpdateStatusBar (INPUT_ERROR, TRUE);
+        break;
+      }
+
+      UpdateStatusBar (INPUT_ERROR, FALSE);
+      CopyMem (&gUserInput->InputValue, &Question->CurrentValue, sizeof (EFI_HII_VALUE));
+      QuestionValue = &gUserInput->InputValue;
+      //
+      // Store Edit value back to Question
+      //
+      if (Question->OpCode->OpCode == EFI_IFR_DATE_OP) {
+        switch (MenuOption->Sequence) {
+        case 0:
+          QuestionValue->Value.date.Month = (UINT8) EditValue;
+          break;
+
+        case 1:
+          QuestionValue->Value.date.Day = (UINT8) EditValue;
+          break;
+
+        case 2:
+          QuestionValue->Value.date.Year = (UINT16) EditValue;
+          break;
+
+        default:
+          break;
+        }
+      } else if (Question->OpCode->OpCode  == EFI_IFR_TIME_OP) {
+        switch (MenuOption->Sequence) {
+        case 0:
+          QuestionValue->Value.time.Hour = (UINT8) EditValue;
+          break;
+
+        case 1:
+          QuestionValue->Value.time.Minute = (UINT8) EditValue;
+          break;
+
+        case 2:
+          QuestionValue->Value.time.Second = (UINT8) EditValue;
+          break;
+
+        default:
+          break;
+        }
+      } else {
+        //
+        // Numeric
+        //
+        QuestionValue->Value.u64 = EditValue;
+      }
+
+      //
+      // Adjust the value to the correct one.
+      // Sample like: 2012.02.29 -> 2013.02.29 -> 2013.02.01
+      //              2013.03.29 -> 2013.02.29 -> 2013.02.28
+      //
+      if (Question->OpCode->OpCode  == EFI_IFR_DATE_OP && 
+        (MenuOption->Sequence == 0 || MenuOption->Sequence == 2)) {
+        AdjustQuestionValue (QuestionValue, (UINT8)MenuOption->Sequence);
+      }
+
+      return EFI_SUCCESS;
+      break;
+
+    case CHAR_BACKSPACE:
+      if (ManualInput) {
+        if (Count == 0) {
+          if (Negative) {
+            Negative = FALSE;
+            Column--;
+            PrintStringAt (Column, Row, L" ");
+          }
+          break;
+        }
+        //
+        // Remove a character
+        //
+        EditValue = PreviousNumber[Count - 1];
+        UpdateStatusBar (INPUT_ERROR,  FALSE);
+        Count--;
+        Column--;
+        PrintStringAt (Column, Row, L" ");
+      }
+      break;
+
+    default:
+      if (ManualInput) {
+        if (HexInput) {
+          if ((Key.UnicodeChar >= L'0') && (Key.UnicodeChar <= L'9')) {
+            Digital = (UINT8) (Key.UnicodeChar - L'0');
+          } else if ((Key.UnicodeChar >= L'A') && (Key.UnicodeChar <= L'F')) {
+            Digital = (UINT8) (Key.UnicodeChar - L'A' + 0x0A);
+          } else if ((Key.UnicodeChar >= L'a') && (Key.UnicodeChar <= L'f')) {
+            Digital = (UINT8) (Key.UnicodeChar - L'a' + 0x0A);
+          } else {
+            UpdateStatusBar (INPUT_ERROR, TRUE);
+            break;
+          }
+        } else {
+          if (Key.UnicodeChar > L'9' || Key.UnicodeChar < L'0') {
+            UpdateStatusBar (INPUT_ERROR, TRUE);
+            break;
+          }
+        }
+
+        //
+        // If Count exceed input width, there is no way more is valid
+        //
+        if (Count >= InputWidth) {
+          break;
+        }
+        //
+        // Someone typed something valid!
+        //
+        if (Count != 0) {
+          if (HexInput) {
+            EditValue = LShiftU64 (EditValue, 4) + Digital;
+          } else if (IntInput && Negative) {
+            //
+            // Save the negative number.
+            //
+            EditValue = ~(MultU64x32 (~(EditValue - 1), 10) + (Key.UnicodeChar - L'0')) + 1;
+          } else {
+            EditValue = MultU64x32 (EditValue, 10) + (Key.UnicodeChar - L'0');
+          }
+        } else {
+          if (HexInput) {
+            EditValue = Digital;
+          } else if (IntInput && Negative) {
+            //
+            // Save the negative number.
+            //
+            EditValue = ~(Key.UnicodeChar - L'0') + 1;
+          } else {
+            EditValue = Key.UnicodeChar - L'0';
+          }
+        }
+
+        if (IntInput) {
+          ValidateFail = FALSE;
+          //
+          // When user input a new value, should check the current value.
+          // If user input a negative value, should compare it with minimum
+          // value, else compare it with maximum value.
+          //
+          if (Negative) {
+            ValidateFail = (INT64) EditValue < (INT64) Minimum ? TRUE : FALSE;
+          } else {
+            ValidateFail = (INT64) EditValue > (INT64) Maximum ? TRUE : FALSE;
+          }
+
+          if (ValidateFail) {
+            UpdateStatusBar (INPUT_ERROR, TRUE);
+            ASSERT (Count < sizeof (PreviousNumber) / sizeof (PreviousNumber[0]));
+            EditValue = PreviousNumber[Count];
+            break;
+          }
+        } else {
+          if (EditValue > Maximum) {
+            UpdateStatusBar (INPUT_ERROR, TRUE);
+            ASSERT (Count < sizeof (PreviousNumber) / sizeof (PreviousNumber[0]));
+            EditValue = PreviousNumber[Count];
+            break;
+          }
+        }
+
+        UpdateStatusBar (INPUT_ERROR, FALSE);
+
+        Count++;
+        ASSERT (Count < (sizeof (PreviousNumber) / sizeof (PreviousNumber[0])));
+        PreviousNumber[Count] = EditValue;
+
+        gST->ConOut->SetAttribute (gST->ConOut, GetHighlightTextColor ());
+        PrintCharAt (Column, Row, Key.UnicodeChar);
+        Column++;
+      }
+      break;
+    }
+  } while (TRUE);
+}
+
+/**
+  Adjust option order base on the question value.
+
+  @param  Question           Pointer to current question.
+  @param  PopUpMenuLines     The line number of the pop up menu.
+
+  @retval EFI_SUCCESS       If Option input is processed successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+AdjustOptionOrder (
+  IN  FORM_DISPLAY_ENGINE_STATEMENT  *Question,
+  OUT UINTN                          *PopUpMenuLines
+  )
+{
+  UINTN                   Index;
+  EFI_IFR_ORDERED_LIST    *OrderList;
+  UINT8                   *ValueArray;
+  UINT8                   ValueType;
+  LIST_ENTRY              *Link;
+  DISPLAY_QUESTION_OPTION *OneOfOption;
+  EFI_HII_VALUE           *HiiValueArray;
+
+  Link        = GetFirstNode (&Question->OptionListHead);
+  OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+  ValueArray  = Question->CurrentValue.Buffer;
+  ValueType   =  OneOfOption->OptionOpCode->Type;
+  OrderList   = (EFI_IFR_ORDERED_LIST *) Question->OpCode;
+
+  for (Index = 0; Index < OrderList->MaxContainers; Index++) {
+    if (GetArrayData (ValueArray, ValueType, Index) == 0) {
+      break;
+    }
+  }
+  
+  *PopUpMenuLines = Index;
+  
+  //
+  // Prepare HiiValue array
+  //  
+  HiiValueArray = AllocateZeroPool (*PopUpMenuLines * sizeof (EFI_HII_VALUE));
+  ASSERT (HiiValueArray != NULL);
+
+  for (Index = 0; Index < *PopUpMenuLines; Index++) {
+    HiiValueArray[Index].Type = ValueType;
+    HiiValueArray[Index].Value.u64 = GetArrayData (ValueArray, ValueType, Index);
+  }
+  
+  for (Index = 0; Index < *PopUpMenuLines; Index++) {
+    OneOfOption = ValueToOption (Question, &HiiValueArray[*PopUpMenuLines - Index - 1]);
+    if (OneOfOption == NULL) {
+      return EFI_NOT_FOUND;
+    }
+  
+    RemoveEntryList (&OneOfOption->Link);
+  
+    //
+    // Insert to head.
+    //
+    InsertHeadList (&Question->OptionListHead, &OneOfOption->Link);
+  }
+  
+  FreePool (HiiValueArray);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Base on the type to compare the value.
+
+  @param  Value1                The first value need to compare.
+  @param  Value2                The second value need to compare.
+  @param  Type                  The value type for above two values.
+
+  @retval TRUE                  The two value are same.
+  @retval FALSE                 The two value are different.
+
+**/
+BOOLEAN
+IsValuesEqual (
+  IN EFI_IFR_TYPE_VALUE *Value1,
+  IN EFI_IFR_TYPE_VALUE *Value2,
+  IN UINT8              Type
+  )
+{
+  switch (Type) {
+  case EFI_IFR_TYPE_BOOLEAN:
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    return (BOOLEAN) (Value1->u8 == Value2->u8);
+  
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    return (BOOLEAN) (Value1->u16 == Value2->u16);
+  
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    return (BOOLEAN) (Value1->u32 == Value2->u32);
+  
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    return (BOOLEAN) (Value1->u64 == Value2->u64);
+
+  default:
+    ASSERT (FALSE);
+    return FALSE;
+  }
+}
+
+/**
+  Base on the type to set the value.
+
+  @param  Dest                  The dest value.
+  @param  Source                The source value.
+  @param  Type                  The value type for above two values.
+
+**/
+VOID
+SetValuesByType (
+  OUT EFI_IFR_TYPE_VALUE *Dest,
+  IN  EFI_IFR_TYPE_VALUE *Source,
+  IN  UINT8              Type
+  )
+{
+  switch (Type) {
+  case EFI_IFR_TYPE_BOOLEAN:
+    Dest->b = Source->b;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    Dest->u8 = Source->u8;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    Dest->u16 = Source->u16;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    Dest->u32 = Source->u32;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    Dest->u64 = Source->u64;
+    break;
+
+  default:
+    ASSERT (FALSE);
+    break;
+  }
+}
+
+/**
+  Get selection for OneOf and OrderedList (Left/Right will be ignored).
+
+  @param  MenuOption        Pointer to the current input menu.
+
+  @retval EFI_SUCCESS       If Option input is processed successfully
+  @retval EFI_DEVICE_ERROR  If operation fails
+
+**/
+EFI_STATUS
+GetSelectionInputPopUp (
+  IN  UI_MENU_OPTION              *MenuOption
+  )
+{
+  EFI_INPUT_KEY           Key;
+  UINTN                   Index;
+  CHAR16                  *StringPtr;
+  CHAR16                  *TempStringPtr;
+  UINTN                   Index2;
+  UINTN                   TopOptionIndex;
+  UINTN                   HighlightOptionIndex;
+  UINTN                   Start;
+  UINTN                   End;
+  UINTN                   Top;
+  UINTN                   Bottom;
+  UINTN                   PopUpMenuLines;
+  UINTN                   MenuLinesInView;
+  UINTN                   PopUpWidth;
+  CHAR16                  Character;
+  INT32                   SavedAttribute;
+  BOOLEAN                 ShowDownArrow;
+  BOOLEAN                 ShowUpArrow;
+  UINTN                   DimensionsWidth;
+  LIST_ENTRY              *Link;
+  BOOLEAN                 OrderedList;
+  UINT8                   *ValueArray;
+  UINT8                   *ReturnValue;
+  UINT8                   ValueType;
+  EFI_HII_VALUE           HiiValue;
+  DISPLAY_QUESTION_OPTION         *OneOfOption;
+  DISPLAY_QUESTION_OPTION         *CurrentOption;
+  FORM_DISPLAY_ENGINE_STATEMENT  *Question;
+  INTN                    Result;
+  EFI_IFR_ORDERED_LIST    *OrderList;
+
+  DimensionsWidth   = gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn;
+
+  ValueArray        = NULL;
+  ValueType         = 0;
+  CurrentOption     = NULL;
+  ShowDownArrow     = FALSE;
+  ShowUpArrow       = FALSE;
+
+  StringPtr = AllocateZeroPool ((gOptionBlockWidth + 1) * 2);
+  ASSERT (StringPtr);
+
+  ZeroMem (&HiiValue, sizeof (EFI_HII_VALUE));
+
+  Question = MenuOption->ThisTag;
+  if (Question->OpCode->OpCode == EFI_IFR_ORDERED_LIST_OP) {
+    Link = GetFirstNode (&Question->OptionListHead);
+    OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+    ValueArray = Question->CurrentValue.Buffer;
+    ValueType =  OneOfOption->OptionOpCode->Type;
+    OrderedList = TRUE;
+    OrderList = (EFI_IFR_ORDERED_LIST *) Question->OpCode;
+  } else {
+    OrderedList = FALSE;
+    OrderList = NULL;
+  }
+
+  //
+  // Calculate Option count
+  //
+  PopUpMenuLines = 0;
+  if (OrderedList) {
+    AdjustOptionOrder(Question, &PopUpMenuLines);
+  } else {
+    Link = GetFirstNode (&Question->OptionListHead);
+    while (!IsNull (&Question->OptionListHead, Link)) {
+      OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+      PopUpMenuLines++;
+      Link = GetNextNode (&Question->OptionListHead, Link);
+    }
+  }
+
+  //
+  // Get the number of one of options present and its size
+  //
+  PopUpWidth = 0;
+  HighlightOptionIndex = 0;
+  Link = GetFirstNode (&Question->OptionListHead);
+  for (Index = 0; Index < PopUpMenuLines; Index++) {
+    OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+
+    StringPtr = GetToken (OneOfOption->OptionOpCode->Option, gFormData->HiiHandle);
+    if (StrLen (StringPtr) > PopUpWidth) {
+      PopUpWidth = StrLen (StringPtr);
+    }
+    FreePool (StringPtr);
+    HiiValue.Type = OneOfOption->OptionOpCode->Type;
+    SetValuesByType (&HiiValue.Value, &OneOfOption->OptionOpCode->Value, HiiValue.Type);
+    if (!OrderedList && (CompareHiiValue (&Question->CurrentValue, &HiiValue, &Result, NULL) == EFI_SUCCESS) && (Result == 0)) {
+      //
+      // Find current selected Option for OneOf
+      //
+      HighlightOptionIndex = Index;
+    }
+
+    Link = GetNextNode (&Question->OptionListHead, Link);
+  }
+
+  //
+  // Perform popup menu initialization.
+  //
+  PopUpWidth = PopUpWidth + POPUP_PAD_SPACE_COUNT;
+
+  SavedAttribute = gST->ConOut->Mode->Attribute;
+  gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+
+  if ((PopUpWidth + POPUP_FRAME_WIDTH) > DimensionsWidth) {
+    PopUpWidth = DimensionsWidth - POPUP_FRAME_WIDTH;
+  }
+
+  Start  = (DimensionsWidth - PopUpWidth - POPUP_FRAME_WIDTH) / 2 + gStatementDimensions.LeftColumn;
+  End    = Start + PopUpWidth + POPUP_FRAME_WIDTH;
+  Top    = gStatementDimensions.TopRow;
+  Bottom = gStatementDimensions.BottomRow - 1;
+
+  MenuLinesInView = Bottom - Top - 1;
+  if (MenuLinesInView >= PopUpMenuLines) {
+    Top     = Top + (MenuLinesInView - PopUpMenuLines) / 2;
+    Bottom  = Top + PopUpMenuLines + 1;
+  } else {
+    ShowDownArrow = TRUE;
+  }
+
+  if (HighlightOptionIndex > (MenuLinesInView - 1)) {
+    TopOptionIndex = HighlightOptionIndex - MenuLinesInView + 1;
+  } else {
+    TopOptionIndex = 0;
+  }
+
+  do {
+    //
+    // Clear that portion of the screen
+    //
+    ClearLines (Start, End, Top, Bottom, GetPopupColor ());
+
+    //
+    // Draw "One of" pop-up menu
+    //
+    Character = BOXDRAW_DOWN_RIGHT;
+    PrintCharAt (Start, Top, Character);
+    for (Index = Start; Index + 2 < End; Index++) {
+      if ((ShowUpArrow) && ((Index + 1) == (Start + End) / 2)) {
+        Character = GEOMETRICSHAPE_UP_TRIANGLE;
+      } else {
+        Character = BOXDRAW_HORIZONTAL;
+      }
+
+      PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+    }
+
+    Character = BOXDRAW_DOWN_LEFT;
+    PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+    Character = BOXDRAW_VERTICAL;
+    for (Index = Top + 1; Index < Bottom; Index++) {
+      PrintCharAt (Start, Index, Character);
+      PrintCharAt (End - 1, Index, Character);
+    }
+
+    //
+    // Move to top Option
+    //
+    Link = GetFirstNode (&Question->OptionListHead);
+    for (Index = 0; Index < TopOptionIndex; Index++) {
+      Link = GetNextNode (&Question->OptionListHead, Link);
+    }
+
+    //
+    // Display the One of options
+    //
+    Index2 = Top + 1;
+    for (Index = TopOptionIndex; (Index < PopUpMenuLines) && (Index2 < Bottom); Index++) {
+      OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+      Link = GetNextNode (&Question->OptionListHead, Link);
+
+      StringPtr = GetToken (OneOfOption->OptionOpCode->Option, gFormData->HiiHandle);
+      ASSERT (StringPtr != NULL);
+      //
+      // If the string occupies multiple lines, truncate it to fit in one line,
+      // and append a "..." for indication.
+      //
+      if (StrLen (StringPtr) > (PopUpWidth - 1)) {
+        TempStringPtr = AllocateZeroPool (sizeof (CHAR16) * (PopUpWidth - 1));
+        ASSERT ( TempStringPtr != NULL );
+        CopyMem (TempStringPtr, StringPtr, (sizeof (CHAR16) * (PopUpWidth - 5)));
+        FreePool (StringPtr);
+        StringPtr = TempStringPtr;
+        StrCatS (StringPtr, PopUpWidth - 1, L"...");
+      }
+
+      if (Index == HighlightOptionIndex) {
+          //
+          // Highlight the selected one
+          //
+          CurrentOption = OneOfOption;
+
+          gST->ConOut->SetAttribute (gST->ConOut, GetPickListColor ());
+          PrintStringAt (Start + 2, Index2, StringPtr);
+          gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+        } else {
+          gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+          PrintStringAt (Start + 2, Index2, StringPtr);
+        }
+
+      Index2++;
+      FreePool (StringPtr);
+    }
+
+    Character = BOXDRAW_UP_RIGHT;
+    PrintCharAt (Start, Bottom, Character);
+    for (Index = Start; Index + 2 < End; Index++) {
+      if ((ShowDownArrow) && ((Index + 1) == (Start + End) / 2)) {
+        Character = GEOMETRICSHAPE_DOWN_TRIANGLE;
+      } else {
+        Character = BOXDRAW_HORIZONTAL;
+      }
+
+      PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+    }
+
+    Character = BOXDRAW_UP_LEFT;
+    PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+
+    //
+    // Get User selection
+    //
+    Key.UnicodeChar = CHAR_NULL;
+    if ((gDirection == SCAN_UP) || (gDirection == SCAN_DOWN)) {
+      Key.ScanCode  = gDirection;
+      gDirection    = 0;
+      goto TheKey;
+    }
+
+    WaitForKeyStroke (&Key);
+
+TheKey:
+    switch (Key.UnicodeChar) {
+    case '+':
+      if (OrderedList) {
+        if ((TopOptionIndex > 0) && (TopOptionIndex == HighlightOptionIndex)) {
+          //
+          // Highlight reaches the top of the popup window, scroll one menu item.
+          //
+          TopOptionIndex--;
+          ShowDownArrow = TRUE;
+        }
+
+        if (TopOptionIndex == 0) {
+          ShowUpArrow = FALSE;
+        }
+
+        if (HighlightOptionIndex > 0) {
+          HighlightOptionIndex--;
+
+          ASSERT (CurrentOption != NULL);
+          SwapListEntries (CurrentOption->Link.BackLink, &CurrentOption->Link);
+        }
+      }
+      break;
+
+    case '-':
+      //
+      // If an ordered list op-code, we will allow for a popup of +/- keys
+      // to create an ordered list of items
+      //
+      if (OrderedList) {
+        if (((TopOptionIndex + MenuLinesInView) < PopUpMenuLines) &&
+            (HighlightOptionIndex == (TopOptionIndex + MenuLinesInView - 1))) {
+          //
+          // Highlight reaches the bottom of the popup window, scroll one menu item.
+          //
+          TopOptionIndex++;
+          ShowUpArrow = TRUE;
+        }
+
+        if ((TopOptionIndex + MenuLinesInView) == PopUpMenuLines) {
+          ShowDownArrow = FALSE;
+        }
+
+        if (HighlightOptionIndex < (PopUpMenuLines - 1)) {
+          HighlightOptionIndex++;
+
+          ASSERT (CurrentOption != NULL);
+          SwapListEntries (&CurrentOption->Link, CurrentOption->Link.ForwardLink);
+        }
+      }
+      break;
+
+    case CHAR_NULL:
+      switch (Key.ScanCode) {
+      case SCAN_UP:
+      case SCAN_DOWN:
+        if (Key.ScanCode == SCAN_UP) {
+          if ((TopOptionIndex > 0) && (TopOptionIndex == HighlightOptionIndex)) {
+            //
+            // Highlight reaches the top of the popup window, scroll one menu item.
+            //
+            TopOptionIndex--;
+            ShowDownArrow = TRUE;
+          }
+
+          if (TopOptionIndex == 0) {
+            ShowUpArrow = FALSE;
+          }
+
+          if (HighlightOptionIndex > 0) {
+            HighlightOptionIndex--;
+          }
+        } else {
+          if (((TopOptionIndex + MenuLinesInView) < PopUpMenuLines) &&
+              (HighlightOptionIndex == (TopOptionIndex + MenuLinesInView - 1))) {
+            //
+            // Highlight reaches the bottom of the popup window, scroll one menu item.
+            //
+            TopOptionIndex++;
+            ShowUpArrow = TRUE;
+          }
+
+          if ((TopOptionIndex + MenuLinesInView) == PopUpMenuLines) {
+            ShowDownArrow = FALSE;
+          }
+
+          if (HighlightOptionIndex < (PopUpMenuLines - 1)) {
+            HighlightOptionIndex++;
+          }
+        }
+        break;
+
+      case SCAN_ESC:
+        gST->ConOut->SetAttribute (gST->ConOut, SavedAttribute);
+
+        //
+        // Restore link list order for orderedlist
+        //
+        if (OrderedList) {
+          HiiValue.Type = ValueType;
+          HiiValue.Value.u64 = 0;
+          for (Index = 0; Index < OrderList->MaxContainers; Index++) {
+            HiiValue.Value.u64 = GetArrayData (ValueArray, ValueType, Index);
+            if (HiiValue.Value.u64 == 0) {
+              break;
+            }
+
+            OneOfOption = ValueToOption (Question, &HiiValue);
+            if (OneOfOption == NULL) {
+              return EFI_NOT_FOUND;
+            }
+
+            RemoveEntryList (&OneOfOption->Link);
+            InsertTailList (&Question->OptionListHead, &OneOfOption->Link);
+          }
+        }
+
+        return EFI_DEVICE_ERROR;
+
+      default:
+        break;
+      }
+
+      break;
+
+    case CHAR_CARRIAGE_RETURN:
+      //
+      // return the current selection
+      //
+      if (OrderedList) {
+        ReturnValue = AllocateZeroPool (Question->CurrentValue.BufferLen);
+        ASSERT (ReturnValue != NULL);
+        Index = 0;
+        Link = GetFirstNode (&Question->OptionListHead);
+        while (!IsNull (&Question->OptionListHead, Link)) {
+          OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+          Link = GetNextNode (&Question->OptionListHead, Link);
+
+          SetArrayData (ReturnValue, ValueType, Index, OneOfOption->OptionOpCode->Value.u64);
+
+          Index++;
+          if (Index > OrderList->MaxContainers) {
+            break;
+          }
+        }
+        if (CompareMem (ReturnValue, ValueArray, Question->CurrentValue.BufferLen) == 0) {
+          FreePool (ReturnValue);
+          return EFI_DEVICE_ERROR;
+        } else {
+          gUserInput->InputValue.Buffer = ReturnValue;
+          gUserInput->InputValue.BufferLen = Question->CurrentValue.BufferLen;
+        }
+      } else {
+        ASSERT (CurrentOption != NULL);
+        gUserInput->InputValue.Type = CurrentOption->OptionOpCode->Type;
+        if (IsValuesEqual (&Question->CurrentValue.Value, &CurrentOption->OptionOpCode->Value, gUserInput->InputValue.Type)) {
+          return EFI_DEVICE_ERROR;
+        } else {
+          SetValuesByType (&gUserInput->InputValue.Value, &CurrentOption->OptionOpCode->Value, gUserInput->InputValue.Type);
+        }
+      }
+
+      gST->ConOut->SetAttribute (gST->ConOut, SavedAttribute);
+
+      return EFI_SUCCESS;
+      
+    default:
+      break;
+    }
+  } while (TRUE);
+
+}
+
diff --git a/Core/MdeModulePkg/Universal/DisplayEngineDxe/ProcessOptions.c b/Core/MdeModulePkg/Universal/DisplayEngineDxe/ProcessOptions.c
new file mode 100644
index 0000000000..c074f4b4a3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DisplayEngineDxe/ProcessOptions.c
@@ -0,0 +1,1475 @@
+/** @file
+Implementation for handling the User Interface option processing.
+
+
+Copyright (c) 2004 - 2015, 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 "FormDisplay.h"
+
+#define MAX_TIME_OUT_LEN  0x10
+
+/**
+  Concatenate a narrow string to another string.
+
+  @param Destination The destination string.
+  @param DestMax     The Max length of destination string.
+  @param Source      The source string. The string to be concatenated.
+                     to the end of Destination.
+
+**/
+VOID
+NewStrCat (
+  IN OUT CHAR16               *Destination,
+  IN     UINTN                DestMax,
+  IN     CHAR16               *Source
+  )
+{
+  UINTN Length;
+
+  for (Length = 0; Destination[Length] != 0; Length++)
+    ;
+
+  //
+  // We now have the length of the original string
+  // We can safely assume for now that we are concatenating a narrow value to this string.
+  // For instance, the string is "XYZ" and cat'ing ">"
+  // If this assumption changes, we need to make this routine a bit more complex
+  //
+  Destination[Length] = NARROW_CHAR;
+  Length++;
+
+  StrCpyS (Destination + Length, DestMax - Length, Source);
+}
+
+/**
+  Get UINT64 type value.
+
+  @param  Value                  Input Hii value.
+
+  @retval UINT64                 Return the UINT64 type value.
+
+**/
+UINT64
+HiiValueToUINT64 (
+  IN EFI_HII_VALUE      *Value
+  )
+{
+  UINT64  RetVal;
+
+  RetVal = 0;
+
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    RetVal = Value->Value.u8;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    RetVal = Value->Value.u16;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    RetVal = Value->Value.u32;
+    break;
+
+  case EFI_IFR_TYPE_BOOLEAN:
+    RetVal = Value->Value.b;
+    break;
+
+  case EFI_IFR_TYPE_DATE:
+    RetVal = *(UINT64*) &Value->Value.date;
+    break;
+
+  case EFI_IFR_TYPE_TIME:
+    RetVal = (*(UINT64*) &Value->Value.time) & 0xffffff;
+    break;
+
+  default:
+    RetVal = Value->Value.u64;
+    break;
+  }
+
+  return RetVal;
+}
+
+/**
+  Check whether this value type can be transfer to EFI_IFR_TYPE_BUFFER type.
+  
+  EFI_IFR_TYPE_REF, EFI_IFR_TYPE_DATE and EFI_IFR_TYPE_TIME are converted to 
+  EFI_IFR_TYPE_BUFFER when do the value compare.
+
+  @param  Value                  Expression value to compare on.
+
+  @retval TRUE                   This value type can be transter to EFI_IFR_TYPE_BUFFER type.
+  @retval FALSE                  This value type can't be transter to EFI_IFR_TYPE_BUFFER type.
+
+**/
+BOOLEAN
+IsTypeInBuffer (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_BUFFER:
+  case EFI_IFR_TYPE_DATE:
+  case EFI_IFR_TYPE_TIME:
+  case EFI_IFR_TYPE_REF:
+    return TRUE;
+
+  default:
+    return FALSE;
+  }
+}
+
+/**
+  Check whether this value type can be transfer to EFI_IFR_TYPE_UINT64
+
+  @param  Value                  Expression value to compare on.
+
+  @retval TRUE                   This value type can be transter to EFI_IFR_TYPE_BUFFER type.
+  @retval FALSE                  This value type can't be transter to EFI_IFR_TYPE_BUFFER type.
+
+**/
+BOOLEAN
+IsTypeInUINT64 (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+  case EFI_IFR_TYPE_BOOLEAN:
+    return TRUE;
+
+  default:
+    return FALSE;
+  }
+}
+
+/**
+  Return the buffer length and buffer pointer for this value.
+  
+  EFI_IFR_TYPE_REF, EFI_IFR_TYPE_DATE and EFI_IFR_TYPE_TIME are converted to 
+  EFI_IFR_TYPE_BUFFER when do the value compare.
+
+  @param  Value                  Expression value to compare on.
+  @param  Buf                    Return the buffer pointer.
+  @param  BufLen                 Return the buffer length.
+
+**/
+VOID
+GetBufAndLenForValue (
+  IN  EFI_HII_VALUE   *Value,
+  OUT UINT8           **Buf,
+  OUT UINT16          *BufLen
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_BUFFER:
+    *Buf    = Value->Buffer;
+    *BufLen = Value->BufferLen;
+    break;
+
+  case EFI_IFR_TYPE_DATE:
+    *Buf    = (UINT8 *) (&Value->Value.date);
+    *BufLen = (UINT16) sizeof (EFI_HII_DATE);
+    break;
+
+  case EFI_IFR_TYPE_TIME:
+    *Buf    = (UINT8 *) (&Value->Value.time);
+    *BufLen = (UINT16) sizeof (EFI_HII_TIME);
+    break;
+
+  case EFI_IFR_TYPE_REF:
+    *Buf    = (UINT8 *) (&Value->Value.ref);
+    *BufLen = (UINT16) sizeof (EFI_HII_REF);
+    break;
+
+  default:
+    *Buf    = NULL;
+    *BufLen = 0;
+  }
+}
+
+/**
+  Compare two Hii value.
+
+  @param  Value1                 Expression value to compare on left-hand.
+  @param  Value2                 Expression value to compare on right-hand.
+  @param  Result                 Return value after compare.
+                                 retval 0                      Two operators equal.
+                                 return Positive value if Value1 is greater than Value2.
+                                 retval Negative value if Value1 is less than Value2.
+  @param  HiiHandle              Only required for string compare.
+
+  @retval other                  Could not perform compare on two values.
+  @retval EFI_SUCCESS            Compare the value success.
+
+**/
+EFI_STATUS
+CompareHiiValue (
+  IN  EFI_HII_VALUE   *Value1,
+  IN  EFI_HII_VALUE   *Value2,
+  OUT INTN            *Result,
+  IN  EFI_HII_HANDLE  HiiHandle OPTIONAL
+  )
+{
+  INT64   Temp64;
+  CHAR16  *Str1;
+  CHAR16  *Str2;
+  UINTN   Len;
+  UINT8   *Buf1;
+  UINT16  Buf1Len;
+  UINT8   *Buf2;
+  UINT16  Buf2Len;
+
+  if (Value1->Type == EFI_IFR_TYPE_STRING && Value2->Type == EFI_IFR_TYPE_STRING) {
+    if (Value1->Value.string == 0 || Value2->Value.string == 0) {
+      //
+      // StringId 0 is reserved
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (Value1->Value.string == Value2->Value.string) {
+      *Result = 0;
+      return EFI_SUCCESS;
+    }
+
+    Str1 = GetToken (Value1->Value.string, HiiHandle);
+    if (Str1 == NULL) {
+      //
+      // String not found
+      //
+      return EFI_NOT_FOUND;
+    }
+
+    Str2 = GetToken (Value2->Value.string, HiiHandle);
+    if (Str2 == NULL) {
+      FreePool (Str1);
+      return EFI_NOT_FOUND;
+    }
+
+    *Result = StrCmp (Str1, Str2);
+
+    FreePool (Str1);
+    FreePool (Str2);
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Take types(date, time, ref, buffer) as buffer
+  //
+  if (IsTypeInBuffer(Value1) && IsTypeInBuffer(Value2)) {
+    GetBufAndLenForValue(Value1, &Buf1, &Buf1Len);
+    GetBufAndLenForValue(Value2, &Buf2, &Buf2Len);
+
+    Len = Buf1Len > Buf2Len ? Buf2Len : Buf1Len;
+    *Result = CompareMem (Buf1, Buf2, Len);
+    if ((*Result == 0) && (Buf1Len != Buf2Len)) {
+      //
+      // In this case, means base on samll number buffer, the data is same
+      // So which value has more data, which value is bigger.
+      //
+      *Result = Buf1Len > Buf2Len ? 1 : -1;
+    }
+    return EFI_SUCCESS;
+  }  
+
+  //
+  // Take remain types(integer, boolean, date/time) as integer
+  //
+  if (IsTypeInUINT64(Value1) && IsTypeInUINT64(Value2)) {
+    Temp64 = HiiValueToUINT64(Value1) - HiiValueToUINT64(Value2);
+    if (Temp64 > 0) {
+      *Result = 1;
+    } else if (Temp64 < 0) {
+      *Result = -1;
+    } else {
+      *Result = 0;
+    }
+    return EFI_SUCCESS;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Search an Option of a Question by its value.
+
+  @param  Question               The Question
+  @param  OptionValue            Value for Option to be searched.
+
+  @retval Pointer                Pointer to the found Option.
+  @retval NULL                   Option not found.
+
+**/
+DISPLAY_QUESTION_OPTION *
+ValueToOption (
+  IN FORM_DISPLAY_ENGINE_STATEMENT   *Question,
+  IN EFI_HII_VALUE                   *OptionValue
+  )
+{
+  LIST_ENTRY               *Link;
+  DISPLAY_QUESTION_OPTION  *Option;
+  INTN                     Result;
+  EFI_HII_VALUE            Value;
+
+  Link = GetFirstNode (&Question->OptionListHead);
+  while (!IsNull (&Question->OptionListHead, Link)) {
+    Option = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+
+    ZeroMem (&Value, sizeof (EFI_HII_VALUE));
+    Value.Type = Option->OptionOpCode->Type;
+    CopyMem (&Value.Value, &Option->OptionOpCode->Value, Option->OptionOpCode->Header.Length - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+    
+    if ((CompareHiiValue (&Value, OptionValue, &Result, NULL) == EFI_SUCCESS) && (Result == 0)) {
+      return Option;
+    }
+
+    Link = GetNextNode (&Question->OptionListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Return data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+
+  @retval Value                  The data to be returned
+
+**/
+UINT64
+GetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index
+  )
+{
+  UINT64 Data;
+
+  ASSERT (Array != NULL);
+
+  Data = 0;
+  switch (Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    Data = (UINT64) *(((UINT8 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    Data = (UINT64) *(((UINT16 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    Data = (UINT64) *(((UINT32 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    Data = (UINT64) *(((UINT64 *) Array) + Index);
+    break;
+
+  default:
+    break;
+  }
+
+  return Data;
+}
+
+
+/**
+  Set value of a data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+  @param  Value                  The value to be set.
+
+**/
+VOID
+SetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index,
+  IN UINT64                   Value
+  )
+{
+
+  ASSERT (Array != NULL);
+
+  switch (Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    *(((UINT8 *) Array) + Index) = (UINT8) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    *(((UINT16 *) Array) + Index) = (UINT16) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    *(((UINT32 *) Array) + Index) = (UINT32) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    *(((UINT64 *) Array) + Index) = (UINT64) Value;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Check whether this value already in the array, if yes, return the index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Value                  The value to be find.
+  @param  Index                  The index in the array which has same value with Value.
+  
+  @retval   TRUE Found the value in the array.
+  @retval   FALSE Not found the value.
+
+**/
+BOOLEAN 
+FindArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINT64                   Value,
+  OUT UINTN                   *Index OPTIONAL
+  )
+{
+  UINTN  Count;
+  UINT64 TmpValue;
+  UINT64 ValueComp;
+  
+  ASSERT (Array != NULL);
+
+  Count    = 0;
+  TmpValue = 0;
+
+  switch (Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    ValueComp = (UINT8) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    ValueComp = (UINT16) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    ValueComp = (UINT32) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    ValueComp = (UINT64) Value;
+    break;
+
+  default:
+    ValueComp = 0;
+    break;
+  }
+
+  while ((TmpValue = GetArrayData (Array, Type, Count)) != 0) {
+    if (ValueComp == TmpValue) {
+      if (Index != NULL) {
+        *Index = Count;
+      }
+      return TRUE;
+    }
+
+    Count ++;
+  }
+
+  return FALSE;
+}
+
+/**
+  Print Question Value according to it's storage width and display attributes.
+
+  @param  Question               The Question to be printed.
+  @param  FormattedNumber        Buffer for output string.
+  @param  BufferSize             The FormattedNumber buffer size in bytes.
+
+  @retval EFI_SUCCESS            Print success.
+  @retval EFI_BUFFER_TOO_SMALL   Buffer size is not enough for formatted number.
+
+**/
+EFI_STATUS
+PrintFormattedNumber (
+  IN FORM_DISPLAY_ENGINE_STATEMENT   *Question,
+  IN OUT CHAR16               *FormattedNumber,
+  IN UINTN                    BufferSize
+  )
+{
+  INT64          Value;
+  CHAR16         *Format;
+  EFI_HII_VALUE  *QuestionValue;
+  EFI_IFR_NUMERIC *NumericOp;
+
+  if (BufferSize < (21 * sizeof (CHAR16))) {
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  QuestionValue = &Question->CurrentValue;
+  NumericOp     = (EFI_IFR_NUMERIC *) Question->OpCode;
+
+  Value = (INT64) QuestionValue->Value.u64;
+  switch (NumericOp->Flags & EFI_IFR_DISPLAY) {
+  case EFI_IFR_DISPLAY_INT_DEC:
+    switch (QuestionValue->Type) {
+    case EFI_IFR_NUMERIC_SIZE_1:
+      Value = (INT64) ((INT8) QuestionValue->Value.u8);
+      break;
+
+    case EFI_IFR_NUMERIC_SIZE_2:
+      Value = (INT64) ((INT16) QuestionValue->Value.u16);
+      break;
+
+    case EFI_IFR_NUMERIC_SIZE_4:
+      Value = (INT64) ((INT32) QuestionValue->Value.u32);
+      break;
+
+    case EFI_IFR_NUMERIC_SIZE_8:
+    default:
+      break;
+    }
+
+    if (Value < 0) {
+      Value = -Value;
+      Format = L"-%ld";
+    } else {
+      Format = L"%ld";
+    }
+    break;
+
+  case EFI_IFR_DISPLAY_UINT_DEC:
+    Format = L"%ld";
+    break;
+
+  case EFI_IFR_DISPLAY_UINT_HEX:
+    Format = L"%lx";
+    break;
+
+  default:
+    return EFI_UNSUPPORTED;
+    break;
+  }
+
+  UnicodeSPrint (FormattedNumber, BufferSize, Format, Value);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Draw a pop up windows based on the dimension, number of lines and
+  strings specified.
+
+  @param RequestedWidth  The width of the pop-up.
+  @param NumberOfLines   The number of lines.
+  @param Marker          The variable argument list for the list of string to be printed.
+
+**/
+VOID
+CreateSharedPopUp (
+  IN  UINTN                       RequestedWidth,
+  IN  UINTN                       NumberOfLines,
+  IN  VA_LIST                     Marker
+  )
+{
+  UINTN   Index;
+  UINTN   Count;
+  CHAR16  Character;
+  UINTN   Start;
+  UINTN   End;
+  UINTN   Top;
+  UINTN   Bottom;
+  CHAR16  *String;
+  UINTN   DimensionsWidth;
+  UINTN   DimensionsHeight;
+
+  DimensionsWidth   = gStatementDimensions.RightColumn - gStatementDimensions.LeftColumn;
+  DimensionsHeight  = gStatementDimensions.BottomRow - gStatementDimensions.TopRow;
+
+  gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+
+  if ((RequestedWidth + 2) > DimensionsWidth) {
+    RequestedWidth = DimensionsWidth - 2;
+  }
+
+  //
+  // Subtract the PopUp width from total Columns, allow for one space extra on
+  // each end plus a border.
+  //
+  Start     = (DimensionsWidth - RequestedWidth - 2) / 2 + gStatementDimensions.LeftColumn + 1;
+  End       = Start + RequestedWidth + 1;
+
+  Top       = ((DimensionsHeight - NumberOfLines - 2) / 2) + gStatementDimensions.TopRow - 1;
+  Bottom    = Top + NumberOfLines + 2;
+
+  Character = BOXDRAW_DOWN_RIGHT;
+  PrintCharAt (Start, Top, Character);
+  Character = BOXDRAW_HORIZONTAL;
+  for (Index = Start; Index + 2 < End; Index++) {
+    PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+  }
+
+  Character = BOXDRAW_DOWN_LEFT;
+  PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+  Character = BOXDRAW_VERTICAL;
+
+  Count = 0;
+  for (Index = Top; Index + 2 < Bottom; Index++, Count++) {
+    String = VA_ARG (Marker, CHAR16*);
+
+    //
+    // This will clear the background of the line - we never know who might have been
+    // here before us.  This differs from the next clear in that it used the non-reverse
+    // video for normal printing.
+    //
+    if (GetStringWidth (String) / 2 > 1) {
+      ClearLines (Start, End, Index + 1, Index + 1, GetPopupColor ());
+    }
+
+    //
+    // Passing in a space results in the assumption that this is where typing will occur
+    //
+    if (String[0] == L' ') {
+      ClearLines (Start + 1, End - 1, Index + 1, Index + 1, GetPopupInverseColor ());
+    }
+
+    //
+    // Passing in a NULL results in a blank space
+    //
+    if (String[0] == CHAR_NULL) {
+      ClearLines (Start, End, Index + 1, Index + 1, GetPopupColor ());
+    }
+
+    PrintStringAt (
+      ((DimensionsWidth - GetStringWidth (String) / 2) / 2) + gStatementDimensions.LeftColumn + 1,
+      Index + 1,
+      String
+      );
+    gST->ConOut->SetAttribute (gST->ConOut, GetPopupColor ());
+    PrintCharAt (Start, Index + 1, Character);
+    PrintCharAt (End - 1, Index + 1, Character);
+  }
+
+  Character = BOXDRAW_UP_RIGHT;
+  PrintCharAt (Start, Bottom - 1, Character);
+  Character = BOXDRAW_HORIZONTAL;
+  for (Index = Start; Index + 2 < End; Index++) {
+    PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+  }
+
+  Character = BOXDRAW_UP_LEFT;
+  PrintCharAt ((UINTN)-1, (UINTN)-1, Character);
+}
+
+/**
+  Draw a pop up windows based on the dimension, number of lines and
+  strings specified.
+
+  @param RequestedWidth  The width of the pop-up.
+  @param NumberOfLines   The number of lines.
+  @param ...             A series of text strings that displayed in the pop-up.
+
+**/
+VOID
+EFIAPI
+CreateMultiStringPopUp (
+  IN  UINTN                       RequestedWidth,
+  IN  UINTN                       NumberOfLines,
+  ...
+  )
+{
+  VA_LIST Marker;
+
+  VA_START (Marker, NumberOfLines);
+
+  CreateSharedPopUp (RequestedWidth, NumberOfLines, Marker);
+
+  VA_END (Marker);
+}
+
+/**
+  Process nothing.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+EmptyEventProcess (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  )
+{
+}
+
+/**
+  Process for the refresh interval statement.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+RefreshTimeOutProcess (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  )
+{
+  WARNING_IF_CONTEXT     *EventInfo;
+  CHAR16                 TimeOutString[MAX_TIME_OUT_LEN];
+
+  EventInfo   = (WARNING_IF_CONTEXT *) Context;
+
+  if (*(EventInfo->TimeOut) == 0) {
+    gBS->CloseEvent (Event);
+
+    gBS->SignalEvent (EventInfo->SyncEvent);
+    return;
+  }
+
+  UnicodeSPrint(TimeOutString, MAX_TIME_OUT_LEN, L"%d", *(EventInfo->TimeOut));
+
+  CreateDialog (NULL, gEmptyString, EventInfo->ErrorInfo, gPressEnter, gEmptyString, TimeOutString, NULL);
+
+  *(EventInfo->TimeOut) -= 1;
+}
+
+/**
+  Display error message for invalid password.
+
+**/
+VOID
+PasswordInvalid (
+  VOID
+  )
+{
+  EFI_INPUT_KEY  Key;
+
+  //
+  // Invalid password, prompt error message
+  //
+  do {
+    CreateDialog (&Key, gEmptyString, gPassowordInvalid, gPressEnter, gEmptyString, NULL);
+  } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+}
+
+/**
+  Process password op code.
+
+  @param  MenuOption             The menu for current password op code.
+
+  @retval EFI_SUCCESS            Question Option process success.
+  @retval Other                  Question Option process fail.
+
+**/
+EFI_STATUS
+PasswordProcess (
+  IN  UI_MENU_OPTION              *MenuOption
+  )
+{
+  CHAR16                          *StringPtr;
+  CHAR16                          *TempString;
+  UINTN                           Maximum;
+  EFI_STATUS                      Status;
+  EFI_IFR_PASSWORD                *PasswordInfo;
+  FORM_DISPLAY_ENGINE_STATEMENT   *Question;
+  EFI_INPUT_KEY                   Key;
+
+  Question     = MenuOption->ThisTag;
+  PasswordInfo = (EFI_IFR_PASSWORD *) Question->OpCode;
+  Maximum      = PasswordInfo->MaxSize;
+  Status       = EFI_SUCCESS;
+
+  StringPtr = AllocateZeroPool ((Maximum + 1) * sizeof (CHAR16));
+  ASSERT (StringPtr);
+  
+  //
+  // Use a NULL password to test whether old password is required
+  //
+  *StringPtr = 0;
+  Status = Question->PasswordCheck (gFormData, Question, StringPtr);
+  if (Status == EFI_NOT_AVAILABLE_YET || Status == EFI_UNSUPPORTED) {
+    //
+    // Password can't be set now. 
+    //
+    FreePool (StringPtr);
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Old password exist, ask user for the old password
+    //
+    Status = ReadString (MenuOption, gPromptForPassword, StringPtr);
+    if (EFI_ERROR (Status)) {
+      FreePool (StringPtr);
+      return Status;
+    }
+
+    //
+    // Check user input old password
+    //
+    Status = Question->PasswordCheck (gFormData, Question, StringPtr);
+    if (EFI_ERROR (Status)) {
+      if (Status == EFI_NOT_READY) {
+        //
+        // Typed in old password incorrect
+        //
+        PasswordInvalid ();
+      } else {
+        Status = EFI_SUCCESS;
+      }
+
+      FreePool (StringPtr);
+      return Status;
+    }
+  }
+  
+  //
+  // Ask for new password
+  //
+  ZeroMem (StringPtr, (Maximum + 1) * sizeof (CHAR16));
+  Status = ReadString (MenuOption, gPromptForNewPassword, StringPtr);
+  if (EFI_ERROR (Status)) {
+    //
+    // Reset state machine for password
+    //
+    Question->PasswordCheck (gFormData, Question, NULL);
+    FreePool (StringPtr);
+    return Status;
+  }
+  
+  //
+  // Confirm new password
+  //
+  TempString = AllocateZeroPool ((Maximum + 1) * sizeof (CHAR16));
+  ASSERT (TempString);
+  Status = ReadString (MenuOption, gConfirmPassword, TempString);
+  if (EFI_ERROR (Status)) {
+    //
+    // Reset state machine for password
+    //
+    Question->PasswordCheck (gFormData, Question, NULL);
+    FreePool (StringPtr);
+    FreePool (TempString);
+    return Status;
+  }
+  
+  //
+  // Compare two typed-in new passwords
+  //
+  if (StrCmp (StringPtr, TempString) == 0) {     
+    gUserInput->InputValue.Buffer = AllocateCopyPool (Question->CurrentValue.BufferLen, StringPtr);
+    gUserInput->InputValue.BufferLen = Question->CurrentValue.BufferLen;
+    gUserInput->InputValue.Type = Question->CurrentValue.Type;
+    gUserInput->InputValue.Value.string = HiiSetString(gFormData->HiiHandle, gUserInput->InputValue.Value.string, StringPtr, NULL);
+    FreePool (StringPtr); 
+
+    Status = EFI_SUCCESS;
+
+    if (EFI_ERROR (Status)) {
+      //
+      // Reset state machine for password
+      //
+      Question->PasswordCheck (gFormData, Question, NULL);
+    }
+
+    return Status;
+  } else {
+    //
+    // Reset state machine for password
+    //
+    Question->PasswordCheck (gFormData, Question, NULL);
+  
+    //
+    // Two password mismatch, prompt error message
+    //
+    do {
+      CreateDialog (&Key, gEmptyString, gConfirmError, gPressEnter, gEmptyString, NULL);
+    } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+    Status = EFI_INVALID_PARAMETER;
+  }
+  
+  FreePool (TempString);
+  FreePool (StringPtr);
+
+  return Status;
+}
+
+/**
+  Process a Question's Option (whether selected or un-selected).
+
+  @param  MenuOption             The MenuOption for this Question.
+  @param  Selected               TRUE: if Question is selected.
+  @param  OptionString           Pointer of the Option String to be displayed.
+  @param  SkipErrorValue         Whether need to return when value without option for it.
+
+  @retval EFI_SUCCESS            Question Option process success.
+  @retval Other                  Question Option process fail.
+
+**/
+EFI_STATUS
+ProcessOptions (
+  IN  UI_MENU_OPTION              *MenuOption,
+  IN  BOOLEAN                     Selected,
+  OUT CHAR16                      **OptionString,
+  IN  BOOLEAN                     SkipErrorValue
+  )
+{
+  EFI_STATUS                      Status;
+  CHAR16                          *StringPtr;
+  UINTN                           Index;
+  FORM_DISPLAY_ENGINE_STATEMENT   *Question;
+  CHAR16                          FormattedNumber[21];
+  UINT16                          Number;
+  CHAR16                          Character[2];
+  EFI_INPUT_KEY                   Key;
+  UINTN                           BufferSize;
+  DISPLAY_QUESTION_OPTION         *OneOfOption;
+  LIST_ENTRY                      *Link;
+  EFI_HII_VALUE                   HiiValue;
+  EFI_HII_VALUE                   *QuestionValue;
+  DISPLAY_QUESTION_OPTION         *Option;
+  UINTN                           Index2;
+  UINT8                           *ValueArray;
+  UINT8                           ValueType;
+  EFI_IFR_ORDERED_LIST            *OrderList;
+  BOOLEAN                         ValueInvalid;
+  UINTN                           MaxLen;
+
+  Status        = EFI_SUCCESS;
+
+  StringPtr     = NULL;
+  Character[1]  = L'\0';
+  *OptionString = NULL;
+  ValueInvalid  = FALSE;
+
+  ZeroMem (FormattedNumber, 21 * sizeof (CHAR16));
+  BufferSize = (gOptionBlockWidth + 1) * 2 * gStatementDimensions.BottomRow;
+
+  Question = MenuOption->ThisTag;
+  QuestionValue = &Question->CurrentValue;
+
+  switch (Question->OpCode->OpCode) {
+  case EFI_IFR_ORDERED_LIST_OP:
+
+    //
+    // Check whether there are Options of this OrderedList
+    //
+    if (IsListEmpty (&Question->OptionListHead)) {
+      break;
+    }
+
+    OrderList = (EFI_IFR_ORDERED_LIST *) Question->OpCode;
+
+    Link = GetFirstNode (&Question->OptionListHead);
+    OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+
+    ValueType =  OneOfOption->OptionOpCode->Type;
+    ValueArray = Question->CurrentValue.Buffer;
+
+    if (Selected) {
+      //
+      // Go ask for input
+      //
+      Status = GetSelectionInputPopUp (MenuOption);
+    } else {
+      //
+      // We now know how many strings we will have, so we can allocate the
+      // space required for the array or strings.
+      //
+      MaxLen = OrderList->MaxContainers * BufferSize / sizeof (CHAR16);
+      *OptionString = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+      ASSERT (*OptionString);
+
+      HiiValue.Type = ValueType;
+      HiiValue.Value.u64 = 0;
+      for (Index = 0; Index < OrderList->MaxContainers; Index++) {
+        HiiValue.Value.u64 = GetArrayData (ValueArray, ValueType, Index);
+        if (HiiValue.Value.u64 == 0) {
+          //
+          // Values for the options in ordered lists should never be a 0
+          //
+          break;
+        }
+
+        OneOfOption = ValueToOption (Question, &HiiValue);
+        if (OneOfOption == NULL) {
+          if (SkipErrorValue) {
+            //
+            // Just try to get the option string, skip the value which not has option.
+            //
+            continue;
+          }
+
+          //
+          // Show error message
+          //
+          do {
+            CreateDialog (&Key, gEmptyString, gOptionMismatch, gPressEnter, gEmptyString, NULL);
+          } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+          //
+          // The initial value of the orderedlist is invalid, force to be valid value
+          // Exit current DisplayForm with new value.
+          //
+          gUserInput->SelectedStatement = Question;
+          gMisMatch = TRUE;
+          ValueArray = AllocateZeroPool (Question->CurrentValue.BufferLen);
+          ASSERT (ValueArray != NULL);
+          gUserInput->InputValue.Buffer    = ValueArray;
+          gUserInput->InputValue.BufferLen = Question->CurrentValue.BufferLen;
+          gUserInput->InputValue.Type      = Question->CurrentValue.Type;
+          
+          Link = GetFirstNode (&Question->OptionListHead);
+          Index2 = 0;
+          while (!IsNull (&Question->OptionListHead, Link) && Index2 < OrderList->MaxContainers) {
+            Option = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+            Link = GetNextNode (&Question->OptionListHead, Link);
+            SetArrayData (ValueArray, ValueType, Index2, Option->OptionOpCode->Value.u64);
+            Index2++;
+          }
+          SetArrayData (ValueArray, ValueType, Index2, 0);
+
+          FreePool (*OptionString);
+          *OptionString = NULL;
+          return EFI_NOT_FOUND;
+        }
+
+        Character[0] = LEFT_ONEOF_DELIMITER;
+        NewStrCat (OptionString[0], MaxLen, Character);
+        StringPtr = GetToken (OneOfOption->OptionOpCode->Option, gFormData->HiiHandle);
+        ASSERT (StringPtr != NULL);
+        NewStrCat (OptionString[0], MaxLen, StringPtr);
+        Character[0] = RIGHT_ONEOF_DELIMITER;
+        NewStrCat (OptionString[0], MaxLen, Character);
+        Character[0] = CHAR_CARRIAGE_RETURN;
+        NewStrCat (OptionString[0], MaxLen, Character);
+        FreePool (StringPtr);
+      }
+
+      //
+      // If valid option more than the max container, skip these options.
+      //
+      if (Index >= OrderList->MaxContainers) {
+        break;
+      }
+
+      //
+      // Search the other options, try to find the one not in the container.
+      //
+      Link = GetFirstNode (&Question->OptionListHead);
+      while (!IsNull (&Question->OptionListHead, Link)) {
+        OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+        Link = GetNextNode (&Question->OptionListHead, Link);
+
+        if (FindArrayData (ValueArray, ValueType, OneOfOption->OptionOpCode->Value.u64, NULL)) {
+          continue;
+        }
+
+        if (SkipErrorValue) {
+          //
+          // Not report error, just get the correct option string info.
+          //
+          Character[0] = LEFT_ONEOF_DELIMITER;
+          NewStrCat (OptionString[0], MaxLen, Character);
+          StringPtr = GetToken (OneOfOption->OptionOpCode->Option, gFormData->HiiHandle);
+          ASSERT (StringPtr != NULL);
+          NewStrCat (OptionString[0], MaxLen, StringPtr);
+          Character[0] = RIGHT_ONEOF_DELIMITER;
+          NewStrCat (OptionString[0], MaxLen, Character);
+          Character[0] = CHAR_CARRIAGE_RETURN;
+          NewStrCat (OptionString[0], MaxLen, Character);
+          FreePool (StringPtr);
+
+          continue;
+        }
+
+        if (!ValueInvalid) {
+          ValueInvalid = TRUE;
+          //
+          // Show error message
+          //
+          do {
+            CreateDialog (&Key, gEmptyString, gOptionMismatch, gPressEnter, gEmptyString, NULL);
+          } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+          //
+          // The initial value of the orderedlist is invalid, force to be valid value
+          // Exit current DisplayForm with new value.
+          //
+          gUserInput->SelectedStatement = Question;
+          gMisMatch = TRUE;
+          ValueArray = AllocateCopyPool (Question->CurrentValue.BufferLen, Question->CurrentValue.Buffer);
+          ASSERT (ValueArray != NULL);
+          gUserInput->InputValue.Buffer    = ValueArray;
+          gUserInput->InputValue.BufferLen = Question->CurrentValue.BufferLen;
+          gUserInput->InputValue.Type      = Question->CurrentValue.Type;
+        }
+       
+        SetArrayData (ValueArray, ValueType, Index++, OneOfOption->OptionOpCode->Value.u64);
+      }
+
+      if (ValueInvalid) {
+        FreePool (*OptionString);
+        *OptionString = NULL;
+        return EFI_NOT_FOUND;
+      }
+    }
+    break;
+
+  case EFI_IFR_ONE_OF_OP:
+    //
+    // Check whether there are Options of this OneOf
+    //
+    if (IsListEmpty (&Question->OptionListHead)) {
+      break;
+    }
+    if (Selected) {
+      //
+      // Go ask for input
+      //
+      Status = GetSelectionInputPopUp (MenuOption);
+    } else {
+      MaxLen = BufferSize / sizeof(CHAR16);
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      OneOfOption = ValueToOption (Question, QuestionValue);
+      if (OneOfOption == NULL) {
+        if (SkipErrorValue) {
+          //
+          // Not report error, just get the correct option string info.
+          //          
+          Link = GetFirstNode (&Question->OptionListHead);
+          OneOfOption = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+        } else {
+          //
+          // Show error message
+          //
+          do {
+            CreateDialog (&Key, gEmptyString, gOptionMismatch, gPressEnter, gEmptyString, NULL);
+          } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+          //
+          // Force the Question value to be valid
+          // Exit current DisplayForm with new value.
+          //
+          Link = GetFirstNode (&Question->OptionListHead);
+          Option = DISPLAY_QUESTION_OPTION_FROM_LINK (Link);
+
+          gUserInput->InputValue.Type = Option->OptionOpCode->Type;
+          switch (gUserInput->InputValue.Type) {
+          case EFI_IFR_TYPE_NUM_SIZE_8:
+            gUserInput->InputValue.Value.u8 = Option->OptionOpCode->Value.u8;
+            break;
+          case EFI_IFR_TYPE_NUM_SIZE_16:
+            CopyMem (&gUserInput->InputValue.Value.u16, &Option->OptionOpCode->Value.u16, sizeof (UINT16));
+            break;
+          case EFI_IFR_TYPE_NUM_SIZE_32:
+            CopyMem (&gUserInput->InputValue.Value.u32, &Option->OptionOpCode->Value.u32, sizeof (UINT32));
+            break;
+          case EFI_IFR_TYPE_NUM_SIZE_64:
+            CopyMem (&gUserInput->InputValue.Value.u64, &Option->OptionOpCode->Value.u64, sizeof (UINT64));
+            break;
+          default:
+            ASSERT (FALSE);
+            break;
+          }
+          gUserInput->SelectedStatement = Question;
+          gMisMatch = TRUE;
+          FreePool (*OptionString);
+          *OptionString = NULL;
+          return EFI_NOT_FOUND;
+        }
+      }
+
+      Character[0] = LEFT_ONEOF_DELIMITER;
+      NewStrCat (OptionString[0], MaxLen, Character);
+      StringPtr = GetToken (OneOfOption->OptionOpCode->Option, gFormData->HiiHandle);
+      ASSERT (StringPtr != NULL);
+      NewStrCat (OptionString[0], MaxLen, StringPtr);
+      Character[0] = RIGHT_ONEOF_DELIMITER;
+      NewStrCat (OptionString[0], MaxLen, Character);
+
+      FreePool (StringPtr);
+    }
+    break;
+
+  case EFI_IFR_CHECKBOX_OP:
+    if (Selected) {
+      //
+      // Since this is a BOOLEAN operation, flip it upon selection
+      //
+      gUserInput->InputValue.Type    = QuestionValue->Type;
+      gUserInput->InputValue.Value.b = (BOOLEAN) (QuestionValue->Value.b ? FALSE : TRUE);
+
+      //
+      // Perform inconsistent check
+      //
+      return EFI_SUCCESS;
+    } else {    
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      *OptionString[0] = LEFT_CHECKBOX_DELIMITER;
+
+      if (QuestionValue->Value.b) {
+        *(OptionString[0] + 1) = CHECK_ON;
+      } else {
+        *(OptionString[0] + 1) = CHECK_OFF;
+      }
+      *(OptionString[0] + 2) = RIGHT_CHECKBOX_DELIMITER;
+    }
+    break;
+
+  case EFI_IFR_NUMERIC_OP:
+    if (Selected) {
+      //
+      // Go ask for input
+      //
+      Status = GetNumericInput (MenuOption);
+    } else {
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      *OptionString[0] = LEFT_NUMERIC_DELIMITER;
+
+      //
+      // Formatted print
+      //
+      PrintFormattedNumber (Question, FormattedNumber, 21 * sizeof (CHAR16));
+      Number = (UINT16) GetStringWidth (FormattedNumber);
+      CopyMem (OptionString[0] + 1, FormattedNumber, Number);
+
+      *(OptionString[0] + Number / 2) = RIGHT_NUMERIC_DELIMITER;
+    }
+    break;
+
+  case EFI_IFR_DATE_OP:
+    if (Selected) {
+      //
+      // This is similar to numerics
+      //
+      Status = GetNumericInput (MenuOption);
+    } else {
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      switch (MenuOption->Sequence) {
+      case 0:
+        *OptionString[0] = LEFT_NUMERIC_DELIMITER;
+        if (QuestionValue->Value.date.Month == 0xff){
+          UnicodeSPrint (OptionString[0] + 1, 21 * sizeof (CHAR16), L"??");
+        } else {
+          UnicodeSPrint (OptionString[0] + 1, 21 * sizeof (CHAR16), L"%02d", QuestionValue->Value.date.Month);
+        }
+        *(OptionString[0] + 3) = DATE_SEPARATOR;
+        break;
+
+      case 1:
+        SetUnicodeMem (OptionString[0], 4, L' ');
+        if (QuestionValue->Value.date.Day == 0xff){
+          UnicodeSPrint (OptionString[0] + 4, 21 * sizeof (CHAR16), L"??");
+        } else {
+          UnicodeSPrint (OptionString[0] + 4, 21 * sizeof (CHAR16), L"%02d", QuestionValue->Value.date.Day);
+        }
+        *(OptionString[0] + 6) = DATE_SEPARATOR;
+        break;
+
+      case 2:
+        SetUnicodeMem (OptionString[0], 7, L' ');
+        if (QuestionValue->Value.date.Year == 0xff){
+          UnicodeSPrint (OptionString[0] + 7, 21 * sizeof (CHAR16), L"????");
+        } else {
+          UnicodeSPrint (OptionString[0] + 7, 21 * sizeof (CHAR16), L"%04d", QuestionValue->Value.date.Year);
+        }
+        *(OptionString[0] + 11) = RIGHT_NUMERIC_DELIMITER;
+        break;
+      }
+    }
+    break;
+
+  case EFI_IFR_TIME_OP:
+    if (Selected) {
+      //
+      // This is similar to numerics
+      //
+      Status = GetNumericInput (MenuOption);
+    } else {
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      switch (MenuOption->Sequence) {
+      case 0:
+        *OptionString[0] = LEFT_NUMERIC_DELIMITER;
+        if (QuestionValue->Value.time.Hour == 0xff){
+          UnicodeSPrint (OptionString[0] + 1, 21 * sizeof (CHAR16), L"??");
+        } else {
+          UnicodeSPrint (OptionString[0] + 1, 21 * sizeof (CHAR16), L"%02d", QuestionValue->Value.time.Hour);
+        }
+        *(OptionString[0] + 3) = TIME_SEPARATOR;
+        break;
+
+      case 1:
+        SetUnicodeMem (OptionString[0], 4, L' ');
+        if (QuestionValue->Value.time.Minute == 0xff){
+          UnicodeSPrint (OptionString[0] + 4, 21 * sizeof (CHAR16), L"??");
+        } else {
+          UnicodeSPrint (OptionString[0] + 4, 21 * sizeof (CHAR16), L"%02d", QuestionValue->Value.time.Minute);
+        }
+        *(OptionString[0] + 6) = TIME_SEPARATOR;
+        break;
+
+      case 2:
+        SetUnicodeMem (OptionString[0], 7, L' ');
+        if (QuestionValue->Value.time.Second == 0xff){
+          UnicodeSPrint (OptionString[0] + 7, 21 * sizeof (CHAR16), L"??");
+        } else {
+          UnicodeSPrint (OptionString[0] + 7, 21 * sizeof (CHAR16), L"%02d", QuestionValue->Value.time.Second);
+        }
+        *(OptionString[0] + 9) = RIGHT_NUMERIC_DELIMITER;
+        break;
+      }
+    }
+    break;
+
+  case EFI_IFR_STRING_OP:
+    if (Selected) {
+      StringPtr = AllocateZeroPool (Question->CurrentValue.BufferLen + sizeof (CHAR16));
+      ASSERT (StringPtr);
+      CopyMem(StringPtr, Question->CurrentValue.Buffer, Question->CurrentValue.BufferLen);
+
+      Status = ReadString (MenuOption, gPromptForData, StringPtr);
+      if (EFI_ERROR (Status)) {
+        FreePool (StringPtr);
+        return Status;
+      }
+      
+      gUserInput->InputValue.Buffer = AllocateCopyPool (Question->CurrentValue.BufferLen, StringPtr);
+      gUserInput->InputValue.BufferLen = Question->CurrentValue.BufferLen;
+      gUserInput->InputValue.Type = Question->CurrentValue.Type;
+      gUserInput->InputValue.Value.string = HiiSetString(gFormData->HiiHandle, gUserInput->InputValue.Value.string, StringPtr, NULL);
+      FreePool (StringPtr);
+      return EFI_SUCCESS;
+    } else {
+      *OptionString = AllocateZeroPool (BufferSize);
+      ASSERT (*OptionString);
+
+      if (((CHAR16 *) Question->CurrentValue.Buffer)[0] == 0x0000) {
+        *(OptionString[0]) = '_';
+      } else {
+        if (Question->CurrentValue.BufferLen < BufferSize) {
+          BufferSize = Question->CurrentValue.BufferLen;
+        }
+        CopyMem (OptionString[0], (CHAR16 *) Question->CurrentValue.Buffer, BufferSize);
+      }
+    }
+    break;
+
+  case EFI_IFR_PASSWORD_OP:
+    if (Selected) {
+      Status = PasswordProcess (MenuOption);
+    }
+    break;
+
+  default:
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Process the help string: Split StringPtr to several lines of strings stored in
+  FormattedString and the glyph width of each line cannot exceed gHelpBlockWidth.
+
+  @param  StringPtr              The entire help string.
+  @param  FormattedString        The oupput formatted string.
+  @param  EachLineWidth          The max string length of each line in the formatted string.
+  @param  RowCount               TRUE: if Question is selected.
+
+**/
+UINTN
+ProcessHelpString (
+  IN  CHAR16  *StringPtr,
+  OUT CHAR16  **FormattedString,
+  OUT UINT16  *EachLineWidth,
+  IN  UINTN   RowCount
+  )
+{
+  UINTN   Index;
+  CHAR16  *OutputString;
+  UINTN   TotalRowNum;
+  UINTN   CheckedNum;
+  UINT16  GlyphWidth;
+  UINT16  LineWidth;
+  UINT16  MaxStringLen;
+  UINT16  StringLen;
+
+  TotalRowNum    = 0;
+  CheckedNum     = 0;
+  GlyphWidth     = 1;
+  Index          = 0;
+  MaxStringLen   = 0;
+  StringLen      = 0;
+
+  //
+  // Set default help string width.
+  //
+  LineWidth      = (UINT16) (gHelpBlockWidth - 1);
+
+  //
+  // Get row number of the String.
+  //
+  while ((StringLen = GetLineByWidth (StringPtr, LineWidth, &GlyphWidth, &Index, &OutputString)) != 0) {
+    if (StringLen > MaxStringLen) {
+      MaxStringLen = StringLen;
+    }
+
+    TotalRowNum ++;
+    FreePool (OutputString);
+  }
+  *EachLineWidth = MaxStringLen;
+
+  *FormattedString = AllocateZeroPool (TotalRowNum * MaxStringLen * sizeof (CHAR16));
+  ASSERT (*FormattedString != NULL);
+
+  //
+  // Generate formatted help string array.
+  //
+  GlyphWidth  = 1;
+  Index       = 0;
+  while((StringLen = GetLineByWidth (StringPtr, LineWidth, &GlyphWidth, &Index, &OutputString)) != 0) {
+    CopyMem (*FormattedString + CheckedNum * MaxStringLen, OutputString, StringLen * sizeof (CHAR16));
+    CheckedNum ++;
+    FreePool (OutputString);
+  }
+
+  return TotalRowNum; 
+}
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthConfigureVfr.Vfr b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthConfigureVfr.Vfr
new file mode 100644
index 0000000000..12c8d6063c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthConfigureVfr.Vfr
@@ -0,0 +1,39 @@
+///** @file
+//  
+//    VFR to produce the formset used by BDS. This form only lists
+//    the Configure Required driver health instances.
+//  
+//  Copyright (c) 2013 - 2015, 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 "DriverHealthManagerVfr.h"
+
+formset
+  guid      = DRIVER_HEALTH_CONFIGURE_FORMSET_GUID,
+  title     = STRING_TOKEN(STR_FORM_TITLE),
+  help      = STRING_TOKEN(STR_FORM_HELP),
+  classguid = DRIVER_HEALTH_CONFIGURE_FORMSET_GUID,
+
+  form formid = DRIVER_HEALTH_FORM_ID,
+      title  = STRING_TOKEN(STR_FORM_TITLE);
+
+      label LABEL_BEGIN;
+      label LABEL_END;
+            
+      suppressif TRUE;
+          text
+              help  = STRING_TOKEN(STR_NULL),
+              text  = STRING_TOKEN(STR_NULL),
+              flags = INTERACTIVE,
+              key   = QUESTION_ID_REFRESH_CONFIGURE;
+      endif;
+
+  endform;
+endformset;
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.c b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.c
new file mode 100644
index 0000000000..16b703495a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.c
@@ -0,0 +1,990 @@
+/** @file
+  This module produces two driver health manager forms.
+  One will be used by BDS core to configure the Configured Required
+  driver health instances, the other will be automatically included by
+  firmware setup (UI).
+
+Copyright (c) 2013 - 2015, 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 "DriverHealthManagerDxe.h"
+#include "DriverHealthManagerVfr.h"
+
+EFI_HII_CONFIG_ACCESS_PROTOCOL mDriverHealthManagerConfigAccess     = {
+  DriverHealthManagerFakeExtractConfig,
+  DriverHealthManagerFakeRouteConfig,
+  DriverHealthManagerCallback
+};
+
+EFI_GUID mDriverHealthManagerForm = DRIVER_HEALTH_MANAGER_FORMSET_GUID;
+
+FORM_DEVICE_PATH  mDriverHealthManagerFormDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    EFI_CALLER_ID_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+EFI_HII_HANDLE                       mDriverHealthManagerHiiHandle;
+EFI_BOOT_MANAGER_DRIVER_HEALTH_INFO  *mDriverHealthManagerHealthInfo     = NULL;
+UINTN                                mDriverHealthManagerHealthInfoCount = 0;
+EFI_HII_DATABASE_PROTOCOL            *mDriverHealthManagerDatabase;
+
+
+extern UINT8 DriverHealthManagerVfrBin[];
+extern UINT8 DriverHealthConfigureVfrBin[];
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Request         A null-terminated Unicode string in <ConfigRequest> format.
+  @param Progress        On return, points to a character in the Request string.
+                         Points to the string's null terminator if request was successful.
+                         Points to the most recent '&' before the first failing name/value
+                         pair (or the beginning of the string if the failure is in the
+                         first name/value pair) if the request was not successful.
+  @param Results         A null-terminated Unicode string in <ConfigAltResp> format which
+                         has all values filled in for the names in the Request string.
+                         String to be allocated by the called function.
+
+  @retval  EFI_SUCCESS            The Results is filled with the requested values.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval  EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerFakeExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Request;
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Configuration   A null-terminated Unicode string in <ConfigResp> format.
+  @param Progress        A pointer to a string filled in with the offset of the most
+                         recent '&' before the first failing name/value pair (or the
+                         beginning of the string if the failure is in the first
+                         name/value pair) or the terminating NULL if all was successful.
+
+  @retval  EFI_SUCCESS            The Results is processed successfully.
+  @retval  EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerFakeRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+
+  Install the health manager forms.
+  One will be used by BDS core to configure the Configured Required
+  driver health instances, the other will be automatically included by
+  firmware setup (UI).
+
+  @param ImageHandle     The image handle.
+  @param SystemTable     The system table.
+
+  @retval  EFI_SUCEESS   The health manager forms are successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeDriverHealthManager (
+  EFI_HANDLE                 ImageHandle,
+  EFI_SYSTEM_TABLE           *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_HANDLE                  Handle;
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &mDriverHealthManagerDatabase
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mDriverHealthManagerFormDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &mDriverHealthManagerConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+
+  //
+  // Publish Driver Health HII data.
+  //
+  mDriverHealthManagerHiiHandle = HiiAddPackages (
+                                    &gEfiCallerIdGuid,
+                                    Handle,
+                                    DriverHealthManagerVfrBin,
+                                    DriverHealthConfigureVfrBin,
+                                    STRING_ARRAY_NAME,
+                                    NULL
+                                    );
+  ASSERT (mDriverHealthManagerHiiHandle != NULL);
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Select the best matching language according to front page policy for best user experience.
+
+  This function supports both ISO 639-2 and RFC 4646 language codes, but language
+  code types may not be mixed in a single call to this function.
+
+  @param  SupportedLanguages   A pointer to a Null-terminated ASCII string that
+                               contains a set of language codes in the format
+                               specified by Iso639Language.
+  @param  Iso639Language       If TRUE, then all language codes are assumed to be
+                               in ISO 639-2 format.  If FALSE, then all language
+                               codes are assumed to be in RFC 4646 language format.
+
+  @retval NULL                 The best matching language could not be found in SupportedLanguages.
+  @retval NULL                 There are not enough resources available to return the best matching
+                               language.
+  @retval Other                A pointer to a Null-terminated ASCII string that is the best matching
+                               language in SupportedLanguages.
+**/
+CHAR8 *
+DriverHealthManagerSelectBestLanguage (
+  IN CHAR8        *SupportedLanguages,
+  IN BOOLEAN      Iso639Language
+  )
+{
+  CHAR8           *LanguageVariable;
+  CHAR8           *BestLanguage;
+
+  GetEfiGlobalVariable2 (Iso639Language ? L"Lang" : L"PlatformLang", (VOID**)&LanguageVariable, NULL);
+
+  BestLanguage = GetBestLanguage(
+                   SupportedLanguages,
+                   Iso639Language,
+                   (LanguageVariable != NULL) ? LanguageVariable : "",
+                   Iso639Language ? "eng" : "en-US",
+                   NULL
+                   );
+  if (LanguageVariable != NULL) {
+    FreePool (LanguageVariable);
+  }
+
+  return BestLanguage;
+}
+
+
+
+/**
+
+  This is an internal worker function to get the Component Name (2) protocol interface
+  and the language it supports.
+
+  @param  ProtocolGuid         A pointer to an EFI_GUID. It points to Component Name (2) protocol GUID.
+  @param  DriverBindingHandle  The handle on which the Component Name (2) protocol instance is retrieved.
+  @param  ComponentName        A pointer to the Component Name (2) protocol interface.
+  @param  SupportedLanguage    The best suitable language that matches the SupportedLangues interface for the
+                               located Component Name (2) instance.
+
+  @retval EFI_SUCCESS          The Component Name (2) protocol instance is successfully located and we find
+                               the best matching language it support.
+  @retval EFI_UNSUPPORTED      The input Language is not supported by the Component Name (2) protocol.
+  @retval Other                Some error occurs when locating Component Name (2) protocol instance or finding
+                               the supported language.
+
+**/
+EFI_STATUS
+DriverHealthManagerGetComponentNameWorker (
+  IN  EFI_GUID                    *ProtocolGuid,
+  IN  EFI_HANDLE                  DriverBindingHandle,
+  OUT EFI_COMPONENT_NAME_PROTOCOL **ComponentName,
+  OUT CHAR8                       **SupportedLanguage
+  )
+{
+  EFI_STATUS                      Status;
+
+  //
+  // Locate Component Name (2) protocol on the driver binging handle.
+  //
+  Status = gBS->OpenProtocol (
+                 DriverBindingHandle,
+                 ProtocolGuid,
+                 (VOID **) ComponentName,
+                 NULL,
+                 NULL,
+                 EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                 );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Apply shell policy to select the best language.
+  //
+  *SupportedLanguage = DriverHealthManagerSelectBestLanguage (
+                         (*ComponentName)->SupportedLanguages,
+                         (BOOLEAN) (ProtocolGuid == &gEfiComponentNameProtocolGuid)
+                         );
+  if (*SupportedLanguage == NULL) {
+    Status = EFI_UNSUPPORTED;
+  }
+
+  return Status;
+}
+
+/**
+
+  This is an internal worker function to get driver name from Component Name (2) protocol interface.
+
+  @param  ProtocolGuid         A pointer to an EFI_GUID. It points to Component Name (2) protocol GUID.
+  @param  DriverBindingHandle  The handle on which the Component Name (2) protocol instance is retrieved.
+  @param  DriverName           A pointer to the Unicode string to return. This Unicode string is the name
+                               of the driver specified by This.
+
+  @retval EFI_SUCCESS          The driver name is successfully retrieved from Component Name (2) protocol
+                               interface.
+  @retval Other                The driver name cannot be retrieved from Component Name (2) protocol
+                               interface.
+
+**/
+EFI_STATUS
+DriverHealthManagerGetDriverNameWorker (
+  IN  EFI_GUID    *ProtocolGuid,
+  IN  EFI_HANDLE  DriverBindingHandle,
+  OUT CHAR16      **DriverName
+  )
+{
+  EFI_STATUS                     Status;
+  CHAR8                          *BestLanguage;
+  EFI_COMPONENT_NAME_PROTOCOL    *ComponentName;
+
+  //
+  // Retrieve Component Name (2) protocol instance on the driver binding handle and
+  // find the best language this instance supports.
+  //
+  Status = DriverHealthManagerGetComponentNameWorker (
+             ProtocolGuid,
+             DriverBindingHandle,
+             &ComponentName,
+             &BestLanguage
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the driver name from Component Name (2) protocol instance on the driver binging handle.
+  //
+  Status = ComponentName->GetDriverName (
+                            ComponentName,
+                            BestLanguage,
+                            DriverName
+                            );
+  FreePool (BestLanguage);
+
+  return Status;
+}
+
+/**
+  This function gets driver name from Component Name 2 protocol interface and Component Name protocol interface
+  in turn. It first tries UEFI 2.0 Component Name 2 protocol interface and try to get the driver name.
+  If the attempt fails, it then gets the driver name from EFI 1.1 Component Name protocol for backward
+  compatibility support.
+
+  @param  DriverBindingHandle  The handle on which the Component Name (2) protocol instance is retrieved.
+
+  @return A pointer to the Unicode string to return. This Unicode string is the name of the controller
+          specified by ControllerHandle and ChildHandle.
+
+
+**/
+CHAR16 *
+DriverHealthManagerGetDriverName (
+  IN  EFI_HANDLE  DriverBindingHandle
+  )
+{
+  EFI_STATUS      Status;
+  CHAR16          *DriverName;
+
+  //
+  // Get driver name from UEFI 2.0 Component Name 2 protocol interface.
+  //
+  Status = DriverHealthManagerGetDriverNameWorker (&gEfiComponentName2ProtocolGuid, DriverBindingHandle, &DriverName);
+  if (EFI_ERROR (Status)) {
+    //
+    // If it fails to get the driver name from Component Name protocol interface, we should fall back on
+    // EFI 1.1 Component Name protocol interface.
+    //
+    Status = DriverHealthManagerGetDriverNameWorker (&gEfiComponentNameProtocolGuid, DriverBindingHandle, &DriverName);
+  }
+
+  if (!EFI_ERROR (Status)) {
+    return AllocateCopyPool (StrSize (DriverName), DriverName);
+  } else {
+    return ConvertDevicePathToText (DevicePathFromHandle (DriverBindingHandle), FALSE, TRUE);
+  }
+}
+
+
+
+/**
+  This function gets controller name from Component Name 2 protocol interface and Component Name protocol interface
+  in turn. It first tries UEFI 2.0 Component Name 2 protocol interface and try to get the controller name.
+  If the attempt fails, it then gets the controller name from EFI 1.1 Component Name protocol for backward
+  compatibility support.
+
+  @param  ProtocolGuid         A pointer to an EFI_GUID. It points to Component Name (2) protocol GUID.
+  @param  DriverBindingHandle  The handle on which the Component Name (2) protocol instance is retrieved.
+  @param  ControllerHandle     The handle of a controller that the driver specified by This is managing.
+                               This handle specifies the controller whose name is to be returned.
+  @param  ChildHandle          The handle of the child controller to retrieve the name of. This is an
+                               optional parameter that may be NULL. It will be NULL for device drivers.
+                               It will also be NULL for bus drivers that attempt to retrieve the name
+                               of the bus controller. It will not be NULL for a bus driver that attempts
+                               to retrieve the name of a child controller.
+  @param  ControllerName       A pointer to the Unicode string to return. This Unicode string
+                               is the name of the controller specified by ControllerHandle and ChildHandle.
+
+  @retval  EFI_SUCCESS         The controller name is successfully retrieved from Component Name (2) protocol
+                               interface.
+  @retval  Other               The controller name cannot be retrieved from Component Name (2) protocol.
+
+**/
+EFI_STATUS
+DriverHealthManagerGetControllerNameWorker (
+  IN  EFI_GUID    *ProtocolGuid,
+  IN  EFI_HANDLE  DriverBindingHandle,
+  IN  EFI_HANDLE  ControllerHandle,
+  IN  EFI_HANDLE  ChildHandle,
+  OUT CHAR16      **ControllerName
+  )
+{
+  EFI_STATUS                     Status;
+  CHAR8                          *BestLanguage;
+  EFI_COMPONENT_NAME_PROTOCOL    *ComponentName;
+
+  //
+  // Retrieve Component Name (2) protocol instance on the driver binding handle and
+  // find the best language this instance supports.
+  //
+  Status = DriverHealthManagerGetComponentNameWorker (
+             ProtocolGuid,
+             DriverBindingHandle,
+             &ComponentName,
+             &BestLanguage
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the controller name from Component Name (2) protocol instance on the driver binging handle.
+  //
+  Status = ComponentName->GetControllerName (
+                            ComponentName,
+                            ControllerHandle,
+                            ChildHandle,
+                            BestLanguage,
+                            ControllerName
+                            );
+  FreePool (BestLanguage);
+
+  return Status;
+}
+
+/**
+
+  This function gets controller name from Component Name 2 protocol interface and Component Name protocol interface
+  in turn. It first tries UEFI 2.0 Component Name 2 protocol interface and try to get the controller name.
+  If the attempt fails, it then gets the controller name from EFI 1.1 Component Name protocol for backward
+  compatibility support.
+
+  @param  DriverBindingHandle  The handle on which the Component Name (2) protocol instance is retrieved.
+  @param  ControllerHandle     The handle of a controller that the driver specified by DriverBindingHandle is managing.
+                               This handle specifies the controller whose name is to be returned.
+  @param  ChildHandle          The handle of the child controller to retrieve the name of. This is an
+                               optional parameter that may be NULL. It will be NULL for device drivers.
+                               It will also be NULL for bus drivers that attempt to retrieve the name
+                               of the bus controller. It will not be NULL for a bus driver that attempts
+                               to retrieve the name of a child controller.
+
+  @return A pointer to the Unicode string to return. This Unicode string is the name of the controller
+          specified by ControllerHandle and ChildHandle.
+**/
+CHAR16 *
+DriverHealthManagerGetControllerName (
+  IN  EFI_HANDLE  DriverBindingHandle,
+  IN  EFI_HANDLE  ControllerHandle,
+  IN  EFI_HANDLE  ChildHandle
+  )
+{
+  EFI_STATUS      Status;
+  CHAR16          *ControllerName;
+
+  //
+  // Get controller name from UEFI 2.0 Component Name 2 protocol interface.
+  //
+  Status = DriverHealthManagerGetControllerNameWorker (
+             &gEfiComponentName2ProtocolGuid,
+             DriverBindingHandle,
+             ControllerHandle,
+             ChildHandle,
+             &ControllerName
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // If it fails to get the controller name from Component Name protocol interface, we should fall back on
+    // EFI 1.1 Component Name protocol interface.
+    //
+    Status = DriverHealthManagerGetControllerNameWorker (
+               &gEfiComponentNameProtocolGuid,
+               DriverBindingHandle,
+               ControllerHandle,
+               ChildHandle,
+               &ControllerName
+               );
+  }
+
+  if (!EFI_ERROR (Status)) {
+    return AllocateCopyPool (StrSize (ControllerName), ControllerName);
+  } else {
+    return ConvertDevicePathToText (DevicePathFromHandle (ChildHandle != NULL ? ChildHandle : ControllerHandle), FALSE, TRUE);
+  }
+}
+
+/**
+  The repair notify function.
+  @param Value  A value between 0 and Limit that identifies the current progress
+                of the repair operation.
+  @param Limit  The maximum value of Value for the current repair operation.
+                If Limit is 0, then the completion progress is indeterminate.
+                For example, a driver that wants to specify progress in percent
+                would use a Limit value of 100.
+
+  @retval EFI_SUCCESS  Successfully return from the notify function.
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerRepairNotify (
+  IN UINTN        Value,
+  IN UINTN        Limit
+  )
+{
+  DEBUG ((EFI_D_INFO, "[DriverHealthManagement]RepairNotify: %d/%d\n", Value, Limit));
+  return EFI_SUCCESS;
+}
+
+/**
+  Look for the formset GUID which has the gEfiHiiDriverHealthFormsetGuid class GUID in the specified HII package list.
+
+  @param Handle         Handle to the HII package list.
+  @param FormsetGuid    Return the formset GUID.
+
+  @retval EFI_SUCCESS   The formset is found successfully.
+  @retval EFI_NOT_FOUND The formset cannot be found.
+**/
+EFI_STATUS
+DriverHealthManagerGetFormsetId (
+  IN  EFI_HII_HANDLE   Handle,
+  OUT EFI_GUID         *FormsetGuid
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINTN                        BufferSize;
+  UINT8                        *Package;
+  UINT8                        *OpCodeData;
+  UINT32                       Offset;
+  UINT32                       Offset2;
+  EFI_HII_PACKAGE_HEADER       PackageHeader;
+  UINT8                        Index;
+  UINT8                        NumberOfClassGuid;
+  EFI_GUID                     *ClassGuid;
+
+  //
+  // Get HII PackageList
+  //
+  BufferSize     = 0;
+  HiiPackageList = NULL;
+  Status = mDriverHealthManagerDatabase->ExportPackageLists (mDriverHealthManagerDatabase, Handle, &BufferSize, HiiPackageList);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    HiiPackageList = AllocatePool (BufferSize);
+    ASSERT (HiiPackageList != NULL);
+
+    Status = mDriverHealthManagerDatabase->ExportPackageLists (mDriverHealthManagerDatabase, Handle, &BufferSize, HiiPackageList);
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (HiiPackageList != NULL);
+
+  //
+  // Get Form package from this HII package List
+  //
+  for (Offset = sizeof (EFI_HII_PACKAGE_LIST_HEADER); Offset < ReadUnaligned32 (&HiiPackageList->PackageLength); Offset += PackageHeader.Length) {
+    Package = ((UINT8 *) HiiPackageList) + Offset;
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+
+    if (PackageHeader.Type == EFI_HII_PACKAGE_FORMS) {
+      //
+      // Search FormSet in this Form Package
+      //
+      
+      for (Offset2 = sizeof (EFI_HII_PACKAGE_HEADER); Offset2 < PackageHeader.Length; Offset2 += ((EFI_IFR_OP_HEADER *) OpCodeData)->Length) {
+        OpCodeData = Package + Offset2;
+
+        if ((((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode == EFI_IFR_FORM_SET_OP) &&
+            (((EFI_IFR_OP_HEADER *) OpCodeData)->Length > OFFSET_OF (EFI_IFR_FORM_SET, Flags))) {
+          //
+          // Try to compare against formset class GUID
+          //
+          NumberOfClassGuid = (UINT8) (((EFI_IFR_FORM_SET *) OpCodeData)->Flags & 0x3);
+          ClassGuid         = (EFI_GUID *) (OpCodeData + sizeof (EFI_IFR_FORM_SET));
+          for (Index = 0; Index < NumberOfClassGuid; Index++) {
+            if (CompareGuid (&gEfiHiiDriverHealthFormsetGuid, &ClassGuid[Index])) {
+              CopyMem (FormsetGuid, &((EFI_IFR_FORM_SET *) OpCodeData)->Guid, sizeof (EFI_GUID));
+              FreePool (HiiPackageList);
+              return EFI_SUCCESS;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  //
+  // Form package not found in this Package List
+  //
+  FreePool (HiiPackageList);
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Processes a single controller using the EFI Driver Health Protocol associated with
+  that controller.
+
+  @param DriverHealth       A pointer to the EFI_DRIVER_HEALTH_PROTOCOL instance.
+  @param ControllerHandle   The class guid specifies which form set will be displayed.
+  @param ChildHandle        The handle of the child controller to retrieve the health
+                            status on.  This is an optional parameter that may be NULL.
+  @param HealthStatus       The health status of the controller.
+  @param MessageList        An array of warning or error messages associated
+                            with the controller specified by ControllerHandle and
+                            ChildHandle.  This is an optional parameter that may be NULL.
+  @param FormHiiHandle      The HII handle for an HII form associated with the
+                            controller specified by ControllerHandle and ChildHandle.
+**/
+VOID
+DriverHealthManagerProcessSingleControllerHealth (
+  IN  EFI_DRIVER_HEALTH_PROTOCOL         *DriverHealth,
+  IN  EFI_HANDLE                         ControllerHandle, OPTIONAL
+  IN  EFI_HANDLE                         ChildHandle,      OPTIONAL
+  IN  EFI_DRIVER_HEALTH_STATUS           HealthStatus,
+  IN  EFI_DRIVER_HEALTH_HII_MESSAGE      **MessageList,    OPTIONAL
+  IN  EFI_HII_HANDLE                     FormHiiHandle
+  )
+{
+  EFI_STATUS                         Status;
+
+  ASSERT (HealthStatus != EfiDriverHealthStatusConfigurationRequired);
+  //
+  // If the module need to be repaired or reconfiguration,  will process it until
+  // reach a terminal status. The status from EfiDriverHealthStatusRepairRequired after repair
+  // will be in (Health, Failed, Configuration Required).
+  //
+  switch (HealthStatus) {
+
+  case EfiDriverHealthStatusRepairRequired:
+    Status = DriverHealth->Repair (
+                             DriverHealth,
+                             ControllerHandle,
+                             ChildHandle,
+                             DriverHealthManagerRepairNotify
+                             );
+    break;
+
+  case EfiDriverHealthStatusRebootRequired:
+    gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+    break;
+
+  case EfiDriverHealthStatusReconnectRequired:
+    Status = gBS->DisconnectController (ControllerHandle, NULL, NULL);
+    if (EFI_ERROR (Status)) {
+      //
+      // Disconnect failed.  Need to promote reconnect to a reboot.
+      //
+      gRT->ResetSystem (EfiResetWarm, EFI_SUCCESS, 0, NULL);
+    } else {
+      gBS->ConnectController (ControllerHandle, NULL, NULL, TRUE);
+    }
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Update the form to include the driver health instances.
+
+  @param ConfigureOnly  Only include the configure required driver health instances
+                        when TRUE, include all the driver health instances otherwise.
+**/
+VOID
+DriverHealthManagerUpdateForm (
+  BOOLEAN                     ConfigureOnly
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_IFR_GUID_LABEL          *StartLabel;
+  EFI_IFR_GUID_LABEL          *EndLabel;
+  VOID                        *StartOpCodeHandle;
+  VOID                        *EndOpCodeHandle;
+  UINTN                       Index;
+  EFI_STRING_ID               Prompt;
+  EFI_STRING_ID               Help;
+  CHAR16                      String[512];
+  UINTN                       StringCount;
+  EFI_STRING                  TmpString;
+  EFI_STRING                  DriverName;
+  EFI_STRING                  ControllerName;
+  UINTN                       MessageIndex;
+  EFI_HANDLE                  DriverHandle;
+  EFI_STRING_ID               DevicePath;
+  EFI_GUID                    FormsetGuid;
+
+  EfiBootManagerFreeDriverHealthInfo (mDriverHealthManagerHealthInfo, mDriverHealthManagerHealthInfoCount);
+  mDriverHealthManagerHealthInfo = EfiBootManagerGetDriverHealthInfo (&mDriverHealthManagerHealthInfoCount);
+
+  //
+  // Allocate space for creation of UpdateData Buffer
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = LABEL_BEGIN;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  for (Index = 0; Index < mDriverHealthManagerHealthInfoCount; Index++) {
+    if (ConfigureOnly && mDriverHealthManagerHealthInfo[Index].HealthStatus != EfiDriverHealthStatusConfigurationRequired) {
+      continue;
+    }
+    DriverName = DriverHealthManagerGetDriverName (mDriverHealthManagerHealthInfo[Index].DriverHealthHandle);
+    ASSERT (DriverName != NULL);
+
+    if (mDriverHealthManagerHealthInfo[Index].ControllerHandle == NULL) {
+      //
+      // The ControllerHandle is set to NULL and the HealthStatus is set to EfiDriverHealthStatusHealthy
+      // if all the controllers managed by the driver are in healthy state.
+      //
+      ASSERT (mDriverHealthManagerHealthInfo[Index].HealthStatus == EfiDriverHealthStatusHealthy);
+      UnicodeSPrint (String, sizeof (String), L"%s", DriverName);
+    } else {
+      ControllerName = DriverHealthManagerGetControllerName (
+                         mDriverHealthManagerHealthInfo[Index].DriverHealthHandle,
+                         mDriverHealthManagerHealthInfo[Index].ControllerHandle,
+                         mDriverHealthManagerHealthInfo[Index].ChildHandle
+                         );
+      ASSERT (ControllerName != NULL);
+      UnicodeSPrint (String, sizeof (String), L"%s    %s", DriverName, ControllerName);
+      FreePool (ControllerName);
+    }
+    FreePool (DriverName);
+
+    Prompt = HiiSetString (mDriverHealthManagerHiiHandle, 0, String, NULL);
+
+    switch(mDriverHealthManagerHealthInfo[Index].HealthStatus) {
+    case EfiDriverHealthStatusRepairRequired:
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_REPAIR_REQUIRED), NULL);
+      break;
+    case EfiDriverHealthStatusConfigurationRequired:
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_CONFIGURATION_REQUIRED), NULL);
+      break;
+    case EfiDriverHealthStatusFailed:
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_FAILED), NULL);
+      break;
+    case EfiDriverHealthStatusReconnectRequired:
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_RECONNECT_REQUIRED), NULL);
+      break;
+    case EfiDriverHealthStatusRebootRequired:
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_REBOOT_REQUIRED), NULL);
+      break;
+    default:
+      ASSERT (mDriverHealthManagerHealthInfo[Index].HealthStatus == EfiDriverHealthStatusHealthy);
+      TmpString = HiiGetString (mDriverHealthManagerHiiHandle, STRING_TOKEN (STR_HEALTHY), NULL);
+      break;
+    }
+    StringCount = UnicodeSPrint (String, sizeof (String), L"%s\n", TmpString);
+    FreePool (TmpString);
+
+    //
+    // Add the message of the Module itself provided as the help.
+    //
+    if (mDriverHealthManagerHealthInfo[Index].MessageList != NULL) {
+      for (MessageIndex = 0; mDriverHealthManagerHealthInfo[Index].MessageList[MessageIndex].HiiHandle != NULL; MessageIndex++) {
+        TmpString = HiiGetString (
+                      mDriverHealthManagerHealthInfo[Index].MessageList[MessageIndex].HiiHandle,
+                      mDriverHealthManagerHealthInfo[Index].MessageList[MessageIndex].StringId,
+                      NULL
+                      );
+        StringCount += UnicodeSPrint (String + StringCount, sizeof (String) - sizeof (String[0]) * StringCount, L"\n%s", TmpString);
+        FreePool (TmpString);
+      }
+    }
+    Help = HiiSetString (mDriverHealthManagerHiiHandle, 0, String, NULL);
+
+    switch (mDriverHealthManagerHealthInfo[Index].HealthStatus) {
+    case EfiDriverHealthStatusConfigurationRequired:
+      Status = mDriverHealthManagerDatabase->GetPackageListHandle (
+                                               mDriverHealthManagerDatabase,
+                                               mDriverHealthManagerHealthInfo[Index].HiiHandle,
+                                               &DriverHandle
+                                               );
+      ASSERT_EFI_ERROR (Status);
+      TmpString  = ConvertDevicePathToText (DevicePathFromHandle (DriverHandle), FALSE, TRUE);
+      DevicePath = HiiSetString (mDriverHealthManagerHiiHandle, 0, TmpString, NULL);
+      FreePool (TmpString);
+
+      Status = DriverHealthManagerGetFormsetId (mDriverHealthManagerHealthInfo[Index].HiiHandle, &FormsetGuid);
+      ASSERT_EFI_ERROR (Status);
+
+      HiiCreateGotoExOpCode (
+        StartOpCodeHandle,
+        0,
+        Prompt,
+        Help,
+        0,
+        0,
+        0,
+        &FormsetGuid,
+        DevicePath
+        );
+      break;
+
+    case EfiDriverHealthStatusRepairRequired:
+    case EfiDriverHealthStatusReconnectRequired:
+    case EfiDriverHealthStatusRebootRequired:
+      HiiCreateActionOpCode (
+        StartOpCodeHandle,
+        (EFI_QUESTION_ID) (Index + QUESTION_ID_DRIVER_HEALTH_BASE),
+        Prompt,
+        Help,
+        EFI_IFR_FLAG_CALLBACK,
+        0
+        );
+      break;
+
+    default:
+      ASSERT (mDriverHealthManagerHealthInfo[Index].HealthStatus == EfiDriverHealthStatusHealthy ||
+              mDriverHealthManagerHealthInfo[Index].HealthStatus == EfiDriverHealthStatusFailed);
+      HiiCreateTextOpCode (
+        StartOpCodeHandle,
+        Prompt,
+        Help,
+        0
+        );
+      break;
+    }
+  }
+
+  Status = HiiUpdateForm (
+             mDriverHealthManagerHiiHandle,
+             ConfigureOnly ? PcdGetPtr (PcdDriverHealthConfigureForm) : &mDriverHealthManagerForm,
+             DRIVER_HEALTH_FORM_ID,
+             StartOpCodeHandle,
+             EndOpCodeHandle
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+}
+
+/**
+  Called when the form is closing to remove the dynamicly added string from the HII package list.
+**/
+VOID
+DriverHealthManagerCleanDynamicString (
+  VOID
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINTN                        BufferSize;
+  EFI_HII_PACKAGE_HEADER       *PackageHeader;
+  UINT32                       FixedStringSize;
+
+  FixedStringSize = *(UINT32 *) &STRING_ARRAY_NAME - sizeof (UINT32);
+  BufferSize      = sizeof (EFI_HII_PACKAGE_LIST_HEADER) + FixedStringSize + sizeof (EFI_HII_PACKAGE_HEADER);
+  HiiPackageList  = AllocatePool (BufferSize);
+  ASSERT (HiiPackageList != NULL);
+
+  HiiPackageList->PackageLength = (UINT32) BufferSize;
+  CopyMem (&HiiPackageList->PackageListGuid, &gEfiCallerIdGuid, sizeof (EFI_GUID));
+
+  PackageHeader = (EFI_HII_PACKAGE_HEADER *) (HiiPackageList + 1);
+  CopyMem (PackageHeader, STRING_ARRAY_NAME + sizeof (UINT32), FixedStringSize);
+
+  PackageHeader = (EFI_HII_PACKAGE_HEADER *) ((UINT8 *) PackageHeader + PackageHeader->Length);
+  PackageHeader->Type   = EFI_HII_PACKAGE_END;
+  PackageHeader->Length = sizeof (EFI_HII_PACKAGE_HEADER);
+
+  Status = mDriverHealthManagerDatabase->UpdatePackageList (
+                                           mDriverHealthManagerDatabase,
+                                           mDriverHealthManagerHiiHandle,
+                                           HiiPackageList
+                                           );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Form package not found in this Package List
+  //
+  FreePool (HiiPackageList);
+}
+
+/**
+  This function is invoked if user selected a interactive opcode from Driver Health's
+  Formset.
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  UINTN                                      Index;
+
+  if (QuestionId == QUESTION_ID_REFRESH_MANAGER || QuestionId == QUESTION_ID_REFRESH_CONFIGURE) {
+    if (Action == EFI_BROWSER_ACTION_FORM_OPEN) {
+      DriverHealthManagerUpdateForm ((BOOLEAN) (QuestionId == QUESTION_ID_REFRESH_CONFIGURE));
+    } else if (Action == EFI_BROWSER_ACTION_FORM_CLOSE) {
+      DriverHealthManagerCleanDynamicString ();
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (Action != EFI_BROWSER_ACTION_CHANGED) {
+    //
+    // Do nothing for other UEFI Action. Only do call back when data is changed.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((Value == NULL) || (ActionRequest == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((EFI_D_ERROR, "QuestionId = %x\n", QuestionId));
+
+  //
+  // We will have returned from processing a callback - user either hit ESC to exit, or selected
+  // a target to display.
+  // Process the diver health status states here.
+  //
+  Index = QuestionId - QUESTION_ID_DRIVER_HEALTH_BASE;
+  ASSERT (Index < mDriverHealthManagerHealthInfoCount);
+  //
+  // Process the driver's healthy status for the specify module
+  //
+  DriverHealthManagerProcessSingleControllerHealth (
+    mDriverHealthManagerHealthInfo[Index].DriverHealth,
+    mDriverHealthManagerHealthInfo[Index].ControllerHandle,
+    mDriverHealthManagerHealthInfo[Index].ChildHandle,
+    mDriverHealthManagerHealthInfo[Index].HealthStatus,
+    &(mDriverHealthManagerHealthInfo[Index].MessageList),
+    mDriverHealthManagerHealthInfo[Index].HiiHandle
+    );
+
+  DriverHealthManagerUpdateForm ((BOOLEAN) (QuestionId == QUESTION_ID_REFRESH_CONFIGURE));
+
+  return EFI_SUCCESS;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.h b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.h
new file mode 100644
index 0000000000..179bc94470
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.h
@@ -0,0 +1,133 @@
+/** @file
+  This module produces two driver health manager forms.
+  One will be used by BDS core to configure the Configured Required
+  driver health instances, the other will be automatically included by
+  firmware setup (UI).
+
+Copyright (c) 2013 - 2015, 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.
+
+**/
+
+#ifndef _DRIVER_HEALTH_MANAGEMENT_DXE_H_
+#define _DRIVER_HEALTH_MANAGEMENT_DXE_H_
+
+#include <Uefi.h>
+#include <Base.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverHealth.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/HiiDatabase.h>
+#include <Guid/MdeModuleHii.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiBootManagerLib.h>
+#include <Library/HiiLib.h>
+#include <Library/PrintLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PcdLib.h>
+
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} FORM_DEVICE_PATH;
+
+/**
+  This function is invoked if user selected a interactive opcode from Driver Health's
+  Formset. The decision by user is saved to gCallbackKey for later processing.
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Action          Specifies the type of action taken by the browser.
+  @param QuestionId      A unique value which is sent to the original exporting driver
+                         so that it can identify the type of data to expect.
+  @param Type            The type of value for the question.
+  @param Value           A pointer to the data being sent to the original exporting driver.
+  @param ActionRequest   On return, points to the action requested by the callback function.
+
+  @retval  EFI_SUCCESS           The callback successfully handled the action.
+  @retval  EFI_INVALID_PARAMETER The setup browser call this function with invalid parameters.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  );
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Request         A null-terminated Unicode string in <ConfigRequest> format.
+  @param Progress        On return, points to a character in the Request string.
+                         Points to the string's null terminator if request was successful.
+                         Points to the most recent '&' before the first failing name/value
+                         pair (or the beginning of the string if the failure is in the
+                         first name/value pair) if the request was not successful.
+  @param Results         A null-terminated Unicode string in <ConfigAltResp> format which
+                         has all values filled in for the names in the Request string.
+                         String to be allocated by the called function.
+
+  @retval  EFI_SUCCESS            The Results is filled with the requested values.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval  EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerFakeExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  );
+
+/**
+  This function processes the results of changes in configuration.
+
+
+  @param This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param Configuration   A null-terminated Unicode string in <ConfigResp> format.
+  @param Progress        A pointer to a string filled in with the offset of the most
+                         recent '&' before the first failing name/value pair (or the
+                         beginning of the string if the failure is in the first
+                         name/value pair) or the terminating NULL if all was successful.
+
+  @retval  EFI_SUCCESS            The Results is processed successfully.
+  @retval  EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval  EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverHealthManagerFakeRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.inf b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.inf
new file mode 100644
index 0000000000..482981c143
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.inf
@@ -0,0 +1,80 @@
+## @file
+#  Driver Health Manager DXE driver.
+#
+#  This module produces two driver health manager forms.
+#  One will be used by BDS core to configure the Configured Required
+#  driver health instances, the other will be automatically included by
+#  firmware setup (UI).
+#
+#  Copyright (c) 2013 - 2015, 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.
+#  
+##
+################################################################################
+#
+# Defines Section - statements that will be processed to create a Makefile.
+#
+################################################################################
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DriverHealthManagerDxe
+  MODULE_UNI_FILE                = DriverHealthManagerDxe.uni
+  FILE_GUID                      = EBF8ED7C-0DD1-4787-84F1-F48D537DCACF
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeDriverHealthManager
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+
+[Sources.common]
+  DriverHealthManagerDxe.h
+  DriverHealthManagerDxe.c
+  DriverHealthManagerStrings.uni
+  DriverHealthManagerVfr.Vfr
+  DriverHealthManagerVfr.h
+  DriverHealthConfigureVfr.Vfr
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  HiiLib
+  UefiBootManagerLib
+  PcdLib
+  DevicePathLib
+
+[Protocols]
+  gEfiHiiConfigAccessProtocolGuid               ## PRODUCES
+
+[Guids]
+  gEfiHiiDriverHealthFormsetGuid                ## CONSUMES ## GUID
+  gEfiIfrTianoGuid                              ## CONSUMES ## HII
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdDriverHealthConfigureForm      ## CONSUMES
+
+[Depex]
+  gEfiHiiDatabaseProtocolGuid AND gEfiFormBrowser2ProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DriverHealthManagerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.uni b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.uni
new file mode 100644
index 0000000000..f88d76d64c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxe.uni
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxeExtra.uni b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxeExtra.uni
new file mode 100644
index 0000000000..772a75030c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerStrings.uni b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerStrings.uni
new file mode 100644
index 0000000000..e3e4311c7f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerStrings.uni
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.Vfr b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.Vfr
new file mode 100644
index 0000000000..cfc1cf7f7e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.Vfr
@@ -0,0 +1,38 @@
+///** @file
+//  
+//    VFR to produce the formset used by UI.
+//  
+//  Copyright (c) 2013 - 2015, 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 "DriverHealthManagerVfr.h"
+
+formset
+  guid      = DRIVER_HEALTH_MANAGER_FORMSET_GUID,
+  title     = STRING_TOKEN(STR_FORM_TITLE),
+  help      = STRING_TOKEN(STR_FORM_HELP),
+  classguid = EFI_HII_PLATFORM_SETUP_FORMSET_GUID,
+
+  form formid = DRIVER_HEALTH_FORM_ID,
+      title  = STRING_TOKEN(STR_FORM_TITLE);
+
+      label LABEL_BEGIN;
+      label LABEL_END;
+
+      suppressif TRUE;
+          text
+              help  = STRING_TOKEN(STR_NULL),
+              text  = STRING_TOKEN(STR_NULL),
+              flags = INTERACTIVE,
+              key   = QUESTION_ID_REFRESH_MANAGER;
+      endif;
+
+  endform;
+endformset;
diff --git a/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.h b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.h
new file mode 100644
index 0000000000..61ca04af11
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverHealthManagerDxe/DriverHealthManagerVfr.h
@@ -0,0 +1,32 @@
+/** @file
+  Definition shared by VFR file and C file.
+
+Copyright (c) 2013 - 2015, 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.
+
+**/
+
+#ifndef _DRIVER_HEALTH_VFR_H_
+#define _DRIVER_HEALTH_VFR_H_
+#include <Guid/HiiPlatformSetupFormset.h>
+
+#define DRIVER_HEALTH_MANAGER_FORMSET_GUID         { 0xcfb3b000, 0x0b63, 0x444b, { 0xb1, 0xd1, 0x12, 0xd5, 0xd9, 0x5d, 0xc4, 0xfc } } 
+#define DRIVER_HEALTH_CONFIGURE_FORMSET_GUID       { 0x4296d9f4, 0xf6fc, 0x4dde, { 0x86, 0x85, 0x8c, 0xe2, 0xd7, 0x9d, 0x90, 0xf0 } } 
+
+#define LABEL_BEGIN                        0x2000
+#define LABEL_END                          0x2001
+
+#define DRIVER_HEALTH_FORM_ID              0x1001
+
+#define QUESTION_ID_REFRESH_MANAGER        0x0001
+#define QUESTION_ID_REFRESH_CONFIGURE      0x0002
+
+#define QUESTION_ID_DRIVER_HEALTH_BASE     0x0003
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.c b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.c
new file mode 100644
index 0000000000..8ec1d4e6e8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.c
@@ -0,0 +1,2283 @@
+/** @file
+This is an example of how a driver might export data to the HII protocol to be
+later utilized by the Setup Protocol
+
+Copyright (c) 2004 - 2015, 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 "DriverSample.h"
+
+#define DISPLAY_ONLY_MY_ITEM  0x0002
+
+CHAR16     VariableName[] = L"MyIfrNVData";
+CHAR16     MyEfiVar[] = L"MyEfiVar";
+EFI_HANDLE                      DriverHandle[2] = {NULL, NULL};
+DRIVER_SAMPLE_PRIVATE_DATA      *mPrivateData = NULL;
+EFI_EVENT                       mEvent;
+
+HII_VENDOR_DEVICE_PATH  mHiiVendorDevicePath0 = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    DRIVER_SAMPLE_FORMSET_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+HII_VENDOR_DEVICE_PATH  mHiiVendorDevicePath1 = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    DRIVER_SAMPLE_INVENTORY_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    {
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  Add empty function for event process function.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+DriverSampleInternalEmptyFunction (
+  IN  EFI_EVENT Event,
+  IN  VOID      *Context
+  )
+{
+}
+
+/**
+  Notification function for keystrokes.
+
+  @param[in] KeyData    The key that was pressed.
+
+  @retval EFI_SUCCESS   The operation was successful.
+**/
+EFI_STATUS
+EFIAPI
+NotificationFunction(
+  IN EFI_KEY_DATA *KeyData
+  )
+{
+  gBS->SignalEvent (mEvent);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Function to start monitoring for CTRL-C using SimpleTextInputEx. 
+
+  @retval EFI_SUCCESS           The feature is enabled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough mnemory available.
+**/
+EFI_STATUS
+EFIAPI
+InternalStartMonitor(
+  VOID
+  )
+{
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *SimpleEx;
+  EFI_KEY_DATA                      KeyData;
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *Handles;
+  UINTN                             HandleCount;
+  UINTN                             HandleIndex;
+  EFI_HANDLE                        NotifyHandle;
+
+  Status = gBS->LocateHandleBuffer (
+              ByProtocol,
+              &gEfiSimpleTextInputExProtocolGuid,
+              NULL,
+              &HandleCount,
+              &Handles
+              );
+  for (HandleIndex = 0; HandleIndex < HandleCount; HandleIndex++) {
+    Status = gBS->HandleProtocol (Handles[HandleIndex], &gEfiSimpleTextInputExProtocolGuid, (VOID **) &SimpleEx);
+    ASSERT_EFI_ERROR (Status);
+
+    KeyData.KeyState.KeyToggleState = 0;
+    KeyData.Key.ScanCode            = 0;
+    KeyData.KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID|EFI_LEFT_CONTROL_PRESSED;
+    KeyData.Key.UnicodeChar         = L'c';
+
+    Status = SimpleEx->RegisterKeyNotify(
+      SimpleEx,
+      &KeyData,
+      NotificationFunction,
+      &NotifyHandle);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    
+    KeyData.KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID|EFI_RIGHT_CONTROL_PRESSED;
+    Status = SimpleEx->RegisterKeyNotify(
+      SimpleEx,
+      &KeyData,
+      NotificationFunction,
+      &NotifyHandle);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Function to stop monitoring for CTRL-C using SimpleTextInputEx.  
+
+  @retval EFI_SUCCESS           The feature is enabled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough mnemory available.
+**/
+EFI_STATUS
+EFIAPI
+InternalStopMonitor(
+  VOID
+  )
+{
+  EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL *SimpleEx;
+  EFI_STATUS                        Status;
+  EFI_HANDLE                        *Handles;
+  EFI_KEY_DATA                      KeyData;  
+  UINTN                             HandleCount;
+  UINTN                             HandleIndex;
+  EFI_HANDLE                        NotifyHandle;
+
+  Status = gBS->LocateHandleBuffer (
+                ByProtocol,
+                &gEfiSimpleTextInputExProtocolGuid,
+                NULL,
+                &HandleCount,
+                &Handles
+                );
+  for (HandleIndex = 0; HandleIndex < HandleCount; HandleIndex++) {
+    Status = gBS->HandleProtocol (Handles[HandleIndex], &gEfiSimpleTextInputExProtocolGuid, (VOID **) &SimpleEx);
+    ASSERT_EFI_ERROR (Status);
+
+    KeyData.KeyState.KeyToggleState = 0;
+    KeyData.Key.ScanCode            = 0;
+    KeyData.KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID|EFI_LEFT_CONTROL_PRESSED;
+    KeyData.Key.UnicodeChar         = L'c';
+
+    Status = SimpleEx->RegisterKeyNotify(
+      SimpleEx,
+      &KeyData,
+      NotificationFunction,
+      &NotifyHandle);
+    if (!EFI_ERROR (Status)) {
+      Status = SimpleEx->UnregisterKeyNotify (SimpleEx, NotifyHandle);
+    }
+
+    KeyData.KeyState.KeyShiftState  = EFI_SHIFT_STATE_VALID|EFI_RIGHT_CONTROL_PRESSED;
+    Status = SimpleEx->RegisterKeyNotify(
+      SimpleEx,
+      &KeyData,
+      NotificationFunction,
+      &NotifyHandle);
+    if (!EFI_ERROR (Status)) {
+      Status = SimpleEx->UnregisterKeyNotify (SimpleEx, NotifyHandle);
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Encode the password using a simple algorithm.
+
+  @param Password The string to be encoded.
+  @param MaxSize  The size of the string.
+
+**/
+VOID
+EncodePassword (
+  IN  CHAR16                      *Password,
+  IN  UINTN                       MaxSize
+  )
+{
+  UINTN   Index;
+  UINTN   Loop;
+  CHAR16  *Buffer;
+  CHAR16  *Key;
+
+  Key     = L"MAR10648567";
+  Buffer  = AllocateZeroPool (MaxSize);
+  ASSERT (Buffer != NULL);
+
+  for (Index = 0; Key[Index] != 0; Index++) {
+    for (Loop = 0; Loop < (UINT8) (MaxSize / 2); Loop++) {
+      Buffer[Loop] = (CHAR16) (Password[Loop] ^ Key[Index]);
+    }
+  }
+
+  CopyMem (Password, Buffer, MaxSize);
+
+  FreePool (Buffer);
+  return ;
+}
+
+/**
+  Validate the user's password.
+
+  @param PrivateData This driver's private context data.
+  @param StringId    The user's input.
+
+  @retval EFI_SUCCESS   The user's input matches the password.
+  @retval EFI_NOT_READY The user's input does not match the password.
+**/
+EFI_STATUS
+ValidatePassword (
+  IN       DRIVER_SAMPLE_PRIVATE_DATA      *PrivateData,
+  IN       EFI_STRING_ID                   StringId
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           Index;
+  UINTN                           BufferSize;
+  UINTN                           PasswordMaxSize;
+  CHAR16                          *Password;
+  CHAR16                          *EncodedPassword;
+  BOOLEAN                         OldPassword;
+
+  //
+  // Get encoded password first
+  //
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = gRT->GetVariable (
+                  VariableName,
+                  &gDriverSampleFormSetGuid,
+                  NULL,
+                  &BufferSize,
+                  &PrivateData->Configuration
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Old password not exist, prompt for new password
+    //
+    return EFI_SUCCESS;
+  }
+
+  OldPassword = FALSE;
+  PasswordMaxSize = sizeof (PrivateData->Configuration.WhatIsThePassword2);
+  //
+  // Check whether we have any old password set
+  //
+  for (Index = 0; Index < PasswordMaxSize / sizeof (UINT16); Index++) {
+    if (PrivateData->Configuration.WhatIsThePassword2[Index] != 0) {
+      OldPassword = TRUE;
+      break;
+    }
+  }
+  if (!OldPassword) {
+    //
+    // Old password not exist, return EFI_SUCCESS to prompt for new password
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get user input password
+  //
+  Password = HiiGetString (PrivateData->HiiHandle[0], StringId, NULL);
+  if (Password == NULL) {
+    return EFI_NOT_READY;
+  }
+  if (StrSize (Password) > PasswordMaxSize) {
+    FreePool (Password);
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Validate old password
+  //
+  EncodedPassword = AllocateZeroPool (PasswordMaxSize);
+  ASSERT (EncodedPassword != NULL);
+  StrnCpyS (EncodedPassword, PasswordMaxSize / sizeof (CHAR16), Password, StrLen (Password));
+  EncodePassword (EncodedPassword, StrLen (EncodedPassword) * sizeof (CHAR16));
+  if (CompareMem (EncodedPassword, PrivateData->Configuration.WhatIsThePassword2, PasswordMaxSize) != 0) {
+    //
+    // Old password mismatch, return EFI_NOT_READY to prompt for error message
+    //
+    Status = EFI_NOT_READY;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+  FreePool (Password);
+  FreePool (EncodedPassword);
+
+  return Status;
+}
+
+/**
+  Encode the password using a simple algorithm.
+
+  @param PrivateData This driver's private context data.
+  @param StringId    The password from User.
+
+  @retval  EFI_SUCESS The operation is successful.
+  @return  Other value if gRT->SetVariable () fails.
+
+**/
+EFI_STATUS
+SetPassword (
+  IN DRIVER_SAMPLE_PRIVATE_DATA      *PrivateData,
+  IN EFI_STRING_ID                   StringId
+  )
+{
+  EFI_STATUS                      Status;
+  CHAR16                          *Password;
+  CHAR16                          *TempPassword;
+  UINTN                           PasswordSize;
+  DRIVER_SAMPLE_CONFIGURATION     *Configuration;
+  UINTN                           BufferSize;
+
+  //
+  // Get Buffer Storage data from EFI variable
+  //
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = gRT->GetVariable (
+                  VariableName,
+                  &gDriverSampleFormSetGuid,
+                  NULL,
+                  &BufferSize,
+                  &PrivateData->Configuration
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get user input password
+  //
+  Password = PrivateData->Configuration.WhatIsThePassword2;
+  PasswordSize = sizeof (PrivateData->Configuration.WhatIsThePassword2);
+  ZeroMem (Password, PasswordSize);
+
+  TempPassword = HiiGetString (PrivateData->HiiHandle[0], StringId, NULL);
+  if (TempPassword == NULL) {
+    return EFI_NOT_READY;
+  }
+  if (StrSize (TempPassword) > PasswordSize) {
+    FreePool (TempPassword);
+    return EFI_NOT_READY;
+  }
+  StrnCpyS (Password, PasswordSize / sizeof (CHAR16), TempPassword, StrLen (TempPassword));
+  FreePool (TempPassword);
+
+  //
+  // Retrive uncommitted data from Browser
+  //
+  Configuration = AllocateZeroPool (sizeof (DRIVER_SAMPLE_CONFIGURATION));
+  ASSERT (Configuration != NULL);
+  if (HiiGetBrowserData (&gDriverSampleFormSetGuid, VariableName, sizeof (DRIVER_SAMPLE_CONFIGURATION), (UINT8 *) Configuration)) {
+    //
+    // Update password's clear text in the screen
+    //
+    CopyMem (Configuration->PasswordClearText, Password, StrSize (Password));
+
+    //
+    // Update uncommitted data of Browser
+    //
+    HiiSetBrowserData (
+       &gDriverSampleFormSetGuid,
+       VariableName,
+       sizeof (DRIVER_SAMPLE_CONFIGURATION),
+       (UINT8 *) Configuration,
+       NULL
+       );
+  }
+
+  //
+  // Free Configuration Buffer
+  //
+  FreePool (Configuration);
+
+
+  //
+  // Set password
+  //
+  EncodePassword (Password, StrLen (Password) * 2);
+  Status = gRT->SetVariable(
+                  VariableName,
+                  &gDriverSampleFormSetGuid,
+                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  sizeof (DRIVER_SAMPLE_CONFIGURATION),
+                  &PrivateData->Configuration
+                  );
+  return Status;
+}
+
+/**
+ Update names of Name/Value storage to current language.
+
+ @param PrivateData   Points to the driver private data.
+
+ @retval EFI_SUCCESS   All names are successfully updated.
+ @retval EFI_NOT_FOUND Failed to get Name from HII database.
+
+**/
+EFI_STATUS
+LoadNameValueNames (
+  IN DRIVER_SAMPLE_PRIVATE_DATA      *PrivateData
+  )
+{
+  UINTN      Index;
+
+  //
+  // Get Name/Value name string of current language
+  //
+  for (Index = 0; Index < NAME_VALUE_NAME_NUMBER; Index++) {
+    PrivateData->NameValueName[Index] = HiiGetString (
+                                         PrivateData->HiiHandle[0],
+                                         PrivateData->NameStringId[Index],
+                                         NULL
+                                         );
+    if (PrivateData->NameValueName[Index] == NULL) {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the value of <Number> in <BlockConfig> format, i.e. the value of OFFSET
+  or WIDTH or VALUE.
+  <BlockConfig> ::= 'OFFSET='<Number>&'WIDTH='<Number>&'VALUE'=<Number>
+
+  This is a internal function.
+
+  @param  StringPtr              String in <BlockConfig> format and points to the
+                                 first character of <Number>.
+  @param  Number                 The output value. Caller takes the responsibility
+                                 to free memory.
+  @param  Len                    Length of the <Number>, in characters.
+
+  @retval EFI_OUT_OF_RESOURCES   Insufficient resources to store neccessary
+                                 structures.
+  @retval EFI_SUCCESS            Value of <Number> is outputted in Number
+                                 successfully.
+
+**/
+EFI_STATUS
+GetValueOfNumber (
+  IN EFI_STRING                    StringPtr,
+  OUT UINT8                        **Number,
+  OUT UINTN                        *Len
+  )
+{
+  EFI_STRING               TmpPtr;
+  UINTN                    Length;
+  EFI_STRING               Str;
+  UINT8                    *Buf;
+  EFI_STATUS               Status;
+  UINT8                    DigitUint8;
+  UINTN                    Index;
+  CHAR16                   TemStr[2];
+
+  if (StringPtr == NULL || *StringPtr == L'\0' || Number == NULL || Len == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Buf = NULL;
+
+  TmpPtr = StringPtr;
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr++;
+  }
+  *Len   = StringPtr - TmpPtr;
+  Length = *Len + 1;
+
+  Str = (EFI_STRING) AllocateZeroPool (Length * sizeof (CHAR16));
+  if (Str == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  CopyMem (Str, TmpPtr, *Len * sizeof (CHAR16));
+  *(Str + *Len) = L'\0';
+
+  Length = (Length + 1) / 2;
+  Buf = (UINT8 *) AllocateZeroPool (Length);
+  if (Buf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  
+  Length = *Len;
+  ZeroMem (TemStr, sizeof (TemStr));
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = Str[Length - Index - 1];
+    DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+    if ((Index & 1) == 0) {
+      Buf [Index/2] = DigitUint8;
+    } else {
+      Buf [Index/2] = (UINT8) ((DigitUint8 << 4) + Buf [Index/2]);
+    }
+  }
+
+  *Number = Buf;
+  Status  = EFI_SUCCESS;
+
+Exit:
+  if (Str != NULL) {
+    FreePool (Str);
+  }
+
+  return Status;
+}
+
+/**
+  Create altcfg string. 
+
+  @param  Result               The request result string.
+  @param  ConfigHdr            The request head info. <ConfigHdr> format.
+  @param  Offset               The offset of the parameter int he structure.
+  @param  Width                The width of the parameter.
+
+
+  @retval  The string with altcfg info append at the end.
+**/
+EFI_STRING 
+CreateAltCfgString (
+  IN     EFI_STRING     Result,
+  IN     EFI_STRING     ConfigHdr,
+  IN     UINTN          Offset,
+  IN     UINTN          Width
+  )
+{
+  EFI_STRING StringPtr;
+  EFI_STRING TmpStr;
+  UINTN      NewLen;
+
+  NewLen = StrLen (Result);
+  //
+  // String Len = ConfigResp + AltConfig + AltConfig + 1("\0")
+  //
+  NewLen = (NewLen + ((1 + StrLen (ConfigHdr) + 8 + 4) + (8 + 4 + 7 + 4 + 7 + 4)) * 2 + 1) * sizeof (CHAR16);
+  StringPtr = AllocateZeroPool (NewLen);
+  if (StringPtr == NULL) {
+    return NULL;
+  }
+
+  TmpStr = StringPtr;
+  if (Result != NULL) {
+    StrCpyS (StringPtr, NewLen / sizeof (CHAR16), Result);
+    StringPtr += StrLen (Result);  
+    FreePool (Result);
+  }
+  
+  UnicodeSPrint (
+  StringPtr, 
+  (1 + StrLen (ConfigHdr) + 8 + 4 + 1) * sizeof (CHAR16), 
+  L"&%s&ALTCFG=%04x", 
+  ConfigHdr, 
+  EFI_HII_DEFAULT_CLASS_STANDARD
+  );
+  StringPtr += StrLen (StringPtr);
+
+  UnicodeSPrint (
+    StringPtr, 
+    (8 + 4 + 7 + 4 + 7 + 4 + 1) * sizeof (CHAR16),
+    L"&OFFSET=%04x&WIDTH=%04x&VALUE=%04x", 
+    Offset, 
+    Width,
+    DEFAULT_CLASS_STANDARD_VALUE
+    );
+  StringPtr += StrLen (StringPtr);  
+
+  UnicodeSPrint (
+  StringPtr, 
+  (1 + StrLen (ConfigHdr) + 8 + 4 + 1) * sizeof (CHAR16), 
+  L"&%s&ALTCFG=%04x", 
+  ConfigHdr, 
+  EFI_HII_DEFAULT_CLASS_MANUFACTURING
+  );
+  StringPtr += StrLen (StringPtr);
+
+  UnicodeSPrint (
+    StringPtr, 
+    (8 + 4 + 7 + 4 + 7 + 4 + 1) * sizeof (CHAR16),
+    L"&OFFSET=%04x&WIDTH=%04x&VALUE=%04x", 
+    Offset, 
+    Width,
+    DEFAULT_CLASS_MANUFACTURING_VALUE
+    );
+  StringPtr += StrLen (StringPtr); 
+
+  return TmpStr;
+}
+
+/**
+  Check whether need to add the altcfg string. if need to add, add the altcfg 
+  string.
+
+  @param  RequestResult              The request result string.
+  @param  ConfigRequestHdr           The request head info. <ConfigHdr> format.
+
+**/
+VOID 
+AppendAltCfgString (
+  IN OUT EFI_STRING                       *RequestResult,
+  IN     EFI_STRING                       ConfigRequestHdr
+  )
+{
+  EFI_STRING                          StringPtr;
+  UINTN                               Length;
+  UINT8                               *TmpBuffer;
+  UINTN                               Offset;
+  UINTN                               Width;
+  UINTN                               BlockSize;
+  UINTN                               ValueOffset;
+  UINTN                               ValueWidth;
+  EFI_STATUS                          Status;
+
+  TmpBuffer = NULL;
+  StringPtr = *RequestResult;
+  StringPtr = StrStr (StringPtr, L"OFFSET");
+  BlockSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  ValueOffset = OFFSET_OF (DRIVER_SAMPLE_CONFIGURATION, GetDefaultValueFromAccess);
+  ValueWidth  = sizeof (((DRIVER_SAMPLE_CONFIGURATION *)0)->GetDefaultValueFromAccess);
+
+  if (StringPtr == NULL) {
+    return;
+  }
+
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"OFFSET=", StrLen (L"OFFSET=")) == 0) {
+    StringPtr += StrLen (L"OFFSET=");
+    //
+    // Get Offset
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+    Offset = 0;
+    CopyMem (
+     &Offset,
+     TmpBuffer,
+     (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+     );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      return;
+    }
+    StringPtr += StrLen (L"&WIDTH=");
+
+    //
+    // Get Width
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+    Width = 0;
+    CopyMem (
+     &Width,
+     TmpBuffer,
+     (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+     );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&VALUE=", StrLen (L"&VALUE=")) != 0) {
+      return;
+    }
+    StringPtr += StrLen (L"&VALUE=");
+
+    //
+    // Get Value
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      return;
+    }
+    StringPtr += Length;
+
+    //
+    // Calculate Value and convert it to hex string.
+    //
+    if (Offset + Width > BlockSize) {
+      return;
+    }
+
+    if (Offset <= ValueOffset && Offset + Width >= ValueOffset + ValueWidth) {
+      *RequestResult = CreateAltCfgString(*RequestResult, ConfigRequestHdr, ValueOffset, ValueWidth);
+      return;
+    }
+  }
+}
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+  @param  This                   Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Request                A null-terminated Unicode string in
+                                 <ConfigRequest> format.
+  @param  Progress               On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 '&' before the first failing name/value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name/value pair) if the request was not
+                                 successful.
+  @param  Results                A null-terminated Unicode string in
+                                 <ConfigAltResp> format which has all values filled
+                                 in for the names in the Request string. String to
+                                 be allocated by the called function.
+
+  @retval EFI_SUCCESS            The Results is filled with the requested values.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+ExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS                       Status;
+  UINTN                            BufferSize;
+  DRIVER_SAMPLE_PRIVATE_DATA       *PrivateData;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *HiiConfigRouting;
+  EFI_STRING                       ConfigRequest;
+  EFI_STRING                       ConfigRequestHdr;
+  UINTN                            Size;
+  EFI_STRING                       Value;
+  UINTN                            ValueStrLen;
+  CHAR16                           BackupChar;
+  CHAR16                           *StrPointer;
+  BOOLEAN                          AllocatedRequest;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Initialize the local variables.
+  //
+  ConfigRequestHdr  = NULL;
+  ConfigRequest     = NULL;
+  Size              = 0;
+  *Progress         = Request;
+  AllocatedRequest  = FALSE;
+
+  PrivateData = DRIVER_SAMPLE_PRIVATE_FROM_THIS (This);
+  HiiConfigRouting = PrivateData->HiiConfigRouting;
+
+  //
+  // Get Buffer Storage data from EFI variable.
+  // Try to get the current setting from variable.
+  //
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = gRT->GetVariable (
+            VariableName,
+            &gDriverSampleFormSetGuid,
+            NULL,
+            &BufferSize,
+            &PrivateData->Configuration
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Request == NULL) {
+    //
+    // Request is set to NULL, construct full request string.
+    //
+
+    //
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&gDriverSampleFormSetGuid, VariableName, PrivateData->DriverHandle[0]);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+    ConfigRequestHdr = NULL;
+  } else {
+    //
+    // Check routing data in <ConfigHdr>.
+    // Note: if only one Storage is used, then this checking could be skipped.
+    //
+    if (!HiiIsConfigHdrMatch (Request, &gDriverSampleFormSetGuid, NULL)) {
+      return EFI_NOT_FOUND;
+    }
+    //
+    // Check whether request for EFI Varstore. EFI varstore get data
+    // through hii database, not support in this path.
+    //
+    if (HiiIsConfigHdrMatch(Request, &gDriverSampleFormSetGuid, MyEfiVar)) {
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Set Request to the unified request string.
+    //
+    ConfigRequest = Request;
+    //
+    // Check whether Request includes Request Element.
+    //
+    if (StrStr (Request, L"OFFSET") == NULL) {
+      //
+      // Check Request Element does exist in Reques String
+      //
+      StrPointer = StrStr (Request, L"PATH");
+      if (StrPointer == NULL) {
+        return EFI_INVALID_PARAMETER;
+      }
+      if (StrStr (StrPointer, L"&") == NULL) {
+        Size = (StrLen (Request) + 32 + 1) * sizeof (CHAR16);
+        ConfigRequest    = AllocateZeroPool (Size);
+        ASSERT (ConfigRequest != NULL);
+        AllocatedRequest = TRUE;
+        UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", Request, (UINT64)BufferSize);
+      }
+    }
+  }
+
+  //
+  // Check if requesting Name/Value storage
+  //
+  if (StrStr (ConfigRequest, L"OFFSET") == NULL) {
+    //
+    // Update Name/Value storage Names
+    //
+    Status = LoadNameValueNames (PrivateData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Allocate memory for <ConfigResp>, e.g. Name0=0x11, Name1=0x1234, Name2="ABCD"
+    // <Request>   ::=<ConfigHdr>&Name0&Name1&Name2
+    // <ConfigResp>::=<ConfigHdr>&Name0=11&Name1=1234&Name2=0041004200430044
+    //
+    BufferSize = (StrLen (ConfigRequest) +
+      1 + sizeof (PrivateData->Configuration.NameValueVar0) * 2 +
+      1 + sizeof (PrivateData->Configuration.NameValueVar1) * 2 +
+      1 + sizeof (PrivateData->Configuration.NameValueVar2) * 2 + 1) * sizeof (CHAR16);
+    *Results = AllocateZeroPool (BufferSize);
+    ASSERT (*Results != NULL);
+    StrCpyS (*Results, BufferSize / sizeof (CHAR16), ConfigRequest);
+    Value = *Results;
+
+    //
+    // Append value of NameValueVar0, type is UINT8
+    //
+    if ((Value = StrStr (*Results, PrivateData->NameValueName[0])) != NULL) {
+      Value += StrLen (PrivateData->NameValueName[0]);
+      ValueStrLen = ((sizeof (PrivateData->Configuration.NameValueVar0) * 2) + 1);
+      CopyMem (Value + ValueStrLen, Value, StrSize (Value));
+
+      BackupChar = Value[ValueStrLen];
+      *Value++   = L'=';
+      Value += UnicodeValueToString (
+                 Value, 
+                 PREFIX_ZERO | RADIX_HEX, 
+                 PrivateData->Configuration.NameValueVar0, 
+                 sizeof (PrivateData->Configuration.NameValueVar0) * 2
+                 );
+      *Value = BackupChar;
+    }
+
+    //
+    // Append value of NameValueVar1, type is UINT16
+    //
+    if ((Value = StrStr (*Results, PrivateData->NameValueName[1])) != NULL) {
+      Value += StrLen (PrivateData->NameValueName[1]);
+      ValueStrLen = ((sizeof (PrivateData->Configuration.NameValueVar1) * 2) + 1);
+      CopyMem (Value + ValueStrLen, Value, StrSize (Value));
+
+      BackupChar = Value[ValueStrLen];
+      *Value++   = L'=';
+      Value += UnicodeValueToString (
+                Value, 
+                PREFIX_ZERO | RADIX_HEX, 
+                PrivateData->Configuration.NameValueVar1, 
+                sizeof (PrivateData->Configuration.NameValueVar1) * 2
+                );
+      *Value = BackupChar;
+    }
+
+    //
+    // Append value of NameValueVar2, type is CHAR16 *
+    //
+    if ((Value = StrStr (*Results, PrivateData->NameValueName[2])) != NULL) {
+      Value += StrLen (PrivateData->NameValueName[2]);
+      ValueStrLen = StrLen (PrivateData->Configuration.NameValueVar2) * 4 + 1;
+      CopyMem (Value + ValueStrLen, Value, StrSize (Value));
+
+      *Value++ = L'=';
+      //
+      // Convert Unicode String to Config String, e.g. "ABCD" => "0041004200430044"
+      //
+      StrPointer = (CHAR16 *) PrivateData->Configuration.NameValueVar2;
+      for (; *StrPointer != L'\0'; StrPointer++) {
+        Value += UnicodeValueToString (Value, PREFIX_ZERO | RADIX_HEX, *StrPointer, 4);
+      }
+    }
+    
+    Status = EFI_SUCCESS;
+  } else {
+    //
+    // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+    //
+    Status = HiiConfigRouting->BlockToConfig (
+                                  HiiConfigRouting,
+                                  ConfigRequest,
+                                  (UINT8 *) &PrivateData->Configuration,
+                                  BufferSize,
+                                  Results,
+                                  Progress
+                                  );
+    if (!EFI_ERROR (Status)) {
+      ConfigRequestHdr = HiiConstructConfigHdr (&gDriverSampleFormSetGuid, VariableName, PrivateData->DriverHandle[0]);
+      AppendAltCfgString(Results, ConfigRequestHdr);
+    }
+  }
+
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+  }
+
+  if (ConfigRequestHdr != NULL) {
+    FreePool (ConfigRequestHdr);
+  }
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param  This                   Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Configuration          A null-terminated Unicode string in <ConfigResp>
+                                 format.
+  @param  Progress               A pointer to a string filled in with the offset of
+                                 the most recent '&' before the first failing
+                                 name/value pair (or the beginning of the string if
+                                 the failure is in the first name/value pair) or
+                                 the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The Results is processed successfully.
+  @retval EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+RouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  EFI_STATUS                       Status;
+  UINTN                            BufferSize;
+  DRIVER_SAMPLE_PRIVATE_DATA       *PrivateData;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *HiiConfigRouting;
+  CHAR16                           *Value;
+  CHAR16                           *StrPtr;
+  CHAR16                           TemStr[5];
+  UINT8                            *DataBuffer;
+  UINT8                            DigitUint8;
+  UINTN                            Index;
+  CHAR16                           *StrBuffer;
+
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PrivateData = DRIVER_SAMPLE_PRIVATE_FROM_THIS (This);
+  HiiConfigRouting = PrivateData->HiiConfigRouting;
+  *Progress = Configuration;
+
+  //
+  // Check routing data in <ConfigHdr>.
+  // Note: if only one Storage is used, then this checking could be skipped.
+  //
+  if (!HiiIsConfigHdrMatch (Configuration, &gDriverSampleFormSetGuid, NULL)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check whether request for EFI Varstore. EFI varstore get data
+  // through hii database, not support in this path.
+  //
+  if (HiiIsConfigHdrMatch(Configuration, &gDriverSampleFormSetGuid, MyEfiVar)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Buffer Storage data from EFI variable
+  //
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = gRT->GetVariable (
+            VariableName,
+            &gDriverSampleFormSetGuid,
+            NULL,
+            &BufferSize,
+            &PrivateData->Configuration
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Check if configuring Name/Value storage
+  //
+  if (StrStr (Configuration, L"OFFSET") == NULL) {
+    //
+    // Update Name/Value storage Names
+    //
+    Status = LoadNameValueNames (PrivateData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Convert value for NameValueVar0
+    //
+    if ((Value = StrStr (Configuration, PrivateData->NameValueName[0])) != NULL) {
+      //
+      // Skip "Name="
+      //
+      Value += StrLen (PrivateData->NameValueName[0]);
+      Value++;
+      //
+      // Get Value String
+      //
+      StrPtr = StrStr (Value, L"&");
+      if (StrPtr == NULL) {
+        StrPtr = Value + StrLen (Value);
+      }
+      //
+      // Convert Value to Buffer data
+      //
+      DataBuffer = (UINT8 *) &PrivateData->Configuration.NameValueVar0;
+      ZeroMem (TemStr, sizeof (TemStr));
+      for (Index = 0, StrPtr --; StrPtr >= Value; StrPtr --, Index ++) {
+        TemStr[0] = *StrPtr;
+        DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+        if ((Index & 1) == 0) {
+          DataBuffer [Index/2] = DigitUint8;
+        } else {
+          DataBuffer [Index/2] = (UINT8) ((UINT8) (DigitUint8 << 4) + DataBuffer [Index/2]);
+        }
+      }
+    }
+
+    //
+    // Convert value for NameValueVar1
+    //
+    if ((Value = StrStr (Configuration, PrivateData->NameValueName[1])) != NULL) {
+      //
+      // Skip "Name="
+      //
+      Value += StrLen (PrivateData->NameValueName[1]);
+      Value++;
+      //
+      // Get Value String
+      //
+      StrPtr = StrStr (Value, L"&");
+      if (StrPtr == NULL) {
+        StrPtr = Value + StrLen (Value);
+      }
+      //
+      // Convert Value to Buffer data
+      //
+      DataBuffer = (UINT8 *) &PrivateData->Configuration.NameValueVar1;
+      ZeroMem (TemStr, sizeof (TemStr));
+      for (Index = 0, StrPtr --; StrPtr >= Value; StrPtr --, Index ++) {
+        TemStr[0] = *StrPtr;
+        DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+        if ((Index & 1) == 0) {
+          DataBuffer [Index/2] = DigitUint8;
+        } else {
+          DataBuffer [Index/2] = (UINT8) ((UINT8) (DigitUint8 << 4) + DataBuffer [Index/2]);
+        }
+      }
+    }
+
+    //
+    // Convert value for NameValueVar2
+    //
+    if ((Value = StrStr (Configuration, PrivateData->NameValueName[2])) != NULL) {
+      //
+      // Skip "Name="
+      //
+      Value += StrLen (PrivateData->NameValueName[2]);
+      Value++;
+      //
+      // Get Value String
+      //
+      StrPtr = StrStr (Value, L"&");
+      if (StrPtr == NULL) {
+        StrPtr = Value + StrLen (Value);
+      }
+      //
+      // Convert Config String to Unicode String, e.g "0041004200430044" => "ABCD"
+      //
+      StrBuffer = (CHAR16 *) PrivateData->Configuration.NameValueVar2;
+      ZeroMem (TemStr, sizeof (TemStr));
+      while (Value < StrPtr) {
+        StrnCpyS (TemStr, sizeof (TemStr) / sizeof (CHAR16), Value, 4);
+        *(StrBuffer++) = (CHAR16) StrHexToUint64 (TemStr);
+        Value += 4;
+      }
+      *StrBuffer = L'\0';
+    }
+
+    //
+    // Store Buffer Storage back to EFI variable
+    //
+    Status = gRT->SetVariable(
+      VariableName,
+      &gDriverSampleFormSetGuid,
+      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+      sizeof (DRIVER_SAMPLE_CONFIGURATION),
+      &PrivateData->Configuration
+      );
+
+    return Status;
+  }
+
+  //
+  // Convert <ConfigResp> to buffer data by helper function ConfigToBlock()
+  //
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = HiiConfigRouting->ConfigToBlock (
+                               HiiConfigRouting,
+                               Configuration,
+                               (UINT8 *) &PrivateData->Configuration,
+                               &BufferSize,
+                               Progress
+                               );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Store Buffer Storage back to EFI variable
+  //
+  Status = gRT->SetVariable(
+                  VariableName,
+                  &gDriverSampleFormSetGuid,
+                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  sizeof (DRIVER_SAMPLE_CONFIGURATION),
+                  &PrivateData->Configuration
+                  );
+
+  return Status;
+}
+
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param  This                   Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Action                 Specifies the type of action taken by the browser.
+  @param  QuestionId             A unique value which is sent to the original
+                                 exporting driver so that it can identify the type
+                                 of data to expect.
+  @param  Type                   The type of value for the question.
+  @param  Value                  A pointer to the data being sent to the original
+                                 exporting driver.
+  @param  ActionRequest          On return, points to the action requested by the
+                                 callback function.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the
+                                 variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved.
+  @retval EFI_UNSUPPORTED        The specified Action is not supported by the
+                                 callback.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  DRIVER_SAMPLE_PRIVATE_DATA      *PrivateData;
+  EFI_STATUS                      Status;
+  VOID                            *StartOpCodeHandle;
+  VOID                            *OptionsOpCodeHandle;
+  EFI_IFR_GUID_LABEL              *StartLabel;
+  VOID                            *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL              *EndLabel;
+  EFI_INPUT_KEY                   Key;
+  DRIVER_SAMPLE_CONFIGURATION     *Configuration;
+  MY_EFI_VARSTORE_DATA            *EfiData;
+  EFI_FORM_ID                     FormId;
+  EFI_STRING                      Progress;
+  EFI_STRING                      Results;
+  UINT32                          ProgressErr;
+  CHAR16                          *TmpStr;
+  
+  if (((Value == NULL) && (Action != EFI_BROWSER_ACTION_FORM_OPEN) && (Action != EFI_BROWSER_ACTION_FORM_CLOSE))||
+    (ActionRequest == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+
+  FormId = 0;
+  ProgressErr = 0;
+  Status = EFI_SUCCESS;
+  PrivateData = DRIVER_SAMPLE_PRIVATE_FROM_THIS (This);
+
+  switch (Action) {
+  case EFI_BROWSER_ACTION_FORM_OPEN:
+    {
+      if (QuestionId == 0x1234) {
+        //
+        // Sample CallBack for UEFI FORM_OPEN action:
+        //   Add Save action into Form 3 when Form 1 is opened.
+        //   This will be done only in FORM_OPEN CallBack of question with ID 0x1234 from Form 1.
+        //
+        PrivateData = DRIVER_SAMPLE_PRIVATE_FROM_THIS (This);
+
+        //
+        // Initialize the container for dynamic opcodes
+        //
+        StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+        ASSERT (StartOpCodeHandle != NULL);
+
+        //
+        // Create Hii Extend Label OpCode as the start opcode
+        //
+        StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+        StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+        StartLabel->Number       = LABEL_UPDATE2;
+
+        HiiCreateActionOpCode (
+          StartOpCodeHandle,                // Container for dynamic created opcodes
+          0x1238,                           // Question ID
+          STRING_TOKEN(STR_SAVE_TEXT),      // Prompt text
+          STRING_TOKEN(STR_SAVE_TEXT),      // Help text
+          EFI_IFR_FLAG_CALLBACK,            // Question flag
+          0                                 // Action String ID
+        );
+
+        HiiUpdateForm (
+          PrivateData->HiiHandle[0],  // HII handle
+          &gDriverSampleFormSetGuid,  // Formset GUID
+          0x3,                        // Form ID
+          StartOpCodeHandle,          // Label for where to insert opcodes
+          NULL                        // Insert data
+          );
+
+        HiiFreeOpCodeHandle (StartOpCodeHandle);
+      }
+
+      if (QuestionId == 0x1247) {
+        Status = InternalStartMonitor ();
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+    break;
+
+  case EFI_BROWSER_ACTION_FORM_CLOSE:
+    {
+      if (QuestionId == 0x5678) {
+        //
+        // Sample CallBack for UEFI FORM_CLOSE action:
+        //   Show up a pop-up to specify Form 3 will be closed when exit Form 3.
+        //
+        do {
+          CreatePopUp (
+            EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE,
+            &Key,
+            L"",
+            L"You are going to leave third Form!",
+            L"Press ESC or ENTER to continue ...",
+            L"",
+            NULL
+            );
+        } while ((Key.ScanCode != SCAN_ESC) && (Key.UnicodeChar != CHAR_CARRIAGE_RETURN));
+      }
+
+      if (QuestionId == 0x1247) {
+        Status = InternalStopMonitor ();
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+    break;
+    
+  case EFI_BROWSER_ACTION_RETRIEVE:
+    {
+      switch (QuestionId ) {
+      case 0x1248:
+        if (Type != EFI_IFR_TYPE_REF) {
+          return EFI_INVALID_PARAMETER;
+        }
+        Value->ref.FormId = 0x3;
+        break;
+
+      case 0x5678:
+      case 0x1247:
+        //
+        // We will reach here once the Question is refreshed
+        //
+
+        //
+        // Initialize the container for dynamic opcodes
+        //
+        StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+        ASSERT (StartOpCodeHandle != NULL);
+
+        //
+        // Create Hii Extend Label OpCode as the start opcode
+        //
+        StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+        StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+        if (QuestionId == 0x5678) {
+          StartLabel->Number       = LABEL_UPDATE2;
+          FormId                   = 0x03;
+          PrivateData->Configuration.DynamicRefresh++;
+        } else if (QuestionId == 0x1247 ) {
+          StartLabel->Number       = LABEL_UPDATE3;
+          FormId                   = 0x06;
+          PrivateData->Configuration.RefreshGuidCount++;
+        }
+
+        HiiCreateActionOpCode (
+          StartOpCodeHandle,                // Container for dynamic created opcodes
+          0x1237,                           // Question ID
+          STRING_TOKEN(STR_EXIT_TEXT),      // Prompt text
+          STRING_TOKEN(STR_EXIT_TEXT),      // Help text
+          EFI_IFR_FLAG_CALLBACK,            // Question flag
+          0                                 // Action String ID
+        );
+      
+        HiiUpdateForm (
+          PrivateData->HiiHandle[0],        // HII handle
+          &gDriverSampleFormSetGuid,        // Formset GUID
+          FormId,                           // Form ID
+          StartOpCodeHandle,                // Label for where to insert opcodes
+          NULL                              // Insert data
+        );
+
+        HiiFreeOpCodeHandle (StartOpCodeHandle);
+
+        //
+        // Refresh the Question value
+        //
+        Status = gRT->SetVariable(
+                        VariableName,
+                        &gDriverSampleFormSetGuid,
+                        EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                        sizeof (DRIVER_SAMPLE_CONFIGURATION),
+                        &PrivateData->Configuration
+                        );
+
+        if (QuestionId == 0x5678) {
+          //
+          // Update uncommitted data of Browser
+          //
+          EfiData = AllocateZeroPool (sizeof (MY_EFI_VARSTORE_DATA));
+          ASSERT (EfiData != NULL);
+          if (HiiGetBrowserData (&gDriverSampleFormSetGuid, MyEfiVar, sizeof (MY_EFI_VARSTORE_DATA), (UINT8 *) EfiData)) {
+            EfiData->Field8 = 111;
+            HiiSetBrowserData (
+              &gDriverSampleFormSetGuid,
+              MyEfiVar,
+              sizeof (MY_EFI_VARSTORE_DATA),
+              (UINT8 *) EfiData,
+              NULL
+            );
+          }
+          FreePool (EfiData);
+        }
+        break;
+      }
+    }
+    break;
+
+  case EFI_BROWSER_ACTION_DEFAULT_STANDARD:
+    {
+      switch (QuestionId) {
+      case 0x1240:
+        Value->u8 = DEFAULT_CLASS_STANDARD_VALUE;
+      break;
+
+      default:
+        Status = EFI_UNSUPPORTED;
+      break;
+      }
+    }
+    break;
+
+  case EFI_BROWSER_ACTION_DEFAULT_MANUFACTURING:
+    {
+      switch (QuestionId) {
+      case 0x1240:
+        Value->u8 = DEFAULT_CLASS_MANUFACTURING_VALUE;
+      break;
+
+      default:
+        Status = EFI_UNSUPPORTED;      
+      break;
+      }
+    }
+    break;
+
+  case EFI_BROWSER_ACTION_CHANGING:
+  {
+    switch (QuestionId) {
+    case 0x1249:
+      {
+        if (Type != EFI_IFR_TYPE_REF) {
+          return EFI_INVALID_PARAMETER;
+        }
+
+        Value->ref.FormId = 0x1234;
+      }
+    break;
+    case 0x1234:
+      //
+      // Initialize the container for dynamic opcodes
+      //
+      StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+      ASSERT (StartOpCodeHandle != NULL);
+
+      EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+      ASSERT (EndOpCodeHandle != NULL);
+
+      //
+      // Create Hii Extend Label OpCode as the start opcode
+      //
+      StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+      StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+      StartLabel->Number       = LABEL_UPDATE1;
+
+      //
+      // Create Hii Extend Label OpCode as the end opcode
+      //
+      EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+      EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+      EndLabel->Number       = LABEL_END;
+
+      HiiCreateActionOpCode (
+        StartOpCodeHandle,                // Container for dynamic created opcodes
+        0x1237,                           // Question ID
+        STRING_TOKEN(STR_EXIT_TEXT),      // Prompt text
+        STRING_TOKEN(STR_EXIT_TEXT),      // Help text
+        EFI_IFR_FLAG_CALLBACK,            // Question flag
+        0                                 // Action String ID
+      );
+
+      //
+      // Create Option OpCode
+      //
+      OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+      ASSERT (OptionsOpCodeHandle != NULL);
+
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        STRING_TOKEN (STR_BOOT_OPTION1),
+        0,
+        EFI_IFR_NUMERIC_SIZE_1,
+        1
+        );
+
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        STRING_TOKEN (STR_BOOT_OPTION2),
+        0,
+        EFI_IFR_NUMERIC_SIZE_1,
+        2
+        );
+
+      //
+      // Prepare initial value for the dynamic created oneof Question
+      //
+      PrivateData->Configuration.DynamicOneof = 2;
+      Status = gRT->SetVariable(
+                      VariableName,
+                      &gDriverSampleFormSetGuid,
+                      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                      sizeof (DRIVER_SAMPLE_CONFIGURATION),
+                      &PrivateData->Configuration
+                      );
+
+      //
+      // Set initial vlaue of dynamic created oneof Question in Form Browser
+      //
+      Configuration = AllocateZeroPool (sizeof (DRIVER_SAMPLE_CONFIGURATION));
+      ASSERT (Configuration != NULL);
+      if (HiiGetBrowserData (&gDriverSampleFormSetGuid, VariableName, sizeof (DRIVER_SAMPLE_CONFIGURATION), (UINT8 *) Configuration)) {
+        Configuration->DynamicOneof = 2;
+
+        //
+        // Update uncommitted data of Browser
+        //
+        HiiSetBrowserData (
+          &gDriverSampleFormSetGuid,
+          VariableName,
+          sizeof (DRIVER_SAMPLE_CONFIGURATION),
+          (UINT8 *) Configuration,
+          NULL
+          );
+      }
+      FreePool (Configuration);
+
+      HiiCreateOneOfOpCode (
+        StartOpCodeHandle,                         // Container for dynamic created opcodes
+        0x8001,                                    // Question ID (or call it "key")
+        CONFIGURATION_VARSTORE_ID,                 // VarStore ID
+        (UINT16) DYNAMIC_ONE_OF_VAR_OFFSET,        // Offset in Buffer Storage
+        STRING_TOKEN (STR_ONE_OF_PROMPT),          // Question prompt text
+        STRING_TOKEN (STR_ONE_OF_HELP),            // Question help text
+        EFI_IFR_FLAG_CALLBACK,                     // Question flag
+        EFI_IFR_NUMERIC_SIZE_1,                    // Data type of Question Value
+        OptionsOpCodeHandle,                       // Option Opcode list
+        NULL                                       // Default Opcode is NULl
+        );
+
+      HiiCreateOrderedListOpCode (
+        StartOpCodeHandle,                         // Container for dynamic created opcodes
+        0x8002,                                    // Question ID
+        CONFIGURATION_VARSTORE_ID,                 // VarStore ID
+        (UINT16) DYNAMIC_ORDERED_LIST_VAR_OFFSET,  // Offset in Buffer Storage
+        STRING_TOKEN (STR_BOOT_OPTIONS),           // Question prompt text
+        STRING_TOKEN (STR_BOOT_OPTIONS),           // Question help text
+        EFI_IFR_FLAG_RESET_REQUIRED,               // Question flag
+        0,                                         // Ordered list flag, e.g. EFI_IFR_UNIQUE_SET
+        EFI_IFR_NUMERIC_SIZE_1,                    // Data type of Question value
+        5,                                         // Maximum container
+        OptionsOpCodeHandle,                       // Option Opcode list
+        NULL                                       // Default Opcode is NULl
+        );
+
+      HiiCreateTextOpCode (
+        StartOpCodeHandle,
+        STRING_TOKEN(STR_TEXT_SAMPLE_HELP),
+        STRING_TOKEN(STR_TEXT_SAMPLE_HELP),
+        STRING_TOKEN(STR_TEXT_SAMPLE_STRING)
+      );
+
+      HiiCreateDateOpCode (
+        StartOpCodeHandle,
+        0x8004,
+        0x0,
+        0x0,
+        STRING_TOKEN(STR_DATE_SAMPLE_HELP),
+        STRING_TOKEN(STR_DATE_SAMPLE_HELP),
+        0,
+        QF_DATE_STORAGE_TIME,
+        NULL
+        );
+
+      HiiCreateTimeOpCode (
+        StartOpCodeHandle,
+        0x8005,
+        0x0,
+        0x0,
+        STRING_TOKEN(STR_TIME_SAMPLE_HELP),
+        STRING_TOKEN(STR_TIME_SAMPLE_HELP),
+        0,
+        QF_TIME_STORAGE_TIME,
+        NULL
+        );
+
+      HiiCreateGotoOpCode (
+        StartOpCodeHandle,                // Container for dynamic created opcodes
+        1,                                // Target Form ID
+        STRING_TOKEN (STR_GOTO_FORM1),    // Prompt text
+        STRING_TOKEN (STR_GOTO_HELP),     // Help text
+        0,                                // Question flag
+        0x8003                            // Question ID
+        );
+
+      HiiUpdateForm (
+        PrivateData->HiiHandle[0],  // HII handle
+        &gDriverSampleFormSetGuid,  // Formset GUID
+        0x1234,                     // Form ID
+        StartOpCodeHandle,          // Label for where to insert opcodes
+        EndOpCodeHandle             // Replace data
+        );
+
+      HiiFreeOpCodeHandle (StartOpCodeHandle);
+      HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+      HiiFreeOpCodeHandle (EndOpCodeHandle);
+      break;
+
+    case 0x2000:
+      //
+      // Only used to update the state.
+      //
+      if ((Type == EFI_IFR_TYPE_STRING) && (Value->string == 0) && 
+        (PrivateData->PasswordState == BROWSER_STATE_SET_PASSWORD)) {
+        PrivateData->PasswordState = BROWSER_STATE_VALIDATE_PASSWORD;
+        return EFI_INVALID_PARAMETER;
+      }
+
+      //
+      // When try to set a new password, user will be chanlleged with old password.
+      // The Callback is responsible for validating old password input by user,
+      // If Callback return EFI_SUCCESS, it indicates validation pass.
+      //
+      switch (PrivateData->PasswordState) {
+      case BROWSER_STATE_VALIDATE_PASSWORD:
+        Status = ValidatePassword (PrivateData, Value->string);
+        if (Status == EFI_SUCCESS) {
+          PrivateData->PasswordState = BROWSER_STATE_SET_PASSWORD;
+        }
+        break;
+
+      case BROWSER_STATE_SET_PASSWORD:
+        Status = SetPassword (PrivateData, Value->string);
+        PrivateData->PasswordState = BROWSER_STATE_VALIDATE_PASSWORD;
+        break;
+
+      default:
+        break;
+      }
+
+      break;
+
+    default:
+      break;
+    }
+  }
+  break;
+
+  case EFI_BROWSER_ACTION_CHANGED:
+    switch (QuestionId) {
+      case 0x1237:
+        //
+        // User press "Exit now", request Browser to exit
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_EXIT;
+        break;
+      
+      case 0x1238:
+        //
+        // User press "Save now", request Browser to save the uncommitted data.
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_SUBMIT;
+        break;
+      
+      case 0x1241:
+      case 0x1246:
+        //
+        // User press "Submit current form and Exit now", request Browser to submit current form and exit
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_SUBMIT_EXIT;
+        break;
+      
+      case 0x1242:
+        //
+        // User press "Discard current form now", request Browser to discard the uncommitted data.
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD;
+        break;
+      
+      case 0x1243:
+        //
+        // User press "Submit current form now", request Browser to save the uncommitted data.
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_APPLY;
+        break;
+      
+      case 0x1244:
+      case 0x1245:
+        //
+        // User press "Discard current form and Exit now", request Browser to discard the uncommitted data and exit.
+        //
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD_EXIT;
+        break;
+
+      case 0x1231:
+        //
+        // 1. Check to see whether system support keyword.
+        //
+        Status = PrivateData->HiiKeywordHandler->GetData (PrivateData->HiiKeywordHandler,
+                                                          L"NAMESPACE=x-UEFI-ns",
+                                                          L"KEYWORD=iSCSIBootEnable",
+                                                          &Progress,
+                                                          &ProgressErr,
+                                                          &Results
+                                                         );
+        if (EFI_ERROR (Status)) {
+          do {
+            CreatePopUp (
+              EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE,
+              &Key,
+              L"",
+              L"This system not support this keyword!",
+              L"Press ENTER to continue ...",
+              L"",
+              NULL
+              );
+          } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+          Status = EFI_SUCCESS;
+          break;
+        }
+
+        //
+        // 2. If system support this keyword, just try to change value.
+        //
+        
+        //
+        // Change value from '0' to '1' or from '1' to '0'
+        //
+        TmpStr = StrStr (Results, L"&VALUE=");
+        ASSERT (TmpStr != NULL);
+        TmpStr += StrLen (L"&VALUE=");
+        TmpStr++;
+        if (*TmpStr == L'0') {
+          *TmpStr = L'1';
+        } else {
+          *TmpStr = L'0';
+        }
+
+        //
+        // 3. Call the keyword handler protocol to change the value.
+        //
+        Status = PrivateData->HiiKeywordHandler->SetData (PrivateData->HiiKeywordHandler,
+                                                          Results,
+                                                          &Progress,
+                                                          &ProgressErr
+                                                         );
+        if (EFI_ERROR (Status)) {
+          do {
+            CreatePopUp (
+              EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE,
+              &Key,
+              L"",
+              L"Set keyword to the system failed!",
+              L"Press ENTER to continue ...",
+              L"",
+              NULL
+              );
+          } while (Key.UnicodeChar != CHAR_CARRIAGE_RETURN);
+
+          Status = EFI_SUCCESS;
+          break;
+        }
+        break;
+
+      default:
+      break;
+    }
+  break;
+
+  default:
+    Status = EFI_UNSUPPORTED;
+    break;
+  }
+
+  return Status;
+}
+
+/**
+  Main entry for this driver.
+
+  @param ImageHandle     Image handle this driver.
+  @param SystemTable     Pointer to SystemTable.
+
+  @retval EFI_SUCESS     This function always complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DriverSampleInit (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_HII_HANDLE                  HiiHandle[2];
+  EFI_SCREEN_DESCRIPTOR           Screen;
+  EFI_HII_DATABASE_PROTOCOL       *HiiDatabase;
+  EFI_HII_STRING_PROTOCOL         *HiiString;
+  EFI_FORM_BROWSER2_PROTOCOL      *FormBrowser2;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL *HiiConfigRouting;
+  EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL *HiiKeywordHandler;
+  CHAR16                          *NewString;
+  UINTN                           BufferSize;
+  DRIVER_SAMPLE_CONFIGURATION     *Configuration;
+  BOOLEAN                         ActionFlag;
+  EFI_STRING                      ConfigRequestHdr;
+  EFI_STRING                      NameRequestHdr;
+  MY_EFI_VARSTORE_DATA            *VarStoreConfig;
+  EFI_INPUT_KEY                   HotKey;
+  EFI_FORM_BROWSER_EXTENSION_PROTOCOL *FormBrowserEx;
+
+  //
+  // Initialize the local variables.
+  //
+  ConfigRequestHdr = NULL;
+  NewString        = NULL;
+
+  //
+  // Initialize screen dimensions for SendForm().
+  // Remove 3 characters from top and bottom
+  //
+  ZeroMem (&Screen, sizeof (EFI_SCREEN_DESCRIPTOR));
+  gST->ConOut->QueryMode (gST->ConOut, gST->ConOut->Mode->Mode, &Screen.RightColumn, &Screen.BottomRow);
+
+  Screen.TopRow     = 3;
+  Screen.BottomRow  = Screen.BottomRow - 3;
+
+  //
+  // Initialize driver private data
+  //
+  mPrivateData = AllocateZeroPool (sizeof (DRIVER_SAMPLE_PRIVATE_DATA));
+  if (mPrivateData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mPrivateData->Signature = DRIVER_SAMPLE_PRIVATE_SIGNATURE;
+
+  mPrivateData->ConfigAccess.ExtractConfig = ExtractConfig;
+  mPrivateData->ConfigAccess.RouteConfig = RouteConfig;
+  mPrivateData->ConfigAccess.Callback = DriverCallback;
+  mPrivateData->PasswordState = BROWSER_STATE_VALIDATE_PASSWORD;
+
+  //
+  // Locate Hii Database protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiDatabaseProtocolGuid, NULL, (VOID **) &HiiDatabase);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  mPrivateData->HiiDatabase = HiiDatabase;
+
+  //
+  // Locate HiiString protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiStringProtocolGuid, NULL, (VOID **) &HiiString);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  mPrivateData->HiiString = HiiString;
+
+  //
+  // Locate Formbrowser2 protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiFormBrowser2ProtocolGuid, NULL, (VOID **) &FormBrowser2);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  mPrivateData->FormBrowser2 = FormBrowser2;
+
+  //
+  // Locate ConfigRouting protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiHiiConfigRoutingProtocolGuid, NULL, (VOID **) &HiiConfigRouting);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  mPrivateData->HiiConfigRouting = HiiConfigRouting;
+
+  //
+  // Locate keyword handler protocol
+  //
+  Status = gBS->LocateProtocol (&gEfiConfigKeywordHandlerProtocolGuid, NULL, (VOID **) &HiiKeywordHandler);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  mPrivateData->HiiKeywordHandler = HiiKeywordHandler;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &DriverHandle[0],
+                  &gEfiDevicePathProtocolGuid,
+                  &mHiiVendorDevicePath0,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &mPrivateData->ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mPrivateData->DriverHandle[0] = DriverHandle[0];
+
+  //
+  // Publish our HII data
+  //
+  HiiHandle[0] = HiiAddPackages (
+                   &gDriverSampleFormSetGuid,
+                   DriverHandle[0],
+                   DriverSampleStrings,
+                   VfrBin,
+                   NULL
+                   );
+  if (HiiHandle[0] == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mPrivateData->HiiHandle[0] = HiiHandle[0];
+
+  //
+  // Publish another Fromset
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &DriverHandle[1],
+                  &gEfiDevicePathProtocolGuid,
+                  &mHiiVendorDevicePath1,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &mPrivateData->ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mPrivateData->DriverHandle[1] = DriverHandle[1];
+
+  HiiHandle[1] = HiiAddPackages (
+                   &gDriverSampleInventoryGuid,
+                   DriverHandle[1],
+                   DriverSampleStrings,
+                   InventoryBin,
+                   NULL
+                   );
+  if (HiiHandle[1] == NULL) {
+    DriverSampleUnload (ImageHandle);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mPrivateData->HiiHandle[1] = HiiHandle[1];
+
+  //
+  // Update the device path string.
+  //
+  NewString = ConvertDevicePathToText((EFI_DEVICE_PATH_PROTOCOL*)&mHiiVendorDevicePath0, FALSE, FALSE);
+  if (HiiSetString (HiiHandle[0], STRING_TOKEN (STR_DEVICE_PATH), NewString, NULL) == 0) {
+    DriverSampleUnload (ImageHandle);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  if (NewString != NULL) {
+    FreePool (NewString);
+  }
+
+  //
+  // Very simple example of how one would update a string that is already
+  // in the HII database
+  //
+  NewString = L"700 Mhz";
+
+  if (HiiSetString (HiiHandle[0], STRING_TOKEN (STR_CPU_STRING2), NewString, NULL) == 0) {
+    DriverSampleUnload (ImageHandle);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  HiiSetString (HiiHandle[0], 0, NewString, NULL);
+
+  //
+  // Initialize Name/Value name String ID
+  //
+  mPrivateData->NameStringId[0] = STR_NAME_VALUE_VAR_NAME0;
+  mPrivateData->NameStringId[1] = STR_NAME_VALUE_VAR_NAME1;
+  mPrivateData->NameStringId[2] = STR_NAME_VALUE_VAR_NAME2;
+
+  //
+  // Initialize configuration data
+  //
+  Configuration = &mPrivateData->Configuration;
+  ZeroMem (Configuration, sizeof (DRIVER_SAMPLE_CONFIGURATION));
+
+  //
+  // Try to read NV config EFI variable first
+  //
+  ConfigRequestHdr = HiiConstructConfigHdr (&gDriverSampleFormSetGuid, VariableName, DriverHandle[0]);
+  ASSERT (ConfigRequestHdr != NULL);
+
+  NameRequestHdr = HiiConstructConfigHdr (&gDriverSampleFormSetGuid, NULL, DriverHandle[0]);
+  ASSERT (NameRequestHdr != NULL);
+
+  BufferSize = sizeof (DRIVER_SAMPLE_CONFIGURATION);
+  Status = gRT->GetVariable (VariableName, &gDriverSampleFormSetGuid, NULL, &BufferSize, Configuration);
+  if (EFI_ERROR (Status)) {
+    //
+    // Store zero data Buffer Storage to EFI variable
+    //
+    Status = gRT->SetVariable(
+                    VariableName,
+                    &gDriverSampleFormSetGuid,
+                    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                    sizeof (DRIVER_SAMPLE_CONFIGURATION),
+                    Configuration
+                    );
+    if (EFI_ERROR (Status)) {
+      DriverSampleUnload (ImageHandle);
+      return Status;
+    }
+    //
+    // EFI variable for NV config doesn't exit, we should build this variable
+    // based on default values stored in IFR
+    //
+    ActionFlag = HiiSetToDefaults (NameRequestHdr, EFI_HII_DEFAULT_CLASS_STANDARD);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+
+    ActionFlag = HiiSetToDefaults (ConfigRequestHdr, EFI_HII_DEFAULT_CLASS_STANDARD);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    // EFI variable does exist and Validate Current Setting
+    //
+    ActionFlag = HiiValidateSettings (NameRequestHdr);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+
+    ActionFlag = HiiValidateSettings (ConfigRequestHdr);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  FreePool (ConfigRequestHdr);
+
+  //
+  // Initialize efi varstore configuration data
+  //
+  VarStoreConfig = &mPrivateData->VarStoreConfig;
+  ZeroMem (VarStoreConfig, sizeof (MY_EFI_VARSTORE_DATA));
+
+  ConfigRequestHdr = HiiConstructConfigHdr (&gDriverSampleFormSetGuid, MyEfiVar, DriverHandle[0]);
+  ASSERT (ConfigRequestHdr != NULL);
+
+  BufferSize = sizeof (MY_EFI_VARSTORE_DATA);
+  Status = gRT->GetVariable (MyEfiVar, &gDriverSampleFormSetGuid, NULL, &BufferSize, VarStoreConfig);
+  if (EFI_ERROR (Status)) {
+    //
+    // Store zero data to EFI variable Storage.
+    //
+    Status = gRT->SetVariable(
+                    MyEfiVar,
+                    &gDriverSampleFormSetGuid,
+                    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                    sizeof (MY_EFI_VARSTORE_DATA),
+                    VarStoreConfig
+                    );
+    if (EFI_ERROR (Status)) {
+      DriverSampleUnload (ImageHandle);
+      return Status;
+    }
+    //
+    // EFI variable for NV config doesn't exit, we should build this variable
+    // based on default values stored in IFR
+    //
+    ActionFlag = HiiSetToDefaults (ConfigRequestHdr, EFI_HII_DEFAULT_CLASS_STANDARD);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    // EFI variable does exist and Validate Current Setting
+    //
+    ActionFlag = HiiValidateSettings (ConfigRequestHdr);
+    if (!ActionFlag) {
+      DriverSampleUnload (ImageHandle);
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  FreePool (ConfigRequestHdr);
+
+  Status = gBS->CreateEventEx (
+        EVT_NOTIFY_SIGNAL, 
+        TPL_NOTIFY,
+        DriverSampleInternalEmptyFunction,
+        NULL,
+        &gEfiIfrRefreshIdOpGuid,
+        &mEvent
+        );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Example of how to use BrowserEx protocol to register HotKey.
+  // 
+  Status = gBS->LocateProtocol (&gEfiFormBrowserExProtocolGuid, NULL, (VOID **) &FormBrowserEx);
+  if (!EFI_ERROR (Status)) {
+    //
+    // First unregister the default hot key F9 and F10.
+    //
+    HotKey.UnicodeChar = CHAR_NULL;
+    HotKey.ScanCode    = SCAN_F9;
+    FormBrowserEx->RegisterHotKey (&HotKey, 0, 0, NULL);
+    HotKey.ScanCode    = SCAN_F10;
+    FormBrowserEx->RegisterHotKey (&HotKey, 0, 0, NULL);
+    
+    //
+    // Register the default HotKey F9 and F10 again.
+    //
+    HotKey.ScanCode   = SCAN_F10;
+    NewString         = HiiGetString (mPrivateData->HiiHandle[0], STRING_TOKEN (FUNCTION_TEN_STRING), NULL);
+    ASSERT (NewString != NULL);
+    FormBrowserEx->RegisterHotKey (&HotKey, BROWSER_ACTION_SUBMIT, 0, NewString);
+    HotKey.ScanCode   = SCAN_F9;
+    NewString         = HiiGetString (mPrivateData->HiiHandle[0], STRING_TOKEN (FUNCTION_NINE_STRING), NULL);
+    ASSERT (NewString != NULL);
+    FormBrowserEx->RegisterHotKey (&HotKey, BROWSER_ACTION_DEFAULT, EFI_HII_DEFAULT_CLASS_STANDARD, NewString);
+  }
+
+  //
+  // In default, this driver is built into Flash device image,
+  // the following code doesn't run.
+  //
+
+  //
+  // Example of how to display only the item we sent to HII
+  // When this driver is not built into Flash device image,
+  // it need to call SendForm to show front page by itself.
+  //
+  if (DISPLAY_ONLY_MY_ITEM <= 1) {
+    //
+    // Have the browser pull out our copy of the data, and only display our data
+    //
+    Status = FormBrowser2->SendForm (
+                             FormBrowser2,
+                             &(HiiHandle[DISPLAY_ONLY_MY_ITEM]),
+                             1,
+                             NULL,
+                             0,
+                             NULL,
+                             NULL
+                             );
+
+    HiiRemovePackages (HiiHandle[0]);
+
+    HiiRemovePackages (HiiHandle[1]);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unloads the application and its installed protocol.
+
+  @param[in]  ImageHandle       Handle that identifies the image to be unloaded.
+
+  @retval EFI_SUCCESS           The image has been unloaded.
+**/
+EFI_STATUS
+EFIAPI
+DriverSampleUnload (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  UINTN Index;
+
+  ASSERT (mPrivateData != NULL);
+
+  if (DriverHandle[0] != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+            DriverHandle[0],
+            &gEfiDevicePathProtocolGuid,
+            &mHiiVendorDevicePath0,
+            &gEfiHiiConfigAccessProtocolGuid,
+            &mPrivateData->ConfigAccess,
+            NULL
+           );
+    DriverHandle[0] = NULL;
+  }
+
+  if (DriverHandle[1] != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+            DriverHandle[1],
+            &gEfiDevicePathProtocolGuid,
+            &mHiiVendorDevicePath1,
+            NULL
+           );
+    DriverHandle[1] = NULL;
+  }
+
+  if (mPrivateData->HiiHandle[0] != NULL) {
+    HiiRemovePackages (mPrivateData->HiiHandle[0]);
+  }
+
+  if (mPrivateData->HiiHandle[1] != NULL) {
+    HiiRemovePackages (mPrivateData->HiiHandle[1]);
+  }
+
+  for (Index = 0; Index < NAME_VALUE_NAME_NUMBER; Index++) {
+    if (mPrivateData->NameValueName[Index] != NULL) {
+      FreePool (mPrivateData->NameValueName[Index]);
+    }
+  }
+  FreePool (mPrivateData);
+  mPrivateData = NULL;
+
+  gBS->CloseEvent (mEvent);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.h b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.h
new file mode 100644
index 0000000000..97dee9c79a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.h
@@ -0,0 +1,125 @@
+/** @file
+
+Copyright (c) 2007 - 2015, 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.
+
+Module Name:
+
+  DriverSample.h
+
+Abstract:
+
+
+Revision History
+
+
+**/
+
+#ifndef _DRIVER_SAMPLE_H_
+#define _DRIVER_SAMPLE_H_
+
+#include <Uefi.h>
+
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/FormBrowserEx.h>
+#include <Protocol/HiiConfigKeyword.h>
+
+#include <Guid/MdeModuleHii.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/HiiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiLib.h>
+
+#include "NVDataStruc.h"
+
+//
+// This is the generated IFR binary data for each formset defined in VFR.
+// This data array is ready to be used as input of HiiAddPackages() to
+// create a packagelist (which contains Form packages, String packages, etc).
+//
+extern UINT8  VfrBin[];
+extern UINT8  InventoryBin[];
+
+//
+// This is the generated String package data for all .UNI files.
+// This data array is ready to be used as input of HiiAddPackages() to
+// create a packagelist (which contains Form packages, String packages, etc).
+//
+extern UINT8  DriverSampleStrings[];
+
+#define DYNAMIC_ONE_OF_VAR_OFFSET        OFFSET_OF (DRIVER_SAMPLE_CONFIGURATION, DynamicOneof)
+#define DYNAMIC_ORDERED_LIST_VAR_OFFSET  OFFSET_OF (DRIVER_SAMPLE_CONFIGURATION, DynamicOrderedList)
+
+#define DEFAULT_CLASS_MANUFACTURING_VALUE     0xFF
+#define DEFAULT_CLASS_STANDARD_VALUE          0x0
+
+//
+// Number of name in Name/Value storage
+//
+#define NAME_VALUE_NAME_NUMBER       3
+
+#define DRIVER_SAMPLE_PRIVATE_SIGNATURE SIGNATURE_32 ('D', 'S', 'p', 's')
+
+typedef struct {
+  UINTN                            Signature;
+
+  EFI_HANDLE                       DriverHandle[2];
+  EFI_HII_HANDLE                   HiiHandle[2];
+  DRIVER_SAMPLE_CONFIGURATION      Configuration;
+  MY_EFI_VARSTORE_DATA             VarStoreConfig;
+  UINT8                            PasswordState;
+
+  //
+  // Name/Value storage Name list
+  //
+  EFI_STRING_ID                    NameStringId[NAME_VALUE_NAME_NUMBER];
+  EFI_STRING                       NameValueName[NAME_VALUE_NAME_NUMBER];
+
+  //
+  // Consumed protocol
+  //
+  EFI_HII_DATABASE_PROTOCOL        *HiiDatabase;
+  EFI_HII_STRING_PROTOCOL          *HiiString;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *HiiConfigRouting;
+  EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL *HiiKeywordHandler;
+
+  EFI_FORM_BROWSER2_PROTOCOL       *FormBrowser2;
+
+  //
+  // Produced protocol
+  //
+  EFI_HII_CONFIG_ACCESS_PROTOCOL   ConfigAccess;
+} DRIVER_SAMPLE_PRIVATE_DATA;
+
+#define DRIVER_SAMPLE_PRIVATE_FROM_THIS(a)  CR (a, DRIVER_SAMPLE_PRIVATE_DATA, ConfigAccess, DRIVER_SAMPLE_PRIVATE_SIGNATURE)
+
+#pragma pack(1)
+
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} HII_VENDOR_DEVICE_PATH;
+
+#pragma pack()
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.uni b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.uni
new file mode 100644
index 0000000000..82b0bc5077
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSample.uni
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleDxe.inf b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleDxe.inf
new file mode 100644
index 0000000000..f5c0b4670b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleDxe.inf
@@ -0,0 +1,101 @@
+## @file
+# This is a sample HII driver. 
+#
+# This driver shows how HII protocol, VFR and UNI files are used to create a HII 
+# driver which can be dipslayed and configured by a UEFI HII Form Browser.
+#
+# Copyright (c) 2007 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = DriverSample
+  MODULE_UNI_FILE                = DriverSample.uni
+  FILE_GUID                      = FE3542FE-C1D3-4EF8-657C-8048606FF671
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = DriverSampleInit
+  UNLOAD_IMAGE                   = DriverSampleUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  DriverSample.c
+  InventoryStrings.uni
+  NVDataStruc.h
+  VfrStrings.uni
+  DriverSample.h
+  Inventory.vfr
+  Vfr.vfr
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  HiiLib
+  PrintLib
+  UefiLib
+  DevicePathLib
+
+[Guids]
+  gEfiIfrTianoGuid                              ## PRODUCES ## UNDEFINED
+  gDriverSampleInventoryGuid                    ## CONSUMES ## HII
+  ## SOMETIMES_PRODUCES ## Event
+  ## CONSUMES ## Event
+  gEfiIfrRefreshIdOpGuid
+  ## CONSUMES ## HII
+  ## PRODUCES ## Variable:L"MyIfrNVData"
+  ## SOMETIMES_CONSUMES ## Variable:L"MyIfrNVData"
+  ## PRODUCES ## Variable:L"MyEfiVar"
+  ## SOMETIMES_CONSUMES ## Variable:L"MyEfiVar"
+  ## PRODUCES ## GUID  # HiiConstructConfigHdr MyEfiVar
+  ## PRODUCES ## GUID  # HiiConstructConfigHdr MyIfrNVData
+  ## SOMETIMES_CONSUMES ## GUID  # HiiIsConfigHdrMatch MyEfiVar
+  ## SOMETIMES_CONSUMES ## GUID  # HiiIsConfigHdrMatch MyIfrNVData
+  ## SOMETIMES_PRODUCES ## GUID  # HiiGetBrowserData MyIfrNVData
+  ## SOMETIMES_CONSUMES ## GUID  # HiiSetBrowserData MyIfrNVData
+  ## SOMETIMES_PRODUCES ## GUID  # HiiGetBrowserData MyEfiVar
+  ## SOMETIMES_CONSUMES ## GUID  # HiiSetBrowserData MyEfiVar
+  gDriverSampleFormSetGuid
+
+[Protocols]
+  ## PRODUCES # DriverSampleFormSet
+  ## PRODUCES # DriverSampleInventory
+  gEfiDevicePathProtocolGuid
+  gEfiHiiStringProtocolGuid                     ## CONSUMES
+  gEfiHiiConfigRoutingProtocolGuid              ## CONSUMES
+  gEfiHiiConfigAccessProtocolGuid               ## PRODUCES
+  gEfiFormBrowser2ProtocolGuid                  ## CONSUMES
+  gEfiHiiDatabaseProtocolGuid                   ## CONSUMES
+  gEfiSimpleTextInputExProtocolGuid             ## SOMETIMES_CONSUMES
+  gEfiFormBrowserExProtocolGuid                 ## CONSUMES
+  gEfiConfigKeywordHandlerProtocolGuid          ## CONSUMES
+
+[Depex]
+  gEfiSimpleTextOutProtocolGuid AND gEfiHiiDatabaseProtocolGuid AND gEfiVariableArchProtocolGuid AND gEfiVariableWriteArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  DriverSampleExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleExtra.uni b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleExtra.uni
new file mode 100644
index 0000000000..b97d184964
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/DriverSampleExtra.uni
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/Inventory.vfr b/Core/MdeModulePkg/Universal/DriverSampleDxe/Inventory.vfr
new file mode 100644
index 0000000000..daa2577383
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/Inventory.vfr
@@ -0,0 +1,117 @@
+///** @file
+//
+//    Sample Inventory Data
+//
+//  Copyright (c) 2004 - 2015, 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 "NVDataStruc.h"
+
+formset
+  guid     = DRIVER_SAMPLE_INVENTORY_GUID,
+  title    = STRING_TOKEN(STR_INV_FORM_SET_TITLE),
+  help     = STRING_TOKEN(STR_INV_FORM_SET_HELP),
+
+  form formid = 1,
+       title  = STRING_TOKEN(STR_INV_FORM1_TITLE);  // note formid is a variable (for readability) (UINT16) - also added Form to the line to signify the Op-Code
+
+  	text
+      help   = STRING_TOKEN(STR_INV_VERSION_HELP),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT2),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT3),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT4),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+    subtitle text = STRING_TOKEN(STR_INV_EMPTY_STRING);
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT5),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT6),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT7),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT8),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT9),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT10),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+  	text
+      help   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      text   = STRING_TOKEN(STR_INV_VERSION_TEXT11),
+	    text   = STRING_TOKEN(STR_INV_EMPTY_STRING),
+      flags  = 0,
+      key    = 0;
+
+    text
+      help   = STRING_TOKEN(STR_CHECK_KEYWORD_SUPPORT),
+      text   = STRING_TOKEN(STR_CHECK_KEYWORD_SUPPORT),
+      flags  = INTERACTIVE,
+      key    = 0x1231;
+
+    subtitle text = STRING_TOKEN(STR_INV_EMPTY_STRING);
+
+    subtitle text = STRING_TOKEN(STR_INV_VERSION_TEXT12);
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/InventoryStrings.uni b/Core/MdeModulePkg/Universal/DriverSampleDxe/InventoryStrings.uni
new file mode 100644
index 0000000000..24e48f8868
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/InventoryStrings.uni
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/NVDataStruc.h b/Core/MdeModulePkg/Universal/DriverSampleDxe/NVDataStruc.h
new file mode 100644
index 0000000000..c8262a45a0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/NVDataStruc.h
@@ -0,0 +1,93 @@
+/** @file
+
+Copyright (c) 2007 - 2011, 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.
+
+Module Name:
+
+  NVDataStruc.h
+
+Abstract:
+
+  NVData structure used by the sample driver
+
+Revision History:
+
+
+**/
+
+#ifndef _NVDATASTRUC_H_
+#define _NVDATASTRUC_H_
+
+#include <Guid/HiiPlatformSetupFormset.h>
+#include <Guid/HiiFormMapMethodGuid.h>
+#include <Guid/DriverSampleHii.h>
+#include <Guid/ZeroGuid.h>
+
+#define CONFIGURATION_VARSTORE_ID    0x1234
+
+#pragma pack(1)
+typedef struct {
+  UINT16  WhatIsThePassword[20];
+  UINT16  WhatIsThePassword2[20];
+  UINT16  MyStringData[40];
+  UINT16  PasswordClearText[20];
+  UINT16  SomethingHiddenForHtml;
+  UINT8   HowOldAreYouInYearsManual;
+  UINT16  HowTallAreYouManual;
+  UINT8   HowOldAreYouInYears;
+  UINT16  HowTallAreYou;
+  UINT8   MyFavoriteNumber;
+  UINT8   TestLateCheck;
+  UINT8   TestLateCheck2;
+  UINT8   QuestionAboutTreeHugging;
+  UINT8   ChooseToActivateNuclearWeaponry;
+  UINT8   SuppressGrayOutSomething;
+  UINT8   OrderedList[8];
+  UINT16  BootOrder[8];
+  UINT8   BootOrderLarge;
+  UINT8   DynamicRefresh;
+  UINT8   DynamicOneof;
+  UINT8   DynamicOrderedList[5];
+  UINT8   Reserved;
+  EFI_HII_REF RefData;
+  UINT8   NameValueVar0;
+  UINT16  NameValueVar1;
+  UINT16  NameValueVar2[20];
+  UINT8   SerialPortNo;
+  UINT8   SerialPortStatus;
+  UINT16  SerialPortIo;
+  UINT8   SerialPortIrq;
+  UINT8   GetDefaultValueFromCallBack;
+  UINT8   GetDefaultValueFromAccess;
+  EFI_HII_TIME  Time;
+  UINT8   RefreshGuidCount;
+  UINT8   Match2;
+} DRIVER_SAMPLE_CONFIGURATION;
+
+//
+// 2nd NV data structure definition
+//
+typedef struct {
+  UINT8         Field8;
+  UINT16        Field16;
+  UINT8         OrderedList[3];
+} MY_EFI_VARSTORE_DATA;
+
+//
+// Labels definition
+//
+#define LABEL_UPDATE1               0x1234
+#define LABEL_UPDATE2               0x2234
+#define LABEL_UPDATE3               0x3234
+#define LABEL_END                   0x2223
+
+#pragma pack()
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/Vfr.vfr b/Core/MdeModulePkg/Universal/DriverSampleDxe/Vfr.vfr
new file mode 100644
index 0000000000..bd28797a55
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/Vfr.vfr
@@ -0,0 +1,786 @@
+///** @file
+//
+//    Sample Setup formset.
+//
+//  Copyright (c) 2004 - 2015, 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 "NVDataStruc.h"
+
+//
+// Formset class used by Device Manager
+//
+#define EFI_NON_DEVICE_CLASS              0x00
+#define EFI_DISK_DEVICE_CLASS             0x01
+#define EFI_VIDEO_DEVICE_CLASS            0x02
+#define EFI_NETWORK_DEVICE_CLASS          0x04
+#define EFI_INPUT_DEVICE_CLASS            0x08
+#define EFI_ON_BOARD_DEVICE_CLASS         0x10
+#define EFI_OTHER_DEVICE_CLASS            0x20
+
+//
+// Formset subclass
+//
+#define EFI_SETUP_APPLICATION_SUBCLASS    0x00
+#define EFI_GENERAL_APPLICATION_SUBCLASS  0x01
+#define EFI_FRONT_PAGE_SUBCLASS           0x02
+#define EFI_SINGLE_USE_SUBCLASS           0x03
+
+//
+// EFI Variable attributes
+//
+#define EFI_VARIABLE_NON_VOLATILE       0x00000001
+#define EFI_VARIABLE_BOOTSERVICE_ACCESS 0x00000002
+#define EFI_VARIABLE_RUNTIME_ACCESS     0x00000004
+#define EFI_VARIABLE_READ_ONLY          0x00000008
+
+#define EFI_USER_INFO_ACCESS_SETUP_ADMIN_GUID \
+  { 0x85b75607, 0xf7ce, 0x471e, { 0xb7, 0xe4, 0x2a, 0xea, 0x5f, 0x72, 0x32, 0xee } }
+
+#define PERL_GUID \
+  { 0x63E60A51, 0x497D, 0xD427, {0xC4, 0xA5, 0xB8, 0xAB, 0xDC, 0x3A, 0xAE, 0xB6 }}
+
+//
+// Labels definition
+//
+#define LABEL_1_VALUE               0x01
+#define LABEL_2_VALUE               0x1000
+#define LABEL_UPDATE_BBS            0x2222
+
+formset
+  guid      = DRIVER_SAMPLE_FORMSET_GUID,
+  title     = STRING_TOKEN(STR_FORM_SET_TITLE),
+  help      = STRING_TOKEN(STR_FORM_SET_TITLE_HELP),
+  classguid = EFI_HII_PLATFORM_SETUP_FORMSET_GUID,
+  
+  //
+  // Notes: VfrCompiler will insert a Standard Default Storage declaration 
+  // after the formset declaration.    >00000040: 5C 06 00 00 00 00.
+  // So we don't need to declare the Standard Default.
+  // Please check the vfr.lst file for details.
+  // To enable list file for VFR, add "-l" to VfrCompile <Command> in [Build.Visual-Form-Representation-File] as follows:
+  //          VfrCompile -l --no-pre-processing --output-directory ${d_path} $(OUTPUT_DIR)(+)${s_dir}(+)${s_base}.iii
+  //
+  
+   //
+  // Define a Buffer Storage (EFI_IFR_VARSTORE)
+  //
+  varstore DRIVER_SAMPLE_CONFIGURATION,     // This is the data structure type
+    varid = CONFIGURATION_VARSTORE_ID,      // Optional VarStore ID
+    name  = MyIfrNVData,                    // Define referenced name in vfr
+    guid  = DRIVER_SAMPLE_FORMSET_GUID;     // GUID of this buffer storage
+
+  //
+  // Define a EFI variable Storage (EFI_IFR_VARSTORE_EFI)
+  //
+  efivarstore MY_EFI_VARSTORE_DATA,
+    attribute = EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,  // EFI variable attribures  
+    name  = MyEfiVar,
+    guid  = DRIVER_SAMPLE_FORMSET_GUID;
+
+  //
+  // Define a Name/Value Storage (EFI_IFR_VARSTORE_NAME_VALUE)
+  //
+  namevaluevarstore MyNameValueVar,                // Define storage reference name in vfr
+    name = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME0), // Define Name list of this storage, refer it by MyNameValueVar[0]
+    name = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME1), // Define Name list of this storage, refer it by MyNameValueVar[1]
+    name = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME2), // Define Name list of this storage, refer it by MyNameValueVar[2]
+    guid = DRIVER_SAMPLE_FORMSET_GUID;             // GUID of this Name/Value storage
+
+  defaultstore MyStandardDefault,
+    prompt      = STRING_TOKEN(STR_STANDARD_DEFAULT_PROMPT),
+    attribute   = 0x0000;                         // Default ID: 0000 standard default
+
+  defaultstore MyManufactureDefault,
+    prompt      = STRING_TOKEN(STR_MANUFACTURE_DEFAULT_PROMPT),
+    attribute   = 0x0001;                         // Default ID: 0001 manufacture default
+
+  //
+  // Define a Form (EFI_IFR_FORM)
+  //
+  form formid = 1,                                // Form ID
+       title  = STRING_TOKEN(STR_FORM1_TITLE);    // Form title
+
+    subtitle text = STRING_TOKEN(STR_SUBTITLE_TEXT);
+
+    subtitle text = STRING_TOKEN(STR_SUBTITLE_TEXT2);
+
+    //
+    // Define a display only text (EFI_IFR_TEXT)
+    //
+    text
+      help   = STRING_TOKEN(STR_TEXT_HELP),       // Help string
+      text   = STRING_TOKEN(STR_CPU_STRING),      // Prompt string
+        text   = STRING_TOKEN(STR_CPU_STRING2);   // TextTwo
+
+    //
+    // Define action button (EFI_IFR_ACTION)
+    //
+    text
+      help   = STRING_TOKEN(STR_EXIT_TEXT),
+      text   = STRING_TOKEN(STR_EXIT_TEXT),
+      flags  = INTERACTIVE,   // VfrCompiler will generate opcode EFI_IFR_ACTION for Text marked as INTERACTIVE
+      key    = 0x1237;
+
+    text
+      help   = STRING_TOKEN(STR_SAVE_TEXT),
+      text   = STRING_TOKEN(STR_SAVE_TEXT),
+      flags  = INTERACTIVE,
+      key    = 0x1238;
+
+    text
+      help   = STRING_TOKEN(STR_SAVE_CURRENT),
+      text   = STRING_TOKEN(STR_SAVE_CURRENT),
+      flags  = INTERACTIVE,
+      key    = 0x1243;
+
+    text
+      help   = STRING_TOKEN(STR_DISCARD_CURRENT_AND_EXIT),
+      text   = STRING_TOKEN(STR_DISCARD_CURRENT_AND_EXIT),
+      flags  = INTERACTIVE,
+      key    = 0x1244;
+    //
+    // Define oneof (EFI_IFR_ONE_OF)
+    //
+    oneof name = MyOneOf,                                // Define reference name for Question
+      varid   = MyIfrNVData.SuppressGrayOutSomething,    // Use "DataStructure.Member" to reference Buffer Storage
+      prompt  = STRING_TOKEN(STR_ONE_OF_PROMPT),
+      help    = STRING_TOKEN(STR_ONE_OF_HELP),
+      //
+      // Define an option (EFI_IFR_ONE_OF_OPTION)
+      //
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT4), value = 0x0, flags = 0;
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT5), value = 0x1, flags = 0;
+      //
+      // DEFAULT indicate this option will be marked with EFI_IFR_OPTION_DEFAULT
+      //
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT6), value = 0x2, flags = DEFAULT;
+    endoneof;
+
+    oneof varid  = MyIfrNVData.BootOrderLarge,
+      prompt      = STRING_TOKEN(STR_ONE_OF_PROMPT),
+      help        = STRING_TOKEN(STR_ONE_OF_HELP),
+      default value = cond (pushthis == 0 ? 0 : cond ((questionref(MyOneOf) >> 0x4 & 0xF00) == 0x0 + 0x2 ? 0 : 1)),
+      option text = STRING_TOKEN(STR_BOOT_ORDER1), value = 0x0, flags = 0;
+      option text = STRING_TOKEN(STR_BOOT_ORDER2), value = 0x1, flags = 0;
+    endoneof;
+
+    grayoutif  ideqval MyIfrNVData.SuppressGrayOutSomething == 0x1;
+      suppressif questionref(MyOneOf) == 0x0;
+
+        checkbox varid   = MyIfrNVData.ChooseToActivateNuclearWeaponry,
+                 prompt   = STRING_TOKEN(STR_CHECK_BOX_PROMPT),
+                 help     = STRING_TOKEN(STR_CHECK_BOX_HELP),
+                 //
+                 // CHECKBOX_DEFAULT indicate this checkbox is marked with EFI_IFR_CHECKBOX_DEFAULT
+                 // CHECKBOX_DEFAULT_MFG indicate EFI_IFR_CHECKBOX_DEFAULT_MFG.
+                 //
+                 flags    = CHECKBOX_DEFAULT | CHECKBOX_DEFAULT_MFG,
+                 default  = TRUE,
+        endcheckbox;
+      endif;
+    endif;
+
+    //
+    // Ordered list:
+    //   sizeof(MyIfrNVData) storage must be UINT8 array, and
+    //   size written for the variable must be size of the entire
+    //   variable.
+    //
+    //
+    suppressif ideqval MyIfrNVData.SuppressGrayOutSomething == 0x0;
+
+      //
+      // label is defined as an anchor where you want to insert some dynamic
+      // opcodes created on-the-fly
+      //
+      label LABEL_UPDATE_BBS;
+
+      orderedlist
+                varid       = MyIfrNVData.BootOrder,
+                prompt      = STRING_TOKEN(STR_BOOT_OPTIONS),
+                help        = STRING_TOKEN(STR_NULL_STRING),
+                flags       = RESET_REQUIRED,
+                option text = STRING_TOKEN(STR_BOOT_OPTION2), value = 2, flags = 0;
+                option text = STRING_TOKEN(STR_BOOT_OPTION1), value = 1, flags = 0;
+                option text = STRING_TOKEN(STR_BOOT_OPTION3), value = 3, flags = 0;
+              suppressif ideqval MyIfrNVData.BootOrderLarge == 0;
+                option text = STRING_TOKEN(STR_BOOT_OPTION4), value = 4, flags = 0;
+              endif
+      endlist;
+
+      //
+      // label should be paired with each other
+      //
+      label LABEL_END;
+
+    endif;    // end suppressif
+
+    disableif ideqval MyIfrNVData.SuppressGrayOutSomething == 0x2;
+      orderedlist
+        varid       = MyIfrNVData.OrderedList,
+        prompt      = STRING_TOKEN(STR_TEST_OPCODE),
+        help        = STRING_TOKEN(STR_TEXT_HELP),
+        flags       = RESET_REQUIRED,
+        option text = STRING_TOKEN(STR_ONE_OF_TEXT1), value = 3, flags = 0;
+        option text = STRING_TOKEN(STR_ONE_OF_TEXT2), value = 2, flags = 0;
+        option text = STRING_TOKEN(STR_ONE_OF_TEXT3), value = 1, flags = 0;
+        default     = {1,2,3},
+      endlist;
+    endif;
+
+    label 100;
+
+    //
+    // Define a hyperlink (EFI_IFR_REF)
+    //
+    goto 0x1234,                                  // Destination Form ID
+      prompt  = STRING_TOKEN(STR_GOTO_DYNAMIC),   // Prompt string
+      help    = STRING_TOKEN(STR_GOTO_HELP),      // Help string
+      flags   = INTERACTIVE,                      // INTERACTIVE indicate it's marked with EFI_IFR_FLAG_CALLBACK
+      key     = 0x1234;                           // Question ID which will be passed-in in COnfigAccess.Callback()
+
+    goto 0x1234,
+      prompt  = STRING_TOKEN(STR_GOTO_DYNAMIC2),
+      help    = STRING_TOKEN(STR_GOTO_HELP),
+      flags   = INTERACTIVE,
+      key     = 0x1235;
+
+    oneof varid  = MyIfrNVData.TestLateCheck,
+      prompt      = STRING_TOKEN(STR_TEST_OPCODE),
+      help        = STRING_TOKEN(STR_ONE_OF_HELP),
+      flags       = RESET_REQUIRED,
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT1), value = 0, flags = 0;
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT2), value = 1, flags = DEFAULT;
+      warningif prompt = STRING_TOKEN(STR_WARNING_POPUP), timeout = 5,
+        ideqval MyIfrNVData.TestLateCheck == 0
+      endif
+
+    endoneof;
+
+    oneof varid  = MyIfrNVData.TestLateCheck2,
+      prompt      = STRING_TOKEN(STR_TEST_OPCODE2),
+      help        = STRING_TOKEN(STR_ONE_OF_HELP),
+      flags       = RESET_REQUIRED,
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT1), value = 0, flags = DEFAULT;
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT2), value = 1, flags = 0;
+
+      inconsistentif prompt = STRING_TOKEN(STR_ERROR_POPUP),
+        ideqid MyIfrNVData.TestLateCheck == MyIfrNVData.TestLateCheck2
+      endif
+
+    endoneof;
+
+    oneof varid  = MyIfrNVData.QuestionAboutTreeHugging,
+      prompt      = STRING_TOKEN(STR_ONE_OF_PROMPT_KEYWORD),
+      help        = STRING_TOKEN(STR_ONE_OF_HELP),
+      flags       = RESET_REQUIRED,
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT1), value = 0, flags = 0;
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT2), value = 1, flags = DEFAULT;
+      option text = STRING_TOKEN(STR_ONE_OF_TEXT3), value = 3, flags = 0;
+    endoneof;
+
+    //
+    // Define a string (EFI_IFR_STRING)
+    //
+    string    varid    = MyIfrNVData.MyStringData,
+              prompt   = STRING_TOKEN(STR_MY_STRING_PROMPT2),
+              help     = STRING_TOKEN(STR_MY_STRING_HELP2),
+              flags    = INTERACTIVE,
+              key      = 0x1236,
+              minsize  = 6,
+              maxsize  = 40,
+              inconsistentif prompt = STRING_TOKEN(STR_STRING_CHECK_ERROR_POPUP),
+                pushthis != stringref(STRING_TOKEN(STR_STRING_CHECK))
+              endif
+    endstring;
+
+    //
+    // Define a numeric (EFI_IFR_NUMERIC)
+    //
+    numeric varid   = MyIfrNVData.HowOldAreYouInYearsManual,
+            prompt  = STRING_TOKEN(STR_NUMERIC_READONLY_PROMPT),
+            help    = STRING_TOKEN(STR_NUMERIC_HELP0),
+            flags   = READ_ONLY,  // READ_ONLY indicate it's marked with EFI_IFR_FLAG_READ_ONLY
+            minimum = 0,
+            maximum = 0xf0,
+            step    = 0,          // Stepping of 0 equates to a manual entering
+                                  // of a value, otherwise it will be adjusted by "+"/"-"
+            default = 21,         // defaultstore could be used to specify the default type
+                                  // If no defaultstore is specified, it implies Standard Default
+
+    endnumeric;
+
+    numeric varid   = MyIfrNVData.HowOldAreYouInYearsManual,
+            prompt  = STRING_TOKEN(STR_NUMERIC_MANUAL_PROMPT),
+            help    = STRING_TOKEN(STR_NUMERIC_HELP0),
+            minimum = 0,
+            maximum = 0xf0,
+            step    = 0,
+            default value = questionrefval(devicepath = STRING_TOKEN (STR_DEVICE_PATH), guid = DRIVER_SAMPLE_FORMSET_GUID, 0x1111),
+
+            inconsistentif prompt = STRING_TOKEN(STR_ERROR_POPUP),
+              ideqval MyIfrNVData.HowOldAreYouInYearsManual == 99
+              OR
+              ideqid  MyIfrNVData.HowOldAreYouInYearsManual == MyEfiVar.Field8
+              OR
+              ideqvallist MyIfrNVData.HowOldAreYouInYearsManual == 1 3 5 7
+            endif
+
+    endnumeric;
+
+    numeric varid   = MyEfiVar.Field8,                    // Reference of EFI variable storage
+            questionid  = 0x1111,
+            prompt  = STRING_TOKEN(STR_TALL_HEX_PROMPT),
+            help    = STRING_TOKEN(STR_NUMERIC_HELP1),
+            flags   = DISPLAY_UINT_HEX | INTERACTIVE,     // Display in HEX format (if not specified, default is in decimal format)
+            minimum = 0,
+            maximum = 250,
+            default = 18, defaultstore = MyStandardDefault,     // This is standard default value
+            default = 19, defaultstore = MyManufactureDefault,  // This is manufacture default value
+
+    endnumeric;
+
+    //
+    // Define numeric using Name/Value Storage
+    //
+    numeric varid   = MyNameValueVar[0],     // This numeric take NameValueVar0 as storage
+            prompt  = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME0),
+            help    = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME0_HELP),
+            //
+            // Size should be defined for numeric when use Name/Value storage
+            // Valid value for numerice size are: NUMERIC_SIZE_1, NUMERIC_SIZE_2, NUMERIC_SIZE_4 and NUMERIC_SIZE_8
+            //
+            flags   = NUMERIC_SIZE_1,        // Size of this numeric is 1 byte
+            minimum = 0,
+            maximum = 0xff,
+            step    = 0,
+            locked,
+            default = 16, defaultstore = MyStandardDefault,     // This is standard default value
+            default = 17, defaultstore = MyManufactureDefault,  // This is manufacture default value
+    endnumeric;
+
+    numeric varid   = MyNameValueVar[1],     // This numeric take NameValueVar1 as storage
+            prompt  = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME1),
+            help    = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME1_HELP),
+            flags   = NUMERIC_SIZE_2,        // Size of this numeric is 2 bytes
+            minimum = 0,
+            maximum = 0xffff,
+            step    = 0,
+            default = 18, defaultstore = MyStandardDefault,     // This is standard default value
+            default = 19, defaultstore = MyManufactureDefault,  // This is manufacture default value
+    endnumeric;
+
+    //
+    // Define string using Name/Value Storage
+    //
+    string    varid    = MyNameValueVar[2],     // This string take NameValueVar2 as storage
+              prompt   = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME2),
+              help     = STRING_TOKEN(STR_NAME_VALUE_VAR_NAME2_HELP),
+              minsize  = 2,
+              maxsize  = 0x14,
+    endstring;
+
+    oneof varid   = MyEfiVar.Field16,
+      prompt      = STRING_TOKEN(STR_ONE_OF_PROMPT),
+      help        = STRING_TOKEN(STR_NUMERIC_NUM_HELP),
+      option text = STRING_TOKEN(STR_BOOT_ORDER1), value = 0x0, flags = 0;
+      option text = STRING_TOKEN(STR_BOOT_ORDER2), value = 0x1, flags = DEFAULT;
+    endoneof;
+
+    label LABEL_1_VALUE;
+    label LABEL_2_VALUE;
+
+    grayoutif  ideqval MyIfrNVData.HowOldAreYouInYearsManual == 23 AND ideqval MyIfrNVData.SuppressGrayOutSomething == 0x1;
+      numeric varid   = MyIfrNVData.HowOldAreYouInYears,
+              prompt  = STRING_TOKEN(STR_NUMERIC_STEP_PROMPT),
+              help    = STRING_TOKEN(STR_NUMERIC_HELP2),
+              minimum = 0,
+              maximum = 243,
+              step    = 1,
+              default = 18, defaultstore = MyStandardDefault,     // This is standard default value
+              default = 19, defaultstore = MyManufactureDefault,  // This is manufacture default value
+        
+      endnumeric;
+    endif;
+
+    numeric varid   = MyIfrNVData.GetDefaultValueFromAccess,
+            questionid = 0x1239,
+            prompt  = STRING_TOKEN(STR_DEFAULT_VALUE_FROM_ACCESS_PROMPT),
+            help    = STRING_TOKEN(STR_DEFAULT_VALUE_FROM_ACCESS_HELP),
+            flags   = DISPLAY_UINT_HEX | INTERACTIVE, 
+            minimum = 0,
+            maximum = 255,
+            step    = 1,
+            default = 18,
+    endnumeric;
+
+    numeric varid   = MyIfrNVData.GetDefaultValueFromCallBack,
+            questionid = 0x1240,
+            prompt  = STRING_TOKEN(STR_DEFAULT_VALUE_FROM_CALLBACK_PROMPT),
+            help    = STRING_TOKEN(STR_DEFAULT_VALUE_FROM_CALLBACK_HELP),
+            flags   = DISPLAY_UINT_HEX | INTERACTIVE, 
+            minimum = 0,
+            maximum = 255,
+            step    = 1,
+            default = 18,
+    endnumeric;
+        
+    resetbutton
+      defaultstore = MyStandardDefault,
+      prompt   = STRING_TOKEN(STR_STANDARD_DEFAULT_PROMPT),
+      help     = STRING_TOKEN(STR_STANDARD_DEFAULT_HELP),
+    endresetbutton;
+    
+    resetbutton
+      defaultstore = MyManufactureDefault,
+      prompt   = STRING_TOKEN(STR_MANUFACTURE_DEFAULT_PROMPT),
+      help     = STRING_TOKEN(STR_MANUFACTURE_DEFAULT_HELP),
+    endresetbutton;
+
+    //
+    // Non-interactive password, validate by Setup Browser
+    //
+    password  varid    = MyIfrNVData.WhatIsThePassword,
+              prompt   = STRING_TOKEN(STR_PASSWORD_PROMPT),
+              help     = STRING_TOKEN(STR_PASSWORD_HELP),
+              minsize  = 6,
+              maxsize  = 20,
+    endpassword;
+
+    string    varid    = MyIfrNVData.PasswordClearText,
+              prompt   = STRING_TOKEN(STR_MY_STRING_PROMPT),
+              help     = STRING_TOKEN(STR_MY_STRING_HELP),
+              minsize  = 6,
+              maxsize  = 0x14,
+              default  = STRING_TOKEN(STR_MY_STRING_DEFAULT),
+    endstring;
+
+    //
+    // Interactive password, validate via ConfigAccess.Callback()
+    //
+    password  varid    = MyIfrNVData.WhatIsThePassword2,
+              prompt   = STRING_TOKEN(STR_PASSWORD_CALLBACK_PROMPT),
+              help     = STRING_TOKEN(STR_PASSWORD_HELP),
+              flags    = INTERACTIVE,
+              key      = 0x2000,
+              minsize  = 6,
+              maxsize  = 20,
+    endpassword;
+
+    //
+    // Sample use case for IFR Security op-code
+    //
+    grayoutif NOT security (EFI_USER_INFO_ACCESS_SETUP_ADMIN_GUID);
+      text
+        help = STRING_TOKEN(STR_TEXT_SECRUITY_TEST_HELP),
+        text = STRING_TOKEN(STR_TEXT_SECRUITY_TEST_TEXT);
+    endif;
+    
+    goto 2,
+      prompt = STRING_TOKEN(STR_GOTO_FORM2), //SecondSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    goto 3,
+      prompt = STRING_TOKEN(STR_GOTO_FORM3), //ThirdSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    goto 4,
+      prompt = STRING_TOKEN(STR_GOTO_FORM4), //FourthSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    goto 5,
+      prompt = STRING_TOKEN(STR_GOTO_FORM5), //FifthSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+      help   = STRING_TOKEN(STR_GOTO_FORM5_HELP);
+      
+    goto 6,
+      prompt = STRING_TOKEN(STR_GOTO_FORM6), //SixthSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    goto 
+      formsetguid = DRIVER_SAMPLE_INVENTORY_GUID,
+      formid  = 0x1,
+      question = 0x1,
+      prompt  = STRING_TOKEN(STR_GOTO_ANOTHER_FORMSET),
+      help    = STRING_TOKEN(STR_GOTO_ANOTHER_FORMSET_HELP);
+
+    guidop 
+      guid = DRIVER_SAMPLE_FORMSET_GUID,
+      datatype = MY_EFI_VARSTORE_DATA,
+        data.Field8  = 0x21, 
+        data.Field16 = 0x2121,
+        data.OrderedList[0] = 0x21,        
+    endguidop;
+
+
+  endform;
+
+  suppressif ideqval MyIfrNVData.BootOrderLarge == 0;
+    form formid = 2,               // SecondSetupPage,
+      title = STRING_TOKEN(STR_FORM2_TITLE);  // note formid is a variable (for readability) (UINT16) - also added Form to the line to signify the Op-Code
+
+      date
+        name    = Date,
+        prompt  = STRING_TOKEN(STR_DATE_PROMPT),
+        help    = STRING_TOKEN(STR_DATE_HELP),
+        flags   = STORAGE_TIME,
+        default = 2004/1/1,
+
+        inconsistentif prompt = STRING_TOKEN(STR_ERROR_POPUP),
+          ideqval Date.Day == 31
+          AND
+          ideqvallist Date.Month == 2 4 6 9 11
+        endif
+
+        //
+        // If the day is 30 AND month is 2
+        //
+        inconsistentif prompt = STRING_TOKEN(STR_ERROR_POPUP),
+          ideqval Date.Day == 30
+          AND
+          ideqval Date.Month == 2
+        endif
+
+        //
+        // If the day is 29 AND month is 2 AND it year is NOT a leapyear
+        //
+        inconsistentif prompt = STRING_TOKEN(STR_ERROR_POPUP),
+          ideqval Date.Day == 0x1D
+          AND
+          ideqval Date.Month == 2
+          AND
+          NOT
+          ideqvallist Date.Year == 2004 2008 20012 20016 2020 2024 2028 2032 2036
+        endif
+
+      enddate;
+
+      text
+        help   = STRING_TOKEN(STR_SAVE_CURRENT_AND_EXIT),
+        text   = STRING_TOKEN(STR_SAVE_CURRENT_AND_EXIT),
+        flags  = INTERACTIVE,
+        key    = 0x1241;
+
+      text
+        help   = STRING_TOKEN(STR_DISCARD_CURRENT),
+        text   = STRING_TOKEN(STR_DISCARD_CURRENT),
+        flags  = INTERACTIVE,
+        key    = 0x1242;
+
+      time
+        prompt  = STRING_TOKEN(STR_TIME_PROMPT),
+        help    = STRING_TOKEN(STR_TIME_HELP),
+        flags   = STORAGE_TIME,
+      endtime;
+      
+      time 
+            name    = MyTime, 
+            varid   = MyIfrNVData.Time, 
+            prompt  = STRING_TOKEN(STR_TIME_PROMPT), 
+            help    = STRING_TOKEN(STR_TIME_PROMPT), 
+            flags   = STORAGE_NORMAL | SECOND_SUPPRESS,
+            default = 15:33:33, 
+      endtime;
+      
+      checkbox varid   = MyIfrNVData.ChooseToActivateNuclearWeaponry,
+              prompt   = STRING_TOKEN(STR_CHECK_BOX_PROMPT),
+              help     = STRING_TOKEN(STR_CHECK_BOX_HELP),
+              flags    = CHECKBOX_DEFAULT,
+      endcheckbox;
+
+      text
+        help = STRING_TOKEN(STR_TEXT_HELP),
+        text = STRING_TOKEN(STR_TEXT_TEXT_1);
+
+      text
+        help   = STRING_TOKEN(STR_TEXT_HELP),
+        text   = STRING_TOKEN(STR_TEXT_TEXT_1),
+        text   = STRING_TOKEN(STR_TEXT_TEXT_2);
+
+      goto 1,
+        prompt = STRING_TOKEN(STR_GOTO_FORM1), //MainSetupPage  // this too has no end-op and basically it's a jump to a form ONLY
+        help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    goto 
+      varid   = MyIfrNVData.RefData,
+      prompt  = STRING_TOKEN(STR_GOTO_DYNAMIC3),
+      help    = STRING_TOKEN(STR_GOTO_DYNAMIC3_HELP),
+      flags   = INTERACTIVE,
+      key     = 0x1248,
+      //
+      // Set the defult value, format is QuestionId; FormId; FormsetGuid; Device Path String Token
+      //
+      default = 0;0;ZERO_GUID;STRING_TOKEN(STR_NULL_STRING),  
+    ;                                                         // goto opcode end flag.
+
+    goto 
+      prompt  = STRING_TOKEN(STR_GOTO_DYNAMIC4),
+      help    = STRING_TOKEN(STR_GOTO_DYNAMIC4_HELP),
+      flags   = INTERACTIVE,
+      key     = 0x1249;
+      
+    endform;
+  endif;
+
+  form formid = 3, title = STRING_TOKEN(STR_FORM3_TITLE);  // note formid is a variable (for readability) (UINT16) - also added Form to the line to signify the Op-Code
+
+    suppressif  ideqval MyEfiVar.Field8 == 111;
+      text
+        help = STRING_TOKEN(STR_TEXT_HELP),
+        text = STRING_TOKEN(STR_TEXT_TEXT_1);
+    endif;
+
+    goto 1,
+      prompt = STRING_TOKEN(STR_GOTO_FORM1), //MainSetupPage
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+    numeric varid   = MyIfrNVData.DynamicRefresh,
+            prompt  = STRING_TOKEN(STR_NUMERIC_MANUAL_PROMPT),
+            help    = STRING_TOKEN(STR_NUMERIC_HELP0),
+            flags   = INTERACTIVE,
+            key     = 0x5678,
+            minimum = 0,
+            maximum = 0xff,
+            step    = 0,
+            default = 0,
+            refresh interval = 3             // Refresh interval in seconds
+    endnumeric;
+
+    grayoutif  match2 (stringref(STRING_TOKEN(STR_PATTERN)), stringref(STRING_TOKEN(STR_STRING)), PERL_GUID);
+      numeric 
+        varid   = MyIfrNVData.Match2,
+        prompt  = STRING_TOKEN(STR_MATCH2_PROMPT),
+        help    = STRING_TOKEN(STR_MATCH2_HELP),
+        minimum = 0,
+        maximum = 243,
+      endnumeric;
+    endif;
+
+    label LABEL_UPDATE2;
+    label LABEL_END;
+
+  endform;
+
+  formmap formid = 4,            
+    maptitle = STRING_TOKEN(STR_SAMPL_MAP_METHOD);  
+    mapguid  = DRIVER_SAMPLE_FORMSET_GUID;
+    maptitle = STRING_TOKEN(STR_STANDARD_MAP_METHOD);  
+    mapguid  = EFI_HII_STANDARD_FORM_GUID;
+
+    oneof varid = MyIfrNVData.SerialPortNo,    
+      prompt  = STRING_TOKEN(STR_SERIAL_PORT),
+      help    = STRING_TOKEN(STR_ONE_OF_HELP),
+
+      read    cond (get(MyIfrNVData.SerialPortStatus) != 0 ? 0 : cond ((get(MyIfrNVData.SerialPortIo) & 0xF00) >> 0x8 == get(MyIfrNVData.SerialPortIrq) - 1 ? UNDEFINED : map (get(MyIfrNVData.SerialPortIo) : 0x3f8,1; 0x2F8,2; 0x3E8,3; 0x2E8,4;)));
+      write   set(MyIfrNVData.SerialPortStatus, pushthis != 0) AND set(MyIfrNVData.SerialPortIo, map (pushthis : 1,0x3F8; 2,0x2F8; 3,0x3E8; 4,0x2E8;)) AND set (MyIfrNVData.SerialPortIrq, map (pushthis: 1,4; 2,3; 3,4; 4,3;));
+      
+      option text = STRING_TOKEN(STR_SERIAL_PORT_DISABLE), value = 0x0, flags = DEFAULT;
+      option text = STRING_TOKEN(STR_SERIAL_PORT1), value = 0x1, flags = 0;
+      option text = STRING_TOKEN(STR_SERIAL_PORT2), value = 0x2, flags = 0;
+      option text = STRING_TOKEN(STR_SERIAL_PORT3), value = 0x3, flags = 0;
+      option text = STRING_TOKEN(STR_SERIAL_PORT4), value = 0x4, flags = 0;
+    endoneof;
+
+    grayoutif TRUE;
+      checkbox varid = MyIfrNVData.SerialPortStatus,
+        prompt   = STRING_TOKEN(STR_SERIAL_PORT_STATUS),
+        help     = STRING_TOKEN(STR_CHECK_BOX_HELP),
+      endcheckbox;
+    endif;
+
+    grayoutif TRUE;
+      suppressif ideqval MyIfrNVData.SerialPortStatus == 0;
+        oneof varid = MyIfrNVData.SerialPortIo,    
+          prompt  = STRING_TOKEN(STR_SERIAL_PORT_IO_ADDRESS),
+          help    = STRING_TOKEN(STR_ONE_OF_HELP),
+          
+          option text = STRING_TOKEN(STR_SERIAL_PORT1_IOADDR), value = 0x3F8, flags = DEFAULT;
+          option text = STRING_TOKEN(STR_SERIAL_PORT2_IOADDR), value = 0x2F8, flags = 0;
+          option text = STRING_TOKEN(STR_SERIAL_PORT3_IOADDR), value = 0x3E8, flags = 0;
+          option text = STRING_TOKEN(STR_SERIAL_PORT4_IOADDR), value = 0x2E8, flags = 0;
+        endoneof;
+      endif;
+    endif;
+
+    grayoutif TRUE;
+      suppressif ideqval MyIfrNVData.SerialPortStatus == 0;
+        oneof varid = MyIfrNVData.SerialPortIrq,    
+          prompt  = STRING_TOKEN(STR_SERIAL_PORT_IRQ),
+          help    = STRING_TOKEN(STR_ONE_OF_HELP),
+          
+          option text = STRING_TOKEN(STR_SERIAL_PORT13_IRQ), value = 0x4, flags = DEFAULT;
+          option text = STRING_TOKEN(STR_SERIAL_PORT24_IRQ), value = 0x3, flags = 0;
+        endoneof;
+      endif;
+    endif;
+
+    goto 1,
+      prompt = STRING_TOKEN(STR_GOTO_FORM1), //MainSetupPage
+      help   = STRING_TOKEN(STR_GOTO_HELP);
+
+  endform;
+
+  form formid = 5,            // Modal form
+       title = STRING_TOKEN(STR_MODAL_FORM_TITLE);
+    //
+    //  This form is a modal form.
+    //
+    modal;
+    text
+      help   = STRING_TOKEN(STR_EXIT_TEXT),
+      text   = STRING_TOKEN(STR_EXIT_TEXT),
+      flags  = INTERACTIVE,   // VfrCompiler will generate opcode EFI_IFR_ACTION for Text marked as INTERACTIVE
+      key    = 0x1245;
+
+    text
+      help   = STRING_TOKEN(STR_SAVE_TEXT),
+      text   = STRING_TOKEN(STR_SAVE_TEXT),
+      flags  = INTERACTIVE,   // VfrCompiler will generate opcode EFI_IFR_ACTION for Text marked as INTERACTIVE
+      key    = 0x1246;
+  endform;
+
+  form formid = 6,            // Form to show the refresh guid group op-code
+       title = STRING_TOKEN(STR_FORM6_TITLE); 
+
+    text
+      help   = STRING_TOKEN(STR_TEXT_REFRESH_GUID),
+      text   = STRING_TOKEN(STR_TEXT_REFRESH_GUID);
+
+    numeric varid   = MyIfrNVData.RefreshGuidCount,
+            prompt  = STRING_TOKEN(STR_TEXT_REFRESH_GUID_COUNT),
+            help    = STRING_TOKEN(STR_NUMERIC_HELP0),
+            flags   = INTERACTIVE,
+            key     = 0x1247,
+            minimum = 0,
+            maximum = 0xff,
+            step    = 0,
+            default = 0,
+            refreshguid = EFI_IFR_REFRESH_ID_OP_GUID,
+    endnumeric;
+
+    label LABEL_UPDATE3;
+    label LABEL_END;
+
+  endform;
+
+  form formid = 0x1234,            // Dynamically created page,
+       title = STRING_TOKEN(STR_DYNAMIC_TITLE);  // note formid is a variable (for readability) (UINT16) - also added Form to the line to signify the Op-Code
+
+    label LABEL_UPDATE1;
+    //
+    // This is where we will insert dynamic created opcodes
+    //
+    label LABEL_END;
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Universal/DriverSampleDxe/VfrStrings.uni b/Core/MdeModulePkg/Universal/DriverSampleDxe/VfrStrings.uni
new file mode 100644
index 0000000000..0a5c2424d5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/DriverSampleDxe/VfrStrings.uni
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.inf b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.inf
new file mode 100644
index 0000000000..0dfe005e7b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.inf
@@ -0,0 +1,85 @@
+## @file
+#  Module that produces EBC Interprete and EBC Debug Support protocols.
+#  
+#  This module implements EFI Byte Code (EBC) Virtual Machine that can provide 
+#  platform and processor-independent mechanisms for loading and executing EFI
+#  device drivers.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EbcDxe
+  MODULE_UNI_FILE                = EbcDxe.uni
+  FILE_GUID                      = 13AC6DD0-73D0-11D4-B06B-00AA00BD6DE7
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeEbcDriver
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF
+#
+
+[Sources]
+  EbcExecute.h
+  EbcExecute.c
+  EbcInt.h
+  EbcInt.c
+
+[Sources.Ia32]
+  Ia32/EbcSupport.c
+  Ia32/EbcLowLevel.S
+  Ia32/EbcLowLevel.asm
+
+[Sources.X64]
+  X64/EbcSupport.c
+  X64/EbcLowLevel.S
+  X64/EbcLowLevel.asm
+
+[Sources.IPF]
+  Ipf/EbcSupport.h
+  Ipf/EbcSupport.c
+  Ipf/EbcLowLevel.s
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  UefiDriverEntryPoint
+  DebugLib
+  BaseLib
+
+
+[Protocols]
+  gEfiDebugSupportProtocolGuid                  ## PRODUCES
+  gEfiEbcProtocolGuid                           ## PRODUCES
+  gEfiEbcVmTestProtocolGuid                     ## SOMETIMES_PRODUCES
+  gEfiEbcSimpleDebuggerProtocolGuid             ## SOMETIMES_CONSUMES
+
+[Depex]
+  TRUE
+
+# [Event]
+#
+# Periodic timer event to support EFI debug support protocol for EBC image.
+#
+# EVENT_TYPE_PERIODIC_TIMER ## CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EbcDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.uni b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.uni
new file mode 100644
index 0000000000..2c81a105c4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxe.uni
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcDxeExtra.uni b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxeExtra.uni
new file mode 100644
index 0000000000..6a8adee72d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.c b/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.c
new file mode 100644
index 0000000000..433eea250f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.c
@@ -0,0 +1,5350 @@
+/** @file
+  Contains code that implements the virtual machine.
+
+Copyright (c) 2006 - 2014, 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 "EbcInt.h"
+#include "EbcExecute.h"
+
+
+//
+// Define some useful data size constants to allow switch statements based on
+// size of operands or data.
+//
+#define DATA_SIZE_INVALID 0
+#define DATA_SIZE_8       1
+#define DATA_SIZE_16      2
+#define DATA_SIZE_32      4
+#define DATA_SIZE_64      8
+#define DATA_SIZE_N       48  // 4 or 8
+//
+// Structure we'll use to dispatch opcodes to execute functions.
+//
+typedef struct {
+  EFI_STATUS (*ExecuteFunction) (IN VM_CONTEXT * VmPtr);
+}
+VM_TABLE_ENTRY;
+
+typedef
+UINT64
+(*DATA_MANIP_EXEC_FUNCTION) (
+  IN VM_CONTEXT * VmPtr,
+  IN UINT64     Op1,
+  IN UINT64     Op2
+  );
+
+/**
+  Decode a 16-bit index to determine the offset. Given an index value:
+
+    b15     - sign bit
+    b14:12  - number of bits in this index assigned to natural units (=a)
+    ba:11   - constant units = ConstUnits
+    b0:a    - natural units = NaturalUnits
+
+  Given this info, the offset can be computed by:
+    offset = sign_bit * (ConstUnits + NaturalUnits * sizeof(UINTN))
+
+  Max offset is achieved with index = 0x7FFF giving an offset of
+  0x27B (32-bit machine) or 0x477 (64-bit machine).
+  Min offset is achieved with index =
+
+  @param  VmPtr             A pointer to VM context.
+  @param  CodeOffset        Offset from IP of the location of the 16-bit index
+                            to decode.
+
+  @return The decoded offset.
+
+**/
+INT16
+VmReadIndex16 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  );
+
+/**
+  Decode a 32-bit index to determine the offset.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  CodeOffset        Offset from IP of the location of the 32-bit index
+                            to decode.
+
+  @return Converted index per EBC VM specification.
+
+**/
+INT32
+VmReadIndex32 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  );
+
+/**
+  Decode a 64-bit index to determine the offset.
+
+  @param  VmPtr             A pointer to VM context.s
+  @param  CodeOffset        Offset from IP of the location of the 64-bit index
+                            to decode.
+
+  @return Converted index per EBC VM specification
+
+**/
+INT64
+VmReadIndex64 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  );
+
+/**
+  Reads 8-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 8-bit value from the memory address.
+
+**/
+UINT8
+VmReadMem8 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr
+  );
+
+/**
+  Reads 16-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 16-bit value from the memory address.
+
+**/
+UINT16
+VmReadMem16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINTN      Addr
+  );
+
+/**
+  Reads 32-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 32-bit value from the memory address.
+
+**/
+UINT32
+VmReadMem32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINTN      Addr
+  );
+
+/**
+  Reads 64-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 64-bit value from the memory address.
+
+**/
+UINT64
+VmReadMem64 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr
+  );
+
+/**
+  Read a natural value from memory. May or may not be aligned.
+
+  @param  VmPtr             current VM context
+  @param  Addr              the address to read from
+
+  @return The natural value at address Addr.
+
+**/
+UINTN
+VmReadMemN (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr
+  );
+
+/**
+  Writes 8-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem8 (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr,
+  IN UINT8         Data
+  );
+
+/**
+  Writes 16-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem16 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT16       Data
+  );
+
+/**
+  Writes 32-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem32 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT32       Data
+  );
+
+/**
+  Reads 16-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 16-bit value from the code stream.
+
+**/
+UINT16
+VmReadCode16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 32-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 32-bit value from the code stream.
+
+**/
+UINT32
+VmReadCode32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 64-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 64-bit value from the code stream.
+
+**/
+UINT64
+VmReadCode64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 8-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT8
+VmReadImmed8 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 16-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT16
+VmReadImmed16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 32-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT32
+VmReadImmed32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Reads 64-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT64
+VmReadImmed64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  );
+
+/**
+  Given an address that EBC is going to read from or write to, return
+  an appropriate address that accounts for a gap in the stack.
+  The stack for this application looks like this (high addr on top)
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+  The EBC assumes that its arguments are at the top of its stack, which
+  is where the VM stack is really. Therefore if the EBC does memory
+  accesses into the VM stack area, then we need to convert the address
+  to point to the EBC entry point arguments area. Do this here.
+
+  @param  VmPtr             A Pointer to VM context.
+  @param  Addr              Address of interest
+
+  @return The unchanged address if it's not in the VM stack region. Otherwise,
+          adjust for the stack gap and return the modified address.
+
+**/
+UINTN
+ConvertStackAddr (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr
+  );
+
+/**
+  Execute all the EBC data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+  @param  IsSignedOp        Indicates whether the operand is signed or not.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteDataManip (
+  IN VM_CONTEXT   *VmPtr,
+  IN BOOLEAN      IsSignedOp
+  );
+
+//
+// Functions that execute VM opcodes
+//
+/**
+  Execute the EBC BREAK instruction.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteBREAK (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the JMP instruction.
+
+  Instruction syntax:
+    JMP64{cs|cc} Immed64
+    JMP32{cs|cc} {@}R1 {Immed32|Index32}
+
+  Encoding:
+    b0.7 -  immediate data present
+    b0.6 -  1 = 64 bit immediate data
+            0 = 32 bit immediate data
+    b1.7 -  1 = conditional
+    b1.6    1 = CS (condition set)
+            0 = CC (condition clear)
+    b1.4    1 = relative address
+            0 = absolute address
+    b1.3    1 = operand1 indirect
+    b1.2-0  operand 1
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteJMP (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC JMP8 instruction.
+
+  Instruction syntax:
+    JMP8{cs|cc}  Offset/2
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteJMP8 (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Implements the EBC CALL instruction.
+
+  Instruction format:
+    CALL64 Immed64
+    CALL32 {@}R1 {Immed32|Index32}
+    CALLEX64 Immed64
+    CALLEX16 {@}R1 {Immed32}
+
+    If Rx == R0, then it's a PC relative call to PC = PC + imm32.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCALL (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC RET instruction.
+
+  Instruction syntax:
+    RET
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteRET (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC CMP instruction.
+
+  Instruction syntax:
+    CMP[32|64][eq|lte|gte|ulte|ugte] R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCMP (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC CMPI instruction
+
+  Instruction syntax:
+    CMPI[32|64]{w|d}[eq|lte|gte|ulte|ugte] {@}Rx {Index16}, Immed16|Immed32
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCMPI (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the MOVxx instructions.
+
+  Instruction format:
+
+    MOV[b|w|d|q|n]{w|d} {@}R1 {Index16|32}, {@}R2 {Index16|32}
+    MOVqq {@}R1 {Index64}, {@}R2 {Index64}
+
+    Copies contents of [R2] -> [R1], zero extending where required.
+
+    First character indicates the size of the move.
+    Second character indicates the size of the index(s).
+
+    Invalid to have R1 direct with index.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVxx (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC MOVI.
+
+  Instruction syntax:
+
+    MOVI[b|w|d|q][w|d|q] {@}R1 {Index16}, ImmData16|32|64
+
+    First variable character specifies the move size
+    Second variable character specifies size of the immediate data
+
+    Sign-extend the immediate data to the size of the operation, and zero-extend
+    if storing to a register.
+
+    Operand1 direct with index/immed is invalid.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVI (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC MOV immediate natural. This instruction moves an immediate
+  index value into a register or memory location.
+
+  Instruction syntax:
+
+    MOVIn[w|d|q] {@}R1 {Index16}, Index16|32|64
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVIn (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC MOVREL instruction.
+  Dest <- Ip + ImmData
+
+  Instruction syntax:
+
+    MOVREL[w|d|q] {@}R1 {Index16}, ImmData16|32|64
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVREL (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC PUSHn instruction
+
+  Instruction syntax:
+    PUSHn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePUSHn (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC PUSH instruction.
+
+  Instruction syntax:
+    PUSH[32|64] {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePUSH (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC POPn instruction.
+
+  Instruction syntax:
+    POPn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePOPn (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC POP instruction.
+
+  Instruction syntax:
+    POPn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePOP (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute all the EBC signed data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteSignedDataManip (
+  IN VM_CONTEXT   *VmPtr
+  );
+
+/**
+  Execute all the EBC unsigned data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteUnsignedDataManip (
+  IN VM_CONTEXT   *VmPtr
+  );
+
+/**
+  Execute the EBC LOADSP instruction.
+
+  Instruction syntax:
+    LOADSP  SP1, R2
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteLOADSP (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC STORESP instruction.
+
+  Instruction syntax:
+    STORESP  Rx, FLAGS|IP
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteSTORESP (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC MOVsnw instruction. This instruction loads a signed
+  natural value from memory or register to another memory or register. On
+  32-bit machines, the value gets sign-extended to 64 bits if the destination
+  is a register.
+
+  Instruction syntax:
+
+    MOVsnd {@}R1 {Indx32}, {@}R2 {Index32|Immed32}
+
+    0:7 1=>operand1 index present
+    0:6 1=>operand2 index present
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVsnd (
+  IN VM_CONTEXT *VmPtr
+  );
+
+/**
+  Execute the EBC MOVsnw instruction. This instruction loads a signed
+  natural value from memory or register to another memory or register. On
+  32-bit machines, the value gets sign-extended to 64 bits if the destination
+  is a register.
+
+  Instruction syntax:
+
+    MOVsnw {@}R1 {Index16}, {@}R2 {Index16|Immed16}
+
+    0:7 1=>operand1 index present
+    0:6 1=>operand2 index present
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVsnw (
+  IN VM_CONTEXT *VmPtr
+  );
+
+//
+// Data manipulation subfunctions
+//
+/**
+  Execute the EBC NOT instruction.s
+
+  Instruction syntax:
+    NOT[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return ~Op2
+
+**/
+UINT64
+ExecuteNOT (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT64         Op1,
+  IN UINT64         Op2
+  );
+
+/**
+  Execute the EBC NEG instruction.
+
+  Instruction syntax:
+    NEG[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op2 * -1
+
+**/
+UINT64
+ExecuteNEG (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC ADD instruction.
+
+  Instruction syntax:
+    ADD[32|64] {@}R1, {@}R2 {Index16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 + Op2
+
+**/
+UINT64
+ExecuteADD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC SUB instruction.
+
+  Instruction syntax:
+    SUB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 - Op2
+
+**/
+UINT64
+ExecuteSUB (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC MUL instruction.
+
+  Instruction syntax:
+    SUB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 * Op2
+
+**/
+UINT64
+ExecuteMUL (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC MULU instruction
+
+  Instruction syntax:
+    MULU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (unsigned)Op1 * (unsigned)Op2
+
+**/
+UINT64
+ExecuteMULU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC DIV instruction.
+
+  Instruction syntax:
+    DIV[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 / Op2
+
+**/
+UINT64
+ExecuteDIV (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC DIVU instruction
+
+  Instruction syntax:
+    DIVU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (unsigned)Op1 / (unsigned)Op2
+
+**/
+UINT64
+ExecuteDIVU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC MOD instruction.
+
+  Instruction syntax:
+    MOD[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 MODULUS Op2
+
+**/
+UINT64
+ExecuteMOD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC MODU instruction.
+
+  Instruction syntax:
+    MODU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 UNSIGNED_MODULUS Op2
+
+**/
+UINT64
+ExecuteMODU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC AND instruction.
+
+  Instruction syntax:
+    AND[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 AND Op2
+
+**/
+UINT64
+ExecuteAND (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC OR instruction.
+
+  Instruction syntax:
+    OR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 OR Op2
+
+**/
+UINT64
+ExecuteOR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC XOR instruction.
+
+  Instruction syntax:
+    XOR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 XOR Op2
+
+**/
+UINT64
+ExecuteXOR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC SHL shift left instruction.
+
+  Instruction syntax:
+    SHL[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 << Op2
+
+**/
+UINT64
+ExecuteSHL (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC SHR instruction.
+
+  Instruction syntax:
+    SHR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 >> Op2  (unsigned operands)
+
+**/
+UINT64
+ExecuteSHR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC ASHR instruction.
+
+  Instruction syntax:
+    ASHR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 >> Op2 (signed)
+
+**/
+UINT64
+ExecuteASHR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC EXTNDB instruction to sign-extend a byte value.
+
+  Instruction syntax:
+    EXTNDB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT8)Op2
+
+**/
+UINT64
+ExecuteEXTNDB (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC EXTNDW instruction to sign-extend a 16-bit value.
+
+  Instruction syntax:
+    EXTNDW[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT16)Op2
+
+**/
+UINT64
+ExecuteEXTNDW (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+/**
+  Execute the EBC EXTNDD instruction to sign-extend a 32-bit value.
+
+  Instruction syntax:
+    EXTNDD[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT32)Op2
+
+**/
+UINT64
+ExecuteEXTNDD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  );
+
+//
+// Once we retrieve the operands for the data manipulation instructions,
+// call these functions to perform the operation.
+//
+CONST DATA_MANIP_EXEC_FUNCTION mDataManipDispatchTable[] = {
+  ExecuteNOT,
+  ExecuteNEG,
+  ExecuteADD,
+  ExecuteSUB,
+  ExecuteMUL,
+  ExecuteMULU,
+  ExecuteDIV,
+  ExecuteDIVU,
+  ExecuteMOD,
+  ExecuteMODU,
+  ExecuteAND,
+  ExecuteOR,
+  ExecuteXOR,
+  ExecuteSHL,
+  ExecuteSHR,
+  ExecuteASHR,
+  ExecuteEXTNDB,
+  ExecuteEXTNDW,
+  ExecuteEXTNDD,
+};
+
+CONST VM_TABLE_ENTRY           mVmOpcodeTable[] = {
+  { ExecuteBREAK },             // opcode 0x00
+  { ExecuteJMP },               // opcode 0x01
+  { ExecuteJMP8 },              // opcode 0x02
+  { ExecuteCALL },              // opcode 0x03
+  { ExecuteRET },               // opcode 0x04
+  { ExecuteCMP },               // opcode 0x05 CMPeq
+  { ExecuteCMP },               // opcode 0x06 CMPlte
+  { ExecuteCMP },               // opcode 0x07 CMPgte
+  { ExecuteCMP },               // opcode 0x08 CMPulte
+  { ExecuteCMP },               // opcode 0x09 CMPugte
+  { ExecuteUnsignedDataManip }, // opcode 0x0A NOT
+  { ExecuteSignedDataManip },   // opcode 0x0B NEG
+  { ExecuteSignedDataManip },   // opcode 0x0C ADD
+  { ExecuteSignedDataManip },   // opcode 0x0D SUB
+  { ExecuteSignedDataManip },   // opcode 0x0E MUL
+  { ExecuteUnsignedDataManip }, // opcode 0x0F MULU
+  { ExecuteSignedDataManip },   // opcode 0x10 DIV
+  { ExecuteUnsignedDataManip }, // opcode 0x11 DIVU
+  { ExecuteSignedDataManip },   // opcode 0x12 MOD
+  { ExecuteUnsignedDataManip }, // opcode 0x13 MODU
+  { ExecuteUnsignedDataManip }, // opcode 0x14 AND
+  { ExecuteUnsignedDataManip }, // opcode 0x15 OR
+  { ExecuteUnsignedDataManip }, // opcode 0x16 XOR
+  { ExecuteUnsignedDataManip }, // opcode 0x17 SHL
+  { ExecuteUnsignedDataManip }, // opcode 0x18 SHR
+  { ExecuteSignedDataManip },   // opcode 0x19 ASHR
+  { ExecuteUnsignedDataManip }, // opcode 0x1A EXTNDB
+  { ExecuteUnsignedDataManip }, // opcode 0x1B EXTNDW
+  { ExecuteUnsignedDataManip }, // opcode 0x1C EXTNDD
+  { ExecuteMOVxx },             // opcode 0x1D MOVBW
+  { ExecuteMOVxx },             // opcode 0x1E MOVWW
+  { ExecuteMOVxx },             // opcode 0x1F MOVDW
+  { ExecuteMOVxx },             // opcode 0x20 MOVQW
+  { ExecuteMOVxx },             // opcode 0x21 MOVBD
+  { ExecuteMOVxx },             // opcode 0x22 MOVWD
+  { ExecuteMOVxx },             // opcode 0x23 MOVDD
+  { ExecuteMOVxx },             // opcode 0x24 MOVQD
+  { ExecuteMOVsnw },            // opcode 0x25 MOVsnw
+  { ExecuteMOVsnd },            // opcode 0x26 MOVsnd
+  { NULL },                     // opcode 0x27
+  { ExecuteMOVxx },             // opcode 0x28 MOVqq
+  { ExecuteLOADSP },            // opcode 0x29 LOADSP SP1, R2
+  { ExecuteSTORESP },           // opcode 0x2A STORESP R1, SP2
+  { ExecutePUSH },              // opcode 0x2B PUSH {@}R1 [imm16]
+  { ExecutePOP },               // opcode 0x2C POP {@}R1 [imm16]
+  { ExecuteCMPI },              // opcode 0x2D CMPIEQ
+  { ExecuteCMPI },              // opcode 0x2E CMPILTE
+  { ExecuteCMPI },              // opcode 0x2F CMPIGTE
+  { ExecuteCMPI },              // opcode 0x30 CMPIULTE
+  { ExecuteCMPI },              // opcode 0x31 CMPIUGTE
+  { ExecuteMOVxx },             // opcode 0x32 MOVN
+  { ExecuteMOVxx },             // opcode 0x33 MOVND
+  { NULL },                     // opcode 0x34
+  { ExecutePUSHn },             // opcode 0x35
+  { ExecutePOPn },              // opcode 0x36
+  { ExecuteMOVI },              // opcode 0x37 - mov immediate data
+  { ExecuteMOVIn },             // opcode 0x38 - mov immediate natural
+  { ExecuteMOVREL },            // opcode 0x39 - move data relative to PC
+  { NULL },                     // opcode 0x3a
+  { NULL },                     // opcode 0x3b 
+  { NULL },                     // opcode 0x3c 
+  { NULL },                     // opcode 0x3d 
+  { NULL },                     // opcode 0x3e 
+  { NULL }                      // opcode 0x3f 
+};
+
+//
+// Length of JMP instructions, depending on upper two bits of opcode.
+//
+CONST UINT8                    mJMPLen[] = { 2, 2, 6, 10 };
+
+/**
+  Given a pointer to a new VM context, execute one or more instructions. This
+  function is only used for test purposes via the EBC VM test protocol.
+
+  @param  This              A pointer to the EFI_EBC_VM_TEST_PROTOCOL structure.
+  @param  VmPtr             A pointer to a VM context.
+  @param  InstructionCount  A pointer to a UINTN value holding the number of
+                            instructions to execute. If it holds value of 0,
+                            then the instruction to be executed is 1.
+
+  @retval EFI_UNSUPPORTED   At least one of the opcodes is not supported.
+  @retval EFI_SUCCESS       All of the instructions are executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcExecuteInstructions (
+  IN EFI_EBC_VM_TEST_PROTOCOL *This,
+  IN VM_CONTEXT               *VmPtr,
+  IN OUT UINTN                *InstructionCount
+  )
+{
+  UINTN       ExecFunc;
+  EFI_STATUS  Status;
+  UINTN       InstructionsLeft;
+  UINTN       SavedInstructionCount;
+
+  Status = EFI_SUCCESS;
+
+  if (*InstructionCount == 0) {
+    InstructionsLeft = 1;
+  } else {
+    InstructionsLeft = *InstructionCount;
+  }
+
+  SavedInstructionCount = *InstructionCount;
+  *InstructionCount     = 0;
+
+  //
+  // Index into the opcode table using the opcode byte for this instruction.
+  // This gives you the execute function, which we first test for null, then
+  // call it if it's not null.
+  //
+  while (InstructionsLeft != 0) {
+    ExecFunc = (UINTN) mVmOpcodeTable[(*VmPtr->Ip & OPCODE_M_OPCODE)].ExecuteFunction;
+    if (ExecFunc == (UINTN) NULL) {
+      EbcDebugSignalException (EXCEPT_EBC_INVALID_OPCODE, EXCEPTION_FLAG_FATAL, VmPtr);
+      return EFI_UNSUPPORTED;
+    } else {
+      mVmOpcodeTable[(*VmPtr->Ip & OPCODE_M_OPCODE)].ExecuteFunction (VmPtr);
+      *InstructionCount = *InstructionCount + 1;
+    }
+
+    //
+    // Decrement counter if applicable
+    //
+    if (SavedInstructionCount != 0) {
+      InstructionsLeft--;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Execute an EBC image from an entry point or from a published protocol.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   At least one of the opcodes is not supported.
+  @retval EFI_SUCCESS       All of the instructions are executed successfully.
+
+**/
+EFI_STATUS
+EbcExecute (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINTN                             ExecFunc;
+  UINT8                             StackCorrupted;
+  EFI_STATUS                        Status;
+  EFI_EBC_SIMPLE_DEBUGGER_PROTOCOL  *EbcSimpleDebugger;
+
+  mVmPtr            = VmPtr;
+  EbcSimpleDebugger = NULL;
+  Status            = EFI_SUCCESS;
+  StackCorrupted    = 0;
+
+  //
+  // Make sure the magic value has been put on the stack before we got here.
+  //
+  if (*VmPtr->StackMagicPtr != (UINTN) VM_STACK_KEY_VALUE) {
+    StackCorrupted = 1;
+  }
+
+  VmPtr->FramePtr = (VOID *) ((UINT8 *) (UINTN) VmPtr->Gpr[0] + 8);
+
+  //
+  // Try to get the debug support for EBC
+  //
+  DEBUG_CODE_BEGIN ();
+    Status = gBS->LocateProtocol (
+                    &gEfiEbcSimpleDebuggerProtocolGuid,
+                    NULL,
+                    (VOID **) &EbcSimpleDebugger
+                    );
+    if (EFI_ERROR (Status)) {
+      EbcSimpleDebugger = NULL;
+    }
+  DEBUG_CODE_END ();
+
+  //
+  // Save the start IP for debug. For example, if we take an exception we
+  // can print out the location of the exception relative to the entry point,
+  // which could then be used in a disassembly listing to find the problem.
+  //
+  VmPtr->EntryPoint = (VOID *) VmPtr->Ip;
+
+  //
+  // We'll wait for this flag to know when we're done. The RET
+  // instruction sets it if it runs out of stack.
+  //
+  VmPtr->StopFlags = 0;
+  while ((VmPtr->StopFlags & STOPFLAG_APP_DONE) == 0) {
+    //
+    // If we've found a simple debugger protocol, call it
+    //
+    DEBUG_CODE_BEGIN ();
+      if (EbcSimpleDebugger != NULL) {
+        EbcSimpleDebugger->Debugger (EbcSimpleDebugger, VmPtr);
+      }
+    DEBUG_CODE_END ();
+
+    //
+    // Use the opcode bits to index into the opcode dispatch table. If the
+    // function pointer is null then generate an exception.
+    //
+    ExecFunc = (UINTN) mVmOpcodeTable[(*VmPtr->Ip & OPCODE_M_OPCODE)].ExecuteFunction;
+    if (ExecFunc == (UINTN) NULL) {
+      EbcDebugSignalException (EXCEPT_EBC_INVALID_OPCODE, EXCEPTION_FLAG_FATAL, VmPtr);
+      Status = EFI_UNSUPPORTED;
+      goto Done;
+    }
+    //
+    // The EBC VM is a strongly ordered processor, so perform a fence operation before
+    // and after each instruction is executed.
+    //
+    MemoryFence ();
+
+    mVmOpcodeTable[(*VmPtr->Ip & OPCODE_M_OPCODE)].ExecuteFunction (VmPtr);
+
+    MemoryFence ();
+
+    //
+    // If the step flag is set, signal an exception and continue. We don't
+    // clear it here. Assuming the debugger is responsible for clearing it.
+    //
+    if (VMFLAG_ISSET (VmPtr, VMFLAGS_STEP)) {
+      EbcDebugSignalException (EXCEPT_EBC_STEP, EXCEPTION_FLAG_NONE, VmPtr);
+    }
+    //
+    // Make sure stack has not been corrupted. Only report it once though.
+    //
+    if ((StackCorrupted == 0) && (*VmPtr->StackMagicPtr != (UINTN) VM_STACK_KEY_VALUE)) {
+      EbcDebugSignalException (EXCEPT_EBC_STACK_FAULT, EXCEPTION_FLAG_FATAL, VmPtr);
+      StackCorrupted = 1;
+    }
+    if ((StackCorrupted == 0) && ((UINT64)VmPtr->Gpr[0] <= (UINT64)(UINTN) VmPtr->StackTop)) {
+      EbcDebugSignalException (EXCEPT_EBC_STACK_FAULT, EXCEPTION_FLAG_FATAL, VmPtr);
+      StackCorrupted = 1;
+    }
+  }
+
+Done:
+  mVmPtr          = NULL;
+
+  return Status;
+}
+
+
+/**
+  Execute the MOVxx instructions.
+
+  Instruction format:
+
+    MOV[b|w|d|q|n]{w|d} {@}R1 {Index16|32}, {@}R2 {Index16|32}
+    MOVqq {@}R1 {Index64}, {@}R2 {Index64}
+
+    Copies contents of [R2] -> [R1], zero extending where required.
+
+    First character indicates the size of the move.
+    Second character indicates the size of the index(s).
+
+    Invalid to have R1 direct with index.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVxx (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   OpcMasked;
+  UINT8   Operands;
+  UINT8   Size;
+  UINT8   MoveSize;
+  INT16   Index16;
+  INT32   Index32;
+  INT64   Index64Op1;
+  INT64   Index64Op2;
+  UINT64  Data64;
+  UINT64  DataMask;
+  UINTN   Source;
+
+  Opcode    = GETOPCODE (VmPtr);
+  OpcMasked = (UINT8) (Opcode & OPCODE_M_OPCODE);
+
+  //
+  // Get the operands byte so we can get R1 and R2
+  //
+  Operands = GETOPERANDS (VmPtr);
+
+  //
+  // Assume no indexes
+  //
+  Index64Op1  = 0;
+  Index64Op2  = 0;
+  Data64      = 0;
+
+  //
+  // Determine if we have an index/immediate data. Base instruction size
+  // is 2 (opcode + operands). Add to this size each index specified.
+  //
+  Size = 2;
+  if ((Opcode & (OPCODE_M_IMMED_OP1 | OPCODE_M_IMMED_OP2)) != 0) {
+    //
+    // Determine size of the index from the opcode. Then get it.
+    //
+    if ((OpcMasked <= OPCODE_MOVQW) || (OpcMasked == OPCODE_MOVNW)) {
+      //
+      // MOVBW, MOVWW, MOVDW, MOVQW, and MOVNW have 16-bit immediate index.
+      // Get one or both index values.
+      //
+      if ((Opcode & OPCODE_M_IMMED_OP1) != 0) {
+        Index16     = VmReadIndex16 (VmPtr, 2);
+        Index64Op1  = (INT64) Index16;
+        Size += sizeof (UINT16);
+      }
+
+      if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+        Index16     = VmReadIndex16 (VmPtr, Size);
+        Index64Op2  = (INT64) Index16;
+        Size += sizeof (UINT16);
+      }
+    } else if ((OpcMasked <= OPCODE_MOVQD) || (OpcMasked == OPCODE_MOVND)) {
+      //
+      // MOVBD, MOVWD, MOVDD, MOVQD, and MOVND have 32-bit immediate index
+      //
+      if ((Opcode & OPCODE_M_IMMED_OP1) != 0) {
+        Index32     = VmReadIndex32 (VmPtr, 2);
+        Index64Op1  = (INT64) Index32;
+        Size += sizeof (UINT32);
+      }
+
+      if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+        Index32     = VmReadIndex32 (VmPtr, Size);
+        Index64Op2  = (INT64) Index32;
+        Size += sizeof (UINT32);
+      }
+    } else if (OpcMasked == OPCODE_MOVQQ) {
+      //
+      // MOVqq -- only form with a 64-bit index
+      //
+      if ((Opcode & OPCODE_M_IMMED_OP1) != 0) {
+        Index64Op1 = VmReadIndex64 (VmPtr, 2);
+        Size += sizeof (UINT64);
+      }
+
+      if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+        Index64Op2 = VmReadIndex64 (VmPtr, Size);
+        Size += sizeof (UINT64);
+      }
+    } else {
+      //
+      // Obsolete MOVBQ, MOVWQ, MOVDQ, and MOVNQ have 64-bit immediate index
+      //
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+  }
+  //
+  // Determine the size of the move, and create a mask for it so we can
+  // clear unused bits.
+  //
+  if ((OpcMasked == OPCODE_MOVBW) || (OpcMasked == OPCODE_MOVBD)) {
+    MoveSize  = DATA_SIZE_8;
+    DataMask  = 0xFF;
+  } else if ((OpcMasked == OPCODE_MOVWW) || (OpcMasked == OPCODE_MOVWD)) {
+    MoveSize  = DATA_SIZE_16;
+    DataMask  = 0xFFFF;
+  } else if ((OpcMasked == OPCODE_MOVDW) || (OpcMasked == OPCODE_MOVDD)) {
+    MoveSize  = DATA_SIZE_32;
+    DataMask  = 0xFFFFFFFF;
+  } else if ((OpcMasked == OPCODE_MOVQW) || (OpcMasked == OPCODE_MOVQD) || (OpcMasked == OPCODE_MOVQQ)) {
+    MoveSize  = DATA_SIZE_64;
+    DataMask  = (UINT64)~0;
+  } else if ((OpcMasked == OPCODE_MOVNW) || (OpcMasked == OPCODE_MOVND)) {
+    MoveSize  = DATA_SIZE_N;
+    DataMask  = (UINT64)~0 >> (64 - 8 * sizeof (UINTN));
+  } else {
+    //
+    // We were dispatched to this function and we don't recognize the opcode
+    //
+    EbcDebugSignalException (EXCEPT_EBC_UNDEFINED, EXCEPTION_FLAG_FATAL, VmPtr);
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Now get the source address
+  //
+  if (OPERAND2_INDIRECT (Operands)) {
+    //
+    // Indirect form @R2. Compute address of operand2
+    //
+    Source = (UINTN) (VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index64Op2);
+    //
+    // Now get the data from the source. Always 0-extend and let the compiler
+    // sign-extend where required.
+    //
+    switch (MoveSize) {
+    case DATA_SIZE_8:
+      Data64 = (UINT64) (UINT8) VmReadMem8 (VmPtr, Source);
+      break;
+
+    case DATA_SIZE_16:
+      Data64 = (UINT64) (UINT16) VmReadMem16 (VmPtr, Source);
+      break;
+
+    case DATA_SIZE_32:
+      Data64 = (UINT64) (UINT32) VmReadMem32 (VmPtr, Source);
+      break;
+
+    case DATA_SIZE_64:
+      Data64 = (UINT64) VmReadMem64 (VmPtr, Source);
+      break;
+
+    case DATA_SIZE_N:
+      Data64 = (UINT64) (UINTN) VmReadMemN (VmPtr, Source);
+      break;
+
+    default:
+      //
+      // not reached
+      //
+      break;
+    }
+  } else {
+    //
+    // Not indirect source: MOVxx {@}Rx, Ry [Index]
+    //
+    Data64 = (UINT64) (VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index64Op2);
+    //
+    // Did Operand2 have an index? If so, treat as two signed values since
+    // indexes are signed values.
+    //
+    if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+      //
+      // NOTE: need to find a way to fix this, most likely by changing the VM
+      // implementation to remove the stack gap. To do that, we'd need to
+      // allocate stack space for the VM and actually set the system
+      // stack pointer to the allocated buffer when the VM starts.
+      //
+      // Special case -- if someone took the address of a function parameter
+      // then we need to make sure it's not in the stack gap. We can identify
+      // this situation if (Operand2 register == 0) && (Operand2 is direct)
+      // && (Index applies to Operand2) && (Index > 0) && (Operand1 register != 0)
+      // Situations that to be aware of:
+      //   * stack adjustments at beginning and end of functions R0 = R0 += stacksize
+      //
+      if ((OPERAND2_REGNUM (Operands) == 0) &&
+          (!OPERAND2_INDIRECT (Operands)) &&
+          (Index64Op2 > 0) &&
+          (OPERAND1_REGNUM (Operands) == 0) &&
+          (OPERAND1_INDIRECT (Operands))
+          ) {
+        Data64 = (UINT64) ConvertStackAddr (VmPtr, (UINTN) (INT64) Data64);
+      }
+    }
+  }
+  //
+  // Now write it back
+  //
+  if (OPERAND1_INDIRECT (Operands)) {
+    //
+    // Reuse the Source variable to now be dest.
+    //
+    Source = (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index64Op1);
+    //
+    // Do the write based on the size
+    //
+    switch (MoveSize) {
+    case DATA_SIZE_8:
+      VmWriteMem8 (VmPtr, Source, (UINT8) Data64);
+      break;
+
+    case DATA_SIZE_16:
+      VmWriteMem16 (VmPtr, Source, (UINT16) Data64);
+      break;
+
+    case DATA_SIZE_32:
+      VmWriteMem32 (VmPtr, Source, (UINT32) Data64);
+      break;
+
+    case DATA_SIZE_64:
+      VmWriteMem64 (VmPtr, Source, Data64);
+      break;
+
+    case DATA_SIZE_N:
+      VmWriteMemN (VmPtr, Source, (UINTN) Data64);
+      break;
+
+    default:
+      //
+      // not reached
+      //
+      break;
+    }
+  } else {
+    //
+    // Operand1 direct.
+    // Make sure we didn't have an index on operand1.
+    //
+    if ((Opcode & OPCODE_M_IMMED_OP1) != 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Direct storage in register. Clear unused bits and store back to
+    // register.
+    //
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = Data64 & DataMask;
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC BREAK instruction.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteBREAK (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       Operands;
+  VOID        *EbcEntryPoint;
+  VOID        *Thunk;
+  UINT64      U64EbcEntryPoint;
+  INT32       Offset;
+
+  Thunk = NULL;
+  Operands = GETOPERANDS (VmPtr);
+  switch (Operands) {
+  //
+  // Runaway program break. Generate an exception and terminate
+  //
+  case 0:
+    EbcDebugSignalException (EXCEPT_EBC_BAD_BREAK, EXCEPTION_FLAG_FATAL, VmPtr);
+    break;
+
+  //
+  // Get VM version -- return VM revision number in R7
+  //
+  case 1:
+    //
+    // Bits:
+    //  63-17 = 0
+    //  16-8  = Major version
+    //  7-0   = Minor version
+    //
+    VmPtr->Gpr[7] = GetVmVersion ();
+    break;
+
+  //
+  // Debugger breakpoint
+  //
+  case 3:
+    VmPtr->StopFlags |= STOPFLAG_BREAKPOINT;
+    //
+    // See if someone has registered a handler
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_BREAKPOINT,
+      EXCEPTION_FLAG_NONE,
+      VmPtr
+      );
+    break;
+
+  //
+  // System call, which there are none, so NOP it.
+  //
+  case 4:
+    break;
+
+  //
+  // Create a thunk for EBC code. R7 points to a 32-bit (in a 64-bit slot)
+  // "offset from self" pointer to the EBC entry point.
+  // After we're done, *(UINT64 *)R7 will be the address of the new thunk.
+  //
+  case 5:
+    Offset            = (INT32) VmReadMem32 (VmPtr, (UINTN) VmPtr->Gpr[7]);
+    U64EbcEntryPoint  = (UINT64) (VmPtr->Gpr[7] + Offset + 4);
+    EbcEntryPoint     = (VOID *) (UINTN) U64EbcEntryPoint;
+
+    //
+    // Now create a new thunk
+    //
+    Status = EbcCreateThunks (VmPtr->ImageHandle, EbcEntryPoint, &Thunk, 0);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Finally replace the EBC entry point memory with the thunk address
+    //
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[7], (UINT64) (UINTN) Thunk);
+    break;
+
+  //
+  // Compiler setting version per value in R7
+  //
+  case 6:
+    VmPtr->CompilerVersion = (UINT32) VmPtr->Gpr[7];
+    //
+    // Check compiler version against VM version?
+    //
+    break;
+
+  //
+  // Unhandled break code. Signal exception.
+  //
+  default:
+    EbcDebugSignalException (EXCEPT_EBC_BAD_BREAK, EXCEPTION_FLAG_FATAL, VmPtr);
+    break;
+  }
+  //
+  // Advance IP
+  //
+  VmPtr->Ip += 2;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the JMP instruction.
+
+  Instruction syntax:
+    JMP64{cs|cc} Immed64
+    JMP32{cs|cc} {@}R1 {Immed32|Index32}
+
+  Encoding:
+    b0.7 -  immediate data present
+    b0.6 -  1 = 64 bit immediate data
+            0 = 32 bit immediate data
+    b1.7 -  1 = conditional
+    b1.6    1 = CS (condition set)
+            0 = CC (condition clear)
+    b1.4    1 = relative address
+            0 = absolute address
+    b1.3    1 = operand1 indirect
+    b1.2-0  operand 1
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteJMP (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   CompareSet;
+  UINT8   ConditionFlag;
+  UINT8   Size;
+  UINT8   Operand;
+  UINT64  Data64;
+  INT32   Index32;
+  UINTN   Addr;
+
+  Operand = GETOPERANDS (VmPtr);
+  Opcode  = GETOPCODE (VmPtr);
+
+  //
+  // Get instruction length from the opcode. The upper two bits are used here
+  // to index into the length array.
+  //
+  Size = mJMPLen[(Opcode >> 6) & 0x03];
+
+  //
+  // Decode instruction conditions
+  // If we haven't met the condition, then simply advance the IP and return.
+  //
+  CompareSet    = (UINT8) (((Operand & JMP_M_CS) != 0) ? 1 : 0);
+  ConditionFlag = (UINT8) VMFLAG_ISSET (VmPtr, VMFLAGS_CC);
+  if ((Operand & CONDITION_M_CONDITIONAL) != 0) {
+    if (CompareSet != ConditionFlag) {
+      VmPtr->Ip += Size;
+      return EFI_SUCCESS;
+    }
+  }
+  //
+  // Check for 64-bit form and do it right away since it's the most
+  // straight-forward form.
+  //
+  if ((Opcode & OPCODE_M_IMMDATA64) != 0) {
+    //
+    // Double check for immediate-data, which is required. If not there,
+    // then signal an exception
+    //
+    if ((Opcode & OPCODE_M_IMMDATA) == 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_ERROR,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // 64-bit immediate data is full address. Read the immediate data,
+    // check for alignment, and jump absolute.
+    //
+    Data64 = (UINT64) VmReadImmed64 (VmPtr, 2);
+    if (!IS_ALIGNED ((UINTN) Data64, sizeof (UINT16))) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_ALIGNMENT_CHECK,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+
+      return EFI_UNSUPPORTED;
+    }
+
+    //
+    // Take jump -- relative or absolute
+    //
+    if ((Operand & JMP_M_RELATIVE) != 0) {
+      VmPtr->Ip += (UINTN) Data64 + Size;
+    } else {
+      VmPtr->Ip = (VMIP) (UINTN) Data64;
+    }
+
+    return EFI_SUCCESS;
+  }
+  //
+  // 32-bit forms:
+  // Get the index if there is one. May be either an index, or an immediate
+  // offset depending on indirect operand.
+  //   JMP32 @R1 Index32 -- immediate data is an index
+  //   JMP32 R1 Immed32  -- immedate data is an offset
+  //
+  if ((Opcode & OPCODE_M_IMMDATA) != 0) {
+    if (OPERAND1_INDIRECT (Operand)) {
+      Index32 = VmReadIndex32 (VmPtr, 2);
+    } else {
+      Index32 = VmReadImmed32 (VmPtr, 2);
+    }
+  } else {
+    Index32 = 0;
+  }
+  //
+  // Get the register data. If R == 0, then special case where it's ignored.
+  //
+  if (OPERAND1_REGNUM (Operand) == 0) {
+    Data64 = 0;
+  } else {
+    Data64 = (UINT64) OPERAND1_REGDATA (VmPtr, Operand);
+  }
+  //
+  // Decode the forms
+  //
+  if (OPERAND1_INDIRECT (Operand)) {
+    //
+    // Form: JMP32 @Rx {Index32}
+    //
+    Addr = VmReadMemN (VmPtr, (UINTN) Data64 + Index32);
+    if (!IS_ALIGNED ((UINTN) Addr, sizeof (UINT16))) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_ALIGNMENT_CHECK,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+
+      return EFI_UNSUPPORTED;
+    }
+
+    if ((Operand & JMP_M_RELATIVE) != 0) {
+      VmPtr->Ip += (UINTN) Addr + Size;
+    } else {
+      VmPtr->Ip = (VMIP) Addr;
+    }
+  } else {
+    //
+    // Form: JMP32 Rx {Immed32}
+    //
+    Addr = (UINTN) (Data64 + Index32);
+    if (!IS_ALIGNED ((UINTN) Addr, sizeof (UINT16))) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_ALIGNMENT_CHECK,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+
+      return EFI_UNSUPPORTED;
+    }
+
+    if ((Operand & JMP_M_RELATIVE) != 0) {
+      VmPtr->Ip += (UINTN) Addr + Size;
+    } else {
+      VmPtr->Ip = (VMIP) Addr;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC JMP8 instruction.
+
+  Instruction syntax:
+    JMP8{cs|cc}  Offset/2
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteJMP8 (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Opcode;
+  UINT8 ConditionFlag;
+  UINT8 CompareSet;
+  INT8  Offset;
+
+  //
+  // Decode instruction.
+  //
+  Opcode        = GETOPCODE (VmPtr);
+  CompareSet    = (UINT8) (((Opcode & JMP_M_CS) != 0) ? 1 : 0);
+  ConditionFlag = (UINT8) VMFLAG_ISSET (VmPtr, VMFLAGS_CC);
+
+  //
+  // If we haven't met the condition, then simply advance the IP and return
+  //
+  if ((Opcode & CONDITION_M_CONDITIONAL) != 0) {
+    if (CompareSet != ConditionFlag) {
+      VmPtr->Ip += 2;
+      return EFI_SUCCESS;
+    }
+  }
+  //
+  // Get the offset from the instruction stream. It's relative to the
+  // following instruction, and divided by 2.
+  //
+  Offset = VmReadImmed8 (VmPtr, 1);
+  //
+  // Want to check for offset == -2 and then raise an exception?
+  //
+  VmPtr->Ip += (Offset * 2) + 2;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC MOVI.
+
+  Instruction syntax:
+
+    MOVI[b|w|d|q][w|d|q] {@}R1 {Index16}, ImmData16|32|64
+
+    First variable character specifies the move size
+    Second variable character specifies size of the immediate data
+
+    Sign-extend the immediate data to the size of the operation, and zero-extend
+    if storing to a register.
+
+    Operand1 direct with index/immed is invalid.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVI (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT16   Index16;
+  INT64   ImmData64;
+  UINT64  Op1;
+  UINT64  Mask64;
+
+  //
+  // Get the opcode and operands byte so we can get R1 and R2
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Get the index (16-bit) if present
+  //
+  if ((Operands & MOVI_M_IMMDATA) != 0) {
+    Index16 = VmReadIndex16 (VmPtr, 2);
+    Size    = 4;
+  } else {
+    Index16 = 0;
+    Size    = 2;
+  }
+  //
+  // Extract the immediate data. Sign-extend always.
+  //
+  if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH16) {
+    ImmData64 = (INT64) (INT16) VmReadImmed16 (VmPtr, Size);
+    Size += 2;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH32) {
+    ImmData64 = (INT64) (INT32) VmReadImmed32 (VmPtr, Size);
+    Size += 4;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH64) {
+    ImmData64 = (INT64) VmReadImmed64 (VmPtr, Size);
+    Size += 8;
+  } else {
+    //
+    // Invalid encoding
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_INSTRUCTION_ENCODING,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Now write back the result
+  //
+  if (!OPERAND1_INDIRECT (Operands)) {
+    //
+    // Operand1 direct. Make sure it didn't have an index.
+    //
+    if ((Operands & MOVI_M_IMMDATA) != 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Writing directly to a register. Clear unused bits.
+    //
+    if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH8) {
+      Mask64 = 0x000000FF;
+    } else if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH16) {
+      Mask64 = 0x0000FFFF;
+    } else if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH32) {
+      Mask64 = 0x00000000FFFFFFFF;
+    } else {
+      Mask64 = (UINT64)~0;
+    }
+
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = ImmData64 & Mask64;
+  } else {
+    //
+    // Get the address then write back based on size of the move
+    //
+    Op1 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16;
+    if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH8) {
+      VmWriteMem8 (VmPtr, (UINTN) Op1, (UINT8) ImmData64);
+    } else if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH16) {
+      VmWriteMem16 (VmPtr, (UINTN) Op1, (UINT16) ImmData64);
+    } else if ((Operands & MOVI_M_MOVEWIDTH) == MOVI_MOVEWIDTH32) {
+      VmWriteMem32 (VmPtr, (UINTN) Op1, (UINT32) ImmData64);
+    } else {
+      VmWriteMem64 (VmPtr, (UINTN) Op1, (UINT64) ImmData64);
+    }
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC MOV immediate natural. This instruction moves an immediate
+  index value into a register or memory location.
+
+  Instruction syntax:
+
+    MOVIn[w|d|q] {@}R1 {Index16}, Index16|32|64
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVIn (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT16   Index16;
+  INT16   ImmedIndex16;
+  INT32   ImmedIndex32;
+  INT64   ImmedIndex64;
+  UINT64  Op1;
+
+  //
+  // Get the opcode and operands byte so we can get R1 and R2
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Get the operand1 index (16-bit) if present
+  //
+  if ((Operands & MOVI_M_IMMDATA) != 0) {
+    Index16 = VmReadIndex16 (VmPtr, 2);
+    Size    = 4;
+  } else {
+    Index16 = 0;
+    Size    = 2;
+  }
+  //
+  // Extract the immediate data and convert to a 64-bit index.
+  //
+  if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH16) {
+    ImmedIndex16  = VmReadIndex16 (VmPtr, Size);
+    ImmedIndex64  = (INT64) ImmedIndex16;
+    Size += 2;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH32) {
+    ImmedIndex32  = VmReadIndex32 (VmPtr, Size);
+    ImmedIndex64  = (INT64) ImmedIndex32;
+    Size += 4;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH64) {
+    ImmedIndex64 = VmReadIndex64 (VmPtr, Size);
+    Size += 8;
+  } else {
+    //
+    // Invalid encoding
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_INSTRUCTION_ENCODING,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Now write back the result
+  //
+  if (!OPERAND1_INDIRECT (Operands)) {
+    //
+    // Check for MOVIn R1 Index16, Immed (not indirect, with index), which
+    // is illegal
+    //
+    if ((Operands & MOVI_M_IMMDATA) != 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = ImmedIndex64;
+  } else {
+    //
+    // Get the address
+    //
+    Op1 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16;
+    VmWriteMemN (VmPtr, (UINTN) Op1, (UINTN)(INTN) ImmedIndex64);
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC MOVREL instruction.
+  Dest <- Ip + ImmData
+
+  Instruction syntax:
+
+    MOVREL[w|d|q] {@}R1 {Index16}, ImmData16|32|64
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVREL (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT16   Index16;
+  INT64   ImmData64;
+  UINT64  Op1;
+  UINT64  Op2;
+
+  //
+  // Get the opcode and operands byte so we can get R1 and R2
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Get the Operand 1 index (16-bit) if present
+  //
+  if ((Operands & MOVI_M_IMMDATA) != 0) {
+    Index16 = VmReadIndex16 (VmPtr, 2);
+    Size    = 4;
+  } else {
+    Index16 = 0;
+    Size    = 2;
+  }
+  //
+  // Get the immediate data.
+  //
+  if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH16) {
+    ImmData64 = (INT64) VmReadImmed16 (VmPtr, Size);
+    Size += 2;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH32) {
+    ImmData64 = (INT64) VmReadImmed32 (VmPtr, Size);
+    Size += 4;
+  } else if ((Opcode & MOVI_M_DATAWIDTH) == MOVI_DATAWIDTH64) {
+    ImmData64 = VmReadImmed64 (VmPtr, Size);
+    Size += 8;
+  } else {
+    //
+    // Invalid encoding
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_INSTRUCTION_ENCODING,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Compute the value and write back the result
+  //
+  Op2 = (UINT64) ((INT64) ((UINT64) (UINTN) VmPtr->Ip) + (INT64) ImmData64 + Size);
+  if (!OPERAND1_INDIRECT (Operands)) {
+    //
+    // Check for illegal combination of operand1 direct with immediate data
+    //
+    if ((Operands & MOVI_M_IMMDATA) != 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = (VM_REGISTER) Op2;
+  } else {
+    //
+    // Get the address = [Rx] + Index16
+    // Write back the result. Always a natural size write, since
+    // we're talking addresses here.
+    //
+    Op1 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16;
+    VmWriteMemN (VmPtr, (UINTN) Op1, (UINTN) Op2);
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC MOVsnw instruction. This instruction loads a signed
+  natural value from memory or register to another memory or register. On
+  32-bit machines, the value gets sign-extended to 64 bits if the destination
+  is a register.
+
+  Instruction syntax:
+
+    MOVsnw {@}R1 {Index16}, {@}R2 {Index16|Immed16}
+
+    0:7 1=>operand1 index present
+    0:6 1=>operand2 index present
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVsnw (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT16   Op1Index;
+  INT16   Op2Index;
+  UINT64  Op2;
+
+  //
+  // Get the opcode and operand bytes
+  //
+  Opcode              = GETOPCODE (VmPtr);
+  Operands            = GETOPERANDS (VmPtr);
+
+  Op1Index            = Op2Index = 0;
+
+  //
+  // Get the indexes if present.
+  //
+  Size = 2;
+  if ((Opcode & OPCODE_M_IMMED_OP1) !=0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Op1Index = VmReadIndex16 (VmPtr, 2);
+    } else {
+      //
+      // Illegal form operand1 direct with index:  MOVsnw R1 Index16, {@}R2
+      //
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+
+    Size += sizeof (UINT16);
+  }
+
+  if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+    if (OPERAND2_INDIRECT (Operands)) {
+      Op2Index = VmReadIndex16 (VmPtr, Size);
+    } else {
+      Op2Index = VmReadImmed16 (VmPtr, Size);
+    }
+
+    Size += sizeof (UINT16);
+  }
+  //
+  // Get the data from the source.
+  //
+  Op2 = (UINT64)(INT64)(INTN)(VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Op2Index);
+  if (OPERAND2_INDIRECT (Operands)) {
+    Op2 = (UINT64)(INT64)(INTN)VmReadMemN (VmPtr, (UINTN) Op2);
+  }
+  //
+  // Now write back the result.
+  //
+  if (!OPERAND1_INDIRECT (Operands)) {
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = Op2;
+  } else {
+    VmWriteMemN (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Op1Index), (UINTN) Op2);
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC MOVsnw instruction. This instruction loads a signed
+  natural value from memory or register to another memory or register. On
+  32-bit machines, the value gets sign-extended to 64 bits if the destination
+  is a register.
+
+  Instruction syntax:
+
+    MOVsnd {@}R1 {Indx32}, {@}R2 {Index32|Immed32}
+
+    0:7 1=>operand1 index present
+    0:6 1=>operand2 index present
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteMOVsnd (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT32   Op1Index;
+  INT32   Op2Index;
+  UINT64  Op2;
+
+  //
+  // Get the opcode and operand bytes
+  //
+  Opcode              = GETOPCODE (VmPtr);
+  Operands            = GETOPERANDS (VmPtr);
+
+  Op1Index            = Op2Index = 0;
+
+  //
+  // Get the indexes if present.
+  //
+  Size = 2;
+  if ((Opcode & OPCODE_M_IMMED_OP1) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Op1Index = VmReadIndex32 (VmPtr, 2);
+    } else {
+      //
+      // Illegal form operand1 direct with index:  MOVsnd R1 Index16,..
+      //
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+      return EFI_UNSUPPORTED;
+    }
+
+    Size += sizeof (UINT32);
+  }
+
+  if ((Opcode & OPCODE_M_IMMED_OP2) != 0) {
+    if (OPERAND2_INDIRECT (Operands)) {
+      Op2Index = VmReadIndex32 (VmPtr, Size);
+    } else {
+      Op2Index = VmReadImmed32 (VmPtr, Size);
+    }
+
+    Size += sizeof (UINT32);
+  }
+  //
+  // Get the data from the source.
+  //
+  Op2 = (UINT64)(INT64)(INTN)(INT64)(VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Op2Index);
+  if (OPERAND2_INDIRECT (Operands)) {
+    Op2 = (UINT64)(INT64)(INTN)(INT64)VmReadMemN (VmPtr, (UINTN) Op2);
+  }
+  //
+  // Now write back the result.
+  //
+  if (!OPERAND1_INDIRECT (Operands)) {
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = Op2;
+  } else {
+    VmWriteMemN (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Op1Index), (UINTN) Op2);
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC PUSHn instruction
+
+  Instruction syntax:
+    PUSHn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePUSHn (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Opcode;
+  UINT8 Operands;
+  INT16 Index16;
+  UINTN DataN;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Get index if present
+  //
+  if ((Opcode & PUSHPOP_M_IMMDATA) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    VmPtr->Ip += 4;
+  } else {
+    Index16 = 0;
+    VmPtr->Ip += 2;
+  }
+  //
+  // Get the data to push
+  //
+  if (OPERAND1_INDIRECT (Operands)) {
+    DataN = VmReadMemN (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16));
+  } else {
+    DataN = (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16);
+  }
+  //
+  // Adjust the stack down.
+  //
+  VmPtr->Gpr[0] -= sizeof (UINTN);
+  VmWriteMemN (VmPtr, (UINTN) VmPtr->Gpr[0], DataN);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC PUSH instruction.
+
+  Instruction syntax:
+    PUSH[32|64] {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePUSH (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT32  Data32;
+  UINT64  Data64;
+  INT16   Index16;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+  //
+  // Get immediate index if present, then advance the IP.
+  //
+  if ((Opcode & PUSHPOP_M_IMMDATA) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    VmPtr->Ip += 4;
+  } else {
+    Index16 = 0;
+    VmPtr->Ip += 2;
+  }
+  //
+  // Get the data to push
+  //
+  if ((Opcode & PUSHPOP_M_64) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Data64 = VmReadMem64 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16));
+    } else {
+      Data64 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16;
+    }
+    //
+    // Adjust the stack down, then write back the data
+    //
+    VmPtr->Gpr[0] -= sizeof (UINT64);
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[0], Data64);
+  } else {
+    //
+    // 32-bit data
+    //
+    if (OPERAND1_INDIRECT (Operands)) {
+      Data32 = VmReadMem32 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16));
+    } else {
+      Data32 = (UINT32) VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16;
+    }
+    //
+    // Adjust the stack down and write the data
+    //
+    VmPtr->Gpr[0] -= sizeof (UINT32);
+    VmWriteMem32 (VmPtr, (UINTN) VmPtr->Gpr[0], Data32);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC POPn instruction.
+
+  Instruction syntax:
+    POPn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePOPn (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Opcode;
+  UINT8 Operands;
+  INT16 Index16;
+  UINTN DataN;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+  //
+  // Get immediate data if present, and advance the IP
+  //
+  if ((Opcode & PUSHPOP_M_IMMDATA) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    VmPtr->Ip += 4;
+  } else {
+    Index16 = 0;
+    VmPtr->Ip += 2;
+  }
+  //
+  // Read the data off the stack, then adjust the stack pointer
+  //
+  DataN = VmReadMemN (VmPtr, (UINTN) VmPtr->Gpr[0]);
+  VmPtr->Gpr[0] += sizeof (UINTN);
+  //
+  // Do the write-back
+  //
+  if (OPERAND1_INDIRECT (Operands)) {
+    VmWriteMemN (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16), DataN);
+  } else {
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = (INT64) (UINT64) ((UINTN) DataN + Index16);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC POP instruction.
+
+  Instruction syntax:
+    POPn {@}R1 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecutePOP (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  INT16   Index16;
+  INT32   Data32;
+  UINT64  Data64;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+  //
+  // Get immediate data if present, and advance the IP.
+  //
+  if ((Opcode & PUSHPOP_M_IMMDATA) != 0) {
+    if (OPERAND1_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    VmPtr->Ip += 4;
+  } else {
+    Index16 = 0;
+    VmPtr->Ip += 2;
+  }
+  //
+  // Get the data off the stack, then write it to the appropriate location
+  //
+  if ((Opcode & PUSHPOP_M_64) != 0) {
+    //
+    // Read the data off the stack, then adjust the stack pointer
+    //
+    Data64 = VmReadMem64 (VmPtr, (UINTN) VmPtr->Gpr[0]);
+    VmPtr->Gpr[0] += sizeof (UINT64);
+    //
+    // Do the write-back
+    //
+    if (OPERAND1_INDIRECT (Operands)) {
+      VmWriteMem64 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16), Data64);
+    } else {
+      VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = Data64 + Index16;
+    }
+  } else {
+    //
+    // 32-bit pop. Read it off the stack and adjust the stack pointer
+    //
+    Data32 = (INT32) VmReadMem32 (VmPtr, (UINTN) VmPtr->Gpr[0]);
+    VmPtr->Gpr[0] += sizeof (UINT32);
+    //
+    // Do the write-back
+    //
+    if (OPERAND1_INDIRECT (Operands)) {
+      VmWriteMem32 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND1_REGNUM (Operands)] + Index16), Data32);
+    } else {
+      VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = (INT64) Data32 + Index16;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Implements the EBC CALL instruction.
+
+  Instruction format:
+    CALL64 Immed64
+    CALL32 {@}R1 {Immed32|Index32}
+    CALLEX64 Immed64
+    CALLEX16 {@}R1 {Immed32}
+
+    If Rx == R0, then it's a PC relative call to PC = PC + imm32.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCALL (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Opcode;
+  UINT8 Operands;
+  INT32 Immed32;
+  UINT8 Size;
+  INT64 Immed64;
+  VOID  *FramePtr;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+  //
+  // Assign these as well to avoid compiler warnings
+  //
+  Immed64   = 0;
+  Immed32   = 0;
+
+  FramePtr  = VmPtr->FramePtr;
+  //
+  // Determine the instruction size, and get immediate data if present
+  //
+  if ((Opcode & OPCODE_M_IMMDATA) != 0) {
+    if ((Opcode & OPCODE_M_IMMDATA64) != 0) {
+      Immed64 = VmReadImmed64 (VmPtr, 2);
+      Size    = 10;
+    } else {
+      //
+      // If register operand is indirect, then the immediate data is an index
+      //
+      if (OPERAND1_INDIRECT (Operands)) {
+        Immed32 = VmReadIndex32 (VmPtr, 2);
+      } else {
+        Immed32 = VmReadImmed32 (VmPtr, 2);
+      }
+
+      Size = 6;
+    }
+  } else {
+    Size = 2;
+  }
+  //
+  // If it's a call to EBC, adjust the stack pointer down 16 bytes and
+  // put our return address and frame pointer on the VM stack.
+  //
+  if ((Operands & OPERAND_M_NATIVE_CALL) == 0) {
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMemN (VmPtr, (UINTN) VmPtr->Gpr[0], (UINTN) FramePtr);
+    VmPtr->FramePtr = (VOID *) (UINTN) VmPtr->Gpr[0];
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[0], (UINT64) (UINTN) (VmPtr->Ip + Size));
+  }
+  //
+  // If 64-bit data, then absolute jump only
+  //
+  if ((Opcode & OPCODE_M_IMMDATA64) != 0) {
+    //
+    // Native or EBC call?
+    //
+    if ((Operands & OPERAND_M_NATIVE_CALL) == 0) {
+      VmPtr->Ip = (VMIP) (UINTN) Immed64;
+    } else {
+      //
+      // Call external function, get the return value, and advance the IP
+      //
+      EbcLLCALLEX (VmPtr, (UINTN) Immed64, (UINTN) VmPtr->Gpr[0], FramePtr, Size);
+    }
+  } else {
+    //
+    // Get the register data. If operand1 == 0, then ignore register and
+    // take immediate data as relative or absolute address.
+    // Compiler should take care of upper bits if 32-bit machine.
+    //
+    if (OPERAND1_REGNUM (Operands) != 0) {
+      Immed64 = (UINT64) (UINTN) VmPtr->Gpr[OPERAND1_REGNUM (Operands)];
+    }
+    //
+    // Get final address
+    //
+    if (OPERAND1_INDIRECT (Operands)) {
+      Immed64 = (INT64) (UINT64) (UINTN) VmReadMemN (VmPtr, (UINTN) (Immed64 + Immed32));
+    } else {
+      Immed64 += Immed32;
+    }
+    //
+    // Now determine if external call, and then if relative or absolute
+    //
+    if ((Operands & OPERAND_M_NATIVE_CALL) == 0) {
+      //
+      // EBC call. Relative or absolute? If relative, then it's relative to the
+      // start of the next instruction.
+      //
+      if ((Operands & OPERAND_M_RELATIVE_ADDR) != 0) {
+        VmPtr->Ip += Immed64 + Size;
+      } else {
+        VmPtr->Ip = (VMIP) (UINTN) Immed64;
+      }
+    } else {
+      //
+      // Native call. Relative or absolute?
+      //
+      if ((Operands & OPERAND_M_RELATIVE_ADDR) != 0) {
+        EbcLLCALLEX (VmPtr, (UINTN) (Immed64 + VmPtr->Ip + Size), (UINTN) VmPtr->Gpr[0], FramePtr, Size);
+      } else {
+        if ((VmPtr->StopFlags & STOPFLAG_BREAK_ON_CALLEX) != 0) {
+          CpuBreakpoint ();
+        }
+
+        EbcLLCALLEX (VmPtr, (UINTN) Immed64, (UINTN) VmPtr->Gpr[0], FramePtr, Size);
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC RET instruction.
+
+  Instruction syntax:
+    RET
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteRET (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  //
+  // If we're at the top of the stack, then simply set the done
+  // flag and return
+  //
+  if (VmPtr->StackRetAddr == (UINT64) VmPtr->Gpr[0]) {
+    VmPtr->StopFlags |= STOPFLAG_APP_DONE;
+  } else {
+    //
+    // Pull the return address off the VM app's stack and set the IP
+    // to it
+    //
+    if (!IS_ALIGNED ((UINTN) VmPtr->Gpr[0], sizeof (UINT16))) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_ALIGNMENT_CHECK,
+        EXCEPTION_FLAG_FATAL,
+        VmPtr
+        );
+    }
+    //
+    // Restore the IP and frame pointer from the stack
+    //
+    VmPtr->Ip = (VMIP) (UINTN) VmReadMem64 (VmPtr, (UINTN) VmPtr->Gpr[0]);
+    VmPtr->Gpr[0] += 8;
+    VmPtr->FramePtr = (VOID *) VmReadMemN (VmPtr, (UINTN) VmPtr->Gpr[0]);
+    VmPtr->Gpr[0] += 8;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC CMP instruction.
+
+  Instruction syntax:
+    CMP[32|64][eq|lte|gte|ulte|ugte] R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCMP (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT16   Index16;
+  UINT32  Flag;
+  INT64   Op2;
+  INT64   Op1;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+  //
+  // Get the register data we're going to compare to
+  //
+  Op1 = VmPtr->Gpr[OPERAND1_REGNUM (Operands)];
+  //
+  // Get immediate data
+  //
+  if ((Opcode & OPCODE_M_IMMDATA) != 0) {
+    if (OPERAND2_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    Size = 4;
+  } else {
+    Index16 = 0;
+    Size    = 2;
+  }
+  //
+  // Now get Op2
+  //
+  if (OPERAND2_INDIRECT (Operands)) {
+    if ((Opcode & OPCODE_M_64BIT) != 0) {
+      Op2 = (INT64) VmReadMem64 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index16));
+    } else {
+      //
+      // 32-bit operations. 0-extend the values for all cases.
+      //
+      Op2 = (INT64) (UINT64) ((UINT32) VmReadMem32 (VmPtr, (UINTN) (VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index16)));
+    }
+  } else {
+    Op2 = VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index16;
+  }
+  //
+  // Now do the compare
+  //
+  Flag = 0;
+  if ((Opcode & OPCODE_M_64BIT) != 0) {
+    //
+    // 64-bit compares
+    //
+    switch (Opcode & OPCODE_M_OPCODE) {
+    case OPCODE_CMPEQ:
+      if (Op1 == Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPLTE:
+      if (Op1 <= Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPGTE:
+      if (Op1 >= Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPULTE:
+      if ((UINT64) Op1 <= (UINT64) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPUGTE:
+      if ((UINT64) Op1 >= (UINT64) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    default:
+      ASSERT (0);
+    }
+  } else {
+    //
+    // 32-bit compares
+    //
+    switch (Opcode & OPCODE_M_OPCODE) {
+    case OPCODE_CMPEQ:
+      if ((INT32) Op1 == (INT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPLTE:
+      if ((INT32) Op1 <= (INT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPGTE:
+      if ((INT32) Op1 >= (INT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPULTE:
+      if ((UINT32) Op1 <= (UINT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPUGTE:
+      if ((UINT32) Op1 >= (UINT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    default:
+      ASSERT (0);
+    }
+  }
+  //
+  // Now set the flag accordingly for the comparison
+  //
+  if (Flag != 0) {
+    VMFLAG_SET (VmPtr, VMFLAGS_CC);
+  } else {
+    VMFLAG_CLEAR (VmPtr, (UINT64)VMFLAGS_CC);
+  }
+  //
+  // Advance the IP
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC CMPI instruction
+
+  Instruction syntax:
+    CMPI[32|64]{w|d}[eq|lte|gte|ulte|ugte] {@}Rx {Index16}, Immed16|Immed32
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteCMPI (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8   Opcode;
+  UINT8   Operands;
+  UINT8   Size;
+  INT64   Op1;
+  INT64   Op2;
+  INT16   Index16;
+  UINT32  Flag;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Get operand1 index if present
+  //
+  Size = 2;
+  if ((Operands & OPERAND_M_CMPI_INDEX) != 0) {
+    Index16 = VmReadIndex16 (VmPtr, 2);
+    Size += 2;
+  } else {
+    Index16 = 0;
+  }
+  //
+  // Get operand1 data we're going to compare to
+  //
+  Op1 = (INT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)];
+  if (OPERAND1_INDIRECT (Operands)) {
+    //
+    // Indirect operand1. Fetch 32 or 64-bit value based on compare size.
+    //
+    if ((Opcode & OPCODE_M_CMPI64) != 0) {
+      Op1 = (INT64) VmReadMem64 (VmPtr, (UINTN) Op1 + Index16);
+    } else {
+      Op1 = (INT64) VmReadMem32 (VmPtr, (UINTN) Op1 + Index16);
+    }
+  } else {
+    //
+    // Better not have been an index with direct. That is, CMPI R1 Index,...
+    // is illegal.
+    //
+    if ((Operands & OPERAND_M_CMPI_INDEX) != 0) {
+      EbcDebugSignalException (
+        EXCEPT_EBC_INSTRUCTION_ENCODING,
+        EXCEPTION_FLAG_ERROR,
+        VmPtr
+        );
+      VmPtr->Ip += Size;
+      return EFI_UNSUPPORTED;
+    }
+  }
+  //
+  // Get immediate data -- 16- or 32-bit sign extended
+  //
+  if ((Opcode & OPCODE_M_CMPI32_DATA) != 0) {
+    Op2 = (INT64) VmReadImmed32 (VmPtr, Size);
+    Size += 4;
+  } else {
+    //
+    // 16-bit immediate data. Sign extend always.
+    //
+    Op2 = (INT64) ((INT16) VmReadImmed16 (VmPtr, Size));
+    Size += 2;
+  }
+  //
+  // Now do the compare
+  //
+  Flag = 0;
+  if ((Opcode & OPCODE_M_CMPI64) != 0) {
+    //
+    // 64 bit comparison
+    //
+    switch (Opcode & OPCODE_M_OPCODE) {
+    case OPCODE_CMPIEQ:
+      if (Op1 == (INT64) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPILTE:
+      if (Op1 <= (INT64) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIGTE:
+      if (Op1 >= (INT64) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIULTE:
+      if ((UINT64) Op1 <= (UINT64) ((UINT32) Op2)) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIUGTE:
+      if ((UINT64) Op1 >= (UINT64) ((UINT32) Op2)) {
+        Flag = 1;
+      }
+      break;
+
+    default:
+      ASSERT (0);
+    }
+  } else {
+    //
+    // 32-bit comparisons
+    //
+    switch (Opcode & OPCODE_M_OPCODE) {
+    case OPCODE_CMPIEQ:
+      if ((INT32) Op1 == Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPILTE:
+      if ((INT32) Op1 <= Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIGTE:
+      if ((INT32) Op1 >= Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIULTE:
+      if ((UINT32) Op1 <= (UINT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    case OPCODE_CMPIUGTE:
+      if ((UINT32) Op1 >= (UINT32) Op2) {
+        Flag = 1;
+      }
+      break;
+
+    default:
+      ASSERT (0);
+    }
+  }
+  //
+  // Now set the flag accordingly for the comparison
+  //
+  if (Flag != 0) {
+    VMFLAG_SET (VmPtr, VMFLAGS_CC);
+  } else {
+    VMFLAG_CLEAR (VmPtr, (UINT64)VMFLAGS_CC);
+  }
+  //
+  // Advance the IP
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC NOT instruction.s
+
+  Instruction syntax:
+    NOT[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return ~Op2
+
+**/
+UINT64
+ExecuteNOT (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT64         Op1,
+  IN UINT64         Op2
+  )
+{
+  return ~Op2;
+}
+
+
+/**
+  Execute the EBC NEG instruction.
+
+  Instruction syntax:
+    NEG[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op2 * -1
+
+**/
+UINT64
+ExecuteNEG (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  return ~Op2 + 1;
+}
+
+
+/**
+  Execute the EBC ADD instruction.
+
+  Instruction syntax:
+    ADD[32|64] {@}R1, {@}R2 {Index16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 + Op2
+
+**/
+UINT64
+ExecuteADD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  return Op1 + Op2;
+}
+
+
+/**
+  Execute the EBC SUB instruction.
+
+  Instruction syntax:
+    SUB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 - Op2
+
+**/
+UINT64
+ExecuteSUB (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return (UINT64) ((INT64) ((INT64) Op1 - (INT64) Op2));
+  } else {
+    return (UINT64) ((INT64) ((INT32) Op1 - (INT32) Op2));
+  }
+}
+
+
+/**
+  Execute the EBC MUL instruction.
+
+  Instruction syntax:
+    SUB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 * Op2
+
+**/
+UINT64
+ExecuteMUL (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return MultS64x64 ((INT64)Op1, (INT64)Op2);
+  } else {
+    return (UINT64) ((INT64) ((INT32) Op1 * (INT32) Op2));
+  }
+}
+
+
+/**
+  Execute the EBC MULU instruction
+
+  Instruction syntax:
+    MULU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (unsigned)Op1 * (unsigned)Op2
+
+**/
+UINT64
+ExecuteMULU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return MultU64x64 (Op1, Op2);
+  } else {
+    return (UINT64) ((UINT32) Op1 * (UINT32) Op2);
+  }
+}
+
+
+/**
+  Execute the EBC DIV instruction.
+
+  Instruction syntax:
+    DIV[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 / Op2
+
+**/
+UINT64
+ExecuteDIV (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  INT64   Remainder;
+
+  //
+  // Check for divide-by-0
+  //
+  if (Op2 == 0) {
+    EbcDebugSignalException (
+      EXCEPT_EBC_DIVIDE_ERROR,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+
+    return 0;
+  } else {
+    if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+      return (UINT64) (DivS64x64Remainder (Op1, Op2, &Remainder));
+    } else {
+      return (UINT64) ((INT64) ((INT32) Op1 / (INT32) Op2));
+    }
+  }
+}
+
+
+/**
+  Execute the EBC DIVU instruction
+
+  Instruction syntax:
+    DIVU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (unsigned)Op1 / (unsigned)Op2
+
+**/
+UINT64
+ExecuteDIVU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  UINT64  Remainder;
+
+  //
+  // Check for divide-by-0
+  //
+  if (Op2 == 0) {
+    EbcDebugSignalException (
+      EXCEPT_EBC_DIVIDE_ERROR,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return 0;
+  } else {
+    //
+    // Get the destination register
+    //
+    if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+      return (UINT64) (DivU64x64Remainder (Op1, Op2, &Remainder));
+    } else {
+      return (UINT64) ((UINT32) Op1 / (UINT32) Op2);
+    }
+  }
+}
+
+
+/**
+  Execute the EBC MOD instruction.
+
+  Instruction syntax:
+    MOD[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 MODULUS Op2
+
+**/
+UINT64
+ExecuteMOD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  INT64   Remainder;
+
+  //
+  // Check for divide-by-0
+  //
+  if (Op2 == 0) {
+    EbcDebugSignalException (
+      EXCEPT_EBC_DIVIDE_ERROR,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return 0;
+  } else {
+    DivS64x64Remainder ((INT64)Op1, (INT64)Op2, &Remainder);
+    return Remainder;
+  }
+}
+
+
+/**
+  Execute the EBC MODU instruction.
+
+  Instruction syntax:
+    MODU[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 UNSIGNED_MODULUS Op2
+
+**/
+UINT64
+ExecuteMODU (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  UINT64  Remainder;
+
+  //
+  // Check for divide-by-0
+  //
+  if (Op2 == 0) {
+    EbcDebugSignalException (
+      EXCEPT_EBC_DIVIDE_ERROR,
+      EXCEPTION_FLAG_FATAL,
+      VmPtr
+      );
+    return 0;
+  } else {
+    DivU64x64Remainder (Op1, Op2, &Remainder);
+    return Remainder;
+  }
+}
+
+
+/**
+  Execute the EBC AND instruction.
+
+  Instruction syntax:
+    AND[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 AND Op2
+
+**/
+UINT64
+ExecuteAND (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  return Op1 & Op2;
+}
+
+
+/**
+  Execute the EBC OR instruction.
+
+  Instruction syntax:
+    OR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 OR Op2
+
+**/
+UINT64
+ExecuteOR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  return Op1 | Op2;
+}
+
+
+/**
+  Execute the EBC XOR instruction.
+
+  Instruction syntax:
+    XOR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 XOR Op2
+
+**/
+UINT64
+ExecuteXOR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  return Op1 ^ Op2;
+}
+
+
+/**
+  Execute the EBC SHL shift left instruction.
+
+  Instruction syntax:
+    SHL[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 << Op2
+
+**/
+UINT64
+ExecuteSHL (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return LShiftU64 (Op1, (UINTN)Op2);
+  } else {
+    return (UINT64) ((UINT32) ((UINT32) Op1 << (UINT32) Op2));
+  }
+}
+
+
+/**
+  Execute the EBC SHR instruction.
+
+  Instruction syntax:
+    SHR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 >> Op2  (unsigned operands)
+
+**/
+UINT64
+ExecuteSHR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return RShiftU64 (Op1, (UINTN)Op2);
+  } else {
+    return (UINT64) ((UINT32) Op1 >> (UINT32) Op2);
+  }
+}
+
+
+/**
+  Execute the EBC ASHR instruction.
+
+  Instruction syntax:
+    ASHR[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return Op1 >> Op2 (signed)
+
+**/
+UINT64
+ExecuteASHR (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  if ((*VmPtr->Ip & DATAMANIP_M_64) != 0) {
+    return ARShiftU64 (Op1, (UINTN)Op2);
+  } else {
+    return (UINT64) ((INT64) ((INT32) Op1 >> (UINT32) Op2));
+  }
+}
+
+
+/**
+  Execute the EBC EXTNDB instruction to sign-extend a byte value.
+
+  Instruction syntax:
+    EXTNDB[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT8)Op2
+
+**/
+UINT64
+ExecuteEXTNDB (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  INT8  Data8;
+  INT64 Data64;
+  //
+  // Convert to byte, then return as 64-bit signed value to let compiler
+  // sign-extend the value
+  //
+  Data8   = (INT8) Op2;
+  Data64  = (INT64) Data8;
+
+  return (UINT64) Data64;
+}
+
+
+/**
+  Execute the EBC EXTNDW instruction to sign-extend a 16-bit value.
+
+  Instruction syntax:
+    EXTNDW[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT16)Op2
+
+**/
+UINT64
+ExecuteEXTNDW (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  INT16 Data16;
+  INT64 Data64;
+  //
+  // Convert to word, then return as 64-bit signed value to let compiler
+  // sign-extend the value
+  //
+  Data16  = (INT16) Op2;
+  Data64  = (INT64) Data16;
+
+  return (UINT64) Data64;
+}
+//
+// Execute the EBC EXTNDD instruction.
+//
+// Format: EXTNDD {@}Rx, {@}Ry [Index16|Immed16]
+//         EXTNDD Dest, Source
+//
+// Operation:  Dest <- SignExtended((DWORD)Source))
+//
+
+/**
+  Execute the EBC EXTNDD instruction to sign-extend a 32-bit value.
+
+  Instruction syntax:
+    EXTNDD[32|64] {@}R1, {@}R2 {Index16|Immed16}
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Op1               Operand 1 from the instruction
+  @param  Op2               Operand 2 from the instruction
+
+  @return (INT64)(INT32)Op2
+
+**/
+UINT64
+ExecuteEXTNDD (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINT64       Op1,
+  IN UINT64       Op2
+  )
+{
+  INT32 Data32;
+  INT64 Data64;
+  //
+  // Convert to 32-bit value, then return as 64-bit signed value to let compiler
+  // sign-extend the value
+  //
+  Data32  = (INT32) Op2;
+  Data64  = (INT64) Data32;
+
+  return (UINT64) Data64;
+}
+
+
+/**
+  Execute all the EBC signed data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteSignedDataManip (
+  IN VM_CONTEXT   *VmPtr
+  )
+{
+  //
+  // Just call the data manipulation function with a flag indicating this
+  // is a signed operation.
+  //
+  return ExecuteDataManip (VmPtr, TRUE);
+}
+
+
+/**
+  Execute all the EBC unsigned data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteUnsignedDataManip (
+  IN VM_CONTEXT   *VmPtr
+  )
+{
+  //
+  // Just call the data manipulation function with a flag indicating this
+  // is not a signed operation.
+  //
+  return ExecuteDataManip (VmPtr, FALSE);
+}
+
+
+/**
+  Execute all the EBC data manipulation instructions.
+  Since the EBC data manipulation instructions all have the same basic form,
+  they can share the code that does the fetch of operands and the write-back
+  of the result. This function performs the fetch of the operands (even if
+  both are not needed to be fetched, like NOT instruction), dispatches to the
+  appropriate subfunction, then writes back the returned result.
+
+  Format:
+    INSTRUCITON[32|64] {@}R1, {@}R2 {Immed16|Index16}
+
+  @param  VmPtr             A pointer to VM context.
+  @param  IsSignedOp        Indicates whether the operand is signed or not.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteDataManip (
+  IN VM_CONTEXT   *VmPtr,
+  IN BOOLEAN      IsSignedOp
+  )
+{
+  UINT8   Opcode;
+  INT16   Index16;
+  UINT8   Operands;
+  UINT8   Size;
+  UINT64  Op1;
+  UINT64  Op2;
+  INTN    DataManipDispatchTableIndex;
+
+  //
+  // Get opcode and operands
+  //
+  Opcode    = GETOPCODE (VmPtr);
+  Operands  = GETOPERANDS (VmPtr);
+
+  //
+  // Determine if we have immediate data by the opcode
+  //
+  if ((Opcode & DATAMANIP_M_IMMDATA) != 0) {
+    //
+    // Index16 if Ry is indirect, or Immed16 if Ry direct.
+    //
+    if (OPERAND2_INDIRECT (Operands)) {
+      Index16 = VmReadIndex16 (VmPtr, 2);
+    } else {
+      Index16 = VmReadImmed16 (VmPtr, 2);
+    }
+
+    Size = 4;
+  } else {
+    Index16 = 0;
+    Size    = 2;
+  }
+  //
+  // Now get operand2 (source). It's of format {@}R2 {Index16|Immed16}
+  //
+  Op2 = (UINT64) VmPtr->Gpr[OPERAND2_REGNUM (Operands)] + Index16;
+  if (OPERAND2_INDIRECT (Operands)) {
+    //
+    // Indirect form: @R2 Index16. Fetch as 32- or 64-bit data
+    //
+    if ((Opcode & DATAMANIP_M_64) != 0) {
+      Op2 = VmReadMem64 (VmPtr, (UINTN) Op2);
+    } else {
+      //
+      // Read as signed value where appropriate.
+      //
+      if (IsSignedOp) {
+        Op2 = (UINT64) (INT64) ((INT32) VmReadMem32 (VmPtr, (UINTN) Op2));
+      } else {
+        Op2 = (UINT64) VmReadMem32 (VmPtr, (UINTN) Op2);
+      }
+    }
+  } else {
+    if ((Opcode & DATAMANIP_M_64) == 0) {
+      if (IsSignedOp) {
+        Op2 = (UINT64) (INT64) ((INT32) Op2);
+      } else {
+        Op2 = (UINT64) ((UINT32) Op2);
+      }
+    }
+  }
+  //
+  // Get operand1 (destination and sometimes also an actual operand)
+  // of form {@}R1
+  //
+  Op1 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)];
+  if (OPERAND1_INDIRECT (Operands)) {
+    if ((Opcode & DATAMANIP_M_64) != 0) {
+      Op1 = VmReadMem64 (VmPtr, (UINTN) Op1);
+    } else {
+      if (IsSignedOp) {
+        Op1 = (UINT64) (INT64) ((INT32) VmReadMem32 (VmPtr, (UINTN) Op1));
+      } else {
+        Op1 = (UINT64) VmReadMem32 (VmPtr, (UINTN) Op1);
+      }
+    }
+  } else {
+    if ((Opcode & DATAMANIP_M_64) == 0) {
+      if (IsSignedOp) {
+        Op1 = (UINT64) (INT64) ((INT32) Op1);
+      } else {
+        Op1 = (UINT64) ((UINT32) Op1);
+      }
+    }
+  }
+  //
+  // Dispatch to the computation function
+  //
+  DataManipDispatchTableIndex = (Opcode & OPCODE_M_OPCODE) - OPCODE_NOT;
+  if ((DataManipDispatchTableIndex < 0) ||
+      (DataManipDispatchTableIndex >= sizeof (mDataManipDispatchTable) / sizeof (mDataManipDispatchTable[0]))) {
+    EbcDebugSignalException (
+      EXCEPT_EBC_INVALID_OPCODE,
+      EXCEPTION_FLAG_ERROR,
+      VmPtr
+      );
+    //
+    // Advance and return
+    //
+    VmPtr->Ip += Size;
+    return EFI_UNSUPPORTED;
+  } else {
+    Op2 = mDataManipDispatchTable[DataManipDispatchTableIndex](VmPtr, Op1, Op2);
+  }
+  //
+  // Write back the result.
+  //
+  if (OPERAND1_INDIRECT (Operands)) {
+    Op1 = (UINT64) VmPtr->Gpr[OPERAND1_REGNUM (Operands)];
+    if ((Opcode & DATAMANIP_M_64) != 0) {
+      VmWriteMem64 (VmPtr, (UINTN) Op1, Op2);
+    } else {
+      VmWriteMem32 (VmPtr, (UINTN) Op1, (UINT32) Op2);
+    }
+  } else {
+    //
+    // Storage back to a register. Write back, clearing upper bits (as per
+    // the specification) if 32-bit operation.
+    //
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = Op2;
+    if ((Opcode & DATAMANIP_M_64) == 0) {
+      VmPtr->Gpr[OPERAND1_REGNUM (Operands)] &= 0xFFFFFFFF;
+    }
+  }
+  //
+  // Advance the instruction pointer
+  //
+  VmPtr->Ip += Size;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC LOADSP instruction.
+
+  Instruction syntax:
+    LOADSP  SP1, R2
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteLOADSP (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Operands;
+
+  //
+  // Get the operands
+  //
+  Operands = GETOPERANDS (VmPtr);
+
+  //
+  // Do the operation
+  //
+  switch (OPERAND1_REGNUM (Operands)) {
+  //
+  // Set flags
+  //
+  case 0:
+    //
+    // Spec states that this instruction will not modify reserved bits in
+    // the flags register.
+    //
+    VmPtr->Flags = (VmPtr->Flags &~VMFLAGS_ALL_VALID) | (VmPtr->Gpr[OPERAND2_REGNUM (Operands)] & VMFLAGS_ALL_VALID);
+    break;
+
+  default:
+    EbcDebugSignalException (
+      EXCEPT_EBC_INSTRUCTION_ENCODING,
+      EXCEPTION_FLAG_WARNING,
+      VmPtr
+      );
+    VmPtr->Ip += 2;
+    return EFI_UNSUPPORTED;
+  }
+
+  VmPtr->Ip += 2;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Execute the EBC STORESP instruction.
+
+  Instruction syntax:
+    STORESP  Rx, FLAGS|IP
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   The opcodes/operands is not supported.
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+
+**/
+EFI_STATUS
+ExecuteSTORESP (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  UINT8 Operands;
+
+  //
+  // Get the operands
+  //
+  Operands = GETOPERANDS (VmPtr);
+
+  //
+  // Do the operation
+  //
+  switch (OPERAND2_REGNUM (Operands)) {
+  //
+  // Get flags
+  //
+  case 0:
+    //
+    // Retrieve the value in the flags register, then clear reserved bits
+    //
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = (UINT64) (VmPtr->Flags & VMFLAGS_ALL_VALID);
+    break;
+
+  //
+  // Get IP -- address of following instruction
+  //
+  case 1:
+    VmPtr->Gpr[OPERAND1_REGNUM (Operands)] = (UINT64) (UINTN) VmPtr->Ip + 2;
+    break;
+
+  default:
+    EbcDebugSignalException (
+      EXCEPT_EBC_INSTRUCTION_ENCODING,
+      EXCEPTION_FLAG_WARNING,
+      VmPtr
+      );
+    VmPtr->Ip += 2;
+    return EFI_UNSUPPORTED;
+    break;
+  }
+
+  VmPtr->Ip += 2;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Decode a 16-bit index to determine the offset. Given an index value:
+
+    b15     - sign bit
+    b14:12  - number of bits in this index assigned to natural units (=a)
+    ba:11   - constant units = ConstUnits
+    b0:a    - natural units = NaturalUnits
+
+  Given this info, the offset can be computed by:
+    offset = sign_bit * (ConstUnits + NaturalUnits * sizeof(UINTN))
+
+  Max offset is achieved with index = 0x7FFF giving an offset of
+  0x27B (32-bit machine) or 0x477 (64-bit machine).
+  Min offset is achieved with index =
+
+  @param  VmPtr             A pointer to VM context.
+  @param  CodeOffset        Offset from IP of the location of the 16-bit index
+                            to decode.
+
+  @return The decoded offset.
+
+**/
+INT16
+VmReadIndex16 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  )
+{
+  UINT16  Index;
+  INT16   Offset;
+  INT16   ConstUnits;
+  INT16   NaturalUnits;
+  INT16   NBits;
+  INT16   Mask;
+
+  //
+  // First read the index from the code stream
+  //
+  Index = VmReadCode16 (VmPtr, CodeOffset);
+
+  //
+  // Get the mask for NaturalUnits. First get the number of bits from the index.
+  //
+  NBits = (INT16) ((Index & 0x7000) >> 12);
+
+  //
+  // Scale it for 16-bit indexes
+  //
+  NBits *= 2;
+
+  //
+  // Now using the number of bits, create a mask.
+  //
+  Mask = (INT16) ((INT16)~0 << NBits);
+
+  //
+  // Now using the mask, extract NaturalUnits from the lower bits of the index.
+  //
+  NaturalUnits = (INT16) (Index &~Mask);
+
+  //
+  // Now compute ConstUnits
+  //
+  ConstUnits       = (INT16) (((Index &~0xF000) & Mask) >> NBits);
+
+  Offset  = (INT16) (NaturalUnits * sizeof (UINTN) + ConstUnits);
+
+  //
+  // Now set the sign
+  //
+  if ((Index & 0x8000) != 0) {
+    //
+    // Do it the hard way to work around a bogus compiler warning
+    //
+    // Offset = -1 * Offset;
+    //
+    Offset = (INT16) ((INT32) Offset * -1);
+  }
+
+  return Offset;
+}
+
+
+/**
+  Decode a 32-bit index to determine the offset.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  CodeOffset        Offset from IP of the location of the 32-bit index
+                            to decode.
+
+  @return Converted index per EBC VM specification.
+
+**/
+INT32
+VmReadIndex32 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  )
+{
+  UINT32  Index;
+  INT32   Offset;
+  INT32   ConstUnits;
+  INT32   NaturalUnits;
+  INT32   NBits;
+  INT32   Mask;
+
+  Index = VmReadImmed32 (VmPtr, CodeOffset);
+
+  //
+  // Get the mask for NaturalUnits. First get the number of bits from the index.
+  //
+  NBits = (Index & 0x70000000) >> 28;
+
+  //
+  // Scale it for 32-bit indexes
+  //
+  NBits *= 4;
+
+  //
+  // Now using the number of bits, create a mask.
+  //
+  Mask = (INT32)~0 << NBits;
+
+  //
+  // Now using the mask, extract NaturalUnits from the lower bits of the index.
+  //
+  NaturalUnits = Index &~Mask;
+
+  //
+  // Now compute ConstUnits
+  //
+  ConstUnits       = ((Index &~0xF0000000) & Mask) >> NBits;
+
+  Offset  = NaturalUnits * sizeof (UINTN) + ConstUnits;
+
+  //
+  // Now set the sign
+  //
+  if ((Index & 0x80000000) != 0) {
+    Offset = Offset * -1;
+  }
+
+  return Offset;
+}
+
+
+/**
+  Decode a 64-bit index to determine the offset.
+
+  @param  VmPtr             A pointer to VM context.s
+  @param  CodeOffset        Offset from IP of the location of the 64-bit index
+                            to decode.
+
+  @return Converted index per EBC VM specification
+
+**/
+INT64
+VmReadIndex64 (
+  IN VM_CONTEXT     *VmPtr,
+  IN UINT32         CodeOffset
+  )
+{
+  UINT64  Index;
+  INT64   Offset;
+  INT64   ConstUnits;
+  INT64   NaturalUnits;
+  INT64   NBits;
+  INT64   Mask;
+
+  Index = VmReadCode64 (VmPtr, CodeOffset);
+
+  //
+  // Get the mask for NaturalUnits. First get the number of bits from the index.
+  //
+  NBits = RShiftU64 ((Index & 0x7000000000000000ULL), 60);
+
+  //
+  // Scale it for 64-bit indexes (multiply by 8 by shifting left 3)
+  //
+  NBits = LShiftU64 ((UINT64)NBits, 3);
+
+  //
+  // Now using the number of bits, create a mask.
+  //
+  Mask = (LShiftU64 ((UINT64)~0, (UINTN)NBits));
+
+  //
+  // Now using the mask, extract NaturalUnits from the lower bits of the index.
+  //
+  NaturalUnits = Index &~Mask;
+
+  //
+  // Now compute ConstUnits
+  //
+  ConstUnits = ARShiftU64 (((Index &~0xF000000000000000ULL) & Mask), (UINTN)NBits);
+
+  Offset  = MultU64x64 ((UINT64) NaturalUnits, sizeof (UINTN)) + ConstUnits;
+
+  //
+  // Now set the sign
+  //
+  if ((Index & 0x8000000000000000ULL) != 0) {
+    Offset = MultS64x64 (Offset, -1);
+  }
+
+  return Offset;
+}
+
+
+/**
+  Writes 8-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem8 (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr,
+  IN UINT8         Data
+  )
+{
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr            = ConvertStackAddr (VmPtr, Addr);
+  *(UINT8 *) Addr = Data;
+  return EFI_SUCCESS;
+}
+
+/**
+  Writes 16-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem16 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT16       Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+
+  //
+  // Do a simple write if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT16))) {
+    *(UINT16 *) Addr = Data;
+  } else {
+    //
+    // Write as two bytes
+    //
+    MemoryFence ();
+    if ((Status = VmWriteMem8 (VmPtr, Addr, (UINT8) Data)) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+    if ((Status = VmWriteMem8 (VmPtr, Addr + 1, (UINT8) (Data >> 8))) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Writes 32-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem32 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT32       Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+
+  //
+  // Do a simple write if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT32))) {
+    *(UINT32 *) Addr = Data;
+  } else {
+    //
+    // Write as two words
+    //
+    MemoryFence ();
+    if ((Status = VmWriteMem16 (VmPtr, Addr, (UINT16) Data)) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+    if ((Status = VmWriteMem16 (VmPtr, Addr + sizeof (UINT16), (UINT16) (Data >> 16))) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Writes 64-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem64 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT64       Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+
+  //
+  // Do a simple write if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT64))) {
+    *(UINT64 *) Addr = Data;
+  } else {
+    //
+    // Write as two 32-bit words
+    //
+    MemoryFence ();
+    if ((Status = VmWriteMem32 (VmPtr, Addr, (UINT32) Data)) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+    if ((Status = VmWriteMem32 (VmPtr, Addr + sizeof (UINT32), (UINT32) RShiftU64(Data, 32))) != EFI_SUCCESS) {
+      return Status;
+    }
+
+    MemoryFence ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Writes UINTN data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMemN (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINTN        Data
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+
+  //
+  // Do a simple write if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINTN))) {
+    *(UINTN *) Addr = Data;
+  } else {
+    for (Index = 0; Index < sizeof (UINTN) / sizeof (UINT32); Index++) {
+      MemoryFence ();
+      Status = VmWriteMem32 (VmPtr, Addr + Index * sizeof (UINT32), (UINT32) Data);
+      MemoryFence ();
+      Data = (UINTN) RShiftU64 ((UINT64)Data, 32);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Reads 8-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT8
+VmReadImmed8 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  //
+  // Simply return the data in flat memory space
+  //
+  return * (INT8 *) (VmPtr->Ip + Offset);
+}
+
+/**
+  Reads 16-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT16
+VmReadImmed16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (INT16))) {
+    return * (INT16 *) (VmPtr->Ip + Offset);
+  } else {
+    //
+    // All code word reads should be aligned
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_ALIGNMENT_CHECK,
+      EXCEPTION_FLAG_WARNING,
+      VmPtr
+      );
+  }
+  //
+  // Return unaligned data
+  //
+  return (INT16) (*(UINT8 *) (VmPtr->Ip + Offset) + (*(UINT8 *) (VmPtr->Ip + Offset + 1) << 8));
+}
+
+
+/**
+  Reads 32-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT32
+VmReadImmed32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  UINT32  Data;
+
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (UINT32))) {
+    return * (INT32 *) (VmPtr->Ip + Offset);
+  }
+  //
+  // Return unaligned data
+  //
+  Data  = (UINT32) VmReadCode16 (VmPtr, Offset);
+  Data |= (UINT32)(VmReadCode16 (VmPtr, Offset + 2) << 16);
+  return Data;
+}
+
+
+/**
+  Reads 64-bit immediate value at the offset.
+
+  This routine is called by the EBC execute
+  functions to read EBC immediate values from the code stream.
+  Since we can't assume alignment, each tries to read in the biggest
+  chunks size available, but will revert to smaller reads if necessary.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Offset            offset from IP of the code bytes to read.
+
+  @return Signed data of the requested size from the specified address.
+
+**/
+INT64
+VmReadImmed64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  UINT64  Data64;
+  UINT32  Data32;
+  UINT8   *Ptr;
+
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (UINT64))) {
+    return * (UINT64 *) (VmPtr->Ip + Offset);
+  }
+  //
+  // Return unaligned data.
+  //
+  Ptr             = (UINT8 *) &Data64;
+  Data32          = VmReadCode32 (VmPtr, Offset);
+  *(UINT32 *) Ptr = Data32;
+  Ptr            += sizeof (Data32);
+  Data32          = VmReadCode32 (VmPtr, Offset + sizeof (UINT32));
+  *(UINT32 *) Ptr = Data32;
+  return Data64;
+}
+
+
+/**
+  Reads 16-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 16-bit value from the code stream.
+
+**/
+UINT16
+VmReadCode16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (UINT16))) {
+    return * (UINT16 *) (VmPtr->Ip + Offset);
+  } else {
+    //
+    // All code word reads should be aligned
+    //
+    EbcDebugSignalException (
+      EXCEPT_EBC_ALIGNMENT_CHECK,
+      EXCEPTION_FLAG_WARNING,
+      VmPtr
+      );
+  }
+  //
+  // Return unaligned data
+  //
+  return (UINT16) (*(UINT8 *) (VmPtr->Ip + Offset) + (*(UINT8 *) (VmPtr->Ip + Offset + 1) << 8));
+}
+
+
+/**
+  Reads 32-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 32-bit value from the code stream.
+
+**/
+UINT32
+VmReadCode32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  UINT32  Data;
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (UINT32))) {
+    return * (UINT32 *) (VmPtr->Ip + Offset);
+  }
+  //
+  // Return unaligned data
+  //
+  Data = (UINT32) VmReadCode16 (VmPtr, Offset);
+  Data |= (VmReadCode16 (VmPtr, Offset + 2) << 16);
+  return Data;
+}
+
+
+/**
+  Reads 64-bit unsigned data from the code stream.
+
+  This routine provides the ability to read raw unsigned data from the code
+  stream.
+
+  @param  VmPtr             A pointer to VM context
+  @param  Offset            Offset from current IP to the raw data to read.
+
+  @return The raw unsigned 64-bit value from the code stream.
+
+**/
+UINT64
+VmReadCode64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT32     Offset
+  )
+{
+  UINT64  Data64;
+  UINT32  Data32;
+  UINT8   *Ptr;
+
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED ((UINTN) VmPtr->Ip + Offset, sizeof (UINT64))) {
+    return * (UINT64 *) (VmPtr->Ip + Offset);
+  }
+  //
+  // Return unaligned data.
+  //
+  Ptr             = (UINT8 *) &Data64;
+  Data32          = VmReadCode32 (VmPtr, Offset);
+  *(UINT32 *) Ptr = Data32;
+  Ptr            += sizeof (Data32);
+  Data32          = VmReadCode32 (VmPtr, Offset + sizeof (UINT32));
+  *(UINT32 *) Ptr = Data32;
+  return Data64;
+}
+
+
+/**
+  Reads 8-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 8-bit value from the memory address.
+
+**/
+UINT8
+VmReadMem8 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr
+  )
+{
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+  //
+  // Simply return the data in flat memory space
+  //
+  return * (UINT8 *) Addr;
+}
+
+/**
+  Reads 16-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 16-bit value from the memory address.
+
+**/
+UINT16
+VmReadMem16 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINTN      Addr
+  )
+{
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT16))) {
+    return * (UINT16 *) Addr;
+  }
+  //
+  // Return unaligned data
+  //
+  return (UINT16) (*(UINT8 *) Addr + (*(UINT8 *) (Addr + 1) << 8));
+}
+
+/**
+  Reads 32-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 32-bit value from the memory address.
+
+**/
+UINT32
+VmReadMem32 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINTN      Addr
+  )
+{
+  UINT32  Data;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT32))) {
+    return * (UINT32 *) Addr;
+  }
+  //
+  // Return unaligned data
+  //
+  Data = (UINT32) VmReadMem16 (VmPtr, Addr);
+  Data |= (VmReadMem16 (VmPtr, Addr + 2) << 16);
+  return Data;
+}
+
+/**
+  Reads 64-bit data form the memory address.
+
+  @param  VmPtr             A pointer to VM context.
+  @param  Addr              The memory address.
+
+  @return The 64-bit value from the memory address.
+
+**/
+UINT64
+VmReadMem64 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr
+  )
+{
+  UINT64  Data;
+  UINT32  Data32;
+
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINT64))) {
+    return * (UINT64 *) Addr;
+  }
+  //
+  // Return unaligned data. Assume little endian.
+  //
+  Data32 = VmReadMem32 (VmPtr, Addr);
+  Data  = (UINT64) VmReadMem32 (VmPtr, Addr + sizeof (UINT32));
+  Data  = LShiftU64 (Data, 32) | Data32;
+  return Data;
+}
+
+
+/**
+  Given an address that EBC is going to read from or write to, return
+  an appropriate address that accounts for a gap in the stack.
+  The stack for this application looks like this (high addr on top)
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+  The EBC assumes that its arguments are at the top of its stack, which
+  is where the VM stack is really. Therefore if the EBC does memory
+  accesses into the VM stack area, then we need to convert the address
+  to point to the EBC entry point arguments area. Do this here.
+
+  @param  VmPtr             A Pointer to VM context.
+  @param  Addr              Address of interest
+
+  @return The unchanged address if it's not in the VM stack region. Otherwise,
+          adjust for the stack gap and return the modified address.
+
+**/
+UINTN
+ConvertStackAddr (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr
+  )
+{
+  ASSERT(((Addr < VmPtr->LowStackTop) || (Addr > VmPtr->HighStackBottom)));
+  return Addr;
+}
+
+
+/**
+  Read a natural value from memory. May or may not be aligned.
+
+  @param  VmPtr             current VM context
+  @param  Addr              the address to read from
+
+  @return The natural value at address Addr.
+
+**/
+UINTN
+VmReadMemN (
+  IN VM_CONTEXT    *VmPtr,
+  IN UINTN         Addr
+  )
+{
+  UINTN   Data;
+  volatile UINT32  Size;
+  UINT8   *FromPtr;
+  UINT8   *ToPtr;
+  //
+  // Convert the address if it's in the stack gap
+  //
+  Addr = ConvertStackAddr (VmPtr, Addr);
+  //
+  // Read direct if aligned
+  //
+  if (IS_ALIGNED (Addr, sizeof (UINTN))) {
+    return * (UINTN *) Addr;
+  }
+  //
+  // Return unaligned data
+  //
+  Data    = 0;
+  FromPtr = (UINT8 *) Addr;
+  ToPtr   = (UINT8 *) &Data;
+
+  for (Size = 0; Size < sizeof (Data); Size++) {
+    *ToPtr = *FromPtr;
+    ToPtr++;
+    FromPtr++;
+  }
+
+  return Data;
+}
+
+/**
+  Returns the version of the EBC virtual machine.
+
+  @return The 64-bit version of EBC virtual machine.
+
+**/
+UINT64
+GetVmVersion (
+  VOID
+  )
+{
+  return (UINT64) (((VM_MAJOR_VERSION & 0xFFFF) << 16) | ((VM_MINOR_VERSION & 0xFFFF)));
+}
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.h b/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.h
new file mode 100644
index 0000000000..bdc70b1d43
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcExecute.h
@@ -0,0 +1,339 @@
+/** @file
+  Header file for Virtual Machine support. Contains EBC defines that can
+  be of use to a disassembler for the most part. Also provides function
+  prototypes for VM functions.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _EBC_EXECUTE_H_
+#define _EBC_EXECUTE_H_
+
+//
+// VM major/minor version
+//
+#define VM_MAJOR_VERSION  1
+#define VM_MINOR_VERSION  0
+
+//
+// Macros to check and set alignment
+//
+#define ASSERT_ALIGNED(addr, size)  ASSERT (!((UINT32) (addr) & (size - 1)))
+#define IS_ALIGNED(addr, size)      !((UINT32) (addr) & (size - 1))
+
+//
+// Define a macro to get the operand. Then we can change it to be either a
+// direct read or have it call a function to read memory.
+//
+#define GETOPERANDS(pVM)  (UINT8) (*(UINT8 *) (pVM->Ip + 1))
+#define GETOPCODE(pVM)    (UINT8) (*(UINT8 *) pVM->Ip)
+
+//
+// Bit masks for opcode encodings
+//
+#define OPCODE_M_OPCODE       0x3F  // bits of interest for first level decode
+#define OPCODE_M_IMMDATA      0x80
+#define OPCODE_M_IMMDATA64    0x40
+#define OPCODE_M_64BIT        0x40  // for CMP
+#define OPCODE_M_RELADDR      0x10  // for CALL instruction
+#define OPCODE_M_CMPI32_DATA  0x80  // for CMPI
+#define OPCODE_M_CMPI64       0x40  // for CMPI 32 or 64 bit comparison
+#define OPERAND_M_MOVIN_N     0x80
+#define OPERAND_M_CMPI_INDEX  0x10
+
+//
+// Masks for instructions that encode presence of indexes for operand1 and/or
+// operand2.
+//
+#define OPCODE_M_IMMED_OP1  0x80
+#define OPCODE_M_IMMED_OP2  0x40
+
+//
+// Bit masks for operand encodings
+//
+#define OPERAND_M_INDIRECT1 0x08
+#define OPERAND_M_INDIRECT2 0x80
+#define OPERAND_M_OP1       0x07
+#define OPERAND_M_OP2       0x70
+
+//
+// Masks for data manipulation instructions
+//
+#define DATAMANIP_M_64      0x40  // 64-bit width operation
+#define DATAMANIP_M_IMMDATA 0x80
+
+//
+// For MOV instructions, need a mask for the opcode when immediate
+// data applies to R2.
+//
+#define OPCODE_M_IMMED_OP2  0x40
+
+//
+// The MOVI/MOVIn instructions use bit 6 of operands byte to indicate
+// if an index is present. Then bits 4 and 5 are used to indicate the width
+// of the move.
+//
+#define MOVI_M_IMMDATA    0x40
+#define MOVI_M_DATAWIDTH  0xC0
+#define MOVI_DATAWIDTH16  0x40
+#define MOVI_DATAWIDTH32  0x80
+#define MOVI_DATAWIDTH64  0xC0
+#define MOVI_M_MOVEWIDTH  0x30
+#define MOVI_MOVEWIDTH8   0x00
+#define MOVI_MOVEWIDTH16  0x10
+#define MOVI_MOVEWIDTH32  0x20
+#define MOVI_MOVEWIDTH64  0x30
+
+//
+// Masks for CALL instruction encodings
+//
+#define OPERAND_M_RELATIVE_ADDR 0x10
+#define OPERAND_M_NATIVE_CALL   0x20
+
+//
+// Masks for decoding push/pop instructions
+//
+#define PUSHPOP_M_IMMDATA 0x80  // opcode bit indicating immediate data
+#define PUSHPOP_M_64      0x40  // opcode bit indicating 64-bit operation
+//
+// Mask for operand of JMP instruction
+//
+#define JMP_M_RELATIVE    0x10
+#define JMP_M_CONDITIONAL 0x80
+#define JMP_M_CS          0x40
+
+//
+// Macros to determine if a given operand is indirect
+//
+#define OPERAND1_INDIRECT(op) ((op) & OPERAND_M_INDIRECT1)
+#define OPERAND2_INDIRECT(op) ((op) & OPERAND_M_INDIRECT2)
+
+//
+// Macros to extract the operands from second byte of instructions
+//
+#define OPERAND1_REGNUM(op)       ((op) & OPERAND_M_OP1)
+#define OPERAND2_REGNUM(op)       (((op) & OPERAND_M_OP2) >> 4)
+
+#define OPERAND1_CHAR(op)         ('0' + OPERAND1_REGNUM (op))
+#define OPERAND2_CHAR(op)         ('0' + OPERAND2_REGNUM (op))
+
+#define OPERAND1_REGDATA(pvm, op) pvm->Gpr[OPERAND1_REGNUM (op)]
+#define OPERAND2_REGDATA(pvm, op) pvm->Gpr[OPERAND2_REGNUM (op)]
+
+//
+// Condition masks usually for byte 1 encodings of code
+//
+#define CONDITION_M_CONDITIONAL 0x80
+#define CONDITION_M_CS          0x40
+
+//
+// Bits in the VM->StopFlags field
+//
+#define STOPFLAG_APP_DONE         0x0001
+#define STOPFLAG_BREAKPOINT       0x0002
+#define STOPFLAG_INVALID_BREAK    0x0004
+#define STOPFLAG_BREAK_ON_CALLEX  0x0008
+
+//
+// Masks for working with the VM flags register
+//
+#define VMFLAGS_CC        0x0001  // condition flag
+#define VMFLAGS_STEP      0x0002  // step instruction mode
+#define VMFLAGS_ALL_VALID (VMFLAGS_CC | VMFLAGS_STEP)
+
+//
+// Macros for operating on the VM flags register
+//
+#define VMFLAG_SET(pVM, Flag)   (pVM->Flags |= (Flag))
+#define VMFLAG_ISSET(pVM, Flag) ((pVM->Flags & (Flag)) ? 1 : 0)
+#define VMFLAG_CLEAR(pVM, Flag) (pVM->Flags &= ~(Flag))
+
+//
+// Debug macro
+//
+#define EBCMSG(s) gST->ConOut->OutputString (gST->ConOut, s)
+
+//
+// Define OPCODES
+//
+#define OPCODE_BREAK    0x00
+#define OPCODE_JMP      0x01
+#define OPCODE_JMP8     0x02
+#define OPCODE_CALL     0x03
+#define OPCODE_RET      0x04
+#define OPCODE_CMPEQ    0x05
+#define OPCODE_CMPLTE   0x06
+#define OPCODE_CMPGTE   0x07
+#define OPCODE_CMPULTE  0x08
+#define OPCODE_CMPUGTE  0x09
+#define OPCODE_NOT      0x0A
+#define OPCODE_NEG      0x0B
+#define OPCODE_ADD      0x0C
+#define OPCODE_SUB      0x0D
+#define OPCODE_MUL      0x0E
+#define OPCODE_MULU     0x0F
+#define OPCODE_DIV      0x10
+#define OPCODE_DIVU     0x11
+#define OPCODE_MOD      0x12
+#define OPCODE_MODU     0x13
+#define OPCODE_AND      0x14
+#define OPCODE_OR       0x15
+#define OPCODE_XOR      0x16
+#define OPCODE_SHL      0x17
+#define OPCODE_SHR      0x18
+#define OPCODE_ASHR     0x19
+#define OPCODE_EXTNDB   0x1A
+#define OPCODE_EXTNDW   0x1B
+#define OPCODE_EXTNDD   0x1C
+#define OPCODE_MOVBW    0x1D
+#define OPCODE_MOVWW    0x1E
+#define OPCODE_MOVDW    0x1F
+#define OPCODE_MOVQW    0x20
+#define OPCODE_MOVBD    0x21
+#define OPCODE_MOVWD    0x22
+#define OPCODE_MOVDD    0x23
+#define OPCODE_MOVQD    0x24
+#define OPCODE_MOVSNW   0x25  // Move signed natural with word index
+#define OPCODE_MOVSND   0x26  // Move signed natural with dword index
+//
+// #define OPCODE_27         0x27
+//
+#define OPCODE_MOVQQ    0x28  // Does this go away?
+#define OPCODE_LOADSP   0x29
+#define OPCODE_STORESP  0x2A
+#define OPCODE_PUSH     0x2B
+#define OPCODE_POP      0x2C
+#define OPCODE_CMPIEQ   0x2D
+#define OPCODE_CMPILTE  0x2E
+#define OPCODE_CMPIGTE  0x2F
+#define OPCODE_CMPIULTE 0x30
+#define OPCODE_CMPIUGTE 0x31
+#define OPCODE_MOVNW    0x32
+#define OPCODE_MOVND    0x33
+//
+// #define OPCODE_34         0x34
+//
+#define OPCODE_PUSHN  0x35
+#define OPCODE_POPN   0x36
+#define OPCODE_MOVI   0x37
+#define OPCODE_MOVIN  0x38
+#define OPCODE_MOVREL 0x39
+
+/**
+  Execute an EBC image from an entry point or from a published protocol.
+
+  @param  VmPtr             A pointer to a VM context.
+
+  @retval EFI_UNSUPPORTED   At least one of the opcodes is not supported.
+  @retval EFI_SUCCESS       All of the instructions are executed successfully.
+
+**/
+EFI_STATUS
+EbcExecute (
+  IN VM_CONTEXT *VmPtr
+  );
+
+
+
+/**
+  Returns the version of the EBC virtual machine.
+
+  @return The 64-bit version of EBC virtual machine.
+
+**/
+UINT64
+GetVmVersion (
+  VOID
+  );
+
+/**
+  Writes UINTN data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMemN (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINTN        Data
+  );
+
+/**
+  Writes 64-bit data to memory address.
+
+  This routine is called by the EBC data
+  movement instructions that write to memory. Since these writes
+  may be to the stack, which looks like (high address on top) this,
+
+  [EBC entry point arguments]
+  [VM stack]
+  [EBC stack]
+
+  we need to detect all attempts to write to the EBC entry point argument
+  stack area and adjust the address (which will initially point into the
+  VM stack) to point into the EBC entry point arguments.
+
+  @param  VmPtr             A pointer to a VM context.
+  @param  Addr              Address to write to.
+  @param  Data              Value to write to Addr.
+
+  @retval EFI_SUCCESS       The instruction is executed successfully.
+  @retval Other             Some error occurs when writing data to the address.
+
+**/
+EFI_STATUS
+VmWriteMem64 (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        Addr,
+  IN UINT64       Data
+  );
+
+/**
+  Given a pointer to a new VM context, execute one or more instructions. This
+  function is only used for test purposes via the EBC VM test protocol.
+
+  @param  This              A pointer to the EFI_EBC_VM_TEST_PROTOCOL structure.
+  @param  VmPtr             A pointer to a VM context.
+  @param  InstructionCount  A pointer to a UINTN value holding the number of
+                            instructions to execute. If it holds value of 0,
+                            then the instruction to be executed is 1.
+
+  @retval EFI_UNSUPPORTED   At least one of the opcodes is not supported.
+  @retval EFI_SUCCESS       All of the instructions are executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcExecuteInstructions (
+  IN EFI_EBC_VM_TEST_PROTOCOL *This,
+  IN VM_CONTEXT               *VmPtr,
+  IN OUT UINTN                *InstructionCount
+  );
+
+#endif // ifndef _EBC_EXECUTE_H_
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.c b/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.c
new file mode 100644
index 0000000000..549e0dd8dc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.c
@@ -0,0 +1,1406 @@
+/** @file
+  Top level module for the EBC virtual machine implementation.
+  Provides auxiliary support routines for the VM. That is, routines
+  that are not particularly related to VM execution of EBC instructions.
+
+Copyright (c) 2006 - 2011, 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 "EbcInt.h"
+#include "EbcExecute.h"
+
+//
+// We'll keep track of all thunks we create in a linked list. Each
+// thunk is tied to an image handle, so we have a linked list of
+// image handles, with each having a linked list of thunks allocated
+// to that image handle.
+//
+typedef struct _EBC_THUNK_LIST EBC_THUNK_LIST;
+struct _EBC_THUNK_LIST {
+  VOID            *ThunkBuffer;
+  EBC_THUNK_LIST  *Next;
+};
+
+typedef struct _EBC_IMAGE_LIST EBC_IMAGE_LIST;
+struct _EBC_IMAGE_LIST {
+  EBC_IMAGE_LIST  *Next;
+  EFI_HANDLE      ImageHandle;
+  EBC_THUNK_LIST  *ThunkList;
+};
+
+/**
+  This routine is called by the core when an image is being unloaded from
+  memory. Basically we now have the opportunity to do any necessary cleanup.
+  Typically this will include freeing any memory allocated for thunk-creation.
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  ImageHandle           Handle of image for which the thunk is being
+                                created.
+
+  @retval EFI_INVALID_PARAMETER The ImageHandle passed in was not found in the
+                                internal list of EBC image handles.
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcUnloadImage (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EFI_HANDLE         ImageHandle
+  );
+
+/**
+  This is the top-level routine plugged into the EBC protocol. Since thunks
+  are very processor-specific, from here we dispatch directly to the very
+  processor-specific routine EbcCreateThunks().
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  ImageHandle           Handle of image for which the thunk is being
+                                created. The EBC interpreter may use this to
+                                keep track of any resource allocations
+                                performed in loading and executing the image.
+  @param  EbcEntryPoint         Address of the actual EBC entry point or
+                                protocol service the thunk should call.
+  @param  Thunk                 Returned pointer to a thunk created.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER Image entry point is not 2-byte aligned.
+  @retval EFI_OUT_OF_RESOURCES  Memory could not be allocated for the thunk.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcCreateThunk (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EFI_HANDLE         ImageHandle,
+  IN VOID               *EbcEntryPoint,
+  OUT VOID              **Thunk
+  );
+
+/**
+  Called to get the version of the interpreter.
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  Version               Pointer to where to store the returned version
+                                of the interpreter.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER Version pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcGetVersion (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN OUT UINT64         *Version
+  );
+
+/**
+  To install default Callback function for the VM interpreter.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval Others                Some error occurs when creating periodic event.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeEbcCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This
+  );
+
+/**
+  The default Exception Callback for the VM interpreter.
+  In this function, we report status code, and print debug information
+  about EBC_CONTEXT, then dead loop.
+
+  @param  InterruptType          Interrupt type.
+  @param  SystemContext          EBC system context.
+
+**/
+VOID
+EFIAPI
+CommonEbcExceptionHandler (
+  IN EFI_EXCEPTION_TYPE   InterruptType,
+  IN EFI_SYSTEM_CONTEXT   SystemContext
+  );
+
+/**
+  The periodic callback function for EBC VM interpreter, which is used
+  to support the EFI debug support protocol.
+
+  @param  Event                  The Periodic Callback Event.
+  @param  Context                It should be the address of VM_CONTEXT pointer.
+
+**/
+VOID
+EFIAPI
+EbcPeriodicNotifyFunction (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  );
+
+/**
+  The VM interpreter calls this function on a periodic basis to support
+  the EFI debug support protocol.
+
+  @param  VmPtr                  Pointer to a VM context for passing info to the
+                                 debugger.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugPeriodic (
+  IN VM_CONTEXT *VmPtr
+  );
+
+//
+// These two functions and the  GUID are used to produce an EBC test protocol.
+// This functionality is definitely not required for execution.
+//
+/**
+  Produces an EBC VM test protocol that can be used for regression tests.
+
+  @param  IHandle                Handle on which to install the protocol.
+
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+InitEbcVmTestProtocol (
+  IN EFI_HANDLE     *IHandle
+  );
+
+/**
+  Returns the EFI_UNSUPPORTED Status.
+
+  @return EFI_UNSUPPORTED  This function always return EFI_UNSUPPORTED status.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcVmTestUnsupported (
+  VOID
+  );
+
+/**
+  Registers a callback function that the EBC interpreter calls to flush the
+  processor instruction cache following creation of thunks.
+
+  @param  This        A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  Flush       Pointer to a function of type EBC_ICACH_FLUSH.
+
+  @retval EFI_SUCCESS The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcRegisterICacheFlush (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EBC_ICACHE_FLUSH   Flush
+  );
+
+/**
+  This EBC debugger protocol service is called by the debug agent
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the
+                                maximum supported processor index is returned.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugGetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL          *This,
+  OUT UINTN                              *MaxProcessorIndex
+  );
+
+/**
+  This protocol service is called by the debug agent to register a function
+  for us to call on a periodic basis.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  PeriodicCallback      A pointer to a function of type
+                                PERIODIC_CALLBACK that is the main periodic
+                                entry point of the debug agent. It receives as a
+                                parameter a pointer to the full context of the
+                                interrupted execution thread.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a
+                                callback function was previously registered.
+  @retval EFI_INVALID_PARAMETER Null PeriodicCallback parameter when no
+                                callback function was previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugRegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This,
+  IN UINTN                       ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK       PeriodicCallback
+  );
+
+/**
+  This protocol service is called by the debug agent to register a function
+  for us to call when we detect an exception.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  ExceptionCallback     A pointer to a function of type
+                                EXCEPTION_CALLBACK that is called when the
+                                processor exception specified by ExceptionType
+                                occurs. Passing NULL unregisters any previously
+                                registered function associated with
+                                ExceptionType.
+  @param  ExceptionType         Specifies which processor exception to hook.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL ExceptionCallback parameter when a
+                                callback function was previously registered.
+  @retval EFI_INVALID_PARAMETER ExceptionType parameter is negative or exceeds
+                                MAX_EBC_EXCEPTION.
+  @retval EFI_INVALID_PARAMETER Null ExceptionCallback parameter when no
+                                callback function was previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugRegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This,
+  IN UINTN                       ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK      ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE          ExceptionType
+  );
+
+/**
+  This EBC debugger protocol service is called by the debug agent.  Required
+  for DebugSupport compliance but is only stubbed out for EBC.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  Start                 StartSpecifies the physical base of the memory
+                                range to be invalidated.
+  @param  Length                Specifies the minimum number of bytes in the
+                                processor's instruction cache to invalidate.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugInvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL          *This,
+  IN UINTN                               ProcessorIndex,
+  IN VOID                                *Start,
+  IN UINT64                              Length
+  );
+
+//
+// We have one linked list of image handles for the whole world. Since
+// there should only be one interpreter, make them global. They must
+// also be global since the execution of an EBC image does not provide
+// a This pointer.
+//
+EBC_IMAGE_LIST         *mEbcImageList = NULL;
+
+//
+// Callback function to flush the icache after thunk creation
+//
+EBC_ICACHE_FLUSH       mEbcICacheFlush;
+
+//
+// These get set via calls by the debug agent
+//
+EFI_PERIODIC_CALLBACK  mDebugPeriodicCallback = NULL;
+EFI_EXCEPTION_CALLBACK mDebugExceptionCallback[MAX_EBC_EXCEPTION + 1] = {NULL};
+
+VOID                   *mStackBuffer[MAX_STACK_NUM];
+EFI_HANDLE             mStackBufferIndex[MAX_STACK_NUM];
+UINTN                  mStackNum = 0;
+
+//
+// Event for Periodic callback
+//
+EFI_EVENT              mEbcPeriodicEvent;
+VM_CONTEXT             *mVmPtr = NULL;
+
+
+/**
+  Initializes the VM EFI interface.  Allocates memory for the VM interface
+  and registers the VM protocol.
+
+  @param  ImageHandle            EFI image handle.
+  @param  SystemTable            Pointer to the EFI system table.
+
+  @return Standard EFI status code.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeEbcDriver (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_EBC_PROTOCOL            *EbcProtocol;
+  EFI_EBC_PROTOCOL            *OldEbcProtocol;
+  EFI_STATUS                  Status;
+  EFI_DEBUG_SUPPORT_PROTOCOL  *EbcDebugProtocol;
+  EFI_HANDLE                  *HandleBuffer;
+  UINTN                       NumHandles;
+  UINTN                       Index;
+  BOOLEAN                     Installed;
+
+  EbcProtocol      = NULL;
+  EbcDebugProtocol = NULL;
+
+  //
+  // Allocate memory for our protocol. Then fill in the blanks.
+  //
+  EbcProtocol = AllocatePool (sizeof (EFI_EBC_PROTOCOL));
+
+  if (EbcProtocol == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  EbcProtocol->CreateThunk          = EbcCreateThunk;
+  EbcProtocol->UnloadImage          = EbcUnloadImage;
+  EbcProtocol->RegisterICacheFlush  = EbcRegisterICacheFlush;
+  EbcProtocol->GetVersion           = EbcGetVersion;
+  mEbcICacheFlush                   = NULL;
+
+  //
+  // Find any already-installed EBC protocols and uninstall them
+  //
+  Installed     = FALSE;
+  HandleBuffer  = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiEbcProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &HandleBuffer
+                  );
+  if (Status == EFI_SUCCESS) {
+    //
+    // Loop through the handles
+    //
+    for (Index = 0; Index < NumHandles; Index++) {
+      Status = gBS->HandleProtocol (
+                      HandleBuffer[Index],
+                      &gEfiEbcProtocolGuid,
+                      (VOID **) &OldEbcProtocol
+                      );
+      if (Status == EFI_SUCCESS) {
+        if (gBS->ReinstallProtocolInterface (
+                  HandleBuffer[Index],
+                  &gEfiEbcProtocolGuid,
+                  OldEbcProtocol,
+                  EbcProtocol
+                  ) == EFI_SUCCESS) {
+          Installed = TRUE;
+        }
+      }
+    }
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool (HandleBuffer);
+    HandleBuffer = NULL;
+  }
+  //
+  // Add the protocol so someone can locate us if we haven't already.
+  //
+  if (!Installed) {
+    Status = gBS->InstallProtocolInterface (
+                    &ImageHandle,
+                    &gEfiEbcProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    EbcProtocol
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (EbcProtocol);
+      return Status;
+    }
+  }
+
+  Status = InitEBCStack();
+  if (EFI_ERROR(Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Allocate memory for our debug protocol. Then fill in the blanks.
+  //
+  EbcDebugProtocol = AllocatePool (sizeof (EFI_DEBUG_SUPPORT_PROTOCOL));
+
+  if (EbcDebugProtocol == NULL) {
+    goto ErrorExit;
+  }
+
+  EbcDebugProtocol->Isa                         = IsaEbc;
+  EbcDebugProtocol->GetMaximumProcessorIndex    = EbcDebugGetMaximumProcessorIndex;
+  EbcDebugProtocol->RegisterPeriodicCallback    = EbcDebugRegisterPeriodicCallback;
+  EbcDebugProtocol->RegisterExceptionCallback   = EbcDebugRegisterExceptionCallback;
+  EbcDebugProtocol->InvalidateInstructionCache  = EbcDebugInvalidateInstructionCache;
+
+  //
+  // Add the protocol so the debug agent can find us
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &ImageHandle,
+                  &gEfiDebugSupportProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  EbcDebugProtocol
+                  );
+  //
+  // This is recoverable, so free the memory and continue.
+  //
+  if (EFI_ERROR (Status)) {
+    FreePool (EbcDebugProtocol);
+    goto ErrorExit;
+  }
+  //
+  // Install EbcDebugSupport Protocol Successfully
+  // Now we need to initialize the Ebc default Callback
+  //
+  Status = InitializeEbcCallback (EbcDebugProtocol);
+
+  //
+  // Produce a VM test interface protocol. Not required for execution.
+  //
+  DEBUG_CODE_BEGIN ();
+    InitEbcVmTestProtocol (&ImageHandle);
+  DEBUG_CODE_END ();
+
+  return EFI_SUCCESS;
+
+ErrorExit:
+  FreeEBCStack();
+  HandleBuffer  = NULL;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiEbcProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &HandleBuffer
+                  );
+  if (Status == EFI_SUCCESS) {
+    //
+    // Loop through the handles
+    //
+    for (Index = 0; Index < NumHandles; Index++) {
+      Status = gBS->HandleProtocol (
+                      HandleBuffer[Index],
+                      &gEfiEbcProtocolGuid,
+                      (VOID **) &OldEbcProtocol
+                      );
+      if (Status == EFI_SUCCESS) {
+        gBS->UninstallProtocolInterface (
+               HandleBuffer[Index],
+               &gEfiEbcProtocolGuid,
+               OldEbcProtocol
+               );
+      }
+    }
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool (HandleBuffer);
+    HandleBuffer = NULL;
+  }
+
+  FreePool (EbcProtocol);
+
+  return Status;
+}
+
+
+/**
+  This is the top-level routine plugged into the EBC protocol. Since thunks
+  are very processor-specific, from here we dispatch directly to the very
+  processor-specific routine EbcCreateThunks().
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  ImageHandle           Handle of image for which the thunk is being
+                                created. The EBC interpreter may use this to
+                                keep track of any resource allocations
+                                performed in loading and executing the image.
+  @param  EbcEntryPoint         Address of the actual EBC entry point or
+                                protocol service the thunk should call.
+  @param  Thunk                 Returned pointer to a thunk created.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER Image entry point is not 2-byte aligned.
+  @retval EFI_OUT_OF_RESOURCES  Memory could not be allocated for the thunk.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcCreateThunk (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EFI_HANDLE         ImageHandle,
+  IN VOID               *EbcEntryPoint,
+  OUT VOID              **Thunk
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EbcCreateThunks (
+            ImageHandle,
+            EbcEntryPoint,
+            Thunk,
+            FLAG_THUNK_ENTRY_POINT
+            );
+  return Status;
+}
+
+
+/**
+  This EBC debugger protocol service is called by the debug agent
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  MaxProcessorIndex     Pointer to a caller-allocated UINTN in which the
+                                maximum supported processor index is returned.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugGetMaximumProcessorIndex (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL          *This,
+  OUT UINTN                              *MaxProcessorIndex
+  )
+{
+  *MaxProcessorIndex = 0;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This protocol service is called by the debug agent to register a function
+  for us to call on a periodic basis.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  PeriodicCallback      A pointer to a function of type
+                                PERIODIC_CALLBACK that is the main periodic
+                                entry point of the debug agent. It receives as a
+                                parameter a pointer to the full context of the
+                                interrupted execution thread.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL PeriodicCallback parameter when a
+                                callback function was previously registered.
+  @retval EFI_INVALID_PARAMETER Null PeriodicCallback parameter when no
+                                callback function was previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugRegisterPeriodicCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This,
+  IN UINTN                       ProcessorIndex,
+  IN EFI_PERIODIC_CALLBACK       PeriodicCallback
+  )
+{
+  if ((mDebugPeriodicCallback == NULL) && (PeriodicCallback == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((mDebugPeriodicCallback != NULL) && (PeriodicCallback != NULL)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  mDebugPeriodicCallback = PeriodicCallback;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This protocol service is called by the debug agent to register a function
+  for us to call when we detect an exception.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  ExceptionCallback     A pointer to a function of type
+                                EXCEPTION_CALLBACK that is called when the
+                                processor exception specified by ExceptionType
+                                occurs. Passing NULL unregisters any previously
+                                registered function associated with
+                                ExceptionType.
+  @param  ExceptionType         Specifies which processor exception to hook.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_ALREADY_STARTED   Non-NULL ExceptionCallback parameter when a
+                                callback function was previously registered.
+  @retval EFI_INVALID_PARAMETER ExceptionType parameter is negative or exceeds
+                                MAX_EBC_EXCEPTION.
+  @retval EFI_INVALID_PARAMETER Null ExceptionCallback parameter when no
+                                callback function was previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugRegisterExceptionCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This,
+  IN UINTN                       ProcessorIndex,
+  IN EFI_EXCEPTION_CALLBACK      ExceptionCallback,
+  IN EFI_EXCEPTION_TYPE          ExceptionType
+  )
+{
+  if ((ExceptionType < 0) || (ExceptionType > MAX_EBC_EXCEPTION)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((mDebugExceptionCallback[ExceptionType] == NULL) && (ExceptionCallback == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((mDebugExceptionCallback[ExceptionType] != NULL) && (ExceptionCallback != NULL)) {
+    return EFI_ALREADY_STARTED;
+  }
+  mDebugExceptionCallback[ExceptionType] = ExceptionCallback;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This EBC debugger protocol service is called by the debug agent.  Required
+  for DebugSupport compliance but is only stubbed out for EBC.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+  @param  ProcessorIndex        Specifies which processor the callback function
+                                applies to.
+  @param  Start                 StartSpecifies the physical base of the memory
+                                range to be invalidated.
+  @param  Length                Specifies the minimum number of bytes in the
+                                processor's instruction cache to invalidate.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugInvalidateInstructionCache (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL          *This,
+  IN UINTN                               ProcessorIndex,
+  IN VOID                                *Start,
+  IN UINT64                              Length
+  )
+{
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The VM interpreter calls this function when an exception is detected.
+
+  @param  ExceptionType          Specifies the processor exception detected.
+  @param  ExceptionFlags         Specifies the exception context.
+  @param  VmPtr                  Pointer to a VM context for passing info to the
+                                 EFI debugger.
+
+  @retval EFI_SUCCESS            This function completed successfully.
+
+**/
+EFI_STATUS
+EbcDebugSignalException (
+  IN EFI_EXCEPTION_TYPE                   ExceptionType,
+  IN EXCEPTION_FLAGS                      ExceptionFlags,
+  IN VM_CONTEXT                           *VmPtr
+  )
+{
+  EFI_SYSTEM_CONTEXT_EBC  EbcContext;
+  EFI_SYSTEM_CONTEXT      SystemContext;
+
+  ASSERT ((ExceptionType >= 0) && (ExceptionType <= MAX_EBC_EXCEPTION));
+  //
+  // Save the exception in the context passed in
+  //
+  VmPtr->ExceptionFlags |= ExceptionFlags;
+  VmPtr->LastException = (UINTN) ExceptionType;
+  //
+  // If it's a fatal exception, then flag it in the VM context in case an
+  // attached debugger tries to return from it.
+  //
+  if ((ExceptionFlags & EXCEPTION_FLAG_FATAL) != 0) {
+    VmPtr->StopFlags |= STOPFLAG_APP_DONE;
+  }
+
+  //
+  // If someone's registered for exception callbacks, then call them.
+  //
+  // EBC driver will register default exception callback to report the
+  // status code via the status code API
+  //
+  if (mDebugExceptionCallback[ExceptionType] != NULL) {
+
+    //
+    // Initialize the context structure
+    //
+    EbcContext.R0                   = (UINT64) VmPtr->Gpr[0];
+    EbcContext.R1                   = (UINT64) VmPtr->Gpr[1];
+    EbcContext.R2                   = (UINT64) VmPtr->Gpr[2];
+    EbcContext.R3                   = (UINT64) VmPtr->Gpr[3];
+    EbcContext.R4                   = (UINT64) VmPtr->Gpr[4];
+    EbcContext.R5                   = (UINT64) VmPtr->Gpr[5];
+    EbcContext.R6                   = (UINT64) VmPtr->Gpr[6];
+    EbcContext.R7                   = (UINT64) VmPtr->Gpr[7];
+    EbcContext.Ip                   = (UINT64)(UINTN)VmPtr->Ip;
+    EbcContext.Flags                = VmPtr->Flags;
+    EbcContext.ControlFlags         = 0;
+    SystemContext.SystemContextEbc  = &EbcContext;
+
+    mDebugExceptionCallback[ExceptionType] (ExceptionType, SystemContext);
+    //
+    // Restore the context structure and continue to execute
+    //
+    VmPtr->Gpr[0]  = EbcContext.R0;
+    VmPtr->Gpr[1]  = EbcContext.R1;
+    VmPtr->Gpr[2]  = EbcContext.R2;
+    VmPtr->Gpr[3]  = EbcContext.R3;
+    VmPtr->Gpr[4]  = EbcContext.R4;
+    VmPtr->Gpr[5]  = EbcContext.R5;
+    VmPtr->Gpr[6]  = EbcContext.R6;
+    VmPtr->Gpr[7]  = EbcContext.R7;
+    VmPtr->Ip    = (VMIP)(UINTN)EbcContext.Ip;
+    VmPtr->Flags = EbcContext.Flags;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  To install default Callback function for the VM interpreter.
+
+  @param  This                  A pointer to the EFI_DEBUG_SUPPORT_PROTOCOL
+                                instance.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval Others                Some error occurs when creating periodic event.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeEbcCallback (
+  IN EFI_DEBUG_SUPPORT_PROTOCOL  *This
+  )
+{
+  INTN       Index;
+  EFI_STATUS Status;
+
+  //
+  // For ExceptionCallback
+  //
+  for (Index = 0; Index <= MAX_EBC_EXCEPTION; Index++) {
+    EbcDebugRegisterExceptionCallback (
+      This,
+      0,
+      CommonEbcExceptionHandler,
+      Index
+      );
+  }
+
+  //
+  // For PeriodicCallback
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  EbcPeriodicNotifyFunction,
+                  &mVmPtr,
+                  &mEbcPeriodicEvent
+                  );
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (
+                  mEbcPeriodicEvent,
+                  TimerPeriodic,
+                  EBC_VM_PERIODIC_CALLBACK_RATE
+                  );
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The default Exception Callback for the VM interpreter.
+  In this function, we report status code, and print debug information
+  about EBC_CONTEXT, then dead loop.
+
+  @param  InterruptType          Interrupt type.
+  @param  SystemContext          EBC system context.
+
+**/
+VOID
+EFIAPI
+CommonEbcExceptionHandler (
+  IN EFI_EXCEPTION_TYPE   InterruptType,
+  IN EFI_SYSTEM_CONTEXT   SystemContext
+  )
+{
+  //
+  // We print debug information to let user know what happen.
+  //
+  DEBUG ((
+    EFI_D_ERROR,
+    "EBC Interrupter Version - 0x%016lx\n",
+    (UINT64) (((VM_MAJOR_VERSION & 0xFFFF) << 16) | ((VM_MINOR_VERSION & 0xFFFF)))
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "Exception Type - 0x%016lx\n",
+    (UINT64)(UINTN)InterruptType
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  R0 - 0x%016lx, R1 - 0x%016lx\n",
+    SystemContext.SystemContextEbc->R0,
+    SystemContext.SystemContextEbc->R1
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  R2 - 0x%016lx, R3 - 0x%016lx\n",
+    SystemContext.SystemContextEbc->R2,
+    SystemContext.SystemContextEbc->R3
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  R4 - 0x%016lx, R5 - 0x%016lx\n",
+    SystemContext.SystemContextEbc->R4,
+    SystemContext.SystemContextEbc->R5
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  R6 - 0x%016lx, R7 - 0x%016lx\n",
+    SystemContext.SystemContextEbc->R6,
+    SystemContext.SystemContextEbc->R7
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  Flags - 0x%016lx\n",
+    SystemContext.SystemContextEbc->Flags
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  ControlFlags - 0x%016lx\n",
+    SystemContext.SystemContextEbc->ControlFlags
+    ));
+  DEBUG ((
+    EFI_D_ERROR,
+    "  Ip - 0x%016lx\n\n",
+    SystemContext.SystemContextEbc->Ip
+    ));
+
+  //
+  // We deadloop here to make it easy to debug this issue.
+  //
+  CpuDeadLoop ();
+
+  return ;
+}
+
+
+/**
+  The periodic callback function for EBC VM interpreter, which is used
+  to support the EFI debug support protocol.
+
+  @param  Event                  The Periodic Callback Event.
+  @param  Context                It should be the address of VM_CONTEXT pointer.
+
+**/
+VOID
+EFIAPI
+EbcPeriodicNotifyFunction (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  VM_CONTEXT *VmPtr;
+
+  VmPtr = *(VM_CONTEXT **)Context;
+
+  if (VmPtr != NULL) {
+    EbcDebugPeriodic (VmPtr);
+  }
+
+  return ;
+}
+
+
+/**
+  The VM interpreter calls this function on a periodic basis to support
+  the EFI debug support protocol.
+
+  @param  VmPtr                  Pointer to a VM context for passing info to the
+                                 debugger.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcDebugPeriodic (
+  IN VM_CONTEXT *VmPtr
+  )
+{
+  EFI_SYSTEM_CONTEXT_EBC   EbcContext;
+  EFI_SYSTEM_CONTEXT       SystemContext;
+
+  //
+  // If someone's registered for periodic callbacks, then call them.
+  //
+  if (mDebugPeriodicCallback != NULL) {
+
+    //
+    // Initialize the context structure
+    //
+    EbcContext.R0                   = (UINT64) VmPtr->Gpr[0];
+    EbcContext.R1                   = (UINT64) VmPtr->Gpr[1];
+    EbcContext.R2                   = (UINT64) VmPtr->Gpr[2];
+    EbcContext.R3                   = (UINT64) VmPtr->Gpr[3];
+    EbcContext.R4                   = (UINT64) VmPtr->Gpr[4];
+    EbcContext.R5                   = (UINT64) VmPtr->Gpr[5];
+    EbcContext.R6                   = (UINT64) VmPtr->Gpr[6];
+    EbcContext.R7                   = (UINT64) VmPtr->Gpr[7];
+    EbcContext.Ip                   = (UINT64)(UINTN)VmPtr->Ip;
+    EbcContext.Flags                = VmPtr->Flags;
+    EbcContext.ControlFlags         = 0;
+    SystemContext.SystemContextEbc  = &EbcContext;
+
+    mDebugPeriodicCallback (SystemContext);
+
+    //
+    // Restore the context structure and continue to execute
+    //
+    VmPtr->Gpr[0]  = EbcContext.R0;
+    VmPtr->Gpr[1]  = EbcContext.R1;
+    VmPtr->Gpr[2]  = EbcContext.R2;
+    VmPtr->Gpr[3]  = EbcContext.R3;
+    VmPtr->Gpr[4]  = EbcContext.R4;
+    VmPtr->Gpr[5]  = EbcContext.R5;
+    VmPtr->Gpr[6]  = EbcContext.R6;
+    VmPtr->Gpr[7]  = EbcContext.R7;
+    VmPtr->Ip    = (VMIP)(UINTN)EbcContext.Ip;
+    VmPtr->Flags = EbcContext.Flags;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This routine is called by the core when an image is being unloaded from
+  memory. Basically we now have the opportunity to do any necessary cleanup.
+  Typically this will include freeing any memory allocated for thunk-creation.
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  ImageHandle           Handle of image for which the thunk is being
+                                created.
+
+  @retval EFI_INVALID_PARAMETER The ImageHandle passed in was not found in the
+                                internal list of EBC image handles.
+  @retval EFI_SUCCESS           The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcUnloadImage (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EFI_HANDLE         ImageHandle
+  )
+{
+  EBC_THUNK_LIST  *ThunkList;
+  EBC_THUNK_LIST  *NextThunkList;
+  EBC_IMAGE_LIST  *ImageList;
+  EBC_IMAGE_LIST  *PrevImageList;
+  //
+  // First go through our list of known image handles and see if we've already
+  // created an image list element for this image handle.
+  //
+  ReturnEBCStackByHandle(ImageHandle);
+  PrevImageList = NULL;
+  for (ImageList = mEbcImageList; ImageList != NULL; ImageList = ImageList->Next) {
+    if (ImageList->ImageHandle == ImageHandle) {
+      break;
+    }
+    //
+    // Save the previous so we can connect the lists when we remove this one
+    //
+    PrevImageList = ImageList;
+  }
+
+  if (ImageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Free up all the thunk buffers and thunks list elements for this image
+  // handle.
+  //
+  ThunkList = ImageList->ThunkList;
+  while (ThunkList != NULL) {
+    NextThunkList = ThunkList->Next;
+    FreePool (ThunkList->ThunkBuffer);
+    FreePool (ThunkList);
+    ThunkList = NextThunkList;
+  }
+  //
+  // Now remove this image list element from the chain
+  //
+  if (PrevImageList == NULL) {
+    //
+    // Remove from head
+    //
+    mEbcImageList = ImageList->Next;
+  } else {
+    PrevImageList->Next = ImageList->Next;
+  }
+  //
+  // Now free up the image list element
+  //
+  FreePool (ImageList);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Add a thunk to our list of thunks for a given image handle.
+  Also flush the instruction cache since we've written thunk code
+  to memory that will be executed eventually.
+
+  @param  ImageHandle            The image handle to which the thunk is tied.
+  @param  ThunkBuffer            The buffer that has been created/allocated.
+  @param  ThunkSize              The size of the thunk memory allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+EbcAddImageThunk (
+  IN EFI_HANDLE      ImageHandle,
+  IN VOID            *ThunkBuffer,
+  IN UINT32          ThunkSize
+  )
+{
+  EBC_THUNK_LIST  *ThunkList;
+  EBC_IMAGE_LIST  *ImageList;
+  EFI_STATUS      Status;
+
+  //
+  // It so far so good, then flush the instruction cache
+  //
+  if (mEbcICacheFlush != NULL) {
+    Status = mEbcICacheFlush ((EFI_PHYSICAL_ADDRESS) (UINTN) ThunkBuffer, ThunkSize);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  //
+  // Go through our list of known image handles and see if we've already
+  // created a image list element for this image handle.
+  //
+  for (ImageList = mEbcImageList; ImageList != NULL; ImageList = ImageList->Next) {
+    if (ImageList->ImageHandle == ImageHandle) {
+      break;
+    }
+  }
+
+  if (ImageList == NULL) {
+    //
+    // Allocate a new one
+    //
+    ImageList = AllocatePool (sizeof (EBC_IMAGE_LIST));
+
+    if (ImageList == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ImageList->ThunkList    = NULL;
+    ImageList->ImageHandle  = ImageHandle;
+    ImageList->Next         = mEbcImageList;
+    mEbcImageList           = ImageList;
+  }
+  //
+  // Ok, now create a new thunk element to add to the list
+  //
+  ThunkList = AllocatePool (sizeof (EBC_THUNK_LIST));
+
+  if (ThunkList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Add it to the head of the list
+  //
+  ThunkList->Next         = ImageList->ThunkList;
+  ThunkList->ThunkBuffer  = ThunkBuffer;
+  ImageList->ThunkList    = ThunkList;
+  return EFI_SUCCESS;
+}
+
+/**
+  Registers a callback function that the EBC interpreter calls to flush the
+  processor instruction cache following creation of thunks.
+
+  @param  This        A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  Flush       Pointer to a function of type EBC_ICACH_FLUSH.
+
+  @retval EFI_SUCCESS The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcRegisterICacheFlush (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN EBC_ICACHE_FLUSH   Flush
+  )
+{
+  mEbcICacheFlush = Flush;
+  return EFI_SUCCESS;
+}
+
+/**
+  Called to get the version of the interpreter.
+
+  @param  This                  A pointer to the EFI_EBC_PROTOCOL instance.
+  @param  Version               Pointer to where to store the returned version
+                                of the interpreter.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER Version pointer is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcGetVersion (
+  IN EFI_EBC_PROTOCOL   *This,
+  IN OUT UINT64         *Version
+  )
+{
+  if (Version == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Version = GetVmVersion ();
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns the stack index and buffer assosicated with the Handle parameter.
+
+  @param  Handle                The EFI handle as the index to the EBC stack.
+  @param  StackBuffer           A pointer to hold the returned stack buffer.
+  @param  BufferIndex           A pointer to hold the returned stack index.
+
+  @retval EFI_OUT_OF_RESOURCES  The Handle parameter does not correspond to any
+                                existing EBC stack.
+  @retval EFI_SUCCESS           The stack index and buffer were found and
+                                returned to the caller.
+
+**/
+EFI_STATUS
+GetEBCStack(
+  IN  EFI_HANDLE Handle,
+  OUT VOID       **StackBuffer,
+  OUT UINTN      *BufferIndex
+  )
+{
+  UINTN   Index;
+  EFI_TPL OldTpl;
+  OldTpl = gBS->RaiseTPL(TPL_HIGH_LEVEL);
+  for (Index = 0; Index < mStackNum; Index ++) {
+    if (mStackBufferIndex[Index] == NULL) {
+      mStackBufferIndex[Index] = Handle;
+      break;
+    }
+  }
+  gBS->RestoreTPL(OldTpl);
+  if (Index == mStackNum) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  *BufferIndex = Index;
+  *StackBuffer = mStackBuffer[Index];
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns from the EBC stack by stack Index.
+
+  @param  Index        Specifies which EBC stack to return from.
+
+  @retval EFI_SUCCESS  The function completed successfully.
+
+**/
+EFI_STATUS
+ReturnEBCStack(
+  IN UINTN Index
+  )
+{
+  mStackBufferIndex[Index] = NULL;
+  return EFI_SUCCESS;
+}
+
+/**
+  Returns from the EBC stack associated with the Handle parameter.
+
+  @param  Handle      Specifies the EFI handle to find the EBC stack with.
+
+  @retval EFI_SUCCESS The function completed successfully.
+
+**/
+EFI_STATUS
+ReturnEBCStackByHandle(
+  IN EFI_HANDLE Handle
+  )
+{
+  UINTN Index;
+  for (Index = 0; Index < mStackNum; Index ++) {
+    if (mStackBufferIndex[Index] == Handle) {
+      break;
+    }
+  }
+  if (Index == mStackNum) {
+    return EFI_NOT_FOUND;
+  }
+  mStackBufferIndex[Index] = NULL;
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates memory to hold all the EBC stacks.
+
+  @retval EFI_SUCCESS          The EBC stacks were allocated successfully.
+  @retval EFI_OUT_OF_RESOURCES Not enough memory available for EBC stacks.
+
+**/
+EFI_STATUS
+InitEBCStack (
+  VOID
+  )
+{
+  for (mStackNum = 0; mStackNum < MAX_STACK_NUM; mStackNum ++) {
+    mStackBuffer[mStackNum] = AllocatePool(STACK_POOL_SIZE);
+    mStackBufferIndex[mStackNum] = NULL;
+    if (mStackBuffer[mStackNum] == NULL) {
+      break;
+    }
+  }
+  if (mStackNum == 0) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Free all EBC stacks allocated before.
+
+  @retval EFI_SUCCESS   All the EBC stacks were freed.
+
+**/
+EFI_STATUS
+FreeEBCStack(
+  VOID
+  )
+{
+  UINTN Index;
+  for (Index = 0; Index < mStackNum; Index ++) {
+    FreePool(mStackBuffer[Index]);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Produces an EBC VM test protocol that can be used for regression tests.
+
+  @param  IHandle                Handle on which to install the protocol.
+
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+InitEbcVmTestProtocol (
+  IN EFI_HANDLE     *IHandle
+  )
+{
+  EFI_HANDLE Handle;
+  EFI_STATUS Status;
+  EFI_EBC_VM_TEST_PROTOCOL *EbcVmTestProtocol;
+
+  //
+  // Allocate memory for the protocol, then fill in the fields
+  //
+  EbcVmTestProtocol = AllocatePool (sizeof (EFI_EBC_VM_TEST_PROTOCOL));
+  if (EbcVmTestProtocol == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  EbcVmTestProtocol->Execute      = (EBC_VM_TEST_EXECUTE) EbcExecuteInstructions;
+
+  DEBUG_CODE_BEGIN ();
+    EbcVmTestProtocol->Assemble     = (EBC_VM_TEST_ASM) EbcVmTestUnsupported;
+    EbcVmTestProtocol->Disassemble  = (EBC_VM_TEST_DASM) EbcVmTestUnsupported;
+  DEBUG_CODE_END ();
+
+  //
+  // Publish the protocol
+  //
+  Handle  = NULL;
+  Status  = gBS->InstallProtocolInterface (&Handle, &gEfiEbcVmTestProtocolGuid, EFI_NATIVE_INTERFACE, EbcVmTestProtocol);
+  if (EFI_ERROR (Status)) {
+    FreePool (EbcVmTestProtocol);
+  }
+  return Status;
+}
+
+
+/**
+  Returns the EFI_UNSUPPORTED Status.
+
+  @return EFI_UNSUPPORTED  This function always return EFI_UNSUPPORTED status.
+
+**/
+EFI_STATUS
+EFIAPI
+EbcVmTestUnsupported (
+  VOID
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.h b/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.h
new file mode 100644
index 0000000000..01ac441215
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/EbcInt.h
@@ -0,0 +1,278 @@
+/** @file
+  Main routines for the EBC interpreter.  Includes the initialization and
+  main interpreter routines.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _EBC_INT_H_
+#define _EBC_INT_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/DebugSupport.h>
+#include <Protocol/Ebc.h>
+#include <Protocol/EbcVmTest.h>
+#include <Protocol/EbcSimpleDebugger.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+extern VM_CONTEXT                    *mVmPtr;
+
+//
+// Bits of exception flags field of VM context
+//
+#define EXCEPTION_FLAG_FATAL    0x80000000  // can't continue
+#define EXCEPTION_FLAG_ERROR    0x40000000  // bad, but try to continue
+#define EXCEPTION_FLAG_WARNING  0x20000000  // harmless problem
+#define EXCEPTION_FLAG_NONE     0x00000000  // for normal return
+//
+// Flags passed to the internal create-thunks function.
+//
+#define FLAG_THUNK_ENTRY_POINT  0x01  // thunk for an image entry point
+#define FLAG_THUNK_PROTOCOL     0x00  // thunk for an EBC protocol service
+//
+// Put this value at the bottom of the VM's stack gap so we can check it on
+// occasion to make sure the stack has not been corrupted.
+//
+#define VM_STACK_KEY_VALUE  0xDEADBEEF
+
+/**
+  Create thunks for an EBC image entry point, or an EBC protocol service.
+
+  @param  ImageHandle           Image handle for the EBC image. If not null, then
+                                we're creating a thunk for an image entry point.
+  @param  EbcEntryPoint         Address of the EBC code that the thunk is to call
+  @param  Thunk                 Returned thunk we create here
+  @param  Flags                 Flags indicating options for creating the thunk
+
+  @retval EFI_SUCCESS           The thunk was created successfully.
+  @retval EFI_INVALID_PARAMETER The parameter of EbcEntryPoint is not 16-bit
+                                aligned.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to created the EBC
+                                Thunk.
+  @retval EFI_BUFFER_TOO_SMALL  EBC_THUNK_SIZE is not larger enough.
+
+**/
+EFI_STATUS
+EbcCreateThunks (
+  IN EFI_HANDLE           ImageHandle,
+  IN VOID                 *EbcEntryPoint,
+  OUT VOID                **Thunk,
+  IN  UINT32              Flags
+  );
+
+/**
+  Add a thunk to our list of thunks for a given image handle.
+  Also flush the instruction cache since we've written thunk code
+  to memory that will be executed eventually.
+
+  @param  ImageHandle            The image handle to which the thunk is tied.
+  @param  ThunkBuffer            The buffer that has been created/allocated.
+  @param  ThunkSize              The size of the thunk memory allocated.
+
+  @retval EFI_OUT_OF_RESOURCES   Memory allocation failed.
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+EbcAddImageThunk (
+  IN EFI_HANDLE      ImageHandle,
+  IN VOID            *ThunkBuffer,
+  IN UINT32          ThunkSize
+  );
+
+//
+// The interpreter calls these when an exception is detected,
+// or as a periodic callback.
+//
+/**
+  The VM interpreter calls this function when an exception is detected.
+
+  @param  ExceptionType          Specifies the processor exception detected.
+  @param  ExceptionFlags         Specifies the exception context.
+  @param  VmPtr                  Pointer to a VM context for passing info to the
+                                 EFI debugger.
+
+  @retval EFI_SUCCESS            This function completed successfully.
+
+**/
+EFI_STATUS
+EbcDebugSignalException (
+  IN EFI_EXCEPTION_TYPE                   ExceptionType,
+  IN EXCEPTION_FLAGS                      ExceptionFlags,
+  IN VM_CONTEXT                           *VmPtr
+  );
+
+//
+// Define a constant of how often to call the debugger periodic callback
+// function.
+//
+#define EFI_TIMER_UNIT_1MS            (1000 * 10)
+#define EBC_VM_PERIODIC_CALLBACK_RATE (1000 * EFI_TIMER_UNIT_1MS)
+#define STACK_POOL_SIZE               (1024 * 1020)
+#define MAX_STACK_NUM                 4
+
+//
+// External low level functions that are native-processor dependent
+//
+/**
+  The VM thunk code stuffs an EBC entry point into a processor
+  register. Since we can't use inline assembly to get it from
+  the interpreter C code, stuff it into the return value
+  register and return.
+
+  @return  The contents of the register in which the entry point is passed.
+
+**/
+UINTN
+EFIAPI
+EbcLLGetEbcEntryPoint (
+  VOID
+  );
+
+/**
+  This function is called to execute an EBC CALLEX instruction.
+  This instruction requires that we thunk out to external native
+  code. For x64, we switch stacks, copy the arguments to the stack
+  and jump to the specified function.
+  On return, we restore the stack pointer to its original location.
+  Destroys no working registers.
+
+  @param  CallAddr     The function address.
+  @param  EbcSp        The new EBC stack pointer.
+  @param  FramePtr     The frame pointer.
+
+  @return The unmodified value returned by the native code.
+
+**/
+INT64
+EFIAPI
+EbcLLCALLEXNative (
+  IN UINTN        CallAddr,
+  IN UINTN        EbcSp,
+  IN VOID         *FramePtr
+  );
+
+/**
+  This function is called to execute an EBC CALLEX instruction.
+  The function check the callee's content to see whether it is common native
+  code or a thunk to another piece of EBC code.
+  If the callee is common native code, use EbcLLCAllEXASM to manipulate,
+  otherwise, set the VM->IP to target EBC code directly to avoid another VM
+  be startup which cost time and stack space.
+
+  @param  VmPtr            Pointer to a VM context.
+  @param  FuncAddr         Callee's address
+  @param  NewStackPointer  New stack pointer after the call
+  @param  FramePtr         New frame pointer after the call
+  @param  Size             The size of call instruction
+
+**/
+VOID
+EbcLLCALLEX (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        FuncAddr,
+  IN UINTN        NewStackPointer,
+  IN VOID         *FramePtr,
+  IN UINT8        Size
+  );
+
+/**
+  Returns the stack index and buffer assosicated with the Handle parameter.
+
+  @param  Handle                The EFI handle as the index to the EBC stack.
+  @param  StackBuffer           A pointer to hold the returned stack buffer.
+  @param  BufferIndex           A pointer to hold the returned stack index.
+
+  @retval EFI_OUT_OF_RESOURCES  The Handle parameter does not correspond to any
+                                existing EBC stack.
+  @retval EFI_SUCCESS           The stack index and buffer were found and
+                                returned to the caller.
+
+**/
+EFI_STATUS
+GetEBCStack(
+  IN  EFI_HANDLE Handle,
+  OUT VOID       **StackBuffer,
+  OUT UINTN      *BufferIndex
+  );
+
+/**
+  Returns from the EBC stack by stack Index.
+
+  @param  Index        Specifies which EBC stack to return from.
+
+  @retval EFI_SUCCESS  The function completed successfully.
+
+**/
+EFI_STATUS
+ReturnEBCStack(
+  IN UINTN Index
+  );
+
+/**
+  Allocates memory to hold all the EBC stacks.
+
+  @retval EFI_SUCCESS          The EBC stacks were allocated successfully.
+  @retval EFI_OUT_OF_RESOURCES Not enough memory available for EBC stacks.
+
+**/
+EFI_STATUS
+InitEBCStack (
+  VOID
+  );
+
+/**
+  Free all EBC stacks allocated before.
+
+  @retval EFI_SUCCESS   All the EBC stacks were freed.
+
+**/
+EFI_STATUS
+FreeEBCStack(
+  VOID
+  );
+
+/**
+  Returns from the EBC stack associated with the Handle parameter.
+
+  @param  Handle      Specifies the EFI handle to find the EBC stack with.
+
+  @retval EFI_SUCCESS The function completed successfully.
+
+**/
+EFI_STATUS
+ReturnEBCStackByHandle(
+  IN EFI_HANDLE Handle
+  );
+
+typedef struct {
+  EFI_EBC_PROTOCOL  *This;
+  VOID              *EntryPoint;
+  EFI_HANDLE        ImageHandle;
+  VM_CONTEXT        VmContext;
+} EFI_EBC_THUNK_DATA;
+
+#define EBC_PROTOCOL_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('e', 'b', 'c', 'p')
+
+
+#define EBC_PROTOCOL_PRIVATE_DATA_FROM_THIS(a) \
+      CR(a, EBC_PROTOCOL_PRIVATE_DATA, EbcProtocol, EBC_PROTOCOL_PRIVATE_DATA_SIGNATURE)
+
+
+#endif // #ifndef _EBC_INT_H_
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.S b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.S
new file mode 100644
index 0000000000..caf8d40ffb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.S
@@ -0,0 +1,83 @@
+#/** @file
+#  
+#    Low level IA32 specific EBC support routines.
+#  
+#  Copyright (c) 2007 - 2011, 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.
+#  
+#**/
+
+ASM_GLOBAL ASM_PFX(CopyMem)
+ASM_GLOBAL ASM_PFX(EbcInterpret)
+ASM_GLOBAL ASM_PFX(ExecuteEbcImageEntryPoint)
+
+ASM_GLOBAL ASM_PFX(EbcLLCALLEXNative)
+ASM_PFX(EbcLLCALLEXNative):
+         push   %ebp
+         push   %ebx
+         mov    %esp,%ebp
+         mov    0xc(%esp),%ecx
+         mov    0x14(%esp),%eax
+         mov    0x10(%esp),%edx
+         sub    %edx,%eax
+         sub    %eax,%esp
+         mov    %esp,%ebx
+         push   %ecx
+         push   %eax
+         push   %edx
+         push   %ebx
+         call   ASM_PFX(CopyMem)
+         pop    %eax
+         pop    %eax
+         pop    %eax
+         pop    %ecx
+         call   *%ecx
+         mov    %ebp,%esp
+         mov    %ebp,%esp
+         pop    %ebx
+         pop    %ebp
+         ret
+
+ASM_GLOBAL ASM_PFX(EbcLLEbcInterpret)
+ASM_PFX(EbcLLEbcInterpret):
+    # Construct new stack
+    push %ebp
+    mov  %esp, %ebp
+    push %esi
+    push %edi
+    sub  $0x40, %esp
+    push %eax
+    mov  %ebp, %esi
+    add  $0x8, %esi
+    mov  %esp, %edi
+    add  $0x4, %edi
+    mov  $0x10, %ecx
+    rep  movsd
+    
+    # call C-code
+    call ASM_PFX(EbcInterpret)
+    add  $0x44, %esp
+    pop  %edi
+    pop  %esi
+    pop  %ebp
+    ret
+
+ASM_GLOBAL ASM_PFX(EbcLLExecuteEbcImageEntryPoint)
+ASM_PFX(EbcLLExecuteEbcImageEntryPoint):
+    # Construct new stack
+    mov  %eax, -0xC(%esp)
+    mov  0x4(%esp), %eax
+    mov  %eax, -0x8(%esp)
+    mov  0x8(%esp), %eax
+    mov  %eax, -0x4(%esp)
+    # call C-code
+    sub  $0xC, %esp
+    call ASM_PFX(ExecuteEbcImageEntryPoint)
+    add  $0xC, %esp
+    ret
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.asm b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.asm
new file mode 100644
index 0000000000..b16fda69fa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcLowLevel.asm
@@ -0,0 +1,207 @@
+;/** @file
+;  
+;    This code provides low level routines that support the Virtual Machine
+;    for option ROMs.
+;  
+;  Copyright (c) 2006 - 2011, 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.
+;  
+;**/
+
+  page    ,132
+  title   VM ASSEMBLY LANGUAGE ROUTINES
+
+;---------------------------------------------------------------------------
+; Equate files needed.
+;---------------------------------------------------------------------------
+
+.XLIST
+
+.LIST
+
+;---------------------------------------------------------------------------
+; Assembler options
+;---------------------------------------------------------------------------
+
+.686p
+.model  flat, C
+.code
+CopyMem  PROTO  Destination:PTR DWORD, Source:PTR DWORD, Count:DWORD
+EbcInterpret               PROTO
+ExecuteEbcImageEntryPoint  PROTO
+
+;****************************************************************************
+; EbcLLCALLEXNative
+;
+; This function is called to execute an EBC CALLEX instruction
+; to native code.
+; This instruction requires that we thunk out to external native
+; code. For IA32, we simply switch stacks and jump to the
+; specified function. On return, we restore the stack pointer
+; to its original location.
+;
+; Destroys no working registers.
+;****************************************************************************
+; INT64 EbcLLCALLEXNative(UINTN FuncAddr, UINTN NewStackPointer, VOID *FramePtr)
+EbcLLCALLEXNative        PROC        PUBLIC
+      push   ebp
+      push   ebx
+      mov    ebp, esp              ; standard function prolog
+
+      ; Get function address in a register
+      ; mov ecx, FuncAddr => mov ecx, dword ptr [FuncAddr]
+      mov    ecx, dword ptr [esp]+0Ch
+
+      ; Set stack pointer to new value
+      ; mov eax, NewStackPointer => mov eax, dword ptr [NewSp]
+      mov    eax, dword ptr [esp] + 14h
+      mov    edx, dword ptr [esp] + 10h
+      sub    eax, edx
+      sub    esp, eax
+      mov    ebx, esp
+      push   ecx
+      push   eax
+      push   edx
+      push   ebx
+      call   CopyMem
+      pop    eax
+      pop    eax
+      pop    eax
+      pop    ecx
+
+      ; Now call the external routine
+      call  ecx
+
+      ; ebp is preserved by the callee. In this function it
+      ; equals the original esp, so set them equal
+      mov    esp, ebp
+
+      ; Standard function epilog
+      mov      esp, ebp
+      pop      ebx
+      pop      ebp
+      ret
+EbcLLCALLEXNative    ENDP
+
+;****************************************************************************
+; EbcLLEbcInterpret
+;
+; Begin executing an EBC image.
+;****************************************************************************
+; UINT64 EbcLLEbcInterpret(VOID)
+EbcLLEbcInterpret PROC PUBLIC
+    ;
+    ;; mov eax, 0xca112ebc
+    ;; mov eax, EbcEntryPoint
+    ;; mov ecx, EbcLLEbcInterpret
+    ;; jmp ecx
+    ;
+    ; Caller uses above instruction to jump here
+    ; The stack is below:
+    ; +-----------+
+    ; |  RetAddr  |
+    ; +-----------+
+    ; |EntryPoint | (EAX)
+    ; +-----------+
+    ; |   Arg1    | <- EDI
+    ; +-----------+
+    ; |   Arg2    |
+    ; +-----------+
+    ; |   ...     |
+    ; +-----------+
+    ; |   Arg16   |
+    ; +-----------+
+    ; |   EDI     |
+    ; +-----------+
+    ; |   ESI     |
+    ; +-----------+
+    ; |   EBP     | <- EBP
+    ; +-----------+
+    ; |  RetAddr  | <- ESP is here
+    ; +-----------+
+    ; |   Arg1    | <- ESI
+    ; +-----------+
+    ; |   Arg2    |
+    ; +-----------+
+    ; |   ...     |
+    ; +-----------+
+    ; |   Arg16   |
+    ; +-----------+
+    ; 
+
+    ; Construct new stack
+    push ebp
+    mov  ebp, esp
+    push esi
+    push edi
+    sub  esp, 40h
+    push eax
+    mov  esi, ebp
+    add  esi, 8
+    mov  edi, esp
+    add  edi, 4
+    mov  ecx, 16
+    rep  movsd
+    
+    ; call C-code
+    call EbcInterpret
+    add  esp, 44h
+    pop  edi
+    pop  esi
+    pop  ebp
+    ret
+EbcLLEbcInterpret ENDP
+
+;****************************************************************************
+; EbcLLExecuteEbcImageEntryPoint
+;
+; Begin executing an EBC image.
+;****************************************************************************
+; UINT64 EbcLLExecuteEbcImageEntryPoint(VOID)
+EbcLLExecuteEbcImageEntryPoint PROC PUBLIC
+    ;
+    ;; mov eax, 0xca112ebc
+    ;; mov eax, EbcEntryPoint
+    ;; mov ecx, EbcLLExecuteEbcImageEntryPoint
+    ;; jmp ecx
+    ;
+    ; Caller uses above instruction to jump here
+    ; The stack is below:
+    ; +-----------+
+    ; |  RetAddr  |
+    ; +-----------+
+    ; |EntryPoint | (EAX)
+    ; +-----------+
+    ; |ImageHandle|
+    ; +-----------+
+    ; |SystemTable|
+    ; +-----------+
+    ; |  RetAddr  | <- ESP is here
+    ; +-----------+
+    ; |ImageHandle|
+    ; +-----------+
+    ; |SystemTable|
+    ; +-----------+
+    ; 
+    
+    ; Construct new stack
+    mov  [esp - 0Ch], eax
+    mov  eax, [esp + 04h]
+    mov  [esp - 08h], eax
+    mov  eax, [esp + 08h]
+    mov  [esp - 04h], eax
+    
+    ; call C-code
+    sub  esp, 0Ch
+    call ExecuteEbcImageEntryPoint
+    add  esp, 0Ch
+    ret
+EbcLLExecuteEbcImageEntryPoint ENDP
+
+END
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcSupport.c b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcSupport.c
new file mode 100644
index 0000000000..bddfbf630d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ia32/EbcSupport.c
@@ -0,0 +1,529 @@
+/** @file
+  This module contains EBC support routines that are customized based on
+  the target ia32 processor.
+
+Copyright (c) 2006 - 2014, 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 "EbcInt.h"
+#include "EbcExecute.h"
+
+//
+// NOTE: This is the stack size allocated for the interpreter
+//       when it executes an EBC image. The requirements can change
+//       based on whether or not a debugger is present, and other
+//       platform-specific configurations.
+//
+#define VM_STACK_SIZE   (1024 * 4)
+
+#define STACK_REMAIN_SIZE (1024 * 4)
+
+//
+// This is instruction buffer used to create EBC thunk
+//
+#define EBC_ENTRYPOINT_SIGNATURE           0xAFAFAFAF
+#define EBC_LL_EBC_ENTRYPOINT_SIGNATURE    0xFAFAFAFA
+UINT8  mInstructionBufferTemplate[] = {
+  //
+  // Add a magic code here to help the VM recognize the thunk..
+  // mov eax, 0xca112ebc  => B8 BC 2E 11 CA
+  //
+  0xB8, 0xBC, 0x2E, 0x11, 0xCA,
+  //
+  // Add code bytes to load up a processor register with the EBC entry point.
+  //  mov eax, EbcEntryPoint  => B8 XX XX XX XX (To be fixed at runtime)
+  // These 4 bytes of the thunk entry is the address of the EBC
+  // entry point.
+  //
+  0xB8, 
+    (UINT8)(EBC_ENTRYPOINT_SIGNATURE & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 8) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 16) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 24) & 0xFF),
+  //
+  // Stick in a load of ecx with the address of appropriate VM function.
+  //  mov ecx, EbcLLEbcInterpret  => B9 XX XX XX XX (To be fixed at runtime)
+  //
+  0xB9,
+    (UINT8)(EBC_LL_EBC_ENTRYPOINT_SIGNATURE & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 8) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 16) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 24) & 0xFF),
+  //
+  // Stick in jump opcode bytes
+  //  jmp ecx => FF E1
+  //
+  0xFF, 0xE1,
+};
+
+/**
+  Begin executing an EBC image.
+  This is used for Ebc Thunk call.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcLLEbcInterpret (
+  VOID
+  );
+
+/**
+  Begin executing an EBC image.
+  This is used for Ebc image entrypoint.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcLLExecuteEbcImageEntryPoint (
+  VOID
+  );
+
+/**
+  This function is called to execute an EBC CALLEX instruction.
+  The function check the callee's content to see whether it is common native
+  code or a thunk to another piece of EBC code.
+  If the callee is common native code, use EbcLLCAllEXASM to manipulate,
+  otherwise, set the VM->IP to target EBC code directly to avoid another VM
+  be startup which cost time and stack space.
+
+  @param  VmPtr            Pointer to a VM context.
+  @param  FuncAddr         Callee's address
+  @param  NewStackPointer  New stack pointer after the call
+  @param  FramePtr         New frame pointer after the call
+  @param  Size             The size of call instruction
+
+**/
+VOID
+EbcLLCALLEX (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        FuncAddr,
+  IN UINTN        NewStackPointer,
+  IN VOID         *FramePtr,
+  IN UINT8        Size
+  )
+{
+  UINTN    IsThunk;
+  UINTN    TargetEbcAddr;
+  UINT8    InstructionBuffer[sizeof(mInstructionBufferTemplate)];
+  UINTN    Index;
+  UINTN    IndexOfEbcEntrypoint;
+
+  IsThunk       = 1;
+  TargetEbcAddr = 0;
+  IndexOfEbcEntrypoint = 0;
+
+  //
+  // Processor specific code to check whether the callee is a thunk to EBC.
+  //
+  CopyMem (InstructionBuffer, (VOID *)FuncAddr, sizeof(InstructionBuffer));
+  //
+  // Fill the signature according to mInstructionBufferTemplate
+  //
+  for (Index = 0; Index < sizeof(mInstructionBufferTemplate) - sizeof(UINTN); Index++) {
+    if (*(UINTN *)&mInstructionBufferTemplate[Index] == EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&InstructionBuffer[Index] = EBC_ENTRYPOINT_SIGNATURE;
+      IndexOfEbcEntrypoint = Index;
+    }
+    if (*(UINTN *)&mInstructionBufferTemplate[Index] == EBC_LL_EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&InstructionBuffer[Index] = EBC_LL_EBC_ENTRYPOINT_SIGNATURE;
+    }
+  }
+  //
+  // Check if we need thunk to native
+  //
+  if (CompareMem (InstructionBuffer, mInstructionBufferTemplate, sizeof(mInstructionBufferTemplate)) != 0) {
+    IsThunk = 0;
+  }
+
+  if (IsThunk == 1){
+    //
+    // The callee is a thunk to EBC, adjust the stack pointer down 16 bytes and
+    // put our return address and frame pointer on the VM stack.
+    // Then set the VM's IP to new EBC code.
+    //
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMemN (VmPtr, (UINTN) VmPtr->Gpr[0], (UINTN) FramePtr);
+    VmPtr->FramePtr = (VOID *) (UINTN) VmPtr->Gpr[0];
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[0], (UINT64) (UINTN) (VmPtr->Ip + Size));
+
+    CopyMem (&TargetEbcAddr, (UINT8 *)FuncAddr + IndexOfEbcEntrypoint, sizeof(UINTN));
+    VmPtr->Ip = (VMIP) (UINTN) TargetEbcAddr;
+  } else {
+    //
+    // The callee is not a thunk to EBC, call native code,
+    // and get return value.
+    //
+    VmPtr->Gpr[7] = EbcLLCALLEXNative (FuncAddr, NewStackPointer, FramePtr);
+
+    //
+    // Advance the IP.
+    //
+    VmPtr->Ip += Size;
+  }
+}
+
+
+/**
+  Begin executing an EBC image.
+
+  This is a thunk function. Microsoft x64 compiler only provide fast_call
+  calling convention, so the first four arguments are passed by rcx, rdx,
+  r8, and r9, while other arguments are passed in stack.
+
+  @param  EntryPoint            The entrypoint of EBC code.
+  @param  Arg1                  The 1st argument.
+  @param  Arg2                  The 2nd argument.
+  @param  Arg3                  The 3rd argument.
+  @param  Arg4                  The 4th argument.
+  @param  Arg5                  The 5th argument.
+  @param  Arg6                  The 6th argument.
+  @param  Arg7                  The 7th argument.
+  @param  Arg8                  The 8th argument.
+  @param  Arg9                  The 9th argument.
+  @param  Arg10                 The 10th argument.
+  @param  Arg11                 The 11th argument.
+  @param  Arg12                 The 12th argument.
+  @param  Arg13                 The 13th argument.
+  @param  Arg14                 The 14th argument.
+  @param  Arg15                 The 15th argument.
+  @param  Arg16                 The 16th argument.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcInterpret (
+  IN UINTN      EntryPoint,
+  IN UINTN      Arg1,
+  IN UINTN      Arg2,
+  IN UINTN      Arg3,
+  IN UINTN      Arg4,
+  IN UINTN      Arg5,
+  IN UINTN      Arg6,
+  IN UINTN      Arg7,
+  IN UINTN      Arg8,
+  IN UINTN      Arg9,
+  IN UINTN      Arg10,
+  IN UINTN      Arg11,
+  IN UINTN      Arg12,
+  IN UINTN      Arg13,
+  IN UINTN      Arg14,
+  IN UINTN      Arg15,
+  IN UINTN      Arg16
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+
+  //
+  // Get the EBC entry point
+  //
+  Addr = EntryPoint;
+
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+  //
+  // Initialize the stack pointer for the EBC. Get the current system stack
+  // pointer and adjust it down by the max needed for the interpreter.
+  //
+
+  //
+  // Align the stack on a natural boundary
+  //
+
+  //
+  // Allocate stack pool
+  //
+  Status = GetEBCStack((EFI_HANDLE)-1, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64)(UINTN) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN)VmContext.Gpr[0];
+  VmContext.Gpr[0] &= ~((VM_REGISTER)(sizeof (UINTN) - 1));
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+  //
+  // Put a magic value in the stack gap, then adjust down again
+  //
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) VM_STACK_KEY_VALUE;
+  VmContext.StackMagicPtr             = (UINTN *) (UINTN) VmContext.Gpr[0];
+  VmContext.LowStackTop   = (UINTN) VmContext.Gpr[0];
+
+  //
+  // For IA32, this is where we say our return address is
+  //
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg16;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg15;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg14;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg13;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg12;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg11;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg10;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg9;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg8;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg7;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg6;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg5;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg4;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg3;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg2;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) Arg1;
+  VmContext.Gpr[0] -= 16;
+  VmContext.StackRetAddr  = (UINT64) VmContext.Gpr[0];
+
+  //
+  // We need to keep track of where the EBC stack starts. This way, if the EBC
+  // accesses any stack variables above its initial stack setting, then we know
+  // it's accessing variables passed into it, which means the data is on the
+  // VM's stack.
+  // When we're called, on the stack (high to low) we have the parameters, the
+  // return address, then the saved ebp. Save the pointer to the return address.
+  // EBC code knows that's there, so should look above it for function parameters.
+  // The offset is the size of locals (VMContext + Addr + saved ebp).
+  // Note that the interpreter assumes there is a 16 bytes of return address on
+  // the stack too, so adjust accordingly.
+  //  VmContext.HighStackBottom = (UINTN)(Addr + sizeof (VmContext) + sizeof (Addr));
+  //
+
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Begin executing an EBC image.
+
+  @param  EntryPoint       The entrypoint of EBC code.
+  @param  ImageHandle      image handle for the EBC application we're executing
+  @param  SystemTable      standard system table passed into an driver's entry
+                           point
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+ExecuteEbcImageEntryPoint (
+  IN UINTN                EntryPoint,
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+
+  //
+  // Get the EBC entry point
+  //
+  Addr = EntryPoint;
+
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+
+  //
+  // Save the image handle so we can track the thunks created for this image
+  //
+  VmContext.ImageHandle = ImageHandle;
+  VmContext.SystemTable = SystemTable;
+
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+
+  //
+  // Initialize the stack pointer for the EBC. Get the current system stack
+  // pointer and adjust it down by the max needed for the interpreter.
+  //
+
+  //
+  // Allocate stack pool
+  //
+  Status = GetEBCStack(ImageHandle, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64)(UINTN) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN)VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+  //
+  // Put a magic value in the stack gap, then adjust down again
+  //
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) VM_STACK_KEY_VALUE;
+  VmContext.StackMagicPtr             = (UINTN *) (UINTN) VmContext.Gpr[0];
+
+  //
+  // Align the stack on a natural boundary
+  //  VmContext.Gpr[0] &= ~(sizeof(UINTN) - 1);
+  //
+  VmContext.LowStackTop   = (UINTN) VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) SystemTable;
+  VmContext.Gpr[0] -= sizeof (UINTN);
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) ImageHandle;
+
+  VmContext.Gpr[0] -= 16;
+  VmContext.StackRetAddr  = (UINT64) VmContext.Gpr[0];
+  //
+  // VM pushes 16-bytes for return address. Simulate that here.
+  //
+
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Create thunks for an EBC image entry point, or an EBC protocol service.
+
+  @param  ImageHandle           Image handle for the EBC image. If not null, then
+                                we're creating a thunk for an image entry point.
+  @param  EbcEntryPoint         Address of the EBC code that the thunk is to call
+  @param  Thunk                 Returned thunk we create here
+  @param  Flags                 Flags indicating options for creating the thunk
+
+  @retval EFI_SUCCESS           The thunk was created successfully.
+  @retval EFI_INVALID_PARAMETER The parameter of EbcEntryPoint is not 16-bit
+                                aligned.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to created the EBC
+                                Thunk.
+  @retval EFI_BUFFER_TOO_SMALL  EBC_THUNK_SIZE is not larger enough.
+
+**/
+EFI_STATUS
+EbcCreateThunks (
+  IN EFI_HANDLE           ImageHandle,
+  IN VOID                 *EbcEntryPoint,
+  OUT VOID                **Thunk,
+  IN  UINT32              Flags
+  )
+{
+  UINT8       *Ptr;
+  UINT8       *ThunkBase;
+  UINT32      Index;
+  INT32       ThunkSize;
+
+  //
+  // Check alignment of pointer to EBC code
+  //
+  if ((UINT32) (UINTN) EbcEntryPoint & 0x01) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ThunkSize = sizeof(mInstructionBufferTemplate);
+
+  Ptr = AllocatePool (sizeof(mInstructionBufferTemplate));
+
+  if (Ptr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  //  Print(L"Allocate TH: 0x%X\n", (UINT32)Ptr);
+  //
+  // Save the start address so we can add a pointer to it to a list later.
+  //
+  ThunkBase = Ptr;
+
+  //
+  // Give them the address of our buffer we're going to fix up
+  //
+  *Thunk = (VOID *) Ptr;
+
+  //
+  // Copy whole thunk instruction buffer template
+  //
+  CopyMem (Ptr, mInstructionBufferTemplate, sizeof(mInstructionBufferTemplate));
+
+  //
+  // Patch EbcEntryPoint and EbcLLEbcInterpret
+  //
+  for (Index = 0; Index < sizeof(mInstructionBufferTemplate) - sizeof(UINTN); Index++) {
+    if (*(UINTN *)&Ptr[Index] == EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&Ptr[Index] = (UINTN)EbcEntryPoint;
+    }
+    if (*(UINTN *)&Ptr[Index] == EBC_LL_EBC_ENTRYPOINT_SIGNATURE) {
+      if ((Flags & FLAG_THUNK_ENTRY_POINT) != 0) {
+        *(UINTN *)&Ptr[Index] = (UINTN)EbcLLExecuteEbcImageEntryPoint;
+      } else {
+        *(UINTN *)&Ptr[Index] = (UINTN)EbcLLEbcInterpret;
+      }
+    }
+  }
+
+  //
+  // Add the thunk to the list for this image. Do this last since the add
+  // function flushes the cache for us.
+  //
+  EbcAddImageThunk (ImageHandle, (VOID *) ThunkBase, ThunkSize);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcLowLevel.s b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcLowLevel.s
new file mode 100644
index 0000000000..4ae24dee7d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcLowLevel.s
@@ -0,0 +1,206 @@
+///** @file
+//  
+//  Contains low level routines for the Virtual Machine implementation
+//  on an Itanium-based platform.
+//
+//  Copyright (c) 2006 - 2011, 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.
+//  
+//**/
+
+.file  "EbcLowLevel.s"
+
+#define PROCEDURE_ENTRY(name)   .##text;            \
+                                .##type name, @function;    \
+                                .##proc name;           \
+name::
+
+#define PROCEDURE_EXIT(name)    .##endp name
+
+// Note: use of NESTED_SETUP requires number of locals (l) >= 3
+
+#define NESTED_SETUP(i,l,o,r) \
+         alloc loc1=ar##.##pfs,i,l,o,r ;\
+         mov loc0=b0
+
+#define NESTED_RETURN \
+         mov b0=loc0 ;\
+         mov ar##.##pfs=loc1 ;;\
+         br##.##ret##.##dpnt  b0;;
+
+.type CopyMem, @function;
+
+//-----------------------------------------------------------------------------
+//++
+// EbcAsmLLCALLEX
+//
+//  Implements the low level EBC CALLEX instruction. Sets up the
+//  stack pointer, does the spill of function arguments, and
+//  calls the native function. On return it restores the original
+//  stack pointer and returns to the caller.
+//
+// Arguments :
+//
+// On Entry :
+//    in0 = Address of native code to call
+//    in1 = New stack pointer
+//
+// Return Value:
+//
+// As per static calling conventions.
+//
+//--
+//---------------------------------------------------------------------------
+;// void EbcAsmLLCALLEX (UINTN FunctionAddr, UINTN EbcStackPointer)
+PROCEDURE_ENTRY(EbcAsmLLCALLEX)
+  NESTED_SETUP (2,6,8,0)
+
+  // NESTED_SETUP uses loc0 and loc1 for context save
+
+  //
+  // Save a copy of the EBC VM stack pointer
+  //
+  mov r8 = in1;;
+
+  //
+  // Copy stack arguments from EBC stack into registers.
+  // Assume worst case and copy 8.
+  //
+  ld8   out0 = [r8], 8;;
+  ld8   out1 = [r8], 8;;
+  ld8   out2 = [r8], 8;;
+  ld8   out3 = [r8], 8;;
+  ld8   out4 = [r8], 8;;
+  ld8   out5 = [r8], 8;;
+  ld8   out6 = [r8], 8;;
+  ld8   out7 = [r8], 8;;
+
+  //
+  // Save the original stack pointer
+  //
+  mov   loc2 = r12;
+
+  //
+  // Save the gp
+  //
+  or    loc3 = r1, r0
+
+  //
+  // Set the new aligned stack pointer. Reserve space for the required
+  // 16-bytes of scratch area as well.
+  //
+  add  r12 = 48, in1
+
+  //
+  // Now call the function. Load up the function address from the descriptor
+  // pointed to by in0. Then get the gp from the descriptor at the following
+  // address in the descriptor.
+  //
+  ld8   r31 = [in0], 8;;
+  ld8   r30 = [in0];;
+  mov   b1 = r31
+  mov   r1 = r30
+  (p0) br.call.dptk.many b0 = b1;;
+
+  //
+  // Restore the original stack pointer and gp
+  //
+  mov   r12 = loc2
+  or    r1 = loc3, r0
+
+  //
+  // Now return
+  //
+  NESTED_RETURN
+
+PROCEDURE_EXIT(EbcAsmLLCALLEX)
+
+//-----------------------------------------------------------------------------
+//++
+// EbcLLCALLEXNative
+//
+//  This function is called to execute an EBC CALLEX instruction.
+//  This instruction requires that we thunk out to external native
+//  code. On return, we restore the stack pointer to its original location.
+//  Destroys no working registers.  For IPF, at least 8 register slots
+//  must be allocated on the stack frame to support any number of 
+//  arguments beiung passed to the external native function.  The
+//  size of the stack frame is FramePtr - EbcSp.  If this size is less
+//  than 64-bytes, the amount of stack frame allocated is rounded up
+//  to 64-bytes 
+//
+// Arguments On Entry :
+//    in0 = CallAddr     The function address.
+//    in1 = EbcSp        The new EBC stack pointer.
+//    in2 = FramePtr     The frame pointer.
+//
+// Return Value:
+//    None
+//
+// C Function Prototype:
+//    VOID
+//    EFIAPI
+//    EbcLLCALLEXNative (
+//      IN UINTN        CallAddr,
+//      IN UINTN        EbcSp,
+//      IN VOID         *FramePtr
+//      );
+//--
+//---------------------------------------------------------------------------
+
+PROCEDURE_ENTRY(EbcLLCALLEXNative)
+  NESTED_SETUP (3,6,3,0)
+
+  mov   loc2 = in2;;              // loc2 = in2 = FramePtr
+  mov   loc3 = in1;;              // loc3 = in1 = EbcSp
+  sub   loc2 = loc2, loc3;;       // loc2 = loc2 - loc3 = FramePtr - EbcSp
+  mov   out2 = loc2;;             // out2 = loc2 = FramePtr - EbcSp
+  mov   loc4 = 0x40;;             // loc4 = 0x40
+  cmp.leu p6  = out2, loc4;;      // IF out2 < loc4 THEN P6=1 ELSE P6=0; IF (FramePtr - EbcSp) < 0x40 THEN P6 = 1 ELSE P6=0
+  (p6) mov   loc2 = loc4;;        // IF P6==1 THEN loc2 = loc4 = 0x40
+  mov   loc4 = r12;;              // save sp
+  or    loc5 = r1, r0             // save gp
+
+  sub   r12 = r12, loc2;;         // sp = sp - loc2 = sp - MAX (0x40, FramePtr - EbcSp)
+
+  and   r12 = -0x10, r12          // Round sp down to the nearest 16-byte boundary
+  mov   out1 = in1;;              // out1 = EbcSp
+  mov   out0 = r12;;              // out0 = sp
+  adds  r12 = -0x8, r12           
+  (p0) br.call.dptk.many b0 = CopyMem;;      // CopyMem (sp, EbcSp, (FramePtr - EbcSp))
+  adds  r12 = 0x8, r12            
+
+  mov   out0 = in0;;              // out0 = CallAddr
+  mov   out1 = r12;;              // out1 = sp
+  (p0) br.call.dptk.many b0 = EbcAsmLLCALLEX;;    // EbcAsmLLCALLEX (CallAddr, sp)
+  mov   r12 = loc4;;              // restore sp
+  or    r1 = loc5, r0             // restore gp
+
+  NESTED_RETURN
+PROCEDURE_EXIT(EbcLLCALLEXNative)
+
+
+//
+// UINTN EbcLLGetEbcEntryPoint(VOID)
+//
+// Description:
+//    Simply return, so that the caller retrieves the return register
+//    contents (R8). That's where the thunk-to-ebc code stuffed the
+//    EBC entry point.
+//
+PROCEDURE_ENTRY(EbcLLGetEbcEntryPoint)
+    br.ret.sptk  b0 ;;
+PROCEDURE_EXIT(EbcLLGetEbcEntryPoint)
+
+
+
+
+
+
+
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.c b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.c
new file mode 100644
index 0000000000..23b6c1bd84
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.c
@@ -0,0 +1,879 @@
+/** @file
+  This module contains EBC support routines that are customized based on
+  the target processor.
+
+Copyright (c) 2006 - 2012, 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 "EbcInt.h"
+#include "EbcExecute.h"
+#include "EbcSupport.h"
+
+/**
+  Given raw bytes of Itanium based code, format them into a bundle and
+  write them out.
+
+  @param  MemPtr                 pointer to memory location to write the bundles
+                                 to.
+  @param  Template               5-bit template.
+  @param  Slot0                  Instruction slot 0 data for the bundle.
+  @param  Slot1                  Instruction slot 1 data for the bundle.
+  @param  Slot2                  Instruction slot 2 data for the bundle.
+
+  @retval EFI_INVALID_PARAMETER  Pointer is not aligned
+  @retval EFI_INVALID_PARAMETER  No more than 5 bits in template
+  @retval EFI_INVALID_PARAMETER  More than 41 bits used in code
+  @retval EFI_SUCCESS            All data is written.
+
+**/
+EFI_STATUS
+WriteBundle (
+  IN    VOID    *MemPtr,
+  IN    UINT8   Template,
+  IN    UINT64  Slot0,
+  IN    UINT64  Slot1,
+  IN    UINT64  Slot2
+  );
+
+/**
+  Pushes a 64 bit unsigned value to the VM stack.
+
+  @param VmPtr  The pointer to current VM context.
+  @param Arg    The value to be pushed.
+
+**/
+VOID
+PushU64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT64     Arg
+  )
+{
+  //
+  // Advance the VM stack down, and then copy the argument to the stack.
+  // Hope it's aligned.
+  //
+  VmPtr->Gpr[0] -= sizeof (UINT64);
+  *(UINT64 *) VmPtr->Gpr[0] = Arg;
+}
+
+/**
+  Begin executing an EBC image. The address of the entry point is passed
+  in via a processor register, so we'll need to make a call to get the
+  value.
+
+  This is a thunk function. Microsoft x64 compiler only provide fast_call
+  calling convention, so the first four arguments are passed by rcx, rdx,
+  r8, and r9, while other arguments are passed in stack.
+
+  @param  Arg1                  The 1st argument.
+  @param  ...                   The variable arguments list.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcInterpret (
+  UINT64      Arg1,
+  ...
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+  VA_LIST     List;
+  UINT64      Arg2;
+  UINT64      Arg3;
+  UINT64      Arg4;
+  UINT64      Arg5;
+  UINT64      Arg6;
+  UINT64      Arg7;
+  UINT64      Arg8;
+  UINT64      Arg9;
+  UINT64      Arg10;
+  UINT64      Arg11;
+  UINT64      Arg12;
+  UINT64      Arg13;
+  UINT64      Arg14;
+  UINT64      Arg15;
+  UINT64      Arg16;
+  //
+  // Get the EBC entry point from the processor register. Make sure you don't
+  // call any functions before this or you could mess up the register the
+  // entry point is passed in.
+  //
+  Addr = EbcLLGetEbcEntryPoint ();
+  //
+  // Need the args off the stack.
+  //
+  VA_START (List, Arg1);
+  Arg2      = VA_ARG (List, UINT64);
+  Arg3      = VA_ARG (List, UINT64);
+  Arg4      = VA_ARG (List, UINT64);
+  Arg5      = VA_ARG (List, UINT64);
+  Arg6      = VA_ARG (List, UINT64);
+  Arg7      = VA_ARG (List, UINT64);
+  Arg8      = VA_ARG (List, UINT64);
+  Arg9      = VA_ARG (List, UINT64);
+  Arg10     = VA_ARG (List, UINT64);
+  Arg11     = VA_ARG (List, UINT64);
+  Arg12     = VA_ARG (List, UINT64);
+  Arg13     = VA_ARG (List, UINT64);
+  Arg14     = VA_ARG (List, UINT64);
+  Arg15     = VA_ARG (List, UINT64);
+  Arg16     = VA_ARG (List, UINT64);
+  VA_END (List);
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+  //
+  // Initialize the stack pointer for the EBC. Get the current system stack
+  // pointer and adjust it down by the max needed for the interpreter.
+  //
+  //
+  // NOTE: Eventually we should have the interpreter allocate memory
+  //       for stack space which it will use during its execution. This
+  //       would likely improve performance because the interpreter would
+  //       no longer be required to test each memory access and adjust
+  //       those reading from the stack gap.
+  //
+  // For IPF, the stack looks like (assuming 10 args passed)
+  //   arg10
+  //   arg9       (Bottom of high stack)
+  //   [ stack gap for interpreter execution ]
+  //   [ magic value for detection of stack corruption ]
+  //   arg8       (Top of low stack)
+  //   arg7....
+  //   arg1
+  //   [ 64-bit return address ]
+  //   [ ebc stack ]
+  // If the EBC accesses memory in the stack gap, then we assume that it's
+  // actually trying to access args9 and greater. Therefore we need to
+  // adjust memory accesses in this region to point above the stack gap.
+  //
+  //
+  // Now adjust the EBC stack pointer down to leave a gap for interpreter
+  // execution. Then stuff a magic value there.
+  //
+
+  Status = GetEBCStack((EFI_HANDLE)(UINTN)-1, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN) VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+
+  PushU64 (&VmContext, (UINT64) VM_STACK_KEY_VALUE);
+  VmContext.StackMagicPtr = (UINTN *) VmContext.Gpr[0];
+  VmContext.LowStackTop   = (UINTN) VmContext.Gpr[0];
+  //
+  // Push the EBC arguments on the stack. Does not matter that they may not
+  // all be valid.
+  //
+  PushU64 (&VmContext, Arg16);
+  PushU64 (&VmContext, Arg15);
+  PushU64 (&VmContext, Arg14);
+  PushU64 (&VmContext, Arg13);
+  PushU64 (&VmContext, Arg12);
+  PushU64 (&VmContext, Arg11);
+  PushU64 (&VmContext, Arg10);
+  PushU64 (&VmContext, Arg9);
+  PushU64 (&VmContext, Arg8);
+  PushU64 (&VmContext, Arg7);
+  PushU64 (&VmContext, Arg6);
+  PushU64 (&VmContext, Arg5);
+  PushU64 (&VmContext, Arg4);
+  PushU64 (&VmContext, Arg3);
+  PushU64 (&VmContext, Arg2);
+  PushU64 (&VmContext, Arg1);
+  //
+  // Push a bogus return address on the EBC stack because the
+  // interpreter expects one there. For stack alignment purposes on IPF,
+  // EBC return addresses are always 16 bytes. Push a bogus value as well.
+  //
+  PushU64 (&VmContext, 0);
+  PushU64 (&VmContext, 0xDEADBEEFDEADBEEF);
+  VmContext.StackRetAddr = (UINT64) VmContext.Gpr[0];
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Begin executing an EBC image. The address of the entry point is passed
+  in via a processor register, so we'll need to make a call to get the
+  value.
+
+  @param  ImageHandle      image handle for the EBC application we're executing
+  @param  SystemTable      standard system table passed into an driver's entry
+                           point
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+ExecuteEbcImageEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+
+  //
+  // Get the EBC entry point from the processor register. Make sure you don't
+  // call any functions before this or you could mess up the register the
+  // entry point is passed in.
+  //
+  Addr = EbcLLGetEbcEntryPoint ();
+
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+
+  //
+  // Save the image handle so we can track the thunks created for this image
+  //
+  VmContext.ImageHandle = ImageHandle;
+  VmContext.SystemTable = SystemTable;
+
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+
+  //
+  // Get the stack pointer. This is the bottom of the upper stack.
+  //
+
+  Status = GetEBCStack(ImageHandle, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN) VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+
+  //
+  // Allocate stack space for the interpreter. Then put a magic value
+  // at the bottom so we can detect stack corruption.
+  //
+  PushU64 (&VmContext, (UINT64) VM_STACK_KEY_VALUE);
+  VmContext.StackMagicPtr = (UINTN *) (UINTN) VmContext.Gpr[0];
+
+  //
+  // When we thunk to external native code, we copy the last 8 qwords from
+  // the EBC stack into the processor registers, and adjust the stack pointer
+  // up. If the caller is not passing 8 parameters, then we've moved the
+  // stack pointer up into the stack gap. If this happens, then the caller
+  // can mess up the stack gap contents (in particular our magic value).
+  // Therefore, leave another gap below the magic value. Pick 10 qwords down,
+  // just as a starting point.
+  //
+  VmContext.Gpr[0] -= 10 * sizeof (UINT64);
+
+  //
+  // Align the stack pointer such that after pushing the system table,
+  // image handle, and return address on the stack, it's aligned on a 16-byte
+  // boundary as required for IPF.
+  //
+  VmContext.Gpr[0] &= (INT64)~0x0f;
+  VmContext.LowStackTop = (UINTN) VmContext.Gpr[0];
+  //
+  // Simply copy the image handle and system table onto the EBC stack.
+  // Greatly simplifies things by not having to spill the args
+  //
+  PushU64 (&VmContext, (UINT64) SystemTable);
+  PushU64 (&VmContext, (UINT64) ImageHandle);
+
+  //
+  // Interpreter assumes 64-bit return address is pushed on the stack.
+  // IPF does not do this so pad the stack accordingly. Also, a
+  // "return address" is 16 bytes as required for IPF stack alignments.
+  //
+  PushU64 (&VmContext, (UINT64) 0);
+  PushU64 (&VmContext, (UINT64) 0x1234567887654321);
+  VmContext.StackRetAddr = (UINT64) VmContext.Gpr[0];
+
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Create thunks for an EBC image entry point, or an EBC protocol service.
+
+  @param  ImageHandle           Image handle for the EBC image. If not null, then
+                                we're creating a thunk for an image entry point.
+  @param  EbcEntryPoint         Address of the EBC code that the thunk is to call
+  @param  Thunk                 Returned thunk we create here
+  @param  Flags                 Flags indicating options for creating the thunk
+
+  @retval EFI_SUCCESS           The thunk was created successfully.
+  @retval EFI_INVALID_PARAMETER The parameter of EbcEntryPoint is not 16-bit
+                                aligned.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to created the EBC
+                                Thunk.
+  @retval EFI_BUFFER_TOO_SMALL  EBC_THUNK_SIZE is not larger enough.
+
+**/
+EFI_STATUS
+EbcCreateThunks (
+  IN EFI_HANDLE           ImageHandle,
+  IN VOID                 *EbcEntryPoint,
+  OUT VOID                **Thunk,
+  IN  UINT32              Flags
+  )
+{
+  UINT8       *Ptr;
+  UINT8       *ThunkBase;
+  UINT64      Addr;
+  UINT64      Code[3];    // Code in a bundle
+  UINT64      RegNum;     // register number for MOVL
+  UINT64      BitI;       // bits of MOVL immediate data
+  UINT64      BitIc;         // bits of MOVL immediate data
+  UINT64      BitImm5c;      // bits of MOVL immediate data
+  UINT64      BitImm9d;      // bits of MOVL immediate data
+  UINT64      BitImm7b;      // bits of MOVL immediate data
+  UINT64      Br;         // branch register for loading and jumping
+  UINT64      *Data64Ptr;
+  UINT32      ThunkSize;
+  UINT32      Size;
+
+  //
+  // Check alignment of pointer to EBC code, which must always be aligned
+  // on a 2-byte boundary.
+  //
+  if ((UINT32) (UINTN) EbcEntryPoint & 0x01) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Allocate memory for the thunk. Make the (most likely incorrect) assumption
+  // that the returned buffer is not aligned, so round up to the next
+  // alignment size.
+  //
+  Size      = EBC_THUNK_SIZE + EBC_THUNK_ALIGNMENT - 1;
+  ThunkSize = Size;
+  Ptr = AllocatePool (Size);
+
+  if (Ptr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Save the start address of the buffer.
+  //
+  ThunkBase = Ptr;
+
+  //
+  // Make sure it's aligned for code execution. If not, then
+  // round up.
+  //
+  if ((UINT32) (UINTN) Ptr & (EBC_THUNK_ALIGNMENT - 1)) {
+    Ptr = (UINT8 *) (((UINTN) Ptr + (EBC_THUNK_ALIGNMENT - 1)) &~ (UINT64) (EBC_THUNK_ALIGNMENT - 1));
+  }
+  //
+  // Return the pointer to the thunk to the caller to user as the
+  // image entry point.
+  //
+  *Thunk = (VOID *) Ptr;
+
+  //
+  // Clear out the thunk entry
+  // ZeroMem(Ptr, Size);
+  //
+  // For IPF, when you do a call via a function pointer, the function pointer
+  // actually points to a function descriptor which consists of a 64-bit
+  // address of the function, followed by a 64-bit gp for the function being
+  // called. See the the Software Conventions and Runtime Architecture Guide
+  // for details.
+  // So first off in our thunk, create a descriptor for our actual thunk code.
+  // This means we need to create a pointer to the thunk code (which follows
+  // the descriptor we're going to create), followed by the gp of the Vm
+  // interpret function we're going to eventually execute.
+  //
+  Data64Ptr = (UINT64 *) Ptr;
+
+  //
+  // Write the function's entry point (which is our thunk code that follows
+  // this descriptor we're creating).
+  //
+  *Data64Ptr = (UINT64) (Data64Ptr + 2);
+  //
+  // Get the gp from the descriptor for EbcInterpret and stuff it in our thunk
+  // descriptor.
+  //
+  *(Data64Ptr + 1) = *(UINT64 *) ((UINT64 *) (UINTN) EbcInterpret + 1);
+  //
+  // Advance our thunk data pointer past the descriptor. Since the
+  // descriptor consists of 16 bytes, the pointer is still aligned for
+  // IPF code execution (on 16-byte boundary).
+  //
+  Ptr += sizeof (UINT64) * 2;
+
+  //
+  // *************************** MAGIC BUNDLE ********************************
+  //
+  // Write magic code bundle for: movl r8 = 0xca112ebcca112ebc to help the VM
+  // to recognize it is a thunk.
+  //
+  Addr = (UINT64) 0xCA112EBCCA112EBC;
+
+  //
+  // Now generate the code bytes. First is nop.m 0x0
+  //
+  Code[0] = OPCODE_NOP;
+
+  //
+  // Next is simply Addr[62:22] (41 bits) of the address
+  //
+  Code[1] = RShiftU64 (Addr, 22) & 0x1ffffffffff;
+
+  //
+  // Extract bits from the address for insertion into the instruction
+  // i = Addr[63:63]
+  //
+  BitI = RShiftU64 (Addr, 63) & 0x01;
+  //
+  // ic = Addr[21:21]
+  //
+  BitIc = RShiftU64 (Addr, 21) & 0x01;
+  //
+  // imm5c = Addr[20:16] for 5 bits
+  //
+  BitImm5c = RShiftU64 (Addr, 16) & 0x1F;
+  //
+  // imm9d = Addr[15:7] for 9 bits
+  //
+  BitImm9d = RShiftU64 (Addr, 7) & 0x1FF;
+  //
+  // imm7b = Addr[6:0] for 7 bits
+  //
+  BitImm7b = Addr & 0x7F;
+
+  //
+  // The EBC entry point will be put into r8, so r8 can be used here
+  // temporary. R8 is general register and is auto-serialized.
+  //
+  RegNum = 8;
+
+  //
+  // Next is jumbled data, including opcode and rest of address
+  //
+  Code[2] = LShiftU64 (BitImm7b, 13);
+  Code[2] = Code[2] | LShiftU64 (0x00, 20);   // vc
+  Code[2] = Code[2] | LShiftU64 (BitIc, 21);
+  Code[2] = Code[2] | LShiftU64 (BitImm5c, 22);
+  Code[2] = Code[2] | LShiftU64 (BitImm9d, 27);
+  Code[2] = Code[2] | LShiftU64 (BitI, 36);
+  Code[2] = Code[2] | LShiftU64 ((UINT64)MOVL_OPCODE, 37);
+  Code[2] = Code[2] | LShiftU64 ((RegNum & 0x7F), 6);
+
+  WriteBundle ((VOID *) Ptr, 0x05, Code[0], Code[1], Code[2]);
+
+  //
+  // *************************** FIRST BUNDLE ********************************
+  //
+  // Write code bundle for: movl r8 = EBC_ENTRY_POINT so we pass
+  // the ebc entry point in to the interpreter function via a processor
+  // register.
+  // Note -- we could easily change this to pass in a pointer to a structure
+  // that contained, among other things, the EBC image's entry point. But
+  // for now pass it directly.
+  //
+  Ptr += 16;
+  Addr = (UINT64) EbcEntryPoint;
+
+  //
+  // Now generate the code bytes. First is nop.m 0x0
+  //
+  Code[0] = OPCODE_NOP;
+
+  //
+  // Next is simply Addr[62:22] (41 bits) of the address
+  //
+  Code[1] = RShiftU64 (Addr, 22) & 0x1ffffffffff;
+
+  //
+  // Extract bits from the address for insertion into the instruction
+  // i = Addr[63:63]
+  //
+  BitI = RShiftU64 (Addr, 63) & 0x01;
+  //
+  // ic = Addr[21:21]
+  //
+  BitIc = RShiftU64 (Addr, 21) & 0x01;
+  //
+  // imm5c = Addr[20:16] for 5 bits
+  //
+  BitImm5c = RShiftU64 (Addr, 16) & 0x1F;
+  //
+  // imm9d = Addr[15:7] for 9 bits
+  //
+  BitImm9d = RShiftU64 (Addr, 7) & 0x1FF;
+  //
+  // imm7b = Addr[6:0] for 7 bits
+  //
+  BitImm7b = Addr & 0x7F;
+
+  //
+  // Put the EBC entry point in r8, which is the location of the return value
+  // for functions.
+  //
+  RegNum = 8;
+
+  //
+  // Next is jumbled data, including opcode and rest of address
+  //
+  Code[2] = LShiftU64 (BitImm7b, 13);
+  Code[2] = Code[2] | LShiftU64 (0x00, 20);   // vc
+  Code[2] = Code[2] | LShiftU64 (BitIc, 21);
+  Code[2] = Code[2] | LShiftU64 (BitImm5c, 22);
+  Code[2] = Code[2] | LShiftU64 (BitImm9d, 27);
+  Code[2] = Code[2] | LShiftU64 (BitI, 36);
+  Code[2] = Code[2] | LShiftU64 ((UINT64)MOVL_OPCODE, 37);
+  Code[2] = Code[2] | LShiftU64 ((RegNum & 0x7F), 6);
+
+  WriteBundle ((VOID *) Ptr, 0x05, Code[0], Code[1], Code[2]);
+
+  //
+  // *************************** NEXT BUNDLE *********************************
+  //
+  // Write code bundle for:
+  //   movl rx = offset_of(EbcInterpret|ExecuteEbcImageEntryPoint)
+  //
+  // Advance pointer to next bundle, then compute the offset from this bundle
+  // to the address of the entry point of the interpreter.
+  //
+  Ptr += 16;
+  if ((Flags & FLAG_THUNK_ENTRY_POINT) != 0) {
+    Addr = (UINT64) ExecuteEbcImageEntryPoint;
+  } else {
+    Addr = (UINT64) EbcInterpret;
+  }
+  //
+  // Indirection on Itanium-based systems
+  //
+  Addr = *(UINT64 *) Addr;
+
+  //
+  // Now write the code to load the offset into a register
+  //
+  Code[0] = OPCODE_NOP;
+
+  //
+  // Next is simply Addr[62:22] (41 bits) of the address
+  //
+  Code[1] = RShiftU64 (Addr, 22) & 0x1ffffffffff;
+
+  //
+  // Extract bits from the address for insertion into the instruction
+  // i = Addr[63:63]
+  //
+  BitI = RShiftU64 (Addr, 63) & 0x01;
+  //
+  // ic = Addr[21:21]
+  //
+  BitIc = RShiftU64 (Addr, 21) & 0x01;
+  //
+  // imm5c = Addr[20:16] for 5 bits
+  //
+  BitImm5c = RShiftU64 (Addr, 16) & 0x1F;
+  //
+  // imm9d = Addr[15:7] for 9 bits
+  //
+  BitImm9d = RShiftU64 (Addr, 7) & 0x1FF;
+  //
+  // imm7b = Addr[6:0] for 7 bits
+  //
+  BitImm7b = Addr & 0x7F;
+
+  //
+  // Put it in r31, a scratch register
+  //
+  RegNum = 31;
+
+  //
+  // Next is jumbled data, including opcode and rest of address
+  //
+  Code[2] =   LShiftU64(BitImm7b, 13);
+  Code[2] = Code[2] | LShiftU64 (0x00, 20);   // vc
+  Code[2] = Code[2] | LShiftU64 (BitIc, 21);
+  Code[2] = Code[2] | LShiftU64 (BitImm5c, 22);
+  Code[2] = Code[2] | LShiftU64 (BitImm9d, 27);
+  Code[2] = Code[2] | LShiftU64 (BitI, 36);
+  Code[2] = Code[2] | LShiftU64 ((UINT64)MOVL_OPCODE, 37);
+  Code[2] = Code[2] | LShiftU64 ((RegNum & 0x7F), 6);
+
+  WriteBundle ((VOID *) Ptr, 0x05, Code[0], Code[1], Code[2]);
+
+  //
+  // *************************** NEXT BUNDLE *********************************
+  //
+  // Load branch register with EbcInterpret() function offset from the bundle
+  // address: mov b6 = RegNum
+  //
+  // See volume 3 page 4-29 of the Arch. Software Developer's Manual.
+  //
+  // Advance pointer to next bundle
+  //
+  Ptr += 16;
+  Code[0] = OPCODE_NOP;
+  Code[1] = OPCODE_NOP;
+  Code[2] = OPCODE_MOV_BX_RX;
+
+  //
+  // Pick a branch register to use. Then fill in the bits for the branch
+  // register and user register (same user register as previous bundle).
+  //
+  Br = 6;
+  Code[2] |= LShiftU64 (Br, 6);
+  Code[2] |= LShiftU64 (RegNum, 13);
+  WriteBundle ((VOID *) Ptr, 0x0d, Code[0], Code[1], Code[2]);
+
+  //
+  // *************************** NEXT BUNDLE *********************************
+  //
+  // Now do the branch:  (p0) br.cond.sptk.few b6
+  //
+  // Advance pointer to next bundle.
+  // Fill in the bits for the branch register (same reg as previous bundle)
+  //
+  Ptr += 16;
+  Code[0] = OPCODE_NOP;
+  Code[1] = OPCODE_NOP;
+  Code[2] = OPCODE_BR_COND_SPTK_FEW;
+  Code[2] |= LShiftU64 (Br, 13);
+  WriteBundle ((VOID *) Ptr, 0x1d, Code[0], Code[1], Code[2]);
+
+  //
+  // Add the thunk to our list of allocated thunks so we can do some cleanup
+  // when the image is unloaded. Do this last since the Add function flushes
+  // the instruction cache for us.
+  //
+  EbcAddImageThunk (ImageHandle, (VOID *) ThunkBase, ThunkSize);
+
+  //
+  // Done
+  //
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Given raw bytes of Itanium based code, format them into a bundle and
+  write them out.
+
+  @param  MemPtr                 pointer to memory location to write the bundles
+                                 to.
+  @param  Template               5-bit template.
+  @param  Slot0                  Instruction slot 0 data for the bundle.
+  @param  Slot1                  Instruction slot 1 data for the bundle.
+  @param  Slot2                  Instruction slot 2 data for the bundle.
+
+  @retval EFI_INVALID_PARAMETER  Pointer is not aligned
+  @retval EFI_INVALID_PARAMETER  No more than 5 bits in template
+  @retval EFI_INVALID_PARAMETER  More than 41 bits used in code
+  @retval EFI_SUCCESS            All data is written.
+
+**/
+EFI_STATUS
+WriteBundle (
+  IN    VOID    *MemPtr,
+  IN    UINT8   Template,
+  IN    UINT64  Slot0,
+  IN    UINT64  Slot1,
+  IN    UINT64  Slot2
+  )
+{
+  UINT8   *BPtr;
+  UINT32  Index;
+  UINT64  Low64;
+  UINT64  High64;
+
+  //
+  // Verify pointer is aligned
+  //
+  if ((UINT64) MemPtr & 0xF) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Verify no more than 5 bits in template
+  //
+  if ((Template &~0x1F) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Verify max of 41 bits used in code
+  //
+  if (((Slot0 | Slot1 | Slot2) &~0x1ffffffffff) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Low64   = LShiftU64 (Slot1, 46);
+  Low64   = Low64 | LShiftU64 (Slot0, 5) | Template;
+
+  High64  = RShiftU64 (Slot1, 18);
+  High64  = High64 | LShiftU64 (Slot2, 23);
+
+  //
+  // Now write it all out
+  //
+  BPtr = (UINT8 *) MemPtr;
+  for (Index = 0; Index < 8; Index++) {
+    *BPtr = (UINT8) Low64;
+    Low64 = RShiftU64 (Low64, 8);
+    BPtr++;
+  }
+
+  for (Index = 0; Index < 8; Index++) {
+    *BPtr   = (UINT8) High64;
+    High64  = RShiftU64 (High64, 8);
+    BPtr++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function is called to execute an EBC CALLEX instruction.
+  The function check the callee's content to see whether it is common native
+  code or a thunk to another piece of EBC code.
+  If the callee is common native code, use EbcLLCAllEXASM to manipulate,
+  otherwise, set the VM->IP to target EBC code directly to avoid another VM
+  be startup which cost time and stack space.
+
+  @param  VmPtr            Pointer to a VM context.
+  @param  FuncAddr         Callee's address
+  @param  NewStackPointer  New stack pointer after the call
+  @param  FramePtr         New frame pointer after the call
+  @param  Size             The size of call instruction
+
+**/
+VOID
+EbcLLCALLEX (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        FuncAddr,
+  IN UINTN        NewStackPointer,
+  IN VOID         *FramePtr,
+  IN UINT8        Size
+  )
+{
+  UINTN    IsThunk;
+  UINTN    TargetEbcAddr;
+  UINTN    CodeOne18;
+  UINTN    CodeOne23;
+  UINTN    CodeTwoI;
+  UINTN    CodeTwoIc;
+  UINTN    CodeTwo7b;
+  UINTN    CodeTwo5c;
+  UINTN    CodeTwo9d;
+  UINTN    CalleeAddr;
+
+  IsThunk       = 1;
+  TargetEbcAddr = 0;
+
+  //
+  // FuncAddr points to the descriptor of the target instructions.
+  //
+  CalleeAddr = *((UINT64 *)FuncAddr);
+
+  //
+  // Processor specific code to check whether the callee is a thunk to EBC.
+  //
+  if (*((UINT64 *)CalleeAddr) != 0xBCCA000100000005) {
+    IsThunk = 0;
+    goto Action;
+  }
+  if (*((UINT64 *)CalleeAddr + 1) != 0x697623C1004A112E)  {
+    IsThunk = 0;
+    goto Action;
+  }
+
+  CodeOne18 = RShiftU64 (*((UINT64 *)CalleeAddr + 2), 46) & 0x3FFFF;
+  CodeOne23 = (*((UINT64 *)CalleeAddr + 3)) & 0x7FFFFF;
+  CodeTwoI  = RShiftU64 (*((UINT64 *)CalleeAddr + 3), 59) & 0x1;
+  CodeTwoIc = RShiftU64 (*((UINT64 *)CalleeAddr + 3), 44) & 0x1;
+  CodeTwo7b = RShiftU64 (*((UINT64 *)CalleeAddr + 3), 36) & 0x7F;
+  CodeTwo5c = RShiftU64 (*((UINT64 *)CalleeAddr + 3), 45) & 0x1F;
+  CodeTwo9d = RShiftU64 (*((UINT64 *)CalleeAddr + 3), 50) & 0x1FF;
+
+  TargetEbcAddr = CodeTwo7b;
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeTwo9d, 7);
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeTwo5c, 16);
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeTwoIc, 21);
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeOne18, 22);
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeOne23, 40);
+  TargetEbcAddr = TargetEbcAddr | LShiftU64 (CodeTwoI, 63);
+
+Action:
+  if (IsThunk == 1){
+    //
+    // The callee is a thunk to EBC, adjust the stack pointer down 16 bytes and
+    // put our return address and frame pointer on the VM stack.
+    // Then set the VM's IP to new EBC code.
+    //
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMemN (VmPtr, (UINTN) VmPtr->Gpr[0], (UINTN) FramePtr);
+    VmPtr->FramePtr = (VOID *) (UINTN) VmPtr->Gpr[0];
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[0], (UINT64) (VmPtr->Ip + Size));
+
+    VmPtr->Ip = (VMIP) (UINTN) TargetEbcAddr;
+  } else {
+    //
+    // The callee is not a thunk to EBC, call native code,
+    // and get return value.
+    //
+    VmPtr->Gpr[7] = EbcLLCALLEXNative (FuncAddr, NewStackPointer, FramePtr);
+
+    //
+    // Advance the IP.
+    //
+    VmPtr->Ip += Size;
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.h b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.h
new file mode 100644
index 0000000000..d90ea82ad0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/Ipf/EbcSupport.h
@@ -0,0 +1,41 @@
+/** @file
+  Definition of EBC Support function.
+
+Copyright (c) 2006 - 2008, 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.
+
+**/
+
+#ifndef _IPF_EBC_SUPPORT_H_
+#define _IPF_EBC_SUPPORT_H_
+
+#define VM_STACK_SIZE   (1024 * 32)
+
+#define EBC_THUNK_SIZE  128
+#define STACK_REMAIN_SIZE (1024 * 4)
+
+//
+// For code execution, thunks must be aligned on 16-byte boundary
+//
+#define EBC_THUNK_ALIGNMENT 16
+
+//
+// Opcodes for IPF instructions. We'll need to hand-create thunk code (stuffing
+// bits) to insert a jump to the interpreter.
+//
+#define OPCODE_NOP              (UINT64) 0x00008000000
+#define OPCODE_BR_COND_SPTK_FEW (UINT64) 0x00100000000
+#define OPCODE_MOV_BX_RX        (UINT64) 0x00E00100000
+
+//
+// Opcode for MOVL instruction
+//
+#define MOVL_OPCODE 0x06
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.S b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.S
new file mode 100644
index 0000000000..b01486a871
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.S
@@ -0,0 +1,147 @@
+#/** @file
+#  
+#    This code provides low level routines that support the Virtual Machine
+#   for option ROMs.
+#  
+#  Copyright (c) 2007 - 2014, 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.
+#  
+#**/
+
+#---------------------------------------------------------------------------
+# Equate files needed.
+#---------------------------------------------------------------------------
+
+ASM_GLOBAL ASM_PFX(CopyMem);
+ASM_GLOBAL ASM_PFX(EbcInterpret);
+ASM_GLOBAL ASM_PFX(ExecuteEbcImageEntryPoint);
+
+#****************************************************************************
+# EbcLLCALLEX
+#
+# This function is called to execute an EBC CALLEX instruction.
+# This instruction requires that we thunk out to external native
+# code. For x64, we switch stacks, copy the arguments to the stack
+# and jump to the specified function.
+# On return, we restore the stack pointer to its original location.
+#
+# Destroys no working registers.
+#****************************************************************************
+# VOID EbcLLCALLEXNative(UINTN FuncAddr, UINTN NewStackPointer, VOID *FramePtr)
+ASM_GLOBAL ASM_PFX(EbcLLCALLEXNative);
+ASM_PFX(EbcLLCALLEXNative):
+      push   %rbp
+      push   %rbx
+      mov    %rsp, %rbp
+      # Function prolog
+
+      # Copy FuncAddr to a preserved register.
+      mov    %rcx, %rbx
+
+      # Set stack pointer to new value
+      sub    %rdx, %r8  
+      
+      #
+      # Fix X64 native function call prolog. Prepare space for at least 4 arguments,
+      # even if the native function's arguments are less than 4.
+      #
+      # From MSDN x64 Software Conventions, Overview of x64 Calling Conventions:
+      #   "The caller is responsible for allocating space for parameters to the
+      #   callee, and must always allocate sufficient space for the 4 register
+      #   parameters, even if the callee doesn't have that many parameters.
+      #   This aids in the simplicity of supporting C unprototyped functions,
+      #   and vararg C/C++ functions."
+      #
+      cmp    $0x20, %r8
+      jae    skip_expansion
+      mov    $0x20, %r8
+skip_expansion:
+      
+      sub    %r8,  %rsp 
+      
+      #
+      # Fix X64 native function call 16-byte alignment.
+      #
+      # From MSDN x64 Software Conventions, Stack Usage:
+      #   "The stack will always be maintained 16-byte aligned, except within 
+      #   the prolog (for example, after the return address is pushed)."
+      #
+      and    $0xFFFFFFFFFFFFFFF0, %rsp
+      
+      mov    %rsp, %rcx
+      sub    $0x20, %rsp 
+      call   ASM_PFX(CopyMem)
+      add    $0x20, %rsp
+
+      # Considering the worst case, load 4 potiential arguments
+      # into registers.
+      mov    (%rsp), %rcx
+      mov    0x8(%rsp), %rdx
+      mov    0x10(%rsp), %r8
+      mov    0x18(%rsp), %r9
+
+      # Now call the external routine
+      call  *%rbx
+
+      # Function epilog
+      mov      %rbp, %rsp
+      pop      %rbx
+      pop      %rbp
+      ret
+
+ASM_GLOBAL ASM_PFX(EbcLLEbcInterpret);
+ASM_PFX(EbcLLEbcInterpret):
+    # save old parameter to stack
+    mov  %rcx, 0x8(%rsp)
+    mov  %rdx, 0x10(%rsp)
+    mov  %r8, 0x18(%rsp)
+    mov  %r9, 0x20(%rsp)
+
+    # Construct new stack
+    push %rbp
+    mov  %rsp, %rbp
+    push %rsi
+    push %rdi
+    push %rbx
+    sub  $0x80, %rsp
+    push %r10
+    mov  %rbp, %rsi
+    add  $0x10, %rsi
+    mov  %rsp, %rdi
+    add  $0x8, %rdi
+    mov  $0x10, %rcx
+    rep  movsq
+    
+    # build new paramater calling convention
+    mov  0x18(%rsp), %r9
+    mov  0x10(%rsp), %r8
+    mov  0x8(%rsp), %rdx
+    mov  %r10, %rcx
+
+    # call C-code
+    call ASM_PFX(EbcInterpret)
+    add  $0x88, %esp
+    pop  %rbx
+    pop  %rdi
+    pop  %rsi
+    pop  %rbp
+    ret
+
+ASM_GLOBAL ASM_PFX(EbcLLExecuteEbcImageEntryPoint);
+ASM_PFX(EbcLLExecuteEbcImageEntryPoint):
+    # build new paramater calling convention
+    mov  %rdx, %r8
+    mov  %rcx, %rdx
+    mov  %r10, %rcx
+
+    # call C-code
+    sub  $0x28, %rsp
+    call ASM_PFX(ExecuteEbcImageEntryPoint)
+    add  $0x28, %rsp
+    ret
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.asm b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.asm
new file mode 100644
index 0000000000..1fbd165be7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcLowLevel.asm
@@ -0,0 +1,246 @@
+;/** @file
+;  
+;    This code provides low level routines that support the Virtual Machine.
+;    for option ROMs.
+;  
+;  Copyright (c) 2006 - 2011, Intel Corporation. All rights reserved.<BR>
+;  Copyright (c) 2014 Hewlett-Packard Development Company, L.P.<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.
+;  
+;**/
+
+  page    ,132
+  title   VM ASSEMBLY LANGUAGE ROUTINES
+
+;---------------------------------------------------------------------------
+; Equate files needed.
+;---------------------------------------------------------------------------
+
+.CODE
+
+CopyMem  PROTO  Destination:PTR DWORD, Source:PTR DWORD, Count:DWORD
+EbcInterpret               PROTO
+ExecuteEbcImageEntryPoint  PROTO
+
+;****************************************************************************
+; EbcLLCALLEX
+;
+; This function is called to execute an EBC CALLEX instruction.
+; This instruction requires that we thunk out to external native
+; code. For x64, we switch stacks, copy the arguments to the stack
+; and jump to the specified function.
+; On return, we restore the stack pointer to its original location.
+;
+; Destroys no working registers.
+;****************************************************************************
+; INT64 EbcLLCALLEXNative(UINTN FuncAddr, UINTN NewStackPointer, VOID *FramePtr)
+EbcLLCALLEXNative        PROC    PUBLIC
+      push   rbp
+      push   rbx
+      mov    rbp, rsp
+      ; Function prolog
+
+      ; Copy FuncAddr to a preserved register.
+      mov    rbx, rcx
+
+      ; Set stack pointer to new value
+      sub    r8,  rdx
+
+      ;
+      ; Fix X64 native function call prolog. Prepare space for at least 4 arguments,
+      ; even if the native function's arguments are less than 4.
+      ;
+      ; From MSDN x64 Software Conventions, Overview of x64 Calling Conventions:
+      ;   "The caller is responsible for allocating space for parameters to the
+      ;   callee, and must always allocate sufficient space for the 4 register
+      ;   parameters, even if the callee doesn't have that many parameters.
+      ;   This aids in the simplicity of supporting C unprototyped functions,
+      ;   and vararg C/C++ functions."
+      ;
+      cmp    r8, 20h
+      jae    skip_expansion
+      mov    r8, 20h
+skip_expansion:
+      
+      sub    rsp, r8
+
+      ;
+      ; Fix X64 native function call 16-byte alignment.
+      ;
+      ; From MSDN x64 Software Conventions, Stack Usage:
+      ;   "The stack will always be maintained 16-byte aligned, except within 
+      ;   the prolog (for example, after the return address is pushed)."
+      ;
+      and    rsp, NOT 0fh
+
+      mov    rcx, rsp
+      sub    rsp, 20h
+      call   CopyMem
+      add    rsp, 20h
+
+      ; Considering the worst case, load 4 potiential arguments
+      ; into registers.
+      mov    rcx, qword ptr [rsp]
+      mov    rdx, qword ptr [rsp+8h]
+      mov    r8,  qword ptr [rsp+10h]
+      mov    r9,  qword ptr [rsp+18h]
+
+      ; Now call the external routine
+      call  rbx
+
+      ; Function epilog
+      mov      rsp, rbp
+      pop      rbx
+      pop      rbp
+      ret
+EbcLLCALLEXNative    ENDP
+
+;****************************************************************************
+; EbcLLEbcInterpret
+;
+; Begin executing an EBC image.
+;****************************************************************************
+; UINT64 EbcLLEbcInterpret(VOID)
+EbcLLEbcInterpret PROC PUBLIC
+    ;
+    ;; mov rax, ca112ebccall2ebch
+    ;; mov r10, EbcEntryPoint
+    ;; mov r11, EbcLLEbcInterpret
+    ;; jmp r11
+    ;
+    ; Caller uses above instruction to jump here
+    ; The stack is below:
+    ; +-----------+
+    ; |  RetAddr  |
+    ; +-----------+
+    ; |EntryPoint | (R10)
+    ; +-----------+
+    ; |   Arg1    | <- RDI
+    ; +-----------+
+    ; |   Arg2    |
+    ; +-----------+
+    ; |   ...     |
+    ; +-----------+
+    ; |   Arg16   |
+    ; +-----------+
+    ; |   Dummy   |
+    ; +-----------+
+    ; |   RDI     |
+    ; +-----------+
+    ; |   RSI     |
+    ; +-----------+
+    ; |   RBP     | <- RBP
+    ; +-----------+
+    ; |  RetAddr  | <- RSP is here
+    ; +-----------+
+    ; |  Scratch1 | (RCX) <- RSI
+    ; +-----------+
+    ; |  Scratch2 | (RDX)
+    ; +-----------+
+    ; |  Scratch3 | (R8)
+    ; +-----------+
+    ; |  Scratch4 | (R9)
+    ; +-----------+
+    ; |   Arg5    |
+    ; +-----------+
+    ; |   Arg6    |
+    ; +-----------+
+    ; |   ...     |
+    ; +-----------+
+    ; |   Arg16   |
+    ; +-----------+
+    ;
+
+    ; save old parameter to stack
+    mov  [rsp + 08h], rcx
+    mov  [rsp + 10h], rdx
+    mov  [rsp + 18h], r8
+    mov  [rsp + 20h], r9
+
+    ; Construct new stack
+    push rbp
+    mov  rbp, rsp
+    push rsi
+    push rdi
+    push rbx
+    sub  rsp, 80h
+    push r10
+    mov  rsi, rbp
+    add  rsi, 10h
+    mov  rdi, rsp
+    add  rdi, 8
+    mov  rcx, 16
+    rep  movsq
+    
+    ; build new paramater calling convention
+    mov  r9,  [rsp + 18h]
+    mov  r8,  [rsp + 10h]
+    mov  rdx, [rsp + 08h]
+    mov  rcx, r10
+
+    ; call C-code
+    call EbcInterpret
+    add  rsp, 88h
+    pop  rbx
+    pop  rdi
+    pop  rsi
+    pop  rbp
+    ret
+EbcLLEbcInterpret ENDP
+
+;****************************************************************************
+; EbcLLExecuteEbcImageEntryPoint
+;
+; Begin executing an EBC image.
+;****************************************************************************
+; UINT64 EbcLLExecuteEbcImageEntryPoint(VOID)
+EbcLLExecuteEbcImageEntryPoint PROC PUBLIC
+    ;
+    ;; mov rax, ca112ebccall2ebch
+    ;; mov r10, EbcEntryPoint
+    ;; mov r11, EbcLLExecuteEbcImageEntryPoint
+    ;; jmp r11
+    ;
+    ; Caller uses above instruction to jump here
+    ; The stack is below:
+    ; +-----------+
+    ; |  RetAddr  |
+    ; +-----------+
+    ; |EntryPoint | (R10)
+    ; +-----------+
+    ; |ImageHandle|
+    ; +-----------+
+    ; |SystemTable|
+    ; +-----------+
+    ; |   Dummy   |
+    ; +-----------+
+    ; |   Dummy   |
+    ; +-----------+
+    ; |  RetAddr  | <- RSP is here
+    ; +-----------+
+    ; |ImageHandle| (RCX)
+    ; +-----------+
+    ; |SystemTable| (RDX)
+    ; +-----------+
+    ; 
+
+    ; build new paramater calling convention
+    mov  r8, rdx
+    mov  rdx, rcx
+    mov  rcx, r10
+
+    ; call C-code
+    sub  rsp, 28h
+    call ExecuteEbcImageEntryPoint
+    add  rsp, 28h
+    ret
+EbcLLExecuteEbcImageEntryPoint ENDP
+
+END
+
diff --git a/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcSupport.c b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcSupport.c
new file mode 100644
index 0000000000..bdde5e41fa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EbcDxe/X64/EbcSupport.c
@@ -0,0 +1,573 @@
+/** @file
+  This module contains EBC support routines that are customized based on
+  the target x64 processor.
+
+Copyright (c) 2006 - 2014, 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 "EbcInt.h"
+#include "EbcExecute.h"
+
+//
+// NOTE: This is the stack size allocated for the interpreter
+//       when it executes an EBC image. The requirements can change
+//       based on whether or not a debugger is present, and other
+//       platform-specific configurations.
+//
+#define VM_STACK_SIZE   (1024 * 8)
+
+#define STACK_REMAIN_SIZE (1024 * 4)
+
+//
+// This is instruction buffer used to create EBC thunk
+//
+#define EBC_ENTRYPOINT_SIGNATURE           0xAFAFAFAFAFAFAFAFull
+#define EBC_LL_EBC_ENTRYPOINT_SIGNATURE    0xFAFAFAFAFAFAFAFAull
+UINT8  mInstructionBufferTemplate[] = {
+  //
+  // Add a magic code here to help the VM recognize the thunk..
+  // mov rax, 0xca112ebcca112ebc  => 48 B8 BC 2E 11 CA BC 2E 11 CA
+  //
+  0x48, 0xB8, 0xBC, 0x2E, 0x11, 0xCA, 0xBC, 0x2E, 0x11, 0xCA,
+  //
+  // Add code bytes to load up a processor register with the EBC entry point.
+  //  mov r10, EbcEntryPoint  => 49 BA XX XX XX XX XX XX XX XX (To be fixed at runtime)
+  // These 8 bytes of the thunk entry is the address of the EBC
+  // entry point.
+  //
+  0x49, 0xBA, 
+    (UINT8)(EBC_ENTRYPOINT_SIGNATURE & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 8) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 16) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 24) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 32) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 40) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 48) & 0xFF),
+    (UINT8)((EBC_ENTRYPOINT_SIGNATURE >> 56) & 0xFF),
+  //
+  // Stick in a load of r11 with the address of appropriate VM function.
+  //  mov r11, EbcLLEbcInterpret  => 49 BB XX XX XX XX XX XX XX XX (To be fixed at runtime)
+  //
+  0x49, 0xBB,
+    (UINT8)(EBC_LL_EBC_ENTRYPOINT_SIGNATURE & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 8) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 16) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 24) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 32) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 40) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 48) & 0xFF),
+    (UINT8)((EBC_LL_EBC_ENTRYPOINT_SIGNATURE >> 56) & 0xFF),
+  //
+  // Stick in jump opcode bytes
+  //  jmp r11 => 41 FF E3
+  //
+  0x41, 0xFF, 0xE3,
+};
+
+/**
+  Begin executing an EBC image.
+  This is used for Ebc Thunk call.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcLLEbcInterpret (
+  VOID
+  );
+
+/**
+  Begin executing an EBC image.
+  This is used for Ebc image entrypoint.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcLLExecuteEbcImageEntryPoint (
+  VOID
+  );
+
+/**
+  Pushes a 64 bit unsigned value to the VM stack.
+
+  @param VmPtr  The pointer to current VM context.
+  @param Arg    The value to be pushed.
+
+**/
+VOID
+PushU64 (
+  IN VM_CONTEXT *VmPtr,
+  IN UINT64     Arg
+  )
+{
+  //
+  // Advance the VM stack down, and then copy the argument to the stack.
+  // Hope it's aligned.
+  //
+  VmPtr->Gpr[0] -= sizeof (UINT64);
+  *(UINT64 *) VmPtr->Gpr[0] = Arg;
+  return;
+}
+
+
+/**
+  Begin executing an EBC image.
+
+  This is a thunk function. Microsoft x64 compiler only provide fast_call
+  calling convention, so the first four arguments are passed by rcx, rdx,
+  r8, and r9, while other arguments are passed in stack.
+
+  @param  EntryPoint            The entrypoint of EBC code.
+  @param  Arg1                  The 1st argument.
+  @param  Arg2                  The 2nd argument.
+  @param  Arg3                  The 3rd argument.
+  @param  Arg4                  The 4th argument.
+  @param  Arg5                  The 5th argument.
+  @param  Arg6                  The 6th argument.
+  @param  Arg7                  The 7th argument.
+  @param  Arg8                  The 8th argument.
+  @param  Arg9                  The 9th argument.
+  @param  Arg10                 The 10th argument.
+  @param  Arg11                 The 11th argument.
+  @param  Arg12                 The 12th argument.
+  @param  Arg13                 The 13th argument.
+  @param  Arg14                 The 14th argument.
+  @param  Arg15                 The 15th argument.
+  @param  Arg16                 The 16th argument.
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+EbcInterpret (
+  IN UINTN      EntryPoint,
+  IN UINTN      Arg1,
+  IN UINTN      Arg2,
+  IN UINTN      Arg3,
+  IN UINTN      Arg4,
+  IN UINTN      Arg5,
+  IN UINTN      Arg6,
+  IN UINTN      Arg7,
+  IN UINTN      Arg8,
+  IN UINTN      Arg9,
+  IN UINTN      Arg10,
+  IN UINTN      Arg11,
+  IN UINTN      Arg12,
+  IN UINTN      Arg13,
+  IN UINTN      Arg14,
+  IN UINTN      Arg15,
+  IN UINTN      Arg16
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+
+  //
+  // Get the EBC entry point
+  //
+  Addr = EntryPoint;
+
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+
+  //
+  // Initialize the stack pointer for the EBC. Get the current system stack
+  // pointer and adjust it down by the max needed for the interpreter.
+  //
+
+  //
+  // Adjust the VM's stack pointer down.
+  //
+
+  Status = GetEBCStack((EFI_HANDLE)(UINTN)-1, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN) VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+  //
+  // Align the stack on a natural boundary.
+  //
+  VmContext.Gpr[0] &= ~(VM_REGISTER)(sizeof (UINTN) - 1);
+
+  //
+  // Put a magic value in the stack gap, then adjust down again.
+  //
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) VM_STACK_KEY_VALUE;
+  VmContext.StackMagicPtr             = (UINTN *) (UINTN) VmContext.Gpr[0];
+
+  //
+  // The stack upper to LowStackTop is belong to the VM.
+  //
+  VmContext.LowStackTop   = (UINTN) VmContext.Gpr[0];
+
+  //
+  // For the worst case, assume there are 4 arguments passed in registers, store
+  // them to VM's stack.
+  //
+  PushU64 (&VmContext, (UINT64) Arg16);
+  PushU64 (&VmContext, (UINT64) Arg15);
+  PushU64 (&VmContext, (UINT64) Arg14);
+  PushU64 (&VmContext, (UINT64) Arg13);
+  PushU64 (&VmContext, (UINT64) Arg12);
+  PushU64 (&VmContext, (UINT64) Arg11);
+  PushU64 (&VmContext, (UINT64) Arg10);
+  PushU64 (&VmContext, (UINT64) Arg9);
+  PushU64 (&VmContext, (UINT64) Arg8);
+  PushU64 (&VmContext, (UINT64) Arg7);
+  PushU64 (&VmContext, (UINT64) Arg6);
+  PushU64 (&VmContext, (UINT64) Arg5);
+  PushU64 (&VmContext, (UINT64) Arg4);
+  PushU64 (&VmContext, (UINT64) Arg3);
+  PushU64 (&VmContext, (UINT64) Arg2);
+  PushU64 (&VmContext, (UINT64) Arg1);
+
+  //
+  // Interpreter assumes 64-bit return address is pushed on the stack.
+  // The x64 does not do this so pad the stack accordingly.
+  //
+  PushU64 (&VmContext, (UINT64) 0);
+  PushU64 (&VmContext, (UINT64) 0x1234567887654321ULL);
+
+  //
+  // For x64, this is where we say our return address is
+  //
+  VmContext.StackRetAddr  = (UINT64) VmContext.Gpr[0];
+
+  //
+  // We need to keep track of where the EBC stack starts. This way, if the EBC
+  // accesses any stack variables above its initial stack setting, then we know
+  // it's accessing variables passed into it, which means the data is on the
+  // VM's stack.
+  // When we're called, on the stack (high to low) we have the parameters, the
+  // return address, then the saved ebp. Save the pointer to the return address.
+  // EBC code knows that's there, so should look above it for function parameters.
+  // The offset is the size of locals (VMContext + Addr + saved ebp).
+  // Note that the interpreter assumes there is a 16 bytes of return address on
+  // the stack too, so adjust accordingly.
+  //  VmContext.HighStackBottom = (UINTN)(Addr + sizeof (VmContext) + sizeof (Addr));
+  //
+
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Begin executing an EBC image.
+
+  @param  EntryPoint       The entrypoint of EBC code.
+  @param  ImageHandle      image handle for the EBC application we're executing
+  @param  SystemTable      standard system table passed into an driver's entry
+                           point
+
+  @return The value returned by the EBC application we're going to run.
+
+**/
+UINT64
+EFIAPI
+ExecuteEbcImageEntryPoint (
+  IN UINTN                EntryPoint,
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  //
+  // Create a new VM context on the stack
+  //
+  VM_CONTEXT  VmContext;
+  UINTN       Addr;
+  EFI_STATUS  Status;
+  UINTN       StackIndex;
+
+  //
+  // Get the EBC entry point
+  //
+  Addr = EntryPoint;
+
+  //
+  // Now clear out our context
+  //
+  ZeroMem ((VOID *) &VmContext, sizeof (VM_CONTEXT));
+
+  //
+  // Save the image handle so we can track the thunks created for this image
+  //
+  VmContext.ImageHandle = ImageHandle;
+  VmContext.SystemTable = SystemTable;
+
+  //
+  // Set the VM instruction pointer to the correct location in memory.
+  //
+  VmContext.Ip = (VMIP) Addr;
+
+  //
+  // Initialize the stack pointer for the EBC. Get the current system stack
+  // pointer and adjust it down by the max needed for the interpreter.
+  //
+
+  Status = GetEBCStack(ImageHandle, &VmContext.StackPool, &StackIndex);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  VmContext.StackTop = (UINT8*)VmContext.StackPool + (STACK_REMAIN_SIZE);
+  VmContext.Gpr[0] = (UINT64) ((UINT8*)VmContext.StackPool + STACK_POOL_SIZE);
+  VmContext.HighStackBottom = (UINTN) VmContext.Gpr[0];
+  VmContext.Gpr[0] -= sizeof (UINTN);
+
+
+  //
+  // Put a magic value in the stack gap, then adjust down again
+  //
+  *(UINTN *) (UINTN) (VmContext.Gpr[0]) = (UINTN) VM_STACK_KEY_VALUE;
+  VmContext.StackMagicPtr             = (UINTN *) (UINTN) VmContext.Gpr[0];
+
+  //
+  // Align the stack on a natural boundary
+  VmContext.Gpr[0] &= ~(VM_REGISTER)(sizeof(UINTN) - 1);
+  //
+  VmContext.LowStackTop   = (UINTN) VmContext.Gpr[0];
+
+  //
+  // Simply copy the image handle and system table onto the EBC stack.
+  // Greatly simplifies things by not having to spill the args.
+  //
+  PushU64 (&VmContext, (UINT64) SystemTable);
+  PushU64 (&VmContext, (UINT64) ImageHandle);
+
+  //
+  // VM pushes 16-bytes for return address. Simulate that here.
+  //
+  PushU64 (&VmContext, (UINT64) 0);
+  PushU64 (&VmContext, (UINT64) 0x1234567887654321ULL);
+
+  //
+  // For x64, this is where we say our return address is
+  //
+  VmContext.StackRetAddr  = (UINT64) VmContext.Gpr[0];
+
+  //
+  // Entry function needn't access high stack context, simply
+  // put the stack pointer here.
+  //
+
+  //
+  // Begin executing the EBC code
+  //
+  EbcExecute (&VmContext);
+
+  //
+  // Return the value in R[7] unless there was an error
+  //
+  ReturnEBCStack(StackIndex);
+  return (UINT64) VmContext.Gpr[7];
+}
+
+
+/**
+  Create thunks for an EBC image entry point, or an EBC protocol service.
+
+  @param  ImageHandle           Image handle for the EBC image. If not null, then
+                                we're creating a thunk for an image entry point.
+  @param  EbcEntryPoint         Address of the EBC code that the thunk is to call
+  @param  Thunk                 Returned thunk we create here
+  @param  Flags                 Flags indicating options for creating the thunk
+
+  @retval EFI_SUCCESS           The thunk was created successfully.
+  @retval EFI_INVALID_PARAMETER The parameter of EbcEntryPoint is not 16-bit
+                                aligned.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory to created the EBC
+                                Thunk.
+  @retval EFI_BUFFER_TOO_SMALL  EBC_THUNK_SIZE is not larger enough.
+
+**/
+EFI_STATUS
+EbcCreateThunks (
+  IN EFI_HANDLE           ImageHandle,
+  IN VOID                 *EbcEntryPoint,
+  OUT VOID                **Thunk,
+  IN  UINT32              Flags
+  )
+{
+  UINT8       *Ptr;
+  UINT8       *ThunkBase;
+  UINT32      Index;
+  INT32       ThunkSize;
+
+  //
+  // Check alignment of pointer to EBC code
+  //
+  if ((UINT32) (UINTN) EbcEntryPoint & 0x01) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ThunkSize = sizeof(mInstructionBufferTemplate);
+
+  Ptr = AllocatePool (sizeof(mInstructionBufferTemplate));
+
+  if (Ptr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  //  Print(L"Allocate TH: 0x%X\n", (UINT32)Ptr);
+  //
+  // Save the start address so we can add a pointer to it to a list later.
+  //
+  ThunkBase = Ptr;
+
+  //
+  // Give them the address of our buffer we're going to fix up
+  //
+  *Thunk = (VOID *) Ptr;
+
+  //
+  // Copy whole thunk instruction buffer template
+  //
+  CopyMem (Ptr, mInstructionBufferTemplate, sizeof(mInstructionBufferTemplate));
+
+  //
+  // Patch EbcEntryPoint and EbcLLEbcInterpret
+  //
+  for (Index = 0; Index < sizeof(mInstructionBufferTemplate) - sizeof(UINTN); Index++) {
+    if (*(UINTN *)&Ptr[Index] == EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&Ptr[Index] = (UINTN)EbcEntryPoint;
+    }
+    if (*(UINTN *)&Ptr[Index] == EBC_LL_EBC_ENTRYPOINT_SIGNATURE) {
+      if ((Flags & FLAG_THUNK_ENTRY_POINT) != 0) {
+        *(UINTN *)&Ptr[Index] = (UINTN)EbcLLExecuteEbcImageEntryPoint;
+      } else {
+        *(UINTN *)&Ptr[Index] = (UINTN)EbcLLEbcInterpret;
+      }
+    }
+  }
+
+  //
+  // Add the thunk to the list for this image. Do this last since the add
+  // function flushes the cache for us.
+  //
+  EbcAddImageThunk (ImageHandle, (VOID *) ThunkBase, ThunkSize);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function is called to execute an EBC CALLEX instruction.
+  The function check the callee's content to see whether it is common native
+  code or a thunk to another piece of EBC code.
+  If the callee is common native code, use EbcLLCAllEXASM to manipulate,
+  otherwise, set the VM->IP to target EBC code directly to avoid another VM
+  be startup which cost time and stack space.
+
+  @param  VmPtr            Pointer to a VM context.
+  @param  FuncAddr         Callee's address
+  @param  NewStackPointer  New stack pointer after the call
+  @param  FramePtr         New frame pointer after the call
+  @param  Size             The size of call instruction
+
+**/
+VOID
+EbcLLCALLEX (
+  IN VM_CONTEXT   *VmPtr,
+  IN UINTN        FuncAddr,
+  IN UINTN        NewStackPointer,
+  IN VOID         *FramePtr,
+  IN UINT8        Size
+  )
+{
+  UINTN    IsThunk;
+  UINTN    TargetEbcAddr;
+  UINT8    InstructionBuffer[sizeof(mInstructionBufferTemplate)];
+  UINTN    Index;
+  UINTN    IndexOfEbcEntrypoint;
+
+  IsThunk       = 1;
+  TargetEbcAddr = 0;
+  IndexOfEbcEntrypoint = 0;
+
+  //
+  // Processor specific code to check whether the callee is a thunk to EBC.
+  //
+  CopyMem (InstructionBuffer, (VOID *)FuncAddr, sizeof(InstructionBuffer));
+  //
+  // Fill the signature according to mInstructionBufferTemplate
+  //
+  for (Index = 0; Index < sizeof(mInstructionBufferTemplate) - sizeof(UINTN); Index++) {
+    if (*(UINTN *)&mInstructionBufferTemplate[Index] == EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&InstructionBuffer[Index] = EBC_ENTRYPOINT_SIGNATURE;
+      IndexOfEbcEntrypoint = Index;
+    }
+    if (*(UINTN *)&mInstructionBufferTemplate[Index] == EBC_LL_EBC_ENTRYPOINT_SIGNATURE) {
+      *(UINTN *)&InstructionBuffer[Index] = EBC_LL_EBC_ENTRYPOINT_SIGNATURE;
+    }
+  }
+  //
+  // Check if we need thunk to native
+  //
+  if (CompareMem (InstructionBuffer, mInstructionBufferTemplate, sizeof(mInstructionBufferTemplate)) != 0) {
+    IsThunk = 0;
+  }
+
+  if (IsThunk == 1){
+    //
+    // The callee is a thunk to EBC, adjust the stack pointer down 16 bytes and
+    // put our return address and frame pointer on the VM stack.
+    // Then set the VM's IP to new EBC code.
+    //
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMemN (VmPtr, (UINTN) VmPtr->Gpr[0], (UINTN) FramePtr);
+    VmPtr->FramePtr = (VOID *) (UINTN) VmPtr->Gpr[0];
+    VmPtr->Gpr[0] -= 8;
+    VmWriteMem64 (VmPtr, (UINTN) VmPtr->Gpr[0], (UINT64) (UINTN) (VmPtr->Ip + Size));
+
+    CopyMem (&TargetEbcAddr, (UINT8 *)FuncAddr + IndexOfEbcEntrypoint, sizeof(UINTN));
+    VmPtr->Ip = (VMIP) (UINTN) TargetEbcAddr;
+  } else {
+    //
+    // The callee is not a thunk to EBC, call native code,
+    // and get return value.
+    //
+    VmPtr->Gpr[7] = EbcLLCALLEXNative (FuncAddr, NewStackPointer, FramePtr);
+
+    //
+    // Advance the IP.
+    //
+    VmPtr->Ip += Size;
+  }
+}
+
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.c b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.c
new file mode 100644
index 0000000000..16b18e0a54
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.c
@@ -0,0 +1,667 @@
+/** @file
+  Esrt management module.
+
+Copyright (c) 2015, 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 "EsrtImpl.h"
+
+
+//
+// Module globals.
+//
+
+ESRT_PRIVATE_DATA mPrivate;
+
+ESRT_MANAGEMENT_PROTOCOL  mEsrtManagementProtocolTemplate = { 
+                            EsrtDxeGetEsrtEntry,
+                            EsrtDxeUpdateEsrtEntry,
+                            EsrtDxeRegisterEsrtEntry,
+                            EsrtDxeUnRegisterEsrtEntry,
+                            EsrtDxeSyncFmp,
+                            EsrtDxeLockEsrtRepository
+                            };
+
+/**
+  Get ESRT entry from ESRT Cache by FwClass Guid 
+
+  @param[in]       FwClass                FwClass of Esrt entry to get  
+  @param[in, out]  Entry                  Esrt entry returned 
+  
+  @retval EFI_SUCCESS                   The variable saving this Esrt Entry exists.
+  @retval EF_NOT_FOUND                  No correct variable found.
+  @retval EFI_WRITE_PROTECTED           ESRT Cache repository is locked
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeGetEsrtEntry(
+  IN     EFI_GUID                  *FwClass,
+  IN OUT EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  )
+{
+  EFI_STATUS                Status;
+
+  if (FwClass == NULL || Entry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EfiAcquireLockOrFail (&mPrivate.NonFmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Find in Non-FMP Cached Esrt Repository
+  //
+  Status = GetEsrtEntry(
+             FwClass,
+             ESRT_FROM_NONFMP,
+             Entry
+             );
+
+  EfiReleaseLock(&mPrivate.NonFmpLock);
+
+  if (EFI_ERROR(Status)) {
+    Status = EfiAcquireLockOrFail (&mPrivate.FmpLock);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Find in FMP Cached Esrt NV Variable
+    //
+    Status = GetEsrtEntry(
+               FwClass,
+               ESRT_FROM_FMP,
+               Entry
+               );
+
+    EfiReleaseLock(&mPrivate.FmpLock);
+  }
+
+  return Status;
+}
+
+/**
+  Update one ESRT entry in ESRT Cache.
+
+  @param[in]  Entry                         Esrt entry to be updated
+  
+  @retval EFI_SUCCESS                   Successfully update an ESRT entry in cache.
+  @retval EFI_INVALID_PARAMETER         Entry does't exist in ESRT Cache
+  @retval EFI_WRITE_PROTECTED           ESRT Cache repositoy is locked
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeUpdateEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  )
+{
+  EFI_STATUS                Status;
+  
+  if (Entry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EfiAcquireLockOrFail (&mPrivate.FmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UpdateEsrtEntry(Entry, ESRT_FROM_FMP);
+
+  if (!EFI_ERROR(Status)) {
+    EfiReleaseLock(&mPrivate.FmpLock);
+    return Status;
+  }
+  EfiReleaseLock(&mPrivate.FmpLock);
+
+
+  Status = EfiAcquireLockOrFail (&mPrivate.NonFmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = UpdateEsrtEntry(Entry, ESRT_FROM_NONFMP);
+
+  EfiReleaseLock(&mPrivate.NonFmpLock);
+
+  return Status;
+}
+
+/**
+  Non-FMP instance to unregister Esrt Entry from ESRT Cache. 
+
+  @param[in]    FwClass                FwClass of Esrt entry to Unregister  
+  
+  @retval EFI_SUCCESS             Insert all entries Successfully 
+  @retval EFI_NOT_FOUND           Entry of FwClass does not exsit
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeUnRegisterEsrtEntry(
+  IN  EFI_GUID        *FwClass
+  )
+{
+  EFI_STATUS Status; 
+
+  if (FwClass == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EfiAcquireLockOrFail (&mPrivate.NonFmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = DeleteEsrtEntry(FwClass, ESRT_FROM_NONFMP);
+
+  EfiReleaseLock(&mPrivate.NonFmpLock);
+
+  return Status;
+}
+
+/**
+  Non-FMP instance to register one ESRT entry into ESRT Cache.
+
+  @param[in]  Entry                Esrt entry to be set
+
+  @retval EFI_SUCCESS              Successfully set a variable.
+  @retval EFI_INVALID_PARAMETER    ESRT Entry is already exist
+  @retval EFI_OUT_OF_RESOURCES     Non-FMP ESRT repository is full
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeRegisterEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  )
+{
+  EFI_STATUS                Status;
+  EFI_SYSTEM_RESOURCE_ENTRY EsrtEntryTmp;
+
+  if (Entry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = EfiAcquireLockOrFail (&mPrivate.NonFmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = GetEsrtEntry(
+             &Entry->FwClass,
+             ESRT_FROM_NONFMP,
+             &EsrtEntryTmp
+             );
+
+  if (Status == EFI_NOT_FOUND) {
+    Status = InsertEsrtEntry(Entry, ESRT_FROM_NONFMP);
+  }
+
+  EfiReleaseLock(&mPrivate.NonFmpLock);
+
+  return Status;
+}
+
+/**
+  This function syn up Cached ESRT with data from FMP instances
+  Function should be called after Connect All in order to locate all FMP protocols
+  installed.
+
+  @retval EFI_SUCCESS                      Successfully sync cache repository from FMP instances
+  @retval EFI_NOT_FOUND                   No FMP Instance are found
+  @retval EFI_OUT_OF_RESOURCES     Resource allocaton fail
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeSyncFmp(
+  VOID
+  ) 
+{
+  EFI_STATUS                                Status;
+  UINTN                                     Index1;
+  UINTN                                     Index2;
+  UINTN                                     Index3;
+  EFI_HANDLE                                *HandleBuffer;
+  EFI_FIRMWARE_MANAGEMENT_PROTOCOL          **FmpBuf;
+  UINTN                                     NumberOfHandles;
+  UINTN                                     *DescriptorSizeBuf;
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR             **FmpImageInfoBuf;  
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR             *TempFmpImageInfo;  
+  UINT8                                     *FmpImageInfoCountBuf;
+  UINT32                                    *FmpImageInfoDescriptorVerBuf;
+  UINTN                                     ImageInfoSize;
+  UINT32                                    PackageVersion;
+  CHAR16                                    *PackageVersionName;
+  EFI_SYSTEM_RESOURCE_ENTRY                 *EsrtRepositoryNew;
+  UINTN                                     EntryNumNew;
+
+  NumberOfHandles              = 0;
+  EntryNumNew                  = 0;
+  FmpBuf                       = NULL;
+  HandleBuffer                 = NULL;
+  FmpImageInfoBuf              = NULL;
+  FmpImageInfoCountBuf         = NULL;
+  PackageVersionName           = NULL;
+  DescriptorSizeBuf            = NULL;
+  FmpImageInfoDescriptorVerBuf = NULL;
+  EsrtRepositoryNew            = NULL;
+
+  //
+  // Get image information from all FMP protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareManagementProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+
+ 
+  if (Status == EFI_NOT_FOUND) { 
+    EntryNumNew = 0;
+    goto UPDATE_REPOSITORY;
+  } else if (EFI_ERROR(Status)){ 
+    goto END;
+  }
+
+  //
+  // Allocate buffer to hold new FMP ESRT Cache repository
+  //
+  EsrtRepositoryNew = AllocateZeroPool(PcdGet32(PcdMaxFmpEsrtCacheNum) * sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+  if (EsrtRepositoryNew == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto END;
+  }
+
+  FmpBuf = AllocatePool(sizeof(EFI_FIRMWARE_MANAGEMENT_PROTOCOL *) * NumberOfHandles);
+  if (FmpBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto END;
+  }
+
+  FmpImageInfoBuf = AllocateZeroPool(sizeof(EFI_FIRMWARE_IMAGE_DESCRIPTOR *) * NumberOfHandles);
+  if (FmpImageInfoBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;    
+    goto END;
+  }
+
+  FmpImageInfoCountBuf = AllocateZeroPool(sizeof(UINT8) * NumberOfHandles);
+  if (FmpImageInfoCountBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto END;
+  }
+
+  DescriptorSizeBuf = AllocateZeroPool(sizeof(UINTN) * NumberOfHandles);
+  if (DescriptorSizeBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto END;
+  }
+
+  FmpImageInfoDescriptorVerBuf = AllocateZeroPool(sizeof(UINT32) * NumberOfHandles);
+   if (FmpImageInfoDescriptorVerBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto END;
+  }
+
+  //
+  // Get all FmpImageInfo Descriptor into FmpImageInfoBuf
+  //
+  for (Index1 = 0; Index1 < NumberOfHandles; Index1++){
+    Status = gBS->HandleProtocol(
+                    HandleBuffer[Index1],
+                    &gEfiFirmwareManagementProtocolGuid,
+                    (VOID **)&FmpBuf[Index1]
+                    );
+
+    if (EFI_ERROR(Status)) {
+      continue;
+    }
+
+    ImageInfoSize = 0;
+    Status = FmpBuf[Index1]->GetImageInfo (
+                               FmpBuf[Index1],
+                               &ImageInfoSize,
+                               NULL,
+                               NULL,
+                               NULL,
+                               NULL,
+                               NULL,
+                               NULL
+                               );
+
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      FmpImageInfoBuf[Index1] = AllocateZeroPool(ImageInfoSize);
+      if (FmpImageInfoBuf[Index1] == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto END;
+      }
+    } else {
+      continue;
+    }
+
+    PackageVersionName = NULL;
+    Status = FmpBuf[Index1]->GetImageInfo (
+                               FmpBuf[Index1],
+                               &ImageInfoSize,
+                               FmpImageInfoBuf[Index1],
+                               &FmpImageInfoDescriptorVerBuf[Index1],
+                               &FmpImageInfoCountBuf[Index1],
+                               &DescriptorSizeBuf[Index1],
+                               &PackageVersion,
+                               &PackageVersionName
+                               );
+
+    //
+    // If FMP GetInformation interface failed, skip this resource
+    //
+    if (EFI_ERROR(Status)){
+      FmpImageInfoCountBuf[Index1] = 0;
+      continue;
+    }
+
+    if (PackageVersionName != NULL) {
+      FreePool(PackageVersionName);
+    }
+  }
+
+  //
+  // Create new FMP cache repository based on FmpImageInfoBuf
+  // 
+  for (Index2 = 0; Index2 < NumberOfHandles; Index2++){
+    TempFmpImageInfo = FmpImageInfoBuf[Index2];
+    for (Index3 = 0; Index3 < FmpImageInfoCountBuf[Index2]; Index3++){
+      if ((TempFmpImageInfo->AttributesSupported & IMAGE_ATTRIBUTE_IN_USE) != 0 
+      && (TempFmpImageInfo->AttributesSetting & IMAGE_ATTRIBUTE_IN_USE) != 0){
+        //
+        // Always put the first smallest version of Image info into ESRT cache 
+        //
+        for(Index1 = 0; Index1 < EntryNumNew; Index1++) {
+          if (CompareGuid(&EsrtRepositoryNew[Index1].FwClass, &TempFmpImageInfo->ImageTypeId)) {
+            if(EsrtRepositoryNew[Index1].FwVersion > TempFmpImageInfo->Version) {
+              SetEsrtEntryFromFmpInfo(&EsrtRepositoryNew[Index1], TempFmpImageInfo, FmpImageInfoDescriptorVerBuf[Index2]);
+            }
+            break;
+          }
+        }
+        //
+        // New ImageTypeId can't be found in EsrtRepositoryNew. Create a new one
+        //
+        if (Index1 == EntryNumNew){
+          SetEsrtEntryFromFmpInfo(&EsrtRepositoryNew[EntryNumNew], TempFmpImageInfo, FmpImageInfoDescriptorVerBuf[Index2]);
+          EntryNumNew++; 
+          if (EntryNumNew >= PcdGet32(PcdMaxFmpEsrtCacheNum)) {
+            break;
+          }
+        }
+      }
+
+      //
+      // Use DescriptorSize to move ImageInfo Pointer to stay compatible with different ImageInfo version
+      //
+      TempFmpImageInfo = (EFI_FIRMWARE_IMAGE_DESCRIPTOR *)((UINT8 *)TempFmpImageInfo + DescriptorSizeBuf[Index2]);
+    }
+  }
+
+UPDATE_REPOSITORY:
+
+  Status = EfiAcquireLockOrFail (&mPrivate.FmpLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gRT->SetVariable(
+                  EFI_ESRT_FMP_VARIABLE_NAME,
+                  &gEfiCallerIdGuid,
+                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  EntryNumNew * sizeof(EFI_SYSTEM_RESOURCE_ENTRY),
+                  EsrtRepositoryNew
+                  );
+
+  EfiReleaseLock(&mPrivate.FmpLock);
+
+END:
+  if (EsrtRepositoryNew != NULL) {
+    FreePool(EsrtRepositoryNew);
+  }
+
+  if (HandleBuffer != NULL) {
+    FreePool(HandleBuffer);
+  }
+
+  if (FmpBuf != NULL) {
+    FreePool(FmpBuf);
+  }
+
+  if (FmpImageInfoCountBuf != NULL) {
+    FreePool(FmpImageInfoCountBuf);
+  }
+
+  if (DescriptorSizeBuf != NULL) {
+    FreePool(DescriptorSizeBuf);
+  }
+
+  if (FmpImageInfoDescriptorVerBuf != NULL) {
+    FreePool(FmpImageInfoDescriptorVerBuf);
+  }
+
+  if (FmpImageInfoBuf != NULL) {
+    for (Index1 = 0; Index1 < NumberOfHandles; Index1++){
+      if (FmpImageInfoBuf[Index1] != NULL) {
+        FreePool(FmpImageInfoBuf[Index1]);
+      }
+    }
+    FreePool(FmpImageInfoBuf);
+  }
+
+  return Status;
+}
+
+/**
+  This function locks up Esrt repository to be readonly. It should be called 
+  before gEfiEndOfDxeEventGroupGuid event signaled
+
+  @retval EFI_SUCCESS              Locks up FMP Non-FMP repository successfully 
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeLockEsrtRepository(
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  EDKII_VARIABLE_LOCK_PROTOCOL  *VariableLock;
+  //
+  // Mark ACPI_GLOBAL_VARIABLE variable to read-only if the Variable Lock protocol exists
+  //
+  Status = gBS->LocateProtocol (&gEdkiiVariableLockProtocolGuid, NULL, (VOID **) &VariableLock);
+  if (!EFI_ERROR (Status)) {
+    Status = VariableLock->RequestToLock (VariableLock, EFI_ESRT_FMP_VARIABLE_NAME, &gEfiCallerIdGuid);
+    DEBUG((EFI_D_INFO, "EsrtDxe Lock EsrtFmp Variable Status 0x%x", Status));
+
+    Status = VariableLock->RequestToLock (VariableLock, EFI_ESRT_NONFMP_VARIABLE_NAME, &gEfiCallerIdGuid);
+    DEBUG((EFI_D_INFO, "EsrtDxe Lock EsrtNonFmp Variable Status 0x%x", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Notify function for event group EFI_EVENT_GROUP_READY_TO_BOOT. This is used to
+  install the Esrt Table into system configuration table
+
+  @param[in]  Event   The Event that is being processed.
+  @param[in]  Context The Event Context.
+
+**/
+VOID
+EFIAPI
+EsrtReadyToBootEventNotify (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{  
+  EFI_STATUS                 Status;
+  EFI_SYSTEM_RESOURCE_TABLE  *EsrtTable;
+  EFI_SYSTEM_RESOURCE_ENTRY  *FmpEsrtRepository;
+  EFI_SYSTEM_RESOURCE_ENTRY  *NonFmpEsrtRepository;
+  UINTN                      FmpRepositorySize;
+  UINTN                      NonFmpRepositorySize;
+  
+
+  FmpEsrtRepository    = NULL;
+  NonFmpEsrtRepository = NULL;
+  FmpRepositorySize    = 0;
+  NonFmpRepositorySize = 0;
+
+  Status = EfiAcquireLockOrFail (&mPrivate.NonFmpLock);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  Status = GetVariable2 (
+             EFI_ESRT_NONFMP_VARIABLE_NAME,
+             &gEfiCallerIdGuid,
+             (VOID **) &NonFmpEsrtRepository,
+             &NonFmpRepositorySize
+             );
+
+  if (EFI_ERROR(Status)) {
+    NonFmpRepositorySize = 0;
+  }
+
+  if (NonFmpRepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY) != 0) {
+    DEBUG((EFI_D_ERROR, "NonFmp Repository Corrupt. Need to rebuild NonFmp Repository.\n"));
+    NonFmpRepositorySize = 0;
+  }
+
+  EfiReleaseLock(&mPrivate.NonFmpLock);
+
+  Status = EfiAcquireLockOrFail (&mPrivate.FmpLock);
+  Status = GetVariable2 (
+             EFI_ESRT_FMP_VARIABLE_NAME,
+             &gEfiCallerIdGuid,
+             (VOID **) &FmpEsrtRepository,
+             &FmpRepositorySize
+             );
+
+  if (EFI_ERROR(Status)) {
+    FmpRepositorySize = 0;
+  }
+
+  if (FmpRepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY) != 0) {
+    DEBUG((EFI_D_ERROR, "Fmp Repository Corrupt. Need to rebuild Fmp Repository.\n"));
+    FmpRepositorySize = 0;
+  }
+
+  EfiReleaseLock(&mPrivate.FmpLock);
+
+  //
+  // Skip ESRT table publish if no ESRT entry exists
+  //
+  if (NonFmpRepositorySize + FmpRepositorySize == 0) {
+    goto EXIT;
+  }
+
+  EsrtTable = AllocatePool(sizeof(EFI_SYSTEM_RESOURCE_TABLE) + NonFmpRepositorySize + FmpRepositorySize);
+  if (EsrtTable == NULL) {
+    DEBUG ((EFI_D_ERROR, "Esrt table memory allocation failure\n"));
+    goto EXIT;
+  }
+
+  EsrtTable->FwResourceVersion  = EFI_SYSTEM_RESOURCE_TABLE_FIRMWARE_RESOURCE_VERSION;                                            
+  EsrtTable->FwResourceCount    = (UINT32)((NonFmpRepositorySize + FmpRepositorySize) / sizeof(EFI_SYSTEM_RESOURCE_ENTRY));  
+  EsrtTable->FwResourceCountMax = PcdGet32(PcdMaxNonFmpEsrtCacheNum) + PcdGet32(PcdMaxFmpEsrtCacheNum);
+
+  if (NonFmpRepositorySize != 0 && NonFmpEsrtRepository != NULL) {
+    CopyMem(EsrtTable + 1, NonFmpEsrtRepository, NonFmpRepositorySize);
+  }
+
+  if (FmpRepositorySize != 0 && FmpEsrtRepository != NULL) {
+    CopyMem((UINT8 *)(EsrtTable + 1) + NonFmpRepositorySize, FmpEsrtRepository, FmpRepositorySize);
+  }
+
+  //
+  // Publish Esrt to system config table
+  //
+  Status = gBS->InstallConfigurationTable (&gEfiSystemResourceTableGuid, EsrtTable);
+
+  //
+  // Only one successful install
+  //
+  gBS->CloseEvent(Event);
+
+EXIT:
+
+  if (FmpEsrtRepository != NULL) {
+    FreePool(FmpEsrtRepository);
+  }
+
+  if (NonFmpEsrtRepository != NULL) {
+    FreePool(NonFmpEsrtRepository);
+  }
+}
+
+/**
+  The module Entry Point of the Esrt DXE driver that manages cached ESRT repository 
+  & publishes ESRT table
+
+  @param[in]  ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in]  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval Other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{  
+  EFI_STATUS                    Status;
+
+  EfiInitializeLock (&mPrivate.FmpLock,    TPL_CALLBACK);
+  EfiInitializeLock (&mPrivate.NonFmpLock, TPL_CALLBACK);
+
+  //
+  // Install Esrt management Protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mPrivate.Handle,
+                  &gEsrtManagementProtocolGuid,
+                  &mEsrtManagementProtocolTemplate,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register notify function to install Esrt Table on ReadyToBoot Event.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  EsrtReadyToBootEventNotify,
+                  NULL,
+                  &gEfiEventReadyToBootGuid,
+                  &mPrivate.Event
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.inf b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.inf
new file mode 100644
index 0000000000..8fbc75bfcf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.inf
@@ -0,0 +1,72 @@
+## @file
+#  Esrt DXE driver that manages cached ESRT repository & publishes ESRT table
+#
+#  This driver produces EsrtManagement protocol to manage cache ESRT repository for FMP/Non-FMP instances.
+#  ESRT table based on repository is published on gEfiEventReadyToBootGuid.
+#
+#  Copyright (c) 2015, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EsrtDxe
+  MODULE_UNI_FILE                = EsrtDxe.uni
+  FILE_GUID                      = 999BD818-7DF7-4A9A-A502-9B75033E6A0F
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = EsrtDxeEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  EsrtImpl.h
+  EsrtImpl.c
+  EsrtDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  UefiLib
+  PcdLib
+  DebugLib
+  MemoryAllocationLib
+  DxeServicesTableLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  PrintLib
+
+[Guids]
+  gEfiSystemResourceTableGuid             ## PRODUCES             ## SystemTable
+  gEfiEventReadyToBootGuid                ## CONSUMES             ## Event
+
+[Protocols]
+  gEfiFirmwareManagementProtocolGuid      ## SOMETIMES_CONSUMES
+  gEsrtManagementProtocolGuid             ## PRODUCES
+  gEdkiiVariableLockProtocolGuid          ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxFmpEsrtCacheNum                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxNonFmpEsrtCacheNum               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSystemRebootAfterCapsuleProcessFlag ## CONSUMES
+
+[Depex]
+  gEfiVariableArchProtocolGuid AND gEfiVariableWriteArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EsrtDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.uni b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.uni
new file mode 100644
index 0000000000..029041531d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxe.uni
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxeExtra.uni b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxeExtra.uni
new file mode 100644
index 0000000000..dc762dd93a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.c b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.c
new file mode 100644
index 0000000000..f6d1e97451
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.c
@@ -0,0 +1,438 @@
+/** @file
+  Esrt management implementation.
+
+Copyright (c) 2015, 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 "EsrtImpl.h"
+
+/**
+  Find Esrt Entry stored in ESRT repository. 
+
+  @param[in]     FwClass           Firmware class guid in Esrt entry
+  @param[in]     Attribute         Esrt from Non FMP or FMP instance
+  @param[out]    Entry             Esrt entry returned
+  
+  @retval EFI_SUCCESS            Successfully find an Esrt entry
+  @retval EF_NOT_FOUND           No Esrt entry found
+
+**/
+EFI_STATUS
+GetEsrtEntry (
+  IN  EFI_GUID              *FwClass,
+  IN  UINTN                 Attribute,
+  OUT EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  )
+{
+  EFI_STATUS                 Status;
+  CHAR16                     *VariableName;
+  EFI_SYSTEM_RESOURCE_ENTRY  *EsrtRepository;
+  UINTN                      RepositorySize;
+  UINTN                      Index;
+  UINTN                      EsrtNum;
+
+  EsrtRepository = NULL;
+
+  //
+  // Get Esrt index buffer
+  //
+  if (Attribute == ESRT_FROM_FMP) {
+    VariableName = EFI_ESRT_FMP_VARIABLE_NAME;
+  } else {
+    VariableName = EFI_ESRT_NONFMP_VARIABLE_NAME;
+  }
+  
+  Status = GetVariable2 (
+             VariableName,
+             &gEfiCallerIdGuid,
+             (VOID **) &EsrtRepository,
+             &RepositorySize
+             );
+
+  if (EFI_ERROR(Status)) {
+    goto EXIT;
+  }
+
+  if (RepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY) != 0) {
+    DEBUG((EFI_D_ERROR, "Repository Corrupt. Need to rebuild Repository.\n"));
+    Status = EFI_ABORTED;
+    goto EXIT;
+  }
+
+  Status  = EFI_NOT_FOUND;
+  EsrtNum = RepositorySize/sizeof(EFI_SYSTEM_RESOURCE_ENTRY);
+  for (Index = 0; Index < EsrtNum; Index++) {
+    if (CompareGuid(FwClass, &EsrtRepository[Index].FwClass)) {
+      CopyMem(Entry, &EsrtRepository[Index], sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+      Status = EFI_SUCCESS;
+      break;
+    }
+  }
+
+EXIT:
+  if (EsrtRepository != NULL) {
+    FreePool(EsrtRepository);
+  }
+
+  return Status;
+}
+
+/**
+  Insert a new ESRT entry into ESRT Cache repository.
+
+  @param[in]  Entry                Esrt entry to be set
+  @param[in]  Attribute            Esrt from Esrt private protocol or FMP instance
+  
+  @retval EFI_SUCCESS          Successfully set a variable.
+
+**/
+EFI_STATUS
+InsertEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry,
+  UINTN                        Attribute
+  )
+{
+  EFI_STATUS                 Status;
+  CHAR16                     *VariableName;
+  EFI_SYSTEM_RESOURCE_ENTRY  *EsrtRepository;
+  UINTN                      RepositorySize;
+  EFI_SYSTEM_RESOURCE_ENTRY  *EsrtRepositoryNew;
+
+  EsrtRepository    = NULL;
+  EsrtRepositoryNew = NULL;
+
+  //
+  // Get Esrt index buffer
+  //
+  if (Attribute == ESRT_FROM_FMP) {
+    VariableName = EFI_ESRT_FMP_VARIABLE_NAME;
+  } else {
+    VariableName = EFI_ESRT_NONFMP_VARIABLE_NAME;
+  }
+
+  Status = GetVariable2 (
+             VariableName,
+             &gEfiCallerIdGuid,
+             (VOID **) &EsrtRepository,
+             &RepositorySize
+             );
+
+  if (Status == EFI_NOT_FOUND) {
+    //
+    // If not exist, create new Esrt cache repository
+    //
+    Status = gRT->SetVariable(
+                    VariableName,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                    sizeof(EFI_SYSTEM_RESOURCE_ENTRY),
+                    Entry
+                    );
+    return Status;
+
+  } else if (Status == EFI_SUCCESS) {
+    //
+    // if exist, update Esrt cache repository
+    //
+    if (RepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY) != 0) {
+      DEBUG((EFI_D_ERROR, "Repository Corrupt. Need to rebuild Repository.\n"));
+      //
+      // Repository is corrupt. Clear Repository before insert new entry
+      //
+      Status = gRT->SetVariable(
+                      VariableName,
+                      &gEfiCallerIdGuid,
+                      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                      0,
+                      EsrtRepository
+                      );
+      FreePool(EsrtRepository);
+      RepositorySize = 0;
+      EsrtRepository = NULL;
+    }
+
+    //
+    // Check Repository size constraint
+    //
+    if ((Attribute == ESRT_FROM_FMP && RepositorySize >= PcdGet32(PcdMaxFmpEsrtCacheNum) * sizeof(EFI_SYSTEM_RESOURCE_ENTRY))
+      ||(Attribute == ESRT_FROM_NONFMP && RepositorySize >= PcdGet32(PcdMaxNonFmpEsrtCacheNum) * sizeof(EFI_SYSTEM_RESOURCE_ENTRY)) ) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto EXIT;
+    }
+
+    EsrtRepositoryNew = AllocatePool(RepositorySize + sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+    if (EsrtRepositoryNew == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto EXIT;
+    }
+
+    if (RepositorySize != 0 && EsrtRepository != NULL) {
+      CopyMem(EsrtRepositoryNew, EsrtRepository, RepositorySize);
+    }
+    CopyMem((UINT8 *)EsrtRepositoryNew + RepositorySize, Entry, sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+
+    Status = gRT->SetVariable(
+                    VariableName,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                    RepositorySize + sizeof(EFI_SYSTEM_RESOURCE_ENTRY),
+                    EsrtRepositoryNew
+                    );
+  }
+
+EXIT:
+  if (EsrtRepository != NULL) {
+    FreePool(EsrtRepository);
+  }
+
+  if (EsrtRepositoryNew != NULL) {
+    FreePool(EsrtRepositoryNew);
+  }
+
+  return Status;
+}
+
+/**
+  Delete ESRT Entry from ESRT repository. 
+
+  @param[in]    FwClass              FwClass of Esrt entry to delete  
+  @param[in]    Attribute            Esrt from Esrt private protocol or FMP instance
+  
+  @retval EFI_SUCCESS         Insert all entries Successfully 
+  @retval EFI_NOT_FOUND       ESRT entry with FwClass doesn't exsit
+  
+**/
+EFI_STATUS
+DeleteEsrtEntry(
+  IN  EFI_GUID        *FwClass,
+  IN  UINTN           Attribute
+  )
+{  
+  EFI_STATUS                 Status;
+  CHAR16                     *VariableName;
+  EFI_SYSTEM_RESOURCE_ENTRY  *EsrtRepository;
+  UINTN                      RepositorySize;
+  UINTN                      Index;
+  UINTN                      EsrtNum;
+
+  EsrtRepository = NULL;
+  
+  //
+  // Get Esrt index buffer
+  //
+  if (Attribute == ESRT_FROM_FMP) {
+    VariableName = EFI_ESRT_FMP_VARIABLE_NAME;
+  } else {
+    VariableName = EFI_ESRT_NONFMP_VARIABLE_NAME;
+  }
+  
+  Status = GetVariable2 (
+             VariableName,
+             &gEfiCallerIdGuid,
+             (VOID **) &EsrtRepository,
+             &RepositorySize
+             );
+
+  if (EFI_ERROR(Status)) {
+    goto EXIT;
+  }
+
+  if ((RepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY)) != 0) {
+    DEBUG((EFI_D_ERROR, "Repository Corrupt. Need to rebuild Repository.\n"));
+    //
+    // Repository is corrupt. Clear Repository before insert new entry
+    //
+    Status = gRT->SetVariable(
+                    VariableName,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                    0,
+                    EsrtRepository
+                    );
+    goto EXIT;
+  }
+
+  Status = EFI_NOT_FOUND;
+  EsrtNum = RepositorySize/sizeof(EFI_SYSTEM_RESOURCE_ENTRY);
+  for (Index = 0; Index < EsrtNum; Index++) {
+    //
+    // Delete Esrt entry if it is found in repository
+    //
+    if (CompareGuid(FwClass, &EsrtRepository[Index].FwClass)) {
+      //
+      // If delete Esrt entry is not at the rail
+      //
+      if (Index < EsrtNum - 1) {
+        CopyMem(&EsrtRepository[Index], &EsrtRepository[Index + 1], (EsrtNum - Index - 1) * sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+      }
+
+      //
+      // Update New Repository
+      //
+      Status = gRT->SetVariable(
+                      VariableName,
+                      &gEfiCallerIdGuid,
+                      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                      (EsrtNum - 1) * sizeof(EFI_SYSTEM_RESOURCE_ENTRY),
+                      EsrtRepository
+                      );
+      break;
+    }
+  }
+
+EXIT:
+  if (EsrtRepository != NULL) {
+    FreePool(EsrtRepository);
+  }
+
+  return Status;
+
+}
+
+/**
+  Update one ESRT entry in ESRT repository
+
+  @param[in]    Entry                Esrt entry to be set
+  @param[in]    Attribute            Esrt from Non Esrt or FMP instance
+  
+  @retval EFI_SUCCESS          Successfully Update a variable.
+  @retval EFI_NOT_FOUND        The Esrt enry doesn't exist
+
+**/
+EFI_STATUS
+UpdateEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry,
+  UINTN                        Attribute
+  )
+{
+  EFI_STATUS                 Status;
+  CHAR16                     *VariableName;
+  EFI_SYSTEM_RESOURCE_ENTRY  *EsrtRepository;
+  UINTN                      RepositorySize;
+  UINTN                      Index;
+  UINTN                      EsrtNum;
+
+  EsrtRepository    = NULL;
+
+  //
+  // Get Esrt index buffer
+  //
+  if (Attribute == ESRT_FROM_FMP) {
+    VariableName = EFI_ESRT_FMP_VARIABLE_NAME;
+  } else {
+    VariableName = EFI_ESRT_NONFMP_VARIABLE_NAME;
+  }
+
+  Status = GetVariable2 (
+             VariableName,
+             &gEfiCallerIdGuid,
+             (VOID **) &EsrtRepository,
+             &RepositorySize
+             );
+
+  if (!EFI_ERROR(Status)) {
+    //
+    // if exist, update Esrt cache repository
+    //
+    if (RepositorySize % sizeof(EFI_SYSTEM_RESOURCE_ENTRY) != 0) {
+      DEBUG((EFI_D_ERROR, "Repository Corrupt. Need to rebuild Repository.\n"));
+      //
+      // Repository is corrupt. Clear Repository before insert new entry
+      //
+      Status = gRT->SetVariable(
+                      VariableName,
+                      &gEfiCallerIdGuid,
+                      EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                      0,
+                      EsrtRepository
+                      );
+      Status = EFI_NOT_FOUND;
+      goto EXIT;
+    }
+
+    Status = EFI_NOT_FOUND;
+    EsrtNum = RepositorySize/sizeof(EFI_SYSTEM_RESOURCE_ENTRY);
+    for (Index = 0; Index < EsrtNum; Index++) {
+      //
+      // Update Esrt entry if it is found in repository
+      //
+      if (CompareGuid(&Entry->FwClass, &EsrtRepository[Index].FwClass)) {
+
+        CopyMem(&EsrtRepository[Index], Entry, sizeof(EFI_SYSTEM_RESOURCE_ENTRY));
+        //
+        // Update New Repository
+        //
+        Status = gRT->SetVariable(
+                        VariableName,
+                        &gEfiCallerIdGuid,
+                        EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                        RepositorySize,
+                        EsrtRepository
+                        );
+        break;
+      }
+    }
+  }
+
+EXIT:
+  if (EsrtRepository != NULL) {
+    FreePool(EsrtRepository);
+  }
+
+  return Status;
+}
+
+/**
+  Init one ESRT entry according to input FmpImageInfo (V1, V2, V3) .
+
+  @param[in, out]     EsrtEntry            Esrt entry to be Init
+  @param[in]          FmpImageInfo         FMP image info descriptor
+  @param[in]          DescriptorVersion    FMP Image info descriptor version
+  
+**/
+VOID 
+SetEsrtEntryFromFmpInfo (
+  IN OUT EFI_SYSTEM_RESOURCE_ENTRY   *EsrtEntry,
+  IN EFI_FIRMWARE_IMAGE_DESCRIPTOR   *FmpImageInfo,
+  IN UINT32                          DescriptorVersion
+  )
+{
+  EsrtEntry->FwVersion                = FmpImageInfo->Version;
+  EsrtEntry->FwClass                  = FmpImageInfo->ImageTypeId;
+  EsrtEntry->FwType                   = ESRT_FW_TYPE_DEVICEFIRMWARE;
+  EsrtEntry->LowestSupportedFwVersion = 0;
+  EsrtEntry->CapsuleFlags             = 0;
+  EsrtEntry->LastAttemptVersion       = 0;
+  EsrtEntry->LastAttemptStatus        = LAST_ATTEMPT_STATUS_SUCCESS;
+
+  if (DescriptorVersion >= 2) {
+    //
+    // LowestSupportedImageVersion only available in FMP V2 or higher
+    //
+    EsrtEntry->LowestSupportedFwVersion = FmpImageInfo->LowestSupportedImageVersion;
+  } 
+
+  if (DescriptorVersion >= 3) {
+    //
+    // LastAttemptVersion & LastAttemptStatus only available in FMP V3 or higher
+    //
+    EsrtEntry->LastAttemptVersion = FmpImageInfo->LastAttemptVersion;
+    EsrtEntry->LastAttemptStatus  = FmpImageInfo->LastAttemptStatus;
+  } 
+
+  //
+  // Set capsule customized flag
+  //
+  if ((FmpImageInfo->AttributesSupported & IMAGE_ATTRIBUTE_RESET_REQUIRED) != 0
+   && (FmpImageInfo->AttributesSetting & IMAGE_ATTRIBUTE_RESET_REQUIRED) != 0) {
+    EsrtEntry->CapsuleFlags = PcdGet16(PcdSystemRebootAfterCapsuleProcessFlag);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.h b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.h
new file mode 100644
index 0000000000..a238dfb8f7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/EsrtDxe/EsrtImpl.h
@@ -0,0 +1,245 @@
+/** @file
+  Esrt management implementation head file.
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _DXE_ESRT_IMPL_H_
+#define _DXE_ESRT_IMPL_H_
+
+#include <Guid/EventGroup.h>
+#include <Guid/SystemResourceTable.h>
+
+#include <Library/UefiLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/CapsuleLib.h>
+#include <Library/PrintLib.h>
+
+#include <Protocol/FirmwareManagement.h>
+#include <Protocol/EsrtManagement.h>
+#include <Protocol/VariableLock.h>
+
+//
+// Name of  Variable for Non-FMP ESRT Repository
+// 
+#define EFI_ESRT_NONFMP_VARIABLE_NAME    L"EsrtNonFmp"
+
+//
+// Name of Variable for FMP
+// 
+#define EFI_ESRT_FMP_VARIABLE_NAME       L"EsrtFmp"
+
+//
+// Attribute of Cached ESRT entry
+//
+#define ESRT_FROM_FMP                    0x00000001
+#define ESRT_FROM_NONFMP                 0x00000002
+
+typedef struct {
+  EFI_HANDLE            Handle;
+  //
+  // Ready to boot event
+  //
+  EFI_EVENT             Event;
+  
+  //
+  // Updates to Fmp storage must be locked.
+  //
+  EFI_LOCK              FmpLock;
+
+  //
+  // Update to Non-Fmp storage must be locked
+  //
+  EFI_LOCK              NonFmpLock;
+} ESRT_PRIVATE_DATA;
+
+
+/**
+  Find Esrt Entry stored in ESRT repository. 
+
+  @param[in]     FwClass           Firmware class guid in Esrt entry
+  @param[in]     Attribute         Esrt from Non FMP or FMP instance
+  @param[out]    Entry             Esrt entry returned
+  
+  @retval EFI_SUCCESS            Successfully find an Esrt entry
+  @retval EF_NOT_FOUND           No Esrt entry found
+
+**/
+EFI_STATUS
+GetEsrtEntry (
+  IN  EFI_GUID              *FwClass,
+  IN  UINTN                 Attribute,
+  OUT EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+/**
+  Insert a new ESRT entry into ESRT Cache repository.
+
+  @param[in]  Entry                Esrt entry to be set
+  @param[in]  Attribute            Esrt from Esrt private protocol or FMP instance
+  
+  @retval EFI_SUCCESS          Successfully set a variable.
+
+**/
+EFI_STATUS
+InsertEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry,
+  UINTN                        Attribute
+  );
+
+/**
+  Delete ESRT Entry from ESRT repository. 
+
+  @param[in]    FwClass              FwClass of Esrt entry to delete  
+  @param[in]    Attribute            Esrt from Esrt private protocol or FMP instance
+  
+  @retval EFI_SUCCESS         Insert all entries Successfully 
+  @retval EFI_NOT_FOUND       ESRT entry with FwClass doesn't exsit
+  
+**/
+EFI_STATUS
+DeleteEsrtEntry(
+  IN  EFI_GUID        *FwClass,
+  IN  UINTN           Attribute
+  );
+
+/**
+  Update one ESRT entry in ESRT repository
+
+  @param[in]    Entry                Esrt entry to be set
+  @param[in]    Attribute            Esrt from Non Esrt or FMP instance
+  
+  @retval EFI_SUCCESS          Successfully Update a variable.
+  @retval EFI_NOT_FOUND        The Esrt enry doesn't exist
+
+**/
+EFI_STATUS
+UpdateEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry,
+  UINTN                        Attribute
+  );
+
+/**
+  Init one ESRT entry according to input FmpImageInfo (V1, V2, V3) .
+
+  @param[in, out]    EsrtEntry             Esrt entry to be Init
+  @param[in]         FmpImageInfo          FMP image info descriptor
+  @param[in]         DescriptorVersion     FMP Image info descriptor version
+  
+**/
+VOID 
+SetEsrtEntryFromFmpInfo (
+  IN OUT EFI_SYSTEM_RESOURCE_ENTRY   *EsrtEntry,
+  IN EFI_FIRMWARE_IMAGE_DESCRIPTOR   *FmpImageInfo,
+  IN UINT32                          DescriptorVersion
+  );
+
+/**
+  Get ESRT entry from ESRT Cache by FwClass Guid 
+
+  @param[in]       FwClass                FwClass of Esrt entry to get  
+  @param[in, out]  Entry                  Esrt entry returned 
+  
+  @retval EFI_SUCCESS                   The variable saving this Esrt Entry exists.
+  @retval EF_NOT_FOUND                  No correct variable found.
+  @retval EFI_WRITE_PROTECTED           ESRT Cache repository is locked
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeGetEsrtEntry(
+  IN     EFI_GUID                  *FwClass,
+  IN OUT EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+/**
+  Update one ESRT entry in ESRT Cache.
+
+  @param[in]  Entry                         Esrt entry to be updated
+  
+  @retval EFI_SUCCESS                   Successfully update an ESRT entry in cache.
+  @retval EFI_INVALID_PARAMETER  Entry does't exist in ESRT Cache
+  @retval EFI_WRITE_PROTECTED     ESRT Cache is locked
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeUpdateEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+/**
+  Non-FMP instance to unregister Esrt Entry from ESRT Cache. 
+
+  @param[in]    FwClass                FwClass of Esrt entry to Unregister  
+  
+  @retval EFI_SUCCESS             Insert all entries Successfully 
+  @retval EFI_NOT_FOUND           Entry of FwClass does not exsit
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeUnRegisterEsrtEntry(
+  IN  EFI_GUID        *FwClass
+  );
+
+/**
+  Non-FMP instance to register one ESRT entry into ESRT Cache.
+
+  @param[in]  Entry                Esrt entry to be set
+
+  @retval EFI_SUCCESS          Successfully set a variable.
+  @retval EFI_INVALID_PARAMETER  ESRT Entry is already exist
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeRegisterEsrtEntry(
+  IN EFI_SYSTEM_RESOURCE_ENTRY *Entry
+  );
+
+/**
+  This function syn up Cached ESRT with data from FMP instances
+  Function should be called after Connect All in order to locate all FMP protocols
+  installed.
+
+  @retval EFI_SUCCESS                      Successfully sync cache repository from FMP instances
+  @retval EFI_NOT_FOUND                   No FMP Instance are found
+  @retval EFI_OUT_OF_RESOURCES     Resource allocaton fail
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeSyncFmp(
+  VOID
+  );
+
+/**
+  This function locks up Esrt repository to be readonly. It should be called 
+  before gEfiEndOfDxeEventGroupGuid event signaled
+
+  @retval EFI_SUCCESS              Locks up FMP Non-FMP repository successfully 
+
+**/
+EFI_STATUS
+EFIAPI
+EsrtDxeLockEsrtRepository(
+  VOID
+  );
+
+#endif // #ifndef _EFI_ESRT_IMPL_H_
+
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.c
new file mode 100644
index 0000000000..7a6c377854
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.c
@@ -0,0 +1,893 @@
+/** @file
+
+  These are the common Fault Tolerant Write (FTW) functions that are shared 
+  by DXE FTW driver and SMM FTW driver.
+
+Copyright (c) 2006 - 2015, 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 "FaultTolerantWrite.h"
+
+//
+// Fault Tolerant Write Protocol API
+//
+/**
+  Query the largest block that may be updated in a fault tolerant manner.
+
+
+  @param This            The pointer to this protocol instance. 
+  @param BlockSize       A pointer to a caller allocated UINTN that is updated to
+                         indicate the size of the largest block that can be updated.
+
+  @return EFI_SUCCESS   The function completed successfully
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetMaxBlockSize (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    *This,
+  OUT UINTN                               *BlockSize
+  )
+{
+  EFI_FTW_DEVICE  *FtwDevice;
+
+  if (!FeaturePcdGet(PcdFullFtwServiceEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  FtwDevice   = FTW_CONTEXT_FROM_THIS (This);
+
+  *BlockSize  = FtwDevice->SpareAreaLength;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocates space for the protocol to maintain information about writes.
+  Since writes must be completed in a fault tolerant manner and multiple
+  updates will require more resources to be successful, this function
+  enables the protocol to ensure that enough space exists to track
+  information about the upcoming writes.
+
+  All writes must be completed or aborted before another fault tolerant write can occur.
+
+  @param This            The pointer to this protocol instance. 
+  @param CallerId        The GUID identifying the write.
+  @param PrivateDataSize The size of the caller's private data
+                         that must be recorded for each write.
+  @param NumberOfWrites  The number of fault tolerant block writes
+                         that will need to occur.
+
+  @return EFI_SUCCESS        The function completed successfully
+  @retval EFI_ABORTED        The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED  All allocated writes have not been completed.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAllocate (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    *This,
+  IN EFI_GUID                             *CallerId,
+  IN UINTN                                PrivateDataSize,
+  IN UINTN                                NumberOfWrites
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           Offset;
+  EFI_FTW_DEVICE                  *FtwDevice;
+  EFI_FAULT_TOLERANT_WRITE_HEADER *FtwHeader;
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  Status    = WorkSpaceRefresh (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Check if there is enough space for the coming allocation
+  //
+  if (FTW_WRITE_TOTAL_SIZE (NumberOfWrites, PrivateDataSize) > FtwDevice->FtwWorkSpaceHeader->WriteQueueSize) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Allocate() request exceed Workspace, Caller: %g\n", CallerId));
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  //
+  // Find the last write header and record.
+  // If the FtwHeader is complete, skip the completed last write header/records
+  //
+  FtwHeader = FtwDevice->FtwLastWriteHeader;
+
+  //
+  // Previous write has not completed, access denied.
+  //
+  if ((FtwHeader->HeaderAllocated == FTW_VALID_STATE) || (FtwHeader->WritesAllocated == FTW_VALID_STATE)) {
+    return EFI_ACCESS_DENIED;
+  }
+  //
+  // If workspace is not enough, then reclaim workspace
+  //
+  Offset = (UINT8 *) FtwHeader - (UINT8 *) FtwDevice->FtwWorkSpace;
+  if (Offset + FTW_WRITE_TOTAL_SIZE (NumberOfWrites, PrivateDataSize) > FtwDevice->FtwWorkSpaceSize) {
+    Status = FtwReclaimWorkSpace (FtwDevice, TRUE);
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+
+    FtwHeader = FtwDevice->FtwLastWriteHeader;
+  }
+  //
+  // Prepare FTW write header,
+  // overwrite the buffer and write to workspace.
+  //
+  FtwHeader->WritesAllocated  = FTW_INVALID_STATE;
+  FtwHeader->Complete         = FTW_INVALID_STATE;
+  CopyMem (&FtwHeader->CallerId, CallerId, sizeof (EFI_GUID));
+  FtwHeader->NumberOfWrites   = NumberOfWrites;
+  FtwHeader->PrivateDataSize  = PrivateDataSize;
+  FtwHeader->HeaderAllocated  = FTW_VALID_STATE;
+
+  Status = WriteWorkSpaceData (
+             FtwDevice->FtwFvBlock,
+             FtwDevice->WorkBlockSize,
+             FtwDevice->FtwWorkSpaceLba,
+             FtwDevice->FtwWorkSpaceBase + Offset,
+             sizeof (EFI_FAULT_TOLERANT_WRITE_HEADER),
+             (UINT8 *) FtwHeader
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Update Header->WriteAllocated as VALID
+  //
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + Offset,
+            WRITES_ALLOCATED
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  DEBUG (
+    (EFI_D_INFO,
+    "Ftw: Allocate() success, Caller:%g, # %d\n",
+    CallerId,
+    NumberOfWrites)
+    );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Write a record with fault tolerant manner.
+  Since the content has already backuped in spare block, the write is
+  guaranteed to be completed with fault tolerant manner.
+
+  @param This            The pointer to this protocol instance. 
+  @param Fvb             The FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+  @param BlockSize       The size of the block.
+
+  @retval  EFI_SUCCESS          The function completed successfully
+  @retval  EFI_ABORTED          The function could not complete successfully
+
+**/
+EFI_STATUS
+FtwWriteRecord (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL    *Fvb,
+  IN UINTN                                 BlockSize
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_FTW_DEVICE                  *FtwDevice;
+  EFI_FAULT_TOLERANT_WRITE_HEADER *Header;
+  EFI_FAULT_TOLERANT_WRITE_RECORD *Record;
+  UINTN                           Offset;
+  UINTN                           NumberOfWriteBlocks;
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  //
+  // Spare Complete but Destination not complete,
+  // Recover the target block with the spare block.
+  //
+  Header  = FtwDevice->FtwLastWriteHeader;
+  Record  = FtwDevice->FtwLastWriteRecord;
+
+  //
+  // IF target block is working block, THEN Flush Spare Block To Working Block;
+  // ELSE flush spare block to target block, which may be boot block after all.
+  //
+  if (IsWorkingBlock (FtwDevice, Fvb, Record->Lba)) {
+    //
+    // If target block is working block,
+    // it also need to set SPARE_COMPLETED to spare block.
+    //
+    Offset = (UINT8 *) Record - FtwDevice->FtwWorkSpace;
+    Status = FtwUpdateFvState (
+              FtwDevice->FtwBackupFvb,
+              FtwDevice->SpareBlockSize,
+              FtwDevice->FtwSpareLba + FtwDevice->FtwWorkSpaceLbaInSpare,
+              FtwDevice->FtwWorkSpaceBaseInSpare + Offset,
+              SPARE_COMPLETED
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+
+    Status = FlushSpareBlockToWorkingBlock (FtwDevice);
+  } else if (IsBootBlock (FtwDevice, Fvb)) {
+    //
+    // Update boot block
+    //
+    Status = FlushSpareBlockToBootBlock (FtwDevice);
+  } else {
+    //
+    // Update blocks other than working block or boot block
+    //
+    NumberOfWriteBlocks = FTW_BLOCKS ((UINTN) (Record->Offset + Record->Length), BlockSize);
+    Status = FlushSpareBlockToTargetBlock (FtwDevice, Fvb, Record->Lba, BlockSize, NumberOfWriteBlocks);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Record the DestionationComplete in record
+  //
+  Offset = (UINT8 *) Record - FtwDevice->FtwWorkSpace;
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + Offset,
+            DEST_COMPLETED
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  Record->DestinationComplete = FTW_VALID_STATE;
+
+  //
+  // If this is the last Write in these write sequence,
+  // set the complete flag of write header.
+  //
+  if (IsLastRecordOfWrites (Header, Record)) {
+    Offset = (UINT8 *) Header - FtwDevice->FtwWorkSpace;
+    Status = FtwUpdateFvState (
+              FtwDevice->FtwFvBlock,
+              FtwDevice->WorkBlockSize,
+              FtwDevice->FtwWorkSpaceLba,
+              FtwDevice->FtwWorkSpaceBase + Offset,
+              WRITES_COMPLETED
+              );
+    Header->Complete = FTW_VALID_STATE;
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a target block update. This function will record data about write
+  in fault tolerant storage and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param This            The pointer to this protocol instance. 
+  @param Lba             The logical block address of the target block.
+  @param Offset          The offset within the target block to place the data.
+  @param Length          The number of bytes to write to the target block.
+  @param PrivateData     A pointer to private data that the caller requires to
+                         complete any pending writes in the event of a fault.
+  @param FvBlockHandle   The handle of FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+  @param Buffer          The data to write.
+
+  @retval EFI_SUCCESS          The function completed successfully 
+  @retval EFI_ABORTED          The function could not complete successfully. 
+  @retval EFI_BAD_BUFFER_SIZE  The input data can't fit within the spare block. 
+                               Offset + *NumBytes > SpareAreaLength.
+  @retval EFI_ACCESS_DENIED    No writes have been allocated. 
+  @retval EFI_OUT_OF_RESOURCES Cannot allocate enough memory resource.
+  @retval EFI_NOT_FOUND        Cannot find FVB protocol by handle.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  IN EFI_LBA                               Lba,
+  IN UINTN                                 Offset,
+  IN UINTN                                 Length,
+  IN VOID                                  *PrivateData,
+  IN EFI_HANDLE                            FvBlockHandle,
+  IN VOID                                  *Buffer
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_FTW_DEVICE                      *FtwDevice;
+  EFI_FAULT_TOLERANT_WRITE_HEADER     *Header;
+  EFI_FAULT_TOLERANT_WRITE_RECORD     *Record;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  UINTN                               MyLength;
+  UINTN                               MyOffset;
+  UINTN                               MyBufferSize;
+  UINT8                               *MyBuffer;
+  UINTN                               SpareBufferSize;
+  UINT8                               *SpareBuffer;
+  UINTN                               Index;
+  UINT8                               *Ptr;
+  EFI_PHYSICAL_ADDRESS                FvbPhysicalAddress;
+  UINTN                               BlockSize;
+  UINTN                               NumberOfBlocks;
+  UINTN                               NumberOfWriteBlocks;
+  UINTN                               WriteLength;
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  Status    = WorkSpaceRefresh (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  Header  = FtwDevice->FtwLastWriteHeader;
+  Record  = FtwDevice->FtwLastWriteRecord;
+  
+  if (IsErasedFlashBuffer ((UINT8 *) Header, sizeof (EFI_FAULT_TOLERANT_WRITE_HEADER))) {
+    if (PrivateData == NULL) {
+      //
+      // Ftw Write Header is not allocated.
+      // No additional private data, the private data size is zero. Number of record can be set to 1.
+      //
+      Status = FtwAllocate (This, &gEfiCallerIdGuid, 0, 1);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    } else {
+      //
+      // Ftw Write Header is not allocated
+      // Additional private data is not NULL, the private data size can't be determined.
+      //
+      DEBUG ((EFI_D_ERROR, "Ftw: no allocates space for write record!\n"));
+      DEBUG ((EFI_D_ERROR, "Ftw: Allocate service should be called before Write service!\n"));
+      return EFI_NOT_READY;
+    }
+  }
+
+  //
+  // If Record is out of the range of Header, return access denied.
+  //
+  if (((UINTN)((UINT8 *) Record - (UINT8 *) Header)) > FTW_WRITE_TOTAL_SIZE (Header->NumberOfWrites - 1, Header->PrivateDataSize)) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Check the COMPLETE flag of last write header
+  //
+  if (Header->Complete == FTW_VALID_STATE) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (Record->DestinationComplete == FTW_VALID_STATE) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  if ((Record->SpareComplete == FTW_VALID_STATE) && (Record->DestinationComplete != FTW_VALID_STATE)) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Get the FVB protocol by handle
+  //
+  Status = FtwGetFvbByHandle (FvBlockHandle, &Fvb);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status = Fvb->GetPhysicalAddress (Fvb, &FvbPhysicalAddress);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Write(), Get FVB physical address - %r\n", Status));
+    return EFI_ABORTED;
+  }
+
+  //
+  // Now, one FVB has one type of BlockSize.
+  //
+  Status = Fvb->GetBlockSize (Fvb, 0, &BlockSize, &NumberOfBlocks);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Write(), Get block size - %r\n", Status));
+    return EFI_ABORTED;
+  }
+
+  NumberOfWriteBlocks = FTW_BLOCKS (Offset + Length, BlockSize);
+  DEBUG ((EFI_D_INFO, "Ftw: Write(), BlockSize - 0x%x, NumberOfWriteBlock - 0x%x\n", BlockSize, NumberOfWriteBlocks));
+  WriteLength = NumberOfWriteBlocks * BlockSize;
+
+  //
+  // Check if the input data can fit within the spare block.
+  //
+  if (WriteLength > FtwDevice->SpareAreaLength) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  //
+  // Set BootBlockUpdate FLAG if it's updating boot block.
+  //
+  if (IsBootBlock (FtwDevice, Fvb)) {
+    Record->BootBlockUpdate = FTW_VALID_STATE;
+    //
+    // Boot Block and Spare Block should have same block size and block numbers.
+    //
+    ASSERT ((BlockSize == FtwDevice->SpareBlockSize) && (NumberOfWriteBlocks == FtwDevice->NumberOfSpareBlock));
+  }
+  //
+  // Write the record to the work space.
+  //
+  Record->Lba     = Lba;
+  Record->Offset  = Offset;
+  Record->Length  = Length;
+  Record->RelativeOffset = (INT64) (FvbPhysicalAddress + (UINTN) Lba * BlockSize) - (INT64) FtwDevice->SpareAreaAddress;
+  if (PrivateData != NULL) {
+    CopyMem ((Record + 1), PrivateData, (UINTN) Header->PrivateDataSize);
+  }
+
+  MyOffset  = (UINT8 *) Record - FtwDevice->FtwWorkSpace;
+  MyLength  = FTW_RECORD_SIZE (Header->PrivateDataSize);
+
+  Status = WriteWorkSpaceData (
+             FtwDevice->FtwFvBlock,
+             FtwDevice->WorkBlockSize,
+             FtwDevice->FtwWorkSpaceLba,
+             FtwDevice->FtwWorkSpaceBase + MyOffset,
+             MyLength,
+             (UINT8 *) Record
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Record has written to working block, then do the data.
+  //
+  //
+  // Allocate a memory buffer
+  //
+  MyBufferSize  = WriteLength;
+  MyBuffer      = AllocatePool (MyBufferSize);
+  if (MyBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Read all original data from target block to memory buffer
+  //
+  Ptr = MyBuffer;
+  for (Index = 0; Index < NumberOfWriteBlocks; Index += 1) {
+    MyLength  = BlockSize;
+    Status    = Fvb->Read (Fvb, Lba + Index, 0, &MyLength, Ptr);
+    if (EFI_ERROR (Status)) {
+      FreePool (MyBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += MyLength;
+  }
+  //
+  // Overwrite the updating range data with
+  // the input buffer content
+  //
+  CopyMem (MyBuffer + Offset, Buffer, Length);
+
+  //
+  // Try to keep the content of spare block
+  // Save spare block into a spare backup memory buffer (Sparebuffer)
+  //
+  SpareBufferSize = FtwDevice->SpareAreaLength;
+  SpareBuffer     = AllocatePool (SpareBufferSize);
+  if (SpareBuffer == NULL) {
+    FreePool (MyBuffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ptr = SpareBuffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    MyLength = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Read (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &MyLength,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (MyBuffer);
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += MyLength;
+  }
+  //
+  // Write the memory buffer to spare block
+  // Do not assume Spare Block and Target Block have same block size
+  //
+  Status  = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (MyBuffer);
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  Ptr     = MyBuffer;
+  for (Index = 0; MyBufferSize > 0; Index += 1) {
+    if (MyBufferSize > FtwDevice->SpareBlockSize) {
+      MyLength = FtwDevice->SpareBlockSize;
+    } else {
+      MyLength = MyBufferSize;
+    }
+    Status = FtwDevice->FtwBackupFvb->Write (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &MyLength,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (MyBuffer);
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += MyLength;
+    MyBufferSize -= MyLength;
+  }
+  //
+  // Free MyBuffer
+  //
+  FreePool (MyBuffer);
+
+  //
+  // Set the SpareComplete in the FTW record,
+  //
+  MyOffset = (UINT8 *) Record - FtwDevice->FtwWorkSpace;
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + MyOffset,
+            SPARE_COMPLETED
+            );
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+
+  Record->SpareComplete = FTW_VALID_STATE;
+
+  //
+  //  Since the content has already backuped in spare block, the write is
+  //  guaranteed to be completed with fault tolerant manner.
+  //
+  Status = FtwWriteRecord (This, Fvb, BlockSize);
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  //
+  // Restore spare backup buffer into spare block , if no failure happened during FtwWrite.
+  //
+  Status  = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  Ptr     = SpareBuffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    MyLength = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Write (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &MyLength,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += MyLength;
+  }
+  //
+  // All success.
+  //
+  FreePool (SpareBuffer);
+
+  DEBUG (
+    (EFI_D_INFO,
+    "Ftw: Write() success, (Lba:Offset)=(%lx:0x%x), Length: 0x%x\n",
+    Lba,
+    Offset,
+    Length)
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Restarts a previously interrupted write. The caller must provide the
+  block protocol needed to complete the interrupted write.
+
+  @param This            The pointer to this protocol instance. 
+  @param FvBlockHandle   The handle of FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+
+  @retval  EFI_SUCCESS          The function completed successfully
+  @retval  EFI_ACCESS_DENIED    No pending writes exist
+  @retval  EFI_NOT_FOUND        FVB protocol not found by the handle
+  @retval  EFI_ABORTED          The function could not complete successfully
+
+**/
+EFI_STATUS
+EFIAPI
+FtwRestart (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  IN EFI_HANDLE                            FvBlockHandle
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_FTW_DEVICE                      *FtwDevice;
+  EFI_FAULT_TOLERANT_WRITE_HEADER     *Header;
+  EFI_FAULT_TOLERANT_WRITE_RECORD     *Record;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  UINTN                               BlockSize;
+  UINTN                               NumberOfBlocks;
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  Status    = WorkSpaceRefresh (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  Header  = FtwDevice->FtwLastWriteHeader;
+  Record  = FtwDevice->FtwLastWriteRecord;
+
+  //
+  // Spare Complete but Destination not complete,
+  // Recover the targt block with the spare block.
+  //
+  Status = FtwGetFvbByHandle (FvBlockHandle, &Fvb);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Now, one FVB has one type of BlockSize
+  //
+  Status = Fvb->GetBlockSize (Fvb, 0, &BlockSize, &NumberOfBlocks);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Restart(), Get block size - %r\n", Status));
+    return EFI_ABORTED;
+  }
+
+  //
+  // Check the COMPLETE flag of last write header
+  //
+  if (Header->Complete == FTW_VALID_STATE) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Check the flags of last write record
+  //
+  if (Record->DestinationComplete == FTW_VALID_STATE) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  if ((Record->SpareComplete != FTW_VALID_STATE)) {
+    return EFI_ABORTED;
+  }
+
+  //
+  //  Since the content has already backuped in spare block, the write is
+  //  guaranteed to be completed with fault tolerant manner.
+  //
+  Status = FtwWriteRecord (This, Fvb, BlockSize);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  //
+  // Erase Spare block
+  // This is restart, no need to keep spareblock content.
+  //
+  Status = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  DEBUG ((EFI_D_ERROR, "Ftw: Restart() success \n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Aborts all previous allocated writes.
+
+  @param This                  The pointer to this protocol instance. 
+
+  @retval EFI_SUCCESS          The function completed successfully
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAbort (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This
+  )
+{
+  EFI_STATUS      Status;
+  UINTN           Offset;
+  EFI_FTW_DEVICE  *FtwDevice;
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  Status    = WorkSpaceRefresh (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  if (FtwDevice->FtwLastWriteHeader->HeaderAllocated != FTW_VALID_STATE) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (FtwDevice->FtwLastWriteHeader->Complete == FTW_VALID_STATE) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Update the complete state of the header as VALID and abort.
+  //
+  Offset = (UINT8 *) FtwDevice->FtwLastWriteHeader - FtwDevice->FtwWorkSpace;
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + Offset,
+            WRITES_COMPLETED
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  FtwDevice->FtwLastWriteHeader->Complete = FTW_VALID_STATE;
+
+  DEBUG ((EFI_D_ERROR, "Ftw: Abort() success \n"));
+  return EFI_SUCCESS;
+}
+
+/**
+  Starts a target block update. This records information about the write
+  in fault tolerant storage and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param This            The pointer to this protocol instance. 
+  @param CallerId        The GUID identifying the last write.
+  @param Lba             The logical block address of the last write.
+  @param Offset          The offset within the block of the last write.
+  @param Length          The length of the last write.
+  @param PrivateDataSize bytes from the private data
+                         stored for this write.
+  @param PrivateData     A pointer to a buffer. The function will copy
+  @param Complete        A Boolean value with TRUE indicating
+                         that the write was completed.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_ABORTED           The function could not complete successfully
+  @retval EFI_NOT_FOUND         No allocated writes exist
+  @retval EFI_BUFFER_TOO_SMALL  Input buffer is not larget enough
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetLastWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  OUT EFI_GUID                             *CallerId,
+  OUT EFI_LBA                              *Lba,
+  OUT UINTN                                *Offset,
+  OUT UINTN                                *Length,
+  IN OUT UINTN                             *PrivateDataSize,
+  OUT VOID                                 *PrivateData,
+  OUT BOOLEAN                              *Complete
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_FTW_DEVICE                  *FtwDevice;
+  EFI_FAULT_TOLERANT_WRITE_HEADER *Header;
+  EFI_FAULT_TOLERANT_WRITE_RECORD *Record;
+
+  if (!FeaturePcdGet(PcdFullFtwServiceEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  FtwDevice = FTW_CONTEXT_FROM_THIS (This);
+
+  Status    = WorkSpaceRefresh (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  Header  = FtwDevice->FtwLastWriteHeader;
+  Record  = FtwDevice->FtwLastWriteRecord;
+
+  //
+  // If Header is incompleted and the last record has completed, then
+  // call Abort() to set the Header->Complete FLAG.
+  //
+  if ((Header->Complete != FTW_VALID_STATE) &&
+      (Record->DestinationComplete == FTW_VALID_STATE) &&
+      IsLastRecordOfWrites (Header, Record)
+        ) {
+
+    Status    = FtwAbort (This);
+    *Complete = TRUE;
+    return EFI_NOT_FOUND;
+  }
+  //
+  // If there is no write header/record, return not found.
+  //
+  if (Header->HeaderAllocated != FTW_VALID_STATE) {
+    *Complete = TRUE;
+    return EFI_NOT_FOUND;
+  }
+  //
+  // If this record SpareComplete has not set, then it can not restart.
+  //
+  if (Record->SpareComplete != FTW_VALID_STATE) {
+    Status = GetPreviousRecordOfWrites (Header, &Record);
+    if (EFI_ERROR (Status)) {
+      FtwAbort (This);
+      *Complete = TRUE;
+      return EFI_NOT_FOUND;
+    }
+    ASSERT (Record != NULL);
+  }
+
+  //
+  // Fill all the requested values
+  //
+  CopyMem (CallerId, &Header->CallerId, sizeof (EFI_GUID));
+  *Lba      = Record->Lba;
+  *Offset   = (UINTN) Record->Offset;
+  *Length   = (UINTN) Record->Length;
+  *Complete = (BOOLEAN) (Record->DestinationComplete == FTW_VALID_STATE);
+
+  if (*PrivateDataSize < Header->PrivateDataSize) {
+    *PrivateDataSize  = (UINTN) Header->PrivateDataSize;
+    PrivateData       = NULL;
+    Status            = EFI_BUFFER_TOO_SMALL;
+  } else {
+    *PrivateDataSize = (UINTN) Header->PrivateDataSize;
+    CopyMem (PrivateData, Record + 1, *PrivateDataSize);
+    Status = EFI_SUCCESS;
+  }
+
+  DEBUG ((EFI_D_ERROR, "Ftw: GetLasetWrite() success\n"));
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.h b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.h
new file mode 100644
index 0000000000..849d1f4233
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWrite.h
@@ -0,0 +1,769 @@
+/** @file
+
+  The internal header file includes the common header files, defines
+  internal structure and functions used by Ftw module.
+
+Copyright (c) 2006 - 2014, 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. 
+
+**/
+
+#ifndef _EFI_FAULT_TOLERANT_WRITE_H_
+#define _EFI_FAULT_TOLERANT_WRITE_H_
+
+#include <PiDxe.h>
+
+#include <Guid/SystemNvDataGuid.h>
+#include <Guid/ZeroGuid.h>
+#include <Protocol/FaultTolerantWrite.h>
+#include <Protocol/FirmwareVolumeBlock.h>
+#include <Protocol/SwapAddressRange.h>
+
+#include <Library/PcdLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+//
+// Flash erase polarity is 1
+//
+#define FTW_ERASE_POLARITY  1
+
+#define FTW_ERASED_BYTE     ((UINT8) (255))
+#define FTW_POLARITY_REVERT ((UINT8) (255))
+
+#define HEADER_ALLOCATED  0x1
+#define WRITES_ALLOCATED  0x2
+#define WRITES_COMPLETED  0x4
+
+#define BOOT_BLOCK_UPDATE 0x1
+#define SPARE_COMPLETED   0x2
+#define DEST_COMPLETED    0x4
+
+#define FTW_BLOCKS(Length, BlockSize) ((UINTN) ((Length) / (BlockSize) + (((Length) & ((BlockSize) - 1)) ? 1 : 0)))
+
+#define FTW_DEVICE_SIGNATURE  SIGNATURE_32 ('F', 'T', 'W', 'D')
+
+//
+// EFI Fault tolerant protocol private data structure
+//
+typedef struct {
+  UINTN                                   Signature;
+  EFI_HANDLE                              Handle;
+  EFI_FAULT_TOLERANT_WRITE_PROTOCOL       FtwInstance;
+  EFI_PHYSICAL_ADDRESS                    WorkSpaceAddress;   // Base address of working space range in flash.
+  EFI_PHYSICAL_ADDRESS                    SpareAreaAddress;   // Base address of spare range in flash.
+  UINTN                                   WorkSpaceLength;    // Size of working space range in flash.
+  UINTN                                   NumberOfWorkSpaceBlock; // Number of the blocks in work block for work space.
+  UINTN                                   WorkBlockSize;      // Block size in bytes of the work blocks in flash
+  UINTN                                   SpareAreaLength;    // Size of spare range in flash.
+  UINTN                                   NumberOfSpareBlock; // Number of the blocks in spare block.
+  UINTN                                   SpareBlockSize;     // Block size in bytes of the spare blocks in flash
+  EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *FtwWorkSpaceHeader;// Pointer to Working Space Header in memory buffer
+  EFI_FAULT_TOLERANT_WRITE_HEADER         *FtwLastWriteHeader;// Pointer to last record header in memory buffer
+  EFI_FAULT_TOLERANT_WRITE_RECORD         *FtwLastWriteRecord;// Pointer to last record in memory buffer
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL      *FtwFvBlock;        // FVB of working block
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL      *FtwBackupFvb;      // FVB of spare block
+  EFI_LBA                                 FtwSpareLba;        // Start LBA of spare block
+  EFI_LBA                                 FtwWorkBlockLba;    // Start LBA of working block that contains working space in its last block.
+  UINTN                                   NumberOfWorkBlock;  // Number of the blocks in work block.
+  EFI_LBA                                 FtwWorkSpaceLba;    // Start LBA of working space
+  UINTN                                   FtwWorkSpaceBase;   // Offset into the FtwWorkSpaceLba block.
+  UINTN                                   FtwWorkSpaceSize;   // Size of working space range that stores write record.
+  EFI_LBA                                 FtwWorkSpaceLbaInSpare; // Start LBA of working space in spare block.
+  UINTN                                   FtwWorkSpaceBaseInSpare;// Offset into the FtwWorkSpaceLbaInSpare block.
+  UINT8                                   *FtwWorkSpace;      // Point to Work Space in memory buffer 
+  //
+  // Following a buffer of FtwWorkSpace[FTW_WORK_SPACE_SIZE],
+  // Allocated with EFI_FTW_DEVICE.
+  //
+} EFI_FTW_DEVICE;
+
+#define FTW_CONTEXT_FROM_THIS(a)  CR (a, EFI_FTW_DEVICE, FtwInstance, FTW_DEVICE_SIGNATURE)
+
+//
+// Driver entry point
+//
+/**
+  This function is the entry point of the Fault Tolerant Write driver.
+
+  @param ImageHandle     A handle for the image that is initializing this driver
+  @param SystemTable     A pointer to the EFI system table
+
+  @return EFI_SUCCESS           FTW has finished the initialization
+  @retval EFI_NOT_FOUND         Locate FVB protocol error
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
+  @retval EFI_VOLUME_CORRUPTED  Firmware volume is error
+  @retval EFI_ABORTED           FTW initialization error
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeFaultTolerantWrite (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+//
+// Fault Tolerant Write Protocol API
+//
+
+/**
+  Query the largest block that may be updated in a fault tolerant manner.
+
+
+  @param This            Indicates a pointer to the calling context.
+  @param BlockSize       A pointer to a caller allocated UINTN that is updated to
+                         indicate the size of the largest block that can be updated.
+
+  @return EFI_SUCCESS   The function completed successfully
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetMaxBlockSize (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    *This,
+  OUT UINTN                               *BlockSize
+  );
+
+/**
+  Allocates space for the protocol to maintain information about writes.
+  Since writes must be completed in a fault tolerant manner and multiple
+  updates will require more resources to be successful, this function
+  enables the protocol to ensure that enough space exists to track
+  information about the upcoming writes.
+
+  All writes must be completed or aborted before another fault tolerant write can occur.
+
+  @param This            Indicates a pointer to the calling context.
+  @param CallerId        The GUID identifying the write.
+  @param PrivateDataSize The size of the caller's private data
+                         that must be recorded for each write.
+  @param NumberOfWrites  The number of fault tolerant block writes
+                         that will need to occur.
+
+  @return EFI_SUCCESS        The function completed successfully
+  @retval EFI_ABORTED        The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED  All allocated writes have not been completed.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAllocate (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL    *This,
+  IN EFI_GUID                             *CallerId,
+  IN UINTN                                PrivateDataSize,
+  IN UINTN                                NumberOfWrites
+  );
+
+/**
+  Starts a target block update. This function will record data about write
+  in fault tolerant storage and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+
+  @param This            Calling context
+  @param Lba             The logical block address of the target block.
+  @param Offset          The offset within the target block to place the data.
+  @param Length          The number of bytes to write to the target block.
+  @param PrivateData     A pointer to private data that the caller requires to
+                         complete any pending writes in the event of a fault.
+  @param FvBlockHandle   The handle of FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+  @param Buffer          The data to write.
+
+  @retval EFI_SUCCESS          The function completed successfully 
+  @retval EFI_ABORTED          The function could not complete successfully. 
+  @retval EFI_BAD_BUFFER_SIZE  The input data can't fit within the spare block. 
+                               Offset + *NumBytes > SpareAreaLength.
+  @retval EFI_ACCESS_DENIED    No writes have been allocated. 
+  @retval EFI_OUT_OF_RESOURCES Cannot allocate enough memory resource.
+  @retval EFI_NOT_FOUND        Cannot find FVB protocol by handle.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  IN EFI_LBA                               Lba,
+  IN UINTN                                 Offset,
+  IN UINTN                                 Length,
+  IN VOID                                  *PrivateData,
+  IN EFI_HANDLE                            FvBlockHandle,
+  IN VOID                                  *Buffer
+  );
+
+/**
+  Restarts a previously interrupted write. The caller must provide the
+  block protocol needed to complete the interrupted write.
+
+  @param This            Calling context.
+  @param FvBlockHandle   The handle of FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+
+  @retval  EFI_SUCCESS          The function completed successfully
+  @retval  EFI_ACCESS_DENIED    No pending writes exist
+  @retval  EFI_NOT_FOUND        FVB protocol not found by the handle
+  @retval  EFI_ABORTED          The function could not complete successfully
+
+**/
+EFI_STATUS
+EFIAPI
+FtwRestart (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  IN EFI_HANDLE                            FvBlockHandle
+  );
+
+/**
+  Aborts all previous allocated writes.
+
+  @param  This                 Calling context
+
+  @retval EFI_SUCCESS          The function completed successfully
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAbort (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This
+  );
+
+/**
+  Starts a target block update. This records information about the write
+  in fault tolerant storage and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param This            Indicates a pointer to the calling context.
+  @param CallerId        The GUID identifying the last write.
+  @param Lba             The logical block address of the last write.
+  @param Offset          The offset within the block of the last write.
+  @param Length          The length of the last write.
+  @param PrivateDataSize bytes from the private data
+                         stored for this write.
+  @param PrivateData     A pointer to a buffer. The function will copy
+  @param Complete        A Boolean value with TRUE indicating
+                         that the write was completed.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_ABORTED           The function could not complete successfully
+  @retval EFI_NOT_FOUND         No allocated writes exist
+  @retval EFI_BUFFER_TOO_SMALL  Input buffer is not larget enough
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetLastWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL     *This,
+  OUT EFI_GUID                             *CallerId,
+  OUT EFI_LBA                              *Lba,
+  OUT UINTN                                *Offset,
+  OUT UINTN                                *Length,
+  IN OUT UINTN                             *PrivateDataSize,
+  OUT VOID                                 *PrivateData,
+  OUT BOOLEAN                              *Complete
+  );
+
+/**
+  Erase spare block.
+
+  @param FtwDevice        The private data of FTW driver
+
+  @retval EFI_SUCCESS           The erase request was successfully completed.
+  @retval EFI_ACCESS_DENIED     The firmware volume is in the WriteDisabled state.
+  @retval EFI_DEVICE_ERROR      The block device is not functioning
+                                correctly and could not be written.
+                                The firmware device may have been
+                                partially erased.
+  @retval EFI_INVALID_PARAMETER One or more of the LBAs listed
+                                in the variable argument list do
+                                not exist in the firmware volume.  
+
+
+**/
+EFI_STATUS
+FtwEraseSpareBlock (
+  IN EFI_FTW_DEVICE   *FtwDevice
+  );
+
+/**
+  Retrive the proper FVB protocol interface by HANDLE.
+
+
+  @param FvBlockHandle   The handle of FVB protocol that provides services for
+                         reading, writing, and erasing the target block.
+  @param FvBlock         The interface of FVB protocol
+
+  @retval  EFI_SUCCESS   The function completed successfully
+  @retval  EFI_ABORTED   The function could not complete successfully
+
+**/
+EFI_STATUS
+FtwGetFvbByHandle (
+  IN EFI_HANDLE                           FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  );
+
+/**
+
+  Is it in working block?
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         Fvb protocol instance
+  @param Lba             The block specified
+
+  @return A BOOLEAN value indicating in working block or not.
+
+**/
+BOOLEAN
+IsWorkingBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  EFI_LBA                             Lba
+  );
+
+/**
+
+  Is it in boot block?
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         Fvb protocol instance
+
+  @return A BOOLEAN value indicating in boot block or not.
+
+**/
+BOOLEAN
+IsBootBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock
+  );
+
+/**
+  Copy the content of spare block to a target block. Size is FTW_BLOCK_SIZE.
+  Spare block is accessed by FTW backup FVB protocol interface.
+  Target block is accessed by FvBlock protocol interface.
+
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         FVB Protocol interface to access target block
+  @param Lba             Lba of the target block
+  @param BlockSize       The size of the block
+  @param NumberOfBlocks  The number of consecutive blocks starting with Lba
+
+  @retval  EFI_SUCCESS               Spare block content is copied to target block
+  @retval  EFI_INVALID_PARAMETER     Input parameter error
+  @retval  EFI_OUT_OF_RESOURCES      Allocate memory error
+  @retval  EFI_ABORTED               The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToTargetBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  EFI_LBA                             Lba,
+  UINTN                               BlockSize,
+  UINTN                               NumberOfBlocks
+  );
+
+/**
+  Copy the content of spare block to working block. Size is FTW_BLOCK_SIZE.
+  Spare block is accessed by FTW backup FVB protocol interface. LBA is
+  FtwDevice->FtwSpareLba.
+  Working block is accessed by FTW working FVB protocol interface. LBA is
+  FtwDevice->FtwWorkBlockLba.
+
+  Since the working block header is important when FTW initializes, the
+  state of the operation should be handled carefully. The Crc value is
+  calculated without STATE element.
+
+  @param FtwDevice       The private data of FTW driver
+
+  @retval  EFI_SUCCESS               Spare block content is copied to target block
+  @retval  EFI_OUT_OF_RESOURCES      Allocate memory error
+  @retval  EFI_ABORTED               The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToWorkingBlock (
+  EFI_FTW_DEVICE                      *FtwDevice
+  );
+
+/**
+  Copy the content of spare block to a boot block. Size is FTW_BLOCK_SIZE.
+  Spare block is accessed by FTW working FVB protocol interface.
+  Target block is accessed by FvBlock protocol interface.
+
+  FTW will do extra work on boot block update.
+  FTW should depend on a protocol of EFI_ADDRESS_RANGE_SWAP_PROTOCOL,
+  which is produced by a chipset driver.
+  FTW updating boot block steps may be:
+  1. GetRangeLocation(), if the Range is inside the boot block, FTW know
+  that boot block will be update. It shall add a FLAG in the working block.
+  2. When spare block is ready,
+  3. SetSwapState(SWAPPED)
+  4. erasing boot block,
+  5. programming boot block until the boot block is ok.
+  6. SetSwapState(UNSWAPPED)
+  FTW shall not allow to update boot block when battery state is error.
+
+  @param FtwDevice       The private data of FTW driver
+
+  @retval EFI_SUCCESS             Spare block content is copied to boot block
+  @retval EFI_INVALID_PARAMETER   Input parameter error
+  @retval EFI_OUT_OF_RESOURCES    Allocate memory error
+  @retval EFI_ABORTED             The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToBootBlock (
+  EFI_FTW_DEVICE                      *FtwDevice
+  );
+
+/**
+  Update a bit of state on a block device. The location of the bit is
+  calculated by the (Lba, Offset, bit). Here bit is determined by the
+  the name of a certain bit.
+
+
+  @param FvBlock         FVB Protocol interface to access SrcBlock and DestBlock
+  @param BlockSize       The size of the block
+  @param Lba             Lba of a block
+  @param Offset          Offset on the Lba
+  @param NewBit          New value that will override the old value if it can be change
+
+  @retval  EFI_SUCCESS    A state bit has been updated successfully
+  @retval  Others         Access block device error.
+                          Notes:
+                          Assume all bits of State are inside the same BYTE.
+  @retval  EFI_ABORTED    Read block fail
+
+**/
+EFI_STATUS
+FtwUpdateFvState (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  IN UINTN                               BlockSize,
+  IN EFI_LBA                             Lba,
+  IN UINTN                               Offset,
+  IN UINT8                               NewBit
+  );
+
+/**
+  Get the last Write Header pointer.
+  The last write header is the header whose 'complete' state hasn't been set.
+  After all, this header may be a EMPTY header entry for next Allocate.
+
+
+  @param FtwWorkSpaceHeader Pointer of the working block header
+  @param FtwWorkSpaceSize   Size of the work space
+  @param FtwWriteHeader     Pointer to retrieve the last write header
+
+  @retval  EFI_SUCCESS      Get the last write record successfully
+  @retval  EFI_ABORTED      The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteHeader (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER  *FtwWorkSpaceHeader,
+  IN UINTN                                    FtwWorkSpaceSize,
+  OUT EFI_FAULT_TOLERANT_WRITE_HEADER         **FtwWriteHeader
+  );
+
+/**
+  Get the last Write Record pointer. The last write Record is the Record
+  whose DestinationCompleted state hasn't been set. After all, this Record
+  may be a EMPTY record entry for next write.
+
+
+  @param FtwWriteHeader  Pointer to the write record header
+  @param FtwWriteRecord  Pointer to retrieve the last write record
+
+  @retval EFI_SUCCESS        Get the last write record successfully
+  @retval EFI_ABORTED        The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteRecord (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER          *FtwWriteHeader,
+  OUT EFI_FAULT_TOLERANT_WRITE_RECORD         **FtwWriteRecord
+  );
+
+/**
+  To check if FtwRecord is the first record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to the write record
+
+  @retval TRUE      FtwRecord is the first Record of the FtwHeader
+  @retval FALSE     FtwRecord is not the first Record of the FtwHeader
+
+**/
+BOOLEAN
+IsFirstRecordOfWrites (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN EFI_FAULT_TOLERANT_WRITE_RECORD    *FtwRecord
+  );
+
+/**
+  To check if FtwRecord is the last record of FtwHeader. Because the
+  FtwHeader has NumberOfWrites & PrivateDataSize, the FtwRecord can be
+  determined if it is the last record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to the write record
+
+  @retval TRUE      FtwRecord is the last Record of the FtwHeader
+  @retval FALSE     FtwRecord is not the last Record of the FtwHeader
+
+**/
+BOOLEAN
+IsLastRecordOfWrites (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN EFI_FAULT_TOLERANT_WRITE_RECORD    *FtwRecord
+  );
+
+/**
+  To check if FtwRecord is the first record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to retrieve the previous write record
+
+  @retval EFI_ACCESS_DENIED  Input record is the first record, no previous record is return.
+  @retval EFI_SUCCESS        The previous write record is found.
+
+**/
+EFI_STATUS
+GetPreviousRecordOfWrites (
+  IN     EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN OUT EFI_FAULT_TOLERANT_WRITE_RECORD    **FtwRecord
+  );
+
+/**
+
+  Check whether a flash buffer is erased.
+
+  @param Buffer          Buffer to check
+  @param BufferSize      Size of the buffer
+
+  @return A BOOLEAN value indicating erased or not.
+
+**/
+BOOLEAN
+IsErasedFlashBuffer (
+  IN UINT8           *Buffer,
+  IN UINTN           BufferSize
+  );
+/**
+  Initialize a work space when there is no work space.
+
+  @param WorkingHeader   Pointer of working block header
+
+  @retval  EFI_SUCCESS    The function completed successfully
+  @retval  EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+InitWorkSpaceHeader (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingHeader
+  );
+/**
+  Read from working block to refresh the work space in memory.
+
+  @param FtwDevice   Point to private data of FTW driver
+
+  @retval  EFI_SUCCESS    The function completed successfully
+  @retval  EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+WorkSpaceRefresh (
+  IN EFI_FTW_DEVICE  *FtwDevice
+  );
+/**
+  Check to see if it is a valid work space.
+
+
+  @param WorkingHeader   Pointer of working block header
+
+  @retval  EFI_SUCCESS    The function completed successfully
+  @retval  EFI_ABORTED    The function could not complete successfully.
+
+**/
+BOOLEAN
+IsValidWorkSpace (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingHeader
+  );
+/**
+  Reclaim the work space on the working block.
+
+  @param FtwDevice       Point to private data of FTW driver
+  @param PreserveRecord  Whether to preserve the working record is needed
+
+  @retval EFI_SUCCESS            The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES   Allocate memory error
+  @retval EFI_ABORTED            The function could not complete successfully
+
+**/
+EFI_STATUS
+FtwReclaimWorkSpace (
+  IN EFI_FTW_DEVICE  *FtwDevice,
+  IN BOOLEAN         PreserveRecord
+  );
+
+/**
+
+  Get firmware volume block by address.
+
+
+  @param Address         Address specified the block
+  @param FvBlock         The block caller wanted
+
+  @retval  EFI_SUCCESS    The protocol instance if found.
+  @retval  EFI_NOT_FOUND  Block not found
+
+**/
+EFI_HANDLE
+GetFvbByAddress (
+  IN  EFI_PHYSICAL_ADDRESS               Address,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL **FvBlock
+  );
+
+/**
+  Retrive the proper Swap Address Range protocol interface.
+
+  @param[out] SarProtocol       The interface of SAR protocol
+
+  @retval EFI_SUCCESS           The SAR protocol instance was found and returned in SarProtocol.
+  @retval EFI_NOT_FOUND         The SAR protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+FtwGetSarProtocol (
+  OUT VOID                                **SarProtocol
+  );
+  
+/**
+  Function returns an array of handles that support the FVB protocol
+  in a buffer allocated from pool. 
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  );
+
+
+/**
+  Allocate private data for FTW driver and initialize it.
+
+  @param[out] FtwData           Pointer to the FTW device structure
+
+  @retval EFI_SUCCESS           Initialize the FTW device successfully.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
+  @retval EFI_INVALID_PARAMETER Workspace or Spare block does not exist
+
+**/
+EFI_STATUS
+InitFtwDevice (
+  OUT EFI_FTW_DEVICE               **FtwData 
+  );
+
+
+/**
+  Initialization for Fault Tolerant Write is done in this handler.
+
+  @param[in, out] FtwDevice     Pointer to the FTW device structure
+
+  @retval EFI_SUCCESS           Initialize the FTW protocol successfully.
+  @retval EFI_NOT_FOUND         No proper FVB protocol was found.
+  
+**/
+EFI_STATUS
+InitFtwProtocol (
+  IN OUT EFI_FTW_DEVICE               *FtwDevice
+  );
+
+/**
+  Initialize a local work space header.
+
+  Since Signature and WriteQueueSize have been known, Crc can be calculated out,
+  then the work space header will be fixed.
+**/
+VOID
+InitializeLocalWorkSpaceHeader (
+  VOID
+  );
+
+/**
+  Read work space data from work block or spare block.
+
+  @param FvBlock        FVB Protocol interface to access the block.
+  @param BlockSize      The size of the block.
+  @param Lba            Lba of the block.
+  @param Offset         The offset within the block.
+  @param Length         The number of bytes to read from the block.
+  @param Buffer         The data is read.
+
+  @retval EFI_SUCCESS   The function completed successfully.
+  @retval EFI_ABORTED   The function could not complete successfully.
+
+**/
+EFI_STATUS
+ReadWorkSpaceData (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL *FvBlock,
+  IN UINTN                              BlockSize,
+  IN EFI_LBA                            Lba,
+  IN UINTN                              Offset,
+  IN UINTN                              Length,
+  OUT UINT8                             *Buffer
+  );
+
+/**
+  Write data to work block.
+
+  @param FvBlock        FVB Protocol interface to access the block.
+  @param BlockSize      The size of the block.
+  @param Lba            Lba of the block.
+  @param Offset         The offset within the block to place the data.
+  @param Length         The number of bytes to write to the block.
+  @param Buffer         The data to write.
+
+  @retval EFI_SUCCESS   The function completed successfully.
+  @retval EFI_ABORTED   The function could not complete successfully.
+
+**/
+EFI_STATUS
+WriteWorkSpaceData (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL *FvBlock,
+  IN UINTN                              BlockSize,
+  IN EFI_LBA                            Lba,
+  IN UINTN                              Offset,
+  IN UINTN                              Length,
+  IN UINT8                              *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.c
new file mode 100644
index 0000000000..f570729273
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.c
@@ -0,0 +1,252 @@
+/** @file
+
+  This is a simple fault tolerant write driver.
+
+  This boot service protocol only provides fault tolerant write capability for 
+  block devices.  The protocol has internal non-volatile intermediate storage 
+  of the data and private information. It should be able to recover 
+  automatically from a critical fault, such as power failure. 
+
+  The implementation uses an FTW (Fault Tolerant Write) Work Space. 
+  This work space is a memory copy of the work space on the Working Block,
+  the size of the work space is the FTW_WORK_SPACE_SIZE bytes.
+  
+  The work space stores each write record as EFI_FTW_RECORD structure.
+  The spare block stores the write buffer before write to the target block.
+  
+  The write record has three states to specify the different phase of write operation.
+  1) WRITE_ALLOCATED is that the record is allocated in write space.
+     The information of write operation is stored in write record structure.
+  2) SPARE_COMPLETED is that the data from write buffer is writed into the spare block as the backup.
+  3) WRITE_COMPLETED is that the data is copied from the spare block to the target block.
+
+  This driver operates the data as the whole size of spare block.
+  It first read the SpareAreaLength data from the target block into the spare memory buffer.
+  Then copy the write buffer data into the spare memory buffer.
+  Then write the spare memory buffer into the spare block.
+  Final copy the data from the spare block to the target block.
+
+  To make this drive work well, the following conditions must be satisfied:
+  1. The write NumBytes data must be fit within Spare area. 
+     Offset + NumBytes <= SpareAreaLength
+  2. The whole flash range has the same block size.
+  3. Working block is an area which contains working space in its last block and has the same size as spare block.
+  4. Working Block area must be in the single one Firmware Volume Block range which FVB protocol is produced on.  
+  5. Spare area must be in the single one Firmware Volume Block range which FVB protocol is produced on.
+  6. Any write data area (SpareAreaLength Area) which the data will be written into must be 
+     in the single one Firmware Volume Block range which FVB protocol is produced on.
+  7. If write data area (such as Variable range) is enlarged, the spare area range must be enlarged.
+     The spare area must be enough large to store the write data before write them into the target range.
+  If one of them is not satisfied, FtwWrite may fail.
+  Usually, Spare area only takes one block. That's SpareAreaLength = BlockSize, NumberOfSpareBlock = 1.
+
+Copyright (c) 2006 - 2014, 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 "FaultTolerantWrite.h"
+EFI_EVENT                                 mFvbRegistration = NULL;
+
+
+/**
+  Retrive the FVB protocol interface by HANDLE.
+
+  @param[in]  FvBlockHandle     The handle of FVB protocol that provides services for
+                                reading, writing, and erasing the target block.
+  @param[out] FvBlock           The interface of FVB protocol
+
+  @retval EFI_SUCCESS           The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED       The device does not support the FVB protocol.
+  @retval EFI_INVALID_PARAMETER FvBlockHandle is not a valid EFI_HANDLE or FvBlock is NULL.
+  
+**/
+EFI_STATUS
+FtwGetFvbByHandle (
+  IN  EFI_HANDLE                          FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  )
+{
+  //
+  // To get the FVB protocol interface on the handle
+  //
+  return gBS->HandleProtocol (
+                FvBlockHandle,
+                &gEfiFirmwareVolumeBlockProtocolGuid,
+                (VOID **) FvBlock
+                );
+}
+
+/**
+  Retrive the Swap Address Range protocol interface.
+
+  @param[out] SarProtocol       The interface of SAR protocol
+
+  @retval EFI_SUCCESS           The SAR protocol instance was found and returned in SarProtocol.
+  @retval EFI_NOT_FOUND         The SAR protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+FtwGetSarProtocol (
+  OUT VOID                                **SarProtocol
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate Swap Address Range protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiSwapAddressRangeProtocolGuid, 
+                  NULL, 
+                  SarProtocol
+                  );
+  return Status;
+}
+
+/**
+  Function returns an array of handles that support the FVB protocol
+  in a buffer allocated from pool. 
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+  
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate all handles of Fvb protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolumeBlockProtocolGuid,
+                  NULL,
+                  NumberHandles,
+                  Buffer
+                  );
+  return Status;
+}
+
+
+/**
+  Firmware Volume Block Protocol notification event handler.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+FvbNotificationEvent (
+  IN  EFI_EVENT                           Event,
+  IN  VOID                                *Context
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_FAULT_TOLERANT_WRITE_PROTOCOL       *FtwProtocol;
+  EFI_FTW_DEVICE                          *FtwDevice;
+
+  //
+  // Just return to avoid installing FaultTolerantWriteProtocol again
+  // if Fault Tolerant Write protocol has been installed.
+  //  
+  Status = gBS->LocateProtocol (
+                  &gEfiFaultTolerantWriteProtocolGuid, 
+                  NULL, 
+                  (VOID **) &FtwProtocol
+                  );
+  if (!EFI_ERROR (Status)) {
+    return ;
+  }
+
+  //
+  // Found proper FVB protocol and initialize FtwDevice for protocol installation
+  //
+  FtwDevice = (EFI_FTW_DEVICE *)Context;
+  Status = InitFtwProtocol (FtwDevice);
+  if (EFI_ERROR(Status)) {
+    return ;
+  }                          
+    
+  //
+  // Install protocol interface
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &FtwDevice->Handle,
+                  &gEfiFaultTolerantWriteProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &FtwDevice->FtwInstance
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  Status = gBS->CloseEvent (Event);
+  ASSERT_EFI_ERROR (Status);
+  
+  return;
+}
+
+
+/**
+  This function is the entry point of the Fault Tolerant Write driver.
+
+  @param[in] ImageHandle        A handle for the image that is initializing this driver
+  @param[in] SystemTable        A pointer to the EFI system table
+
+  @retval EFI_SUCCESS           The initialization finished successfully.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
+  @retval EFI_INVALID_PARAMETER Workspace or Spare block does not exist
+  
+**/
+EFI_STATUS
+EFIAPI
+FaultTolerantWriteInitialize (
+  IN EFI_HANDLE                           ImageHandle,
+  IN EFI_SYSTEM_TABLE                     *SystemTable
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_FTW_DEVICE                          *FtwDevice;
+
+  FtwDevice = NULL;
+
+  //
+  // Allocate private data structure for FTW protocol and do some initialization
+  //
+  Status = InitFtwDevice (&FtwDevice);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Register FvbNotificationEvent () notify function.
+  // 
+  EfiCreateProtocolNotifyEvent (
+    &gEfiFirmwareVolumeBlockProtocolGuid,
+    TPL_CALLBACK,
+    FvbNotificationEvent,
+    (VOID *)FtwDevice,
+    &mFvbRegistration
+    );
+  
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.inf b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.inf
new file mode 100644
index 0000000000..5649241f71
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.inf
@@ -0,0 +1,91 @@
+## @file
+# Fault Tolerant Write Dxe Driver.
+#
+# This driver installs Fault Tolerant Write (FTW) protocol, 
+# which provides fault tolerant write capability for block devices.
+# Its implementation depends on the full functionality FVB protocol that support read, write/erase flash access.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FaultTolerantWriteDxe
+  MODULE_UNI_FILE                = FaultTolerantWriteDxe.uni
+  FILE_GUID                      = FE5CEA76-4F72-49e8-986F-2CD899DFFE5D
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = FaultTolerantWriteInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FtwMisc.c
+  UpdateWorkingBlock.c
+  FaultTolerantWrite.c
+  FaultTolerantWriteDxe.c
+  FaultTolerantWrite.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiDriverEntryPoint
+  DebugLib
+  UefiLib
+  PcdLib
+  ReportStatusCodeLib
+
+[Guids]
+  #
+  # Signature in EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER
+  #
+  ## CONSUMES           ## GUID
+  ## PRODUCES           ## GUID
+  gEdkiiWorkingBlockSignatureGuid
+
+[Protocols]
+  gEfiSwapAddressRangeProtocolGuid | gEfiMdeModulePkgTokenSpaceGuid.PcdFullFtwServiceEnable ## SOMETIMES_CONSUMES
+  ## NOTIFY
+  ## CONSUMES
+  gEfiFirmwareVolumeBlockProtocolGuid
+  gEfiFaultTolerantWriteProtocolGuid            ## PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFullFtwServiceEnable    ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase64  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase64    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareSize      ## CONSUMES
+
+#
+# gBS->CalculateCrc32() is consumed in EntryPoint.
+# PI spec said: When the DXE Foundation is notified that the EFI_RUNTIME_ARCH_PROTOCOL
+# has been installed, then the Boot Service CalculateCrc32() is available.
+# So add gEfiRuntimeArchProtocolGuid Depex here.
+#
+[Depex]
+  gEfiFirmwareVolumeBlockProtocolGuid AND gEfiRuntimeArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FaultTolerantWriteDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.uni
new file mode 100644
index 0000000000..694784ba57
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxe.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxeExtra.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxeExtra.uni
new file mode 100644
index 0000000000..d4135ffab3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.c
new file mode 100644
index 0000000000..bc5e6177c3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.c
@@ -0,0 +1,650 @@
+/** @file
+
+  This is a simple fault tolerant write driver that is intended to use in the SMM environment.
+
+  This boot service protocol only provides fault tolerant write capability for 
+  block devices.  The protocol has internal non-volatile intermediate storage 
+  of the data and private information. It should be able to recover 
+  automatically from a critical fault, such as power failure. 
+
+  The implementation uses an FTW (Fault Tolerant Write) Work Space. 
+  This work space is a memory copy of the work space on the Working Block,
+  the size of the work space is the FTW_WORK_SPACE_SIZE bytes.
+  
+  The work space stores each write record as EFI_FTW_RECORD structure.
+  The spare block stores the write buffer before write to the target block.
+  
+  The write record has three states to specify the different phase of write operation.
+  1) WRITE_ALLOCATED is that the record is allocated in write space.
+     The information of write operation is stored in write record structure.
+  2) SPARE_COMPLETED is that the data from write buffer is writed into the spare block as the backup.
+  3) WRITE_COMPLETED is that the data is copied from the spare block to the target block.
+
+  This driver operates the data as the whole size of spare block.
+  It first read the SpareAreaLength data from the target block into the spare memory buffer.
+  Then copy the write buffer data into the spare memory buffer.
+  Then write the spare memory buffer into the spare block.
+  Final copy the data from the spare block to the target block.
+
+  To make this drive work well, the following conditions must be satisfied:
+  1. The write NumBytes data must be fit within Spare area. 
+     Offset + NumBytes <= SpareAreaLength
+  2. The whole flash range has the same block size.
+  3. Working block is an area which contains working space in its last block and has the same size as spare block.
+  4. Working Block area must be in the single one Firmware Volume Block range which FVB protocol is produced on.  
+  5. Spare area must be in the single one Firmware Volume Block range which FVB protocol is produced on.
+  6. Any write data area (SpareAreaLength Area) which the data will be written into must be 
+     in the single one Firmware Volume Block range which FVB protocol is produced on.
+  7. If write data area (such as Variable range) is enlarged, the spare area range must be enlarged.
+     The spare area must be enough large to store the write data before write them into the target range.
+  If one of them is not satisfied, FtwWrite may fail.
+  Usually, Spare area only takes one block. That's SpareAreaLength = BlockSize, NumberOfSpareBlock = 1.
+
+  Caution: This module requires additional review when modified.
+  This driver need to make sure the CommBuffer is not in the SMRAM range. 
+
+Copyright (c) 2010 - 2015, 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 <PiSmm.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/SmmMemLib.h>
+#include <Protocol/SmmSwapAddressRange.h>
+#include "FaultTolerantWrite.h"
+#include "FaultTolerantWriteSmmCommon.h"
+#include <Protocol/SmmEndOfDxe.h>
+
+EFI_EVENT                                 mFvbRegistration = NULL;
+EFI_FTW_DEVICE                            *mFtwDevice      = NULL;
+
+///
+/// The flag to indicate whether the platform has left the DXE phase of execution.
+///
+BOOLEAN                                   mEndOfDxe = FALSE;
+
+/**
+  Retrive the SMM FVB protocol interface by HANDLE.
+
+  @param[in]  FvBlockHandle     The handle of SMM FVB protocol that provides services for
+                                reading, writing, and erasing the target block.
+  @param[out] FvBlock           The interface of SMM FVB protocol
+
+  @retval EFI_SUCCESS           The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED       The device does not support the SMM FVB protocol.
+  @retval EFI_INVALID_PARAMETER FvBlockHandle is not a valid EFI_HANDLE or FvBlock is NULL.
+
+**/
+EFI_STATUS
+FtwGetFvbByHandle (
+  IN  EFI_HANDLE                          FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  )
+{
+  //
+  // To get the SMM FVB protocol interface on the handle
+  //
+  return gSmst->SmmHandleProtocol (
+                  FvBlockHandle,
+                  &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                  (VOID **) FvBlock
+                  );
+}
+
+/**
+  Retrive the SMM Swap Address Range protocol interface.
+
+  @param[out] SarProtocol       The interface of SMM SAR protocol
+
+  @retval EFI_SUCCESS           The SMM SAR protocol instance was found and returned in SarProtocol.
+  @retval EFI_NOT_FOUND         The SMM SAR protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+FtwGetSarProtocol (
+  OUT VOID                                **SarProtocol
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate Smm Swap Address Range protocol
+  //
+  Status = gSmst->SmmLocateProtocol (
+                    &gEfiSmmSwapAddressRangeProtocolGuid, 
+                    NULL, 
+                    SarProtocol
+                    );
+  return Status;
+}
+
+/**
+  Function returns an array of handles that support the SMM FVB protocol
+  in a buffer allocated from pool. 
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support SMM FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No SMM FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  )
+{
+  EFI_STATUS                              Status;
+  UINTN                                   BufferSize;
+
+  if ((NumberHandles == NULL) || (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BufferSize     = 0;
+  *NumberHandles = 0;
+  *Buffer        = NULL;
+  Status = gSmst->SmmLocateHandle (
+                    ByProtocol,
+                    &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                    NULL,
+                    &BufferSize,
+                    *Buffer
+                    );
+  if (EFI_ERROR(Status) && Status != EFI_BUFFER_TOO_SMALL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Buffer = AllocatePool (BufferSize);
+  if (*Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gSmst->SmmLocateHandle (
+                    ByProtocol,
+                    &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                    NULL,
+                    &BufferSize,
+                    *Buffer
+                    );
+
+  *NumberHandles = BufferSize / sizeof(EFI_HANDLE);
+  if (EFI_ERROR(Status)) {
+    *NumberHandles = 0;
+    FreePool (*Buffer);
+    *Buffer = NULL;
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the handle of the SMM FVB protocol by the FVB base address and attributes.
+
+  @param[in]  Address       The base address of SMM FVB protocol.
+  @param[in]  Attributes    The attributes of the SMM FVB protocol.
+  @param[out] SmmFvbHandle  The handle of the SMM FVB protocol.
+
+  @retval  EFI_SUCCESS    The FVB handle is found.
+  @retval  EFI_ABORTED    The FVB protocol is not found.
+
+**/
+EFI_STATUS
+GetFvbByAddressAndAttribute (
+  IN  EFI_PHYSICAL_ADDRESS            Address,
+  IN  EFI_FVB_ATTRIBUTES_2            Attributes,
+  OUT EFI_HANDLE                      *SmmFvbHandle
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HANDLE                          *HandleBuffer;
+  UINTN                               HandleCount;
+  UINTN                               Index;
+  EFI_PHYSICAL_ADDRESS                FvbBaseAddress;
+  EFI_FVB_ATTRIBUTES_2                FvbAttributes;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+
+  HandleBuffer = NULL;
+
+  //
+  // Locate all handles of SMM Fvb protocol.
+  //
+  Status = GetFvbCountAndBuffer (&HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  
+  //
+  // Find the proper SMM Fvb handle by the address and attributes.
+  //
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = FtwGetFvbByHandle (HandleBuffer[Index], &Fvb);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    //
+    // Compare the address.
+    //
+    Status = Fvb->GetPhysicalAddress (Fvb, &FvbBaseAddress);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    if (Address != FvbBaseAddress) {
+     continue;
+    }
+
+    //
+    // Compare the attribute.
+    //
+    Status = Fvb->GetAttributes (Fvb, &FvbAttributes);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    if (Attributes != FvbAttributes) {
+     continue;
+    }
+
+    //
+    // Found the proper FVB handle.
+    //
+    *SmmFvbHandle = HandleBuffer[Index];
+    FreePool (HandleBuffer);
+    return EFI_SUCCESS;
+  }
+
+  FreePool (HandleBuffer);
+  return EFI_ABORTED;
+}
+
+/**
+  Communication service SMI Handler entry.
+
+  This SMI handler provides services for the fault tolerant write wrapper driver.
+
+  Caution: This function requires additional review when modified.
+  This driver need to make sure the CommBuffer is not in the SMRAM range. 
+  Also in FTW_FUNCTION_GET_LAST_WRITE case, check SmmFtwGetLastWriteHeader->Data + 
+  SmmFtwGetLastWriteHeader->PrivateDataSize within communication buffer.
+
+  @param[in]     DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param[in]     RegisterContext Points to an optional handler context which was specified when the
+                                 handler was registered.
+  @param[in, out] CommBuffer     A pointer to a collection of data in memory that will be conveyed
+                                 from a non-SMM environment into an SMM environment.
+  @param[in, out] CommBufferSize The size of the CommBuffer.
+
+  @retval EFI_SUCCESS                         The interrupt was handled and quiesced. No other handlers 
+                                              should still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED  The interrupt has been quiesced but other handlers should 
+                                              still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING   The interrupt is still pending and other handlers should still 
+                                              be called.
+  @retval EFI_INTERRUPT_PENDING               The interrupt could not be quiesced.
+  
+**/
+EFI_STATUS
+EFIAPI
+SmmFaultTolerantWriteHandler (
+  IN     EFI_HANDLE                                DispatchHandle,
+  IN     CONST VOID                                *RegisterContext,
+  IN OUT VOID                                      *CommBuffer,
+  IN OUT UINTN                                     *CommBufferSize
+  )
+{
+  EFI_STATUS                                       Status;
+  SMM_FTW_COMMUNICATE_FUNCTION_HEADER              *SmmFtwFunctionHeader;
+  SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER                *SmmGetMaxBlockSizeHeader;
+  SMM_FTW_ALLOCATE_HEADER                          *SmmFtwAllocateHeader;
+  SMM_FTW_WRITE_HEADER                             *SmmFtwWriteHeader;
+  SMM_FTW_RESTART_HEADER                           *SmmFtwRestartHeader;
+  SMM_FTW_GET_LAST_WRITE_HEADER                    *SmmFtwGetLastWriteHeader;
+  VOID                                             *PrivateData;
+  EFI_HANDLE                                       SmmFvbHandle;
+  UINTN                                            InfoSize;
+  UINTN                                            CommBufferPayloadSize;
+  UINTN                                            PrivateDataSize;
+  UINTN                                            Length;
+  UINTN                                            TempCommBufferSize;
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if (TempCommBufferSize < SMM_FTW_COMMUNICATE_HEADER_SIZE) {
+    DEBUG ((EFI_D_ERROR, "SmmFtwHandler: SMM communication buffer size invalid!\n"));
+    return EFI_SUCCESS;
+  }
+  CommBufferPayloadSize = TempCommBufferSize - SMM_FTW_COMMUNICATE_HEADER_SIZE;
+
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmmFtwHandler: SMM communication buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  SmmFtwFunctionHeader = (SMM_FTW_COMMUNICATE_FUNCTION_HEADER *)CommBuffer;
+
+  if (mEndOfDxe) {
+    //
+    // It will be not safe to expose the operations after End Of Dxe.
+    //
+    DEBUG ((EFI_D_ERROR, "SmmFtwHandler: Not safe to do the operation: %x after End Of Dxe, so access denied!\n", SmmFtwFunctionHeader->Function));
+    SmmFtwFunctionHeader->ReturnStatus = EFI_ACCESS_DENIED;
+    return EFI_SUCCESS;
+  }
+
+  switch (SmmFtwFunctionHeader->Function) {
+    case FTW_FUNCTION_GET_MAX_BLOCK_SIZE:
+      if (CommBufferPayloadSize < sizeof (SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER)) {
+        DEBUG ((EFI_D_ERROR, "GetMaxBlockSize: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      SmmGetMaxBlockSizeHeader = (SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER *) SmmFtwFunctionHeader->Data;
+
+      Status = FtwGetMaxBlockSize (
+                 &mFtwDevice->FtwInstance,
+                 &SmmGetMaxBlockSizeHeader->BlockSize
+                 );
+      break;
+      
+    case FTW_FUNCTION_ALLOCATE:
+      if (CommBufferPayloadSize < sizeof (SMM_FTW_ALLOCATE_HEADER)) {
+        DEBUG ((EFI_D_ERROR, "Allocate: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      SmmFtwAllocateHeader = (SMM_FTW_ALLOCATE_HEADER *) SmmFtwFunctionHeader->Data;
+      Status = FtwAllocate (
+                 &mFtwDevice->FtwInstance,
+                 &SmmFtwAllocateHeader->CallerId,
+                 SmmFtwAllocateHeader->PrivateDataSize,
+                 SmmFtwAllocateHeader->NumberOfWrites
+                 );
+      break;
+      
+    case FTW_FUNCTION_WRITE:
+      if (CommBufferPayloadSize < OFFSET_OF (SMM_FTW_WRITE_HEADER, Data)) {
+        DEBUG ((EFI_D_ERROR, "Write: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      SmmFtwWriteHeader = (SMM_FTW_WRITE_HEADER *) SmmFtwFunctionHeader->Data;
+      Length = SmmFtwWriteHeader->Length;
+      PrivateDataSize = SmmFtwWriteHeader->PrivateDataSize;
+      if (((UINTN)(~0) - Length < OFFSET_OF (SMM_FTW_WRITE_HEADER, Data)) ||
+        ((UINTN)(~0) - PrivateDataSize < OFFSET_OF (SMM_FTW_WRITE_HEADER, Data) + Length)) {
+        //
+        // Prevent InfoSize overflow
+        //
+        Status = EFI_ACCESS_DENIED;
+        break;
+      }
+      InfoSize = OFFSET_OF (SMM_FTW_WRITE_HEADER, Data) + Length + PrivateDataSize;
+
+      //
+      // SMRAM range check already covered before
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "Write: Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        break;
+      }
+
+      if (PrivateDataSize == 0) {
+        PrivateData = NULL;
+      } else {
+        PrivateData = (VOID *)&SmmFtwWriteHeader->Data[Length];
+      }
+      Status = GetFvbByAddressAndAttribute (
+                 SmmFtwWriteHeader->FvbBaseAddress, 
+                 SmmFtwWriteHeader->FvbAttributes,
+                 &SmmFvbHandle
+                 );
+      if (!EFI_ERROR (Status)) {
+        Status = FtwWrite(
+                   &mFtwDevice->FtwInstance,
+                   SmmFtwWriteHeader->Lba,
+                   SmmFtwWriteHeader->Offset,
+                   Length,
+                   PrivateData,
+                   SmmFvbHandle,
+                   SmmFtwWriteHeader->Data
+                   );
+      }
+      break;
+      
+    case FTW_FUNCTION_RESTART:
+      if (CommBufferPayloadSize < sizeof (SMM_FTW_RESTART_HEADER)) {
+        DEBUG ((EFI_D_ERROR, "Restart: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      SmmFtwRestartHeader = (SMM_FTW_RESTART_HEADER *) SmmFtwFunctionHeader->Data;
+      Status = GetFvbByAddressAndAttribute (
+                 SmmFtwRestartHeader->FvbBaseAddress, 
+                 SmmFtwRestartHeader->FvbAttributes,
+                 &SmmFvbHandle
+                 );      
+      if (!EFI_ERROR (Status)) {
+        Status = FtwRestart (&mFtwDevice->FtwInstance, SmmFvbHandle);
+      }
+      break;
+
+    case FTW_FUNCTION_ABORT:
+      Status = FtwAbort (&mFtwDevice->FtwInstance);
+      break;
+      
+    case FTW_FUNCTION_GET_LAST_WRITE:
+      if (CommBufferPayloadSize < OFFSET_OF (SMM_FTW_GET_LAST_WRITE_HEADER, Data)) {
+        DEBUG ((EFI_D_ERROR, "GetLastWrite: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      SmmFtwGetLastWriteHeader = (SMM_FTW_GET_LAST_WRITE_HEADER *) SmmFtwFunctionHeader->Data;
+      PrivateDataSize = SmmFtwGetLastWriteHeader->PrivateDataSize;
+      if ((UINTN)(~0) - PrivateDataSize < OFFSET_OF (SMM_FTW_GET_LAST_WRITE_HEADER, Data)){
+        //
+        // Prevent InfoSize overflow
+        //
+        Status = EFI_ACCESS_DENIED;
+        break;
+      }
+      InfoSize = OFFSET_OF (SMM_FTW_GET_LAST_WRITE_HEADER, Data) + PrivateDataSize;
+
+      //
+      // SMRAM range check already covered before
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        break;
+      }
+
+      Status = FtwGetLastWrite (
+                 &mFtwDevice->FtwInstance,
+                 &SmmFtwGetLastWriteHeader->CallerId,
+                 &SmmFtwGetLastWriteHeader->Lba,
+                 &SmmFtwGetLastWriteHeader->Offset,
+                 &SmmFtwGetLastWriteHeader->Length,
+                 &PrivateDataSize,
+                 (VOID *)SmmFtwGetLastWriteHeader->Data,
+                 &SmmFtwGetLastWriteHeader->Complete
+                 );
+      SmmFtwGetLastWriteHeader->PrivateDataSize = PrivateDataSize;
+      break;
+
+    default:
+      Status = EFI_UNSUPPORTED;
+  }
+
+  SmmFtwFunctionHeader->ReturnStatus = Status;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  SMM Firmware Volume Block Protocol notification event handler.
+  
+  @param[in]  Protocol      Points to the protocol's unique identifier
+  @param[in]  Interface     Points to the interface instance
+  @param[in]  Handle        The handle on which the interface was installed
+
+  @retval EFI_SUCCESS       SmmEventCallback runs successfully
+  
+ **/
+EFI_STATUS
+EFIAPI
+FvbNotificationEvent (
+  IN CONST EFI_GUID                       *Protocol,
+  IN VOID                                 *Interface,
+  IN EFI_HANDLE                           Handle
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_SMM_FAULT_TOLERANT_WRITE_PROTOCOL   *FtwProtocol;
+  EFI_HANDLE                              SmmFtwHandle;
+  EFI_HANDLE                              FtwHandle;
+  
+  //
+  // Just return to avoid install SMM FaultTolerantWriteProtocol again
+  // if SMM Fault Tolerant Write protocol had been installed.
+  //  
+  Status = gSmst->SmmLocateProtocol (
+                    &gEfiSmmFaultTolerantWriteProtocolGuid, 
+                    NULL, 
+                    (VOID **) &FtwProtocol
+                    );
+  if (!EFI_ERROR (Status)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Found proper FVB protocol and initialize FtwDevice for protocol installation
+  //
+  Status = InitFtwProtocol (mFtwDevice);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Install protocol interface
+  //
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &mFtwDevice->Handle,
+                    &gEfiSmmFaultTolerantWriteProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mFtwDevice->FtwInstance
+                    );
+  ASSERT_EFI_ERROR (Status); 
+
+  ///
+  /// Register SMM FTW SMI handler
+  ///
+  Status = gSmst->SmiHandlerRegister (SmmFaultTolerantWriteHandler, &gEfiSmmFaultTolerantWriteProtocolGuid, &SmmFtwHandle);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Notify the Ftw wrapper driver SMM Ftw is ready
+  //
+  FtwHandle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &FtwHandle,
+                  &gEfiSmmFaultTolerantWriteProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  SMM END_OF_DXE protocol notification event handler.
+ 
+  @param  Protocol   Points to the protocol's unique identifier
+  @param  Interface  Points to the interface instance
+  @param  Handle     The handle on which the interface was installed
+
+  @retval EFI_SUCCESS   SmmEndOfDxeCallback runs successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEndOfDxeCallback (
+  IN CONST EFI_GUID                       *Protocol,
+  IN VOID                                 *Interface,
+  IN EFI_HANDLE                           Handle
+  )
+{
+  mEndOfDxe = TRUE;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is the entry point of the Fault Tolerant Write driver.
+
+  @param[in] ImageHandle        A handle for the image that is initializing this driver
+  @param[in] SystemTable        A pointer to the EFI system table
+
+  @retval EFI_SUCCESS           The initialization finished successfully.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
+  @retval EFI_INVALID_PARAMETER Workspace or Spare block does not exist
+
+**/
+EFI_STATUS
+EFIAPI
+SmmFaultTolerantWriteInitialize (
+  IN EFI_HANDLE                           ImageHandle,
+  IN EFI_SYSTEM_TABLE                     *SystemTable
+  )
+{
+  EFI_STATUS                              Status;
+  VOID                                    *SmmEndOfDxeRegistration;
+
+  //
+  // Allocate private data structure for SMM FTW protocol and do some initialization
+  //
+  Status = InitFtwDevice (&mFtwDevice);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  //
+  // Register EFI_SMM_END_OF_DXE_PROTOCOL_GUID notify function.
+  //
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmEndOfDxeProtocolGuid,
+                    SmmEndOfDxeCallback,
+                    &SmmEndOfDxeRegistration
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register FvbNotificationEvent () notify function.
+  // 
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                    FvbNotificationEvent,
+                    &mFvbRegistration
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  FvbNotificationEvent (NULL, NULL, NULL);
+  
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.inf b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.inf
new file mode 100644
index 0000000000..3b57c3cf64
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmm.inf
@@ -0,0 +1,98 @@
+## @file
+#   Fault Tolerant Write Smm Driver.
+#
+#   This driver installs SMM Fault Tolerant Write (FTW) protocol, which provides fault 
+#   tolerant write capability in SMM environment for block devices. Its implementation 
+#   depends on the full functionality SMM FVB protocol that support read, write/erase 
+#   flash access.
+#
+# Copyright (c) 2010 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmFaultTolerantWriteDxe
+  MODULE_UNI_FILE                = SmmFaultTolerantWriteDxe.uni
+  FILE_GUID                      = 470CB248-E8AC-473c-BB4F-81069A1FE6FD
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = SmmFaultTolerantWriteInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  FtwMisc.c
+  UpdateWorkingBlock.c
+  FaultTolerantWrite.c
+  FaultTolerantWriteSmm.c
+  FaultTolerantWrite.h
+  FaultTolerantWriteSmmCommon.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SmmServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiDriverEntryPoint
+  DebugLib
+  UefiLib
+  PcdLib
+  ReportStatusCodeLib
+  SmmMemLib
+
+[Guids]
+  #
+  # Signature in EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER
+  #
+  ## CONSUMES           ## GUID
+  ## PRODUCES           ## GUID
+  gEdkiiWorkingBlockSignatureGuid
+
+[Protocols]
+  gEfiSmmSwapAddressRangeProtocolGuid | gEfiMdeModulePkgTokenSpaceGuid.PcdFullFtwServiceEnable  ## SOMETIMES_CONSUMES
+  ## NOTIFY
+  ## CONSUMES
+  gEfiSmmFirmwareVolumeBlockProtocolGuid
+  ## PRODUCES
+  ## UNDEFINED # SmiHandlerRegister
+  gEfiSmmFaultTolerantWriteProtocolGuid
+  gEfiSmmEndOfDxeProtocolGuid                      ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFullFtwServiceEnable    ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase64  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase64    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareSize      ## CONSUMES
+
+#
+# gBS->CalculateCrc32() is consumed in EntryPoint.
+# PI spec said: When the DXE Foundation is notified that the EFI_RUNTIME_ARCH_PROTOCOL
+# has been installed, then the Boot Service CalculateCrc32() is available.
+# So add gEfiRuntimeArchProtocolGuid Depex here.
+#
+[Depex]
+  gEfiSmmFirmwareVolumeBlockProtocolGuid AND gEfiRuntimeArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmmFaultTolerantWriteDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmCommon.h b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmCommon.h
new file mode 100644
index 0000000000..061673bce2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmCommon.h
@@ -0,0 +1,80 @@
+/** @file
+
+  The common header file for SMM FTW module and SMM FTW DXE Module. 
+
+Copyright (c) 2011, 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. 
+
+**/
+
+#ifndef __SMM_FTW_COMMON_H__
+#define __SMM_FTW_COMMON_H__
+
+#include <Protocol/SmmFirmwareVolumeBlock.h>
+#include <Protocol/SmmFaultTolerantWrite.h>
+
+#define FTW_FUNCTION_GET_MAX_BLOCK_SIZE       1
+#define FTW_FUNCTION_ALLOCATE                 2
+#define FTW_FUNCTION_WRITE                    3
+#define FTW_FUNCTION_RESTART                  4
+#define FTW_FUNCTION_ABORT                    5
+#define FTW_FUNCTION_GET_LAST_WRITE           6
+
+typedef struct {
+  UINTN       Function;
+  EFI_STATUS  ReturnStatus;
+  UINT8       Data[1];
+} SMM_FTW_COMMUNICATE_FUNCTION_HEADER;
+
+///
+/// Size of SMM communicate header, without including the payload.
+///
+#define SMM_COMMUNICATE_HEADER_SIZE  (OFFSET_OF (EFI_SMM_COMMUNICATE_HEADER, Data))
+
+///
+/// Size of SMM FTW communicate function header, without including the payload.
+///
+#define SMM_FTW_COMMUNICATE_HEADER_SIZE  (OFFSET_OF (SMM_FTW_COMMUNICATE_FUNCTION_HEADER, Data))
+
+typedef struct {
+  UINTN                                 BlockSize;
+} SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER;
+
+typedef struct {
+  EFI_GUID                              CallerId;
+  UINTN                                 PrivateDataSize;
+  UINTN                                 NumberOfWrites;
+} SMM_FTW_ALLOCATE_HEADER;
+
+typedef struct {
+  EFI_LBA                               Lba;
+  UINTN                                 Offset;
+  UINTN                                 PrivateDataSize;
+  EFI_PHYSICAL_ADDRESS                  FvbBaseAddress;
+  EFI_FVB_ATTRIBUTES_2                  FvbAttributes;
+  UINTN                                 Length;
+  UINT8                                 Data[1];
+} SMM_FTW_WRITE_HEADER;
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS                  FvbBaseAddress;
+  EFI_FVB_ATTRIBUTES_2                  FvbAttributes;
+} SMM_FTW_RESTART_HEADER;
+
+typedef struct {
+  EFI_GUID                              CallerId;
+  EFI_LBA                               Lba;
+  UINTN                                 Offset;
+  UINTN                                 Length;
+  UINTN                                 PrivateDataSize;
+  BOOLEAN                               Complete;
+  UINT8                                 Data[1];
+} SMM_FTW_GET_LAST_WRITE_HEADER;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.c
new file mode 100644
index 0000000000..772d10dcd4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.c
@@ -0,0 +1,562 @@
+/** @file
+
+  Implement the Fault Tolerant Write (FTW) protocol based on SMM FTW 
+  module.
+
+Copyright (c) 2011 - 2013, 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 "FaultTolerantWriteSmmDxe.h"
+
+EFI_HANDLE                         mHandle                   = NULL;
+EFI_SMM_COMMUNICATION_PROTOCOL     *mSmmCommunication        = NULL;
+UINTN                              mPrivateDataSize          = 0;
+
+EFI_FAULT_TOLERANT_WRITE_PROTOCOL  mFaultTolerantWriteDriver = {
+  FtwGetMaxBlockSize,
+  FtwAllocate,
+  FtwWrite,
+  FtwRestart,
+  FtwAbort,
+  FtwGetLastWrite
+};
+
+/**
+  Initialize the communicate buffer using DataSize and Function number.
+
+  @param[out]      CommunicateBuffer The communicate buffer. Caller should free it after use.
+  @param[out]      DataPtr           Points to the data in the communicate buffer. Caller should not free it.
+  @param[in]       DataSize          The payload size.
+  @param[in]       Function          The function number used to initialize the communicate header.
+
+**/
+VOID
+InitCommunicateBuffer (
+  OUT     VOID                              **CommunicateBuffer,
+  OUT     VOID                              **DataPtr,
+  IN      UINTN                             DataSize,
+  IN      UINTN                             Function
+  )
+{
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_COMMUNICATE_FUNCTION_HEADER       *SmmFtwFunctionHeader; 
+
+  //
+  // The whole buffer size: SMM_COMMUNICATE_HEADER_SIZE + SMM_FTW_COMMUNICATE_HEADER_SIZE + DataSize.
+  //
+  SmmCommunicateHeader = AllocateZeroPool (DataSize + SMM_COMMUNICATE_HEADER_SIZE + SMM_FTW_COMMUNICATE_HEADER_SIZE);
+  ASSERT (SmmCommunicateHeader != NULL);
+   
+  //
+  // Prepare data buffer.
+  //
+  CopyGuid (&SmmCommunicateHeader->HeaderGuid, &gEfiSmmFaultTolerantWriteProtocolGuid);
+  SmmCommunicateHeader->MessageLength = DataSize + SMM_FTW_COMMUNICATE_HEADER_SIZE;
+ 
+  SmmFtwFunctionHeader = (SMM_FTW_COMMUNICATE_FUNCTION_HEADER *) SmmCommunicateHeader->Data;
+  SmmFtwFunctionHeader->Function = Function;
+
+  *CommunicateBuffer = SmmCommunicateHeader;
+  if (DataPtr != NULL) {
+    *DataPtr = SmmFtwFunctionHeader->Data;
+  }  
+}
+
+
+/**
+  Send the data in communicate buffer to SMI handler and get response.
+
+  @param[in, out]  SmmCommunicateHeader    The communicate buffer.
+  @param[in]       DataSize                The payload size.
+                      
+**/
+EFI_STATUS
+SendCommunicateBuffer (
+  IN OUT  EFI_SMM_COMMUNICATE_HEADER        *SmmCommunicateHeader,
+  IN      UINTN                             DataSize
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     CommSize;
+  SMM_FTW_COMMUNICATE_FUNCTION_HEADER       *SmmFtwFunctionHeader; 
+ 
+  CommSize = DataSize + SMM_COMMUNICATE_HEADER_SIZE + SMM_FTW_COMMUNICATE_HEADER_SIZE;
+  Status = mSmmCommunication->Communicate (mSmmCommunication, SmmCommunicateHeader, &CommSize);
+  ASSERT_EFI_ERROR (Status);
+
+  SmmFtwFunctionHeader = (SMM_FTW_COMMUNICATE_FUNCTION_HEADER *) SmmCommunicateHeader->Data;
+  return  SmmFtwFunctionHeader->ReturnStatus;
+}
+
+
+/**
+  Get the FvbBaseAddress and FvbAttributes from the FVB handle FvbHandle.
+
+  @param[in]   FvbHandle         The handle of FVB protocol that provides services.
+  @param[out]  FvbBaseAddress    The base address of the FVB attached with FvbHandle.
+  @param[out]  FvbAttributes     The attributes of the FVB attached with FvbHandle.
+    
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval Others                 The function could not complete successfully.
+
+**/
+EFI_STATUS
+ConvertFvbHandle (
+  IN  EFI_HANDLE                            FvbHandle,
+  OUT EFI_PHYSICAL_ADDRESS                  *FvbBaseAddress,
+  OUT EFI_FVB_ATTRIBUTES_2                  *FvbAttributes
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL        *Fvb;
+
+  Status = gBS->HandleProtocol (FvbHandle, &gEfiFirmwareVolumeBlockProtocolGuid, (VOID **) &Fvb);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  Status = Fvb->GetPhysicalAddress (Fvb, FvbBaseAddress);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = Fvb->GetAttributes (Fvb, FvbAttributes);
+  return Status;  
+}
+
+
+/**
+  Get the size of the largest block that can be updated in a fault-tolerant manner.
+
+  @param[in]  This             Indicates a pointer to the calling context.
+  @param[out] BlockSize        A pointer to a caller-allocated UINTN that is
+                               updated to indicate the size of the largest block
+                               that can be updated.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetMaxBlockSize (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  OUT UINTN                                 *BlockSize
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER         *SmmFtwBlockSizeHeader;
+
+  //
+  // Initialize the communicate buffer.
+  //
+  PayloadSize  = sizeof (SMM_FTW_GET_MAX_BLOCK_SIZE_HEADER);
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, (VOID **)&SmmFtwBlockSizeHeader, PayloadSize, FTW_FUNCTION_GET_MAX_BLOCK_SIZE);
+    
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, PayloadSize);
+
+  //
+  // Get data from SMM
+  //
+  *BlockSize = SmmFtwBlockSizeHeader->BlockSize; 
+  FreePool (SmmCommunicateHeader);
+  
+  return Status;
+}
+
+
+/**
+  Allocates space for the protocol to maintain information about writes.
+  Since writes must be completed in a fault-tolerant manner and multiple
+  writes require more resources to be successful, this function
+  enables the protocol to ensure that enough space exists to track
+  information about upcoming writes.
+
+  @param[in]  This             A pointer to the calling context.
+  @param[in]  CallerId         The GUID identifying the write.
+  @param[in]  PrivateDataSize  The size of the caller's private data  that must be
+                               recorded for each write.
+  @param[in]  NumberOfWrites   The number of fault tolerant block writes that will
+                               need to occur.
+
+  @retval EFI_SUCCESS          The function completed successfully
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    Not all allocated writes have been completed.  All
+                               writes must be completed or aborted before another
+                               fault tolerant write can occur.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAllocate (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_GUID                               *CallerId,
+  IN UINTN                                  PrivateDataSize,
+  IN UINTN                                  NumberOfWrites
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_ALLOCATE_HEADER                   *SmmFtwAllocateHeader;
+
+  //
+  // Initialize the communicate buffer.
+  //
+  PayloadSize  = sizeof (SMM_FTW_ALLOCATE_HEADER);
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, (VOID **)&SmmFtwAllocateHeader, PayloadSize, FTW_FUNCTION_ALLOCATE);
+  CopyGuid (&SmmFtwAllocateHeader->CallerId, CallerId);
+  SmmFtwAllocateHeader->PrivateDataSize = PrivateDataSize;
+  SmmFtwAllocateHeader->NumberOfWrites  = NumberOfWrites; 
+    
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, PayloadSize);
+  if (!EFI_ERROR( Status)) {
+    mPrivateDataSize = PrivateDataSize;
+  }
+
+  FreePool (SmmCommunicateHeader);
+  return Status;
+}
+
+
+/**
+  Starts a target block update. This records information about the write
+  in fault tolerant storage, and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param[in]  This             The calling context.
+  @param[in]  Lba              The logical block address of the target block.
+  @param[in]  Offset           The offset within the target block to place the
+                               data.
+  @param[in]  Length           The number of bytes to write to the target block.
+  @param[in]  PrivateData      A pointer to private data that the caller requires
+                               to complete any pending writes in the event of a
+                               fault.
+  @param[in]  FvBlockHandle    The handle of FVB protocol that provides services
+                               for reading, writing, and erasing the target block.
+  @param[in]  Buffer           The data to write.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_BAD_BUFFER_SIZE  The write would span a block boundary, which is not
+                               a valid action.
+  @retval EFI_ACCESS_DENIED    No writes have been allocated.
+  @retval EFI_NOT_READY        The last write has not been completed. Restart()
+                               must be called to complete it.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_LBA                                Lba,
+  IN UINTN                                  Offset,
+  IN UINTN                                  Length,
+  IN VOID                                   *PrivateData,
+  IN EFI_HANDLE                             FvBlockHandle,
+  IN VOID                                   *Buffer
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_WRITE_HEADER                      *SmmFtwWriteHeader;
+
+  //
+  // Initialize the communicate buffer.
+  //
+  PayloadSize  = OFFSET_OF (SMM_FTW_WRITE_HEADER, Data) + Length;
+  if (PrivateData != NULL) {
+    //
+    // The private data buffer size should be the same one in FtwAllocate API.
+    //
+    PayloadSize += mPrivateDataSize;
+  }
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, (VOID **)&SmmFtwWriteHeader, PayloadSize, FTW_FUNCTION_WRITE);
+
+  //
+  // FvBlockHandle can not be used in SMM environment. Here we get the FVB protocol first, then get FVB base address 
+  // and its attribute. Send these information to SMM handler, the SMM handler will find the proper FVB to write data.
+  //
+  Status = ConvertFvbHandle (FvBlockHandle, &SmmFtwWriteHeader->FvbBaseAddress, &SmmFtwWriteHeader->FvbAttributes);
+  if (EFI_ERROR (Status)) {
+    FreePool (SmmCommunicateHeader);
+    return EFI_ABORTED;
+  }
+  
+  SmmFtwWriteHeader->Lba    = Lba;
+  SmmFtwWriteHeader->Offset = Offset; 
+  SmmFtwWriteHeader->Length = Length;
+  CopyMem (SmmFtwWriteHeader->Data, Buffer, Length);
+  if (PrivateData == NULL) {
+    SmmFtwWriteHeader->PrivateDataSize = 0;
+  } else {
+    SmmFtwWriteHeader->PrivateDataSize = mPrivateDataSize;
+    CopyMem (&SmmFtwWriteHeader->Data[Length], PrivateData, mPrivateDataSize);
+  }
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, PayloadSize);
+  FreePool (SmmCommunicateHeader);  
+  return Status;
+}
+
+
+/**
+  Restarts a previously interrupted write. The caller must provide the
+  block protocol needed to complete the interrupted write.
+
+  @param[in]  This             The calling context.
+  @param[in]  FvBlockHandle    The handle of FVB protocol that provides services.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    No pending writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwRestart (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_HANDLE                             FvBlockHandle
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_RESTART_HEADER                    *SmmFtwRestartHeader;
+ 
+  //
+  // Initialize the communicate buffer.
+  //
+  PayloadSize  = sizeof (SMM_FTW_RESTART_HEADER);
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, (VOID **)&SmmFtwRestartHeader, PayloadSize, FTW_FUNCTION_RESTART); 
+
+  //
+  // FvBlockHandle can not be used in SMM environment. Here we get the FVB protocol first, then get FVB base address 
+  // and its attribute. Send these information to SMM handler, the SMM handler will find the proper FVB to write data.
+  //
+  Status = ConvertFvbHandle (FvBlockHandle, &SmmFtwRestartHeader->FvbBaseAddress, &SmmFtwRestartHeader->FvbAttributes);
+  if (EFI_ERROR (Status)) {
+    FreePool (SmmCommunicateHeader); 
+    return EFI_ABORTED;
+  }
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, PayloadSize);
+  FreePool (SmmCommunicateHeader);  
+  return Status;
+}
+
+
+/**
+  Aborts all previously allocated writes.
+
+  @param[in]  This             The calling context.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAbort (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+ 
+  //
+  // Initialize the communicate buffer.
+  //
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, NULL, 0, FTW_FUNCTION_ABORT);
+ 
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, 0);
+
+  FreePool (SmmCommunicateHeader);  
+  return Status;
+}
+
+
+/**
+  Starts a target block update. This function records information about the write
+  in fault-tolerant storage and completes the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param[in]      This            Indicates a pointer to the calling context.
+  @param[out]     CallerId        The GUID identifying the last write.
+  @param[out]     Lba             The logical block address of the last write.
+  @param[out]     Offset          The offset within the block of the last write.
+  @param[out]     Length          The length of the last write.
+  @param[in, out] PrivateDataSize On input, the size of the PrivateData buffer. On
+                                  output, the size of the private data stored for
+                                  this write.
+  @param[out]     PrivateData     A pointer to a buffer. The function will copy
+                                  PrivateDataSize bytes from the private data stored
+                                  for this write.
+  @param[out]     Complete        A Boolean value with TRUE indicating that the write
+                                  was completed.
+
+  @retval EFI_SUCCESS             The function completed successfully.
+  @retval EFI_ABORTED             The function could not complete successfully.
+  @retval EFI_NOT_FOUND           No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetLastWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  OUT EFI_GUID                              *CallerId,
+  OUT EFI_LBA                               *Lba,
+  OUT UINTN                                 *Offset,
+  OUT UINTN                                 *Length,
+  IN OUT UINTN                              *PrivateDataSize,
+  OUT VOID                                  *PrivateData,
+  OUT BOOLEAN                               *Complete
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;  
+  SMM_FTW_GET_LAST_WRITE_HEADER             *SmmFtwGetLastWriteHeader;
+
+  //
+  // Initialize the communicate buffer.
+  //
+  PayloadSize  = OFFSET_OF (SMM_FTW_GET_LAST_WRITE_HEADER, Data) + *PrivateDataSize;
+  InitCommunicateBuffer ((VOID **)&SmmCommunicateHeader, (VOID **)&SmmFtwGetLastWriteHeader, PayloadSize, FTW_FUNCTION_GET_LAST_WRITE);
+  SmmFtwGetLastWriteHeader->PrivateDataSize = *PrivateDataSize;
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (SmmCommunicateHeader, PayloadSize);
+
+  //
+  // Get data from SMM
+  //
+  *PrivateDataSize = SmmFtwGetLastWriteHeader->PrivateDataSize;
+  if (Status == EFI_SUCCESS || Status == EFI_BUFFER_TOO_SMALL) {
+    *Lba      = SmmFtwGetLastWriteHeader->Lba;
+    *Offset   = SmmFtwGetLastWriteHeader->Offset; 
+    *Length   = SmmFtwGetLastWriteHeader->Length;
+    *Complete = SmmFtwGetLastWriteHeader->Complete;
+    CopyGuid (CallerId, &SmmFtwGetLastWriteHeader->CallerId);
+    if (Status == EFI_SUCCESS) {
+      CopyMem (PrivateData, SmmFtwGetLastWriteHeader->Data, *PrivateDataSize);
+    }
+  } else if (Status == EFI_NOT_FOUND) {
+    *Complete = SmmFtwGetLastWriteHeader->Complete;
+  }
+
+  FreePool (SmmCommunicateHeader);  
+  return Status;
+}
+
+/**
+  SMM Fault Tolerant Write Protocol notification event handler.
+
+  Install Fault Tolerant Write Protocol.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+**/
+VOID
+EFIAPI
+SmmFtwReady (
+  IN  EFI_EVENT                             Event,
+  IN  VOID                                  *Context
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_FAULT_TOLERANT_WRITE_PROTOCOL         *FtwProtocol;
+
+  //
+  // Just return to avoid install SMM FaultTolerantWriteProtocol again
+  // if Fault Tolerant Write protocol had been installed.
+  //  
+  Status = gBS->LocateProtocol (&gEfiFaultTolerantWriteProtocolGuid, NULL, (VOID **)&FtwProtocol);
+  if (!EFI_ERROR (Status)) {
+    return;
+  }
+  
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &mSmmCommunication);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install protocol interface
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiFaultTolerantWriteProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mFaultTolerantWriteDriver
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  Status = gBS->CloseEvent (Event);
+  ASSERT_EFI_ERROR (Status);  
+}
+
+
+/**
+  The driver entry point for Fault Tolerant Write driver.
+
+  The function does the necessary initialization work.
+
+  @param[in]  ImageHandle       The firmware allocated handle for the UEFI image.
+  @param[in]  SystemTable       A pointer to the EFI system table.
+
+  @retval     EFI_SUCCESS       This funtion always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+FaultTolerantWriteSmmInitialize (
+  IN EFI_HANDLE                             ImageHandle,
+  IN EFI_SYSTEM_TABLE                       *SystemTable
+  )
+{
+  VOID                                      *SmmFtwRegistration;
+
+  //
+  // Smm FTW driver is ready
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEfiSmmFaultTolerantWriteProtocolGuid,
+    TPL_CALLBACK, 
+    SmmFtwReady, 
+    NULL, 
+    &SmmFtwRegistration
+    );
+  
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.h b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.h
new file mode 100644
index 0000000000..b4c20aee0f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.h
@@ -0,0 +1,202 @@
+/** @file
+
+  The internal header file includes the common header files, defines
+  internal structure and functions used by FTW module.
+
+Copyright (c) 2011, 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. 
+
+**/
+
+#ifndef __SMM_FTW_DXE_H__
+#define __SMM_FTW_DXE_H__
+
+#include <PiDxe.h>
+
+#include <Protocol/SmmCommunication.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#include <Guid/EventGroup.h>
+
+#include "FaultTolerantWriteSmmCommon.h"
+
+/**
+  Get the size of the largest block that can be updated in a fault-tolerant manner.
+
+  @param[in]  This             Indicates a pointer to the calling context.
+  @param[out] BlockSize        A pointer to a caller-allocated UINTN that is
+                               updated to indicate the size of the largest block
+                               that can be updated.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetMaxBlockSize (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  OUT UINTN                                 *BlockSize
+  );
+
+
+/**
+  Allocates space for the protocol to maintain information about writes.
+  Since writes must be completed in a fault-tolerant manner and multiple
+  writes require more resources to be successful, this function
+  enables the protocol to ensure that enough space exists to track
+  information about upcoming writes.
+
+  @param[in]  This             A pointer to the calling context.
+  @param[in]  CallerId         The GUID identifying the write.
+  @param[in]  PrivateDataSize  The size of the caller's private data  that must be
+                               recorded for each write.
+  @param[in]  NumberOfWrites   The number of fault tolerant block writes that will
+                               need to occur.
+
+  @retval EFI_SUCCESS          The function completed successfully
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    Not all allocated writes have been completed.  All
+                               writes must be completed or aborted before another
+                               fault tolerant write can occur.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAllocate (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_GUID                               *CallerId,
+  IN UINTN                                  PrivateDataSize,
+  IN UINTN                                  NumberOfWrites
+  );
+
+
+/**
+  Starts a target block update. This records information about the write
+  in fault tolerant storage, and will complete the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param[in]  This             The calling context.
+  @param[in]  Lba              The logical block address of the target block.
+  @param[in]  Offset           The offset within the target block to place the
+                               data.
+  @param[in]  Length           The number of bytes to write to the target block.
+  @param[in]  PrivateData      A pointer to private data that the caller requires
+                               to complete any pending writes in the event of a
+                               fault.
+  @param[in]  FvBlockHandle    The handle of FVB protocol that provides services
+                               for reading, writing, and erasing the target block.
+  @param[in]  Buffer           The data to write.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_BAD_BUFFER_SIZE  The write would span a block boundary, which is not
+                               a valid action.
+  @retval EFI_ACCESS_DENIED    No writes have been allocated.
+  @retval EFI_NOT_READY        The last write has not been completed. Restart()
+                               must be called to complete it.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_LBA                                Lba,
+  IN UINTN                                  Offset,
+  IN UINTN                                  Length,
+  IN VOID                                   *PrivateData,
+  IN EFI_HANDLE                             FvBlockHandle,
+  IN VOID                                   *Buffer
+  );
+
+
+/**
+  Restarts a previously interrupted write. The caller must provide the
+  block protocol needed to complete the interrupted write.
+
+  @param[in]  This             The calling context.
+  @param[in]  FvBlockHandle    The handle of FVB protocol that provides services.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_ACCESS_DENIED    No pending writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwRestart (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  IN EFI_HANDLE                             FvBlockHandle
+  );
+
+
+/**
+  Aborts all previously allocated writes.
+
+  @param  This                 The calling context.
+
+  @retval EFI_SUCCESS          The function completed successfully.
+  @retval EFI_ABORTED          The function could not complete successfully.
+  @retval EFI_NOT_FOUND        No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwAbort (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This
+  );
+
+
+/**
+  Starts a target block update. This function records information about the write
+  in fault-tolerant storage and completes the write in a recoverable
+  manner, ensuring at all times that either the original contents or
+  the modified contents are available.
+
+  @param[in]      This            Indicates a pointer to the calling context.
+  @param[out]     CallerId        The GUID identifying the last write.
+  @param[out]     Lba             The logical block address of the last write.
+  @param[out]     Offset          The offset within the block of the last write.
+  @param[out]     Length          The length of the last write.
+  @param[in, out] PrivateDataSize On input, the size of the PrivateData buffer. On
+                                  output, the size of the private data stored for
+                                  this write.
+  @param[out]     PrivateData     A pointer to a buffer. The function will copy
+                                  PrivateDataSize bytes from the private data stored
+                                  for this write.
+  @param[out]     Complete        A Boolean value with TRUE indicating that the write
+                                  was completed.
+
+  @retval EFI_SUCCESS             The function completed successfully.
+  @retval EFI_ABORTED             The function could not complete successfully.
+  @retval EFI_NOT_FOUND           No allocated writes exist.
+
+**/
+EFI_STATUS
+EFIAPI
+FtwGetLastWrite (
+  IN EFI_FAULT_TOLERANT_WRITE_PROTOCOL      *This,
+  OUT EFI_GUID                              *CallerId,
+  OUT EFI_LBA                               *Lba,
+  OUT UINTN                                 *Offset,
+  OUT UINTN                                 *Length,
+  IN OUT UINTN                              *PrivateDataSize,
+  OUT VOID                                  *PrivateData,
+  OUT BOOLEAN                               *Complete
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.inf b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.inf
new file mode 100644
index 0000000000..82dd36c320
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.inf
@@ -0,0 +1,64 @@
+## @file
+# This module is the Runtime DXE part corresponding to SMM Fault Tolerant Write (FTW) module.
+#
+# It installs FTW protocol and works with SMM FTW module together.
+# The FTW protocol will not work after End Of Dxe because it will be not safe to expose
+# the related operations in SMM handler in SMM FTW module. You can use the FTW protocol
+# before End Of Dxe or use FaultTolerantWriteDxe module instead if you really want to.
+#
+# Copyright (c) 2011 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FaultTolerantWriteSmmDxe
+  MODULE_UNI_FILE                = FaultTolerantWriteSmmDxe.uni
+  FILE_GUID                      = 98948C4A-70F2-4035-8E9F-5927493CFC07
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = FaultTolerantWriteSmmInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  FaultTolerantWriteSmmDxe.c
+  FaultTolerantWriteSmmDxe.h
+  FaultTolerantWriteSmmCommon.h
+  
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib  
+  UefiBootServicesTableLib
+  DebugLib
+  DxeServicesTableLib
+  UefiDriverEntryPoint
+
+[Protocols]
+  gEfiFaultTolerantWriteProtocolGuid            ## PRODUCES
+  gEfiSmmCommunicationProtocolGuid              ## CONSUMES
+  ## NOTIFY
+  ## UNDEFINED # Used to do smm communication
+  ## CONSUMES
+  gEfiSmmFaultTolerantWriteProtocolGuid
+  gEfiFirmwareVolumeBlockProtocolGuid           ## CONSUMES
+   
+[Depex]
+  gEfiSmmCommunicationProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FaultTolerantWriteSmmDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.uni
new file mode 100644
index 0000000000..14ecfa127e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxe.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxeExtra.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxeExtra.uni
new file mode 100644
index 0000000000..6b0a8f99fd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FaultTolerantWriteSmmDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FtwMisc.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FtwMisc.c
new file mode 100644
index 0000000000..09223217e4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/FtwMisc.c
@@ -0,0 +1,1382 @@
+/** @file
+
+  Internal generic functions to operate flash block.
+
+Copyright (c) 2006 - 2014, 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 "FaultTolerantWrite.h"
+
+/**
+
+  Check whether a flash buffer is erased.
+
+  @param Buffer          Buffer to check
+  @param BufferSize      Size of the buffer
+
+  @return A BOOLEAN value indicating erased or not.
+
+**/
+BOOLEAN
+IsErasedFlashBuffer (
+  IN UINT8           *Buffer,
+  IN UINTN           BufferSize
+  )
+{
+  BOOLEAN IsEmpty;
+  UINT8   *Ptr;
+  UINTN   Index;
+
+  Ptr     = Buffer;
+  IsEmpty = TRUE;
+  for (Index = 0; Index < BufferSize; Index += 1) {
+    if (*Ptr++ != FTW_ERASED_BYTE) {
+      IsEmpty = FALSE;
+      break;
+    }
+  }
+
+  return IsEmpty;
+}
+
+/**
+  To erase the block with specified blocks.
+
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         FVB Protocol interface
+  @param Lba             Lba of the firmware block
+  @param NumberOfBlocks  The number of consecutive blocks starting with Lba
+
+  @retval  EFI_SUCCESS    Block LBA is Erased successfully
+  @retval  Others         Error occurs
+
+**/
+EFI_STATUS
+FtwEraseBlock (
+  IN EFI_FTW_DEVICE                   *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  EFI_LBA                             Lba,
+  UINTN                               NumberOfBlocks
+  )
+{
+  return FvBlock->EraseBlocks (
+                    FvBlock,
+                    Lba,
+                    NumberOfBlocks,
+                    EFI_LBA_LIST_TERMINATOR
+                    );
+}
+
+/**
+  Erase spare block.
+
+  @param FtwDevice        The private data of FTW driver
+
+  @retval EFI_SUCCESS           The erase request was successfully completed.
+  @retval EFI_ACCESS_DENIED     The firmware volume is in the WriteDisabled state.
+  @retval EFI_DEVICE_ERROR      The block device is not functioning
+                                correctly and could not be written.
+                                The firmware device may have been
+                                partially erased.
+  @retval EFI_INVALID_PARAMETER One or more of the LBAs listed
+                                in the variable argument list do
+                                not exist in the firmware volume.  
+
+
+**/
+EFI_STATUS
+FtwEraseSpareBlock (
+  IN EFI_FTW_DEVICE   *FtwDevice
+  )
+{
+  return FtwDevice->FtwBackupFvb->EraseBlocks (
+                                    FtwDevice->FtwBackupFvb,
+                                    FtwDevice->FtwSpareLba,
+                                    FtwDevice->NumberOfSpareBlock,
+                                    EFI_LBA_LIST_TERMINATOR
+                                    );
+}
+
+/**
+
+  Is it in working block?
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         Fvb protocol instance
+  @param Lba             The block specified
+
+  @return A BOOLEAN value indicating in working block or not.
+
+**/
+BOOLEAN
+IsWorkingBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  EFI_LBA                             Lba
+  )
+{
+  //
+  // If matching the following condition, the target block is in working block.
+  // 1. Target block is on the FV of working block (Using the same FVB protocol instance).
+  // 2. Lba falls into the range of working block.
+  //
+  return (BOOLEAN)
+    (
+      (FvBlock == FtwDevice->FtwFvBlock) &&
+      (Lba >= FtwDevice->FtwWorkBlockLba) &&
+      (Lba <= FtwDevice->FtwWorkSpaceLba)
+    );
+}
+
+/**
+
+  Get firmware volume block by address.
+
+
+  @param Address         Address specified the block
+  @param FvBlock         The block caller wanted
+
+  @retval  EFI_SUCCESS    The protocol instance if found.
+  @retval  EFI_NOT_FOUND  Block not found
+
+**/
+EFI_HANDLE
+GetFvbByAddress (
+  IN  EFI_PHYSICAL_ADDRESS               Address,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL **FvBlock
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HANDLE                          *HandleBuffer;
+  UINTN                               HandleCount;
+  UINTN                               Index;
+  EFI_PHYSICAL_ADDRESS                FvbBaseAddress;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  EFI_HANDLE                          FvbHandle;
+  UINTN                               BlockSize;
+  UINTN                               NumberOfBlocks;
+
+  *FvBlock  = NULL;
+  FvbHandle = NULL;
+  HandleBuffer = NULL;
+  //
+  // Locate all handles of Fvb protocol
+  //
+  Status = GetFvbCountAndBuffer (&HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  //
+  // Get the FVB to access variable store
+  //
+  for (Index = 0; Index < HandleCount; Index += 1) {
+    Status = FtwGetFvbByHandle (HandleBuffer[Index], &Fvb);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    //
+    // Compare the address and select the right one
+    //
+    Status = Fvb->GetPhysicalAddress (Fvb, &FvbBaseAddress);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Now, one FVB has one type of BlockSize
+    //
+    Status = Fvb->GetBlockSize (Fvb, 0, &BlockSize, &NumberOfBlocks);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if ((Address >= FvbBaseAddress) && (Address < (FvbBaseAddress + BlockSize * NumberOfBlocks))) {
+      *FvBlock  = Fvb;
+      FvbHandle  = HandleBuffer[Index];
+      break;
+    }
+  }
+
+  FreePool (HandleBuffer);
+  return FvbHandle;
+}
+
+/**
+
+  Is it in boot block?
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         Fvb protocol instance
+
+  @return A BOOLEAN value indicating in boot block or not.
+
+**/
+BOOLEAN
+IsBootBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_SWAP_ADDRESS_RANGE_PROTOCOL     *SarProtocol;
+  EFI_PHYSICAL_ADDRESS                BootBlockBase;
+  UINTN                               BootBlockSize;
+  EFI_PHYSICAL_ADDRESS                BackupBlockBase;
+  UINTN                               BackupBlockSize;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *BootFvb;
+  BOOLEAN                             IsSwapped;
+  EFI_HANDLE                          FvbHandle;
+
+  if (!FeaturePcdGet(PcdFullFtwServiceEnable)) {
+    return FALSE;
+  }
+
+  Status = FtwGetSarProtocol ((VOID **) &SarProtocol);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Get the boot block range
+  //
+  Status = SarProtocol->GetRangeLocation (
+                          SarProtocol,
+                          &BootBlockBase,
+                          &BootBlockSize,
+                          &BackupBlockBase,
+                          &BackupBlockSize
+                          );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  Status = SarProtocol->GetSwapState (SarProtocol, &IsSwapped);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  //
+  // Get FVB by address
+  //
+  if (!IsSwapped) {
+    FvbHandle = GetFvbByAddress (BootBlockBase, &BootFvb);
+  } else {
+    FvbHandle = GetFvbByAddress (BackupBlockBase, &BootFvb);
+  }
+
+  if (FvbHandle == NULL) {
+    return FALSE;
+  }
+  //
+  // Compare the Fvb
+  //
+  return (BOOLEAN) (FvBlock == BootFvb);
+}
+
+/**
+  Copy the content of spare block to a boot block. Size is FTW_BLOCK_SIZE.
+  Spare block is accessed by FTW working FVB protocol interface.
+  Target block is accessed by FvBlock protocol interface.
+
+  FTW will do extra work on boot block update.
+  FTW should depend on a protocol of EFI_ADDRESS_RANGE_SWAP_PROTOCOL,
+  which is produced by a chipset driver.
+  FTW updating boot block steps may be:
+  1. GetRangeLocation(), if the Range is inside the boot block, FTW know
+  that boot block will be update. It shall add a FLAG in the working block.
+  2. When spare block is ready,
+  3. SetSwapState(SWAPPED)
+  4. erasing boot block,
+  5. programming boot block until the boot block is ok.
+  6. SetSwapState(UNSWAPPED)
+  FTW shall not allow to update boot block when battery state is error.
+
+  @param FtwDevice       The private data of FTW driver
+
+  @retval EFI_SUCCESS             Spare block content is copied to boot block
+  @retval EFI_INVALID_PARAMETER   Input parameter error
+  @retval EFI_OUT_OF_RESOURCES    Allocate memory error
+  @retval EFI_ABORTED             The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToBootBlock (
+  EFI_FTW_DEVICE                      *FtwDevice
+  )
+{
+  EFI_STATUS                          Status;
+  UINTN                               Length;
+  UINT8                               *Buffer;
+  UINTN                               Count;
+  UINT8                               *Ptr;
+  UINTN                               Index;
+  BOOLEAN                             TopSwap;
+  EFI_SWAP_ADDRESS_RANGE_PROTOCOL     *SarProtocol;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *BootFvb;
+  EFI_LBA                             BootLba;
+
+  if (!FeaturePcdGet(PcdFullFtwServiceEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Locate swap address range protocol
+  //
+  Status = FtwGetSarProtocol ((VOID **) &SarProtocol);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Allocate a memory buffer
+  //
+  Length = FtwDevice->SpareAreaLength;
+  Buffer  = AllocatePool (Length);
+  if (Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Get TopSwap bit state
+  //
+  Status = SarProtocol->GetSwapState (SarProtocol, &TopSwap);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Get Top Swapped status - %r\n", Status));
+    FreePool (Buffer);
+    return EFI_ABORTED;
+  }
+
+  if (TopSwap) {
+    //
+    // Get FVB of current boot block
+    //
+    if (GetFvbByAddress (FtwDevice->SpareAreaAddress + FtwDevice->SpareAreaLength, &BootFvb) == NULL) {
+      FreePool (Buffer);
+      return EFI_ABORTED;
+    }
+    //
+    // Read data from current boot block
+    //
+    BootLba = 0;
+    Ptr     = Buffer;
+    for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+      Count = FtwDevice->SpareBlockSize;
+      Status = BootFvb->Read (
+                          BootFvb,
+                          BootLba + Index,
+                          0,
+                          &Count,
+                          Ptr
+                          );
+      if (EFI_ERROR (Status)) {
+        FreePool (Buffer);
+        return Status;
+      }
+
+      Ptr += Count;
+    }
+  } else {
+    //
+    // Read data from spare block
+    //
+    Ptr = Buffer;
+    for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+      Count = FtwDevice->SpareBlockSize;
+      Status = FtwDevice->FtwBackupFvb->Read (
+                                          FtwDevice->FtwBackupFvb,
+                                          FtwDevice->FtwSpareLba + Index,
+                                          0,
+                                          &Count,
+                                          Ptr
+                                          );
+      if (EFI_ERROR (Status)) {
+        FreePool (Buffer);
+        return Status;
+      }
+
+      Ptr += Count;
+    }
+    //
+    // Set TopSwap bit
+    //
+    Status = SarProtocol->SetSwapState (SarProtocol, TRUE);
+    if (EFI_ERROR (Status)) {
+      FreePool (Buffer);
+      return Status;
+    }
+  }
+  //
+  // Erase current spare block
+  // Because TopSwap is set, this actually erase the top block (boot block)!
+  //
+  Status = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (Buffer);
+    return EFI_ABORTED;
+  }
+  //
+  // Write memory buffer to current spare block. Still top block.
+  //
+  Ptr = Buffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    Count = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Write (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &Count,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ftw: FVB Write boot block - %r\n", Status));
+      FreePool (Buffer);
+      return Status;
+    }
+
+    Ptr += Count;
+  }
+
+  FreePool (Buffer);
+
+  //
+  // Clear TopSwap bit
+  //
+  Status = SarProtocol->SetSwapState (SarProtocol, FALSE);
+
+  return Status;
+}
+
+/**
+  Copy the content of spare block to a target block.
+  Spare block is accessed by FTW backup FVB protocol interface.
+  Target block is accessed by FvBlock protocol interface.
+
+
+  @param FtwDevice       The private data of FTW driver
+  @param FvBlock         FVB Protocol interface to access target block
+  @param Lba             Lba of the target block
+  @param BlockSize       The size of the block
+  @param NumberOfBlocks  The number of consecutive blocks starting with Lba
+
+  @retval  EFI_SUCCESS               Spare block content is copied to target block
+  @retval  EFI_INVALID_PARAMETER     Input parameter error
+  @retval  EFI_OUT_OF_RESOURCES      Allocate memory error
+  @retval  EFI_ABORTED               The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToTargetBlock (
+  EFI_FTW_DEVICE                      *FtwDevice,
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  EFI_LBA                             Lba,
+  UINTN                               BlockSize,
+  UINTN                               NumberOfBlocks
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Length;
+  UINT8       *Buffer;
+  UINTN       Count;
+  UINT8       *Ptr;
+  UINTN       Index;
+
+  if ((FtwDevice == NULL) || (FvBlock == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Allocate a memory buffer
+  //
+  Length = FtwDevice->SpareAreaLength;
+  Buffer  = AllocatePool (Length);
+  if (Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Read all content of spare block to memory buffer
+  //
+  Ptr = Buffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    Count = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Read (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &Count,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (Buffer);
+      return Status;
+    }
+
+    Ptr += Count;
+  }
+  //
+  // Erase the target block
+  //
+  Status = FtwEraseBlock (FtwDevice, FvBlock, Lba, NumberOfBlocks);
+  if (EFI_ERROR (Status)) {
+    FreePool (Buffer);
+    return EFI_ABORTED;
+  }
+  //
+  // Write memory buffer to block, using the FvBlock protocol interface
+  //
+  Ptr = Buffer;
+  for (Index = 0; Index < NumberOfBlocks; Index += 1) {
+    Count   = BlockSize;
+    Status  = FvBlock->Write (FvBlock, Lba + Index, 0, &Count, Ptr);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ftw: FVB Write block - %r\n", Status));
+      FreePool (Buffer);
+      return Status;
+    }
+
+    Ptr += Count;
+  }
+
+  FreePool (Buffer);
+
+  return Status;
+}
+
+/**
+  Copy the content of spare block to working block. Size is FTW_BLOCK_SIZE.
+  Spare block is accessed by FTW backup FVB protocol interface. LBA is
+  FtwDevice->FtwSpareLba.
+  Working block is accessed by FTW working FVB protocol interface. LBA is
+  FtwDevice->FtwWorkBlockLba.
+
+  Since the working block header is important when FTW initializes, the
+  state of the operation should be handled carefully. The Crc value is
+  calculated without STATE element.
+
+  @param FtwDevice       The private data of FTW driver
+
+  @retval  EFI_SUCCESS               Spare block content is copied to target block
+  @retval  EFI_OUT_OF_RESOURCES      Allocate memory error
+  @retval  EFI_ABORTED               The function could not complete successfully
+
+**/
+EFI_STATUS
+FlushSpareBlockToWorkingBlock (
+  EFI_FTW_DEVICE                      *FtwDevice
+  )
+{
+  EFI_STATUS                              Status;
+  UINTN                                   Length;
+  UINT8                                   *Buffer;
+  EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingBlockHeader;
+  UINTN                                   Count;
+  UINT8                                   *Ptr;
+  UINTN                                   Index;
+
+  //
+  // Allocate a memory buffer
+  //
+  Length = FtwDevice->SpareAreaLength;
+  Buffer  = AllocatePool (Length);
+  if (Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // To guarantee that the WorkingBlockValid is set on spare block
+  //
+  //  Offset = OFFSET_OF(EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER,
+  //                            WorkingBlockValid);
+  // To skip Signature and Crc: sizeof(EFI_GUID)+sizeof(UINT32).
+  //
+  FtwUpdateFvState (
+    FtwDevice->FtwBackupFvb,
+    FtwDevice->SpareBlockSize,
+    FtwDevice->FtwSpareLba + FtwDevice->FtwWorkSpaceLbaInSpare,
+    FtwDevice->FtwWorkSpaceBaseInSpare + sizeof (EFI_GUID) + sizeof (UINT32),
+    WORKING_BLOCK_VALID
+    );
+  //
+  // Read from spare block to memory buffer
+  //
+  Ptr = Buffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    Count = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Read (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &Count,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (Buffer);
+      return Status;
+    }
+
+    Ptr += Count;
+  }
+  //
+  // Clear the CRC and STATE, copy data from spare to working block.
+  //
+  WorkingBlockHeader = (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *) (Buffer + (UINTN) FtwDevice->FtwWorkSpaceLbaInSpare * FtwDevice->SpareBlockSize + FtwDevice->FtwWorkSpaceBaseInSpare);
+  InitWorkSpaceHeader (WorkingBlockHeader);
+  WorkingBlockHeader->WorkingBlockValid   = FTW_ERASE_POLARITY;
+  WorkingBlockHeader->WorkingBlockInvalid = FTW_ERASE_POLARITY;
+
+  //
+  // target block is working block, then
+  //   Set WorkingBlockInvalid in EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER
+  //   before erase the working block.
+  //
+  //  Offset = OFFSET_OF(EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER,
+  //                            WorkingBlockInvalid);
+  // So hardcode offset as sizeof(EFI_GUID)+sizeof(UINT32) to
+  // skip Signature and Crc.
+  //
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + sizeof (EFI_GUID) + sizeof (UINT32),
+            WORKING_BLOCK_INVALID
+            );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buffer);
+    return EFI_ABORTED;
+  }
+
+  FtwDevice->FtwWorkSpaceHeader->WorkingBlockInvalid = FTW_VALID_STATE;
+
+  //
+  // Erase the working block
+  //
+  Status = FtwEraseBlock (FtwDevice, FtwDevice->FtwFvBlock, FtwDevice->FtwWorkBlockLba, FtwDevice->NumberOfWorkBlock);
+  if (EFI_ERROR (Status)) {
+    FreePool (Buffer);
+    return EFI_ABORTED;
+  }
+  //
+  // Write memory buffer to working block, using the FvBlock protocol interface
+  //
+  Ptr = Buffer;
+  for (Index = 0; Index < FtwDevice->NumberOfWorkBlock; Index += 1) {
+    Count = FtwDevice->WorkBlockSize;
+    Status = FtwDevice->FtwFvBlock->Write (
+                                      FtwDevice->FtwFvBlock,
+                                      FtwDevice->FtwWorkBlockLba + Index,
+                                      0,
+                                      &Count,
+                                      Ptr
+                                      );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ftw: FVB Write block - %r\n", Status));
+      FreePool (Buffer);
+      return Status;
+    }
+
+    Ptr += Count;
+  }
+  //
+  // Since the memory buffer will not be used, free memory Buffer.
+  //
+  FreePool (Buffer);
+
+  //
+  // Update the VALID of the working block
+  //
+  // Offset = OFFSET_OF(EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER, WorkingBlockValid);
+  // So hardcode offset as sizeof(EFI_GUID)+sizeof(UINT32) to skip Signature and Crc.
+  //
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + sizeof (EFI_GUID) + sizeof (UINT32),
+            WORKING_BLOCK_VALID
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  FtwDevice->FtwWorkSpaceHeader->WorkingBlockInvalid = FTW_INVALID_STATE;
+  FtwDevice->FtwWorkSpaceHeader->WorkingBlockValid = FTW_VALID_STATE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update a bit of state on a block device. The location of the bit is
+  calculated by the (Lba, Offset, bit). Here bit is determined by the
+  the name of a certain bit.
+
+
+  @param FvBlock         FVB Protocol interface to access SrcBlock and DestBlock
+  @param BlockSize       The size of the block
+  @param Lba             Lba of a block
+  @param Offset          Offset on the Lba
+  @param NewBit          New value that will override the old value if it can be change
+
+  @retval  EFI_SUCCESS    A state bit has been updated successfully
+  @retval  Others         Access block device error.
+                          Notes:
+                          Assume all bits of State are inside the same BYTE.
+  @retval  EFI_ABORTED    Read block fail
+
+**/
+EFI_STATUS
+FtwUpdateFvState (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvBlock,
+  IN UINTN                               BlockSize,
+  IN EFI_LBA                             Lba,
+  IN UINTN                               Offset,
+  IN UINT8                               NewBit
+  )
+{
+  EFI_STATUS  Status;
+  UINT8       State;
+  UINTN       Length;
+
+  //
+  // Calculate the real Offset and Lba to write.
+  //
+  while (Offset >= BlockSize) {
+    Offset -= BlockSize;
+    Lba++;
+  }
+
+  //
+  // Read state from device, assume State is only one byte.
+  //
+  Length  = sizeof (UINT8);
+  Status  = FvBlock->Read (FvBlock, Lba, Offset, &Length, &State);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  State ^= FTW_POLARITY_REVERT;
+  State  = (UINT8) (State | NewBit);
+  State ^= FTW_POLARITY_REVERT;
+
+  //
+  // Write state back to device
+  //
+  Length  = sizeof (UINT8);
+  Status  = FvBlock->Write (FvBlock, Lba, Offset, &Length, &State);
+
+  return Status;
+}
+
+/**
+  Get the last Write Header pointer.
+  The last write header is the header whose 'complete' state hasn't been set.
+  After all, this header may be a EMPTY header entry for next Allocate.
+
+
+  @param FtwWorkSpaceHeader Pointer of the working block header
+  @param FtwWorkSpaceSize   Size of the work space
+  @param FtwWriteHeader     Pointer to retrieve the last write header
+
+  @retval  EFI_SUCCESS      Get the last write record successfully
+  @retval  EFI_ABORTED      The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteHeader (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER  *FtwWorkSpaceHeader,
+  IN UINTN                                    FtwWorkSpaceSize,
+  OUT EFI_FAULT_TOLERANT_WRITE_HEADER         **FtwWriteHeader
+  )
+{
+  UINTN                           Offset;
+  EFI_FAULT_TOLERANT_WRITE_HEADER *FtwHeader;
+
+  *FtwWriteHeader = NULL;
+  FtwHeader       = (EFI_FAULT_TOLERANT_WRITE_HEADER *) (FtwWorkSpaceHeader + 1);
+  Offset          = sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER);
+
+  while (FtwHeader->Complete == FTW_VALID_STATE) {
+    Offset += FTW_WRITE_TOTAL_SIZE (FtwHeader->NumberOfWrites, FtwHeader->PrivateDataSize);
+    //
+    // If Offset exceed the FTW work space boudary, return error.
+    //
+    if (Offset >= FtwWorkSpaceSize) {
+      *FtwWriteHeader = FtwHeader;
+      return EFI_ABORTED;
+    }
+
+    FtwHeader = (EFI_FAULT_TOLERANT_WRITE_HEADER *) ((UINT8 *) FtwWorkSpaceHeader + Offset);
+  }
+  //
+  // Last write header is found
+  //
+  *FtwWriteHeader = FtwHeader;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the last Write Record pointer. The last write Record is the Record
+  whose DestinationCompleted state hasn't been set. After all, this Record
+  may be a EMPTY record entry for next write.
+
+
+  @param FtwWriteHeader  Pointer to the write record header
+  @param FtwWriteRecord  Pointer to retrieve the last write record
+
+  @retval EFI_SUCCESS        Get the last write record successfully
+  @retval EFI_ABORTED        The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteRecord (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER          *FtwWriteHeader,
+  OUT EFI_FAULT_TOLERANT_WRITE_RECORD         **FtwWriteRecord
+  )
+{
+  UINTN                           Index;
+  EFI_FAULT_TOLERANT_WRITE_RECORD *FtwRecord;
+
+  *FtwWriteRecord = NULL;
+  FtwRecord       = (EFI_FAULT_TOLERANT_WRITE_RECORD *) (FtwWriteHeader + 1);
+
+  //
+  // Try to find the last write record "that has not completed"
+  //
+  for (Index = 0; Index < FtwWriteHeader->NumberOfWrites; Index += 1) {
+    if (FtwRecord->DestinationComplete != FTW_VALID_STATE) {
+      //
+      // The last write record is found
+      //
+      *FtwWriteRecord = FtwRecord;
+      return EFI_SUCCESS;
+    }
+
+    FtwRecord++;
+
+    if (FtwWriteHeader->PrivateDataSize != 0) {
+      FtwRecord = (EFI_FAULT_TOLERANT_WRITE_RECORD *) ((UINTN) FtwRecord + (UINTN) FtwWriteHeader->PrivateDataSize);
+    }
+  }
+  //
+  //  if Index == NumberOfWrites, then
+  //  the last record has been written successfully,
+  //  but the Header->Complete Flag has not been set.
+  //  also return the last record.
+  //
+  if (Index == FtwWriteHeader->NumberOfWrites) {
+    *FtwWriteRecord = (EFI_FAULT_TOLERANT_WRITE_RECORD *) ((UINTN) FtwRecord - FTW_RECORD_SIZE (FtwWriteHeader->PrivateDataSize));
+    return EFI_SUCCESS;
+  }
+
+  return EFI_ABORTED;
+}
+
+/**
+  To check if FtwRecord is the first record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to the write record
+
+  @retval TRUE      FtwRecord is the first Record of the FtwHeader
+  @retval FALSE     FtwRecord is not the first Record of the FtwHeader
+
+**/
+BOOLEAN
+IsFirstRecordOfWrites (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN EFI_FAULT_TOLERANT_WRITE_RECORD    *FtwRecord
+  )
+{
+  UINT8 *Head;
+  UINT8 *Ptr;
+
+  Head  = (UINT8 *) FtwHeader;
+  Ptr   = (UINT8 *) FtwRecord;
+
+  Head += sizeof (EFI_FAULT_TOLERANT_WRITE_HEADER);
+  return (BOOLEAN) (Head == Ptr);
+}
+
+/**
+  To check if FtwRecord is the last record of FtwHeader. Because the
+  FtwHeader has NumberOfWrites & PrivateDataSize, the FtwRecord can be
+  determined if it is the last record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to the write record
+
+  @retval TRUE      FtwRecord is the last Record of the FtwHeader
+  @retval FALSE     FtwRecord is not the last Record of the FtwHeader
+
+**/
+BOOLEAN
+IsLastRecordOfWrites (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN EFI_FAULT_TOLERANT_WRITE_RECORD    *FtwRecord
+  )
+{
+  UINT8 *Head;
+  UINT8 *Ptr;
+
+  Head  = (UINT8 *) FtwHeader;
+  Ptr   = (UINT8 *) FtwRecord;
+
+  Head += FTW_WRITE_TOTAL_SIZE (FtwHeader->NumberOfWrites - 1, FtwHeader->PrivateDataSize);
+  return (BOOLEAN) (Head == Ptr);
+}
+
+/**
+  To check if FtwRecord is the first record of FtwHeader.
+
+  @param FtwHeader  Pointer to the write record header
+  @param FtwRecord  Pointer to retrieve the previous write record
+
+  @retval EFI_ACCESS_DENIED  Input record is the first record, no previous record is return.
+  @retval EFI_SUCCESS        The previous write record is found.
+
+**/
+EFI_STATUS
+GetPreviousRecordOfWrites (
+  IN     EFI_FAULT_TOLERANT_WRITE_HEADER    *FtwHeader,
+  IN OUT EFI_FAULT_TOLERANT_WRITE_RECORD    **FtwRecord
+  )
+{
+  UINT8 *Ptr;
+
+  if (IsFirstRecordOfWrites (FtwHeader, *FtwRecord)) {
+    *FtwRecord = NULL;
+    return EFI_ACCESS_DENIED;
+  }
+
+  Ptr = (UINT8 *) (*FtwRecord);
+  Ptr -= FTW_RECORD_SIZE (FtwHeader->PrivateDataSize);
+  *FtwRecord = (EFI_FAULT_TOLERANT_WRITE_RECORD *) Ptr;
+  return EFI_SUCCESS;
+}
+
+/**
+  Allocate private data for FTW driver and initialize it.
+
+  @param[out] FtwData           Pointer to the FTW device structure
+
+  @retval EFI_SUCCESS           Initialize the FTW device successfully.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
+  @retval EFI_INVALID_PARAMETER Workspace or Spare block does not exist
+
+**/
+EFI_STATUS
+InitFtwDevice (
+  OUT EFI_FTW_DEVICE               **FtwData 
+  )
+{
+  EFI_FTW_DEVICE                   *FtwDevice;
+  
+  //
+  // Allocate private data of this driver,
+  // Including the FtwWorkSpace[FTW_WORK_SPACE_SIZE].
+  //
+  FtwDevice = AllocateZeroPool (sizeof (EFI_FTW_DEVICE) + PcdGet32 (PcdFlashNvStorageFtwWorkingSize));
+  if (FtwDevice == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize other parameters, and set WorkSpace as FTW_ERASED_BYTE.
+  //
+  FtwDevice->WorkSpaceLength  = (UINTN) PcdGet32 (PcdFlashNvStorageFtwWorkingSize);
+  FtwDevice->SpareAreaLength  = (UINTN) PcdGet32 (PcdFlashNvStorageFtwSpareSize);
+  if ((FtwDevice->WorkSpaceLength == 0) || (FtwDevice->SpareAreaLength == 0)) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Workspace or Spare block does not exist!\n"));
+    FreePool (FtwDevice);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FtwDevice->Signature        = FTW_DEVICE_SIGNATURE;
+  FtwDevice->FtwFvBlock       = NULL;
+  FtwDevice->FtwBackupFvb     = NULL;
+  FtwDevice->FtwWorkSpaceLba  = (EFI_LBA) (-1);
+  FtwDevice->FtwSpareLba      = (EFI_LBA) (-1);
+
+  FtwDevice->WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwWorkingBase64);
+  if (FtwDevice->WorkSpaceAddress == 0) {
+    FtwDevice->WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwWorkingBase);
+  }
+  
+  FtwDevice->SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwSpareBase64);
+  if (FtwDevice->SpareAreaAddress == 0) {
+    FtwDevice->SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwSpareBase);
+  }  
+
+  *FtwData = FtwDevice;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Find the proper Firmware Volume Block protocol for FTW operation.
+
+  @param[in, out] FtwDevice     Pointer to the FTW device structure
+
+  @retval EFI_SUCCESS           Find the FVB protocol successfully.
+  @retval EFI_NOT_FOUND         No proper FVB protocol was found.
+  @retval EFI_ABORTED           Some data can not be got or be invalid.
+  
+**/
+EFI_STATUS
+FindFvbForFtw (
+  IN OUT EFI_FTW_DEVICE               *FtwDevice
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HANDLE                          *HandleBuffer;
+  UINTN                               HandleCount;
+  UINTN                               Index;
+  EFI_PHYSICAL_ADDRESS                FvbBaseAddress;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  EFI_FVB_ATTRIBUTES_2                Attributes;
+  UINT32                              LbaIndex;
+  UINTN                               BlockSize;
+  UINTN                               NumberOfBlocks;
+
+  HandleBuffer = NULL;
+
+  //
+  // Get all FVB handle.
+  //
+  Status = GetFvbCountAndBuffer (&HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Get the FVB to access variable store
+  //
+  Fvb = NULL;
+  for (Index = 0; Index < HandleCount; Index += 1) {
+    Status = FtwGetFvbByHandle (HandleBuffer[Index], &Fvb);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_NOT_FOUND;
+      break;
+    }
+
+    //
+    // Ensure this FVB protocol support Write operation.
+    //
+    Status = Fvb->GetAttributes (Fvb, &Attributes);
+    if (EFI_ERROR (Status) || ((Attributes & EFI_FVB2_WRITE_STATUS) == 0)) {
+      continue;     
+    }
+    //
+    // Compare the address and select the right one
+    //
+    Status = Fvb->GetPhysicalAddress (Fvb, &FvbBaseAddress);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Now, one FVB has one type of BlockSize.
+    //
+    Status = Fvb->GetBlockSize (Fvb, 0, &BlockSize, &NumberOfBlocks);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if ((FtwDevice->FtwFvBlock == NULL) && (FtwDevice->WorkSpaceAddress >= FvbBaseAddress) &&
+        ((FtwDevice->WorkSpaceAddress + FtwDevice->WorkSpaceLength) <= (FvbBaseAddress + BlockSize * NumberOfBlocks))) {
+      FtwDevice->FtwFvBlock = Fvb;
+      //
+      // To get the LBA of work space
+      //
+      for (LbaIndex = 1; LbaIndex <= NumberOfBlocks; LbaIndex += 1) {
+        if ((FtwDevice->WorkSpaceAddress >= (FvbBaseAddress + BlockSize * (LbaIndex - 1)))
+            && (FtwDevice->WorkSpaceAddress < (FvbBaseAddress + BlockSize * LbaIndex))) {
+          FtwDevice->FtwWorkSpaceLba = LbaIndex - 1;
+          //
+          // Get the Work space size and Base(Offset)
+          //
+          FtwDevice->FtwWorkSpaceSize = FtwDevice->WorkSpaceLength;
+          FtwDevice->WorkBlockSize    = BlockSize;
+          FtwDevice->FtwWorkSpaceBase = (UINTN) (FtwDevice->WorkSpaceAddress - (FvbBaseAddress + FtwDevice->WorkBlockSize * (LbaIndex - 1)));
+          FtwDevice->NumberOfWorkSpaceBlock = FTW_BLOCKS (FtwDevice->FtwWorkSpaceBase + FtwDevice->FtwWorkSpaceSize, FtwDevice->WorkBlockSize);
+          if (FtwDevice->FtwWorkSpaceSize >= FtwDevice->WorkBlockSize) {
+            //
+            // Check the alignment of work space address and length, they should be block size aligned when work space size is larger than one block size.
+            //
+            if (((FtwDevice->WorkSpaceAddress & (FtwDevice->WorkBlockSize - 1)) != 0) ||
+                ((FtwDevice->WorkSpaceLength & (FtwDevice->WorkBlockSize - 1)) != 0)) {
+              DEBUG ((EFI_D_ERROR, "Ftw: Work space address or length is not block size aligned when work space size is larger than one block size\n"));
+              FreePool (HandleBuffer);
+              ASSERT (FALSE);
+              return EFI_ABORTED;
+            }
+          } else if ((FtwDevice->FtwWorkSpaceBase + FtwDevice->FtwWorkSpaceSize) > FtwDevice->WorkBlockSize) {
+            DEBUG ((EFI_D_ERROR, "Ftw: The work space range should not span blocks when work space size is less than one block size\n"));
+            FreePool (HandleBuffer);
+            ASSERT (FALSE);
+            return EFI_ABORTED;
+          }
+          break;
+        }
+      }
+    }
+
+    if ((FtwDevice->FtwBackupFvb == NULL) && (FtwDevice->SpareAreaAddress >= FvbBaseAddress) &&
+        ((FtwDevice->SpareAreaAddress + FtwDevice->SpareAreaLength) <= (FvbBaseAddress + BlockSize * NumberOfBlocks))) {
+      FtwDevice->FtwBackupFvb = Fvb;
+      //
+      // To get the LBA of spare
+      //
+      for (LbaIndex = 1; LbaIndex <= NumberOfBlocks; LbaIndex += 1) {
+        if ((FtwDevice->SpareAreaAddress >= (FvbBaseAddress + BlockSize * (LbaIndex - 1)))
+            && (FtwDevice->SpareAreaAddress < (FvbBaseAddress + BlockSize * LbaIndex))) {
+          //
+          // Get the NumberOfSpareBlock and BlockSize
+          //
+          FtwDevice->FtwSpareLba        = LbaIndex - 1;
+          FtwDevice->SpareBlockSize     = BlockSize;
+          FtwDevice->NumberOfSpareBlock = FtwDevice->SpareAreaLength / FtwDevice->SpareBlockSize;
+          //
+          // Check the range of spare area to make sure that it's in FV range
+          //
+          if ((FtwDevice->FtwSpareLba + FtwDevice->NumberOfSpareBlock) > NumberOfBlocks) {
+            DEBUG ((EFI_D_ERROR, "Ftw: Spare area is out of FV range\n"));
+            FreePool (HandleBuffer);
+            ASSERT (FALSE);
+            return EFI_ABORTED;
+          }
+          //
+          // Check the alignment of spare area address and length, they should be block size aligned
+          //
+          if (((FtwDevice->SpareAreaAddress & (FtwDevice->SpareBlockSize - 1)) != 0) ||
+              ((FtwDevice->SpareAreaLength & (FtwDevice->SpareBlockSize - 1)) != 0)) {
+            DEBUG ((EFI_D_ERROR, "Ftw: Spare area address or length is not block size aligned\n"));
+            FreePool (HandleBuffer);
+            //
+            // Report Status Code EFI_SW_EC_ABORTED.
+            //
+            REPORT_STATUS_CODE ((EFI_ERROR_CODE | EFI_ERROR_UNRECOVERED), (EFI_SOFTWARE_DXE_BS_DRIVER | EFI_SW_EC_ABORTED));
+            ASSERT (FALSE);
+            CpuDeadLoop ();
+          }
+          break;
+        }
+      }
+    }
+  }
+  FreePool (HandleBuffer);
+
+  if ((FtwDevice->FtwBackupFvb == NULL) || (FtwDevice->FtwFvBlock == NULL) ||
+    (FtwDevice->FtwWorkSpaceLba == (EFI_LBA) (-1)) || (FtwDevice->FtwSpareLba == (EFI_LBA) (-1))) {
+    return EFI_ABORTED;
+  }
+  DEBUG ((EFI_D_INFO, "Ftw: FtwWorkSpaceLba - 0x%lx, WorkBlockSize  - 0x%x, FtwWorkSpaceBase - 0x%x\n", FtwDevice->FtwWorkSpaceLba, FtwDevice->WorkBlockSize, FtwDevice->FtwWorkSpaceBase));
+  DEBUG ((EFI_D_INFO, "Ftw: FtwSpareLba     - 0x%lx, SpareBlockSize - 0x%x\n", FtwDevice->FtwSpareLba, FtwDevice->SpareBlockSize));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialization for Fault Tolerant Write protocol.
+
+  @param[in, out] FtwDevice     Pointer to the FTW device structure
+
+  @retval EFI_SUCCESS           Initialize the FTW protocol successfully.
+  @retval EFI_NOT_FOUND         No proper FVB protocol was found.
+  
+**/
+EFI_STATUS
+InitFtwProtocol (
+  IN OUT EFI_FTW_DEVICE               *FtwDevice
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  EFI_FAULT_TOLERANT_WRITE_HEADER     *FtwHeader;
+  UINTN                               Offset;
+  EFI_HANDLE                          FvbHandle;
+  EFI_LBA                             WorkSpaceLbaOffset;
+
+  //
+  // Find the right SMM Fvb protocol instance for FTW.
+  //
+  Status = FindFvbForFtw (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }  
+
+  //
+  // Calculate the start LBA of working block.
+  //
+  if (FtwDevice->FtwWorkSpaceSize >= FtwDevice->WorkBlockSize) {
+    //
+    // Working block is a standalone area which only contains working space.
+    //
+    FtwDevice->NumberOfWorkBlock = FtwDevice->NumberOfWorkSpaceBlock;
+  } else {
+    //
+    // Working block is an area which
+    // contains working space in its last block and has the same size as spare
+    // block, unless there are not enough blocks before the block that contains
+    // working space.
+    //
+    FtwDevice->NumberOfWorkBlock = (UINTN) (FtwDevice->FtwWorkSpaceLba + FtwDevice->NumberOfWorkSpaceBlock);
+    while (FtwDevice->NumberOfWorkBlock * FtwDevice->WorkBlockSize > FtwDevice->SpareAreaLength) {
+      FtwDevice->NumberOfWorkBlock--;
+    }
+  }
+  FtwDevice->FtwWorkBlockLba = FtwDevice->FtwWorkSpaceLba + FtwDevice->NumberOfWorkSpaceBlock - FtwDevice->NumberOfWorkBlock;
+  DEBUG ((EFI_D_INFO, "Ftw: NumberOfWorkBlock - 0x%x, FtwWorkBlockLba - 0x%lx\n", FtwDevice->NumberOfWorkBlock, FtwDevice->FtwWorkBlockLba));
+
+  //
+  // Calcualte the LBA and base of work space in spare block.
+  // Note: Do not assume Spare Block and Work Block have same block size.
+  //
+  WorkSpaceLbaOffset = FtwDevice->FtwWorkSpaceLba - FtwDevice->FtwWorkBlockLba;
+  FtwDevice->FtwWorkSpaceLbaInSpare = (EFI_LBA) (((UINTN) WorkSpaceLbaOffset * FtwDevice->WorkBlockSize + FtwDevice->FtwWorkSpaceBase) / FtwDevice->SpareBlockSize);
+  FtwDevice->FtwWorkSpaceBaseInSpare = ((UINTN) WorkSpaceLbaOffset * FtwDevice->WorkBlockSize + FtwDevice->FtwWorkSpaceBase) % FtwDevice->SpareBlockSize;
+  DEBUG ((EFI_D_INFO, "Ftw: WorkSpaceLbaInSpare - 0x%lx, WorkSpaceBaseInSpare - 0x%x\n", FtwDevice->FtwWorkSpaceLbaInSpare, FtwDevice->FtwWorkSpaceBaseInSpare));
+
+  //
+  // Initialize other parameters, and set WorkSpace as FTW_ERASED_BYTE.
+  //
+  FtwDevice->FtwWorkSpace = (UINT8 *) (FtwDevice + 1);
+  FtwDevice->FtwWorkSpaceHeader = (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *) FtwDevice->FtwWorkSpace;
+
+  FtwDevice->FtwLastWriteHeader = NULL;
+  FtwDevice->FtwLastWriteRecord = NULL;
+
+  InitializeLocalWorkSpaceHeader ();
+
+  //
+  // Refresh the working space data from working block
+  //
+  Status = WorkSpaceRefresh (FtwDevice);
+  ASSERT_EFI_ERROR (Status);
+  //
+  // If the working block workspace is not valid, try the spare block
+  //
+  if (!IsValidWorkSpace (FtwDevice->FtwWorkSpaceHeader)) {
+    //
+    // Read from spare block
+    //
+    Status = ReadWorkSpaceData (
+               FtwDevice->FtwBackupFvb,
+               FtwDevice->SpareBlockSize,
+               FtwDevice->FtwSpareLba + FtwDevice->FtwWorkSpaceLbaInSpare,
+               FtwDevice->FtwWorkSpaceBaseInSpare,
+               FtwDevice->FtwWorkSpaceSize,
+               FtwDevice->FtwWorkSpace
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // If spare block is valid, then replace working block content.
+    //
+    if (IsValidWorkSpace (FtwDevice->FtwWorkSpaceHeader)) {
+      Status = FlushSpareBlockToWorkingBlock (FtwDevice);
+      DEBUG ((EFI_D_ERROR, "Ftw: Restart working block update in InitFtwProtocol() - %r\n", Status));
+      FtwAbort (&FtwDevice->FtwInstance);
+      //
+      // Refresh work space.
+      //
+      Status = WorkSpaceRefresh (FtwDevice);
+      ASSERT_EFI_ERROR (Status);
+    } else {
+      DEBUG ((EFI_D_ERROR, "Ftw: Both are invalid, init workspace\n"));
+      //
+      // If both are invalid, then initialize work space.
+      //
+      SetMem (
+        FtwDevice->FtwWorkSpace,
+        FtwDevice->FtwWorkSpaceSize,
+        FTW_ERASED_BYTE
+        );
+      InitWorkSpaceHeader (FtwDevice->FtwWorkSpaceHeader);
+      //
+      // Initialize the work space
+      //
+      Status = FtwReclaimWorkSpace (FtwDevice, FALSE);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+  //
+  // If the FtwDevice->FtwLastWriteRecord is 1st record of write header &&
+  // (! SpareComplete) THEN call Abort().
+  //
+  if ((FtwDevice->FtwLastWriteHeader->HeaderAllocated == FTW_VALID_STATE) &&
+    (FtwDevice->FtwLastWriteRecord->SpareComplete != FTW_VALID_STATE) &&
+    IsFirstRecordOfWrites (FtwDevice->FtwLastWriteHeader, FtwDevice->FtwLastWriteRecord)
+    ) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Init.. find first record not SpareCompleted, abort()\n"));
+    FtwAbort (&FtwDevice->FtwInstance);
+  }
+  //
+  // If Header is incompleted and the last record has completed, then
+  // call Abort() to set the Header->Complete FLAG.
+  //
+  if ((FtwDevice->FtwLastWriteHeader->Complete != FTW_VALID_STATE) &&
+    (FtwDevice->FtwLastWriteRecord->DestinationComplete == FTW_VALID_STATE) &&
+    IsLastRecordOfWrites (FtwDevice->FtwLastWriteHeader, FtwDevice->FtwLastWriteRecord)
+    ) {
+    DEBUG ((EFI_D_ERROR, "Ftw: Init.. find last record completed but header not, abort()\n"));
+    FtwAbort (&FtwDevice->FtwInstance);
+  }
+  //
+  // To check the workspace buffer following last Write header/records is EMPTY or not.
+  // If it's not EMPTY, FTW also need to call reclaim().
+  //
+  FtwHeader = FtwDevice->FtwLastWriteHeader;
+  Offset    = (UINT8 *) FtwHeader - FtwDevice->FtwWorkSpace;
+  if (FtwDevice->FtwWorkSpace[Offset] != FTW_ERASED_BYTE) {
+    Offset += FTW_WRITE_TOTAL_SIZE (FtwHeader->NumberOfWrites, FtwHeader->PrivateDataSize);
+  }
+  
+  if (!IsErasedFlashBuffer (FtwDevice->FtwWorkSpace + Offset, FtwDevice->FtwWorkSpaceSize - Offset)) {
+    Status = FtwReclaimWorkSpace (FtwDevice, TRUE);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Restart if it's boot block
+  //
+  if ((FtwDevice->FtwLastWriteHeader->Complete != FTW_VALID_STATE) &&
+    (FtwDevice->FtwLastWriteRecord->SpareComplete == FTW_VALID_STATE)
+    ) {
+    if (FtwDevice->FtwLastWriteRecord->BootBlockUpdate == FTW_VALID_STATE) {
+      Status = FlushSpareBlockToBootBlock (FtwDevice);
+      DEBUG ((EFI_D_ERROR, "Ftw: Restart boot block update - %r\n", Status));
+      ASSERT_EFI_ERROR (Status);
+      FtwAbort (&FtwDevice->FtwInstance);
+    } else {
+      //
+      // if (SpareCompleted) THEN  Restart to fault tolerant write.
+      //
+      FvbHandle = NULL;
+      FvbHandle = GetFvbByAddress ((EFI_PHYSICAL_ADDRESS) (UINTN) ((INT64) FtwDevice->SpareAreaAddress + FtwDevice->FtwLastWriteRecord->RelativeOffset), &Fvb);
+      if (FvbHandle != NULL) {
+        Status = FtwRestart (&FtwDevice->FtwInstance, FvbHandle);
+        DEBUG ((EFI_D_ERROR, "Ftw: Restart last write - %r\n", Status));
+        ASSERT_EFI_ERROR (Status);
+      }
+      FtwAbort (&FtwDevice->FtwInstance);
+    }
+  }
+  //
+  // Hook the protocol API
+  //
+  FtwDevice->FtwInstance.GetMaxBlockSize = FtwGetMaxBlockSize;
+  FtwDevice->FtwInstance.Allocate        = FtwAllocate;
+  FtwDevice->FtwInstance.Write           = FtwWrite;
+  FtwDevice->FtwInstance.Restart         = FtwRestart;
+  FtwDevice->FtwInstance.Abort           = FtwAbort;
+  FtwDevice->FtwInstance.GetLastWrite    = FtwGetLastWrite;
+    
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxe.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxe.uni
new file mode 100644
index 0000000000..8b7cf4863b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxe.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxeExtra.uni b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxeExtra.uni
new file mode 100644
index 0000000000..54aec337c5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/SmmFaultTolerantWriteDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/UpdateWorkingBlock.c b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/UpdateWorkingBlock.c
new file mode 100644
index 0000000000..31f1e0bda6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWriteDxe/UpdateWorkingBlock.c
@@ -0,0 +1,619 @@
+/** @file
+
+   Internal functions to operate Working Block Space.
+
+Copyright (c) 2006 - 2015, 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 "FaultTolerantWrite.h"
+
+EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER mWorkingBlockHeader = {ZERO_GUID, 0, 0, 0, 0, {0, 0, 0}, 0};
+
+/**
+  Initialize a local work space header.
+
+  Since Signature and WriteQueueSize have been known, Crc can be calculated out,
+  then the work space header will be fixed.
+**/
+VOID
+InitializeLocalWorkSpaceHeader (
+  VOID
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Check signature with gEdkiiWorkingBlockSignatureGuid.
+  //
+  if (CompareGuid (&gEdkiiWorkingBlockSignatureGuid, &mWorkingBlockHeader.Signature)) {
+    //
+    // The local work space header has been initialized.
+    //
+    return;
+  }
+
+  SetMem (
+    &mWorkingBlockHeader,
+    sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER),
+    FTW_ERASED_BYTE
+    );
+
+  //
+  // Here using gEdkiiWorkingBlockSignatureGuid as the signature.
+  //
+  CopyMem (
+    &mWorkingBlockHeader.Signature,
+    &gEdkiiWorkingBlockSignatureGuid,
+    sizeof (EFI_GUID)
+    );
+  mWorkingBlockHeader.WriteQueueSize = (UINT64) (PcdGet32 (PcdFlashNvStorageFtwWorkingSize) - sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER));
+
+  //
+  // Crc is calculated with all the fields except Crc and STATE, so leave them as FTW_ERASED_BYTE.
+  //
+
+  //
+  // Calculate the Crc of woking block header
+  //
+  Status = gBS->CalculateCrc32 (
+                  &mWorkingBlockHeader,
+                  sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER),
+                  &mWorkingBlockHeader.Crc
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mWorkingBlockHeader.WorkingBlockValid    = FTW_VALID_STATE;
+  mWorkingBlockHeader.WorkingBlockInvalid  = FTW_INVALID_STATE;
+}
+
+/**
+  Check to see if it is a valid work space.
+
+
+  @param WorkingHeader   Pointer of working block header
+
+  @retval TRUE          The work space is valid.
+  @retval FALSE         The work space is invalid.
+
+**/
+BOOLEAN
+IsValidWorkSpace (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingHeader
+  )
+{
+  if (WorkingHeader == NULL) {
+    return FALSE;
+  }
+
+  if (CompareMem (WorkingHeader, &mWorkingBlockHeader, sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER)) == 0) {
+    return TRUE;
+  }
+
+  DEBUG ((EFI_D_ERROR, "Ftw: Work block header check error\n"));
+  return FALSE;
+}
+
+/**
+  Initialize a work space when there is no work space.
+
+  @param WorkingHeader   Pointer of working block header
+
+  @retval  EFI_SUCCESS    The function completed successfully
+  @retval  EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+InitWorkSpaceHeader (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingHeader
+  )
+{
+  if (WorkingHeader == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CopyMem (WorkingHeader, &mWorkingBlockHeader, sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read work space data from work block or spare block.
+
+  @param FvBlock        FVB Protocol interface to access the block.
+  @param BlockSize      The size of the block.
+  @param Lba            Lba of the block.
+  @param Offset         The offset within the block.
+  @param Length         The number of bytes to read from the block.
+  @param Buffer         The data is read.
+
+  @retval EFI_SUCCESS   The function completed successfully.
+  @retval EFI_ABORTED   The function could not complete successfully.
+
+**/
+EFI_STATUS
+ReadWorkSpaceData (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL *FvBlock,
+  IN UINTN                              BlockSize,
+  IN EFI_LBA                            Lba,
+  IN UINTN                              Offset,
+  IN UINTN                              Length,
+  OUT UINT8                             *Buffer
+  )
+{
+  EFI_STATUS            Status;
+  UINT8                 *Ptr;
+  UINTN                 MyLength;
+
+  //
+  // Calculate the real Offset and Lba to write.
+  //
+  while (Offset >= BlockSize) {
+    Offset -= BlockSize;
+    Lba++;
+  }
+
+  Ptr = Buffer;
+  while (Length > 0) {
+    if ((Offset + Length) > BlockSize) {
+      MyLength = BlockSize - Offset;
+    } else {
+      MyLength = Length;
+    }
+
+    Status = FvBlock->Read (
+                        FvBlock,
+                        Lba,
+                        Offset,
+                        &MyLength,
+                        Ptr
+                        );
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+    Offset = 0;
+    Length -= MyLength;
+    Ptr += MyLength;
+    Lba++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write work space data to work block.
+
+  @param FvBlock        FVB Protocol interface to access the block.
+  @param BlockSize      The size of the block.
+  @param Lba            Lba of the block.
+  @param Offset         The offset within the block to place the data.
+  @param Length         The number of bytes to write to the block.
+  @param Buffer         The data to write.
+
+  @retval EFI_SUCCESS   The function completed successfully.
+  @retval EFI_ABORTED   The function could not complete successfully.
+
+**/
+EFI_STATUS
+WriteWorkSpaceData (
+  IN EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL *FvBlock,
+  IN UINTN                              BlockSize,
+  IN EFI_LBA                            Lba,
+  IN UINTN                              Offset,
+  IN UINTN                              Length,
+  IN UINT8                              *Buffer
+  )
+{
+  EFI_STATUS            Status;
+  UINT8                 *Ptr;
+  UINTN                 MyLength;
+
+  //
+  // Calculate the real Offset and Lba to write.
+  //
+  while (Offset >= BlockSize) {
+    Offset -= BlockSize;
+    Lba++;
+  }
+
+  Ptr = Buffer;
+  while (Length > 0) {
+    if ((Offset + Length) > BlockSize) {
+      MyLength = BlockSize - Offset;
+    } else {
+      MyLength = Length;
+    }
+
+    Status = FvBlock->Write (
+                        FvBlock,
+                        Lba,
+                        Offset,
+                        &MyLength,
+                        Ptr
+                        );
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+    Offset = 0;
+    Length -= MyLength;
+    Ptr += MyLength;
+    Lba++;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Read from working block to refresh the work space in memory.
+
+  @param FtwDevice   Point to private data of FTW driver
+
+  @retval  EFI_SUCCESS    The function completed successfully
+  @retval  EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+WorkSpaceRefresh (
+  IN EFI_FTW_DEVICE  *FtwDevice
+  )
+{
+  EFI_STATUS                      Status;
+  UINTN                           RemainingSpaceSize;
+
+  //
+  // Initialize WorkSpace as FTW_ERASED_BYTE
+  //
+  SetMem (
+    FtwDevice->FtwWorkSpace,
+    FtwDevice->FtwWorkSpaceSize,
+    FTW_ERASED_BYTE
+    );
+
+  //
+  // Read from working block
+  //
+  Status = ReadWorkSpaceData (
+             FtwDevice->FtwFvBlock,
+             FtwDevice->WorkBlockSize,
+             FtwDevice->FtwWorkSpaceLba,
+             FtwDevice->FtwWorkSpaceBase,
+             FtwDevice->FtwWorkSpaceSize,
+             FtwDevice->FtwWorkSpace
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+  //
+  // Refresh the FtwLastWriteHeader
+  //
+  Status = FtwGetLastWriteHeader (
+            FtwDevice->FtwWorkSpaceHeader,
+            FtwDevice->FtwWorkSpaceSize,
+            &FtwDevice->FtwLastWriteHeader
+            );
+  RemainingSpaceSize = FtwDevice->FtwWorkSpaceSize - ((UINTN) FtwDevice->FtwLastWriteHeader - (UINTN) FtwDevice->FtwWorkSpace);
+  DEBUG ((EFI_D_INFO, "Ftw: Remaining work space size - %x\n", RemainingSpaceSize));
+  //
+  // If FtwGetLastWriteHeader() returns error, or the remaining space size is even not enough to contain
+  // one EFI_FAULT_TOLERANT_WRITE_HEADER + one EFI_FAULT_TOLERANT_WRITE_RECORD(It will cause that the header
+  // pointed by FtwDevice->FtwLastWriteHeader or record pointed by FtwDevice->FtwLastWriteRecord may contain invalid data),
+  // it needs to reclaim work space.
+  //
+  if (EFI_ERROR (Status) || RemainingSpaceSize < sizeof (EFI_FAULT_TOLERANT_WRITE_HEADER) + sizeof (EFI_FAULT_TOLERANT_WRITE_RECORD)) {
+    //
+    // reclaim work space in working block.
+    //
+    Status = FtwReclaimWorkSpace (FtwDevice, TRUE);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "Ftw: Reclaim workspace - %r\n", Status));
+      return EFI_ABORTED;
+    }
+    //
+    // Read from working block again
+    //
+    Status = ReadWorkSpaceData (
+               FtwDevice->FtwFvBlock,
+               FtwDevice->WorkBlockSize,
+               FtwDevice->FtwWorkSpaceLba,
+               FtwDevice->FtwWorkSpaceBase,
+               FtwDevice->FtwWorkSpaceSize,
+               FtwDevice->FtwWorkSpace
+               );
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+
+    Status = FtwGetLastWriteHeader (
+              FtwDevice->FtwWorkSpaceHeader,
+              FtwDevice->FtwWorkSpaceSize,
+              &FtwDevice->FtwLastWriteHeader
+              );
+    if (EFI_ERROR (Status)) {
+      return EFI_ABORTED;
+    }
+  }
+  //
+  // Refresh the FtwLastWriteRecord
+  //
+  Status = FtwGetLastWriteRecord (
+            FtwDevice->FtwLastWriteHeader,
+            &FtwDevice->FtwLastWriteRecord
+            );
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reclaim the work space on the working block.
+
+  @param FtwDevice       Point to private data of FTW driver
+  @param PreserveRecord  Whether to preserve the working record is needed
+
+  @retval EFI_SUCCESS            The function completed successfully
+  @retval EFI_OUT_OF_RESOURCES   Allocate memory error
+  @retval EFI_ABORTED            The function could not complete successfully
+
+**/
+EFI_STATUS
+FtwReclaimWorkSpace (
+  IN EFI_FTW_DEVICE  *FtwDevice,
+  IN BOOLEAN         PreserveRecord
+  )
+{
+  EFI_STATUS                              Status;
+  UINTN                                   Length;
+  EFI_FAULT_TOLERANT_WRITE_HEADER         *Header;
+  UINT8                                   *TempBuffer;
+  UINTN                                   TempBufferSize;
+  UINTN                                   SpareBufferSize;
+  UINT8                                   *SpareBuffer;
+  EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *WorkingBlockHeader;
+  UINTN                                   Index;
+  UINT8                                   *Ptr;
+  EFI_LBA                                 WorkSpaceLbaOffset;
+
+  DEBUG ((EFI_D_INFO, "Ftw: start to reclaim work space\n"));
+
+  WorkSpaceLbaOffset = FtwDevice->FtwWorkSpaceLba - FtwDevice->FtwWorkBlockLba;
+
+  //
+  // Read all original data from working block to a memory buffer
+  //
+  TempBufferSize = FtwDevice->NumberOfWorkBlock * FtwDevice->WorkBlockSize;
+  TempBuffer     = AllocateZeroPool (TempBufferSize);
+  if (TempBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ptr = TempBuffer;
+  for (Index = 0; Index < FtwDevice->NumberOfWorkBlock; Index += 1) {
+    Length = FtwDevice->WorkBlockSize;
+    Status = FtwDevice->FtwFvBlock->Read (
+                                          FtwDevice->FtwFvBlock,
+                                          FtwDevice->FtwWorkBlockLba + Index,
+                                          0,
+                                          &Length,
+                                          Ptr
+                                          );
+    if (EFI_ERROR (Status)) {
+      FreePool (TempBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += Length;
+  }
+  //
+  // Clean up the workspace, remove all the completed records.
+  //
+  Ptr = TempBuffer +
+        (UINTN) WorkSpaceLbaOffset * FtwDevice->WorkBlockSize +
+        FtwDevice->FtwWorkSpaceBase;
+
+  //
+  // Clear the content of buffer that will save the new work space data
+  //
+  SetMem (Ptr, FtwDevice->FtwWorkSpaceSize, FTW_ERASED_BYTE);
+
+  //
+  // Copy EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER to buffer
+  //
+  CopyMem (
+    Ptr,
+    FtwDevice->FtwWorkSpaceHeader,
+    sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER)
+    );
+  if (PreserveRecord) {
+    //
+    // Get the last record following the header,
+    //
+    Status = FtwGetLastWriteHeader (
+               FtwDevice->FtwWorkSpaceHeader,
+               FtwDevice->FtwWorkSpaceSize,
+               &FtwDevice->FtwLastWriteHeader
+               );
+    Header = FtwDevice->FtwLastWriteHeader;
+    if (!EFI_ERROR (Status) && (Header != NULL) && (Header->Complete != FTW_VALID_STATE) && (Header->HeaderAllocated == FTW_VALID_STATE)) {
+      CopyMem (
+        Ptr + sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER),
+        FtwDevice->FtwLastWriteHeader,
+        FTW_WRITE_TOTAL_SIZE (Header->NumberOfWrites, Header->PrivateDataSize)
+        );
+    }
+  }
+
+  CopyMem (
+    FtwDevice->FtwWorkSpace,
+    Ptr,
+    FtwDevice->FtwWorkSpaceSize
+    );
+
+  FtwGetLastWriteHeader (
+    FtwDevice->FtwWorkSpaceHeader,
+    FtwDevice->FtwWorkSpaceSize,
+    &FtwDevice->FtwLastWriteHeader
+    );
+
+  FtwGetLastWriteRecord (
+    FtwDevice->FtwLastWriteHeader,
+    &FtwDevice->FtwLastWriteRecord
+    );
+
+  //
+  // Set the WorkingBlockValid and WorkingBlockInvalid as INVALID
+  //
+  WorkingBlockHeader                      = (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *) (TempBuffer +
+                                            (UINTN) WorkSpaceLbaOffset * FtwDevice->WorkBlockSize +
+                                            FtwDevice->FtwWorkSpaceBase);
+  WorkingBlockHeader->WorkingBlockValid   = FTW_INVALID_STATE;
+  WorkingBlockHeader->WorkingBlockInvalid = FTW_INVALID_STATE;
+
+  //
+  // Try to keep the content of spare block
+  // Save spare block into a spare backup memory buffer (Sparebuffer)
+  //
+  SpareBufferSize = FtwDevice->SpareAreaLength;
+  SpareBuffer     = AllocatePool (SpareBufferSize);
+  if (SpareBuffer == NULL) {
+    FreePool (TempBuffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ptr = SpareBuffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    Length = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Read (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &Length,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (TempBuffer);
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += Length;
+  }
+  //
+  // Write the memory buffer to spare block
+  //
+  Status  = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (TempBuffer);
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  Ptr     = TempBuffer;
+  for (Index = 0; TempBufferSize > 0; Index += 1) {
+    if (TempBufferSize > FtwDevice->SpareBlockSize) {
+      Length = FtwDevice->SpareBlockSize;
+    } else {
+      Length = TempBufferSize;
+    }
+    Status = FtwDevice->FtwBackupFvb->Write (
+                                            FtwDevice->FtwBackupFvb,
+                                            FtwDevice->FtwSpareLba + Index,
+                                            0,
+                                            &Length,
+                                            Ptr
+                                            );
+    if (EFI_ERROR (Status)) {
+      FreePool (TempBuffer);
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += Length;
+    TempBufferSize -= Length;
+  }
+  //
+  // Free TempBuffer
+  //
+  FreePool (TempBuffer);
+
+  //
+  // Set the WorkingBlockValid in spare block
+  //
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwBackupFvb,
+            FtwDevice->SpareBlockSize,
+            FtwDevice->FtwSpareLba + FtwDevice->FtwWorkSpaceLbaInSpare,
+            FtwDevice->FtwWorkSpaceBaseInSpare + sizeof (EFI_GUID) + sizeof (UINT32),
+            WORKING_BLOCK_VALID
+            );
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  //
+  // Before erase the working block, set WorkingBlockInvalid in working block.
+  //
+  // Offset = OFFSET_OF(EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER,
+  //                          WorkingBlockInvalid);
+  //
+  Status = FtwUpdateFvState (
+            FtwDevice->FtwFvBlock,
+            FtwDevice->WorkBlockSize,
+            FtwDevice->FtwWorkSpaceLba,
+            FtwDevice->FtwWorkSpaceBase + sizeof (EFI_GUID) + sizeof (UINT32),
+            WORKING_BLOCK_INVALID
+            );
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+
+  FtwDevice->FtwWorkSpaceHeader->WorkingBlockInvalid = FTW_VALID_STATE;
+
+  //
+  // Write the spare block to working block
+  //
+  Status = FlushSpareBlockToWorkingBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return Status;
+  }
+  //
+  // Restore spare backup buffer into spare block , if no failure happened during FtwWrite.
+  //
+  Status  = FtwEraseSpareBlock (FtwDevice);
+  if (EFI_ERROR (Status)) {
+    FreePool (SpareBuffer);
+    return EFI_ABORTED;
+  }
+  Ptr     = SpareBuffer;
+  for (Index = 0; Index < FtwDevice->NumberOfSpareBlock; Index += 1) {
+    Length = FtwDevice->SpareBlockSize;
+    Status = FtwDevice->FtwBackupFvb->Write (
+                                        FtwDevice->FtwBackupFvb,
+                                        FtwDevice->FtwSpareLba + Index,
+                                        0,
+                                        &Length,
+                                        Ptr
+                                        );
+    if (EFI_ERROR (Status)) {
+      FreePool (SpareBuffer);
+      return EFI_ABORTED;
+    }
+
+    Ptr += Length;
+  }
+
+  FreePool (SpareBuffer);
+
+  DEBUG ((EFI_D_INFO, "Ftw: reclaim work space successfully\n"));
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.c b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.c
new file mode 100644
index 0000000000..14e18e5ae2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.c
@@ -0,0 +1,321 @@
+/** @file
+  This driver installs gEdkiiFaultTolerantWriteGuid PPI to inform
+  the check for FTW last write data has been done.
+
+Copyright (c) 2013, 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 <PiPei.h>
+
+#include <Guid/SystemNvDataGuid.h>
+#include <Guid/FaultTolerantWrite.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/HobLib.h>
+
+EFI_PEI_PPI_DESCRIPTOR     mPpiListVariable = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEdkiiFaultTolerantWriteGuid,
+  NULL
+};
+
+/**
+  Get the last Write Header pointer.
+  The last write header is the header whose 'complete' state hasn't been set.
+  After all, this header may be a EMPTY header entry for next Allocate.
+
+
+  @param FtwWorkSpaceHeader Pointer of the working block header
+  @param FtwWorkSpaceSize   Size of the work space
+  @param FtwWriteHeader     Pointer to retrieve the last write header
+
+  @retval  EFI_SUCCESS      Get the last write record successfully
+  @retval  EFI_ABORTED      The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteHeader (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER  *FtwWorkSpaceHeader,
+  IN UINTN                                    FtwWorkSpaceSize,
+  OUT EFI_FAULT_TOLERANT_WRITE_HEADER         **FtwWriteHeader
+  )
+{
+  UINTN                           Offset;
+  EFI_FAULT_TOLERANT_WRITE_HEADER *FtwHeader;
+
+  *FtwWriteHeader = NULL;
+  FtwHeader       = (EFI_FAULT_TOLERANT_WRITE_HEADER *) (FtwWorkSpaceHeader + 1);
+  Offset          = sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER);
+
+  while (FtwHeader->Complete == FTW_VALID_STATE) {
+    Offset += FTW_WRITE_TOTAL_SIZE (FtwHeader->NumberOfWrites, FtwHeader->PrivateDataSize);
+    //
+    // If Offset exceed the FTW work space boudary, return error.
+    //
+    if (Offset >= FtwWorkSpaceSize) {
+      *FtwWriteHeader = FtwHeader;
+      return EFI_ABORTED;
+    }
+
+    FtwHeader = (EFI_FAULT_TOLERANT_WRITE_HEADER *) ((UINT8 *) FtwWorkSpaceHeader + Offset);
+  }
+  //
+  // Last write header is found
+  //
+  *FtwWriteHeader = FtwHeader;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the last Write Record pointer. The last write Record is the Record
+  whose DestinationCompleted state hasn't been set. After all, this Record
+  may be a EMPTY record entry for next write.
+
+
+  @param FtwWriteHeader  Pointer to the write record header
+  @param FtwWriteRecord  Pointer to retrieve the last write record
+
+  @retval EFI_SUCCESS        Get the last write record successfully
+  @retval EFI_ABORTED        The FTW work space is damaged
+
+**/
+EFI_STATUS
+FtwGetLastWriteRecord (
+  IN EFI_FAULT_TOLERANT_WRITE_HEADER          *FtwWriteHeader,
+  OUT EFI_FAULT_TOLERANT_WRITE_RECORD         **FtwWriteRecord
+  )
+{
+  UINTN                           Index;
+  EFI_FAULT_TOLERANT_WRITE_RECORD *FtwRecord;
+
+  *FtwWriteRecord = NULL;
+  FtwRecord       = (EFI_FAULT_TOLERANT_WRITE_RECORD *) (FtwWriteHeader + 1);
+
+  //
+  // Try to find the last write record "that has not completed"
+  //
+  for (Index = 0; Index < FtwWriteHeader->NumberOfWrites; Index += 1) {
+    if (FtwRecord->DestinationComplete != FTW_VALID_STATE) {
+      //
+      // The last write record is found
+      //
+      *FtwWriteRecord = FtwRecord;
+      return EFI_SUCCESS;
+    }
+
+    FtwRecord++;
+
+    if (FtwWriteHeader->PrivateDataSize != 0) {
+      FtwRecord = (EFI_FAULT_TOLERANT_WRITE_RECORD *) ((UINTN) FtwRecord + (UINTN) FtwWriteHeader->PrivateDataSize);
+    }
+  }
+  //
+  //  if Index == NumberOfWrites, then
+  //  the last record has been written successfully,
+  //  but the Header->Complete Flag has not been set.
+  //  also return the last record.
+  //
+  if (Index == FtwWriteHeader->NumberOfWrites) {
+    *FtwWriteRecord = (EFI_FAULT_TOLERANT_WRITE_RECORD *) ((UINTN) FtwRecord - FTW_RECORD_SIZE (FtwWriteHeader->PrivateDataSize));
+    return EFI_SUCCESS;
+  }
+
+  return EFI_ABORTED;
+}
+
+/**
+  Check to see if it is a valid work space.
+
+
+  @param WorkingHeader   Pointer of working block header
+  @param WorkingLength   Working block length
+
+  @retval TRUE          The work space is valid.
+  @retval FALSE         The work space is invalid.
+
+**/
+BOOLEAN
+IsValidWorkSpace (
+  IN EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER    *WorkingHeader,
+  IN UINTN                                      WorkingLength
+  )
+{
+  UINT8 Data;
+
+  if (WorkingHeader == NULL) {
+    return FALSE;
+  }
+
+  if ((WorkingHeader->WorkingBlockValid != FTW_VALID_STATE) || (WorkingHeader->WorkingBlockInvalid == FTW_VALID_STATE)) {
+    DEBUG ((EFI_D_ERROR, "FtwPei: Work block header valid bit check error\n"));
+    return FALSE;
+  }
+
+  if (WorkingHeader->WriteQueueSize != (WorkingLength - sizeof (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER))) {
+    DEBUG ((EFI_D_ERROR, "FtwPei: Work block header WriteQueueSize check error\n"));
+    return FALSE;
+  }
+
+  //
+  // Check signature with gEdkiiWorkingBlockSignatureGuid
+  //
+  if (!CompareGuid (&gEdkiiWorkingBlockSignatureGuid, &WorkingHeader->Signature)) {
+    DEBUG ((EFI_D_ERROR, "FtwPei: Work block header signature check error, it should be gEdkiiWorkingBlockSignatureGuid\n"));
+    //
+    // To be compatible with old signature gEfiSystemNvDataFvGuid.
+    //
+    if (!CompareGuid (&gEfiSystemNvDataFvGuid, &WorkingHeader->Signature)) {
+      return FALSE;
+    } else {
+      Data = *(UINT8 *) (WorkingHeader + 1);
+      if (Data != 0xff) {
+        DEBUG ((EFI_D_ERROR, "FtwPei: Old format FTW structure can't be handled\n"));
+        ASSERT (FALSE);
+        return FALSE;
+      }
+    }
+  }
+
+  return TRUE;
+
+}
+
+/**
+  Main entry for Fault Tolerant Write PEIM.
+
+  @param[in]  FileHandle              Handle of the file being invoked.
+  @param[in]  PeiServices             Pointer to PEI Services table.
+
+  @retval EFI_SUCCESS  If the interface could be successfully installed
+  @retval Others       Returned from PeiServicesInstallPpi()
+
+**/
+EFI_STATUS
+EFIAPI
+PeimFaultTolerantWriteInitialize (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER   *FtwWorkingBlockHeader;
+  EFI_FAULT_TOLERANT_WRITE_HEADER           *FtwLastWriteHeader;
+  EFI_FAULT_TOLERANT_WRITE_RECORD           *FtwLastWriteRecord;
+  EFI_PHYSICAL_ADDRESS                      WorkSpaceAddress;
+  UINTN                                     WorkSpaceLength;
+  EFI_PHYSICAL_ADDRESS                      SpareAreaAddress;
+  UINTN                                     SpareAreaLength;
+  EFI_PHYSICAL_ADDRESS                      WorkSpaceInSpareArea;
+  FAULT_TOLERANT_WRITE_LAST_WRITE_DATA      FtwLastWrite;
+
+  FtwWorkingBlockHeader = NULL;
+  FtwLastWriteHeader = NULL;
+  FtwLastWriteRecord = NULL;
+
+  WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwWorkingBase64);
+  if (WorkSpaceAddress == 0) {
+    WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwWorkingBase);
+  }
+  WorkSpaceLength = (UINTN) PcdGet32 (PcdFlashNvStorageFtwWorkingSize);
+
+  SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwSpareBase64);
+  if (SpareAreaAddress == 0) {
+    SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwSpareBase);
+  }
+  SpareAreaLength = (UINTN) PcdGet32 (PcdFlashNvStorageFtwSpareSize);
+
+  //
+  // The address of FTW working base and spare base must not be 0.
+  //
+  ASSERT ((WorkSpaceAddress != 0) && (SpareAreaAddress != 0));
+
+  FtwWorkingBlockHeader = (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *) (UINTN) WorkSpaceAddress;
+  if (IsValidWorkSpace (FtwWorkingBlockHeader, WorkSpaceLength)) {
+    Status = FtwGetLastWriteHeader (
+               FtwWorkingBlockHeader,
+               WorkSpaceLength,
+               &FtwLastWriteHeader
+               );
+    if (!EFI_ERROR (Status)) {
+      Status = FtwGetLastWriteRecord (
+                 FtwLastWriteHeader,
+                 &FtwLastWriteRecord
+                 );
+    }
+
+    if (!EFI_ERROR (Status)) {
+      ASSERT (FtwLastWriteRecord != NULL);
+      if ((FtwLastWriteRecord->SpareComplete == FTW_VALID_STATE) && (FtwLastWriteRecord->DestinationComplete != FTW_VALID_STATE)) {
+        //
+        // If FTW last write was still in progress with SpareComplete set and DestinationComplete not set.
+        // It means the target buffer has been backed up in spare block, then target block has been erased,
+        // but the target buffer has not been writen in target block from spare block, we need to build
+        // FAULT_TOLERANT_WRITE_LAST_WRITE_DATA GUID hob to hold the FTW last write data.
+        //
+        FtwLastWrite.TargetAddress = (EFI_PHYSICAL_ADDRESS) (UINTN) ((INT64) SpareAreaAddress + FtwLastWriteRecord->RelativeOffset);
+        FtwLastWrite.SpareAddress = SpareAreaAddress;
+        FtwLastWrite.Length = SpareAreaLength;
+        DEBUG ((
+          EFI_D_INFO,
+          "FtwPei last write data: TargetAddress - 0x%x SpareAddress - 0x%x Length - 0x%x\n",
+          (UINTN) FtwLastWrite.TargetAddress,
+          (UINTN) FtwLastWrite.SpareAddress,
+          (UINTN) FtwLastWrite.Length));
+        BuildGuidDataHob (&gEdkiiFaultTolerantWriteGuid, (VOID *) &FtwLastWrite, sizeof (FAULT_TOLERANT_WRITE_LAST_WRITE_DATA));
+      }
+    }
+  } else {
+    FtwWorkingBlockHeader = NULL;
+    //
+    // If the working block workspace is not valid, try to find workspace in the spare block.
+    //
+    WorkSpaceInSpareArea = SpareAreaAddress + SpareAreaLength - WorkSpaceLength;
+    while (WorkSpaceInSpareArea >= SpareAreaAddress) {
+      if (CompareGuid (&gEdkiiWorkingBlockSignatureGuid, (EFI_GUID *) (UINTN) WorkSpaceInSpareArea)) {
+        //
+        // Found the workspace.
+        //
+        DEBUG ((EFI_D_INFO, "FtwPei: workspace in spare block is at 0x%x.\n", (UINTN) WorkSpaceInSpareArea));
+        FtwWorkingBlockHeader = (EFI_FAULT_TOLERANT_WORKING_BLOCK_HEADER *) (UINTN) WorkSpaceInSpareArea;
+        break;
+      }
+      WorkSpaceInSpareArea = WorkSpaceInSpareArea - sizeof (EFI_GUID);
+    }
+    if ((FtwWorkingBlockHeader != NULL) && IsValidWorkSpace (FtwWorkingBlockHeader, WorkSpaceLength)) {
+      //
+      // It was workspace self reclaim, build FAULT_TOLERANT_WRITE_LAST_WRITE_DATA GUID hob for it.
+      //
+      FtwLastWrite.TargetAddress = WorkSpaceAddress - (WorkSpaceInSpareArea - SpareAreaAddress);
+      FtwLastWrite.SpareAddress = SpareAreaAddress;
+      FtwLastWrite.Length = SpareAreaLength;
+      DEBUG ((
+        EFI_D_INFO,
+        "FtwPei last write data: TargetAddress - 0x%x SpareAddress - 0x%x Length - 0x%x\n",
+        (UINTN) FtwLastWrite.TargetAddress,
+        (UINTN) FtwLastWrite.SpareAddress,
+        (UINTN) FtwLastWrite.Length));
+      BuildGuidDataHob (&gEdkiiFaultTolerantWriteGuid, (VOID *) &FtwLastWrite, sizeof (FAULT_TOLERANT_WRITE_LAST_WRITE_DATA));
+    } else {
+      //
+      // Both are invalid.
+      //
+      DEBUG ((EFI_D_ERROR, "FtwPei: Both working and spare block are invalid.\n"));
+    }
+  }
+
+  //
+  // Install gEdkiiFaultTolerantWriteGuid PPI to inform the check for FTW last write data has been done.
+  //
+  return PeiServicesInstallPpi (&mPpiListVariable);
+}
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.inf b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.inf
new file mode 100644
index 0000000000..12b26604e2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.inf
@@ -0,0 +1,67 @@
+## @file
+# Fault Tolerant Write PEI Driver.
+#
+# This module installs gEdkiiFaultTolerantWriteGuid PPI to inform the check for FTW last write data has been done.
+#
+# Copyright (c) 2013 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FaultTolerantWritePei
+  MODULE_UNI_FILE                = FaultTolerantWritePei.uni
+  FILE_GUID                      = AAC33064-9ED0-4b89-A5AD-3EA767960B22
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PeimFaultTolerantWriteInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  FaultTolerantWritePei.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeimEntryPoint
+  PeiServicesLib
+  BaseLib
+  DebugLib
+  HobLib
+  BaseMemoryLib
+  PcdLib
+
+[Guids]
+  ## SOMETIMES_PRODUCES   ## HOB
+  ## PRODUCES             ## GUID # Install ppi
+  gEdkiiFaultTolerantWriteGuid
+  gEdkiiWorkingBlockSignatureGuid               ## SOMETIMES_CONSUMES   ## GUID
+  gEfiSystemNvDataFvGuid                        ## SOMETIMES_CONSUMES   ## GUID
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase64  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase64    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareSize      ## CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FaultTolerantWritePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.uni b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.uni
new file mode 100644
index 0000000000..7ef1733c5e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePei.uni
diff --git a/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePeiExtra.uni b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePeiExtra.uni
new file mode 100644
index 0000000000..d13ce532aa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FaultTolerantWritePei/FaultTolerantWritePeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/ComponentName.c b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/ComponentName.c
new file mode 100644
index 0000000000..5ea05975ae
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/ComponentName.c
@@ -0,0 +1,187 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for FvSimpleFileSystem driver.
+
+Copyright (c) 2014, ARM Limited. All rights reserved.
+Copyright (c) 2014, 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 "FvSimpleFileSystemInternal.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gFvSimpleFileSystemComponentName = {
+  FvSimpleFileSystemComponentNameGetDriverName,
+  FvSimpleFileSystemComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gFvSimpleFileSystemComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) FvSimpleFileSystemComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) FvSimpleFileSystemComponentNameGetControllerName,
+  "en"
+};
+
+//
+// Driver name table for FvSimpleFileSystem module.
+// It is shared by the implementation of ComponentName & ComponentName2 Protocol.
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mFvSimpleFileSystemDriverNameTable[] = {
+  {
+    "eng;en",
+    (CHAR16 *)L"Fv Simple File System Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mFvSimpleFileSystemDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gFvSimpleFileSystemComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.c b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.c
new file mode 100644
index 0000000000..c6137aca1f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.c
@@ -0,0 +1,1027 @@
+/** @file
+  This driver uses the EFI_FIRMWARE_VOLUME2_PROTOCOL to expose files in firmware
+  volumes via the the EFI_SIMPLE_FILESYSTEM_PROTOCOL and EFI_FILE_PROTOCOL.
+
+  It will expose a single directory, containing one file for each file in the firmware
+  volume. If a file has a UI section, its contents will be used as a filename.
+  Otherwise, a string representation of the GUID will be used.
+  Files of an executable type (That is PEIM, DRIVER, COMBINED_PEIM_DRIVER and APPLICATION)
+  will have ".efi" added to their filename.
+
+  Its primary intended use is to be able to start EFI applications embedded in FVs
+  from the UEFI shell. It is entirely read-only.
+
+Copyright (c) 2014, ARM Limited. All rights reserved.
+Copyright (c) 2014 - 2015, 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 "FvSimpleFileSystemInternal.h"
+
+//
+// Template for EFI_FILE_SYSTEM_INFO data structure.
+//
+EFI_FILE_SYSTEM_INFO mFsInfoTemplate = {
+  0,    // Populate at runtime
+  TRUE, // Read-only
+  0,    // Don't know volume size
+  0,    // No free space
+  0,    // Don't know block size
+  L""   // Populate at runtime
+};
+
+//
+// Template for EFI_FILE_PROTOCOL data structure.
+//
+EFI_FILE_PROTOCOL mFileSystemTemplate = {
+  EFI_FILE_PROTOCOL_REVISION,
+  FvSimpleFileSystemOpen,
+  FvSimpleFileSystemClose,
+  FvSimpleFileSystemDelete,
+  FvSimpleFileSystemRead,
+  FvSimpleFileSystemWrite,
+  FvSimpleFileSystemGetPosition,
+  FvSimpleFileSystemSetPosition,
+  FvSimpleFileSystemGetInfo,
+  FvSimpleFileSystemSetInfo,
+  FvSimpleFileSystemFlush
+};
+
+/**
+  Find and call ReadSection on the first section found of an executable type.
+
+  @param  FvProtocol                  A pointer to the EFI_FIRMWARE_VOLUME2_PROTOCOL instance.
+  @param  FvFileInfo                  A pointer to the FV_FILESYSTEM_FILE_INFO instance that is a struct
+                                      representing a file's info.
+  @param  BufferSize                  Pointer to a caller-allocated UINTN. It indicates the size of
+                                      the memory represented by *Buffer.
+  @param  Buffer                      Pointer to a pointer to a data buffer to contain file content.
+
+  @retval EFI_SUCCESS                 The call completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL   The buffer is too small to contain the requested output.
+  @retval EFI_ACCESS_DENIED           The firmware volume is configured to disallow reads.
+  @retval EFI_NOT_FOUND               The requested file was not found in the firmware volume.
+  @retval EFI_DEVICE_ERROR            A hardware error occurred when attempting toaccess the firmware volume.
+
+**/
+EFI_STATUS
+FvFsFindExecutableSection (
+  IN     EFI_FIRMWARE_VOLUME2_PROTOCOL     *FvProtocol,
+  IN     FV_FILESYSTEM_FILE_INFO           *FvFileInfo,
+  IN OUT UINTN                             *BufferSize,
+  IN OUT VOID                              **Buffer
+  )
+{
+  EFI_SECTION_TYPE                    SectionType;
+  UINT32                              AuthenticationStatus;
+  EFI_STATUS                          Status;
+
+  for (SectionType = EFI_SECTION_PE32; SectionType <= EFI_SECTION_TE; SectionType++) {
+    Status = FvProtocol->ReadSection (
+                           FvProtocol,
+                           &FvFileInfo->NameGuid,
+                           SectionType,
+                           0,
+                           Buffer,
+                           BufferSize,
+                           &AuthenticationStatus
+                           );
+    if (Status != EFI_NOT_FOUND) {
+      return Status;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Get the size of the buffer that will be returned by FvFsReadFile.
+
+  @param  FvProtocol                  A pointer to the EFI_FIRMWARE_VOLUME2_PROTOCOL instance.
+  @param  FvFileInfo                  A pointer to the FV_FILESYSTEM_FILE_INFO instance that is a struct
+                                      representing a file's info.
+
+  @retval EFI_SUCCESS                 The file size was gotten correctly.
+  @retval Others                      The file size wasn't gotten correctly.
+
+**/
+EFI_STATUS
+FvFsGetFileSize (
+  IN     EFI_FIRMWARE_VOLUME2_PROTOCOL     *FvProtocol,
+  IN OUT FV_FILESYSTEM_FILE_INFO           *FvFileInfo
+  )
+{
+  UINT32                         AuthenticationStatus;
+  EFI_FV_FILETYPE                FoundType;
+  EFI_FV_FILE_ATTRIBUTES         Attributes;
+  EFI_STATUS                     Status;
+  UINT8                          IgnoredByte;
+  VOID                           *IgnoredPtr;
+
+  //
+  // To get the size of a section, we pass 0 for BufferSize. But we can't pass
+  // NULL for Buffer, as that will cause a return of INVALID_PARAMETER, and we
+  // can't pass NULL for *Buffer, as that will cause the callee to allocate
+  // a buffer of the sections size.
+  //
+  IgnoredPtr = &IgnoredByte;
+  FvFileInfo->FileInfo.FileSize = 0;
+
+  if (FV_FILETYPE_IS_EXECUTABLE (FvFileInfo->Type)) {
+    //
+    // Get the size of the first executable section out of the file.
+    //
+    Status = FvFsFindExecutableSection (FvProtocol, FvFileInfo, (UINTN*)&FvFileInfo->FileInfo.FileSize, &IgnoredPtr);
+    if (Status == EFI_WARN_BUFFER_TOO_SMALL) {
+      return EFI_SUCCESS;
+    }
+  } else if (FvFileInfo->Type == EFI_FV_FILETYPE_FREEFORM) {
+    //
+    // Try to get the size of a raw section out of the file
+    //
+    Status = FvProtocol->ReadSection (
+                           FvProtocol,
+                           &FvFileInfo->NameGuid,
+                           EFI_SECTION_RAW,
+                           0,
+                           &IgnoredPtr,
+                           (UINTN*)&FvFileInfo->FileInfo.FileSize,
+                           &AuthenticationStatus
+                           );
+    if (Status == EFI_WARN_BUFFER_TOO_SMALL) {
+      return EFI_SUCCESS;
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // Didn't find a raw section, just return the whole file's size.
+      //
+      return FvProtocol->ReadFile (
+                           FvProtocol,
+                           &FvFileInfo->NameGuid,
+                           NULL,
+                           (UINTN*)&FvFileInfo->FileInfo.FileSize,
+                           &FoundType,
+                           &Attributes,
+                           &AuthenticationStatus
+                           );
+    }
+  } else {
+    //
+    // Get the size of the entire file
+    //
+    return FvProtocol->ReadFile (
+                         FvProtocol,
+                         &FvFileInfo->NameGuid,
+                         NULL,
+                         (UINTN*)&FvFileInfo->FileInfo.FileSize,
+                         &FoundType,
+                         &Attributes,
+                         &AuthenticationStatus
+                         );
+  }
+
+  return Status;
+}
+
+/**
+  Helper function to read a file.
+
+  The data returned depends on the type of the underlying FV file:
+  - For executable types, the first section found that contains executable code is returned.
+  - For files of type FREEFORM, the driver attempts to return the first section of type RAW.
+    If none is found, the entire contents of the FV file are returned.
+  - On all other files the entire contents of the FV file is returned, as by
+    EFI_FIRMWARE_VOLUME2_PROTOCOL.ReadFile.
+
+  @param  FvProtocol                  A pointer to the EFI_FIRMWARE_VOLUME2_PROTOCOL instance.
+  @param  FvFileInfo                  A pointer to the FV_FILESYSTEM_FILE_INFO instance that is a struct
+                                      representing a file's info.
+  @param  BufferSize                  Pointer to a caller-allocated UINTN. It indicates the size of
+                                      the memory represented by *Buffer.
+  @param  Buffer                      Pointer to a pointer to a data buffer to contain file content.
+
+  @retval EFI_SUCCESS                 The call completed successfully.
+  @retval EFI_WARN_BUFFER_TOO_SMALL   The buffer is too small to contain the requested output.
+  @retval EFI_ACCESS_DENIED           The firmware volume is configured to disallow reads.
+  @retval EFI_NOT_FOUND               The requested file was not found in the firmware volume.
+  @retval EFI_DEVICE_ERROR            A hardware error occurred when attempting toaccess the firmware volume.
+
+**/
+EFI_STATUS
+FvFsReadFile (
+  IN     EFI_FIRMWARE_VOLUME2_PROTOCOL     *FvProtocol,
+  IN     FV_FILESYSTEM_FILE_INFO           *FvFileInfo,
+  IN OUT UINTN                             *BufferSize,
+  IN OUT VOID                              **Buffer
+  )
+{
+  UINT32                         AuthenticationStatus;
+  EFI_FV_FILETYPE                FoundType;
+  EFI_FV_FILE_ATTRIBUTES         Attributes;
+  EFI_STATUS                     Status;
+
+  if (FV_FILETYPE_IS_EXECUTABLE (FvFileInfo->Type)) {
+    //
+    // Read the first executable section out of the file.
+    //
+    Status = FvFsFindExecutableSection (FvProtocol, FvFileInfo, BufferSize, Buffer);
+  } else if (FvFileInfo->Type == EFI_FV_FILETYPE_FREEFORM) {
+    //
+    // Try to read a raw section out of the file
+    //
+    Status = FvProtocol->ReadSection (
+                           FvProtocol,
+                           &FvFileInfo->NameGuid,
+                           EFI_SECTION_RAW,
+                           0,
+                           Buffer,
+                           BufferSize,
+                           &AuthenticationStatus
+                           );
+    if (EFI_ERROR (Status)) {
+      //
+      // Didn't find a raw section, just return the whole file.
+      //
+      Status = FvProtocol->ReadFile (
+                             FvProtocol,
+                             &FvFileInfo->NameGuid,
+                             Buffer,
+                             BufferSize,
+                             &FoundType,
+                             &Attributes,
+                             &AuthenticationStatus
+                             );
+    }
+  } else {
+    //
+    // Read the entire file
+    //
+    Status = FvProtocol->ReadFile (
+                           FvProtocol,
+                           &FvFileInfo->NameGuid,
+                           Buffer,
+                           BufferSize,
+                           &FoundType,
+                           &Attributes,
+                           &AuthenticationStatus
+                           );
+  }
+
+  return Status;
+}
+
+/**
+  Helper function for populating an EFI_FILE_INFO for a file.
+
+  Note the CreateTime, LastAccessTime and ModificationTime fields in EFI_FILE_INFO
+  are full zero as FV2 protocol has no corresponding info to fill.
+
+  @param  FvFileInfo                  A pointer to the FV_FILESYSTEM_FILE_INFO instance that is a struct
+                                      representing a file's info.
+  @param  BufferSize                  Pointer to a caller-allocated UINTN. It indicates the size of
+                                      the memory represented by FileInfo.
+  @param  FileInfo                    A pointer to EFI_FILE_INFO to contain the returned file info.
+
+  @retval EFI_SUCCESS                 The call completed successfully.
+  @retval EFI_BUFFER_TOO_SMALL        The buffer is too small to contain the requested output.
+
+**/
+EFI_STATUS
+FvFsGetFileInfo (
+  IN     FV_FILESYSTEM_FILE_INFO           *FvFileInfo,
+  IN OUT UINTN                             *BufferSize,
+     OUT EFI_FILE_INFO                     *FileInfo
+  )
+{
+  UINTN                      InfoSize;
+
+  InfoSize = (UINTN)FvFileInfo->FileInfo.Size;
+  if (*BufferSize < InfoSize) {
+    *BufferSize = InfoSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Initialize FileInfo
+  //
+  CopyMem (FileInfo, &FvFileInfo->FileInfo, InfoSize);
+
+  *BufferSize = InfoSize;
+  return EFI_SUCCESS;
+}
+
+/**
+  Removes the last directory or file entry in a path by changing the last
+  L'\' to a CHAR_NULL.
+
+  @param  Path      The pointer to the path to modify.
+
+  @retval FALSE     Nothing was found to remove.
+  @retval TRUE      A directory or file was removed.
+
+**/
+BOOLEAN
+EFIAPI
+RemoveLastItemFromPath (
+  IN OUT CHAR16 *Path
+  )
+{
+  CHAR16        *Walker;
+  CHAR16        *LastSlash;
+  //
+  // get directory name from path... ('chop' off extra)
+  //
+  for ( Walker = Path, LastSlash = NULL
+      ; Walker != NULL && *Walker != CHAR_NULL
+      ; Walker++
+     ){
+    if (*Walker == L'\\' && *(Walker + 1) != CHAR_NULL) {
+      LastSlash = Walker + 1;
+    }
+  }
+
+  if (LastSlash != NULL) {
+    *LastSlash = CHAR_NULL;
+    return (TRUE);
+  }
+
+  return (FALSE);
+}
+
+/**
+  Function to clean up paths.
+
+  - Single periods in the path are removed.
+  - Double periods in the path are removed along with a single parent directory.
+  - Forward slashes L'/' are converted to backward slashes L'\'.
+
+  This will be done inline and the existing buffer may be larger than required
+  upon completion.
+
+  @param  Path          The pointer to the string containing the path.
+
+  @retval NULL          An error occured.
+  @return Path in all other instances.
+
+**/
+CHAR16*
+EFIAPI
+TrimFilePathToAbsolutePath (
+  IN CHAR16 *Path
+  )
+{
+  CHAR16  *TempString;
+  UINTN   TempSize;
+
+  if (Path == NULL) {
+    return NULL;
+  }
+
+  //
+  // Fix up the '/' vs '\'
+  //
+  for (TempString = Path ; (TempString != NULL) && (*TempString != CHAR_NULL); TempString++) {
+    if (*TempString == L'/') {
+      *TempString = L'\\';
+    }
+  }
+
+  //
+  // Fix up the ..
+  //
+  while ((TempString = StrStr (Path, L"\\..\\")) != NULL) {
+    *TempString  = CHAR_NULL;
+    TempString  += 4;
+    RemoveLastItemFromPath (Path);
+    TempSize     = StrSize (TempString);
+    CopyMem (Path + StrLen (Path), TempString, TempSize);
+  }
+
+  if (((TempString = StrStr (Path, L"\\..")) != NULL) && (*(TempString + 3) == CHAR_NULL)) {
+    *TempString  = CHAR_NULL;
+    RemoveLastItemFromPath (Path);
+  }
+
+  //
+  // Fix up the .
+  //
+  while ((TempString = StrStr (Path, L"\\.\\")) != NULL) {
+    *TempString  = CHAR_NULL;
+    TempString  += 2;
+    TempSize     = StrSize (TempString);
+    CopyMem(Path + StrLen (Path), TempString, TempSize);
+  }
+
+  if (((TempString = StrStr (Path, L"\\.")) != NULL) && (*(TempString + 2) == CHAR_NULL)) {
+    *(TempString + 1) = CHAR_NULL;
+  }
+
+  while ((TempString = StrStr (Path, L"\\\\")) != NULL) {
+    *TempString  = CHAR_NULL;
+    TempString  += 1;
+    TempSize     = StrSize(TempString);
+    CopyMem(Path + StrLen(Path), TempString, TempSize);
+  }
+
+  if (((TempString = StrStr(Path, L"\\\\")) != NULL) && (*(TempString + 1) == CHAR_NULL)) {
+    *(TempString) = CHAR_NULL;
+  }
+
+  return Path;
+}
+
+/**
+  Opens a new file relative to the source file's location.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to the source location. This would typically be an open
+                     handle to a directory.
+  @param  NewHandle  A pointer to the location to return the opened handle for the new
+                     file.
+  @param  FileName   The Null-terminated string of the name of the file to be opened.
+                     The file name may contain the following path modifiers: "\", ".",
+                     and "..".
+  @param  OpenMode   The mode to open the file. The only valid combinations that the
+                     file may be opened with are: Read, Read/Write, or Create/Read/Write.
+  @param  Attributes Only valid for EFI_FILE_MODE_CREATE, in which case these are the
+                     attribute bits for the newly created file.
+
+  @retval EFI_SUCCESS          The file was opened.
+  @retval EFI_NOT_FOUND        The specified file could not be found on the device.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_MEDIA_CHANGED    The device has a different medium in it or the medium is no
+                               longer supported.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  An attempt was made to create a file, or open a file for write
+                               when the media is write-protected.
+  @retval EFI_ACCESS_DENIED    The service denied access to the file.
+  @retval EFI_OUT_OF_RESOURCES Not enough resources were available to open the file.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemOpen (
+  IN     EFI_FILE_PROTOCOL    *This,
+     OUT EFI_FILE_PROTOCOL    **NewHandle,
+  IN     CHAR16               *FileName,
+  IN     UINT64               OpenMode,
+  IN     UINT64               Attributes
+  )
+{
+  FV_FILESYSTEM_INSTANCE      *Instance;
+  FV_FILESYSTEM_FILE          *File;
+  FV_FILESYSTEM_FILE          *NewFile;
+  FV_FILESYSTEM_FILE_INFO     *FvFileInfo;
+  LIST_ENTRY                  *FvFileInfoLink;
+  EFI_STATUS                  Status;
+  UINTN                       FileNameLength;
+  UINTN                       NewFileNameLength;
+  CHAR16                      *FileNameWithExtension;
+
+  //
+  // Check for a valid mode
+  //
+  switch (OpenMode) {
+  case EFI_FILE_MODE_READ:
+    break;
+
+  default:
+    return EFI_WRITE_PROTECTED;
+  }
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  FileName = TrimFilePathToAbsolutePath (FileName);
+  if (FileName == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (FileName[0] == L'\\') {
+    FileName++;
+  }
+
+  //
+  // Check for opening root
+  //
+  if (StrCmp (FileName, L".") == 0 || StrCmp (FileName, L"") == 0) {
+    NewFile = AllocateZeroPool (sizeof (FV_FILESYSTEM_FILE));
+    if (NewFile == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    NewFile->Signature = FVFS_FILE_SIGNATURE;
+    NewFile->Instance  = Instance;
+    NewFile->FvFileInfo = File->FvFileInfo;
+    CopyMem (&NewFile->FileProtocol, &mFileSystemTemplate, sizeof (mFileSystemTemplate));
+    InitializeListHead (&NewFile->Link);
+    InsertHeadList (&Instance->FileHead, &NewFile->Link);
+
+    NewFile->DirReadNext = FVFS_GET_FIRST_FILE_INFO (Instance);
+
+    *NewHandle = &NewFile->FileProtocol;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Do a linear search for a file in the FV with a matching filename
+  //
+  Status     = EFI_NOT_FOUND;
+  FvFileInfo = NULL;
+  for (FvFileInfoLink = GetFirstNode (&Instance->FileInfoHead);
+      !IsNull (&Instance->FileInfoHead, FvFileInfoLink);
+       FvFileInfoLink = GetNextNode (&Instance->FileInfoHead, FvFileInfoLink)) {
+    FvFileInfo = FVFS_FILE_INFO_FROM_LINK (FvFileInfoLink);
+    if (mUnicodeCollation->StriColl (mUnicodeCollation, &FvFileInfo->FileInfo.FileName[0], FileName) == 0) {
+      Status = EFI_SUCCESS;
+      break;
+    }
+  }
+
+  // If the file has not been found check if the filename exists with an extension
+  // in case there was no extension present.
+  // FvFileSystem adds a 'virtual' extension '.EFI' to EFI applications and drivers
+  // present in the Firmware Volume
+  if (Status == EFI_NOT_FOUND) {
+    FileNameLength = StrLen (FileName);
+
+    // Does the filename already contain the '.EFI' extension?
+    if (mUnicodeCollation->StriColl (mUnicodeCollation, FileName + FileNameLength - 4, L".efi") != 0) {
+      // No, there was no extension. So add one and search again for the file
+      // NewFileNameLength = FileNameLength + 1 + 4 = (Number of non-null character) + (file extension) + (a null character)
+      NewFileNameLength = FileNameLength + 1 + 4;
+      FileNameWithExtension = AllocateCopyPool (NewFileNameLength * 2, FileName);
+      StrCatS (FileNameWithExtension, NewFileNameLength, L".EFI");
+
+      for (FvFileInfoLink = GetFirstNode (&Instance->FileInfoHead);
+          !IsNull (&Instance->FileInfoHead, FvFileInfoLink);
+           FvFileInfoLink = GetNextNode (&Instance->FileInfoHead, FvFileInfoLink)) {
+        FvFileInfo = FVFS_FILE_INFO_FROM_LINK (FvFileInfoLink);
+        if (mUnicodeCollation->StriColl (mUnicodeCollation, &FvFileInfo->FileInfo.FileName[0], FileNameWithExtension) == 0) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      }
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    NewFile = AllocateZeroPool (sizeof (FV_FILESYSTEM_FILE));
+    if (NewFile == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NewFile->Signature = FVFS_FILE_SIGNATURE;
+    NewFile->Instance  = Instance;
+    NewFile->FvFileInfo = FvFileInfo;
+    CopyMem (&NewFile->FileProtocol, &mFileSystemTemplate, sizeof (mFileSystemTemplate));
+    InitializeListHead (&NewFile->Link);
+    InsertHeadList (&Instance->FileHead, &NewFile->Link);
+
+    *NewHandle = &NewFile->FileProtocol;
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Closes a specified file handle.
+
+  @param  This          A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                        handle to close.
+
+  @retval EFI_SUCCESS   The file was closed.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemClose (
+  IN EFI_FILE_PROTOCOL  *This
+  )
+{
+  FV_FILESYSTEM_INSTANCE      *Instance;
+  FV_FILESYSTEM_FILE          *File;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  if (File != Instance->Root) {
+    RemoveEntryList (&File->Link);
+    FreePool (File);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads data from a file.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to read data from.
+  @param  BufferSize On input, the size of the Buffer. On output, the amount of data
+                     returned in Buffer. In both cases, the size is measured in bytes.
+  @param  Buffer     The buffer into which the data is read.
+
+  @retval EFI_SUCCESS          Data was read.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_DEVICE_ERROR     An attempt was made to read from a deleted file.
+  @retval EFI_DEVICE_ERROR     On entry, the current file position is beyond the end of the file.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_BUFFER_TOO_SMALL The BufferSize is too small to read the current directory
+                               entry. BufferSize has been updated with the size
+                               needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemRead (
+  IN     EFI_FILE_PROTOCOL      *This,
+  IN OUT UINTN                  *BufferSize,
+     OUT VOID                   *Buffer
+  )
+{
+  FV_FILESYSTEM_INSTANCE        *Instance;
+  FV_FILESYSTEM_FILE            *File;
+  EFI_STATUS                    Status;
+  LIST_ENTRY                    *FvFileInfoLink;
+  VOID                          *FileBuffer;
+  UINTN                         FileSize;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  if (File->FvFileInfo == Instance->Root->FvFileInfo) {
+    if (File->DirReadNext) {
+      //
+      // Directory read: populate Buffer with an EFI_FILE_INFO
+      //
+      Status = FvFsGetFileInfo (File->DirReadNext, BufferSize, Buffer);
+      if (!EFI_ERROR (Status)) {
+        //
+        // Successfully read a directory entry, now update the pointer to the
+        // next file, which will be read on the next call to this function
+        //
+        FvFileInfoLink = GetNextNode (&Instance->FileInfoHead, &File->DirReadNext->Link);
+        if (IsNull (&Instance->FileInfoHead, FvFileInfoLink)) {
+          //
+          // No more files left
+          //
+          File->DirReadNext = NULL;
+        } else {
+          File->DirReadNext = FVFS_FILE_INFO_FROM_LINK (FvFileInfoLink);
+        }
+      }
+      return Status;
+    } else {
+      //
+      // Directory read. All entries have been read, so return a zero-size
+      // buffer.
+      //
+      *BufferSize = 0;
+      return EFI_SUCCESS;
+    }
+  } else {
+    FileSize = (UINTN)File->FvFileInfo->FileInfo.FileSize;
+
+    FileBuffer = AllocateZeroPool (FileSize);
+    if (FileBuffer == NULL) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    Status = FvFsReadFile (File->Instance->FvProtocol, File->FvFileInfo, &FileSize, &FileBuffer);
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (*BufferSize + File->Position > FileSize) {
+      *BufferSize = (UINTN)(FileSize - File->Position);
+    }
+
+    CopyMem (Buffer, (UINT8*)FileBuffer + File->Position, *BufferSize);
+    File->Position += *BufferSize;
+
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Writes data to a file.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to write data to.
+  @param  BufferSize On input, the size of the Buffer. On output, the amount of data
+                     actually written. In both cases, the size is measured in bytes.
+  @param  Buffer     The buffer of data to write.
+
+  @retval EFI_SUCCESS          Data was written.
+  @retval EFI_UNSUPPORTED      Writes to open directory files are not supported.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_DEVICE_ERROR     An attempt was made to write to a deleted file.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  The file or medium is write-protected.
+  @retval EFI_ACCESS_DENIED    The file was opened read only.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemWrite (
+  IN     EFI_FILE_PROTOCOL    *This,
+  IN OUT UINTN                *BufferSize,
+  IN     VOID                 *Buffer
+  )
+{
+  FV_FILESYSTEM_INSTANCE        *Instance;
+  FV_FILESYSTEM_FILE            *File;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  if (File->FvFileInfo == Instance->Root->FvFileInfo) {
+    return EFI_UNSUPPORTED;
+  } else {
+    return EFI_WRITE_PROTECTED;
+  }
+}
+
+/**
+  Returns a file's current position.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle to get the current position on.
+  @param  Position        The address to return the file's current position value.
+
+  @retval EFI_SUCCESS      The position was returned.
+  @retval EFI_UNSUPPORTED  The request is not valid on open directories.
+  @retval EFI_DEVICE_ERROR An attempt was made to get the position from a deleted file.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemGetPosition (
+  IN     EFI_FILE_PROTOCOL    *This,
+     OUT UINT64               *Position
+  )
+{
+  FV_FILESYSTEM_INSTANCE        *Instance;
+  FV_FILESYSTEM_FILE            *File;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  if (File->FvFileInfo == Instance->Root->FvFileInfo) {
+    return EFI_UNSUPPORTED;
+  } else {
+    *Position = File->Position;
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Sets a file's current position.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the
+                          file handle to set the requested position on.
+  @param  Position        The byte position from the start of the file to set.
+
+  @retval EFI_SUCCESS      The position was set.
+  @retval EFI_UNSUPPORTED  The seek request for nonzero is not valid on open
+                           directories.
+  @retval EFI_DEVICE_ERROR An attempt was made to set the position of a deleted file.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemSetPosition (
+  IN EFI_FILE_PROTOCOL        *This,
+  IN UINT64                   Position
+  )
+{
+  FV_FILESYSTEM_INSTANCE      *Instance;
+  FV_FILESYSTEM_FILE          *File;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+  Instance = File->Instance;
+
+  if (File->FvFileInfo == Instance->Root->FvFileInfo) {
+    if (Position != 0) {
+      return EFI_UNSUPPORTED;
+    }
+    //
+    // Reset directory position to first entry
+    //
+    File->DirReadNext = FVFS_GET_FIRST_FILE_INFO (Instance);
+  } else if (Position == 0xFFFFFFFFFFFFFFFFull) {
+    File->Position = File->FvFileInfo->FileInfo.FileSize;
+  } else {
+    File->Position = Position;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Flushes all modified data associated with a file to a device.
+
+  @param  This A pointer to the EFI_FILE_PROTOCOL instance that is the file
+               handle to flush.
+
+  @retval EFI_SUCCESS          The data was flushed.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  The file or medium is write-protected.
+  @retval EFI_ACCESS_DENIED    The file was opened read-only.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemFlush (
+  IN EFI_FILE_PROTOCOL  *This
+  )
+{
+  return EFI_WRITE_PROTECTED;
+}
+
+/**
+  Close and delete the file handle.
+
+  @param  This                     A pointer to the EFI_FILE_PROTOCOL instance that is the
+                                   handle to the file to delete.
+
+  @retval EFI_SUCCESS              The file was closed and deleted, and the handle was closed.
+  @retval EFI_WARN_DELETE_FAILURE  The handle was closed, but the file was not deleted.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDelete (
+  IN EFI_FILE_PROTOCOL *This
+  )
+{
+  EFI_STATUS       Status;
+
+  Status = FvSimpleFileSystemClose (This);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_WARN_DELETE_FAILURE;
+}
+
+/**
+  Returns information about a file.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle the requested information is for.
+  @param  InformationType The type identifier for the information being requested.
+  @param  BufferSize      On input, the size of Buffer. On output, the amount of data
+                          returned in Buffer. In both cases, the size is measured in bytes.
+  @param  Buffer          A pointer to the data buffer to return. The buffer's type is
+                          indicated by InformationType.
+
+  @retval EFI_SUCCESS          The information was returned.
+  @retval EFI_UNSUPPORTED      The InformationType is not known.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_BUFFER_TOO_SMALL The BufferSize is too small to read the current directory entry.
+                               BufferSize has been updated with the size needed to complete
+                               the request.
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemGetInfo (
+  IN     EFI_FILE_PROTOCOL    *This,
+  IN     EFI_GUID             *InformationType,
+  IN OUT UINTN                *BufferSize,
+     OUT VOID                 *Buffer
+  )
+{
+  FV_FILESYSTEM_FILE           *File;
+  EFI_FILE_SYSTEM_INFO         *FsInfoOut;
+  EFI_FILE_SYSTEM_VOLUME_LABEL *FsVolumeLabel;
+  FV_FILESYSTEM_INSTANCE       *Instance;
+  UINTN                        Size;
+  EFI_STATUS                   Status;
+
+  File = FVFS_FILE_FROM_FILE_THIS (This);
+
+  if (CompareGuid (InformationType, &gEfiFileSystemInfoGuid)) {
+    //
+    // Return filesystem info
+    //
+    Instance = File->Instance;
+
+    Size = sizeof (EFI_FILE_SYSTEM_INFO) + StrSize (Instance->VolumeLabel) - sizeof (CHAR16);
+
+    if (*BufferSize < Size) {
+      *BufferSize = Size;
+      return EFI_BUFFER_TOO_SMALL;
+    }
+
+    //
+    // Cast output buffer for convenience
+    //
+    FsInfoOut = (EFI_FILE_SYSTEM_INFO *) Buffer;
+
+    CopyMem (FsInfoOut, &mFsInfoTemplate, sizeof (EFI_FILE_SYSTEM_INFO));
+    Status = StrnCpyS ( FsInfoOut->VolumeLabel, 
+                        (*BufferSize - OFFSET_OF (EFI_FILE_SYSTEM_INFO, VolumeLabel)) / sizeof (CHAR16), 
+                        Instance->VolumeLabel, 
+                        StrLen (Instance->VolumeLabel)
+                        );
+    ASSERT_EFI_ERROR (Status);
+    FsInfoOut->Size = Size;
+    return Status;
+  } else if (CompareGuid (InformationType, &gEfiFileInfoGuid)) {
+    //
+    // Return file info
+    //
+    return FvFsGetFileInfo (File->FvFileInfo, BufferSize, (EFI_FILE_INFO *) Buffer);
+  } else if (CompareGuid (InformationType, &gEfiFileSystemVolumeLabelInfoIdGuid)) {
+    //
+    // Return Volume Label
+    //
+    Instance = File->Instance;
+    Size     = sizeof (EFI_FILE_SYSTEM_VOLUME_LABEL) + StrSize (Instance->VolumeLabel) - sizeof (CHAR16);;
+    if (*BufferSize < Size) {
+      *BufferSize = Size;
+      return EFI_BUFFER_TOO_SMALL;
+    }
+
+    FsVolumeLabel = (EFI_FILE_SYSTEM_VOLUME_LABEL*) Buffer;
+    Status        = StrnCpyS (FsVolumeLabel->VolumeLabel, 
+                              (*BufferSize - OFFSET_OF (EFI_FILE_SYSTEM_VOLUME_LABEL, VolumeLabel)) / sizeof (CHAR16),
+                              Instance->VolumeLabel, 
+                              StrLen (Instance->VolumeLabel)
+                              );
+    ASSERT_EFI_ERROR (Status);
+    return Status;
+  } else {
+    return EFI_UNSUPPORTED;
+  }
+}
+
+/**
+  Sets information about a file.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle the information is for.
+  @param  InformationType The type identifier for the information being set.
+  @param  BufferSize      The size, in bytes, of Buffer.
+  @param  Buffer          A pointer to the data buffer to write. The buffer's type is
+                          indicated by InformationType.
+
+  @retval EFI_SUCCESS          The information was set.
+  @retval EFI_UNSUPPORTED      The InformationType is not known.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_INFO_ID and the media is
+                               read-only.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_PROTOCOL_SYSTEM_INFO_ID
+                               and the media is read only.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_SYSTEM_VOLUME_LABEL_ID
+                               and the media is read-only.
+  @retval EFI_ACCESS_DENIED    An attempt is made to change the name of a file to a
+                               file that is already present.
+  @retval EFI_ACCESS_DENIED    An attempt is being made to change the EFI_FILE_DIRECTORY
+                               Attribute.
+  @retval EFI_ACCESS_DENIED    An attempt is being made to change the size of a directory.
+  @retval EFI_ACCESS_DENIED    InformationType is EFI_FILE_INFO_ID and the file was opened
+                               read-only and an attempt is being made to modify a field
+                               other than Attribute.
+  @retval EFI_VOLUME_FULL      The volume is full.
+  @retval EFI_BAD_BUFFER_SIZE  BufferSize is smaller than the size of the type indicated
+                               by InformationType.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemSetInfo (
+  IN EFI_FILE_PROTOCOL        *This,
+  IN EFI_GUID                 *InformationType,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  )
+{
+  if (CompareGuid (InformationType, &gEfiFileSystemInfoGuid) || 
+      CompareGuid (InformationType, &gEfiFileInfoGuid) ||
+      CompareGuid (InformationType, &gEfiFileSystemVolumeLabelInfoIdGuid)) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.uni b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.uni
new file mode 100644
index 0000000000..fc8c9291ab
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystem.uni
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemDxe.inf b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemDxe.inf
new file mode 100644
index 0000000000..a648c2efc8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemDxe.inf
@@ -0,0 +1,74 @@
+## @file
+#  Support for Simple File System over Firmware Volume.
+#
+#  This driver uses the EFI_FIRMWARE_VOLUME2_PROTOCOL to expose files in firmware
+#  volumes via the the EFI_SIMPLE_FILESYSTEM_PROTOCOL and EFI_FILE_PROTOCOL.
+#
+#  It will expose a single directory, containing one file for each file in the firmware
+#  volume. If a file has a UI section, its contents will be used as a filename.
+#  Otherwise, a string representation of the GUID will be used.
+#  Files of an executable type (That is PEIM, DRIVER, COMBINED_PEIM_DRIVER and APPLICATION)
+#  will have ".efi" added to their filename.
+#
+#  Its primary intended use is to be able to start EFI applications embedded in FVs
+#  from the UEFI shell. It is entirely read-only.
+#
+#  Copyright (c) 2014, ARM Ltd. All rights reserved.<BR>
+#  Copyright (c) 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = FvSimpleFileSystem
+  MODULE_UNI_FILE                = FvSimpleFileSystem.uni
+  FILE_GUID                      = 907125c0-a5f1-11e3-a3fe-a3198b49350c
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = FvSimpleFileSystemEntryPoint
+
+[Sources]
+
+  ComponentName.c
+  FvSimpleFileSystem.c
+  FvSimpleFileSystemEntryPoint.c
+  FvSimpleFileSystemInternal.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DevicePathLib
+  MemoryAllocationLib
+  PrintLib
+  UefiDriverEntryPoint
+  UefiLib
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLang              ## SOMETIMES_CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLang                      ## SOMETIMES_CONSUMES
+
+[Guids]
+  gEfiFileInfoGuid                                                         ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiFileSystemInfoGuid                                                   ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiFileSystemVolumeLabelInfoIdGuid                                      ## SOMETIMES_CONSUMES   ## UNDEFINED
+
+[Protocols]
+  gEfiDevicePathProtocolGuid                                               ## TO_START
+  gEfiFirmwareVolume2ProtocolGuid                                          ## TO_START
+  gEfiUnicodeCollationProtocolGuid                                         ## TO_START
+  gEfiUnicodeCollation2ProtocolGuid                                        ## TO_START
+  gEfiSimpleFileSystemProtocolGuid                                         ## BY_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  FvSimpleFileSystemExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemEntryPoint.c b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemEntryPoint.c
new file mode 100644
index 0000000000..975ab88ca1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemEntryPoint.c
@@ -0,0 +1,676 @@
+/** @file
+  This driver uses the EFI_FIRMWARE_VOLUME2_PROTOCOL to expose files in firmware
+  volumes via the the EFI_SIMPLE_FILESYSTEM_PROTOCOL and EFI_FILE_PROTOCOL.
+
+  It will expose a single directory, containing one file for each file in the firmware
+  volume. If a file has a UI section, its contents will be used as a filename.
+  Otherwise, a string representation of the GUID will be used.
+  Files of an executable type (That is PEIM, DRIVER, COMBINED_PEIM_DRIVER and APPLICATION)
+  will have ".efi" added to their filename.
+
+  Its primary intended use is to be able to start EFI applications embedded in FVs
+  from the UEFI shell. It is entirely read-only.
+
+Copyright (c) 2014, ARM Limited. All rights reserved.
+Copyright (c) 2014, 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 "FvSimpleFileSystemInternal.h"
+
+EFI_UNICODE_COLLATION_PROTOCOL          *mUnicodeCollation = NULL;
+
+//
+// A Guid string is 32 hex characters with 4 hyphens and a NULL-terminated char: 37 characters total
+//
+#define GUID_STRING_SIZE                (37 * sizeof (CHAR16))
+
+#define FVFS_VOLUME_LABEL_PREFIX        L"Firmware Volume: "
+#define FVFS_VOLUME_LABEL_SIZE          (sizeof (FVFS_VOLUME_LABEL_PREFIX) + GUID_STRING_SIZE - sizeof (CHAR16))
+#define FVFS_FALLBACK_VOLUME_LABEL      L"Firmware Volume"
+
+//
+// Template for EFI_SIMPLE_FILE_SYSTEM_PROTOCOL data structure.
+//
+EFI_SIMPLE_FILE_SYSTEM_PROTOCOL mSimpleFsTemplate = {
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_REVISION,
+  FvSimpleFileSystemOpenVolume
+};
+
+//
+// Template for EFI_DRIVER_BINDING_PROTOCOL data structure.
+//
+EFI_DRIVER_BINDING_PROTOCOL mDriverBinding = {
+  FvSimpleFileSystemDriverSupported,
+  FvSimpleFileSystemDriverStart,
+  FvSimpleFileSystemDriverStop,
+  0,
+  NULL,
+  NULL
+};
+
+/**
+  Open the root directory on a volume.
+
+  @param  This     A pointer to the volume to open the root directory.
+  @param  RootFile A pointer to the location to return the opened file handle for the
+                   root directory.
+
+  @retval EFI_SUCCESS          The device was opened.
+  @retval EFI_UNSUPPORTED      This volume does not support the requested file system type.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_ACCESS_DENIED    The service denied access to the file.
+  @retval EFI_OUT_OF_RESOURCES The volume was not opened due to lack of resources.
+  @retval EFI_MEDIA_CHANGED    The device has a different medium in it or the medium is no
+                               longer supported. Any existing file handles for this volume are
+                               no longer valid. To access the files on the new medium, the
+                               volume must be reopened with OpenVolume().
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemOpenVolume (
+  IN     EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *This,
+     OUT EFI_FILE_PROTOCOL               **RootFile
+  )
+{
+  EFI_STATUS                      Status;
+  FV_FILESYSTEM_FILE              *Root;
+  CHAR16                          *UiSection;
+  EFI_GUID                        NameGuid;
+  EFI_FV_FILE_ATTRIBUTES          Attributes;
+  UINT32                          Authentication;
+  UINTN                           Key;
+  EFI_FV_FILETYPE                 FileType;
+  UINTN                           Size;
+  FV_FILESYSTEM_INSTANCE          *Instance;
+  FV_FILESYSTEM_FILE_INFO         *FvFileInfo;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL   *FvProtocol;
+  CHAR16                          *Name;
+  UINTN                           NameLen;
+  UINTN                           NumChars;
+  UINTN                           DestMax;
+
+  Instance = FVFS_INSTANCE_FROM_SIMPLE_FS_THIS (This);
+  Status = EFI_SUCCESS;
+
+  if (Instance->Root == NULL) {
+    //
+    // Allocate file structure for root file
+    //
+    Root = AllocateZeroPool (sizeof (FV_FILESYSTEM_FILE));
+    if (Root == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Instance->Root  = Root;
+    Root->Instance  = Instance;
+    Root->Signature = FVFS_FILE_SIGNATURE;
+    CopyMem (&Root->FileProtocol, &mFileSystemTemplate, sizeof (mFileSystemTemplate));
+    Root->FvFileInfo = AllocateZeroPool (sizeof (FV_FILESYSTEM_FILE_INFO));
+    if (Root->FvFileInfo == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+    }
+    Root->FvFileInfo->FileInfo.Size      = sizeof (EFI_FILE_INFO);
+    Root->FvFileInfo->FileInfo.Attribute = EFI_FILE_DIRECTORY | EFI_FILE_READ_ONLY;
+
+    //
+    // Populate the instance's list of files. We consider anything a file that
+    // has a UI_SECTION, which we consider to be its filename.
+    //
+    FvProtocol = Instance->FvProtocol;
+    //
+    // Allocate Key
+    //
+    Key = 0;
+
+    do {
+      FileType = EFI_FV_FILETYPE_ALL;
+
+      Status = FvProtocol->GetNextFile (
+                             FvProtocol,
+                             &Key,
+                             &FileType,
+                             &NameGuid,
+                             &Attributes,
+                             &Size
+                             );
+      if (EFI_ERROR (Status)) {
+        ASSERT (Status == EFI_NOT_FOUND);
+        break;
+      }
+
+      //
+      // Get a file's name: If it has a UI section, use that, otherwise use
+      // its NameGuid.
+      //
+      UiSection = NULL;
+      Status = FvProtocol->ReadSection (
+                             FvProtocol,
+                             &NameGuid,
+                             EFI_SECTION_USER_INTERFACE,
+                             0,
+                             (VOID **)&UiSection,
+                             &Size,
+                             &Authentication
+                             );
+      if (!EFI_ERROR (Status)) {
+        Name = UiSection;
+      } else {
+        Name = AllocateZeroPool (GUID_STRING_SIZE);
+        if (Name == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+        NumChars = UnicodeSPrint (Name, GUID_STRING_SIZE, L"%g", &NameGuid);
+        ASSERT ((NumChars + 1) * sizeof (CHAR16) == GUID_STRING_SIZE);
+      }
+
+      //
+      // Found a file.
+      // Allocate a file structure and populate it.
+      //
+      NameLen = StrSize (Name);
+      if (FV_FILETYPE_IS_EXECUTABLE (FileType)) {
+        NameLen += StrSize (L".efi") - sizeof (CHAR16);
+      }
+
+      FvFileInfo = AllocateZeroPool (sizeof (FV_FILESYSTEM_FILE_INFO) + NameLen - sizeof (CHAR16));
+      if (FvFileInfo == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      FvFileInfo->Signature = FVFS_FILE_INFO_SIGNATURE;
+      InitializeListHead (&FvFileInfo->Link);
+      CopyMem (&FvFileInfo->NameGuid, &NameGuid, sizeof (EFI_GUID));
+      FvFileInfo->Type = FileType;
+
+      //
+      // Add ".efi" to filenames of drivers and applications.
+      //
+      DestMax = NameLen / sizeof (CHAR16);
+      Status  = StrnCpyS (&FvFileInfo->FileInfo.FileName[0], DestMax, Name, StrLen (Name));
+      ASSERT_EFI_ERROR (Status);
+
+      if (FV_FILETYPE_IS_EXECUTABLE (FileType)) {
+        Status  = StrnCatS (&FvFileInfo->FileInfo.FileName[0], DestMax, L".efi", StrLen (L".efi"));
+        ASSERT_EFI_ERROR (Status);
+      }
+
+      FvFileInfo->FileInfo.Size     = sizeof (EFI_FILE_INFO) + NameLen - sizeof (CHAR16);
+      Status = FvFsGetFileSize (FvProtocol, FvFileInfo);
+      ASSERT_EFI_ERROR (Status);
+      FvFileInfo->FileInfo.PhysicalSize = FvFileInfo->FileInfo.FileSize;
+      FvFileInfo->FileInfo.Attribute    = EFI_FILE_READ_ONLY;
+
+      InsertHeadList (&Instance->FileInfoHead, &FvFileInfo->Link);
+
+      FreePool (Name);
+
+    } while (TRUE);
+
+    if (Status == EFI_NOT_FOUND) {
+      Status = EFI_SUCCESS;
+    }
+  }
+
+  Instance->Root->DirReadNext = FVFS_GET_FIRST_FILE_INFO (Instance);
+  *RootFile = &Instance->Root->FileProtocol;
+  return Status;
+}
+
+/**
+  Worker function to initialize Unicode Collation support.
+
+  It tries to locate Unicode Collation (2) protocol and matches it with current
+  platform language code.
+
+  @param  AgentHandle          The handle used to open Unicode Collation (2) protocol.
+  @param  ProtocolGuid         The pointer to Unicode Collation (2) protocol GUID.
+  @param  VariableName         The name of the RFC 4646 or ISO 639-2 language variable.
+  @param  DefaultLanguage      The default language in case the RFC 4646 or ISO 639-2 language is absent.
+
+  @retval EFI_SUCCESS          The Unicode Collation (2) protocol has been successfully located.
+  @retval Others               The Unicode Collation (2) protocol has not been located.
+
+**/
+EFI_STATUS
+InitializeUnicodeCollationSupportWorker (
+  IN       EFI_HANDLE             AgentHandle,
+  IN       EFI_GUID               *ProtocolGuid,
+  IN CONST CHAR16                 *VariableName,
+  IN CONST CHAR8                  *DefaultLanguage
+  )
+{
+  EFI_STATUS                      ReturnStatus;
+  EFI_STATUS                      Status;
+  UINTN                           NumHandles;
+  UINTN                           Index;
+  EFI_HANDLE                      *Handles;
+  EFI_UNICODE_COLLATION_PROTOCOL  *Uci;
+  BOOLEAN                         Iso639Language;
+  CHAR8                           *Language;
+  CHAR8                           *BestLanguage;
+
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  ProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &Handles
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Iso639Language = (BOOLEAN) (ProtocolGuid == &gEfiUnicodeCollationProtocolGuid);
+  GetEfiGlobalVariable2 (VariableName, (VOID**) &Language, NULL);
+
+  ReturnStatus = EFI_UNSUPPORTED;
+  for (Index = 0; Index < NumHandles; Index++) {
+    //
+    // Open Unicode Collation Protocol
+    //
+    Status = gBS->OpenProtocol (
+                    Handles[Index],
+                    ProtocolGuid,
+                    (VOID **) &Uci,
+                    AgentHandle,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Find the best matching matching language from the supported languages
+    // of Unicode Collation (2) protocol.
+    //
+    BestLanguage = GetBestLanguage (
+                     Uci->SupportedLanguages,
+                     Iso639Language,
+                     (Language == NULL) ? "" : Language,
+                     DefaultLanguage,
+                     NULL
+                     );
+    if (BestLanguage != NULL) {
+      FreePool (BestLanguage);
+      mUnicodeCollation = Uci;
+      ReturnStatus = EFI_SUCCESS;
+      break;
+    }
+  }
+
+  if (Language != NULL) {
+    FreePool (Language);
+  }
+
+  FreePool (Handles);
+
+  return ReturnStatus;
+}
+
+/**
+  Initialize Unicode Collation support.
+
+  It tries to locate Unicode Collation 2 protocol and matches it with current
+  platform language code. If for any reason the first attempt fails, it then tries to
+  use Unicode Collation Protocol.
+
+  @param  AgentHandle          The handle used to open Unicode Collation (2) protocol.
+
+  @retval EFI_SUCCESS          The Unicode Collation (2) protocol has been successfully located.
+  @retval Others               The Unicode Collation (2) protocol has not been located.
+
+**/
+EFI_STATUS
+InitializeUnicodeCollationSupport (
+  IN EFI_HANDLE    AgentHandle
+  )
+{
+
+  EFI_STATUS       Status;
+
+  Status = EFI_UNSUPPORTED;
+
+  //
+  // First try to use RFC 4646 Unicode Collation 2 Protocol.
+  //
+  Status = InitializeUnicodeCollationSupportWorker (
+             AgentHandle,
+             &gEfiUnicodeCollation2ProtocolGuid,
+             L"PlatformLang",
+             (CONST CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultPlatformLang)
+             );
+  //
+  // If the attempt to use Unicode Collation 2 Protocol fails, then we fall back
+  // on the ISO 639-2 Unicode Collation Protocol.
+  //
+  if (EFI_ERROR (Status)) {
+    Status = InitializeUnicodeCollationSupportWorker (
+               AgentHandle,
+               &gEfiUnicodeCollationProtocolGuid,
+               L"Lang",
+               (CONST CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultLang)
+               );
+  }
+
+  return Status;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  @param  DriverBinding       Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *DriverBinding,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  return gBS->OpenProtocol (
+                ControllerHandle,
+                &gEfiFirmwareVolume2ProtocolGuid,
+                NULL,
+                gImageHandle,
+                ControllerHandle,
+                EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                );
+}
+
+/**
+  Start this driver on ControllerHandle by opening a FV protocol and
+  installing a SimpleFileSystem protocol on ControllerHandle.
+
+  @param  DriverBinding        Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *DriverBinding,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL    *FvProtocol;
+  FV_FILESYSTEM_INSTANCE           *Instance;
+  EFI_DEVICE_PATH_PROTOCOL         *FvDevicePath;
+  EFI_GUID                         *FvGuid;
+  UINTN                            NumChars;
+
+  Status = InitializeUnicodeCollationSupport (DriverBinding->DriverBindingHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open FV protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  (VOID **) &FvProtocol,
+                  gImageHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Create an instance
+  //
+  Instance = AllocateZeroPool (sizeof (FV_FILESYSTEM_INSTANCE));
+  if (Instance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Instance->Root = NULL;
+  Instance->FvProtocol = FvProtocol;
+  Instance->Signature = FVFS_INSTANCE_SIGNATURE;
+  InitializeListHead (&Instance->FileInfoHead);
+  InitializeListHead (&Instance->FileHead);
+  CopyMem (&Instance->SimpleFs, &mSimpleFsTemplate, sizeof (mSimpleFsTemplate));
+
+  Status = gBS->InstallProtocolInterface(
+                  &ControllerHandle,
+                  &gEfiSimpleFileSystemProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Instance->SimpleFs
+                  );
+
+  //
+  // Decide on a filesystem volume label, which will include the FV's guid.
+  // Get the device path to find the FV's GUID
+  //
+  Instance->VolumeLabel = NULL;
+  Status =  gBS->OpenProtocol (
+                   ControllerHandle,
+                   &gEfiDevicePathProtocolGuid,
+                   (VOID **) &FvDevicePath,
+                   gImageHandle,
+                   ControllerHandle,
+                   EFI_OPEN_PROTOCOL_BY_DRIVER
+                   );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Iterate over device path until we find a firmware volume node
+    //
+    while (!IsDevicePathEndType (FvDevicePath)) {
+      if (DevicePathType (FvDevicePath) == MEDIA_DEVICE_PATH &&
+          DevicePathSubType (FvDevicePath) == MEDIA_PIWG_FW_VOL_DP) {
+        //
+        // Allocate the volume label
+        //
+        Instance->VolumeLabel = AllocateZeroPool (FVFS_VOLUME_LABEL_SIZE);
+        //
+        // Check the allocation was successful
+        //
+        if (Instance->VolumeLabel != NULL) {
+          //
+          // Extract the FV's guid
+          //
+          FvGuid = &((MEDIA_FW_VOL_DEVICE_PATH *) FvDevicePath)->FvName;
+          //
+          // Build the volume label string
+          //
+          NumChars = UnicodeSPrint (
+                       Instance->VolumeLabel,
+                       FVFS_VOLUME_LABEL_SIZE,
+                       FVFS_VOLUME_LABEL_PREFIX L"%g",
+                       FvGuid
+                       );
+          ASSERT ((NumChars + 1) * sizeof (CHAR16) == FVFS_VOLUME_LABEL_SIZE);
+        }
+        break;
+      }
+      FvDevicePath = NextDevicePathNode (FvDevicePath);
+    }
+  }
+  //
+  // If we didn't decide on a volume label, set a fallback one
+  //
+  if (Instance->VolumeLabel == NULL) {
+    Instance->VolumeLabel = AllocateCopyPool (
+                              sizeof (FVFS_FALLBACK_VOLUME_LABEL),
+                              FVFS_FALLBACK_VOLUME_LABEL
+                              );
+  }
+
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle by removing SimpleFileSystem protocol and closing
+  the FV protocol on ControllerHandle.
+
+  @param  DriverBinding     Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *DriverBinding,
+  IN  EFI_HANDLE                        ControllerHandle,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer OPTIONAL
+  )
+{
+  EFI_STATUS                       Status;
+  FV_FILESYSTEM_INSTANCE           *Instance;
+  FV_FILESYSTEM_FILE_INFO          *FvFileInfo;
+  LIST_ENTRY                       *Entry;
+  LIST_ENTRY                       *DelEntry;
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL  *SimpleFile;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleFileSystemProtocolGuid,
+                  (VOID **) &SimpleFile,
+                  DriverBinding->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Instance = FVFS_INSTANCE_FROM_SIMPLE_FS_THIS (SimpleFile);
+
+  if (IsListEmpty (&Instance->FileHead) == FALSE) {
+    //
+    // Not all opened files are closed
+    //
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Close and uninstall protocols.
+  //
+  Status = gBS->CloseProtocol (
+                   ControllerHandle,
+                   &gEfiFirmwareVolume2ProtocolGuid,
+                   gImageHandle,
+                   ControllerHandle
+                   );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->UninstallProtocolInterface (
+                  ControllerHandle,
+                  &gEfiSimpleFileSystemProtocolGuid,
+                  &Instance->SimpleFs
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Free file structures
+  //
+  if (!IsListEmpty (&Instance->FileInfoHead)) {
+    //
+    // Free the Subtask list.
+    //
+    for(Entry = Instance->FileInfoHead.ForwardLink;
+        Entry != (&Instance->FileInfoHead);
+       ) {
+      DelEntry   = Entry;
+      Entry      = Entry->ForwardLink;
+      FvFileInfo = FVFS_FILE_INFO_FROM_LINK (DelEntry);
+
+      RemoveEntryList (DelEntry);
+      FreePool (FvFileInfo);
+    }
+  }
+
+  if (Instance->Root != NULL) {
+    //
+    // Root->Name is statically allocated, no need to free.
+    //
+    if (Instance->Root->FvFileInfo != NULL) {
+      FreePool (Instance->Root->FvFileInfo);
+    }
+    FreePool (Instance->Root);
+  }
+
+  //
+  // Free Instance
+  //
+  if (Instance->VolumeLabel != NULL) {
+    FreePool (Instance->VolumeLabel);
+  }
+  FreePool (Instance);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The user Entry Point for module FvSimpleFileSystem. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemEntryPoint (
+  IN EFI_HANDLE               ImageHandle,
+  IN EFI_SYSTEM_TABLE         *SystemTable
+  )
+{
+  EFI_STATUS Status;
+
+  //
+  // Install driver model protocol(s).
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &mDriverBinding,
+             ImageHandle,
+             &gFvSimpleFileSystemComponentName,
+             &gFvSimpleFileSystemComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemExtra.uni b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemExtra.uni
new file mode 100644
index 0000000000..7988ee5297
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemExtra.uni
diff --git a/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemInternal.h b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemInternal.h
new file mode 100644
index 0000000000..495ba97a59
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/FvSimpleFileSystemDxe/FvSimpleFileSystemInternal.h
@@ -0,0 +1,622 @@
+/** @file
+  The internal header file of FvSimpleFileSystem driver.
+
+Copyright (c) 2014, ARM Limited. All rights reserved.
+Copyright (c) 2014, 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.
+
+**/
+
+#ifndef __FVFS_INTERNAL_H__
+#define __FVFS_INTERNAL_H__
+
+#include <Uefi.h>
+#include <PiDxe.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+#include <Protocol/DriverBinding.h>
+#include <Protocol/FirmwareVolume2.h>
+#include <Protocol/SimpleFileSystem.h>
+#include <Protocol/UnicodeCollation.h>
+
+#include <Guid/FileSystemInfo.h>
+#include <Guid/FileInfo.h>
+#include <Guid/FileSystemVolumeLabelInfo.h>
+
+typedef struct _FV_FILESYSTEM_FILE       FV_FILESYSTEM_FILE;
+typedef struct _FV_FILESYSTEM_FILE_INFO  FV_FILESYSTEM_FILE_INFO;
+typedef struct _FV_FILESYSTEM_INSTANCE   FV_FILESYSTEM_INSTANCE;
+
+//
+// Struct representing an instance of the "filesystem". There will be one of
+// these structs per FV.
+//
+struct _FV_FILESYSTEM_INSTANCE {
+  UINT32                           Signature;
+  LIST_ENTRY                       FileInfoHead;
+  LIST_ENTRY                       FileHead;
+  EFI_DRIVER_BINDING_PROTOCOL      *DriverBinding;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL    *FvProtocol;
+  EFI_SIMPLE_FILE_SYSTEM_PROTOCOL  SimpleFs;
+  FV_FILESYSTEM_FILE               *Root;
+  CHAR16                           *VolumeLabel;
+};
+
+//
+// Struct representing a opening file. Each opening operation on file will 
+// create such an instance except for the "root directory", which will only
+// be created once for each FV.
+//
+struct _FV_FILESYSTEM_FILE {
+  UINT32                           Signature;
+  LIST_ENTRY                       Link;
+  FV_FILESYSTEM_FILE_INFO          *DirReadNext;
+  FV_FILESYSTEM_INSTANCE           *Instance;
+  EFI_FILE_PROTOCOL                FileProtocol;
+  FV_FILESYSTEM_FILE_INFO          *FvFileInfo;
+  UINT64                           Position;
+};
+
+//
+// Struct representing the info of a file.
+//
+struct _FV_FILESYSTEM_FILE_INFO {
+  UINT32                           Signature;
+  LIST_ENTRY                       Link;
+  EFI_GUID                         NameGuid;
+  EFI_FV_FILETYPE                  Type;
+  EFI_FILE_INFO                    FileInfo;
+};
+
+#define FVFS_FILE_SIGNATURE        SIGNATURE_32 ('f', 'v', 'f', 'i')
+#define FVFS_FILE_INFO_SIGNATURE   SIGNATURE_32 ('f', 'v', 'i', 'n')
+#define FVFS_INSTANCE_SIGNATURE    SIGNATURE_32 ('f', 'v', 'f', 's')
+
+#define FVFS_INSTANCE_FROM_SIMPLE_FS_THIS(This) CR (  \
+          This,                                       \
+          FV_FILESYSTEM_INSTANCE,                     \
+          SimpleFs,                                   \
+          FVFS_INSTANCE_SIGNATURE                     \
+          )
+
+#define FVFS_FILE_FROM_FILE_THIS(This) CR (           \
+          This,                                       \
+          FV_FILESYSTEM_FILE,                         \
+          FileProtocol,                               \
+          FVFS_FILE_SIGNATURE                         \
+          )
+
+#define FVFS_FILE_INFO_FROM_LINK(This) CR (           \
+          This,                                       \
+          FV_FILESYSTEM_FILE_INFO,                    \
+          Link,                                       \
+          FVFS_FILE_INFO_SIGNATURE                    \
+          )
+
+#define FVFS_FILE_FROM_LINK(FileLink) CR (FileLink, FV_FILESYSTEM_FILE, Link, FVFS_FILE_SIGNATURE)
+
+#define FVFS_GET_FIRST_FILE(Instance) FVFS_FILE_FROM_LINK (GetFirstNode (&Instance->FileHead))
+
+#define FVFS_GET_FIRST_FILE_INFO(Instance) FVFS_FILE_INFO_FROM_LINK (GetFirstNode (&Instance->FileInfoHead))
+
+
+#define FV_FILETYPE_IS_EXECUTABLE(Type) ((Type) == EFI_FV_FILETYPE_PEIM                  || \
+                                         (Type) == EFI_FV_FILETYPE_DRIVER                || \
+                                         (Type) == EFI_FV_FILETYPE_COMBINED_PEIM_DRIVER  || \
+                                         (Type) == EFI_FV_FILETYPE_APPLICATION)
+
+/**
+  Open the root directory on a volume.
+
+  @param  This     A pointer to the volume to open the root directory.
+  @param  RootFile A pointer to the location to return the opened file handle for the
+                   root directory.
+
+  @retval EFI_SUCCESS          The device was opened.
+  @retval EFI_UNSUPPORTED      This volume does not support the requested file system type.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_ACCESS_DENIED    The service denied access to the file.
+  @retval EFI_OUT_OF_RESOURCES The volume was not opened due to lack of resources.
+  @retval EFI_MEDIA_CHANGED    The device has a different medium in it or the medium is no
+                               longer supported. Any existing file handles for this volume are
+                               no longer valid. To access the files on the new medium, the
+                               volume must be reopened with OpenVolume().
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemOpenVolume (
+  IN     EFI_SIMPLE_FILE_SYSTEM_PROTOCOL *This,
+     OUT EFI_FILE_PROTOCOL               **RootFile
+  );
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  @param  DriverBinding       Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverSupported (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *DriverBinding,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle by opening a FV protocol and
+  installing a SimpleFileSystem protocol on ControllerHandle.
+
+  @param  DriverBinding        Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverStart (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *DriverBinding,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle by removing SimpleFileSystem protocol and closing
+  the FV protocol on ControllerHandle.
+
+  @param  DriverBinding     Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL       *DriverBinding,
+  IN  EFI_HANDLE                        ControllerHandle,
+  IN  UINTN                             NumberOfChildren,
+  IN  EFI_HANDLE                        *ChildHandleBuffer OPTIONAL
+  );
+
+/**
+  Opens a new file relative to the source file's location.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to the source location. This would typically be an open
+                     handle to a directory.
+  @param  NewHandle  A pointer to the location to return the opened handle for the new
+                     file.
+  @param  FileName   The Null-terminated string of the name of the file to be opened.
+                     The file name may contain the following path modifiers: "\", ".",
+                     and "..".
+  @param  OpenMode   The mode to open the file. The only valid combinations that the
+                     file may be opened with are: Read, Read/Write, or Create/Read/Write.
+  @param  Attributes Only valid for EFI_FILE_MODE_CREATE, in which case these are the
+                     attribute bits for the newly created file.
+
+  @retval EFI_SUCCESS          The file was opened.
+  @retval EFI_NOT_FOUND        The specified file could not be found on the device.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_MEDIA_CHANGED    The device has a different medium in it or the medium is no
+                               longer supported.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  An attempt was made to create a file, or open a file for write
+                               when the media is write-protected.
+  @retval EFI_ACCESS_DENIED    The service denied access to the file.
+  @retval EFI_OUT_OF_RESOURCES Not enough resources were available to open the file.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemOpen (
+  IN     EFI_FILE_PROTOCOL    *This,
+     OUT EFI_FILE_PROTOCOL    **NewHandle,
+  IN     CHAR16               *FileName,
+  IN     UINT64               OpenMode,
+  IN     UINT64               Attributes
+  );
+
+/**
+  Closes a specified file handle.
+
+  @param  This          A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                        handle to close.
+
+  @retval EFI_SUCCESS   The file was closed.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemClose (
+  IN EFI_FILE_PROTOCOL  *This
+  );
+
+/**
+  Reads data from a file.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to read data from.
+  @param  BufferSize On input, the size of the Buffer. On output, the amount of data
+                     returned in Buffer. In both cases, the size is measured in bytes.
+  @param  Buffer     The buffer into which the data is read.
+
+  @retval EFI_SUCCESS          Data was read.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_DEVICE_ERROR     An attempt was made to read from a deleted file.
+  @retval EFI_DEVICE_ERROR     On entry, the current file position is beyond the end of the file.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_BUFFER_TOO_SMALL The BufferSize is too small to read the current directory
+                               entry. BufferSize has been updated with the size
+                               needed to complete the request.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemRead (
+  IN     EFI_FILE_PROTOCOL      *This,
+  IN OUT UINTN                  *BufferSize,
+     OUT VOID                   *Buffer
+  );
+
+/**
+  Writes data to a file.
+
+  @param  This       A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                     handle to write data to.
+  @param  BufferSize On input, the size of the Buffer. On output, the amount of data
+                     actually written. In both cases, the size is measured in bytes.
+  @param  Buffer     The buffer of data to write.
+
+  @retval EFI_SUCCESS          Data was written.
+  @retval EFI_UNSUPPORTED      Writes to open directory files are not supported.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_DEVICE_ERROR     An attempt was made to write to a deleted file.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  The file or medium is write-protected.
+  @retval EFI_ACCESS_DENIED    The file was opened read only.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemWrite (
+  IN     EFI_FILE_PROTOCOL    *This,
+  IN OUT UINTN                *BufferSize,
+  IN     VOID                 *Buffer
+  );
+
+/**
+  Returns a file's current position.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle to get the current position on.
+  @param  Position        The address to return the file's current position value.
+
+  @retval EFI_SUCCESS      The position was returned.
+  @retval EFI_UNSUPPORTED  The request is not valid on open directories.
+  @retval EFI_DEVICE_ERROR An attempt was made to get the position from a deleted file.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemGetPosition (
+  IN     EFI_FILE_PROTOCOL    *This,
+     OUT UINT64               *Position
+  );
+
+/**
+  Sets a file's current position.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the
+                          file handle to set the requested position on.
+  @param  Position        The byte position from the start of the file to set.
+
+  @retval EFI_SUCCESS      The position was set.
+  @retval EFI_UNSUPPORTED  The seek request for nonzero is not valid on open
+                           directories.
+  @retval EFI_DEVICE_ERROR An attempt was made to set the position of a deleted file.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemSetPosition (
+  IN EFI_FILE_PROTOCOL        *This,
+  IN UINT64                   Position
+  );
+
+/**
+  Flushes all modified data associated with a file to a device.
+
+  @param  This A pointer to the EFI_FILE_PROTOCOL instance that is the file
+               handle to flush.
+
+  @retval EFI_SUCCESS          The data was flushed.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  The file or medium is write-protected.
+  @retval EFI_ACCESS_DENIED    The file was opened read-only.
+  @retval EFI_VOLUME_FULL      The volume is full.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemFlush (
+  IN EFI_FILE_PROTOCOL  *This
+  );
+
+/**
+  Close and delete the file handle.
+
+  @param  This                     A pointer to the EFI_FILE_PROTOCOL instance that is the
+                                   handle to the file to delete.
+
+  @retval EFI_SUCCESS              The file was closed and deleted, and the handle was closed.
+  @retval EFI_WARN_DELETE_FAILURE  The handle was closed, but the file was not deleted.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemDelete (
+  IN EFI_FILE_PROTOCOL *This
+  );
+
+/**
+  Returns information about a file.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle the requested information is for.
+  @param  InformationType The type identifier for the information being requested.
+  @param  BufferSize      On input, the size of Buffer. On output, the amount of data
+                          returned in Buffer. In both cases, the size is measured in bytes.
+  @param  Buffer          A pointer to the data buffer to return. The buffer's type is
+                          indicated by InformationType.
+
+  @retval EFI_SUCCESS          The information was returned.
+  @retval EFI_UNSUPPORTED      The InformationType is not known.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_BUFFER_TOO_SMALL The BufferSize is too small to read the current directory entry.
+                               BufferSize has been updated with the size needed to complete
+                               the request.
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemGetInfo (
+  IN     EFI_FILE_PROTOCOL    *This,
+  IN     EFI_GUID             *InformationType,
+  IN OUT UINTN                *BufferSize,
+     OUT VOID                 *Buffer
+  );
+
+/**
+  Sets information about a file.
+
+  @param  This            A pointer to the EFI_FILE_PROTOCOL instance that is the file
+                          handle the information is for.
+  @param  InformationType The type identifier for the information being set.
+  @param  BufferSize      The size, in bytes, of Buffer.
+  @param  Buffer          A pointer to the data buffer to write. The buffer's type is
+                          indicated by InformationType.
+
+  @retval EFI_SUCCESS          The information was set.
+  @retval EFI_UNSUPPORTED      The InformationType is not known.
+  @retval EFI_NO_MEDIA         The device has no medium.
+  @retval EFI_DEVICE_ERROR     The device reported an error.
+  @retval EFI_VOLUME_CORRUPTED The file system structures are corrupted.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_INFO_ID and the media is
+                               read-only.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_PROTOCOL_SYSTEM_INFO_ID
+                               and the media is read only.
+  @retval EFI_WRITE_PROTECTED  InformationType is EFI_FILE_SYSTEM_VOLUME_LABEL_ID
+                               and the media is read-only.
+  @retval EFI_ACCESS_DENIED    An attempt is made to change the name of a file to a
+                               file that is already present.
+  @retval EFI_ACCESS_DENIED    An attempt is being made to change the EFI_FILE_DIRECTORY
+                               Attribute.
+  @retval EFI_ACCESS_DENIED    An attempt is being made to change the size of a directory.
+  @retval EFI_ACCESS_DENIED    InformationType is EFI_FILE_INFO_ID and the file was opened
+                               read-only and an attempt is being made to modify a field
+                               other than Attribute.
+  @retval EFI_VOLUME_FULL      The volume is full.
+  @retval EFI_BAD_BUFFER_SIZE  BufferSize is smaller than the size of the type indicated
+                               by InformationType.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemSetInfo (
+  IN EFI_FILE_PROTOCOL        *This,
+  IN EFI_GUID                 *InformationType,
+  IN UINTN                    BufferSize,
+  IN VOID                     *Buffer
+  );
+
+/**
+  Get the size of the buffer that will be returned by FvFsReadFile.
+
+  @param  FvProtocol                  A pointer to the EFI_FIRMWARE_VOLUME2_PROTOCOL instance.
+  @param  FvFileInfo                  A pointer to the FV_FILESYSTEM_FILE_INFO instance that is a struct
+                                      representing a file's info.
+
+  @retval EFI_SUCCESS                 The file size was gotten correctly.
+  @retval Others                      The file size wasn't gotten correctly.
+
+**/
+EFI_STATUS
+FvFsGetFileSize (
+  IN     EFI_FIRMWARE_VOLUME2_PROTOCOL     *FvProtocol,
+  IN OUT FV_FILESYSTEM_FILE_INFO           *FvFileInfo
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+FvSimpleFileSystemComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+extern EFI_UNICODE_COLLATION_PROTOCOL  *mUnicodeCollation;
+extern EFI_FILE_PROTOCOL               mFileSystemTemplate;
+extern EFI_COMPONENT_NAME_PROTOCOL     gFvSimpleFileSystemComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL    gFvSimpleFileSystemComponentName2;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigKeywordHandler.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigKeywordHandler.c
new file mode 100644
index 0000000000..d1306b3f38
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigKeywordHandler.c
@@ -0,0 +1,3300 @@
+/** @file
+Implementation of interfaces function for EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL.
+
+Copyright (c) 2015, 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 "HiiDatabase.h"
+
+extern HII_DATABASE_PRIVATE_DATA mPrivate;
+
+/**
+  Convert the hex UNICODE %02x encoding of a UEFI device path to binary
+  from <PathHdr> of <MultiKeywordRequest>.
+
+  This is a internal function.
+
+  @param  String                 MultiKeywordRequest string.
+  @param  DevicePathData         Binary of a UEFI device path.
+  @param  NextString             string follow the possible PathHdr string.
+
+  @retval EFI_INVALID_PARAMETER  The device path is not valid or the incoming parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Lake of resources to store neccesary structures.
+  @retval EFI_SUCCESS            The device path is retrieved and translated to binary format.
+                                 The Input string not include PathHdr section.
+
+**/
+EFI_STATUS
+ExtractDevicePath (
+  IN  EFI_STRING                   String,
+  OUT UINT8                        **DevicePathData,
+  OUT EFI_STRING                   *NextString
+  )
+{
+  UINTN                    Length;
+  EFI_STRING               PathHdr;
+  UINT8                    *DevicePathBuffer;
+  CHAR16                   TemStr[2];
+  UINTN                    Index;
+  UINT8                    DigitUint8;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+
+  ASSERT (NextString != NULL && DevicePathData != NULL);
+
+  //
+  // KeywordRequest == NULL case.
+  //
+  if (String == NULL) {
+    *DevicePathData = NULL;
+    *NextString = NULL;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip '&' if exist.
+  //
+  if (*String == L'&') {
+    String ++;
+  }
+
+  //
+  // Find the 'PATH=' of <PathHdr>.
+  //
+  if (StrnCmp (String, L"PATH=", StrLen (L"PATH=")) != 0) {
+    if (StrnCmp (String, L"KEYWORD=", StrLen (L"KEYWORD=")) != 0) {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      //
+      // Not include PathHdr, return success and DevicePath = NULL.
+      //
+      *DevicePathData = NULL;
+      *NextString = String;
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Check whether path data does exist.
+  //
+  String += StrLen (L"PATH=");
+  if (*String == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  PathHdr = String;
+
+  //
+  // The content between 'PATH=' of <ConfigHdr> and '&' of next element
+  // or '\0' (end of configuration string) is the UNICODE %02x bytes encoding
+  // of UEFI device path.
+  //
+  for (Length = 0; *String != 0 && *String != L'&'; String++, Length++);
+
+  //
+  // Save the return next keyword string value.
+  //
+  *NextString = String;
+
+  //
+  // Check DevicePath Length
+  //
+  if (((Length + 1) / 2) < sizeof (EFI_DEVICE_PATH_PROTOCOL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // The data in <PathHdr> is encoded as hex UNICODE %02x bytes in the same order
+  // as the device path resides in RAM memory.
+  // Translate the data into binary.
+  //
+  DevicePathBuffer = (UINT8 *) AllocateZeroPool ((Length + 1) / 2);
+  if (DevicePathBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Convert DevicePath
+  //
+  ZeroMem (TemStr, sizeof (TemStr));
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = PathHdr[Index];
+    DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+    if ((Index & 1) == 0) {
+      DevicePathBuffer [Index/2] = DigitUint8;
+    } else {
+      DevicePathBuffer [Index/2] = (UINT8) ((DevicePathBuffer [Index/2] << 4) + DigitUint8);
+    }
+  }
+  
+  //
+  // Validate DevicePath
+  //
+  DevicePath  = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathBuffer;
+  while (!IsDevicePathEnd (DevicePath)) {
+    if ((DevicePath->Type == 0) || (DevicePath->SubType == 0) || (DevicePathNodeLength (DevicePath) < sizeof (EFI_DEVICE_PATH_PROTOCOL))) {
+      //
+      // Invalid device path
+      //
+      FreePool (DevicePathBuffer);
+      return EFI_INVALID_PARAMETER;
+    }
+    DevicePath = NextDevicePathNode (DevicePath);
+  }
+
+  //
+  // return the device path
+  //
+  *DevicePathData = DevicePathBuffer;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get NameSpace from the input NameSpaceId string.
+
+  This is a internal function.
+
+  @param  String                 <NameSpaceId> format string.
+  @param  NameSpace              Return the name space string.
+  @param  NextString             Return the next string follow namespace.
+
+  @retval   EFI_SUCCESS             Get the namespace string success.
+  @retval   EFI_INVALID_PARAMETER   The NameSpaceId string not follow spec definition.
+
+**/
+EFI_STATUS
+ExtractNameSpace (
+  IN  EFI_STRING                   String,
+  OUT CHAR8                        **NameSpace,
+  OUT EFI_STRING                   *NextString
+  )
+{
+  CHAR16    *TmpPtr;
+  UINTN     NameSpaceSize;
+
+  ASSERT (NameSpace != NULL);
+
+  TmpPtr = NULL;
+
+  //
+  // Input NameSpaceId == NULL
+  //
+  if (String == NULL) {
+    *NameSpace = NULL;
+    if (NextString != NULL) {
+      *NextString = NULL;
+    }
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip '&' if exist.
+  //
+  if (*String == L'&') {
+    String++;
+  }
+
+  if (StrnCmp (String, L"NAMESPACE=", StrLen (L"NAMESPACE=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  String += StrLen (L"NAMESPACE=");
+
+  TmpPtr = StrStr (String, L"&");
+  if (TmpPtr != NULL) {
+    *TmpPtr = 0; 
+  }
+  if (NextString != NULL) {
+    *NextString = String + StrLen (String);
+  }
+
+  //
+  // Input NameSpace is unicode string. The language in String package is ascii string.
+  // Here will convert the unicode string to ascii and save it.
+  //
+  NameSpaceSize = StrLen (String) + 1;
+  *NameSpace = AllocatePool (NameSpaceSize);
+  if (*NameSpace == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  UnicodeStrToAsciiStrS (String, *NameSpace, NameSpaceSize);
+
+  if (TmpPtr != NULL) {
+    *TmpPtr = L'&'; 
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get Keyword from the input KeywordRequest string.
+
+  This is a internal function.
+
+  @param  String                 KeywordRequestformat string.
+  @param  Keyword                return the extract keyword string.
+  @param  NextString             return the next string follow this keyword sectin.
+
+  @retval EFI_SUCCESS            Success to get the keyword string.
+  @retval EFI_INVALID_PARAMETER  Parsr the input string return error.
+
+**/
+EFI_STATUS
+ExtractKeyword (
+  IN  EFI_STRING                   String,
+  OUT EFI_STRING                   *Keyword,
+  OUT EFI_STRING                   *NextString
+  )
+{
+  EFI_STRING  TmpPtr;
+
+  ASSERT ((Keyword != NULL) && (NextString != NULL));
+  
+  TmpPtr = NULL;
+
+  //
+  // KeywordRequest == NULL case.
+  //
+  if (String == NULL) {
+    *Keyword = NULL;
+    *NextString = NULL;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Skip '&' if exist.
+  //
+  if (*String == L'&') {
+    String++;
+  }
+
+  if (StrnCmp (String, L"KEYWORD=", StrLen (L"KEYWORD=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  String += StrLen (L"KEYWORD=");
+  
+  TmpPtr = StrStr (String, L"&");
+  if (TmpPtr != NULL) {
+    *TmpPtr = 0; 
+  }
+  *NextString = String + StrLen (String);
+
+  *Keyword = AllocateCopyPool (StrSize (String), String);
+  if (*Keyword == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (TmpPtr != NULL) {
+    *TmpPtr = L'&';
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Get value from the input KeywordRequest string.
+
+  This is a internal function.
+
+  @param  String                 KeywordRequestformat string.
+  @param  Value                  return the extract value string.
+  @param  NextString             return the next string follow this keyword sectin.
+
+  @retval EFI_SUCCESS            Success to get the keyword string.
+  @retval EFI_INVALID_PARAMETER  Parsr the input string return error.
+
+**/
+EFI_STATUS
+ExtractValue (
+  IN  EFI_STRING                   String,
+  OUT EFI_STRING                   *Value,
+  OUT EFI_STRING                   *NextString
+  )
+{
+  EFI_STRING  TmpPtr;
+
+  ASSERT ((Value != NULL) && (NextString != NULL) && (String != NULL));
+
+  //
+  // Skip '&' if exist.
+  //
+  if (*String == L'&') {
+    String++;
+  }
+
+  if (StrnCmp (String, L"VALUE=", StrLen (L"VALUE=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  String += StrLen (L"VALUE=");
+  
+  TmpPtr = StrStr (String, L"&");
+  if (TmpPtr != NULL) {
+    *TmpPtr = 0; 
+  }
+  *NextString = String + StrLen (String);
+
+  *Value = AllocateCopyPool (StrSize (String), String);
+  if (*Value == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (TmpPtr != NULL) {
+    *TmpPtr = L'&';
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Get filter from the input KeywordRequest string.
+
+  This is a internal function.
+
+  @param  String                 KeywordRequestformat string.
+  @param  FilterFlags            return the filter condition.
+  @param  NextString             return the next string follow this keyword sectin.
+
+  @retval EFI_SUCCESS            Success to get the keyword string.
+  @retval EFI_INVALID_PARAMETER  Parsr the input string return error.
+
+**/
+BOOLEAN
+ExtractFilter (
+  IN  EFI_STRING                   String,
+  OUT UINT8                        *FilterFlags,
+  OUT EFI_STRING                   *NextString
+  )
+{
+  CHAR16      *PathPtr;
+  CHAR16      *KeywordPtr;
+  BOOLEAN     RetVal; 
+
+  ASSERT ((FilterFlags != NULL) && (NextString != NULL));
+
+  //
+  // String end, no filter section.
+  //
+  if (String == NULL) {
+    *NextString = NULL;
+    return FALSE;
+  }
+  
+  *FilterFlags = 0;
+  RetVal = TRUE;
+
+  //
+  // Skip '&' if exist.
+  //
+  if (*String == L'&') {
+    String++;
+  }
+
+  if (StrnCmp (String, L"ReadOnly", StrLen (L"ReadOnly")) == 0) {
+    //
+    // Find ReadOnly filter.
+    //
+    *FilterFlags |= EFI_KEYWORD_FILTER_READONY;
+    String += StrLen (L"ReadOnly");
+  } else if (StrnCmp (String, L"ReadWrite", StrLen (L"ReadWrite")) == 0) {
+    //
+    // Find ReadWrite filter.
+    //
+    *FilterFlags |= EFI_KEYWORD_FILTER_REAWRITE;
+    String += StrLen (L"ReadWrite");
+  } else if (StrnCmp (String, L"Buffer", StrLen (L"Buffer")) == 0) {
+    //
+    // Find Buffer Filter.
+    //
+    *FilterFlags |= EFI_KEYWORD_FILTER_BUFFER;
+    String += StrLen (L"Buffer");
+  } else if (StrnCmp (String, L"Numeric", StrLen (L"Numeric")) == 0) {
+    //
+    // Find Numeric Filter
+    //
+    String += StrLen (L"Numeric");
+    if (*String != L':') {
+      *FilterFlags |= EFI_KEYWORD_FILTER_NUMERIC;
+    } else {
+      String++;
+      switch (*String) {
+      case L'1':
+        *FilterFlags |= EFI_KEYWORD_FILTER_NUMERIC_1;
+        break;
+      case L'2':
+        *FilterFlags |= EFI_KEYWORD_FILTER_NUMERIC_2;
+        break;
+      case L'4':
+        *FilterFlags |= EFI_KEYWORD_FILTER_NUMERIC_4;
+        break;
+      case L'8':
+        *FilterFlags |= EFI_KEYWORD_FILTER_NUMERIC_8;
+        break;
+      default:
+        ASSERT (FALSE);
+        break;
+      }
+      String++;
+    }
+  } else {
+    //
+    // Check whether other filter item defined by Platform.
+    //
+    if ((StrnCmp (String, L"&PATH", StrLen (L"&PATH")) == 0) ||
+        (StrnCmp (String, L"&KEYWORD", StrLen (L"&KEYWORD")) == 0)) {
+      //
+      // New KeywordRequest start, no platform defined filter.
+      //
+    } else {
+      //
+      // Platform defined filter rule.
+      // Just skip platform defined filter rule, return success.
+      //
+      PathPtr = StrStr(String, L"&PATH");
+      KeywordPtr = StrStr(String, L"&KEYWORD");
+      if (PathPtr != NULL && KeywordPtr != NULL) {
+        //
+        // If both sections exist, return the first follow string.
+        //
+        String = KeywordPtr > PathPtr ? PathPtr : KeywordPtr;
+      } else if (PathPtr != NULL) {
+        //
+        // Should not exist PathPtr != NULL && KeywordPtr == NULL case.
+        //
+        ASSERT (FALSE);
+      } else if (KeywordPtr != NULL) {
+        //
+        // Just to the next keyword section.
+        //
+        String = KeywordPtr;
+      } else {
+        //
+        // Only has paltform defined filter section, just skip it.
+        //
+        String += StrLen (String);
+      }
+    }
+    RetVal = FALSE;
+  }
+
+  *NextString = String;
+
+  return RetVal;
+}
+
+/**
+  Extract Readonly flag from opcode.
+
+  This is a internal function.
+
+  @param  OpCodeData             Input opcode for this question.
+
+  @retval TRUE                   This question is readonly.
+  @retval FALSE                  This question is not readonly.
+
+**/
+BOOLEAN
+ExtractReadOnlyFromOpCode (
+  IN  UINT8         *OpCodeData
+  )
+{
+  EFI_IFR_QUESTION_HEADER   *QuestionHdr;
+
+  ASSERT (OpCodeData != NULL);
+
+  QuestionHdr = (EFI_IFR_QUESTION_HEADER *) (OpCodeData + sizeof (EFI_IFR_OP_HEADER));
+
+  return (QuestionHdr->Flags & EFI_IFR_FLAG_READ_ONLY) != 0;
+}
+
+/**
+  Create a circuit to check the filter section.
+
+  This is a internal function.
+
+  @param  OpCodeData             The questin binary ifr data.
+  @param  KeywordRequest         KeywordRequestformat string.
+  @param  NextString             return the next string follow this keyword sectin.
+  @param  ReadOnly               Return whether this question is read only.
+
+  @retval KEYWORD_HANDLER_NO_ERROR                     Success validate.
+  @retval KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED  Validate fail.
+
+**/
+UINT32
+ValidateFilter (
+  IN  UINT8         *OpCodeData,
+  IN  CHAR16        *KeywordRequest,
+  OUT CHAR16        **NextString,
+  OUT BOOLEAN       *ReadOnly
+  )
+{
+  CHAR16                    *NextFilter;
+  CHAR16                    *StringPtr;
+  UINT8                     FilterFlags;
+  EFI_IFR_QUESTION_HEADER   *QuestionHdr;
+  EFI_IFR_OP_HEADER         *OpCodeHdr;
+  UINT8                     Flags;
+  UINT32                    RetVal;
+
+  RetVal = KEYWORD_HANDLER_NO_ERROR;
+  StringPtr = KeywordRequest;
+
+  OpCodeHdr = (EFI_IFR_OP_HEADER *) OpCodeData;
+  QuestionHdr = (EFI_IFR_QUESTION_HEADER *) (OpCodeData + sizeof (EFI_IFR_OP_HEADER));
+  if (OpCodeHdr->OpCode == EFI_IFR_ONE_OF_OP || OpCodeHdr->OpCode == EFI_IFR_NUMERIC_OP) {
+    Flags = *(OpCodeData + sizeof (EFI_IFR_OP_HEADER) + sizeof (EFI_IFR_QUESTION_HEADER));
+  } else {
+    Flags = 0;
+  }
+
+  //
+  // Get ReadOnly flag from Question.
+  // 
+  *ReadOnly = ExtractReadOnlyFromOpCode(OpCodeData);
+
+  while (ExtractFilter (StringPtr, &FilterFlags, &NextFilter)) {
+    switch (FilterFlags) {
+    case EFI_KEYWORD_FILTER_READONY:
+      if ((QuestionHdr->Flags & EFI_IFR_FLAG_READ_ONLY) == 0) {
+        RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+        goto Done;
+      }
+      break;
+
+    case EFI_KEYWORD_FILTER_REAWRITE:
+      if ((QuestionHdr->Flags & EFI_IFR_FLAG_READ_ONLY) != 0) {
+        RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+        goto Done;
+      }
+      break;
+
+    case EFI_KEYWORD_FILTER_BUFFER:
+      //
+      // Only these three opcode use numeric value type.
+      //
+      if (OpCodeHdr->OpCode == EFI_IFR_ONE_OF_OP || OpCodeHdr->OpCode == EFI_IFR_NUMERIC_OP || OpCodeHdr->OpCode == EFI_IFR_CHECKBOX_OP) {
+        RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+        goto Done;
+      }
+      break;
+
+    case EFI_KEYWORD_FILTER_NUMERIC:
+      if (OpCodeHdr->OpCode != EFI_IFR_ONE_OF_OP && OpCodeHdr->OpCode != EFI_IFR_NUMERIC_OP && OpCodeHdr->OpCode != EFI_IFR_CHECKBOX_OP) {
+        RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+        goto Done;
+      }
+      break;
+
+    case EFI_KEYWORD_FILTER_NUMERIC_1:
+    case EFI_KEYWORD_FILTER_NUMERIC_2:
+    case EFI_KEYWORD_FILTER_NUMERIC_4:
+    case EFI_KEYWORD_FILTER_NUMERIC_8:
+      if (OpCodeHdr->OpCode != EFI_IFR_ONE_OF_OP && OpCodeHdr->OpCode != EFI_IFR_NUMERIC_OP && OpCodeHdr->OpCode != EFI_IFR_CHECKBOX_OP) {
+        RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+        goto Done;
+      }
+
+      //
+      // For numeric and oneof, it has flags field to specify the detail numeric type.
+      //
+      if (OpCodeHdr->OpCode == EFI_IFR_ONE_OF_OP || OpCodeHdr->OpCode == EFI_IFR_NUMERIC_OP) {
+        switch (Flags & EFI_IFR_NUMERIC_SIZE) {
+        case EFI_IFR_NUMERIC_SIZE_1:
+          if (FilterFlags != EFI_KEYWORD_FILTER_NUMERIC_1) {
+            RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+            goto Done;
+          }
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_2:
+          if (FilterFlags != EFI_KEYWORD_FILTER_NUMERIC_2) {
+            RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+            goto Done;
+          }
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_4:
+          if (FilterFlags != EFI_KEYWORD_FILTER_NUMERIC_4) {
+            RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+            goto Done;
+          }
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_8:
+          if (FilterFlags != EFI_KEYWORD_FILTER_NUMERIC_8) {
+            RetVal = KEYWORD_HANDLER_INCOMPATIBLE_VALUE_DETECTED;
+            goto Done;
+          }
+          break;
+
+        default:
+          ASSERT (FALSE);
+          break;
+        }
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+    }
+
+    //
+    // Jump to the next filter.
+    //
+    StringPtr = NextFilter;
+  }
+  
+Done:
+  //
+  // The current filter which is processing.
+  //
+  *NextString = StringPtr;
+
+  return RetVal;
+}
+
+/**
+  Get HII_DATABASE_RECORD from the input device path info.
+
+  This is a internal function.
+
+  @param  DevicePath             UEFI device path protocol.
+
+  @retval Internal data base record.
+
+**/
+HII_DATABASE_RECORD *
+GetRecordFromDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL   *DevicePath
+  )
+{
+  LIST_ENTRY             *Link;
+  UINT8                  *DevicePathPkg;
+  UINT8                  *CurrentDevicePath;
+  UINTN                  DevicePathSize;
+  HII_DATABASE_RECORD    *TempDatabase;
+
+  ASSERT (DevicePath != NULL);
+
+  for (Link = mPrivate.DatabaseList.ForwardLink; Link != &mPrivate.DatabaseList; Link = Link->ForwardLink) {
+    TempDatabase = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    DevicePathPkg = TempDatabase->PackageList->DevicePathPkg;
+    if (DevicePathPkg != NULL) {
+      CurrentDevicePath = DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER);
+      DevicePathSize = GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) CurrentDevicePath);
+      if ((CompareMem (DevicePath, CurrentDevicePath, DevicePathSize) == 0)) {
+        return TempDatabase;
+      }
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Calculate the size of StringSrc and output it. Also copy string text from src 
+  to dest.
+
+  This is a internal function.
+
+  @param  StringSrc              Points to current null-terminated string.
+  @param  BufferSize             Length of the buffer.
+  @param  StringDest             Buffer to store the string text. 
+
+  @retval EFI_SUCCESS            The string text was outputed successfully.
+  @retval EFI_OUT_OF_RESOURCES   Out of resource.
+
+**/
+EFI_STATUS
+GetUnicodeStringTextAndSize (
+  IN  UINT8            *StringSrc,
+  OUT UINTN            *BufferSize,
+  OUT EFI_STRING       *StringDest
+  )
+{
+  UINTN  StringSize;
+  UINT8  *StringPtr;
+
+  ASSERT (StringSrc != NULL && BufferSize != NULL && StringDest != NULL);
+
+  StringSize = sizeof (CHAR16);
+  StringPtr  = StringSrc;
+  while (ReadUnaligned16 ((UINT16 *) StringPtr) != 0) {
+    StringSize += sizeof (CHAR16);
+    StringPtr += sizeof (CHAR16);
+  }
+
+  *StringDest = AllocatePool (StringSize);
+  if (*StringDest == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  CopyMem (*StringDest, StringSrc, StringSize);
+
+  *BufferSize = StringSize;
+  return EFI_SUCCESS;
+}
+
+/**
+  Find the string id for the input keyword.
+
+  @param  StringPackage           Hii string package instance.
+  @param  KeywordValue            Input keyword value.
+  @param  StringId                The string's id, which is unique within PackageList.
+
+
+  @retval EFI_SUCCESS             The string text and font is retrieved
+                                  successfully.
+  @retval EFI_NOT_FOUND           The specified text or font info can not be found
+                                  out.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+**/
+EFI_STATUS
+GetStringIdFromString (
+  IN HII_STRING_PACKAGE_INSTANCE      *StringPackage,
+  IN CHAR16                           *KeywordValue,
+  OUT EFI_STRING_ID                   *StringId
+  )
+{
+  UINT8                                *BlockHdr;
+  EFI_STRING_ID                        CurrentStringId;
+  UINTN                                BlockSize;
+  UINTN                                Index;
+  UINT8                                *StringTextPtr;
+  UINTN                                Offset;
+  UINT16                               StringCount;
+  UINT16                               SkipCount;
+  UINT8                                Length8;
+  EFI_HII_SIBT_EXT2_BLOCK              Ext2;
+  UINT32                               Length32;
+  UINTN                                StringSize;
+  CHAR16                               *String;
+  CHAR8                                *AsciiKeywordValue;
+  UINTN                                KeywordValueSize;
+  EFI_STATUS                           Status;
+
+  ASSERT (StringPackage != NULL && KeywordValue != NULL && StringId != NULL);
+  ASSERT (StringPackage->Signature == HII_STRING_PACKAGE_SIGNATURE);
+
+  CurrentStringId = 1;
+  Status = EFI_SUCCESS;
+  String = NULL;
+  BlockHdr = StringPackage->StringBlock;
+  BlockSize = 0;
+  Offset = 0;
+
+  //
+  // Make a ascii keyword value for later use.
+  //
+  KeywordValueSize = StrLen (KeywordValue) + 1;
+  AsciiKeywordValue = AllocatePool (KeywordValueSize);
+  if (AsciiKeywordValue == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  UnicodeStrToAsciiStrS (KeywordValue, AsciiKeywordValue, KeywordValueSize);
+
+  while (*BlockHdr != EFI_HII_SIBT_END) {
+    switch (*BlockHdr) {
+    case EFI_HII_SIBT_STRING_SCSU:
+      Offset = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      if (AsciiStrCmp(AsciiKeywordValue, (CHAR8 *) StringTextPtr) == 0) {
+        *StringId = CurrentStringId;
+        goto Done;
+      }
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_SCSU_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_SCSU_FONT_BLOCK) - sizeof (UINT8);
+      StringTextPtr = BlockHdr + Offset;
+      if (AsciiStrCmp(AsciiKeywordValue, (CHAR8 *) StringTextPtr) == 0) {
+        *StringId = CurrentStringId;
+        goto Done;
+      }
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU:
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        if (AsciiStrCmp(AsciiKeywordValue, (CHAR8 *) StringTextPtr) == 0) {
+          *StringId = CurrentStringId;
+          goto Done;
+        }
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_FONT_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        if (AsciiStrCmp(AsciiKeywordValue, (CHAR8 *) StringTextPtr) == 0) {
+          *StringId = CurrentStringId;
+          goto Done;
+        }
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2:
+      Offset        = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StringSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      Status = GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      ASSERT (String != NULL);
+      if (StrCmp(KeywordValue, String) == 0) {
+        *StringId = CurrentStringId;
+        goto Done;
+      }
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK)  - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StringSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      Status = GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      ASSERT (String != NULL);
+      if (StrCmp(KeywordValue, String) == 0) {
+        *StringId = CurrentStringId;
+        goto Done;
+      }
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      for (Index = 0; Index < StringCount; Index++) {
+        Status = GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+        ASSERT (String != NULL);
+        BlockSize += StringSize;
+        if (StrCmp(KeywordValue, String) == 0) {
+          *StringId = CurrentStringId;
+          goto Done;
+        }
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_FONT_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      for (Index = 0; Index < StringCount; Index++) {
+        Status = GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+        ASSERT (String != NULL);
+        BlockSize += StringSize;
+        if (StrCmp(KeywordValue, String) == 0) {
+          *StringId = CurrentStringId;
+          goto Done;
+        }
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_DUPLICATE:
+      BlockSize       += sizeof (EFI_HII_SIBT_DUPLICATE_BLOCK);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_SKIP1:
+      SkipCount = (UINT16) (*(UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK)));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP1_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_SKIP2:
+      CopyMem (&SkipCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_EXT1:
+      CopyMem (
+        &Length8,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT8)
+        );
+      BlockSize += Length8;
+      break;
+
+    case EFI_HII_SIBT_EXT2:
+      CopyMem (&Ext2, BlockHdr, sizeof (EFI_HII_SIBT_EXT2_BLOCK));
+      BlockSize += Ext2.Length;
+      break;
+
+    case EFI_HII_SIBT_EXT4:
+      CopyMem (
+        &Length32,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT32)
+        );
+
+      BlockSize += Length32;
+      break;
+
+    default:
+      break;
+    }
+
+    if (String != NULL) {
+      FreePool (String);
+      String = NULL;
+    }
+
+    BlockHdr  = StringPackage->StringBlock + BlockSize;
+  }
+
+  Status = EFI_NOT_FOUND;
+
+Done:
+  if (AsciiKeywordValue != NULL) {
+    FreePool (AsciiKeywordValue);
+  }
+  if (String != NULL) {
+    FreePool (String);
+  }
+  return Status;
+}
+
+/**
+  Find the next valid string id for the input string id.
+
+  @param  StringPackage           Hii string package instance.
+  @param  StringId                The current string id which is already got.
+                                  1 means just begin to get the string id.
+  @param  KeywordValue            Return the string for the next string id.
+
+
+  @retval EFI_STRING_ID           Not 0 means a valid stringid found.
+                                  0 means not found a valid string id.
+**/
+EFI_STRING_ID
+GetNextStringId (
+  IN  HII_STRING_PACKAGE_INSTANCE      *StringPackage,
+  IN  EFI_STRING_ID                    StringId,
+  OUT EFI_STRING                       *KeywordValue
+  )
+{
+  UINT8                                *BlockHdr;
+  EFI_STRING_ID                        CurrentStringId;
+  UINTN                                BlockSize;
+  UINTN                                Index;
+  UINT8                                *StringTextPtr;
+  UINTN                                Offset;
+  UINT16                               StringCount;
+  UINT16                               SkipCount;
+  UINT8                                Length8;
+  EFI_HII_SIBT_EXT2_BLOCK              Ext2;
+  UINT32                               Length32;
+  BOOLEAN                              FindString;
+  UINTN                                StringSize;
+  CHAR16                               *String;
+
+  ASSERT (StringPackage != NULL);
+  ASSERT (StringPackage->Signature == HII_STRING_PACKAGE_SIGNATURE);
+
+  CurrentStringId = 1;
+  FindString = FALSE;
+  String = NULL;
+
+  //
+  // Parse the string blocks to get the string text and font.
+  //
+  BlockHdr  = StringPackage->StringBlock;
+  BlockSize = 0;
+  Offset    = 0;
+  while (*BlockHdr != EFI_HII_SIBT_END) {
+    switch (*BlockHdr) {
+    case EFI_HII_SIBT_STRING_SCSU:
+      Offset = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      
+      if (FindString) {
+        StringSize = AsciiStrSize ((CHAR8 *) StringTextPtr);
+        *KeywordValue = AllocatePool (StringSize * sizeof (CHAR16));
+        if (*KeywordValue == NULL) {
+          return 0;
+        }
+        AsciiStrToUnicodeStrS ((CHAR8 *) StringTextPtr, *KeywordValue, StringSize);
+        return CurrentStringId;
+      } else if (CurrentStringId == StringId) {
+        FindString = TRUE;
+      }
+
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_SCSU_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_SCSU_FONT_BLOCK) - sizeof (UINT8);
+      StringTextPtr = BlockHdr + Offset;
+      
+      if (FindString) {
+        StringSize = AsciiStrSize ((CHAR8 *) StringTextPtr);
+        *KeywordValue = AllocatePool (StringSize * sizeof (CHAR16));
+        if (*KeywordValue == NULL) {
+          return 0;
+        }
+        AsciiStrToUnicodeStrS ((CHAR8 *) StringTextPtr, *KeywordValue, StringSize);
+        return CurrentStringId;
+      } else if (CurrentStringId == StringId) {
+        FindString = TRUE;
+      }
+
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU:
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        if (FindString) {
+          StringSize = AsciiStrSize ((CHAR8 *) StringTextPtr);
+          *KeywordValue = AllocatePool (StringSize * sizeof (CHAR16));
+          if (*KeywordValue == NULL) {
+            return 0;
+          }
+          AsciiStrToUnicodeStrS ((CHAR8 *) StringTextPtr, *KeywordValue, StringSize);
+          return CurrentStringId;
+        } else if (CurrentStringId == StringId) {
+          FindString = TRUE;
+        }
+
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_FONT_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        if (FindString) {
+          StringSize = AsciiStrSize ((CHAR8 *) StringTextPtr);
+          *KeywordValue = AllocatePool (StringSize * sizeof (CHAR16));
+          if (*KeywordValue == NULL) {
+            return 0;
+          }
+          AsciiStrToUnicodeStrS ((CHAR8 *) StringTextPtr, *KeywordValue, StringSize);
+          return CurrentStringId;
+        } else if (CurrentStringId == StringId) {
+          FindString = TRUE;
+        }
+
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2:
+      Offset        = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StringSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+      if (FindString && (String != NULL) && (*String != L'\0')) {
+        //
+        // String protocol use this type for the string id which has value for other package.
+        // It will allocate an empty string block for this string id. so here we also check
+        // *String != L'\0' to prohibit this case.
+        //
+        *KeywordValue = String;
+        return CurrentStringId;
+      } else if (CurrentStringId == StringId) {
+        FindString = TRUE;
+      }
+
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK)  - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StringSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+      if (FindString) {
+        *KeywordValue = String;
+        return CurrentStringId;
+      } else if (CurrentStringId == StringId) {
+        FindString = TRUE;
+      }
+
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      for (Index = 0; Index < StringCount; Index++) {
+        GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);        
+
+        if (FindString) {
+          *KeywordValue = String;
+          return CurrentStringId;
+        } else if (CurrentStringId == StringId) {
+          FindString = TRUE;
+        }
+
+        BlockSize += StringSize;
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_FONT_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      for (Index = 0; Index < StringCount; Index++) {
+        GetUnicodeStringTextAndSize (StringTextPtr, &StringSize, &String);
+        if (FindString) {
+          *KeywordValue = String;
+          return CurrentStringId;
+        } else if (CurrentStringId == StringId) {
+          FindString = TRUE;
+        }
+
+        BlockSize += StringSize;
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_DUPLICATE:
+      BlockSize       += sizeof (EFI_HII_SIBT_DUPLICATE_BLOCK);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_SKIP1:
+      SkipCount = (UINT16) (*(UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK)));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP1_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_SKIP2:
+      CopyMem (&SkipCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_EXT1:
+      CopyMem (
+        &Length8,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT8)
+        );
+      BlockSize += Length8;
+      break;
+
+    case EFI_HII_SIBT_EXT2:
+      CopyMem (&Ext2, BlockHdr, sizeof (EFI_HII_SIBT_EXT2_BLOCK));
+      BlockSize += Ext2.Length;
+      break;
+
+    case EFI_HII_SIBT_EXT4:
+      CopyMem (
+        &Length32,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT32)
+        );
+
+      BlockSize += Length32;
+      break;
+
+    default:
+      break;
+    }
+
+    if (String != NULL) {
+      FreePool (String);
+      String = NULL;
+    }
+
+    BlockHdr  = StringPackage->StringBlock + BlockSize;
+  }
+
+  return 0;
+}
+
+/**
+  Get string package from the input NameSpace string.
+
+  This is a internal function.
+
+  @param  DatabaseRecord                 HII_DATABASE_RECORD format string.
+  @param  NameSpace                      NameSpace format string.
+  @param  KeywordValue                   Keyword value.
+  @param  StringId                       String Id for this keyword.
+
+  @retval KEYWORD_HANDLER_NO_ERROR                     Get String id succes.
+  @retval KEYWORD_HANDLER_KEYWORD_NOT_FOUND            Not found the string id in the string package.
+  @retval KEYWORD_HANDLER_NAMESPACE_ID_NOT_FOUND       Not found the string package for this namespace.
+  @retval KEYWORD_HANDLER_UNDEFINED_PROCESSING_ERROR   Out of resource error.
+
+**/
+UINT32
+GetStringIdFromRecord (
+  IN HII_DATABASE_RECORD   *DatabaseRecord,
+  IN CHAR8                 **NameSpace,
+  IN CHAR16                *KeywordValue,
+  OUT EFI_STRING_ID        *StringId
+  )
+{
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  EFI_STATUS                          Status;
+  CHAR8                               *Name;
+  UINT32                              RetVal;
+
+  ASSERT (DatabaseRecord != NULL && NameSpace != NULL && KeywordValue != NULL);
+
+  PackageListNode = DatabaseRecord->PackageList;
+  RetVal = KEYWORD_HANDLER_NAMESPACE_ID_NOT_FOUND;
+
+  if (*NameSpace != NULL) {
+    Name = *NameSpace;
+  } else {
+    Name = UEFI_CONFIG_LANG;
+  }
+
+  for (Link = PackageListNode->StringPkgHdr.ForwardLink; Link != &PackageListNode->StringPkgHdr; Link = Link->ForwardLink) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+
+    if (AsciiStrnCmp(Name, StringPackage->StringPkgHdr->Language, AsciiStrLen (Name)) == 0) {
+      Status = GetStringIdFromString (StringPackage, KeywordValue, StringId); 
+      if (EFI_ERROR (Status)) {
+        return KEYWORD_HANDLER_KEYWORD_NOT_FOUND;
+      } else {
+        if (*NameSpace == NULL) {
+          *NameSpace = AllocateCopyPool (AsciiStrSize (StringPackage->StringPkgHdr->Language), StringPackage->StringPkgHdr->Language);
+          if (*NameSpace == NULL) {
+            return KEYWORD_HANDLER_UNDEFINED_PROCESSING_ERROR;
+          }
+        }
+        return KEYWORD_HANDLER_NO_ERROR;
+      }
+    }
+  }
+
+  return RetVal;
+}
+
+/**
+  Tell whether this Operand is an Statement OpCode.
+
+  @param  Operand                Operand of an IFR OpCode.
+
+  @retval TRUE                   This is an Statement OpCode.
+  @retval FALSE                  Not an Statement OpCode.
+
+**/
+BOOLEAN
+IsStatementOpCode (
+  IN UINT8              Operand
+  )
+{
+  if ((Operand == EFI_IFR_SUBTITLE_OP) ||
+      (Operand == EFI_IFR_TEXT_OP) ||
+      (Operand == EFI_IFR_RESET_BUTTON_OP) ||
+      (Operand == EFI_IFR_REF_OP) ||
+      (Operand == EFI_IFR_ACTION_OP) ||
+      (Operand == EFI_IFR_NUMERIC_OP) ||
+      (Operand == EFI_IFR_ORDERED_LIST_OP) ||
+      (Operand == EFI_IFR_CHECKBOX_OP) ||
+      (Operand == EFI_IFR_STRING_OP) ||
+      (Operand == EFI_IFR_PASSWORD_OP) ||
+      (Operand == EFI_IFR_DATE_OP) ||
+      (Operand == EFI_IFR_TIME_OP) ||
+      (Operand == EFI_IFR_GUID_OP) ||
+      (Operand == EFI_IFR_ONE_OF_OP)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Tell whether this Operand is an Statement OpCode.
+
+  @param  Operand                Operand of an IFR OpCode.
+
+  @retval TRUE                   This is an Statement OpCode.
+  @retval FALSE                  Not an Statement OpCode.
+
+**/
+BOOLEAN
+IsStorageOpCode (
+  IN UINT8              Operand
+  )
+{
+  if ((Operand == EFI_IFR_VARSTORE_OP) ||
+      (Operand == EFI_IFR_VARSTORE_NAME_VALUE_OP) ||
+      (Operand == EFI_IFR_VARSTORE_EFI_OP)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Base on the prompt string id to find the question.
+
+  @param  FormPackage            The input form package.
+  @param  KeywordStrId           The input prompt string id for one question.
+
+  @retval  the opcode for the question.
+
+**/
+UINT8 * 
+FindQuestionFromStringId (
+  IN HII_IFR_PACKAGE_INSTANCE      *FormPackage,
+  IN EFI_STRING_ID                 KeywordStrId 
+  )
+{
+  UINT8                        *OpCodeData;
+  UINT32                       Offset;
+  EFI_IFR_STATEMENT_HEADER     *StatementHeader;
+  EFI_IFR_OP_HEADER            *OpCodeHeader;
+  UINT32                       FormDataLen;
+
+  ASSERT (FormPackage != NULL);
+
+  FormDataLen = FormPackage->FormPkgHdr.Length - sizeof (EFI_HII_PACKAGE_HEADER);
+  Offset = 0;
+  while (Offset < FormDataLen) {
+    OpCodeData = FormPackage->IfrData + Offset;
+    OpCodeHeader = (EFI_IFR_OP_HEADER *) OpCodeData;
+
+    if (IsStatementOpCode(OpCodeHeader->OpCode)) {
+      StatementHeader = (EFI_IFR_STATEMENT_HEADER *) (OpCodeData + sizeof (EFI_IFR_OP_HEADER));
+      if (StatementHeader->Prompt == KeywordStrId) {
+        return OpCodeData;
+      }
+    }
+
+    Offset += OpCodeHeader->Length;
+  }
+
+  return NULL;
+}
+
+/**
+  Base on the varstore id to find the storage info.
+
+  @param  FormPackage            The input form package.
+  @param  VarStoreId             The input storage id.
+
+  @retval  the opcode for the storage.
+
+**/
+UINT8 *
+FindStorageFromVarId (
+  IN HII_IFR_PACKAGE_INSTANCE      *FormPackage,
+  IN EFI_VARSTORE_ID               VarStoreId
+  )
+{
+  UINT8                        *OpCodeData;
+  UINT32                       Offset;
+  EFI_IFR_OP_HEADER            *OpCodeHeader;
+  UINT32                       FormDataLen;
+
+  ASSERT (FormPackage != NULL);
+
+  FormDataLen = FormPackage->FormPkgHdr.Length - sizeof (EFI_HII_PACKAGE_HEADER);
+  Offset = 0;
+  while (Offset < FormDataLen) {
+    OpCodeData = FormPackage->IfrData + Offset;
+    OpCodeHeader = (EFI_IFR_OP_HEADER *) OpCodeData;
+
+    if (IsStorageOpCode(OpCodeHeader->OpCode)) {
+      switch (OpCodeHeader->OpCode) {
+      case EFI_IFR_VARSTORE_OP:
+        if (VarStoreId == ((EFI_IFR_VARSTORE *) OpCodeData)->VarStoreId) {
+          return OpCodeData;
+        }
+        break;
+
+      case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+        if (VarStoreId == ((EFI_IFR_VARSTORE_NAME_VALUE *) OpCodeData)->VarStoreId) {
+          return OpCodeData;
+        }
+        break;
+
+      case EFI_IFR_VARSTORE_EFI_OP:
+        if (VarStoreId == ((EFI_IFR_VARSTORE_EFI *) OpCodeData)->VarStoreId) {
+          return OpCodeData;
+        }
+        break;
+
+      default:
+        break;
+      }
+    }
+
+    Offset += OpCodeHeader->Length;
+  }
+
+  return NULL;
+}
+
+/**
+  Get width info for one question.
+
+  @param  OpCodeData            The input opcode for one question.
+
+  @retval  the width info for one question.
+
+**/
+UINT16 
+GetWidth (
+  IN UINT8        *OpCodeData
+  )
+{
+  UINT8      *NextOpCodeData;
+
+  ASSERT (OpCodeData != NULL);
+
+  switch (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode) {
+  case EFI_IFR_REF_OP:
+    return (UINT16) sizeof (EFI_HII_REF);
+
+  case EFI_IFR_ONE_OF_OP:
+  case EFI_IFR_NUMERIC_OP:
+    switch (((EFI_IFR_ONE_OF *) OpCodeData)->Flags & EFI_IFR_NUMERIC_SIZE) {
+    case EFI_IFR_NUMERIC_SIZE_1:
+      return (UINT16) sizeof (UINT8);
+    
+    case EFI_IFR_NUMERIC_SIZE_2:
+      return  (UINT16) sizeof (UINT16);
+    
+    case EFI_IFR_NUMERIC_SIZE_4:
+      return (UINT16) sizeof (UINT32);
+    
+    case EFI_IFR_NUMERIC_SIZE_8:
+      return (UINT16) sizeof (UINT64);
+    
+    default:
+      ASSERT (FALSE);
+      return 0;
+    }
+
+  case EFI_IFR_ORDERED_LIST_OP:
+    NextOpCodeData = OpCodeData + ((EFI_IFR_ORDERED_LIST *) OpCodeData)->Header.Length;
+    //
+    // OneOfOption must follow the orderedlist opcode.
+    //
+    ASSERT (((EFI_IFR_OP_HEADER *) NextOpCodeData)->OpCode == EFI_IFR_ONE_OF_OPTION_OP);
+    switch (((EFI_IFR_ONE_OF_OPTION *) NextOpCodeData)->Type) {
+    case EFI_IFR_TYPE_NUM_SIZE_8:
+      return (UINT16) sizeof (UINT8) * ((EFI_IFR_ORDERED_LIST *) OpCodeData)->MaxContainers;
+    
+    case EFI_IFR_TYPE_NUM_SIZE_16:
+      return (UINT16) sizeof (UINT16) * ((EFI_IFR_ORDERED_LIST *) OpCodeData)->MaxContainers ;
+    
+    case EFI_IFR_TYPE_NUM_SIZE_32:
+      return (UINT16) sizeof (UINT32) * ((EFI_IFR_ORDERED_LIST *) OpCodeData)->MaxContainers;
+    
+    case EFI_IFR_TYPE_NUM_SIZE_64:
+      return (UINT16) sizeof (UINT64) * ((EFI_IFR_ORDERED_LIST *) OpCodeData)->MaxContainers;
+    
+    default:
+      ASSERT (FALSE);
+      return 0;
+    }
+
+  case EFI_IFR_CHECKBOX_OP:
+    return (UINT16) sizeof (BOOLEAN);
+    
+  case EFI_IFR_PASSWORD_OP:
+    return (UINT16)((UINTN) ((EFI_IFR_PASSWORD *) OpCodeData)->MaxSize * sizeof (CHAR16));
+
+  case EFI_IFR_STRING_OP:
+    return (UINT16)((UINTN) ((EFI_IFR_STRING *) OpCodeData)->MaxSize * sizeof (CHAR16));
+
+  case EFI_IFR_DATE_OP:
+    return (UINT16) sizeof (EFI_HII_DATE);
+
+  case EFI_IFR_TIME_OP:
+    return (UINT16) sizeof (EFI_HII_TIME);
+
+  default:
+    ASSERT (FALSE);
+    return 0;
+  }
+}
+
+/**
+  Converts all hex dtring characters in range ['A'..'F'] to ['a'..'f'] for 
+  hex digits that appear between a '=' and a '&' in a config string.
+
+  If ConfigString is NULL, then ASSERT().
+
+  @param[in] ConfigString  Pointer to a Null-terminated Unicode string.
+
+  @return  Pointer to the Null-terminated Unicode result string.
+
+**/
+EFI_STRING
+EFIAPI
+InternalLowerConfigString (
+  IN EFI_STRING  ConfigString
+  )
+{
+  EFI_STRING  String;
+  BOOLEAN     Lower;
+
+  ASSERT (ConfigString != NULL);
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  for (String = ConfigString, Lower = FALSE; *String != L'\0'; String++) {
+    if (*String == L'=') {
+      Lower = TRUE;
+    } else if (*String == L'&') {
+      Lower = FALSE;
+    } else if (Lower && *String >= L'A' && *String <= L'F') {
+      *String = (CHAR16) (*String - L'A' + L'a');
+    }
+  }
+
+  return ConfigString;
+}
+
+/**
+  Allocates and returns a Null-terminated Unicode <ConfigHdr> string.
+  
+  The format of a <ConfigHdr> is as follows:
+
+    GUID=<HexCh>32&NAME=<Char>NameLength&PATH=<HexChar>DevicePathSize<Null>
+
+  @param[in]  OpCodeData    The opcode for the storage.                  
+  @param[in]  DriverHandle  The driver handle which supports a Device Path Protocol
+                            that is the routing information PATH.  Each byte of
+                            the Device Path associated with DriverHandle is converted
+                            to a 2 Unicode character hexidecimal string.
+
+  @retval NULL   DriverHandle does not support the Device Path Protocol.
+  @retval Other  A pointer to the Null-terminate Unicode <ConfigHdr> string
+
+**/
+EFI_STRING
+ConstructConfigHdr (
+  IN UINT8           *OpCodeData,
+  IN EFI_HANDLE      DriverHandle
+  )
+{
+  UINTN                     NameLength;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  UINTN                     DevicePathSize;
+  CHAR16                    *String;
+  CHAR16                    *ReturnString;
+  UINTN                     Index;
+  UINT8                     *Buffer;
+  CHAR16                    *Name;
+  CHAR8                     *AsciiName;
+  UINTN                     NameSize;
+  EFI_GUID                  *Guid;
+  UINTN                     MaxLen;
+
+  ASSERT (OpCodeData != NULL);
+
+  switch (((EFI_IFR_OP_HEADER *)OpCodeData)->OpCode) {
+  case EFI_IFR_VARSTORE_OP:
+    Guid      = (EFI_GUID *)(UINTN *)&((EFI_IFR_VARSTORE *) OpCodeData)->Guid;
+    AsciiName = (CHAR8 *) ((EFI_IFR_VARSTORE *) OpCodeData)->Name;
+    break;
+  
+  case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+    Guid      = (EFI_GUID *)(UINTN *)&((EFI_IFR_VARSTORE_NAME_VALUE *) OpCodeData)->Guid;
+    AsciiName = NULL;
+    break;
+  
+  case EFI_IFR_VARSTORE_EFI_OP:
+    Guid      = (EFI_GUID *)(UINTN *)&((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Guid;
+    AsciiName = (CHAR8 *) ((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Name;
+    break;
+  
+  default:
+    ASSERT (FALSE);
+    Guid      = NULL;
+    AsciiName = NULL;
+    break;
+  }
+
+  if (AsciiName != NULL) {
+    NameSize = AsciiStrSize (AsciiName);
+    Name = AllocateZeroPool (NameSize * sizeof (CHAR16));
+    ASSERT (Name != NULL);
+    AsciiStrToUnicodeStrS (AsciiName, Name, NameSize);
+  } else {
+    Name = NULL;
+  }
+
+  //
+  // Compute the length of Name in Unicode characters.  
+  // If Name is NULL, then the length is 0.
+  //
+  NameLength = 0;
+  if (Name != NULL) {
+    NameLength = StrLen (Name);
+  }
+
+  DevicePath = NULL;
+  DevicePathSize = 0;
+  //
+  // Retrieve DevicePath Protocol associated with DriverHandle
+  //
+  if (DriverHandle != NULL) {
+    DevicePath = DevicePathFromHandle (DriverHandle);
+    if (DevicePath == NULL) {
+      return NULL;
+    }
+    //
+    // Compute the size of the device path in bytes
+    //
+    DevicePathSize = GetDevicePathSize (DevicePath);
+  }
+
+  //
+  // GUID=<HexCh>32&NAME=<Char>NameLength&PATH=<HexChar>DevicePathSize <Null>
+  // | 5 | sizeof (EFI_GUID) * 2 | 6 | NameStrLen*4 | 6 | DevicePathSize * 2 | 1 |
+  //
+  MaxLen = 5 + sizeof (EFI_GUID) * 2 + 6 + NameLength * 4 + 6 + DevicePathSize * 2 + 1;
+  String = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  if (String == NULL) {
+    return NULL;
+  }
+
+  //
+  // Start with L"GUID="
+  //
+  StrCpyS (String, MaxLen, L"GUID=");
+  ReturnString = String;
+  String += StrLen (String);
+
+  if (Guid != NULL) {
+    //
+    // Append Guid converted to <HexCh>32
+    //
+    for (Index = 0, Buffer = (UINT8 *)Guid; Index < sizeof (EFI_GUID); Index++) {
+      String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(Buffer++), 2);
+    }
+  }
+  
+  //
+  // Append L"&NAME="
+  //
+  StrCatS (ReturnString, MaxLen, L"&NAME=");
+  String += StrLen (String);
+
+  if (Name != NULL) {
+    //
+    // Append Name converted to <Char>NameLength
+    //
+    for (; *Name != L'\0'; Name++) {
+      String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *Name, 4);
+    }
+  }
+
+  //
+  // Append L"&PATH="
+  //
+  StrCatS (ReturnString, MaxLen, L"&PATH=");
+  String += StrLen (String);
+
+  //
+  // Append the device path associated with DriverHandle converted to <HexChar>DevicePathSize
+  //
+  for (Index = 0, Buffer = (UINT8 *)DevicePath; Index < DevicePathSize; Index++) {
+    String += UnicodeValueToString (String, PREFIX_ZERO | RADIX_HEX, *(Buffer++), 2);
+  }
+
+  //
+  // Null terminate the Unicode string
+  //
+  *String = L'\0';
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  return InternalLowerConfigString (ReturnString);
+}
+
+/**
+  Generate the Config request element for one question.
+
+  @param   Name    The name info for one question.
+  @param   Offset  The offset info for one question.
+  @param   Width   The width info for one question.
+
+  @return  Pointer to the Null-terminated Unicode request element string.
+
+**/
+EFI_STRING
+ConstructRequestElement (
+  IN CHAR16      *Name,
+  IN UINT16      Offset,
+  IN UINT16      Width  
+  )
+{
+  CHAR16    *StringPtr;
+  UINTN     Length;
+  
+  if (Name != NULL) {
+    //
+    // Add <BlockName> length for each Name
+    //
+    // <BlockName> ::= Name + \0
+    //                 StrLen(Name) | 1
+    //
+    Length = StrLen (Name) + 1;
+  } else {
+    //
+    // Add <BlockName> length for each Offset/Width pair
+    //
+    // <BlockName> ::= OFFSET=1234&WIDTH=1234 + \0
+    //                 |  7   | 4 |   7  | 4 |  1
+    //
+    Length = (7 + 4 + 7 + 4 + 1);
+  }
+
+  //
+  // Allocate buffer for the entire <ConfigRequest>
+  //
+  StringPtr = AllocateZeroPool (Length * sizeof (CHAR16));
+  ASSERT (StringPtr != NULL);
+
+  if (Name != NULL) {
+    //
+    // Append Name\0
+    //
+    UnicodeSPrint (
+      StringPtr,
+      (StrLen (Name) + 1) * sizeof (CHAR16),
+      L"%s",
+      Name
+    );
+  } else {
+    //
+    // Append OFFSET=XXXX&WIDTH=YYYY\0
+    //
+    UnicodeSPrint (
+      StringPtr, 
+      (7 + 4 + 7 + 4 + 1) * sizeof (CHAR16), 
+      L"OFFSET=%04X&WIDTH=%04X", 
+      Offset, 
+      Width
+    );
+  }
+
+  return StringPtr;
+}
+
+/**
+  Get string value for question's name field.
+
+  @param  DatabaseRecord                 HII_DATABASE_RECORD format string.
+  @param  NameId                         The string id for the name field.
+
+  @retval Name string.
+
+**/
+CHAR16 *
+GetNameFromId (
+  IN HII_DATABASE_RECORD   *DatabaseRecord,
+  IN EFI_STRING_ID         NameId
+  )
+{
+  CHAR16      *Name;
+  CHAR8       *PlatformLanguage;
+  CHAR8       *SupportedLanguages;
+  CHAR8       *BestLanguage;
+  UINTN       StringSize;
+  CHAR16      TempString;
+  EFI_STATUS  Status;
+
+  Name = NULL;
+  BestLanguage = NULL;
+  PlatformLanguage = NULL; 
+  SupportedLanguages = NULL;
+
+  GetEfiGlobalVariable2 (L"PlatformLang", (VOID**)&PlatformLanguage, NULL);
+  SupportedLanguages = GetSupportedLanguages(DatabaseRecord->Handle);
+
+  //
+  // Get the best matching language from SupportedLanguages
+  //
+  BestLanguage = GetBestLanguage (
+                   SupportedLanguages, 
+                   FALSE,                                             // RFC 4646 mode
+                   PlatformLanguage != NULL ? PlatformLanguage : "",  // Highest priority
+                   SupportedLanguages,                                // Lowest priority 
+                   NULL
+                   );
+  if (BestLanguage == NULL) {
+    BestLanguage = AllocateCopyPool (AsciiStrLen ("en-US"), "en-US");
+    ASSERT (BestLanguage != NULL);
+  }
+
+  StringSize = 0;
+  Status = mPrivate.HiiString.GetString (
+                                 &mPrivate.HiiString,
+                                 BestLanguage,
+                                 DatabaseRecord->Handle,
+                                 NameId,
+                                 &TempString,
+                                 &StringSize,
+                                 NULL
+                                 );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    goto Done;
+  }
+
+  Name = AllocateZeroPool (StringSize);
+  if (Name == NULL) {
+    goto Done;
+  }
+
+  Status = mPrivate.HiiString.GetString (
+                          &mPrivate.HiiString,
+                          BestLanguage,
+                          DatabaseRecord->Handle,
+                          NameId,
+                          Name,
+                          &StringSize,
+                          NULL
+                          );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Name);
+    Name = NULL;
+    goto Done;
+  }
+
+Done:
+  if (SupportedLanguages != NULL) {
+    FreePool(SupportedLanguages);
+  }
+  if (BestLanguage != NULL) {
+    FreePool (BestLanguage);
+  }
+  if (PlatformLanguage != NULL) {
+    FreePool (PlatformLanguage);
+  }
+
+  return Name;
+}
+
+/**
+  Base on the input parameter to generate the ConfigRequest string.
+
+  This is a internal function.
+
+  @param  DatabaseRecord                 HII_DATABASE_RECORD format string.
+  @param  KeywordStrId                   Keyword string id.
+  @param  OpCodeData                     The IFR data for this question.
+  @param  ConfigRequest                  Return the generate ConfigRequest string.
+
+  @retval EFI_SUCCESS               Generate ConfigResp string success.
+  @retval EFI_OUT_OF_RESOURCES      System out of memory resource error.
+  @retval EFI_NOT_FOUND             Not found the question which use this string id
+                                    as the prompt string id.
+**/
+EFI_STATUS
+ExtractConfigRequest (
+  IN  HII_DATABASE_RECORD   *DatabaseRecord,
+  IN  EFI_STRING_ID         KeywordStrId,
+  OUT UINT8                 **OpCodeData,
+  OUT EFI_STRING            *ConfigRequest
+  ) 
+{
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_IFR_PACKAGE_INSTANCE            *FormPackage;
+  EFI_IFR_QUESTION_HEADER             *Header;
+  UINT8                               *Storage;
+  UINT8                               *OpCode;
+  CHAR16                              *Name;
+  UINT16                              Offset;
+  UINT16                              Width;
+  CHAR16                              *ConfigHdr;
+  CHAR16                              *RequestElement;
+  UINTN                               MaxLen;
+  CHAR16                              *StringPtr;
+
+  ASSERT (DatabaseRecord != NULL && OpCodeData != NULL && ConfigRequest != NULL);
+
+  OpCode = NULL;
+  Name   = NULL;
+  Width  = 0;
+  Offset = 0;
+
+  PackageListNode = DatabaseRecord->PackageList;
+
+  //
+  // Search the languages in the specified packagelist.
+  //
+  for (Link = PackageListNode->FormPkgHdr.ForwardLink; Link != &PackageListNode->FormPkgHdr; Link = Link->ForwardLink) {
+    FormPackage = CR (Link, HII_IFR_PACKAGE_INSTANCE, IfrEntry, HII_IFR_PACKAGE_SIGNATURE);
+
+    OpCode = FindQuestionFromStringId (FormPackage, KeywordStrId);
+    if (OpCode != NULL) {
+      *OpCodeData = OpCode;
+      Header = (EFI_IFR_QUESTION_HEADER *) (OpCode + sizeof (EFI_IFR_OP_HEADER));
+      //
+      // Header->VarStoreId == 0 means no storage for this question.
+      //
+      ASSERT (Header->VarStoreId != 0);
+      DEBUG ((EFI_D_INFO, "Varstore Id: 0x%x\n", Header->VarStoreId));
+      
+      Storage = FindStorageFromVarId (FormPackage, Header->VarStoreId);
+      ASSERT (Storage != NULL);
+
+      if (((EFI_IFR_OP_HEADER *) Storage)->OpCode == EFI_IFR_VARSTORE_NAME_VALUE_OP) {
+        Name = GetNameFromId (DatabaseRecord, Header->VarStoreInfo.VarName);
+      } else {
+        Offset = Header->VarStoreInfo.VarOffset;
+        Width = GetWidth (OpCode);
+      }
+      RequestElement = ConstructRequestElement(Name, Offset, Width);
+      ConfigHdr = ConstructConfigHdr(Storage, DatabaseRecord->DriverHandle);
+      ASSERT (ConfigHdr != NULL);
+
+      MaxLen = StrLen (ConfigHdr) + 1 + StrLen(RequestElement) + 1;
+      *ConfigRequest = AllocatePool (MaxLen * sizeof (CHAR16));
+      if (*ConfigRequest == NULL) {
+        FreePool (ConfigHdr);
+        FreePool (RequestElement);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      StringPtr = *ConfigRequest;
+
+      StrCpyS (StringPtr, MaxLen, ConfigHdr);
+
+      StrCatS (StringPtr, MaxLen, L"&");
+
+      StrCatS (StringPtr, MaxLen, RequestElement);
+
+      FreePool (ConfigHdr);
+      FreePool (RequestElement);
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Base on the input parameter to generate the ConfigResp string.
+
+  This is a internal function.
+
+  @param  DatabaseRecord                 HII_DATABASE_RECORD format string.
+  @param  KeywordStrId                   Keyword string id.
+  @param  ValueElement                   The value for the question which use keyword string id
+                                         as the prompt string id.
+  @param  OpCodeData                     The IFR data for this question.
+  @param  ConfigResp                     Return the generate ConfigResp string.
+
+  @retval EFI_SUCCESS               Generate ConfigResp string success.
+  @retval EFI_OUT_OF_RESOURCES      System out of memory resource error.
+  @retval EFI_NOT_FOUND             Not found the question which use this string id
+                                    as the prompt string id.
+**/
+EFI_STATUS
+ExtractConfigResp (
+  IN  HII_DATABASE_RECORD   *DatabaseRecord,
+  IN  EFI_STRING_ID         KeywordStrId,
+  IN  EFI_STRING            ValueElement,
+  OUT UINT8                 **OpCodeData,
+  OUT EFI_STRING            *ConfigResp
+  ) 
+{
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_IFR_PACKAGE_INSTANCE            *FormPackage;
+  EFI_IFR_QUESTION_HEADER             *Header;
+  UINT8                               *Storage;
+  UINT8                               *OpCode;
+  CHAR16                              *Name;
+  UINT16                              Offset;
+  UINT16                              Width;
+  CHAR16                              *ConfigHdr;
+  CHAR16                              *RequestElement;
+  UINTN                               MaxLen;
+  CHAR16                              *StringPtr;
+
+  ASSERT ((DatabaseRecord != NULL) && (OpCodeData != NULL) && (ConfigResp != NULL) && (ValueElement != NULL));
+
+  OpCode = NULL;
+  Name   = NULL;
+  Width  = 0;
+  Offset = 0;
+
+  PackageListNode = DatabaseRecord->PackageList;
+
+  //
+  // Search the languages in the specified packagelist.
+  //
+  for (Link = PackageListNode->FormPkgHdr.ForwardLink; Link != &PackageListNode->FormPkgHdr; Link = Link->ForwardLink) {
+    FormPackage = CR (Link, HII_IFR_PACKAGE_INSTANCE, IfrEntry, HII_IFR_PACKAGE_SIGNATURE);
+
+    OpCode = FindQuestionFromStringId (FormPackage, KeywordStrId);
+    if (OpCode != NULL) {
+      *OpCodeData = OpCode;
+      Header = (EFI_IFR_QUESTION_HEADER *) (OpCode + sizeof (EFI_IFR_OP_HEADER));
+      //
+      // Header->VarStoreId == 0 means no storage for this question.
+      //
+      ASSERT (Header->VarStoreId != 0);
+      DEBUG ((EFI_D_INFO, "Varstore Id: 0x%x\n", Header->VarStoreId));
+      
+      Storage = FindStorageFromVarId (FormPackage, Header->VarStoreId);
+      ASSERT (Storage != NULL);
+
+      if (((EFI_IFR_OP_HEADER *) Storage)->OpCode == EFI_IFR_VARSTORE_NAME_VALUE_OP) {
+        Name = GetNameFromId (DatabaseRecord, Header->VarStoreInfo.VarName);
+      } else {
+        Offset = Header->VarStoreInfo.VarOffset;
+        Width  = GetWidth (OpCode);
+      }
+      RequestElement = ConstructRequestElement(Name, Offset, Width);
+
+      ConfigHdr = ConstructConfigHdr(Storage, DatabaseRecord->DriverHandle);
+      ASSERT (ConfigHdr != NULL);
+
+      MaxLen = StrLen (ConfigHdr) + 1 + StrLen(RequestElement) + 1 + StrLen (L"VALUE=") + StrLen(ValueElement) + 1;
+      *ConfigResp = AllocatePool (MaxLen * sizeof (CHAR16));
+      if (*ConfigResp == NULL) {
+        FreePool (ConfigHdr);
+        FreePool (RequestElement);
+        return EFI_OUT_OF_RESOURCES;
+      }
+      StringPtr = *ConfigResp;
+
+      StrCpyS (StringPtr, MaxLen, ConfigHdr);
+
+      StrCatS (StringPtr, MaxLen, L"&");
+
+
+      StrCatS (StringPtr, MaxLen, RequestElement);
+
+      StrCatS (StringPtr, MaxLen, L"&");
+
+      StrCatS (StringPtr, MaxLen, L"VALUE=");
+
+      StrCatS (StringPtr, MaxLen, ValueElement);
+
+      FreePool (ConfigHdr);
+      FreePool (RequestElement);
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Get the Value section from the Hii driver.
+
+  This is a internal function.
+
+  @param  ConfigRequest                  The input ConfigRequest string.
+  @param  ValueElement                   The respond Value section from the hii driver.
+
+  @retval Misc value                     The error status return from ExtractConfig function.
+  @retval EFI_OUT_OF_RESOURCES           The memory can't be allocated
+  @retval EFI_SUCCESS                    Get the value section success.
+
+**/
+EFI_STATUS
+ExtractValueFromDriver (
+  IN  CHAR16           *ConfigRequest,
+  OUT CHAR16           **ValueElement
+  )
+{
+  EFI_STATUS   Status;
+  EFI_STRING   Result;
+  EFI_STRING   Progress;
+  CHAR16       *StringPtr;
+  CHAR16       *StringEnd;
+
+  ASSERT ((ConfigRequest != NULL) && (ValueElement != NULL));
+
+  Status = mPrivate.ConfigRouting.ExtractConfig (
+                      &mPrivate.ConfigRouting,
+                      (EFI_STRING) ConfigRequest,
+                      &Progress,
+                      &Result
+                      );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Find Value Section and return it.
+  //
+  StringPtr = StrStr (Result, L"&VALUE=");
+  ASSERT (StringPtr != NULL);
+  StringEnd = StrStr (StringPtr + 1, L"&");
+  if (StringEnd != NULL) {
+    *StringEnd = L'\0';
+  }
+
+  *ValueElement = AllocateCopyPool (StrSize (StringPtr), StringPtr);
+  if (*ValueElement == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (StringEnd != NULL) {
+    *StringEnd = L'&';
+  }
+  FreePool (Result);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get EFI_STRING_ID info from the input device path, namespace and keyword.
+
+  This is a internal function.
+
+  @param  DevicePath                     Input device path info.
+  @param  NameSpace                      NameSpace format string.
+  @param  KeywordData                    Keyword used to get string id.
+  @param  ProgressErr                    Return extra error type.
+  @param  KeywordStringId                Return EFI_STRING_ID.
+  @param  DataBaseRecord                 DataBase record data for this driver.
+
+  @retval EFI_INVALID_PARAMETER          Can't find the database record base on the input device path or namespace.
+  @retval EFI_NOT_FOUND                  Can't find the EFI_STRING_ID for the keyword.
+  @retval EFI_SUCCESS                    Find the EFI_STRING_ID.
+
+**/
+EFI_STATUS
+GetStringIdFromDatabase (
+  IN  EFI_DEVICE_PATH_PROTOCOL            **DevicePath,
+  IN  CHAR8                               **NameSpace,
+  IN  CHAR16                              *KeywordData,
+  OUT UINT32                              *ProgressErr,
+  OUT EFI_STRING_ID                       *KeywordStringId,
+  OUT HII_DATABASE_RECORD                 **DataBaseRecord
+ )
+{
+  HII_DATABASE_RECORD                 *Record;
+  LIST_ENTRY                          *Link;
+  BOOLEAN                             FindNameSpace;
+  EFI_DEVICE_PATH_PROTOCOL            *DestDevicePath;
+  UINT8                               *DevicePathPkg;
+  UINTN                               DevicePathSize;
+
+  ASSERT ((NameSpace != NULL) && (KeywordData != NULL) && (ProgressErr != NULL) && (KeywordStringId != NULL) && (DataBaseRecord != NULL));
+
+  FindNameSpace = FALSE;
+
+  if (*DevicePath != NULL) {
+    //
+    // Get DataBaseRecord from device path protocol.
+    //
+    Record = GetRecordFromDevicePath(*DevicePath);
+    if (Record == NULL) {
+      //
+      // Can't find the DatabaseRecord base on the input device path info.
+      // NEED TO CONFIRM the return ProgressErr.
+      //
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // Get string id from the record.
+    //
+    *ProgressErr = GetStringIdFromRecord (Record, NameSpace, KeywordData, KeywordStringId);
+    switch (*ProgressErr) {
+    case KEYWORD_HANDLER_NO_ERROR:
+      *DataBaseRecord = Record;
+      return EFI_SUCCESS;
+
+    case KEYWORD_HANDLER_NAMESPACE_ID_NOT_FOUND:
+      return EFI_INVALID_PARAMETER;
+
+    default:
+      ASSERT (*ProgressErr == KEYWORD_HANDLER_KEYWORD_NOT_FOUND);
+      return EFI_NOT_FOUND;
+    }
+  } else {
+    //
+    // Find driver which matches the routing data.
+    //
+    for (Link = mPrivate.DatabaseList.ForwardLink; Link != &mPrivate.DatabaseList; Link = Link->ForwardLink) {
+      Record = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+
+      *ProgressErr = GetStringIdFromRecord (Record, NameSpace, KeywordData, KeywordStringId);
+      if (*ProgressErr == KEYWORD_HANDLER_NO_ERROR) {
+        *DataBaseRecord = Record;
+        
+        if ((DevicePathPkg = Record->PackageList->DevicePathPkg) != NULL) {
+          DestDevicePath = (EFI_DEVICE_PATH_PROTOCOL *) (DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER));
+          DevicePathSize = GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) DestDevicePath);
+          *DevicePath = AllocateCopyPool (DevicePathSize, DestDevicePath);
+          if (*DevicePath == NULL) {
+            return EFI_OUT_OF_RESOURCES;
+          }
+        } else {
+          //
+          // Need to verify this ASSERT.
+          //
+          ASSERT (FALSE);
+        }
+
+        return EFI_SUCCESS;
+      } else if (*ProgressErr == KEYWORD_HANDLER_UNDEFINED_PROCESSING_ERROR) {
+        return EFI_OUT_OF_RESOURCES;
+      } else if (*ProgressErr == KEYWORD_HANDLER_KEYWORD_NOT_FOUND) {
+        FindNameSpace = TRUE;
+      }  
+    }
+
+    //
+    // When PathHdr not input, if ever find the namespace, will return KEYWORD_HANDLER_KEYWORD_NOT_FOUND.
+    // This is a bit more progress than KEYWORD_HANDLER_NAMESPACE_ID_NOT_FOUND.
+    //
+    if (FindNameSpace) {
+      return EFI_NOT_FOUND;
+    } else {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+}
+
+/**
+  Genereate the KeywordResp String.
+
+  <KeywordResp> ::= <NameSpaceId><PathHdr>'&'<Keyword>'&VALUE='<Number>['&READONLY']
+
+  @param  NameSpace                      NameSpace format string.
+  @param  DevicePath                     Input device path info.
+  @param  KeywordData                    Keyword used to get string id.
+  @param  ValueStr                       The value section for the keyword.
+  @param  ReadOnly                       Whether this value is readonly.
+  @param  KeywordResp                    Return the point to the KeywordResp string.
+
+  @retval EFI_OUT_OF_RESOURCES           The memory can't be allocated.
+  @retval EFI_SUCCESS                    Generate the KeywordResp string.
+
+**/
+EFI_STATUS
+GenerateKeywordResp (
+  IN  CHAR8                          *NameSpace, 
+  IN  EFI_DEVICE_PATH_PROTOCOL       *DevicePath,
+  IN  EFI_STRING                     KeywordData,
+  IN  EFI_STRING                     ValueStr,
+  IN  BOOLEAN                        ReadOnly,
+  OUT EFI_STRING                     *KeywordResp
+  )
+{
+  UINTN     RespStrLen;
+  CHAR16    *RespStr;
+  CHAR16    *PathHdr;
+  CHAR16    *UnicodeNameSpace;
+  UINTN     NameSpaceLength;
+
+  ASSERT ((NameSpace != NULL) && (DevicePath != NULL) && (KeywordData != NULL) && (ValueStr != NULL) && (KeywordResp != NULL));
+
+  //
+  // 1. Calculate the string length.
+  //
+  //
+  // 1.1 NameSpaceId size.
+  // 'NAMESPACE='<String>
+  //
+  NameSpaceLength = AsciiStrLen (NameSpace);
+  RespStrLen = 10 + NameSpaceLength;
+  UnicodeNameSpace = AllocatePool ((NameSpaceLength + 1) * sizeof (CHAR16));
+  if (UnicodeNameSpace == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  AsciiStrToUnicodeStrS (NameSpace, UnicodeNameSpace, NameSpaceLength + 1);
+
+  //
+  // 1.2 PathHdr size.
+  // PATH=<UEFI binary Device Path represented as hex number>'&'
+  // Attention: The output include the '&' at the end.
+  //
+  GenerateSubStr (
+    L"&PATH=",
+    GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) DevicePath),
+    (VOID *) DevicePath,
+    1,
+    &PathHdr
+    );
+  RespStrLen += StrLen (PathHdr);
+
+  //
+  // 1.3 Keyword setion.
+  // 'KEYWORD='<String>[':'<DecCh>(1/4)]
+  //
+  RespStrLen += 8 + StrLen (KeywordData);
+
+  //
+  // 1.4 Value section.
+  // ValueStr = '&VALUE='<Number>
+  //
+  RespStrLen += StrLen (ValueStr);
+
+  //
+  // 1.5 ReadOnly Section.
+  // '&READONLY'
+  //
+  if (ReadOnly) {
+    RespStrLen += 9;
+  }
+
+  //
+  // 2. Allocate the buffer and create the KeywordResp string include '\0'.
+  //
+  RespStrLen += 1;
+  *KeywordResp = AllocatePool (RespStrLen * sizeof (CHAR16));
+  if (*KeywordResp == NULL) {
+    if (UnicodeNameSpace != NULL) {
+      FreePool (UnicodeNameSpace);
+    }
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+  RespStr = *KeywordResp;
+
+  //
+  // 2.1 Copy NameSpaceId section.
+  //
+  StrCpyS (RespStr, RespStrLen, L"NAMESPACE=");
+
+  StrCatS (RespStr, RespStrLen, UnicodeNameSpace);
+
+  //
+  // 2.2 Copy PathHdr section.
+  //
+  StrCatS (RespStr, RespStrLen, PathHdr);
+
+  //
+  // 2.3 Copy Keyword section.
+  //
+  StrCatS (RespStr, RespStrLen, L"KEYWORD=");
+
+  StrCatS (RespStr, RespStrLen, KeywordData);
+
+  //
+  // 2.4 Copy the Value section.
+  //
+  StrCatS (RespStr, RespStrLen, ValueStr);
+
+  //
+  // 2.5 Copy ReadOnly section if exist.
+  //
+  if (ReadOnly) {
+    StrCatS (RespStr, RespStrLen, L"&READONLY");
+  }
+
+  if (UnicodeNameSpace != NULL) {
+    FreePool (UnicodeNameSpace);
+  }
+  if (PathHdr != NULL) {
+    FreePool (PathHdr);
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Merge the KeywordResp String to MultiKeywordResp string.
+
+  This is a internal function.
+
+  @param  MultiKeywordResp               The existed multikeywordresp string.
+  @param  KeywordResp                    The input keywordResp string.
+
+  @retval EFI_OUT_OF_RESOURCES           The memory can't be allocated.
+  @retval EFI_SUCCESS                    Generate the MultiKeywordResp string.
+
+**/
+EFI_STATUS
+MergeToMultiKeywordResp (
+  IN OUT EFI_STRING         *MultiKeywordResp,
+  IN     EFI_STRING         *KeywordResp
+  )
+{
+  UINTN       MultiKeywordRespLen;
+  EFI_STRING  StringPtr;
+
+  if (*MultiKeywordResp == NULL) {
+    *MultiKeywordResp = *KeywordResp;
+    *KeywordResp = NULL;
+    return EFI_SUCCESS;
+  }
+
+  MultiKeywordRespLen = (StrLen (*MultiKeywordResp) + 1 + StrLen (*KeywordResp) + 1) * sizeof (CHAR16);
+
+  StringPtr = AllocateCopyPool (MultiKeywordRespLen, *MultiKeywordResp);
+  if (StringPtr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  FreePool (*MultiKeywordResp);
+  *MultiKeywordResp = StringPtr;
+
+  StrCatS (StringPtr, MultiKeywordRespLen / sizeof (CHAR16), L"&");
+
+  StrCatS (StringPtr, MultiKeywordRespLen / sizeof (CHAR16), *KeywordResp);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Enumerate all keyword in the system. 
+  
+  If error occur when parse one keyword, just skip it and parse the next one. 
+
+  This is a internal function.
+
+  @param  NameSpace                      The namespace used to search the string.
+  @param  MultiResp                      Return the MultiKeywordResp string for the system.
+  @param  ProgressErr                    Return the error status.
+
+  @retval EFI_OUT_OF_RESOURCES           The memory can't be allocated.
+  @retval EFI_SUCCESS                    Generate the MultiKeywordResp string.
+  @retval EFI_NOT_FOUND                  No keyword found.
+
+**/
+EFI_STATUS
+EnumerateAllKeywords (
+  IN  CHAR8             *NameSpace,
+  OUT EFI_STRING        *MultiResp,
+  OUT UINT32            *ProgressErr
+  )
+{
+  LIST_ENTRY                          *Link;
+  LIST_ENTRY                          *StringLink;
+  UINT8                               *DevicePathPkg;
+  UINT8                               *DevicePath;
+  HII_DATABASE_RECORD                 *DataBaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  CHAR8                               *LocalNameSpace;
+  EFI_STRING_ID                       NextStringId;
+  EFI_STATUS                          Status;
+  UINT8                               *OpCode;
+  CHAR16                              *ConfigRequest;
+  CHAR16                              *ValueElement;
+  CHAR16                              *KeywordResp;
+  CHAR16                              *MultiKeywordResp;
+  CHAR16                              *KeywordData;
+  BOOLEAN                             ReadOnly;
+  BOOLEAN                             FindKeywordPackages;
+
+  DataBaseRecord   = NULL;
+  Status           = EFI_SUCCESS;
+  MultiKeywordResp = NULL;
+  DevicePath       = NULL;
+  LocalNameSpace   = NULL;
+  ConfigRequest    = NULL;
+  ValueElement     = NULL;
+  KeywordResp      = NULL;
+  FindKeywordPackages = FALSE;
+
+  if (NameSpace == NULL) {
+    NameSpace = UEFI_CONFIG_LANG;
+  }
+
+  //
+  // Find driver which matches the routing data.
+  //
+  for (Link = mPrivate.DatabaseList.ForwardLink; Link != &mPrivate.DatabaseList; Link = Link->ForwardLink) {
+    DataBaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if ((DevicePathPkg = DataBaseRecord->PackageList->DevicePathPkg) != NULL) {
+      DevicePath = DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER);
+    }
+    PackageListNode = DataBaseRecord->PackageList;
+
+    for (StringLink = PackageListNode->StringPkgHdr.ForwardLink; StringLink != &PackageListNode->StringPkgHdr; StringLink = StringLink->ForwardLink) {
+      StringPackage = CR (StringLink, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+
+      //
+      // Check whether has keyword string package.
+      //
+      if (AsciiStrnCmp(NameSpace, StringPackage->StringPkgHdr->Language, AsciiStrLen (NameSpace)) == 0) {
+        FindKeywordPackages = TRUE;
+        //
+        // Keep the NameSpace string.
+        //
+        LocalNameSpace = AllocateCopyPool (AsciiStrSize (StringPackage->StringPkgHdr->Language), StringPackage->StringPkgHdr->Language);
+        if (LocalNameSpace == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        //
+        // 1 means just begin the enumerate the valid string ids.
+        // StringId == 1 is always used to save the language for this string package.
+        // Any valid string start from 2. so here initial it to 1.
+        //
+        NextStringId = 1;
+        
+        //
+        // Enumerate all valid stringid in the package.
+        //
+        while ((NextStringId = GetNextStringId (StringPackage, NextStringId, &KeywordData)) != 0) {
+          //
+          // 3.3 Construct the ConfigRequest string.
+          //
+          Status = ExtractConfigRequest (DataBaseRecord, NextStringId, &OpCode, &ConfigRequest);
+          if (EFI_ERROR (Status)) {
+            //
+            // If can't generate ConfigRequest for this question, skip it and start the next.
+            //
+            goto Error;
+          }
+          
+          //
+          // 3.4 Extract Value for the input keyword.
+          //
+          Status = ExtractValueFromDriver(ConfigRequest, &ValueElement);
+          if (EFI_ERROR (Status)) {
+            if (Status != EFI_OUT_OF_RESOURCES) {
+              //
+              // If can't generate ConfigRequest for this question, skip it and start the next.
+              //
+              goto Error;
+            }
+            //
+            // If EFI_OUT_OF_RESOURCES error occur, no need to continue.
+            //
+            goto Done;
+          }
+
+          //
+          // Extract readonly flag from opcode.
+          //
+          ReadOnly = ExtractReadOnlyFromOpCode(OpCode);
+
+          //
+          // 5. Generate KeywordResp string.
+          //
+          ASSERT (DevicePath != NULL);
+          Status = GenerateKeywordResp(LocalNameSpace, (EFI_DEVICE_PATH_PROTOCOL *)DevicePath, KeywordData, ValueElement, ReadOnly, &KeywordResp);
+          if (Status != EFI_SUCCESS) {
+            //
+            // If EFI_OUT_OF_RESOURCES error occur, no need to continue.
+            //
+            goto Done;
+          }
+          
+          //
+          // 6. Merge to the MultiKeywordResp string.
+          //
+          Status = MergeToMultiKeywordResp(&MultiKeywordResp, &KeywordResp);
+          if (EFI_ERROR (Status)) {
+            goto Done;
+          }
+Error:
+          //
+          // Clean the temp buffer to later use again.
+          //
+          if (ConfigRequest != NULL) {
+            FreePool (ConfigRequest);
+            ConfigRequest = NULL;
+          }
+          if (ValueElement != NULL) {
+            FreePool (ValueElement);
+            ValueElement = NULL;
+          }
+          if (KeywordResp != NULL) {
+            FreePool (KeywordResp);
+            KeywordResp = NULL;
+          } 
+        }
+
+        if (LocalNameSpace != NULL) {
+          FreePool (LocalNameSpace);
+          LocalNameSpace = NULL;
+        }
+      }
+    } 
+  }
+
+  //
+  // return the already get MultiKeywordString even error occured.
+  //
+  if (MultiKeywordResp == NULL) {
+    Status = EFI_NOT_FOUND;
+    if (!FindKeywordPackages) {
+      *ProgressErr = KEYWORD_HANDLER_NAMESPACE_ID_NOT_FOUND;
+    } else {
+      *ProgressErr = KEYWORD_HANDLER_KEYWORD_NOT_FOUND;
+    }
+  } else {
+    Status = EFI_SUCCESS;
+  }
+  *MultiResp = MultiKeywordResp;
+  
+Done:
+  if (LocalNameSpace != NULL) {
+    FreePool (LocalNameSpace);
+  }
+  if (ConfigRequest != NULL) {
+    FreePool (ConfigRequest);
+  }
+  if (ValueElement != NULL) {
+    FreePool (ValueElement);
+  }
+
+  return Status;
+}
+
+/**
+
+  This function accepts a <MultiKeywordResp> formatted string, finds the associated
+  keyword owners, creates a <MultiConfigResp> string from it and forwards it to the
+  EFI_HII_ROUTING_PROTOCOL.RouteConfig function.
+  
+  If there is an issue in resolving the contents of the KeywordString, then the 
+  function returns an error and also sets the Progress and ProgressErr with the 
+  appropriate information about where the issue occurred and additional data about
+  the nature of the issue. 
+  
+  In the case when KeywordString containing multiple keywords, when an EFI_NOT_FOUND
+  error is generated during processing the second or later keyword element, the system
+  storage associated with earlier keywords is not modified. All elements of the 
+  KeywordString must successfully pass all tests for format and access prior to making
+  any modifications to storage.
+  
+  In the case when EFI_DEVICE_ERROR is returned from the processing of a KeywordString
+  containing multiple keywords, the state of storage associated with earlier keywords
+  is undefined.
+
+
+  @param This             Pointer to the EFI_KEYWORD_HANDLER _PROTOCOL instance.
+
+  @param KeywordString    A null-terminated string in <MultiKeywordResp> format. 
+
+  @param Progress         On return, points to a character in the KeywordString. 
+                          Points to the string's NULL terminator if the request 
+                          was successful. Points to the most recent '&' before 
+                          the first failing string element if the request was 
+                          not successful.
+
+  @param ProgressErr      If during the processing of the KeywordString there was
+                          a failure, this parameter gives additional information 
+                          about the possible source of the problem. The various 
+                          errors are defined in "Related Definitions" below.
+
+
+  @retval EFI_SUCCESS             The specified action was completed successfully.
+
+  @retval EFI_INVALID_PARAMETER   One or more of the following are TRUE:
+                                  1. KeywordString is NULL.
+                                  2. Parsing of the KeywordString resulted in an 
+                                     error. See Progress and ProgressErr for more data.
+
+  @retval EFI_NOT_FOUND           An element of the KeywordString was not found. 
+                                  See ProgressErr for more data.
+
+  @retval EFI_OUT_OF_RESOURCES    Required system resources could not be allocated.  
+                                  See ProgressErr for more data.
+                                  
+  @retval EFI_ACCESS_DENIED       The action violated system policy. See ProgressErr 
+                                  for more data.
+
+  @retval EFI_DEVICE_ERROR        An unexpected system error occurred. See ProgressErr
+                                  for more data.
+
+**/
+EFI_STATUS
+EFIAPI 
+EfiConfigKeywordHandlerSetData (
+  IN EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL *This,
+  IN CONST EFI_STRING                    KeywordString,
+  OUT EFI_STRING                         *Progress,
+  OUT UINT32                             *ProgressErr
+  )
+{
+  CHAR8                               *NameSpace;
+  EFI_STATUS                          Status;
+  CHAR16                              *StringPtr;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  CHAR16                              *NextStringPtr;
+  CHAR16                              *KeywordData;
+  EFI_STRING_ID                       KeywordStringId;
+  UINT32                              RetVal;
+  HII_DATABASE_RECORD                 *DataBaseRecord;
+  UINT8                               *OpCode;
+  CHAR16                              *ConfigResp;
+  CHAR16                              *MultiConfigResp;
+  CHAR16                              *ValueElement;
+  BOOLEAN                             ReadOnly;
+  EFI_STRING                          InternalProgress;
+  CHAR16                              *TempString;
+  CHAR16                              *KeywordStartPos;
+
+  if (This == NULL || Progress == NULL || ProgressErr == NULL || KeywordString == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress    = KeywordString;
+  *ProgressErr = KEYWORD_HANDLER_UNDEFINED_PROCESSING_ERROR;
+  Status       = EFI_SUCCESS;
+  MultiConfigResp = NULL;
+  NameSpace       = NULL;
+  DevicePath      = NULL;
+  KeywordData     = NULL;
+  ValueElement    = NULL;
+  ConfigResp      = NULL;
+  KeywordStartPos = NULL;
+  KeywordStringId = 0;
+
+  //
+  // Use temp string to avoid changing input string buffer.
+  //
+  TempString = AllocateCopyPool (StrSize (KeywordString), KeywordString);
+  ASSERT (TempString != NULL);
+  StringPtr = TempString;
+
+  while ((StringPtr != NULL) && (*StringPtr != L'\0')) {
+    //
+    // 1. Get NameSpace from NameSpaceId keyword.
+    //
+    Status = ExtractNameSpace (StringPtr, &NameSpace, &NextStringPtr);
+    if (EFI_ERROR (Status)) {
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      goto Done;
+    }
+    ASSERT (NameSpace != NULL);
+    //
+    // 1.1 Check whether the input namespace is valid.
+    //
+    if (AsciiStrnCmp(NameSpace, UEFI_CONFIG_LANG, AsciiStrLen (UEFI_CONFIG_LANG)) != 0) {
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    StringPtr = NextStringPtr;
+
+    //
+    // 2. Get possible Device Path info from KeywordString.
+    //
+    Status = ExtractDevicePath (StringPtr, (UINT8 **)&DevicePath, &NextStringPtr);
+    if (EFI_ERROR (Status)) {
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      goto Done;
+    }
+    StringPtr = NextStringPtr;
+
+    //
+    // 3. Extract keyword from the KeywordRequest string.
+    //
+    KeywordStartPos = StringPtr;
+    Status = ExtractKeyword(StringPtr, &KeywordData, &NextStringPtr);
+    if (EFI_ERROR (Status)) {
+      //
+      // Can't find Keyword base on the input device path info.
+      //
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr = NextStringPtr;
+
+    //
+    // 4. Extract Value from the KeywordRequest string.
+    //
+    Status = ExtractValue (StringPtr, &ValueElement, &NextStringPtr);
+    if (EFI_ERROR (Status)) {
+      //
+      // Can't find Value base on the input device path info.
+      //
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr = NextStringPtr;
+
+    //
+    // 5. Find ReadOnly filter.
+    //
+    if ((StringPtr != NULL) && StrnCmp (StringPtr, L"&ReadOnly", StrLen (L"&ReadOnly")) == 0) {
+      ReadOnly = TRUE;
+      StringPtr += StrLen (L"&ReadOnly");
+    } else {
+      ReadOnly = FALSE;
+    }
+
+    //
+    // 6. Get EFI_STRING_ID for the input keyword.
+    //
+    Status = GetStringIdFromDatabase (&DevicePath, &NameSpace, KeywordData, &RetVal, &KeywordStringId, &DataBaseRecord);
+    if (EFI_ERROR (Status)) {
+      *ProgressErr = RetVal;
+      goto Done;
+    }
+
+    //
+    // 7. Construct the ConfigRequest string.
+    //
+    Status = ExtractConfigResp (DataBaseRecord, KeywordStringId, ValueElement, &OpCode, &ConfigResp);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // 8. Check the readonly flag.
+    //
+    if (ExtractReadOnlyFromOpCode (OpCode) != ReadOnly) {
+      *ProgressErr = KEYWORD_HANDLER_ACCESS_NOT_PERMITTED;
+      Status = EFI_ACCESS_DENIED;
+      goto Done;      
+    }
+    
+    //
+    // 9. Merge to the MultiKeywordResp string.
+    //
+    Status = MergeToMultiKeywordResp(&MultiConfigResp, &ConfigResp);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // 10. Clean the temp buffer point.
+    //
+    FreePool (NameSpace);
+    FreePool (DevicePath);
+    FreePool (KeywordData);
+    FreePool (ValueElement);
+    NameSpace = NULL;
+    DevicePath = NULL; 
+    KeywordData = NULL;
+    ValueElement = NULL;
+    if (ConfigResp != NULL) {
+      FreePool (ConfigResp);
+      ConfigResp = NULL;
+    }
+    KeywordStartPos = NULL;
+  }
+
+  //
+  // 11. Set value to driver.
+  //
+  Status = mPrivate.ConfigRouting.RouteConfig(
+                    &mPrivate.ConfigRouting,
+                    (EFI_STRING) MultiConfigResp,
+                    &InternalProgress
+                    );
+  if (EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+    goto Done;
+  }
+  
+  *ProgressErr = KEYWORD_HANDLER_NO_ERROR;
+
+Done:
+  if (KeywordStartPos != NULL) {
+    *Progress = KeywordString + (KeywordStartPos - TempString);
+  } else {
+    *Progress = KeywordString + (StringPtr - TempString);
+  }
+
+  ASSERT (TempString != NULL);
+  FreePool (TempString);
+  if (NameSpace != NULL) {
+    FreePool (NameSpace);
+  }
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+  if (KeywordData != NULL) {
+    FreePool (KeywordData);
+  }
+  if (ValueElement != NULL) {
+    FreePool (ValueElement);
+  }
+  if (ConfigResp != NULL) {
+    FreePool (ConfigResp);
+  }
+  if (MultiConfigResp != NULL && MultiConfigResp != ConfigResp) {
+    FreePool (MultiConfigResp);
+  }
+
+  return Status;
+}
+
+/**
+
+  This function accepts a <MultiKeywordRequest> formatted string, finds the underlying 
+  keyword owners, creates a <MultiConfigRequest> string from it and forwards it to the
+  EFI_HII_ROUTING_PROTOCOL.ExtractConfig function.
+  
+  If there is an issue in resolving the contents of the KeywordString, then the function
+  returns an EFI_INVALID_PARAMETER and also set the Progress and ProgressErr with the
+  appropriate information about where the issue occurred and additional data about the
+  nature of the issue.
+  
+  In the case when KeywordString is NULL, or contains multiple keywords, or when
+  EFI_NOT_FOUND is generated while processing the keyword elements, the Results string
+  contains values returned for all keywords processed prior to the keyword generating the 
+  error but no values for the keyword with error or any following keywords.
+
+  
+  @param This           Pointer to the EFI_KEYWORD_HANDLER _PROTOCOL instance.
+  
+  @param NameSpaceId    A null-terminated string containing the platform configuration
+                        language to search through in the system. If a NULL is passed
+                        in, then it is assumed that any platform configuration language
+                        with the prefix of "x-UEFI-" are searched.
+                        
+  @param KeywordString  A null-terminated string in <MultiKeywordRequest> format. If a
+                        NULL is passed in the KeywordString field, all of the known 
+                        keywords in the system for the NameSpaceId specified are 
+                        returned in the Results field.
+  
+  @param Progress       On return, points to a character in the KeywordString. Points
+                        to the string's NULL terminator if the request was successful. 
+                        Points to the most recent '&' before the first failing string
+                        element if the request was not successful.
+                        
+  @param ProgressErr    If during the processing of the KeywordString there was a
+                        failure, this parameter gives additional information about the 
+                        possible source of the problem. See the definitions in SetData()
+                        for valid value definitions.
+  
+  @param Results        A null-terminated string in <MultiKeywordResp> format is returned
+                        which has all the values filled in for the keywords in the 
+                        KeywordString. This is a callee-allocated field, and must be freed
+                        by the caller after being used. 
+
+  @retval EFI_SUCCESS             The specified action was completed successfully.
+  
+  @retval EFI_INVALID_PARAMETER   One or more of the following are TRUE:
+                                  1.Progress, ProgressErr, or Resuts is NULL.
+                                  2.Parsing of the KeywordString resulted in an error. See
+                                    Progress and ProgressErr for more data.
+  
+
+  @retval EFI_NOT_FOUND           An element of the KeywordString was not found. See
+                                  ProgressErr for more data.
+
+  @retval EFI_NOT_FOUND           The NamespaceId specified was not found.  See ProgressErr
+                                  for more data.
+
+  @retval EFI_OUT_OF_RESOURCES    Required system resources could not be allocated.  See
+                                  ProgressErr for more data.
+                                  
+  @retval EFI_ACCESS_DENIED       The action violated system policy.  See ProgressErr for
+                                  more data.
+
+  @retval EFI_DEVICE_ERROR        An unexpected system error occurred.  See ProgressErr
+                                  for more data.
+
+**/
+EFI_STATUS
+EFIAPI 
+EfiConfigKeywordHandlerGetData (
+  IN EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL  *This,
+  IN CONST EFI_STRING                     NameSpaceId, OPTIONAL
+  IN CONST EFI_STRING                     KeywordString, OPTIONAL
+  OUT EFI_STRING                          *Progress, 
+  OUT UINT32                              *ProgressErr,
+  OUT EFI_STRING                          *Results
+  )
+{
+  CHAR8                               *NameSpace;
+  EFI_STATUS                          Status;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  HII_DATABASE_RECORD                 *DataBaseRecord;
+  CHAR16                              *StringPtr;
+  CHAR16                              *NextStringPtr;  
+  CHAR16                              *KeywordData;
+  EFI_STRING_ID                       KeywordStringId;
+  UINT8                               *OpCode;
+  CHAR16                              *ConfigRequest;
+  CHAR16                              *ValueElement;
+  UINT32                              RetVal;
+  BOOLEAN                             ReadOnly;
+  CHAR16                              *KeywordResp;
+  CHAR16                              *MultiKeywordResp;
+  CHAR16                              *TempString;
+
+  if (This == NULL || Progress == NULL || ProgressErr == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *ProgressErr = KEYWORD_HANDLER_UNDEFINED_PROCESSING_ERROR;
+  Status       = EFI_SUCCESS;
+  DevicePath   = NULL;
+  NameSpace    = NULL;
+  KeywordData  = NULL;
+  ConfigRequest= NULL;
+  StringPtr    = KeywordString;
+  ReadOnly     = FALSE;
+  MultiKeywordResp = NULL;
+  KeywordStringId  = 0;
+  TempString   = NULL;
+
+  //
+  // Use temp string to avoid changing input string buffer.
+  //
+  if (NameSpaceId != NULL) {
+    TempString = AllocateCopyPool (StrSize (NameSpaceId), NameSpaceId);
+    ASSERT (TempString != NULL);
+  }
+  //
+  // 1. Get NameSpace from NameSpaceId keyword.
+  //
+  Status = ExtractNameSpace (TempString, &NameSpace, NULL);
+  if (TempString != NULL) {
+    FreePool (TempString);
+    TempString = NULL;
+  }
+  if (EFI_ERROR (Status)) {
+    *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+    return Status;
+  }
+  //
+  // 1.1 Check whether the input namespace is valid.
+  //
+  if (NameSpace != NULL){
+    if (AsciiStrnCmp(NameSpace, UEFI_CONFIG_LANG, AsciiStrLen (UEFI_CONFIG_LANG)) != 0) {
+      *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  
+  if (KeywordString != NULL) {
+    //
+    // Use temp string to avoid changing input string buffer.
+    //
+    TempString = AllocateCopyPool (StrSize (KeywordString), KeywordString);
+    ASSERT (TempString != NULL);
+    StringPtr = TempString;
+
+    while (*StringPtr != L'\0') {
+      //
+      // 2. Get possible Device Path info from KeywordString.
+      //
+      Status = ExtractDevicePath (StringPtr, (UINT8 **)&DevicePath, &NextStringPtr);
+      if (EFI_ERROR (Status)) {
+        *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+        goto Done;
+      }
+      StringPtr = NextStringPtr;
+
+     
+      //
+      // 3. Process Keyword section from the input keywordRequest string.
+      //
+      // 3.1 Extract keyword from the KeywordRequest string.
+      //
+      Status = ExtractKeyword(StringPtr, &KeywordData, &NextStringPtr);
+      if (EFI_ERROR (Status)) {
+        //
+        // Can't find Keyword base on the input device path info.
+        //
+        *ProgressErr = KEYWORD_HANDLER_MALFORMED_STRING;
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+
+      //
+      // 3.2 Get EFI_STRING_ID for the input keyword.
+      //
+      Status = GetStringIdFromDatabase (&DevicePath, &NameSpace, KeywordData, &RetVal, &KeywordStringId, &DataBaseRecord);
+      if (EFI_ERROR (Status)) {
+        *ProgressErr = RetVal;
+        goto Done;
+      }
+
+      //
+      // 3.3 Construct the ConfigRequest string.
+      //
+      Status = ExtractConfigRequest (DataBaseRecord, KeywordStringId, &OpCode, &ConfigRequest);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      
+      //
+      // 3.4 Extract Value for the input keyword.
+      //
+      Status = ExtractValueFromDriver(ConfigRequest, &ValueElement);
+      if (EFI_ERROR (Status)) {
+        if (Status != EFI_OUT_OF_RESOURCES) {
+          Status = EFI_DEVICE_ERROR;
+        }
+        goto Done;
+      }
+      StringPtr = NextStringPtr;
+
+      //
+      // 4. Process the possible filter section.
+      //
+      RetVal = ValidateFilter (OpCode, StringPtr, &NextStringPtr, &ReadOnly);
+      if (RetVal != KEYWORD_HANDLER_NO_ERROR) {
+        *ProgressErr = RetVal;
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      StringPtr = NextStringPtr;
+
+
+      //
+      // 5. Generate KeywordResp string.
+      //
+      Status = GenerateKeywordResp(NameSpace, DevicePath, KeywordData, ValueElement, ReadOnly, &KeywordResp);
+      if (Status != EFI_SUCCESS) {
+        goto Done;
+      }
+
+      //
+      // 6. Merge to the MultiKeywordResp string.
+      //
+      Status = MergeToMultiKeywordResp(&MultiKeywordResp, &KeywordResp);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // 7. Update return value.
+      //
+      *Results = MultiKeywordResp;
+
+      //
+      // 8. Clean the temp buffer.
+      //
+      FreePool (DevicePath);
+      FreePool (KeywordData);
+      FreePool (ValueElement);
+      FreePool (ConfigRequest);
+      DevicePath = NULL; 
+      KeywordData = NULL;
+      ValueElement = NULL;
+      ConfigRequest = NULL;
+      if (KeywordResp != NULL) {
+        FreePool (KeywordResp);
+        KeywordResp = NULL;
+      }
+    }
+  } else {
+    //
+    // Enumerate all keyword in the system.
+    //
+    Status = EnumerateAllKeywords(NameSpace, &MultiKeywordResp, ProgressErr);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    *Results = MultiKeywordResp;
+  }
+
+  *ProgressErr = KEYWORD_HANDLER_NO_ERROR;
+
+Done:
+  *Progress = KeywordString + (StringPtr - TempString);
+
+  if (TempString != NULL) {
+    FreePool (TempString);
+  }
+  if (NameSpace != NULL) {
+    FreePool (NameSpace);
+  }
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }
+  if (KeywordData != NULL) {
+    FreePool (KeywordData);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigRouting.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigRouting.c
new file mode 100644
index 0000000000..2f53197d46
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/ConfigRouting.c
@@ -0,0 +1,5183 @@
+/** @file
+Implementation of interfaces function for EFI_HII_CONFIG_ROUTING_PROTOCOL.
+
+Copyright (c) 2007 - 2015, 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 "HiiDatabase.h"
+extern HII_DATABASE_PRIVATE_DATA mPrivate;
+
+/**
+  Calculate the number of Unicode characters of the incoming Configuration string,
+  not including NULL terminator.
+
+  This is a internal function.
+
+  @param  String                 String in <MultiConfigRequest> or
+                                 <MultiConfigResp> format.
+
+  @return The number of Unicode characters.
+
+**/
+UINTN
+CalculateConfigStringLen (
+  IN EFI_STRING                    String
+  )
+{
+  EFI_STRING  TmpPtr;
+
+  //
+  // "GUID=" should be the first element of incoming string.
+  //
+  ASSERT (String != NULL);
+  ASSERT (StrnCmp (String, L"GUID=", StrLen (L"GUID=")) == 0);
+
+  //
+  // The beginning of next <ConfigRequest>/<ConfigResp> should be "&GUID=".
+  // Will meet '\0' if there is only one <ConfigRequest>/<ConfigResp>.
+  // 
+  TmpPtr = StrStr (String, L"&GUID=");
+  if (TmpPtr == NULL) {
+    return StrLen (String);
+  }
+
+  return (TmpPtr - String);
+}
+
+
+/**
+  Convert the hex UNICODE %02x encoding of a UEFI device path to binary
+  from <PathHdr> of <ConfigHdr>.
+
+  This is a internal function.
+
+  @param  String                 UEFI configuration string
+  @param  DevicePathData         Binary of a UEFI device path.
+
+  @retval EFI_NOT_FOUND          The device path is not invalid.
+  @retval EFI_INVALID_PARAMETER  Any incoming parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Lake of resources to store neccesary structures.
+  @retval EFI_SUCCESS            The device path is retrieved and translated to
+                                 binary format.
+
+**/
+EFI_STATUS
+GetDevicePath (
+  IN  EFI_STRING                   String,
+  OUT UINT8                        **DevicePathData
+  )
+{
+  UINTN                    Length;
+  EFI_STRING               PathHdr;
+  UINT8                    *DevicePathBuffer;
+  CHAR16                   TemStr[2];
+  UINTN                    Index;
+  UINT8                    DigitUint8;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+
+
+  if (String == NULL || DevicePathData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Find the 'PATH=' of <PathHdr> and skip it.
+  //
+  for (; (*String != 0 && StrnCmp (String, L"PATH=", StrLen (L"PATH=")) != 0); String++);
+  if (*String == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Check whether path data does exist.
+  //
+  String += StrLen (L"PATH=");
+  if (*String == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  PathHdr = String;
+
+  //
+  // The content between 'PATH=' of <ConfigHdr> and '&' of next element
+  // or '\0' (end of configuration string) is the UNICODE %02x bytes encoding
+  // of UEFI device path.
+  //
+  for (Length = 0; *String != 0 && *String != L'&'; String++, Length++);
+  //
+  // Check DevicePath Length
+  //
+  if (((Length + 1) / 2) < sizeof (EFI_DEVICE_PATH_PROTOCOL)) {
+    return EFI_NOT_FOUND;
+  }
+  
+  //
+  // The data in <PathHdr> is encoded as hex UNICODE %02x bytes in the same order
+  // as the device path resides in RAM memory.
+  // Translate the data into binary.
+  //
+  DevicePathBuffer = (UINT8 *) AllocateZeroPool ((Length + 1) / 2);
+  if (DevicePathBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Convert DevicePath
+  //
+  ZeroMem (TemStr, sizeof (TemStr));
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = PathHdr[Index];
+    DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+    if ((Index & 1) == 0) {
+      DevicePathBuffer [Index/2] = DigitUint8;
+    } else {
+      DevicePathBuffer [Index/2] = (UINT8) ((DevicePathBuffer [Index/2] << 4) + DigitUint8);
+    }
+  }
+  
+  //
+  // Validate DevicePath
+  //
+  DevicePath  = (EFI_DEVICE_PATH_PROTOCOL *) DevicePathBuffer;
+  while (!IsDevicePathEnd (DevicePath)) {
+    if ((DevicePath->Type == 0) || (DevicePath->SubType == 0) || (DevicePathNodeLength (DevicePath) < sizeof (EFI_DEVICE_PATH_PROTOCOL))) {
+      //
+      // Invalid device path
+      //
+      FreePool (DevicePathBuffer);
+      return EFI_NOT_FOUND;
+    }
+    DevicePath = NextDevicePathNode (DevicePath);
+  }
+
+  //
+  // return the device path
+  //
+  *DevicePathData = DevicePathBuffer;
+  return EFI_SUCCESS;
+}
+
+/**
+  Converts the unicode character of the string from uppercase to lowercase.
+  This is a internal function.
+
+  @param ConfigString  String to be converted
+
+**/
+VOID
+EFIAPI
+HiiToLower (
+  IN EFI_STRING  ConfigString
+  )
+{
+  EFI_STRING  String;
+  BOOLEAN     Lower;
+
+  ASSERT (ConfigString != NULL);
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  for (String = ConfigString, Lower = FALSE; *String != L'\0'; String++) {
+    if (*String == L'=') {
+      Lower = TRUE;
+    } else if (*String == L'&') {
+      Lower = FALSE;
+    } else if (Lower && *String >= L'A' && *String <= L'F') {
+      *String = (CHAR16) (*String - L'A' + L'a');
+    }
+  }
+
+  return;
+}
+
+/**
+  Generate a sub string then output it.
+
+  This is a internal function.
+
+  @param  String                 A constant string which is the prefix of the to be
+                                 generated string, e.g. GUID=
+
+  @param  BufferLen              The length of the Buffer in bytes.
+
+  @param  Buffer                 Points to a buffer which will be converted to be the 
+                                 content of the generated string.
+
+  @param  Flag                   If 1, the buffer contains data for the value of GUID or PATH stored in 
+                                 UINT8 *; if 2, the buffer contains unicode string for the value of NAME;
+                                 if 3, the buffer contains other data.
+
+  @param  SubStr                 Points to the output string. It's caller's
+                                 responsibility to free this buffer.
+
+
+**/
+VOID
+GenerateSubStr (
+  IN CONST EFI_STRING              String,
+  IN  UINTN                        BufferLen,
+  IN  VOID                         *Buffer,
+  IN  UINT8                        Flag,
+  OUT EFI_STRING                   *SubStr
+  )
+{
+  UINTN       Length;
+  EFI_STRING  Str;
+  EFI_STRING  StringHeader;
+  CHAR16      *TemString;
+  CHAR16      *TemName;
+  UINT8       *TemBuffer;
+  UINTN       Index;
+
+  ASSERT (String != NULL && SubStr != NULL);
+
+  if (Buffer == NULL) {
+    *SubStr = AllocateCopyPool (StrSize (String), String);
+    ASSERT (*SubStr != NULL);
+    return;
+  }
+  
+  //
+  // Header + Data + '&' + '\0'
+  //
+  Length = StrLen (String) + BufferLen * 2 + 1 + 1;
+  Str    = AllocateZeroPool (Length * sizeof (CHAR16));
+  ASSERT (Str != NULL);
+
+  StrCpyS (Str, Length, String);
+
+  StringHeader = Str + StrLen (String);
+  TemString    = (CHAR16 *) StringHeader;
+
+  switch (Flag) {
+  case 1:
+    //
+    // Convert Buffer to Hex String in reverse order
+    //
+    TemBuffer = ((UINT8 *) Buffer);
+    for (Index = 0; Index < BufferLen; Index ++, TemBuffer ++) {
+      TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemBuffer, 2);
+    }
+    break;
+  case 2:
+    //
+    // Check buffer is enough
+    //
+    TemName = (CHAR16 *) Buffer;
+    ASSERT ((BufferLen * 2 + 1) >= (StrLen (TemName) * 4 + 1));
+    //
+    // Convert Unicode String to Config String, e.g. "ABCD" => "0041004200430044"
+    //
+    for (; *TemName != L'\0'; TemName++) {
+      TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemName, 4);
+    }
+    break;
+  case 3:
+    //
+    // Convert Buffer to Hex String
+    //
+    TemBuffer = ((UINT8 *) Buffer) + BufferLen - 1;
+    for (Index = 0; Index < BufferLen; Index ++, TemBuffer --) {
+      TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemBuffer, 2);
+    }
+    break;
+  default:
+    break;
+  }
+
+  //
+  // Convert the uppercase to lowercase since <HexAf> is defined in lowercase format.
+  //
+  StrCatS (Str, Length, L"&");  
+  HiiToLower (Str);
+
+  *SubStr = Str;
+}
+
+
+/**
+  Retrieve the <ConfigBody> from String then output it.
+
+  This is a internal function.
+
+  @param  String                 A sub string of a configuration string in
+                                 <MultiConfigAltResp> format.
+  @param  ConfigBody             Points to the output string. It's caller's
+                                 responsibility to free this buffer.
+
+  @retval EFI_INVALID_PARAMETER  There is no form package in current hii database.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to finish this operation.
+  @retval EFI_SUCCESS            All existing storage is exported.
+
+**/
+EFI_STATUS
+OutputConfigBody (
+  IN  EFI_STRING                   String,
+  OUT EFI_STRING                   *ConfigBody
+  )
+{
+  EFI_STRING  TmpPtr;
+  EFI_STRING  Result;
+  UINTN       Length;
+
+  if (String == NULL || ConfigBody == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // The setting information should start OFFSET, not ALTCFG.
+  //
+  if (StrnCmp (String, L"&ALTCFG=", StrLen (L"&ALTCFG=")) == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TmpPtr = StrStr (String, L"GUID=");
+  if (TmpPtr == NULL) {
+    //
+    // It is the last <ConfigResp> of the incoming configuration string.
+    //
+    Result = AllocateCopyPool (StrSize (String), String);
+    if (Result == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    } else {
+      *ConfigBody = Result;
+      return EFI_SUCCESS;
+    }
+  }
+
+  Length = TmpPtr - String;
+  if (Length == 0) {
+    return EFI_NOT_FOUND;
+  }
+  Result = AllocateCopyPool (Length * sizeof (CHAR16), String);
+  if (Result == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  *(Result + Length - 1) = 0;
+  *ConfigBody = Result;
+  return EFI_SUCCESS;
+}
+
+/**
+  Append a string to a multi-string format.
+
+  This is a internal function.
+
+  @param  MultiString            String in <MultiConfigRequest>,
+                                 <MultiConfigAltResp>, or <MultiConfigResp>. On
+                                 input, the buffer length of  this string is
+                                 MAX_STRING_LENGTH. On output, the  buffer length
+                                 might be updated.
+  @param  AppendString           NULL-terminated Unicode string.
+
+  @retval EFI_INVALID_PARAMETER  Any incoming parameter is invalid.
+  @retval EFI_SUCCESS            AppendString is append to the end of MultiString
+
+**/
+EFI_STATUS
+AppendToMultiString (
+  IN OUT EFI_STRING                *MultiString,
+  IN EFI_STRING                    AppendString
+  )
+{
+  UINTN AppendStringSize;
+  UINTN MultiStringSize;
+  UINTN MaxLen;
+
+  if (MultiString == NULL || *MultiString == NULL || AppendString == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AppendStringSize = StrSize (AppendString);
+  MultiStringSize  = StrSize (*MultiString);
+  MaxLen = MAX_STRING_LENGTH / sizeof (CHAR16);
+
+  //
+  // Enlarge the buffer each time when length exceeds MAX_STRING_LENGTH.
+  //
+  if (MultiStringSize + AppendStringSize > MAX_STRING_LENGTH ||
+      MultiStringSize > MAX_STRING_LENGTH) {
+    *MultiString = (EFI_STRING) ReallocatePool (
+                                  MultiStringSize,
+                                  MultiStringSize + AppendStringSize,
+                                  (VOID *) (*MultiString)
+                                  );
+    MaxLen = (MultiStringSize + AppendStringSize) / sizeof (CHAR16);
+    ASSERT (*MultiString != NULL);
+  }
+  //
+  // Append the incoming string
+  //
+  StrCatS (*MultiString, MaxLen, AppendString);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the value of <Number> in <BlockConfig> format, i.e. the value of OFFSET
+  or WIDTH or VALUE.
+  <BlockConfig> ::= 'OFFSET='<Number>&'WIDTH='<Number>&'VALUE'=<Number>
+
+  This is a internal function.
+
+  @param  StringPtr              String in <BlockConfig> format and points to the
+                                 first character of <Number>.
+  @param  Number                 The output value. Caller takes the responsibility
+                                 to free memory.
+  @param  Len                    Length of the <Number>, in characters.
+
+  @retval EFI_OUT_OF_RESOURCES   Insufficient resources to store neccessary
+                                 structures.
+  @retval EFI_SUCCESS            Value of <Number> is outputted in Number
+                                 successfully.
+
+**/
+EFI_STATUS
+GetValueOfNumber (
+  IN EFI_STRING                    StringPtr,
+  OUT UINT8                        **Number,
+  OUT UINTN                        *Len
+  )
+{
+  EFI_STRING               TmpPtr;
+  UINTN                    Length;
+  EFI_STRING               Str;
+  UINT8                    *Buf;
+  EFI_STATUS               Status;
+  UINT8                    DigitUint8;
+  UINTN                    Index;
+  CHAR16                   TemStr[2];
+
+  if (StringPtr == NULL || *StringPtr == L'\0' || Number == NULL || Len == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Buf = NULL;
+
+  TmpPtr = StringPtr;
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr++;
+  }
+  *Len   = StringPtr - TmpPtr;
+  Length = *Len + 1;
+
+  Str = (EFI_STRING) AllocateZeroPool (Length * sizeof (CHAR16));
+  if (Str == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  CopyMem (Str, TmpPtr, *Len * sizeof (CHAR16));
+  *(Str + *Len) = L'\0';
+
+  Length = (Length + 1) / 2;
+  Buf = (UINT8 *) AllocateZeroPool (Length);
+  if (Buf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  
+  Length = *Len;
+  ZeroMem (TemStr, sizeof (TemStr));
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = Str[Length - Index - 1];
+    DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+    if ((Index & 1) == 0) {
+      Buf [Index/2] = DigitUint8;
+    } else {
+      Buf [Index/2] = (UINT8) ((DigitUint8 << 4) + Buf [Index/2]);
+    }
+  }
+
+  *Number = Buf;
+  Status  = EFI_SUCCESS;
+
+Exit:
+  if (Str != NULL) {
+    FreePool (Str);
+  }
+
+  return Status;
+}
+
+/**
+  This function merges DefaultAltCfgResp string into AltCfgResp string for
+  the missing AltCfgId in AltCfgResq.
+
+  @param  AltCfgResp             Pointer to a null-terminated Unicode string in
+                                 <ConfigAltResp> format. The default value string 
+                                 will be merged into it. 
+  @param  DefaultAltCfgResp      Pointer to a null-terminated Unicode string in
+                                 <MultiConfigAltResp> format. The default value 
+                                 string may contain more than one ConfigAltResp
+                                 string for the different varstore buffer.
+
+  @retval EFI_SUCCESS            The merged string returns.
+  @retval EFI_INVALID_PARAMETER  *AltCfgResp is to NULL.
+**/
+EFI_STATUS
+EFIAPI
+MergeDefaultString (
+  IN OUT EFI_STRING  *AltCfgResp,
+  IN     EFI_STRING  DefaultAltCfgResp
+  )
+{
+  EFI_STRING   StringPtrDefault;
+  EFI_STRING   StringPtrEnd;
+  CHAR16       TempChar;
+  EFI_STRING   StringPtr;
+  EFI_STRING   AltConfigHdr;
+  UINTN        HeaderLength;
+  UINTN        SizeAltCfgResp;
+  UINTN        MaxLen;
+  UINTN        TotalSize;
+  
+  if (*AltCfgResp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Get the requestr ConfigHdr
+  //
+  SizeAltCfgResp  = 0;
+  StringPtr       = *AltCfgResp;
+  
+  //
+  // Find <ConfigHdr> GUID=...&NAME=...&PATH=...
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&NAME=", StrLen (L"&NAME=")) != 0) {
+    StringPtr++;
+  }
+  while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&PATH=", StrLen (L"&PATH=")) != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == L'\0') {
+    return EFI_INVALID_PARAMETER;
+  }
+  StringPtr += StrLen (L"&PATH=");
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr ++;
+  }
+  HeaderLength = StringPtr - *AltCfgResp;
+
+  //
+  // Construct AltConfigHdr string  "&<ConfigHdr>&ALTCFG=XXXX\0"
+  //                                  |1| StrLen (ConfigHdr) | 8 | 4 | 1 |
+  //
+  MaxLen = 1 + HeaderLength + 8 + 4 + 1;
+  AltConfigHdr = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  if (AltConfigHdr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  StrCpyS (AltConfigHdr, MaxLen, L"&");
+  StrnCatS (AltConfigHdr, MaxLen, *AltCfgResp, HeaderLength);
+  StrCatS (AltConfigHdr, MaxLen, L"&ALTCFG=");
+  HeaderLength = StrLen (AltConfigHdr);
+  
+  StringPtrDefault = StrStr (DefaultAltCfgResp, AltConfigHdr);
+  while (StringPtrDefault != NULL) {
+    //
+    // Get AltCfg Name
+    //
+    StrnCatS (AltConfigHdr, MaxLen, StringPtrDefault + HeaderLength, 4);
+    StringPtr = StrStr (*AltCfgResp, AltConfigHdr); 
+    
+    //
+    // Append the found default value string to the input AltCfgResp
+    // 
+    if (StringPtr == NULL) {
+      StringPtrEnd   = StrStr (StringPtrDefault + 1, L"&GUID");
+      SizeAltCfgResp = StrSize (*AltCfgResp);
+      TotalSize = SizeAltCfgResp + StrSize (StringPtrDefault);
+      if (StringPtrEnd == NULL) {
+        //
+        // No more default string is found.
+        //
+        *AltCfgResp    = (EFI_STRING) ReallocatePool (
+                                     SizeAltCfgResp,
+                                     TotalSize,
+                                     (VOID *) (*AltCfgResp)
+                                     );
+        if (*AltCfgResp == NULL) {
+          FreePool (AltConfigHdr);
+          return EFI_OUT_OF_RESOURCES;
+        }
+        StrCatS (*AltCfgResp, TotalSize / sizeof (CHAR16), StringPtrDefault);
+        break;
+      } else {
+        TempChar = *StringPtrEnd;
+        *StringPtrEnd = L'\0';
+        *AltCfgResp = (EFI_STRING) ReallocatePool (
+                                     SizeAltCfgResp,
+                                     TotalSize,
+                                     (VOID *) (*AltCfgResp)
+                                     );
+        if (*AltCfgResp == NULL) {
+          FreePool (AltConfigHdr);
+          return EFI_OUT_OF_RESOURCES;
+        }
+        StrCatS (*AltCfgResp, TotalSize / sizeof (CHAR16), StringPtrDefault);
+        *StringPtrEnd = TempChar;
+      }
+    }
+    
+    //
+    // Find next AltCfg String
+    //
+    *(AltConfigHdr + HeaderLength) = L'\0';
+    StringPtrDefault = StrStr (StringPtrDefault + 1, AltConfigHdr);
+  }
+  
+  FreePool (AltConfigHdr);
+  return EFI_SUCCESS;  
+}
+
+/**
+  This function inserts new DefaultValueData into the BlockData DefaultValue array.
+
+  @param  BlockData         The BlockData is updated to add new default value.
+  @param  DefaultValueData  The DefaultValue is added.
+
+**/
+VOID
+InsertDefaultValue (
+  IN IFR_BLOCK_DATA         *BlockData,
+  IN IFR_DEFAULT_DATA       *DefaultValueData
+  )
+{
+  LIST_ENTRY             *Link;
+  IFR_DEFAULT_DATA       *DefaultValueArray; 
+  LIST_ENTRY             *DefaultLink;
+ 
+  DefaultLink   = &BlockData->DefaultValueEntry;
+
+  for (Link = DefaultLink->ForwardLink; Link != DefaultLink; Link = Link->ForwardLink) {
+    DefaultValueArray = BASE_CR (Link, IFR_DEFAULT_DATA, Entry);
+    if (DefaultValueArray->DefaultId == DefaultValueData->DefaultId) {
+      //
+      // DEFAULT_VALUE_FROM_OPCODE has high priority, DEFAULT_VALUE_FROM_DEFAULT has low priority.
+      //
+      if (DefaultValueData->Type > DefaultValueArray->Type) {
+        //
+        // Update the default value array in BlockData.
+        //
+        CopyMem (&DefaultValueArray->Value, &DefaultValueData->Value, sizeof (EFI_IFR_TYPE_VALUE));
+        DefaultValueArray->Type  = DefaultValueData->Type;
+        DefaultValueArray->Cleaned = DefaultValueData->Cleaned;
+      }
+      return;
+    } 
+  }
+
+  //
+  // Insert new default value data in tail.
+  //
+  DefaultValueArray = AllocateZeroPool (sizeof (IFR_DEFAULT_DATA));
+  ASSERT (DefaultValueArray != NULL);
+  CopyMem (DefaultValueArray, DefaultValueData, sizeof (IFR_DEFAULT_DATA));
+  InsertTailList (Link, &DefaultValueArray->Entry);
+}
+
+/**
+  This function inserts new BlockData into the block link
+
+  @param  BlockLink      The list entry points to block array.
+  @param  BlockData      The point to BlockData is added.
+  
+**/
+VOID
+InsertBlockData (
+  IN LIST_ENTRY        *BlockLink,
+  IN IFR_BLOCK_DATA    **BlockData
+  )
+{
+  LIST_ENTRY          *Link;
+  IFR_BLOCK_DATA      *BlockArray;
+  IFR_BLOCK_DATA      *BlockSingleData;
+
+  BlockSingleData = *BlockData;
+
+  if (BlockSingleData->Name != NULL) {
+    InsertTailList (BlockLink, &BlockSingleData->Entry);
+    return;
+  }
+
+  //
+  // Insert block data in its Offset and Width order.
+  //
+  for (Link = BlockLink->ForwardLink; Link != BlockLink; Link = Link->ForwardLink) {
+    BlockArray = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    if (BlockArray->Offset == BlockSingleData->Offset) {
+      if (BlockArray->Width > BlockSingleData->Width) {
+        //
+        // Insert this block data in the front of block array
+        //
+        InsertTailList (Link, &BlockSingleData->Entry);
+        return;
+      }
+
+      if (BlockArray->Width == BlockSingleData->Width) {
+        //
+        // The same block array has been added.
+        //
+        if (BlockSingleData != BlockArray) {
+          FreePool (BlockSingleData);
+          *BlockData = BlockArray;
+        }
+        return;
+      }
+    } else if (BlockArray->Offset > BlockSingleData->Offset) {
+      //
+      // Insert new block data in the front of block array 
+      //
+      InsertTailList (Link, &BlockSingleData->Entry);
+      return;
+    }
+  }
+  
+  //
+  // Add new block data into the tail.
+  //
+  InsertTailList (Link, &BlockSingleData->Entry); 
+}
+
+/**
+  Retrieves a pointer to the a Null-terminated ASCII string containing the list 
+  of languages that an HII handle in the HII Database supports.  The returned 
+  string is allocated using AllocatePool().  The caller is responsible for freeing
+  the returned string using FreePool().  The format of the returned string follows
+  the language format assumed the HII Database.
+  
+  If HiiHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+
+  @retval NULL   HiiHandle is not registered in the HII database
+  @retval NULL   There are not enough resources available to retrieve the suported 
+                 languages.
+  @retval NULL   The list of suported languages could not be retrieved.
+  @retval Other  A pointer to the Null-terminated ASCII string of supported languages.
+
+**/
+CHAR8 *
+GetSupportedLanguages (
+  IN EFI_HII_HANDLE           HiiHandle
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       LanguageSize;
+  CHAR8       TempSupportedLanguages;
+  CHAR8       *SupportedLanguages;
+
+  ASSERT (HiiHandle != NULL);
+
+  //
+  // Retrieve the size required for the supported languages buffer.
+  //
+  LanguageSize = 0;
+  Status = mPrivate.HiiString.GetLanguages (&mPrivate.HiiString, HiiHandle, &TempSupportedLanguages, &LanguageSize);
+
+  //
+  // If GetLanguages() returns EFI_SUCCESS for a zero size, 
+  // then there are no supported languages registered for HiiHandle.  If GetLanguages() 
+  // returns an error other than EFI_BUFFER_TOO_SMALL, then HiiHandle is not present
+  // in the HII Database
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    //
+    // Return NULL if the size can not be retrieved, or if HiiHandle is not in the HII Database
+    //
+    return NULL;
+  }
+
+  //
+  // Allocate the supported languages buffer.
+  //
+  SupportedLanguages = AllocateZeroPool (LanguageSize);
+  if (SupportedLanguages == NULL) {
+    //
+    // Return NULL if allocation fails.
+    //
+    return NULL;
+  }
+
+  //
+  // Retrieve the supported languages string
+  //
+  Status = mPrivate.HiiString.GetLanguages (&mPrivate.HiiString, HiiHandle, SupportedLanguages, &LanguageSize);
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the buffer and return NULL if the supported languages can not be retrieved.
+    //
+    FreePool (SupportedLanguages);
+    return NULL;
+  }
+
+  //
+  // Return the Null-terminated ASCII string of supported languages
+  //
+  return SupportedLanguages;
+}
+
+/**
+  Retrieves a string from a string package.
+  
+  If HiiHandle is NULL, then ASSERT().
+  If StringId is 0, then ASSET.
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+  @param[in]  StringId   The identifier of the string to retrieved from the string 
+                         package associated with HiiHandle.
+
+  @retval NULL   The string specified by StringId is not present in the string package.
+  @retval Other  The string was returned.
+
+**/
+EFI_STRING
+InternalGetString (
+  IN EFI_HII_HANDLE  HiiHandle,
+  IN EFI_STRING_ID   StringId
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       StringSize;
+  CHAR16      TempString;
+  EFI_STRING  String;
+  CHAR8       *SupportedLanguages;
+  CHAR8       *PlatformLanguage;
+  CHAR8       *BestLanguage;
+  CHAR8       *Language;
+
+  ASSERT (HiiHandle != NULL);
+  ASSERT (StringId != 0);
+
+  //
+  // Initialize all allocated buffers to NULL
+  // 
+  SupportedLanguages = NULL;
+  PlatformLanguage   = NULL;
+  BestLanguage       = NULL;
+  String             = NULL;
+  Language           = "";
+
+  //
+  // Get the languages that the package specified by HiiHandle supports
+  //
+  SupportedLanguages = GetSupportedLanguages (HiiHandle);
+  if (SupportedLanguages == NULL) {
+    goto Error;
+  }
+
+  //
+  // Get the current platform language setting
+  //
+  GetEfiGlobalVariable2 (L"PlatformLang", (VOID**)&PlatformLanguage, NULL);
+
+  //
+  // Get the best matching language from SupportedLanguages
+  //
+  BestLanguage = GetBestLanguage (
+                   SupportedLanguages, 
+                   FALSE,                                             // RFC 4646 mode
+                   Language,                                          // Highest priority 
+                   PlatformLanguage != NULL ? PlatformLanguage : "",  // Next highest priority
+                   SupportedLanguages,                                // Lowest priority 
+                   NULL
+                   );
+  if (BestLanguage == NULL) {
+    goto Error;
+  }
+
+  //
+  // Retrieve the size of the string in the string package for the BestLanguage
+  //
+  StringSize = 0;
+  Status = mPrivate.HiiString.GetString (
+                         &mPrivate.HiiString,
+                         BestLanguage,
+                         HiiHandle,
+                         StringId,
+                         &TempString,
+                         &StringSize,
+                         NULL
+                         );
+  //
+  // If GetString() returns EFI_SUCCESS for a zero size, 
+  // then there are no supported languages registered for HiiHandle.  If GetString() 
+  // returns an error other than EFI_BUFFER_TOO_SMALL, then HiiHandle is not present
+  // in the HII Database
+  //
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    goto Error;
+  }
+
+  //
+  // Allocate a buffer for the return string
+  //
+  String = AllocateZeroPool (StringSize);
+  if (String == NULL) {
+    goto Error;
+  }
+
+  //
+  // Retrieve the string from the string package
+  //
+  Status = mPrivate.HiiString.GetString (
+                         &mPrivate.HiiString,
+                         BestLanguage,
+                         HiiHandle,
+                         StringId,
+                         String,
+                         &StringSize,
+                         NULL
+                         );
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the buffer and return NULL if the supported languages can not be retrieved.
+    //
+    FreePool (String);
+    String = NULL;
+  }
+
+Error:
+  //
+  // Free allocated buffers
+  //
+  if (SupportedLanguages != NULL) {
+    FreePool (SupportedLanguages);
+  }
+  if (PlatformLanguage != NULL) {
+    FreePool (PlatformLanguage);
+  }
+  if (BestLanguage != NULL) {
+    FreePool (BestLanguage);
+  }
+
+  //
+  // Return the Null-terminated Unicode string
+  //
+  return String;
+}
+
+/**
+  This function checks VarOffset and VarWidth is in the block range.
+
+  @param  RequestBlockArray  The block array is to be checked. 
+  @param  VarOffset          Offset of var to the structure
+  @param  VarWidth           Width of var.
+  @param  IsNameValueType    Whether this varstore is name/value varstore or not.
+  @param  HiiHandle          Hii handle for this hii package.
+  
+  @retval TRUE   This Var is in the block range.
+  @retval FALSE  This Var is not in the block range.
+**/
+BOOLEAN
+BlockArrayCheck (
+  IN IFR_BLOCK_DATA  *RequestBlockArray,
+  IN UINT16          VarOffset,
+  IN UINT16          VarWidth,
+  IN BOOLEAN         IsNameValueType,
+  IN EFI_HII_HANDLE  HiiHandle
+  )
+{
+  LIST_ENTRY          *Link;
+  IFR_BLOCK_DATA      *BlockData;
+  EFI_STRING          Name;
+
+  //
+  // No Request Block array, all vars are got.
+  //
+  if (RequestBlockArray == NULL) {
+    return TRUE;
+  }
+
+  //
+  // Check the input var is in the request block range.
+  //
+  for (Link = RequestBlockArray->Entry.ForwardLink; Link != &RequestBlockArray->Entry; Link = Link->ForwardLink) {
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+
+    if (IsNameValueType) {
+      Name = InternalGetString (HiiHandle, VarOffset);
+      ASSERT (Name != NULL);
+
+      if (StrnCmp (BlockData->Name, Name, StrLen (Name)) == 0) {
+        FreePool (Name);
+        return TRUE;
+      }
+      FreePool (Name);
+    } else {
+      if ((VarOffset >= BlockData->Offset) && ((VarOffset + VarWidth) <= (BlockData->Offset + BlockData->Width))) {
+        return TRUE;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Get form package data from data base.
+
+  @param  DataBaseRecord         The DataBaseRecord instance contains the found Hii handle and package.
+  @param  HiiFormPackage         The buffer saves the package data.
+  @param  PackageSize            The buffer size of the package data.
+
+**/
+EFI_STATUS
+GetFormPackageData (
+  IN     HII_DATABASE_RECORD        *DataBaseRecord,
+  IN OUT UINT8                      **HiiFormPackage,
+  OUT    UINTN                      *PackageSize
+  )
+{
+  EFI_STATUS                   Status;
+  UINTN                        Size;
+  UINTN                        ResultSize;
+
+  if (DataBaseRecord == NULL || HiiFormPackage == NULL || PackageSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Size       = 0;
+  ResultSize = 0;
+  //
+  // 0. Get Hii Form Package by HiiHandle
+  //
+  Status = ExportFormPackages (
+             &mPrivate, 
+             DataBaseRecord->Handle, 
+             DataBaseRecord->PackageList, 
+             0, 
+             Size, 
+             HiiFormPackage,
+             &ResultSize
+           );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+ 
+  (*HiiFormPackage) = AllocatePool (ResultSize);
+  if (*HiiFormPackage == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    return Status;
+  }
+
+  //
+  // Get HiiFormPackage by HiiHandle
+  //
+  Size   = ResultSize;
+  ResultSize    = 0;
+  Status = ExportFormPackages (
+             &mPrivate, 
+             DataBaseRecord->Handle, 
+             DataBaseRecord->PackageList, 
+             0,
+             Size, 
+             *HiiFormPackage,
+             &ResultSize
+           );
+  if (EFI_ERROR (Status)) {
+    FreePool (*HiiFormPackage);
+  }
+  
+  *PackageSize = Size;
+
+  return Status;
+}
+
+
+/**
+  This function parses Form Package to get the efi varstore info according to the request ConfigHdr.
+
+  @param  DataBaseRecord        The DataBaseRecord instance contains the found Hii handle and package.
+  @param  ConfigHdr             Request string ConfigHdr. If it is NULL,
+                                the first found varstore will be as ConfigHdr.
+  @param  IsEfiVarstore         Whether the request storage type is efi varstore type.
+  @param  EfiVarStore           The efi varstore info which will return.
+**/                                
+EFI_STATUS
+GetVarStoreType (
+  IN     HII_DATABASE_RECORD        *DataBaseRecord,
+  IN     EFI_STRING                 ConfigHdr,
+  OUT    BOOLEAN                    *IsEfiVarstore,
+  OUT    EFI_IFR_VARSTORE_EFI       **EfiVarStore
+  )
+{
+  EFI_STATUS               Status;
+  UINTN                    IfrOffset;
+  UINTN                    PackageOffset;
+  EFI_IFR_OP_HEADER        *IfrOpHdr;
+  CHAR16                   *VarStoreName;
+  UINTN                    NameSize;
+  EFI_STRING               GuidStr;
+  EFI_STRING               NameStr;
+  EFI_STRING               TempStr;
+  UINTN                    LengthString;  
+  UINT8                    *HiiFormPackage;
+  UINTN                    PackageSize;
+  EFI_IFR_VARSTORE_EFI     *IfrEfiVarStore;
+  EFI_HII_PACKAGE_HEADER   *PackageHeader;
+  
+  HiiFormPackage = NULL;
+  LengthString     = 0;
+  Status           = EFI_SUCCESS;
+  GuidStr          = NULL;
+  NameStr          = NULL;
+  TempStr          = NULL;
+  *IsEfiVarstore   = FALSE;
+
+  Status = GetFormPackageData(DataBaseRecord, &HiiFormPackage, &PackageSize);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  IfrOffset     = sizeof (EFI_HII_PACKAGE_HEADER);
+  PackageOffset = IfrOffset;
+  PackageHeader = (EFI_HII_PACKAGE_HEADER *) HiiFormPackage;
+
+  while (IfrOffset < PackageSize) {
+    //
+    // More than one form packages exist.
+    //
+    if (PackageOffset >= PackageHeader->Length) {
+        //
+        // Process the new form package.
+        //
+        PackageOffset = sizeof (EFI_HII_PACKAGE_HEADER);
+        IfrOffset    += PackageOffset;
+        PackageHeader = (EFI_HII_PACKAGE_HEADER *) (HiiFormPackage + IfrOffset);
+    }
+
+    IfrOpHdr  = (EFI_IFR_OP_HEADER *) (HiiFormPackage + IfrOffset);
+    IfrOffset += IfrOpHdr->Length;
+    PackageOffset += IfrOpHdr->Length;
+
+    if (IfrOpHdr->OpCode == EFI_IFR_VARSTORE_EFI_OP ) {
+      IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpHdr;
+      //
+      // If the length is small than the structure, this is from old efi 
+      // varstore definition. Old efi varstore get config directly from 
+      // GetVariable function.
+      //
+      if (IfrOpHdr->Length < sizeof (EFI_IFR_VARSTORE_EFI)) {
+        continue;
+      }
+
+      NameSize = AsciiStrSize ((CHAR8 *)IfrEfiVarStore->Name);
+      VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+      if (VarStoreName == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      AsciiStrToUnicodeStrS ((CHAR8 *) IfrEfiVarStore->Name, VarStoreName, NameSize);
+
+      GenerateSubStr (L"GUID=", sizeof (EFI_GUID), (VOID *) &IfrEfiVarStore->Guid, 1, &GuidStr);
+      GenerateSubStr (L"NAME=", StrLen (VarStoreName) * sizeof (CHAR16), (VOID *) VarStoreName, 2, &NameStr);
+      LengthString = StrLen (GuidStr);
+      LengthString = LengthString + StrLen (NameStr) + 1;
+      TempStr = AllocateZeroPool (LengthString * sizeof (CHAR16));
+      if (TempStr == NULL) {
+        FreePool (GuidStr);
+        FreePool (NameStr);
+        FreePool (VarStoreName);
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      StrCpyS (TempStr, LengthString, GuidStr);
+      StrCatS (TempStr, LengthString, NameStr);
+      if (ConfigHdr == NULL || StrnCmp (ConfigHdr, TempStr, StrLen (TempStr)) == 0) {
+        *EfiVarStore = (EFI_IFR_VARSTORE_EFI *) AllocateZeroPool (IfrOpHdr->Length);
+        if (*EfiVarStore == NULL) {
+          FreePool (VarStoreName);
+          FreePool (GuidStr);
+          FreePool (NameStr);
+          FreePool (TempStr);
+          Status = EFI_OUT_OF_RESOURCES;
+          goto Done;
+        }
+        *IsEfiVarstore = TRUE;
+        CopyMem (*EfiVarStore, IfrEfiVarStore, IfrOpHdr->Length);
+      } 
+        
+      //
+      // Free alllocated temp string.
+      //
+      FreePool (VarStoreName);
+      FreePool (GuidStr);
+      FreePool (NameStr);
+      FreePool (TempStr);
+
+      //
+      // Already found the varstore, break;
+      //
+      if (*IsEfiVarstore) {
+        break;
+      }
+    }
+  }
+Done:
+  if (HiiFormPackage != NULL) {
+    FreePool (HiiFormPackage);
+  }
+
+  return Status;
+}
+
+/**
+  Check whether the ConfigRequest string has the request elements.
+  For EFI_HII_VARSTORE_BUFFER type, the request has "&OFFSET=****&WIDTH=****..." format.
+  For EFI_HII_VARSTORE_NAME_VALUE type, the request has "&NAME1**&NAME2..." format.
+
+  @param  ConfigRequest      The input config request string.
+
+  @retval  TRUE              The input include config request elements.
+  @retval  FALSE             The input string not includes.
+
+**/
+BOOLEAN
+GetElementsFromRequest (
+  IN EFI_STRING    ConfigRequest
+  )
+{
+  EFI_STRING   TmpRequest;
+
+  TmpRequest = StrStr (ConfigRequest, L"PATH=");
+  ASSERT (TmpRequest != NULL);
+
+  if ((StrStr (TmpRequest, L"&OFFSET=") != NULL) || (StrStr (TmpRequest, L"&") != NULL)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Check whether the this varstore is the request varstore.
+
+  @param  VarstoreGuid      Varstore guid.
+  @param  Name              Varstore name.
+  @param  ConfigHdr         Current configRequest info.
+
+  @retval  TRUE              This varstore is the requst one.
+  @retval  FALSE             This varstore is not the requst one.
+                                 
+**/
+BOOLEAN
+IsThisVarstore (
+  IN EFI_GUID    *VarstoreGuid,
+  IN CHAR16      *Name,
+  IN CHAR16      *ConfigHdr
+  )
+{
+  EFI_STRING               GuidStr;
+  EFI_STRING               NameStr;
+  EFI_STRING               TempStr;
+  UINTN                    LengthString;
+  BOOLEAN                  RetVal;
+
+  RetVal       = FALSE;
+  GuidStr      = NULL;
+  TempStr      = NULL;
+
+  //
+  // If ConfigHdr has name field and varstore not has name, return FALSE.
+  //
+  if (Name == NULL && ConfigHdr != NULL && StrStr (ConfigHdr, L"NAME=&") == NULL) {
+    return FALSE;
+  }
+
+  GenerateSubStr (L"GUID=", sizeof (EFI_GUID), (VOID *)VarstoreGuid, 1, &GuidStr);
+  if (Name != NULL) {
+    GenerateSubStr (L"NAME=", StrLen (Name) * sizeof (CHAR16), (VOID *) Name, 2, &NameStr);
+  } else {
+    GenerateSubStr (L"NAME=", 0, NULL, 2, &NameStr);
+  }
+  LengthString = StrLen (GuidStr);
+  LengthString = LengthString + StrLen (NameStr) + 1;
+  TempStr = AllocateZeroPool (LengthString * sizeof (CHAR16));
+  if (TempStr == NULL) {
+    goto Done;
+  }
+
+  StrCpyS (TempStr, LengthString, GuidStr);
+  StrCatS (TempStr, LengthString, NameStr);
+
+  if (ConfigHdr == NULL || StrnCmp (ConfigHdr, TempStr, StrLen (TempStr)) == 0) {
+    RetVal = TRUE;
+  }
+
+Done:
+  if (GuidStr != NULL) {
+    FreePool (GuidStr); 
+  }
+
+  if (NameStr != NULL) {
+    FreePool (NameStr);
+  }
+
+  if (TempStr != NULL) {
+    FreePool (TempStr);
+  }
+
+  return RetVal;
+}
+
+/**
+  This function parses Form Package to get the efi varstore info according to the request ConfigHdr.
+
+  @param  DataBaseRecord        The DataBaseRecord instance contains the found Hii handle and package.
+  @param  ConfigHdr             Request string ConfigHdr. If it is NULL,
+                                the first found varstore will be as ConfigHdr.
+  @retval  TRUE                 This hii package is the reqeust one.
+  @retval  FALSE                This hii package is not the reqeust one.
+**/                                
+BOOLEAN
+IsThisPackageList (
+  IN     HII_DATABASE_RECORD        *DataBaseRecord,
+  IN     EFI_STRING                 ConfigHdr
+  )
+{
+  EFI_STATUS               Status;
+  UINTN                    IfrOffset;
+  UINTN                    PackageOffset;
+  EFI_IFR_OP_HEADER        *IfrOpHdr;
+  CHAR16                   *VarStoreName;
+  UINTN                    NameSize;
+  UINT8                    *HiiFormPackage;
+  UINTN                    PackageSize;
+  EFI_IFR_VARSTORE_EFI     *IfrEfiVarStore;
+  EFI_HII_PACKAGE_HEADER   *PackageHeader;
+  EFI_IFR_VARSTORE         *IfrVarStore;
+  EFI_IFR_VARSTORE_NAME_VALUE *IfrNameValueVarStore;
+  BOOLEAN                  FindVarstore;
+
+  HiiFormPackage   = NULL;
+  VarStoreName     = NULL;
+  Status           = EFI_SUCCESS;
+  FindVarstore     = FALSE;
+
+  Status = GetFormPackageData(DataBaseRecord, &HiiFormPackage, &PackageSize);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  IfrOffset     = sizeof (EFI_HII_PACKAGE_HEADER);
+  PackageOffset = IfrOffset;
+  PackageHeader = (EFI_HII_PACKAGE_HEADER *) HiiFormPackage;
+
+  while (IfrOffset < PackageSize) {
+    //
+    // More than one form packages exist.
+    //
+    if (PackageOffset >= PackageHeader->Length) {
+        //
+        // Process the new form package.
+        //
+        PackageOffset = sizeof (EFI_HII_PACKAGE_HEADER);
+        IfrOffset    += PackageOffset;
+        PackageHeader = (EFI_HII_PACKAGE_HEADER *) (HiiFormPackage + IfrOffset);
+    }
+
+    IfrOpHdr  = (EFI_IFR_OP_HEADER *) (HiiFormPackage + IfrOffset);
+    IfrOffset += IfrOpHdr->Length;
+    PackageOffset += IfrOpHdr->Length;
+
+    switch (IfrOpHdr->OpCode) {
+    
+    case EFI_IFR_VARSTORE_OP:
+      IfrVarStore = (EFI_IFR_VARSTORE *) IfrOpHdr;
+
+      NameSize = AsciiStrSize ((CHAR8 *)IfrVarStore->Name);
+      VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+      if (VarStoreName == NULL) {
+        goto Done;
+      }
+      AsciiStrToUnicodeStrS ((CHAR8 *)IfrVarStore->Name, VarStoreName, NameSize);
+
+      if (IsThisVarstore((VOID *)&IfrVarStore->Guid, VarStoreName, ConfigHdr)) {
+        FindVarstore = TRUE;
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_VARSTORE_EFI_OP:
+      IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpHdr;
+      NameSize = AsciiStrSize ((CHAR8 *)IfrEfiVarStore->Name);
+      VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+      if (VarStoreName == NULL) {
+        goto Done;
+      }
+      AsciiStrToUnicodeStrS ((CHAR8 *)IfrEfiVarStore->Name, VarStoreName, NameSize);
+
+      if (IsThisVarstore (&IfrEfiVarStore->Guid, VarStoreName, ConfigHdr)) {
+        FindVarstore = TRUE;
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+      IfrNameValueVarStore = (EFI_IFR_VARSTORE_NAME_VALUE *) IfrOpHdr;
+
+      if (IsThisVarstore (&IfrNameValueVarStore->Guid, NULL, ConfigHdr)) {
+        FindVarstore = TRUE;
+        goto Done;
+      }
+      break;
+      
+    case EFI_IFR_FORM_OP:
+    case EFI_IFR_FORM_MAP_OP:
+      //
+      // No matched varstore is found and directly return.
+      //
+      goto Done;
+
+    default:
+      break;
+    }
+  }
+Done:
+  if (HiiFormPackage != NULL) {
+    FreePool (HiiFormPackage);
+  }
+
+  if (VarStoreName != NULL) {
+    FreePool (VarStoreName);
+  }
+
+  return FindVarstore;
+}
+
+/**
+  Check whether the this op code is required.
+
+  @param  RequestBlockArray      The array includes all the request info or NULL.
+  @param  HiiHandle              The hii handle for this form package.
+  @param  VarStorageData         The varstore data strucure.
+  @param  IfrOpHdr               Ifr opcode header for this opcode.
+  @param  VarWidth               The buffer width for this opcode.
+  @param  ReturnData             The data block added for this opcode.
+
+  @retval  EFI_SUCCESS           This opcode is required.
+  @retval  Others                This opcode is not required or error occur.
+                                 
+**/
+EFI_STATUS
+IsThisOpcodeRequired (
+  IN     IFR_BLOCK_DATA           *RequestBlockArray,
+  IN     EFI_HII_HANDLE           HiiHandle,
+  IN OUT IFR_VARSTORAGE_DATA      *VarStorageData,
+  IN     EFI_IFR_OP_HEADER        *IfrOpHdr,
+  IN     UINT16                   VarWidth,
+  OUT    IFR_BLOCK_DATA           **ReturnData
+  )
+{
+  IFR_BLOCK_DATA           *BlockData;
+  UINT16                   VarOffset;
+  EFI_STRING_ID            NameId;
+  EFI_IFR_QUESTION_HEADER  *IfrQuestionHdr;
+
+  NameId    = 0;
+  VarOffset = 0;
+  IfrQuestionHdr = (EFI_IFR_QUESTION_HEADER  *)((CHAR8 *) IfrOpHdr + sizeof (EFI_IFR_OP_HEADER));
+
+  if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    NameId = IfrQuestionHdr->VarStoreInfo.VarName;
+
+    //
+    // Check whether this question is in requested block array.
+    //
+    if (!BlockArrayCheck (RequestBlockArray, NameId, 0, TRUE, HiiHandle)) {
+      //
+      // This question is not in the requested string. Skip it.
+      //
+      return EFI_SUCCESS;
+    }
+  } else {
+    VarOffset = IfrQuestionHdr->VarStoreInfo.VarOffset;
+    
+    //
+    // Check whether this question is in requested block array.
+    //
+    if (!BlockArrayCheck (RequestBlockArray, VarOffset, VarWidth, FALSE, HiiHandle)) {
+      //
+      // This question is not in the requested string. Skip it.
+      //
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Check this var question is in the var storage 
+    //
+    if (((VarOffset + VarWidth) > VarStorageData->Size)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  BlockData = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+  if (BlockData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    BlockData->Name   = InternalGetString(HiiHandle, NameId);
+  } else {
+    BlockData->Offset = VarOffset;
+  }
+
+  BlockData->Width      = VarWidth;
+  BlockData->QuestionId = IfrQuestionHdr->QuestionId;
+  BlockData->OpCode     = IfrOpHdr->OpCode;
+  BlockData->Scope      = IfrOpHdr->Scope;
+  InitializeListHead (&BlockData->DefaultValueEntry);
+  //
+  // Add Block Data into VarStorageData BlockEntry
+  //
+  InsertBlockData (&VarStorageData->BlockEntry, &BlockData);
+  *ReturnData = BlockData;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function parses Form Package to get the block array and the default
+  value array according to the request ConfigHdr.
+
+  @param  HiiHandle             Hii Handle for this hii package.
+  @param  Package               Pointer to the form package data.
+  @param  PackageLength         Length of the pacakge.
+  @param  ConfigHdr             Request string ConfigHdr. If it is NULL,
+                                the first found varstore will be as ConfigHdr.
+  @param  RequestBlockArray     The block array is retrieved from the request string.
+  @param  VarStorageData        VarStorage structure contains the got block and default value.
+  @param  DefaultIdArray        Point to the got default id and default name array.
+
+  @retval EFI_SUCCESS           The block array and the default value array are got.
+  @retval EFI_INVALID_PARAMETER The varstore defintion in the differnt form pacakges
+                                are conflicted. 
+  @retval EFI_OUT_OF_RESOURCES  No enough memory.
+**/
+EFI_STATUS
+EFIAPI
+ParseIfrData (
+  IN     EFI_HII_HANDLE      HiiHandle,
+  IN     UINT8               *Package,
+  IN     UINT32              PackageLength,
+  IN     EFI_STRING          ConfigHdr,
+  IN     IFR_BLOCK_DATA      *RequestBlockArray,
+  IN OUT IFR_VARSTORAGE_DATA *VarStorageData,
+  OUT    IFR_DEFAULT_DATA    *DefaultIdArray
+  )
+{
+  EFI_STATUS               Status;
+  UINTN                    IfrOffset;
+  UINTN                    PackageOffset;
+  EFI_IFR_VARSTORE         *IfrVarStore;
+  EFI_IFR_VARSTORE_EFI     *IfrEfiVarStore;
+  EFI_IFR_OP_HEADER        *IfrOpHdr;
+  EFI_IFR_ONE_OF           *IfrOneOf;
+  EFI_IFR_REF4             *IfrRef;
+  EFI_IFR_ONE_OF_OPTION    *IfrOneOfOption;
+  EFI_IFR_DEFAULT          *IfrDefault;
+  EFI_IFR_ORDERED_LIST     *IfrOrderedList;
+  EFI_IFR_CHECKBOX         *IfrCheckBox;
+  EFI_IFR_PASSWORD         *IfrPassword;
+  EFI_IFR_STRING           *IfrString;
+  EFI_IFR_DATE             *IfrDate;
+  EFI_IFR_TIME             *IfrTime;
+  IFR_DEFAULT_DATA         DefaultData;
+  IFR_DEFAULT_DATA         *DefaultDataPtr;
+  IFR_BLOCK_DATA           *BlockData;
+  CHAR16                   *VarStoreName;
+  UINTN                    NameSize;
+  UINT16                   VarWidth;
+  UINT16                   VarDefaultId;
+  BOOLEAN                  FirstOneOfOption;
+  BOOLEAN                  FirstOrderedList;
+  LIST_ENTRY               *LinkData;
+  LIST_ENTRY               *LinkDefault;
+  EFI_IFR_VARSTORE_NAME_VALUE *IfrNameValueVarStore;
+  EFI_HII_PACKAGE_HEADER   *PackageHeader;
+  EFI_VARSTORE_ID          VarStoreId;
+
+  Status           = EFI_SUCCESS;
+  BlockData        = NULL;
+  DefaultDataPtr   = NULL;
+  FirstOneOfOption = FALSE;
+  VarStoreId       = 0;
+  FirstOrderedList = FALSE;
+  ZeroMem (&DefaultData, sizeof (IFR_DEFAULT_DATA));
+
+  //
+  // Go through the form package to parse OpCode one by one.
+  //
+  PackageOffset = sizeof (EFI_HII_PACKAGE_HEADER);
+  PackageHeader = (EFI_HII_PACKAGE_HEADER *) Package;
+  IfrOffset     = PackageOffset;
+  while (IfrOffset < PackageLength) {
+
+    //
+    // More than one form package found.
+    //
+    if (PackageOffset >= PackageHeader->Length) {
+        //
+        // Already found varstore for this request, break;
+        //
+        if (VarStoreId != 0) {
+          VarStoreId = 0;
+        }
+
+        //
+        // Get next package header info.
+        //
+        IfrOffset    += sizeof (EFI_HII_PACKAGE_HEADER);
+        PackageOffset = sizeof (EFI_HII_PACKAGE_HEADER);
+        PackageHeader = (EFI_HII_PACKAGE_HEADER *) (Package + IfrOffset);
+    }
+
+    IfrOpHdr  = (EFI_IFR_OP_HEADER *) (Package + IfrOffset);
+    switch (IfrOpHdr->OpCode) {
+    case EFI_IFR_VARSTORE_OP:
+      //
+      // VarStore is found. Don't need to search any more.
+      //
+      if (VarStoreId != 0) {
+        break;
+      }
+
+      IfrVarStore = (EFI_IFR_VARSTORE *) IfrOpHdr;
+
+      NameSize = AsciiStrSize ((CHAR8 *)IfrVarStore->Name);
+      VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+      if (VarStoreName == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      AsciiStrToUnicodeStrS ((CHAR8 *)IfrVarStore->Name, VarStoreName, NameSize);
+
+      if (IsThisVarstore((VOID *)&IfrVarStore->Guid, VarStoreName, ConfigHdr)) {
+        //
+        // Find the matched VarStore
+        //
+        CopyGuid (&VarStorageData->Guid, (EFI_GUID *) (VOID *) &IfrVarStore->Guid);
+        VarStorageData->Size       = IfrVarStore->Size;
+        VarStorageData->Name       = VarStoreName;
+        VarStorageData->Type       = EFI_HII_VARSTORE_BUFFER;
+        VarStoreId                 = IfrVarStore->VarStoreId;
+      }
+      break;
+
+    case EFI_IFR_VARSTORE_EFI_OP:
+      //
+      // VarStore is found. Don't need to search any more.
+      //
+      if (VarStoreId != 0) {
+        break;
+      }
+
+      IfrEfiVarStore = (EFI_IFR_VARSTORE_EFI *) IfrOpHdr;
+
+      //
+      // If the length is small than the structure, this is from old efi 
+      // varstore definition. Old efi varstore get config directly from 
+      // GetVariable function.
+      //      
+      if (IfrOpHdr->Length < sizeof (EFI_IFR_VARSTORE_EFI)) {
+        break;
+      }
+
+      NameSize = AsciiStrSize ((CHAR8 *)IfrEfiVarStore->Name);
+      VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+      if (VarStoreName == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      AsciiStrToUnicodeStrS ((CHAR8 *)IfrEfiVarStore->Name, VarStoreName, NameSize);
+
+      if (IsThisVarstore (&IfrEfiVarStore->Guid, VarStoreName, ConfigHdr)) {
+        //
+        // Find the matched VarStore
+        //
+        CopyGuid (&VarStorageData->Guid, (EFI_GUID *) (VOID *) &IfrEfiVarStore->Guid);
+        VarStorageData->Size       = IfrEfiVarStore->Size;
+        VarStorageData->Name       = VarStoreName;
+        VarStorageData->Type       = EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER;
+        VarStoreId                 = IfrEfiVarStore->VarStoreId;
+      }
+      break;
+
+    case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+      //
+      // VarStore is found. Don't need to search any more.
+      //
+      if (VarStoreId != 0) {
+        break;
+      }
+
+      IfrNameValueVarStore = (EFI_IFR_VARSTORE_NAME_VALUE *) IfrOpHdr;
+
+      if (IsThisVarstore (&IfrNameValueVarStore->Guid, NULL, ConfigHdr)) {
+        //
+        // Find the matched VarStore
+        //
+        CopyGuid (&VarStorageData->Guid, (EFI_GUID *) (VOID *) &IfrNameValueVarStore->Guid);
+        VarStorageData->Type       = EFI_HII_VARSTORE_NAME_VALUE;
+        VarStoreId                 = IfrNameValueVarStore->VarStoreId;
+      }
+      break;
+
+    case EFI_IFR_DEFAULTSTORE_OP:
+      //
+      // Add new the map between default id and default name.
+      //
+      DefaultDataPtr = (IFR_DEFAULT_DATA *) AllocateZeroPool (sizeof (IFR_DEFAULT_DATA));
+      if (DefaultDataPtr == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      DefaultDataPtr->DefaultId   = ((EFI_IFR_DEFAULTSTORE *) IfrOpHdr)->DefaultId;
+      InsertTailList (&DefaultIdArray->Entry, &DefaultDataPtr->Entry);
+      DefaultDataPtr = NULL;
+      break;
+
+    case EFI_IFR_FORM_OP:
+    case EFI_IFR_FORM_MAP_OP:
+      //
+      // No matched varstore is found and directly return.
+      //
+      if ( VarStoreId == 0) {
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_REF_OP:
+      //
+      // Ref question is not in IFR Form. This IFR form is not valid. 
+      //
+      if ( VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrRef = (EFI_IFR_REF4 *) IfrOpHdr;
+      if (IfrRef->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+      VarWidth  = (UINT16) (sizeof (EFI_HII_REF));
+
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_ONE_OF_OP:
+    case EFI_IFR_NUMERIC_OP:
+      //
+      // Numeric and OneOf has the same opcode structure.
+      //
+
+      //
+      // Numeric and OneOf question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrOneOf = (EFI_IFR_ONE_OF *) IfrOpHdr;
+      if (IfrOneOf->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+      VarWidth  = (UINT16) (1 << (IfrOneOf->Flags & EFI_IFR_NUMERIC_SIZE));
+
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (BlockData == NULL) {
+        //
+        // BlockData == NULL means this opcode is not in the requst array.
+        //
+        break;
+      }
+
+      if (IfrOpHdr->OpCode == EFI_IFR_ONE_OF_OP) {
+        //
+        // Set this flag to TRUE for the first oneof option.
+        //
+        FirstOneOfOption = TRUE;
+      } else if (IfrOpHdr->OpCode == EFI_IFR_NUMERIC_OP) {
+        //
+        // Numeric minimum value will be used as default value when no default is specified. 
+        //
+        DefaultData.Type        = DefaultValueFromDefault;
+        switch (IfrOneOf->Flags & EFI_IFR_NUMERIC_SIZE) {
+        case EFI_IFR_NUMERIC_SIZE_1:
+          DefaultData.Value.u8 = IfrOneOf->data.u8.MinValue;
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_2:
+          CopyMem (&DefaultData.Value.u16, &IfrOneOf->data.u16.MinValue, sizeof (UINT16));
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_4:
+          CopyMem (&DefaultData.Value.u32, &IfrOneOf->data.u32.MinValue, sizeof (UINT32));
+          break;
+
+        case EFI_IFR_NUMERIC_SIZE_8:
+          CopyMem (&DefaultData.Value.u64, &IfrOneOf->data.u64.MinValue, sizeof (UINT64));
+          break;
+
+        default:
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+        //
+        // Set default value base on the DefaultId list get from IFR data.
+        //        
+        for (LinkData = DefaultIdArray->Entry.ForwardLink; LinkData != &DefaultIdArray->Entry; LinkData = LinkData->ForwardLink) {
+          DefaultDataPtr = BASE_CR (LinkData, IFR_DEFAULT_DATA, Entry);
+          DefaultData.DefaultId   = DefaultDataPtr->DefaultId;
+          InsertDefaultValue (BlockData, &DefaultData);
+        }
+      }
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      //
+      // offset by question header
+      // width by EFI_IFR_ORDERED_LIST MaxContainers * OneofOption Type
+      //
+
+      FirstOrderedList = TRUE;
+      //
+      // OrderedList question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrOrderedList = (EFI_IFR_ORDERED_LIST *) IfrOpHdr;
+      if (IfrOrderedList->Question.VarStoreId != VarStoreId) {
+        BlockData = NULL;
+        break;
+      }
+      VarWidth  = IfrOrderedList->MaxContainers;
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      //
+      // EFI_IFR_DEFAULT_OP
+      // offset by question header
+      // width is 1 sizeof (BOOLEAN)
+      // default id by CheckBox Flags if CheckBox flags (Default or Mau) is set, the default value is 1 to be set.
+      // value by DefaultOption
+      // default id by DeaultOption DefaultId can override CheckBox Flags and Default value.
+      // 
+
+      //
+      // CheckBox question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrCheckBox = (EFI_IFR_CHECKBOX *) IfrOpHdr;
+      if (IfrCheckBox->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+      VarWidth  = (UINT16) sizeof (BOOLEAN);
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (BlockData == NULL) {
+        //
+        // BlockData == NULL means this opcode is not in the requst array.
+        //
+        break;
+      }
+
+      //
+      // Add default value for standard ID by CheckBox Flag
+      //
+      VarDefaultId = EFI_HII_DEFAULT_CLASS_STANDARD;
+      //
+      // Prepare new DefaultValue
+      //
+      DefaultData.DefaultId   = VarDefaultId;
+      if ((IfrCheckBox->Flags & EFI_IFR_CHECKBOX_DEFAULT) == EFI_IFR_CHECKBOX_DEFAULT) {
+        //
+        // When flag is set, defautl value is TRUE.
+        //
+        DefaultData.Type    = DefaultValueFromFlag;
+        DefaultData.Value.b = TRUE;
+      } else {
+        //
+        // When flag is not set, defautl value is FASLE.
+        //
+        DefaultData.Type    = DefaultValueFromDefault;
+        DefaultData.Value.b = FALSE;
+      }
+      //
+      // Add DefaultValue into current BlockData
+      //
+      InsertDefaultValue (BlockData, &DefaultData);
+
+      //
+      // Add default value for Manufacture ID by CheckBox Flag
+      //
+      VarDefaultId = EFI_HII_DEFAULT_CLASS_MANUFACTURING;
+      //
+      // Prepare new DefaultValue
+      //
+      DefaultData.DefaultId   = VarDefaultId;
+      if ((IfrCheckBox->Flags & EFI_IFR_CHECKBOX_DEFAULT_MFG) == EFI_IFR_CHECKBOX_DEFAULT_MFG) {
+        //
+        // When flag is set, defautl value is TRUE.
+        //
+        DefaultData.Type    = DefaultValueFromFlag;
+        DefaultData.Value.b = TRUE;
+      } else {
+        //
+        // When flag is not set, defautl value is FASLE.
+        //
+        DefaultData.Type    = DefaultValueFromDefault;
+        DefaultData.Value.b = FALSE;
+      }
+      //
+      // Add DefaultValue into current BlockData
+      //
+      InsertDefaultValue (BlockData, &DefaultData);
+      break;
+
+    case EFI_IFR_DATE_OP:
+      //
+      // offset by question header
+      // width MaxSize * sizeof (CHAR16)
+      // no default value, only block array
+      //
+
+      //
+      // Date question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrDate = (EFI_IFR_DATE *) IfrOpHdr;
+      if (IfrDate->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+
+      VarWidth  = (UINT16) sizeof (EFI_HII_DATE);
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_TIME_OP:
+      //
+      // offset by question header
+      // width MaxSize * sizeof (CHAR16)
+      // no default value, only block array
+      //
+
+      //
+      // Time question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrTime = (EFI_IFR_TIME *) IfrOpHdr;
+      if (IfrTime->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+
+      VarWidth  = (UINT16) sizeof (EFI_HII_TIME);
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_STRING_OP:
+      //
+      // offset by question header
+      // width MaxSize * sizeof (CHAR16)
+      // no default value, only block array
+      //
+
+      //
+      // String question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrString = (EFI_IFR_STRING *) IfrOpHdr;
+      if (IfrString->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+
+      VarWidth  = (UINT16) (IfrString->MaxSize * sizeof (UINT16));
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      break;
+
+    case EFI_IFR_PASSWORD_OP:
+      //
+      // offset by question header
+      // width MaxSize * sizeof (CHAR16)
+      // no default value, only block array
+      //
+
+      //
+      // Password question is not in IFR Form. This IFR form is not valid. 
+      //
+      if (VarStoreId == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Check whether this question is for the requested varstore.
+      //
+      IfrPassword = (EFI_IFR_PASSWORD *) IfrOpHdr;
+      if (IfrPassword->Question.VarStoreId != VarStoreId) {
+        break;
+      }
+
+      VarWidth  = (UINT16) (IfrPassword->MaxSize * sizeof (UINT16));
+      Status = IsThisOpcodeRequired(RequestBlockArray, HiiHandle, VarStorageData, IfrOpHdr, VarWidth, &BlockData);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // No default value for string.
+      //
+      BlockData = NULL;
+      break;
+
+    case EFI_IFR_ONE_OF_OPTION_OP:
+      //
+      // No matched block data is ignored.
+      //
+      if (BlockData == NULL || BlockData->Scope == 0) {
+        break;
+      }
+
+      IfrOneOfOption = (EFI_IFR_ONE_OF_OPTION *) IfrOpHdr;
+      if (BlockData->OpCode == EFI_IFR_ORDERED_LIST_OP) {
+
+        if (!FirstOrderedList){
+          break;
+        }
+        //
+        // Get ordered list option data type.
+        //
+        if (IfrOneOfOption->Type == EFI_IFR_TYPE_NUM_SIZE_8 || IfrOneOfOption->Type == EFI_IFR_TYPE_BOOLEAN) {
+          VarWidth = 1;
+        } else if (IfrOneOfOption->Type == EFI_IFR_TYPE_NUM_SIZE_16) {
+          VarWidth = 2;
+        } else if (IfrOneOfOption->Type == EFI_IFR_TYPE_NUM_SIZE_32) {
+          VarWidth = 4;
+        } else if (IfrOneOfOption->Type == EFI_IFR_TYPE_NUM_SIZE_64) {
+          VarWidth = 8;
+        } else {
+          //
+          // Invalid ordered list option data type.
+          //
+          Status = EFI_INVALID_PARAMETER;
+          if (BlockData->Name != NULL) {
+            FreePool (BlockData->Name);
+          }
+          FreePool (BlockData);
+          goto Done;
+        }
+
+        //
+        // Calculate Ordered list QuestionId width.
+        //
+        BlockData->Width = (UINT16) (BlockData->Width * VarWidth);
+        //
+        // Check whether this question is in requested block array.
+        //
+        if (!BlockArrayCheck (RequestBlockArray, BlockData->Offset, BlockData->Width, (BOOLEAN)(BlockData->Name != NULL), HiiHandle)) {
+          //
+          // This question is not in the requested string. Skip it.
+          //
+          if (BlockData->Name != NULL) {
+            FreePool (BlockData->Name);
+          }
+          FreePool (BlockData);
+          BlockData = NULL;
+          break;
+        }
+        //
+        // Check this var question is in the var storage 
+        //
+        if ((BlockData->Name == NULL) && ((BlockData->Offset + BlockData->Width) > VarStorageData->Size)) {
+          Status = EFI_INVALID_PARAMETER;
+          if (BlockData->Name != NULL) {
+            FreePool (BlockData->Name);
+          }
+          FreePool (BlockData);
+          goto Done;
+        }
+        //
+        // Add Block Data into VarStorageData BlockEntry
+        //
+        InsertBlockData (&VarStorageData->BlockEntry, &BlockData);
+
+        FirstOrderedList = FALSE;
+
+        break;
+      }
+
+      //
+      // 1. Set default value for OneOf option when flag field has default attribute.
+      //
+      if (((IfrOneOfOption->Flags & EFI_IFR_OPTION_DEFAULT) == EFI_IFR_OPTION_DEFAULT) ||
+          ((IfrOneOfOption->Flags & EFI_IFR_OPTION_DEFAULT_MFG) == EFI_IFR_OPTION_DEFAULT_MFG)) {
+        //
+        // This flag is used to specify whether this option is the first. Set it to FALSE for the following options. 
+        // The first oneof option value will be used as default value when no default value is specified. 
+        //
+        FirstOneOfOption = FALSE;
+        
+        // Prepare new DefaultValue
+        //
+        DefaultData.Type     = DefaultValueFromFlag;
+        CopyMem (&DefaultData.Value, &IfrOneOfOption->Value, IfrOneOfOption->Header.Length - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+        if ((IfrOneOfOption->Flags & EFI_IFR_OPTION_DEFAULT) == EFI_IFR_OPTION_DEFAULT) {
+          DefaultData.DefaultId = EFI_HII_DEFAULT_CLASS_STANDARD;
+          InsertDefaultValue (BlockData, &DefaultData);
+        } 
+        if ((IfrOneOfOption->Flags & EFI_IFR_OPTION_DEFAULT_MFG) == EFI_IFR_OPTION_DEFAULT_MFG) {
+          DefaultData.DefaultId = EFI_HII_DEFAULT_CLASS_MANUFACTURING;
+          InsertDefaultValue (BlockData, &DefaultData);
+        }
+      }
+
+      //
+      // 2. Set as the default value when this is the first option.
+      // The first oneof option value will be used as default value when no default value is specified. 
+      //
+      if (FirstOneOfOption) {
+        // This flag is used to specify whether this option is the first. Set it to FALSE for the following options. 
+        FirstOneOfOption = FALSE;
+        
+        //
+        // Prepare new DefaultValue
+        //        
+        DefaultData.Type     = DefaultValueFromDefault;
+        CopyMem (&DefaultData.Value, &IfrOneOfOption->Value, IfrOneOfOption->Header.Length - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+        for (LinkData = DefaultIdArray->Entry.ForwardLink; LinkData != &DefaultIdArray->Entry; LinkData = LinkData->ForwardLink) {
+          DefaultDataPtr = BASE_CR (LinkData, IFR_DEFAULT_DATA, Entry); 
+          DefaultData.DefaultId   = DefaultDataPtr->DefaultId;
+          InsertDefaultValue (BlockData, &DefaultData);
+        }
+      }
+      break;
+
+    case EFI_IFR_DEFAULT_OP:
+      //
+      // Update Current BlockData to the default value.
+      //
+      if (BlockData == NULL || BlockData->Scope == 0) {
+        //
+        // No matched block data is ignored.
+        //
+        break;
+      }
+
+      //
+      // Get the DefaultId
+      //
+      IfrDefault     = (EFI_IFR_DEFAULT *) IfrOpHdr;
+      VarDefaultId   = IfrDefault->DefaultId;
+      //
+      // Prepare new DefaultValue
+      //
+      DefaultData.Type        = DefaultValueFromOpcode;
+      DefaultData.DefaultId   = VarDefaultId;
+      CopyMem (&DefaultData.Value, &IfrDefault->Value, IfrDefault->Header.Length - OFFSET_OF (EFI_IFR_DEFAULT, Value));
+
+      // If the value field is expression, set the cleaned flag.
+      if (IfrDefault->Type ==  EFI_IFR_TYPE_OTHER) {
+        DefaultData.Cleaned = TRUE;
+      }
+      //
+      // Add DefaultValue into current BlockData
+      //
+      InsertDefaultValue (BlockData, &DefaultData);
+
+      //
+      // After insert the default value, reset the cleaned value for next 
+      // time used. If not set here, need to set the value before everytime 
+      // use it.
+      //
+      DefaultData.Cleaned     = FALSE;
+      break;
+
+    case EFI_IFR_END_OP:
+      //
+      // End Opcode is for Var question.
+      //
+      if (BlockData != NULL) {
+        if (BlockData->Scope > 0) {
+          BlockData->Scope--;
+        }
+        if (BlockData->Scope == 0) {
+          BlockData = NULL;
+        }
+      }
+
+      break;
+
+    default:
+      if (BlockData != NULL) {
+        if (BlockData->Scope > 0) {
+          BlockData->Scope = (UINT8) (BlockData->Scope + IfrOpHdr->Scope);
+        }
+
+        if (BlockData->Scope == 0) {
+          BlockData = NULL;
+        }
+      }
+      break;
+    }
+
+    IfrOffset     += IfrOpHdr->Length;
+    PackageOffset += IfrOpHdr->Length;
+  }
+
+Done:
+  for (LinkData = VarStorageData->BlockEntry.ForwardLink; LinkData != &VarStorageData->BlockEntry; LinkData = LinkData->ForwardLink) {
+    BlockData = BASE_CR (LinkData, IFR_BLOCK_DATA, Entry);
+    for (LinkDefault = BlockData->DefaultValueEntry.ForwardLink; LinkDefault != &BlockData->DefaultValueEntry; ) {
+      DefaultDataPtr = BASE_CR (LinkDefault, IFR_DEFAULT_DATA, Entry);
+      LinkDefault = LinkDefault->ForwardLink;
+      if (DefaultDataPtr->Cleaned == TRUE) {
+        RemoveEntryList (&DefaultDataPtr->Entry);
+        FreePool (DefaultDataPtr);
+      }
+    }
+  }
+
+  return Status;
+}
+
+/**
+  parse the configrequest string, get the elements.
+
+  @param      ConfigRequest         The input configrequest string.
+  @param      Progress              Return the progress data.
+
+  @retval     Block data pointer.
+**/
+IFR_BLOCK_DATA *
+GetBlockElement (
+  IN  EFI_STRING          ConfigRequest,
+  OUT EFI_STRING          *Progress
+  )
+{
+  EFI_STRING           StringPtr;
+  IFR_BLOCK_DATA       *BlockData;
+  IFR_BLOCK_DATA       *RequestBlockArray;
+  EFI_STATUS           Status;
+  UINT8                *TmpBuffer;
+  UINT16               Offset;
+  UINT16               Width;
+  LIST_ENTRY           *Link;
+  IFR_BLOCK_DATA       *NextBlockData;
+  UINTN                Length;
+
+  TmpBuffer = NULL;
+
+  //
+  // Init RequestBlockArray
+  //
+  RequestBlockArray = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+  if (RequestBlockArray == NULL) {
+    goto Done;
+  }
+  InitializeListHead (&RequestBlockArray->Entry);
+
+  //
+  // Get the request Block array from the request string
+  // Offset and Width
+  //
+
+  //
+  // Parse each <RequestElement> if exists
+  // Only <BlockName> format is supported by this help function.
+  // <BlockName> ::= &'OFFSET='<Number>&'WIDTH='<Number>
+  //
+  StringPtr = ConfigRequest;
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"&OFFSET=", StrLen (L"&OFFSET=")) == 0) {
+    //
+    // Skip the OFFSET string
+    //
+    *Progress   = StringPtr;
+    StringPtr += StrLen (L"&OFFSET=");
+    //
+    // Get Offset
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    Offset = 0;
+    CopyMem (
+      &Offset,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINT16)) ? ((Length + 1) / 2) : sizeof (UINT16)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      goto Done;
+    }
+    StringPtr += StrLen (L"&WIDTH=");
+
+    //
+    // Get Width
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    Width = 0;
+    CopyMem (
+      &Width,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINT16)) ? ((Length + 1) / 2) : sizeof (UINT16)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (*StringPtr != 0 && *StringPtr != L'&') {
+      goto Done;
+    }
+    
+    //
+    // Set Block Data
+    //
+    BlockData = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+    if (BlockData == NULL) {
+      goto Done;
+    }
+    BlockData->Offset = Offset;
+    BlockData->Width  = Width;
+    InsertBlockData (&RequestBlockArray->Entry, &BlockData);
+
+    //
+    // Skip &VALUE string if &VALUE does exists.
+    //
+    if (StrnCmp (StringPtr, L"&VALUE=", StrLen (L"&VALUE=")) == 0) {
+      StringPtr += StrLen (L"&VALUE=");
+
+      //
+      // Get Value
+      //
+      Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      StringPtr += Length;
+      if (*StringPtr != 0 && *StringPtr != L'&') {
+        goto Done;
+      }
+    }
+    //
+    // If '\0', parsing is finished. 
+    //
+    if (*StringPtr == 0) {
+      break;
+    }
+  }
+
+  //
+  // Merge the requested block data.
+  //
+  Link = RequestBlockArray->Entry.ForwardLink;
+  while ((Link != &RequestBlockArray->Entry) && (Link->ForwardLink != &RequestBlockArray->Entry)) {
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    NextBlockData = BASE_CR (Link->ForwardLink, IFR_BLOCK_DATA, Entry);
+    if ((NextBlockData->Offset >= BlockData->Offset) && (NextBlockData->Offset <= (BlockData->Offset + BlockData->Width))) {
+      if ((NextBlockData->Offset + NextBlockData->Width) > (BlockData->Offset + BlockData->Width)) {
+        BlockData->Width = (UINT16) (NextBlockData->Offset + NextBlockData->Width - BlockData->Offset);
+      }
+      RemoveEntryList (Link->ForwardLink);
+      FreePool (NextBlockData);
+      continue;
+    }
+    Link = Link->ForwardLink;
+  }
+
+  return RequestBlockArray;
+
+Done:
+  if (RequestBlockArray != NULL) {
+    //
+    // Free Link Array RequestBlockArray
+    //
+    while (!IsListEmpty (&RequestBlockArray->Entry)) {
+      BlockData = BASE_CR (RequestBlockArray->Entry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      FreePool (BlockData);
+    }
+
+    FreePool (RequestBlockArray);
+  }
+
+  return NULL;
+}
+
+/**
+  parse the configrequest string, get the elements.
+
+  @param      ConfigRequest         The input config request string.
+  @param      Progress              Return the progress data.
+
+  @retval     return data block array.
+**/
+IFR_BLOCK_DATA *
+GetNameElement (
+  IN  EFI_STRING           ConfigRequest,
+  OUT EFI_STRING           *Progress
+  )
+{
+  EFI_STRING           StringPtr;
+  EFI_STRING           NextTag;
+  IFR_BLOCK_DATA       *BlockData;
+  IFR_BLOCK_DATA       *RequestBlockArray;
+  BOOLEAN              HasValue;
+
+  StringPtr = ConfigRequest;
+
+  //
+  // Init RequestBlockArray
+  //
+  RequestBlockArray = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+  if (RequestBlockArray == NULL) {
+    goto Done;
+  }
+  InitializeListHead (&RequestBlockArray->Entry);
+
+  //
+  // Get the request Block array from the request string
+  //
+
+  //
+  // Parse each <RequestElement> if exists
+  // Only <BlockName> format is supported by this help function.
+  // <BlockName> ::= &'Name***=***
+  //
+  while (StringPtr != NULL && *StringPtr == L'&') {
+
+    *Progress   = StringPtr;
+    //
+    // Skip the L"&" string
+    //
+    StringPtr += 1;
+
+    HasValue = FALSE;
+    if ((NextTag = StrStr (StringPtr, L"=")) != NULL) {
+      *NextTag = L'\0';
+      HasValue = TRUE;
+    } else if ((NextTag = StrStr (StringPtr, L"&")) != NULL) {
+      *NextTag = L'\0';
+    }
+
+    //
+    // Set Block Data
+    //
+    BlockData = (IFR_BLOCK_DATA *) AllocateZeroPool (sizeof (IFR_BLOCK_DATA));
+    if (BlockData == NULL) {
+      goto Done;
+    }
+
+    //
+    // Get Name
+    //
+    BlockData->Name = AllocateCopyPool(StrSize (StringPtr), StringPtr);
+    InsertBlockData (&RequestBlockArray->Entry, &BlockData);
+
+    if (HasValue) {
+      //
+      // If has value, skip the value.
+      //    
+      StringPtr = NextTag + 1;
+      *NextTag  = L'=';
+      StringPtr = StrStr (StringPtr, L"&");
+    } else if (NextTag != NULL) {
+      //
+      // restore the '&' text.
+      //
+      StringPtr = NextTag;
+      *NextTag  = L'&';
+    }
+  }
+
+  return RequestBlockArray;
+
+Done:
+  if (RequestBlockArray != NULL) {
+    //
+    // Free Link Array RequestBlockArray
+    //
+    while (!IsListEmpty (&RequestBlockArray->Entry)) {
+      BlockData = BASE_CR (RequestBlockArray->Entry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      if (BlockData->Name != NULL) {
+        FreePool (BlockData->Name);
+      }
+      FreePool (BlockData);
+    }
+
+    FreePool (RequestBlockArray);
+  }
+
+  return NULL;
+}
+
+/**
+  Generate ConfigRequest string base on the varstore info.
+
+  @param      ConfigHdr             The config header for this varstore.
+  @param      VarStorageData        The varstore info.
+  @param      Status                Return Status.
+  @param      ConfigRequest         The ConfigRequest info may be return.
+
+  @retval     TRUE                  Need to continue
+  @retval     Others                NO need to continue or error occur.
+**/
+BOOLEAN
+GenerateConfigRequest (
+  IN  CHAR16                       *ConfigHdr,
+  IN  IFR_VARSTORAGE_DATA          *VarStorageData,
+  OUT EFI_STATUS                   *Status,
+  IN OUT EFI_STRING                *ConfigRequest
+  )
+{
+  BOOLEAN               DataExist;
+  UINTN                 Length;
+  LIST_ENTRY            *Link;
+  CHAR16                *FullConfigRequest;
+  CHAR16                *StringPtr;
+  IFR_BLOCK_DATA        *BlockData;
+
+  //
+  // Append VarStorageData BlockEntry into *Request string
+  // Now support only one varstore in a form package.
+  //
+  
+  //
+  // Go through all VarStorageData Entry and get BlockEntry for each one for the multiple varstore in a single form package
+  // Then construct them all to return MultiRequest string : ConfigHdr BlockConfig
+  //
+  
+  //
+  // Compute the length of the entire request starting with <ConfigHdr> and a 
+  // Null-terminator
+  //
+  DataExist = FALSE;
+  Length    = StrLen (ConfigHdr) + 1;
+
+  for (Link = VarStorageData->BlockEntry.ForwardLink; Link != &VarStorageData->BlockEntry; Link = Link->ForwardLink) {
+    DataExist = TRUE;
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+      //
+      // Add <BlockName> length for each Name
+      //
+      // <BlockName> ::= &Name1&Name2&... 
+      //                 |1| StrLen(Name1)
+      //
+      Length = Length + (1 + StrLen (BlockData->Name));
+    } else {
+      //
+      // Add <BlockName> length for each Offset/Width pair
+      //
+      // <BlockName> ::= &OFFSET=1234&WIDTH=1234
+      //                 |  8   | 4 |   7  | 4 |
+      //
+      Length = Length + (8 + 4 + 7 + 4);
+    }
+  }
+  //
+  // No any request block data is found. The request string can't be constructed.
+  //
+  if (!DataExist) {
+    *Status = EFI_SUCCESS;
+    return FALSE;
+  }
+
+  //
+  // Allocate buffer for the entire <ConfigRequest>
+  //
+  FullConfigRequest = AllocateZeroPool (Length * sizeof (CHAR16));
+  if (FullConfigRequest == NULL) {
+    *Status = EFI_OUT_OF_RESOURCES;
+    return FALSE;
+  }
+  StringPtr = FullConfigRequest;
+
+  //
+  // Start with <ConfigHdr>
+  //
+  StrCpyS (StringPtr, Length, ConfigHdr);
+  StringPtr += StrLen (StringPtr);
+
+  //
+  // Loop through all the Offset/Width pairs and append them to ConfigRequest
+  //
+  for (Link = VarStorageData->BlockEntry.ForwardLink; Link != &VarStorageData->BlockEntry; Link = Link->ForwardLink) {
+    BlockData = BASE_CR (Link, IFR_BLOCK_DATA, Entry);
+    if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+      //
+      // Append &Name1\0
+      //
+      UnicodeSPrint (
+        StringPtr,
+        (1 + StrLen (BlockData->Name) + 1) * sizeof (CHAR16),
+        L"&%s",
+        BlockData->Name
+      );
+    } else {
+      //
+      // Append &OFFSET=XXXX&WIDTH=YYYY\0
+      //
+      UnicodeSPrint (
+        StringPtr, 
+        (8 + 4 + 7 + 4 + 1) * sizeof (CHAR16), 
+        L"&OFFSET=%04X&WIDTH=%04X", 
+        BlockData->Offset, 
+        BlockData->Width
+      );
+    }
+    StringPtr += StrLen (StringPtr);
+  }
+  //
+  // Set to the got full request string.
+  //
+  HiiToLower (FullConfigRequest);
+
+  if (*ConfigRequest != NULL) {
+    FreePool (*ConfigRequest);
+  }
+  *ConfigRequest = FullConfigRequest;
+
+  return TRUE;
+}
+
+/**
+  Generate ConfigRequest Header base on the varstore info.
+
+  @param      VarStorageData        The varstore info.
+  @param      DevicePath            Device path for this varstore.
+  @param      ConfigHdr             The config header for this varstore.
+
+  @retval     EFI_SUCCESS           Generate the header success.
+  @retval     EFI_OUT_OF_RESOURCES  Allocate buffer fail.
+**/
+EFI_STATUS
+GenerateHdr (
+  IN   IFR_VARSTORAGE_DATA          *VarStorageData,
+  IN   EFI_DEVICE_PATH_PROTOCOL     *DevicePath,
+  OUT  EFI_STRING                   *ConfigHdr
+  )
+{
+  EFI_STRING                   GuidStr;
+  EFI_STRING                   NameStr;
+  EFI_STRING                   PathStr;
+  UINTN                        Length;
+  EFI_STATUS                   Status;
+
+  Status  = EFI_SUCCESS;
+  NameStr = NULL;
+  GuidStr = NULL;
+  PathStr = NULL;
+
+  //
+  // Construct <ConfigHdr> : "GUID=...&NAME=...&PATH=..." by VarStorageData Guid, Name and DriverHandle
+  //
+  GenerateSubStr (L"GUID=", sizeof (EFI_GUID), (VOID *) &VarStorageData->Guid, 1, &GuidStr);
+  if (VarStorageData->Name != NULL) {
+    GenerateSubStr (L"NAME=", StrLen (VarStorageData->Name) * sizeof (CHAR16), (VOID *) VarStorageData->Name, 2, &NameStr);
+  } else {
+    GenerateSubStr (L"NAME=", 0, NULL, 2, &NameStr);
+  }
+  GenerateSubStr (
+    L"PATH=",
+    GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) DevicePath),
+    (VOID *) DevicePath,
+    1,
+    &PathStr
+    );
+  Length = StrLen (GuidStr) + StrLen (NameStr) + StrLen (PathStr) + 1;
+  if (VarStorageData->Name == NULL) {
+    Length += 1;
+  }
+
+  *ConfigHdr = AllocateZeroPool (Length * sizeof (CHAR16));
+  if (*ConfigHdr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  StrCpyS (*ConfigHdr, Length, GuidStr);
+  StrCatS (*ConfigHdr, Length, NameStr);
+  if (VarStorageData->Name == NULL) {
+    StrCatS (*ConfigHdr, Length, L"&");
+  }
+  StrCatS (*ConfigHdr, Length, PathStr);
+
+  //
+  // Remove the last character L'&'
+  //
+  *(*ConfigHdr + StrLen (*ConfigHdr) - 1) = L'\0';
+
+Done:
+  if (GuidStr != NULL) {
+    FreePool (GuidStr);
+  }
+
+  if (NameStr != NULL) {
+    FreePool (NameStr);
+  }
+
+  if (PathStr != NULL) {
+    FreePool (PathStr);
+  }
+
+  return Status;
+}
+
+/**
+  Get Data buffer size based on data type.
+
+  @param      ValueType             The input data type.
+
+  @retval     The data buffer size for the input type.
+**/
+UINT16
+GetStorageWidth (
+  IN UINT8       ValueType
+  )
+{
+  UINT16         StorageWidth;
+
+  switch (ValueType) {
+  case EFI_IFR_NUMERIC_SIZE_1:
+  case EFI_IFR_TYPE_BOOLEAN:
+    StorageWidth = (UINT16) sizeof (UINT8);
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_2:
+    StorageWidth = (UINT16) sizeof (UINT16);
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_4:
+    StorageWidth = (UINT16) sizeof (UINT32);
+    break;
+
+  case EFI_IFR_NUMERIC_SIZE_8:
+    StorageWidth = (UINT16) sizeof (UINT64);
+    break;
+
+  case EFI_IFR_TYPE_TIME:
+    StorageWidth = (UINT16) sizeof (EFI_IFR_TIME);
+    break;
+
+  case EFI_IFR_TYPE_DATE:
+    StorageWidth = (UINT16) sizeof (EFI_IFR_DATE);
+    break;
+
+  default:
+    StorageWidth = 0;
+    break;
+  }
+
+  return StorageWidth;
+}
+
+/**
+  Generate ConfigAltResp string base on the varstore info.
+
+  @param      HiiHandle             Hii Handle for this hii package.
+  @param      ConfigHdr             The config header for this varstore.
+  @param      VarStorageData        The varstore info.
+  @param      DefaultIdArray        The Default id array.
+  @param      DefaultAltCfgResp     The DefaultAltCfgResp info may be return.
+
+  @retval     TRUE                  Need to continue
+  @retval     Others                NO need to continue or error occur.
+**/
+EFI_STATUS
+GenerateAltConfigResp (
+  IN  EFI_HII_HANDLE               HiiHandle,
+  IN  CHAR16                       *ConfigHdr,
+  IN  IFR_VARSTORAGE_DATA          *VarStorageData,
+  IN  IFR_DEFAULT_DATA             *DefaultIdArray,
+  IN OUT EFI_STRING                *DefaultAltCfgResp
+  )
+{
+  BOOLEAN               DataExist;
+  UINTN                 Length;
+  LIST_ENTRY            *Link;
+  LIST_ENTRY            *LinkData;
+  LIST_ENTRY            *LinkDefault;
+  LIST_ENTRY            *ListEntry;
+  CHAR16                *StringPtr;
+  IFR_BLOCK_DATA        *BlockData;
+  IFR_DEFAULT_DATA      *DefaultId;
+  IFR_DEFAULT_DATA      *DefaultValueData;
+  UINTN                 Width;
+  UINT8                 *TmpBuffer;
+  CHAR16                *DefaultString;
+
+  BlockData     = NULL;
+  DataExist     = FALSE;
+  DefaultString = NULL;
+  //
+  // Add length for <ConfigHdr> + '\0'
+  //
+  Length = StrLen (ConfigHdr) + 1;
+
+  for (Link = DefaultIdArray->Entry.ForwardLink; Link != &DefaultIdArray->Entry; Link = Link->ForwardLink) {
+    DefaultId = BASE_CR (Link, IFR_DEFAULT_DATA, Entry);
+    //
+    // Add length for "&<ConfigHdr>&ALTCFG=XXXX"
+    //                |1| StrLen (ConfigHdr) | 8 | 4 |
+    //
+    Length += (1 + StrLen (ConfigHdr) + 8 + 4);
+    
+    for (LinkData = VarStorageData->BlockEntry.ForwardLink; LinkData != &VarStorageData->BlockEntry; LinkData = LinkData->ForwardLink) {
+      BlockData = BASE_CR (LinkData, IFR_BLOCK_DATA, Entry);
+      ListEntry     = &BlockData->DefaultValueEntry;
+      for (LinkDefault = ListEntry->ForwardLink; LinkDefault != ListEntry; LinkDefault = LinkDefault->ForwardLink) {
+        DefaultValueData = BASE_CR (LinkDefault, IFR_DEFAULT_DATA, Entry);
+        if (DefaultValueData->DefaultId != DefaultId->DefaultId) {
+          continue;
+        }
+        if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+          //
+          // Add length for "&Name1=zzzzzzzzzzzz"
+          //                |1|Name|1|Value|
+          //
+          Length += (1 + StrLen (BlockData->Name) + 1 + BlockData->Width * 2);
+        } else {
+          //
+          // Add length for "&OFFSET=XXXX&WIDTH=YYYY&VALUE=zzzzzzzzzzzz"
+          //                |    8  | 4 |   7  | 4 |   7  | Width * 2 |
+          //
+          Length += (8 + 4 + 7 + 4 + 7 + BlockData->Width * 2);
+        }
+        DataExist = TRUE;
+      }
+    }
+  }
+  
+  //
+  // No default value is found. The default string doesn't exist.
+  //
+  if (!DataExist) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Allocate buffer for the entire <DefaultAltCfgResp>
+  //
+  *DefaultAltCfgResp = AllocateZeroPool (Length * sizeof (CHAR16));
+  if (*DefaultAltCfgResp == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  StringPtr = *DefaultAltCfgResp;
+
+  //
+  // Start with <ConfigHdr>
+  //
+  StrCpyS (StringPtr, Length, ConfigHdr);
+  StringPtr += StrLen (StringPtr);
+
+  for (Link = DefaultIdArray->Entry.ForwardLink; Link != &DefaultIdArray->Entry; Link = Link->ForwardLink) {
+    DefaultId = BASE_CR (Link, IFR_DEFAULT_DATA, Entry);
+    //
+    // Add <AltConfigHdr> of the form "&<ConfigHdr>&ALTCFG=XXXX\0"
+    //                                |1| StrLen (ConfigHdr) | 8 | 4 |
+    //
+    UnicodeSPrint (
+      StringPtr, 
+      (1 + StrLen (ConfigHdr) + 8 + 4 + 1) * sizeof (CHAR16), 
+      L"&%s&ALTCFG=%04X", 
+      ConfigHdr, 
+      DefaultId->DefaultId
+      );
+    StringPtr += StrLen (StringPtr);
+
+    for (LinkData = VarStorageData->BlockEntry.ForwardLink; LinkData != &VarStorageData->BlockEntry; LinkData = LinkData->ForwardLink) {
+      BlockData = BASE_CR (LinkData, IFR_BLOCK_DATA, Entry);
+      ListEntry     = &BlockData->DefaultValueEntry;
+      for (LinkDefault = ListEntry->ForwardLink; LinkDefault != ListEntry; LinkDefault = LinkDefault->ForwardLink) {
+        DefaultValueData = BASE_CR (LinkDefault, IFR_DEFAULT_DATA, Entry);
+        if (DefaultValueData->DefaultId != DefaultId->DefaultId) {
+          continue;
+        }
+        if (VarStorageData->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+          UnicodeSPrint (
+            StringPtr, 
+            (1 + StrLen (ConfigHdr) + 1) * sizeof (CHAR16), 
+            L"&%s=", 
+            BlockData->Name
+            );
+          StringPtr += StrLen (StringPtr);
+        } else {
+          //
+          // Add <BlockConfig>
+          // <BlockConfig> ::= 'OFFSET='<Number>&'WIDTH='<Number>&'VALUE'=<Number>
+          //
+          UnicodeSPrint (
+            StringPtr, 
+            (8 + 4 + 7 + 4 + 7 + 1) * sizeof (CHAR16),
+            L"&OFFSET=%04X&WIDTH=%04X&VALUE=", 
+            BlockData->Offset, 
+            BlockData->Width
+            );
+          StringPtr += StrLen (StringPtr);
+        }
+        Width = BlockData->Width;
+        //
+        // Convert Value to a hex string in "%x" format
+        // NOTE: This is in the opposite byte that GUID and PATH use
+        //
+        if (BlockData->OpCode == EFI_IFR_STRING_OP){
+          DefaultString   = InternalGetString(HiiHandle, DefaultValueData->Value.string);
+          TmpBuffer = (UINT8 *) DefaultString;
+        } else {
+          TmpBuffer = (UINT8 *) &(DefaultValueData->Value);
+        }
+        for (; Width > 0 && (TmpBuffer != NULL); Width--) {
+          StringPtr += UnicodeValueToString (StringPtr, PREFIX_ZERO | RADIX_HEX, TmpBuffer[Width - 1], 2);
+        }
+        if (DefaultString != NULL){
+          FreePool(DefaultString);
+          DefaultString = NULL;
+        }
+      }
+    }
+  }
+
+  HiiToLower (*DefaultAltCfgResp);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function gets the full request string and full default value string by 
+  parsing IFR data in HII form packages. 
+  
+  When Request points to NULL string, the request string and default value string 
+  for each varstore in form package will return. 
+
+  @param  DataBaseRecord         The DataBaseRecord instance contains the found Hii handle and package.
+  @param  DevicePath             Device Path which Hii Config Access Protocol is registered.
+  @param  Request                Pointer to a null-terminated Unicode string in
+                                 <ConfigRequest> format. When it doesn't contain
+                                 any RequestElement, it will be updated to return 
+                                 the full RequestElement retrieved from IFR data.
+                                 If it points to NULL, the request string for the first
+                                 varstore in form package will be merged into a
+                                 <MultiConfigRequest> format string and return. 
+  @param  AltCfgResp             Pointer to a null-terminated Unicode string in
+                                 <ConfigAltResp> format. When the pointer is to NULL,
+                                 the full default value string retrieved from IFR data
+                                 will return. When the pinter is to a string, the
+                                 full default value string retrieved from IFR data
+                                 will be merged into the input string and return.
+                                 When Request points to NULL, the default value string 
+                                 for each varstore in form package will be merged into 
+                                 a <MultiConfigAltResp> format string and return.
+  @param  PointerProgress        Optional parameter, it can be be NULL. 
+                                 When it is not NULL, if Request is NULL, it returns NULL. 
+                                 On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 & before the first failing name / value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name / value pair) if the request was
+                                 not successful.
+  @retval EFI_SUCCESS            The Results string is set to the full request string.
+                                 And AltCfgResp contains all default value string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory for the return string.
+  @retval EFI_NOT_FOUND          The varstore (Guid and Name) in Request string 
+                                 can't be found in Form package.
+  @retval EFI_NOT_FOUND          HiiPackage can't be got on the input HiiHandle.
+  @retval EFI_INVALID_PARAMETER  Request points to NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+GetFullStringFromHiiFormPackages (
+  IN     HII_DATABASE_RECORD        *DataBaseRecord,
+  IN     EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  IN OUT EFI_STRING                 *Request,
+  IN OUT EFI_STRING                 *AltCfgResp,
+  OUT    EFI_STRING                 *PointerProgress OPTIONAL
+  )
+{
+  EFI_STATUS                   Status;
+  UINT8                        *HiiFormPackage;
+  UINTN                        PackageSize;
+  IFR_BLOCK_DATA               *RequestBlockArray;
+  IFR_BLOCK_DATA               *BlockData;
+  IFR_DEFAULT_DATA             *DefaultValueData;
+  IFR_DEFAULT_DATA             *DefaultId;
+  IFR_DEFAULT_DATA             *DefaultIdArray;
+  IFR_VARSTORAGE_DATA          *VarStorageData;
+  EFI_STRING                   DefaultAltCfgResp;
+  EFI_STRING                   ConfigHdr;
+  EFI_STRING                   StringPtr;
+  EFI_STRING                   Progress;
+
+  if (DataBaseRecord == NULL || DevicePath == NULL || Request == NULL || AltCfgResp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Initialize the local variables.
+  //
+  RequestBlockArray = NULL;
+  DefaultIdArray    = NULL;
+  VarStorageData    = NULL;
+  DefaultAltCfgResp = NULL;
+  ConfigHdr         = NULL;
+  HiiFormPackage    = NULL;
+  PackageSize       = 0;
+  Progress          = *Request;
+
+  Status = GetFormPackageData (DataBaseRecord, &HiiFormPackage, &PackageSize);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // 1. Get the request block array by Request String when Request string containts the block array.
+  //
+  StringPtr = NULL;
+  if (*Request != NULL) {
+    StringPtr = *Request;
+    //
+    // Jump <ConfigHdr>
+    //
+    if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+      Status   = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"GUID=");
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&NAME=", StrLen (L"&NAME=")) != 0) {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&NAME=");
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&PATH=", StrLen (L"&PATH=")) != 0) {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    StringPtr += StrLen (L"&PATH=");
+    while (*StringPtr != L'\0' && *StringPtr != L'&') {
+      StringPtr ++;
+    }
+
+    if (*StringPtr == L'\0') {
+      //
+      // No request block is found.
+      //
+      StringPtr = NULL;
+    }
+  }
+
+  //
+  // If StringPtr != NULL, get the request elements.
+  //
+  if (StringPtr != NULL) {
+    if (StrStr (StringPtr, L"&OFFSET=") != NULL) {
+      RequestBlockArray = GetBlockElement(StringPtr, &Progress);
+    } else {
+      RequestBlockArray = GetNameElement(StringPtr, &Progress);
+    }
+
+    if (RequestBlockArray == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+  }
+
+  //
+  // Initialize DefaultIdArray to store the map between DeaultId and DefaultName
+  //
+  DefaultIdArray   = (IFR_DEFAULT_DATA *) AllocateZeroPool (sizeof (IFR_DEFAULT_DATA));
+  if (DefaultIdArray == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  InitializeListHead (&DefaultIdArray->Entry);
+
+  //
+  // Initialize VarStorageData to store the var store Block and Default value information.
+  //
+  VarStorageData = (IFR_VARSTORAGE_DATA *) AllocateZeroPool (sizeof (IFR_VARSTORAGE_DATA));
+  if (VarStorageData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  InitializeListHead (&VarStorageData->Entry);
+  InitializeListHead (&VarStorageData->BlockEntry);
+
+  //
+  // 2. Parse FormPackage to get BlockArray and DefaultId Array for the request BlockArray.
+  //
+
+  //
+  // Parse the opcode in form pacakge to get the default setting.
+  //
+  Status = ParseIfrData (DataBaseRecord->Handle,
+                         HiiFormPackage,
+                         (UINT32) PackageSize,
+                         *Request,
+                         RequestBlockArray,
+                         VarStorageData,
+                         DefaultIdArray);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // No requested varstore in IFR data and directly return
+  //
+  if (VarStorageData->Type == 0 && VarStorageData->Name == NULL) {
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  //
+  // 3. Construct Request Element (Block Name) for 2.1 and 2.2 case.
+  //
+  Status = GenerateHdr (VarStorageData, DevicePath, &ConfigHdr);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  if (RequestBlockArray == NULL) {
+    if (!GenerateConfigRequest(ConfigHdr, VarStorageData, &Status, Request)) {
+      goto Done;
+    }
+  }
+
+  //
+  // 4. Construct Default Value string in AltResp according to request element.
+  // Go through all VarStorageData Entry and get the DefaultId array for each one
+  // Then construct them all to : ConfigHdr AltConfigHdr ConfigBody AltConfigHdr ConfigBody
+  //
+  Status = GenerateAltConfigResp (DataBaseRecord->Handle,ConfigHdr, VarStorageData, DefaultIdArray, &DefaultAltCfgResp);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // 5. Merge string into the input AltCfgResp if the iput *AltCfgResp is not NULL.
+  //
+  if (*AltCfgResp != NULL && DefaultAltCfgResp != NULL) {
+    Status = MergeDefaultString (AltCfgResp, DefaultAltCfgResp);
+    FreePool (DefaultAltCfgResp);
+  } else if (*AltCfgResp == NULL) {
+    *AltCfgResp = DefaultAltCfgResp;
+  }
+
+Done:
+  if (RequestBlockArray != NULL) {
+    //
+    // Free Link Array RequestBlockArray
+    //
+    while (!IsListEmpty (&RequestBlockArray->Entry)) {
+      BlockData = BASE_CR (RequestBlockArray->Entry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      if (BlockData->Name != NULL) {
+        FreePool (BlockData->Name);
+      }
+      FreePool (BlockData);
+    }
+
+    FreePool (RequestBlockArray);
+  }
+
+  if (VarStorageData != NULL) {
+    //
+    // Free link array VarStorageData
+    //
+    while (!IsListEmpty (&VarStorageData->BlockEntry)) {
+      BlockData = BASE_CR (VarStorageData->BlockEntry.ForwardLink, IFR_BLOCK_DATA, Entry);
+      RemoveEntryList (&BlockData->Entry);
+      if (BlockData->Name != NULL) {
+        FreePool (BlockData->Name);
+      }
+      //
+      // Free default value link array
+      //
+      while (!IsListEmpty (&BlockData->DefaultValueEntry)) {
+        DefaultValueData = BASE_CR (BlockData->DefaultValueEntry.ForwardLink, IFR_DEFAULT_DATA, Entry);
+        RemoveEntryList (&DefaultValueData->Entry);
+        FreePool (DefaultValueData);
+      }
+      FreePool (BlockData);
+    }
+    FreePool (VarStorageData);
+  }
+
+  if (DefaultIdArray != NULL) {
+    //
+    // Free DefaultId Array
+    //
+    while (!IsListEmpty (&DefaultIdArray->Entry)) {
+      DefaultId = BASE_CR (DefaultIdArray->Entry.ForwardLink, IFR_DEFAULT_DATA, Entry);
+      RemoveEntryList (&DefaultId->Entry);
+      FreePool (DefaultId);
+    }
+    FreePool (DefaultIdArray);
+  }
+
+  //
+  // Free the allocated string 
+  //
+  if (ConfigHdr != NULL) {
+    FreePool (ConfigHdr);
+  }
+
+  //
+  // Free Pacakge data
+  //
+  if (HiiFormPackage != NULL) {
+    FreePool (HiiFormPackage);
+  }
+
+  if (PointerProgress != NULL) {
+    if (*Request == NULL) {
+      *PointerProgress = NULL;
+    } else if (EFI_ERROR (Status)) {
+      *PointerProgress = *Request;
+    } else {
+      *PointerProgress = *Request + StrLen (*Request);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  This function gets the full request resp string by 
+  parsing IFR data in HII form packages.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  EfiVarStoreInfo        The efi varstore info which is save in the EFI 
+                                 varstore data structure.                       
+  @param  Request                Pointer to a null-terminated Unicode string in
+                                 <ConfigRequest> format.
+  @param  RequestResp            Pointer to a null-terminated Unicode string in
+                                 <ConfigResp> format.
+  @param  AccessProgress         On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 & before the first failing name / value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name / value pair) if the request was
+                                 not successful.
+
+  @retval EFI_SUCCESS            The Results string is set to the full request string.
+                                 And AltCfgResp contains all default value string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory for the return string.
+  @retval EFI_INVALID_PARAMETER  Request points to NULL.
+
+**/
+EFI_STATUS
+GetConfigRespFromEfiVarStore (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  EFI_IFR_VARSTORE_EFI                   *EfiVarStoreInfo,    
+  IN  EFI_STRING                             Request,
+  OUT EFI_STRING                             *RequestResp,
+  OUT EFI_STRING                             *AccessProgress
+  )
+{
+  EFI_STATUS Status;
+  EFI_STRING VarStoreName;
+  UINTN      NameSize;
+  UINT8      *VarStore;
+  UINTN      BufferSize;
+
+  Status          = EFI_SUCCESS;
+  BufferSize      = 0;
+  VarStore        = NULL;
+  VarStoreName    = NULL;
+  *AccessProgress = Request;
+
+  NameSize = AsciiStrSize ((CHAR8 *)EfiVarStoreInfo->Name);
+  VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+  if (VarStoreName == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  AsciiStrToUnicodeStrS ((CHAR8 *) EfiVarStoreInfo->Name, VarStoreName, NameSize);
+   
+  
+  Status = gRT->GetVariable (VarStoreName, &EfiVarStoreInfo->Guid, NULL, &BufferSize, NULL);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    goto Done;
+  }
+
+  VarStore = AllocateZeroPool (BufferSize);
+  ASSERT (VarStore != NULL);
+  Status = gRT->GetVariable (VarStoreName, &EfiVarStoreInfo->Guid, NULL, &BufferSize, VarStore);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = HiiBlockToConfig(This, Request, VarStore, BufferSize, RequestResp, AccessProgress);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+Done:
+  if (VarStoreName != NULL) {
+    FreePool (VarStoreName);
+  }
+
+  if (VarStore != NULL) {
+    FreePool (VarStore);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function route the full request resp string for efi varstore. 
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  EfiVarStoreInfo        The efi varstore info which is save in the EFI 
+                                 varstore data structure.      
+  @param  RequestResp            Pointer to a null-terminated Unicode string in
+                                 <ConfigResp> format.
+  @param  Result                 Pointer to a null-terminated Unicode string in
+                                 <ConfigResp> format.
+                                 
+  @retval EFI_SUCCESS            The Results string is set to the full request string.
+                                 And AltCfgResp contains all default value string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory for the return string.
+  @retval EFI_INVALID_PARAMETER  Request points to NULL.
+
+**/
+EFI_STATUS
+RouteConfigRespForEfiVarStore (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  EFI_IFR_VARSTORE_EFI                   *EfiVarStoreInfo,  
+  IN  EFI_STRING                             RequestResp,
+  OUT EFI_STRING                             *Result
+  )
+{
+  EFI_STATUS Status;
+  EFI_STRING VarStoreName;
+  UINTN      NameSize;
+  UINT8      *VarStore;
+  UINTN      BufferSize;
+  UINTN      BlockSize;
+
+  Status       = EFI_SUCCESS;
+  BufferSize   = 0;
+  VarStore     = NULL;
+  VarStoreName = NULL;
+
+  NameSize = AsciiStrSize ((CHAR8 *)EfiVarStoreInfo->Name);
+  VarStoreName = AllocateZeroPool (NameSize * sizeof (CHAR16));
+  if (VarStoreName == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+  AsciiStrToUnicodeStrS ((CHAR8 *) EfiVarStoreInfo->Name, VarStoreName, NameSize);
+      
+  Status = gRT->GetVariable (VarStoreName, &EfiVarStoreInfo->Guid, NULL, &BufferSize, NULL);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    goto Done;
+  }
+
+  BlockSize = BufferSize;
+  VarStore = AllocateZeroPool (BufferSize);
+  ASSERT (VarStore != NULL);
+  Status = gRT->GetVariable (VarStoreName, &EfiVarStoreInfo->Guid, NULL, &BufferSize, VarStore);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = HiiConfigToBlock(This, RequestResp, VarStore, &BlockSize, Result);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = gRT->SetVariable (VarStoreName, &EfiVarStoreInfo->Guid, EfiVarStoreInfo->Attributes, BufferSize, VarStore);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+Done:
+  if (VarStoreName != NULL) {
+    FreePool (VarStoreName);
+  }
+
+  if (VarStore != NULL) {
+    FreePool (VarStore);
+  }
+
+  return Status;
+}
+
+/**
+  Validate the config request elements.
+
+  @param  ConfigElements                A null-terminated Unicode string in <ConfigRequest> format, 
+                                        without configHdr field.
+
+  @retval     CHAR16 *    THE first Name/value pair not correct.
+  @retval     NULL        Success parse the name/value pair
+**/
+CHAR16 *
+OffsetWidthValidate (
+  CHAR16          *ConfigElements
+  )
+{
+  CHAR16    *StringPtr;
+  CHAR16    *RetVal;
+
+  StringPtr = ConfigElements;
+
+  while (1) {
+    RetVal    = StringPtr;
+    if (StrnCmp (StringPtr, L"&OFFSET=", StrLen (L"&OFFSET=")) != 0) {
+      return RetVal;
+    }
+
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      StringPtr++;
+    }
+    if (*StringPtr == L'\0') {
+      return RetVal;
+    }
+
+    StringPtr += StrLen (L"&WIDTH=");
+    while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&OFFSET=", StrLen (L"&OFFSET=")) != 0) {
+      StringPtr ++;
+    }
+
+    if (*StringPtr == L'\0') {
+      return NULL;
+    }
+  }
+}
+
+/**
+  Validate the config request elements.
+
+  @param  ConfigElements                A null-terminated Unicode string in <ConfigRequest> format, 
+                                        without configHdr field.
+
+  @retval     CHAR16 *    THE first Name/value pair not correct.
+  @retval     NULL        Success parse the name/value pair
+
+**/
+CHAR16 *
+NameValueValidate (
+  CHAR16          *ConfigElements
+  )
+{
+  CHAR16    *StringPtr;
+  CHAR16    *RetVal;
+
+  StringPtr = ConfigElements;
+
+  while (1) {
+    RetVal = StringPtr;
+    if (*StringPtr != L'&') {
+      return RetVal;
+    }
+    StringPtr += 1;
+
+    StringPtr = StrStr (StringPtr, L"&");
+    
+    if (StringPtr == NULL) {
+      return NULL;
+    }
+  }
+}
+
+/**
+  Validate the config request string.
+
+  @param  ConfigRequest                A null-terminated Unicode string in <ConfigRequest> format.
+
+  @retval     CHAR16 *    THE first element not correct.
+  @retval     NULL        Success parse the name/value pair
+
+**/
+CHAR16 *
+ConfigRequestValidate (
+  CHAR16          *ConfigRequest
+  )
+{
+  BOOLEAN            HasNameField;
+  CHAR16             *StringPtr;
+
+  HasNameField = TRUE;
+  StringPtr    = ConfigRequest;
+
+  //
+  // Check <ConfigHdr>
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    return ConfigRequest;
+  }
+  StringPtr += StrLen (L"GUID=");
+  while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&NAME=", StrLen (L"&NAME=")) != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == L'\0') {
+    return ConfigRequest;
+  }
+  StringPtr += StrLen (L"&NAME=");
+  if (*StringPtr == L'&') {
+    HasNameField = FALSE;
+  }
+  while (*StringPtr != L'\0' && StrnCmp (StringPtr, L"&PATH=", StrLen (L"&PATH=")) != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == L'\0') {
+    return ConfigRequest;
+  }
+  StringPtr += StrLen (L"&PATH=");
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr ++;
+  }
+
+  if (*StringPtr == L'\0') {
+    return NULL;
+  }
+
+  if (HasNameField) {
+    //
+    // Should be Buffer varstore, config request should be "OFFSET/Width" pairs.
+    //
+    return OffsetWidthValidate(StringPtr);
+  } else {
+    //
+    // Should be Name/Value varstore, config request should be "&name1&name2..." pairs.
+    //
+    return NameValueValidate(StringPtr);
+  }
+}
+
+/**
+  This function allows a caller to extract the current configuration
+  for one or more named elements from one or more drivers.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  Request                A null-terminated Unicode string in
+                                 <MultiConfigRequest> format.
+  @param  Progress               On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 & before the first failing name / value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name / value pair) if the request was
+                                 not successful.
+  @param  Results                Null-terminated Unicode string in
+                                 <MultiConfigAltResp> format which has all values
+                                 filled in for the names in the Request string.
+                                 String to be allocated by the called function.
+
+  @retval EFI_SUCCESS            The Results string is filled with the values
+                                 corresponding to all requested names.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the parts of the
+                                 results that must be stored awaiting possible
+                                 future        protocols.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any known driver.
+                                   Progress set to the "G" in "GUID" of the routing
+                                  header that doesn't match. Note: There is no
+                                    requirement that all routing data be validated
+                                 before any configuration extraction.
+  @retval EFI_INVALID_PARAMETER  For example, passing in a NULL for the Request
+                                 parameter would result in this type of error. The
+                                 Progress parameter is set to NULL.
+  @retval EFI_INVALID_PARAMETER  Illegal syntax. Progress set to most recent &
+                                 before the error or the beginning of the string.
+  @retval EFI_INVALID_PARAMETER  The ExtractConfig function of the underlying HII
+                                 Configuration Access Protocol returned 
+                                 EFI_INVALID_PARAMETER. Progress set to most recent
+                                 & before the error or the beginning of the string.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          ConfigRequest;
+  UINTN                               Length;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *TempDevicePath;
+  EFI_STATUS                          Status;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *Database;
+  UINT8                               *DevicePathPkg;
+  UINT8                               *CurrentDevicePath;
+  EFI_HANDLE                          DriverHandle;
+  EFI_HII_HANDLE                      HiiHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL      *ConfigAccess;
+  EFI_STRING                          AccessProgress;
+  EFI_STRING                          AccessResults;
+  EFI_STRING                          DefaultResults;
+  BOOLEAN                             FirstElement;
+  BOOLEAN                             IfrDataParsedFlag;
+  BOOLEAN                             IsEfiVarStore;
+  EFI_IFR_VARSTORE_EFI                *EfiVarStoreInfo;
+  EFI_STRING                          ErrorPtr;
+  UINTN                               DevicePathSize;
+
+  if (This == NULL || Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Request == NULL) {
+    *Progress = NULL;
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private   = CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  StringPtr = Request;
+  *Progress = StringPtr;
+  DefaultResults = NULL;
+  ConfigRequest  = NULL;
+  Status         = EFI_SUCCESS;
+  AccessResults  = NULL;
+  AccessProgress = NULL;
+  DevicePath     = NULL;
+  IfrDataParsedFlag = FALSE;
+  IsEfiVarStore     = FALSE;
+  EfiVarStoreInfo   = NULL;
+
+  //
+  // The first element of <MultiConfigRequest> should be
+  // <GuidHdr>, which is in 'GUID='<Guid> syntax.
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FirstElement = TRUE;
+
+  //
+  // Allocate a fix length of memory to store Results. Reallocate memory for
+  // Results if this fix length is insufficient.
+  //
+  *Results = (EFI_STRING) AllocateZeroPool (MAX_STRING_LENGTH);
+  if (*Results == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) == 0) {
+    //
+    // If parsing error, set Progress to the beginning of the <MultiConfigRequest>
+    // or most recent & before the error.
+    //
+    if (StringPtr == Request) {
+      *Progress = StringPtr;
+    } else {
+      *Progress = StringPtr - 1;
+    }
+
+    //
+    // Process each <ConfigRequest> of <MultiConfigRequest>
+    //
+    Length = CalculateConfigStringLen (StringPtr);
+    ConfigRequest = AllocateCopyPool ((Length + 1) * sizeof (CHAR16), StringPtr);
+    if (ConfigRequest == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    *(ConfigRequest + Length) = 0;
+
+    //
+    // Get the UEFI device path
+    //
+    Status = GetDevicePath (ConfigRequest, (UINT8 **) &DevicePath);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // Find driver which matches the routing data.
+    //
+    DriverHandle     = NULL;
+    HiiHandle        = NULL;
+    Database         = NULL;
+    for (Link = Private->DatabaseList.ForwardLink;
+         Link != &Private->DatabaseList;
+         Link = Link->ForwardLink
+        ) {
+      Database = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+      if ((DevicePathPkg = Database->PackageList->DevicePathPkg) != NULL) {
+        CurrentDevicePath = DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER);
+        DevicePathSize    = GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) CurrentDevicePath);
+        if ((CompareMem (DevicePath,CurrentDevicePath,DevicePathSize) == 0) && IsThisPackageList(Database, ConfigRequest)) {
+          DriverHandle = Database->DriverHandle;
+          HiiHandle    = Database->Handle;
+          break;
+        }
+      }
+    }
+    
+    //
+    // Try to find driver handle by device path.
+    //
+    if (DriverHandle == NULL) {
+      TempDevicePath = DevicePath;
+      Status = gBS->LocateDevicePath (
+                      &gEfiDevicePathProtocolGuid,
+                      &TempDevicePath,
+                      &DriverHandle
+                      );
+      if (EFI_ERROR (Status) || (DriverHandle == NULL)) {
+        //
+        // Routing data does not match any known driver.
+        // Set Progress to the 'G' in "GUID" of the routing header.
+        //
+        *Progress = StringPtr;
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+    }
+
+    //
+    // Validate ConfigRequest String.
+    //
+    ErrorPtr = ConfigRequestValidate(ConfigRequest);
+    if (ErrorPtr != NULL) {
+      *Progress = StrStr (StringPtr, ErrorPtr);
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    //
+    // Check whether ConfigRequest contains request string.
+    //
+    IfrDataParsedFlag = FALSE;
+    if ((HiiHandle != NULL) && !GetElementsFromRequest(ConfigRequest)) {
+      //
+      // Get the full request string from IFR when HiiPackage is registered to HiiHandle 
+      //
+      IfrDataParsedFlag = TRUE;
+      Status = GetFullStringFromHiiFormPackages (Database, DevicePath, &ConfigRequest, &DefaultResults, &AccessProgress);
+      if (EFI_ERROR (Status)) {
+        //
+        // AccessProgress indicates the parsing progress on <ConfigRequest>.
+        // Map it to the progress on <MultiConfigRequest> then return it.
+        //
+        ASSERT (AccessProgress != NULL);
+        *Progress = StrStr (StringPtr, AccessProgress);
+        goto Done;
+      }
+      //
+      // Not any request block is found.
+      //
+      if (!GetElementsFromRequest(ConfigRequest)) {
+        AccessResults = AllocateCopyPool (StrSize (ConfigRequest), ConfigRequest);
+        goto NextConfigString;
+      }
+    }
+
+    //
+    // Check whether this ConfigRequest is search from Efi varstore type storage.
+    //
+    Status = GetVarStoreType(Database, ConfigRequest, &IsEfiVarStore, &EfiVarStoreInfo);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    
+    if (IsEfiVarStore) {
+      //
+      // Call the GetVariable function to extract settings.
+      //
+      Status = GetConfigRespFromEfiVarStore(This, EfiVarStoreInfo, ConfigRequest, &AccessResults, &AccessProgress);
+      FreePool (EfiVarStoreInfo);
+    } else {
+      //
+      // Call corresponding ConfigAccess protocol to extract settings
+      //
+      Status = gBS->HandleProtocol (
+                      DriverHandle,
+                      &gEfiHiiConfigAccessProtocolGuid,
+                      (VOID **) &ConfigAccess
+                      );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = ConfigAccess->ExtractConfig (
+                               ConfigAccess,
+                               ConfigRequest,
+                               &AccessProgress,
+                               &AccessResults
+                               );
+    }
+    if (EFI_ERROR (Status)) {
+      //
+      // AccessProgress indicates the parsing progress on <ConfigRequest>.
+      // Map it to the progress on <MultiConfigRequest> then return it.
+      //
+      *Progress = StrStr (StringPtr, AccessProgress);
+      goto Done;
+    }
+
+    //
+    // Attach this <ConfigAltResp> to a <MultiConfigAltResp>. There is a '&'
+    // which seperates the first <ConfigAltResp> and the following ones.
+    //
+    ASSERT (*AccessProgress == 0);
+
+    //
+    // Update AccessResults by getting default setting from IFR when HiiPackage is registered to HiiHandle 
+    //
+    if (!IfrDataParsedFlag && HiiHandle != NULL) {
+      Status = GetFullStringFromHiiFormPackages (Database, DevicePath, &ConfigRequest, &DefaultResults, NULL);
+      ASSERT_EFI_ERROR (Status);
+    }
+
+    FreePool (DevicePath);
+    DevicePath = NULL;
+
+    if (DefaultResults != NULL) {
+      Status = MergeDefaultString (&AccessResults, DefaultResults);
+      ASSERT_EFI_ERROR (Status);
+      FreePool (DefaultResults);
+      DefaultResults = NULL;
+    }
+    
+NextConfigString:
+    if (!FirstElement) {
+      Status = AppendToMultiString (Results, L"&");
+      ASSERT_EFI_ERROR (Status);
+    }
+    
+    Status = AppendToMultiString (Results, AccessResults);
+    ASSERT_EFI_ERROR (Status);
+
+    FirstElement = FALSE;
+
+    FreePool (AccessResults);
+    AccessResults = NULL;
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+
+    //
+    // Go to next <ConfigRequest> (skip '&').
+    //
+    StringPtr += Length;
+    if (*StringPtr == 0) {
+      *Progress = StringPtr;
+      break;
+    }
+
+    StringPtr++;
+  }
+
+Done:
+  if (EFI_ERROR (Status)) {
+    FreePool (*Results);
+    *Results = NULL;
+  }
+  
+  if (ConfigRequest != NULL) {
+    FreePool (ConfigRequest);
+  }
+  
+  if (AccessResults != NULL) {
+    FreePool (AccessResults);
+  }
+  
+  if (DefaultResults != NULL) {
+    FreePool (DefaultResults);
+  }
+  
+  if (DevicePath != NULL) {
+    FreePool (DevicePath);
+  }  
+
+  return Status;
+}
+
+
+/**
+  This function allows the caller to request the current configuration for the
+  entirety of the current HII database and returns the data in a
+  null-terminated Unicode string.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  Results                Null-terminated Unicode string in
+                                 <MultiConfigAltResp> format which has all values
+                                 filled in for the entirety of the current HII 
+                                 database. String to be allocated by the  called 
+                                 function. De-allocation is up to the caller.
+
+  @retval EFI_SUCCESS            The Results string is filled with the values
+                                 corresponding to all requested names.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the parts of the
+                                 results that must be stored awaiting possible
+                                 future        protocols.
+  @retval EFI_INVALID_PARAMETER  For example, passing in a NULL for the Results
+                                 parameter would result in this type of error.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingExportConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL      *ConfigAccess;
+  EFI_STRING                          AccessResults;
+  EFI_STRING                          Progress;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          ConfigRequest;
+  UINTN                               Index;
+  EFI_HANDLE                          *ConfigAccessHandles;
+  UINTN                               NumberConfigAccessHandles;
+  BOOLEAN                             FirstElement;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  EFI_HII_HANDLE                      HiiHandle;
+  EFI_STRING                          DefaultResults;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *Database;
+  UINT8                               *DevicePathPkg;
+  UINT8                               *CurrentDevicePath;
+  BOOLEAN                             IfrDataParsedFlag;
+
+  if (This == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Allocate a fix length of memory to store Results. Reallocate memory for
+  // Results if this fix length is insufficient.
+  //
+  *Results = (EFI_STRING) AllocateZeroPool (MAX_STRING_LENGTH);
+  if (*Results == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NumberConfigAccessHandles = 0;
+  Status = gBS->LocateHandleBuffer (
+             ByProtocol,
+             &gEfiHiiConfigAccessProtocolGuid,
+             NULL,
+             &NumberConfigAccessHandles,
+             &ConfigAccessHandles
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FirstElement = TRUE;
+
+  for (Index = 0; Index < NumberConfigAccessHandles; Index++) {
+    Status = gBS->HandleProtocol (
+                    ConfigAccessHandles[Index],
+                    &gEfiHiiConfigAccessProtocolGuid,
+                    (VOID **) &ConfigAccess
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Get DevicePath and HiiHandle for this ConfigAccess driver handle
+    //
+    IfrDataParsedFlag = FALSE;
+    Progress         = NULL;
+    HiiHandle        = NULL;
+    DefaultResults   = NULL;
+    Database         = NULL;
+    ConfigRequest    = NULL;
+    DevicePath       = DevicePathFromHandle (ConfigAccessHandles[Index]);
+    if (DevicePath != NULL) {
+      for (Link = Private->DatabaseList.ForwardLink;
+           Link != &Private->DatabaseList;
+           Link = Link->ForwardLink
+          ) {
+        Database = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+        if ((DevicePathPkg = Database->PackageList->DevicePathPkg) != NULL) {
+          CurrentDevicePath = DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER);
+          if (CompareMem (
+                DevicePath,
+                CurrentDevicePath,
+                GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) CurrentDevicePath)
+                ) == 0) {
+            HiiHandle = Database->Handle;
+            break;
+          }
+        }
+      }
+    }
+
+    Status = ConfigAccess->ExtractConfig (
+                             ConfigAccess,
+                             NULL,
+                             &Progress,
+                             &AccessResults
+                             );
+    if (EFI_ERROR (Status)) {
+      //
+      // Update AccessResults by getting default setting from IFR when HiiPackage is registered to HiiHandle 
+      //
+      if (HiiHandle != NULL && DevicePath != NULL) {
+        IfrDataParsedFlag = TRUE;
+        Status = GetFullStringFromHiiFormPackages (Database, DevicePath, &ConfigRequest, &DefaultResults, NULL);
+        //
+        // Get the full request string to get the Current setting again.
+        //
+        if (!EFI_ERROR (Status) && ConfigRequest != NULL) {
+          Status = ConfigAccess->ExtractConfig (
+                                   ConfigAccess,
+                                   ConfigRequest,
+                                   &Progress,
+                                   &AccessResults
+                                   );
+          FreePool (ConfigRequest);
+        } else {
+          Status = EFI_NOT_FOUND;
+        }
+      }
+    }
+
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update AccessResults by getting default setting from IFR when HiiPackage is registered to HiiHandle 
+      //
+      if (!IfrDataParsedFlag && HiiHandle != NULL && DevicePath != NULL) {
+        StringPtr = StrStr (AccessResults, L"&GUID=");
+        if (StringPtr != NULL) {
+          *StringPtr = 0;
+        }
+        if (GetElementsFromRequest (AccessResults)) {
+          Status = GetFullStringFromHiiFormPackages (Database, DevicePath, &AccessResults, &DefaultResults, NULL);
+          ASSERT_EFI_ERROR (Status);
+        }
+        if (StringPtr != NULL) {
+          *StringPtr = L'&';
+        }
+      }
+      //
+      // Merge the default sting from IFR code into the got setting from driver.
+      //
+      if (DefaultResults != NULL) {
+        Status = MergeDefaultString (&AccessResults, DefaultResults);
+        ASSERT_EFI_ERROR (Status);
+        FreePool (DefaultResults);
+        DefaultResults = NULL;
+      }
+      
+      //
+      // Attach this <ConfigAltResp> to a <MultiConfigAltResp>. There is a '&'
+      // which seperates the first <ConfigAltResp> and the following ones.      
+      //
+      if (!FirstElement) {
+        Status = AppendToMultiString (Results, L"&");
+        ASSERT_EFI_ERROR (Status);
+      }
+      
+      Status = AppendToMultiString (Results, AccessResults);
+      ASSERT_EFI_ERROR (Status);
+
+      FirstElement = FALSE;
+      
+      FreePool (AccessResults);
+      AccessResults = NULL;
+    }
+  }
+  FreePool (ConfigAccessHandles);
+
+  return EFI_SUCCESS;  
+}
+
+
+/**
+  This function processes the results of processing forms and routes it to the
+  appropriate handlers or storage.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  Configuration          A null-terminated Unicode string in
+                                 <MulltiConfigResp> format.
+  @param  Progress               A pointer to a string filled in with the offset of
+                                 the most recent & before the first failing name /
+                                 value pair (or the beginning of the string if the
+                                 failure is in the first name / value pair) or the
+                                 terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The results have been distributed or are awaiting
+                                 distribution.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the parts of the
+                                 results that must be stored awaiting possible
+                                 future        protocols.
+  @retval EFI_INVALID_PARAMETER  Passing in a NULL for the Configuration parameter
+                                 would result in this type of error.
+  @retval EFI_NOT_FOUND          Target for the specified routing data was not
+                                 found.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          ConfigResp;
+  UINTN                               Length;
+  EFI_STATUS                          Status;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL            *TempDevicePath;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *Database;
+  UINT8                               *DevicePathPkg;
+  UINT8                               *CurrentDevicePath;
+  EFI_HANDLE                          DriverHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL      *ConfigAccess;
+  EFI_STRING                          AccessProgress;
+  EFI_IFR_VARSTORE_EFI                *EfiVarStoreInfo;
+  BOOLEAN                             IsEfiVarstore;
+  UINTN                               DevicePathSize;
+
+  if (This == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Configuration == NULL) {
+    *Progress = NULL;
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private   = CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  StringPtr = Configuration;
+  *Progress = StringPtr;
+  Database       = NULL;
+  AccessProgress = NULL;
+  EfiVarStoreInfo= NULL;
+  IsEfiVarstore  = FALSE;
+
+  //
+  // The first element of <MultiConfigResp> should be
+  // <GuidHdr>, which is in 'GUID='<Guid> syntax.
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) == 0) {
+    //
+    // If parsing error, set Progress to the beginning of the <MultiConfigResp>
+    // or most recent & before the error.
+    //
+    if (StringPtr == Configuration) {
+      *Progress = StringPtr;
+    } else {
+      *Progress = StringPtr - 1;
+    }
+
+    //
+    // Process each <ConfigResp> of <MultiConfigResp>
+    //
+    Length = CalculateConfigStringLen (StringPtr);
+    ConfigResp = AllocateCopyPool ((Length + 1) * sizeof (CHAR16), StringPtr);
+    if (ConfigResp == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Append '\0' to the end of ConfigRequest
+    //
+    *(ConfigResp + Length) = 0;
+
+    //
+    // Get the UEFI device path
+    //
+    Status = GetDevicePath (ConfigResp, (UINT8 **) &DevicePath);
+    if (EFI_ERROR (Status)) {
+      FreePool (ConfigResp);
+      return Status;
+    }
+
+    //
+    // Find driver which matches the routing data.
+    //
+    DriverHandle     = NULL;
+    for (Link = Private->DatabaseList.ForwardLink;
+         Link != &Private->DatabaseList;
+         Link = Link->ForwardLink
+        ) {
+      Database = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+
+      if ((DevicePathPkg = Database->PackageList->DevicePathPkg) != NULL) {
+        CurrentDevicePath = DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER);
+        DevicePathSize    = GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) CurrentDevicePath);
+        if ((CompareMem (DevicePath,CurrentDevicePath,DevicePathSize) == 0) && IsThisPackageList(Database, ConfigResp)) {
+          DriverHandle = Database->DriverHandle;
+          break;
+        }
+      }
+    }
+
+    //
+    // Try to find driver handle by device path.
+    //
+    if (DriverHandle == NULL) {
+      TempDevicePath = DevicePath;
+      Status = gBS->LocateDevicePath (
+                      &gEfiDevicePathProtocolGuid,
+                      &TempDevicePath,
+                      &DriverHandle
+                      );
+      if (EFI_ERROR (Status) || (DriverHandle == NULL)) {
+        //
+        // Routing data does not match any known driver.
+        // Set Progress to the 'G' in "GUID" of the routing header.
+        //
+        FreePool (DevicePath);
+        *Progress = StringPtr;
+        FreePool (ConfigResp);
+        return EFI_NOT_FOUND;
+      }
+    }
+
+    FreePool (DevicePath);
+
+    //
+    // Check whether this ConfigRequest is search from Efi varstore type storage.
+    //
+    Status = GetVarStoreType(Database, ConfigResp, &IsEfiVarstore, &EfiVarStoreInfo);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (IsEfiVarstore) {
+      //
+      // Call the SetVariable function to route settings.
+      //    
+      Status = RouteConfigRespForEfiVarStore(This, EfiVarStoreInfo, ConfigResp, &AccessProgress);
+      FreePool (EfiVarStoreInfo);
+    } else {
+      //
+      // Call corresponding ConfigAccess protocol to route settings
+      //
+      Status = gBS->HandleProtocol (
+                      DriverHandle,
+                      &gEfiHiiConfigAccessProtocolGuid,
+                      (VOID **)  &ConfigAccess
+                      );
+      ASSERT_EFI_ERROR (Status);
+
+      Status = ConfigAccess->RouteConfig (
+                               ConfigAccess,
+                               ConfigResp,
+                               &AccessProgress
+                               );
+    }
+    if (EFI_ERROR (Status)) {
+      ASSERT (AccessProgress != NULL);
+      //
+      // AccessProgress indicates the parsing progress on <ConfigResp>.
+      // Map it to the progress on <MultiConfigResp> then return it.
+      //
+      *Progress = StrStr (StringPtr, AccessProgress);
+
+      FreePool (ConfigResp);
+      return Status;
+    }
+
+    FreePool (ConfigResp);
+    ConfigResp = NULL;
+
+    //
+    // Go to next <ConfigResp> (skip '&').
+    //
+    StringPtr += Length;
+    if (*StringPtr == 0) {
+      *Progress = StringPtr;
+      break;
+    }
+
+    StringPtr++;
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This helper function is to be called by drivers to map configuration data
+  stored in byte array ("block") formats such as UEFI Variables into current
+  configuration strings.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  ConfigRequest          A null-terminated Unicode string in
+                                 <ConfigRequest> format.
+  @param  Block                  Array of bytes defining the block's configuration.
+  @param  BlockSize              Length in bytes of Block.
+  @param  Config                 Filled-in configuration string. String allocated
+                                 by  the function. Returned only if call is
+                                 successful. It is <ConfigResp> string format.
+  @param  Progress               A pointer to a string filled in with the offset of
+                                  the most recent & before the first failing
+                                 name/value pair (or the beginning of the string if
+                                 the failure is in the first name / value pair) or
+                                 the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The request succeeded. Progress points to the null
+                                 terminator at the end of the ConfigRequest
+                                 string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to allocate Config.     Progress
+                                 points to the first character of ConfigRequest.
+  @retval EFI_INVALID_PARAMETER  Passing in a NULL for the ConfigRequest or
+                                 Block parameter would result in this type of
+                                 error. Progress points to the first character of
+                                 ConfigRequest.
+  @retval EFI_DEVICE_ERROR       Block not large enough. Progress undefined.
+  @retval EFI_INVALID_PARAMETER  Encountered non <BlockName> formatted string.
+                                     Block is left updated and Progress points at
+                                 the "&" preceding the first non-<BlockName>.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiBlockToConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       ConfigRequest,
+  IN  CONST UINT8                            *Block,
+  IN  CONST UINTN                            BlockSize,
+  OUT EFI_STRING                             *Config,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  EFI_STRING                          StringPtr;
+  UINTN                               Length;
+  EFI_STATUS                          Status;
+  EFI_STRING                          TmpPtr;
+  UINT8                               *TmpBuffer;
+  UINTN                               Offset;
+  UINTN                               Width;
+  UINT8                               *Value;
+  EFI_STRING                          ValueStr;
+  EFI_STRING                          ConfigElement;
+  UINTN                               Index;
+  UINT8                               *TemBuffer;
+  CHAR16                              *TemString;
+  CHAR16                              TemChar;
+
+  TmpBuffer = NULL;
+
+  if (This == NULL || Progress == NULL || Config == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Block == NULL || ConfigRequest == NULL) {
+    *Progress = ConfigRequest;
+    return EFI_INVALID_PARAMETER;
+  }
+
+
+  Private = CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  ASSERT (Private != NULL);
+
+  StringPtr     = ConfigRequest;
+  ValueStr      = NULL;
+  Value         = NULL;
+  ConfigElement = NULL;
+
+  //
+  // Allocate a fix length of memory to store Results. Reallocate memory for
+  // Results if this fix length is insufficient.
+  //
+  *Config = (EFI_STRING) AllocateZeroPool (MAX_STRING_LENGTH);
+  if (*Config == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Jump <ConfigHdr>
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    *Progress = StringPtr;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"PATH=", StrLen (L"PATH=")) != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == 0) {
+    *Progress = StringPtr - 1;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+
+  while (*StringPtr != L'&' && *StringPtr != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == 0) {
+    *Progress = StringPtr;
+
+    AppendToMultiString(Config, ConfigRequest);
+    HiiToLower (*Config);
+
+    return EFI_SUCCESS;
+  }
+  //
+  // Skip '&'
+  //
+  StringPtr++;
+
+  //
+  // Copy <ConfigHdr> and an additional '&' to <ConfigResp>
+  //
+  TemChar = *StringPtr;
+  *StringPtr = '\0';
+  AppendToMultiString(Config, ConfigRequest);
+  *StringPtr = TemChar;
+
+  //
+  // Parse each <RequestElement> if exists
+  // Only <BlockName> format is supported by this help function.
+  // <BlockName> ::= 'OFFSET='<Number>&'WIDTH='<Number>
+  //
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"OFFSET=", StrLen (L"OFFSET=")) == 0) {
+    //
+    // Back up the header of one <BlockName>
+    //
+    TmpPtr = StringPtr;
+
+    StringPtr += StrLen (L"OFFSET=");
+    //
+    // Get Offset
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      *Progress = TmpPtr - 1;
+      goto Exit;
+    }
+    Offset = 0;
+    CopyMem (
+      &Offset,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      *Progress = TmpPtr - 1;
+      Status = EFI_INVALID_PARAMETER;
+      goto Exit;
+    }
+    StringPtr += StrLen (L"&WIDTH=");
+
+    //
+    // Get Width
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      *Progress =  TmpPtr - 1;
+      goto Exit;
+    }
+    Width = 0;
+    CopyMem (
+      &Width,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (*StringPtr != 0 && *StringPtr != L'&') {
+      *Progress =  TmpPtr - 1;
+      Status = EFI_INVALID_PARAMETER;
+      goto Exit;
+    }
+
+    //
+    // Calculate Value and convert it to hex string.
+    //
+    if (Offset + Width > BlockSize) {
+      *Progress = StringPtr;
+      Status = EFI_DEVICE_ERROR;
+      goto Exit;
+    }
+
+    Value = (UINT8 *) AllocateZeroPool (Width);
+    if (Value == NULL) {
+      *Progress = ConfigRequest;
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    CopyMem (Value, (UINT8 *) Block + Offset, Width);
+
+    Length = Width * 2 + 1;
+    ValueStr = (EFI_STRING) AllocateZeroPool (Length  * sizeof (CHAR16));
+    if (ValueStr == NULL) {
+      *Progress = ConfigRequest;
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    
+    TemString = ValueStr;
+    TemBuffer = Value + Width - 1;
+    for (Index = 0; Index < Width; Index ++, TemBuffer --) {
+      TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemBuffer, 2);
+    }
+
+    FreePool (Value);
+    Value = NULL;
+
+    //
+    // Build a ConfigElement
+    //
+    Length += StringPtr - TmpPtr + 1 + StrLen (L"VALUE=");
+    ConfigElement = (EFI_STRING) AllocateZeroPool (Length * sizeof (CHAR16));
+    if (ConfigElement == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    CopyMem (ConfigElement, TmpPtr, (StringPtr - TmpPtr + 1) * sizeof (CHAR16));
+    if (*StringPtr == 0) {
+      *(ConfigElement + (StringPtr - TmpPtr)) = L'&';
+    }
+    *(ConfigElement + (StringPtr - TmpPtr) + 1) = 0;
+    StrCatS (ConfigElement, Length, L"VALUE=");
+    StrCatS (ConfigElement, Length, ValueStr);
+
+    AppendToMultiString (Config, ConfigElement);
+
+    FreePool (ConfigElement);
+    FreePool (ValueStr);
+    ConfigElement = NULL;
+    ValueStr = NULL;
+
+    //
+    // If '\0', parsing is finished. Otherwise skip '&' to continue
+    //
+    if (*StringPtr == 0) {
+      break;
+    }
+    AppendToMultiString (Config, L"&");
+    StringPtr++;
+
+  }
+
+  if (*StringPtr != 0) {
+    *Progress = StringPtr - 1;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+  
+  HiiToLower (*Config);
+  *Progress = StringPtr;
+  return EFI_SUCCESS;
+
+Exit:
+  if (*Config != NULL) {
+  FreePool (*Config);
+  *Config = NULL;
+  }
+  if (ValueStr != NULL) {
+    FreePool (ValueStr);
+  }
+  if (Value != NULL) {
+    FreePool (Value);
+  }
+  if (ConfigElement != NULL) {
+    FreePool (ConfigElement);
+  }
+
+  return Status;
+
+}
+
+
+/**
+  This helper function is to be called by drivers to map configuration strings
+  to configurations stored in byte array ("block") formats such as UEFI Variables.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  ConfigResp             A null-terminated Unicode string in <ConfigResp>
+                                 format.
+  @param  Block                  A possibly null array of bytes representing the
+                                 current  block. Only bytes referenced in the
+                                 ConfigResp string  in the block are modified. If
+                                 this parameter is null or if the *BlockSize
+                                 parameter is (on input) shorter than required by
+                                 the Configuration string, only the BlockSize
+                                 parameter is updated and an appropriate status
+                                 (see below)  is returned.
+  @param  BlockSize              The length of the Block in units of UINT8.  On
+                                 input, this is the size of the Block. On output,
+                                 if successful, contains the largest index of the
+                                 modified byte in the Block, or the required buffer
+                                 size if the Block is not large enough.
+  @param  Progress               On return, points to an element of the ConfigResp
+                                 string filled in with the offset of the most
+                                 recent '&' before the first failing name / value
+                                 pair (or  the beginning of the string if the
+                                 failure is in the  first name / value pair) or the
+                                 terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The request succeeded. Progress points to the null
+                                 terminator at the end of the ConfigResp string.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to allocate Config.     Progress
+                                 points to the first character of ConfigResp.
+  @retval EFI_INVALID_PARAMETER  Passing in a NULL for the ConfigResp or
+                                 Block parameter would result in this type of
+                                 error. Progress points to the first character of
+                                         ConfigResp.
+  @retval EFI_INVALID_PARAMETER  Encountered non <BlockName> formatted name /
+                                 value pair. Block is left updated and
+                                 Progress points at the '&' preceding the first
+                                 non-<BlockName>.
+  @retval EFI_BUFFER_TOO_SMALL   Block not large enough. Progress undefined. 
+                                 BlockSize is updated with the required buffer size.
+  @retval EFI_NOT_FOUND          Target for the specified routing data was not found.
+                                 Progress points to the "G" in "GUID" of the errant
+                                 routing data.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigToBlock (
+  IN     CONST EFI_HII_CONFIG_ROUTING_PROTOCOL *This,
+  IN     CONST EFI_STRING                      ConfigResp,
+  IN OUT UINT8                                 *Block,
+  IN OUT UINTN                                 *BlockSize,
+  OUT    EFI_STRING                            *Progress
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          TmpPtr;
+  UINTN                               Length;
+  EFI_STATUS                          Status;
+  UINT8                               *TmpBuffer;
+  UINTN                               Offset;
+  UINTN                               Width;
+  UINT8                               *Value;
+  UINTN                               BufferSize;
+  UINTN                               MaxBlockSize;
+
+  TmpBuffer = NULL;
+
+  if (This == NULL || BlockSize == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = ConfigResp;
+  if (ConfigResp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  ASSERT (Private != NULL);
+
+  StringPtr  = ConfigResp;
+  BufferSize = *BlockSize;
+  Value      = NULL;
+  MaxBlockSize = 0;
+
+  //
+  // Jump <ConfigHdr>
+  //
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    *Progress = StringPtr;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"PATH=", StrLen (L"PATH=")) != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == 0) {
+    *Progress = StringPtr;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+
+  while (*StringPtr != L'&' && *StringPtr != 0) {
+    StringPtr++;
+  }
+  if (*StringPtr == 0) {
+    *Progress = StringPtr;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+
+  //
+  // Parse each <ConfigElement> if exists
+  // Only '&'<BlockConfig> format is supported by this help function.
+  // <BlockConfig> ::= 'OFFSET='<Number>&'WIDTH='<Number>&'VALUE='<Number>
+  //
+  while (*StringPtr != 0 && StrnCmp (StringPtr, L"&OFFSET=", StrLen (L"&OFFSET=")) == 0) {
+    TmpPtr     = StringPtr;
+    StringPtr += StrLen (L"&OFFSET=");
+    //
+    // Get Offset
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      *Progress = TmpPtr;
+      goto Exit;
+    }
+    Offset = 0;
+    CopyMem (
+      &Offset,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&WIDTH=", StrLen (L"&WIDTH=")) != 0) {
+      *Progress = TmpPtr;
+      Status = EFI_INVALID_PARAMETER;
+      goto Exit;
+    }
+    StringPtr += StrLen (L"&WIDTH=");
+
+    //
+    // Get Width
+    //
+    Status = GetValueOfNumber (StringPtr, &TmpBuffer, &Length);
+    if (EFI_ERROR (Status)) {
+      *Progress = TmpPtr;
+      goto Exit;
+    }
+    Width = 0;
+    CopyMem (
+      &Width,
+      TmpBuffer,
+      (((Length + 1) / 2) < sizeof (UINTN)) ? ((Length + 1) / 2) : sizeof (UINTN)
+      );
+    FreePool (TmpBuffer);
+
+    StringPtr += Length;
+    if (StrnCmp (StringPtr, L"&VALUE=", StrLen (L"&VALUE=")) != 0) {
+      *Progress = TmpPtr;
+      Status = EFI_INVALID_PARAMETER;
+      goto Exit;
+    }
+    StringPtr += StrLen (L"&VALUE=");
+
+    //
+    // Get Value
+    //
+    Status = GetValueOfNumber (StringPtr, &Value, &Length);
+    if (EFI_ERROR (Status)) {
+      *Progress = TmpPtr;
+      goto Exit;
+    }
+
+    StringPtr += Length;
+    if (*StringPtr != 0 && *StringPtr != L'&') {
+      *Progress = TmpPtr;
+      Status = EFI_INVALID_PARAMETER;
+      goto Exit;
+    }
+
+    //
+    // Update the Block with configuration info
+    //
+    if ((Block != NULL) && (Offset + Width <= BufferSize)) {
+      CopyMem (Block + Offset, Value, Width);
+    }
+    if (Offset + Width > MaxBlockSize) {
+      MaxBlockSize = Offset + Width;
+    }
+
+    FreePool (Value);
+    Value = NULL;
+
+    //
+    // If '\0', parsing is finished.
+    //
+    if (*StringPtr == 0) {
+      break;
+    }
+  }
+  
+  //
+  // The input string is not ConfigResp format, return error.
+  //
+  if (*StringPtr != 0) {
+    *Progress = StringPtr;
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+
+  *Progress = StringPtr + StrLen (StringPtr);
+  *BlockSize = MaxBlockSize - 1;
+
+  if (MaxBlockSize > BufferSize) {
+    *BlockSize = MaxBlockSize;
+    if (Block != NULL) {
+      return EFI_BUFFER_TOO_SMALL;
+    }
+  }
+
+  if (Block == NULL) {
+    *Progress = ConfigResp;
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+
+Exit:
+
+  if (Value != NULL) {
+    FreePool (Value);
+  }
+  return Status;
+}
+
+
+/**
+  This helper function is to be called by drivers to extract portions of
+  a larger configuration string.
+
+  @param  This                   A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                 instance.
+  @param  Configuration          A null-terminated Unicode string in
+                                 <MultiConfigAltResp> format.
+  @param  Guid                   A pointer to the GUID value to search for in the
+                                 routing portion of the ConfigResp string when
+                                 retrieving  the requested data. If Guid is NULL,
+                                 then all GUID  values will be searched for.
+  @param  Name                   A pointer to the NAME value to search for in the
+                                 routing portion of the ConfigResp string when
+                                 retrieving  the requested data. If Name is NULL,
+                                 then all Name  values will be searched for.
+  @param  DevicePath             A pointer to the PATH value to search for in the
+                                 routing portion of the ConfigResp string when
+                                 retrieving  the requested data. If DevicePath is
+                                 NULL, then all  DevicePath values will be searched
+                                 for.
+  @param  AltCfgId               A pointer to the ALTCFG value to search for in the
+                                  routing portion of the ConfigResp string when
+                                 retrieving  the requested data.  If this parameter
+                                 is NULL,  then the current setting will be
+                                 retrieved.
+  @param  AltCfgResp             A pointer to a buffer which will be allocated by
+                                 the  function which contains the retrieved string
+                                 as requested.   This buffer is only allocated if
+                                 the call was successful. It is <ConfigResp> format.
+
+  @retval EFI_SUCCESS            The request succeeded. The requested data was
+                                 extracted  and placed in the newly allocated
+                                 AltCfgResp buffer.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to allocate AltCfgResp.
+  @retval EFI_INVALID_PARAMETER  Any parameter is invalid.
+  @retval EFI_NOT_FOUND          Target for the specified routing data was not
+                                 found.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetAltCfg (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL    *This,
+  IN  CONST EFI_STRING                         Configuration,
+  IN  CONST EFI_GUID                           *Guid,
+  IN  CONST EFI_STRING                         Name,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  IN  CONST UINT16                             *AltCfgId,
+  OUT EFI_STRING                               *AltCfgResp
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          HdrStart;
+  EFI_STRING                          HdrEnd;
+  EFI_STRING                          TmpPtr;
+  UINTN                               Length;
+  EFI_STRING                          GuidStr;
+  EFI_STRING                          NameStr;
+  EFI_STRING                          PathStr;
+  EFI_STRING                          AltIdStr;
+  EFI_STRING                          Result;
+  BOOLEAN                             GuidFlag;
+  BOOLEAN                             NameFlag;
+  BOOLEAN                             PathFlag;
+
+  HdrStart = NULL;
+  HdrEnd   = NULL;
+  GuidStr  = NULL;
+  NameStr  = NULL;
+  PathStr  = NULL;
+  AltIdStr = NULL;
+  Result   = NULL;
+  GuidFlag = FALSE;
+  NameFlag = FALSE;
+  PathFlag = FALSE;
+
+  if (This == NULL || Configuration == NULL || AltCfgResp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StringPtr = Configuration;
+  if (StrnCmp (StringPtr, L"GUID=", StrLen (L"GUID=")) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Generate the sub string for later matching.
+  //
+  GenerateSubStr (L"GUID=", sizeof (EFI_GUID), (VOID *) Guid, 1, &GuidStr);
+  GenerateSubStr (
+    L"PATH=",
+    GetDevicePathSize ((EFI_DEVICE_PATH_PROTOCOL *) DevicePath),
+    (VOID *) DevicePath,
+    1,
+    &PathStr
+    );
+  if (AltCfgId != NULL) {
+    GenerateSubStr (L"ALTCFG=", sizeof (UINT16), (VOID *) AltCfgId, 3, &AltIdStr);  
+  }
+  if (Name != NULL) {
+    GenerateSubStr (L"NAME=", StrLen (Name) * sizeof (CHAR16), (VOID *) Name, 2, &NameStr);    
+  } else {
+    GenerateSubStr (L"NAME=", 0, NULL, 2, &NameStr);
+  }
+
+  while (*StringPtr != 0) {
+    //
+    // Try to match the GUID
+    //
+    if (!GuidFlag) {
+      TmpPtr = StrStr (StringPtr, GuidStr);
+      if (TmpPtr == NULL) {
+        Status = EFI_NOT_FOUND;
+        goto Exit;
+      }
+      HdrStart = TmpPtr;
+
+      //
+      // Jump to <NameHdr>
+      //
+      if (Guid != NULL) {
+        StringPtr = TmpPtr + StrLen (GuidStr);
+      } else {
+        StringPtr = StrStr (TmpPtr, L"NAME=");
+        if (StringPtr == NULL) {
+          Status = EFI_NOT_FOUND;
+          goto Exit;
+        }
+      }
+      GuidFlag = TRUE;
+    }
+
+    //
+    // Try to match the NAME
+    //
+    if (GuidFlag && !NameFlag) {
+      if (StrnCmp (StringPtr, NameStr, StrLen (NameStr)) != 0) {
+        GuidFlag = FALSE;
+      } else {
+        //
+        // Jump to <PathHdr>
+        //
+        if (Name != NULL) {
+          StringPtr += StrLen (NameStr);
+        } else {
+          StringPtr = StrStr (StringPtr, L"PATH=");
+          if (StringPtr == NULL) {
+            Status = EFI_NOT_FOUND;
+            goto Exit;
+          }
+        }
+        NameFlag = TRUE;
+      }
+    }
+
+    //
+    // Try to match the DevicePath
+    //
+    if (GuidFlag && NameFlag && !PathFlag) {
+      if (StrnCmp (StringPtr, PathStr, StrLen (PathStr)) != 0) {
+        GuidFlag = FALSE;
+        NameFlag = FALSE;
+      } else {
+        //
+        // Jump to '&' before <DescHdr> or <ConfigBody>
+        //
+        if (DevicePath != NULL) {
+          StringPtr += StrLen (PathStr);
+        } else {
+          StringPtr = StrStr (StringPtr, L"&");
+          if (StringPtr == NULL) {
+            Status = EFI_NOT_FOUND;
+            goto Exit;
+          }
+          StringPtr ++;
+        }
+        PathFlag = TRUE;
+        HdrEnd   = StringPtr;
+      }
+    }
+
+    //
+    // Try to match the AltCfgId
+    //
+    if (GuidFlag && NameFlag && PathFlag) {
+      if (AltCfgId == NULL) {
+        //
+        // Return Current Setting when AltCfgId is NULL.
+        //
+        Status = OutputConfigBody (StringPtr, &Result);
+        goto Exit;
+      }
+      //
+      // Search the <ConfigAltResp> to get the <AltResp> with AltCfgId.
+      //
+      if (StrnCmp (StringPtr, AltIdStr, StrLen (AltIdStr)) != 0) {
+        GuidFlag = FALSE;
+        NameFlag = FALSE;
+        PathFlag = FALSE;
+      } else {
+        //
+        // Skip AltIdStr and &
+        //
+        StringPtr = StringPtr + StrLen (AltIdStr);
+        Status    = OutputConfigBody (StringPtr, &Result);
+        goto Exit;
+      }
+    }
+  }
+
+  Status = EFI_NOT_FOUND;
+
+Exit:
+  *AltCfgResp = NULL;
+  if (!EFI_ERROR (Status) && (Result != NULL)) {
+    //
+    // Copy the <ConfigHdr> and <ConfigBody>
+    //
+    Length = HdrEnd - HdrStart + StrLen (Result) + 1;
+    *AltCfgResp = AllocateZeroPool (Length * sizeof (CHAR16));
+    if (*AltCfgResp == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+    } else {
+      StrnCpyS (*AltCfgResp, Length, HdrStart, HdrEnd - HdrStart);
+      StrCatS (*AltCfgResp, Length, Result);
+      Status = EFI_SUCCESS;
+    }
+  }
+
+  if (GuidStr != NULL) {
+    FreePool (GuidStr);
+  }
+  if (NameStr != NULL) {
+    FreePool (NameStr);
+  }
+  if (PathStr != NULL) {
+    FreePool (PathStr);
+  }
+  if (AltIdStr != NULL) {
+    FreePool (AltIdStr);
+  }
+  if (Result != NULL) {
+    FreePool (Result);
+  }
+
+  return Status;
+
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Database.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Database.c
new file mode 100644
index 0000000000..ec56795ebb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Database.c
@@ -0,0 +1,3860 @@
+/** @file
+Implementation for EFI_HII_DATABASE_PROTOCOL.
+
+Copyright (c) 2007 - 2015, 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 "HiiDatabase.h"
+
+/**
+  This function generates a HII_DATABASE_RECORD node and adds into hii database.
+  This is a internal function.
+
+  @param  Private                hii database private structure
+  @param  DatabaseNode           HII_DATABASE_RECORD node which is used to store a
+                                 package list
+
+  @retval EFI_SUCCESS            A database record is generated successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 database contents.
+  @retval EFI_INVALID_PARAMETER  Private is NULL or DatabaseRecord is NULL.
+
+**/
+EFI_STATUS
+GenerateHiiDatabaseRecord (
+  IN  HII_DATABASE_PRIVATE_DATA *Private,
+  OUT HII_DATABASE_RECORD       **DatabaseNode
+  )
+{
+  HII_DATABASE_RECORD                *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList;
+  HII_HANDLE                         *HiiHandle;
+
+  if (Private == NULL || DatabaseNode == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DatabaseRecord = (HII_DATABASE_RECORD *) AllocateZeroPool (sizeof (HII_DATABASE_RECORD));
+  if (DatabaseRecord == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  DatabaseRecord->Signature = HII_DATABASE_RECORD_SIGNATURE;
+
+  DatabaseRecord->PackageList = AllocateZeroPool (sizeof (HII_DATABASE_PACKAGE_LIST_INSTANCE));
+  if (DatabaseRecord->PackageList == NULL) {
+    FreePool (DatabaseRecord);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  PackageList = DatabaseRecord->PackageList;
+
+  InitializeListHead (&PackageList->GuidPkgHdr);
+  InitializeListHead (&PackageList->FormPkgHdr);
+  InitializeListHead (&PackageList->KeyboardLayoutHdr);
+  InitializeListHead (&PackageList->StringPkgHdr);
+  InitializeListHead (&PackageList->FontPkgHdr);
+  InitializeListHead (&PackageList->SimpleFontPkgHdr);
+  PackageList->ImagePkg      = NULL;
+  PackageList->DevicePathPkg = NULL;
+
+  //
+  // Create a new hii handle
+  //
+  HiiHandle = (HII_HANDLE *) AllocateZeroPool (sizeof (HII_HANDLE));
+  if (HiiHandle == NULL) {
+    FreePool (DatabaseRecord->PackageList);
+    FreePool (DatabaseRecord);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  HiiHandle->Signature = HII_HANDLE_SIGNATURE;
+  //
+  // Backup the number of Hii handles
+  //
+  Private->HiiHandleCount++;
+  HiiHandle->Key = (UINTN) Private->HiiHandleCount;
+  //
+  // Insert the handle to hii handle list of the whole database.
+  //
+  InsertTailList (&Private->HiiHandleList, &HiiHandle->Handle);
+
+  DatabaseRecord->Handle = (EFI_HII_HANDLE) HiiHandle;
+
+  //
+  // Insert the Package List node to Package List link of the whole database.
+  //
+  InsertTailList (&Private->DatabaseList, &DatabaseRecord->DatabaseEntry);
+
+  *DatabaseNode = DatabaseRecord;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  This function checks whether a handle is a valid EFI_HII_HANDLE
+  This is a internal function.
+
+  @param  Handle                 Pointer to a EFI_HII_HANDLE
+
+  @retval TRUE                   Valid
+  @retval FALSE                  Invalid
+
+**/
+BOOLEAN
+IsHiiHandleValid (
+  EFI_HII_HANDLE Handle
+  )
+{
+  HII_HANDLE    *HiiHandle;
+
+  HiiHandle = (HII_HANDLE *) Handle;
+
+  if (HiiHandle == NULL) {
+    return FALSE;
+  }
+
+  if (HiiHandle->Signature != HII_HANDLE_SIGNATURE) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  This function invokes the matching registered function.
+  This is a internal function.
+
+  @param  Private                HII Database driver private structure.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageInstance        Points to the package referred to by the
+                                 notification.
+  @param  PackageType            Package type
+  @param  Handle                 The handle of the package list which contains the
+                                 specified package.
+
+  @retval EFI_SUCCESS            Already checked all registered function and
+                                 invoked  if matched.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+InvokeRegisteredFunction (
+  IN HII_DATABASE_PRIVATE_DATA    *Private,
+  IN EFI_HII_DATABASE_NOTIFY_TYPE NotifyType,
+  IN VOID                         *PackageInstance,
+  IN UINT8                        PackageType,
+  IN EFI_HII_HANDLE               Handle
+  )
+{
+  HII_DATABASE_NOTIFY             *Notify;
+  LIST_ENTRY                      *Link;
+  EFI_HII_PACKAGE_HEADER          *Package;
+  UINT8                           *Buffer;
+  UINT32                          BufferSize;
+  UINT32                          HeaderSize;
+  UINT32                          ImageBlockSize;
+  UINT32                          PaletteInfoSize;
+
+  if (Private == NULL || (NotifyType & 0xF) == 0 || PackageInstance == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Private->Signature != HII_DATABASE_PRIVATE_DATA_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (!IsHiiHandleValid (Handle)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Buffer  = NULL;
+  Package = NULL;
+
+  //
+  // Convert the incoming package from hii database storage format to UEFI
+  // storage format. e.g. HII_GUID_PACKAGE_INSTANCE to EFI_HII_GUID_PACKAGE_HDR.
+  //
+  switch (PackageType) {
+  case EFI_HII_PACKAGE_TYPE_GUID:
+    Package = (EFI_HII_PACKAGE_HEADER *) (((HII_GUID_PACKAGE_INSTANCE *) PackageInstance)->GuidPkg);
+    break;
+
+  case EFI_HII_PACKAGE_FORMS:
+    BufferSize = ((HII_IFR_PACKAGE_INSTANCE *) PackageInstance)->FormPkgHdr.Length;
+    Buffer = (UINT8 *) AllocateZeroPool (BufferSize);
+    ASSERT (Buffer != NULL);
+    CopyMem (
+      Buffer,
+      &((HII_IFR_PACKAGE_INSTANCE *) PackageInstance)->FormPkgHdr,
+      sizeof (EFI_HII_PACKAGE_HEADER)
+      );
+    CopyMem (
+      Buffer + sizeof (EFI_HII_PACKAGE_HEADER),
+      ((HII_IFR_PACKAGE_INSTANCE *) PackageInstance)->IfrData,
+      BufferSize - sizeof (EFI_HII_PACKAGE_HEADER)
+      );
+    Package = (EFI_HII_PACKAGE_HEADER *) Buffer;
+    break;
+
+  case EFI_HII_PACKAGE_KEYBOARD_LAYOUT:
+    Package = (EFI_HII_PACKAGE_HEADER *) (((HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *) PackageInstance)->KeyboardPkg);
+    break;
+
+  case EFI_HII_PACKAGE_STRINGS:
+    BufferSize = ((HII_STRING_PACKAGE_INSTANCE *) PackageInstance)->StringPkgHdr->Header.Length;
+    HeaderSize = ((HII_STRING_PACKAGE_INSTANCE *) PackageInstance)->StringPkgHdr->HdrSize;
+    Buffer = (UINT8 *) AllocateZeroPool (BufferSize);
+    ASSERT (Buffer != NULL);
+    CopyMem (
+      Buffer,
+      ((HII_STRING_PACKAGE_INSTANCE *) PackageInstance)->StringPkgHdr,
+      HeaderSize
+      );
+    CopyMem (
+      Buffer + HeaderSize,
+      ((HII_STRING_PACKAGE_INSTANCE *) PackageInstance)->StringBlock,
+      BufferSize - HeaderSize
+      );
+    Package = (EFI_HII_PACKAGE_HEADER *) Buffer;
+    break;
+
+  case EFI_HII_PACKAGE_FONTS:
+    BufferSize = ((HII_FONT_PACKAGE_INSTANCE *) PackageInstance)->FontPkgHdr->Header.Length;
+    HeaderSize = ((HII_FONT_PACKAGE_INSTANCE *) PackageInstance)->FontPkgHdr->HdrSize;
+    Buffer = (UINT8 *) AllocateZeroPool (BufferSize);
+    ASSERT (Buffer != NULL);
+    CopyMem (
+      Buffer,
+      ((HII_FONT_PACKAGE_INSTANCE *) PackageInstance)->FontPkgHdr,
+      HeaderSize
+      );
+    CopyMem (
+      Buffer + HeaderSize,
+      ((HII_FONT_PACKAGE_INSTANCE *) PackageInstance)->GlyphBlock,
+      BufferSize - HeaderSize
+      );
+    Package = (EFI_HII_PACKAGE_HEADER *) Buffer;
+    break;
+
+  case EFI_HII_PACKAGE_IMAGES:
+    BufferSize = ((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->ImagePkgHdr.Header.Length;
+    HeaderSize = sizeof (EFI_HII_IMAGE_PACKAGE_HDR);
+    Buffer = (UINT8 *) AllocateZeroPool (BufferSize);
+    ASSERT (Buffer != NULL);
+
+    CopyMem (
+      Buffer,
+      &((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->ImagePkgHdr,
+      HeaderSize
+      );
+    CopyMem (
+      Buffer + sizeof (EFI_HII_PACKAGE_HEADER),
+      &HeaderSize,
+      sizeof (UINT32)
+      );
+
+    ImageBlockSize = ((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->ImageBlockSize;
+    if (ImageBlockSize != 0) {
+      CopyMem (
+        Buffer + HeaderSize,
+        ((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->ImageBlock,
+        ImageBlockSize
+        );
+    }
+
+    PaletteInfoSize = ((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->PaletteInfoSize;
+    if (PaletteInfoSize != 0) {
+      CopyMem (
+        Buffer + HeaderSize + ImageBlockSize,
+        ((HII_IMAGE_PACKAGE_INSTANCE *) PackageInstance)->PaletteBlock,
+        PaletteInfoSize
+        );
+      HeaderSize += ImageBlockSize;
+      CopyMem (
+        Buffer + sizeof (EFI_HII_PACKAGE_HEADER) + sizeof (UINT32),
+        &HeaderSize,
+        sizeof (UINT32)
+        );
+    }
+    Package = (EFI_HII_PACKAGE_HEADER *) Buffer;
+    break;
+
+  case EFI_HII_PACKAGE_SIMPLE_FONTS:
+    BufferSize = ((HII_SIMPLE_FONT_PACKAGE_INSTANCE *) PackageInstance)->SimpleFontPkgHdr->Header.Length;
+    Buffer = (UINT8 *) AllocateZeroPool (BufferSize);
+    ASSERT (Buffer != NULL);
+    CopyMem (
+      Buffer,
+      ((HII_SIMPLE_FONT_PACKAGE_INSTANCE *) PackageInstance)->SimpleFontPkgHdr,
+      BufferSize
+      );
+    Package = (EFI_HII_PACKAGE_HEADER *) Buffer;
+    break;
+
+  case EFI_HII_PACKAGE_DEVICE_PATH:
+    Package = (EFI_HII_PACKAGE_HEADER *) PackageInstance;
+    break;
+
+  default:
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Link = Private->DatabaseNotifyList.ForwardLink;
+       Link != &Private->DatabaseNotifyList;
+       Link = Link->ForwardLink
+      ) {
+    Notify = CR (Link, HII_DATABASE_NOTIFY, DatabaseNotifyEntry, HII_DATABASE_NOTIFY_SIGNATURE);
+    if (Notify->NotifyType == NotifyType && Notify->PackageType == PackageType) {
+      //
+      // Check in case PackageGuid is not NULL when Package is GUID package
+      //
+      if (PackageType != EFI_HII_PACKAGE_TYPE_GUID) {
+        Notify->PackageGuid = NULL;
+      }
+      //
+      // Status of Registered Function is unknown so did not check it
+      //
+      Notify->PackageNotifyFn (
+        Notify->PackageType,
+        Notify->PackageGuid,
+        Package,
+        Handle,
+        NotifyType
+        );
+    }
+  }
+
+  if (Buffer != NULL) {
+    FreePool (Buffer);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a GUID package to a package list node.
+  This is a internal function.
+
+  @param  PackageHdr             Pointer to a buffer stored with GUID package
+                                 information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created GUID pacakge
+
+  @retval EFI_SUCCESS            Guid Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Guid package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertGuidPackage (
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT    HII_GUID_PACKAGE_INSTANCE          **Package
+  )
+{
+  HII_GUID_PACKAGE_INSTANCE            *GuidPackage;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+
+  if (PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CopyMem (&PackageHeader, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  //
+  // Create a GUID package node
+  //
+  GuidPackage = (HII_GUID_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_GUID_PACKAGE_INSTANCE));
+  if (GuidPackage == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  GuidPackage->GuidPkg = (UINT8 *) AllocateZeroPool (PackageHeader.Length);
+  if (GuidPackage->GuidPkg == NULL) {
+    FreePool (GuidPackage);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  GuidPackage->Signature = HII_GUID_PACKAGE_SIGNATURE;
+  CopyMem (GuidPackage->GuidPkg, PackageHdr, PackageHeader.Length);
+  InsertTailList (&PackageList->GuidPkgHdr, &GuidPackage->GuidEntry);
+  *Package = GuidPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += PackageHeader.Length;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function exports GUID packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Guid Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportGuidPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  HII_GUID_PACKAGE_INSTANCE            *GuidPackage;
+  LIST_ENTRY                           *Link;
+  UINTN                                PackageLength;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+  EFI_STATUS                           Status;
+
+  if (PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  for (Link = PackageList->GuidPkgHdr.ForwardLink; Link != &PackageList->GuidPkgHdr; Link = Link->ForwardLink) {
+    GuidPackage = CR (Link, HII_GUID_PACKAGE_INSTANCE, GuidEntry, HII_GUID_PACKAGE_SIGNATURE);
+    CopyMem (&PackageHeader, GuidPackage->GuidPkg, sizeof (EFI_HII_PACKAGE_HEADER));
+    PackageLength += PackageHeader.Length;
+    if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (VOID *) GuidPackage,
+                 EFI_HII_PACKAGE_TYPE_GUID,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+      CopyMem (Buffer, GuidPackage->GuidPkg, PackageHeader.Length);
+      Buffer = (UINT8 *) Buffer + PackageHeader.Length;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes all GUID packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed GUID packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            GUID Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveGuidPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                           *ListHead;
+  HII_GUID_PACKAGE_INSTANCE            *Package;
+  EFI_STATUS                           Status;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+
+  ListHead = &PackageList->GuidPkgHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_GUID_PACKAGE_INSTANCE,
+                GuidEntry,
+                HII_GUID_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_TYPE_GUID,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->GuidEntry);
+    CopyMem (&PackageHeader, Package->GuidPkg, sizeof (EFI_HII_PACKAGE_HEADER));
+    PackageList->PackageListHdr.PackageLength -= PackageHeader.Length;
+    FreePool (Package->GuidPkg);
+    FreePool (Package);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a Form package to a package list node.
+  This is a internal function.
+
+  @param  PackageHdr             Pointer to a buffer stored with Form package
+                                 information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created Form package
+
+  @retval EFI_SUCCESS            Form Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Form package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertFormPackage (
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT    HII_IFR_PACKAGE_INSTANCE           **Package
+  )
+{
+  HII_IFR_PACKAGE_INSTANCE *FormPackage;
+  EFI_HII_PACKAGE_HEADER   PackageHeader;
+
+  if (PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get the length of the package, including package header itself
+  //
+  CopyMem (&PackageHeader, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  //
+  // Create a Form package node
+  //
+  FormPackage = (HII_IFR_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_IFR_PACKAGE_INSTANCE));
+  if (FormPackage == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  FormPackage->IfrData = (UINT8 *) AllocateZeroPool (PackageHeader.Length - sizeof (EFI_HII_PACKAGE_HEADER));
+  if (FormPackage->IfrData == NULL) {
+    FreePool (FormPackage);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  FormPackage->Signature = HII_IFR_PACKAGE_SIGNATURE;
+  //
+  // Copy Package Header
+  //
+  CopyMem (&FormPackage->FormPkgHdr, &PackageHeader, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  //
+  // Copy Ifr contents
+  //
+  CopyMem (
+    FormPackage->IfrData,
+    (UINT8 *) PackageHdr + sizeof (EFI_HII_PACKAGE_HEADER),
+    PackageHeader.Length - sizeof (EFI_HII_PACKAGE_HEADER)
+    );
+
+  InsertTailList (&PackageList->FormPkgHdr, &FormPackage->IfrEntry);
+  *Package = FormPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += FormPackage->FormPkgHdr.Length;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function exports Form packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Form Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportFormPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  HII_IFR_PACKAGE_INSTANCE *FormPackage;
+  UINTN                    PackageLength;
+  LIST_ENTRY               *Link;
+  EFI_STATUS               Status;
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  //
+  // Export Form packages.
+  //
+  for (Link = PackageList->FormPkgHdr.ForwardLink; Link != &PackageList->FormPkgHdr; Link = Link->ForwardLink) {
+    FormPackage = CR (Link, HII_IFR_PACKAGE_INSTANCE, IfrEntry, HII_IFR_PACKAGE_SIGNATURE);
+    PackageLength += FormPackage->FormPkgHdr.Length;
+    if ((Buffer != NULL) && (PackageLength + *ResultSize + UsedSize <= BufferSize)) {
+      //
+      // Invoke registered notification if exists
+      //
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (VOID *) FormPackage,
+                 EFI_HII_PACKAGE_FORMS,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+      //
+      // Copy the Form package content.
+      //
+      CopyMem (Buffer, (VOID *) (&FormPackage->FormPkgHdr), sizeof (EFI_HII_PACKAGE_HEADER));
+      Buffer = (UINT8 *) Buffer + sizeof (EFI_HII_PACKAGE_HEADER);
+      CopyMem (
+        Buffer,
+        (VOID *) FormPackage->IfrData,
+        FormPackage->FormPkgHdr.Length - sizeof (EFI_HII_PACKAGE_HEADER)
+        );
+      Buffer = (UINT8 *) Buffer + FormPackage->FormPkgHdr.Length - sizeof (EFI_HII_PACKAGE_HEADER);
+    }
+  }
+
+  *ResultSize += PackageLength;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  This function deletes all Form packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed Form packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            Form Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveFormPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                      *ListHead;
+  HII_IFR_PACKAGE_INSTANCE        *Package;
+  EFI_STATUS                      Status;
+
+  ListHead = &PackageList->FormPkgHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_IFR_PACKAGE_INSTANCE,
+                IfrEntry,
+                HII_IFR_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_FORMS,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->IfrEntry);
+    PackageList->PackageListHdr.PackageLength -= Package->FormPkgHdr.Length;
+    FreePool (Package->IfrData);
+    FreePool (Package);
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  This function insert a String package to a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  PackageHdr             Pointer to a buffer stored with String package
+                                 information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created String package
+
+  @retval EFI_SUCCESS            String Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 String package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+  @retval EFI_UNSUPPORTED        A string package with the same language already
+                                 exists in current package list.
+
+**/
+EFI_STATUS
+InsertStringPackage (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT    HII_STRING_PACKAGE_INSTANCE        **Package
+  )
+{
+  HII_STRING_PACKAGE_INSTANCE *StringPackage;
+  UINT32                      HeaderSize;
+  EFI_STATUS                  Status;
+  EFI_HII_PACKAGE_HEADER      PackageHeader;
+  CHAR8                       *Language;
+  UINT32                      LanguageSize;
+  LIST_ENTRY                  *Link;
+
+  if (Private == NULL || PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Private->Signature != HII_DATABASE_PRIVATE_DATA_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CopyMem (&PackageHeader, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+  CopyMem (&HeaderSize, (UINT8 *) PackageHdr + sizeof (EFI_HII_PACKAGE_HEADER), sizeof (UINT32));
+
+  //
+  // It is illegal to have two string packages with same language within one packagelist
+  // since the stringid will be duplicate if so. Check it to avoid this potential issue.
+  //
+  LanguageSize = HeaderSize - sizeof (EFI_HII_STRING_PACKAGE_HDR) + sizeof (CHAR8);
+  Language = (CHAR8 *) AllocateZeroPool (LanguageSize);
+  if (Language == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  AsciiStrCpyS (Language, LanguageSize / sizeof (CHAR8), (CHAR8 *) PackageHdr + HeaderSize - LanguageSize);
+  for (Link = PackageList->StringPkgHdr.ForwardLink; Link != &PackageList->StringPkgHdr; Link = Link->ForwardLink) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    if (HiiCompareLanguage (Language, StringPackage->StringPkgHdr->Language)) {
+      FreePool (Language);
+      return EFI_UNSUPPORTED;
+    }
+  }
+  FreePool (Language);
+
+  //
+  // Create a String package node
+  //
+  StringPackage = (HII_STRING_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_STRING_PACKAGE_INSTANCE));
+  if (StringPackage == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  StringPackage->StringPkgHdr = (EFI_HII_STRING_PACKAGE_HDR *) AllocateZeroPool (HeaderSize);
+  if (StringPackage->StringPkgHdr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  StringPackage->StringBlock = (UINT8 *) AllocateZeroPool (PackageHeader.Length - HeaderSize);
+  if (StringPackage->StringBlock == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  StringPackage->Signature = HII_STRING_PACKAGE_SIGNATURE;
+  StringPackage->FontId    = 0;
+  InitializeListHead (&StringPackage->FontInfoList);
+
+  //
+  // Copy the String package header.
+  //
+  CopyMem (StringPackage->StringPkgHdr, PackageHdr, HeaderSize);
+
+  //
+  // Copy the String blocks
+  //
+  CopyMem (
+    StringPackage->StringBlock,
+    (UINT8 *) PackageHdr + HeaderSize,
+    PackageHeader.Length - HeaderSize
+    );
+
+  //
+  // Collect all font block info
+  //
+  Status = FindStringBlock (Private, StringPackage, (EFI_STRING_ID) (-1), NULL, NULL, NULL, &StringPackage->MaxStringId, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Insert to String package array
+  //
+  InsertTailList (&PackageList->StringPkgHdr, &StringPackage->StringEntry);
+  *Package = StringPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += StringPackage->StringPkgHdr->Header.Length;
+  }
+
+  return EFI_SUCCESS;
+
+Error:
+
+  if (StringPackage != NULL) {
+    if (StringPackage->StringBlock != NULL) {
+      FreePool (StringPackage->StringBlock);
+    }
+    if (StringPackage->StringPkgHdr != NULL) {
+      FreePool (StringPackage->StringPkgHdr);
+    }
+    FreePool (StringPackage);
+  }
+  return Status;
+
+}
+
+/**
+ Adjust all string packages in a single package list to have the same max string ID.
+ 
+ @param  PackageList        Pointer to a package list which will be adjusted.
+
+ @retval EFI_SUCCESS  Adjust all string packages successfully.
+ @retval others       Can't adjust string packges.
+
+**/
+EFI_STATUS
+AdjustStringPackage (
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+)
+{
+  LIST_ENTRY                  *Link;
+  HII_STRING_PACKAGE_INSTANCE *StringPackage;
+  UINT32                      Skip2BlockSize;
+  UINT32                      OldBlockSize;
+  UINT8                       *StringBlock;
+  UINT8                       *BlockPtr;
+  EFI_STRING_ID               MaxStringId;
+  UINT16                      SkipCount;
+
+  MaxStringId = 0;
+  for (Link = PackageList->StringPkgHdr.ForwardLink;
+       Link != &PackageList->StringPkgHdr;
+       Link = Link->ForwardLink
+      ) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    if (MaxStringId < StringPackage->MaxStringId) {
+      MaxStringId = StringPackage->MaxStringId;
+    }
+  }
+
+  for (Link = PackageList->StringPkgHdr.ForwardLink;
+       Link != &PackageList->StringPkgHdr;
+       Link = Link->ForwardLink
+      ) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    if (StringPackage->MaxStringId < MaxStringId) {
+      OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+      //
+      // Create SKIP2 EFI_HII_SIBT_SKIP2_BLOCKs to reserve the missing string IDs.
+      //
+      SkipCount      = (UINT16) (MaxStringId - StringPackage->MaxStringId);
+      Skip2BlockSize = (UINT32) sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+
+      StringBlock = (UINT8 *) AllocateZeroPool (OldBlockSize + Skip2BlockSize);
+      if (StringBlock == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      //
+      // Copy original string blocks, except the EFI_HII_SIBT_END.
+      //
+      CopyMem (StringBlock, StringPackage->StringBlock, OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK));
+      //
+      // Create SKIP2 EFI_HII_SIBT_SKIP2_BLOCK blocks
+      //
+      BlockPtr  = StringBlock + OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK);
+      *BlockPtr = EFI_HII_SIBT_SKIP2;
+      CopyMem (BlockPtr + 1, &SkipCount, sizeof (UINT16));
+      BlockPtr  += sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+
+      //
+      // Append a EFI_HII_SIBT_END block to the end.
+      //
+      *BlockPtr = EFI_HII_SIBT_END;
+      FreePool (StringPackage->StringBlock);
+      StringPackage->StringBlock = StringBlock;
+      StringPackage->StringPkgHdr->Header.Length += Skip2BlockSize;
+      PackageList->PackageListHdr.PackageLength += Skip2BlockSize;
+      StringPackage->MaxStringId = MaxStringId;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function exports String packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            String Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportStringPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  LIST_ENTRY                  *Link;
+  UINTN                       PackageLength;
+  EFI_STATUS                  Status;
+  HII_STRING_PACKAGE_INSTANCE *StringPackage;
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  for (Link = PackageList->StringPkgHdr.ForwardLink; Link != &PackageList->StringPkgHdr; Link = Link->ForwardLink) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    PackageLength += StringPackage->StringPkgHdr->Header.Length;
+    if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+      //
+      // Invoke registered notification function with EXPORT_PACK notify type
+      //
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (VOID *) StringPackage,
+                 EFI_HII_PACKAGE_STRINGS,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+      //
+      // Copy String package header
+      //
+      CopyMem (Buffer, StringPackage->StringPkgHdr, StringPackage->StringPkgHdr->HdrSize);
+      Buffer = (UINT8 *) Buffer + StringPackage->StringPkgHdr->HdrSize;
+
+      //
+      // Copy String blocks information
+      //
+      CopyMem (
+        Buffer,
+        StringPackage->StringBlock,
+        StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize
+        );
+      Buffer = (UINT8 *) Buffer + StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes all String packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed String packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            String Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveStringPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                      *ListHead;
+  HII_STRING_PACKAGE_INSTANCE     *Package;
+  HII_FONT_INFO                   *FontInfo;
+  EFI_STATUS                      Status;
+
+  ListHead = &PackageList->StringPkgHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_STRING_PACKAGE_INSTANCE,
+                StringEntry,
+                HII_STRING_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_STRINGS,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->StringEntry);
+    PackageList->PackageListHdr.PackageLength -= Package->StringPkgHdr->Header.Length;
+    FreePool (Package->StringBlock);
+    FreePool (Package->StringPkgHdr);
+    //
+    // Delete font information
+    //
+    while (!IsListEmpty (&Package->FontInfoList)) {
+      FontInfo = CR (
+                   Package->FontInfoList.ForwardLink,
+                   HII_FONT_INFO,
+                   Entry,
+                   HII_FONT_INFO_SIGNATURE
+                   );
+      RemoveEntryList (&FontInfo->Entry);
+      FreePool (FontInfo);
+    }
+
+    FreePool (Package);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a Font package to a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  PackageHdr             Pointer to a buffer stored with Font package
+                                 information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created Font package
+
+  @retval EFI_SUCCESS            Font Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Font package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+  @retval EFI_UNSUPPORTED        A font package with same EFI_FONT_INFO already
+                                 exists in current hii database.
+
+**/
+EFI_STATUS
+InsertFontPackage (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT    HII_FONT_PACKAGE_INSTANCE          **Package
+  )
+{
+  HII_FONT_PACKAGE_INSTANCE *FontPackage;
+  EFI_HII_FONT_PACKAGE_HDR  *FontPkgHdr;
+  UINT32                    HeaderSize;
+  EFI_STATUS                Status;
+  EFI_HII_PACKAGE_HEADER    PackageHeader;
+  EFI_FONT_INFO             *FontInfo;
+  UINT32                    FontInfoSize;
+  HII_GLOBAL_FONT_INFO      *GlobalFont;
+
+  if (Private == NULL || PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CopyMem (&PackageHeader, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+  CopyMem (&HeaderSize, (UINT8 *) PackageHdr + sizeof (EFI_HII_PACKAGE_HEADER), sizeof (UINT32));
+
+  FontInfo    = NULL;
+  FontPackage = NULL;
+  GlobalFont  = NULL;
+
+  //
+  // It is illegal to have two font packages with same EFI_FONT_INFO within hii
+  // database. EFI_FONT_INFO (FontName, FontSize, FontStyle) describes font's
+  // attributes and identify a font uniquely.
+  //
+  FontPkgHdr = (EFI_HII_FONT_PACKAGE_HDR *) AllocateZeroPool (HeaderSize);
+  if (FontPkgHdr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  CopyMem (FontPkgHdr, PackageHdr, HeaderSize);
+
+  FontInfoSize = sizeof (EFI_FONT_INFO) + HeaderSize - sizeof (EFI_HII_FONT_PACKAGE_HDR);
+  FontInfo = (EFI_FONT_INFO *) AllocateZeroPool (FontInfoSize);
+  if (FontInfo == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  FontInfo->FontStyle = FontPkgHdr->FontStyle;
+  FontInfo->FontSize  = FontPkgHdr->Cell.Height;
+  StrCpyS (FontInfo->FontName, (FontInfoSize - OFFSET_OF(EFI_FONT_INFO,FontName)) / sizeof (CHAR16), FontPkgHdr->FontFamily);
+
+  if (IsFontInfoExisted (Private, FontInfo, NULL, NULL, NULL)) {
+    Status = EFI_UNSUPPORTED;
+    goto Error;
+  }
+
+  //
+  // Create a Font package node
+  //
+  FontPackage = (HII_FONT_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_FONT_PACKAGE_INSTANCE));
+  if (FontPackage == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  FontPackage->Signature  = HII_FONT_PACKAGE_SIGNATURE;
+  FontPackage->FontPkgHdr = FontPkgHdr;
+  InitializeListHead (&FontPackage->GlyphInfoList);
+
+  FontPackage->GlyphBlock = (UINT8 *) AllocateZeroPool (PackageHeader.Length - HeaderSize);
+  if (FontPackage->GlyphBlock == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  CopyMem (FontPackage->GlyphBlock, (UINT8 *) PackageHdr + HeaderSize, PackageHeader.Length - HeaderSize);
+
+  //
+  // Collect all default character cell information and backup in GlyphInfoList.
+  //
+  Status = FindGlyphBlock (FontPackage, (CHAR16) (-1), NULL, NULL, NULL);
+  if (EFI_ERROR (Status)) {
+    goto Error;
+  }
+
+  //
+  // This font package describes an unique EFI_FONT_INFO. Backup it in global
+  // font info list.
+  //
+  GlobalFont = (HII_GLOBAL_FONT_INFO *) AllocateZeroPool (sizeof (HII_GLOBAL_FONT_INFO));
+  if (GlobalFont == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  GlobalFont->Signature    = HII_GLOBAL_FONT_INFO_SIGNATURE;
+  GlobalFont->FontPackage  = FontPackage;
+  GlobalFont->FontInfoSize = FontInfoSize;
+  GlobalFont->FontInfo     = FontInfo;
+  InsertTailList (&Private->FontInfoList, &GlobalFont->Entry);
+
+  //
+  // Insert this font package to Font package array
+  //
+  InsertTailList (&PackageList->FontPkgHdr, &FontPackage->FontEntry);
+  *Package = FontPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += FontPackage->FontPkgHdr->Header.Length;
+  }
+
+  return EFI_SUCCESS;
+
+Error:
+
+  if (FontPkgHdr != NULL) {
+    FreePool (FontPkgHdr);
+  }
+  if (FontInfo != NULL) {
+    FreePool (FontInfo);
+  }
+  if (FontPackage != NULL) {
+    if (FontPackage->GlyphBlock != NULL) {
+      FreePool (FontPackage->GlyphBlock);
+    }
+    FreePool (FontPackage);
+  }
+  if (GlobalFont != NULL) {
+    FreePool (GlobalFont);
+  }
+
+  return Status;
+
+}
+
+
+/**
+  This function exports Font packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Font Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportFontPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  LIST_ENTRY                  *Link;
+  UINTN                       PackageLength;
+  EFI_STATUS                  Status;
+  HII_FONT_PACKAGE_INSTANCE   *Package;
+
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  for (Link = PackageList->FontPkgHdr.ForwardLink; Link != &PackageList->FontPkgHdr; Link = Link->ForwardLink) {
+    Package = CR (Link, HII_FONT_PACKAGE_INSTANCE, FontEntry, HII_FONT_PACKAGE_SIGNATURE);
+    PackageLength += Package->FontPkgHdr->Header.Length;
+    if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+      //
+      // Invoke registered notification function with EXPORT_PACK notify type
+      //
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (VOID *) Package,
+                 EFI_HII_PACKAGE_FONTS,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+      //
+      // Copy Font package header
+      //
+      CopyMem (Buffer, Package->FontPkgHdr, Package->FontPkgHdr->HdrSize);
+      Buffer = (UINT8 *) Buffer + Package->FontPkgHdr->HdrSize;
+
+      //
+      // Copy Glyph blocks information
+      //
+      CopyMem (
+        Buffer,
+        Package->GlyphBlock,
+        Package->FontPkgHdr->Header.Length - Package->FontPkgHdr->HdrSize
+        );
+      Buffer = (UINT8 *) Buffer + Package->FontPkgHdr->Header.Length - Package->FontPkgHdr->HdrSize;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes all Font packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed Font packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            Font Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveFontPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                      *ListHead;
+  HII_FONT_PACKAGE_INSTANCE       *Package;
+  EFI_STATUS                      Status;
+  HII_GLYPH_INFO                  *GlyphInfo;
+  LIST_ENTRY                      *Link;
+  HII_GLOBAL_FONT_INFO            *GlobalFont;
+
+  ListHead = &PackageList->FontPkgHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_FONT_PACKAGE_INSTANCE,
+                FontEntry,
+                HII_FONT_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_FONTS,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->FontEntry);
+    PackageList->PackageListHdr.PackageLength -= Package->FontPkgHdr->Header.Length;
+
+    if (Package->GlyphBlock != NULL) {
+      FreePool (Package->GlyphBlock);
+    }
+    FreePool (Package->FontPkgHdr);
+    //
+    // Delete default character cell information
+    //
+    while (!IsListEmpty (&Package->GlyphInfoList)) {
+      GlyphInfo = CR (
+                    Package->GlyphInfoList.ForwardLink,
+                    HII_GLYPH_INFO,
+                    Entry,
+                    HII_GLYPH_INFO_SIGNATURE
+                    );
+      RemoveEntryList (&GlyphInfo->Entry);
+      FreePool (GlyphInfo);
+    }
+
+    //
+    // Remove corresponding global font info
+    //
+    for (Link = Private->FontInfoList.ForwardLink; Link != &Private->FontInfoList; Link = Link->ForwardLink) {
+      GlobalFont = CR (Link, HII_GLOBAL_FONT_INFO, Entry, HII_GLOBAL_FONT_INFO_SIGNATURE);
+      if (GlobalFont->FontPackage == Package) {
+        RemoveEntryList (&GlobalFont->Entry);
+        FreePool (GlobalFont->FontInfo);
+        FreePool (GlobalFont);
+        break;
+      }
+    }
+
+    FreePool (Package);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a Image package to a package list node.
+  This is a internal function.
+
+  @param  PackageHdr             Pointer to a buffer stored with Image package
+                                 information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created Image package
+
+  @retval EFI_SUCCESS            Image Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Image package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertImagePackage (
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT    HII_IMAGE_PACKAGE_INSTANCE         **Package
+  )
+{
+  HII_IMAGE_PACKAGE_INSTANCE        *ImagePackage;
+  UINT32                            PaletteSize;
+  UINT32                            ImageSize;
+  UINT16                            Index;
+  EFI_HII_IMAGE_PALETTE_INFO_HEADER *PaletteHdr;
+  EFI_HII_IMAGE_PALETTE_INFO        *PaletteInfo;
+  UINT32                            PaletteInfoOffset;
+  UINT32                            ImageInfoOffset;
+  UINT16                            CurrentSize;
+
+  if (PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Less than one image package is allowed in one package list.
+  //
+  if (PackageList->ImagePkg != NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Create a Image package node
+  //
+  ImagePackage = (HII_IMAGE_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_IMAGE_PACKAGE_INSTANCE));
+  if (ImagePackage == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Copy the Image package header.
+  //
+  CopyMem (&ImagePackage->ImagePkgHdr, PackageHdr, sizeof (EFI_HII_IMAGE_PACKAGE_HDR));
+
+  PaletteInfoOffset = ImagePackage->ImagePkgHdr.PaletteInfoOffset;
+  ImageInfoOffset   = ImagePackage->ImagePkgHdr.ImageInfoOffset;
+
+  //
+  // If PaletteInfoOffset is zero, there are no palettes in this image package.
+  //
+  PaletteSize                = 0;
+  ImagePackage->PaletteBlock = NULL;
+  if (PaletteInfoOffset != 0) {
+    PaletteHdr  = (EFI_HII_IMAGE_PALETTE_INFO_HEADER *) ((UINT8 *) PackageHdr + PaletteInfoOffset);
+    PaletteSize = sizeof (EFI_HII_IMAGE_PALETTE_INFO_HEADER);
+    PaletteInfo = (EFI_HII_IMAGE_PALETTE_INFO *) ((UINT8 *) PaletteHdr + PaletteSize);
+
+    for (Index = 0; Index < PaletteHdr->PaletteCount; Index++) {
+      CopyMem (&CurrentSize, PaletteInfo, sizeof (UINT16));
+      CurrentSize += sizeof (UINT16);
+      PaletteSize += (UINT32) CurrentSize;
+      PaletteInfo = (EFI_HII_IMAGE_PALETTE_INFO *) ((UINT8 *) PaletteInfo + CurrentSize);
+    }
+
+    ImagePackage->PaletteBlock = (UINT8 *) AllocateZeroPool (PaletteSize);
+    if (ImagePackage->PaletteBlock == NULL) {
+      FreePool (ImagePackage);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyMem (
+      ImagePackage->PaletteBlock,
+      (UINT8 *) PackageHdr + PaletteInfoOffset,
+      PaletteSize
+      );
+  }
+
+  //
+  // If ImageInfoOffset is zero, there are no images in this package.
+  //
+  ImageSize                = 0;
+  ImagePackage->ImageBlock = NULL;
+  if (ImageInfoOffset != 0) {
+    ImageSize = ImagePackage->ImagePkgHdr.Header.Length -
+                sizeof (EFI_HII_IMAGE_PACKAGE_HDR) - PaletteSize;
+    ImagePackage->ImageBlock = (UINT8 *) AllocateZeroPool (ImageSize);
+    if (ImagePackage->ImageBlock == NULL) {
+      FreePool (ImagePackage->PaletteBlock);
+      FreePool (ImagePackage);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyMem (
+      ImagePackage->ImageBlock,
+      (UINT8 *) PackageHdr + ImageInfoOffset,
+      ImageSize
+      );
+  }
+
+  ImagePackage->ImageBlockSize  = ImageSize;
+  ImagePackage->PaletteInfoSize = PaletteSize;
+  PackageList->ImagePkg         = ImagePackage;
+  *Package                      = ImagePackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += ImagePackage->ImagePkgHdr.Header.Length;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function exports Image packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Image Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportImagePackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  UINTN                       PackageLength;
+  EFI_STATUS                  Status;
+  HII_IMAGE_PACKAGE_INSTANCE  *Package;
+
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Package = PackageList->ImagePkg;
+
+  if (Package == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  PackageLength = Package->ImagePkgHdr.Header.Length;
+
+  if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+    //
+    // Invoke registered notification function with EXPORT_PACK notify type
+    //
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_IMAGES,
+               Handle
+               );
+    ASSERT_EFI_ERROR (Status);
+    ASSERT (Package->ImagePkgHdr.Header.Length ==
+            sizeof (EFI_HII_IMAGE_PACKAGE_HDR) + Package->ImageBlockSize + Package->PaletteInfoSize);
+    //
+    // Copy Image package header,
+    // then justify the offset for image info and palette info in the header.
+    //
+    CopyMem (Buffer, &Package->ImagePkgHdr, sizeof (EFI_HII_IMAGE_PACKAGE_HDR));
+    Buffer = (UINT8 *) Buffer + sizeof (EFI_HII_IMAGE_PACKAGE_HDR);
+
+    //
+    // Copy Image blocks information
+    //
+    if (Package->ImageBlockSize != 0) {
+      CopyMem (Buffer, Package->ImageBlock, Package->ImageBlockSize);
+      Buffer = (UINT8 *) Buffer + Package->ImageBlockSize;
+    }
+    //
+    // Copy Palette information
+    //
+    if (Package->PaletteInfoSize != 0) {
+      CopyMem (Buffer, Package->PaletteBlock, Package->PaletteInfoSize);
+      Buffer = (UINT8 *) Buffer + Package->PaletteInfoSize;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes Image package from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed Image packages.
+  @param  PackageList            Package List which contains the to be  removed
+                                 Image package.
+
+  @retval EFI_SUCCESS            Image Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveImagePackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  HII_IMAGE_PACKAGE_INSTANCE      *Package;
+  EFI_STATUS                      Status;
+
+  Package = PackageList->ImagePkg;
+
+  //
+  // Image package does not exist, return directly.
+  //
+  if (Package == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = InvokeRegisteredFunction (
+             Private,
+             EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+             (VOID *) Package,
+             EFI_HII_PACKAGE_IMAGES,
+             Handle
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PackageList->PackageListHdr.PackageLength -= Package->ImagePkgHdr.Header.Length;
+
+  FreePool (Package->ImageBlock);
+  if (Package->PaletteBlock != NULL) {
+    FreePool (Package->PaletteBlock);
+  }
+  FreePool (Package);
+
+  PackageList->ImagePkg = NULL;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a Simple Font package to a package list node.
+  This is a internal function.
+
+  @param  PackageHdr             Pointer to a buffer stored with Simple Font
+                                 package information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created Simple Font package
+
+  @retval EFI_SUCCESS            Simple Font Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Simple Font package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertSimpleFontPackage (
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT HII_SIMPLE_FONT_PACKAGE_INSTANCE      **Package
+  )
+{
+  HII_SIMPLE_FONT_PACKAGE_INSTANCE *SimpleFontPackage;
+  EFI_STATUS                       Status;
+  EFI_HII_PACKAGE_HEADER           Header;
+
+  if (PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Create a Simple Font package node
+  //
+  SimpleFontPackage = AllocateZeroPool (sizeof (HII_SIMPLE_FONT_PACKAGE_INSTANCE));
+  if (SimpleFontPackage == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  SimpleFontPackage->Signature = HII_S_FONT_PACKAGE_SIGNATURE;
+
+  //
+  // Copy the Simple Font package.
+  //
+  CopyMem (&Header, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  SimpleFontPackage->SimpleFontPkgHdr = AllocateZeroPool (Header.Length);
+  if (SimpleFontPackage->SimpleFontPkgHdr == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  CopyMem (SimpleFontPackage->SimpleFontPkgHdr, PackageHdr, Header.Length);
+
+  //
+  // Insert to Simple Font package array
+  //
+  InsertTailList (&PackageList->SimpleFontPkgHdr, &SimpleFontPackage->SimpleFontEntry);
+  *Package = SimpleFontPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += Header.Length;
+  }
+
+  return EFI_SUCCESS;
+
+Error:
+
+  if (SimpleFontPackage != NULL) {
+    if (SimpleFontPackage->SimpleFontPkgHdr != NULL) {
+      FreePool (SimpleFontPackage->SimpleFontPkgHdr);
+    }
+    FreePool (SimpleFontPackage);
+  }
+  return Status;
+}
+
+
+/**
+  This function exports SimpleFont packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            SimpleFont Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportSimpleFontPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  LIST_ENTRY                        *Link;
+  UINTN                             PackageLength;
+  EFI_STATUS                        Status;
+  HII_SIMPLE_FONT_PACKAGE_INSTANCE  *Package;
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  for (Link = PackageList->SimpleFontPkgHdr.ForwardLink; Link != &PackageList->SimpleFontPkgHdr; Link = Link->ForwardLink) {
+    Package = CR (Link, HII_SIMPLE_FONT_PACKAGE_INSTANCE, SimpleFontEntry, HII_S_FONT_PACKAGE_SIGNATURE);
+    PackageLength += Package->SimpleFontPkgHdr->Header.Length;
+    if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+      //
+      // Invoke registered notification function with EXPORT_PACK notify type
+      //
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (VOID *) Package,
+                 EFI_HII_PACKAGE_SIMPLE_FONTS,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+
+      //
+      // Copy SimpleFont package
+      //
+      CopyMem (Buffer, Package->SimpleFontPkgHdr, Package->SimpleFontPkgHdr->Header.Length);
+      Buffer = (UINT8 *) Buffer + Package->SimpleFontPkgHdr->Header.Length;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes all Simple Font packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed Simple Font packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            Simple Font Package(s) is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveSimpleFontPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                       *ListHead;
+  HII_SIMPLE_FONT_PACKAGE_INSTANCE *Package;
+  EFI_STATUS                       Status;
+
+  ListHead = &PackageList->SimpleFontPkgHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_SIMPLE_FONT_PACKAGE_INSTANCE,
+                SimpleFontEntry,
+                HII_S_FONT_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_SIMPLE_FONTS,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->SimpleFontEntry);
+    PackageList->PackageListHdr.PackageLength -= Package->SimpleFontPkgHdr->Header.Length;
+    FreePool (Package->SimpleFontPkgHdr);
+    FreePool (Package);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function insert a Device path package to a package list node.
+  This is a internal function.
+
+  @param  DevicePath             Pointer to a EFI_DEVICE_PATH_PROTOCOL protocol
+                                 instance
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+
+  @retval EFI_SUCCESS            Device path Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Device path package.
+  @retval EFI_INVALID_PARAMETER  DevicePath is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertDevicePathPackage (
+  IN     EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  UINT32                           PackageLength;
+  EFI_HII_PACKAGE_HEADER           Header;
+
+  if (DevicePath == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Less than one device path package is allowed in one package list.
+  //
+  if (PackageList->DevicePathPkg != NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = (UINT32) GetDevicePathSize (DevicePath) + sizeof (EFI_HII_PACKAGE_HEADER);
+  PackageList->DevicePathPkg = (UINT8 *) AllocateZeroPool (PackageLength);
+  if (PackageList->DevicePathPkg == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Header.Length = PackageLength;
+  Header.Type   = EFI_HII_PACKAGE_DEVICE_PATH;
+  CopyMem (PackageList->DevicePathPkg, &Header, sizeof (EFI_HII_PACKAGE_HEADER));
+  CopyMem (
+    PackageList->DevicePathPkg + sizeof (EFI_HII_PACKAGE_HEADER),
+    DevicePath,
+    PackageLength - sizeof (EFI_HII_PACKAGE_HEADER)
+    );
+
+  //
+  // Since Device Path package is created by NewPackageList, either NEW_PACK
+  // or ADD_PACK should increase the length of package list.
+  //
+  PackageList->PackageListHdr.PackageLength += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function exports device path package to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Device path Package is exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportDevicePathPackage (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  EFI_STATUS                       Status;
+  UINT8                            *Package;
+  EFI_HII_PACKAGE_HEADER           Header;
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Package = PackageList->DevicePathPkg;
+
+  if (Package == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  CopyMem (&Header, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  if (Header.Length + *ResultSize + UsedSize <= BufferSize) {
+    //
+    // Invoke registered notification function with EXPORT_PACK notify type
+    //
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_DEVICE_PATH,
+               Handle
+               );
+    ASSERT_EFI_ERROR (Status);
+
+    //
+    // Copy Device path package
+    //
+    CopyMem (Buffer, Package, Header.Length);
+  }
+
+  *ResultSize += Header.Length;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes Device Path package from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list.
+  @param  PackageList            Package List which contains the to be  removed
+                                 Device Path package.
+
+  @retval EFI_SUCCESS            Device Path Package is deleted successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveDevicePathPackage (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  EFI_STATUS                       Status;
+  UINT8                            *Package;
+  EFI_HII_PACKAGE_HEADER           Header;
+
+  Package = PackageList->DevicePathPkg;
+
+  //
+  // No device path, return directly.
+  //
+  if (Package == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = InvokeRegisteredFunction (
+             Private,
+             EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+             (VOID *) Package,
+             EFI_HII_PACKAGE_DEVICE_PATH,
+             Handle
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CopyMem (&Header, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+  PackageList->PackageListHdr.PackageLength -= Header.Length;
+
+  FreePool (Package);
+
+  PackageList->DevicePathPkg = NULL;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function will insert a device path package to package list firstly then
+  invoke notification functions if any.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  NotifyType             The type of change concerning the database.
+  @param  DevicePath             Pointer to a EFI_DEVICE_PATH_PROTOCOL protocol
+                                 instance
+  @param  DatabaseRecord         Pointer to a database record contains  a package
+                                 list which will be inserted to.
+
+  @retval EFI_SUCCESS            Device path Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Device path package.
+  @retval EFI_INVALID_PARAMETER  DevicePath is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+AddDevicePathPackage (
+  IN HII_DATABASE_PRIVATE_DATA        *Private,
+  IN EFI_HII_DATABASE_NOTIFY_TYPE     NotifyType,
+  IN EFI_DEVICE_PATH_PROTOCOL         *DevicePath,
+  IN OUT HII_DATABASE_RECORD          *DatabaseRecord
+  )
+{
+  EFI_STATUS                          Status;
+
+  if (DevicePath == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (Private != NULL);
+  ASSERT (DatabaseRecord != NULL);
+
+  //
+  // Create a device path package and insert to packagelist
+  //
+  Status = InsertDevicePathPackage (
+               DevicePath,
+               NotifyType,
+               DatabaseRecord->PackageList
+               );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return InvokeRegisteredFunction (
+            Private,
+            NotifyType,
+            (VOID *) DatabaseRecord->PackageList->DevicePathPkg,
+            EFI_HII_PACKAGE_DEVICE_PATH,
+            DatabaseRecord->Handle
+            );
+}
+
+
+/**
+  This function insert a Keyboard Layout package to a package list node.
+  This is a internal function.
+
+  @param  PackageHdr             Pointer to a buffer stored with Keyboard Layout
+                                 package information.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list which will be inserted
+                                 to.
+  @param  Package                Created Keyboard Layout package
+
+  @retval EFI_SUCCESS            Keyboard Layout Package is inserted successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Keyboard Layout package.
+  @retval EFI_INVALID_PARAMETER  PackageHdr is NULL or PackageList is NULL.
+
+**/
+EFI_STATUS
+InsertKeyboardLayoutPackage (
+  IN     VOID                               *PackageHdr,
+  IN     EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  OUT HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE  **Package
+  )
+{
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *KeyboardLayoutPackage;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+  EFI_STATUS                           Status;
+
+  if (PackageHdr == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  CopyMem (&PackageHeader, PackageHdr, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  //
+  // Create a Keyboard Layout package node
+  //
+  KeyboardLayoutPackage = AllocateZeroPool (sizeof (HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE));
+  if (KeyboardLayoutPackage == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+  KeyboardLayoutPackage->Signature = HII_KB_LAYOUT_PACKAGE_SIGNATURE;
+
+  KeyboardLayoutPackage->KeyboardPkg = (UINT8 *) AllocateZeroPool (PackageHeader.Length);
+  if (KeyboardLayoutPackage->KeyboardPkg == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  CopyMem (KeyboardLayoutPackage->KeyboardPkg, PackageHdr, PackageHeader.Length);
+  InsertTailList (&PackageList->KeyboardLayoutHdr, &KeyboardLayoutPackage->KeyboardEntry);
+
+  *Package = KeyboardLayoutPackage;
+
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    PackageList->PackageListHdr.PackageLength += PackageHeader.Length;
+  }
+
+  return EFI_SUCCESS;
+
+Error:
+
+
+  if (KeyboardLayoutPackage != NULL) {
+    if (KeyboardLayoutPackage->KeyboardPkg != NULL) {
+      FreePool (KeyboardLayoutPackage->KeyboardPkg);
+    }
+    FreePool (KeyboardLayoutPackage);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function exports Keyboard Layout packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Keyboard Layout Packages are exported
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportKeyboardLayoutPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  )
+{
+  LIST_ENTRY                           *Link;
+  UINTN                                PackageLength;
+  EFI_STATUS                           Status;
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *Package;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+
+  if (Private == NULL || PackageList == NULL || ResultSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PackageLength = 0;
+  Status        = EFI_SUCCESS;
+
+  for (Link = PackageList->KeyboardLayoutHdr.ForwardLink; Link != &PackageList->KeyboardLayoutHdr; Link = Link->ForwardLink) {
+    Package = CR (Link, HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE, KeyboardEntry, HII_KB_LAYOUT_PACKAGE_SIGNATURE);
+    CopyMem (&PackageHeader, Package->KeyboardPkg, sizeof (EFI_HII_PACKAGE_HEADER));
+    PackageLength += PackageHeader.Length;
+    if (PackageLength + *ResultSize + UsedSize <= BufferSize) {
+      //
+      // Invoke registered notification function with EXPORT_PACK notify type
+      //
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 EFI_HII_DATABASE_NOTIFY_EXPORT_PACK,
+                 (EFI_HII_PACKAGE_HEADER *) Package,
+                 EFI_HII_PACKAGE_KEYBOARD_LAYOUT,
+                 Handle
+                 );
+      ASSERT_EFI_ERROR (Status);
+
+      //
+      // Copy Keyboard Layout package
+      //
+      CopyMem (Buffer, Package->KeyboardPkg, PackageHeader.Length);
+      Buffer = (UINT8 *) Buffer + PackageHeader.Length;
+    }
+  }
+
+  *ResultSize += PackageLength;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function deletes all Keyboard Layout packages from a package list node.
+  This is a internal function.
+
+  @param  Private                Hii database private data.
+  @param  Handle                 Handle of the package list which contains the to
+                                 be  removed Keyboard Layout packages.
+  @param  PackageList            Pointer to a package list that contains removing
+                                 packages.
+
+  @retval EFI_SUCCESS            Keyboard Layout Package(s) is deleted
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+
+**/
+EFI_STATUS
+RemoveKeyboardLayoutPackages (
+  IN     HII_DATABASE_PRIVATE_DATA          *Private,
+  IN     EFI_HII_HANDLE                     Handle,
+  IN OUT HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList
+  )
+{
+  LIST_ENTRY                           *ListHead;
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *Package;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+  EFI_STATUS                           Status;
+
+  ListHead = &PackageList->KeyboardLayoutHdr;
+
+  while (!IsListEmpty (ListHead)) {
+    Package = CR (
+                ListHead->ForwardLink,
+                HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE,
+                KeyboardEntry,
+                HII_KB_LAYOUT_PACKAGE_SIGNATURE
+                );
+    Status = InvokeRegisteredFunction (
+               Private,
+               EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+               (VOID *) Package,
+               EFI_HII_PACKAGE_KEYBOARD_LAYOUT,
+               Handle
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    RemoveEntryList (&Package->KeyboardEntry);
+    CopyMem (&PackageHeader, Package->KeyboardPkg, sizeof (EFI_HII_PACKAGE_HEADER));
+    PackageList->PackageListHdr.PackageLength -= PackageHeader.Length;
+    FreePool (Package->KeyboardPkg);
+    FreePool (Package);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function will insert a package list to hii database firstly then
+  invoke notification functions if any. It is the worker function of
+  HiiNewPackageList and HiiUpdatePackageList.
+
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  NotifyType             The type of change concerning the database.
+  @param  PackageList            Pointer to a package list.
+  @param  DatabaseRecord         Pointer to a database record contains  a package
+                                 list instance which will be inserted to.
+
+  @retval EFI_SUCCESS            All incoming packages are inserted to current
+                                 database.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 Device path package.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+AddPackages (
+  IN HII_DATABASE_PRIVATE_DATA         *Private,
+  IN EFI_HII_DATABASE_NOTIFY_TYPE      NotifyType,
+  IN CONST EFI_HII_PACKAGE_LIST_HEADER *PackageList,
+  IN OUT   HII_DATABASE_RECORD         *DatabaseRecord
+  )
+{
+  EFI_STATUS                           Status;
+  HII_GUID_PACKAGE_INSTANCE            *GuidPackage;
+  HII_IFR_PACKAGE_INSTANCE             *FormPackage;
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *KeyboardLayoutPackage;
+  HII_STRING_PACKAGE_INSTANCE          *StringPackage;
+  HII_FONT_PACKAGE_INSTANCE            *FontPackage;
+  HII_SIMPLE_FONT_PACKAGE_INSTANCE     *SimpleFontPackage;
+  HII_IMAGE_PACKAGE_INSTANCE           *ImagePackage;
+  EFI_HII_PACKAGE_HEADER               *PackageHdrPtr;
+  EFI_HII_PACKAGE_HEADER               PackageHeader;
+  UINT32                               OldPackageListLen;
+  BOOLEAN                              StringPkgIsAdd;
+
+  //
+  // Initialize Variables
+  //
+  StringPkgIsAdd        = FALSE;
+  FontPackage           = NULL;
+  StringPackage         = NULL;
+  GuidPackage           = NULL;
+  FormPackage           = NULL;
+  ImagePackage          = NULL;
+  SimpleFontPackage     = NULL;
+  KeyboardLayoutPackage = NULL;
+
+  //
+  // Process the package list header
+  //
+  OldPackageListLen = DatabaseRecord->PackageList->PackageListHdr.PackageLength;
+  CopyMem (
+    &DatabaseRecord->PackageList->PackageListHdr,
+    (VOID *) PackageList,
+    sizeof (EFI_HII_PACKAGE_LIST_HEADER)
+    );
+  if (NotifyType == EFI_HII_DATABASE_NOTIFY_ADD_PACK) {
+    DatabaseRecord->PackageList->PackageListHdr.PackageLength = OldPackageListLen;
+  }
+
+  PackageHdrPtr = (EFI_HII_PACKAGE_HEADER *) ((UINT8 *) PackageList + sizeof (EFI_HII_PACKAGE_LIST_HEADER));
+  CopyMem (&PackageHeader, PackageHdrPtr, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  Status = EFI_SUCCESS;
+
+  while (PackageHeader.Type != EFI_HII_PACKAGE_END) {
+    switch (PackageHeader.Type) {
+    case EFI_HII_PACKAGE_TYPE_GUID:
+      Status = InsertGuidPackage (
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &GuidPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) GuidPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_FORMS:
+      Status = InsertFormPackage (
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &FormPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) FormPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_KEYBOARD_LAYOUT:
+      Status = InsertKeyboardLayoutPackage (
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &KeyboardLayoutPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) KeyboardLayoutPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_STRINGS:
+      Status = InsertStringPackage (
+                 Private,
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &StringPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      ASSERT (StringPackage != NULL);
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) StringPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      StringPkgIsAdd = TRUE;
+      break;
+    case EFI_HII_PACKAGE_FONTS:
+      Status = InsertFontPackage (
+                 Private,
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &FontPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) FontPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_IMAGES:
+      Status = InsertImagePackage (
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &ImagePackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) ImagePackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_SIMPLE_FONTS:
+      Status = InsertSimpleFontPackage (
+                 PackageHdrPtr,
+                 NotifyType,
+                 DatabaseRecord->PackageList,
+                 &SimpleFontPackage
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = InvokeRegisteredFunction (
+                 Private,
+                 NotifyType,
+                 (VOID *) SimpleFontPackage,
+                 (UINT8) (PackageHeader.Type),
+                 DatabaseRecord->Handle
+                 );
+      break;
+    case EFI_HII_PACKAGE_DEVICE_PATH:
+      Status = AddDevicePathPackage (
+                 Private,
+                 NotifyType,
+                 (EFI_DEVICE_PATH_PROTOCOL *) ((UINT8 *) PackageHdrPtr + sizeof (EFI_HII_PACKAGE_HEADER)),
+                 DatabaseRecord
+                 );
+      break;
+    default:
+      break;
+    }
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // goto header of next package
+    //
+    PackageHdrPtr = (EFI_HII_PACKAGE_HEADER *) ((UINT8 *) PackageHdrPtr + PackageHeader.Length);
+    CopyMem (&PackageHeader, PackageHdrPtr, sizeof (EFI_HII_PACKAGE_HEADER));
+  }
+  
+  //
+  // Adjust String Package to make sure all string packages have the same max string ID.
+  //
+  if (!EFI_ERROR (Status) && StringPkgIsAdd) {
+    Status = AdjustStringPackage (DatabaseRecord->PackageList);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function exports a package list to a buffer. It is the worker function
+  of HiiExportPackageList.
+
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer has been used by exporting
+                                 package lists when Handle is NULL.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+
+  @retval EFI_SUCCESS            Keyboard Layout Packages are exported
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportPackageList (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN OUT UINTN                          *UsedSize,
+  IN UINTN                              BufferSize,
+  OUT EFI_HII_PACKAGE_LIST_HEADER       *Buffer
+  )
+{
+  EFI_STATUS                          Status;
+  UINTN                               ResultSize;
+  EFI_HII_PACKAGE_HEADER              EndofPackageList;
+
+  ASSERT (Private != NULL && PackageList != NULL && UsedSize != NULL);
+  ASSERT (Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+  ASSERT (IsHiiHandleValid (Handle));
+
+  if (BufferSize > 0 && Buffer == NULL ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Copy the package list header
+  // ResultSize indicates the length of the exported bytes of this package list
+  //
+  ResultSize = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  if (ResultSize + *UsedSize <= BufferSize) {
+    CopyMem ((VOID *) Buffer, PackageList, ResultSize);
+  }
+  //
+  // Copy the packages and invoke EXPORT_PACK notify functions if exists.
+  //
+  Status = ExportGuidPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportFormPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportKeyboardLayoutPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportStringPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportFontPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportImagePackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportSimpleFontPackages (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  Status = ExportDevicePathPackage (
+             Private,
+             Handle,
+             PackageList,
+             *UsedSize,
+             BufferSize,
+             (VOID *) ((UINT8 *) Buffer + ResultSize),
+             &ResultSize
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Append the package list end.
+  //
+  EndofPackageList.Length = sizeof (EFI_HII_PACKAGE_HEADER);
+  EndofPackageList.Type   = EFI_HII_PACKAGE_END;
+  if (ResultSize + *UsedSize + sizeof (EFI_HII_PACKAGE_HEADER) <= BufferSize) {
+    CopyMem (
+      (VOID *) ((UINT8 *) Buffer + ResultSize),
+      (VOID *) &EndofPackageList,
+      sizeof (EFI_HII_PACKAGE_HEADER)
+      );
+  }
+
+  *UsedSize += ResultSize + sizeof (EFI_HII_PACKAGE_HEADER);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function adds the packages in the package list to the database and returns a handle. If there is a
+  EFI_DEVICE_PATH_PROTOCOL associated with the DriverHandle, then this function will
+  create a package of type EFI_PACKAGE_TYPE_DEVICE_PATH and add it to the package list.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  PackageList            A pointer to an EFI_HII_PACKAGE_LIST_HEADER
+                                 structure.
+  @param  DriverHandle           Associate the package list with this EFI handle.
+                                 If a NULL is specified, this data will not be associate
+                                 with any drivers and cannot have a callback induced.
+  @param  Handle                 A pointer to the EFI_HII_HANDLE instance.
+
+  @retval EFI_SUCCESS            The package list associated with the Handle was
+                                 added to the HII database.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary resources for the new
+                                 database contents.
+  @retval EFI_INVALID_PARAMETER  PackageList is NULL or Handle is NULL.
+  @retval EFI_INVALID_PARAMETER  PackageListGuid already exists in database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewPackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN CONST EFI_HII_PACKAGE_LIST_HEADER  *PackageList,
+  IN CONST EFI_HANDLE                   DriverHandle, OPTIONAL
+  OUT EFI_HII_HANDLE                    *Handle
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  EFI_DEVICE_PATH_PROTOCOL            *DevicePath;
+  LIST_ENTRY                          *Link;
+  EFI_GUID                            PackageListGuid;
+
+  if (This == NULL || PackageList == NULL || Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  CopyMem (&PackageListGuid, (VOID *) PackageList, sizeof (EFI_GUID));
+
+  //
+  // Check the Package list GUID to guarantee this GUID is unique in database.
+  //
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (CompareGuid (
+          &(DatabaseRecord->PackageList->PackageListHdr.PackageListGuid),
+          &PackageListGuid) && 
+        DatabaseRecord->DriverHandle == DriverHandle) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Build a PackageList node
+  //
+  Status = GenerateHiiDatabaseRecord (Private, &DatabaseRecord);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Fill in information of the created Package List node
+  // according to incoming package list.
+  //
+  Status = AddPackages (Private, EFI_HII_DATABASE_NOTIFY_NEW_PACK, PackageList, DatabaseRecord);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DatabaseRecord->DriverHandle = DriverHandle;
+
+  //
+  // Create a Device path package and add into the package list if exists.
+  //
+  Status = gBS->HandleProtocol (
+                  DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = AddDevicePathPackage (Private, EFI_HII_DATABASE_NOTIFY_NEW_PACK, DevicePath, DatabaseRecord);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  *Handle = DatabaseRecord->Handle;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function removes the package list that is associated with a handle Handle
+  from the HII database. Before removing the package, any registered functions
+  with the notification type REMOVE_PACK and the same package type will be called.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  Handle                 The handle that was registered to the data that is
+                                 requested  for removal.
+
+  @retval EFI_SUCCESS            The data associated with the Handle was removed
+                                 from  the HII database.
+  @retval EFI_NOT_FOUND          The specified andle is not in database.
+  @retval EFI_INVALID_PARAMETER  The Handle was not valid.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiRemovePackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                     Handle
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *Node;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageList;
+  HII_HANDLE                          *HiiHandle;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (Handle)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Get the packagelist to be removed.
+  //
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (Node->Handle == Handle) {
+      PackageList = (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (Node->PackageList);
+      ASSERT (PackageList != NULL);
+
+      //
+      // Call registered functions with REMOVE_PACK before removing packages
+      // then remove them.
+      //
+      Status = RemoveGuidPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveFormPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveKeyboardLayoutPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveStringPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveFontPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveImagePackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveSimpleFontPackages (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      Status = RemoveDevicePathPackage (Private, Handle, PackageList);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      //
+      // Free resources of the package list
+      //
+      RemoveEntryList (&Node->DatabaseEntry);
+
+      HiiHandle = (HII_HANDLE *) Handle;
+      RemoveEntryList (&HiiHandle->Handle);
+      Private->HiiHandleCount--;
+      ASSERT (Private->HiiHandleCount >= 0);
+
+      HiiHandle->Signature = 0;
+      FreePool (HiiHandle);
+      FreePool (Node->PackageList);
+      FreePool (Node);
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function updates the existing package list (which has the specified Handle)
+  in the HII databases, using the new package list specified by PackageList.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  Handle                 The handle that was registered to the data that is
+                                  requested to be updated.
+  @param  PackageList            A pointer to an EFI_HII_PACKAGE_LIST_HEADER
+                                 package.
+
+  @retval EFI_SUCCESS            The HII database was successfully updated.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate enough memory for the updated
+                                 database.
+  @retval EFI_INVALID_PARAMETER  PackageList was NULL.
+  @retval EFI_NOT_FOUND          The specified Handle is not in database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUpdatePackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                     Handle,
+  IN CONST EFI_HII_PACKAGE_LIST_HEADER  *PackageList
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *Node;
+  EFI_HII_PACKAGE_HEADER              *PackageHdrPtr;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *OldPackageList;
+  EFI_HII_PACKAGE_HEADER              PackageHeader;
+
+  if (This == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (Handle)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  PackageHdrPtr = (EFI_HII_PACKAGE_HEADER *) ((UINT8 *) PackageList + sizeof (EFI_HII_PACKAGE_LIST_HEADER));
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Get original packagelist to be updated
+  //
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (Node->Handle == Handle) {
+      OldPackageList = Node->PackageList;
+      //
+      // Remove the package if its type matches one of the package types which is
+      // contained in the new package list.
+      //
+      CopyMem (&PackageHeader, PackageHdrPtr, sizeof (EFI_HII_PACKAGE_HEADER));
+      while (PackageHeader.Type != EFI_HII_PACKAGE_END) {
+        switch (PackageHeader.Type) {
+        case EFI_HII_PACKAGE_TYPE_GUID:
+          Status = RemoveGuidPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_FORMS:
+          Status = RemoveFormPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_KEYBOARD_LAYOUT:
+          Status = RemoveKeyboardLayoutPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_STRINGS:
+          Status = RemoveStringPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_FONTS:
+          Status = RemoveFontPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_IMAGES:
+          Status = RemoveImagePackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_SIMPLE_FONTS:
+          Status = RemoveSimpleFontPackages (Private, Handle, OldPackageList);
+          break;
+        case EFI_HII_PACKAGE_DEVICE_PATH:
+          Status = RemoveDevicePathPackage (Private, Handle, OldPackageList);
+          break;
+        }
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        PackageHdrPtr = (EFI_HII_PACKAGE_HEADER *) ((UINT8 *) PackageHdrPtr + PackageHeader.Length);
+        CopyMem (&PackageHeader, PackageHdrPtr, sizeof (EFI_HII_PACKAGE_HEADER));
+      }
+
+      //
+      // Add all of the packages within the new package list
+      //
+      return AddPackages (Private, EFI_HII_DATABASE_NOTIFY_ADD_PACK, PackageList, Node);
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function returns a list of the package handles of the specified type
+  that are currently active in the database. The pseudo-type
+  EFI_HII_PACKAGE_TYPE_ALL will cause all package handles to be listed.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  PackageType            Specifies the package type of the packages to list
+                                 or EFI_HII_PACKAGE_TYPE_ALL for all packages to be
+                                 listed.
+  @param  PackageGuid            If PackageType is EFI_HII_PACKAGE_TYPE_GUID, then
+                                 this  is the pointer to the GUID which must match
+                                 the Guid field of EFI_HII_GUID_PACKAGE_GUID_HDR.
+                                 Otherwise,  it must be NULL.
+  @param  HandleBufferLength     On input, a pointer to the length of the handle
+                                 buffer.  On output, the length of the handle
+                                 buffer that is required for the handles found.
+  @param  Handle                 An array of EFI_HII_HANDLE instances returned.
+
+  @retval EFI_SUCCESS            The matching handles are outputed successfully.
+                                 HandleBufferLength is updated with the actual length.
+  @retval EFI_BUFFER_TO_SMALL    The HandleBufferLength parameter indicates that
+                                 Handle is too small to support the number of
+                                 handles. HandleBufferLength is updated with a
+                                 value that will  enable the data to fit.
+  @retval EFI_NOT_FOUND          No matching handle could not be found in database.
+  @retval EFI_INVALID_PARAMETER  HandleBufferLength was NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by HandleBufferLength was not
+                                 zero and Handle was NULL.
+  @retval EFI_INVALID_PARAMETER  PackageType is not a EFI_HII_PACKAGE_TYPE_GUID but
+                                 PackageGuid is not NULL, PackageType is a EFI_HII_
+                                 PACKAGE_TYPE_GUID but PackageGuid is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiListPackageLists (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  UINT8                             PackageType,
+  IN  CONST EFI_GUID                    *PackageGuid,
+  IN  OUT UINTN                         *HandleBufferLength,
+  OUT EFI_HII_HANDLE                    *Handle
+  )
+{
+  HII_GUID_PACKAGE_INSTANCE           *GuidPackage;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *Node;
+  LIST_ENTRY                          *Link;
+  BOOLEAN                             Matched;
+  HII_HANDLE                          **Result;
+  UINTN                               ResultSize;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageList;
+  LIST_ENTRY                          *Link1;
+
+  //
+  // Check input parameters
+  //
+  if (This == NULL || HandleBufferLength == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*HandleBufferLength > 0 && Handle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((PackageType == EFI_HII_PACKAGE_TYPE_GUID && PackageGuid == NULL) ||
+      (PackageType != EFI_HII_PACKAGE_TYPE_GUID && PackageGuid != NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private    = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  Matched    = FALSE;
+  Result     = (HII_HANDLE **) Handle;
+  ResultSize = 0;
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    PackageList = (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (Node->PackageList);
+    switch (PackageType) {
+      case EFI_HII_PACKAGE_TYPE_GUID:
+        for (Link1 = PackageList->GuidPkgHdr.ForwardLink; Link1 != &PackageList->GuidPkgHdr; Link1 = Link1->ForwardLink) {
+          GuidPackage = CR (Link1, HII_GUID_PACKAGE_INSTANCE, GuidEntry, HII_GUID_PACKAGE_SIGNATURE);
+          if (CompareGuid (
+                (EFI_GUID *) PackageGuid,
+                (EFI_GUID *) (GuidPackage->GuidPkg + sizeof (EFI_HII_PACKAGE_HEADER))
+                )) {
+            Matched = TRUE;
+            break;
+          }
+        }
+        break;
+      case EFI_HII_PACKAGE_FORMS:
+        if (!IsListEmpty (&PackageList->FormPkgHdr)) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_KEYBOARD_LAYOUT:
+        if (!IsListEmpty (&PackageList->KeyboardLayoutHdr)) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_STRINGS:
+        if (!IsListEmpty (&PackageList->StringPkgHdr)) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_FONTS:
+        if (!IsListEmpty (&PackageList->FontPkgHdr)) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_IMAGES:
+        if (PackageList->ImagePkg != NULL) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_SIMPLE_FONTS:
+        if (!IsListEmpty (&PackageList->SimpleFontPkgHdr)) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_HII_PACKAGE_DEVICE_PATH:
+        if (PackageList->DevicePathPkg != NULL) {
+          Matched = TRUE;
+        }
+        break;
+        //
+        // Pesudo-type EFI_HII_PACKAGE_TYPE_ALL will cause all package handles
+        // to be listed.
+        //
+      case EFI_HII_PACKAGE_TYPE_ALL:
+        Matched = TRUE;
+        break;
+      default:
+        break;
+    }
+
+    //
+    // This active package list has the specified package type, list it.
+    //
+    if (Matched) {
+      ResultSize += sizeof (EFI_HII_HANDLE);
+      if (ResultSize <= *HandleBufferLength) {
+        *Result++ = Node->Handle;
+      }
+    }
+    Matched = FALSE;
+  }
+
+  if (ResultSize == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (*HandleBufferLength < ResultSize) {
+    *HandleBufferLength = ResultSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *HandleBufferLength = ResultSize;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function will export one or all package lists in the database to a buffer.
+  For each package list exported, this function will call functions registered
+  with EXPORT_PACK and then copy the package list to the buffer.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  Handle                 An EFI_HII_HANDLE that corresponds to the desired
+                                 package list in the HII database to export or NULL
+                                 to indicate  all package lists should be exported.
+  @param  BufferSize             On input, a pointer to the length of the buffer.
+                                 On output, the length of the buffer that is
+                                 required for the exported data.
+  @param  Buffer                 A pointer to a buffer that will contain the
+                                 results of  the export function.
+
+  @retval EFI_SUCCESS            Package exported.
+  @retval EFI_BUFFER_TO_SMALL    The HandleBufferLength parameter indicates that
+                                 Handle is too small to support the number of
+                                 handles.      HandleBufferLength is updated with a
+                                 value that will enable the data to fit.
+  @retval EFI_NOT_FOUND          The specifiecd Handle could not be found in the
+                                 current database.
+  @retval EFI_INVALID_PARAMETER  BufferSize was NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by BufferSize was not zero 
+                                 and Buffer was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiExportPackageLists (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                    Handle,
+  IN  OUT UINTN                         *BufferSize,
+  OUT EFI_HII_PACKAGE_LIST_HEADER       *Buffer
+  )
+{
+  LIST_ENTRY                          *Link;
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *Node;
+  UINTN                               UsedSize;
+
+  if (This == NULL || BufferSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*BufferSize > 0 && Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((Handle != NULL) && (!IsHiiHandleValid (Handle))) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private  = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  UsedSize = 0;
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (Handle == NULL) {
+      //
+      // Export all package lists in current hii database.
+      //
+      Status = ExportPackageList (
+                 Private,
+                 Node->Handle,
+                 (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (Node->PackageList),
+                 &UsedSize,
+                 *BufferSize,
+                 (EFI_HII_PACKAGE_LIST_HEADER *)((UINT8 *) Buffer + UsedSize)
+                 );
+      ASSERT_EFI_ERROR (Status);
+    } else if (Handle != NULL && Node->Handle == Handle) {
+      Status = ExportPackageList (
+                 Private,
+                 Handle,
+                 (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (Node->PackageList),
+                 &UsedSize,
+                 *BufferSize,
+                 Buffer
+                 );
+      ASSERT_EFI_ERROR (Status);
+      if (*BufferSize < UsedSize) {
+        *BufferSize = UsedSize;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+      return EFI_SUCCESS;
+    }
+  }
+
+  if (Handle == NULL && UsedSize != 0) {
+    if (*BufferSize < UsedSize) {
+      *BufferSize = UsedSize;
+      return EFI_BUFFER_TOO_SMALL;
+    }
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function registers a function which will be called when specified actions related to packages of
+  the specified type occur in the HII database. By registering a function, other HII-related drivers are
+  notified when specific package types are added, removed or updated in the HII database.
+  Each driver or application which registers a notification should use
+  EFI_HII_DATABASE_PROTOCOL.UnregisterPackageNotify() before exiting.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  PackageType            Specifies the package type of the packages to list
+                                 or EFI_HII_PACKAGE_TYPE_ALL for all packages to be
+                                 listed.
+  @param  PackageGuid            If PackageType is EFI_HII_PACKAGE_TYPE_GUID, then
+                                 this is the pointer to the GUID which must match
+                                 the Guid field of
+                                 EFI_HII_GUID_PACKAGE_GUID_HDR. Otherwise, it must
+                                 be NULL.
+  @param  PackageNotifyFn        Points to the function to be called when the event
+                                 specified by
+                                 NotificationType occurs.
+  @param  NotifyType             Describes the types of notification which this
+                                 function will be receiving.
+  @param  NotifyHandle           Points to the unique handle assigned to the
+                                 registered notification. Can be used in
+                                 EFI_HII_DATABASE_PROTOCOL.UnregisterPackageNotify()
+                                 to stop notifications.
+
+  @retval EFI_SUCCESS            Notification registered successfully.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate necessary data structures
+  @retval EFI_INVALID_PARAMETER  NotifyHandle is NULL.
+  @retval EFI_INVALID_PARAMETER  PackageGuid is not NULL when PackageType is not
+                                 EFI_HII_PACKAGE_TYPE_GUID.
+  @retval EFI_INVALID_PARAMETER  PackageGuid is NULL when PackageType is
+                                 EFI_HII_PACKAGE_TYPE_GUID.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiRegisterPackageNotify (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  UINT8                             PackageType,
+  IN  CONST EFI_GUID                    *PackageGuid,
+  IN  CONST EFI_HII_DATABASE_NOTIFY     PackageNotifyFn,
+  IN  EFI_HII_DATABASE_NOTIFY_TYPE      NotifyType,
+  OUT EFI_HANDLE                        *NotifyHandle
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_NOTIFY                 *Notify;
+  EFI_STATUS                          Status;
+
+  if (This == NULL || NotifyHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((PackageType == EFI_HII_PACKAGE_TYPE_GUID && PackageGuid == NULL) ||
+      (PackageType != EFI_HII_PACKAGE_TYPE_GUID && PackageGuid != NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Allocate a notification node
+  //
+  Notify = (HII_DATABASE_NOTIFY *) AllocateZeroPool (sizeof (HII_DATABASE_NOTIFY));
+  if (Notify == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Generate a notify handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Notify->NotifyHandle,
+                  &gEfiCallerIdGuid,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Fill in the information to the notification node
+  //
+  Notify->Signature       = HII_DATABASE_NOTIFY_SIGNATURE;
+  Notify->PackageType     = PackageType;
+  Notify->PackageGuid     = (EFI_GUID *) PackageGuid;
+  Notify->PackageNotifyFn = (EFI_HII_DATABASE_NOTIFY) PackageNotifyFn;
+  Notify->NotifyType      = NotifyType;
+
+  InsertTailList (&Private->DatabaseNotifyList, &Notify->DatabaseNotifyEntry);
+  *NotifyHandle = Notify->NotifyHandle;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Removes the specified HII database package-related notification.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  NotificationHandle     The handle of the notification function being
+                                 unregistered.
+
+  @retval EFI_SUCCESS            Notification is unregistered successfully.
+  @retval EFI_INVALID_PARAMETER  The Handle is invalid.
+  @retval EFI_NOT_FOUND          The incoming notification handle does not exist
+                                 in current hii database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUnregisterPackageNotify (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HANDLE                         NotificationHandle
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_NOTIFY                 *Notify;
+  LIST_ENTRY                          *Link;
+  EFI_STATUS                          Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NotificationHandle == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status = gBS->OpenProtocol (
+                  NotificationHandle,
+                  &gEfiCallerIdGuid,
+                  NULL,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  for (Link = Private->DatabaseNotifyList.ForwardLink; Link != &Private->DatabaseNotifyList; Link = Link->ForwardLink) {
+    Notify = CR (Link, HII_DATABASE_NOTIFY, DatabaseNotifyEntry, HII_DATABASE_NOTIFY_SIGNATURE);
+    if (Notify->NotifyHandle == NotificationHandle) {
+      //
+      // Remove the matching notification node
+      //
+      RemoveEntryList (&Notify->DatabaseNotifyEntry);
+      Status = gBS->UninstallMultipleProtocolInterfaces (
+                      Notify->NotifyHandle,
+                      &gEfiCallerIdGuid,
+                      NULL,
+                      NULL
+                      );
+      ASSERT_EFI_ERROR (Status);
+      FreePool (Notify);
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This routine retrieves an array of GUID values for each keyboard layout that
+  was previously registered in the system.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  KeyGuidBufferLength    On input, a pointer to the length of the keyboard
+                                 GUID  buffer. On output, the length of the handle
+                                 buffer  that is required for the handles found.
+  @param  KeyGuidBuffer          An array of keyboard layout GUID instances
+                                 returned.
+
+  @retval EFI_SUCCESS            KeyGuidBuffer was updated successfully.
+  @retval EFI_BUFFER_TOO_SMALL   The KeyGuidBufferLength parameter indicates
+                                 that KeyGuidBuffer is too small to support the
+                                 number of GUIDs. KeyGuidBufferLength is
+                                 updated with a value that will enable the data to
+                                 fit.
+  @retval EFI_INVALID_PARAMETER  The KeyGuidBufferLength is NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by KeyGuidBufferLength is not
+                                 zero and KeyGuidBuffer is NULL.
+  @retval EFI_NOT_FOUND          There was no keyboard layout.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiFindKeyboardLayouts (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  OUT UINT16                        *KeyGuidBufferLength,
+  OUT EFI_GUID                          *KeyGuidBuffer
+  )
+{
+  HII_DATABASE_PRIVATE_DATA            *Private;
+  HII_DATABASE_RECORD                  *Node;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE   *PackageList;
+  LIST_ENTRY                           *Link;
+  LIST_ENTRY                           *Link1;
+  UINT16                               ResultSize;
+  UINTN                                Index;
+  UINT16                               LayoutCount;
+  UINT16                               LayoutLength;
+  UINT8                                *Layout;
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *Package;
+
+  if (This == NULL || KeyGuidBufferLength == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*KeyGuidBufferLength > 0 && KeyGuidBuffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private     = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  ResultSize  = 0;
+
+  //
+  // Search all package lists in whole database to retrieve keyboard layout.
+  //
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    PackageList = Node->PackageList;
+    for (Link1 = PackageList->KeyboardLayoutHdr.ForwardLink;
+         Link1 != &PackageList->KeyboardLayoutHdr;
+         Link1 = Link1->ForwardLink
+        ) {
+      //
+      // Find out all Keyboard Layout packages in this package list.
+      //
+      Package = CR (
+                  Link1,
+                  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE,
+                  KeyboardEntry,
+                  HII_KB_LAYOUT_PACKAGE_SIGNATURE
+                  );
+      Layout = (UINT8 *) Package->KeyboardPkg + sizeof (EFI_HII_PACKAGE_HEADER) + sizeof (UINT16);
+      CopyMem (
+        &LayoutCount,
+        (UINT8 *) Package->KeyboardPkg + sizeof (EFI_HII_PACKAGE_HEADER),
+        sizeof (UINT16)
+        );
+      for (Index = 0; Index < LayoutCount; Index++) {
+        ResultSize += sizeof (EFI_GUID);
+        if (ResultSize <= *KeyGuidBufferLength) {
+          CopyMem (KeyGuidBuffer + (ResultSize / sizeof (EFI_GUID) - 1), Layout + sizeof (UINT16), sizeof (EFI_GUID));
+          CopyMem (&LayoutLength, Layout, sizeof (UINT16));
+          Layout = Layout + LayoutLength;
+        }
+      }
+    }
+  }
+
+  if (ResultSize == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (*KeyGuidBufferLength < ResultSize) {
+    *KeyGuidBufferLength = ResultSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *KeyGuidBufferLength = ResultSize;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This routine retrieves the requested keyboard layout. The layout is a physical description of the keys
+  on a keyboard and the character(s) that are associated with a particular set of key strokes.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  KeyGuid                A pointer to the unique ID associated with a given
+                                 keyboard layout. If KeyGuid is NULL then the
+                                 current layout will be retrieved.
+  @param  KeyboardLayoutLength   On input, a pointer to the length of the
+                                 KeyboardLayout buffer.  On output, the length of
+                                 the data placed into KeyboardLayout.
+  @param  KeyboardLayout         A pointer to a buffer containing the retrieved
+                                 keyboard layout.
+
+  @retval EFI_SUCCESS            The keyboard layout was retrieved successfully.
+  @retval EFI_NOT_FOUND          The requested keyboard layout was not found.
+  @retval EFI_INVALID_PARAMETER  The KeyboardLayout or KeyboardLayoutLength was
+                                 NULL.
+  @retval EFI_BUFFER_TOO_SMALL   The KeyboardLayoutLength parameter indicates
+                                 that KeyboardLayout is too small to support the
+                                 requested keyboard layout. KeyboardLayoutLength is
+                                        updated with a value that will enable the
+                                 data to fit.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetKeyboardLayout (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  CONST EFI_GUID                          *KeyGuid,
+  IN OUT UINT16                         *KeyboardLayoutLength,
+  OUT EFI_HII_KEYBOARD_LAYOUT           *KeyboardLayout
+  )
+{
+  HII_DATABASE_PRIVATE_DATA            *Private;
+  HII_DATABASE_RECORD                  *Node;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE   *PackageList;
+  LIST_ENTRY                           *Link;
+  LIST_ENTRY                           *Link1;
+  UINTN                                Index;
+  UINT8                                *Layout;
+  UINT16                               LayoutCount;
+  UINT16                               LayoutLength;
+  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE *Package;
+
+  if (This == NULL || KeyboardLayoutLength == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*KeyboardLayoutLength > 0 && KeyboardLayout == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  //
+  // Retrieve the current keyboard layout.
+  //
+  if (KeyGuid == NULL) {
+    if (Private->CurrentLayout == NULL) {
+      return EFI_NOT_FOUND;
+    }
+    CopyMem (&LayoutLength, Private->CurrentLayout, sizeof (UINT16));
+    if (*KeyboardLayoutLength < LayoutLength) {
+      *KeyboardLayoutLength = LayoutLength;
+      return EFI_BUFFER_TOO_SMALL;
+    }
+    CopyMem (KeyboardLayout, Private->CurrentLayout, LayoutLength);
+    return EFI_SUCCESS;
+  }
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    PackageList = (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (Node->PackageList);
+    for (Link1 = PackageList->KeyboardLayoutHdr.ForwardLink;
+         Link1 != &PackageList->KeyboardLayoutHdr;
+         Link1 = Link1->ForwardLink
+        ) {
+      Package = CR (
+                  Link1,
+                  HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE,
+                  KeyboardEntry,
+                  HII_KB_LAYOUT_PACKAGE_SIGNATURE
+                  );
+
+      Layout = (UINT8 *) Package->KeyboardPkg +
+               sizeof (EFI_HII_PACKAGE_HEADER) + sizeof (UINT16);
+      CopyMem (&LayoutCount, Layout - sizeof (UINT16), sizeof (UINT16));
+      for (Index = 0; Index < LayoutCount; Index++) {
+        CopyMem (&LayoutLength, Layout, sizeof (UINT16));
+        if (CompareMem (Layout + sizeof (UINT16), KeyGuid, sizeof (EFI_GUID)) == 0) {
+          if (LayoutLength <= *KeyboardLayoutLength) {
+            CopyMem (KeyboardLayout, Layout, LayoutLength);
+            return EFI_SUCCESS;
+          } else {
+            *KeyboardLayoutLength = LayoutLength;
+            return EFI_BUFFER_TOO_SMALL;
+          }
+        }
+        Layout = Layout + LayoutLength;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This routine sets the default keyboard layout to the one referenced by KeyGuid. When this routine
+  is called, an event will be signaled of the EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID
+  group type. This is so that agents which are sensitive to the current keyboard layout being changed
+  can be notified of this change.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  KeyGuid                A pointer to the unique ID associated with a given
+                                 keyboard layout.
+
+  @retval EFI_SUCCESS            The current keyboard layout was successfully set.
+  @retval EFI_NOT_FOUND          The referenced keyboard layout was not found, so
+                                 action was taken.
+  @retval EFI_INVALID_PARAMETER  The KeyGuid was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetKeyboardLayout (
+  IN CONST EFI_HII_DATABASE_PROTOCOL          *This,
+  IN CONST EFI_GUID                           *KeyGuid
+  )
+{
+  HII_DATABASE_PRIVATE_DATA            *Private;
+  EFI_HII_KEYBOARD_LAYOUT              *KeyboardLayout;
+  UINT16                               KeyboardLayoutLength;
+  EFI_STATUS                           Status;
+
+  if (This == NULL || KeyGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // The specified GUID equals the current keyboard layout GUID,
+  // return directly.
+  //
+  if (CompareGuid (&Private->CurrentLayoutGuid, KeyGuid)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Try to find the incoming keyboard layout data in current database.
+  //
+  KeyboardLayoutLength = 0;
+  KeyboardLayout       = NULL;
+  Status = HiiGetKeyboardLayout (This, KeyGuid, &KeyboardLayoutLength, KeyboardLayout);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  KeyboardLayout = (EFI_HII_KEYBOARD_LAYOUT *) AllocateZeroPool (KeyboardLayoutLength);
+  ASSERT (KeyboardLayout != NULL);
+  Status = HiiGetKeyboardLayout (This, KeyGuid, &KeyboardLayoutLength, KeyboardLayout);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Backup current keyboard layout.
+  //
+  CopyMem (&Private->CurrentLayoutGuid, KeyGuid, sizeof (EFI_GUID));
+  if (Private->CurrentLayout != NULL) {
+    FreePool(Private->CurrentLayout);
+  }
+  Private->CurrentLayout = KeyboardLayout;
+
+  //
+  // Signal EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group to notify
+  // current keyboard layout is changed.
+  //
+  Status = gBS->SignalEvent (gHiiKeyboardLayoutChanged);
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Return the EFI handle associated with a package list.
+
+  @param  This                   A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                 instance.
+  @param  PackageListHandle      An EFI_HII_HANDLE that corresponds to the desired
+                                 package list in the HIIdatabase.
+  @param  DriverHandle           On return, contains the EFI_HANDLE which was
+                                 registered with the package list in
+                                 NewPackageList().
+
+  @retval EFI_SUCCESS            The DriverHandle was returned successfully.
+  @retval EFI_INVALID_PARAMETER  The PackageListHandle was not valid or
+                                 DriverHandle was NULL.
+  @retval EFI_NOT_FOUND          This PackageList handle can not be found in
+                                 current database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetPackageListHandle (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL         *This,
+  IN  EFI_HII_HANDLE                    PackageListHandle,
+  OUT EFI_HANDLE                        *DriverHandle
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *Node;
+  LIST_ENTRY                          *Link;
+
+  if (This == NULL || DriverHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageListHandle)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (Node->Handle == PackageListHandle) {
+      *DriverHandle = Node->DriverHandle;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Font.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Font.c
new file mode 100644
index 0000000000..4b70b995f5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Font.c
@@ -0,0 +1,2892 @@
+/** @file
+Implementation for EFI_HII_FONT_PROTOCOL.
+
+
+Copyright (c) 2007 - 2015, 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 "HiiDatabase.h"
+
+EFI_GRAPHICS_OUTPUT_BLT_PIXEL        mHiiEfiColors[16] = {
+  //
+  // B     G     R
+  //
+  {0x00, 0x00, 0x00, 0x00},  // BLACK
+  {0x98, 0x00, 0x00, 0x00},  // BLUE
+  {0x00, 0x98, 0x00, 0x00},  // GREEN
+  {0x98, 0x98, 0x00, 0x00},  // CYAN
+  {0x00, 0x00, 0x98, 0x00},  // RED
+  {0x98, 0x00, 0x98, 0x00},  // MAGENTA
+  {0x00, 0x98, 0x98, 0x00},  // BROWN
+  {0x98, 0x98, 0x98, 0x00},  // LIGHTGRAY
+  {0x30, 0x30, 0x30, 0x00},  // DARKGRAY - BRIGHT BLACK
+  {0xff, 0x00, 0x00, 0x00},  // LIGHTBLUE
+  {0x00, 0xff, 0x00, 0x00},  // LIGHTGREEN
+  {0xff, 0xff, 0x00, 0x00},  // LIGHTCYAN
+  {0x00, 0x00, 0xff, 0x00},  // LIGHTRED
+  {0xff, 0x00, 0xff, 0x00},  // LIGHTMAGENTA
+  {0x00, 0xff, 0xff, 0x00},  // YELLOW
+  {0xff, 0xff, 0xff, 0x00},  // WHITE
+};
+
+
+/**
+  Insert a character cell information to the list specified by GlyphInfoList.
+
+  This is a internal function.
+
+  @param  CharValue               Unicode character value, which identifies a glyph
+                                  block.
+  @param  GlyphInfoList           HII_GLYPH_INFO list head.
+  @param  Cell                    Incoming character cell information.
+
+  @retval EFI_SUCCESS             Cell information is added to the GlyphInfoList.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+NewCell (
+  IN  CHAR16                         CharValue,
+  IN  LIST_ENTRY                     *GlyphInfoList,
+  IN  EFI_HII_GLYPH_INFO             *Cell
+  )
+{
+  HII_GLYPH_INFO           *GlyphInfo;
+
+  ASSERT (Cell != NULL && GlyphInfoList != NULL);
+
+  GlyphInfo = (HII_GLYPH_INFO *) AllocateZeroPool (sizeof (HII_GLYPH_INFO));
+  if (GlyphInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // GlyphInfoList stores a list of default character cell information, each is
+  // identified by "CharId".
+  //
+  GlyphInfo->Signature = HII_GLYPH_INFO_SIGNATURE;
+  GlyphInfo->CharId    = CharValue;
+  if (Cell->AdvanceX == 0) {
+    Cell->AdvanceX = Cell->Width;
+  }
+  CopyMem (&GlyphInfo->Cell, Cell, sizeof (EFI_HII_GLYPH_INFO));
+  InsertTailList (GlyphInfoList, &GlyphInfo->Entry);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get a character cell information from the list specified by GlyphInfoList.
+
+  This is a internal function.
+
+  @param  CharValue               Unicode character value, which identifies a glyph
+                                  block.
+  @param  GlyphInfoList           HII_GLYPH_INFO list head.
+  @param  Cell                    Buffer which stores output character cell
+                                  information.
+
+  @retval EFI_SUCCESS             Cell information is added to the GlyphInfoList.
+  @retval EFI_NOT_FOUND           The character info specified by CharValue does
+                                  not exist.
+
+**/
+EFI_STATUS
+GetCell (
+  IN  CHAR16                         CharValue,
+  IN  LIST_ENTRY                     *GlyphInfoList,
+  OUT EFI_HII_GLYPH_INFO             *Cell
+  )
+{
+  HII_GLYPH_INFO           *GlyphInfo;
+  LIST_ENTRY               *Link;
+
+  ASSERT (Cell != NULL && GlyphInfoList != NULL);
+
+  //
+  // Since the EFI_HII_GIBT_DEFAULTS block won't increment CharValueCurrent,
+  // the value of "CharId" of a default character cell which is used for a
+  // EFI_HII_GIBT_GLYPH_DEFAULT or EFI_HII_GIBT_GLYPHS_DEFAULT should be
+  // less or equal to the value of "CharValueCurrent" of this default block.
+  //
+  // For instance, if the CharId of a GlyphInfoList is {1, 3, 7}, a default glyph
+  // with CharValue equals "7" uses the GlyphInfo with CharId = 7;
+  // a default glyph with CharValue equals "6" uses the GlyphInfo with CharId = 3.
+  //
+  for (Link = GlyphInfoList->BackLink; Link != GlyphInfoList; Link = Link->BackLink) {
+    GlyphInfo = CR (Link, HII_GLYPH_INFO, Entry, HII_GLYPH_INFO_SIGNATURE);
+    if (GlyphInfo->CharId <= CharValue) {
+      CopyMem (Cell, &GlyphInfo->Cell, sizeof (EFI_HII_GLYPH_INFO));
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Convert the glyph for a single character into a bitmap.
+
+  This is a internal function.
+
+  @param  Private                 HII database driver private data.
+  @param  Char                    Character to retrieve.
+  @param  StringInfo              Points to the string font and color information
+                                  or NULL  if the string should use the default
+                                  system font and color.
+  @param  GlyphBuffer             Buffer to store the retrieved bitmap data.
+  @param  Cell                    Points to EFI_HII_GLYPH_INFO structure.
+  @param  Attributes              If not NULL, output the glyph attributes if any.
+
+  @retval EFI_SUCCESS             Glyph bitmap outputted.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate the output buffer GlyphBuffer.
+  @retval EFI_NOT_FOUND           The glyph was unknown can not be found.
+  @retval EFI_INVALID_PARAMETER   Any input parameter is invalid.
+
+**/
+EFI_STATUS
+GetGlyphBuffer (
+  IN  HII_DATABASE_PRIVATE_DATA      *Private,
+  IN  CHAR16                         Char,
+  IN  EFI_FONT_INFO                  *StringInfo,
+  OUT UINT8                          **GlyphBuffer,
+  OUT EFI_HII_GLYPH_INFO             *Cell,
+  OUT UINT8                          *Attributes OPTIONAL
+  )
+{
+  HII_DATABASE_RECORD                *Node;
+  LIST_ENTRY                         *Link;
+  HII_SIMPLE_FONT_PACKAGE_INSTANCE   *SimpleFont;
+  LIST_ENTRY                         *Link1;
+  UINT16                             Index;
+  EFI_NARROW_GLYPH                   Narrow;
+  EFI_WIDE_GLYPH                     Wide;
+  HII_GLOBAL_FONT_INFO               *GlobalFont;
+  UINTN                              HeaderSize;
+  EFI_NARROW_GLYPH                   *NarrowPtr;
+  EFI_WIDE_GLYPH                     *WidePtr;
+
+  if (GlyphBuffer == NULL || Cell == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Private == NULL || Private->Signature != HII_DATABASE_PRIVATE_DATA_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (Cell, sizeof (EFI_HII_GLYPH_INFO));
+
+  //
+  // If StringInfo is not NULL, it must point to an existing EFI_FONT_INFO rather
+  // than system default font and color.
+  // If NULL, try to find the character in simplified font packages since
+  // default system font is the fixed font (narrow or wide glyph).
+  //
+  if (StringInfo != NULL) {
+    if(!IsFontInfoExisted (Private, StringInfo, NULL, NULL, &GlobalFont)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if (Attributes != NULL) {
+      *Attributes = PROPORTIONAL_GLYPH;
+    }
+    return FindGlyphBlock (GlobalFont->FontPackage, Char, GlyphBuffer, Cell, NULL);
+  } else {
+    HeaderSize = sizeof (EFI_HII_SIMPLE_FONT_PACKAGE_HDR);
+
+    for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+      Node = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+      for (Link1 = Node->PackageList->SimpleFontPkgHdr.ForwardLink;
+           Link1 != &Node->PackageList->SimpleFontPkgHdr;
+           Link1 = Link1->ForwardLink
+          ) {
+        SimpleFont = CR (Link1, HII_SIMPLE_FONT_PACKAGE_INSTANCE, SimpleFontEntry, HII_S_FONT_PACKAGE_SIGNATURE);
+        //
+        // Search the narrow glyph array
+        //
+        NarrowPtr = (EFI_NARROW_GLYPH *) ((UINT8 *) (SimpleFont->SimpleFontPkgHdr) + HeaderSize);
+        for (Index = 0; Index < SimpleFont->SimpleFontPkgHdr->NumberOfNarrowGlyphs; Index++) {
+          CopyMem (&Narrow, NarrowPtr + Index,sizeof (EFI_NARROW_GLYPH));
+          if (Narrow.UnicodeWeight == Char) {
+            *GlyphBuffer = (UINT8 *) AllocateZeroPool (EFI_GLYPH_HEIGHT);
+            if (*GlyphBuffer == NULL) {
+              return EFI_OUT_OF_RESOURCES;
+            }
+            Cell->Width    = EFI_GLYPH_WIDTH;
+            Cell->Height   = EFI_GLYPH_HEIGHT;
+            Cell->AdvanceX = Cell->Width;
+            CopyMem (*GlyphBuffer, Narrow.GlyphCol1, Cell->Height);
+            if (Attributes != NULL) {
+              *Attributes = (UINT8) (Narrow.Attributes | NARROW_GLYPH);
+            }
+            return EFI_SUCCESS;
+          }
+        }
+        //
+        // Search the wide glyph array
+        //
+        WidePtr = (EFI_WIDE_GLYPH *) (NarrowPtr + SimpleFont->SimpleFontPkgHdr->NumberOfNarrowGlyphs);
+        for (Index = 0; Index < SimpleFont->SimpleFontPkgHdr->NumberOfWideGlyphs; Index++) {
+          CopyMem (&Wide, WidePtr + Index, sizeof (EFI_WIDE_GLYPH));
+          if (Wide.UnicodeWeight == Char) {
+            *GlyphBuffer    = (UINT8 *) AllocateZeroPool (EFI_GLYPH_HEIGHT * 2);
+            if (*GlyphBuffer == NULL) {
+              return EFI_OUT_OF_RESOURCES;
+            }
+            Cell->Width    = EFI_GLYPH_WIDTH * 2;
+            Cell->Height   = EFI_GLYPH_HEIGHT;
+            Cell->AdvanceX = Cell->Width;
+            CopyMem (*GlyphBuffer, Wide.GlyphCol1, EFI_GLYPH_HEIGHT);
+            CopyMem (*GlyphBuffer + EFI_GLYPH_HEIGHT, Wide.GlyphCol2, EFI_GLYPH_HEIGHT);
+            if (Attributes != NULL) {
+              *Attributes = (UINT8) (Wide.Attributes | EFI_GLYPH_WIDE);
+            }
+            return EFI_SUCCESS;
+          }
+        }
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Convert bitmap data of the glyph to blt structure.
+
+  This is a internal function.
+
+  @param GlyphBuffer     Buffer points to bitmap data of glyph.
+  @param  Foreground     The color of the "on" pixels in the glyph in the
+                         bitmap.
+  @param  Background     The color of the "off" pixels in the glyph in the
+                         bitmap.
+  @param  ImageWidth     Width of the whole image in pixels.
+  @param  RowWidth       The width of the text on the line, in pixels.
+  @param  RowHeight      The height of the line, in pixels.
+  @param  Transparent    If TRUE, the Background color is ignored and all
+                         "off" pixels in the character's drawn wil use the
+                         pixel value from BltBuffer.
+  @param  Origin         On input, points to the origin of the to be
+                         displayed character, on output, points to the
+                         next glyph's origin.
+
+**/
+VOID
+NarrowGlyphToBlt (
+  IN     UINT8                         *GlyphBuffer,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Foreground,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Background,
+  IN     UINT16                        ImageWidth,
+  IN     UINTN                         RowWidth,
+  IN     UINTN                         RowHeight,
+  IN     BOOLEAN                       Transparent,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL **Origin
+  )
+{
+  UINT8                                Xpos;
+  UINT8                                Ypos;
+  UINT8                                Height;
+  UINT8                                Width;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL        *Buffer;
+
+  ASSERT (GlyphBuffer != NULL && Origin != NULL && *Origin != NULL);
+
+  Height = EFI_GLYPH_HEIGHT;
+  Width  = EFI_GLYPH_WIDTH;
+  
+  //
+  // Move position to the left-top corner of char.
+  //
+  Buffer = *Origin - EFI_GLYPH_HEIGHT * ImageWidth;
+
+  //
+  // Char may be partially displayed when CLIP_X or CLIP_Y is not set. 
+  //
+  if (RowHeight < Height) {
+    Height = (UINT8) RowHeight;
+  }
+  if (RowWidth < Width) {
+    Width = (UINT8) RowWidth;
+  }
+
+  for (Ypos = 0; Ypos < Height; Ypos++) {
+    for (Xpos = 0; Xpos < Width; Xpos++) {
+      if ((GlyphBuffer[Ypos] & (1 << (EFI_GLYPH_WIDTH - Xpos - 1))) != 0) {
+        Buffer[Ypos * ImageWidth + Xpos] = Foreground;
+      } else {
+        if (!Transparent) {
+          Buffer[Ypos * ImageWidth + Xpos] = Background;
+        }
+      }
+    }
+  }
+
+  *Origin = *Origin + EFI_GLYPH_WIDTH;
+}
+
+
+/**
+  Convert bitmap data of the glyph to blt structure.
+
+  This is a internal function.
+
+  @param  GlyphBuffer             Buffer points to bitmap data of glyph.
+  @param  Foreground              The color of the "on" pixels in the glyph in the
+                                  bitmap.
+  @param  Background              The color of the "off" pixels in the glyph in the
+                                  bitmap.
+  @param  ImageWidth              Width of the whole image in pixels.
+  @param  BaseLine                BaseLine in the line.
+  @param  RowWidth                The width of the text on the line, in pixels.
+  @param  RowHeight               The height of the line, in pixels.
+  @param  Transparent             If TRUE, the Background color is ignored and all
+                                  "off" pixels in the character's drawn wil use the
+                                  pixel value from BltBuffer.
+  @param  Cell                    Points to EFI_HII_GLYPH_INFO structure.
+  @param  Attributes              The attribute of incoming glyph in GlyphBuffer.
+  @param  Origin                  On input, points to the origin of the to be
+                                  displayed character, on output, points to the
+                                  next glyph's origin.
+
+
+**/
+VOID
+GlyphToBlt (
+  IN     UINT8                         *GlyphBuffer,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Foreground,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Background,
+  IN     UINT16                        ImageWidth,
+  IN     UINT16                        BaseLine,
+  IN     UINTN                         RowWidth,
+  IN     UINTN                         RowHeight,
+  IN     BOOLEAN                       Transparent,
+  IN     CONST EFI_HII_GLYPH_INFO      *Cell,
+  IN     UINT8                         Attributes,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL **Origin
+  )
+{
+  UINT16                                Xpos;
+  UINT16                                Ypos;
+  UINT8                                 Data;
+  UINT16                                Index;
+  UINT16                                YposOffset;
+  UINTN                                 OffsetY;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL         *BltBuffer;
+
+  ASSERT (Origin != NULL && *Origin != NULL && Cell != NULL);
+
+  //
+  // Only adjust origin position if char has no bitmap.
+  //
+  if (GlyphBuffer == NULL) {
+    *Origin = *Origin + Cell->AdvanceX;
+    return;
+  }
+  //
+  // Move position to the left-top corner of char.
+  //
+  BltBuffer  = *Origin + Cell->OffsetX - (Cell->OffsetY + Cell->Height) * ImageWidth;
+  YposOffset = (UINT16) (BaseLine - (Cell->OffsetY + Cell->Height));
+
+  //
+  // Since non-spacing key will be printed OR'd with the previous glyph, don't
+  // write 0.
+  //
+  if ((Attributes & EFI_GLYPH_NON_SPACING) == EFI_GLYPH_NON_SPACING) {
+    Transparent = TRUE;
+  }
+
+  //
+  // The glyph's upper left hand corner pixel is the most significant bit of the
+  // first bitmap byte.
+  //
+  for (Ypos = 0; Ypos < Cell->Height && ((UINTN) (Ypos + YposOffset) < RowHeight); Ypos++) {
+    OffsetY = BITMAP_LEN_1_BIT (Cell->Width, Ypos);
+
+    //
+    // All bits in these bytes are meaningful.
+    //
+    for (Xpos = 0; Xpos < Cell->Width / 8; Xpos++) {
+      Data  = *(GlyphBuffer + OffsetY + Xpos);
+      for (Index = 0; Index < 8 && ((UINTN) (Xpos * 8 + Index + Cell->OffsetX) < RowWidth); Index++) {
+        if ((Data & (1 << (8 - Index - 1))) != 0) {
+          BltBuffer[Ypos * ImageWidth + Xpos * 8 + Index] = Foreground;
+        } else {
+          if (!Transparent) {
+            BltBuffer[Ypos * ImageWidth + Xpos * 8 + Index] = Background;
+          }
+        }
+      }
+    }
+
+    if (Cell->Width % 8 != 0) {
+      //
+      // There are some padding bits in this byte. Ignore them.
+      //
+      Data  = *(GlyphBuffer + OffsetY + Xpos);
+      for (Index = 0; Index < Cell->Width % 8 && ((UINTN) (Xpos * 8 + Index + Cell->OffsetX) < RowWidth); Index++) {
+        if ((Data & (1 << (8 - Index - 1))) != 0) {
+          BltBuffer[Ypos * ImageWidth + Xpos * 8 + Index] = Foreground;
+        } else {
+          if (!Transparent) {
+            BltBuffer[Ypos * ImageWidth + Xpos * 8 + Index] = Background;
+          }
+        }
+      }
+    } // end of if (Width % 8...)
+
+  } // end of for (Ypos=0...)
+
+  *Origin = *Origin + Cell->AdvanceX;
+}
+
+
+/**
+  Convert bitmap data of the glyph to blt structure.
+
+  This is a internal function.
+
+  @param  GlyphBuffer             Buffer points to bitmap data of glyph.
+  @param  Foreground              The color of the "on" pixels in the glyph in the
+                                  bitmap.
+  @param  Background              The color of the "off" pixels in the glyph in the
+                                  bitmap.
+  @param  ImageWidth              Width of the whole image in pixels.
+  @param  BaseLine                BaseLine in the line.
+  @param  RowWidth                The width of the text on the line, in pixels.
+  @param  RowHeight               The height of the line, in pixels.
+  @param  Transparent             If TRUE, the Background color is ignored and all
+                                  "off" pixels in the character's drawn wil use the
+                                  pixel value from BltBuffer.
+  @param  Cell                    Points to EFI_HII_GLYPH_INFO structure.
+  @param  Attributes              The attribute of incoming glyph in GlyphBuffer.
+  @param  Origin                  On input, points to the origin of the to be
+                                  displayed character, on output, points to the
+                                  next glyph's origin.
+
+  @return Points to the address of next origin node in BltBuffer.
+
+**/
+VOID
+GlyphToImage (
+  IN     UINT8                         *GlyphBuffer,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Foreground,
+  IN     EFI_GRAPHICS_OUTPUT_BLT_PIXEL Background,
+  IN     UINT16                        ImageWidth,
+  IN     UINT16                        BaseLine,
+  IN     UINTN                         RowWidth,
+  IN     UINTN                         RowHeight,
+  IN     BOOLEAN                       Transparent,
+  IN     CONST EFI_HII_GLYPH_INFO      *Cell,
+  IN     UINT8                         Attributes,
+  IN OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL **Origin
+  )
+{
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL        *Buffer;
+
+  ASSERT (Origin != NULL && *Origin != NULL && Cell != NULL);
+
+  Buffer = *Origin;
+
+  if ((Attributes & EFI_GLYPH_NON_SPACING) == EFI_GLYPH_NON_SPACING) {
+    //
+    // This character is a non-spacing key, print it OR'd with the previous glyph.
+    // without advancing cursor.
+    //
+    Buffer -= Cell->AdvanceX;
+    GlyphToBlt (
+      GlyphBuffer,
+      Foreground,
+      Background,
+      ImageWidth,
+      BaseLine,
+      RowWidth,
+      RowHeight,
+      Transparent,
+      Cell,
+      Attributes,
+      &Buffer
+      );
+
+  } else if ((Attributes & EFI_GLYPH_WIDE) == EFI_GLYPH_WIDE) {
+    //
+    // This character is wide glyph, i.e. 16 pixels * 19 pixels.
+    // Draw it as two narrow glyphs.
+    //
+    NarrowGlyphToBlt (
+      GlyphBuffer,
+      Foreground,
+      Background,
+      ImageWidth,
+      RowWidth,
+      RowHeight,
+      Transparent,
+      Origin
+      );
+
+    NarrowGlyphToBlt (
+      GlyphBuffer + EFI_GLYPH_HEIGHT,
+      Foreground,
+      Background,
+      ImageWidth,
+      RowWidth,
+      RowHeight,
+      Transparent,
+      Origin
+      );
+
+  } else if ((Attributes & NARROW_GLYPH) == NARROW_GLYPH) {
+    //
+    // This character is narrow glyph, i.e. 8 pixels * 19 pixels.
+    //
+    NarrowGlyphToBlt (
+      GlyphBuffer,
+      Foreground,
+      Background,
+      ImageWidth,
+      RowWidth,
+      RowHeight,
+      Transparent,
+      Origin
+      );
+
+  } else if ((Attributes & PROPORTIONAL_GLYPH) == PROPORTIONAL_GLYPH) {
+    //
+    // This character is proportional glyph, i.e. Cell->Width * Cell->Height pixels.
+    //
+    GlyphToBlt (
+      GlyphBuffer,
+      Foreground,
+      Background,
+      ImageWidth,
+      BaseLine,
+      RowWidth,
+      RowHeight,
+      Transparent,
+      Cell,
+      Attributes,
+      Origin
+      );
+  }
+}
+
+
+/**
+  Write the output parameters of FindGlyphBlock().
+
+  This is a internal function.
+
+  @param  BufferIn                Buffer which stores the bitmap data of the found
+                                  block.
+  @param  BufferLen               Length of BufferIn.
+  @param  InputCell               Buffer which stores cell information of the
+                                  encoded bitmap.
+  @param  GlyphBuffer             Output the corresponding bitmap data of the found
+                                  block. It is the caller's responsiblity to free
+                                  this buffer.
+  @param  Cell                    Output cell information of the encoded bitmap.
+  @param  GlyphBufferLen          If not NULL, output the length of GlyphBuffer.
+
+  @retval EFI_SUCCESS             The operation is performed successfully.
+  @retval EFI_INVALID_PARAMETER   Any input parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+WriteOutputParam (
+  IN  UINT8                          *BufferIn,
+  IN  UINTN                          BufferLen,
+  IN  EFI_HII_GLYPH_INFO             *InputCell,
+  OUT UINT8                          **GlyphBuffer, OPTIONAL
+  OUT EFI_HII_GLYPH_INFO             *Cell, OPTIONAL
+  OUT UINTN                          *GlyphBufferLen OPTIONAL
+  )
+{
+  if (BufferIn == NULL || InputCell == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Cell != NULL) {
+    CopyMem (Cell, InputCell, sizeof (EFI_HII_GLYPH_INFO));
+  }
+
+  if (GlyphBuffer != NULL && BufferLen > 0) {
+    *GlyphBuffer = (UINT8 *) AllocateZeroPool (BufferLen);
+    if (*GlyphBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyMem (*GlyphBuffer, BufferIn, BufferLen);
+  }
+
+  if (GlyphBufferLen != NULL) {
+    *GlyphBufferLen = BufferLen;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parse all glyph blocks to find a glyph block specified by CharValue.
+  If CharValue = (CHAR16) (-1), collect all default character cell information
+  within this font package and backup its information.
+
+  @param  FontPackage             Hii string package instance.
+  @param  CharValue               Unicode character value, which identifies a glyph
+                                  block.
+  @param  GlyphBuffer             Output the corresponding bitmap data of the found
+                                  block. It is the caller's responsiblity to free
+                                  this buffer.
+  @param  Cell                    Output cell information of the encoded bitmap.
+  @param  GlyphBufferLen          If not NULL, output the length of GlyphBuffer.
+
+  @retval EFI_SUCCESS             The bitmap data is retrieved successfully.
+  @retval EFI_NOT_FOUND           The specified CharValue does not exist in current
+                                  database.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+FindGlyphBlock (
+  IN  HII_FONT_PACKAGE_INSTANCE      *FontPackage,
+  IN  CHAR16                         CharValue,
+  OUT UINT8                          **GlyphBuffer, OPTIONAL
+  OUT EFI_HII_GLYPH_INFO             *Cell, OPTIONAL
+  OUT UINTN                          *GlyphBufferLen OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  UINT8                               *BlockPtr;
+  UINT16                              CharCurrent;
+  UINT16                              Length16;
+  UINT32                              Length32;
+  EFI_HII_GIBT_GLYPHS_BLOCK           Glyphs;
+  UINTN                               BufferLen;
+  UINT16                              Index;
+  EFI_HII_GLYPH_INFO                  DefaultCell;
+  EFI_HII_GLYPH_INFO                  LocalCell;
+  INT16                               MinOffsetY;
+  UINT16                              BaseLine;
+
+  ASSERT (FontPackage != NULL);
+  ASSERT (FontPackage->Signature == HII_FONT_PACKAGE_SIGNATURE);
+  BaseLine  = 0;
+  MinOffsetY = 0;
+  
+  if (CharValue == (CHAR16) (-1)) {
+    //
+    // Collect the cell information specified in font package fixed header.
+    // Use CharValue =0 to represent this particular cell.
+    //
+    Status = NewCell (
+               0,
+               &FontPackage->GlyphInfoList,
+               (EFI_HII_GLYPH_INFO *) ((UINT8 *) FontPackage->FontPkgHdr + 3 * sizeof (UINT32))
+               );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    CopyMem (
+      &LocalCell,
+      (UINT8 *) FontPackage->FontPkgHdr + 3 * sizeof (UINT32),
+      sizeof (EFI_HII_GLYPH_INFO)
+      );
+    BaseLine = (UINT16) (LocalCell.Height + LocalCell.OffsetY);
+    if (MinOffsetY > LocalCell.OffsetY) {
+      MinOffsetY = LocalCell.OffsetY;
+    }
+  }
+
+  BlockPtr    = FontPackage->GlyphBlock;
+  CharCurrent = 1;
+  BufferLen   = 0;
+
+  while (*BlockPtr != EFI_HII_GIBT_END) {
+    switch (*BlockPtr) {
+    case EFI_HII_GIBT_DEFAULTS:
+      //
+      // Collect all default character cell information specified by
+      // EFI_HII_GIBT_DEFAULTS.
+      //
+      if (CharValue == (CHAR16) (-1)) {
+        Status = NewCell (
+                   CharCurrent,
+                   &FontPackage->GlyphInfoList,
+                   (EFI_HII_GLYPH_INFO *) (BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK))
+                   );
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        CopyMem (
+          &LocalCell,
+          BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK),
+          sizeof (EFI_HII_GLYPH_INFO)
+          );
+        if (BaseLine < LocalCell.Height + LocalCell.OffsetY) {
+          BaseLine = (UINT16) (LocalCell.Height + LocalCell.OffsetY);
+        }
+        if (MinOffsetY > LocalCell.OffsetY) {
+          MinOffsetY = LocalCell.OffsetY;
+        }
+      }
+      BlockPtr += sizeof (EFI_HII_GIBT_DEFAULTS_BLOCK);
+      break;
+
+    case EFI_HII_GIBT_DUPLICATE:
+      if (CharCurrent == CharValue) {
+        CopyMem (&CharValue, BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK), sizeof (CHAR16));
+        CharCurrent = 1;
+        BlockPtr    = FontPackage->GlyphBlock;
+        continue;
+      }
+      CharCurrent++;
+      BlockPtr += sizeof (EFI_HII_GIBT_DUPLICATE_BLOCK);
+      break;
+
+    case EFI_HII_GIBT_EXT1:
+      BlockPtr += *(UINT8*)((UINTN)BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK) + sizeof (UINT8));
+      break;
+    case EFI_HII_GIBT_EXT2:
+      CopyMem (
+        &Length16,
+        (UINT8*)((UINTN)BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      BlockPtr += Length16;
+      break;
+    case EFI_HII_GIBT_EXT4:
+      CopyMem (
+        &Length32,
+        (UINT8*)((UINTN)BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT32)
+        );
+      BlockPtr += Length32;
+      break;
+
+    case EFI_HII_GIBT_GLYPH:
+      CopyMem (
+        &LocalCell,
+        BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK),
+        sizeof (EFI_HII_GLYPH_INFO)
+        );
+      if (CharValue == (CHAR16) (-1)) {
+        if (BaseLine < LocalCell.Height + LocalCell.OffsetY) {
+          BaseLine = (UINT16) (LocalCell.Height + LocalCell.OffsetY);
+        }
+        if (MinOffsetY > LocalCell.OffsetY) {
+          MinOffsetY = LocalCell.OffsetY;
+        }
+      }
+      BufferLen = BITMAP_LEN_1_BIT (LocalCell.Width, LocalCell.Height);
+      if (CharCurrent == CharValue) {
+        return WriteOutputParam (
+                 (UINT8*)((UINTN)BlockPtr + sizeof (EFI_HII_GIBT_GLYPH_BLOCK) - sizeof (UINT8)),
+                 BufferLen,
+                 &LocalCell,
+                 GlyphBuffer,
+                 Cell,
+                 GlyphBufferLen
+                 );
+      }
+      CharCurrent++;
+      BlockPtr += sizeof (EFI_HII_GIBT_GLYPH_BLOCK) - sizeof (UINT8) + BufferLen;
+      break;
+
+    case EFI_HII_GIBT_GLYPHS:
+      BlockPtr += sizeof (EFI_HII_GLYPH_BLOCK);
+      CopyMem (&Glyphs.Cell, BlockPtr, sizeof (EFI_HII_GLYPH_INFO));
+      BlockPtr += sizeof (EFI_HII_GLYPH_INFO);
+      CopyMem (&Glyphs.Count, BlockPtr, sizeof (UINT16));
+      BlockPtr += sizeof (UINT16);
+
+      if (CharValue == (CHAR16) (-1)) {
+        if (BaseLine < Glyphs.Cell.Height + Glyphs.Cell.OffsetY) {
+          BaseLine = (UINT16) (Glyphs.Cell.Height + Glyphs.Cell.OffsetY);
+        }
+        if (MinOffsetY > Glyphs.Cell.OffsetY) {
+          MinOffsetY = Glyphs.Cell.OffsetY;
+        }
+      }
+
+      BufferLen = BITMAP_LEN_1_BIT (Glyphs.Cell.Width, Glyphs.Cell.Height);
+      for (Index = 0; Index < Glyphs.Count; Index++) {
+        if (CharCurrent + Index == CharValue) {
+          return WriteOutputParam (
+                   BlockPtr,
+                   BufferLen,
+                   &Glyphs.Cell,
+                   GlyphBuffer,
+                   Cell,
+                   GlyphBufferLen
+                   );
+        }
+        BlockPtr += BufferLen;
+      }
+      CharCurrent = (UINT16) (CharCurrent + Glyphs.Count);
+      break;
+
+    case EFI_HII_GIBT_GLYPH_DEFAULT:
+      Status = GetCell (CharCurrent, &FontPackage->GlyphInfoList, &DefaultCell);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      BufferLen = BITMAP_LEN_1_BIT (DefaultCell.Width, DefaultCell.Height);
+
+      if (CharCurrent == CharValue) {
+        return WriteOutputParam (
+                 BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK),
+                 BufferLen,
+                 &DefaultCell,
+                 GlyphBuffer,
+                 Cell,
+                 GlyphBufferLen
+                 );
+      }
+      CharCurrent++;
+      BlockPtr += sizeof (EFI_HII_GLYPH_BLOCK) + BufferLen;
+      break;
+
+    case EFI_HII_GIBT_GLYPHS_DEFAULT:
+      CopyMem (&Length16, BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK), sizeof (UINT16));
+      Status = GetCell (CharCurrent, &FontPackage->GlyphInfoList, &DefaultCell);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+      BufferLen = BITMAP_LEN_1_BIT (DefaultCell.Width, DefaultCell.Height);
+      BlockPtr += sizeof (EFI_HII_GIBT_GLYPHS_DEFAULT_BLOCK) - sizeof (UINT8);
+      for (Index = 0; Index < Length16; Index++) {
+        if (CharCurrent + Index == CharValue) {
+          return WriteOutputParam (
+                   BlockPtr,
+                   BufferLen,
+                   &DefaultCell,
+                   GlyphBuffer,
+                   Cell,
+                   GlyphBufferLen
+                   );
+        }
+        BlockPtr += BufferLen;
+      }
+      CharCurrent = (UINT16) (CharCurrent + Length16);
+      break;
+
+    case EFI_HII_GIBT_SKIP1:
+      CharCurrent = (UINT16) (CharCurrent + (UINT16) (*(BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK))));
+      BlockPtr    += sizeof (EFI_HII_GIBT_SKIP1_BLOCK);
+      break;
+    case EFI_HII_GIBT_SKIP2:
+      CopyMem (&Length16, BlockPtr + sizeof (EFI_HII_GLYPH_BLOCK), sizeof (UINT16));
+      CharCurrent = (UINT16) (CharCurrent + Length16);
+      BlockPtr    += sizeof (EFI_HII_GIBT_SKIP2_BLOCK);
+      break;
+    default:
+      ASSERT (FALSE);
+      break;
+    }
+
+    if (CharValue < CharCurrent) {
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  if (CharValue == (CHAR16) (-1)) {
+    FontPackage->BaseLine = BaseLine;
+    FontPackage->Height   = (UINT16) (BaseLine - MinOffsetY);
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Copy a Font Name to a new created EFI_FONT_INFO structure.
+
+  This is a internal function.
+
+  @param  FontName                NULL-terminated string.
+  @param  FontInfo                a new EFI_FONT_INFO which stores the FontName.
+                                  It's caller's responsibility to free this buffer.
+
+  @retval EFI_SUCCESS             FontInfo is allocated and copied with FontName.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+SaveFontName (
+  IN  EFI_STRING                       FontName,
+  OUT EFI_FONT_INFO                    **FontInfo
+  )
+{
+  UINTN         FontInfoLen;
+  UINTN         NameSize;
+
+  ASSERT (FontName != NULL && FontInfo != NULL);
+
+  NameSize = StrSize (FontName);
+  FontInfoLen = sizeof (EFI_FONT_INFO) - sizeof (CHAR16) + NameSize;
+  *FontInfo = (EFI_FONT_INFO *) AllocateZeroPool (FontInfoLen);
+  if (*FontInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  StrCpyS ((*FontInfo)->FontName, NameSize / sizeof (CHAR16), FontName);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retrieve system default font and color.
+
+  @param  Private                 HII database driver private data.
+  @param  FontInfo                Points to system default font output-related
+                                  information. It's caller's responsibility to free
+                                  this buffer.
+  @param  FontInfoSize            If not NULL, output the size of buffer FontInfo.
+
+  @retval EFI_SUCCESS             Cell information is added to the GlyphInfoList.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+  @retval EFI_INVALID_PARAMETER   Any input parameter is invalid.
+
+**/
+EFI_STATUS
+GetSystemFont (
+  IN  HII_DATABASE_PRIVATE_DATA      *Private,
+  OUT EFI_FONT_DISPLAY_INFO          **FontInfo,
+  OUT UINTN                          *FontInfoSize OPTIONAL
+  )
+{
+  EFI_FONT_DISPLAY_INFO              *Info;
+  UINTN                              InfoSize;
+  UINTN                              NameSize;
+
+  if (Private == NULL || Private->Signature != HII_DATABASE_PRIVATE_DATA_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (FontInfo == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // The standard font always has the name "sysdefault".
+  //
+  NameSize = StrSize (L"sysdefault");
+  InfoSize = sizeof (EFI_FONT_DISPLAY_INFO) - sizeof (CHAR16) + NameSize;
+  Info = (EFI_FONT_DISPLAY_INFO *) AllocateZeroPool (InfoSize);
+  if (Info == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Info->ForegroundColor    = mHiiEfiColors[Private->Attribute & 0x0f];
+  Info->BackgroundColor    = mHiiEfiColors[Private->Attribute >> 4];
+  Info->FontInfoMask       = EFI_FONT_INFO_SYS_FONT | EFI_FONT_INFO_SYS_SIZE | EFI_FONT_INFO_SYS_STYLE;
+  Info->FontInfo.FontStyle = 0;
+  Info->FontInfo.FontSize  = EFI_GLYPH_HEIGHT;
+  StrCpyS (Info->FontInfo.FontName, NameSize / sizeof (CHAR16), L"sysdefault");
+
+  *FontInfo = Info;
+  if (FontInfoSize != NULL) {
+    *FontInfoSize = InfoSize;
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Check whether EFI_FONT_DISPLAY_INFO points to system default font and color or
+  returns the system default according to the optional inputs.
+
+  This is a internal function.
+
+  @param  Private                 HII database driver private data.
+  @param  StringInfo              Points to the string output information,
+                                  including the color and font.
+  @param  SystemInfo              If not NULL, points to system default font and color.
+
+  @param  SystemInfoLen           If not NULL, output the length of default system
+                                  info.
+
+  @retval TRUE                    Yes, it points to system default.
+  @retval FALSE                   No.
+
+**/
+BOOLEAN
+IsSystemFontInfo (
+  IN  HII_DATABASE_PRIVATE_DATA      *Private,
+  IN  EFI_FONT_DISPLAY_INFO          *StringInfo,
+  OUT EFI_FONT_DISPLAY_INFO          **SystemInfo, OPTIONAL
+  OUT UINTN                          *SystemInfoLen OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_FONT_DISPLAY_INFO               *SystemDefault;
+  UINTN                               DefaultLen;
+  BOOLEAN                             Flag;
+
+  ASSERT (Private != NULL && Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+
+  if (StringInfo == NULL && SystemInfo == NULL) {
+    return TRUE;
+  }
+
+  SystemDefault = NULL;
+  DefaultLen    = 0;
+
+  Status = GetSystemFont (Private, &SystemDefault, &DefaultLen);
+  ASSERT_EFI_ERROR (Status);
+  ASSERT ((SystemDefault != NULL) && (DefaultLen != 0));
+
+  //
+  // Record the system default info.
+  //
+  if (SystemInfo != NULL) {
+    *SystemInfo = SystemDefault;
+  }
+
+  if (SystemInfoLen != NULL) {
+    *SystemInfoLen = DefaultLen;
+  }
+
+  if (StringInfo == NULL) {
+    return TRUE;
+  }
+
+  Flag = FALSE;
+  //
+  // Check the FontInfoMask to see whether it is retrieving system info.
+  //
+  if ((StringInfo->FontInfoMask & (EFI_FONT_INFO_SYS_FONT | EFI_FONT_INFO_ANY_FONT)) == 0) {
+    if (StrCmp (StringInfo->FontInfo.FontName, SystemDefault->FontInfo.FontName) != 0) {
+      goto Exit;
+    }
+  }
+  if ((StringInfo->FontInfoMask & (EFI_FONT_INFO_SYS_SIZE | EFI_FONT_INFO_ANY_SIZE)) == 0) {
+    if (StringInfo->FontInfo.FontSize != SystemDefault->FontInfo.FontSize) {
+      goto Exit;
+    }
+  }
+  if ((StringInfo->FontInfoMask & (EFI_FONT_INFO_SYS_STYLE | EFI_FONT_INFO_ANY_STYLE)) == 0) {
+    if (StringInfo->FontInfo.FontStyle != SystemDefault->FontInfo.FontStyle) {
+      goto Exit;
+    }
+  }
+  if ((StringInfo->FontInfoMask & EFI_FONT_INFO_SYS_FORE_COLOR) == 0) {
+    if (CompareMem (
+          &StringInfo->ForegroundColor, 
+          &SystemDefault->ForegroundColor, 
+          sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+          ) != 0) {
+      goto Exit;
+    }
+  }
+  if ((StringInfo->FontInfoMask & EFI_FONT_INFO_SYS_BACK_COLOR) == 0) {
+    if (CompareMem (
+          &StringInfo->BackgroundColor, 
+          &SystemDefault->BackgroundColor, 
+          sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL)
+          ) != 0) {
+      goto Exit;
+    }
+  }
+
+  Flag = TRUE;
+
+Exit:
+  if (SystemInfo == NULL) {
+    if (SystemDefault != NULL) {
+      FreePool (SystemDefault);
+    }
+  }
+  return Flag;
+}
+
+
+/**
+  This function checks whether EFI_FONT_INFO exists in current database. If
+  FontInfoMask is specified, check what options can be used to make a match.
+  Note that the masks relate to where the system default should be supplied
+  are ignored by this function.
+
+  @param  Private                 Hii database private structure.
+  @param  FontInfo                Points to EFI_FONT_INFO structure.
+  @param  FontInfoMask            If not NULL, describes what options can be used
+                                  to make a match between the font requested and
+                                  the font available. The caller must guarantee
+                                  this mask is valid.
+  @param  FontHandle              On entry, Points to the font handle returned by a
+                                  previous  call to GetFontInfo() or NULL to start
+                                  with the first font.
+  @param  GlobalFontInfo          If not NULL, output the corresponding globa font
+                                  info.
+
+  @retval TRUE                    Existed
+  @retval FALSE                   Not existed
+
+**/
+BOOLEAN
+IsFontInfoExisted (
+  IN  HII_DATABASE_PRIVATE_DATA *Private,
+  IN  EFI_FONT_INFO             *FontInfo,
+  IN  EFI_FONT_INFO_MASK        *FontInfoMask,   OPTIONAL
+  IN  EFI_FONT_HANDLE           FontHandle,      OPTIONAL
+  OUT HII_GLOBAL_FONT_INFO      **GlobalFontInfo OPTIONAL
+  )
+{
+  HII_GLOBAL_FONT_INFO          *GlobalFont;
+  HII_GLOBAL_FONT_INFO          *GlobalFontBackup1;
+  HII_GLOBAL_FONT_INFO          *GlobalFontBackup2;
+  LIST_ENTRY                    *Link;
+  EFI_FONT_INFO_MASK            Mask;
+  BOOLEAN                       Matched;
+  BOOLEAN                       VagueMatched1;
+  BOOLEAN                       VagueMatched2;
+
+  ASSERT (Private != NULL && Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+  ASSERT (FontInfo != NULL);
+
+  //
+  // Matched flag represents an exactly match; VagueMatched1 repensents a RESIZE
+  // or RESTYLE match; VagueMatched2 represents a RESIZE | RESTYLE match.
+  //
+  Matched           = FALSE;
+  VagueMatched1     = FALSE;
+  VagueMatched2     = FALSE;
+
+  Mask              = 0;
+  GlobalFontBackup1 = NULL;
+  GlobalFontBackup2 = NULL;
+
+  // The process of where the system default should be supplied instead of
+  // the specified font info beyonds this function's scope.
+  //
+  if (FontInfoMask != NULL) {
+    Mask = *FontInfoMask & (~SYS_FONT_INFO_MASK);
+  }
+
+  //
+  // If not NULL, FontHandle points to the next node of the last searched font
+  // node by previous call.
+  //
+  if (FontHandle == NULL) {
+    Link = Private->FontInfoList.ForwardLink;
+  } else {
+    Link = (LIST_ENTRY     *) FontHandle;
+  }
+
+  for (; Link != &Private->FontInfoList; Link = Link->ForwardLink) {
+    GlobalFont = CR (Link, HII_GLOBAL_FONT_INFO, Entry, HII_GLOBAL_FONT_INFO_SIGNATURE);
+    if (FontInfoMask == NULL) {
+      if (CompareMem (GlobalFont->FontInfo, FontInfo, GlobalFont->FontInfoSize) == 0) {
+        if (GlobalFontInfo != NULL) {
+          *GlobalFontInfo = GlobalFont;
+        }
+        return TRUE;
+      }
+    } else {
+      //
+      // Check which options could be used to make a match.
+      //
+      switch (Mask) {
+      case EFI_FONT_INFO_ANY_FONT:
+        if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle &&
+            GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_ANY_STYLE:
+        if (GlobalFont->FontInfo->FontSize == FontInfo->FontSize) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_ANY_SIZE:
+        if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_ANY_SIZE | EFI_FONT_INFO_ANY_STYLE:
+        Matched   = TRUE;
+        break;
+      //
+      // If EFI_FONT_INFO_RESTYLE is specified, then the system may attempt to
+      // remove some of the specified styles to meet the style requested.
+      //
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_RESTYLE:
+        if (GlobalFont->FontInfo->FontSize == FontInfo->FontSize) {
+          if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+            Matched           = TRUE;
+          } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      //
+      // If EFI_FONT_INFO_RESIZE is specified, then the sytem may attempt to
+      // stretch or shrink a font to meet the size requested.
+      //
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_RESIZE:
+        if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+          if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+            Matched           = TRUE;
+          } else {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_RESTYLE | EFI_FONT_INFO_RESIZE:
+        if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+          if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+            Matched           = TRUE;
+          } else {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+          if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          } else {
+            VagueMatched2     = TRUE;
+            GlobalFontBackup2 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_ANY_STYLE | EFI_FONT_INFO_RESIZE:
+        if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+          Matched           = TRUE;
+        } else {
+          VagueMatched1     = TRUE;
+          GlobalFontBackup1 = GlobalFont;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_FONT | EFI_FONT_INFO_ANY_SIZE | EFI_FONT_INFO_RESTYLE:
+        if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+          Matched           = TRUE;
+        } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+          VagueMatched1     = TRUE;
+          GlobalFontBackup1 = GlobalFont;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_STYLE:
+        if ((CompareMem (
+               GlobalFont->FontInfo->FontName,
+               FontInfo->FontName,
+               StrSize (FontInfo->FontName)
+               ) == 0) &&
+            GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_STYLE | EFI_FONT_INFO_ANY_SIZE:
+        if (CompareMem (
+              GlobalFont->FontInfo->FontName,
+              FontInfo->FontName,
+              StrSize (FontInfo->FontName)
+              ) == 0) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_STYLE | EFI_FONT_INFO_RESIZE:
+        if (CompareMem (
+              GlobalFont->FontInfo->FontName,
+              FontInfo->FontName,
+              StrSize (FontInfo->FontName)
+              ) == 0) {
+          if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+            Matched           = TRUE;
+          } else {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_ANY_SIZE:
+        if ((CompareMem (
+               GlobalFont->FontInfo->FontName,
+               FontInfo->FontName,
+               StrSize (FontInfo->FontName)
+               ) == 0) &&
+            GlobalFont->FontInfo->FontStyle  == FontInfo->FontStyle) {
+          Matched = TRUE;
+        }
+        break;
+      case EFI_FONT_INFO_ANY_SIZE | EFI_FONT_INFO_RESTYLE:
+        if (CompareMem (
+              GlobalFont->FontInfo->FontName,
+              FontInfo->FontName,
+              StrSize (FontInfo->FontName)
+              ) == 0) {
+          if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+            Matched           = TRUE;
+          } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_RESTYLE:
+        if ((CompareMem (
+               GlobalFont->FontInfo->FontName,
+               FontInfo->FontName,
+               StrSize (FontInfo->FontName)
+               ) == 0) &&
+            GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+
+          if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+            Matched           = TRUE;
+          } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_RESIZE:
+        if ((CompareMem (
+               GlobalFont->FontInfo->FontName,
+               FontInfo->FontName,
+               StrSize (FontInfo->FontName)
+               ) == 0) &&
+            GlobalFont->FontInfo->FontStyle  == FontInfo->FontStyle) {
+
+          if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+            Matched           = TRUE;
+          } else {
+            VagueMatched1     = TRUE;
+            GlobalFontBackup1 = GlobalFont;
+          }
+        }
+        break;
+      case EFI_FONT_INFO_RESIZE | EFI_FONT_INFO_RESTYLE:
+        if (CompareMem (
+              GlobalFont->FontInfo->FontName,
+              FontInfo->FontName,
+              StrSize (FontInfo->FontName)
+              ) == 0) {
+          if (GlobalFont->FontInfo->FontStyle == FontInfo->FontStyle) {
+            if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+              Matched           = TRUE;
+            } else {
+              VagueMatched1     = TRUE;
+              GlobalFontBackup1 = GlobalFont;
+            }
+          } else if ((GlobalFont->FontInfo->FontStyle & FontInfo->FontStyle) == FontInfo->FontStyle) {
+            if (GlobalFont->FontInfo->FontSize  == FontInfo->FontSize) {
+              VagueMatched1     = TRUE;
+              GlobalFontBackup1 = GlobalFont;
+            } else {
+              VagueMatched2     = TRUE;
+              GlobalFontBackup2 = GlobalFont;
+            }
+          }
+        }
+        break;
+      default:
+        break;
+      }
+
+      if (Matched) {
+        if (GlobalFontInfo != NULL) {
+          *GlobalFontInfo = GlobalFont;
+        }
+        return TRUE;
+      }
+    }
+  }
+
+  if (VagueMatched1) {
+    if (GlobalFontInfo != NULL) {
+      *GlobalFontInfo = GlobalFontBackup1;
+    }
+    return TRUE;
+  } else if (VagueMatched2) {
+    if (GlobalFontInfo != NULL) {
+      *GlobalFontInfo = GlobalFontBackup2;
+    }
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Check whether the unicode represents a line break or not.
+
+  This is a internal function. Please see Section 27.2.6 of the UEFI Specification
+  for a description of the supported string format.
+
+  @param  Char                    Unicode character
+
+  @retval 0                       Yes, it forces a line break.
+  @retval 1                       Yes, it presents a line break opportunity
+  @retval 2                       Yes, it requires a line break happen before and after it.
+  @retval -1                      No, it is not a link break.
+
+**/
+INT8
+IsLineBreak (
+  IN  CHAR16    Char
+  )
+{
+  switch (Char) {
+    //
+    // Mandatory line break characters, which force a line-break
+    //
+    case 0x000A:
+    case 0x000C:
+    case 0x000D:
+    case 0x2028:
+    case 0x2029:
+      return 0;
+    //
+    // Space characters, which is taken as a line-break opportunity
+    //
+    case 0x0020:
+    case 0x1680:
+    case 0x2000:
+    case 0x2001:
+    case 0x2002:
+    case 0x2003:
+    case 0x2004:
+    case 0x2005:
+    case 0x2006:
+    case 0x2008:
+    case 0x2009:
+    case 0x200A:
+    case 0x205F:
+    //
+    // In-Word Break Opportunities
+    //
+    case 0x200B:
+      return 1;
+    //
+    // A space which is not a line-break opportunity
+    //
+    case 0x00A0:
+    case 0x202F:
+    //
+    // A hyphen which is not a line-break opportunity
+    //
+    case 0x2011:
+      return -1;
+    //
+    // Hyphen characters which describe line break opportunities after the character
+    //
+    case 0x058A:
+    case 0x2010:
+    case 0x2012:
+    case 0x2013:
+    case 0x0F0B:
+    case 0x1361:
+    case 0x17D5:
+      return 1;
+    //
+    // A hyphen which describes line break opportunities before and after them, but not between a pair of them
+    //
+    case 0x2014:
+      return 2;
+  }
+  return -1;
+}
+
+
+/**
+  Renders a string to a bitmap or to the display.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Flags                   Describes how the string is to be drawn.
+  @param  String                  Points to the null-terminated string to be
+                                  displayed.
+  @param  StringInfo              Points to the string output information,
+                                  including the color and font.  If NULL, then the
+                                  string will be output in the default system font
+                                  and color.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels   wide
+                                  and Height pixels high. The string will be drawn
+                                  onto this image and
+                                  EFI_HII_OUT_FLAG_CLIP is implied. If this points
+                                  to a NULL on entry, then a              buffer
+                                  will be allocated to hold the generated image and
+                                  the pointer updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  BltX                    Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  BltY                    Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  RowInfoArray            If this is non-NULL on entry, then on exit, this
+                                  will point to an allocated buffer    containing
+                                  row information and RowInfoArraySize will be
+                                  updated to contain the        number of elements.
+                                  This array describes the characters which were at
+                                  least partially drawn and the heights of the
+                                  rows. It is the caller's responsibility to free
+                                  this buffer.
+  @param  RowInfoArraySize        If this is non-NULL on entry, then on exit it
+                                  contains the number of elements in RowInfoArray.
+  @param  ColumnInfoArray         If this is non-NULL, then on return it will be
+                                  filled with the horizontal offset for each
+                                  character in the string on the row where it is
+                                  displayed. Non-printing characters will     have
+                                  the offset ~0. The caller is responsible to
+                                  allocate a buffer large enough so that    there
+                                  is one entry for each character in the string,
+                                  not including the null-terminator. It is possible
+                                  when character display is normalized that some
+                                  character cells overlap.
+
+  @retval EFI_SUCCESS             The string was successfully rendered.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for
+                                  RowInfoArray or Blt.
+  @retval EFI_INVALID_PARAMETER   The String or Blt was NULL.
+  @retval EFI_INVALID_PARAMETER Flags were invalid combination..
+
+**/
+EFI_STATUS
+EFIAPI
+HiiStringToImage (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  EFI_HII_OUT_FLAGS              Flags,
+  IN  CONST EFI_STRING               String,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo       OPTIONAL,
+  IN  OUT EFI_IMAGE_OUTPUT           **Blt,
+  IN  UINTN                          BltX,
+  IN  UINTN                          BltY,
+  OUT EFI_HII_ROW_INFO               **RowInfoArray    OPTIONAL,
+  OUT UINTN                          *RowInfoArraySize OPTIONAL,
+  OUT UINTN                          *ColumnInfoArray  OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  UINT8                               **GlyphBuf;
+  EFI_HII_GLYPH_INFO                  *Cell;
+  UINT8                               *Attributes;
+  EFI_IMAGE_OUTPUT                    *Image;
+  EFI_STRING                          StringPtr;
+  EFI_STRING                          StringTmp;
+  EFI_HII_ROW_INFO                    *RowInfo;
+  UINTN                               LineWidth;
+  UINTN                               LineHeight;
+  UINTN                               LineOffset;
+  UINTN                               LastLineHeight;
+  UINTN                               BaseLineOffset;
+  UINT16                              MaxRowNum;
+  UINT16                              RowIndex;
+  UINTN                               Index;
+  UINTN                               NextIndex;
+  UINTN                               Index1;
+  EFI_FONT_DISPLAY_INFO               *StringInfoOut;
+  EFI_FONT_DISPLAY_INFO               *SystemDefault;
+  EFI_FONT_HANDLE                     FontHandle;
+  EFI_STRING                          StringIn;
+  EFI_STRING                          StringIn2;
+  UINT16                              Height;
+  UINT16                              BaseLine;
+  EFI_FONT_INFO                       *FontInfo;
+  BOOLEAN                             SysFontFlag;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       Foreground;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       Background;
+  BOOLEAN                             Transparent;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       *BltBuffer;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       *BufferPtr;
+  UINTN                               RowInfoSize;
+  BOOLEAN                             LineBreak;
+  UINTN                               StrLength;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       *RowBufferPtr;
+  HII_GLOBAL_FONT_INFO                *GlobalFont;
+
+  //
+  // Check incoming parameters.
+  //
+
+  if (This == NULL || String == NULL || Blt == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*Blt == NULL) {
+    //
+    // These two flag cannot be used if Blt is NULL upon entry.
+    //
+    if ((Flags & EFI_HII_OUT_FLAG_TRANSPARENT) == EFI_HII_OUT_FLAG_TRANSPARENT) {
+      return EFI_INVALID_PARAMETER;
+    }
+    if ((Flags & EFI_HII_OUT_FLAG_CLIP) == EFI_HII_OUT_FLAG_CLIP) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  //
+  // These two flags require that EFI_HII_OUT_FLAG_CLIP be also set.
+  //
+  if ((Flags & (EFI_HII_OUT_FLAG_CLIP | EFI_HII_OUT_FLAG_CLIP_CLEAN_X)) ==  EFI_HII_OUT_FLAG_CLIP_CLEAN_X) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if ((Flags & (EFI_HII_OUT_FLAG_CLIP | EFI_HII_OUT_FLAG_CLIP_CLEAN_Y)) ==  EFI_HII_OUT_FLAG_CLIP_CLEAN_Y) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // This flag cannot be used with EFI_HII_OUT_FLAG_CLEAN_X.
+  //
+  if ((Flags & (EFI_HII_OUT_FLAG_WRAP | EFI_HII_OUT_FLAG_CLIP_CLEAN_X)) ==  (EFI_HII_OUT_FLAG_WRAP | EFI_HII_OUT_FLAG_CLIP_CLEAN_X)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*Blt == NULL) {
+    //
+    // Create a new bitmap and draw the string onto this image.
+    //
+    Image = AllocateZeroPool (sizeof (EFI_IMAGE_OUTPUT));
+    if (Image == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    Image->Width  = 800;
+    Image->Height = 600;
+    Image->Image.Bitmap = AllocateZeroPool (Image->Width * Image->Height *sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+    if (Image->Image.Bitmap == NULL) {
+      FreePool (Image);
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Other flags are not permitted when Blt is NULL.
+    //
+    Flags &= EFI_HII_OUT_FLAG_WRAP | EFI_HII_IGNORE_IF_NO_GLYPH | EFI_HII_IGNORE_LINE_BREAK;
+    *Blt = Image;
+  }
+
+  StrLength = StrLen(String);
+  GlyphBuf = (UINT8 **) AllocateZeroPool (StrLength * sizeof (UINT8 *));
+  ASSERT (GlyphBuf != NULL);
+  Cell = (EFI_HII_GLYPH_INFO *) AllocateZeroPool (StrLength * sizeof (EFI_HII_GLYPH_INFO));
+  ASSERT (Cell != NULL);
+  Attributes = (UINT8 *) AllocateZeroPool (StrLength * sizeof (UINT8));
+  ASSERT (Attributes != NULL);
+
+  RowInfo       = NULL;
+  Status        = EFI_SUCCESS;
+  StringIn2     = NULL;
+  SystemDefault = NULL;
+  StringIn      = NULL;
+
+  //
+  // Calculate the string output information, including specified color and font .
+  // If StringInfo does not points to system font info, it must indicate an existing
+  // EFI_FONT_INFO.
+  //
+  StringInfoOut = NULL;
+  FontHandle    = NULL;
+  Private       = HII_FONT_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  SysFontFlag   = IsSystemFontInfo (Private, (EFI_FONT_DISPLAY_INFO *) StringInfo, &SystemDefault, NULL);
+
+  if (SysFontFlag) {
+    ASSERT (SystemDefault != NULL);
+    FontInfo   = NULL;
+    Height     = SystemDefault->FontInfo.FontSize;
+    BaseLine   = SystemDefault->FontInfo.FontSize;
+    Foreground = SystemDefault->ForegroundColor;
+    Background = SystemDefault->BackgroundColor;
+
+  } else {
+    //
+    //  StringInfo must not be NULL if it is not system info.
+    //
+    ASSERT (StringInfo != NULL);
+    Status = HiiGetFontInfo (This, &FontHandle, (EFI_FONT_DISPLAY_INFO *) StringInfo, &StringInfoOut, NULL);
+    if (Status == EFI_NOT_FOUND) {
+      //
+      // The specified EFI_FONT_DISPLAY_INFO does not exist in current database.
+      // Use the system font instead. Still use the color specified by StringInfo.
+      //
+      SysFontFlag = TRUE;
+      FontInfo    = NULL;
+      Height      = SystemDefault->FontInfo.FontSize;
+      BaseLine    = SystemDefault->FontInfo.FontSize;
+      Foreground  = ((EFI_FONT_DISPLAY_INFO *) StringInfo)->ForegroundColor;
+      Background  = ((EFI_FONT_DISPLAY_INFO *) StringInfo)->BackgroundColor;
+
+    } else if (Status == EFI_SUCCESS) {
+      FontInfo   = &StringInfoOut->FontInfo;
+      IsFontInfoExisted (Private, FontInfo, NULL, NULL, &GlobalFont);
+      Height     = GlobalFont->FontPackage->Height;
+      BaseLine   = GlobalFont->FontPackage->BaseLine;
+      Foreground = StringInfoOut->ForegroundColor;
+      Background = StringInfoOut->BackgroundColor;
+    } else {
+      goto Exit;
+    }
+  }
+  
+  //
+  // Use the maxinum height of font as the base line.
+  // And, use the maxinum height as line height.
+  //
+  LineHeight     = Height;
+  LastLineHeight = Height;
+  BaseLineOffset = Height - BaseLine;
+  
+  //
+  // Parse the string to be displayed to drop some ignored characters.
+  //
+
+  StringPtr = String;
+
+  //
+  // Ignore line-break characters only. Hyphens or dash character will be displayed
+  // without line-break opportunity.
+  //
+  if ((Flags & EFI_HII_IGNORE_LINE_BREAK) == EFI_HII_IGNORE_LINE_BREAK) {
+    StringIn = AllocateZeroPool (StrSize (StringPtr));
+    if (StringIn == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    StringTmp = StringIn;
+    while (*StringPtr != 0) {
+      if (IsLineBreak (*StringPtr) == 0) {
+        StringPtr++;
+      } else {
+        *StringTmp++ = *StringPtr++;
+      }
+    }
+    *StringTmp = 0;
+    StringPtr  = StringIn;
+  }
+  //
+  // If EFI_HII_IGNORE_IF_NO_GLYPH is set, then characters which have no glyphs
+  // are not drawn. Otherwise they are replaced wth Unicode character 0xFFFD.
+  //
+  StringIn2  = AllocateZeroPool (StrSize (StringPtr));
+  if (StringIn2 == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  Index     = 0;
+  StringTmp = StringIn2;
+  StrLength = StrLen(StringPtr);
+  while (*StringPtr != 0 && Index < StrLength) {
+    if (IsLineBreak (*StringPtr) == 0) {
+      *StringTmp++ = *StringPtr++;
+      Index++;
+      continue;
+    }
+    
+    Status = GetGlyphBuffer (Private, *StringPtr, FontInfo, &GlyphBuf[Index], &Cell[Index], &Attributes[Index]);
+    if (Status == EFI_NOT_FOUND) {
+      if ((Flags & EFI_HII_IGNORE_IF_NO_GLYPH) == EFI_HII_IGNORE_IF_NO_GLYPH) {
+        GlyphBuf[Index] = NULL;
+        ZeroMem (&Cell[Index], sizeof (Cell[Index]));
+        Status = EFI_SUCCESS;
+      } else {
+        //
+        // Unicode 0xFFFD must exist in current hii database if this flag is not set.
+        //
+        Status = GetGlyphBuffer (
+                   Private,
+                   REPLACE_UNKNOWN_GLYPH,
+                   FontInfo,
+                   &GlyphBuf[Index],
+                   &Cell[Index],
+                   &Attributes[Index]
+                   );
+        if (EFI_ERROR (Status)) {
+          Status = EFI_INVALID_PARAMETER;
+        }
+      }
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    *StringTmp++ = *StringPtr++;
+    Index++;
+  }
+  *StringTmp = 0;
+  StringPtr  = StringIn2;
+
+  //
+  // Draw the string according to the specified EFI_HII_OUT_FLAGS and Blt.
+  // If Blt is not NULL, then EFI_HII_OUT_FLAG_CLIP is implied, render this string
+  // to an existing image (bitmap or screen depending on flags) pointed by "*Blt".
+  // Otherwise render this string to a new allocated image and output it.
+  //
+  Image     = *Blt;
+  BufferPtr = Image->Image.Bitmap + Image->Width * BltY + BltX;
+  if (Image->Height < BltY) {
+    //
+    // the top edge of the image should be in Image resolution scope.
+    //
+    Status = EFI_INVALID_PARAMETER;
+    goto Exit;
+  }
+  MaxRowNum = (UINT16) ((Image->Height - BltY) / Height);
+  if ((Image->Height - BltY) % Height != 0) {
+    LastLineHeight = (Image->Height - BltY) % Height;
+    MaxRowNum++;
+  }
+
+  RowInfo = (EFI_HII_ROW_INFO *) AllocateZeroPool (MaxRowNum * sizeof (EFI_HII_ROW_INFO));
+  if (RowInfo == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Format the glyph buffer according to flags.
+  //
+  Transparent = (BOOLEAN) ((Flags & EFI_HII_OUT_FLAG_TRANSPARENT) == EFI_HII_OUT_FLAG_TRANSPARENT ? TRUE : FALSE);
+
+  for (RowIndex = 0, Index = 0; RowIndex < MaxRowNum && StringPtr[Index] != 0; ) {
+    LineWidth      = 0;
+    LineBreak      = FALSE;
+
+    //
+    // Clip the final row if the row's bottom-most on pixel cannot fit when
+    // EFI_HII_OUT_FLAG_CLEAN_Y is set.
+    //
+    if (RowIndex == MaxRowNum - 1) {
+      if ((Flags & EFI_HII_OUT_FLAG_CLIP_CLEAN_Y) == EFI_HII_OUT_FLAG_CLIP_CLEAN_Y && LastLineHeight < LineHeight ) {
+        //
+        // Don't draw at all if the row's bottom-most on pixel cannot fit.
+        //
+        break;
+      }
+      LineHeight = LastLineHeight;
+    }
+
+    //
+    // Calculate how many characters there are in a row.
+    //
+    RowInfo[RowIndex].StartIndex = Index;
+
+    while (LineWidth + BltX < Image->Width && StringPtr[Index] != 0) {
+      if ((Flags & EFI_HII_IGNORE_LINE_BREAK) == 0 &&
+           IsLineBreak (StringPtr[Index]) == 0) {
+        //
+        // It forces a line break that ends this row.
+        //
+        Index++;
+        LineBreak = TRUE;
+        break;
+      }
+
+      //
+      // If the glyph of the character is existing, then accumulate the actual printed width
+      //
+      LineWidth += (UINTN) Cell[Index].AdvanceX;
+
+      Index++;
+    }
+
+    //
+    // Record index of next char.
+    //
+    NextIndex = Index;
+    //
+    // Return to the previous char.
+    //
+    Index--;
+    if (LineBreak && Index > 0 ) {
+      //
+      // Return the previous non line break char.
+      //
+      Index --;
+    }
+
+    //
+    // If this character is the last character of a row, we need not
+    // draw its (AdvanceX - Width - OffsetX) for next character.
+    //
+    LineWidth -= (UINTN) (Cell[Index].AdvanceX - Cell[Index].Width - Cell[Index].OffsetX);
+
+    //
+    // Clip the right-most character if cannot fit when EFI_HII_OUT_FLAG_CLEAN_X is set.
+    //
+    if (LineWidth + BltX <= Image->Width ||
+      (LineWidth + BltX > Image->Width && (Flags & EFI_HII_OUT_FLAG_CLIP_CLEAN_X) == 0)) {
+      //
+      // Record right-most character in RowInfo even if it is partially displayed.
+      //
+      RowInfo[RowIndex].EndIndex       = Index;
+      RowInfo[RowIndex].LineWidth      = LineWidth;
+      RowInfo[RowIndex].LineHeight     = LineHeight;
+      RowInfo[RowIndex].BaselineOffset = BaseLineOffset;
+    } else {
+      //
+      // When EFI_HII_OUT_FLAG_CLEAN_X is set, it will not draw a character
+      // if its right-most on pixel cannot fit.
+      //
+      if (Index > RowInfo[RowIndex].StartIndex) {
+        //
+        // Don't draw the last char on this row. And, don't draw the second last char (AdvanceX - Width - OffsetX).
+        //
+        LineWidth -= (UINTN) (Cell[Index].Width + Cell[Index].OffsetX);
+        LineWidth -= (UINTN) (Cell[Index - 1].AdvanceX - Cell[Index - 1].Width - Cell[Index - 1].OffsetX);
+        RowInfo[RowIndex].EndIndex       = Index - 1;
+        RowInfo[RowIndex].LineWidth      = LineWidth;
+        RowInfo[RowIndex].LineHeight     = LineHeight;
+        RowInfo[RowIndex].BaselineOffset = BaseLineOffset;
+      } else {
+        //
+        // There is no enough column to draw any character, so set current line width to zero.
+        // And go to draw Next line if LineBreak is set.
+        //
+        RowInfo[RowIndex].LineWidth      = 0;
+        goto NextLine;
+      }
+    }
+
+    //
+    // EFI_HII_OUT_FLAG_WRAP will wrap the text at the right-most line-break
+    // opportunity prior to a character whose right-most extent would exceed Width.
+    // Search the right-most line-break opportunity here.
+    //
+    if ((Flags & EFI_HII_OUT_FLAG_WRAP) == EFI_HII_OUT_FLAG_WRAP && 
+        (RowInfo[RowIndex].LineWidth + BltX > Image->Width || StringPtr[NextIndex] != 0) && 
+        !LineBreak) {
+      if ((Flags & EFI_HII_IGNORE_LINE_BREAK) == 0) {
+        LineWidth = RowInfo[RowIndex].LineWidth;
+        for (Index1 = RowInfo[RowIndex].EndIndex; Index1 >= RowInfo[RowIndex].StartIndex; Index1--) {
+          if (Index1 == RowInfo[RowIndex].EndIndex) {
+            LineWidth -= (Cell[Index1].Width + Cell[Index1].OffsetX);
+          } else {
+            LineWidth -= Cell[Index1].AdvanceX;
+          }
+          if (IsLineBreak (StringPtr[Index1]) > 0) {
+            LineBreak = TRUE;
+            if (Index1 > RowInfo[RowIndex].StartIndex) {
+              RowInfo[RowIndex].EndIndex = Index1 - 1;
+            }
+            //
+            // relocate to the character after the right-most line break opportunity of this line
+            //
+            NextIndex = Index1 + 1;
+            break;
+          }
+          //
+          // If don't find a line break opportunity from EndIndex to StartIndex,
+          // then jump out.
+          //
+          if (Index1 == RowInfo[RowIndex].StartIndex)
+            break;
+        }
+
+        //
+        // Update LineWidth to the real width
+        //
+        if (IsLineBreak (StringPtr[Index1]) > 0) {
+          if (Index1 == RowInfo[RowIndex].StartIndex) {
+            LineWidth = 0;
+          } else {
+            LineWidth -= (UINTN) (Cell[Index1 - 1].AdvanceX - Cell[Index1 - 1].Width - Cell[Index1 - 1].OffsetX);
+          }
+          RowInfo[RowIndex].LineWidth = LineWidth;
+        }
+      }
+      //
+      // If no line-break opportunity can be found, then the text will
+      // behave as if EFI_HII_OUT_FLAG_CLEAN_X is set.
+      //
+      if (!LineBreak) {
+        LineWidth = RowInfo[RowIndex].LineWidth;
+        Index1    = RowInfo[RowIndex].EndIndex;
+        if (LineWidth + BltX > Image->Width) {
+          if (Index1 > RowInfo[RowIndex].StartIndex) {
+            //
+            // Don't draw the last char on this row. And, don't draw the second last char (AdvanceX - Width - OffsetX).
+            //
+            LineWidth -= (UINTN) (Cell[Index1].Width + Cell[Index1].OffsetX);
+            LineWidth -= (UINTN) (Cell[Index1 - 1].AdvanceX - Cell[Index1 - 1].Width - Cell[Index1 - 1].OffsetX);
+            RowInfo[RowIndex].EndIndex       = Index1 - 1;
+            RowInfo[RowIndex].LineWidth      = LineWidth;
+          } else {
+            //
+            // There is no enough column to draw any character, so set current line width to zero.
+            // And go to draw Next line if LineBreak is set.
+            //
+            RowInfo[RowIndex].LineWidth = 0;
+            goto NextLine;
+          }
+        }
+      }
+    }
+    
+    //
+    // LineWidth can't exceed Image width.
+    //
+    if (RowInfo[RowIndex].LineWidth + BltX > Image->Width) {
+      RowInfo[RowIndex].LineWidth = Image->Width - BltX;
+    }
+
+    //
+    // Draw it to screen or existing bitmap depending on whether
+    // EFI_HII_DIRECT_TO_SCREEN is set.
+    //
+    LineOffset = 0;
+    if ((Flags & EFI_HII_DIRECT_TO_SCREEN) == EFI_HII_DIRECT_TO_SCREEN) {
+      BltBuffer = NULL;
+      if (RowInfo[RowIndex].LineWidth != 0) {
+        BltBuffer = AllocateZeroPool (RowInfo[RowIndex].LineWidth * RowInfo[RowIndex].LineHeight * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+        if (BltBuffer == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto Exit;
+        }
+        //
+        // Set BufferPtr to Origin by adding baseline to the starting position.
+        //
+        BufferPtr = BltBuffer + BaseLine * RowInfo[RowIndex].LineWidth;
+      }
+      for (Index1 = RowInfo[RowIndex].StartIndex; Index1 <= RowInfo[RowIndex].EndIndex; Index1++) {
+        if (RowInfo[RowIndex].LineWidth > 0 && RowInfo[RowIndex].LineWidth > LineOffset) {
+          //
+          // Only BLT these character which have corrsponding glyph in font basebase.
+          //
+          GlyphToImage (
+            GlyphBuf[Index1],
+            Foreground,
+            Background,
+            (UINT16) RowInfo[RowIndex].LineWidth,
+            BaseLine,
+            RowInfo[RowIndex].LineWidth - LineOffset,
+            RowInfo[RowIndex].LineHeight,
+            Transparent,
+            &Cell[Index1],
+            Attributes[Index1],
+            &BufferPtr
+          );
+        }
+        if (ColumnInfoArray != NULL) {
+          if ((GlyphBuf[Index1] == NULL && Cell[Index1].AdvanceX == 0) 
+              || RowInfo[RowIndex].LineWidth == 0) {
+            *ColumnInfoArray = (UINTN) ~0;
+          } else {
+            *ColumnInfoArray = LineOffset + Cell[Index1].OffsetX + BltX;
+          }
+          ColumnInfoArray++;
+        }
+        LineOffset += Cell[Index1].AdvanceX;
+      }
+
+      if (BltBuffer != NULL) {
+        Status = Image->Image.Screen->Blt (
+                                        Image->Image.Screen,
+                                        BltBuffer,
+                                        EfiBltBufferToVideo,
+                                        0,
+                                        0,
+                                        BltX,
+                                        BltY,
+                                        RowInfo[RowIndex].LineWidth,
+                                        RowInfo[RowIndex].LineHeight,
+                                        0
+                                        );
+        if (EFI_ERROR (Status)) {
+          FreePool (BltBuffer);
+          goto Exit;
+        }
+  
+        FreePool (BltBuffer);
+      }
+    } else {
+      //
+      // Save the starting position for calculate the starting postition of next row. 
+      //
+      RowBufferPtr = BufferPtr;
+      //
+      // Set BufferPtr to Origin by adding baseline to the starting position.
+      //
+      BufferPtr = BufferPtr + BaseLine * Image->Width;
+      for (Index1 = RowInfo[RowIndex].StartIndex; Index1 <= RowInfo[RowIndex].EndIndex; Index1++) {
+        if (RowInfo[RowIndex].LineWidth > 0 && RowInfo[RowIndex].LineWidth > LineOffset) {
+          //
+          // Only BLT these character which have corrsponding glyph in font basebase.
+          //
+          GlyphToImage (
+            GlyphBuf[Index1],
+            Foreground,
+            Background,
+            Image->Width,
+            BaseLine,
+            RowInfo[RowIndex].LineWidth - LineOffset,
+            RowInfo[RowIndex].LineHeight,
+            Transparent,
+            &Cell[Index1],
+            Attributes[Index1],
+            &BufferPtr
+          );
+        }
+        if (ColumnInfoArray != NULL) {
+          if ((GlyphBuf[Index1] == NULL && Cell[Index1].AdvanceX == 0) 
+              || RowInfo[RowIndex].LineWidth == 0) {
+            *ColumnInfoArray = (UINTN) ~0;
+          } else {
+            *ColumnInfoArray = LineOffset + Cell[Index1].OffsetX + BltX;
+          }
+          ColumnInfoArray++;
+        }
+        LineOffset += Cell[Index1].AdvanceX;
+      }
+
+      //
+      // Jump to starting position of next row.
+      //
+      if (RowIndex == 0) {
+        BufferPtr = RowBufferPtr - BltX + LineHeight * Image->Width;
+      } else {
+        BufferPtr = RowBufferPtr + LineHeight * Image->Width;
+      }
+    }
+
+NextLine:
+    //
+    // Recalculate the start point of X/Y axis to draw multi-lines with the order of top-to-down
+    //
+    BltX = 0;
+    BltY += RowInfo[RowIndex].LineHeight;
+
+    RowIndex++;
+    Index = NextIndex;
+
+    if (!LineBreak) {
+      //
+      // If there is not a mandatory line break or line break opportunity, only render one line to image
+      //
+      break;
+    }
+  }
+
+  //
+  // Write output parameters.
+  //
+  RowInfoSize = RowIndex * sizeof (EFI_HII_ROW_INFO);
+  if (RowInfoArray != NULL) {
+    if (RowInfoSize > 0) {
+      *RowInfoArray = AllocateZeroPool (RowInfoSize);
+      if (*RowInfoArray == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Exit;
+      }
+      CopyMem (*RowInfoArray, RowInfo, RowInfoSize);
+    } else {
+      *RowInfoArray = NULL;
+    }
+  }
+  if (RowInfoArraySize != NULL) {
+    *RowInfoArraySize = RowIndex;
+  }
+
+  Status = EFI_SUCCESS;
+
+Exit:
+
+  for (Index = 0; Index < StrLength; Index++) {
+    if (GlyphBuf[Index] != NULL) {
+      FreePool (GlyphBuf[Index]);
+    }
+  }
+  if (StringIn != NULL) {
+    FreePool (StringIn);
+  }
+  if (StringIn2 != NULL) {
+    FreePool (StringIn2);
+  }
+  if (StringInfoOut != NULL) {
+    FreePool (StringInfoOut);
+  }
+  if (RowInfo != NULL) {
+    FreePool (RowInfo);
+  }
+  if (SystemDefault != NULL) {
+    FreePool (SystemDefault);
+  }
+  if (GlyphBuf != NULL) {
+    FreePool (GlyphBuf);
+  }
+  if (Cell != NULL) {
+    FreePool (Cell);
+  }
+  if (Attributes != NULL) {
+    FreePool (Attributes);
+  }
+
+  return Status;
+}
+
+
+/**
+  Render a string to a bitmap or the screen containing the contents of the specified string.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Flags                   Describes how the string is to be drawn.
+  @param  PackageList             The package list in the HII database to search
+                                  for the specified string.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  Language                Points to the language for the retrieved string.
+                                  If NULL, then the current system language is
+                                  used.
+  @param  StringInfo              Points to the string output information,
+                                  including the color and font.  If NULL, then the
+                                  string will be output in the default system font
+                                  and color.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels   wide
+                                  and Height pixels high. The string will be drawn
+                                  onto this image and
+                                  EFI_HII_OUT_FLAG_CLIP is implied. If this points
+                                  to a NULL on entry, then a              buffer
+                                  will be allocated to hold the generated image and
+                                  the pointer updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  BltX                    Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  BltY                    Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  RowInfoArray            If this is non-NULL on entry, then on exit, this
+                                  will point to an allocated buffer    containing
+                                  row information and RowInfoArraySize will be
+                                  updated to contain the        number of elements.
+                                  This array describes the characters which were at
+                                  least partially drawn and the heights of the
+                                  rows. It is the caller's responsibility to free
+                                  this buffer.
+  @param  RowInfoArraySize        If this is non-NULL on entry, then on exit it
+                                  contains the number of elements in RowInfoArray.
+  @param  ColumnInfoArray         If this is non-NULL, then on return it will be
+                                  filled with the horizontal offset for each
+                                  character in the string on the row where it is
+                                  displayed. Non-printing characters will     have
+                                  the offset ~0. The caller is responsible to
+                                  allocate a buffer large enough so that    there
+                                  is one entry for each character in the string,
+                                  not including the null-terminator. It is possible
+                                  when character display is normalized that some
+                                  character cells overlap.
+
+  @retval EFI_SUCCESS             The string was successfully rendered.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for
+                                  RowInfoArray or Blt.
+  @retval EFI_INVALID_PARAMETER  The Blt or PackageList was NULL.
+  @retval EFI_INVALID_PARAMETER  Flags were invalid combination.
+  @retval EFI_NOT_FOUND         The specified PackageList is not in the Database or the stringid is not 
+                          in the specified PackageList. 
+
+**/
+EFI_STATUS
+EFIAPI
+HiiStringIdToImage (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  EFI_HII_OUT_FLAGS              Flags,
+  IN  EFI_HII_HANDLE                 PackageList,
+  IN  EFI_STRING_ID                  StringId,
+  IN  CONST CHAR8*                   Language,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo       OPTIONAL,
+  IN  OUT EFI_IMAGE_OUTPUT           **Blt,
+  IN  UINTN                          BltX,
+  IN  UINTN                          BltY,
+  OUT EFI_HII_ROW_INFO               **RowInfoArray    OPTIONAL,
+  OUT UINTN                          *RowInfoArraySize OPTIONAL,
+  OUT UINTN                          *ColumnInfoArray  OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  EFI_HII_STRING_PROTOCOL             *HiiString;
+  EFI_STRING                          String;
+  UINTN                               StringSize;
+  UINTN                               FontLen;
+  UINTN                               NameSize;
+  EFI_FONT_INFO                       *StringFontInfo;
+  EFI_FONT_DISPLAY_INFO               *NewStringInfo;
+  CHAR8                               TempSupportedLanguages;
+  CHAR8                               *SupportedLanguages;
+  UINTN                               SupportedLanguagesSize;
+  CHAR8                               *CurrentLanguage;
+  CHAR8                               *BestLanguage;
+
+  if (This == NULL || PackageList == NULL || Blt == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Initialize string pointers to be NULL
+  //
+  SupportedLanguages = NULL;
+  CurrentLanguage    = NULL;
+  BestLanguage       = NULL;
+  String             = NULL;
+  StringFontInfo     = NULL;
+  NewStringInfo      = NULL;
+
+  //
+  // Get the string to be displayed.
+  //
+  Private   = HII_FONT_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  HiiString = &Private->HiiString;
+
+  //
+  // Get the size of supported language.
+  //
+  SupportedLanguagesSize = 0;
+  Status = HiiString->GetLanguages (
+                        HiiString,
+                        PackageList,
+                        &TempSupportedLanguages,
+                        &SupportedLanguagesSize
+                        );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  SupportedLanguages = AllocatePool (SupportedLanguagesSize);
+  if (SupportedLanguages == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = HiiString->GetLanguages (
+                        HiiString,
+                        PackageList,
+                        SupportedLanguages,
+                        &SupportedLanguagesSize
+                        );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+ 
+  if (Language == NULL) {
+    Language = "";
+  }
+  GetEfiGlobalVariable2 (L"PlatformLang", (VOID**)&CurrentLanguage, NULL);
+  BestLanguage = GetBestLanguage (
+                   SupportedLanguages,
+                   FALSE,
+                   Language,
+                   (CurrentLanguage == NULL) ? CurrentLanguage : "",
+                   (CHAR8 *) PcdGetPtr (PcdUefiVariableDefaultPlatformLang),
+                   NULL
+                   );
+  if (BestLanguage == NULL) {
+    Status = EFI_NOT_FOUND;
+    goto Exit;
+  }
+    
+  StringSize = MAX_STRING_LENGTH;
+  String = (EFI_STRING) AllocateZeroPool (StringSize);
+  if (String == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  Status = HiiString->GetString (
+                        HiiString,
+                        BestLanguage,
+                        PackageList,
+                        StringId,
+                        String,
+                        &StringSize,
+                        &StringFontInfo
+                        );
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    FreePool (String);
+    String = (EFI_STRING) AllocateZeroPool (StringSize);
+    if (String == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    Status = HiiString->GetString (
+                          HiiString,
+                          BestLanguage,
+                          PackageList,
+                          StringId,
+                          String,
+                          &StringSize,
+                          NULL
+                          );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+    
+  //
+  // When StringInfo specifies that string will be output in the system default font and color,
+  // use particular stringfontinfo described in string package instead if exists. 
+  // StringFontInfo equals NULL means system default font attaches with the string block.
+  //
+  if (StringFontInfo != NULL && IsSystemFontInfo (Private, (EFI_FONT_DISPLAY_INFO *) StringInfo, NULL, NULL)) {
+    NameSize = StrSize (StringFontInfo->FontName);
+    FontLen = sizeof (EFI_FONT_DISPLAY_INFO) - sizeof (CHAR16) + NameSize;
+    NewStringInfo = AllocateZeroPool (FontLen);
+    if (NewStringInfo == NULL) {      
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    NewStringInfo->FontInfoMask       = EFI_FONT_INFO_SYS_FORE_COLOR | EFI_FONT_INFO_SYS_BACK_COLOR;
+    NewStringInfo->FontInfo.FontStyle = StringFontInfo->FontStyle;
+    NewStringInfo->FontInfo.FontSize  = StringFontInfo->FontSize;    
+    StrCpyS (NewStringInfo->FontInfo.FontName, NameSize / sizeof (CHAR16), StringFontInfo->FontName);
+  
+    Status = HiiStringToImage (
+               This, 
+               Flags, 
+               String, 
+               NewStringInfo, 
+               Blt, 
+               BltX, 
+               BltY, 
+               RowInfoArray,
+               RowInfoArraySize,
+               ColumnInfoArray
+               );
+    goto Exit;
+  }
+
+  Status = HiiStringToImage (
+           This,
+           Flags,
+           String,
+           StringInfo,
+           Blt,
+           BltX,
+           BltY,
+           RowInfoArray,
+           RowInfoArraySize,
+           ColumnInfoArray
+           );
+
+Exit:
+  if (SupportedLanguages != NULL) {
+    FreePool (SupportedLanguages);
+  }
+  if (CurrentLanguage != NULL) {
+    FreePool (CurrentLanguage);
+  }
+  if (BestLanguage != NULL) {
+    FreePool (BestLanguage);
+  }
+  if (String != NULL) {
+    FreePool (String);
+  }
+  if (StringFontInfo != NULL) {
+    FreePool (StringFontInfo);
+  }
+  if (NewStringInfo != NULL) {
+    FreePool (NewStringInfo);
+  }
+
+  return Status;
+}
+
+
+/**
+  Convert the glyph for a single character into a bitmap.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Char                    Character to retrieve.
+  @param  StringInfo              Points to the string font and color information
+                                  or NULL if the string should use the default
+                                  system font and color.
+  @param  Blt                     Thus must point to a NULL on entry. A buffer will
+                                  be allocated to hold the output and the pointer
+                                  updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  Baseline                Number of pixels from the bottom of the bitmap to
+                                  the baseline.
+
+  @retval EFI_SUCCESS             Glyph bitmap created.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate the output buffer Blt.
+  @retval EFI_WARN_UNKNOWN_GLYPH  The glyph was unknown and was replaced with the
+                                  glyph for Unicode character 0xFFFD.
+  @retval EFI_INVALID_PARAMETER   Blt is NULL or *Blt is not NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetGlyph (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  CHAR16                         Char,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo,
+  OUT EFI_IMAGE_OUTPUT               **Blt,
+  OUT UINTN                          *Baseline OPTIONAL
+  )
+{
+  EFI_STATUS                         Status;
+  HII_DATABASE_PRIVATE_DATA          *Private;
+  EFI_IMAGE_OUTPUT                   *Image;
+  UINT8                              *GlyphBuffer;
+  EFI_FONT_DISPLAY_INFO              *SystemDefault;
+  EFI_FONT_DISPLAY_INFO              *StringInfoOut;
+  BOOLEAN                            Default;
+  EFI_FONT_HANDLE                    FontHandle;
+  EFI_STRING                         String;
+  EFI_HII_GLYPH_INFO                 Cell;
+  EFI_FONT_INFO                      *FontInfo;
+  UINT8                              Attributes;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      Foreground;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      Background;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      *BltBuffer;
+  UINT16                             BaseLine;
+
+  if (This == NULL || Blt == NULL || *Blt != NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = HII_FONT_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  Default       = FALSE;
+  Image         = NULL;
+  SystemDefault = NULL;
+  FontHandle    = NULL;
+  String        = NULL;
+  GlyphBuffer   = NULL;
+  StringInfoOut = NULL;
+  FontInfo      = NULL;
+
+  ZeroMem (&Foreground, sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+  ZeroMem (&Background, sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+
+  Default = IsSystemFontInfo (Private, (EFI_FONT_DISPLAY_INFO *) StringInfo, &SystemDefault, NULL);
+
+  if (!Default) {
+    //
+    // Find out a EFI_FONT_DISPLAY_INFO which could display the character in
+    // the specified color and font.
+    //
+    String = (EFI_STRING) AllocateZeroPool (sizeof (CHAR16) * 2);
+    if (String == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    *String = Char;
+    *(String + 1) = 0;
+
+    Status = HiiGetFontInfo (This, &FontHandle, StringInfo, &StringInfoOut, String);
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+    ASSERT (StringInfoOut != NULL);
+    FontInfo   = &StringInfoOut->FontInfo;
+    Foreground = StringInfoOut->ForegroundColor;
+    Background = StringInfoOut->BackgroundColor;
+  } else {
+    ASSERT (SystemDefault != NULL);
+    Foreground = SystemDefault->ForegroundColor;
+    Background = SystemDefault->BackgroundColor;
+  }
+
+  Status = GetGlyphBuffer (Private, Char, FontInfo, &GlyphBuffer, &Cell, &Attributes);
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  Image = (EFI_IMAGE_OUTPUT *) AllocateZeroPool (sizeof (EFI_IMAGE_OUTPUT));
+  if (Image == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+  Image->Width   = Cell.Width;
+  Image->Height  = Cell.Height;
+
+  if (Image->Width * Image->Height > 0) {
+    Image->Image.Bitmap = AllocateZeroPool (Image->Width * Image->Height * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+    if (Image->Image.Bitmap == NULL) {
+      FreePool (Image);
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    //
+    // Set BaseLine to the char height.
+    //
+    BaseLine  = (UINT16) (Cell.Height + Cell.OffsetY);
+    //
+    // Set BltBuffer to the position of Origin. 
+    //
+    BltBuffer = Image->Image.Bitmap + (Cell.Height + Cell.OffsetY) * Image->Width - Cell.OffsetX;
+    GlyphToImage (
+      GlyphBuffer,
+      Foreground,
+      Background,
+      Image->Width,
+      BaseLine,
+      Cell.Width + Cell.OffsetX,
+      BaseLine - Cell.OffsetY,
+      FALSE,
+      &Cell,
+      Attributes,
+      &BltBuffer
+      );
+  }
+
+  *Blt = Image;
+  if (Baseline != NULL) {
+    *Baseline = Cell.OffsetY;
+  }
+
+  Status = EFI_SUCCESS;
+
+Exit:
+
+  if (Status == EFI_NOT_FOUND) {
+    //
+    // Glyph is unknown and replaced with the glyph for unicode character 0xFFFD
+    //
+    if (Char != REPLACE_UNKNOWN_GLYPH) {
+      Status = HiiGetGlyph (This, REPLACE_UNKNOWN_GLYPH, StringInfo, Blt, Baseline);
+      if (!EFI_ERROR (Status)) {
+        Status = EFI_WARN_UNKNOWN_GLYPH;
+      }
+    } else {
+      Status = EFI_WARN_UNKNOWN_GLYPH;
+    }
+  }
+
+  if (SystemDefault != NULL) {
+   FreePool (SystemDefault);
+  }
+  if (StringInfoOut != NULL) {
+    FreePool (StringInfoOut);
+  }
+  if (String != NULL) {
+    FreePool (String);
+  }
+  if (GlyphBuffer != NULL) {
+    FreePool (GlyphBuffer);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function iterates through fonts which match the specified font, using
+  the specified criteria. If String is non-NULL, then all of the characters in
+  the string must exist in order for a candidate font to be returned.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  FontHandle              On entry, points to the font handle returned by a
+                                   previous call to GetFontInfo() or NULL to start
+                                  with the  first font. On return, points to the
+                                  returned font handle or points to NULL if there
+                                  are no more matching fonts.
+  @param  StringInfoIn            Upon entry, points to the font to return information
+                                  about. If NULL, then the information about the system
+                                  default font will be returned.
+  @param  StringInfoOut           Upon return, contains the matching font's information.
+                                  If NULL, then no information is returned. This buffer
+                                  is allocated with a call to the Boot Service AllocatePool().
+                                  It is the caller's responsibility to call the Boot 
+                                  Service FreePool() when the caller no longer requires
+                                  the contents of StringInfoOut.
+  @param  String                  Points to the string which will be tested to
+                                  determine  if all characters are available. If
+                                  NULL, then any font  is acceptable.
+
+  @retval EFI_SUCCESS             Matching font returned successfully.
+  @retval EFI_NOT_FOUND           No matching font was found.
+  @retval EFI_INVALID_PARAMETER  StringInfoIn->FontInfoMask is an invalid combination.
+  @retval EFI_OUT_OF_RESOURCES    There were insufficient resources to complete the
+                                  request.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetFontInfo (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  OUT   EFI_FONT_HANDLE          *FontHandle,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfoIn, OPTIONAL
+  OUT       EFI_FONT_DISPLAY_INFO    **StringInfoOut,
+  IN  CONST EFI_STRING               String OPTIONAL
+  )
+{
+  HII_DATABASE_PRIVATE_DATA          *Private;
+  EFI_STATUS                         Status;
+  EFI_FONT_DISPLAY_INFO              *SystemDefault;
+  EFI_FONT_DISPLAY_INFO              InfoOut;
+  UINTN                              StringInfoOutLen;
+  EFI_FONT_INFO                      *FontInfo;
+  HII_GLOBAL_FONT_INFO               *GlobalFont;
+  EFI_STRING                         StringIn;
+  EFI_FONT_HANDLE                    LocalFontHandle;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StringInfoOutLen = 0;
+  FontInfo        = NULL;
+  SystemDefault   = NULL;
+  LocalFontHandle = NULL;
+  if (FontHandle != NULL) {
+    LocalFontHandle = *FontHandle;
+  }
+
+  Private = HII_FONT_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Already searched to the end of the whole list, return directly.
+  //
+  if (LocalFontHandle == &Private->FontInfoList) {
+    LocalFontHandle = NULL;
+    Status = EFI_NOT_FOUND;
+    goto Exit;
+  }
+
+  //
+  // Get default system display info, if StringInfoIn points to
+  // system display info, return it directly.
+  //
+  if (IsSystemFontInfo (Private, (EFI_FONT_DISPLAY_INFO *) StringInfoIn, &SystemDefault, &StringInfoOutLen)) {
+    //
+    // System font is the first node. When handle is not NULL, system font can not
+    // be found any more.
+    //
+    if (LocalFontHandle == NULL) {
+      if (StringInfoOut != NULL) {
+        *StringInfoOut = AllocateCopyPool (StringInfoOutLen, SystemDefault);
+        if (*StringInfoOut == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          LocalFontHandle = NULL;
+          goto Exit;
+        }
+      }
+
+      LocalFontHandle = Private->FontInfoList.ForwardLink;
+      Status = EFI_SUCCESS;
+      goto Exit;
+    } else {
+      LocalFontHandle = NULL;
+      Status = EFI_NOT_FOUND;
+      goto Exit;
+    }
+  }
+  
+  //
+  // StringInfoIn must not be NULL if it is not system default font info.
+  //
+  ASSERT (StringInfoIn != NULL);
+  //
+  // Check the font information mask to make sure it is valid.
+  //
+  if (((StringInfoIn->FontInfoMask & (EFI_FONT_INFO_SYS_FONT  | EFI_FONT_INFO_ANY_FONT))  == 
+       (EFI_FONT_INFO_SYS_FONT | EFI_FONT_INFO_ANY_FONT))   ||
+      ((StringInfoIn->FontInfoMask & (EFI_FONT_INFO_SYS_SIZE  | EFI_FONT_INFO_ANY_SIZE))  == 
+       (EFI_FONT_INFO_SYS_SIZE | EFI_FONT_INFO_ANY_SIZE))   ||
+      ((StringInfoIn->FontInfoMask & (EFI_FONT_INFO_SYS_STYLE | EFI_FONT_INFO_ANY_STYLE)) == 
+       (EFI_FONT_INFO_SYS_STYLE | EFI_FONT_INFO_ANY_STYLE)) ||
+      ((StringInfoIn->FontInfoMask & (EFI_FONT_INFO_RESIZE    | EFI_FONT_INFO_ANY_SIZE))  == 
+       (EFI_FONT_INFO_RESIZE | EFI_FONT_INFO_ANY_SIZE))     ||           
+      ((StringInfoIn->FontInfoMask & (EFI_FONT_INFO_RESTYLE   | EFI_FONT_INFO_ANY_STYLE)) == 
+       (EFI_FONT_INFO_RESTYLE | EFI_FONT_INFO_ANY_STYLE))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Parse the font information mask to find a matching font.
+  //
+
+  CopyMem (&InfoOut, (EFI_FONT_DISPLAY_INFO *) StringInfoIn, sizeof (EFI_FONT_DISPLAY_INFO));
+
+  if ((StringInfoIn->FontInfoMask & EFI_FONT_INFO_SYS_FONT) == EFI_FONT_INFO_SYS_FONT) {
+    Status = SaveFontName (SystemDefault->FontInfo.FontName, &FontInfo);
+  } else {
+    Status = SaveFontName (((EFI_FONT_DISPLAY_INFO *) StringInfoIn)->FontInfo.FontName, &FontInfo);
+  }
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if ((StringInfoIn->FontInfoMask & EFI_FONT_INFO_SYS_SIZE) == EFI_FONT_INFO_SYS_SIZE) {
+    InfoOut.FontInfo.FontSize = SystemDefault->FontInfo.FontSize;
+  } 
+  if ((StringInfoIn->FontInfoMask & EFI_FONT_INFO_SYS_STYLE) == EFI_FONT_INFO_SYS_STYLE) {
+    InfoOut.FontInfo.FontStyle = SystemDefault->FontInfo.FontStyle;
+  }
+  if ((StringInfoIn->FontInfoMask & EFI_FONT_INFO_SYS_FORE_COLOR) == EFI_FONT_INFO_SYS_FORE_COLOR) {
+    InfoOut.ForegroundColor = SystemDefault->ForegroundColor;
+  }
+  if ((StringInfoIn->FontInfoMask & EFI_FONT_INFO_SYS_BACK_COLOR) == EFI_FONT_INFO_SYS_BACK_COLOR) {
+    InfoOut.BackgroundColor = SystemDefault->BackgroundColor;
+  }
+  
+  ASSERT (FontInfo != NULL);
+  FontInfo->FontSize  = InfoOut.FontInfo.FontSize;
+  FontInfo->FontStyle = InfoOut.FontInfo.FontStyle;
+
+  if (IsFontInfoExisted (Private, FontInfo, &InfoOut.FontInfoMask, LocalFontHandle, &GlobalFont)) {
+    //
+    // Test to guarantee all characters are available in the found font.
+    //    
+    if (String != NULL) {
+      StringIn = String;
+      while (*StringIn != 0) {
+        Status = FindGlyphBlock (GlobalFont->FontPackage, *StringIn, NULL, NULL, NULL);
+        if (EFI_ERROR (Status)) {
+          LocalFontHandle = NULL;
+          goto Exit;
+        }
+        StringIn++;
+      }
+    }
+    //
+    // Write to output parameter
+    //
+    if (StringInfoOut != NULL) {
+      StringInfoOutLen = sizeof (EFI_FONT_DISPLAY_INFO) - sizeof (EFI_FONT_INFO) + GlobalFont->FontInfoSize;
+      *StringInfoOut   = (EFI_FONT_DISPLAY_INFO *) AllocateZeroPool (StringInfoOutLen);      
+      if (*StringInfoOut == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        LocalFontHandle = NULL;
+        goto Exit;
+      }
+      
+      CopyMem (*StringInfoOut, &InfoOut, sizeof (EFI_FONT_DISPLAY_INFO));
+      CopyMem (&(*StringInfoOut)->FontInfo, GlobalFont->FontInfo, GlobalFont->FontInfoSize);
+    }
+    
+    LocalFontHandle = GlobalFont->Entry.ForwardLink;    
+    Status = EFI_SUCCESS;
+    goto Exit;
+  }  
+
+  Status = EFI_NOT_FOUND;
+
+Exit:
+
+  if (FontHandle != NULL) {
+    *FontHandle = LocalFontHandle;
+  }
+
+  if (SystemDefault != NULL) {
+   FreePool (SystemDefault);
+  }
+  if (FontInfo != NULL) {
+   FreePool (FontInfo);
+  }
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.h b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.h
new file mode 100644
index 0000000000..bb0090aaa6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.h
@@ -0,0 +1,2022 @@
+/** @file
+Private structures definitions in HiiDatabase.
+
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef __HII_DATABASE_PRIVATE_H__
+#define __HII_DATABASE_PRIVATE_H__
+
+#include <Uefi.h>
+
+#include <Protocol/DevicePath.h>
+#include <Protocol/HiiFont.h>
+#include <Protocol/HiiImage.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigKeyword.h>
+#include <Protocol/SimpleTextOut.h>
+
+#include <Guid/HiiKeyBoardLayout.h>
+#include <Guid/GlobalVariable.h>
+
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/PrintLib.h>
+
+#define MAX_STRING_LENGTH                  1024
+#define MAX_FONT_NAME_LEN                  256
+#define NARROW_BASELINE                    15
+#define WIDE_BASELINE                      14
+#define SYS_FONT_INFO_MASK                 0x37
+#define REPLACE_UNKNOWN_GLYPH              0xFFFD
+#define PROPORTIONAL_GLYPH                 0x80
+#define NARROW_GLYPH                       0x40
+
+#define BITMAP_LEN_1_BIT(Width, Height)  (((Width) + 7) / 8 * (Height))
+#define BITMAP_LEN_4_BIT(Width, Height)  (((Width) + 1) / 2 * (Height))
+#define BITMAP_LEN_8_BIT(Width, Height)  ((Width) * (Height))
+#define BITMAP_LEN_24_BIT(Width, Height) ((Width) * (Height) * 3)
+
+//
+// IFR data structure
+//
+// BASE_CR (a, IFR_DEFAULT_VALUE_DATA, Entry) to get the whole structure.
+
+typedef struct {
+  LIST_ENTRY          Entry;             // Link to VarStorage
+  EFI_GUID            Guid;
+  CHAR16              *Name;
+  UINT16              Size;
+  UINT8               Type;
+  LIST_ENTRY          BlockEntry;        // Link to its Block array
+} IFR_VARSTORAGE_DATA;
+
+typedef struct {
+  LIST_ENTRY          Entry;             // Link to Block array
+  UINT16              Offset;
+  UINT16              Width;
+  EFI_QUESTION_ID     QuestionId;
+  UINT8               OpCode;
+  UINT8               Scope;
+  LIST_ENTRY          DefaultValueEntry; // Link to its default value array
+  CHAR16              *Name;
+} IFR_BLOCK_DATA;
+
+//
+// Get default value from IFR data.
+//
+typedef enum {
+  DefaultValueFromDefault = 0,     // Get from the minimum or first one when not set default value.
+  DefaultValueFromFlag,            // Get default value from the defalut flag.
+  DefaultValueFromOpcode           // Get default value from default opcode, highest priority.
+} DEFAULT_VALUE_TYPE;
+
+typedef struct {
+  LIST_ENTRY          Entry;
+  DEFAULT_VALUE_TYPE  Type;
+  BOOLEAN             Cleaned;       // Whether this value is cleaned
+                                     // TRUE  Cleaned, the value can't be used
+                                     // FALSE Not cleaned, the value can  be used.
+  UINT16              DefaultId;
+  EFI_IFR_TYPE_VALUE  Value;
+} IFR_DEFAULT_DATA;
+
+//
+// Storage types
+//
+#define EFI_HII_VARSTORE_BUFFER              0
+#define EFI_HII_VARSTORE_NAME_VALUE          1
+#define EFI_HII_VARSTORE_EFI_VARIABLE        2
+#define EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER 3
+
+//
+// Keyword handler protocol filter type.
+//
+#define EFI_KEYWORD_FILTER_READONY           0x01
+#define EFI_KEYWORD_FILTER_REAWRITE          0x02
+#define EFI_KEYWORD_FILTER_BUFFER            0x10
+#define EFI_KEYWORD_FILTER_NUMERIC           0x20
+#define EFI_KEYWORD_FILTER_NUMERIC_1         0x30
+#define EFI_KEYWORD_FILTER_NUMERIC_2         0x40
+#define EFI_KEYWORD_FILTER_NUMERIC_4         0x50
+#define EFI_KEYWORD_FILTER_NUMERIC_8         0x60
+
+
+#define HII_FORMSET_STORAGE_SIGNATURE           SIGNATURE_32 ('H', 'S', 'T', 'G')
+typedef struct {
+  UINTN               Signature;
+  LIST_ENTRY          Entry;
+
+  EFI_HII_HANDLE      HiiHandle;
+  EFI_HANDLE          DriverHandle;
+
+  UINT8               Type;   // EFI_HII_VARSTORE_BUFFER, EFI_HII_VARSTORE_NAME_VALUE, EFI_HII_VARSTORE_EFI_VARIABLE
+  EFI_GUID            Guid;
+  CHAR16              *Name;
+  UINT16              Size;
+} HII_FORMSET_STORAGE;
+
+
+//
+// String Package definitions
+//
+#define HII_STRING_PACKAGE_SIGNATURE    SIGNATURE_32 ('h','i','s','p')
+typedef struct _HII_STRING_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  EFI_HII_STRING_PACKAGE_HDR            *StringPkgHdr;
+  UINT8                                 *StringBlock;
+  LIST_ENTRY                            StringEntry;
+  LIST_ENTRY                            FontInfoList;  // local font info list
+  UINT8                                 FontId;
+  EFI_STRING_ID                         MaxStringId;   // record StringId
+} HII_STRING_PACKAGE_INSTANCE;
+
+//
+// Form Package definitions
+//
+#define HII_IFR_PACKAGE_SIGNATURE       SIGNATURE_32 ('h','f','r','p')
+typedef struct _HII_IFR_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  EFI_HII_PACKAGE_HEADER                FormPkgHdr;
+  UINT8                                 *IfrData;
+  LIST_ENTRY                            IfrEntry;
+} HII_IFR_PACKAGE_INSTANCE;
+
+//
+// Simple Font Package definitions
+//
+#define HII_S_FONT_PACKAGE_SIGNATURE    SIGNATURE_32 ('h','s','f','p')
+typedef struct _HII_SIMPLE_FONT_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  EFI_HII_SIMPLE_FONT_PACKAGE_HDR       *SimpleFontPkgHdr;
+  LIST_ENTRY                            SimpleFontEntry;
+} HII_SIMPLE_FONT_PACKAGE_INSTANCE;
+
+//
+// Font Package definitions
+//
+#define HII_FONT_PACKAGE_SIGNATURE      SIGNATURE_32 ('h','i','f','p')
+typedef struct _HII_FONT_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  EFI_HII_FONT_PACKAGE_HDR              *FontPkgHdr;
+  UINT16                                Height;
+  UINT16                                BaseLine;
+  UINT8                                 *GlyphBlock;
+  LIST_ENTRY                            FontEntry;
+  LIST_ENTRY                            GlyphInfoList;
+} HII_FONT_PACKAGE_INSTANCE;
+
+#define HII_GLYPH_INFO_SIGNATURE        SIGNATURE_32 ('h','g','i','s')
+typedef struct _HII_GLYPH_INFO {
+  UINTN                                 Signature;
+  LIST_ENTRY                            Entry;
+  CHAR16                                CharId;
+  EFI_HII_GLYPH_INFO                    Cell;
+} HII_GLYPH_INFO;
+
+#define HII_FONT_INFO_SIGNATURE         SIGNATURE_32 ('h','l','f','i')
+typedef struct _HII_FONT_INFO {
+  UINTN                                 Signature;
+  LIST_ENTRY                            Entry;
+  LIST_ENTRY                            *GlobalEntry;
+  UINT8                                 FontId;
+} HII_FONT_INFO;
+
+#define HII_GLOBAL_FONT_INFO_SIGNATURE  SIGNATURE_32 ('h','g','f','i')
+typedef struct _HII_GLOBAL_FONT_INFO {
+  UINTN                                 Signature;
+  LIST_ENTRY                            Entry;
+  HII_FONT_PACKAGE_INSTANCE             *FontPackage;
+  UINTN                                 FontInfoSize;
+  EFI_FONT_INFO                         *FontInfo;
+} HII_GLOBAL_FONT_INFO;
+
+//
+// Image Package definitions
+//
+
+#define HII_PIXEL_MASK                  0x80
+
+typedef struct _HII_IMAGE_PACKAGE_INSTANCE {
+  EFI_HII_IMAGE_PACKAGE_HDR             ImagePkgHdr;
+  UINT32                                ImageBlockSize;
+  UINT32                                PaletteInfoSize;
+  UINT8                                 *ImageBlock;
+  UINT8                                 *PaletteBlock;
+} HII_IMAGE_PACKAGE_INSTANCE;
+
+//
+// Keyboard Layout Pacakge definitions
+//
+#define HII_KB_LAYOUT_PACKAGE_SIGNATURE SIGNATURE_32 ('h','k','l','p')
+typedef struct _HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  UINT8                                 *KeyboardPkg;
+  LIST_ENTRY                            KeyboardEntry;
+} HII_KEYBOARD_LAYOUT_PACKAGE_INSTANCE;
+
+//
+// Guid Package definitions
+//
+#define HII_GUID_PACKAGE_SIGNATURE      SIGNATURE_32 ('h','i','g','p')
+typedef struct _HII_GUID_PACKAGE_INSTANCE {
+  UINTN                                 Signature;
+  UINT8                                 *GuidPkg;
+  LIST_ENTRY                            GuidEntry;
+} HII_GUID_PACKAGE_INSTANCE;
+
+//
+// A package list can contain only one or less than one device path package.
+// This rule also applies to image package since ImageId can not be duplicate.
+//
+typedef struct _HII_DATABASE_PACKAGE_LIST_INSTANCE {
+  EFI_HII_PACKAGE_LIST_HEADER           PackageListHdr;
+  LIST_ENTRY                            GuidPkgHdr;
+  LIST_ENTRY                            FormPkgHdr;
+  LIST_ENTRY                            KeyboardLayoutHdr;
+  LIST_ENTRY                            StringPkgHdr;
+  LIST_ENTRY                            FontPkgHdr;
+  HII_IMAGE_PACKAGE_INSTANCE            *ImagePkg;
+  LIST_ENTRY                            SimpleFontPkgHdr;
+  UINT8                                 *DevicePathPkg;
+} HII_DATABASE_PACKAGE_LIST_INSTANCE;
+
+#define HII_HANDLE_SIGNATURE            SIGNATURE_32 ('h','i','h','l')
+
+typedef struct {
+  UINTN               Signature;
+  LIST_ENTRY          Handle;
+  UINTN               Key;
+} HII_HANDLE;
+
+#define HII_DATABASE_RECORD_SIGNATURE   SIGNATURE_32 ('h','i','d','r')
+
+typedef struct _HII_DATABASE_RECORD {
+  UINTN                                 Signature;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE    *PackageList;
+  EFI_HANDLE                            DriverHandle;
+  EFI_HII_HANDLE                        Handle;
+  LIST_ENTRY                            DatabaseEntry;
+} HII_DATABASE_RECORD;
+
+#define HII_DATABASE_NOTIFY_SIGNATURE   SIGNATURE_32 ('h','i','d','n')
+
+typedef struct _HII_DATABASE_NOTIFY {
+  UINTN                                 Signature;
+  EFI_HANDLE                            NotifyHandle;
+  UINT8                                 PackageType;
+  EFI_GUID                              *PackageGuid;
+  EFI_HII_DATABASE_NOTIFY               PackageNotifyFn;
+  EFI_HII_DATABASE_NOTIFY_TYPE          NotifyType;
+  LIST_ENTRY                            DatabaseNotifyEntry;
+} HII_DATABASE_NOTIFY;
+
+#define HII_DATABASE_PRIVATE_DATA_SIGNATURE SIGNATURE_32 ('H', 'i', 'D', 'p')
+
+typedef struct _HII_DATABASE_PRIVATE_DATA {
+  UINTN                                 Signature;
+  LIST_ENTRY                            DatabaseList;
+  LIST_ENTRY                            DatabaseNotifyList;
+  EFI_HII_FONT_PROTOCOL                 HiiFont;
+  EFI_HII_IMAGE_PROTOCOL                HiiImage;
+  EFI_HII_STRING_PROTOCOL               HiiString;
+  EFI_HII_DATABASE_PROTOCOL             HiiDatabase;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL       ConfigRouting;
+  EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL   ConfigKeywordHandler;
+  LIST_ENTRY                            HiiHandleList;
+  INTN                                  HiiHandleCount;
+  LIST_ENTRY                            FontInfoList;  // global font info list
+  UINTN                                 Attribute;     // default system color
+  EFI_GUID                              CurrentLayoutGuid;
+  EFI_HII_KEYBOARD_LAYOUT               *CurrentLayout;
+} HII_DATABASE_PRIVATE_DATA;
+
+#define HII_FONT_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      HiiFont, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define HII_IMAGE_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      HiiImage, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      HiiString, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define HII_DATABASE_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      HiiDatabase, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define CONFIG_ROUTING_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      ConfigRouting, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+#define CONFIG_KEYWORD_HANDLER_DATABASE_PRIVATE_DATA_FROM_THIS(a) \
+  CR (a, \
+      HII_DATABASE_PRIVATE_DATA, \
+      ConfigKeywordHandler, \
+      HII_DATABASE_PRIVATE_DATA_SIGNATURE \
+      )
+
+//
+// Internal function prototypes.
+//
+
+/**
+  Generate a sub string then output it.
+
+  This is a internal function.
+
+  @param  String                 A constant string which is the prefix of the to be
+                                 generated string, e.g. GUID=
+
+  @param  BufferLen              The length of the Buffer in bytes.
+
+  @param  Buffer                 Points to a buffer which will be converted to be the 
+                                 content of the generated string.
+
+  @param  Flag                   If 1, the buffer contains data for the value of GUID or PATH stored in 
+                                 UINT8 *; if 2, the buffer contains unicode string for the value of NAME;
+                                 if 3, the buffer contains other data.
+
+  @param  SubStr                 Points to the output string. It's caller's
+                                 responsibility to free this buffer.
+
+
+**/
+VOID
+GenerateSubStr (
+  IN CONST EFI_STRING              String,
+  IN  UINTN                        BufferLen,
+  IN  VOID                         *Buffer,
+  IN  UINT8                        Flag,
+  OUT EFI_STRING                   *SubStr
+  );
+
+/**
+  This function checks whether a handle is a valid EFI_HII_HANDLE.
+
+  @param  Handle                  Pointer to a EFI_HII_HANDLE
+
+  @retval TRUE                    Valid
+  @retval FALSE                   Invalid
+
+**/
+BOOLEAN
+IsHiiHandleValid (
+  EFI_HII_HANDLE Handle
+  );
+
+
+/**
+  This function checks whether EFI_FONT_INFO exists in current database. If
+  FontInfoMask is specified, check what options can be used to make a match.
+  Note that the masks relate to where the system default should be supplied
+  are ignored by this function.
+
+  @param  Private                 Hii database private structure.
+  @param  FontInfo                Points to EFI_FONT_INFO structure.
+  @param  FontInfoMask            If not NULL, describes what options can be used
+                                  to make a match between the font requested and
+                                  the font available. The caller must guarantee
+                                  this mask is valid.
+  @param  FontHandle              On entry, Points to the font handle returned by a
+                                  previous  call to GetFontInfo() or NULL to start
+                                  with the first font.
+  @param  GlobalFontInfo          If not NULL, output the corresponding globa font
+                                  info.
+
+  @retval TRUE                    Existed
+  @retval FALSE                   Not existed
+
+**/
+BOOLEAN
+IsFontInfoExisted (
+  IN  HII_DATABASE_PRIVATE_DATA *Private,
+  IN  EFI_FONT_INFO             *FontInfo,
+  IN  EFI_FONT_INFO_MASK        *FontInfoMask,   OPTIONAL
+  IN  EFI_FONT_HANDLE           FontHandle,      OPTIONAL
+  OUT HII_GLOBAL_FONT_INFO      **GlobalFontInfo OPTIONAL
+  );
+
+/**
+
+   This function invokes the matching registered function.
+    
+   @param  Private           HII Database driver private structure.
+   @param  NotifyType        The type of change concerning the database.
+   @param  PackageInstance   Points to the package referred to by the notification.
+   @param  PackageType       Package type
+   @param  Handle            The handle of the package list which contains the specified package.
+    
+   @retval EFI_SUCCESS            Already checked all registered function and invoked 
+                                  if matched.
+   @retval EFI_INVALID_PARAMETER  Any input parameter is not valid.
+     
+**/
+EFI_STATUS
+InvokeRegisteredFunction (
+  IN HII_DATABASE_PRIVATE_DATA    *Private, 
+  IN EFI_HII_DATABASE_NOTIFY_TYPE NotifyType,
+  IN VOID                         *PackageInstance,
+  IN UINT8                        PackageType,
+  IN EFI_HII_HANDLE               Handle
+  )
+;
+
+/**
+  Retrieve system default font and color.
+
+  @param  Private                 HII database driver private data.
+  @param  FontInfo                Points to system default font output-related
+                                  information. It's caller's responsibility to free
+                                  this buffer.
+  @param  FontInfoSize            If not NULL, output the size of buffer FontInfo.
+
+  @retval EFI_SUCCESS             Cell information is added to the GlyphInfoList.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+  @retval EFI_INVALID_PARAMETER   Any input parameter is invalid.
+
+**/
+EFI_STATUS
+GetSystemFont (
+  IN  HII_DATABASE_PRIVATE_DATA      *Private,
+  OUT EFI_FONT_DISPLAY_INFO          **FontInfo,
+  OUT UINTN                          *FontInfoSize OPTIONAL
+  );
+
+
+/**
+  Parse all string blocks to find a String block specified by StringId.
+  If StringId = (EFI_STRING_ID) (-1), find out all EFI_HII_SIBT_FONT blocks
+  within this string package and backup its information. If LastStringId is 
+  specified, the string id of last string block will also be output.
+  If StringId = 0, output the string id of last string block (EFI_HII_SIBT_STRING).
+
+  @param  Private                 Hii database private structure.
+  @param  StringPackage           Hii string package instance.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  BlockType               Output the block type of found string block.
+  @param  StringBlockAddr         Output the block address of found string block.
+  @param  StringTextOffset        Offset, relative to the found block address, of
+                                  the  string text information.
+  @param  LastStringId            Output the last string id when StringId = 0 or StringId = -1.
+  @param  StartStringId           The first id in the skip block which StringId in the block.
+
+  @retval EFI_SUCCESS             The string text and font is retrieved
+                                  successfully.
+  @retval EFI_NOT_FOUND           The specified text or font info can not be found
+                                  out.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+FindStringBlock (
+  IN HII_DATABASE_PRIVATE_DATA        *Private,
+  IN  HII_STRING_PACKAGE_INSTANCE     *StringPackage,
+  IN  EFI_STRING_ID                   StringId,
+  OUT UINT8                           *BlockType, OPTIONAL
+  OUT UINT8                           **StringBlockAddr, OPTIONAL
+  OUT UINTN                           *StringTextOffset, OPTIONAL
+  OUT EFI_STRING_ID                   *LastStringId, OPTIONAL
+  OUT EFI_STRING_ID                   *StartStringId OPTIONAL
+  );
+
+
+/**
+  Parse all glyph blocks to find a glyph block specified by CharValue.
+  If CharValue = (CHAR16) (-1), collect all default character cell information
+  within this font package and backup its information.
+
+  @param  FontPackage             Hii string package instance.
+  @param  CharValue               Unicode character value, which identifies a glyph
+                                  block.
+  @param  GlyphBuffer             Output the corresponding bitmap data of the found
+                                  block. It is the caller's responsiblity to free
+                                  this buffer.
+  @param  Cell                    Output cell information of the encoded bitmap.
+  @param  GlyphBufferLen          If not NULL, output the length of GlyphBuffer.
+
+  @retval EFI_SUCCESS             The bitmap data is retrieved successfully.
+  @retval EFI_NOT_FOUND           The specified CharValue does not exist in current
+                                  database.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+FindGlyphBlock (
+  IN  HII_FONT_PACKAGE_INSTANCE      *FontPackage,
+  IN  CHAR16                         CharValue,
+  OUT UINT8                          **GlyphBuffer, OPTIONAL
+  OUT EFI_HII_GLYPH_INFO             *Cell, OPTIONAL
+  OUT UINTN                          *GlyphBufferLen OPTIONAL
+  );
+
+/**
+  This function exports Form packages to a buffer.
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  Handle                 Identification of a package list.
+  @param  PackageList            Pointer to a package list which will be exported.
+  @param  UsedSize               The length of buffer be used.
+  @param  BufferSize             Length of the Buffer.
+  @param  Buffer                 Allocated space for storing exported data.
+  @param  ResultSize             The size of the already exported content of  this
+                                 package list.
+
+  @retval EFI_SUCCESS            Form Packages are exported successfully.
+  @retval EFI_INVALID_PARAMETER  Any input parameter is invalid.
+
+**/
+EFI_STATUS
+ExportFormPackages (
+  IN HII_DATABASE_PRIVATE_DATA          *Private,
+  IN EFI_HII_HANDLE                     Handle,
+  IN HII_DATABASE_PACKAGE_LIST_INSTANCE *PackageList,
+  IN UINTN                              UsedSize,
+  IN UINTN                              BufferSize,
+  IN OUT VOID                           *Buffer,
+  IN OUT UINTN                          *ResultSize
+  );
+
+//
+// EFI_HII_FONT_PROTOCOL protocol interfaces
+//
+
+
+/**
+  Renders a string to a bitmap or to the display.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Flags                   Describes how the string is to be drawn.
+  @param  String                  Points to the null-terminated string to be
+                                  displayed.
+  @param  StringInfo              Points to the string output information,
+                                  including the color and font.  If NULL, then the
+                                  string will be output in the default system font
+                                  and color.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels   wide
+                                  and Height pixels high. The string will be drawn
+                                  onto this image and
+                                  EFI_HII_OUT_FLAG_CLIP is implied. If this points
+                                  to a NULL on entry, then a              buffer
+                                  will be allocated to hold the generated image and
+                                  the pointer updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  BltX                    Together with BltX, Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  BltY                    Together with BltY, Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  RowInfoArray            If this is non-NULL on entry, then on exit, this
+                                  will point to an allocated buffer    containing
+                                  row information and RowInfoArraySize will be
+                                  updated to contain the        number of elements.
+                                  This array describes the characters which were at
+                                  least partially drawn and the heights of the
+                                  rows. It is the caller's responsibility to free
+                                  this buffer.
+  @param  RowInfoArraySize        If this is non-NULL on entry, then on exit it
+                                  contains the number of elements in RowInfoArray.
+  @param  ColumnInfoArray         If this is non-NULL, then on return it will be
+                                  filled with the horizontal offset for each
+                                  character in the string on the row where it is
+                                  displayed. Non-printing characters will     have
+                                  the offset ~0. The caller is responsible to
+                                  allocate a buffer large enough so that    there
+                                  is one entry for each character in the string,
+                                  not including the null-terminator. It is possible
+                                  when character display is normalized that some
+                                  character cells overlap.
+
+  @retval EFI_SUCCESS             The string was successfully rendered.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for
+                                  RowInfoArray or Blt.
+  @retval EFI_INVALID_PARAMETER The String or Blt.
+  @retval EFI_INVALID_PARAMETER Flags were invalid combination..
+
+**/
+EFI_STATUS
+EFIAPI
+HiiStringToImage (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  EFI_HII_OUT_FLAGS              Flags,
+  IN  CONST EFI_STRING               String,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo       OPTIONAL,
+  IN  OUT EFI_IMAGE_OUTPUT           **Blt,
+  IN  UINTN                          BltX,
+  IN  UINTN                          BltY,
+  OUT EFI_HII_ROW_INFO               **RowInfoArray    OPTIONAL,
+  OUT UINTN                          *RowInfoArraySize OPTIONAL,
+  OUT UINTN                          *ColumnInfoArray  OPTIONAL
+  );
+
+
+/**
+  Render a string to a bitmap or the screen containing the contents of the specified string.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Flags                   Describes how the string is to be drawn.
+  @param  PackageList             The package list in the HII database to search
+                                  for the specified string.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  Language                Points to the language for the retrieved string.
+                                  If NULL, then the current system language is
+                                  used.
+  @param  StringInfo              Points to the string output information,
+                                  including the color and font.  If NULL, then the
+                                  string will be output in the default system font
+                                  and color.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels   wide
+                                  and Height pixels high. The string will be drawn
+                                  onto this image and
+                                  EFI_HII_OUT_FLAG_CLIP is implied. If this points
+                                  to a NULL on entry, then a              buffer
+                                  will be allocated to hold the generated image and
+                                  the pointer updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  BltX                    Together with BltX, Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  BltY                    Together with BltY, Specifies the offset from the left and top edge
+                                  of the image of the first character cell in the
+                                  image.
+  @param  RowInfoArray            If this is non-NULL on entry, then on exit, this
+                                  will point to an allocated buffer    containing
+                                  row information and RowInfoArraySize will be
+                                  updated to contain the        number of elements.
+                                  This array describes the characters which were at
+                                  least partially drawn and the heights of the
+                                  rows. It is the caller's responsibility to free
+                                  this buffer.
+  @param  RowInfoArraySize        If this is non-NULL on entry, then on exit it
+                                  contains the number of elements in RowInfoArray.
+  @param  ColumnInfoArray         If this is non-NULL, then on return it will be
+                                  filled with the horizontal offset for each
+                                  character in the string on the row where it is
+                                  displayed. Non-printing characters will     have
+                                  the offset ~0. The caller is responsible to
+                                  allocate a buffer large enough so that    there
+                                  is one entry for each character in the string,
+                                  not including the null-terminator. It is possible
+                                  when character display is normalized that some
+                                  character cells overlap.
+
+  @retval EFI_SUCCESS             The string was successfully rendered.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for
+                                  RowInfoArray or Blt.
+  @retval EFI_INVALID_PARAMETER The Blt or PackageList was NULL.
+  @retval EFI_INVALID_PARAMETER Flags were invalid combination.
+  @retval EFI_NOT_FOUND         The specified PackageList is not in the Database or the stringid is not 
+                          in the specified PackageList. 
+
+**/
+EFI_STATUS
+EFIAPI
+HiiStringIdToImage (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  EFI_HII_OUT_FLAGS              Flags,
+  IN  EFI_HII_HANDLE                 PackageList,
+  IN  EFI_STRING_ID                  StringId,
+  IN  CONST CHAR8*                   Language,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo       OPTIONAL,
+  IN  OUT EFI_IMAGE_OUTPUT           **Blt,
+  IN  UINTN                          BltX,
+  IN  UINTN                          BltY,
+  OUT EFI_HII_ROW_INFO               **RowInfoArray    OPTIONAL,
+  OUT UINTN                          *RowInfoArraySize OPTIONAL,
+  OUT UINTN                          *ColumnInfoArray  OPTIONAL
+  );
+
+
+/**
+  Convert the glyph for a single character into a bitmap.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  Char                    Character to retrieve.
+  @param  StringInfo              Points to the string font and color information
+                                  or NULL if the string should use the default
+                                  system font and color.
+  @param  Blt                     Thus must point to a NULL on entry. A buffer will
+                                  be allocated to hold the output and the pointer
+                                  updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  Baseline                Number of pixels from the bottom of the bitmap to
+                                  the baseline.
+
+  @retval EFI_SUCCESS             Glyph bitmap created.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate the output buffer Blt.
+  @retval EFI_WARN_UNKNOWN_GLYPH  The glyph was unknown and was replaced with the
+                                  glyph for Unicode character 0xFFFD.
+  @retval EFI_INVALID_PARAMETER   Blt is NULL or *Blt is not NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetGlyph (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  CHAR16                         Char,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfo,
+  OUT EFI_IMAGE_OUTPUT               **Blt,
+  OUT UINTN                          *Baseline OPTIONAL
+  );
+
+
+/**
+  This function iterates through fonts which match the specified font, using
+  the specified criteria. If String is non-NULL, then all of the characters in
+  the string must exist in order for a candidate font to be returned.
+
+  @param  This                    A pointer to the EFI_HII_FONT_PROTOCOL instance.
+  @param  FontHandle              On entry, points to the font handle returned by a
+                                   previous call to GetFontInfo() or NULL to start
+                                  with the  first font. On return, points to the
+                                  returned font handle or points to NULL if there
+                                  are no more matching fonts.
+  @param  StringInfoIn            Upon entry, points to the font to return information
+                                  about. If NULL, then the information about the system
+                                  default font will be returned.
+  @param  StringInfoOut           Upon return, contains the matching font's information.
+                                  If NULL, then no information is returned. This buffer
+                                  is allocated with a call to the Boot Service AllocatePool().
+                                  It is the caller's responsibility to call the Boot 
+                                  Service FreePool() when the caller no longer requires
+                                  the contents of StringInfoOut.
+  @param  String                  Points to the string which will be tested to
+                                  determine  if all characters are available. If
+                                  NULL, then any font  is acceptable.
+
+  @retval EFI_SUCCESS             Matching font returned successfully.
+  @retval EFI_NOT_FOUND           No matching font was found.
+  @retval EFI_INVALID_PARAMETER   StringInfoIn is NULL.
+  @retval EFI_INVALID_PARAMETER   StringInfoIn->FontInfoMask is an invalid combination.
+  @retval EFI_OUT_OF_RESOURCES    There were insufficient resources to complete the
+                                  request.
+**/
+EFI_STATUS
+EFIAPI
+HiiGetFontInfo (
+  IN  CONST EFI_HII_FONT_PROTOCOL    *This,
+  IN  OUT   EFI_FONT_HANDLE          *FontHandle,
+  IN  CONST EFI_FONT_DISPLAY_INFO    *StringInfoIn, OPTIONAL
+  OUT       EFI_FONT_DISPLAY_INFO    **StringInfoOut,
+  IN  CONST EFI_STRING               String OPTIONAL
+  );
+
+//
+// EFI_HII_IMAGE_PROTOCOL interfaces
+//
+
+
+/**
+  This function adds the image Image to the group of images owned by PackageList, and returns
+  a new image identifier (ImageId).
+
+  @param  This                    A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList             Handle of the package list where this image will
+                                  be added.
+  @param  ImageId                 On return, contains the new image id, which is
+                                  unique within PackageList.
+  @param  Image                   Points to the image.
+
+  @retval EFI_SUCCESS             The new image was added successfully.
+  @retval EFI_NOT_FOUND           The specified PackageList could not be found in
+                                  database.
+  @retval EFI_OUT_OF_RESOURCES    Could not add the image due to lack of resources.
+  @retval EFI_INVALID_PARAMETER   Image is NULL or ImageId is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewImage (
+  IN  CONST EFI_HII_IMAGE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                 PackageList,
+  OUT EFI_IMAGE_ID                   *ImageId,
+  IN  CONST EFI_IMAGE_INPUT          *Image
+  );
+
+
+/**
+  This function retrieves the image specified by ImageId which is associated with
+  the specified PackageList and copies it into the buffer specified by Image.
+
+  @param  This                    A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList             Handle of the package list where this image will
+                                  be searched.
+  @param  ImageId                 The image's id,, which is unique within
+                                  PackageList.
+  @param  Image                   Points to the image.
+
+  @retval EFI_SUCCESS             The new image was returned successfully.
+  @retval EFI_NOT_FOUND           The image specified by ImageId is not available.
+                                                 The specified PackageList is not in the database.
+  @retval EFI_BUFFER_TOO_SMALL    The buffer specified by ImageSize is too small to
+                                  hold the image.
+  @retval EFI_INVALID_PARAMETER   The Image or ImageSize was NULL.
+  @retval EFI_OUT_OF_RESOURCES   The bitmap could not be retrieved because there was not
+                                                       enough memory.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetImage (
+  IN  CONST EFI_HII_IMAGE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                 PackageList,
+  IN  EFI_IMAGE_ID                   ImageId,
+  OUT EFI_IMAGE_INPUT                *Image
+  );
+
+
+/**
+  This function updates the image specified by ImageId in the specified PackageListHandle to
+  the image specified by Image.
+
+  @param  This                    A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList             The package list containing the images.
+  @param  ImageId                 The image's id,, which is unique within
+                                  PackageList.
+  @param  Image                   Points to the image.
+
+  @retval EFI_SUCCESS             The new image was updated successfully.
+  @retval EFI_NOT_FOUND           The image specified by ImageId is not in the
+                                                database. The specified PackageList is not in the database.
+  @retval EFI_INVALID_PARAMETER   The Image was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetImage (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN EFI_IMAGE_ID                    ImageId,
+  IN CONST EFI_IMAGE_INPUT           *Image
+  );
+
+
+/**
+  This function renders an image to a bitmap or the screen using the specified
+  color and options. It draws the image on an existing bitmap, allocates a new
+  bitmap or uses the screen. The images can be clipped.
+
+  @param  This                    A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  Flags                   Describes how the image is to be drawn.
+  @param  Image                   Points to the image to be displayed.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels wide
+                                  and Height pixels high.  The image will be drawn
+                                  onto this image and  EFI_HII_DRAW_FLAG_CLIP is
+                                  implied. If this points to a  NULL on entry, then
+                                  a buffer will be allocated to hold  the generated
+                                  image and the pointer updated on exit. It is the
+                                  caller's responsibility to free this buffer.
+  @param  BltX                    Specifies the offset from the left and top edge
+                                  of the  output image of the first pixel in the
+                                  image.
+  @param  BltY                    Specifies the offset from the left and top edge
+                                  of the  output image of the first pixel in the
+                                  image.
+
+  @retval EFI_SUCCESS             The image was successfully drawn.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for Blt.
+  @retval EFI_INVALID_PARAMETER   The Image or Blt was NULL.
+  @retval EFI_INVALID_PARAMETER   Any combination of Flags is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiDrawImage (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_DRAW_FLAGS              Flags,
+  IN CONST EFI_IMAGE_INPUT           *Image,
+  IN OUT EFI_IMAGE_OUTPUT            **Blt,
+  IN UINTN                           BltX,
+  IN UINTN                           BltY
+  );
+
+
+/**
+  This function renders an image to a bitmap or the screen using the specified
+  color and options. It draws the image on an existing bitmap, allocates a new
+  bitmap or uses the screen. The images can be clipped.
+
+  @param  This                    A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  Flags                   Describes how the image is to be drawn.
+  @param  PackageList             The package list in the HII database to search
+                                  for the  specified image.
+  @param  ImageId                 The image's id, which is unique within
+                                  PackageList.
+  @param  Blt                     If this points to a non-NULL on entry, this
+                                  points to the image, which is Width pixels wide
+                                  and Height pixels high. The image will be drawn
+                                  onto this image and
+                                  EFI_HII_DRAW_FLAG_CLIP is implied. If this points
+                                  to a  NULL on entry, then a buffer will be
+                                  allocated to hold  the generated image and the
+                                  pointer updated on exit. It is the caller's
+                                  responsibility to free this buffer.
+  @param  BltX                    Specifies the offset from the left and top edge
+                                  of the  output image of the first pixel in the
+                                  image.
+  @param  BltY                    Specifies the offset from the left and top edge
+                                  of the  output image of the first pixel in the
+                                  image.
+
+  @retval EFI_SUCCESS             The image was successfully drawn.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate an output buffer for Blt.
+  @retval EFI_INVALID_PARAMETER  The Blt was NULL.
+  @retval EFI_NOT_FOUND          The image specified by ImageId is not in the database. 
+                           The specified PackageList is not in the database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiDrawImageId (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_DRAW_FLAGS              Flags,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN EFI_IMAGE_ID                    ImageId,
+  IN OUT EFI_IMAGE_OUTPUT            **Blt,
+  IN UINTN                           BltX,
+  IN UINTN                           BltY
+  )
+
+;
+
+//
+// EFI_HII_STRING_PROTOCOL
+//
+
+
+/**
+  This function adds the string String to the group of strings owned by PackageList, with the
+  specified font information StringFontInfo and returns a new string id.
+
+  @param  This                    A pointer to the EFI_HII_STRING_PROTOCOL
+                                  instance.
+  @param  PackageList             Handle of the package list where this string will
+                                  be added.
+  @param  StringId                On return, contains the new strings id, which is
+                                  unique within PackageList.
+  @param  Language                Points to the language for the new string.
+  @param  LanguageName            Points to the printable language name to
+                                  associate with the passed in  Language field.If
+                                  LanguageName is not NULL and the string package
+                                  header's LanguageName  associated with a given
+                                  Language is not zero, the LanguageName being
+                                  passed  in will be ignored.
+  @param  String                  Points to the new null-terminated string.
+  @param  StringFontInfo          Points to the new string's font information or
+                                  NULL if the string should have the default system
+                                  font, size and style.
+
+  @retval EFI_SUCCESS             The new string was added successfully.
+  @retval EFI_NOT_FOUND           The specified PackageList could not be found in
+                                  database.
+  @retval EFI_OUT_OF_RESOURCES    Could not add the string due to lack of
+                                  resources.
+  @retval EFI_INVALID_PARAMETER   String is NULL or StringId is NULL or Language is
+                                  NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewString (
+  IN  CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                  PackageList,
+  OUT EFI_STRING_ID                   *StringId,
+  IN  CONST CHAR8                     *Language,
+  IN  CONST CHAR16                    *LanguageName, OPTIONAL
+  IN  CONST EFI_STRING                String,
+  IN  CONST EFI_FONT_INFO             *StringFontInfo OPTIONAL
+  );
+
+
+/**
+  This function retrieves the string specified by StringId which is associated
+  with the specified PackageList in the language Language and copies it into
+  the buffer specified by String.
+
+  @param  This                    A pointer to the EFI_HII_STRING_PROTOCOL
+                                  instance.
+  @param  Language                Points to the language for the retrieved string.
+  @param  PackageList             The package list in the HII database to search
+                                  for the  specified string.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  String                  Points to the new null-terminated string.
+  @param  StringSize              On entry, points to the size of the buffer
+                                  pointed to by  String, in bytes. On return,
+                                  points to the length of the string, in bytes.
+  @param  StringFontInfo          If not NULL, points to the string's font
+                                  information.  It's caller's responsibility to
+                                  free this buffer.
+
+  @retval EFI_SUCCESS             The string was returned successfully.
+  @retval EFI_NOT_FOUND           The string specified by StringId is not
+                                  available.
+                                  The specified PackageList is not in the database.
+  @retval EFI_INVALID_LANGUAGE    The string specified by StringId is available but
+                                  not in the specified language.
+  @retval EFI_BUFFER_TOO_SMALL    The buffer specified by StringSize is too small
+                                  to  hold the string.
+  @retval EFI_INVALID_PARAMETER   The Language or StringSize was NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by StringSize was not zero
+                                  and String was NULL.
+  @retval EFI_OUT_OF_RESOURCES    There were insufficient resources to complete the
+                                   request.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetString (
+  IN  CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN  CONST CHAR8                     *Language,
+  IN  EFI_HII_HANDLE                  PackageList,
+  IN  EFI_STRING_ID                   StringId,
+  OUT EFI_STRING                      String,
+  IN  OUT UINTN                       *StringSize,
+  OUT EFI_FONT_INFO                   **StringFontInfo OPTIONAL
+  );
+
+
+/**
+  This function updates the string specified by StringId in the specified PackageList to the text
+  specified by String and, optionally, the font information specified by StringFontInfo.
+
+  @param  This                    A pointer to the EFI_HII_STRING_PROTOCOL
+                                  instance.
+  @param  PackageList             The package list containing the strings.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  Language                Points to the language for the updated string.
+  @param  String                  Points to the new null-terminated string.
+  @param  StringFontInfo          Points to the string's font information or NULL
+                                  if the string font information is not changed.
+
+  @retval EFI_SUCCESS             The string was updated successfully.
+  @retval EFI_NOT_FOUND           The string specified by StringId is not in the
+                                  database.
+  @retval EFI_INVALID_PARAMETER   The String or Language was NULL.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetString (
+  IN CONST EFI_HII_STRING_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                   PackageList,
+  IN EFI_STRING_ID                    StringId,
+  IN CONST CHAR8                      *Language,
+  IN CONST EFI_STRING                 String,
+  IN CONST EFI_FONT_INFO              *StringFontInfo OPTIONAL
+  );
+
+
+/**
+  This function returns the list of supported languages, in the format specified
+  in Appendix M of UEFI 2.1 spec.
+
+  @param  This                    A pointer to the EFI_HII_STRING_PROTOCOL
+                                  instance.
+  @param  PackageList             The package list to examine.
+  @param  Languages               Points to the buffer to hold the returned
+                                  null-terminated ASCII string.
+  @param  LanguagesSize           On entry, points to the size of the buffer
+                                  pointed to by  Languages, in bytes. On  return,
+                                  points to the length of Languages, in bytes.
+
+  @retval EFI_SUCCESS             The languages were returned successfully.
+  @retval EFI_INVALID_PARAMETER   The LanguagesSize was NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by LanguagesSize is not zero and Languages is NULL.
+  @retval EFI_BUFFER_TOO_SMALL    The LanguagesSize is too small to hold the list
+                                  of  supported languages. LanguageSize is updated
+                                  to contain the required size.
+  @retval EFI_NOT_FOUND           Could not find string package in specified
+                                  packagelist.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetLanguages (
+  IN CONST EFI_HII_STRING_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                   PackageList,
+  IN OUT CHAR8                        *Languages,
+  IN OUT UINTN                        *LanguagesSize
+  );
+
+
+/**
+  Each string package has associated with it a single primary language and zero
+  or more secondary languages. This routine returns the secondary languages
+  associated with a package list.
+
+  @param  This                    A pointer to the EFI_HII_STRING_PROTOCOL
+                                  instance.
+  @param  PackageList             The package list to examine.
+  @param  PrimaryLanguage         Points to the null-terminated ASCII string that specifies
+                                  the primary language. Languages are specified in the
+                                  format specified in Appendix M of the UEFI 2.0 specification.
+  @param  SecondaryLanguages      Points to the buffer to hold the returned null-terminated
+                                  ASCII string that describes the list of
+                                  secondary languages for the specified
+                                  PrimaryLanguage. If there are no secondary
+                                  languages, the function returns successfully,
+                                  but this is set to NULL.
+  @param  SecondaryLanguagesSize  On entry, points to the size of the buffer
+                                  pointed to by SecondaryLanguages, in bytes. On
+                                  return, points to the length of SecondaryLanguages
+                                  in bytes.
+
+  @retval EFI_SUCCESS             Secondary languages were correctly returned.
+  @retval EFI_INVALID_PARAMETER   PrimaryLanguage or SecondaryLanguagesSize was NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by SecondaryLanguagesSize is not
+                                  zero and SecondaryLanguages is NULL.
+  @retval EFI_BUFFER_TOO_SMALL    The buffer specified by SecondaryLanguagesSize is
+                                  too small to hold the returned information.
+                                  SecondaryLanguageSize is updated to hold the size of
+                                  the buffer required.
+  @retval EFI_INVALID_LANGUAGE    The language specified by PrimaryLanguage is not
+                                  present in the specified package list.
+  @retval EFI_NOT_FOUND           The specified PackageList is not in the Database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetSecondaryLanguages (
+  IN CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN CONST CHAR8                     *PrimaryLanguage,
+  IN OUT CHAR8                       *SecondaryLanguages,
+  IN OUT UINTN                       *SecondaryLanguagesSize
+  );
+
+//
+// EFI_HII_DATABASE_PROTOCOL protocol interfaces
+//
+
+
+/**
+  This function adds the packages in the package list to the database and returns a handle. If there is a
+  EFI_DEVICE_PATH_PROTOCOL associated with the DriverHandle, then this function will
+  create a package of type EFI_PACKAGE_TYPE_DEVICE_PATH and add it to the package list.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  PackageList             A pointer to an EFI_HII_PACKAGE_LIST_HEADER
+                                  structure.
+  @param  DriverHandle            Associate the package list with this EFI handle.
+                                  If a NULL is specified, this data will not be associate
+                                  with any drivers and cannot have a callback induced.
+  @param  Handle                  A pointer to the EFI_HII_HANDLE instance.
+
+  @retval EFI_SUCCESS             The package list associated with the Handle was
+                                  added to the HII database.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate necessary resources for the
+                                  new database contents.
+  @retval EFI_INVALID_PARAMETER   PackageList is NULL or Handle is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewPackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN CONST EFI_HII_PACKAGE_LIST_HEADER  *PackageList,
+  IN CONST EFI_HANDLE                   DriverHandle, OPTIONAL
+  OUT EFI_HII_HANDLE                    *Handle
+  );
+
+
+/**
+  This function removes the package list that is associated with a handle Handle
+  from the HII database. Before removing the package, any registered functions
+  with the notification type REMOVE_PACK and the same package type will be called.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  Handle                  The handle that was registered to the data that
+                                  is requested  for removal.
+
+  @retval EFI_SUCCESS             The data associated with the Handle was removed
+                                  from  the HII database.
+  @retval EFI_NOT_FOUND           The specified Handle is not in database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiRemovePackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                     Handle
+  );
+
+
+/**
+  This function updates the existing package list (which has the specified Handle)
+  in the HII databases, using the new package list specified by PackageList.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  Handle                  The handle that was registered to the data that
+                                  is  requested to be updated.
+  @param  PackageList             A pointer to an EFI_HII_PACKAGE_LIST_HEADER
+                                  package.
+
+  @retval EFI_SUCCESS             The HII database was successfully updated.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate enough memory for the updated
+                                  database.
+  @retval EFI_INVALID_PARAMETER  PackageList was NULL.
+  @retval EFI_NOT_FOUND          The specified Handle is not in database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUpdatePackageList (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                     Handle,
+  IN CONST EFI_HII_PACKAGE_LIST_HEADER  *PackageList
+  );
+
+
+/**
+  This function returns a list of the package handles of the specified type
+  that are currently active in the database. The pseudo-type
+  EFI_HII_PACKAGE_TYPE_ALL will cause all package handles to be listed.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  PackageType             Specifies the package type of the packages to
+                                  list or EFI_HII_PACKAGE_TYPE_ALL for all packages
+                                  to be listed.
+  @param  PackageGuid             If PackageType is EFI_HII_PACKAGE_TYPE_GUID, then
+                                  this  is the pointer to the GUID which must match
+                                  the Guid field of EFI_HII_GUID_PACKAGE_GUID_HDR.
+                                  Otherwise,  it must be NULL.
+  @param  HandleBufferLength      On input, a pointer to the length of the handle
+                                  buffer.  On output, the length of the handle
+                                  buffer that is required for the handles found.
+  @param  Handle                  An array of EFI_HII_HANDLE instances returned.
+
+  @retval EFI_SUCCESS             The matching handles are outputed successfully.
+                                  HandleBufferLength is updated with the actual length.
+  @retval EFI_BUFFER_TO_SMALL     The HandleBufferLength parameter indicates that
+                                  Handle is too small to support the number of
+                                  handles. HandleBufferLength is updated with a
+                                  value that will  enable the data to fit.
+  @retval EFI_NOT_FOUND           No matching handle could not be found in
+                                  database.
+  @retval EFI_INVALID_PARAMETER   HandleBufferLength was NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by HandleBufferLength was not
+                                  zero and Handle was NULL.
+  @retval EFI_INVALID_PARAMETER   PackageType is not a EFI_HII_PACKAGE_TYPE_GUID but
+                                  PackageGuid is not NULL, PackageType is a EFI_HII_
+                                  PACKAGE_TYPE_GUID but PackageGuid is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiListPackageLists (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  UINT8                             PackageType,
+  IN  CONST EFI_GUID                    *PackageGuid,
+  IN  OUT UINTN                         *HandleBufferLength,
+  OUT EFI_HII_HANDLE                    *Handle
+  );
+
+
+/**
+  This function will export one or all package lists in the database to a buffer.
+  For each package list exported, this function will call functions registered
+  with EXPORT_PACK and then copy the package list to the buffer.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  Handle                  An EFI_HII_HANDLE that corresponds to the desired
+                                  package list in the HII database to export or
+                                  NULL to indicate  all package lists should be
+                                  exported.
+  @param  BufferSize              On input, a pointer to the length of the buffer.
+                                  On output, the length of the buffer that is
+                                  required for the exported data.
+  @param  Buffer                  A pointer to a buffer that will contain the
+                                  results of  the export function.
+
+  @retval EFI_SUCCESS             Package exported.
+  @retval EFI_BUFFER_TO_SMALL     The HandleBufferLength parameter indicates that
+                                  Handle is too small to support the number of
+                                  handles.      HandleBufferLength is updated with
+                                  a value that will enable the data to fit.
+  @retval EFI_NOT_FOUND           The specifiecd Handle could not be found in the
+                                  current database.
+  @retval EFI_INVALID_PARAMETER   BufferSize was NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by BufferSize was not zero 
+                                  and Buffer was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiExportPackageLists (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                    Handle,
+  IN  OUT UINTN                         *BufferSize,
+  OUT EFI_HII_PACKAGE_LIST_HEADER       *Buffer
+  );
+
+
+/**
+  This function registers a function which will be called when specified actions related to packages of
+  the specified type occur in the HII database. By registering a function, other HII-related drivers are
+  notified when specific package types are added, removed or updated in the HII database.
+  Each driver or application which registers a notification should use
+  EFI_HII_DATABASE_PROTOCOL.UnregisterPackageNotify() before exiting.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  PackageType             Specifies the package type of the packages to
+                                  list or EFI_HII_PACKAGE_TYPE_ALL for all packages
+                                  to be listed.
+  @param  PackageGuid             If PackageType is EFI_HII_PACKAGE_TYPE_GUID, then
+                                  this is the pointer to the GUID which must match
+                                  the Guid field of
+                                  EFI_HII_GUID_PACKAGE_GUID_HDR. Otherwise, it must
+                                  be NULL.
+  @param  PackageNotifyFn         Points to the function to be called when the
+                                  event specified by
+                                  NotificationType occurs.
+  @param  NotifyType              Describes the types of notification which this
+                                  function will be receiving.
+  @param  NotifyHandle            Points to the unique handle assigned to the
+                                  registered notification. Can be used in
+                                  EFI_HII_DATABASE_PROTOCOL.UnregisterPackageNotify()
+                                  to stop notifications.
+
+  @retval EFI_SUCCESS             Notification registered successfully.
+  @retval EFI_OUT_OF_RESOURCES    Unable to allocate necessary data structures
+  @retval EFI_INVALID_PARAMETER   NotifyHandle is NULL.
+  @retval EFI_INVALID_PARAMETER   PackageGuid is not NULL when PackageType is not
+                                  EFI_HII_PACKAGE_TYPE_GUID.
+  @retval EFI_INVALID_PARAMETER   PackageGuid is NULL when PackageType is
+                                  EFI_HII_PACKAGE_TYPE_GUID.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiRegisterPackageNotify (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  UINT8                             PackageType,
+  IN  CONST EFI_GUID                    *PackageGuid,
+  IN  CONST EFI_HII_DATABASE_NOTIFY     PackageNotifyFn,
+  IN  EFI_HII_DATABASE_NOTIFY_TYPE      NotifyType,
+  OUT EFI_HANDLE                        *NotifyHandle
+  );
+
+
+/**
+  Removes the specified HII database package-related notification.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  NotificationHandle      The handle of the notification function being
+                                  unregistered.
+
+  @retval EFI_SUCCESS             Notification is unregistered successfully.
+  @retval EFI_NOT_FOUND          The incoming notification handle does not exist 
+                           in current hii database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiUnregisterPackageNotify (
+  IN CONST EFI_HII_DATABASE_PROTOCOL    *This,
+  IN EFI_HANDLE                         NotificationHandle
+  );
+
+
+/**
+  This routine retrieves an array of GUID values for each keyboard layout that
+  was previously registered in the system.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  KeyGuidBufferLength     On input, a pointer to the length of the keyboard
+                                  GUID  buffer. On output, the length of the handle
+                                  buffer  that is required for the handles found.
+  @param  KeyGuidBuffer           An array of keyboard layout GUID instances
+                                  returned.
+
+  @retval EFI_SUCCESS             KeyGuidBuffer was updated successfully.
+  @retval EFI_BUFFER_TOO_SMALL    The KeyGuidBufferLength parameter indicates
+                                  that KeyGuidBuffer is too small to support the
+                                  number of GUIDs. KeyGuidBufferLength is
+                                  updated with a value that will enable the data to
+                                  fit.
+  @retval EFI_INVALID_PARAMETER   The KeyGuidBufferLength is NULL.
+  @retval EFI_INVALID_PARAMETER   The value referenced by KeyGuidBufferLength is not
+                                  zero and KeyGuidBuffer is NULL.
+  @retval EFI_NOT_FOUND           There was no keyboard layout.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiFindKeyboardLayouts (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  OUT UINT16                        *KeyGuidBufferLength,
+  OUT EFI_GUID                          *KeyGuidBuffer
+  );
+
+
+/**
+  This routine retrieves the requested keyboard layout. The layout is a physical description of the keys
+  on a keyboard and the character(s) that are associated with a particular set of key strokes.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  KeyGuid                 A pointer to the unique ID associated with a
+                                  given keyboard layout. If KeyGuid is NULL then
+                                  the current layout will be retrieved.
+  @param  KeyboardLayoutLength    On input, a pointer to the length of the
+                                  KeyboardLayout buffer.  On output, the length of
+                                  the data placed into KeyboardLayout.
+  @param  KeyboardLayout          A pointer to a buffer containing the retrieved
+                                  keyboard layout.
+
+  @retval EFI_SUCCESS             The keyboard layout was retrieved successfully.
+  @retval EFI_NOT_FOUND           The requested keyboard layout was not found.
+  @retval EFI_INVALID_PARAMETER   The KeyboardLayout or KeyboardLayoutLength was
+                                  NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetKeyboardLayout (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL   *This,
+  IN  CONST EFI_GUID                          *KeyGuid,
+  IN OUT UINT16                         *KeyboardLayoutLength,
+  OUT EFI_HII_KEYBOARD_LAYOUT           *KeyboardLayout
+  );
+
+
+/**
+  This routine sets the default keyboard layout to the one referenced by KeyGuid. When this routine
+  is called, an event will be signaled of the EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID
+  group type. This is so that agents which are sensitive to the current keyboard layout being changed
+  can be notified of this change.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  KeyGuid                 A pointer to the unique ID associated with a
+                                  given keyboard layout.
+
+  @retval EFI_SUCCESS             The current keyboard layout was successfully set.
+  @retval EFI_NOT_FOUND           The referenced keyboard layout was not found, so
+                                  action was taken.
+  @retval EFI_INVALID_PARAMETER   The KeyGuid was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetKeyboardLayout (
+  IN CONST EFI_HII_DATABASE_PROTOCOL          *This,
+  IN CONST EFI_GUID                           *KeyGuid
+  );
+
+
+/**
+  Return the EFI handle associated with a package list.
+
+  @param  This                    A pointer to the EFI_HII_DATABASE_PROTOCOL
+                                  instance.
+  @param  PackageListHandle       An EFI_HII_HANDLE that corresponds to the desired
+                                  package list in the HIIdatabase.
+  @param  DriverHandle            On return, contains the EFI_HANDLE which was
+                                  registered with the package list in
+                                  NewPackageList().
+
+  @retval EFI_SUCCESS             The DriverHandle was returned successfully.
+  @retval EFI_INVALID_PARAMETER   The PackageListHandle was not valid or
+                                  DriverHandle was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetPackageListHandle (
+  IN  CONST EFI_HII_DATABASE_PROTOCOL         *This,
+  IN  EFI_HII_HANDLE                    PackageListHandle,
+  OUT EFI_HANDLE                        *DriverHandle
+  );
+
+//
+// EFI_HII_CONFIG_ROUTING_PROTOCOL interfaces
+//
+
+
+/**
+  This function allows a caller to extract the current configuration
+  for one or more named elements from one or more drivers.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  Request                 A null-terminated Unicode string in
+                                  <MultiConfigRequest> format.
+  @param  Progress                On return, points to a character in the Request
+                                  string. Points to the string's null terminator if
+                                  request was successful. Points to the most recent
+                                  & before the first failing name / value pair (or
+                                  the beginning of the string if the failure is in
+                                  the first name / value pair) if the request was
+                                  not successful.
+  @param  Results                 Null-terminated Unicode string in
+                                  <MultiConfigAltResp> format which has all values
+                                  filled in for the names in the Request string.
+                                  String to be allocated by the called function.
+
+  @retval EFI_SUCCESS             The Results string is filled with the values
+                                  corresponding to all requested names.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the parts of the
+                                  results that must be stored awaiting possible
+                                  future        protocols.
+  @retval EFI_NOT_FOUND           Routing data doesn't match any known driver.
+                                     Progress set to the "G" in "GUID" of the
+                                  routing  header that doesn't match. Note: There
+                                  is no         requirement that all routing data
+                                  be validated before any configuration extraction.
+  @retval EFI_INVALID_PARAMETER   For example, passing in a NULL for the Request
+                                  parameter would result in this type of error. The
+                                  Progress parameter is set to NULL.
+  @retval EFI_INVALID_PARAMETER   Illegal syntax. Progress set to most recent &
+                                  before the error or the beginning of the string.
+  @retval EFI_INVALID_PARAMETER   Unknown name. Progress points to the & before the
+                                  name in question.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  );
+
+
+/**
+  This function allows the caller to request the current configuration for the
+  entirety of the current HII database and returns the data in a null-terminated Unicode string.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  Results                 Null-terminated Unicode string in
+                                  <MultiConfigAltResp> format which has all values
+                                  filled in for the entirety of the current HII 
+                                  database. String to be allocated by the  called 
+                                  function. De-allocation is up to the caller.
+
+  @retval EFI_SUCCESS             The Results string is filled with the values
+                                  corresponding to all requested names.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the parts of the
+                                  results that must be stored awaiting possible
+                                  future        protocols.
+  @retval EFI_INVALID_PARAMETER   For example, passing in a NULL for the Results
+                                  parameter would result in this type of error.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingExportConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  OUT EFI_STRING                             *Results
+  );
+
+
+/**
+  This function processes the results of processing forms and routes it to the
+  appropriate handlers or storage.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  Configuration           A null-terminated Unicode string in
+                                  <MulltiConfigResp> format.
+  @param  Progress                A pointer to a string filled in with the offset
+                                  of the most recent & before the first failing
+                                  name / value pair (or the beginning of the string
+                                  if the failure is in the first name / value pair)
+                                  or the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are awaiting
+                                  distribution.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the parts of the
+                                  results that must be stored awaiting possible
+                                  future        protocols.
+  @retval EFI_INVALID_PARAMETER   Passing in a NULL for the Configuration parameter
+                                  would result in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data was not
+                                  found.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigRoutingRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  );
+
+
+
+/**
+  This helper function is to be called by drivers to map configuration data stored
+  in byte array ("block") formats such as UEFI Variables into current configuration strings.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  ConfigRequest           A null-terminated Unicode string in
+                                  <ConfigRequest> format.
+  @param  Block                   Array of bytes defining the block's
+                                  configuration.
+  @param  BlockSize               Length in bytes of Block.
+  @param  Config                  Filled-in configuration string. String allocated
+                                  by  the function. Returned only if call is
+                                  successful.
+  @param  Progress                A pointer to a string filled in with the offset
+                                  of  the most recent & before the first failing
+                                  name/value pair (or the beginning of the string
+                                  if the failure is in the first name / value pair)
+                                  or the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS             The request succeeded. Progress points to the
+                                  null terminator at the end of the ConfigRequest
+                                        string.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to allocate Config.
+                                  Progress points to the first character of
+                                  ConfigRequest.
+  @retval EFI_INVALID_PARAMETER   Passing in a NULL for the ConfigRequest or
+                                  Block parameter would result in this type of
+                                  error. Progress points to the first character of
+                                  ConfigRequest.
+  @retval EFI_NOT_FOUND           Target for the specified routing data was not
+                                  found. Progress points to the "G" in "GUID" of
+                                  the      errant routing data.
+  @retval EFI_DEVICE_ERROR        Block not large enough. Progress undefined.
+  @retval EFI_INVALID_PARAMETER   Encountered non <BlockName> formatted string.
+                                       Block is left updated and Progress points at
+                                  the '&' preceding the first non-<BlockName>.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiBlockToConfig (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL  *This,
+  IN  CONST EFI_STRING                       ConfigRequest,
+  IN  CONST UINT8                            *Block,
+  IN  CONST UINTN                            BlockSize,
+  OUT EFI_STRING                             *Config,
+  OUT EFI_STRING                             *Progress
+  );
+
+
+/**
+  This helper function is to be called by drivers to map configuration strings
+  to configurations stored in byte array ("block") formats such as UEFI Variables.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  ConfigResp              A null-terminated Unicode string in <ConfigResp>
+                                  format.
+  @param  Block                   A possibly null array of bytes representing the
+                                  current  block. Only bytes referenced in the
+                                  ConfigResp string  in the block are modified. If
+                                  this parameter is null or if the *BlockSize
+                                  parameter is (on input) shorter than required by
+                                  the Configuration string, only the BlockSize
+                                  parameter is updated and an appropriate status
+                                  (see below)  is returned.
+  @param  BlockSize               The length of the Block in units of UINT8.  On
+                                  input, this is the size of the Block. On output,
+                                  if successful, contains the largest index of the
+                                  modified byte in the Block, or the required buffer
+                                  size if the Block is not large enough.
+  @param  Progress                On return, points to an element of the ConfigResp
+                                   string filled in with the offset of the most
+                                  recent '&' before the first failing name / value
+                                  pair (or  the beginning of the string if the
+                                  failure is in the  first name / value pair) or
+                                  the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS             The request succeeded. Progress points to the
+                                  null terminator at the end of the ConfigResp
+                                  string.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to allocate Config.
+                                  Progress points to the first character of
+                                  ConfigResp.
+  @retval EFI_INVALID_PARAMETER   Passing in a NULL for the ConfigResp or
+                                  Block parameter would result in this type of
+                                  error. Progress points to the first character of
+                                           ConfigResp.
+  @retval EFI_NOT_FOUND           Target for the specified routing data was not
+                                  found. Progress points to the "G" in "GUID" of
+                                  the      errant routing data.
+  @retval EFI_INVALID_PARAMETER   Encountered non <BlockName> formatted name /
+                                  value pair. Block is left updated and
+                                  Progress points at the '&' preceding the first
+                                  non-<BlockName>.
+  @retval EFI_BUFFER_TOO_SMALL    Block not large enough. Progress undefined. 
+                                  BlockSize is updated with the required buffer size.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiConfigToBlock (
+  IN     CONST EFI_HII_CONFIG_ROUTING_PROTOCOL *This,
+  IN     CONST EFI_STRING                      ConfigResp,
+  IN OUT UINT8                                 *Block,
+  IN OUT UINTN                                 *BlockSize,
+  OUT    EFI_STRING                            *Progress
+  );
+
+
+/**
+  This helper function is to be called by drivers to extract portions of
+  a larger configuration string.
+
+  @param  This                    A pointer to the EFI_HII_CONFIG_ROUTING_PROTOCOL
+                                  instance.
+  @param  Configuration           A null-terminated Unicode string in
+                                  <MultiConfigAltResp> format.
+  @param  Guid                    A pointer to the GUID value to search for in the
+                                  routing portion of the ConfigResp string when
+                                  retrieving  the requested data. If Guid is NULL,
+                                  then all GUID  values will be searched for.
+  @param  Name                    A pointer to the NAME value to search for in the
+                                  routing portion of the ConfigResp string when
+                                  retrieving  the requested data. If Name is NULL,
+                                  then all Name  values will be searched for.
+  @param  DevicePath              A pointer to the PATH value to search for in the
+                                  routing portion of the ConfigResp string when
+                                  retrieving  the requested data. If DevicePath is
+                                  NULL, then all  DevicePath values will be
+                                  searched for.
+  @param  AltCfgId                A pointer to the ALTCFG value to search for in
+                                  the  routing portion of the ConfigResp string
+                                  when retrieving  the requested data.  If this
+                                  parameter is NULL,  then the current setting will
+                                  be retrieved.
+  @param  AltCfgResp              A pointer to a buffer which will be allocated by
+                                  the  function which contains the retrieved string
+                                  as requested.   This buffer is only allocated if
+                                  the call was successful.
+
+  @retval EFI_SUCCESS             The request succeeded. The requested data was
+                                  extracted  and placed in the newly allocated
+                                  AltCfgResp buffer.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to allocate AltCfgResp.
+  @retval EFI_INVALID_PARAMETER   Any parameter is invalid.
+  @retval EFI_NOT_FOUND           Target for the specified routing data was not
+                                  found.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetAltCfg (
+  IN  CONST EFI_HII_CONFIG_ROUTING_PROTOCOL    *This,
+  IN  CONST EFI_STRING                         Configuration,
+  IN  CONST EFI_GUID                           *Guid,
+  IN  CONST EFI_STRING                         Name,
+  IN  CONST EFI_DEVICE_PATH_PROTOCOL           *DevicePath,
+  IN  CONST UINT16                             *AltCfgId,
+  OUT EFI_STRING                               *AltCfgResp
+  );
+
+/**
+
+  This function accepts a <MultiKeywordResp> formatted string, finds the associated
+  keyword owners, creates a <MultiConfigResp> string from it and forwards it to the
+  EFI_HII_ROUTING_PROTOCOL.RouteConfig function.
+  
+  If there is an issue in resolving the contents of the KeywordString, then the 
+  function returns an error and also sets the Progress and ProgressErr with the 
+  appropriate information about where the issue occurred and additional data about
+  the nature of the issue. 
+  
+  In the case when KeywordString containing multiple keywords, when an EFI_NOT_FOUND
+  error is generated during processing the second or later keyword element, the system
+  storage associated with earlier keywords is not modified. All elements of the 
+  KeywordString must successfully pass all tests for format and access prior to making
+  any modifications to storage.
+  
+  In the case when EFI_DEVICE_ERROR is returned from the processing of a KeywordString
+  containing multiple keywords, the state of storage associated with earlier keywords
+  is undefined.
+
+
+  @param This             Pointer to the EFI_KEYWORD_HANDLER _PROTOCOL instance.
+
+  @param KeywordString    A null-terminated string in <MultiKeywordResp> format. 
+
+  @param Progress         On return, points to a character in the KeywordString. 
+                          Points to the string's NULL terminator if the request 
+                          was successful. Points to the most recent '&' before 
+                          the first failing string element if the request was 
+                          not successful.
+
+  @param ProgressErr      If during the processing of the KeywordString there was
+                          a failure, this parameter gives additional information 
+                          about the possible source of the problem. The various 
+                          errors are defined in "Related Definitions" below.
+
+
+  @retval EFI_SUCCESS             The specified action was completed successfully.
+
+  @retval EFI_INVALID_PARAMETER   One or more of the following are TRUE:
+                                  1. KeywordString is NULL.
+                                  2. Parsing of the KeywordString resulted in an 
+                                     error. See Progress and ProgressErr for more data.
+
+  @retval EFI_NOT_FOUND           An element of the KeywordString was not found. 
+                                  See ProgressErr for more data.
+
+  @retval EFI_OUT_OF_RESOURCES    Required system resources could not be allocated.  
+                                  See ProgressErr for more data.
+                                  
+  @retval EFI_ACCESS_DENIED       The action violated system policy. See ProgressErr 
+                                  for more data.
+
+  @retval EFI_DEVICE_ERROR        An unexpected system error occurred. See ProgressErr
+                                  for more data.
+
+**/
+EFI_STATUS
+EFIAPI 
+EfiConfigKeywordHandlerSetData (
+  IN EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL *This,
+  IN CONST EFI_STRING                    KeywordString,
+  OUT EFI_STRING                         *Progress,
+  OUT UINT32                             *ProgressErr
+  );
+
+/**
+
+  This function accepts a <MultiKeywordRequest> formatted string, finds the underlying 
+  keyword owners, creates a <MultiConfigRequest> string from it and forwards it to the
+  EFI_HII_ROUTING_PROTOCOL.ExtractConfig function.
+  
+  If there is an issue in resolving the contents of the KeywordString, then the function
+  returns an EFI_INVALID_PARAMETER and also set the Progress and ProgressErr with the
+  appropriate information about where the issue occurred and additional data about the
+  nature of the issue.
+  
+  In the case when KeywordString is NULL, or contains multiple keywords, or when
+  EFI_NOT_FOUND is generated while processing the keyword elements, the Results string
+  contains values returned for all keywords processed prior to the keyword generating the 
+  error but no values for the keyword with error or any following keywords.
+
+  
+  @param This           Pointer to the EFI_KEYWORD_HANDLER _PROTOCOL instance.
+  
+  @param NameSpaceId    A null-terminated string containing the platform configuration
+                        language to search through in the system. If a NULL is passed
+                        in, then it is assumed that any platform configuration language
+                        with the prefix of "x-UEFI-" are searched.
+                        
+  @param KeywordString  A null-terminated string in <MultiKeywordRequest> format. If a
+                        NULL is passed in the KeywordString field, all of the known 
+                        keywords in the system for the NameSpaceId specified are 
+                        returned in the Results field.
+  
+  @param Progress       On return, points to a character in the KeywordString. Points
+                        to the string's NULL terminator if the request was successful. 
+                        Points to the most recent '&' before the first failing string
+                        element if the request was not successful.
+                        
+  @param ProgressErr    If during the processing of the KeywordString there was a
+                        failure, this parameter gives additional information about the 
+                        possible source of the problem. See the definitions in SetData()
+                        for valid value definitions.
+  
+  @param Results        A null-terminated string in <MultiKeywordResp> format is returned
+                        which has all the values filled in for the keywords in the 
+                        KeywordString. This is a callee-allocated field, and must be freed
+                        by the caller after being used. 
+
+  @retval EFI_SUCCESS             The specified action was completed successfully.
+  
+  @retval EFI_INVALID_PARAMETER   One or more of the following are TRUE:
+                                  1.Progress, ProgressErr, or Resuts is NULL.
+                                  2.Parsing of the KeywordString resulted in an error. See
+                                    Progress and ProgressErr for more data.
+  
+
+  @retval EFI_NOT_FOUND           An element of the KeywordString was not found. See
+                                  ProgressErr for more data.
+
+  @retval EFI_NOT_FOUND           The NamespaceId specified was not found.  See ProgressErr
+                                  for more data.
+
+  @retval EFI_OUT_OF_RESOURCES    Required system resources could not be allocated.  See
+                                  ProgressErr for more data.
+                                  
+  @retval EFI_ACCESS_DENIED       The action violated system policy.  See ProgressErr for
+                                  more data.
+
+  @retval EFI_DEVICE_ERROR        An unexpected system error occurred.  See ProgressErr
+                                  for more data.
+
+**/
+EFI_STATUS
+EFIAPI 
+EfiConfigKeywordHandlerGetData (
+  IN EFI_CONFIG_KEYWORD_HANDLER_PROTOCOL  *This,
+  IN CONST EFI_STRING                     NameSpaceId, OPTIONAL
+  IN CONST EFI_STRING                     KeywordString, OPTIONAL
+  OUT EFI_STRING                          *Progress, 
+  OUT UINT32                              *ProgressErr,
+  OUT EFI_STRING                          *Results
+  );
+
+/**
+  Compare whether two names of languages are identical.
+
+  @param  Language1              Name of language 1 from StringPackage
+  @param  Language2              Name of language 2 to be compared with language 1.
+
+  @retval TRUE                   same
+  @retval FALSE                  not same
+
+**/
+BOOLEAN
+HiiCompareLanguage (
+  IN  CHAR8  *Language1,
+  IN  CHAR8  *Language2
+  )
+;
+
+/**
+  Retrieves a pointer to the a Null-terminated ASCII string containing the list 
+  of languages that an HII handle in the HII Database supports.  The returned 
+  string is allocated using AllocatePool().  The caller is responsible for freeing
+  the returned string using FreePool().  The format of the returned string follows
+  the language format assumed the HII Database.
+  
+  If HiiHandle is NULL, then ASSERT().
+
+  @param[in]  HiiHandle  A handle that was previously registered in the HII Database.
+
+  @retval NULL   HiiHandle is not registered in the HII database
+  @retval NULL   There are not enough resources available to retrieve the suported 
+                 languages.
+  @retval NULL   The list of suported languages could not be retrieved.
+  @retval Other  A pointer to the Null-terminated ASCII string of supported languages.
+
+**/
+CHAR8 *
+GetSupportedLanguages (
+  IN EFI_HII_HANDLE           HiiHandle
+  );
+
+//
+// Global variables
+//
+extern EFI_EVENT gHiiKeyboardLayoutChanged;
+#endif
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.uni b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.uni
new file mode 100644
index 0000000000..81138b1371
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabase.uni
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseDxe.inf b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseDxe.inf
new file mode 100644
index 0000000000..7892503b47
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseDxe.inf
@@ -0,0 +1,92 @@
+## @file
+# The DXE driver produces HII protocols defined in UEFI specification.
+#
+# This driver produces all required HII serivces that includes HiiDataBase, HiiString,
+# HiiFont, HiiConfigRouting. To support UEFI HII, this driver is required.
+#
+# Copyright (c) 2007 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = HiiDatabase
+  MODULE_UNI_FILE                = HiiDatabase.uni
+  FILE_GUID                      = 348C4D62-BFBD-4882-9ECE-C80BB1C4783B
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeHiiDatabase
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  HiiDatabaseEntry.c
+  Image.c
+  HiiDatabase.h
+  ConfigRouting.c
+  String.c
+  Database.c
+  Font.c
+  ConfigKeywordHandler.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  MemoryAllocationLib
+  DevicePathLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  UefiLib
+  PcdLib
+  UefiRuntimeServicesTableLib
+  PrintLib
+
+[Protocols]
+  gEfiDevicePathProtocolGuid                                            ## SOMETIMES_CONSUMES
+  gEfiHiiStringProtocolGuid                                             ## PRODUCES
+  gEfiHiiImageProtocolGuid |gEfiMdeModulePkgTokenSpaceGuid.PcdSupportHiiImageProtocol  ## SOMETIMES_PRODUCES
+  gEfiHiiConfigRoutingProtocolGuid                                      ## PRODUCES
+  gEfiHiiDatabaseProtocolGuid                                           ## PRODUCES
+  gEfiHiiFontProtocolGuid                                               ## PRODUCES
+  gEfiHiiConfigAccessProtocolGuid                                       ## SOMETIMES_CONSUMES
+  gEfiConfigKeywordHandlerProtocolGuid                                  ## PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSupportHiiImageProtocol   ## CONSUMES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultPlatformLang ## CONSUMES
+
+[Guids]  
+  #
+  # Event registered to EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group,
+  # which will be triggered by EFI_HII_DATABASE_PROTOCOL.SetKeyboardLayout().
+  #
+  ## CONSUMES  ## Event
+  ## PRODUCES  ## Event
+  gEfiHiiKeyBoardLayoutGuid
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  HiiDatabaseExtra.uni
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseEntry.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseEntry.c
new file mode 100644
index 0000000000..6448c97256
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseEntry.c
@@ -0,0 +1,216 @@
+/** @file
+This file contains the entry code to the HII database, which is defined by
+UEFI 2.1 specification.
+
+Copyright (c) 2007 - 2015, 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 "HiiDatabase.h"
+
+//
+// Global variables
+//
+EFI_EVENT gHiiKeyboardLayoutChanged;
+
+HII_DATABASE_PRIVATE_DATA mPrivate = {
+  HII_DATABASE_PRIVATE_DATA_SIGNATURE,
+  {
+    (LIST_ENTRY *) NULL,
+    (LIST_ENTRY *) NULL
+  },
+  {
+    (LIST_ENTRY *) NULL,
+    (LIST_ENTRY *) NULL
+  },
+  {
+    HiiStringToImage,
+    HiiStringIdToImage,
+    HiiGetGlyph,
+    HiiGetFontInfo
+  },
+  {
+    NULL,
+    NULL,
+    NULL,
+    NULL,
+    NULL
+  },
+  {
+    HiiNewString,
+    HiiGetString,
+    HiiSetString,
+    HiiGetLanguages,
+    HiiGetSecondaryLanguages
+  },
+  {
+    HiiNewPackageList,
+    HiiRemovePackageList,
+    HiiUpdatePackageList,
+    HiiListPackageLists,
+    HiiExportPackageLists,
+    HiiRegisterPackageNotify,
+    HiiUnregisterPackageNotify,
+    HiiFindKeyboardLayouts,
+    HiiGetKeyboardLayout,
+    HiiSetKeyboardLayout,
+    HiiGetPackageListHandle
+  },
+  {
+    HiiConfigRoutingExtractConfig,
+    HiiConfigRoutingExportConfig,
+    HiiConfigRoutingRouteConfig,
+    HiiBlockToConfig,
+    HiiConfigToBlock,
+    HiiGetAltCfg
+  },
+  {
+    EfiConfigKeywordHandlerSetData,
+    EfiConfigKeywordHandlerGetData
+  },
+  {
+    (LIST_ENTRY *) NULL,
+    (LIST_ENTRY *) NULL
+  },
+  0,
+  {
+    (LIST_ENTRY *) NULL,
+    (LIST_ENTRY *) NULL
+  },
+  EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK),
+  {
+    0x00000000,
+    0x0000,
+    0x0000,
+    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
+  },
+  NULL
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST EFI_HII_IMAGE_PROTOCOL mImageProtocol = {
+  HiiNewImage,
+  HiiGetImage,
+  HiiSetImage,
+  HiiDrawImage,
+  HiiDrawImageId
+};
+
+/**
+  The default event handler for gHiiKeyboardLayoutChanged
+  event group.
+
+  This is internal function.
+
+  @param Event           The event that triggered this notification function.
+  @param Context         Pointer to the notification functions context.
+
+**/
+VOID
+EFIAPI
+KeyboardLayoutChangeNullEvent (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  return;
+}
+
+/**
+  Initialize HII Database.
+
+
+  @param ImageHandle     The image handle.
+  @param SystemTable     The system table.
+
+  @retval EFI_SUCCESS    The Hii database is setup correctly.
+  @return Other value if failed to create the default event for
+          gHiiKeyboardLayoutChanged. Check gBS->CreateEventEx for
+          details. Or failed to insatll the protocols.
+          Check gBS->InstallMultipleProtocolInterfaces for details.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeHiiDatabase (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS                             Status;
+  EFI_HANDLE                             Handle;
+
+  //
+  // There will be only one HII Database in the system
+  // If there is another out there, someone is trying to install us
+  // again.  Fail that scenario.
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiHiiDatabaseProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiHiiFontProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiHiiImageProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiHiiStringProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiHiiConfigRoutingProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiConfigKeywordHandlerProtocolGuid);
+  
+  InitializeListHead (&mPrivate.DatabaseList);
+  InitializeListHead (&mPrivate.DatabaseNotifyList);
+  InitializeListHead (&mPrivate.HiiHandleList);
+  InitializeListHead (&mPrivate.FontInfoList);
+
+  //
+  // Create a event with EFI_HII_SET_KEYBOARD_LAYOUT_EVENT_GUID group type.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  KeyboardLayoutChangeNullEvent,
+                  NULL,
+                  &gEfiHiiKeyBoardLayoutGuid,
+                  &gHiiKeyboardLayoutChanged
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Handle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Handle,
+                  &gEfiHiiFontProtocolGuid,
+                  &mPrivate.HiiFont,
+                  &gEfiHiiStringProtocolGuid,
+                  &mPrivate.HiiString,
+                  &gEfiHiiDatabaseProtocolGuid,
+                  &mPrivate.HiiDatabase,
+                  &gEfiHiiConfigRoutingProtocolGuid,
+                  &mPrivate.ConfigRouting,
+                  &gEfiConfigKeywordHandlerProtocolGuid,
+                  &mPrivate.ConfigKeywordHandler,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (FeaturePcdGet (PcdSupportHiiImageProtocol)) {
+    CopyMem (&mPrivate.HiiImage, &mImageProtocol, sizeof (mImageProtocol));
+
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &Handle,
+                    &gEfiHiiImageProtocolGuid,
+                    &mPrivate.HiiImage,
+                    NULL
+                    );
+
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseExtra.uni b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseExtra.uni
new file mode 100644
index 0000000000..d9687316dc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/HiiDatabaseExtra.uni
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Image.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Image.c
new file mode 100644
index 0000000000..c46c96545e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/Image.c
@@ -0,0 +1,1498 @@
+/** @file
+Implementation for EFI_HII_IMAGE_PROTOCOL.
+
+
+Copyright (c) 2007 - 2016, 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 "HiiDatabase.h"
+
+
+/**
+  Get the imageid of last image block: EFI_HII_IIBT_END_BLOCK when input
+  ImageId is zero, otherwise return the address of the
+  corresponding image block with identifier specified by ImageId.
+
+  This is a internal function.
+
+  @param ImageBlock      Points to the beginning of a series of image blocks stored in order.
+  @param ImageId         If input ImageId is 0, output the image id of the EFI_HII_IIBT_END_BLOCK;
+                         else use this id to find its corresponding image block address.
+
+  @return The image block address when input ImageId is not zero; otherwise return NULL.
+
+**/
+UINT8*
+GetImageIdOrAddress (
+  IN  UINT8           *ImageBlock,
+  IN OUT EFI_IMAGE_ID *ImageId
+  )
+{
+  EFI_IMAGE_ID                   ImageIdCurrent;
+  UINT8                          *ImageBlockHdr;
+  UINT8                          Length8;
+  UINT16                         Length16;
+  UINT32                         Length32;
+  EFI_HII_IIBT_IMAGE_1BIT_BLOCK  Iibt1bit;
+  EFI_HII_IIBT_IMAGE_4BIT_BLOCK  Iibt4bit;
+  EFI_HII_IIBT_IMAGE_8BIT_BLOCK  Iibt8bit;
+  UINT16                         Width;
+  UINT16                         Height;
+
+  ASSERT (ImageBlock != NULL && ImageId != NULL);
+
+  ImageBlockHdr  = ImageBlock;
+  ImageIdCurrent = 1;
+
+  while (((EFI_HII_IMAGE_BLOCK *) ImageBlock)->BlockType != EFI_HII_IIBT_END) {
+    if (*ImageId > 0) {
+      if (*ImageId == ImageIdCurrent) {
+        //
+        // If the found image block is a duplicate block, update the ImageId to
+        // find the previous defined image block.
+        //
+        if (((EFI_HII_IMAGE_BLOCK *) ImageBlock)->BlockType == EFI_HII_IIBT_DUPLICATE) {
+          CopyMem (ImageId, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (EFI_IMAGE_ID));
+          ASSERT (*ImageId != ImageIdCurrent);
+          ImageBlock = ImageBlockHdr;
+          ImageIdCurrent = 1;
+          continue;
+        }
+
+        return ImageBlock;
+      }
+      if (*ImageId < ImageIdCurrent) {
+        //
+        // Can not find the specified image block in this image.
+        //
+        return NULL;
+      }
+    }
+    switch (((EFI_HII_IMAGE_BLOCK *) ImageBlock)->BlockType) {
+    case EFI_HII_IIBT_EXT1:
+      Length8 = *(UINT8*)((UINTN)ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT8));
+      ImageBlock += Length8;
+      break;
+    case EFI_HII_IIBT_EXT2:
+      CopyMem (
+        &Length16,
+        (UINT8*)((UINTN)ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      ImageBlock += Length16;
+      break;
+    case EFI_HII_IIBT_EXT4:
+      CopyMem (
+        &Length32,
+        (UINT8*)((UINTN)ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT32)
+        );
+      ImageBlock += Length32;
+      break;
+
+    case EFI_HII_IIBT_IMAGE_1BIT:
+    case EFI_HII_IIBT_IMAGE_1BIT_TRANS:
+      CopyMem (&Iibt1bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK));
+      ImageBlock += sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK) - sizeof (UINT8) +
+                    BITMAP_LEN_1_BIT (Iibt1bit.Bitmap.Width, Iibt1bit.Bitmap.Height);
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_IMAGE_4BIT:
+    case EFI_HII_IIBT_IMAGE_4BIT_TRANS:
+      CopyMem (&Iibt4bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_4BIT_BLOCK));
+      ImageBlock += sizeof (EFI_HII_IIBT_IMAGE_4BIT_BLOCK) - sizeof (UINT8) +
+                    BITMAP_LEN_4_BIT (Iibt4bit.Bitmap.Width, Iibt4bit.Bitmap.Height);
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_IMAGE_8BIT:
+    case EFI_HII_IIBT_IMAGE_8BIT_TRANS:
+      CopyMem (&Iibt8bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_8BIT_BLOCK));
+      ImageBlock += sizeof (EFI_HII_IIBT_IMAGE_8BIT_BLOCK) - sizeof (UINT8) +
+                    BITMAP_LEN_8_BIT (Iibt8bit.Bitmap.Width, Iibt8bit.Bitmap.Height);
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_IMAGE_24BIT:
+    case EFI_HII_IIBT_IMAGE_24BIT_TRANS:
+      CopyMem (&Width, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (UINT16));
+      CopyMem (
+        &Height,
+        ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT16),
+        sizeof (UINT16)
+        );
+      ImageBlock += sizeof (EFI_HII_IIBT_IMAGE_24BIT_BLOCK) - sizeof (EFI_HII_RGB_PIXEL) +
+                    BITMAP_LEN_24_BIT (Width, Height);
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_DUPLICATE:
+      ImageBlock += sizeof (EFI_HII_IIBT_DUPLICATE_BLOCK);
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_IMAGE_JPEG:
+      CopyMem (&Length32, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (UINT32));
+      ImageBlock += Length32;
+      ImageIdCurrent++;
+      break;
+
+    case EFI_HII_IIBT_SKIP1:
+      Length8 = *(ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK));
+      ImageBlock += sizeof (EFI_HII_IIBT_SKIP1_BLOCK);
+      ImageIdCurrent = (UINT16) (ImageIdCurrent + Length8);
+      break;
+
+    case EFI_HII_IIBT_SKIP2:
+      CopyMem (&Length16, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (UINT16));
+      ImageBlock += sizeof (EFI_HII_IIBT_SKIP2_BLOCK);
+      ImageIdCurrent = (UINT16) (ImageIdCurrent + Length16);
+      break;
+
+    default:
+      //
+      // Unknown image blocks can not be skipped, processing halts.
+      //
+      ASSERT (FALSE);
+    }
+  }
+
+  //
+  // When ImageId is zero, return the imageid of last image block: EFI_HII_IIBT_END_BLOCK.
+  //
+  if (*ImageId == 0) {
+    *ImageId = ImageIdCurrent;
+    return ImageBlock;
+  }
+
+  return NULL;
+}
+
+
+
+/**
+  Convert pixels from EFI_GRAPHICS_OUTPUT_BLT_PIXEL to EFI_HII_RGB_PIXEL style.
+
+  This is a internal function.
+
+
+  @param  BitMapOut              Pixels in EFI_HII_RGB_PIXEL format.
+  @param  BitMapIn               Pixels in EFI_GRAPHICS_OUTPUT_BLT_PIXEL format.
+  @param  PixelNum               The number of pixels to be converted.
+
+
+**/
+VOID
+CopyGopToRgbPixel (
+  OUT EFI_HII_RGB_PIXEL              *BitMapOut,
+  IN  EFI_GRAPHICS_OUTPUT_BLT_PIXEL  *BitMapIn,
+  IN  UINTN                          PixelNum
+  )
+{
+  UINTN Index;
+
+  ASSERT (BitMapOut != NULL && BitMapIn != NULL);
+
+  for (Index = 0; Index < PixelNum; Index++) {
+    CopyMem (BitMapOut + Index, BitMapIn + Index, sizeof (EFI_HII_RGB_PIXEL));
+  }
+}
+
+
+/**
+  Convert pixels from EFI_HII_RGB_PIXEL to EFI_GRAPHICS_OUTPUT_BLT_PIXEL style.
+
+  This is a internal function.
+
+
+  @param  BitMapOut              Pixels in EFI_GRAPHICS_OUTPUT_BLT_PIXEL format.
+  @param  BitMapIn               Pixels in EFI_HII_RGB_PIXEL format.
+  @param  PixelNum               The number of pixels to be converted.
+
+
+**/
+VOID
+CopyRgbToGopPixel (
+  OUT EFI_GRAPHICS_OUTPUT_BLT_PIXEL  *BitMapOut,
+  IN  EFI_HII_RGB_PIXEL              *BitMapIn,
+  IN  UINTN                          PixelNum
+  )
+{
+  UINTN Index;
+
+  ASSERT (BitMapOut != NULL && BitMapIn != NULL);
+
+  for (Index = 0; Index < PixelNum; Index++) {
+    CopyMem (BitMapOut + Index, BitMapIn + Index, sizeof (EFI_HII_RGB_PIXEL));
+  }
+}
+
+
+/**
+  Output pixels in "1 bit per pixel" format to an image.
+
+  This is a internal function.
+
+
+  @param  Image                  Points to the image which will store the pixels.
+  @param  Data                   Stores the value of output pixels, 0 or 1.
+  @param  PaletteInfo            PaletteInfo which stores the color of the output
+                                 pixels. First entry corresponds to color 0 and
+                                 second one to color 1.
+
+
+**/
+VOID
+Output1bitPixel (
+  IN OUT EFI_IMAGE_INPUT             *Image,
+  IN UINT8                           *Data,
+  IN EFI_HII_IMAGE_PALETTE_INFO      *PaletteInfo
+  )
+{
+  UINT16                             Xpos;
+  UINT16                             Ypos;
+  UINTN                              OffsetY;
+  UINT8                              Index;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      *BitMapPtr;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      PaletteValue[2];
+  EFI_HII_IMAGE_PALETTE_INFO         *Palette;
+  UINTN                              PaletteSize;
+  UINT8                              Byte;
+
+  ASSERT (Image != NULL && Data != NULL && PaletteInfo != NULL);
+
+  BitMapPtr = Image->Bitmap;
+
+  //
+  // First entry corresponds to color 0 and second entry corresponds to color 1.
+  //
+  PaletteSize = 0;
+  CopyMem (&PaletteSize, PaletteInfo, sizeof (UINT16));
+  PaletteSize += sizeof (UINT16);
+  Palette = AllocateZeroPool (PaletteSize);
+  ASSERT (Palette != NULL);
+  if (Palette == NULL) {
+    return;
+  }
+  CopyMem (Palette, PaletteInfo, PaletteSize);
+
+  ZeroMem (PaletteValue, sizeof (PaletteValue));
+  CopyRgbToGopPixel (&PaletteValue[0], &Palette->PaletteValue[0], 1);
+  CopyRgbToGopPixel (&PaletteValue[1], &Palette->PaletteValue[1], 1);
+  FreePool (Palette);
+
+  //
+  // Convert the pixel from one bit to corresponding color.
+  //
+  for (Ypos = 0; Ypos < Image->Height; Ypos++) {
+    OffsetY = BITMAP_LEN_1_BIT (Image->Width, Ypos);
+    //
+    // All bits in these bytes are meaningful
+    //
+    for (Xpos = 0; Xpos < Image->Width / 8; Xpos++) {
+      Byte = *(Data + OffsetY + Xpos);
+      for (Index = 0; Index < 8; Index++) {
+        if ((Byte & (1 << Index)) != 0) {
+          BitMapPtr[Ypos * Image->Width + Xpos * 8 + (8 - Index - 1)] = PaletteValue[1];
+        } else {
+          BitMapPtr[Ypos * Image->Width + Xpos * 8 + (8 - Index - 1)] = PaletteValue[0];
+        }
+      }
+    }
+
+    if (Image->Width % 8 != 0) {
+      //
+      // Padding bits in this byte should be ignored.
+      //
+      Byte = *(Data + OffsetY + Xpos);
+      for (Index = 0; Index < Image->Width % 8; Index++) {
+        if ((Byte & (1 << (8 - Index - 1))) != 0) {
+          BitMapPtr[Ypos * Image->Width + Xpos * 8 + Index] = PaletteValue[1];
+        } else {
+          BitMapPtr[Ypos * Image->Width + Xpos * 8 + Index] = PaletteValue[0];
+        }
+      }
+    }
+  }
+}
+
+
+/**
+  Output pixels in "4 bit per pixel" format to an image.
+
+  This is a internal function.
+
+
+  @param  Image                  Points to the image which will store the pixels.
+  @param  Data                   Stores the value of output pixels, 0 ~ 15.
+  @param[in]  PaletteInfo            PaletteInfo which stores the color of the output
+                                 pixels. Each entry corresponds to a color within
+                                 [0, 15].
+
+
+**/
+VOID
+Output4bitPixel (
+  IN OUT EFI_IMAGE_INPUT             *Image,
+  IN UINT8                           *Data,
+  IN EFI_HII_IMAGE_PALETTE_INFO      *PaletteInfo
+  )
+{
+  UINT16                             Xpos;
+  UINT16                             Ypos;
+  UINTN                              OffsetY;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      *BitMapPtr;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      PaletteValue[16];
+  EFI_HII_IMAGE_PALETTE_INFO         *Palette;
+  UINTN                              PaletteSize;
+  UINT16                             PaletteNum;
+  UINT8                              Byte;
+
+  ASSERT (Image != NULL && Data != NULL && PaletteInfo != NULL);
+
+  BitMapPtr = Image->Bitmap;
+
+  //
+  // The bitmap should allocate each color index starting from 0.
+  //
+  PaletteSize = 0;
+  CopyMem (&PaletteSize, PaletteInfo, sizeof (UINT16));
+  PaletteSize += sizeof (UINT16);
+  Palette = AllocateZeroPool (PaletteSize);
+  ASSERT (Palette != NULL);
+  if (Palette == NULL) {
+    return;
+  }
+  CopyMem (Palette, PaletteInfo, PaletteSize);
+  PaletteNum = (UINT16)(Palette->PaletteSize / sizeof (EFI_HII_RGB_PIXEL));
+
+  ZeroMem (PaletteValue, sizeof (PaletteValue));
+  CopyRgbToGopPixel (PaletteValue, Palette->PaletteValue, PaletteNum);
+  FreePool (Palette);
+
+  //
+  // Convert the pixel from 4 bit to corresponding color.
+  //
+  for (Ypos = 0; Ypos < Image->Height; Ypos++) {
+    OffsetY = BITMAP_LEN_4_BIT (Image->Width, Ypos);
+    //
+    // All bits in these bytes are meaningful
+    //
+    for (Xpos = 0; Xpos < Image->Width / 2; Xpos++) {
+      Byte = *(Data + OffsetY + Xpos);
+      BitMapPtr[Ypos * Image->Width + Xpos * 2]     = PaletteValue[Byte >> 4];
+      BitMapPtr[Ypos * Image->Width + Xpos * 2 + 1] = PaletteValue[Byte & 0x0F];
+    }
+
+    if (Image->Width % 2 != 0) {
+      //
+      // Padding bits in this byte should be ignored.
+      //
+      Byte = *(Data + OffsetY + Xpos);
+      BitMapPtr[Ypos * Image->Width + Xpos * 2]     = PaletteValue[Byte >> 4];
+    }
+  }
+}
+
+
+/**
+  Output pixels in "8 bit per pixel" format to an image.
+
+  This is a internal function.
+
+
+  @param  Image                  Points to the image which will store the pixels.
+  @param  Data                   Stores the value of output pixels, 0 ~ 255.
+  @param[in]  PaletteInfo        PaletteInfo which stores the color of the output
+                                 pixels. Each entry corresponds to a color within
+                                 [0, 255].
+
+
+**/
+VOID
+Output8bitPixel (
+  IN OUT EFI_IMAGE_INPUT             *Image,
+  IN UINT8                           *Data,
+  IN EFI_HII_IMAGE_PALETTE_INFO      *PaletteInfo
+  )
+{
+  UINT16                             Xpos;
+  UINT16                             Ypos;
+  UINTN                              OffsetY;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      *BitMapPtr;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      PaletteValue[256];
+  EFI_HII_IMAGE_PALETTE_INFO         *Palette;
+  UINTN                              PaletteSize;
+  UINT16                             PaletteNum;
+  UINT8                              Byte;
+
+  ASSERT (Image != NULL && Data != NULL && PaletteInfo != NULL);
+
+  BitMapPtr = Image->Bitmap;
+
+  //
+  // The bitmap should allocate each color index starting from 0.
+  //
+  PaletteSize = 0;
+  CopyMem (&PaletteSize, PaletteInfo, sizeof (UINT16));
+  PaletteSize += sizeof (UINT16);
+  Palette = AllocateZeroPool (PaletteSize);
+  ASSERT (Palette != NULL);
+  if (Palette == NULL) {
+    return;
+  }
+  CopyMem (Palette, PaletteInfo, PaletteSize);
+  PaletteNum = (UINT16)(Palette->PaletteSize / sizeof (EFI_HII_RGB_PIXEL));
+  ZeroMem (PaletteValue, sizeof (PaletteValue));
+  CopyRgbToGopPixel (PaletteValue, Palette->PaletteValue, PaletteNum);
+  FreePool (Palette);
+
+  //
+  // Convert the pixel from 8 bits to corresponding color.
+  //
+  for (Ypos = 0; Ypos < Image->Height; Ypos++) {
+    OffsetY = BITMAP_LEN_8_BIT (Image->Width, Ypos);
+    //
+    // All bits are meaningful since the bitmap is 8 bits per pixel.
+    //
+    for (Xpos = 0; Xpos < Image->Width; Xpos++) {
+      Byte = *(Data + OffsetY + Xpos);
+      BitMapPtr[OffsetY + Xpos] = PaletteValue[Byte];
+    }
+  }
+
+}
+
+
+/**
+  Output pixels in "24 bit per pixel" format to an image.
+
+  This is a internal function.
+
+
+  @param  Image                  Points to the image which will store the pixels.
+  @param  Data                   Stores the color of output pixels, allowing 16.8
+                                 millions colors.
+
+
+**/
+VOID
+Output24bitPixel (
+  IN OUT EFI_IMAGE_INPUT             *Image,
+  IN EFI_HII_RGB_PIXEL               *Data
+  )
+{
+  UINT16                             Ypos;
+  UINTN                              OffsetY;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      *BitMapPtr;
+
+  ASSERT (Image != NULL && Data != NULL);
+
+  BitMapPtr = Image->Bitmap;
+
+  for (Ypos = 0; Ypos < Image->Height; Ypos++) {
+    OffsetY = BITMAP_LEN_8_BIT (Image->Width, Ypos);
+    CopyRgbToGopPixel (&BitMapPtr[OffsetY], &Data[OffsetY], Image->Width);
+  }
+
+}
+
+
+/**
+  Convert the image from EFI_IMAGE_INPUT to EFI_IMAGE_OUTPUT format.
+
+  This is a internal function.
+
+
+  @param  BltBuffer              Buffer points to bitmap data of incoming image.
+  @param  BltX                   Specifies the offset from the left and top edge of
+                                  the output image of the first pixel in the image.
+  @param  BltY                   Specifies the offset from the left and top edge of
+                                  the output image of the first pixel in the image.
+  @param  Width                  Width of the incoming image, in pixels.
+  @param  Height                 Height of the incoming image, in pixels.
+  @param  Transparent            If TRUE, all "off" pixels in the image will be
+                                 drawn using the pixel value from blt and all other
+                                 pixels will be copied.
+  @param  Blt                    Buffer points to bitmap data of output image.
+
+  @retval EFI_SUCCESS            The image was successfully converted.
+  @retval EFI_INVALID_PARAMETER  Any incoming parameter is invalid.
+
+**/
+EFI_STATUS
+ImageToBlt (
+  IN EFI_GRAPHICS_OUTPUT_BLT_PIXEL   *BltBuffer,
+  IN UINTN                           BltX,
+  IN UINTN                           BltY,
+  IN UINTN                           Width,
+  IN UINTN                           Height,
+  IN BOOLEAN                         Transparent,
+  IN OUT EFI_IMAGE_OUTPUT            **Blt
+  )
+{
+  EFI_IMAGE_OUTPUT                   *ImageOut;
+  UINTN                              Xpos;
+  UINTN                              Ypos;
+  UINTN                              OffsetY1; // src buffer
+  UINTN                              OffsetY2; // dest buffer
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      SrcPixel;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL      ZeroPixel;
+
+  if (BltBuffer == NULL || Blt == NULL || *Blt == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ImageOut = *Blt;
+
+  if (Width + BltX > ImageOut->Width) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (Height + BltY > ImageOut->Height) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ZeroMem (&ZeroPixel, sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL));
+
+  for (Ypos = 0; Ypos < Height; Ypos++) {
+    OffsetY1 = Width * Ypos;
+    OffsetY2 = ImageOut->Width * (BltY + Ypos);
+    for (Xpos = 0; Xpos < Width; Xpos++) {
+      SrcPixel = BltBuffer[OffsetY1 + Xpos];
+      if (Transparent) {
+        if (CompareMem (&SrcPixel, &ZeroPixel, 3) != 0) {
+          ImageOut->Image.Bitmap[OffsetY2 + BltX + Xpos] = SrcPixel;
+        }
+      } else {
+        ImageOut->Image.Bitmap[OffsetY2 + BltX + Xpos] = SrcPixel;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function adds the image Image to the group of images owned by PackageList, and returns
+  a new image identifier (ImageId).
+
+  @param  This                   A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList            Handle of the package list where this image will
+                                 be added.
+  @param  ImageId                On return, contains the new image id, which is
+                                 unique within PackageList.
+  @param  Image                  Points to the image.
+
+  @retval EFI_SUCCESS            The new image was added successfully.
+  @retval EFI_NOT_FOUND          The specified PackageList could not be found in
+                                 database.
+  @retval EFI_OUT_OF_RESOURCES   Could not add the image due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  Image is NULL or ImageId is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewImage (
+  IN  CONST EFI_HII_IMAGE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                 PackageList,
+  OUT EFI_IMAGE_ID                   *ImageId,
+  IN  CONST EFI_IMAGE_INPUT          *Image
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_IMAGE_PACKAGE_INSTANCE          *ImagePackage;
+  UINT8                               *ImageBlock;
+  UINTN                               BlockSize;
+  UINT8                               *NewBlock;
+  UINT8                               *NewBlockPtr;
+  UINTN                               NewBlockSize;
+  EFI_IMAGE_INPUT                     *ImageIn;
+
+  if (This == NULL || ImageId == NULL || Image == NULL || Image->Bitmap == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_IMAGE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Get the specified package list
+  //
+
+  PackageListNode = NULL;
+
+  for (Link = Private->DatabaseList.ForwardLink;
+       Link != &Private->DatabaseList;
+       Link = Link->ForwardLink
+      ) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+
+  if (PackageListNode == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  ImageIn = (EFI_IMAGE_INPUT *) Image;
+
+  NewBlockSize = sizeof (EFI_HII_IIBT_IMAGE_24BIT_BLOCK) - sizeof (EFI_HII_RGB_PIXEL) +
+                 BITMAP_LEN_24_BIT (ImageIn->Width, ImageIn->Height);
+
+  //
+  // Get the image package in the package list,
+  // or create a new image package if image package does not exist.
+  //
+  if (PackageListNode->ImagePkg != NULL) {
+    ImagePackage = PackageListNode->ImagePkg;
+
+    //
+    // Output the image id of the incoming image being inserted, which is the
+    // image id of the EFI_HII_IIBT_END block of old image package.
+    //
+    *ImageId = 0;
+    GetImageIdOrAddress (ImagePackage->ImageBlock, ImageId);
+
+    //
+    // Update the package's image block by appending the new block to the end.
+    //
+    BlockSize  = ImagePackage->ImageBlockSize + NewBlockSize;
+    ImageBlock = (UINT8 *) AllocateZeroPool (BlockSize);
+    if (ImageBlock == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Copy the original content.
+    //
+    CopyMem (
+      ImageBlock,
+      ImagePackage->ImageBlock,
+      ImagePackage->ImageBlockSize - sizeof (EFI_HII_IIBT_END_BLOCK)
+      );
+    FreePool (ImagePackage->ImageBlock);
+    ImagePackage->ImageBlock = ImageBlock;
+    ImageBlock += ImagePackage->ImageBlockSize - sizeof (EFI_HII_IIBT_END_BLOCK);
+    //
+    // Temp memory to store new block.
+    //
+    NewBlock = AllocateZeroPool (NewBlockSize);
+    if (NewBlock == NULL) {
+      FreePool (ImagePackage->ImageBlock);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    NewBlockPtr = NewBlock;
+
+    //
+    // Update the length record.
+    //
+    ImagePackage->ImageBlockSize = (UINT32) BlockSize;
+    ImagePackage->ImagePkgHdr.Header.Length += (UINT32) NewBlockSize;
+    PackageListNode->PackageListHdr.PackageLength += (UINT32) NewBlockSize;
+
+  } else {
+    //
+    // The specified package list does not contain image package.
+    // Create one to add this image block.
+    //
+    ImagePackage = (HII_IMAGE_PACKAGE_INSTANCE *) AllocateZeroPool (sizeof (HII_IMAGE_PACKAGE_INSTANCE));
+    if (ImagePackage == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    //
+    // Output the image id of the incoming image being inserted, which is the
+    // first image block so that id is initially to one.
+    //
+    *ImageId = 1;
+    BlockSize    = sizeof (EFI_HII_IIBT_END_BLOCK) + NewBlockSize;
+    //
+    // Fill in image package header.
+    //
+    ImagePackage->ImagePkgHdr.Header.Length     = (UINT32) BlockSize + sizeof (EFI_HII_IMAGE_PACKAGE_HDR);
+    ImagePackage->ImagePkgHdr.Header.Type       = EFI_HII_PACKAGE_IMAGES;
+    ImagePackage->ImagePkgHdr.ImageInfoOffset   = sizeof (EFI_HII_IMAGE_PACKAGE_HDR);
+    ImagePackage->ImagePkgHdr.PaletteInfoOffset = 0;
+
+    //
+    // Fill in palette info.
+    //
+    ImagePackage->PaletteBlock    = NULL;
+    ImagePackage->PaletteInfoSize = 0;
+
+    //
+    // Fill in image blocks.
+    //
+    ImagePackage->ImageBlockSize = (UINT32) BlockSize;
+    ImagePackage->ImageBlock = (UINT8 *) AllocateZeroPool (BlockSize);
+    if (ImagePackage->ImageBlock == NULL) {
+      FreePool (ImagePackage);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    ImageBlock = ImagePackage->ImageBlock;
+
+    //
+    // Temp memory to store new block.
+    //
+    NewBlock = AllocateZeroPool (NewBlockSize);
+    if (NewBlock == NULL) {
+      FreePool (ImagePackage->ImageBlock);
+      FreePool (ImagePackage);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    NewBlockPtr = NewBlock;
+
+    //
+    // Insert this image package.
+    //
+    PackageListNode->ImagePkg = ImagePackage;
+    PackageListNode->PackageListHdr.PackageLength += ImagePackage->ImagePkgHdr.Header.Length;
+  }
+
+  //
+  // Append the new block here
+  //
+  if (ImageIn->Flags == EFI_IMAGE_TRANSPARENT) {
+    *NewBlock = EFI_HII_IIBT_IMAGE_24BIT_TRANS;
+  } else {
+    *NewBlock = EFI_HII_IIBT_IMAGE_24BIT;
+  }
+  NewBlock++;
+  CopyMem (NewBlock, &ImageIn->Width, sizeof (UINT16));
+  NewBlock += sizeof (UINT16);
+  CopyMem (NewBlock, &ImageIn->Height, sizeof (UINT16));
+  NewBlock += sizeof (UINT16);
+  CopyGopToRgbPixel ((EFI_HII_RGB_PIXEL *) NewBlock, ImageIn->Bitmap, ImageIn->Width * ImageIn->Height);
+
+  CopyMem (ImageBlock, NewBlockPtr, NewBlockSize);
+  FreePool (NewBlockPtr);
+
+  //
+  // Append the block end
+  //
+  ImageBlock += NewBlockSize;
+  ((EFI_HII_IIBT_END_BLOCK *) (ImageBlock))->Header.BlockType = EFI_HII_IIBT_END;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function retrieves the image specified by ImageId which is associated with
+  the specified PackageList and copies it into the buffer specified by Image.
+
+  @param  This                   A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList            Handle of the package list where this image will
+                                 be searched.
+  @param  ImageId                The image's id,, which is unique within
+                                 PackageList.
+  @param  Image                  Points to the image.
+
+  @retval EFI_SUCCESS            The new image was returned successfully.
+  @retval EFI_NOT_FOUND           The image specified by ImageId is not in the
+                                                database. The specified PackageList is not in the database.
+  @retval EFI_BUFFER_TOO_SMALL   The buffer specified by ImageSize is too small to
+                                 hold the image.
+  @retval EFI_INVALID_PARAMETER  The Image or ImageSize was NULL.
+  @retval EFI_OUT_OF_RESOURCES   The bitmap could not be retrieved because there was not
+                                                     enough memory.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetImage (
+  IN  CONST EFI_HII_IMAGE_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                 PackageList,
+  IN  EFI_IMAGE_ID                   ImageId,
+  OUT EFI_IMAGE_INPUT                *Image
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_IMAGE_PACKAGE_INSTANCE          *ImagePackage;
+  UINT8                               *ImageBlock;
+  EFI_IMAGE_ID                        LocalImageId;
+  UINT8                               BlockType;
+  EFI_HII_IIBT_IMAGE_1BIT_BLOCK       Iibt1bit;
+  UINT16                              Width;
+  UINT16                              Height;
+  UINTN                               ImageLength;
+  BOOLEAN                             Flag;
+  UINT8                               *PaletteInfo;
+  UINT8                               PaletteIndex;
+  UINT16                              PaletteSize;
+
+  if (This == NULL || Image == NULL || ImageId < 1) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_IMAGE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Get the specified package list and image package.
+  //
+  PackageListNode = NULL;
+  for (Link = Private->DatabaseList.ForwardLink;
+       Link != &Private->DatabaseList;
+       Link = Link->ForwardLink
+      ) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+  if (PackageListNode == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  ImagePackage = PackageListNode->ImagePkg;
+  if (ImagePackage == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Find the image block specified by ImageId
+  //
+  LocalImageId = ImageId;
+  ImageBlock = GetImageIdOrAddress (ImagePackage->ImageBlock, &LocalImageId);
+  if (ImageBlock == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Flag      = FALSE;
+  BlockType = *ImageBlock;
+
+  switch (BlockType) {
+  case EFI_HII_IIBT_IMAGE_JPEG:
+    //
+    // BUGBUG: need to be supported as soon as image tool is designed.
+    //
+    return EFI_UNSUPPORTED;
+
+  case EFI_HII_IIBT_IMAGE_1BIT_TRANS:
+  case EFI_HII_IIBT_IMAGE_4BIT_TRANS:
+  case EFI_HII_IIBT_IMAGE_8BIT_TRANS:
+    Flag = TRUE;
+    //
+    // fall through
+    //
+  case EFI_HII_IIBT_IMAGE_1BIT:
+  case EFI_HII_IIBT_IMAGE_4BIT:
+  case EFI_HII_IIBT_IMAGE_8BIT:
+    //
+    // Use the common block code since the definition of these structures is the same.
+    //
+    CopyMem (&Iibt1bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK));
+    ImageLength = sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL) *
+                  (Iibt1bit.Bitmap.Width * Iibt1bit.Bitmap.Height);
+    Image->Bitmap = (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) AllocateZeroPool (ImageLength);
+    if (Image->Bitmap == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    if (Flag) {
+      Image->Flags = EFI_IMAGE_TRANSPARENT;
+    }
+    Image->Width  = Iibt1bit.Bitmap.Width;
+    Image->Height = Iibt1bit.Bitmap.Height;
+
+    PaletteInfo = ImagePackage->PaletteBlock + sizeof (EFI_HII_IMAGE_PALETTE_INFO_HEADER);
+    for (PaletteIndex = 1; PaletteIndex < Iibt1bit.PaletteIndex; PaletteIndex++) {
+      CopyMem (&PaletteSize, PaletteInfo, sizeof (UINT16));
+      PaletteInfo += PaletteSize + sizeof (UINT16);
+    }
+    ASSERT (PaletteIndex == Iibt1bit.PaletteIndex);
+
+    //
+    // Output bitmap data
+    //
+    if (BlockType == EFI_HII_IIBT_IMAGE_1BIT || BlockType == EFI_HII_IIBT_IMAGE_1BIT_TRANS) {
+      Output1bitPixel (
+        Image,
+        (UINT8 *) ((UINTN)ImageBlock + sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK) - sizeof (UINT8)),
+        (EFI_HII_IMAGE_PALETTE_INFO *) PaletteInfo
+        );
+    } else if (BlockType == EFI_HII_IIBT_IMAGE_4BIT || BlockType == EFI_HII_IIBT_IMAGE_4BIT_TRANS) {
+      Output4bitPixel (
+        Image,
+        (UINT8 *) ((UINTN)ImageBlock + sizeof (EFI_HII_IIBT_IMAGE_4BIT_BLOCK) - sizeof (UINT8)),
+        (EFI_HII_IMAGE_PALETTE_INFO *) PaletteInfo
+        );
+    } else {
+      Output8bitPixel (
+        Image,
+        (UINT8 *) ((UINTN)ImageBlock + sizeof (EFI_HII_IIBT_IMAGE_8BIT_BLOCK) - sizeof (UINT8)),
+        (EFI_HII_IMAGE_PALETTE_INFO *) PaletteInfo
+        );
+    }
+
+    return EFI_SUCCESS;
+
+  case EFI_HII_IIBT_IMAGE_24BIT_TRANS:
+    Flag = TRUE;
+    //
+    // fall through
+    //
+  case EFI_HII_IIBT_IMAGE_24BIT:
+    CopyMem (&Width, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (UINT16));
+    CopyMem (
+      &Height,
+      ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT16),
+      sizeof (UINT16)
+      );
+    ImageLength = sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL) * (Width * Height);
+    Image->Bitmap = (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) AllocateZeroPool (ImageLength);
+    if (Image->Bitmap == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    if (Flag) {
+      Image->Flags = EFI_IMAGE_TRANSPARENT;
+    }
+    Image->Width  = Width;
+    Image->Height = Height;
+
+    //
+    // Output the bimap data directly.
+    //
+    Output24bitPixel (
+      Image,
+      (EFI_HII_RGB_PIXEL *) (ImageBlock + sizeof (EFI_HII_IIBT_IMAGE_24BIT_BLOCK) - sizeof (EFI_HII_RGB_PIXEL))
+      );
+    return EFI_SUCCESS;
+
+  default:
+    return EFI_NOT_FOUND;
+  }
+}
+
+
+/**
+  This function updates the image specified by ImageId in the specified PackageListHandle to
+  the image specified by Image.
+
+  @param  This                   A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  PackageList            The package list containing the images.
+  @param  ImageId                The image's id,, which is unique within
+                                 PackageList.
+  @param  Image                  Points to the image.
+
+  @retval EFI_SUCCESS            The new image was updated successfully.
+  @retval EFI_NOT_FOUND          The image specified by ImageId is not in the
+                                                database. The specified PackageList is not in the database.    
+  @retval EFI_INVALID_PARAMETER  The Image was NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetImage (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN EFI_IMAGE_ID                    ImageId,
+  IN CONST EFI_IMAGE_INPUT           *Image
+  )
+{
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_IMAGE_PACKAGE_INSTANCE          *ImagePackage;
+  UINT8                               *ImageBlock;
+  EFI_IMAGE_ID                        LocalImageId;
+  UINT8                               BlockType;
+  EFI_HII_IIBT_IMAGE_1BIT_BLOCK       Iibt1bit;
+  EFI_HII_IIBT_IMAGE_4BIT_BLOCK       Iibt4bit;
+  EFI_HII_IIBT_IMAGE_8BIT_BLOCK       Iibt8bit;
+  UINT16                              Width;
+  UINT16                              Height;
+  UINT32                              BlockSize;
+  UINT32                              NewBlockSize;
+  UINT32                              OldBlockSize;
+  EFI_IMAGE_INPUT                     *ImageIn;
+  UINT8                               *NewBlock;
+  UINT8                               *NewBlockPtr;
+  UINT8                               *Block;
+  UINT8                               *BlockPtr;
+  UINT32                               Part1Size;
+  UINT32                               Part2Size;
+
+  if (This == NULL || Image == NULL || ImageId < 1 || Image->Bitmap == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_IMAGE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // Get the specified package list and image package.
+  //
+  PackageListNode = NULL;
+  for (Link = Private->DatabaseList.ForwardLink;
+       Link != &Private->DatabaseList;
+       Link = Link->ForwardLink
+      ) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+  if (PackageListNode == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  ImagePackage = PackageListNode->ImagePkg;
+  if (ImagePackage == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Find the image block specified by ImageId
+  //
+  LocalImageId = ImageId;
+  ImageBlock = GetImageIdOrAddress (ImagePackage->ImageBlock, &LocalImageId);
+  if (ImageBlock == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  BlockType = *ImageBlock;
+
+  //
+  // Get the size of original image block. Use some common block code here
+  // since the definition of some structures is the same.
+  //
+  switch (BlockType) {
+  case EFI_HII_IIBT_IMAGE_JPEG:
+    //
+    // BUGBUG: need to be supported as soon as image tool is designed.
+    //
+    return EFI_UNSUPPORTED;
+
+  case EFI_HII_IIBT_IMAGE_1BIT:
+  case EFI_HII_IIBT_IMAGE_1BIT_TRANS:
+    CopyMem (&Iibt1bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK));
+    OldBlockSize = sizeof (EFI_HII_IIBT_IMAGE_1BIT_BLOCK) - sizeof (UINT8) +
+                   BITMAP_LEN_1_BIT (Iibt1bit.Bitmap.Width, Iibt1bit.Bitmap.Height);
+    break;
+  case EFI_HII_IIBT_IMAGE_4BIT:
+  case EFI_HII_IIBT_IMAGE_4BIT_TRANS:
+    CopyMem (&Iibt4bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_4BIT_BLOCK));
+    OldBlockSize = sizeof (EFI_HII_IIBT_IMAGE_4BIT_BLOCK) - sizeof (UINT8) +
+                   BITMAP_LEN_4_BIT (Iibt4bit.Bitmap.Width, Iibt4bit.Bitmap.Height);
+    break;
+  case EFI_HII_IIBT_IMAGE_8BIT:
+  case EFI_HII_IIBT_IMAGE_8BIT_TRANS:
+    CopyMem (&Iibt8bit, ImageBlock, sizeof (EFI_HII_IIBT_IMAGE_8BIT_BLOCK));
+    OldBlockSize = sizeof (EFI_HII_IIBT_IMAGE_8BIT_BLOCK) - sizeof (UINT8) +
+                   BITMAP_LEN_8_BIT (Iibt8bit.Bitmap.Width, Iibt8bit.Bitmap.Height);
+    break;
+  case EFI_HII_IIBT_IMAGE_24BIT:
+  case EFI_HII_IIBT_IMAGE_24BIT_TRANS:
+    CopyMem (&Width, ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK), sizeof (UINT16));
+    CopyMem (
+      &Height,
+      ImageBlock + sizeof (EFI_HII_IMAGE_BLOCK) + sizeof (UINT16),
+      sizeof (UINT16)
+      );
+    OldBlockSize = sizeof (EFI_HII_IIBT_IMAGE_24BIT_BLOCK) - sizeof (EFI_HII_RGB_PIXEL) +
+                   BITMAP_LEN_24_BIT (Width , Height);
+    break;
+  default:
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Create the new image block according to input image.
+  //
+  ImageIn = (EFI_IMAGE_INPUT *) Image;
+  NewBlockSize = sizeof (EFI_HII_IIBT_IMAGE_24BIT_BLOCK) - sizeof (EFI_HII_RGB_PIXEL) +
+                 BITMAP_LEN_24_BIT (ImageIn->Width, ImageIn->Height);
+  NewBlock = (UINT8 *) AllocateZeroPool (NewBlockSize);
+  if (NewBlock == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NewBlockPtr = NewBlock;
+  if ((ImageIn->Flags & EFI_IMAGE_TRANSPARENT) == EFI_IMAGE_TRANSPARENT) {
+    *NewBlockPtr = EFI_HII_IIBT_IMAGE_24BIT_TRANS;
+  } else {
+    *NewBlockPtr = EFI_HII_IIBT_IMAGE_24BIT;
+  }
+  NewBlockPtr++;
+
+  CopyMem (NewBlockPtr, &ImageIn->Width, sizeof (UINT16));
+  NewBlockPtr += sizeof (UINT16);
+  CopyMem (NewBlockPtr, &ImageIn->Height, sizeof (UINT16));
+  NewBlockPtr += sizeof (UINT16);
+
+  CopyGopToRgbPixel ((EFI_HII_RGB_PIXEL *) NewBlockPtr, ImageIn->Bitmap, ImageIn->Width * ImageIn->Height);
+
+  //
+  // Adjust the image package to remove the original block firstly then add the new block.
+  //
+  BlockSize = ImagePackage->ImageBlockSize + NewBlockSize - OldBlockSize;
+  Block = (UINT8 *) AllocateZeroPool (BlockSize);
+  if (Block == NULL) {
+    FreePool (NewBlock);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlockPtr  = Block;
+  Part1Size = (UINT32) (ImageBlock - ImagePackage->ImageBlock);
+  Part2Size = ImagePackage->ImageBlockSize - Part1Size - OldBlockSize;
+  CopyMem (BlockPtr, ImagePackage->ImageBlock, Part1Size);
+  BlockPtr += Part1Size;
+  CopyMem (BlockPtr, NewBlock, NewBlockSize);
+  BlockPtr += NewBlockSize;
+  CopyMem (BlockPtr, ImageBlock + OldBlockSize, Part2Size);
+
+  FreePool (ImagePackage->ImageBlock);
+  FreePool (NewBlock);
+  ImagePackage->ImageBlock     = Block;
+  ImagePackage->ImageBlockSize = BlockSize;
+  ImagePackage->ImagePkgHdr.Header.Length += NewBlockSize - OldBlockSize;
+  PackageListNode->PackageListHdr.PackageLength += NewBlockSize - OldBlockSize;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  This function renders an image to a bitmap or the screen using the specified
+  color and options. It draws the image on an existing bitmap, allocates a new
+  bitmap or uses the screen. The images can be clipped.
+
+  @param  This                   A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  Flags                  Describes how the image is to be drawn.
+  @param  Image                  Points to the image to be displayed.
+  @param  Blt                    If this points to a non-NULL on entry, this points
+                                 to the image, which is Width pixels wide and
+                                 Height pixels high.  The image will be drawn onto
+                                 this image and  EFI_HII_DRAW_FLAG_CLIP is implied.
+                                 If this points to a  NULL on entry, then a buffer
+                                 will be allocated to hold  the generated image and
+                                 the pointer updated on exit. It is the caller's
+                                 responsibility to free this buffer.
+  @param  BltX                   Specifies the offset from the left and top edge of
+                                 the  output image of the first pixel in the image.
+  @param  BltY                   Specifies the offset from the left and top edge of
+                                 the  output image of the first pixel in the image.
+
+  @retval EFI_SUCCESS            The image was successfully drawn.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate an output buffer for Blt.
+  @retval EFI_INVALID_PARAMETER  The Image or Blt was NULL.
+  @retval EFI_INVALID_PARAMETER  Any combination of Flags is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiDrawImage (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_DRAW_FLAGS              Flags,
+  IN CONST EFI_IMAGE_INPUT           *Image,
+  IN OUT EFI_IMAGE_OUTPUT            **Blt,
+  IN UINTN                           BltX,
+  IN UINTN                           BltY
+  )
+{
+  EFI_STATUS                          Status;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  BOOLEAN                             Transparent;
+  EFI_IMAGE_INPUT                     *ImageIn;
+  EFI_IMAGE_OUTPUT                    *ImageOut;
+  EFI_GRAPHICS_OUTPUT_BLT_PIXEL       *BltBuffer;
+  UINTN                               BufferLen;
+  UINTN                               Width;
+  UINTN                               Height;
+  UINTN                               Xpos;
+  UINTN                               Ypos;
+  UINTN                               OffsetY1;
+  UINTN                               OffsetY2;
+  EFI_FONT_DISPLAY_INFO               *FontInfo;
+  UINTN                               Index;
+
+  if (This == NULL || Image == NULL || Blt == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Flags & EFI_HII_DRAW_FLAG_CLIP) == EFI_HII_DRAW_FLAG_CLIP && *Blt == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Flags & EFI_HII_DRAW_FLAG_TRANSPARENT) == EFI_HII_DRAW_FLAG_TRANSPARENT) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FontInfo = NULL;
+  ImageIn = (EFI_IMAGE_INPUT *) Image;
+
+  //
+  // Check whether the image will be drawn transparently or opaquely.
+  //
+  Transparent = FALSE;
+  if ((Flags & EFI_HII_DRAW_FLAG_TRANSPARENT) == EFI_HII_DRAW_FLAG_FORCE_TRANS) {
+    Transparent = TRUE;
+  } else if ((Flags & EFI_HII_DRAW_FLAG_TRANSPARENT) == EFI_HII_DRAW_FLAG_FORCE_OPAQUE){
+    Transparent = FALSE;
+  } else {
+    //
+    // Now EFI_HII_DRAW_FLAG_DEFAULT is set, whether image will be drawn depending
+    // on the image's transparency setting.
+    //
+    if ((ImageIn->Flags & EFI_IMAGE_TRANSPARENT) == EFI_IMAGE_TRANSPARENT) {
+      Transparent = TRUE;
+    }
+  }
+
+  //
+  // Image cannot be drawn transparently if Blt points to NULL on entry.
+  // Currently output to Screen transparently is not supported, either.
+  //
+  if (Transparent) {
+    if (*Blt == NULL) {
+      return EFI_INVALID_PARAMETER;
+    } else if ((Flags & EFI_HII_DIRECT_TO_SCREEN) == EFI_HII_DIRECT_TO_SCREEN) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  Private = HII_IMAGE_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  //
+  // When Blt points to a non-NULL on entry, this image will be drawn onto
+  // this bitmap or screen pointed by "*Blt" and EFI_HII_DRAW_FLAG_CLIP is implied.
+  // Otherwise a new bitmap will be allocated to hold this image.
+  //
+  if (*Blt != NULL) {
+    //
+    // Clip the image by (Width, Height)
+    //
+
+    Width  = ImageIn->Width;
+    Height = ImageIn->Height;
+
+    if (Width > (*Blt)->Width - BltX) {
+      Width = (*Blt)->Width - BltX;
+    }
+    if (Height > (*Blt)->Height - BltY) {
+      Height = (*Blt)->Height - BltY;
+    }
+
+    BufferLen = Width * Height * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL);
+    BltBuffer = (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) AllocateZeroPool (BufferLen);
+    if (BltBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    if (Width == ImageIn->Width && Height == ImageIn->Height) {
+      CopyMem (BltBuffer, ImageIn->Bitmap, BufferLen);
+    } else {
+      for (Ypos = 0; Ypos < Height; Ypos++) {
+        OffsetY1 = ImageIn->Width * Ypos;
+        OffsetY2 = Width * Ypos;
+        for (Xpos = 0; Xpos < Width; Xpos++) {
+          BltBuffer[OffsetY2 + Xpos] = ImageIn->Bitmap[OffsetY1 + Xpos];
+        }
+      }
+    }
+
+    //
+    // Draw the image to existing bitmap or screen depending on flag.
+    //
+    if ((Flags & EFI_HII_DIRECT_TO_SCREEN) == EFI_HII_DIRECT_TO_SCREEN) {
+      //
+      // Caller should make sure the current UGA console is grarphic mode.
+      //
+
+      //
+      // Write the image directly to the output device specified by Screen.
+      //
+      Status = (*Blt)->Image.Screen->Blt (
+                                       (*Blt)->Image.Screen,
+                                       BltBuffer,
+                                       EfiBltBufferToVideo,
+                                       0,
+                                       0,
+                                       BltX,
+                                       BltY,
+                                       Width,
+                                       Height,
+                                       0
+                                       );
+    } else {
+      //
+      // Draw the image onto the existing bitmap specified by Bitmap.
+      //
+      Status = ImageToBlt (
+                 BltBuffer,
+                 BltX,
+                 BltY,
+                 Width,
+                 Height,
+                 Transparent,
+                 Blt
+                 );
+
+    }
+
+    FreePool (BltBuffer);
+    return Status;
+
+  } else {
+    //
+    // Allocate a new bitmap to hold the incoming image.
+    //
+    Width  = ImageIn->Width  + BltX;
+    Height = ImageIn->Height + BltY;
+
+    BufferLen = Width * Height * sizeof (EFI_GRAPHICS_OUTPUT_BLT_PIXEL);
+    BltBuffer = (EFI_GRAPHICS_OUTPUT_BLT_PIXEL *) AllocateZeroPool (BufferLen);
+    if (BltBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ImageOut = (EFI_IMAGE_OUTPUT *) AllocateZeroPool (sizeof (EFI_IMAGE_OUTPUT));
+    if (ImageOut == NULL) {
+      FreePool (BltBuffer);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    ImageOut->Width        = (UINT16) Width;
+    ImageOut->Height       = (UINT16) Height;
+    ImageOut->Image.Bitmap = BltBuffer;
+
+    //
+    // BUGBUG: Now all the "blank" pixels are filled with system default background
+    // color. Not sure if it need to be updated or not.
+    //
+    Status = GetSystemFont (Private, &FontInfo, NULL);
+    if (EFI_ERROR (Status)) {
+      FreePool (BltBuffer);
+      FreePool (ImageOut);
+      return Status;
+    }
+    ASSERT (FontInfo != NULL);
+    for (Index = 0; Index < Width * Height; Index++) {
+      BltBuffer[Index] = FontInfo->BackgroundColor;
+    }
+    FreePool (FontInfo);
+
+    //
+    // Draw the incoming image to the new created image.
+    //
+    *Blt = ImageOut;
+    return ImageToBlt (
+             ImageIn->Bitmap,
+             BltX,
+             BltY,
+             ImageIn->Width,
+             ImageIn->Height,
+             Transparent,
+             Blt
+             );
+
+  }
+}
+
+
+/**
+  This function renders an image to a bitmap or the screen using the specified
+  color and options. It draws the image on an existing bitmap, allocates a new
+  bitmap or uses the screen. The images can be clipped.
+
+  @param  This                   A pointer to the EFI_HII_IMAGE_PROTOCOL instance.
+  @param  Flags                  Describes how the image is to be drawn.
+  @param  PackageList            The package list in the HII database to search for
+                                 the  specified image.
+  @param  ImageId                The image's id, which is unique within
+                                 PackageList.
+  @param  Blt                    If this points to a non-NULL on entry, this points
+                                 to the image, which is Width pixels wide and
+                                 Height pixels high. The image will be drawn onto
+                                 this image and
+                                 EFI_HII_DRAW_FLAG_CLIP is implied. If this points
+                                 to a  NULL on entry, then a buffer will be
+                                 allocated to hold  the generated image and the
+                                 pointer updated on exit. It is the caller's
+                                 responsibility to free this buffer.
+  @param  BltX                   Specifies the offset from the left and top edge of
+                                 the  output image of the first pixel in the image.
+  @param  BltY                   Specifies the offset from the left and top edge of
+                                 the  output image of the first pixel in the image.
+
+  @retval EFI_SUCCESS            The image was successfully drawn.
+  @retval EFI_OUT_OF_RESOURCES   Unable to allocate an output buffer for Blt.
+  @retval EFI_INVALID_PARAMETER  The Blt was NULL.
+  @retval EFI_NOT_FOUND          The image specified by ImageId is not in the database. 
+                           The specified PackageList is not in the database.                             
+
+**/
+EFI_STATUS
+EFIAPI
+HiiDrawImageId (
+  IN CONST EFI_HII_IMAGE_PROTOCOL    *This,
+  IN EFI_HII_DRAW_FLAGS              Flags,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN EFI_IMAGE_ID                    ImageId,
+  IN OUT EFI_IMAGE_OUTPUT            **Blt,
+  IN UINTN                           BltX,
+  IN UINTN                           BltY
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_IMAGE_INPUT                     Image;
+
+  //
+  // Check input parameter.
+  //
+  if (This == NULL || Blt == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Get the specified Image.
+  //
+  Status = HiiGetImage (This, PackageList, ImageId, &Image);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Draw this image.
+  //
+  Status = HiiDrawImage (This, Flags, &Image, Blt, BltX, BltY);
+  if (Image.Bitmap != NULL) {
+    FreePool (Image.Bitmap);
+  }
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/HiiDatabaseDxe/String.c b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/String.c
new file mode 100644
index 0000000000..2d04be4b62
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiDatabaseDxe/String.c
@@ -0,0 +1,2054 @@
+/** @file
+Implementation for EFI_HII_STRING_PROTOCOL.
+
+
+Copyright (c) 2007 - 2015, 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 "HiiDatabase.h"
+
+CHAR16 mLanguageWindow[16] = {
+  0x0000, 0x0080, 0x0100, 0x0300,
+  0x2000, 0x2080, 0x2100, 0x3000,
+  0x0080, 0x00C0, 0x0400, 0x0600,
+  0x0900, 0x3040, 0x30A0, 0xFF00
+};
+
+
+/**
+  This function checks whether a global font info is referred by local
+  font info list or not. (i.e. HII_FONT_INFO is generated.) If not, create
+  a HII_FONT_INFO to refer it locally.
+
+  This is a internal function.
+
+
+  @param  Private                Hii database private structure.
+  @param  StringPackage          HII string package instance.
+  @param  FontId                Font identifer, which must be unique within the string package.
+  @param  DuplicateEnable        If true, duplicate HII_FONT_INFO which refers to
+                                 the same EFI_FONT_INFO is permitted. Otherwise it
+                                 is not allowed.
+  @param  GlobalFontInfo         Input a global font info which specify a
+                                 EFI_FONT_INFO.
+  @param  LocalFontInfo          Output a local font info which refers to a
+                                 EFI_FONT_INFO.
+
+  @retval TRUE                   Already referred before calling this function.
+  @retval FALSE                  Not referred before calling this function.
+
+**/
+BOOLEAN
+ReferFontInfoLocally (
+  IN  HII_DATABASE_PRIVATE_DATA   *Private,
+  IN  HII_STRING_PACKAGE_INSTANCE *StringPackage,
+  IN  UINT8                       FontId,
+  IN  BOOLEAN                     DuplicateEnable,
+  IN  HII_GLOBAL_FONT_INFO        *GlobalFontInfo,
+  OUT HII_FONT_INFO               **LocalFontInfo
+  )
+{
+  HII_FONT_INFO                 *LocalFont;
+  LIST_ENTRY                    *Link;
+
+  ASSERT (Private != NULL && StringPackage != NULL && GlobalFontInfo != NULL && LocalFontInfo != NULL);
+
+  if (!DuplicateEnable) {
+    for (Link = StringPackage->FontInfoList.ForwardLink;
+         Link != &StringPackage->FontInfoList;
+         Link = Link->ForwardLink
+        ) {
+      LocalFont = CR (Link, HII_FONT_INFO, Entry, HII_FONT_INFO_SIGNATURE);
+      if (LocalFont->GlobalEntry == &GlobalFontInfo->Entry) {
+        //
+        // Already referred by local font info list, return directly.
+        //
+        *LocalFontInfo = LocalFont;
+        return TRUE;
+      }
+    }
+  }
+  // FontId identifies EFI_FONT_INFO in local string package uniquely.
+  // GlobalEntry points to a HII_GLOBAL_FONT_INFO which identifies
+  // EFI_FONT_INFO uniquely in whole hii database.
+  //
+  LocalFont = (HII_FONT_INFO *) AllocateZeroPool (sizeof (HII_FONT_INFO));
+  ASSERT (LocalFont != NULL);
+
+  LocalFont->Signature   = HII_FONT_INFO_SIGNATURE;
+  LocalFont->FontId      = FontId;
+  LocalFont->GlobalEntry = &GlobalFontInfo->Entry;
+  InsertTailList (&StringPackage->FontInfoList, &LocalFont->Entry);
+
+  *LocalFontInfo = LocalFont;
+  return FALSE;
+}
+
+
+/**
+  Convert Ascii string text to unicode string test.
+
+  This is a internal function.
+
+
+  @param  StringDest             Buffer to store the string text. If it is NULL,
+                                 only the size will be returned.
+  @param  StringSrc              Points to current null-terminated string.
+  @param  BufferSize             Length of the buffer.
+
+  @retval EFI_SUCCESS            The string text was outputed successfully.
+  @retval EFI_BUFFER_TOO_SMALL   Buffer is insufficient to store the found string
+                                 text. BufferSize is updated to the required buffer
+                                 size.
+
+**/
+EFI_STATUS
+ConvertToUnicodeText (
+  OUT EFI_STRING       StringDest,
+  IN  CHAR8            *StringSrc,
+  IN  OUT UINTN        *BufferSize
+  )
+{
+  UINTN  StringSize;
+  UINTN  Index;
+
+  ASSERT (StringSrc != NULL && BufferSize != NULL);
+
+  StringSize = AsciiStrSize (StringSrc) * 2;
+  if (*BufferSize < StringSize || StringDest == NULL) {
+    *BufferSize = StringSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  for (Index = 0; Index < AsciiStrLen (StringSrc); Index++) {
+    StringDest[Index] = (CHAR16) StringSrc[Index];
+  }
+
+  StringDest[Index] = 0;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Calculate the size of StringSrc and output it. If StringDest is not NULL,
+  copy string text from src to dest.
+
+  This is a internal function.
+
+  @param  StringDest             Buffer to store the string text. If it is NULL,
+                                 only the size will be returned.
+  @param  StringSrc              Points to current null-terminated string.
+  @param  BufferSize             Length of the buffer.
+
+  @retval EFI_SUCCESS            The string text was outputed successfully.
+  @retval EFI_BUFFER_TOO_SMALL   Buffer is insufficient to store the found string
+                                 text. BufferSize is updated to the required buffer
+                                 size.
+
+**/
+EFI_STATUS
+GetUnicodeStringTextOrSize (
+  OUT EFI_STRING       StringDest, OPTIONAL
+  IN  UINT8            *StringSrc,
+  IN  OUT UINTN        *BufferSize
+  )
+{
+  UINTN  StringSize;
+  UINT8  *StringPtr;
+
+  ASSERT (StringSrc != NULL && BufferSize != NULL);
+
+  StringSize = sizeof (CHAR16);
+  StringPtr  = StringSrc;
+  while (ReadUnaligned16 ((UINT16 *) StringPtr) != 0) {
+    StringSize += sizeof (CHAR16);
+    StringPtr += sizeof (CHAR16);
+  }
+
+  if (*BufferSize < StringSize) {
+    *BufferSize = StringSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  if (StringDest != NULL) {
+    CopyMem (StringDest, StringSrc, StringSize);
+  }
+
+  *BufferSize = StringSize;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Copy string font info to a buffer.
+
+  This is a internal function.
+
+  @param  StringPackage          Hii string package instance.
+  @param  FontId                 Font identifier which is unique in a string
+                                 package.
+  @param  StringFontInfo         Buffer to record the output font info. It's
+                                 caller's responsibility to free this buffer.
+
+  @retval EFI_SUCCESS            The string font is outputed successfully.
+  @retval EFI_NOT_FOUND          The specified font id does not exist.
+
+**/
+EFI_STATUS
+GetStringFontInfo (
+  IN  HII_STRING_PACKAGE_INSTANCE     *StringPackage,
+  IN  UINT8                           FontId,
+  OUT EFI_FONT_INFO                   **StringFontInfo
+  )
+{
+  LIST_ENTRY                           *Link;
+  HII_FONT_INFO                        *FontInfo;
+  HII_GLOBAL_FONT_INFO                 *GlobalFont;
+
+  ASSERT (StringFontInfo != NULL && StringPackage != NULL);
+
+  for (Link = StringPackage->FontInfoList.ForwardLink; Link != &StringPackage->FontInfoList; Link = Link->ForwardLink) {
+    FontInfo = CR (Link, HII_FONT_INFO, Entry, HII_FONT_INFO_SIGNATURE);
+    if (FontInfo->FontId == FontId) {
+      GlobalFont = CR (FontInfo->GlobalEntry, HII_GLOBAL_FONT_INFO, Entry, HII_GLOBAL_FONT_INFO_SIGNATURE);
+      *StringFontInfo = (EFI_FONT_INFO *) AllocateZeroPool (GlobalFont->FontInfoSize);
+      if (*StringFontInfo == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      CopyMem (*StringFontInfo, GlobalFont->FontInfo, GlobalFont->FontInfoSize);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Parse all string blocks to find a String block specified by StringId.
+  If StringId = (EFI_STRING_ID) (-1), find out all EFI_HII_SIBT_FONT blocks
+  within this string package and backup its information. If LastStringId is 
+  specified, the string id of last string block will also be output.
+  If StringId = 0, output the string id of last string block (EFI_HII_SIBT_STRING).
+
+  @param  Private                 Hii database private structure.
+  @param  StringPackage           Hii string package instance.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.
+  @param  BlockType               Output the block type of found string block.
+  @param  StringBlockAddr         Output the block address of found string block.
+  @param  StringTextOffset        Offset, relative to the found block address, of
+                                  the  string text information.
+  @param  LastStringId            Output the last string id when StringId = 0 or StringId = -1.
+  @param  StartStringId           The first id in the skip block which StringId in the block.
+
+  @retval EFI_SUCCESS             The string text and font is retrieved
+                                  successfully.
+  @retval EFI_NOT_FOUND           The specified text or font info can not be found
+                                  out.
+  @retval EFI_OUT_OF_RESOURCES    The system is out of resources to accomplish the
+                                  task.
+
+**/
+EFI_STATUS
+FindStringBlock (
+  IN HII_DATABASE_PRIVATE_DATA        *Private,
+  IN  HII_STRING_PACKAGE_INSTANCE     *StringPackage,
+  IN  EFI_STRING_ID                   StringId,
+  OUT UINT8                           *BlockType, OPTIONAL
+  OUT UINT8                           **StringBlockAddr, OPTIONAL
+  OUT UINTN                           *StringTextOffset, OPTIONAL
+  OUT EFI_STRING_ID                   *LastStringId, OPTIONAL
+  OUT EFI_STRING_ID                   *StartStringId OPTIONAL
+  )
+{
+  UINT8                                *BlockHdr;
+  EFI_STRING_ID                        CurrentStringId;
+  UINTN                                BlockSize;
+  UINTN                                Index;
+  UINT8                                *StringTextPtr;
+  UINTN                                Offset;
+  HII_FONT_INFO                        *LocalFont;
+  EFI_FONT_INFO                        *FontInfo;
+  HII_GLOBAL_FONT_INFO                 *GlobalFont;
+  UINTN                                FontInfoSize;
+  UINT16                               StringCount;
+  UINT16                               SkipCount;
+  EFI_HII_FONT_STYLE                   FontStyle;
+  UINT16                               FontSize;
+  UINT8                                Length8;
+  EFI_HII_SIBT_EXT2_BLOCK              Ext2;
+  UINT8                                FontId;
+  UINT32                               Length32;
+  UINTN                                StringSize;
+  CHAR16                               Zero;
+
+  ASSERT (StringPackage != NULL);
+  ASSERT (StringPackage->Signature == HII_STRING_PACKAGE_SIGNATURE);
+
+  CurrentStringId = 1;
+
+  if (StringId != (EFI_STRING_ID) (-1) && StringId != 0) {
+    ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+    if (StringId > StringPackage->MaxStringId) {
+      return EFI_NOT_FOUND;
+    }
+  } else {
+    ASSERT (Private != NULL && Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+    if (StringId == 0 && LastStringId != NULL) {
+      *LastStringId = StringPackage->MaxStringId;
+      return EFI_SUCCESS;
+    }
+  }
+
+  ZeroMem (&Zero, sizeof (CHAR16));
+
+  //
+  // Parse the string blocks to get the string text and font.
+  //
+  BlockHdr  = StringPackage->StringBlock;
+  BlockSize = 0;
+  Offset    = 0;
+  while (*BlockHdr != EFI_HII_SIBT_END) {
+    switch (*BlockHdr) {
+    case EFI_HII_SIBT_STRING_SCSU:
+      Offset = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_SCSU_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_SCSU_FONT_BLOCK) - sizeof (UINT8);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset + AsciiStrSize ((CHAR8 *) StringTextPtr);
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU:
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        if (CurrentStringId == StringId) {
+          ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+          *BlockType        = *BlockHdr;
+          *StringBlockAddr  = BlockHdr;
+          *StringTextOffset = StringTextPtr - BlockHdr;
+          return EFI_SUCCESS;
+        }
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      StringTextPtr = (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_SIBT_STRINGS_SCSU_FONT_BLOCK) - sizeof (UINT8));
+      BlockSize += StringTextPtr - BlockHdr;
+
+      for (Index = 0; Index < StringCount; Index++) {
+        BlockSize += AsciiStrSize ((CHAR8 *) StringTextPtr);
+        if (CurrentStringId == StringId) {
+          ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+          *BlockType        = *BlockHdr;
+          *StringBlockAddr  = BlockHdr;
+          *StringTextOffset = StringTextPtr - BlockHdr;
+          return EFI_SUCCESS;
+        }
+        StringTextPtr = StringTextPtr + AsciiStrSize ((CHAR8 *) StringTextPtr);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2:
+      Offset        = sizeof (EFI_HII_STRING_BLOCK);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StringSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      GetUnicodeStringTextOrSize (NULL, StringTextPtr, &StringSize);
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRING_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK)  - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      //
+      // Use StrSize to store the size of the specified string, including the NULL
+      // terminator.
+      //
+      GetUnicodeStringTextOrSize (NULL, StringTextPtr, &StringSize);
+      BlockSize += Offset + StringSize;
+      CurrentStringId++;
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (&StringCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      for (Index = 0; Index < StringCount; Index++) {
+        GetUnicodeStringTextOrSize (NULL, StringTextPtr, &StringSize);
+        BlockSize += StringSize;
+        if (CurrentStringId == StringId) {
+          ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+          *BlockType        = *BlockHdr;
+          *StringBlockAddr  = BlockHdr;
+          *StringTextOffset = StringTextPtr - BlockHdr;
+          return EFI_SUCCESS;
+        }
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+      Offset = sizeof (EFI_HII_SIBT_STRINGS_UCS2_FONT_BLOCK) - sizeof (CHAR16);
+      StringTextPtr = BlockHdr + Offset;
+      BlockSize += Offset;
+      CopyMem (
+        &StringCount,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT16)
+        );
+      for (Index = 0; Index < StringCount; Index++) {
+        GetUnicodeStringTextOrSize (NULL, StringTextPtr, &StringSize);
+        BlockSize += StringSize;
+        if (CurrentStringId == StringId) {
+          ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+          *BlockType        = *BlockHdr;
+          *StringBlockAddr  = BlockHdr;
+          *StringTextOffset = StringTextPtr - BlockHdr;
+          return EFI_SUCCESS;
+        }
+        StringTextPtr = StringTextPtr + StringSize;
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_DUPLICATE:
+      if (CurrentStringId == StringId) {
+        //
+        // Incoming StringId is an id of a duplicate string block.
+        // Update the StringId to be the previous string block.
+        // Go back to the header of string block to search.
+        //
+        CopyMem (
+          &StringId,
+          BlockHdr + sizeof (EFI_HII_STRING_BLOCK),
+          sizeof (EFI_STRING_ID)
+          );
+        ASSERT (StringId != CurrentStringId);
+        CurrentStringId = 1;
+        BlockSize       = 0;
+      } else {
+        BlockSize       += sizeof (EFI_HII_SIBT_DUPLICATE_BLOCK);
+        CurrentStringId++;
+      }
+      break;
+
+    case EFI_HII_SIBT_SKIP1:
+      SkipCount = (UINT16) (*(UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK)));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP1_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_SKIP2:
+      CopyMem (&SkipCount, BlockHdr + sizeof (EFI_HII_STRING_BLOCK), sizeof (UINT16));
+      CurrentStringId = (UINT16) (CurrentStringId + SkipCount);
+      BlockSize       +=  sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+      break;
+
+    case EFI_HII_SIBT_EXT1:
+      CopyMem (
+        &Length8,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT8)
+        );
+      BlockSize += Length8;
+      break;
+
+    case EFI_HII_SIBT_EXT2:
+      CopyMem (&Ext2, BlockHdr, sizeof (EFI_HII_SIBT_EXT2_BLOCK));
+      if (Ext2.BlockType2 == EFI_HII_SIBT_FONT && StringId == (EFI_STRING_ID) (-1)) {
+        //
+        // Find the relationship between global font info and the font info of
+        // this EFI_HII_SIBT_FONT block then backup its information in local package.
+        //
+        BlockHdr += sizeof (EFI_HII_SIBT_EXT2_BLOCK);
+        CopyMem (&FontId, BlockHdr, sizeof (UINT8));
+        BlockHdr ++;
+        CopyMem (&FontSize, BlockHdr, sizeof (UINT16));
+        BlockHdr += sizeof (UINT16);
+        CopyMem (&FontStyle, BlockHdr, sizeof (EFI_HII_FONT_STYLE));
+        BlockHdr += sizeof (EFI_HII_FONT_STYLE);
+        GetUnicodeStringTextOrSize (NULL, BlockHdr, &StringSize);
+
+        FontInfoSize = sizeof (EFI_FONT_INFO) - sizeof (CHAR16) + StringSize;
+        FontInfo = (EFI_FONT_INFO *) AllocateZeroPool (FontInfoSize);
+        if (FontInfo == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+        FontInfo->FontStyle = FontStyle;
+        FontInfo->FontSize  = FontSize;
+        CopyMem (FontInfo->FontName, BlockHdr, StringSize);
+
+        //
+        // If find the corresponding global font info, save the relationship.
+        // Otherwise ignore this EFI_HII_SIBT_FONT block.
+        //
+        if (IsFontInfoExisted (Private, FontInfo, NULL, NULL, &GlobalFont)) {
+          ReferFontInfoLocally (Private, StringPackage, FontId, TRUE, GlobalFont, &LocalFont);
+        }
+
+        //
+        // Since string package tool set FontId initially to 0 and increases it
+        // progressively by one, StringPackage->FondId always represents an unique
+        // and available FontId.
+        //        
+        StringPackage->FontId++;
+
+        FreePool (FontInfo);
+      }
+
+      BlockSize += Ext2.Length;
+
+      break;
+
+    case EFI_HII_SIBT_EXT4:
+      CopyMem (
+        &Length32,
+        (UINT8*)((UINTN)BlockHdr + sizeof (EFI_HII_STRING_BLOCK) + sizeof (UINT8)),
+        sizeof (UINT32)
+        );
+
+      BlockSize += Length32;
+      break;
+
+    default:
+      break;
+    }
+
+    if (StringId > 0 && StringId != (EFI_STRING_ID)(-1)) {
+      ASSERT (BlockType != NULL && StringBlockAddr != NULL && StringTextOffset != NULL);
+      *BlockType        = *BlockHdr;
+      *StringBlockAddr  = BlockHdr;
+      *StringTextOffset = Offset;
+
+      if (StringId == CurrentStringId - 1) {
+        //
+        // if only one skip item, return EFI_NOT_FOUND.
+        //
+        if(*BlockType == EFI_HII_SIBT_SKIP2 || *BlockType == EFI_HII_SIBT_SKIP1) {
+          return EFI_NOT_FOUND;
+        } else {
+          return EFI_SUCCESS;
+        }
+      }
+
+      if (StringId < CurrentStringId - 1) {
+        return EFI_NOT_FOUND;
+      }
+    }
+    BlockHdr  = StringPackage->StringBlock + BlockSize;
+    if (StartStringId != NULL) {
+        *StartStringId  = CurrentStringId;
+    }
+  }
+  
+  //
+  // Get last string ID
+  //
+  if (StringId == (EFI_STRING_ID) (-1) && LastStringId != NULL) {
+    *LastStringId = (EFI_STRING_ID) (CurrentStringId - 1);
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Parse all string blocks to get a string specified by StringId.
+
+  This is a internal function.
+
+  @param  Private                Hii database private structure.
+  @param  StringPackage          Hii string package instance.
+  @param  StringId               The string's id, which is unique within
+                                 PackageList.
+  @param  String                 Points to retrieved null-terminated string.
+  @param  StringSize             On entry, points to the size of the buffer pointed
+                                 to by String, in bytes. On return, points to the
+                                 length of the string, in bytes.
+  @param  StringFontInfo         If not NULL, allocate a buffer to record the
+                                 output font info. It's caller's responsibility to
+                                 free this buffer.
+
+  @retval EFI_SUCCESS            The string text and font is retrieved
+                                 successfully.
+  @retval EFI_NOT_FOUND          The specified text or font info can not be found
+                                 out.
+  @retval EFI_BUFFER_TOO_SMALL   The buffer specified by StringSize is too small to
+                                 hold the string.
+
+**/
+EFI_STATUS
+GetStringWorker (
+  IN HII_DATABASE_PRIVATE_DATA        *Private,
+  IN  HII_STRING_PACKAGE_INSTANCE     *StringPackage,
+  IN  EFI_STRING_ID                   StringId,
+  OUT EFI_STRING                      String,
+  IN  OUT UINTN                       *StringSize, OPTIONAL
+  OUT EFI_FONT_INFO                   **StringFontInfo OPTIONAL
+  )
+{
+  UINT8                                *StringTextPtr;
+  UINT8                                BlockType;
+  UINT8                                *StringBlockAddr;
+  UINTN                                StringTextOffset;
+  EFI_STATUS                           Status;
+  UINT8                                FontId;
+
+  ASSERT (StringPackage != NULL);
+  ASSERT (Private != NULL && Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+
+  //
+  // Find the specified string block
+  //
+  Status = FindStringBlock (
+             Private,
+             StringPackage,
+             StringId,
+             &BlockType,
+             &StringBlockAddr,
+             &StringTextOffset,
+             NULL,
+             NULL
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (StringSize == NULL) {
+    //
+    // String text buffer is not requested
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get the string text.
+  //
+  StringTextPtr = StringBlockAddr + StringTextOffset;
+  switch (BlockType) {
+  case EFI_HII_SIBT_STRING_SCSU:
+  case EFI_HII_SIBT_STRING_SCSU_FONT:
+  case EFI_HII_SIBT_STRINGS_SCSU:
+  case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+    Status = ConvertToUnicodeText (String, (CHAR8 *) StringTextPtr, StringSize);
+    break;
+  case EFI_HII_SIBT_STRING_UCS2:
+  case EFI_HII_SIBT_STRING_UCS2_FONT:
+  case EFI_HII_SIBT_STRINGS_UCS2:
+  case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+    Status = GetUnicodeStringTextOrSize (String, StringTextPtr, StringSize);
+    break;
+  default:
+    return EFI_NOT_FOUND;
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the string font. The FontId 0 is the default font for those string blocks which 
+  // do not specify a font identifier. If default font is not specified, return NULL.
+  //
+  if (StringFontInfo != NULL) {
+    switch (BlockType) {
+    case EFI_HII_SIBT_STRING_SCSU_FONT:
+    case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+    case EFI_HII_SIBT_STRING_UCS2_FONT:
+    case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+      FontId = *(StringBlockAddr + sizeof (EFI_HII_STRING_BLOCK));
+      break;
+    default:
+      FontId = 0;
+    }
+    Status = GetStringFontInfo (StringPackage, FontId, StringFontInfo);
+    if (Status == EFI_NOT_FOUND) {
+        *StringFontInfo = NULL;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  If GetStringBlock find the StringId's string is not saved in the exist string block,
+  this function will create the UCS2 string block to save the string; also split the 
+  skip block into two or one skip block.
+
+  This is a internal function.
+  
+  @param  StringPackage           Hii string package instance.
+  @param  StartStringId           The first id in the skip block which StringId in the block.
+  @param  StringId                The string's id, which is unique within
+                                  PackageList.  
+  @param  BlockType               Output the block type of found string block.  
+  @param  StringBlockAddr         Output the block address of found string block.  
+  @param  FontBlock               whether this string block has font info.
+
+  @retval EFI_SUCCESS            The string font is outputed successfully.
+  @retval EFI_OUT_OF_RESOURCES   NO resource for the memory to save the new string block.
+
+**/
+EFI_STATUS
+InsertLackStringBlock (
+  IN OUT HII_STRING_PACKAGE_INSTANCE         *StringPackage,
+  IN EFI_STRING_ID                           StartStringId,
+  IN EFI_STRING_ID                           StringId,
+  IN OUT UINT8                               *BlockType,
+  IN OUT UINT8                               **StringBlockAddr,
+  IN BOOLEAN                                 FontBlock
+  )
+{
+  UINT8                                *BlockPtr;
+  UINT8                                *StringBlock;
+  UINT32                               SkipLen;    
+  UINT32                               OldBlockSize;
+  UINT32                               NewBlockSize;
+  UINT32                               FrontSkipNum;
+  UINT32                               NewUCSBlockLen;
+  UINT8                                *OldStringAddr;
+  UINT32                               IdCount;
+
+  FrontSkipNum  = 0;
+  SkipLen       = 0;
+  OldStringAddr = *StringBlockAddr;
+  
+  ASSERT (*BlockType == EFI_HII_SIBT_SKIP1 || *BlockType == EFI_HII_SIBT_SKIP2);
+  //
+  // Old skip block size.
+  //
+  if (*BlockType == EFI_HII_SIBT_SKIP1) {
+    SkipLen = sizeof (EFI_HII_SIBT_SKIP1_BLOCK);
+    IdCount = *(UINT8*)(OldStringAddr + sizeof (EFI_HII_STRING_BLOCK));
+  } else {
+    SkipLen = sizeof (EFI_HII_SIBT_SKIP2_BLOCK);
+    IdCount = *(UINT16*)(OldStringAddr + sizeof (EFI_HII_STRING_BLOCK));
+  } 
+
+  //
+  // New create UCS or UCS2 block size.
+  //
+  if (FontBlock) {
+    NewUCSBlockLen = sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK);
+  } else {
+    NewUCSBlockLen = sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK);
+  }
+
+  OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+
+  if (StartStringId == StringId) {
+    //
+    // New block + [Skip block]
+    //
+    if (IdCount > 1) {
+      NewBlockSize = OldBlockSize + NewUCSBlockLen;
+    } else {
+      NewBlockSize = OldBlockSize + NewUCSBlockLen - SkipLen;
+    }
+  } else if (StartStringId + IdCount - 1 == StringId){
+    //
+    // Skip block + New block
+    //
+    NewBlockSize = OldBlockSize + NewUCSBlockLen;
+    FrontSkipNum = StringId - StartStringId;
+  } else {
+    //
+    // Skip block + New block + [Skip block]
+    //
+    NewBlockSize = OldBlockSize + NewUCSBlockLen + SkipLen;
+    FrontSkipNum = StringId - StartStringId;
+  }
+
+  StringBlock = (UINT8 *) AllocateZeroPool (NewBlockSize);
+  if (StringBlock == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Copy old block in front of skip block.
+  //
+  CopyMem (StringBlock, StringPackage->StringBlock, OldStringAddr - StringPackage->StringBlock);  
+  BlockPtr = StringBlock + (OldStringAddr - StringPackage->StringBlock);
+
+  if (FrontSkipNum > 0) {
+    *BlockPtr = *BlockType;
+    if (*BlockType == EFI_HII_SIBT_SKIP1) {
+      *(BlockPtr + sizeof (EFI_HII_STRING_BLOCK)) = (UINT8) FrontSkipNum;
+    } else {
+      *(UINT16 *)(BlockPtr + sizeof (EFI_HII_STRING_BLOCK)) = (UINT16) FrontSkipNum;
+    }
+    BlockPtr += SkipLen;
+  }
+
+  //
+  // Create a EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK
+  //
+  *StringBlockAddr = BlockPtr;
+  if (FontBlock) {
+    *BlockPtr = EFI_HII_SIBT_STRING_UCS2_FONT;
+  } else {
+    *BlockPtr = EFI_HII_SIBT_STRING_UCS2;
+  }
+  BlockPtr += NewUCSBlockLen;
+
+  if (IdCount > FrontSkipNum + 1) {
+    *BlockPtr = *BlockType;
+    if (*BlockType == EFI_HII_SIBT_SKIP1) {
+      *(BlockPtr + sizeof (EFI_HII_STRING_BLOCK)) = (UINT8) (IdCount - FrontSkipNum - 1);
+    } else {
+      *(UINT16 *)(BlockPtr + sizeof (EFI_HII_STRING_BLOCK)) = (UINT16) (IdCount - FrontSkipNum - 1);
+    }
+    BlockPtr += SkipLen;
+  }
+
+  //
+  // Append a EFI_HII_SIBT_END block to the end.
+  //
+  CopyMem (BlockPtr, OldStringAddr + SkipLen, OldBlockSize - (OldStringAddr - StringPackage->StringBlock) - SkipLen);  
+
+  if (FontBlock) {
+    *BlockType = EFI_HII_SIBT_STRING_UCS2_FONT;
+  } else {
+    *BlockType = EFI_HII_SIBT_STRING_UCS2;
+  }
+  FreePool (StringPackage->StringBlock);
+  StringPackage->StringBlock = StringBlock;
+  StringPackage->StringPkgHdr->Header.Length += NewBlockSize - OldBlockSize;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Parse all string blocks to set a String specified by StringId.
+
+  This is a internal function.
+
+  @param  Private                HII database driver private structure.
+  @param  StringPackage          HII string package instance.
+  @param  StringId               The string's id, which is unique within
+                                 PackageList.
+  @param  String                 Points to the new null-terminated string.
+  @param  StringFontInfo         Points to the input font info.
+
+  @retval EFI_SUCCESS            The string was updated successfully.
+  @retval EFI_NOT_FOUND          The string specified by StringId is not in the
+                                 database.
+  @retval EFI_INVALID_PARAMETER  The String or Language was NULL.
+  @retval EFI_INVALID_PARAMETER  The specified StringFontInfo does not exist in
+                                 current database.
+  @retval EFI_OUT_OF_RESOURCES   The system is out of resources to accomplish the
+                                 task.
+
+**/
+EFI_STATUS
+SetStringWorker (
+  IN  HII_DATABASE_PRIVATE_DATA       *Private,
+  IN OUT HII_STRING_PACKAGE_INSTANCE  *StringPackage,
+  IN  EFI_STRING_ID                   StringId,
+  IN  EFI_STRING                      String,
+  IN  EFI_FONT_INFO                   *StringFontInfo OPTIONAL
+  )
+{
+  UINT8                                *StringTextPtr;
+  UINT8                                BlockType;
+  UINT8                                *StringBlockAddr;
+  UINTN                                StringTextOffset;
+  EFI_STATUS                           Status;
+  UINT8                                *Block;
+  UINT8                                *BlockPtr;
+  UINTN                                BlockSize;
+  UINTN                                OldBlockSize;
+  HII_FONT_INFO                        *LocalFont;
+  HII_GLOBAL_FONT_INFO                 *GlobalFont;
+  BOOLEAN                              Referred;
+  EFI_HII_SIBT_EXT2_BLOCK              Ext2;
+  UINTN                                StringSize;
+  UINTN                                TmpSize;
+  EFI_STRING_ID                        StartStringId;
+
+  StartStringId = 0;
+  StringSize    = 0;
+  ASSERT (Private != NULL && StringPackage != NULL && String != NULL);
+  ASSERT (Private->Signature == HII_DATABASE_PRIVATE_DATA_SIGNATURE);
+  //
+  // Find the specified string block
+  //
+  Status = FindStringBlock (
+             Private,
+             StringPackage,
+             StringId,
+             &BlockType,
+             &StringBlockAddr,
+             &StringTextOffset,
+             NULL,
+             &StartStringId
+             );
+  if (EFI_ERROR (Status) && (BlockType == EFI_HII_SIBT_SKIP1 || BlockType == EFI_HII_SIBT_SKIP2)) {
+    Status = InsertLackStringBlock(StringPackage, 
+                          StartStringId, 
+                          StringId, 
+                          &BlockType,
+                          &StringBlockAddr,
+                          (BOOLEAN)(StringFontInfo != NULL)
+                          );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    if (StringFontInfo != NULL) {
+      StringTextOffset = sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK) - sizeof (CHAR16);
+    } else {
+      StringTextOffset = sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK) - sizeof (CHAR16);
+    }
+  }
+
+  LocalFont  = NULL;
+  GlobalFont = NULL;
+  Referred   = FALSE;
+
+  //
+  // The input StringFontInfo should exist in current database if specified.
+  //
+  if (StringFontInfo != NULL) {
+    if (!IsFontInfoExisted (Private, StringFontInfo, NULL, NULL, &GlobalFont)) {
+      return EFI_INVALID_PARAMETER;
+    } else {
+      Referred = ReferFontInfoLocally (
+                   Private, 
+                   StringPackage, 
+                   StringPackage->FontId, 
+                   FALSE, 
+                   GlobalFont, 
+                   &LocalFont
+                   );
+      if (!Referred) {
+        StringPackage->FontId++;
+      }
+    }
+    //
+    // Update the FontId of the specified string block to input font info.
+    //
+    switch (BlockType) {
+    case EFI_HII_SIBT_STRING_SCSU_FONT:  
+    case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+    case EFI_HII_SIBT_STRING_UCS2_FONT:
+    case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+      *(StringBlockAddr + sizeof (EFI_HII_STRING_BLOCK)) = LocalFont->FontId;
+      break;
+    default:
+      //
+      // When modify the font info of these blocks, the block type should be updated
+      // to contain font info thus the whole structure should be revised.
+      // It is recommended to use tool to modify the block type not in the code.
+      //      
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+
+  //
+  // Set the string text and font.
+  //
+  StringTextPtr = StringBlockAddr + StringTextOffset;
+  switch (BlockType) {
+  case EFI_HII_SIBT_STRING_SCSU:
+  case EFI_HII_SIBT_STRING_SCSU_FONT:
+  case EFI_HII_SIBT_STRINGS_SCSU:
+  case EFI_HII_SIBT_STRINGS_SCSU_FONT:
+    BlockSize = OldBlockSize + StrLen (String);
+    BlockSize -= AsciiStrSize ((CHAR8 *) StringTextPtr);
+    Block = AllocateZeroPool (BlockSize);
+    if (Block == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    CopyMem (Block, StringPackage->StringBlock, StringTextPtr - StringPackage->StringBlock);
+    BlockPtr = Block + (StringTextPtr - StringPackage->StringBlock);
+
+    while (*String != 0) {
+      *BlockPtr++ = (CHAR8) *String++;
+    }
+    *BlockPtr++ = 0;
+
+    
+    TmpSize = OldBlockSize - (StringTextPtr - StringPackage->StringBlock) - AsciiStrSize ((CHAR8 *) StringTextPtr);
+    CopyMem (
+      BlockPtr,
+      StringTextPtr + AsciiStrSize ((CHAR8 *)StringTextPtr),
+      TmpSize
+      );
+
+    FreePool (StringPackage->StringBlock);
+    StringPackage->StringBlock = Block;
+    StringPackage->StringPkgHdr->Header.Length += (UINT32) (BlockSize - OldBlockSize);
+    break;
+
+  case EFI_HII_SIBT_STRING_UCS2:
+  case EFI_HII_SIBT_STRING_UCS2_FONT:
+  case EFI_HII_SIBT_STRINGS_UCS2:
+  case EFI_HII_SIBT_STRINGS_UCS2_FONT:
+    //
+    // Use StrSize to store the size of the specified string, including the NULL
+    // terminator.
+    //
+    GetUnicodeStringTextOrSize (NULL, StringTextPtr, &StringSize);
+
+    BlockSize = OldBlockSize + StrSize (String) - StringSize;
+    Block = AllocateZeroPool (BlockSize);
+    if (Block == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    CopyMem (Block, StringPackage->StringBlock, StringTextPtr - StringPackage->StringBlock);
+    BlockPtr = Block + (StringTextPtr - StringPackage->StringBlock);
+
+    CopyMem (BlockPtr, String, StrSize (String));
+    BlockPtr += StrSize (String);
+
+    CopyMem (
+      BlockPtr,
+      StringTextPtr + StringSize,
+      OldBlockSize - (StringTextPtr - StringPackage->StringBlock) - StringSize
+      );
+
+    FreePool (StringPackage->StringBlock);
+    StringPackage->StringBlock = Block;
+    StringPackage->StringPkgHdr->Header.Length += (UINT32) (BlockSize - OldBlockSize);
+    break;
+
+  default:
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Insert a new EFI_HII_SIBT_FONT_BLOCK to the header of string block, if incoming
+  // StringFontInfo does not exist in current string package.
+  //
+  // This new block does not impact on the value of StringId.
+  //
+  //
+  if (StringFontInfo == NULL || Referred) {
+    return EFI_SUCCESS;
+  }
+
+  OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+  BlockSize = OldBlockSize + sizeof (EFI_HII_SIBT_FONT_BLOCK) - sizeof (CHAR16) +
+              StrSize (GlobalFont->FontInfo->FontName);
+
+  Block = AllocateZeroPool (BlockSize);
+  if (Block == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  BlockPtr = Block;
+  Ext2.Header.BlockType = EFI_HII_SIBT_EXT2;
+  Ext2.BlockType2       = EFI_HII_SIBT_FONT;
+  Ext2.Length           = (UINT16) (BlockSize - OldBlockSize);
+  CopyMem (BlockPtr, &Ext2, sizeof (EFI_HII_SIBT_EXT2_BLOCK));
+  BlockPtr += sizeof (EFI_HII_SIBT_EXT2_BLOCK);
+
+  *BlockPtr = LocalFont->FontId;
+  BlockPtr ++;
+  CopyMem (BlockPtr, &GlobalFont->FontInfo->FontSize, sizeof (UINT16));
+  BlockPtr += sizeof (UINT16);
+  CopyMem (BlockPtr, &GlobalFont->FontInfo->FontStyle, sizeof (UINT32));
+  BlockPtr += sizeof (UINT32);
+  CopyMem (
+    BlockPtr,
+    GlobalFont->FontInfo->FontName,
+    StrSize (GlobalFont->FontInfo->FontName)
+    );
+  BlockPtr += StrSize (GlobalFont->FontInfo->FontName);
+
+  CopyMem (BlockPtr, StringPackage->StringBlock, OldBlockSize);
+
+  FreePool (StringPackage->StringBlock);
+  StringPackage->StringBlock = Block;
+  StringPackage->StringPkgHdr->Header.Length += Ext2.Length;
+
+  return EFI_SUCCESS;
+
+}
+
+
+/**
+  This function adds the string String to the group of strings owned by PackageList, with the
+  specified font information StringFontInfo and returns a new string id. 
+  The new string identifier is guaranteed to be unique within the package list. 
+  That new string identifier is reserved for all languages in the package list. 
+
+
+  @param  This                   A pointer to the EFI_HII_STRING_PROTOCOL instance.
+  @param  PackageList            Handle of the package list where this string will
+                                 be added.
+  @param  StringId               On return, contains the new strings id, which is
+                                 unique within PackageList.
+  @param  Language               Points to the language for the new string.
+  @param  LanguageName           Points to the printable language name to associate
+                                 with the passed in  Language field.If LanguageName
+                                 is not NULL and the string package header's
+                                 LanguageName  associated with a given Language is
+                                 not zero, the LanguageName being passed  in will
+                                 be ignored.
+  @param  String                 Points to the new null-terminated string.
+  @param  StringFontInfo         Points to the new string's font information or
+                                 NULL if the string should have the default system
+                                 font, size and style.
+
+  @retval EFI_SUCCESS            The new string was added successfully.
+  @retval EFI_NOT_FOUND          The specified PackageList could not be found in
+                                 database.
+  @retval EFI_OUT_OF_RESOURCES   Could not add the string due to lack of resources.
+  @retval EFI_INVALID_PARAMETER  String is NULL or StringId is NULL or Language is
+                                 NULL.
+  @retval EFI_INVALID_PARAMETER  The specified StringFontInfo does not exist in
+                                 current database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiNewString (
+  IN  CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN  EFI_HII_HANDLE                  PackageList,
+  OUT EFI_STRING_ID                   *StringId,
+  IN  CONST CHAR8                     *Language,
+  IN  CONST CHAR16                    *LanguageName, OPTIONAL
+  IN  CONST EFI_STRING                String,
+  IN  CONST EFI_FONT_INFO             *StringFontInfo OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  UINT32                              HeaderSize;
+  UINT32                              BlockSize;
+  UINT32                              OldBlockSize;
+  UINT8                               *StringBlock;
+  UINT8                               *BlockPtr;
+  UINT32                              Ucs2BlockSize;
+  UINT32                              FontBlockSize;
+  UINT32                              Ucs2FontBlockSize;
+  EFI_HII_SIBT_EXT2_BLOCK             Ext2;
+  HII_FONT_INFO                       *LocalFont;
+  HII_GLOBAL_FONT_INFO                *GlobalFont;
+  EFI_STRING_ID                       NewStringId;
+  EFI_STRING_ID                       NextStringId;
+  EFI_STRING_ID                       Index;
+  HII_STRING_PACKAGE_INSTANCE         *MatchStringPackage;
+  BOOLEAN                             NewStringPackageCreated;
+
+
+  if (This == NULL || String == NULL || StringId == NULL || Language == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private    = HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  GlobalFont = NULL;
+
+  //
+  // If StringFontInfo specify a paritcular font, it should exist in current database.
+  //
+  if (StringFontInfo != NULL) {
+    if (!IsFontInfoExisted (Private, (EFI_FONT_INFO *) StringFontInfo, NULL, NULL, &GlobalFont)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Get the matching package list.
+  //
+  PackageListNode = NULL;
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+  if (PackageListNode == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Status = EFI_SUCCESS;
+  NewStringPackageCreated = FALSE;
+  NewStringId   = 0;
+  NextStringId  = 0;
+  StringPackage = NULL;
+  MatchStringPackage = NULL;
+  for (Link = PackageListNode->StringPkgHdr.ForwardLink;
+       Link != &PackageListNode->StringPkgHdr;
+       Link = Link->ForwardLink
+      ) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    //
+    // Create a string block and corresponding font block if exists, then append them
+    // to the end of the string package.
+    //
+    Status = FindStringBlock (
+               Private,
+               StringPackage,
+               0,
+               NULL,
+               NULL,
+               NULL,
+               &NextStringId,
+               NULL
+               );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    //
+    // Make sure that new StringId is same in all String Packages for the different language.
+    //
+    if (NewStringId != 0 && NewStringId != NextStringId) {
+      ASSERT (FALSE);
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    NewStringId = NextStringId;
+    //
+    // Get the matched string package with language.
+    //
+    if (HiiCompareLanguage (StringPackage->StringPkgHdr->Language, (CHAR8 *) Language)) {
+      MatchStringPackage = StringPackage;
+    } else {
+      OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+      //
+      // Create a blank EFI_HII_SIBT_STRING_UCS2_BLOCK to reserve new string ID.
+      //
+      Ucs2BlockSize = (UINT32) sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK);
+
+      StringBlock = (UINT8 *) AllocateZeroPool (OldBlockSize + Ucs2BlockSize);
+      if (StringBlock == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      //
+      // Copy original string blocks, except the EFI_HII_SIBT_END.
+      //
+      CopyMem (StringBlock, StringPackage->StringBlock, OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK));
+      //
+      // Create a blank EFI_HII_SIBT_STRING_UCS2 block
+      //
+      BlockPtr  = StringBlock + OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK);
+      *BlockPtr = EFI_HII_SIBT_STRING_UCS2;
+      BlockPtr  += sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK);
+
+      //
+      // Append a EFI_HII_SIBT_END block to the end.
+      //
+      *BlockPtr = EFI_HII_SIBT_END;
+      FreePool (StringPackage->StringBlock);
+      StringPackage->StringBlock = StringBlock;
+      StringPackage->StringPkgHdr->Header.Length += Ucs2BlockSize;
+      PackageListNode->PackageListHdr.PackageLength += Ucs2BlockSize;
+    }
+  }
+  if (NewStringId == 0) {
+    //
+    // No string package is found.
+    // Create new string package. StringId 1 is reserved for Language Name string.
+    //
+    *StringId = 2;
+  } else {
+    //
+    // Set new StringId
+    //
+    *StringId = (EFI_STRING_ID) (NewStringId + 1);
+  }
+
+  if (MatchStringPackage != NULL) {
+    StringPackage = MatchStringPackage;
+  } else {
+    //
+    // LanguageName is required to create a new string package.
+    //
+    if (LanguageName == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    StringPackage = AllocateZeroPool (sizeof (HII_STRING_PACKAGE_INSTANCE));
+    if (StringPackage == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    StringPackage->Signature   = HII_STRING_PACKAGE_SIGNATURE;
+    StringPackage->MaxStringId = *StringId;
+    StringPackage->FontId      = 0;
+    InitializeListHead (&StringPackage->FontInfoList);
+
+    //
+    // Fill in the string package header
+    //
+    HeaderSize = (UINT32) (AsciiStrSize ((CHAR8 *) Language) - 1 + sizeof (EFI_HII_STRING_PACKAGE_HDR));
+    StringPackage->StringPkgHdr = AllocateZeroPool (HeaderSize);
+    if (StringPackage->StringPkgHdr == NULL) {
+      FreePool (StringPackage);
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    StringPackage->StringPkgHdr->Header.Type      = EFI_HII_PACKAGE_STRINGS;
+    StringPackage->StringPkgHdr->HdrSize          = HeaderSize;
+    StringPackage->StringPkgHdr->StringInfoOffset = HeaderSize;
+    CopyMem (StringPackage->StringPkgHdr->LanguageWindow, mLanguageWindow, 16 * sizeof (CHAR16));
+    StringPackage->StringPkgHdr->LanguageName     = 1;
+    AsciiStrCpyS (StringPackage->StringPkgHdr->Language, (HeaderSize - OFFSET_OF(EFI_HII_STRING_PACKAGE_HDR,Language)) / sizeof (CHAR8), (CHAR8 *) Language);
+
+    //
+    // Calculate the length of the string blocks, including string block to record
+    // printable language full name and EFI_HII_SIBT_END_BLOCK.
+    //
+    Ucs2BlockSize = (UINT32) (StrSize ((CHAR16 *) LanguageName) + 
+                              (*StringId - 1) * sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK) - sizeof (CHAR16));
+
+    BlockSize     = Ucs2BlockSize + sizeof (EFI_HII_SIBT_END_BLOCK);
+    StringPackage->StringBlock = (UINT8 *) AllocateZeroPool (BlockSize);
+    if (StringPackage->StringBlock == NULL) {
+      FreePool (StringPackage->StringPkgHdr);
+      FreePool (StringPackage);
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    //
+    // Insert the string block of printable language full name
+    //
+    BlockPtr  = StringPackage->StringBlock;
+    *BlockPtr = EFI_HII_SIBT_STRING_UCS2;
+    BlockPtr  += sizeof (EFI_HII_STRING_BLOCK);
+    CopyMem (BlockPtr, (EFI_STRING) LanguageName, StrSize ((EFI_STRING) LanguageName));
+    BlockPtr += StrSize ((EFI_STRING) LanguageName);
+    for (Index = 2; Index <= *StringId - 1; Index ++) {
+      *BlockPtr = EFI_HII_SIBT_STRING_UCS2;
+      BlockPtr += sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK);
+    }
+    //
+    // Insert the end block
+    //
+    *BlockPtr = EFI_HII_SIBT_END;
+
+    //
+    // Append this string package node to string package array in this package list.
+    //
+    StringPackage->StringPkgHdr->Header.Length    = HeaderSize + BlockSize;
+    PackageListNode->PackageListHdr.PackageLength += StringPackage->StringPkgHdr->Header.Length;
+    InsertTailList (&PackageListNode->StringPkgHdr, &StringPackage->StringEntry);
+    NewStringPackageCreated = TRUE;
+  }
+
+  OldBlockSize = StringPackage->StringPkgHdr->Header.Length - StringPackage->StringPkgHdr->HdrSize;
+
+  if (StringFontInfo == NULL) {
+    //
+    // Create a EFI_HII_SIBT_STRING_UCS2_BLOCK since font info is not specified.
+    //
+    Ucs2BlockSize = (UINT32) (StrSize (String) + sizeof (EFI_HII_SIBT_STRING_UCS2_BLOCK)
+                              - sizeof (CHAR16));
+
+    StringBlock = (UINT8 *) AllocateZeroPool (OldBlockSize + Ucs2BlockSize);
+    if (StringBlock == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    //
+    // Copy original string blocks, except the EFI_HII_SIBT_END.
+    //
+    CopyMem (StringBlock, StringPackage->StringBlock, OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK));
+    //
+    // Create a EFI_HII_SIBT_STRING_UCS2 block
+    //
+    BlockPtr  = StringBlock + OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK);
+    *BlockPtr = EFI_HII_SIBT_STRING_UCS2;
+    BlockPtr  += sizeof (EFI_HII_STRING_BLOCK);
+    CopyMem (BlockPtr, (EFI_STRING) String, StrSize ((EFI_STRING) String));
+    BlockPtr += StrSize ((EFI_STRING) String);
+
+    //
+    // Append a EFI_HII_SIBT_END block to the end.
+    //
+    *BlockPtr = EFI_HII_SIBT_END;
+    FreePool (StringPackage->StringBlock);
+    StringPackage->StringBlock = StringBlock;
+    StringPackage->StringPkgHdr->Header.Length += Ucs2BlockSize;
+    PackageListNode->PackageListHdr.PackageLength += Ucs2BlockSize;
+
+  } else {
+    //
+    // StringFontInfo is specified here. If there is a EFI_HII_SIBT_FONT_BLOCK
+    // which refers to this font info, create a EFI_HII_SIBT_STRING_UCS2_FONT block
+    // only. Otherwise create a EFI_HII_SIBT_FONT block with a EFI_HII_SIBT_STRING
+    // _UCS2_FONT block.
+    //
+    Ucs2FontBlockSize = (UINT32) (StrSize (String) + sizeof (EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK) -
+                                  sizeof (CHAR16));
+    if (ReferFontInfoLocally (Private, StringPackage, StringPackage->FontId, FALSE, GlobalFont, &LocalFont)) {
+      //
+      // Create a EFI_HII_SIBT_STRING_UCS2_FONT block only.
+      //
+      StringBlock = (UINT8 *) AllocateZeroPool (OldBlockSize + Ucs2FontBlockSize);
+      if (StringBlock == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      //
+      // Copy original string blocks, except the EFI_HII_SIBT_END.
+      //
+      CopyMem (StringBlock, StringPackage->StringBlock, OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK));
+      //
+      // Create a EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK
+      //
+      BlockPtr  = StringBlock + OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK);
+      *BlockPtr = EFI_HII_SIBT_STRING_UCS2_FONT;
+      BlockPtr  += sizeof (EFI_HII_STRING_BLOCK);
+      *BlockPtr = LocalFont->FontId;
+      BlockPtr ++;
+      CopyMem (BlockPtr, (EFI_STRING) String, StrSize ((EFI_STRING) String));
+      BlockPtr += StrSize ((EFI_STRING) String);
+
+      //
+      // Append a EFI_HII_SIBT_END block to the end.
+      //
+      *BlockPtr = EFI_HII_SIBT_END;
+      FreePool (StringPackage->StringBlock);
+      StringPackage->StringBlock = StringBlock;
+      StringPackage->StringPkgHdr->Header.Length += Ucs2FontBlockSize;
+      PackageListNode->PackageListHdr.PackageLength += Ucs2FontBlockSize;
+
+    } else {
+      //
+      // EFI_HII_SIBT_FONT_BLOCK does not exist in current string package, so
+      // create a EFI_HII_SIBT_FONT block to record the font info, then generate
+      // a EFI_HII_SIBT_STRING_UCS2_FONT block to record the incoming string.
+      //
+      FontBlockSize = (UINT32) (StrSize (((EFI_FONT_INFO *) StringFontInfo)->FontName) +
+                                sizeof (EFI_HII_SIBT_FONT_BLOCK) - sizeof (CHAR16));
+      StringBlock = (UINT8 *) AllocateZeroPool (OldBlockSize + FontBlockSize + Ucs2FontBlockSize);
+      if (StringBlock == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      //
+      // Copy original string blocks, except the EFI_HII_SIBT_END.
+      //
+      CopyMem (StringBlock, StringPackage->StringBlock, OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK));
+
+      //
+      // Create a EFI_HII_SIBT_FONT block firstly and then backup its info in string
+      // package instance for future reference.
+      //
+      BlockPtr = StringBlock + OldBlockSize - sizeof (EFI_HII_SIBT_END_BLOCK);
+
+      Ext2.Header.BlockType = EFI_HII_SIBT_EXT2;
+      Ext2.BlockType2       = EFI_HII_SIBT_FONT;
+      Ext2.Length           = (UINT16) FontBlockSize;
+      CopyMem (BlockPtr, &Ext2, sizeof (EFI_HII_SIBT_EXT2_BLOCK));
+      BlockPtr += sizeof (EFI_HII_SIBT_EXT2_BLOCK);
+
+      *BlockPtr = LocalFont->FontId;
+      BlockPtr ++;
+      CopyMem (BlockPtr, &((EFI_FONT_INFO *) StringFontInfo)->FontSize, sizeof (UINT16));
+      BlockPtr += sizeof (UINT16);
+      CopyMem (BlockPtr, &((EFI_FONT_INFO *) StringFontInfo)->FontStyle, sizeof (EFI_HII_FONT_STYLE));
+      BlockPtr += sizeof (EFI_HII_FONT_STYLE);
+      CopyMem (
+        BlockPtr,
+        &((EFI_FONT_INFO *) StringFontInfo)->FontName,
+        StrSize (((EFI_FONT_INFO *) StringFontInfo)->FontName)
+        );
+      BlockPtr += StrSize (((EFI_FONT_INFO *) StringFontInfo)->FontName);
+      //
+      // Create a EFI_HII_SIBT_STRING_UCS2_FONT_BLOCK
+      //
+      *BlockPtr = EFI_HII_SIBT_STRING_UCS2_FONT;
+      BlockPtr  += sizeof (EFI_HII_STRING_BLOCK);
+      *BlockPtr = LocalFont->FontId;
+      BlockPtr  ++;
+      CopyMem (BlockPtr, (EFI_STRING) String, StrSize ((EFI_STRING) String));
+      BlockPtr += StrSize ((EFI_STRING) String);
+
+      //
+      // Append a EFI_HII_SIBT_END block to the end.
+      //
+      *BlockPtr = EFI_HII_SIBT_END;
+      FreePool (StringPackage->StringBlock);
+      StringPackage->StringBlock = StringBlock;
+      StringPackage->StringPkgHdr->Header.Length += FontBlockSize + Ucs2FontBlockSize;
+      PackageListNode->PackageListHdr.PackageLength += FontBlockSize + Ucs2FontBlockSize;
+
+      //
+      // Increase the FontId to make it unique since we already add 
+      // a EFI_HII_SIBT_FONT block to this string package.
+      //
+      StringPackage->FontId++;
+    }
+  }
+
+Done:
+  if (!EFI_ERROR (Status) && NewStringPackageCreated) {
+    //
+    // Trigger any registered notification function for new string package
+    //
+    Status = InvokeRegisteredFunction (
+      Private,
+      EFI_HII_DATABASE_NOTIFY_NEW_PACK,
+      (VOID *) StringPackage,
+      EFI_HII_PACKAGE_STRINGS,
+      PackageList
+      );
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Update MaxString Id to new StringId
+    //
+    for (Link = PackageListNode->StringPkgHdr.ForwardLink;
+      Link != &PackageListNode->StringPkgHdr;
+      Link = Link->ForwardLink
+      ) {
+        StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+        StringPackage->MaxStringId = *StringId;
+    }
+  } else if (NewStringPackageCreated) {
+    //
+    // Free the allocated new string Package when new string can't be added.
+    //
+    RemoveEntryList (&StringPackage->StringEntry);
+    FreePool (StringPackage->StringBlock);
+    FreePool (StringPackage->StringPkgHdr);
+    FreePool (StringPackage);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function retrieves the string specified by StringId which is associated
+  with the specified PackageList in the language Language and copies it into
+  the buffer specified by String.
+
+  @param  This                   A pointer to the EFI_HII_STRING_PROTOCOL instance.
+  @param  Language               Points to the language for the retrieved string.
+  @param  PackageList            The package list in the HII database to search for
+                                 the  specified string.
+  @param  StringId               The string's id, which is unique within
+                                 PackageList.
+  @param  String                 Points to the new null-terminated string.
+  @param  StringSize             On entry, points to the size of the buffer pointed
+                                 to by  String, in bytes. On return, points to the
+                                 length of the string, in bytes.
+  @param  StringFontInfo         If not NULL, points to the string's font
+                                 information.  It's caller's responsibility to free
+                                 this buffer.
+
+  @retval EFI_SUCCESS            The string was returned successfully.
+  @retval EFI_NOT_FOUND          The string specified by StringId is not available.
+  @retval EFI_NOT_FOUND          The string specified by StringId is available but
+                                                not in the specified language.
+                                                The specified PackageList is not in the database.
+  @retval EFI_INVALID_LANGUAGE   - The string specified by StringId is available but
+  @retval EFI_BUFFER_TOO_SMALL   The buffer specified by StringSize is too small to
+                                  hold the string.
+  @retval EFI_INVALID_PARAMETER  The Language or StringSize was NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by StringSize was not zero and String was NULL.
+  @retval EFI_OUT_OF_RESOURCES   There were insufficient resources to complete the
+                                 request.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetString (
+  IN  CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN  CONST CHAR8                     *Language,
+  IN  EFI_HII_HANDLE                  PackageList,
+  IN  EFI_STRING_ID                   StringId,
+  OUT EFI_STRING                      String,
+  IN  OUT UINTN                       *StringSize,
+  OUT EFI_FONT_INFO                   **StringFontInfo OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+
+  if (This == NULL || Language == NULL || StringId < 1 || StringSize == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (String == NULL && *StringSize != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  PackageListNode = NULL;
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+
+  if (PackageListNode != NULL) {
+    //
+    // First search: to match the StringId in the specified language.
+    //
+    for (Link =  PackageListNode->StringPkgHdr.ForwardLink;
+         Link != &PackageListNode->StringPkgHdr;
+         Link =  Link->ForwardLink
+        ) {
+        StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+        if (HiiCompareLanguage (StringPackage->StringPkgHdr->Language, (CHAR8 *) Language)) {
+          Status = GetStringWorker (Private, StringPackage, StringId, String, StringSize, StringFontInfo);
+          if (Status != EFI_NOT_FOUND) {
+            return Status;
+          }
+        }
+      }
+      //
+      // Second search: to match the StringId in other available languages if exist.
+      //
+      for (Link =  PackageListNode->StringPkgHdr.ForwardLink; 
+           Link != &PackageListNode->StringPkgHdr;
+           Link =  Link->ForwardLink
+          ) {
+      StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);      
+      Status = GetStringWorker (Private, StringPackage, StringId, NULL, NULL, NULL);
+      if (!EFI_ERROR (Status)) {
+        return EFI_INVALID_LANGUAGE;
+      }
+    }    
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+
+/**
+  This function updates the string specified by StringId in the specified PackageList to the text
+  specified by String and, optionally, the font information specified by StringFontInfo.
+
+  @param  This                   A pointer to the EFI_HII_STRING_PROTOCOL instance.
+  @param  PackageList            The package list containing the strings.
+  @param  StringId               The string's id, which is unique within
+                                 PackageList.
+  @param  Language               Points to the language for the updated string.
+  @param  String                 Points to the new null-terminated string.
+  @param  StringFontInfo         Points to the string's font information or NULL if
+                                 the  string font information is not changed.
+
+  @retval EFI_SUCCESS            The string was updated successfully.
+  @retval EFI_NOT_FOUND          The string specified by StringId is not in the
+                                 database.
+  @retval EFI_INVALID_PARAMETER  The String or Language was NULL.
+  @retval EFI_INVALID_PARAMETER  The specified StringFontInfo does not exist in
+                                 current database.
+  @retval EFI_OUT_OF_RESOURCES   The system is out of resources to accomplish the
+                                 task.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiSetString (
+  IN CONST EFI_HII_STRING_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                   PackageList,
+  IN EFI_STRING_ID                    StringId,
+  IN CONST CHAR8                      *Language,
+  IN CONST EFI_STRING                 String,
+  IN CONST EFI_FONT_INFO              *StringFontInfo OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  UINT32                              OldPackageLen;
+
+  if (This == NULL || Language == NULL || StringId < 1 || String == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+  PackageListNode = NULL;
+
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (DatabaseRecord->PackageList);
+    }
+  }
+
+  if (PackageListNode != NULL) {
+    for (Link =  PackageListNode->StringPkgHdr.ForwardLink;
+         Link != &PackageListNode->StringPkgHdr;
+         Link =  Link->ForwardLink
+        ) {
+      StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+      if (HiiCompareLanguage (StringPackage->StringPkgHdr->Language, (CHAR8 *) Language)) {
+        OldPackageLen = StringPackage->StringPkgHdr->Header.Length;
+        Status = SetStringWorker (
+                   Private,
+                   StringPackage,
+                   StringId,
+                   (EFI_STRING) String,
+                   (EFI_FONT_INFO *) StringFontInfo
+                   );
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+        PackageListNode->PackageListHdr.PackageLength += StringPackage->StringPkgHdr->Header.Length - OldPackageLen;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+
+/**
+  This function returns the list of supported languages, in the format specified
+  in Appendix M of UEFI 2.1 spec.
+
+  @param  This                   A pointer to the EFI_HII_STRING_PROTOCOL instance.
+  @param  PackageList            The package list to examine.
+  @param  Languages              Points to the buffer to hold the returned
+                                 null-terminated ASCII string.
+  @param  LanguagesSize          On entry, points to the size of the buffer pointed
+                                 to by  Languages, in bytes. On  return, points to
+                                 the length of Languages, in bytes.
+
+  @retval EFI_SUCCESS            The languages were returned successfully.
+  @retval EFI_INVALID_PARAMETER  The LanguagesSize was NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by LanguagesSize is not zero and Languages is NULL.
+  @retval EFI_BUFFER_TOO_SMALL   The LanguagesSize is too small to hold the list of
+                                  supported languages. LanguageSize is updated to
+                                 contain the required size.
+  @retval EFI_NOT_FOUND          Could not find string package in specified
+                                 packagelist.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetLanguages (
+  IN CONST EFI_HII_STRING_PROTOCOL    *This,
+  IN EFI_HII_HANDLE                   PackageList,
+  IN OUT CHAR8                        *Languages,
+  IN OUT UINTN                        *LanguagesSize
+  )
+{
+  LIST_ENTRY                          *Link;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  UINTN                               ResultSize;
+
+  if (This == NULL || LanguagesSize == NULL || PackageList == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (*LanguagesSize != 0 && Languages == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private = HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  PackageListNode = NULL;
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord  = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = DatabaseRecord->PackageList;
+      break;
+    }
+  }
+  if (PackageListNode == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Search the languages in the specified packagelist.
+  //
+  ResultSize = 0;
+  for (Link = PackageListNode->StringPkgHdr.ForwardLink;
+       Link != &PackageListNode->StringPkgHdr;
+       Link = Link->ForwardLink
+      ) {
+    StringPackage = CR (Link, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    ResultSize += AsciiStrSize (StringPackage->StringPkgHdr->Language);
+    if (ResultSize <= *LanguagesSize) {
+      AsciiStrCpyS (Languages, *LanguagesSize / sizeof (CHAR8), StringPackage->StringPkgHdr->Language);
+      Languages += AsciiStrSize (StringPackage->StringPkgHdr->Language);
+      *(Languages - 1) = L';';
+    }
+  }
+  if (ResultSize == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (*LanguagesSize < ResultSize) {
+    *LanguagesSize = ResultSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *(Languages - 1) = 0;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Each string package has associated with it a single primary language and zero
+  or more secondary languages. This routine returns the secondary languages
+  associated with a package list.
+
+  @param  This                   A pointer to the EFI_HII_STRING_PROTOCOL instance.
+  @param  PackageList            The package list to examine.
+  @param  PrimaryLanguage        Points to the null-terminated ASCII string that specifies
+                                 the primary language. Languages are specified in the
+                                 format specified in Appendix M of the UEFI 2.0 specification.
+  @param  SecondaryLanguages     Points to the buffer to hold the returned null-terminated
+                                 ASCII string that describes the list of
+                                 secondary languages for the specified
+                                 PrimaryLanguage. If there are no secondary
+                                 languages, the function returns successfully, but
+                                 this is set to NULL.
+  @param  SecondaryLanguagesSize On entry, points to the size of the buffer pointed
+                                 to by SecondaryLanguages, in bytes. On return,
+                                 points to the length of SecondaryLanguages in bytes.
+
+  @retval EFI_SUCCESS            Secondary languages were correctly returned.
+  @retval EFI_INVALID_PARAMETER  PrimaryLanguage or SecondaryLanguagesSize was NULL.
+  @retval EFI_INVALID_PARAMETER  The value referenced by SecondaryLanguagesSize is not
+                                 zero and SecondaryLanguages is NULL.
+  @retval EFI_BUFFER_TOO_SMALL   The buffer specified by SecondaryLanguagesSize is
+                                 too small to hold the returned information.
+                                 SecondaryLanguageSize is updated to hold the size of
+                                 the buffer required.
+  @retval EFI_INVALID_LANGUAGE   The language specified by PrimaryLanguage is not
+                                 present in the specified package list.
+  @retval EFI_NOT_FOUND          The specified PackageList is not in the Database.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiGetSecondaryLanguages (
+  IN CONST EFI_HII_STRING_PROTOCOL   *This,
+  IN EFI_HII_HANDLE                  PackageList,
+  IN CONST CHAR8                     *PrimaryLanguage,
+  IN OUT CHAR8                       *SecondaryLanguages,
+  IN OUT UINTN                       *SecondaryLanguagesSize
+  )
+{
+  LIST_ENTRY                          *Link;
+  LIST_ENTRY                          *Link1;
+  HII_DATABASE_PRIVATE_DATA           *Private;
+  HII_DATABASE_RECORD                 *DatabaseRecord;
+  HII_DATABASE_PACKAGE_LIST_INSTANCE  *PackageListNode;
+  HII_STRING_PACKAGE_INSTANCE         *StringPackage;
+  CHAR8                               *Languages;
+  UINTN                               ResultSize;
+
+  if (This == NULL || PackageList == NULL || PrimaryLanguage == NULL || SecondaryLanguagesSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (SecondaryLanguages == NULL && *SecondaryLanguagesSize != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (!IsHiiHandleValid (PackageList)) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private    = HII_STRING_DATABASE_PRIVATE_DATA_FROM_THIS (This);
+
+  PackageListNode = NULL;     
+  for (Link = Private->DatabaseList.ForwardLink; Link != &Private->DatabaseList; Link = Link->ForwardLink) {
+    DatabaseRecord  = CR (Link, HII_DATABASE_RECORD, DatabaseEntry, HII_DATABASE_RECORD_SIGNATURE);
+    if (DatabaseRecord->Handle == PackageList) {
+      PackageListNode = (HII_DATABASE_PACKAGE_LIST_INSTANCE *) (DatabaseRecord->PackageList);
+        break;
+      }
+    }
+    if (PackageListNode == NULL) {
+      return EFI_NOT_FOUND;
+    }
+      
+    Languages  = NULL;
+    ResultSize = 0;
+    for (Link1 = PackageListNode->StringPkgHdr.ForwardLink;
+         Link1 != &PackageListNode->StringPkgHdr;
+         Link1 = Link1->ForwardLink
+        ) {
+    StringPackage = CR (Link1, HII_STRING_PACKAGE_INSTANCE, StringEntry, HII_STRING_PACKAGE_SIGNATURE);
+    if (HiiCompareLanguage (StringPackage->StringPkgHdr->Language, (CHAR8 *) PrimaryLanguage)) {
+      Languages = StringPackage->StringPkgHdr->Language;
+      //
+      // Language is a series of ';' terminated strings, first one is primary
+      // language and following with other secondary languages or NULL if no
+      // secondary languages any more.
+      //
+      Languages = AsciiStrStr (Languages, ";");
+      if (Languages == NULL) {
+        break;
+      }
+      Languages++;
+
+      ResultSize = AsciiStrSize (Languages);
+      if (ResultSize <= *SecondaryLanguagesSize) {
+        AsciiStrCpyS (SecondaryLanguages, *SecondaryLanguagesSize / sizeof (CHAR8), Languages);
+      } else {
+        *SecondaryLanguagesSize = ResultSize;
+        return EFI_BUFFER_TOO_SMALL;
+      }
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_INVALID_LANGUAGE;
+}
+
+/**
+  Converts the ascii character of the string from uppercase to lowercase.
+  This is a internal function.
+
+  @param ConfigString  String to be converted
+
+**/
+VOID
+EFIAPI
+AsciiHiiToLower (
+  IN CHAR8  *ConfigString
+  )
+{
+  ASSERT (ConfigString != NULL);
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  for (; *ConfigString != '\0'; ConfigString++) {
+    if ( *ConfigString >= 'A' && *ConfigString <= 'Z') {
+      *ConfigString = (CHAR8) (*ConfigString - 'A' + 'a');
+    }
+  }
+}
+
+/**
+  Compare whether two names of languages are identical.
+
+  @param  Language1              Name of language 1 from StringPackage
+  @param  Language2              Name of language 2 to be compared with language 1.
+
+  @retval TRUE                   same
+  @retval FALSE                  not same
+
+**/
+BOOLEAN
+HiiCompareLanguage (
+  IN  CHAR8  *Language1,
+  IN  CHAR8  *Language2
+  )
+{
+  UINTN  Index;
+  UINTN  StrLen;
+  CHAR8  *Lan1;
+  CHAR8  *Lan2;
+
+  //
+  // Convert to lower to compare.
+  //
+  StrLen = AsciiStrSize (Language1);
+  Lan1   = AllocateZeroPool (StrLen);
+  ASSERT (Lan1 != NULL);
+  AsciiStrCpyS(Lan1, StrLen / sizeof (CHAR8), Language1);
+  AsciiHiiToLower (Lan1);
+
+  StrLen = AsciiStrSize (Language2);
+  Lan2   = AllocateZeroPool (StrLen);
+  ASSERT (Lan2 != NULL);
+  AsciiStrCpyS(Lan2, StrLen / sizeof (CHAR8), Language2);
+  AsciiHiiToLower (Lan2);
+
+  //
+  // Compare the Primary Language in Language1 to Language2
+  //
+  for (Index = 0; Lan1[Index] != 0 && Lan1[Index] != ';'; Index++) {
+    if (Lan1[Index] != Lan2[Index]) {
+      //
+      // Return FALSE if any characters are different.
+      //
+      FreePool (Lan1);
+      FreePool (Lan2);
+      return FALSE;
+    }
+  }
+
+  FreePool (Lan1);
+  FreePool (Lan2);
+
+  //
+  // Only return TRUE if Language2[Index] is a Null-terminator which means
+  // the Primary Language in Language1 is the same length as Language2.  If
+  // Language2[Index] is not a Null-terminator, then Language2 is longer than
+  // the Primary Language in Language1, and FALSE must be returned.
+  //
+  return (BOOLEAN) (Language2[Index] == 0);
+}
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.c b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.c
new file mode 100644
index 0000000000..adce780e09
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.c
@@ -0,0 +1,152 @@
+/** @file
+This is an example of how a driver retrieve HII data using HII Package List
+Protocol, and how to publish the HII data.
+
+Copyright (c) 2009 - 2011, 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 <Uefi.h>
+#include <Guid/HiiResourceSampleHii.h>
+#include <Protocol/HiiPackageList.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiHiiServicesLib.h>
+#include <Library/HiiLib.h>
+
+#pragma pack(1)
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} HII_VENDOR_DEVICE_PATH;
+#pragma pack()
+
+
+EFI_HII_HANDLE  mHiiHandle = NULL;
+EFI_HANDLE      mDriverHandle = NULL;
+
+HII_VENDOR_DEVICE_PATH  mHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    HII_RESOURCE_SAMPLE_FORM_SET_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  Main entry for this driver.
+  
+  @param[in] ImageHandle     Image handle this driver.
+  @param[in] SystemTable     Pointer to SystemTable.
+
+  @retval EFI_SUCESS     This function always complete successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+HiiResourcesSampleInit (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_HII_PACKAGE_LIST_HEADER     *PackageList;
+
+  //
+  // Retrieve HII package list from ImageHandle
+  //
+  Status = gBS->OpenProtocol (
+                  ImageHandle,
+                  &gEfiHiiPackageListProtocolGuid,
+                  (VOID **) &PackageList,
+                  ImageHandle,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Publish sample Fromset
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mDriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mHiiVendorDevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Publish HII package list to HII Database.
+  //
+  Status = gHiiDatabase->NewPackageList (
+                          gHiiDatabase,
+                          PackageList,
+                          mDriverHandle,
+                          &mHiiHandle
+                          );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unloads the application and its installed protocol.
+
+  @param[in]  ImageHandle       Handle that identifies the image to be unloaded.
+
+  @retval EFI_SUCCESS           The image has been unloaded.
+**/
+EFI_STATUS
+EFIAPI
+HiiResourcesSampleUnload (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  if (mDriverHandle != NULL) {
+    gBS->UninstallProtocolInterface (
+            mDriverHandle,
+            &gEfiDevicePathProtocolGuid,
+            &mHiiVendorDevicePath
+           );
+    mDriverHandle = NULL;
+  }
+
+  if (mHiiHandle != NULL) {
+    HiiRemovePackages (mHiiHandle);
+    mHiiHandle = NULL;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.uni b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.uni
new file mode 100644
index 0000000000..f84a4d59e9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSample.uni
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleDxe.inf b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleDxe.inf
new file mode 100644
index 0000000000..51a598cb05
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleDxe.inf
@@ -0,0 +1,60 @@
+## @file
+#  This is a sample HII resource driver.
+#
+#  This driver show how a HII driver retrieve HII data using HII Package List protocol
+#  and publish the HII data.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = HiiResourcesSample
+  MODULE_UNI_FILE                = HiiResourcesSample.uni
+  FILE_GUID                      = D49D2EB0-44D5-4621-9FD6-1A92C9109B99
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = HiiResourcesSampleInit
+  UNLOAD_IMAGE                   = HiiResourcesSampleUnload
+#
+#  This flag specifies whether HII resource section is generated into PE image.
+#
+  UEFI_HII_RESOURCE_SECTION      = TRUE
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  HiiResourcesSample.c
+  SampleStrings.uni
+  Sample.vfr
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiHiiServicesLib
+  HiiLib
+
+[Protocols]
+  gEfiHiiPackageListProtocolGuid    ## CONSUMES
+  gEfiDevicePathProtocolGuid        ## PRODUCES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  HiiResourcesSampleExtra.uni
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleExtra.uni b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleExtra.uni
new file mode 100644
index 0000000000..63969d3e31
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/HiiResourcesSampleExtra.uni
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/Sample.vfr b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/Sample.vfr
new file mode 100644
index 0000000000..3ca20988e8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/Sample.vfr
@@ -0,0 +1,45 @@
+// *++
+//
+// Copyright (c) 2009 - 2011, 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.
+//
+// Module Name:
+//
+//   Sample.vfr
+//
+// Abstract:
+//
+//   Sample Form.
+//
+// Revision History:
+//
+// --*/
+
+#include <Guid/HiiResourceSampleHii.h>
+
+formset
+  guid     = HII_RESOURCE_SAMPLE_FORM_SET_GUID,
+  title    = STRING_TOKEN(STR_SAMPLE_FORM_SET_TITLE),
+  help     = STRING_TOKEN(STR_SAMPLE_FORM_SET_HELP),
+
+  form formid = 1,
+       title  = STRING_TOKEN(STR_SAMPLE_FORM1_TITLE);
+
+  	text
+      help   = STRING_TOKEN(STR_SAMPLE_VERSION_HELP),
+      text   = STRING_TOKEN(STR_SAMPLE_VERSION_TEXT),
+	    text   = STRING_TOKEN(STR_SAMPLE_EMPTY);
+
+    subtitle text = STRING_TOKEN(STR_SAMPLE_EMPTY);
+
+    subtitle text = STRING_TOKEN(STR_SAMPLE_ESC);
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/SampleStrings.uni b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/SampleStrings.uni
new file mode 100644
index 0000000000..ba5a1d3fe4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/HiiResourcesSampleDxe/SampleStrings.uni
diff --git a/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.c b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.c
new file mode 100644
index 0000000000..a956ea975a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.c
@@ -0,0 +1,257 @@
+/** @file
+  Dummy implementation of Legacy Region 2 Protocol.
+
+  This generic implementation of the Legacy Region 2 Protocol does not actually 
+  perform any lock/unlock operations.  This module may be used on platforms 
+  that do not provide HW locking of the legacy memory regions.  It can also 
+  be used as a template driver for implementing the Legacy Region 2 Protocol on
+  a platform that does support HW locking of the legacy memory regions.
+
+Copyright (c) 2009 - 2010, 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 <LegacyRegion2.h>
+
+EFI_HANDLE   mLegacyRegion2Handle = NULL;
+
+EFI_LEGACY_REGION2_PROTOCOL  mLegacyRegion2 = {
+  LegacyRegion2Decode,
+  LegacyRegion2Lock,
+  LegacyRegion2BootLock,
+  LegacyRegion2Unlock,
+  LegacyRegionGetInfo
+};
+
+/**
+  Modify the hardware to allow (decode) or disallow (not decode) memory reads in a region.
+
+  If the On parameter evaluates to TRUE, this function enables memory reads in the address range 
+  Start to (Start + Length - 1).
+  If the On parameter evaluates to FALSE, this function disables memory reads in the address range 
+  Start to (Start + Length - 1).
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose attributes
+                                should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address
+                                was not aligned to a region's starting address or if the length
+                                was greater than the number of bytes in the first region.
+  @param  On[in]                Decode / Non-Decode flag.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Decode (
+  IN  EFI_LEGACY_REGION2_PROTOCOL  *This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity,
+  IN  BOOLEAN                      *On
+  )
+{
+  if ((Start < 0xC0000) || ((Start + Length - 1) > 0xFFFFF)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ASSERT (Granularity != NULL);
+  *Granularity = 0;
+  return EFI_SUCCESS;
+}
+
+/**
+  Modify the hardware to disallow memory writes in a region.
+
+  This function changes the attributes of a memory range to not allow writes.
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Lock (
+  IN  EFI_LEGACY_REGION2_PROTOCOL *This,
+  IN  UINT32                      Start,
+  IN  UINT32                      Length,
+  OUT UINT32                      *Granularity
+  )
+{
+  if ((Start < 0xC0000) || ((Start + Length - 1) > 0xFFFFF)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ASSERT (Granularity != NULL);
+  *Granularity = 0;
+  return EFI_SUCCESS;
+}
+
+/**
+  Modify the hardware to disallow memory attribute changes in a region.
+
+  This function makes the attributes of a region read only. Once a region is boot-locked with this 
+  function, the read and write attributes of that region cannot be changed until a power cycle has
+  reset the boot-lock attribute. Calls to Decode(), Lock() and Unlock() will have no effect.
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+  @retval EFI_UNSUPPORTED       The chipset does not support locking the configuration registers in
+                                a way that will not affect memory regions outside the legacy memory
+                                region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2BootLock (
+  IN  EFI_LEGACY_REGION2_PROTOCOL         *This,
+  IN  UINT32                              Start,
+  IN  UINT32                              Length,
+  OUT UINT32                              *Granularity
+  )
+{
+  if ((Start < 0xC0000) || ((Start + Length - 1) > 0xFFFFF)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Modify the hardware to allow memory writes in a region.
+
+  This function changes the attributes of a memory range to allow writes.  
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Unlock (
+  IN  EFI_LEGACY_REGION2_PROTOCOL  *This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity
+  )
+{
+  if ((Start < 0xC0000) || ((Start + Length - 1) > 0xFFFFF)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ASSERT (Granularity != NULL);
+  *Granularity = 0;
+  return EFI_SUCCESS;
+}
+
+/**
+  Get region information for the attributes of the Legacy Region.
+
+  This function is used to discover the granularity of the attributes for the memory in the legacy 
+  region. Each attribute may have a different granularity and the granularity may not be the same
+  for all memory ranges in the legacy region.  
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION2_PROTOCOL instance.
+  @param  DescriptorCount[out]  The number of region descriptor entries returned in the Descriptor
+                                buffer.
+  @param  Descriptor[out]       A pointer to a pointer used to return a buffer where the legacy
+                                region information is deposited. This buffer will contain a list of
+                                DescriptorCount number of region descriptors.  This function will
+                                provide the memory for the buffer.
+
+  @retval EFI_SUCCESS           The information structure was returned.
+  @retval EFI_UNSUPPORTED       This function is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegionGetInfo (
+  IN  EFI_LEGACY_REGION2_PROTOCOL   *This,
+  OUT UINT32                        *DescriptorCount,
+  OUT EFI_LEGACY_REGION_DESCRIPTOR  **Descriptor
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  The user Entry Point for module LegacyRegionDxe.  The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Install (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  
+  //
+  // Make sure the Legacy Region 2 Protocol is not already installed in the system
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiLegacyRegion2ProtocolGuid);
+  
+  //
+  // Install the protocol on a new handle.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mLegacyRegion2Handle,
+                  &gEfiLegacyRegion2ProtocolGuid, &mLegacyRegion2,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.h b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.h
new file mode 100644
index 0000000000..be4f85481f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2.h
@@ -0,0 +1,175 @@
+/** @file
+  Internal include file for the dummy Legacy Region 2 Protocol implementation.
+
+Copyright (c) 2009, 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.
+
+**/
+
+#ifndef __DUMMY_LEGACY_REGION2_H__
+#define __DUMMY_LEGACY_REGION2_H__
+
+#include <Protocol/LegacyRegion2.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+/**
+  Modify the hardware to allow (decode) or disallow (not decode) memory reads in a region.
+
+  If the On parameter evaluates to TRUE, this function enables memory reads in the address range 
+  Start to (Start + Length - 1).
+  If the On parameter evaluates to FALSE, this function disables memory reads in the address range 
+  Start to (Start + Length - 1).
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose attributes
+                                should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address
+                                was not aligned to a region's starting address or if the length
+                                was greater than the number of bytes in the first region.
+  @param  On[in]                Decode / Non-Decode flag.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Decode (
+  IN  EFI_LEGACY_REGION2_PROTOCOL  *This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity,
+  IN  BOOLEAN                      *On
+  );
+
+/**
+  Modify the hardware to disallow memory writes in a region.
+
+  This function changes the attributes of a memory range to not allow writes.
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Lock (
+  IN  EFI_LEGACY_REGION2_PROTOCOL *This,
+  IN  UINT32                      Start,
+  IN  UINT32                      Length,
+  OUT UINT32                      *Granularity
+  );
+
+/**
+  Modify the hardware to disallow memory attribute changes in a region.
+
+  This function makes the attributes of a region read only. Once a region is boot-locked with this 
+  function, the read and write attributes of that region cannot be changed until a power cycle has
+  reset the boot-lock attribute. Calls to Decode(), Lock() and Unlock() will have no effect.
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+  @retval EFI_UNSUPPORTED       The chipset does not support locking the configuration registers in
+                                a way that will not affect memory regions outside the legacy memory
+                                region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2BootLock (
+  IN EFI_LEGACY_REGION2_PROTOCOL          *This,
+  IN  UINT32                              Start,
+  IN  UINT32                              Length,
+  OUT UINT32                              *Granularity
+  );
+
+/**
+  Modify the hardware to allow memory writes in a region.
+
+  This function changes the attributes of a memory range to allow writes.  
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  Start[in]             The beginning of the physical address of the region whose
+                                attributes should be modified.
+  @param  Length[in]            The number of bytes of memory whose attributes should be modified.
+                                The actual number of bytes modified may be greater than the number
+                                specified.
+  @param  Granularity[out]      The number of bytes in the last region affected. This may be less
+                                than the total number of bytes affected if the starting address was
+                                not aligned to a region's starting address or if the length was
+                                greater than the number of bytes in the first region.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegion2Unlock (
+  IN  EFI_LEGACY_REGION2_PROTOCOL  *This,
+  IN  UINT32                       Start,
+  IN  UINT32                       Length,
+  OUT UINT32                       *Granularity
+  );
+
+/**
+  Get region information for the attributes of the Legacy Region.
+
+  This function is used to discover the granularity of the attributes for the memory in the legacy 
+  region. Each attribute may have a different granularity and the granularity may not be the same
+  for all memory ranges in the legacy region.  
+
+  @param  This[in]              Indicates the EFI_LEGACY_REGION_PROTOCOL instance.
+  @param  DescriptorCount[out]  The number of region descriptor entries returned in the Descriptor
+                                buffer.
+  @param  Descriptor[out]       A pointer to a pointer used to return a buffer where the legacy
+                                region information is deposited. This buffer will contain a list of
+                                DescriptorCount number of region descriptors.  This function will
+                                provide the memory for the buffer.
+
+  @retval EFI_SUCCESS           The region's attributes were successfully modified.
+  @retval EFI_INVALID_PARAMETER If Start or Length describe an address not in the Legacy Region.
+
+**/
+EFI_STATUS
+EFIAPI
+LegacyRegionGetInfo (
+  IN  EFI_LEGACY_REGION2_PROTOCOL   *This,
+  OUT UINT32                        *DescriptorCount,
+  OUT EFI_LEGACY_REGION_DESCRIPTOR  **Descriptor
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.inf b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.inf
new file mode 100644
index 0000000000..46cd490d8f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.inf
@@ -0,0 +1,60 @@
+## @file
+#  Produces the Legacy Region 2 Protocol.
+#
+#  This generic implementation of the Legacy Region 2 Protocol does not actually 
+#  perform any lock/unlock operations.  This module may be used on platforms 
+#  that do not provide HW locking of the legacy memory regions.  It can also 
+#  be used as a template driver for implementing the Legacy Region 2 Protocol on
+#  a platform that does support HW locking of the legacy memory regions.
+#
+#  Note: This module does not fully comply with PI Specification of Legacy Region 2
+#  Protocol. For Lock/UnLock/Decode, EFI_SUCCESS is returned although the region's
+#  attributes were not actually modified.
+#
+# Copyright (c) 2009 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = LegacyRegion2Dxe
+  MODULE_UNI_FILE                = LegacyRegion2Dxe.uni
+  FILE_GUID                      = EC2BEECA-E84A-445B-869B-F7A73C96F58A
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0  
+  ENTRY_POINT                    = LegacyRegion2Install
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  LegacyRegion2.c
+  LegacyRegion2.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  DebugLib
+  UefiBootServicesTableLib
+
+[Protocols]
+  gEfiLegacyRegion2ProtocolGuid                 ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  LegacyRegion2DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.uni b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.uni
new file mode 100644
index 0000000000..068418c453
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2DxeExtra.uni b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2DxeExtra.uni
new file mode 100644
index 0000000000..0d1d0b8fcc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LegacyRegion2Dxe/LegacyRegion2DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.c b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.c
new file mode 100644
index 0000000000..f1880d464d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.c
@@ -0,0 +1,425 @@
+/** @file
+  LockBox SMM driver.
+  
+  Caution: This module requires additional review when modified.
+  This driver will have external input - communicate buffer in SMM mode.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+  
+  SmmLockBoxHandler(), SmmLockBoxRestore(), SmmLockBoxUpdate(), SmmLockBoxSave()
+  will receive untrusted input and do basic validation.
+
+Copyright (c) 2010 - 2015, 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 <PiSmm.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/SmmMemLib.h>
+#include <Library/LockBoxLib.h>
+
+#include <Protocol/SmmReadyToLock.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/LockBox.h>
+#include <Guid/SmmLockBox.h>
+
+BOOLEAN              mLocked = FALSE;
+
+/**
+  Dispatch function for SMM lock box save.
+
+  Caution: This function may receive untrusted input.
+  Restore buffer and length are external input, so this function will validate
+  it is in SMRAM.
+
+  @param LockBoxParameterSave  parameter of lock box save 
+**/
+VOID
+SmmLockBoxSave (
+  IN EFI_SMM_LOCK_BOX_PARAMETER_SAVE *LockBoxParameterSave
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SMM_LOCK_BOX_PARAMETER_SAVE TempLockBoxParameterSave;
+
+  //
+  // Sanity check
+  //
+  if (mLocked) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Locked!\n"));
+    LockBoxParameterSave->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  CopyMem (&TempLockBoxParameterSave, LockBoxParameterSave, sizeof (EFI_SMM_LOCK_BOX_PARAMETER_SAVE));
+
+  //
+  // Sanity check
+  //
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)TempLockBoxParameterSave.Buffer, (UINTN)TempLockBoxParameterSave.Length)) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Save address in SMRAM or buffer overflow!\n"));
+    LockBoxParameterSave->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  //
+  // Save data
+  //
+  Status = SaveLockBox (
+             &TempLockBoxParameterSave.Guid,
+             (VOID *)(UINTN)TempLockBoxParameterSave.Buffer,
+             (UINTN)TempLockBoxParameterSave.Length
+             );
+  LockBoxParameterSave->Header.ReturnStatus = (UINT64)Status;
+  return ;
+}
+
+/**
+  Dispatch function for SMM lock box set attributes.
+
+  @param LockBoxParameterSetAttributes  parameter of lock box set attributes
+**/
+VOID
+SmmLockBoxSetAttributes (
+  IN EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES *LockBoxParameterSetAttributes
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES TempLockBoxParameterSetAttributes;
+
+  //
+  // Sanity check
+  //
+  if (mLocked) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Locked!\n"));
+    LockBoxParameterSetAttributes->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  CopyMem (&TempLockBoxParameterSetAttributes, LockBoxParameterSetAttributes, sizeof (EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES));
+
+  //
+  // Update data
+  //
+  Status = SetLockBoxAttributes (
+             &TempLockBoxParameterSetAttributes.Guid,
+             TempLockBoxParameterSetAttributes.Attributes
+             );
+  LockBoxParameterSetAttributes->Header.ReturnStatus = (UINT64)Status;
+  return ;
+}
+
+/**
+  Dispatch function for SMM lock box update.
+
+  Caution: This function may receive untrusted input.
+  Restore buffer and length are external input, so this function will validate
+  it is in SMRAM.
+
+  @param LockBoxParameterUpdate  parameter of lock box update 
+**/
+VOID
+SmmLockBoxUpdate (
+  IN EFI_SMM_LOCK_BOX_PARAMETER_UPDATE *LockBoxParameterUpdate
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SMM_LOCK_BOX_PARAMETER_UPDATE TempLockBoxParameterUpdate;
+
+  //
+  // Sanity check
+  //
+  if (mLocked) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Locked!\n"));
+    LockBoxParameterUpdate->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  CopyMem (&TempLockBoxParameterUpdate, LockBoxParameterUpdate, sizeof (EFI_SMM_LOCK_BOX_PARAMETER_UPDATE));
+
+  //
+  // Sanity check
+  //
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)TempLockBoxParameterUpdate.Buffer, (UINTN)TempLockBoxParameterUpdate.Length)) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Update address in SMRAM or buffer overflow!\n"));
+    LockBoxParameterUpdate->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  //
+  // Update data
+  //
+  Status = UpdateLockBox (
+             &TempLockBoxParameterUpdate.Guid,
+             (UINTN)TempLockBoxParameterUpdate.Offset,
+             (VOID *)(UINTN)TempLockBoxParameterUpdate.Buffer,
+             (UINTN)TempLockBoxParameterUpdate.Length
+             );
+  LockBoxParameterUpdate->Header.ReturnStatus = (UINT64)Status;
+  return ;
+}
+
+/**
+  Dispatch function for SMM lock box restore.
+
+  Caution: This function may receive untrusted input.
+  Restore buffer and length are external input, so this function will validate
+  it is in SMRAM.
+
+  @param LockBoxParameterRestore  parameter of lock box restore 
+**/
+VOID
+SmmLockBoxRestore (
+  IN EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *LockBoxParameterRestore
+  )
+{
+  EFI_STATUS                     Status;
+  EFI_SMM_LOCK_BOX_PARAMETER_RESTORE TempLockBoxParameterRestore;
+
+  CopyMem (&TempLockBoxParameterRestore, LockBoxParameterRestore, sizeof (EFI_SMM_LOCK_BOX_PARAMETER_RESTORE));
+
+  //
+  // Sanity check
+  //
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)TempLockBoxParameterRestore.Buffer, (UINTN)TempLockBoxParameterRestore.Length)) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Restore address in SMRAM or buffer overflow!\n"));
+    LockBoxParameterRestore->Header.ReturnStatus = (UINT64)EFI_ACCESS_DENIED;
+    return ;
+  }
+
+  //
+  // Restore data
+  //
+  if ((TempLockBoxParameterRestore.Length == 0) && (TempLockBoxParameterRestore.Buffer == 0)) {
+    Status = RestoreLockBox (
+               &TempLockBoxParameterRestore.Guid,
+               NULL,
+               NULL
+               );
+  } else {
+    Status = RestoreLockBox (
+               &TempLockBoxParameterRestore.Guid,
+               (VOID *)(UINTN)TempLockBoxParameterRestore.Buffer,
+               (UINTN *)&TempLockBoxParameterRestore.Length
+               );
+  }
+  LockBoxParameterRestore->Header.ReturnStatus = (UINT64)Status;
+  return ;
+}
+
+/**
+  Dispatch function for SMM lock box restore all in place.
+
+  @param LockBoxParameterRestoreAllInPlace  parameter of lock box restore all in place
+**/
+VOID
+SmmLockBoxRestoreAllInPlace (
+  IN EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *LockBoxParameterRestoreAllInPlace
+  )
+{
+  EFI_STATUS                     Status;
+
+  Status = RestoreAllLockBoxInPlace ();
+  LockBoxParameterRestoreAllInPlace->Header.ReturnStatus = (UINT64)Status;
+  return ;
+}
+
+/**
+  Dispatch function for a Software SMI handler.
+
+  Caution: This function may receive untrusted input.
+  Communicate buffer and buffer size are external input, so this function will do basic validation.
+
+  @param DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param Context         Points to an optional handler context which was specified when the
+                         handler was registered.
+  @param CommBuffer      A pointer to a collection of data in memory that will
+                         be conveyed from a non-SMM environment into an SMM environment.
+  @param CommBufferSize  The size of the CommBuffer.
+
+  @retval EFI_SUCCESS Command is handled successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmLockBoxHandler (
+  IN EFI_HANDLE  DispatchHandle,
+  IN CONST VOID  *Context         OPTIONAL,
+  IN OUT VOID    *CommBuffer      OPTIONAL,
+  IN OUT UINTN   *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_SMM_LOCK_BOX_PARAMETER_HEADER *LockBoxParameterHeader;
+  UINTN                             TempCommBufferSize;
+
+  DEBUG ((EFI_D_ERROR, "SmmLockBox SmmLockBoxHandler Enter\n"));
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  //
+  // Sanity check
+  //
+  if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_HEADER)) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size invalid!\n"));
+    return EFI_SUCCESS;
+  }
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  LockBoxParameterHeader = (EFI_SMM_LOCK_BOX_PARAMETER_HEADER *)((UINTN)CommBuffer);
+
+  LockBoxParameterHeader->ReturnStatus = (UINT64)-1;
+
+  DEBUG ((EFI_D_ERROR, "SmmLockBox LockBoxParameterHeader - %x\n", (UINTN)LockBoxParameterHeader));
+
+  DEBUG ((EFI_D_ERROR, "SmmLockBox Command - %x\n", (UINTN)LockBoxParameterHeader->Command));
+
+  switch (LockBoxParameterHeader->Command) {
+  case EFI_SMM_LOCK_BOX_COMMAND_SAVE:
+    if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_SAVE)) {
+      DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size for SAVE invalid!\n"));
+      break;
+    }
+    SmmLockBoxSave ((EFI_SMM_LOCK_BOX_PARAMETER_SAVE *)(UINTN)LockBoxParameterHeader);
+    break;
+  case EFI_SMM_LOCK_BOX_COMMAND_UPDATE:
+    if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_UPDATE)) {
+      DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size for UPDATE invalid!\n"));
+      break;
+    }
+    SmmLockBoxUpdate ((EFI_SMM_LOCK_BOX_PARAMETER_UPDATE *)(UINTN)LockBoxParameterHeader);
+    break;
+  case EFI_SMM_LOCK_BOX_COMMAND_RESTORE:
+    if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE)) {
+      DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size for RESTORE invalid!\n"));
+      break;
+    }
+    SmmLockBoxRestore ((EFI_SMM_LOCK_BOX_PARAMETER_RESTORE *)(UINTN)LockBoxParameterHeader);
+    break;
+  case EFI_SMM_LOCK_BOX_COMMAND_SET_ATTRIBUTES:
+    if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES)) {
+      DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size for SET_ATTRIBUTES invalid!\n"));
+      break;
+    }
+    SmmLockBoxSetAttributes ((EFI_SMM_LOCK_BOX_PARAMETER_SET_ATTRIBUTES *)(UINTN)LockBoxParameterHeader);
+    break;
+  case EFI_SMM_LOCK_BOX_COMMAND_RESTORE_ALL_IN_PLACE:
+    if (TempCommBufferSize < sizeof(EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE)) {
+      DEBUG ((EFI_D_ERROR, "SmmLockBox Command Buffer Size for RESTORE_ALL_IN_PLACE invalid!\n"));
+      break;
+    }
+    SmmLockBoxRestoreAllInPlace ((EFI_SMM_LOCK_BOX_PARAMETER_RESTORE_ALL_IN_PLACE *)(UINTN)LockBoxParameterHeader);
+    break;
+  default:
+    DEBUG ((EFI_D_ERROR, "SmmLockBox Command invalid!\n"));
+    break;
+  }
+
+  LockBoxParameterHeader->Command = (UINT32)-1;
+
+  DEBUG ((EFI_D_ERROR, "SmmLockBox SmmLockBoxHandler Exit\n"));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Smm Ready To Lock event notification handler.
+
+  It sets a flag indicating that SMRAM has been locked.
+  
+  @param[in] Protocol   Points to the protocol's unique identifier.
+  @param[in] Interface  Points to the interface instance.
+  @param[in] Handle     The handle on which the interface was installed.
+
+  @retval EFI_SUCCESS   Notification handler runs successfully.
+ **/
+EFI_STATUS
+EFIAPI
+SmmReadyToLockEventNotify (
+  IN CONST EFI_GUID  *Protocol,
+  IN VOID            *Interface,
+  IN EFI_HANDLE      Handle
+  )
+{
+  mLocked = TRUE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Entry Point for LockBox SMM driver.
+
+  @param[in] ImageHandle  Image handle of this driver.
+  @param[in] SystemTable  A Pointer to the EFI System Table.
+
+  @retval EFI_SUCEESS     
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+EFIAPI
+SmmLockBoxEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    DispatchHandle;
+  VOID                          *Registration;
+
+  //
+  // Register LockBox communication handler
+  //
+  Status = gSmst->SmiHandlerRegister (
+                    SmmLockBoxHandler,
+                    &gEfiSmmLockBoxCommunicationGuid,
+                    &DispatchHandle
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register SMM Ready To Lock Protocol notification
+  //
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmReadyToLockProtocolGuid,
+                    SmmReadyToLockEventNotify,
+                    &Registration
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install NULL to DXE data base as notify
+  //
+  ImageHandle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &ImageHandle,
+                  &gEfiLockBoxProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.inf b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.inf
new file mode 100644
index 0000000000..772cad9890
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.inf
@@ -0,0 +1,66 @@
+## @file
+#  LockBox SMM driver.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - communicate buffer in SMM mode.
+#  This external input must be validated carefully to avoid security issue like
+#  buffer overflow, integer overflow.
+#
+#  Copyright (c) 2010 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmLockBox
+  MODULE_UNI_FILE                = SmmLockBox.uni
+  FILE_GUID                      = 33FB3535-F15E-4c17-B303-5EB94595ECB6
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = SmmLockBoxEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 
+#
+
+[Sources]
+  SmmLockBox.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  SmmServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  LockBoxLib
+  SmmMemLib
+
+[Guids]
+  gEfiSmmLockBoxCommunicationGuid   ## PRODUCES ## GUID # SmiHandlerRegister
+
+[Protocols]
+  gEfiSmmReadyToLockProtocolGuid    ## NOTIFY
+  gEfiLockBoxProtocolGuid           ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmmLockBoxExtra.uni
diff --git a/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.uni b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.uni
new file mode 100644
index 0000000000..dd3292dcf3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBox.uni
diff --git a/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBoxExtra.uni b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBoxExtra.uni
new file mode 100644
index 0000000000..ebc5a0d139
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/LockBox/SmmLockBox/SmmLockBoxExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.inf b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.inf
new file mode 100644
index 0000000000..975dc4cd3a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.inf
@@ -0,0 +1,59 @@
+## @file
+# This driver first constructs the non-tested memory range, then performs the R/W/V memory test.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = GenericMemoryTestDxe
+  MODULE_UNI_FILE                = GenericMemoryTestDxe.uni
+  FILE_GUID                      = 9C1080EE-D02E-487f-9432-F3BF086EC180
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = GenericMemoryTestEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  LightMemoryTest.h
+  LightMemoryTest.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  ReportStatusCodeLib
+  DxeServicesTableLib
+  HobLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Protocols]
+  gEfiCpuArchProtocolGuid                       ## CONSUMES
+  gEfiGenericMemTestProtocolGuid                ## PRODUCES
+
+[Depex]
+  gEfiCpuArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  GenericMemoryTestDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.uni b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.uni
new file mode 100644
index 0000000000..925947ac89
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxe.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxeExtra.uni b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxeExtra.uni
new file mode 100644
index 0000000000..0f03bb7a9a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/GenericMemoryTestDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.c b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.c
new file mode 100644
index 0000000000..6161274a94
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.c
@@ -0,0 +1,906 @@
+/** @file
+
+  Copyright (c) 2006 - 2010, 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 "LightMemoryTest.h"
+
+//
+// Global:
+// Since this driver will only ever produce one instance of the memory test
+// protocol, so we do not need to dynamically allocate the PrivateData.
+//
+EFI_PHYSICAL_ADDRESS    mCurrentAddress;
+LIST_ENTRY          *mCurrentLink;
+NONTESTED_MEMORY_RANGE  *mCurrentRange;
+UINT64                  mTestedSystemMemory;
+UINT64                  mNonTestedSystemMemory;
+
+UINT32                  GenericMemoryTestMonoPattern[GENERIC_CACHELINE_SIZE / 4] = {
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5,
+  0x5a5a5a5a,
+  0xa5a5a5a5
+};
+
+/**
+  Compares the contents of two buffers.
+
+  This function compares Length bytes of SourceBuffer to Length bytes of DestinationBuffer.
+  If all Length bytes of the two buffers are identical, then 0 is returned.  Otherwise, the
+  value returned is the first mismatched byte in SourceBuffer subtracted from the first
+  mismatched byte in DestinationBuffer.
+  
+  If Length = 0, then ASSERT().
+
+  @param[in] DestinationBuffer The pointer to the destination buffer to compare.
+  @param[in] SourceBuffer      The pointer to the source buffer to compare.
+  @param[in] Length            The number of bytes to compare.
+
+  @return 0                 All Length bytes of the two buffers are identical.
+  @retval Non-zero          The first mismatched byte in SourceBuffer subtracted from the first
+                            mismatched byte in DestinationBuffer.
+                            
+**/
+INTN
+EFIAPI
+CompareMemWithoutCheckArgument (
+  IN      CONST VOID                *DestinationBuffer,
+  IN      CONST VOID                *SourceBuffer,
+  IN      UINTN                     Length
+  )
+{
+  ASSERT (Length > 0);
+  while ((--Length != 0) &&
+         (*(INT8*)DestinationBuffer == *(INT8*)SourceBuffer)) {
+    DestinationBuffer = (INT8*)DestinationBuffer + 1;
+    SourceBuffer = (INT8*)SourceBuffer + 1;
+  }
+  return (INTN)*(UINT8*)DestinationBuffer - (INTN)*(UINT8*)SourceBuffer;
+}
+
+/**
+  Construct the system base memory range through GCD service.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS          Successful construct the base memory range through GCD service.
+  @retval EFI_OUT_OF_RESOURCE  Could not allocate needed resource from base memory.
+  @retval Others               Failed to construct base memory range through GCD service.
+                            
+**/
+EFI_STATUS
+ConstructBaseMemoryRange (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  )
+{
+  UINTN                           NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap;
+  UINTN                           Index;
+
+  //
+  // Base memory will always below 4G
+  //
+  gDS->GetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeSystemMemory) {
+      Private->BaseMemorySize += MemorySpaceMap[Index].Length;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Destroy the link list base on the correspond link list type.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+                            
+**/
+VOID
+DestroyLinkList (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  )
+{
+  LIST_ENTRY          *Link;
+  NONTESTED_MEMORY_RANGE  *NontestedRange;
+
+  Link = Private->NonTestedMemRanList.BackLink;
+
+  while (Link != &Private->NonTestedMemRanList) {
+    RemoveEntryList (Link);
+    NontestedRange = NONTESTED_MEMORY_RANGE_FROM_LINK (Link);
+    gBS->FreePool (NontestedRange);
+    Link = Private->NonTestedMemRanList.BackLink;;
+  }
+}
+
+/**
+  Add the extened memory to whole system memory map.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS Successful add all the extended memory to system memory map.
+  @retval Others      Failed to add the tested extended memory.
+                            
+**/
+EFI_STATUS
+UpdateMemoryMap (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  )
+{
+  LIST_ENTRY          *Link;
+  NONTESTED_MEMORY_RANGE  *Range;
+
+  Link = Private->NonTestedMemRanList.ForwardLink;
+
+  while (Link != &Private->NonTestedMemRanList) {
+    Range = NONTESTED_MEMORY_RANGE_FROM_LINK (Link);
+
+    gDS->RemoveMemorySpace (
+          Range->StartAddress,
+          Range->Length
+          );
+
+    gDS->AddMemorySpace (
+          EfiGcdMemoryTypeSystemMemory,
+          Range->StartAddress,
+          Range->Length,
+          Range->Capabilities &~(EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED | EFI_MEMORY_RUNTIME)
+          );
+
+    Link = Link->ForwardLink;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Test a range of the memory directly .
+
+  @param[in] Private       Point to generic memory test driver's private data.
+  @param[in] StartAddress  Starting address of the memory range to be tested.
+  @param[in] Length        Length in bytes of the memory range to be tested.
+  @param[in] Capabilities  The bit mask of attributes that the memory range supports.
+
+  @retval EFI_SUCCESS      Successful test the range of memory.
+  @retval Others           Failed to test the range of memory.
+                            
+**/
+EFI_STATUS
+DirectRangeTest (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         StartAddress,
+  IN  UINT64                       Length,
+  IN  UINT64                       Capabilities
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Perform a dummy memory test, so directly write the pattern to all range
+  //
+  WriteMemory (Private, StartAddress, Length);
+
+  //
+  // Verify the memory range
+  //
+  Status = VerifyMemory (Private, StartAddress, Length);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Add the tested compatible memory to system memory using GCD service
+  //
+  gDS->RemoveMemorySpace (
+        StartAddress,
+        Length
+        );
+
+  gDS->AddMemorySpace (
+        EfiGcdMemoryTypeSystemMemory,
+        StartAddress,
+        Length,
+        Capabilities &~(EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED | EFI_MEMORY_RUNTIME)
+        );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Construct the system non-tested memory range through GCD service.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS          Successful construct the non-tested memory range through GCD service.
+  @retval EFI_OUT_OF_RESOURCE  Could not allocate needed resource from base memory.
+  @retval Others               Failed to construct non-tested memory range through GCD service.
+                            
+**/
+EFI_STATUS
+ConstructNonTestedMemoryRange (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  )
+{
+  NONTESTED_MEMORY_RANGE          *Range;
+  BOOLEAN                         NoFound;
+  UINTN                           NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap;
+  UINTN                           Index;
+
+  //
+  // Non tested memory range may be span 4G here
+  //
+  NoFound = TRUE;
+
+  gDS->GetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeReserved &&
+        (MemorySpaceMap[Index].Capabilities & (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED)) ==
+          (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED)
+          ) {
+      NoFound = FALSE;
+      //
+      // Light version do not need to process >4G memory range
+      //
+      gBS->AllocatePool (
+            EfiBootServicesData,
+            sizeof (NONTESTED_MEMORY_RANGE),
+            (VOID **) &Range
+            );
+
+      Range->Signature    = EFI_NONTESTED_MEMORY_RANGE_SIGNATURE;
+      Range->StartAddress = MemorySpaceMap[Index].BaseAddress;
+      Range->Length       = MemorySpaceMap[Index].Length;
+      Range->Capabilities = MemorySpaceMap[Index].Capabilities;
+
+      mNonTestedSystemMemory += MemorySpaceMap[Index].Length;
+      InsertTailList (&Private->NonTestedMemRanList, &Range->Link);
+    }
+  }
+
+  if (NoFound) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write the memory test pattern into a range of physical memory.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  @param[in] Start    The memory range's start address.
+  @param[in] Size     The memory range's size.
+
+  @retval EFI_SUCCESS Successful write the test pattern into the non-tested memory.
+  @retval Others      The test pattern may not really write into the physical memory.
+                            
+**/
+EFI_STATUS
+WriteMemory (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         Start,
+  IN  UINT64                       Size
+  )
+{
+  EFI_PHYSICAL_ADDRESS  Address;
+
+  Address = Start;
+
+  //
+  // Add 4G memory address check for IA32 platform
+  // NOTE: Without page table, there is no way to use memory above 4G.
+  //
+  if (Start + Size > MAX_ADDRESS) {
+    return EFI_SUCCESS;
+  }
+
+  while (Address < (Start + Size)) {
+    CopyMem ((VOID *) (UINTN) Address, Private->MonoPattern, Private->MonoTestSize);
+    Address += Private->CoverageSpan;
+  }
+  //
+  // bug bug: we may need GCD service to make the code cache and data uncache,
+  // if GCD do not support it or return fail, then just flush the whole cache.
+  //
+  if (Private->Cpu != NULL) {
+    Private->Cpu->FlushDataCache (Private->Cpu, Start, Size, EfiCpuFlushTypeWriteBackInvalidate);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Verify the range of physical memory which covered by memory test pattern.
+
+  This function will also do not return any informatin just cause system reset,
+  because the handle error encount fatal error and disable the bad DIMMs.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  @param[in] Start    The memory range's start address.
+  @param[in] Size     The memory range's size.
+
+  @retval EFI_SUCCESS Successful verify the range of memory, no errors' location found.
+  @retval Others      The range of memory have errors contained.
+                            
+**/
+EFI_STATUS
+VerifyMemory (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         Start,
+  IN  UINT64                       Size
+  )
+{
+  EFI_PHYSICAL_ADDRESS            Address;
+  INTN                            ErrorFound;
+  EFI_MEMORY_EXTENDED_ERROR_DATA  *ExtendedErrorData;
+
+  Address           = Start;
+  ExtendedErrorData = NULL;
+
+  //
+  // Add 4G memory address check for IA32 platform
+  // NOTE: Without page table, there is no way to use memory above 4G.
+  //
+  if (Start + Size > MAX_ADDRESS) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Use the software memory test to check whether have detected miscompare
+  // error here. If there is miscompare error here then check if generic
+  // memory test driver can disable the bad DIMM.
+  //
+  while (Address < (Start + Size)) {
+    ErrorFound = CompareMemWithoutCheckArgument (
+                  (VOID *) (UINTN) (Address),
+                  Private->MonoPattern,
+                  Private->MonoTestSize
+                  );
+    if (ErrorFound != 0) {
+      //
+      // Report uncorrectable errors
+      //
+      ExtendedErrorData = AllocateZeroPool (sizeof (EFI_MEMORY_EXTENDED_ERROR_DATA));
+      if (ExtendedErrorData == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      ExtendedErrorData->DataHeader.HeaderSize  = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+      ExtendedErrorData->DataHeader.Size        = (UINT16) (sizeof (EFI_MEMORY_EXTENDED_ERROR_DATA) - sizeof (EFI_STATUS_CODE_DATA));
+      ExtendedErrorData->Granularity            = EFI_MEMORY_ERROR_DEVICE;
+      ExtendedErrorData->Operation              = EFI_MEMORY_OPERATION_READ;
+      ExtendedErrorData->Syndrome               = 0x0;
+      ExtendedErrorData->Address                = Address;
+      ExtendedErrorData->Resolution             = 0x40;
+
+      REPORT_STATUS_CODE_EX (
+          EFI_ERROR_CODE,
+          EFI_COMPUTING_UNIT_MEMORY | EFI_CU_MEMORY_EC_UNCORRECTABLE,
+          0,
+          &gEfiGenericMemTestProtocolGuid,
+          NULL,
+          (UINT8 *) ExtendedErrorData + sizeof (EFI_STATUS_CODE_DATA),
+          ExtendedErrorData->DataHeader.Size
+          ); 
+
+      return EFI_DEVICE_ERROR;
+    }
+
+    Address += Private->CoverageSpan;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the generic memory test.
+
+  @param[in]  This                The protocol instance pointer. 
+  @param[in]  Level               The coverage level of the memory test. 
+  @param[out] RequireSoftECCInit  Indicate if the memory need software ECC init. 
+
+  @retval EFI_SUCCESS         The generic memory test is initialized correctly. 
+  @retval EFI_NO_MEDIA        The system had no memory to be tested. 
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EXTENDMEM_COVERAGE_LEVEL                 Level,
+  OUT BOOLEAN                                  *RequireSoftECCInit
+  )
+{
+  EFI_STATUS                  Status;
+  GENERIC_MEMORY_TEST_PRIVATE *Private;
+  EFI_CPU_ARCH_PROTOCOL       *Cpu;
+
+  Private             = GENERIC_MEMORY_TEST_PRIVATE_FROM_THIS (This);
+  *RequireSoftECCInit = FALSE;
+
+  //
+  // This is initialize for default value, but some value may be reset base on
+  // platform memory test driver.
+  //
+  Private->CoverLevel   = Level;
+  Private->BdsBlockSize = TEST_BLOCK_SIZE;
+  Private->MonoPattern  = GenericMemoryTestMonoPattern;
+  Private->MonoTestSize = GENERIC_CACHELINE_SIZE;
+
+  //
+  // Initialize several internal link list
+  //
+  InitializeListHead (&Private->NonTestedMemRanList);
+
+  //
+  // Construct base memory range
+  //
+  ConstructBaseMemoryRange (Private);
+
+  //
+  // get the cpu arch protocol to support flash cache
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiCpuArchProtocolGuid,
+                  NULL,
+                  (VOID **) &Cpu
+                  );
+  if (!EFI_ERROR (Status)) {
+    Private->Cpu = Cpu;
+  }
+  //
+  // Create the CoverageSpan of the memory test base on the coverage level
+  //
+  switch (Private->CoverLevel) {
+  case EXTENSIVE:
+    Private->CoverageSpan = GENERIC_CACHELINE_SIZE;
+    break;
+
+  case SPARSE:
+    Private->CoverageSpan = SPARSE_SPAN_SIZE;
+    break;
+
+  //
+  // Even the BDS do not need to test any memory, but in some case it
+  // still need to init ECC memory.
+  //
+  default:
+    Private->CoverageSpan = QUICK_SPAN_SIZE;
+    break;
+  }
+  //
+  // This is the first time we construct the non-tested memory range, if no
+  // extended memory found, we know the system have not any extended memory
+  // need to be test
+  //
+  Status = ConstructNonTestedMemoryRange (Private);
+  if (Status == EFI_NOT_FOUND) {
+    return EFI_NO_MEDIA;
+  }
+  //
+  // ready to perform the R/W/V memory test
+  //
+  mTestedSystemMemory = Private->BaseMemorySize;
+  mCurrentLink        = Private->NonTestedMemRanList.ForwardLink;
+  mCurrentRange       = NONTESTED_MEMORY_RANGE_FROM_LINK (mCurrentLink);
+  mCurrentAddress     = mCurrentRange->StartAddress;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform the memory test.
+
+  @param[in]  This              The protocol instance pointer. 
+  @param[out] TestedMemorySize  Return the tested extended memory size. 
+  @param[out] TotalMemorySize   Return the whole system physical memory size. 
+                                The total memory size does not include memory in a slot with a disabled DIMM.  
+  @param[out] ErrorOut          TRUE if the memory error occured.
+  @param[in]  IfTestAbort       Indicates that the user pressed "ESC" to skip the memory test. 
+
+  @retval EFI_SUCCESS         One block of memory passed the test.
+  @retval EFI_NOT_FOUND       All memory blocks have already been tested.
+  @retval EFI_DEVICE_ERROR    Memory device error occured, and no agent can handle it.
+
+**/
+EFI_STATUS
+EFIAPI
+GenPerformMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  OUT UINT64                                   *TestedMemorySize,
+  OUT UINT64                                   *TotalMemorySize,
+  OUT BOOLEAN                                  *ErrorOut,
+  IN BOOLEAN                                   TestAbort
+  )
+{
+  EFI_STATUS                      Status;
+  GENERIC_MEMORY_TEST_PRIVATE     *Private;
+  EFI_MEMORY_RANGE_EXTENDED_DATA  *RangeData;
+  UINT64                          BlockBoundary;
+
+  Private       = GENERIC_MEMORY_TEST_PRIVATE_FROM_THIS (This);
+  *ErrorOut     = FALSE;
+  RangeData     = NULL;
+  BlockBoundary = 0;
+
+  //
+  // In extensive mode the boundary of "mCurrentRange->Length" may will lost
+  // some range that is not Private->BdsBlockSize size boundry, so need
+  // the software mechanism to confirm all memory location be covered.
+  //
+  if (mCurrentAddress < (mCurrentRange->StartAddress + mCurrentRange->Length)) {
+    if ((mCurrentAddress + Private->BdsBlockSize) <= (mCurrentRange->StartAddress + mCurrentRange->Length)) {
+      BlockBoundary = Private->BdsBlockSize;
+    } else {
+      BlockBoundary = mCurrentRange->StartAddress + mCurrentRange->Length - mCurrentAddress;
+    }
+    //
+    // If TestAbort is true, means user cancel the memory test
+    //
+    if (!TestAbort && Private->CoverLevel != IGNORE) {
+      //
+      // Report status code of every memory range
+      //
+      RangeData                         = AllocateZeroPool (sizeof (EFI_MEMORY_RANGE_EXTENDED_DATA));
+      if (RangeData == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+      RangeData->DataHeader.HeaderSize  = (UINT16) sizeof (EFI_STATUS_CODE_DATA);
+      RangeData->DataHeader.Size        = (UINT16) (sizeof (EFI_MEMORY_RANGE_EXTENDED_DATA) - sizeof (EFI_STATUS_CODE_DATA));
+      RangeData->Start                  = mCurrentAddress;
+      RangeData->Length                 = BlockBoundary;
+
+      REPORT_STATUS_CODE_EX (
+          EFI_PROGRESS_CODE,
+          EFI_COMPUTING_UNIT_MEMORY | EFI_CU_MEMORY_PC_TEST,
+          0,
+          &gEfiGenericMemTestProtocolGuid,
+          NULL,
+          (UINT8 *) RangeData + sizeof (EFI_STATUS_CODE_DATA),
+          RangeData->DataHeader.Size
+          );
+
+      //
+      // The software memory test (R/W/V) perform here. It will detect the
+      // memory mis-compare error.
+      //
+      WriteMemory (Private, mCurrentAddress, BlockBoundary);
+
+      Status = VerifyMemory (Private, mCurrentAddress, BlockBoundary);
+      if (EFI_ERROR (Status)) {
+        //
+        // If perform here, means there is mis-compare error, and no agent can
+        // handle it, so we return to BDS EFI_DEVICE_ERROR.
+        //
+        *ErrorOut = TRUE;
+        return EFI_DEVICE_ERROR;
+      }
+    }
+
+    mTestedSystemMemory += BlockBoundary;
+    *TestedMemorySize = mTestedSystemMemory;
+
+    //
+    // If the memory test restart after the platform driver disable dimms,
+    // the NonTestSystemMemory may be changed, but the base memory size will
+    // not changed, so we can get the current total memory size.
+    //
+    *TotalMemorySize = Private->BaseMemorySize + mNonTestedSystemMemory;
+
+    //
+    // Update the current test address pointing to next BDS BLOCK
+    //
+    mCurrentAddress += Private->BdsBlockSize;
+
+    return EFI_SUCCESS;
+  }
+  //
+  // Change to next non tested memory range
+  //
+  mCurrentLink = mCurrentLink->ForwardLink;
+  if (mCurrentLink != &Private->NonTestedMemRanList) {
+    mCurrentRange   = NONTESTED_MEMORY_RANGE_FROM_LINK (mCurrentLink);
+    mCurrentAddress = mCurrentRange->StartAddress;
+    return EFI_SUCCESS;
+  } else {
+    //
+    // Here means all the memory test have finished
+    //
+    *TestedMemorySize = mTestedSystemMemory;
+    *TotalMemorySize  = Private->BaseMemorySize + mNonTestedSystemMemory;
+    return EFI_NOT_FOUND;
+  }
+
+}
+
+/**
+  Finish the memory test.
+
+  @param[in] This             The protocol instance pointer. 
+
+  @retval EFI_SUCCESS         Success. All resources used in the memory test are freed.
+
+**/
+EFI_STATUS
+EFIAPI
+GenMemoryTestFinished (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL *This
+  )
+{
+  EFI_STATUS                  Status;
+  GENERIC_MEMORY_TEST_PRIVATE *Private;
+
+  Private = GENERIC_MEMORY_TEST_PRIVATE_FROM_THIS (This);
+
+  //
+  // Perform Data and Address line test
+  //
+  Status = PerformAddressDataLineTest (Private);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Add the non tested memory range to system memory map through GCD service
+  //
+  UpdateMemoryMap (Private);
+
+  //
+  // we need to free all the memory allocate
+  //
+  DestroyLinkList (Private);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Provides the capability to test the compatible range used by some special drivers.
+
+  @param[in]  This              The protocol instance pointer. 
+  @param[in]  StartAddress      The start address of the compatible memory range that
+                                must be below 16M.
+  @param[in]  Length            The compatible memory range's length. 
+  
+  @retval EFI_SUCCESS           The compatible memory range pass the memory test. 
+  @retval EFI_INVALID_PARAMETER The compatible memory range are not below Low 16M.
+
+**/
+EFI_STATUS
+EFIAPI
+GenCompatibleRangeTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL         *This,
+  IN EFI_PHYSICAL_ADDRESS                     StartAddress,
+  IN UINT64                                   Length
+  )
+{
+  EFI_STATUS                      Status;
+  GENERIC_MEMORY_TEST_PRIVATE     *Private;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR Descriptor;
+  EFI_PHYSICAL_ADDRESS            CurrentBase;
+  UINT64                          CurrentLength;
+
+  Private = GENERIC_MEMORY_TEST_PRIVATE_FROM_THIS (This);
+
+  //
+  // Check if the parameter is below 16MB
+  //
+  if (StartAddress + Length > 0x1000000) {
+    return EFI_INVALID_PARAMETER;
+  }
+  CurrentBase = StartAddress;
+  do {
+    //
+    // Check the required memory range status; if the required memory range span
+    // the different GCD memory descriptor, it may be cause different action.
+    //
+    Status = gDS->GetMemorySpaceDescriptor (
+                    CurrentBase,
+                    &Descriptor
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (Descriptor.GcdMemoryType == EfiGcdMemoryTypeReserved &&
+        (Descriptor.Capabilities & (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED)) ==
+          (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED)
+          ) {
+      CurrentLength = Descriptor.BaseAddress + Descriptor.Length - CurrentBase;
+      if (CurrentBase + CurrentLength > StartAddress + Length) {
+        CurrentLength = StartAddress + Length - CurrentBase;
+      }
+      Status = DirectRangeTest (
+                 Private,
+                 CurrentBase,
+                 CurrentLength,
+                 Descriptor.Capabilities
+                 );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+    CurrentBase = Descriptor.BaseAddress + Descriptor.Length;
+  } while (CurrentBase < StartAddress + Length);
+  //
+  // Here means the required range already be tested, so just return success.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform the address line walking ones test.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  
+  @retval EFI_SUCCESS          Successful finished walking ones test. 
+  @retval EFI_OUT_OF_RESOURCE  Could not get resource in base memory. 
+  @retval EFI_ACCESS_DENIED    Code may can not run here because if walking one test
+                               failed, system may be already halt.
+
+**/
+EFI_STATUS
+PerformAddressDataLineTest (
+  IN  GENERIC_MEMORY_TEST_PRIVATE      *Private
+  )
+{
+  LIST_ENTRY              *ExtendedLink;
+  NONTESTED_MEMORY_RANGE  *ExtendedRange;
+  BOOLEAN                 InExtendedRange;
+  EFI_PHYSICAL_ADDRESS    TestAddress;
+
+  //
+  // Light version no data line test, only perform the address line test
+  //
+  TestAddress = (EFI_PHYSICAL_ADDRESS) 0x1;
+  while (TestAddress < MAX_ADDRESS && TestAddress > 0) {
+    //
+    // only test if the address falls in the enabled range
+    //
+    InExtendedRange = FALSE;
+    ExtendedLink    = Private->NonTestedMemRanList.BackLink;
+    while (ExtendedLink != &Private->NonTestedMemRanList) {
+      ExtendedRange = NONTESTED_MEMORY_RANGE_FROM_LINK (ExtendedLink);
+      if ((TestAddress >= ExtendedRange->StartAddress) &&
+          (TestAddress < (ExtendedRange->StartAddress + ExtendedRange->Length))
+          ) {
+        InExtendedRange = TRUE;
+      }
+
+      ExtendedLink = ExtendedLink->BackLink;
+    }
+
+    if (InExtendedRange) {
+      *(EFI_PHYSICAL_ADDRESS *) (UINTN) TestAddress = TestAddress;
+      Private->Cpu->FlushDataCache (Private->Cpu, TestAddress, 1, EfiCpuFlushTypeWriteBackInvalidate);
+      if (*(EFI_PHYSICAL_ADDRESS *) (UINTN) TestAddress != TestAddress) {
+        return EFI_ACCESS_DENIED;
+      }
+    }
+
+    TestAddress = LShiftU64 (TestAddress, 1);
+  }
+
+  return EFI_SUCCESS;
+}
+//
+// Driver entry here
+//
+GENERIC_MEMORY_TEST_PRIVATE mGenericMemoryTestPrivate = {
+  EFI_GENERIC_MEMORY_TEST_PRIVATE_SIGNATURE,
+  NULL,
+  NULL,
+  {
+    InitializeMemoryTest,
+    GenPerformMemoryTest,
+    GenMemoryTestFinished,
+    GenCompatibleRangeTest
+  },
+  (EXTENDMEM_COVERAGE_LEVEL) 0,
+  0,
+  0,
+  NULL,
+  0,
+  0,
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  The generic memory test driver's entry point.
+
+  It initializes private data to default value.
+
+  @param[in] ImageHandle  The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable  A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS     The entry point is executed successfully.
+  @retval EFI_NOT_FOUND   Can't find HandOff Hob in HobList.
+  @retval other           Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+GenericMemoryTestEntryPoint (
+  IN  EFI_HANDLE           ImageHandle,
+  IN  EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+  VOID                  *HobList;
+  EFI_BOOT_MODE         BootMode;
+  EFI_PEI_HOB_POINTERS  Hob;
+
+  //
+  // Use the generic pattern to test compatible memory range
+  //
+  mGenericMemoryTestPrivate.MonoPattern   = GenericMemoryTestMonoPattern;
+  mGenericMemoryTestPrivate.MonoTestSize  = GENERIC_CACHELINE_SIZE;
+
+  //
+  // Get the platform boot mode
+  //
+  HobList = GetHobList ();
+
+  Hob.Raw = HobList;
+  if (Hob.Header->HobType != EFI_HOB_TYPE_HANDOFF) {
+    return EFI_NOT_FOUND;
+  }
+
+  BootMode = Hob.HandoffInformationTable->BootMode;
+
+  //
+  // Get the platform boot mode and create the default memory test coverage
+  // level and span size for compatible memory test using
+  //
+  switch (BootMode) {
+  case BOOT_WITH_FULL_CONFIGURATION:
+  case BOOT_WITH_DEFAULT_SETTINGS:
+    mGenericMemoryTestPrivate.CoverageSpan = SPARSE_SPAN_SIZE;
+    break;
+
+  case BOOT_WITH_FULL_CONFIGURATION_PLUS_DIAGNOSTICS:
+    mGenericMemoryTestPrivate.CoverageSpan = GENERIC_CACHELINE_SIZE;
+    break;
+
+  default:
+    mGenericMemoryTestPrivate.CoverageSpan = QUICK_SPAN_SIZE;
+    break;
+  }
+  //
+  // Install the protocol
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mGenericMemoryTestPrivate.Handle,
+                  &gEfiGenericMemTestProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mGenericMemoryTestPrivate.GenericMemoryTest
+                  );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.h b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.h
new file mode 100644
index 0000000000..391c8a4c81
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/GenericMemoryTestDxe/LightMemoryTest.h
@@ -0,0 +1,342 @@
+/** @file
+  The generic memory test driver definition
+
+  Copyright (c) 2006 - 2010, 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.
+
+**/
+
+#ifndef _GENERIC_MEMORY_TEST_H_
+#define _GENERIC_MEMORY_TEST_H_
+
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Protocol/GenericMemoryTest.h>
+#include <Protocol/Cpu.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/HobLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+//
+// Some global define
+//
+#define GENERIC_CACHELINE_SIZE  0x40
+
+//
+// attributes for reserved memory before it is promoted to system memory
+//
+#define EFI_MEMORY_PRESENT      0x0100000000000000ULL
+#define EFI_MEMORY_INITIALIZED  0x0200000000000000ULL
+#define EFI_MEMORY_TESTED       0x0400000000000000ULL
+
+//
+// The SPARSE_SPAN_SIZE size can not small then the MonoTestSize
+//
+#define TEST_BLOCK_SIZE   0x2000000
+#define QUICK_SPAN_SIZE   (TEST_BLOCK_SIZE >> 2)
+#define SPARSE_SPAN_SIZE  (TEST_BLOCK_SIZE >> 4)
+
+//
+// This structure records every nontested memory range parsed through GCD
+// service.
+//
+#define EFI_NONTESTED_MEMORY_RANGE_SIGNATURE  SIGNATURE_32 ('N', 'T', 'M', 'E')
+
+typedef struct {
+  UINTN                 Signature;
+  LIST_ENTRY        Link;
+  EFI_PHYSICAL_ADDRESS  StartAddress;
+  UINT64                Length;
+  UINT64                Capabilities;
+  BOOLEAN               Above4G;
+  BOOLEAN               AlreadyMapped;
+} NONTESTED_MEMORY_RANGE;
+
+#define NONTESTED_MEMORY_RANGE_FROM_LINK(link) \
+  CR ( \
+  link, \
+  NONTESTED_MEMORY_RANGE, \
+  Link, \
+  EFI_NONTESTED_MEMORY_RANGE_SIGNATURE \
+  )
+
+//
+// This is the memory test driver's structure definition
+//
+#define EFI_GENERIC_MEMORY_TEST_PRIVATE_SIGNATURE SIGNATURE_32 ('G', 'E', 'M', 'T')
+
+typedef struct {
+
+  UINTN                             Signature;
+  EFI_HANDLE                        Handle;
+
+  //
+  // Cpu arch protocol's pointer
+  //
+  EFI_CPU_ARCH_PROTOCOL             *Cpu;
+
+  //
+  // generic memory test driver's protocol
+  //
+  EFI_GENERIC_MEMORY_TEST_PROTOCOL  GenericMemoryTest;
+
+  //
+  // memory test covered spans
+  //
+  EXTENDMEM_COVERAGE_LEVEL          CoverLevel;
+  UINTN                             CoverageSpan;
+  UINT64                            BdsBlockSize;
+
+  //
+  // the memory test pattern and size every time R/W/V memory
+  //
+  VOID                              *MonoPattern;
+  UINTN                             MonoTestSize;
+
+  //
+  // base memory's size which tested in PEI phase
+  //
+  UINT64                            BaseMemorySize;
+
+  //
+  // memory range list
+  //
+  LIST_ENTRY                    NonTestedMemRanList;
+
+} GENERIC_MEMORY_TEST_PRIVATE;
+
+#define GENERIC_MEMORY_TEST_PRIVATE_FROM_THIS(a) \
+  CR ( \
+  a, \
+  GENERIC_MEMORY_TEST_PRIVATE, \
+  GenericMemoryTest, \
+  EFI_GENERIC_MEMORY_TEST_PRIVATE_SIGNATURE \
+  )
+
+//
+// Function Prototypes
+//
+
+/**
+  Construct the system base memory range through GCD service.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS          Successful construct the base memory range through GCD service.
+  @retval EFI_OUT_OF_RESOURCE  Could not allocate needed resource from base memory.
+  @retval Others               Failed to construct base memory range through GCD service.
+                            
+**/
+EFI_STATUS
+ConstructBaseMemoryRange (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  );
+
+/**
+  Construct the system non-tested memory range through GCD service.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS          Successful construct the non-tested memory range through GCD service.
+  @retval EFI_OUT_OF_RESOURCE  Could not allocate needed resource from base memory.
+  @retval Others               Failed to construct non-tested memory range through GCD service.
+                            
+**/
+EFI_STATUS
+ConstructNonTestedMemoryRange (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  );
+
+/**
+  Perform the address line walking ones test.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  
+  @retval EFI_SUCCESS          Successful finished walking ones test. 
+  @retval EFI_OUT_OF_RESOURCE  Could not get resource in base memory. 
+  @retval EFI_ACCESS_DENIED    Code may can not run here because if walking one test
+                               failed, system may be already halt.
+
+**/
+EFI_STATUS
+PerformAddressDataLineTest (
+  IN  GENERIC_MEMORY_TEST_PRIVATE      *Private
+  );
+
+/**
+  Destroy the link list base on the correspond link list type.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+                            
+**/
+VOID
+DestroyLinkList (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  );
+
+/**
+  Add the extened memory to whole system memory map.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+
+  @retval EFI_SUCCESS Successful add all the extended memory to system memory map.
+  @retval Others      Failed to add the tested extended memory.
+                            
+**/
+EFI_STATUS
+UpdateMemoryMap (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private
+  );
+
+/**
+  Write the memory test pattern into a range of physical memory.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  @param[in] Start    The memory range's start address.
+  @param[in] Size     The memory range's size.
+
+  @retval EFI_SUCCESS Successful write the test pattern into the non-tested memory.
+  @retval Others      The test pattern may not really write into the physical memory.
+                            
+**/
+EFI_STATUS
+WriteMemory (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         Start,
+  IN  UINT64                       Size
+  );
+
+/**
+  Verify the range of physical memory which covered by memory test pattern.
+
+  This function will also do not return any informatin just cause system reset,
+  because the handle error encount fatal error and disable the bad DIMMs.
+
+  @param[in] Private  Point to generic memory test driver's private data.
+  @param[in] Start    The memory range's start address.
+  @param[in] Size     The memory range's size.
+
+  @retval EFI_SUCCESS Successful verify the range of memory, no errors' location found.
+  @retval Others      The range of memory have errors contained.
+                            
+**/
+EFI_STATUS
+VerifyMemory (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         Start,
+  IN  UINT64                       Size
+  );
+
+/**
+  Test a range of the memory directly .
+
+  @param[in] Private       Point to generic memory test driver's private data.
+  @param[in] StartAddress  Starting address of the memory range to be tested.
+  @param[in] Length        Length in bytes of the memory range to be tested.
+  @param[in] Capabilities  The bit mask of attributes that the memory range supports.
+
+  @retval EFI_SUCCESS      Successful test the range of memory.
+  @retval Others           Failed to test the range of memory.
+                            
+**/
+EFI_STATUS
+DirectRangeTest (
+  IN  GENERIC_MEMORY_TEST_PRIVATE  *Private,
+  IN  EFI_PHYSICAL_ADDRESS         StartAddress,
+  IN  UINT64                       Length,
+  IN  UINT64                       Capabilities
+  );
+
+/**
+  Initialize the generic memory test.
+
+  @param[in]  This                The protocol instance pointer. 
+  @param[in]  Level               The coverage level of the memory test. 
+  @param[out] RequireSoftECCInit  Indicate if the memory need software ECC init. 
+
+  @retval EFI_SUCCESS         The generic memory test is initialized correctly. 
+  @retval EFI_NO_MEDIA        The system had no memory to be tested. 
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EXTENDMEM_COVERAGE_LEVEL                 Level,
+  OUT BOOLEAN                                  *RequireSoftECCInit
+  );
+
+/**
+  Perform the memory test.
+
+  @param[in]  This              The protocol instance pointer. 
+  @param[out] TestedMemorySize  Return the tested extended memory size. 
+  @param[out] TotalMemorySize   Return the whole system physical memory size. 
+                                The total memory size does not include memory in a slot with a disabled DIMM.  
+  @param[out] ErrorOut          TRUE if the memory error occured.
+  @param[in]  IfTestAbort       Indicates that the user pressed "ESC" to skip the memory test. 
+
+  @retval EFI_SUCCESS         One block of memory passed the test.
+  @retval EFI_NOT_FOUND       All memory blocks have already been tested.
+  @retval EFI_DEVICE_ERROR    Memory device error occured, and no agent can handle it.
+
+**/
+EFI_STATUS
+EFIAPI
+GenPerformMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  OUT UINT64                                   *TestedMemorySize,
+  OUT UINT64                                   *TotalMemorySize,
+  OUT BOOLEAN                                  *ErrorOut,
+  IN BOOLEAN                                   TestAbort
+  );
+
+/**
+  Finish the memory test.
+
+  @param[in] This             The protocol instance pointer. 
+
+  @retval EFI_SUCCESS         Success. All resources used in the memory test are freed.
+
+**/
+EFI_STATUS
+EFIAPI
+GenMemoryTestFinished (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL *This
+  );
+
+/**
+  Provides the capability to test the compatible range used by some special drivers.
+
+  @param[in]  This              The protocol instance pointer. 
+  @param[in]  StartAddress      The start address of the compatible memory range that
+                                must be below 16M.
+  @param[in]  Length            The compatible memory range's length. 
+  
+  @retval EFI_SUCCESS           The compatible memory range pass the memory test. 
+  @retval EFI_INVALID_PARAMETER The compatible memory range are not below Low 16M.
+
+**/
+EFI_STATUS
+EFIAPI
+GenCompatibleRangeTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EFI_PHYSICAL_ADDRESS                     StartAddress,
+  IN  UINT64                                   Length
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.c b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.c
new file mode 100644
index 0000000000..9c9849c776
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.c
@@ -0,0 +1,225 @@
+/** @file
+  Implementation of Generic Memory Test Protocol which does not perform real memory test.
+
+Copyright (c) 2006 - 2008, 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 "NullMemoryTest.h"
+
+UINT64                            mTestedSystemMemory = 0;
+UINT64                            mTotalSystemMemory  = 0;
+EFI_HANDLE                        mGenericMemoryTestHandle;
+
+EFI_GENERIC_MEMORY_TEST_PROTOCOL  mGenericMemoryTest = {
+  InitializeMemoryTest,
+  GenPerformMemoryTest,
+  GenMemoryTestFinished,
+  GenCompatibleRangeTest
+};
+
+/**
+  Entry point of the NULL memory test driver.
+  
+  This function is the entry point of the NULL memory test driver.
+  It simply installs the Generic Memory Test Protocol.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    Generic Memory Test Protocol is successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+GenericMemoryTestEntryPoint (
+  IN  EFI_HANDLE           ImageHandle,
+  IN  EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->InstallProtocolInterface (
+                  &mGenericMemoryTestHandle,
+                  &gEfiGenericMemTestProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mGenericMemoryTest
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the generic memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.MemoryTestInit.
+  It simply promotes untested reserved memory to system memory without real test.
+
+  @param  This                Protocol instance pointer. 
+  @param  Level               The coverage level of the memory test. 
+  @param  RequireSoftECCInit  Indicate if the memory need software ECC init. 
+
+  @retval EFI_SUCCESS         The generic memory test initialized correctly. 
+  @retval EFI_NO_MEDIA        There is not any non-tested memory found, in this 
+                              function if not any non-tesed memory found means  
+                              that the memory test driver have not detect any 
+                              non-tested extended memory of current system. 
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EXTENDMEM_COVERAGE_LEVEL                 Level,
+  OUT BOOLEAN                                  *RequireSoftECCInit
+  )
+{
+  UINTN                           NumberOfDescriptors;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR *MemorySpaceMap;
+  UINTN                           Index;
+
+  gDS->GetMemorySpaceMap (&NumberOfDescriptors, &MemorySpaceMap);
+  for (Index = 0; Index < NumberOfDescriptors; Index++) {
+    if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeReserved &&
+        (MemorySpaceMap[Index].Capabilities & (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED)) ==
+          (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED)
+          ) {
+      //
+      // For those reserved memory that have not been tested, simply promote to system memory.
+      //
+      gDS->RemoveMemorySpace (
+            MemorySpaceMap[Index].BaseAddress,
+            MemorySpaceMap[Index].Length
+            );
+
+      gDS->AddMemorySpace (
+            EfiGcdMemoryTypeSystemMemory,
+            MemorySpaceMap[Index].BaseAddress,
+            MemorySpaceMap[Index].Length,
+            MemorySpaceMap[Index].Capabilities &~
+            (EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED | EFI_MEMORY_RUNTIME)
+            );
+
+      mTestedSystemMemory += MemorySpaceMap[Index].Length;
+      mTotalSystemMemory += MemorySpaceMap[Index].Length;
+    } else if (MemorySpaceMap[Index].GcdMemoryType == EfiGcdMemoryTypeSystemMemory) {
+      mTotalSystemMemory += MemorySpaceMap[Index].Length;
+    }
+  }
+
+  FreePool (MemorySpaceMap);
+
+  *RequireSoftECCInit = FALSE;
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform the memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.PerformMemoryTest.
+  It simply returns EFI_NOT_FOUND.
+
+  @param  This                Protocol instance pointer. 
+  @param  TestedMemorySize    Return the tested extended memory size. 
+  @param  TotalMemorySize     Return the whole system physical memory size, this  
+                              value may be changed if in some case some error  
+                              DIMMs be disabled. 
+  @param  ErrorOut            Any time the memory error occurs, this will be 
+                              TRUE. 
+  @param  IfTestAbort         Indicate if the user press "ESC" to skip the memory 
+                              test. 
+
+  @retval EFI_SUCCESS         One block of memory test ok, the block size is hide 
+                              internally. 
+  @retval EFI_NOT_FOUND       Indicate all the non-tested memory blocks have  
+                              already go through. 
+  @retval EFI_DEVICE_ERROR    Mis-compare error, and no agent can handle it
+
+**/
+EFI_STATUS
+EFIAPI
+GenPerformMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN OUT UINT64                                *TestedMemorySize,
+  OUT UINT64                                   *TotalMemorySize,
+  OUT BOOLEAN                                  *ErrorOut,
+  IN BOOLEAN                                   TestAbort
+  )
+{
+  *ErrorOut         = FALSE;
+  *TestedMemorySize = mTestedSystemMemory;
+  *TotalMemorySize  = mTotalSystemMemory;
+
+  return EFI_NOT_FOUND;
+
+}
+
+/**
+  The memory test finished.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.Finished.
+  It simply returns EFI_SUCCESS.
+
+  @param  This                Protocol instance pointer. 
+
+  @retval EFI_SUCCESS         Successful free all the generic memory test driver 
+                              allocated resource and notify to platform memory 
+                              test driver that memory test finished. 
+
+**/
+EFI_STATUS
+EFIAPI
+GenMemoryTestFinished (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL *This
+  )
+{
+  return EFI_SUCCESS;
+}
+
+/**
+  Provide capability to test compatible range which used by some special
+  driver required using memory range before BDS perform memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.CompatibleRangeTest.
+  It simply sets the memory range to system memory.
+
+  @param  This                Protocol instance pointer. 
+  @param  StartAddress        The start address of the memory range. 
+  @param  Length              The memory range's length. 
+  
+  @retval EFI_SUCCESS           The compatible memory range pass the memory test. 
+  @retval EFI_INVALID_PARAMETER The compatible memory range must be below 16M.
+
+**/
+EFI_STATUS
+EFIAPI
+GenCompatibleRangeTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EFI_PHYSICAL_ADDRESS                     StartAddress,
+  IN  UINT64                                   Length
+  )
+{
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR Descriptor;
+
+  gDS->GetMemorySpaceDescriptor (StartAddress, &Descriptor);
+
+  gDS->RemoveMemorySpace (StartAddress, Length);
+
+  gDS->AddMemorySpace (
+        EfiGcdMemoryTypeSystemMemory,
+        StartAddress,
+        Length,
+        Descriptor.Capabilities &~(EFI_MEMORY_PRESENT | EFI_MEMORY_INITIALIZED | EFI_MEMORY_TESTED | EFI_MEMORY_RUNTIME)
+        );
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.h b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.h
new file mode 100644
index 0000000000..77c18eb331
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTest.h
@@ -0,0 +1,137 @@
+/** @file
+  Include file of the NULL memory test driver.
+
+Copyright (c) 2006 - 2008, 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.
+
+**/
+
+#ifndef _NULL_MEMORY_TEST_H_
+#define _NULL_MEMORY_TEST_H_
+
+
+#include <PiDxe.h>
+
+
+#include <Protocol/GenericMemoryTest.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+//
+// Definition of memory status.
+//
+#define EFI_MEMORY_PRESENT      0x0100000000000000ULL
+#define EFI_MEMORY_INITIALIZED  0x0200000000000000ULL
+#define EFI_MEMORY_TESTED       0x0400000000000000ULL
+
+/**
+  Initialize the generic memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.MemoryTestInit.
+  It simply promotes untested reserved memory to system memory without real test.
+
+  @param  This                Protocol instance pointer. 
+  @param  Level               The coverage level of the memory test. 
+  @param  RequireSoftECCInit  Indicate if the memory need software ECC init. 
+
+  @retval EFI_SUCCESS         The generic memory test initialized correctly. 
+  @retval EFI_NO_MEDIA        There is not any non-tested memory found, in this 
+                              function if not any non-tesed memory found means  
+                              that the memory test driver have not detect any 
+                              non-tested extended memory of current system. 
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EXTENDMEM_COVERAGE_LEVEL                 Level,
+  OUT BOOLEAN                                  *RequireSoftECCInit
+  );
+
+/**
+  Perform the memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.PerformMemoryTest.
+  It simply returns EFI_NOT_FOUND.
+
+  @param  This                Protocol instance pointer. 
+  @param  TestedMemorySize    Return the tested extended memory size. 
+  @param  TotalMemorySize     Return the whole system physical memory size, this  
+                              value may be changed if in some case some error  
+                              DIMMs be disabled. 
+  @param  ErrorOut            Any time the memory error occurs, this will be 
+                              TRUE. 
+  @param  IfTestAbort         Indicate if the user press "ESC" to skip the memory 
+                              test. 
+
+  @retval EFI_SUCCESS         One block of memory test ok, the block size is hide 
+                              internally. 
+  @retval EFI_NOT_FOUND       Indicate all the non-tested memory blocks have  
+                              already go through. 
+  @retval EFI_DEVICE_ERROR    Mis-compare error, and no agent can handle it
+
+**/
+EFI_STATUS
+EFIAPI
+GenPerformMemoryTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN OUT UINT64                                *TestedMemorySize,
+  OUT UINT64                                   *TotalMemorySize,
+  OUT BOOLEAN                                  *ErrorOut,
+  IN BOOLEAN                                   TestAbort
+  );
+
+/**
+  The memory test finished.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.Finished.
+  It simply returns EFI_SUCCESS.
+
+  @param  This                Protocol instance pointer. 
+
+  @retval EFI_SUCCESS         Successful free all the generic memory test driver 
+                              allocated resource and notify to platform memory 
+                              test driver that memory test finished. 
+
+**/
+EFI_STATUS
+EFIAPI
+GenMemoryTestFinished (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL *This
+  );
+
+/**
+  Provide capability to test compatible range which used by some special
+  driver required using memory range before BDS perform memory test.
+
+  This function implements EFI_GENERIC_MEMORY_TEST_PROTOCOL.CompatibleRangeTest.
+  It simply set the memory range to system memory.
+
+  @param  This                Protocol instance pointer. 
+  @param  StartAddress        The start address of the memory range. 
+  @param  Length              The memory range's length. 
+  
+  @retval EFI_SUCCESS           The compatible memory range pass the memory test. 
+  @retval EFI_INVALID_PARAMETER The compatible memory range must be below 16M.
+
+**/
+EFI_STATUS
+EFIAPI
+GenCompatibleRangeTest (
+  IN EFI_GENERIC_MEMORY_TEST_PROTOCOL          *This,
+  IN  EFI_PHYSICAL_ADDRESS                     StartAddress,
+  IN  UINT64                                   Length
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.inf b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.inf
new file mode 100644
index 0000000000..96770e5b8d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.inf
@@ -0,0 +1,52 @@
+## @file
+#   This driver installs Generic Memory Test Protocol which does not perform real memory test.
+#  
+#  Copyright (c) 2006 - 2014, 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.
+#  
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = NullMemoryTestDxe
+  MODULE_UNI_FILE                = NullMemoryTestDxe.uni
+  FILE_GUID                      = 96B5C032-DF4C-4b6e-8232-438DCF448D0E
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = GenericMemoryTestEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  NullMemoryTest.h
+  NullMemoryTest.c
+
+[Packages]
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  DxeServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Protocols]
+  gEfiGenericMemTestProtocolGuid                ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  NullMemoryTestDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.uni b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.uni
new file mode 100644
index 0000000000..9607f257e3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.uni
diff --git a/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxeExtra.uni b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxeExtra.uni
new file mode 100644
index 0000000000..336523d8cc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Metronome/Metronome.c b/Core/MdeModulePkg/Universal/Metronome/Metronome.c
new file mode 100644
index 0000000000..c3dc0b875a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Metronome/Metronome.c
@@ -0,0 +1,125 @@
+/** @file
+  Produces the Metronome Architectural Protocol on top of Timer Library.
+
+  This is a generic implementation of the Metronome Architectural Protocol that
+  layers on top of an instance of the Timer Library.  The Timer Library provides
+  functions for nanosecond and microsecond delays.  This generic implementation
+  produces a fixed TickPeriod of 1 100ns unit, and when the WaitForTick() service
+  is called, the number of ticks passed in is converted to either nanosecond or
+  microsecond units.  If the number of ticks is small, then nanoseconds are used.
+  If the number of ticks is large, then microseconds are used.  This prevents
+  overflows that could occur for long delays if only nanoseconds were used and also
+  provides the greatest accuracy for small delays. 
+
+Copyright (c) 2008, 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 "Metronome.h"
+
+//
+// Handle for the Metronome Architectural Protocol instance produced by this driver
+//
+EFI_HANDLE  mMetronomeHandle = NULL;
+
+//
+// The Metronome Architectural Protocol instance produced by this driver
+//
+EFI_METRONOME_ARCH_PROTOCOL mMetronome = {
+  WaitForTick,
+  1  // TickPeriod = 1*100 ns units
+};
+
+/**
+  Waits for the specified number of ticks.
+  
+  This function implements EFI_METRONOME_ARCH_PROTOCOL.WaitForTick().
+  The WaitForTick() function waits for the number of ticks specified by 
+  TickNumber from a known time source in the platform.  If TickNumber of 
+  ticks are detected, then EFI_SUCCESS is returned.  The actual time passed 
+  between entry of this function and the first tick is between 0 and 
+  TickPeriod 100 nS units.  If you want to guarantee that at least TickPeriod 
+  time has elapsed, wait for two ticks.  This function waits for a hardware 
+  event to determine when a tick occurs.  It is possible for interrupt 
+  processing, or exception processing to interrupt the execution of the 
+  WaitForTick() function.  Depending on the hardware source for the ticks, it 
+  is possible for a tick to be missed.  This function cannot guarantee that 
+  ticks will not be missed.  If a timeout occurs waiting for the specified 
+  number of ticks, then EFI_TIMEOUT is returned.
+
+  @param  This             The EFI_METRONOME_ARCH_PROTOCOL instance.
+  @param  TickNumber       Number of ticks to wait.
+
+  @retval EFI_SUCCESS      The wait for the number of ticks specified by TickNumber
+                           succeeded.
+  @retval EFI_TIMEOUT      A timeout occurred waiting for the specified number of ticks.
+
+**/
+EFI_STATUS
+EFIAPI
+WaitForTick (
+  IN EFI_METRONOME_ARCH_PROTOCOL  *This,
+  IN UINT32                       TickNumber
+  )
+{
+  //
+  // Check the value of TickNumber, so a 32-bit overflow can be avoided
+  // when TickNumber of converted to nanosecond units
+  //
+  if (TickNumber < 10000000) {
+    //
+    // If TickNumber is small, then use NanoSecondDelay()
+    //
+    NanoSecondDelay (TickNumber * 100);
+  } else {
+    //
+    // If TickNumber is large, then use MicroSecondDelay()
+    //
+    MicroSecondDelay (TickNumber / 10);
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+  The user Entry Point for module Metronome. The user code starts with this function.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+InstallMetronome (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Make sure the Metronome Architectural Protocol is not already installed in the system
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiMetronomeArchProtocolGuid);
+
+  //
+  // Install on a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mMetronomeHandle,
+                  &gEfiMetronomeArchProtocolGuid, &mMetronome,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Metronome/Metronome.h b/Core/MdeModulePkg/Universal/Metronome/Metronome.h
new file mode 100644
index 0000000000..3556e6b342
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Metronome/Metronome.h
@@ -0,0 +1,57 @@
+/** @file
+  Include file of Metronome driver.
+
+Copyright (c) 2008, 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.
+
+**/
+
+#ifndef _METRONOME_H_
+#define _METRONOME_H_
+
+#include <PiDxe.h>
+#include <Protocol/Metronome.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/TimerLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+/**
+  Waits for the specified number of ticks.
+  
+  This function implements EFI_METRONOME_ARCH_PROTOCOL.WaitForTick().
+  The WaitForTick() function waits for the number of ticks specified by 
+  TickNumber from a known time source in the platform.  If TickNumber of 
+  ticks are detected, then EFI_SUCCESS is returned.  The actual time passed 
+  between entry of this function and the first tick is between 0 and 
+  TickPeriod 100 nS units.  If you want to guarantee that at least TickPeriod 
+  time has elapsed, wait for two ticks.  This function waits for a hardware 
+  event to determine when a tick occurs.  It is possible for interrupt 
+  processing, or exception processing to interrupt the execution of the 
+  WaitForTick() function.  Depending on the hardware source for the ticks, it 
+  is possible for a tick to be missed.  This function cannot guarantee that 
+  ticks will not be missed.  If a timeout occurs waiting for the specified 
+  number of ticks, then EFI_TIMEOUT is returned.
+
+  @param  This             The EFI_METRONOME_ARCH_PROTOCOL instance.
+  @param  TickNumber       Number of ticks to wait.
+
+  @retval EFI_SUCCESS      The wait for the number of ticks specified by TickNumber
+                           succeeded.
+  @retval EFI_TIMEOUT      A timeout occurred waiting for the specified number of ticks.
+
+**/
+EFI_STATUS
+EFIAPI
+WaitForTick (
+  IN EFI_METRONOME_ARCH_PROTOCOL  *This,
+  IN UINT32                       TickNumber
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Metronome/Metronome.inf b/Core/MdeModulePkg/Universal/Metronome/Metronome.inf
new file mode 100644
index 0000000000..a688f3cc02
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Metronome/Metronome.inf
@@ -0,0 +1,60 @@
+## @file
+# This module produces the Metronome Architectural Protocol on top of Timer Library.
+#
+# This is a generic implementation of the Metronome Architectural Protocol that
+# layers on top of an instance of the Timer Library.  The Timer Library provides
+# functions for nanosecond and microsecond delays.  This generic implementation
+# produces a fixed TickPeriod of 100ns unit, and when the WaitForTick() service
+# is called, the number of ticks passed in is converted to either nanosecond or
+# microsecond units.  If the number of ticks is small, then nanoseconds are used.
+# If the number of ticks is large, then microseconds are used.  This prevents
+# overflows that could occur for long delays if only nanoseconds were used and also
+# provides the greatest accuracy for small delays. 
+#
+# Copyright (c) 2008 - 2014, 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.             
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Metronome
+  MODULE_UNI_FILE                = Metronome.uni
+  FILE_GUID                      = C8339973-A563-4561-B858-D8476F9DEFC4
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InstallMetronome
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  TimerLib
+  DebugLib
+
+[Sources]
+  Metronome.c
+  Metronome.h
+
+[Protocols]
+  gEfiMetronomeArchProtocolGuid      ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  MetronomeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Metronome/Metronome.uni b/Core/MdeModulePkg/Universal/Metronome/Metronome.uni
new file mode 100644
index 0000000000..fa9fef9770
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Metronome/Metronome.uni
diff --git a/Core/MdeModulePkg/Universal/Metronome/MetronomeExtra.uni b/Core/MdeModulePkg/Universal/Metronome/MetronomeExtra.uni
new file mode 100644
index 0000000000..30a767db19
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Metronome/MetronomeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounter.c b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounter.c
new file mode 100644
index 0000000000..ddb0d2ed49
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounter.c
@@ -0,0 +1,274 @@
+/** @file
+  Produce the UEFI boot service GetNextMonotonicCount() and runtime service
+  GetNextHighMonotonicCount().
+
+Copyright (c) 2006 - 2011, 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 <Uefi.h>
+
+#include <Protocol/MonotonicCounter.h>
+#include <Guid/MtcVendor.h>
+
+#include <Library/BaseLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+
+//
+// The handle to install Monotonic Counter Architctural Protocol
+//
+EFI_HANDLE  mMonotonicCounterHandle = NULL;
+
+//
+// The current monotonic counter value
+//
+UINT64      mEfiMtc;
+
+//
+// Event to update the monotonic Counter's high part when low part overflows.
+//
+EFI_EVENT   mEfiMtcEvent;
+
+/**
+  Returns a monotonically increasing count for the platform.
+
+  This function returns a 64-bit value that is numerically larger then the last
+  time the function was called.
+  The platform monotonic counter is comprised of two parts: the high 32 bits
+  and the low 32 bits. The low 32-bit value is volatile and is reset to zero on
+  every system reset. It is increased by 1 on every call to GetNextMonotonicCount().
+  The high 32-bit value is nonvolatile and is increased by one on whenever the
+  system resets or the low 32-bit counter overflows.
+
+  @param  Count	                Pointer to returned value.
+
+  @retval EFI_SUCCESS           The next monotonic count was returned.
+  @retval EFI_DEVICE_ERROR      The device is not functioning properly.
+  @retval EFI_INVALID_PARAMETER Count is NULL.
+  @retval EFI_UNSUPPORTED       This function is called at runtime.
+
+**/
+EFI_STATUS
+EFIAPI
+MonotonicCounterDriverGetNextMonotonicCount (
+  OUT UINT64  *Count
+  )
+{
+  EFI_TPL OldTpl;
+
+  //
+  // Cannot be called after ExitBootServices()
+  //
+  if (EfiAtRuntime ()) {
+    return EFI_UNSUPPORTED;
+  }
+  //
+  // Check input parameters
+  //
+  if (Count == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Update the monotonic counter with a lock
+  //
+  OldTpl  = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  *Count  = mEfiMtc;
+  mEfiMtc++;
+  gBS->RestoreTPL (OldTpl);
+
+  //
+  // If the low 32-bit counter overflows (MSB bit toggled),
+  // then signal that the high part needs update now.
+  //
+  if ((((UINT32) mEfiMtc) ^ ((UINT32) *Count)) & BIT31) {
+    gBS->SignalEvent (mEfiMtcEvent);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Returns the next high 32 bits of the platform's monotonic counter.
+
+  The GetNextHighMonotonicCount() function returns the next high 32 bits
+  of the platform's monotonic counter. The platform's monotonic counter is
+  comprised of two 32 bit quantities:  the high 32 bits and the low 32 bits.
+  During boot service time the low 32 bit value is volatile:  it is reset to
+  zero on every system reset and is increased by 1 on every call to GetNextMonotonicCount().
+  The high 32 bit value is non-volatile and is increased by 1 whenever the system resets
+  or whenever the low 32 bit count [returned by GetNextMonoticCount()] overflows.
+  The GetNextMonotonicCount() function is only available at boot services time.
+  If the operating system wishes to extend the platform monotonic counter to runtime,
+  it may do so by utilizing GetNextHighMonotonicCount().  To do this, before calling
+  ExitBootServices() the operating system would call GetNextMonotonicCount() to obtain
+  the current platform monotonic count.  The operating system would then provide an
+  interface that returns the next count by:
+    Adding 1 to the last count.
+    Before the lower 32 bits of the count overflows, call GetNextHighMonotonicCount().
+    This will increase the high 32 bits of the platform's non-volatile portion of the monotonic
+    count by 1.
+
+  This function may only be called at Runtime.
+
+  @param  HighCount	            Pointer to returned value.
+
+  @retval EFI_SUCCESS           The next high monotonic count was returned.
+  @retval EFI_INVALID_PARAMETER HighCount is NULL.
+  @retval EFI_DEVICE_ERROR      The variable could not be saved due to a hardware failure.
+  @retval EFI_OUT_OF_RESOURCES  If variable service reports that not enough storage
+                                is available to hold the variable and its data.
+
+**/
+EFI_STATUS
+EFIAPI
+MonotonicCounterDriverGetNextHighMonotonicCount (
+  OUT UINT32  *HighCount
+  )
+{
+  EFI_TPL     OldTpl;
+
+  //
+  // Check input parameters
+  //
+  if (HighCount == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!EfiAtRuntime ()) {
+    //
+    // Use a lock if called before ExitBootServices()
+    //
+    OldTpl      = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+    *HighCount  = (UINT32) RShiftU64 (mEfiMtc, 32) + 1;
+    mEfiMtc     = LShiftU64 (*HighCount, 32);
+    gBS->RestoreTPL (OldTpl);
+  } else {
+    *HighCount  = (UINT32) RShiftU64 (mEfiMtc, 32) + 1;
+    mEfiMtc     = LShiftU64 (*HighCount, 32);
+  }
+  //
+  // Update the NV variable to match the new high part
+  //
+  return EfiSetVariable (
+           MTC_VARIABLE_NAME,
+           &gMtcVendorGuid,
+           EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+           sizeof (UINT32),
+           HighCount
+           );
+
+}
+
+/**
+  Monotonic counter event handler.  This handler updates the high part of monotonic counter.
+
+  @param Event           The event to handle.
+  @param Context         The event context.
+
+**/
+VOID
+EFIAPI
+EfiMtcEventHandler (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  UINT32  HighCount;
+
+  MonotonicCounterDriverGetNextHighMonotonicCount (&HighCount);
+}
+
+/**
+  Entry point of monotonic counter driver.
+
+  @param  ImageHandle   The image handle of this driver.
+  @param  SystemTable   The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS   The initialization is successful.
+
+**/
+EFI_STATUS
+EFIAPI
+MonotonicCounterDriverInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      HighCount;
+  UINTN       BufferSize;
+
+  //
+  // Make sure the Monotonic Counter Architectural Protocol has not been installed in the system yet.
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiMonotonicCounterArchProtocolGuid);
+
+  //
+  // Initialize event to handle low-part overflow
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  EfiMtcEventHandler,
+                  NULL,
+                  &mEfiMtcEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Read the last high part
+  //
+  BufferSize = sizeof (UINT32);
+  Status = EfiGetVariable (
+             MTC_VARIABLE_NAME,
+             &gMtcVendorGuid,
+             NULL,
+             &BufferSize,
+             &HighCount
+             );
+  if (EFI_ERROR (Status)) {
+    HighCount = 0;
+  }
+  //
+  // Set the current value
+  //
+  mEfiMtc = LShiftU64 (HighCount, 32);
+
+  //
+  // Increment the upper 32 bits for this boot
+  // Continue even if it fails.  It will only fail if the variable services are
+  // not functional.
+  //
+  MonotonicCounterDriverGetNextHighMonotonicCount (&HighCount);
+
+  //
+  // Fill in the EFI Boot Services and EFI Runtime Services Monotonic Counter Fields
+  //
+  gBS->GetNextMonotonicCount      = MonotonicCounterDriverGetNextMonotonicCount;
+  gRT->GetNextHighMonotonicCount  = MonotonicCounterDriverGetNextHighMonotonicCount;
+
+  //
+  // Install the Monotonic Counter Architctural Protocol onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mMonotonicCounterHandle,
+                  &gEfiMonotonicCounterArchProtocolGuid,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.inf b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.inf
new file mode 100644
index 0000000000..e44e268014
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.inf
@@ -0,0 +1,60 @@
+## @file
+# This module produces the UEFI boot service GetNextMonotonicCount() and runtime service GetNextHighMonotonicCount().
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = MonotonicCounterRuntimeDxe
+  MODULE_UNI_FILE                = MonotonicCounterRuntimeDxe.uni
+  FILE_GUID                      = AD608272-D07F-4964-801E-7BD3B7888652
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = MonotonicCounterDriverInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  MonotonicCounter.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiRuntimeServicesTableLib
+  UefiBootServicesTableLib
+  DebugLib
+  UefiRuntimeLib
+  UefiDriverEntryPoint
+  BaseLib
+
+[Guids]
+  ## PRODUCES ## Variable:L"MTC"
+  ## CONSUMES ## Variable:L"MTC"
+  gMtcVendorGuid
+
+[Protocols]
+  gEfiMonotonicCounterArchProtocolGuid  ## PRODUCES
+
+
+[Depex]
+  gEfiVariableArchProtocolGuid AND gEfiVariableWriteArchProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  MonotonicCounterRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.uni b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.uni
new file mode 100644
index 0000000000..032a1dd221
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..851afdd2c8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/MonotonicCounterRuntimeDxe/MonotonicCounterRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.c b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.c
new file mode 100644
index 0000000000..81ddd62634
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.c
@@ -0,0 +1,817 @@
+/** @file
+  ARP driver functions.
+  
+Copyright (c) 2006 - 2012, 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<BR>
+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 "ArpDriver.h"
+#include "ArpImpl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gArpDriverBinding = {
+  ArpDriverBindingSupported,
+  ArpDriverBindingStart,
+  ArpDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+
+/**
+  Create and initialize the arp service context data.
+
+  @param[in]       ImageHandle       The image handle representing the loaded driver
+                                     image.
+  @param[in]       ControllerHandle  The controller handle the driver binds to.
+  @param[in, out]  ArpService        Pointer to the buffer containing the arp service
+                                     context data.
+
+  @retval EFI_SUCCESS                The arp service context is initialized.
+  
+  @retval EFI_UNSUPPORTED            The underlayer Snp mode type is not ethernet.
+                                     Failed to initialize the service context.
+  @retval other                      Failed to initialize the arp service context.
+
+**/
+EFI_STATUS
+ArpCreateService (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_HANDLE        ControllerHandle,
+  IN OUT ARP_SERVICE_DATA  *ArpService
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (ArpService != NULL);
+
+  ArpService->Signature = ARP_SERVICE_DATA_SIGNATURE;
+
+  //
+  // Init the lists.
+  //
+  InitializeListHead (&ArpService->ChildrenList);
+  InitializeListHead (&ArpService->PendingRequestTable);
+  InitializeListHead (&ArpService->DeniedCacheTable);
+  InitializeListHead (&ArpService->ResolvedCacheTable);
+
+  //
+  // Init the servicebinding protocol members.
+  //
+  ArpService->ServiceBinding.CreateChild  = ArpServiceBindingCreateChild;
+  ArpService->ServiceBinding.DestroyChild = ArpServiceBindingDestroyChild;
+
+  //
+  // Save the handles.
+  //
+  ArpService->ImageHandle      = ImageHandle;
+  ArpService->ControllerHandle = ControllerHandle;
+
+  //
+  // Create a MNP child instance.
+  //
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             ImageHandle,
+             &gEfiManagedNetworkServiceBindingProtocolGuid,
+             &ArpService->MnpChildHandle
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the MNP protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  ArpService->MnpChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **)&ArpService->Mnp,
+                  ImageHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ERROR_EXIT;
+  }
+
+  //
+  // Get the underlayer Snp mode data.
+  //
+  Status = ArpService->Mnp->GetModeData (ArpService->Mnp, NULL, &ArpService->SnpMode);
+  if ((Status != EFI_NOT_STARTED) && EFI_ERROR (Status)) {
+    goto ERROR_EXIT;
+  }
+
+  if (ArpService->SnpMode.IfType != NET_IFTYPE_ETHERNET) {
+    //
+    // Only support the ethernet.
+    //
+    Status = EFI_UNSUPPORTED;
+    goto ERROR_EXIT;
+  }
+
+  //
+  // Set the Mnp config parameters.
+  //
+  ArpService->MnpConfigData.ReceivedQueueTimeoutValue = 0;
+  ArpService->MnpConfigData.TransmitQueueTimeoutValue = 0;
+  ArpService->MnpConfigData.ProtocolTypeFilter        = ARP_ETHER_PROTO_TYPE;
+  ArpService->MnpConfigData.EnableUnicastReceive      = TRUE;
+  ArpService->MnpConfigData.EnableMulticastReceive    = FALSE;
+  ArpService->MnpConfigData.EnableBroadcastReceive    = TRUE;
+  ArpService->MnpConfigData.EnablePromiscuousReceive  = FALSE;
+  ArpService->MnpConfigData.FlushQueuesOnReset        = TRUE;
+  ArpService->MnpConfigData.EnableReceiveTimestamps   = FALSE;
+  ArpService->MnpConfigData.DisableBackgroundPolling  = FALSE;
+
+  //
+  // Configure the Mnp child.
+  //
+  Status = ArpService->Mnp->Configure (ArpService->Mnp, &ArpService->MnpConfigData);
+  if (EFI_ERROR (Status)) {
+    goto ERROR_EXIT;
+  }
+
+  //
+  // Create the event used in the RxToken.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ArpOnFrameRcvd,
+                  ArpService,
+                  &ArpService->RxToken.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ERROR_EXIT;
+  }
+
+  //
+  // Create the Arp heartbeat timer.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  ArpTimerHandler,
+                  ArpService,
+                  &ArpService->PeriodicTimer
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ERROR_EXIT;
+  }
+
+  //
+  // Start the heartbeat timer.
+  //
+  Status = gBS->SetTimer (
+                  ArpService->PeriodicTimer,
+                  TimerPeriodic,
+                  ARP_PERIODIC_TIMER_INTERVAL
+                  );
+
+ERROR_EXIT:
+
+  return Status;
+}
+
+
+/**
+  Clean the arp service context data.
+
+  @param[in, out]  ArpService        Pointer to the buffer containing the arp service
+                                     context data.
+
+  @return None.
+
+**/
+VOID
+ArpCleanService (
+  IN OUT ARP_SERVICE_DATA  *ArpService
+  )
+{
+  NET_CHECK_SIGNATURE (ArpService, ARP_SERVICE_DATA_SIGNATURE);
+
+  if (ArpService->PeriodicTimer != NULL) {
+    //
+    // Cancle and close the PeriodicTimer.
+    //
+    gBS->SetTimer (ArpService->PeriodicTimer, TimerCancel, 0);
+    gBS->CloseEvent (ArpService->PeriodicTimer);
+  }
+
+  if (ArpService->RxToken.Event != NULL) {
+    //
+    // Cancle the RxToken and close the event in the RxToken.
+    //
+    ArpService->Mnp->Cancel (ArpService->Mnp, NULL);
+    gBS->CloseEvent (ArpService->RxToken.Event);
+  }
+
+  if (ArpService->Mnp != NULL) {
+    //
+    // Reset the Mnp child and close the Mnp protocol.
+    //
+    ArpService->Mnp->Configure (ArpService->Mnp, NULL);
+    gBS->CloseProtocol (
+           ArpService->MnpChildHandle,
+           &gEfiManagedNetworkProtocolGuid,
+           ArpService->ImageHandle,
+           ArpService->ControllerHandle
+           );
+  }
+
+  if (ArpService->MnpChildHandle != NULL) {
+    //
+    // Destroy the mnp child.
+    //
+    NetLibDestroyServiceChild(
+      ArpService->ControllerHandle,
+      ArpService->ImageHandle,
+      &gEfiManagedNetworkServiceBindingProtocolGuid,
+      ArpService->MnpChildHandle
+      );
+  }
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDestroyChildEntryInHandleBuffer (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  ARP_INSTANCE_DATA             *Instance;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = NET_LIST_USER_STRUCT_S (Entry, ARP_INSTANCE_DATA, List, ARP_INSTANCE_DATA_SIGNATURE);
+  ServiceBinding    = (EFI_SERVICE_BINDING_PROTOCOL *) Context;
+
+  return ServiceBinding->DestroyChild (ServiceBinding, Instance->Handle);
+}
+
+/**
+  Tests to see if this driver supports a given controller. 
+  
+  If a child device is provided, it further tests to see if this driver supports 
+  creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, 
+                                   and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver 
+                                   specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed 
+                                   by the driver specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by 
+                                   a different driver or an application that 
+                                   requires exclusive acces. Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the 
+                                   driver specified by This.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Test to see if Arp SB is already installed.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiArpServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (Status == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Test to see if MNP SB is installed.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle. 
+  
+  The Start() function is designed to be invoked from the EFI boot service ConnectController(). 
+  As a result, much of the error checking on the parameters to Start() has been 
+  moved into this common boot service. It is legal to call Start() from other locations, 
+  but the following calling restrictions must be followed or the system behavior 
+  will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally 
+     aligned EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified 
+     by This must have been called with the same calling parameters, and Supported() 
+     must have returned EFI_SUCCESS.  
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, 
+                                   and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.
+                                   Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of 
+                                   resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  ARP_SERVICE_DATA  *ArpService;
+
+  //
+  // Allocate a zero pool for ArpService.
+  //
+  ArpService = AllocateZeroPool (sizeof(ARP_SERVICE_DATA));
+  if (ArpService == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Initialize the arp service context data.
+  //
+  Status = ArpCreateService (This->DriverBindingHandle, ControllerHandle, ArpService);
+  if (EFI_ERROR (Status)) {
+    goto ERROR;
+  }
+
+  //
+  // Install the ARP service binding protocol.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiArpServiceBindingProtocolGuid,
+                  &ArpService->ServiceBinding,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ERROR;
+  }
+
+  //
+  // OK, start to receive arp packets from Mnp.
+  //
+  Status = ArpService->Mnp->Receive (ArpService->Mnp, &ArpService->RxToken);
+  if (EFI_ERROR (Status)) {
+    goto ERROR;
+  }
+
+  return Status;
+
+ERROR:
+
+  //
+  // On error, clean the arp service context data, and free the memory allocated.
+  //
+  ArpCleanService (ArpService);
+  FreePool (ArpService);
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. 
+  
+  Release the control of this controller and remove the IScsi functions. The Stop()
+  function is designed to be invoked from the EFI boot service DisconnectController(). 
+  As a result, much of the error checking on the parameters to Stop() has been moved 
+  into this common boot service. It is legal to call Stop() from other locations, 
+  but the following calling restrictions must be followed or the system behavior 
+  will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+                                Not used.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.Not used.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_HANDLE                    NicHandle;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  ARP_SERVICE_DATA              *ArpService;
+  LIST_ENTRY                    *List;
+
+  //
+  // Get the NicHandle which the arp servicebinding is installed on.
+  //
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiManagedNetworkProtocolGuid);
+  if (NicHandle == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Try to get the arp servicebinding protocol on the NicHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiArpServiceBindingProtocolGuid,
+                  (VOID **)&ServiceBinding,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "ArpDriverBindingStop: Open ArpSb failed, %r.\n", Status));
+    return EFI_DEVICE_ERROR;
+  }
+
+  ArpService = ARP_SERVICE_DATA_FROM_THIS (ServiceBinding);
+
+  if (NumberOfChildren != 0) {
+    //
+    // NumberOfChildren is not zero, destroy all the ARP children instances.
+    //
+    List = &ArpService->ChildrenList;
+    Status = NetDestroyLinkList (
+               List,
+               ArpDestroyChildEntryInHandleBuffer,
+               ServiceBinding,
+               NULL
+               );
+    ASSERT (IsListEmpty (&ArpService->PendingRequestTable));
+    ASSERT (IsListEmpty (&ArpService->DeniedCacheTable));
+    ASSERT (IsListEmpty (&ArpService->ResolvedCacheTable));
+  } else if (IsListEmpty (&ArpService->ChildrenList)) {
+    //
+    // Uninstall the ARP ServiceBinding protocol.
+    //
+    gBS->UninstallMultipleProtocolInterfaces (
+           NicHandle,
+           &gEfiArpServiceBindingProtocolGuid,
+           &ArpService->ServiceBinding,
+           NULL
+           );
+
+    //
+    // Clean the arp servicebinding context data and free the memory allocated.
+    //
+    ArpCleanService (ArpService);
+
+    FreePool (ArpService);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned 
+  in ChildHandle. If ChildHandle is not a pointer to NULL, then the protocol 
+  installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing 
+                      UEFI handle, then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+ArpServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  )
+{
+  EFI_STATUS         Status;
+  ARP_SERVICE_DATA   *ArpService;
+  ARP_INSTANCE_DATA  *Instance;
+  VOID               *Mnp;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ArpService = ARP_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Allocate memory for the instance context data.
+  //
+  Instance = AllocateZeroPool (sizeof(ARP_INSTANCE_DATA));
+  if (Instance == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpSBCreateChild: Failed to allocate memory for Instance.\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Init the instance context data.
+  //
+  ArpInitInstance (ArpService, Instance);
+
+  //
+  // Install the ARP protocol onto the ChildHandle.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiArpProtocolGuid,
+                  (VOID *)&Instance->ArpProto,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "ArpSBCreateChild: faild to install ARP protocol, %r.\n", Status));
+
+    FreePool (Instance);
+    return Status;
+  }
+
+  //
+  // Save the ChildHandle.
+  //
+  Instance->Handle = *ChildHandle;
+
+  //
+  // Open the Managed Network protocol BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  ArpService->MnpChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **) &Mnp,
+                  gArpDriverBinding.DriverBindingHandle,
+                  Instance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Insert the instance into children list managed by the arp service context data.
+  //
+  InsertTailList (&ArpService->ChildrenList, &Instance->List);
+  ArpService->ChildrenNumber++;
+
+  gBS->RestoreTPL (OldTpl);
+
+ERROR:
+
+  if (EFI_ERROR (Status)) {
+
+    gBS->CloseProtocol (
+           ArpService->MnpChildHandle,
+           &gEfiManagedNetworkProtocolGuid,
+           gArpDriverBinding.DriverBindingHandle,
+           Instance->Handle
+           );
+
+    gBS->UninstallMultipleProtocolInterfaces (
+           Instance->Handle,
+           &gEfiArpProtocolGuid,
+           &Instance->ArpProto,
+           NULL
+           );
+
+    //
+    // Free the allocated memory.
+    //
+    FreePool (Instance);
+  }
+
+  return Status;
+}
+
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is 
+                                being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+ArpServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  )
+{
+  EFI_STATUS         Status;
+  ARP_SERVICE_DATA   *ArpService;
+  ARP_INSTANCE_DATA  *Instance;
+  EFI_ARP_PROTOCOL   *Arp;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ArpService = ARP_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Get the arp protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiArpProtocolGuid,
+                  (VOID **)&Arp,
+                  ArpService->ImageHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (Arp);
+
+  if (Instance->InDestroy) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Use the InDestroy as a flag to avoid re-entrance.
+  //
+  Instance->InDestroy = TRUE;
+
+  //
+  // Close the Managed Network protocol.
+  //
+  gBS->CloseProtocol (
+         ArpService->MnpChildHandle,
+         &gEfiManagedNetworkProtocolGuid,
+         gArpDriverBinding.DriverBindingHandle,
+         ChildHandle
+         );
+
+  //
+  // Uninstall the ARP protocol.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiArpProtocolGuid,
+                  &Instance->ArpProto,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "ArpSBDestroyChild: Failed to uninstall the arp protocol, %r.\n",
+      Status));
+
+    Instance->InDestroy = FALSE;
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Instance->Configured) {
+    //
+    // Delete the related cache entry.
+    //
+    ArpDeleteCacheEntry (Instance, FALSE, NULL, TRUE);
+
+    //
+    // Reset the instance configuration.
+    //
+    ArpConfigureInstance (Instance, NULL);
+  }
+
+  //
+  // Remove this instance from the ChildrenList.
+  //
+  RemoveEntryList (&Instance->List);
+  ArpService->ChildrenNumber--;
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (Instance);
+
+  return Status;
+}
+
+/**
+  The entry point for Arp driver which installs the driver binding and component name
+  protocol on its ImageHandle.
+
+  @param[in]  ImageHandle        The image handle of the driver.
+  @param[in]  SystemTable        The system table.
+
+  @retval EFI_SUCCESS            if the driver binding and component name protocols 
+                                 are successfully
+  @retval Others                 Failed to install the protocols.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gArpDriverBinding,
+           ImageHandle,
+           &gArpComponentName,
+           &gArpComponentName2
+           );
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.h b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.h
new file mode 100644
index 0000000000..24b466fdbc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDriver.h
@@ -0,0 +1,340 @@
+/** @file
+  ARP driver header file.
+  
+Copyright (c) 2006 - 2011, 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<BR>
+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.
+
+**/
+
+#ifndef _ARP_DRIVER_H_
+#define _ARP_DRIVER_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Arp.h>
+#include <Protocol/ManagedNetwork.h>
+#include <Protocol/ServiceBinding.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+
+//
+// Global variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL    gArpDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL    gArpComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gArpComponentName2;
+
+//
+// Function prototypes for the Drivr Binding Protocol
+//
+/**
+  Tests to see if this driver supports a given controller. 
+  
+  If a child device is provided, it further tests to see if this driver supports 
+  creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, 
+                                   and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver 
+                                   specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed 
+                                   by the driver specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by 
+                                   a different driver or an application that 
+                                   requires exclusive acces. Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the 
+                                   driver specified by This.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. 
+  
+  The Start() function is designed to be invoked from the EFI boot service ConnectController(). 
+  As a result, much of the error checking on the parameters to Start() has been 
+  moved into this common boot service. It is legal to call Start() from other locations, 
+  but the following calling restrictions must be followed or the system behavior 
+  will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally 
+     aligned EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified 
+     by This must have been called with the same calling parameters, and Supported() 
+     must have returned EFI_SUCCESS.  
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, 
+                                   and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.
+                                   Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of 
+                                   resources.
+  @retval Others                   The driver failded to start the device.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle. 
+  
+  Release the control of this controller and remove the IScsi functions. The Stop()
+  function is designed to be invoked from the EFI boot service DisconnectController(). 
+  As a result, much of the error checking on the parameters to Stop() has been moved 
+  into this common boot service. It is legal to call Stop() from other locations, 
+  but the following calling restrictions must be followed or the system behavior 
+  will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+                                Not used.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.Not used.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned 
+  in ChildHandle. If ChildHandle is not a pointer to NULL, then the protocol 
+  installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing 
+                      UEFI handle, then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+ArpServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is 
+                                being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+ArpServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  );
+
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.inf b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.inf
new file mode 100644
index 0000000000..9dff604a94
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.inf
@@ -0,0 +1,68 @@
+## @file
+#  This module produces EFI ARP Protocol and EFI ARP Service Binding Protocol.
+#
+#  This module produces EFI ARP Protocol upon EFI MNP Protocol, to provide a generic
+#  implementation of the Address Resolution Protocol that is described in RFCs 826
+#  and 1122.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ArpDxe
+  MODULE_UNI_FILE                = ArpDxe.uni
+  FILE_GUID                      = 529D3F93-E8E9-4e73-B1E1-BDF6A9D50113
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = ArpDriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gArpDriverBinding                       
+#  COMPONENT_NAME                =  gArpComponentName
+#  COMPONENT_NAME2               =  gArpComponentName2
+#
+
+[Sources]
+  ArpMain.c
+  ArpDriver.h
+  ComponentName.c
+  ArpImpl.h
+  ArpImpl.c
+  ArpDriver.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  NetLib
+  DpcLib
+
+[Protocols]
+  gEfiArpServiceBindingProtocolGuid             ## BY_START
+  gEfiManagedNetworkServiceBindingProtocolGuid  ## TO_START
+  gEfiArpProtocolGuid                           ## BY_START
+  gEfiManagedNetworkProtocolGuid                ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ArpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.uni b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.uni
new file mode 100644
index 0000000000..38148cba45
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxeExtra.uni
new file mode 100644
index 0000000000..de8045a6d8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.c b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.c
new file mode 100644
index 0000000000..633c3e7d38
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.c
@@ -0,0 +1,1660 @@
+/** @file
+  The implementation of the ARP protocol.
+  
+Copyright (c) 2006 - 2012, 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<BR>
+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 "ArpImpl.h"
+
+//
+// Global variable of EFI ARP Protocol Interface.
+//
+EFI_ARP_PROTOCOL  mEfiArpProtocolTemplate = {
+  ArpConfigure,
+  ArpAdd,
+  ArpFind,
+  ArpDelete,
+  ArpFlush,
+  ArpRequest,
+  ArpCancel
+};
+
+
+/**
+  Initialize the instance context data.
+
+  @param[in]   ArpService        Pointer to the arp service context data this
+                                 instance belongs to.
+  @param[out]  Instance          Pointer to the instance context data.
+
+  @return None.
+
+**/
+VOID
+ArpInitInstance (
+  IN  ARP_SERVICE_DATA   *ArpService,
+  OUT ARP_INSTANCE_DATA  *Instance
+  )
+{
+  NET_CHECK_SIGNATURE (ArpService, ARP_SERVICE_DATA_SIGNATURE);
+
+  Instance->Signature  = ARP_INSTANCE_DATA_SIGNATURE;
+  Instance->ArpService = ArpService;
+
+  CopyMem (&Instance->ArpProto, &mEfiArpProtocolTemplate, sizeof (Instance->ArpProto));
+
+  Instance->Configured = FALSE;
+  Instance->InDestroy  = FALSE;
+
+  InitializeListHead (&Instance->List);
+}
+
+
+/**
+  Process the Arp packets received from Mnp, the procedure conforms to RFC826.
+
+  @param[in]  Context            Pointer to the context data registerd to the
+                                 Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameRcvdDpc (
+  IN VOID       *Context
+  )
+{
+  EFI_STATUS                            Status;
+  ARP_SERVICE_DATA                      *ArpService;
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *RxToken;
+  EFI_MANAGED_NETWORK_RECEIVE_DATA      *RxData;
+  ARP_HEAD                              *Head;
+  ARP_ADDRESS                           ArpAddress;
+  ARP_CACHE_ENTRY                       *CacheEntry;
+  LIST_ENTRY                            *Entry;
+  ARP_INSTANCE_DATA                     *Instance;
+  EFI_ARP_CONFIG_DATA                   *ConfigData;
+  NET_ARP_ADDRESS                       SenderAddress[2];
+  BOOLEAN                               ProtoMatched;
+  BOOLEAN                               IsTarget;
+  BOOLEAN                               MergeFlag;
+
+  ArpService = (ARP_SERVICE_DATA *)Context;
+  NET_CHECK_SIGNATURE (ArpService, ARP_SERVICE_DATA_SIGNATURE);
+
+  RxToken = &ArpService->RxToken;
+
+  if (RxToken->Status == EFI_ABORTED) {
+    //
+    // The Token is aborted, possibly by arp itself, just return and the receiving
+    // process is stopped.
+    //
+    return;
+  }
+
+  if (EFI_ERROR (RxToken->Status)) {
+    //
+    // Restart the receiving if any other error Status occurs.
+    //
+    goto RESTART_RECEIVE;
+  }
+
+  //
+  // Status is EFI_SUCCESS, process the received frame.
+  //
+  RxData = RxToken->Packet.RxData;
+  Head   = (ARP_HEAD *) RxData->PacketData;
+
+  //
+  // Convert the byte order of the multi-byte fields.
+  //
+  Head->HwType    = NTOHS (Head->HwType);
+  Head->ProtoType = NTOHS (Head->ProtoType);
+  Head->OpCode    = NTOHS (Head->OpCode);
+
+  if ((Head->HwType != ArpService->SnpMode.IfType) ||
+    (Head->HwAddrLen != ArpService->SnpMode.HwAddressSize) ||
+    (RxData->ProtocolType != ARP_ETHER_PROTO_TYPE)) {
+    //
+    // The hardware type or the hardware address length doesn't match.
+    // There is a sanity check for the protocol type too.
+    //
+    goto RECYCLE_RXDATA;
+  }
+
+  //
+  // Set the pointers to the addresses contained in the arp packet.
+  //
+  ArpAddress.SenderHwAddr    = (UINT8 *)(Head + 1);
+  ArpAddress.SenderProtoAddr = ArpAddress.SenderHwAddr + Head->HwAddrLen;
+  ArpAddress.TargetHwAddr    = ArpAddress.SenderProtoAddr + Head->ProtoAddrLen;
+  ArpAddress.TargetProtoAddr = ArpAddress.TargetHwAddr + Head->HwAddrLen;
+
+  SenderAddress[Hardware].Type       = Head->HwType;
+  SenderAddress[Hardware].Length     = Head->HwAddrLen;
+  SenderAddress[Hardware].AddressPtr = ArpAddress.SenderHwAddr;
+
+  SenderAddress[Protocol].Type       = Head->ProtoType;
+  SenderAddress[Protocol].Length     = Head->ProtoAddrLen;
+  SenderAddress[Protocol].AddressPtr = ArpAddress.SenderProtoAddr;
+
+  //
+  // First, check the denied cache table.
+  //
+  CacheEntry = ArpFindDeniedCacheEntry (
+                 ArpService,
+                 &SenderAddress[Protocol],
+                 &SenderAddress[Hardware]
+                 );
+  if (CacheEntry != NULL) {
+    //
+    // This address (either hardware or protocol address, or both) is configured to
+    // be a deny entry, silently skip the normal process.
+    //
+    goto RECYCLE_RXDATA;
+  }
+
+  ProtoMatched = FALSE;
+  IsTarget     = FALSE;
+  Instance     = NULL;
+  NET_LIST_FOR_EACH (Entry, &ArpService->ChildrenList) {
+    //
+    // Iterate all the children.
+    //
+    Instance = NET_LIST_USER_STRUCT (Entry, ARP_INSTANCE_DATA, List);
+    NET_CHECK_SIGNATURE (Instance, ARP_INSTANCE_DATA_SIGNATURE);
+    ConfigData = &Instance->ConfigData;
+
+    if ((Instance->Configured) &&
+      (Head->ProtoType == ConfigData->SwAddressType) &&
+      (Head->ProtoAddrLen == ConfigData->SwAddressLength)) {
+      //
+      // The protocol type is matched for the received arp packet.
+      //
+      ProtoMatched = TRUE;
+      if (0 == CompareMem (
+                 (VOID *)ArpAddress.TargetProtoAddr,
+                 ConfigData->StationAddress,
+                 ConfigData->SwAddressLength
+                 )) {
+        //
+        // The arp driver has the target address required by the received arp packet.
+        //
+        IsTarget = TRUE;
+        break;
+      }
+    }
+  }
+
+  if (!ProtoMatched) {
+    //
+    // Protocol type unmatchable, skip.
+    //
+    goto RECYCLE_RXDATA;
+  }
+
+  //
+  // Check whether the sender's address information is already in the cache.
+  //
+  MergeFlag  = FALSE;
+  CacheEntry = ArpFindNextCacheEntryInTable (
+                 &ArpService->ResolvedCacheTable,
+                 NULL,
+                 ByProtoAddress,
+                 &SenderAddress[Protocol],
+                 NULL
+                 );
+  if (CacheEntry != NULL) {
+    //
+    // Update the entry with the new information.
+    //
+    ArpFillAddressInCacheEntry (CacheEntry, &SenderAddress[Hardware], NULL);
+    CacheEntry->DecayTime = CacheEntry->DefaultDecayTime;
+    MergeFlag = TRUE;
+  }
+
+  if (!IsTarget) {
+    //
+    // This arp packet isn't targeted to us, skip now.
+    //
+    goto RECYCLE_RXDATA;
+  }
+
+  if (!MergeFlag) {
+    //
+    // Add the triplet <protocol type, sender protocol address, sender hardware address>
+    // to the translation table.
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->PendingRequestTable,
+                   NULL,
+                   ByProtoAddress,
+                   &SenderAddress[Protocol],
+                   NULL
+                   );
+    if (CacheEntry == NULL) {
+      //
+      // Allocate a new CacheEntry.
+      //
+      CacheEntry = ArpAllocCacheEntry (NULL);
+      if (CacheEntry == NULL) {
+        goto RECYCLE_RXDATA;
+      }
+    }
+
+    if (!IsListEmpty (&CacheEntry->List)) {
+      RemoveEntryList (&CacheEntry->List);
+    }
+
+    //
+    // Fill the addresses into the CacheEntry.
+    //
+    ArpFillAddressInCacheEntry (
+      CacheEntry,
+      &SenderAddress[Hardware],
+      &SenderAddress[Protocol]
+      );
+
+    //
+    // Inform the user.
+    //
+    ArpAddressResolved (CacheEntry, NULL, NULL);
+
+    //
+    // Add this entry into the ResolvedCacheTable
+    //
+    InsertHeadList (&ArpService->ResolvedCacheTable, &CacheEntry->List);
+  }
+
+  if (Head->OpCode == ARP_OPCODE_REQUEST) {
+    //
+    // Send back the ARP Reply. If we reach here, Instance is not NULL and CacheEntry
+    // is not NULL.
+    //
+    ArpSendFrame (Instance, CacheEntry, ARP_OPCODE_REPLY);
+  }
+
+RECYCLE_RXDATA:
+
+  //
+  // Signal Mnp to recycle the RxData.
+  //
+  gBS->SignalEvent (RxData->RecycleEvent);
+
+RESTART_RECEIVE:
+
+  //
+  // Continue to receive packets from Mnp.
+  //
+  Status = ArpService->Mnp->Receive (ArpService->Mnp, RxToken);
+
+  DEBUG_CODE (
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "ArpOnFrameRcvd: ArpService->Mnp->Receive "
+        "failed, %r\n.", Status));
+    }
+  );
+}
+
+/**
+  Queue ArpOnFrameRcvdDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameRcvd (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  //
+  // Request ArpOnFrameRcvdDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, ArpOnFrameRcvdDpc, Context);
+}
+
+/**
+  Process the already sent arp packets.
+  
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameSentDpc (
+  IN VOID       *Context
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TxToken;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA     *TxData;
+
+  ASSERT (Context != NULL);
+
+  TxToken = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *)Context;
+  TxData  = TxToken->Packet.TxData;
+
+  DEBUG_CODE (
+    if (EFI_ERROR (TxToken->Status)) {
+      DEBUG ((EFI_D_ERROR, "ArpOnFrameSent: TxToken->Status, %r.\n", TxToken->Status));
+    }
+  );
+
+  //
+  // Free the allocated memory and close the event.
+  //
+  FreePool (TxData->FragmentTable[0].FragmentBuffer);
+  FreePool (TxData);
+  gBS->CloseEvent (TxToken->Event);
+  FreePool (TxToken);
+}
+
+/**
+  Request ArpOnFrameSentDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameSent (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  //
+  // Request ArpOnFrameSentDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, ArpOnFrameSentDpc, Context);
+}
+
+
+/**
+  Process the arp cache olding and drive the retrying arp requests.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpTimerHandler (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  ARP_SERVICE_DATA      *ArpService;
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *NextEntry;
+  LIST_ENTRY            *ContextEntry;
+  ARP_CACHE_ENTRY       *CacheEntry;
+  USER_REQUEST_CONTEXT  *RequestContext;
+
+  ASSERT (Context != NULL);
+  ArpService = (ARP_SERVICE_DATA *)Context;
+
+  //
+  // Iterate all the pending requests to see whether a retry is needed to send out
+  // or the request finally fails because the retry time reaches the limitation.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &ArpService->PendingRequestTable) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+
+    if (CacheEntry->NextRetryTime <= ARP_PERIODIC_TIMER_INTERVAL) {
+      //
+      // Timeout, if we can retry more, send out the request again, otherwise abort
+      // this request.
+      //
+      if (CacheEntry->RetryCount == 0) {
+        //
+        // Abort this request.
+        //
+        ArpAddressResolved (CacheEntry, NULL, NULL);
+        ASSERT (IsListEmpty (&CacheEntry->UserRequestList));
+
+        RemoveEntryList (&CacheEntry->List);
+        FreePool (CacheEntry);
+      } else {
+        //
+        // resend the ARP request.
+        //
+        ASSERT (!IsListEmpty(&CacheEntry->UserRequestList));
+
+        ContextEntry   = CacheEntry->UserRequestList.ForwardLink;
+        RequestContext = NET_LIST_USER_STRUCT (ContextEntry, USER_REQUEST_CONTEXT, List);
+
+        ArpSendFrame (RequestContext->Instance, CacheEntry, ARP_OPCODE_REQUEST);
+
+        CacheEntry->RetryCount--;
+        CacheEntry->NextRetryTime = RequestContext->Instance->ConfigData.RetryTimeOut;
+      }
+    } else {
+      //
+      // Update the NextRetryTime.
+      //
+      CacheEntry->NextRetryTime -= ARP_PERIODIC_TIMER_INTERVAL;
+    }
+  }
+
+  //
+  // Check the timeouts for the DeniedCacheTable.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &ArpService->DeniedCacheTable) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+    ASSERT (IsListEmpty (&CacheEntry->UserRequestList));
+
+    if (CacheEntry->DefaultDecayTime == 0) {
+      //
+      // It's a static entry, skip it.
+      //
+      continue;
+    }
+
+    if (CacheEntry->DecayTime <= ARP_PERIODIC_TIMER_INTERVAL) {
+      //
+      // Time out, remove it.
+      //
+      RemoveEntryList (&CacheEntry->List);
+      FreePool (CacheEntry);
+    } else {
+      //
+      // Update the DecayTime.
+      //
+      CacheEntry->DecayTime -= ARP_PERIODIC_TIMER_INTERVAL;
+    }
+  }
+
+  //
+  // Check the timeouts for the ResolvedCacheTable.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &ArpService->ResolvedCacheTable) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+    ASSERT (IsListEmpty (&CacheEntry->UserRequestList));
+
+    if (CacheEntry->DefaultDecayTime == 0) {
+      //
+      // It's a static entry, skip it.
+      //
+      continue;
+    }
+
+    if (CacheEntry->DecayTime <= ARP_PERIODIC_TIMER_INTERVAL) {
+      //
+      // Time out, remove it.
+      //
+      RemoveEntryList (&CacheEntry->List);
+      FreePool (CacheEntry);
+    } else {
+      //
+      // Update the DecayTime.
+      //
+      CacheEntry->DecayTime -= ARP_PERIODIC_TIMER_INTERVAL;
+    }
+  }
+}
+
+
+/**
+  Match the two NET_ARP_ADDRESSes.
+
+  @param[in]  AddressOne             Pointer to the first address to match.
+  @param[in]  AddressTwo             Pointer to the second address to match.
+
+  @return The two addresses match or not.
+
+**/
+BOOLEAN
+ArpMatchAddress (
+  IN NET_ARP_ADDRESS  *AddressOne,
+  IN NET_ARP_ADDRESS  *AddressTwo
+  )
+{
+  ASSERT (AddressOne != NULL && AddressTwo != NULL);
+
+  if ((AddressOne->Type != AddressTwo->Type) ||
+    (AddressOne->Length != AddressTwo->Length)) {
+    //
+    // Either Type or Length doesn't match.
+    //
+    return FALSE;
+  }
+
+  if ((AddressOne->AddressPtr != NULL) &&
+    (CompareMem (
+      AddressOne->AddressPtr,
+      AddressTwo->AddressPtr,
+      AddressOne->Length
+      ) != 0)) {
+    //
+    // The address is not the same.
+    //
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Find the CacheEntry which matches the requirements in the specified CacheTable.
+
+  @param[in]  CacheTable             Pointer to the arp cache table.
+  @param[in]  StartEntry             Pointer to the start entry this search begins with
+                                     in the cache table.
+  @param[in]  FindOpType             The search type.
+  @param[in]  ProtocolAddress        Pointer to the protocol address to match.
+  @param[in]  HardwareAddress        Pointer to the hardware address to match.
+
+  @return Pointer to the matched arp cache entry, if NULL, no match is found.
+
+**/
+ARP_CACHE_ENTRY *
+ArpFindNextCacheEntryInTable (
+  IN LIST_ENTRY        *CacheTable,
+  IN LIST_ENTRY        *StartEntry,
+  IN FIND_OPTYPE       FindOpType,
+  IN NET_ARP_ADDRESS   *ProtocolAddress OPTIONAL,
+  IN NET_ARP_ADDRESS   *HardwareAddress OPTIONAL
+  )
+{
+  LIST_ENTRY       *Entry;
+  ARP_CACHE_ENTRY  *CacheEntry;
+
+  if (StartEntry == NULL) {
+    //
+    // Start from the beginning of the table if no StartEntry is specified.
+    //
+    StartEntry = CacheTable;
+  }
+
+  for (Entry = StartEntry->ForwardLink; Entry != CacheTable; Entry = Entry->ForwardLink) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+
+    if ((FindOpType & MATCH_SW_ADDRESS) != 0) {
+      //
+      // Find by the software address.
+      //
+      if (!ArpMatchAddress (ProtocolAddress, &CacheEntry->Addresses[Protocol])) {
+        //
+        // The ProtocolAddress doesn't match, continue to the next cache entry.
+        //
+        continue;
+      }
+    }
+
+    if ((FindOpType & MATCH_HW_ADDRESS) != 0) {
+      //
+      // Find by the hardware address.
+      //
+      if (!ArpMatchAddress (HardwareAddress, &CacheEntry->Addresses[Hardware])) {
+        //
+        // The HardwareAddress doesn't match, continue to the next cache entry.
+        //
+        continue;
+      }
+    }
+
+    //
+    // The CacheEntry meets the requirements now, return this entry.
+    //
+    return CacheEntry;
+  }
+
+  //
+  // No matching.
+  //
+  return NULL;
+}
+
+
+/**
+  Find the CacheEntry, using ProtocolAddress or HardwareAddress or both, as the keyword,
+  in the DeniedCacheTable.
+
+  @param[in]  ArpService             Pointer to the arp service context data.
+  @param[in]  ProtocolAddress        Pointer to the protocol address.
+  @param[in]  HardwareAddress        Pointer to the hardware address.
+
+  @return Pointer to the matched cache entry, if NULL no match is found.
+
+**/
+ARP_CACHE_ENTRY *
+ArpFindDeniedCacheEntry (
+  IN ARP_SERVICE_DATA  *ArpService,
+  IN NET_ARP_ADDRESS   *ProtocolAddress OPTIONAL,
+  IN NET_ARP_ADDRESS   *HardwareAddress OPTIONAL
+  )
+{
+  ARP_CACHE_ENTRY  *CacheEntry;
+
+  ASSERT ((ProtocolAddress != NULL) || (HardwareAddress != NULL));
+  NET_CHECK_SIGNATURE (ArpService, ARP_SERVICE_DATA_SIGNATURE);
+
+  CacheEntry = NULL;
+
+  if ((ProtocolAddress != NULL) && (ProtocolAddress->AddressPtr != NULL)) {
+    //
+    // Find the cache entry in the DeniedCacheTable by the protocol address.
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->DeniedCacheTable,
+                   NULL,
+                   ByProtoAddress,
+                   ProtocolAddress,
+                   NULL
+                   );
+    if (CacheEntry != NULL) {
+      //
+      // There is a match.
+      //
+      return CacheEntry;
+    }
+  }
+
+  if ((HardwareAddress != NULL) && (HardwareAddress->AddressPtr != NULL)) {
+    //
+    // Find the cache entry in the DeniedCacheTable by the hardware address.
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->DeniedCacheTable,
+                   NULL,
+                   ByHwAddress,
+                   NULL,
+                   HardwareAddress
+                   );
+  }
+
+  return CacheEntry;
+}
+
+
+/**
+  Allocate a cache entry and initialize it.
+
+  @param[in]  Instance               Pointer to the instance context data.
+
+  @return Pointer to the new created cache entry.
+
+**/
+ARP_CACHE_ENTRY *
+ArpAllocCacheEntry (
+  IN ARP_INSTANCE_DATA  *Instance
+  )
+{
+  ARP_CACHE_ENTRY  *CacheEntry;
+  NET_ARP_ADDRESS  *Address;
+  UINT16           Index;
+
+  //
+  // Allocate memory for the cache entry.
+  //
+  CacheEntry = AllocatePool (sizeof (ARP_CACHE_ENTRY));
+  if (CacheEntry == NULL) {
+    return NULL;
+  }
+
+  //
+  // Init the lists.
+  //
+  InitializeListHead (&CacheEntry->List);
+  InitializeListHead (&CacheEntry->UserRequestList);
+
+  for (Index = 0; Index < 2; Index++) {
+    //
+    // Init the address pointers to point to the concrete buffer.
+    //
+    Address = &CacheEntry->Addresses[Index];
+    Address->AddressPtr = Address->Buffer.ProtoAddress;
+  }
+
+  //
+  // Zero the hardware address first.
+  //
+  ZeroMem (CacheEntry->Addresses[Hardware].AddressPtr, ARP_MAX_HARDWARE_ADDRESS_LEN);
+
+  if (Instance != NULL) {
+    //
+    // Inherit the parameters from the instance configuration.
+    //
+    CacheEntry->RetryCount       = Instance->ConfigData.RetryCount;
+    CacheEntry->NextRetryTime    = Instance->ConfigData.RetryTimeOut;
+    CacheEntry->DefaultDecayTime = Instance->ConfigData.EntryTimeOut;
+    CacheEntry->DecayTime        = Instance->ConfigData.EntryTimeOut;
+  } else {
+    //
+    // Use the default parameters if this cache entry isn't allocate in a
+    // instance's  scope.
+    //
+    CacheEntry->RetryCount       = ARP_DEFAULT_RETRY_COUNT;
+    CacheEntry->NextRetryTime    = ARP_DEFAULT_RETRY_INTERVAL;
+    CacheEntry->DefaultDecayTime = ARP_DEFAULT_TIMEOUT_VALUE;
+    CacheEntry->DecayTime        = ARP_DEFAULT_TIMEOUT_VALUE;
+  }
+
+  return CacheEntry;
+}
+
+
+/**
+  Turn the CacheEntry into the resolved status.
+
+  @param[in]  CacheEntry             Pointer to the resolved cache entry.
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  UserEvent              Pointer to the UserEvent to notify.
+
+  @return The count of notifications sent to the instance.
+
+**/
+UINTN
+ArpAddressResolved (
+  IN ARP_CACHE_ENTRY    *CacheEntry,
+  IN ARP_INSTANCE_DATA  *Instance OPTIONAL,
+  IN EFI_EVENT          UserEvent OPTIONAL
+  )
+{
+  LIST_ENTRY            *Entry;
+  LIST_ENTRY            *NextEntry;
+  USER_REQUEST_CONTEXT  *Context;
+  UINTN                 Count;
+
+  Count = 0;
+
+  //
+  // Iterate all the linked user requests to notify them.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &CacheEntry->UserRequestList) {
+    Context = NET_LIST_USER_STRUCT (Entry, USER_REQUEST_CONTEXT, List);
+
+    if (((Instance == NULL) || (Context->Instance == Instance)) &&
+      ((UserEvent == NULL) || (Context->UserRequestEvent == UserEvent))) {
+      //
+      // Copy the address to the user-provided buffer and notify the user.
+      //
+      CopyMem (
+        Context->UserHwAddrBuffer,
+        CacheEntry->Addresses[Hardware].AddressPtr,
+        CacheEntry->Addresses[Hardware].Length
+        );
+      gBS->SignalEvent (Context->UserRequestEvent);
+
+      //
+      // Remove this user request and free the context data.
+      //
+      RemoveEntryList (&Context->List);
+      FreePool (Context);
+
+      Count++;
+    }
+  }
+
+  //
+  // Dispatch the DPCs queued by the NotifyFunction of the Context->UserRequestEvent.
+  //
+  DispatchDpc ();
+
+  return Count;
+}
+
+
+/**
+  Fill the addresses in the CacheEntry using the information passed in by
+  HwAddr and SwAddr.
+
+  @param[in]  CacheEntry             Pointer to the cache entry.
+  @param[in]  HwAddr                 Pointer to the software address.
+  @param[in]  SwAddr                 Pointer to the hardware address.
+
+  @return None.
+
+**/
+VOID
+ArpFillAddressInCacheEntry (
+  IN ARP_CACHE_ENTRY  *CacheEntry,
+  IN NET_ARP_ADDRESS  *HwAddr OPTIONAL,
+  IN NET_ARP_ADDRESS  *SwAddr OPTIONAL
+  )
+{
+  NET_ARP_ADDRESS  *Address[2];
+  NET_ARP_ADDRESS  *CacheAddress;
+  UINT32           Index;
+
+  Address[Hardware] = HwAddr;
+  Address[Protocol] = SwAddr;
+
+  for (Index = 0; Index < 2; Index++) {
+    if (Address[Index] != NULL) {
+      //
+      // Fill the address if the passed in pointer is not NULL.
+      //
+      CacheAddress = &CacheEntry->Addresses[Index];
+
+      CacheAddress->Type   = Address[Index]->Type;
+      CacheAddress->Length = Address[Index]->Length;
+
+      if (Address[Index]->AddressPtr != NULL) {
+        //
+        // Copy it if the AddressPtr points to some buffer.
+        //
+        CopyMem (
+          CacheAddress->AddressPtr,
+          Address[Index]->AddressPtr,
+          CacheAddress->Length
+          );
+      } else {
+        //
+        // Zero the corresponding address buffer in the CacheEntry.
+        //
+        ZeroMem (CacheAddress->AddressPtr, CacheAddress->Length);
+      }
+    }
+  }
+}
+
+
+/**
+  Configure the instance using the ConfigData. ConfigData is already validated.
+
+  @param[in]  Instance           Pointer to the instance context data to be
+                                 configured.
+  @param[in]  ConfigData         Pointer to the configuration data used to
+                                 configure the instance.
+
+  @retval EFI_SUCCESS            The instance is configured with the ConfigData.
+  @retval EFI_ACCESS_DENIED      The instance is already configured and the
+                                 ConfigData tries to reset some unchangeable
+                                 fields.
+  @retval EFI_INVALID_PARAMETER  The ConfigData provides a non-unicast IPv4 address
+                                 when the SwAddressType is IPv4.
+  @retval EFI_OUT_OF_RESOURCES   The instance fails to configure due to memory
+                                 limitation.
+
+**/
+EFI_STATUS
+ArpConfigureInstance (
+  IN ARP_INSTANCE_DATA    *Instance,
+  IN EFI_ARP_CONFIG_DATA  *ConfigData OPTIONAL
+  )
+{
+  EFI_ARP_CONFIG_DATA  *OldConfigData;
+  IP4_ADDR             Ip;
+
+  OldConfigData = &Instance->ConfigData;
+
+  if (ConfigData != NULL) {
+
+    if (Instance->Configured) {
+      //
+      // The instance is configured, check the unchangeable fields.
+      //
+      if ((OldConfigData->SwAddressType != ConfigData->SwAddressType) ||
+        (OldConfigData->SwAddressLength != ConfigData->SwAddressLength) ||
+        (CompareMem (
+           OldConfigData->StationAddress,
+           ConfigData->StationAddress,
+           OldConfigData->SwAddressLength
+           ) != 0)) {
+        //
+        // Deny the unallowed changes.
+        //
+        return EFI_ACCESS_DENIED;
+      }
+    } else {
+      //
+      // The instance is not configured.
+      //
+
+      if (ConfigData->SwAddressType == IPV4_ETHER_PROTO_TYPE) {
+        CopyMem (&Ip, ConfigData->StationAddress, sizeof (IP4_ADDR));
+
+        if (!NetIp4IsUnicast (NTOHL (Ip), 0)) {
+          //
+          // The station address is not a valid IPv4 unicast address.
+          //
+          return EFI_INVALID_PARAMETER;
+        }
+      }
+
+      //
+      // Save the configuration.
+      //
+      CopyMem (OldConfigData, ConfigData, sizeof (*OldConfigData));
+
+      OldConfigData->StationAddress = AllocatePool (OldConfigData->SwAddressLength);
+      if (OldConfigData->StationAddress == NULL) {
+        DEBUG ((EFI_D_ERROR, "ArpConfigInstance: AllocatePool for the StationAddress "
+          "failed.\n"));
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      //
+      // Save the StationAddress.
+      //
+      CopyMem (
+        OldConfigData->StationAddress,
+        ConfigData->StationAddress,
+        OldConfigData->SwAddressLength
+        );
+
+      //
+      // Set the state to configured.
+      //
+      Instance->Configured = TRUE;
+    }
+
+    //
+    // Use the implementation specific values if the following field is zero.
+    //
+    OldConfigData->EntryTimeOut = (ConfigData->EntryTimeOut == 0) ?
+      ARP_DEFAULT_TIMEOUT_VALUE : ConfigData->EntryTimeOut;
+
+    OldConfigData->RetryCount   = (ConfigData->RetryCount == 0) ?
+      ARP_DEFAULT_RETRY_COUNT : ConfigData->RetryCount;
+
+    OldConfigData->RetryTimeOut = (ConfigData->RetryTimeOut == 0) ?
+      ARP_DEFAULT_RETRY_INTERVAL : ConfigData->RetryTimeOut;
+  } else {
+    //
+    // Reset the configuration.
+    //
+
+    if (Instance->Configured) {
+      //
+      // Cancel the arp requests issued by this instance.
+      //
+      Instance->ArpProto.Cancel (&Instance->ArpProto, NULL, NULL);
+
+      //
+      // Free the buffer previously allocated to hold the station address.
+      //
+      FreePool (OldConfigData->StationAddress);
+    }
+
+    Instance->Configured = FALSE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Send out an arp frame using the CachEntry and the ArpOpCode.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  CacheEntry             Pointer to the configuration data used to
+                                     configure the instance.
+  @param[in]  ArpOpCode              The opcode used to send out this Arp frame, either
+                                     request or reply.
+
+  @return None.
+
+**/
+VOID
+ArpSendFrame (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN ARP_CACHE_ENTRY    *CacheEntry,
+  IN UINT16             ArpOpCode
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TxToken;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA     *TxData;
+  UINT32                                TotalLength;
+  UINT8                                 *Packet;
+  ARP_SERVICE_DATA                      *ArpService;
+  EFI_SIMPLE_NETWORK_MODE               *SnpMode;
+  EFI_ARP_CONFIG_DATA                   *ConfigData;
+  UINT8                                 *TmpPtr;
+  ARP_HEAD                              *ArpHead;
+
+  ASSERT ((Instance != NULL) && (CacheEntry != NULL));
+
+  //
+  // Allocate memory for the TxToken.
+  //
+  TxToken = AllocatePool (sizeof(EFI_MANAGED_NETWORK_COMPLETION_TOKEN));
+  if (TxToken == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpSendFrame: Allocate memory for TxToken failed.\n"));
+    return;
+  }
+
+  TxToken->Event = NULL;
+  TxData         = NULL;
+  Packet         = NULL;
+
+  //
+  // Create the event for this TxToken.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  ArpOnFrameSent,
+                  (VOID *)TxToken,
+                  &TxToken->Event
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "ArpSendFrame: CreateEvent failed for TxToken->Event.\n"));
+    goto CLEAN_EXIT;
+  }
+
+  //
+  // Allocate memory for the TxData used in the TxToken.
+  //
+  TxData = AllocatePool (sizeof(EFI_MANAGED_NETWORK_TRANSMIT_DATA));
+  if (TxData == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpSendFrame: Allocate memory for TxData failed.\n"));
+    goto CLEAN_EXIT;
+  }
+
+  ArpService = Instance->ArpService;
+  SnpMode    = &ArpService->SnpMode;
+  ConfigData = &Instance->ConfigData;
+
+  //
+  // Calculate the buffer length for this arp frame.
+  //
+  TotalLength = SnpMode->MediaHeaderSize + sizeof (ARP_HEAD) +
+                2 * (ConfigData->SwAddressLength + SnpMode->HwAddressSize);
+
+  //
+  // Allocate buffer for the arp frame.
+  //
+  Packet = AllocatePool (TotalLength);
+  if (Packet == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpSendFrame: Allocate memory for Packet failed.\n"));
+    ASSERT (Packet != NULL);
+  }
+
+  TmpPtr = Packet;
+
+  //
+  // The destination MAC address.
+  //
+  if (ArpOpCode == ARP_OPCODE_REQUEST) {
+    CopyMem (TmpPtr, &SnpMode->BroadcastAddress, SnpMode->HwAddressSize);
+  } else {
+    CopyMem (
+      TmpPtr,
+      CacheEntry->Addresses[Hardware].AddressPtr,
+      SnpMode->HwAddressSize
+      );
+  }
+  TmpPtr += SnpMode->HwAddressSize;
+
+  //
+  // The source MAC address.
+  //
+  CopyMem (TmpPtr, &SnpMode->CurrentAddress, SnpMode->HwAddressSize);
+  TmpPtr += SnpMode->HwAddressSize;
+
+  //
+  // The ethernet protocol type.
+  //
+  *(UINT16 *)TmpPtr = HTONS (ARP_ETHER_PROTO_TYPE);
+  TmpPtr            += 2;
+
+  //
+  // The ARP Head.
+  //
+  ArpHead               = (ARP_HEAD *) TmpPtr;
+  ArpHead->HwType       = HTONS ((UINT16)SnpMode->IfType);
+  ArpHead->ProtoType    = HTONS (ConfigData->SwAddressType);
+  ArpHead->HwAddrLen    = (UINT8)SnpMode->HwAddressSize;
+  ArpHead->ProtoAddrLen = ConfigData->SwAddressLength;
+  ArpHead->OpCode       = HTONS (ArpOpCode);
+  TmpPtr                += sizeof (ARP_HEAD);
+
+  //
+  // The sender hardware address.
+  //
+  CopyMem (TmpPtr, &SnpMode->CurrentAddress, SnpMode->HwAddressSize);
+  TmpPtr += SnpMode->HwAddressSize;
+
+  //
+  // The sender protocol address.
+  //
+  CopyMem (TmpPtr, ConfigData->StationAddress, ConfigData->SwAddressLength);
+  TmpPtr += ConfigData->SwAddressLength;
+
+  //
+  // The target hardware address.
+  //
+  CopyMem (
+    TmpPtr,
+    CacheEntry->Addresses[Hardware].AddressPtr,
+    SnpMode->HwAddressSize
+    );
+  TmpPtr += SnpMode->HwAddressSize;
+
+  //
+  // The target protocol address.
+  //
+  CopyMem (
+    TmpPtr,
+    CacheEntry->Addresses[Protocol].AddressPtr,
+    ConfigData->SwAddressLength
+    );
+
+  //
+  // Set all the fields of the TxData.
+  //
+  TxData->DestinationAddress = NULL;
+  TxData->SourceAddress      = NULL;
+  TxData->ProtocolType       = 0;
+  TxData->DataLength         = TotalLength - SnpMode->MediaHeaderSize;
+  TxData->HeaderLength       = (UINT16) SnpMode->MediaHeaderSize;
+  TxData->FragmentCount      = 1;
+
+  TxData->FragmentTable[0].FragmentBuffer = Packet;
+  TxData->FragmentTable[0].FragmentLength = TotalLength;
+
+  //
+  // Associate the TxData with the TxToken.
+  //
+  TxToken->Packet.TxData = TxData;
+  TxToken->Status        = EFI_NOT_READY;
+
+  //
+  // Send out this arp packet by Mnp.
+  //
+  Status = ArpService->Mnp->Transmit (ArpService->Mnp, TxToken);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Mnp->Transmit failed, %r.\n", Status));
+    goto CLEAN_EXIT;
+  }
+
+  return;
+
+CLEAN_EXIT:
+
+  if (Packet != NULL) {
+    FreePool (Packet);
+  }
+
+  if (TxData != NULL) {
+    FreePool (TxData);
+  }
+
+  if (TxToken->Event != NULL) {
+    gBS->CloseEvent (TxToken->Event);
+  }
+
+  FreePool (TxToken);
+}
+
+
+/**
+  Delete the cache entries in the specified CacheTable, using the BySwAddress,
+  SwAddressType, AddressBuffer combination as the matching key, if Force is TRUE,
+  the cache is deleted event it's a static entry.
+
+  @param[in]  CacheTable             Pointer to the cache table to do the deletion.
+  @param[in]  BySwAddress            Delete the cache entry by software address or by
+                                     hardware address.
+  @param[in]  SwAddressType          The software address used to do the deletion.
+  @param[in]  AddressBuffer          Pointer to the buffer containing the address to
+                                     match for the deletion.
+  @param[in]  Force                  This deletion is forced or not.
+
+  @return The count of the deleted cache entries.
+
+**/
+UINTN
+ArpDeleteCacheEntryInTable (
+  IN LIST_ENTRY      *CacheTable,
+  IN BOOLEAN         BySwAddress,
+  IN UINT16          SwAddressType,
+  IN UINT8           *AddressBuffer OPTIONAL,
+  IN BOOLEAN         Force
+  )
+{
+  LIST_ENTRY       *Entry;
+  LIST_ENTRY       *NextEntry;
+  ARP_CACHE_ENTRY  *CacheEntry;
+  UINTN            Count;
+
+  Count = 0;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, CacheTable) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+
+    if ((CacheEntry->DefaultDecayTime == 0) && !Force) {
+      //
+      // It's a static entry and we are not forced to delete it, skip.
+      //
+      continue;
+    }
+
+    if (BySwAddress) {
+      if (SwAddressType == CacheEntry->Addresses[Protocol].Type) {
+        //
+        // Protocol address type matched. Check the address.
+        //
+        if ((AddressBuffer == NULL) ||
+          (CompareMem (
+             AddressBuffer,
+             CacheEntry->Addresses[Protocol].AddressPtr,
+             CacheEntry->Addresses[Protocol].Length
+             ) == 0)) {
+          //
+          // Address matched.
+          //
+          goto MATCHED;
+        }
+      }
+    } else {
+      if ((AddressBuffer == NULL) ||
+        (CompareMem (
+           AddressBuffer,
+           CacheEntry->Addresses[Hardware].AddressPtr,
+           CacheEntry->Addresses[Hardware].Length
+           ) == 0)) {
+        //
+        // Address matched.
+        //
+        goto MATCHED;
+      }
+    }
+
+    continue;
+
+MATCHED:
+
+    //
+    // Delete this entry.
+    //
+    RemoveEntryList (&CacheEntry->List);
+    ASSERT (IsListEmpty (&CacheEntry->UserRequestList));
+    FreePool (CacheEntry);
+
+    Count++;
+  }
+
+  return Count;
+}
+
+
+/**
+  Delete cache entries in all the cache tables.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  BySwAddress            Delete the cache entry by software address or by
+                                     hardware address.
+  @param[in]  AddressBuffer          Pointer to the buffer containing the address to
+                                     match for the deletion.
+  @param[in]  Force                  This deletion is forced or not.
+
+  @return The count of the deleted cache entries.
+
+**/
+UINTN
+ArpDeleteCacheEntry (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN BOOLEAN            BySwAddress,
+  IN UINT8              *AddressBuffer OPTIONAL,
+  IN BOOLEAN            Force
+  )
+{
+  ARP_SERVICE_DATA  *ArpService;
+  UINTN             Count;
+
+  NET_CHECK_SIGNATURE (Instance, ARP_INSTANCE_DATA_SIGNATURE);
+
+  ArpService = Instance->ArpService;
+
+  //
+  // Delete the cache entries in the DeniedCacheTable.
+  //
+  Count = ArpDeleteCacheEntryInTable (
+            &ArpService->DeniedCacheTable,
+            BySwAddress,
+            Instance->ConfigData.SwAddressType,
+            AddressBuffer,
+            Force
+            );
+
+  //
+  // Delete the cache entries inthe ResolvedCacheTable.
+  //
+  Count += ArpDeleteCacheEntryInTable (
+             &ArpService->ResolvedCacheTable,
+             BySwAddress,
+             Instance->ConfigData.SwAddressType,
+             AddressBuffer,
+             Force
+             );
+
+  return Count;
+}
+
+
+/**
+  Cancel the arp request.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  TargetSwAddress        Pointer to the buffer containing the target
+                                     software address to match the arp request.
+  @param[in]  UserEvent              The user event used to notify this request
+                                     cancellation.
+
+  @return The count of the cancelled requests.
+
+**/
+UINTN
+ArpCancelRequest (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN VOID               *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT          UserEvent        OPTIONAL
+  )
+{
+  ARP_SERVICE_DATA  *ArpService;
+  LIST_ENTRY        *Entry;
+  LIST_ENTRY        *NextEntry;
+  ARP_CACHE_ENTRY   *CacheEntry;
+  UINTN             Count;
+
+  NET_CHECK_SIGNATURE (Instance, ARP_INSTANCE_DATA_SIGNATURE);
+
+  ArpService = Instance->ArpService;
+
+  Count = 0;
+  NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &ArpService->PendingRequestTable) {
+    CacheEntry = NET_LIST_USER_STRUCT (Entry, ARP_CACHE_ENTRY, List);
+
+    if ((TargetSwAddress == NULL) ||
+      (CompareMem (
+         TargetSwAddress,
+         CacheEntry->Addresses[Protocol].AddressPtr,
+         CacheEntry->Addresses[Protocol].Length
+         ) == 0)) {
+      //
+      // This request entry matches the TargetSwAddress or all requests are to be
+      // cancelled as TargetSwAddress is NULL.
+      //
+      Count += ArpAddressResolved (CacheEntry, Instance, UserEvent);
+
+      if (IsListEmpty (&CacheEntry->UserRequestList)) {
+        //
+        // No user requests any more, remove this request cache entry.
+        //
+        RemoveEntryList (&CacheEntry->List);
+        FreePool (CacheEntry);
+      }
+    }
+  }
+
+  return Count;
+}
+
+
+/**
+  Find the cache entry in the cache table.
+
+  @param[in]  Instance           Pointer to the instance context data.
+  @param[in]  BySwAddress        Set to TRUE to look for matching software protocol
+                                 addresses. Set to FALSE to look for matching
+                                 hardware protocol addresses.
+  @param[in]  AddressBuffer      Pointer to address buffer. Set to NULL to match
+                                 all addresses.
+  @param[out] EntryLength        The size of an entry in the entries buffer.
+  @param[out] EntryCount         The number of ARP cache entries that are found by
+                                 the specified criteria.
+  @param[out] Entries            Pointer to the buffer that will receive the ARP
+                                 cache entries.
+  @param[in]  Refresh            Set to TRUE to refresh the timeout value of the
+                                 matching ARP cache entry.
+
+  @retval EFI_SUCCESS            The requested ARP cache entries are copied into
+                                 the buffer.
+  @retval EFI_NOT_FOUND          No matching entries found.
+  @retval EFI_OUT_OF_RESOURCE    There is a memory allocation failure.
+
+**/
+EFI_STATUS
+ArpFindCacheEntry (
+  IN ARP_INSTANCE_DATA   *Instance,
+  IN BOOLEAN             BySwAddress,
+  IN VOID                *AddressBuffer OPTIONAL,
+  OUT UINT32             *EntryLength   OPTIONAL,
+  OUT UINT32             *EntryCount    OPTIONAL,
+  OUT EFI_ARP_FIND_DATA  **Entries      OPTIONAL,
+  IN BOOLEAN             Refresh
+  )
+{
+  EFI_STATUS         Status;
+  ARP_SERVICE_DATA   *ArpService;
+  NET_ARP_ADDRESS    MatchAddress;
+  FIND_OPTYPE        FindOpType;
+  LIST_ENTRY         *StartEntry;
+  ARP_CACHE_ENTRY    *CacheEntry;
+  NET_MAP            FoundEntries;
+  UINT32             FoundCount;
+  EFI_ARP_FIND_DATA  *FindData;
+  LIST_ENTRY         *CacheTable;
+  UINT32             FoundEntryLength;
+
+  ArpService = Instance->ArpService;
+
+  //
+  // Init the FounEntries used to hold the found cache entries.
+  //
+  NetMapInit (&FoundEntries);
+
+  //
+  // Set the MatchAddress.
+  //
+  if (BySwAddress) {
+    MatchAddress.Type   = Instance->ConfigData.SwAddressType;
+    MatchAddress.Length = Instance->ConfigData.SwAddressLength;
+    FindOpType          = ByProtoAddress;
+  } else {
+    MatchAddress.Type   = ArpService->SnpMode.IfType;
+    MatchAddress.Length = (UINT8)ArpService->SnpMode.HwAddressSize;
+    FindOpType          = ByHwAddress;
+  }
+
+  MatchAddress.AddressPtr = AddressBuffer;
+
+  //
+  // Search the DeniedCacheTable
+  //
+  StartEntry = NULL;
+  while (TRUE) {
+    //
+    // Try to find the matched entries in the DeniedCacheTable.
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->DeniedCacheTable,
+                   StartEntry,
+                   FindOpType,
+                   &MatchAddress,
+                   &MatchAddress
+                   );
+    if (CacheEntry == NULL) {
+      //
+      // Once the CacheEntry is NULL, there are no more matches.
+      //
+      break;
+    }
+
+    //
+    // Insert the found entry into the map.
+    //
+    NetMapInsertTail (
+      &FoundEntries,
+      (VOID *)CacheEntry,
+      (VOID *)&ArpService->DeniedCacheTable
+      );
+
+    //
+    // Let the next search start from this cache entry.
+    //
+    StartEntry = &CacheEntry->List;
+
+    if (Refresh) {
+      //
+      // Refresh the DecayTime if needed.
+      //
+      CacheEntry->DecayTime = CacheEntry->DefaultDecayTime;
+    }
+  }
+
+  //
+  // Search the ResolvedCacheTable
+  //
+  StartEntry = NULL;
+  while (TRUE) {
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->ResolvedCacheTable,
+                   StartEntry,
+                   FindOpType,
+                   &MatchAddress,
+                   &MatchAddress
+                   );
+    if (CacheEntry == NULL) {
+      //
+      // Once the CacheEntry is NULL, there are no more matches.
+      //
+      break;
+    }
+
+    //
+    // Insert the found entry into the map.
+    //
+    NetMapInsertTail (
+      &FoundEntries,
+      (VOID *)CacheEntry,
+      (VOID *)&ArpService->ResolvedCacheTable
+      );
+
+    //
+    // Let the next search start from this cache entry.
+    //
+    StartEntry = &CacheEntry->List;
+
+    if (Refresh) {
+      //
+      // Refresh the DecayTime if needed.
+      //
+      CacheEntry->DecayTime = CacheEntry->DefaultDecayTime;
+    }
+  }
+
+  Status = EFI_SUCCESS;
+
+  FoundCount = (UINT32) NetMapGetCount (&FoundEntries);
+  if (FoundCount == 0) {
+    Status = EFI_NOT_FOUND;
+    goto CLEAN_EXIT;
+  }
+
+  //
+  // Found the entry length, make sure its 8 bytes alignment.
+  //
+  FoundEntryLength = (((sizeof (EFI_ARP_FIND_DATA) + Instance->ConfigData.SwAddressLength +
+                       ArpService->SnpMode.HwAddressSize) + 3) & ~(0x3));
+
+  if (EntryLength != NULL) {
+    *EntryLength = FoundEntryLength;
+  }
+
+  if (EntryCount != NULL) {
+    //
+    // Return the found entry count.
+    //
+    *EntryCount = FoundCount;
+  }
+
+  if (Entries == NULL) {
+    goto CLEAN_EXIT;
+  }
+
+  //
+  // Allocate buffer to copy the found entries.
+  //
+  FindData = AllocatePool (FoundCount * FoundEntryLength);
+  if (FindData == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpFindCacheEntry: Failed to allocate memory.\n"));
+    Status = EFI_OUT_OF_RESOURCES;
+    goto CLEAN_EXIT;
+  }
+
+  //
+  // Return the address to the user.
+  //
+  *Entries = FindData;
+
+  //
+  // Dump the entries.
+  //
+  while (!NetMapIsEmpty (&FoundEntries)) {
+    //
+    // Get a cache entry from the map.
+    //
+    CacheEntry = NetMapRemoveHead (&FoundEntries, (VOID **)&CacheTable);
+
+    //
+    // Set the fields in FindData.
+    //
+    FindData->Size            = FoundEntryLength;
+    FindData->DenyFlag        = (BOOLEAN)(CacheTable == &ArpService->DeniedCacheTable);
+    FindData->StaticFlag      = (BOOLEAN)(CacheEntry->DefaultDecayTime == 0);
+    FindData->HwAddressType   = ArpService->SnpMode.IfType;
+    FindData->SwAddressType   = Instance->ConfigData.SwAddressType;
+    FindData->HwAddressLength = (UINT8)ArpService->SnpMode.HwAddressSize;
+    FindData->SwAddressLength = Instance->ConfigData.SwAddressLength;
+
+    //
+    // Copy the software address.
+    //
+    CopyMem (
+      FindData + 1,
+      CacheEntry->Addresses[Protocol].AddressPtr,
+      FindData->SwAddressLength
+      );
+
+    //
+    // Copy the hardware address.
+    //
+    CopyMem (
+      (UINT8 *)(FindData + 1) + FindData->SwAddressLength,
+      CacheEntry->Addresses[Hardware].AddressPtr,
+      FindData->HwAddressLength
+      );
+
+    //
+    // Slip to the next FindData.
+    //
+    FindData = (EFI_ARP_FIND_DATA *)((UINT8 *)FindData + FoundEntryLength);
+  }
+
+CLEAN_EXIT:
+
+  NetMapClean (&FoundEntries);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.h b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.h
new file mode 100644
index 0000000000..a5dce7d9a5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpImpl.h
@@ -0,0 +1,776 @@
+/** @file
+  EFI Address Resolution Protocol (ARP) Protocol interface header file.
+
+Copyright (c) 2006 - 2012, 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<BR>
+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.
+
+**/
+
+#ifndef _ARP_IMPL_H_
+#define _ARP_IMPL_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/Arp.h>
+#include <Protocol/ManagedNetwork.h>
+#include <Protocol/ServiceBinding.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/NetLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DpcLib.h>
+
+//
+// Ethernet protocol type definitions.
+//
+#define ARP_ETHER_PROTO_TYPE         0x0806
+#define IPV4_ETHER_PROTO_TYPE        0x0800
+#define IPV6_ETHER_PROTO_TYPE        0x86DD
+
+//
+// ARP opcode definitions.
+//
+#define ARP_OPCODE_REQUEST           0x0001
+#define ARP_OPCODE_REPLY             0x0002
+
+//
+// ARP timeout, retry count and interval definitions.
+//
+#define ARP_DEFAULT_TIMEOUT_VALUE    (400 * TICKS_PER_SECOND)
+#define ARP_DEFAULT_RETRY_COUNT      2
+#define ARP_DEFAULT_RETRY_INTERVAL   (5   * TICKS_PER_MS)
+#define ARP_PERIODIC_TIMER_INTERVAL  (500 * TICKS_PER_MS)
+
+//
+// ARP packet head definition.
+//
+#pragma pack(1)
+typedef struct {
+  UINT16  HwType;
+  UINT16  ProtoType;
+  UINT8   HwAddrLen;
+  UINT8   ProtoAddrLen;
+  UINT16  OpCode;
+} ARP_HEAD;
+#pragma pack()
+
+//
+// ARP Address definition for internal use.
+//
+typedef struct {
+  UINT8  *SenderHwAddr;
+  UINT8  *SenderProtoAddr;
+  UINT8  *TargetHwAddr;
+  UINT8  *TargetProtoAddr;
+} ARP_ADDRESS;
+
+#define MATCH_SW_ADDRESS  0x1
+#define MATCH_HW_ADDRESS  0x2
+
+//
+// Enumeration for the search type. A search type is specified as the keyword to find
+// a cache entry in the cache table.
+//
+typedef enum {
+  ByNone         = 0,
+  ByProtoAddress = MATCH_SW_ADDRESS,
+  ByHwAddress    = MATCH_HW_ADDRESS,
+  ByBoth         = MATCH_SW_ADDRESS | MATCH_HW_ADDRESS
+} FIND_OPTYPE;
+
+#define ARP_INSTANCE_DATA_SIGNATURE  SIGNATURE_32('A', 'R', 'P', 'I')
+
+/**
+  Returns a pointer to the ARP_INSTANCE_DATA structure from the input a.
+  
+  If the signatures matches, then a pointer to the data structure that contains 
+  a specified field of that data structure is returned.
+   
+  @param  a              Pointer to the field specified by ArpProto within a data 
+                         structure of type ARP_INSTANCE_DATA.
+ 
+**/
+#define ARP_INSTANCE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  ARP_INSTANCE_DATA, \
+  ArpProto, \
+  ARP_INSTANCE_DATA_SIGNATURE \
+  )
+
+typedef struct _ARP_SERVICE_DATA  ARP_SERVICE_DATA;
+
+//
+// ARP instance context data structure.
+//
+typedef struct {
+  UINT32               Signature;
+  ARP_SERVICE_DATA     *ArpService;
+  EFI_HANDLE           Handle;
+  EFI_ARP_PROTOCOL     ArpProto;
+  LIST_ENTRY           List;
+  EFI_ARP_CONFIG_DATA  ConfigData;
+  BOOLEAN              Configured;
+  BOOLEAN              InDestroy;
+} ARP_INSTANCE_DATA;
+
+#define ARP_SERVICE_DATA_SIGNATURE  SIGNATURE_32('A', 'R', 'P', 'S')
+
+/**
+  Returns a pointer to the ARP_SERVICE_DATA structure from the input a.
+  
+  If the signatures matches, then a pointer to the data structure that contains 
+  a specified field of that data structure is returned.
+   
+  @param  a              Pointer to the field specified by ServiceBinding within 
+                         a data structure of type ARP_SERVICE_DATA.
+ 
+**/
+#define ARP_SERVICE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  ARP_SERVICE_DATA, \
+  ServiceBinding, \
+  ARP_SERVICE_DATA_SIGNATURE \
+  )
+
+//
+// ARP service data structure.
+//
+struct _ARP_SERVICE_DATA {
+  UINT32                           Signature;
+  EFI_SERVICE_BINDING_PROTOCOL     ServiceBinding;
+
+  EFI_HANDLE                       MnpChildHandle;
+  EFI_HANDLE                       ImageHandle;
+  EFI_HANDLE                       ControllerHandle;
+
+  EFI_MANAGED_NETWORK_PROTOCOL          *Mnp;
+  EFI_MANAGED_NETWORK_CONFIG_DATA       MnpConfigData;
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  RxToken;
+
+  EFI_SIMPLE_NETWORK_MODE          SnpMode;
+
+  UINTN                            ChildrenNumber;
+  LIST_ENTRY                       ChildrenList;
+
+  LIST_ENTRY                       PendingRequestTable;
+  LIST_ENTRY                       DeniedCacheTable;
+  LIST_ENTRY                       ResolvedCacheTable;
+
+  EFI_EVENT                        PeriodicTimer;
+};
+
+//
+// User request context structure.
+//
+typedef struct {
+  LIST_ENTRY         List;
+  ARP_INSTANCE_DATA  *Instance;
+  EFI_EVENT          UserRequestEvent;
+  VOID               *UserHwAddrBuffer;
+} USER_REQUEST_CONTEXT;
+
+#define ARP_MAX_PROTOCOL_ADDRESS_LEN  sizeof(EFI_IP_ADDRESS)
+#define ARP_MAX_HARDWARE_ADDRESS_LEN  sizeof(EFI_MAC_ADDRESS)
+
+typedef union {
+  UINT8  ProtoAddress[ARP_MAX_PROTOCOL_ADDRESS_LEN];
+  UINT8  HwAddress[ARP_MAX_HARDWARE_ADDRESS_LEN];
+} NET_ARP_ADDRESS_UNION;
+
+//
+// ARP address structure in an ARP packet.
+//
+typedef struct {
+  UINT16                Type;
+  UINT8                 Length;
+  UINT8                 *AddressPtr;
+  NET_ARP_ADDRESS_UNION Buffer;
+} NET_ARP_ADDRESS;
+
+//
+// Enumeration for ARP address type.
+//
+typedef enum {
+  Hardware,
+  Protocol
+} ARP_ADDRESS_TYPE;
+
+//
+// ARP cache entry definition.
+//
+typedef struct {
+  LIST_ENTRY      List;
+
+  UINT32          RetryCount;
+  UINT32          DefaultDecayTime;
+  UINT32          DecayTime;
+  UINT32          NextRetryTime;
+
+  NET_ARP_ADDRESS  Addresses[2];
+
+  LIST_ENTRY      UserRequestList;
+} ARP_CACHE_ENTRY;
+
+/**
+  This function is used to assign a station address to the ARP cache for this instance
+  of the ARP driver.
+  
+  Each ARP instance has one station address. The EFI_ARP_PROTOCOL driver will 
+  respond to ARP requests that match this registered station address. A call to 
+  this function with the ConfigData field set to NULL will reset this ARP instance.
+  
+  Once a protocol type and station address have been assigned to this ARP instance, 
+  all the following ARP functions will use this information. Attempting to change 
+  the protocol type or station address to a configured ARP instance will result in errors.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  ConfigData             Pointer to the EFI_ARP_CONFIG_DATA structure.
+
+  @retval EFI_SUCCESS            The new station address was successfully
+                                 registered.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. SwAddressLength is zero when
+                                 ConfigData is not NULL. StationAddress is NULL
+                                 when ConfigData is not NULL.
+  @retval EFI_ACCESS_DENIED      The SwAddressType, SwAddressLength, or
+                                 StationAddress is different from the one that is
+                                 already registered.
+  @retval EFI_OUT_OF_RESOURCES   Storage for the new StationAddress could not be
+                                 allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpConfigure (
+  IN EFI_ARP_PROTOCOL     *This,
+  IN EFI_ARP_CONFIG_DATA  *ConfigData OPTIONAL
+  );
+
+/**
+  This function is used to insert entries into the ARP cache.
+
+  ARP cache entries are typically inserted and updated by network protocol drivers 
+  as network traffic is processed. Most ARP cache entries will time out and be 
+  deleted if the network traffic stops. ARP cache entries that were inserted 
+  by the Add() function may be static (will not time out) or dynamic (will time out).
+  Default ARP cache timeout values are not covered in most network protocol 
+  specifications (although RFC 1122 comes pretty close) and will only be 
+  discussed in general in this specification. The timeout values that are 
+  used in the EFI Sample Implementation should be used only as a guideline. 
+  Final product implementations of the EFI network stack should be tuned for 
+  their expected network environments.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  DenyFlag               Set to TRUE if this entry is a deny entry. Set to
+                                 FALSE if this  entry is a normal entry.
+  @param  TargetSwAddress        Pointer to a protocol address to add (or deny).
+                                 May be set to NULL if DenyFlag is TRUE.
+  @param  TargetHwAddress        Pointer to a hardware address to add (or deny).
+                                 May be set to NULL if DenyFlag is TRUE.
+  @param  TimeoutValue           Time in 100-ns units that this entry will remain
+                                 in the ARP cache. A value of zero means that the
+                                 entry is permanent. A nonzero value will override
+                                 the one given by Configure() if the entry to be
+                                 added is a dynamic entry.
+  @param  Overwrite              If TRUE, the matching cache entry will be
+                                 overwritten with the supplied parameters. If
+                                 FALSE, EFI_ACCESS_DENIED is returned if the
+                                 corresponding cache entry already exists.
+
+  @retval EFI_SUCCESS            The entry has been added or updated.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. DenyFlag is FALSE and
+                                 TargetHwAddress is NULL. DenyFlag is FALSE and
+                                 TargetSwAddress is NULL. TargetHwAddress is NULL
+                                 and TargetSwAddress is NULL. Both TargetSwAddress
+                                 and TargetHwAddress are not NULL when DenyFlag is
+                                 TRUE.
+  @retval EFI_OUT_OF_RESOURCES   The new ARP cache entry could not be allocated.
+  @retval EFI_ACCESS_DENIED      The ARP cache entry already exists and Overwrite
+                                 is not true.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpAdd (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN BOOLEAN           DenyFlag,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN VOID              *TargetHwAddress OPTIONAL,
+  IN UINT32            TimeoutValue,
+  IN BOOLEAN           Overwrite
+  );
+
+/**
+  This function searches the ARP cache for matching entries and allocates a buffer into
+  which those entries are copied.
+  
+  The first part of the allocated buffer is EFI_ARP_FIND_DATA, following which 
+  are protocol address pairs and hardware address pairs.
+  When finding a specific protocol address (BySwAddress is TRUE and AddressBuffer 
+  is not NULL), the ARP cache timeout for the found entry is reset if Refresh is 
+  set to TRUE. If the found ARP cache entry is a permanent entry, it is not 
+  affected by Refresh.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  BySwAddress            Set to TRUE to look for matching software protocol
+                                 addresses. Set to FALSE to look for matching
+                                 hardware protocol addresses.
+  @param  AddressBuffer          Pointer to address buffer. Set to NULL to match
+                                 all addresses.
+  @param  EntryLength            The size of an entry in the entries buffer.
+  @param  EntryCount             The number of ARP cache entries that are found by
+                                 the specified criteria.
+  @param  Entries                Pointer to the buffer that will receive the ARP
+                                 cache entries.
+  @param  Refresh                Set to TRUE to refresh the timeout value of the
+                                 matching ARP cache entry.
+
+  @retval EFI_SUCCESS            The requested ARP cache entries were copied into
+                                 the buffer.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. Both EntryCount and EntryLength are
+                                 NULL, when Refresh is FALSE.
+  @retval EFI_NOT_FOUND          No matching entries were found.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpFind (
+  IN EFI_ARP_PROTOCOL    *This,
+  IN BOOLEAN             BySwAddress,
+  IN VOID                *AddressBuffer OPTIONAL,
+  OUT UINT32             *EntryLength   OPTIONAL,
+  OUT UINT32             *EntryCount    OPTIONAL,
+  OUT EFI_ARP_FIND_DATA  **Entries      OPTIONAL,
+  IN BOOLEAN             Refresh
+  );
+
+/**
+  This function removes specified ARP cache entries.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  BySwAddress            Set to TRUE to delete matching protocol addresses.
+                                 Set to FALSE to delete matching hardware
+                                 addresses.
+  @param  AddressBuffer          Pointer to the address buffer that is used as a
+                                 key to look for the cache entry. Set to NULL to
+                                 delete all entries.
+
+  @retval EFI_SUCCESS            The entry was removed from the ARP cache.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          The specified deletion key was not found.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDelete (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN BOOLEAN           BySwAddress,
+  IN VOID              *AddressBuffer OPTIONAL
+  );
+
+/**
+  This function delete all dynamic entries from the ARP cache that match the specified
+  software protocol type.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+
+  @retval EFI_SUCCESS            The cache has been flushed.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          There are no matching dynamic cache entries.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpFlush (
+  IN EFI_ARP_PROTOCOL  *This
+  );
+
+/**
+  This function tries to resolve the TargetSwAddress and optionally returns a
+  TargetHwAddress if it already exists in the ARP cache.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  TargetSwAddress        Pointer to the protocol address to resolve.
+  @param  ResolvedEvent          Pointer to the event that will be signaled when
+                                 the address is resolved or some error occurs.
+  @param  TargetHwAddress        Pointer to the buffer for the resolved hardware
+                                 address in network byte order.
+
+  @retval EFI_SUCCESS            The data is copied from the ARP cache into the
+                                 TargetHwAddress buffer.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. TargetHwAddress is NULL.
+  @retval EFI_ACCESS_DENIED      The requested address is not present in the normal
+                                 ARP cache but is present in the deny address list.
+                                 Outgoing traffic to that address is forbidden.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+  @retval EFI_NOT_READY          The request has been started and is not finished.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpRequest (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT         ResolvedEvent    OPTIONAL,
+  OUT VOID             *TargetHwAddress
+  );
+
+/**
+  This function aborts the previous ARP request (identified by This,  TargetSwAddress
+  and ResolvedEvent) that is issued by EFI_ARP_PROTOCOL.Request().
+  
+  If the request is in the internal ARP request queue, the request is aborted 
+  immediately and its ResolvedEvent is signaled. Only an asynchronous address 
+  request needs to be canceled. If TargeSwAddress and ResolveEvent are both 
+  NULL, all the pending asynchronous requests that have been issued by This 
+  instance will be cancelled and their corresponding events will be signaled.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  TargetSwAddress        Pointer to the protocol address in previous
+                                 request session.
+  @param  ResolvedEvent          Pointer to the event that is used as the
+                                 notification event in previous request session.
+
+  @retval EFI_SUCCESS            The pending request session(s) is/are aborted and
+                                 corresponding event(s) is/are signaled.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. TargetSwAddress is not NULL and
+                                 ResolvedEvent is NULL. TargetSwAddress is NULL and
+                                 ResolvedEvent is not NULL.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+  @retval EFI_NOT_FOUND          The request is not issued by
+                                 EFI_ARP_PROTOCOL.Request().
+
+**/
+EFI_STATUS
+EFIAPI
+ArpCancel (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT         ResolvedEvent    OPTIONAL
+  );
+
+/**
+  Configure the instance using the ConfigData. ConfigData is already validated.
+
+  @param[in]  Instance           Pointer to the instance context data to be
+                                 configured.
+  @param[in]  ConfigData         Pointer to the configuration data used to
+                                 configure the instance.
+
+  @retval EFI_SUCCESS            The instance is configured with the ConfigData.
+  @retval EFI_ACCESS_DENIED      The instance is already configured and the
+                                 ConfigData tries to reset some unchangeable
+                                 fields.
+  @retval EFI_INVALID_PARAMETER  The ConfigData provides a non-unicast IPv4 address
+                                 when the SwAddressType is IPv4.
+  @retval EFI_OUT_OF_RESOURCES   The instance fails to configure due to memory
+                                 limitation.
+
+**/
+EFI_STATUS
+ArpConfigureInstance (
+  IN ARP_INSTANCE_DATA    *Instance,
+  IN EFI_ARP_CONFIG_DATA  *ConfigData OPTIONAL
+  );
+
+/**
+  Find the CacheEntry, using ProtocolAddress or HardwareAddress or both, as the keyword,
+  in the DeniedCacheTable.
+
+  @param[in]  ArpService             Pointer to the arp service context data.
+  @param[in]  ProtocolAddress        Pointer to the protocol address.
+  @param[in]  HardwareAddress        Pointer to the hardware address.
+
+  @return Pointer to the matched cache entry, if NULL no match is found.
+
+**/
+ARP_CACHE_ENTRY *
+ArpFindDeniedCacheEntry (
+  IN ARP_SERVICE_DATA  *ArpService,
+  IN NET_ARP_ADDRESS   *ProtocolAddress OPTIONAL,
+  IN NET_ARP_ADDRESS   *HardwareAddress OPTIONAL
+  );
+
+/**
+  Find the CacheEntry which matches the requirements in the specified CacheTable.
+
+  @param[in]  CacheTable             Pointer to the arp cache table.
+  @param[in]  StartEntry             Pointer to the start entry this search begins with
+                                     in the cache table.
+  @param[in]  FindOpType             The search type.
+  @param[in]  ProtocolAddress        Pointer to the protocol address to match.
+  @param[in]  HardwareAddress        Pointer to the hardware address to match.
+
+  @return Pointer to the matched arp cache entry, if NULL, no match is found.
+
+**/
+ARP_CACHE_ENTRY *
+ArpFindNextCacheEntryInTable (
+  IN LIST_ENTRY        *CacheTable,
+  IN LIST_ENTRY        *StartEntry,
+  IN FIND_OPTYPE       FindOpType,
+  IN NET_ARP_ADDRESS   *ProtocolAddress OPTIONAL,
+  IN NET_ARP_ADDRESS   *HardwareAddress OPTIONAL
+  );
+
+/**
+  Allocate a cache entry and initialize it.
+
+  @param[in]  Instance               Pointer to the instance context data.
+
+  @return Pointer to the new created cache entry.
+
+**/
+ARP_CACHE_ENTRY *
+ArpAllocCacheEntry (
+  IN ARP_INSTANCE_DATA  *Instance
+  );
+
+/**
+  Fill the addresses in the CacheEntry using the information passed in by
+  HwAddr and SwAddr.
+
+  @param[in]  CacheEntry             Pointer to the cache entry.
+  @param[in]  HwAddr                 Pointer to the software address.
+  @param[in]  SwAddr                 Pointer to the hardware address.
+
+  @return None.
+
+**/
+VOID
+ArpFillAddressInCacheEntry (
+  IN ARP_CACHE_ENTRY  *CacheEntry,
+  IN NET_ARP_ADDRESS  *HwAddr OPTIONAL,
+  IN NET_ARP_ADDRESS  *SwAddr OPTIONAL
+  );
+
+/**
+  Turn the CacheEntry into the resolved status.
+
+  @param[in]  CacheEntry             Pointer to the resolved cache entry.
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  UserEvent              Pointer to the UserEvent to notify.
+
+  @return The count of notifications sent to the instance.
+
+**/
+UINTN
+ArpAddressResolved (
+  IN ARP_CACHE_ENTRY    *CacheEntry,
+  IN ARP_INSTANCE_DATA  *Instance OPTIONAL,
+  IN EFI_EVENT          UserEvent OPTIONAL
+  );
+
+/**
+  Delete cache entries in all the cache tables.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  BySwAddress            Delete the cache entry by software address or by
+                                     hardware address.
+  @param[in]  AddressBuffer          Pointer to the buffer containing the address to
+                                     match for the deletion.
+  @param[in]  Force                  This deletion is forced or not.
+
+  @return The count of the deleted cache entries.
+
+**/
+UINTN
+ArpDeleteCacheEntry (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN BOOLEAN            BySwAddress,
+  IN UINT8              *AddressBuffer OPTIONAL,
+  IN BOOLEAN            Force
+  );
+
+/**
+  Send out an arp frame using the CachEntry and the ArpOpCode.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  CacheEntry             Pointer to the configuration data used to
+                                     configure the instance.
+  @param[in]  ArpOpCode              The opcode used to send out this Arp frame, either
+                                     request or reply.
+
+  @return None.
+
+**/
+VOID
+ArpSendFrame (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN ARP_CACHE_ENTRY    *CacheEntry,
+  IN UINT16             ArpOpCode
+  );
+
+/**
+  Initialize the instance context data.
+
+  @param[in]   ArpService        Pointer to the arp service context data this
+                                 instance belongs to.
+  @param[out]  Instance          Pointer to the instance context data.
+
+  @return None.
+
+**/
+VOID
+ArpInitInstance (
+  IN  ARP_SERVICE_DATA   *ArpService,
+  OUT ARP_INSTANCE_DATA  *Instance
+  );
+
+/**
+  Process the Arp packets received from Mnp, the procedure conforms to RFC826.
+
+  @param[in]  Context            Pointer to the context data registerd to the
+                                 Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameRcvdDpc (
+  IN VOID       *Context
+  );
+
+/**
+  Queue ArpOnFrameRcvdDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameRcvd (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Process the already sent arp packets.
+  
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameSentDpc (
+  IN VOID       *Context
+  );
+
+/**
+  Request ArpOnFrameSentDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpOnFrameSent (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Process the arp cache olding and drive the retrying arp requests.
+
+  @param[in]  Event                  The Event this notify function registered to.
+  @param[in]  Context                Pointer to the context data registerd to the
+                                     Event.
+
+  @return None.
+
+**/
+VOID
+EFIAPI
+ArpTimerHandler (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Cancel the arp request.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  TargetSwAddress        Pointer to the buffer containing the target
+                                     software address to match the arp request.
+  @param[in]  UserEvent              The user event used to notify this request
+                                     cancellation.
+
+  @return The count of the cancelled requests.
+
+**/
+UINTN
+ArpCancelRequest (
+  IN ARP_INSTANCE_DATA  *Instance,
+  IN VOID               *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT          UserEvent        OPTIONAL
+  );
+
+/**
+  Find the cache entry in the cache table.
+
+  @param[in]  Instance           Pointer to the instance context data.
+  @param[in]  BySwAddress        Set to TRUE to look for matching software protocol
+                                 addresses. Set to FALSE to look for matching
+                                 hardware protocol addresses.
+  @param[in]  AddressBuffer      Pointer to address buffer. Set to NULL to match
+                                 all addresses.
+  @param[out] EntryLength        The size of an entry in the entries buffer.
+  @param[out] EntryCount         The number of ARP cache entries that are found by
+                                 the specified criteria.
+  @param[out] Entries            Pointer to the buffer that will receive the ARP
+                                 cache entries.
+  @param[in]  Refresh            Set to TRUE to refresh the timeout value of the
+                                 matching ARP cache entry.
+
+  @retval EFI_SUCCESS            The requested ARP cache entries are copied into
+                                 the buffer.
+  @retval EFI_NOT_FOUND          No matching entries found.
+  @retval EFI_OUT_OF_RESOURCE    There is a memory allocation failure.
+
+**/
+EFI_STATUS
+ArpFindCacheEntry (
+  IN ARP_INSTANCE_DATA   *Instance,
+  IN BOOLEAN             BySwAddress,
+  IN VOID                *AddressBuffer OPTIONAL,
+  OUT UINT32             *EntryLength   OPTIONAL,
+  OUT UINT32             *EntryCount    OPTIONAL,
+  OUT EFI_ARP_FIND_DATA  **Entries      OPTIONAL,
+  IN BOOLEAN             Refresh
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpMain.c b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpMain.c
new file mode 100644
index 0000000000..abe02c1af4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ArpMain.c
@@ -0,0 +1,745 @@
+/** @file
+  Implementation of EFI Address Resolution Protocol (ARP) Protocol interface functions.
+  
+Copyright (c) 2006 - 2009, 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<BR>
+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 "ArpImpl.h"
+
+
+/**
+  This function is used to assign a station address to the ARP cache for this instance
+  of the ARP driver.
+  
+  Each ARP instance has one station address. The EFI_ARP_PROTOCOL driver will 
+  respond to ARP requests that match this registered station address. A call to 
+  this function with the ConfigData field set to NULL will reset this ARP instance.
+  
+  Once a protocol type and station address have been assigned to this ARP instance, 
+  all the following ARP functions will use this information. Attempting to change 
+  the protocol type or station address to a configured ARP instance will result in errors.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  ConfigData             Pointer to the EFI_ARP_CONFIG_DATA structure.
+
+  @retval EFI_SUCCESS            The new station address was successfully
+                                 registered.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. SwAddressLength is zero when
+                                 ConfigData is not NULL. StationAddress is NULL
+                                 when ConfigData is not NULL.
+  @retval EFI_ACCESS_DENIED      The SwAddressType, SwAddressLength, or
+                                 StationAddress is different from the one that is
+                                 already registered.
+  @retval EFI_OUT_OF_RESOURCES   Storage for the new StationAddress could not be
+                                 allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpConfigure (
+  IN EFI_ARP_PROTOCOL     *This,
+  IN EFI_ARP_CONFIG_DATA  *ConfigData OPTIONAL
+  )
+{
+  EFI_STATUS         Status;
+  ARP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((ConfigData != NULL) &&
+    ((ConfigData->SwAddressLength == 0) ||
+    (ConfigData->StationAddress == NULL) ||
+    (ConfigData->SwAddressType <= 1500))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Configure this instance, the ConfigData has already passed the basic checks.
+  //
+  Status = ArpConfigureInstance (Instance, ConfigData);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  This function is used to insert entries into the ARP cache.
+
+  ARP cache entries are typically inserted and updated by network protocol drivers 
+  as network traffic is processed. Most ARP cache entries will time out and be 
+  deleted if the network traffic stops. ARP cache entries that were inserted 
+  by the Add() function may be static (will not time out) or dynamic (will time out).
+  Default ARP cache timeout values are not covered in most network protocol 
+  specifications (although RFC 1122 comes pretty close) and will only be 
+  discussed in general in this specification. The timeout values that are 
+  used in the EFI Sample Implementation should be used only as a guideline. 
+  Final product implementations of the EFI network stack should be tuned for 
+  their expected network environments.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  DenyFlag               Set to TRUE if this entry is a deny entry. Set to
+                                 FALSE if this  entry is a normal entry.
+  @param  TargetSwAddress        Pointer to a protocol address to add (or deny).
+                                 May be set to NULL if DenyFlag is TRUE.
+  @param  TargetHwAddress        Pointer to a hardware address to add (or deny).
+                                 May be set to NULL if DenyFlag is TRUE.
+  @param  TimeoutValue           Time in 100-ns units that this entry will remain
+                                 in the ARP cache. A value of zero means that the
+                                 entry is permanent. A nonzero value will override
+                                 the one given by Configure() if the entry to be
+                                 added is a dynamic entry.
+  @param  Overwrite              If TRUE, the matching cache entry will be
+                                 overwritten with the supplied parameters. If
+                                 FALSE, EFI_ACCESS_DENIED is returned if the
+                                 corresponding cache entry already exists.
+
+  @retval EFI_SUCCESS            The entry has been added or updated.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. DenyFlag is FALSE and
+                                 TargetHwAddress is NULL. DenyFlag is FALSE and
+                                 TargetSwAddress is NULL. TargetHwAddress is NULL
+                                 and TargetSwAddress is NULL. Both TargetSwAddress
+                                 and TargetHwAddress are not NULL when DenyFlag is
+                                 TRUE.
+  @retval EFI_OUT_OF_RESOURCES   The new ARP cache entry could not be allocated.
+  @retval EFI_ACCESS_DENIED      The ARP cache entry already exists and Overwrite
+                                 is not true.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpAdd (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN BOOLEAN           DenyFlag,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN VOID              *TargetHwAddress OPTIONAL,
+  IN UINT32            TimeoutValue,
+  IN BOOLEAN           Overwrite
+  )
+{
+  EFI_STATUS               Status;
+  ARP_INSTANCE_DATA        *Instance;
+  ARP_SERVICE_DATA         *ArpService;
+  ARP_CACHE_ENTRY          *CacheEntry;
+  EFI_SIMPLE_NETWORK_MODE  *SnpMode;
+  NET_ARP_ADDRESS          MatchAddress[2];
+  EFI_TPL                  OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((!DenyFlag) && ((TargetHwAddress == NULL) || (TargetSwAddress == NULL))) ||
+    (DenyFlag && (TargetHwAddress != NULL) && (TargetSwAddress != NULL)) ||
+    ((TargetHwAddress == NULL) && (TargetSwAddress == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  Status     = EFI_SUCCESS;
+  ArpService = Instance->ArpService;
+  SnpMode    = &Instance->ArpService->SnpMode;
+
+  //
+  // Fill the hardware address part in the MatchAddress.
+  //
+  MatchAddress[Hardware].Type       = SnpMode->IfType;
+  MatchAddress[Hardware].Length     = (UINT8) SnpMode->HwAddressSize;
+  MatchAddress[Hardware].AddressPtr = TargetHwAddress;
+
+  //
+  // Fill the software address part in the MatchAddress.
+  //
+  MatchAddress[Protocol].Type       = Instance->ConfigData.SwAddressType;
+  MatchAddress[Protocol].Length     = Instance->ConfigData.SwAddressLength;
+  MatchAddress[Protocol].AddressPtr = TargetSwAddress;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // See whether the entry to add exists. Check the DeinedCacheTable first.
+  //
+  CacheEntry = ArpFindDeniedCacheEntry (
+                 ArpService,
+                 &MatchAddress[Protocol],
+                 &MatchAddress[Hardware]
+                 );
+
+  if (CacheEntry == NULL) {
+    //
+    // Check the ResolvedCacheTable
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->ResolvedCacheTable,
+                   NULL,
+                   ByBoth,
+                   &MatchAddress[Protocol],
+                   &MatchAddress[Hardware]
+                   );
+  }
+
+  if ((CacheEntry != NULL) && !Overwrite) {
+    //
+    // The entry to add exists, if not Overwirte, deny this add request.
+    //
+    Status = EFI_ACCESS_DENIED;
+    goto UNLOCK_EXIT;
+  }
+
+  if ((CacheEntry == NULL) && (TargetSwAddress != NULL)) {
+    //
+    // Check whether there are pending requests matching the entry to be added.
+    //
+    CacheEntry = ArpFindNextCacheEntryInTable (
+                   &ArpService->PendingRequestTable,
+                   NULL,
+                   ByProtoAddress,
+                   &MatchAddress[Protocol],
+                   NULL
+                   );
+  }
+
+  if (CacheEntry != NULL) {
+    //
+    // Remove it from the Table.
+    //
+    RemoveEntryList (&CacheEntry->List);
+  } else {
+    //
+    // It's a new entry, allocate memory for the entry.
+    //
+    CacheEntry = ArpAllocCacheEntry (Instance);
+
+    if (CacheEntry == NULL) {
+      DEBUG ((EFI_D_ERROR, "ArpAdd: Failed to allocate pool for CacheEntry.\n"));
+      Status = EFI_OUT_OF_RESOURCES;
+      goto UNLOCK_EXIT;
+    }
+  }
+
+  //
+  // Overwrite these parameters.
+  //
+  CacheEntry->DefaultDecayTime = TimeoutValue;
+  CacheEntry->DecayTime        = TimeoutValue;
+
+  //
+  // Fill in the addresses.
+  //
+  ArpFillAddressInCacheEntry (
+    CacheEntry,
+    &MatchAddress[Hardware],
+    &MatchAddress[Protocol]
+    );
+
+  //
+  // Inform the user if there is any.
+  //
+  ArpAddressResolved (CacheEntry, NULL, NULL);
+
+  //
+  // Add this CacheEntry to the corresponding CacheTable.
+  //
+  if (DenyFlag) {
+    InsertHeadList (&ArpService->DeniedCacheTable, &CacheEntry->List);
+  } else {
+    InsertHeadList (&ArpService->ResolvedCacheTable, &CacheEntry->List);
+  }
+
+UNLOCK_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  This function searches the ARP cache for matching entries and allocates a buffer into
+  which those entries are copied.
+  
+  The first part of the allocated buffer is EFI_ARP_FIND_DATA, following which 
+  are protocol address pairs and hardware address pairs.
+  When finding a specific protocol address (BySwAddress is TRUE and AddressBuffer 
+  is not NULL), the ARP cache timeout for the found entry is reset if Refresh is 
+  set to TRUE. If the found ARP cache entry is a permanent entry, it is not 
+  affected by Refresh.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  BySwAddress            Set to TRUE to look for matching software protocol
+                                 addresses. Set to FALSE to look for matching
+                                 hardware protocol addresses.
+  @param  AddressBuffer          Pointer to address buffer. Set to NULL to match
+                                 all addresses.
+  @param  EntryLength            The size of an entry in the entries buffer.
+  @param  EntryCount             The number of ARP cache entries that are found by
+                                 the specified criteria.
+  @param  Entries                Pointer to the buffer that will receive the ARP
+                                 cache entries.
+  @param  Refresh                Set to TRUE to refresh the timeout value of the
+                                 matching ARP cache entry.
+
+  @retval EFI_SUCCESS            The requested ARP cache entries were copied into
+                                 the buffer.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. Both EntryCount and EntryLength are
+                                 NULL, when Refresh is FALSE.
+  @retval EFI_NOT_FOUND          No matching entries were found.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpFind (
+  IN EFI_ARP_PROTOCOL    *This,
+  IN BOOLEAN             BySwAddress,
+  IN VOID                *AddressBuffer OPTIONAL,
+  OUT UINT32             *EntryLength   OPTIONAL,
+  OUT UINT32             *EntryCount    OPTIONAL,
+  OUT EFI_ARP_FIND_DATA  **Entries      OPTIONAL,
+  IN BOOLEAN             Refresh
+  )
+{
+  EFI_STATUS         Status;
+  ARP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) ||
+    (!Refresh && (EntryCount == NULL) && (EntryLength == NULL)) ||
+    ((Entries != NULL) && ((EntryLength == NULL) || (EntryCount == NULL)))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance   = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // All the check passed, find the cache entries now.
+  //
+  Status = ArpFindCacheEntry (
+             Instance,
+             BySwAddress,
+             AddressBuffer,
+             EntryLength,
+             EntryCount,
+             Entries,
+             Refresh
+             );
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  This function removes specified ARP cache entries.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  BySwAddress            Set to TRUE to delete matching protocol addresses.
+                                 Set to FALSE to delete matching hardware
+                                 addresses.
+  @param  AddressBuffer          Pointer to the address buffer that is used as a
+                                 key to look for the cache entry. Set to NULL to
+                                 delete all entries.
+
+  @retval EFI_SUCCESS            The entry was removed from the ARP cache.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          The specified deletion key was not found.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpDelete (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN BOOLEAN           BySwAddress,
+  IN VOID              *AddressBuffer OPTIONAL
+  )
+{
+  ARP_INSTANCE_DATA  *Instance;
+  UINTN              Count;
+  EFI_TPL            OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Delete the specified cache entries.
+  //
+  Count = ArpDeleteCacheEntry (Instance, BySwAddress, AddressBuffer, TRUE);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return (Count == 0) ? EFI_NOT_FOUND : EFI_SUCCESS;
+}
+
+
+/**
+  This function delete all dynamic entries from the ARP cache that match the specified
+  software protocol type.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+
+  @retval EFI_SUCCESS            The cache has been flushed.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          There are no matching dynamic cache entries.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpFlush (
+  IN EFI_ARP_PROTOCOL  *This
+  )
+{
+  ARP_INSTANCE_DATA  *Instance;
+  UINTN              Count;
+  EFI_TPL            OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Delete the dynamic entries from the cache table.
+  //
+  Count = ArpDeleteCacheEntry (Instance, FALSE, NULL, FALSE);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return (Count == 0) ? EFI_NOT_FOUND : EFI_SUCCESS;
+}
+
+
+/**
+  This function tries to resolve the TargetSwAddress and optionally returns a
+  TargetHwAddress if it already exists in the ARP cache.
+
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  TargetSwAddress        Pointer to the protocol address to resolve.
+  @param  ResolvedEvent          Pointer to the event that will be signaled when
+                                 the address is resolved or some error occurs.
+  @param  TargetHwAddress        Pointer to the buffer for the resolved hardware
+                                 address in network byte order.
+
+  @retval EFI_SUCCESS            The data is copied from the ARP cache into the
+                                 TargetHwAddress buffer.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. TargetHwAddress is NULL.
+  @retval EFI_ACCESS_DENIED      The requested address is not present in the normal
+                                 ARP cache but is present in the deny address list.
+                                 Outgoing traffic to that address is forbidden.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+  @retval EFI_NOT_READY          The request has been started and is not finished.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpRequest (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT         ResolvedEvent    OPTIONAL,
+  OUT VOID             *TargetHwAddress
+  )
+{
+  EFI_STATUS               Status;
+  ARP_INSTANCE_DATA        *Instance;
+  ARP_SERVICE_DATA         *ArpService;
+  EFI_SIMPLE_NETWORK_MODE  *SnpMode;
+  ARP_CACHE_ENTRY          *CacheEntry;
+  NET_ARP_ADDRESS          HardwareAddress;
+  NET_ARP_ADDRESS          ProtocolAddress;
+  USER_REQUEST_CONTEXT     *RequestContext;
+  EFI_TPL                  OldTpl;
+
+  if ((This == NULL) || (TargetHwAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  Status     = EFI_SUCCESS;
+  ArpService = Instance->ArpService;
+  SnpMode    = &ArpService->SnpMode;
+
+  if ((TargetSwAddress == NULL) ||
+    ((Instance->ConfigData.SwAddressType == IPV4_ETHER_PROTO_TYPE) &&
+    IP4_IS_LOCAL_BROADCAST (*((UINT32 *)TargetSwAddress)))) {
+    //
+    // Return the hardware broadcast address.
+    //
+    CopyMem (TargetHwAddress, &SnpMode->BroadcastAddress, SnpMode->HwAddressSize);
+
+    goto SIGNAL_USER;
+  }
+
+  if ((Instance->ConfigData.SwAddressType == IPV4_ETHER_PROTO_TYPE) &&
+    IP4_IS_MULTICAST (NTOHL (*((UINT32 *)TargetSwAddress)))) {
+    //
+    // If the software address is an IPv4 multicast address, invoke Mnp to
+    // resolve the address.
+    //
+    Status = ArpService->Mnp->McastIpToMac (
+                                ArpService->Mnp,
+                                FALSE,
+                                TargetSwAddress,
+                                TargetHwAddress
+                                );
+    goto SIGNAL_USER;
+  }
+
+  HardwareAddress.Type       = SnpMode->IfType;
+  HardwareAddress.Length     = (UINT8)SnpMode->HwAddressSize;
+  HardwareAddress.AddressPtr = NULL;
+
+  ProtocolAddress.Type       = Instance->ConfigData.SwAddressType;
+  ProtocolAddress.Length     = Instance->ConfigData.SwAddressLength;
+  ProtocolAddress.AddressPtr = TargetSwAddress;
+
+  //
+  // Initialize the TargetHwAddrss to a zero address.
+  //
+  ZeroMem (TargetHwAddress, SnpMode->HwAddressSize);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Check whether the software address is in the denied table.
+  //
+  CacheEntry = ArpFindDeniedCacheEntry (ArpService, &ProtocolAddress, NULL);
+  if (CacheEntry != NULL) {
+    Status = EFI_ACCESS_DENIED;
+    goto UNLOCK_EXIT;
+  }
+
+  //
+  // Check whether the software address is already resolved.
+  //
+  CacheEntry = ArpFindNextCacheEntryInTable (
+                 &ArpService->ResolvedCacheTable,
+                 NULL,
+                 ByProtoAddress,
+                 &ProtocolAddress,
+                 NULL
+                 );
+  if (CacheEntry != NULL) {
+    //
+    // Resolved, copy the address into the user buffer.
+    //
+    CopyMem (
+      TargetHwAddress,
+      CacheEntry->Addresses[Hardware].AddressPtr,
+      CacheEntry->Addresses[Hardware].Length
+      );
+
+    goto UNLOCK_EXIT;
+  }
+
+  if (ResolvedEvent == NULL) {
+    Status = EFI_NOT_READY;
+    goto UNLOCK_EXIT;
+  }
+
+  //
+  // Create a request context for this arp request.
+  //
+  RequestContext = AllocatePool (sizeof(USER_REQUEST_CONTEXT));
+  if (RequestContext == NULL) {
+    DEBUG ((EFI_D_ERROR, "ArpRequest: Allocate memory for RequestContext failed.\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto UNLOCK_EXIT;
+  }
+
+  RequestContext->Instance         = Instance;
+  RequestContext->UserRequestEvent = ResolvedEvent;
+  RequestContext->UserHwAddrBuffer = TargetHwAddress;
+  InitializeListHead (&RequestContext->List);
+
+  //
+  // Check whether there is a same request.
+  //
+  CacheEntry = ArpFindNextCacheEntryInTable (
+                 &ArpService->PendingRequestTable,
+                 NULL,
+                 ByProtoAddress,
+                 &ProtocolAddress,
+                 NULL
+                 );
+  if (CacheEntry != NULL) {
+
+    CacheEntry->NextRetryTime = Instance->ConfigData.RetryTimeOut;
+    CacheEntry->RetryCount    = Instance->ConfigData.RetryCount;
+  } else {
+    //
+    // Allocate a cache entry for this request.
+    //
+    CacheEntry = ArpAllocCacheEntry (Instance);
+    if (CacheEntry == NULL) {
+      DEBUG ((EFI_D_ERROR, "ArpRequest: Allocate memory for CacheEntry failed.\n"));
+      FreePool (RequestContext);
+
+      Status = EFI_OUT_OF_RESOURCES;
+      goto UNLOCK_EXIT;
+    }
+
+    //
+    // Fill the software address.
+    //
+    ArpFillAddressInCacheEntry (CacheEntry, &HardwareAddress, &ProtocolAddress);
+
+    //
+    // Add this entry into the PendingRequestTable.
+    //
+    InsertTailList (&ArpService->PendingRequestTable, &CacheEntry->List);
+  }
+
+  //
+  // Link this request context into the cache entry.
+  //
+  InsertHeadList (&CacheEntry->UserRequestList, &RequestContext->List);
+
+  //
+  // Send out the ARP Request frame.
+  //
+  ArpSendFrame (Instance, CacheEntry, ARP_OPCODE_REQUEST);
+  Status = EFI_NOT_READY;
+
+UNLOCK_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+SIGNAL_USER:
+
+  if ((ResolvedEvent != NULL) && (Status == EFI_SUCCESS)) {
+    gBS->SignalEvent (ResolvedEvent);
+
+    //
+    // Dispatch the DPC queued by the NotifyFunction of ResolvedEvent.
+    //
+    DispatchDpc ();
+  }
+
+  return Status;
+}
+
+
+/**
+  This function aborts the previous ARP request (identified by This,  TargetSwAddress
+  and ResolvedEvent) that is issued by EFI_ARP_PROTOCOL.Request().
+  
+  If the request is in the internal ARP request queue, the request is aborted 
+  immediately and its ResolvedEvent is signaled. Only an asynchronous address 
+  request needs to be canceled. If TargeSwAddress and ResolveEvent are both 
+  NULL, all the pending asynchronous requests that have been issued by This 
+  instance will be cancelled and their corresponding events will be signaled.
+  
+  @param  This                   Pointer to the EFI_ARP_PROTOCOL instance.
+  @param  TargetSwAddress        Pointer to the protocol address in previous
+                                 request session.
+  @param  ResolvedEvent          Pointer to the event that is used as the
+                                 notification event in previous request session.
+
+  @retval EFI_SUCCESS            The pending request session(s) is/are aborted and
+                                 corresponding event(s) is/are signaled.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 This is NULL. TargetSwAddress is not NULL and
+                                 ResolvedEvent is NULL. TargetSwAddress is NULL and
+                                 ResolvedEvent is not NULL.
+  @retval EFI_NOT_STARTED        The ARP driver instance has not been configured.
+  @retval EFI_NOT_FOUND          The request is not issued by
+                                 EFI_ARP_PROTOCOL.Request().
+
+**/
+EFI_STATUS
+EFIAPI
+ArpCancel (
+  IN EFI_ARP_PROTOCOL  *This,
+  IN VOID              *TargetSwAddress OPTIONAL,
+  IN EFI_EVENT         ResolvedEvent    OPTIONAL
+  )
+{
+  ARP_INSTANCE_DATA  *Instance;
+  UINTN              Count;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) ||
+    ((TargetSwAddress != NULL) && (ResolvedEvent == NULL)) ||
+    ((TargetSwAddress == NULL) && (ResolvedEvent != NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = ARP_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Cancel the specified request.
+  //
+  Count = ArpCancelRequest (Instance, TargetSwAddress, ResolvedEvent);
+
+  //
+  // Dispatch the DPCs queued by the NotifyFunction of the events signaled
+  // by ArpCancleRequest.
+  //
+  DispatchDpc ();
+
+  gBS->RestoreTPL (OldTpl);
+
+  return (Count == 0) ? EFI_NOT_FOUND : EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/ArpDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/ArpDxe/ComponentName.c
new file mode 100644
index 0000000000..ee47fb1ef7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/ArpDxe/ComponentName.c
@@ -0,0 +1,225 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for ArpDxe driver.
+
+Copyright (c) 2006 - 2012, 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<BR>
+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 "ArpDriver.h"
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gArpComponentName = {
+  ArpComponentNameGetDriverName,
+  ArpComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gArpComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) ArpComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) ArpComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mArpDriverNameTable[] = {
+  { "eng;en", L"ARP Network Service Driver" },
+  { NULL, NULL }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mArpControllerNameTable[] = {
+  { "eng;en", L"ARP Controller" },
+  { NULL, NULL }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mArpDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gArpComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+ArpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_ARP_PROTOCOL              *Arp;
+
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiManagedNetworkProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle  
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiArpProtocolGuid,
+                 (VOID **)&Arp,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mArpControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gArpComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/ComponentName.c
new file mode 100644
index 0000000000..2ea07608f1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/ComponentName.c
@@ -0,0 +1,437 @@
+/** @file
+
+Copyright (c) 2006 - 2012, 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 "Dhcp4Impl.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gDhcp4ComponentName = {
+  DhcpComponentNameGetDriverName,
+  DhcpComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gDhcp4ComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) DhcpComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) DhcpComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mDhcpDriverNameTable[] = {
+  {
+    "eng;en",
+    L"DHCP Protocol Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE    *gDhcpControllerNameTable = NULL;
+
+CHAR16 *mDhcp4ControllerName[] = {
+  L"DHCPv4 (State=0, Stopped)",
+  L"DHCPv4 (State=1, Init)",
+  L"DHCPv4 (State=2, Selecting)",
+  L"DHCPv4 (State=3, Requesting)",
+  L"DHCPv4 (State=4, Bound)",
+  L"DHCPv4 (State=5, Renewing)",
+  L"DHCPv4 (State=6, Rebinding)",
+  L"DHCPv4 (State=7, InitReboot)",
+  L"DHCPv4 (State=8, Rebooting)"
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mDhcpDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gDhcp4ComponentName)
+           );
+}
+
+/**
+  Update the component name for the Dhcp4 child handle.
+
+  @param  Dhcp4[in]               A pointer to the EFI_DHCP4_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS             Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER   The input parameter is invalid.
+  @retval EFI_DEVICE_ERROR        DHCP is in unknown state.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN     EFI_DHCP4_PROTOCOL             *Dhcp4
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_DHCP4_MODE_DATA              Dhcp4ModeData;
+
+  if (Dhcp4 == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Format the child name into the string buffer.
+  //
+  Status = Dhcp4->GetModeData (Dhcp4, &Dhcp4ModeData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  if (gDhcpControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gDhcpControllerNameTable);
+    gDhcpControllerNameTable = NULL;
+  }
+  
+  if (Dhcp4ModeData.State > Dhcp4Rebooting) {
+    return EFI_DEVICE_ERROR;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gDhcp4ComponentName.SupportedLanguages,
+             &gDhcpControllerNameTable,
+             mDhcp4ControllerName[Dhcp4ModeData.State],
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gDhcp4ComponentName2.SupportedLanguages,
+           &gDhcpControllerNameTable,
+           mDhcp4ControllerName[Dhcp4ModeData.State],
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_DHCP4_PROTOCOL            *Dhcp4;
+
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiUdp4ProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle  
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiDhcp4ProtocolGuid,
+                  (VOID **)&Dhcp4,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Dhcp4);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gDhcpControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gDhcp4ComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.c b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.c
new file mode 100644
index 0000000000..9253df2b3f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.c
@@ -0,0 +1,738 @@
+/** @file
+
+Copyright (c) 2006 - 2015, 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 "Dhcp4Impl.h"
+#include "Dhcp4Driver.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gDhcp4DriverBinding = {
+  Dhcp4DriverBindingSupported,
+  Dhcp4DriverBindingStart,
+  Dhcp4DriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_SERVICE_BINDING_PROTOCOL mDhcp4ServiceBindingTemplate = {
+  Dhcp4ServiceBindingCreateChild,
+  Dhcp4ServiceBindingDestroyChild
+};
+
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers.
+
+  Entry point of the DHCP driver to install various protocols.
+
+  @param[in]  ImageHandle           The firmware allocated handle for the UEFI image.
+  @param[in]  SystemTable           A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverEntryPoint (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gDhcp4DriverBinding,
+           ImageHandle,
+           &gDhcp4ComponentName,
+           &gDhcp4ComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUdp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+
+
+/**
+  Configure the default UDP child to receive all the DHCP traffics
+  on this network interface.
+
+  @param[in]  UdpIo                  The UDP IO to configure
+  @param[in]  Context                The context to the function
+
+  @retval EFI_SUCCESS            The UDP IO is successfully configured.
+  @retval Others                 Failed to configure the UDP child.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpConfigUdpIo (
+  IN UDP_IO                 *UdpIo,
+  IN VOID                   *Context
+  )
+{
+  EFI_UDP4_CONFIG_DATA      UdpConfigData;
+
+  UdpConfigData.AcceptBroadcast           = TRUE;
+  UdpConfigData.AcceptPromiscuous         = FALSE;
+  UdpConfigData.AcceptAnyPort             = FALSE;
+  UdpConfigData.AllowDuplicatePort        = TRUE;
+  UdpConfigData.TypeOfService             = 0;
+  UdpConfigData.TimeToLive                = 64;
+  UdpConfigData.DoNotFragment             = FALSE;
+  UdpConfigData.ReceiveTimeout            = 0;
+  UdpConfigData.TransmitTimeout           = 0;
+
+  UdpConfigData.UseDefaultAddress         = FALSE;
+  UdpConfigData.StationPort               = DHCP_CLIENT_PORT;
+  UdpConfigData.RemotePort                = DHCP_SERVER_PORT;
+
+  ZeroMem (&UdpConfigData.StationAddress, sizeof (EFI_IPv4_ADDRESS));
+  ZeroMem (&UdpConfigData.SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  ZeroMem (&UdpConfigData.RemoteAddress, sizeof (EFI_IPv4_ADDRESS));
+
+  return UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, &UdpConfigData);;
+}
+
+
+
+/**
+  Destroy the DHCP service. The Dhcp4 service may be partly initialized,
+  or partly destroyed. If a resource is destroyed, it is marked as so in
+  case the destroy failed and being called again later.
+
+  @param[in]  DhcpSb                 The DHCP service instance to destroy.
+
+  @retval EFI_SUCCESS            Always return success.
+
+**/
+EFI_STATUS
+Dhcp4CloseService (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  DhcpCleanLease (DhcpSb);
+
+  if (DhcpSb->UdpIo != NULL) {
+    UdpIoFreeIo (DhcpSb->UdpIo);
+    DhcpSb->UdpIo = NULL;
+  }
+
+  if (DhcpSb->Timer != NULL) {
+    gBS->SetTimer (DhcpSb->Timer, TimerCancel, 0);
+    gBS->CloseEvent (DhcpSb->Timer);
+
+    DhcpSb->Timer = NULL;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Create a new DHCP service binding instance for the controller.
+
+  @param[in]  Controller             The controller to install DHCP service binding
+                                     protocol onto
+  @param[in]  ImageHandle            The driver's image handle
+  @param[out] Service                The variable to receive the created DHCP service
+                                     instance.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource .
+  @retval EFI_SUCCESS            The DHCP service instance is created.
+  @retval other                  Other error occurs.
+
+**/
+EFI_STATUS
+Dhcp4CreateService (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            ImageHandle,
+  OUT DHCP_SERVICE          **Service
+  )
+{
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+
+  *Service  = NULL;
+  DhcpSb    = AllocateZeroPool (sizeof (DHCP_SERVICE));
+
+  if (DhcpSb == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DhcpSb->Signature       = DHCP_SERVICE_SIGNATURE;
+  DhcpSb->ServiceState    = DHCP_UNCONFIGED;
+  DhcpSb->Controller      = Controller;
+  DhcpSb->Image           = ImageHandle;
+  InitializeListHead (&DhcpSb->Children);
+  DhcpSb->DhcpState       = Dhcp4Stopped;
+  DhcpSb->Xid             = NET_RANDOM (NetRandomInitSeed ());
+  CopyMem (
+    &DhcpSb->ServiceBinding,
+    &mDhcp4ServiceBindingTemplate,
+    sizeof (EFI_SERVICE_BINDING_PROTOCOL)
+    );
+  //
+  // Create various resources, UdpIo, Timer, and get Mac address
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  DhcpOnTimerTick,
+                  DhcpSb,
+                  &DhcpSb->Timer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  DhcpSb->UdpIo = UdpIoCreateIo (
+                    Controller,
+                    ImageHandle,
+                    DhcpConfigUdpIo,
+                    UDP_IO_UDP4_VERSION,
+                    NULL
+                    );
+
+  if (DhcpSb->UdpIo == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  DhcpSb->HwLen  = (UINT8) DhcpSb->UdpIo->SnpMode.HwAddressSize;
+  DhcpSb->HwType = DhcpSb->UdpIo->SnpMode.IfType;
+  CopyMem (&DhcpSb->Mac, &DhcpSb->UdpIo->SnpMode.CurrentAddress, sizeof (DhcpSb->Mac));
+
+  *Service       = DhcpSb;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  Dhcp4CloseService (DhcpSb);
+  FreePool (DhcpSb);
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+
+  //
+  // First: test for the DHCP4 Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDhcp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (Status == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  Status = Dhcp4CreateService (ControllerHandle, This->DriverBindingHandle, &DhcpSb);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (DhcpSb != NULL);
+
+  //
+  // Start the receiving
+  //
+  Status = UdpIoRecvDatagram (DhcpSb->UdpIo, DhcpInput, DhcpSb, 0);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+  Status = gBS->SetTimer (DhcpSb->Timer, TimerPeriodic, TICKS_PER_SECOND);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Install the Dhcp4ServiceBinding Protocol onto ControlerHandle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiDhcp4ServiceBindingProtocolGuid,
+                  &DhcpSb->ServiceBinding,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return Status;
+
+ON_ERROR:
+  Dhcp4CloseService (DhcpSb);
+  FreePool (DhcpSb);
+  return Status;
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DestroyChildEntry (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  DHCP_PROTOCOL                    *Instance;
+  EFI_SERVICE_BINDING_PROTOCOL     *ServiceBinding;
+
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = NET_LIST_USER_STRUCT_S (Entry, DHCP_PROTOCOL, Link, DHCP_PROTOCOL_SIGNATURE);
+  ServiceBinding = (EFI_SERVICE_BINDING_PROTOCOL *) Context;
+
+  return ServiceBinding->DestroyChild (ServiceBinding, Instance->Handle);
+}
+
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                                children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  DHCP_SERVICE                  *DhcpSb;
+  EFI_HANDLE                    NicHandle;
+  EFI_STATUS                    Status;
+  LIST_ENTRY                    *List;
+  UINTN                         ListLength;
+
+  //
+  // DHCP driver opens UDP child, So, the ControllerHandle is the
+  // UDP child handle. locate the Nic handle first.
+  //
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiUdp4ProtocolGuid);
+
+  if (NicHandle == NULL) {
+    return EFI_SUCCESS;
+  }
+
+   Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiDhcp4ServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  NicHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  DhcpSb = DHCP_SERVICE_FROM_THIS (ServiceBinding);
+  if (!IsListEmpty (&DhcpSb->Children)) {
+    //
+    // Destroy all the children instances before destory the service.
+    //  
+    List = &DhcpSb->Children;
+    Status = NetDestroyLinkList (
+               List,
+               Dhcp4DestroyChildEntry,
+               ServiceBinding,
+               &ListLength
+               );
+    if (EFI_ERROR (Status) || ListLength != 0) {
+      Status = EFI_DEVICE_ERROR;
+    }
+  }
+
+  if (NumberOfChildren == 0 && !IsListEmpty (&DhcpSb->Children)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  if (NumberOfChildren == 0 && IsListEmpty (&DhcpSb->Children)) {
+    //
+    // Destroy the service itself if no child instance left.
+    //
+    DhcpSb->ServiceState = DHCP_DESTROY;
+
+    gBS->UninstallProtocolInterface (
+           NicHandle,
+           &gEfiDhcp4ServiceBindingProtocolGuid,
+           ServiceBinding
+           );
+
+    Dhcp4CloseService (DhcpSb);
+
+    if (gDhcpControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gDhcpControllerNameTable);
+      gDhcpControllerNameTable = NULL;
+    }
+    FreePool (DhcpSb);
+    
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+
+/**
+  Initialize a new DHCP instance.
+
+  @param  DhcpSb                 The dhcp service instance
+  @param  Instance               The dhcp instance to initialize
+
+**/
+VOID
+DhcpInitProtocol (
+  IN     DHCP_SERVICE           *DhcpSb,
+  IN OUT DHCP_PROTOCOL          *Instance
+  )
+{
+  Instance->Signature         = DHCP_PROTOCOL_SIGNATURE;
+  CopyMem (&Instance->Dhcp4Protocol, &mDhcp4ProtocolTemplate, sizeof (Instance->Dhcp4Protocol));
+  InitializeListHead (&Instance->Link);
+  Instance->Handle            = NULL;
+  Instance->Service           = DhcpSb;
+  Instance->InDestroy         = FALSE;
+  Instance->CompletionEvent   = NULL;
+  Instance->RenewRebindEvent  = NULL;
+  Instance->Token             = NULL;
+  Instance->UdpIo             = NULL;
+  Instance->ElaspedTime       = 0;
+  NetbufQueInit (&Instance->ResponseQueue);
+}
+
+
+/**
+  Creates a child handle and installs a protocol.
+
+  The CreateChild() function installs a protocol on ChildHandle.
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle.
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle,
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  )
+{
+  DHCP_SERVICE              *DhcpSb;
+  DHCP_PROTOCOL             *Instance;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  VOID                      *Udp4;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = AllocatePool (sizeof (*Instance));
+
+  if (Instance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DhcpSb = DHCP_SERVICE_FROM_THIS (This);
+  DhcpInitProtocol (DhcpSb, Instance);
+
+  //
+  // Install DHCP4 onto ChildHandle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiDhcp4ProtocolGuid,
+                  &Instance->Dhcp4Protocol,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Instance);
+    return Status;
+  }
+
+  Instance->Handle  = *ChildHandle;
+
+  //
+  // Open the Udp4 protocol BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  DhcpSb->UdpIo->UdpHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Udp4,
+                  gDhcp4DriverBinding.DriverBindingHandle,
+                  Instance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Instance->Handle,
+           &gEfiDhcp4ProtocolGuid,
+           &Instance->Dhcp4Protocol,
+           NULL
+           );
+
+    FreePool (Instance);
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  InsertTailList (&DhcpSb->Children, &Instance->Link);
+  DhcpSb->NumChildren++;
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Destroys a child handle with a protocol installed on it.
+
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  )
+{
+  DHCP_SERVICE              *DhcpSb;
+  DHCP_PROTOCOL             *Instance;
+  EFI_DHCP4_PROTOCOL        *Dhcp;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the private context data structures
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiDhcp4ProtocolGuid,
+                  (VOID **) &Dhcp,
+                  gDhcp4DriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance  = DHCP_INSTANCE_FROM_THIS (Dhcp);
+  DhcpSb    = DHCP_SERVICE_FROM_THIS (This);
+
+  if (Instance->Service != DhcpSb) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // A child can be destroyed more than once. For example,
+  // Dhcp4DriverBindingStop will destroy all of its children.
+  // when caller driver is being stopped, it will destroy the
+  // dhcp child it opens.
+  //
+  if (Instance->InDestroy) {
+    return EFI_SUCCESS;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  Instance->InDestroy = TRUE;
+
+  //
+  // Close the Udp4 protocol.
+  //
+  gBS->CloseProtocol (
+         DhcpSb->UdpIo->UdpHandle,
+         &gEfiUdp4ProtocolGuid,
+         gDhcp4DriverBinding.DriverBindingHandle,
+         ChildHandle
+         );
+
+  //
+  // Uninstall the DHCP4 protocol first to enable a top down destruction.
+  //
+  gBS->RestoreTPL (OldTpl);
+  Status = gBS->UninstallProtocolInterface (
+                  ChildHandle,
+                  &gEfiDhcp4ProtocolGuid,
+                  Dhcp
+                  );
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  if (EFI_ERROR (Status)) {
+    Instance->InDestroy = FALSE;
+
+    gBS->RestoreTPL (OldTpl);
+    return Status;
+  }
+
+  if (DhcpSb->ActiveChild == Instance) {
+    DhcpYieldControl (DhcpSb);
+  }
+
+  RemoveEntryList (&Instance->Link);
+  DhcpSb->NumChildren--;
+
+  if (Instance->UdpIo != NULL) {
+    UdpIoCleanIo (Instance->UdpIo);
+    gBS->CloseProtocol (
+           Instance->UdpIo->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           Instance->Service->Image,
+           Instance->Handle
+           );
+    UdpIoFreeIo (Instance->UdpIo);
+    Instance->UdpIo = NULL;
+    Instance->Token = NULL;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (Instance);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.h b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.h
new file mode 100644
index 0000000000..b4a63ef173
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Driver.h
@@ -0,0 +1,152 @@
+/** @file
+  Header for the DHCP4 driver.
+  
+Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef __EFI_DHCP4_DRIVER_H__
+#define __EFI_DHCP4_DRIVER_H__
+
+extern EFI_COMPONENT_NAME_PROTOCOL  gDhcp4ComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL gDhcp4ComponentName2;
+extern EFI_UNICODE_STRING_TABLE     *gDhcpControllerNameTable;
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+  
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                                children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.inf b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.inf
new file mode 100644
index 0000000000..9b6c9c1dcc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.inf
@@ -0,0 +1,72 @@
+## @file
+#  This module produces EFI DHCPv4 Protocol and EFI DHCPv4 Service Binding Protocol.
+#
+#  This module produces EFI DHCPv4 Protocol upon EFI UDPv4 Protocol, to provide the
+#  capability to collect configuration information for the EFI IPv4 Protocol drivers
+#  and to provide DHCPv4 server and PXE boot server discovery services.
+#
+#  Copyright (c) 2007 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Dhcp4Dxe
+  MODULE_UNI_FILE                = Dhcp4Dxe.uni
+  FILE_GUID                      = 94734718-0BBC-47fb-96A5-EE7A5AE6A2AD
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = Dhcp4DriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gDhcp4DriverBinding                        
+#  COMPONENT_NAME                =  gDhcp4ComponentName
+#  COMPONENT_NAME2               =  gDhcp4ComponentName2
+#
+
+[Sources]
+  Dhcp4Impl.c
+  Dhcp4Io.c
+  Dhcp4Io.h
+  ComponentName.c
+  Dhcp4Driver.h
+  Dhcp4Driver.c
+  Dhcp4Option.c
+  Dhcp4Option.h
+  Dhcp4Impl.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  NetLib
+  UdpIoLib
+
+
+[Protocols]
+  gEfiDhcp4ServiceBindingProtocolGuid           ## BY_START
+  gEfiUdp4ServiceBindingProtocolGuid            ## TO_START
+  gEfiDhcp4ProtocolGuid                         ## BY_START
+  gEfiUdp4ProtocolGuid                          ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  Dhcp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.uni b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.uni
new file mode 100644
index 0000000000..43f1ab374e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4DxeExtra.uni
new file mode 100644
index 0000000000..f06b0d4803
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.c b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.c
new file mode 100644
index 0000000000..4f491b4bba
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.c
@@ -0,0 +1,1781 @@
+/** @file
+  This file implement the EFI_DHCP4_PROTOCOL interface.
+
+Copyright (c) 2006 - 2015, 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 "Dhcp4Impl.h"
+
+/**
+  Returns the current operating mode and cached data packet for the EFI DHCPv4 Protocol driver.
+
+  The GetModeData() function returns the current operating mode and cached data
+  packet for the EFI DHCPv4 Protocol driver.
+
+  @param[in]  This          Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[out] Dhcp4ModeData Pointer to storage for the EFI_DHCP4_MODE_DATA structure.
+
+  @retval EFI_SUCCESS           The mode data was returned.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4GetModeData (
+  IN  EFI_DHCP4_PROTOCOL    *This,
+  OUT EFI_DHCP4_MODE_DATA   *Dhcp4ModeData
+  );
+
+/**
+  Initializes, changes, or resets the operational settings for the EFI DHCPv4 Protocol driver.
+
+  The Configure() function is used to initialize, change, or reset the operational
+  settings of the EFI DHCPv4 Protocol driver for the communication device on which
+  the EFI DHCPv4 Service Binding Protocol is installed. This function can be
+  successfully called only if both of the following are true:
+  * This instance of the EFI DHCPv4 Protocol driver is in the Dhcp4Stopped, Dhcp4Init,
+    Dhcp4InitReboot, or Dhcp4Bound states.
+  * No other EFI DHCPv4 Protocol driver instance that is controlled by this EFI
+    DHCPv4 Service Binding Protocol driver instance has configured this EFI DHCPv4
+    Protocol driver.
+  When this driver is in the Dhcp4Stopped state, it can transfer into one of the
+  following two possible initial states:
+  * Dhcp4Init
+  * Dhcp4InitReboot
+  The driver can transfer into these states by calling Configure() with a non-NULL
+  Dhcp4CfgData. The driver will transfer into the appropriate state based on the
+  supplied client network address in the ClientAddress parameter and DHCP options
+  in the OptionList parameter as described in RFC 2131.
+  When Configure() is called successfully while Dhcp4CfgData is set to NULL, the
+  default configuring data will be reset in the EFI DHCPv4 Protocol driver and
+  the state of the EFI DHCPv4 Protocol driver will not be changed. If one instance
+  wants to make it possible for another instance to configure the EFI DHCPv4 Protocol
+  driver, it must call this function with Dhcp4CfgData set to NULL.
+
+  @param[in]  This                   Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  Dhcp4CfgData           Pointer to the EFI_DHCP4_CONFIG_DATA.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Init or
+                                Dhcp4InitReboot state, if the original state of this driver
+                                was Dhcp4Stopped and the value of Dhcp4CfgData was
+                                not NULL. Otherwise, the state was left unchanged.
+  @retval EFI_ACCESS_DENIED     This instance of the EFI DHCPv4 Protocol driver was not in the
+                                Dhcp4Stopped, Dhcp4Init, Dhcp4InitReboot, or Dhcp4Bound state;
+                                Or onother instance of this EFI DHCPv4 Protocol driver is already
+                                in a valid configured state.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Configure (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN EFI_DHCP4_CONFIG_DATA  *Dhcp4CfgData       OPTIONAL
+  );
+
+/**
+  Starts the DHCP configuration process.
+
+  The Start() function starts the DHCP configuration process. This function can
+  be called only when the EFI DHCPv4 Protocol driver is in the Dhcp4Init or
+  Dhcp4InitReboot state.
+  If the DHCP process completes successfully, the state of the EFI DHCPv4 Protocol
+  driver will be transferred through Dhcp4Selecting and Dhcp4Requesting to the
+  Dhcp4Bound state. The CompletionEvent will then be signaled if it is not NULL.
+  If the process aborts, either by the user or by some unexpected network error,
+  the state is restored to the Dhcp4Init state. The Start() function can be called
+  again to restart the process.
+  Refer to RFC 2131 for precise state transitions during this process. At the
+  time when each event occurs in this process, the callback function that was set
+  by EFI_DHCP4_PROTOCOL.Configure() will be called and the user can take this
+  opportunity to control the process.
+
+  @param[in]  This            Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  CompletionEvent If not NULL, indicates the event that will be signaled when the
+                              EFI DHCPv4 Protocol driver is transferred into the
+                              Dhcp4Bound state or when the DHCP process is aborted.
+                              EFI_DHCP4_PROTOCOL.GetModeData() can be called to
+                              check the completion status. If NULL,
+                              EFI_DHCP4_PROTOCOL.Start() will wait until the driver
+                              is transferred into the Dhcp4Bound state or the process fails.
+
+  @retval EFI_SUCCESS           The DHCP configuration process has started, or it has completed
+                                when CompletionEvent is NULL.
+  @retval EFI_NOT_STARTED       The EFI DHCPv4 Protocol driver is in the Dhcp4Stopped
+                                state. EFI_DHCP4_PROTOCOL. Configure() needs to be called.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_TIMEOUT           The DHCP configuration process failed because no response was
+                                received from the server within the specified timeout value.
+  @retval EFI_ABORTED           The user aborted the DHCP process.
+  @retval EFI_ALREADY_STARTED   Some other EFI DHCPv4 Protocol instance already started the
+                                DHCP process.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Start (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN EFI_EVENT              CompletionEvent   OPTIONAL
+  );
+
+/**
+  Extends the lease time by sending a request packet.
+
+  The RenewRebind() function is used to manually extend the lease time when the
+  EFI DHCPv4 Protocol driver is in the Dhcp4Bound state and the lease time has
+  not expired yet. This function will send a request packet to the previously
+  found server (or to any server when RebindRequest is TRUE) and transfer the
+  state into the Dhcp4Renewing state (or Dhcp4Rebinding when RebindingRequest is
+  TRUE). When a response is received, the state is returned to Dhcp4Bound.
+  If no response is received before the try count is exceeded (the RequestTryCount
+  field that is specified in EFI_DHCP4_CONFIG_DATA) but before the lease time that
+  was issued by the previous server expires, the driver will return to the Dhcp4Bound
+  state and the previous configuration is restored. The outgoing and incoming packets
+  can be captured by the EFI_DHCP4_CALLBACK function.
+
+  @param[in]  This            Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  RebindRequest   If TRUE, this function broadcasts the request packets and enters
+                              the Dhcp4Rebinding state. Otherwise, it sends a unicast
+                              request packet and enters the Dhcp4Renewing state.
+  @param[in]  CompletionEvent If not NULL, this event is signaled when the renew/rebind phase
+                              completes or some error occurs.
+                              EFI_DHCP4_PROTOCOL.GetModeData() can be called to
+                              check the completion status. If NULL,
+                              EFI_DHCP4_PROTOCOL.RenewRebind() will busy-wait
+                              until the DHCP process finishes.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the
+                                Dhcp4Renewing state or is back to the Dhcp4Bound state.
+  @retval EFI_NOT_STARTED       The EFI DHCPv4 Protocol driver is in the Dhcp4Stopped
+                                state. EFI_DHCP4_PROTOCOL.Configure() needs to
+                                be called.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_TIMEOUT           There was no response from the server when the try count was
+                                exceeded.
+  @retval EFI_ACCESS_DENIED     The driver is not in the Dhcp4Bound state.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4RenewRebind (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN BOOLEAN                RebindRequest,
+  IN EFI_EVENT              CompletionEvent   OPTIONAL
+  );
+
+/**
+  Releases the current address configuration.
+
+  The Release() function releases the current configured IP address by doing either
+  of the following:
+  * Sending a DHCPRELEASE packet when the EFI DHCPv4 Protocol driver is in the
+    Dhcp4Bound state
+  * Setting the previously assigned IP address that was provided with the
+    EFI_DHCP4_PROTOCOL.Configure() function to 0.0.0.0 when the driver is in
+    Dhcp4InitReboot state
+  After a successful call to this function, the EFI DHCPv4 Protocol driver returns
+  to the Dhcp4Init state and any subsequent incoming packets will be discarded silently.
+
+  @param[in]  This                  Pointer to the EFI_DHCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Init phase.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_ACCESS_DENIED     The EFI DHCPv4 Protocol driver is not Dhcp4InitReboot state.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Release (
+  IN EFI_DHCP4_PROTOCOL     *This
+  );
+
+/**
+  Stops the current address configuration.
+
+  The Stop() function is used to stop the DHCP configuration process. After this
+  function is called successfully, the EFI DHCPv4 Protocol driver is transferred
+  into the Dhcp4Stopped state. EFI_DHCP4_PROTOCOL.Configure() needs to be called
+  before DHCP configuration process can be started again. This function can be
+  called when the EFI DHCPv4 Protocol driver is in any state.
+
+  @param[in]  This                  Pointer to the EFI_DHCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Stopped phase.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Stop (
+  IN EFI_DHCP4_PROTOCOL     *This
+  );
+
+/**
+  Builds a DHCP packet, given the options to be appended or deleted or replaced.
+
+  The Build() function is used to assemble a new packet from the original packet
+  by replacing or deleting existing options or appending new options. This function
+  does not change any state of the EFI DHCPv4 Protocol driver and can be used at
+  any time.
+
+  @param[in]  This        Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  SeedPacket  Initial packet to be used as a base for building new packet.
+  @param[in]  DeleteCount Number of opcodes in the DeleteList.
+  @param[in]  DeleteList  List of opcodes to be deleted from the seed packet.
+                          Ignored if DeleteCount is zero.
+  @param[in]  AppendCount Number of entries in the OptionList.
+  @param[in]  AppendList  Pointer to a DHCP option list to be appended to SeedPacket.
+                          If SeedPacket also contains options in this list, they are
+                          replaced by new options (except pad option). Ignored if
+                          AppendCount is zero. Type EFI_DHCP4_PACKET_OPTION
+  @param[out] NewPacket   Pointer to storage for the pointer to the new allocated packet.
+                          Use the EFI Boot Service FreePool() on the resulting pointer
+                          when done with the packet.
+
+  @retval EFI_SUCCESS           The new packet was built.
+  @retval EFI_OUT_OF_RESOURCES  Storage for the new packet could not be allocated.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Build (
+  IN EFI_DHCP4_PROTOCOL       *This,
+  IN EFI_DHCP4_PACKET         *SeedPacket,
+  IN UINT32                   DeleteCount,
+  IN UINT8                    *DeleteList OPTIONAL,
+  IN UINT32                   AppendCount,
+  IN EFI_DHCP4_PACKET_OPTION  *AppendList[] OPTIONAL,
+  OUT EFI_DHCP4_PACKET        **NewPacket
+  );
+
+/**
+  Transmits a DHCP formatted packet and optionally waits for responses.
+
+  The TransmitReceive() function is used to transmit a DHCP packet and optionally
+  wait for the response from servers. This function does not change the state of
+  the EFI DHCPv4 Protocol driver and thus can be used at any time.
+
+  @param[in]  This    Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  Token   Pointer to the EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN structure.
+
+  @retval EFI_SUCCESS           The packet was successfully queued for transmission.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_NOT_READY         The previous call to this function has not finished yet. Try to call
+                                this function after collection process completes.
+  @retval EFI_NO_MAPPING        The default station address is not available yet.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval Others                Some other unexpected error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4TransmitReceive (
+  IN EFI_DHCP4_PROTOCOL                *This,
+  IN EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token
+  );
+
+/**
+  Parses the packed DHCP option data.
+
+  The Parse() function is used to retrieve the option list from a DHCP packet.
+  If *OptionCount isn't zero, and there is enough space for all the DHCP options
+  in the Packet, each element of PacketOptionList is set to point to somewhere in
+  the Packet->Dhcp4.Option where a new DHCP option begins. If RFC3396 is supported,
+  the caller should reassemble the parsed DHCP options to get the finial result.
+  If *OptionCount is zero or there isn't enough space for all of them, the number
+  of DHCP options in the Packet is returned in OptionCount.
+
+  @param  This             Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param  Packet           Pointer to packet to be parsed.
+  @param  OptionCount      On input, the number of entries in the PacketOptionList.
+                           On output, the number of entries that were written into the
+                           PacketOptionList.
+  @param  PacketOptionList List of packet option entries to be filled in. End option or pad
+                           options are not included.
+
+  @retval EFI_SUCCESS           The packet was successfully parsed.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_BUFFER_TOO_SMALL  One or more of the following conditions is TRUE:
+                                1) *OptionCount is smaller than the number of options that
+                                were found in the Packet.
+                                2) PacketOptionList is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Parse (
+  IN EFI_DHCP4_PROTOCOL       *This,
+  IN EFI_DHCP4_PACKET         *Packet,
+  IN OUT UINT32               *OptionCount,
+  OUT EFI_DHCP4_PACKET_OPTION *PacketOptionList[] OPTIONAL
+  );
+
+EFI_DHCP4_PROTOCOL  mDhcp4ProtocolTemplate = {
+  EfiDhcp4GetModeData,
+  EfiDhcp4Configure,
+  EfiDhcp4Start,
+  EfiDhcp4RenewRebind,
+  EfiDhcp4Release,
+  EfiDhcp4Stop,
+  EfiDhcp4Build,
+  EfiDhcp4TransmitReceive,
+  EfiDhcp4Parse
+};
+
+/**
+  Returns the current operating mode and cached data packet for the EFI DHCPv4 Protocol driver.
+
+  The GetModeData() function returns the current operating mode and cached data
+  packet for the EFI DHCPv4 Protocol driver.
+
+  @param[in]  This          Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[out] Dhcp4ModeData Pointer to storage for the EFI_DHCP4_MODE_DATA structure.
+
+  @retval EFI_SUCCESS           The mode data was returned.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4GetModeData (
+  IN  EFI_DHCP4_PROTOCOL    *This,
+  OUT EFI_DHCP4_MODE_DATA   *Dhcp4ModeData
+  )
+{
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  DHCP_PARAMETER            *Para;
+  EFI_TPL                   OldTpl;
+  IP4_ADDR                  Ip;
+
+  //
+  // First validate the parameters.
+  //
+  if ((This == NULL) || (Dhcp4ModeData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  DhcpSb  = Instance->Service;
+
+  //
+  // Caller can use GetModeData to retrieve current DHCP states
+  // no matter whether it is the active child or not.
+  //
+  Dhcp4ModeData->State = (EFI_DHCP4_STATE) DhcpSb->DhcpState;
+  CopyMem (&Dhcp4ModeData->ConfigData, &DhcpSb->ActiveConfig, sizeof (Dhcp4ModeData->ConfigData));
+  CopyMem (&Dhcp4ModeData->ClientMacAddress, &DhcpSb->Mac, sizeof (Dhcp4ModeData->ClientMacAddress));
+
+  Ip = HTONL (DhcpSb->ClientAddr);
+  CopyMem (&Dhcp4ModeData->ClientAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  Ip = HTONL (DhcpSb->Netmask);
+  CopyMem (&Dhcp4ModeData->SubnetMask, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  Ip = HTONL (DhcpSb->ServerAddr);
+  CopyMem (&Dhcp4ModeData->ServerAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  Para = DhcpSb->Para;
+
+  if (Para != NULL) {
+    Ip = HTONL (Para->Router);
+    CopyMem (&Dhcp4ModeData->RouterAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+    Dhcp4ModeData->LeaseTime = Para->Lease;
+  } else {
+    ZeroMem (&Dhcp4ModeData->RouterAddress, sizeof (EFI_IPv4_ADDRESS));
+    Dhcp4ModeData->LeaseTime = 0xffffffff;
+  }
+
+  Dhcp4ModeData->ReplyPacket = DhcpSb->Selected;
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Free the resource related to the configure parameters.
+  DHCP driver will make a copy of the user's configure
+  such as the time out value.
+
+  @param  Config                 The DHCP configure data
+
+**/
+VOID
+DhcpCleanConfigure (
+  IN OUT EFI_DHCP4_CONFIG_DATA  *Config
+  )
+{
+  UINT32                    Index;
+
+  if (Config->DiscoverTimeout != NULL) {
+    FreePool (Config->DiscoverTimeout);
+  }
+
+  if (Config->RequestTimeout != NULL) {
+    FreePool (Config->RequestTimeout);
+  }
+
+  if (Config->OptionList != NULL) {
+    for (Index = 0; Index < Config->OptionCount; Index++) {
+      if (Config->OptionList[Index] != NULL) {
+        FreePool (Config->OptionList[Index]);
+      }
+    }
+
+    FreePool (Config->OptionList);
+  }
+
+  ZeroMem (Config, sizeof (EFI_DHCP4_CONFIG_DATA));
+}
+
+
+/**
+  Allocate memory for configure parameter such as timeout value for Dst,
+  then copy the configure parameter from Src to Dst.
+
+  @param[out]  Dst                    The destination DHCP configure data.
+  @param[in]   Src                    The source DHCP configure data.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory.
+  @retval EFI_SUCCESS            The configure is copied.
+
+**/
+EFI_STATUS
+DhcpCopyConfigure (
+  OUT EFI_DHCP4_CONFIG_DATA  *Dst,
+  IN  EFI_DHCP4_CONFIG_DATA  *Src
+  )
+{
+  EFI_DHCP4_PACKET_OPTION   **DstOptions;
+  EFI_DHCP4_PACKET_OPTION   **SrcOptions;
+  UINTN                     Len;
+  UINT32                    Index;
+
+  CopyMem (Dst, Src, sizeof (*Dst));
+  Dst->DiscoverTimeout  = NULL;
+  Dst->RequestTimeout   = NULL;
+  Dst->OptionList       = NULL;
+
+  //
+  // Allocate a memory then copy DiscoverTimeout to it
+  //
+  if (Src->DiscoverTimeout != NULL) {
+    Len                   = Src->DiscoverTryCount * sizeof (UINT32);
+    Dst->DiscoverTimeout  = AllocatePool (Len);
+
+    if (Dst->DiscoverTimeout == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    for (Index = 0; Index < Src->DiscoverTryCount; Index++) {
+      Dst->DiscoverTimeout[Index] = MAX (Src->DiscoverTimeout[Index], 1);
+    }
+  }
+
+  //
+  // Allocate a memory then copy RequestTimeout to it
+  //
+  if (Src->RequestTimeout != NULL) {
+    Len                 = Src->RequestTryCount * sizeof (UINT32);
+    Dst->RequestTimeout = AllocatePool (Len);
+
+    if (Dst->RequestTimeout == NULL) {
+      goto ON_ERROR;
+    }
+
+    for (Index = 0; Index < Src->RequestTryCount; Index++) {
+      Dst->RequestTimeout[Index] = MAX (Src->RequestTimeout[Index], 1);
+    }
+  }
+
+  //
+  // Allocate an array of dhcp option point, then allocate memory
+  // for each option and copy the source option to it
+  //
+  if (Src->OptionList != NULL) {
+    Len             = Src->OptionCount * sizeof (EFI_DHCP4_PACKET_OPTION *);
+    Dst->OptionList = AllocateZeroPool (Len);
+
+    if (Dst->OptionList == NULL) {
+      goto ON_ERROR;
+    }
+
+    DstOptions  = Dst->OptionList;
+    SrcOptions  = Src->OptionList;
+
+    for (Index = 0; Index < Src->OptionCount; Index++) {
+      Len = sizeof (EFI_DHCP4_PACKET_OPTION) + MAX (SrcOptions[Index]->Length - 1, 0);
+
+      DstOptions[Index] = AllocatePool (Len);
+
+      if (DstOptions[Index] == NULL) {
+        goto ON_ERROR;
+      }
+
+      CopyMem (DstOptions[Index], SrcOptions[Index], Len);
+    }
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  DhcpCleanConfigure (Dst);
+  return EFI_OUT_OF_RESOURCES;
+}
+
+
+/**
+  Give up the control of the DHCP service to let other child
+  resume. Don't change the service's DHCP state and the Client
+  address and option list configure as required by RFC2131.
+
+  @param  DhcpSb                 The DHCP service instance.
+
+**/
+VOID
+DhcpYieldControl (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  EFI_DHCP4_CONFIG_DATA     *Config;
+
+  Config    = &DhcpSb->ActiveConfig;
+
+  DhcpSb->ServiceState  = DHCP_UNCONFIGED;
+  DhcpSb->ActiveChild   = NULL;
+
+  if (Config->DiscoverTimeout != NULL) {
+    FreePool (Config->DiscoverTimeout);
+
+    Config->DiscoverTryCount  = 0;
+    Config->DiscoverTimeout   = NULL;
+  }
+
+  if (Config->RequestTimeout != NULL) {
+    FreePool (Config->RequestTimeout);
+
+    Config->RequestTryCount = 0;
+    Config->RequestTimeout  = NULL;
+  }
+
+  Config->Dhcp4Callback   = NULL;
+  Config->CallbackContext = NULL;
+}
+
+
+/**
+  Initializes, changes, or resets the operational settings for the EFI DHCPv4 Protocol driver.
+
+  The Configure() function is used to initialize, change, or reset the operational
+  settings of the EFI DHCPv4 Protocol driver for the communication device on which
+  the EFI DHCPv4 Service Binding Protocol is installed. This function can be
+  successfully called only if both of the following are true:
+  * This instance of the EFI DHCPv4 Protocol driver is in the Dhcp4Stopped, Dhcp4Init,
+    Dhcp4InitReboot, or Dhcp4Bound states.
+  * No other EFI DHCPv4 Protocol driver instance that is controlled by this EFI
+    DHCPv4 Service Binding Protocol driver instance has configured this EFI DHCPv4
+    Protocol driver.
+  When this driver is in the Dhcp4Stopped state, it can transfer into one of the
+  following two possible initial states:
+  * Dhcp4Init
+  * Dhcp4InitReboot
+  The driver can transfer into these states by calling Configure() with a non-NULL
+  Dhcp4CfgData. The driver will transfer into the appropriate state based on the
+  supplied client network address in the ClientAddress parameter and DHCP options
+  in the OptionList parameter as described in RFC 2131.
+  When Configure() is called successfully while Dhcp4CfgData is set to NULL, the
+  default configuring data will be reset in the EFI DHCPv4 Protocol driver and
+  the state of the EFI DHCPv4 Protocol driver will not be changed. If one instance
+  wants to make it possible for another instance to configure the EFI DHCPv4 Protocol
+  driver, it must call this function with Dhcp4CfgData set to NULL.
+
+  @param[in]  This                   Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  Dhcp4CfgData           Pointer to the EFI_DHCP4_CONFIG_DATA.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Init or
+                                Dhcp4InitReboot state, if the original state of this driver
+                                was Dhcp4Stopped and the value of Dhcp4CfgData was
+                                not NULL. Otherwise, the state was left unchanged.
+  @retval EFI_ACCESS_DENIED     This instance of the EFI DHCPv4 Protocol driver was not in the
+                                Dhcp4Stopped, Dhcp4Init, Dhcp4InitReboot, or Dhcp4Bound state;
+                                Or onother instance of this EFI DHCPv4 Protocol driver is already
+                                in a valid configured state.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Configure (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN EFI_DHCP4_CONFIG_DATA  *Dhcp4CfgData       OPTIONAL
+  )
+{
+  EFI_DHCP4_CONFIG_DATA     *Config;
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  UINT32                    Index;
+  IP4_ADDR                  Ip;
+
+  //
+  // First validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Dhcp4CfgData != NULL) {
+    if ((Dhcp4CfgData->DiscoverTryCount != 0) && (Dhcp4CfgData->DiscoverTimeout == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((Dhcp4CfgData->RequestTryCount != 0) && (Dhcp4CfgData->RequestTimeout == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((Dhcp4CfgData->OptionCount != 0) && (Dhcp4CfgData->OptionList == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    CopyMem (&Ip, &Dhcp4CfgData->ClientAddress, sizeof (IP4_ADDR));
+
+    if ((Ip != 0) && !NetIp4IsUnicast (NTOHL (Ip), 0)) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  if (Instance->Signature != DHCP_PROTOCOL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  DhcpSb  = Instance->Service;
+  Config  = &DhcpSb->ActiveConfig;
+
+  Status  = EFI_ACCESS_DENIED;
+
+  if ((DhcpSb->DhcpState != Dhcp4Stopped) &&
+      (DhcpSb->DhcpState != Dhcp4Init) &&
+      (DhcpSb->DhcpState != Dhcp4InitReboot) &&
+      (DhcpSb->DhcpState != Dhcp4Bound)) {
+
+    goto ON_EXIT;
+  }
+
+  if ((DhcpSb->ActiveChild != NULL) && (DhcpSb->ActiveChild != Instance)) {
+    goto ON_EXIT;
+  }
+
+  if (Dhcp4CfgData != NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    DhcpCleanConfigure (Config);
+
+    if (EFI_ERROR (DhcpCopyConfigure (Config, Dhcp4CfgData))) {
+      goto ON_EXIT;
+    }
+
+    DhcpSb->UserOptionLen = 0;
+
+    for (Index = 0; Index < Dhcp4CfgData->OptionCount; Index++) {
+      DhcpSb->UserOptionLen += Dhcp4CfgData->OptionList[Index]->Length + 2;
+    }
+
+    DhcpSb->ActiveChild = Instance;
+
+    if (DhcpSb->DhcpState == Dhcp4Stopped) {
+      DhcpSb->ClientAddr = EFI_NTOHL (Dhcp4CfgData->ClientAddress);
+
+      if (DhcpSb->ClientAddr != 0) {
+        DhcpSb->DhcpState = Dhcp4InitReboot;
+      } else {
+        DhcpSb->DhcpState = Dhcp4Init;
+      }
+    }
+
+    DhcpSb->ServiceState  = DHCP_CONFIGED;
+    Status                = EFI_SUCCESS;
+
+  } else if (DhcpSb->ActiveChild == Instance) {
+    Status = EFI_SUCCESS;
+    DhcpYieldControl (DhcpSb);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Starts the DHCP configuration process.
+
+  The Start() function starts the DHCP configuration process. This function can
+  be called only when the EFI DHCPv4 Protocol driver is in the Dhcp4Init or
+  Dhcp4InitReboot state.
+  If the DHCP process completes successfully, the state of the EFI DHCPv4 Protocol
+  driver will be transferred through Dhcp4Selecting and Dhcp4Requesting to the
+  Dhcp4Bound state. The CompletionEvent will then be signaled if it is not NULL.
+  If the process aborts, either by the user or by some unexpected network error,
+  the state is restored to the Dhcp4Init state. The Start() function can be called
+  again to restart the process.
+  Refer to RFC 2131 for precise state transitions during this process. At the
+  time when each event occurs in this process, the callback function that was set
+  by EFI_DHCP4_PROTOCOL.Configure() will be called and the user can take this
+  opportunity to control the process.
+
+  @param[in]  This            Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  CompletionEvent If not NULL, indicates the event that will be signaled when the
+                              EFI DHCPv4 Protocol driver is transferred into the
+                              Dhcp4Bound state or when the DHCP process is aborted.
+                              EFI_DHCP4_PROTOCOL.GetModeData() can be called to
+                              check the completion status. If NULL,
+                              EFI_DHCP4_PROTOCOL.Start() will wait until the driver
+                              is transferred into the Dhcp4Bound state or the process fails.
+
+  @retval EFI_SUCCESS           The DHCP configuration process has started, or it has completed
+                                when CompletionEvent is NULL.
+  @retval EFI_NOT_STARTED       The EFI DHCPv4 Protocol driver is in the Dhcp4Stopped
+                                state. EFI_DHCP4_PROTOCOL. Configure() needs to be called.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_TIMEOUT           The DHCP configuration process failed because no response was
+                                received from the server within the specified timeout value.
+  @retval EFI_ABORTED           The user aborted the DHCP process.
+  @retval EFI_ALREADY_STARTED   Some other EFI DHCPv4 Protocol instance already started the
+                                DHCP process.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval EFI_NO_MEDIA          There was a media error.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Start (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN EFI_EVENT              CompletionEvent   OPTIONAL
+  )
+{
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  if (Instance->Signature != DHCP_PROTOCOL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  DhcpSb  = Instance->Service;
+
+  if (DhcpSb->DhcpState == Dhcp4Stopped) {
+    Status = EFI_NOT_STARTED;
+    goto ON_ERROR;
+  }
+
+  if ((DhcpSb->DhcpState != Dhcp4Init) && (DhcpSb->DhcpState != Dhcp4InitReboot)) {
+    Status = EFI_ALREADY_STARTED;
+    goto ON_ERROR;
+  }
+
+  DhcpSb->IoStatus = EFI_ALREADY_STARTED;
+
+  if (EFI_ERROR (Status = DhcpInitRequest (DhcpSb))) {
+    goto ON_ERROR;
+  }
+
+
+  Instance->CompletionEvent = CompletionEvent;
+
+  //
+  // Restore the TPL now, don't call poll function at TPL_CALLBACK.
+  //
+  gBS->RestoreTPL (OldTpl);
+
+  if (CompletionEvent == NULL) {
+    while (DhcpSb->IoStatus == EFI_ALREADY_STARTED) {
+      DhcpSb->UdpIo->Protocol.Udp4->Poll (DhcpSb->UdpIo->Protocol.Udp4);
+    }
+
+    return DhcpSb->IoStatus;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Extends the lease time by sending a request packet.
+
+  The RenewRebind() function is used to manually extend the lease time when the
+  EFI DHCPv4 Protocol driver is in the Dhcp4Bound state and the lease time has
+  not expired yet. This function will send a request packet to the previously
+  found server (or to any server when RebindRequest is TRUE) and transfer the
+  state into the Dhcp4Renewing state (or Dhcp4Rebinding when RebindingRequest is
+  TRUE). When a response is received, the state is returned to Dhcp4Bound.
+  If no response is received before the try count is exceeded (the RequestTryCount
+  field that is specified in EFI_DHCP4_CONFIG_DATA) but before the lease time that
+  was issued by the previous server expires, the driver will return to the Dhcp4Bound
+  state and the previous configuration is restored. The outgoing and incoming packets
+  can be captured by the EFI_DHCP4_CALLBACK function.
+
+  @param[in]  This            Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  RebindRequest   If TRUE, this function broadcasts the request packets and enters
+                              the Dhcp4Rebinding state. Otherwise, it sends a unicast
+                              request packet and enters the Dhcp4Renewing state.
+  @param[in]  CompletionEvent If not NULL, this event is signaled when the renew/rebind phase
+                              completes or some error occurs.
+                              EFI_DHCP4_PROTOCOL.GetModeData() can be called to
+                              check the completion status. If NULL,
+                              EFI_DHCP4_PROTOCOL.RenewRebind() will busy-wait
+                              until the DHCP process finishes.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the
+                                Dhcp4Renewing state or is back to the Dhcp4Bound state.
+  @retval EFI_NOT_STARTED       The EFI DHCPv4 Protocol driver is in the Dhcp4Stopped
+                                state. EFI_DHCP4_PROTOCOL.Configure() needs to
+                                be called.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_TIMEOUT           There was no response from the server when the try count was
+                                exceeded.
+  @retval EFI_ACCESS_DENIED     The driver is not in the Dhcp4Bound state.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4RenewRebind (
+  IN EFI_DHCP4_PROTOCOL     *This,
+  IN BOOLEAN                RebindRequest,
+  IN EFI_EVENT              CompletionEvent   OPTIONAL
+  )
+{
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  if (Instance->Signature != DHCP_PROTOCOL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  DhcpSb  = Instance->Service;
+
+  if (DhcpSb->DhcpState == Dhcp4Stopped) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (DhcpSb->DhcpState != Dhcp4Bound) {
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  if (DHCP_IS_BOOTP (DhcpSb->Para)) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Transit the states then send a extra DHCP request
+  //
+  if (!RebindRequest) {
+    DhcpSetState (DhcpSb, Dhcp4Renewing, FALSE);
+  } else {
+    DhcpSetState (DhcpSb, Dhcp4Rebinding, FALSE);
+  }
+
+  //
+  // Clear initial time to make sure that elapsed-time
+  // is set to 0 for first REQUEST in renewal process.
+  //
+  Instance->ElaspedTime = 0;
+
+  Status = DhcpSendMessage (
+             DhcpSb,
+             DhcpSb->Selected,
+             DhcpSb->Para,
+             DHCP_MSG_REQUEST,
+             (UINT8 *) "Extra renew/rebind by the application"
+             );
+
+  if (EFI_ERROR (Status)) {
+    DhcpSetState (DhcpSb, Dhcp4Bound, FALSE);
+    goto ON_EXIT;
+  }
+
+  DhcpSb->ExtraRefresh        = TRUE;
+  DhcpSb->IoStatus            = EFI_ALREADY_STARTED;
+  Instance->RenewRebindEvent  = CompletionEvent;
+
+  gBS->RestoreTPL (OldTpl);
+
+  if (CompletionEvent == NULL) {
+    while (DhcpSb->IoStatus == EFI_ALREADY_STARTED) {
+      DhcpSb->UdpIo->Protocol.Udp4->Poll (DhcpSb->UdpIo->Protocol.Udp4);
+
+    }
+
+    return DhcpSb->IoStatus;
+  }
+
+  return EFI_SUCCESS;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Releases the current address configuration.
+
+  The Release() function releases the current configured IP address by doing either
+  of the following:
+  * Sending a DHCPRELEASE packet when the EFI DHCPv4 Protocol driver is in the
+    Dhcp4Bound state
+  * Setting the previously assigned IP address that was provided with the
+    EFI_DHCP4_PROTOCOL.Configure() function to 0.0.0.0 when the driver is in
+    Dhcp4InitReboot state
+  After a successful call to this function, the EFI DHCPv4 Protocol driver returns
+  to the Dhcp4Init state and any subsequent incoming packets will be discarded silently.
+
+  @param[in]  This                  Pointer to the EFI_DHCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Init phase.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_ACCESS_DENIED     The EFI DHCPv4 Protocol driver is not Dhcp4InitReboot state.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Release (
+  IN EFI_DHCP4_PROTOCOL     *This
+  )
+{
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  if (Instance->Signature != DHCP_PROTOCOL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_SUCCESS;
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  DhcpSb  = Instance->Service;
+
+  if ((DhcpSb->DhcpState != Dhcp4InitReboot) && (DhcpSb->DhcpState != Dhcp4Bound)) {
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  if (!DHCP_IS_BOOTP (DhcpSb->Para) && (DhcpSb->DhcpState == Dhcp4Bound)) {
+    Status = DhcpSendMessage (
+               DhcpSb,
+               DhcpSb->Selected,
+               DhcpSb->Para,
+               DHCP_MSG_RELEASE,
+               NULL
+               );
+
+    if (EFI_ERROR (Status)) {
+      Status = EFI_DEVICE_ERROR;
+      goto ON_EXIT;
+    }
+  }
+
+  DhcpCleanLease (DhcpSb);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Stops the current address configuration.
+
+  The Stop() function is used to stop the DHCP configuration process. After this
+  function is called successfully, the EFI DHCPv4 Protocol driver is transferred
+  into the Dhcp4Stopped state. EFI_DHCP4_PROTOCOL.Configure() needs to be called
+  before DHCP configuration process can be started again. This function can be
+  called when the EFI DHCPv4 Protocol driver is in any state.
+
+  @param[in]  This                  Pointer to the EFI_DHCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The EFI DHCPv4 Protocol driver is now in the Dhcp4Stopped phase.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Stop (
+  IN EFI_DHCP4_PROTOCOL     *This
+  )
+{
+  DHCP_PROTOCOL             *Instance;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+
+  if (Instance->Signature != DHCP_PROTOCOL_SIGNATURE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+  DhcpSb  = Instance->Service;
+
+  DhcpCleanLease (DhcpSb);
+
+  DhcpSb->DhcpState     = Dhcp4Stopped;
+  DhcpSb->ServiceState  = DHCP_UNCONFIGED;
+
+  gBS->RestoreTPL (OldTpl);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Builds a DHCP packet, given the options to be appended or deleted or replaced.
+
+  The Build() function is used to assemble a new packet from the original packet
+  by replacing or deleting existing options or appending new options. This function
+  does not change any state of the EFI DHCPv4 Protocol driver and can be used at
+  any time.
+
+  @param[in]  This        Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  SeedPacket  Initial packet to be used as a base for building new packet.
+  @param[in]  DeleteCount Number of opcodes in the DeleteList.
+  @param[in]  DeleteList  List of opcodes to be deleted from the seed packet.
+                          Ignored if DeleteCount is zero.
+  @param[in]  AppendCount Number of entries in the OptionList.
+  @param[in]  AppendList  Pointer to a DHCP option list to be appended to SeedPacket.
+                          If SeedPacket also contains options in this list, they are
+                          replaced by new options (except pad option). Ignored if
+                          AppendCount is zero. Type EFI_DHCP4_PACKET_OPTION
+  @param[out] NewPacket   Pointer to storage for the pointer to the new allocated packet.
+                          Use the EFI Boot Service FreePool() on the resulting pointer
+                          when done with the packet.
+
+  @retval EFI_SUCCESS           The new packet was built.
+  @retval EFI_OUT_OF_RESOURCES  Storage for the new packet could not be allocated.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Build (
+  IN EFI_DHCP4_PROTOCOL       *This,
+  IN EFI_DHCP4_PACKET         *SeedPacket,
+  IN UINT32                   DeleteCount,
+  IN UINT8                    *DeleteList OPTIONAL,
+  IN UINT32                   AppendCount,
+  IN EFI_DHCP4_PACKET_OPTION  *AppendList[] OPTIONAL,
+  OUT EFI_DHCP4_PACKET        **NewPacket
+  )
+{
+  //
+  // First validate the parameters
+  //
+  if ((This == NULL) || (NewPacket == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((SeedPacket == NULL) || (SeedPacket->Dhcp4.Magik != DHCP_OPTION_MAGIC) ||
+      EFI_ERROR (DhcpValidateOptions (SeedPacket, NULL))) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((DeleteCount == 0) && (AppendCount == 0)) ||
+      ((DeleteCount != 0) && (DeleteList == NULL)) ||
+      ((AppendCount != 0) && (AppendList == NULL))) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return DhcpBuild (
+           SeedPacket,
+           DeleteCount,
+           DeleteList,
+           AppendCount,
+           AppendList,
+           NewPacket
+           );
+}
+
+/**
+  Callback by UdpIoCreatePort() when creating UdpIo for this Dhcp4 instance.
+
+  @param[in] UdpIo      The UdpIo being created.
+  @param[in] Context    Dhcp4 instance.
+
+  @retval EFI_SUCCESS   UdpIo is configured successfully.
+  @retval other         Other error occurs.
+**/
+EFI_STATUS
+EFIAPI
+Dhcp4InstanceConfigUdpIo (
+  IN UDP_IO       *UdpIo,
+  IN VOID         *Context
+  )
+{
+  DHCP_PROTOCOL                     *Instance;
+  EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token;
+  EFI_UDP4_CONFIG_DATA              UdpConfigData;
+  IP4_ADDR                          ClientAddr;
+  IP4_ADDR                          Ip;   
+  INTN                              Class; 
+  IP4_ADDR                          SubnetMask;
+
+  Instance = (DHCP_PROTOCOL *) Context;
+  Token    = Instance->Token;
+
+  ZeroMem (&UdpConfigData, sizeof (EFI_UDP4_CONFIG_DATA));
+
+  UdpConfigData.AcceptBroadcast    = TRUE;
+  UdpConfigData.AllowDuplicatePort = TRUE;
+  UdpConfigData.TimeToLive         = 64;
+  UdpConfigData.DoNotFragment      = TRUE;
+
+  ClientAddr = EFI_NTOHL (Token->Packet->Dhcp4.Header.ClientAddr);
+  Ip = HTONL (ClientAddr);
+  CopyMem (&UdpConfigData.StationAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+  
+  Class = NetGetIpClass (ClientAddr);
+  ASSERT (Class < IP4_ADDR_CLASSE);
+  SubnetMask = gIp4AllMasks[Class << 3];
+  Ip = HTONL (SubnetMask);
+  CopyMem (&UdpConfigData.SubnetMask, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  if ((Token->ListenPointCount == 0) || (Token->ListenPoints[0].ListenPort == 0)) {
+    UdpConfigData.StationPort = DHCP_CLIENT_PORT;
+  } else {
+    UdpConfigData.StationPort = Token->ListenPoints[0].ListenPort;
+  }
+
+  return UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, &UdpConfigData);
+}
+
+/**
+  Create UdpIo for this Dhcp4 instance.
+
+  @param Instance   The Dhcp4 instance.
+
+  @retval EFI_SUCCESS                UdpIo is created successfully.
+  @retval EFI_OUT_OF_RESOURCES       Fails to create UdpIo because of limited
+                                     resources or configuration failure.
+**/
+EFI_STATUS
+Dhcp4InstanceCreateUdpIo (
+  IN OUT DHCP_PROTOCOL  *Instance
+  )
+{
+  DHCP_SERVICE  *DhcpSb;
+  EFI_STATUS    Status;
+  VOID          *Udp4;
+
+  ASSERT (Instance->Token != NULL);
+
+  DhcpSb          = Instance->Service;
+  Instance->UdpIo = UdpIoCreateIo (
+                      DhcpSb->Controller,
+                      DhcpSb->Image,
+                      Dhcp4InstanceConfigUdpIo,
+                      UDP_IO_UDP4_VERSION,
+                      Instance
+                      );
+  if (Instance->UdpIo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  } else {
+    Status = gBS->OpenProtocol (
+                    Instance->UdpIo->UdpHandle,
+                    &gEfiUdp4ProtocolGuid,
+                    (VOID **) &Udp4,
+                    Instance->Service->Image,
+                    Instance->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      UdpIoFreeIo (Instance->UdpIo);
+      Instance->UdpIo = NULL;
+    }
+    return Status;
+  }
+}
+
+/**
+  Callback of Dhcp packet. Does nothing.
+
+  @param Arg           The context.
+
+**/
+VOID
+EFIAPI
+DhcpDummyExtFree (
+  IN VOID                   *Arg
+  )
+{
+}
+
+/**
+  Callback of UdpIoRecvDatagram() that handles a Dhcp4 packet.
+
+  Only BOOTP responses will be handled that correspond to the Xid of the request
+  sent out. The packet will be queued to the response queue.
+
+  @param UdpPacket        The Dhcp4 packet.
+  @param EndPoint         Udp4 address pair.
+  @param IoStatus         Status of the input.
+  @param Context          Extra info for the input.
+
+**/
+VOID
+EFIAPI
+PxeDhcpInput (
+  NET_BUF                   *UdpPacket,
+  UDP_END_POINT             *EndPoint,
+  EFI_STATUS                IoStatus,
+  VOID                      *Context
+  )
+{
+  DHCP_PROTOCOL                     *Instance;
+  EFI_DHCP4_HEADER                  *Head;
+  NET_BUF                           *Wrap;
+  EFI_DHCP4_PACKET                  *Packet;
+  EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token;
+  UINT32                            Len;
+  EFI_STATUS                        Status;
+
+  Wrap     = NULL;
+  Instance = (DHCP_PROTOCOL *) Context;
+  Token    = Instance->Token;
+
+  //
+  // Don't restart receive if error occurs or DHCP is destroyed.
+  //
+  if (EFI_ERROR (IoStatus)) {
+    return ;
+  }
+
+  ASSERT (UdpPacket != NULL);
+
+  //
+  // Validate the packet received
+  //
+  if (UdpPacket->TotalSize < sizeof (EFI_DHCP4_HEADER)) {
+    goto RESTART;
+  }
+
+  //
+  // Copy the DHCP message to a continuous memory block, make the buffer size
+  // of the EFI_DHCP4_PACKET a multiple of 4-byte.
+  //
+  Len  = NET_ROUNDUP (sizeof (EFI_DHCP4_PACKET) + UdpPacket->TotalSize - sizeof (EFI_DHCP4_HEADER), 4);
+  Wrap = NetbufAlloc (Len);
+  if (Wrap == NULL) {
+    goto RESTART;
+  }
+
+  Packet         = (EFI_DHCP4_PACKET *) NetbufAllocSpace (Wrap, Len, NET_BUF_TAIL);
+  ASSERT (Packet != NULL);
+
+  Packet->Size   = Len;
+  Head           = &Packet->Dhcp4.Header;
+  Packet->Length = NetbufCopy (UdpPacket, 0, UdpPacket->TotalSize, (UINT8 *) Head);
+
+  if (Packet->Length != UdpPacket->TotalSize) {
+    goto RESTART;
+  }
+
+  //
+  // Is this packet the answer to our packet?
+  //
+  if ((Head->OpCode != BOOTP_REPLY) ||
+      (Head->Xid != Token->Packet->Dhcp4.Header.Xid) ||
+      (CompareMem (&Token->Packet->Dhcp4.Header.ClientHwAddr[0], Head->ClientHwAddr, Head->HwAddrLen) != 0)) {
+    goto RESTART;
+  }
+
+  //
+  // Validate the options and retrieve the interested options
+  //
+  if ((Packet->Length > sizeof (EFI_DHCP4_HEADER) + sizeof (UINT32)) &&
+      (Packet->Dhcp4.Magik == DHCP_OPTION_MAGIC) &&
+      EFI_ERROR (DhcpValidateOptions (Packet, NULL))) {
+
+    goto RESTART;
+  }
+
+  //
+  // Keep this packet in the ResponseQueue.
+  //
+  NET_GET_REF (Wrap);
+  NetbufQueAppend (&Instance->ResponseQueue, Wrap);
+
+RESTART:
+
+  NetbufFree (UdpPacket);
+
+  if (Wrap != NULL) {
+    NetbufFree (Wrap);
+  }
+
+  Status = UdpIoRecvDatagram (Instance->UdpIo, PxeDhcpInput, Instance, 0);
+  if (EFI_ERROR (Status)) {
+    PxeDhcpDone (Instance);
+  }
+}
+
+/**
+  Complete a Dhcp4 transaction and signal the upper layer.
+
+  @param Instance      Dhcp4 instance.
+
+**/
+VOID
+PxeDhcpDone (
+  IN DHCP_PROTOCOL  *Instance
+  )
+{
+  EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token;
+
+  Token = Instance->Token;
+
+  Token->ResponseCount = Instance->ResponseQueue.BufNum;
+  if (Token->ResponseCount != 0) {
+    Token->ResponseList = (EFI_DHCP4_PACKET *) AllocatePool (Instance->ResponseQueue.BufSize);
+    if (Token->ResponseList == NULL) {
+      Token->Status = EFI_OUT_OF_RESOURCES;
+      goto SIGNAL_USER;
+    }
+
+    //
+    // Copy the received DHCP responses.
+    //
+    NetbufQueCopy (&Instance->ResponseQueue, 0, Instance->ResponseQueue.BufSize, (UINT8 *) Token->ResponseList);
+    Token->Status = EFI_SUCCESS;
+  } else {
+    Token->ResponseList = NULL;
+    Token->Status       = EFI_TIMEOUT;
+  }
+
+SIGNAL_USER:
+  //
+  // Clean up the resources dedicated for this transmit receive transaction.
+  //
+  NetbufQueFlush (&Instance->ResponseQueue);
+  UdpIoCleanIo (Instance->UdpIo);
+  gBS->CloseProtocol (
+         Instance->UdpIo->UdpHandle,
+         &gEfiUdp4ProtocolGuid,
+         Instance->Service->Image,
+         Instance->Handle
+         );
+  UdpIoFreeIo (Instance->UdpIo);
+  Instance->UdpIo = NULL;
+  Instance->Token = NULL;
+
+  if (Token->CompletionEvent != NULL) {
+    gBS->SignalEvent (Token->CompletionEvent);
+  }
+}
+
+
+/**
+  Transmits a DHCP formatted packet and optionally waits for responses.
+
+  The TransmitReceive() function is used to transmit a DHCP packet and optionally
+  wait for the response from servers. This function does not change the state of
+  the EFI DHCPv4 Protocol driver and thus can be used at any time.
+
+  @param[in]  This    Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param[in]  Token   Pointer to the EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN structure.
+
+  @retval EFI_SUCCESS           The packet was successfully queued for transmission.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_NOT_READY         The previous call to this function has not finished yet. Try to call
+                                this function after collection process completes.
+  @retval EFI_NO_MAPPING        The default station address is not available yet.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval Others                Some other unexpected error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4TransmitReceive (
+  IN EFI_DHCP4_PROTOCOL                *This,
+  IN EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token
+  )
+{
+  DHCP_PROTOCOL  *Instance;
+  EFI_TPL        OldTpl;
+  EFI_STATUS     Status;
+  NET_FRAGMENT   Frag;
+  NET_BUF        *Wrap;
+  UDP_END_POINT  EndPoint;
+  IP4_ADDR       Ip;
+  DHCP_SERVICE   *DhcpSb;
+  EFI_IP_ADDRESS Gateway;
+  IP4_ADDR       ClientAddr;
+  INTN           Class;
+  IP4_ADDR       SubnetMask;
+
+  if ((This == NULL) || (Token == NULL) || (Token->Packet == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = DHCP_INSTANCE_FROM_THIS (This);
+  DhcpSb   = Instance->Service;
+
+  if (Instance->Token != NULL) {
+    //
+    // The previous call to TransmitReceive is not finished.
+    //
+    return EFI_NOT_READY;
+  }
+
+  if ((Token->Packet->Dhcp4.Magik != DHCP_OPTION_MAGIC)                   ||
+      (NTOHL (Token->Packet->Dhcp4.Header.Xid) == Instance->Service->Xid) ||
+      (Token->TimeoutValue == 0)                                          ||
+      ((Token->ListenPointCount != 0) && (Token->ListenPoints == NULL))   ||
+      EFI_ERROR (DhcpValidateOptions (Token->Packet, NULL))               ||
+      EFI_IP4_EQUAL (&Token->RemoteAddress, &mZeroIp4Addr)
+      ) {
+    //
+    // The DHCP packet isn't well-formed, the Transaction ID is already used,
+    // the timeout value is zero, the ListenPoint is invalid, or the
+    // RemoteAddress is zero.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ClientAddr = EFI_NTOHL (Token->Packet->Dhcp4.Header.ClientAddr);
+  
+  if (ClientAddr == 0) {
+    return EFI_NO_MAPPING;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Save the token and the timeout value.
+  //
+  Instance->Token   = Token;
+  Instance->Timeout = Token->TimeoutValue;
+
+  //
+  // Create a UDP IO for this transmit receive transaction.
+  //
+  Status = Dhcp4InstanceCreateUdpIo (Instance);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Save the Client Address is sent out
+  //
+  CopyMem (
+    &DhcpSb->ClientAddressSendOut[0],
+    &Token->Packet->Dhcp4.Header.ClientHwAddr[0],
+    Token->Packet->Dhcp4.Header.HwAddrLen
+    );
+
+  //
+  // Wrap the DHCP packet into a net buffer.
+  //
+  Frag.Bulk = (UINT8 *) &Token->Packet->Dhcp4;
+  Frag.Len  = Token->Packet->Length;
+  Wrap      = NetbufFromExt (&Frag, 1, 0, 0, DhcpDummyExtFree, NULL);
+  if (Wrap == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  //
+  // Set the local address and local port to ZERO.
+  //
+  ZeroMem (&EndPoint, sizeof (UDP_END_POINT));
+
+  //
+  // Set the destination address and destination port.
+  //
+  CopyMem (&Ip, &Token->RemoteAddress, sizeof (EFI_IPv4_ADDRESS));
+  EndPoint.RemoteAddr.Addr[0] = NTOHL (Ip);
+
+  if (Token->RemotePort == 0) {
+    EndPoint.RemotePort = DHCP_SERVER_PORT;
+  } else {
+    EndPoint.RemotePort = Token->RemotePort;
+  }
+
+  //
+  // Get the gateway.
+  //
+  Class = NetGetIpClass (ClientAddr);
+  ASSERT (Class < IP4_ADDR_CLASSE);
+  SubnetMask = gIp4AllMasks[Class << 3];
+  ZeroMem (&Gateway, sizeof (Gateway));
+  if (!IP4_NET_EQUAL (ClientAddr, EndPoint.RemoteAddr.Addr[0], SubnetMask)) {
+    CopyMem (&Gateway.v4, &Token->GatewayAddress, sizeof (EFI_IPv4_ADDRESS));
+    Gateway.Addr[0] = NTOHL (Gateway.Addr[0]);
+  }
+
+  //
+  // Transmit the DHCP packet.
+  //
+  Status = UdpIoSendDatagram (Instance->UdpIo, Wrap, &EndPoint, &Gateway, DhcpOnPacketSent, NULL);
+  if (EFI_ERROR (Status)) {
+    NetbufFree (Wrap);
+    goto ON_ERROR;
+  }
+
+  //
+  // Start to receive the DHCP response.
+  //
+  Status = UdpIoRecvDatagram (Instance->UdpIo, PxeDhcpInput, Instance, 0);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+ON_ERROR:
+
+  if (EFI_ERROR (Status) && (Instance->UdpIo != NULL)) {
+    UdpIoCleanIo (Instance->UdpIo);
+    gBS->CloseProtocol (
+           Instance->UdpIo->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           Instance->Service->Image,
+           Instance->Handle
+           );
+    UdpIoFreeIo (Instance->UdpIo);
+    Instance->UdpIo = NULL;
+    Instance->Token = NULL;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  if (!EFI_ERROR (Status) && (Token->CompletionEvent == NULL)) {
+    //
+    // Keep polling until timeout if no error happens and the CompletionEvent
+    // is NULL.
+    //
+    while (TRUE) {
+      OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+      //
+      // Raise TPL to protect the UDPIO in instance, in case that DhcpOnTimerTick
+      // free it when timeout.
+      //
+      if (Instance->Timeout > 0) {
+        Instance->UdpIo->Protocol.Udp4->Poll (Instance->UdpIo->Protocol.Udp4);
+        gBS->RestoreTPL (OldTpl);
+      } else {
+        gBS->RestoreTPL (OldTpl);
+        break;
+      }
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Callback function for DhcpIterateOptions. This callback sets the
+  EFI_DHCP4_PACKET_OPTION array in the DHCP_PARSE_CONTEXT to point
+  the individual DHCP option in the packet.
+
+  @param[in]  Tag                    The DHCP option type
+  @param[in]  Len                    Length of the DHCP option data
+  @param[in]  Data                   The DHCP option data
+  @param[in]  Context                The context, to pass several parameters in.
+
+  @retval EFI_SUCCESS            It always returns EFI_SUCCESS
+
+**/
+EFI_STATUS
+Dhcp4ParseCheckOption (
+  IN UINT8                  Tag,
+  IN UINT8                  Len,
+  IN UINT8                  *Data,
+  IN VOID                   *Context
+  )
+{
+  DHCP_PARSE_CONTEXT        *Parse;
+
+  Parse = (DHCP_PARSE_CONTEXT *) Context;
+  Parse->Index++;
+
+  if (Parse->Index <= Parse->OptionCount) {
+    //
+    // Use BASE_CR to get the memory position of EFI_DHCP4_PACKET_OPTION for
+    // the EFI_DHCP4_PACKET_OPTION->Data because DhcpIterateOptions only
+    // pass in the point to option data.
+    //
+    Parse->Option[Parse->Index - 1] = BASE_CR (Data, EFI_DHCP4_PACKET_OPTION, Data);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parses the packed DHCP option data.
+
+  The Parse() function is used to retrieve the option list from a DHCP packet.
+  If *OptionCount isn't zero, and there is enough space for all the DHCP options
+  in the Packet, each element of PacketOptionList is set to point to somewhere in
+  the Packet->Dhcp4.Option where a new DHCP option begins. If RFC3396 is supported,
+  the caller should reassemble the parsed DHCP options to get the finial result.
+  If *OptionCount is zero or there isn't enough space for all of them, the number
+  of DHCP options in the Packet is returned in OptionCount.
+
+  @param  This             Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param  Packet           Pointer to packet to be parsed.
+  @param  OptionCount      On input, the number of entries in the PacketOptionList.
+                           On output, the number of entries that were written into the
+                           PacketOptionList.
+  @param  PacketOptionList List of packet option entries to be filled in. End option or pad
+                           options are not included.
+
+  @retval EFI_SUCCESS           The packet was successfully parsed.
+  @retval EFI_INVALID_PARAMETER Some parameter is NULL.
+  @retval EFI_BUFFER_TOO_SMALL  One or more of the following conditions is TRUE:
+                                1) *OptionCount is smaller than the number of options that
+                                were found in the Packet.
+                                2) PacketOptionList is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiDhcp4Parse (
+  IN EFI_DHCP4_PROTOCOL       *This,
+  IN EFI_DHCP4_PACKET         *Packet,
+  IN OUT UINT32               *OptionCount,
+  OUT EFI_DHCP4_PACKET_OPTION *PacketOptionList[] OPTIONAL
+  )
+{
+  DHCP_PARSE_CONTEXT        Context;
+  EFI_STATUS                Status;
+
+  //
+  // First validate the parameters
+  //
+  if ((This == NULL) || (Packet == NULL) || (OptionCount == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet->Size < Packet->Length + 2 * sizeof (UINT32)) ||
+      (Packet->Dhcp4.Magik != DHCP_OPTION_MAGIC) ||
+      EFI_ERROR (DhcpValidateOptions (Packet, NULL))) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((*OptionCount != 0) && (PacketOptionList == NULL)) {
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  ZeroMem (PacketOptionList, *OptionCount * sizeof (EFI_DHCP4_PACKET_OPTION *));
+
+  Context.Option      = PacketOptionList;
+  Context.OptionCount = *OptionCount;
+  Context.Index       = 0;
+
+  Status              = DhcpIterateOptions (Packet, Dhcp4ParseCheckOption, &Context);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  *OptionCount = Context.Index;
+
+  if (Context.Index > Context.OptionCount) {
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the elapsed time based on the given instance and the pointer to the
+  elapsed time option.
+
+  @param[in]      Elapsed       The pointer to the position to append.
+  @param[in]      Instance      The pointer to the Dhcp4 instance.
+**/
+VOID
+SetElapsedTime (
+  IN     UINT16                 *Elapsed,
+  IN     DHCP_PROTOCOL          *Instance
+  )
+{
+  WriteUnaligned16 (Elapsed, HTONS(Instance->ElaspedTime));
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.h b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.h
new file mode 100644
index 0000000000..44213cf40a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Impl.h
@@ -0,0 +1,199 @@
+/** @file
+  EFI DHCP protocol implementation.
+  RFCs supported are:
+  RFC 2131: Dynamic Host Configuration Protocol
+  RFC 2132: DHCP Options and BOOTP Vendor Extensions
+  RFC 1534: Interoperation Between DHCP and BOOTP
+  RFC 3396: Encoding Long Options in DHCP.
+  
+Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef __EFI_DHCP4_IMPL_H__
+#define __EFI_DHCP4_IMPL_H__
+
+
+
+#include <Uefi.h>
+
+#include <Protocol/Dhcp4.h>
+#include <Protocol/Udp4.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/NetLib.h>
+
+typedef struct _DHCP_SERVICE  DHCP_SERVICE;
+typedef struct _DHCP_PROTOCOL DHCP_PROTOCOL;
+
+#include "Dhcp4Option.h"
+#include "Dhcp4Io.h"
+
+#define DHCP_SERVICE_SIGNATURE   SIGNATURE_32 ('D', 'H', 'C', 'P')
+#define DHCP_PROTOCOL_SIGNATURE  SIGNATURE_32 ('d', 'h', 'c', 'p')
+
+
+//
+// The state of the DHCP service. It starts as UNCONFIGED. If
+// and active child configures the service successfully, it
+// goes to CONFIGED. If the active child configures NULL, it
+// goes back to UNCONFIGED. It becomes DESTROY if it is (partly)
+// destroyed.
+//
+#define DHCP_UNCONFIGED          0
+#define DHCP_CONFIGED            1
+#define DHCP_DESTROY             2
+
+
+struct _DHCP_PROTOCOL {
+  UINT32                            Signature;
+  EFI_DHCP4_PROTOCOL                Dhcp4Protocol;
+  LIST_ENTRY                        Link;
+  EFI_HANDLE                        Handle;
+  DHCP_SERVICE                      *Service;
+
+  BOOLEAN                           InDestroy;
+
+  EFI_EVENT                         CompletionEvent;
+  EFI_EVENT                         RenewRebindEvent;
+
+  EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN  *Token;
+  UDP_IO                            *UdpIo; // The UDP IO used for TransmitReceive.
+  UINT32                            Timeout;
+  UINT16                            ElaspedTime;
+  NET_BUF_QUEUE                     ResponseQueue;
+};
+
+//
+// DHCP driver is specical in that it is a singleton. Although it
+// has a service binding, there can be only one active child.
+//
+struct _DHCP_SERVICE {
+  UINT32                        Signature;
+  EFI_SERVICE_BINDING_PROTOCOL  ServiceBinding;
+
+  INTN                          ServiceState; // CONFIGED, UNCONFIGED, and DESTROY
+
+  EFI_HANDLE                    Controller;
+  EFI_HANDLE                    Image;
+
+  LIST_ENTRY                    Children;
+  UINTN                         NumChildren;
+
+  INTN                          DhcpState;
+  EFI_STATUS                    IoStatus;     // the result of last user operation
+  UINT32                        Xid;
+
+  IP4_ADDR                      ClientAddr;   // lease IP or configured client address
+  IP4_ADDR                      Netmask;
+  IP4_ADDR                      ServerAddr;
+
+  EFI_DHCP4_PACKET              *LastOffer;   // The last received offer
+  EFI_DHCP4_PACKET              *Selected;
+  DHCP_PARAMETER                *Para;
+
+  UINT32                        Lease;
+  UINT32                        T1;
+  UINT32                        T2;
+  INTN                          ExtraRefresh; // This refresh is reqested by user
+
+  UDP_IO                        *UdpIo;       // Udp child receiving all DHCP message
+  UDP_IO                        *LeaseIoPort; // Udp child with lease IP
+  EFI_DHCP4_PACKET              *LastPacket;  // The last sent packet for retransmission
+  EFI_MAC_ADDRESS               Mac;
+  UINT8                         HwType;
+  UINT8                         HwLen;
+  UINT8                         ClientAddressSendOut[16];
+
+  DHCP_PROTOCOL                 *ActiveChild;
+  EFI_DHCP4_CONFIG_DATA         ActiveConfig;
+  UINT32                        UserOptionLen;
+
+  //
+  // Timer event and various timer
+  //
+  EFI_EVENT                     Timer;
+
+  UINT32                        PacketToLive; // Retransmission timer for our packets
+  UINT32                        LastTimeout;  // Record the init value of PacketToLive every time
+  INTN                          CurRetry;
+  INTN                          MaxRetries;
+  UINT32                        LeaseLife;
+};
+
+typedef struct {
+  EFI_DHCP4_PACKET_OPTION       **Option;
+  UINT32                        OptionCount;
+  UINT32                        Index;
+} DHCP_PARSE_CONTEXT;
+
+#define DHCP_INSTANCE_FROM_THIS(Proto)  \
+  CR ((Proto), DHCP_PROTOCOL, Dhcp4Protocol, DHCP_PROTOCOL_SIGNATURE)
+
+#define DHCP_SERVICE_FROM_THIS(Sb)      \
+  CR ((Sb), DHCP_SERVICE, ServiceBinding, DHCP_SERVICE_SIGNATURE)
+
+extern EFI_DHCP4_PROTOCOL mDhcp4ProtocolTemplate;
+
+/**
+  Give up the control of the DHCP service to let other child
+  resume. Don't change the service's DHCP state and the Client
+  address and option list configure as required by RFC2131.
+
+  @param  DhcpSb                 The DHCP service instance.
+
+**/
+VOID
+DhcpYieldControl (
+  IN DHCP_SERVICE           *DhcpSb
+  );
+
+/**
+  Complete a Dhcp4 transaction and signal the upper layer.
+
+  @param Instance      Dhcp4 instance.
+
+**/
+VOID
+PxeDhcpDone (
+  IN DHCP_PROTOCOL  *Instance
+  );
+
+/**
+  Free the resource related to the configure parameters.
+  DHCP driver will make a copy of the user's configure
+  such as the time out value.
+
+  @param  Config                 The DHCP configure data
+
+**/
+VOID
+DhcpCleanConfigure (
+  IN OUT EFI_DHCP4_CONFIG_DATA  *Config
+  );
+
+/**
+  Set the elapsed time based on the given instance and the pointer to the
+  elapsed time option.
+
+  @param[in]      Elapsed       The pointer to the position to append.
+  @param[in]      Instance      The pointer to the Dhcp4 instance.
+**/
+VOID
+SetElapsedTime (
+  IN     UINT16                 *Elapsed,
+  IN     DHCP_PROTOCOL          *Instance
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.c b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.c
new file mode 100644
index 0000000000..4bc991557a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.c
@@ -0,0 +1,1690 @@
+/** @file
+  EFI DHCP protocol implementation.
+  
+Copyright (c) 2006 - 2014, 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 "Dhcp4Impl.h"
+
+UINT32  mDhcp4DefaultTimeout[4] = { 4, 8, 16, 32 };
+
+
+/**
+  Send an initial DISCOVER or REQUEST message according to the
+  DHCP service's current state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+
+  @retval EFI_SUCCESS           The request has been sent
+  @retval other                 Some error occurs when sending the request.
+
+**/
+EFI_STATUS
+DhcpInitRequest (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  EFI_STATUS                Status;
+
+  ASSERT ((DhcpSb->DhcpState == Dhcp4Init) || (DhcpSb->DhcpState == Dhcp4InitReboot));
+
+  //
+  // Clear initial time to make sure that elapsed-time is set to 0 for first Discover or REQUEST message.
+  //
+  DhcpSb->ActiveChild->ElaspedTime= 0;
+  
+  if (DhcpSb->DhcpState == Dhcp4Init) {
+    DhcpSetState (DhcpSb, Dhcp4Selecting, FALSE);
+    Status = DhcpSendMessage (DhcpSb, NULL, NULL, DHCP_MSG_DISCOVER, NULL);
+
+    if (EFI_ERROR (Status)) {
+      DhcpSb->DhcpState = Dhcp4Init;
+      return Status;
+    }
+  } else {
+    DhcpSetState (DhcpSb, Dhcp4Rebooting, FALSE);
+    Status = DhcpSendMessage (DhcpSb, NULL, NULL, DHCP_MSG_REQUEST, NULL);
+
+    if (EFI_ERROR (Status)) {
+      DhcpSb->DhcpState = Dhcp4InitReboot;
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Call user provided callback function, and return the value the
+  function returns. If the user doesn't provide a callback, a
+  proper return value is selected to let the caller continue the
+  normal process.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Event                 The event as defined in the spec
+  @param[in]  Packet                The current packet trigger the event
+  @param[out] NewPacket             The user's return new packet
+
+  @retval EFI_NOT_READY         Direct the caller to continue collecting the offer.
+  @retval EFI_SUCCESS           The user function returns success.
+  @retval EFI_ABORTED           The user function ask it to abort.
+
+**/
+EFI_STATUS
+DhcpCallUser (
+  IN  DHCP_SERVICE          *DhcpSb,
+  IN  EFI_DHCP4_EVENT       Event,
+  IN  EFI_DHCP4_PACKET      *Packet,      OPTIONAL
+  OUT EFI_DHCP4_PACKET      **NewPacket   OPTIONAL
+  )
+{
+  EFI_DHCP4_CONFIG_DATA     *Config;
+  EFI_STATUS                Status;
+
+  if (NewPacket != NULL) {
+    *NewPacket = NULL;
+  }
+
+  //
+  // If user doesn't provide the call back function, return the value
+  // that directs the client to continue the normal process.
+  // In Dhcp4Selecting EFI_SUCCESS tells the client to stop collecting
+  // the offers and select a offer, EFI_NOT_READY tells the client to
+  // collect more offers.
+  //
+  Config = &DhcpSb->ActiveConfig;
+
+  if (Config->Dhcp4Callback == NULL) {
+    if (Event == Dhcp4RcvdOffer) {
+      return EFI_NOT_READY;
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  Status = Config->Dhcp4Callback (
+                     &DhcpSb->ActiveChild->Dhcp4Protocol,
+                     Config->CallbackContext,
+                     (EFI_DHCP4_STATE) DhcpSb->DhcpState,
+                     Event,
+                     Packet,
+                     NewPacket
+                     );
+
+  //
+  // User's callback should only return EFI_SUCCESS, EFI_NOT_READY,
+  // and EFI_ABORTED. If it returns values other than those, assume
+  // it to be EFI_ABORTED.
+  //
+  if ((Status == EFI_SUCCESS) || (Status == EFI_NOT_READY)) {
+    return Status;
+  }
+
+  return EFI_ABORTED;
+}
+
+
+/**
+  Notify the user about the operation result.
+
+  @param  DhcpSb                DHCP service instance
+  @param  Which                 Which notify function to signal
+
+**/
+VOID
+DhcpNotifyUser (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN INTN                   Which
+  )
+{
+  DHCP_PROTOCOL             *Child;
+
+  if ((Child = DhcpSb->ActiveChild) == NULL) {
+    return ;
+  }
+
+  if ((Child->CompletionEvent != NULL) &&
+      ((Which == DHCP_NOTIFY_COMPLETION) || (Which == DHCP_NOTIFY_ALL))
+      ) {
+
+    gBS->SignalEvent (Child->CompletionEvent);
+    Child->CompletionEvent = NULL;
+  }
+
+  if ((Child->RenewRebindEvent != NULL) &&
+      ((Which == DHCP_NOTIFY_RENEWREBIND) || (Which == DHCP_NOTIFY_ALL))
+      ) {
+
+    gBS->SignalEvent (Child->RenewRebindEvent);
+    Child->RenewRebindEvent = NULL;
+  }
+}
+
+
+
+/**
+  Set the DHCP state. If CallUser is true, it will try to notify
+  the user before change the state by DhcpNotifyUser. It returns
+  EFI_ABORTED if the user return EFI_ABORTED, otherwise, it returns
+  EFI_SUCCESS. If CallUser is FALSE, it isn't necessary to test
+  the return value of this function.
+
+  @param  DhcpSb                The DHCP service instance
+  @param  State                 The new DHCP state to change to
+  @param  CallUser              Whether we need to call user
+
+  @retval EFI_SUCCESS           The state is changed
+  @retval EFI_ABORTED           The user asks to abort the DHCP process.
+
+**/
+EFI_STATUS
+DhcpSetState (
+  IN OUT DHCP_SERVICE           *DhcpSb,
+  IN     INTN                   State,
+  IN     BOOLEAN                CallUser
+  )
+{
+  EFI_STATUS                Status;
+
+  if (CallUser) {
+    Status = EFI_SUCCESS;
+
+    if (State == Dhcp4Renewing) {
+      Status = DhcpCallUser (DhcpSb, Dhcp4EnterRenewing, NULL, NULL);
+
+    } else if (State == Dhcp4Rebinding) {
+      Status = DhcpCallUser (DhcpSb, Dhcp4EnterRebinding, NULL, NULL);
+
+    } else if (State == Dhcp4Bound) {
+      Status = DhcpCallUser (DhcpSb, Dhcp4BoundCompleted, NULL, NULL);
+
+    }
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Update the retransmission timer during the state transition.
+  // This will clear the retry count. This is also why the rule
+  // first transit the state, then send packets.
+  //
+  if (State == Dhcp4Selecting) {
+    DhcpSb->MaxRetries = DhcpSb->ActiveConfig.DiscoverTryCount;
+  } else {
+    DhcpSb->MaxRetries = DhcpSb->ActiveConfig.RequestTryCount;
+  }
+
+  if (DhcpSb->MaxRetries == 0) {
+    DhcpSb->MaxRetries = 4;
+  }
+
+  DhcpSb->CurRetry      = 0;
+  DhcpSb->PacketToLive  = 0;
+  DhcpSb->LastTimeout   = 0;
+  DhcpSb->DhcpState     = State;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Set the retransmit timer for the packet. It will select from either
+  the discover timeouts/request timeouts or the default timeout values.
+
+  @param  DhcpSb                The DHCP service instance.
+
+**/
+VOID
+DhcpSetTransmitTimer (
+  IN OUT DHCP_SERVICE           *DhcpSb
+  )
+{
+  UINT32                    *Times;
+
+  ASSERT (DhcpSb->MaxRetries > DhcpSb->CurRetry);
+
+  if (DhcpSb->DhcpState == Dhcp4Selecting) {
+    Times = DhcpSb->ActiveConfig.DiscoverTimeout;
+  } else {
+    Times = DhcpSb->ActiveConfig.RequestTimeout;
+  }
+
+  if (Times == NULL) {
+    Times = mDhcp4DefaultTimeout;
+  }
+
+  DhcpSb->PacketToLive = Times[DhcpSb->CurRetry];
+  DhcpSb->LastTimeout  = DhcpSb->PacketToLive;
+
+  return;
+}
+
+/**
+  Compute the lease. If the server grants a permanent lease, just
+  process it as a normal timeout value since the lease will last
+  more than 100 years.
+
+  @param  DhcpSb                The DHCP service instance
+  @param  Para                  The DHCP parameter extracted from the server's
+                                response.
+**/
+VOID
+DhcpComputeLease (
+  IN OUT DHCP_SERVICE           *DhcpSb,
+  IN     DHCP_PARAMETER         *Para
+  )
+{
+  ASSERT (Para != NULL);
+
+  DhcpSb->Lease = Para->Lease;
+  DhcpSb->T2    = Para->T2;
+  DhcpSb->T1    = Para->T1;
+
+  if (DhcpSb->Lease == 0) {
+    DhcpSb->Lease = DHCP_DEFAULT_LEASE;
+  }
+
+  if ((DhcpSb->T2 == 0) || (DhcpSb->T2 >= Para->Lease)) {
+    DhcpSb->T2 = Para->Lease - (Para->Lease >> 3);
+  }
+
+  if ((DhcpSb->T1 == 0) || (DhcpSb->T1 >= Para->T2)) {
+    DhcpSb->T1 = DhcpSb->Lease >> 1;
+  }
+}
+
+
+/**
+  Configure a UDP IO port to use the acquired lease address.
+  DHCP driver needs this port to unicast packet to the server
+  such as DHCP release.
+
+  @param[in]  UdpIo                 The UDP IO to configure
+  @param[in]  Context               Dhcp service instance.
+
+  @retval EFI_SUCCESS           The UDP IO port is successfully configured.
+  @retval Others                It failed to configure the port.
+
+**/
+EFI_STATUS
+EFIAPI
+DhcpConfigLeaseIoPort (
+  IN UDP_IO                 *UdpIo,
+  IN VOID                   *Context
+  )
+{
+  EFI_UDP4_CONFIG_DATA      UdpConfigData;
+  EFI_IPv4_ADDRESS          Subnet;
+  EFI_IPv4_ADDRESS          Gateway;
+  DHCP_SERVICE              *DhcpSb;
+  EFI_STATUS                Status;
+  IP4_ADDR                  Ip;
+
+  DhcpSb = (DHCP_SERVICE *) Context;
+
+  UdpConfigData.AcceptBroadcast     = FALSE;
+  UdpConfigData.AcceptPromiscuous   = FALSE;
+  UdpConfigData.AcceptAnyPort       = FALSE;
+  UdpConfigData.AllowDuplicatePort  = TRUE;
+  UdpConfigData.TypeOfService       = 0;
+  UdpConfigData.TimeToLive          = 64;
+  UdpConfigData.DoNotFragment       = FALSE;
+  UdpConfigData.ReceiveTimeout      = 1;
+  UdpConfigData.TransmitTimeout     = 0;
+
+  UdpConfigData.UseDefaultAddress   = FALSE;
+  UdpConfigData.StationPort         = DHCP_CLIENT_PORT;
+  UdpConfigData.RemotePort          = DHCP_SERVER_PORT;
+
+  Ip = HTONL (DhcpSb->ClientAddr);
+  CopyMem (&UdpConfigData.StationAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  Ip = HTONL (DhcpSb->Netmask);
+  CopyMem (&UdpConfigData.SubnetMask, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+  ZeroMem (&UdpConfigData.RemoteAddress, sizeof (EFI_IPv4_ADDRESS));
+
+  Status = UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, &UdpConfigData);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Add a default route if received from the server.
+  //
+  if ((DhcpSb->Para != NULL) && (DhcpSb->Para->Router != 0)) {
+    ZeroMem (&Subnet, sizeof (EFI_IPv4_ADDRESS));
+
+    Ip = HTONL (DhcpSb->Para->Router);
+    CopyMem (&Gateway, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+    UdpIo->Protocol.Udp4->Routes (UdpIo->Protocol.Udp4, FALSE, &Subnet, &Subnet, &Gateway);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Update the lease states when a new lease is acquired. It will not only
+  save the acquired the address and lease time, it will also create a UDP
+  child to provide address resolution for the address.
+
+  @param  DhcpSb                The DHCP service instance
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources.
+  @retval EFI_SUCCESS           The lease is recorded.
+
+**/
+EFI_STATUS
+DhcpLeaseAcquired (
+  IN OUT DHCP_SERVICE           *DhcpSb
+  )
+{
+  INTN                      Class;
+
+  DhcpSb->ClientAddr = EFI_NTOHL (DhcpSb->Selected->Dhcp4.Header.YourAddr);
+
+  if (DhcpSb->Para != NULL) {
+    DhcpSb->Netmask     = DhcpSb->Para->NetMask;
+    DhcpSb->ServerAddr  = DhcpSb->Para->ServerId;
+  }
+
+  if (DhcpSb->Netmask == 0) {
+    Class           = NetGetIpClass (DhcpSb->ClientAddr);
+    ASSERT (Class < IP4_ADDR_CLASSE);
+    DhcpSb->Netmask = gIp4AllMasks[Class << 3];
+  }
+
+  if (DhcpSb->LeaseIoPort != NULL) {
+    UdpIoFreeIo (DhcpSb->LeaseIoPort);
+  }
+
+  //
+  // Create a UDP/IP child to provide ARP service for the Leased IP,
+  // and transmit unicast packet with it as source address. Don't
+  // start receive on this port, the queued packet will be timeout.
+  //
+  DhcpSb->LeaseIoPort = UdpIoCreateIo (
+                          DhcpSb->Controller,
+                          DhcpSb->Image,
+                          DhcpConfigLeaseIoPort,
+                          UDP_IO_UDP4_VERSION,
+                          DhcpSb
+                          );
+
+  if (DhcpSb->LeaseIoPort == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (!DHCP_IS_BOOTP (DhcpSb->Para)) {
+    DhcpComputeLease (DhcpSb, DhcpSb->Para);
+  }
+
+  return DhcpSetState (DhcpSb, Dhcp4Bound, TRUE);
+}
+
+
+/**
+  Clean up the DHCP related states, IoStatus isn't reset.
+
+  @param  DhcpSb                The DHCP instance service.
+
+**/
+VOID
+DhcpCleanLease (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  DhcpSb->DhcpState   = Dhcp4Init;
+  DhcpSb->Xid         = DhcpSb->Xid + 1;
+  DhcpSb->ClientAddr  = 0;
+  DhcpSb->Netmask     = 0;
+  DhcpSb->ServerAddr  = 0;
+
+  if (DhcpSb->LastOffer != NULL) {
+    FreePool (DhcpSb->LastOffer);
+    DhcpSb->LastOffer = NULL;
+  }
+
+  if (DhcpSb->Selected != NULL) {
+    FreePool (DhcpSb->Selected);
+    DhcpSb->Selected = NULL;
+  }
+
+  if (DhcpSb->Para != NULL) {
+    FreePool (DhcpSb->Para);
+    DhcpSb->Para = NULL;
+  }
+
+  DhcpSb->Lease         = 0;
+  DhcpSb->T1            = 0;
+  DhcpSb->T2            = 0;
+  DhcpSb->ExtraRefresh  = FALSE;
+
+  if (DhcpSb->LeaseIoPort != NULL) {
+    UdpIoFreeIo (DhcpSb->LeaseIoPort);
+    DhcpSb->LeaseIoPort = NULL;
+  }
+
+  if (DhcpSb->LastPacket != NULL) {
+    FreePool (DhcpSb->LastPacket);
+    DhcpSb->LastPacket = NULL;
+  }
+
+  DhcpSb->PacketToLive  = 0;
+  DhcpSb->LastTimeout   = 0;
+  DhcpSb->CurRetry      = 0;
+  DhcpSb->MaxRetries    = 0;
+  DhcpSb->LeaseLife     = 0;
+
+  //
+  // Clean active config data.
+  //
+  DhcpCleanConfigure (&DhcpSb->ActiveConfig);
+}
+
+
+/**
+  Select a offer among all the offers collected. If the offer selected is
+  of BOOTP, the lease is recorded and user notified. If the offer is of
+  DHCP, it will request the offer from the server.
+
+  @param[in]  DhcpSb                The DHCP service instance.
+
+  @retval EFI_SUCCESS           One of the offer is selected.
+
+**/
+EFI_STATUS
+DhcpChooseOffer (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  EFI_DHCP4_PACKET          *Selected;
+  EFI_DHCP4_PACKET          *NewPacket;
+  EFI_DHCP4_PACKET          *TempPacket;
+  EFI_STATUS                Status;
+
+  ASSERT (DhcpSb->LastOffer != NULL);
+
+  //
+  // User will cache previous offers if he wants to select
+  // from multiple offers. If user provides an invalid packet,
+  // use the last offer, otherwise use the provided packet.
+  //
+  NewPacket = NULL;
+  Status    = DhcpCallUser (DhcpSb, Dhcp4SelectOffer, DhcpSb->LastOffer, &NewPacket);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Selected = DhcpSb->LastOffer;
+
+  if ((NewPacket != NULL) && !EFI_ERROR (DhcpValidateOptions (NewPacket, NULL))) {
+    TempPacket = (EFI_DHCP4_PACKET *) AllocatePool (NewPacket->Size);
+    if (TempPacket != NULL) {
+      CopyMem (TempPacket, NewPacket, NewPacket->Size);
+      FreePool (Selected);
+      Selected = TempPacket;
+    }
+  }
+
+  DhcpSb->Selected  = Selected;
+  DhcpSb->LastOffer = NULL;
+  DhcpSb->Para      = NULL;
+  DhcpValidateOptions (Selected, &DhcpSb->Para);
+
+  //
+  // A bootp offer has been selected, save the lease status,
+  // enter bound state then notify the user.
+  //
+  if (DHCP_IS_BOOTP (DhcpSb->Para)) {
+    Status = DhcpLeaseAcquired (DhcpSb);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    DhcpSb->IoStatus = EFI_SUCCESS;
+    DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_ALL);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Send a DHCP requests
+  //
+  Status = DhcpSetState (DhcpSb, Dhcp4Requesting, TRUE);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return DhcpSendMessage (DhcpSb, Selected, DhcpSb->Para, DHCP_MSG_REQUEST, NULL);
+}
+
+
+/**
+  Terminate the current address acquire. All the allocated resources
+  are released. Be careful when calling this function. A rule related
+  to this is: only call DhcpEndSession at the highest level, such as
+  DhcpInput, DhcpOnTimerTick...At the other level, just return error.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Status                The result of the DHCP process.
+
+**/
+VOID
+DhcpEndSession (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_STATUS             Status
+  )
+{
+  if (DHCP_CONNECTED (DhcpSb->DhcpState)) {
+    DhcpCallUser (DhcpSb, Dhcp4AddressLost, NULL, NULL);
+  } else {
+    DhcpCallUser (DhcpSb, Dhcp4Fail, NULL, NULL);
+  }
+
+  DhcpCleanLease (DhcpSb);
+
+  DhcpSb->IoStatus = Status;
+  DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_ALL);
+}
+
+
+/**
+  Handle packets in DHCP select state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Packet                The DHCP packet received
+  @param[in]  Para                  The DHCP parameter extracted from the packet. That
+                                    is, all the option value that we care.
+
+  @retval EFI_SUCCESS           The packet is successfully processed.
+  @retval Others                Some error occured.
+
+**/
+EFI_STATUS
+DhcpHandleSelect (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Packet,
+  IN DHCP_PARAMETER         *Para
+  )
+{
+  EFI_STATUS                Status;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // First validate the message:
+  // 1. the offer is a unicast
+  // 2. if it is a DHCP message, it must contains a server ID.
+  // Don't return a error for these two case otherwise the session is ended.
+  //
+  if (!DHCP_IS_BOOTP (Para) &&
+      ((Para->DhcpType != DHCP_MSG_OFFER) || (Para->ServerId == 0))
+      ) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Call the user's callback. The action according to the return is as:
+  // 1. EFI_SUCESS: stop waiting for more offers, select the offer now
+  // 2. EFI_NOT_READY: wait for more offers
+  // 3. EFI_ABORTED: abort the address acquiring.
+  //
+  Status = DhcpCallUser (DhcpSb, Dhcp4RcvdOffer, Packet, NULL);
+
+  if (Status == EFI_SUCCESS) {
+    if (DhcpSb->LastOffer != NULL) {
+      FreePool (DhcpSb->LastOffer);
+    }
+
+    DhcpSb->LastOffer = Packet;
+
+    return DhcpChooseOffer (DhcpSb);
+
+  } else if (Status == EFI_NOT_READY) {
+    if (DhcpSb->LastOffer != NULL) {
+      FreePool (DhcpSb->LastOffer);
+    }
+
+    DhcpSb->LastOffer = Packet;
+
+  } else if (Status == EFI_ABORTED) {
+    //
+    // DhcpInput will end the session upon error return. Remember
+    // only to call DhcpEndSession at the top level call.
+    //
+    goto ON_EXIT;
+  }
+
+  return EFI_SUCCESS;
+
+ON_EXIT:
+  FreePool (Packet);
+  return Status;
+}
+
+
+/**
+  Handle packets in DHCP request state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Packet                The DHCP packet received
+  @param[in]  Para                  The DHCP parameter extracted from the packet. That
+                                    is, all the option value that we care.
+
+  @retval EFI_SUCCESS           The packet is successfully processed.
+  @retval Others                Some error occured.
+
+**/
+EFI_STATUS
+DhcpHandleRequest (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Packet,
+  IN DHCP_PARAMETER         *Para
+  )
+{
+  EFI_DHCP4_HEADER          *Head;
+  EFI_DHCP4_HEADER          *Selected;
+  EFI_STATUS                Status;
+  UINT8                     *Message;
+
+  ASSERT (!DHCP_IS_BOOTP (DhcpSb->Para));
+
+  Head      = &Packet->Dhcp4.Header;
+  Selected  = &DhcpSb->Selected->Dhcp4.Header;
+
+  //
+  // Ignore the BOOTP message and DHCP messages other than DHCP ACK/NACK.
+  //
+  if (DHCP_IS_BOOTP (Para) ||
+      (Para->ServerId != DhcpSb->Para->ServerId) ||
+      ((Para->DhcpType != DHCP_MSG_ACK) && (Para->DhcpType != DHCP_MSG_NAK))
+      ) {
+
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Received a NAK, end the session no matter what the user returns
+  //
+  Status = EFI_DEVICE_ERROR;
+
+  if (Para->DhcpType == DHCP_MSG_NAK) {
+    DhcpCallUser (DhcpSb, Dhcp4RcvdNak, Packet, NULL);
+    goto ON_EXIT;
+  }
+
+  //
+  // Check whether the ACK matches the selected offer
+  //
+  Message = NULL;
+
+  if (!EFI_IP4_EQUAL (&Head->YourAddr, &Selected->YourAddr)) {
+    Message = (UINT8 *) "Lease confirmed isn't the same as that in the offer";
+    goto REJECT;
+  }
+
+  Status = DhcpCallUser (DhcpSb, Dhcp4RcvdAck, Packet, NULL);
+
+  if (EFI_ERROR (Status)) {
+    Message = (UINT8 *) "Lease is denied upon received ACK";
+    goto REJECT;
+  }
+
+  //
+  // Record the lease, transit to BOUND state, then notify the user
+  //
+  Status = DhcpLeaseAcquired (DhcpSb);
+
+  if (EFI_ERROR (Status)) {
+    Message = (UINT8 *) "Lease is denied upon entering bound";
+    goto REJECT;
+  }
+
+  DhcpSb->IoStatus = EFI_SUCCESS;
+  DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_COMPLETION);
+
+  FreePool (Packet);
+  return EFI_SUCCESS;
+
+REJECT:
+  DhcpSendMessage (DhcpSb, DhcpSb->Selected, DhcpSb->Para, DHCP_MSG_DECLINE, Message);
+
+ON_EXIT:
+  FreePool (Packet);
+  return Status;
+}
+
+
+/**
+  Handle packets in DHCP renew/rebound state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Packet                The DHCP packet received
+  @param[in]  Para                  The DHCP parameter extracted from the packet. That
+                                    is, all the option value that we care.
+
+  @retval EFI_SUCCESS           The packet is successfully processed.
+  @retval Others                Some error occured.
+
+**/
+EFI_STATUS
+DhcpHandleRenewRebind (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Packet,
+  IN DHCP_PARAMETER         *Para
+  )
+{
+  EFI_DHCP4_HEADER          *Head;
+  EFI_DHCP4_HEADER          *Selected;
+  EFI_STATUS                Status;
+
+  ASSERT (!DHCP_IS_BOOTP (DhcpSb->Para));
+
+  Head      = &Packet->Dhcp4.Header;
+  Selected  = &DhcpSb->Selected->Dhcp4.Header;
+
+  //
+  // Ignore the BOOTP message and DHCP messages other than DHCP ACK/NACK
+  //
+  if (DHCP_IS_BOOTP (Para) ||
+      (Para->ServerId != DhcpSb->Para->ServerId) ||
+      ((Para->DhcpType != DHCP_MSG_ACK) && (Para->DhcpType != DHCP_MSG_NAK))
+      ) {
+
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Received a NAK, ignore the user's return then terminate the process
+  //
+  Status = EFI_DEVICE_ERROR;
+
+  if (Para->DhcpType == DHCP_MSG_NAK) {
+    DhcpCallUser (DhcpSb, Dhcp4RcvdNak, Packet, NULL);
+    goto ON_EXIT;
+  }
+
+  //
+  // The lease is different from the selected. Don't send a DECLINE
+  // since it isn't existed in the client's FSM.
+  //
+  if (!EFI_IP4_EQUAL (&Head->YourAddr, &Selected->YourAddr)) {
+    goto ON_EXIT;
+  }
+
+  Status = DhcpCallUser (DhcpSb, Dhcp4RcvdAck, Packet, NULL);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Record the lease, start timer for T1 and T2,
+  //
+  DhcpComputeLease (DhcpSb, Para);
+  DhcpSb->LeaseLife = 0;
+  DhcpSetState (DhcpSb, Dhcp4Bound, TRUE);
+
+  if (DhcpSb->ExtraRefresh != 0) {
+    DhcpSb->ExtraRefresh  = FALSE;
+
+    DhcpSb->IoStatus      = EFI_SUCCESS;
+    DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_RENEWREBIND);
+  }
+
+ON_EXIT:
+  FreePool (Packet);
+  return Status;
+}
+
+
+/**
+  Handle packets in DHCP reboot state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Packet                The DHCP packet received
+  @param[in]  Para                  The DHCP parameter extracted from the packet. That
+                                    is, all the option value that we care.
+
+  @retval EFI_SUCCESS           The packet is successfully processed.
+  @retval Others                Some error occured.
+
+**/
+EFI_STATUS
+DhcpHandleReboot (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Packet,
+  IN DHCP_PARAMETER         *Para
+  )
+{
+  EFI_DHCP4_HEADER          *Head;
+  EFI_STATUS                Status;
+
+  Head = &Packet->Dhcp4.Header;
+
+  //
+  // Ignore the BOOTP message and DHCP messages other than DHCP ACK/NACK
+  //
+  if (DHCP_IS_BOOTP (Para) ||
+      ((Para->DhcpType != DHCP_MSG_ACK) && (Para->DhcpType != DHCP_MSG_NAK))
+      ) {
+
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // If a NAK is received, transit to INIT and try again.
+  //
+  if (Para->DhcpType == DHCP_MSG_NAK) {
+    DhcpCallUser (DhcpSb, Dhcp4RcvdNak, Packet, NULL);
+
+    DhcpSb->ClientAddr  = 0;
+    DhcpSb->DhcpState   = Dhcp4Init;
+
+    Status              = DhcpInitRequest (DhcpSb);
+    goto ON_EXIT;
+  }
+
+  //
+  // Check whether the ACK matches the selected offer
+  //
+  if (EFI_NTOHL (Head->YourAddr) != DhcpSb->ClientAddr) {
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = DhcpCallUser (DhcpSb, Dhcp4RcvdAck, Packet, NULL);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // OK, get the parameter from server, record the lease
+  //
+  DhcpSb->Para = AllocateCopyPool (sizeof (DHCP_PARAMETER), Para);
+  if (DhcpSb->Para == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  DhcpSb->Selected  = Packet;
+  Status            = DhcpLeaseAcquired (DhcpSb);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DhcpSb->IoStatus = EFI_SUCCESS;
+  DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_COMPLETION);
+  return EFI_SUCCESS;
+
+ON_EXIT:
+  FreePool (Packet);
+  return Status;
+}
+
+
+/**
+  Handle the received DHCP packets. This function drives the DHCP
+  state machine.
+
+  @param  UdpPacket             The UDP packets received.
+  @param  EndPoint              The local/remote UDP access point
+  @param  IoStatus              The status of the UDP receive
+  @param  Context               The opaque parameter to the function.
+
+**/
+VOID
+EFIAPI
+DhcpInput (
+  NET_BUF                   *UdpPacket,
+  UDP_END_POINT             *EndPoint,
+  EFI_STATUS                IoStatus,
+  VOID                      *Context
+  )
+{
+  DHCP_SERVICE              *DhcpSb;
+  EFI_DHCP4_HEADER          *Head;
+  EFI_DHCP4_PACKET          *Packet;
+  DHCP_PARAMETER            *Para;
+  EFI_STATUS                Status;
+  UINT32                    Len;
+
+  Packet  = NULL;
+  DhcpSb  = (DHCP_SERVICE *) Context;
+
+  //
+  // Don't restart receive if error occurs or DHCP is destroyed.
+  //
+  if (EFI_ERROR (IoStatus)) {
+    return ;
+  } else if (DhcpSb->ServiceState == DHCP_DESTROY) {
+    NetbufFree (UdpPacket);
+    return ;
+  }
+
+  ASSERT (UdpPacket != NULL);
+
+  if (DhcpSb->DhcpState == Dhcp4Stopped) {
+    goto RESTART;
+  }
+
+  //
+  // Validate the packet received
+  //
+  if (UdpPacket->TotalSize < sizeof (EFI_DHCP4_HEADER)) {
+    goto RESTART;
+  }
+
+  //
+  // Copy the DHCP message to a continuous memory block
+  //
+  Len     = sizeof (EFI_DHCP4_PACKET) + UdpPacket->TotalSize - sizeof (EFI_DHCP4_HEADER);
+  Packet  = (EFI_DHCP4_PACKET *) AllocatePool (Len);
+
+  if (Packet == NULL) {
+    goto RESTART;
+  }
+
+  Packet->Size    = Len;
+  Head            = &Packet->Dhcp4.Header;
+  Packet->Length  = NetbufCopy (UdpPacket, 0, UdpPacket->TotalSize, (UINT8 *) Head);
+
+  if (Packet->Length != UdpPacket->TotalSize) {
+    goto RESTART;
+  }
+
+  //
+  // Is this packet the answer to our packet?
+  //
+  if ((Head->OpCode != BOOTP_REPLY) ||
+      (NTOHL (Head->Xid) != DhcpSb->Xid) ||
+      (CompareMem (DhcpSb->ClientAddressSendOut, Head->ClientHwAddr, Head->HwAddrLen) != 0)) {
+    goto RESTART;
+  }
+
+  //
+  // Validate the options and retrieve the interested options
+  //
+  Para = NULL;
+  if ((Packet->Length > sizeof (EFI_DHCP4_HEADER) + sizeof (UINT32)) &&
+      (Packet->Dhcp4.Magik == DHCP_OPTION_MAGIC) &&
+      EFI_ERROR (DhcpValidateOptions (Packet, &Para))) {
+
+    goto RESTART;
+  }
+
+  //
+  // Call the handler for each state. The handler should return
+  // EFI_SUCCESS if the process can go on no matter whether the
+  // packet is ignored or not. If the return is EFI_ERROR, the
+  // session will be terminated. Packet's ownership is handled
+  // over to the handlers. If operation succeeds, the handler
+  // must notify the user. It isn't necessary to do if EFI_ERROR
+  // is returned because the DhcpEndSession will notify the user.
+  //
+  Status = EFI_SUCCESS;
+
+  switch (DhcpSb->DhcpState) {
+  case Dhcp4Selecting:
+    Status = DhcpHandleSelect (DhcpSb, Packet, Para);
+    break;
+
+  case Dhcp4Requesting:
+    Status = DhcpHandleRequest (DhcpSb, Packet, Para);
+    break;
+
+  case Dhcp4InitReboot:
+  case Dhcp4Init:
+  case Dhcp4Bound:
+    //
+    // Ignore the packet in INITREBOOT, INIT and BOUND states
+    //
+    FreePool (Packet);
+    Status = EFI_SUCCESS;
+    break;
+
+  case Dhcp4Renewing:
+  case Dhcp4Rebinding:
+    Status = DhcpHandleRenewRebind (DhcpSb, Packet, Para);
+    break;
+
+  case Dhcp4Rebooting:
+    Status = DhcpHandleReboot (DhcpSb, Packet, Para);
+    break;
+  }
+
+  if (Para != NULL) {
+    FreePool (Para);
+  }
+
+  Packet = NULL;
+
+  if (EFI_ERROR (Status)) {
+    NetbufFree (UdpPacket);
+    UdpIoRecvDatagram (DhcpSb->UdpIo, DhcpInput, DhcpSb, 0);
+    DhcpEndSession (DhcpSb, Status);
+    return ;
+  }
+
+RESTART:
+  NetbufFree (UdpPacket);
+
+  if (Packet != NULL) {
+    FreePool (Packet);
+  }
+
+  Status = UdpIoRecvDatagram (DhcpSb->UdpIo, DhcpInput, DhcpSb, 0);
+
+  if (EFI_ERROR (Status)) {
+    DhcpEndSession (DhcpSb, EFI_DEVICE_ERROR);
+  }
+}
+
+
+/**
+  Release the packet.
+
+  @param[in]  Arg                   The packet to release
+
+**/
+VOID
+EFIAPI
+DhcpReleasePacket (
+  IN VOID                   *Arg
+  )
+{
+  FreePool (Arg);
+}
+
+
+/**
+  Release the net buffer when packet is sent.
+
+  @param  UdpPacket             The UDP packets received.
+  @param  EndPoint              The local/remote UDP access point
+  @param  IoStatus              The status of the UDP receive
+  @param  Context               The opaque parameter to the function.
+
+**/
+VOID
+EFIAPI
+DhcpOnPacketSent (
+  NET_BUF                   *Packet,
+  UDP_END_POINT             *EndPoint,
+  EFI_STATUS                IoStatus,
+  VOID                      *Context
+  )
+{
+  NetbufFree (Packet);
+}
+
+
+
+/**
+  Build and transmit a DHCP message according to the current states.
+  This function implement the Table 5. of RFC 2131. Always transits
+  the state (as defined in Figure 5. of the same RFC) before sending
+  a DHCP message. The table is adjusted accordingly.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Seed                  The seed packet which the new packet is based on
+  @param[in]  Para                  The DHCP parameter of the Seed packet
+  @param[in]  Type                  The message type to send
+  @param[in]  Msg                   The human readable message to include in the packet
+                                    sent.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources for the packet
+  @retval EFI_ACCESS_DENIED     Failed to transmit the packet through UDP
+  @retval EFI_SUCCESS           The message is sent
+  @retval other                 Other error occurs
+
+**/
+EFI_STATUS
+DhcpSendMessage (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Seed,
+  IN DHCP_PARAMETER         *Para,
+  IN UINT8                  Type,
+  IN UINT8                  *Msg
+  )
+{
+  EFI_DHCP4_CONFIG_DATA     *Config;
+  EFI_DHCP4_PACKET          *Packet;
+  EFI_DHCP4_PACKET          *NewPacket;
+  EFI_DHCP4_HEADER          *Head;
+  EFI_DHCP4_HEADER          *SeedHead;
+  UDP_IO                    *UdpIo;
+  UDP_END_POINT             EndPoint;
+  NET_BUF                   *Wrap;
+  NET_FRAGMENT              Frag;
+  EFI_STATUS                Status;
+  IP4_ADDR                  IpAddr;
+  UINT8                     *Buf;
+  UINT16                    MaxMsg;
+  UINT32                    Len;
+  UINT32                    Index;
+
+  //
+  // Allocate a big enough memory block to hold the DHCP packet
+  //
+  Len = sizeof (EFI_DHCP4_PACKET) + 128 + DhcpSb->UserOptionLen;
+
+  if (Msg != NULL) {
+    Len += (UINT32)AsciiStrLen ((CHAR8 *) Msg);
+  }
+
+  Packet = AllocatePool (Len);
+
+  if (Packet == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Packet->Size    = Len;
+  Packet->Length  = sizeof (EFI_DHCP4_HEADER) + sizeof (UINT32);
+
+  //
+  // Fill in the DHCP header fields
+  //
+  Config    = &DhcpSb->ActiveConfig;
+  SeedHead  = NULL;
+
+  if (Seed != NULL) {
+    SeedHead = &Seed->Dhcp4.Header;
+  }
+
+  Head = &Packet->Dhcp4.Header;
+  ZeroMem (Head, sizeof (EFI_DHCP4_HEADER));
+
+  Head->OpCode       = BOOTP_REQUEST;
+  Head->HwType       = DhcpSb->HwType;
+  Head->HwAddrLen    = DhcpSb->HwLen;
+  Head->Xid          = HTONL (DhcpSb->Xid);
+  Head->Reserved     = HTONS (0x8000);  //Server, broadcast the message please.
+
+  EFI_IP4 (Head->ClientAddr) = HTONL (DhcpSb->ClientAddr);
+  CopyMem (Head->ClientHwAddr, DhcpSb->Mac.Addr, DhcpSb->HwLen);
+
+  if ((Type == DHCP_MSG_DECLINE) || (Type == DHCP_MSG_RELEASE)) {
+    Head->Seconds = 0;
+  } else if ((Type == DHCP_MSG_REQUEST) && (DhcpSb->DhcpState == Dhcp4Requesting)) {
+    //
+    // Use the same value as the original DHCPDISCOVER message.
+    //
+    Head->Seconds = DhcpSb->LastPacket->Dhcp4.Header.Seconds;
+  } else {
+    SetElapsedTime(&Head->Seconds, DhcpSb->ActiveChild);
+  }
+
+  //
+  // Append the DHCP message type
+  //
+  Packet->Dhcp4.Magik = DHCP_OPTION_MAGIC;
+  Buf                 = Packet->Dhcp4.Option;
+  Buf                 = DhcpAppendOption (Buf, DHCP_TAG_TYPE, 1, &Type);
+
+  //
+  // Append the serverid option if necessary:
+  //   1. DHCP decline message
+  //   2. DHCP release message
+  //   3. DHCP request to confirm one lease.
+  //
+  if ((Type == DHCP_MSG_DECLINE) || (Type == DHCP_MSG_RELEASE) ||
+      ((Type == DHCP_MSG_REQUEST) && (DhcpSb->DhcpState == Dhcp4Requesting))
+      ) {
+
+    ASSERT ((Para != NULL) && (Para->ServerId != 0));
+
+    IpAddr  = HTONL (Para->ServerId);
+    Buf     = DhcpAppendOption (Buf, DHCP_TAG_SERVER_ID, 4, (UINT8 *) &IpAddr);
+  }
+
+  //
+  // Append the requested IP option if necessary:
+  //   1. DHCP request to use the previously allocated address
+  //   2. DHCP request to confirm one lease
+  //   3. DHCP decline to decline one lease
+  //
+  IpAddr = 0;
+
+  if (Type == DHCP_MSG_REQUEST) {
+    if (DhcpSb->DhcpState == Dhcp4Rebooting) {
+      IpAddr = EFI_IP4 (Config->ClientAddress);
+
+    } else if (DhcpSb->DhcpState == Dhcp4Requesting) {
+      ASSERT (SeedHead != NULL);
+      IpAddr = EFI_IP4 (SeedHead->YourAddr);
+    }
+
+  } else if (Type == DHCP_MSG_DECLINE) {
+    ASSERT (SeedHead != NULL);
+    IpAddr = EFI_IP4 (SeedHead->YourAddr);
+  }
+
+  if (IpAddr != 0) {
+    Buf = DhcpAppendOption (Buf, DHCP_TAG_REQUEST_IP, 4, (UINT8 *) &IpAddr);
+  }
+
+  //
+  // Append the Max Message Length option if it isn't a DECLINE
+  // or RELEASE to direct the server use large messages instead of
+  // override the BOOTFILE and SERVER fields in the message head.
+  //
+  if ((Type != DHCP_MSG_DECLINE) && (Type != DHCP_MSG_RELEASE)) {
+    MaxMsg  = HTONS (0xFF00);
+    Buf     = DhcpAppendOption (Buf, DHCP_TAG_MAXMSG, 2, (UINT8 *) &MaxMsg);
+  }
+
+  //
+  // Append the user's message if it isn't NULL
+  //
+  if (Msg != NULL) {
+    Len     = MIN ((UINT32) AsciiStrLen ((CHAR8 *) Msg), 255);
+    Buf     = DhcpAppendOption (Buf, DHCP_TAG_MESSAGE, (UINT16) Len, Msg);
+  }
+
+  //
+  // Append the user configured options
+  //
+  if (DhcpSb->UserOptionLen != 0) {
+    for (Index = 0; Index < Config->OptionCount; Index++) {
+      //
+      // We can't use any option other than the client ID from user
+      // if it is a DHCP decline or DHCP release .
+      //
+      if (((Type == DHCP_MSG_DECLINE) || (Type == DHCP_MSG_RELEASE)) &&
+          (Config->OptionList[Index]->OpCode != DHCP_TAG_CLIENT_ID)) {
+        continue;
+      }
+
+      Buf = DhcpAppendOption (
+              Buf,
+              Config->OptionList[Index]->OpCode,
+              Config->OptionList[Index]->Length,
+              Config->OptionList[Index]->Data
+              );
+    }
+  }
+
+  *(Buf++) = DHCP_TAG_EOP;
+  Packet->Length += (UINT32) (Buf - Packet->Dhcp4.Option);
+
+  //
+  // OK, the message is built, call the user to override it.
+  //
+  Status    = EFI_SUCCESS;
+  NewPacket = NULL;
+
+  if (Type == DHCP_MSG_DISCOVER) {
+    Status = DhcpCallUser (DhcpSb, Dhcp4SendDiscover, Packet, &NewPacket);
+
+  } else if (Type == DHCP_MSG_REQUEST) {
+    Status = DhcpCallUser (DhcpSb, Dhcp4SendRequest, Packet, &NewPacket);
+
+  } else if (Type == DHCP_MSG_DECLINE) {
+    Status = DhcpCallUser (DhcpSb, Dhcp4SendDecline, Packet, &NewPacket);
+  }
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Packet);
+    return Status;
+  }
+
+  if (NewPacket != NULL) {
+    FreePool (Packet);
+    Packet = NewPacket;
+  }
+
+  //
+  // Save the Client Address will be sent out
+  //
+  CopyMem (
+    &DhcpSb->ClientAddressSendOut[0],
+    &Packet->Dhcp4.Header.ClientHwAddr[0],
+    Packet->Dhcp4.Header.HwAddrLen
+    );
+
+
+  //
+  // Wrap it into a netbuf then send it.
+  //
+  Frag.Bulk = (UINT8 *) &Packet->Dhcp4.Header;
+  Frag.Len  = Packet->Length;
+  Wrap      = NetbufFromExt (&Frag, 1, 0, 0, DhcpReleasePacket, Packet);
+
+  if (Wrap == NULL) {
+    FreePool (Packet);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Save it as the last sent packet for retransmission
+  //
+  if (DhcpSb->LastPacket != NULL) {
+    FreePool (DhcpSb->LastPacket);
+  }
+
+  DhcpSb->LastPacket = Packet;
+  DhcpSetTransmitTimer (DhcpSb);
+
+  //
+  // Broadcast the message, unless we know the server address.
+  // Use the lease UdpIo port to send the unicast packet.
+  //
+  EndPoint.RemoteAddr.Addr[0] = 0xffffffff;
+  EndPoint.LocalAddr.Addr[0]  = 0;
+  EndPoint.RemotePort         = DHCP_SERVER_PORT;
+  EndPoint.LocalPort          = DHCP_CLIENT_PORT;
+  UdpIo                       = DhcpSb->UdpIo;
+
+  if ((DhcpSb->DhcpState == Dhcp4Renewing) || (Type == DHCP_MSG_RELEASE)) {
+    EndPoint.RemoteAddr.Addr[0] = DhcpSb->ServerAddr;
+    EndPoint.LocalAddr.Addr[0]  = DhcpSb->ClientAddr;
+    UdpIo                       = DhcpSb->LeaseIoPort;
+  }
+
+  ASSERT (UdpIo != NULL);
+  NET_GET_REF (Wrap);
+  
+  Status = UdpIoSendDatagram (
+             UdpIo, 
+             Wrap, 
+             &EndPoint, 
+             NULL, 
+             DhcpOnPacketSent, 
+             DhcpSb
+             );
+
+  if (EFI_ERROR (Status)) {
+    NET_PUT_REF (Wrap);
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Retransmit a saved packet. Only DISCOVER and REQUEST messages
+  will be retransmitted.
+
+  @param[in]  DhcpSb                The DHCP service instance
+
+  @retval EFI_ACCESS_DENIED     Failed to transmit packet through UDP port
+  @retval EFI_SUCCESS           The packet is retransmitted.
+
+**/
+EFI_STATUS
+DhcpRetransmit (
+  IN DHCP_SERVICE           *DhcpSb
+  )
+{
+  UDP_IO                    *UdpIo;
+  UDP_END_POINT             EndPoint;
+  NET_BUF                   *Wrap;
+  NET_FRAGMENT              Frag;
+  EFI_STATUS                Status;
+
+  ASSERT (DhcpSb->LastPacket != NULL);
+
+  //
+  // For REQUEST message in Dhcp4Requesting state, do not change the secs fields.
+  //
+  if (DhcpSb->DhcpState != Dhcp4Requesting) {
+    SetElapsedTime(&DhcpSb->LastPacket->Dhcp4.Header.Seconds, DhcpSb->ActiveChild);
+  }
+
+  //
+  // Wrap it into a netbuf then send it.
+  //
+  Frag.Bulk = (UINT8 *) &DhcpSb->LastPacket->Dhcp4.Header;
+  Frag.Len  = DhcpSb->LastPacket->Length;
+  Wrap      = NetbufFromExt (&Frag, 1, 0, 0, DhcpReleasePacket, DhcpSb->LastPacket);
+
+  if (Wrap == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Broadcast the message, unless we know the server address.
+  //
+  EndPoint.RemotePort         = DHCP_SERVER_PORT;
+  EndPoint.LocalPort          = DHCP_CLIENT_PORT;
+  EndPoint.RemoteAddr.Addr[0] = 0xffffffff;
+  EndPoint.LocalAddr.Addr[0]  = 0;
+  UdpIo                       = DhcpSb->UdpIo;
+
+  if (DhcpSb->DhcpState == Dhcp4Renewing) {
+    EndPoint.RemoteAddr.Addr[0] = DhcpSb->ServerAddr;
+    EndPoint.LocalAddr.Addr[0]  = DhcpSb->ClientAddr;
+    UdpIo                       = DhcpSb->LeaseIoPort;
+  }
+
+  ASSERT (UdpIo != NULL);
+
+  NET_GET_REF (Wrap);
+  Status = UdpIoSendDatagram (
+             UdpIo,
+             Wrap,
+             &EndPoint,
+             NULL,
+             DhcpOnPacketSent,
+             DhcpSb
+             );
+
+  if (EFI_ERROR (Status)) {
+    NET_PUT_REF (Wrap);
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Each DHCP service has three timer. Two of them are count down timer.
+  One for the packet retransmission. The other is to collect the offers.
+  The third timer increaments the lease life which is compared to T1, T2,
+  and lease to determine the time to renew and rebind the lease.
+  DhcpOnTimerTick will be called once every second.
+
+  @param[in]  Event                 The timer event
+  @param[in]  Context               The context, which is the DHCP service instance.
+
+**/
+VOID
+EFIAPI
+DhcpOnTimerTick (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  DHCP_SERVICE              *DhcpSb;
+  DHCP_PROTOCOL             *Instance;
+  EFI_STATUS                Status;
+
+  DhcpSb   = (DHCP_SERVICE *) Context;
+  Instance = DhcpSb->ActiveChild;
+
+  //
+  // 0xffff is the maximum supported value for elapsed time according to RFC.
+  //
+  if (Instance != NULL && Instance->ElaspedTime < 0xffff) {
+    Instance->ElaspedTime++;
+  }
+  
+  //
+  // Check the retransmit timer
+  //
+  if ((DhcpSb->PacketToLive > 0) && (--DhcpSb->PacketToLive == 0)) {
+
+    //
+    // Select offer at each timeout if any offer received.
+    //
+    if (DhcpSb->DhcpState == Dhcp4Selecting && DhcpSb->LastOffer != NULL) {
+
+      Status = DhcpChooseOffer (DhcpSb);
+
+      if (EFI_ERROR(Status)) {
+        if (DhcpSb->LastOffer != NULL) {
+          FreePool (DhcpSb->LastOffer);
+          DhcpSb->LastOffer = NULL;
+        }
+      } else {
+        goto ON_EXIT;
+      }
+    }
+    
+    if (++DhcpSb->CurRetry < DhcpSb->MaxRetries) {
+      //
+      // Still has another try
+      //
+      DhcpRetransmit (DhcpSb);
+      DhcpSetTransmitTimer (DhcpSb);
+
+    } else if (DHCP_CONNECTED (DhcpSb->DhcpState)) {
+
+      //
+      // Retransmission failed, if the DHCP request is initiated by
+      // user, adjust the current state according to the lease life.
+      // Otherwise do nothing to wait the lease to timeout
+      //
+      if (DhcpSb->ExtraRefresh != 0) {
+        Status = EFI_SUCCESS;
+
+        if (DhcpSb->LeaseLife < DhcpSb->T1) {
+          Status = DhcpSetState (DhcpSb, Dhcp4Bound, FALSE);
+
+        } else if (DhcpSb->LeaseLife < DhcpSb->T2) {
+          Status = DhcpSetState (DhcpSb, Dhcp4Renewing, FALSE);
+
+        } else if (DhcpSb->LeaseLife < DhcpSb->Lease) {
+          Status = DhcpSetState (DhcpSb, Dhcp4Rebinding, FALSE);
+
+        } else {
+          goto END_SESSION;
+
+        }
+
+        DhcpSb->IoStatus = EFI_TIMEOUT;
+        DhcpNotifyUser (DhcpSb, DHCP_NOTIFY_RENEWREBIND);
+      }
+    } else {
+      goto END_SESSION;
+    }
+  }
+  
+  //
+  // If an address has been acquired, check whether need to
+  // refresh or whether it has expired.
+  //
+  if (DHCP_CONNECTED (DhcpSb->DhcpState)) {
+    DhcpSb->LeaseLife++;
+
+    //
+    // Don't timeout the lease, only count the life if user is
+    // requesting extra renew/rebind. Adjust the state after that.
+    //
+    if (DhcpSb->ExtraRefresh != 0) {
+      return ;
+    }
+
+    if (DhcpSb->LeaseLife == DhcpSb->Lease) {
+      //
+      // Lease expires, end the session
+      //
+      goto END_SESSION;
+
+    } else if (DhcpSb->LeaseLife == DhcpSb->T2) {
+      //
+      // T2 expires, transit to rebinding then send a REQUEST to any server
+      //
+      if (EFI_ERROR (DhcpSetState (DhcpSb, Dhcp4Rebinding, TRUE))) {
+        goto END_SESSION;
+      }
+
+      if (Instance != NULL) {
+        Instance->ElaspedTime= 0;
+      }      
+      
+      Status = DhcpSendMessage (
+                 DhcpSb,
+                 DhcpSb->Selected,
+                 DhcpSb->Para,
+                 DHCP_MSG_REQUEST,
+                 NULL
+                 );
+
+      if (EFI_ERROR (Status)) {
+        goto END_SESSION;
+      }
+
+    } else if (DhcpSb->LeaseLife == DhcpSb->T1) {
+      //
+      // T1 expires, transit to renewing, then send a REQUEST to the server
+      //
+      if (EFI_ERROR (DhcpSetState (DhcpSb, Dhcp4Renewing, TRUE))) {
+        goto END_SESSION;
+      }
+
+      if (Instance != NULL) {
+        Instance->ElaspedTime= 0;
+      }    
+
+      Status = DhcpSendMessage (
+                 DhcpSb,
+                 DhcpSb->Selected,
+                 DhcpSb->Para,
+                 DHCP_MSG_REQUEST,
+                 NULL
+                 );
+
+      if (EFI_ERROR (Status)) {
+        goto END_SESSION;
+      }
+    }
+  }
+
+ON_EXIT:
+  //
+  // Iterate through all the DhcpSb Children.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &DhcpSb->Children) {
+    Instance = NET_LIST_USER_STRUCT (Entry, DHCP_PROTOCOL, Link);
+    
+    if ((Instance != NULL) && (Instance->Token != NULL)) {
+      Instance->Timeout--;
+      if (Instance->Timeout == 0) {
+        PxeDhcpDone (Instance);
+      }
+    }
+  }
+
+  return ;
+
+END_SESSION:
+  DhcpEndSession (DhcpSb, EFI_TIMEOUT);
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.h b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.h
new file mode 100644
index 0000000000..1e85651554
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Io.h
@@ -0,0 +1,195 @@
+/** @file
+  The DHCP4 protocol implementation.
+  
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef __EFI_DHCP4_IO_H__
+#define __EFI_DHCP4_IO_H__
+
+#include <Uefi.h>
+
+#include <Protocol/ServiceBinding.h>
+
+#include <Library/NetLib.h>
+#include <Library/UdpIoLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+
+
+#define DHCP_WAIT_OFFER                    3  // Time to wait the offers
+#define DHCP_DEFAULT_LEASE  7 * 24 * 60 * 60  // Seven days as default.
+#define DHCP_SERVER_PORT                  67
+#define DHCP_CLIENT_PORT                  68
+
+//
+// BOOTP header "op" field
+//
+#define BOOTP_REQUEST           1
+#define BOOTP_REPLY             2
+
+//
+// DHCP message types
+//
+#define DHCP_MSG_DISCOVER       1
+#define DHCP_MSG_OFFER          2
+#define DHCP_MSG_REQUEST        3
+#define DHCP_MSG_DECLINE        4
+#define DHCP_MSG_ACK            5
+#define DHCP_MSG_NAK            6
+#define DHCP_MSG_RELEASE        7
+#define DHCP_MSG_INFORM         8
+
+//
+// DHCP notify user type
+//
+#define DHCP_NOTIFY_COMPLETION  1
+#define DHCP_NOTIFY_RENEWREBIND 2
+#define DHCP_NOTIFY_ALL         3
+
+#define DHCP_IS_BOOTP(Parameter)  (((Parameter) == NULL) || ((Parameter)->DhcpType == 0))
+
+#define DHCP_CONNECTED(State)     \
+  (((State) == Dhcp4Bound) || ((State) == (Dhcp4Renewing)) || ((State) == Dhcp4Rebinding))
+
+/**
+  Set the DHCP state. If CallUser is true, it will try to notify
+  the user before change the state by DhcpNotifyUser. It returns
+  EFI_ABORTED if the user return EFI_ABORTED, otherwise, it returns
+  EFI_SUCCESS. If CallUser is FALSE, it isn't necessary to test
+  the return value of this function.
+
+  @param  DhcpSb                The DHCP service instance
+  @param  State                 The new DHCP state to change to
+  @param  CallUser              Whether we need to call user
+
+  @retval EFI_SUCCESS           The state is changed
+  @retval EFI_ABORTED           The user asks to abort the DHCP process.
+
+**/
+EFI_STATUS
+DhcpSetState (
+  IN OUT DHCP_SERVICE           *DhcpSb,
+  IN     INTN                   State,
+  IN     BOOLEAN                CallUser
+  );
+
+/**
+  Build and transmit a DHCP message according to the current states.
+  This function implement the Table 5. of RFC 2131. Always transits
+  the state (as defined in Figure 5. of the same RFC) before sending
+  a DHCP message. The table is adjusted accordingly.
+
+  @param[in]  DhcpSb                The DHCP service instance
+  @param[in]  Seed                  The seed packet which the new packet is based on
+  @param[in]  Para                  The DHCP parameter of the Seed packet
+  @param[in]  Type                  The message type to send
+  @param[in]  Msg                   The human readable message to include in the packet
+                                    sent.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources for the packet
+  @retval EFI_ACCESS_DENIED     Failed to transmit the packet through UDP
+  @retval EFI_SUCCESS           The message is sent
+  @retval other                 Other error occurs
+
+**/
+EFI_STATUS
+DhcpSendMessage (
+  IN DHCP_SERVICE           *DhcpSb,
+  IN EFI_DHCP4_PACKET       *Seed,
+  IN DHCP_PARAMETER         *Para,
+  IN UINT8                  Type,
+  IN UINT8                  *Msg
+  );
+
+/**
+  Each DHCP service has three timer. Two of them are count down timer.
+  One for the packet retransmission. The other is to collect the offers.
+  The third timer increaments the lease life which is compared to T1, T2,
+  and lease to determine the time to renew and rebind the lease.
+  DhcpOnTimerTick will be called once every second.
+
+  @param[in]  Event                 The timer event
+  @param[in]  Context               The context, which is the DHCP service instance.
+
+**/
+VOID
+EFIAPI
+DhcpOnTimerTick (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  );
+
+/**
+  Handle the received DHCP packets. This function drives the DHCP
+  state machine.
+
+  @param  UdpPacket             The UDP packets received.
+  @param  EndPoint              The local/remote UDP access point
+  @param  IoStatus              The status of the UDP receive
+  @param  Context               The opaque parameter to the function.
+
+**/
+VOID
+EFIAPI
+DhcpInput (
+  NET_BUF                   *UdpPacket,
+  UDP_END_POINT             *EndPoint,
+  EFI_STATUS                IoStatus,
+  VOID                      *Context
+  );
+
+/**
+  Send an initial DISCOVER or REQUEST message according to the
+  DHCP service's current state.
+
+  @param[in]  DhcpSb                The DHCP service instance
+
+  @retval EFI_SUCCESS           The request has been sent
+  @retval other                 Some error occurs when sending the request.
+
+**/
+EFI_STATUS
+DhcpInitRequest (
+  IN DHCP_SERVICE           *DhcpSb
+  );
+
+/**
+  Clean up the DHCP related states, IoStatus isn't reset.
+
+  @param  DhcpSb                The DHCP instance service.
+
+**/
+VOID
+DhcpCleanLease (
+  IN DHCP_SERVICE           *DhcpSb
+  );
+
+/**
+  Release the net buffer when packet is sent.
+
+  @param  UdpPacket             The UDP packets received.
+  @param  EndPoint              The local/remote UDP access point
+  @param  IoStatus              The status of the UDP receive
+  @param  Context               The opaque parameter to the function.
+
+**/
+VOID
+EFIAPI
+DhcpOnPacketSent (
+  NET_BUF                   *Packet,
+  UDP_END_POINT             *EndPoint,
+  EFI_STATUS                IoStatus,
+  VOID                      *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.c b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.c
new file mode 100644
index 0000000000..d7700bccc4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.c
@@ -0,0 +1,896 @@
+/** @file
+  Function to validate, parse, process the DHCP options.
+  
+Copyright (c) 2006 - 2009, 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 "Dhcp4Impl.h"
+
+///
+/// A list of the format of DHCP Options sorted by option tag
+/// to validate a dhcp message. Refere the comments of the
+/// DHCP_OPTION_FORMAT structure.
+///
+DHCP_OPTION_FORMAT DhcpOptionFormats[] = {
+  {DHCP_TAG_NETMASK,        DHCP_OPTION_IP,     1, 1  , TRUE},
+  {DHCP_TAG_TIME_OFFSET,    DHCP_OPTION_INT32,  1, 1  , FALSE},
+  {DHCP_TAG_ROUTER,         DHCP_OPTION_IP,     1, -1 , TRUE},
+  {DHCP_TAG_TIME_SERVER,    DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_NAME_SERVER,    DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_DNS_SERVER,     DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_LOG_SERVER,     DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_COOKIE_SERVER,  DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_LPR_SERVER,     DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_IMPRESS_SERVER, DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_RL_SERVER,      DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_HOSTNAME,       DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_BOOTFILE_LEN,   DHCP_OPTION_INT16,  1, 1  , FALSE},
+  {DHCP_TAG_DUMP,           DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_DOMAINNAME,     DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_SWAP_SERVER,    DHCP_OPTION_IP,     1, 1  , FALSE},
+  {DHCP_TAG_ROOTPATH,       DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_EXTEND_PATH,    DHCP_OPTION_INT8,   1, -1 , FALSE},
+
+  {DHCP_TAG_IPFORWARD,      DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_NONLOCAL_SRR,   DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_POLICY_SRR,     DHCP_OPTION_IPPAIR, 1, -1 , FALSE},
+  {DHCP_TAG_EMTU,           DHCP_OPTION_INT16,  1, 1  , FALSE},
+  {DHCP_TAG_TTL,            DHCP_OPTION_INT8,   1, 1  , FALSE},
+  {DHCP_TAG_PATHMTU_AGE,    DHCP_OPTION_INT32,  1, 1  , FALSE},
+  {DHCP_TAG_PATHMTU_PLATEAU,DHCP_OPTION_INT16,  1, -1 , FALSE},
+
+  {DHCP_TAG_IFMTU,          DHCP_OPTION_INT16,  1, 1  , FALSE},
+  {DHCP_TAG_SUBNET_LOCAL,   DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_BROADCAST,      DHCP_OPTION_IP,     1, 1  , FALSE},
+  {DHCP_TAG_DISCOVER_MASK,  DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_SUPPLY_MASK,    DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_DISCOVER_ROUTE, DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_ROUTER_SOLICIT, DHCP_OPTION_IP,     1, 1  , FALSE},
+  {DHCP_TAG_STATIC_ROUTE,   DHCP_OPTION_IPPAIR, 1, -1 , FALSE},
+
+  {DHCP_TAG_TRAILER,        DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+  {DHCP_TAG_ARPAGE,         DHCP_OPTION_INT32,  1, 1  , FALSE},
+  {DHCP_TAG_ETHER_ENCAP,    DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+
+  {DHCP_TAG_TCP_TTL,        DHCP_OPTION_INT8,   1, 1  , FALSE},
+  {DHCP_TAG_KEEP_INTERVAL,  DHCP_OPTION_INT32,  1, 1  , FALSE},
+  {DHCP_TAG_KEEP_GARBAGE,   DHCP_OPTION_SWITCH, 1, 1  , FALSE},
+
+  {DHCP_TAG_NIS_DOMAIN,     DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_NIS_SERVER,     DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_NTP_SERVER,     DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_VENDOR,         DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_NBNS,           DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_NBDD,           DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_NBTYPE,         DHCP_OPTION_INT8,   1, 1  , FALSE},
+  {DHCP_TAG_NBSCOPE,        DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_XFONT,          DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_XDM,            DHCP_OPTION_IP,     1, -1 , FALSE},
+
+  {DHCP_TAG_REQUEST_IP,     DHCP_OPTION_IP,     1, 1  , FALSE},
+  {DHCP_TAG_LEASE,          DHCP_OPTION_INT32,  1, 1  , TRUE},
+  {DHCP_TAG_OVERLOAD,       DHCP_OPTION_INT8,   1, 1  , TRUE},
+  {DHCP_TAG_TYPE,           DHCP_OPTION_INT8,   1, 1  , TRUE},
+  {DHCP_TAG_SERVER_ID,      DHCP_OPTION_IP,     1, 1  , TRUE},
+  {DHCP_TAG_PARA_LIST,      DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_MESSAGE,        DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_MAXMSG,         DHCP_OPTION_INT16,  1, 1  , FALSE},
+  {DHCP_TAG_T1,             DHCP_OPTION_INT32,  1, 1  , TRUE},
+  {DHCP_TAG_T2,             DHCP_OPTION_INT32,  1, 1  , TRUE},
+  {DHCP_TAG_VENDOR_CLASS,   DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_CLIENT_ID,      DHCP_OPTION_INT8,   2, -1 , FALSE},
+
+  {DHCP_TAG_NISPLUS,        DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_NISPLUS_SERVER, DHCP_OPTION_IP,     1, -1 , FALSE},
+
+  {DHCP_TAG_TFTP,           DHCP_OPTION_INT8,   1, -1 , FALSE},
+  {DHCP_TAG_BOOTFILE,       DHCP_OPTION_INT8,   1, -1 , FALSE},
+
+  {DHCP_TAG_MOBILEIP,       DHCP_OPTION_IP,     0, -1 , FALSE},
+  {DHCP_TAG_SMTP,           DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_POP3,           DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_NNTP,           DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_WWW,            DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_FINGER,         DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_IRC,            DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_STTALK,         DHCP_OPTION_IP,     1, -1 , FALSE},
+  {DHCP_TAG_STDA,           DHCP_OPTION_IP,     1, -1 , FALSE},
+
+  {DHCP_TAG_CLASSLESS_ROUTE,DHCP_OPTION_INT8,   5, -1 , FALSE},
+};
+
+
+/**
+  Binary search the DhcpOptionFormats array to find the format
+  information about a specific option.
+
+  @param[in]  Tag                    The option's tag.
+
+  @return The point to the option's format, NULL if not found.
+
+**/
+DHCP_OPTION_FORMAT *
+DhcpFindOptionFormat (
+  IN UINT8                  Tag
+  )
+{
+  INTN                      Left;
+  INTN                      Right;
+  INTN                      Middle;
+
+  Left  = 0;
+  Right = sizeof (DhcpOptionFormats) / sizeof (DHCP_OPTION_FORMAT) - 1;
+
+  while (Right >= Left) {
+    Middle = (Left + Right) / 2;
+
+    if (Tag == DhcpOptionFormats[Middle].Tag) {
+      return &DhcpOptionFormats[Middle];
+    }
+
+    if (Tag < DhcpOptionFormats[Middle].Tag) {
+      Right = Middle - 1;
+    } else {
+      Left  = Middle + 1;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Validate whether a single DHCP option is valid according to its format.
+
+  @param[in]  Format                 The option's format
+  @param[in]  OptValue               The value of the option
+  @param[in]  Len                    The length of the option value
+
+  @retval TRUE     The option is valid.
+  @retval FALSE    Otherwise.
+
+**/
+BOOLEAN
+DhcpOptionIsValid (
+  IN DHCP_OPTION_FORMAT     *Format,
+  IN UINT8                  *OptValue,
+  IN INTN                   Len
+  )
+{
+  INTN                      Unit;
+  INTN                      Occur;
+  INTN                      Index;
+
+  Unit = 0;
+
+  switch (Format->Type) {
+  case DHCP_OPTION_SWITCH:
+  case DHCP_OPTION_INT8:
+    Unit = 1;
+    break;
+
+  case DHCP_OPTION_INT16:
+    Unit = 2;
+    break;
+
+  case DHCP_OPTION_INT32:
+  case DHCP_OPTION_IP:
+    Unit = 4;
+    break;
+
+  case DHCP_OPTION_IPPAIR:
+    Unit = 8;
+    break;
+  }
+
+  ASSERT (Unit != 0);
+
+  //
+  // Validate that the option appears in the full units.
+  //
+  if ((Len % Unit) != 0) {
+    return FALSE;
+  }
+
+  //
+  // Validate the occurance of the option unit is with in [MinOccur, MaxOccur]
+  //
+  Occur = Len / Unit;
+
+  if (((Format->MinOccur != -1) && (Occur < Format->MinOccur)) ||
+      ((Format->MaxOccur != -1) && (Occur > Format->MaxOccur))
+      ) {
+    return FALSE;
+  }
+
+  //
+  // If the option is of type switch, only 0/1 are valid values.
+  //
+  if (Format->Type == DHCP_OPTION_SWITCH) {
+    for (Index = 0; Index < Occur; Index++) {
+      if ((OptValue[Index] != 0) && (OptValue[Index] != 1)) {
+        return FALSE;
+      }
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Extract the client interested options, all the parameters are
+  converted to host byte order.
+
+  @param[in]  Tag                    The DHCP option tag
+  @param[in]  Len                    The length of the option
+  @param[in]  Data                   The value of the DHCP option
+  @param[out] Para                   The variable to save the interested parameter
+
+  @retval EFI_SUCCESS            The DHCP option is successfully extracted.
+  @retval EFI_INVALID_PARAMETER  The DHCP option is mal-formated
+
+**/
+EFI_STATUS
+DhcpGetParameter (
+  IN  UINT8                  Tag,
+  IN  INTN                   Len,
+  IN  UINT8                  *Data,
+  OUT DHCP_PARAMETER         *Para
+  )
+{
+  switch (Tag) {
+  case DHCP_TAG_NETMASK:
+    Para->NetMask = NetGetUint32 (Data);
+    break;
+
+  case DHCP_TAG_ROUTER:
+    //
+    // Return the first router to consumer which is the preferred one
+    //
+    Para->Router = NetGetUint32 (Data);
+    break;
+
+  case DHCP_TAG_LEASE:
+    Para->Lease = NetGetUint32 (Data);
+    break;
+
+  case DHCP_TAG_OVERLOAD:
+    Para->Overload = *Data;
+
+    if ((Para->Overload < 1) || (Para->Overload > 3)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    break;
+
+  case DHCP_TAG_TYPE:
+    Para->DhcpType = *Data;
+
+    if ((Para->DhcpType < 1) || (Para->DhcpType > 9)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    break;
+
+  case DHCP_TAG_SERVER_ID:
+    Para->ServerId = NetGetUint32 (Data);
+    break;
+
+  case DHCP_TAG_T1:
+    Para->T1 = NetGetUint32 (Data);
+    break;
+
+  case DHCP_TAG_T2:
+    Para->T2 = NetGetUint32 (Data);
+    break;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Inspect all the options in a single buffer. DHCP options may be contained
+  in several buffers, such as the BOOTP options filed, boot file or server
+  name. Each option buffer is required to end with DHCP_TAG_EOP.
+
+  @param[in]  Buffer                 The buffer which contains DHCP options
+  @param[in]  BufLen                 The length of the buffer
+  @param[in]  Check                  The callback function for each option found
+  @param[in]  Context                The opaque parameter for the Check
+  @param[out] Overload               Variable to save the value of DHCP_TAG_OVERLOAD
+                                     option.
+
+  @retval EFI_SUCCESS            All the options are valid
+  @retval EFI_INVALID_PARAMETER  The options are mal-formated.
+
+**/
+EFI_STATUS
+DhcpIterateBufferOptions (
+  IN  UINT8                 *Buffer,
+  IN  INTN                  BufLen,
+  IN  DHCP_CHECK_OPTION     Check             OPTIONAL,
+  IN  VOID                  *Context,
+  OUT UINT8                 *Overload         OPTIONAL
+  )
+{
+  INTN                      Cur;
+  UINT8                     Tag;
+  UINT8                     Len;
+
+  Cur = 0;
+
+  while (Cur < BufLen) {
+    Tag = Buffer[Cur];
+
+    if (Tag == DHCP_TAG_PAD) {
+      Cur++;
+      continue;
+    } else if (Tag == DHCP_TAG_EOP) {
+      return EFI_SUCCESS;
+    }
+
+    Cur++;
+
+    if (Cur == BufLen) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Len = Buffer[Cur++];
+
+    if (Cur + Len > BufLen) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((Tag == DHCP_TAG_OVERLOAD) && (Overload != NULL)) {
+      if (Len != 1) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      *Overload = Buffer[Cur];
+    }
+
+    if ((Check != NULL) && EFI_ERROR (Check (Tag, Len, Buffer + Cur, Context))) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Cur += Len;
+  }
+
+  //
+  // Each option buffer is expected to end with an EOP
+  //
+  return EFI_INVALID_PARAMETER;
+}
+
+
+/**
+  Iterate through a DHCP message to visit each option. First inspect
+  all the options in the OPTION field. Then if overloaded, inspect
+  the options in FILENAME and SERVERNAME fields. One option may be
+  encoded in several places. See RFC 3396 Encoding Long Options in DHCP
+
+  @param[in]  Packet                 The DHCP packet to check the options for
+  @param[in]  Check                  The callback function to be called for each option
+                                     found
+  @param[in]  Context                The opaque parameter for Check
+
+  @retval EFI_SUCCESS            The DHCP packet's options are well formated
+  @retval EFI_INVALID_PARAMETER  The DHCP packet's options are not well formated
+
+**/
+EFI_STATUS
+DhcpIterateOptions (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  IN  DHCP_CHECK_OPTION     Check         OPTIONAL,
+  IN  VOID                  *Context
+  )
+{
+  EFI_STATUS                Status;
+  UINT8                     Overload;
+
+  Overload = 0;
+
+  Status   = DhcpIterateBufferOptions (
+               Packet->Dhcp4.Option,
+               Packet->Length - sizeof (EFI_DHCP4_HEADER) - sizeof (UINT32),
+               Check,
+               Context,
+               &Overload
+               );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if ((Overload == DHCP_OVERLOAD_FILENAME) || (Overload == DHCP_OVERLOAD_BOTH)) {
+    Status = DhcpIterateBufferOptions (
+               (UINT8 *) Packet->Dhcp4.Header.BootFileName,
+               128,
+               Check,
+               Context,
+               NULL
+               );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if ((Overload == DHCP_OVERLOAD_SVRNAME) || (Overload == DHCP_OVERLOAD_BOTH)) {
+    Status = DhcpIterateBufferOptions (
+               (UINT8 *) Packet->Dhcp4.Header.ServerName,
+               64,
+               Check,
+               Context,
+               NULL
+               );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Call back function to DhcpIterateOptions to compute each option's
+  length. It just adds the data length of all the occurances of this
+  Tag. Context is an array of 256 DHCP_OPTION_COUNT.
+
+  @param[in]  Tag                    The current option to check
+  @param[in]  Len                    The length of the option data
+  @param[in]  Data                   The option data
+  @param[in]  Context                The context, which is a array of 256
+                                     DHCP_OPTION_COUNT.
+
+  @retval EFI_SUCCESS            It always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+DhcpGetOptionLen (
+  IN UINT8                  Tag,
+  IN UINT8                  Len,
+  IN UINT8                  *Data,
+  IN VOID                   *Context
+  )
+{
+  DHCP_OPTION_COUNT         *OpCount;
+
+  OpCount             = (DHCP_OPTION_COUNT *) Context;
+  OpCount[Tag].Offset = (UINT16) (OpCount[Tag].Offset + Len);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Call back function to DhcpIterateOptions to consolidate each option's
+  data. There are maybe several occurrence of the same option.
+
+  @param[in]  Tag                    The option to consolidate its data
+  @param[in]  Len                    The length of option data
+  @param[in]  Data                   The data of the option's current occurance
+  @param[in]  Context                The context, which is DHCP_OPTION_CONTEXT. This
+                                     array is  just a wrap to pass THREE parameters.
+
+  @retval EFI_SUCCESS            It always returns EFI_SUCCESS
+
+**/
+EFI_STATUS
+DhcpFillOption (
+  IN UINT8                  Tag,
+  IN UINT8                  Len,
+  IN UINT8                  *Data,
+  IN VOID                   *Context
+  )
+{
+  DHCP_OPTION_CONTEXT       *OptContext;
+  DHCP_OPTION_COUNT         *OptCount;
+  DHCP_OPTION               *Options;
+  UINT8                     *Buf;
+  UINT8                     Index;
+
+  OptContext  = (DHCP_OPTION_CONTEXT *) Context;
+
+  OptCount    = OptContext->OpCount;
+  Index       = OptCount[Tag].Index;
+  Options     = OptContext->Options;
+  Buf         = OptContext->Buf;
+
+  if (Options[Index].Data == NULL) {
+    Options[Index].Tag  = Tag;
+    Options[Index].Data = Buf + OptCount[Tag].Offset;
+  }
+
+  CopyMem (Buf + OptCount[Tag].Offset, Data, Len);
+
+  OptCount[Tag].Offset  = (UINT16) (OptCount[Tag].Offset + Len);
+  Options[Index].Len    = (UINT16) (Options[Index].Len + Len);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parse the options of a DHCP packet. It supports RFC 3396: Encoding
+  Long Options in DHCP. That is, it will combine all the option value
+  of all the occurances of each option.
+  A little bit of implemenation:
+  It adopts the "Key indexed counting" algorithm. First, it allocates
+  an array of 256 DHCP_OPTION_COUNTs because DHCP option tag is encoded
+  as a UINT8. It then iterates the DHCP packet to get data length of
+  each option by calling DhcpIterOptions with DhcpGetOptionLen. Now, it
+  knows the number of present options and their length. It allocates a
+  array of DHCP_OPTION and a continuous buffer after the array to put
+  all the options' data. Each option's data is pointed to by the Data
+  field in DHCP_OPTION structure. At last, it call DhcpIterateOptions
+  with DhcpFillOption to fill each option's data to its position in the
+  buffer.
+
+  @param[in]  Packet                 The DHCP packet to parse the options
+  @param[out] Count                  The number of valid dhcp options present in the
+                                     packet
+  @param[out] OptionPoint            The array that contains the DHCP options. Caller
+                                     should free it.
+
+  @retval EFI_NOT_FOUND          Cannot find any option.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to parse the packet.
+  @retval EFI_INVALID_PARAMETER  The options are mal-formated
+  @retval EFI_SUCCESS            The options are parsed into OptionPoint
+
+**/
+EFI_STATUS
+DhcpParseOption (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  OUT INTN                  *Count,
+  OUT DHCP_OPTION           **OptionPoint
+  )
+{
+  DHCP_OPTION_CONTEXT       Context;
+  DHCP_OPTION               *Options;
+  DHCP_OPTION_COUNT         *OptCount;
+  EFI_STATUS                Status;
+  UINT16                    TotalLen;
+  INTN                      OptNum;
+  INTN                      Index;
+
+  ASSERT ((Count != NULL) && (OptionPoint != NULL));
+
+  //
+  // First compute how many options and how long each option is
+  // with the "Key indexed counting" algorithms.
+  //
+  OptCount = AllocateZeroPool (DHCP_MAX_OPTIONS * sizeof (DHCP_OPTION_COUNT));
+
+  if (OptCount == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = DhcpIterateOptions (Packet, DhcpGetOptionLen, OptCount);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Before the loop, Offset is the length of the option. After loop,
+  // OptCount[Index].Offset specifies the offset into the continuous
+  // option value buffer to put the data.
+  //
+  TotalLen  = 0;
+  OptNum    = 0;
+
+  for (Index = 0; Index < DHCP_MAX_OPTIONS; Index++) {
+    if (OptCount[Index].Offset != 0) {
+      OptCount[Index].Index   = (UINT8) OptNum;
+
+      TotalLen                = (UINT16) (TotalLen + OptCount[Index].Offset);
+      OptCount[Index].Offset  = (UINT16) (TotalLen - OptCount[Index].Offset);
+
+      OptNum++;
+    }
+  }
+
+  *Count        = OptNum;
+  *OptionPoint  = NULL;
+
+  if (OptNum == 0) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Allocate a buffer to hold the DHCP options, and after that, a
+  // continuous buffer to put all the options' data.
+  //
+  Options = AllocateZeroPool ((UINTN) (OptNum * sizeof (DHCP_OPTION)) + TotalLen);
+
+  if (Options == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  Context.OpCount = OptCount;
+  Context.Options = Options;
+  Context.Buf     = (UINT8 *) (Options + OptNum);
+
+  Status          = DhcpIterateOptions (Packet, DhcpFillOption, &Context);
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Options);
+    goto ON_EXIT;
+  }
+
+  *OptionPoint = Options;
+
+ON_EXIT:
+  FreePool (OptCount);
+  return Status;
+}
+
+
+/**
+  Validate the packet's options. If necessary, allocate
+  and fill in the interested parameters.
+
+  @param[in]  Packet                 The packet to validate the options
+  @param[out] Para                   The variable to save the DHCP parameters.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to validate the packet.
+  @retval EFI_INVALID_PARAMETER  The options are mal-formated
+  @retval EFI_SUCCESS            The options are parsed into OptionPoint
+
+**/
+EFI_STATUS
+DhcpValidateOptions (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  OUT DHCP_PARAMETER        **Para       OPTIONAL
+  )
+{
+  DHCP_PARAMETER            Parameter;
+  DHCP_OPTION_FORMAT        *Format;
+  DHCP_OPTION               *AllOption;
+  DHCP_OPTION               *Option;
+  EFI_STATUS                Status;
+  BOOLEAN                   Updated;
+  INTN                      Count;
+  INTN                      Index;
+
+  if (Para != NULL) {
+    *Para = NULL;
+  }
+
+  AllOption = NULL;
+
+  Status = DhcpParseOption (Packet, &Count, &AllOption);
+  if (EFI_ERROR (Status) || (Count == 0)) {
+    return Status;
+  }
+  ASSERT (AllOption != NULL);
+
+  Updated = FALSE;
+  ZeroMem (&Parameter, sizeof (Parameter));
+
+  for (Index = 0; Index < Count; Index++) {
+    Option = &AllOption[Index];
+
+    //
+    // Find the format of the option then validate it.
+    //
+    Format = DhcpFindOptionFormat (Option->Tag);
+
+    if (Format == NULL) {
+      continue;
+    }
+
+    if (!DhcpOptionIsValid (Format, Option->Data, Option->Len)) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    //
+    // Get the client interested parameters
+    //
+    if (Format->Alert && (Para != NULL)) {
+      Updated = TRUE;
+      Status  = DhcpGetParameter (Option->Tag, Option->Len, Option->Data, &Parameter);
+
+      if (EFI_ERROR (Status)) {
+        goto ON_EXIT;
+      }
+    }
+  }
+
+  if (Updated && (Para != NULL)) {
+    *Para = AllocateCopyPool (sizeof (DHCP_PARAMETER), &Parameter);
+    if (*Para == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+  }
+
+ON_EXIT:
+  FreePool (AllOption);
+  return Status;
+}
+
+
+
+/**
+  Append an option to the memory, if the option is longer than
+  255 bytes, splits it into several options.
+
+  @param[out] Buf                    The buffer to append the option to
+  @param[in]  Tag                    The option's tag
+  @param[in]  DataLen                The length of the option's data
+  @param[in]  Data                   The option's data
+
+  @return The position to append the next option
+
+**/
+UINT8 *
+DhcpAppendOption (
+  OUT UINT8                  *Buf,
+  IN  UINT8                  Tag,
+  IN  UINT16                 DataLen,
+  IN  UINT8                  *Data
+  )
+{
+  INTN                      Index;
+  INTN                      Len;
+
+  ASSERT (DataLen != 0);
+
+  for (Index = 0; Index < (DataLen + 254) / 255; Index++) {
+    Len      = MIN (255, DataLen - Index * 255);
+
+    *(Buf++) = Tag;
+    *(Buf++) = (UINT8) Len;
+    CopyMem (Buf, Data + Index * 255, (UINTN) Len);
+
+    Buf     += Len;
+  }
+
+  return Buf;
+}
+
+
+/**
+  Build a new DHCP packet from a seed packet. Options may be deleted or
+  appended. The caller should free the NewPacket when finished using it.
+
+  @param[in]  SeedPacket             The seed packet to start with
+  @param[in]  DeleteCount            The number of options to delete
+  @param[in]  DeleteList             The options to delete from the packet
+  @param[in]  AppendCount            The number of options to append
+  @param[in]  AppendList             The options to append to the packet
+  @param[out] NewPacket              The new packet, allocated and built by this
+                                     function.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory
+  @retval EFI_INVALID_PARAMETER  The options in SeekPacket are mal-formated
+  @retval EFI_SUCCESS            The packet is build.
+
+**/
+EFI_STATUS
+DhcpBuild (
+  IN  EFI_DHCP4_PACKET        *SeedPacket,
+  IN  UINT32                  DeleteCount,
+  IN  UINT8                   *DeleteList     OPTIONAL,
+  IN  UINT32                  AppendCount,
+  IN  EFI_DHCP4_PACKET_OPTION *AppendList[]   OPTIONAL,
+  OUT EFI_DHCP4_PACKET        **NewPacket
+  )
+{
+  DHCP_OPTION               *Mark;
+  DHCP_OPTION               *SeedOptions;
+  EFI_DHCP4_PACKET          *Packet;
+  EFI_STATUS                Status;
+  INTN                      Count;
+  UINT32                    Index;
+  UINT32                    Len;
+  UINT8                     *Buf;
+
+  //
+  // Use an array of DHCP_OPTION to mark the existance
+  // and position of each valid options.
+  //
+  Mark = AllocatePool (sizeof (DHCP_OPTION) * DHCP_MAX_OPTIONS);
+
+  if (Mark == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Index = 0; Index < DHCP_MAX_OPTIONS; Index++) {
+    Mark[Index].Tag = (UINT8) Index;
+    Mark[Index].Len = 0;
+  }
+
+  //
+  // Get list of the options from the seed packet, then put
+  // them to the mark array according to their tags.
+  //
+  SeedOptions = NULL;
+  Status      = DhcpParseOption (SeedPacket, &Count, &SeedOptions);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  if (SeedOptions != NULL) {
+    for (Index = 0; Index < (UINT32) Count; Index++) {
+      Mark[SeedOptions[Index].Tag] = SeedOptions[Index];
+    }
+  }
+
+  //
+  // Mark the option's length is zero if it is in the DeleteList.
+  //
+  for (Index = 0; Index < DeleteCount; Index++) {
+    Mark[DeleteList[Index]].Len = 0;
+  }
+
+  //
+  // Add or replace the option if it is in the append list.
+  //
+  for (Index = 0; Index < AppendCount; Index++) {
+    Mark[AppendList[Index]->OpCode].Len  = AppendList[Index]->Length;
+    Mark[AppendList[Index]->OpCode].Data = AppendList[Index]->Data;
+  }
+
+  //
+  // compute the new packet length. No need to add 1 byte for
+  // EOP option since EFI_DHCP4_PACKET includes one extra byte
+  // for option. It is necessary to split the option if it is
+  // longer than 255 bytes.
+  //
+  Len = sizeof (EFI_DHCP4_PACKET);
+
+  for (Index = 0; Index < DHCP_MAX_OPTIONS; Index++) {
+    if (Mark[Index].Len != 0) {
+      Len += ((Mark[Index].Len + 254) / 255) * 2 + Mark[Index].Len;
+    }
+  }
+
+  Status  = EFI_OUT_OF_RESOURCES;
+  Packet  = (EFI_DHCP4_PACKET *) AllocatePool (Len);
+
+  if (Packet == NULL) {
+    goto ON_ERROR;
+  }
+
+  Packet->Size         = Len;
+  Packet->Length       = 0;
+  CopyMem (&Packet->Dhcp4.Header, &SeedPacket->Dhcp4.Header, sizeof (Packet->Dhcp4.Header));
+  Packet->Dhcp4.Magik  = DHCP_OPTION_MAGIC;
+  Buf                  = Packet->Dhcp4.Option;
+
+  for (Index = 0; Index < DHCP_MAX_OPTIONS; Index++) {
+    if (Mark[Index].Len != 0) {
+      Buf = DhcpAppendOption (Buf, Mark[Index].Tag, Mark[Index].Len, Mark[Index].Data);
+    }
+  }
+
+  *(Buf++)        = DHCP_TAG_EOP;
+  Packet->Length  = sizeof (EFI_DHCP4_HEADER) + sizeof (UINT32)
+                      + (UINT32) (Buf - Packet->Dhcp4.Option);
+
+  *NewPacket      = Packet;
+  Status          = EFI_SUCCESS;
+
+ON_ERROR:
+  if (SeedOptions != NULL) {
+    FreePool (SeedOptions);
+  }
+
+  FreePool (Mark);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.h b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.h
new file mode 100644
index 0000000000..f532780970
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Dhcp4Dxe/Dhcp4Option.h
@@ -0,0 +1,340 @@
+/** @file
+  To validate, parse and process the DHCP options.
+  
+Copyright (c) 2006 - 2009, 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.
+
+**/
+
+#ifndef __EFI_DHCP4_OPTION_H__
+#define __EFI_DHCP4_OPTION_H__
+
+///
+/// DHCP option tags (types)
+///
+
+//
+// RFC1497 vendor extensions
+//
+#define DHCP_TAG_PAD              0    // Pad Option
+#define DHCP_TAG_EOP              255  // End Option
+#define DHCP_TAG_NETMASK          1    // Subnet Mask
+#define DHCP_TAG_TIME_OFFSET      2    // Time Offset from UTC
+#define DHCP_TAG_ROUTER           3    // Router option,
+#define DHCP_TAG_TIME_SERVER      4    // Time Server
+#define DHCP_TAG_NAME_SERVER      5    // Name Server
+#define DHCP_TAG_DNS_SERVER       6    // Domain Name Server
+#define DHCP_TAG_LOG_SERVER       7    // Log Server
+#define DHCP_TAG_COOKIE_SERVER    8    // Cookie Server
+#define DHCP_TAG_LPR_SERVER       9    // LPR Print Server
+#define DHCP_TAG_IMPRESS_SERVER   10   // Impress Server
+#define DHCP_TAG_RL_SERVER        11   // Resource Location Server
+#define DHCP_TAG_HOSTNAME         12   // Host Name
+#define DHCP_TAG_BOOTFILE_LEN     13   // Boot File Size
+#define DHCP_TAG_DUMP             14   // Merit Dump File
+#define DHCP_TAG_DOMAINNAME       15   // Domain Name
+#define DHCP_TAG_SWAP_SERVER      16   // Swap Server
+#define DHCP_TAG_ROOTPATH         17   // Root path
+#define DHCP_TAG_EXTEND_PATH      18   // Extensions Path
+
+//
+// IP Layer Parameters per Host
+//
+#define DHCP_TAG_IPFORWARD        19 // IP Forwarding Enable/Disable
+#define DHCP_TAG_NONLOCAL_SRR     20 // on-Local Source Routing Enable/Disable
+#define DHCP_TAG_POLICY_SRR       21 // Policy Filter
+#define DHCP_TAG_EMTU             22 // Maximum Datagram Reassembly Size
+#define DHCP_TAG_TTL              23 // Default IP Time-to-live
+#define DHCP_TAG_PATHMTU_AGE      24 // Path MTU Aging Timeout
+#define DHCP_TAG_PATHMTU_PLATEAU  25 // Path MTU Plateau Table
+
+//
+// IP Layer Parameters per Interface
+//
+#define DHCP_TAG_IFMTU            26 // Interface MTU
+#define DHCP_TAG_SUBNET_LOCAL     27 // All Subnets are Local
+#define DHCP_TAG_BROADCAST        28 // Broadcast Address
+#define DHCP_TAG_DISCOVER_MASK    29 // Perform Mask Discovery
+#define DHCP_TAG_SUPPLY_MASK      30 // Mask Supplier
+#define DHCP_TAG_DISCOVER_ROUTE   31 // Perform Router Discovery
+#define DHCP_TAG_ROUTER_SOLICIT   32 // Router Solicitation Address
+#define DHCP_TAG_STATIC_ROUTE     33 // Static Route
+
+//
+// Link Layer Parameters per Interface
+//
+#define DHCP_TAG_TRAILER          34 // Trailer Encapsulation
+#define DHCP_TAG_ARPAGE           35 // ARP Cache Timeout
+#define DHCP_TAG_ETHER_ENCAP      36 // Ethernet Encapsulation
+
+//
+// TCP Parameters
+//
+#define DHCP_TAG_TCP_TTL          37 // TCP Default TTL
+#define DHCP_TAG_KEEP_INTERVAL    38 // TCP Keepalive Interval
+#define DHCP_TAG_KEEP_GARBAGE     39 // TCP Keepalive Garbage
+
+//
+// Application and Service Parameters
+//
+#define DHCP_TAG_NIS_DOMAIN       40   // Network Information Service Domain
+#define DHCP_TAG_NIS_SERVER       41   // Network Information Servers
+#define DHCP_TAG_NTP_SERVER       42   // Network Time Protocol Servers
+#define DHCP_TAG_VENDOR           43   // Vendor Specific Information
+#define DHCP_TAG_NBNS             44   // NetBIOS over TCP/IP Name Server
+#define DHCP_TAG_NBDD             45   // NetBIOS Datagram Distribution Server
+#define DHCP_TAG_NBTYPE           46   // NetBIOS over TCP/IP Node Type
+#define DHCP_TAG_NBSCOPE          47   // NetBIOS over TCP/IP Scope
+#define DHCP_TAG_XFONT            48   // X Window System Font Server
+#define DHCP_TAG_XDM              49   // X Window System Display Manager
+#define DHCP_TAG_NISPLUS          64   // Network Information Service+ Domain
+#define DHCP_TAG_NISPLUS_SERVER   65   // Network Information Service+ Servers
+#define DHCP_TAG_MOBILEIP         68   // Mobile IP Home Agent
+#define DHCP_TAG_SMTP             69   // Simple Mail Transport Protocol Server
+#define DHCP_TAG_POP3             70   // Post Office Protocol (POP3) Server
+#define DHCP_TAG_NNTP             71   // Network News Transport Protocol Server
+#define DHCP_TAG_WWW              72   // Default World Wide Web (WWW) Server
+#define DHCP_TAG_FINGER           73   // Default Finger Server
+#define DHCP_TAG_IRC              74   // Default Internet Relay Chat (IRC) Server
+#define DHCP_TAG_STTALK           75   // StreetTalk Server
+#define DHCP_TAG_STDA             76   // StreetTalk Directory Assistance Server
+#define DHCP_TAG_CLASSLESS_ROUTE  121  // Classless Route
+
+//
+// DHCP Extensions
+//
+#define DHCP_TAG_REQUEST_IP       50         // Requested IP Address
+#define DHCP_TAG_LEASE            51         // IP Address Lease Time
+#define DHCP_TAG_OVERLOAD         52         // Option Overload
+#define DHCP_TAG_TFTP             66         // TFTP server name
+#define DHCP_TAG_BOOTFILE         67         // Bootfile name
+#define DHCP_TAG_TYPE             53         // DHCP Message Type
+#define DHCP_TAG_SERVER_ID        54         // Server Identifier
+#define DHCP_TAG_PARA_LIST        55         // Parameter Request List
+#define DHCP_TAG_MESSAGE          56         // Message
+#define DHCP_TAG_MAXMSG           57         // Maximum DHCP Message Size
+#define DHCP_TAG_T1               58         // Renewal (T1) Time Value
+#define DHCP_TAG_T2               59         // Rebinding (T2) Time Value
+#define DHCP_TAG_VENDOR_CLASS     60         // Vendor class identifier
+#define DHCP_TAG_CLIENT_ID        61         // Client-identifier
+
+
+#define DHCP_OPTION_MAGIC         0x63538263 // Network byte order
+#define DHCP_MAX_OPTIONS          256
+
+ 
+//
+// DHCP option types, this is used to validate the DHCP options.
+//
+#define DHCP_OPTION_SWITCH        1
+#define DHCP_OPTION_INT8          2
+#define DHCP_OPTION_INT16         3
+#define DHCP_OPTION_INT32         4
+#define DHCP_OPTION_IP            5
+#define DHCP_OPTION_IPPAIR        6
+
+//
+// Value of DHCP overload option
+//
+#define DHCP_OVERLOAD_FILENAME    1
+#define DHCP_OVERLOAD_SVRNAME     2
+#define DHCP_OVERLOAD_BOTH        3
+
+///
+/// The DHCP option structure. This structure extends the EFI_DHCP_OPTION
+/// structure to support options longer than 255 bytes, such as classless route.
+///
+typedef struct {
+  UINT8                     Tag;
+  UINT16                    Len;
+  UINT8                     *Data;
+} DHCP_OPTION;
+
+///
+/// Structures used to parse the DHCP options with RFC3396 support.
+///
+typedef struct {
+  UINT8                     Index;
+  UINT16                    Offset;
+} DHCP_OPTION_COUNT;
+
+typedef struct {
+  DHCP_OPTION_COUNT         *OpCount;
+  DHCP_OPTION               *Options;
+  UINT8                     *Buf;
+} DHCP_OPTION_CONTEXT;
+
+///
+/// The options that matters to DHCP driver itself. The user of
+/// DHCP clients may be interested in other options, such as
+/// classless route, who can parse the DHCP offer to get them.
+///
+typedef struct {
+  IP4_ADDR                  NetMask;  // DHCP_TAG_NETMASK
+  IP4_ADDR                  Router;   // DHCP_TAG_ROUTER, only the first router is used
+
+  //
+  // DHCP specific options
+  //
+  UINT8                     DhcpType; // DHCP_TAG_TYPE
+  UINT8                     Overload; // DHCP_TAG_OVERLOAD
+  IP4_ADDR                  ServerId; // DHCP_TAG_SERVER_ID
+  UINT32                    Lease;    // DHCP_TAG_LEASE
+  UINT32                    T1;       // DHCP_TAG_T1
+  UINT32                    T2;       // DHCP_TAG_T2
+} DHCP_PARAMETER;
+
+///
+/// Structure used to describe and validate the format of DHCP options.
+/// Type is the options' data type, such as DHCP_OPTION_INT8. MinOccur
+/// is the minium occurance of this data type. MaxOccur is defined
+/// similarly. If MaxOccur is -1, it means that there is no limit on the
+/// maximum occurance. Alert tells whether DHCP client should further
+/// inspect the option to parse DHCP_PARAMETER.
+///
+typedef struct {
+  UINT8                     Tag;
+  INTN                      Type;
+  INTN                      MinOccur;
+  INTN                      MaxOccur;
+  BOOLEAN                   Alert;
+} DHCP_OPTION_FORMAT;
+
+typedef
+EFI_STATUS
+(*DHCP_CHECK_OPTION) (
+  IN UINT8                  Tag,
+  IN UINT8                  Len,
+  IN UINT8                  *Data,
+  IN VOID                   *Context
+  );
+
+/**
+  Iterate through a DHCP message to visit each option. First inspect
+  all the options in the OPTION field. Then if overloaded, inspect
+  the options in FILENAME and SERVERNAME fields. One option may be
+  encoded in several places. See RFC 3396 Encoding Long Options in DHCP
+
+  @param[in]  Packet                 The DHCP packet to check the options for
+  @param[in]  Check                  The callback function to be called for each option
+                                     found
+  @param[in]  Context                The opaque parameter for Check
+
+  @retval EFI_SUCCESS            The DHCP packet's options are well formated
+  @retval EFI_INVALID_PARAMETER  The DHCP packet's options are not well formated
+
+**/
+EFI_STATUS
+DhcpIterateOptions (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  IN  DHCP_CHECK_OPTION     Check         OPTIONAL,
+  IN  VOID                  *Context
+  );
+
+/**
+  Validate the packet's options. If necessary, allocate
+  and fill in the interested parameters.
+
+  @param[in]  Packet                 The packet to validate the options
+  @param[out] Para                   The variable to save the DHCP parameters.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to validate the packet.
+  @retval EFI_INVALID_PARAMETER  The options are mal-formated
+  @retval EFI_SUCCESS            The options are parsed into OptionPoint
+
+**/
+EFI_STATUS
+DhcpValidateOptions (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  OUT DHCP_PARAMETER        **Para       OPTIONAL
+  );
+
+/**
+  Parse the options of a DHCP packet. It supports RFC 3396: Encoding
+  Long Options in DHCP. That is, it will combine all the option value
+  of all the occurances of each option.
+  A little bit of implemenation:
+  It adopts the "Key indexed counting" algorithm. First, it allocates
+  an array of 256 DHCP_OPTION_COUNTs because DHCP option tag is encoded
+  as a UINT8. It then iterates the DHCP packet to get data length of
+  each option by calling DhcpIterOptions with DhcpGetOptionLen. Now, it
+  knows the number of present options and their length. It allocates a
+  array of DHCP_OPTION and a continuous buffer after the array to put
+  all the options' data. Each option's data is pointed to by the Data
+  field in DHCP_OPTION structure. At last, it call DhcpIterateOptions
+  with DhcpFillOption to fill each option's data to its position in the
+  buffer.
+
+  @param[in]  Packet                 The DHCP packet to parse the options
+  @param[out] Count                  The number of valid dhcp options present in the
+                                     packet
+  @param[out] OptionPoint            The array that contains the DHCP options. Caller
+                                     should free it.
+
+  @retval EFI_NOT_FOUND          Cannot find any option.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to parse the packet.
+  @retval EFI_INVALID_PARAMETER  The options are mal-formated
+  @retval EFI_SUCCESS            The options are parsed into OptionPoint
+
+**/
+EFI_STATUS
+DhcpParseOption (
+  IN  EFI_DHCP4_PACKET      *Packet,
+  OUT INTN                  *Count,
+  OUT DHCP_OPTION           **OptionPoint
+  );
+
+/**
+  Append an option to the memory, if the option is longer than
+  255 bytes, splits it into several options.
+
+  @param[out] Buf                    The buffer to append the option to
+  @param[in]  Tag                    The option's tag
+  @param[in]  DataLen                The length of the option's data
+  @param[in]  Data                   The option's data
+
+  @return The position to append the next option
+
+**/
+UINT8 *
+DhcpAppendOption (
+  OUT UINT8                  *Buf,
+  IN  UINT8                  Tag,
+  IN  UINT16                 DataLen,
+  IN  UINT8                  *Data
+  );
+
+/**
+  Build a new DHCP packet from a seed packet. Options may be deleted or
+  appended. The caller should free the NewPacket when finished using it.
+
+  @param[in]  SeedPacket             The seed packet to start with
+  @param[in]  DeleteCount            The number of options to delete
+  @param[in]  DeleteList             The options to delete from the packet
+  @param[in]  AppendCount            The number of options to append
+  @param[in]  AppendList             The options to append to the packet
+  @param[out] NewPacket              The new packet, allocated and built by this
+                                     function.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory
+  @retval EFI_INVALID_PARAMETER  The options in SeekPacket are mal-formated
+  @retval EFI_SUCCESS            The packet is build.
+
+**/
+EFI_STATUS
+DhcpBuild (
+  IN  EFI_DHCP4_PACKET        *SeedPacket,
+  IN  UINT32                  DeleteCount,
+  IN  UINT8                   *DeleteList     OPTIONAL,
+  IN  UINT32                  AppendCount,
+  IN  EFI_DHCP4_PACKET_OPTION *AppendList[]   OPTIONAL,
+  OUT EFI_DHCP4_PACKET        **NewPacket
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.c b/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.c
new file mode 100644
index 0000000000..c605f721eb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.c
@@ -0,0 +1,347 @@
+/** @file
+
+Copyright (c) 2007 - 2008, 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.
+
+Module Name:
+
+  Dpc.c
+
+Abstract:
+
+
+**/
+
+#include "Dpc.h"
+
+//
+// Handle for the EFI_DPC_PROTOCOL instance
+//
+EFI_HANDLE  mDpcHandle = NULL;
+
+//
+// The EFI_DPC_PROTOCOL instances that is installed onto mDpcHandle
+//
+EFI_DPC_PROTOCOL mDpc = {
+  DpcQueueDpc,
+  DpcDispatchDpc
+};
+
+//
+// Global variables used to meaasure the DPC Queue Depths
+//
+UINTN  mDpcQueueDepth = 0;
+UINTN  mMaxDpcQueueDepth = 0;
+
+//
+// Free list of DPC entries.  As DPCs are queued, entries are removed from this
+// free list.  As DPC entries are dispatched, DPC entries are added to the free list.
+// If the free list is empty and a DPC is queued, the free list is grown by allocating
+// an additional set of DPC entries.
+//
+LIST_ENTRY      mDpcEntryFreeList = INITIALIZE_LIST_HEAD_VARIABLE(mDpcEntryFreeList);
+
+//
+// An array of DPC queues.  A DPC queue is allocated for every leval EFI_TPL value.
+// As DPCs are queued, they are added to the end of the linked list.
+// As DPCs are dispatched, they are removed from the beginning of the linked list.
+//
+LIST_ENTRY      mDpcQueue[TPL_HIGH_LEVEL + 1];
+
+/**
+  Add a Deferred Procedure Call to the end of the DPC queue.
+
+  @param  This          Protocol instance pointer.
+  @param  DpcTpl        The EFI_TPL that the DPC should be invoked.
+  @param  DpcProcedure  Pointer to the DPC's function.
+  @param  DpcContext    Pointer to the DPC's context.  Passed to DpcProcedure
+                        when DpcProcedure is invoked.
+
+  @retval EFI_SUCCESS            The DPC was queued.
+  @retval EFI_INVALID_PARAMETER  DpcTpl is not a valid EFI_TPL.
+  @retval EFI_INVALID_PARAMETER  DpcProcedure is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to
+                                 add the DPC to the queue.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcQueueDpc (
+  IN EFI_DPC_PROTOCOL   *This,
+  IN EFI_TPL            DpcTpl,
+  IN EFI_DPC_PROCEDURE  DpcProcedure,
+  IN VOID               *DpcContext    OPTIONAL
+  )
+{
+  EFI_STATUS  ReturnStatus;
+  EFI_TPL     OriginalTpl;
+  DPC_ENTRY   *DpcEntry;
+  UINTN       Index;
+
+  //
+  // Make sure DpcTpl is valid
+  //
+  if (DpcTpl < TPL_APPLICATION || DpcTpl > TPL_HIGH_LEVEL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Make sure DpcProcedure is valid
+  //
+  if (DpcProcedure == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Assume this function will succeed
+  //
+  ReturnStatus = EFI_SUCCESS;
+
+  //
+  // Raise the TPL level to TPL_HIGH_LEVEL for DPC list operation and save the
+  // current TPL value so it can be restored when this function returns.
+  //
+  OriginalTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  //
+  // Check to see if there are any entries in the DPC free list
+  //
+  if (IsListEmpty (&mDpcEntryFreeList)) {
+    //
+    // If the current TPL is greater than TPL_NOTIFY, then memory allocations
+    // can not be performed, so the free list can not be expanded.  In this case
+    // return EFI_OUT_OF_RESOURCES.
+    //
+    if (OriginalTpl > TPL_NOTIFY) {
+      ReturnStatus = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    //
+    // Add 64 DPC entries to the free list
+    //
+    for (Index = 0; Index < 64; Index++) {
+      //
+      // Lower the TPL level to perform a memory allocation
+      //
+      gBS->RestoreTPL (OriginalTpl);
+
+      //
+      // Allocate a new DPC entry
+      //
+      DpcEntry = AllocatePool (sizeof (DPC_ENTRY));
+
+      //
+      // Raise the TPL level back to TPL_HIGH_LEVEL for DPC list operations
+      //
+      gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+      //
+      // If the allocation of a DPC entry fails, and the free list is empty,
+      // then return EFI_OUT_OF_RESOURCES.
+      //
+      if (DpcEntry == NULL) {
+        if (IsListEmpty (&mDpcEntryFreeList)) {
+          ReturnStatus = EFI_OUT_OF_RESOURCES;
+          goto Done;
+        }
+      }
+
+      //
+      // Add the newly allocated DPC entry to the DPC free list
+      //
+      InsertTailList (&mDpcEntryFreeList, &DpcEntry->ListEntry);
+    }
+  }
+
+  //
+  // Retrieve the first node from the free list of DPCs
+  //
+  DpcEntry = (DPC_ENTRY *)(GetFirstNode (&mDpcEntryFreeList));
+
+  //
+  // Remove the first node from the free list of DPCs
+  //
+  RemoveEntryList (&DpcEntry->ListEntry);
+
+  //
+  // Fill in the DPC entry with the DpcProcedure and DpcContext
+  //
+  DpcEntry->DpcProcedure = DpcProcedure;
+  DpcEntry->DpcContext   = DpcContext;
+
+  //
+  // Add the DPC entry to the end of the list for the specified DplTpl.
+  //
+  InsertTailList (&mDpcQueue[DpcTpl], &DpcEntry->ListEntry);
+
+  //
+  // Increment the measured DPC queue depth across all TPLs
+  //
+  mDpcQueueDepth++;
+
+  //
+  // Measure the maximum DPC queue depth across all TPLs
+  //
+  if (mDpcQueueDepth > mMaxDpcQueueDepth) {
+    mMaxDpcQueueDepth = mDpcQueueDepth;
+  }
+
+Done:
+  //
+  // Restore the original TPL level when this function was called
+  //
+  gBS->RestoreTPL (OriginalTpl);
+
+  return ReturnStatus;
+}
+
+/**
+  Dispatch the queue of DPCs.  ALL DPCs that have been queued with a DpcTpl
+  value greater than or equal to the current TPL are invoked in the order that
+  they were queued.  DPCs with higher DpcTpl values are invoked before DPCs with
+  lower DpcTpl values.
+
+  @param  This  Protocol instance pointer.
+
+  @retval EFI_SUCCESS    One or more DPCs were invoked.
+  @retval EFI_NOT_FOUND  No DPCs were invoked.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcDispatchDpc (
+  IN EFI_DPC_PROTOCOL  *This
+  )
+{
+  EFI_STATUS  ReturnStatus;
+  EFI_TPL     OriginalTpl;
+  EFI_TPL     Tpl;
+  DPC_ENTRY   *DpcEntry;
+
+  //
+  // Assume that no DPCs will be invoked
+  //
+  ReturnStatus = EFI_NOT_FOUND;
+
+  //
+  // Raise the TPL level to TPL_HIGH_LEVEL for DPC list operation and save the
+  // current TPL value so it can be restored when this function returns.
+  //
+  OriginalTpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+  //
+  // Check to see if there are 1 or more DPCs currently queued
+  //
+  if (mDpcQueueDepth > 0) {
+    //
+    // Loop from TPL_HIGH_LEVEL down to the current TPL value
+    //
+    for (Tpl = TPL_HIGH_LEVEL; Tpl >= OriginalTpl; Tpl--) {
+      //
+      // Check to see if the DPC queue is empty
+      //
+      while (!IsListEmpty (&mDpcQueue[Tpl])) {
+        //
+        // Retrieve the first DPC entry from the DPC queue specified by Tpl
+        //
+        DpcEntry = (DPC_ENTRY *)(GetFirstNode (&mDpcQueue[Tpl]));
+
+        //
+        // Remove the first DPC entry from the DPC queue specified by Tpl
+        //
+        RemoveEntryList (&DpcEntry->ListEntry);
+
+        //
+        // Decrement the measured DPC Queue Depth across all TPLs
+        //
+        mDpcQueueDepth--;
+
+        //
+        // Lower the TPL to TPL value of the current DPC queue
+        //
+        gBS->RestoreTPL (Tpl);
+
+        //
+        // Invoke the DPC passing in its context
+        //
+        (DpcEntry->DpcProcedure) (DpcEntry->DpcContext);
+
+        //
+        // At least one DPC has been invoked, so set the return status to EFI_SUCCESS
+        //
+        ReturnStatus = EFI_SUCCESS;
+
+        //
+        // Raise the TPL level back to TPL_HIGH_LEVEL for DPC list operations
+        //
+        gBS->RaiseTPL (TPL_HIGH_LEVEL);
+
+        //
+        // Add the invoked DPC entry to the DPC free list
+        //
+        InsertTailList (&mDpcEntryFreeList, &DpcEntry->ListEntry);
+      }
+    }
+  }
+
+  //
+  // Restore the original TPL level when this function was called
+  //
+  gBS->RestoreTPL (OriginalTpl);
+
+  return ReturnStatus;
+}
+
+/**
+  The entry point for DPC driver which installs the EFI_DPC_PROTOCOL onto a new handle.
+
+  @param  ImageHandle            The image handle of the driver.
+  @param  SystemTable            The system table.
+
+  @retval EFI_SUCCES             The DPC queues were initialized and the EFI_DPC_PROTOCOL was
+                                 installed onto a new handle.
+  @retval Others                 Failed to install EFI_DPC_PROTOCOL.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcDriverEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+
+  //
+  // ASSERT() if the EFI_DPC_PROTOCOL is already present in the handle database
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiDpcProtocolGuid);
+
+  //
+  // Initialize the DPC queue for all possible TPL values
+  //
+  for (Index = 0; Index <= TPL_HIGH_LEVEL; Index++) {
+    InitializeListHead (&mDpcQueue[Index]);
+  }
+
+  //
+  // Install the EFI_DPC_PROTOCOL instance onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mDpcHandle,
+                  &gEfiDpcProtocolGuid, 
+                  &mDpc,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.h b/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.h
new file mode 100644
index 0000000000..443bc3904e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/DpcDxe/Dpc.h
@@ -0,0 +1,86 @@
+/** @file
+
+Copyright (c) 2007, 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.
+
+Module Name:
+
+  Dpc.h
+
+Abstract:
+
+
+**/
+
+#ifndef _DPC_H_
+#define _DPC_H_
+
+#include <Uefi.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Protocol/Dpc.h>
+
+//
+// Internal data struture for managing DPCs.  A DPC entry is either on the free
+// list or on a DPC queue at a specific EFI_TPL.
+//
+typedef struct {
+  LIST_ENTRY             ListEntry;
+  EFI_DPC_PROCEDURE  DpcProcedure;
+  VOID               *DpcContext;
+} DPC_ENTRY;
+
+/**
+  Add a Deferred Procedure Call to the end of the DPC queue.
+
+  @param  This          Protocol instance pointer.
+  @param  DpcTpl        The EFI_TPL that the DPC should be invoked.
+  @param  DpcProcedure  Pointer to the DPC's function.
+  @param  DpcContext    Pointer to the DPC's context.  Passed to DpcProcedure
+                        when DpcProcedure is invoked.
+
+  @retval EFI_SUCCESS            The DPC was queued.
+  @retval EFI_INVALID_PARAMETER  DpcTpl is not a valid EFI_TPL.
+  @retval EFI_INVALID_PARAMETER  DpcProcedure is NULL.
+  @retval EFI_OUT_OF_RESOURCES   There are not enough resources available to
+                                 add the DPC to the queue.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcQueueDpc (
+  IN EFI_DPC_PROTOCOL   *This,
+  IN EFI_TPL            DpcTpl,
+  IN EFI_DPC_PROCEDURE  DpcProcedure,
+  IN VOID               *DpcContext    OPTIONAL
+  );
+
+/**
+  Dispatch the queue of DPCs.  ALL DPCs that have been queued with a DpcTpl
+  value greater than or equal to the current TPL are invoked in the order that
+  they were queued.  DPCs with higher DpcTpl values are invoked before DPCs with
+  lower DpcTpl values.
+
+  @param  This  Protocol instance pointer.
+
+  @retval EFI_SUCCESS    One or more DPCs were invoked.
+  @retval EFI_NOT_FOUND  No DPCs were invoked.
+
+**/
+EFI_STATUS
+EFIAPI
+DpcDispatchDpc (
+  IN EFI_DPC_PROTOCOL  *This
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.inf b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.inf
new file mode 100644
index 0000000000..8a1f8baf79
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.inf
@@ -0,0 +1,52 @@
+## @file
+#  This module produces Deferred Procedure Call Protocol.
+#
+#  Copyright (c) 2007 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION               = 0x00010005
+  BASE_NAME                 = DpcDxe
+  MODULE_UNI_FILE           = DpcDxe.uni
+  FILE_GUID                 = A210F973-229D-4f4d-AA37-9895E6C9EABA
+  MODULE_TYPE               = DXE_DRIVER
+  VERSION_STRING            = 1.0
+  ENTRY_POINT               = DpcDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES      = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Dpc.c
+  Dpc.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  BaseLib
+  DebugLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+
+[Protocols]
+  gEfiDpcProtocolGuid                           ## PRODUCES
+
+[Depex]
+  TRUE
+[UserExtensions.TianoCore."ExtraFiles"]
+  DpcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.uni b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.uni
new file mode 100644
index 0000000000..82e3e7985c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxeExtra.uni
new file mode 100644
index 0000000000..ca47539550
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/DpcDxe/DpcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.c
new file mode 100644
index 0000000000..063b372292
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.c
@@ -0,0 +1,283 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for iSCSI.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL     gIScsiComponentName = {
+  IScsiComponentNameGetDriverName,
+  IScsiComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL    gIScsiComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) IScsiComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) IScsiComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mIScsiDriverNameTable[] = {
+  {"eng;en", L"iSCSI Driver"}, 
+  {NULL, NULL}
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE  *mIScsiControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the EFI Driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+  
+  @param[in]  This        A pointer to the EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param[in]  Language    A pointer to a three character ISO 639-2 language identifier.
+                          This is the language of the driver name that that the caller
+                          is requesting, and it must match one of the languages specified
+                          in SupportedLanguages.  The number of languages supported by a
+                          driver is up to the driver writer.
+  @param[out]  DriverName A pointer to the Unicode string to return.  This Unicode string
+                          is the name of the driver specified by This in the language
+                          specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by This
+                                and the language specified by Language was returned
+                                in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support the
+                                language specified by Language.
+**/
+EFI_STATUS
+EFIAPI
+IScsiComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN  CHAR8                         *Language,
+  OUT CHAR16                        **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+          Language,
+          This->SupportedLanguages,
+          mIScsiDriverNameTable,
+          DriverName,
+          (BOOLEAN)(This == &gIScsiComponentName)
+          );
+}
+
+/**
+  Update the component name for the iSCSI instance.
+
+  @param[in]  IScsiExtScsiPassThru  A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  
+  @retval EFI_SUCCESS               Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER     The input parameter is invalid.
+  @retval EFI_UNSUPPORTED           Can't get the corresponding NIC info from the Controller handle.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN   EFI_EXT_SCSI_PASS_THRU_PROTOCOL *IScsiExtScsiPassThru
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[150];
+  ISCSI_DRIVER_DATA                *Private;
+  EFI_MAC_ADDRESS                  MacAddress;
+  UINTN                            HwAddressSize;
+  UINT16                           VlanId;
+  CHAR16                           MacString[70];
+
+  if (IScsiExtScsiPassThru == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Private  = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (IScsiExtScsiPassThru);
+
+  //
+  // Get the mac string, it's the name of various variable
+  //
+  Status = NetLibGetMacAddress (Private->Controller, &MacAddress, &HwAddressSize);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  VlanId = NetLibGetVlanId (Private->Controller);
+  IScsiMacAddrToStr (&MacAddress, (UINT32) HwAddressSize, VlanId, MacString);
+    
+  UnicodeSPrint (
+    HandleName,
+    sizeof (HandleName),
+    L"iSCSI IPv4 (MacString=%s)",
+    MacString
+  );
+
+  if (mIScsiControllerNameTable != NULL) {
+    FreeUnicodeStringTable (mIScsiControllerNameTable);
+    mIScsiControllerNameTable = NULL;
+  }
+
+  Status = AddUnicodeString2 (
+             "eng",
+             gIScsiComponentName.SupportedLanguages,
+             &mIScsiControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return AddUnicodeString2 (
+           "en",
+           gIScsiComponentName2.SupportedLanguages,
+           &mIScsiControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by an EFI Driver.Currently not implemented.
+
+  @param[in]  This             A pointer to the EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param[in]  ControllerHandle The handle of a controller that the driver specified by
+                               This is managing.  This handle specifies the controller
+                               whose name is to be returned.
+  @param[in]  ChildHandle      The handle of the child controller to retrieve the name
+                               of.  This is an optional parameter that may be NULL.  It
+                               will be NULL for device drivers.  It will also be NULL
+                               for a bus drivers that wish to retrieve the name of the
+                               bus controller.  It will not be NULL for a bus driver
+                               that wishes to retrieve the name of a child controller.
+  @param[in]  Language         A pointer to a three character ISO 639-2 language
+                               identifier.  This is the language of the controller name
+                               that that the caller is requesting, and it must match one
+                               of the languages specified in SupportedLanguages.  The
+                               number of languages supported by a driver is up to the
+                               driver writer.
+  @param[out]  ControllerName  A pointer to the Unicode string to return.  This Unicode
+                               string is the name of the controller specified by
+                               ControllerHandle and ChildHandle in the language specified
+                               by Language from the point of view of the driver specified
+                               by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in the
+                                language specified by Language for the driver
+                                specified by This was returned in DriverName.                                
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently managing
+                                the controller specified by ControllerHandle and
+                                ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support the
+                                language specified by Language.
+**/
+EFI_STATUS
+EFIAPI
+IScsiComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  EFI_HANDLE                    ChildHandle        OPTIONAL,
+  IN  CHAR8                         *Language,
+  OUT CHAR16                        **ControllerName
+  )
+{
+  EFI_STATUS                      Status;
+
+  EFI_HANDLE                      IScsiController;
+  ISCSI_PRIVATE_PROTOCOL          *IScsiIdentifier;
+
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL *IScsiExtScsiPassThru;
+
+  if (ControllerHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get the handle of the controller we are controling.
+  //
+  IScsiController = NetLibGetNicHandle (ControllerHandle, &gEfiTcp4ProtocolGuid);
+  if (IScsiController == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  IScsiController,
+                  &gEfiCallerIdGuid,
+                  (VOID **)&IScsiIdentifier,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (ChildHandle != NULL) {
+    //
+    // Make sure this driver produced ChildHandle
+    //
+    Status = EfiTestChildHandle (
+               ControllerHandle,
+               ChildHandle,
+               &gEfiTcp4ProtocolGuid
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Retrieve an instance of a produced protocol from ChildHandle
+      //
+      Status = gBS->OpenProtocol (
+                      ChildHandle,
+                      &gEfiExtScsiPassThruProtocolGuid,
+                     (VOID **)&IScsiExtScsiPassThru,
+                      NULL,
+                      NULL,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      //
+      // Update the component name for this child handle.
+      //
+      Status = UpdateName (IScsiExtScsiPassThru);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    } else {
+      return Status;
+    }
+  }
+  
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mIScsiControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gIScsiComponentName)
+           );
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.h
new file mode 100644
index 0000000000..b400abaa41
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/ComponentName.h
@@ -0,0 +1,165 @@
+/** @file
+  The header file of UEFI Component Name(2) protocol.
+
+Copyright (c) 2004 - 2011, 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.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+
+extern EFI_COMPONENT_NAME2_PROTOCOL       gIScsiComponentName2;
+extern EFI_COMPONENT_NAME_PROTOCOL        gIScsiComponentName;
+
+//
+// EFI Component Name Protocol for iSCSI driver.
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the EFI Driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+  
+  @param[in]  This        A pointer to the EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param[in]  Language    A pointer to a three characters ISO 639-2 language identifier.
+                          This is the language of the driver name that that the caller
+                          is requesting, and it must match one of the languages specified
+                          in SupportedLanguages.  The number of languages supported by a
+                          driver is up to the driver writer.
+  @param[out]  DriverName A pointer to the Unicode string to return.  This Unicode string
+                          is the name of the driver specified by This in the language
+                          specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by This
+                                and the language specified by Language was returned
+                                in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support the
+                                language specified by Language.
+**/
+EFI_STATUS
+EFIAPI
+IScsiComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN  CHAR8                         *Language,
+  OUT CHAR16                        **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by an EFI Driver. Currently not implemented.
+
+  @param[in]  This             A pointer to the EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param[in]  ControllerHandle The handle of a controller that the driver specified by
+                               This is managing.  This handle specifies the controller
+                               whose name is to be returned.
+  @param[in]  ChildHandle      The handle of the child controller to retrieve the name
+                               of.  This is an optional parameter that may be NULL.  It
+                               will be NULL for device drivers.  It will also be NULL
+                               for a bus drivers that wish to retrieve the name of the
+                               bus controller.  It will not be NULL for a bus driver
+                               that wishes to retrieve the name of a child controller.
+  @param[in]  Language         A pointer to a three characters ISO 639-2 language
+                               identifier.  This is the language of the controller name
+                               that that the caller is requesting, and it must match one
+                               of the languages specified in SupportedLanguages.  The
+                               number of languages supported by a driver is up to the
+                               driver writer.
+  @param[out]  ControllerName  A pointer to the Unicode string to return.  This Unicode
+                               string is the name of the controller specified by
+                               ControllerHandle and ChildHandle in the language specified
+                               by Language from the point of view of the driver specified
+                               by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in the
+                                language specified by Language for the driver
+                                specified by This was returned in DriverName.                                
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently managing
+                                the controller specified by ControllerHandle and
+                                ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support the
+                                language specified by Language.
+**/
+EFI_STATUS
+EFIAPI
+IScsiComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN  EFI_HANDLE                    ControllerHandle,
+  IN  EFI_HANDLE                    ChildHandle        OPTIONAL,
+  IN  CHAR8                         *Language,
+  OUT CHAR16                        **ControllerName
+  );
+  
+//
+// EFI iSCSI Initiator Name Protocol for IScsi driver.
+//
+
+/**
+  Retrieves the current set value of iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer / Actual size of the
+                              variable data buffer.
+  @param[out]      Buffer     Pointer to the buffer for data to be read.
+
+  @retval EFI_SUCCESS           Data was successfully retrieved into the provided buffer and the
+                                BufferSize was sufficient to handle the iSCSI initiator name
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the result.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL.
+  @retval EFI_DEVICE_ERROR      The iSCSI initiator name could not be retrieved due to a hardware error.
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiGetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  OUT    VOID                               *Buffer
+  );
+
+/**
+  Sets the iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer.
+  @param[in]       Buffer     Pointer to the buffer for data to be written.
+
+  @retval EFI_SUCCESS           Data was successfully stored by the protocol.
+  @retval EFI_UNSUPPORTED       Platform policies do not allow for data to be written.
+                                Currently not implemented.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL, or BufferSize exceeds the maximum allowed limit.
+  @retval EFI_DEVICE_ERROR      The data could not be stored due to a hardware error.
+  @retval EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the data.
+  @retval EFI_PROTOCOL_ERROR    Input iSCSI initiator name does not adhere to RFC 3720
+                                (and other related protocols)
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiSetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  IN     VOID                               *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4Dxe.uni b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4Dxe.uni
new file mode 100644
index 0000000000..21266b08c5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4DxeExtra.uni
new file mode 100644
index 0000000000..0753072161
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsi4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.c
new file mode 100644
index 0000000000..6307684ff0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.c
@@ -0,0 +1,430 @@
+/** @file
+  This file is for Challenge-Handshake Authentication Protocol (CHAP) Configuration.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+#include "Md5.h"
+
+/**
+  Initator calculates its own expected hash value.
+
+  @param[in]   ChapIdentifier     iSCSI CHAP identifier sent by authenticator.
+  @param[in]   ChapSecret         iSCSI CHAP secret of the authenticator.
+  @param[in]   SecretLength       The length of iSCSI CHAP secret.
+  @param[in]   ChapChallenge      The challenge message sent by authenticator.
+  @param[in]   ChallengeLength    The length of iSCSI CHAP challenge message.
+  @param[out]  ChapResponse       The calculation of the expected hash value.
+
+  @retval EFI_SUCCESS             The expected hash value was calculatedly successfully.
+  @retval EFI_PROTOCOL_ERROR      The length of the secret should be at least the
+                                  length of the hash value for the hashing algorithm chosen.
+  @retval Others                  Other errors as indicated.
+**/
+EFI_STATUS
+IScsiCHAPCalculateResponse (
+  IN  UINT32  ChapIdentifier,
+  IN  CHAR8   *ChapSecret,
+  IN  UINT32  SecretLength,
+  IN  UINT8   *ChapChallenge,
+  IN  UINT32  ChallengeLength,
+  OUT UINT8   *ChapResponse
+  )
+{
+  MD5_CTX     Md5Ctx;
+  CHAR8       IdByte[1];
+  EFI_STATUS  Status;
+
+  Status = MD5Init (&Md5Ctx);
+
+  //
+  // Hash Identifier - Only calculate 1 byte data (RFC1994)
+  //
+  IdByte[0] = (CHAR8) ChapIdentifier;
+  MD5Update (&Md5Ctx, IdByte, 1);
+
+  //
+  // Hash Secret
+  //
+  if (SecretLength < ISCSI_CHAP_SECRET_MIN_LEN - 1) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  MD5Update (&Md5Ctx, ChapSecret, SecretLength);
+
+  //
+  // Hash Challenge received from Target
+  //
+  MD5Update (&Md5Ctx, ChapChallenge, ChallengeLength);
+
+  Status = MD5Final (&Md5Ctx, ChapResponse);
+
+  return Status;
+}
+
+/**
+  The initator checks the CHAP response replied by target against its own
+  calculation of the expected hash value.
+
+  @param[in]   AuthData             iSCSI CHAP authentication data.
+  @param[in]   TargetResponse       The response from target.
+
+  @retval EFI_SUCCESS               The response from target passed authentication.
+  @retval EFI_SECURITY_VIOLATION    The response from target was not expected value.
+  @retval Others                    Other errors as indicated.
+**/
+EFI_STATUS
+IScsiCHAPAuthTarget (
+  IN  ISCSI_CHAP_AUTH_DATA  *AuthData,
+  IN  UINT8                 *TargetResponse
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      SecretSize;
+  UINT8       VerifyRsp[ISCSI_CHAP_RSP_LEN];
+
+  Status      = EFI_SUCCESS;
+
+  SecretSize  = (UINT32) AsciiStrLen (AuthData->AuthConfig.ReverseCHAPSecret);
+  Status = IScsiCHAPCalculateResponse (
+            AuthData->OutIdentifier,
+            AuthData->AuthConfig.ReverseCHAPSecret,
+            SecretSize,
+            AuthData->OutChallenge,
+            AuthData->OutChallengeLength,
+            VerifyRsp
+            );
+
+  if (CompareMem (VerifyRsp, TargetResponse, ISCSI_CHAP_RSP_LEN) != 0) {
+    Status = EFI_SECURITY_VIOLATION;
+  }
+
+  return Status;
+}
+
+/**
+  This function checks the received iSCSI Login Response during the security
+  negotiation stage.
+
+  @param[in] Conn             The iSCSI connection.
+
+  @retval EFI_SUCCESS          The Login Response passed the CHAP validation.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of protocol error happend.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiCHAPOnRspReceived (
+  IN ISCSI_CONNECTION  *Conn
+  )
+{
+  EFI_STATUS                Status;
+  ISCSI_SESSION             *Session;
+  ISCSI_CHAP_AUTH_DATA      *AuthData;
+  CHAR8                     *Value;
+  UINT8                     *Data;
+  UINT32                    Len;
+  LIST_ENTRY                *KeyValueList;
+  UINTN                     Algorithm;
+  CHAR8                     *Identifier;
+  CHAR8                     *Challenge;
+  CHAR8                     *Name;
+  CHAR8                     *Response;
+  UINT8                     TargetRsp[ISCSI_CHAP_RSP_LEN];
+  UINT32                    RspLen;
+
+  ASSERT (Conn->CurrentStage == ISCSI_SECURITY_NEGOTIATION);
+  ASSERT (Conn->RspQue.BufNum != 0);
+
+  Session     = Conn->Session;
+  AuthData    = &Session->AuthData;
+
+  Len         = Conn->RspQue.BufSize;
+  Data        = AllocatePool (Len);
+  if (Data == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Copy the data in case the data spans over multiple PDUs.
+  //
+  NetbufQueCopy (&Conn->RspQue, 0, Len, Data);
+
+  //
+  // Build the key-value list from the data segment of the Login Response.
+  //
+  KeyValueList = IScsiBuildKeyValueList ((CHAR8 *) Data, Len);
+  if (KeyValueList == NULL) {
+    FreePool (Data);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = EFI_PROTOCOL_ERROR;
+
+  switch (Conn->CHAPStep) {
+  case ISCSI_CHAP_INITIAL:
+    //
+    // The first Login Response.
+    //
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_TARGET_PORTAL_GROUP_TAG);
+    if (Value == NULL) {
+      goto ON_EXIT;
+    }
+
+    Session->TargetPortalGroupTag = (UINT16) AsciiStrDecimalToUintn (Value);
+
+    Value                         = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_AUTH_METHOD);
+    if (Value == NULL) {
+      goto ON_EXIT;
+    }
+    //
+    // Initiator mandates CHAP authentication but target replies without "CHAP" or
+    // initiator suggets "None" but target replies with some kind of auth method.
+    //
+    if (AsciiStrCmp (Value, ISCSI_AUTH_METHOD_CHAP) == 0) {
+      if (AuthData->AuthConfig.CHAPType == ISCSI_CHAP_NONE) {
+        goto ON_EXIT;
+      }
+    } else {
+      if (AuthData->AuthConfig.CHAPType != ISCSI_CHAP_NONE) {
+        goto ON_EXIT;
+      }
+    }
+    //
+    // Transit to CHAP step one.
+    //
+    Conn->CHAPStep  = ISCSI_CHAP_STEP_ONE;
+    Status          = EFI_SUCCESS;
+    break;
+
+  case ISCSI_CHAP_STEP_TWO:
+    //
+    // The Target replies with CHAP_A=<A> CHAP_I=<I> CHAP_C=<C>
+    //
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_CHAP_ALGORITHM);
+    if (Value == NULL) {
+      goto ON_EXIT;
+    }
+
+    Algorithm = AsciiStrDecimalToUintn (Value);
+    if (Algorithm != ISCSI_CHAP_ALGORITHM_MD5) {
+      //
+      // Unsupported algorithm is chosen by target.
+      //
+      goto ON_EXIT;
+    }
+
+    Identifier = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_CHAP_IDENTIFIER);
+    if (Identifier == NULL) {
+      goto ON_EXIT;
+    }
+
+    Challenge = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_CHAP_CHALLENGE);
+    if (Challenge == NULL) {
+      goto ON_EXIT;
+    }
+    //
+    // Process the CHAP identifier and CHAP Challenge from Target
+    // Calculate Response value
+    //
+    AuthData->InIdentifier      = (UINT32) AsciiStrDecimalToUintn (Identifier);
+    AuthData->InChallengeLength = ISCSI_CHAP_AUTH_MAX_LEN;
+    IScsiHexToBin ((UINT8 *) AuthData->InChallenge, &AuthData->InChallengeLength, Challenge);
+    Status = IScsiCHAPCalculateResponse (
+              AuthData->InIdentifier,
+              AuthData->AuthConfig.CHAPSecret,
+              (UINT32) AsciiStrLen (AuthData->AuthConfig.CHAPSecret),
+              AuthData->InChallenge,
+              AuthData->InChallengeLength,
+              AuthData->CHAPResponse
+              );
+
+    //
+    // Transit to next step.
+    //
+    Conn->CHAPStep = ISCSI_CHAP_STEP_THREE;
+    break;
+
+  case ISCSI_CHAP_STEP_THREE:
+    //
+    // one way CHAP authentication and the target would like to
+    // authenticate us.
+    //
+    Status = EFI_SUCCESS;
+    break;
+
+  case ISCSI_CHAP_STEP_FOUR:
+    ASSERT (AuthData->AuthConfig.CHAPType == ISCSI_CHAP_MUTUAL);
+    //
+    // The forth step, CHAP_N=<N> CHAP_R=<R> is received from Target.
+    //
+    Name = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_CHAP_NAME);
+    if (Name == NULL) {
+      goto ON_EXIT;
+    }
+
+    Response = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_CHAP_RESPONSE);
+    if (Response == NULL) {
+      goto ON_EXIT;
+    }
+
+    RspLen = ISCSI_CHAP_RSP_LEN;
+    IScsiHexToBin (TargetRsp, &RspLen, Response);
+
+    //
+    // Check the CHAP Response replied by Target.
+    //
+    Status = IScsiCHAPAuthTarget (AuthData, TargetRsp);
+    break;
+
+  default:
+    break;
+  }
+
+ON_EXIT:
+
+  IScsiFreeKeyValueList (KeyValueList);
+
+  FreePool (Data);
+
+  return Status;
+}
+
+/**
+  This function fills the CHAP authentication information into the login PDU
+  during the security negotiation stage in the iSCSI connection login.
+
+  @param[in]       Conn        The iSCSI connection.
+  @param[in, out]  Pdu         The PDU to send out.
+
+  @retval EFI_SUCCESS          All check passed and the phase-related CHAP
+                               authentication info is filled into the iSCSI PDU.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of protocol error happend.
+**/
+EFI_STATUS
+IScsiCHAPToSendReq (
+  IN      ISCSI_CONNECTION  *Conn,
+  IN OUT  NET_BUF           *Pdu
+  )
+{
+  EFI_STATUS                Status;
+  ISCSI_SESSION             *Session;
+  ISCSI_LOGIN_REQUEST       *LoginReq;
+  ISCSI_CHAP_AUTH_DATA      *AuthData;
+  CHAR8                     *Value;
+  CHAR8                     ValueStr[256];
+  CHAR8                     *Response;
+  UINT32                    RspLen;
+  CHAR8                     *Challenge;
+  UINT32                    ChallengeLen;
+
+  ASSERT (Conn->CurrentStage == ISCSI_SECURITY_NEGOTIATION);
+
+  Session     = Conn->Session;
+  AuthData    = &Session->AuthData;
+  LoginReq    = (ISCSI_LOGIN_REQUEST *) NetbufGetByte (Pdu, 0, 0);
+  if (LoginReq == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+  Status      = EFI_SUCCESS;
+
+  RspLen      = 2 * ISCSI_CHAP_RSP_LEN + 3;
+  Response    = AllocatePool (RspLen);
+  if (Response == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ChallengeLen  = 2 * ISCSI_CHAP_RSP_LEN + 3;
+  Challenge     = AllocatePool (ChallengeLen);
+  if (Challenge == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  switch (Conn->CHAPStep) {
+  case ISCSI_CHAP_INITIAL:
+    //
+    // It's the initial Login Request. Fill in the key=value pairs mandatory
+    // for the initial Login Request.
+    //
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_INITIATOR_NAME, Session->InitiatorName);
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_SESSION_TYPE, "Normal");
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_TARGET_NAME, Session->ConfigData.NvData.TargetName);
+
+    if (AuthData->AuthConfig.CHAPType == ISCSI_CHAP_NONE) {
+      Value = ISCSI_KEY_VALUE_NONE;
+      ISCSI_SET_FLAG (LoginReq, ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT);
+    } else {
+      Value = ISCSI_AUTH_METHOD_CHAP;
+    }
+
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_AUTH_METHOD, Value);
+
+    break;
+
+  case ISCSI_CHAP_STEP_ONE:
+    //
+    // First step, send the Login Request with CHAP_A=<A1,A2...> key-value pair.
+    //
+    AsciiSPrint (ValueStr, sizeof (ValueStr), "%d", ISCSI_CHAP_ALGORITHM_MD5);
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_CHAP_ALGORITHM, ValueStr);
+
+    Conn->CHAPStep = ISCSI_CHAP_STEP_TWO;
+    break;
+
+  case ISCSI_CHAP_STEP_THREE:
+    //
+    // Third step, send the Login Request with CHAP_N=<N> CHAP_R=<R> or
+    // CHAP_N=<N> CHAP_R=<R> CHAP_I=<I> CHAP_C=<C> if target ahtentication is
+    // required too.
+    //
+    // CHAP_N=<N>
+    //
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_CHAP_NAME, (CHAR8 *) &AuthData->AuthConfig.CHAPName);
+    //
+    // CHAP_R=<R>
+    //
+    IScsiBinToHex ((UINT8 *) AuthData->CHAPResponse, ISCSI_CHAP_RSP_LEN, Response, &RspLen);
+    IScsiAddKeyValuePair (Pdu, ISCSI_KEY_CHAP_RESPONSE, Response);
+
+    if (AuthData->AuthConfig.CHAPType == ISCSI_CHAP_MUTUAL) {
+      //
+      // CHAP_I=<I>
+      //
+      IScsiGenRandom ((UINT8 *) &AuthData->OutIdentifier, 1);
+      AsciiSPrint (ValueStr, sizeof (ValueStr), "%d", AuthData->OutIdentifier);
+      IScsiAddKeyValuePair (Pdu, ISCSI_KEY_CHAP_IDENTIFIER, ValueStr);
+      //
+      // CHAP_C=<C>
+      //
+      IScsiGenRandom ((UINT8 *) AuthData->OutChallenge, ISCSI_CHAP_RSP_LEN);
+      AuthData->OutChallengeLength = ISCSI_CHAP_RSP_LEN;
+      IScsiBinToHex ((UINT8 *) AuthData->OutChallenge, ISCSI_CHAP_RSP_LEN, Challenge, &ChallengeLen);
+      IScsiAddKeyValuePair (Pdu, ISCSI_KEY_CHAP_CHALLENGE, Challenge);
+
+      Conn->CHAPStep = ISCSI_CHAP_STEP_FOUR;
+    }
+    //
+    // set the stage transition flag.
+    //
+    ISCSI_SET_FLAG (LoginReq, ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT);
+    break;
+
+  default:
+    Status = EFI_PROTOCOL_ERROR;
+    break;
+  }
+
+  FreePool (Response);
+  FreePool (Challenge);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.h
new file mode 100644
index 0000000000..f6a64e56da
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCHAP.h
@@ -0,0 +1,106 @@
+/** @file
+  The header file of CHAP configuration.
+
+Copyright (c) 2004 - 2011, 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.
+
+**/
+
+#ifndef _ISCSI_CHAP_H_
+#define _ISCSI_CHAP_H_
+
+#define ISCSI_AUTH_METHOD_CHAP    "CHAP"
+
+#define ISCSI_KEY_CHAP_ALGORITHM  "CHAP_A"
+#define ISCSI_KEY_CHAP_IDENTIFIER "CHAP_I"
+#define ISCSI_KEY_CHAP_CHALLENGE  "CHAP_C"
+#define ISCSI_KEY_CHAP_NAME       "CHAP_N"
+#define ISCSI_KEY_CHAP_RESPONSE   "CHAP_R"
+
+#define ISCSI_CHAP_ALGORITHM_MD5  5
+
+#define ISCSI_CHAP_AUTH_MAX_LEN   1024
+///
+/// MD5_HASHSIZE
+///
+#define ISCSI_CHAP_RSP_LEN        16  
+
+#define ISCSI_CHAP_INITIAL        0 
+#define ISCSI_CHAP_STEP_ONE       1
+#define ISCSI_CHAP_STEP_TWO       2
+#define ISCSI_CHAP_STEP_THREE     3
+#define ISCSI_CHAP_STEP_FOUR      4
+
+#pragma pack(1)
+
+typedef struct _ISCSI_CHAP_AUTH_CONFIG_NVDATA {
+  UINT8 CHAPType;
+  CHAR8 CHAPName[ISCSI_CHAP_NAME_STORAGE];
+  CHAR8 CHAPSecret[ISCSI_CHAP_SECRET_STORAGE];
+  CHAR8 ReverseCHAPName[ISCSI_CHAP_NAME_STORAGE];
+  CHAR8 ReverseCHAPSecret[ISCSI_CHAP_SECRET_STORAGE];
+} ISCSI_CHAP_AUTH_CONFIG_NVDATA;
+
+#pragma pack()
+
+///
+/// ISCSI CHAP Authentication Data
+///
+typedef struct _ISCSI_CHAP_AUTH_DATA {
+  ISCSI_CHAP_AUTH_CONFIG_NVDATA AuthConfig;
+  UINT32                        InIdentifier;
+  UINT8                         InChallenge[ISCSI_CHAP_AUTH_MAX_LEN];
+  UINT32                        InChallengeLength;
+  //
+  // Calculated CHAP Response (CHAP_R) value
+  //
+  UINT8                         CHAPResponse[ISCSI_CHAP_RSP_LEN];
+
+  //
+  // Auth-data to be sent out for mutual authentication
+  //
+  UINT32                        OutIdentifier;
+  UINT8                         OutChallenge[ISCSI_CHAP_AUTH_MAX_LEN];
+  UINT32                        OutChallengeLength;
+} ISCSI_CHAP_AUTH_DATA;
+
+/**
+  This function checks the received iSCSI Login Response during the security
+  negotiation stage.
+  
+  @param[in] Conn             The iSCSI connection.
+
+  @retval EFI_SUCCESS          The Login Response passed the CHAP validation.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of protocol error happend.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiCHAPOnRspReceived (
+  IN ISCSI_CONNECTION  *Conn
+  );
+/**
+  This function fills the CHAP authentication information into the login PDU
+  during the security negotiation stage in the iSCSI connection login.
+
+  @param[in]       Conn        The iSCSI connection.
+  @param[in, out]  Pdu         The PDU to send out.
+
+  @retval EFI_SUCCESS          All check passed and the phase-related CHAP
+                               authentication info is filled into the iSCSI PDU.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of protocol error happend.
+**/
+EFI_STATUS
+IScsiCHAPToSendReq (
+  IN      ISCSI_CONNECTION  *Conn,
+  IN OUT  NET_BUF           *Pdu
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCommon.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCommon.h
new file mode 100644
index 0000000000..650c68d53b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiCommon.h
@@ -0,0 +1,22 @@
+/** @file
+  The common header file of Iscsi.
+
+Copyright (c) 2004 - 2008, 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.
+
+**/
+
+#ifndef _ISCSI_COMMON_H_
+#define _ISCSI_COMMON_H_
+
+typedef struct _ISCSI_SESSION             ISCSI_SESSION;
+typedef struct _ISCSI_CONNECTION          ISCSI_CONNECTION;
+typedef struct _ISCSI_DRIVER_DATA         ISCSI_DRIVER_DATA;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.c
new file mode 100644
index 0000000000..7b77fd386b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.c
@@ -0,0 +1,1258 @@
+/** @file
+  Helper functions for configuring or getting the parameters relating to iSCSI.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+CHAR16          mVendorStorageName[]     = L"ISCSI_CONFIG_IFR_NVDATA";
+BOOLEAN         mIScsiDeviceListUpdated  = FALSE;
+UINTN           mNumberOfIScsiDevices    = 0;
+ISCSI_FORM_CALLBACK_INFO  *mCallbackInfo = NULL;
+
+LIST_ENTRY      mIScsiConfigFormList = {
+  &mIScsiConfigFormList,
+  &mIScsiConfigFormList
+};
+
+HII_VENDOR_DEVICE_PATH  mIScsiHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    IP4_ISCSI_CONFIG_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  Convert the IPv4 address into a dotted string.
+
+  @param[in]   Ip   The IPv4 address.
+  @param[out]  Str  The dotted IP string.
+**/
+VOID
+IScsiIpToStr (
+  IN  EFI_IPv4_ADDRESS  *Ip,
+  OUT CHAR16            *Str
+  )
+{
+  UnicodeSPrint ( Str, 2 * IP4_STR_MAX_SIZE, L"%d.%d.%d.%d", Ip->Addr[0], Ip->Addr[1], Ip->Addr[2], Ip->Addr[3]);
+}
+
+
+/**
+  Parse IsId in string format and convert it to binary.
+
+  @param[in]        String  The buffer of the string to be parsed.
+  @param[in, out]   IsId    The buffer to store IsId.
+
+  @retval EFI_SUCCESS              The operation finished successfully.
+  @retval EFI_INVALID_PARAMETER    Any input parameter is invalid.
+
+**/
+EFI_STATUS
+IScsiParseIsIdFromString (
+  IN CONST CHAR16                    *String,
+  IN OUT   UINT8                     *IsId
+  )
+{
+  UINT8                          Index;
+  CHAR16                         *IsIdStr;
+  CHAR16                         TempStr[3];
+  UINTN                          NodeVal;
+  CHAR16                         PortString[ISCSI_NAME_IFR_MAX_SIZE];
+  EFI_INPUT_KEY                  Key;
+
+  if ((String == NULL) || (IsId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IsIdStr = (CHAR16 *) String;
+
+  if (StrLen (IsIdStr) != 6) {
+    UnicodeSPrint (
+      PortString,
+      (UINTN) sizeof (PortString),
+      L"Error! Input is incorrect, please input 6 hex numbers!\n"
+      );
+
+    CreatePopUp (
+      EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE,
+      &Key,
+      PortString,
+      NULL
+      );
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Index = 3; Index < 6; Index++) {
+    CopyMem (TempStr, IsIdStr, sizeof (TempStr));
+    TempStr[2] = L'\0';
+
+    //
+    // Convert the string to IsId. StrHexToUintn stops at the first character
+    // that is not a valid hex character, '\0' here.
+    //
+    NodeVal = StrHexToUintn (TempStr);
+
+    IsId[Index] = (UINT8) NodeVal;
+
+    IsIdStr = IsIdStr + 2;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert IsId from binary to string format.
+
+  @param[out]      String  The buffer to store the converted string.
+  @param[in]       IsId    The buffer to store IsId.
+
+  @retval EFI_SUCCESS              The string converted successfully.
+  @retval EFI_INVALID_PARAMETER    Any input parameter is invalid.
+
+**/
+EFI_STATUS
+IScsiConvertIsIdToString (
+  OUT CHAR16                         *String,
+  IN  UINT8                          *IsId
+  )
+{
+  UINT8                          Index;
+  UINTN                          Number;
+
+  if ((String == NULL) || (IsId == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Index = 0; Index < 6; Index++) {
+    if (IsId[Index] <= 0xF) {
+      Number = UnicodeSPrint (
+                 String,
+                 2 * ISID_CONFIGURABLE_STORAGE,
+                 L"0%X",
+                 (UINTN) IsId[Index]
+                 );
+    } else {
+      Number = UnicodeSPrint (
+                 String,
+                 2 * ISID_CONFIGURABLE_STORAGE,
+                 L"%X",
+                 (UINTN) IsId[Index]
+                 );
+
+    }
+
+    String = String + Number;
+  }
+
+  *String = L'\0';
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Update the list of iSCSI devices the iSCSI driver is controlling.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval Others                 Other errors as indicated.   
+**/
+EFI_STATUS
+IScsiUpdateDeviceList (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  ISCSI_DEVICE_LIST           *DeviceList;
+  UINTN                       DataSize;
+  UINTN                       NumHandles;
+  EFI_HANDLE                  *Handles;
+  UINTN                       HandleIndex;
+  UINTN                       Index;
+  UINTN                       LastDeviceIndex;
+  EFI_MAC_ADDRESS             MacAddress;
+  UINTN                       HwAddressSize;
+  UINT16                      VlanId;
+  ISCSI_MAC_INFO              *CurMacInfo;
+  ISCSI_MAC_INFO              TempMacInfo;
+  CHAR16                      MacString[70];
+  UINTN                       DeviceListSize;
+
+  //
+  // Dump all the handles the Managed Network Service Binding Protocol is installed on.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  NULL,
+                  &NumHandles,
+                  &Handles
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DataSize = 0;
+  Status = gRT->GetVariable (
+                  L"iSCSIDeviceList",
+                  &gIp4IScsiConfigGuid,
+                  NULL,
+                  &DataSize,
+                  NULL
+                  );
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    DeviceList = (ISCSI_DEVICE_LIST *) AllocatePool (DataSize);
+    ASSERT (DeviceList != NULL);
+
+    gRT->GetVariable (
+          L"iSCSIDeviceList",
+          &gIp4IScsiConfigGuid,
+          NULL,
+          &DataSize,
+          DeviceList
+          );
+
+    LastDeviceIndex = 0;
+
+    for (HandleIndex = 0; HandleIndex < NumHandles; HandleIndex++) {
+      Status = NetLibGetMacAddress (Handles[HandleIndex], &MacAddress, &HwAddressSize);
+      ASSERT (Status == EFI_SUCCESS);
+      VlanId = NetLibGetVlanId (Handles[HandleIndex]);
+
+      for (Index = LastDeviceIndex; Index < DeviceList->NumDevice; Index++) {
+        CurMacInfo = &DeviceList->MacInfo[Index];
+        if ((CurMacInfo->Len == HwAddressSize) &&
+            (CurMacInfo->VlanId == VlanId) &&
+            (NET_MAC_EQUAL (&CurMacInfo->Mac, MacAddress.Addr, HwAddressSize))
+            ) {
+          //
+          // The previous configured NIC is still here.
+          //
+          if (Index != LastDeviceIndex) {
+            //
+            // Swap the current MAC address entry with the one indexed by
+            // LastDeviceIndex.
+            //
+            CopyMem (&TempMacInfo, CurMacInfo, sizeof (ISCSI_MAC_INFO));
+            CopyMem (CurMacInfo, &DeviceList->MacInfo[LastDeviceIndex], sizeof (ISCSI_MAC_INFO));
+            CopyMem (&DeviceList->MacInfo[LastDeviceIndex], &TempMacInfo, sizeof (ISCSI_MAC_INFO));
+          }
+
+          LastDeviceIndex++;
+        }
+      }
+
+      if (LastDeviceIndex == DeviceList->NumDevice) {
+        break;
+      }
+    }
+
+    for (Index = LastDeviceIndex; Index < DeviceList->NumDevice; Index++) {
+      //
+      // delete the variables
+      //
+      CurMacInfo = &DeviceList->MacInfo[Index];
+      IScsiMacAddrToStr (&CurMacInfo->Mac, CurMacInfo->Len, CurMacInfo->VlanId, MacString);
+      gRT->SetVariable (MacString, &gEfiIScsiInitiatorNameProtocolGuid, 0, 0, NULL);
+      gRT->SetVariable (MacString, &gIScsiCHAPAuthInfoGuid, 0, 0, NULL);
+    }
+
+    FreePool (DeviceList);
+  } else if (Status != EFI_NOT_FOUND) {
+    FreePool (Handles);
+    return Status;
+  }
+  //
+  // Construct the new iSCSI device list.
+  //
+  DeviceListSize        = sizeof (ISCSI_DEVICE_LIST) + (NumHandles - 1) * sizeof (ISCSI_MAC_INFO);
+  DeviceList            = (ISCSI_DEVICE_LIST *) AllocatePool (DeviceListSize);
+  ASSERT (DeviceList != NULL);
+  DeviceList->NumDevice = (UINT8) NumHandles;
+
+  for (Index = 0; Index < NumHandles; Index++) {
+    NetLibGetMacAddress (Handles[Index], &MacAddress, &HwAddressSize);
+
+    CurMacInfo  = &DeviceList->MacInfo[Index];
+    CopyMem (&CurMacInfo->Mac, MacAddress.Addr, HwAddressSize);
+    CurMacInfo->Len = (UINT8) HwAddressSize;
+    CurMacInfo->VlanId = NetLibGetVlanId (Handles[Index]);
+  }
+
+  gRT->SetVariable (
+        L"iSCSIDeviceList",
+        &gIp4IScsiConfigGuid,
+        ISCSI_CONFIG_VAR_ATTR,
+        DeviceListSize,
+        DeviceList
+        );
+
+  FreePool (DeviceList);
+  FreePool (Handles);
+
+  return Status;
+}
+
+/**
+  Get the iSCSI configuration form entry by the index of the goto opcode actived.
+
+  @param[in]  Index The 0-based index of the goto opcode actived.
+
+  @return The iSCSI configuration form entry found.
+**/
+ISCSI_CONFIG_FORM_ENTRY *
+IScsiGetConfigFormEntryByIndex (
+  IN UINT32 Index
+  )
+{
+  UINT32                  CurrentIndex;
+  LIST_ENTRY              *Entry;
+  ISCSI_CONFIG_FORM_ENTRY *ConfigFormEntry;
+
+  CurrentIndex    = 0;
+  ConfigFormEntry = NULL;
+
+  NET_LIST_FOR_EACH (Entry, &mIScsiConfigFormList) {
+    if (CurrentIndex == Index) {
+      ConfigFormEntry = NET_LIST_USER_STRUCT (Entry, ISCSI_CONFIG_FORM_ENTRY, Link);
+      break;
+    }
+
+    CurrentIndex++;
+  }
+
+  return ConfigFormEntry;
+}
+
+/**
+  Convert the iSCSI configuration data into the IFR data.
+
+  @param[in]   ConfigFormEntry The iSCSI configuration form entry.
+  @param[out]  IfrNvData       The IFR nv data.
+
+**/
+VOID
+IScsiConvertDeviceConfigDataToIfrNvData (
+  IN ISCSI_CONFIG_FORM_ENTRY      *ConfigFormEntry,
+  OUT ISCSI_CONFIG_IFR_NVDATA     *IfrNvData
+  )
+{
+  ISCSI_SESSION_CONFIG_NVDATA   *SessionConfigData;
+  ISCSI_CHAP_AUTH_CONFIG_NVDATA *AuthConfigData;
+
+  //
+  // Normal session configuration parameters.
+  //
+  SessionConfigData                 = &ConfigFormEntry->SessionConfigData;
+  IfrNvData->Enabled                = SessionConfigData->Enabled;
+
+  IfrNvData->InitiatorInfoFromDhcp  = SessionConfigData->InitiatorInfoFromDhcp;
+  IfrNvData->TargetInfoFromDhcp     = SessionConfigData->TargetInfoFromDhcp;
+  IfrNvData->TargetPort             = SessionConfigData->TargetPort;
+
+  IScsiIpToStr (&SessionConfigData->LocalIp, IfrNvData->LocalIp);
+  IScsiIpToStr (&SessionConfigData->SubnetMask, IfrNvData->SubnetMask);
+  IScsiIpToStr (&SessionConfigData->Gateway, IfrNvData->Gateway);
+  IScsiIpToStr (&SessionConfigData->TargetIp, IfrNvData->TargetIp);
+
+  IScsiAsciiStrToUnicodeStr (SessionConfigData->TargetName, IfrNvData->TargetName);
+
+  IScsiLunToUnicodeStr (SessionConfigData->BootLun, IfrNvData->BootLun);
+
+  IScsiConvertIsIdToString (IfrNvData->IsId, SessionConfigData->IsId);
+
+  //
+  // CHAP authentication parameters.
+  //
+  AuthConfigData      = &ConfigFormEntry->AuthConfigData;
+
+  IfrNvData->CHAPType = AuthConfigData->CHAPType;
+
+  IScsiAsciiStrToUnicodeStr (AuthConfigData->CHAPName, IfrNvData->CHAPName);
+  IScsiAsciiStrToUnicodeStr (AuthConfigData->CHAPSecret, IfrNvData->CHAPSecret);
+  IScsiAsciiStrToUnicodeStr (AuthConfigData->ReverseCHAPName, IfrNvData->ReverseCHAPName);
+  IScsiAsciiStrToUnicodeStr (AuthConfigData->ReverseCHAPSecret, IfrNvData->ReverseCHAPSecret);
+}
+
+/**
+  This function allows the caller to request the current
+  configuration for one or more named elements. The resulting
+  string is in <ConfigAltResp> format. Any and all alternative
+  configuration strings shall also be appended to the end of the
+  current configuration string. If they are, they must appear
+  after the current configuration. They must contain the same
+  routing (GUID, NAME, PATH) as the current configuration string.
+  They must have an additional description indicating the type of
+  alternative configuration the string represents,
+  "ALTCFG=<StringToken>". That <StringToken> (when
+  converted from Hex UNICODE to binary) is a reference to a
+  string in the associated string pack.
+
+  @param[in] This       Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in] Request    A null-terminated Unicode string in
+                        <ConfigRequest> format. Note that this
+                        includes the routing information as well as
+                        the configurable name / value pairs. It is
+                        invalid for this string to be in
+                        <MultiConfigRequest> format.
+  @param[out] Progress  On return, points to a character in the
+                        Request string. Points to the string's null
+                        terminator if request was successful. Points
+                        to the most recent "&" before the first
+                        failing name / value pair (or the beginning
+                        of the string if the failure is in the first
+                        name / value pair) if the request was not
+                        successful.
+  @param[out] Results   A null-terminated Unicode string in
+                        <ConfigAltResp> format which has all values
+                        filled in for the names in the Request string.
+                        String to be allocated by the called function.
+
+  @retval EFI_SUCCESS             The Results string is filled with the
+                                  values corresponding to all requested
+                                  names.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETER   For example, passing in a NULL
+                                  for the Request parameter
+                                  would result in this type of
+                                  error. In this case, the
+                                  Progress parameter would be
+                                  set to NULL. 
+  @retval EFI_NOT_FOUND           Routing data doesn't match any
+                                  known driver. Progress set to the
+                                  first character in the routing header.
+                                  Note: There is no requirement that the
+                                  driver validate the routing data. It
+                                  must skip the <ConfigHdr> in order to
+                                  process the names.
+  @retval EFI_INVALID_PARAMETER   Illegal syntax. Progress set
+                                  to most recent & before the
+                                  error or the beginning of the
+                                  string.
+  @retval EFI_INVALID_PARAMETER   Unknown name. Progress points
+                                  to the & before the name in
+                                  question.Currently not implemented.
+**/
+EFI_STATUS
+EFIAPI
+IScsiFormExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR8                            InitiatorName[ISCSI_NAME_MAX_SIZE];
+  UINTN                            BufferSize;
+  ISCSI_CONFIG_IFR_NVDATA          *IfrNvData;
+  ISCSI_FORM_CALLBACK_INFO         *Private;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *HiiConfigRouting;
+  EFI_STRING                       ConfigRequestHdr;
+  EFI_STRING                       ConfigRequest;
+  BOOLEAN                          AllocatedRequest;
+  UINTN                            Size;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Request;
+  if ((Request != NULL) && !HiiIsConfigHdrMatch (Request, &gIp4IScsiConfigGuid, mVendorStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  ConfigRequestHdr = NULL;
+  ConfigRequest    = NULL;
+  AllocatedRequest = FALSE;
+  Size             = 0;
+
+  if (!mIScsiDeviceListUpdated) {
+    //
+    // Update the device list.
+    //
+    IScsiUpdateDeviceList ();
+    mIScsiDeviceListUpdated = TRUE;
+  }
+
+  Private = ISCSI_FORM_CALLBACK_INFO_FROM_FORM_CALLBACK (This);
+  IfrNvData = AllocateZeroPool (sizeof (ISCSI_CONFIG_IFR_NVDATA));
+  ASSERT (IfrNvData != NULL);
+  if (Private->Current != NULL) {
+    IScsiConvertDeviceConfigDataToIfrNvData (Private->Current, IfrNvData);
+  }
+
+  BufferSize  = ISCSI_NAME_MAX_SIZE;
+  Status      = gIScsiInitiatorName.Get (&gIScsiInitiatorName, &BufferSize, InitiatorName);
+  if (EFI_ERROR (Status)) {
+    IfrNvData->InitiatorName[0] = L'\0';
+  } else {
+    IScsiAsciiStrToUnicodeStr (InitiatorName, IfrNvData->InitiatorName);
+  }
+
+  //
+  // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+  //
+  HiiConfigRouting = Private->ConfigRouting;
+  BufferSize = sizeof (ISCSI_CONFIG_IFR_NVDATA);
+  ConfigRequest = Request;
+  if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+    //
+    // Request has no request element, construct full request string.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&gIp4IScsiConfigGuid, mVendorStorageName, Private->DriverHandle);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+  }
+  Status = HiiConfigRouting->BlockToConfig (
+                               HiiConfigRouting,
+                               ConfigRequest,
+                               (UINT8 *) IfrNvData,
+                               BufferSize,
+                               Results,
+                               Progress
+                               );
+  FreePool (IfrNvData);
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+  }
+
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This           Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration  A null-terminated Unicode string in
+                             <ConfigString> format.   
+  @param[out] Progress       A pointer to a string filled in with the
+                             offset of the most recent '&' before the
+                             first failing name / value pair (or the
+                             beginn ing of the string if the failure
+                             is in the first name / value pair) or
+                             the terminating NULL if all was
+                             successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are
+                                  awaiting distribution.  
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETERS  Passing in a NULL for the
+                                  Results parameter would result
+                                  in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data
+                                  was not found.
+**/
+EFI_STATUS
+EFIAPI
+IScsiFormRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check routing data in <ConfigHdr>.
+  // Note: if only one Storage is used, then this checking could be skipped.
+  //
+  if (!HiiIsConfigHdrMatch (Configuration, &gIp4IScsiConfigGuid, mVendorStorageName)) {
+    *Progress = Configuration;
+    return EFI_NOT_FOUND;
+  }
+
+  *Progress = Configuration + StrLen (Configuration);
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is called to provide results data to the driver.
+  This data consists of a unique key that is used to identify
+  which data is either being passed back or being asked for.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Action             Specifies the type of action taken by the browser.
+  @param[in]  QuestionId         A unique value which is sent to the original
+                                 exporting driver so that it can identify the type
+                                 of data to expect. The format of the data tends to 
+                                 vary based on the opcode that enerated the callback.
+  @param[in]  Type               The type of value for the question.
+  @param[in]  Value              A pointer to the data being sent to the original
+                                 exporting driver.
+  @param[out]  ActionRequest     On return, points to the action requested by the
+                                 callback function.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the
+                                 variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved.
+  @retval EFI_UNSUPPORTED        The specified Action is not supported by the
+                                 callback.Currently not implemented.
+  @retval EFI_INVALID_PARAMETERS Passing in wrong parameter. 
+  @retval Others                 Other errors as indicated. 
+**/
+EFI_STATUS
+EFIAPI
+IScsiFormCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  ISCSI_FORM_CALLBACK_INFO  *Private;
+  UINTN                     BufferSize;
+  CHAR8                     IScsiName[ISCSI_NAME_MAX_SIZE];
+  CHAR16                    PortString[128];
+  CHAR8                     Ip4String[IP4_STR_MAX_SIZE];
+  CHAR8                     LunString[ISCSI_LUN_STR_MAX_LEN];
+  UINT64                    Lun;
+  EFI_STRING_ID             DeviceFormTitleToken;
+  ISCSI_CONFIG_IFR_NVDATA   *IfrNvData;
+  ISCSI_CONFIG_FORM_ENTRY   *ConfigFormEntry;
+  EFI_IP_ADDRESS            HostIp;
+  EFI_IP_ADDRESS            SubnetMask;
+  EFI_IP_ADDRESS            Gateway;
+  EFI_STATUS                Status;
+  EFI_INPUT_KEY             Key;
+
+  if (Action != EFI_BROWSER_ACTION_CHANGING && Action != EFI_BROWSER_ACTION_CHANGED) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Private   = ISCSI_FORM_CALLBACK_INFO_FROM_FORM_CALLBACK (This);
+  //
+  // Retrive uncommitted data from Browser
+  //
+  IfrNvData = AllocateZeroPool (sizeof (ISCSI_CONFIG_IFR_NVDATA));
+  ASSERT (IfrNvData != NULL);
+  if (!HiiGetBrowserData (&gIp4IScsiConfigGuid, mVendorStorageName, sizeof (ISCSI_CONFIG_IFR_NVDATA), (UINT8 *) IfrNvData)) {
+    FreePool (IfrNvData);
+    return EFI_NOT_FOUND;
+  }
+  Status = EFI_SUCCESS;
+
+  if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if ((QuestionId >= KEY_DEVICE_ENTRY_BASE) && (QuestionId < (mNumberOfIScsiDevices + KEY_DEVICE_ENTRY_BASE))) {
+      //
+      // In case goto the device configuration form, update the device form title.
+      //
+      ConfigFormEntry = IScsiGetConfigFormEntryByIndex ((UINT32) (QuestionId - KEY_DEVICE_ENTRY_BASE));
+      ASSERT (ConfigFormEntry != NULL);
+
+      UnicodeSPrint (PortString, (UINTN) sizeof (PortString), L"Port %s", ConfigFormEntry->MacString);
+      DeviceFormTitleToken = (EFI_STRING_ID) STR_ISCSI_DEVICE_FORM_TITLE;
+      HiiSetString (Private->RegisteredHandle, DeviceFormTitleToken, PortString, NULL);
+
+      IScsiConvertDeviceConfigDataToIfrNvData (ConfigFormEntry, IfrNvData);
+
+      Private->Current = ConfigFormEntry;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    switch (QuestionId) { 
+    case KEY_INITIATOR_NAME:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->InitiatorName, IScsiName);
+      BufferSize  = AsciiStrSize (IScsiName);
+
+      Status      = gIScsiInitiatorName.Set (&gIScsiInitiatorName, &BufferSize, IScsiName);
+      if (EFI_ERROR (Status)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid iSCSI Name!", NULL);
+      }
+
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_APPLY;
+      break;
+
+    case KEY_LOCAL_IP:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->LocalIp, Ip4String);
+      Status = IScsiAsciiStrToIp (Ip4String, &HostIp.v4);
+      if (EFI_ERROR (Status) || !NetIp4IsUnicast (NTOHL (HostIp.Addr[0]), 0)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid IP address!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      } else {
+        CopyMem (&Private->Current->SessionConfigData.LocalIp, &HostIp.v4, sizeof (HostIp.v4));
+      }
+
+      break;
+
+    case KEY_SUBNET_MASK:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->SubnetMask, Ip4String);
+      Status = IScsiAsciiStrToIp (Ip4String, &SubnetMask.v4);
+      if (EFI_ERROR (Status) || ((SubnetMask.Addr[0] != 0) && (IScsiGetSubnetMaskPrefixLength (&SubnetMask.v4) == 0))) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Subnet Mask!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      } else {
+        CopyMem (&Private->Current->SessionConfigData.SubnetMask, &SubnetMask.v4, sizeof (SubnetMask.v4));
+      }
+
+      break;
+
+    case KEY_GATE_WAY:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->Gateway, Ip4String);
+      Status = IScsiAsciiStrToIp (Ip4String, &Gateway.v4);
+      if (EFI_ERROR (Status) || ((Gateway.Addr[0] != 0) && !NetIp4IsUnicast (NTOHL (Gateway.Addr[0]), 0))) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Gateway!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      } else {
+        CopyMem (&Private->Current->SessionConfigData.Gateway, &Gateway.v4, sizeof (Gateway.v4));
+      }
+
+      break;
+
+    case KEY_TARGET_IP:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->TargetIp, Ip4String);
+      Status = IScsiAsciiStrToIp (Ip4String, &HostIp.v4);
+      if (EFI_ERROR (Status) || !NetIp4IsUnicast (NTOHL (HostIp.Addr[0]), 0)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid IP address!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      } else {
+        CopyMem (&Private->Current->SessionConfigData.TargetIp, &HostIp.v4, sizeof (HostIp.v4));
+      }
+
+      break;
+
+    case KEY_TARGET_NAME:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->TargetName, IScsiName);
+      Status = IScsiNormalizeName (IScsiName, AsciiStrLen (IScsiName));
+      if (EFI_ERROR (Status)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid iSCSI Name!", NULL);
+      } else {
+        AsciiStrCpyS (Private->Current->SessionConfigData.TargetName, ISCSI_NAME_MAX_SIZE, IScsiName);
+      }
+
+      break;
+
+    case KEY_DHCP_ENABLE:
+      if (IfrNvData->InitiatorInfoFromDhcp == 0) {
+        IfrNvData->TargetInfoFromDhcp = 0;
+      }
+
+      break;
+
+    case KEY_BOOT_LUN:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->BootLun, LunString);
+      Status = IScsiAsciiStrToLun (LunString, (UINT8 *) &Lun);
+      if (EFI_ERROR (Status)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid LUN string!", NULL);
+      } else {
+        CopyMem (Private->Current->SessionConfigData.BootLun, &Lun, sizeof (Lun));
+      }
+
+      break;
+
+    case KEY_CHAP_NAME:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->CHAPName, Private->Current->AuthConfigData.CHAPName);
+      break;
+
+    case KEY_CHAP_SECRET:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->CHAPSecret, Private->Current->AuthConfigData.CHAPSecret);
+      break;
+
+    case KEY_REVERSE_CHAP_NAME:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->ReverseCHAPName, Private->Current->AuthConfigData.ReverseCHAPName);
+      break;
+
+    case KEY_REVERSE_CHAP_SECRET:
+      IScsiUnicodeStrToAsciiStr (IfrNvData->ReverseCHAPSecret, Private->Current->AuthConfigData.ReverseCHAPSecret);
+      break;
+
+    case KEY_CONFIG_ISID:
+      IScsiParseIsIdFromString (IfrNvData->IsId, Private->Current->SessionConfigData.IsId);
+      IScsiConvertIsIdToString (IfrNvData->IsId, Private->Current->SessionConfigData.IsId);
+
+      break;
+
+    case KEY_SAVE_CHANGES:
+      //
+      // First, update those fields which don't have INTERACTIVE set.
+      //
+      Private->Current->SessionConfigData.Enabled               = IfrNvData->Enabled;
+      Private->Current->SessionConfigData.InitiatorInfoFromDhcp = IfrNvData->InitiatorInfoFromDhcp;
+      Private->Current->SessionConfigData.TargetPort            = IfrNvData->TargetPort;
+      if (Private->Current->SessionConfigData.TargetPort == 0) {
+        Private->Current->SessionConfigData.TargetPort = ISCSI_WELL_KNOWN_PORT;
+      }
+
+      Private->Current->SessionConfigData.TargetInfoFromDhcp  = IfrNvData->TargetInfoFromDhcp;
+      Private->Current->AuthConfigData.CHAPType               = IfrNvData->CHAPType;
+
+      //
+      // Only do full parameter validation if iSCSI is enabled on this device.
+      //
+      if (Private->Current->SessionConfigData.Enabled) {
+        //
+        // Validate the address configuration of the Initiator if DHCP isn't
+        // deployed.
+        //
+        if (!Private->Current->SessionConfigData.InitiatorInfoFromDhcp) {
+          CopyMem (&HostIp.v4, &Private->Current->SessionConfigData.LocalIp, sizeof (HostIp.v4));
+          CopyMem (&SubnetMask.v4, &Private->Current->SessionConfigData.SubnetMask, sizeof (SubnetMask.v4));
+          CopyMem (&Gateway.v4, &Private->Current->SessionConfigData.Gateway, sizeof (Gateway.v4));
+
+          if ((Gateway.Addr[0] != 0)) {
+            if (SubnetMask.Addr[0] == 0) {
+              CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Gateway address is set but subnet mask is zero.", NULL);
+              Status = EFI_INVALID_PARAMETER;
+              break;
+            } else if (!IP4_NET_EQUAL (HostIp.Addr[0], Gateway.Addr[0], SubnetMask.Addr[0])) {
+              CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Local IP and Gateway are not in the same subnet.", NULL);
+              Status = EFI_INVALID_PARAMETER;
+              break;
+            }
+          }
+        }
+        //
+        // Validate target configuration if DHCP isn't deployed.
+        //
+        if (!Private->Current->SessionConfigData.TargetInfoFromDhcp) {
+          CopyMem (&HostIp.v4, &Private->Current->SessionConfigData.TargetIp, sizeof (HostIp.v4));
+          if (!NetIp4IsUnicast (NTOHL (HostIp.Addr[0]), 0)) {
+            CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Target IP is invalid!", NULL);
+            Status = EFI_INVALID_PARAMETER;
+            break;
+          }
+
+          //
+          // Validate iSCSI target name configuration again:
+          // The format of iSCSI target name is already verified when user input the name;
+          // here we only check the case user does not input the name.
+          //
+          if (Private->Current->SessionConfigData.TargetName[0] == '\0') {
+            CreatePopUp (
+              EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE,
+              &Key,
+              L"iSCSI target name is NULL!",
+              NULL
+              );
+            Status = EFI_INVALID_PARAMETER;
+            break;
+          }
+
+        }
+
+        if (IfrNvData->CHAPType != ISCSI_CHAP_NONE) {
+          if ((IfrNvData->CHAPName[0] == '\0') || (IfrNvData->CHAPSecret[0] == '\0')) {
+            CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"CHAP Name or CHAP Secret is invalid!", NULL);
+            Status = EFI_INVALID_PARAMETER;
+            break;
+          }
+
+          if ((IfrNvData->CHAPType == ISCSI_CHAP_MUTUAL) &&
+              ((IfrNvData->ReverseCHAPName[0] == '\0') || (IfrNvData->ReverseCHAPSecret[0] == '\0'))
+              ) {
+            CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Reverse CHAP Name or Reverse CHAP Secret is invalid!", NULL);
+            Status = EFI_INVALID_PARAMETER;
+            break;
+          }
+        }
+      }
+
+      BufferSize = sizeof (Private->Current->SessionConfigData);
+      gRT->SetVariable (
+            Private->Current->MacString,
+            &gEfiIScsiInitiatorNameProtocolGuid,
+            ISCSI_CONFIG_VAR_ATTR,
+            BufferSize,
+            &Private->Current->SessionConfigData
+            );
+
+      BufferSize = sizeof (Private->Current->AuthConfigData);
+      gRT->SetVariable (
+            Private->Current->MacString,
+            &gIScsiCHAPAuthInfoGuid,
+            ISCSI_CONFIG_VAR_ATTR,
+            BufferSize,
+            &Private->Current->AuthConfigData
+            );
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_APPLY;
+      break;
+
+    default:
+      break;
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Pass changed uncommitted data back to Form Browser
+    //
+    HiiSetBrowserData (&gIp4IScsiConfigGuid, mVendorStorageName, sizeof (ISCSI_CONFIG_IFR_NVDATA), (UINT8 *) IfrNvData, NULL);
+  }
+  
+  FreePool (IfrNvData);
+  
+  return Status;
+}
+
+/**
+  Updates the iSCSI configuration form to add/delete an entry for the iSCSI
+  device specified by the Controller.
+
+  @param[in]  DriverBindingHandle The driverbinding handle.
+  @param[in]  Controller          The controller handle of the iSCSI device.
+  @param[in]  AddForm             Whether to add or delete a form entry.
+
+  @retval EFI_SUCCESS             The iSCSI configuration form is updated.
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate memory.
+  @retval Others                  Other errors as indicated.
+**/
+EFI_STATUS
+IScsiConfigUpdateForm (
+  IN EFI_HANDLE  DriverBindingHandle,
+  IN EFI_HANDLE  Controller,
+  IN BOOLEAN     AddForm
+  )
+{
+  LIST_ENTRY                  *Entry;
+  ISCSI_CONFIG_FORM_ENTRY     *ConfigFormEntry;
+  BOOLEAN                     EntryExisted;
+  EFI_STATUS                  Status;
+  EFI_MAC_ADDRESS             MacAddress;
+  UINTN                       HwAddressSize;
+  UINT16                      VlanId;
+  CHAR16                      PortString[128];
+  UINT16                      FormIndex;
+  UINTN                       BufferSize;
+  VOID                        *StartOpCodeHandle;
+  VOID                        *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL          *StartLabel;
+  EFI_IFR_GUID_LABEL          *EndLabel;
+
+  ConfigFormEntry = NULL;
+  EntryExisted    = FALSE;
+
+  NET_LIST_FOR_EACH (Entry, &mIScsiConfigFormList) {
+    ConfigFormEntry = NET_LIST_USER_STRUCT (Entry, ISCSI_CONFIG_FORM_ENTRY, Link);
+
+    if (ConfigFormEntry->Controller == Controller) {
+      EntryExisted = TRUE;
+      break;
+    }
+  }
+
+  if (AddForm) {
+    if (EntryExisted) {
+      return EFI_SUCCESS;
+    } else {
+      //
+      // Add a new form.
+      //
+      ConfigFormEntry = (ISCSI_CONFIG_FORM_ENTRY *) AllocateZeroPool (sizeof (ISCSI_CONFIG_FORM_ENTRY));
+      if (ConfigFormEntry == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      InitializeListHead (&ConfigFormEntry->Link);
+      ConfigFormEntry->Controller = Controller;
+
+      //
+      // Get the MAC address and convert it into the formatted string.
+      //
+      Status = NetLibGetMacAddress (Controller, &MacAddress, &HwAddressSize);
+      ASSERT (Status == EFI_SUCCESS);
+      VlanId = NetLibGetVlanId (Controller);
+
+      IScsiMacAddrToStr (&MacAddress, (UINT32) HwAddressSize, VlanId, ConfigFormEntry->MacString);
+
+      //
+      // Get the normal session configuration data.
+      //
+      BufferSize = sizeof (ConfigFormEntry->SessionConfigData);
+      Status = gRT->GetVariable (
+                      ConfigFormEntry->MacString,
+                      &gEfiIScsiInitiatorNameProtocolGuid,
+                      NULL,
+                      &BufferSize,
+                      &ConfigFormEntry->SessionConfigData
+                      );
+      if (EFI_ERROR (Status)) {
+        ZeroMem (&ConfigFormEntry->SessionConfigData, sizeof (ConfigFormEntry->SessionConfigData));
+        
+        //
+        // Generate OUI-format ISID based on MAC address.
+        //
+        CopyMem (ConfigFormEntry->SessionConfigData.IsId, &MacAddress, 6);
+        ConfigFormEntry->SessionConfigData.IsId[0] = 
+          (UINT8) (ConfigFormEntry->SessionConfigData.IsId[0] & 0x3F);
+      }
+      //
+      // Get the CHAP authentication configuration data.
+      //
+      BufferSize = sizeof (ConfigFormEntry->AuthConfigData);
+      Status = gRT->GetVariable (
+                      ConfigFormEntry->MacString,
+                      &gIScsiCHAPAuthInfoGuid,
+                      NULL,
+                      &BufferSize,
+                      &ConfigFormEntry->AuthConfigData
+                      );
+      if (EFI_ERROR (Status)) {
+        ZeroMem (&ConfigFormEntry->AuthConfigData, sizeof (ConfigFormEntry->AuthConfigData));
+      }
+      //
+      // Compose the Port string and create a new EFI_STRING_ID.
+      //
+      UnicodeSPrint (PortString, sizeof (PortString), L"Port %s", ConfigFormEntry->MacString);
+      ConfigFormEntry->PortTitleToken = HiiSetString (mCallbackInfo->RegisteredHandle, 0, PortString, NULL);
+
+      //
+      // Compose the help string of this port and create a new EFI_STRING_ID.
+      //
+      UnicodeSPrint (PortString, sizeof (PortString), L"Set the iSCSI parameters on port %s", ConfigFormEntry->MacString);
+      ConfigFormEntry->PortTitleHelpToken = HiiSetString (mCallbackInfo->RegisteredHandle, 0, PortString, NULL);
+
+      InsertTailList (&mIScsiConfigFormList, &ConfigFormEntry->Link);
+      mNumberOfIScsiDevices++;
+    }
+  } else {
+    ASSERT (EntryExisted);
+
+    mNumberOfIScsiDevices--;
+    RemoveEntryList (&ConfigFormEntry->Link);
+    FreePool (ConfigFormEntry);
+  }
+  //
+  // Allocate space for creation of Buffer
+  //
+
+  //
+  // Init OpCode Handle
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = DEVICE_ENTRY_LABEL;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  FormIndex = 0;
+  NET_LIST_FOR_EACH (Entry, &mIScsiConfigFormList) {
+    ConfigFormEntry = NET_LIST_USER_STRUCT (Entry, ISCSI_CONFIG_FORM_ENTRY, Link);
+
+    HiiCreateGotoOpCode (
+      StartOpCodeHandle,                            // Container for dynamic created opcodes
+      FORMID_DEVICE_FORM,                           // Target Form ID
+      ConfigFormEntry->PortTitleToken,              // Prompt text
+      ConfigFormEntry->PortTitleHelpToken,          // Help text
+      EFI_IFR_FLAG_CALLBACK,                        // Question flag
+      (UINT16)(KEY_DEVICE_ENTRY_BASE + FormIndex)   // Question ID
+      );
+
+    FormIndex++;
+  }
+
+  HiiUpdateForm (
+    mCallbackInfo->RegisteredHandle,
+    &gIp4IScsiConfigGuid,
+    FORMID_MAIN_FORM,
+    StartOpCodeHandle, // Label DEVICE_ENTRY_LABEL
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the iSCSI configuration form.
+
+  @param[in]  DriverBindingHandle  The iSCSI driverbinding handle.
+
+  @retval EFI_SUCCESS              The iSCSI configuration form is initialized.
+  @retval EFI_OUT_OF_RESOURCES     Failed to allocate memory.
+  @retval Others                   Other errors as indicated.
+**/
+EFI_STATUS
+IScsiConfigFormInit (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_HII_DATABASE_PROTOCOL   *HiiDatabase;
+  ISCSI_FORM_CALLBACK_INFO    *CallbackInfo;
+
+  Status = gBS->LocateProtocol (&gEfiHiiDatabaseProtocolGuid, NULL, (VOID **)&HiiDatabase);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  CallbackInfo = (ISCSI_FORM_CALLBACK_INFO *) AllocateZeroPool (sizeof (ISCSI_FORM_CALLBACK_INFO));
+  if (CallbackInfo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CallbackInfo->Signature   = ISCSI_FORM_CALLBACK_INFO_SIGNATURE;
+  CallbackInfo->HiiDatabase = HiiDatabase;
+  CallbackInfo->Current     = NULL;
+
+  CallbackInfo->ConfigAccess.ExtractConfig = IScsiFormExtractConfig;
+  CallbackInfo->ConfigAccess.RouteConfig = IScsiFormRouteConfig;
+  CallbackInfo->ConfigAccess.Callback = IScsiFormCallback;
+
+  Status = gBS->LocateProtocol (&gEfiHiiConfigRoutingProtocolGuid, NULL, (VOID **)&CallbackInfo->ConfigRouting);
+  if (EFI_ERROR (Status)) {
+    FreePool(CallbackInfo);
+    return Status;
+  }
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &CallbackInfo->DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mIScsiHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &CallbackInfo->ConfigAccess,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  //
+  // Publish our HII data
+  //
+  CallbackInfo->RegisteredHandle = HiiAddPackages (
+                                     &gIp4IScsiConfigGuid,
+                                     CallbackInfo->DriverHandle,
+                                     IScsi4DxeStrings,
+                                     IScsiConfigDxeBin,
+                                     NULL
+                                     );
+  if (CallbackInfo->RegisteredHandle == NULL) {
+    FreePool(CallbackInfo);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mCallbackInfo = CallbackInfo;
+
+  return Status;
+}
+
+/**
+  Unload the iSCSI configuration form, this includes: delete all the iSCSI
+  device configuration entries, uninstall the form callback protocol and
+  free the resources used.
+
+  @param[in]  DriverBindingHandle The iSCSI driverbinding handle.
+  
+  @retval EFI_SUCCESS             The iSCSI configuration form is unloaded.
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate memory.
+**/
+EFI_STATUS
+IScsiConfigFormUnload (
+  IN EFI_HANDLE  DriverBindingHandle
+  )
+{
+  ISCSI_CONFIG_FORM_ENTRY     *ConfigFormEntry;
+
+  while (!IsListEmpty (&mIScsiConfigFormList)) {
+    //
+    // Uninstall the device forms as the iSCSI driver instance may fail to
+    // control the controller but still install the device configuration form.
+    // In such case, upon driver unloading, the driver instance's driverbinding.
+    // stop () won't be called, so we have to take this chance here to uninstall
+    // the device form.
+    //
+    ConfigFormEntry = NET_LIST_USER_STRUCT (mIScsiConfigFormList.ForwardLink, ISCSI_CONFIG_FORM_ENTRY, Link);
+    IScsiConfigUpdateForm (DriverBindingHandle, ConfigFormEntry->Controller, FALSE);
+  }
+
+  //
+  // Remove HII package list
+  //
+  mCallbackInfo->HiiDatabase->RemovePackageList (
+                                mCallbackInfo->HiiDatabase,
+                                mCallbackInfo->RegisteredHandle
+                                );
+
+  //
+  // Uninstall EFI_HII_CONFIG_ACCESS_PROTOCOL
+  //
+  gBS->UninstallMultipleProtocolInterfaces (
+         mCallbackInfo->DriverHandle,
+         &gEfiDevicePathProtocolGuid,
+         &mIScsiHiiVendorDevicePath,
+         &gEfiHiiConfigAccessProtocolGuid,
+         &mCallbackInfo->ConfigAccess,
+         NULL
+         );
+  FreePool (mCallbackInfo);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.h
new file mode 100644
index 0000000000..ef881d8827
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfig.h
@@ -0,0 +1,166 @@
+/** @file
+  The header file of IScsiConfig.c.
+
+Copyright (c) 2004 - 2014, 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.
+
+**/
+
+#ifndef _ISCSI_CONFIG_H_
+#define _ISCSI_CONFIG_H_
+
+#include <Guid/MdeModuleHii.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Library/HiiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/NetLib.h>
+
+extern UINT8  IScsiConfigDxeBin[];
+extern UINT8  IScsi4DxeStrings[];
+
+#define ISCSI_INITATOR_NAME_VAR_NAME        L"I_NAME"
+
+#define ISCSI_CONFIG_VAR_ATTR               (EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE)
+
+#define ISCSI_FORM_CALLBACK_INFO_SIGNATURE  SIGNATURE_32 ('I', 'f', 'c', 'i')
+
+
+
+/**
+  If the DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED bit of PcdDebugProperyMask is clear, 
+  then this macro return a pointer to a data structure ISCSI_FORM_CALLBACK_INFO.
+
+  If the DEBUG_PROPERTY_DEBUG_ASSERT_ENABLED bit of PcdDebugProperyMask is set,  
+  The Signature field of the data structure ISCSI_FORM_CALLBACK_INFO 
+  is compared to TestSignature.  If the signatures match, then a pointer 
+  to the pointer to a data structure ISCSI_FORM_CALLBACK_INFO is returned.  
+  If the signatures do not match, then DebugAssert() is called with a description 
+  of "CR has a bad signature" and Callback is returned.  
+
+  If the data type ISCSI_FORM_CALLBACK_INFO_SIGNATURE does not contain the field
+  specified by Callback, then the module will not compile.
+
+  If ISCSI_FORM_CALLBACK_INFO_SIGNATURE does not contain a field called Signature, 
+  then the module will not compile.
+
+  @param   Callback      Pointer to the specified field within the data 
+                         structure ISCSI_FORM_CALLBACK_INFO.
+  @return  A pointer to the pointer to a data structure ISCSI_FORM_CALLBACK_INFO.
+  @retval  Others        Some unexpected error happened.
+**/
+
+#define ISCSI_FORM_CALLBACK_INFO_FROM_FORM_CALLBACK(Callback) \
+  CR ( \
+  Callback, \
+  ISCSI_FORM_CALLBACK_INFO, \
+  ConfigAccess, \
+  ISCSI_FORM_CALLBACK_INFO_SIGNATURE \
+  )
+
+#pragma pack(1)
+
+typedef struct _ISCSI_MAC_INFO {
+  EFI_MAC_ADDRESS Mac;
+  UINT8           Len;
+  UINT16          VlanId;
+} ISCSI_MAC_INFO;
+
+typedef struct _ISCSI_DEVICE_LIST {
+  UINT8           NumDevice;
+  ISCSI_MAC_INFO  MacInfo[1];
+} ISCSI_DEVICE_LIST;
+
+#pragma pack()
+
+typedef struct _ISCSI_CONFIG_FORM_ENTRY {
+  LIST_ENTRY                    Link;
+  EFI_HANDLE                    Controller;
+  CHAR16                        MacString[95];
+  EFI_STRING_ID                 PortTitleToken;
+  EFI_STRING_ID                 PortTitleHelpToken;
+
+  ISCSI_SESSION_CONFIG_NVDATA   SessionConfigData;
+  ISCSI_CHAP_AUTH_CONFIG_NVDATA AuthConfigData;
+} ISCSI_CONFIG_FORM_ENTRY;
+
+typedef struct _ISCSI_FORM_CALLBACK_INFO {
+  UINTN                            Signature;
+  EFI_HANDLE                       DriverHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL   ConfigAccess;
+  EFI_HII_DATABASE_PROTOCOL        *HiiDatabase;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *ConfigRouting;
+  UINT16                           *KeyList;
+  VOID                             *FormBuffer;
+  EFI_HII_HANDLE                   RegisteredHandle;
+  ISCSI_CONFIG_FORM_ENTRY          *Current;
+} ISCSI_FORM_CALLBACK_INFO;
+
+#pragma pack(1)
+
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} HII_VENDOR_DEVICE_PATH;
+
+#pragma pack()
+
+/**
+  Updates the iSCSI configuration form to add/delete an entry for the iSCSI
+  device specified by the Controller.
+
+  @param[in]  DriverBindingHandle The driverbinding handle.
+  @param[in]  Controller          The controller handle of the iSCSI device.
+  @param[in]  AddForm             Whether to add or delete a form entry.
+
+  @retval EFI_SUCCESS             The iSCSI configuration form is updated.
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate memory.
+  @retval Others                  Other errors as indicated.
+**/
+EFI_STATUS
+IScsiConfigUpdateForm (
+  IN EFI_HANDLE  DriverBindingHandle,
+  IN EFI_HANDLE  Controller,
+  IN BOOLEAN     AddForm
+  );
+
+/**
+  Initialize the iSCSI configuration form.
+
+  @param[in]  DriverBindingHandle  The iSCSI driverbinding handle.
+
+  @retval EFI_SUCCESS              The iSCSI configuration form is initialized.
+  @retval EFI_OUT_OF_RESOURCES     Failed to allocate memory.
+  @retval Others                   Other errors as indicated.
+**/
+EFI_STATUS
+IScsiConfigFormInit (
+  VOID
+  );
+
+/**
+  Unload the iSCSI configuration form, this includes: delete all the iSCSI
+  device configuration entries, uninstall the form callback protocol and
+  free the resources used.
+
+  @param[in]  DriverBindingHandle The iSCSI driverbinding handle.
+  
+  @retval EFI_SUCCESS             The iSCSI configuration form is unloaded.
+  @retval EFI_OUT_OF_RESOURCES    Failed to allocate memory.
+**/
+EFI_STATUS
+IScsiConfigFormUnload (
+  IN EFI_HANDLE  DriverBindingHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxe.vfr b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxe.vfr
new file mode 100644
index 0000000000..2456c51c8c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxe.vfr
@@ -0,0 +1,219 @@
+/** @file
+  Vfr file for iSCSI config.
+  
+Copyright (c) 2004 - 2011, 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 "IScsiConfigNVDataStruc.h"
+#define EFI_NETWORK_DEVICE_CLASS  0x04
+
+formset 
+  guid     = IP4_ISCSI_CONFIG_GUID,
+  title    = STRING_TOKEN(STR_ISCSI_CONFIG_FORM_TITLE),
+  help     = STRING_TOKEN(STR_ISCSI_CONFIG_FORM_HELP),
+
+  varstore ISCSI_CONFIG_IFR_NVDATA,
+    name = ISCSI_CONFIG_IFR_NVDATA,
+    guid = IP4_ISCSI_CONFIG_GUID;
+  form formid = FORMID_MAIN_FORM,
+    title  = STRING_TOKEN(STR_ISCSI_MAIN_FORM_TITLE);
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.InitiatorName, 
+            prompt  = STRING_TOKEN(STR_ISCSI_CONFIG_INIT_NAME),
+            help    = STRING_TOKEN(STR_ISCSI_CONFIG_INIT_NAME_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_INITIATOR_NAME,
+            minsize = ISCSI_NAME_IFR_MIN_SIZE,
+            maxsize = ISCSI_NAME_IFR_MAX_SIZE,
+    endstring;
+
+    label DEVICE_ENTRY_LABEL;
+    label LABEL_END;
+
+  endform;
+
+  form formid = FORMID_DEVICE_FORM,
+    title  = STRING_TOKEN(STR_ISCSI_DEVICE_FORM_TITLE);
+
+    checkbox varid = ISCSI_CONFIG_IFR_NVDATA.Enabled,
+            prompt = STRING_TOKEN(STR_ISCSI_DEVICE_ENABLE),
+            help   = STRING_TOKEN(STR_NULL),
+            flags  = 0,
+    endcheckbox;
+
+    checkbox varid = ISCSI_CONFIG_IFR_NVDATA.InitiatorInfoFromDhcp,
+            prompt = STRING_TOKEN(STR_ISCSI_ENABLE_DHCP),
+            help   = STRING_TOKEN(STR_ISCSI_ENABLE_DHCP),
+            flags  = INTERACTIVE,
+            key    = KEY_DHCP_ENABLE,
+    endcheckbox;
+
+    suppressif ideqval ISCSI_CONFIG_IFR_NVDATA.InitiatorInfoFromDhcp == 0x01; 
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.LocalIp, 
+            prompt  = STRING_TOKEN(STR_ISCSI_LOCAL_IP_ADDRESS),
+            help    = STRING_TOKEN(STR_ISCSI_IP_ADDRESS_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_LOCAL_IP,
+            minsize = IP_MIN_SIZE,
+            maxsize = IP_MAX_SIZE,
+    endstring;
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.SubnetMask, 
+            prompt  = STRING_TOKEN(STR_ISCSI_LOCAL_MASK),
+            help    = STRING_TOKEN(STR_ISCSI_IP_ADDRESS_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_SUBNET_MASK,
+            minsize = IP_MIN_SIZE,
+            maxsize = IP_MAX_SIZE,
+    endstring;
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.Gateway, 
+            prompt  = STRING_TOKEN(STR_ISCSI_LOCAL_GATEWAY),
+            help    = STRING_TOKEN(STR_ISCSI_IP_ADDRESS_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_GATE_WAY,
+            minsize = IP_MIN_SIZE,
+            maxsize = IP_MAX_SIZE,
+    endstring;
+    endif;
+
+    subtitle text = STRING_TOKEN(STR_NULL); 
+
+    suppressif ideqval ISCSI_CONFIG_IFR_NVDATA.InitiatorInfoFromDhcp == 0x00; 
+    checkbox varid  = ISCSI_CONFIG_IFR_NVDATA.TargetInfoFromDhcp,
+             prompt = STRING_TOKEN(STR_ISCSI_ENABLE_DHCP_ON_TARGET),
+             help   = STRING_TOKEN(STR_ISCSI_ENABLE_DHCP_ON_TARGET),
+             flags  = 0,
+    endcheckbox;
+    endif;
+      
+    suppressif ideqval ISCSI_CONFIG_IFR_NVDATA.TargetInfoFromDhcp == 0x01; 
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.TargetName, 
+            prompt  = STRING_TOKEN(STR_ISCSI_TARGET_NAME),
+            help    = STRING_TOKEN(STR_ISCSI_TARGET_NAME),
+            flags   = INTERACTIVE,
+            key     = KEY_TARGET_NAME,
+            minsize = ISCSI_NAME_IFR_MIN_SIZE,
+            maxsize = ISCSI_NAME_IFR_MAX_SIZE,
+    endstring;
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.TargetIp, 
+            prompt  = STRING_TOKEN(STR_ISCSI_TARGET_IP_ADDRESS),
+            help    = STRING_TOKEN(STR_ISCSI_IP_ADDRESS_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_TARGET_IP,
+            minsize = IP_MIN_SIZE,
+            maxsize = IP_MAX_SIZE,
+    endstring;
+    
+    numeric varid   = ISCSI_CONFIG_IFR_NVDATA.TargetPort, 
+            prompt  = STRING_TOKEN(STR_ISCSI_TARGET_PORT),
+            help    = STRING_TOKEN(STR_ISCSI_TARGET_PORT),
+            flags   = 0,
+            minimum = TARGET_PORT_MIN_NUM,
+            maximum = TARGET_PORT_MAX_NUM,     
+            step    = 0,
+    endnumeric;
+    
+    string varid    = ISCSI_CONFIG_IFR_NVDATA.BootLun, 
+            prompt  = STRING_TOKEN(STR_ISCSI_BOOT_LUN),
+            help    = STRING_TOKEN(STR_ISCSI_BOOT_LUN_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_BOOT_LUN,
+            minsize = LUN_MIN_SIZE,
+            maxsize = LUN_MAX_SIZE,
+    endstring;
+    endif;
+
+    subtitle text = STRING_TOKEN(STR_NULL); 
+
+    oneof varid  = ISCSI_CONFIG_IFR_NVDATA.CHAPType,
+          prompt = STRING_TOKEN(STR_CHAP_TYPE_PROMPT),
+          help   = STRING_TOKEN(STR_CHAP_TYPE_HELP),  
+          option text = STRING_TOKEN(STR_CHAP_TYPE_NONE),   value = ISCSI_CHAP_NONE,   flags = DEFAULT;
+          option text = STRING_TOKEN(STR_CHAP_TYPE_UNI),    value = ISCSI_CHAP_UNI,    flags = 0;
+          option text = STRING_TOKEN(STR_CHAP_TYPE_MUTUAL), value = ISCSI_CHAP_MUTUAL, flags = 0;
+    endoneof;
+
+    suppressif ideqval ISCSI_CONFIG_IFR_NVDATA.CHAPType == ISCSI_CHAP_NONE; 
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.CHAPName, 
+            prompt  = STRING_TOKEN(STR_ISCSI_CHAP_NAME),
+            help    = STRING_TOKEN(STR_ISCSI_CHAP_NAME),
+            flags   = INTERACTIVE,
+            key     = KEY_CHAP_NAME,
+            minsize = 0,
+            maxsize = ISCSI_CHAP_NAME_MAX_LEN,
+    endstring;
+
+    string  varid    = ISCSI_CONFIG_IFR_NVDATA.CHAPSecret, 
+            prompt   = STRING_TOKEN(STR_ISCSI_CHAP_SECRET),
+            help     = STRING_TOKEN(STR_ISCSI_CHAP_SECRET_HELP),
+            flags    = INTERACTIVE,
+            key      = KEY_CHAP_SECRET,
+            minsize  = ISCSI_CHAP_SECRET_MIN_LEN,
+            maxsize  = ISCSI_CHAP_SECRET_MAX_LEN,
+    endstring;
+
+    endif;
+
+    suppressif NOT ideqval ISCSI_CONFIG_IFR_NVDATA.CHAPType == ISCSI_CHAP_MUTUAL;
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.ReverseCHAPName, 
+            prompt  = STRING_TOKEN(STR_ISCSI_REVERSE_CHAP_NAME),
+            help    = STRING_TOKEN(STR_ISCSI_REVERSE_CHAP_NAME),
+            flags   = INTERACTIVE,
+            key     = KEY_REVERSE_CHAP_NAME,
+            minsize = 0,
+            maxsize = ISCSI_CHAP_NAME_MAX_LEN,
+    endstring;
+
+    string  varid    = ISCSI_CONFIG_IFR_NVDATA.ReverseCHAPSecret, 
+            prompt   = STRING_TOKEN(STR_ISCSI_REVERSE_CHAP_SECRET),
+            help     = STRING_TOKEN(STR_ISCSI_CHAP_SECRET_HELP),
+            flags    = INTERACTIVE,
+            key      = KEY_REVERSE_CHAP_SECRET,
+            minsize  = ISCSI_CHAP_SECRET_MIN_LEN,
+            maxsize  = ISCSI_CHAP_SECRET_MAX_LEN,
+    endstring;
+
+    endif;
+
+    subtitle text = STRING_TOKEN(STR_NULL); 
+
+    string  varid   = ISCSI_CONFIG_IFR_NVDATA.IsId, 
+            prompt  = STRING_TOKEN(STR_ISCSI_CONFIG_ISID),
+            help    = STRING_TOKEN(STR_ISCSI_CONFIG_ISID_HELP),
+            flags   = INTERACTIVE,
+            key     = KEY_CONFIG_ISID,
+            minsize = ISID_CONFIGURABLE_MIN_LEN,
+            maxsize = ISID_CONFIGURABLE_MAX_LEN,
+    endstring;
+
+    subtitle text = STRING_TOKEN(STR_NULL); 
+
+    text
+      help   = STRING_TOKEN (STR_SAVE_CHANGES),
+      text   = STRING_TOKEN (STR_SAVE_CHANGES),
+      flags  = INTERACTIVE,
+      key    = KEY_SAVE_CHANGES;
+      
+    goto FORMID_MAIN_FORM, 
+    prompt = STRING_TOKEN (STR_RETURN_MAIN_FORM), 
+    help   = STRING_TOKEN (STR_RETURN_MAIN_FORM),
+    flags  = 0;
+
+  endform;
+
+endformset;
+
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxeStrings.uni b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxeStrings.uni
new file mode 100644
index 0000000000..140f2425a2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigDxeStrings.uni
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigNVDataStruc.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigNVDataStruc.h
new file mode 100644
index 0000000000..7c2e4e65fa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiConfigNVDataStruc.h
@@ -0,0 +1,109 @@
+/** @file
+  Define NVData structures used by the iSCSI configuration component
+
+Copyright (c) 2004 - 2011, 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.
+
+**/
+
+#ifndef _ISCSI_NVDATASTRUC_H_
+#define _ISCSI_NVDATASTRUC_H_
+
+#include <Guid/Ip4IScsiConfigHii.h>
+
+#define VAR_EQ_TEST_NAME    0x100
+
+#define FORMID_MAIN_FORM    1
+#define FORMID_DEVICE_FORM  2
+
+#define ISCSI_NAME_MAX_SIZE 224
+
+//
+// Vfr has a limit on the size, it's 255 bytes.
+//
+#define ISCSI_NAME_IFR_MIN_SIZE   4
+#define ISCSI_NAME_IFR_MAX_SIZE   223
+
+#define IP_MIN_SIZE               7
+#define IP_MAX_SIZE               15
+#define IP4_STR_MAX_SIZE          16
+
+#define LUN_MIN_SIZE              1
+#define LUN_MAX_SIZE              20
+
+#define ISCSI_CHAP_NONE           0
+#define ISCSI_CHAP_UNI            1
+#define ISCSI_CHAP_MUTUAL         2
+
+#define TARGET_PORT_MIN_NUM       0
+#define TARGET_PORT_MAX_NUM       65535
+
+#define DEVICE_ENTRY_LABEL        0x1234
+#define LABEL_END                 0xffff
+
+#define KEY_INITIATOR_NAME        0x101
+#define KEY_DHCP_ENABLE           0x102
+#define KEY_LOCAL_IP              0x103
+#define KEY_SUBNET_MASK           0x104
+#define KEY_GATE_WAY              0x105
+#define KEY_TARGET_IP             0x106
+#define KEY_CHAP_NAME             0x107
+#define KEY_CHAP_SECRET           0x108
+#define KEY_REVERSE_CHAP_NAME     0x109
+#define KEY_REVERSE_CHAP_SECRET   0x10a
+#define KEY_SAVE_CHANGES          0x10b
+#define KEY_TARGET_NAME           0x10c
+#define KEY_BOOT_LUN              0x10d
+#define KEY_CONFIG_ISID           0x10e
+
+#define KEY_DEVICE_ENTRY_BASE     0x1000
+
+#define ISCSI_LUN_STR_MAX_LEN     21
+#define ISCSI_CHAP_SECRET_MIN_LEN 12
+#define ISCSI_CHAP_SECRET_MAX_LEN 16
+//
+//  ISCSI_CHAP_SECRET_STORAGE = ISCSI_CHAP_SECRET_MAX_LEN + sizeof (NULL-Terminator)
+//
+#define ISCSI_CHAP_SECRET_STORAGE 17
+
+#define ISCSI_CHAP_NAME_MAX_LEN   126
+#define ISCSI_CHAP_NAME_STORAGE   127
+
+#define ISID_CONFIGURABLE_MIN_LEN 6
+#define ISID_CONFIGURABLE_MAX_LEN 12
+#define ISID_CONFIGURABLE_STORAGE 13
+
+#pragma pack(1)
+typedef struct {
+  CHAR16  InitiatorName[ISCSI_NAME_MAX_SIZE];
+
+  UINT8   Enabled;
+
+  UINT8   InitiatorInfoFromDhcp;
+  CHAR16  LocalIp[IP4_STR_MAX_SIZE];
+  CHAR16  SubnetMask[IP4_STR_MAX_SIZE];
+  CHAR16  Gateway[IP4_STR_MAX_SIZE];
+
+  CHAR16  TargetName[ISCSI_NAME_MAX_SIZE];
+  CHAR16  TargetIp[IP4_STR_MAX_SIZE];
+  UINT16  TargetPort;
+  CHAR16  BootLun[ISCSI_LUN_STR_MAX_LEN];
+  UINT8   TargetInfoFromDhcp;
+
+  UINT8   CHAPType;
+  CHAR16  CHAPName[ISCSI_CHAP_NAME_STORAGE];
+  CHAR16  CHAPSecret[ISCSI_CHAP_SECRET_STORAGE];
+  CHAR16  ReverseCHAPName[ISCSI_CHAP_NAME_STORAGE];
+  CHAR16  ReverseCHAPSecret[ISCSI_CHAP_SECRET_STORAGE];
+
+  CHAR16  IsId[ISID_CONFIGURABLE_STORAGE];
+} ISCSI_CONFIG_IFR_NVDATA;
+#pragma pack()
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.c
new file mode 100644
index 0000000000..d574ce21dd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.c
@@ -0,0 +1,472 @@
+/** @file
+  iSCSI DHCP related configuration routines.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+/**
+  Extract the Root Path option and get the required target information.
+
+  @param[in]        RootPath         The RootPath.
+  @param[in]        Length           Length of the RootPath option payload.
+  @param[in, out]   ConfigNvData     The iSCSI session configuration data read from nonvolatile device.
+
+  @retval EFI_SUCCESS           All required information is extracted from the RootPath option.
+  @retval EFI_NOT_FOUND         The RootPath is not an iSCSI RootPath.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory.
+  @retval EFI_INVALID_PARAMETER The RootPath is mal-formatted.
+**/
+EFI_STATUS
+IScsiDhcpExtractRootPath (
+  IN      CHAR8                        *RootPath,
+  IN      UINT8                        Length,
+  IN OUT  ISCSI_SESSION_CONFIG_NVDATA  *ConfigNvData
+  )
+{
+  EFI_STATUS            Status;
+  UINT8                 IScsiRootPathIdLen;
+  CHAR8                 *TmpStr;
+  ISCSI_ROOT_PATH_FIELD Fields[RP_FIELD_IDX_MAX];
+  ISCSI_ROOT_PATH_FIELD *Field;
+  UINT32                FieldIndex;
+  UINT8                 Index;
+
+  //
+  // "iscsi:"<servername>":"<protocol>":"<port>":"<LUN>":"<targetname>
+  //
+  IScsiRootPathIdLen = (UINT8) AsciiStrLen (ISCSI_ROOT_PATH_ID);
+
+  if ((Length <= IScsiRootPathIdLen) || (CompareMem (RootPath, ISCSI_ROOT_PATH_ID, IScsiRootPathIdLen) != 0)) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Skip the iSCSI RootPath ID "iscsi:".
+  //
+  RootPath += IScsiRootPathIdLen;
+  Length  = (UINT8) (Length - IScsiRootPathIdLen);
+
+  TmpStr  = (CHAR8 *) AllocatePool (Length + 1);
+  if (TmpStr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CopyMem (TmpStr, RootPath, Length);
+  TmpStr[Length]  = '\0';
+
+  Index           = 0;
+  FieldIndex      = RP_FIELD_IDX_SERVERNAME;
+  ZeroMem (&Fields[0], sizeof (Fields));
+
+  //
+  // Extract the fields in the Root Path option string.
+  //
+  for (FieldIndex = RP_FIELD_IDX_SERVERNAME; (FieldIndex < RP_FIELD_IDX_MAX) && (Index < Length); FieldIndex++) {
+    if (TmpStr[Index] != ISCSI_ROOT_PATH_FIELD_DELIMITER) {
+      Fields[FieldIndex].Str = &TmpStr[Index];
+    }
+
+    while ((TmpStr[Index] != ISCSI_ROOT_PATH_FIELD_DELIMITER) && (Index < Length)) {
+      Index++;
+    }
+
+    if (TmpStr[Index] == ISCSI_ROOT_PATH_FIELD_DELIMITER) {
+      if (FieldIndex != RP_FIELD_IDX_TARGETNAME) {
+        TmpStr[Index] = '\0';
+        Index++;
+      }
+
+      if (Fields[FieldIndex].Str != NULL) {
+        Fields[FieldIndex].Len = (UINT8) AsciiStrLen (Fields[FieldIndex].Str);
+      }
+    }
+  }
+
+  if (FieldIndex != RP_FIELD_IDX_MAX) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  
+    if ((Fields[RP_FIELD_IDX_SERVERNAME].Str == NULL) ||
+      (Fields[RP_FIELD_IDX_TARGETNAME].Str == NULL) ||
+      (Fields[RP_FIELD_IDX_PROTOCOL].Len > 1)
+      ) {
+
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  //
+  // Get the IP address of the target.
+  //
+  Field   = &Fields[RP_FIELD_IDX_SERVERNAME];
+  Status  = IScsiAsciiStrToIp (Field->Str, &ConfigNvData->TargetIp);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  //
+  // Check the protocol type.
+  //
+  Field = &Fields[RP_FIELD_IDX_PROTOCOL];
+  if ((Field->Str != NULL) && ((*(Field->Str) - '0') != EFI_IP_PROTO_TCP)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  //
+  // Get the port of the iSCSI target.
+  //
+  Field = &Fields[RP_FIELD_IDX_PORT];
+  if (Field->Str != NULL) {
+    ConfigNvData->TargetPort = (UINT16) AsciiStrDecimalToUintn (Field->Str);
+  } else {
+    ConfigNvData->TargetPort = ISCSI_WELL_KNOWN_PORT;
+  }
+  //
+  // Get the LUN.
+  //
+  Field = &Fields[RP_FIELD_IDX_LUN];
+  if (Field->Str != NULL) {
+    Status = IScsiAsciiStrToLun (Field->Str, ConfigNvData->BootLun);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  } else {
+    ZeroMem (ConfigNvData->BootLun, sizeof (ConfigNvData->BootLun));
+  }
+  //
+  // Get the target iSCSI Name.
+  //
+  Field = &Fields[RP_FIELD_IDX_TARGETNAME];
+
+  if (AsciiStrLen (Field->Str) > ISCSI_NAME_MAX_SIZE - 1) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  //
+  // Validate the iSCSI name.
+  //
+  Status = IScsiNormalizeName (Field->Str, AsciiStrLen (Field->Str));
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  AsciiStrCpyS (ConfigNvData->TargetName, ISCSI_NAME_MAX_SIZE, Field->Str);
+
+ON_EXIT:
+
+  FreePool (TmpStr);
+
+  return Status;
+}
+
+/**
+  The callback function registerd to the DHCP4 instance which is used to select
+  the qualified DHCP OFFER.
+  
+  @param[in]  This         The DHCP4 protocol.
+  @param[in]  Context      The context set when configuring the DHCP4 protocol.
+  @param[in]  CurrentState The current state of the DHCP4 protocol.
+  @param[in]  Dhcp4Event   The event occurs in the current state.
+  @param[in]  Packet       The DHCP packet that is to be sent or already received. 
+  @param[out] NewPacket    The packet used to replace the above Packet.
+  
+  @retval EFI_SUCCESS      Either the DHCP OFFER is qualified or we're not intereseted
+                           in the Dhcp4Event.
+  @retval EFI_NOT_READY    The DHCP OFFER packet doesn't match our requirements.
+  @retval Others           Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDhcpSelectOffer (
+  IN  EFI_DHCP4_PROTOCOL  * This,
+  IN  VOID                *Context,
+  IN  EFI_DHCP4_STATE     CurrentState,
+  IN  EFI_DHCP4_EVENT     Dhcp4Event,
+  IN  EFI_DHCP4_PACKET    * Packet, OPTIONAL
+  OUT EFI_DHCP4_PACKET    **NewPacket OPTIONAL
+  )
+{
+  EFI_STATUS              Status;
+  UINT32                  OptionCount;
+  EFI_DHCP4_PACKET_OPTION **OptionList;
+  UINT32                  Index;
+
+  if ((Dhcp4Event != Dhcp4RcvdOffer) && (Dhcp4Event != Dhcp4SelectOffer)) {
+    return EFI_SUCCESS;
+  }
+
+  OptionCount = 0;
+
+  Status      = This->Parse (This, Packet, &OptionCount, NULL);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return EFI_NOT_READY;
+  }
+
+  OptionList = AllocatePool (OptionCount * sizeof (EFI_DHCP4_PACKET_OPTION *));
+  if (OptionList == NULL) {
+    return EFI_NOT_READY;
+  }
+
+  Status = This->Parse (This, Packet, &OptionCount, OptionList);
+  if (EFI_ERROR (Status)) {
+    FreePool (OptionList);
+    return EFI_NOT_READY;
+  }
+
+  for (Index = 0; Index < OptionCount; Index++) {
+    if (OptionList[Index]->OpCode != DHCP4_TAG_ROOT_PATH) {
+      continue;
+    }
+
+    Status = IScsiDhcpExtractRootPath (
+              (CHAR8 *) &OptionList[Index]->Data[0],
+              OptionList[Index]->Length,
+              (ISCSI_SESSION_CONFIG_NVDATA *) Context
+              );
+
+    break;
+  }
+
+  if (Index == OptionCount) {
+    Status = EFI_NOT_READY;
+  }
+
+  FreePool (OptionList);
+
+  return Status;
+}
+
+/**
+  Parse the DHCP ACK to get the address configuration and DNS information.
+
+  @param[in]       Dhcp4        The DHCP4 protocol.
+  @param[in, out]  ConfigData   The session configuration data.
+
+  @retval EFI_SUCCESS           The DNS information is got from the DHCP ACK.
+  @retval EFI_NO_MAPPING        DHCP failed to acquire address and other information.
+  @retval EFI_INVALID_PARAMETER The DHCP ACK's DNS option is mal-formatted.
+  @retval EFI_DEVICE_ERROR      Other errors as indicated.
+**/
+EFI_STATUS
+IScsiParseDhcpAck (
+  IN      EFI_DHCP4_PROTOCOL         *Dhcp4,
+  IN OUT  ISCSI_SESSION_CONFIG_DATA  *ConfigData
+  )
+{
+  EFI_STATUS              Status;
+  EFI_DHCP4_MODE_DATA     Dhcp4ModeData;
+  UINT32                  OptionCount;
+  EFI_DHCP4_PACKET_OPTION **OptionList;
+  UINT32                  Index;
+
+  Status = Dhcp4->GetModeData (Dhcp4, &Dhcp4ModeData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Dhcp4ModeData.State != Dhcp4Bound) {
+    return EFI_NO_MAPPING;
+  }
+
+  CopyMem (&ConfigData->NvData.LocalIp, &Dhcp4ModeData.ClientAddress, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&ConfigData->NvData.SubnetMask, &Dhcp4ModeData.SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&ConfigData->NvData.Gateway, &Dhcp4ModeData.RouterAddress, sizeof (EFI_IPv4_ADDRESS));
+
+  OptionCount = 0;
+  OptionList  = NULL;
+
+  Status      = Dhcp4->Parse (Dhcp4, Dhcp4ModeData.ReplyPacket, &OptionCount, OptionList);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  OptionList = AllocatePool (OptionCount * sizeof (EFI_DHCP4_PACKET_OPTION *));
+  if (OptionList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = Dhcp4->Parse (Dhcp4, Dhcp4ModeData.ReplyPacket, &OptionCount, OptionList);
+  if (EFI_ERROR (Status)) {
+    FreePool (OptionList);
+    return EFI_DEVICE_ERROR;
+  }
+
+  for (Index = 0; Index < OptionCount; Index++) {
+    //
+    // Get DNS server addresses and DHCP server address from this offer.
+    //
+    if (OptionList[Index]->OpCode == DHCP4_TAG_DNS) {
+
+      if (((OptionList[Index]->Length & 0x3) != 0) || (OptionList[Index]->Length == 0)) {
+        Status = EFI_INVALID_PARAMETER;
+        break;
+      }
+      //
+      // Primary DNS server address.
+      //
+      CopyMem (&ConfigData->PrimaryDns, &OptionList[Index]->Data[0], sizeof (EFI_IPv4_ADDRESS));
+
+      if (OptionList[Index]->Length > 4) {
+        //
+        // Secondary DNS server address
+        //
+        CopyMem (&ConfigData->SecondaryDns, &OptionList[Index]->Data[4], sizeof (EFI_IPv4_ADDRESS));
+      }
+    } else if (OptionList[Index]->OpCode == DHCP4_TAG_SERVER_ID) {
+      if (OptionList[Index]->Length != 4) {
+        Status = EFI_INVALID_PARAMETER;
+        break;
+      }
+
+      CopyMem (&ConfigData->DhcpServer, &OptionList[Index]->Data[0], sizeof (EFI_IPv4_ADDRESS));
+    }
+  }
+
+  FreePool (OptionList);
+
+  return Status;
+}
+
+/**
+  Parse the DHCP ACK to get the address configuration and DNS information.
+  
+  @param[in]       Image            The handle of the driver image.
+  @param[in]       Controller       The handle of the controller;
+  @param[in, out]  ConfigData       The session configuration data.
+
+  @retval EFI_SUCCESS           The DNS information is got from the DHCP ACK.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory.
+  @retval EFI_NO_MEDIA          There was a media error.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+IScsiDoDhcp (
+  IN     EFI_HANDLE                 Image,
+  IN     EFI_HANDLE                 Controller,
+  IN OUT ISCSI_SESSION_CONFIG_DATA  *ConfigData
+  )
+{
+  EFI_HANDLE              Dhcp4Handle;
+  EFI_DHCP4_PROTOCOL      *Dhcp4;
+  EFI_STATUS              Status;
+  EFI_DHCP4_PACKET_OPTION *ParaList;
+  EFI_DHCP4_CONFIG_DATA   Dhcp4ConfigData;
+  BOOLEAN                 MediaPresent;
+  UINT8                   *Data;
+
+  Dhcp4Handle = NULL;
+  Dhcp4       = NULL;
+  ParaList    = NULL;
+
+  //
+  // Check media status before do DHCP
+  //
+  MediaPresent = TRUE;
+  NetLibDetectMedia (Controller, &MediaPresent);
+  if (!MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  //
+  // Create a DHCP4 child instance and get the protocol.
+  //
+  Status = NetLibCreateServiceChild (
+            Controller,
+            Image,
+            &gEfiDhcp4ServiceBindingProtocolGuid,
+            &Dhcp4Handle
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Dhcp4Handle,
+                  &gEfiDhcp4ProtocolGuid,
+                  (VOID **)&Dhcp4,
+                  Image,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  ParaList = AllocatePool (sizeof (EFI_DHCP4_PACKET_OPTION) + 3);
+  if (ParaList == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+  //
+  // Ask the server to reply with Netmask, Router, DNS and RootPath options.
+  //
+  ParaList->OpCode  = DHCP4_TAG_PARA_LIST;
+  ParaList->Length  = (UINT8) (ConfigData->NvData.TargetInfoFromDhcp ? 4 : 3);
+  Data = &ParaList->Data[0];
+  Data[0] = DHCP4_TAG_NETMASK;
+  Data[1] = DHCP4_TAG_ROUTER;
+  Data[2] = DHCP4_TAG_DNS;
+  Data[3] = DHCP4_TAG_ROOT_PATH;
+
+  ZeroMem (&Dhcp4ConfigData, sizeof (EFI_DHCP4_CONFIG_DATA));
+  Dhcp4ConfigData.OptionCount = 1;
+  Dhcp4ConfigData.OptionList  = &ParaList;
+
+  if (ConfigData->NvData.TargetInfoFromDhcp) {
+    //
+    // Use callback to select an offer which contains target information.
+    //
+    Dhcp4ConfigData.Dhcp4Callback   = IScsiDhcpSelectOffer;
+    Dhcp4ConfigData.CallbackContext = &ConfigData->NvData;
+  }
+
+  Status = Dhcp4->Configure (Dhcp4, &Dhcp4ConfigData);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = Dhcp4->Start (Dhcp4, NULL);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  //
+  // Parse the ACK to get required information.
+  //
+  Status = IScsiParseDhcpAck (Dhcp4, ConfigData);
+
+ON_EXIT:
+
+  if (ParaList != NULL) {
+    FreePool (ParaList);
+  }
+
+  if (Dhcp4 != NULL) {
+    Dhcp4->Stop (Dhcp4);
+    Dhcp4->Configure (Dhcp4, NULL);
+
+    gBS->CloseProtocol (
+          Dhcp4Handle,
+          &gEfiDhcp4ProtocolGuid,
+          Image,
+          Controller
+          );
+  }
+
+  NetLibDestroyServiceChild (
+    Controller,
+    Image,
+    &gEfiDhcp4ServiceBindingProtocolGuid,
+    Dhcp4Handle
+    );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.h
new file mode 100644
index 0000000000..77754b896a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDhcp.h
@@ -0,0 +1,61 @@
+/** @file
+  The header file of IScsiDhcp.
+
+Copyright (c) 2004 - 2010, 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.
+
+**/
+
+#ifndef _ISCSI_DHCP_H_
+#define _ISCSI_DHCP_H_
+
+#include <Protocol/Dhcp4.h>
+
+#define DHCP4_TAG_PARA_LIST             55
+#define DHCP4_TAG_NETMASK               1
+#define DHCP4_TAG_ROUTER                3
+#define DHCP4_TAG_DNS                   6
+#define DHCP4_TAG_SERVER_ID             54
+#define DHCP4_TAG_ROOT_PATH             17
+#define ISCSI_ROOT_PATH_ID              "iscsi:"
+#define ISCSI_ROOT_PATH_FIELD_DELIMITER ':'
+
+#define RP_FIELD_IDX_SERVERNAME         0
+#define RP_FIELD_IDX_PROTOCOL           1
+#define RP_FIELD_IDX_PORT               2
+#define RP_FIELD_IDX_LUN                3
+#define RP_FIELD_IDX_TARGETNAME         4
+#define RP_FIELD_IDX_MAX                5
+
+typedef struct _ISCSI_ROOT_PATH_FIELD {
+  CHAR8 *Str;
+  UINT8 Len;
+} ISCSI_ROOT_PATH_FIELD;
+
+/**
+  Parse the DHCP ACK to get the address configuration and DNS information.
+  
+  @param[in]       Image            The handle of the driver image.
+  @param[in]       Controller       The handle of the controller;
+  @param[in, out]  ConfigData       The session configuration data.
+
+  @retval EFI_SUCCESS           The DNS information is got from the DHCP ACK.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory.
+  @retval EFI_NO_MEDIA          There was a media error.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+IScsiDoDhcp (
+  IN     EFI_HANDLE                 Image,
+  IN     EFI_HANDLE                 Controller,
+  IN OUT ISCSI_SESSION_CONFIG_DATA  *ConfigData
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.c
new file mode 100644
index 0000000000..e55bee8af9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.c
@@ -0,0 +1,669 @@
+/** @file
+  The entry point of IScsi driver.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gIScsiDriverBinding = {
+  IScsiDriverBindingSupported,
+  IScsiDriverBindingStart,
+  IScsiDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Tests to see if this driver supports the RemainingDevicePath. 
+
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path.  This 
+                                   parameter is ignored by device drivers, and is optional for bus 
+                                   drivers. For bus drivers, if this parameter is not NULL, then 
+                                   the bus driver must determine if the bus controller specified 
+                                   by ControllerHandle and the child controller specified 
+                                   by RemainingDevicePath are both supported by this 
+                                   bus driver.
+
+  @retval EFI_SUCCESS              The RemainingDevicePath is supported or NULL.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+IScsiIsDevicePathSupported (
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL  *CurrentDevicePath;
+
+  CurrentDevicePath = RemainingDevicePath;
+  if (CurrentDevicePath != NULL) {
+    while (!IsDevicePathEnd (CurrentDevicePath)) {
+      if ((CurrentDevicePath->Type == MESSAGING_DEVICE_PATH) && (CurrentDevicePath->SubType == MSG_ISCSI_DP)) {
+        return EFI_SUCCESS;
+      }
+
+      CurrentDevicePath = NextDevicePathNode (CurrentDevicePath);
+    }
+
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided, 
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, and is optional for bus drivers.
+
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive acces.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiTcp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = IScsiIsDevicePathSupported (RemainingDevicePath);
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (IScsiDhcpIsConfigured (ControllerHandle)) {
+    Status = gBS->OpenProtocol (
+                    ControllerHandle,
+                    &gEfiDhcp4ServiceBindingProtocolGuid,
+                    NULL,
+                    This->DriverBindingHandle,
+                    ControllerHandle,
+                    EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Start this driver on ControllerHandle. 
+  
+  The Start() function is designed to be invoked from the EFI boot service ConnectController(). 
+  As a result, much of the error checking on the parameters to Start() has been moved into this 
+  common boot service. It is legal to call Start() from other locations, but the following calling 
+  restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.  
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.
+                                   Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  ISCSI_DRIVER_DATA *Private;
+  VOID              *Interface;
+
+  Private = IScsiCreateDriverData (This->DriverBindingHandle, ControllerHandle);
+  if (Private == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Create a underlayer child instance, but not need to configure it. Just open ChildHandle
+  // via BY_DRIVER. That is, establishing the relationship between ControllerHandle and ChildHandle.
+  // Therefore, when DisconnectController(), especially VLAN virtual controller handle,
+  // IScsiDriverBindingStop() will be called.
+  //
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             This->DriverBindingHandle,
+             &gEfiTcp4ServiceBindingProtocolGuid,
+             &Private->ChildHandle
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->ChildHandle,
+                  &gEfiTcp4ProtocolGuid,
+                  &Interface,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Always install private protocol no matter what happens later. We need to 
+  // keep the relationship between ControllerHandle and ChildHandle.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Private->IScsiIdentifier
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Try to add a port configuration page for this controller.
+  //
+  IScsiConfigUpdateForm (This->DriverBindingHandle, ControllerHandle, TRUE);
+
+  //
+  // Get the iSCSI configuration data of this controller.
+  //
+  Status = IScsiGetConfigData (Private);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+  //
+  // Try to login and create an iSCSI session according to the configuration.
+  //
+  Status = IScsiSessionLogin (Private);
+  if (Status == EFI_MEDIA_CHANGED) {
+    //
+    // The specified target is not available and the redirection information is
+    // got, login the session again with the updated target address.
+    //
+    Status = IScsiSessionLogin (Private);
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+  //
+  // Duplicate the Session's tcp connection device path. The source port field
+  // will be set to zero as one iSCSI session is comprised of several iSCSI
+  // connections.
+  //
+  Private->DevicePath = IScsiGetTcpConnDevicePath (Private);
+  if (Private->DevicePath == NULL) {
+    goto ON_ERROR;
+  }
+  //
+  // Install the updated device path onto the ExtScsiPassThruHandle.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &Private->ExtScsiPassThruHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  Private->DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // ISCSI children should share the default Tcp child, just open the default Tcp child via BY_CHILD_CONTROLLER.
+  //
+  Status = gBS->OpenProtocol (
+                  Private->ChildHandle, /// Default Tcp child
+                  &gEfiTcp4ProtocolGuid,
+                  &Interface,
+                  This->DriverBindingHandle,
+                  Private->ExtScsiPassThruHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );              
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Private->ExtScsiPassThruHandle,
+           &gEfiExtScsiPassThruProtocolGuid,
+           &Private->IScsiExtScsiPassThru,
+           &gEfiDevicePathProtocolGuid,
+           Private->DevicePath,
+           NULL
+           );
+    
+    goto ON_ERROR;
+  }
+
+  //
+  // Update/Publish the iSCSI Boot Firmware Table.
+  //
+  IScsiPublishIbft ();
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  IScsiSessionAbort (&Private->Session);
+
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle. 
+  
+  Release the control of this controller and remove the IScsi functions. The Stop()
+  function is designed to be invoked from the EFI boot service DisconnectController(). 
+  As a result, much of the error checking on the parameters to Stop() has been moved 
+  into this common boot service. It is legal to call Stop() from other locations, 
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.Not used.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.Not used.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer OPTIONAL
+  )
+{
+  EFI_HANDLE                      IScsiController;
+  EFI_STATUS                      Status;
+  ISCSI_PRIVATE_PROTOCOL          *IScsiIdentifier;
+  ISCSI_DRIVER_DATA               *Private;
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL *PassThru;
+  ISCSI_CONNECTION                *Conn;
+
+  if (NumberOfChildren != 0) {
+    //
+    // We should have only one child.
+    //
+    Status = gBS->OpenProtocol (
+                    ChildHandleBuffer[0],
+                    &gEfiExtScsiPassThruProtocolGuid,
+                    (VOID **) &PassThru,
+                    This->DriverBindingHandle,
+                    ControllerHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    Private = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (PassThru);
+    Conn    = NET_LIST_HEAD (&Private->Session.Conns, ISCSI_CONNECTION, Link);
+
+    //
+    // Previously the TCP4 protocol is opened BY_CHILD_CONTROLLER. Just close
+    // the protocol here but not uninstall the device path protocol and
+    // EXT SCSI PASS THRU protocol installed on ExtScsiPassThruHandle.
+    //
+    gBS->CloseProtocol (
+           Private->ChildHandle,
+           &gEfiTcp4ProtocolGuid,
+           Private->Image,
+           Private->ExtScsiPassThruHandle
+           );
+    
+    gBS->CloseProtocol (
+          Conn->Tcp4Io.Handle,
+          &gEfiTcp4ProtocolGuid,
+          Private->Image,
+          Private->ExtScsiPassThruHandle
+          );
+
+    return EFI_SUCCESS;
+  }
+  //
+  // Get the handle of the controller we are controling.
+  //
+  IScsiController = NetLibGetNicHandle (ControllerHandle, &gEfiTcp4ProtocolGuid);
+
+  Status = gBS->OpenProtocol (
+                  IScsiController,
+                  &gEfiCallerIdGuid,
+                  (VOID **)&IScsiIdentifier,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Private = ISCSI_DRIVER_DATA_FROM_IDENTIFIER (IScsiIdentifier);
+
+  if (Private->ChildHandle != NULL) {
+    Status = gBS->CloseProtocol (
+                    Private->ChildHandle,
+                    &gEfiTcp4ProtocolGuid,
+                    This->DriverBindingHandle,
+                    IScsiController
+                    );
+
+    ASSERT (!EFI_ERROR (Status));
+
+    Status = NetLibDestroyServiceChild (
+               IScsiController,
+               This->DriverBindingHandle,
+               &gEfiTcp4ServiceBindingProtocolGuid,
+               Private->ChildHandle
+               );
+    ASSERT (!EFI_ERROR (Status));
+  }
+
+  IScsiConfigUpdateForm (This->DriverBindingHandle, IScsiController, FALSE);
+
+  //
+  // Uninstall the private protocol.
+  //
+  gBS->UninstallProtocolInterface (
+        IScsiController,
+        &gEfiCallerIdGuid,
+        &Private->IScsiIdentifier
+        );
+
+  //
+  // Update the iSCSI Boot Firware Table.
+  //
+  IScsiPublishIbft ();
+
+  IScsiSessionAbort (&Private->Session);
+  IScsiCleanDriverData (Private);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Unloads an image(the iSCSI driver).
+
+  @param[in]  ImageHandle       Handle that identifies the image to be unloaded.
+
+  @retval EFI_SUCCESS           The image has been unloaded.
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+EfiIScsiUnload (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             DeviceHandleCount;
+  EFI_HANDLE                        *DeviceHandleBuffer;
+  UINTN                             Index;
+  EFI_COMPONENT_NAME_PROTOCOL       *ComponentName;
+  EFI_COMPONENT_NAME2_PROTOCOL      *ComponentName2;
+
+  //
+  // Try to disonnect the driver from the devices it's controlling.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  AllHandles,
+                  NULL,
+                  NULL,
+                  &DeviceHandleCount,
+                  &DeviceHandleBuffer
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  for (Index = 0; Index < DeviceHandleCount; Index++) {
+    Status = IScsiTestManagedDevice (
+               DeviceHandleBuffer[Index],
+               gIScsiDriverBinding.DriverBindingHandle,
+               &gEfiTcp4ProtocolGuid
+               );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+    Status = gBS->DisconnectController (
+                    DeviceHandleBuffer[Index],
+                    gIScsiDriverBinding.DriverBindingHandle,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }  
+
+  //
+  // Unload the iSCSI configuration form.
+  //
+  Status = IScsiConfigFormUnload (gIScsiDriverBinding.DriverBindingHandle);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Uninstall the ComponentName and ComponentName2 protocol from iSCSI4 driver binding handle
+  // if it has been installed.
+  //
+  Status = gBS->HandleProtocol (
+                  gIScsiDriverBinding.DriverBindingHandle,
+                  &gEfiComponentNameProtocolGuid,
+                  (VOID **) &ComponentName
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+           gIScsiDriverBinding.DriverBindingHandle,
+           &gEfiComponentNameProtocolGuid,
+           ComponentName,
+           NULL
+           );
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+  
+  Status = gBS->HandleProtocol (
+                  gIScsiDriverBinding.DriverBindingHandle,
+                  &gEfiComponentName2ProtocolGuid,
+                  (VOID **) &ComponentName2
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           gIScsiDriverBinding.DriverBindingHandle,
+           &gEfiComponentName2ProtocolGuid,
+           ComponentName2,
+           NULL
+           );
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ImageHandle,
+                  &gEfiDriverBindingProtocolGuid,
+                  &gIScsiDriverBinding,
+                  &gEfiIScsiInitiatorNameProtocolGuid,
+                  &gIScsiInitiatorName,
+                  NULL
+                  );
+ON_EXIT:
+
+  if (DeviceHandleBuffer != NULL) {
+    FreePool (DeviceHandleBuffer);
+  }
+  
+  return Status;
+}
+
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers. It initialize the global variables and 
+  publish the driver binding protocol.
+
+  @param[in]   ImageHandle      The firmware allocated handle for the UEFI image.
+  @param[in]   SystemTable      A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_ACCESS_DENIED     EFI_ISCSI_INITIATOR_NAME_PROTOCOL was installed unexpectedly.
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverEntryPoint (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *IScsiInitiatorName;
+
+  //
+  // There should be only one EFI_ISCSI_INITIATOR_NAME_PROTOCOL.
+  //
+  Status = gBS->LocateProtocol (
+                   &gEfiIScsiInitiatorNameProtocolGuid,
+                   NULL,
+                   (VOID**) &IScsiInitiatorName
+                   );
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Initialize the EFI Driver Library
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gIScsiDriverBinding,
+             ImageHandle,
+             &gIScsiComponentName,
+             &gIScsiComponentName2
+           );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Install the iSCSI Initiator Name Protocol.
+    //
+    Status = gBS->InstallProtocolInterface (
+                    &ImageHandle,
+                    &gEfiIScsiInitiatorNameProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &gIScsiInitiatorName
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            ImageHandle,
+            &gEfiDriverBindingProtocolGuid,
+            &gIScsiDriverBinding,
+            &gEfiComponentName2ProtocolGuid,
+            &gIScsiComponentName2,
+            &gEfiComponentNameProtocolGuid,
+            &gIScsiComponentName,
+            NULL
+            );
+      return Status;
+    }
+  
+    //
+    // Initialize the configuration form of iSCSI.
+    //
+    Status = IScsiConfigFormInit ();
+    if (EFI_ERROR (Status)) {
+      gBS->UninstallMultipleProtocolInterfaces (
+            ImageHandle,
+            &gEfiDriverBindingProtocolGuid,
+            &gIScsiDriverBinding,
+            &gEfiComponentName2ProtocolGuid,
+            &gIScsiComponentName2,
+            &gEfiComponentNameProtocolGuid,
+            &gIScsiComponentName,
+            &gEfiIScsiInitiatorNameProtocolGuid,
+            &gIScsiInitiatorName,
+            NULL
+            );
+    }
+  }
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.h
new file mode 100644
index 0000000000..14f640444f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDriver.h
@@ -0,0 +1,140 @@
+/** @file
+  The header file of IScsiDriver.c.
+
+Copyright (c) 2004 - 2011, 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.
+
+**/
+
+#ifndef _ISCSI_DRIVER_H_
+#define _ISCSI_DRIVER_H_
+
+#include <Uefi.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiDatabase.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/ScsiPassThruExt.h>
+
+#define ISCSI_INITIATOR_NAME_VAR_NAME L"I_NAME"
+
+typedef struct _ISCSI_PRIVATE_PROTOCOL {
+  UINT32  Reserved;
+} ISCSI_PRIVATE_PROTOCOL;
+
+//
+// EFI Driver Binding Protocol for iSCSI driver.
+//
+
+/**
+  Tests to see if this driver supports a given controller. If a child device is provided, 
+  it further tests to see if this driver supports creating a handle for the specified child device.
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to test. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, and is optional for bus drivers.
+
+
+  @retval EFI_SUCCESS              The device specified by ControllerHandle and
+                                   RemainingDevicePath is supported by the driver specified by This.
+  @retval EFI_ALREADY_STARTED      The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by the driver
+                                   specified by This.
+  @retval EFI_ACCESS_DENIED        The device specified by ControllerHandle and
+                                   RemainingDevicePath is already being managed by a different
+                                   driver or an application that requires exclusive acces.
+                                   Currently not implemented.
+  @retval EFI_UNSUPPORTED          The device specified by ControllerHandle and
+                                   RemainingDevicePath is not supported by the driver specified by This.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. The Start() function is designed to be 
+  invoked from the EFI boot service ConnectController(). As a result, much of 
+  the error checking on the parameters to Start() has been moved into this 
+  common boot service. It is legal to call Start() from other locations, 
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally aligned
+     EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified by This must
+     have been called with the same calling parameters, and Supported() must have returned EFI_SUCCESS.  
+
+  @param[in]  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle     The handle of the controller to start. This handle 
+                                   must support a protocol interface that supplies 
+                                   an I/O abstraction to the driver.
+  @param[in]  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                                   This parameter is ignored by device drivers, and is optional for bus drivers.
+
+  @retval EFI_SUCCESS              The device was started.
+  @retval EFI_DEVICE_ERROR         The device could not be started due to a device error.
+                                   Currently not implemented.
+  @retval EFI_OUT_OF_RESOURCES     The request could not be completed due to a lack of resources.
+  @retval Others                   The driver failded to start the device.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle. 
+  
+  Release the control of this controller and remove the IScsi functions. The Stop()
+  function is designed to be invoked from the EFI boot service DisconnectController(). 
+  As a result, much of the error checking on the parameters to Stop() has been moved 
+  into this common boot service. It is legal to call Stop() from other locations, 
+  but the following calling restrictions must be followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call to this
+     same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this driver's
+     Start() function, and the Start() function must have called OpenProtocol() on
+     ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param[in]  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param[in]  ControllerHandle  A handle to the device being stopped. The handle must 
+                                support a bus specific I/O protocol for the driver 
+                                to use to stop the device.
+  @param[in]  NumberOfChildren  The number of child device handles in ChildHandleBuffer.Not used.
+  @param[in]  ChildHandleBuffer An array of child handles to be freed. May be NULL 
+                                if NumberOfChildren is 0.Not used.
+
+  @retval EFI_SUCCESS           The device was stopped.
+  @retval EFI_DEVICE_ERROR      The device could not be stopped due to a device error.
+**/
+EFI_STATUS
+EFIAPI
+IScsiDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDxe.inf b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDxe.inf
new file mode 100644
index 0000000000..4fd2a62af9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiDxe.inf
@@ -0,0 +1,125 @@
+## @file
+#  This module produces EFI iSCSI Initiator Name Protocol.
+#
+#  This module produces EFI iSCSI Initiator Name Protocol upon EFI TCPv4 Protocol
+#  and EFI DHCPv4 Protocol, to provide the capability to do the transport for SCSI
+#  data over TCP/IP. It installs EFI HII Configuration Access Protocol to provide
+#  one way to configurate the iSCSI setting.
+#
+#  Copyright (c) 2004 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = IScsi4Dxe
+  MODULE_UNI_FILE                = IScsi4Dxe.uni
+  FILE_GUID                      = 4579B72D-7EC4-4dd4-8486-083C86B182A7
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = IScsiDriverEntryPoint
+  UNLOAD_IMAGE                   = EfiIScsiUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gIScsiDriverBinding
+#  COMPONENT_NAME                =  gIScsiComponentName
+#  COMPONENT_NAME2               =  gIScsiComponentName2
+#
+
+[Sources]
+  IScsiTcp4Io.h
+  IScsiProto.h
+  IScsiMisc.h
+  IScsiIbft.h
+  IScsiExtScsiPassThru.h
+  IScsiDriver.h
+  IScsiDhcp.h
+  IScsiCommon.h
+  IScsiCHAP.h
+  IScsiInitiatorName.h
+  ComponentName.h
+  Md5.h
+  IScsiTcp4Io.c
+  IScsiProto.c
+  IScsiMisc.c
+  IScsiInitiatorName.c
+  IScsiIbft.c
+  IScsiExtScsiPassThru.c
+  IScsiDriver.c
+  IScsiDhcp.c
+  IScsiCHAP.c
+  ComponentName.c
+  Md5.c
+  IScsiConfigDxeStrings.uni
+  IScsiConfigDxe.vfr
+  IScsiConfig.c
+  IScsiConfig.h
+  IScsiImpl.h
+  IScsiConfigNVDataStruc.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  DevicePathLib
+  DebugLib
+  PrintLib
+  HiiLib
+  NetLib
+
+[Protocols]
+  ## PRODUCES
+  ## UNDEFINED # Variable
+  gEfiIScsiInitiatorNameProtocolGuid
+  gEfiExtScsiPassThruProtocolGuid               ## BY_START
+  gEfiTcp4ProtocolGuid                          ## TO_START
+  gEfiTcp4ServiceBindingProtocolGuid            ## TO_START
+  gEfiDhcp4ProtocolGuid                         ## TO_START
+  gEfiDhcp4ServiceBindingProtocolGuid           ## TO_START
+  ## TO_START
+  ## PRODUCES
+  gEfiDevicePathProtocolGuid
+  gEfiHiiConfigAccessProtocolGuid               ## PRODUCES
+  gEfiHiiDatabaseProtocolGuid                   ## CONSUMES
+  gEfiPciIoProtocolGuid                         ## SOMETIMES_CONSUMES
+  gEfiAcpiTableProtocolGuid                     ## SOMETIMES_CONSUMES
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+  gEfiIfrTianoGuid                              ## SOMETIMES_PRODUCES ## UNDEFINED
+  gEfiAcpiTableGuid                             ## SOMETIMES_CONSUMES ## SystemTable
+  gEfiAcpi10TableGuid                           ## SOMETIMES_CONSUMES ## SystemTable
+  gEfiAcpi20TableGuid                           ## SOMETIMES_CONSUMES ## SystemTable
+  ## SOMETIMES_PRODUCES ## Variable:L"iSCSIDeviceList"
+  ## SOMETIMES_CONSUMES ## Variable:L"iSCSIDeviceList"
+  ## SOMETIMES_CONSUMES ## UNDEFINED # HiiIsConfigHdrMatch   mVendorStorageName
+  ## SOMETIMES_PRODUCES ## UNDEFINED # HiiConstructConfigHdr mVendorStorageName
+  ## SOMETIMES_PRODUCES ## UNDEFINED # HiiGetBrowserData     mVendorStorageName
+  ## SOMETIMES_CONSUMES ## UNDEFINED # HiiSetBrowserData     mVendorStorageName
+  ## SOMETIMES_CONSUMES ## HII
+  gIp4IScsiConfigGuid
+  ## SOMETIMES_PRODUCES ## Variable
+  ## SOMETIMES_CONSUMES ## Variable
+  gIScsiCHAPAuthInfoGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  IScsi4DxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.c
new file mode 100644
index 0000000000..e9e37b7e37
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.c
@@ -0,0 +1,412 @@
+/** @file
+  The IScsi's EFI_EXT_SCSI_PASS_THRU_PROTOCOL driver.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+/**
+  Sends a SCSI Request Packet to a SCSI device that is attached to the SCSI channel. This function   
+  supports both blocking I/O and nonblocking I/O. The blocking I/O functionality is required, and the
+  nonblocking I/O functionality is optional.                                                             
+
+  @param[in]      This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in]      Target  The Target is an array of size TARGET_MAX_BYTES and it represents
+                          the id of the SCSI device to send the SCSI Request Packet. Each
+                          transport driver may chose to utilize a subset of this size to suit the needs
+                          of transport target representation. For example, a Fibre Channel driver
+                          may use only 8 bytes (WWN) to represent an FC target.
+  @param[in]       Lun    The LUN of the SCSI device to send the SCSI Request Packet.
+  @param[in, out]  Packet A pointer to the SCSI Request Packet to send to the SCSI device
+                          specified by Target and Lun.
+  @param[in]       Event  If nonblocking I/O is not supported then Event is ignored, and blocking
+                          I/O is performed. If Event is NULL, then blocking I/O is performed. If
+                          Event is not NULL and non blocking I/O is supported, then
+                          nonblocking I/O is performed, and Event will be signaled when the
+                          SCSI Request Packet completes.
+
+  @retval EFI_SUCCESS           The SCSI Request Packet was sent by the host. For bi-directional
+                                commands, InTransferLength bytes were transferred from
+                                InDataBuffer. For write and bi-directional commands,
+                                OutTransferLength bytes were transferred by
+                                OutDataBuffer.
+  @retval EFI_BAD_BUFFER_SIZE   The SCSI Request Packet was not executed. The number of bytes that
+                                could be transferred is returned in InTransferLength. For write
+                                and bi-directional commands, OutTransferLength bytes were
+                                transferred by OutDataBuffer. Currently not implemeted.                                
+  @retval EFI_NOT_READY         The SCSI Request Packet could not be sent because there are too many
+                                SCSI Request Packets already queued. The caller may retry again later.                             
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to send the SCSI Request
+                                Packet. 
+  @retval EFI_INVALID_PARAMETER Target, Lun, or the contents of ScsiRequestPacket are invalid.
+  @retval EFI_UNSUPPORTED       The command described by the SCSI Request Packet is not supported
+                                by the host adapter. This includes the case of Bi-directional SCSI
+                                commands not supported by the implementation. The SCSI Request
+                                Packet was not sent, so no additional status information is available.
+                                Currently not implemeted.
+  @retval EFI_TIMEOUT           A timeout occurred while waiting for the SCSI Request Packet to execute.                              
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruFunction (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                          *This,
+  IN UINT8                                                    *Target,
+  IN UINT64                                                   Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET           *Packet,
+  IN EFI_EVENT                                                Event     OPTIONAL
+  )
+{
+  ISCSI_DRIVER_DATA           *Private;
+  ISCSI_SESSION_CONFIG_NVDATA *ConfigNvData;
+  EFI_STATUS                  Status; 
+
+  Private       = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (This);
+  ConfigNvData  = &Private->Session.ConfigData.NvData;
+
+  if (Target[0] != 0 || (CompareMem (&Lun, ConfigNvData->BootLun, sizeof (UINT64)) != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Packet == NULL) || (Packet->Cdb == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = IScsiExecuteScsiCommand (This, Target, Lun, Packet);
+  if ((Status != EFI_SUCCESS) && (Status != EFI_NOT_READY)) {
+    //
+    // Try to reinstate the session and re-execute the Scsi command.
+    //
+    if (EFI_ERROR (IScsiSessionReinstatement (Private))) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    Status = IScsiExecuteScsiCommand (This, Target, Lun, Packet);
+  }
+
+  return Status;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs and LUNs for SCSI devices on a SCSI channel. These       
+  can either be the list SCSI devices that are actually present on the SCSI channel, or the list of legal
+  Target Ids and LUNs for the SCSI channel. Regardless, the caller of this function must probe the       
+  Target ID and LUN returned to see if a SCSI device is actually present at that location on the SCSI    
+  channel.                                                                                               
+
+  @param[in]       This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in, out]  Target  On input, a pointer to the Target ID (an array of size
+                           TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                           On output, a pointer to the Target ID (an array of
+                           TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                           channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                           Target array retrieves the Target ID of the first SCSI device present on a
+                           SCSI channel.
+  @param[in, out]  Lun     On input, a pointer to the LUN of a SCSI device present on the SCSI
+                           channel. On output, a pointer to the LUN of the next SCSI device present
+                           on a SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID and LUN of the next SCSI device on the SCSI
+                                channel was returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER Target array is not all 0xF, and Target and Lun were
+                                not returned on a previous call to GetNextTargetLun().
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruGetNextTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This,
+  IN OUT UINT8                        **Target,
+  IN OUT UINT64                       *Lun
+  )
+{
+  ISCSI_DRIVER_DATA           *Private;
+  ISCSI_SESSION_CONFIG_NVDATA *ConfigNvData;
+  UINT8                       TargetId[TARGET_MAX_BYTES];
+
+  Private       = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (This);
+  ConfigNvData  = &Private->Session.ConfigData.NvData;
+
+  if ((*Target)[0] == 0 && (CompareMem (Lun, ConfigNvData->BootLun, sizeof (UINT64)) == 0)) {
+    //
+    // Only one <Target, Lun> pair per iSCSI Driver instance.
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+  if (CompareMem (*Target, TargetId, TARGET_MAX_BYTES) == 0) {
+    (*Target)[0] = 0;
+    CopyMem (Lun, ConfigNvData->BootLun, sizeof (UINT64));
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Used to allocate and build a device path node for a SCSI device on a SCSI channel.
+
+  @param[in]      This        A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in]      Target      The Target is an array of size TARGET_MAX_BYTES and it specifies the
+                              Target ID of the SCSI device for which a device path node is to be
+                              allocated and built. Transport drivers may chose to utilize a subset of
+                              this size to suit the representation of targets. For example, a Fibre
+                              Channel driver may use only 8 bytes (WWN) in the array to represent a
+                              FC target.
+  @param[in]       Lun        The LUN of the SCSI device for which a device path node is to be
+                              allocated and built.
+  @param[in, out]  DevicePath A pointer to a single device path node that describes the SCSI device
+                              specified by Target and Lun. This function is responsible for
+                              allocating the buffer DevicePath with the boot service
+                              AllocatePool(). It is the caller's responsibility to free
+                              DevicePath when the caller is finished with DevicePath.
+
+  @retval EFI_SUCCESS           The device path node that describes the SCSI device specified by
+                                Target and Lun was allocated and returned in
+                                DevicePath.
+  @retval EFI_INVALID_PARAMETER DevicePath is NULL.
+  @retval EFI_NOT_FOUND         The SCSI devices specified by Target and Lun does not exist
+                                on the SCSI channel.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources to allocate DevicePath.
+
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruBuildDevicePath (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This,
+  IN UINT8                            *Target,
+  IN UINT64                           Lun,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL     **DevicePath
+  )
+{
+  ISCSI_DRIVER_DATA             *Private;
+  ISCSI_SESSION                 *Session;
+  ISCSI_SESSION_CONFIG_NVDATA   *ConfigNvData;
+  ISCSI_CHAP_AUTH_CONFIG_NVDATA *AuthConfig;
+  EFI_DEV_PATH                  *Node;
+  UINTN                         DevPathNodeLen;
+
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Target[0] != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  Private       = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (This);
+  Session       = &Private->Session;
+  ConfigNvData  = &Session->ConfigData.NvData;
+  AuthConfig    = &Session->AuthData.AuthConfig;
+
+  if (CompareMem (&Lun, ConfigNvData->BootLun, sizeof (UINT64)) != 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  DevPathNodeLen  = sizeof (ISCSI_DEVICE_PATH) + AsciiStrLen (ConfigNvData->TargetName) + 1;
+  Node            = AllocatePool (DevPathNodeLen);
+  if (Node == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Node->DevPath.Type    = MESSAGING_DEVICE_PATH;
+  Node->DevPath.SubType = MSG_ISCSI_DP;
+  SetDevicePathNodeLength (&Node->DevPath, (UINT16)DevPathNodeLen);
+
+  //
+  // 0 for TCP, others are reserved.
+  //
+  Node->Iscsi.NetworkProtocol = 0;
+
+  Node->Iscsi.LoginOption     = 0;
+  switch (AuthConfig->CHAPType) {
+  case ISCSI_CHAP_NONE:
+    Node->Iscsi.LoginOption |= 0x0800;
+    break;
+
+  case ISCSI_CHAP_UNI:
+    Node->Iscsi.LoginOption |= 0x1000;
+    break;
+
+  default:
+    break;
+  }
+
+  CopyMem (&Node->Iscsi.Lun, ConfigNvData->BootLun, sizeof (UINT64));
+  Node->Iscsi.TargetPortalGroupTag = Session->TargetPortalGroupTag;
+  AsciiStrCpyS ((CHAR8 *) Node + sizeof (ISCSI_DEVICE_PATH), AsciiStrLen (ConfigNvData->TargetName) + 1, ConfigNvData->TargetName);
+
+  *DevicePath = (EFI_DEVICE_PATH_PROTOCOL *) Node;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Used to translate a device path node to a Target ID and LUN.
+
+  @param[in]  This       A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in]  DevicePath A pointer to a single device path node that describes the SCSI device
+                         on the SCSI channel.
+  @param[out] Target     A pointer to the Target Array which represents the ID of a SCSI device
+                         on the SCSI channel.
+  @param[out]  Lun       A pointer to the LUN of a SCSI device on the SCSI channel.
+
+  @retval EFI_SUCCESS           DevicePath was successfully translated to a Target ID and
+                                LUN, and they were returned in Target and Lun.
+  @retval EFI_INVALID_PARAMETER DevicePath or Target or Lun is NULL.
+  @retval EFI_NOT_FOUND         A valid translation from DevicePath to a Target ID and LUN
+                                does not exist.Currently not implemented.
+  @retval EFI_UNSUPPORTED       This driver does not support the device path node type in
+                                DevicePath.
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruGetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This,
+  IN EFI_DEVICE_PATH_PROTOCOL         *DevicePath,
+  OUT UINT8                           **Target,
+  OUT UINT64                          *Lun
+  )
+{
+  ISCSI_DRIVER_DATA           *Private;
+  ISCSI_SESSION_CONFIG_NVDATA *ConfigNvData;
+
+  if ((DevicePath == NULL) || (Target == NULL) || (Lun == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((DevicePath->Type != MESSAGING_DEVICE_PATH) ||
+      (DevicePath->SubType != MSG_ISCSI_DP) ||
+      (DevicePathNodeLength (DevicePath) <= sizeof (ISCSI_DEVICE_PATH))
+      ) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Private       = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (This);
+  ConfigNvData  = &Private->Session.ConfigData.NvData;
+
+  SetMem (*Target, TARGET_MAX_BYTES, 0xFF);
+  (*Target)[0] = 0;
+
+  if (AsciiStrCmp (ConfigNvData->TargetName, (CHAR8 *) DevicePath + sizeof (ISCSI_DEVICE_PATH)) != 0) {
+    return EFI_UNSUPPORTED;
+  }
+
+  CopyMem (Lun, ConfigNvData->BootLun, sizeof (UINT64));
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Resets a SCSI channel. This operation resets all the SCSI devices connected to the SCSI channel.
+  Currently not implemented.
+  
+  @param[in]  This A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      The SCSI channel was reset.
+  @retval EFI_DEVICE_ERROR A device error occurred while attempting to reset the SCSI channel.
+  @retval EFI_TIMEOUT      A timeout occurred while attempting to reset the SCSI channel.
+  @retval EFI_UNSUPPORTED  The SCSI channel does not support a channel reset operation.
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruResetChannel (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Resets a SCSI logical unit that is connected to a SCSI channel. Currently not implemented.
+
+  @param[in]  This   A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in]  Target The Target is an array of size TARGET_MAX_BYTE and it represents the
+                     target port ID of the SCSI device containing the SCSI logical unit to
+                     reset. Transport drivers may chose to utilize a subset of this array to suit
+                     the representation of their targets.
+  @param[in]  Lun    The LUN of the SCSI device to reset.
+
+  @retval EFI_SUCCESS           The SCSI device specified by Target and Lun was reset.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           A timeout occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+  @retval EFI_UNSUPPORTED       The SCSI channel does not support a target reset operation.
+  @retval EFI_DEVICE_ERROR      A device error occurred while attempting to reset the SCSI device
+                                specified by Target and Lun.
+
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruResetTargetLun (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This,
+  IN UINT8                            *Target,
+  IN UINT64                           Lun
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Used to retrieve the list of legal Target IDs for SCSI devices on a SCSI channel. These can either     
+  be the list SCSI devices that are actually present on the SCSI channel, or the list of legal Target IDs
+  for the SCSI channel. Regardless, the caller of this function must probe the Target ID returned to     
+  see if a SCSI device is actually present at that location on the SCSI channel.                         
+
+  @param[in]       This    A pointer to the EFI_EXT_SCSI_PASS_THRU_PROTOCOL instance.
+  @param[in, out]  Target  (TARGET_MAX_BYTES) of a SCSI device present on the SCSI channel.
+                           On output, a pointer to the Target ID (an array of
+                           TARGET_MAX_BYTES) of the next SCSI device present on a SCSI
+                           channel. An input value of 0xF(all bytes in the array are 0xF) in the
+                           Target array retrieves the Target ID of the first SCSI device present on a
+                           SCSI channel.
+
+  @retval EFI_SUCCESS           The Target ID of the next SCSI device on the SCSI
+                                channel was returned in Target.
+  @retval EFI_INVALID_PARAMETER Target or Lun is NULL.
+  @retval EFI_TIMEOUT           Target array is not all 0xF, and Target were not
+                                returned on a previous call to GetNextTarget().
+                                Currently not implemented.
+  @retval EFI_NOT_FOUND         There are no more SCSI devices on this SCSI channel.
+**/
+EFI_STATUS
+EFIAPI
+IScsiExtScsiPassThruGetNextTarget (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL  *This,
+  IN OUT UINT8                        **Target
+  )
+{
+  UINT8 TargetId[TARGET_MAX_BYTES];
+
+  SetMem (TargetId, TARGET_MAX_BYTES, 0xFF);
+
+  if (CompareMem (*Target, TargetId, TARGET_MAX_BYTES) == 0) {
+    (*Target)[0] = 0;
+    return EFI_SUCCESS;
+  } else if ((*Target)[0] == 0) {
+    return EFI_NOT_FOUND;
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+}
+
+EFI_EXT_SCSI_PASS_THRU_PROTOCOL gIScsiExtScsiPassThruProtocolTemplate = {
+  NULL,
+  IScsiExtScsiPassThruFunction,
+  IScsiExtScsiPassThruGetNextTargetLun,
+  IScsiExtScsiPassThruBuildDevicePath,
+  IScsiExtScsiPassThruGetTargetLun,
+  IScsiExtScsiPassThruResetChannel,
+  IScsiExtScsiPassThruResetTargetLun,
+  IScsiExtScsiPassThruGetNextTarget
+};
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.h
new file mode 100644
index 0000000000..e635e47b92
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiExtScsiPassThru.h
@@ -0,0 +1,22 @@
+/** @file
+  The header file of IScsiExtScsiPassThru.c.
+
+Copyright (c) 2004 - 2008, 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.
+
+**/
+
+#ifndef _ISCSI_EXT_SCSI_PASS_THRU_H_
+#define _ISCSI_EXT_SCSI_PASS_THRU_H_
+
+#include <Protocol/ScsiPassThruExt.h>
+
+extern EFI_EXT_SCSI_PASS_THRU_PROTOCOL  gIScsiExtScsiPassThruProtocolTemplate;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.c
new file mode 100644
index 0000000000..372efdc15e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.c
@@ -0,0 +1,534 @@
+/** @file
+  Implementation for iSCSI Boot Firmware Table publication.
+
+Copyright (c) 2004 - 2013, 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 "IScsiImpl.h"
+
+BOOLEAN mIbftInstalled = FALSE;
+UINTN   mTableKey;
+
+/**
+  Initialize the header of the iSCSI Boot Firmware Table.
+  
+  @param[out]  Header     The header of the iSCSI Boot Firmware Table.
+  @param[in]   OemId      The OEM ID.
+  @param[in]   OemTableId The OEM table ID for the iBFT.
+**/
+VOID
+IScsiInitIbfTableHeader (
+  OUT EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER   *Header,
+  IN  UINT8                                       *OemId,
+  IN  UINT64                                      *OemTableId
+  )
+{
+  ZeroMem (Header, sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER));
+
+  Header->Signature = EFI_ACPI_3_0_ISCSI_BOOT_FIRMWARE_TABLE_SIGNATURE;
+  Header->Length    = IBFT_HEAP_OFFSET;
+  Header->Revision  = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_REVISION;
+  Header->Checksum  = 0;
+  
+  CopyMem (Header->OemId, OemId, sizeof (Header->OemId));
+  CopyMem (&Header->OemTableId, OemTableId, sizeof (UINT64));
+}
+
+/**
+  Initialize the control section of the iSCSI Boot Firmware Table.
+  
+  @param[in]  Table       The ACPI table.
+  @param[in]  HandleCount The number of the handles associated with iSCSI sessions, it's
+                          equal to the number of iSCSI sessions.
+**/
+VOID
+IScsiInitControlSection (
+  IN EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER  *Table,
+  IN UINTN                                      HandleCount
+  )
+{
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE  *Control;
+  UINTN                                                 NumOffset;
+
+  Control = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE *) (Table + 1);
+
+  ZeroMem (Control, sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE));
+
+  Control->Header.StructureId = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE_ID;
+  Control->Header.Version     = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE_VERSION;
+  Control->Header.Length      = (UINT16) sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE);
+
+  //
+  // Each session occupies two offsets, one for the NIC section,
+  // the other for the Target section.
+  //
+  NumOffset = 2 * HandleCount;
+  if (NumOffset > 4) {
+    //
+    // Need expand the control section if more than 2 NIC/Target sections
+    // exist.
+    //
+    Control->Header.Length = (UINT16) (Control->Header.Length + (NumOffset - 4) * sizeof (UINT16));
+  }
+}
+
+/**
+  Add one item into the heap.
+
+  @param[in, out]  Heap  On input, the current address of the heap; On output, the address of
+                         the heap after the item is added.
+  @param[in]       Data  The data to add into the heap.
+  @param[in]       Len   Length of the Data in byte.
+**/
+VOID
+IScsiAddHeapItem (
+  IN OUT UINT8  **Heap,
+  IN     VOID   *Data,
+  IN     UINTN  Len
+  )
+{
+  //
+  // Add one byte for the NULL delimiter.
+  //
+  *Heap -= Len + 1;
+
+  CopyMem (*Heap, Data, Len);
+  *(*Heap + Len) = 0;
+}
+
+/**
+  Fill the Initiator section of the iSCSI Boot Firmware Table.
+
+  @param[in]       Table    The ACPI table.
+  @param[in, out]  Heap     The heap.
+  @param[in]       Handle   The handle associated with the iSCSI session.
+**/
+VOID
+IScsiFillInitiatorSection (
+  IN     EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER  *Table,
+  IN OUT UINT8                                      **Heap,
+  IN     EFI_HANDLE                                 Handle
+  )
+{
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE    *Control;
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE  *Initiator;
+  ISCSI_DRIVER_DATA                                       *DriverData;
+  ISCSI_SESSION                                           *Session;
+  ISCSI_PRIVATE_PROTOCOL                                  *IScsiIdentifier;
+  EFI_STATUS                                              Status;
+
+  Control = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE *) (Table + 1);
+
+  //
+  // Initiator section immediately follows the control section.
+  //
+  Initiator = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE *) ((UINT8 *) Control + IBFT_ROUNDUP (Control->Header.Length));
+
+  Control->InitiatorOffset = (UINT16) ((UINTN) Initiator - (UINTN) Table);
+
+  ZeroMem (Initiator, sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE));
+
+  Initiator->Header.StructureId = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE_ID;
+  Initiator->Header.Version     = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE_VERSION;
+  Initiator->Header.Length      = (UINT16) sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE);
+  Initiator->Header.Flags       = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE_FLAG_BLOCK_VALID | EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE_FLAG_BOOT_SELECTED;
+
+  //
+  // Get the identifier from the handle.
+  //
+  Status = gBS->HandleProtocol (Handle, &gEfiCallerIdGuid, (VOID **) &IScsiIdentifier);
+  if (EFI_ERROR (Status)) {
+    ASSERT (FALSE);
+    return ;
+  }
+
+  DriverData  = ISCSI_DRIVER_DATA_FROM_IDENTIFIER (IScsiIdentifier);
+  Session     = &DriverData->Session;
+
+  //
+  // Fill the iSCSI Initiator Name into the heap.
+  //
+  IScsiAddHeapItem (Heap, Session->InitiatorName, Session->InitiatorNameLength - 1);
+
+  Initiator->IScsiNameLength  = (UINT16) (Session->InitiatorNameLength - 1);
+  Initiator->IScsiNameOffset  = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+}
+
+/**
+  Map the v4 IP address into v6 IP address.
+
+  @param[in]   V4 The v4 IP address.
+  @param[out]  V6 The v6 IP address.
+**/
+VOID
+IScsiMapV4ToV6Addr (
+  IN  EFI_IPv4_ADDRESS *V4,
+  OUT EFI_IPv6_ADDRESS *V6
+  )
+{
+  UINTN Index;
+
+  ZeroMem (V6, sizeof (EFI_IPv6_ADDRESS));
+
+  V6->Addr[10]  = 0xff;
+  V6->Addr[11]  = 0xff;
+
+  for (Index = 0; Index < 4; Index++) {
+    V6->Addr[12 + Index] = V4->Addr[Index];
+  }
+}
+
+/**
+  Get the NIC's PCI location and return it accroding to the composited
+  format defined in iSCSI Boot Firmware Table.
+
+  @param[in]  Controller  The handle of the controller.
+
+  @return UINT16          The composited representation of the NIC PCI location.
+  @retval 0               Other errors as indicated.
+**/
+UINT16
+IScsiGetNICPciLocation (
+  IN EFI_HANDLE  Controller
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_HANDLE                PciIoHandle;
+  EFI_PCI_IO_PROTOCOL       *PciIo;
+  UINTN                     Segment;
+  UINTN                     Bus;
+  UINTN                     Device;
+  UINTN                     Function;
+
+  Status = gBS->HandleProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **)&DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  Status = gBS->LocateDevicePath (
+                  &gEfiPciIoProtocolGuid,
+                  &DevicePath,
+                  &PciIoHandle
+                  );
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  Status = gBS->HandleProtocol (PciIoHandle, &gEfiPciIoProtocolGuid, (VOID **)&PciIo);
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  Status = PciIo->GetLocation (PciIo, &Segment, &Bus, &Device, &Function);
+  if (EFI_ERROR (Status)) {
+    return 0;
+  }
+
+  return (UINT16) ((Bus << 8) | (Device << 3) | Function);
+}
+
+/**
+  Fill the NIC and target sections in iSCSI Boot Firmware Table.
+
+  @param[in]       Table       The buffer of the ACPI table.
+  @param[in, out]  Heap        The heap buffer used to store the variable length parameters such as iSCSI name.
+  @param[in]       HandleCount Count The number of handles having iSCSI private protocol installed.
+  @param[in]       Handles     The handle buffer.
+**/
+VOID
+IScsiFillNICAndTargetSections (
+  IN     EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER  *Table,
+  IN OUT UINT8                                      **Heap,
+  IN     UINTN                                      HandleCount,
+  IN     EFI_HANDLE                                 *Handles
+  )
+{
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE  *Control;
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE      *Nic;
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE   *Target;
+  ISCSI_DRIVER_DATA                                     *DriverData;
+  ISCSI_SESSION_CONFIG_DATA                             *SessionConfigData;
+  ISCSI_CHAP_AUTH_CONFIG_NVDATA                         *AuthConfig;
+  UINT16                                                *SectionOffset;
+  UINTN                                                 Index;
+  UINT16                                                Length;
+  EFI_MAC_ADDRESS                                       MacAddress;
+  UINTN                                                 HwAddressSize;
+  ISCSI_PRIVATE_PROTOCOL                                *IScsiIdentifier;
+  EFI_STATUS                                            Status;
+
+  //
+  // Get the offset of the first Nic and Target section.
+  //
+  Control = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_CONTROL_STRUCTURE *) (Table + 1);
+  Nic     = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE *) ((UINTN) Table +
+          Control->InitiatorOffset + IBFT_ROUNDUP (sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_INITIATOR_STRUCTURE)));
+  Target  = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE *) ((UINTN) Nic +
+          IBFT_ROUNDUP (sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE)));
+
+  SectionOffset = &Control->NIC0Offset;
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    Status = gBS->HandleProtocol (Handles[Index], &gEfiCallerIdGuid, (VOID **)&IScsiIdentifier);
+    if (EFI_ERROR (Status)) {
+      ASSERT (FALSE);
+      return ;
+    }
+
+    DriverData        = ISCSI_DRIVER_DATA_FROM_IDENTIFIER (IScsiIdentifier);
+    SessionConfigData = &DriverData->Session.ConfigData;
+    AuthConfig        = &DriverData->Session.AuthData.AuthConfig;
+
+    //
+    // Fill the Nic section.
+    //
+    ZeroMem (Nic, sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE));
+
+    Nic->Header.StructureId = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE_ID;
+    Nic->Header.Version     = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE_VERSION;
+    Nic->Header.Length      = (UINT16) sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE);
+    Nic->Header.Index       = (UINT8) Index;
+    Nic->Header.Flags       = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE_FLAG_BLOCK_VALID |
+                            EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE_FLAG_BOOT_SELECTED |
+                            EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE_FLAG_GLOBAL;
+
+    //
+    // Get the subnet mask prefix length.
+    //
+    Nic->SubnetMaskPrefixLength = IScsiGetSubnetMaskPrefixLength (&SessionConfigData->NvData.SubnetMask);
+
+    if (SessionConfigData->NvData.InitiatorInfoFromDhcp) {
+      Nic->Origin = IpPrefixOriginDhcp;
+    } else {
+      Nic->Origin = IpPrefixOriginManual;
+    }
+    //
+    // Map the various v4 addresses into v6 addresses.
+    //
+    IScsiMapV4ToV6Addr (&SessionConfigData->NvData.LocalIp, &Nic->Ip);
+    IScsiMapV4ToV6Addr (&SessionConfigData->NvData.Gateway, &Nic->Gateway);
+    IScsiMapV4ToV6Addr (&SessionConfigData->PrimaryDns, &Nic->PrimaryDns);
+    IScsiMapV4ToV6Addr (&SessionConfigData->SecondaryDns, &Nic->SecondaryDns);
+    IScsiMapV4ToV6Addr (&SessionConfigData->DhcpServer, &Nic->DhcpServer);
+
+    Nic->VLanTag = NetLibGetVlanId (DriverData->Controller);
+
+    Status = NetLibGetMacAddress (DriverData->Controller, &MacAddress, &HwAddressSize);
+    ASSERT (Status == EFI_SUCCESS);
+    CopyMem (Nic->Mac, MacAddress.Addr, sizeof (Nic->Mac));
+
+    //
+    // Get the PCI location of the Nic.
+    //
+    Nic->PciLocation  = IScsiGetNICPciLocation (DriverData->Controller);
+
+    *SectionOffset    = (UINT16) ((UINTN) Nic - (UINTN) Table);
+    SectionOffset++;
+
+    //
+    // Fill the Target section.
+    //
+    ZeroMem (Target, sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE));
+
+    Target->Header.StructureId  = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_ID;
+    Target->Header.Version      = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_VERSION;
+    Target->Header.Length       = (UINT16) sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE);
+    Target->Header.Index        = (UINT8) Index;
+    Target->Header.Flags        = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_FLAG_BLOCK_VALID | EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_FLAG_BOOT_SELECTED;
+    Target->Port                = SessionConfigData->NvData.TargetPort;
+    Target->NicIndex            = (UINT8) Index;
+
+    if (AuthConfig->CHAPType == ISCSI_CHAP_NONE) {
+      Target->CHAPType = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_CHAP_TYPE_NO_CHAP;
+    } if (AuthConfig->CHAPType == ISCSI_CHAP_UNI) {
+      Target->CHAPType = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_CHAP_TYPE_CHAP;
+    } else if (AuthConfig->CHAPType == ISCSI_CHAP_MUTUAL) {
+      Target->CHAPType = EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_CHAP_TYPE_MUTUAL_CHAP;
+    }
+
+    IScsiMapV4ToV6Addr (&SessionConfigData->NvData.TargetIp, &Target->Ip);
+    CopyMem (Target->BootLun, SessionConfigData->NvData.BootLun, sizeof (Target->BootLun));
+
+    //
+    // Target iSCSI Name, CHAP name/secret, reverse CHAP name/secret.
+    //
+    Length = (UINT16) AsciiStrLen (SessionConfigData->NvData.TargetName);
+    IScsiAddHeapItem (Heap, SessionConfigData->NvData.TargetName, Length);
+
+    Target->IScsiNameLength = Length;
+    Target->IScsiNameOffset = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+
+    if (Target->CHAPType != EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_CHAP_TYPE_NO_CHAP) {
+      //
+      // CHAP Name
+      //
+      Length = (UINT16) AsciiStrLen (AuthConfig->CHAPName);
+      IScsiAddHeapItem (Heap, AuthConfig->CHAPName, Length);
+      Target->CHAPNameLength  = Length;
+      Target->CHAPNameOffset  = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+
+      //
+      // CHAP Secret
+      //
+      Length = (UINT16) AsciiStrLen (AuthConfig->CHAPSecret);
+      IScsiAddHeapItem (Heap, AuthConfig->CHAPSecret, Length);
+      Target->CHAPSecretLength  = Length;
+      Target->CHAPSecretOffset  = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+
+      if (Target->CHAPType == EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE_CHAP_TYPE_MUTUAL_CHAP) {
+        //
+        // Reverse CHAP Name
+        //
+        Length = (UINT16) AsciiStrLen (AuthConfig->ReverseCHAPName);
+        IScsiAddHeapItem (Heap, AuthConfig->ReverseCHAPName, Length);
+        Target->ReverseCHAPNameLength = Length;
+        Target->ReverseCHAPNameOffset = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+
+        //
+        // Reverse CHAP Secret
+        //
+        Length = (UINT16) AsciiStrLen (AuthConfig->ReverseCHAPSecret);
+        IScsiAddHeapItem (Heap, AuthConfig->ReverseCHAPSecret, Length);
+        Target->ReverseCHAPSecretLength = Length;
+        Target->ReverseCHAPSecretOffset = (UINT16) ((UINTN) *Heap - (UINTN) Table);
+      }
+    }
+
+    *SectionOffset = (UINT16) ((UINTN) Target - (UINTN) Table);
+    SectionOffset++;
+
+    //
+    // Advance to the next NIC/Target pair
+    //
+    Nic    = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE *) ((UINTN) Target +
+           IBFT_ROUNDUP (sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE)));
+    Target = (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_TARGET_STRUCTURE *) ((UINTN) Nic +
+           IBFT_ROUNDUP (sizeof (EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_NIC_STRUCTURE)));
+  }
+}
+
+/**
+  Publish and remove the iSCSI Boot Firmware Table according to the iSCSI
+  session status.
+**/
+VOID
+IScsiPublishIbft (
+  VOID
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_ACPI_TABLE_PROTOCOL                   *AcpiTableProtocol;
+  EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_HEADER *Table;
+  UINTN                                     HandleCount;
+  EFI_HANDLE                                *HandleBuffer;
+  UINT8                                     *Heap;
+  UINT8                                     Checksum;
+  UINTN                                         Index;
+  EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER  *Rsdp;
+  EFI_ACPI_DESCRIPTION_HEADER                   *Rsdt;
+
+  Status = gBS->LocateProtocol (&gEfiAcpiTableProtocolGuid, NULL, (VOID **)&AcpiTableProtocol);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+
+  //
+  // Find ACPI table RSD_PTR from system table
+  //
+  for (Index = 0, Rsdp = NULL; Index < gST->NumberOfTableEntries; Index++) {
+    if (CompareGuid (&(gST->ConfigurationTable[Index].VendorGuid), &gEfiAcpi20TableGuid) ||
+      CompareGuid (&(gST->ConfigurationTable[Index].VendorGuid), &gEfiAcpi10TableGuid) ||
+      CompareGuid (&(gST->ConfigurationTable[Index].VendorGuid), &gEfiAcpiTableGuid)
+      ) {
+      //
+      // A match was found.
+      //
+      Rsdp = (EFI_ACPI_3_0_ROOT_SYSTEM_DESCRIPTION_POINTER *) gST->ConfigurationTable[Index].VendorTable;
+      break;
+    }
+  }
+
+  if (Rsdp == NULL) {
+    return ;
+  } else {
+    Rsdt = (EFI_ACPI_DESCRIPTION_HEADER *) (UINTN) Rsdp->RsdtAddress;
+  }
+
+
+  if (mIbftInstalled) {
+    Status = AcpiTableProtocol->UninstallAcpiTable (
+                                  AcpiTableProtocol,
+                                  mTableKey
+                                  );
+    if (EFI_ERROR (Status)) {
+      return ;
+    }
+    mIbftInstalled = FALSE;
+  }
+
+  //
+  // Get all iSCSI private protocols.
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiCallerIdGuid,
+                  NULL,
+                  &HandleCount,
+                  &HandleBuffer
+                  );
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+  //
+  // Allocate 4k bytes to hold the ACPI table.
+  //
+  Table = AllocateZeroPool (IBFT_MAX_SIZE);
+  if (Table == NULL) {
+    return ;
+  }
+
+  Heap = (UINT8 *) Table + IBFT_HEAP_OFFSET;
+
+  //
+  // Fill in the various section of the iSCSI Boot Firmware Table.
+  //
+  IScsiInitIbfTableHeader (Table, Rsdt->OemId, &Rsdt->OemTableId);
+  IScsiInitControlSection (Table, HandleCount);
+  IScsiFillInitiatorSection (Table, &Heap, HandleBuffer[0]);
+  IScsiFillNICAndTargetSections (Table, &Heap, HandleCount, HandleBuffer);
+
+  Checksum = CalculateCheckSum8((UINT8 *)Table, Table->Length);
+  Table->Checksum = Checksum;
+
+  FreePool (HandleBuffer);
+
+  //
+  // Install or update the iBFT table.
+  //
+  Status = AcpiTableProtocol->InstallAcpiTable (
+                                AcpiTableProtocol,
+                                Table,
+                                Table->Length,
+                                &mTableKey
+                                );
+  if (EFI_ERROR(Status)) {
+    return;
+  }
+
+  mIbftInstalled = TRUE;
+  FreePool (Table);
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.h
new file mode 100644
index 0000000000..4f3828db63
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiIbft.h
@@ -0,0 +1,38 @@
+/** @file
+  Some extra definitions for iBFT.
+
+Copyright (c) 2004 - 2009, 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.
+
+**/
+
+#ifndef _ISCSI_IBFT_H_
+#define _ISCSI_IBFT_H_
+
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/IScsiBootFirmwareTable.h>
+#include <Protocol/AcpiTable.h>
+#include <Protocol/PciIo.h>
+
+#define IBFT_TABLE_VAR_NAME L"iBFT"
+#define IBFT_MAX_SIZE       4096
+#define IBFT_HEAP_OFFSET    2048
+
+#define IBFT_ROUNDUP(size)  NET_ROUNDUP ((size), EFI_ACPI_ISCSI_BOOT_FIRMWARE_TABLE_STRUCTURE_ALIGNMENT)
+
+/**
+  Publish and remove the iSCSI Boot Firmware Table according to the iSCSI
+  session status.
+**/
+VOID
+IScsiPublishIbft (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiImpl.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiImpl.h
new file mode 100644
index 0000000000..8c761f882c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiImpl.h
@@ -0,0 +1,167 @@
+/** @file
+  The header file of IScsiImpl.c.
+
+Copyright (c) 2004 - 2010, 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.
+
+**/
+
+#ifndef _ISCSI_IMPL_H_
+#define _ISCSI_IMPL_H_
+
+#include <Uefi.h>
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/NetLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+
+#include <Guid/EventGroup.h>
+#include <Guid/Acpi.h>
+
+#include "IScsiCommon.h"
+#include "IScsiDriver.h"
+#include "IScsiInitiatorName.h"
+#include "ComponentName.h"
+#include "IScsiConfigNVDataStruc.h"
+#include "IScsiExtScsiPassThru.h"
+#include "IScsiProto.h"
+#include "IScsiMisc.h"
+#include "IScsiCHAP.h"
+#include "IScsiConfig.h"
+#include "IScsiDhcp.h"
+#include "IScsiTcp4Io.h"
+#include "IScsiIbft.h"
+
+
+#define ISCSI_SESSION_SIGNATURE SIGNATURE_32 ('I', 'S', 'S', 'N')
+
+struct _ISCSI_SESSION {
+  UINT32                    Signature;
+
+  ISCSI_SESSION_CONFIG_DATA ConfigData;
+  ISCSI_CHAP_AUTH_DATA      AuthData;
+
+  CHAR8                     InitiatorName[ISCSI_NAME_MAX_SIZE];
+  UINTN                     InitiatorNameLength;
+  UINT8                     State;
+
+  UINT8                     Isid[6];
+  UINT16                    Tsih;
+
+  UINT32                    CmdSN;
+  UINT32                    ExpCmdSN;
+  UINT32                    MaxCmdSN;
+
+  UINT32                    InitiatorTaskTag;
+  UINT16                    NextCid;
+
+  LIST_ENTRY                Conns;
+  UINT32                    NumConns;
+
+  LIST_ENTRY                TcbList;
+
+  //
+  // session-wide parameters
+  //
+  UINT16                    TargetPortalGroupTag;
+  UINT32                    MaxConnections;
+  BOOLEAN                   InitialR2T;
+  BOOLEAN                   ImmediateData;
+  UINT32                    MaxBurstLength;
+  UINT32                    FirstBurstLength;
+  UINT32                    DefaultTime2Wait;
+  UINT32                    DefaultTime2Retain;
+  UINT16                    MaxOutstandingR2T;
+  BOOLEAN                   DataPDUInOrder;
+  BOOLEAN                   DataSequenceInOrder;
+  UINT8                     ErrorRecoveryLevel;
+};
+
+#define ISCSI_CONNECTION_SIGNATURE  SIGNATURE_32 ('I', 'S', 'C', 'N')
+
+struct _ISCSI_CONNECTION {
+  UINT32            Signature;
+  LIST_ENTRY        Link;
+
+  EFI_EVENT         TimeoutEvent;
+
+  ISCSI_SESSION     *Session;
+
+  UINT8             State;
+  UINT8             CurrentStage;
+  UINT8             NextStage;
+
+  UINT8             CHAPStep;
+
+  BOOLEAN           PartialReqSent;
+  BOOLEAN           PartialRspRcvd;
+
+  BOOLEAN           TransitInitiated;
+
+  UINT16            Cid;
+  UINT32            ExpStatSN;
+
+  //
+  // queues...
+  //
+  NET_BUF_QUEUE     RspQue;
+
+  TCP4_IO           Tcp4Io;
+
+  //
+  // connection-only parameters
+  //
+  UINT32            MaxRecvDataSegmentLength;
+  ISCSI_DIGEST_TYPE HeaderDigest;
+  ISCSI_DIGEST_TYPE DataDigest;
+};
+
+#define ISCSI_DRIVER_DATA_SIGNATURE SIGNATURE_32 ('I', 'S', 'D', 'A')
+
+#define ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU(PassThru) \
+  CR ( \
+  PassThru, \
+  ISCSI_DRIVER_DATA, \
+  IScsiExtScsiPassThru, \
+  ISCSI_DRIVER_DATA_SIGNATURE \
+  )
+#define ISCSI_DRIVER_DATA_FROM_IDENTIFIER(Identifier) \
+  CR ( \
+  Identifier, \
+  ISCSI_DRIVER_DATA, \
+  IScsiIdentifier, \
+  ISCSI_DRIVER_DATA_SIGNATURE \
+  )
+#define ISCSI_DRIVER_DATA_FROM_SESSION(s) \
+  CR ( \
+  s, \
+  ISCSI_DRIVER_DATA, \
+  Session, \
+  ISCSI_DRIVER_DATA_SIGNATURE \
+  )
+
+struct _ISCSI_DRIVER_DATA {
+  UINT32                          Signature;
+  EFI_HANDLE                      Image;
+  EFI_HANDLE                      Controller;
+  ISCSI_PRIVATE_PROTOCOL          IScsiIdentifier;
+  EFI_HANDLE                      ChildHandle;
+  EFI_EVENT                       ExitBootServiceEvent;
+
+  EFI_EXT_SCSI_PASS_THRU_PROTOCOL IScsiExtScsiPassThru;
+  EFI_EXT_SCSI_PASS_THRU_MODE     ExtScsiPassThruMode;
+  EFI_HANDLE                      ExtScsiPassThruHandle;
+  EFI_DEVICE_PATH_PROTOCOL        *DevicePath;
+
+  ISCSI_SESSION                   Session;
+};
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.c
new file mode 100644
index 0000000000..e753d9eebd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.c
@@ -0,0 +1,116 @@
+/** @file
+  Implementation for EFI iSCSI Initiator Name Protocol.
+
+Copyright (c) 2004 - 2011, 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 "IScsiImpl.h"
+
+EFI_ISCSI_INITIATOR_NAME_PROTOCOL gIScsiInitiatorName = {
+  IScsiGetInitiatorName,
+  IScsiSetInitiatorName
+};
+
+/**
+  Retrieves the current set value of iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer / Actual size of the
+                              variable data buffer.
+  @param[out]      Buffer     Pointer to the buffer for data to be read. The data is a null-terminated UTF-8 encoded string.
+                              The maximum length is 223 characters, including the null-terminator.
+
+  @retval EFI_SUCCESS           Data was successfully retrieved into the provided buffer and the
+                                BufferSize was sufficient to handle the iSCSI initiator name.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the result.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL.
+  @retval EFI_DEVICE_ERROR      The iSCSI initiator name could not be retrieved due to a hardware error.
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiGetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  OUT    VOID                               *Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  if ((BufferSize == NULL) || (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = gRT->GetVariable (
+                  ISCSI_INITIATOR_NAME_VAR_NAME,
+                  &gEfiIScsiInitiatorNameProtocolGuid,
+                  NULL,
+                  BufferSize,
+                  Buffer
+                  );
+
+  return Status;
+}
+
+/**
+  Sets the iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer.
+  @param[in]       Buffer     Pointer to the buffer for data to be written. The data is a null-terminated UTF-8 encoded string.
+                              The maximum length is 223 characters, including the null-terminator.
+
+  @retval EFI_SUCCESS           Data was successfully stored by the protocol.
+  @retval EFI_UNSUPPORTED       Platform policies do not allow for data to be written.
+                                Currently not implemented.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL, or BufferSize exceeds the maximum allowed limit.
+  @retval EFI_DEVICE_ERROR      The data could not be stored due to a hardware error.
+  @retval EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the data.
+  @retval EFI_PROTOCOL_ERROR    Input iSCSI initiator name does not adhere to RFC 3720
+                                (and other related protocols).
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiSetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  IN     VOID                               *Buffer
+  )
+{
+  EFI_STATUS  Status;
+
+  if ((BufferSize == NULL) || (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*BufferSize > ISCSI_NAME_MAX_SIZE) {
+    *BufferSize = ISCSI_NAME_MAX_SIZE;
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // only support iqn iSCSI names.
+  //
+  Status = IScsiNormalizeName ((CHAR8 *) Buffer, *BufferSize - 1);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gRT->SetVariable (
+                  ISCSI_INITIATOR_NAME_VAR_NAME,
+                  &gEfiIScsiInitiatorNameProtocolGuid,
+                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                  *BufferSize,
+                  Buffer
+                  );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.h
new file mode 100644
index 0000000000..f967fb3830
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiInitiatorName.h
@@ -0,0 +1,74 @@
+/** @file
+  The header file for EFI iSCSI Initiator Name Protocol.
+
+Copyright (c) 2004 - 2008, 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.
+
+**/
+
+#ifndef _ISCSI_INITIATOR_NAME_H_
+#define _ISCSI_INITIATOR_NAME_H_
+
+#include <Protocol/IScsiInitiatorName.h>
+
+extern EFI_ISCSI_INITIATOR_NAME_PROTOCOL  gIScsiInitiatorName;
+
+//
+// EFI iSCSI Initiator Name Protocol for IScsi driver.
+//
+
+/**
+  Retrieves the current set value of iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer / Actual size of the
+                              variable data buffer.
+  @param[out]      Buffer     Pointer to the buffer for data to be read.
+
+  @retval EFI_SUCCESS           Data was successfully retrieved into the provided buffer and the
+                                BufferSize was sufficient to handle the iSCSI initiator name.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the result.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL.
+  @retval EFI_DEVICE_ERROR      The iSCSI initiator name could not be retrieved due to a hardware error.
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiGetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  OUT    VOID                               *Buffer
+  );
+
+/**
+  Sets the iSCSI Initiator Name.
+
+  @param[in]       This       Pointer to the EFI_ISCSI_INITIATOR_NAME_PROTOCOL instance.
+  @param[in, out]  BufferSize Size of the buffer in bytes pointed to by Buffer.
+  @param[in]       Buffer     Pointer to the buffer for data to be written.
+
+  @retval EFI_SUCCESS           Data was successfully stored by the protocol.
+  @retval EFI_UNSUPPORTED       Platform policies do not allow for data to be written.
+                                Currently not implemented.
+  @retval EFI_INVALID_PARAMETER BufferSize or Buffer is NULL, or BufferSize exceeds the maximum allowed limit.
+  @retval EFI_DEVICE_ERROR      The data could not be stored due to a hardware error.
+  @retval EFI_OUT_OF_RESOURCES  Not enough storage is available to hold the data.
+  @retval EFI_PROTOCOL_ERROR    Input iSCSI initiator name does not adhere to RFC 3720
+                                (and other related protocols).
+  @retval Others                Other errors as indicated.
+**/
+EFI_STATUS
+EFIAPI
+IScsiSetInitiatorName (
+  IN     EFI_ISCSI_INITIATOR_NAME_PROTOCOL  *This,
+  IN OUT UINTN                              *BufferSize,
+  IN     VOID                               *Buffer
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.c
new file mode 100644
index 0000000000..ebd9f5b40d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.c
@@ -0,0 +1,931 @@
+/** @file
+  Miscellaneous routines for iSCSI driver.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+GLOBAL_REMOVE_IF_UNREFERENCED CONST CHAR8  IScsiHexString[] = "0123456789ABCDEFabcdef";
+
+/**
+  Removes (trims) specified leading and trailing characters from a string.
+
+  @param[in, out]  Str  Pointer to the null-terminated string to be trimmed. On return, 
+                        Str will hold the trimmed string. 
+
+  @param[in]      CharC Character will be trimmed from str.
+**/
+VOID
+StrTrim (
+  IN OUT CHAR16   *Str,
+  IN     CHAR16   CharC
+  )
+{
+  CHAR16  *Pointer1;
+  CHAR16  *Pointer2;
+  
+  if (*Str == 0) {
+    return;
+  }
+  
+  //
+  // Trim off the leading and trailing characters c
+  //
+  for (Pointer1 = Str; (*Pointer1 != 0) && (*Pointer1 == CharC); Pointer1++) {
+    ;
+  }
+  
+  Pointer2 = Str;
+  if (Pointer2 == Pointer1) {
+    while (*Pointer1 != 0) {
+      Pointer2++;
+      Pointer1++;
+    }
+  } else {
+    while (*Pointer1 != 0) {    
+    *Pointer2 = *Pointer1;    
+    Pointer1++;
+    Pointer2++;
+    }
+    *Pointer2 = 0;
+  }
+  
+  
+  for (Pointer1 = Str + StrLen(Str) - 1; Pointer1 >= Str && *Pointer1 == CharC; Pointer1--) {
+    ;
+  }
+  if  (Pointer1 !=  Str + StrLen(Str) - 1) { 
+    *(Pointer1 + 1) = 0;
+  }
+}
+
+/**
+  Calculate the prefix length of the IPv4 subnet mask.
+
+  @param[in]  SubnetMask The IPv4 subnet mask.
+
+  @return The prefix length of the subnet mask.
+  @retval 0 Other errors as indicated.
+**/
+UINT8
+IScsiGetSubnetMaskPrefixLength (
+  IN EFI_IPv4_ADDRESS  *SubnetMask
+  )
+{
+  UINT8   Len;
+  UINT32  ReverseMask;
+
+  //
+  // The SubnetMask is in network byte order.
+  //
+  ReverseMask = (SubnetMask->Addr[0] << 24) | (SubnetMask->Addr[1] << 16) | (SubnetMask->Addr[2] << 8) | (SubnetMask->Addr[3]);
+
+  //
+  // Reverse it.
+  //
+  ReverseMask = ~ReverseMask;
+
+  if ((ReverseMask & (ReverseMask + 1)) != 0) {
+    return 0;
+  }
+
+  Len = 0;
+
+  while (ReverseMask != 0) {
+    ReverseMask = ReverseMask >> 1;
+    Len++;
+  }
+
+  return (UINT8) (32 - Len);
+}
+
+/**
+  Convert the hexadecimal encoded LUN string into the 64-bit LUN. 
+
+  @param[in]   Str             The hexadecimal encoded LUN string.
+  @param[out]  Lun             Storage to return the 64-bit LUN.
+
+  @retval EFI_SUCCESS           The 64-bit LUN is stored in Lun.
+  @retval EFI_INVALID_PARAMETER The string is malformatted.
+**/
+EFI_STATUS
+IScsiAsciiStrToLun (
+  IN  CHAR8  *Str,
+  OUT UINT8  *Lun
+  )
+{
+  UINTN   Index, IndexValue, IndexNum, SizeStr;
+  CHAR8   TemStr[2];
+  UINT8   TemValue;
+  UINT16  Value[4];
+  
+  ZeroMem (Lun, 8);
+  ZeroMem (TemStr, 2);
+  ZeroMem ((UINT8 *) Value, sizeof (Value));
+  SizeStr    = AsciiStrLen (Str);  
+  IndexValue = 0;
+  IndexNum   = 0;
+
+  for (Index = 0; Index < SizeStr; Index ++) {
+    TemStr[0] = Str[Index];
+    TemValue = (UINT8) AsciiStrHexToUint64 (TemStr);
+    if (TemValue == 0 && TemStr[0] != '0') {
+      if ((TemStr[0] != '-') || (IndexNum == 0)) {
+        //
+        // Invalid Lun Char
+        //
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+    
+    if ((TemValue == 0) && (TemStr[0] == '-')) {
+      //
+      // Next Lun value
+      //
+      if (++IndexValue >= 4) {
+        //
+        // Max 4 Lun value
+        //
+        return EFI_INVALID_PARAMETER;
+      }
+      //
+      // Restart str index for the next lun value
+      //
+      IndexNum = 0;
+      continue;
+    }
+    
+    if (++IndexNum > 4) {
+      //     
+      // Each Lun Str can't exceed size 4, because it will be as UINT16 value
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    //
+    // Combine UINT16 value
+    //
+    Value[IndexValue] = (UINT16) ((Value[IndexValue] << 4) + TemValue);
+  }
+ 
+  for (Index = 0; Index <= IndexValue; Index ++) {
+    *((UINT16 *) &Lun[Index * 2]) =  HTONS (Value[Index]);
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert the 64-bit LUN into the hexadecimal encoded LUN string.
+
+  @param[in]   Lun The 64-bit LUN.
+  @param[out]  Str The storage to return the hexadecimal encoded LUN string.
+**/
+VOID
+IScsiLunToUnicodeStr (
+  IN UINT8    *Lun,
+  OUT CHAR16  *Str
+  )
+{
+  UINTN   Index;
+  CHAR16  *TempStr;
+
+  TempStr = Str;
+
+  for (Index = 0; Index < 4; Index++) {
+
+    if ((Lun[2 * Index] | Lun[2 * Index + 1]) == 0) {
+      CopyMem(TempStr, L"0-", sizeof (L"0-"));
+    } else {
+      TempStr[0]  = (CHAR16) IScsiHexString[Lun[2 * Index] >> 4];
+      TempStr[1]  = (CHAR16) IScsiHexString[Lun[2 * Index] & 0x0F];
+      TempStr[2]  = (CHAR16) IScsiHexString[Lun[2 * Index + 1] >> 4];
+      TempStr[3]  = (CHAR16) IScsiHexString[Lun[2 * Index + 1] & 0x0F];
+      TempStr[4]  = L'-';
+      TempStr[5]  = 0;
+
+      StrTrim (TempStr, L'0');
+    }
+
+    TempStr += StrLen (TempStr);
+  }
+
+  ASSERT (StrLen(Str) >= 1);
+  Str[StrLen (Str) - 1] = 0;
+
+  for (Index = StrLen (Str) - 1; Index > 1; Index = Index - 2) {
+    if ((Str[Index] == L'0') && (Str[Index - 1] == L'-')) {
+      Str[Index - 1] = 0;
+    } else {
+      break;
+    }
+  }
+}
+
+/**
+  Convert the ASCII string into a UNICODE string.
+
+  @param[in]   Source      The ASCII string.
+  @param[out]  Destination The storage to return the UNICODE string.
+
+  @return CHAR16 *         Pointer to the UNICODE string.
+**/
+CHAR16 *
+IScsiAsciiStrToUnicodeStr (
+  IN  CHAR8   *Source,
+  OUT CHAR16  *Destination
+  )
+{
+  ASSERT (Destination != NULL);
+  ASSERT (Source != NULL);
+
+  while (*Source != '\0') {
+    *(Destination++) = (CHAR16) *(Source++);
+  }
+
+  *Destination = '\0';
+
+  return Destination;
+}
+
+/**
+  Convert the UNICODE string into an ASCII string.
+
+  @param[in]  Source       The UNICODE string.
+  @param[out] Destination  The storage to return the ASCII string.
+
+  @return CHAR8 *          Pointer to the ASCII string.
+**/
+CHAR8 *
+IScsiUnicodeStrToAsciiStr (
+  IN  CHAR16  *Source,
+  OUT CHAR8   *Destination
+  )
+{
+  ASSERT (Destination != NULL);
+  ASSERT (Source != NULL);
+
+  while (*Source != '\0') {
+    //
+    // If any Unicode characters in Source contain
+    // non-zero value in the upper 8 bits, then ASSERT().
+    //
+    ASSERT (*Source < 0x100);
+    *(Destination++) = (CHAR8) *(Source++);
+  }
+
+  *Destination = '\0';
+
+  return Destination;
+}
+
+/**
+  Convert the decimal dotted IPv4 address into the binary IPv4 address.
+
+  @param[in]   Str             The UNICODE string.
+  @param[out]  Ip              The storage to return the ASCII string.
+
+  @retval EFI_SUCCESS           The binary IP address is returned in Ip.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+**/
+EFI_STATUS
+IScsiAsciiStrToIp (
+  IN  CHAR8             *Str,
+  OUT EFI_IPv4_ADDRESS  *Ip
+  )
+{
+  UINTN Index;
+  UINTN Number;
+
+  Index = 0;
+
+  while (*Str != 0) {
+
+    if (Index > 3) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Number = 0;
+    while (NET_IS_DIGIT (*Str)) {
+      Number = Number * 10 + (*Str - '0');
+      Str++;
+    }
+
+    if (Number > 0xFF) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Ip->Addr[Index] = (UINT8) Number;
+
+    if ((*Str != '\0') && (*Str != '.')) {
+      //
+      // The current character should be either the NULL terminator or
+      // the dot delimiter.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (*Str == '.') {
+      //
+      // Skip the delimiter.
+      //
+      Str++;
+    }
+
+    Index++;
+  }
+
+  if (Index != 4) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert the mac address into a hexadecimal encoded "-" seperated string.
+
+  @param[in]  Mac     The mac address.
+  @param[in]  Len     Length in bytes of the mac address.
+  @param[in]  VlanId  VLAN ID of the network device.
+  @param[out] Str     The storage to return the mac string.
+**/
+VOID
+IScsiMacAddrToStr (
+  IN  EFI_MAC_ADDRESS  *Mac,
+  IN  UINT32           Len,
+  IN  UINT16           VlanId,
+  OUT CHAR16           *Str
+  )
+{
+  UINT32  Index;
+  CHAR16  *String;
+
+  for (Index = 0; Index < Len; Index++) {
+    Str[3 * Index]      = (CHAR16) IScsiHexString[(Mac->Addr[Index] >> 4) & 0x0F];
+    Str[3 * Index + 1]  = (CHAR16) IScsiHexString[Mac->Addr[Index] & 0x0F];
+    Str[3 * Index + 2]  = L'-';
+  }
+
+  String = &Str[3 * Index - 1] ;
+  if (VlanId != 0) {
+    String += UnicodeSPrint (String, 6 * sizeof (CHAR16), L"\\%04x", (UINTN) VlanId);
+  }
+
+  *String = L'\0';
+}
+
+/**
+  Convert the binary encoded buffer into a hexadecimal encoded string.
+
+  @param[in]       BinBuffer   The buffer containing the binary data.
+  @param[in]       BinLength   Length of the binary buffer.
+  @param[in, out]  HexStr      Pointer to the string.
+  @param[in, out]  HexLength   The length of the string.
+
+  @retval EFI_SUCCESS          The binary data is converted to the hexadecimal string 
+                               and the length of the string is updated.
+  @retval EFI_BUFFER_TOO_SMALL The string is too small.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+**/
+EFI_STATUS
+IScsiBinToHex (
+  IN     UINT8  *BinBuffer,
+  IN     UINT32 BinLength,
+  IN OUT CHAR8  *HexStr,
+  IN OUT UINT32 *HexLength
+  )
+{
+  UINTN Index;
+
+  if ((HexStr == NULL) || (BinBuffer == NULL) || (BinLength == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((*HexLength) - 3) < BinLength * 2) {
+    *HexLength = BinLength * 2 + 3;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *HexLength = BinLength * 2 + 3;
+  //
+  // Prefix for Hex String
+  //
+  HexStr[0] = '0';
+  HexStr[1] = 'x';
+
+  for (Index = 0; Index < BinLength; Index++) {
+    HexStr[Index * 2 + 2] = IScsiHexString[BinBuffer[Index] >> 4];
+    HexStr[Index * 2 + 3] = IScsiHexString[BinBuffer[Index] & 0x0F];
+  }
+
+  HexStr[Index * 2 + 2] = '\0';
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert the hexadecimal string into a binary encoded buffer.
+
+  @param[in, out]  BinBuffer   The binary buffer.
+  @param[in, out]  BinLength   Length of the binary buffer.
+  @param[in]       HexStr      The hexadecimal string.
+
+  @retval EFI_SUCCESS          The hexadecimal string is converted into a binary
+                               encoded buffer.
+  @retval EFI_BUFFER_TOO_SMALL The binary buffer is too small to hold the converted data.
+**/
+EFI_STATUS
+IScsiHexToBin (
+  IN OUT UINT8  *BinBuffer,
+  IN OUT UINT32 *BinLength,
+  IN     CHAR8  *HexStr
+  )
+{
+  UINTN   Index;
+  UINTN   Length;
+  UINT8   Digit;
+  CHAR8   TemStr[2];
+  
+  ZeroMem (TemStr, sizeof (TemStr));
+
+  //
+  // Find out how many hex characters the string has.
+  //
+  if ((HexStr[0] == '0') && ((HexStr[1] == 'x') || (HexStr[1] == 'X'))) {
+    HexStr += 2;
+  }
+  
+  Length = AsciiStrLen (HexStr);
+
+  for (Index = 0; Index < Length; Index ++) {
+    TemStr[0] = HexStr[Index];
+    Digit = (UINT8) AsciiStrHexToUint64 (TemStr);
+    if (Digit == 0 && TemStr[0] != '0') {
+      //
+      // Invalid Lun Char
+      //
+      break;
+    }
+    if ((Index & 1) == 0) {
+      BinBuffer [Index/2] = Digit;
+    } else {
+      BinBuffer [Index/2] = (UINT8) ((BinBuffer [Index/2] << 4) + Digit);
+    }
+  }
+  
+  *BinLength = (UINT32) ((Index + 1)/2);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Generate random numbers.
+
+  @param[in, out]  Rand       The buffer to contain random numbers.
+  @param[in]       RandLength The length of the Rand buffer.
+**/
+VOID
+IScsiGenRandom (
+  IN OUT UINT8  *Rand,
+  IN     UINTN  RandLength
+  )
+{
+  UINT32  Random;
+
+  while (RandLength > 0) {
+    Random  = NET_RANDOM (NetRandomInitSeed ());
+    *Rand++ = (UINT8) (Random);
+    RandLength--;
+  }
+}
+
+/**
+  Create the iSCSI driver data..
+
+  @param[in] Image      The handle of the driver image.
+  @param[in] Controller The handle of the controller.
+
+  @return The iSCSI driver data created.
+  @retval NULL Other errors as indicated.
+**/
+ISCSI_DRIVER_DATA *
+IScsiCreateDriverData (
+  IN EFI_HANDLE  Image,
+  IN EFI_HANDLE  Controller
+  )
+{
+  ISCSI_DRIVER_DATA *Private;
+  EFI_STATUS        Status;
+
+  Private = AllocateZeroPool (sizeof (ISCSI_DRIVER_DATA));
+  if (Private == NULL) {
+    return NULL;
+  }
+
+  Private->Signature  = ISCSI_DRIVER_DATA_SIGNATURE;
+  Private->Image      = Image;
+  Private->Controller = Controller;
+
+  //
+  // Create an event to be signal when the BS to RT transition is triggerd so
+  // as to abort the iSCSI session.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  IScsiOnExitBootService,
+                  Private,
+                  &gEfiEventExitBootServicesGuid,
+                  &Private->ExitBootServiceEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Private);
+    return NULL;
+  }
+
+  CopyMem(&Private->IScsiExtScsiPassThru, &gIScsiExtScsiPassThruProtocolTemplate, sizeof(EFI_EXT_SCSI_PASS_THRU_PROTOCOL));
+
+  //
+  // 0 is designated to the TargetId, so use another value for the AdapterId.
+  //
+  Private->ExtScsiPassThruMode.AdapterId = 2;
+  Private->ExtScsiPassThruMode.Attributes = EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_PHYSICAL | EFI_EXT_SCSI_PASS_THRU_ATTRIBUTES_LOGICAL;
+  Private->ExtScsiPassThruMode.IoAlign  = 4;
+  Private->IScsiExtScsiPassThru.Mode    = &Private->ExtScsiPassThruMode;
+
+  //
+  // Install the Ext SCSI PASS THRU protocol.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &Private->ExtScsiPassThruHandle,
+                  &gEfiExtScsiPassThruProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &Private->IScsiExtScsiPassThru
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseEvent (Private->ExitBootServiceEvent);
+    FreePool (Private);
+
+    return NULL;
+  }
+
+  IScsiSessionInit (&Private->Session, FALSE);
+
+  return Private;
+}
+
+/**
+  Clean the iSCSI driver data.
+
+  @param[in]  Private The iSCSI driver data.
+**/
+VOID
+IScsiCleanDriverData (
+  IN ISCSI_DRIVER_DATA  *Private
+  )
+{
+  if (Private->DevicePath != NULL) {
+    gBS->UninstallProtocolInterface (
+          Private->ExtScsiPassThruHandle,
+          &gEfiDevicePathProtocolGuid,
+          Private->DevicePath
+          );
+
+    FreePool (Private->DevicePath);
+  }
+
+  if (Private->ExtScsiPassThruHandle != NULL) {
+    gBS->UninstallProtocolInterface (
+          Private->ExtScsiPassThruHandle,
+          &gEfiExtScsiPassThruProtocolGuid,
+          &Private->IScsiExtScsiPassThru
+          );
+  }
+
+  gBS->CloseEvent (Private->ExitBootServiceEvent);
+
+  FreePool (Private);
+}
+
+/**
+  Check wheather the Controller is configured to use DHCP protocol.
+
+  @param[in]  Controller           The handle of the controller.
+  
+  @retval TRUE                     The handle of the controller need the Dhcp protocol.
+  @retval FALSE                    The handle of the controller does not need the Dhcp protocol.
+  
+**/
+BOOLEAN
+IScsiDhcpIsConfigured (
+  IN EFI_HANDLE  Controller
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_MAC_ADDRESS             MacAddress;
+  UINTN                       HwAddressSize;
+  UINT16                      VlanId;
+  CHAR16                      MacString[70];
+  ISCSI_SESSION_CONFIG_NVDATA *ConfigDataTmp;
+
+  //
+  // Get the mac string, it's the name of various variable
+  //
+  Status = NetLibGetMacAddress (Controller, &MacAddress, &HwAddressSize);
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+  VlanId = NetLibGetVlanId (Controller);
+  IScsiMacAddrToStr (&MacAddress, (UINT32) HwAddressSize, VlanId, MacString);
+
+  //
+  // Get the normal configuration.
+  //
+  Status = GetVariable2 (
+             MacString,
+             &gEfiIScsiInitiatorNameProtocolGuid,
+             (VOID**)&ConfigDataTmp,
+             NULL
+             );
+  if (ConfigDataTmp == NULL || EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if (ConfigDataTmp->Enabled && ConfigDataTmp->InitiatorInfoFromDhcp) {
+    FreePool (ConfigDataTmp);
+    return TRUE;
+  }
+
+  FreePool (ConfigDataTmp);
+  return FALSE;
+}
+
+/**
+  Get the various configuration data of this iSCSI instance.
+
+  @param[in]  Private   The iSCSI driver data.
+
+  @retval EFI_SUCCESS   The configuration of this instance is got.
+  @retval EFI_ABORTED   The operation was aborted.
+  @retval Others        Other errors as indicated.
+**/
+EFI_STATUS
+IScsiGetConfigData (
+  IN ISCSI_DRIVER_DATA  *Private
+  )
+{
+  EFI_STATUS                  Status;
+  ISCSI_SESSION               *Session;
+  UINTN                       BufferSize;
+  EFI_MAC_ADDRESS             MacAddress;
+  UINTN                       HwAddressSize;
+  UINT16                      VlanId;
+  CHAR16                      MacString[70];
+
+  //
+  // get the iSCSI Initiator Name
+  //
+  Session                       = &Private->Session;
+  Session->InitiatorNameLength  = ISCSI_NAME_MAX_SIZE;
+  Status = gIScsiInitiatorName.Get (
+                                &gIScsiInitiatorName,
+                                &Session->InitiatorNameLength,
+                                Session->InitiatorName
+                                );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the mac string, it's the name of various variable
+  //
+  Status = NetLibGetMacAddress (Private->Controller, &MacAddress, &HwAddressSize);
+  ASSERT (Status == EFI_SUCCESS);
+  VlanId = NetLibGetVlanId (Private->Controller);
+  IScsiMacAddrToStr (&MacAddress, (UINT32) HwAddressSize, VlanId, MacString);
+
+  //
+  // Get the normal configuration.
+  //
+  BufferSize = sizeof (Session->ConfigData.NvData);
+  Status = gRT->GetVariable (
+                  MacString,
+                  &gEfiIScsiInitiatorNameProtocolGuid,
+                  NULL,
+                  &BufferSize,
+                  &Session->ConfigData.NvData
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (!Session->ConfigData.NvData.Enabled) {
+    return EFI_ABORTED;
+  }
+  //
+  // Get the CHAP Auth information.
+  //
+  BufferSize = sizeof (Session->AuthData.AuthConfig);
+  Status = gRT->GetVariable (
+                  MacString,
+                  &gIScsiCHAPAuthInfoGuid,
+                  NULL,
+                  &BufferSize,
+                  &Session->AuthData.AuthConfig
+                  );
+
+  if (!EFI_ERROR (Status) && Session->ConfigData.NvData.InitiatorInfoFromDhcp) {
+    //
+    // Start dhcp.
+    //
+    Status = IScsiDoDhcp (Private->Image, Private->Controller, &Session->ConfigData);
+  }
+
+  return Status;
+}
+
+/**
+  Get the device path of the iSCSI tcp connection and update it.
+
+  @param[in]  Private The iSCSI driver data.
+
+  @return The updated device path.
+  @retval NULL Other errors as indicated.
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+IScsiGetTcpConnDevicePath (
+  IN ISCSI_DRIVER_DATA  *Private
+  )
+{
+  ISCSI_SESSION             *Session;
+  ISCSI_CONNECTION          *Conn;
+  TCP4_IO                   *Tcp4Io;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_STATUS                Status;
+  EFI_DEV_PATH              *DPathNode;
+
+  Session = &Private->Session;
+  if (Session->State != SESSION_STATE_LOGGED_IN) {
+    return NULL;
+  }
+
+  Conn = NET_LIST_USER_STRUCT_S (
+          Session->Conns.ForwardLink,
+          ISCSI_CONNECTION,
+          Link,
+          ISCSI_CONNECTION_SIGNATURE
+          );
+  Tcp4Io = &Conn->Tcp4Io;
+
+  Status = gBS->HandleProtocol (
+                  Tcp4Io->Handle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **)&DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+  //
+  // Duplicate it.
+  //
+  DevicePath  = DuplicateDevicePath (DevicePath);
+  if (DevicePath == NULL) {
+    return NULL;
+  }
+
+  DPathNode   = (EFI_DEV_PATH *) DevicePath;
+
+  while (!IsDevicePathEnd (&DPathNode->DevPath)) {
+    if ((DevicePathType (&DPathNode->DevPath) == MESSAGING_DEVICE_PATH) &&
+        (DevicePathSubType (&DPathNode->DevPath) == MSG_IPv4_DP)
+        ) {
+
+      DPathNode->Ipv4.LocalPort       = 0;
+      DPathNode->Ipv4.StaticIpAddress = 
+        (BOOLEAN) (!Session->ConfigData.NvData.InitiatorInfoFromDhcp);
+
+      //
+      //  Add a judgement here to support previous versions of IPv4_DEVICE_PATH.
+      //  In previous versions of IPv4_DEVICE_PATH, GatewayIpAddress and SubnetMask
+      //  do not exist.
+      //  In new version of IPv4_DEVICE_PATH, structcure length is 27.
+      //
+      if (DevicePathNodeLength (&DPathNode->Ipv4) == IP4_NODE_LEN_NEW_VERSIONS) {  
+
+        IP4_COPY_ADDRESS (
+          &DPathNode->Ipv4.GatewayIpAddress,
+          &Session->ConfigData.NvData.Gateway
+          );
+
+        IP4_COPY_ADDRESS (
+          &DPathNode->Ipv4.SubnetMask,
+          &Session->ConfigData.NvData.SubnetMask
+          );
+      }
+
+      break;
+    }
+
+    DPathNode = (EFI_DEV_PATH *) NextDevicePathNode (&DPathNode->DevPath);
+  }
+
+  return DevicePath;
+}
+
+/**
+  Abort the session when the transition from BS to RT is initiated.
+
+  @param[in]   Event  The event signaled.
+  @param[in]  Context The iSCSI driver data.
+**/
+VOID
+EFIAPI
+IScsiOnExitBootService (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  ISCSI_DRIVER_DATA *Private;
+
+  Private = (ISCSI_DRIVER_DATA *) Context;
+  gBS->CloseEvent (Private->ExitBootServiceEvent);
+
+  IScsiSessionAbort (&Private->Session);
+}
+
+/**
+  Tests whether a controller handle is being managed by IScsi driver.
+
+  This function tests whether the driver specified by DriverBindingHandle is
+  currently managing the controller specified by ControllerHandle.  This test
+  is performed by evaluating if the the protocol specified by ProtocolGuid is
+  present on ControllerHandle and is was opened by DriverBindingHandle and Nic
+  Device handle with an attribute of EFI_OPEN_PROTOCOL_BY_DRIVER. 
+  If ProtocolGuid is NULL, then ASSERT().
+
+  @param  ControllerHandle     A handle for a controller to test.
+  @param  DriverBindingHandle  Specifies the driver binding handle for the
+                               driver.
+  @param  ProtocolGuid         Specifies the protocol that the driver specified
+                               by DriverBindingHandle opens in its Start()
+                               function.
+
+  @retval EFI_SUCCESS          ControllerHandle is managed by the driver
+                               specified by DriverBindingHandle.
+  @retval EFI_UNSUPPORTED      ControllerHandle is not managed by the driver
+                               specified by DriverBindingHandle.
+
+**/
+EFI_STATUS
+EFIAPI
+IScsiTestManagedDevice (
+  IN  EFI_HANDLE       ControllerHandle,
+  IN  EFI_HANDLE       DriverBindingHandle,
+  IN  EFI_GUID         *ProtocolGuid
+  )
+{
+  EFI_STATUS     Status;
+  VOID           *ManagedInterface;
+  EFI_HANDLE     NicControllerHandle;
+
+  ASSERT (ProtocolGuid != NULL);
+
+  NicControllerHandle = NetLibGetNicHandle (ControllerHandle, ProtocolGuid);
+  if (NicControllerHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  (EFI_GUID *) ProtocolGuid,
+                  &ManagedInterface,
+                  DriverBindingHandle,
+                  NicControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (!EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           ControllerHandle,
+           (EFI_GUID *) ProtocolGuid,
+           DriverBindingHandle,
+           NicControllerHandle
+           );
+    return EFI_UNSUPPORTED;
+  }
+
+  if (Status != EFI_ALREADY_STARTED) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.h
new file mode 100644
index 0000000000..8a95493dcd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiMisc.h
@@ -0,0 +1,314 @@
+/** @file
+  Miscellaneous definitions for iSCSI driver.
+
+Copyright (c) 2004 - 2015, 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.
+
+**/
+
+#ifndef _ISCSI_MISC_H_
+#define _ISCSI_MISC_H_
+
+#include <Library/BaseLib.h>
+
+typedef struct _ISCSI_SESSION_CONFIG_DATA ISCSI_SESSION_CONFIG_DATA;
+
+///
+/// IPv4 Device Path Node Length
+///
+#define IP4_NODE_LEN_NEW_VERSIONS    27
+
+#pragma pack(1)
+typedef struct {
+  BOOLEAN           Enabled;
+
+  BOOLEAN           InitiatorInfoFromDhcp;
+  EFI_IPv4_ADDRESS  LocalIp;
+  EFI_IPv4_ADDRESS  SubnetMask;
+  EFI_IPv4_ADDRESS  Gateway;
+
+  BOOLEAN           TargetInfoFromDhcp;
+  CHAR8             TargetName[ISCSI_NAME_MAX_SIZE];
+  EFI_IPv4_ADDRESS  TargetIp;
+  UINT16            TargetPort;
+  UINT8             BootLun[8];
+
+  UINT8             IsId[6];
+} ISCSI_SESSION_CONFIG_NVDATA;
+#pragma pack()
+
+struct _ISCSI_SESSION_CONFIG_DATA {
+  ISCSI_SESSION_CONFIG_NVDATA NvData;
+
+  EFI_IPv4_ADDRESS            PrimaryDns;
+  EFI_IPv4_ADDRESS            SecondaryDns;
+  EFI_IPv4_ADDRESS            DhcpServer;
+};
+
+/**
+  Calculate the prefix length of the IPv4 subnet mask.
+
+  @param[in]  SubnetMask The IPv4 subnet mask.
+
+  @return The prefix length of the subnet mask.
+  @retval 0 Other errors as indicated.
+**/
+UINT8
+IScsiGetSubnetMaskPrefixLength (
+  IN EFI_IPv4_ADDRESS  *SubnetMask
+  );
+
+/**
+  Convert the hexadecimal encoded LUN string into the 64-bit LUN. 
+
+  @param[in]   Str             The hexadecimal encoded LUN string.
+  @param[out]  Lun             Storage to return the 64-bit LUN.
+
+  @retval EFI_SUCCESS           The 64-bit LUN is stored in Lun.
+  @retval EFI_INVALID_PARAMETER The string is malformatted.
+**/
+EFI_STATUS
+IScsiAsciiStrToLun (
+  IN  CHAR8  *Str,
+  OUT UINT8  *Lun
+  );
+
+/**
+  Convert the 64-bit LUN into the hexadecimal encoded LUN string.
+
+  @param[in]   Lun The 64-bit LUN.
+  @param[out]  Str The storage to return the hexadecimal encoded LUN string.
+**/
+VOID
+IScsiLunToUnicodeStr (
+  IN UINT8    *Lun,
+  OUT CHAR16  *Str
+  );
+
+/**
+  Convert the ASCII string into a UNICODE string.
+
+  @param[in]   Source      The ASCII string.
+  @param[out]  Destination The storage to return the UNICODE string.
+
+  @return CHAR16 *         Pointer to the UNICODE string.
+**/
+CHAR16 *
+IScsiAsciiStrToUnicodeStr (
+  IN  CHAR8   *Source,
+  OUT CHAR16  *Destination
+  );
+
+/**
+  Convert the UNICODE string into an ASCII string.
+
+  @param[in]  Source       The UNICODE string.
+  @param[out] Destination  The storage to return the ASCII string.
+
+  @return CHAR8 *          Pointer to the ASCII string.
+**/
+CHAR8 *
+IScsiUnicodeStrToAsciiStr (
+  IN  CHAR16  *Source,
+  OUT CHAR8   *Destination
+  );
+
+/**
+  Convert the mac address into a hexadecimal encoded "-" seperated string.
+
+  @param[in]  Mac     The mac address.
+  @param[in]  Len     Length in bytes of the mac address.
+  @param[in]  VlanId  VLAN ID of the network device.
+  @param[out] Str     The storage to return the mac string.
+**/
+VOID
+IScsiMacAddrToStr (
+  IN  EFI_MAC_ADDRESS  *Mac,
+  IN  UINT32           Len,
+  IN  UINT16           VlanId,
+  OUT CHAR16           *Str
+  );
+
+/**
+  Convert the decimal dotted IPv4 address into the binary IPv4 address.
+
+  @param[in]   Str             The UNICODE string.
+  @param[out]  Ip              The storage to return the ASCII string.
+
+  @retval EFI_SUCCESS           The binary IP address is returned in Ip.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+**/
+EFI_STATUS
+IScsiAsciiStrToIp (
+  IN  CHAR8             *Str,
+  OUT EFI_IPv4_ADDRESS  *Ip
+  );
+
+/**
+  Convert the binary encoded buffer into a hexadecimal encoded string.
+
+  @param[in]       BinBuffer   The buffer containing the binary data.
+  @param[in]       BinLength   Length of the binary buffer.
+  @param[in, out]  HexStr      Pointer to the string.
+  @param[in, out]  HexLength   The length of the string.
+
+  @retval EFI_SUCCESS          The binary data is converted to the hexadecimal string 
+                               and the length of the string is updated.
+  @retval EFI_BUFFER_TOO_SMALL The string is too small.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+**/
+EFI_STATUS
+IScsiBinToHex (
+  IN     UINT8  *BinBuffer,
+  IN     UINT32 BinLength,
+  IN OUT CHAR8  *HexStr,
+  IN OUT UINT32 *HexLength
+  );
+
+/**
+  Convert the hexadecimal string into a binary encoded buffer.
+
+  @param[in, out]  BinBuffer   The binary buffer.
+  @param[in, out]  BinLength   Length of the binary buffer.
+  @param[in]       HexStr      The hexadecimal string.
+
+  @retval EFI_SUCCESS          The hexadecimal string is converted into a binary
+                               encoded buffer.
+  @retval EFI_BUFFER_TOO_SMALL The binary buffer is too small to hold the converted data.
+**/
+EFI_STATUS
+IScsiHexToBin (
+  IN OUT UINT8  *BinBuffer,
+  IN OUT UINT32 *BinLength,
+  IN     CHAR8  *HexStr
+  );
+
+/**
+  Generate random numbers.
+
+  @param[in, out]  Rand       The buffer to contain random numbers.
+  @param[in]       RandLength The length of the Rand buffer.
+**/
+VOID
+IScsiGenRandom (
+  IN OUT UINT8  *Rand,
+  IN     UINTN  RandLength
+  );
+
+/**
+  Create the iSCSI driver data..
+
+  @param[in] Image      The handle of the driver image.
+  @param[in] Controller The handle of the controller.
+
+  @return The iSCSI driver data created.
+  @retval NULL Other errors as indicated.
+**/
+ISCSI_DRIVER_DATA *
+IScsiCreateDriverData (
+  IN EFI_HANDLE  Image,
+  IN EFI_HANDLE  Controller
+  );
+
+/**
+  Clean the iSCSI driver data.
+
+  @param[in]  Private The iSCSI driver data.
+**/
+VOID
+IScsiCleanDriverData (
+  IN ISCSI_DRIVER_DATA  *Private
+  );
+
+/**
+  Check wheather the Controller is configured to use DHCP protocol.
+
+  @param[in]  Controller           The handle of the controller.
+  
+  @retval TRUE                     The handle of the controller need the Dhcp protocol.
+  @retval FALSE                    The handle of the controller does not need the Dhcp protocol.
+  
+**/
+BOOLEAN
+IScsiDhcpIsConfigured (
+  IN EFI_HANDLE  Controller
+  );
+
+/**
+  Get the various configuration data of this iSCSI instance.
+
+  @param[in]  Private   The iSCSI driver data.
+
+  @retval EFI_SUCCESS   The configuration of this instance is got.
+  @retval EFI_ABORTED   The operation was aborted.
+  @retval Others        Other errors as indicated.
+**/
+EFI_STATUS
+IScsiGetConfigData (
+  IN ISCSI_DRIVER_DATA  *Private
+  );
+
+/**
+  Get the device path of the iSCSI tcp connection and update it.
+
+  @param[in]  Private The iSCSI driver data.
+
+  @return The updated device path.
+  @retval NULL Other errors as indicated.
+**/
+EFI_DEVICE_PATH_PROTOCOL *
+IScsiGetTcpConnDevicePath (
+  IN ISCSI_DRIVER_DATA  *Private
+  );
+
+/**
+  Abort the session when the transition from BS to RT is initiated.
+
+  @param[in]   Event  The event signaled.
+  @param[in]  Context The iSCSI driver data.
+**/
+VOID
+EFIAPI
+IScsiOnExitBootService (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Tests whether a controller handle is being managed by IScsi driver.
+
+  This function tests whether the driver specified by DriverBindingHandle is
+  currently managing the controller specified by ControllerHandle.  This test
+  is performed by evaluating if the the protocol specified by ProtocolGuid is
+  present on ControllerHandle and is was opened by DriverBindingHandle and Nic
+  Device handle with an attribute of EFI_OPEN_PROTOCOL_BY_DRIVER. 
+  If ProtocolGuid is NULL, then ASSERT().
+
+  @param  ControllerHandle     A handle for a controller to test.
+  @param  DriverBindingHandle  Specifies the driver binding handle for the
+                               driver.
+  @param  ProtocolGuid         Specifies the protocol that the driver specified
+                               by DriverBindingHandle opens in its Start()
+                               function.
+
+  @retval EFI_SUCCESS          ControllerHandle is managed by the driver
+                               specified by DriverBindingHandle.
+  @retval EFI_UNSUPPORTED      ControllerHandle is not managed by the driver
+                               specified by DriverBindingHandle.
+
+**/
+EFI_STATUS
+EFIAPI
+IScsiTestManagedDevice (
+  IN  EFI_HANDLE       ControllerHandle,
+  IN  EFI_HANDLE       DriverBindingHandle,
+  IN  EFI_GUID         *ProtocolGuid
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.c
new file mode 100644
index 0000000000..1ac79e7ca1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.c
@@ -0,0 +1,2830 @@
+/** @file
+  The implementation of iSCSI protocol based on RFC3720.
+
+Copyright (c) 2004 - 2015, 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 "IScsiImpl.h"
+
+UINT32 mDataSegPad = 0;
+
+/**
+  Attach the iSCSI connection to the iSCSI session. 
+
+  @param[in, out]  Session The iSCSI session.
+  @param[in, out]  Conn    The iSCSI connection.
+**/
+VOID
+IScsiAttatchConnection (
+  IN OUT ISCSI_SESSION     *Session,
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  InsertTailList (&Session->Conns, &Conn->Link);
+  Conn->Session = Session;
+  Session->NumConns++;
+}
+
+/**
+  Detach the iSCSI connection from the session it belongs to. 
+
+  @param[in, out]  Conn The iSCSI connection.
+**/
+VOID
+IScsiDetatchConnection (
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  RemoveEntryList (&Conn->Link);
+  Conn->Session->NumConns--;
+  Conn->Session = NULL;
+}
+
+/**
+  Check the sequence number according to RFC3720. 
+
+  @param[in, out]  ExpSN   The currently expected sequence number.
+  @param[in]       NewSN   The sequence number to check.
+
+  @retval EFI_SUCCESS         The check passed and the ExpSN is increased.
+  @retval EFI_NOT_READY       Response was sent due to a retransmission request.
+  @retval EFI_PROTOCOL_ERROR  Some kind of iSCSI protocol error happened.
+**/
+EFI_STATUS
+IScsiCheckSN (
+  IN OUT UINT32  *ExpSN,
+  IN UINT32      NewSN
+  )
+{
+  if (!ISCSI_SEQ_EQ (NewSN, *ExpSN)) {
+    if (ISCSI_SEQ_LT (NewSN, *ExpSN)) {
+      //
+      // Duplicate
+      //
+      return EFI_NOT_READY;
+    } else {
+      return EFI_PROTOCOL_ERROR;
+    }
+  } else {
+    //
+    // Advance the ExpSN
+    //
+    (*ExpSN)++;
+    return EFI_SUCCESS;
+  }
+}
+
+/**
+  Update the sequence numbers for the iSCSI command.
+
+  @param[in, out]  Session  The iSCSI session.
+  @param[in]       MaxCmdSN Maximum CmdSN from the target.
+  @param[in]       ExpCmdSN Next expected CmdSN from the target.
+**/
+VOID
+IScsiUpdateCmdSN (
+  IN OUT ISCSI_SESSION  *Session,
+  IN UINT32             MaxCmdSN,
+  IN UINT32             ExpCmdSN
+  )
+{
+  if (ISCSI_SEQ_LT (MaxCmdSN, ExpCmdSN - 1)) {
+    return ;
+  }
+
+  if (ISCSI_SEQ_GT (MaxCmdSN, Session->MaxCmdSN)) {
+    Session->MaxCmdSN = MaxCmdSN;
+  }
+
+  if (ISCSI_SEQ_GT (ExpCmdSN, Session->ExpCmdSN)) {
+    Session->ExpCmdSN = ExpCmdSN;
+  }
+}
+
+/**
+  This function does the iSCSI connection login.
+
+  @param[in, out]  Conn      The iSCSI connection to login.
+
+  @retval EFI_SUCCESS        The iSCSI connection is logged into the iSCSI target.
+  @retval EFI_TIMEOUT        Timeout happened during the login procedure.
+  @retval Others             Other errors as indicated.  
+**/
+EFI_STATUS
+IScsiConnLogin (
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Start the timer, wait 16 seconds to establish the TCP connection.
+  //
+  Status = gBS->SetTimer (Conn->TimeoutEvent, TimerRelative, 16 * TICKS_PER_SECOND);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // try to establish the tcp connection
+  //
+  Status = Tcp4IoConnect (&Conn->Tcp4Io, Conn->TimeoutEvent);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  gBS->SetTimer (Conn->TimeoutEvent, TimerCancel, 0);
+  Conn->State = CONN_STATE_IN_LOGIN;
+
+  //
+  // connection is established, start the iSCSI Login
+  //
+  do {
+    Status = IScsiSendLoginReq (Conn);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    Status = IScsiReceiveLoginRsp (Conn);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  } while (Conn->CurrentStage != ISCSI_FULL_FEATURE_PHASE);
+
+  return Status;
+}
+
+/**
+  Reset the iSCSI connection.
+
+  @param[in, out]  Conn The iSCSI connection to reset.
+**/
+VOID
+IScsiConnReset (
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  Tcp4IoReset (&Conn->Tcp4Io);
+}
+
+/**
+  Create a TCP connection for the iSCSI session.
+
+  @param[in]  Private The iSCSI driver data.
+  @param[in]  Session Maximum CmdSN from the target.
+
+  @return The newly created iSCSI connection.
+**/
+ISCSI_CONNECTION *
+IScsiCreateConnection (
+  IN ISCSI_DRIVER_DATA  *Private,
+  IN ISCSI_SESSION      *Session
+  )
+{
+  ISCSI_CONNECTION    *Conn;
+  TCP4_IO_CONFIG_DATA Tcp4IoConfig;
+  EFI_STATUS          Status;
+
+  Conn = AllocatePool (sizeof (ISCSI_CONNECTION));
+  if (Conn == NULL) {
+    return NULL;
+  }
+
+  Conn->Signature       = ISCSI_CONNECTION_SIGNATURE;
+  Conn->State           = CONN_STATE_FREE;
+  Conn->CurrentStage    = ISCSI_SECURITY_NEGOTIATION;
+  Conn->NextStage       = ISCSI_LOGIN_OPERATIONAL_NEGOTIATION;
+  Conn->CHAPStep        = ISCSI_CHAP_INITIAL;
+  Conn->ExpStatSN       = 0;
+  Conn->PartialReqSent  = FALSE;
+  Conn->PartialRspRcvd  = FALSE;
+  Conn->Cid             = Session->NextCid++;
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &Conn->TimeoutEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Conn);
+    return NULL;
+  }
+
+  NetbufQueInit (&Conn->RspQue);
+
+  //
+  // set the default connection-only parameters
+  //
+  Conn->MaxRecvDataSegmentLength  = DEFAULT_MAX_RECV_DATA_SEG_LEN;
+  Conn->HeaderDigest              = IScsiDigestNone;
+  Conn->DataDigest                = IScsiDigestNone;
+
+  CopyMem (&Tcp4IoConfig.LocalIp, &Session->ConfigData.NvData.LocalIp, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Tcp4IoConfig.SubnetMask, &Session->ConfigData.NvData.SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Tcp4IoConfig.Gateway, &Session->ConfigData.NvData.Gateway, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Tcp4IoConfig.RemoteIp, &Session->ConfigData.NvData.TargetIp, sizeof (EFI_IPv4_ADDRESS));
+
+  Tcp4IoConfig.RemotePort = Session->ConfigData.NvData.TargetPort;
+
+  //
+  // Create the tcp4 IO for this connection
+  //
+  Status = Tcp4IoCreateSocket (
+            Private->Image,
+            Private->Controller,
+            &Tcp4IoConfig,
+            &Conn->Tcp4Io
+            );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseEvent (Conn->TimeoutEvent);
+    FreePool (Conn);
+    Conn = NULL;
+  }
+
+  return Conn;
+}
+
+/**
+  Destroy an iSCSI connection.
+
+  @param[in]  Conn The connection to destroy.
+**/
+VOID
+IScsiDestroyConnection (
+  IN ISCSI_CONNECTION  *Conn
+  )
+{
+  Tcp4IoDestroySocket (&Conn->Tcp4Io);
+  NetbufQueFlush (&Conn->RspQue);
+  gBS->CloseEvent (Conn->TimeoutEvent);
+  FreePool (Conn);
+}
+
+/**
+  Login the iSCSI session.
+
+  @param[in]  Private          The iSCSI driver data.
+
+  @retval EFI_SUCCESS          The iSCSI session login procedure finished.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NO_MEDIA         There was a media error.
+  @retval Others               Other errors as indicated.
+
+**/
+EFI_STATUS
+IScsiSessionLogin (
+  IN ISCSI_DRIVER_DATA  *Private
+  )
+{
+  EFI_STATUS        Status;
+  ISCSI_SESSION     *Session;
+  ISCSI_CONNECTION  *Conn;
+  EFI_TCP4_PROTOCOL *Tcp4;
+  BOOLEAN           MediaPresent;
+
+  Session = &Private->Session;
+
+  //
+  // Check media status before session login
+  //
+  MediaPresent = TRUE;
+  NetLibDetectMedia (Private->Controller, &MediaPresent);
+  if (!MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  //
+  // Set session identifier
+  //
+  CopyMem (Session->Isid, Session->ConfigData.NvData.IsId, 6);
+
+  //
+  // Create a connection for the session.
+  //
+  Conn = IScsiCreateConnection (Private, Session);
+  if (Conn == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  IScsiAttatchConnection (Session, Conn);
+
+  //
+  // Login througth the newly created connection.
+  //
+  Status = IScsiConnLogin (Conn);
+  if (EFI_ERROR (Status)) {
+    IScsiConnReset (Conn);
+    IScsiDetatchConnection (Conn);
+    IScsiDestroyConnection (Conn);
+  } else {
+    Session->State = SESSION_STATE_LOGGED_IN;
+
+    gBS->OpenProtocol (
+          Conn->Tcp4Io.Handle,
+          &gEfiTcp4ProtocolGuid,
+          (VOID **)&Tcp4,
+          Private->Image,
+          Private->ExtScsiPassThruHandle,
+          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+          );
+  }
+
+  return Status;
+}
+
+/**
+  Build and send the iSCSI login request to the iSCSI target according to
+  the current login stage.
+
+  @param[in]  Conn             The connection in the iSCSI login phase.
+
+  @retval EFI_SUCCESS          The iSCSI login request PDU is built and sent on this
+                               connection.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_DEVICE_ERROR     Some kind of device error happened.
+**/
+EFI_STATUS
+IScsiSendLoginReq (
+  IN ISCSI_CONNECTION  *Conn
+  )
+{
+  NET_BUF     *Pdu;
+  EFI_STATUS  Status;
+
+  //
+  // build the Login Request PDU
+  //
+  Pdu = IScsiPrepareLoginReq (Conn);
+  if (Pdu == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Send it to the iSCSI target.
+  //
+  Status = Tcp4IoTransmit (&Conn->Tcp4Io, Pdu);
+
+  NetbufFree (Pdu);
+
+  return Status;
+}
+
+/**
+  Receive and process the iSCSI login response.
+
+  @param[in]  Conn             The connection in the iSCSI login phase.
+  
+  @retval EFI_SUCCESS          The iSCSI login response PDU is received and processed.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiReceiveLoginRsp (
+  IN ISCSI_CONNECTION  *Conn
+  )
+{
+  EFI_STATUS  Status;
+  NET_BUF     *Pdu;
+
+  Pdu = NULL;
+
+  //
+  // Receive the iSCSI login response.
+  //
+  Status = IScsiReceivePdu (Conn, &Pdu, NULL, FALSE, FALSE, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // A Login Response is received, process it.
+  //
+  ASSERT (Pdu != NULL);
+  Status = IScsiProcessLoginRsp (Conn, Pdu);
+
+  NetbufFree (Pdu);
+
+  return Status;
+}
+
+/**
+  Add an iSCSI key-value pair as a string into the data segment of the Login Request PDU.
+  The DataSegmentLength and the actual size of the net buffer containing this PDU will be
+  updated.
+
+  @param[in, out]  Pdu         The iSCSI PDU whose data segment the key-value pair will
+                               be added to.
+  @param[in]       Key         The key name string.
+  @param[in]       Value       The value string.
+
+  @retval EFI_SUCCESS          The key-valu pair is added to the PDU's datasegment and
+                               the correspondence length fields are updated.
+  @retval EFI_OUT_OF_RESOURCES There is not enough space in the PDU to add the key-value
+                               pair.
+**/
+EFI_STATUS
+IScsiAddKeyValuePair (
+  IN OUT NET_BUF      *Pdu,
+  IN CHAR8            *Key,
+  IN CHAR8            *Value
+  )
+{
+  UINT32              DataSegLen;
+  UINT32              KeyLen;
+  UINT32              ValueLen;
+  UINT32              TotalLen;
+  ISCSI_LOGIN_REQUEST *LoginReq;
+  CHAR8               *Data;
+
+  LoginReq    = (ISCSI_LOGIN_REQUEST *) NetbufGetByte (Pdu, 0, NULL);
+  if (LoginReq == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+  DataSegLen  = NTOH24 (LoginReq->DataSegmentLength);
+
+  KeyLen      = (UINT32) AsciiStrLen (Key);
+  ValueLen    = (UINT32) AsciiStrLen (Value);
+
+  //
+  // 1 byte for the key value separator '=' and 1 byte for the null
+  // delimiter after the value.
+  //
+  TotalLen = KeyLen + 1 + ValueLen + 1;
+
+  //
+  // Allocate the space for the key-value pair.
+  //
+  Data = (CHAR8 *)NetbufAllocSpace (Pdu, TotalLen, NET_BUF_TAIL);
+  if (Data == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Add the key.
+  //
+  CopyMem (Data, Key, KeyLen);
+  Data += KeyLen;
+
+  *Data = '=';
+  Data++;
+
+  //
+  // Add the value.
+  //
+  CopyMem (Data, Value, ValueLen);
+  Data += ValueLen;
+
+  *Data = '\0';
+
+  //
+  // update the DataSegmentLength
+  //
+  ISCSI_SET_DATASEG_LEN (LoginReq, DataSegLen + TotalLen);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare the iSCSI login request to be sent according to the current login status.
+
+  @param[in, out]  Conn The connection in the iSCSI login phase.
+
+  @return The pointer to the net buffer containing the iSCSI login request built.
+  @retval Others    Other errors as indicated.
+**/
+NET_BUF *
+IScsiPrepareLoginReq (
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  ISCSI_SESSION       *Session;
+  NET_BUF             *Nbuf;
+  ISCSI_LOGIN_REQUEST *LoginReq;
+  EFI_STATUS          Status;
+
+  Session = Conn->Session;
+
+  Nbuf    = NetbufAlloc (sizeof (ISCSI_LOGIN_REQUEST) + DEFAULT_MAX_RECV_DATA_SEG_LEN);
+  if (Nbuf == NULL) {
+    return NULL;
+  }
+
+  LoginReq = (ISCSI_LOGIN_REQUEST *) NetbufAllocSpace (Nbuf, sizeof (ISCSI_LOGIN_REQUEST), NET_BUF_TAIL);
+  ASSERT (LoginReq != NULL);
+  ZeroMem (LoginReq, sizeof (ISCSI_LOGIN_REQUEST));
+
+  //
+  // Init the login request pdu
+  //
+  ISCSI_SET_OPCODE (LoginReq, ISCSI_OPCODE_LOGIN_REQ, ISCSI_REQ_IMMEDIATE);
+  ISCSI_SET_STAGES (LoginReq, Conn->CurrentStage, Conn->NextStage);
+  LoginReq->VersionMax        = ISCSI_VERSION_MAX;
+  LoginReq->VersionMin        = ISCSI_VERSION_MIN;
+  LoginReq->Tsih              = HTONS (Session->Tsih);
+  LoginReq->InitiatorTaskTag  = HTONL (Session->InitiatorTaskTag);
+  LoginReq->Cid               = HTONS (Conn->Cid);
+  LoginReq->CmdSN             = HTONL (Session->CmdSN);
+
+  //
+  // For the first Login Request on a coonection this is ExpStatSN for the
+  // old connection and this field is only valid if the Login Request restarts
+  // a connection.
+  // For subsequent Login Requests it is used to acknowledge the Login Responses
+  // with their increasing StatSN values.
+  //
+  LoginReq->ExpStatSN = HTONL (Conn->ExpStatSN);
+  CopyMem (LoginReq->Isid, Session->Isid, sizeof (LoginReq->Isid));
+
+  if (Conn->PartialRspRcvd) {
+    //
+    // A partial response, initiator must send an empty Login Request.
+    //
+    return Nbuf;
+  }
+
+  switch (Conn->CurrentStage) {
+  case ISCSI_SECURITY_NEGOTIATION:
+    Status = IScsiCHAPToSendReq (Conn, Nbuf);
+    break;
+
+  case ISCSI_LOGIN_OPERATIONAL_NEGOTIATION:
+    Status = IScsiFillOpParams (Conn, Nbuf);
+    ISCSI_SET_FLAG (LoginReq, ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT);
+    break;
+
+  default:
+    //
+    // something error happens...
+    //
+    Status = EFI_DEVICE_ERROR;
+    break;
+  }
+
+  if (EFI_ERROR (Status)) {
+    NetbufFree (Nbuf);
+    Nbuf = NULL;
+  } else {
+    //
+    // Pad the data segment if needed.
+    //
+    IScsiPadSegment (Nbuf, ISCSI_GET_DATASEG_LEN (LoginReq));
+    //
+    // Check whether we will issue the stage transition signal?
+    //
+    Conn->TransitInitiated = (BOOLEAN) ISCSI_FLAG_ON (LoginReq, ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT);
+  }
+
+  return Nbuf;
+}
+
+/**
+  Process the iSCSI Login Response.
+
+  @param[in, out]  Conn The connection on which the iSCSI login response is received.
+  @param[in, out]  Pdu  The iSCSI login response PDU.
+
+  @retval EFI_SUCCESS        The iSCSI login response PDU is processed and all check are passed.
+  @retval EFI_PROTOCOL_ERROR Some kind of iSCSI protocol error happened.
+  @retval EFI_MEDIA_CHANGED  Target is redirected.
+  @retval Others             Other errors as indicated.
+**/
+EFI_STATUS
+IScsiProcessLoginRsp (
+  IN OUT ISCSI_CONNECTION  *Conn,
+  IN OUT NET_BUF           *Pdu
+  )
+{
+  EFI_STATUS            Status;
+  ISCSI_SESSION         *Session;
+  ISCSI_LOGIN_RESPONSE  *LoginRsp;
+  BOOLEAN               Transit;
+  BOOLEAN               Continue;
+  UINT8                 CurrentStage;
+  UINT8                 NextStage;
+  UINT8                 *DataSeg;
+  UINT32                DataSegLen;
+
+  Session   = Conn->Session;
+
+  LoginRsp  = (ISCSI_LOGIN_RESPONSE *) NetbufGetByte (Pdu, 0, NULL);
+  if (LoginRsp == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+  if (!ISCSI_CHECK_OPCODE (LoginRsp, ISCSI_OPCODE_LOGIN_RSP)) {
+    //
+    // It's not a Login Response
+    //
+    return EFI_PROTOCOL_ERROR;
+  }
+  //
+  // Get the data segment if any.
+  //
+  DataSegLen = ISCSI_GET_DATASEG_LEN (LoginRsp);
+  if (DataSegLen != 0) {
+    DataSeg = NetbufGetByte (Pdu, sizeof (ISCSI_LOGIN_RESPONSE), NULL);
+  } else {
+    DataSeg = NULL;
+  }
+  //
+  // Check the status class in the login response PDU.
+  //
+  switch (LoginRsp->StatusClass) {
+  case ISCSI_LOGIN_STATUS_SUCCESS:
+    //
+    // Just break here, the response and the data segment will be processed later.
+    //
+    break;
+
+  case ISCSI_LOGIN_STATUS_REDIRECTION:
+    //
+    // The target may be moved to a different address
+    //
+    if (DataSeg == NULL) {
+      return EFI_PROTOCOL_ERROR;
+    }
+    //
+    // Process the TargetAddress key-value strings in the data segment to update the
+    // target address info.
+    //
+    Status = IScsiUpdateTargetAddress (Session, (CHAR8 *)DataSeg, DataSegLen);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    //
+    // Session will be restarted on this error status because the Target is
+    // redirected by this Login Response.
+    //
+    return EFI_MEDIA_CHANGED;
+
+  default:
+    //
+    // Initiator Error, Target Error, or any other undefined error code.
+    //
+    return EFI_PROTOCOL_ERROR;
+  }
+  //
+  // The status is sucess, extract the wanted fields from the header segment.
+  //
+  Transit                     = (BOOLEAN) ISCSI_FLAG_ON (LoginRsp, ISCSI_LOGIN_RSP_PDU_FLAG_TRANSIT);
+  Continue                    = (BOOLEAN) ISCSI_FLAG_ON (LoginRsp, ISCSI_LOGIN_RSP_PDU_FLAG_CONTINUE);
+
+  CurrentStage                = (UINT8) ISCSI_GET_CURRENT_STAGE (LoginRsp);
+  NextStage                   = (UINT8) ISCSI_GET_NEXT_STAGE (LoginRsp);
+
+  LoginRsp->InitiatorTaskTag  = NTOHL (LoginRsp->InitiatorTaskTag);
+
+  if ((Transit && Continue) ||
+      (CurrentStage != Conn->CurrentStage) ||
+      (!Conn->TransitInitiated && Transit) ||
+      (Transit && (NextStage != Conn->NextStage)) ||
+      (CompareMem (Session->Isid, LoginRsp->Isid, sizeof (LoginRsp->Isid)) != 0) ||
+      (LoginRsp->InitiatorTaskTag != Session->InitiatorTaskTag)
+      ) {
+    //
+    // A Login Response with the C bit set to 1 MUST have the T bit set to 0;
+    // The CSG in the Login Response MUST be the same with the I-end of this connection;
+    // The T bit can't be 1 if the last Login Response sent by the initiator doesn't
+    // initiate the transistion;
+    // The NSG MUST be the same with the I-end of this connection if Transit is required.
+    // The ISID in the Login Response MUST be the same with this session.
+    //
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  LoginRsp->StatSN    = NTOHL (LoginRsp->StatSN);
+  LoginRsp->ExpCmdSN  = NTOHL (LoginRsp->ExpCmdSN);
+  LoginRsp->MaxCmdSN  = NTOHL (LoginRsp->MaxCmdSN);
+
+  if ((Conn->CurrentStage == ISCSI_SECURITY_NEGOTIATION) && (Conn->CHAPStep == ISCSI_CHAP_INITIAL)) {
+    //
+    // If the Login Request is a leading Login Request, the target MUST use
+    // the value presented in CmdSN as the target value for ExpCmdSN.
+    //
+    if ((Session->State == SESSION_STATE_FREE) && (Session->CmdSN != LoginRsp->ExpCmdSN)) {
+      return EFI_PROTOCOL_ERROR;
+    }
+
+    //
+    // It's the initial Login Response, initialize the local ExpStatSN, MaxCmdSN
+    // and ExpCmdSN.
+    //
+    Conn->ExpStatSN   = LoginRsp->StatSN + 1;
+    Session->MaxCmdSN = LoginRsp->MaxCmdSN;
+    Session->ExpCmdSN = LoginRsp->ExpCmdSN;
+  } else {
+    //
+    // Check the StatSN of this PDU
+    //
+    Status = IScsiCheckSN (&Conn->ExpStatSN, LoginRsp->StatSN);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update the MaxCmdSN and ExpCmdSN
+      //
+      IScsiUpdateCmdSN (Session, LoginRsp->MaxCmdSN, LoginRsp->ExpCmdSN);
+    } else {
+      return Status;
+    }
+  }
+  //
+  // Trim off the header segment.
+  //
+  NetbufTrim (Pdu, sizeof (ISCSI_LOGIN_RESPONSE), NET_BUF_HEAD);
+
+  //
+  // Queue this login response first in case it's a partial response so that
+  // later when the full response list is received we can combine these scattered
+  // responses' data segment and then process it.
+  //
+  NET_GET_REF (Pdu);
+  NetbufQueAppend (&Conn->RspQue, Pdu);
+
+  Conn->PartialRspRcvd = Continue;
+  if (Continue) {
+    //
+    // It's a partial response, have to wait for another or more Request/Response
+    // conversations to get the full response.
+    //
+    return EFI_SUCCESS;
+  }
+
+  switch (CurrentStage) {
+  case ISCSI_SECURITY_NEGOTIATION:
+    //
+    // In security negotiation stage, let CHAP module handle it.
+    //
+    Status = IScsiCHAPOnRspReceived (Conn);
+    break;
+
+  case ISCSI_LOGIN_OPERATIONAL_NEGOTIATION:
+    //
+    // Response received with negotiation resonse on iSCSI parameters, check them.
+    //
+    Status = IScsiCheckOpParams (Conn);
+    break;
+
+  default:
+    //
+    // Should never get here.
+    //
+    Status = EFI_PROTOCOL_ERROR;
+    break;
+  }
+
+  if (Transit && (Status == EFI_SUCCESS)) {
+    //
+    // Do the state transition.
+    //
+    Conn->CurrentStage = Conn->NextStage;
+
+    if (Conn->CurrentStage == ISCSI_LOGIN_OPERATIONAL_NEGOTIATION) {
+      Conn->NextStage = ISCSI_FULL_FEATURE_PHASE;
+    } else {
+      //
+      // CurrentStage is iSCSI Full Feature, it's the Login-Final Response,
+      // get the TSIH from the Login Response.
+      //
+      Session->Tsih = NTOHS (LoginRsp->Tsih);
+    }
+  }
+  //
+  // Flush the response(s) received.
+  //
+  NetbufQueFlush (&Conn->RspQue);
+
+  return Status;
+}
+
+/**
+  Updated the target information according the data received in the iSCSI
+  login response with an target redirection status.
+
+  @param[in, out] Session      The iSCSI session.
+  @param[in]      Data         The data segment which should contain the
+                               TargetAddress key-value list.
+  @param[in]      Len          Length of the data.
+  
+  @retval EFI_SUCCESS          The target address is updated.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NOT_FOUND        The TargetAddress key is not found.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiUpdateTargetAddress (
+  IN OUT ISCSI_SESSION  *Session,
+  IN CHAR8              *Data,
+  IN UINT32             Len
+  )
+{
+  LIST_ENTRY      *KeyValueList;
+  CHAR8           *TargetAddress;
+  CHAR8           *IpStr;
+  EFI_STATUS      Status;
+  UINTN           Number;
+
+  KeyValueList = IScsiBuildKeyValueList (Data, Len);
+  if (KeyValueList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = EFI_NOT_FOUND;
+
+  while (TRUE) {
+    TargetAddress = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_TARGET_ADDRESS);
+    if (TargetAddress == NULL) {
+      break;
+    }
+
+    if (!NET_IS_DIGIT (TargetAddress[0])) {
+      //
+      // The domainname of the target may be presented in three formats: a DNS host name,
+      // a dotted-decimal IPv4 address, or a bracketed IPv6 address. Only accept dotted
+      // IPv4 address.
+      //
+      continue;
+    }
+
+    IpStr = TargetAddress;
+
+    while ((*TargetAddress != 0) && (*TargetAddress != ':') && (*TargetAddress != ',')) {
+      //
+      // NULL, ':' or ',' ends the IPv4 string.
+      //
+      TargetAddress++;
+    }
+
+    if (*TargetAddress == ',') {
+      //
+      // Comma and the portal group tag MUST be ommitted if the TargetAddress is sent
+      // as the result of a redirection.
+      //
+      continue;
+    } else if (*TargetAddress == ':') {
+      *TargetAddress = '\0';
+
+      TargetAddress++;
+
+      Number = AsciiStrDecimalToUintn (TargetAddress);
+      if (Number > 0xFFFF) {
+        continue;
+      } else {
+        Session->ConfigData.NvData.TargetPort = (UINT16) Number;
+      }
+    } else {
+      //
+      // The string only contains the IPv4 address. Use the well known port.
+      //
+      Session->ConfigData.NvData.TargetPort = ISCSI_WELL_KNOWN_PORT;
+    }
+    //
+    // Update the target IP address.
+    //
+    Status = IScsiAsciiStrToIp (IpStr, &Session->ConfigData.NvData.TargetIp);
+    if (EFI_ERROR (Status)) {
+      continue;
+    } else {
+      break;
+    }
+  }
+
+  IScsiFreeKeyValueList (KeyValueList);
+
+  return Status;
+}
+
+/**
+  The callback function to free the net buffer list.
+
+  @param[in]  Arg The opaque parameter.
+**/
+VOID
+EFIAPI
+IScsiFreeNbufList (
+  VOID *Arg
+  )
+{
+  ASSERT (Arg != NULL);
+
+  NetbufFreeList ((LIST_ENTRY     *) Arg);
+  FreePool (Arg);
+}
+
+/**
+  The callback function called in NetBufFree, it does nothing.
+
+  @param[in]   Arg  The opaque parameter.
+**/
+VOID
+EFIAPI
+IScsiNbufExtFree (
+  VOID *Arg
+  )
+{
+}
+
+/**
+  Receive an iSCSI response PDU. An iSCSI response PDU contains an iSCSI PDU header and
+  an optional data segment. The two parts will be put into two blocks of buffers in the
+  net buffer. The digest check will be conducted in this function if needed and the digests
+  will be trimmed from the PDU buffer.
+
+  @param[in]   Conn        The iSCSI connection to receive data from.
+  @param[out]  Pdu         The received iSCSI pdu.
+  @param[in]   Context     The context used to describe information on the caller provided
+                           buffer to receive data segment of the iSCSI pdu, it's optional.
+  @param[in]  HeaderDigest Whether there will be header digest received.
+  @param[in]  DataDigest   Whether there will be data digest.
+  @param[in]  TimeoutEvent The timeout event, it's optional.
+
+  @retval EFI_SUCCESS          An iSCSI pdu is received.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of iSCSI protocol error happened.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiReceivePdu (
+  IN ISCSI_CONNECTION                      *Conn,
+  OUT NET_BUF                              **Pdu,
+  IN ISCSI_IN_BUFFER_CONTEXT               *Context, OPTIONAL
+  IN BOOLEAN                               HeaderDigest,
+  IN BOOLEAN                               DataDigest,
+  IN EFI_EVENT                             TimeoutEvent OPTIONAL
+  )
+{
+  LIST_ENTRY      *NbufList;
+  UINT32          Len;
+  NET_BUF         *PduHdr;
+  UINT8           *Header;
+  EFI_STATUS      Status;
+  UINT32          PadLen;
+  UINT32          InDataOffset;
+  NET_FRAGMENT    Fragment[2];
+  UINT32          FragmentCount;
+  NET_BUF         *DataSeg;
+  UINT32          PadAndCRC32[2];
+
+  NbufList = AllocatePool (sizeof (LIST_ENTRY));
+  if (NbufList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  InitializeListHead (NbufList);
+
+  //
+  // The header digest will be received together with the PDU header if exists.
+  //
+  Len     = sizeof (ISCSI_BASIC_HEADER) + (HeaderDigest ? sizeof (UINT32) : 0);
+  PduHdr  = NetbufAlloc (Len);
+  if (PduHdr == NULL) {
+    FreePool (NbufList);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Header = NetbufAllocSpace (PduHdr, Len, NET_BUF_TAIL);
+  ASSERT (Header != NULL);
+  InsertTailList (NbufList, &PduHdr->List);
+
+  //
+  // First step, receive the BHS of the PDU.
+  //
+  Status = Tcp4IoReceive (&Conn->Tcp4Io, PduHdr, FALSE, TimeoutEvent);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  if (HeaderDigest) {
+    //
+    // TODO: check the header-digest.
+    //
+    //
+    // Trim off the digest.
+    //
+    NetbufTrim (PduHdr, sizeof (UINT32), NET_BUF_TAIL);
+  }
+
+  Len = ISCSI_GET_DATASEG_LEN (Header);
+  if (Len == 0) {
+    //
+    // No data segment.Form the pdu from a list of pdu segments.
+    //
+    *Pdu = NetbufFromBufList (NbufList, 0, 0, IScsiFreeNbufList, NbufList);
+    if (*Pdu == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+    return Status;
+  }
+  //
+  // Get the length of the padding bytes of the data segment.
+  //
+  PadLen = ISCSI_GET_PAD_LEN (Len);
+
+  switch (ISCSI_GET_OPCODE (Header)) {
+  case ISCSI_OPCODE_SCSI_DATA_IN:
+    //
+    // Try to use the buffer described by Context if the PDU is an
+    // iSCSI SCSI data in pdu so as to reduce memory copy overhead.
+    //
+    InDataOffset = ISCSI_GET_BUFFER_OFFSET (Header);
+    if ((Context == NULL) || ((InDataOffset + Len) > Context->InDataLen)) {
+      Status = EFI_PROTOCOL_ERROR;
+      goto ON_EXIT;
+    }
+
+    Fragment[0].Len   = Len;
+    Fragment[0].Bulk  = Context->InData + InDataOffset;
+
+    if (DataDigest || (PadLen != 0)) {
+      //
+      // The data segment is padded, use two fragments to receive it.
+      // The first to receive the useful data. The second to receive the padding.
+      //
+      Fragment[1].Len   = PadLen + (DataDigest ? sizeof (UINT32) : 0);
+      Fragment[1].Bulk  = (UINT8 *)PadAndCRC32 + (4 - PadLen);
+      FragmentCount     = 2;
+    } else {
+      FragmentCount = 1;
+    }
+
+    DataSeg = NetbufFromExt (&Fragment[0], FragmentCount, 0, 0, IScsiNbufExtFree, NULL);
+    if (DataSeg == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    break;
+
+  case ISCSI_OPCODE_SCSI_RSP:
+  case ISCSI_OPCODE_NOP_IN:
+  case ISCSI_OPCODE_LOGIN_RSP:
+  case ISCSI_OPCODE_TEXT_RSP:
+  case ISCSI_OPCODE_ASYNC_MSG:
+  case ISCSI_OPCODE_REJECT:
+  case ISCSI_OPCODE_VENDOR_T0:
+  case ISCSI_OPCODE_VENDOR_T1:
+  case ISCSI_OPCODE_VENDOR_T2:
+    //
+    // Allocate buffer to receive the data segment.
+    //
+    Len += PadLen + (DataDigest ? sizeof (UINT32) : 0);
+    DataSeg = NetbufAlloc (Len);
+    if (DataSeg == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    NetbufAllocSpace (DataSeg, Len, NET_BUF_TAIL);
+    break;
+
+  default:
+    Status = EFI_PROTOCOL_ERROR;
+    goto ON_EXIT;
+  }
+
+  InsertTailList (NbufList, &DataSeg->List);
+
+  //
+  // Receive the data segment with the data digest if any.
+  //
+  Status = Tcp4IoReceive (&Conn->Tcp4Io, DataSeg, FALSE, TimeoutEvent);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  if (DataDigest) {
+    //
+    // TODO: Check the data digest.
+    //
+    NetbufTrim (DataSeg, sizeof (UINT32), NET_BUF_TAIL);
+  }
+
+  if (PadLen != 0) {
+    //
+    // Trim off the padding bytes in the data segment.
+    //
+    NetbufTrim (DataSeg, PadLen, NET_BUF_TAIL);
+  }
+
+  //
+  // Form the pdu from a list of pdu segments.
+  //
+  *Pdu = NetbufFromBufList (NbufList, 0, 0, IScsiFreeNbufList, NbufList);
+  if (*Pdu == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+  }
+
+ON_EXIT:
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the Nbufs in this NbufList and the NbufList itself.
+    //
+    IScsiFreeNbufList (NbufList);
+  }
+
+  return Status;
+}
+
+/**
+  Check and get the result of the prameter negotiation.
+
+  @param[in, out]  Conn          The connection in iSCSI login.
+
+  @retval EFI_SUCCESS          The parmeter check is passed and negotiation is finished.
+  @retval EFI_PROTOCOL_ERROR   Some kind of iSCSI protocol error happened.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+**/
+EFI_STATUS
+IScsiCheckOpParams (
+  IN OUT ISCSI_CONNECTION  *Conn
+  )
+{
+  EFI_STATUS      Status;
+  LIST_ENTRY      *KeyValueList;
+  CHAR8           *Data;
+  UINT32          Len;
+  ISCSI_SESSION   *Session;
+  CHAR8           *Value;
+  UINTN           NumericValue;
+
+  ASSERT (Conn->RspQue.BufNum != 0);
+
+  Session = Conn->Session;
+
+  Len     = Conn->RspQue.BufSize;
+  Data    = AllocatePool (Len);
+  if (Data == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NetbufQueCopy (&Conn->RspQue, 0, Len, (UINT8 *) Data);
+
+  Status = EFI_PROTOCOL_ERROR;
+
+  //
+  // Extract the Key-Value pairs into a list.
+  //
+  KeyValueList = IScsiBuildKeyValueList (Data, Len);
+  if (KeyValueList == NULL) {
+    FreePool (Data);
+    return Status;
+  }
+  //
+  // HeaderDigest
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_HEADER_DIGEST);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  if (AsciiStrCmp (Value, "CRC32") == 0) {
+    if (Conn->HeaderDigest != IScsiDigestCRC32) {
+      goto ON_ERROR;
+    }
+  } else if (AsciiStrCmp (Value, ISCSI_KEY_VALUE_NONE) == 0) {
+    Conn->HeaderDigest = IScsiDigestNone;
+  } else {
+    goto ON_ERROR;
+  }
+  //
+  // DataDigest
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DATA_DIGEST);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  if (AsciiStrCmp (Value, "CRC32") == 0) {
+    if (Conn->DataDigest != IScsiDigestCRC32) {
+      goto ON_ERROR;
+    }
+  } else if (AsciiStrCmp (Value, ISCSI_KEY_VALUE_NONE) == 0) {
+    Conn->DataDigest = IScsiDigestNone;
+  } else {
+    goto ON_ERROR;
+  }
+  //
+  // ErrorRecoveryLevel, result fuction is Minimum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_ERROR_RECOVERY_LEVEL);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue = AsciiStrDecimalToUintn (Value);
+  if (NumericValue > 2) {
+    goto ON_ERROR;
+  }
+
+  Session->ErrorRecoveryLevel = (UINT8) MIN (Session->ErrorRecoveryLevel, NumericValue);
+
+  //
+  // InitialR2T, result function is OR.
+  //
+  if (!Session->InitialR2T) {
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_INITIAL_R2T);
+    if (Value == NULL) {
+      goto ON_ERROR;
+    }
+
+    Session->InitialR2T = (BOOLEAN) (AsciiStrCmp (Value, "Yes") == 0);
+  }
+
+  //
+  // ImmediateData, result function is AND.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_IMMEDIATE_DATA);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  Session->ImmediateData = (BOOLEAN) (Session->ImmediateData && (BOOLEAN) (AsciiStrCmp (Value, "Yes") == 0));
+
+  //
+  // MaxRecvDataSegmentLength is declarative.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_MAX_RECV_DATA_SEGMENT_LENGTH);
+  if (Value != NULL) {
+    Conn->MaxRecvDataSegmentLength = (UINT32) AsciiStrDecimalToUintn (Value);
+  }
+  //
+  // MaxBurstLength, result funtion is Mininum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_MAX_BURST_LENGTH);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue            = AsciiStrDecimalToUintn (Value);
+  Session->MaxBurstLength = (UINT32) MIN (Session->MaxBurstLength, NumericValue);
+
+  //
+  // FirstBurstLength, result function is Minimum. Irrelevant when InitialR2T=Yes and
+  // ImmediateData=No.
+  // This Key/Value is negotiation type.
+  //
+  if (!(Session->InitialR2T && !Session->ImmediateData)) {
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_FIRST_BURST_LENGTH);
+    if (Value == NULL) {
+      goto ON_ERROR;
+    }
+
+    NumericValue              = AsciiStrDecimalToUintn (Value);
+    Session->FirstBurstLength = (UINT32) MIN (Session->FirstBurstLength, NumericValue);
+  }
+
+  //
+  // MaxConnections, result function is Minimum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_MAX_CONNECTIONS);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue = AsciiStrDecimalToUintn (Value);
+  if ((NumericValue == 0) || (NumericValue > 65535)) {
+    goto ON_ERROR;
+  }
+
+  Session->MaxConnections = (UINT32) MIN (Session->MaxConnections, NumericValue);
+
+  //
+  // DataPDUInOrder, result function is OR.
+  //
+  if (!Session->DataPDUInOrder) {
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DATA_PDU_IN_ORDER);
+    if (Value == NULL) {
+      goto ON_ERROR;
+    }
+
+    Session->DataPDUInOrder = (BOOLEAN) (AsciiStrCmp (Value, "Yes") == 0);
+  }
+
+  //
+  // DataSequenceInorder, result function is OR.
+  //
+  if (!Session->DataSequenceInOrder) {
+    Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DATA_SEQUENCE_IN_ORDER);
+    if (Value == NULL) {
+      goto ON_ERROR;
+    }
+
+    Session->DataSequenceInOrder = (BOOLEAN) (AsciiStrCmp (Value, "Yes") == 0);
+  }
+
+  //
+  // DefaultTime2Wait, result function is Maximum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DEFAULT_TIME2WAIT);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue = AsciiStrDecimalToUintn (Value);
+  if (NumericValue == 0) {
+    Session->DefaultTime2Wait = 0;
+  } else if (NumericValue > 3600) {
+    goto ON_ERROR;
+  } else {
+    Session->DefaultTime2Wait = (UINT32) MAX (Session->DefaultTime2Wait, NumericValue);
+  }
+  //
+  // DefaultTime2Retain, result function is Minimum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DEFAULT_TIME2RETAIN);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue = AsciiStrDecimalToUintn (Value);
+  if (NumericValue == 0) {
+    Session->DefaultTime2Retain = 0;
+  } else if (NumericValue > 3600) {
+    goto ON_ERROR;
+  } else {
+    Session->DefaultTime2Retain = (UINT32) MIN (Session->DefaultTime2Retain, NumericValue);
+  }
+  //
+  // MaxOutstandingR2T, result function is Minimum.
+  //
+  Value = IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_MAX_OUTSTANDING_R2T);
+  if (Value == NULL) {
+    goto ON_ERROR;
+  }
+
+  NumericValue = AsciiStrDecimalToUintn (Value);
+  if ((NumericValue == 0) || (NumericValue > 65535)) {
+    goto ON_ERROR;
+  }
+
+  Session->MaxOutstandingR2T = (UINT16) MIN (Session->MaxOutstandingR2T, NumericValue);
+
+  //
+  // Remove declarative key-value pairs, if any.
+  //
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_SESSION_TYPE);
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_TARGET_ALIAS);
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_TARGET_PORTAL_GROUP_TAG);
+  //
+  // Remove the key-value that may not needed for result function is OR.
+  //
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_INITIAL_R2T);
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DATA_PDU_IN_ORDER);
+  IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_DATA_SEQUENCE_IN_ORDER);
+
+  //
+  // Remove irrelevant parameter, if any.
+  //
+  if (Session->InitialR2T && !Session->ImmediateData) {
+    IScsiGetValueByKeyFromList (KeyValueList, ISCSI_KEY_FIRST_BURST_LENGTH);
+  }
+
+  if (IsListEmpty (KeyValueList)) {
+    //
+    // Succeed if no more keys in the list.
+    //
+    Status = EFI_SUCCESS;
+  }
+
+ON_ERROR:
+
+  IScsiFreeKeyValueList (KeyValueList);
+
+  FreePool (Data);
+
+  return Status;
+}
+
+/**
+  Fill the oprational prameters.
+
+  @param[in]       Conn        The connection in iSCSI login.
+  @param[in, out]  Pdu         The iSCSI login request PDU to fill the parameters.
+
+  @retval EFI_SUCCESS          The parmeters are filled into the iSCSI login request PDU.
+**/
+EFI_STATUS
+IScsiFillOpParams (
+  IN ISCSI_CONNECTION  *Conn,
+  IN OUT NET_BUF       *Pdu
+  )
+{
+  ISCSI_SESSION *Session;
+  CHAR8         Value[256];
+
+  Session = Conn->Session;
+
+  AsciiSPrint (Value, sizeof (Value), "%a", (Conn->HeaderDigest == IScsiDigestCRC32) ? "None,CRC32" : "None");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_HEADER_DIGEST, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%a", (Conn->DataDigest == IScsiDigestCRC32) ? "None,CRC32" : "None");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_DATA_DIGEST, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->ErrorRecoveryLevel);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_ERROR_RECOVERY_LEVEL, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%a", Session->InitialR2T ? "Yes" : "No");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_INITIAL_R2T, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%a", Session->ImmediateData ? "Yes" : "No");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_IMMEDIATE_DATA, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", MAX_RECV_DATA_SEG_LEN_IN_FFP);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_MAX_RECV_DATA_SEGMENT_LENGTH, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->MaxBurstLength);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_MAX_BURST_LENGTH, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->FirstBurstLength);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_FIRST_BURST_LENGTH, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->MaxConnections);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_MAX_CONNECTIONS, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%a", Session->DataPDUInOrder ? "Yes" : "No");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_DATA_PDU_IN_ORDER, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%a", Session->DataSequenceInOrder ? "Yes" : "No");
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_DATA_SEQUENCE_IN_ORDER, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->DefaultTime2Wait);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_DEFAULT_TIME2WAIT, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->DefaultTime2Retain);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_DEFAULT_TIME2RETAIN, Value);
+
+  AsciiSPrint (Value, sizeof (Value), "%d", Session->MaxOutstandingR2T);
+  IScsiAddKeyValuePair (Pdu, ISCSI_KEY_MAX_OUTSTANDING_R2T, Value);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Pad the iSCSI AHS or data segment to an integer number of 4 byte words.
+
+  @param[in, out]  Pdu         The iSCSI pdu which contains segments to pad.
+  @param[in]       Len         The length of the last semgnet in the PDU.
+
+  @retval EFI_SUCCESS          The segment is padded or no need to pad it.
+  @retval EFI_OUT_OF_RESOURCES There is not enough remaining free space to add the
+                               padding bytes.
+**/
+EFI_STATUS
+IScsiPadSegment (
+  IN OUT NET_BUF  *Pdu,
+  IN UINT32       Len
+  )
+{
+  UINT32  PadLen;
+  UINT8   *Data;
+
+  PadLen = ISCSI_GET_PAD_LEN (Len);
+
+  if (PadLen != 0) {
+    Data = NetbufAllocSpace (Pdu, PadLen, NET_BUF_TAIL);
+    if (Data == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    ZeroMem (Data, PadLen);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Build a key-value list from the data segment.
+
+  @param[in]  Data The data segment containing the key-value pairs.
+  @param[in]  Len  Length of the data segment.
+
+  @return The key-value list.
+  @retval NULL Other errors as indicated.
+**/
+LIST_ENTRY *
+IScsiBuildKeyValueList (
+  IN CHAR8  *Data,
+  IN UINT32 Len
+  )
+{
+  LIST_ENTRY            *ListHead;
+  ISCSI_KEY_VALUE_PAIR  *KeyValuePair;
+
+  ListHead = AllocatePool (sizeof (LIST_ENTRY));
+  if (ListHead == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (ListHead);
+
+  while (Len > 0) {
+    KeyValuePair = AllocatePool (sizeof (ISCSI_KEY_VALUE_PAIR));
+    if (KeyValuePair == NULL) {
+      goto ON_ERROR;
+    }
+
+    InitializeListHead (&KeyValuePair->List);
+
+    KeyValuePair->Key = Data;
+
+    while ((Len > 0) && (*Data != '=')) {
+      Len--;
+      Data++;
+    }
+
+    if (*Data == '=') {
+      *Data = '\0';
+
+      Data++;
+      Len--;
+    } else {
+      FreePool (KeyValuePair);
+      goto ON_ERROR;
+    }
+
+    KeyValuePair->Value = Data;
+
+    InsertTailList (ListHead, &KeyValuePair->List);;
+
+    Data += AsciiStrLen (KeyValuePair->Value) + 1;
+    Len -= (UINT32) AsciiStrLen (KeyValuePair->Value) + 1;
+  }
+
+  return ListHead;
+
+ON_ERROR:
+
+  IScsiFreeKeyValueList (ListHead);
+
+  return NULL;
+}
+
+/**
+  Get the value string by the key name from the key-value list. If found,
+  the key-value entry will be removed from the list.
+
+  @param[in, out]  KeyValueList  The key-value list.
+  @param[in]       Key           The key name to find.
+
+  @return The value string.
+**/
+CHAR8 *
+IScsiGetValueByKeyFromList (
+  IN OUT LIST_ENTRY  *KeyValueList,
+  IN CHAR8           *Key
+  )
+{
+  LIST_ENTRY            *Entry;
+  ISCSI_KEY_VALUE_PAIR  *KeyValuePair;
+  CHAR8                 *Value;
+
+  Value = NULL;
+
+  NET_LIST_FOR_EACH (Entry, KeyValueList) {
+    KeyValuePair = NET_LIST_USER_STRUCT (Entry, ISCSI_KEY_VALUE_PAIR, List);
+
+    if (AsciiStrCmp (KeyValuePair->Key, Key) == 0) {
+      Value = KeyValuePair->Value;
+
+      RemoveEntryList (&KeyValuePair->List);
+      FreePool (KeyValuePair);
+      break;
+    }
+  }
+
+  return Value;
+}
+
+/**
+  Free the key-value list.
+
+  @param[in]  KeyValueList The key-value list.
+**/
+VOID
+IScsiFreeKeyValueList (
+  IN LIST_ENTRY      *KeyValueList
+  )
+{
+  LIST_ENTRY            *Entry;
+  ISCSI_KEY_VALUE_PAIR  *KeyValuePair;
+
+  while (!IsListEmpty (KeyValueList)) {
+    Entry         = NetListRemoveHead (KeyValueList);
+    KeyValuePair  = NET_LIST_USER_STRUCT (Entry, ISCSI_KEY_VALUE_PAIR, List);
+
+    FreePool (KeyValuePair);
+  }
+
+  FreePool (KeyValueList);
+}
+
+/**
+  Normalize the iSCSI name according to RFC.
+
+  @param[in, out]  Name       The iSCSI name.
+  @param[in]       Len        length of the iSCSI name.
+
+  @retval EFI_SUCCESS        The iSCSI name is valid and normalized.
+  @retval EFI_PROTOCOL_ERROR The iSCSI name is mal-formatted or not in the IQN format.
+**/
+EFI_STATUS
+IScsiNormalizeName (
+  IN OUT CHAR8  *Name,
+  IN UINTN      Len
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < Len; Index++) {
+    if (NET_IS_UPPER_CASE_CHAR (Name[Index])) {
+      //
+      // Convert the upper-case characters to lower-case ones
+      //
+      Name[Index] = (CHAR8) (Name[Index] - 'A' + 'a');
+    }
+
+    if (!NET_IS_LOWER_CASE_CHAR (Name[Index]) &&
+        !NET_IS_DIGIT (Name[Index]) &&
+        (Name[Index] != '-') &&
+        (Name[Index] != '.') &&
+        (Name[Index] != ':')
+        ) {
+      //
+      // ASCII dash, dot, colon lower-case characters and digit characters
+      // are allowed.
+      //
+      return EFI_PROTOCOL_ERROR;
+    }
+  }
+
+  if ((Len < 4) || (CompareMem (Name, "iqn.", 4) != 0)) {
+    //
+    // Only IQN format is accepted now.
+    //
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create an iSCSI task control block.
+
+  @param[in]   Conn           The connection on which the task control block will be created.
+  @param[out]  Tcb            The newly created task control block.
+
+  @retval EFI_SUCCESS          The task control block is created.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NOT_READY        The target can not accept new commands.
+**/
+EFI_STATUS
+IScsiNewTcb (
+  IN  ISCSI_CONNECTION  *Conn,
+  OUT ISCSI_TCB         **Tcb
+  )
+{
+  ISCSI_SESSION *Session;
+  ISCSI_TCB     *NewTcb;
+
+  ASSERT (Tcb != NULL);
+
+  Session = Conn->Session;
+
+  if (ISCSI_SEQ_GT (Session->CmdSN, Session->MaxCmdSN)) {
+    return EFI_NOT_READY;
+  }
+
+  NewTcb = AllocateZeroPool (sizeof (ISCSI_TCB));
+  if (NewTcb == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  InitializeListHead (&NewTcb->Link);
+
+  NewTcb->SoFarInOrder      = TRUE;
+  NewTcb->InitiatorTaskTag  = Session->InitiatorTaskTag;
+  NewTcb->CmdSN             = Session->CmdSN;
+  NewTcb->Conn              = Conn;
+
+  InsertTailList (&Session->TcbList, &NewTcb->Link);
+
+  //
+  // Advance the initiator task tag.
+  //
+  Session->InitiatorTaskTag++;
+  Session->CmdSN++;
+
+  *Tcb = NewTcb;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Delete the tcb from the connection and destroy it.
+
+  @param[in]  Tcb The tcb to delete.
+**/
+VOID
+IScsiDelTcb (
+  IN ISCSI_TCB  *Tcb
+  )
+{
+  RemoveEntryList (&Tcb->Link);
+
+  FreePool (Tcb);
+}
+
+/**
+  Find the task control block by the initator task tag.
+
+  @param[in]  TcbList         The tcb list.
+  @param[in]  InitiatorTaskTag The initiator task tag.
+
+  @return The task control block found.
+**/
+ISCSI_TCB *
+IScsiFindTcbByITT (
+  IN LIST_ENTRY      *TcbList,
+  IN UINT32          InitiatorTaskTag
+  )
+{
+  ISCSI_TCB       *Tcb;
+  LIST_ENTRY      *Entry;
+
+  Tcb = NULL;
+
+  NET_LIST_FOR_EACH (Entry, TcbList) {
+    Tcb = NET_LIST_USER_STRUCT (Entry, ISCSI_TCB, Link);
+
+    if (Tcb->InitiatorTaskTag == InitiatorTaskTag) {
+      break;
+    }
+
+    Tcb = NULL;
+  }
+
+  return Tcb;
+}
+
+/**
+  Create a data segment, pad it and calculate the CRC if needed.
+
+  @param[in]  Data       The data to fill into the data segment.
+  @param[in]  Len        Length of the data.
+  @param[in]  DataDigest Whether to calculate CRC for this data segment.
+
+  @return The net buffer wrapping the data segment.
+**/
+NET_BUF *
+IScsiNewDataSegment (
+  IN UINT8    *Data,
+  IN UINT32   Len,
+  IN BOOLEAN  DataDigest
+  )
+{
+  NET_FRAGMENT  Fragment[2];
+  UINT32        FragmentCount;
+  UINT32        PadLen;
+  NET_BUF       *DataSeg;
+
+  Fragment[0].Len   = Len;
+  Fragment[0].Bulk  = Data;
+
+  PadLen            = ISCSI_GET_PAD_LEN (Len);
+  if (PadLen != 0) {
+    Fragment[1].Len   = PadLen;
+    Fragment[1].Bulk  = (UINT8 *) &mDataSegPad;
+
+    FragmentCount     = 2;
+  } else {
+    FragmentCount = 1;
+  }
+
+  DataSeg = NetbufFromExt (&Fragment[0], FragmentCount, 0, 0, IScsiNbufExtFree, NULL);
+
+  return DataSeg;
+}
+
+/**
+  Create a iSCSI SCSI command PDU to encapsulate the command issued
+  by SCSI through the EXT SCSI PASS THRU Protocol.
+
+  @param[in]  Packet The EXT SCSI PASS THRU request packet containing the SCSI command.
+  @param[in]  Lun    The LUN.
+  @param[in]  Tcb    The tcb assocated with this SCSI command.
+
+  @return The  created iSCSI SCSI command PDU.
+  @retval NULL Other errors as indicated.
+**/
+NET_BUF *
+IScsiNewScsiCmdPdu (
+  IN EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET *Packet,
+  IN UINT64                                     Lun,
+  IN ISCSI_TCB                                  *Tcb
+  )
+{
+  LIST_ENTRY                      *NbufList;
+  NET_BUF                         *Pdu;
+  NET_BUF                         *PduHeader;
+  NET_BUF                         *DataSeg;
+  SCSI_COMMAND                    *ScsiCmd;
+  UINT8                           AHSLength;
+  UINT32                          Length;
+  ISCSI_ADDITIONAL_HEADER         *Header;
+  ISCSI_BI_EXP_READ_DATA_LEN_AHS  *BiExpReadDataLenAHS;
+  ISCSI_SESSION                   *Session;
+  UINT32                          ImmediateDataLen;
+
+  AHSLength = 0;
+
+  if (Packet->DataDirection == DataBi) {
+    //
+    // Bi directional Read/Write command, the bidirectional expected
+    // read data length AHS is required.
+    //
+    AHSLength += sizeof (ISCSI_BI_EXP_READ_DATA_LEN_AHS);
+  }
+
+  if (Packet->CdbLength > 16) {
+    //
+    // The CDB exceeds 16 bytes, an extended CDB AHS is required.
+    //
+    AHSLength = (UINT8) (AHSLength + (ISCSI_ROUNDUP (Packet->CdbLength - 16) + sizeof (ISCSI_ADDITIONAL_HEADER)));
+  }
+
+  Length    = sizeof (SCSI_COMMAND) + AHSLength;
+  PduHeader = NetbufAlloc (Length);
+  if (PduHeader == NULL) {
+    return NULL;
+  }
+
+  ScsiCmd = (SCSI_COMMAND *) NetbufAllocSpace (PduHeader, Length, NET_BUF_TAIL);
+  if (ScsiCmd == NULL) {
+    NetbufFree (PduHeader);
+    return NULL;
+  }	
+  Header  = (ISCSI_ADDITIONAL_HEADER *) (ScsiCmd + 1);
+
+  ZeroMem (ScsiCmd, Length);
+
+  ISCSI_SET_OPCODE (ScsiCmd, ISCSI_OPCODE_SCSI_CMD, 0);
+  ISCSI_SET_FLAG (ScsiCmd, ISCSI_TASK_ATTR_SIMPLE);
+
+  //
+  // Set the READ/WRITE flags according to the IO type of this request.
+  //
+  switch (Packet->DataDirection) {
+  case DataIn:
+    ISCSI_SET_FLAG (ScsiCmd, SCSI_CMD_PDU_FLAG_READ);
+    ScsiCmd->ExpDataXferLength = NTOHL (Packet->InTransferLength);
+    break;
+
+  case DataOut:
+    ISCSI_SET_FLAG (ScsiCmd, SCSI_CMD_PDU_FLAG_WRITE);
+    ScsiCmd->ExpDataXferLength = NTOHL (Packet->OutTransferLength);
+    break;
+
+  case DataBi:
+    ISCSI_SET_FLAG (ScsiCmd, SCSI_CMD_PDU_FLAG_READ | SCSI_CMD_PDU_FLAG_WRITE);
+    ScsiCmd->ExpDataXferLength = NTOHL (Packet->OutTransferLength);
+
+    //
+    // Fill the bidirectional expected read data length AHS.
+    //
+    BiExpReadDataLenAHS                     = (ISCSI_BI_EXP_READ_DATA_LEN_AHS *) Header;
+    Header = (ISCSI_ADDITIONAL_HEADER *) (BiExpReadDataLenAHS + 1);
+
+    BiExpReadDataLenAHS->Length = NTOHS (5);
+    BiExpReadDataLenAHS->Type = ISCSI_AHS_TYPE_BI_EXP_READ_DATA_LEN;
+    BiExpReadDataLenAHS->ExpReadDataLength = NTOHL (Packet->InTransferLength);
+
+    break;
+  }
+
+  ScsiCmd->TotalAHSLength = AHSLength;
+  CopyMem (ScsiCmd->Lun, &Lun, sizeof (ScsiCmd->Lun));
+  ScsiCmd->InitiatorTaskTag = NTOHL (Tcb->InitiatorTaskTag);
+  ScsiCmd->CmdSN            = NTOHL (Tcb->CmdSN);
+  ScsiCmd->ExpStatSN        = NTOHL (Tcb->Conn->ExpStatSN);
+
+  CopyMem (ScsiCmd->Cdb, Packet->Cdb, sizeof (ScsiCmd->Cdb));
+
+  if (Packet->CdbLength > 16) {
+    Header->Length  = NTOHS ((UINT16) (Packet->CdbLength - 15));
+    Header->Type    = ISCSI_AHS_TYPE_EXT_CDB;
+
+    CopyMem (Header + 1, (UINT8 *) Packet->Cdb + 16, Packet->CdbLength - 16);
+  }
+
+  Pdu               = PduHeader;
+  Session           = Tcb->Conn->Session;
+  ImmediateDataLen  = 0;
+
+  if (Session->ImmediateData && (Packet->OutTransferLength != 0)) {
+    //
+    // Send immediate data in this SCSI Command PDU. The length of the immeidate
+    // data is the minimum of FirstBurstLength, the data length to be xfered and
+    // the MaxRecvdataSegmentLength on this connection.
+    //
+    ImmediateDataLen  = MIN (Session->FirstBurstLength, Packet->OutTransferLength);
+    ImmediateDataLen  = MIN (ImmediateDataLen, Tcb->Conn->MaxRecvDataSegmentLength);
+
+    //
+    // Update the data segment length in the PDU header.
+    //
+    ISCSI_SET_DATASEG_LEN (ScsiCmd, ImmediateDataLen);
+
+    //
+    // Create the data segment.
+    //
+    DataSeg = IScsiNewDataSegment ((UINT8 *) Packet->OutDataBuffer, ImmediateDataLen, FALSE);
+    if (DataSeg == NULL) {
+      NetbufFree (PduHeader);
+      Pdu = NULL;
+      goto ON_EXIT;
+    }
+
+    NbufList = AllocatePool (sizeof (LIST_ENTRY));
+    if (NbufList == NULL) {
+      NetbufFree (PduHeader);
+      NetbufFree (DataSeg);
+
+      Pdu = NULL;
+      goto ON_EXIT;
+    }
+
+    InitializeListHead (NbufList);
+    InsertTailList (NbufList, &PduHeader->List);
+    InsertTailList (NbufList, &DataSeg->List);
+
+    Pdu = NetbufFromBufList (NbufList, 0, 0, IScsiFreeNbufList, NbufList);
+    if (Pdu == NULL) {
+      IScsiFreeNbufList (NbufList);
+    }
+  }
+
+  if (Session->InitialR2T ||
+      (ImmediateDataLen == Session->FirstBurstLength) ||
+      (ImmediateDataLen == Packet->OutTransferLength)
+      ) {
+    //
+    // Unsolicited data out sequence is not allowed,
+    // or FirstBurstLength data is already sent out by immediate data
+    // or all the OUT data accompany this SCSI packet is sent as
+    // immediate data, the final flag should be set on this SCSI Command
+    // PDU.
+    //
+    ISCSI_SET_FLAG (ScsiCmd, ISCSI_BHS_FLAG_FINAL);
+  }
+
+ON_EXIT:
+
+  return Pdu;
+}
+
+/**
+  Create a new iSCSI SCSI Data Out PDU.
+
+  @param[in]  Data   The data to put into the Data Out PDU.
+  @param[in]  Len    Length of the data.
+  @param[in]  DataSN The DataSN of the Data Out PDU.
+  @param[in]  Tcb    The task control block of this Data Out PDU.
+  @param[in]  Lun    The LUN.
+
+  @return The net buffer wrapping the Data Out PDU.
+  @retval NULL Other errors as indicated.
+**/
+NET_BUF *
+IScsiNewDataOutPdu (
+  IN UINT8      *Data,
+  IN UINT32     Len,
+  IN UINT32     DataSN,
+  IN ISCSI_TCB  *Tcb,
+  IN UINT64     Lun
+  )
+{
+  LIST_ENTRY          *NbufList;
+  NET_BUF             *PduHdr;
+  NET_BUF             *DataSeg;
+  NET_BUF             *Pdu;
+  ISCSI_SCSI_DATA_OUT *DataOutHdr;
+  ISCSI_XFER_CONTEXT  *XferContext;
+
+  NbufList = AllocatePool (sizeof (LIST_ENTRY));
+  if (NbufList == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (NbufList);
+
+  //
+  // Allocate memory for the BHS.
+  //
+  PduHdr = NetbufAlloc (sizeof (ISCSI_SCSI_DATA_OUT));
+  if (PduHdr == NULL) {
+    FreePool (NbufList);
+    return NULL;
+  }
+  //
+  // Insert the BHS into the buffer list.
+  //
+  InsertTailList (NbufList, &PduHdr->List);
+
+  DataOutHdr  = (ISCSI_SCSI_DATA_OUT *) NetbufAllocSpace (PduHdr, sizeof (ISCSI_SCSI_DATA_OUT), NET_BUF_TAIL);
+  ASSERT (DataOutHdr != NULL);
+  XferContext = &Tcb->XferContext;
+
+  ZeroMem (DataOutHdr, sizeof (ISCSI_SCSI_DATA_OUT));
+
+  //
+  // Set the flags and fields of the Data Out PDU BHS.
+  //
+  ISCSI_SET_OPCODE (DataOutHdr, ISCSI_OPCODE_SCSI_DATA_OUT, 0);
+  ISCSI_SET_DATASEG_LEN (DataOutHdr, Len);
+
+  DataOutHdr->InitiatorTaskTag  = HTONL (Tcb->InitiatorTaskTag);
+  DataOutHdr->TargetTransferTag = HTONL (XferContext->TargetTransferTag);
+  DataOutHdr->ExpStatSN         = HTONL (Tcb->Conn->ExpStatSN);
+  DataOutHdr->DataSN            = HTONL (DataSN);
+  DataOutHdr->BufferOffset      = HTONL (XferContext->Offset);
+
+  if (XferContext->TargetTransferTag != ISCSI_RESERVED_TAG) {
+    CopyMem (&DataOutHdr->Lun, &Lun, sizeof (DataOutHdr->Lun));
+  }
+  //
+  // Build the data segment for this Data Out PDU.
+  //
+  DataSeg = IScsiNewDataSegment (Data, Len, FALSE);
+  if (DataSeg == NULL) {
+    IScsiFreeNbufList (NbufList);
+    return NULL;
+  }
+  //
+  // Put the data segment into the buffer list and combine it with the BHS
+  // into a full Data Out PDU.
+  //
+  InsertTailList (NbufList, &DataSeg->List);
+  Pdu = NetbufFromBufList (NbufList, 0, 0, IScsiFreeNbufList, NbufList);
+  if (Pdu == NULL) {
+    IScsiFreeNbufList (NbufList);
+  }
+
+  return Pdu;
+}
+
+/**
+  Generate a consecutive sequence of iSCSI SCSI Data Out PDUs.
+
+  @param[in]  Data The data  which will be carried by the sequence of iSCSI SCSI Data Out PDUs.
+  @param[in]  Tcb  The task control block of the data to send out.
+  @param[in]  Lun  The LUN the data will be sent to.
+
+  @return A list of net buffers with each of them wraps an iSCSI SCSI Data Out PDU.
+  @retval NULL Other errors as indicated.
+**/
+LIST_ENTRY *
+IScsiGenerateDataOutPduSequence (
+  IN UINT8      *Data,
+  IN ISCSI_TCB  *Tcb,
+  IN UINT64     Lun
+  )
+{
+  LIST_ENTRY          *PduList;
+  UINT32              DataSN;
+  UINT32              DataLen;
+  NET_BUF             *DataOutPdu;
+  ISCSI_CONNECTION    *Conn;
+  ISCSI_XFER_CONTEXT  *XferContext;
+  UINT8               *DataOutPacket;
+
+  PduList = AllocatePool (sizeof (LIST_ENTRY));
+  if (PduList == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (PduList);
+
+  DataSN      = 0;
+  Conn        = Tcb->Conn;
+  DataOutPdu  = NULL;
+  XferContext = &Tcb->XferContext;
+
+  while (XferContext->DesiredLength > 0) {
+    //
+    // Determine the length of data this Data Out PDU can carry.
+    //
+    DataLen = MIN (XferContext->DesiredLength, Conn->MaxRecvDataSegmentLength);
+
+    //
+    // Create a Data Out PDU.
+    //
+    DataOutPdu = IScsiNewDataOutPdu (Data, DataLen, DataSN, Tcb, Lun);
+    if (DataOutPdu == NULL) {
+      IScsiFreeNbufList (PduList);
+      PduList = NULL;
+
+      goto ON_EXIT;
+    }
+
+    InsertTailList (PduList, &DataOutPdu->List);
+
+    //
+    // Update the context and DataSN.
+    //
+    XferContext->Offset += DataLen;
+    XferContext->DesiredLength -= DataLen;
+    DataSN++;
+    Data += DataLen;
+  }
+  //
+  // Set the F bit for the last data out PDU in this sequence.
+  //
+  DataOutPacket = NetbufGetByte (DataOutPdu, 0, NULL);
+  if (DataOutPacket == NULL) {
+    IScsiFreeNbufList (PduList);
+    PduList = NULL;
+    goto ON_EXIT;
+  }
+
+  ISCSI_SET_FLAG (DataOutPacket, ISCSI_BHS_FLAG_FINAL);
+
+ON_EXIT:
+
+  return PduList;
+}
+
+/**
+  Send the Data in a sequence of Data Out PDUs one by one.
+
+  @param[in]  Data            The data to carry by Data Out PDUs.
+  @param[in]  Lun             The LUN the data will be sent to.
+  @param[in]  Tcb             The task control block.
+
+  @retval EFI_SUCCES           The data is sent out to the LUN.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiSendDataOutPduSequence (
+  IN UINT8      *Data,
+  IN UINT64     Lun,
+  IN ISCSI_TCB  *Tcb
+  )
+{
+  LIST_ENTRY      *DataOutPduList;
+  LIST_ENTRY      *Entry;
+  NET_BUF         *Pdu;
+  EFI_STATUS      Status;
+
+  //
+  // Generate the Data Out PDU sequence.
+  //
+  DataOutPduList = IScsiGenerateDataOutPduSequence (Data, Tcb, Lun);
+  if (DataOutPduList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Send the Data Out PDU's one by one.
+  //
+  NET_LIST_FOR_EACH (Entry, DataOutPduList) {
+    Pdu     = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    Status  = Tcp4IoTransmit (&Tcb->Conn->Tcp4Io, Pdu);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  IScsiFreeNbufList (DataOutPduList);
+
+  return Status;
+}
+
+/**
+  Process the received iSCSI SCSI Data In PDU.
+
+  @param[in]        Pdu      The Data In PDU received.
+  @param[in]        Tcb      The task control block.
+  @param[in, out]   Packet   The EXT SCSI PASS THRU request packet.
+
+  @retval EFI_SUCCES           The check on the Data IN PDU is passed and some update
+                               actions are taken.
+  @retval EFI_PROTOCOL_ERROR   Some kind of iSCSI protocol errror happened.
+  @retval EFI_BAD_BUFFER_SIZEE The buffer was not the proper size for the request.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiOnDataInRcvd (
+  IN NET_BUF                                         *Pdu,
+  IN ISCSI_TCB                                       *Tcb,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet
+  )
+{
+  ISCSI_SCSI_DATA_IN  *DataInHdr;
+  EFI_STATUS          Status;
+
+  DataInHdr                   = (ISCSI_SCSI_DATA_IN *) NetbufGetByte (Pdu, 0, NULL);
+  if (DataInHdr == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  DataInHdr->InitiatorTaskTag = NTOHL (DataInHdr->InitiatorTaskTag);
+  DataInHdr->ExpCmdSN         = NTOHL (DataInHdr->ExpCmdSN);
+  DataInHdr->MaxCmdSN         = NTOHL (DataInHdr->MaxCmdSN);
+  DataInHdr->DataSN           = NTOHL (DataInHdr->DataSN);
+
+  //
+  // Check the DataSN.
+  //
+  Status = IScsiCheckSN (&Tcb->ExpDataSN, DataInHdr->DataSN);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (DataInHdr->InitiatorTaskTag != Tcb->InitiatorTaskTag) {
+    return EFI_PROTOCOL_ERROR;
+  }
+  //
+  // Update the command related sequence numbers.
+  //
+  IScsiUpdateCmdSN (Tcb->Conn->Session, DataInHdr->MaxCmdSN, DataInHdr->ExpCmdSN);
+
+  if (ISCSI_FLAG_ON (DataInHdr, SCSI_DATA_IN_PDU_FLAG_STATUS_VALID)) {
+    if (!ISCSI_FLAG_ON (DataInHdr, ISCSI_BHS_FLAG_FINAL)) {
+      //
+      // The S bit is on but the F bit is off.
+      //
+      return EFI_PROTOCOL_ERROR;
+    }
+
+    Tcb->StatusXferd = TRUE;
+
+    if (ISCSI_FLAG_ON (DataInHdr, SCSI_DATA_IN_PDU_FLAG_OVERFLOW | SCSI_DATA_IN_PDU_FLAG_UNDERFLOW)) {
+      //
+      // Underflow and Overflow are mutual flags.
+      //
+      return EFI_PROTOCOL_ERROR;
+    }
+    //
+    // S bit is on, the StatSN is valid.
+    //
+    Status = IScsiCheckSN (&Tcb->Conn->ExpStatSN, NTOHL (DataInHdr->StatSN));
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Packet->HostAdapterStatus = 0;
+    Packet->TargetStatus      = DataInHdr->Status;
+
+    if (ISCSI_FLAG_ON (DataInHdr, SCSI_RSP_PDU_FLAG_OVERFLOW)) {
+      Packet->InTransferLength += NTOHL (DataInHdr->ResidualCount);
+      Status = EFI_BAD_BUFFER_SIZE;
+    }
+
+    if (ISCSI_FLAG_ON (DataInHdr, SCSI_RSP_PDU_FLAG_UNDERFLOW)) {
+      Packet->InTransferLength -= NTOHL (DataInHdr->ResidualCount);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Process the received iSCSI R2T PDU.
+
+  @param[in]       Pdu       The R2T PDU received.
+  @param[in]       Tcb       The task control block.
+  @param[in]       Lun       The Lun.
+  @param[in, out]  Packet    The EXT SCSI PASS THRU request packet.
+
+  @retval EFI_SUCCES         The R2T PDU is valid and the solicited data is sent out.
+  @retval EFI_PROTOCOL_ERROR Some kind of iSCSI protocol errror happened.
+  @retval Others             Other errors as indicated.
+**/
+EFI_STATUS
+IScsiOnR2TRcvd (
+  IN NET_BUF                                         *Pdu,
+  IN ISCSI_TCB                                       *Tcb,
+  IN UINT64                                          Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet
+  )
+{
+  ISCSI_READY_TO_TRANSFER *R2THdr;
+  EFI_STATUS              Status;
+  ISCSI_XFER_CONTEXT      *XferContext;
+  UINT8                   *Data;
+
+  R2THdr = (ISCSI_READY_TO_TRANSFER *) NetbufGetByte (Pdu, 0, NULL);
+  if (R2THdr == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  R2THdr->InitiatorTaskTag = NTOHL (R2THdr->InitiatorTaskTag);
+  R2THdr->TargetTransferTag = NTOHL (R2THdr->TargetTransferTag);
+  R2THdr->StatSN = NTOHL (R2THdr->StatSN);
+  R2THdr->R2TSeqNum = NTOHL (R2THdr->R2TSeqNum);
+  R2THdr->BufferOffset = NTOHL (R2THdr->BufferOffset);
+  R2THdr->DesiredDataTransferLength = NTOHL (R2THdr->DesiredDataTransferLength);
+
+  if ((R2THdr->InitiatorTaskTag != Tcb->InitiatorTaskTag) || !ISCSI_SEQ_EQ (R2THdr->StatSN, Tcb->Conn->ExpStatSN)) {
+    return EFI_PROTOCOL_ERROR;;
+  }
+  //
+  // Check the sequence number.
+  //
+  Status = IScsiCheckSN (&Tcb->ExpDataSN, R2THdr->R2TSeqNum);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  XferContext                     = &Tcb->XferContext;
+  XferContext->TargetTransferTag  = R2THdr->TargetTransferTag;
+  XferContext->Offset             = R2THdr->BufferOffset;
+  XferContext->DesiredLength      = R2THdr->DesiredDataTransferLength;
+
+  if (((XferContext->Offset + XferContext->DesiredLength) > Packet->OutTransferLength) ||
+      (XferContext->DesiredLength > Tcb->Conn->Session->MaxBurstLength)
+      ) {
+    return EFI_PROTOCOL_ERROR;
+  }
+  //
+  // Send the data solicited by this R2T.
+  //
+  Data    = (UINT8 *) Packet->OutDataBuffer + XferContext->Offset;
+  Status  = IScsiSendDataOutPduSequence (Data, Lun, Tcb);
+
+  return Status;
+}
+
+/**
+  Process the received iSCSI SCSI Response PDU.
+
+  @param[in]       Pdu      The Response PDU received.
+  @param[in]       Tcb      The task control block.
+  @param[in, out]  Packet   The EXT SCSI PASS THRU request packet.
+
+  @retval EFI_SUCCES         The Response PDU is processed.
+  @retval EFI_PROTOCOL_ERROR Some kind of iSCSI protocol errror happened.
+  @retval EFI_BAD_BUFFER_SIZEE The buffer was not the proper size for the request.
+  @retval Others             Other errors as indicated.
+**/
+EFI_STATUS
+IScsiOnScsiRspRcvd (
+  IN NET_BUF                                         *Pdu,
+  IN ISCSI_TCB                                       *Tcb,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet
+  )
+{
+  SCSI_RESPONSE     *ScsiRspHdr;
+  ISCSI_SENSE_DATA  *SenseData;
+  EFI_STATUS        Status;
+  UINT32            DataSegLen;
+
+  ScsiRspHdr                    = (SCSI_RESPONSE *) NetbufGetByte (Pdu, 0, NULL);
+  if (ScsiRspHdr == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  ScsiRspHdr->InitiatorTaskTag  = NTOHL (ScsiRspHdr->InitiatorTaskTag);
+  if (ScsiRspHdr->InitiatorTaskTag != Tcb->InitiatorTaskTag) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  ScsiRspHdr->StatSN  = NTOHL (ScsiRspHdr->StatSN);
+
+  Status              = IScsiCheckSN (&Tcb->Conn->ExpStatSN, ScsiRspHdr->StatSN);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  ScsiRspHdr->MaxCmdSN  = NTOHL (ScsiRspHdr->MaxCmdSN);
+  ScsiRspHdr->ExpCmdSN  = NTOHL (ScsiRspHdr->ExpCmdSN);
+  IScsiUpdateCmdSN (Tcb->Conn->Session, ScsiRspHdr->MaxCmdSN, ScsiRspHdr->ExpCmdSN);
+
+  Tcb->StatusXferd          = TRUE;
+
+  Packet->HostAdapterStatus = ScsiRspHdr->Response;
+  if (Packet->HostAdapterStatus != ISCSI_SERVICE_RSP_COMMAND_COMPLETE_AT_TARGET) {
+    return EFI_SUCCESS;
+  }
+
+  Packet->TargetStatus = ScsiRspHdr->Status;
+
+  if (ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_BI_READ_OVERFLOW | SCSI_RSP_PDU_FLAG_BI_READ_UNDERFLOW) ||
+      ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_OVERFLOW | SCSI_RSP_PDU_FLAG_UNDERFLOW)
+        ) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  if (ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_BI_READ_OVERFLOW)) {
+    Packet->InTransferLength += NTOHL (ScsiRspHdr->BiReadResidualCount);
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_BI_READ_UNDERFLOW)) {
+    Packet->InTransferLength -= NTOHL (ScsiRspHdr->BiReadResidualCount);
+  }
+
+  if (ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_OVERFLOW)) {
+    if (Packet->DataDirection == DataIn) {
+      Packet->InTransferLength += NTOHL (ScsiRspHdr->ResidualCount);
+    } else {
+      Packet->OutTransferLength += NTOHL (ScsiRspHdr->ResidualCount);
+    }
+
+    Status = EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (ISCSI_FLAG_ON (ScsiRspHdr, SCSI_RSP_PDU_FLAG_UNDERFLOW)) {
+    if (Packet->DataDirection == DataIn) {
+      Packet->InTransferLength -= NTOHL (ScsiRspHdr->ResidualCount);
+    } else {
+      Packet->OutTransferLength -= NTOHL (ScsiRspHdr->ResidualCount);
+    }
+  }
+
+  DataSegLen = ISCSI_GET_DATASEG_LEN (ScsiRspHdr);
+  if (DataSegLen != 0) {
+    SenseData               = (ISCSI_SENSE_DATA *) NetbufGetByte (Pdu, sizeof (SCSI_RESPONSE), NULL);
+    if (SenseData == NULL) {
+      return EFI_PROTOCOL_ERROR;
+    }
+
+    SenseData->Length       = NTOHS (SenseData->Length);
+
+    Packet->SenseDataLength = (UINT8) MIN (SenseData->Length, Packet->SenseDataLength);
+    if (Packet->SenseDataLength != 0) {
+      CopyMem (Packet->SenseData, &SenseData->Data[0], Packet->SenseDataLength);
+    }
+  } else {
+    Packet->SenseDataLength = 0;
+  }
+
+  return Status;
+}
+
+/**
+  Process the received NOP In PDU.
+
+  @param[in]  Pdu            The NOP In PDU received.
+  @param[in]  Tcb            The task control block.
+
+  @retval EFI_SUCCES         The NOP In PDU is processed and the related sequence
+                             numbers are updated.
+  @retval EFI_PROTOCOL_ERROR Some kind of iSCSI protocol errror happened.
+**/
+EFI_STATUS
+IScsiOnNopInRcvd (
+  IN NET_BUF    *Pdu,
+  IN ISCSI_TCB  *Tcb
+  )
+{
+  ISCSI_NOP_IN  *NopInHdr;
+  EFI_STATUS    Status;
+
+  NopInHdr            = (ISCSI_NOP_IN *) NetbufGetByte (Pdu, 0, NULL);
+  if (NopInHdr == NULL) {
+    return EFI_PROTOCOL_ERROR;
+  }
+
+  NopInHdr->StatSN    = NTOHL (NopInHdr->StatSN);
+  NopInHdr->ExpCmdSN  = NTOHL (NopInHdr->ExpCmdSN);
+  NopInHdr->MaxCmdSN  = NTOHL (NopInHdr->MaxCmdSN);
+
+  if (NopInHdr->InitiatorTaskTag == ISCSI_RESERVED_TAG) {
+    if (NopInHdr->StatSN != Tcb->Conn->ExpStatSN) {
+      return EFI_PROTOCOL_ERROR;
+    }
+  } else {
+    Status = IScsiCheckSN (&Tcb->Conn->ExpStatSN, NopInHdr->StatSN);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  IScsiUpdateCmdSN (Tcb->Conn->Session, NopInHdr->MaxCmdSN, NopInHdr->ExpCmdSN);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Execute the SCSI command issued through the EXT SCSI PASS THRU protocol.
+
+  @param[in]       PassThru  The EXT SCSI PASS THRU protocol.
+  @param[in]       Target    The target ID.
+  @param[in]       Lun       The LUN.
+  @param[in, out]  Packet    The request packet containing IO request, SCSI command
+                             buffer and buffers to read/write.
+                             
+  @retval EFI_SUCCES           The SCSI command is executed and the result is updated to 
+                               the Packet.
+  @retval EFI_DEVICE_ERROR     Session state was not as required.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NOT_READY        The target can not accept new commands.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiExecuteScsiCommand (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                 *PassThru,
+  IN UINT8                                           *Target,
+  IN UINT64                                          Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet
+  )
+{
+  EFI_STATUS              Status;
+  ISCSI_DRIVER_DATA       *Private;
+  ISCSI_SESSION           *Session;
+  EFI_EVENT               TimeoutEvent;
+  ISCSI_CONNECTION        *Conn;
+  ISCSI_TCB               *Tcb;
+  NET_BUF                 *Pdu;
+  ISCSI_XFER_CONTEXT      *XferContext;
+  UINT8                   *Data;
+  ISCSI_IN_BUFFER_CONTEXT InBufferContext;
+  UINT64                  Timeout;
+  UINT8                   *PduHdr;
+
+  Private       = ISCSI_DRIVER_DATA_FROM_EXT_SCSI_PASS_THRU (PassThru);
+  Session       = &Private->Session;
+  Status        = EFI_SUCCESS;
+  Tcb           = NULL;
+  TimeoutEvent  = NULL;
+  Timeout       = 0;
+
+  if (Session->State != SESSION_STATE_LOGGED_IN) {
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Conn = NET_LIST_USER_STRUCT_S (
+          Session->Conns.ForwardLink,
+          ISCSI_CONNECTION,
+          Link,
+          ISCSI_CONNECTION_SIGNATURE
+          );
+
+  if (Packet->Timeout != 0) {
+    Timeout = MultU64x32 (Packet->Timeout, 2);
+  }
+
+  Status = IScsiNewTcb (Conn, &Tcb);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  //
+  // Encapsulate the SCSI request packet into an iSCSI SCSI Command PDU.
+  //
+  Pdu = IScsiNewScsiCmdPdu (Packet, Lun, Tcb);
+  if (Pdu == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  XferContext         = &Tcb->XferContext;
+  PduHdr              = NetbufGetByte (Pdu, 0, NULL);
+  if (PduHdr == NULL) {
+    Status = EFI_PROTOCOL_ERROR;
+    NetbufFree (Pdu);
+    goto ON_EXIT;
+  }
+  XferContext->Offset = ISCSI_GET_DATASEG_LEN (PduHdr);
+
+  //
+  // Transmit the SCSI Command PDU.
+  //
+  Status = Tcp4IoTransmit (&Conn->Tcp4Io, Pdu);
+
+  NetbufFree (Pdu);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  if (!Session->InitialR2T &&
+      (XferContext->Offset < Session->FirstBurstLength) &&
+      (XferContext->Offset < Packet->OutTransferLength)
+      ) {
+    //
+    // Unsolicited Data-Out sequence is allowed, there is remaining SCSI
+    // OUT data and the limit of FirstBurstLength is not reached.
+    //
+    XferContext->TargetTransferTag = ISCSI_RESERVED_TAG;
+    XferContext->DesiredLength = MIN (
+                                  Session->FirstBurstLength,
+                                  Packet->OutTransferLength - XferContext->Offset
+                                  );
+
+    Data    = (UINT8 *) Packet->OutDataBuffer + XferContext->Offset;
+    Status  = IScsiSendDataOutPduSequence (Data, Lun, Tcb);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  InBufferContext.InData    = (UINT8 *) Packet->InDataBuffer;
+  InBufferContext.InDataLen = Packet->InTransferLength;
+
+  while (!Tcb->StatusXferd) {
+    //
+    // Start the timeout timer.
+    //
+    if (Timeout != 0) {
+      Status = gBS->SetTimer (Conn->TimeoutEvent, TimerRelative, Timeout);
+      if (EFI_ERROR (Status)) {
+        goto ON_EXIT;
+      }
+      TimeoutEvent = Conn->TimeoutEvent; 
+    }
+    //
+    // try to receive PDU from target.
+    //
+    Status = IScsiReceivePdu (Conn, &Pdu, &InBufferContext, FALSE, FALSE, TimeoutEvent);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+
+    PduHdr = NetbufGetByte (Pdu, 0, NULL);
+    if (PduHdr == NULL) {
+      Status = EFI_PROTOCOL_ERROR;
+      NetbufFree (Pdu);
+      goto ON_EXIT;
+    }
+    switch (ISCSI_GET_OPCODE (PduHdr)) {
+    case ISCSI_OPCODE_SCSI_DATA_IN:
+      Status = IScsiOnDataInRcvd (Pdu, Tcb, Packet);
+      break;
+
+    case ISCSI_OPCODE_R2T:
+      Status = IScsiOnR2TRcvd (Pdu, Tcb, Lun, Packet);
+      break;
+
+    case ISCSI_OPCODE_SCSI_RSP:
+      Status = IScsiOnScsiRspRcvd (Pdu, Tcb, Packet);
+      break;
+
+    case ISCSI_OPCODE_NOP_IN:
+      Status = IScsiOnNopInRcvd (Pdu, Tcb);
+      break;
+
+    case ISCSI_OPCODE_VENDOR_T0:
+    case ISCSI_OPCODE_VENDOR_T1:
+    case ISCSI_OPCODE_VENDOR_T2:
+      //
+      // These messages are vendor specific, skip them.
+      //
+      break;
+
+    default:
+      Status = EFI_PROTOCOL_ERROR;
+      break;
+    }
+
+    NetbufFree (Pdu);
+
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+ON_EXIT:
+
+  if (TimeoutEvent != NULL) {
+    gBS->SetTimer (TimeoutEvent, TimerCancel, 0);
+  }
+
+  if (Tcb != NULL) {
+    IScsiDelTcb (Tcb);
+  }
+
+  return Status;
+}
+
+/**
+  Reinstate the session on some error.
+
+  @param[in, out]  Private The iSCSI driver data.
+
+  @retval EFI_SUCCES  The session is reinstated from some error.
+  @retval Other       Reinstatement failed.
+**/
+EFI_STATUS
+IScsiSessionReinstatement (
+  IN OUT ISCSI_DRIVER_DATA  *Private
+  )
+{
+  ISCSI_SESSION *Session;
+  EFI_STATUS    Status;
+
+  Session = &Private->Session;
+  ASSERT (Session->State != SESSION_STATE_FREE);
+
+  //
+  // Abort the session and re-init it.
+  //
+  IScsiSessionAbort (Session);
+  IScsiSessionInit (Session, TRUE);
+
+  //
+  // Login again.
+  //
+  Status = IScsiSessionLogin (Private);
+
+  return Status;
+}
+
+/**
+  Initialize some session parameters before login.
+
+  @param[in, out]  Session  The iSCSI session.
+  @param[in]       Recovery Whether the request is from a fresh new start or recovery.
+**/
+VOID
+IScsiSessionInit (
+  IN OUT ISCSI_SESSION  *Session,
+  IN BOOLEAN            Recovery
+  )
+{
+  if (!Recovery) {
+    Session->Signature  = ISCSI_SESSION_SIGNATURE;
+    Session->State      = SESSION_STATE_FREE;
+
+    InitializeListHead (&Session->Conns);
+    InitializeListHead (&Session->TcbList);
+  }
+
+  Session->Tsih                 = 0;
+
+  Session->CmdSN                = 1;
+  Session->InitiatorTaskTag     = 1;
+  Session->NextCid              = 1;
+
+  Session->TargetPortalGroupTag = 0;
+  Session->MaxConnections       = ISCSI_MAX_CONNS_PER_SESSION;
+  Session->InitialR2T           = FALSE;
+  Session->ImmediateData        = TRUE;
+  Session->MaxBurstLength       = 262144;
+  Session->FirstBurstLength     = MAX_RECV_DATA_SEG_LEN_IN_FFP;
+  Session->DefaultTime2Wait     = 2;
+  Session->DefaultTime2Retain   = 20;
+  Session->MaxOutstandingR2T    = DEFAULT_MAX_OUTSTANDING_R2T;
+  Session->DataPDUInOrder       = TRUE;
+  Session->DataSequenceInOrder  = TRUE;
+  Session->ErrorRecoveryLevel   = 0;
+}
+
+/**
+  Abort the iSCSI session, that is, reset all the connection and free the
+  resources.
+
+  @param[in, out]  Session The iSCSI session.
+
+  @retval EFI_SUCCES  The session is aborted.
+**/
+EFI_STATUS
+IScsiSessionAbort (
+  IN OUT ISCSI_SESSION  *Session
+  )
+{
+  ISCSI_DRIVER_DATA *Private;
+  ISCSI_CONNECTION  *Conn;
+
+  if (Session->State != SESSION_STATE_LOGGED_IN) {
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (!IsListEmpty (&Session->Conns));
+
+  Private = ISCSI_DRIVER_DATA_FROM_SESSION (Session);
+
+  while (!IsListEmpty (&Session->Conns)) {
+    Conn = NET_LIST_USER_STRUCT_S (
+            Session->Conns.ForwardLink,
+            ISCSI_CONNECTION,
+            Link,
+            ISCSI_CONNECTION_SIGNATURE
+            );
+
+    gBS->CloseProtocol (
+          Conn->Tcp4Io.Handle,
+          &gEfiTcp4ProtocolGuid,
+          Private->Image,
+          Private->ExtScsiPassThruHandle
+          );
+
+    IScsiConnReset (Conn);
+
+    IScsiDetatchConnection (Conn);
+    IScsiDestroyConnection (Conn);
+  }
+
+  Session->State = SESSION_STATE_FAILED;
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.h
new file mode 100644
index 0000000000..48b4e7251f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiProto.h
@@ -0,0 +1,1002 @@
+/** @file
+  The header file of iSCSI Protocol that defines many specific data structures.
+
+Copyright (c) 2004 - 2014, 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.
+
+**/
+
+#ifndef _ISCSI_PROTO_H_
+#define _ISCSI_PROTO_H_
+
+#include <Protocol/ScsiPassThruExt.h>
+
+//
+// RFC 1982 Serial Number Arithmetic, SERIAL_BITS = 32
+//
+#define ISCSI_SEQ_EQ(s1, s2)  ((s1) == (s2))
+#define ISCSI_SEQ_LT(s1, s2) \
+    ( \
+      (((INT32) (s1) < (INT32) (s2)) && (s2 - s1) < ((UINT32) 1 << 31)) || \
+      (((INT32) (s1) > (INT32) (s2)) && (s1 - s2) > ((UINT32) 1 << 31)) \
+    )
+#define ISCSI_SEQ_GT(s1, s2) \
+    ( \
+      (((INT32) (s1) < (INT32) (s2)) && (s2 - s1) > ((UINT32) 1 << 31)) || \
+      (((INT32) (s1) > (INT32) (s2)) && (s1 - s2) < ((UINT32) 1 << 31)) \
+    )
+
+#define ISCSI_WELL_KNOWN_PORT                   3260
+#define ISCSI_MAX_CONNS_PER_SESSION             1
+
+#define DEFAULT_MAX_RECV_DATA_SEG_LEN           8192
+#define MAX_RECV_DATA_SEG_LEN_IN_FFP            65536
+#define DEFAULT_MAX_OUTSTANDING_R2T             1
+
+#define ISCSI_VERSION_MAX                       0x00
+#define ISCSI_VERSION_MIN                       0x00
+
+#define ISCSI_KEY_AUTH_METHOD                   "AuthMethod"
+#define ISCSI_KEY_HEADER_DIGEST                 "HeaderDigest"
+#define ISCSI_KEY_DATA_DIGEST                   "DataDigest"
+#define ISCSI_KEY_MAX_CONNECTIONS               "MaxConnections"
+#define ISCSI_KEY_TARGET_NAME                   "TargetName"
+#define ISCSI_KEY_INITIATOR_NAME                "InitiatorName"
+#define ISCSI_KEY_TARGET_ALIAS                  "TargetAlias"
+#define ISCSI_KEY_INITIATOR_ALIAS               "InitiatorAlias"
+#define ISCSI_KEY_TARGET_ADDRESS                "TargetAddress"
+#define ISCSI_KEY_INITIAL_R2T                   "InitialR2T"
+#define ISCSI_KEY_IMMEDIATE_DATA                "ImmediateData"
+#define ISCSI_KEY_TARGET_PORTAL_GROUP_TAG       "TargetPortalGroupTag"
+#define ISCSI_KEY_MAX_BURST_LENGTH              "MaxBurstLength"
+#define ISCSI_KEY_FIRST_BURST_LENGTH            "FirstBurstLength"
+#define ISCSI_KEY_DEFAULT_TIME2WAIT             "DefaultTime2Wait"
+#define ISCSI_KEY_DEFAULT_TIME2RETAIN           "DefaultTime2Retain"
+#define ISCSI_KEY_MAX_OUTSTANDING_R2T           "MaxOutstandingR2T"
+#define ISCSI_KEY_DATA_PDU_IN_ORDER             "DataPDUInOrder"
+#define ISCSI_KEY_DATA_SEQUENCE_IN_ORDER        "DataSequenceInOrder"
+#define ISCSI_KEY_ERROR_RECOVERY_LEVEL          "ErrorRecoveryLevel"
+#define ISCSI_KEY_SESSION_TYPE                  "SessionType"
+#define ISCSI_KEY_MAX_RECV_DATA_SEGMENT_LENGTH  "MaxRecvDataSegmentLength"
+
+#define ISCSI_KEY_VALUE_NONE                    "None"
+
+///
+/// connection state for initiator
+///
+
+#define CONN_STATE_FREE                         0
+#define CONN_STATE_XPT_WAIT                     1
+#define CONN_STATE_IN_LOGIN                     2
+#define CONN_STATE_LOGGED_IN                    3
+#define CONN_STATE_IN_LOGOUT                    4
+#define CONN_STATE_LOGOUT_REQUESTED             5
+#define CONN_STATE_CLEANUP_WAIT                 6
+#define CONN_STATE_IN_CLEANUP                   7
+
+///
+/// session state for initiator
+///
+#define SESSION_STATE_FREE                      0
+#define SESSION_STATE_LOGGED_IN                 1
+#define SESSION_STATE_FAILED                    2
+
+typedef enum {
+  DataIn  = 0,
+  DataOut = 1,
+  DataBi  = 2
+} DATA_DIRECTION;
+
+#define ISCSI_RESERVED_TAG                  0xffffffff
+
+#define ISCSI_REQ_IMMEDIATE                 0x40
+#define ISCSI_OPCODE_MASK                   0x3F
+
+#define ISCSI_SET_OPCODE(PduHdr, Op, Flgs)  ((((ISCSI_BASIC_HEADER *) (PduHdr))->OpCode) = ((Op) | (Flgs)))
+#define ISCSI_GET_OPCODE(PduHdr)            ((((ISCSI_BASIC_HEADER *) (PduHdr))->OpCode) & ISCSI_OPCODE_MASK)
+#define ISCSI_CHECK_OPCODE(PduHdr, Op)      ((((PduHdr)->OpCode) & ISCSI_OPCODE_MASK) == (Op))
+#define ISCSI_IMMEDIATE_ON(PduHdr)          ((PduHdr)->OpCode & ISCSI_REQ_IMMEDIATE)
+#define ISCSI_SET_FLAG(PduHdr, Flag)        (((ISCSI_BASIC_HEADER *) (PduHdr))->Flags |= (BOOLEAN)(Flag))
+#define ISCSI_CLEAR_FLAG(PduHdr, Flag)      (((ISCSI_BASIC_HEADER *) (PduHdr))->Flags &= ~(Flag))
+#define ISCSI_FLAG_ON(PduHdr, Flag)         ((((ISCSI_BASIC_HEADER *) (PduHdr))->Flags & (Flag)) == (Flag))
+#define ISCSI_SET_STAGES(PduHdr, Cur, Nxt)  ((PduHdr)->Flags = (UINT8) ((PduHdr)->Flags | ((Cur) << 2 | (Nxt))))
+#define ISCSI_GET_CURRENT_STAGE(PduHdr)     (((PduHdr)->Flags >> 2) & 0x3)
+#define ISCSI_GET_NEXT_STAGE(PduHdr)        (((PduHdr)->Flags) & 0x3)
+
+#define ISCSI_GET_PAD_LEN(DataLen)          ((~(DataLen) + 1) & 0x3)
+#define ISCSI_ROUNDUP(DataLen)              (((DataLen) + 3) &~(0x3))
+
+#define HTON24(Dst, Src) \
+  do { \
+    (Dst)[0]  = (UINT8) (((Src) >> 16) & 0xFF); \
+    (Dst)[1]  = (UINT8) (((Src) >> 8) & 0xFF); \
+    (Dst)[2]  = (UINT8) ((Src) & 0xFF); \
+  } while (0);
+
+#define NTOH24(src)                         (((src)[0] << 16) | ((src)[1] << 8) | ((src)[2]))
+
+#define ISCSI_GET_DATASEG_LEN(PduHdr)       NTOH24 (((ISCSI_BASIC_HEADER *) (PduHdr))->DataSegmentLength)
+#define ISCSI_SET_DATASEG_LEN(PduHdr, Len)  HTON24 (((ISCSI_BASIC_HEADER *) (PduHdr))->DataSegmentLength, (Len))
+
+//
+// initiator opcodes
+//
+#define ISCSI_OPCODE_NOP_OUT        0x00
+#define ISCSI_OPCODE_SCSI_CMD       0x01
+#define ISCSI_OPCODE_SCSI_TMF_REQ   0x02
+#define ISCSI_OPCODE_LOGIN_REQ      0x03
+#define ISCSI_OPCODE_TEXT_REQ       0x04
+#define ISCSI_OPCODE_SCSI_DATA_OUT  0x05
+#define ISCSI_OPCODE_LOGOUT_REQ     0x06
+#define ISCSI_OPCODE_SNACK_REQ      0x10
+#define ISCSI_OPCODE_VENDOR_I0      0x1c
+#define ISCSI_OPCODE_VENDOR_I1      0x1d
+#define ISCSI_OPCODE_VENDOR_I2      0x1e
+
+//
+// target opcodes
+//
+#define ISCSI_OPCODE_NOP_IN       0x20
+#define ISCSI_OPCODE_SCSI_RSP     0x21
+#define ISCSI_OPCODE_SCSI_TMF_RSP 0x22
+#define ISCSI_OPCODE_LOGIN_RSP    0x23
+#define ISCSI_OPCODE_TEXT_RSP     0x24
+#define ISCSI_OPCODE_SCSI_DATA_IN 0x25
+#define ISCSI_OPCODE_LOGOUT_RSP   0x26
+#define ISCSI_OPCODE_R2T          0x31
+#define ISCSI_OPCODE_ASYNC_MSG    0x32
+#define ISCSI_OPCODE_VENDOR_T0    0x3c
+#define ISCSI_OPCODE_VENDOR_T1    0x3d
+#define ISCSI_OPCODE_VENDOR_T2    0x3e
+#define ISCSI_OPCODE_REJECT       0x3f
+
+#define ISCSI_BHS_FLAG_FINAL      0x80
+
+///
+/// iSCSI Basic Header Segment
+///
+typedef struct _ISCSI_BASIC_HEADER {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT16  OpCodeSpecific1;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  OpCodeSpecific2[7];
+} ISCSI_BASIC_HEADER;
+
+//
+// Defined AHS types, others are reserved.
+//
+#define ISCSI_AHS_TYPE_EXT_CDB              0x1
+#define ISCSI_AHS_TYPE_BI_EXP_READ_DATA_LEN 0x2
+
+typedef struct _ISCSI_ADDTIONAL_HEADER {
+  UINT16  Length;
+  UINT8   Type;
+  UINT8   TypeSpecific[1];
+} ISCSI_ADDITIONAL_HEADER;
+
+typedef struct _ISCSI_BI_EXP_READ_DATA_LEN_AHS {
+  UINT16  Length;
+  UINT8   Type;
+  UINT8   Reserved;
+  UINT32  ExpReadDataLength;
+} ISCSI_BI_EXP_READ_DATA_LEN_AHS;
+
+#define SCSI_CMD_PDU_FLAG_READ        0x40
+#define SCSI_CMD_PDU_FLAG_WRITE       0x20
+
+#define ISCSI_CMD_PDU_TASK_ATTR_MASK  0x07
+
+//
+// task attributes
+//
+#define ISCSI_TASK_ATTR_UNTAGGED  0x00
+#define ISCSI_TASK_ATTR_SIMPLE    0x01
+#define ISCSI_TASK_ATTR_ORDERD    0x02
+#define ISCSI_TASK_ATTR_HOQ       0x03
+#define ISCSI_TASK_ATTR_ACA       0x04
+
+///
+/// SCSI Command
+///
+typedef struct _SCSI_COMMAND {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT16  Reserved;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  ExpDataXferLength;
+  UINT32  CmdSN;
+  UINT32  ExpStatSN;
+  UINT8   Cdb[16];
+} SCSI_COMMAND;
+
+//
+// flag bit definitions in SCSI response
+//
+#define SCSI_RSP_PDU_FLAG_BI_READ_OVERFLOW  0x10
+#define SCSI_RSP_PDU_FLAG_BI_READ_UNDERFLOW 0x08
+#define SCSI_RSP_PDU_FLAG_OVERFLOW          0x04
+#define SCSI_RSP_PDU_FLAG_UNDERFLOW         0x02
+
+//
+// iSCSI service response codes
+//
+#define ISCSI_SERVICE_RSP_COMMAND_COMPLETE_AT_TARGET  0x00
+#define ISCSI_SERVICE_RSP_TARGET_FAILURE              0x01
+
+///
+/// SCSI Response
+///
+typedef struct _SCSI_RESPONSE {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT8   Response;
+  UINT8   Status;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Reserved[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  SNACKTag;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  ExpDataSN;
+  UINT32  BiReadResidualCount;
+  UINT32  ResidualCount;
+} SCSI_RESPONSE;
+
+typedef struct _ISCSI_SENSE_DATA {
+  UINT16  Length;
+  UINT8   Data[2];
+} ISCSI_SENSE_DATA;
+
+///
+/// iSCSI Task Managment Function Request
+///
+typedef struct _ISCSI_TMF_REQUEST {
+  UINT8   OpCode;
+  UINT8   Fuction;
+  UINT16  Reserved1;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  ReferencedTaskTag;
+  UINT32  CmdSN;
+  UINT32  ExpStatSN;
+  UINT32  RefCmdSN;
+  UINT32  ExpDataSN;
+  UINT32  Reserved2[2];
+} ISCSI_TMF_REQUEST;
+
+#define ISCSI_TMF_RSP_PDU_RSP_FUNCTION_COMPLETE           0
+#define ISCSI_TMF_RSP_PDU_RSP_TASK_NOT_EXIST              1
+#define ISCSI_TMF_RSP_PDU_RSP_LUN_NOT_EXIST               2
+#define ISCSI_TMF_RSP_PDU_RSP_TASK_STILL_ALLEGIANT        3
+#define ISCSI_TMF_RSP_PDU_RSP_TASK_REASSGIN_NOT_SUPPORTED 4
+#define ISCSI_TMF_RSP_PDU_RSP_NOT_SUPPORTED               5
+#define ISCSI_TMF_RSP_PDU_RSP_FUNCTION_AHTH_FAILED        6
+#define ISCSI_TMF_RSP_PDU_RSP_FUNCTION_REJECTED           255
+
+///
+/// iSCSI Task Management Function Response
+///
+typedef struct _ISCSI_TMF_RESPONSE {
+  UINT8   OpCode;
+  UINT8   Reserved1;
+  UINT8   Response;
+  UINT8   Reserved2;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT32  Reserver3[2];
+  UINT32  InitiatorTaskTag;
+  UINT32  Reserved4;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  Reserved[3];
+} ISCSI_TMF_RESPONSE;
+
+///
+/// SCSI Data-Out
+///
+typedef struct _ISCSI_SCSI_DATA_OUT {
+  UINT8   OpCode;
+  UINT8   Reserved1[3];
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  Reserved2;
+  UINT32  ExpStatSN;
+  UINT32  Reserved3;
+  UINT32  DataSN;
+  UINT32  BufferOffset;
+  UINT32  Reserved4;
+} ISCSI_SCSI_DATA_OUT;
+
+#define SCSI_DATA_IN_PDU_FLAG_ACKKNOWLEDGE  0x40
+#define SCSI_DATA_IN_PDU_FLAG_OVERFLOW      SCSI_RSP_PDU_FLAG_OVERFLOW
+#define SCSI_DATA_IN_PDU_FLAG_UNDERFLOW     SCSI_RSP_PDU_FLAG_UNDERFLOW
+#define SCSI_DATA_IN_PDU_FLAG_STATUS_VALID  0x01
+
+///
+/// SCSI Data-In
+///
+typedef struct _ISCSI_SCSI_DATA_IN {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT8   Reserved1;
+  UINT8   Status;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  DataSN;
+  UINT32  BufferOffset;
+  UINT32  ResidualCount;
+} ISCSI_SCSI_DATA_IN;
+
+#define ISCSI_GET_BUFFER_OFFSET(PduHdr) NTOHL (((ISCSI_SCSI_DATA_IN *) (PduHdr))->BufferOffset)
+
+///
+/// Ready To Transfer
+///
+typedef struct _ISCSI_READY_TO_TRANSFER {
+  UINT8   OpCode;
+  UINT8   Reserved1[3];
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  R2TSeqNum;
+  UINT32  BufferOffset;
+  UINT32  DesiredDataTransferLength;
+} ISCSI_READY_TO_TRANSFER;
+
+typedef struct _ISCSI_ASYNC_MESSAGE {
+  UINT8   OpCode;
+  UINT8   Reserved1[8];
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  Reserved2;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT8   AsyncEvent;
+  UINT8   AsyncVCode;
+  UINT16  Parameter1;
+  UINT16  Parameter2;
+  UINT16  Parameter3;
+  UINT32  Reserved3;
+} ISCSI_ASYNC_MESSAGE;
+
+#define ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT  0x80
+#define ISCSI_LOGIN_REQ_PDU_FLAG_CONTINUE 0x40
+
+///
+/// Login Request
+///
+typedef struct _ISCSI_LOGIN_REQUEST {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT8   VersionMax;
+  UINT8   VersionMin;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Isid[6];
+  UINT16  Tsih;
+  UINT32  InitiatorTaskTag;
+  UINT16  Cid;
+  UINT16  Reserved1;
+  UINT32  CmdSN;
+  UINT32  ExpStatSN;
+  UINT32  Reserved2[4];
+} ISCSI_LOGIN_REQUEST;
+
+#define ISCSI_LOGIN_RSP_PDU_FLAG_TRANSIT    ISCSI_LOGIN_REQ_PDU_FLAG_TRANSIT
+#define ISCSI_LOGIN_RSP_PDU_FLAG_CONTINUE   ISCSI_LOGIN_REQ_PDU_FLAG_CONTINUE
+
+#define ISCSI_LOGIN_STATUS_SUCCESS          0
+#define ISCSI_LOGIN_STATUS_REDIRECTION      1
+#define ISCSI_LOGIN_STATUS_INITIATOR_ERROR  2
+#define ISCSI_LOGIN_STATUS_TARGET_ERROR     3
+
+///
+/// Login Response
+///
+typedef struct _ISCSI_LOGIN_RESPONSE {
+  UINT8   OpCode;
+  UINT8   Flags;
+  UINT8   VersionMax;
+  UINT8   VersionActive;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Isid[6];
+  UINT16  Tsih;
+  UINT32  InitiatorTaskTag;
+  UINT32  Reserved1;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT8   StatusClass;
+  UINT8   StatusDetail;
+  UINT8   Reserved2[10];
+} ISCSI_LOGIN_RESPONSE;
+
+#define ISCSI_LOGOUT_REASON_CLOSE_SESSION                   0
+#define ISCSI_LOGOUT_REASON_CLOSE_CONNECTION                1
+#define ISCSI_LOGOUT_REASON_REMOVE_CONNECTION_FOR_RECOVERY  2
+
+///
+/// Logout Request
+///
+typedef struct _ISCSI_LOGOUT_REQUEST {
+  UINT8   OpCode;
+  UINT8   ReasonCode;
+  UINT16  Reserved1;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT32  Reserved2[2];
+  UINT32  InitiatorTaskTag;
+  UINT16  Cid;
+  UINT16  Reserved3;
+  UINT32  CmdSN;
+  UINT32  ExpStatSN;
+  UINT32  Reserved4[4];
+} ISCSI_LOGOUT_REQUEST;
+
+#define ISCSI_LOGOUT_RESPONSE_SESSION_CLOSED_SUCCESS  0
+#define ISCSI_LOGOUT_RESPONSE_CID_NOT_FOUND           1
+#define ISCSI_LOGOUT_RESPONSE_RECOVERY_NOT_SUPPORTED  2
+#define ISCSI_LOGOUT_RESPONSE_CLEANUP_FAILED          3
+
+///
+/// Logout Response
+///
+typedef struct _ISCSI_LOGOUT_RESPONSE {
+  UINT8   OpCode;
+  UINT8   Reserved1;
+  UINT8   Response;
+  UINT8   Reserved2;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT32  Reserved3[2];
+  UINT32  InitiatorTaskTag;
+  UINT32  Reserved4;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  Reserved5;
+  UINT16  Time2Wait;
+  UINT16  Time2Retain;
+  UINT32  Reserved6;
+} ISCSI_LOGOUT_RESPONSE;
+
+#define ISCSI_SNACK_REQUEST_TYPE_DATA_OR_R2T  0
+#define ISCSI_SNACK_REQUEST_TYPE_STATUS       1
+#define ISCSI_SNACK_REQUEST_TYPE_DATA_ACK     2
+#define ISCSI_SNACK_REQUEST_TYPE_RDATA        3
+
+///
+/// SNACK Request
+///
+typedef struct _ISCSI_SNACK_REQUEST {
+  UINT8   OpCode;
+  UINT8   Type;
+  UINT16  Reserved1;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  Reserved2;
+  UINT32  ExpStatSN;
+  UINT32  Reserved[2];
+  UINT32  BegRun;
+  UINT32  RunLength;
+} ISCSI_SNACK_REQUEST;
+
+///
+/// Reject
+///
+typedef struct _ISCSI_REJECT {
+  UINT8   OpCode;
+  UINT8   Reserved1;
+  UINT8   Reason;
+  UINT8   Reserved2;
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT32  Reserved3[2];
+  UINT32  InitiatorTaskTag;
+  UINT32  Reserved4;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  DataSN;
+  UINT32  Reserved5[2];
+} ISCSI_REJECT;
+
+///
+/// NOP-Out
+///
+typedef struct _ISCSI_NOP_OUT {
+  UINT8   OpCode;
+  UINT8   Reserved1[3];
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  CmdSN;
+  UINT32  ExpStatSN;
+  UINT32  Reserved2[4];
+} ISCSI_NOP_OUT;
+
+///
+/// NOP-In
+///
+typedef struct _ISCSI_NOP_IN {
+  UINT8   OpCode;
+  UINT8   Reserved1[3];
+  UINT8   TotalAHSLength;
+  UINT8   DataSegmentLength[3];
+  UINT8   Lun[8];
+  UINT32  InitiatorTaskTag;
+  UINT32  TargetTransferTag;
+  UINT32  StatSN;
+  UINT32  ExpCmdSN;
+  UINT32  MaxCmdSN;
+  UINT32  Reserved2[3];
+} ISCSI_NOP_IN;
+
+#define ISCSI_SECURITY_NEGOTIATION          0
+#define ISCSI_LOGIN_OPERATIONAL_NEGOTIATION 1
+#define ISCSI_FULL_FEATURE_PHASE            3
+
+typedef enum {
+  IScsiDigestNone,
+  IScsiDigestCRC32
+} ISCSI_DIGEST_TYPE;
+
+typedef struct _ISCSI_XFER_CONTEXT {
+  UINT32  TargetTransferTag;
+  UINT32  Offset;
+  UINT32  DesiredLength;
+  UINT32  ExpDataSN;
+} ISCSI_XFER_CONTEXT;
+
+typedef struct _ISCSI_IN_BUFFER_CONTEXT {
+  UINT8   *InData;
+  UINT32  InDataLen;
+} ISCSI_IN_BUFFER_CONTEXT;
+
+typedef struct _ISCSI_TCB {
+  LIST_ENTRY          Link;
+
+  BOOLEAN             SoFarInOrder;
+  UINT32              ExpDataSN;
+  BOOLEAN             FbitReceived;
+  BOOLEAN             StatusXferd;
+  UINT32              ActiveR2Ts;
+  UINT32              Response;
+  CHAR8               *Reason;
+  UINT32              InitiatorTaskTag;
+  UINT32              CmdSN;
+  UINT32              SNACKTag;
+
+  ISCSI_XFER_CONTEXT  XferContext;
+
+  ISCSI_CONNECTION    *Conn;
+} ISCSI_TCB;
+
+typedef struct _ISCSI_KEY_VALUE_PAIR {
+  LIST_ENTRY      List;
+
+  CHAR8           *Key;
+  CHAR8           *Value;
+} ISCSI_KEY_VALUE_PAIR;
+
+/**
+  Attach the iSCSI connection to the iSCSI session. 
+
+  @param[in, out]  Session The iSCSI session.
+  @param[in, out]  Conn    The iSCSI connection.
+**/
+VOID
+IScsiAttatchConnection (
+  IN OUT ISCSI_SESSION     *Session,
+  IN OUT ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Detach the iSCSI connection from the session it belongs to. 
+
+  @param[in, out]  Conn The iSCSI connection.
+**/
+VOID
+IScsiDetatchConnection (
+  IN OUT ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  This function does the iSCSI connection login.
+
+  @param[in, out]  Conn      The iSCSI connection to login.
+
+  @retval EFI_SUCCESS        The iSCSI connection is logged into the iSCSI target.
+  @retval EFI_TIMEOUT        Timeout happened during the login procedure.
+  @retval Others             Other errors as indicated.  
+**/
+EFI_STATUS
+IScsiConnLogin (
+  IN OUT ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Create a TCP connection for the iSCSI session.
+
+  @param[in]  Private The iSCSI driver data.
+  @param[in]  Session Maximum CmdSN from the target.
+
+  @return The newly created iSCSI connection.
+**/
+ISCSI_CONNECTION *
+IScsiCreateConnection (
+  IN ISCSI_DRIVER_DATA  *Private,
+  IN ISCSI_SESSION      *Session
+  );
+
+/**
+  Destroy an iSCSI connection.
+
+  @param[in]  Conn The connection to destroy.
+**/
+VOID
+IScsiDestroyConnection (
+  IN ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Login the iSCSI session.
+
+  @param[in]  Private          The iSCSI driver data.
+
+  @retval EFI_SUCCESS          The iSCSI session login procedure finished.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NO_MEDIA         There was a media error.
+  @retval Others               Other errors as indicated.
+
+**/
+EFI_STATUS
+IScsiSessionLogin (
+  IN ISCSI_DRIVER_DATA  *Private
+  );
+
+/**
+  Build and send the iSCSI login request to the iSCSI target according to
+  the current login stage.
+
+  @param[in]  Conn             The connection in the iSCSI login phase.
+
+  @retval EFI_SUCCESS          The iSCSI login request PDU is built and sent on this
+                               connection.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_DEVICE_ERROR     Some kind of device error happened.
+**/
+EFI_STATUS
+IScsiSendLoginReq (
+  IN ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Receive and process the iSCSI login response.
+
+  @param[in]  Conn             The connection in the iSCSI login phase.
+  
+  @retval EFI_SUCCESS          The iSCSI login response PDU is received and processed.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiReceiveLoginRsp (
+  IN ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Add an iSCSI key-value pair as a string into the data segment of the Login Request PDU.
+  The DataSegmentLength and the actual size of the net buffer containing this PDU will be
+  updated.
+
+  @param[in, out]  Pdu         The iSCSI PDU whose data segment the key-value pair will
+                               be added to.
+  @param[in]       Key         The key name string.
+  @param[in]       Value       The value string.
+
+  @retval EFI_SUCCESS          The key-valu pair is added to the PDU's datasegment and
+                               the correspondence length fields are updated.
+  @retval EFI_OUT_OF_RESOURCES There is not enough space in the PDU to add the key-value
+                               pair.
+**/
+EFI_STATUS
+IScsiAddKeyValuePair (
+  IN OUT NET_BUF      *Pdu,
+  IN CHAR8            *Key,
+  IN CHAR8            *Value
+  );
+
+/**
+  Prepare the iSCSI login request to be sent according to the current login status.
+
+  @param[in, out]  Conn The connection in the iSCSI login phase.
+
+  @return The pointer to the net buffer containing the iSCSI login request built.
+  @retval Others    Other errors as indicated.
+**/
+NET_BUF *
+IScsiPrepareLoginReq (
+  IN OUT ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Process the iSCSI Login Response.
+
+  @param[in, out]  Conn The connection on which the iSCSI login response is received.
+  @param[in, out]  Pdu  The iSCSI login response PDU.
+
+  @retval EFI_SUCCESS        The iSCSI login response PDU is processed and all check are passed.
+  @retval EFI_PROTOCOL_ERROR Some kind of iSCSI protocol error happened.
+  @retval EFI_MEDIA_CHANGED  Target is redirected.
+  @retval Others             Other errors as indicated.
+**/
+EFI_STATUS
+IScsiProcessLoginRsp (
+  IN OUT ISCSI_CONNECTION  *Conn,
+  IN OUT NET_BUF           *Pdu
+  );
+
+/**
+  Updated the target information according the data received in the iSCSI
+  login response with an target redirection status.
+
+  @param[in, out] Session      The iSCSI session.
+  @param[in]      Data         The data segment which should contain the
+                               TargetAddress key-value list.
+  @param[in]      Len          Length of the data.
+  
+  @retval EFI_SUCCESS          The target address is updated.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NOT_FOUND        The TargetAddress key is not found.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiUpdateTargetAddress (
+  IN OUT ISCSI_SESSION  *Session,
+  IN CHAR8              *Data,
+  IN UINT32             Len
+  );
+
+/**
+  The callback function to free the net buffer list.
+
+  @param[in]  Arg The opaque parameter.
+**/
+VOID
+EFIAPI
+IScsiFreeNbufList (
+  VOID *Arg
+  );
+
+/**
+  Receive an iSCSI response PDU. An iSCSI response PDU contains an iSCSI PDU header and
+  an optional data segment. The two parts will be put into two blocks of buffers in the
+  net buffer. The digest check will be conducted in this function if needed and the digests
+  will be trimmed from the PDU buffer.
+
+  @param[in]   Conn        The iSCSI connection to receive data from.
+  @param[out]  Pdu         The received iSCSI pdu.
+  @param[in]   Context     The context used to describe information on the caller provided
+                           buffer to receive data segment of the iSCSI pdu, it's optional.
+  @param[in]  HeaderDigest Whether there will be header digest received.
+  @param[in]  DataDigest   Whether there will be data digest.
+  @param[in]  TimeoutEvent The timeout event, it's optional.
+
+  @retval EFI_SUCCESS          An iSCSI pdu is received.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_PROTOCOL_ERROR   Some kind of iSCSI protocol error happened.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiReceivePdu (
+  IN ISCSI_CONNECTION                      *Conn,
+  OUT NET_BUF                              **Pdu,
+  IN ISCSI_IN_BUFFER_CONTEXT               *Context, OPTIONAL
+  IN BOOLEAN                               HeaderDigest,
+  IN BOOLEAN                               DataDigest,
+  IN EFI_EVENT                             TimeoutEvent OPTIONAL
+  );
+
+/**
+  Check and get the result of the prameter negotiation.
+
+  @param[in, out]  Conn          The connection in iSCSI login.
+
+  @retval EFI_SUCCESS          The parmeter check is passed and negotiation is finished.
+  @retval EFI_PROTOCOL_ERROR   Some kind of iSCSI protocol error happened.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+**/
+EFI_STATUS
+IScsiCheckOpParams (
+  IN OUT ISCSI_CONNECTION  *Conn
+  );
+
+/**
+  Fill the oprational prameters.
+
+  @param[in]       Conn        The connection in iSCSI login.
+  @param[in, out]  Pdu         The iSCSI login request PDU to fill the parameters.
+
+  @retval EFI_SUCCESS          The parmeters are filled into the iSCSI login request PDU.
+**/
+EFI_STATUS
+IScsiFillOpParams (
+  IN ISCSI_CONNECTION  *Conn,
+  IN OUT NET_BUF       *Pdu
+  );
+
+/**
+  Pad the iSCSI AHS or data segment to an integer number of 4 byte words.
+
+  @param[in, out]  Pdu         The iSCSI pdu which contains segments to pad.
+  @param[in]       Len         The length of the last semgnet in the PDU.
+
+  @retval EFI_SUCCESS          The segment is padded or no need to pad it.
+  @retval EFI_OUT_OF_RESOURCES There is not enough remaining free space to add the
+                               padding bytes.
+**/
+EFI_STATUS
+IScsiPadSegment (
+  IN OUT NET_BUF  *Pdu,
+  IN UINT32       Len
+  );
+
+/**
+  Build a key-value list from the data segment.
+
+  @param[in]  Data The data segment containing the key-value pairs.
+  @param[in]  Len  Length of the data segment.
+
+  @return The key-value list.
+  @retval NULL Other errors as indicated.
+**/
+LIST_ENTRY *
+IScsiBuildKeyValueList (
+  IN CHAR8  *Data,
+  IN UINT32 Len
+  );
+
+/**
+  Get the value string by the key name from the key-value list. If found,
+  the key-value entry will be removed from the list.
+
+  @param[in, out]  KeyValueList  The key-value list.
+  @param[in]       Key           The key name to find.
+
+  @return The value string.
+**/
+CHAR8 *
+IScsiGetValueByKeyFromList (
+  IN OUT LIST_ENTRY  *KeyValueList,
+  IN CHAR8           *Key
+  );
+
+/**
+  Free the key-value list.
+
+  @param[in]  KeyValueList The key-value list.
+**/
+VOID
+IScsiFreeKeyValueList (
+  IN LIST_ENTRY      *KeyValueList
+  );
+
+/**
+  Normalize the iSCSI name according to RFC.
+
+  @param[in, out]  Name       The iSCSI name.
+  @param[in]       Len        length of the iSCSI name.
+
+  @retval EFI_SUCCESS        The iSCSI name is valid and normalized.
+  @retval EFI_PROTOCOL_ERROR The iSCSI name is mal-formatted or not in the IQN format.
+**/
+EFI_STATUS
+IScsiNormalizeName (
+  IN OUT CHAR8  *Name,
+  IN UINTN      Len
+  );
+
+/**
+  Execute the SCSI command issued through the EXT SCSI PASS THRU protocol.
+
+  @param[in]       PassThru  The EXT SCSI PASS THRU protocol.
+  @param[in]       Target    The target ID.
+  @param[in]       Lun       The LUN.
+  @param[in, out]  Packet    The request packet containing IO request, SCSI command
+                             buffer and buffers to read/write.
+                             
+  @retval EFI_SUCCES           The SCSI command is executed and the result is updated to 
+                               the Packet.
+  @retval EFI_DEVICE_ERROR     Session state was not as required.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval EFI_NOT_READY        The target can not accept new commands.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+IScsiExecuteScsiCommand (
+  IN EFI_EXT_SCSI_PASS_THRU_PROTOCOL                 *PassThru,
+  IN UINT8                                           *Target,
+  IN UINT64                                          Lun,
+  IN OUT EFI_EXT_SCSI_PASS_THRU_SCSI_REQUEST_PACKET  *Packet
+  );
+
+/**
+  Reinstate the session on some error.
+
+  @param[in, out]  Private The iSCSI driver data.
+
+  @retval EFI_SUCCES  The session is reinstated from some error.
+  @retval Other       Reinstatement failed.
+**/
+EFI_STATUS
+IScsiSessionReinstatement (
+  IN OUT ISCSI_DRIVER_DATA  *Private
+  );
+
+/**
+  Initialize some session parameters before login.
+
+  @param[in, out]  Session  The iSCSI session.
+  @param[in]       Recovery Whether the request is from a fresh new start or recovery.
+**/
+VOID
+IScsiSessionInit (
+  IN OUT ISCSI_SESSION  *Session,
+  IN BOOLEAN            Recovery
+  );
+  
+/**
+  Abort the iSCSI session, that is, reset all the connection and free the
+  resources.
+
+  @param[in, out]  Session The iSCSI session.
+
+  @retval EFI_SUCCES  The session is aborted.
+**/
+EFI_STATUS
+IScsiSessionAbort (
+  IN OUT ISCSI_SESSION  *Session
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.c
new file mode 100644
index 0000000000..e73a6dd5f7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.c
@@ -0,0 +1,487 @@
+/** @file
+  The wrap of TCP/IP Socket interface.
+
+Copyright (c) 2004 - 2009, 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 "IScsiImpl.h"
+
+/**
+  The common notify function associated with various Tcp4Io events. 
+
+  @param[in]  Event   The event signaled.
+  @param[in]  Context The context.
+**/
+VOID
+EFIAPI
+Tcp4IoCommonNotify (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  *((BOOLEAN *) Context) = TRUE;
+}
+
+/**
+  Create a TCP socket with the specified configuration data. 
+
+  @param[in]  Image      The handle of the driver image.
+  @param[in]  Controller The handle of the controller.
+  @param[in]  ConfigData The Tcp4 configuration data.
+  @param[in]  Tcp4Io     The Tcp4Io.
+  
+  @retval EFI_SUCCESS    The TCP socket is created and configured.
+  @retval Others         Failed to create the TCP socket or configure it.
+**/
+EFI_STATUS
+Tcp4IoCreateSocket (
+  IN EFI_HANDLE           Image,
+  IN EFI_HANDLE           Controller,
+  IN TCP4_IO_CONFIG_DATA  *ConfigData,
+  IN TCP4_IO              *Tcp4Io
+  )
+{
+  EFI_STATUS            Status;
+  EFI_TCP4_PROTOCOL     *Tcp4;
+  EFI_TCP4_CONFIG_DATA  Tcp4ConfigData;
+  EFI_TCP4_OPTION       ControlOption;
+  EFI_TCP4_ACCESS_POINT *AccessPoint;
+
+  Tcp4Io->Handle = NULL;
+  Tcp4Io->ConnToken.CompletionToken.Event = NULL;
+  Tcp4Io->TxToken.CompletionToken.Event = NULL;
+  Tcp4Io->RxToken.CompletionToken.Event = NULL;
+  Tcp4Io->CloseToken.CompletionToken.Event = NULL;
+  Tcp4 = NULL;
+
+  //
+  // Create the TCP4 child instance and get the TCP4 protocol.
+  //
+  Status = NetLibCreateServiceChild (
+            Controller,
+            Image,
+            &gEfiTcp4ServiceBindingProtocolGuid,
+            &Tcp4Io->Handle
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Tcp4Io->Handle,
+                  &gEfiTcp4ProtocolGuid,
+                  (VOID **)&Tcp4Io->Tcp4,
+                  Image,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Tcp4Io->Image       = Image;
+  Tcp4Io->Controller  = Controller;
+  Tcp4                = Tcp4Io->Tcp4;
+
+  //
+  // Set the configuration parameters.
+  //
+  ControlOption.ReceiveBufferSize       = 0x200000;
+  ControlOption.SendBufferSize          = 0x200000;
+  ControlOption.MaxSynBackLog           = 0;
+  ControlOption.ConnectionTimeout       = 0;
+  ControlOption.DataRetries             = 6;
+  ControlOption.FinTimeout              = 0;
+  ControlOption.TimeWaitTimeout         = 0;
+  ControlOption.KeepAliveProbes         = 4;
+  ControlOption.KeepAliveTime           = 0;
+  ControlOption.KeepAliveInterval       = 0;
+  ControlOption.EnableNagle             = FALSE;
+  ControlOption.EnableTimeStamp         = FALSE;
+  ControlOption.EnableWindowScaling     = TRUE;
+  ControlOption.EnableSelectiveAck      = FALSE;
+  ControlOption.EnablePathMtuDiscovery  = FALSE;
+
+  Tcp4ConfigData.TypeOfService          = 8;
+  Tcp4ConfigData.TimeToLive             = 255;
+  Tcp4ConfigData.ControlOption          = &ControlOption;
+
+  AccessPoint = &Tcp4ConfigData.AccessPoint;
+
+  AccessPoint->UseDefaultAddress = FALSE;
+  AccessPoint->StationPort = 0;
+  AccessPoint->RemotePort = ConfigData->RemotePort;
+  AccessPoint->ActiveFlag = TRUE;
+
+  CopyMem (&AccessPoint->StationAddress, &ConfigData->LocalIp, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&AccessPoint->SubnetMask, &ConfigData->SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&AccessPoint->RemoteAddress, &ConfigData->RemoteIp, sizeof (EFI_IPv4_ADDRESS));
+
+  //
+  // Configure the TCP4 protocol.
+  //
+  Status = Tcp4->Configure (Tcp4, &Tcp4ConfigData);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  if (!EFI_IP4_EQUAL (&ConfigData->Gateway, &mZeroIp4Addr)) {
+    //
+    // the gateway is not zero, add the default route by hand
+    //
+    Status = Tcp4->Routes (Tcp4, FALSE, &mZeroIp4Addr, &mZeroIp4Addr, &ConfigData->Gateway);
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+  }
+  //
+  // Create events for variuos asynchronous operations.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Tcp4IoCommonNotify,
+                  &Tcp4Io->IsConnDone,
+                  &Tcp4Io->ConnToken.CompletionToken.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Tcp4IoCommonNotify,
+                  &Tcp4Io->IsTxDone,
+                  &Tcp4Io->TxToken.CompletionToken.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Tcp4IoCommonNotify,
+                  &Tcp4Io->IsRxDone,
+                  &Tcp4Io->RxToken.CompletionToken.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Tcp4IoCommonNotify,
+                  &Tcp4Io->IsCloseDone,
+                  &Tcp4Io->CloseToken.CompletionToken.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Tcp4Io->IsTxDone  = FALSE;
+  Tcp4Io->IsRxDone  = FALSE;
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  if (Tcp4Io->RxToken.CompletionToken.Event != NULL) {
+    gBS->CloseEvent (Tcp4Io->RxToken.CompletionToken.Event);
+  }
+
+  if (Tcp4Io->TxToken.CompletionToken.Event != NULL) {
+    gBS->CloseEvent (Tcp4Io->TxToken.CompletionToken.Event);
+  }
+
+  if (Tcp4Io->ConnToken.CompletionToken.Event != NULL) {
+    gBS->CloseEvent (Tcp4Io->ConnToken.CompletionToken.Event);
+  }
+
+  if (Tcp4 != NULL) {
+    Tcp4->Configure (Tcp4, NULL);
+
+    gBS->CloseProtocol (
+          Tcp4Io->Handle,
+          &gEfiTcp4ProtocolGuid,
+          Image,
+          Controller
+          );
+  }
+
+  NetLibDestroyServiceChild (
+    Controller,
+    Image,
+    &gEfiTcp4ServiceBindingProtocolGuid,
+    Tcp4Io->Handle
+    );
+
+  return Status;
+}
+
+/**
+  Destroy the socket. 
+
+  @param[in]  Tcp4Io The Tcp4Io which wraps the socket to be destroyeds.
+**/
+VOID
+Tcp4IoDestroySocket (
+  IN TCP4_IO  *Tcp4Io
+  )
+{
+  EFI_TCP4_PROTOCOL *Tcp4;
+
+  Tcp4 = Tcp4Io->Tcp4;
+
+  Tcp4->Configure (Tcp4, NULL);
+
+  gBS->CloseEvent (Tcp4Io->TxToken.CompletionToken.Event);
+  gBS->CloseEvent (Tcp4Io->RxToken.CompletionToken.Event);
+  gBS->CloseEvent (Tcp4Io->ConnToken.CompletionToken.Event);
+
+  gBS->CloseProtocol (
+        Tcp4Io->Handle,
+        &gEfiTcp4ProtocolGuid,
+        Tcp4Io->Image,
+        Tcp4Io->Controller
+        );
+
+  NetLibDestroyServiceChild (
+    Tcp4Io->Controller,
+    Tcp4Io->Image,
+    &gEfiTcp4ServiceBindingProtocolGuid,
+    Tcp4Io->Handle
+    );
+}
+
+/**
+  Connect to the other endpoint of the TCP socket.
+
+  @param[in, out]  Tcp4Io    The Tcp4Io wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+  
+  @retval EFI_SUCCESS          Connect to the other endpoint of the TCP socket successfully.
+  @retval EFI_TIMEOUT          Failed to connect to the other endpoint of the TCP socket in the                               specified time period.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoConnect (
+  IN OUT TCP4_IO    *Tcp4Io,
+  IN EFI_EVENT      Timeout
+  )
+{
+  EFI_TCP4_PROTOCOL *Tcp4;
+  EFI_STATUS        Status;
+
+  Tcp4Io->IsConnDone  = FALSE;
+  Tcp4                = Tcp4Io->Tcp4;
+  Status              = Tcp4->Connect (Tcp4, &Tcp4Io->ConnToken);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  while (!Tcp4Io->IsConnDone && EFI_ERROR (gBS->CheckEvent (Timeout))) {
+    Tcp4->Poll (Tcp4);
+  }
+
+  if (!Tcp4Io->IsConnDone) {
+    Status = EFI_TIMEOUT;
+  } else {
+    Status = Tcp4Io->ConnToken.CompletionToken.Status;
+  }
+
+  return Status;
+}
+
+/**
+  Reset the socket.
+
+  @param[in, out]  Tcp4Io The Tcp4Io wrapping the TCP socket.
+**/
+VOID
+Tcp4IoReset (
+  IN OUT TCP4_IO  *Tcp4Io
+  )
+{
+  EFI_STATUS        Status;
+  EFI_TCP4_PROTOCOL *Tcp4;
+
+  Tcp4Io->CloseToken.AbortOnClose = TRUE;
+  Tcp4Io->IsCloseDone             = FALSE;
+
+  Tcp4 = Tcp4Io->Tcp4;
+  Status = Tcp4->Close (Tcp4, &Tcp4Io->CloseToken);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  while (!Tcp4Io->IsCloseDone) {
+    Tcp4->Poll (Tcp4);
+  }
+}
+
+/**
+  Transmit the Packet to the other endpoint of the socket.
+
+  @param[in]   Tcp4Io          The Tcp4Io wrapping the TCP socket.
+  @param[in]   Packet          The packet to transmit.
+  
+  @retval EFI_SUCCESS          The packet is trasmitted.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoTransmit (
+  IN TCP4_IO  *Tcp4Io,
+  IN NET_BUF  *Packet
+  )
+{
+  EFI_TCP4_TRANSMIT_DATA  *TxData;
+  EFI_TCP4_PROTOCOL       *Tcp4;
+  EFI_STATUS              Status;
+
+  TxData = AllocatePool (sizeof (EFI_TCP4_TRANSMIT_DATA) + (Packet->BlockOpNum - 1) * sizeof (EFI_TCP4_FRAGMENT_DATA));
+  if (TxData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  TxData->Push        = TRUE;
+  TxData->Urgent      = FALSE;
+  TxData->DataLength  = Packet->TotalSize;
+
+  //
+  // Build the fragment table.
+  //
+  TxData->FragmentCount = Packet->BlockOpNum;
+  NetbufBuildExt (Packet, (NET_FRAGMENT *) &TxData->FragmentTable[0], &TxData->FragmentCount);
+
+  Tcp4Io->TxToken.Packet.TxData = TxData;
+
+  //
+  // Trasnmit the packet.
+  //
+  Tcp4    = Tcp4Io->Tcp4;
+  Status  = Tcp4->Transmit (Tcp4, &Tcp4Io->TxToken);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  while (!Tcp4Io->IsTxDone) {
+    Tcp4->Poll (Tcp4);
+  }
+
+  Tcp4Io->IsTxDone  = FALSE;
+
+  Status            = Tcp4Io->TxToken.CompletionToken.Status;
+
+ON_EXIT:
+
+  FreePool (TxData);
+
+  return Status;
+}
+
+/**
+  Receive data from the socket.
+
+  @param[in]  Tcp4Io           The Tcp4Io which wraps the socket to be destroyed.
+  @param[in]  Packet           The buffer to hold the data copy from the soket rx buffer.
+  @param[in]  AsyncMode        Is this receive asyncronous or not.
+  @param[in]  Timeout          The time to wait for receiving the amount of data the Packet
+                               can hold.
+
+  @retval EFI_SUCCESS          The required amount of data is received from the socket.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate momery.
+  @retval EFI_TIMEOUT          Failed to receive the required amount of data in the
+                               specified time period.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoReceive (
+  IN TCP4_IO    *Tcp4Io,
+  IN NET_BUF    *Packet,
+  IN BOOLEAN    AsyncMode,
+  IN EFI_EVENT  Timeout
+  )
+{
+  EFI_TCP4_PROTOCOL     *Tcp4;
+  EFI_TCP4_RECEIVE_DATA RxData;
+  EFI_STATUS            Status;
+  NET_FRAGMENT          *Fragment;
+  UINT32                FragmentCount;
+  UINT32                CurrentFragment;
+
+  FragmentCount = Packet->BlockOpNum;
+  Fragment      = AllocatePool (FragmentCount * sizeof (NET_FRAGMENT));
+  if (Fragment == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  //
+  // Build the fragment table.
+  //
+  NetbufBuildExt (Packet, Fragment, &FragmentCount);
+
+  RxData.FragmentCount          = 1;
+  Tcp4Io->RxToken.Packet.RxData = &RxData;
+  CurrentFragment               = 0;
+  Tcp4                          = Tcp4Io->Tcp4;
+  Status                        = EFI_SUCCESS;
+
+  while (CurrentFragment < FragmentCount) {
+    RxData.DataLength                       = Fragment[CurrentFragment].Len;
+    RxData.FragmentTable[0].FragmentLength  = Fragment[CurrentFragment].Len;
+    RxData.FragmentTable[0].FragmentBuffer  = Fragment[CurrentFragment].Bulk;
+
+    Status = Tcp4->Receive (Tcp4, &Tcp4Io->RxToken);
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+
+    while (!Tcp4Io->IsRxDone && ((Timeout == NULL) || EFI_ERROR (gBS->CheckEvent (Timeout)))) {
+      //
+      // Poll until some data is received or something error happens.
+      //
+      Tcp4->Poll (Tcp4);
+    }
+
+    if (!Tcp4Io->IsRxDone) {
+      //
+      // Timeout occurs, cancel the receive request.
+      //
+      Tcp4->Cancel (Tcp4, &Tcp4Io->RxToken.CompletionToken);
+
+      Status = EFI_TIMEOUT;
+      goto ON_EXIT;
+    } else {
+      Tcp4Io->IsRxDone = FALSE;
+    }
+
+    if (EFI_ERROR (Tcp4Io->RxToken.CompletionToken.Status)) {
+      Status = Tcp4Io->RxToken.CompletionToken.Status;
+      goto ON_EXIT;
+    }
+
+    Fragment[CurrentFragment].Len -= RxData.FragmentTable[0].FragmentLength;
+    if (Fragment[CurrentFragment].Len == 0) {
+      CurrentFragment++;
+    } else {
+      Fragment[CurrentFragment].Bulk += RxData.FragmentTable[0].FragmentLength;
+    }
+  }
+
+ON_EXIT:
+  Tcp4Io->RxToken.Packet.RxData = NULL;
+  FreePool (Fragment);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.h
new file mode 100644
index 0000000000..2b3c0b77d9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/IScsiTcp4Io.h
@@ -0,0 +1,142 @@
+/** @file
+  iSCSI Tcp4 IO related definitions.
+
+Copyright (c) 2004 - 2008, 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.
+
+**/
+
+#ifndef _ISCSI_TCP4_IO_H_
+#define _ISCSI_TCP4_IO_H_
+
+#include <Library/NetLib.h>
+#include <Protocol/Tcp4.h>
+
+typedef struct _TCP4_IO_CONFIG_DATA {
+  EFI_IPv4_ADDRESS  LocalIp;
+  EFI_IPv4_ADDRESS  SubnetMask;
+  EFI_IPv4_ADDRESS  Gateway;
+
+  EFI_IPv4_ADDRESS  RemoteIp;
+  UINT16            RemotePort;
+} TCP4_IO_CONFIG_DATA;
+
+typedef struct _TCP4_IO {
+  EFI_HANDLE                Image;
+  EFI_HANDLE                Controller;
+
+  EFI_HANDLE                Handle;
+  EFI_TCP4_PROTOCOL         *Tcp4;
+
+  EFI_TCP4_CONNECTION_TOKEN ConnToken;
+  EFI_TCP4_IO_TOKEN         TxToken;
+  EFI_TCP4_IO_TOKEN         RxToken;
+  EFI_TCP4_CLOSE_TOKEN      CloseToken;
+
+  BOOLEAN                   IsConnDone;
+  BOOLEAN                   IsTxDone;
+  BOOLEAN                   IsRxDone;
+  BOOLEAN                   IsCloseDone;
+} TCP4_IO;
+
+/**
+  Create a TCP socket with the specified configuration data. 
+
+  @param[in]  Image      The handle of the driver image.
+  @param[in]  Controller The handle of the controller.
+  @param[in]  ConfigData The Tcp4 configuration data.
+  @param[in]  Tcp4Io     The Tcp4Io.
+  
+  @retval EFI_SUCCESS    The TCP socket is created and configured.
+  @retval Others         Failed to create the TCP socket or configure it.
+**/
+EFI_STATUS
+Tcp4IoCreateSocket (
+  IN EFI_HANDLE           Image,
+  IN EFI_HANDLE           Controller,
+  IN TCP4_IO_CONFIG_DATA  *ConfigData,
+  IN TCP4_IO              *Tcp4Io
+  );
+
+/**
+  Destroy the socket. 
+
+  @param[in]  Tcp4Io The Tcp4Io which wraps the socket to be destroyeds.
+**/
+VOID
+Tcp4IoDestroySocket (
+  IN TCP4_IO  *Tcp4Io
+  );
+
+/**
+  Connect to the other endpoint of the TCP socket.
+
+  @param[in, out]  Tcp4Io    The Tcp4Io wrapping the TCP socket.
+  @param[in]       Timeout   The time to wait for connection done.
+  
+  @retval EFI_SUCCESS          Connect to the other endpoint of the TCP socket successfully.
+  @retval EFI_TIMEOUT          Failed to connect to the other endpoint of the TCP socket in the                               specified time period.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoConnect (
+  IN OUT TCP4_IO    *Tcp4Io,
+  IN EFI_EVENT      Timeout
+  );
+
+/**
+  Reset the socket.
+
+  @param[in, out]  Tcp4Io The Tcp4Io wrapping the TCP socket.
+**/
+VOID
+Tcp4IoReset (
+  IN OUT TCP4_IO  *Tcp4Io
+  );
+
+/**
+  Transmit the Packet to the other endpoint of the socket.
+
+  @param[in]   Tcp4Io          The Tcp4Io wrapping the TCP socket.
+  @param[in]   Packet          The packet to transmit.
+  
+  @retval EFI_SUCCESS          The packet is trasmitted.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate memory.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoTransmit (
+  IN TCP4_IO  *Tcp4Io,
+  IN NET_BUF  *Packet
+  );
+
+/**
+  Receive data from the socket.
+
+  @param[in]  Tcp4Io           The Tcp4Io which wraps the socket to be destroyed.
+  @param[in]  Packet           The buffer to hold the data copy from the soket rx buffer.
+  @param[in]  AsyncMode        Is this receive asyncronous or not.
+  @param[in]  Timeout          The time to wait for receiving the amount of data the Packet
+                               can hold.
+
+  @retval EFI_SUCCESS          The required amount of data is received from the socket.
+  @retval EFI_OUT_OF_RESOURCES Failed to allocate momery.
+  @retval EFI_TIMEOUT          Failed to receive the required amount of data in the
+                               specified time period.
+  @retval Others               Other errors as indicated.
+**/
+EFI_STATUS
+Tcp4IoReceive (
+  IN TCP4_IO    *Tcp4Io,
+  IN NET_BUF    *Packet,
+  IN BOOLEAN    AsyncMode,
+  IN EFI_EVENT  Timeout
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.c b/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.c
new file mode 100644
index 0000000000..5dc7d994d6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.c
@@ -0,0 +1,350 @@
+/** @file
+  Implementation of MD5 algorithm.
+
+Copyright (c) 2004 - 2008, 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 "Md5.h"
+
+CONST UINT32  Md5_Data[][2] = {
+  { 0, 1 },
+  { 1, 5 },
+  { 5, 3 },
+  { 0, 7 }
+};
+
+CONST UINT32  Md5_S[][4] = {
+  { 7, 22, 17, 12 },
+  { 5, 20, 14, 9 },
+  { 4, 23, 16 ,11 },
+  { 6, 21, 15, 10 },
+};
+
+CONST UINT32  Md5_T[] = {
+  0xD76AA478, 0xE8C7B756, 0x242070DB, 0xC1BDCEEE,
+  0xF57C0FAF, 0x4787C62A, 0xA8304613, 0xFD469501,
+  0x698098D8, 0x8B44F7AF, 0xFFFF5BB1, 0x895CD7BE,
+  0x6B901122, 0xFD987193, 0xA679438E, 0x49B40821,
+  0xF61E2562, 0xC040B340, 0x265E5A51, 0xE9B6C7AA,
+  0xD62F105D, 0x02441453, 0xD8A1E681, 0xE7D3FBC8,
+  0x21E1CDE6, 0xC33707D6, 0xF4D50D87, 0x455A14ED,
+  0xA9E3E905, 0xFCEFA3F8, 0x676F02D9, 0x8D2A4C8A,
+  0xFFFA3942, 0x8771F681, 0x6D9D6122, 0xFDE5380C,
+  0xA4BEEA44, 0x4BDECFA9, 0xF6BB4B60, 0xBEBFBC70,
+  0x289B7EC6, 0xEAA127FA, 0xD4EF3085, 0x04881D05,
+  0xD9D4D039, 0xE6DB99E5, 0x1FA27CF8, 0xC4AC5665,
+  0xF4292244, 0x432AFF97, 0xAB9423A7, 0xFC93A039,
+  0x655B59C3, 0x8F0CCC92, 0xFFEFF47D, 0x85845DD1,
+  0x6FA87E4F, 0xFE2CE6E0, 0xA3014314, 0x4E0811A1,
+  0xF7537E82, 0xBD3AF235, 0x2AD7D2BB, 0xEB86D391
+};
+
+CONST UINT8 Md5HashPadding[] =
+{
+  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 
+};
+
+//
+// ROTATE_LEFT rotates x left n bits.
+//
+#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n))))
+
+#define SA            MedStates[Index2 & 3]
+#define SB            MedStates[(Index2 + 1) & 3]
+#define SC            MedStates[(Index2 + 2) & 3]
+#define SD            MedStates[(Index2 + 3) & 3]
+
+/**
+  Tf1 is one basic MD5 transform function.
+  
+  @param[in]  A      A  32-bit quantity.
+  @param[in]  B      A  32-bit quantity. 
+  @param[in]  C      A  32-bit quantity.
+
+  @return             Output was produced as a 32-bit quantity based on the
+                      three 32-bit input quantity.   
+**/
+UINT32 
+Tf1 (
+  IN UINT32 A, 
+  IN UINT32 B, 
+  IN UINT32 C
+  )
+{
+  return (A & B) | (~A & C);
+}
+
+/**
+  Tf2 is one basic MD5 transform function.
+  
+  @param[in]  A      A  32-bit quantity.
+  @param[in]  B      A  32-bit quantity. 
+  @param[in]  C      A  32-bit quantity.
+
+  @return             Output was produced as a 32-bit quantity based on the
+                      three 32-bit input quantity.   
+**/
+UINT32 
+Tf2 (
+  IN UINT32 A, 
+  IN UINT32 B, 
+  IN UINT32 C
+  )
+{
+  return (A & C) | (B & ~C);
+}
+
+/**
+  Tf3 is one basic MD5 transform function.
+  
+  @param[in]  A      A  32-bit quantity.
+  @param[in]  B      A  32-bit quantity. 
+  @param[in]  C      A  32-bit quantity.
+
+  @return             Output was produced as a 32-bit quantity based on the
+                      three 32-bit input quantity.   
+**/
+UINT32 
+Tf3 (
+  IN UINT32 A, 
+  IN UINT32 B, 
+  IN UINT32 C
+  )
+{
+  return A ^ B ^ C;
+}
+
+/**
+  Tf4 is one basic MD5 transform function.
+  
+  @param[in]  A      A  32-bit quantity.
+  @param[in]  B      A  32-bit quantity. 
+  @param[in]  C      A  32-bit quantity.
+
+  @return             Output was produced as a 32-bit quantity based on the
+                      three 32-bit input quantity.   
+**/
+UINT32 
+Tf4 (
+  IN UINT32 A, 
+  IN UINT32 B, 
+  IN UINT32 C
+  )
+{
+  return B ^ (A | ~C);
+}
+
+typedef
+UINT32
+(*MD5_TRANSFORM_FUNC) (
+  IN UINT32  A,
+  IN UINT32  B,
+  IN UINT32  C
+  );
+
+CONST MD5_TRANSFORM_FUNC Md5_F[] = {
+  Tf1,
+  Tf2,
+  Tf3,
+  Tf4
+};
+
+/**
+  Perform the MD5 transform on 64 bytes data segment.
+
+  @param[in, out]  Md5Ctx  It includes the data segment for Md5 transform.
+**/
+VOID
+MD5Transform (
+  IN OUT MD5_CTX  *Md5Ctx
+  )
+{
+  UINT32  Index1;
+  UINT32  Index2;
+  UINT32  MedStates[MD5_HASHSIZE >> 2];
+  UINT32  *Data;
+  UINT32  IndexD;
+  UINT32  IndexT;
+
+  Data = (UINT32 *) Md5Ctx->M;
+
+  //
+  // Copy MD5 states to MedStates
+  //
+  CopyMem (MedStates, Md5Ctx->States, MD5_HASHSIZE);
+
+  IndexT = 0;
+  for (Index1 = 0; Index1 < 4; Index1++) {
+    IndexD = Md5_Data[Index1][0];
+    for (Index2 = 16; Index2 > 0; Index2--) {
+      SA += (*Md5_F[Index1]) (SB, SC, SD) + Data[IndexD] + Md5_T[IndexT];
+      SA  = ROTATE_LEFT (SA, Md5_S[Index1][Index2 & 3]);
+      SA += SB;
+
+      IndexD += Md5_Data[Index1][1];
+      IndexD &= 15;
+
+      IndexT++;
+    }
+  }
+
+  for (Index1 = 0; Index1 < 4; Index1++) {
+    Md5Ctx->States[Index1] += MedStates[Index1];
+  }
+}
+
+/**
+  Copy data segment into the M field of MD5_CTX structure for later transform.
+  If the length of data segment is larger than 64 bytes, then does the transform
+  immediately and the generated Md5 code is stored in the States field of MD5_CTX
+  data struct for later accumulation. 
+  All of Md5 code generated for the sequential 64-bytes data segaments are be
+  accumulated in MD5Final() function.
+
+  @param[in, out]  Md5Ctx  The data structure of storing the original data
+                           segment and the final result.
+  @param[in]       Data    The data wanted to be transformed.
+  @param[in]       DataLen The length of data.
+**/
+VOID
+MD5UpdateBlock (
+  IN OUT MD5_CTX  *Md5Ctx,
+  IN CONST UINT8  *Data,
+  IN       UINTN  DataLen
+  )
+{
+  UINTN Limit;
+
+  for (Limit = 64 - Md5Ctx->Count; DataLen >= 64 - Md5Ctx->Count; Limit = 64) {
+    CopyMem (Md5Ctx->M + Md5Ctx->Count, (VOID *)Data, Limit);
+    MD5Transform (Md5Ctx);
+    
+    Md5Ctx->Count = 0;
+    Data         += Limit;
+    DataLen      -= Limit;
+  }
+
+  CopyMem (Md5Ctx->M + Md5Ctx->Count, (VOID *)Data, DataLen);
+  Md5Ctx->Count += DataLen;
+}
+
+/**
+  Initialize four 32-bits chaining variables and use them to do the Md5 transform.
+
+  @param[out]  Md5Ctx The data structure of Md5.
+
+  @retval EFI_SUCCESS Initialization is ok.
+**/
+EFI_STATUS
+MD5Init (
+  OUT MD5_CTX  *Md5Ctx
+  )
+{
+  ZeroMem (Md5Ctx, sizeof (*Md5Ctx));
+
+  //
+  // Set magic initialization constants.
+  //
+  Md5Ctx->States[0] = 0x67452301;
+  Md5Ctx->States[1] = 0xefcdab89;
+  Md5Ctx->States[2] = 0x98badcfe;
+  Md5Ctx->States[3] = 0x10325476;  
+
+  return EFI_SUCCESS;
+}
+
+/**
+  the external interface of Md5 algorithm
+
+  @param[in, out]  Md5Ctx  The data structure of storing the original data
+                           segment and the final result.
+  @param[in]       Data    The data wanted to be transformed.
+  @param[in]       DataLen The length of data.
+
+  @retval EFI_SUCCESS The transform is ok.
+  @retval Others      Other errors as indicated.
+**/
+EFI_STATUS
+MD5Update (
+  IN  OUT MD5_CTX  *Md5Ctx,
+  IN  VOID         *Data,
+  IN  UINTN        DataLen
+  )
+{
+  if (EFI_ERROR (Md5Ctx->Status)) {
+    return Md5Ctx->Status;
+  }
+
+  MD5UpdateBlock (Md5Ctx, (CONST UINT8 *) Data, DataLen);
+  Md5Ctx->Length += DataLen;
+  return EFI_SUCCESS;
+}
+
+/**
+  Accumulate the MD5 value of every data segment and generate the finial
+  result according to MD5 algorithm.
+
+  @param[in, out]   Md5Ctx  The data structure of storing the original data
+                            segment and the final result.
+  @param[out]      HashVal  The final 128-bits output.
+
+  @retval EFI_SUCCESS  The transform is ok.
+  @retval Others       Other errors as indicated.
+**/
+EFI_STATUS
+MD5Final (
+  IN  OUT MD5_CTX  *Md5Ctx,
+  OUT UINT8        *HashVal
+  )
+{
+  UINTN PadLength;
+
+  if (Md5Ctx->Status == EFI_ALREADY_STARTED) {
+    //
+    // Store Hashed value & Zeroize sensitive context information.
+    //
+    CopyMem (HashVal, (UINT8 *) Md5Ctx->States, MD5_HASHSIZE);
+    ZeroMem ((UINT8 *)Md5Ctx, sizeof (*Md5Ctx));
+    
+    return EFI_SUCCESS;
+  }
+
+  if (EFI_ERROR (Md5Ctx->Status)) {
+    return Md5Ctx->Status;
+  }
+
+  PadLength  = Md5Ctx->Count >= 56 ? 120 : 56;
+  PadLength -= Md5Ctx->Count;
+  MD5UpdateBlock (Md5Ctx, Md5HashPadding, PadLength);
+  Md5Ctx->Length = LShiftU64 (Md5Ctx->Length, 3);
+  MD5UpdateBlock (Md5Ctx, (CONST UINT8 *) &Md5Ctx->Length, 8);
+
+  ZeroMem (Md5Ctx->M, sizeof (Md5Ctx->M));
+  Md5Ctx->Length  = 0;
+  Md5Ctx->Status  = EFI_ALREADY_STARTED;
+  return MD5Final (Md5Ctx, HashVal);
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.h b/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.h
new file mode 100644
index 0000000000..8de4e869b4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/IScsiDxe/Md5.h
@@ -0,0 +1,80 @@
+/** @file
+  Header file for Md5.
+
+Copyright (c) 2004 - 2008, 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.
+
+**/
+
+#ifndef _MD5_H_
+#define _MD5_H_
+
+#include <Uefi.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/NetLib.h>
+ 
+#define MD5_HASHSIZE  16
+
+typedef struct _MD5_CTX {
+  EFI_STATUS  Status;
+  UINT64      Length;
+  UINT32      States[MD5_HASHSIZE / sizeof (UINT32)];
+  UINT8       M[64];
+  UINTN       Count;
+} MD5_CTX;
+
+/**
+  Initialize four 32-bits chaining variables and use them to do the Md5 transform.
+
+  @param[out]  Md5Ctx The data structure of Md5.
+
+  @retval EFI_SUCCESS Initialization is ok.
+**/
+EFI_STATUS
+MD5Init (
+  OUT MD5_CTX  *Md5Ctx
+  );
+
+/**
+  the external interface of Md5 algorithm
+
+  @param[in, out]  Md5Ctx  The data structure of storing the original data
+                           segment and the final result.
+  @param[in]       Data    The data wanted to be transformed.
+  @param[in]       DataLen The length of data.
+
+  @retval EFI_SUCCESS The transform is ok.
+  @retval Others      Other errors as indicated.
+**/
+EFI_STATUS
+MD5Update (
+  IN  OUT MD5_CTX  *Md5Ctx,
+  IN  VOID         *Data,
+  IN  UINTN        DataLen
+  );
+
+/**
+  Accumulate the MD5 value of every data segment and generate the finial
+  result according to MD5 algorithm.
+
+  @param[in, out]   Md5Ctx  The data structure of storing the original data
+                            segment and the final result.
+  @param[out]      HashVal  The final 128-bits output.
+
+  @retval EFI_SUCCESS  The transform is ok.
+  @retval Others       Other errors as indicated.
+**/
+EFI_STATUS
+MD5Final (
+  IN  OUT MD5_CTX  *Md5Ctx,
+  OUT UINT8        *HashVal
+  );
+
+#endif 
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/ComponentName.c
new file mode 100644
index 0000000000..2ccfb3cc1c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/ComponentName.c
@@ -0,0 +1,434 @@
+/** @file
+  
+Copyright (c) 2005 - 2012, 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<BR>
+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 "Ip4Impl.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gIp4ComponentName = {
+  Ip4ComponentNameGetDriverName,
+  Ip4ComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gIp4ComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) Ip4ComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) Ip4ComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mIp4DriverNameTable[] = {
+  {
+    "eng;en",
+    L"IP4 Network Service Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE  *gIp4ControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mIp4DriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gIp4ComponentName)
+           );
+
+}
+
+/**
+  Update the component name for the IP4 child handle.
+
+  @param  Ip4[in]                 A pointer to the EFI_IP4_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS             Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER   The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN     EFI_IP4_PROTOCOL         *Ip4
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[80];
+  EFI_IP4_MODE_DATA                Ip4ModeData;
+
+  if (Ip4 == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Format the child name into the string buffer as:
+  // IPv4 (SrcIP=127.0.0.1, DestIP=127.0.0.1)
+  //
+  Status = Ip4->GetModeData (Ip4, &Ip4ModeData, NULL, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (!Ip4ModeData.IsStarted || !Ip4ModeData.IsConfigured) {
+    UnicodeSPrint (HandleName, sizeof (HandleName), L"IPv4 (Not started)");
+  } else {
+    UnicodeSPrint (HandleName, sizeof (HandleName),
+      L"IPv4 (SrcIP=%d.%d.%d.%d)",
+      Ip4ModeData.ConfigData.StationAddress.Addr[0],
+      Ip4ModeData.ConfigData.StationAddress.Addr[1],
+      Ip4ModeData.ConfigData.StationAddress.Addr[2],
+      Ip4ModeData.ConfigData.StationAddress.Addr[3]
+      );
+  }
+
+  if (gIp4ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gIp4ControllerNameTable);
+    gIp4ControllerNameTable = NULL;
+  }
+  Status = AddUnicodeString2 (
+             "eng",
+             gIp4ComponentName.SupportedLanguages,
+             &gIp4ControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gIp4ComponentName2.SupportedLanguages,
+           &gIp4ControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;  
+  EFI_IP4_PROTOCOL              *Ip4;
+  
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiManagedNetworkProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle  
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **)&Ip4,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Ip4);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gIp4ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gIp4ComponentName)
+           );
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.c
new file mode 100644
index 0000000000..004a8bc703
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.c
@@ -0,0 +1,269 @@
+/** @file
+
+Copyright (c) 2005 - 2014, 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 "Ip4Impl.h"
+
+
+/**
+  Return the cast type (Unicast/Boradcast) specific to an
+  interface. All the addresses are host byte ordered.
+
+  @param[in]  IpAddr                The IP address to classify in host byte order
+  @param[in]  IpIf                  The interface that IpAddr received from
+
+  @return The cast type of this IP address specific to the interface.
+  @retval IP4_LOCAL_HOST        The IpAddr equals to the interface's address
+  @retval IP4_SUBNET_BROADCAST  The IpAddr is a directed subnet boradcast to  the
+                                interface
+  @retval IP4_NET_BROADCAST     The IpAddr is a network broadcast to the interface
+  @retval 0                     Otherwise.
+
+**/
+INTN
+Ip4GetNetCast (
+  IN  IP4_ADDR          IpAddr,
+  IN  IP4_INTERFACE     *IpIf
+  )
+{
+  if (IpAddr == IpIf->Ip) {
+    return IP4_LOCAL_HOST;
+
+  } else if (IpAddr == IpIf->SubnetBrdcast) {
+    return IP4_SUBNET_BROADCAST;
+
+  } else if (IpAddr == IpIf->NetBrdcast) {
+    return IP4_NET_BROADCAST;
+
+  }
+
+  return 0;
+}
+
+
+/**
+  Find the cast type of the packet related to the local host.
+  This isn't the same as link layer cast type. For example, DHCP
+  server may send local broadcast to the local unicast MAC.
+
+  @param[in]  IpSb                  The IP4 service binding instance that received the
+                                    packet
+  @param[in]  Dst                   The destination address in the packet (host byte
+                                    order)
+  @param[in]  Src                   The source address in the packet (host byte order)
+
+  @return The cast type for the Dst, it will return on the first non-promiscuous
+          cast type to a configured interface. If the packet doesn't match any of
+          the interface, multicast address and local broadcast address are checked.
+
+**/
+INTN
+Ip4GetHostCast (
+  IN  IP4_SERVICE       *IpSb,
+  IN  IP4_ADDR          Dst,
+  IN  IP4_ADDR          Src
+  )
+{
+  LIST_ENTRY            *Entry;
+  IP4_INTERFACE         *IpIf;
+  INTN                  Type;
+  INTN                  Class;
+
+  Type = 0;
+
+  if (IpSb->MnpConfigData.EnablePromiscuousReceive) {
+    Type = IP4_PROMISCUOUS;
+  }
+
+  //
+  // Go through the interface list of the IP service, most likely.
+  //
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    //
+    // Skip the unconfigured interface and invalid source address:
+    // source address can't be broadcast.
+    //
+    if (!IpIf->Configured || IP4_IS_BROADCAST (Ip4GetNetCast (Src, IpIf))) {
+      continue;
+    }
+
+    if ((Class = Ip4GetNetCast (Dst, IpIf)) > Type) {
+      return Class;
+    }
+  }
+
+  //
+  // If it is local broadcast address. The source address must
+  // be a unicast address on one of the direct connected network.
+  // If it is a multicast address, accept it only if we are in
+  // the group.
+  //
+  if (Dst == IP4_ALLONE_ADDRESS) {
+    IpIf = Ip4FindNet (IpSb, Src);
+
+    if (IpIf != NULL && !IP4_IS_BROADCAST (Ip4GetNetCast (Src, IpIf))) {
+      return IP4_LOCAL_BROADCAST;
+    }
+
+  } else if (IP4_IS_MULTICAST (Dst) && Ip4FindGroup (&IpSb->IgmpCtrl, Dst) != NULL) {
+    return IP4_MULTICAST;
+  }
+
+  return Type;
+}
+
+
+/**
+  Find an interface whose configured IP address is Ip.
+
+  @param[in]  IpSb                  The IP4 service binding instance
+  @param[in]  Ip                    The Ip address (host byte order) to find
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindInterface (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip
+  )
+{
+  LIST_ENTRY            *Entry;
+  IP4_INTERFACE         *IpIf;
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured && (IpIf->Ip == Ip)) {
+      return IpIf;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Find an interface that Ip is on that connected network.
+
+  @param[in]  IpSb                  The IP4 service binding instance
+  @param[in]  Ip                    The Ip address (host byte order) to find
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindNet (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip
+  )
+{
+  LIST_ENTRY            *Entry;
+  IP4_INTERFACE         *IpIf;
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured && IP4_NET_EQUAL (Ip, IpIf->Ip, IpIf->SubnetMask)) {
+      return IpIf;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Find an interface of the service with the same Ip/Netmask pair.
+
+  @param[in]  IpSb                  Ip4 service binding instance
+  @param[in]  Ip                    The Ip adress to find (host byte order)
+  @param[in]  Netmask               The network to find (host byte order)
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindStationAddress (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip,
+  IN IP4_ADDR           Netmask
+  )
+{
+  LIST_ENTRY      *Entry;
+  IP4_INTERFACE   *IpIf;
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured && (IpIf->Ip == Ip) && (IpIf->SubnetMask == Netmask)) {
+      return IpIf;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get the MAC address for a multicast IP address. Call
+  Mnp's McastIpToMac to find the MAC address in stead of
+  hard code the NIC to be Ethernet.
+
+  @param[in]  Mnp                   The Mnp instance to get the MAC address.
+  @param[in]  Multicast             The multicast IP address to translate.
+  @param[out] Mac                   The buffer to hold the translated address.
+
+  @retval EFI_SUCCESS if the multicast IP is successfully translated to a
+                      multicast MAC address.
+  @retval other       Otherwise some error.
+
+**/
+EFI_STATUS
+Ip4GetMulticastMac (
+  IN  EFI_MANAGED_NETWORK_PROTOCOL *Mnp,
+  IN  IP4_ADDR                     Multicast,
+  OUT EFI_MAC_ADDRESS              *Mac
+  )
+{
+  EFI_IP_ADDRESS        EfiIp;
+
+  EFI_IP4 (EfiIp.v4) = HTONL (Multicast);
+  return Mnp->McastIpToMac (Mnp, FALSE, &EfiIp, Mac);
+}
+
+
+/**
+  Convert the multibyte field in IP header's byter order.
+  In spite of its name, it can also be used to convert from
+  host to network byte order.
+
+  @param[in]  Head                  The IP head to convert
+
+  @return Point to the converted IP head
+
+**/
+IP4_HEAD *
+Ip4NtohHead (
+  IN IP4_HEAD           *Head
+  )
+{
+  Head->TotalLen  = NTOHS (Head->TotalLen);
+  Head->Id        = NTOHS (Head->Id);
+  Head->Fragment  = NTOHS (Head->Fragment);
+  Head->Src       = NTOHL (Head->Src);
+  Head->Dst       = NTOHL (Head->Dst);
+
+  return Head;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.h
new file mode 100644
index 0000000000..d38857c677
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Common.h
@@ -0,0 +1,204 @@
+/** @file
+  Common definition for IP4.
+  
+Copyright (c) 2005 - 2014, 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.
+
+**/
+
+#ifndef __EFI_IP4_COMMON_H__
+#define __EFI_IP4_COMMON_H__
+
+typedef struct _IP4_INTERFACE  IP4_INTERFACE;
+typedef struct _IP4_PROTOCOL   IP4_PROTOCOL;
+typedef struct _IP4_SERVICE    IP4_SERVICE;
+
+#define IP4_ETHER_PROTO       0x0800
+
+//
+// The packet is received as link level broadcast/multicast/promiscuous.
+//
+#define IP4_LINK_BROADCAST    0x00000001
+#define IP4_LINK_MULTICAST    0x00000002
+#define IP4_LINK_PROMISC      0x00000004
+
+//
+// IP4 address cast type classfication. Keep it true that any
+// type bigger than or equal to LOCAL_BROADCAST is broadcast.
+//
+#define IP4_PROMISCUOUS       1
+#define IP4_LOCAL_HOST        2
+#define IP4_MULTICAST         3
+#define IP4_LOCAL_BROADCAST   4  // Destination is 255.255.255.255
+#define IP4_SUBNET_BROADCAST  5
+#define IP4_NET_BROADCAST     6
+
+//
+// IP4 header flags
+//
+#define IP4_HEAD_DF_MASK      0x4000
+#define IP4_HEAD_MF_MASK      0x2000
+#define IP4_HEAD_OFFSET_MASK  0x1fff
+
+#define IP4_ALLZERO_ADDRESS   0x00000000u
+#define IP4_ALLONE_ADDRESS    0xFFFFFFFFu
+#define IP4_ALLSYSTEM_ADDRESS 0xE0000001u
+#define IP4_ALLROUTER_ADDRESS 0xE0000002u
+
+///
+/// Compose the fragment field to be used in the IP4 header.
+///
+#define IP4_HEAD_FRAGMENT_FIELD(Df, Mf, Offset) \
+    ((UINT16)(((Df) ? 0x4000 : 0) | ((Mf) ? 0x2000 : 0) | (((Offset) >> 3) & 0x1fff)))
+
+#define IP4_LAST_FRAGMENT(FragmentField)  \
+          (((FragmentField) & IP4_HEAD_MF_MASK) == 0)
+
+#define IP4_FIRST_FRAGMENT(FragmentField) \
+          ((BOOLEAN)(((FragmentField) & IP4_HEAD_OFFSET_MASK) == 0))
+
+#define IP4_DO_NOT_FRAGMENT(FragmentField) \
+          ((BOOLEAN)(((FragmentField) & IP4_HEAD_DF_MASK) == IP4_HEAD_DF_MASK))
+
+#define IP4_IS_BROADCAST(CastType) ((CastType) >= IP4_LOCAL_BROADCAST)
+
+///
+/// Conver the Microsecond to second. IP transmit/receive time is
+/// in the unit of microsecond. IP ticks once per second.
+///
+#define IP4_US_TO_SEC(Us) (((Us) + 999999) / 1000000)
+
+/**
+  Return the cast type (Unicast/Boradcast) specific to an
+  interface. All the addresses are host byte ordered.
+
+  @param[in]  IpAddr                The IP address to classify in host byte order
+  @param[in]  IpIf                  The interface that IpAddr received from
+
+  @return The cast type of this IP address specific to the interface.
+  @retval IP4_LOCAL_HOST        The IpAddr equals to the interface's address
+  @retval IP4_SUBNET_BROADCAST  The IpAddr is a directed subnet boradcast to  the
+                                interface
+  @retval IP4_NET_BROADCAST     The IpAddr is a network broadcast to the interface
+  @retval 0                     Otherwise.
+
+**/
+INTN
+Ip4GetNetCast (
+  IN  IP4_ADDR          IpAddr,
+  IN  IP4_INTERFACE     *IpIf
+  );
+
+/**
+  Find the cast type of the packet related to the local host.
+  This isn't the same as link layer cast type. For example, DHCP
+  server may send local broadcast to the local unicast MAC.
+
+  @param[in]  IpSb                  The IP4 service binding instance that received the
+                                    packet
+  @param[in]  Dst                   The destination address in the packet (host byte
+                                    order)
+  @param[in]  Src                   The source address in the packet (host byte order)
+
+  @return The cast type for the Dst, it will return on the first non-promiscuous
+          cast type to a configured interface. If the packet doesn't match any of
+          the interface, multicast address and local broadcast address are checked.
+
+**/
+INTN
+Ip4GetHostCast (
+  IN  IP4_SERVICE       *IpSb,
+  IN  IP4_ADDR          Dst,
+  IN  IP4_ADDR          Src
+  );
+
+/**
+  Find an interface whose configured IP address is Ip.
+
+  @param[in]  IpSb                  The IP4 service binding instance
+  @param[in]  Ip                    The Ip address (host byte order) to find
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindInterface (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip
+  );
+
+/**
+  Find an interface that Ip is on that connected network.
+
+  @param[in]  IpSb                  The IP4 service binding instance
+  @param[in]  Ip                    The Ip address (host byte order) to find
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindNet (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip
+  );
+
+/**
+  Find an interface of the service with the same Ip/Netmask pair.
+
+  @param[in]  IpSb                  Ip4 service binding instance
+  @param[in]  Ip                    The Ip adress to find (host byte order)
+  @param[in]  Netmask               The network to find (host byte order)
+
+  @return The IP4_INTERFACE point if found, otherwise NULL
+
+**/
+IP4_INTERFACE *
+Ip4FindStationAddress (
+  IN IP4_SERVICE        *IpSb,
+  IN IP4_ADDR           Ip,
+  IN IP4_ADDR           Netmask
+  );
+
+/**
+  Get the MAC address for a multicast IP address. Call
+  Mnp's McastIpToMac to find the MAC address in stead of
+  hard code the NIC to be Ethernet.
+
+  @param[in]  Mnp                   The Mnp instance to get the MAC address.
+  @param[in]  Multicast             The multicast IP address to translate.
+  @param[out] Mac                   The buffer to hold the translated address.
+
+  @retval EFI_SUCCESS if the multicast IP is successfully translated to a
+                      multicast MAC address.
+  @retval other       Otherwise some error.
+
+**/
+EFI_STATUS
+Ip4GetMulticastMac (
+  IN  EFI_MANAGED_NETWORK_PROTOCOL *Mnp,
+  IN  IP4_ADDR                     Multicast,
+  OUT EFI_MAC_ADDRESS              *Mac
+  );
+
+/**
+  Convert the multibyte field in IP header's byter order.
+  In spite of its name, it can also be used to convert from
+  host to network byte order.
+
+  @param[in]  Head                  The IP head to convert
+
+  @return Point to the converted IP head
+
+**/
+IP4_HEAD *
+Ip4NtohHead (
+  IN IP4_HEAD           *Head
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2.vfr b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2.vfr
new file mode 100644
index 0000000000..a18db22846
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2.vfr
@@ -0,0 +1,100 @@
+/** @file
+  Vfr file for IP4Dxe.
+
+Copyright (c) 2015, 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 "Ip4NvData.h"
+
+#define EFI_NETWORK_DEVICE_CLASS  0x04
+
+formset
+  guid     = IP4_CONFIG2_NVDATA_GUID,
+  title    = STRING_TOKEN(STR_IP4_CONFIG2_FORM_TITLE),
+  help     = STRING_TOKEN(STR_IP4_CONFIG2_FORM_HELP),
+  class    = EFI_NETWORK_DEVICE_CLASS,
+  subclass = 0x03,
+
+  varstore IP4_CONFIG2_IFR_NVDATA,
+    name = IP4_CONFIG2_IFR_NVDATA,
+    guid = IP4_CONFIG2_NVDATA_GUID;
+
+  form formid = FORMID_MAIN_FORM,
+    title  = STRING_TOKEN(STR_IP4_DEVICE_FORM_TITLE);
+
+    checkbox varid = IP4_CONFIG2_IFR_NVDATA.Configure,
+            prompt = STRING_TOKEN(STR_IP4_CONFIGURE),
+            help   = STRING_TOKEN(STR_IP4_CONFIGURE),
+            flags  = INTERACTIVE,
+            key    = KEY_ENABLE,
+    endcheckbox;
+
+    suppressif ideqval IP4_CONFIG2_IFR_NVDATA.Configure == 0x00;
+
+      checkbox varid = IP4_CONFIG2_IFR_NVDATA.DhcpEnable,
+              prompt = STRING_TOKEN(STR_IP4_ENABLE_DHCP),
+              help   = STRING_TOKEN(STR_IP4_ENABLE_DHCP),
+              flags  = INTERACTIVE,
+              key    = KEY_DHCP_ENABLE,
+      endcheckbox;
+    endif;
+
+    suppressif ideqval IP4_CONFIG2_IFR_NVDATA.DhcpEnable == 0x01 OR ideqval IP4_CONFIG2_IFR_NVDATA.Configure == 0x00;
+
+      string  varid   = IP4_CONFIG2_IFR_NVDATA.StationAddress,
+              prompt  = STRING_TOKEN(STR_IP4_LOCAL_IP_ADDRESS),
+              help    = STRING_TOKEN(STR_IP4_IP_ADDRESS_HELP),
+              flags   = INTERACTIVE,
+              key     = KEY_LOCAL_IP,
+              minsize = IP_MIN_SIZE,
+              maxsize = IP_MAX_SIZE,
+      endstring;
+
+      string  varid   = IP4_CONFIG2_IFR_NVDATA.SubnetMask,
+              prompt  = STRING_TOKEN(STR_IP4_LOCAL_MASK),
+              help    = STRING_TOKEN(STR_IP4_MASK_HELP),
+              flags   = INTERACTIVE,
+              key     = KEY_SUBNET_MASK,
+              minsize = IP_MIN_SIZE,
+             maxsize = IP_MAX_SIZE,
+      endstring;
+
+      string  varid   = IP4_CONFIG2_IFR_NVDATA.GatewayAddress,
+              prompt  = STRING_TOKEN(STR_IP4_LOCAL_GATEWAY),
+              help    = STRING_TOKEN(STR_IP4_GATEWAY_HELP),
+              flags   = INTERACTIVE,
+              key     = KEY_GATE_WAY,
+              minsize = IP_MIN_SIZE,
+              maxsize = IP_MAX_SIZE,
+      endstring;
+
+      string  varid   = IP4_CONFIG2_IFR_NVDATA.DnsAddress,
+              prompt  = STRING_TOKEN(STR_IP4_LOCAL_DNS),
+              help    = STRING_TOKEN(STR_IP4_DNS_HELP),
+              flags   = INTERACTIVE,
+              key     = KEY_DNS,
+              minsize = IP_MIN_SIZE,
+              maxsize = ADDRESS_STR_MAX_SIZE,
+      endstring;
+
+    endif;
+
+    subtitle text = STRING_TOKEN(STR_NULL);
+    
+    text
+      help   = STRING_TOKEN(STR_SAVE_CHANGES),
+      text   = STRING_TOKEN(STR_SAVE_CHANGES),
+      flags  = INTERACTIVE,
+      key    = KEY_SAVE_CHANGES;
+      
+  endform;
+
+endformset;
+
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.c
new file mode 100644
index 0000000000..edbddba021
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.c
@@ -0,0 +1,2031 @@
+/** @file
+  The implementation of EFI IPv4 Configuration II Protocol.
+
+  Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
+  (C) Copyright 2015 Hewlett Packard Enterprise Development LP<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 "Ip4Impl.h"
+
+LIST_ENTRY  mIp4Config2InstanceList = {&mIp4Config2InstanceList, &mIp4Config2InstanceList};
+
+/**
+  The event process routine when the DHCPv4 service binding protocol is installed
+  in the system.
+
+  @param[in]     Event         Not used.
+  @param[in]     Context       Pointer to the IP4 config2 instance data.
+
+**/
+VOID
+EFIAPI
+Ip4Config2OnDhcp4SbInstalled (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  Destroy the Dhcp4 child in IP4_CONFIG2_INSTANCE and release the resources.
+
+  @param[in, out] Instance    The buffer of IP4 config2 instance to be freed.
+
+  @retval EFI_SUCCESS         The child was successfully destroyed.
+  @retval Others              Failed to destroy the child.
+
+**/
+EFI_STATUS
+Ip4Config2DestroyDhcp4 (
+  IN OUT IP4_CONFIG2_INSTANCE  *Instance
+  )
+{
+  IP4_SERVICE                 *IpSb;
+  EFI_STATUS                  Status;
+  EFI_DHCP4_PROTOCOL          *Dhcp4;
+
+  Dhcp4 = Instance->Dhcp4;
+  ASSERT (Dhcp4 != NULL);
+
+  Dhcp4->Stop (Dhcp4);
+  Dhcp4->Configure (Dhcp4, NULL);
+  Instance->Dhcp4 = NULL;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  //
+  // Close DHCPv4 protocol and destroy the child.
+  //
+  Status = gBS->CloseProtocol (
+                  Instance->Dhcp4Handle,
+                  &gEfiDhcp4ProtocolGuid,
+                  IpSb->Image,
+                  IpSb->Controller
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = NetLibDestroyServiceChild (
+             IpSb->Controller,
+             IpSb->Image,
+             &gEfiDhcp4ServiceBindingProtocolGuid,
+             Instance->Dhcp4Handle
+             );
+
+  Instance->Dhcp4Handle = NULL;
+
+  return Status;  
+}
+
+/**
+  Update the current policy to NewPolicy. During the transition
+  period, the default router list
+  and address list in all interfaces will be released.
+
+  @param[in]  IpSb               The IP4 service binding instance.
+  @param[in]  NewPolicy          The new policy to be updated to.
+
+**/
+VOID
+Ip4Config2OnPolicyChanged (
+  IN IP4_SERVICE            *IpSb,
+  IN EFI_IP4_CONFIG2_POLICY NewPolicy
+  )
+{
+  IP4_INTERFACE   *IpIf;
+  IP4_ROUTE_TABLE *RouteTable;
+
+  //
+  // Currently there are only two policies: static and dhcp. Regardless of
+  // what transition is going on, i.e., static -> dhcp and dhcp ->
+  // static, we have to free default router table and all addresses.
+  //
+
+  if (IpSb->DefaultInterface != NULL) {
+    if (IpSb->DefaultRouteTable != NULL) {
+      Ip4FreeRouteTable (IpSb->DefaultRouteTable);
+      IpSb->DefaultRouteTable = NULL;    
+    }
+
+    Ip4CancelReceive (IpSb->DefaultInterface);
+
+    Ip4FreeInterface (IpSb->DefaultInterface, NULL);
+    IpSb->DefaultInterface = NULL;
+  }
+
+  Ip4CleanAssembleTable (&IpSb->Assemble);
+
+  //
+  // Create new default interface and route table.
+  //    
+  IpIf = Ip4CreateInterface (IpSb->Mnp, IpSb->Controller, IpSb->Image);
+  if (IpIf == NULL) {
+    return ;
+  }
+
+  RouteTable = Ip4CreateRouteTable ();
+  if (RouteTable == NULL) {
+    Ip4FreeInterface (IpIf, NULL);
+    return ;
+  }
+  
+  IpSb->DefaultInterface  = IpIf;
+  InsertHeadList (&IpSb->Interfaces, &IpIf->Link);
+  IpSb->DefaultRouteTable = RouteTable;
+  Ip4ReceiveFrame (IpIf, NULL, Ip4AccpetFrame, IpSb);
+
+  if (IpSb->State == IP4_SERVICE_CONFIGED) {
+    IpSb->State = IP4_SERVICE_UNSTARTED;
+  }
+
+  //
+  // Start the dhcp configuration.
+  //
+  if (NewPolicy == Ip4Config2PolicyDhcp) {
+    IpSb->Reconfig = TRUE;
+    Ip4StartAutoConfig (&IpSb->Ip4Config2Instance);
+  }
+
+}
+
+/**
+  Signal the registered event. It is the callback routine for NetMapIterate.
+
+  @param[in]  Map    Points to the list of registered event.
+  @param[in]  Item   The registered event.
+  @param[in]  Arg    Not used.
+
+  @retval EFI_SUCCESS           The event was signaled successfully.
+**/
+EFI_STATUS
+EFIAPI
+Ip4Config2SignalEvent (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Arg
+  )
+{
+  gBS->SignalEvent ((EFI_EVENT) Item->Key);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Read the configuration data from variable storage according to the VarName and
+  gEfiIp4Config2ProtocolGuid. It checks the integrity of variable data. If the
+  data is corrupted, it clears the variable data to ZERO. Othewise, it outputs the
+  configuration data to IP4_CONFIG2_INSTANCE.
+
+  @param[in]      VarName       The pointer to the variable name
+  @param[in, out] Instance      The pointer to the IP4 config2 instance data.
+
+  @retval EFI_NOT_FOUND         The variable can not be found or already corrupted.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate resource to complete the operation.
+  @retval EFI_SUCCESS           The configuration data was retrieved successfully.
+
+**/
+EFI_STATUS
+Ip4Config2ReadConfigData (
+  IN     CHAR16               *VarName,
+  IN OUT IP4_CONFIG2_INSTANCE *Instance
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   VarSize;
+  IP4_CONFIG2_VARIABLE    *Variable;
+  IP4_CONFIG2_DATA_ITEM   *DataItem;
+  UINTN                   Index;
+  IP4_CONFIG2_DATA_RECORD DataRecord;
+  CHAR8                   *Data;
+
+  //
+  // Try to read the configuration variable.
+  //
+  VarSize = 0;
+  Status  = gRT->GetVariable (
+                   VarName,
+                   &gEfiIp4Config2ProtocolGuid,
+                   NULL,
+                   &VarSize,
+                   NULL
+                   );
+
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // Allocate buffer and read the config variable.
+    //
+    Variable = AllocatePool (VarSize);
+    if (Variable == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = gRT->GetVariable (
+                    VarName,
+                    &gEfiIp4Config2ProtocolGuid,
+                    NULL,
+                    &VarSize,
+                    Variable
+                    );
+    if (EFI_ERROR (Status) || (UINT16) (~NetblockChecksum ((UINT8 *) Variable, (UINT32) VarSize)) != 0) {
+      //
+      // GetVariable still error or the variable is corrupted.
+      // Fall back to the default value.
+      //
+      FreePool (Variable);
+
+      //
+      // Remove the problematic variable and return EFI_NOT_FOUND, a new
+      // variable will be set again.
+      //
+      gRT->SetVariable (
+             VarName,
+             &gEfiIp4Config2ProtocolGuid,
+             IP4_CONFIG2_VARIABLE_ATTRIBUTE,
+             0,
+             NULL
+             );
+
+      return EFI_NOT_FOUND;
+    }
+
+ 
+    for (Index = 0; Index < Variable->DataRecordCount; Index++) {
+
+      CopyMem (&DataRecord, &Variable->DataRecord[Index], sizeof (DataRecord));
+
+      DataItem = &Instance->DataItem[DataRecord.DataType];
+      if (DATA_ATTRIB_SET (DataItem->Attribute, DATA_ATTRIB_SIZE_FIXED) &&
+          (DataItem->DataSize != DataRecord.DataSize)
+          ) {
+        //
+        // Perhaps a corrupted data record...
+        //
+        continue;
+      }
+
+      if (!DATA_ATTRIB_SET (DataItem->Attribute, DATA_ATTRIB_SIZE_FIXED)) {
+        //
+        // This data item has variable length data.
+        //
+        DataItem->Data.Ptr = AllocatePool (DataRecord.DataSize);
+        if (DataItem->Data.Ptr == NULL) {
+          //
+          // no memory resource
+          //
+          continue;
+        }
+      }
+
+      Data = (CHAR8 *) Variable + DataRecord.Offset;
+      CopyMem (DataItem->Data.Ptr, Data, DataRecord.DataSize);
+
+      DataItem->DataSize = DataRecord.DataSize;
+      DataItem->Status   = EFI_SUCCESS;
+    }
+
+    FreePool (Variable);
+    return EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+/**
+  Write the configuration data from IP4_CONFIG2_INSTANCE to variable storage.
+
+  @param[in]      VarName       The pointer to the variable name.
+  @param[in]      Instance      The pointer to the IP4 config2 instance data.
+
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate resource to complete the operation.
+  @retval EFI_SUCCESS           The configuration data is written successfully.
+
+**/
+EFI_STATUS
+Ip4Config2WriteConfigData (
+  IN CHAR16               *VarName,
+  IN IP4_CONFIG2_INSTANCE *Instance
+  )
+{
+  UINTN                   Index;
+  UINTN                   VarSize;
+  IP4_CONFIG2_DATA_ITEM   *DataItem;
+  IP4_CONFIG2_VARIABLE    *Variable;
+  IP4_CONFIG2_DATA_RECORD *DataRecord;
+  CHAR8                   *Heap;
+  EFI_STATUS              Status;
+
+  VarSize = sizeof (IP4_CONFIG2_VARIABLE) - sizeof (IP4_CONFIG2_DATA_RECORD);
+
+  for (Index = 0; Index < Ip4Config2DataTypeMaximum; Index++) {
+
+    DataItem = &Instance->DataItem[Index];
+    if (!DATA_ATTRIB_SET (DataItem->Attribute, DATA_ATTRIB_VOLATILE) && !EFI_ERROR (DataItem->Status)) {
+
+      VarSize += sizeof (IP4_CONFIG2_DATA_RECORD) + DataItem->DataSize;
+    }
+  }
+
+  Variable = AllocatePool (VarSize);
+  if (Variable == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Heap                      = (CHAR8 *) Variable + VarSize;
+  Variable->DataRecordCount = 0;
+
+  for (Index = 0; Index < Ip4Config2DataTypeMaximum; Index++) {
+
+    DataItem = &Instance->DataItem[Index];
+    if (!DATA_ATTRIB_SET (DataItem->Attribute, DATA_ATTRIB_VOLATILE) && !EFI_ERROR (DataItem->Status)) {
+
+      Heap -= DataItem->DataSize;
+      CopyMem (Heap, DataItem->Data.Ptr, DataItem->DataSize);
+
+      DataRecord           = &Variable->DataRecord[Variable->DataRecordCount];
+      DataRecord->DataType = (EFI_IP4_CONFIG2_DATA_TYPE) Index;
+      DataRecord->DataSize = (UINT32) DataItem->DataSize;
+      DataRecord->Offset   = (UINT16) (Heap - (CHAR8 *) Variable);
+
+      Variable->DataRecordCount++;
+    }
+  }
+
+  Variable->Checksum = 0;
+  Variable->Checksum = (UINT16) ~NetblockChecksum ((UINT8 *) Variable, (UINT32) VarSize);
+
+  Status = gRT->SetVariable (
+                  VarName,
+                  &gEfiIp4Config2ProtocolGuid,
+                  IP4_CONFIG2_VARIABLE_ATTRIBUTE,
+                  VarSize,
+                  Variable
+                  );
+
+  FreePool (Variable);
+
+  return Status;
+}
+
+
+/**
+  Build a EFI_IP4_ROUTE_TABLE to be returned to the caller of GetModeData. 
+  The EFI_IP4_ROUTE_TABLE is clumsy to use in the internal operation of the 
+  IP4 driver.
+
+  @param[in]   IpSb        The IP4 service binding instance.
+  @param[out]  Table       The built IP4 route table.
+
+  @retval EFI_SUCCESS           The route table is successfully build
+  @retval EFI_NOT_FOUND         Failed to allocate the memory for the rotue table.
+
+**/
+EFI_STATUS
+Ip4Config2BuildDefaultRouteTable (
+  IN  IP4_SERVICE               *IpSb,
+  OUT EFI_IP4_ROUTE_TABLE       *Table
+  )
+{
+  LIST_ENTRY                *Entry; 
+  IP4_ROUTE_ENTRY           *RtEntry;
+  UINT32                    Count;
+  INT32                     Index;
+
+  if (IpSb->DefaultRouteTable == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Count = IpSb->DefaultRouteTable->TotalNum;
+
+  if (Count == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Copy the route entry to EFI route table. Keep the order of
+  // route entry copied from most specific to default route. That
+  // is, interlevel the route entry from the instance's route area
+  // and those from the default route table's route area.
+  //
+  Count = 0;
+
+  for (Index = IP4_MASK_NUM - 1; Index >= 0; Index--) {
+
+    NET_LIST_FOR_EACH (Entry, &(IpSb->DefaultRouteTable->RouteArea[Index])) {
+      RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+      EFI_IP4 (Table[Count].SubnetAddress)  = HTONL (RtEntry->Dest & RtEntry->Netmask);
+      EFI_IP4 (Table[Count].SubnetMask)     = HTONL (RtEntry->Netmask);
+      EFI_IP4 (Table[Count].GatewayAddress) = HTONL (RtEntry->NextHop);
+
+      Count++;
+    }
+
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The event process routine when the DHCPv4 service binding protocol is installed
+  in the system.
+
+  @param[in]     Event         Not used.
+  @param[in]     Context       The pointer to the IP4 config2 instance data.
+
+**/
+VOID
+EFIAPI
+Ip4Config2OnDhcp4SbInstalled (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  IP4_CONFIG2_INSTANCE  *Instance;
+
+  Instance = (IP4_CONFIG2_INSTANCE *) Context;
+
+  if ((Instance->Dhcp4Handle != NULL) || (Instance->Policy != Ip4Config2PolicyDhcp)) {
+    //
+    // The DHCP4 child is already created or the policy is no longer DHCP.
+    //
+    return ;
+  }
+
+  Ip4StartAutoConfig (Instance);
+}
+
+/**
+  Set the station address and subnetmask for the default interface.
+
+  @param[in]  IpSb               The pointer to the IP4 service binding instance.
+  @param[in]  StationAddress     Ip address to be set.
+  @param[in]  SubnetMask         Subnet to be set.
+
+  @retval EFI_SUCCESS   Set default address successful.     
+  @retval Others        Some errors occur in setting.     
+
+**/
+EFI_STATUS
+Ip4Config2SetDefaultAddr (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_ADDR               StationAddress,
+  IN IP4_ADDR               SubnetMask
+  )
+{
+  EFI_STATUS                Status;
+  IP4_INTERFACE             *IpIf;
+  IP4_PROTOCOL              *Ip4Instance;
+  EFI_ARP_PROTOCOL          *Arp;
+  LIST_ENTRY                *Entry;
+  IP4_ADDR                  Subnet;
+  IP4_ROUTE_TABLE           *RouteTable;
+
+  IpIf = IpSb->DefaultInterface;
+  ASSERT (IpIf != NULL);
+
+  if ((IpIf->Ip == StationAddress) && (IpIf->SubnetMask == SubnetMask)) {
+    IpSb->State = IP4_SERVICE_CONFIGED;
+    return EFI_SUCCESS;
+  }
+
+  if (IpSb->Reconfig) {
+    //
+    // The default address is changed, free the previous interface first.
+    //
+    if (IpSb->DefaultRouteTable != NULL) {
+      Ip4FreeRouteTable (IpSb->DefaultRouteTable);
+      IpSb->DefaultRouteTable = NULL;    
+    }
+
+    Ip4CancelReceive (IpSb->DefaultInterface);
+    Ip4FreeInterface (IpSb->DefaultInterface, NULL);
+    IpSb->DefaultInterface = NULL;
+    //
+    // Create new default interface and route table.
+    //    
+    IpIf = Ip4CreateInterface (IpSb->Mnp, IpSb->Controller, IpSb->Image);
+    if (IpIf == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    RouteTable = Ip4CreateRouteTable ();
+    if (RouteTable == NULL) {
+      Ip4FreeInterface (IpIf, NULL);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    
+    IpSb->DefaultInterface  = IpIf;
+    InsertHeadList (&IpSb->Interfaces, &IpIf->Link);
+    IpSb->DefaultRouteTable = RouteTable;
+    Ip4ReceiveFrame (IpIf, NULL, Ip4AccpetFrame, IpSb);
+  }
+
+  if (IpSb->State == IP4_SERVICE_CONFIGED) {
+    IpSb->State = IP4_SERVICE_UNSTARTED;
+  }
+
+  Status = Ip4SetAddress (IpIf, StationAddress, SubnetMask);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (IpIf->Arp != NULL) {
+    //   
+    // A non-NULL IpIf->Arp here means a new ARP child is created when setting default address, 
+    // but some IP children may have referenced the default interface before it is configured,
+    // these IP instances also consume this ARP protocol so they need to open it BY_CHILD_CONTROLLER.
+    //
+    Arp = NULL;
+    NET_LIST_FOR_EACH (Entry, &IpIf->IpInstances) {
+      Ip4Instance = NET_LIST_USER_STRUCT_S (Entry, IP4_PROTOCOL, AddrLink, IP4_PROTOCOL_SIGNATURE);
+      Status = gBS->OpenProtocol (
+                      IpIf->ArpHandle,
+                      &gEfiArpProtocolGuid,
+                      (VOID **) &Arp,
+                      gIp4DriverBinding.DriverBindingHandle,
+                      Ip4Instance->Handle,
+                      EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                      );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+
+  Ip4AddRoute (
+    IpSb->DefaultRouteTable,
+    StationAddress,
+    SubnetMask,
+    IP4_ALLZERO_ADDRESS
+    );
+
+  //
+  // Add a route for the connected network.
+  //
+  Subnet = StationAddress & SubnetMask;
+
+  Ip4AddRoute (
+    IpSb->DefaultRouteTable,
+    Subnet,
+    SubnetMask,
+    IP4_ALLZERO_ADDRESS
+    );
+
+  IpSb->State = IP4_SERVICE_CONFIGED;
+  IpSb->Reconfig = FALSE;
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Set the station address, subnetmask and gateway address for the default interface.
+
+  @param[in]  Instance         The pointer to the IP4 config2 instance data.  
+  @param[in]  StationAddress   Ip address to be set.
+  @param[in]  SubnetMask       Subnet to be set.
+  @param[in]  GatewayAddress   Gateway to be set.
+
+  @retval EFI_SUCCESS     Set default If successful.    
+  @retval Others          Errors occur as indicated.  
+
+**/
+EFI_STATUS
+Ip4Config2SetDefaultIf (
+  IN IP4_CONFIG2_INSTANCE   *Instance,
+  IN IP4_ADDR               StationAddress,
+  IN IP4_ADDR               SubnetMask,
+  IN IP4_ADDR               GatewayAddress
+  )
+{
+  EFI_STATUS                Status;
+  IP4_SERVICE               *IpSb;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  Status = Ip4Config2SetDefaultAddr (IpSb, StationAddress, SubnetMask);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Create a route if there is a default router.
+  //
+  if (GatewayAddress != IP4_ALLZERO_ADDRESS) {
+    Ip4AddRoute (
+      IpSb->DefaultRouteTable,
+      IP4_ALLZERO_ADDRESS,
+      IP4_ALLZERO_ADDRESS,
+      GatewayAddress
+      );        
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Release all the DHCP related resources.
+
+  @param  Instance              The IP4 config2 instance.
+
+  @return None
+
+**/
+VOID
+Ip4Config2CleanDhcp4 (
+  IN IP4_CONFIG2_INSTANCE   *Instance
+  )
+{
+  IP4_SERVICE               *IpSb;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  if (Instance->Dhcp4 != NULL) {
+    Instance->Dhcp4->Stop (Instance->Dhcp4);
+
+    gBS->CloseProtocol (
+          Instance->Dhcp4Handle,
+          &gEfiDhcp4ProtocolGuid,
+          IpSb->Image,
+          IpSb->Controller
+          );
+
+    Instance->Dhcp4 = NULL;
+  }
+
+  if (Instance->Dhcp4Handle != NULL) {
+    NetLibDestroyServiceChild (
+      IpSb->Controller,
+      IpSb->Image,
+      &gEfiDhcp4ServiceBindingProtocolGuid,
+      Instance->Dhcp4Handle
+      );
+
+    Instance->Dhcp4Handle = NULL;
+  }
+
+  if (Instance->Dhcp4Event != NULL) {
+    gBS->CloseEvent (Instance->Dhcp4Event);
+    Instance->Dhcp4Event = NULL;
+  }
+}
+
+/**
+  This worker function sets the DNS server list for the EFI IPv4 network
+  stack running on the communication device that this EFI_IP4_CONFIG2_PROTOCOL
+  manages. The DNS server addresses must be unicast IPv4 addresses. 
+
+  @param[in]     Instance        The pointer to the IP4 config2 instance data.
+  @param[in]     DataSize        The size of the buffer pointed to by Data in bytes.
+  @param[in]     Data            The data buffer to set, points to an array of
+                                 EFI_IPv4_ADDRESS instances.
+
+  @retval EFI_BAD_BUFFER_SIZE    The DataSize does not match the size of the type.
+  @retval EFI_INVALID_PARAMETER  One or more fields in Data is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources to complete the operation.
+  @retval EFI_ABORTED            The DNS server addresses to be set equal the current
+                                 configuration.
+  @retval EFI_SUCCESS            The specified configuration data for the EFI IPv4
+                                 network stack was set.
+
+**/
+EFI_STATUS
+Ip4Config2SetDnsServerWorker (
+  IN IP4_CONFIG2_INSTANCE    *Instance,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data
+  )
+{
+  UINTN                 OldIndex;
+  UINTN                 NewIndex;
+  UINTN                 Index1;
+  EFI_IPv4_ADDRESS      *OldDns;
+  EFI_IPv4_ADDRESS      *NewDns;
+  UINTN                 OldDnsCount;
+  UINTN                 NewDnsCount;
+  IP4_CONFIG2_DATA_ITEM *Item;
+  BOOLEAN               OneAdded;
+  VOID                  *Tmp;
+  IP4_ADDR              DnsAddress;
+
+  if ((DataSize % sizeof (EFI_IPv4_ADDRESS) != 0) || (DataSize == 0)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  Item        = &Instance->DataItem[Ip4Config2DataTypeDnsServer];
+  NewDns      = (EFI_IPv4_ADDRESS *) Data;
+  OldDns      = Item->Data.DnsServers;
+  NewDnsCount = DataSize / sizeof (EFI_IPv4_ADDRESS);  
+  OldDnsCount = Item->DataSize / sizeof (EFI_IPv4_ADDRESS);
+  OneAdded    = FALSE;
+
+  if (NewDnsCount != OldDnsCount) {
+    Tmp = AllocatePool (DataSize);
+    if (Tmp == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else {
+    Tmp = NULL;
+  }
+
+  for (NewIndex = 0; NewIndex < NewDnsCount; NewIndex++) {
+    CopyMem (&DnsAddress, NewDns + NewIndex, sizeof (IP4_ADDR));
+
+    if (!NetIp4IsUnicast (NTOHL (DnsAddress), 0)) {
+      //
+      // The dns server address must be unicast.
+      //
+      FreePool (Tmp);
+      return EFI_INVALID_PARAMETER;
+    }
+
+    for (Index1 = NewIndex + 1; Index1 < NewDnsCount; Index1++) {
+      if (EFI_IP4_EQUAL (NewDns + NewIndex, NewDns + Index1)) {
+        FreePool (Tmp);
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+
+    if (OneAdded) {
+      //
+      // If any address in the new setting is not in the old settings, skip the
+      // comparision below.
+      //
+      continue;
+    }
+
+    for (OldIndex = 0; OldIndex < OldDnsCount; OldIndex++) {
+      if (EFI_IP4_EQUAL (NewDns + NewIndex, OldDns + OldIndex)) {
+        //
+        // If found break out.
+        //
+        break;
+      }
+    }
+
+    if (OldIndex == OldDnsCount) {
+      OneAdded = TRUE;
+    }
+  }
+
+  if (!OneAdded && (DataSize == Item->DataSize)) {
+    //
+    // No new item is added and the size is the same.
+    //
+    Item->Status = EFI_SUCCESS;
+    return EFI_ABORTED;
+  } else {
+    if (Tmp != NULL) {
+      if (Item->Data.Ptr != NULL) {
+        FreePool (Item->Data.Ptr);
+      }      
+      Item->Data.Ptr = Tmp;
+    }
+
+    CopyMem (Item->Data.Ptr, Data, DataSize);
+    Item->DataSize = DataSize;
+    Item->Status   = EFI_SUCCESS;
+    return EFI_SUCCESS;
+  }
+}
+
+
+
+/**
+  Callback function when DHCP process finished. It will save the
+  retrieved IP configure parameter from DHCP to the NVRam.
+
+  @param  Event                  The callback event
+  @param  Context                Opaque context to the callback
+
+  @return None
+
+**/
+VOID
+EFIAPI
+Ip4Config2OnDhcp4Complete (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  IP4_CONFIG2_INSTANCE      *Instance;
+  EFI_DHCP4_MODE_DATA       Dhcp4Mode;
+  EFI_STATUS                Status;
+  IP4_ADDR                  StationAddress;
+  IP4_ADDR                  SubnetMask;
+  IP4_ADDR                  GatewayAddress;
+  UINT32                    Index;
+  UINT32                    OptionCount;
+  EFI_DHCP4_PACKET_OPTION   **OptionList;
+
+  Instance = (IP4_CONFIG2_INSTANCE *) Context;
+  ASSERT (Instance->Dhcp4 != NULL);
+
+  //
+  // Get the DHCP retrieved parameters
+  //
+  Status = Instance->Dhcp4->GetModeData (Instance->Dhcp4, &Dhcp4Mode);
+
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  if (Dhcp4Mode.State == Dhcp4Bound) {
+    StationAddress = EFI_NTOHL (Dhcp4Mode.ClientAddress);
+    SubnetMask = EFI_NTOHL (Dhcp4Mode.SubnetMask);
+    GatewayAddress = EFI_NTOHL (Dhcp4Mode.RouterAddress);
+
+    Status = Ip4Config2SetDefaultIf (Instance, StationAddress, SubnetMask, GatewayAddress);
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  
+    //
+    // Parse the ACK to get required DNS server information.
+    //
+    OptionCount = 0;
+    OptionList  = NULL;
+
+    Status      = Instance->Dhcp4->Parse (Instance->Dhcp4, Dhcp4Mode.ReplyPacket, &OptionCount, OptionList);
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      goto Exit;
+    }
+
+    OptionList = AllocateZeroPool (OptionCount * sizeof (EFI_DHCP4_PACKET_OPTION *));
+    if (OptionList == NULL) {
+      goto Exit;
+    }
+
+    Status = Instance->Dhcp4->Parse (Instance->Dhcp4, Dhcp4Mode.ReplyPacket, &OptionCount, OptionList);
+    if (EFI_ERROR (Status)) {
+      FreePool (OptionList);
+      goto Exit;
+    }
+
+    for (Index = 0; Index < OptionCount; Index++) {
+      //
+      // Look for DNS Server opcode (6).
+      //
+      if (OptionList[Index]->OpCode == DHCP_TAG_DNS_SERVER) {
+        if (((OptionList[Index]->Length & 0x3) != 0) || (OptionList[Index]->Length == 0)) {
+          break;
+        }
+
+        Ip4Config2SetDnsServerWorker (Instance, OptionList[Index]->Length, &OptionList[Index]->Data[0]);
+        break;
+      }
+    }
+
+    FreePool (OptionList);
+
+    Instance->DhcpSuccess = TRUE;
+  }
+
+Exit:
+  Ip4Config2CleanDhcp4 (Instance);
+  DispatchDpc ();
+}
+
+
+/**
+  Start the DHCP configuration for this IP service instance.
+  It will locates the EFI_IP4_CONFIG2_PROTOCOL, then start the
+  DHCP configuration.
+
+  @param[in]  Instance           The IP4 config2 instance to configure
+
+  @retval EFI_SUCCESS            The auto configuration is successfull started
+  @retval Others                 Failed to start auto configuration.
+
+**/
+EFI_STATUS
+Ip4StartAutoConfig (
+  IN IP4_CONFIG2_INSTANCE   *Instance
+  )
+{
+  IP4_SERVICE                    *IpSb;
+  EFI_DHCP4_PROTOCOL             *Dhcp4;
+  EFI_DHCP4_MODE_DATA            Dhcp4Mode;
+  EFI_DHCP4_PACKET_OPTION        *OptionList[1];
+  IP4_CONFIG2_DHCP4_OPTION       ParaList;
+  EFI_STATUS                     Status;
+ 
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  if (IpSb->State > IP4_SERVICE_UNSTARTED) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // A host must not invoke DHCP configuration if it is already
+  // participating in the DHCP configuraiton process.
+  //
+  if (Instance->Dhcp4Handle != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = NetLibCreateServiceChild (
+             IpSb->Controller,
+             IpSb->Image,
+             &gEfiDhcp4ServiceBindingProtocolGuid,
+             &Instance->Dhcp4Handle
+             );
+
+  if (Status == EFI_UNSUPPORTED) {
+    //
+    // No DHCPv4 Service Binding protocol, register a notify.
+    //
+    if (Instance->Dhcp4SbNotifyEvent == NULL) {
+      Instance->Dhcp4SbNotifyEvent = EfiCreateProtocolNotifyEvent (
+                                       &gEfiDhcp4ServiceBindingProtocolGuid,
+                                       TPL_CALLBACK,
+                                       Ip4Config2OnDhcp4SbInstalled,
+                                       (VOID *) Instance,
+                                       &Instance->Registration
+                                       );
+    }
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Instance->Dhcp4SbNotifyEvent != NULL) {
+    gBS->CloseEvent (Instance->Dhcp4SbNotifyEvent);
+  }
+
+  Status = gBS->OpenProtocol (
+                  Instance->Dhcp4Handle,
+                  &gEfiDhcp4ProtocolGuid,
+                  (VOID **) &Instance->Dhcp4,
+                  IpSb->Image,
+                  IpSb->Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+
+  //
+  // Check the current DHCP status, if the DHCP process has
+  // already finished, return now.
+  //
+  Dhcp4  = Instance->Dhcp4;
+  Status = Dhcp4->GetModeData (Dhcp4, &Dhcp4Mode);
+
+  if (Dhcp4Mode.State == Dhcp4Bound) {
+    Ip4Config2OnDhcp4Complete (NULL, Instance);
+    return EFI_SUCCESS;
+
+  }
+
+  //
+  // Try to start the DHCP process. Use most of the current
+  // DHCP configuration to avoid problems if some DHCP client
+  // yields the control of this DHCP service to us.
+  //
+  ParaList.Head.OpCode             = DHCP_TAG_PARA_LIST;
+  ParaList.Head.Length             = 3;
+  ParaList.Head.Data[0]            = DHCP_TAG_NETMASK;
+  ParaList.Route                   = DHCP_TAG_ROUTER;
+  ParaList.Dns                     = DHCP_TAG_DNS_SERVER;
+  OptionList[0]                    = &ParaList.Head;
+  Dhcp4Mode.ConfigData.OptionCount = 1;
+  Dhcp4Mode.ConfigData.OptionList  = OptionList;
+
+  Status = Dhcp4->Configure (Dhcp4, &Dhcp4Mode.ConfigData);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  //
+  // Start the DHCP process
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  Ip4Config2OnDhcp4Complete,
+                  Instance,
+                  &Instance->Dhcp4Event
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = Dhcp4->Start (Dhcp4, Instance->Dhcp4Event);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+ 
+  IpSb->State     = IP4_SERVICE_STARTED;
+  DispatchDpc ();
+  return EFI_SUCCESS;
+
+}
+
+
+
+/**
+  The work function is to get the interface information of the communication 
+  device this IP4_CONFIG2_INSTANCE manages.
+
+  @param[in]      Instance Pointer to the IP4 config2 instance data.
+  @param[in, out] DataSize On input, in bytes, the size of Data. On output, in
+                           bytes, the size of buffer required to store the specified
+                           configuration data.
+  @param[in]      Data     The data buffer in which the configuration data is returned.
+                           Ignored if DataSize is ZERO.
+
+  @retval EFI_BUFFER_TOO_SMALL The size of Data is too small for the specified
+                               configuration data, and the required size is
+                               returned in DataSize.
+  @retval EFI_SUCCESS          The specified configuration data was obtained.
+
+**/
+EFI_STATUS
+Ip4Config2GetIfInfo (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN OUT UINTN            *DataSize,
+  IN VOID                 *Data      OPTIONAL
+  )
+{
+
+  IP4_SERVICE                    *IpSb;
+  UINTN                          Length;
+  IP4_CONFIG2_DATA_ITEM          *Item;
+  EFI_IP4_CONFIG2_INTERFACE_INFO *IfInfo;
+  IP4_ADDR                       Address;
+
+  IpSb   = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  Length = sizeof (EFI_IP4_CONFIG2_INTERFACE_INFO);
+
+  if (IpSb->DefaultRouteTable != NULL) {
+    Length += IpSb->DefaultRouteTable->TotalNum * sizeof (EFI_IP4_ROUTE_TABLE);
+  }
+  
+  if (*DataSize < Length) {
+    *DataSize = Length;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Copy the fixed size part of the interface info.
+  //
+  Item = &Instance->DataItem[Ip4Config2DataTypeInterfaceInfo];
+  IfInfo = (EFI_IP4_CONFIG2_INTERFACE_INFO *) Data;
+  CopyMem (IfInfo, Item->Data.Ptr, sizeof (EFI_IP4_CONFIG2_INTERFACE_INFO));
+
+  //
+  // Update the address info.
+  //
+  if (IpSb->DefaultInterface != NULL) {
+    Address = HTONL (IpSb->DefaultInterface->Ip);
+    CopyMem (&IfInfo->StationAddress, &Address, sizeof (EFI_IPv4_ADDRESS));
+    Address = HTONL (IpSb->DefaultInterface->SubnetMask);
+    CopyMem (&IfInfo->SubnetMask, &Address, sizeof (EFI_IPv4_ADDRESS));
+  }
+
+  if (IpSb->DefaultRouteTable != NULL) {
+    IfInfo->RouteTableSize = IpSb->DefaultRouteTable->TotalNum;
+    IfInfo->RouteTable   = (EFI_IP4_ROUTE_TABLE *) ((UINT8 *) Data + sizeof (EFI_IP4_CONFIG2_INTERFACE_INFO));
+
+    Ip4Config2BuildDefaultRouteTable (IpSb, IfInfo->RouteTable);  
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  The work function is to set the general configuration policy for the EFI IPv4 network 
+  stack that is running on the communication device managed by this IP4_CONFIG2_INSTANCE.
+  The policy will affect other configuration settings.
+
+  @param[in]     Instance Pointer to the IP4 config2 instance data.
+  @param[in]     DataSize Size of the buffer pointed to by Data in bytes.
+  @param[in]     Data     The data buffer to set.
+
+  @retval EFI_INVALID_PARAMETER The to be set policy is invalid.
+  @retval EFI_BAD_BUFFER_SIZE   The DataSize does not match the size of the type.
+  @retval EFI_ABORTED           The new policy equals the current policy.
+  @retval EFI_SUCCESS           The specified configuration data for the EFI IPv6
+                                network stack was set.
+
+**/
+EFI_STATUS
+Ip4Config2SetPolicy (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN UINTN                DataSize,
+  IN VOID                 *Data
+  )
+{
+  EFI_IP4_CONFIG2_POLICY NewPolicy;
+  IP4_CONFIG2_DATA_ITEM  *DataItem;
+  IP4_SERVICE            *IpSb;
+
+  if (DataSize != sizeof (EFI_IP4_CONFIG2_POLICY)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  NewPolicy = *((EFI_IP4_CONFIG2_POLICY *) Data);
+
+  if (NewPolicy >= Ip4Config2PolicyMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NewPolicy == Instance->Policy) {
+     return EFI_ABORTED;
+  } else {
+    if (NewPolicy == Ip4Config2PolicyDhcp) {
+      //
+      // The policy is changed from static to dhcp:
+      // Clean the ManualAddress, Gateway and DnsServers, shrink the variable
+      // data size, and fire up all the related events.
+      //
+      DataItem           = &Instance->DataItem[Ip4Config2DataTypeManualAddress];
+      if (DataItem->Data.Ptr != NULL) {
+        FreePool (DataItem->Data.Ptr);
+      }
+      DataItem->Data.Ptr = NULL;
+      DataItem->DataSize = 0;
+      DataItem->Status   = EFI_NOT_FOUND;
+      NetMapIterate (&DataItem->EventMap, Ip4Config2SignalEvent, NULL);
+
+      DataItem           = &Instance->DataItem[Ip4Config2DataTypeGateway];
+      if (DataItem->Data.Ptr != NULL) {
+        FreePool (DataItem->Data.Ptr);
+      }
+      DataItem->Data.Ptr = NULL;
+      DataItem->DataSize = 0;
+      DataItem->Status   = EFI_NOT_FOUND;
+      NetMapIterate (&DataItem->EventMap, Ip4Config2SignalEvent, NULL);
+
+      DataItem           = &Instance->DataItem[Ip4Config2DataTypeDnsServer];
+      if (DataItem->Data.Ptr != NULL) {
+        FreePool (DataItem->Data.Ptr);
+      }
+      DataItem->Data.Ptr = NULL;
+      DataItem->DataSize = 0;
+      DataItem->Status   = EFI_NOT_FOUND;
+      NetMapIterate (&DataItem->EventMap, Ip4Config2SignalEvent, NULL);
+    } else {
+      //
+      // The policy is changed from dhcp to static. Stop the DHCPv4 process
+      // and destroy the DHCPv4 child.
+      //
+      if (Instance->Dhcp4Handle != NULL) {
+        Ip4Config2DestroyDhcp4 (Instance);
+      }
+      
+      //
+      // Close the event.
+      //
+      if (Instance->Dhcp4Event != NULL) {
+        gBS->CloseEvent (Instance->Dhcp4Event);
+      }
+    }
+  }
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  Ip4Config2OnPolicyChanged (IpSb, NewPolicy);
+
+  Instance->Policy = NewPolicy;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The work function is to set the station addresses manually for the EFI IPv4 
+  network stack. It is only configurable when the policy is Ip4Config2PolicyStatic.
+
+  @param[in]     Instance Pointer to the IP4 config2 instance data.
+  @param[in]     DataSize Size of the buffer pointed to by Data in bytes.
+  @param[in]     Data     The data buffer to set.
+
+  @retval EFI_BAD_BUFFER_SIZE   The DataSize does not match the size of the type.
+  @retval EFI_WRITE_PROTECTED   The specified configuration data cannot be set
+                                under the current policy.
+  @retval EFI_INVALID_PARAMETER One or more fields in Data is invalid.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate resource to complete the operation.
+  @retval EFI_NOT_READY         An asynchrous process is invoked to set the specified
+                                configuration data, and the process is not finished.
+  @retval EFI_ABORTED           The manual addresses to be set equal current
+                                configuration.
+  @retval EFI_SUCCESS           The specified configuration data for the EFI IPv6
+                                network stack was set.
+
+**/
+EFI_STATUS
+Ip4Config2SetMaunualAddress (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN UINTN                DataSize,
+  IN VOID                 *Data
+  )
+{
+  EFI_IP4_CONFIG2_MANUAL_ADDRESS NewAddress;
+  IP4_CONFIG2_DATA_ITEM          *DataItem;
+  EFI_STATUS                     Status;
+  IP4_ADDR                       StationAddress;
+  IP4_ADDR                       SubnetMask;
+  VOID                           *Ptr;
+  IP4_SERVICE                    *IpSb;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  ASSERT (Instance->DataItem[Ip4Config2DataTypeManualAddress].Status != EFI_NOT_READY);
+
+  if (((DataSize % sizeof (EFI_IP4_CONFIG2_MANUAL_ADDRESS)) != 0) || (DataSize == 0)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (Instance->Policy != Ip4Config2PolicyStatic) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  NewAddress = *((EFI_IP4_CONFIG2_MANUAL_ADDRESS *) Data);
+
+  //
+  // Store the new data, and init the DataItem status to EFI_NOT_READY because
+  // we may have an asynchronous configuration process.
+  //
+  Ptr = AllocateCopyPool (DataSize, Data);
+  if (Ptr == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DataItem = &Instance->DataItem[Ip4Config2DataTypeManualAddress];
+  if (DataItem->Data.Ptr != NULL) {
+    FreePool (DataItem->Data.Ptr);
+  }
+  
+  DataItem->Data.Ptr = Ptr;
+  DataItem->DataSize = DataSize;
+  DataItem->Status   = EFI_NOT_READY;
+
+  StationAddress = EFI_NTOHL (NewAddress.Address);
+  SubnetMask = EFI_NTOHL (NewAddress.SubnetMask);
+
+  IpSb->Reconfig = TRUE;
+  Status = Ip4Config2SetDefaultAddr (IpSb, StationAddress, SubnetMask);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }  
+
+  DataItem->Status = EFI_SUCCESS;   
+
+ON_EXIT:
+  if (EFI_ERROR (DataItem->Status)) {
+    if (Ptr != NULL) {
+      FreePool (Ptr);
+    }
+    DataItem->Data.Ptr = NULL; 
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  The work function is to set the gateway addresses manually for the EFI IPv4 
+  network stack that is running on the communication device that this EFI IPv4 
+  Configuration Protocol manages. It is not configurable when the policy is
+  Ip4Config2PolicyDhcp. The gateway addresses must be unicast IPv4 addresses.
+
+  @param[in]     Instance The pointer to the IP4 config2 instance data.
+  @param[in]     DataSize The size of the buffer pointed to by Data in bytes.
+  @param[in]     Data     The data buffer to set. This points to an array of
+                          EFI_IPv6_ADDRESS instances.
+
+  @retval EFI_BAD_BUFFER_SIZE   The DataSize does not match the size of the type.
+  @retval EFI_WRITE_PROTECTED   The specified configuration data cannot be set
+                                under the current policy.
+  @retval EFI_INVALID_PARAMETER One or more fields in Data is invalid.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to complete the operation.
+  @retval EFI_ABORTED           The manual gateway addresses to be set equal the
+                                current configuration.
+  @retval EFI_SUCCESS           The specified configuration data for the EFI IPv6
+                                network stack was set.
+
+**/
+EFI_STATUS
+Ip4Config2SetGateway (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN UINTN                DataSize,
+  IN VOID                 *Data
+  )
+{
+  IP4_SERVICE                    *IpSb;
+  IP4_CONFIG2_DATA_ITEM          *DataItem;
+  IP4_ADDR                       Gateway;
+
+  UINTN                 Index1;
+  UINTN                 Index2;
+  EFI_IPv4_ADDRESS      *OldGateway;
+  EFI_IPv4_ADDRESS      *NewGateway;
+  UINTN                 OldGatewayCount;
+  UINTN                 NewGatewayCount;
+  BOOLEAN               OneRemoved;
+  BOOLEAN               OneAdded;
+  VOID                  *Tmp;
+
+  if ((DataSize % sizeof (EFI_IPv4_ADDRESS) != 0) || (DataSize == 0)) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  if (Instance->Policy != Ip4Config2PolicyStatic) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+
+  NewGateway      = (EFI_IPv4_ADDRESS *) Data;
+  NewGatewayCount = DataSize / sizeof (EFI_IPv4_ADDRESS);
+  for (Index1 = 0; Index1 < NewGatewayCount; Index1++) {
+    CopyMem (&Gateway, NewGateway + Index1, sizeof (IP4_ADDR));
+    
+    if (!NetIp4IsUnicast (NTOHL (Gateway), 0)) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+
+    for (Index2 = Index1 + 1; Index2 < NewGatewayCount; Index2++) {
+      if (EFI_IP4_EQUAL (NewGateway + Index1, NewGateway + Index2)) {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+  }
+ 
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  DataItem = &Instance->DataItem[Ip4Config2DataTypeGateway];
+  OldGateway      = DataItem->Data.Gateway;
+  OldGatewayCount = DataItem->DataSize / sizeof (EFI_IPv4_ADDRESS);
+  OneRemoved      = FALSE;
+  OneAdded        = FALSE;
+
+  if (NewGatewayCount != OldGatewayCount) {
+    Tmp = AllocatePool (DataSize);
+    if (Tmp == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else {
+    Tmp = NULL;
+  }
+
+  for (Index1 = 0; Index1 < OldGatewayCount; Index1++) {
+    //
+    // Remove this route entry.
+    //
+    CopyMem (&Gateway, OldGateway + Index1, sizeof (IP4_ADDR));
+    Ip4DelRoute (
+      IpSb->DefaultRouteTable,
+      IP4_ALLZERO_ADDRESS,
+      IP4_ALLZERO_ADDRESS,
+      NTOHL (Gateway)
+      );
+    OneRemoved = TRUE;
+
+  }
+
+  for (Index1 = 0; Index1 < NewGatewayCount; Index1++) {
+    CopyMem (&Gateway, NewGateway + Index1, sizeof (IP4_ADDR));
+    Ip4AddRoute (
+      IpSb->DefaultRouteTable,
+      IP4_ALLZERO_ADDRESS,
+      IP4_ALLZERO_ADDRESS,
+      NTOHL (Gateway)
+      );    
+
+    OneAdded = TRUE;
+  }
+
+
+  if (!OneRemoved && !OneAdded) {
+    DataItem->Status = EFI_SUCCESS;
+    return EFI_ABORTED;
+  } else {
+
+    if (Tmp != NULL) {
+      if (DataItem->Data.Ptr != NULL) {
+        FreePool (DataItem->Data.Ptr);
+      }
+      DataItem->Data.Ptr = Tmp;
+    }
+
+    CopyMem (DataItem->Data.Ptr, Data, DataSize);
+    DataItem->DataSize = DataSize;
+    DataItem->Status   = EFI_SUCCESS;
+    return EFI_SUCCESS;
+  }
+
+}
+
+/**
+  The work function is to set the DNS server list for the EFI IPv4 network 
+  stack running on the communication device that this EFI_IP4_CONFIG2_PROTOCOL 
+  manages. It is not configurable when the policy is Ip4Config2PolicyDhcp. 
+  The DNS server addresses must be unicast IPv4 addresses.
+
+  @param[in]     Instance The pointer to the IP4 config2 instance data.
+  @param[in]     DataSize The size of the buffer pointed to by Data in bytes.
+  @param[in]     Data     The data buffer to set, points to an array of
+                          EFI_IPv4_ADDRESS instances.
+
+  @retval EFI_BAD_BUFFER_SIZE   The DataSize does not match the size of the type.
+  @retval EFI_WRITE_PROTECTED   The specified configuration data cannot be set
+                                under the current policy.
+  @retval EFI_INVALID_PARAMETER One or more fields in Data is invalid.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources to complete the operation.
+  @retval EFI_ABORTED           The DNS server addresses to be set equal the current
+                                configuration.
+  @retval EFI_SUCCESS           The specified configuration data for the EFI IPv4
+                                network stack was set.
+
+**/
+EFI_STATUS
+Ip4Config2SetDnsServer (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN UINTN                DataSize,
+  IN VOID                 *Data
+  )
+{
+  if (Instance->Policy != Ip4Config2PolicyStatic) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  return Ip4Config2SetDnsServerWorker (Instance, DataSize, Data);
+}
+
+/**
+  Generate the operational state of the interface this IP4 config2 instance manages
+  and output in EFI_IP4_CONFIG2_INTERFACE_INFO.
+
+  @param[in]      IpSb     The pointer to the IP4 service binding instance.
+  @param[out]     IfInfo   The pointer to the IP4 config2 interface information structure.
+
+**/
+VOID
+Ip4Config2InitIfInfo (
+  IN  IP4_SERVICE                    *IpSb,
+  OUT EFI_IP4_CONFIG2_INTERFACE_INFO *IfInfo
+  )
+{
+  IfInfo->Name[0] = L'e';
+  IfInfo->Name[1] = L't';
+  IfInfo->Name[2] = L'h';
+  IfInfo->Name[3] = (CHAR16) (L'0' + IpSb->Ip4Config2Instance.IfIndex);
+  IfInfo->Name[4] = 0;
+
+  IfInfo->IfType        = IpSb->SnpMode.IfType;
+  IfInfo->HwAddressSize = IpSb->SnpMode.HwAddressSize;
+  CopyMem (&IfInfo->HwAddress, &IpSb->SnpMode.CurrentAddress, IfInfo->HwAddressSize);
+}
+
+/**
+  The event handle routine when DHCPv4 process is finished or is updated.
+
+  @param[in]     Event         Not used.
+  @param[in]     Context       The pointer to the IP4 configuration instance data.
+
+**/
+VOID
+EFIAPI
+Ip4Config2OnDhcp4Event (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  return ;
+}
+
+
+/**
+  Set the configuration for the EFI IPv4 network stack running on the communication
+  device this EFI_IP4_CONFIG2_PROTOCOL instance manages.
+
+  This function is used to set the configuration data of type DataType for the EFI
+  IPv4 network stack that is running on the communication device that this EFI IPv4
+  Configuration Protocol instance manages.
+
+  DataSize is used to calculate the count of structure instances in the Data for
+  a DataType in which multiple structure instances are allowed.
+
+  This function is always non-blocking. When setting some type of configuration data,
+  an asynchronous process is invoked to check the correctness of the data, such as
+  performing Duplicate Address Detection on the manually set local IPv4 addresses.
+  EFI_NOT_READY is returned immediately to indicate that such an asynchronous process
+  is invoked, and the process is not finished yet. The caller wanting to get the result
+  of the asynchronous process is required to call RegisterDataNotify() to register an
+  event on the specified configuration data. Once the event is signaled, the caller
+  can call GetData() to obtain the configuration data and know the result.
+  For other types of configuration data that do not require an asynchronous configuration
+  process, the result of the operation is immediately returned.
+
+  @param[in]     This           The pointer to the EFI_IP4_CONFIG2_PROTOCOL instance.
+  @param[in]     DataType       The type of data to set.
+  @param[in]     DataSize       Size of the buffer pointed to by Data in bytes.
+  @param[in]     Data           The data buffer to set. The type of the data buffer is
+                                associated with the DataType.
+
+  @retval EFI_SUCCESS           The specified configuration data for the EFI IPv6
+                                network stack was set successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the following are TRUE:
+                                - This is NULL.
+                                - Data is NULL.
+                                - One or more fields in Data do not match the requirement of the
+                                  data type indicated by DataType.
+  @retval EFI_WRITE_PROTECTED   The specified configuration data is read-only or the specified
+                                configuration data cannot be set under the current policy.
+  @retval EFI_ACCESS_DENIED     Another set operation on the specified configuration
+                                data is already in process.
+  @retval EFI_NOT_READY         An asynchronous process was invoked to set the specified
+                                configuration data, and the process is not finished yet.
+  @retval EFI_BAD_BUFFER_SIZE   The DataSize does not match the size of the type
+                                indicated by DataType.
+  @retval EFI_UNSUPPORTED       This DataType is not supported.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_DEVICE_ERROR      An unexpected system error or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Config2SetData (
+  IN EFI_IP4_CONFIG2_PROTOCOL   *This,
+  IN EFI_IP4_CONFIG2_DATA_TYPE  DataType,
+  IN UINTN                      DataSize,
+  IN VOID                       *Data
+  )
+{
+  EFI_TPL              OldTpl;
+  EFI_STATUS           Status;
+  IP4_CONFIG2_INSTANCE *Instance;
+  IP4_SERVICE          *IpSb;
+
+  if ((This == NULL) || (Data == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataType >= Ip4Config2DataTypeMaximum) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = IP4_CONFIG2_INSTANCE_FROM_PROTOCOL (This);
+  IpSb     = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  NET_CHECK_SIGNATURE (IpSb, IP4_SERVICE_SIGNATURE);
+
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Status = Instance->DataItem[DataType].Status;
+  if (Status != EFI_NOT_READY) {
+
+    if (Instance->DataItem[DataType].SetData == NULL) {
+      //
+      // This type of data is readonly.
+      //
+      Status = EFI_WRITE_PROTECTED;
+    } else {
+
+      Status = Instance->DataItem[DataType].SetData (Instance, DataSize, Data);
+      if (!EFI_ERROR (Status)) {
+        //
+        // Fire up the events registered with this type of data.
+        //
+        NetMapIterate (&Instance->DataItem[DataType].EventMap, Ip4Config2SignalEvent, NULL);
+        Ip4Config2WriteConfigData (IpSb->MacString, Instance);
+      } else if (Status == EFI_ABORTED) {
+        //
+        // The SetData is aborted because the data to set is the same with
+        // the one maintained.
+        //
+        Status = EFI_SUCCESS;
+        NetMapIterate (&Instance->DataItem[DataType].EventMap, Ip4Config2SignalEvent, NULL);
+      }
+    }
+  } else {
+    //
+    // Another asynchornous process is on the way.
+    //
+    Status = EFI_ACCESS_DENIED;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Get the configuration data for the EFI IPv4 network stack running on the communication
+  device that this EFI_IP4_CONFIG2_PROTOCOL instance manages.
+
+  This function returns the configuration data of type DataType for the EFI IPv4 network
+  stack running on the communication device that this EFI IPv4 Configuration Protocol instance
+  manages.
+
+  The caller is responsible for allocating the buffer used to return the specified
+  configuration data. The required size will be returned to the caller if the size of
+  the buffer is too small.
+
+  EFI_NOT_READY is returned if the specified configuration data is not ready due to an
+  asynchronous configuration process already in progress. The caller can call RegisterDataNotify()
+  to register an event on the specified configuration data. Once the asynchronous configuration
+  process is finished, the event will be signaled, and a subsequent GetData() call will return
+  the specified configuration data.
+
+  @param[in]      This           Pointer to the EFI_IP4_CONFIG2_PROTOCOL instance.
+  @param[in]      DataType       The type of data to get.
+  @param[in, out] DataSize       On input, in bytes, the size of Data. On output, in bytes, the
+                                 size of buffer required to store the specified configuration data.
+  @param[in]     Data            The data buffer in which the configuration data is returned. The
+                                 type of the data buffer is associated with the DataType.
+                                 This is an optional parameter that may be NULL.
+
+  @retval EFI_SUCCESS           The specified configuration data was obtained successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the followings are TRUE:
+                                - This is NULL.
+                                - DataSize is NULL.
+                                - Data is NULL if *DataSize is not zero.
+  @retval EFI_BUFFER_TOO_SMALL  The size of Data is too small for the specified configuration data,
+                                and the required size is returned in DataSize.
+  @retval EFI_NOT_READY         The specified configuration data is not ready due to an
+                                asynchronous configuration process already in progress.
+  @retval EFI_NOT_FOUND         The specified configuration data is not found.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Config2GetData (
+  IN EFI_IP4_CONFIG2_PROTOCOL   *This,
+  IN EFI_IP4_CONFIG2_DATA_TYPE  DataType,
+  IN OUT UINTN                  *DataSize,
+  IN VOID                       *Data   OPTIONAL
+  )
+{
+  EFI_TPL               OldTpl;
+  EFI_STATUS            Status;
+  IP4_CONFIG2_INSTANCE  *Instance;
+  IP4_CONFIG2_DATA_ITEM *DataItem;
+
+  if ((This == NULL) || (DataSize == NULL) || ((*DataSize != 0) && (Data == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataType >= Ip4Config2DataTypeMaximum) {
+    return EFI_NOT_FOUND;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Instance = IP4_CONFIG2_INSTANCE_FROM_PROTOCOL (This);
+  DataItem = &Instance->DataItem[DataType];
+
+  Status   = Instance->DataItem[DataType].Status;
+  if (!EFI_ERROR (Status)) {
+
+    if (DataItem->GetData != NULL) {
+
+      Status = DataItem->GetData (Instance, DataSize, Data);
+    } else if (*DataSize < Instance->DataItem[DataType].DataSize) {
+      //
+      // Update the buffer length.
+      //
+      *DataSize = Instance->DataItem[DataType].DataSize;
+      Status    = EFI_BUFFER_TOO_SMALL;
+    } else {
+
+      *DataSize = Instance->DataItem[DataType].DataSize;
+      CopyMem (Data, Instance->DataItem[DataType].Data.Ptr, *DataSize);
+    }
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Register an event that is signaled whenever a configuration process on the specified
+  configuration data is done.
+
+  This function registers an event that is to be signaled whenever a configuration
+  process on the specified configuration data is performed. An event can be registered
+  for a different DataType simultaneously. The caller is responsible for determining
+  which type of configuration data causes the signaling of the event in such an event.
+
+  @param[in]     This           Pointer to the EFI_IP4_CONFIG2_PROTOCOL instance.
+  @param[in]     DataType       The type of data to unregister the event for.
+  @param[in]     Event          The event to register.
+
+  @retval EFI_SUCCESS           The notification event for the specified configuration data is
+                                registered.
+  @retval EFI_INVALID_PARAMETER This is NULL or Event is NULL.
+  @retval EFI_UNSUPPORTED       The configuration data type specified by DataType is not
+                                supported.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_ACCESS_DENIED     The Event is already registered for the DataType.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Config2RegisterDataNotify (
+  IN EFI_IP4_CONFIG2_PROTOCOL   *This,
+  IN EFI_IP4_CONFIG2_DATA_TYPE  DataType,
+  IN EFI_EVENT                  Event
+  )
+{
+  EFI_TPL              OldTpl;
+  EFI_STATUS           Status;
+  IP4_CONFIG2_INSTANCE *Instance;
+  NET_MAP              *EventMap;
+  NET_MAP_ITEM         *Item;
+
+  if ((This == NULL) || (Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataType >= Ip4Config2DataTypeMaximum) {
+    return EFI_UNSUPPORTED;
+  }
+
+  OldTpl    = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Instance  = IP4_CONFIG2_INSTANCE_FROM_PROTOCOL (This);
+  EventMap  = &Instance->DataItem[DataType].EventMap;
+
+  //
+  // Check whether this event is already registered for this DataType.
+  //
+  Item = NetMapFindKey (EventMap, Event);
+  if (Item == NULL) {
+
+    Status = NetMapInsertTail (EventMap, Event, NULL);
+
+    if (EFI_ERROR (Status)) {
+
+      Status = EFI_OUT_OF_RESOURCES;
+    }
+
+  } else {
+
+    Status = EFI_ACCESS_DENIED;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Remove a previously registered event for the specified configuration data.
+
+  @param  This                   The pointer to the EFI_IP4_CONFIG2_PROTOCOL instance.
+  @param  DataType               The type of data to remove from the previously
+                                 registered event.
+  @param  Event                  The event to be unregistered.
+
+  @retval EFI_SUCCESS            The event registered for the specified
+                                 configuration data was removed.
+  @retval EFI_INVALID_PARAMETER  This is NULL or Event is NULL.
+  @retval EFI_NOT_FOUND          The Event has not been registered for the
+                                 specified DataType.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Config2UnregisterDataNotify (
+  IN EFI_IP4_CONFIG2_PROTOCOL   *This,
+  IN EFI_IP4_CONFIG2_DATA_TYPE  DataType,
+  IN EFI_EVENT                  Event
+  )
+{
+  EFI_TPL              OldTpl;
+  EFI_STATUS           Status;
+  IP4_CONFIG2_INSTANCE *Instance;
+  NET_MAP_ITEM         *Item;
+
+  if ((This == NULL) || (Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataType >= Ip4Config2DataTypeMaximum) {
+    return EFI_NOT_FOUND;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Instance = IP4_CONFIG2_INSTANCE_FROM_PROTOCOL (This);
+
+  Item = NetMapFindKey (&Instance->DataItem[DataType].EventMap, Event);
+  if (Item != NULL) {
+
+    NetMapRemoveItem (&Instance->DataItem[DataType].EventMap, Item, NULL);
+    Status = EFI_SUCCESS;
+  } else {
+
+    Status = EFI_NOT_FOUND;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Initialize an IP4_CONFIG2_INSTANCE.
+
+  @param[out]    Instance       The buffer of IP4_CONFIG2_INSTANCE to be initialized.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources to complete the operation.
+  @retval EFI_SUCCESS           The IP4_CONFIG2_INSTANCE initialized successfully.
+
+**/
+EFI_STATUS
+Ip4Config2InitInstance (
+  OUT IP4_CONFIG2_INSTANCE  *Instance
+  )
+{
+  IP4_SERVICE           *IpSb;
+  IP4_CONFIG2_INSTANCE  *TmpInstance;
+  LIST_ENTRY            *Entry;
+  EFI_STATUS            Status;
+  UINTN                 Index;
+  UINT16                IfIndex;
+  IP4_CONFIG2_DATA_ITEM *DataItem;
+
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  Instance->Signature = IP4_CONFIG2_INSTANCE_SIGNATURE;
+
+
+  //
+  // Determine the index of this interface.
+  //
+  IfIndex = 0;
+  NET_LIST_FOR_EACH (Entry, &mIp4Config2InstanceList) {
+    TmpInstance = NET_LIST_USER_STRUCT_S (Entry, IP4_CONFIG2_INSTANCE, Link, IP4_CONFIG2_INSTANCE_SIGNATURE);
+
+    if (TmpInstance->IfIndex > IfIndex) {
+      //
+      // There is a sequence hole because some interface is down.
+      //
+      break;
+    }
+
+    IfIndex++;
+  }
+
+  Instance->IfIndex = IfIndex;
+  NetListInsertBefore (Entry, &Instance->Link);
+
+  for (Index = 0; Index < Ip4Config2DataTypeMaximum; Index++) {
+    //
+    // Initialize the event map for each data item.
+    //
+    NetMapInit (&Instance->DataItem[Index].EventMap);
+  }
+
+  
+  //
+  // Initialize each data type: associate storage and set data size for the
+  // fixed size data types, hook the SetData function, set the data attribute.
+  //
+  DataItem           = &Instance->DataItem[Ip4Config2DataTypeInterfaceInfo];
+  DataItem->GetData  = Ip4Config2GetIfInfo;
+  DataItem->Data.Ptr = &Instance->InterfaceInfo;
+  DataItem->DataSize = sizeof (Instance->InterfaceInfo);
+  SET_DATA_ATTRIB (DataItem->Attribute, DATA_ATTRIB_SIZE_FIXED | DATA_ATTRIB_VOLATILE);
+  Ip4Config2InitIfInfo (IpSb, &Instance->InterfaceInfo);
+
+  DataItem           = &Instance->DataItem[Ip4Config2DataTypePolicy];
+  DataItem->SetData  = Ip4Config2SetPolicy;
+  DataItem->Data.Ptr = &Instance->Policy;
+  DataItem->DataSize = sizeof (Instance->Policy);
+  Instance->Policy   = Ip4Config2PolicyDhcp;
+  SET_DATA_ATTRIB (DataItem->Attribute, DATA_ATTRIB_SIZE_FIXED);
+
+  DataItem           = &Instance->DataItem[Ip4Config2DataTypeManualAddress];
+  DataItem->SetData  = Ip4Config2SetMaunualAddress;
+  DataItem->Status   = EFI_NOT_FOUND;
+
+  DataItem           = &Instance->DataItem[Ip4Config2DataTypeGateway];
+  DataItem->SetData  = Ip4Config2SetGateway;
+  DataItem->Status   = EFI_NOT_FOUND;
+
+  DataItem           = &Instance->DataItem[Ip4Config2DataTypeDnsServer];
+  DataItem->SetData  = Ip4Config2SetDnsServer;
+  DataItem->Status   = EFI_NOT_FOUND;
+
+  //
+  // Create the event used for DHCP.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  Ip4Config2OnDhcp4Event,
+                  Instance,
+                  &Instance->Dhcp4Event
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Instance->Configured  = TRUE;
+
+  //
+  // Try to read the config data from NV variable.
+  //
+  Status = Ip4Config2ReadConfigData (IpSb->MacString, Instance);
+  if (Status == EFI_NOT_FOUND) {
+    Status = Ip4Config2WriteConfigData (IpSb->MacString, Instance);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Try to set the configured parameter.
+  //
+  for (Index = Ip4Config2DataTypePolicy; Index < Ip4Config2DataTypeMaximum; Index++) {
+    DataItem = &IpSb->Ip4Config2Instance.DataItem[Index];
+    if (DataItem->Data.Ptr != NULL) {
+      DataItem->SetData (
+                  &IpSb->Ip4Config2Instance,
+                  DataItem->DataSize,
+                  DataItem->Data.Ptr
+                  );
+    }
+  }
+
+  Instance->Ip4Config2.SetData              = EfiIp4Config2SetData;
+  Instance->Ip4Config2.GetData              = EfiIp4Config2GetData;
+  Instance->Ip4Config2.RegisterDataNotify   = EfiIp4Config2RegisterDataNotify;
+  Instance->Ip4Config2.UnregisterDataNotify = EfiIp4Config2UnregisterDataNotify;
+
+  //
+  // Publish the IP4 configuration form
+  //
+  return Ip4Config2FormInit (Instance);
+}
+
+
+/**
+  Release an IP4_CONFIG2_INSTANCE.
+
+  @param[in, out] Instance    The buffer of IP4_CONFIG2_INSTANCE to be freed.
+
+**/
+VOID
+Ip4Config2CleanInstance (
+  IN OUT IP4_CONFIG2_INSTANCE  *Instance
+  )
+{
+  UINTN                 Index;
+  IP4_CONFIG2_DATA_ITEM *DataItem;
+
+  if (Instance->DeclineAddress != NULL) {
+    FreePool (Instance->DeclineAddress);
+  }
+
+  if (!Instance->Configured) {
+    return ;
+  }
+
+  if (Instance->Dhcp4Handle != NULL) {
+
+    Ip4Config2DestroyDhcp4 (Instance);
+  }
+
+  //
+  // Close the event.
+  //
+  if (Instance->Dhcp4Event != NULL) {
+    gBS->CloseEvent (Instance->Dhcp4Event);
+  }
+
+  for (Index = 0; Index < Ip4Config2DataTypeMaximum; Index++) {
+
+    DataItem = &Instance->DataItem[Index];
+
+    if (!DATA_ATTRIB_SET (DataItem->Attribute, DATA_ATTRIB_SIZE_FIXED)) {
+      if (DataItem->Data.Ptr != NULL) {
+        FreePool (DataItem->Data.Ptr);
+      }
+      DataItem->Data.Ptr = NULL;
+      DataItem->DataSize = 0;
+    }
+
+    NetMapClean (&Instance->DataItem[Index].EventMap);
+  }
+
+  Ip4Config2FormUnload (Instance);
+
+  RemoveEntryList (&Instance->Link);
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.h
new file mode 100644
index 0000000000..ab72525646
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Impl.h
@@ -0,0 +1,270 @@
+/** @file
+  Definitions for EFI IPv4 Configuration II Protocol implementation.
+
+  Copyright (c) 2015, Intel Corporation. All rights reserved.<BR>
+  (C) Copyright 2015 Hewlett Packard Enterprise Development LP<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.
+
+**/
+
+#ifndef __IP4_CONFIG2_IMPL_H__
+#define __IP4_CONFIG2_IMPL_H__
+
+#define IP4_CONFIG2_INSTANCE_SIGNATURE    SIGNATURE_32 ('I', 'P', 'C', '2')
+#define IP4_FORM_CALLBACK_INFO_SIGNATURE  SIGNATURE_32 ('I', 'F', 'C', 'I')
+
+#define IP4_CONFIG2_VARIABLE_ATTRIBUTE    (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS)
+
+#define DATA_ATTRIB_SIZE_FIXED              0x1
+#define DATA_ATTRIB_VOLATILE                0x2
+
+#define DHCP_TAG_PARA_LIST             55
+#define DHCP_TAG_NETMASK               1
+#define DHCP_TAG_ROUTER                3
+#define DHCP_TAG_DNS_SERVER            6
+
+#define DATA_ATTRIB_SET(Attrib, Bits)       (BOOLEAN)((Attrib) & (Bits))
+#define SET_DATA_ATTRIB(Attrib, Bits)       ((Attrib) |= (Bits))
+
+typedef struct _IP4_CONFIG2_INSTANCE IP4_CONFIG2_INSTANCE;
+
+#define IP4_CONFIG2_INSTANCE_FROM_PROTOCOL(Proto) \
+  CR ((Proto), \
+      IP4_CONFIG2_INSTANCE, \
+      Ip4Config2, \
+      IP4_CONFIG2_INSTANCE_SIGNATURE \
+      )
+
+#define IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE(Instance) \
+  CR ((Instance), \
+      IP4_SERVICE, \
+      Ip4Config2Instance, \
+      IP4_SERVICE_SIGNATURE \
+      )
+
+#define IP4_CONFIG2_INSTANCE_FROM_FORM_CALLBACK(Callback) \
+  CR ((Callback), \
+      IP4_CONFIG2_INSTANCE, \
+      CallbackInfo, \
+      IP4_CONFIG2_INSTANCE_SIGNATURE \
+      )
+
+#define IP4_FORM_CALLBACK_INFO_FROM_CONFIG_ACCESS(ConfigAccess) \
+  CR ((ConfigAccess), \
+      IP4_FORM_CALLBACK_INFO, \
+      HiiConfigAccessProtocol, \
+      IP4_FORM_CALLBACK_INFO_SIGNATURE \
+      )
+
+/**
+  The prototype of work function for EfiIp4Config2SetData().
+
+  @param[in]     Instance The pointer to the IP4 config2 instance data.
+  @param[in]     DataSize In bytes, the size of the buffer pointed to by Data.
+  @param[in]     Data     The data buffer to set.
+
+  @retval EFI_BAD_BUFFER_SIZE  The DataSize does not match the size of the type,
+                               8 bytes.
+  @retval EFI_SUCCESS          The specified configuration data for the EFI IPv4
+                               network stack was set successfully.
+  
+**/
+typedef
+EFI_STATUS
+(*IP4_CONFIG2_SET_DATA) (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN UINTN                DataSize,
+  IN VOID                 *Data
+  );
+
+/**
+  The prototype of work function for EfiIp4Config2GetData().
+
+  @param[in]      Instance The pointer to the IP4 config2 instance data.
+  @param[in, out] DataSize On input, in bytes, the size of Data. On output, in
+                           bytes, the size of buffer required to store the specified
+                           configuration data.
+  @param[in]      Data     The data buffer in which the configuration data is returned.  
+                           Ignored if DataSize is ZERO.
+
+  @retval EFI_BUFFER_TOO_SMALL The size of Data is too small for the specified
+                               configuration data, and the required size is 
+                               returned in DataSize.
+  @retval EFI_SUCCESS          The specified configuration data was obtained successfully.                               
+  
+**/
+typedef
+EFI_STATUS
+(*IP4_CONFIG2_GET_DATA) (
+  IN IP4_CONFIG2_INSTANCE *Instance,
+  IN OUT UINTN            *DataSize,
+  IN VOID                 *Data      OPTIONAL
+  );
+
+typedef union {
+  VOID                                      *Ptr;
+  EFI_IP4_CONFIG2_INTERFACE_INFO            *IfInfo;
+  EFI_IP4_CONFIG2_POLICY                    *Policy;
+  EFI_IP4_CONFIG2_MANUAL_ADDRESS            *ManualAddress;
+  EFI_IPv4_ADDRESS                          *Gateway;
+  EFI_IPv4_ADDRESS                          *DnsServers;
+} IP4_CONFIG2_DATA;
+
+typedef struct {
+  IP4_CONFIG2_SET_DATA SetData;
+  IP4_CONFIG2_GET_DATA GetData;
+  EFI_STATUS           Status;
+  UINT8                Attribute;
+  NET_MAP              EventMap;
+  IP4_CONFIG2_DATA     Data;
+  UINTN                DataSize;
+} IP4_CONFIG2_DATA_ITEM;
+
+typedef struct {
+  UINT16                    Offset;
+  UINT32                    DataSize;
+  EFI_IP4_CONFIG2_DATA_TYPE DataType;
+} IP4_CONFIG2_DATA_RECORD;
+
+#pragma pack(1)
+
+//
+// heap data that contains the data for each data record.
+//
+//  EFI_IP4_CONFIG2_POLICY                    Policy;
+//  UINT32                                    ManualaddressCount;
+//  UINT32                                    GatewayCount;
+//  UINT32                                    DnsServersCount;
+//  EFI_IP4_CONFIG2_MANUAL_ADDRESS            ManualAddress[];
+//  EFI_IPv4_ADDRESS                          Gateway[];
+//  EFI_IPv4_ADDRESS                          DnsServers[];
+//
+typedef struct {
+  UINT16                  Checksum;
+  UINT16                  DataRecordCount;
+  IP4_CONFIG2_DATA_RECORD DataRecord[1];
+} IP4_CONFIG2_VARIABLE;
+
+#pragma pack()
+
+typedef struct {
+  EFI_IP4_CONFIG2_POLICY                   Policy;               ///< manual or automatic  
+  EFI_IP4_CONFIG2_MANUAL_ADDRESS           *ManualAddress;       ///< IP addresses
+  UINT32                                   ManualAddressCount;   ///< IP addresses count
+  EFI_IPv4_ADDRESS                         *GatewayAddress;      ///< Gateway address
+  UINT32                                   GatewayAddressCount;  ///< Gateway address count
+  EFI_IPv4_ADDRESS                         *DnsAddress;          ///< DNS server address
+  UINT32                                   DnsAddressCount;      ///< DNS server address count
+} IP4_CONFIG2_NVDATA;
+
+typedef struct _IP4_FORM_CALLBACK_INFO {
+  UINT32                           Signature;
+  EFI_HANDLE                       ChildHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL   HiiConfigAccessProtocol;
+  EFI_DEVICE_PATH_PROTOCOL         *HiiVendorDevicePath;
+  EFI_HII_HANDLE                   RegisteredHandle;
+} IP4_FORM_CALLBACK_INFO;
+
+struct _IP4_CONFIG2_INSTANCE {
+  UINT32                                    Signature;
+  BOOLEAN                                   Configured;
+  LIST_ENTRY                                Link;
+  UINT16                                    IfIndex;
+
+  EFI_IP4_CONFIG2_PROTOCOL                  Ip4Config2;
+
+  EFI_IP4_CONFIG2_INTERFACE_INFO            InterfaceInfo;
+  EFI_IP4_CONFIG2_POLICY                    Policy;  
+  IP4_CONFIG2_DATA_ITEM                     DataItem[Ip4Config2DataTypeMaximum];
+
+  EFI_EVENT                                 Dhcp4SbNotifyEvent;
+  VOID                                      *Registration;
+  EFI_HANDLE                                Dhcp4Handle;
+  EFI_DHCP4_PROTOCOL                        *Dhcp4;
+  BOOLEAN                                   DhcpSuccess;
+  BOOLEAN                                   OtherInfoOnly;
+  EFI_EVENT                                 Dhcp4Event;
+  UINT32                                    FailedIaAddressCount;
+  EFI_IPv4_ADDRESS                          *DeclineAddress;
+  UINT32                                    DeclineAddressCount;
+  
+  IP4_FORM_CALLBACK_INFO                    CallbackInfo;
+
+  IP4_CONFIG2_NVDATA                        Ip4NvData;
+};
+
+//
+// Configure the DHCP to request the routers and netmask
+// from server. The DHCP_TAG_NETMASK is included in Head.
+//
+#pragma pack(1)
+typedef struct {
+  EFI_DHCP4_PACKET_OPTION Head;
+  UINT8                   Route;
+  UINT8                   Dns;
+} IP4_CONFIG2_DHCP4_OPTION;
+#pragma pack()
+
+/**
+  Start the DHCP configuration for this IP service instance.
+  It will locates the EFI_IP4_CONFIG2_PROTOCOL, then start the
+  DHCP configuration.
+
+  @param[in]  Instance           The IP4 config2 instance to configure.
+
+  @retval EFI_SUCCESS            The auto configuration is successfull started.
+  @retval Others                 Failed to start auto configuration.
+
+**/
+EFI_STATUS
+Ip4StartAutoConfig (
+  IN IP4_CONFIG2_INSTANCE   *Instance
+  );
+
+/**
+  Initialize an IP4_CONFIG2_INSTANCE.
+
+  @param[out]    Instance       The buffer of IP4_CONFIG2_INSTANCE to be initialized.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resources to complete the operation.
+  @retval EFI_SUCCESS           The IP4_CONFIG2_INSTANCE initialized successfully.
+
+**/
+EFI_STATUS
+Ip4Config2InitInstance (
+  OUT IP4_CONFIG2_INSTANCE  *Instance
+  );
+
+/**
+  Release an IP4_CONFIG2_INSTANCE.
+
+  @param[in, out] Instance    The buffer of IP4_CONFIG2_INSTANCE to be freed.
+
+**/
+VOID
+Ip4Config2CleanInstance (
+  IN OUT IP4_CONFIG2_INSTANCE  *Instance
+  );
+
+/**
+  Destroy the Dhcp4 child in IP4_CONFIG2_INSTANCE and release the resources.
+
+  @param[in, out] Instance    The buffer of IP4 config2 instance to be freed.
+
+  @retval EFI_SUCCESS         The child was successfully destroyed.
+  @retval Others              Failed to destroy the child.
+
+**/
+EFI_STATUS
+Ip4Config2DestroyDhcp4 (
+  IN OUT IP4_CONFIG2_INSTANCE  *Instance
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.c
new file mode 100644
index 0000000000..381dcb1e97
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.c
@@ -0,0 +1,1436 @@
+/** @file
+  Helper functions for configuring or getting the parameters relating to Ip4.
+
+Copyright (c) 2015, 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 "Ip4Impl.h"
+
+CHAR16    mIp4Config2StorageName[]     = L"IP4_CONFIG2_IFR_NVDATA";
+
+/**
+  Calculate the prefix length of the IPv4 subnet mask.
+
+  @param[in]  SubnetMask The IPv4 subnet mask.
+
+  @return The prefix length of the subnet mask.
+  @retval 0 Other errors as indicated.
+  
+**/
+UINT8
+GetSubnetMaskPrefixLength (
+  IN EFI_IPv4_ADDRESS  *SubnetMask
+  )
+{
+  UINT8   Len;
+  UINT32  ReverseMask;
+
+  //
+  // The SubnetMask is in network byte order.
+  //
+  ReverseMask = SwapBytes32 (*(UINT32 *)&SubnetMask[0]);
+
+  //
+  // Reverse it.
+  //
+  ReverseMask = ~ReverseMask;
+
+  if ((ReverseMask & (ReverseMask + 1)) != 0) {
+    return 0;
+  }
+
+  Len = 0;
+
+  while (ReverseMask != 0) {
+    ReverseMask = ReverseMask >> 1;
+    Len++;
+  }
+
+  return (UINT8) (32 - Len);
+}
+
+/**
+  Convert the decimal dotted IPv4 address into the binary IPv4 address.
+
+  @param[in]   Str             The UNICODE string.
+  @param[out]  Ip              The storage to return the IPv4 address.
+
+  @retval EFI_SUCCESS           The binary IP address is returned in Ip.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+  
+**/
+EFI_STATUS
+Ip4Config2StrToIp (
+  IN  CHAR16            *Str,
+  OUT EFI_IPv4_ADDRESS  *Ip
+  )
+{
+  UINTN Index;
+  UINTN Number;
+
+  Index = 0;
+
+  while (*Str != L'\0') {
+
+    if (Index > 3) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Number = 0;
+    while ((*Str >= L'0') && (*Str <= L'9')) {
+      Number = Number * 10 + (*Str - L'0');
+      Str++;
+    }
+
+    if (Number > 0xFF) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Ip->Addr[Index] = (UINT8) Number;
+
+    if ((*Str != L'\0') && (*Str != L'.')) {
+      //
+      // The current character should be either the NULL terminator or
+      // the dot delimiter.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (*Str == L'.') {
+      //
+      // Skip the delimiter.
+      //
+      Str++;
+    }
+
+    Index++;
+  }
+
+  if (Index != 4) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert the decimal dotted IPv4 addresses separated by space into the binary IPv4 address list.
+
+  @param[in]   Str             The UNICODE string contains IPv4 addresses.
+  @param[out]  PtrIpList       The storage to return the IPv4 address list.
+  @param[out]  IpCount         The size of the IPv4 address list.
+
+  @retval EFI_SUCCESS           The binary IP address list is returned in PtrIpList.
+  @retval EFI_OUT_OF_RESOURCES  Error occurs in allocating memory.
+  @retval EFI_INVALID_PARAMETER The IP string is malformatted.
+  
+**/
+EFI_STATUS
+Ip4Config2StrToIpList (
+  IN  CHAR16            *Str,
+  OUT EFI_IPv4_ADDRESS  **PtrIpList,
+  OUT UINTN             *IpCount
+  )
+{
+  UINTN              BeginIndex;
+  UINTN              EndIndex; 
+  UINTN              Index;
+  UINTN              IpIndex;
+  CHAR16             *StrTemp;
+  BOOLEAN            SpaceTag;
+  
+  BeginIndex = 0;
+  EndIndex   = BeginIndex;
+  Index      = 0;
+  IpIndex    = 0;
+  StrTemp    = NULL;
+  SpaceTag   = TRUE;
+  
+  *PtrIpList = NULL;
+  *IpCount   = 0;
+
+  if (Str == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get the number of Ip.
+  //
+  while (*(Str + Index) != L'\0') {
+    if (*(Str + Index) == L' ') {
+      SpaceTag = TRUE;
+    } else {
+      if (SpaceTag) {
+        (*IpCount)++;
+        SpaceTag = FALSE;
+      }
+    }
+     
+    Index++;
+  }
+
+  if (*IpCount == 0) {
+    return EFI_SUCCESS;
+  }
+  
+  //
+  // Allocate buffer for IpList.
+  //
+  *PtrIpList = AllocateZeroPool(*IpCount * sizeof(EFI_IPv4_ADDRESS));
+  if (*PtrIpList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Get IpList from Str.
+  //
+  Index = 0;
+  while (*(Str + Index) != L'\0') {
+    if (*(Str + Index) == L' ') {
+      if(!SpaceTag) {
+        StrTemp = AllocateZeroPool((EndIndex - BeginIndex + 1) * sizeof(CHAR16));
+        if (StrTemp == NULL) {
+          FreePool(*PtrIpList);
+          *PtrIpList = NULL;
+          *IpCount = 0;
+          return EFI_OUT_OF_RESOURCES;
+        }
+        
+        CopyMem (StrTemp, Str + BeginIndex, (EndIndex - BeginIndex) * sizeof(CHAR16));
+        *(StrTemp + (EndIndex - BeginIndex)) = L'\0';
+          
+        if (Ip4Config2StrToIp (StrTemp, &((*PtrIpList)[IpIndex])) != EFI_SUCCESS) {
+          FreePool(StrTemp);
+          FreePool(*PtrIpList);
+          *PtrIpList = NULL;
+          *IpCount = 0;
+          return EFI_INVALID_PARAMETER;
+        }
+          
+        BeginIndex = EndIndex;
+        IpIndex++;
+
+        FreePool(StrTemp);
+      }
+
+      BeginIndex++;
+      EndIndex++;
+      SpaceTag = TRUE;
+    } else {
+      EndIndex++;
+      SpaceTag = FALSE;
+    }
+    
+    Index++;
+    
+    if (*(Str + Index) == L'\0') {
+      if (!SpaceTag) {
+        StrTemp = AllocateZeroPool((EndIndex - BeginIndex + 1) * sizeof(CHAR16));
+        if (StrTemp == NULL) {
+          FreePool(*PtrIpList);
+          *PtrIpList = NULL;
+          *IpCount = 0;
+          return EFI_OUT_OF_RESOURCES;
+        }
+        
+        CopyMem (StrTemp, Str + BeginIndex, (EndIndex - BeginIndex) * sizeof(CHAR16));
+        *(StrTemp + (EndIndex - BeginIndex)) = L'\0';
+        
+        if (Ip4Config2StrToIp (StrTemp, &((*PtrIpList)[IpIndex])) != EFI_SUCCESS) {
+          FreePool(StrTemp);
+          FreePool(*PtrIpList);
+          *PtrIpList = NULL;
+          *IpCount = 0;
+          return EFI_INVALID_PARAMETER;
+        }
+
+        FreePool(StrTemp);
+      }
+    }
+  } 
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Convert the IPv4 address into a dotted string.
+
+  @param[in]   Ip   The IPv4 address.
+  @param[out]  Str  The dotted IP string.
+  
+**/
+VOID
+Ip4Config2IpToStr (
+  IN  EFI_IPv4_ADDRESS  *Ip,
+  OUT CHAR16            *Str
+  )
+{
+  UnicodeSPrint (
+    Str,
+    2 * IP4_STR_MAX_SIZE, 
+    L"%d.%d.%d.%d", 
+    Ip->Addr[0],
+    Ip->Addr[1],
+    Ip->Addr[2],
+    Ip->Addr[3]
+    );
+}
+
+
+/**
+  Convert the IPv4 address list into string consists of several decimal 
+  dotted IPv4 addresses separated by space.
+
+  @param[in]   Ip        The IPv4 address list.
+  @param[in]   IpCount   The size of IPv4 address list.
+  @param[out]  Str       The string contains several decimal dotted
+                         IPv4 addresses separated by space.       
+**/
+VOID
+Ip4Config2IpListToStr (
+  IN  EFI_IPv4_ADDRESS  *Ip,
+  IN  UINTN             IpCount,
+  OUT CHAR16            *Str
+  )
+{
+  UINTN            Index;
+  UINTN            TemIndex;
+  UINTN            StrIndex;
+  CHAR16           *TempStr;
+  EFI_IPv4_ADDRESS *TempIp;
+  
+  Index    = 0;
+  TemIndex = 0;
+  StrIndex = 0;
+  TempStr  = NULL;
+  TempIp   = NULL;
+
+  for (Index = 0; Index < IpCount; Index ++) {
+    TempIp = Ip + Index;
+    if (TempStr == NULL) {
+      TempStr = AllocateZeroPool(2 * IP4_STR_MAX_SIZE);
+      ASSERT(TempStr != NULL);
+    }
+
+    UnicodeSPrint (
+      TempStr, 
+      2 * IP4_STR_MAX_SIZE, 
+      L"%d.%d.%d.%d", 
+      TempIp->Addr[0],
+      TempIp->Addr[1],
+      TempIp->Addr[2],
+      TempIp->Addr[3]
+      );
+
+    for (TemIndex = 0; TemIndex < IP4_STR_MAX_SIZE; TemIndex ++) {
+      if (*(TempStr + TemIndex) == L'\0') {
+        if (Index == IpCount - 1) {
+          Str[StrIndex++] = L'\0';
+        } else {
+          Str[StrIndex++] = L' ';
+        }  
+        break;
+      } else {
+        Str[StrIndex++] = *(TempStr + TemIndex);
+      }
+    }
+  }
+
+  if (TempStr != NULL) {
+    FreePool(TempStr);
+  }
+}
+
+/**
+  The notify function of create event when performing a manual configuration.
+
+  @param[in]    Event        The pointer of Event.
+  @param[in]    Context      The pointer of Context.
+  
+**/
+VOID
+EFIAPI
+Ip4Config2ManualAddressNotify (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  *((BOOLEAN *) Context) = TRUE;
+}
+
+/**
+  Convert the network configuration data into the IFR data.
+
+  @param[in]       Instance          The IP4 config2 instance.
+  @param[in, out]  IfrNvData         The IFR nv data.
+
+  @retval EFI_SUCCESS            The configure parameter to IFR data was
+                                 set successfully.
+  @retval EFI_INVALID_PARAMETER  Source instance or target IFR data is not available.
+  @retval Others                 Other errors as indicated.
+  
+**/
+EFI_STATUS
+Ip4Config2ConvertConfigNvDataToIfrNvData (
+  IN     IP4_CONFIG2_INSTANCE       *Instance,
+  IN OUT IP4_CONFIG2_IFR_NVDATA     *IfrNvData
+  )
+{
+  IP4_SERVICE                                *IpSb;
+  EFI_IP4_CONFIG2_PROTOCOL                   *Ip4Config2;
+  EFI_IP4_CONFIG2_INTERFACE_INFO             *Ip4Info;
+  EFI_IP4_CONFIG2_POLICY                     Policy;
+  UINTN                                      DataSize;
+  UINTN                                      GatewaySize;
+  EFI_IPv4_ADDRESS                           GatewayAddress;
+  EFI_STATUS                                 Status;
+  UINTN                                      DnsSize;
+  UINTN                                      DnsCount;
+  EFI_IPv4_ADDRESS                           *DnsAddress;
+
+  Status      = EFI_SUCCESS;
+  Ip4Config2  = &Instance->Ip4Config2;
+  Ip4Info     = NULL;
+  DnsAddress  = NULL;
+  GatewaySize = sizeof (EFI_IPv4_ADDRESS);
+  
+  if ((IfrNvData == NULL) || (Instance == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  NET_CHECK_SIGNATURE (Instance, IP4_CONFIG2_INSTANCE_SIGNATURE);
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+
+  if (IpSb->DefaultInterface->Configured) {
+    IfrNvData->Configure = 1;
+  } else {
+    IfrNvData->Configure = 0;
+    goto Exit;
+  }
+
+  //
+  // Get the Policy info.
+  // 
+  DataSize = sizeof (EFI_IP4_CONFIG2_POLICY);
+  Status   = Ip4Config2->GetData (
+                           Ip4Config2,
+                           Ip4Config2DataTypePolicy,
+                           &DataSize,
+                           &Policy
+                           );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+  
+  if (Policy == Ip4Config2PolicyStatic) {
+    IfrNvData->DhcpEnable = FALSE;
+  } else if (Policy == Ip4Config2PolicyDhcp) {
+    IfrNvData->DhcpEnable = TRUE;
+    goto Exit;
+  }
+  
+  //
+  // Get the interface info.
+  //
+  DataSize    = 0;
+  Status = Ip4Config2->GetData (
+                         Ip4Config2,
+                         Ip4Config2DataTypeInterfaceInfo,
+                         &DataSize,
+                         NULL
+                         );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+  
+  Ip4Info = AllocateZeroPool (DataSize);
+  if (Ip4Info == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    return Status;
+  }
+
+  Status = Ip4Config2->GetData (
+                         Ip4Config2,
+                         Ip4Config2DataTypeInterfaceInfo,
+                         &DataSize,
+                         Ip4Info
+                         );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+  
+  //
+  // Get the Gateway info.
+  //
+  Status = Ip4Config2->GetData (
+                         Ip4Config2,
+                         Ip4Config2DataTypeGateway,
+                         &GatewaySize,
+                         &GatewayAddress
+                         );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get the Dns info.
+  //
+  DnsSize = 0;
+  Status = Ip4Config2->GetData (
+                         Ip4Config2,
+                         Ip4Config2DataTypeDnsServer,
+                         &DnsSize,
+                         NULL
+                         );
+  if ((Status != EFI_BUFFER_TOO_SMALL) && (Status != EFI_NOT_FOUND)) {
+    goto Exit;
+  }
+  
+  DnsCount = (UINT32) (DnsSize / sizeof (EFI_IPv4_ADDRESS));
+
+  if (DnsSize > 0) {
+    DnsAddress = AllocateZeroPool(DnsSize);
+    if (DnsAddress == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+    
+    Status = Ip4Config2->GetData (
+                           Ip4Config2,
+                           Ip4Config2DataTypeDnsServer,
+                           &DnsSize,
+                           DnsAddress
+                           );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+  }
+
+  Ip4Config2IpToStr (&Ip4Info->StationAddress, IfrNvData->StationAddress);
+  Ip4Config2IpToStr (&Ip4Info->SubnetMask, IfrNvData->SubnetMask);
+  Ip4Config2IpToStr (&GatewayAddress, IfrNvData->GatewayAddress);
+  Ip4Config2IpListToStr (DnsAddress, DnsCount, IfrNvData->DnsAddress);
+
+Exit:
+
+  if (DnsAddress != NULL) {
+    FreePool(DnsAddress);
+  }
+
+  if (Ip4Info != NULL) {
+    FreePool(Ip4Info);
+  }
+  
+  return Status;
+}
+
+/**
+  Convert the IFR data into the network configuration data and set the IP
+  configure parameters for the NIC.
+
+  @param[in]       IfrFormNvData     The IFR NV data.
+  @param[in, out]  Instance          The IP4 config2 instance.
+
+  @retval EFI_SUCCESS            The configure parameter for this NIC was
+                                 set successfully.
+  @retval EFI_INVALID_PARAMETER  The address information for setting is invalid.
+  @retval Others                 Other errors as indicated.
+  
+**/
+EFI_STATUS
+Ip4Config2ConvertIfrNvDataToConfigNvData (
+  IN     IP4_CONFIG2_IFR_NVDATA     *IfrFormNvData,
+  IN OUT IP4_CONFIG2_INSTANCE       *Instance
+  )
+{
+  EFI_STATUS                       Status;  
+  EFI_IP4_CONFIG2_PROTOCOL         *Ip4Cfg2;
+  IP4_CONFIG2_NVDATA               *Ip4NvData; 
+
+  EFI_IP_ADDRESS                   StationAddress;
+  EFI_IP_ADDRESS                   SubnetMask;
+  EFI_IP_ADDRESS                   Gateway;
+  IP4_ADDR                         Ip;
+  EFI_IPv4_ADDRESS                 *DnsAddress;
+  UINTN                            DnsCount;
+  UINTN                            Index;
+
+  EFI_EVENT                        TimeoutEvent;
+  EFI_EVENT                        SetAddressEvent;
+  BOOLEAN                          IsAddressOk;
+  UINTN                            DataSize;
+  EFI_INPUT_KEY                    Key;
+
+  Status          = EFI_SUCCESS;
+  Ip4Cfg2         = &Instance->Ip4Config2;
+  Ip4NvData       = &Instance->Ip4NvData;
+  
+  DnsCount        = 0;
+  DnsAddress      = NULL; 
+  
+  TimeoutEvent    = NULL;
+  SetAddressEvent = NULL;
+
+
+  
+  if (Instance == NULL || IfrFormNvData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IfrFormNvData->Configure != TRUE) {
+    return EFI_SUCCESS;
+  }
+  
+  if (IfrFormNvData->DhcpEnable == TRUE) {
+    Ip4NvData->Policy = Ip4Config2PolicyDhcp;
+    
+    Status = Ip4Cfg2->SetData (
+                        Ip4Cfg2,
+                        Ip4Config2DataTypePolicy,
+                        sizeof (EFI_IP4_CONFIG2_POLICY),
+                        &Ip4NvData->Policy
+                        );
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+  } else {
+    //
+    // Get Ip4NvData from IfrFormNvData if it is valid.
+    //
+    Ip4NvData->Policy = Ip4Config2PolicyStatic;
+
+    Status = Ip4Config2StrToIp (IfrFormNvData->StationAddress, &StationAddress.v4);
+    if (EFI_ERROR (Status) || !NetIp4IsUnicast (NTOHL (StationAddress.Addr[0]), 0)) {
+      CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid IP address!", NULL);
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    Status = Ip4Config2StrToIp (IfrFormNvData->SubnetMask, &SubnetMask.v4);
+    if (EFI_ERROR (Status) || ((SubnetMask.Addr[0] != 0) && (GetSubnetMaskPrefixLength (&SubnetMask.v4) == 0))) {
+      CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Subnet Mask!", NULL);
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    Status = Ip4Config2StrToIp (IfrFormNvData->GatewayAddress, &Gateway.v4);
+    if (EFI_ERROR (Status) || ((Gateway.Addr[0] != 0) && !NetIp4IsUnicast (NTOHL (Gateway.Addr[0]), 0))) {
+      CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Gateway!", NULL);
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Status = Ip4Config2StrToIpList (IfrFormNvData->DnsAddress, &DnsAddress, &DnsCount);
+    if (!EFI_ERROR (Status) && DnsCount > 0) {
+      for (Index = 0; Index < DnsCount; Index ++) {
+        CopyMem (&Ip, &DnsAddress[Index], sizeof (IP4_ADDR));
+        if (!NetIp4IsUnicast (NTOHL (Ip), 0)) {
+          CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Dns Server!", NULL);
+          FreePool(DnsAddress);
+          return EFI_INVALID_PARAMETER;
+        } 
+      } 
+    } else {
+      if (EFI_ERROR (Status)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Dns Server!", NULL);
+      }
+    }
+    
+    if (Ip4NvData->ManualAddress != NULL) {
+      FreePool(Ip4NvData->ManualAddress); 
+    }
+    Ip4NvData->ManualAddressCount = 1;
+    Ip4NvData->ManualAddress = AllocateZeroPool(sizeof(EFI_IP4_CONFIG2_MANUAL_ADDRESS));
+    if (Ip4NvData->ManualAddress == NULL) {
+      if (DnsAddress != NULL) {
+        FreePool(DnsAddress);
+      }
+      
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyMem(&Ip4NvData->ManualAddress->Address, &StationAddress.v4, sizeof(EFI_IPv4_ADDRESS));
+    CopyMem(&Ip4NvData->ManualAddress->SubnetMask, &SubnetMask.v4, sizeof(EFI_IPv4_ADDRESS));
+    
+    if (Ip4NvData->GatewayAddress != NULL) {
+      FreePool(Ip4NvData->GatewayAddress); 
+    }
+    Ip4NvData->GatewayAddressCount = 1;
+    Ip4NvData->GatewayAddress = AllocateZeroPool(sizeof(EFI_IPv4_ADDRESS));
+    if (Ip4NvData->GatewayAddress == NULL) {
+      if (DnsAddress != NULL) {
+        FreePool(DnsAddress);
+      }
+      return EFI_OUT_OF_RESOURCES;
+    }
+    CopyMem(Ip4NvData->GatewayAddress, &Gateway.v4, sizeof(EFI_IPv4_ADDRESS));
+    
+    if (Ip4NvData->DnsAddress != NULL) {
+      FreePool(Ip4NvData->DnsAddress); 
+    }
+    Ip4NvData->DnsAddressCount = (UINT32) DnsCount;
+    Ip4NvData->DnsAddress      = DnsAddress;
+
+    //
+    // Setting Ip4NvData.
+    //
+    Status = Ip4Cfg2->SetData (
+                        Ip4Cfg2,
+                        Ip4Config2DataTypePolicy,
+                        sizeof (EFI_IP4_CONFIG2_POLICY),
+                        &Ip4NvData->Policy
+                        );
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    //
+    // Create events & timers for asynchronous settings.
+    //
+    Status = gBS->CreateEvent (
+                    EVT_TIMER,
+                    TPL_CALLBACK,
+                    NULL,
+                    NULL,
+                    &TimeoutEvent
+                    );
+    if (EFI_ERROR (Status)) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    Ip4Config2ManualAddressNotify,
+                    &IsAddressOk,
+                    &SetAddressEvent
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    IsAddressOk = FALSE;
+    
+    Status = Ip4Cfg2->RegisterDataNotify (
+                        Ip4Cfg2,
+                        Ip4Config2DataTypeManualAddress,
+                        SetAddressEvent
+                        );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Set ManualAddress.
+    //
+    DataSize = Ip4NvData->ManualAddressCount * sizeof (EFI_IP4_CONFIG2_MANUAL_ADDRESS);
+    Status = Ip4Cfg2->SetData (
+                        Ip4Cfg2,
+                        Ip4Config2DataTypeManualAddress,
+                        DataSize,
+                        (VOID *) Ip4NvData->ManualAddress
+                        );
+
+    if (Status == EFI_NOT_READY) {
+      gBS->SetTimer (TimeoutEvent, TimerRelative, 50000000);
+      while (EFI_ERROR (gBS->CheckEvent (TimeoutEvent))) {
+        if (IsAddressOk) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+      }
+    }
+
+    Ip4Cfg2->UnregisterDataNotify (
+               Ip4Cfg2,
+               Ip4Config2DataTypeManualAddress,
+               SetAddressEvent
+               );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Set gateway.
+    //
+    DataSize = Ip4NvData->GatewayAddressCount * sizeof (EFI_IPv4_ADDRESS);
+    Status = Ip4Cfg2->SetData (
+                        Ip4Cfg2,
+                        Ip4Config2DataTypeGateway,
+                        DataSize,
+                        Ip4NvData->GatewayAddress
+                        );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Set DNS addresses.
+    //
+    if (Ip4NvData->DnsAddressCount > 0 && Ip4NvData->DnsAddress != NULL) {
+      DataSize = Ip4NvData->DnsAddressCount * sizeof (EFI_IPv4_ADDRESS);
+      Status = Ip4Cfg2->SetData (
+                          Ip4Cfg2,
+                          Ip4Config2DataTypeDnsServer,
+                          DataSize,
+                          Ip4NvData->DnsAddress
+                          );
+      
+      if (EFI_ERROR (Status)) {
+        goto Exit;
+      } 
+    } 
+  } 
+
+Exit:
+  if (SetAddressEvent != NULL) {
+    gBS->CloseEvent (SetAddressEvent);
+  }
+
+  if (TimeoutEvent != NULL) {
+    gBS->CloseEvent (TimeoutEvent);
+  }
+
+  return Status;
+}
+
+/**
+  This function allows the caller to request the current
+  configuration for one or more named elements. The resulting
+  string is in <ConfigAltResp> format. Any and all alternative
+  configuration strings shall also be appended to the end of the
+  current configuration string. If they are, they must appear
+  after the current configuration. They must contain the same
+  routing (GUID, NAME, PATH) as the current configuration string.
+  They must have an additional description indicating the type of
+  alternative configuration the string represents,
+  "ALTCFG=<StringToken>". That <StringToken> (when
+  converted from Hex UNICODE to binary) is a reference to a
+  string in the associated string pack.
+
+  @param[in] This       Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in] Request    A null-terminated Unicode string in
+                        <ConfigRequest> format. Note that this
+                        includes the routing information as well as
+                        the configurable name / value pairs. It is
+                        invalid for this string to be in
+                        <MultiConfigRequest> format.
+  @param[out] Progress  On return, points to a character in the
+                        Request string. Points to the string's null
+                        terminator if request was successful. Points
+                        to the most recent "&" before the first
+                        failing name / value pair (or the beginning
+                        of the string if the failure is in the first
+                        name / value pair) if the request was not
+                        successful.
+  @param[out] Results   A null-terminated Unicode string in
+                        <ConfigAltResp> format which has all values
+                        filled in for the names in the Request string.
+                        String to be allocated by the called function.
+
+  @retval EFI_SUCCESS             The Results string is filled with the
+                                  values corresponding to all requested
+                                  names.
+  @retval EFI_OUT_OF_RESOURCES    Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_NOT_FOUND           Routing data doesn't match any
+                                  known driver. Progress set to the
+                                  first character in the routing header.
+                                  Note: There is no requirement that the
+                                  driver validate the routing data. It
+                                  must skip the <ConfigHdr> in order to
+                                  process the names.
+  @retval EFI_INVALID_PARAMETER   Illegal syntax. Progress set
+                                  to most recent & before the
+                                  error or the beginning of the
+                                  string.
+  @retval EFI_INVALID_PARAMETER   Unknown name. Progress points
+                                  to the & before the name in
+                                  question.Currently not implemented.
+**/
+EFI_STATUS
+EFIAPI
+Ip4FormExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS                       Status;  
+  IP4_CONFIG2_INSTANCE             *Ip4Config2Instance;
+  IP4_FORM_CALLBACK_INFO           *Private;  
+  IP4_CONFIG2_IFR_NVDATA           *IfrFormNvData;
+  EFI_STRING                       ConfigRequestHdr;
+  EFI_STRING                       ConfigRequest;  
+  BOOLEAN                          AllocatedRequest;
+  EFI_STRING                       FormResult;
+  UINTN                            Size;
+  UINTN                            BufferSize;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Status             = EFI_SUCCESS; 
+  IfrFormNvData      = NULL;
+  ConfigRequest      = NULL;
+  FormResult         = NULL; 
+  Size               = 0;
+  AllocatedRequest   = FALSE;    
+  ConfigRequest      = Request; 
+  Private            = IP4_FORM_CALLBACK_INFO_FROM_CONFIG_ACCESS(This);
+  Ip4Config2Instance = IP4_CONFIG2_INSTANCE_FROM_FORM_CALLBACK(Private);
+  BufferSize         = sizeof (IP4_CONFIG2_IFR_NVDATA);
+  *Progress          = Request;
+  
+  //
+  // Check Request data in <ConfigHdr>.
+  //
+  if ((Request == NULL) || HiiIsConfigHdrMatch (Request, &gIp4Config2NvDataGuid, mIp4Config2StorageName)) {
+    IfrFormNvData = AllocateZeroPool (sizeof (IP4_CONFIG2_IFR_NVDATA));
+    if (IfrFormNvData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    
+    Ip4Config2ConvertConfigNvDataToIfrNvData (Ip4Config2Instance, IfrFormNvData);
+    
+    if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+      //
+      // Request has no request element, construct full request string.
+      // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+      // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+      //
+      ConfigRequestHdr = HiiConstructConfigHdr (&gIp4Config2NvDataGuid, mIp4Config2StorageName, Private->ChildHandle);
+      Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+      ConfigRequest = AllocateZeroPool (Size);
+      ASSERT (ConfigRequest != NULL);
+      AllocatedRequest = TRUE;
+      
+      UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+      FreePool (ConfigRequestHdr);
+    }
+
+    //
+    // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+    //
+    Status = gHiiConfigRouting->BlockToConfig (
+                                  gHiiConfigRouting,
+                                  ConfigRequest,
+                                  (UINT8 *) IfrFormNvData,
+                                  BufferSize,
+                                  &FormResult,
+                                  Progress
+                                  );
+
+    FreePool (IfrFormNvData);
+    
+    //
+    // Free the allocated config request string.
+    //
+    if (AllocatedRequest) {
+      FreePool (ConfigRequest);
+      ConfigRequest = NULL;
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto Failure;
+    }
+  }
+  
+  if (Request == NULL || HiiIsConfigHdrMatch (Request, &gIp4Config2NvDataGuid, mIp4Config2StorageName)) {
+    *Results = FormResult;
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+Failure:
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+/**
+  This function applies changes in a driver's configuration.
+  Input is a Configuration, which has the routing data for this
+  driver followed by name / value configuration pairs. The driver
+  must apply those pairs to its configurable storage. If the
+  driver's configuration is stored in a linear block of data
+  and the driver's name / value pairs are in <BlockConfig>
+  format, it may use the ConfigToBlock helper function (above) to
+  simplify the job. Currently not implemented.
+
+  @param[in]  This           Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration  A null-terminated Unicode string in
+                             <ConfigString> format.
+  @param[out] Progress       A pointer to a string filled in with the
+                             offset of the most recent '&' before the
+                             first failing name / value pair (or the
+                             beginn ing of the string if the failure
+                             is in the first name / value pair) or
+                             the terminating NULL if all was
+                             successful.
+
+  @retval EFI_SUCCESS             The results have been distributed or are
+                                  awaiting distribution.
+  @retval EFI_OUT_OF_MEMORY       Not enough memory to store the
+                                  parts of the results that must be
+                                  stored awaiting possible future
+                                  protocols.
+  @retval EFI_INVALID_PARAMETERS  Passing in a NULL for the
+                                  Results parameter would result
+                                  in this type of error.
+  @retval EFI_NOT_FOUND           Target for the specified routing data
+                                  was not found.
+**/
+EFI_STATUS
+EFIAPI
+Ip4FormRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  EFI_STATUS                       Status;
+  UINTN                            BufferSize;
+  IP4_CONFIG2_IFR_NVDATA           *IfrFormNvData;
+  IP4_CONFIG2_INSTANCE             *Ip4Config2Instance;
+  IP4_FORM_CALLBACK_INFO           *Private;
+
+  Status        = EFI_SUCCESS;
+  IfrFormNvData = NULL;
+
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Configuration;
+
+  Private            = IP4_FORM_CALLBACK_INFO_FROM_CONFIG_ACCESS(This);
+  Ip4Config2Instance = IP4_CONFIG2_INSTANCE_FROM_FORM_CALLBACK(Private);
+
+  //
+  // Check Routing data in <ConfigHdr>.
+  //
+  if (HiiIsConfigHdrMatch (Configuration, &gIp4Config2NvDataGuid, mIp4Config2StorageName)) {
+    //
+    // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+    //
+    IfrFormNvData = AllocateZeroPool (sizeof (IP4_CONFIG2_IFR_NVDATA));
+    if (IfrFormNvData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    BufferSize = 0;
+
+    Status = gHiiConfigRouting->ConfigToBlock (
+                                  gHiiConfigRouting,
+                                  Configuration,
+                                  (UINT8 *) IfrFormNvData,
+                                  &BufferSize,
+                                  Progress
+                                  );
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      return Status;
+    }
+
+    Status = gHiiConfigRouting->ConfigToBlock (
+                                  gHiiConfigRouting,
+                                  Configuration,
+                                  (UINT8 *) IfrFormNvData,
+                                  &BufferSize,
+                                  Progress
+                                  );
+    if (!EFI_ERROR (Status)) {
+      Status = Ip4Config2ConvertIfrNvDataToConfigNvData (IfrFormNvData, Ip4Config2Instance);
+    }
+
+    FreePool (IfrFormNvData);
+  } else {
+    return EFI_NOT_FOUND;
+  }
+
+  return Status;
+
+}
+
+/**
+  This function is called to provide results data to the driver.
+  This data consists of a unique key that is used to identify
+  which data is either being passed back or being asked for.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Action             Specifies the type of action taken by the browser.
+  @param[in]  QuestionId         A unique value which is sent to the original
+                                 exporting driver so that it can identify the type
+                                 of data to expect. The format of the data tends to
+                                 vary based on the opcode that enerated the callback.
+  @param[in]  Type               The type of value for the question.
+  @param[in]  Value              A pointer to the data being sent to the original
+                                 exporting driver.
+  @param[out] ActionRequest      On return, points to the action requested by the
+                                 callback function.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the
+                                 variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved.
+  @retval EFI_UNSUPPORTED        The specified Action is not supported by the
+                                 callback.Currently not implemented.
+  @retval EFI_INVALID_PARAMETERS Passing in wrong parameter.
+  @retval Others                 Other errors as indicated.
+  
+**/
+EFI_STATUS
+EFIAPI
+Ip4FormCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        QuestionId,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  EFI_STATUS                Status;
+  IP4_CONFIG2_INSTANCE      *Instance;
+  IP4_CONFIG2_IFR_NVDATA    *IfrFormNvData;
+  IP4_FORM_CALLBACK_INFO    *Private;
+  
+  EFI_IP_ADDRESS            StationAddress;
+  EFI_IP_ADDRESS            SubnetMask;
+  EFI_IP_ADDRESS            Gateway;
+  IP4_ADDR                  Ip;
+  EFI_IPv4_ADDRESS          *DnsAddress;
+  UINTN                     DnsCount;
+  UINTN                     Index;
+  EFI_INPUT_KEY             Key;
+  
+  IfrFormNvData = NULL;
+  DnsCount      = 0;
+  DnsAddress    = NULL; 
+
+  if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    Private = IP4_FORM_CALLBACK_INFO_FROM_CONFIG_ACCESS(This);
+    Instance = IP4_CONFIG2_INSTANCE_FROM_FORM_CALLBACK(Private);
+    
+    IfrFormNvData = AllocateZeroPool (sizeof (IP4_CONFIG2_IFR_NVDATA));
+    if (IfrFormNvData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Retrive uncommitted data from Browser
+    //
+    if (!HiiGetBrowserData (&gIp4Config2NvDataGuid, mIp4Config2StorageName, sizeof (IP4_CONFIG2_IFR_NVDATA), (UINT8 *) IfrFormNvData)) {
+      FreePool (IfrFormNvData);
+      return EFI_NOT_FOUND;
+    }
+
+    Status = EFI_SUCCESS;
+
+    switch (QuestionId) {
+    case KEY_LOCAL_IP:
+      Status = Ip4Config2StrToIp (IfrFormNvData->StationAddress, &StationAddress.v4);
+      if (EFI_ERROR (Status) || !NetIp4IsUnicast (NTOHL (StationAddress.Addr[0]), 0)) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid IP address!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      }
+      break;
+
+    case KEY_SUBNET_MASK:
+      Status = Ip4Config2StrToIp (IfrFormNvData->SubnetMask, &SubnetMask.v4);
+      if (EFI_ERROR (Status) || ((SubnetMask.Addr[0] != 0) && (GetSubnetMaskPrefixLength (&SubnetMask.v4) == 0))) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Subnet Mask!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      }
+      break;
+
+    case KEY_GATE_WAY:
+      Status = Ip4Config2StrToIp (IfrFormNvData->GatewayAddress, &Gateway.v4);
+      if (EFI_ERROR (Status) || ((Gateway.Addr[0] != 0) && !NetIp4IsUnicast (NTOHL (Gateway.Addr[0]), 0))) {
+        CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Gateway!", NULL);
+        Status = EFI_INVALID_PARAMETER;
+      }
+      break;
+    
+    case KEY_DNS:
+      Status = Ip4Config2StrToIpList (IfrFormNvData->DnsAddress, &DnsAddress, &DnsCount);
+      if (!EFI_ERROR (Status) && DnsCount > 0) {
+        for (Index = 0; Index < DnsCount; Index ++) {
+          CopyMem (&Ip, &DnsAddress[Index], sizeof (IP4_ADDR));
+          if (!NetIp4IsUnicast (NTOHL (Ip), 0)) {
+            CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Dns Server!", NULL);
+            Status = EFI_INVALID_PARAMETER;
+            break;
+          } 
+        }
+      } else {
+        if (EFI_ERROR (Status)) {
+          CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Invalid Dns Server!", NULL);
+        }
+      }
+      
+      if(DnsAddress != NULL) {  
+        FreePool(DnsAddress);
+      }
+      break;
+      
+    case KEY_SAVE_CHANGES:
+      Status = Ip4Config2ConvertIfrNvDataToConfigNvData (IfrFormNvData, Instance);
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_SUBMIT;
+      break;
+
+    default:
+      break;
+    }
+
+    FreePool (IfrFormNvData);
+
+    return Status;
+  }
+
+  //
+  // All other action return unsupported.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Install HII Config Access protocol for network device and allocate resource.
+
+  @param[in, out]  Instance        The IP4 config2 Instance.
+
+  @retval EFI_SUCCESS              The HII Config Access protocol is installed.
+  @retval EFI_OUT_OF_RESOURCES     Failed to allocate memory.
+  @retval Others                   Other errors as indicated.
+  
+**/
+EFI_STATUS
+Ip4Config2FormInit (
+  IN OUT IP4_CONFIG2_INSTANCE     *Instance
+  )
+{
+  EFI_STATUS                     Status;
+  IP4_SERVICE                    *IpSb;
+  IP4_FORM_CALLBACK_INFO         *CallbackInfo;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL *ConfigAccess;
+  VENDOR_DEVICE_PATH             VendorDeviceNode;
+  EFI_SERVICE_BINDING_PROTOCOL   *MnpSb;
+  CHAR16                         *MacString;
+  CHAR16                         MenuString[128];
+  CHAR16                         PortString[128];
+  CHAR16                         *OldMenuString;
+  EFI_DEVICE_PATH_PROTOCOL       *ParentDevicePath;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  ASSERT (IpSb != NULL);
+  
+  CallbackInfo = &Instance->CallbackInfo;
+  
+  CallbackInfo->Signature = IP4_FORM_CALLBACK_INFO_SIGNATURE;
+
+  Status = gBS->HandleProtocol (
+                  IpSb->Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Construct device path node for EFI HII Config Access protocol,
+  // which consists of controller physical device path and one hardware
+  // vendor guid node.
+  //
+  ZeroMem (&VendorDeviceNode, sizeof (VENDOR_DEVICE_PATH));
+  VendorDeviceNode.Header.Type    = HARDWARE_DEVICE_PATH;
+  VendorDeviceNode.Header.SubType = HW_VENDOR_DP;
+
+  CopyGuid (&VendorDeviceNode.Guid, &gEfiCallerIdGuid);
+
+  SetDevicePathNodeLength (&VendorDeviceNode.Header, sizeof (VENDOR_DEVICE_PATH));
+  CallbackInfo->HiiVendorDevicePath = AppendDevicePathNode (
+                                        ParentDevicePath,
+                                        (EFI_DEVICE_PATH_PROTOCOL *) &VendorDeviceNode
+                                        );
+  if (CallbackInfo->HiiVendorDevicePath == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  ConfigAccess                = &CallbackInfo->HiiConfigAccessProtocol;
+  ConfigAccess->ExtractConfig = Ip4FormExtractConfig;
+  ConfigAccess->RouteConfig   = Ip4FormRouteConfig;
+  ConfigAccess->Callback      = Ip4FormCallback;
+
+  //
+  // Install Device Path Protocol and Config Access protocol on new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &CallbackInfo->ChildHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  CallbackInfo->HiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  ConfigAccess,
+                  NULL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Open the Parent Handle for the child
+    //
+    Status = gBS->OpenProtocol (
+                    IpSb->Controller,
+                    &gEfiManagedNetworkServiceBindingProtocolGuid,
+                    (VOID **) &MnpSb,
+                    IpSb->Image,
+                    CallbackInfo->ChildHandle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Error;
+  }
+
+  //
+  // Publish our HII data
+  //
+  CallbackInfo->RegisteredHandle = HiiAddPackages (
+                                     &gIp4Config2NvDataGuid,
+                                     CallbackInfo->ChildHandle,
+                                     Ip4DxeStrings,
+                                     Ip4Config2Bin,
+                                     NULL
+                                     );
+  if (CallbackInfo->RegisteredHandle == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error;
+  }
+
+  //
+  // Append MAC string in the menu help string and tile help string
+  //
+  Status = NetLibGetMacString (IpSb->Controller, IpSb->Image, &MacString);
+  if (!EFI_ERROR (Status)) {
+    OldMenuString = HiiGetString (
+                      CallbackInfo->RegisteredHandle, 
+                      STRING_TOKEN (STR_IP4_CONFIG2_FORM_HELP), 
+                      NULL
+                      );
+    UnicodeSPrint (MenuString, 128, L"%s (MAC:%s)", OldMenuString, MacString);
+    HiiSetString (
+      CallbackInfo->RegisteredHandle, 
+      STRING_TOKEN (STR_IP4_CONFIG2_FORM_HELP), 
+      MenuString, 
+      NULL
+      );
+
+    UnicodeSPrint (PortString, 128, L"MAC:%s", MacString);
+    HiiSetString (
+      CallbackInfo->RegisteredHandle,  
+      STRING_TOKEN (STR_IP4_DEVICE_FORM_HELP), 
+      PortString, 
+      NULL
+      );
+    
+    FreePool (MacString);
+    FreePool (OldMenuString);
+
+    return EFI_SUCCESS;
+  }
+
+Error:
+  Ip4Config2FormUnload (Instance);
+  return Status;
+}
+
+/**
+  Uninstall the HII Config Access protocol for network devices and free up the resources.
+
+  @param[in, out]  Instance      The IP4 config2 instance to unload a form.
+
+**/
+VOID
+Ip4Config2FormUnload (
+  IN OUT IP4_CONFIG2_INSTANCE     *Instance
+  )
+{
+  IP4_SERVICE                    *IpSb;
+  IP4_FORM_CALLBACK_INFO         *CallbackInfo;
+  IP4_CONFIG2_NVDATA             *Ip4NvData;
+
+  IpSb = IP4_SERVICE_FROM_IP4_CONFIG2_INSTANCE (Instance);
+  ASSERT (IpSb != NULL);
+
+  CallbackInfo = &Instance->CallbackInfo;
+
+  if (CallbackInfo->ChildHandle != NULL) {
+    //
+    // Close the child handle
+    //
+    gBS->CloseProtocol (
+           IpSb->Controller,
+           &gEfiManagedNetworkServiceBindingProtocolGuid,
+           IpSb->Image,
+           CallbackInfo->ChildHandle
+           );
+    
+    //
+    // Uninstall EFI_HII_CONFIG_ACCESS_PROTOCOL
+    //
+    gBS->UninstallMultipleProtocolInterfaces (
+           CallbackInfo->ChildHandle,
+           &gEfiDevicePathProtocolGuid,
+           CallbackInfo->HiiVendorDevicePath,
+           &gEfiHiiConfigAccessProtocolGuid,
+           &CallbackInfo->HiiConfigAccessProtocol,
+           NULL
+           );
+  }
+
+  if (CallbackInfo->HiiVendorDevicePath != NULL) {
+    FreePool (CallbackInfo->HiiVendorDevicePath);
+  }
+
+  if (CallbackInfo->RegisteredHandle != NULL) {
+    //
+    // Remove HII package list
+    //
+    HiiRemovePackages (CallbackInfo->RegisteredHandle);
+  }
+
+  Ip4NvData = &Instance->Ip4NvData;
+
+  if(Ip4NvData->ManualAddress != NULL) {
+    FreePool(Ip4NvData->ManualAddress);
+  }
+
+  if(Ip4NvData->GatewayAddress != NULL) {
+    FreePool(Ip4NvData->GatewayAddress);
+  }
+
+  if(Ip4NvData->DnsAddress != NULL) {
+    FreePool(Ip4NvData->DnsAddress);
+  }
+
+  Ip4NvData->ManualAddressCount  = 0;
+  Ip4NvData->GatewayAddressCount = 0;
+  Ip4NvData->DnsAddressCount     = 0;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.h
new file mode 100644
index 0000000000..5a82e6d229
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Config2Nv.h
@@ -0,0 +1,51 @@
+/** @file
+  The header file of IP4Config2Nv.c
+
+Copyright (c) 2015, 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.
+
+**/
+
+#ifndef _IP4_CONFIG2NV_H_
+#define _IP4_CONFIG2NV_H_
+
+#include "Ip4Impl.h"
+
+extern UINT8  Ip4Config2Bin[];
+extern UINT8  Ip4DxeStrings[];
+
+#define NIC_ITEM_CONFIG_SIZE   (sizeof (IP4_CONFIG2_INSTANCE) + (sizeof (EFI_IPv4_ADDRESS) * MAX_IP4_CONFIG_DNS))
+
+/**
+  Install HII Config Access protocol for network device and allocate resource.
+
+  @param[in, out]  Instance         The IP4 config2 Instance.
+
+  @retval EFI_SUCCESS              The HII Config Access protocol is installed.
+  @retval EFI_OUT_OF_RESOURCES     Failed to allocate memory.
+  @retval Others                   Other errors as indicated.
+  
+**/
+EFI_STATUS
+Ip4Config2FormInit (
+  IN OUT IP4_CONFIG2_INSTANCE     *Instance
+  );
+
+/**
+  Uninstall the HII Config Access protocol for network devices and free up the resources.
+
+  @param[in, out]  Instance      The IP4 config2 instance to unload a form.
+
+**/
+VOID
+Ip4Config2FormUnload (
+  IN OUT IP4_CONFIG2_INSTANCE     *Instance
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.c
new file mode 100644
index 0000000000..3dc4d88f13
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.c
@@ -0,0 +1,989 @@
+/** @file
+  The driver binding and service binding protocol for IP4 driver.
+
+Copyright (c) 2005 - 2015, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<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 "Ip4Impl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gIp4DriverBinding = {
+  Ip4DriverBindingSupported,
+  Ip4DriverBindingStart,
+  Ip4DriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+BOOLEAN  mIpSec2Installed = FALSE;
+
+/**
+   Callback function for IpSec2 Protocol install.
+
+   @param[in] Event           Event whose notification function is being invoked
+   @param[in] Context         Pointer to the notification function's context
+
+**/
+VOID
+EFIAPI
+IpSec2InstalledCallback (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  //
+  // Close the event so it does not get called again.
+  //
+  gBS->CloseEvent (Event);
+
+  mIpSec2Installed = TRUE;
+}
+
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers.
+
+  The entry point for IP4 driver which install the driver
+  binding and component name protocol on its image.
+
+  @param[in]  ImageHandle           The firmware allocated handle for the UEFI image.
+  @param[in]  SystemTable           A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverEntryPoint (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  VOID            *Registration;
+
+  EfiCreateProtocolNotifyEvent (
+    &gEfiIpSec2ProtocolGuid,
+    TPL_CALLBACK,
+    IpSec2InstalledCallback,
+    NULL,
+    &Registration
+    );
+
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gIp4DriverBinding,
+           ImageHandle,
+           &gIp4ComponentName,
+           &gIp4ComponentName2
+           );
+}
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+
+  //
+  // Test for the MNP service binding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Test for the Arp service binding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiArpServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+/**
+  Clean up a IP4 service binding instance. It will release all
+  the resource allocated by the instance. The instance may be
+  partly initialized, or partly destroyed. If a resource is
+  destroyed, it is marked as that in case the destroy failed and
+  being called again later.
+
+  @param[in]  IpSb               The IP4 service binding instance to clean up
+
+  @retval EFI_SUCCESS            The resource used by the instance are cleaned up
+  @retval other                  Failed to clean up some of the resources.
+
+**/
+EFI_STATUS
+Ip4CleanService (
+  IN IP4_SERVICE            *IpSb
+  );
+
+
+/**
+  Create a new IP4 driver service binding private instance.
+
+  @param  Controller         The controller that has MNP service binding
+                             installed
+  @param  ImageHandle        The IP4 driver's image handle
+  @param  Service            The variable to receive the newly created IP4
+                             service.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate some resource
+  @retval EFI_SUCCESS            A new IP4 service binding private is created.
+  @retval other                  Other error occurs.
+
+**/
+EFI_STATUS
+Ip4CreateService (
+  IN  EFI_HANDLE            Controller,
+  IN  EFI_HANDLE            ImageHandle,
+  OUT IP4_SERVICE           **Service
+  )
+{
+  IP4_SERVICE               *IpSb;
+  EFI_STATUS                Status;
+
+  ASSERT (Service != NULL);
+
+  *Service = NULL;
+
+  //
+  // allocate a service private data then initialize all the filed to
+  // empty resources, so if any thing goes wrong when allocating
+  // resources, Ip4CleanService can be called to clean it up.
+  //
+  IpSb = AllocateZeroPool (sizeof (IP4_SERVICE));
+
+  if (IpSb == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  IpSb->Signature                   = IP4_SERVICE_SIGNATURE;
+  IpSb->ServiceBinding.CreateChild  = Ip4ServiceBindingCreateChild;
+  IpSb->ServiceBinding.DestroyChild = Ip4ServiceBindingDestroyChild;
+  IpSb->State                       = IP4_SERVICE_UNSTARTED;
+
+  IpSb->NumChildren                 = 0;
+  InitializeListHead (&IpSb->Children);
+
+  InitializeListHead (&IpSb->Interfaces);
+  IpSb->DefaultInterface            = NULL;
+  IpSb->DefaultRouteTable           = NULL;
+
+  Ip4InitAssembleTable (&IpSb->Assemble);
+
+  IpSb->IgmpCtrl.Igmpv1QuerySeen    = 0;
+  InitializeListHead (&IpSb->IgmpCtrl.Groups);
+
+  IpSb->Image                       = ImageHandle;
+  IpSb->Controller                  = Controller;
+
+  IpSb->MnpChildHandle              = NULL;
+  IpSb->Mnp                         = NULL;
+
+  IpSb->MnpConfigData.ReceivedQueueTimeoutValue = 0;
+  IpSb->MnpConfigData.TransmitQueueTimeoutValue = 0;
+  IpSb->MnpConfigData.ProtocolTypeFilter        = IP4_ETHER_PROTO;
+  IpSb->MnpConfigData.EnableUnicastReceive      = TRUE;
+  IpSb->MnpConfigData.EnableMulticastReceive    = TRUE;
+  IpSb->MnpConfigData.EnableBroadcastReceive    = TRUE;
+  IpSb->MnpConfigData.EnablePromiscuousReceive  = FALSE;
+  IpSb->MnpConfigData.FlushQueuesOnReset        = TRUE;
+  IpSb->MnpConfigData.EnableReceiveTimestamps   = FALSE;
+  IpSb->MnpConfigData.DisableBackgroundPolling  = FALSE;
+
+  ZeroMem (&IpSb->SnpMode, sizeof (EFI_SIMPLE_NETWORK_MODE));
+
+  IpSb->Timer = NULL;
+
+  IpSb->ReconfigEvent = NULL;
+
+  IpSb->Reconfig = FALSE;
+  
+  IpSb->MediaPresent = TRUE;
+
+  //
+  // Create various resources. First create the route table, timer
+  // event and MNP child. IGMP, interface's initialization depend
+  // on the MNP child.
+  //
+  IpSb->DefaultRouteTable = Ip4CreateRouteTable ();
+
+  if (IpSb->DefaultRouteTable == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  Ip4TimerTicking,
+                  IpSb,
+                  &IpSb->Timer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = NetLibCreateServiceChild (
+             Controller,
+             ImageHandle,
+             &gEfiManagedNetworkServiceBindingProtocolGuid,
+             &IpSb->MnpChildHandle
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  IpSb->MnpChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **) &IpSb->Mnp,
+                  ImageHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Ip4ServiceConfigMnp (IpSb, TRUE);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = IpSb->Mnp->GetModeData (IpSb->Mnp, NULL, &IpSb->SnpMode);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Ip4InitIgmp (IpSb);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  IpSb->MacString = NULL;
+  Status = NetLibGetMacString (IpSb->Controller, IpSb->Image, &IpSb->MacString);
+  
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  IpSb->DefaultInterface = Ip4CreateInterface (IpSb->Mnp, Controller, ImageHandle);
+
+  if (IpSb->DefaultInterface == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  InsertHeadList (&IpSb->Interfaces, &IpSb->DefaultInterface->Link);
+
+  ZeroMem (&IpSb->Ip4Config2Instance, sizeof (IP4_CONFIG2_INSTANCE));
+  
+  Status = Ip4Config2InitInstance (&IpSb->Ip4Config2Instance);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  IpSb->MaxPacketSize = IpSb->SnpMode.MaxPacketSize - sizeof (IP4_HEAD);
+  if (NetLibGetVlanId (IpSb->Controller) != 0) {
+    //
+    // This is a VLAN device, reduce MTU by VLAN tag length
+    //
+    IpSb->MaxPacketSize -= NET_VLAN_TAG_LEN;
+  }
+  IpSb->OldMaxPacketSize = IpSb->MaxPacketSize;
+  *Service = IpSb;
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  Ip4CleanService (IpSb);
+  FreePool (IpSb);
+
+  return Status;
+}
+
+
+/**
+  Clean up a IP4 service binding instance. It will release all
+  the resource allocated by the instance. The instance may be
+  partly initialized, or partly destroyed. If a resource is
+  destroyed, it is marked as that in case the destroy failed and
+  being called again later.
+
+  @param[in]  IpSb               The IP4 service binding instance to clean up
+
+  @retval EFI_SUCCESS            The resource used by the instance are cleaned up
+  @retval other                  Failed to clean up some of the resources.
+
+**/
+EFI_STATUS
+Ip4CleanService (
+  IN IP4_SERVICE            *IpSb
+  )
+{
+  EFI_STATUS                Status;
+
+  if (IpSb->DefaultInterface != NULL) {
+    Status = Ip4FreeInterface (IpSb->DefaultInterface, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    IpSb->DefaultInterface = NULL;
+  }
+
+  if (IpSb->DefaultRouteTable != NULL) {
+    Ip4FreeRouteTable (IpSb->DefaultRouteTable);
+    IpSb->DefaultRouteTable = NULL;
+  }
+
+  Ip4CleanAssembleTable (&IpSb->Assemble);
+
+  if (IpSb->MnpChildHandle != NULL) {
+    if (IpSb->Mnp != NULL) {
+      gBS->CloseProtocol (
+             IpSb->MnpChildHandle,
+             &gEfiManagedNetworkProtocolGuid,
+             IpSb->Image,
+             IpSb->Controller
+             );
+
+      IpSb->Mnp = NULL;
+    }
+
+    NetLibDestroyServiceChild (
+      IpSb->Controller,
+      IpSb->Image,
+      &gEfiManagedNetworkServiceBindingProtocolGuid,
+      IpSb->MnpChildHandle
+      );
+
+    IpSb->MnpChildHandle = NULL;
+  }
+
+  if (IpSb->Timer != NULL) {
+    gBS->SetTimer (IpSb->Timer, TimerCancel, 0);
+    gBS->CloseEvent (IpSb->Timer);
+
+    IpSb->Timer = NULL;
+  }
+
+  if (IpSb->ReconfigEvent != NULL) {
+    gBS->CloseEvent (IpSb->ReconfigEvent);
+
+    IpSb->ReconfigEvent = NULL;
+  }
+
+  IpSb->Reconfig = FALSE;
+
+  if (IpSb->MacString != NULL) {
+    FreePool (IpSb->MacString);
+  }
+
+  Ip4Config2CleanInstance (&IpSb->Ip4Config2Instance);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DestroyChildEntryInHandleBuffer (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  IP4_PROTOCOL                  *IpInstance;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = NET_LIST_USER_STRUCT_S (Entry, IP4_PROTOCOL, Link, IP4_PROTOCOL_SIGNATURE);
+  ServiceBinding    = ((IP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ServiceBinding;
+  NumberOfChildren  = ((IP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->NumberOfChildren;
+  ChildHandleBuffer = ((IP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ChildHandleBuffer;
+
+  if (!NetIsInHandleBuffer (IpInstance->Handle, NumberOfChildren, ChildHandleBuffer)) {
+    return EFI_SUCCESS;
+  }
+
+  return ServiceBinding->DestroyChild (ServiceBinding, IpInstance->Handle);
+}
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{ 
+  EFI_STATUS                    Status;
+  IP4_SERVICE                   *IpSb;
+
+  //
+  // Test for the Ip4 service binding protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiIp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (Status == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+  
+  Status = Ip4CreateService (ControllerHandle, This->DriverBindingHandle, &IpSb);
+  
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  ASSERT (IpSb != NULL);
+
+  //
+  // Install the Ip4ServiceBinding Protocol onto ControlerHandle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiIp4ServiceBindingProtocolGuid,
+                  &IpSb->ServiceBinding,
+                  &gEfiIp4Config2ProtocolGuid,
+                  &IpSb->Ip4Config2Instance.Ip4Config2,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto FREE_SERVICE;
+  }
+ 
+  //
+  // Ready to go: start the receiving and timer.
+  // Ip4Config2SetPolicy maybe call Ip4ReceiveFrame() to set the default interface's RecvRequest first after
+  // Ip4Config2 instance is initialized. So, EFI_ALREADY_STARTED is the allowed return status.
+  //
+  Status = Ip4ReceiveFrame (IpSb->DefaultInterface, NULL, Ip4AccpetFrame, IpSb);
+
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    goto UNINSTALL_PROTOCOL;
+  }
+
+  Status = gBS->SetTimer (IpSb->Timer, TimerPeriodic, TICKS_PER_SECOND);
+
+  if (EFI_ERROR (Status)) {
+    goto UNINSTALL_PROTOCOL;
+  }
+
+  //
+  // Initialize the IP4 ID
+  //
+  mIp4Id = (UINT16)NET_RANDOM (NetRandomInitSeed ());
+
+  return Status;
+
+UNINSTALL_PROTOCOL:
+  gBS->UninstallProtocolInterface (
+         ControllerHandle,
+         &gEfiIp4ServiceBindingProtocolGuid,
+         &IpSb->ServiceBinding
+         );
+
+FREE_SERVICE:
+  Ip4CleanService (IpSb);
+  FreePool (IpSb);
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number
+                                of children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS           This driver is removed ControllerHandle
+  @retval other                 This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_SERVICE_BINDING_PROTOCOL             *ServiceBinding;
+  IP4_SERVICE                              *IpSb; 
+  EFI_HANDLE                               NicHandle;
+  EFI_STATUS                               Status; 
+  INTN                                     State;
+  LIST_ENTRY                               *List;
+  IP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT  Context;
+  IP4_INTERFACE                            *IpIf;
+  IP4_ROUTE_TABLE                          *RouteTable;
+
+  BOOLEAN                                  IsDhcp4;
+
+  IsDhcp4   = FALSE;
+   
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiManagedNetworkProtocolGuid);
+  if (NicHandle == NULL) {
+    NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiArpProtocolGuid);
+    if (NicHandle == NULL) {
+      NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiDhcp4ProtocolGuid);
+      if (NicHandle != NULL) {
+        IsDhcp4 = TRUE;  
+      } else {
+        return EFI_SUCCESS;
+      }
+    }
+  }
+   
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiIp4ServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  NicHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+  
+  IpSb = IP4_SERVICE_FROM_PROTOCOL (ServiceBinding);
+
+  if (IsDhcp4) {
+    Status = Ip4Config2DestroyDhcp4 (&IpSb->Ip4Config2Instance);
+    gBS->CloseEvent (IpSb->Ip4Config2Instance.Dhcp4Event);
+    IpSb->Ip4Config2Instance.Dhcp4Event = NULL;
+  } else if (NumberOfChildren != 0) {
+    List = &IpSb->Children;
+    Context.ServiceBinding    = ServiceBinding;
+    Context.NumberOfChildren  = NumberOfChildren;
+    Context.ChildHandleBuffer = ChildHandleBuffer;
+    Status = NetDestroyLinkList (
+               List,
+               Ip4DestroyChildEntryInHandleBuffer,
+               &Context,
+               NULL
+               );
+  } else if (IpSb->DefaultInterface->ArpHandle == ControllerHandle) {
+  
+    //
+    // The ARP protocol for the default interface is being uninstalled and all
+    // its IP child handles should have been destroyed before. So, release the
+    // default interface and route table, create a new one and mark it as not started.
+    //
+    Ip4CancelReceive (IpSb->DefaultInterface);
+    Ip4FreeInterface (IpSb->DefaultInterface, NULL);
+    Ip4FreeRouteTable (IpSb->DefaultRouteTable);
+    
+    IpIf = Ip4CreateInterface (IpSb->Mnp, IpSb->Controller, IpSb->Image);
+    if (IpIf == NULL) {
+      goto ON_ERROR;
+    }
+    RouteTable = Ip4CreateRouteTable ();
+    if (RouteTable == NULL) {
+      Ip4FreeInterface (IpIf, NULL);
+      goto ON_ERROR;;
+    }
+    
+    IpSb->DefaultInterface  = IpIf;
+    InsertHeadList (&IpSb->Interfaces, &IpIf->Link);
+    IpSb->DefaultRouteTable = RouteTable;
+    Ip4ReceiveFrame (IpIf, NULL, Ip4AccpetFrame, IpSb);
+
+    IpSb->State = IP4_SERVICE_UNSTARTED;
+
+  } else if (IsListEmpty (&IpSb->Children)) {
+    State           = IpSb->State;
+    IpSb->State     = IP4_SERVICE_DESTROY;
+
+    //
+    // OK, clean other resources then uninstall the service binding protocol.
+    //
+    Status = Ip4CleanService (IpSb);
+    if (EFI_ERROR (Status)) {
+      IpSb->State = State;
+      goto ON_ERROR;
+    }
+
+    gBS->UninstallMultipleProtocolInterfaces (
+           NicHandle,
+           &gEfiIp4ServiceBindingProtocolGuid,
+           ServiceBinding,
+           &gEfiIp4Config2ProtocolGuid,
+           &IpSb->Ip4Config2Instance.Ip4Config2,
+           NULL
+           );
+    
+    if (gIp4ControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gIp4ControllerNameTable);
+      gIp4ControllerNameTable = NULL;
+    }
+    FreePool (IpSb);
+  }
+
+ON_ERROR:
+  return Status;
+}
+
+
+/**
+  Creates a child handle and installs a protocol.
+
+  The CreateChild() function installs a protocol on ChildHandle.
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle.
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle,
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                *ChildHandle
+  )
+{
+  IP4_SERVICE               *IpSb;
+  IP4_PROTOCOL              *IpInstance;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+  VOID                      *Mnp;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpSb       = IP4_SERVICE_FROM_PROTOCOL (This);
+  IpInstance = AllocatePool (sizeof (IP4_PROTOCOL));
+
+  if (IpInstance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ip4InitProtocol (IpSb, IpInstance);
+
+  //
+  // Install Ip4 onto ChildHandle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  &IpInstance->Ip4Proto,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  IpInstance->Handle = *ChildHandle;
+
+  //
+  // Open the Managed Network protocol BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  IpSb->MnpChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **) &Mnp,
+                  gIp4DriverBinding.DriverBindingHandle,
+                  IpInstance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           ChildHandle,
+           &gEfiIp4ProtocolGuid,
+           &IpInstance->Ip4Proto,
+           NULL
+           );
+
+    goto ON_ERROR;
+  }
+
+  //
+  // Insert it into the service binding instance.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  InsertTailList (&IpSb->Children, &IpInstance->Link);
+  IpSb->NumChildren++;
+
+  gBS->RestoreTPL (OldTpl);
+
+ON_ERROR:
+
+  if (EFI_ERROR (Status)) {
+
+    Ip4CleanProtocol (IpInstance);
+
+    FreePool (IpInstance);
+  }
+
+  return Status;
+}
+
+
+/**
+  Destroys a child handle with a protocol installed on it.
+
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  )
+{
+  EFI_STATUS                Status;
+  IP4_SERVICE               *IpSb;
+  IP4_PROTOCOL              *IpInstance;
+  EFI_IP4_PROTOCOL          *Ip4;
+  EFI_TPL                   OldTpl;
+  INTN                      State;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the private context data structures
+  //
+  IpSb   = IP4_SERVICE_FROM_PROTOCOL (This);
+
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **) &Ip4,
+                  gIp4DriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (Ip4);
+
+  if (IpInstance->Service != IpSb) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // A child can be destroyed more than once. For example,
+  // Ip4DriverBindingStop will destroy all of its children.
+  // when UDP driver is being stopped, it will destroy all
+  // the IP child it opens.
+  //
+  if (IpInstance->State == IP4_STATE_DESTROY) {
+    gBS->RestoreTPL (OldTpl);
+    return EFI_SUCCESS;
+  }
+
+  State             = IpInstance->State;
+  IpInstance->State = IP4_STATE_DESTROY;
+
+  //
+  // Close the Managed Network protocol.
+  //
+  gBS->CloseProtocol (
+         IpSb->MnpChildHandle,
+         &gEfiManagedNetworkProtocolGuid,
+         gIp4DriverBinding.DriverBindingHandle,
+         ChildHandle
+         );
+
+  if (IpInstance->Interface != NULL && IpInstance->Interface->Arp != NULL) {
+    gBS->CloseProtocol (
+           IpInstance->Interface->ArpHandle,
+           &gEfiArpProtocolGuid,
+           gIp4DriverBinding.DriverBindingHandle,
+           ChildHandle
+           );
+  }
+
+  //
+  // Uninstall the IP4 protocol first. Many thing happens during
+  // this:
+  // 1. The consumer of the IP4 protocol will be stopped if it
+  // opens the protocol BY_DRIVER. For eaxmple, if MNP driver is
+  // stopped, IP driver's stop function will be called, and uninstall
+  // EFI_IP4_PROTOCOL will trigger the UDP's stop function. This
+  // makes it possible to create the network stack bottom up, and
+  // stop it top down.
+  // 2. the upper layer will recycle the received packet. The recycle
+  // event's TPL is higher than this function. The recycle events
+  // will be called back before preceeding. If any packets not recycled,
+  // that means there is a resource leak.
+  //
+  gBS->RestoreTPL (OldTpl);
+  Status = gBS->UninstallProtocolInterface (
+                  ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  &IpInstance->Ip4Proto
+                  );
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Ip4CleanProtocol (IpInstance);
+  if (EFI_ERROR (Status)) {
+    gBS->InstallMultipleProtocolInterfaces (
+           &ChildHandle,
+           &gEfiIp4ProtocolGuid,
+           Ip4,
+           NULL
+           );
+
+    goto ON_ERROR;
+  }
+
+  RemoveEntryList (&IpInstance->Link);
+  IpSb->NumChildren--;
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (IpInstance);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  IpInstance->State = State;
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.h
new file mode 100644
index 0000000000..32984bad66
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Driver.h
@@ -0,0 +1,190 @@
+/** @file
+
+Copyright (c) 2005 - 2012, 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.
+
+**/
+
+#ifndef __EFI_IP4_DRIVER_H__
+#define __EFI_IP4_DRIVER_H__
+
+#include <Protocol/ServiceBinding.h>
+
+extern EFI_DRIVER_BINDING_PROTOCOL   gIp4DriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL   gIp4ComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gIp4ComponentName2;
+extern EFI_UNICODE_STRING_TABLE      *gIp4ControllerNameTable;
+
+typedef struct {
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+} IP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT;
+
+//
+// Function prototype for the driver's entry point
+//
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers.
+  
+  The entry point for IP4 driver which install the driver
+  binding and component name protocol on its image.
+
+  @param[in]  ImageHandle           The firmware allocated handle for the UEFI image.
+  @param[in]  SystemTable           A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverEntryPoint (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  );
+
+//
+// Function prototypes for the Drivr Binding Protocol
+//
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+  
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number
+                                of children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS           This driver is removed ControllerHandle
+  @retval other                 This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+//
+// Function ptototypes for the ServiceBinding Prococol
+//
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                      then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                      then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.inf b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.inf
new file mode 100644
index 0000000000..f561af0c75
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.inf
@@ -0,0 +1,115 @@
+## @file
+#  This module produces EFI IPv4 Protocol and EFI IPv4 Service Binding Protocol.
+#
+#  This module produces EFI IPv4 Protocol upon EFI MNP Protocol and EFI ARP Protocol,
+#  to provide basic network IPv4 packet I/O services, which includes support for a
+#  subset of the Internet Control Message Protocol (ICMP) and may include support for
+#  the Internet Group Management Protocol (IGMP).
+#
+#  Copyright (c) 2007 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Ip4Dxe
+  MODULE_UNI_FILE                = Ip4Dxe.uni
+  FILE_GUID                      = 9FB1A1F3-3B71-4324-B39A-745CBB015FFF
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0 
+  ENTRY_POINT                    = Ip4DriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gIp4DriverBinding                        
+#  COMPONENT_NAME                =  gIp4ComponentName
+#  COMPONENT_NAME2               =  gIp4ComponentName2
+#
+
+[Sources]
+  Ip4Driver.c
+  Ip4Option.h
+  Ip4Route.h
+  Ip4If.c
+  Ip4Igmp.h
+  Ip4Output.c
+  Ip4Icmp.c
+  Ip4Igmp.c
+  Ip4Impl.c
+  Ip4Common.h
+  Ip4Impl.h
+  Ip4Driver.h
+  Ip4Common.c
+  Ip4If.h
+  Ip4Option.c
+  Ip4Output.h
+  ComponentName.c
+  Ip4Input.h
+  Ip4Route.c
+  Ip4Icmp.h
+  Ip4Input.c
+  Ip4Config2Impl.c
+  Ip4Config2Impl.h
+  Ip4Config2.vfr
+  Ip4DxeStrings.uni
+  Ip4NvData.h
+  Ip4Config2Nv.h
+  Ip4Config2Nv.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  DebugLib
+  NetLib
+  DpcLib
+  HiiLib
+  PrintLib
+  DevicePathLib
+  UefiHiiServicesLib
+
+[Protocols]
+  ## BY_START
+  ## UNDEFINED # variable
+  gEfiIp4ServiceBindingProtocolGuid
+  gEfiIp4ProtocolGuid                           ## BY_START
+  gEfiManagedNetworkServiceBindingProtocolGuid  ## TO_START
+  gEfiManagedNetworkProtocolGuid                ## TO_START
+  gEfiArpServiceBindingProtocolGuid             ## TO_START
+  gEfiIp4Config2ProtocolGuid                    ## BY_START
+  gEfiArpProtocolGuid                           ## TO_START
+  gEfiDhcp4ServiceBindingProtocolGuid           ## TO_START
+  gEfiDhcp4ProtocolGuid                         ## TO_START
+  gEfiIpSec2ProtocolGuid                        ## SOMETIMES_CONSUMES
+  gEfiHiiConfigAccessProtocolGuid               ## BY_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  
+[Guids]
+  ## SOMETIMES_CONSUMES ## GUID # HiiIsConfigHdrMatch   EFI_NIC_IP4_CONFIG_VARIABLE
+  ## SOMETIMES_PRODUCES ## GUID # HiiConstructConfigHdr EFI_NIC_IP4_CONFIG_VARIABLE
+  ## SOMETIMES_PRODUCES ## GUID # HiiGetBrowserData     EFI_NIC_IP4_CONFIG_VARIABLE
+  ## SOMETIMES_CONSUMES ## HII
+  gIp4Config2NvDataGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  Ip4DxeExtra.uni
+
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.uni b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.uni
new file mode 100644
index 0000000000..398f730582
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeExtra.uni
new file mode 100644
index 0000000000..7923ef1c7f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeStrings.uni b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeStrings.uni
new file mode 100644
index 0000000000..c940cf688c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4DxeStrings.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.c
new file mode 100644
index 0000000000..b4b086496c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.c
@@ -0,0 +1,366 @@
+/** @file
+
+Copyright (c) 2005 - 2015, 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 "Ip4Impl.h"
+
+IP4_ICMP_CLASS
+mIcmpClass[] = {
+  {ICMP_ECHO_REPLY,         ICMP_QUERY_MESSAGE  },
+  {1,                       ICMP_INVALID_MESSAGE},
+  {2,                       ICMP_INVALID_MESSAGE},
+  {ICMP_DEST_UNREACHABLE,   ICMP_ERROR_MESSAGE  },
+  {ICMP_SOURCE_QUENCH,      ICMP_ERROR_MESSAGE  },
+  {ICMP_REDIRECT,           ICMP_ERROR_MESSAGE  },
+  {6,                       ICMP_INVALID_MESSAGE},
+  {7,                       ICMP_INVALID_MESSAGE},
+  {ICMP_ECHO_REQUEST,       ICMP_QUERY_MESSAGE  },
+  {9,                       ICMP_INVALID_MESSAGE},
+  {10,                      ICMP_INVALID_MESSAGE},
+  {ICMP_TIME_EXCEEDED,      ICMP_ERROR_MESSAGE  },
+  {ICMP_PARAMETER_PROBLEM,  ICMP_ERROR_MESSAGE  },
+  {ICMP_TIMESTAMP ,         ICMP_QUERY_MESSAGE  },
+  {14,                      ICMP_INVALID_MESSAGE},
+  {ICMP_INFO_REQUEST ,      ICMP_QUERY_MESSAGE  },
+  {ICMP_INFO_REPLY ,        ICMP_QUERY_MESSAGE  },
+};
+
+EFI_IP4_ICMP_TYPE
+mIp4SupportedIcmp[23] = {
+  {ICMP_ECHO_REPLY,        ICMP_DEFAULT_CODE        },
+
+  {ICMP_DEST_UNREACHABLE,  ICMP_NET_UNREACHABLE     },
+  {ICMP_DEST_UNREACHABLE,  ICMP_HOST_UNREACHABLE    },
+  {ICMP_DEST_UNREACHABLE,  ICMP_PROTO_UNREACHABLE   },
+  {ICMP_DEST_UNREACHABLE,  ICMP_PORT_UNREACHABLE    },
+  {ICMP_DEST_UNREACHABLE,  ICMP_FRAGMENT_FAILED     },
+  {ICMP_DEST_UNREACHABLE,  ICMP_SOURCEROUTE_FAILED  },
+  {ICMP_DEST_UNREACHABLE,  ICMP_NET_UNKNOWN         },
+  {ICMP_DEST_UNREACHABLE,  ICMP_HOST_UNKNOWN        },
+  {ICMP_DEST_UNREACHABLE,  ICMP_SOURCE_ISOLATED     },
+  {ICMP_DEST_UNREACHABLE,  ICMP_NET_PROHIBITED      },
+  {ICMP_DEST_UNREACHABLE,  ICMP_HOST_PROHIBITED     },
+  {ICMP_DEST_UNREACHABLE,  ICMP_NET_UNREACHABLE_TOS },
+  {ICMP_DEST_UNREACHABLE,  ICMP_HOST_UNREACHABLE_TOS},
+
+  {ICMP_SOURCE_QUENCH,     ICMP_DEFAULT_CODE        },
+
+  {ICMP_REDIRECT,          ICMP_NET_REDIRECT        },
+  {ICMP_REDIRECT,          ICMP_HOST_REDIRECT       },
+  {ICMP_REDIRECT,          ICMP_NET_TOS_REDIRECT    },
+  {ICMP_REDIRECT,          ICMP_HOST_TOS_REDIRECT   },
+
+  {ICMP_ECHO_REQUEST,      ICMP_DEFAULT_CODE        },
+
+  {ICMP_TIME_EXCEEDED,     ICMP_TIMEOUT_IN_TRANSIT  },
+  {ICMP_TIME_EXCEEDED,     ICMP_TIMEOUT_REASSEMBLE  },
+
+  {ICMP_PARAMETER_PROBLEM, ICMP_DEFAULT_CODE        },
+};
+
+
+
+/**
+  Process the ICMP redirect. Find the instance then update
+  its route cache.
+  
+  All kinds of redirect is treated as host redirect as
+  specified by RFC1122 3.3.1.2:
+  "Since the subnet mask appropriate to the destination
+  address is generally not known, a Network Redirect
+  message SHOULD be treated identically to a Host Redirect
+  message;"
+
+  @param[in]  IpSb               The IP4 service binding instance that received 
+                                 the packet.
+  @param[in]  Head               The IP head of the received ICMPpacket.
+  @param[in]  Packet             The content of the ICMP redirect packet with IP
+                                 head removed.
+  @param[in]  Icmp               The buffer to store the ICMP error message if
+                                 something is wrong.
+
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid
+  @retval EFI_SUCCESS            Successfully updated the route caches
+
+**/
+EFI_STATUS
+Ip4ProcessIcmpRedirect (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet,
+  IN IP4_ICMP_ERROR_HEAD    *Icmp
+  )
+{
+  LIST_ENTRY                *Entry;
+  IP4_PROTOCOL              *Ip4Instance;
+  IP4_ROUTE_CACHE_ENTRY     *CacheEntry;
+  IP4_INTERFACE             *IpIf;
+  IP4_ADDR                  Gateway;
+  IP4_ADDR                  Src;
+  IP4_ADDR                  Dst;
+
+  //
+  // Find the interface whose IP address is the source of the
+  // orgianl IP packet.
+  //
+  IpIf    = Ip4FindInterface (IpSb, NTOHL (Icmp->IpHead.Src));
+  Gateway = NTOHL (Icmp->Fourth);
+
+  //
+  // discard the packet if the new gateway address it specifies
+  // is not on the same connected net through which the Redirect
+  // arrived. (RFC1122 3.2.2.2).
+  //
+  if ((IpIf == NULL) || !IP4_NET_EQUAL (Gateway, IpIf->Ip, IpIf->SubnetMask)) {
+    NetbufFree (Packet);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Update each IP child's route cache on the interface.
+  //
+  NET_LIST_FOR_EACH (Entry, &IpIf->IpInstances) {
+    Ip4Instance = NET_LIST_USER_STRUCT (Entry, IP4_PROTOCOL, AddrLink);
+
+    if (Ip4Instance->RouteTable == NULL) {
+      continue;
+    }
+
+    Dst = NTOHL (Icmp->IpHead.Dst);
+    Src = NTOHL (Icmp->IpHead.Src);
+    CacheEntry = Ip4FindRouteCache (Ip4Instance->RouteTable, Dst, Src);
+
+    //
+    // Only update the route cache's gateway if the source of the
+    // Redirect is the current first-hop gateway
+    //
+    if ((CacheEntry != NULL) && (NTOHL (Head->Src) == CacheEntry->NextHop)) {
+      CacheEntry->NextHop = Gateway;
+    }
+  }
+
+  NetbufFree (Packet);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Process the ICMP error packet. If it is an ICMP redirect packet,
+  update call Ip4ProcessIcmpRedirect to update the IP instance's
+  route cache, otherwise, deliver the packet to upper layer.
+
+  @param[in]  IpSb               The IP4 service that received the packet.
+  @param[in]  Head               The IP4 head of the ICMP error packet
+  @param[in]  Packet             The content of the ICMP error with IP4 head
+                                 removed.
+
+  @retval EFI_SUCCESS            The ICMP error is processed successfully.
+  @retval EFI_INVALID_PARAMETER  The packet is invalid
+  @retval Others                 Failed to process the packet.
+  
+**/
+EFI_STATUS
+Ip4ProcessIcmpError (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_ICMP_ERROR_HEAD       Icmp;
+
+  if (Packet->TotalSize < sizeof (Icmp)) {
+    NetbufFree (Packet);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NetbufCopy (Packet, 0, sizeof (Icmp), (UINT8 *) &Icmp);
+
+  //
+  // If it is an ICMP redirect error, update the route cache
+  // as RFC1122. Otherwise, demultiplex it to IP instances.
+  //
+  if (Icmp.Head.Type == ICMP_REDIRECT) {
+    return Ip4ProcessIcmpRedirect (IpSb, Head, Packet, &Icmp);
+  }
+
+  IP4_GET_CLIP_INFO (Packet)->Status = EFI_ICMP_ERROR;
+  return Ip4Demultiplex (IpSb, Head, Packet, NULL, 0);
+}
+
+
+/**
+  Replay an ICMP echo request.
+
+  @param[in]  IpSb               The IP4 service that receivd the packet
+  @param[in]  Head               The IP4 head of the ICMP error packet
+  @param[in]  Packet             The content of the ICMP error with IP4 head
+                                 removed.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource.
+  @retval EFI_SUCCESS            The ICMP Echo request is successfully answered.
+  @retval Others                 Failed to answer the ICMP echo request.
+
+**/
+EFI_STATUS
+Ip4IcmpReplyEcho (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_ICMP_QUERY_HEAD       *Icmp;
+  NET_BUF                   *Data;
+  EFI_STATUS                Status;
+  IP4_HEAD                  ReplyHead;
+
+  //
+  // make a copy the packet, it is really a bad idea to
+  // send the MNP's buffer back to MNP.
+  //
+  Data = NetbufDuplicate (Packet, NULL, IP4_MAX_HEADLEN);
+
+  if (Data == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  //
+  // Change the ICMP type to echo reply, exchange the source
+  // and destination, then send it. The source is updated to
+  // use specific destination. See RFC1122. SRR/RR option
+  // update is omitted.
+  //
+  Icmp                = (IP4_ICMP_QUERY_HEAD *) NetbufGetByte (Data, 0, NULL);
+  ASSERT (Icmp != NULL);
+  Icmp->Head.Type     = ICMP_ECHO_REPLY;
+  Icmp->Head.Checksum = 0;
+  Icmp->Head.Checksum = (UINT16) (~NetblockChecksum ((UINT8 *) Icmp, Data->TotalSize));
+
+  ReplyHead.Tos       = 0;
+  ReplyHead.Fragment  = 0;
+  ReplyHead.Ttl       = 64;
+  ReplyHead.Protocol  = EFI_IP_PROTO_ICMP;
+  ReplyHead.Src       = 0;
+
+  //
+  // Ip4Output will select a source for us
+  //
+  ReplyHead.Dst = Head->Src;
+
+  Status = Ip4Output (
+             IpSb,
+             NULL,
+             Data,
+             &ReplyHead,
+             NULL,
+             0,
+             IP4_ALLZERO_ADDRESS,
+             Ip4SysPacketSent,
+             NULL
+             );
+
+ON_EXIT:
+  NetbufFree (Packet);
+  return Status;
+}
+
+
+/**
+  Process the ICMP query message. If it is an ICMP echo
+  request, answer it. Otherwise deliver it to upper layer.
+
+  @param[in]  IpSb               The IP4 service that receivd the packet
+  @param[in]  Head               The IP4 head of the ICMP query packet
+  @param[in]  Packet             The content of the ICMP query with IP4 head
+                                 removed.
+
+  @retval EFI_INVALID_PARAMETER  The packet is invalid
+  @retval EFI_SUCCESS            The ICMP query message is processed
+  @retval Others                 Failed to process ICMP query.
+
+**/
+EFI_STATUS
+Ip4ProcessIcmpQuery (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_ICMP_QUERY_HEAD       Icmp;
+
+  if (Packet->TotalSize < sizeof (Icmp)) {
+    NetbufFree (Packet);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NetbufCopy (Packet, 0, sizeof (Icmp), (UINT8 *) &Icmp);
+
+  if (Icmp.Head.Type == ICMP_ECHO_REQUEST) {
+    return Ip4IcmpReplyEcho (IpSb, Head, Packet);
+  }
+
+  return Ip4Demultiplex (IpSb, Head, Packet, NULL, 0);
+}
+
+
+/**
+  Handle the ICMP packet. First validate the message format,
+  then according to the message types, process it as query or
+  error packet.
+
+  @param[in]  IpSb               The IP4 service that receivd the packet.
+  @param[in]  Head               The IP4 head of the ICMP query packet.
+  @param[in]  Packet             The content of the ICMP query with IP4 head
+                                 removed.
+
+  @retval EFI_INVALID_PARAMETER  The packet is malformated.
+  @retval EFI_SUCCESS            The ICMP message is successfully processed.
+  @retval Others                 Failed to handle ICMP packet.
+
+**/
+EFI_STATUS
+Ip4IcmpHandle (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_ICMP_HEAD             Icmp;
+  UINT16                    Checksum;
+
+  if (Packet->TotalSize < sizeof (Icmp)) {
+    goto DROP;
+  }
+
+  NetbufCopy (Packet, 0, sizeof (Icmp), (UINT8 *) &Icmp);
+
+  if (Icmp.Type > ICMP_TYPE_MAX) {
+    goto DROP;
+  }
+
+  Checksum = (UINT16) (~NetbufChecksum (Packet));
+  if ((Icmp.Checksum != 0) && (Checksum != 0)) {
+    goto DROP;
+  }
+
+  if (mIcmpClass[Icmp.Type].IcmpClass == ICMP_ERROR_MESSAGE) {
+    return Ip4ProcessIcmpError (IpSb, Head, Packet);
+
+  } else if (mIcmpClass[Icmp.Type].IcmpClass == ICMP_QUERY_MESSAGE) {
+    return Ip4ProcessIcmpQuery (IpSb, Head, Packet);
+
+  }
+
+DROP:
+  NetbufFree (Packet);
+  return EFI_INVALID_PARAMETER;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.h
new file mode 100644
index 0000000000..30199dced6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Icmp.h
@@ -0,0 +1,103 @@
+/** @file
+  Header file for ICMP protocol.
+  
+Copyright (c) 2005 - 2015, 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.
+
+**/
+
+#ifndef __EFI_IP4_ICMP_H__
+#define __EFI_IP4_ICMP_H__
+
+  //
+  // ICMP type definations
+  //
+#define ICMP_ECHO_REPLY            0
+#define ICMP_DEST_UNREACHABLE      3
+#define ICMP_SOURCE_QUENCH         4
+#define ICMP_REDIRECT              5
+#define ICMP_ECHO_REQUEST          8
+#define ICMP_TIME_EXCEEDED         11
+#define ICMP_PARAMETER_PROBLEM     12
+#define ICMP_TIMESTAMP             13
+#define ICMP_INFO_REQUEST          15
+#define ICMP_INFO_REPLY            16
+#define ICMP_TYPE_MAX              ICMP_INFO_REPLY
+
+#define ICMP_DEFAULT_CODE          0
+
+  //
+  // ICMP code definations for ICMP_DEST_UNREACHABLE
+  //
+#define ICMP_NET_UNREACHABLE       0
+#define ICMP_HOST_UNREACHABLE      1
+#define ICMP_PROTO_UNREACHABLE     2  // Host may generate
+#define ICMP_PORT_UNREACHABLE      3  // Host may generate
+#define ICMP_FRAGMENT_FAILED       4
+#define ICMP_SOURCEROUTE_FAILED    5  // Host may generate
+#define ICMP_NET_UNKNOWN           6
+#define ICMP_HOST_UNKNOWN          7
+#define ICMP_SOURCE_ISOLATED       8
+#define ICMP_NET_PROHIBITED        9
+#define ICMP_HOST_PROHIBITED       10
+#define ICMP_NET_UNREACHABLE_TOS   11
+#define ICMP_HOST_UNREACHABLE_TOS  12
+
+  //
+  // ICMP code definations for ICMP_TIME_EXCEEDED
+  //
+#define ICMP_TIMEOUT_IN_TRANSIT    0
+#define ICMP_TIMEOUT_REASSEMBLE    1  // Host may generate
+
+  //
+  // ICMP code definations for ICMP_TIME_EXCEEDED
+  //
+#define ICMP_NET_REDIRECT          0
+#define ICMP_HOST_REDIRECT         1
+#define ICMP_NET_TOS_REDIRECT      2
+#define ICMP_HOST_TOS_REDIRECT     3
+
+  //
+  // ICMP message classes, each class of ICMP message shares
+  // a common message format. INVALID_MESSAGE is only a flag.
+  //
+#define ICMP_INVALID_MESSAGE       0
+#define ICMP_ERROR_MESSAGE         1
+#define ICMP_QUERY_MESSAGE         2
+
+typedef struct {
+  UINT8                   IcmpType;
+  UINT8                   IcmpClass;
+} IP4_ICMP_CLASS;
+
+extern IP4_ICMP_CLASS     mIcmpClass[];
+extern EFI_IP4_ICMP_TYPE  mIp4SupportedIcmp[];
+
+/**
+  Handle the ICMP packet. First validate the message format,
+  then according to the message types, process it as query or
+  error packet.
+
+  @param[in]  IpSb               The IP4 service that receivd the packet.
+  @param[in]  Head               The IP4 head of the ICMP query packet.
+  @param[in]  Packet             The content of the ICMP query with IP4 head
+                                 removed.
+
+  @retval EFI_INVALID_PARAMETER  The packet is malformated.
+  @retval EFI_SUCCESS            The ICMP message is successfully processed.
+  @retval Others                 Failed to handle ICMP packet.
+
+**/
+EFI_STATUS
+Ip4IcmpHandle (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.c
new file mode 100644
index 0000000000..669e041c10
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.c
@@ -0,0 +1,1263 @@
+/** @file
+  Implement IP4 pesudo interface.
+  
+Copyright (c) 2005 - 2015, 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 "Ip4Impl.h"
+
+//
+// Mac address with all zero, used to determine whethter the ARP
+// resolve succeeded. Failed ARP requests zero the MAC address buffer.
+//
+EFI_MAC_ADDRESS  mZeroMacAddress;
+
+/**
+  Callback funtion when frame transmission is finished. It will
+  call the frame owner's callback function to tell it the result.
+
+  @param[in]  Context            Context which is point to the token.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameSentDpc (
+  IN VOID                    *Context
+  );
+
+/**
+  Request Ip4OnFrameSentDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event              The transmit token's event.
+  @param[in]  Context            Context which is point to the token.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameSent (
+  IN EFI_EVENT               Event,
+  IN VOID                    *Context
+  );
+
+/**
+  Callback function when ARP request are finished. It will cancelled
+  all the queued frame if the ARP requests failed. Or transmit them
+  if the request succeed.
+
+  @param[in]  Context           The context of the callback, a point to the ARP
+                                queue
+
+**/
+VOID
+EFIAPI
+Ip4OnArpResolvedDpc (
+  IN VOID                   *Context
+  );
+
+/**
+  Request Ip4OnArpResolvedDpc as a DPC at TPL_CALLBACK.
+
+  @param  Event             The Arp request event.
+  @param  Context           The context of the callback, a point to the ARP
+                            queue.
+                                
+**/
+VOID
+EFIAPI
+Ip4OnArpResolved (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  );
+
+/**
+  Received a frame from MNP, wrap it in net buffer then deliver
+  it to IP's input function. The ownship of the packet also
+  transferred to IP. When Ip is finished with this packet, it
+  will call NetbufFree to release the packet, NetbufFree will
+  again call the Ip4RecycleFrame to signal MNP's event and free
+  the token used.
+
+  @param  Context               Context for the callback.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameReceivedDpc (
+  IN VOID                     *Context
+  );
+
+/**
+  Request Ip4OnFrameReceivedDpc as a DPC at TPL_CALLBACK.
+
+  @param Event      The receive event delivered to MNP for receive.
+  @param Context    Context for the callback.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameReceived (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  );
+
+/**
+  Remove all the frames on the ARP queue that pass the FrameToCancel,
+  that is, either FrameToCancel is NULL or it returns true for the frame.
+
+  @param[in]  ArpQue            ARP frame to remove the frames from.
+  @param[in]  IoStatus          The status returned to the cancelled frames'
+                                callback function.
+  @param[in]  FrameToCancel     Function to select which frame to cancel.
+  @param[in]  Context           Opaque parameter to the FrameToCancel.
+
+**/
+VOID
+Ip4CancelFrameArp (
+  IN IP4_ARP_QUE            *ArpQue,
+  IN EFI_STATUS             IoStatus,
+  IN IP4_FRAME_TO_CANCEL    FrameToCancel  OPTIONAL,
+  IN VOID                   *Context
+  );
+
+
+/**
+  Wrap a transmit request into a newly allocated IP4_LINK_TX_TOKEN.
+
+  @param[in]  Interface         The interface to send out to.
+  @param[in]  IpInstance        The IpInstance that transmit the packet.  NULL if
+                                the packet is sent by the IP4 driver itself.
+  @param[in]  Packet            The packet to transmit
+  @param[in]  CallBack          Call back function to execute if transmission
+                                finished.
+  @param[in]  Context           Opaque parameter to the call back.
+
+  @retval   Token               The wrapped token if succeed 
+  @retval   NULL                The wrapped token if NULL
+
+**/
+IP4_LINK_TX_TOKEN *
+Ip4WrapLinkTxToken (
+  IN IP4_INTERFACE          *Interface,
+  IN IP4_PROTOCOL           *IpInstance     OPTIONAL,
+  IN NET_BUF                *Packet,
+  IN IP4_FRAME_CALLBACK     CallBack,
+  IN VOID                   *Context
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *MnpToken;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA     *MnpTxData;
+  IP4_LINK_TX_TOKEN                     *Token;
+  EFI_STATUS                            Status;
+  UINT32                                Count;
+
+  Token = AllocatePool (sizeof (IP4_LINK_TX_TOKEN) + \
+            (Packet->BlockOpNum - 1) * sizeof (EFI_MANAGED_NETWORK_FRAGMENT_DATA));
+
+  if (Token == NULL) {
+    return NULL;
+  }
+
+  Token->Signature = IP4_FRAME_TX_SIGNATURE;
+  InitializeListHead (&Token->Link);
+
+  Token->Interface  = Interface;
+  Token->IpInstance = IpInstance;
+  Token->CallBack   = CallBack;
+  Token->Packet     = Packet;
+  Token->Context    = Context;
+  CopyMem (&Token->DstMac, &mZeroMacAddress, sizeof (Token->DstMac));
+  CopyMem (&Token->SrcMac, &Interface->Mac, sizeof (Token->SrcMac));
+
+  MnpToken          = &(Token->MnpToken);
+  MnpToken->Status  = EFI_NOT_READY;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Ip4OnFrameSent,
+                  Token,
+                  &MnpToken->Event
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Token);
+    return NULL;
+  }
+
+  MnpTxData                     = &Token->MnpTxData;
+  MnpToken->Packet.TxData       = MnpTxData;
+
+  MnpTxData->DestinationAddress = &Token->DstMac;
+  MnpTxData->SourceAddress      = &Token->SrcMac;
+  MnpTxData->ProtocolType       = IP4_ETHER_PROTO;
+  MnpTxData->DataLength         = Packet->TotalSize;
+  MnpTxData->HeaderLength       = 0;
+
+  Count                         = Packet->BlockOpNum;
+
+  NetbufBuildExt (Packet, (NET_FRAGMENT *) MnpTxData->FragmentTable, &Count);
+  MnpTxData->FragmentCount      = (UINT16)Count;
+
+  return Token;
+}
+
+
+/**
+  Free the link layer transmit token. It will close the event
+  then free the memory used.
+
+  @param[in]  Token                 Token to free
+
+**/
+VOID
+Ip4FreeLinkTxToken (
+  IN IP4_LINK_TX_TOKEN      *Token
+  )
+{
+  NET_CHECK_SIGNATURE (Token, IP4_FRAME_TX_SIGNATURE);
+
+  gBS->CloseEvent (Token->MnpToken.Event);
+  FreePool (Token);
+}
+
+
+/**
+  Create an IP_ARP_QUE structure to request ARP service.
+
+  @param[in]  Interface         The interface to send ARP from.
+  @param[in]  DestIp            The destination IP (host byte order) to request MAC
+                                for
+
+  @return Point to newly created IP4_ARP_QUE if succeed, otherwise NULL.
+
+**/
+IP4_ARP_QUE *
+Ip4CreateArpQue (
+  IN IP4_INTERFACE          *Interface,
+  IN IP4_ADDR               DestIp
+  )
+{
+  IP4_ARP_QUE               *ArpQue;
+  EFI_STATUS                Status;
+
+  ArpQue = AllocatePool (sizeof (IP4_ARP_QUE));
+
+  if (ArpQue == NULL) {
+    return NULL;
+  }
+
+  ArpQue->Signature = IP4_FRAME_ARP_SIGNATURE;
+  InitializeListHead (&ArpQue->Link);
+
+  InitializeListHead (&ArpQue->Frames);
+  ArpQue->Interface = Interface;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Ip4OnArpResolved,
+                  ArpQue,
+                  &ArpQue->OnResolved
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (ArpQue);
+    return NULL;
+  }
+
+  ArpQue->Ip  = DestIp;
+  CopyMem (&ArpQue->Mac, &mZeroMacAddress, sizeof (ArpQue->Mac));
+
+  return ArpQue;
+}
+
+
+/**
+  Remove all the transmit requests queued on the ARP queue, then free it.
+
+  @param[in]  ArpQue            Arp queue to free
+  @param[in]  IoStatus          The transmit status returned to transmit requests'
+                                callback.
+
+**/
+VOID
+Ip4FreeArpQue (
+  IN IP4_ARP_QUE            *ArpQue,
+  IN EFI_STATUS             IoStatus
+  )
+{
+  NET_CHECK_SIGNATURE (ArpQue, IP4_FRAME_ARP_SIGNATURE);
+
+  //
+  // Remove all the frame waiting the ARP response
+  //
+  Ip4CancelFrameArp (ArpQue, IoStatus, NULL, NULL);
+
+  gBS->CloseEvent (ArpQue->OnResolved);
+  FreePool (ArpQue);
+}
+
+
+/**
+  Create a link layer receive token to wrap the receive request
+
+  @param[in]  Interface         The interface to receive from
+  @param[in]  IpInstance        The instance that request the receive (NULL for IP4
+                                driver itself)
+  @param[in]  CallBack          Call back function to execute when finished.
+  @param[in]  Context           Opaque parameters to the callback
+
+  @return Point to created IP4_LINK_RX_TOKEN if succeed, otherwise NULL.
+
+**/
+IP4_LINK_RX_TOKEN *
+Ip4CreateLinkRxToken (
+  IN IP4_INTERFACE          *Interface,
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_FRAME_CALLBACK     CallBack,
+  IN VOID                   *Context
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *MnpToken;
+  IP4_LINK_RX_TOKEN                     *Token;
+  EFI_STATUS                            Status;
+
+  Token = AllocatePool (sizeof (IP4_LINK_RX_TOKEN));
+  if (Token == NULL) {
+    return NULL;
+  }
+
+  Token->Signature  = IP4_FRAME_RX_SIGNATURE;
+  Token->Interface  = Interface;
+  Token->IpInstance = IpInstance;
+  Token->CallBack   = CallBack;
+  Token->Context    = Context;
+
+  MnpToken          = &Token->MnpToken;
+  MnpToken->Status  = EFI_NOT_READY;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Ip4OnFrameReceived,
+                  Token,
+                  &MnpToken->Event
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Token);
+    return NULL;
+  }
+
+  MnpToken->Packet.RxData = NULL;
+  return Token;
+}
+
+
+/**
+  Free the link layer request token. It will close the event
+  then free the memory used.
+
+  @param[in]  Token                 Request token to free.
+
+**/
+VOID
+Ip4FreeFrameRxToken (
+  IN IP4_LINK_RX_TOKEN      *Token
+  )
+{
+
+  NET_CHECK_SIGNATURE (Token, IP4_FRAME_RX_SIGNATURE);
+
+  gBS->CloseEvent (Token->MnpToken.Event);
+  FreePool (Token);
+}
+
+
+/**
+  Remove all the frames on the ARP queue that pass the FrameToCancel,
+  that is, either FrameToCancel is NULL or it returns true for the frame.
+
+  @param[in]  ArpQue            ARP frame to remove the frames from.
+  @param[in]  IoStatus          The status returned to the cancelled frames'
+                                callback function.
+  @param[in]  FrameToCancel     Function to select which frame to cancel.
+  @param[in]  Context           Opaque parameter to the FrameToCancel.
+
+**/
+VOID
+Ip4CancelFrameArp (
+  IN IP4_ARP_QUE            *ArpQue,
+  IN EFI_STATUS             IoStatus,
+  IN IP4_FRAME_TO_CANCEL    FrameToCancel  OPTIONAL,
+  IN VOID                   *Context
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_LINK_TX_TOKEN         *Token;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &ArpQue->Frames) {
+    Token = NET_LIST_USER_STRUCT (Entry, IP4_LINK_TX_TOKEN, Link);
+
+    if ((FrameToCancel == NULL) || FrameToCancel (Token, Context)) {
+      RemoveEntryList (Entry);
+
+      Token->CallBack (Token->IpInstance, Token->Packet, IoStatus, 0, Token->Context);
+      Ip4FreeLinkTxToken (Token);
+    }
+  }
+}
+
+
+/**
+  Remove all the frames on the interface that pass the FrameToCancel,
+  either queued on ARP queues or that have already been delivered to
+  MNP and not yet recycled.
+
+  @param[in]  Interface         Interface to remove the frames from.
+  @param[in]  IoStatus          The transmit status returned to the frames'
+                                callback.
+  @param[in]  FrameToCancel     Function to select the frame to cancel, NULL to
+                                select all.
+  @param[in]  Context           Opaque parameters passed to FrameToCancel.
+
+**/
+VOID
+Ip4CancelFrames (
+  IN IP4_INTERFACE          *Interface,
+  IN EFI_STATUS             IoStatus,
+  IN IP4_FRAME_TO_CANCEL    FrameToCancel    OPTIONAL,
+  IN VOID                   *Context
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ARP_QUE               *ArpQue;
+  IP4_LINK_TX_TOKEN         *Token;
+
+  //
+  // Cancel all the pending frames on ARP requests
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Interface->ArpQues) {
+    ArpQue = NET_LIST_USER_STRUCT (Entry, IP4_ARP_QUE, Link);
+
+    Ip4CancelFrameArp (ArpQue, IoStatus, FrameToCancel, Context);
+
+    if (IsListEmpty (&ArpQue->Frames)) {
+      Interface->Arp->Cancel (Interface->Arp, &ArpQue->Ip, ArpQue->OnResolved);
+    }
+  }
+
+  //
+  // Cancel all the frames that have been delivered to MNP
+  // but not yet recycled.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Interface->SentFrames) {
+    Token = NET_LIST_USER_STRUCT (Entry, IP4_LINK_TX_TOKEN, Link);
+
+    if ((FrameToCancel == NULL) || FrameToCancel (Token, Context)) {
+      Interface->Mnp->Cancel (Interface->Mnp, &Token->MnpToken);
+    }
+  }
+}
+
+
+/**
+  Create an IP4_INTERFACE. Delay the creation of ARP instance until
+  the interface is configured.
+
+  @param[in]  Mnp               The shared MNP child of this IP4 service binding
+                                instance.
+  @param[in]  Controller        The controller this IP4 service binding instance
+                                is installed. Most like the UNDI handle.
+  @param[in]  ImageHandle       This driver's image handle.
+
+  @return Point to the created IP4_INTERFACE, otherwise NULL.
+
+**/
+IP4_INTERFACE *
+Ip4CreateInterface (
+  IN  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp,
+  IN  EFI_HANDLE                    Controller,
+  IN  EFI_HANDLE                    ImageHandle
+  )
+{
+  IP4_INTERFACE             *Interface;
+  EFI_SIMPLE_NETWORK_MODE   SnpMode;
+
+  Interface = AllocatePool (sizeof (IP4_INTERFACE));
+
+  if ((Interface == NULL) || (Mnp == NULL)) {
+    return NULL;
+  }
+
+  Interface->Signature = IP4_INTERFACE_SIGNATURE;
+  InitializeListHead (&Interface->Link);
+  Interface->RefCnt     = 1;
+
+  Interface->Ip         = IP4_ALLZERO_ADDRESS;
+  Interface->SubnetMask = IP4_ALLZERO_ADDRESS;
+  Interface->Configured = FALSE;
+
+  Interface->Controller = Controller;
+  Interface->Image      = ImageHandle;
+  Interface->Mnp        = Mnp;
+  Interface->Arp        = NULL;
+  Interface->ArpHandle  = NULL;
+
+  InitializeListHead (&Interface->ArpQues);
+  InitializeListHead (&Interface->SentFrames);
+
+  Interface->RecvRequest = NULL;
+
+  //
+  // Get the interface's Mac address and broadcast mac address from SNP
+  //
+  if (EFI_ERROR (Mnp->GetModeData (Mnp, NULL, &SnpMode))) {
+    FreePool (Interface);
+    return NULL;
+  }
+
+  CopyMem (&Interface->Mac, &SnpMode.CurrentAddress, sizeof (Interface->Mac));
+  CopyMem (&Interface->BroadcastMac, &SnpMode.BroadcastAddress, sizeof (Interface->BroadcastMac));
+  Interface->HwaddrLen    = SnpMode.HwAddressSize;
+
+  InitializeListHead (&Interface->IpInstances);
+  Interface->PromiscRecv = FALSE;
+
+  return Interface;
+}
+
+
+/**
+  Set the interface's address, create and configure
+  the ARP child if necessary.
+
+  @param  Interface         The interface to set the address.
+  @param  IpAddr            The interface's IP address.
+  @param  SubnetMask        The interface's netmask.
+
+  @retval EFI_SUCCESS           The interface is configured with Ip/netmask pair,
+                                and a ARP is created for it.
+  @retval Others                Failed to set the interface's address.
+
+**/
+EFI_STATUS
+Ip4SetAddress (
+  IN OUT IP4_INTERFACE      *Interface,
+  IN     IP4_ADDR           IpAddr,
+  IN     IP4_ADDR           SubnetMask
+  )
+{
+  EFI_ARP_CONFIG_DATA       ArpConfig;
+  EFI_STATUS                Status;
+  INTN                      Type;
+  INTN                      Len;
+  IP4_ADDR                  Netmask;
+
+  NET_CHECK_SIGNATURE (Interface, IP4_INTERFACE_SIGNATURE);
+
+  //
+  // Set the ip/netmask, then compute the subnet broadcast
+  // and network broadcast for easy access. When computing
+  // nework broadcast, the subnet mask is most like longer
+  // than the default netmask (not subneted) as defined in
+  // RFC793. If that isn't the case, we are aggregating the
+  // networks, use the subnet's mask instead.
+  //
+  Interface->Ip             = IpAddr;
+  Interface->SubnetMask     = SubnetMask;
+  Interface->SubnetBrdcast  = (IpAddr | ~SubnetMask);
+
+  Type                      = NetGetIpClass (IpAddr);
+  ASSERT (Type <= IP4_ADDR_CLASSC);
+  Len                       = NetGetMaskLength (SubnetMask);
+  ASSERT (Len < IP4_MASK_NUM);
+  Netmask                   = gIp4AllMasks[MIN (Len, Type << 3)];
+  Interface->NetBrdcast     = (IpAddr | ~Netmask);
+
+  //
+  // Do clean up for Arp child
+  //
+  if (Interface->ArpHandle != NULL) {
+    if (Interface->Arp != NULL) {
+      gBS->CloseProtocol (
+             Interface->ArpHandle,
+             &gEfiArpProtocolGuid,
+             Interface->Image,
+             Interface->Controller
+             );
+      
+      Interface->Arp = NULL;
+    }
+    
+    NetLibDestroyServiceChild (
+      Interface->Controller,
+      Interface->Image,
+      &gEfiArpServiceBindingProtocolGuid,
+      &Interface->ArpHandle
+      );
+
+    Interface->ArpHandle = NULL;
+  }
+
+  //
+  // If the address is NOT all zero, create then configure an ARP child.
+  // Pay attention: DHCP configures its station address as 0.0.0.0/0
+  //
+  if (IpAddr != IP4_ALLZERO_ADDRESS) {
+    Status = NetLibCreateServiceChild (
+               Interface->Controller,
+               Interface->Image,
+               &gEfiArpServiceBindingProtocolGuid,
+               &Interface->ArpHandle
+               );
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = gBS->OpenProtocol (
+                    Interface->ArpHandle,
+                    &gEfiArpProtocolGuid,
+                    (VOID **) &Interface->Arp,
+                    Interface->Image,
+                    Interface->Controller,
+                    EFI_OPEN_PROTOCOL_BY_DRIVER
+                    );
+
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+
+    IpAddr                    = HTONL (IpAddr);
+    ArpConfig.SwAddressType   = IP4_ETHER_PROTO;
+    ArpConfig.SwAddressLength = 4;
+    ArpConfig.StationAddress  = &IpAddr;
+    ArpConfig.EntryTimeOut    = 0;
+    ArpConfig.RetryCount      = 0;
+    ArpConfig.RetryTimeOut    = 0;
+
+    Status = Interface->Arp->Configure (Interface->Arp, &ArpConfig);
+
+    if (EFI_ERROR (Status)) {
+      gBS->CloseProtocol (
+             Interface->ArpHandle,
+             &gEfiArpProtocolGuid,
+             Interface->Image,
+             Interface->Controller
+             );
+
+      goto ON_ERROR;
+    }
+  }
+
+  Interface->Configured = TRUE;
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  NetLibDestroyServiceChild (
+    Interface->Controller,
+    Interface->Image,
+    &gEfiArpServiceBindingProtocolGuid,
+    &Interface->ArpHandle
+    );
+
+  return Status;
+}
+
+
+/**
+  Filter function to cancel all the frame related to an IP instance.
+
+  @param[in]  Frame             The transmit request to test whether to cancel
+  @param[in]  Context           The context which is the Ip instance that issued
+                                the transmit.
+
+  @retval TRUE                  The frame belongs to this instance and is to be
+                                removed
+  @retval FALSE                 The frame doesn't belong to this instance.
+
+**/
+BOOLEAN
+Ip4CancelInstanceFrame (
+  IN IP4_LINK_TX_TOKEN *Frame,
+  IN VOID              *Context
+  )
+{
+  if (Frame->IpInstance == (IP4_PROTOCOL *) Context) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+
+/**
+  If there is a pending receive request, cancel it. Don't call
+  the receive request's callback because this function can be only
+  called if the instance or driver is tearing itself down. It
+  doesn't make sense to call it back. But it is necessary to call
+  the transmit token's callback to give it a chance to free the
+  packet and update the upper layer's transmit request status, say
+  that from the UDP.
+
+  @param[in]  Interface         The interface used by the IpInstance
+
+**/
+VOID
+Ip4CancelReceive (
+  IN IP4_INTERFACE          *Interface
+  )
+{
+  EFI_TPL                   OldTpl;
+  IP4_LINK_RX_TOKEN         *Token;
+
+  if ((Token = Interface->RecvRequest) != NULL) {
+    OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+    Interface->RecvRequest = NULL;
+    Interface->Mnp->Cancel (Interface->Mnp, &Token->MnpToken);
+
+    gBS->RestoreTPL (OldTpl);
+  }
+}
+
+
+/**
+  Free the interface used by IpInstance. All the IP instance with
+  the same Ip/Netmask pair share the same interface. It is reference
+  counted. All the frames haven't been sent will be cancelled.
+  Because the IpInstance is optional, the caller must remove
+  IpInstance from the interface's instance list itself.
+
+  @param[in]  Interface         The interface used by the IpInstance.
+  @param[in]  IpInstance        The Ip instance that free the interface. NULL if
+                                the Ip driver is releasing the default interface.
+
+  @retval EFI_SUCCESS           The interface use IpInstance is freed.
+
+**/
+EFI_STATUS
+Ip4FreeInterface (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance           OPTIONAL
+  )
+{
+  NET_CHECK_SIGNATURE (Interface, IP4_INTERFACE_SIGNATURE);
+  ASSERT (Interface->RefCnt > 0);
+
+  //
+  // Remove all the pending transmit token related to this IP instance.
+  //
+  Ip4CancelFrames (Interface, EFI_ABORTED, Ip4CancelInstanceFrame, IpInstance);
+
+  if (--Interface->RefCnt > 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Destroy the interface if this is the last IP instance that
+  // has the address. Remove all the system transmitted packets
+  // from this interface, cancel the receive request if there is
+  // one, and destroy the ARP requests.
+  //
+  Ip4CancelFrames (Interface, EFI_ABORTED, Ip4CancelInstanceFrame, NULL);
+  Ip4CancelReceive (Interface);
+
+  ASSERT (IsListEmpty (&Interface->IpInstances));
+  ASSERT (IsListEmpty (&Interface->ArpQues));
+  ASSERT (IsListEmpty (&Interface->SentFrames));
+
+  if (Interface->Arp != NULL) {
+    gBS->CloseProtocol (
+          Interface->ArpHandle,
+          &gEfiArpProtocolGuid,
+          Interface->Image,
+          Interface->Controller
+          );
+
+    NetLibDestroyServiceChild (
+      Interface->Controller,
+      Interface->Image,
+      &gEfiArpServiceBindingProtocolGuid,
+      Interface->ArpHandle
+      );
+  }
+
+  RemoveEntryList (&Interface->Link);
+  FreePool (Interface);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Callback function when ARP request are finished. It will cancelled
+  all the queued frame if the ARP requests failed. Or transmit them
+  if the request succeed.
+
+  @param[in]  Context           The context of the callback, a point to the ARP
+                                queue
+
+**/
+VOID
+EFIAPI
+Ip4OnArpResolvedDpc (
+  IN VOID                   *Context
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ARP_QUE               *ArpQue;
+  IP4_INTERFACE             *Interface;
+  IP4_LINK_TX_TOKEN         *Token;
+  EFI_STATUS                Status;
+
+  ArpQue = (IP4_ARP_QUE *) Context;
+  NET_CHECK_SIGNATURE (ArpQue, IP4_FRAME_ARP_SIGNATURE);
+
+  RemoveEntryList (&ArpQue->Link);
+
+  //
+  // ARP resolve failed for some reason. Release all the frame
+  // and ARP queue itself. Ip4FreeArpQue will call the frame's
+  // owner back.
+  //
+  if (NET_MAC_EQUAL (&ArpQue->Mac, &mZeroMacAddress, ArpQue->Interface->HwaddrLen)) {
+    Ip4FreeArpQue (ArpQue, EFI_NO_MAPPING);
+
+    return ;
+  }
+
+  //
+  // ARP resolve succeeded, Transmit all the frame. Release the ARP
+  // queue. It isn't necessary for us to cache the ARP binding because
+  // we always check the ARP cache first before transmit.
+  //
+  Interface = ArpQue->Interface;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &ArpQue->Frames) {
+    RemoveEntryList (Entry);
+
+    Token         = NET_LIST_USER_STRUCT (Entry, IP4_LINK_TX_TOKEN, Link);
+    CopyMem (&Token->DstMac, &ArpQue->Mac, sizeof (Token->DstMac));
+
+    //
+    // Insert the tx token before transmitting it via MNP as the FrameSentDpc
+    // may be called before Mnp->Transmit returns which will remove this tx
+    // token from the SentFrames list. Remove it from the list if the returned
+    // Status of Mnp->Transmit is not EFI_SUCCESS as in this case the
+    // FrameSentDpc won't be queued.
+    //
+    InsertTailList (&Interface->SentFrames, &Token->Link);
+
+    Status = Interface->Mnp->Transmit (Interface->Mnp, &Token->MnpToken);
+    if (EFI_ERROR (Status)) {
+      RemoveEntryList (Entry);
+      Token->CallBack (Token->IpInstance, Token->Packet, Status, 0, Token->Context);
+
+      Ip4FreeLinkTxToken (Token);
+      continue;
+    }
+  }
+
+  Ip4FreeArpQue (ArpQue, EFI_SUCCESS);
+}
+
+/**
+  Request Ip4OnArpResolvedDpc as a DPC at TPL_CALLBACK.
+
+  @param  Event             The Arp request event.
+  @param  Context           The context of the callback, a point to the ARP
+                            queue.
+                                
+**/
+VOID
+EFIAPI
+Ip4OnArpResolved (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  //
+  // Request Ip4OnArpResolvedDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, Ip4OnArpResolvedDpc, Context);
+}
+
+
+
+/**
+  Callback funtion when frame transmission is finished. It will
+  call the frame owner's callback function to tell it the result.
+
+  @param[in]  Context            Context which is point to the token.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameSentDpc (
+  IN VOID                    *Context
+  )
+{
+  IP4_LINK_TX_TOKEN         *Token;
+
+  Token = (IP4_LINK_TX_TOKEN *) Context;
+  NET_CHECK_SIGNATURE (Token, IP4_FRAME_TX_SIGNATURE);
+
+  RemoveEntryList (&Token->Link);
+
+  Token->CallBack (
+          Token->IpInstance,
+          Token->Packet,
+          Token->MnpToken.Status,
+          0,
+          Token->Context
+          );
+
+  Ip4FreeLinkTxToken (Token);
+}
+
+/**
+  Request Ip4OnFrameSentDpc as a DPC at TPL_CALLBACK.
+
+  @param[in]  Event              The transmit token's event.
+  @param[in]  Context            Context which is point to the token.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameSent (
+  IN EFI_EVENT               Event,
+  IN VOID                    *Context
+  )
+{
+  //
+  // Request Ip4OnFrameSentDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, Ip4OnFrameSentDpc, Context);
+}
+
+
+
+/**
+  Send a frame from the interface. If the next hop is broadcast or
+  multicast address, it is transmitted immediately. If the next hop
+  is a unicast, it will consult ARP to resolve the NextHop's MAC.
+  If some error happened, the CallBack won't be called. So, the caller
+  must test the return value, and take action when there is an error.
+
+  @param[in]  Interface         The interface to send the frame from
+  @param[in]  IpInstance        The IP child that request the transmission.  NULL
+                                if it is the IP4 driver itself.
+  @param[in]  Packet            The packet to transmit.
+  @param[in]  NextHop           The immediate destination to transmit the packet
+                                to.
+  @param[in]  CallBack          Function to call back when transmit finished.
+  @param[in]  Context           Opaque parameter to the call back.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to send the frame
+  @retval EFI_NO_MAPPING        Can't resolve the MAC for the nexthop
+  @retval EFI_SUCCESS           The packet is successfully transmitted.
+  @retval other                 Other error occurs.
+
+**/
+EFI_STATUS
+Ip4SendFrame (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance       OPTIONAL,
+  IN  NET_BUF               *Packet,
+  IN  IP4_ADDR              NextHop,
+  IN  IP4_FRAME_CALLBACK    CallBack,
+  IN  VOID                  *Context
+  )
+{
+  IP4_LINK_TX_TOKEN         *Token;
+  LIST_ENTRY                *Entry;
+  IP4_ARP_QUE               *ArpQue;
+  EFI_ARP_PROTOCOL          *Arp;
+  EFI_STATUS                Status;
+
+  ASSERT (Interface->Configured);
+
+  Token = Ip4WrapLinkTxToken (Interface, IpInstance, Packet, CallBack, Context);
+
+  if (Token == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Get the destination MAC address for multicast and broadcasts.
+  // Don't depend on ARP to solve the address since there maybe no
+  // ARP at all. Ip4Output has set NextHop to 255.255.255.255 for
+  // all the broadcasts.
+  //
+  if (NextHop == IP4_ALLONE_ADDRESS) {
+    CopyMem (&Token->DstMac, &Interface->BroadcastMac, sizeof (Token->DstMac));
+    goto SEND_NOW;
+
+  } else if (IP4_IS_MULTICAST (NextHop)) {
+
+    Status = Ip4GetMulticastMac (Interface->Mnp, NextHop, &Token->DstMac);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+
+    goto SEND_NOW;
+  }
+
+  //
+  // Can only send out multicast/broadcast if the IP address is zero
+  //
+  if ((Arp = Interface->Arp) == NULL) {
+    Status = EFI_NO_MAPPING;
+    goto ON_ERROR;
+  }
+
+  //
+  // First check whether this binding is in the ARP cache.
+  //
+  NextHop = HTONL (NextHop);
+  Status  = Arp->Request (Arp, &NextHop, NULL, &Token->DstMac);
+
+  if (Status == EFI_SUCCESS) {
+    goto SEND_NOW;
+
+  } else if (Status != EFI_NOT_READY) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Have to do asynchronous ARP resolution. First check
+  // whether there is already a pending request.
+  //
+  ArpQue = NULL;
+
+  NET_LIST_FOR_EACH (Entry, &Interface->ArpQues) {
+    ArpQue = NET_LIST_USER_STRUCT (Entry, IP4_ARP_QUE, Link);
+
+    if (ArpQue->Ip == NextHop) {
+      break;
+    }
+  }
+
+  //
+  // Found a pending ARP request, enqueue the frame then return
+  //
+  if (Entry != &Interface->ArpQues) {
+    InsertTailList (&ArpQue->Frames, &Token->Link);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // First frame to NextHop, issue an asynchronous ARP requests
+  //
+  ArpQue = Ip4CreateArpQue (Interface, NextHop);
+
+  if (ArpQue == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  Status = Arp->Request (Arp, &ArpQue->Ip, ArpQue->OnResolved, ArpQue->Mac.Addr);
+
+  if (EFI_ERROR (Status) && (Status != EFI_NOT_READY)) {
+    Ip4FreeArpQue (ArpQue, EFI_NO_MAPPING);
+    goto ON_ERROR;
+  }
+
+  InsertHeadList (&ArpQue->Frames, &Token->Link);
+  InsertHeadList (&Interface->ArpQues, &ArpQue->Link);
+  return EFI_SUCCESS;
+
+SEND_NOW:
+  //
+  // Insert the tx token into the SentFrames list before calling Mnp->Transmit.
+  // Remove it if the returned status is not EFI_SUCCESS.
+  //
+  InsertTailList (&Interface->SentFrames, &Token->Link);
+  Status = Interface->Mnp->Transmit (Interface->Mnp, &Token->MnpToken);
+  if (EFI_ERROR (Status)) {
+    RemoveEntryList (&Interface->SentFrames);
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  Ip4FreeLinkTxToken (Token);
+  return Status;
+}
+
+
+/**
+  Call back function when the received packet is freed.
+  Check Ip4OnFrameReceived for information.
+
+  @param  Context          Context, which is the IP4_LINK_RX_TOKEN.
+
+**/
+VOID
+EFIAPI
+Ip4RecycleFrame (
+  IN VOID                   *Context
+  )
+{
+  IP4_LINK_RX_TOKEN         *Frame;
+
+  Frame = (IP4_LINK_RX_TOKEN *) Context;
+  NET_CHECK_SIGNATURE (Frame, IP4_FRAME_RX_SIGNATURE);
+
+  gBS->SignalEvent (Frame->MnpToken.Packet.RxData->RecycleEvent);
+  Ip4FreeFrameRxToken (Frame);
+}
+
+
+/**
+  Received a frame from MNP, wrap it in net buffer then deliver
+  it to IP's input function. The ownship of the packet also
+  transferred to IP. When Ip is finished with this packet, it
+  will call NetbufFree to release the packet, NetbufFree will
+  again call the Ip4RecycleFrame to signal MNP's event and free
+  the token used.
+
+  @param  Context               Context for the callback.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameReceivedDpc (
+  IN VOID                     *Context
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *MnpToken;
+  EFI_MANAGED_NETWORK_RECEIVE_DATA      *MnpRxData;
+  IP4_LINK_RX_TOKEN                     *Token;
+  NET_FRAGMENT                          Netfrag;
+  NET_BUF                               *Packet;
+  UINT32                                Flag;
+
+  Token = (IP4_LINK_RX_TOKEN *) Context;
+  NET_CHECK_SIGNATURE (Token, IP4_FRAME_RX_SIGNATURE);
+
+  //
+  // First clear the interface's receive request in case the
+  // caller wants to call Ip4ReceiveFrame in the callback.
+  //
+  Token->Interface->RecvRequest = NULL;
+
+  MnpToken  = &Token->MnpToken;
+  MnpRxData = MnpToken->Packet.RxData;
+
+  if (EFI_ERROR (MnpToken->Status) || (MnpRxData == NULL)) {
+    Token->CallBack (Token->IpInstance, NULL, MnpToken->Status, 0, Token->Context);
+    Ip4FreeFrameRxToken (Token);
+
+    return ;
+  }
+
+  //
+  // Wrap the frame in a net buffer then deliever it to IP input.
+  // IP will reassemble the packet, and deliver it to upper layer
+  //
+  Netfrag.Len  = MnpRxData->DataLength;
+  Netfrag.Bulk = MnpRxData->PacketData;
+
+  Packet = NetbufFromExt (&Netfrag, 1, 0, IP4_MAX_HEADLEN, Ip4RecycleFrame, Token);
+
+  if (Packet == NULL) {
+    gBS->SignalEvent (MnpRxData->RecycleEvent);
+
+    Token->CallBack (Token->IpInstance, NULL, EFI_OUT_OF_RESOURCES, 0, Token->Context);
+    Ip4FreeFrameRxToken (Token);
+
+    return ;
+  }
+
+  Flag  = (MnpRxData->BroadcastFlag ? IP4_LINK_BROADCAST : 0);
+  Flag |= (MnpRxData->MulticastFlag ? IP4_LINK_MULTICAST : 0);
+  Flag |= (MnpRxData->PromiscuousFlag ? IP4_LINK_PROMISC : 0);
+
+  Token->CallBack (Token->IpInstance, Packet, EFI_SUCCESS, Flag, Token->Context);
+}
+
+/**
+  Request Ip4OnFrameReceivedDpc as a DPC at TPL_CALLBACK.
+
+  @param Event      The receive event delivered to MNP for receive.
+  @param Context    Context for the callback.
+
+**/
+VOID
+EFIAPI
+Ip4OnFrameReceived (
+  IN EFI_EVENT                Event,
+  IN VOID                     *Context
+  )
+{
+  //
+  // Request Ip4OnFrameReceivedDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, Ip4OnFrameReceivedDpc, Context);
+}
+
+
+/**
+  Request to receive the packet from the interface.
+
+  @param[in]  Interface         The interface to receive the frames from.
+  @param[in]  IpInstance        The instance that requests the receive. NULL for
+                                the driver itself.
+  @param[in]  CallBack          Function to call when receive finished.
+  @param[in]  Context           Opaque parameter to the callback.
+
+  @retval EFI_ALREADY_STARTED   There is already a pending receive request.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to receive.
+  @retval EFI_SUCCESS           The recieve request has been started.
+  @retval other                 Other error occurs.
+
+**/
+EFI_STATUS
+Ip4ReceiveFrame (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance       OPTIONAL,
+  IN  IP4_FRAME_CALLBACK    CallBack,
+  IN  VOID                  *Context
+  )
+{
+  IP4_LINK_RX_TOKEN *Token;
+  EFI_STATUS        Status;
+
+  NET_CHECK_SIGNATURE (Interface, IP4_INTERFACE_SIGNATURE);
+
+  if (Interface->RecvRequest != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  Token = Ip4CreateLinkRxToken (Interface, IpInstance, CallBack, Context);
+
+  if (Token == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Interface->RecvRequest = Token;
+  Status = Interface->Mnp->Receive (Interface->Mnp, &Token->MnpToken);
+  if (EFI_ERROR (Status)) {
+    Interface->RecvRequest = NULL;
+    Ip4FreeFrameRxToken (Token);
+    return Status;
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.h
new file mode 100644
index 0000000000..6dada9605b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4If.h
@@ -0,0 +1,343 @@
+/** @file
+  Definition for IP4 pesudo interface structure.
+  
+Copyright (c) 2005 - 2015, 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.
+
+**/
+
+#ifndef __EFI_IP4_IF_H__
+#define __EFI_IP4_IF_H__
+
+#define IP4_FRAME_RX_SIGNATURE  SIGNATURE_32 ('I', 'P', 'F', 'R')
+#define IP4_FRAME_TX_SIGNATURE  SIGNATURE_32 ('I', 'P', 'F', 'T')
+#define IP4_FRAME_ARP_SIGNATURE SIGNATURE_32 ('I', 'P', 'F', 'A')
+#define IP4_INTERFACE_SIGNATURE SIGNATURE_32 ('I', 'P', 'I', 'F')
+
+/**
+  This prototype is used by both receive and transmission.
+  When receiving Netbuf is allocated by IP4_INTERFACE, and
+  released by IP4. Flag shows whether the frame is received
+  as link broadcast/multicast...
+
+  When transmitting, the Netbuf is from IP4, and provided
+  to the callback as a reference. Flag isn't used.
+
+  @param[in] IpInstance The instance that sent or received the packet.
+                        IpInstance can be NULL which means that it is the IP4 driver
+                        itself sending the packets. IP4 driver may send packets that
+                        don't belong to any instance, such as ICMP errors, ICMP echo
+                        responses, or IGMP packets. IpInstance is used as a tag in
+                        this module.
+  @param[in] Packet     The sent or received packet.
+  @param[in] IoStatus   Status of sending or receiving.
+  @param[in] LinkFlag   Indicate if the frame is received as link broadcast/multicast.
+                        When transmitting, it is not used.
+  @param[in] Context    Additional data for callback.
+
+  @retval None.
+**/
+typedef
+VOID
+(*IP4_FRAME_CALLBACK)(
+  IN IP4_PROTOCOL              *IpInstance       OPTIONAL,
+  IN NET_BUF                   *Packet,
+  IN EFI_STATUS                IoStatus,
+  IN UINT32                    LinkFlag,
+  IN VOID                      *Context
+  );
+
+///
+/// Each receive request is wrapped in an IP4_LINK_RX_TOKEN.
+/// Upon completion, the Callback will be called. Only one
+/// receive request is send to MNP. IpInstance is always NULL.
+/// Reference MNP's spec for information.
+///
+typedef struct {
+  UINT32                                Signature;
+  IP4_INTERFACE                         *Interface;
+
+  IP4_PROTOCOL                          *IpInstance;
+  IP4_FRAME_CALLBACK                    CallBack;
+  VOID                                  *Context;
+
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  MnpToken;
+} IP4_LINK_RX_TOKEN;
+
+///
+/// Each transmit request is wrapped in an IP4_LINK_TX_TOKEN.
+/// Upon completion, the Callback will be called.
+///
+typedef struct {
+  UINT32                                Signature;
+  LIST_ENTRY                            Link;
+
+  IP4_INTERFACE                         *Interface;
+
+  IP4_PROTOCOL                          *IpInstance;
+  IP4_FRAME_CALLBACK                    CallBack;
+  NET_BUF                               *Packet;
+  VOID                                  *Context;
+
+  EFI_MAC_ADDRESS                       DstMac;
+  EFI_MAC_ADDRESS                       SrcMac;
+
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  MnpToken;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA     MnpTxData;
+} IP4_LINK_TX_TOKEN;
+
+///
+/// Only one ARP request is requested for all the frames in
+/// a time. It is started for the first frames to the Ip. Any
+/// subsequent transmission frame will be linked to Frames, and
+/// be sent all at once the ARP requests succeed.
+///
+typedef struct {
+  UINT32                  Signature;
+  LIST_ENTRY              Link;
+
+  LIST_ENTRY              Frames;
+  IP4_INTERFACE           *Interface;
+
+  //
+  // ARP requesting staffs
+  //
+  EFI_EVENT               OnResolved;
+  IP4_ADDR                Ip;
+  EFI_MAC_ADDRESS         Mac;
+} IP4_ARP_QUE;
+
+/**
+  Callback to select which frame to cancel. Caller can cancel a
+  single frame, or all the frame from an IP instance.
+
+  @param Frame      The sending frame to check for cancellation.
+  @param Context    Additional data for callback.
+
+  @retval TRUE      The sending of the frame should be cancelled.
+  @retval FALSE     Do not cancel the frame sending.
+**/
+typedef
+BOOLEAN
+(*IP4_FRAME_TO_CANCEL)(
+  IP4_LINK_TX_TOKEN       *Frame,
+  VOID                    *Context
+  );
+
+//
+// Each IP4 instance has its own station address. All the instances
+// with the same station address share a single interface structure.
+// Each interface has its own ARP child, and shares one MNP child.
+// Notice the special cases that DHCP can configure the interface
+// with 0.0.0.0/0.0.0.0.
+//
+struct _IP4_INTERFACE {
+  UINT32                        Signature;
+  LIST_ENTRY                    Link;
+  INTN                          RefCnt;
+
+  //
+  // IP address and subnet mask of the interface. It also contains
+  // the subnet/net broadcast address for quick access. The fileds
+  // are invalid if (Configured == FALSE)
+  //
+  IP4_ADDR                      Ip;
+  IP4_ADDR                      SubnetMask;
+  IP4_ADDR                      SubnetBrdcast;
+  IP4_ADDR                      NetBrdcast;
+  BOOLEAN                       Configured;
+
+  //
+  // Handle used to create/destroy ARP child. All the IP children
+  // share one MNP which is owned by IP service binding.
+  //
+  EFI_HANDLE                    Controller;
+  EFI_HANDLE                    Image;
+
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+  EFI_ARP_PROTOCOL              *Arp;
+  EFI_HANDLE                    ArpHandle;
+
+  //
+  // Queues to keep the frames sent and waiting ARP request.
+  //
+  LIST_ENTRY                    ArpQues;
+  LIST_ENTRY                    SentFrames;
+  IP4_LINK_RX_TOKEN             *RecvRequest;
+
+  //
+  // The interface's MAC and broadcast MAC address.
+  //
+  EFI_MAC_ADDRESS               Mac;
+  EFI_MAC_ADDRESS               BroadcastMac;
+  UINT32                        HwaddrLen;
+
+  //
+  // All the IP instances that have the same IP/SubnetMask are linked
+  // together through IpInstances. If any of the instance enables
+  // promiscuous receive, PromiscRecv is true.
+  //
+  LIST_ENTRY                    IpInstances;
+  BOOLEAN                       PromiscRecv;
+};
+
+/**
+  Create an IP4_INTERFACE. Delay the creation of ARP instance until
+  the interface is configured.
+
+  @param[in]  Mnp               The shared MNP child of this IP4 service binding
+                                instance.
+  @param[in]  Controller        The controller this IP4 service binding instance
+                                is installed. Most like the UNDI handle.
+  @param[in]  ImageHandle       This driver's image handle.
+
+  @return Point to the created IP4_INTERFACE, otherwise NULL.
+
+**/
+IP4_INTERFACE *
+Ip4CreateInterface (
+  IN  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp,
+  IN  EFI_HANDLE                    Controller,
+  IN  EFI_HANDLE                    ImageHandle
+  );
+
+/**
+  Set the interface's address, create and configure
+  the ARP child if necessary.
+
+  @param  Interface         The interface to set the address.
+  @param  IpAddr            The interface's IP address.
+  @param  SubnetMask        The interface's netmask.
+
+  @retval EFI_SUCCESS           The interface is configured with Ip/netmask pair,
+                                and a ARP is created for it.
+  @retval Others                Failed to set the interface's address.
+
+**/
+EFI_STATUS
+Ip4SetAddress (
+  IN OUT IP4_INTERFACE      *Interface,
+  IN     IP4_ADDR           IpAddr,
+  IN     IP4_ADDR           SubnetMask
+  );
+
+/**
+  Free the interface used by IpInstance. All the IP instance with
+  the same Ip/Netmask pair share the same interface. It is reference
+  counted. All the frames haven't been sent will be cancelled.
+  Because the IpInstance is optional, the caller must remove
+  IpInstance from the interface's instance list itself.
+
+  @param[in]  Interface         The interface used by the IpInstance.
+  @param[in]  IpInstance        The Ip instance that free the interface. NULL if
+                                the Ip driver is releasing the default interface.
+
+  @retval EFI_SUCCESS           The interface use IpInstance is freed.
+
+**/
+EFI_STATUS
+Ip4FreeInterface (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance           OPTIONAL
+  );
+
+/**
+  Send a frame from the interface. If the next hop is broadcast or
+  multicast address, it is transmitted immediately. If the next hop
+  is a unicast, it will consult ARP to resolve the NextHop's MAC.
+  If some error happened, the CallBack won't be called. So, the caller
+  must test the return value, and take action when there is an error.
+
+  @param[in]  Interface         The interface to send the frame from
+  @param[in]  IpInstance        The IP child that request the transmission.  NULL
+                                if it is the IP4 driver itself.
+  @param[in]  Packet            The packet to transmit.
+  @param[in]  NextHop           The immediate destination to transmit the packet
+                                to.
+  @param[in]  CallBack          Function to call back when transmit finished.
+  @param[in]  Context           Opaque parameter to the call back.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to send the frame
+  @retval EFI_NO_MAPPING        Can't resolve the MAC for the nexthop
+  @retval EFI_SUCCESS           The packet is successfully transmitted.
+  @retval other                 Other error occurs.
+
+**/
+EFI_STATUS
+Ip4SendFrame (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance       OPTIONAL,
+  IN  NET_BUF               *Packet,
+  IN  IP4_ADDR              NextHop,
+  IN  IP4_FRAME_CALLBACK    CallBack,
+  IN  VOID                  *Context
+  );
+
+/**
+  Remove all the frames on the interface that pass the FrameToCancel,
+  either queued on ARP queues or that have already been delivered to
+  MNP and not yet recycled.
+
+  @param[in]  Interface         Interface to remove the frames from.
+  @param[in]  IoStatus          The transmit status returned to the frames'
+                                callback.
+  @param[in]  FrameToCancel     Function to select the frame to cancel, NULL to
+                                select all.
+  @param[in]  Context           Opaque parameters passed to FrameToCancel.
+
+**/
+VOID
+Ip4CancelFrames (
+  IN IP4_INTERFACE          *Interface,
+  IN EFI_STATUS             IoStatus,
+  IN IP4_FRAME_TO_CANCEL    FrameToCancel    OPTIONAL,
+  IN VOID                   *Context
+  );
+
+/**
+  If there is a pending receive request, cancel it. Don't call
+  the receive request's callback because this function can be only
+  called if the instance or driver is tearing itself down. It
+  doesn't make sense to call it back. But it is necessary to call
+  the transmit token's callback to give it a chance to free the
+  packet and update the upper layer's transmit request status, say
+  that from the UDP.
+
+  @param[in]  Interface         The interface used by the IpInstance
+
+**/
+VOID
+Ip4CancelReceive (
+  IN IP4_INTERFACE          *Interface
+  );
+
+/**
+  Request to receive the packet from the interface.
+
+  @param[in]  Interface         The interface to receive the frames from.
+  @param[in]  IpInstance        The instance that requests the receive. NULL for
+                                the driver itself.
+  @param[in]  CallBack          Function to call when receive finished.
+  @param[in]  Context           Opaque parameter to the callback.
+
+  @retval EFI_ALREADY_STARTED   There is already a pending receive request.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource to receive.
+  @retval EFI_SUCCESS           The recieve request has been started.
+  @retval other                 Other error occurs.
+
+**/
+EFI_STATUS
+Ip4ReceiveFrame (
+  IN  IP4_INTERFACE         *Interface,
+  IN  IP4_PROTOCOL          *IpInstance       OPTIONAL,
+  IN  IP4_FRAME_CALLBACK    CallBack,
+  IN  VOID                  *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.c
new file mode 100644
index 0000000000..e0edc93ff8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.c
@@ -0,0 +1,621 @@
+/** @file
+  This file implements the RFC2236: IGMP v2.
+  
+Copyright (c) 2005 - 2015, 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 "Ip4Impl.h"
+
+//
+// Route Alert option in IGMP report to direct routers to
+// examine the packet more closely.
+//
+UINT32  mRouteAlertOption = 0x00000494;
+
+
+/**
+  Init the IGMP control data of the IP4 service instance, configure
+  MNP to receive ALL SYSTEM multicast.
+
+  @param[in, out]  IpSb          The IP4 service whose IGMP is to be initialized.
+
+  @retval EFI_SUCCESS            IGMP of the IpSb is successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource to initialize IGMP.
+  @retval Others                 Failed to initialize the IGMP of IpSb.
+
+**/
+EFI_STATUS
+Ip4InitIgmp (
+  IN OUT IP4_SERVICE            *IpSb
+  )
+{
+  IGMP_SERVICE_DATA             *IgmpCtrl;
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+  IGMP_GROUP                    *Group;
+  EFI_STATUS                    Status;
+
+  IgmpCtrl = &IpSb->IgmpCtrl;
+
+  //
+  // Configure MNP to receive ALL_SYSTEM multicast
+  //
+  Group    = AllocatePool (sizeof (IGMP_GROUP));
+
+  if (Group == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Mnp               = IpSb->Mnp;
+
+  Group->Address    = IP4_ALLSYSTEM_ADDRESS;
+  Group->RefCnt     = 1;
+  Group->DelayTime  = 0;
+  Group->ReportByUs = FALSE;
+
+  Status = Ip4GetMulticastMac (Mnp, IP4_ALLSYSTEM_ADDRESS, &Group->Mac);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Mnp->Groups (Mnp, TRUE, &Group->Mac);
+
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    goto ON_ERROR;
+  }
+
+  InsertHeadList (&IgmpCtrl->Groups, &Group->Link);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (Group);
+  return Status;
+}
+
+
+/**
+  Find the IGMP_GROUP structure which contains the status of multicast
+  group Address in this IGMP control block
+
+  @param[in]  IgmpCtrl               The IGMP control block to search from.
+  @param[in]  Address                The multicast address to search.
+
+  @return NULL if the multicast address isn't in the IGMP control block. Otherwise
+          the point to the IGMP_GROUP which contains the status of multicast group
+          for Address.
+
+**/
+IGMP_GROUP *
+Ip4FindGroup (
+  IN IGMP_SERVICE_DATA      *IgmpCtrl,
+  IN IP4_ADDR               Address
+  )
+{
+  LIST_ENTRY                *Entry;
+  IGMP_GROUP                *Group;
+
+  NET_LIST_FOR_EACH (Entry, &IgmpCtrl->Groups) {
+    Group = NET_LIST_USER_STRUCT (Entry, IGMP_GROUP, Link);
+
+    if (Group->Address == Address) {
+      return Group;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Count the number of IP4 multicast groups that are mapped to the
+  same MAC address. Several IP4 multicast address may be mapped to
+  the same MAC address.
+
+  @param[in]  IgmpCtrl               The IGMP control block to search in.
+  @param[in]  Mac                    The MAC address to search.
+
+  @return The number of the IP4 multicast group that mapped to the same
+          multicast group Mac.
+
+**/
+INTN
+Ip4FindMac (
+  IN IGMP_SERVICE_DATA      *IgmpCtrl,
+  IN EFI_MAC_ADDRESS        *Mac
+  )
+{
+  LIST_ENTRY                *Entry;
+  IGMP_GROUP                *Group;
+  INTN                      Count;
+
+  Count = 0;
+
+  NET_LIST_FOR_EACH (Entry, &IgmpCtrl->Groups) {
+    Group = NET_LIST_USER_STRUCT (Entry, IGMP_GROUP, Link);
+
+    if (NET_MAC_EQUAL (&Group->Mac, Mac, sizeof (EFI_MAC_ADDRESS))) {
+      Count++;
+    }
+  }
+
+  return Count;
+}
+
+
+/**
+  Send an IGMP protocol message to the Dst, such as IGMP v1 membership report.
+
+  @param[in]  IpSb               The IP4 service instance that requests the
+                                 transmission.
+  @param[in]  Dst                The destinaton to send to.
+  @param[in]  Type               The IGMP message type, such as IGMP v1 membership
+                                 report.
+  @param[in]  Group              The group address in the IGMP message head.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to build the message.
+  @retval EFI_SUCCESS            The IGMP message is successfully send.
+  @retval Others                 Failed to send the IGMP message.
+
+**/
+EFI_STATUS
+Ip4SendIgmpMessage (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_ADDR               Dst,
+  IN UINT8                  Type,
+  IN IP4_ADDR               Group
+  )
+{
+  IP4_HEAD                  Head;
+  NET_BUF                   *Packet;
+  IGMP_HEAD                 *Igmp;
+
+  //
+  // Allocate a net buffer to hold the message
+  //
+  Packet = NetbufAlloc (IP4_MAX_HEADLEN + sizeof (IGMP_HEAD));
+
+  if (Packet == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Fill in the IGMP and IP header, then transmit the message
+  //
+  NetbufReserve (Packet, IP4_MAX_HEADLEN);
+
+  Igmp = (IGMP_HEAD *) NetbufAllocSpace (Packet, sizeof (IGMP_HEAD), FALSE);
+  if (Igmp == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Igmp->Type        = Type;
+  Igmp->MaxRespTime = 0;
+  Igmp->Checksum    = 0;
+  Igmp->Group       = HTONL (Group);
+  Igmp->Checksum    = (UINT16) (~NetblockChecksum ((UINT8 *) Igmp, sizeof (IGMP_HEAD)));
+
+  Head.Tos          = 0;
+  Head.Protocol     = IP4_PROTO_IGMP;
+  Head.Ttl          = 1;
+  Head.Fragment     = 0;
+  Head.Dst          = Dst;
+  Head.Src          = IP4_ALLZERO_ADDRESS;
+
+  return Ip4Output (
+           IpSb,
+           NULL,
+           Packet,
+           &Head,
+           (UINT8 *) &mRouteAlertOption,
+           sizeof (UINT32),
+           IP4_ALLZERO_ADDRESS,
+           Ip4SysPacketSent,
+           NULL
+           );
+}
+
+
+/**
+  Send an IGMP membership report. Depends on whether the server is
+  v1 or v2, it will send either a V1 or V2 membership report.
+
+  @param[in]  IpSb               The IP4 service instance that requests the
+                                 transmission.
+  @param[in]  Group              The group address to report.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory to build the message.
+  @retval EFI_SUCCESS            The IGMP report message is successfully send.
+  @retval Others                 Failed to send the report.
+
+**/
+EFI_STATUS
+Ip4SendIgmpReport (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_ADDR               Group
+  )
+{
+  if (IpSb->IgmpCtrl.Igmpv1QuerySeen != 0) {
+    return Ip4SendIgmpMessage (IpSb, Group, IGMP_V1_MEMBERSHIP_REPORT, Group);
+  } else {
+    return Ip4SendIgmpMessage (IpSb, Group, IGMP_V2_MEMBERSHIP_REPORT, Group);
+  }
+}
+
+
+/**
+  Join the multicast group on behalf of this IP4 child
+
+  @param[in]  IpInstance         The IP4 child that wants to join the group.
+  @param[in]  Address            The group to join.
+
+  @retval EFI_SUCCESS            Successfully join the multicast group.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+  @retval Others                 Failed to join the multicast group.
+
+**/
+EFI_STATUS
+Ip4JoinGroup (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_ADDR               Address
+  )
+{
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+  IP4_SERVICE                   *IpSb;
+  IGMP_SERVICE_DATA             *IgmpCtrl;
+  IGMP_GROUP                    *Group;
+  EFI_STATUS                    Status;
+
+  IpSb      = IpInstance->Service;
+  IgmpCtrl  = &IpSb->IgmpCtrl;
+  Mnp       = IpSb->Mnp;
+
+  //
+  // If the IP service already is a member in the group, just
+  // increase the refernce count and return.
+  //
+  Group     = Ip4FindGroup (IgmpCtrl, Address);
+
+  if (Group != NULL) {
+    Group->RefCnt++;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Otherwise, create a new IGMP_GROUP,  Get the multicast's MAC address,
+  // send a report, then direct MNP to receive the multicast.
+  //
+  Group = AllocatePool (sizeof (IGMP_GROUP));
+
+  if (Group == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Group->Address    = Address;
+  Group->RefCnt     = 1;
+  Group->DelayTime  = IGMP_UNSOLICIATED_REPORT;
+  Group->ReportByUs = TRUE;
+
+  Status = Ip4GetMulticastMac (Mnp, Address, &Group->Mac);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Ip4SendIgmpReport (IpSb, Address);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = Mnp->Groups (Mnp, TRUE, &Group->Mac);
+
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    goto ON_ERROR;
+  }
+
+  InsertHeadList (&IgmpCtrl->Groups, &Group->Link);
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  FreePool (Group);
+  return Status;
+}
+
+
+/**
+  Leave the IP4 multicast group on behalf of IpInstance.
+
+  @param[in]  IpInstance         The IP4 child that wants to leave the group
+                                 address.
+  @param[in]  Address            The group address to leave.
+
+  @retval EFI_NOT_FOUND          The IP4 service instance isn't in the group.
+  @retval EFI_SUCCESS            Successfully leave the multicast group.
+  @retval Others                 Failed to leave the multicast group.
+
+**/
+EFI_STATUS
+Ip4LeaveGroup (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_ADDR               Address
+  )
+{
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+  IP4_SERVICE                   *IpSb;
+  IGMP_SERVICE_DATA             *IgmpCtrl;
+  IGMP_GROUP                    *Group;
+  EFI_STATUS                    Status;
+
+  IpSb      = IpInstance->Service;
+  IgmpCtrl  = &IpSb->IgmpCtrl;
+  Mnp       = IpSb->Mnp;
+
+  Group     = Ip4FindGroup (IgmpCtrl, Address);
+
+  if (Group == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // If more than one instance is in the group, decrease
+  // the RefCnt then return.
+  //
+  if (--Group->RefCnt > 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If multiple IP4 group addresses are mapped to the same
+  // multicast MAC address, don't configure the MNP to leave
+  // the MAC.
+  //
+  if (Ip4FindMac (IgmpCtrl, &Group->Mac) == 1) {
+    Status = Mnp->Groups (Mnp, FALSE, &Group->Mac);
+
+    if (EFI_ERROR (Status) && (Status != EFI_NOT_FOUND)) {
+      return Status;
+    }
+  }
+
+  //
+  // Send a leave report if the membership is reported by us
+  // and we are talking IGMPv2.
+  //
+  if (Group->ReportByUs && IgmpCtrl->Igmpv1QuerySeen == 0) {
+    Ip4SendIgmpMessage (IpSb, IP4_ALLROUTER_ADDRESS, IGMP_LEAVE_GROUP, Group->Address);
+  }
+
+  RemoveEntryList (&Group->Link);
+  FreePool (Group);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Handle the received IGMP message for the IP4 service instance.
+
+  @param[in]  IpSb               The IP4 service instance that received the message.
+  @param[in]  Head               The IP4 header of the received message.
+  @param[in]  Packet             The IGMP message, without IP4 header.
+
+  @retval EFI_INVALID_PARAMETER  The IGMP message is malformated.
+  @retval EFI_SUCCESS            The IGMP message is successfully processed.
+
+**/
+EFI_STATUS
+Ip4IgmpHandle (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IGMP_SERVICE_DATA         *IgmpCtrl;
+  IGMP_HEAD                 Igmp;
+  IGMP_GROUP                *Group;
+  IP4_ADDR                  Address;
+  LIST_ENTRY                *Entry;
+
+  IgmpCtrl = &IpSb->IgmpCtrl;
+
+  //
+  // Must checksum over the whole packet, later IGMP version
+  // may employ message longer than 8 bytes. IP's header has
+  // already been trimmed off.
+  //
+  if ((Packet->TotalSize < sizeof (Igmp)) || (NetbufChecksum (Packet) != 0)) {
+    NetbufFree (Packet);
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Copy the packet in case it is fragmented
+  //
+  NetbufCopy (Packet, 0, sizeof (IGMP_HEAD), (UINT8 *)&Igmp);
+
+  switch (Igmp.Type) {
+  case IGMP_MEMBERSHIP_QUERY:
+    //
+    // If MaxRespTime is zero, it is most likely that we are
+    // talking to a V1 router
+    //
+    if (Igmp.MaxRespTime == 0) {
+      IgmpCtrl->Igmpv1QuerySeen = IGMP_V1ROUTER_PRESENT;
+      Igmp.MaxRespTime          = 100;
+    }
+
+    //
+    // Igmp is ticking once per second but MaxRespTime is in
+    // the unit of 100ms.
+    //
+    Igmp.MaxRespTime /= 10;
+    Address = NTOHL (Igmp.Group);
+
+    if (Address == IP4_ALLSYSTEM_ADDRESS) {
+      break;
+    }
+
+    NET_LIST_FOR_EACH (Entry, &IgmpCtrl->Groups) {
+      Group = NET_LIST_USER_STRUCT (Entry, IGMP_GROUP, Link);
+
+      //
+      // If address is all zero, all the memberships will be reported.
+      // otherwise only one is reported.
+      //
+      if ((Address == IP4_ALLZERO_ADDRESS) || (Address == Group->Address)) {
+        //
+        // If the timer is pending, only update it if the time left
+        // is longer than the MaxRespTime. TODO: randomize the DelayTime.
+        //
+        if ((Group->DelayTime == 0) || (Group->DelayTime > Igmp.MaxRespTime)) {
+          Group->DelayTime = MAX (1, Igmp.MaxRespTime);
+        }
+      }
+    }
+
+    break;
+
+  case IGMP_V1_MEMBERSHIP_REPORT:
+  case IGMP_V2_MEMBERSHIP_REPORT:
+    Address = NTOHL (Igmp.Group);
+    Group   = Ip4FindGroup (IgmpCtrl, Address);
+
+    if ((Group != NULL) && (Group->DelayTime > 0)) {
+      Group->DelayTime  = 0;
+      Group->ReportByUs = FALSE;
+    }
+
+    break;
+  }
+
+  NetbufFree (Packet);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The periodical timer function for IGMP. It does the following
+  things:
+  1. Decrease the Igmpv1QuerySeen to make it possible to refresh
+     the IGMP server type.
+  2. Decrease the report timer for each IGMP group in "delaying
+     member" state.
+
+  @param[in]  IpSb                   The IP4 service instance that is ticking.
+
+**/
+VOID
+Ip4IgmpTicking (
+  IN IP4_SERVICE            *IpSb
+  )
+{
+  IGMP_SERVICE_DATA         *IgmpCtrl;
+  LIST_ENTRY                *Entry;
+  IGMP_GROUP                *Group;
+
+  IgmpCtrl = &IpSb->IgmpCtrl;
+
+  if (IgmpCtrl->Igmpv1QuerySeen > 0) {
+    IgmpCtrl->Igmpv1QuerySeen--;
+  }
+
+  //
+  // Decrease the report timer for each IGMP group in "delaying member"
+  //
+  NET_LIST_FOR_EACH (Entry, &IgmpCtrl->Groups) {
+    Group = NET_LIST_USER_STRUCT (Entry, IGMP_GROUP, Link);
+    ASSERT (Group->DelayTime >= 0);
+
+    if (Group->DelayTime > 0) {
+      Group->DelayTime--;
+
+      if (Group->DelayTime == 0) {
+        Ip4SendIgmpReport (IpSb, Group->Address);
+        Group->ReportByUs = TRUE;
+      }
+    }
+  }
+}
+
+
+/**
+  Add a group address to the array of group addresses.
+  The caller should make sure that no duplicated address
+  existed in the array. Although the function doesn't
+  assume the byte order of the both Source and Addr, the
+  network byte order is used by the caller.
+
+  @param[in]  Source                 The array of group addresses to add to.
+  @param[in]  Count                  The number of group addresses in the Source.
+  @param[in]  Addr                   The IP4 multicast address to add.
+
+  @return NULL if failed to allocate memory for the new groups,
+          otherwise the new combined group addresses.
+
+**/
+IP4_ADDR *
+Ip4CombineGroups (
+  IN  IP4_ADDR              *Source,
+  IN  UINT32                Count,
+  IN  IP4_ADDR              Addr
+  )
+{
+  IP4_ADDR                  *Groups;
+
+  Groups = AllocatePool (sizeof (IP4_ADDR) * (Count + 1));
+
+  if (Groups == NULL) {
+    return NULL;
+  }
+
+  CopyMem (Groups, Source, Count * sizeof (IP4_ADDR));
+  Groups[Count] = Addr;
+
+  return Groups;
+}
+
+
+/**
+  Remove a group address from the array of group addresses.
+  Although the function doesn't assume the byte order of the
+  both Groups and Addr, the network byte order is used by
+  the caller.
+
+  @param  Groups            The array of group addresses to remove from.
+  @param  Count             The number of group addresses in the Groups.
+  @param  Addr              The IP4 multicast address to remove.
+
+  @return The nubmer of group addresses in the Groups after remove.
+          It is Count if the Addr isn't in the Groups.
+
+**/
+INTN
+Ip4RemoveGroupAddr (
+  IN OUT IP4_ADDR               *Groups,
+  IN     UINT32                 Count,
+  IN     IP4_ADDR               Addr
+  )
+{
+  UINT32                    Index;
+
+  for (Index = 0; Index < Count; Index++) {
+    if (Groups[Index] == Addr) {
+      break;
+    }
+  }
+
+  while (Index < Count - 1) {
+    Groups[Index] = Groups[Index + 1];
+    Index++;
+  }
+
+  return Index;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.h
new file mode 100644
index 0000000000..756c697a3a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Igmp.h
@@ -0,0 +1,207 @@
+/** @file
+
+Copyright (c) 2005 - 2015, 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.
+
+**/
+
+#ifndef __EFI_IP4_IGMP_H__
+#define __EFI_IP4_IGMP_H__
+
+//
+// IGMP message type
+//
+#define IGMP_MEMBERSHIP_QUERY      0x11
+#define IGMP_V1_MEMBERSHIP_REPORT  0x12
+#define IGMP_V2_MEMBERSHIP_REPORT  0x16
+#define IGMP_LEAVE_GROUP           0x17
+
+#define IGMP_V1ROUTER_PRESENT      400
+#define IGMP_UNSOLICIATED_REPORT   10
+
+#pragma pack(1)
+typedef struct {
+  UINT8                   Type;
+  UINT8                   MaxRespTime;
+  UINT16                  Checksum;
+  IP4_ADDR                Group;
+} IGMP_HEAD;
+#pragma pack()
+
+///
+/// The status of multicast group. It isn't necessary to maintain
+/// explicit state of host state diagram. A group with non-zero
+/// DelayTime is in "delaying member" state. otherwise, it is in
+/// "idle member" state.
+///
+typedef struct {
+  LIST_ENTRY              Link;
+  INTN                    RefCnt;
+  IP4_ADDR                Address;
+  INTN                    DelayTime;
+  BOOLEAN                 ReportByUs;
+  EFI_MAC_ADDRESS         Mac;
+} IGMP_GROUP;
+
+///
+/// The IGMP status. Each IP4 service instance has a IGMP_SERVICE_DATA
+/// attached. The Igmpv1QuerySeen remember whether the server on this
+/// connected network is v1 or v2.
+///
+typedef struct {
+  INTN                    Igmpv1QuerySeen;
+  LIST_ENTRY              Groups;
+} IGMP_SERVICE_DATA;
+
+/**
+  Init the IGMP control data of the IP4 service instance, configure
+  MNP to receive ALL SYSTEM multicast.
+
+  @param[in, out]  IpSb          The IP4 service whose IGMP is to be initialized.
+
+  @retval EFI_SUCCESS            IGMP of the IpSb is successfully initialized.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource to initialize IGMP.
+  @retval Others                 Failed to initialize the IGMP of IpSb.
+
+**/
+EFI_STATUS
+Ip4InitIgmp (
+  IN OUT IP4_SERVICE            *IpSb
+  );
+
+/**
+  Join the multicast group on behalf of this IP4 child
+
+  @param[in]  IpInstance         The IP4 child that wants to join the group.
+  @param[in]  Address            The group to join.
+
+  @retval EFI_SUCCESS            Successfully join the multicast group.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources.
+  @retval Others                 Failed to join the multicast group.
+
+**/
+EFI_STATUS
+Ip4JoinGroup (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_ADDR               Address
+  );
+
+/**
+  Leave the IP4 multicast group on behalf of IpInstance.
+
+  @param[in]  IpInstance         The IP4 child that wants to leave the group
+                                 address.
+  @param[in]  Address            The group address to leave.
+
+  @retval EFI_NOT_FOUND          The IP4 service instance isn't in the group.
+  @retval EFI_SUCCESS            Successfully leave the multicast group.
+  @retval Others                 Failed to leave the multicast group.
+
+**/
+EFI_STATUS
+Ip4LeaveGroup (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_ADDR               Address
+  );
+
+/**
+  Handle the received IGMP message for the IP4 service instance.
+
+  @param[in]  IpSb               The IP4 service instance that received the message.
+  @param[in]  Head               The IP4 header of the received message.
+  @param[in]  Packet             The IGMP message, without IP4 header.
+
+  @retval EFI_INVALID_PARAMETER  The IGMP message is malformated.
+  @retval EFI_SUCCESS            The IGMP message is successfully processed.
+
+**/
+EFI_STATUS
+Ip4IgmpHandle (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  );
+
+/**
+  The periodical timer function for IGMP. It does the following
+  things:
+  1. Decrease the Igmpv1QuerySeen to make it possible to refresh
+     the IGMP server type.
+  2. Decrease the report timer for each IGMP group in "delaying
+     member" state.
+
+  @param[in]  IpSb                   The IP4 service instance that is ticking.
+
+**/
+VOID
+Ip4IgmpTicking (
+  IN IP4_SERVICE            *IpSb
+  );
+
+/**
+  Add a group address to the array of group addresses.
+  The caller should make sure that no duplicated address
+  existed in the array. Although the function doesn't
+  assume the byte order of the both Source and Addr, the
+  network byte order is used by the caller.
+
+  @param[in]  Source                 The array of group addresses to add to.
+  @param[in]  Count                  The number of group addresses in the Source.
+  @param[in]  Addr                   The IP4 multicast address to add.
+
+  @return NULL if failed to allocate memory for the new groups,
+          otherwise the new combined group addresses.
+
+**/
+IP4_ADDR *
+Ip4CombineGroups (
+  IN  IP4_ADDR              *Source,
+  IN  UINT32                Count,
+  IN  IP4_ADDR              Addr
+  );
+
+/**
+  Remove a group address from the array of group addresses.
+  Although the function doesn't assume the byte order of the
+  both Groups and Addr, the network byte order is used by
+  the caller.
+
+  @param  Groups            The array of group addresses to remove from.
+  @param  Count             The number of group addresses in the Groups.
+  @param  Addr              The IP4 multicast address to remove.
+
+  @return The nubmer of group addresses in the Groups after remove.
+          It is Count if the Addr isn't in the Groups.
+
+**/
+INTN
+Ip4RemoveGroupAddr (
+  IN OUT IP4_ADDR               *Groups,
+  IN     UINT32                 Count,
+  IN     IP4_ADDR               Addr
+  );
+
+/**
+  Find the IGMP_GROUP structure which contains the status of multicast
+  group Address in this IGMP control block
+
+  @param[in]  IgmpCtrl               The IGMP control block to search from.
+  @param[in]  Address                The multicast address to search.
+
+  @return NULL if the multicast address isn't in the IGMP control block. Otherwise
+          the point to the IGMP_GROUP which contains the status of multicast group
+          for Address.
+
+**/
+IGMP_GROUP *
+Ip4FindGroup (
+  IN IGMP_SERVICE_DATA      *IgmpCtrl,
+  IN IP4_ADDR               Address
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.c
new file mode 100644
index 0000000000..b9a294b199
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.c
@@ -0,0 +1,2432 @@
+/** @file
+
+Copyright (c) 2005 - 2015, 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 "Ip4Impl.h"
+
+EFI_IPSEC2_PROTOCOL    *mIpSec = NULL;
+
+/**
+  Gets the current operational settings for this instance of the EFI IPv4 Protocol driver.
+
+  The GetModeData() function returns the current operational mode data for this
+  driver instance. The data fields in EFI_IP4_MODE_DATA are read only. This
+  function is used optionally to retrieve the operational mode data of underlying
+  networks or drivers.
+
+  @param[in]   This          Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[out]  Ip4ModeData   Pointer to the EFI IPv4 Protocol mode data structure.
+  @param[out]  MnpConfigData Pointer to the managed network configuration data structure.
+  @param[out]  SnpModeData   Pointer to the simple network mode data structure.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_OUT_OF_RESOURCES  The required mode data could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4GetModeData (
+  IN  CONST EFI_IP4_PROTOCOL                *This,
+  OUT       EFI_IP4_MODE_DATA               *Ip4ModeData     OPTIONAL,
+  OUT       EFI_MANAGED_NETWORK_CONFIG_DATA *MnpConfigData   OPTIONAL,
+  OUT       EFI_SIMPLE_NETWORK_MODE         *SnpModeData     OPTIONAL
+  );
+
+/**
+  Assigns an IPv4 address and subnet mask to this EFI IPv4 Protocol driver instance.
+
+  The Configure() function is used to set, change, or reset the operational
+  parameters and filter settings for this EFI IPv4 Protocol instance. Until these
+  parameters have been set, no network traffic can be sent or received by this
+  instance. Once the parameters have been reset (by calling this function with
+  IpConfigData set to NULL), no more traffic can be sent or received until these
+  parameters have been set again. Each EFI IPv4 Protocol instance can be started
+  and stopped independently of each other by enabling or disabling their receive
+  filter settings with the Configure() function.
+
+  When IpConfigData.UseDefaultAddress is set to FALSE, the new station address will
+  be appended as an alias address into the addresses list in the EFI IPv4 Protocol
+  driver. While set to TRUE, Configure() will trigger the EFI_IP4_CONFIG_PROTOCOL
+  to retrieve the default IPv4 address if it is not available yet. Clients could
+  frequently call GetModeData() to check the status to ensure that the default IPv4
+  address is ready.
+
+  If operational parameters are reset or changed, any pending transmit and receive
+  requests will be cancelled. Their completion token status will be set to EFI_ABORTED
+  and their events will be signaled.
+
+  @param[in]  This              Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  IpConfigData      Pointer to the EFI IPv4 Protocol configuration data structure.
+
+  @retval EFI_SUCCESS           The driver instance was successfully opened.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+  @retval EFI_UNSUPPORTED       One or more of the following conditions is TRUE:
+                                A configuration protocol (DHCP, BOOTP, RARP, etc.) could
+                                not be located when clients choose to use the default IPv4
+                                address. This EFI IPv4 Protocol implementation does not
+                                support this requested filter or timeout setting.
+  @retval EFI_OUT_OF_RESOURCES  The EFI IPv4 Protocol driver instance data could not be allocated.
+  @retval EFI_ALREADY_STARTED   The interface is already open and must be stopped before the
+                                IPv4 address or subnet mask can be changed. The interface must
+                                also be stopped when switching to/from raw packet mode.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred. The EFI IPv4
+                                Protocol driver instance is not opened.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Configure (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN EFI_IP4_CONFIG_DATA    *IpConfigData       OPTIONAL
+  );
+
+/**
+  Joins and leaves multicast groups.
+
+  The Groups() function is used to join and leave multicast group sessions. Joining
+  a group will enable reception of matching multicast packets. Leaving a group will
+  disable the multicast packet reception.
+
+  If JoinFlag is FALSE and GroupAddress is NULL, all joined groups will be left.
+
+  @param[in]  This                  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  JoinFlag              Set to TRUE to join the multicast group session and FALSE to leave.
+  @param[in]  GroupAddress          Pointer to the IPv4 multicast address.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the following is TRUE:
+                                - This is NULL.
+                                - JoinFlag is TRUE and GroupAddress is NULL.
+                                - GroupAddress is not NULL and *GroupAddress is
+                                not a multicast IPv4 address.
+  @retval EFI_NOT_STARTED       This instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_OUT_OF_RESOURCES  System resources could not be allocated.
+  @retval EFI_UNSUPPORTED       This EFI IPv4 Protocol implementation does not support multicast groups.
+  @retval EFI_ALREADY_STARTED   The group address is already in the group table (when
+                                JoinFlag is TRUE).
+  @retval EFI_NOT_FOUND         The group address is not in the group table (when JoinFlag is FALSE).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Groups (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN BOOLEAN                JoinFlag,
+  IN EFI_IPv4_ADDRESS       *GroupAddress     OPTIONAL
+  );
+
+/**
+  Adds and deletes routing table entries.
+
+  The Routes() function adds a route to or deletes a route from the routing table.
+
+  Routes are determined by comparing the SubnetAddress with the destination IPv4
+  address arithmetically AND-ed with the SubnetMask. The gateway address must be
+  on the same subnet as the configured station address.
+
+  The default route is added with SubnetAddress and SubnetMask both set to 0.0.0.0.
+  The default route matches all destination IPv4 addresses that do not match any
+  other routes.
+
+  A GatewayAddress that is zero is a nonroute. Packets are sent to the destination
+  IP address if it can be found in the ARP cache or on the local subnet. One automatic
+  nonroute entry will be inserted into the routing table for outgoing packets that
+  are addressed to a local subnet (gateway address of 0.0.0.0).
+
+  Each EFI IPv4 Protocol instance has its own independent routing table. Those EFI
+  IPv4 Protocol instances that use the default IPv4 address will also have copies
+  of the routing table that was provided by the EFI_IP4_CONFIG_PROTOCOL, and these
+  copies will be updated whenever the EIF IPv4 Protocol driver reconfigures its
+  instances. As a result, client modification to the routing table will be lost.
+
+  @param[in]  This                   Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  DeleteRoute            Set to TRUE to delete this route from the routing table. Set to
+                                     FALSE to add this route to the routing table. SubnetAddress
+                                     and SubnetMask are used as the key to each route entry.
+  @param[in]  SubnetAddress          The address of the subnet that needs to be routed.
+  @param[in]  SubnetMask             The subnet mask of SubnetAddress.
+  @param[in]  GatewayAddress         The unicast gateway IPv4 address for this route.
+
+  @retval EFI_SUCCESS            The operation completed successfully.
+  @retval EFI_NOT_STARTED        The driver instance has not been started.
+  @retval EFI_NO_MAPPING         When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 - This is NULL.
+                                 - SubnetAddress is NULL.
+                                 - SubnetMask is NULL.
+                                 - GatewayAddress is NULL.
+                                 - *SubnetAddress is not a valid subnet address.
+                                 - *SubnetMask is not a valid subnet mask.
+                                 - *GatewayAddress is not a valid unicast IPv4 address.
+  @retval EFI_OUT_OF_RESOURCES   Could not add the entry to the routing table.
+  @retval EFI_NOT_FOUND          This route is not in the routing table (when DeleteRoute is TRUE).
+  @retval EFI_ACCESS_DENIED      The route is already defined in the routing table (when
+                                  DeleteRoute is FALSE).
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Routes (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN BOOLEAN                DeleteRoute,
+  IN EFI_IPv4_ADDRESS       *SubnetAddress,
+  IN EFI_IPv4_ADDRESS       *SubnetMask,
+  IN EFI_IPv4_ADDRESS       *GatewayAddress
+  );
+
+/**
+  Places outgoing data packets into the transmit queue.
+
+  The Transmit() function places a sending request in the transmit queue of this
+  EFI IPv4 Protocol instance. Whenever the packet in the token is sent out or some
+  errors occur, the event in the token will be signaled and the status is updated.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to the transmit token.
+
+  @retval  EFI_SUCCESS           The data has been queued for transmission.
+  @retval  EFI_NOT_STARTED       This instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_INVALID_PARAMETER One or more pameters are invalid.
+  @retval  EFI_ACCESS_DENIED     The transmit completion token with the same Token.Event
+                                 was already in the transmit queue.
+  @retval  EFI_NOT_READY         The completion token could not be queued because the transmit
+                                 queue is full.
+  @retval  EFI_NOT_FOUND         Not route is found to destination address.
+  @retval  EFI_OUT_OF_RESOURCES  Could not queue the transmit data.
+  @retval  EFI_BUFFER_TOO_SMALL  Token.Packet.TxData.TotalDataLength is too
+                                 short to transmit.
+  @retval  EFI_BAD_BUFFER_SIZE   The length of the IPv4 header + option length + total data length is
+                                 greater than MTU (or greater than the maximum packet size if
+                                 Token.Packet.TxData.OverrideData.
+                                 DoNotFragment is TRUE.)
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Transmit (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token
+  );
+
+/**
+  Places a receiving request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet queue.
+  This function is always asynchronous.
+
+  The Token.Event field in the completion token must be filled in by the caller
+  and cannot be NULL. When the receive operation completes, the EFI IPv4 Protocol
+  driver updates the Token.Status and Token.Packet.RxData fields and the Token.Event
+  is signaled.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that is associated with the receive data descriptor.
+
+  @retval EFI_SUCCESS           The receive completion token was cached.
+  @retval EFI_NOT_STARTED       This EFI IPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP, RARP, etc.)
+                                is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                - This is NULL.
+                                - Token is NULL.
+                                - Token.Event is NULL.
+  @retval EFI_OUT_OF_RESOURCES  The receive completion token could not be queued due to a lack of system
+                                resources (usually memory).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+                                The EFI IPv4 Protocol instance has been reset to startup defaults.
+                                EFI_ACCESS_DENIED The receive completion token with the same Token.Event was already
+                                in the receive queue.
+  @retval EFI_NOT_READY         The receive request could not be queued because the receive queue is full.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Receive (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token
+  );
+
+/**
+  Abort an asynchronous transmit or receive request.
+
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token->Status will be set to EFI_ABORTED and then Token->Event will
+  be signaled. If the token is not in one of the queues, which usually means the
+  asynchronous operation has completed, this function will not signal the token
+  and EFI_NOT_FOUND is returned.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that has been issued by
+                    EFI_IP4_PROTOCOL.Transmit() or
+                    EFI_IP4_PROTOCOL.Receive(). If NULL, all pending
+                    tokens are aborted. Type EFI_IP4_COMPLETION_TOKEN is
+                    defined in EFI_IP4_PROTOCOL.Transmit().
+
+  @retval EFI_SUCCESS           The asynchronous I/O request was aborted and
+                                Token.->Event was signaled. When Token is NULL, all
+                                pending requests were aborted and their events were signaled.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_NOT_STARTED       This instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_NOT_FOUND         When Token is not NULL, the asynchronous I/O request was
+                                not found in the transmit or receive queue. It has either completed
+                                or was not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Cancel (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token    OPTIONAL
+  );
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+
+  The Poll() function polls for incoming data packets and processes outgoing data
+  packets. Network drivers and applications can call the EFI_IP4_PROTOCOL.Poll()
+  function to increase the rate that data packets are moved between the communications
+  device and the transmit and receive queues.
+
+  In some systems the periodic timer event may not poll the underlying communications
+  device fast enough to transmit and/or receive all data packets without missing
+  incoming packets or dropping outgoing packets. Drivers and applications that are
+  experiencing packet loss should try calling the EFI_IP4_PROTOCOL.Poll() function
+  more often.
+
+  @param[in]  This               Pointer to the EFI_IP4_PROTOCOL instance.
+
+  @retval  EFI_SUCCESS           Incoming or outgoing data was processed.
+  @retval  EFI_NOT_STARTED       This EFI IPv4 Protocol instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_INVALID_PARAMETER This is NULL.
+  @retval  EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval  EFI_NOT_READY         No incoming or outgoing data is processed.
+  @retval  EFI_TIMEOUT           Data was dropped out of the transmit and/or receive queue.
+                                 Consider increasing the polling rate.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Poll (
+  IN EFI_IP4_PROTOCOL       *This
+  );
+
+EFI_IP4_PROTOCOL
+mEfiIp4ProtocolTemplete = {
+  EfiIp4GetModeData,
+  EfiIp4Configure,
+  EfiIp4Groups,
+  EfiIp4Routes,
+  EfiIp4Transmit,
+  EfiIp4Receive,
+  EfiIp4Cancel,
+  EfiIp4Poll
+};
+
+/**
+  Gets the current operational settings for this instance of the EFI IPv4 Protocol driver.
+
+  The GetModeData() function returns the current operational mode data for this
+  driver instance. The data fields in EFI_IP4_MODE_DATA are read only. This
+  function is used optionally to retrieve the operational mode data of underlying
+  networks or drivers.
+
+  @param[in]   This          Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[out]  Ip4ModeData   Pointer to the EFI IPv4 Protocol mode data structure.
+  @param[out]  MnpConfigData Pointer to the managed network configuration data structure.
+  @param[out]  SnpModeData   Pointer to the simple network mode data structure.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_OUT_OF_RESOURCES  The required mode data could not be allocated.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4GetModeData (
+  IN  CONST EFI_IP4_PROTOCOL                *This,
+  OUT       EFI_IP4_MODE_DATA               *Ip4ModeData     OPTIONAL,
+  OUT       EFI_MANAGED_NETWORK_CONFIG_DATA *MnpConfigData   OPTIONAL,
+  OUT       EFI_SIMPLE_NETWORK_MODE         *SnpModeData     OPTIONAL
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  IP4_SERVICE               *IpSb;
+  EFI_IP4_CONFIG_DATA       *Config;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  IP4_ADDR                  Ip;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl     = gBS->RaiseTPL (TPL_CALLBACK);
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+  IpSb       = IpInstance->Service;
+
+  if (Ip4ModeData != NULL) {
+    //
+    // IsStarted is "whether the EfiIp4Configure has been called".
+    // IsConfigured is "whether the station address has been configured"
+    //
+    Ip4ModeData->IsStarted     = (BOOLEAN)(IpInstance->State == IP4_STATE_CONFIGED);
+    CopyMem (&Ip4ModeData->ConfigData, &IpInstance->ConfigData, sizeof (Ip4ModeData->ConfigData));
+    Ip4ModeData->IsConfigured  = FALSE;
+
+    Ip4ModeData->GroupCount    = IpInstance->GroupCount;
+    Ip4ModeData->GroupTable    = (EFI_IPv4_ADDRESS *) IpInstance->Groups;
+
+    Ip4ModeData->IcmpTypeCount = 23;
+    Ip4ModeData->IcmpTypeList  = mIp4SupportedIcmp;
+
+    Ip4ModeData->RouteTable    = NULL;
+    Ip4ModeData->RouteCount    = 0;
+
+    Ip4ModeData->MaxPacketSize = IpSb->MaxPacketSize;
+
+    //
+    // return the current station address for this IP child. So,
+    // the user can get the default address through this. Some
+    // application wants to know it station address even it is
+    // using the default one, such as a ftp server.
+    //
+    if (Ip4ModeData->IsStarted) {
+      Config  = &Ip4ModeData->ConfigData;
+
+      Ip = HTONL (IpInstance->Interface->Ip);
+      CopyMem (&Config->StationAddress, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+      Ip = HTONL (IpInstance->Interface->SubnetMask);
+      CopyMem (&Config->SubnetMask, &Ip, sizeof (EFI_IPv4_ADDRESS));
+
+      Ip4ModeData->IsConfigured = IpInstance->Interface->Configured;
+
+      //
+      // Build a EFI route table for user from the internal route table.
+      //
+      Status = Ip4BuildEfiRouteTable (IpInstance);
+
+      if (EFI_ERROR (Status)) {
+        gBS->RestoreTPL (OldTpl);
+        return Status;
+      }
+
+      Ip4ModeData->RouteTable = IpInstance->EfiRouteTable;
+      Ip4ModeData->RouteCount = IpInstance->EfiRouteCount;
+    }
+  }
+
+  //
+  // Get fresh mode data from MNP, since underlying media status may change
+  //
+  Status = IpSb->Mnp->GetModeData (IpSb->Mnp, MnpConfigData, SnpModeData);
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Config the MNP parameter used by IP. The IP driver use one MNP
+  child to transmit/receive frames. By default, it configures MNP
+  to receive unicast/multicast/broadcast. And it will enable/disable
+  the promiscous receive according to whether there is IP child
+  enable that or not. If Force is FALSE, it will iterate through
+  all the IP children to check whether the promiscuous receive
+  setting has been changed. If it hasn't been changed, it won't
+  reconfigure the MNP. If Force is TRUE, the MNP is configured no
+  matter whether that is changed or not.
+
+  @param[in]  IpSb               The IP4 service instance that is to be changed.
+  @param[in]  Force              Force the configuration or not.
+
+  @retval EFI_SUCCESS            The MNP is successfully configured/reconfigured.
+  @retval Others                 Configuration failed.
+
+**/
+EFI_STATUS
+Ip4ServiceConfigMnp (
+  IN IP4_SERVICE            *IpSb,
+  IN BOOLEAN                Force
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *ProtoEntry;
+  IP4_INTERFACE             *IpIf;
+  IP4_PROTOCOL              *IpInstance;
+  BOOLEAN                   Reconfig;
+  BOOLEAN                   PromiscReceive;
+  EFI_STATUS                Status;
+
+  Reconfig       = FALSE;
+  PromiscReceive = FALSE;
+
+  if (!Force) {
+    //
+    // Iterate through the IP children to check whether promiscuous
+    // receive setting has been changed. Update the interface's receive
+    // filter also.
+    //
+    NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+
+      IpIf              = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+      IpIf->PromiscRecv = FALSE;
+
+      NET_LIST_FOR_EACH (ProtoEntry, &IpIf->IpInstances) {
+        IpInstance = NET_LIST_USER_STRUCT (ProtoEntry, IP4_PROTOCOL, AddrLink);
+
+        if (IpInstance->ConfigData.AcceptPromiscuous) {
+          IpIf->PromiscRecv = TRUE;
+          PromiscReceive    = TRUE;
+        }
+      }
+    }
+
+    //
+    // If promiscuous receive isn't changed, it isn't necessary to reconfigure.
+    //
+    if (PromiscReceive == IpSb->MnpConfigData.EnablePromiscuousReceive) {
+      return EFI_SUCCESS;
+    }
+
+    Reconfig  = TRUE;
+    IpSb->MnpConfigData.EnablePromiscuousReceive = PromiscReceive;
+  }
+
+  Status = IpSb->Mnp->Configure (IpSb->Mnp, &IpSb->MnpConfigData);
+
+  //
+  // recover the original configuration if failed to set the configure.
+  //
+  if (EFI_ERROR (Status) && Reconfig) {
+    IpSb->MnpConfigData.EnablePromiscuousReceive = (BOOLEAN) !PromiscReceive;
+  }
+
+  return Status;
+}
+
+
+/**
+  Intiialize the IP4_PROTOCOL structure to the unconfigured states.
+
+  @param  IpSb                   The IP4 service instance.
+  @param  IpInstance             The IP4 child instance.
+
+**/
+VOID
+Ip4InitProtocol (
+  IN     IP4_SERVICE            *IpSb,
+  IN OUT IP4_PROTOCOL           *IpInstance
+  )
+{
+  ASSERT ((IpSb != NULL) && (IpInstance != NULL));
+
+  ZeroMem (IpInstance, sizeof (IP4_PROTOCOL));
+
+  IpInstance->Signature = IP4_PROTOCOL_SIGNATURE;
+  CopyMem (&IpInstance->Ip4Proto, &mEfiIp4ProtocolTemplete, sizeof (IpInstance->Ip4Proto));
+  IpInstance->State     = IP4_STATE_UNCONFIGED;
+  IpInstance->Service   = IpSb;
+
+  InitializeListHead (&IpInstance->Link);
+  NetMapInit  (&IpInstance->RxTokens);
+  NetMapInit  (&IpInstance->TxTokens);
+  InitializeListHead (&IpInstance->Received);
+  InitializeListHead (&IpInstance->Delivered);
+  InitializeListHead (&IpInstance->AddrLink);
+
+  EfiInitializeLock (&IpInstance->RecycleLock, TPL_NOTIFY);
+}
+
+
+/**
+  The event handle for IP4 auto reconfiguration. The original default
+  interface and route table will be removed as the default.
+
+  @param[in]  Context                The IP4 service binding instance.
+
+**/
+VOID
+EFIAPI
+Ip4AutoReconfigCallBackDpc (
+  IN VOID                   *Context
+  )
+{
+  IP4_SERVICE               *IpSb;
+
+  IpSb      = (IP4_SERVICE *) Context;
+  NET_CHECK_SIGNATURE (IpSb, IP4_SERVICE_SIGNATURE);
+
+  if (IpSb->State > IP4_SERVICE_UNSTARTED) {
+    IpSb->State = IP4_SERVICE_UNSTARTED;
+  }
+  
+  IpSb->Reconfig = TRUE;
+
+  Ip4StartAutoConfig (&IpSb->Ip4Config2Instance);
+
+  return ;
+}
+
+
+/**
+  Request Ip4AutoReconfigCallBackDpc as a DPC at TPL_CALLBACK.
+
+  @param Event     The event that is signalled.
+  @param Context   The IP4 service binding instance.
+
+**/
+VOID
+EFIAPI
+Ip4AutoReconfigCallBack (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  //
+  // Request Ip4AutoReconfigCallBackDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, Ip4AutoReconfigCallBackDpc, Context);
+}
+
+
+/**
+  Configure the IP4 child. If the child is already configured,
+  change the configuration parameter. Otherwise configure it
+  for the first time. The caller should validate the configuration
+  before deliver them to it. It also don't do configure NULL.
+
+  @param[in, out]  IpInstance         The IP4 child to configure.
+  @param[in]       Config             The configure data.
+
+  @retval EFI_SUCCESS            The IP4 child is successfully configured.
+  @retval EFI_DEVICE_ERROR       Failed to free the pending transive or to
+                                 configure  underlying MNP or other errors.
+  @retval EFI_NO_MAPPING         The IP4 child is configured to use default
+                                 address, but the default address hasn't been
+                                 configured. The IP4 child doesn't need to be
+                                 reconfigured when default address is configured.
+  @retval EFI_OUT_OF_RESOURCES   No more memory space is available.
+  @retval other                  Other error occurs.
+
+**/
+EFI_STATUS
+Ip4ConfigProtocol (
+  IN OUT IP4_PROTOCOL         *IpInstance,
+  IN     EFI_IP4_CONFIG_DATA  *Config
+  )
+{
+  IP4_SERVICE               *IpSb;
+  IP4_INTERFACE             *IpIf;
+  EFI_STATUS                Status;
+  IP4_ADDR                  Ip;
+  IP4_ADDR                  Netmask;
+  EFI_ARP_PROTOCOL          *Arp;
+
+  IpSb = IpInstance->Service;
+
+  //
+  // User is changing packet filters. It must be stopped
+  // before the station address can be changed.
+  //
+  if (IpInstance->State == IP4_STATE_CONFIGED) {
+    //
+    // Cancel all the pending transmit/receive from upper layer
+    //
+    Status = Ip4Cancel (IpInstance, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    CopyMem (&IpInstance->ConfigData, Config, sizeof (IpInstance->ConfigData));
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Configure a fresh IP4 protocol instance. Create a route table.
+  // Each IP child has its own route table, which may point to the
+  // default table if it is using default address.
+  //
+  Status                 = EFI_OUT_OF_RESOURCES;
+  IpInstance->RouteTable = Ip4CreateRouteTable ();
+
+  if (IpInstance->RouteTable == NULL) {
+    return Status;
+  }
+
+  //
+  // Set up the interface.
+  //
+  CopyMem (&Ip, &Config->StationAddress, sizeof (IP4_ADDR));
+  CopyMem (&Netmask, &Config->SubnetMask, sizeof (IP4_ADDR));
+
+  Ip      = NTOHL (Ip);
+  Netmask = NTOHL (Netmask);
+
+  if (!Config->UseDefaultAddress) {
+    //
+    // Find whether there is already an interface with the same
+    // station address. All the instances with the same station
+    // address shares one interface.
+    //
+    IpIf = Ip4FindStationAddress (IpSb, Ip, Netmask);
+
+    if (IpIf != NULL) {
+      NET_GET_REF (IpIf);
+
+    } else {
+      IpIf = Ip4CreateInterface (IpSb->Mnp, IpSb->Controller, IpSb->Image);
+
+      if (IpIf == NULL) {
+        goto ON_ERROR;
+      }
+
+      Status = Ip4SetAddress (IpIf, Ip, Netmask);
+
+      if (EFI_ERROR (Status)) {
+        Status = EFI_DEVICE_ERROR;
+        Ip4FreeInterface (IpIf, IpInstance);
+        goto ON_ERROR;
+      }
+
+      InsertTailList (&IpSb->Interfaces, &IpIf->Link);
+    }
+
+    //
+    // Add a route to this connected network in the route table
+    //
+    Ip4AddRoute (IpInstance->RouteTable, Ip, Netmask, IP4_ALLZERO_ADDRESS);
+
+  } else {
+    //
+    // Use the default address. If the default configuration hasn't
+    // been started, start it.
+    //
+    if (IpSb->State == IP4_SERVICE_UNSTARTED) {
+      //
+      // Create the ReconfigEvent to start the new configuration.
+      //
+      if (IpSb->ReconfigEvent == NULL) {
+        Status = gBS->CreateEvent (
+                        EVT_NOTIFY_SIGNAL,
+                        TPL_NOTIFY,
+                        Ip4AutoReconfigCallBack,
+                        IpSb,
+                        &IpSb->ReconfigEvent
+                        );
+
+        if (EFI_ERROR (Status)) {
+          goto ON_ERROR;
+        }
+      }
+      
+      Status = Ip4StartAutoConfig (&IpSb->Ip4Config2Instance);
+
+      if (EFI_ERROR (Status)) {
+        goto CLOSE_RECONFIG_EVENT;
+      }
+    }
+
+    IpIf = IpSb->DefaultInterface;
+    NET_GET_REF (IpSb->DefaultInterface);
+
+    //
+    // If default address is used, so is the default route table.
+    // Any route set by the instance has the precedence over the
+    // routes in the default route table. Link the default table
+    // after the instance's table. Routing will search the local
+    // table first.
+    //
+    NET_GET_REF (IpSb->DefaultRouteTable);
+    IpInstance->RouteTable->Next = IpSb->DefaultRouteTable;
+  }
+
+  IpInstance->Interface = IpIf;
+  if (IpIf->Arp != NULL) {
+    Arp = NULL;
+    Status = gBS->OpenProtocol (
+                    IpIf->ArpHandle,
+                    &gEfiArpProtocolGuid,
+                    (VOID **) &Arp,
+                    gIp4DriverBinding.DriverBindingHandle,
+                    IpInstance->Handle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      goto CLOSE_RECONFIG_EVENT;
+    }
+  }
+  InsertTailList (&IpIf->IpInstances, &IpInstance->AddrLink);
+
+  CopyMem (&IpInstance->ConfigData, Config, sizeof (IpInstance->ConfigData));
+  IpInstance->State       = IP4_STATE_CONFIGED;
+
+  //
+  // Although EFI_NO_MAPPING is an error code, the IP child has been
+  // successfully configured and doesn't need reconfiguration when
+  // default address is acquired.
+  //
+  if (Config->UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+    return EFI_NO_MAPPING;
+  }
+
+  return EFI_SUCCESS;
+
+CLOSE_RECONFIG_EVENT:
+  if (IpSb->ReconfigEvent != NULL) {
+    gBS->CloseEvent (IpSb->ReconfigEvent);
+    IpSb->ReconfigEvent = NULL;
+  }
+
+ON_ERROR:
+  Ip4FreeRouteTable (IpInstance->RouteTable);
+  IpInstance->RouteTable = NULL;
+  return Status;
+}
+
+
+/**
+  Clean up the IP4 child, release all the resources used by it.
+
+  @param[in]  IpInstance         The IP4 child to clean up.
+
+  @retval EFI_SUCCESS            The IP4 child is cleaned up.
+  @retval EFI_DEVICE_ERROR       Some resources failed to be released.
+
+**/
+EFI_STATUS
+Ip4CleanProtocol (
+  IN  IP4_PROTOCOL          *IpInstance
+  )
+{
+  if (EFI_ERROR (Ip4Cancel (IpInstance, NULL))) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  if (EFI_ERROR (Ip4Groups (IpInstance, FALSE, NULL))) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // Some packets haven't been recycled. It is because either the
+  // user forgets to recycle the packets, or because the callback
+  // hasn't been called. Just leave it alone.
+  //
+  if (!IsListEmpty (&IpInstance->Delivered)) {
+    ;
+  }
+
+  if (IpInstance->Interface != NULL) {
+    RemoveEntryList (&IpInstance->AddrLink);
+    if (IpInstance->Interface->Arp != NULL) {
+      gBS->CloseProtocol (
+             IpInstance->Interface->ArpHandle,
+             &gEfiArpProtocolGuid,
+             gIp4DriverBinding.DriverBindingHandle,
+             IpInstance->Handle
+             );
+    }
+    Ip4FreeInterface (IpInstance->Interface, IpInstance);
+    IpInstance->Interface = NULL;
+  }
+
+  if (IpInstance->RouteTable != NULL) {
+    if (IpInstance->RouteTable->Next != NULL) {
+      Ip4FreeRouteTable (IpInstance->RouteTable->Next);
+    }
+
+    Ip4FreeRouteTable (IpInstance->RouteTable);
+    IpInstance->RouteTable = NULL;
+  }
+
+  if (IpInstance->EfiRouteTable != NULL) {
+    FreePool (IpInstance->EfiRouteTable);
+    IpInstance->EfiRouteTable = NULL;
+    IpInstance->EfiRouteCount = 0;
+  }
+
+  if (IpInstance->Groups != NULL) {
+    FreePool (IpInstance->Groups);
+    IpInstance->Groups      = NULL;
+    IpInstance->GroupCount  = 0;
+  }
+
+  NetMapClean (&IpInstance->TxTokens);
+
+  NetMapClean (&IpInstance->RxTokens);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Validate that Ip/Netmask pair is OK to be used as station
+  address. Only continuous netmasks are supported. and check
+  that StationAddress is a unicast address on the newtwork.
+
+  @param[in]  Ip                 The IP address to validate.
+  @param[in]  Netmask            The netmaks of the IP.
+
+  @retval TRUE                   The Ip/Netmask pair is valid.
+  @retval FALSE                  The Ip/Netmask pair is invalid.
+
+**/
+BOOLEAN
+Ip4StationAddressValid (
+  IN IP4_ADDR               Ip,
+  IN IP4_ADDR               Netmask
+  )
+{
+  IP4_ADDR                  NetBrdcastMask;
+  INTN                      Len;
+  INTN                      Type;
+
+  //
+  // Only support the station address with 0.0.0.0/0 to enable DHCP client.
+  //
+  if (Netmask == IP4_ALLZERO_ADDRESS) {
+    return (BOOLEAN) (Ip == IP4_ALLZERO_ADDRESS);
+  }
+
+  //
+  // Only support the continuous net masks
+  //
+  if ((Len = NetGetMaskLength (Netmask)) == IP4_MASK_NUM) {
+    return FALSE;
+  }
+
+  //
+  // Station address can't be class D or class E address
+  //
+  if ((Type = NetGetIpClass (Ip)) > IP4_ADDR_CLASSC) {
+    return FALSE;
+  }
+
+  //
+  // Station address can't be subnet broadcast/net broadcast address
+  //
+  if ((Ip == (Ip & Netmask)) || (Ip == (Ip | ~Netmask))) {
+    return FALSE;
+  }
+
+  NetBrdcastMask = gIp4AllMasks[MIN (Len, Type << 3)];
+
+  if (Ip == (Ip | ~NetBrdcastMask)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Assigns an IPv4 address and subnet mask to this EFI IPv4 Protocol driver instance.
+
+  The Configure() function is used to set, change, or reset the operational
+  parameters and filter settings for this EFI IPv4 Protocol instance. Until these
+  parameters have been set, no network traffic can be sent or received by this
+  instance. Once the parameters have been reset (by calling this function with
+  IpConfigData set to NULL), no more traffic can be sent or received until these
+  parameters have been set again. Each EFI IPv4 Protocol instance can be started
+  and stopped independently of each other by enabling or disabling their receive
+  filter settings with the Configure() function.
+
+  When IpConfigData.UseDefaultAddress is set to FALSE, the new station address will
+  be appended as an alias address into the addresses list in the EFI IPv4 Protocol
+  driver. While set to TRUE, Configure() will trigger the EFI_IP4_CONFIG_PROTOCOL
+  to retrieve the default IPv4 address if it is not available yet. Clients could
+  frequently call GetModeData() to check the status to ensure that the default IPv4
+  address is ready.
+
+  If operational parameters are reset or changed, any pending transmit and receive
+  requests will be cancelled. Their completion token status will be set to EFI_ABORTED
+  and their events will be signaled.
+
+  @param[in]  This              Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  IpConfigData      Pointer to the EFI IPv4 Protocol configuration data structure.
+
+  @retval EFI_SUCCESS           The driver instance was successfully opened.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+  @retval EFI_UNSUPPORTED       One or more of the following conditions is TRUE:
+                                A configuration protocol (DHCP, BOOTP, RARP, etc.) could
+                                not be located when clients choose to use the default IPv4
+                                address. This EFI IPv4 Protocol implementation does not
+                                support this requested filter or timeout setting.
+  @retval EFI_OUT_OF_RESOURCES  The EFI IPv4 Protocol driver instance data could not be allocated.
+  @retval EFI_ALREADY_STARTED   The interface is already open and must be stopped before the
+                                IPv4 address or subnet mask can be changed. The interface must
+                                also be stopped when switching to/from raw packet mode.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred. The EFI IPv4
+                                Protocol driver instance is not opened.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Configure (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN EFI_IP4_CONFIG_DATA    *IpConfigData       OPTIONAL
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  EFI_IP4_CONFIG_DATA       *Current;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+  BOOLEAN                   AddrOk;
+  IP4_ADDR                  IpAddress;
+  IP4_ADDR                  SubnetMask;
+
+  //
+  // First, validate the parameters
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+  OldTpl     = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Validate the configuration first.
+  //
+  if (IpConfigData != NULL) {
+
+    CopyMem (&IpAddress, &IpConfigData->StationAddress, sizeof (IP4_ADDR));
+    CopyMem (&SubnetMask, &IpConfigData->SubnetMask, sizeof (IP4_ADDR));
+
+    IpAddress  = NTOHL (IpAddress);
+    SubnetMask = NTOHL (SubnetMask);
+
+    //
+    // Check whether the station address is a valid unicast address
+    //
+    if (!IpConfigData->UseDefaultAddress) {
+      AddrOk = Ip4StationAddressValid (IpAddress, SubnetMask);
+
+      if (!AddrOk) {
+        Status = EFI_INVALID_PARAMETER;
+        goto ON_EXIT;
+      }
+    }
+
+    //
+    // User can only update packet filters when already configured.
+    // If it wants to change the station address, it must configure(NULL)
+    // the instance first.
+    //
+    if (IpInstance->State == IP4_STATE_CONFIGED) {
+      Current = &IpInstance->ConfigData;
+
+      if (Current->UseDefaultAddress != IpConfigData->UseDefaultAddress) {
+        Status = EFI_ALREADY_STARTED;
+        goto ON_EXIT;
+      }
+
+      if (!Current->UseDefaultAddress &&
+         (!EFI_IP4_EQUAL (&Current->StationAddress, &IpConfigData->StationAddress) ||
+          !EFI_IP4_EQUAL (&Current->SubnetMask, &IpConfigData->SubnetMask))) {
+        Status = EFI_ALREADY_STARTED;
+        goto ON_EXIT;
+      }
+
+      if (Current->UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+        Status = EFI_NO_MAPPING;
+        goto ON_EXIT;
+      }
+    }
+  }
+
+  //
+  // Configure the instance or clean it up.
+  //
+  if (IpConfigData != NULL) {
+    Status = Ip4ConfigProtocol (IpInstance, IpConfigData);
+  } else {
+    Status = Ip4CleanProtocol (IpInstance);
+
+    //
+    // Don't change the state if it is DESTROY, consider the following
+    // valid sequence: Mnp is unloaded-->Ip Stopped-->Udp Stopped,
+    // Configure (ThisIp, NULL). If the state is changed to UNCONFIGED,
+    // the unload fails miserably.
+    //
+    if (IpInstance->State == IP4_STATE_CONFIGED) {
+      IpInstance->State = IP4_STATE_UNCONFIGED;
+    }
+  }
+
+  //
+  // Update the MNP's configure data. Ip4ServiceConfigMnp will check
+  // whether it is necessary to reconfigure the MNP.
+  //
+  Ip4ServiceConfigMnp (IpInstance->Service, FALSE);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+
+}
+
+
+/**
+  Change the IP4 child's multicast setting. The caller
+  should make sure that the parameters is valid.
+
+  @param[in]  IpInstance             The IP4 child to change the setting.
+  @param[in]  JoinFlag               TRUE to join the group, otherwise leave it.
+  @param[in]  GroupAddress           The target group address.
+
+  @retval EFI_ALREADY_STARTED    Want to join the group, but already a member of it.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate some resources.
+  @retval EFI_DEVICE_ERROR       Failed to set the group configuraton.
+  @retval EFI_SUCCESS            Successfully updated the group setting.
+  @retval EFI_NOT_FOUND          Try to leave the group which it isn't a member.
+
+**/
+EFI_STATUS
+Ip4Groups (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN BOOLEAN                JoinFlag,
+  IN EFI_IPv4_ADDRESS       *GroupAddress       OPTIONAL
+  )
+{
+  IP4_ADDR                  *Members;
+  IP4_ADDR                  Group;
+  UINT32                    Index;
+
+  //
+  // Add it to the instance's Groups, and join the group by IGMP.
+  // IpInstance->Groups is in network byte order. IGMP operates in
+  // host byte order
+  //
+  if (JoinFlag) {
+    //
+    // When JoinFlag is TRUE, GroupAddress shouldn't be NULL.
+    //
+    ASSERT (GroupAddress != NULL);
+    CopyMem (&Group, GroupAddress, sizeof (IP4_ADDR));
+
+    for (Index = 0; Index < IpInstance->GroupCount; Index++) {
+      if (IpInstance->Groups[Index] == Group) {
+        return EFI_ALREADY_STARTED;
+      }
+    }
+
+    Members = Ip4CombineGroups (IpInstance->Groups, IpInstance->GroupCount, Group);
+
+    if (Members == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    if (EFI_ERROR (Ip4JoinGroup (IpInstance, NTOHL (Group)))) {
+      FreePool (Members);
+      return EFI_DEVICE_ERROR;
+    }
+
+    if (IpInstance->Groups != NULL) {
+      FreePool (IpInstance->Groups);
+    }
+
+    IpInstance->Groups = Members;
+    IpInstance->GroupCount++;
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Leave the group. Leave all the groups if GroupAddress is NULL.
+  // Must iterate from the end to the beginning because the GroupCount
+  // is decreamented each time an address is removed..
+  //
+  for (Index = IpInstance->GroupCount; Index > 0 ; Index--) {
+    Group = IpInstance->Groups[Index - 1];
+
+    if ((GroupAddress == NULL) || EFI_IP4_EQUAL (&Group, GroupAddress)) {
+      if (EFI_ERROR (Ip4LeaveGroup (IpInstance, NTOHL (Group)))) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      Ip4RemoveGroupAddr (IpInstance->Groups, IpInstance->GroupCount, Group);
+      IpInstance->GroupCount--;
+
+      if (IpInstance->GroupCount == 0) {
+        ASSERT (Index == 1);
+
+        FreePool (IpInstance->Groups);
+        IpInstance->Groups = NULL;
+      }
+
+      if (GroupAddress != NULL) {
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return ((GroupAddress != NULL) ? EFI_NOT_FOUND : EFI_SUCCESS);
+}
+
+
+/**
+  Joins and leaves multicast groups.
+
+  The Groups() function is used to join and leave multicast group sessions. Joining
+  a group will enable reception of matching multicast packets. Leaving a group will
+  disable the multicast packet reception.
+
+  If JoinFlag is FALSE and GroupAddress is NULL, all joined groups will be left.
+
+  @param[in]  This                  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  JoinFlag              Set to TRUE to join the multicast group session and FALSE to leave.
+  @param[in]  GroupAddress          Pointer to the IPv4 multicast address.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the following is TRUE:
+                                - This is NULL.
+                                - JoinFlag is TRUE and GroupAddress is NULL.
+                                - GroupAddress is not NULL and *GroupAddress is
+                                not a multicast IPv4 address.
+  @retval EFI_NOT_STARTED       This instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_OUT_OF_RESOURCES  System resources could not be allocated.
+  @retval EFI_UNSUPPORTED       This EFI IPv4 Protocol implementation does not support multicast groups.
+  @retval EFI_ALREADY_STARTED   The group address is already in the group table (when
+                                JoinFlag is TRUE).
+  @retval EFI_NOT_FOUND         The group address is not in the group table (when JoinFlag is FALSE).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Groups (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN BOOLEAN                JoinFlag,
+  IN EFI_IPv4_ADDRESS       *GroupAddress     OPTIONAL
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  IP4_ADDR                  McastIp;
+
+  if ((This == NULL) || (JoinFlag && (GroupAddress == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GroupAddress != NULL) {
+    CopyMem (&McastIp, GroupAddress, sizeof (IP4_ADDR));
+
+    if (!IP4_IS_MULTICAST (NTOHL (McastIp))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+  OldTpl     = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (IpInstance->ConfigData.UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+    Status = EFI_NO_MAPPING;
+    goto ON_EXIT;
+  }
+
+  Status = Ip4Groups (IpInstance, JoinFlag, GroupAddress);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Adds and deletes routing table entries.
+
+  The Routes() function adds a route to or deletes a route from the routing table.
+
+  Routes are determined by comparing the SubnetAddress with the destination IPv4
+  address arithmetically AND-ed with the SubnetMask. The gateway address must be
+  on the same subnet as the configured station address.
+
+  The default route is added with SubnetAddress and SubnetMask both set to 0.0.0.0.
+  The default route matches all destination IPv4 addresses that do not match any
+  other routes.
+
+  A GatewayAddress that is zero is a nonroute. Packets are sent to the destination
+  IP address if it can be found in the ARP cache or on the local subnet. One automatic
+  nonroute entry will be inserted into the routing table for outgoing packets that
+  are addressed to a local subnet (gateway address of 0.0.0.0).
+
+  Each EFI IPv4 Protocol instance has its own independent routing table. Those EFI
+  IPv4 Protocol instances that use the default IPv4 address will also have copies
+  of the routing table that was provided by the EFI_IP4_CONFIG_PROTOCOL, and these
+  copies will be updated whenever the EIF IPv4 Protocol driver reconfigures its
+  instances. As a result, client modification to the routing table will be lost.
+
+  @param[in]  This                   Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  DeleteRoute            Set to TRUE to delete this route from the routing table. Set to
+                                     FALSE to add this route to the routing table. SubnetAddress
+                                     and SubnetMask are used as the key to each route entry.
+  @param[in]  SubnetAddress          The address of the subnet that needs to be routed.
+  @param[in]  SubnetMask             The subnet mask of SubnetAddress.
+  @param[in]  GatewayAddress         The unicast gateway IPv4 address for this route.
+
+  @retval EFI_SUCCESS            The operation completed successfully.
+  @retval EFI_NOT_STARTED        The driver instance has not been started.
+  @retval EFI_NO_MAPPING         When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 - This is NULL.
+                                 - SubnetAddress is NULL.
+                                 - SubnetMask is NULL.
+                                 - GatewayAddress is NULL.
+                                 - *SubnetAddress is not a valid subnet address.
+                                 - *SubnetMask is not a valid subnet mask.
+                                 - *GatewayAddress is not a valid unicast IPv4 address.
+  @retval EFI_OUT_OF_RESOURCES   Could not add the entry to the routing table.
+  @retval EFI_NOT_FOUND          This route is not in the routing table (when DeleteRoute is TRUE).
+  @retval EFI_ACCESS_DENIED      The route is already defined in the routing table (when
+                                  DeleteRoute is FALSE).
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Routes (
+  IN EFI_IP4_PROTOCOL       *This,
+  IN BOOLEAN                DeleteRoute,
+  IN EFI_IPv4_ADDRESS       *SubnetAddress,
+  IN EFI_IPv4_ADDRESS       *SubnetMask,
+  IN EFI_IPv4_ADDRESS       *GatewayAddress
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  IP4_INTERFACE             *IpIf;
+  IP4_ADDR                  Dest;
+  IP4_ADDR                  Netmask;
+  IP4_ADDR                  Nexthop;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First, validate the parameters
+  //
+  if ((This == NULL) || (SubnetAddress == NULL) ||
+      (SubnetMask == NULL) || (GatewayAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+  OldTpl     = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (IpInstance->ConfigData.UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+    Status = EFI_NO_MAPPING;
+    goto ON_EXIT;
+  }
+
+  CopyMem (&Dest, SubnetAddress, sizeof (IP4_ADDR));
+  CopyMem (&Netmask, SubnetMask, sizeof (IP4_ADDR));
+  CopyMem (&Nexthop, GatewayAddress, sizeof (IP4_ADDR));
+
+  Dest    = NTOHL (Dest);
+  Netmask = NTOHL (Netmask);
+  Nexthop = NTOHL (Nexthop);
+
+  IpIf    = IpInstance->Interface;
+
+  if (!IP4_IS_VALID_NETMASK (Netmask)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  //
+  // the gateway address must be a unicast on the connected network if not zero.
+  //
+  if ((Nexthop != IP4_ALLZERO_ADDRESS) &&
+      (!IP4_NET_EQUAL (Nexthop, IpIf->Ip, IpIf->SubnetMask) ||
+        IP4_IS_BROADCAST (Ip4GetNetCast (Nexthop, IpIf)))) {
+
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (DeleteRoute) {
+    Status = Ip4DelRoute (IpInstance->RouteTable, Dest, Netmask, Nexthop);
+  } else {
+    Status = Ip4AddRoute (IpInstance->RouteTable, Dest, Netmask, Nexthop);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Check whether the user's token or event has already
+  been enqueued on IP4's list.
+
+  @param[in]  Map                    The container of either user's transmit or receive
+                                     token.
+  @param[in]  Item                   Current item to check against.
+  @param[in]  Context                The Token to check againist.
+
+  @retval EFI_ACCESS_DENIED      The token or event has already been enqueued in IP.
+  @retval EFI_SUCCESS            The current item isn't the same token/event as the
+                                 context.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4TokenExist (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Context
+  )
+{
+  EFI_IP4_COMPLETION_TOKEN  *Token;
+  EFI_IP4_COMPLETION_TOKEN  *TokenInItem;
+
+  Token       = (EFI_IP4_COMPLETION_TOKEN *) Context;
+  TokenInItem = (EFI_IP4_COMPLETION_TOKEN *) Item->Key;
+
+  if ((Token == TokenInItem) || (Token->Event == TokenInItem->Event)) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Validate the user's token against current station address.
+
+  @param[in]  Token              User's token to validate.
+  @param[in]  IpIf               The IP4 child's interface.
+  @param[in]  RawData            Set to TRUE to send unformatted packets.
+
+  @retval EFI_INVALID_PARAMETER  Some parameters are invalid.
+  @retval EFI_BAD_BUFFER_SIZE    The user's option/data is too long.
+  @retval EFI_SUCCESS            The token is valid.
+
+**/
+EFI_STATUS
+Ip4TxTokenValid (
+  IN EFI_IP4_COMPLETION_TOKEN   *Token,
+  IN IP4_INTERFACE              *IpIf,
+  IN BOOLEAN                    RawData
+  )
+{
+  EFI_IP4_TRANSMIT_DATA     *TxData;
+  EFI_IP4_OVERRIDE_DATA     *Override;
+  IP4_ADDR                  Src;
+  IP4_ADDR                  Gateway;
+  UINT32                    Offset;
+  UINT32                    Index;
+  UINT32                    HeadLen;
+
+  if ((Token == NULL) || (Token->Event == NULL) || (Token->Packet.TxData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TxData = Token->Packet.TxData;
+
+  //
+  // Check the fragment table: no empty fragment, and length isn't bogus.
+  //
+  if ((TxData->TotalDataLength == 0) || (TxData->FragmentCount == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Offset = TxData->TotalDataLength;
+
+  if (Offset > IP4_MAX_PACKET_SIZE) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  for (Index = 0; Index < TxData->FragmentCount; Index++) {
+    if ((TxData->FragmentTable[Index].FragmentBuffer == NULL) ||
+        (TxData->FragmentTable[Index].FragmentLength == 0)) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Offset -= TxData->FragmentTable[Index].FragmentLength;
+  }
+
+  if (Offset != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // NOTE that OptionsLength/OptionsBuffer/OverrideData are ignored if RawData
+  // is TRUE.
+  //
+  if (RawData) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Check the IP options: no more than 40 bytes and format is OK
+  //
+  if (TxData->OptionsLength != 0) {
+    if ((TxData->OptionsLength > 40) || (TxData->OptionsBuffer == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (!Ip4OptionIsValid (TxData->OptionsBuffer, TxData->OptionsLength, FALSE)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Check the source and gateway: they must be a valid unicast.
+  // Gateway must also be on the connected network.
+  //
+  if (TxData->OverrideData != NULL) {
+    Override = TxData->OverrideData;
+
+    CopyMem (&Src, &Override->SourceAddress, sizeof (IP4_ADDR));
+    CopyMem (&Gateway, &Override->GatewayAddress, sizeof (IP4_ADDR));
+
+    Src     = NTOHL (Src);
+    Gateway = NTOHL (Gateway);
+
+    if ((NetGetIpClass (Src) > IP4_ADDR_CLASSC) ||
+        (Src == IP4_ALLONE_ADDRESS) ||
+        IP4_IS_BROADCAST (Ip4GetNetCast (Src, IpIf))) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // If gateway isn't zero, it must be a unicast address, and
+    // on the connected network.
+    //
+    if ((Gateway != IP4_ALLZERO_ADDRESS) &&
+        ((NetGetIpClass (Gateway) > IP4_ADDR_CLASSC) ||
+         !IP4_NET_EQUAL (Gateway, IpIf->Ip, IpIf->SubnetMask) ||
+         IP4_IS_BROADCAST (Ip4GetNetCast (Gateway, IpIf)))) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Check the packet length: Head length and packet length all has a limit
+  //
+  HeadLen = sizeof (IP4_HEAD) + ((TxData->OptionsLength + 3) &~0x03);
+
+  if ((HeadLen > IP4_MAX_HEADLEN) ||
+      (TxData->TotalDataLength + HeadLen > IP4_MAX_PACKET_SIZE)) {
+
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The callback function for the net buffer which wraps the user's
+  transmit token. Although it seems this function is pretty simple,
+  there are some subtle things.
+  When user requests the IP to transmit a packet by passing it a
+  token, the token is wrapped in an IP4_TXTOKEN_WRAP and the data
+  is wrapped in an net buffer. the net buffer's Free function is
+  set to Ip4FreeTxToken. The Token and token wrap are added to the
+  IP child's TxToken map. Then the buffer is passed to Ip4Output for
+  transmission. If something error happened before that, the buffer
+  is freed, which in turn will free the token wrap. The wrap may
+  have been added to the TxToken map or not, and the user's event
+  shouldn't be fired because we are still in the EfiIp4Transmit. If
+  the buffer has been sent by Ip4Output, it should be removed from
+  the TxToken map and user's event signaled. The token wrap and buffer
+  are bound together. Check the comments in Ip4Output for information
+  about IP fragmentation.
+
+  @param[in]  Context                The token's wrap.
+
+**/
+VOID
+EFIAPI
+Ip4FreeTxToken (
+  IN VOID                   *Context
+  )
+{
+  IP4_TXTOKEN_WRAP          *Wrap;
+  NET_MAP_ITEM              *Item;
+
+  Wrap = (IP4_TXTOKEN_WRAP *) Context;
+
+  //
+  // Signal IpSecRecycleEvent to inform IPsec free the memory
+  //
+  if (Wrap->IpSecRecycleSignal != NULL) {
+    gBS->SignalEvent (Wrap->IpSecRecycleSignal);
+  }
+
+  //
+  // Find the token in the instance's map. EfiIp4Transmit put the
+  // token to the map. If that failed, NetMapFindKey will return NULL.
+  //
+  Item = NetMapFindKey (&Wrap->IpInstance->TxTokens, Wrap->Token);
+
+  if (Item != NULL) {
+    NetMapRemoveItem (&Wrap->IpInstance->TxTokens, Item, NULL);
+  }
+
+  if (Wrap->Sent) {
+    gBS->SignalEvent (Wrap->Token->Event);
+
+    //
+    // Dispatch the DPC queued by the NotifyFunction of Token->Event.
+    //
+    DispatchDpc ();
+  }
+
+  FreePool (Wrap);
+}
+
+
+/**
+  The callback function to Ip4Output to update the transmit status.
+
+  @param  Ip4Instance            The Ip4Instance that request the transmit.
+  @param  Packet                 The user's transmit request.
+  @param  IoStatus               The result of the transmission.
+  @param  Flag                   Not used during transmission.
+  @param  Context                The token's wrap.
+
+**/
+VOID
+Ip4OnPacketSent (
+  IP4_PROTOCOL              *Ip4Instance,
+  NET_BUF                   *Packet,
+  EFI_STATUS                IoStatus,
+  UINT32                    Flag,
+  VOID                      *Context
+  )
+{
+  IP4_TXTOKEN_WRAP          *Wrap;
+
+  //
+  // This is the transmission request from upper layer,
+  // not the IP4 driver itself.
+  //
+  ASSERT (Ip4Instance != NULL);
+
+  //
+  // The first fragment of the packet has been sent. Update
+  // the token's status. That is, if fragmented, the transmit's
+  // status is the first fragment's status. The Wrap will be
+  // release when all the fragments are release. Check the comments
+  // in Ip4FreeTxToken and Ip4Output for information.
+  //
+  Wrap                = (IP4_TXTOKEN_WRAP *) Context;
+  Wrap->Token->Status = IoStatus;
+
+  NetbufFree (Wrap->Packet);
+}
+
+
+/**
+  Places outgoing data packets into the transmit queue.
+
+  The Transmit() function places a sending request in the transmit queue of this
+  EFI IPv4 Protocol instance. Whenever the packet in the token is sent out or some
+  errors occur, the event in the token will be signaled and the status is updated.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to the transmit token.
+
+  @retval  EFI_SUCCESS           The data has been queued for transmission.
+  @retval  EFI_NOT_STARTED       This instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_INVALID_PARAMETER One or more pameters are invalid.
+  @retval  EFI_ACCESS_DENIED     The transmit completion token with the same Token.Event
+                                 was already in the transmit queue.
+  @retval  EFI_NOT_READY         The completion token could not be queued because the transmit
+                                 queue is full.
+  @retval  EFI_NOT_FOUND         Not route is found to destination address.
+  @retval  EFI_OUT_OF_RESOURCES  Could not queue the transmit data.
+  @retval  EFI_BUFFER_TOO_SMALL  Token.Packet.TxData.TotalDataLength is too
+                                 short to transmit.
+  @retval  EFI_BAD_BUFFER_SIZE   The length of the IPv4 header + option length + total data length is
+                                 greater than MTU (or greater than the maximum packet size if
+                                 Token.Packet.TxData.OverrideData.
+                                 DoNotFragment is TRUE).
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Transmit (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token
+  )
+{
+  IP4_SERVICE               *IpSb;
+  IP4_PROTOCOL              *IpInstance;
+  IP4_INTERFACE             *IpIf;
+  IP4_TXTOKEN_WRAP          *Wrap;
+  EFI_IP4_TRANSMIT_DATA     *TxData;
+  EFI_IP4_CONFIG_DATA       *Config;
+  EFI_IP4_OVERRIDE_DATA     *Override;
+  IP4_HEAD                  Head;
+  IP4_ADDR                  GateWay;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  BOOLEAN                   DontFragment;
+  UINT32                    HeadLen;
+  UINT8                     RawHdrLen;
+  UINT32                    OptionsLength;
+  UINT8                     *OptionsBuffer;
+  VOID                      *FirstFragment;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl  = gBS->RaiseTPL (TPL_CALLBACK);
+
+  IpSb    = IpInstance->Service;
+  IpIf    = IpInstance->Interface;
+  Config  = &IpInstance->ConfigData;
+
+  if (Config->UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+    Status = EFI_NO_MAPPING;
+    goto ON_EXIT;
+  }
+
+  //
+  // make sure that token is properly formated
+  //
+  Status = Ip4TxTokenValid (Token, IpIf, Config->RawData);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Check whether the token or signal already existed.
+  //
+  if (EFI_ERROR (NetMapIterate (&IpInstance->TxTokens, Ip4TokenExist, Token))) {
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Build the IP header, need to fill in the Tos, TotalLen, Id,
+  // fragment, Ttl, protocol, Src, and Dst.
+  //
+  TxData = Token->Packet.TxData;
+
+  FirstFragment = NULL;
+
+  if (Config->RawData) {
+    //
+    // When RawData is TRUE, first buffer in FragmentTable points to a raw
+    // IPv4 fragment including IPv4 header and options.
+    //
+    FirstFragment = TxData->FragmentTable[0].FragmentBuffer;
+    CopyMem (&RawHdrLen, FirstFragment, sizeof (UINT8));
+
+    RawHdrLen = (UINT8) (RawHdrLen & 0x0f);
+    if (RawHdrLen < 5) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    RawHdrLen = (UINT8) (RawHdrLen << 2);
+    
+    CopyMem (&Head, FirstFragment, IP4_MIN_HEADLEN);
+
+    Ip4NtohHead (&Head);
+    HeadLen      = 0;
+    DontFragment = IP4_DO_NOT_FRAGMENT (Head.Fragment);
+
+    if (!DontFragment) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    GateWay = IP4_ALLZERO_ADDRESS;
+
+    //
+    // Get IPv4 options from first fragment.
+    //
+    if (RawHdrLen == IP4_MIN_HEADLEN) {
+      OptionsLength = 0;
+      OptionsBuffer = NULL;
+    } else {
+      OptionsLength = RawHdrLen - IP4_MIN_HEADLEN;
+      OptionsBuffer = (UINT8 *) FirstFragment + IP4_MIN_HEADLEN;
+    }
+
+    //
+    // Trim off IPv4 header and options from first fragment.
+    //
+    TxData->FragmentTable[0].FragmentBuffer = (UINT8 *) FirstFragment + RawHdrLen;
+    TxData->FragmentTable[0].FragmentLength = TxData->FragmentTable[0].FragmentLength - RawHdrLen;
+  } else {
+    CopyMem (&Head.Dst, &TxData->DestinationAddress, sizeof (IP4_ADDR));
+    Head.Dst = NTOHL (Head.Dst);
+
+    if (TxData->OverrideData != NULL) {
+      Override      = TxData->OverrideData;
+      Head.Protocol = Override->Protocol;
+      Head.Tos      = Override->TypeOfService;
+      Head.Ttl      = Override->TimeToLive;
+      DontFragment  = Override->DoNotFragment;
+
+      CopyMem (&Head.Src, &Override->SourceAddress, sizeof (IP4_ADDR));
+      CopyMem (&GateWay, &Override->GatewayAddress, sizeof (IP4_ADDR));
+
+      Head.Src = NTOHL (Head.Src);
+      GateWay  = NTOHL (GateWay);
+    } else {
+      Head.Src      = IpIf->Ip;
+      GateWay       = IP4_ALLZERO_ADDRESS;
+      Head.Protocol = Config->DefaultProtocol;
+      Head.Tos      = Config->TypeOfService;
+      Head.Ttl      = Config->TimeToLive;
+      DontFragment  = Config->DoNotFragment;
+    }
+
+    Head.Fragment = IP4_HEAD_FRAGMENT_FIELD (DontFragment, FALSE, 0);
+    HeadLen       = (TxData->OptionsLength + 3) & (~0x03);
+
+    OptionsLength = TxData->OptionsLength;
+    OptionsBuffer = (UINT8 *) (TxData->OptionsBuffer);
+  }
+
+  //
+  // If don't fragment and fragment needed, return error
+  //
+  if (DontFragment && (TxData->TotalDataLength + HeadLen > IpSb->MaxPacketSize)) {
+    Status = EFI_BAD_BUFFER_SIZE;
+    goto ON_EXIT;
+  }
+
+  //
+  // OK, it survives all the validation check. Wrap the token in
+  // a IP4_TXTOKEN_WRAP and the data in a netbuf
+  //
+  Status = EFI_OUT_OF_RESOURCES;
+  Wrap   = AllocateZeroPool (sizeof (IP4_TXTOKEN_WRAP));
+  if (Wrap == NULL) {
+    goto ON_EXIT;
+  }
+
+  Wrap->IpInstance  = IpInstance;
+  Wrap->Token       = Token;
+  Wrap->Sent        = FALSE;
+  Wrap->Life        = IP4_US_TO_SEC (Config->TransmitTimeout);
+  Wrap->Packet      = NetbufFromExt (
+                        (NET_FRAGMENT *) TxData->FragmentTable,
+                        TxData->FragmentCount,
+                        IP4_MAX_HEADLEN,
+                        0,
+                        Ip4FreeTxToken,
+                        Wrap
+                        );
+
+  if (Wrap->Packet == NULL) {
+    FreePool (Wrap);
+    goto ON_EXIT;
+  }
+
+  Token->Status = EFI_NOT_READY;
+
+  if (EFI_ERROR (NetMapInsertTail (&IpInstance->TxTokens, Token, Wrap))) {
+    //
+    // NetbufFree will call Ip4FreeTxToken, which in turn will
+    // free the IP4_TXTOKEN_WRAP. Now, the token wrap hasn't been
+    // enqueued.
+    //
+    if (Config->RawData) {
+      //
+      // Restore pointer of first fragment in RawData mode.
+      //
+      TxData->FragmentTable[0].FragmentBuffer = (UINT8 *) FirstFragment;
+    }
+
+    NetbufFree (Wrap->Packet);
+    goto ON_EXIT;
+  }
+
+  //
+  // Mark the packet sent before output it. Mark it not sent again if the
+  // returned status is not EFI_SUCCESS;
+  //
+  Wrap->Sent = TRUE;
+
+  Status = Ip4Output (
+             IpSb,
+             IpInstance,
+             Wrap->Packet,
+             &Head,
+             OptionsBuffer,
+             OptionsLength,
+             GateWay,
+             Ip4OnPacketSent,
+             Wrap
+             );
+
+  if (EFI_ERROR (Status)) {
+    Wrap->Sent = FALSE;
+
+    if (Config->RawData) {
+      //
+      // Restore pointer of first fragment in RawData mode.
+      //
+      TxData->FragmentTable[0].FragmentBuffer = (UINT8 *) FirstFragment;
+    }
+
+    NetbufFree (Wrap->Packet);
+  }
+
+  if (Config->RawData) {
+    //
+    // Restore pointer of first fragment in RawData mode.
+    //
+    TxData->FragmentTable[0].FragmentBuffer = (UINT8 *) FirstFragment;
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Places a receiving request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet queue.
+  This function is always asynchronous.
+
+  The Token.Event field in the completion token must be filled in by the caller
+  and cannot be NULL. When the receive operation completes, the EFI IPv4 Protocol
+  driver updates the Token.Status and Token.Packet.RxData fields and the Token.Event
+  is signaled.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that is associated with the receive data descriptor.
+
+  @retval EFI_SUCCESS           The receive completion token was cached.
+  @retval EFI_NOT_STARTED       This EFI IPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP, RARP, etc.)
+                                is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                - This is NULL.
+                                - Token is NULL.
+                                - Token.Event is NULL.
+  @retval EFI_OUT_OF_RESOURCES  The receive completion token could not be queued due to a lack of system
+                                resources (usually memory).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+                                The EFI IPv4 Protocol instance has been reset to startup defaults.
+                                EFI_ACCESS_DENIED The receive completion token with the same Token.Event was already
+                                in the receive queue.
+  @retval EFI_NOT_READY         The receive request could not be queued because the receive queue is full.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Receive (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  //
+  // First validate the parameters
+  //
+  if ((This == NULL) || (Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Check whether the toke is already on the receive queue.
+  //
+  Status = NetMapIterate (&IpInstance->RxTokens, Ip4TokenExist, Token);
+
+  if (EFI_ERROR (Status)) {
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Queue the token then check whether there is pending received packet.
+  //
+  Status = NetMapInsertTail (&IpInstance->RxTokens, Token, NULL);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = Ip4InstanceDeliverPacket (IpInstance);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of this instane's receive
+  // event.
+  //
+  DispatchDpc ();
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Cancel the transmitted but not recycled packet. If a matching
+  token is found, it will call Ip4CancelPacket to cancel the
+  packet. Ip4CancelPacket will cancel all the fragments of the
+  packet. When all the fragments are freed, the IP4_TXTOKEN_WRAP
+  will be deleted from the Map, and user's event signalled.
+  Because Ip4CancelPacket and other functions are all called in
+  line, so, after Ip4CancelPacket returns, the Item has been freed.
+
+  @param[in]  Map                The IP4 child's transmit queue.
+  @param[in]  Item               The current transmitted packet to test.
+  @param[in]  Context            The user's token to cancel.
+
+  @retval EFI_SUCCESS            Continue to check the next Item.
+  @retval EFI_ABORTED            The user's Token (Token != NULL) is cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4CancelTxTokens (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Context
+  )
+{
+  EFI_IP4_COMPLETION_TOKEN  *Token;
+  IP4_TXTOKEN_WRAP          *Wrap;
+
+  Token = (EFI_IP4_COMPLETION_TOKEN *) Context;
+
+  //
+  // Return EFI_SUCCESS to check the next item in the map if
+  // this one doesn't match.
+  //
+  if ((Token != NULL) && (Token != Item->Key)) {
+    return EFI_SUCCESS;
+  }
+
+  Wrap = (IP4_TXTOKEN_WRAP *) Item->Value;
+  ASSERT (Wrap != NULL);
+
+  //
+  // Don't access the Item, Wrap and Token's members after this point.
+  // Item and wrap has been freed. And we no longer own the Token.
+  //
+  Ip4CancelPacket (Wrap->IpInstance->Interface, Wrap->Packet, EFI_ABORTED);
+
+  //
+  // If only one item is to be cancel, return EFI_ABORTED to stop
+  // iterating the map any more.
+  //
+  if (Token != NULL) {
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Cancel the receive request. This is quiet simple, because
+  it is only enqueued in our local receive map.
+
+  @param[in]  Map                The IP4 child's receive queue.
+  @param[in]  Item               Current receive request to cancel.
+  @param[in]  Context            The user's token to cancel.
+
+  @retval EFI_SUCCESS            Continue to check the next receive request on the
+                                 queue.
+  @retval EFI_ABORTED            The user's token (token != NULL) has been
+                                 cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4CancelRxTokens (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Context
+  )
+{
+  EFI_IP4_COMPLETION_TOKEN  *Token;
+  EFI_IP4_COMPLETION_TOKEN  *This;
+
+  Token = (EFI_IP4_COMPLETION_TOKEN *) Context;
+  This  = Item->Key;
+
+  if ((Token != NULL) && (Token != This)) {
+    return EFI_SUCCESS;
+  }
+
+  NetMapRemoveItem (Map, Item, NULL);
+
+  This->Status        = EFI_ABORTED;
+  This->Packet.RxData = NULL;
+  gBS->SignalEvent (This->Event);
+
+  if (Token != NULL) {
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Cancel the user's receive/transmit request.
+
+  @param[in]  IpInstance         The IP4 child.
+  @param[in]  Token              The token to cancel. If NULL, all token will be
+                                 cancelled.
+
+  @retval EFI_SUCCESS            The token is cancelled.
+  @retval EFI_NOT_FOUND          The token isn't found on either the
+                                 transmit/receive queue.
+  @retval EFI_DEVICE_ERROR       Not all token is cancelled when Token is NULL.
+
+**/
+EFI_STATUS
+Ip4Cancel (
+  IN IP4_PROTOCOL             *IpInstance,
+  IN EFI_IP4_COMPLETION_TOKEN *Token          OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+
+  //
+  // First check the transmitted packet. Ip4CancelTxTokens returns
+  // EFI_ABORTED to mean that the token has been cancelled when
+  // token != NULL. So, return EFI_SUCCESS for this condition.
+  //
+  Status = NetMapIterate (&IpInstance->TxTokens, Ip4CancelTxTokens, Token);
+
+  if (EFI_ERROR (Status)) {
+    if ((Token != NULL) && (Status == EFI_ABORTED)) {
+      return EFI_SUCCESS;
+    }
+
+    return Status;
+  }
+
+  //
+  // Check the receive queue. Ip4CancelRxTokens also returns EFI_ABORT
+  // for Token!=NULL and it is cancelled.
+  //
+  Status = NetMapIterate (&IpInstance->RxTokens, Ip4CancelRxTokens, Token);
+  //
+  // Dispatch the DPCs queued by the NotifyFunction of the canceled rx token's
+  // events.
+  //
+  DispatchDpc ();
+  if (EFI_ERROR (Status)) {
+    if ((Token != NULL) && (Status == EFI_ABORTED)) {
+      return EFI_SUCCESS;
+    }
+
+    return Status;
+  }
+
+  //
+  // OK, if the Token is found when Token != NULL, the NetMapIterate
+  // will return EFI_ABORTED, which has been interrupted as EFI_SUCCESS.
+  //
+  if (Token != NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // If Token == NULL, cancel all the tokens. return error if no
+  // all of them are cancelled.
+  //
+  if (!NetMapIsEmpty (&IpInstance->TxTokens) ||
+      !NetMapIsEmpty (&IpInstance->RxTokens)) {
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Abort an asynchronous transmit or receive request.
+
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token->Status will be set to EFI_ABORTED and then Token->Event will
+  be signaled. If the token is not in one of the queues, which usually means the
+  asynchronous operation has completed, this function will not signal the token
+  and EFI_NOT_FOUND is returned.
+
+  @param[in]  This  Pointer to the EFI_IP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that has been issued by
+                    EFI_IP4_PROTOCOL.Transmit() or
+                    EFI_IP4_PROTOCOL.Receive(). If NULL, all pending
+                    tokens are aborted. Type EFI_IP4_COMPLETION_TOKEN is
+                    defined in EFI_IP4_PROTOCOL.Transmit().
+
+  @retval EFI_SUCCESS           The asynchronous I/O request was aborted and
+                                Token.->Event was signaled. When Token is NULL, all
+                                pending requests were aborted and their events were signaled.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_NOT_STARTED       This instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_NOT_FOUND         When Token is not NULL, the asynchronous I/O request was
+                                not found in the transmit or receive queue. It has either completed
+                                or was not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Cancel (
+  IN EFI_IP4_PROTOCOL         *This,
+  IN EFI_IP4_COMPLETION_TOKEN *Token    OPTIONAL
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (IpInstance->ConfigData.UseDefaultAddress && IP4_NO_MAPPING (IpInstance)) {
+    Status = EFI_NO_MAPPING;
+    goto ON_EXIT;
+  }
+
+  Status = Ip4Cancel (IpInstance, Token);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+
+  The Poll() function polls for incoming data packets and processes outgoing data
+  packets. Network drivers and applications can call the EFI_IP4_PROTOCOL.Poll()
+  function to increase the rate that data packets are moved between the communications
+  device and the transmit and receive queues.
+
+  In some systems the periodic timer event may not poll the underlying communications
+  device fast enough to transmit and/or receive all data packets without missing
+  incoming packets or dropping outgoing packets. Drivers and applications that are
+  experiencing packet loss should try calling the EFI_IP4_PROTOCOL.Poll() function
+  more often.
+
+  @param[in]  This               Pointer to the EFI_IP4_PROTOCOL instance.
+
+  @retval  EFI_SUCCESS           Incoming or outgoing data was processed.
+  @retval  EFI_NOT_STARTED       This EFI IPv4 Protocol instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_INVALID_PARAMETER This is NULL.
+  @retval  EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval  EFI_NOT_READY         No incoming or outgoing data is processed.
+  @retval  EFI_TIMEOUT           Data was dropped out of the transmit and/or receive queue.
+                                 Consider increasing the polling rate.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiIp4Poll (
+  IN EFI_IP4_PROTOCOL       *This
+  )
+{
+  IP4_PROTOCOL                  *IpInstance;
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IpInstance = IP4_INSTANCE_FROM_PROTOCOL (This);
+
+  if (IpInstance->State == IP4_STATE_UNCONFIGED) {
+    return EFI_NOT_STARTED;
+  }
+
+  Mnp = IpInstance->Service->Mnp;
+
+  //
+  // Don't lock the Poll function to enable the deliver of
+  // the packet polled up.
+  //
+  return Mnp->Poll (Mnp);
+}
+
+/**
+  Decrease the life of the transmitted packets. If it is
+  decreased to zero, cancel the packet. This function is
+  called by Ip4PacketTimerTicking which time out both the
+  received-but-not-delivered and transmitted-but-not-recycle
+  packets.
+
+  @param[in]  Map                    The IP4 child's transmit map.
+  @param[in]  Item                   Current transmitted packet.
+  @param[in]  Context                Not used.
+
+  @retval EFI_SUCCESS            Always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4SentPacketTicking (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Context
+  )
+{
+  IP4_TXTOKEN_WRAP          *Wrap;
+
+  Wrap = (IP4_TXTOKEN_WRAP *) Item->Value;
+  ASSERT (Wrap != NULL);
+
+  if ((Wrap->Life > 0) && (--Wrap->Life == 0)) {
+    Ip4CancelPacket (Wrap->IpInstance->Interface, Wrap->Packet, EFI_ABORTED);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  There are two steps for this the heart beat timer of IP4 service instance. 
+  First, it times out all of its IP4 children's received-but-not-delivered 
+  and transmitted-but-not-recycle packets, and provides time input for its 
+  IGMP protocol.
+  Second, a dedicated timer is used to poll underlying media status. In case 
+  of cable swap, a new round auto configuration will be initiated. The timer 
+  will signal the IP4 to run DHCP configuration again. IP4 driver will free
+  old IP address related resource, such as route table and Interface, then
+  initiate a DHCP process to acquire new IP, eventually create route table 
+  for new IP address.
+
+  @param[in]  Event                  The IP4 service instance's heart beat timer.
+  @param[in]  Context                The IP4 service instance.
+
+**/
+VOID
+EFIAPI
+Ip4TimerTicking (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  IP4_SERVICE               *IpSb;
+  BOOLEAN                   OldMediaPresent;
+  EFI_STATUS                Status;
+  EFI_SIMPLE_NETWORK_MODE   SnpModeData;
+
+  IpSb = (IP4_SERVICE *) Context;
+  NET_CHECK_SIGNATURE (IpSb, IP4_SERVICE_SIGNATURE);
+  
+  OldMediaPresent = IpSb->MediaPresent;
+
+  Ip4PacketTimerTicking (IpSb);
+  Ip4IgmpTicking (IpSb);
+
+  //
+  // Get fresh mode data from MNP, since underlying media status may change. 
+  // Here, it needs to mention that the MediaPresent can also be checked even if 
+  // EFI_NOT_STARTED returned while this MNP child driver instance isn't configured.
+  //
+  Status = IpSb->Mnp->GetModeData (IpSb->Mnp, NULL, &SnpModeData);
+  if (EFI_ERROR (Status) && (Status != EFI_NOT_STARTED)) {
+    return;
+  }
+
+  IpSb->MediaPresent = SnpModeData.MediaPresent;
+  //
+  // Media transimit Unpresent to Present means new link movement is detected.
+  //
+  if (!OldMediaPresent && IpSb->MediaPresent) {
+    //
+    // Signal the IP4 to run the dhcp configuration again. IP4 driver will free
+    // old IP address related resource, such as route table and Interface, then 
+    // initiate a DHCP round to acquire new IP, eventually 
+    // create route table for new IP address.
+    //
+    if (IpSb->ReconfigEvent != NULL) {
+      Status = gBS->SignalEvent (IpSb->ReconfigEvent);
+      DispatchDpc ();
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.h
new file mode 100644
index 0000000000..e802b9516b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Impl.h
@@ -0,0 +1,403 @@
+/** @file
+  Ip4 internal functions and type defintions.
+  
+Copyright (c) 2005 - 2015, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<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.
+
+**/
+
+#ifndef __EFI_IP4_IMPL_H__
+#define __EFI_IP4_IMPL_H__
+
+#include <Uefi.h>
+
+#include <Protocol/IpSec.h>
+#include <Protocol/Ip4.h>
+#include <Protocol/Ip4Config2.h>
+#include <Protocol/Arp.h>
+#include <Protocol/ManagedNetwork.h>
+#include <Protocol/Dhcp4.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/HiiConfigAccess.h>
+
+#include <Library/DebugLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/NetLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DpcLib.h>
+#include <Library/PrintLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/HiiLib.h>
+#include <Library/UefiHiiServicesLib.h>
+
+#include "Ip4Common.h"
+#include "Ip4Driver.h"
+#include "Ip4If.h"
+#include "Ip4Icmp.h"
+#include "Ip4Option.h"
+#include "Ip4Igmp.h"
+#include "Ip4Route.h"
+#include "Ip4Input.h"
+#include "Ip4Output.h"
+#include "Ip4Config2Impl.h"
+#include "Ip4Config2Nv.h"
+#include "Ip4NvData.h"
+
+#define IP4_PROTOCOL_SIGNATURE  SIGNATURE_32 ('I', 'P', '4', 'P')
+#define IP4_SERVICE_SIGNATURE   SIGNATURE_32 ('I', 'P', '4', 'S')
+
+//
+// The state of IP4 protocol. It starts from UNCONFIGED. if it is
+// successfully configured, it goes to CONFIGED. if configure NULL
+// is called, it becomes UNCONFIGED again. If (partly) destroyed, it
+// becomes DESTROY.
+//
+#define IP4_STATE_UNCONFIGED    0
+#define IP4_STATE_CONFIGED      1
+#define IP4_STATE_DESTROY       2
+
+//
+// The state of IP4 service. It starts from UNSTARTED. It transits
+// to STARTED if autoconfigure is started. If default address is
+// configured, it becomes CONFIGED. and if partly destroyed, it goes
+// to DESTROY.
+//
+#define IP4_SERVICE_UNSTARTED   0
+#define IP4_SERVICE_STARTED     1
+#define IP4_SERVICE_CONFIGED    2
+#define IP4_SERVICE_DESTROY     3
+
+
+///
+/// IP4_TXTOKEN_WRAP wraps the upper layer's transmit token.
+/// The user's data is kept in the Packet. When fragment is
+/// needed, each fragment of the Packet has a reference to the
+/// Packet, no data is actually copied. The Packet will be
+/// released when all the fragments of it have been recycled by
+/// MNP. Upon then, the IP4_TXTOKEN_WRAP will be released, and
+/// user's event signalled.
+///
+typedef struct {
+  IP4_PROTOCOL              *IpInstance;
+  EFI_IP4_COMPLETION_TOKEN  *Token;
+  EFI_EVENT                 IpSecRecycleSignal;
+  NET_BUF                   *Packet;
+  BOOLEAN                   Sent;
+  INTN                      Life;
+} IP4_TXTOKEN_WRAP;
+
+///
+/// IP4_IPSEC_WRAP wraps the packet received from MNP layer. The packet
+/// will be released after it has been processed by the receiver. Upon then,
+/// the IP4_IPSEC_WRAP will be released, and the IpSecRecycleSignal will be signaled
+/// to notice IPsec to free the resources.
+///
+typedef struct {
+  EFI_EVENT                 IpSecRecycleSignal;
+  NET_BUF                   *Packet;
+} IP4_IPSEC_WRAP;
+
+///
+/// IP4_RXDATA_WRAP wraps the data IP4 child delivers to the
+/// upper layers. The received packet is kept in the Packet.
+/// The Packet itself may be constructured from some fragments.
+/// All the fragments of the Packet is organized by a
+/// IP4_ASSEMBLE_ENTRY structure. If the Packet is recycled by
+/// the upper layer, the assemble entry and its associated
+/// fragments will be freed at last.
+///
+typedef struct {
+  LIST_ENTRY                Link;
+  IP4_PROTOCOL              *IpInstance;
+  NET_BUF                   *Packet;
+  EFI_IP4_RECEIVE_DATA      RxData;
+} IP4_RXDATA_WRAP;
+
+
+struct _IP4_PROTOCOL {
+  UINT32                    Signature;
+
+  EFI_IP4_PROTOCOL          Ip4Proto;
+  EFI_HANDLE                Handle;
+  INTN                      State;
+
+  IP4_SERVICE               *Service;
+  LIST_ENTRY                Link;       // Link to all the IP protocol from the service
+
+  //
+  // User's transmit/receive tokens, and received/deliverd packets
+  //
+  NET_MAP                   RxTokens;
+  NET_MAP                   TxTokens;   // map between (User's Token, IP4_TXTOKE_WRAP)
+  LIST_ENTRY                Received;   // Received but not delivered packet
+  LIST_ENTRY                Delivered;  // Delivered and to be recycled packets
+  EFI_LOCK                  RecycleLock;
+
+  //
+  // Instance's address and route tables. There are two route tables.
+  // RouteTable is used by the IP4 driver to route packet. EfiRouteTable
+  // is used to communicate the current route info to the upper layer.
+  //
+  IP4_INTERFACE             *Interface;
+  LIST_ENTRY                AddrLink;   // Ip instances with the same IP address.
+  IP4_ROUTE_TABLE           *RouteTable;
+
+  EFI_IP4_ROUTE_TABLE       *EfiRouteTable;
+  UINT32                    EfiRouteCount;
+
+  //
+  // IGMP data for this instance
+  //
+  IP4_ADDR                  *Groups;    // stored in network byte order
+  UINT32                    GroupCount;
+
+  EFI_IP4_CONFIG_DATA       ConfigData;
+
+};
+
+struct _IP4_SERVICE {
+  UINT32                          Signature;
+  EFI_SERVICE_BINDING_PROTOCOL    ServiceBinding;
+  INTN                            State;
+
+  //
+  // List of all the IP instances and interfaces, and default
+  // interface and route table and caches.
+  //
+  UINTN                           NumChildren;
+  LIST_ENTRY                      Children;
+
+  LIST_ENTRY                      Interfaces;
+
+  IP4_INTERFACE                   *DefaultInterface;
+  IP4_ROUTE_TABLE                 *DefaultRouteTable;
+
+  //
+  // Ip reassemble utilities, and IGMP data
+  //
+  IP4_ASSEMBLE_TABLE              Assemble;
+  IGMP_SERVICE_DATA               IgmpCtrl;
+
+  //
+  // Low level protocol used by this service instance
+  //
+  EFI_HANDLE                      Image;
+  EFI_HANDLE                      Controller;
+
+  EFI_HANDLE                      MnpChildHandle;
+  EFI_MANAGED_NETWORK_PROTOCOL    *Mnp;
+
+  EFI_MANAGED_NETWORK_CONFIG_DATA MnpConfigData;
+  EFI_SIMPLE_NETWORK_MODE         SnpMode;
+
+  EFI_EVENT                       Timer;
+
+  EFI_EVENT                       ReconfigEvent;
+
+  BOOLEAN                         Reconfig;
+
+  //
+  // Underlying media present status. 
+  //
+  BOOLEAN                         MediaPresent;
+
+  //
+  // IPv4 Configuration II Protocol instance
+  //
+  IP4_CONFIG2_INSTANCE            Ip4Config2Instance;
+
+  CHAR16                          *MacString;
+
+  UINT32                          MaxPacketSize;
+  UINT32                          OldMaxPacketSize; ///< The MTU before IPsec enable.
+};
+
+#define IP4_INSTANCE_FROM_PROTOCOL(Ip4) \
+          CR ((Ip4), IP4_PROTOCOL, Ip4Proto, IP4_PROTOCOL_SIGNATURE)
+
+#define IP4_SERVICE_FROM_PROTOCOL(Sb)   \
+          CR ((Sb), IP4_SERVICE, ServiceBinding, IP4_SERVICE_SIGNATURE)
+
+#define IP4_SERVICE_FROM_CONFIG2_INSTANCE(This) \
+  CR (This, IP4_SERVICE, Ip4Config2Instance, IP4_SERVICE_SIGNATURE)
+
+
+#define IP4_NO_MAPPING(IpInstance) (!(IpInstance)->Interface->Configured)
+
+extern EFI_IP4_PROTOCOL mEfiIp4ProtocolTemplete;
+
+/**
+  Config the MNP parameter used by IP. The IP driver use one MNP
+  child to transmit/receive frames. By default, it configures MNP
+  to receive unicast/multicast/broadcast. And it will enable/disable
+  the promiscous receive according to whether there is IP child
+  enable that or not. If Force is FALSE, it will iterate through
+  all the IP children to check whether the promiscuous receive
+  setting has been changed. If it hasn't been changed, it won't
+  reconfigure the MNP. If Force is TRUE, the MNP is configured no
+  matter whether that is changed or not.
+
+  @param[in]  IpSb               The IP4 service instance that is to be changed.
+  @param[in]  Force              Force the configuration or not.
+
+  @retval EFI_SUCCESS            The MNP is successfully configured/reconfigured.
+  @retval Others                 Configuration failed.
+
+**/
+EFI_STATUS
+Ip4ServiceConfigMnp (
+  IN IP4_SERVICE            *IpSb,
+  IN BOOLEAN                Force
+  );
+
+/**
+  Intiialize the IP4_PROTOCOL structure to the unconfigured states.
+
+  @param  IpSb                   The IP4 service instance.
+  @param  IpInstance             The IP4 child instance.
+
+**/
+VOID
+Ip4InitProtocol (
+  IN     IP4_SERVICE            *IpSb,
+  IN OUT IP4_PROTOCOL           *IpInstance
+  );
+
+/**
+  Clean up the IP4 child, release all the resources used by it.
+
+  @param[in]  IpInstance         The IP4 child to clean up.
+
+  @retval EFI_SUCCESS            The IP4 child is cleaned up.
+  @retval EFI_DEVICE_ERROR       Some resources failed to be released.
+
+**/
+EFI_STATUS
+Ip4CleanProtocol (
+  IN  IP4_PROTOCOL          *IpInstance
+  );
+
+/**
+  Cancel the user's receive/transmit request.
+
+  @param[in]  IpInstance         The IP4 child.
+  @param[in]  Token              The token to cancel. If NULL, all token will be
+                                 cancelled.
+
+  @retval EFI_SUCCESS            The token is cancelled.
+  @retval EFI_NOT_FOUND          The token isn't found on either the
+                                 transmit/receive queue.
+  @retval EFI_DEVICE_ERROR       Not all token is cancelled when Token is NULL.
+
+**/
+EFI_STATUS
+Ip4Cancel (
+  IN IP4_PROTOCOL             *IpInstance,
+  IN EFI_IP4_COMPLETION_TOKEN *Token          OPTIONAL
+  );
+
+/**
+  Change the IP4 child's multicast setting. The caller
+  should make sure that the parameters is valid.
+
+  @param[in]  IpInstance             The IP4 child to change the setting.
+  @param[in]  JoinFlag               TRUE to join the group, otherwise leave it
+  @param[in]  GroupAddress           The target group address
+
+  @retval EFI_ALREADY_STARTED    Want to join the group, but already a member of it
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate some resources.
+  @retval EFI_DEVICE_ERROR       Failed to set the group configuraton
+  @retval EFI_SUCCESS            Successfully updated the group setting.
+  @retval EFI_NOT_FOUND          Try to leave the group which it isn't a member.
+
+**/
+EFI_STATUS
+Ip4Groups (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN BOOLEAN                JoinFlag,
+  IN EFI_IPv4_ADDRESS       *GroupAddress       OPTIONAL
+  );
+
+/**
+  The heart beat timer of IP4 service instance. It times out
+  all of its IP4 children's received-but-not-delivered and
+  transmitted-but-not-recycle packets, and provides time input
+  for its IGMP protocol.
+
+  @param[in]  Event                  The IP4 service instance's heart beat timer.
+  @param[in]  Context                The IP4 service instance.
+
+**/
+VOID
+EFIAPI
+Ip4TimerTicking (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  );
+
+/**
+  Decrease the life of the transmitted packets. If it is
+  decreased to zero, cancel the packet. This function is
+  called by Ip4PacketTimerTicking which time out both the
+  received-but-not-delivered and transmitted-but-not-recycle
+  packets.
+
+  @param[in]  Map                    The IP4 child's transmit map.
+  @param[in]  Item                   Current transmitted packet.
+  @param[in]  Context                Not used.
+
+  @retval EFI_SUCCESS            Always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+Ip4SentPacketTicking (
+  IN NET_MAP                *Map,
+  IN NET_MAP_ITEM           *Item,
+  IN VOID                   *Context
+  );
+
+/**
+  The callback function for the net buffer which wraps the user's
+  transmit token. Although it seems this function is pretty simple,
+  there are some subtle things.
+  When user requests the IP to transmit a packet by passing it a
+  token, the token is wrapped in an IP4_TXTOKEN_WRAP and the data
+  is wrapped in an net buffer. the net buffer's Free function is
+  set to Ip4FreeTxToken. The Token and token wrap are added to the
+  IP child's TxToken map. Then the buffer is passed to Ip4Output for
+  transmission. If something error happened before that, the buffer
+  is freed, which in turn will free the token wrap. The wrap may
+  have been added to the TxToken map or not, and the user's event
+  shouldn't be fired because we are still in the EfiIp4Transmit. If
+  the buffer has been sent by Ip4Output, it should be removed from
+  the TxToken map and user's event signaled. The token wrap and buffer
+  are bound together. Check the comments in Ip4Output for information
+  about IP fragmentation.
+
+  @param[in]  Context                The token's wrap.
+
+**/
+VOID
+EFIAPI
+Ip4FreeTxToken (
+  IN VOID                   *Context
+  );
+
+extern EFI_IPSEC2_PROTOCOL   *mIpSec;
+extern BOOLEAN               mIpSec2Installed;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.c
new file mode 100644
index 0000000000..09b8f2bac2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.c
@@ -0,0 +1,1609 @@
+/** @file
+  IP4 input process.
+  
+Copyright (c) 2005 - 2014, Intel Corporation. All rights reserved.<BR>
+(C) Copyright 2015 Hewlett-Packard Development Company, L.P.<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 "Ip4Impl.h"
+
+
+/**
+  Create an empty assemble entry for the packet identified by
+  (Dst, Src, Id, Protocol). The default life for the packet is
+  120 seconds.
+
+  @param[in]  Dst                    The destination address
+  @param[in]  Src                    The source address
+  @param[in]  Id                     The ID field in IP header
+  @param[in]  Protocol               The protocol field in IP header
+
+  @return NULL if failed to allocate memory for the entry, otherwise
+          the point to just created reassemble entry.
+
+**/
+IP4_ASSEMBLE_ENTRY *
+Ip4CreateAssembleEntry (
+  IN IP4_ADDR               Dst,
+  IN IP4_ADDR               Src,
+  IN UINT16                 Id,
+  IN UINT8                  Protocol
+  )
+{
+
+  IP4_ASSEMBLE_ENTRY        *Assemble;
+
+  Assemble = AllocatePool (sizeof (IP4_ASSEMBLE_ENTRY));
+
+  if (Assemble == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&Assemble->Link);
+  InitializeListHead (&Assemble->Fragments);
+
+  Assemble->Dst      = Dst;
+  Assemble->Src      = Src;
+  Assemble->Id       = Id;
+  Assemble->Protocol = Protocol;
+  Assemble->TotalLen = 0;
+  Assemble->CurLen   = 0;
+  Assemble->Head     = NULL;
+  Assemble->Info     = NULL;
+  Assemble->Life     = IP4_FRAGMENT_LIFE;
+
+  return Assemble;
+}
+
+
+/**
+  Release all the fragments of a packet, then free the assemble entry.
+
+  @param[in]  Assemble               The assemble entry to free
+
+**/
+VOID
+Ip4FreeAssembleEntry (
+  IN IP4_ASSEMBLE_ENTRY     *Assemble
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  NET_BUF                   *Fragment;
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Assemble->Fragments) {
+    Fragment = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    RemoveEntryList (Entry);
+    NetbufFree (Fragment);
+  }
+
+  FreePool (Assemble);
+}
+
+
+/**
+  Initialize an already allocated assemble table. This is generally
+  the assemble table embedded in the IP4 service instance.
+
+  @param[in, out]  Table                  The assemble table to initialize.
+
+**/
+VOID
+Ip4InitAssembleTable (
+  IN OUT IP4_ASSEMBLE_TABLE     *Table
+  )
+{
+  UINT32                    Index;
+
+  for (Index = 0; Index < IP4_ASSEMLE_HASH_SIZE; Index++) {
+    InitializeListHead (&Table->Bucket[Index]);
+  }
+}
+
+
+/**
+  Clean up the assemble table: remove all the fragments
+  and assemble entries.
+
+  @param[in]  Table                  The assemble table to clean up
+
+**/
+VOID
+Ip4CleanAssembleTable (
+  IN IP4_ASSEMBLE_TABLE     *Table
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ASSEMBLE_ENTRY        *Assemble;
+  UINT32                    Index;
+
+  for (Index = 0; Index < IP4_ASSEMLE_HASH_SIZE; Index++) {
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &Table->Bucket[Index]) {
+      Assemble = NET_LIST_USER_STRUCT (Entry, IP4_ASSEMBLE_ENTRY, Link);
+
+      RemoveEntryList (Entry);
+      Ip4FreeAssembleEntry (Assemble);
+    }
+  }
+}
+
+
+/**
+  Trim the packet to fit in [Start, End), and update the per
+  packet information.
+
+  @param  Packet                 Packet to trim
+  @param  Start                  The sequence of the first byte to fit in
+  @param  End                    One beyond the sequence of last byte to fit in.
+
+**/
+VOID
+Ip4TrimPacket (
+  IN OUT NET_BUF                *Packet,
+  IN     INTN                   Start,
+  IN     INTN                   End
+  )
+{
+  IP4_CLIP_INFO             *Info;
+  INTN                      Len;
+
+  Info = IP4_GET_CLIP_INFO (Packet);
+
+  ASSERT (Info->Start + Info->Length == Info->End);
+  ASSERT ((Info->Start < End) && (Start < Info->End));
+
+   if (Info->Start < Start) {
+    Len = Start - Info->Start;
+
+    NetbufTrim (Packet, (UINT32) Len, NET_BUF_HEAD);
+    Info->Start   = Start;
+    Info->Length -= Len;
+  }
+
+  if (End < Info->End) {
+    Len = End - Info->End;
+
+    NetbufTrim (Packet, (UINT32) Len, NET_BUF_TAIL);
+    Info->End     = End;
+    Info->Length -= Len;
+  }
+}
+
+
+/**
+  Release all the fragments of the packet. This is the callback for
+  the assembled packet's OnFree. It will free the assemble entry,
+  which in turn will free all the fragments of the packet.
+
+  @param[in]  Arg                    The assemble entry to free
+
+**/
+VOID
+EFIAPI
+Ip4OnFreeFragments (
+  IN VOID                   *Arg
+  )
+{
+  Ip4FreeAssembleEntry ((IP4_ASSEMBLE_ENTRY *) Arg);
+}
+
+
+/**
+  Reassemble the IP fragments. If all the fragments of the packet
+  have been received, it will wrap the packet in a net buffer then
+  return it to caller. If the packet can't be assembled, NULL is
+  return.
+
+  @param  Table     The assemble table used. New assemble entry will be created
+                    if the Packet is from a new chain of fragments.
+  @param  Packet    The fragment to assemble. It might be freed if the fragment
+                    can't be re-assembled.
+
+  @return NULL if the packet can't be reassemble. The point to just assembled
+          packet if all the fragments of the packet have arrived.
+
+**/
+NET_BUF *
+Ip4Reassemble (
+  IN OUT IP4_ASSEMBLE_TABLE     *Table,
+  IN OUT NET_BUF                *Packet
+  )
+{
+  IP4_HEAD                  *IpHead;
+  IP4_CLIP_INFO             *This;
+  IP4_CLIP_INFO             *Node;
+  IP4_ASSEMBLE_ENTRY        *Assemble;
+  LIST_ENTRY                *Head;
+  LIST_ENTRY                *Prev;
+  LIST_ENTRY                *Cur;
+  NET_BUF                   *Fragment;
+  NET_BUF                   *NewPacket;
+  INTN                      Index;
+
+  IpHead  = Packet->Ip.Ip4;
+  This    = IP4_GET_CLIP_INFO (Packet);
+
+  ASSERT (IpHead != NULL);
+
+  //
+  // First: find the related assemble entry
+  //
+  Assemble  = NULL;
+  Index     = IP4_ASSEMBLE_HASH (IpHead->Dst, IpHead->Src, IpHead->Id, IpHead->Protocol);
+
+  NET_LIST_FOR_EACH (Cur, &Table->Bucket[Index]) {
+    Assemble = NET_LIST_USER_STRUCT (Cur, IP4_ASSEMBLE_ENTRY, Link);
+
+    if ((Assemble->Dst == IpHead->Dst) && (Assemble->Src == IpHead->Src) &&
+        (Assemble->Id == IpHead->Id)   && (Assemble->Protocol == IpHead->Protocol)) {
+      break;
+    }
+  }
+
+  //
+  // Create a new assemble entry if no assemble entry is related to this packet
+  //
+  if (Cur == &Table->Bucket[Index]) {
+    Assemble = Ip4CreateAssembleEntry (
+                 IpHead->Dst,
+                 IpHead->Src,
+                 IpHead->Id,
+                 IpHead->Protocol
+                 );
+
+    if (Assemble == NULL) {
+      goto DROP;
+    }
+
+    InsertHeadList (&Table->Bucket[Index], &Assemble->Link);
+  }
+  //
+  // Assemble shouldn't be NULL here
+  //
+  ASSERT (Assemble != NULL);
+
+  //
+  // Find the point to insert the packet: before the first
+  // fragment with THIS.Start < CUR.Start. the previous one
+  // has PREV.Start <= THIS.Start < CUR.Start.
+  //
+  Head = &Assemble->Fragments;
+
+  NET_LIST_FOR_EACH (Cur, Head) {
+    Fragment = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+
+    if (This->Start < IP4_GET_CLIP_INFO (Fragment)->Start) {
+      break;
+    }
+  }
+
+  //
+  // Check whether the current fragment overlaps with the previous one.
+  // It holds that: PREV.Start <= THIS.Start < THIS.End. Only need to
+  // check whether THIS.Start < PREV.End for overlap. If two fragments
+  // overlaps, trim the overlapped part off THIS fragment.
+  //
+  if ((Prev = Cur->BackLink) != Head) {
+    Fragment  = NET_LIST_USER_STRUCT (Prev, NET_BUF, List);
+    Node      = IP4_GET_CLIP_INFO (Fragment);
+
+    if (This->Start < Node->End) {
+      if (This->End <= Node->End) {
+        NetbufFree (Packet);
+        return NULL;
+      }
+
+      Ip4TrimPacket (Packet, Node->End, This->End);
+    }
+  }
+
+  //
+  // Insert the fragment into the packet. The fragment may be removed
+  // from the list by the following checks.
+  //
+  NetListInsertBefore (Cur, &Packet->List);
+
+  //
+  // Check the packets after the insert point. It holds that:
+  // THIS.Start <= NODE.Start < NODE.End. The equality holds
+  // if PREV and NEXT are continuous. THIS fragment may fill
+  // several holes. Remove the completely overlapped fragments
+  //
+  while (Cur != Head) {
+    Fragment = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+    Node     = IP4_GET_CLIP_INFO (Fragment);
+
+    //
+    // Remove fragments completely overlapped by this fragment
+    //
+    if (Node->End <= This->End) {
+      Cur = Cur->ForwardLink;
+
+      RemoveEntryList (&Fragment->List);
+      Assemble->CurLen -= Node->Length;
+
+      NetbufFree (Fragment);
+      continue;
+    }
+
+    //
+    // The conditions are: THIS.Start <= NODE.Start, and THIS.End <
+    // NODE.End. Two fragments overlaps if NODE.Start < THIS.End.
+    // If two fragments start at the same offset, remove THIS fragment
+    // because ((THIS.Start == NODE.Start) && (THIS.End < NODE.End)).
+    //
+    if (Node->Start < This->End) {
+      if (This->Start == Node->Start) {
+        RemoveEntryList (&Packet->List);
+        goto DROP;
+      }
+
+      Ip4TrimPacket (Packet, This->Start, Node->Start);
+    }
+
+    break;
+  }
+
+  //
+  // Update the assemble info: increase the current length. If it is
+  // the frist fragment, update the packet's IP head and per packet
+  // info. If it is the last fragment, update the total length.
+  //
+  Assemble->CurLen += This->Length;
+
+  if (This->Start == 0) {
+    //
+    // Once the first fragment is enqueued, it can't be removed
+    // from the fragment list. So, Assemble->Head always point
+    // to valid memory area.
+    //
+    ASSERT (Assemble->Head == NULL);
+
+    Assemble->Head  = IpHead;
+    Assemble->Info  = IP4_GET_CLIP_INFO (Packet);
+  }
+
+  //
+  // Don't update the length more than once.
+  //
+  if (IP4_LAST_FRAGMENT (IpHead->Fragment) && (Assemble->TotalLen == 0)) {
+    Assemble->TotalLen = This->End;
+  }
+
+  //
+  // Deliver the whole packet if all the fragments received.
+  // All fragments received if:
+  //  1. received the last one, so, the total length is know
+  //  2. received all the data. If the last fragment on the
+  //     queue ends at the total length, all data is received.
+  //
+  if ((Assemble->TotalLen != 0) && (Assemble->CurLen >= Assemble->TotalLen)) {
+
+    RemoveEntryList (&Assemble->Link);
+
+    //
+    // If the packet is properly formated, the last fragment's End
+    // equals to the packet's total length. Otherwise, the packet
+    // is a fake, drop it now.
+    //
+    Fragment = NET_LIST_USER_STRUCT (Head->BackLink, NET_BUF, List);
+
+    if (IP4_GET_CLIP_INFO (Fragment)->End != Assemble->TotalLen) {
+      Ip4FreeAssembleEntry (Assemble);
+      return NULL;
+    }
+
+    //
+    // Wrap the packet in a net buffer then deliver it up
+    //
+    NewPacket = NetbufFromBufList (
+                  &Assemble->Fragments,
+                  0,
+                  0,
+                  Ip4OnFreeFragments,
+                  Assemble
+                  );
+
+    if (NewPacket == NULL) {
+      Ip4FreeAssembleEntry (Assemble);
+      return NULL;
+    }
+
+    NewPacket->Ip.Ip4 = Assemble->Head;
+
+    ASSERT (Assemble->Info != NULL);
+
+    CopyMem (
+      IP4_GET_CLIP_INFO (NewPacket),
+      Assemble->Info,
+      sizeof (*IP4_GET_CLIP_INFO (NewPacket))
+      );
+
+    return NewPacket;
+  }
+
+  return NULL;
+
+DROP:
+  NetbufFree (Packet);
+  return NULL;
+}
+
+/**
+  The callback function for the net buffer which wraps the packet processed by 
+  IPsec. It releases the wrap packet and also signals IPsec to free the resources. 
+
+  @param[in]  Arg       The wrap context
+
+**/
+VOID
+EFIAPI
+Ip4IpSecFree (
+  IN VOID                   *Arg
+  )
+{
+  IP4_IPSEC_WRAP            *Wrap;
+
+  Wrap = (IP4_IPSEC_WRAP *) Arg;
+
+  if (Wrap->IpSecRecycleSignal != NULL) {
+    gBS->SignalEvent (Wrap->IpSecRecycleSignal);
+  }
+
+  NetbufFree (Wrap->Packet);
+
+  FreePool (Wrap);
+
+  return;
+}
+
+/**
+  The work function to locate IPsec protocol to process the inbound or 
+  outbound IP packets. The process routine handls the packet with following
+  actions: bypass the packet, discard the packet, or protect the packet.       
+
+  @param[in]       IpSb          The IP4 service instance.
+  @param[in, out]  Head          The The caller supplied IP4 header.
+  @param[in, out]  Netbuf        The IP4 packet to be processed by IPsec.
+  @param[in, out]  Options       The caller supplied options.
+  @param[in, out]  OptionsLen    The length of the option.
+  @param[in]       Direction     The directionality in an SPD entry, 
+                                 EfiIPsecInBound or EfiIPsecOutBound.
+  @param[in]       Context       The token's wrap.
+
+  @retval EFI_SUCCESS            The IPsec protocol is not available or disabled.
+  @retval EFI_SUCCESS            The packet was bypassed and all buffers remain the same.
+  @retval EFI_SUCCESS            The packet was protected.
+  @retval EFI_ACCESS_DENIED      The packet was discarded.  
+  @retval EFI_OUT_OF_RESOURCES   There is no suffcient resource to complete the operation.
+  @retval EFI_BUFFER_TOO_SMALL   The number of non-empty block is bigger than the 
+                                 number of input data blocks when build a fragment table.
+
+**/
+EFI_STATUS
+Ip4IpSecProcessPacket (
+  IN     IP4_SERVICE            *IpSb,
+  IN OUT IP4_HEAD               **Head,
+  IN OUT NET_BUF                **Netbuf,
+  IN OUT UINT8                  **Options,
+  IN OUT UINT32                 *OptionsLen,
+  IN     EFI_IPSEC_TRAFFIC_DIR  Direction,
+  IN     VOID                   *Context
+  )
+{
+  NET_FRAGMENT              *FragmentTable;
+  NET_FRAGMENT              *OriginalFragmentTable;
+  UINT32                    FragmentCount;
+  UINT32                    OriginalFragmentCount;
+  EFI_EVENT                 RecycleEvent;
+  NET_BUF                   *Packet;
+  IP4_TXTOKEN_WRAP          *TxWrap;
+  IP4_IPSEC_WRAP            *IpSecWrap;
+  EFI_STATUS                Status;
+  IP4_HEAD                  ZeroHead;
+
+  Status        = EFI_SUCCESS;
+
+  if (!mIpSec2Installed) {
+    goto ON_EXIT;
+  }
+  
+  Packet        = *Netbuf;
+  RecycleEvent  = NULL;
+  IpSecWrap     = NULL;
+  FragmentTable = NULL;
+  TxWrap        = (IP4_TXTOKEN_WRAP *) Context; 
+  FragmentCount = Packet->BlockOpNum;
+
+  ZeroMem (&ZeroHead, sizeof (IP4_HEAD));
+  
+  if (mIpSec == NULL) {
+    gBS->LocateProtocol (&gEfiIpSec2ProtocolGuid, NULL, (VOID **) &mIpSec);
+    if (mIpSec == NULL) {
+      goto ON_EXIT;
+    }
+  }
+
+  //
+  // Check whether the IPsec enable variable is set.
+  //
+  if (mIpSec->DisabledFlag) {
+    //
+    // If IPsec is disabled, restore the original MTU
+    //   
+    IpSb->MaxPacketSize = IpSb->OldMaxPacketSize;
+    goto ON_EXIT;
+  } else {
+    //
+    // If IPsec is enabled, use the MTU which reduce the IPsec header length.  
+    //
+    IpSb->MaxPacketSize = IpSb->OldMaxPacketSize - IP4_MAX_IPSEC_HEADLEN;   
+  }
+
+  //
+  // Rebuild fragment table from netbuf to ease IPsec process.
+  //
+  FragmentTable = AllocateZeroPool (FragmentCount * sizeof (NET_FRAGMENT));
+
+  if (FragmentTable == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+ 
+  Status = NetbufBuildExt (Packet, FragmentTable, &FragmentCount);
+  
+  //
+  // Record the original FragmentTable and count.
+  //
+  OriginalFragmentTable = FragmentTable;
+  OriginalFragmentCount = FragmentCount;
+
+  if (EFI_ERROR (Status)) {
+    FreePool (FragmentTable);
+    goto ON_EXIT;
+  }
+
+  //
+  // Convert host byte order to network byte order
+  //
+  Ip4NtohHead (*Head);
+  
+  Status = mIpSec->ProcessExt (
+                     mIpSec,
+                     IpSb->Controller,
+                     IP_VERSION_4,
+                     (VOID *) (*Head),
+                     &(*Head)->Protocol,
+                     (VOID **) Options,
+                     OptionsLen,
+                     (EFI_IPSEC_FRAGMENT_DATA **) (&FragmentTable),
+                     &FragmentCount,
+                     Direction,
+                     &RecycleEvent
+                     );
+  //
+  // Convert back to host byte order
+  //
+  Ip4NtohHead (*Head);
+  
+  if (EFI_ERROR (Status)) {
+    FreePool (OriginalFragmentTable);
+    goto ON_EXIT;
+  }
+
+  if (OriginalFragmentTable == FragmentTable && OriginalFragmentCount == FragmentCount) {
+    //
+    // For ByPass Packet
+    //
+    FreePool (FragmentTable);
+    goto ON_EXIT;
+  } else {
+    //
+    // Free the FragmentTable which allocated before calling the IPsec.
+    //
+    FreePool (OriginalFragmentTable);
+  }
+
+  if (Direction == EfiIPsecOutBound && TxWrap != NULL) {
+  
+    TxWrap->IpSecRecycleSignal = RecycleEvent;
+    TxWrap->Packet             = NetbufFromExt (
+                                   FragmentTable,
+                                   FragmentCount,
+                                   IP4_MAX_HEADLEN,
+                                   0,
+                                   Ip4FreeTxToken,
+                                   TxWrap
+                                   );
+    if (TxWrap->Packet == NULL) {
+      //
+      // Recover the TxWrap->Packet, if meet a error, and the caller will free
+      // the TxWrap.
+      //
+      TxWrap->Packet = *Netbuf;
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    //
+    // Free orginal Netbuf.
+    //
+    NetIpSecNetbufFree (*Netbuf);
+    *Netbuf = TxWrap->Packet;
+    
+  } else {
+  
+    IpSecWrap = AllocateZeroPool (sizeof (IP4_IPSEC_WRAP));
+  
+    if (IpSecWrap == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      gBS->SignalEvent (RecycleEvent);
+      goto ON_EXIT;
+    }
+    
+    IpSecWrap->IpSecRecycleSignal = RecycleEvent;
+    IpSecWrap->Packet             = Packet;
+    Packet                        = NetbufFromExt (
+                                      FragmentTable, 
+                                      FragmentCount, 
+                                      IP4_MAX_HEADLEN, 
+                                      0, 
+                                      Ip4IpSecFree, 
+                                      IpSecWrap
+                                      );
+  
+    if (Packet == NULL) {
+      Packet = IpSecWrap->Packet;
+      gBS->SignalEvent (RecycleEvent);
+      FreePool (IpSecWrap);
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    if (Direction == EfiIPsecInBound && 0 != CompareMem (*Head, &ZeroHead, sizeof (IP4_HEAD))) {
+      Ip4PrependHead (Packet, *Head, *Options, *OptionsLen);
+      Ip4NtohHead (Packet->Ip.Ip4);
+      NetbufTrim (Packet, ((*Head)->HeadLen << 2), TRUE);
+
+      CopyMem (
+        IP4_GET_CLIP_INFO (Packet),
+        IP4_GET_CLIP_INFO (IpSecWrap->Packet),
+        sizeof (IP4_CLIP_INFO)
+        );
+    }
+    *Netbuf = Packet;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Pre-process the IPv4 packet. First validates the IPv4 packet, and
+  then reassembles packet if it is necessary.
+  
+  @param[in]       IpSb            Pointer to IP4_SERVICE.
+  @param[in, out]  Packet          Pointer to the Packet to be processed.
+  @param[in]       Head            Pointer to the IP4_HEAD.
+  @param[in]       Option          Pointer to a buffer which contains the IPv4 option.
+  @param[in]       OptionLen       The length of Option in bytes.
+  @param[in]       Flag            The link layer flag for the packet received, such
+                                   as multicast.
+
+  @retval     EFI_SEUCCESS               The recieved packet is in well form.
+  @retval     EFI_INVAILD_PARAMETER      The recieved packet is malformed.  
+
+**/
+EFI_STATUS
+Ip4PreProcessPacket (
+  IN     IP4_SERVICE    *IpSb,
+  IN OUT NET_BUF        **Packet,
+  IN     IP4_HEAD       *Head,
+  IN     UINT8          *Option,
+  IN     UINT32         OptionLen, 
+  IN     UINT32         Flag
+  ) 
+{
+  IP4_CLIP_INFO             *Info;
+  UINT32                    HeadLen;
+  UINT32                    TotalLen;
+  UINT16                    Checksum;
+
+  //
+  // Check if the IP4 header is correctly formatted.
+  //
+  if ((*Packet)->TotalSize < IP4_MIN_HEADLEN) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  HeadLen  = (Head->HeadLen << 2);
+  TotalLen = NTOHS (Head->TotalLen);
+
+  //
+  // Mnp may deliver frame trailer sequence up, trim it off.
+  //
+  if (TotalLen < (*Packet)->TotalSize) {
+    NetbufTrim (*Packet, (*Packet)->TotalSize - TotalLen, FALSE);
+  }
+
+  if ((Head->Ver != 4) || (HeadLen < IP4_MIN_HEADLEN) ||
+      (TotalLen < HeadLen) || (TotalLen != (*Packet)->TotalSize)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Some OS may send IP packets without checksum.
+  //
+  Checksum = (UINT16) (~NetblockChecksum ((UINT8 *) Head, HeadLen));
+
+  if ((Head->Checksum != 0) && (Checksum != 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Convert the IP header to host byte order, then get the per packet info.
+  //
+  (*Packet)->Ip.Ip4  = Ip4NtohHead (Head);
+
+  Info            = IP4_GET_CLIP_INFO (*Packet);
+  Info->LinkFlag  = Flag;
+  Info->CastType  = Ip4GetHostCast (IpSb, Head->Dst, Head->Src);
+  Info->Start     = (Head->Fragment & IP4_HEAD_OFFSET_MASK) << 3;
+  Info->Length    = Head->TotalLen - HeadLen;
+  Info->End       = Info->Start + Info->Length;
+  Info->Status    = EFI_SUCCESS;
+
+  //
+  // The packet is destinated to us if the CastType is non-zero.
+  //
+  if ((Info->CastType == 0) || (Info->End > IP4_MAX_PACKET_SIZE)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Validate the options. Don't call the Ip4OptionIsValid if
+  // there is no option to save some CPU process.
+  //
+  
+  if ((OptionLen > 0) && !Ip4OptionIsValid (Option, OptionLen, TRUE)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Trim the head off, after this point, the packet is headless,
+  // and Packet->TotalLen == Info->Length.
+  //
+  NetbufTrim (*Packet, HeadLen, TRUE);
+
+  //
+  // Reassemble the packet if this is a fragment. The packet is a
+  // fragment if its head has MF (more fragment) set, or it starts
+  // at non-zero byte.
+  //
+  if (((Head->Fragment & IP4_HEAD_MF_MASK) != 0) || (Info->Start != 0)) {
+    //
+    // Drop the fragment if DF is set but it is fragmented. Gateway
+    // need to send a type 4 destination unreache ICMP message here.
+    //
+    if ((Head->Fragment & IP4_HEAD_DF_MASK) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // The length of all but the last fragments is in the unit of 8 bytes.
+    //
+    if (((Head->Fragment & IP4_HEAD_MF_MASK) != 0) && (Info->Length % 8 != 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    *Packet = Ip4Reassemble (&IpSb->Assemble, *Packet);
+
+    //
+    // Packet assembly isn't complete, start receive more packet.
+    //
+    if (*Packet == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  The IP4 input routine. It is called by the IP4_INTERFACE when a
+  IP4 fragment is received from MNP.
+
+  @param[in]  Ip4Instance        The IP4 child that request the receive, most like
+                                 it is NULL.
+  @param[in]  Packet             The IP4 packet received.
+  @param[in]  IoStatus           The return status of receive request.
+  @param[in]  Flag               The link layer flag for the packet received, such
+                                 as multicast.
+  @param[in]  Context            The IP4 service instance that own the MNP.
+
+**/
+VOID
+Ip4AccpetFrame (
+  IN IP4_PROTOCOL           *Ip4Instance,
+  IN NET_BUF                *Packet,
+  IN EFI_STATUS             IoStatus,
+  IN UINT32                 Flag,
+  IN VOID                   *Context
+  )
+{
+  IP4_SERVICE               *IpSb;
+  IP4_HEAD                  *Head;
+  EFI_STATUS                Status;
+  IP4_HEAD                  ZeroHead;
+  UINT8                     *Option;
+  UINT32                    OptionLen;
+  
+  IpSb   = (IP4_SERVICE *) Context;
+  Option = NULL;
+
+  if (EFI_ERROR (IoStatus) || (IpSb->State == IP4_SERVICE_DESTROY)) {
+    goto DROP;
+  }
+
+  Head      = (IP4_HEAD *) NetbufGetByte (Packet, 0, NULL);
+  ASSERT (Head != NULL);
+  OptionLen = (Head->HeadLen << 2) - IP4_MIN_HEADLEN;
+  if (OptionLen > 0) {
+    Option = (UINT8 *) (Head + 1);
+  }
+
+  //
+  // Validate packet format and reassemble packet if it is necessary.
+  //
+  Status = Ip4PreProcessPacket (
+             IpSb, 
+             &Packet, 
+             Head, 
+             Option,
+             OptionLen,  
+             Flag
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto RESTART;
+  }
+
+  //
+  // After trim off, the packet is a esp/ah/udp/tcp/icmp6 net buffer,
+  // and no need consider any other ahead ext headers.
+  //
+  Status = Ip4IpSecProcessPacket (
+             IpSb,
+             &Head,
+             &Packet,
+             &Option,
+             &OptionLen,
+             EfiIPsecInBound,
+             NULL
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto RESTART;
+  }
+  
+  //
+  // If the packet is protected by tunnel mode, parse the inner Ip Packet.
+  //
+  ZeroMem (&ZeroHead, sizeof (IP4_HEAD));
+  if (0 == CompareMem (Head, &ZeroHead, sizeof (IP4_HEAD))) {
+  // Packet may have been changed. Head, HeadLen, TotalLen, and
+  // info must be reloaded bofore use. The ownership of the packet
+  // is transfered to the packet process logic.
+  //
+    Head = (IP4_HEAD *) NetbufGetByte (Packet, 0, NULL);
+    ASSERT (Head != NULL);
+    Status = Ip4PreProcessPacket (
+               IpSb,
+               &Packet,
+               Head,
+               Option,
+               OptionLen,
+               Flag
+               );
+    if (EFI_ERROR (Status)) {
+      goto RESTART;
+    }
+  }
+  
+  ASSERT (Packet != NULL);
+  Head  = Packet->Ip.Ip4;
+  IP4_GET_CLIP_INFO (Packet)->Status = EFI_SUCCESS;
+
+  switch (Head->Protocol) {
+  case EFI_IP_PROTO_ICMP:
+    Ip4IcmpHandle (IpSb, Head, Packet);
+    break;
+
+  case IP4_PROTO_IGMP:
+    Ip4IgmpHandle (IpSb, Head, Packet);
+    break;
+
+  default:
+    Ip4Demultiplex (IpSb, Head, Packet, Option, OptionLen);
+  }
+
+  Packet = NULL;
+
+  //
+  // Dispatch the DPCs queued by the NotifyFunction of the rx token's events
+  // which are signaled with received data.
+  //
+  DispatchDpc ();
+
+RESTART:
+  Ip4ReceiveFrame (IpSb->DefaultInterface, NULL, Ip4AccpetFrame, IpSb);
+
+DROP:
+  if (Packet != NULL) {
+    NetbufFree (Packet);
+  }
+
+  return ;
+}
+
+
+/**
+  Check whether this IP child accepts the packet.
+
+  @param[in]  IpInstance             The IP child to check
+  @param[in]  Head                   The IP header of the packet
+  @param[in]  Packet                 The data of the packet
+
+  @retval TRUE   If the child wants to receive the packet.
+  @retval FALSE  Otherwise.
+
+**/
+BOOLEAN
+Ip4InstanceFrameAcceptable (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_ICMP_ERROR_HEAD       Icmp;
+  EFI_IP4_CONFIG_DATA       *Config;
+  IP4_CLIP_INFO             *Info;
+  UINT16                    Proto;
+  UINT32                    Index;
+
+  Config = &IpInstance->ConfigData;
+
+  //
+  // Dirty trick for the Tiano UEFI network stack implmentation. If
+  // ReceiveTimeout == -1, the receive of the packet for this instance
+  // is disabled. The UEFI spec don't have such capability. We add
+  // this to improve the performance because IP will make a copy of
+  // the received packet for each accepting instance. Some IP instances
+  // used by UDP/TCP only send packets, they don't wants to receive.
+  //
+  if (Config->ReceiveTimeout == (UINT32)(-1)) {
+    return FALSE;
+  }
+
+  if (Config->AcceptPromiscuous) {
+    return TRUE;
+  }
+
+  //
+  // Use protocol from the IP header embedded in the ICMP error
+  // message to filter, instead of ICMP itself. ICMP handle will
+  // call Ip4Demultiplex to deliver ICMP errors.
+  //
+  Proto = Head->Protocol;
+
+  if ((Proto == EFI_IP_PROTO_ICMP) && (!Config->AcceptAnyProtocol) && (Proto != Config->DefaultProtocol)) {
+    NetbufCopy (Packet, 0, sizeof (Icmp.Head), (UINT8 *) &Icmp.Head);
+
+    if (mIcmpClass[Icmp.Head.Type].IcmpClass == ICMP_ERROR_MESSAGE) {
+      if (!Config->AcceptIcmpErrors) {
+        return FALSE;
+      }
+
+      NetbufCopy (Packet, 0, sizeof (Icmp), (UINT8 *) &Icmp);
+      Proto = Icmp.IpHead.Protocol;
+    }
+  }
+
+  //
+  // Match the protocol
+  //
+  if (!Config->AcceptAnyProtocol && (Proto != Config->DefaultProtocol)) {
+    return FALSE;
+  }
+
+  //
+  // Check for broadcast, the caller has computed the packet's
+  // cast type for this child's interface.
+  //
+  Info = IP4_GET_CLIP_INFO (Packet);
+
+  if (IP4_IS_BROADCAST (Info->CastType)) {
+    return Config->AcceptBroadcast;
+  }
+
+  //
+  // If it is a multicast packet, check whether we are in the group.
+  //
+  if (Info->CastType == IP4_MULTICAST) {
+    //
+    // Receive the multicast if the instance wants to receive all packets.
+    //
+    if (!IpInstance->ConfigData.UseDefaultAddress && (IpInstance->Interface->Ip == 0)) {
+      return TRUE;
+    }
+
+    for (Index = 0; Index < IpInstance->GroupCount; Index++) {
+      if (IpInstance->Groups[Index] == HTONL (Head->Dst)) {
+        break;
+      }
+    }
+
+    return (BOOLEAN)(Index < IpInstance->GroupCount);
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Enqueue a shared copy of the packet to the IP4 child if the
+  packet is acceptable to it. Here the data of the packet is
+  shared, but the net buffer isn't.
+
+  @param[in]  IpInstance             The IP4 child to enqueue the packet to
+  @param[in]  Head                   The IP header of the received packet
+  @param[in]  Packet                 The data of the received packet
+
+  @retval EFI_NOT_STARTED        The IP child hasn't been configured.
+  @retval EFI_INVALID_PARAMETER  The child doesn't want to receive the packet
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate some resource
+  @retval EFI_SUCCESS            A shared copy the packet is enqueued to the child.
+
+**/
+EFI_STATUS
+Ip4InstanceEnquePacket (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_CLIP_INFO             *Info;
+  NET_BUF                   *Clone;
+
+  //
+  // Check whether the packet is acceptable to this instance.
+  //
+  if (IpInstance->State != IP4_STATE_CONFIGED) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (!Ip4InstanceFrameAcceptable (IpInstance, Head, Packet)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Enque a shared copy of the packet.
+  //
+  Clone = NetbufClone (Packet);
+
+  if (Clone == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Set the receive time out for the assembled packet. If it expires,
+  // packet will be removed from the queue.
+  //
+  Info        = IP4_GET_CLIP_INFO (Clone);
+  Info->Life  = IP4_US_TO_SEC (IpInstance->ConfigData.ReceiveTimeout);
+
+  InsertTailList (&IpInstance->Received, &Clone->List);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  The signal handle of IP4's recycle event. It is called back
+  when the upper layer release the packet.
+
+  @param  Event              The IP4's recycle event.
+  @param  Context            The context of the handle, which is a
+                             IP4_RXDATA_WRAP
+
+**/
+VOID
+EFIAPI
+Ip4OnRecyclePacket (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  IP4_RXDATA_WRAP           *Wrap;
+
+  Wrap = (IP4_RXDATA_WRAP *) Context;
+
+  EfiAcquireLockOrFail (&Wrap->IpInstance->RecycleLock);
+  RemoveEntryList (&Wrap->Link);
+  EfiReleaseLock (&Wrap->IpInstance->RecycleLock);
+
+  ASSERT (!NET_BUF_SHARED (Wrap->Packet));
+  NetbufFree (Wrap->Packet);
+
+  gBS->CloseEvent (Wrap->RxData.RecycleSignal);
+  FreePool (Wrap);
+}
+
+
+/**
+  Wrap the received packet to a IP4_RXDATA_WRAP, which will be
+  delivered to the upper layer. Each IP4 child that accepts the
+  packet will get a not-shared copy of the packet which is wrapped
+  in the IP4_RXDATA_WRAP. The IP4_RXDATA_WRAP->RxData is passed
+  to the upper layer. Upper layer will signal the recycle event in
+  it when it is done with the packet.
+
+  @param[in]  IpInstance    The IP4 child to receive the packet.
+  @param[in]  Packet        The packet to deliver up.
+
+  @retval Wrap              if warp the packet succeed.
+  @retval NULL              failed to wrap the packet .
+
+**/
+IP4_RXDATA_WRAP *
+Ip4WrapRxData (
+  IN IP4_PROTOCOL           *IpInstance,
+  IN NET_BUF                *Packet
+  )
+{
+  IP4_RXDATA_WRAP           *Wrap;
+  EFI_IP4_RECEIVE_DATA      *RxData;
+  EFI_STATUS                Status;
+  BOOLEAN                   RawData;
+
+  Wrap = AllocatePool (IP4_RXDATA_WRAP_SIZE (Packet->BlockOpNum));
+
+  if (Wrap == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&Wrap->Link);
+
+  Wrap->IpInstance  = IpInstance;
+  Wrap->Packet      = Packet;
+  RxData            = &Wrap->RxData;
+
+  ZeroMem (RxData, sizeof (EFI_IP4_RECEIVE_DATA));
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Ip4OnRecyclePacket,
+                  Wrap,
+                  &RxData->RecycleSignal
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (Wrap);
+    return NULL;
+  }
+
+  ASSERT (Packet->Ip.Ip4 != NULL);
+
+  ASSERT (IpInstance != NULL);
+  RawData = IpInstance->ConfigData.RawData;
+
+  //
+  // The application expects a network byte order header.
+  //
+  if (!RawData) {
+    RxData->HeaderLength  = (Packet->Ip.Ip4->HeadLen << 2);
+    RxData->Header        = (EFI_IP4_HEADER *) Ip4NtohHead (Packet->Ip.Ip4);
+    RxData->OptionsLength = RxData->HeaderLength - IP4_MIN_HEADLEN;
+    RxData->Options       = NULL;
+
+    if (RxData->OptionsLength != 0) {
+      RxData->Options = (VOID *) (RxData->Header + 1);
+    }
+  }
+
+  RxData->DataLength  = Packet->TotalSize;
+
+  //
+  // Build the fragment table to be delivered up.
+  //
+  RxData->FragmentCount = Packet->BlockOpNum;
+  NetbufBuildExt (Packet, (NET_FRAGMENT *) RxData->FragmentTable, &RxData->FragmentCount);
+
+  return Wrap;
+}
+
+
+/**
+  Deliver the received packets to upper layer if there are both received
+  requests and enqueued packets. If the enqueued packet is shared, it will
+  duplicate it to a non-shared packet, release the shared packet, then
+  deliver the non-shared packet up.
+
+  @param[in]  IpInstance         The IP child to deliver the packet up.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources to deliver the
+                                 packets.
+  @retval EFI_SUCCESS            All the enqueued packets that can be delivered
+                                 are delivered up.
+
+**/
+EFI_STATUS
+Ip4InstanceDeliverPacket (
+  IN IP4_PROTOCOL           *IpInstance
+  )
+{
+  EFI_IP4_COMPLETION_TOKEN  *Token;
+  IP4_RXDATA_WRAP           *Wrap;
+  NET_BUF                   *Packet;
+  NET_BUF                   *Dup;
+  UINT8                     *Head;
+  UINT32                    HeadLen;
+
+  //
+  // Deliver a packet if there are both a packet and a receive token.
+  //
+  while (!IsListEmpty (&IpInstance->Received) &&
+         !NetMapIsEmpty (&IpInstance->RxTokens)) {
+
+    Packet = NET_LIST_HEAD (&IpInstance->Received, NET_BUF, List);
+
+    if (!NET_BUF_SHARED (Packet)) {
+      //
+      // If this is the only instance that wants the packet, wrap it up.
+      //
+      Wrap = Ip4WrapRxData (IpInstance, Packet);
+
+      if (Wrap == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      RemoveEntryList (&Packet->List);
+
+    } else {
+      //
+      // Create a duplicated packet if this packet is shared
+      //
+      if (IpInstance->ConfigData.RawData) {
+        HeadLen = 0;
+      } else {
+        HeadLen = IP4_MAX_HEADLEN;
+      }
+
+      Dup = NetbufDuplicate (Packet, NULL, HeadLen);
+
+      if (Dup == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      if (!IpInstance->ConfigData.RawData) {
+        //
+        // Copy the IP head over. The packet to deliver up is
+        // headless. Trim the head off after copy. The IP head
+        // may be not continuous before the data.
+        //
+        Head = NetbufAllocSpace (Dup, IP4_MAX_HEADLEN, NET_BUF_HEAD);
+        ASSERT (Head != NULL);
+        
+        Dup->Ip.Ip4 = (IP4_HEAD *) Head;
+
+        CopyMem (Head, Packet->Ip.Ip4, Packet->Ip.Ip4->HeadLen << 2);
+        NetbufTrim (Dup, IP4_MAX_HEADLEN, TRUE);
+      }
+
+      Wrap = Ip4WrapRxData (IpInstance, Dup);
+
+      if (Wrap == NULL) {
+        NetbufFree (Dup);
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      RemoveEntryList (&Packet->List);
+      NetbufFree (Packet);
+
+      Packet = Dup;
+    }
+
+    //
+    // Insert it into the delivered packet, then get a user's
+    // receive token, pass the wrapped packet up.
+    //
+    EfiAcquireLockOrFail (&IpInstance->RecycleLock);
+    InsertHeadList (&IpInstance->Delivered, &Wrap->Link);
+    EfiReleaseLock (&IpInstance->RecycleLock);
+
+    Token                = NetMapRemoveHead (&IpInstance->RxTokens, NULL);
+    Token->Status        = IP4_GET_CLIP_INFO (Packet)->Status;
+    Token->Packet.RxData = &Wrap->RxData;
+
+    gBS->SignalEvent (Token->Event);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Enqueue a received packet to all the IP children that share
+  the same interface.
+
+  @param[in]  IpSb               The IP4 service instance that receive the packet.
+  @param[in]  Head               The header of the received packet.
+  @param[in]  Packet             The data of the received packet.
+  @param[in]  Option             Point to the IP4 packet header options.
+  @param[in]  OptionLen          Length of the IP4 packet header options.  
+  @param[in]  IpIf               The interface to enqueue the packet to.
+
+  @return The number of the IP4 children that accepts the packet
+
+**/
+INTN
+Ip4InterfaceEnquePacket (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet,
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen,
+  IN IP4_INTERFACE          *IpIf
+  )
+{
+  IP4_PROTOCOL              *IpInstance;
+  IP4_CLIP_INFO             *Info;
+  LIST_ENTRY                *Entry;
+  INTN                      Enqueued;
+  INTN                      LocalType;
+  INTN                      SavedType;
+
+  //
+  // First, check that the packet is acceptable to this interface
+  // and find the local cast type for the interface. A packet sent
+  // to say 192.168.1.1 should NOT be delliever to 10.0.0.1 unless
+  // promiscuous receiving.
+  //
+  LocalType = 0;
+  Info      = IP4_GET_CLIP_INFO (Packet);
+
+  if ((Info->CastType == IP4_MULTICAST) || (Info->CastType == IP4_LOCAL_BROADCAST)) {
+    //
+    // If the CastType is multicast, don't need to filter against
+    // the group address here, Ip4InstanceFrameAcceptable will do
+    // that later.
+    //
+    LocalType = Info->CastType;
+
+  } else {
+    //
+    // Check the destination againist local IP. If the station
+    // address is 0.0.0.0, it means receiving all the IP destined
+    // to local non-zero IP. Otherwise, it is necessary to compare
+    // the destination to the interface's IP address.
+    //
+    if (IpIf->Ip == IP4_ALLZERO_ADDRESS) {
+      LocalType = IP4_LOCAL_HOST;
+
+    } else {
+      LocalType = Ip4GetNetCast (Head->Dst, IpIf);
+
+      if ((LocalType == 0) && IpIf->PromiscRecv) {
+        LocalType = IP4_PROMISCUOUS;
+      }
+    }
+  }
+
+  if (LocalType == 0) {
+    return 0;
+  }
+
+  //
+  // Iterate through the ip instances on the interface, enqueue
+  // the packet if filter passed. Save the original cast type,
+  // and pass the local cast type to the IP children on the
+  // interface. The global cast type will be restored later.
+  //
+  SavedType       = Info->CastType;
+  Info->CastType  = LocalType;
+
+  Enqueued        = 0;
+
+  NET_LIST_FOR_EACH (Entry, &IpIf->IpInstances) {
+    IpInstance = NET_LIST_USER_STRUCT (Entry, IP4_PROTOCOL, AddrLink);
+    NET_CHECK_SIGNATURE (IpInstance, IP4_PROTOCOL_SIGNATURE);
+
+    //
+    // In RawData mode, add IPv4 headers and options back to packet.
+    //
+    if ((IpInstance->ConfigData.RawData) && (Option != NULL) && (OptionLen != 0)){
+      Ip4PrependHead (Packet, Head, Option, OptionLen);
+    }
+
+    if (Ip4InstanceEnquePacket (IpInstance, Head, Packet) == EFI_SUCCESS) {
+      Enqueued++;
+    }
+  }
+
+  Info->CastType = SavedType;
+  return Enqueued;
+}
+
+
+/**
+  Deliver the packet for each IP4 child on the interface.
+
+  @param[in]  IpSb               The IP4 service instance that received the packet
+  @param[in]  IpIf               The IP4 interface to deliver the packet.
+
+  @retval EFI_SUCCESS            It always returns EFI_SUCCESS now
+
+**/
+EFI_STATUS
+Ip4InterfaceDeliverPacket (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_INTERFACE          *IpIf
+  )
+{
+  IP4_PROTOCOL              *Ip4Instance;
+  LIST_ENTRY                *Entry;
+
+  NET_LIST_FOR_EACH (Entry, &IpIf->IpInstances) {
+    Ip4Instance = NET_LIST_USER_STRUCT (Entry, IP4_PROTOCOL, AddrLink);
+    Ip4InstanceDeliverPacket (Ip4Instance);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Demultiple the packet. the packet delivery is processed in two
+  passes. The first pass will enque a shared copy of the packet
+  to each IP4 child that accepts the packet. The second pass will
+  deliver a non-shared copy of the packet to each IP4 child that
+  has pending receive requests. Data is copied if more than one
+  child wants to consume the packet because each IP child needs
+  its own copy of the packet to make changes.
+
+  @param[in]  IpSb               The IP4 service instance that received the packet.
+  @param[in]  Head               The header of the received packet.
+  @param[in]  Packet             The data of the received packet.
+  @param[in]  Option             Point to the IP4 packet header options.
+  @param[in]  OptionLen          Length of the IP4 packet header options.
+
+  @retval EFI_NOT_FOUND          No IP child accepts the packet.
+  @retval EFI_SUCCESS            The packet is enqueued or delivered to some IP
+                                 children.
+
+**/
+EFI_STATUS
+Ip4Demultiplex (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet,
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen
+  )
+{
+  LIST_ENTRY                *Entry;
+  IP4_INTERFACE             *IpIf;
+  INTN                      Enqueued;
+
+  //
+  // Two pass delivery: first, enque a shared copy of the packet
+  // to each instance that accept the packet.
+  //
+  Enqueued = 0;
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured) {
+      Enqueued += Ip4InterfaceEnquePacket (
+                    IpSb,
+                    Head,
+                    Packet,
+                    Option,
+                    OptionLen,
+                    IpIf
+                    );
+    }
+  }
+
+  //
+  // Second: deliver a duplicate of the packet to each instance.
+  // Release the local reference first, so that the last instance
+  // getting the packet will not copy the data.
+  //
+  NetbufFree (Packet);
+
+  if (Enqueued == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured) {
+      Ip4InterfaceDeliverPacket (IpSb, IpIf);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Timeout the fragment and enqueued packets.
+
+  @param[in]  IpSb                   The IP4 service instance to timeout
+
+**/
+VOID
+Ip4PacketTimerTicking (
+  IN IP4_SERVICE            *IpSb
+  )
+{
+  LIST_ENTRY                *InstanceEntry;
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_PROTOCOL              *IpInstance;
+  IP4_ASSEMBLE_ENTRY        *Assemble;
+  NET_BUF                   *Packet;
+  IP4_CLIP_INFO             *Info;
+  UINT32                    Index;
+
+  //
+  // First, time out the fragments. The packet's life is counting down
+  // once the first-arrived fragment was received.
+  //
+  for (Index = 0; Index < IP4_ASSEMLE_HASH_SIZE; Index++) {
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &IpSb->Assemble.Bucket[Index]) {
+      Assemble = NET_LIST_USER_STRUCT (Entry, IP4_ASSEMBLE_ENTRY, Link);
+
+      if ((Assemble->Life > 0) && (--Assemble->Life == 0)) {
+        RemoveEntryList (Entry);
+        Ip4FreeAssembleEntry (Assemble);
+      }
+    }
+  }
+
+  NET_LIST_FOR_EACH (InstanceEntry, &IpSb->Children) {
+    IpInstance = NET_LIST_USER_STRUCT (InstanceEntry, IP4_PROTOCOL, Link);
+
+    //
+    // Second, time out the assembled packets enqueued on each IP child.
+    //
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &IpInstance->Received) {
+      Packet = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+      Info   = IP4_GET_CLIP_INFO (Packet);
+
+      if ((Info->Life > 0) && (--Info->Life == 0)) {
+        RemoveEntryList (Entry);
+        NetbufFree (Packet);
+      }
+    }
+
+    //
+    // Third: time out the transmitted packets.
+    //
+    NetMapIterate (&IpInstance->TxTokens, Ip4SentPacketTicking, NULL);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.h
new file mode 100644
index 0000000000..1c8e8b2a15
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Input.h
@@ -0,0 +1,252 @@
+/** @file
+
+Copyright (c) 2005 - 2012, 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.
+
+**/
+
+#ifndef __EFI_IP4_INPUT_H__
+#define __EFI_IP4_INPUT_H__
+
+#define IP4_MIN_HEADLEN        20
+#define IP4_MAX_HEADLEN        60
+///
+/// 8(ESP header) + 16(max IV) + 16(max padding) + 2(ESP tail) + 12(max ICV) = 54 
+///
+#define IP4_MAX_IPSEC_HEADLEN  54
+
+#define IP4_ASSEMLE_HASH_SIZE  31
+#define IP4_FRAGMENT_LIFE      120
+#define IP4_MAX_PACKET_SIZE    65535
+
+///
+/// Per packet information for input process. LinkFlag specifies whether
+/// the packet is received as Link layer unicast, multicast or broadcast.
+/// The CastType is the IP layer cast type, such as IP multicast or unicast.
+/// Start, End and Length are staffs used to assemble the packets. Start
+/// is the sequence number of the first byte of data in the packet. Length
+/// is the number of bytes of data. End = Start + Length, that is, the
+/// sequence number of last byte + 1. Each assembled packet has a count down
+/// life. If it isn't consumed before Life reaches zero, the packet is released.
+///
+typedef struct {
+  UINTN                     LinkFlag;
+  INTN                      CastType;
+  INTN                      Start;
+  INTN                      End;
+  INTN                      Length;
+  UINT32                    Life;
+  EFI_STATUS                Status;
+} IP4_CLIP_INFO;
+
+///
+/// Structure used to assemble IP packets.
+///
+typedef struct {
+  LIST_ENTRY                Link;
+
+  //
+  // Identity of one IP4 packet. Each fragment of a packet has
+  // the same (Dst, Src, Id, Protocol).
+  //
+  IP4_ADDR                  Dst;
+  IP4_ADDR                  Src;
+  UINT16                    Id;
+  UINT8                     Protocol;
+
+  INTN                      TotalLen;
+  INTN                      CurLen;
+  LIST_ENTRY                Fragments;  // List of all the fragments of this packet
+
+  IP4_HEAD                  *Head;      // IP head of the first fragment
+  IP4_CLIP_INFO             *Info;      // Per packet info of the first fragment
+  INTN                      Life;       // Count down life for the packet.
+} IP4_ASSEMBLE_ENTRY;
+
+///
+/// Each Ip service instance has an assemble table to reassemble
+/// the packets before delivery to its children. It is organized
+/// as hash table.
+///
+typedef struct {
+  LIST_ENTRY      Bucket[IP4_ASSEMLE_HASH_SIZE];
+} IP4_ASSEMBLE_TABLE;
+
+#define IP4_GET_CLIP_INFO(Packet) ((IP4_CLIP_INFO *) ((Packet)->ProtoData))
+
+#define IP4_ASSEMBLE_HASH(Dst, Src, Id, Proto)  \
+          (((Dst) + (Src) + ((Id) << 16) + (Proto)) % IP4_ASSEMLE_HASH_SIZE)
+
+#define IP4_RXDATA_WRAP_SIZE(NumFrag) \
+          (sizeof (IP4_RXDATA_WRAP) + sizeof (EFI_IP4_FRAGMENT_DATA) * ((NumFrag) - 1))
+
+/**
+  Initialize an already allocated assemble table. This is generally
+  the assemble table embedded in the IP4 service instance.
+
+  @param[in, out]  Table                  The assemble table to initialize.
+
+**/
+VOID
+Ip4InitAssembleTable (
+  IN OUT IP4_ASSEMBLE_TABLE     *Table
+  );
+
+/**
+  Clean up the assemble table: remove all the fragments
+  and assemble entries.
+
+  @param[in]  Table                  The assemble table to clean up
+
+**/
+VOID
+Ip4CleanAssembleTable (
+  IN IP4_ASSEMBLE_TABLE     *Table
+  );
+
+/**
+  The IP4 input routine. It is called by the IP4_INTERFACE when a
+  IP4 fragment is received from MNP.
+
+  @param[in]  Ip4Instance        The IP4 child that request the receive, most like
+                                 it is NULL.
+  @param[in]  Packet             The IP4 packet received.
+  @param[in]  IoStatus           The return status of receive request.
+  @param[in]  Flag               The link layer flag for the packet received, such
+                                 as multicast.
+  @param[in]  Context            The IP4 service instance that own the MNP.
+
+**/
+VOID
+Ip4AccpetFrame (
+  IN IP4_PROTOCOL           *Ip4Instance,
+  IN NET_BUF                *Packet,
+  IN EFI_STATUS             IoStatus,
+  IN UINT32                 Flag,
+  IN VOID                   *Context
+  );
+
+/**
+  Demultiple the packet. the packet delivery is processed in two
+  passes. The first pass will enque a shared copy of the packet
+  to each IP4 child that accepts the packet. The second pass will
+  deliver a non-shared copy of the packet to each IP4 child that
+  has pending receive requests. Data is copied if more than one
+  child wants to consume the packet because each IP child needs
+  its own copy of the packet to make changes.
+
+  @param[in]  IpSb               The IP4 service instance that received the packet.
+  @param[in]  Head               The header of the received packet.
+  @param[in]  Packet             The data of the received packet.
+  @param[in]  Option             Point to the IP4 packet header options.
+  @param[in]  OptionLen          Length of the IP4 packet header options.
+
+  @retval EFI_NOT_FOUND          No IP child accepts the packet.
+  @retval EFI_SUCCESS            The packet is enqueued or delivered to some IP
+                                 children.
+
+**/
+EFI_STATUS
+Ip4Demultiplex (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet,
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen
+  );
+
+/**
+  Enqueue a received packet to all the IP children that share
+  the same interface.
+
+  @param[in]  IpSb               The IP4 service instance that receive the packet.
+  @param[in]  Head               The header of the received packet.
+  @param[in]  Packet             The data of the received packet.
+  @param[in]  Option             Point to the IP4 packet header options.
+  @param[in]  OptionLen          Length of the IP4 packet header options.  
+  @param[in]  IpIf               The interface to enqueue the packet to.
+
+  @return The number of the IP4 children that accepts the packet
+
+**/
+INTN
+Ip4InterfaceEnquePacket (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_HEAD               *Head,
+  IN NET_BUF                *Packet,
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen,
+  IN IP4_INTERFACE          *IpIf
+  );
+
+/**
+  Deliver the received packets to upper layer if there are both received
+  requests and enqueued packets. If the enqueued packet is shared, it will
+  duplicate it to a non-shared packet, release the shared packet, then
+  deliver the non-shared packet up.
+
+  @param[in]  IpInstance         The IP child to deliver the packet up.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resources to deliver the
+                                 packets.
+  @retval EFI_SUCCESS            All the enqueued packets that can be delivered
+                                 are delivered up.
+
+**/
+EFI_STATUS
+Ip4InstanceDeliverPacket (
+  IN IP4_PROTOCOL           *IpInstance
+  );
+
+/**
+  Timeout the fragment and enqueued packets.
+
+  @param[in]  IpSb                   The IP4 service instance to timeout
+
+**/
+VOID
+Ip4PacketTimerTicking (
+  IN IP4_SERVICE            *IpSb
+  );
+
+/**
+  The work function to locate IPsec protocol to process the inbound or 
+  outbound IP packets. The process routine handls the packet with following
+  actions: bypass the packet, discard the packet, or protect the packet.       
+
+  @param[in]       IpSb          The IP4 service instance.
+  @param[in, out]  Head          The The caller supplied IP4 header.
+  @param[in, out]  Netbuf        The IP4 packet to be processed by IPsec.
+  @param[in, out]  Options       The caller supplied options.
+  @param[in, out]  OptionsLen    The length of the option.
+  @param[in]       Direction     The directionality in an SPD entry, 
+                                 EfiIPsecInBound or EfiIPsecOutBound.
+  @param[in]       Context       The token's wrap.
+
+  @retval EFI_SUCCESS            The IPsec protocol is not available or disabled.
+  @retval EFI_SUCCESS            The packet was bypassed and all buffers remain the same.
+  @retval EFI_SUCCESS            The packet was protected.
+  @retval EFI_ACCESS_DENIED      The packet was discarded.  
+  @retval EFI_OUT_OF_RESOURCES   There is no suffcient resource to complete the operation.
+  @retval EFI_BUFFER_TOO_SMALL   The number of non-empty block is bigger than the 
+                                 number of input data blocks when build a fragment table.
+
+**/
+EFI_STATUS
+Ip4IpSecProcessPacket (
+  IN     IP4_SERVICE            *IpSb,
+  IN OUT IP4_HEAD               **Head,
+  IN OUT NET_BUF                **Netbuf,
+  IN OUT UINT8                  **Options,
+  IN OUT UINT32                 *OptionsLen,
+  IN     EFI_IPSEC_TRAFFIC_DIR  Direction,
+  IN     VOID                   *Context
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4NvData.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4NvData.h
new file mode 100644
index 0000000000..a02b331980
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4NvData.h
@@ -0,0 +1,51 @@
+/** @file
+  Routines used to operate the Ip4Dxe.
+
+Copyright (c) 2015, 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<BR>
+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.
+
+**/
+
+#ifndef _IP4_NV_DATA_H_
+#define _IP4_NV_DATA_H_
+
+#include <Guid/Ip4Config2Hii.h>
+
+#define FORMID_MAIN_FORM    1
+#define FORMID_DEVICE_FORM  2
+
+#define KEY_ENABLE                0x100
+#define KEY_DHCP_ENABLE           0x101
+#define KEY_LOCAL_IP              0x102
+#define KEY_SUBNET_MASK           0x103
+#define KEY_GATE_WAY              0x104
+#define KEY_DNS                   0x105
+#define KEY_SAVE_CHANGES          0x106
+
+#define IP_MIN_SIZE               7
+#define IP_MAX_SIZE               15
+#define IP4_STR_MAX_SIZE          16
+#define ADDRESS_STR_MAX_SIZE      255
+#define MAX_IP4_CONFIG_DNS        16
+
+///
+/// IP4_CONFIG2_IFR_NVDATA contains the IP4 configure
+/// parameters for that NIC.
+///
+typedef struct {
+  UINT8           Configure;                         ///< NIC configure status
+  UINT8           DhcpEnable;                        ///< Static or DHCP
+  CHAR16          StationAddress[IP4_STR_MAX_SIZE];  ///< IP addresses
+  CHAR16          SubnetMask[IP4_STR_MAX_SIZE];      ///< Subnet address
+  CHAR16          GatewayAddress[IP4_STR_MAX_SIZE];  ///< Gateway address
+  CHAR16          DnsAddress[ADDRESS_STR_MAX_SIZE];  ///< DNS server address
+} IP4_CONFIG2_IFR_NVDATA;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.c
new file mode 100644
index 0000000000..6a92573275
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.c
@@ -0,0 +1,210 @@
+/** @file
+  IP4 option support functions.
+  
+Copyright (c) 2005 - 2011, 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 "Ip4Impl.h"
+
+
+/**
+  Validate the IP4 option format for both the packets we received
+  and will transmit.
+
+  @param[in]  Option            The first byte of the option
+  @param[in]  OptionLen         The length of the whole option
+  @param[in]  Rcvd              The option is from the packet we received if TRUE,
+                                otherwise the option we wants to transmit.
+
+  @retval TRUE     The option is properly formatted
+  @retval FALSE    The option is mal-formated
+
+**/
+BOOLEAN
+Ip4OptionIsValid (
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen,
+  IN BOOLEAN                Rcvd
+  )
+{
+  UINT32                    Cur;
+  UINT32                    Len;
+  UINT32                    Point;
+
+  Cur       = 0;
+
+  while (Cur < OptionLen) {
+    switch (Option[Cur]) {
+    case IP4_OPTION_NOP:
+      Cur++;
+      break;
+
+    case IP4_OPTION_EOP:
+      Cur = OptionLen;
+      break;
+
+    case IP4_OPTION_LSRR:
+    case IP4_OPTION_SSRR:
+    case IP4_OPTION_RR:
+      Len   = Option[Cur + 1];
+      Point = Option[Cur + 2];
+
+      //
+      // SRR/RR options are formatted as |Type|Len|Point|Ip1|Ip2|...
+      //
+      if ((OptionLen - Cur < Len) || (Len < 3) || ((Len - 3) % 4 != 0)) {
+        return FALSE;
+      }
+
+      if ((Point > Len + 1) || (Point % 4 != 0)) {
+        return FALSE;
+      }
+
+      //
+      // The Point must point pass the last entry if the packet is received
+      // by us. It must point to 4 if the packet is to be sent by us for
+      // source route option.
+      //
+      if ((Option[Cur] != IP4_OPTION_RR) &&
+          ((Rcvd && (Point != Len + 1)) || (!Rcvd && (Point != 4)))) {
+
+        return FALSE;
+      }
+
+      Cur += Len;
+      break;
+
+    default:
+      Len = Option[Cur + 1];
+
+      if ((OptionLen - Cur < Len) || (Len < 2)) {
+        return FALSE;
+      }
+
+      Cur = Cur + Len;
+      break;
+    }
+
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Copy the option from the original option to buffer. It
+  handles the details such as:
+  1. whether copy the single IP4 option to the first/non-first
+     fragments.
+  2. Pad the options copied over to aligned to 4 bytes.
+
+  @param[in]       Option            The original option to copy from
+  @param[in]       OptionLen         The length of the original option
+  @param[in]       FirstFragment     Whether it is the first fragment
+  @param[in, out]  Buf               The buffer to copy options to. NULL 
+  @param[in, out]  BufLen            The length of the buffer
+
+  @retval EFI_SUCCESS           The options are copied over
+  @retval EFI_BUFFER_TOO_SMALL  Buf is NULL or BufLen provided is too small.
+
+**/
+EFI_STATUS
+Ip4CopyOption (
+  IN     UINT8              *Option,
+  IN     UINT32             OptionLen,
+  IN     BOOLEAN            FirstFragment,
+  IN OUT UINT8              *Buf,           OPTIONAL
+  IN OUT UINT32             *BufLen
+  )
+{
+  UINT8                     OptBuf[40];
+  UINT32                    Cur;
+  UINT32                    Next;
+  UINT8                     Type;
+  UINT32                    Len;
+
+  ASSERT ((BufLen != NULL) && (OptionLen <= 40));
+
+  Cur   = 0;
+  Next  = 0;
+
+  while (Cur < OptionLen) {
+    Type  = Option[Cur];
+    Len   = Option[Cur + 1];
+
+    if (Type == IP4_OPTION_NOP) {
+      //
+      // Keep the padding, in case that the sender wants to align
+      // the option, say, to 4 bytes
+      //
+      OptBuf[Next] = IP4_OPTION_NOP;
+      Next++;
+      Cur++;
+
+    } else if (Type == IP4_OPTION_EOP) {
+      //
+      // Don't append the EOP to avoid including only a EOP option
+      //
+      break;
+
+    } else {
+      //
+      // don't copy options that is only valid for the first fragment
+      //
+      if (FirstFragment || (Type & IP4_OPTION_COPY_MASK) != 0) {
+        CopyMem (OptBuf + Next, Option + Cur, Len);
+        Next += Len;
+      }
+
+      Cur += Len;
+    }
+  }
+
+  //
+  // Don't append an EOP only option.
+  //
+  if (Next == 0) {
+    *BufLen = 0;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Append an EOP if the end of option doesn't coincide with the
+  // end of the IP header, that is, isn't aligned to 4 bytes..
+  //
+  if ((Next % 4) != 0) {
+    OptBuf[Next] = IP4_OPTION_EOP;
+    Next++;
+  }
+
+  //
+  // Head length is in the unit of 4 bytes. Now, Len is the
+  // acutal option length to appear in the IP header.
+  //
+  Len = ((Next + 3) &~0x03);
+
+  //
+  // If the buffer is too small, set the BufLen then return
+  //
+  if ((Buf == NULL) || (*BufLen < Len)) {
+    *BufLen = Len;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  //
+  // Copy the option to the Buf, zero the buffer first to pad
+  // the options with NOP to align to 4 bytes.
+  //
+  ZeroMem (Buf, Len);
+  CopyMem (Buf, OptBuf, Next);
+  *BufLen = Len;
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.h
new file mode 100644
index 0000000000..f9c65d026d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Option.h
@@ -0,0 +1,72 @@
+/** @file
+  IP4 option support routines.
+  
+Copyright (c) 2005 - 2009, 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.
+
+**/
+
+#ifndef __EFI_IP4_OPTION_H__
+#define __EFI_IP4_OPTION_H__
+
+#define IP4_OPTION_EOP        0
+#define IP4_OPTION_NOP        1
+#define IP4_OPTION_LSRR       131  // Loss source and record routing,   10000011
+#define IP4_OPTION_SSRR       137  // Strict source and record routing, 10001001
+#define IP4_OPTION_RR         7    // Record routing, 00000111
+
+#define IP4_OPTION_COPY_MASK  0x80
+
+/**
+  Validate the IP4 option format for both the packets we received
+  and will transmit. It will compute the ICMP error message fields
+  if the option is mal-formated. But this information isn't used.
+
+  @param[in]  Option            The first byte of the option
+  @param[in]  OptionLen         The length of the whole option
+  @param[in]  Rcvd              The option is from the packet we received if TRUE,
+                                otherwise the option we wants to transmit.
+
+  @retval TRUE     The option is properly formatted
+  @retval FALSE    The option is mal-formated
+
+**/
+BOOLEAN
+Ip4OptionIsValid (
+  IN UINT8                  *Option,
+  IN UINT32                 OptionLen,
+  IN BOOLEAN                Rcvd
+  );
+
+/**
+  Copy the option from the original option to buffer. It
+  handles the details such as:
+  1. whether copy the single IP4 option to the first/non-first
+     fragments.
+  2. Pad the options copied over to aligned to 4 bytes.
+
+  @param[in]       Option            The original option to copy from
+  @param[in]       OptionLen         The length of the original option
+  @param[in]       FirstFragment     Whether it is the first fragment
+  @param[in, out]  Buf               The buffer to copy options to. NULL 
+  @param[in, out]  BufLen            The length of the buffer
+
+  @retval EFI_SUCCESS           The options are copied over
+  @retval EFI_BUFFER_TOO_SMALL  Buf is NULL or BufLen provided is too small.
+
+**/
+EFI_STATUS
+Ip4CopyOption (
+  IN     UINT8              *Option,
+  IN     UINT32             OptionLen,
+  IN     BOOLEAN            FirstFragment,
+  IN OUT UINT8              *Buf,           OPTIONAL
+  IN OUT UINT32             *BufLen
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.c
new file mode 100644
index 0000000000..68d1f0943f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.c
@@ -0,0 +1,487 @@
+/** @file
+  Transmit the IP4 packet.
+  
+Copyright (c) 2005 - 2015, 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 "Ip4Impl.h"
+
+UINT16  mIp4Id;
+
+
+/**
+  Prepend an IP4 head to the Packet. It will copy the options and
+  build the IP4 header fields. Used for IP4 fragmentation.
+
+  @param  Packet           The packet to prepend IP4 header to
+  @param  Head             The caller supplied header. The caller should set
+                           the following header fields: Tos, TotalLen, Id,
+                           Fragment, Ttl, Protocol, Src and Dst. All the fields
+                           are in host byte order. This function will fill in
+                           the Ver, HeadLen, and checksum.
+  @param  Option           The orginal IP4 option to copy from
+  @param  OptLen           The length of the IP4 option
+
+  @retval EFI_BAD_BUFFER_SIZE  There is no enought room in the head space of
+                               Packet.
+  @retval EFI_SUCCESS          The IP4 header is successfully added to the packet.
+
+**/
+EFI_STATUS
+Ip4PrependHead (
+  IN OUT NET_BUF                *Packet,
+  IN     IP4_HEAD               *Head,
+  IN     UINT8                  *Option,
+  IN     UINT32                 OptLen
+  )
+{
+  UINT32                    HeadLen;
+  UINT32                    Len;
+  IP4_HEAD                  *PacketHead;
+  BOOLEAN                   FirstFragment;
+
+  //
+  // Prepend the options: first get the option length, then copy it over.
+  //
+  HeadLen       = 0;
+  FirstFragment = IP4_FIRST_FRAGMENT (Head->Fragment);
+
+  Ip4CopyOption (Option, OptLen, FirstFragment, NULL, &Len);
+
+  HeadLen = IP4_MIN_HEADLEN + Len;
+  ASSERT (((Len % 4) == 0) && (HeadLen <= IP4_MAX_HEADLEN));
+
+  PacketHead = (IP4_HEAD *) NetbufAllocSpace (Packet, HeadLen, NET_BUF_HEAD);
+
+  if (PacketHead == NULL) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  Ip4CopyOption (Option, OptLen, FirstFragment, (UINT8 *) (PacketHead + 1), &Len);
+
+  //
+  // Set the head up, convert the host byte order to network byte order
+  //
+  PacketHead->Ver       = 4;
+  PacketHead->HeadLen   = (UINT8) (HeadLen >> 2);
+  PacketHead->Tos       = Head->Tos;
+  PacketHead->TotalLen  = HTONS ((UINT16) Packet->TotalSize);
+  PacketHead->Id        = HTONS (Head->Id);
+  PacketHead->Fragment  = HTONS (Head->Fragment);
+  PacketHead->Checksum  = 0;
+  PacketHead->Ttl       = Head->Ttl;
+  PacketHead->Protocol  = Head->Protocol;
+  PacketHead->Src       = HTONL (Head->Src);
+  PacketHead->Dst       = HTONL (Head->Dst);
+  PacketHead->Checksum  = (UINT16) (~NetblockChecksum ((UINT8 *) PacketHead, HeadLen));
+
+  Packet->Ip.Ip4        = PacketHead;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Select an interface to send the packet generated in the IP4 driver
+  itself, that is, not by the requests of IP4 child's consumer. Such
+  packets include the ICMP echo replies, and other ICMP error packets.
+
+  @param[in]  IpSb                 The IP4 service that wants to send the packets.
+  @param[in]  Dst                  The destination of the packet
+  @param[in]  Src                  The source of the packet
+
+  @return NULL if no proper interface is found, otherwise the interface that
+          can be used to send the system packet from.
+
+**/
+IP4_INTERFACE *
+Ip4SelectInterface (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_ADDR               Dst,
+  IN IP4_ADDR               Src
+  )
+{
+  IP4_INTERFACE             *IpIf;
+  IP4_INTERFACE             *Selected;
+  LIST_ENTRY                *Entry;
+
+  //
+  // Select the interface the Dst is on if one of the connected
+  // network. Some IP instance may be configured with 0.0.0.0/0,
+  // don't select that interface now.
+  //
+  IpIf = Ip4FindNet (IpSb, Dst);
+
+  if ((IpIf != NULL) && (IpIf->Ip != IP4_ALLZERO_ADDRESS)) {
+    return IpIf;
+  }
+
+  //
+  // If source is one of the interface address, select it.
+  //
+  IpIf = Ip4FindInterface (IpSb, Src);
+
+  if ((IpIf != NULL) && (IpIf->Ip != IP4_ALLZERO_ADDRESS)) {
+    return IpIf;
+  }
+
+  //
+  // Select a configured interface as the fall back. Always prefer
+  // an interface with non-zero address.
+  //
+  Selected = NULL;
+
+  NET_LIST_FOR_EACH (Entry, &IpSb->Interfaces) {
+    IpIf = NET_LIST_USER_STRUCT (Entry, IP4_INTERFACE, Link);
+
+    if (IpIf->Configured && ((Selected == NULL) || (Selected->Ip == 0))) {
+      Selected = IpIf;
+    }
+  }
+
+  return Selected;
+}
+
+
+/**
+  The default callback function for system generated packet.
+  It will free the packet.
+
+  @param  Ip4Instance          The IP4 child that issued the transmission.  It most
+                               like is NULL.
+  @param  Packet               The packet that transmitted.
+  @param  IoStatus             The result of the transmission, succeeded or failed.
+  @param  LinkFlag             Not used when transmission. check IP4_FRAME_CALLBACK
+                               for reference.
+  @param  Context              The context provided by us
+
+**/
+VOID
+Ip4SysPacketSent (
+  IP4_PROTOCOL              *Ip4Instance,
+  NET_BUF                   *Packet,
+  EFI_STATUS                IoStatus,
+  UINT32                    LinkFlag,
+  VOID                      *Context
+  )
+{
+  NetbufFree (Packet);
+}
+
+
+/**
+  Transmit an IP4 packet. The packet comes either from the IP4
+  child's consumer (IpInstance != NULL) or the IP4 driver itself
+  (IpInstance == NULL). It will route the packet, fragment it,
+  then transmit all the fragments through some interface.
+
+  @param[in]  IpSb             The IP4 service instance to transmit the packet
+  @param[in]  IpInstance       The IP4 child that issues the transmission.  It is
+                               NULL if the packet is from the system.
+  @param[in]  Packet           The user data to send, excluding the IP header.
+  @param[in]  Head             The caller supplied header. The caller should set
+                               the following header fields: Tos, TotalLen, Id, tl,
+                               Fragment, Protocol, Src and Dst. All the fields are
+                               in host byte  order. This function will fill in the
+                               Ver, HeadLen,  Fragment, and checksum. The Fragment
+                               only need to include the DF flag. Ip4Output will
+                               compute the MF and offset for  you.
+  @param[in]  Option           The original option to append to the IP headers
+  @param[in]  OptLen           The length of the option
+  @param[in]  GateWay          The next hop address to transmit packet to.
+                               255.255.255.255 means broadcast.
+  @param[in]  Callback         The callback function to issue when transmission
+                               completed.
+  @param[in]  Context          The opaque context for the callback
+
+  @retval EFI_NO_MAPPING       There is no interface to the destination.
+  @retval EFI_NOT_FOUND        There is no route to the destination
+  @retval EFI_SUCCESS          The packet is successfully transmitted.
+  @retval EFI_BAD_BUFFER_SIZE  The length of the IPv4 header + option length +
+                               total data length is greater than MTU (or greater
+                               than the maximum packet size if Token.Packet.TxData.
+                               OverrideData.DoNotFragment is TRUE.)
+  @retval Others               Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Ip4Output (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_PROTOCOL           *IpInstance  OPTIONAL,
+  IN NET_BUF                *Packet,
+  IN IP4_HEAD               *Head,
+  IN UINT8                  *Option,
+  IN UINT32                 OptLen,
+  IN IP4_ADDR               GateWay,
+  IN IP4_FRAME_CALLBACK     Callback,
+  IN VOID                   *Context
+  )
+{
+  IP4_INTERFACE             *IpIf;
+  IP4_ROUTE_CACHE_ENTRY     *CacheEntry;
+  IP4_ADDR                  Dest;
+  EFI_STATUS                Status;
+  NET_BUF                   *Fragment;
+  UINT32                    Index;
+  UINT32                    HeadLen;
+  UINT32                    PacketLen;
+  UINT32                    Offset;
+  UINT32                    Mtu;
+  UINT32                    Num;
+  BOOLEAN                   RawData;
+
+  //
+  // Select an interface/source for system packet, application
+  // should select them itself.
+  //
+  if (IpInstance == NULL) {
+    IpIf = Ip4SelectInterface (IpSb, Head->Dst, Head->Src);
+  } else {
+    IpIf = IpInstance->Interface;
+  }
+
+  if (IpIf == NULL) {
+    return EFI_NO_MAPPING;
+  }
+
+  if ((Head->Src == IP4_ALLZERO_ADDRESS) && (IpInstance == NULL)) {
+    Head->Src = IpIf->Ip;
+  }
+
+  //
+  // Before IPsec process, prepared the IP head.
+  // If Ip4Output is transmitting RawData, don't update IPv4 header.
+  //
+  HeadLen = sizeof (IP4_HEAD) + ((OptLen + 3) & (~0x03));
+
+  if ((IpInstance != NULL) && IpInstance->ConfigData.RawData) {
+    RawData        = TRUE;
+  } else {
+    Head->HeadLen  = (UINT8) (HeadLen >> 2);
+    Head->Id       = mIp4Id++;
+    Head->Ver      = 4;
+    RawData        = FALSE;
+  }
+  
+  //
+  // Call IPsec process.
+  //
+  Status = Ip4IpSecProcessPacket (
+             IpSb, 
+             &Head, 
+             &Packet, 
+             &Option, 
+             &OptLen, 
+             EfiIPsecOutBound,
+             Context
+             );
+
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+  
+  Dest = Head->Dst;
+  if (IP4_IS_BROADCAST (Ip4GetNetCast (Dest, IpIf)) || (Dest == IP4_ALLONE_ADDRESS)) {
+    //
+    // Set the gateway to local broadcast if the Dest is
+    // the broadcast address for the connected network or
+    // it is local broadcast.
+    //
+    GateWay = IP4_ALLONE_ADDRESS;
+
+  } else if (IP4_IS_MULTICAST (Dest)) {
+    //
+    // Set the gateway to the destination if it is an multicast
+    // address. The IP4_INTERFACE won't consult ARP to send local
+    // broadcast and multicast.
+    //
+    GateWay = Head->Dst;
+
+  } else if (GateWay == IP4_ALLZERO_ADDRESS) {
+    //
+    // Route the packet unless overrided, that is, GateWay isn't zero.
+    //
+    if (IpInstance == NULL) {
+      CacheEntry = Ip4Route (IpSb->DefaultRouteTable, Head->Dst, Head->Src);
+    } else {
+      CacheEntry = Ip4Route (IpInstance->RouteTable, Head->Dst, Head->Src);
+      //
+      // If failed to route the packet by using the instance's route table,
+      // try to use the default route table.
+      //
+      if (CacheEntry == NULL) {
+        CacheEntry = Ip4Route (IpSb->DefaultRouteTable, Head->Dst, Head->Src);
+      }
+    }
+
+    if (CacheEntry == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    GateWay = CacheEntry->NextHop;
+    Ip4FreeRouteCacheEntry (CacheEntry);
+  }
+
+  //
+  // OK, selected the source and route, fragment the packet then send
+  // them. Tag each fragment other than the first one as spawn from it.
+  //
+  Mtu = IpSb->MaxPacketSize + sizeof (IP4_HEAD);  
+
+  if (Packet->TotalSize + HeadLen > Mtu) {
+    //
+    // Fragmentation is diabled for RawData mode.
+    //
+    if (RawData) {
+      return EFI_BAD_BUFFER_SIZE;
+    }
+    
+    //
+    // Packet is fragmented from the tail to the head, that is, the
+    // first frame sent is the last fragment of the packet. The first
+    // fragment is NOT sent in this loop. First compute how many
+    // fragments there are.
+    //
+    Mtu       = (Mtu - HeadLen) & (~0x07);
+    Num       = (Packet->TotalSize + Mtu - 1) / Mtu;
+
+    //
+    // Initialize the packet length and Offset. Other than the last
+    // fragment, the packet length equals to MTU. The offset is always
+    // aligned to MTU.
+    //
+    PacketLen = Packet->TotalSize - (Num - 1) * Mtu;
+    Offset    = Mtu * (Num - 1);
+
+    for (Index = 0; Index < Num - 1; Index++, Offset -= Mtu) {
+      Fragment = NetbufGetFragment (Packet, Offset, PacketLen, IP4_MAX_HEADLEN);
+
+      if (Fragment == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto ON_ERROR;
+      }
+
+      //
+      // Update the header's fragment. The caller fills the IP4 header
+      // fields that are required by Ip4PrependHead except the fragment.
+      //
+      Head->Fragment = IP4_HEAD_FRAGMENT_FIELD (FALSE, (Index != 0), Offset);
+      Ip4PrependHead (Fragment, Head, Option, OptLen);
+
+      //
+      // Transmit the fragments, pass the Packet address as the context.
+      // So, we can find all the fragments spawned from the Packet by
+      // compare the NetBuf and Context to the Packet.
+      //
+      Status = Ip4SendFrame (
+                 IpIf,
+                 IpInstance,
+                 Fragment,
+                 GateWay,
+                 Ip4SysPacketSent,
+                 Packet
+                 );
+
+      if (EFI_ERROR (Status)) {
+        goto ON_ERROR;
+      }
+
+      PacketLen = Mtu;
+    }
+
+    //
+    // Trim the already sent data, then adjust the head's fragment field.
+    //
+    NetbufTrim (Packet, Packet->TotalSize - Mtu, FALSE);
+    Head->Fragment = IP4_HEAD_FRAGMENT_FIELD (FALSE, TRUE, 0);
+  }
+
+  //
+  // Send the first fragment, it is either the orginal packet, or the
+  // first fragment of a fragmented packet. It seems that there is a subtle
+  // bug here: what if the caller free the packet in Callback and IpIf (or
+  // MNP child used by that interface) still holds the fragments and try
+  // to access the data? The caller can free the packet if it recycles the
+  // consumer's (such as UDP) data in the Callback. But this can't happen.
+  // The detailed sequence is:
+  // 1. for the packets generated by IP4 driver itself:
+  //    The Callback is Ip4SysPacketSent, which is the same as the
+  //    fragments' callback. Ip4SysPacketSent simply calls NetbufFree
+  //    to release its reference to the packet. So, no problem for
+  //    system packets.
+  //
+  // 2. for the upper layer's packets (use UDP as an example):
+  //    UDP requests the IP layer to transmit some data which is
+  //    wrapped in an asynchronous token, the token is wrapped
+  //    in IP4_TXTOKEN_WRAP by IP4. IP4 also wrap the user's data
+  //    in a net buffer, which is Packet we get here. IP4_TXTOKEN_WRAP
+  //    is bound with the Packet. It will only be freed when all
+  //    the references to Packet have been released. Upon then, the
+  //    Packet's OnFree callback will release the IP4_TXTOKEN_WRAP,
+  //    and singal the user's recycle event. So, also no problem for
+  //    upper layer's packets.
+  //
+  Ip4PrependHead (Packet, Head, Option, OptLen);
+  Status = Ip4SendFrame (IpIf, IpInstance, Packet, GateWay, Callback, Context);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  Ip4CancelPacket (IpIf, Packet, Status);
+  return Status;
+}
+
+
+/**
+  The filter function to find a packet and all its fragments.
+  The packet's fragments have their Context set to the packet.
+
+  @param[in]  Frame            The frames hold by the low level interface
+  @param[in]  Context          Context to the function, which is the packet.
+
+  @retval TRUE                 This is the packet to cancel or its fragments.
+  @retval FALSE                This is unrelated packet.
+
+**/
+BOOLEAN
+Ip4CancelPacketFragments (
+  IN IP4_LINK_TX_TOKEN   *Frame,
+  IN VOID                *Context
+  )
+{
+  if ((Frame->Packet == (NET_BUF *) Context) || (Frame->Context == Context)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Cancel the Packet and all its fragments.
+
+  @param  IpIf                 The interface from which the Packet is sent
+  @param  Packet               The Packet to cancel
+  @param  IoStatus             The status returns to the sender.
+
+**/
+VOID
+Ip4CancelPacket (
+  IN IP4_INTERFACE    *IpIf,
+  IN NET_BUF          *Packet,
+  IN EFI_STATUS       IoStatus
+  )
+{
+  Ip4CancelFrames (IpIf, IoStatus, Ip4CancelPacketFragments, Packet);
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.h
new file mode 100644
index 0000000000..e833352c1b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Output.h
@@ -0,0 +1,126 @@
+/** @file
+
+Copyright (c) 2005 - 2006, 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.
+
+**/
+
+#ifndef __EFI_IP4_OUTPUT_H__
+#define __EFI_IP4_OUTPUT_H__
+
+/**
+  The default callback function for system generated packet.
+  It will free the packet.
+
+  @param  Ip4Instance          The IP4 child that issued the transmission.  It most
+                               like is NULL.
+  @param  Packet               The packet that transmitted.
+  @param  IoStatus             The result of the transmission, succeeded or failed.
+  @param  LinkFlag             Not used when transmission. check IP4_FRAME_CALLBACK
+                               for reference.
+  @param  Context              The context provided by us
+
+**/
+VOID
+Ip4SysPacketSent (
+  IP4_PROTOCOL              *Ip4Instance,
+  NET_BUF                   *Packet,
+  EFI_STATUS                IoStatus,
+  UINT32                    LinkFlag,
+  VOID                      *Context
+  );
+
+/**
+  Transmit an IP4 packet. The packet comes either from the IP4
+  child's consumer (IpInstance != NULL) or the IP4 driver itself
+  (IpInstance == NULL). It will route the packet, fragment it,
+  then transmit all the fragments through some interface.
+
+  @param[in]  IpSb             The IP4 service instance to transmit the packet
+  @param[in]  IpInstance       The IP4 child that issues the transmission.  It is
+                               NULL if the packet is from the system.
+  @param[in]  Packet           The user data to send, excluding the IP header.
+  @param[in]  Head             The caller supplied header. The caller should set
+                               the following header fields: Tos, TotalLen, Id, tl,
+                               Fragment, Protocol, Src and Dst. All the fields are
+                               in host byte  order. This function will fill in the
+                               Ver, HeadLen,  Fragment, and checksum. The Fragment
+                               only need to include the DF flag. Ip4Output will
+                               compute the MF and offset for  you.
+  @param[in]  Option           The original option to append to the IP headers
+  @param[in]  OptLen           The length of the option
+  @param[in]  GateWay          The next hop address to transmit packet to.
+                               255.255.255.255 means broadcast.
+  @param[in]  Callback         The callback function to issue when transmission
+                               completed.
+  @param[in]  Context          The opaque context for the callback
+
+  @retval EFI_NO_MAPPING       There is no interface to the destination.
+  @retval EFI_NOT_FOUND        There is no route to the destination
+  @retval EFI_SUCCESS          The packet is successfully transmitted.
+  @retval Others               Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Ip4Output (
+  IN IP4_SERVICE            *IpSb,
+  IN IP4_PROTOCOL           *IpInstance  OPTIONAL,
+  IN NET_BUF                *Packet,
+  IN IP4_HEAD               *Head,
+  IN UINT8                  *Option,
+  IN UINT32                 OptLen,
+  IN IP4_ADDR               GateWay,
+  IN IP4_FRAME_CALLBACK     Callback,
+  IN VOID                   *Context
+  );
+
+/**
+  Cancel the Packet and all its fragments.
+
+  @param  IpIf                 The interface from which the Packet is sent
+  @param  Packet               The Packet to cancel
+  @param  IoStatus             The status returns to the sender.
+
+**/
+VOID
+Ip4CancelPacket (
+  IN IP4_INTERFACE    *IpIf,
+  IN NET_BUF          *Packet,
+  IN EFI_STATUS       IoStatus
+  );
+
+/**
+  Prepend an IP4 head to the Packet. It will copy the options and
+  build the IP4 header fields. Used for IP4 fragmentation.
+
+  @param  Packet           The packet to prepend IP4 header to
+  @param  Head             The caller supplied header. The caller should set
+                           the following header fields: Tos, TotalLen, Id,
+                           Fragment, Ttl, Protocol, Src and Dst. All the fields
+                           are in host byte order. This function will fill in
+                           the Ver, HeadLen, and checksum.
+  @param  Option           The orginal IP4 option to copy from
+  @param  OptLen           The length of the IP4 option
+
+  @retval EFI_BAD_BUFFER_SIZE  There is no enought room in the head space of
+                               Packet.
+  @retval EFI_SUCCESS          The IP4 header is successfully added to the packet.
+
+**/
+EFI_STATUS
+Ip4PrependHead (
+  IN OUT NET_BUF                *Packet,
+  IN     IP4_HEAD               *Head,
+  IN     UINT8                  *Option,
+  IN     UINT32                 OptLen
+  );
+
+extern UINT16  mIp4Id;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.c b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.c
new file mode 100644
index 0000000000..ea0023ddcb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.c
@@ -0,0 +1,661 @@
+/** @file
+
+Copyright (c) 2005 - 2009, 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 "Ip4Impl.h"
+
+
+/**
+  Allocate a route entry then initialize it with the Dest/Netmaks
+  and Gateway.
+
+  @param[in]  Dest                  The destination network
+  @param[in]  Netmask               The destination network mask
+  @param[in]  GateWay               The nexthop address
+
+  @return NULL if failed to allocate memeory, otherwise the newly created
+          route entry.
+
+**/
+IP4_ROUTE_ENTRY *
+Ip4CreateRouteEntry (
+  IN IP4_ADDR               Dest,
+  IN IP4_ADDR               Netmask,
+  IN IP4_ADDR               GateWay
+  )
+{
+  IP4_ROUTE_ENTRY           *RtEntry;
+
+  RtEntry = AllocatePool (sizeof (IP4_ROUTE_ENTRY));
+
+  if (RtEntry == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&RtEntry->Link);
+
+  RtEntry->RefCnt  = 1;
+  RtEntry->Dest    = Dest;
+  RtEntry->Netmask = Netmask;
+  RtEntry->NextHop = GateWay;
+  RtEntry->Flag    = 0;
+
+  return RtEntry;
+}
+
+
+/**
+  Free the route table entry. It is reference counted.
+
+  @param  RtEntry               The route entry to free.
+
+**/
+VOID
+Ip4FreeRouteEntry (
+  IN IP4_ROUTE_ENTRY    *RtEntry
+  )
+{
+  ASSERT (RtEntry->RefCnt > 0);
+
+  if (--RtEntry->RefCnt == 0) {
+    FreePool (RtEntry);
+  }
+}
+
+
+/**
+  Allocate and initialize an IP4 route cache entry.
+
+  @param[in]  Dst                   The destination address
+  @param[in]  Src                   The source address
+  @param[in]  GateWay               The next hop address
+  @param[in]  Tag                   The tag from the caller. This marks all the cache
+                                    entries spawned from one route table entry.
+
+  @return NULL if failed to allocate memory for the cache, other point
+          to the created route cache entry.
+
+**/
+IP4_ROUTE_CACHE_ENTRY *
+Ip4CreateRouteCacheEntry (
+  IN IP4_ADDR               Dst,
+  IN IP4_ADDR               Src,
+  IN IP4_ADDR               GateWay,
+  IN UINTN                  Tag
+  )
+{
+  IP4_ROUTE_CACHE_ENTRY     *RtCacheEntry;
+
+  RtCacheEntry = AllocatePool (sizeof (IP4_ROUTE_CACHE_ENTRY));
+
+  if (RtCacheEntry == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&RtCacheEntry->Link);
+
+  RtCacheEntry->RefCnt  = 1;
+  RtCacheEntry->Dest    = Dst;
+  RtCacheEntry->Src     = Src;
+  RtCacheEntry->NextHop = GateWay;
+  RtCacheEntry->Tag     = Tag;
+
+  return RtCacheEntry;
+}
+
+
+/**
+  Free the route cache entry. It is reference counted.
+
+  @param  RtCacheEntry          The route cache entry to free.
+
+**/
+VOID
+Ip4FreeRouteCacheEntry (
+  IN IP4_ROUTE_CACHE_ENTRY  *RtCacheEntry
+  )
+{
+  ASSERT (RtCacheEntry->RefCnt > 0);
+
+  if (--RtCacheEntry->RefCnt == 0) {
+    FreePool (RtCacheEntry);
+  }
+}
+
+
+/**
+  Initialize an empty route cache table.
+
+  @param[in, out]  RtCache               The rotue cache table to initialize.
+
+**/
+VOID
+Ip4InitRouteCache (
+  IN OUT IP4_ROUTE_CACHE        *RtCache
+  )
+{
+  UINT32                    Index;
+
+  for (Index = 0; Index < IP4_ROUTE_CACHE_HASH_VALUE; Index++) {
+    InitializeListHead (&(RtCache->CacheBucket[Index]));
+  }
+}
+
+
+/**
+  Clean up a route cache, that is free all the route cache
+  entries enqueued in the cache.
+
+  @param[in]  RtCache               The route cache table to clean up
+
+**/
+VOID
+Ip4CleanRouteCache (
+  IN IP4_ROUTE_CACHE        *RtCache
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ROUTE_CACHE_ENTRY     *RtCacheEntry;
+  UINT32                    Index;
+
+  for (Index = 0; Index < IP4_ROUTE_CACHE_HASH_VALUE; Index++) {
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &(RtCache->CacheBucket[Index])) {
+      RtCacheEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_CACHE_ENTRY, Link);
+
+      RemoveEntryList (Entry);
+      Ip4FreeRouteCacheEntry (RtCacheEntry);
+    }
+  }
+}
+
+
+
+/**
+  Create an empty route table, includes its internal route cache
+
+  @return NULL if failed to allocate memory for the route table, otherwise
+          the point to newly created route table.
+
+**/
+IP4_ROUTE_TABLE *
+Ip4CreateRouteTable (
+  VOID
+  )
+{
+  IP4_ROUTE_TABLE           *RtTable;
+  UINT32                    Index;
+
+  RtTable = AllocatePool (sizeof (IP4_ROUTE_TABLE));
+
+  if (RtTable == NULL) {
+    return NULL;
+  }
+
+  RtTable->RefCnt   = 1;
+  RtTable->TotalNum = 0;
+
+  for (Index = 0; Index < IP4_MASK_NUM; Index++) {
+    InitializeListHead (&(RtTable->RouteArea[Index]));
+  }
+
+  RtTable->Next = NULL;
+
+  Ip4InitRouteCache (&RtTable->Cache);
+  return RtTable;
+}
+
+
+/**
+  Free the route table and its associated route cache. Route
+  table is reference counted.
+
+  @param[in]  RtTable               The route table to free.
+
+**/
+VOID
+Ip4FreeRouteTable (
+  IN IP4_ROUTE_TABLE        *RtTable
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ROUTE_ENTRY           *RtEntry;
+  UINT32                    Index;
+
+  ASSERT (RtTable->RefCnt > 0);
+
+  if (--RtTable->RefCnt > 0) {
+    return ;
+  }
+
+  //
+  // Free all the route table entry and its route cache.
+  //
+  for (Index = 0; Index < IP4_MASK_NUM; Index++) {
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &(RtTable->RouteArea[Index])) {
+      RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+      RemoveEntryList (Entry);
+      Ip4FreeRouteEntry (RtEntry);
+    }
+  }
+
+  Ip4CleanRouteCache (&RtTable->Cache);
+
+  FreePool (RtTable);
+}
+
+
+
+/**
+  Remove all the cache entries bearing the Tag. When a route cache
+  entry is created, it is tagged with the address of route entry
+  from which it is spawned. When a route entry is deleted, the cache
+  entries spawned from it are also deleted.
+
+  @param  RtCache               Route cache to remove the entries from
+  @param  Tag                   The Tag of the entries to remove
+
+**/
+VOID
+Ip4PurgeRouteCache (
+  IN OUT IP4_ROUTE_CACHE        *RtCache,
+  IN     UINTN                  Tag
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ROUTE_CACHE_ENTRY     *RtCacheEntry;
+  UINT32                    Index;
+
+  for (Index = 0; Index < IP4_ROUTE_CACHE_HASH_VALUE; Index++) {
+    NET_LIST_FOR_EACH_SAFE (Entry, Next, &RtCache->CacheBucket[Index]) {
+
+      RtCacheEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_CACHE_ENTRY, Link);
+
+      if (RtCacheEntry->Tag == Tag) {
+        RemoveEntryList (Entry);
+        Ip4FreeRouteCacheEntry (RtCacheEntry);
+      }
+    }
+  }
+}
+
+
+/**
+  Add a route entry to the route table. All the IP4_ADDRs are in
+  host byte order.
+
+  @param[in, out]  RtTable      Route table to add route to
+  @param[in]       Dest         The destination of the network
+  @param[in]       Netmask      The netmask of the destination
+  @param[in]       Gateway      The next hop address
+
+  @retval EFI_ACCESS_DENIED     The same route already exists
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the entry
+  @retval EFI_SUCCESS           The route is added successfully.
+
+**/
+EFI_STATUS
+Ip4AddRoute (
+  IN OUT IP4_ROUTE_TABLE        *RtTable,
+  IN     IP4_ADDR               Dest,
+  IN     IP4_ADDR               Netmask,
+  IN     IP4_ADDR               Gateway
+  )
+{
+  LIST_ENTRY                *Head;
+  LIST_ENTRY                *Entry;
+  IP4_ROUTE_ENTRY           *RtEntry;
+
+  //
+  // All the route entries with the same netmask length are
+  // linke to the same route area
+  //
+  Head = &(RtTable->RouteArea[NetGetMaskLength (Netmask)]);
+
+  //
+  // First check whether the route exists
+  //
+  NET_LIST_FOR_EACH (Entry, Head) {
+    RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+    if (IP4_NET_EQUAL (RtEntry->Dest, Dest, Netmask) && (RtEntry->NextHop == Gateway)) {
+      return EFI_ACCESS_DENIED;
+    }
+  }
+
+  //
+  // Create a route entry and insert it to the route area.
+  //
+  RtEntry = Ip4CreateRouteEntry (Dest, Netmask, Gateway);
+
+  if (RtEntry == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (Gateway == IP4_ALLZERO_ADDRESS) {
+    RtEntry->Flag = IP4_DIRECT_ROUTE;
+  }
+
+  InsertHeadList (Head, &RtEntry->Link);
+  RtTable->TotalNum++;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove a route entry and all the route caches spawn from it.
+
+  @param  RtTable           The route table to remove the route from
+  @param  Dest              The destination network
+  @param  Netmask           The netmask of the Dest
+  @param  Gateway           The next hop address
+
+  @retval EFI_SUCCESS           The route entry is successfully removed
+  @retval EFI_NOT_FOUND         There is no route entry in the table with that
+                                properity.
+
+**/
+EFI_STATUS
+Ip4DelRoute (
+  IN OUT IP4_ROUTE_TABLE      *RtTable,
+  IN     IP4_ADDR             Dest,
+  IN     IP4_ADDR             Netmask,
+  IN     IP4_ADDR             Gateway
+  )
+{
+  LIST_ENTRY                *Head;
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ROUTE_ENTRY           *RtEntry;
+
+  Head = &(RtTable->RouteArea[NetGetMaskLength (Netmask)]);
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {
+    RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+    if (IP4_NET_EQUAL (RtEntry->Dest, Dest, Netmask) && (RtEntry->NextHop == Gateway)) {
+      Ip4PurgeRouteCache (&RtTable->Cache, (UINTN) RtEntry);
+      RemoveEntryList (Entry);
+      Ip4FreeRouteEntry  (RtEntry);
+
+      RtTable->TotalNum--;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Find a route cache with the dst and src. This is used by ICMP
+  redirect messasge process. All kinds of redirect is treated as
+  host redirect according to RFC1122. So, only route cache entries
+  are modified according to the ICMP redirect message.
+
+  @param[in]  RtTable               The route table to search the cache for
+  @param[in]  Dest                  The destination address
+  @param[in]  Src                   The source address
+
+  @return NULL if no route entry to the (Dest, Src). Otherwise the point
+          to the correct route cache entry.
+
+**/
+IP4_ROUTE_CACHE_ENTRY *
+Ip4FindRouteCache (
+  IN IP4_ROUTE_TABLE        *RtTable,
+  IN IP4_ADDR               Dest,
+  IN IP4_ADDR               Src
+  )
+{
+  LIST_ENTRY                *Entry;
+  IP4_ROUTE_CACHE_ENTRY     *RtCacheEntry;
+  UINT32                    Index;
+
+  Index = IP4_ROUTE_CACHE_HASH (Dest, Src);
+
+  NET_LIST_FOR_EACH (Entry, &RtTable->Cache.CacheBucket[Index]) {
+    RtCacheEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_CACHE_ENTRY, Link);
+
+    if ((RtCacheEntry->Dest == Dest) && (RtCacheEntry->Src == Src)) {
+      NET_GET_REF (RtCacheEntry);
+      return RtCacheEntry;
+    }
+  }
+
+  return NULL;
+}
+
+
+/**
+  Search the route table for a most specific match to the Dst. It searches
+  from the longest route area (mask length == 32) to the shortest route area
+  (default routes). In each route area, it will first search the instance's
+  route table, then the default route table. This is required by the following
+  requirements:
+  1. IP search the route table for a most specific match
+  2. The local route entries have precedence over the default route entry.
+
+  @param[in]  RtTable               The route table to search from
+  @param[in]  Dst                   The destionation address to search
+
+  @return NULL if no route matches the Dst, otherwise the point to the
+          most specific route to the Dst.
+
+**/
+IP4_ROUTE_ENTRY *
+Ip4FindRouteEntry (
+  IN IP4_ROUTE_TABLE        *RtTable,
+  IN IP4_ADDR               Dst
+  )
+{
+  LIST_ENTRY                *Entry;
+  IP4_ROUTE_ENTRY           *RtEntry;
+  IP4_ROUTE_TABLE           *Table;
+  INTN                      Index;
+
+  RtEntry = NULL;
+
+  for (Index = IP4_MASK_NUM - 1; Index >= 0; Index--) {
+    for (Table = RtTable; Table != NULL; Table = Table->Next) {
+      NET_LIST_FOR_EACH (Entry, &Table->RouteArea[Index]) {
+        RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+        if (IP4_NET_EQUAL (RtEntry->Dest, Dst, RtEntry->Netmask)) {
+          NET_GET_REF (RtEntry);
+          return RtEntry;
+        }
+      }
+    }
+  }
+
+
+  return NULL;
+}
+
+
+/**
+  Search the route table to route the packet. Return/create a route
+  cache if there is a route to the destination.
+
+  @param[in]  RtTable               The route table to search from
+  @param[in]  Dest                  The destination address to search for
+  @param[in]  Src                   The source address to search for
+
+  @return NULL if failed to route packet, otherwise a route cache
+          entry that can be used to route packet.
+
+**/
+IP4_ROUTE_CACHE_ENTRY *
+Ip4Route (
+  IN IP4_ROUTE_TABLE        *RtTable,
+  IN IP4_ADDR               Dest,
+  IN IP4_ADDR               Src
+  )
+{
+  LIST_ENTRY                *Head;
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  IP4_ROUTE_CACHE_ENTRY     *RtCacheEntry;
+  IP4_ROUTE_CACHE_ENTRY     *Cache;
+  IP4_ROUTE_ENTRY           *RtEntry;
+  IP4_ADDR                  NextHop;
+  UINT32                    Count;
+
+  ASSERT (RtTable != NULL);
+
+  Head          = &RtTable->Cache.CacheBucket[IP4_ROUTE_CACHE_HASH (Dest, Src)];
+  RtCacheEntry  = Ip4FindRouteCache (RtTable, Dest, Src);
+
+  //
+  // If found, promote the cache entry to the head of the hash bucket. LRU
+  //
+  if (RtCacheEntry != NULL) {
+    RemoveEntryList (&RtCacheEntry->Link);
+    InsertHeadList (Head, &RtCacheEntry->Link);
+    return RtCacheEntry;
+  }
+
+  //
+  // Search the route table for the most specific route
+  //
+  RtEntry = Ip4FindRouteEntry (RtTable, Dest);
+
+  if (RtEntry == NULL) {
+    return NULL;
+  }
+
+  //
+  // Found a route to the Dest, if it is a direct route, the packet
+  // will be sent directly to the destination, such as for connected
+  // network. Otherwise, it is an indirect route, the packet will be
+  // sent to the next hop router.
+  //
+  if ((RtEntry->Flag & IP4_DIRECT_ROUTE) != 0) {
+    NextHop = Dest;
+  } else {
+    NextHop = RtEntry->NextHop;
+  }
+
+  Ip4FreeRouteEntry (RtEntry);
+
+  //
+  // Create a route cache entry, and tag it as spawned from this route entry
+  //
+  RtCacheEntry = Ip4CreateRouteCacheEntry (Dest, Src, NextHop, (UINTN) RtEntry);
+
+  if (RtCacheEntry == NULL) {
+    return NULL;
+  }
+
+  InsertHeadList (Head, &RtCacheEntry->Link);
+  NET_GET_REF (RtCacheEntry);
+
+  //
+  // Each bucket of route cache can contain at most 64 entries.
+  // Remove the entries at the tail of the bucket. These entries
+  // are likely to be used least.
+  //
+  Count = 0;
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, Head) {
+    if (++Count < IP4_ROUTE_CACHE_MAX) {
+      continue;
+    }
+
+    Cache = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_CACHE_ENTRY, Link);
+
+    RemoveEntryList (Entry);
+    Ip4FreeRouteCacheEntry (Cache);
+  }
+
+  return RtCacheEntry;
+}
+
+
+/**
+  Build a EFI_IP4_ROUTE_TABLE to be returned to the caller of
+  GetModeData. The EFI_IP4_ROUTE_TABLE is clumsy to use in the
+  internal operation of the IP4 driver.
+
+  @param[in]  IpInstance        The IP4 child that requests the route table.
+
+  @retval EFI_SUCCESS           The route table is successfully build
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the rotue table.
+
+**/
+EFI_STATUS
+Ip4BuildEfiRouteTable (
+  IN IP4_PROTOCOL           *IpInstance
+  )
+{
+  LIST_ENTRY                *Entry;
+  IP4_ROUTE_TABLE           *RtTable;
+  IP4_ROUTE_ENTRY           *RtEntry;
+  EFI_IP4_ROUTE_TABLE       *Table;
+  UINT32                    Count;
+  INT32                     Index;
+
+  RtTable = IpInstance->RouteTable;
+
+  if (IpInstance->EfiRouteTable != NULL) {
+    FreePool (IpInstance->EfiRouteTable);
+
+    IpInstance->EfiRouteTable = NULL;
+    IpInstance->EfiRouteCount = 0;
+  }
+
+  Count = RtTable->TotalNum;
+
+  if (RtTable->Next != NULL) {
+    Count += RtTable->Next->TotalNum;
+  }
+
+  if (Count == 0) {
+    return EFI_SUCCESS;
+  }
+
+  Table = AllocatePool (sizeof (EFI_IP4_ROUTE_TABLE) * Count);
+
+  if (Table == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Copy the route entry to EFI route table. Keep the order of
+  // route entry copied from most specific to default route. That
+  // is, interlevel the route entry from the instance's route area
+  // and those from the default route table's route area.
+  //
+  Count = 0;
+
+  for (Index = IP4_MASK_NUM - 1; Index >= 0; Index--) {
+    for (RtTable = IpInstance->RouteTable; RtTable != NULL; RtTable = RtTable->Next) {
+      NET_LIST_FOR_EACH (Entry, &(RtTable->RouteArea[Index])) {
+        RtEntry = NET_LIST_USER_STRUCT (Entry, IP4_ROUTE_ENTRY, Link);
+
+        EFI_IP4 (Table[Count].SubnetAddress)  = HTONL (RtEntry->Dest & RtEntry->Netmask);
+        EFI_IP4 (Table[Count].SubnetMask)     = HTONL (RtEntry->Netmask);
+        EFI_IP4 (Table[Count].GatewayAddress) = HTONL (RtEntry->NextHop);
+
+        Count++;
+      }
+    }
+  }
+
+  IpInstance->EfiRouteTable = Table;
+  IpInstance->EfiRouteCount = Count;
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.h b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.h
new file mode 100644
index 0000000000..778c97dc8b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Ip4Dxe/Ip4Route.h
@@ -0,0 +1,224 @@
+/** @file
+  EFI IP4 route table and route cache table defintions.
+  
+Copyright (c) 2005 - 2009, 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.
+
+**/
+
+#ifndef __EFI_IP4_ROUTE_H__
+#define __EFI_IP4_ROUTE_H__
+
+#include "Ip4Common.h"
+
+#define IP4_DIRECT_ROUTE       0x00000001
+
+#define IP4_ROUTE_CACHE_HASH_VALUE 31
+#define IP4_ROUTE_CACHE_MAX        64  // Max NO. of cache entry per hash bucket
+
+#define IP4_ROUTE_CACHE_HASH(Dst, Src)  (((Dst) ^ (Src)) % IP4_ROUTE_CACHE_HASH_VALUE)
+
+///
+/// The route entry in the route table. Dest/Netmask is the destion
+/// network. The nexthop is the gateway to send the packet to in
+/// order to reach the Dest/Netmask. If the Flag has IP4_DIRECT_ROUTE
+/// on, the gateway is the destination of the IP packet itself. Route
+/// enties of the connected network have the flag on.
+///
+typedef struct {
+  LIST_ENTRY                Link;
+  INTN                      RefCnt;
+  IP4_ADDR                  Dest;
+  IP4_ADDR                  Netmask;
+  IP4_ADDR                  NextHop;
+  UINT32                    Flag;
+} IP4_ROUTE_ENTRY;
+
+///
+/// The route cache entry. The route cache entry is optional.
+/// But it is necessary to support the ICMP redirect message.
+/// Check Ip4ProcessIcmpRedirect for information.
+///
+/// The cache entry field Tag is used to tag all the route
+/// cache entry spawned from a route table entry. This makes
+/// it simple to delete all the route cache entries from a
+/// to-be-deleted route entry.
+///
+typedef struct {
+  LIST_ENTRY                Link;
+  INTN                      RefCnt;
+  IP4_ADDR                  Dest;
+  IP4_ADDR                  Src;
+  IP4_ADDR                  NextHop;
+  UINTN                     Tag;
+} IP4_ROUTE_CACHE_ENTRY;
+
+///
+/// The route cache table is organized as a hash table. Each
+/// IP4 route table has a embedded route cache. For now the
+/// route cache and route table are binded togehter. But keep
+/// the route cache a seperated structure in case we want to
+/// detach them later.
+///
+typedef struct {
+  LIST_ENTRY                CacheBucket[IP4_ROUTE_CACHE_HASH_VALUE];
+} IP4_ROUTE_CACHE;
+
+///
+/// Each IP4 instance has its own route table. Each ServiceBinding
+/// instance has a default route table and default address.
+///
+/// All the route table entries with the same mask are linked
+/// together in one route area. For example, RouteArea[0] contains
+/// the default routes. A route table also contains a route cache.
+///
+typedef struct _IP4_ROUTE_TABLE IP4_ROUTE_TABLE;
+
+struct _IP4_ROUTE_TABLE {
+  INTN                      RefCnt;
+  UINT32                    TotalNum;
+  LIST_ENTRY                RouteArea[IP4_MASK_NUM];
+  IP4_ROUTE_TABLE           *Next;
+  IP4_ROUTE_CACHE           Cache;
+};
+
+/**
+  Create an empty route table, includes its internal route cache
+
+  @return NULL if failed to allocate memory for the route table, otherwise
+          the point to newly created route table.
+
+**/
+IP4_ROUTE_TABLE *
+Ip4CreateRouteTable (
+  VOID
+  );
+
+/**
+  Free the route table and its associated route cache. Route
+  table is reference counted.
+
+  @param[in]  RtTable               The route table to free.
+
+**/
+VOID
+Ip4FreeRouteTable (
+  IN IP4_ROUTE_TABLE        *RtTable
+  );
+
+/**
+  Add a route entry to the route table. All the IP4_ADDRs are in
+  host byte order.
+
+  @param[in, out]  RtTable      Route table to add route to
+  @param[in]       Dest         The destination of the network
+  @param[in]       Netmask      The netmask of the destination
+  @param[in]       Gateway      The next hop address
+
+  @retval EFI_ACCESS_DENIED     The same route already exists
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the entry
+  @retval EFI_SUCCESS           The route is added successfully.
+
+**/
+EFI_STATUS
+Ip4AddRoute (
+  IN OUT IP4_ROUTE_TABLE        *RtTable,
+  IN     IP4_ADDR               Dest,
+  IN     IP4_ADDR               Netmask,
+  IN     IP4_ADDR               Gateway
+  );
+
+/**
+  Remove a route entry and all the route caches spawn from it.
+
+  @param  RtTable           The route table to remove the route from
+  @param  Dest              The destination network
+  @param  Netmask           The netmask of the Dest
+  @param  Gateway           The next hop address
+
+  @retval EFI_SUCCESS           The route entry is successfully removed
+  @retval EFI_NOT_FOUND         There is no route entry in the table with that
+                                properity.
+
+**/
+EFI_STATUS
+Ip4DelRoute (
+  IN OUT IP4_ROUTE_TABLE      *RtTable,
+  IN     IP4_ADDR             Dest,
+  IN     IP4_ADDR             Netmask,
+  IN     IP4_ADDR             Gateway
+  );
+
+/**
+  Find a route cache with the dst and src. This is used by ICMP
+  redirect messasge process. All kinds of redirect is treated as
+  host redirect according to RFC1122. So, only route cache entries
+  are modified according to the ICMP redirect message.
+
+  @param[in]  RtTable               The route table to search the cache for
+  @param[in]  Dest                  The destination address
+  @param[in]  Src                   The source address
+
+  @return NULL if no route entry to the (Dest, Src). Otherwise the point
+          to the correct route cache entry.
+
+**/
+IP4_ROUTE_CACHE_ENTRY *
+Ip4FindRouteCache (
+  IN IP4_ROUTE_TABLE        *RtTable,
+  IN IP4_ADDR               Dest,
+  IN IP4_ADDR               Src
+  );
+
+/**
+  Free the route cache entry. It is reference counted.
+
+  @param  RtCacheEntry          The route cache entry to free.
+
+**/
+VOID
+Ip4FreeRouteCacheEntry (
+  IN IP4_ROUTE_CACHE_ENTRY  *RtCacheEntry
+  );
+
+/**
+  Search the route table to route the packet. Return/create a route
+  cache if there is a route to the destination.
+
+  @param[in]  RtTable               The route table to search from
+  @param[in]  Dest                  The destination address to search for
+  @param[in]  Src                   The source address to search for
+
+  @return NULL if failed to route packet, otherwise a route cache
+          entry that can be used to route packet.
+
+**/
+IP4_ROUTE_CACHE_ENTRY *
+Ip4Route (
+  IN IP4_ROUTE_TABLE        *RtTable,
+  IN IP4_ADDR               Dest,
+  IN IP4_ADDR               Src
+  );
+
+/**
+  Build a EFI_IP4_ROUTE_TABLE to be returned to the caller of
+  GetModeData. The EFI_IP4_ROUTE_TABLE is clumsy to use in the
+  internal operation of the IP4 driver.
+
+  @param[in]  IpInstance        The IP4 child that requests the route table.
+
+  @retval EFI_SUCCESS           The route table is successfully build
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate the memory for the rotue table.
+
+**/
+EFI_STATUS
+Ip4BuildEfiRouteTable (
+  IN IP4_PROTOCOL           *IpInstance
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.c
new file mode 100644
index 0000000000..b6f96087ed
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.c
@@ -0,0 +1,348 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for MnpDxe driver.
+
+Copyright (c) 2005 - 2014, 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<BR>
+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  "MnpImpl.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL   gMnpComponentName = {
+  MnpComponentNameGetDriverName,
+  MnpComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL  gMnpComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) MnpComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) MnpComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE      mMnpDriverNameTable[] = {
+  {
+    "eng;en",
+    L"MNP Network Service Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE    *gMnpControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]   This             A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]   Language         A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mMnpDriverNameTable,
+           DriverName,
+           (BOOLEAN) (This == &gMnpComponentName)
+           );
+}
+
+/**
+  Update the component name for the MNP child handle.
+
+  @param  Mnp[in]                 A pointer to the EFI_MANAGED_NETWORK_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS             Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER   The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN   EFI_MANAGED_NETWORK_PROTOCOL     *Mnp
+  )
+{
+  EFI_STATUS                       Status;
+  MNP_INSTANCE_DATA                *Instance;
+  CHAR16                           HandleName[80];
+  EFI_MANAGED_NETWORK_CONFIG_DATA  MnpConfigData;
+  EFI_SIMPLE_NETWORK_MODE          SnpModeData;
+  UINTN                            OffSet;
+  UINTN                            Index;
+
+  if (Mnp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (Mnp);
+  //
+  // Format the child name into the string buffer as:
+  // MNP (MAC=FF-FF-FF-FF-FF-FF, ProtocolType=0x0800, VlanId=0)
+  //
+  Status = Mnp->GetModeData (Mnp, &MnpConfigData, &SnpModeData);
+  if (!EFI_ERROR (Status)) {
+    OffSet = 0;
+    //
+    // Print the MAC address.
+    //
+    OffSet += UnicodeSPrint (
+                HandleName,
+                sizeof (HandleName),
+                L"MNP (MAC="
+                );
+    for (Index = 0; Index < SnpModeData.HwAddressSize; Index++) {
+      OffSet += UnicodeSPrint (
+                  HandleName + OffSet,
+                  sizeof (HandleName) - OffSet * sizeof (CHAR16),
+                  L"%02X-",
+                  SnpModeData.CurrentAddress.Addr[Index]
+                  );
+    }
+    ASSERT (OffSet > 0);
+    //
+    // Remove the last '-'
+    //
+    OffSet--;  
+    //
+    // Print the ProtocolType and VLAN ID for this instance.
+    //
+    OffSet += UnicodeSPrint (
+                HandleName + OffSet,
+                sizeof (HandleName) - OffSet * sizeof (CHAR16),
+                L", ProtocolType=0x%X, VlanId=%d)",
+                MnpConfigData.ProtocolTypeFilter,
+                Instance->MnpServiceData->VlanId
+                );
+  } else if (Status == EFI_NOT_STARTED) {
+    UnicodeSPrint (
+      HandleName,
+      sizeof (HandleName),
+      L"MNP (Not started)"
+      );
+  } else {
+    return Status;
+  }
+  
+  if (gMnpControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gMnpControllerNameTable);
+    gMnpControllerNameTable = NULL;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gMnpComponentName.SupportedLanguages,
+             &gMnpControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gMnpComponentName2.SupportedLanguages,
+           &gMnpControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+  Currently not implemented.
+
+  @param[in]   This             A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]   ControllerHandle The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]   ChildHandle      The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]   Language         A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name
+                                specified by This, ControllerHandle, ChildHandle,
+                                and Language was returned in ControllerName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     EFI_HANDLE                    ControllerHandle,
+  IN     EFI_HANDLE                    ChildHandle        OPTIONAL,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_MANAGED_NETWORK_PROTOCOL  *Mnp;
+
+  //
+  // Only provide names for MNP child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+ 
+  // 
+  // Make sure this driver is currently managing ControllerHandle  
+  // 
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gMnpDriverBinding.DriverBindingHandle,
+             &gEfiSimpleNetworkProtocolGuid
+             );
+  if (EFI_ERROR (Status)) { 
+    return Status;
+  }
+  
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiManagedNetworkServiceBindingProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle  
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **)&Mnp,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Mnp);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gMnpControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gMnpComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.h b/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.h
new file mode 100644
index 0000000000..6232c3e1ed
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/ComponentName.h
@@ -0,0 +1,151 @@
+/** @file
+  The header file of UEFI Component Name(2) protocol.
+
+Copyright (c) 2004 - 2012, 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<BR>
+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.
+
+**/
+
+#ifndef _COMPONENT_NAME_H_
+#define _COMPONENT_NAME_H_
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+
+extern EFI_COMPONENT_NAME2_PROTOCOL gMnpComponentName2;
+extern EFI_COMPONENT_NAME_PROTOCOL  gMnpComponentName;
+extern EFI_UNICODE_STRING_TABLE     *gMnpControllerNameTable;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]   This             A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]   Language         A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+  Currently not implemented.
+
+  @param[in]   This             A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]   ControllerHandle The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]   ChildHandle      The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]   Language         A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name
+                                specified by This, ControllerHandle, ChildHandle,
+                                and Language was returned in ControllerName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     EFI_HANDLE                    ControllerHandle,
+  IN     EFI_HANDLE                    ChildHandle        OPTIONAL,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **ControllerName
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpConfig.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpConfig.c
new file mode 100644
index 0000000000..d1a4cb5dd2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpConfig.c
@@ -0,0 +1,1946 @@
+/** @file
+  Implementation of Managed Network Protocol private services.
+
+Copyright (c) 2005 - 2016, 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<BR>
+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 "MnpImpl.h"
+#include "MnpVlan.h"
+
+EFI_SERVICE_BINDING_PROTOCOL    mMnpServiceBindingProtocol = {
+  MnpServiceBindingCreateChild,
+  MnpServiceBindingDestroyChild
+};
+
+EFI_MANAGED_NETWORK_PROTOCOL    mMnpProtocolTemplate = {
+  MnpGetModeData,
+  MnpConfigure,
+  MnpMcastIpToMac,
+  MnpGroups,
+  MnpTransmit,
+  MnpReceive,
+  MnpCancel,
+  MnpPoll
+};
+
+EFI_MANAGED_NETWORK_CONFIG_DATA mMnpDefaultConfigData = {
+  10000000,
+  10000000,
+  0,
+  FALSE,
+  FALSE,
+  FALSE,
+  FALSE,
+  FALSE,
+  FALSE,
+  FALSE
+};
+
+/**
+  Add Count of net buffers to MnpDeviceData->FreeNbufQue. The length of the net
+  buffer is specified by MnpDeviceData->BufferLength.
+
+  @param[in, out]  MnpDeviceData         Pointer to the MNP_DEVICE_DATA.
+  @param[in]       Count                 Number of NET_BUFFERs to add.
+
+  @retval EFI_SUCCESS           The specified amount of NET_BUFs are allocated
+                                and added to MnpDeviceData->FreeNbufQue.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate a NET_BUF structure.
+
+**/
+EFI_STATUS
+MnpAddFreeNbuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     UINTN             Count
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       Index;
+  NET_BUF     *Nbuf;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+  ASSERT ((Count > 0) && (MnpDeviceData->BufferLength > 0));
+
+  Status = EFI_SUCCESS;
+  for (Index = 0; Index < Count; Index++) {
+    Nbuf = NetbufAlloc (MnpDeviceData->BufferLength + MnpDeviceData->PaddingSize);
+    if (Nbuf == NULL) {
+      DEBUG ((EFI_D_ERROR, "MnpAddFreeNbuf: NetBufAlloc failed.\n"));
+
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+
+    if (MnpDeviceData->PaddingSize > 0) {
+      //
+      // Pad padding bytes before the media header
+      //
+      NetbufAllocSpace (Nbuf, MnpDeviceData->PaddingSize, NET_BUF_TAIL);
+      NetbufTrim (Nbuf, MnpDeviceData->PaddingSize, NET_BUF_HEAD);
+    }
+
+    NetbufQueAppend (&MnpDeviceData->FreeNbufQue, Nbuf);
+  }
+
+  MnpDeviceData->NbufCnt += Index;
+  return Status;
+}
+
+
+/**
+  Allocate a free NET_BUF from MnpDeviceData->FreeNbufQue. If there is none
+  in the queue, first try to allocate some and add them into the queue, then
+  fetch the NET_BUF from the updated FreeNbufQue.
+
+  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.
+
+  @return     Pointer to the allocated free NET_BUF structure, if NULL the
+              operation is failed.
+
+**/
+NET_BUF *
+MnpAllocNbuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  )
+{
+  EFI_STATUS    Status;
+  NET_BUF_QUEUE *FreeNbufQue;
+  NET_BUF       *Nbuf;
+  EFI_TPL       OldTpl;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  FreeNbufQue = &MnpDeviceData->FreeNbufQue;
+  OldTpl      = gBS->RaiseTPL (TPL_NOTIFY);
+
+  //
+  // Check whether there are available buffers, or else try to add some.
+  //
+  if (FreeNbufQue->BufNum == 0) {
+    if ((MnpDeviceData->NbufCnt + MNP_NET_BUFFER_INCREASEMENT) > MNP_MAX_NET_BUFFER_NUM) {
+      DEBUG (
+        (EFI_D_ERROR,
+        "MnpAllocNbuf: The maximum NET_BUF size is reached for MNP driver instance %p.\n",
+        MnpDeviceData)
+        );
+
+      Nbuf = NULL;
+      goto ON_EXIT;
+    }
+
+    Status = MnpAddFreeNbuf (MnpDeviceData, MNP_NET_BUFFER_INCREASEMENT);
+    if (EFI_ERROR (Status)) {
+      DEBUG (
+        (EFI_D_ERROR,
+        "MnpAllocNbuf: Failed to add NET_BUFs into the FreeNbufQue, %r.\n",
+        Status)
+        );
+
+      //
+      // Don't return NULL, perhaps MnpAddFreeNbuf does add some NET_BUFs but
+      // the amount is less than MNP_NET_BUFFER_INCREASEMENT.
+      //
+    }
+  }
+
+  Nbuf = NetbufQueRemove (FreeNbufQue);
+
+  //
+  // Increase the RefCnt.
+  //
+  if (Nbuf != NULL) {
+    NET_GET_REF (Nbuf);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Nbuf;
+}
+
+
+/**
+  Try to reclaim the Nbuf into the buffer pool.
+
+  @param[in, out]  MnpDeviceData         Pointer to the mnp device context data.
+  @param[in, out]  Nbuf                  Pointer to the NET_BUF to free.
+
+**/
+VOID
+MnpFreeNbuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN OUT NET_BUF           *Nbuf
+  )
+{
+  EFI_TPL  OldTpl;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+  ASSERT (Nbuf->RefCnt > 1);
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  NET_PUT_REF (Nbuf);
+
+  if (Nbuf->RefCnt == 1) {
+    //
+    // Trim all buffer contained in the Nbuf, then append it to the NbufQue.
+    //
+    NetbufTrim (Nbuf, Nbuf->TotalSize, NET_BUF_TAIL);
+
+    if (NetbufAllocSpace (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD) != NULL) {
+      //
+      // There is space reserved for vlan tag in the head, reclaim it
+      //
+      NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_TAIL);
+    }
+
+    NetbufQueAppend (&MnpDeviceData->FreeNbufQue, Nbuf);
+  }
+
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Add Count of TX buffers to MnpDeviceData->AllTxBufList and MnpDeviceData->FreeTxBufList.
+  The length of the buffer is specified by MnpDeviceData->BufferLength.
+
+  @param[in, out]  MnpDeviceData         Pointer to the MNP_DEVICE_DATA.
+  @param[in]       Count                 Number of TX buffers to add.
+
+  @retval EFI_SUCCESS           The specified amount of TX buffers are allocated.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate a TX buffer.
+
+**/
+EFI_STATUS
+MnpAddFreeTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     UINTN             Count
+  )
+{
+  EFI_STATUS        Status;
+  UINT32            Index;
+  MNP_TX_BUF_WRAP   *TxBufWrap;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+  ASSERT ((Count > 0) && (MnpDeviceData->BufferLength > 0));
+
+  Status = EFI_SUCCESS;
+  for (Index = 0; Index < Count; Index++) {
+    TxBufWrap = (MNP_TX_BUF_WRAP*) AllocatePool (sizeof (MNP_TX_BUF_WRAP) + MnpDeviceData->BufferLength - 1);
+    if (TxBufWrap == NULL) {
+      DEBUG ((EFI_D_ERROR, "MnpAddFreeTxBuf: TxBuf Alloc failed.\n"));
+
+      Status = EFI_OUT_OF_RESOURCES;
+      break;
+    }
+    DEBUG ((EFI_D_INFO, "MnpAddFreeTxBuf: Add TxBufWrap %p, TxBuf %p\n", TxBufWrap, TxBufWrap->TxBuf));
+    TxBufWrap->Signature = MNP_TX_BUF_WRAP_SIGNATURE;
+    TxBufWrap->InUse     = FALSE;
+    InsertTailList (&MnpDeviceData->FreeTxBufList, &TxBufWrap->WrapEntry);
+    InsertTailList (&MnpDeviceData->AllTxBufList, &TxBufWrap->AllEntry);
+  }
+
+  MnpDeviceData->TxBufCount += Index;
+  return Status;
+}
+
+/**
+  Allocate a free TX buffer from MnpDeviceData->FreeTxBufList. If there is none
+  in the queue, first try to recycle some from SNP, then try to allocate some and add 
+  them into the queue, then fetch the NET_BUF from the updated FreeTxBufList.
+
+  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.
+
+  @return     Pointer to the allocated free NET_BUF structure, if NULL the
+              operation is failed.
+
+**/
+UINT8 *
+MnpAllocTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  )
+{
+  EFI_TPL           OldTpl;
+  UINT8             *TxBuf;
+  EFI_STATUS        Status;
+  LIST_ENTRY        *Entry;
+  MNP_TX_BUF_WRAP   *TxBufWrap;
+  
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
+    //
+    // First try to recycle some TX buffer from SNP
+    //
+    Status = MnpRecycleTxBuf (MnpDeviceData);
+    if (EFI_ERROR (Status)) {
+      TxBuf = NULL;
+      goto ON_EXIT;
+    }
+
+    //
+    // If still no free TX buffer, allocate more.
+    //
+    if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
+      if ((MnpDeviceData->TxBufCount + MNP_TX_BUFFER_INCREASEMENT) > MNP_MAX_TX_BUFFER_NUM) {
+        DEBUG (
+          (EFI_D_ERROR,
+          "MnpAllocTxBuf: The maximum TxBuf size is reached for MNP driver instance %p.\n",
+          MnpDeviceData)
+          );
+
+        TxBuf = NULL;
+        goto ON_EXIT;
+      }
+
+      Status = MnpAddFreeTxBuf (MnpDeviceData, MNP_TX_BUFFER_INCREASEMENT);
+      if (IsListEmpty (&MnpDeviceData->FreeTxBufList)) {
+        DEBUG (
+          (EFI_D_ERROR,
+          "MnpAllocNbuf: Failed to add TxBuf into the FreeTxBufList, %r.\n",
+          Status)
+          );
+
+        TxBuf = NULL;
+        goto ON_EXIT;
+      }
+    }
+  }
+
+  ASSERT (!IsListEmpty (&MnpDeviceData->FreeTxBufList));
+  Entry = MnpDeviceData->FreeTxBufList.ForwardLink;
+  RemoveEntryList (MnpDeviceData->FreeTxBufList.ForwardLink);
+  TxBufWrap = NET_LIST_USER_STRUCT_S (Entry, MNP_TX_BUF_WRAP, WrapEntry, MNP_TX_BUF_WRAP_SIGNATURE);
+  TxBufWrap->InUse = TRUE;
+  TxBuf = TxBufWrap->TxBuf;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return TxBuf;
+}
+
+/**
+  Try to reclaim the TX buffer into the buffer pool.
+
+  @param[in, out]  MnpDeviceData         Pointer to the mnp device context data.
+  @param[in, out]  TxBuf                 Pointer to the TX buffer to free.
+
+**/
+VOID
+MnpFreeTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN OUT UINT8             *TxBuf
+  )
+{
+  MNP_TX_BUF_WRAP   *TxBufWrap;
+  EFI_TPL           OldTpl;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  if (TxBuf == NULL) {
+    return;
+  }
+
+  TxBufWrap = NET_LIST_USER_STRUCT (TxBuf, MNP_TX_BUF_WRAP, TxBuf);
+  if (TxBufWrap->Signature != MNP_TX_BUF_WRAP_SIGNATURE) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "MnpFreeTxBuf: Signature check failed in MnpFreeTxBuf.\n")
+      );
+    return;
+  }
+
+  if (!TxBufWrap->InUse) {
+    DEBUG (
+      (EFI_D_WARN,
+      "MnpFreeTxBuf: Duplicated recycle report from SNP.\n")
+      );
+    return;
+  }
+  
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+  InsertTailList (&MnpDeviceData->FreeTxBufList, &TxBufWrap->WrapEntry);
+  TxBufWrap->InUse = FALSE;
+  gBS->RestoreTPL (OldTpl);
+}
+
+/**
+  Try to recycle all the transmitted buffer address from SNP.
+
+  @param[in, out]  MnpDeviceData     Pointer to the mnp device context data.
+
+  @retval EFI_SUCCESS             Successed to recyclethe transmitted buffer address.
+  @retval Others                  Failed to recyclethe transmitted buffer address.
+
+**/
+EFI_STATUS
+MnpRecycleTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  )
+{
+  UINT8                         *TxBuf;
+  EFI_SIMPLE_NETWORK_PROTOCOL   *Snp;
+  EFI_STATUS                    Status;
+
+  Snp = MnpDeviceData->Snp;
+  ASSERT (Snp != NULL);
+
+  do {
+    TxBuf = NULL;
+    Status = Snp->GetStatus (Snp, NULL, (VOID **) &TxBuf);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (TxBuf != NULL) {
+      MnpFreeTxBuf (MnpDeviceData, TxBuf);
+    }
+  } while (TxBuf != NULL);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize the mnp device context data.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       ImageHandle        The driver image handle.
+  @param[in]       ControllerHandle   Handle of device to bind driver to.
+
+  @retval EFI_SUCCESS           The mnp service context is initialized.
+  @retval EFI_UNSUPPORTED       ControllerHandle does not support Simple Network Protocol.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpInitializeDeviceData (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     EFI_HANDLE        ImageHandle,
+  IN     EFI_HANDLE        ControllerHandle
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  EFI_SIMPLE_NETWORK_MODE     *SnpMode;
+
+  MnpDeviceData->Signature        = MNP_DEVICE_DATA_SIGNATURE;
+  MnpDeviceData->ImageHandle      = ImageHandle;
+  MnpDeviceData->ControllerHandle = ControllerHandle;
+
+  //
+  // Copy the MNP Protocol interfaces from the template.
+  //
+  CopyMem (&MnpDeviceData->VlanConfig, &mVlanConfigProtocolTemplate, sizeof (EFI_VLAN_CONFIG_PROTOCOL));
+
+  //
+  // Open the Simple Network protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  (VOID **) &Snp,
+                  ImageHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get MTU from Snp.
+  //
+  SnpMode            = Snp->Mode;
+  MnpDeviceData->Snp = Snp;
+
+  //
+  // Initialize the lists.
+  //
+  InitializeListHead (&MnpDeviceData->ServiceList);
+  InitializeListHead (&MnpDeviceData->GroupAddressList);
+
+  //
+  // Get the buffer length used to allocate NET_BUF to hold data received
+  // from SNP. Do this before fill the FreeNetBufQue.
+  //
+  //
+  MnpDeviceData->BufferLength = SnpMode->MediaHeaderSize + NET_VLAN_TAG_LEN + SnpMode->MaxPacketSize + NET_ETHER_FCS_SIZE;
+
+  //
+  // Make sure the protocol headers immediately following the media header
+  // 4-byte aligned, and also preserve additional space for VLAN tag
+  //
+  MnpDeviceData->PaddingSize = ((4 - SnpMode->MediaHeaderSize) & 0x3) + NET_VLAN_TAG_LEN;
+
+  //
+  // Initialize MAC string which will be used as VLAN configuration variable name
+  //
+  Status = NetLibGetMacString (ControllerHandle, ImageHandle, &MnpDeviceData->MacString);
+  if (EFI_ERROR (Status)) {
+    goto ERROR;
+  }
+
+  //
+  // Initialize the FreeNetBufQue and pre-allocate some NET_BUFs.
+  //
+  NetbufQueInit (&MnpDeviceData->FreeNbufQue);
+  Status = MnpAddFreeNbuf (MnpDeviceData, MNP_INIT_NET_BUFFER_NUM);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: MnpAddFreeNbuf failed, %r.\n", Status));
+
+    goto ERROR;
+  }
+
+  //
+  // Get one NET_BUF from the FreeNbufQue for rx cache.
+  //
+  MnpDeviceData->RxNbufCache = MnpAllocNbuf (MnpDeviceData);
+  NetbufAllocSpace (
+    MnpDeviceData->RxNbufCache,
+    MnpDeviceData->BufferLength,
+    NET_BUF_TAIL
+    );
+
+  //
+  // Allocate buffer pool for tx.
+  //
+  InitializeListHead (&MnpDeviceData->FreeTxBufList);
+  InitializeListHead (&MnpDeviceData->AllTxBufList);
+  MnpDeviceData->TxBufCount = 0;
+
+  //
+  // Create the system poll timer.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  MnpSystemPoll,
+                  MnpDeviceData,
+                  &MnpDeviceData->PollTimer
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for poll timer failed.\n"));
+
+    goto ERROR;
+  }
+
+  //
+  // Create the timer for packet timeout check.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  MnpCheckPacketTimeout,
+                  MnpDeviceData,
+                  &MnpDeviceData->TimeoutCheckTimer
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for packet timeout check failed.\n"));
+
+    goto ERROR;
+  }
+
+  //
+  // Create the timer for media detection.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  MnpCheckMediaStatus,
+                  MnpDeviceData,
+                  &MnpDeviceData->MediaDetectTimer
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpInitializeDeviceData: CreateEvent for media detection failed.\n"));
+
+    goto ERROR;
+  }
+
+ERROR:
+  if (EFI_ERROR (Status)) {
+    //
+    // Free the dynamic allocated resources if necessary.
+    //
+    if (MnpDeviceData->MacString != NULL) {
+      FreePool (MnpDeviceData->MacString);
+    }
+
+    if (MnpDeviceData->TimeoutCheckTimer != NULL) {
+      gBS->CloseEvent (MnpDeviceData->TimeoutCheckTimer);
+    }
+
+    if (MnpDeviceData->MediaDetectTimer != NULL) {
+      gBS->CloseEvent (MnpDeviceData->MediaDetectTimer);
+    }
+
+    if (MnpDeviceData->PollTimer != NULL) {
+      gBS->CloseEvent (MnpDeviceData->PollTimer);
+    }
+
+    if (MnpDeviceData->RxNbufCache != NULL) {
+      MnpFreeNbuf (MnpDeviceData, MnpDeviceData->RxNbufCache);
+    }
+
+    if (MnpDeviceData->FreeNbufQue.BufNum != 0) {
+      NetbufQueFlush (&MnpDeviceData->FreeNbufQue);
+    }
+
+    //
+    // Close the Simple Network Protocol.
+    //
+    gBS->CloseProtocol (
+          ControllerHandle,
+          &gEfiSimpleNetworkProtocolGuid,
+          ImageHandle,
+          ControllerHandle
+          );
+  }
+
+  return Status;
+}
+
+
+/**
+  Destroy the MNP device context data.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       ImageHandle        The driver image handle.
+
+**/
+VOID
+MnpDestroyDeviceData (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     EFI_HANDLE        ImageHandle
+  )
+{
+  LIST_ENTRY         *Entry;
+  LIST_ENTRY         *NextEntry;
+  MNP_TX_BUF_WRAP    *TxBufWrap;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  //
+  // Free Vlan Config variable name string
+  //
+  if (MnpDeviceData->MacString != NULL) {
+    FreePool (MnpDeviceData->MacString);
+  }
+
+  //
+  // The GroupAddressList must be empty.
+  //
+  ASSERT (IsListEmpty (&MnpDeviceData->GroupAddressList));
+
+  //
+  // Close the event.
+  //
+  gBS->CloseEvent (MnpDeviceData->TimeoutCheckTimer);
+  gBS->CloseEvent (MnpDeviceData->MediaDetectTimer);
+  gBS->CloseEvent (MnpDeviceData->PollTimer);
+
+  //
+  // Free the Tx buffer pool.
+  //
+  NET_LIST_FOR_EACH_SAFE(Entry, NextEntry, &MnpDeviceData->AllTxBufList) {
+    TxBufWrap = NET_LIST_USER_STRUCT (Entry, MNP_TX_BUF_WRAP, AllEntry);
+    RemoveEntryList (Entry);
+    FreePool (TxBufWrap);
+    MnpDeviceData->TxBufCount--;
+  }
+  ASSERT (IsListEmpty (&MnpDeviceData->AllTxBufList));
+  ASSERT (MnpDeviceData->TxBufCount == 0);
+
+  //
+  // Free the RxNbufCache.
+  //
+  MnpFreeNbuf (MnpDeviceData, MnpDeviceData->RxNbufCache);
+
+  //
+  // Flush the FreeNbufQue.
+  //
+  MnpDeviceData->NbufCnt -= MnpDeviceData->FreeNbufQue.BufNum;
+  NetbufQueFlush (&MnpDeviceData->FreeNbufQue);
+
+  //
+  // Close the Simple Network Protocol.
+  //
+  gBS->CloseProtocol (
+         MnpDeviceData->ControllerHandle,
+         &gEfiSimpleNetworkProtocolGuid,
+         ImageHandle,
+         MnpDeviceData->ControllerHandle
+         );
+}
+
+
+/**
+  Create mnp service context data.
+
+  @param[in]       MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       VlanId             The VLAN ID.
+  @param[in]       Priority           The VLAN priority. If VlanId is 0,
+                                      Priority is ignored.
+
+  @return A pointer to MNP_SERVICE_DATA or NULL if failed to create MNP service context.
+
+**/
+MNP_SERVICE_DATA *
+MnpCreateServiceData (
+  IN MNP_DEVICE_DATA     *MnpDeviceData,
+  IN UINT16              VlanId,
+  IN UINT8                Priority OPTIONAL
+  )
+{
+  EFI_HANDLE                MnpServiceHandle;
+  MNP_SERVICE_DATA          *MnpServiceData;
+  EFI_STATUS                Status;
+  EFI_SIMPLE_NETWORK_MODE   *SnpMode;
+  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;
+
+  //
+  // Initialize the Mnp Service Data.
+  //
+  MnpServiceData = AllocateZeroPool (sizeof (MNP_SERVICE_DATA));
+  if (MnpServiceData == NULL) {
+    DEBUG ((EFI_D_ERROR, "MnpCreateServiceData: Faild to allocate memory for the new Mnp Service Data.\n"));
+
+    return NULL;
+  }
+
+  //
+  // Add to MNP service list
+  //
+  InsertTailList (&MnpDeviceData->ServiceList, &MnpServiceData->Link);
+
+  MnpServiceData->Signature     = MNP_SERVICE_DATA_SIGNATURE;
+  MnpServiceData->MnpDeviceData = MnpDeviceData;
+
+  //
+  // Copy the ServiceBinding structure.
+  //
+  CopyMem (&MnpServiceData->ServiceBinding, &mMnpServiceBindingProtocol, sizeof (EFI_SERVICE_BINDING_PROTOCOL));
+
+  //
+  // Initialize the lists.
+  //
+  InitializeListHead (&MnpServiceData->ChildrenList);
+
+  SnpMode = MnpDeviceData->Snp->Mode;
+  if (VlanId != 0) {
+    //
+    // Create VLAN child handle
+    //
+    MnpServiceHandle = MnpCreateVlanChild (
+                         MnpDeviceData->ImageHandle,
+                         MnpDeviceData->ControllerHandle,
+                         VlanId,
+                         &MnpServiceData->DevicePath
+                         );
+    if (MnpServiceHandle == NULL) {
+      DEBUG ((EFI_D_ERROR, "MnpCreateServiceData: Faild to create child handle.\n"));
+
+      return NULL;
+    }
+
+    //
+    // Open VLAN Config Protocol by child
+    //
+    Status = gBS->OpenProtocol (
+                    MnpDeviceData->ControllerHandle,
+                    &gEfiVlanConfigProtocolGuid,
+                    (VOID **) &VlanConfig,
+                    MnpDeviceData->ImageHandle,
+                    MnpServiceHandle,
+                    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                    );
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    //
+    // Reduce MTU for VLAN device
+    //
+    MnpServiceData->Mtu = SnpMode->MaxPacketSize - NET_VLAN_TAG_LEN;
+  } else {
+    //
+    // VlanId set to 0 means rx/tx untagged frame
+    //
+    MnpServiceHandle    = MnpDeviceData->ControllerHandle;
+    MnpServiceData->Mtu = SnpMode->MaxPacketSize;
+  }
+
+  MnpServiceData->ServiceHandle = MnpServiceHandle;
+  MnpServiceData->VlanId        = VlanId;
+  MnpServiceData->Priority      = Priority;
+
+  //
+  // Install the MNP Service Binding Protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &MnpServiceHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  &MnpServiceData->ServiceBinding,
+                  NULL
+                  );
+
+Exit:
+  if (EFI_ERROR (Status)) {
+    MnpDestroyServiceData (MnpServiceData);
+    MnpServiceData = NULL;
+  }
+
+  return MnpServiceData;
+}
+
+/**
+  Destroy the MNP service context data.
+
+  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.
+
+  @retval EFI_SUCCESS           The mnp service context is destroyed.
+  @retval Others                Errors as indicated.
+
+**/
+EFI_STATUS
+MnpDestroyServiceData (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Uninstall the MNP Service Binding Protocol
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  MnpServiceData->ServiceHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  &MnpServiceData->ServiceBinding,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (MnpServiceData->VlanId != 0) {
+    //
+    // Close VlanConfig Protocol opened by VLAN child handle
+    //
+    Status = gBS->CloseProtocol (
+                    MnpServiceData->MnpDeviceData->ControllerHandle,
+                    &gEfiVlanConfigProtocolGuid,
+                    MnpServiceData->MnpDeviceData->ImageHandle,
+                    MnpServiceData->ServiceHandle
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Uninstall Device Path Protocol to destroy the VLAN child handle
+    //
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    MnpServiceData->ServiceHandle,
+                    &gEfiDevicePathProtocolGuid,
+                    MnpServiceData->DevicePath,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (MnpServiceData->DevicePath != NULL) {
+      FreePool (MnpServiceData->DevicePath);
+    }
+  }
+
+  //
+  // Remove from MnpDeviceData service list
+  //
+  RemoveEntryList (&MnpServiceData->Link);
+
+  FreePool (MnpServiceData);
+
+  return Status;
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDestoryChildEntry (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  MNP_INSTANCE_DATA             *Instance;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+
+  ServiceBinding = (EFI_SERVICE_BINDING_PROTOCOL *) Context;
+  Instance = CR (Entry, MNP_INSTANCE_DATA, InstEntry, MNP_INSTANCE_DATA_SIGNATURE);
+  return ServiceBinding->DestroyChild (ServiceBinding, Instance->Handle);
+}
+
+/**
+  Destroy all child of the MNP service data.
+
+  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.
+
+  @retval EFI_SUCCESS           All child are destroyed.
+  @retval Others                Failed to destroy all child.
+
+**/
+EFI_STATUS
+MnpDestroyServiceChild (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData
+  )
+{
+  LIST_ENTRY                         *List;
+  EFI_STATUS                         Status;
+  UINTN                              ListLength;
+  
+  List = &MnpServiceData->ChildrenList;
+  
+  Status = NetDestroyLinkList (
+             List,
+             MnpDestoryChildEntry,
+             &MnpServiceData->ServiceBinding,
+             &ListLength
+             );
+  if (EFI_ERROR (Status) || ListLength != 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find the MNP Service Data for given VLAN ID.
+
+  @param[in]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]  VlanId             The VLAN ID.
+
+  @return A pointer to MNP_SERVICE_DATA or NULL if not found.
+
+**/
+MNP_SERVICE_DATA *
+MnpFindServiceData (
+  IN MNP_DEVICE_DATA     *MnpDeviceData,
+  IN UINT16              VlanId
+  )
+{
+  LIST_ENTRY        *Entry;
+  MNP_SERVICE_DATA  *MnpServiceData;
+
+  NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
+    //
+    // Check VLAN ID of each Mnp Service Data
+    //
+    MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+    if (MnpServiceData->VlanId == VlanId) {
+      return MnpServiceData;
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Initialize the mnp instance context data.
+
+  @param[in]       MnpServiceData   Pointer to the mnp service context data.
+  @param[in, out]  Instance         Pointer to the mnp instance context data
+                                    to initialize.
+
+**/
+VOID
+MnpInitializeInstanceData (
+  IN     MNP_SERVICE_DATA    *MnpServiceData,
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  )
+{
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+  ASSERT (Instance != NULL);
+
+  //
+  // Set the signature.
+  //
+  Instance->Signature = MNP_INSTANCE_DATA_SIGNATURE;
+
+  //
+  // Copy the MNP Protocol interfaces from the template.
+  //
+  CopyMem (&Instance->ManagedNetwork, &mMnpProtocolTemplate, sizeof (Instance->ManagedNetwork));
+
+  //
+  // Copy the default config data.
+  //
+  CopyMem (&Instance->ConfigData, &mMnpDefaultConfigData, sizeof (Instance->ConfigData));
+
+  //
+  // Initialize the lists.
+  //
+  InitializeListHead (&Instance->GroupCtrlBlkList);
+  InitializeListHead (&Instance->RcvdPacketQueue);
+  InitializeListHead (&Instance->RxDeliveredPacketQueue);
+
+  //
+  // Initialize the RxToken Map.
+  //
+  NetMapInit (&Instance->RxTokenMap);
+
+  //
+  // Save the MnpServiceData info.
+  //
+  Instance->MnpServiceData = MnpServiceData;
+}
+
+
+/**
+  Check whether the token specified by Arg matches the token in Item.
+
+  @param[in]  Map               Pointer to the NET_MAP.
+  @param[in]  Item              Pointer to the NET_MAP_ITEM.
+  @param[in]  Arg               Pointer to the Arg, it's a pointer to the token to
+                                check.
+
+  @retval EFI_SUCCESS           The token specified by Arg is different from the
+                                token in Item.
+  @retval EFI_ACCESS_DENIED     The token specified by Arg is the same as that in
+                                Item.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpTokenExist (
+  IN NET_MAP         *Map,
+  IN NET_MAP_ITEM    *Item,
+  IN VOID            *Arg
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *Token;
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TokenInItem;
+
+  Token       = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Arg;
+  TokenInItem = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Item->Key;
+
+  if ((Token == TokenInItem) || (Token->Event == TokenInItem->Event)) {
+    //
+    // The token is the same either the two tokens equals or the Events in
+    // the two tokens are the same.
+    //
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Cancel the token specified by Arg if it matches the token in Item.
+
+  @param[in, out]  Map               Pointer to the NET_MAP.
+  @param[in, out]  Item              Pointer to the NET_MAP_ITEM.
+  @param[in]       Arg               Pointer to the Arg, it's a pointer to the
+                                     token to cancel.
+
+  @retval EFI_SUCCESS       The Arg is NULL, and the token in Item is cancelled,
+                            or the Arg isn't NULL, and the token in Item is
+                            different from the Arg.
+  @retval EFI_ABORTED       The Arg isn't NULL, the token in Item mathces the
+                            Arg, and the token is cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpCancelTokens (
+  IN OUT NET_MAP         *Map,
+  IN OUT NET_MAP_ITEM    *Item,
+  IN     VOID            *Arg
+  )
+{
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *TokenToCancel;
+
+  if ((Arg != NULL) && (Item->Key != Arg)) {
+    //
+    // The token in Item is not the token specified by Arg.
+    //
+    return EFI_SUCCESS;
+  }
+
+  TokenToCancel = (EFI_MANAGED_NETWORK_COMPLETION_TOKEN *) Item->Key;
+
+  //
+  // Remove the item from the map.
+  //
+  NetMapRemoveItem (Map, Item, NULL);
+
+  //
+  // Cancel this token with status set to EFI_ABORTED.
+  //
+  TokenToCancel->Status = EFI_ABORTED;
+  gBS->SignalEvent (TokenToCancel->Event);
+
+  if (Arg != NULL) {
+    //
+    // Only abort the token specified by Arg if Arg isn't NULL.
+    //
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Start and initialize the simple network.
+
+  @param[in]  Snp               Pointer to the simple network protocol.
+
+  @retval EFI_SUCCESS           The simple network protocol is started.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpStartSnp (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL     *Snp
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (Snp != NULL);
+
+  //
+  // Start the simple network.
+  //
+  Status = Snp->Start (Snp);
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Initialize the simple network.
+    //
+    Status = Snp->Initialize (Snp, 0, 0);
+  }
+
+  return Status;
+}
+
+
+/**
+  Stop the simple network.
+
+  @param[in]  MnpDeviceData     Pointer to the MNP_DEVICE_DATA.
+
+  @retval EFI_SUCCESS           The simple network is stopped.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpStopSnp (
+  IN  MNP_DEVICE_DATA   *MnpDeviceData
+  )
+{
+  EFI_STATUS  Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL     *Snp;
+  
+  Snp = MnpDeviceData->Snp;
+  ASSERT (Snp != NULL);
+
+  //
+  // Recycle all the transmit buffer from SNP.
+  //
+  Status = MnpRecycleTxBuf (MnpDeviceData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Shut down the simple network.
+  //
+  Status  = Snp->Shutdown (Snp);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Stop the simple network.
+    //
+    Status = Snp->Stop (Snp);
+  }
+
+  return Status;
+}
+
+
+/**
+  Start the managed network, this function is called when one instance is configured
+  or reconfigured.
+
+  @param[in, out]  MnpServiceData       Pointer to the mnp service context data.
+  @param[in]       IsConfigUpdate       The instance is reconfigured or it's the first
+                                        time the instanced is configured.
+  @param[in]       EnableSystemPoll     Enable the system polling or not.
+
+  @retval EFI_SUCCESS                   The managed network is started and some
+                                        configuration is updated.
+  @retval Others                        Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpStart (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData,
+  IN     BOOLEAN             IsConfigUpdate,
+  IN     BOOLEAN             EnableSystemPoll
+  )
+{
+  EFI_STATUS      Status;
+  EFI_TIMER_DELAY TimerOpType;
+  MNP_DEVICE_DATA *MnpDeviceData;
+
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+
+  Status        = EFI_SUCCESS;
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+
+  if (!IsConfigUpdate) {
+    //
+    // If it's not a configuration update, increase the configured children number.
+    //
+    MnpDeviceData->ConfiguredChildrenNumber++;
+
+    if (MnpDeviceData->ConfiguredChildrenNumber == 1) {
+      //
+      // It's the first configured child, start the simple network.
+      //
+      Status = MnpStartSnp (MnpDeviceData->Snp);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "MnpStart: MnpStartSnp failed, %r.\n", Status));
+
+        goto ErrorExit;
+      }
+
+      //
+      // Start the timeout timer.
+      //
+      Status = gBS->SetTimer (
+                      MnpDeviceData->TimeoutCheckTimer,
+                      TimerPeriodic,
+                      MNP_TIMEOUT_CHECK_INTERVAL
+                      );
+      if (EFI_ERROR (Status)) {
+        DEBUG (
+          (EFI_D_ERROR,
+          "MnpStart, gBS->SetTimer for TimeoutCheckTimer %r.\n",
+          Status)
+          );
+
+        goto ErrorExit;
+      }
+
+      //
+      // Start the media detection timer.
+      //
+      Status = gBS->SetTimer (
+                      MnpDeviceData->MediaDetectTimer,
+                      TimerPeriodic,
+                      MNP_MEDIA_DETECT_INTERVAL
+                      );
+      if (EFI_ERROR (Status)) {
+        DEBUG (
+          (EFI_D_ERROR,
+          "MnpStart, gBS->SetTimer for MediaDetectTimer %r.\n",
+          Status)
+          );
+
+        goto ErrorExit;
+      }
+    }
+  }
+
+  if (MnpDeviceData->EnableSystemPoll ^ EnableSystemPoll) {
+    //
+    // The EnableSystemPoll differs with the current state, disable or enable
+    // the system poll.
+    //
+    TimerOpType = EnableSystemPoll ? TimerPeriodic : TimerCancel;
+
+    Status      = gBS->SetTimer (MnpDeviceData->PollTimer, TimerOpType, MNP_SYS_POLL_INTERVAL);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "MnpStart: gBS->SetTimer for PollTimer failed, %r.\n", Status));
+
+      goto ErrorExit;
+    }
+
+    MnpDeviceData->EnableSystemPoll = EnableSystemPoll;
+  }
+
+  //
+  // Change the receive filters if need.
+  //
+  Status = MnpConfigReceiveFilters (MnpDeviceData);
+
+ErrorExit:
+  return Status;
+}
+
+
+/**
+  Stop the managed network.
+
+  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.
+
+  @retval EFI_SUCCESS                The managed network is stopped.
+  @retval Others                     Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpStop (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData
+  )
+{
+  EFI_STATUS      Status;
+  MNP_DEVICE_DATA *MnpDeviceData;
+
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+  ASSERT (MnpDeviceData->ConfiguredChildrenNumber > 0);
+
+  //
+  // Configure the receive filters.
+  //
+  MnpConfigReceiveFilters (MnpDeviceData);
+
+  //
+  // Decrease the children number.
+  //
+  MnpDeviceData->ConfiguredChildrenNumber--;
+
+  if (MnpDeviceData->ConfiguredChildrenNumber > 0) {
+    //
+    // If there are other configured chilren, return and keep the timers and
+    // simple network unchanged.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // No configured children now.
+  //
+  if (MnpDeviceData->EnableSystemPoll) {
+    //
+    //  The system poll in on, cancel the poll timer.
+    //
+    Status  = gBS->SetTimer (MnpDeviceData->PollTimer, TimerCancel, 0);
+    MnpDeviceData->EnableSystemPoll = FALSE;
+  }
+
+  //
+  // Cancel the timeout timer.
+  //
+  Status = gBS->SetTimer (MnpDeviceData->TimeoutCheckTimer, TimerCancel, 0);
+
+  //
+  // Cancel the media detect timer.
+  //
+  Status = gBS->SetTimer (MnpDeviceData->MediaDetectTimer, TimerCancel, 0);
+
+  //
+  // Stop the simple network.
+  //
+  Status = MnpStopSnp (MnpDeviceData);
+  return Status;
+}
+
+
+/**
+  Flush the instance's received data.
+
+  @param[in, out]  Instance              Pointer to the mnp instance context data.
+
+**/
+VOID
+MnpFlushRcvdDataQueue (
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  )
+{
+  EFI_TPL         OldTpl;
+  MNP_RXDATA_WRAP *RxDataWrap;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  while (!IsListEmpty (&Instance->RcvdPacketQueue)) {
+    //
+    // Remove all the Wraps.
+    //
+    RxDataWrap = NET_LIST_HEAD (&Instance->RcvdPacketQueue, MNP_RXDATA_WRAP, WrapEntry);
+
+    //
+    // Recycle the RxDataWrap.
+    //
+    MnpRecycleRxData (NULL, (VOID *) RxDataWrap);
+    Instance->RcvdPacketQueueSize--;
+  }
+
+  ASSERT (Instance->RcvdPacketQueueSize == 0);
+
+  gBS->RestoreTPL (OldTpl);
+}
+
+
+/**
+  Configure the Instance using ConfigData.
+
+  @param[in, out]  Instance     Pointer to the mnp instance context data.
+  @param[in]       ConfigData   Pointer to the configuration data used to configure
+                                the isntance.
+
+  @retval EFI_SUCCESS           The Instance is configured.
+  @retval EFI_UNSUPPORTED       EnableReceiveTimestamps is on and the
+                                implementation doesn't support it.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpConfigureInstance (
+  IN OUT MNP_INSTANCE_DATA                 *Instance,
+  IN     EFI_MANAGED_NETWORK_CONFIG_DATA   *ConfigData OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  MNP_SERVICE_DATA                *MnpServiceData;
+  MNP_DEVICE_DATA                 *MnpDeviceData;
+  EFI_MANAGED_NETWORK_CONFIG_DATA *OldConfigData;
+  EFI_MANAGED_NETWORK_CONFIG_DATA *NewConfigData;
+  BOOLEAN                         IsConfigUpdate;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  if ((ConfigData != NULL) && ConfigData->EnableReceiveTimestamps) {
+    //
+    // Don't support timestamp.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Status          = EFI_SUCCESS;
+
+  MnpServiceData  = Instance->MnpServiceData;
+  MnpDeviceData   = MnpServiceData->MnpDeviceData;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  IsConfigUpdate  = (BOOLEAN) ((Instance->Configured) && (ConfigData != NULL));
+
+  OldConfigData   = &Instance->ConfigData;
+  NewConfigData   = ConfigData;
+  if (NewConfigData == NULL) {
+    //
+    // Restore back the default config data if a reset of this instance
+    // is required.
+    //
+    NewConfigData = &mMnpDefaultConfigData;
+  }
+
+  //
+  // Reset the instance's receive filter.
+  //
+  Instance->ReceiveFilter = 0;
+
+  //
+  // Clear the receive counters according to the old ConfigData.
+  //
+  if (OldConfigData->EnableUnicastReceive) {
+    MnpDeviceData->UnicastCount--;
+  }
+
+  if (OldConfigData->EnableMulticastReceive) {
+    MnpDeviceData->MulticastCount--;
+  }
+
+  if (OldConfigData->EnableBroadcastReceive) {
+    MnpDeviceData->BroadcastCount--;
+  }
+
+  if (OldConfigData->EnablePromiscuousReceive) {
+    MnpDeviceData->PromiscuousCount--;
+  }
+
+  //
+  // Set the receive filter counters and the receive filter of the
+  // instance according to the new ConfigData.
+  //
+  if (NewConfigData->EnableUnicastReceive) {
+    MnpDeviceData->UnicastCount++;
+    Instance->ReceiveFilter |= MNP_RECEIVE_UNICAST;
+  }
+
+  if (NewConfigData->EnableMulticastReceive) {
+    MnpDeviceData->MulticastCount++;
+  }
+
+  if (NewConfigData->EnableBroadcastReceive) {
+    MnpDeviceData->BroadcastCount++;
+    Instance->ReceiveFilter |= MNP_RECEIVE_BROADCAST;
+  }
+
+  if (NewConfigData->EnablePromiscuousReceive) {
+    MnpDeviceData->PromiscuousCount++;
+  }
+
+  if (OldConfigData->FlushQueuesOnReset) {
+    MnpFlushRcvdDataQueue (Instance);
+  }
+
+  if (ConfigData == NULL) {
+    Instance->ManagedNetwork.Cancel (&Instance->ManagedNetwork, NULL);
+  }
+
+  if (!NewConfigData->EnableMulticastReceive) {
+    MnpGroupOp (Instance, FALSE, NULL, NULL);
+  }
+
+  //
+  // Save the new configuration data.
+  //
+  CopyMem (OldConfigData, NewConfigData, sizeof (*OldConfigData));
+
+  Instance->Configured = (BOOLEAN) (ConfigData != NULL);
+  if (Instance->Configured) {
+    //
+    // The instance is configured, start the Mnp.
+    //
+    Status = MnpStart (
+              MnpServiceData,
+              IsConfigUpdate,
+              (BOOLEAN) !NewConfigData->DisableBackgroundPolling
+              );
+  } else {
+    //
+    // The instance is changed to the unconfigured state, stop the Mnp.
+    //
+    Status = MnpStop (MnpServiceData);
+  }
+
+  return Status;
+}
+
+/**
+  Configure the Snp receive filters according to the instances' receive filter
+  settings.
+
+  @param[in]  MnpDeviceData         Pointer to the mnp device context data.
+
+  @retval     EFI_SUCCESS           The receive filters is configured.
+  @retval     EFI_OUT_OF_RESOURCES  The receive filters can't be configured due
+                                    to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpConfigReceiveFilters (
+  IN MNP_DEVICE_DATA     *MnpDeviceData
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  EFI_MAC_ADDRESS             *MCastFilter;
+  UINT32                      MCastFilterCnt;
+  UINT32                      EnableFilterBits;
+  UINT32                      DisableFilterBits;
+  BOOLEAN                     ResetMCastFilters;
+  LIST_ENTRY                  *Entry;
+  UINT32                      Index;
+  MNP_GROUP_ADDRESS           *GroupAddress;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  Snp = MnpDeviceData->Snp;
+
+  //
+  // Initialize the enable filter and disable filter.
+  //
+  EnableFilterBits  = 0;
+  DisableFilterBits = Snp->Mode->ReceiveFilterMask;
+
+  if (MnpDeviceData->UnicastCount != 0) {
+    //
+    // Enable unicast if any instance wants to receive unicast.
+    //
+    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_UNICAST;
+  }
+
+  if (MnpDeviceData->BroadcastCount != 0) {
+    //
+    // Enable broadcast if any instance wants to receive broadcast.
+    //
+    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST;
+  }
+
+  MCastFilter       = NULL;
+  MCastFilterCnt    = 0;
+  ResetMCastFilters = TRUE;
+
+  if ((MnpDeviceData->MulticastCount != 0) && (MnpDeviceData->GroupAddressCount != 0)) {
+    //
+    // There are instances configured to receive multicast and already some group
+    // addresses are joined.
+    //
+
+    ResetMCastFilters = FALSE;
+
+    if (MnpDeviceData->GroupAddressCount <= Snp->Mode->MaxMCastFilterCount) {
+      //
+      // The joind group address is less than simple network's maximum count.
+      // Just configure the snp to do the multicast filtering.
+      //
+
+      EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST;
+
+      //
+      // Allocate pool for the mulicast addresses.
+      //
+      MCastFilterCnt  = MnpDeviceData->GroupAddressCount;
+      MCastFilter     = AllocatePool (sizeof (EFI_MAC_ADDRESS) * MCastFilterCnt);
+      if (MCastFilter == NULL) {
+        DEBUG ((EFI_D_ERROR, "MnpConfigReceiveFilters: Failed to allocate memory resource for MCastFilter.\n"));
+
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      //
+      // Fill the multicast HW address buffer.
+      //
+      Index = 0;
+      NET_LIST_FOR_EACH (Entry, &MnpDeviceData->GroupAddressList) {
+
+        GroupAddress = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_ADDRESS, AddrEntry);
+        CopyMem (MCastFilter + Index, &GroupAddress->Address, sizeof (*(MCastFilter + Index)));
+        Index++;
+
+        ASSERT (Index <= MCastFilterCnt);
+      }
+    } else {
+      //
+      // The maximum multicast is reached, set the filter to be promiscuous
+      // multicast.
+      //
+
+      if ((Snp->Mode->ReceiveFilterMask & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST) != 0) {
+        EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST;
+      } else {
+        //
+        // Either MULTICAST or PROMISCUOUS_MULTICAST is not supported by Snp,
+        // set the NIC to be promiscuous although this will tremendously degrade
+        // the performance.
+        //
+        EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
+      }
+    }
+  }
+
+  if (MnpDeviceData->PromiscuousCount != 0) {
+    //
+    // Enable promiscuous if any instance wants to receive promiscuous.
+    //
+    EnableFilterBits |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
+  }
+
+  //
+  // Set the disable filter.
+  //
+  DisableFilterBits ^= EnableFilterBits;
+
+  //
+  // Configure the receive filters of SNP.
+  //
+  Status = Snp->ReceiveFilters (
+                  Snp,
+                  EnableFilterBits,
+                  DisableFilterBits,
+                  ResetMCastFilters,
+                  MCastFilterCnt,
+                  MCastFilter
+                  );
+  DEBUG_CODE (
+    if (EFI_ERROR (Status)) {
+      DEBUG (
+        (EFI_D_ERROR,
+        "MnpConfigReceiveFilters: Snp->ReceiveFilters failed, %r.\n",
+        Status)
+        );
+    }
+  );
+
+  if (MCastFilter != NULL) {
+    //
+    // Free the buffer used to hold the group addresses.
+    //
+    FreePool (MCastFilter);
+  }
+
+  return Status;
+}
+
+
+/**
+  Add a group address control block which controls the MacAddress for
+  this instance.
+
+  @param[in, out]  Instance        Pointer to the mnp instance context data.
+  @param[in, out]  CtrlBlk         Pointer to the group address control block.
+  @param[in, out]  GroupAddress    Pointer to the group adress.
+  @param[in]       MacAddress      Pointer to the mac address.
+  @param[in]       HwAddressSize   The hardware address size.
+
+  @retval EFI_SUCCESS              The group address control block is added.
+  @retval EFI_OUT_OF_RESOURCES     Failed due to lack of memory resources.
+
+**/
+EFI_STATUS
+MnpGroupOpAddCtrlBlk (
+  IN OUT MNP_INSTANCE_DATA         *Instance,
+  IN OUT MNP_GROUP_CONTROL_BLOCK   *CtrlBlk,
+  IN OUT MNP_GROUP_ADDRESS         *GroupAddress OPTIONAL,
+  IN     EFI_MAC_ADDRESS           *MacAddress,
+  IN     UINT32                    HwAddressSize
+  )
+{
+  MNP_DEVICE_DATA  *MnpDeviceData;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  if (GroupAddress == NULL) {
+    ASSERT (MacAddress != NULL);
+
+    //
+    // Allocate a new GroupAddress to be added into MNP's GroupAddressList.
+    //
+    GroupAddress = AllocatePool (sizeof (MNP_GROUP_ADDRESS));
+    if (GroupAddress == NULL) {
+
+      DEBUG ((EFI_D_ERROR, "MnpGroupOpFormCtrlBlk: Failed to allocate memory resource.\n"));
+
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    CopyMem (&GroupAddress->Address, MacAddress, sizeof (GroupAddress->Address));
+    GroupAddress->RefCnt = 0;
+    InsertTailList (
+      &MnpDeviceData->GroupAddressList,
+      &GroupAddress->AddrEntry
+      );
+    MnpDeviceData->GroupAddressCount++;
+  }
+
+  //
+  // Increase the RefCnt.
+  //
+  GroupAddress->RefCnt++;
+
+  //
+  // Add the CtrlBlk into the instance's GroupCtrlBlkList.
+  //
+  CtrlBlk->GroupAddress = GroupAddress;
+  InsertTailList (&Instance->GroupCtrlBlkList, &CtrlBlk->CtrlBlkEntry);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Delete a group control block from the instance. If the controlled group address's
+  reference count reaches zero, the group address is removed too.
+
+  @param[in]  Instance              Pointer to the instance context data.
+  @param[in]  CtrlBlk               Pointer to the group control block to delete.
+
+  @return The group address controlled by the control block is no longer used or not.
+
+**/
+BOOLEAN
+MnpGroupOpDelCtrlBlk (
+  IN MNP_INSTANCE_DATA           *Instance,
+  IN MNP_GROUP_CONTROL_BLOCK     *CtrlBlk
+  )
+{
+  MNP_DEVICE_DATA   *MnpDeviceData;
+  MNP_GROUP_ADDRESS *GroupAddress;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  //
+  // Remove and free the CtrlBlk.
+  //
+  GroupAddress = CtrlBlk->GroupAddress;
+  RemoveEntryList (&CtrlBlk->CtrlBlkEntry);
+  FreePool (CtrlBlk);
+
+  ASSERT (GroupAddress->RefCnt > 0);
+
+  //
+  // Count down the RefCnt.
+  //
+  GroupAddress->RefCnt--;
+
+  if (GroupAddress->RefCnt == 0) {
+    //
+    // Free this GroupAddress entry if no instance uses it.
+    //
+    MnpDeviceData->GroupAddressCount--;
+    RemoveEntryList (&GroupAddress->AddrEntry);
+    FreePool (GroupAddress);
+
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Do the group operations for this instance.
+
+  @param[in, out]  Instance        Pointer to the instance context data.
+  @param[in]       JoinFlag        Set to TRUE to join a group. Set to TRUE to
+                                   leave a group/groups.
+  @param[in]       MacAddress      Pointer to the group address to join or leave.
+  @param[in]       CtrlBlk         Pointer to the group control block if JoinFlag
+                                   is FALSE.
+
+  @retval EFI_SUCCESS              The group operation finished.
+  @retval EFI_OUT_OF_RESOURCES     Failed due to lack of memory resources.
+  @retval Others                   Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpGroupOp (
+  IN OUT MNP_INSTANCE_DATA         *Instance,
+  IN     BOOLEAN                   JoinFlag,
+  IN     EFI_MAC_ADDRESS           *MacAddress OPTIONAL,
+  IN     MNP_GROUP_CONTROL_BLOCK   *CtrlBlk OPTIONAL
+  )
+{
+  MNP_DEVICE_DATA         *MnpDeviceData;
+  LIST_ENTRY              *Entry;
+  LIST_ENTRY              *NextEntry;
+  MNP_GROUP_ADDRESS       *GroupAddress;
+  EFI_SIMPLE_NETWORK_MODE *SnpMode;
+  MNP_GROUP_CONTROL_BLOCK *NewCtrlBlk;
+  EFI_STATUS              Status;
+  BOOLEAN                 AddressExist;
+  BOOLEAN                 NeedUpdate;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
+  SnpMode       = MnpDeviceData->Snp->Mode;
+
+  if (JoinFlag) {
+    //
+    // A new gropu address is to be added.
+    //
+    GroupAddress  = NULL;
+    AddressExist  = FALSE;
+
+    //
+    // Allocate memory for the control block.
+    //
+    NewCtrlBlk = AllocatePool (sizeof (MNP_GROUP_CONTROL_BLOCK));
+    if (NewCtrlBlk == NULL) {
+      DEBUG ((EFI_D_ERROR, "MnpGroupOp: Failed to allocate memory resource.\n"));
+
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->GroupAddressList) {
+      //
+      // Check whether the MacAddress is already joined by other instances.
+      //
+      GroupAddress = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_ADDRESS, AddrEntry);
+      if (CompareMem (MacAddress, &GroupAddress->Address, SnpMode->HwAddressSize) == 0) {
+        AddressExist = TRUE;
+        break;
+      }
+    }
+
+    if (!AddressExist) {
+      GroupAddress = NULL;
+    }
+
+    //
+    // Add the GroupAddress for this instance.
+    //
+    Status = MnpGroupOpAddCtrlBlk (
+              Instance,
+              NewCtrlBlk,
+              GroupAddress,
+              MacAddress,
+              SnpMode->HwAddressSize
+              );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    NeedUpdate = TRUE;
+  } else {
+    if (MacAddress != NULL) {
+      ASSERT (CtrlBlk != NULL);
+
+      //
+      // Leave the specific multicast mac address.
+      //
+      NeedUpdate = MnpGroupOpDelCtrlBlk (Instance, CtrlBlk);
+    } else {
+      //
+      // Leave all multicast mac addresses.
+      //
+      NeedUpdate = FALSE;
+
+      NET_LIST_FOR_EACH_SAFE (Entry, NextEntry, &Instance->GroupCtrlBlkList) {
+
+        NewCtrlBlk = NET_LIST_USER_STRUCT (
+                      Entry,
+                      MNP_GROUP_CONTROL_BLOCK,
+                      CtrlBlkEntry
+                      );
+        //
+        // Update is required if the group address left is no longer used
+        // by other instances.
+        //
+        NeedUpdate = MnpGroupOpDelCtrlBlk (Instance, NewCtrlBlk);
+      }
+    }
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (NeedUpdate) {
+    //
+    // Reconfigure the receive filters if necessary.
+    //
+    Status = MnpConfigReceiveFilters (MnpDeviceData);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.c
new file mode 100644
index 0000000000..2ddcec8962
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.c
@@ -0,0 +1,690 @@
+/** @file
+  Implementation of driver entry point and driver binding protocol.
+
+Copyright (c) 2005 - 2012, 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<BR>
+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 "MnpDriver.h"
+#include "MnpImpl.h"
+#include "MnpVlan.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gMnpDriverBinding = {
+  MnpDriverBindingSupported,
+  MnpDriverBindingStart,
+  MnpDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDestroyServiceDataEntry (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  MNP_SERVICE_DATA              *MnpServiceData;
+  
+  MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+  return MnpDestroyServiceData (MnpServiceData);
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDestroyServiceChildEntry (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  MNP_SERVICE_DATA              *MnpServiceData;
+
+  MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+  return MnpDestroyServiceChild (MnpServiceData);
+}
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to test.
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific
+                                   child device to start.
+
+  @retval EFI_SUCCESS              This driver supports this device.
+  @retval EFI_ALREADY_STARTED      This driver is already running on this device.
+  @retval Others                   This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL  *Snp;
+
+  //
+  // Test to open the Simple Network protocol BY_DRIVER.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  (VOID **) &Snp,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the openned SNP protocol.
+  //
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiSimpleNetworkProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make drivers as small
+  as possible, there are a few calling restrictions for this service.
+  ConnectController() must follow these calling restrictions. If any other
+  agent wishes to call Start() it must also follow these calling restrictions.
+
+  @param[in]       This                 Protocol instance pointer.
+  @param[in]       ControllerHandle     Handle of device to bind driver to.
+  @param[in]       RemainingDevicePath  Optional parameter use to pick a specific
+                                        child device to start.
+
+  @retval EFI_SUCCESS           This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED   This driver is already running on ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for Mnp Service Data.
+  @retval Others                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS        Status;
+  MNP_SERVICE_DATA  *MnpServiceData;
+  MNP_DEVICE_DATA   *MnpDeviceData;
+  LIST_ENTRY        *Entry;
+  VLAN_TCI          *VlanVariable;
+  UINTN             NumberOfVlan;
+  UINTN             Index;
+
+  VlanVariable = NULL;
+
+  //
+  // Initialize the Mnp Device Data
+  //
+  MnpDeviceData = AllocateZeroPool (sizeof (MNP_DEVICE_DATA));
+  if (MnpDeviceData == NULL) {
+    DEBUG ((EFI_D_ERROR, "MnpDriverBindingStart(): Failed to allocate the Mnp Device Data.\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = MnpInitializeDeviceData (MnpDeviceData, This->DriverBindingHandle, ControllerHandle);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpDriverBindingStart: MnpInitializeDeviceData failed, %r.\n", Status));
+
+    FreePool (MnpDeviceData);
+    return Status;
+  }
+
+  //
+  // Check whether NIC driver has already produced VlanConfig protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    //
+    // NIC hardware already implement VLAN,
+    // no need to provide software VLAN implementation in MNP driver
+    //
+    MnpDeviceData->NumberOfVlan = 0;
+    ZeroMem (&MnpDeviceData->VlanConfig, sizeof (EFI_VLAN_CONFIG_PROTOCOL));
+    MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
+    Status = (MnpServiceData != NULL) ? EFI_SUCCESS : EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Install VLAN Config Protocol
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  &MnpDeviceData->VlanConfig,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Exit;
+  }
+
+  //
+  // Get current VLAN configuration from EFI Variable
+  //
+  NumberOfVlan = 0;
+  Status = MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &VlanVariable);
+  if (EFI_ERROR (Status)) {
+    //
+    // No VLAN is set, create a default MNP service data for untagged frame
+    //
+    MnpDeviceData->NumberOfVlan = 0;
+    MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
+    Status = (MnpServiceData != NULL) ? EFI_SUCCESS : EFI_OUT_OF_RESOURCES;
+    goto Exit;
+  }
+
+  //
+  // Create MNP service data for each VLAN
+  //
+  MnpDeviceData->NumberOfVlan = NumberOfVlan;
+  for (Index = 0; Index < NumberOfVlan; Index++) {
+    MnpServiceData = MnpCreateServiceData (
+                       MnpDeviceData,
+                       VlanVariable[Index].Bits.Vid,
+                       (UINT8) VlanVariable[Index].Bits.Priority
+                       );
+
+    if (MnpServiceData == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+
+      goto Exit;
+    }
+  }
+
+Exit:
+  if (VlanVariable != NULL) {
+    FreePool (VlanVariable);
+  }
+
+  if (EFI_ERROR (Status)) {
+    //
+    // Destroy all MNP service data
+    //
+    while (!IsListEmpty (&MnpDeviceData->ServiceList)) {
+      Entry = GetFirstNode (&MnpDeviceData->ServiceList);
+      MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+      MnpDestroyServiceData (MnpServiceData);
+    }
+
+    //
+    // Uninstall the VLAN Config Protocol if any
+    //
+    if (MnpDeviceData->VlanConfig.Set != NULL) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             MnpDeviceData->ControllerHandle,
+             &gEfiVlanConfigProtocolGuid,
+             &MnpDeviceData->VlanConfig,
+             NULL
+             );
+    }
+
+    //
+    // Destroy Mnp Device Data
+    //
+    MnpDestroyDeviceData (MnpDeviceData, This->DriverBindingHandle);
+    FreePool (MnpDeviceData);
+  }
+
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to make drivers as
+  small as possible, there are a few calling restrictions for this service.
+  DisconnectController() must follow these calling restrictions. If any other
+  agent wishes to call Stop() it must also follow these calling restrictions.
+
+  @param[in]  This               Protocol instance pointer.
+  @param[in]  ControllerHandle   Handle of device to stop driver on.
+  @param[in]  NumberOfChildren   Number of Handles in ChildHandleBuffer. If
+                                 number of children is zero stop the entire
+                                 bus driver.
+  @param[in]  ChildHandleBuffer  List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS            This driver is removed ControllerHandle.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer OPTIONAL
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  EFI_VLAN_CONFIG_PROTOCOL      *VlanConfig;
+  MNP_DEVICE_DATA               *MnpDeviceData;
+  MNP_SERVICE_DATA              *MnpServiceData;
+  LIST_ENTRY                    *List;
+  UINTN                         ListLength;
+
+  //
+  // Try to retrieve MNP service binding protocol from the ControllerHandle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Retrieve VLAN Config Protocol from the ControllerHandle
+    //
+    Status = gBS->OpenProtocol (
+                    ControllerHandle,
+                    &gEfiVlanConfigProtocolGuid,
+                    (VOID **) &VlanConfig,
+                    This->DriverBindingHandle,
+                    ControllerHandle,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "MnpDriverBindingStop: try to stop unknown Controller.\n"));
+      return EFI_DEVICE_ERROR;
+    }
+
+    MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (VlanConfig);
+  } else {
+    MnpServiceData = MNP_SERVICE_DATA_FROM_THIS (ServiceBinding);
+    MnpDeviceData = MnpServiceData->MnpDeviceData;
+  }
+
+  if (NumberOfChildren == 0) {
+    //
+    // Destroy all MNP service data
+    //
+    List = &MnpDeviceData->ServiceList;
+    Status = NetDestroyLinkList (
+               List,
+               MnpDestroyServiceDataEntry,
+               NULL,
+               &ListLength
+               );
+    if (EFI_ERROR (Status) || ListLength !=0) {
+      return EFI_DEVICE_ERROR;
+    }
+
+    //
+    // Uninstall the VLAN Config Protocol if any
+    //
+    if (MnpDeviceData->VlanConfig.Set != NULL) {
+      gBS->UninstallMultipleProtocolInterfaces (
+             MnpDeviceData->ControllerHandle,
+             &gEfiVlanConfigProtocolGuid,
+             &MnpDeviceData->VlanConfig,
+             NULL
+             );
+    }
+
+    //
+    // Destroy Mnp Device Data
+    //
+    MnpDestroyDeviceData (MnpDeviceData, This->DriverBindingHandle);
+    FreePool (MnpDeviceData);
+
+    if (gMnpControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gMnpControllerNameTable);
+      gMnpControllerNameTable = NULL;
+    }
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Stop all MNP child
+  //
+  List = &MnpDeviceData->ServiceList;
+  Status = NetDestroyLinkList (
+             List,
+             MnpDestroyServiceChildEntry,
+             NULL,
+             &ListLength
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Creates a child handle with a set of I/O services.
+
+  @param[in]       This              Protocol instance pointer.
+  @param[in, out]  ChildHandle       Pointer to the handle of the child to create. If
+                                     it is NULL, then a new handle is created. If
+                                     it is not NULL, then the I/O services are added
+                                     to the existing child handle.
+
+  @retval EFI_SUCCES                 The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER      ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources availabe to
+                                     create the child.
+  @retval Others                     The child handle was not created.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpServiceBindingCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL    *This,
+  IN OUT EFI_HANDLE                      *ChildHandle
+  )
+{
+  EFI_STATUS         Status;
+  MNP_SERVICE_DATA   *MnpServiceData;
+  MNP_INSTANCE_DATA  *Instance;
+  VOID               *MnpSb;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MnpServiceData = MNP_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Allocate buffer for the new instance.
+  //
+  Instance = AllocateZeroPool (sizeof (MNP_INSTANCE_DATA));
+  if (Instance == NULL) {
+    DEBUG ((EFI_D_ERROR, "MnpServiceBindingCreateChild: Faild to allocate memory for the new instance.\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Init the instance data.
+  //
+  MnpInitializeInstanceData (MnpServiceData, Instance);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  &Instance->ManagedNetwork,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "MnpServiceBindingCreateChild: Failed to install the MNP protocol, %r.\n",
+      Status)
+      );
+
+    goto ErrorExit;
+  }
+
+  //
+  // Save the instance's childhandle.
+  //
+  Instance->Handle = *ChildHandle;
+
+  Status = gBS->OpenProtocol (
+                  MnpServiceData->ServiceHandle,
+                  &gEfiManagedNetworkServiceBindingProtocolGuid,
+                  (VOID **) &MnpSb,
+                  gMnpDriverBinding.DriverBindingHandle,
+                  Instance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Add the child instance into ChildrenList.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  InsertTailList (&MnpServiceData->ChildrenList, &Instance->InstEntry);
+  MnpServiceData->ChildrenNumber++;
+
+  gBS->RestoreTPL (OldTpl);
+
+ErrorExit:
+
+  if (EFI_ERROR (Status)) {
+
+    if (Instance->Handle != NULL) {
+
+      gBS->UninstallMultipleProtocolInterfaces (
+            Instance->Handle,
+            &gEfiManagedNetworkProtocolGuid,
+            &Instance->ManagedNetwork,
+            NULL
+            );
+    }
+
+    FreePool (Instance);
+  }
+
+  return Status;
+}
+
+
+/**
+  Destroys a child handle with a set of I/O services.
+
+  The DestroyChild() function does the opposite of CreateChild(). It removes a
+  protocol that was installed by CreateChild() from ChildHandle. If the removed
+  protocol is the last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param[in]  This               Pointer to the EFI_SERVICE_BINDING_PROTOCOL
+                                 instance.
+  @param[in]  ChildHandle        Handle of the child to destroy.
+
+  @retval EFI_SUCCES             The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED        ChildHandle does not support the protocol that
+                                 is being removed.
+  @retval EFI_INVALID_PARAMETER  ChildHandle is NULL.
+  @retval EFI_ACCESS_DENIED      The protocol could not be removed from the
+                                 ChildHandle because its services are being
+                                 used.
+  @retval Others                 The child handle was not destroyed.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                      ChildHandle
+  )
+{
+  EFI_STATUS                    Status;
+  MNP_SERVICE_DATA              *MnpServiceData;
+  EFI_MANAGED_NETWORK_PROTOCOL  *ManagedNetwork;
+  MNP_INSTANCE_DATA             *Instance;
+  EFI_TPL                       OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MnpServiceData = MNP_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Try to retrieve ManagedNetwork Protocol from ChildHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  (VOID **) &ManagedNetwork,
+                  gMnpDriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (ManagedNetwork);
+
+  //
+  // MnpServiceBindingDestroyChild may be called twice: first called by
+  // MnpServiceBindingStop, second called by uninstalling the MNP protocol
+  // in this ChildHandle. Use destroyed to make sure the resource clean code
+  // will only excecute once.
+  //
+  if (Instance->Destroyed) {
+    return EFI_SUCCESS;
+  }
+
+  Instance->Destroyed = TRUE;
+
+  //
+  // Close the Simple Network protocol.
+  //
+  gBS->CloseProtocol (
+         MnpServiceData->ServiceHandle,
+         &gEfiManagedNetworkServiceBindingProtocolGuid,
+         MnpServiceData->MnpDeviceData->ImageHandle,
+         ChildHandle
+         );
+
+  //
+  // Uninstall the ManagedNetwork protocol.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiManagedNetworkProtocolGuid,
+                  &Instance->ManagedNetwork,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "MnpServiceBindingDestroyChild: Failed to uninstall the ManagedNetwork protocol, %r.\n",
+      Status)
+      );
+
+    Instance->Destroyed = FALSE;
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Reset the configuration.
+  //
+  ManagedNetwork->Configure (ManagedNetwork, NULL);
+
+  //
+  // Try to flush the RcvdPacketQueue.
+  //
+  MnpFlushRcvdDataQueue (Instance);
+
+  //
+  // Clean the RxTokenMap.
+  //
+  NetMapClean (&Instance->RxTokenMap);
+
+  //
+  // Remove this instance from the ChildrenList.
+  //
+  RemoveEntryList (&Instance->InstEntry);
+  MnpServiceData->ChildrenNumber--;
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (Instance);
+
+  return Status;
+}
+
+/**
+  The entry point for Mnp driver which installs the driver binding and component
+  name protocol on its ImageHandle.
+
+  @param[in]  ImageHandle  The image handle of the driver.
+  @param[in]  SystemTable  The system table.
+
+  @retval EFI_SUCCES       The driver binding and component name protocols are
+                           successfully installed.
+  @retval Others           Other errors as indicated.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverEntryPoint (
+  IN EFI_HANDLE          ImageHandle,
+  IN EFI_SYSTEM_TABLE    *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gMnpDriverBinding,
+           ImageHandle,
+           &gMnpComponentName,
+           &gMnpComponentName2
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.h b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.h
new file mode 100644
index 0000000000..126d968200
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDriver.h
@@ -0,0 +1,275 @@
+/** @file
+  Declaration of strctures and functions for MnpDxe driver.
+
+Copyright (c) 2005 - 2016, 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<BR>
+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.
+
+**/
+
+#ifndef _MNP_DRIVER_H_
+#define _MNP_DRIVER_H_
+
+#include <Uefi.h>
+
+#include <Protocol/ManagedNetwork.h>
+#include <Protocol/SimpleNetwork.h>
+#include <Protocol/ServiceBinding.h>
+#include <Protocol/VlanConfig.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/NetLib.h>
+#include <Library/DpcLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PrintLib.h>
+
+#include "ComponentName.h"
+
+#define MNP_DEVICE_DATA_SIGNATURE  SIGNATURE_32 ('M', 'n', 'p', 'D')
+
+//
+// Global Variables
+//
+extern  EFI_DRIVER_BINDING_PROTOCOL gMnpDriverBinding;
+
+typedef struct {
+  UINT32                        Signature;
+
+  EFI_HANDLE                    ControllerHandle;
+  EFI_HANDLE                    ImageHandle;
+
+  EFI_VLAN_CONFIG_PROTOCOL      VlanConfig;
+  UINTN                         NumberOfVlan;
+  CHAR16                        *MacString;
+  EFI_SIMPLE_NETWORK_PROTOCOL   *Snp;
+
+  //
+  // List of MNP_SERVICE_DATA
+  //
+  LIST_ENTRY                    ServiceList;
+  //
+  // Number of configured MNP Service Binding child
+  //
+  UINTN                         ConfiguredChildrenNumber;
+
+  LIST_ENTRY                    GroupAddressList;
+  UINT32                        GroupAddressCount;
+
+  LIST_ENTRY                    FreeTxBufList;
+  LIST_ENTRY                    AllTxBufList;
+  UINT32                        TxBufCount;
+
+  NET_BUF_QUEUE                 FreeNbufQue;
+  INTN                          NbufCnt;
+
+  EFI_EVENT                     PollTimer;
+  BOOLEAN                       EnableSystemPoll;
+
+  EFI_EVENT                     TimeoutCheckTimer;
+  EFI_EVENT                     MediaDetectTimer;
+
+  UINT32                        UnicastCount;
+  UINT32                        BroadcastCount;
+  UINT32                        MulticastCount;
+  UINT32                        PromiscuousCount;
+
+  //
+  // The size of the data buffer in the MNP_PACKET_BUFFER used to
+  // store a packet.
+  //
+  UINT32                        BufferLength;
+  UINT32                        PaddingSize;
+  NET_BUF                       *RxNbufCache;
+} MNP_DEVICE_DATA;
+
+#define MNP_DEVICE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  MNP_DEVICE_DATA, \
+  VlanConfig, \
+  MNP_DEVICE_DATA_SIGNATURE \
+  )
+
+#define MNP_SERVICE_DATA_SIGNATURE  SIGNATURE_32 ('M', 'n', 'p', 'S')
+
+typedef struct {
+  UINT32                        Signature;
+
+  LIST_ENTRY                    Link;
+
+  MNP_DEVICE_DATA               *MnpDeviceData;
+  EFI_HANDLE                    ServiceHandle;
+  EFI_SERVICE_BINDING_PROTOCOL  ServiceBinding;
+  EFI_DEVICE_PATH_PROTOCOL      *DevicePath;
+
+  LIST_ENTRY                    ChildrenList;
+  UINTN                         ChildrenNumber;
+
+  UINT32                        Mtu;
+
+  UINT16                        VlanId;
+  UINT8                         Priority;
+} MNP_SERVICE_DATA;
+
+
+#define MNP_SERVICE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  MNP_SERVICE_DATA, \
+  ServiceBinding, \
+  MNP_SERVICE_DATA_SIGNATURE \
+  )
+
+#define MNP_SERVICE_DATA_FROM_LINK(a) \
+  CR ( \
+  (a), \
+  MNP_SERVICE_DATA, \
+  Link, \
+  MNP_SERVICE_DATA_SIGNATURE \
+  )
+
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to test.
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific
+                                   child device to start.
+
+  @retval EFI_SUCCESS              This driver supports this device.
+  @retval EFI_ALREADY_STARTED      This driver is already running on this device.
+  @retval Others                   This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make drivers as small
+  as possible, there are a few calling restrictions for this service.
+  ConnectController() must follow these calling restrictions. If any other
+  agent wishes to call Start() it must also follow these calling restrictions.
+
+  @param[in]       This                 Protocol instance pointer.
+  @param[in]       ControllerHandle     Handle of device to bind driver to.
+  @param[in]       RemainingDevicePath  Optional parameter use to pick a specific
+                                        child device to start.
+
+  @retval EFI_SUCCESS           This driver is added to ControllerHandle.
+  @retval EFI_ALREADY_STARTED   This driver is already running on ControllerHandle.
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for Mnp Service Data.
+  @retval Others                This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  );
+
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to make drivers as
+  small as possible, there are a few calling restrictions for this service.
+  DisconnectController() must follow these calling restrictions. If any other
+  agent wishes to call Stop() it must also follow these calling restrictions.
+
+  @param[in]  This               Protocol instance pointer.
+  @param[in]  ControllerHandle   Handle of device to stop driver on.
+  @param[in]  NumberOfChildren   Number of Handles in ChildHandleBuffer. If
+                                 number of children is zero stop the entire
+                                 bus driver.
+  @param[in]  ChildHandleBuffer  List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS            This driver is removed ControllerHandle.
+  @retval EFI_DEVICE_ERROR       The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer OPTIONAL
+  );
+
+/**
+  Creates a child handle with a set of I/O services.
+
+  @param[in]       This              Protocol instance pointer.
+  @param[in, out]  ChildHandle       Pointer to the handle of the child to create. If
+                                     it is NULL, then a new handle is created. If
+                                     it is not NULL, then the I/O services are added
+                                     to the existing child handle.
+
+  @retval EFI_SUCCES                 The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER      ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES       There are not enough resources availabe to
+                                     create the child.
+  @retval Others                     The child handle was not created.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpServiceBindingCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL    *This,
+  IN OUT EFI_HANDLE                      *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a set of I/O services.
+
+  The DestroyChild() function does the opposite of CreateChild(). It removes a
+  protocol that was installed by CreateChild() from ChildHandle. If the removed
+  protocol is the last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param[in]  This               Pointer to the EFI_SERVICE_BINDING_PROTOCOL
+                                 instance.
+  @param[in]  ChildHandle        Handle of the child to destroy.
+
+  @retval EFI_SUCCES             The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED        ChildHandle does not support the protocol that
+                                 is being removed.
+  @retval EFI_INVALID_PARAMETER  ChildHandle is NULL.
+  @retval EFI_ACCESS_DENIED      The protocol could not be removed from the
+                                 ChildHandle because its services are being
+                                 used.
+  @retval Others                 The child handle was not destroyed.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                      ChildHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.inf b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.inf
new file mode 100644
index 0000000000..07e62f5ba2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.inf
@@ -0,0 +1,74 @@
+## @file
+#  This module produces EFI MNP Protocol, EFI MNP Servie Binding Protocol and EFI VLAN Protocol.
+#
+#  This module produces EFI Managed Network Protocol upon EFI Simple Network Protocol,
+#  to provide raw asynchronous network I/O services. It also produces EFI VLAN Protocol
+#  to provide manageability interface for VLAN configuration. 
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = MnpDxe
+  MODULE_UNI_FILE                = MnpDxe.uni
+  FILE_GUID                      = 025BBFC7-E6A9-4b8b-82AD-6815A1AEAF4A
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = MnpDriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gMnpDriverBinding
+#  COMPONENT_NAME                =  gMnpComponentName
+#  COMPONENT_NAME2               =  gMnpComponentName2
+#
+
+[Sources]
+  MnpMain.c
+  MnpIo.c
+  ComponentName.h
+  MnpDriver.h
+  ComponentName.c
+  MnpDriver.c
+  MnpConfig.c
+  MnpImpl.h
+  MnpVlan.h
+  MnpVlan.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  NetLib
+  DpcLib
+
+[Protocols]
+  gEfiManagedNetworkServiceBindingProtocolGuid  ## BY_START
+  gEfiSimpleNetworkProtocolGuid                 ## TO_START
+  gEfiManagedNetworkProtocolGuid                ## BY_START
+  ## BY_START
+  ## UNDEFINED # variable
+  gEfiVlanConfigProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  MnpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.uni b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.uni
new file mode 100644
index 0000000000..a4f5eef7cf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxeExtra.uni
new file mode 100644
index 0000000000..ab0cf08400
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpImpl.h b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpImpl.h
new file mode 100644
index 0000000000..c66be6487b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpImpl.h
@@ -0,0 +1,905 @@
+/** @file
+  Declaration of structures and functions of MnpDxe driver.
+
+Copyright (c) 2005 - 2016, 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<BR>
+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.
+
+**/
+
+#ifndef _MNP_IMPL_H_
+#define _MNP_IMPL_H_
+
+#include "MnpDriver.h"
+
+#define NET_ETHER_FCS_SIZE            4
+
+#define MNP_SYS_POLL_INTERVAL         (10 * TICKS_PER_MS)   // 10 milliseconds
+#define MNP_TIMEOUT_CHECK_INTERVAL    (50 * TICKS_PER_MS)   // 50 milliseconds
+#define MNP_MEDIA_DETECT_INTERVAL     (500 * TICKS_PER_MS)  // 500 milliseconds
+#define MNP_TX_TIMEOUT_TIME           (500 * TICKS_PER_MS)  // 500 milliseconds
+#define MNP_INIT_NET_BUFFER_NUM       512
+#define MNP_NET_BUFFER_INCREASEMENT   64
+#define MNP_MAX_NET_BUFFER_NUM        65536
+#define MNP_TX_BUFFER_INCREASEMENT    64
+#define MNP_MAX_TX_BUFFER_NUM         65536
+
+#define MNP_MAX_RCVD_PACKET_QUE_SIZE  256
+
+#define MNP_RECEIVE_UNICAST           0x01
+#define MNP_RECEIVE_BROADCAST         0x02
+
+#define UNICAST_PACKET                MNP_RECEIVE_UNICAST
+#define BROADCAST_PACKET              MNP_RECEIVE_BROADCAST
+
+#define MNP_INSTANCE_DATA_SIGNATURE   SIGNATURE_32 ('M', 'n', 'p', 'I')
+
+#define MNP_INSTANCE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  MNP_INSTANCE_DATA, \
+  ManagedNetwork, \
+  MNP_INSTANCE_DATA_SIGNATURE \
+  )
+
+typedef struct {
+  UINT32                          Signature;
+
+  MNP_SERVICE_DATA                *MnpServiceData;
+
+  EFI_HANDLE                      Handle;
+
+  LIST_ENTRY                      InstEntry;
+
+  EFI_MANAGED_NETWORK_PROTOCOL    ManagedNetwork;
+
+  BOOLEAN                         Configured;
+  BOOLEAN                         Destroyed;
+
+  LIST_ENTRY                      GroupCtrlBlkList;
+
+  NET_MAP                         RxTokenMap;
+
+  LIST_ENTRY                      RxDeliveredPacketQueue;
+  LIST_ENTRY                      RcvdPacketQueue;
+  UINTN                           RcvdPacketQueueSize;
+
+  EFI_MANAGED_NETWORK_CONFIG_DATA ConfigData;
+
+  UINT8                           ReceiveFilter;
+} MNP_INSTANCE_DATA;
+
+typedef struct {
+  LIST_ENTRY      AddrEntry;
+  EFI_MAC_ADDRESS Address;
+  INTN            RefCnt;
+} MNP_GROUP_ADDRESS;
+
+typedef struct {
+  LIST_ENTRY        CtrlBlkEntry;
+  MNP_GROUP_ADDRESS *GroupAddress;
+} MNP_GROUP_CONTROL_BLOCK;
+
+typedef struct {
+  LIST_ENTRY                        WrapEntry;
+  MNP_INSTANCE_DATA                 *Instance;
+  EFI_MANAGED_NETWORK_RECEIVE_DATA  RxData;
+  NET_BUF                           *Nbuf;
+  UINT64                            TimeoutTick;
+} MNP_RXDATA_WRAP;
+
+#define MNP_TX_BUF_WRAP_SIGNATURE   SIGNATURE_32 ('M', 'T', 'B', 'W')
+
+typedef struct {
+  UINT32                  Signature;
+  LIST_ENTRY              WrapEntry;  // Link to FreeTxBufList
+  LIST_ENTRY              AllEntry;   // Link to AllTxBufList
+  BOOLEAN                 InUse;
+  UINT8                   TxBuf[1];
+} MNP_TX_BUF_WRAP;
+
+/**
+  Initialize the mnp device context data.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       ImageHandle        The driver image handle.
+  @param[in]       ControllerHandle   Handle of device to bind driver to.
+
+  @retval EFI_SUCCESS           The mnp service context is initialized.
+  @retval EFI_UNSUPPORTED       ControllerHandle does not support Simple Network Protocol.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpInitializeDeviceData (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     EFI_HANDLE        ImageHandle,
+  IN     EFI_HANDLE        ControllerHandle
+  );
+
+/**
+  Destroy the MNP device context data.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       ImageHandle        The driver image handle.
+
+**/
+VOID
+MnpDestroyDeviceData (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN     EFI_HANDLE        ImageHandle
+  );
+
+/**
+  Create mnp service context data.
+
+  @param[in]       MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]       VlanId             The VLAN ID.
+  @param[in]       Priority           The VLAN priority. If VlanId is 0,
+                                      Priority is ignored.
+
+  @return A pointer to MNP_SERVICE_DATA or NULL if failed to create MNP service context.
+
+**/
+MNP_SERVICE_DATA *
+MnpCreateServiceData (
+  IN MNP_DEVICE_DATA     *MnpDeviceData,
+  IN UINT16              VlanId,
+  IN UINT8                Priority OPTIONAL
+  );
+
+/**
+  Initialize the mnp service context data.
+
+  @param[in, out]  MnpServiceData     Pointer to the mnp service context data.
+  @param[in]       ImageHandle        The driver image handle.
+  @param[in]       ControllerHandle   Handle of device to bind driver to.
+
+  @retval EFI_SUCCESS           The mnp service context is initialized.
+  @retval EFI_UNSUPPORTED       ControllerHandle does not support Simple Network Protocol.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpInitializeServiceData (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData,
+  IN     EFI_HANDLE          ImageHandle,
+  IN     EFI_HANDLE          ControllerHandle
+  );
+
+/**
+  Destroy the MNP service context data.
+
+  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.
+
+  @retval EFI_SUCCESS           The mnp service context is destroyed.
+  @retval Others                Errors as indicated.
+
+**/
+EFI_STATUS
+MnpDestroyServiceData (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData
+  );
+
+/**
+  Destroy all child of the MNP service data.
+
+  @param[in, out]  MnpServiceData    Pointer to the mnp service context data.
+
+  @retval EFI_SUCCESS           All child are destroyed.
+  @retval Others                Failed to destroy all child.
+
+**/
+EFI_STATUS
+MnpDestroyServiceChild (
+  IN OUT MNP_SERVICE_DATA    *MnpServiceData
+  );
+
+/**
+  Find the MNP Service Data for given VLAN ID.
+
+  @param[in]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in]  VlanId             The VLAN ID.
+
+  @return A pointer to MNP_SERVICE_DATA or NULL if not found.
+
+**/
+MNP_SERVICE_DATA *
+MnpFindServiceData (
+  IN MNP_DEVICE_DATA     *MnpDeviceData,
+  IN UINT16              VlanId
+  );
+
+/**
+  Initialize the mnp instance context data.
+
+  @param[in]       MnpServiceData   Pointer to the mnp service context data.
+  @param[in, out]  Instance         Pointer to the mnp instance context data
+                                    to initialize.
+
+**/
+VOID
+MnpInitializeInstanceData (
+  IN     MNP_SERVICE_DATA    *MnpServiceData,
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  );
+
+/**
+  Check whether the token specified by Arg matches the token in Item.
+
+  @param[in]  Map               Pointer to the NET_MAP.
+  @param[in]  Item              Pointer to the NET_MAP_ITEM.
+  @param[in]  Arg               Pointer to the Arg, it's a pointer to the token to
+                                check.
+
+  @retval EFI_SUCCESS           The token specified by Arg is different from the
+                                token in Item.
+  @retval EFI_ACCESS_DENIED     The token specified by Arg is the same as that in
+                                Item.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpTokenExist (
+  IN NET_MAP         *Map,
+  IN NET_MAP_ITEM    *Item,
+  IN VOID            *Arg
+  );
+
+/**
+  Cancel the token specified by Arg if it matches the token in Item.
+
+  @param[in, out]  Map               Pointer to the NET_MAP.
+  @param[in, out]  Item              Pointer to the NET_MAP_ITEM.
+  @param[in]       Arg               Pointer to the Arg, it's a pointer to the
+                                     token to cancel.
+
+  @retval EFI_SUCCESS       The Arg is NULL, and the token in Item is cancelled,
+                            or the Arg isn't NULL, and the token in Item is
+                            different from the Arg.
+  @retval EFI_ABORTED       The Arg isn't NULL, the token in Item mathces the
+                            Arg, and the token is cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpCancelTokens (
+  IN OUT NET_MAP         *Map,
+  IN OUT NET_MAP_ITEM    *Item,
+  IN     VOID            *Arg
+  );
+
+/**
+  Flush the instance's received data.
+
+  @param[in, out]  Instance              Pointer to the mnp instance context data.
+
+**/
+VOID
+MnpFlushRcvdDataQueue (
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  );
+
+/**
+  Configure the Instance using ConfigData.
+
+  @param[in, out]  Instance     Pointer to the mnp instance context data.
+  @param[in]       ConfigData   Pointer to the configuration data used to configure
+                                the isntance.
+
+  @retval EFI_SUCCESS           The Instance is configured.
+  @retval EFI_UNSUPPORTED       EnableReceiveTimestamps is on and the
+                                implementation doesn't support it.
+  @retval Others                Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpConfigureInstance (
+  IN OUT MNP_INSTANCE_DATA                 *Instance,
+  IN     EFI_MANAGED_NETWORK_CONFIG_DATA   *ConfigData OPTIONAL
+  );
+
+/**
+  Do the group operations for this instance.
+
+  @param[in, out]  Instance        Pointer to the instance context data.
+  @param[in]       JoinFlag        Set to TRUE to join a group. Set to TRUE to
+                                   leave a group/groups.
+  @param[in]       MacAddress      Pointer to the group address to join or leave.
+  @param[in]       CtrlBlk         Pointer to the group control block if JoinFlag
+                                   is FALSE.
+
+  @retval EFI_SUCCESS              The group operation finished.
+  @retval EFI_OUT_OF_RESOURCES     Failed due to lack of memory resources.
+  @retval Others                   Other errors as indicated.
+
+**/
+EFI_STATUS
+MnpGroupOp (
+  IN OUT MNP_INSTANCE_DATA         *Instance,
+  IN     BOOLEAN                   JoinFlag,
+  IN     EFI_MAC_ADDRESS           *MacAddress OPTIONAL,
+  IN     MNP_GROUP_CONTROL_BLOCK   *CtrlBlk OPTIONAL
+  );
+
+/**
+  Validates the Mnp transmit token.
+
+  @param[in]  Instance            Pointer to the Mnp instance context data.
+  @param[in]  Token               Pointer to the transmit token to check.
+
+  @return The Token is valid or not.
+
+**/
+BOOLEAN
+MnpIsValidTxToken (
+  IN MNP_INSTANCE_DATA                       *Instance,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  );
+
+/**
+  Build the packet to transmit from the TxData passed in.
+
+  @param[in]   MnpServiceData      Pointer to the mnp service context data.
+  @param[in]   TxData              Pointer to the transmit data containing the information
+                                   to build the packet.
+  @param[out]  PktBuf              Pointer to record the address of the packet.
+  @param[out]  PktLen              Pointer to a UINT32 variable used to record the packet's
+                                   length.
+
+  @retval EFI_SUCCESS           TxPackage is built.
+  @retval EFI_OUT_OF_RESOURCES  The deliver fails due to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpBuildTxPacket (
+  IN     MNP_SERVICE_DATA                    *MnpServiceData,
+  IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
+     OUT UINT8                               **PktBuf,
+     OUT UINT32                              *PktLen
+  );
+
+/**
+  Synchronously send out the packet. 
+
+  This functon places the packet buffer to SNP driver's tansmit queue. The packet
+  can be considered successfully sent out once SNP acccetp the packet, while the
+  packet buffer recycle is deferred for better performance.
+
+  @param[in]       MnpServiceData      Pointer to the mnp service context data.
+  @param[in]       Packet              Pointer to the pakcet buffer.
+  @param[in]       Length              The length of the packet.
+  @param[in, out]  Token               Pointer to the token the packet generated from.
+
+  @retval EFI_SUCCESS                  The packet is sent out.
+  @retval EFI_TIMEOUT                  Time out occurs, the packet isn't sent.
+  @retval EFI_DEVICE_ERROR             An unexpected network error occurs.
+
+**/
+EFI_STATUS
+MnpSyncSendPacket (
+  IN     MNP_SERVICE_DATA                        *MnpServiceData,
+  IN     UINT8                                   *Packet,
+  IN     UINT32                                  Length,
+  IN OUT EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  );
+
+/**
+  Try to deliver the received packet to the instance.
+
+  @param[in, out]  Instance     Pointer to the mnp instance context data.
+
+  @retval EFI_SUCCESS           The received packet is delivered, or there is no
+                                packet to deliver, or there is no available receive
+                                token.
+  @retval EFI_OUT_OF_RESOURCES  The deliver fails due to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpInstanceDeliverPacket (
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  );
+
+/**
+  Recycle the RxData and other resources used to hold and deliver the received
+  packet.
+
+  @param[in]  Event               The event this notify function registered to.
+  @param[in]  Context             Pointer to the context data registerd to the Event.
+
+**/
+VOID
+EFIAPI
+MnpRecycleRxData (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  );
+
+/**
+  Try to receive a packet and deliver it.
+
+  @param[in, out]  MnpDeviceData        Pointer to the mnp device context data.
+
+  @retval EFI_SUCCESS           add return value to function comment
+  @retval EFI_NOT_STARTED       The simple network protocol is not started.
+  @retval EFI_NOT_READY         No packet received.
+  @retval EFI_DEVICE_ERROR      An unexpected error occurs.
+
+**/
+EFI_STATUS
+MnpReceivePacket (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  );
+
+/**
+  Allocate a free NET_BUF from MnpDeviceData->FreeNbufQue. If there is none
+  in the queue, first try to allocate some and add them into the queue, then
+  fetch the NET_BUF from the updated FreeNbufQue.
+
+  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.
+
+  @return     Pointer to the allocated free NET_BUF structure, if NULL the
+              operation is failed.
+
+**/
+NET_BUF *
+MnpAllocNbuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  );
+
+/**
+  Try to reclaim the Nbuf into the buffer pool.
+
+  @param[in, out]  MnpDeviceData         Pointer to the mnp device context data.
+  @param[in, out]  Nbuf                  Pointer to the NET_BUF to free.
+
+**/
+VOID
+MnpFreeNbuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN OUT NET_BUF           *Nbuf
+  );
+
+/**
+  Allocate a free TX buffer from MnpDeviceData->FreeTxBufList. If there is none
+  in the queue, first try to recycle some from SNP, then try to allocate some and add 
+  them into the queue, then fetch the NET_BUF from the updated FreeTxBufList.
+
+  @param[in, out]  MnpDeviceData        Pointer to the MNP_DEVICE_DATA.
+
+  @return     Pointer to the allocated free NET_BUF structure, if NULL the
+              operation is failed.
+
+**/
+UINT8 *
+MnpAllocTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  );
+
+/**
+  Try to recycle all the transmitted buffer address from SNP.
+
+  @param[in, out]  MnpDeviceData     Pointer to the mnp device context data.
+
+  @retval EFI_SUCCESS             Successed to recyclethe transmitted buffer address.
+  @retval Others                  Failed to recyclethe transmitted buffer address.
+
+**/
+EFI_STATUS
+MnpRecycleTxBuf (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  );
+
+/**
+  Remove the received packets if timeout occurs.
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpCheckPacketTimeout (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  );
+
+/**
+  Poll to update MediaPresent field in SNP ModeData by Snp.GetStatus().
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpCheckMediaStatus (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  );
+
+/**
+  Poll to receive the packets from Snp. This function is either called by upperlayer
+  protocols/applications or the system poll timer notify mechanism.
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpSystemPoll (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  );
+
+/**
+  Returns the operational parameters for the current MNP child driver. May also
+  support returning the underlying SNP driver mode data.
+
+  The GetModeData() function is used to read the current mode data (operational
+  parameters) from the MNP or the underlying SNP.
+
+  @param[in]  This          Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[out] MnpConfigData Pointer to storage for MNP operational parameters. Type
+                            EFI_MANAGED_NETWORK_CONFIG_DATA is defined in "Related
+                            Definitions" below.
+  @param[out] SnpModeData   Pointer to storage for SNP operational parameters. This
+                            feature may be unsupported. Type EFI_SIMPLE_NETWORK_MODE
+                            is defined in the EFI_SIMPLE_NETWORK_PROTOCOL.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this
+                                MNP implementation.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured. The default values are returned in
+                                MnpConfigData if it is not NULL.
+  @retval Others                The mode data could not be read.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpGetModeData (
+  IN     EFI_MANAGED_NETWORK_PROTOCOL      *This,
+     OUT EFI_MANAGED_NETWORK_CONFIG_DATA   *MnpConfigData OPTIONAL,
+     OUT EFI_SIMPLE_NETWORK_MODE           *SnpModeData OPTIONAL
+  );
+
+/**
+  Sets or clears the operational parameters for the MNP child driver.
+
+  The Configure() function is used to set, change, or reset the operational
+  parameters for the MNP child driver instance. Until the operational parameters
+  have been set, no network traffic can be sent or received by this MNP child
+  driver instance. Once the operational parameters have been reset, no more
+  traffic can be sent or received until the operational parameters have been set
+  again.
+  Each MNP child driver instance can be started and stopped independently of
+  each other by setting or resetting their receive filter settings with the
+  Configure() function.
+  After any successful call to Configure(), the MNP child driver instance is
+  started. The internal periodic timer (if supported) is enabled. Data can be
+  transmitted and may be received if the receive filters have also been enabled.
+  Note: If multiple MNP child driver instances will receive the same packet
+  because of overlapping receive filter settings, then the first MNP child
+  driver instance will receive the original packet and additional instances will
+  receive copies of the original packet.
+  Note: Warning: Receive filter settings that overlap will consume extra
+  processor and/or DMA resources and degrade system and network performance.
+
+  @param[in]  This           Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  MnpConfigData  Pointer to configuration data that will be assigned
+                             to the MNP child driver instance. If NULL, the MNP
+                             child driver instance is reset to startup defaults
+                             and all pending transmit and receive requests are
+                             flushed. Type EFI_MANAGED_NETWORK_CONFIG_DATA is
+                             defined in EFI_MANAGED_NETWORK_PROTOCOL.GetModeData().
+
+  @retval EFI_SUCCESS            The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * MnpConfigData.ProtocolTypeFilter is not
+                                   valid.
+                                 The operational data for the MNP child driver
+                                 instance is unchanged.
+  @retval EFI_OUT_OF_RESOURCES   Required system resources (usually memory)
+                                 could not be allocated.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_UNSUPPORTED        The requested feature is unsupported in
+                                 this [MNP] implementation. The operational data
+                                 for the MNP child driver instance is unchanged.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error
+                                 occurred. The MNP child driver instance has
+                                 been reset to startup defaults.
+  @retval Others                 The MNP child driver instance has been reset to
+                                 startup defaults.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpConfigure (
+  IN EFI_MANAGED_NETWORK_PROTOCOL        *This,
+  IN EFI_MANAGED_NETWORK_CONFIG_DATA     *MnpConfigData OPTIONAL
+  );
+
+/**
+  Translates an IP multicast address to a hardware (MAC) multicast address. This
+  function may be unsupported in some MNP implementations.
+
+  The McastIpToMac() function translates an IP multicast address to a hardware
+  (MAC) multicast address. This function may be implemented by calling the
+  underlying EFI_SIMPLE_NETWORK. MCastIpToMac() function, which may also be
+  unsupported in some MNP implementations.
+
+  @param[in]  This        Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Ipv6Flag    Set to TRUE to if IpAddress is an IPv6 multicast address.
+                          Set to FALSE if IpAddress is an IPv4 multicast address.
+  @param[in]  IpAddress   Pointer to the multicast IP address (in network byte
+                          order) to convert.
+  @param[out] MacAddress  Pointer to the resulting multicast MAC address.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One of the following conditions is TRUE:
+                                 * This is NULL.
+                                 * IpAddress is NULL.
+                                 * IpAddress is not a valid multicast IP
+                                   address.
+                                 * MacAddress is NULL.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this
+                                MNP implementation.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred.
+  @retval Others                The address could not be converted.
+**/
+EFI_STATUS
+EFIAPI
+MnpMcastIpToMac (
+  IN     EFI_MANAGED_NETWORK_PROTOCOL    *This,
+  IN     BOOLEAN                         Ipv6Flag,
+  IN     EFI_IP_ADDRESS                  *IpAddress,
+     OUT EFI_MAC_ADDRESS                 *MacAddress
+  );
+
+/**
+  Enables and disables receive filters for multicast address. This function may
+  be unsupported in some MNP implementations.
+
+  The Groups() function only adds and removes multicast MAC addresses from the
+  filter list. The MNP driver does not transmit or process Internet Group
+  Management Protocol (IGMP) packets. If JoinFlag is FALSE and MacAddress is
+  NULL, then all joined groups are left.
+
+  @param[in]  This        Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  JoinFlag    Set to TRUE to join this multicast group.
+                          Set to FALSE to leave this multicast group.
+  @param[in]  MacAddress  Pointer to the multicast MAC group (address) to join or
+                          leave.
+
+  @retval EFI_SUCCESS           The requested operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * This is NULL.
+                                * JoinFlag is TRUE and MacAddress is NULL.
+                                * MacAddress is not a valid multicast MAC
+                                  address.
+                                * The MNP multicast group settings are
+                                  unchanged.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured.
+  @retval EFI_ALREADY_STARTED   The supplied multicast group is already joined.
+  @retval EFI_NOT_FOUND         The supplied multicast group is not joined.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred.
+                                The MNP child driver instance has been reset to
+                                startup defaults.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this MNP
+                                implementation.
+  @retval Others                The requested operation could not be completed.
+                                The MNP multicast group settings are unchanged.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpGroups (
+  IN EFI_MANAGED_NETWORK_PROTOCOL    *This,
+  IN BOOLEAN                         JoinFlag,
+  IN EFI_MAC_ADDRESS                 *MacAddress OPTIONAL
+  );
+
+/**
+  Places asynchronous outgoing data packets into the transmit queue.
+
+  The Transmit() function places a completion token into the transmit packet
+  queue. This function is always asynchronous.
+  The caller must fill in the Token.Event and Token.TxData fields in the
+  completion token, and these fields cannot be NULL. When the transmit operation
+  completes, the MNP updates the Token.Status field and the Token.Event is
+  signaled.
+  Note: There may be a performance penalty if the packet needs to be
+  defragmented before it can be transmitted by the network device. Systems in
+  which performance is critical should review the requirements and features of
+  the underlying communications device and drivers.
+
+
+  @param[in]  This    Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token   Pointer to a token associated with the transmit data
+                      descriptor. Type EFI_MANAGED_NETWORK_COMPLETION_TOKEN
+                      is defined in "Related Definitions" below.
+
+  @retval EFI_SUCCESS            The transmit completion token was cached.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * Token is NULL.
+                                 * Token.Event is NULL.
+                                 * Token.TxData is NULL.
+                                 * Token.TxData.DestinationAddress is not
+                                   NULL and Token.TxData.HeaderLength is zero.
+                                 * Token.TxData.FragmentCount is zero.
+                                 * (Token.TxData.HeaderLength +
+                                   Token.TxData.DataLength) is not equal to the
+                                   sum of the
+                                   Token.TxData.FragmentTable[].FragmentLength
+                                   fields.
+                                 * One or more of the
+                                   Token.TxData.FragmentTable[].FragmentLength
+                                   fields is zero.
+                                 * One or more of the
+                                   Token.TxData.FragmentTable[].FragmentBufferfields
+                                   is NULL.
+                                 * Token.TxData.DataLength is greater than MTU.
+  @retval EFI_ACCESS_DENIED      The transmit completion token is already in the
+                                 transmit queue.
+  @retval EFI_OUT_OF_RESOURCES   The transmit data could not be queued due to a
+                                 lack of system resources (usually memory).
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_NOT_READY          The transmit request could not be queued because
+                                 the transmit queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpTransmit (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  );
+
+/**
+  Aborts an asynchronous transmit or receive request.
+
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token.Status will be set to EFI_ABORTED and then Token.Event will be
+  signaled. If the token is not in one of the queues, which usually means that
+  the asynchronous operation has completed, this function will not signal the
+  token and EFI_NOT_FOUND is returned.
+
+  @param[in]  This     Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token    Pointer to a token that has been issued by
+                       EFI_MANAGED_NETWORK_PROTOCOL.Transmit() or
+                       EFI_MANAGED_NETWORK_PROTOCOL.Receive(). If NULL, all
+                       pending tokens are aborted.
+
+  @retval EFI_SUCCESS            The asynchronous I/O request was aborted and
+                                 Token.Event was signaled. When Token is NULL,
+                                 all pending requests were aborted and their
+                                 events were signaled.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          When Token is not NULL, the asynchronous I/O
+                                 request was not found in the transmit or
+                                 receive queue. It has either completed or was
+                                 not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+MnpCancel (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token OPTIONAL
+  );
+
+/**
+  Places an asynchronous receiving request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet
+  queue. This function is always asynchronous.
+  The caller must fill in the Token.Event field in the completion token, and
+  this field cannot be NULL. When the receive operation completes, the MNP
+  updates the Token.Status and Token.RxData fields and the Token.Event is
+  signaled.
+
+  @param[in]  This      Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token     Pointer to a token associated with the receive
+                        data descriptor. Type
+                        EFI_MANAGED_NETWORK_COMPLETION_TOKEN is defined in
+                        EFI_MANAGED_NETWORK_PROTOCOL.Transmit().
+
+  @retval EFI_SUCCESS            The receive completion token was cached.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * Token is NULL.
+                                 * Token.Event is NULL
+  @retval EFI_OUT_OF_RESOURCES   The transmit data could not be queued due to a
+                                 lack of system resources (usually memory).
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_ACCESS_DENIED      The receive completion token was already in the
+                                 receive queue.
+  @retval EFI_NOT_READY          The receive request could not be queued because
+                                 the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpReceive (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  );
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+
+  The Poll() function can be used by network drivers and applications to
+  increase the rate that data packets are moved between the communications
+  device and the transmit and receive queues.
+  Normally, a periodic timer event internally calls the Poll() function. But, in
+  some systems, the periodic timer event may not call Poll() fast enough to
+  transmit and/or receive all data packets without missing packets. Drivers and
+  applications that are experiencing packet loss should try calling the Poll()
+  function more often.
+
+  @param[in]  This         Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      Incoming or outgoing data was processed.
+  @retval EFI_NOT_STARTED  This MNP child driver instance has not been
+                           configured.
+  @retval EFI_DEVICE_ERROR An unexpected system or network error occurred. The
+                           MNP child driver instance has been reset to startup
+                           defaults.
+  @retval EFI_NOT_READY    No incoming or outgoing data was processed. Consider
+                           increasing the polling rate.
+  @retval EFI_TIMEOUT      Data was dropped out of the transmit and/or receive
+                           queue. Consider increasing the polling rate.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpPoll (
+  IN EFI_MANAGED_NETWORK_PROTOCOL    *This
+  );
+
+/**
+  Configure the Snp receive filters according to the instances' receive filter
+  settings.
+
+  @param[in]  MnpDeviceData         Pointer to the mnp device context data.
+
+  @retval     EFI_SUCCESS           The receive filters is configured.
+  @retval     EFI_OUT_OF_RESOURCES  The receive filters can't be configured due
+                                    to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpConfigReceiveFilters (
+  IN MNP_DEVICE_DATA     *MnpDeviceData
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpIo.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpIo.c
new file mode 100644
index 0000000000..1cbfc30e5c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpIo.c
@@ -0,0 +1,1140 @@
+/** @file
+  Implementation of Managed Network Protocol I/O functions.
+
+Copyright (c) 2005 - 2016, 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<BR>
+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 "MnpImpl.h"
+#include "MnpVlan.h"
+
+/**
+  Validates the Mnp transmit token.
+
+  @param[in]  Instance            Pointer to the Mnp instance context data.
+  @param[in]  Token               Pointer to the transmit token to check.
+
+  @return The Token is valid or not.
+
+**/
+BOOLEAN
+MnpIsValidTxToken (
+  IN MNP_INSTANCE_DATA                       *Instance,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  )
+{
+  MNP_SERVICE_DATA                  *MnpServiceData;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA *TxData;
+  UINT32                            Index;
+  UINT32                            TotalLength;
+  EFI_MANAGED_NETWORK_FRAGMENT_DATA *FragmentTable;
+
+  MnpServiceData = Instance->MnpServiceData;
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+
+  TxData = Token->Packet.TxData;
+
+  if ((Token->Event == NULL) || (TxData == NULL) || (TxData->FragmentCount == 0)) {
+    //
+    // The token is invalid if the Event is NULL, or the TxData is NULL, or
+    // the fragment count is zero.
+    //
+    DEBUG ((EFI_D_WARN, "MnpIsValidTxToken: Invalid Token.\n"));
+    return FALSE;
+  }
+
+  if ((TxData->DestinationAddress != NULL) && (TxData->HeaderLength != 0)) {
+    //
+    // The token is invalid if the HeaderLength isn't zero while the DestinationAddress
+    // is NULL (The destination address is already put into the packet).
+    //
+    DEBUG ((EFI_D_WARN, "MnpIsValidTxToken: DestinationAddress isn't NULL, HeaderLength must be 0.\n"));
+    return FALSE;
+  }
+
+  TotalLength   = 0;
+  FragmentTable = TxData->FragmentTable;
+  for (Index = 0; Index < TxData->FragmentCount; Index++) {
+
+    if ((FragmentTable[Index].FragmentLength == 0) || (FragmentTable[Index].FragmentBuffer == NULL)) {
+      //
+      // The token is invalid if any FragmentLength is zero or any FragmentBuffer is NULL.
+      //
+      DEBUG ((EFI_D_WARN, "MnpIsValidTxToken: Invalid FragmentLength or FragmentBuffer.\n"));
+      return FALSE;
+    }
+
+    TotalLength += FragmentTable[Index].FragmentLength;
+  }
+
+  if ((TxData->DestinationAddress == NULL) && (FragmentTable[0].FragmentLength < TxData->HeaderLength)) {
+    //
+    // Media header is split between fragments.
+    //
+    return FALSE;
+  }
+
+  if (TotalLength != (TxData->DataLength + TxData->HeaderLength)) {
+    //
+    // The length calculated from the fragment information doesn't equal to the
+    // sum of the DataLength and the HeaderLength.
+    //
+    DEBUG ((EFI_D_WARN, "MnpIsValidTxData: Invalid Datalength compared with the sum of fragment length.\n"));
+    return FALSE;
+  }
+
+  if (TxData->DataLength > MnpServiceData->Mtu) {
+    //
+    // The total length is larger than the MTU.
+    //
+    DEBUG ((EFI_D_WARN, "MnpIsValidTxData: TxData->DataLength exceeds Mtu.\n"));
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Build the packet to transmit from the TxData passed in.
+
+  @param[in]   MnpServiceData      Pointer to the mnp service context data.
+  @param[in]   TxData              Pointer to the transmit data containing the information
+                                   to build the packet.
+  @param[out]  PktBuf              Pointer to record the address of the packet.
+  @param[out]  PktLen              Pointer to a UINT32 variable used to record the packet's
+                                   length.
+
+  @retval EFI_SUCCESS           TxPackage is built.
+  @retval EFI_OUT_OF_RESOURCES  The deliver fails due to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpBuildTxPacket (
+  IN     MNP_SERVICE_DATA                    *MnpServiceData,
+  IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
+     OUT UINT8                               **PktBuf,
+     OUT UINT32                              *PktLen
+  )
+{
+  EFI_SIMPLE_NETWORK_MODE *SnpMode;
+  UINT8                   *DstPos;
+  UINT16                  Index;
+  MNP_DEVICE_DATA         *MnpDeviceData;
+  UINT8                   *TxBuf;
+  
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+  
+  TxBuf = MnpAllocTxBuf (MnpDeviceData);
+  if (TxBuf == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  
+  //
+  // Reserve space for vlan tag if needed.
+  //
+  if (MnpServiceData->VlanId != 0) {
+    *PktBuf = TxBuf + NET_VLAN_TAG_LEN;
+  } else {
+    *PktBuf = TxBuf;
+  }
+  
+  if ((TxData->DestinationAddress == NULL) && (TxData->FragmentCount == 1)) {
+    CopyMem (
+        *PktBuf,
+        TxData->FragmentTable[0].FragmentBuffer,
+        TxData->FragmentTable[0].FragmentLength
+        );
+    
+    *PktLen = TxData->FragmentTable[0].FragmentLength;
+  } else {
+    //
+    // Either media header isn't in FragmentTable or there is more than
+    // one fragment, copy the data into the packet buffer. Reserve the
+    // media header space if necessary.
+    //
+    SnpMode = MnpDeviceData->Snp->Mode; 
+    DstPos  = *PktBuf;
+    *PktLen = 0;
+    if (TxData->DestinationAddress != NULL) {
+      //
+      // If dest address is not NULL, move DstPos to reserve space for the
+      // media header. Add the media header length to buflen.
+      //
+      DstPos += SnpMode->MediaHeaderSize;
+      *PktLen += SnpMode->MediaHeaderSize;
+    }
+
+    for (Index = 0; Index < TxData->FragmentCount; Index++) {
+      //
+      // Copy the data.
+      //
+      CopyMem (
+        DstPos,
+        TxData->FragmentTable[Index].FragmentBuffer,
+        TxData->FragmentTable[Index].FragmentLength
+        );
+      DstPos += TxData->FragmentTable[Index].FragmentLength;
+    }
+
+    //
+    // Set the buffer length.
+    //
+    *PktLen += TxData->DataLength + TxData->HeaderLength;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Synchronously send out the packet. 
+
+  This functon places the packet buffer to SNP driver's tansmit queue. The packet
+  can be considered successfully sent out once SNP acccetp the packet, while the
+  packet buffer recycle is deferred for better performance.
+
+  @param[in]       MnpServiceData      Pointer to the mnp service context data.
+  @param[in]       Packet              Pointer to the pakcet buffer.
+  @param[in]       Length              The length of the packet.
+  @param[in, out]  Token               Pointer to the token the packet generated from.
+
+  @retval EFI_SUCCESS                  The packet is sent out.
+  @retval EFI_TIMEOUT                  Time out occurs, the packet isn't sent.
+  @retval EFI_DEVICE_ERROR             An unexpected network error occurs.
+
+**/
+EFI_STATUS
+MnpSyncSendPacket (
+  IN     MNP_SERVICE_DATA                        *MnpServiceData,
+  IN     UINT8                                   *Packet,
+  IN     UINT32                                  Length,
+  IN OUT EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL       *Snp;
+  EFI_MANAGED_NETWORK_TRANSMIT_DATA *TxData;
+  UINT32                            HeaderSize;
+  MNP_DEVICE_DATA                   *MnpDeviceData;
+  UINT16                            ProtocolType;
+
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+  Snp           = MnpDeviceData->Snp;
+  TxData        = Token->Packet.TxData;
+  Token->Status = EFI_SUCCESS;
+  HeaderSize    = Snp->Mode->MediaHeaderSize - TxData->HeaderLength;
+
+  //
+  // Check media status before transmit packet.
+  // Note: media status will be updated by periodic timer MediaDetectTimer.
+  //
+  if (Snp->Mode->MediaPresentSupported && !Snp->Mode->MediaPresent) {
+    //
+    // Media not present, skip packet transmit and report EFI_NO_MEDIA
+    //
+    DEBUG ((EFI_D_WARN, "MnpSyncSendPacket: No network cable detected.\n"));
+    Token->Status = EFI_NO_MEDIA;
+    goto SIGNAL_TOKEN;
+  }
+
+  
+  if (MnpServiceData->VlanId != 0) {
+    //
+    // Insert VLAN tag
+    //
+    MnpInsertVlanTag (MnpServiceData, TxData, &ProtocolType, &Packet, &Length);
+  } else {
+    ProtocolType = TxData->ProtocolType;
+  }
+
+  //
+  // Transmit the packet through SNP.
+  //
+  Status = Snp->Transmit (
+                  Snp,
+                  HeaderSize,
+                  Length,
+                  Packet,
+                  TxData->SourceAddress,
+                  TxData->DestinationAddress,
+                  &ProtocolType
+                  );
+  if (Status == EFI_NOT_READY) {
+    Status = MnpRecycleTxBuf (MnpDeviceData);
+    if (EFI_ERROR (Status)) {
+      Token->Status = EFI_DEVICE_ERROR;
+      goto SIGNAL_TOKEN;
+    }
+
+    Status = Snp->Transmit (
+                    Snp,
+                    HeaderSize,
+                    Length,
+                    Packet,
+                    TxData->SourceAddress,
+                    TxData->DestinationAddress,
+                    &ProtocolType
+                    ); 
+  }
+  
+  if (EFI_ERROR (Status)) {
+    Token->Status = EFI_DEVICE_ERROR;
+  }
+
+SIGNAL_TOKEN:
+
+  gBS->SignalEvent (Token->Event);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of Token->Event.
+  //
+  DispatchDpc ();
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Try to deliver the received packet to the instance.
+
+  @param[in, out]  Instance     Pointer to the mnp instance context data.
+
+  @retval EFI_SUCCESS           The received packet is delivered, or there is no
+                                packet to deliver, or there is no available receive
+                                token.
+  @retval EFI_OUT_OF_RESOURCES  The deliver fails due to lack of memory resource.
+
+**/
+EFI_STATUS
+MnpInstanceDeliverPacket (
+  IN OUT MNP_INSTANCE_DATA   *Instance
+  )
+{
+  MNP_DEVICE_DATA                       *MnpDeviceData;
+  MNP_RXDATA_WRAP                       *RxDataWrap;
+  NET_BUF                               *DupNbuf;
+  EFI_MANAGED_NETWORK_RECEIVE_DATA      *RxData;
+  EFI_SIMPLE_NETWORK_MODE               *SnpMode;
+  EFI_MANAGED_NETWORK_COMPLETION_TOKEN  *RxToken;
+
+  MnpDeviceData = Instance->MnpServiceData->MnpDeviceData;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  if (NetMapIsEmpty (&Instance->RxTokenMap) || IsListEmpty (&Instance->RcvdPacketQueue)) {
+    //
+    // No pending received data or no available receive token, return.
+    //
+    return EFI_SUCCESS;
+  }
+
+  ASSERT (Instance->RcvdPacketQueueSize != 0);
+
+  RxDataWrap = NET_LIST_HEAD (&Instance->RcvdPacketQueue, MNP_RXDATA_WRAP, WrapEntry);
+  if (RxDataWrap->Nbuf->RefCnt > 2) {
+    //
+    // There are other instances share this Nbuf, duplicate to get a
+    // copy to allow the instance to do R/W operations.
+    //
+    DupNbuf = MnpAllocNbuf (MnpDeviceData);
+    if (DupNbuf == NULL) {
+      DEBUG ((EFI_D_WARN, "MnpDeliverPacket: Failed to allocate a free Nbuf.\n"));
+
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    //
+    // Duplicate the net buffer.
+    //
+    NetbufDuplicate (RxDataWrap->Nbuf, DupNbuf, 0);
+    MnpFreeNbuf (MnpDeviceData, RxDataWrap->Nbuf);
+    RxDataWrap->Nbuf = DupNbuf;
+  }
+
+  //
+  // All resources are OK, remove the packet from the queue.
+  //
+  NetListRemoveHead (&Instance->RcvdPacketQueue);
+  Instance->RcvdPacketQueueSize--;
+
+  RxData  = &RxDataWrap->RxData;
+  SnpMode = MnpDeviceData->Snp->Mode;
+
+  //
+  // Set all the buffer pointers.
+  //
+  RxData->MediaHeader         = NetbufGetByte (RxDataWrap->Nbuf, 0, NULL);
+  RxData->DestinationAddress  = RxData->MediaHeader;
+  RxData->SourceAddress       = (UINT8 *) RxData->MediaHeader + SnpMode->HwAddressSize;
+  RxData->PacketData          = (UINT8 *) RxData->MediaHeader + SnpMode->MediaHeaderSize;
+
+  //
+  // Insert this RxDataWrap into the delivered queue.
+  //
+  InsertTailList (&Instance->RxDeliveredPacketQueue, &RxDataWrap->WrapEntry);
+
+  //
+  // Get the receive token from the RxTokenMap.
+  //
+  RxToken = NetMapRemoveHead (&Instance->RxTokenMap, NULL);
+
+  //
+  // Signal this token's event.
+  //
+  RxToken->Packet.RxData  = &RxDataWrap->RxData;
+  RxToken->Status         = EFI_SUCCESS;
+  gBS->SignalEvent (RxToken->Event);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Deliver the received packet for the instances belonging to the MnpServiceData.
+
+  @param[in]  MnpServiceData        Pointer to the mnp service context data.
+
+**/
+VOID
+MnpDeliverPacket (
+  IN MNP_SERVICE_DATA    *MnpServiceData
+  )
+{
+  LIST_ENTRY        *Entry;
+  MNP_INSTANCE_DATA *Instance;
+
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+
+  NET_LIST_FOR_EACH (Entry, &MnpServiceData->ChildrenList) {
+    Instance = NET_LIST_USER_STRUCT (Entry, MNP_INSTANCE_DATA, InstEntry);
+    NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+    //
+    // Try to deliver packet for this instance.
+    //
+    MnpInstanceDeliverPacket (Instance);
+  }
+}
+
+
+/**
+  Recycle the RxData and other resources used to hold and deliver the received
+  packet.
+
+  @param[in]  Event               The event this notify function registered to.
+  @param[in]  Context             Pointer to the context data registerd to the Event.
+
+**/
+VOID
+EFIAPI
+MnpRecycleRxData (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  MNP_RXDATA_WRAP *RxDataWrap;
+  MNP_DEVICE_DATA *MnpDeviceData;
+
+  ASSERT (Context != NULL);
+
+  RxDataWrap = (MNP_RXDATA_WRAP *) Context;
+  NET_CHECK_SIGNATURE (RxDataWrap->Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  ASSERT (RxDataWrap->Nbuf != NULL);
+
+  MnpDeviceData = RxDataWrap->Instance->MnpServiceData->MnpDeviceData;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  //
+  // Free this Nbuf.
+  //
+  MnpFreeNbuf (MnpDeviceData, RxDataWrap->Nbuf);
+  RxDataWrap->Nbuf = NULL;
+
+  //
+  // Close the recycle event.
+  //
+  gBS->CloseEvent (RxDataWrap->RxData.RecycleEvent);
+
+  //
+  // Remove this Wrap entry from the list.
+  //
+  RemoveEntryList (&RxDataWrap->WrapEntry);
+
+  FreePool (RxDataWrap);
+}
+
+
+/**
+  Queue the received packet into instance's receive queue.
+
+  @param[in, out]  Instance        Pointer to the mnp instance context data.
+  @param[in, out]  RxDataWrap      Pointer to the Wrap structure containing the
+                                   received data and other information.
+**/
+VOID
+MnpQueueRcvdPacket (
+  IN OUT MNP_INSTANCE_DATA   *Instance,
+  IN OUT MNP_RXDATA_WRAP     *RxDataWrap
+  )
+{
+  MNP_RXDATA_WRAP *OldRxDataWrap;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  //
+  // Check the queue size. If it exceeds the limit, drop one packet
+  // from the head.
+  //
+  if (Instance->RcvdPacketQueueSize == MNP_MAX_RCVD_PACKET_QUE_SIZE) {
+
+    DEBUG ((EFI_D_WARN, "MnpQueueRcvdPacket: Drop one packet bcz queue size limit reached.\n"));
+
+    //
+    // Get the oldest packet.
+    //
+    OldRxDataWrap = NET_LIST_HEAD (
+                      &Instance->RcvdPacketQueue,
+                      MNP_RXDATA_WRAP,
+                      WrapEntry
+                      );
+
+    //
+    // Recycle this OldRxDataWrap, this entry will be removed by the callee.
+    //
+    MnpRecycleRxData (NULL, (VOID *) OldRxDataWrap);
+    Instance->RcvdPacketQueueSize--;
+  }
+
+  //
+  // Update the timeout tick using the configured parameter.
+  //
+  RxDataWrap->TimeoutTick = Instance->ConfigData.ReceivedQueueTimeoutValue;
+
+  //
+  // Insert this Wrap into the instance queue.
+  //
+  InsertTailList (&Instance->RcvdPacketQueue, &RxDataWrap->WrapEntry);
+  Instance->RcvdPacketQueueSize++;
+}
+
+
+/**
+  Match the received packet with the instance receive filters.
+
+  @param[in]  Instance          Pointer to the mnp instance context data.
+  @param[in]  RxData            Pointer to the EFI_MANAGED_NETWORK_RECEIVE_DATA.
+  @param[in]  GroupAddress      Pointer to the GroupAddress, the GroupAddress is
+                                non-NULL and it contains the destination multicast
+                                mac address of the received packet if the packet
+                                destinated to a multicast mac address.
+  @param[in]  PktAttr           The received packets attribute.
+
+  @return The received packet matches the instance's receive filters or not.
+
+**/
+BOOLEAN
+MnpMatchPacket (
+  IN MNP_INSTANCE_DATA                   *Instance,
+  IN EFI_MANAGED_NETWORK_RECEIVE_DATA    *RxData,
+  IN MNP_GROUP_ADDRESS                   *GroupAddress OPTIONAL,
+  IN UINT8                               PktAttr
+  )
+{
+  EFI_MANAGED_NETWORK_CONFIG_DATA *ConfigData;
+  LIST_ENTRY                      *Entry;
+  MNP_GROUP_CONTROL_BLOCK         *GroupCtrlBlk;
+
+  NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+  ConfigData = &Instance->ConfigData;
+
+  //
+  // Check the protocol type.
+  //
+  if ((ConfigData->ProtocolTypeFilter != 0) && (ConfigData->ProtocolTypeFilter != RxData->ProtocolType)) {
+    return FALSE;
+  }
+
+  if (ConfigData->EnablePromiscuousReceive) {
+    //
+    // Always match if this instance is configured to be promiscuous.
+    //
+    return TRUE;
+  }
+
+  //
+  // The protocol type is matched, check receive filter, include unicast and broadcast.
+  //
+  if ((Instance->ReceiveFilter & PktAttr) != 0) {
+    return TRUE;
+  }
+
+  //
+  // Check multicast addresses.
+  //
+  if (ConfigData->EnableMulticastReceive && RxData->MulticastFlag) {
+
+    ASSERT (GroupAddress != NULL);
+
+    NET_LIST_FOR_EACH (Entry, &Instance->GroupCtrlBlkList) {
+
+      GroupCtrlBlk = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_CONTROL_BLOCK, CtrlBlkEntry);
+      if (GroupCtrlBlk->GroupAddress == GroupAddress) {
+        //
+        // The instance is configured to receiveing packets destinated to this
+        // multicast address.
+        //
+        return TRUE;
+      }
+    }
+  }
+
+  //
+  // No match.
+  //
+  return FALSE;
+}
+
+
+/**
+  Analyse the received packets.
+
+  @param[in]       MnpServiceData    Pointer to the mnp service context data.
+  @param[in]       Nbuf              Pointer to the net buffer holding the received
+                                     packet.
+  @param[in, out]  RxData            Pointer to the buffer used to save the analysed
+                                     result in EFI_MANAGED_NETWORK_RECEIVE_DATA.
+  @param[out]      GroupAddress      Pointer to pointer to a MNP_GROUP_ADDRESS used to
+                                     pass out the address of the multicast address the
+                                     received packet destinated to.
+  @param[out]      PktAttr           Pointer to the buffer used to save the analysed
+                                     packet attribute.
+
+**/
+VOID
+MnpAnalysePacket (
+  IN     MNP_SERVICE_DATA                    *MnpServiceData,
+  IN     NET_BUF                             *Nbuf,
+  IN OUT EFI_MANAGED_NETWORK_RECEIVE_DATA    *RxData,
+     OUT MNP_GROUP_ADDRESS                   **GroupAddress,
+     OUT UINT8                               *PktAttr
+  )
+{
+  EFI_SIMPLE_NETWORK_MODE *SnpMode;
+  MNP_DEVICE_DATA         *MnpDeviceData;
+  UINT8                   *BufPtr;
+  LIST_ENTRY              *Entry;
+
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+  SnpMode       = MnpDeviceData->Snp->Mode;
+
+  //
+  // Get the packet buffer.
+  //
+  BufPtr = NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (BufPtr != NULL);
+
+  //
+  // Set the initial values.
+  //
+  RxData->BroadcastFlag   = FALSE;
+  RxData->MulticastFlag   = FALSE;
+  RxData->PromiscuousFlag = FALSE;
+  *PktAttr                = UNICAST_PACKET;
+
+  if (!NET_MAC_EQUAL (&SnpMode->CurrentAddress, BufPtr, SnpMode->HwAddressSize)) {
+    //
+    // This packet isn't destinated to our current mac address, it't not unicast.
+    //
+    *PktAttr = 0;
+
+    if (NET_MAC_EQUAL (&SnpMode->BroadcastAddress, BufPtr, SnpMode->HwAddressSize)) {
+      //
+      // It's broadcast.
+      //
+      RxData->BroadcastFlag = TRUE;
+      *PktAttr              = BROADCAST_PACKET;
+    } else if ((*BufPtr & 0x01) == 0x1) {
+      //
+      // It's multicast, try to match the multicast filters.
+      //
+      NET_LIST_FOR_EACH (Entry, &MnpDeviceData->GroupAddressList) {
+
+        *GroupAddress = NET_LIST_USER_STRUCT (Entry, MNP_GROUP_ADDRESS, AddrEntry);
+        if (NET_MAC_EQUAL (BufPtr, &((*GroupAddress)->Address), SnpMode->HwAddressSize)) {
+          RxData->MulticastFlag = TRUE;
+          break;
+        }
+      }
+
+      if (!RxData->MulticastFlag) {
+        //
+        // No match, set GroupAddress to NULL. This multicast packet must
+        // be the result of PROMISUCOUS or PROMISUCOUS_MULTICAST flag is on.
+        //
+        *GroupAddress           = NULL;
+        RxData->PromiscuousFlag = TRUE;
+
+        if (MnpDeviceData->PromiscuousCount == 0) {
+          //
+          // Skip the below code, there is no receiver of this packet.
+          //
+          return ;
+        }
+      }
+    } else {
+      RxData->PromiscuousFlag = TRUE;
+    }
+  }
+
+  ZeroMem (&RxData->Timestamp, sizeof (EFI_TIME));
+
+  //
+  // Fill the common parts of RxData.
+  //
+  RxData->PacketLength  = Nbuf->TotalSize;
+  RxData->HeaderLength  = SnpMode->MediaHeaderSize;
+  RxData->AddressLength = SnpMode->HwAddressSize;
+  RxData->DataLength    = RxData->PacketLength - RxData->HeaderLength;
+  RxData->ProtocolType  = NTOHS (*(UINT16 *) (BufPtr + 2 * SnpMode->HwAddressSize));
+}
+
+
+/**
+  Wrap the RxData.
+
+  @param[in]  Instance           Pointer to the mnp instance context data.
+  @param[in]  RxData             Pointer to the receive data to wrap.
+
+  @return Pointer to a MNP_RXDATA_WRAP which wraps the RxData.
+
+**/
+MNP_RXDATA_WRAP *
+MnpWrapRxData (
+  IN MNP_INSTANCE_DATA                   *Instance,
+  IN EFI_MANAGED_NETWORK_RECEIVE_DATA    *RxData
+  )
+{
+  EFI_STATUS      Status;
+  MNP_RXDATA_WRAP *RxDataWrap;
+
+  //
+  // Allocate memory.
+  //
+  RxDataWrap = AllocatePool (sizeof (MNP_RXDATA_WRAP));
+  if (RxDataWrap == NULL) {
+    DEBUG ((EFI_D_ERROR, "MnpDispatchPacket: Failed to allocate a MNP_RXDATA_WRAP.\n"));
+    return NULL;
+  }
+
+  RxDataWrap->Instance = Instance;
+
+  //
+  // Fill the RxData in RxDataWrap,
+  //
+  CopyMem (&RxDataWrap->RxData, RxData, sizeof (RxDataWrap->RxData));
+
+  //
+  // Create the recycle event.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  MnpRecycleRxData,
+                  RxDataWrap,
+                  &RxDataWrap->RxData.RecycleEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "MnpDispatchPacket: gBS->CreateEvent failed, %r.\n", Status));
+
+    FreePool (RxDataWrap);
+    return NULL;
+  }
+
+  return RxDataWrap;
+}
+
+
+/**
+  Enqueue the received the packets to the instances belonging to the
+  MnpServiceData.
+
+  @param[in]  MnpServiceData    Pointer to the mnp service context data.
+  @param[in]  Nbuf              Pointer to the net buffer representing the received
+                                packet.
+
+**/
+VOID
+MnpEnqueuePacket (
+  IN MNP_SERVICE_DATA    *MnpServiceData,
+  IN NET_BUF             *Nbuf
+  )
+{
+  LIST_ENTRY                        *Entry;
+  MNP_INSTANCE_DATA                 *Instance;
+  EFI_MANAGED_NETWORK_RECEIVE_DATA  RxData;
+  UINT8                             PktAttr;
+  MNP_GROUP_ADDRESS                 *GroupAddress;
+  MNP_RXDATA_WRAP                   *RxDataWrap;
+
+
+  GroupAddress = NULL;
+  //
+  // First, analyse the packet header.
+  //
+  MnpAnalysePacket (MnpServiceData, Nbuf, &RxData, &GroupAddress, &PktAttr);
+
+  if (RxData.PromiscuousFlag && (MnpServiceData->MnpDeviceData->PromiscuousCount == 0)) {
+    //
+    // No receivers, no more action need.
+    //
+    return ;
+  }
+
+  //
+  // Iterate the children to find match.
+  //
+  NET_LIST_FOR_EACH (Entry, &MnpServiceData->ChildrenList) {
+
+    Instance = NET_LIST_USER_STRUCT (Entry, MNP_INSTANCE_DATA, InstEntry);
+    NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+    if (!Instance->Configured) {
+      continue;
+    }
+
+    //
+    // Check the packet against the instance receive filters.
+    //
+    if (MnpMatchPacket (Instance, &RxData, GroupAddress, PktAttr)) {
+      //
+      // Wrap the RxData.
+      //
+      RxDataWrap = MnpWrapRxData (Instance, &RxData);
+      if (RxDataWrap == NULL) {
+        continue;
+      }
+
+      //
+      // Associate RxDataWrap with Nbuf and increase the RefCnt.
+      //
+      RxDataWrap->Nbuf = Nbuf;
+      NET_GET_REF (RxDataWrap->Nbuf);
+
+      //
+      // Queue the packet into the instance queue.
+      //
+      MnpQueueRcvdPacket (Instance, RxDataWrap);
+    }
+  }
+}
+
+
+/**
+  Try to receive a packet and deliver it.
+
+  @param[in, out]  MnpDeviceData        Pointer to the mnp device context data.
+
+  @retval EFI_SUCCESS           add return value to function comment
+  @retval EFI_NOT_STARTED       The simple network protocol is not started.
+  @retval EFI_NOT_READY         No packet received.
+  @retval EFI_DEVICE_ERROR      An unexpected error occurs.
+
+**/
+EFI_STATUS
+MnpReceivePacket (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  NET_BUF                     *Nbuf;
+  UINT8                       *BufPtr;
+  UINTN                       BufLen;
+  UINTN                       HeaderSize;
+  UINT32                      Trimmed;
+  MNP_SERVICE_DATA            *MnpServiceData;
+  UINT16                      VlanId;
+  BOOLEAN                     IsVlanPacket;
+
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  Snp = MnpDeviceData->Snp;
+  if (Snp->Mode->State != EfiSimpleNetworkInitialized) {
+    //
+    // The simple network protocol is not started.
+    //
+    return EFI_NOT_STARTED;
+  }
+
+  if (MnpDeviceData->RxNbufCache == NULL) {
+    //
+    // Try to get a new buffer as there may be buffers recycled.
+    //
+    MnpDeviceData->RxNbufCache = MnpAllocNbuf (MnpDeviceData);
+
+    if (MnpDeviceData->RxNbufCache == NULL) {
+      //
+      // No availabe buffer in the buffer pool.
+      //
+      return EFI_DEVICE_ERROR;
+    }
+
+    NetbufAllocSpace (
+      MnpDeviceData->RxNbufCache,
+      MnpDeviceData->BufferLength,
+      NET_BUF_TAIL
+      );
+  }
+
+  Nbuf    = MnpDeviceData->RxNbufCache;
+  BufLen  = Nbuf->TotalSize;
+  BufPtr  = NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (BufPtr != NULL);
+
+  //
+  // Receive packet through Snp.
+  //
+  Status = Snp->Receive (Snp, &HeaderSize, &BufLen, BufPtr, NULL, NULL, NULL);
+  if (EFI_ERROR (Status)) {
+    DEBUG_CODE (
+      if (Status != EFI_NOT_READY) {
+        DEBUG ((EFI_D_WARN, "MnpReceivePacket: Snp->Receive() = %r.\n", Status));
+      }
+    );
+
+    return Status;
+  }
+
+  //
+  // Sanity check.
+  //
+  if ((HeaderSize != Snp->Mode->MediaHeaderSize) || (BufLen < HeaderSize)) {
+    DEBUG (
+      (EFI_D_WARN,
+      "MnpReceivePacket: Size error, HL:TL = %d:%d.\n",
+      HeaderSize,
+      BufLen)
+      );
+    return EFI_DEVICE_ERROR;
+  }
+
+  Trimmed = 0;
+  if (Nbuf->TotalSize != BufLen) {
+    //
+    // Trim the packet from tail.
+    //
+    Trimmed = NetbufTrim (Nbuf, Nbuf->TotalSize - (UINT32) BufLen, NET_BUF_TAIL);
+    ASSERT (Nbuf->TotalSize == BufLen);
+  }
+
+  VlanId = 0;
+  if (MnpDeviceData->NumberOfVlan != 0) {
+    //
+    // VLAN is configured, remove the VLAN tag if any
+    //
+    IsVlanPacket = MnpRemoveVlanTag (MnpDeviceData, Nbuf, &VlanId);
+  } else {
+    IsVlanPacket = FALSE;
+  }
+
+  MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
+  if (MnpServiceData == NULL) {
+    //
+    // VLAN is not set for this tagged frame, ignore this packet
+    //
+    if (Trimmed > 0) {
+      NetbufAllocSpace (Nbuf, Trimmed, NET_BUF_TAIL);
+    }
+
+    if (IsVlanPacket) {
+      NetbufAllocSpace (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);
+    }
+
+    goto EXIT;
+  }
+
+  //
+  // Enqueue the packet to the matched instances.
+  //
+  MnpEnqueuePacket (MnpServiceData, Nbuf);
+
+  if (Nbuf->RefCnt > 2) {
+    //
+    // RefCnt > 2 indicates there is at least one receiver of this packet.
+    // Free the current RxNbufCache and allocate a new one.
+    //
+    MnpFreeNbuf (MnpDeviceData, Nbuf);
+
+    Nbuf                       = MnpAllocNbuf (MnpDeviceData);
+    MnpDeviceData->RxNbufCache = Nbuf;
+    if (Nbuf == NULL) {
+      DEBUG ((EFI_D_ERROR, "MnpReceivePacket: Alloc packet for receiving cache failed.\n"));
+      return EFI_DEVICE_ERROR;
+    }
+
+    NetbufAllocSpace (Nbuf, MnpDeviceData->BufferLength, NET_BUF_TAIL);
+  } else {
+    //
+    // No receiver for this packet.
+    //
+    if (Trimmed > 0) {
+      NetbufAllocSpace (Nbuf, Trimmed, NET_BUF_TAIL);
+    }
+    if (IsVlanPacket) {
+      NetbufAllocSpace (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);
+    }
+
+    goto EXIT;
+  }
+  //
+  // Deliver the queued packets.
+  //
+  MnpDeliverPacket (MnpServiceData);
+
+EXIT:
+
+  ASSERT (Nbuf->TotalSize == MnpDeviceData->BufferLength);
+
+  return Status;
+}
+
+
+/**
+  Remove the received packets if timeout occurs.
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpCheckPacketTimeout (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  MNP_DEVICE_DATA   *MnpDeviceData;
+  MNP_SERVICE_DATA  *MnpServiceData;
+  LIST_ENTRY        *Entry;
+  LIST_ENTRY        *ServiceEntry;
+  LIST_ENTRY        *RxEntry;
+  LIST_ENTRY        *NextEntry;
+  MNP_INSTANCE_DATA *Instance;
+  MNP_RXDATA_WRAP   *RxDataWrap;
+  EFI_TPL           OldTpl;
+
+  MnpDeviceData = (MNP_DEVICE_DATA *) Context;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  NET_LIST_FOR_EACH (ServiceEntry, &MnpDeviceData->ServiceList) {
+    MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (ServiceEntry);
+
+    NET_LIST_FOR_EACH (Entry, &MnpServiceData->ChildrenList) {
+
+      Instance = NET_LIST_USER_STRUCT (Entry, MNP_INSTANCE_DATA, InstEntry);
+      NET_CHECK_SIGNATURE (Instance, MNP_INSTANCE_DATA_SIGNATURE);
+
+      if (!Instance->Configured || (Instance->ConfigData.ReceivedQueueTimeoutValue == 0)) {
+        //
+        // This instance is not configured or there is no receive time out,
+        // just skip to the next instance.
+        //
+        continue;
+      }
+
+      OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+      NET_LIST_FOR_EACH_SAFE (RxEntry, NextEntry, &Instance->RcvdPacketQueue) {
+
+        RxDataWrap = NET_LIST_USER_STRUCT (RxEntry, MNP_RXDATA_WRAP, WrapEntry);
+
+        //
+        // TimeoutTick unit is microsecond, MNP_TIMEOUT_CHECK_INTERVAL unit is 100ns.
+        //
+        if (RxDataWrap->TimeoutTick >= (MNP_TIMEOUT_CHECK_INTERVAL / 10)) {
+          RxDataWrap->TimeoutTick -= (MNP_TIMEOUT_CHECK_INTERVAL / 10);
+        } else {
+          //
+          // Drop the timeout packet.
+          //
+          DEBUG ((EFI_D_WARN, "MnpCheckPacketTimeout: Received packet timeout.\n"));
+          MnpRecycleRxData (NULL, RxDataWrap);
+          Instance->RcvdPacketQueueSize--;
+        }
+      }
+
+      gBS->RestoreTPL (OldTpl);
+    }
+  }
+}
+
+/**
+  Poll to update MediaPresent field in SNP ModeData by Snp->GetStatus().
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpCheckMediaStatus (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  MNP_DEVICE_DATA             *MnpDeviceData;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  UINT32                      InterruptStatus;
+
+  MnpDeviceData = (MNP_DEVICE_DATA *) Context;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  Snp = MnpDeviceData->Snp;
+  if (Snp->Mode->MediaPresentSupported) {
+    //
+    // Upon successful return of GetStatus(), the MediaPresent field of
+    // EFI_SIMPLE_NETWORK_MODE will be updated to reflect any change of media status
+    //
+    Snp->GetStatus (Snp, &InterruptStatus, NULL);
+  }
+}
+
+/**
+  Poll to receive the packets from Snp. This function is either called by upperlayer
+  protocols/applications or the system poll timer notify mechanism.
+
+  @param[in]  Event        The event this notify function registered to.
+  @param[in]  Context      Pointer to the context data registered to the event.
+
+**/
+VOID
+EFIAPI
+MnpSystemPoll (
+  IN EFI_EVENT     Event,
+  IN VOID          *Context
+  )
+{
+  MNP_DEVICE_DATA  *MnpDeviceData;
+
+  MnpDeviceData = (MNP_DEVICE_DATA *) Context;
+  NET_CHECK_SIGNATURE (MnpDeviceData, MNP_DEVICE_DATA_SIGNATURE);
+
+  //
+  // Try to receive packets from Snp.
+  //
+  MnpReceivePacket (MnpDeviceData);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of rx token's events.
+  //
+  DispatchDpc ();
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpMain.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpMain.c
new file mode 100644
index 0000000000..31c2e3e5b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpMain.c
@@ -0,0 +1,796 @@
+/** @file
+  Implementation of Managed Network Protocol public services.
+
+Copyright (c) 2005 - 2016, 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<BR>
+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 "MnpImpl.h"
+
+/**
+  Returns the operational parameters for the current MNP child driver. May also
+  support returning the underlying SNP driver mode data.
+
+  The GetModeData() function is used to read the current mode data (operational
+  parameters) from the MNP or the underlying SNP.
+
+  @param[in]  This          Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[out] MnpConfigData Pointer to storage for MNP operational parameters. Type
+                            EFI_MANAGED_NETWORK_CONFIG_DATA is defined in "Related
+                            Definitions" below.
+  @param[out] SnpModeData   Pointer to storage for SNP operational parameters. This
+                            feature may be unsupported. Type EFI_SIMPLE_NETWORK_MODE
+                            is defined in the EFI_SIMPLE_NETWORK_PROTOCOL.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this
+                                MNP implementation.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured. The default values are returned in
+                                MnpConfigData if it is not NULL.
+  @retval Others                The mode data could not be read.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpGetModeData (
+  IN     EFI_MANAGED_NETWORK_PROTOCOL      *This,
+     OUT EFI_MANAGED_NETWORK_CONFIG_DATA   *MnpConfigData OPTIONAL,
+     OUT EFI_SIMPLE_NETWORK_MODE           *SnpModeData OPTIONAL
+  )
+{
+  MNP_INSTANCE_DATA           *Instance;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  EFI_TPL                     OldTpl;
+  EFI_STATUS                  Status;
+  UINT32                      InterruptStatus;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (MnpConfigData != NULL) {
+    //
+    // Copy the instance configuration data.
+    //
+    CopyMem (MnpConfigData, &Instance->ConfigData, sizeof (*MnpConfigData));
+  }
+
+  if (SnpModeData != NULL) {
+    //
+    // Copy the underlayer Snp mode data.
+    //
+    Snp = Instance->MnpServiceData->MnpDeviceData->Snp;
+
+    //
+    // Upon successful return of GetStatus(), the Snp->Mode->MediaPresent
+    // will be updated to reflect any change of media status
+    //
+    Snp->GetStatus (Snp, &InterruptStatus, NULL);
+    CopyMem (SnpModeData, Snp->Mode, sizeof (*SnpModeData));
+  }
+
+  if (!Instance->Configured) {
+    Status = EFI_NOT_STARTED;
+  } else {
+    Status = EFI_SUCCESS;
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Sets or clears the operational parameters for the MNP child driver.
+
+  The Configure() function is used to set, change, or reset the operational
+  parameters for the MNP child driver instance. Until the operational parameters
+  have been set, no network traffic can be sent or received by this MNP child
+  driver instance. Once the operational parameters have been reset, no more
+  traffic can be sent or received until the operational parameters have been set
+  again.
+  Each MNP child driver instance can be started and stopped independently of
+  each other by setting or resetting their receive filter settings with the
+  Configure() function.
+  After any successful call to Configure(), the MNP child driver instance is
+  started. The internal periodic timer (if supported) is enabled. Data can be
+  transmitted and may be received if the receive filters have also been enabled.
+  Note: If multiple MNP child driver instances will receive the same packet
+  because of overlapping receive filter settings, then the first MNP child
+  driver instance will receive the original packet and additional instances will
+  receive copies of the original packet.
+  Note: Warning: Receive filter settings that overlap will consume extra
+  processor and/or DMA resources and degrade system and network performance.
+
+  @param[in]  This           Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  MnpConfigData  Pointer to configuration data that will be assigned
+                             to the MNP child driver instance. If NULL, the MNP
+                             child driver instance is reset to startup defaults
+                             and all pending transmit and receive requests are
+                             flushed. Type EFI_MANAGED_NETWORK_CONFIG_DATA is
+                             defined in EFI_MANAGED_NETWORK_PROTOCOL.GetModeData().
+
+  @retval EFI_SUCCESS            The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * MnpConfigData.ProtocolTypeFilter is not
+                                   valid.
+                                 The operational data for the MNP child driver
+                                 instance is unchanged.
+  @retval EFI_OUT_OF_RESOURCES   Required system resources (usually memory)
+                                 could not be allocated.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_UNSUPPORTED        The requested feature is unsupported in
+                                 this [MNP] implementation. The operational data
+                                 for the MNP child driver instance is unchanged.
+  @retval EFI_DEVICE_ERROR       An unexpected network or system error
+                                 occurred. The MNP child driver instance has
+                                 been reset to startup defaults.
+  @retval Others                 The MNP child driver instance has been reset to
+                                 startup defaults.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpConfigure (
+  IN EFI_MANAGED_NETWORK_PROTOCOL        *This,
+  IN EFI_MANAGED_NETWORK_CONFIG_DATA     *MnpConfigData OPTIONAL
+  )
+{
+  MNP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+  EFI_STATUS         Status;
+
+  if ((This == NULL) ||
+      ((MnpConfigData != NULL) &&
+       (MnpConfigData->ProtocolTypeFilter > 0) &&
+       (MnpConfigData->ProtocolTypeFilter <= 1500))
+     ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if ((MnpConfigData == NULL) && (!Instance->Configured)) {
+    //
+    // If the instance is not configured and a reset is requested, just return.
+    //
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  //
+  // Configure the instance.
+  //
+  Status = MnpConfigureInstance (Instance, MnpConfigData);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Translates an IP multicast address to a hardware (MAC) multicast address. This
+  function may be unsupported in some MNP implementations.
+
+  The McastIpToMac() function translates an IP multicast address to a hardware
+  (MAC) multicast address. This function may be implemented by calling the
+  underlying EFI_SIMPLE_NETWORK. MCastIpToMac() function, which may also be
+  unsupported in some MNP implementations.
+
+  @param[in]  This        Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Ipv6Flag    Set to TRUE to if IpAddress is an IPv6 multicast address.
+                          Set to FALSE if IpAddress is an IPv4 multicast address.
+  @param[in]  IpAddress   Pointer to the multicast IP address (in network byte
+                          order) to convert.
+  @param[out] MacAddress  Pointer to the resulting multicast MAC address.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One of the following conditions is TRUE:
+                                 * This is NULL.
+                                 * IpAddress is NULL.
+                                 * IpAddress is not a valid multicast IP
+                                   address.
+                                 * MacAddress is NULL.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this
+                                MNP implementation.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred.
+  @retval Others                The address could not be converted.
+**/
+EFI_STATUS
+EFIAPI
+MnpMcastIpToMac (
+  IN     EFI_MANAGED_NETWORK_PROTOCOL    *This,
+  IN     BOOLEAN                         Ipv6Flag,
+  IN     EFI_IP_ADDRESS                  *IpAddress,
+     OUT EFI_MAC_ADDRESS                 *MacAddress
+  )
+{
+  EFI_STATUS                  Status;
+  MNP_INSTANCE_DATA           *Instance;
+  EFI_SIMPLE_NETWORK_PROTOCOL *Snp;
+  EFI_TPL                     OldTpl;
+  EFI_IPv6_ADDRESS            *Ip6Address;
+
+  if ((This == NULL) || (IpAddress == NULL) || (MacAddress == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Ip6Address = &IpAddress->v6;
+
+  if ((Ipv6Flag && !IP6_IS_MULTICAST (Ip6Address)) ||
+      (!Ipv6Flag && !IP4_IS_MULTICAST (EFI_NTOHL (*IpAddress)))
+      ) {
+    //
+    // The IP address passed in is not a multicast address.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  Snp = Instance->MnpServiceData->MnpDeviceData->Snp;
+  ASSERT (Snp != NULL);
+
+  ZeroMem (MacAddress, sizeof (EFI_MAC_ADDRESS));
+
+  if (Snp->Mode->IfType == NET_IFTYPE_ETHERNET) {
+    if (!Ipv6Flag) {
+      //
+      // Translate the IPv4 address into a multicast MAC address if the NIC is an
+      // ethernet NIC according to RFC1112..
+      //
+      MacAddress->Addr[0] = 0x01;
+      MacAddress->Addr[1] = 0x00;
+      MacAddress->Addr[2] = 0x5E;
+      MacAddress->Addr[3] = (UINT8) (IpAddress->v4.Addr[1] & 0x7F);
+      MacAddress->Addr[4] = IpAddress->v4.Addr[2];
+      MacAddress->Addr[5] = IpAddress->v4.Addr[3];
+    } else {
+      //
+      // Translate the IPv6 address into a multicast MAC address if the NIC is an
+      // ethernet NIC according to RFC2464.
+      //
+
+      MacAddress->Addr[0] = 0x33;
+      MacAddress->Addr[1] = 0x33;
+      MacAddress->Addr[2] = Ip6Address->Addr[12];
+      MacAddress->Addr[3] = Ip6Address->Addr[13];
+      MacAddress->Addr[4] = Ip6Address->Addr[14];
+      MacAddress->Addr[5] = Ip6Address->Addr[15];
+    }
+
+    Status = EFI_SUCCESS;
+  } else {
+    //
+    // Invoke Snp to translate the multicast IP address.
+    //
+    Status = Snp->MCastIpToMac (
+                    Snp,
+                    Ipv6Flag,
+                    IpAddress,
+                    MacAddress
+                    );
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Enables and disables receive filters for multicast address. This function may
+  be unsupported in some MNP implementations.
+
+  The Groups() function only adds and removes multicast MAC addresses from the
+  filter list. The MNP driver does not transmit or process Internet Group
+  Management Protocol (IGMP) packets. If JoinFlag is FALSE and MacAddress is
+  NULL, then all joined groups are left.
+
+  @param[in]  This        Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  JoinFlag    Set to TRUE to join this multicast group.
+                          Set to FALSE to leave this multicast group.
+  @param[in]  MacAddress  Pointer to the multicast MAC group (address) to join or
+                          leave.
+
+  @retval EFI_SUCCESS           The requested operation completed successfully.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * This is NULL.
+                                * JoinFlag is TRUE and MacAddress is NULL.
+                                * MacAddress is not a valid multicast MAC
+                                  address.
+                                * The MNP multicast group settings are
+                                  unchanged.
+  @retval EFI_NOT_STARTED       This MNP child driver instance has not been
+                                configured.
+  @retval EFI_ALREADY_STARTED   The supplied multicast group is already joined.
+  @retval EFI_NOT_FOUND         The supplied multicast group is not joined.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred.
+                                The MNP child driver instance has been reset to
+                                startup defaults.
+  @retval EFI_UNSUPPORTED       The requested feature is unsupported in this MNP
+                                implementation.
+  @retval Others                The requested operation could not be completed.
+                                The MNP multicast group settings are unchanged.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpGroups (
+  IN EFI_MANAGED_NETWORK_PROTOCOL    *This,
+  IN BOOLEAN                         JoinFlag,
+  IN EFI_MAC_ADDRESS                 *MacAddress OPTIONAL
+  )
+{
+  MNP_INSTANCE_DATA       *Instance;
+  EFI_SIMPLE_NETWORK_MODE *SnpMode;
+  MNP_GROUP_CONTROL_BLOCK *GroupCtrlBlk;
+  MNP_GROUP_ADDRESS       *GroupAddress;
+  LIST_ENTRY              *ListEntry;
+  BOOLEAN                 AddressExist;
+  EFI_TPL                 OldTpl;
+  EFI_STATUS              Status;
+
+  if (This == NULL || (JoinFlag && (MacAddress == NULL))) {
+    //
+    // This is NULL, or it's a join operation but MacAddress is NULL.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance  = MNP_INSTANCE_DATA_FROM_THIS (This);
+  SnpMode   = Instance->MnpServiceData->MnpDeviceData->Snp->Mode;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if ((!Instance->ConfigData.EnableMulticastReceive) ||
+    ((MacAddress != NULL) && !NET_MAC_IS_MULTICAST (MacAddress, &SnpMode->BroadcastAddress, SnpMode->HwAddressSize))) {
+    //
+    // The instance isn't configured to do mulitcast receive. OR
+    // the passed in MacAddress is not a mutlticast mac address.
+    //
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Status       = EFI_SUCCESS;
+  AddressExist = FALSE;
+  GroupCtrlBlk = NULL;
+
+  if (MacAddress != NULL) {
+    //
+    // Search the instance's GroupCtrlBlkList to find the specific address.
+    //
+    NET_LIST_FOR_EACH (ListEntry, &Instance->GroupCtrlBlkList) {
+
+      GroupCtrlBlk = NET_LIST_USER_STRUCT (
+                      ListEntry,
+                      MNP_GROUP_CONTROL_BLOCK,
+                      CtrlBlkEntry
+                      );
+      GroupAddress = GroupCtrlBlk->GroupAddress;
+      if (0 == CompareMem (
+                MacAddress,
+                &GroupAddress->Address,
+                SnpMode->HwAddressSize
+                )) {
+        //
+        // There is already the same multicast mac address configured.
+        //
+        AddressExist = TRUE;
+        break;
+      }
+    }
+
+    if (JoinFlag && AddressExist) {
+      //
+      // The multicast mac address to join already exists.
+      //
+      Status = EFI_ALREADY_STARTED;
+    }
+
+    if (!JoinFlag && !AddressExist) {
+      //
+      // The multicast mac address to leave doesn't exist in this instance.
+      //
+      Status = EFI_NOT_FOUND;
+    }
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  } else if (IsListEmpty (&Instance->GroupCtrlBlkList)) {
+    //
+    // The MacAddress is NULL and there is no configured multicast mac address,
+    // just return.
+    //
+    goto ON_EXIT;
+  }
+
+  //
+  // OK, it is time to take action.
+  //
+  Status = MnpGroupOp (Instance, JoinFlag, MacAddress, GroupCtrlBlk);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Places asynchronous outgoing data packets into the transmit queue.
+
+  The Transmit() function places a completion token into the transmit packet
+  queue. This function is always asynchronous.
+  The caller must fill in the Token.Event and Token.TxData fields in the
+  completion token, and these fields cannot be NULL. When the transmit operation
+  completes, the MNP updates the Token.Status field and the Token.Event is
+  signaled.
+  Note: There may be a performance penalty if the packet needs to be
+  defragmented before it can be transmitted by the network device. Systems in
+  which performance is critical should review the requirements and features of
+  the underlying communications device and drivers.
+
+
+  @param[in]  This    Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token   Pointer to a token associated with the transmit data
+                      descriptor. Type EFI_MANAGED_NETWORK_COMPLETION_TOKEN
+                      is defined in "Related Definitions" below.
+
+  @retval EFI_SUCCESS            The transmit completion token was cached.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * Token is NULL.
+                                 * Token.Event is NULL.
+                                 * Token.TxData is NULL.
+                                 * Token.TxData.DestinationAddress is not
+                                   NULL and Token.TxData.HeaderLength is zero.
+                                 * Token.TxData.FragmentCount is zero.
+                                 * (Token.TxData.HeaderLength +
+                                   Token.TxData.DataLength) is not equal to the
+                                   sum of the
+                                   Token.TxData.FragmentTable[].FragmentLength
+                                   fields.
+                                 * One or more of the
+                                   Token.TxData.FragmentTable[].FragmentLength
+                                   fields is zero.
+                                 * One or more of the
+                                   Token.TxData.FragmentTable[].FragmentBufferfields
+                                   is NULL.
+                                 * Token.TxData.DataLength is greater than MTU.
+  @retval EFI_ACCESS_DENIED      The transmit completion token is already in the
+                                 transmit queue.
+  @retval EFI_OUT_OF_RESOURCES   The transmit data could not be queued due to a
+                                 lack of system resources (usually memory).
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_NOT_READY          The transmit request could not be queued because
+                                 the transmit queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpTransmit (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  )
+{
+  EFI_STATUS        Status;
+  MNP_INSTANCE_DATA *Instance;
+  MNP_SERVICE_DATA  *MnpServiceData;
+  UINT8             *PktBuf;
+  UINT32            PktLen;
+  EFI_TPL           OldTpl;
+
+  if ((This == NULL) || (Token == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (!MnpIsValidTxToken (Instance, Token)) {
+    //
+    // The Token is invalid.
+    //
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  MnpServiceData = Instance->MnpServiceData;
+  NET_CHECK_SIGNATURE (MnpServiceData, MNP_SERVICE_DATA_SIGNATURE);
+
+  //
+  // Build the tx packet
+  //
+  Status = MnpBuildTxPacket (MnpServiceData, Token->Packet.TxData, &PktBuf, &PktLen);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  //  OK, send the packet synchronously.
+  //
+  Status = MnpSyncSendPacket (MnpServiceData, PktBuf, PktLen, Token);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Places an asynchronous receiving request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet
+  queue. This function is always asynchronous.
+  The caller must fill in the Token.Event field in the completion token, and
+  this field cannot be NULL. When the receive operation completes, the MNP
+  updates the Token.Status and Token.RxData fields and the Token.Event is
+  signaled.
+
+  @param[in]  This      Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token     Pointer to a token associated with the receive
+                        data descriptor. Type
+                        EFI_MANAGED_NETWORK_COMPLETION_TOKEN is defined in
+                        EFI_MANAGED_NETWORK_PROTOCOL.Transmit().
+
+  @retval EFI_SUCCESS            The receive completion token was cached.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is
+                                 TRUE:
+                                 * This is NULL.
+                                 * Token is NULL.
+                                 * Token.Event is NULL
+  @retval EFI_OUT_OF_RESOURCES   The transmit data could not be queued due to a
+                                 lack of system resources (usually memory).
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred.
+                                 The MNP child driver instance has been reset to
+                                 startup defaults.
+  @retval EFI_ACCESS_DENIED      The receive completion token was already in the
+                                 receive queue.
+  @retval EFI_NOT_READY          The receive request could not be queued because
+                                 the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpReceive (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token
+  )
+{
+  EFI_STATUS         Status;
+  MNP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+
+  if ((This == NULL) || (Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Check whether this token(event) is already in the rx token queue.
+  //
+  Status = NetMapIterate (&Instance->RxTokenMap, MnpTokenExist, (VOID *) Token);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Insert the Token into the RxTokenMap.
+  //
+  Status = NetMapInsertTail (&Instance->RxTokenMap, (VOID *) Token, NULL);
+  if (!EFI_ERROR (Status)) {
+    //
+    // Try to deliver any buffered packets.
+    //
+    Status = MnpInstanceDeliverPacket (Instance);
+
+    //
+    // Dispatch the DPC queued by the NotifyFunction of Token->Event.
+    //
+    DispatchDpc ();
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Aborts an asynchronous transmit or receive request.
+
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token.Status will be set to EFI_ABORTED and then Token.Event will be
+  signaled. If the token is not in one of the queues, which usually means that
+  the asynchronous operation has completed, this function will not signal the
+  token and EFI_NOT_FOUND is returned.
+
+  @param[in]  This     Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+  @param[in]  Token    Pointer to a token that has been issued by
+                       EFI_MANAGED_NETWORK_PROTOCOL.Transmit() or
+                       EFI_MANAGED_NETWORK_PROTOCOL.Receive(). If NULL, all
+                       pending tokens are aborted.
+
+  @retval EFI_SUCCESS            The asynchronous I/O request was aborted and
+                                 Token.Event was signaled. When Token is NULL,
+                                 all pending requests were aborted and their
+                                 events were signaled.
+  @retval EFI_NOT_STARTED        This MNP child driver instance has not been
+                                 configured.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_NOT_FOUND          When Token is not NULL, the asynchronous I/O
+                                 request was not found in the transmit or
+                                 receive queue. It has either completed or was
+                                 not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+MnpCancel (
+  IN EFI_MANAGED_NETWORK_PROTOCOL            *This,
+  IN EFI_MANAGED_NETWORK_COMPLETION_TOKEN    *Token OPTIONAL
+  )
+{
+  EFI_STATUS         Status;
+  MNP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Iterate the RxTokenMap to cancel the specified Token.
+  //
+  Status = NetMapIterate (&Instance->RxTokenMap, MnpCancelTokens, (VOID *) Token);
+  if (Token != NULL) {
+    Status = (Status == EFI_ABORTED) ? EFI_SUCCESS : EFI_NOT_FOUND;
+  }
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of the cancled token's events.
+  //
+  DispatchDpc ();
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+
+  The Poll() function can be used by network drivers and applications to
+  increase the rate that data packets are moved between the communications
+  device and the transmit and receive queues.
+  Normally, a periodic timer event internally calls the Poll() function. But, in
+  some systems, the periodic timer event may not call Poll() fast enough to
+  transmit and/or receive all data packets without missing packets. Drivers and
+  applications that are experiencing packet loss should try calling the Poll()
+  function more often.
+
+  @param[in]  This         Pointer to the EFI_MANAGED_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS      Incoming or outgoing data was processed.
+  @retval EFI_NOT_STARTED  This MNP child driver instance has not been
+                           configured.
+  @retval EFI_DEVICE_ERROR An unexpected system or network error occurred. The
+                           MNP child driver instance has been reset to startup
+                           defaults.
+  @retval EFI_NOT_READY    No incoming or outgoing data was processed. Consider
+                           increasing the polling rate.
+  @retval EFI_TIMEOUT      Data was dropped out of the transmit and/or receive
+                           queue. Consider increasing the polling rate.
+
+**/
+EFI_STATUS
+EFIAPI
+MnpPoll (
+  IN EFI_MANAGED_NETWORK_PROTOCOL    *This
+  )
+{
+  EFI_STATUS         Status;
+  MNP_INSTANCE_DATA  *Instance;
+  EFI_TPL            OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MNP_INSTANCE_DATA_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (!Instance->Configured) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  //
+  // Try to receive packets.
+  //
+  Status = MnpReceivePacket (Instance->MnpServiceData->MnpDeviceData);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of rx token's events.
+  //
+  DispatchDpc ();
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c
new file mode 100644
index 0000000000..6e14c1f37f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.c
@@ -0,0 +1,683 @@
+/** @file
+  VLAN Config Protocol implementation and VLAN packet process routine.
+
+Copyright (c) 2009 - 2014, 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<BR>
+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 "MnpImpl.h"
+#include "MnpVlan.h"
+
+VLAN_DEVICE_PATH          mVlanDevicePathTemplate = {
+  {
+    MESSAGING_DEVICE_PATH,
+    MSG_VLAN_DP,
+    {
+      (UINT8) (sizeof (VLAN_DEVICE_PATH)),
+      (UINT8) ((sizeof (VLAN_DEVICE_PATH)) >> 8)
+    }
+  },
+  0
+};
+
+EFI_VLAN_CONFIG_PROTOCOL  mVlanConfigProtocolTemplate = {
+  VlanConfigSet,
+  VlanConfigFind,
+  VlanConfigRemove
+};
+
+
+/**
+  Create a child handle for the VLAN ID.
+
+  @param[in]       ImageHandle        The driver image handle.
+  @param[in]       ControllerHandle   Handle of device to bind driver to.
+  @param[in]       VlanId             The VLAN ID.
+  @param[out]      Devicepath         Pointer to returned device path for child handle.
+
+  @return The handle of VLAN child or NULL if failed to create VLAN child.
+
+**/
+EFI_HANDLE
+MnpCreateVlanChild (
+  IN     EFI_HANDLE                  ImageHandle,
+  IN     EFI_HANDLE                  ControllerHandle,
+  IN     UINT16                      VlanId,
+     OUT EFI_DEVICE_PATH_PROTOCOL    **Devicepath OPTIONAL
+  )
+{
+  EFI_HANDLE                ChildHandle;
+  VLAN_DEVICE_PATH          VlanNode;
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *VlanDevicePath;
+  EFI_STATUS                Status;
+
+  //
+  // Try to get parent device path
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ParentDevicePath,
+                  ImageHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Construct device path for child handle: MAC + VLAN
+  //
+  CopyMem (&VlanNode, &mVlanDevicePathTemplate, sizeof (VLAN_DEVICE_PATH));
+  VlanNode.VlanId = VlanId;
+  VlanDevicePath = AppendDevicePathNode (
+                     ParentDevicePath,
+                     (EFI_DEVICE_PATH_PROTOCOL *) &VlanNode
+                     );
+  if (VlanDevicePath == NULL) {
+    return NULL;
+  }
+
+  //
+  // Create child VLAN handle by installing DevicePath protocol
+  //
+  ChildHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ChildHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  VlanDevicePath,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (VlanDevicePath);
+    return NULL;
+  }
+
+  if (Devicepath != NULL) {
+    *Devicepath = VlanDevicePath;
+  }
+
+  return ChildHandle;
+}
+
+/**
+  Remove VLAN tag from a packet.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in, out]  Nbuf               Pointer to the NET_BUF to remove VLAN tag.
+  @param[out]      VlanId             Pointer to the returned VLAN ID.
+
+  @retval TRUE             VLAN tag is removed from this packet.
+  @retval FALSE            There is no VLAN tag in this packet.
+
+**/
+BOOLEAN
+MnpRemoveVlanTag (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN OUT NET_BUF           *Nbuf,
+     OUT UINT16            *VlanId
+  )
+{
+  UINT8     *Packet;
+  UINTN     ProtocolOffset;
+  UINT16    ProtocolType;
+  VLAN_TCI  VlanTag;
+
+  ProtocolOffset = MnpDeviceData->Snp->Mode->HwAddressSize * 2;
+
+  //
+  // Get the packet buffer.
+  //
+  Packet = NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (Packet != NULL);
+
+  //
+  // Check whether this is VLAN tagged frame by Ether Type
+  //
+  *VlanId      = 0;
+  ProtocolType = NTOHS (*(UINT16 *) (Packet + ProtocolOffset));
+  if (ProtocolType != ETHER_TYPE_VLAN) {
+    //
+    // Not a VLAN tagged frame
+    //
+    return FALSE;
+  }
+
+  VlanTag.Uint16 = NTOHS (*(UINT16 *) (Packet + ProtocolOffset + sizeof (ProtocolType)));
+  *VlanId = VlanTag.Bits.Vid;
+
+  //
+  // Move hardware address (DA + SA) 4 bytes right to override VLAN tag
+  //
+  CopyMem (Packet + NET_VLAN_TAG_LEN, Packet, ProtocolOffset);
+
+  //
+  // Remove VLAN tag from the Nbuf
+  //
+  NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);
+
+  return TRUE;
+}
+
+
+/**
+  Build the vlan packet to transmit from the TxData passed in.
+
+  @param  MnpServiceData         Pointer to the mnp service context data.
+  @param  TxData                 Pointer to the transmit data containing the
+                                 information to build the packet.
+  @param  ProtocolType           Pointer to the Ethernet protocol type.
+  @param  Packet                 Pointer to record the address of the packet.
+  @param  Length                 Pointer to a UINT32 variable used to record the
+                                 packet's length.
+
+**/
+VOID
+MnpInsertVlanTag (
+  IN     MNP_SERVICE_DATA                    *MnpServiceData,
+  IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
+     OUT UINT16                              *ProtocolType,
+  IN OUT UINT8                               **Packet,
+  IN OUT UINT32                              *Length
+  )
+{
+  VLAN_TCI                *VlanTci;
+  UINT16                  *Tpid;
+  UINT16                  *EtherType;
+  MNP_DEVICE_DATA         *MnpDeviceData;
+  EFI_SIMPLE_NETWORK_MODE *SnpMode;
+
+  MnpDeviceData = MnpServiceData->MnpDeviceData;
+  SnpMode       = MnpDeviceData->Snp->Mode;
+
+  *ProtocolType = ETHER_TYPE_VLAN;
+  *Length = *Length + NET_VLAN_TAG_LEN;
+  *Packet = *Packet - NET_VLAN_TAG_LEN;
+
+  Tpid    = (UINT16 *) (*Packet + SnpMode->MediaHeaderSize - sizeof (*ProtocolType));
+  VlanTci = (VLAN_TCI *) (UINTN) (Tpid + 1);
+  if (TxData->HeaderLength != 0) {
+    //
+    // Media header is in packet, move DA+SA 4 bytes left
+    //
+    CopyMem (
+      *Packet,
+      *Packet + NET_VLAN_TAG_LEN,
+      SnpMode->MediaHeaderSize - sizeof (*ProtocolType)
+      );
+    *Tpid = HTONS (ETHER_TYPE_VLAN);
+  } else {
+    //
+    // Media header not in packet, VLAN TCI and original protocol type becomes payload
+    //
+    EtherType  = (UINT16 *) (UINTN) (VlanTci + 1);
+    *EtherType = HTONS (TxData->ProtocolType);
+  }
+
+  VlanTci->Bits.Vid      = MnpServiceData->VlanId;
+  VlanTci->Bits.Cfi      = VLAN_TCI_CFI_CANONICAL_MAC;
+  VlanTci->Bits.Priority = MnpServiceData->Priority;
+  VlanTci->Uint16        = HTONS (VlanTci->Uint16);
+}
+
+
+/**
+  Get VLAN configuration variable.
+
+  @param[in]       MnpDeviceData      Pointer to the MNP device context data.
+  @param[out]      NumberOfVlan       Pointer to number of VLAN to be returned.
+  @param[out]      VlanVariable       Pointer to the buffer to return requested
+                                      array of VLAN_TCI.
+
+  @retval EFI_SUCCESS            The array of VLAN_TCI was returned in VlanVariable
+                                 and number of VLAN was returned in NumberOfVlan.
+  @retval EFI_NOT_FOUND          VLAN configuration variable not found.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the configuration.
+
+**/
+EFI_STATUS
+MnpGetVlanVariable (
+  IN     MNP_DEVICE_DATA   *MnpDeviceData,
+     OUT UINTN             *NumberOfVlan,
+     OUT VLAN_TCI          **VlanVariable
+  )
+{
+  UINTN       BufferSize;
+  EFI_STATUS  Status;
+  VLAN_TCI    *Buffer;
+
+  //
+  // Get VLAN configuration from EFI Variable
+  //
+  Buffer = NULL;
+  BufferSize = 0;
+  Status = gRT->GetVariable (
+                  MnpDeviceData->MacString,
+                  &gEfiVlanConfigProtocolGuid,
+                  NULL,
+                  &BufferSize,
+                  NULL
+                  );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Allocate buffer to read the variable
+  //
+  Buffer = AllocateZeroPool (BufferSize);
+  if (Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gRT->GetVariable (
+                  MnpDeviceData->MacString,
+                  &gEfiVlanConfigProtocolGuid,
+                  NULL,
+                  &BufferSize,
+                  Buffer
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Buffer);
+    return Status;
+  }
+
+  *NumberOfVlan = BufferSize / sizeof (VLAN_TCI);
+  *VlanVariable = Buffer;
+
+  return Status;
+}
+
+
+/**
+  Set VLAN configuration variable.
+
+  @param[in] MnpDeviceData       Pointer to the MNP device context data.
+  @param[in] NumberOfVlan        Number of VLAN in array VlanVariable.
+  @param[in] VlanVariable        Pointer to array of VLAN_TCI.
+
+  @retval EFI_SUCCESS            The VLAN variable is successfully set.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.
+
+**/
+EFI_STATUS
+MnpSetVlanVariable (
+  IN MNP_DEVICE_DATA             *MnpDeviceData,
+  IN UINTN                       NumberOfVlan,
+  IN VLAN_TCI                    *VlanVariable
+  )
+{
+  return gRT->SetVariable (
+                MnpDeviceData->MacString,
+                &gEfiVlanConfigProtocolGuid,
+                EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
+                NumberOfVlan * sizeof (VLAN_TCI),
+                VlanVariable
+                );
+}
+
+
+/**
+  Create a VLAN device or modify the configuration parameter of an
+  already-configured VLAN.
+
+  The Set() function is used to create a new VLAN device or change the VLAN
+  configuration parameters. If the VlanId hasn't been configured in the
+  physical Ethernet device, a new VLAN device will be created. If a VLAN with
+  this VlanId is already configured, then related configuration will be updated
+  as the input parameters.
+
+  If VlanId is zero, the VLAN device will send and receive untagged frames.
+  Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
+  If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
+  If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
+  If there is not enough system memory to perform the registration, then
+  EFI_OUT_OF_RESOURCES is returned.
+
+  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in] VlanId              A unique identifier (1-4094) of the VLAN which is being created
+                                 or modified, or zero (0).
+  @param[in] Priority            3 bit priority in VLAN header. Priority 0 is default value. If
+                                 VlanId is zero (0), Priority is ignored.
+
+  @retval EFI_SUCCESS            The VLAN is successfully configured.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - VlanId is an invalid VLAN Identifier.
+                                 - Priority is invalid.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to perform the registration.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigSet (
+  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN UINT16                      VlanId,
+  IN UINT8                       Priority
+  )
+{
+  EFI_STATUS        Status;
+  MNP_DEVICE_DATA   *MnpDeviceData;
+  MNP_SERVICE_DATA  *MnpServiceData;
+  VLAN_TCI          *OldVariable;
+  VLAN_TCI          *NewVariable;
+  UINTN             NumberOfVlan;
+  UINTN             Index;
+  BOOLEAN           IsAdd;
+  LIST_ENTRY        *Entry;
+
+  if ((This == NULL) || (VlanId > 4094) || (Priority > 7)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  IsAdd = FALSE;
+  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
+  if (MnpDeviceData->NumberOfVlan == 0) {
+    //
+    // No existing VLAN, this is the first VLAN to add
+    //
+    IsAdd = TRUE;
+    Entry = GetFirstNode (&MnpDeviceData->ServiceList);
+    MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+
+    if (VlanId != 0) {
+      //
+      // VlanId is not 0, need destroy the default MNP service data
+      //
+      Status = MnpDestroyServiceChild (MnpServiceData);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Status = MnpDestroyServiceData (MnpServiceData);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      //
+      // Create a new MNP service data for this VLAN
+      //
+      MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
+      if (MnpServiceData == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+  } else {
+    //
+    // Try to find VlanId in existing VLAN list
+    //
+    MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
+    if (MnpServiceData == NULL) {
+      //
+      // VlanId not found, create a new MNP service data
+      //
+      IsAdd = TRUE;
+      MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
+      if (MnpServiceData == NULL) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+  }
+
+  MnpServiceData->VlanId = VlanId;
+  MnpServiceData->Priority = Priority;
+  if (IsAdd) {
+    MnpDeviceData->NumberOfVlan++;
+  }
+
+  //
+  // Update VLAN configuration variable
+  //
+  OldVariable  = NULL;
+  NewVariable  = NULL;
+  NumberOfVlan = 0;
+  MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &OldVariable);
+
+  if (IsAdd) {
+    //
+    // VLAN not exist - add
+    //
+    NewVariable = AllocateZeroPool ((NumberOfVlan + 1) * sizeof (VLAN_TCI));
+    if (NewVariable == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    if (OldVariable != NULL) {
+      CopyMem (NewVariable, OldVariable, NumberOfVlan * sizeof (VLAN_TCI));
+    }
+
+    Index = NumberOfVlan++;
+  } else {
+    //
+    // VLAN already exist - update
+    //
+    for (Index = 0; Index < NumberOfVlan; Index++) {
+      if (OldVariable[Index].Bits.Vid == VlanId) {
+        break;
+      }
+    }
+    ASSERT (Index < NumberOfVlan);
+
+    NewVariable = OldVariable;
+    OldVariable = NULL;
+  }
+
+  NewVariable[Index].Bits.Vid      = VlanId;
+  NewVariable[Index].Bits.Priority = Priority;
+
+  Status = MnpSetVlanVariable (MnpDeviceData, NumberOfVlan, NewVariable);
+  FreePool (NewVariable);
+
+Exit:
+  if (OldVariable != NULL) {
+    FreePool (OldVariable);
+  }
+
+  return Status;
+}
+
+
+/**
+  Find configuration information for specified VLAN or all configured VLANs.
+
+  The Find() function is used to find the configuration information for matching
+  VLAN and allocate a buffer into which those entries are copied.
+
+  @param[in]  This               Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in]  VlanId             Pointer to VLAN identifier. Set to NULL to find all
+                                 configured VLANs.
+  @param[out] NumberOfVlan       The number of VLANs which is found by the specified criteria.
+  @param[out] Entries            The buffer which receive the VLAN configuration.
+
+  @retval EFI_SUCCESS            The VLAN is successfully found.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - Specified VlanId is invalid.
+  @retval EFI_NOT_FOUND          No matching VLAN is found.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigFind (
+  IN     EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN     UINT16                      *VlanId OPTIONAL,
+     OUT UINT16                      *NumberOfVlan,
+     OUT EFI_VLAN_FIND_DATA          **Entries
+  )
+{
+  MNP_DEVICE_DATA     *MnpDeviceData;
+  MNP_SERVICE_DATA    *MnpServiceData;
+  LIST_ENTRY          *Entry;
+  EFI_VLAN_FIND_DATA  *VlanData;
+
+  if ((This == NULL) || (VlanId != NULL && *VlanId > 4094) || (NumberOfVlan == NULL) || (Entries == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *NumberOfVlan = 0;
+  *Entries      = NULL;
+
+  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
+  if (MnpDeviceData->NumberOfVlan == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (VlanId == NULL) {
+    //
+    // Return all current VLAN configuration
+    //
+    *NumberOfVlan = (UINT16) MnpDeviceData->NumberOfVlan;
+    VlanData = AllocateZeroPool (*NumberOfVlan * sizeof (EFI_VLAN_FIND_DATA));
+    if (VlanData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    *Entries = VlanData;
+    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
+      MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+
+      VlanData->VlanId = MnpServiceData->VlanId;
+      VlanData->Priority = MnpServiceData->Priority;
+      VlanData++;
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // VlanId is specified, try to find it in current VLAN list
+  //
+  MnpServiceData = MnpFindServiceData (MnpDeviceData, *VlanId);
+  if (MnpServiceData == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
+  if (VlanData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  VlanData->VlanId = MnpServiceData->VlanId;
+  VlanData->Priority = MnpServiceData->Priority;
+
+  *NumberOfVlan = 1;
+  *Entries = VlanData;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Remove the configured VLAN device.
+
+  The Remove() function is used to remove the specified VLAN device.
+  If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
+  If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.
+
+  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in] VlanId              Identifier (0-4094) of the VLAN to be removed.
+
+  @retval EFI_SUCCESS            The VLAN is successfully removed.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - VlanId  is an invalid parameter.
+  @retval EFI_NOT_FOUND          The to-be-removed VLAN does not exist.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigRemove (
+  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN UINT16                      VlanId
+  )
+{
+  EFI_STATUS        Status;
+  MNP_DEVICE_DATA   *MnpDeviceData;
+  MNP_SERVICE_DATA  *MnpServiceData;
+  LIST_ENTRY        *Entry;
+  VLAN_TCI          *VlanVariable;
+  VLAN_TCI          *VlanData;
+
+  if ((This == NULL) || (VlanId > 4094)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
+  if (MnpDeviceData->NumberOfVlan == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Try to find the VlanId
+  //
+  MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
+  if (MnpServiceData == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  MnpDeviceData->NumberOfVlan--;
+
+  if ((VlanId != 0) || (MnpDeviceData->NumberOfVlan != 0)) {
+    //
+    // If VlanId is not 0 or VlanId is 0 and it is not the last VLAN to remove,
+    // destroy its MNP service data
+    //
+    Status = MnpDestroyServiceChild (MnpServiceData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Status = MnpDestroyServiceData (MnpServiceData);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if ((VlanId != 0) && (MnpDeviceData->NumberOfVlan == 0)) {
+    //
+    // This is the last VLAN to be removed, restore the default MNP service data
+    //
+    MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
+    if (MnpServiceData == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  //
+  // Update VLAN configuration variable
+  //
+  VlanVariable = NULL;
+  if (MnpDeviceData->NumberOfVlan != 0) {
+    VlanVariable = AllocatePool (MnpDeviceData->NumberOfVlan * sizeof (VLAN_TCI));
+    if (VlanVariable == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    VlanData = VlanVariable;
+    NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
+      MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
+
+      VlanData->Bits.Vid      = MnpServiceData->VlanId;
+      VlanData->Bits.Priority = MnpServiceData->Priority;
+      VlanData++;
+    }
+  }
+
+  Status = MnpSetVlanVariable (MnpDeviceData, MnpDeviceData->NumberOfVlan, VlanVariable);
+
+  if (VlanVariable != NULL) {
+    FreePool (VlanVariable);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.h b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.h
new file mode 100644
index 0000000000..d8eac8d07a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/MnpDxe/MnpVlan.h
@@ -0,0 +1,212 @@
+/** @file
+  Header file to be included by MnpVlan.c.
+
+Copyright (c) 2009 - 2013, 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<BR>
+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.
+
+**/
+
+#ifndef __MNP_VLAN_H__
+#define __MNP_VLAN_H__
+
+#include "MnpDriver.h"
+
+extern EFI_VLAN_CONFIG_PROTOCOL mVlanConfigProtocolTemplate;
+
+
+/**
+  Create a child handle for the VLAN ID.
+
+  @param[in]       ImageHandle        The driver image handle.
+  @param[in]       ControllerHandle   Handle of device to bind driver to.
+  @param[in]       VlanId             The VLAN ID.
+  @param[out]      Devicepath         Pointer to returned device path for child handle.
+
+  @return The handle of VLAN child or NULL if failed to create VLAN child.
+
+**/
+EFI_HANDLE
+MnpCreateVlanChild (
+  IN     EFI_HANDLE                  ImageHandle,
+  IN     EFI_HANDLE                  ControllerHandle,
+  IN     UINT16                      VlanId,
+     OUT EFI_DEVICE_PATH_PROTOCOL    **Devicepath OPTIONAL
+  );
+
+/**
+  Remove VLAN tag of a packet.
+
+  @param[in, out]  MnpDeviceData      Pointer to the mnp device context data.
+  @param[in, out]  Nbuf               Pointer to the NET_BUF to remove VLAN tag.
+  @param[out]      VlanId             Pointer to the returned VLAN ID.
+
+  @retval TRUE             VLAN tag is removed from this packet.
+  @retval FALSE            There is no VLAN tag in this packet.
+
+**/
+BOOLEAN
+MnpRemoveVlanTag (
+  IN OUT MNP_DEVICE_DATA   *MnpDeviceData,
+  IN OUT NET_BUF           *Nbuf,
+     OUT UINT16            *VlanId
+  );
+
+/**
+  Build the vlan packet to transmit from the TxData passed in.
+
+  @param  MnpServiceData         Pointer to the mnp service context data.
+  @param  TxData                 Pointer to the transmit data containing the
+                                 information to build the packet.
+  @param  ProtocolType           Pointer to the Ethernet protocol type.
+  @param  Packet                 Pointer to record the address of the packet.
+  @param  Length                 Pointer to a UINT32 variable used to record the
+                                 packet's length.
+
+**/
+VOID
+MnpInsertVlanTag (
+  IN     MNP_SERVICE_DATA                    *MnpServiceData,
+  IN     EFI_MANAGED_NETWORK_TRANSMIT_DATA   *TxData,
+     OUT UINT16                              *ProtocolType,
+  IN OUT UINT8                               **Packet,
+  IN OUT UINT32                              *Length
+  );
+
+/**
+  Get VLAN configuration variable.
+
+  @param[in]       MnpDeviceData      Pointer to the MNP device context data.
+  @param[out]      NumberOfVlan       Pointer to number of VLAN to be returned.
+  @param[out]      VlanVariable       Pointer to the buffer to return requested
+                                      array of VLAN_TCI.
+
+  @retval EFI_SUCCESS            The array of VLAN_TCI was returned in VlanVariable
+                                 and number of VLAN was returned in NumberOfVlan.
+  @retval EFI_NOT_FOUND          VLAN configuration variable not found.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the configuration.
+
+**/
+EFI_STATUS
+MnpGetVlanVariable (
+  IN     MNP_DEVICE_DATA   *MnpDeviceData,
+     OUT UINTN             *NumberOfVlan,
+     OUT VLAN_TCI          **VlanVariable
+  );
+
+/**
+  Set VLAN configuration variable.
+
+  @param[in] MnpDeviceData       Pointer to the MNP device context data.
+  @param[in] NumberOfVlan        Number of VLAN in array VlanVariable.
+  @param[in] VlanVariable        Pointer to array of VLAN_TCI.
+
+  @retval EFI_SUCCESS            The VLAN variable is successfully set.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough resource to set the configuration.
+
+**/
+EFI_STATUS
+MnpSetVlanVariable (
+  IN MNP_DEVICE_DATA             *MnpDeviceData,
+  IN UINTN                       NumberOfVlan,
+  IN VLAN_TCI                    *VlanVariable
+  );
+
+/**
+  Create a VLAN device or modify the configuration parameter of an
+  already-configured VLAN.
+
+  The Set() function is used to create a new VLAN device or change the VLAN
+  configuration parameters. If the VlanId hasn't been configured in the
+  physical Ethernet device, a new VLAN device will be created. If a VLAN with
+  this VlanId is already configured, then related configuration will be updated
+  as the input parameters.
+
+  If VlanId is zero, the VLAN device will send and receive untagged frames.
+  Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
+  If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
+  If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
+  If there is not enough system memory to perform the registration, then
+  EFI_OUT_OF_RESOURCES is returned.
+
+  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in] VlanId              A unique identifier (1-4094) of the VLAN which is being created
+                                 or modified, or zero (0).
+  @param[in] Priority            3 bit priority in VLAN header. Priority 0 is default value. If
+                                 VlanId is zero (0), Priority is ignored.
+
+  @retval EFI_SUCCESS            The VLAN is successfully configured.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - VlanId is an invalid VLAN Identifier.
+                                 - Priority is invalid.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to perform the registration.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigSet (
+  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN UINT16                      VlanId,
+  IN UINT8                       Priority
+  );
+
+/**
+  Find configuration information for specified VLAN or all configured VLANs.
+
+  The Find() function is used to find the configuration information for matching
+  VLAN and allocate a buffer into which those entries are copied.
+
+  @param[in]  This               Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in]  VlanId             Pointer to VLAN identifier. Set to NULL to find all
+                                 configured VLANs.
+  @param[out] NumberOfVlan       The number of VLANs which is found by the specified criteria.
+  @param[out] Entries            The buffer which receive the VLAN configuration.
+
+  @retval EFI_SUCCESS            The VLAN is successfully found.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - Specified VlanId is invalid.
+  @retval EFI_NOT_FOUND          No matching VLAN is found.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigFind (
+  IN     EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN     UINT16                      *VlanId OPTIONAL,
+     OUT UINT16                      *NumberOfVlan,
+     OUT EFI_VLAN_FIND_DATA          **Entries
+  );
+
+/**
+  Remove the configured VLAN device.
+
+  The Remove() function is used to remove the specified VLAN device.
+  If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
+  If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.
+
+  @param[in] This                Points to the EFI_VLAN_CONFIG_PROTOCOL.
+  @param[in] VlanId              Identifier (0-4094) of the VLAN to be removed.
+
+  @retval EFI_SUCCESS            The VLAN is successfully removed.
+  @retval EFI_INVALID_PARAMETER  One or more of following conditions is TRUE:
+                                 - This is NULL.
+                                 - VlanId  is an invalid parameter.
+  @retval EFI_NOT_FOUND          The to-be-removed VLAN does not exist.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigRemove (
+  IN EFI_VLAN_CONFIG_PROTOCOL    *This,
+  IN UINT16                      VlanId
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/ComponentName.c
new file mode 100644
index 0000000000..901c2df064
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/ComponentName.c
@@ -0,0 +1,431 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for Mtftp4Dxe driver.
+  
+Copyright (c) 2006 - 2012, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL               *This,
+  IN     EFI_HANDLE                                ControllerHandle,
+  IN     EFI_HANDLE                                ChildHandle        OPTIONAL,
+  IN     CHAR8                                     *Language,
+     OUT CHAR16                                    **ControllerName
+  );
+
+
+///
+/// EFI Component Name Protocol
+///
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gMtftp4ComponentName = {
+  Mtftp4ComponentNameGetDriverName,
+  Mtftp4ComponentNameGetControllerName,
+  "eng"
+};
+
+///
+/// EFI Component Name 2 Protocol
+///
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gMtftp4ComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) Mtftp4ComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) Mtftp4ComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mMtftp4DriverNameTable[] = {
+  {
+    "eng;en",
+    L"MTFTP4 Network Service"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE *gMtftp4ControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mMtftp4DriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gMtftp4ComponentName)
+           );
+}
+
+/**
+  Update the component name for the Mtftp4 child handle.
+
+  @param  Mtftp4[in]                A pointer to the EFI_MTFTP4_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS               Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER     The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN   EFI_MTFTP4_PROTOCOL             *Mtftp4
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[80];
+  EFI_MTFTP4_MODE_DATA             ModeData;
+
+  if (Mtftp4 == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Format the child name into the string buffer as:
+  // MTFTPv4 (ServerIp=192.168.1.10, ServerPort=69)
+  //
+  Status = Mtftp4->GetModeData (Mtftp4, &ModeData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UnicodeSPrint (HandleName, sizeof (HandleName),
+    L"MTFTPv4 (ServerIp=%d.%d.%d.%d, ServerPort=%d)",
+    ModeData.ConfigData.ServerIp.Addr[0],
+    ModeData.ConfigData.ServerIp.Addr[1],
+    ModeData.ConfigData.ServerIp.Addr[2],
+    ModeData.ConfigData.ServerIp.Addr[3],
+    ModeData.ConfigData.InitialServerPort
+    );
+
+  if (gMtftp4ControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gMtftp4ControllerNameTable);
+    gMtftp4ControllerNameTable = NULL;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gMtftp4ComponentName.SupportedLanguages,
+             &gMtftp4ControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gMtftp4ComponentName2.SupportedLanguages,
+           &gMtftp4ControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL               *This,
+  IN     EFI_HANDLE                                ControllerHandle,
+  IN     EFI_HANDLE                                ChildHandle        OPTIONAL,
+  IN     CHAR8                                     *Language,
+     OUT CHAR16                                    **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_MTFTP4_PROTOCOL           *Mtftp4;
+
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiUdp4ProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiMtftp4ProtocolGuid,
+                  (VOID **)&Mtftp4,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Mtftp4);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gMtftp4ControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gMtftp4ComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.c
new file mode 100644
index 0000000000..a23d405baa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.c
@@ -0,0 +1,723 @@
+/** @file
+  Implementation of Mtftp drivers.
+
+Copyright (c) 2006 - 2012, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL   gMtftp4DriverBinding = {
+  Mtftp4DriverBindingSupported,
+  Mtftp4DriverBindingStart,
+  Mtftp4DriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_SERVICE_BINDING_PROTOCOL  gMtftp4ServiceBindingTemplete = {
+  Mtftp4ServiceBindingCreateChild,
+  Mtftp4ServiceBindingDestroyChild
+};
+
+
+/**
+  The driver entry point which installs multiple protocols to the ImageHandle.
+
+  @param ImageHandle    The MTFTP's image handle.
+  @param SystemTable    The system table.
+
+  @retval EFI_SUCCESS  The handles are successfully installed on the image.
+  @retval others       some EFI_ERROR occured.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverEntryPoint (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gMtftp4DriverBinding,
+           ImageHandle,
+           &gMtftp4ComponentName,
+           &gMtftp4ComponentName2
+           );
+}
+
+
+/**
+  Test whether MTFTP driver support this controller.
+
+  @param  This                   The MTFTP driver binding instance
+  @param  Controller             The controller to test
+  @param  RemainingDevicePath    The remaining device path
+
+  @retval EFI_SUCCESS            The controller has UDP service binding protocol
+                                 installed, MTFTP can support it.
+  @retval EFI_ALREADY_STARTED    The device specified by ControllerHandle and
+                                 RemainingDevicePath is already being managed by
+                                 the driver specified by This.
+  @retval EFI_ACCESS_DENIED      The device specified by ControllerHandle and
+                                 RemainingDevicePath is already being managed by a
+                                 different driver or an application that requires
+                                 exclusive access.
+  @retval EFI_UNSUPPORTED        The device specified by ControllerHandle and
+                                 RemainingDevicePath is not supported by the driver
+                                 specified by This.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiUdp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+
+/**
+  Config a NULL UDP that is used to keep the connection between UDP and MTFTP.
+
+  Just leave the Udp child unconfigured. When UDP is unloaded,
+    MTFTP will be informed with DriverBinding Stop.
+
+  @param  UdpIo                  The UDP_IO to configure
+  @param  Context                The opaque parameter to the callback
+
+  @retval EFI_SUCCESS            It always return EFI_SUCCESS directly.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ConfigNullUdp (
+  IN UDP_IO                 *UdpIo,
+  IN VOID                   *Context
+  )
+{
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create then initialize a MTFTP service binding instance.
+
+  @param  Controller             The controller to install the MTFTP service
+                                 binding on
+  @param  Image                  The driver binding image of the MTFTP driver
+  @param  Service                The variable to receive the created service
+                                 binding instance.
+
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource to create the instance
+  @retval EFI_DEVICE_ERROR       Failed to create a NULL UDP port to keep
+                                 connection  with UDP.
+  @retval EFI_SUCCESS            The service instance is created for the
+                                 controller.
+
+**/
+EFI_STATUS
+Mtftp4CreateService (
+  IN     EFI_HANDLE            Controller,
+  IN     EFI_HANDLE            Image,
+     OUT MTFTP4_SERVICE        **Service
+  )
+{
+  MTFTP4_SERVICE            *MtftpSb;
+  EFI_STATUS                Status;
+
+  *Service  = NULL;
+  MtftpSb   = AllocatePool (sizeof (MTFTP4_SERVICE));
+
+  if (MtftpSb == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  MtftpSb->Signature      = MTFTP4_SERVICE_SIGNATURE;
+  MtftpSb->ServiceBinding = gMtftp4ServiceBindingTemplete;
+  MtftpSb->ChildrenNum    = 0;
+  InitializeListHead (&MtftpSb->Children);
+
+  MtftpSb->Timer          = NULL;
+  MtftpSb->TimerToGetMap  = NULL;
+  MtftpSb->Controller     = Controller;
+  MtftpSb->Image          = Image;
+  MtftpSb->ConnectUdp     = NULL;
+
+  //
+  // Create the timer and a udp to be notified when UDP is uninstalled
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL | EVT_TIMER,
+                  TPL_CALLBACK,
+                  Mtftp4OnTimerTick,
+                  MtftpSb,
+                  &MtftpSb->Timer
+                  );
+
+  if (EFI_ERROR (Status)) {
+    FreePool (MtftpSb);
+    return Status;
+  }
+
+  //
+  // Create the timer used to time out the procedure which is used to
+  // get the default IP address.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &MtftpSb->TimerToGetMap
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseEvent (MtftpSb->Timer);
+    FreePool (MtftpSb);
+    return Status;
+  }
+
+  MtftpSb->ConnectUdp = UdpIoCreateIo (
+                          Controller,
+                          Image,
+                          Mtftp4ConfigNullUdp,
+                          UDP_IO_UDP4_VERSION,
+                          NULL
+                          );
+
+  if (MtftpSb->ConnectUdp == NULL) {
+    gBS->CloseEvent (MtftpSb->TimerToGetMap);
+    gBS->CloseEvent (MtftpSb->Timer);
+    FreePool (MtftpSb);
+    return EFI_DEVICE_ERROR;
+  }
+
+  *Service = MtftpSb;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Release all the resource used the MTFTP service binding instance.
+
+  @param  MtftpSb                The MTFTP service binding instance.
+
+**/
+VOID
+Mtftp4CleanService (
+  IN MTFTP4_SERVICE     *MtftpSb
+  )
+{
+  UdpIoFreeIo (MtftpSb->ConnectUdp);
+  gBS->CloseEvent (MtftpSb->TimerToGetMap);
+  gBS->CloseEvent (MtftpSb->Timer);
+}
+
+
+/**
+  Start the MTFTP driver on this controller.
+
+  MTFTP driver will install a MTFTP SERVICE BINDING protocol on the supported
+  controller, which can be used to create/destroy MTFTP children.
+
+  @param  This                   The MTFTP driver binding protocol.
+  @param  Controller             The controller to manage.
+  @param  RemainingDevicePath    Remaining device path.
+
+  @retval EFI_ALREADY_STARTED    The MTFTP service binding protocol has been
+                                 started  on the controller.
+  @retval EFI_SUCCESS            The MTFTP service binding is installed on the
+                                 controller.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  )
+{
+  MTFTP4_SERVICE            *MtftpSb;
+  EFI_STATUS                Status;
+
+  //
+  // Directly return if driver is already running.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiMtftp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (Status == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  Status = Mtftp4CreateService (Controller, This->DriverBindingHandle, &MtftpSb);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (MtftpSb != NULL);
+
+  Status = gBS->SetTimer (MtftpSb->Timer, TimerPeriodic, TICKS_PER_SECOND);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Install the Mtftp4ServiceBinding Protocol onto Controller
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Controller,
+                  &gEfiMtftp4ServiceBindingProtocolGuid,
+                  &MtftpSb->ServiceBinding,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  Mtftp4CleanService (MtftpSb);
+  FreePool (MtftpSb);
+
+  return Status;
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DestroyChildEntryInHandleBuffer (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  MTFTP4_PROTOCOL               *Instance;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = NET_LIST_USER_STRUCT_S (Entry, MTFTP4_PROTOCOL, Link, MTFTP4_PROTOCOL_SIGNATURE);
+  ServiceBinding    = ((MTFTP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ServiceBinding;
+  NumberOfChildren  = ((MTFTP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->NumberOfChildren;
+  ChildHandleBuffer = ((MTFTP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ChildHandleBuffer;
+
+  if (!NetIsInHandleBuffer (Instance->Handle, NumberOfChildren, ChildHandleBuffer)) {
+    return EFI_SUCCESS;
+  }
+
+  return ServiceBinding->DestroyChild (ServiceBinding, Instance->Handle);
+}
+
+/**
+  Stop the MTFTP driver on controller. The controller is a UDP
+  child handle.
+
+  @param  This                   The MTFTP driver binding protocol
+  @param  Controller             The controller to stop
+  @param  NumberOfChildren       The number of children
+  @param  ChildHandleBuffer      The array of the child handle.
+
+  @retval EFI_SUCCESS            The driver is stopped on the controller.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver on the controller.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  )
+{
+  EFI_SERVICE_BINDING_PROTOCOL               *ServiceBinding;
+  MTFTP4_SERVICE                             *MtftpSb;
+  EFI_HANDLE                                 NicHandle;
+  EFI_STATUS                                 Status;
+  LIST_ENTRY                                 *List;
+  MTFTP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT Context;
+
+  //
+  // MTFTP driver opens UDP child, So, Controller is a UDP
+  // child handle. Locate the Nic handle first. Then get the
+  // MTFTP private data back.
+  //
+  NicHandle = NetLibGetNicHandle (Controller, &gEfiUdp4ProtocolGuid);
+
+  if (NicHandle == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiMtftp4ServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  NicHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  MtftpSb = MTFTP4_SERVICE_FROM_THIS (ServiceBinding);
+
+  if (!IsListEmpty (&MtftpSb->Children)) {
+    //
+    // Destroy the Mtftp4 child instance in ChildHandleBuffer.
+    //
+    List = &MtftpSb->Children;
+    Context.ServiceBinding    = ServiceBinding;
+    Context.NumberOfChildren  = NumberOfChildren;
+    Context.ChildHandleBuffer = ChildHandleBuffer;
+    Status = NetDestroyLinkList (
+               List,
+               Mtftp4DestroyChildEntryInHandleBuffer,
+               &Context,
+               NULL
+               );
+  }
+
+  if (NumberOfChildren == 0 && IsListEmpty (&MtftpSb->Children)) {
+    gBS->UninstallProtocolInterface (
+           NicHandle,
+           &gEfiMtftp4ServiceBindingProtocolGuid,
+           ServiceBinding
+           );
+
+    Mtftp4CleanService (MtftpSb);
+    if (gMtftp4ControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gMtftp4ControllerNameTable);
+      gMtftp4ControllerNameTable = NULL;
+    }
+    FreePool (MtftpSb);
+
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+
+/**
+  Initialize a MTFTP protocol instance which is the child of MtftpSb.
+
+  @param  MtftpSb                The MTFTP service binding protocol.
+  @param  Instance               The MTFTP instance to initialize.
+
+**/
+VOID
+Mtftp4InitProtocol (
+  IN     MTFTP4_SERVICE         *MtftpSb,
+     OUT MTFTP4_PROTOCOL        *Instance
+  )
+{
+  ZeroMem (Instance, sizeof (MTFTP4_PROTOCOL));
+
+  Instance->Signature = MTFTP4_PROTOCOL_SIGNATURE;
+  InitializeListHead (&Instance->Link);
+  CopyMem (&Instance->Mtftp4, &gMtftp4ProtocolTemplate, sizeof (Instance->Mtftp4));
+  Instance->State     = MTFTP4_STATE_UNCONFIGED;
+  Instance->Service   = MtftpSb;
+
+  InitializeListHead (&Instance->Blocks);
+}
+
+
+/**
+  Create a MTFTP child for the service binding instance, then
+  install the MTFTP protocol to the ChildHandle.
+
+  @param  This                   The MTFTP service binding instance.
+  @param  ChildHandle            The Child handle to install the MTFTP protocol.
+
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource for the new child.
+  @retval EFI_SUCCESS            The child is successfully create.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                *ChildHandle
+  )
+{
+  MTFTP4_SERVICE            *MtftpSb;
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+  VOID                      *Udp4;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = AllocatePool (sizeof (*Instance));
+
+  if (Instance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  MtftpSb = MTFTP4_SERVICE_FROM_THIS (This);
+
+  Mtftp4InitProtocol (MtftpSb, Instance);
+
+  Instance->UnicastPort = UdpIoCreateIo (
+                            MtftpSb->Controller,
+                            MtftpSb->Image,
+                            Mtftp4ConfigNullUdp,
+                            UDP_IO_UDP4_VERSION,
+                            Instance
+                            );
+
+  if (Instance->UnicastPort == NULL) {
+    FreePool (Instance);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Install the MTFTP protocol onto ChildHandle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiMtftp4ProtocolGuid,
+                  &Instance->Mtftp4,
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    UdpIoFreeIo (Instance->UnicastPort);
+    FreePool (Instance);
+    return Status;
+  }
+
+  Instance->Handle  = *ChildHandle;
+
+  //
+  // Open the Udp4 protocol BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  MtftpSb->ConnectUdp->UdpHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Udp4,
+                  gMtftp4DriverBinding.DriverBindingHandle,
+                  Instance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Open the Udp4 protocol by child.
+  //
+  Status = gBS->OpenProtocol (
+                  Instance->UnicastPort->UdpHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Udp4,
+                  gMtftp4DriverBinding.DriverBindingHandle,
+                  Instance->Handle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Close the Udp4 protocol.
+    //
+    gBS->CloseProtocol (
+           MtftpSb->ConnectUdp->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           gMtftp4DriverBinding.DriverBindingHandle,
+           ChildHandle
+           );
+    goto ON_ERROR;
+  }
+
+  //
+  // Add it to the parent's child list.
+  //
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  InsertTailList (&MtftpSb->Children, &Instance->Link);
+  MtftpSb->ChildrenNum++;
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+  if (Instance->Handle != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Instance->Handle,
+           &gEfiMtftp4ProtocolGuid,
+           &Instance->Mtftp4,
+           NULL
+           );
+  }
+
+  UdpIoFreeIo (Instance->UnicastPort);
+  FreePool (Instance);
+
+  return Status;
+}
+
+
+/**
+  Destroy one of the service binding's child.
+
+  @param  This                   The service binding instance
+  @param  ChildHandle            The child handle to destroy
+
+  @retval EFI_INVALID_PARAMETER  The parameter is invaid.
+  @retval EFI_UNSUPPORTED        The child may have already been destroyed.
+  @retval EFI_SUCCESS            The child is destroyed and removed from the
+                                 parent's child list.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                   ChildHandle
+  )
+{
+  MTFTP4_SERVICE            *MtftpSb;
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_PROTOCOL       *Mtftp4;
+  EFI_STATUS                Status;
+  EFI_TPL                   OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Retrieve the private context data structures
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiMtftp4ProtocolGuid,
+                  (VOID **) &Mtftp4,
+                  gMtftp4DriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance  = MTFTP4_PROTOCOL_FROM_THIS (Mtftp4);
+  MtftpSb   = MTFTP4_SERVICE_FROM_THIS (This);
+
+  if (Instance->Service != MtftpSb) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Instance->InDestroy) {
+    return EFI_SUCCESS;
+  }
+
+  Instance->InDestroy = TRUE;
+
+  //
+  // Close the Udp4 protocol.
+  //
+  gBS->CloseProtocol (
+         MtftpSb->ConnectUdp->UdpHandle,
+         &gEfiUdp4ProtocolGuid,
+         gMtftp4DriverBinding.DriverBindingHandle,
+         ChildHandle
+         );
+
+  gBS->CloseProtocol (
+         Instance->UnicastPort->UdpHandle,
+         &gEfiUdp4ProtocolGuid,
+         gMtftp4DriverBinding.DriverBindingHandle,
+         ChildHandle
+         );
+
+  if (Instance->McastUdpPort != NULL) {
+    gBS->CloseProtocol (
+           Instance->McastUdpPort->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           gMtftp4DriverBinding.DriverBindingHandle,
+           ChildHandle
+           );  
+  }
+
+  //
+  // Uninstall the MTFTP4 protocol first to enable a top down destruction.
+  //
+  Status = gBS->UninstallProtocolInterface (
+                  ChildHandle,
+                  &gEfiMtftp4ProtocolGuid,
+                  Mtftp4
+                  );
+
+  if (EFI_ERROR (Status)) {
+    Instance->InDestroy = FALSE;
+    return Status;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Mtftp4CleanOperation (Instance, EFI_DEVICE_ERROR);
+  UdpIoFreeIo (Instance->UnicastPort);
+
+  RemoveEntryList (&Instance->Link);
+  MtftpSb->ChildrenNum--;
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (Instance);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.h b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.h
new file mode 100644
index 0000000000..936eab1ba1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Driver.h
@@ -0,0 +1,137 @@
+/** @file
+  Mtftp drivers function header.
+  
+Copyright (c) 2006 - 2012, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef __EFI_MTFTP4_DRIVER_H__
+#define __EFI_MTFTP4_DRIVER_H__
+
+#include <Uefi.h>
+
+#include <Protocol/ServiceBinding.h>
+
+#include <Library/NetLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+extern EFI_COMPONENT_NAME_PROTOCOL   gMtftp4ComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL  gMtftp4ComponentName2;
+extern EFI_DRIVER_BINDING_PROTOCOL   gMtftp4DriverBinding;
+extern EFI_UNICODE_STRING_TABLE      *gMtftp4ControllerNameTable;
+
+/**
+  Test whether MTFTP driver support this controller.
+
+  @param  This                   The MTFTP driver binding instance
+  @param  Controller             The controller to test
+  @param  RemainingDevicePath    The remaining device path
+
+  @retval EFI_SUCCESS            The controller has UDP service binding protocol
+                                 installed, MTFTP can support it.
+  @retval Others                 MTFTP can't support the controller.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  );
+
+/**
+  Start the MTFTP driver on this controller. 
+  
+  MTFTP driver will install a MTFTP SERVICE BINDING protocol on the supported
+  controller, which can be used to create/destroy MTFTP children.
+
+  @param  This                   The MTFTP driver binding protocol.
+  @param  Controller             The controller to manage.
+  @param  RemainingDevicePath    Remaining device path.
+
+  @retval EFI_ALREADY_STARTED    The MTFTP service binding protocol has been
+                                 started  on the controller.
+  @retval EFI_SUCCESS            The MTFTP service binding is installed on the
+                                 controller.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath
+  );
+
+/**
+  Stop the MTFTP driver on controller. The controller is a UDP
+  child handle.
+
+  @param  This                   The MTFTP driver binding protocol
+  @param  Controller             The controller to stop
+  @param  NumberOfChildren       The number of children
+  @param  ChildHandleBuffer      The array of the child handle.
+
+  @retval EFI_SUCCESS            The driver is stopped on the controller.
+  @retval EFI_DEVICE_ERROR       Failed to stop the driver on the controller.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4DriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                  Controller,
+  IN UINTN                       NumberOfChildren,
+  IN EFI_HANDLE                  *ChildHandleBuffer
+  );
+
+/**
+  Create a MTFTP child for the service binding instance, then
+  install the MTFTP protocol to the ChildHandle.
+
+  @param  This                   The MTFTP service binding instance.
+  @param  ChildHandle            The Child handle to install the MTFTP protocol.
+
+  @retval EFI_INVALID_PARAMETER  The parameter is invalid.
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate resource for the new child.
+  @retval EFI_SUCCESS            The child is successfully create.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                *ChildHandle
+  );
+
+/**
+  Destroy one of the service binding's child.
+
+  @param  This                   The service binding instance
+  @param  ChildHandle            The child handle to destroy
+
+  @retval EFI_INVALID_PARAMETER  The parameter is invaid.
+  @retval EFI_UNSUPPORTED        The child may have already been destroyed.
+  @retval EFI_SUCCESS            The child is destroyed and removed from the
+                                 parent's child list.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL *This,
+  IN EFI_HANDLE                   ChildHandle
+  );
+
+
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.inf b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.inf
new file mode 100644
index 0000000000..3d7c8c00e0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.inf
@@ -0,0 +1,75 @@
+## @file
+#  This module produces EFI MTFTPv4 Protocol and EFI MTFTPv4 Service Binding Protocol.
+#
+#  This module produces EFI MTFTPv4 Protocol upon EFI UDPv4 Protocol, to provide
+#  basic services for client-side unicast and/or multicase TFTP operations.
+#
+#  Copyright (c) 2007 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Mtftp4Dxe
+  MODULE_UNI_FILE                = Mtftp4Dxe.uni
+  FILE_GUID                      = DC3641B8-2FA8-4ed3-BC1F-F9962A03454B
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = Mtftp4DriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gMtftp4DriverBinding                        
+#  COMPONENT_NAME                =  gMtftp4ComponentName
+#  COMPONENT_NAME2               =  gMtftp4ComponentName2
+#
+
+[Sources]
+  Mtftp4Option.c
+  Mtftp4Rrq.c
+  Mtftp4Impl.h
+  ComponentName.c
+  Mtftp4Support.c
+  Mtftp4Impl.c
+  Mtftp4Option.h
+  Mtftp4Support.h
+  Mtftp4Driver.h
+  Mtftp4Driver.c
+  Mtftp4Wrq.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  NetLib
+  UdpIoLib
+  MemoryAllocationLib
+  BaseMemoryLib
+
+
+[Protocols]
+  gEfiMtftp4ServiceBindingProtocolGuid          ## BY_START
+  gEfiUdp4ServiceBindingProtocolGuid            ## TO_START
+  gEfiMtftp4ProtocolGuid                        ## BY_START
+  gEfiUdp4ProtocolGuid                          ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  Mtftp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.uni b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.uni
new file mode 100644
index 0000000000..13c4a6c825
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4DxeExtra.uni
new file mode 100644
index 0000000000..8d2508c774
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.c
new file mode 100644
index 0000000000..7ed2e5250f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.c
@@ -0,0 +1,1107 @@
+/** @file
+  Interface routine for Mtftp4.
+  
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2014, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+
+/**
+  Clean up the MTFTP session to get ready for new operation.
+
+  @param  Instance               The MTFTP session to clean up
+  @param  Result                 The result to return to the caller who initiated
+                                 the operation.
+**/
+VOID
+Mtftp4CleanOperation (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN     EFI_STATUS             Result
+  )
+{
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  MTFTP4_BLOCK_RANGE        *Block;
+  EFI_MTFTP4_TOKEN          *Token;
+
+  //
+  // Free various resources.
+  //
+  Token = Instance->Token;
+
+  if (Token != NULL) {
+    Token->Status = Result;
+
+    if (Token->Event != NULL) {
+      gBS->SignalEvent (Token->Event);
+    }
+
+    Instance->Token = NULL;
+  }
+
+  ASSERT (Instance->UnicastPort != NULL);
+  UdpIoCleanIo (Instance->UnicastPort);
+
+  if (Instance->LastPacket != NULL) {
+    NetbufFree (Instance->LastPacket);
+    Instance->LastPacket = NULL;
+  }
+
+  if (Instance->McastUdpPort != NULL) {
+    gBS->CloseProtocol (
+           Instance->McastUdpPort->UdpHandle,
+           &gEfiUdp4ProtocolGuid,
+           gMtftp4DriverBinding.DriverBindingHandle,
+           Instance->Handle
+           );
+    UdpIoFreeIo (Instance->McastUdpPort);
+    Instance->McastUdpPort = NULL;
+  }
+
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &Instance->Blocks) {
+    Block = NET_LIST_USER_STRUCT (Entry, MTFTP4_BLOCK_RANGE, Link);
+    RemoveEntryList (Entry);
+    FreePool (Block);
+  }
+
+  ZeroMem (&Instance->RequestOption, sizeof (MTFTP4_OPTION));
+
+  Instance->Operation     = 0;
+
+  Instance->BlkSize       = MTFTP4_DEFAULT_BLKSIZE;
+  Instance->LastBlock     = 0;
+  Instance->ServerIp      = 0;
+  Instance->ListeningPort = 0;
+  Instance->ConnectedPort = 0;
+  Instance->Gateway       = 0;
+  Instance->PacketToLive  = 0;
+  Instance->MaxRetry      = 0;
+  Instance->CurRetry      = 0;
+  Instance->Timeout       = 0;
+  Instance->McastIp       = 0;
+  Instance->McastPort     = 0;
+  Instance->Master        = TRUE;
+}
+
+
+/**
+  Check packet for GetInfo. 
+  
+  GetInfo is implemented with EfiMtftp4ReadFile. It use Mtftp4GetInfoCheckPacket 
+  to inspect the first packet from server, then abort the session.
+
+  @param  This                   The MTFTP4 protocol instance
+  @param  Token                  The user's token
+  @param  PacketLen              The length of the packet
+  @param  Packet                 The received packet.
+
+  @retval EFI_ABORTED            Abort the ReadFile operation and return.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4GetInfoCheckPacket (
+  IN EFI_MTFTP4_PROTOCOL    *This,
+  IN EFI_MTFTP4_TOKEN       *Token,
+  IN UINT16                 PacketLen,
+  IN EFI_MTFTP4_PACKET      *Packet
+  )
+{
+  MTFTP4_GETINFO_STATE      *State;
+  EFI_STATUS                Status;
+  UINT16                    OpCode;
+  EFI_MTFTP4_ERROR_HEADER  *ErrorHeader;
+
+  State   = (MTFTP4_GETINFO_STATE *) Token->Context;
+  OpCode   = NTOHS (Packet->OpCode);
+
+  //
+  // Set the GetInfo's return status according to the OpCode.
+  //
+  switch (OpCode) {
+  case EFI_MTFTP4_OPCODE_ERROR:
+    ErrorHeader = (EFI_MTFTP4_ERROR_HEADER *) Packet;
+    if (ErrorHeader->ErrorCode == EFI_MTFTP4_ERRORCODE_FILE_NOT_FOUND) {
+      DEBUG ((EFI_D_ERROR, "TFTP error code 1 (File Not Found)\n"));
+    } else {
+      DEBUG ((EFI_D_ERROR, "TFTP error code %d\n", ErrorHeader->ErrorCode));
+    }
+    State->Status = EFI_TFTP_ERROR;
+    break;
+
+  case EFI_MTFTP4_OPCODE_OACK:
+    State->Status = EFI_SUCCESS;
+    break;
+
+  default:
+    State->Status = EFI_PROTOCOL_ERROR;
+  }
+
+  //
+  // Allocate buffer then copy the packet over. Use gBS->AllocatePool
+  // in case AllocatePool will implements something tricky.
+  //
+  Status = gBS->AllocatePool (EfiBootServicesData, PacketLen, (VOID **) State->Packet);
+
+  if (EFI_ERROR (Status)) {
+    State->Status = EFI_OUT_OF_RESOURCES;
+    return EFI_ABORTED;
+  }
+
+  *(State->PacketLen) = PacketLen;
+  CopyMem (*(State->Packet), Packet, PacketLen);
+
+  return EFI_ABORTED;
+}
+
+
+/**
+  Check whether the override data is valid. 
+  
+  It will first validate whether the server is a valid unicast. If a gateway
+  is provided in the Override, it also check that it is a unicast on the 
+  connected network.
+
+  @param  Instance               The MTFTP instance
+  @param  Override               The override data to validate.
+
+  @retval TRUE                   The override data is valid
+  @retval FALSE                  The override data is invalid
+  
+**/
+BOOLEAN
+Mtftp4OverrideValid (
+  IN MTFTP4_PROTOCOL          *Instance,
+  IN EFI_MTFTP4_OVERRIDE_DATA *Override
+  )
+{
+  EFI_MTFTP4_CONFIG_DATA    *Config;
+  IP4_ADDR                  Ip;
+  IP4_ADDR                  Netmask;
+  IP4_ADDR                  Gateway;
+
+  CopyMem (&Ip, &Override->ServerIp, sizeof (IP4_ADDR));
+  if (!NetIp4IsUnicast (NTOHL (Ip), 0)) {
+    return FALSE;
+  }
+
+  Config = &Instance->Config;
+
+  CopyMem (&Gateway, &Override->GatewayIp, sizeof (IP4_ADDR));
+  Gateway = NTOHL (Gateway);
+
+  if (!Config->UseDefaultSetting && (Gateway != 0)) {
+    CopyMem (&Netmask, &Config->SubnetMask, sizeof (IP4_ADDR));
+    CopyMem (&Ip, &Config->StationIp, sizeof (IP4_ADDR));
+
+    Netmask = NTOHL (Netmask);
+    Ip      = NTOHL (Ip);
+
+    if (!NetIp4IsUnicast (Gateway, Netmask) || !IP4_NET_EQUAL (Gateway, Ip, Netmask)) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Poll the UDP to get the IP4 default address, which may be retrieved
+  by DHCP. 
+  
+  The default time out value is 5 seconds. If IP has retrieved the default address, 
+  the UDP is reconfigured.
+
+  @param  Instance               The Mtftp instance
+  @param  UdpIo                  The UDP_IO to poll
+  @param  UdpCfgData             The UDP configure data to reconfigure the UDP_IO
+
+  @retval TRUE                   The default address is retrieved and UDP is reconfigured.
+  @retval FALSE                  Some error occured.
+
+**/
+BOOLEAN
+Mtftp4GetMapping (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UDP_IO                 *UdpIo,
+  IN EFI_UDP4_CONFIG_DATA   *UdpCfgData
+  )
+{
+  MTFTP4_SERVICE            *Service;
+  EFI_IP4_MODE_DATA         Ip4Mode;
+  EFI_UDP4_PROTOCOL         *Udp;
+  EFI_STATUS                Status;
+
+  ASSERT (Instance->Config.UseDefaultSetting);
+
+  Service = Instance->Service;
+  Udp     = UdpIo->Protocol.Udp4;
+
+  Status = gBS->SetTimer (
+                  Service->TimerToGetMap,
+                  TimerRelative,
+                  MTFTP4_TIME_TO_GETMAP * TICKS_PER_SECOND
+                  );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  while (!EFI_ERROR (gBS->CheckEvent (Service->TimerToGetMap))) {
+    Udp->Poll (Udp);
+
+    if (!EFI_ERROR (Udp->GetModeData (Udp, NULL, &Ip4Mode, NULL, NULL)) &&
+        Ip4Mode.IsConfigured) {
+
+      Udp->Configure (Udp, NULL);
+      return (BOOLEAN) (Udp->Configure (Udp, UdpCfgData) == EFI_SUCCESS);
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Configure the UDP port for unicast receiving.
+
+  @param  UdpIo                  The UDP_IO instance
+  @param  Instance               The MTFTP session
+
+  @retval EFI_SUCCESS            The UDP port is successfully configured for the
+                                 session to unicast receive.
+
+**/
+EFI_STATUS
+Mtftp4ConfigUnicastPort (
+  IN UDP_IO                 *UdpIo,
+  IN MTFTP4_PROTOCOL        *Instance
+  )
+{
+  EFI_MTFTP4_CONFIG_DATA    *Config;
+  EFI_UDP4_CONFIG_DATA      UdpConfig;
+  EFI_STATUS                Status;
+  IP4_ADDR                  Ip;
+
+  Config = &Instance->Config;
+
+  UdpConfig.AcceptBroadcast    = FALSE;
+  UdpConfig.AcceptPromiscuous  = FALSE;
+  UdpConfig.AcceptAnyPort      = FALSE;
+  UdpConfig.AllowDuplicatePort = FALSE;
+  UdpConfig.TypeOfService      = 0;
+  UdpConfig.TimeToLive         = 64;
+  UdpConfig.DoNotFragment      = FALSE;
+  UdpConfig.ReceiveTimeout     = 0;
+  UdpConfig.TransmitTimeout    = 0;
+  UdpConfig.UseDefaultAddress  = Config->UseDefaultSetting;
+  IP4_COPY_ADDRESS (&UdpConfig.StationAddress, &Config->StationIp);
+  IP4_COPY_ADDRESS (&UdpConfig.SubnetMask, &Config->SubnetMask);
+  UdpConfig.StationPort        = 0;
+  UdpConfig.RemotePort         = 0;
+
+  Ip = HTONL (Instance->ServerIp);
+  IP4_COPY_ADDRESS (&UdpConfig.RemoteAddress, &Ip);
+
+  Status = UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, &UdpConfig);
+
+  if ((Status == EFI_NO_MAPPING) && Mtftp4GetMapping (Instance, UdpIo, &UdpConfig)) {
+    return EFI_SUCCESS;
+  }
+
+  if (!Config->UseDefaultSetting && !EFI_IP4_EQUAL (&mZeroIp4Addr, &Config->GatewayIp)) {
+    //
+    // The station IP address is manually configured and the Gateway IP is not 0.
+    // Add the default route for this UDP instance.
+    //
+    Status = UdpIo->Protocol.Udp4->Routes (
+                                     UdpIo->Protocol.Udp4,
+                                     FALSE,
+                                     &mZeroIp4Addr,
+                                     &mZeroIp4Addr,
+                                     &Config->GatewayIp
+                                     );
+    if (EFI_ERROR (Status)) {
+      UdpIo->Protocol.Udp4->Configure (UdpIo->Protocol.Udp4, NULL);
+    }
+  }
+  return Status;
+}
+
+
+/**
+  Start the MTFTP session to do the operation, such as read file,
+  write file, and read directory.
+
+  @param  This                   The MTFTP session
+  @param  Token                  The token than encapsues the user's request.
+  @param  Operation              The operation to do
+
+  @retval EFI_INVALID_PARAMETER  Some of the parameters are invalid.
+  @retval EFI_NOT_STARTED        The MTFTP session hasn't been configured.
+  @retval EFI_ALREADY_STARTED    There is pending operation for the session.
+  @retval EFI_SUCCESS            The operation is successfully started.
+
+**/
+EFI_STATUS
+Mtftp4Start (
+  IN EFI_MTFTP4_PROTOCOL    *This,
+  IN EFI_MTFTP4_TOKEN       *Token,
+  IN UINT16                 Operation
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_OVERRIDE_DATA  *Override;
+  EFI_MTFTP4_CONFIG_DATA    *Config;
+  EFI_TPL                   OldTpl;
+  EFI_STATUS                Status;
+
+  //
+  // Validate the parameters
+  //
+  if ((This == NULL) || (Token == NULL) || (Token->Filename == NULL) ||
+      ((Token->OptionCount != 0) && (Token->OptionList == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // User must provide at least one method to collect the data for download.
+  //
+  if (((Operation == EFI_MTFTP4_OPCODE_RRQ) || (Operation == EFI_MTFTP4_OPCODE_DIR)) &&
+      ((Token->Buffer == NULL) && (Token->CheckPacket == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // User must provide at least one method to provide the data for upload.
+  //
+  if ((Operation == EFI_MTFTP4_OPCODE_WRQ) &&
+     ((Token->Buffer == NULL) && (Token->PacketNeeded == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MTFTP4_PROTOCOL_FROM_THIS (This);
+
+  Status = EFI_SUCCESS;
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Instance->State != MTFTP4_STATE_CONFIGED) {
+    Status = EFI_NOT_STARTED;
+  }
+
+  if (Instance->Operation != 0) {
+    Status = EFI_ACCESS_DENIED;
+  }
+
+  if (EFI_ERROR (Status)) {
+    gBS->RestoreTPL (OldTpl);
+    return Status;
+  }
+
+  //
+  // Set the Operation now to prevent the application start other
+  // operations.
+  //
+  Instance->Operation = Operation;
+  Override            = Token->OverrideData;
+
+  if ((Override != NULL) && !Mtftp4OverrideValid (Instance, Override)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_ERROR;
+  }
+
+  if (Token->OptionCount != 0) {
+    Status = Mtftp4ParseOption (
+               Token->OptionList,
+               Token->OptionCount,
+               TRUE,
+               &Instance->RequestOption
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto ON_ERROR;
+    }
+  }
+
+  //
+  // Set the operation parameters from the configuration or override data.
+  //
+  Config                  = &Instance->Config;
+  Instance->Token         = Token;
+  Instance->BlkSize       = MTFTP4_DEFAULT_BLKSIZE;
+
+  CopyMem (&Instance->ServerIp, &Config->ServerIp, sizeof (IP4_ADDR));
+  Instance->ServerIp      = NTOHL (Instance->ServerIp);
+
+  Instance->ListeningPort = Config->InitialServerPort;
+  Instance->ConnectedPort = 0;
+
+  CopyMem (&Instance->Gateway, &Config->GatewayIp, sizeof (IP4_ADDR));
+  Instance->Gateway       = NTOHL (Instance->Gateway);
+
+  Instance->MaxRetry      = Config->TryCount;
+  Instance->Timeout       = Config->TimeoutValue;
+  Instance->Master        = TRUE;
+
+  if (Override != NULL) {
+    CopyMem (&Instance->ServerIp, &Override->ServerIp, sizeof (IP4_ADDR));
+    CopyMem (&Instance->Gateway, &Override->GatewayIp, sizeof (IP4_ADDR));
+
+    Instance->ServerIp      = NTOHL (Instance->ServerIp);
+    Instance->Gateway       = NTOHL (Instance->Gateway);
+
+    Instance->ListeningPort = Override->ServerPort;
+    Instance->MaxRetry      = Override->TryCount;
+    Instance->Timeout       = Override->TimeoutValue;
+  }
+
+  if (Instance->ListeningPort == 0) {
+    Instance->ListeningPort = MTFTP4_DEFAULT_SERVER_PORT;
+  }
+
+  if (Instance->MaxRetry == 0) {
+    Instance->MaxRetry = MTFTP4_DEFAULT_RETRY;
+  }
+
+  if (Instance->Timeout == 0) {
+    Instance->Timeout = MTFTP4_DEFAULT_TIMEOUT;
+  }
+
+  //
+  // Config the unicast UDP child to send initial request
+  //
+  Status = Mtftp4ConfigUnicastPort (Instance->UnicastPort, Instance);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Set initial status.
+  //
+  Token->Status = EFI_NOT_READY;
+
+  //
+  // Build and send an initial requests
+  //
+  if (Operation == EFI_MTFTP4_OPCODE_WRQ) {
+    Status = Mtftp4WrqStart (Instance, Operation);
+  } else {
+    Status = Mtftp4RrqStart (Instance, Operation);
+  }
+
+  gBS->RestoreTPL (OldTpl);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  if (Token->Event != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Return immediately for asynchronous operation or poll the
+  // instance for synchronous operation.
+  //
+  while (Token->Status == EFI_NOT_READY) {
+    This->Poll (This);
+  }
+
+  return Token->Status;
+
+ON_ERROR:
+  Mtftp4CleanOperation (Instance, Status);
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Reads the current operational settings.
+
+  The GetModeData()function reads the current operational settings of this 
+  EFI MTFTPv4 Protocol driver instance.
+
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance.
+  @param  ModeData               Pointer to storage for the EFI MTFTPv4 Protocol
+                                 driver mode data. 
+
+  @retval EFI_SUCCESS            The configuration data was successfully returned.
+  @retval EFI_OUT_OF_RESOURCES   The required mode data could not be allocated.
+  @retval EFI_INVALID_PARAMETER  This is NULL or ModeData is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4GetModeData (
+  IN     EFI_MTFTP4_PROTOCOL    *This,
+     OUT EFI_MTFTP4_MODE_DATA  *ModeData
+  )
+{
+  MTFTP4_PROTOCOL  *Instance;
+  EFI_TPL          OldTpl;
+
+  if ((This == NULL) || (ModeData == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  Instance                         = MTFTP4_PROTOCOL_FROM_THIS (This);
+  CopyMem(&ModeData->ConfigData, &Instance->Config, sizeof (Instance->Config));
+  ModeData->SupportedOptionCount   = MTFTP4_SUPPORTED_OPTIONS;
+  ModeData->SupportedOptoins       = (UINT8 **) mMtftp4SupportedOptions;
+  ModeData->UnsupportedOptionCount = 0;
+  ModeData->UnsupportedOptoins     = NULL;
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Initializes, changes, or resets the default operational setting for this 
+  EFI MTFTPv4 Protocol driver instance.
+  
+  The Configure() function is used to set and change the configuration data for 
+  this EFI MTFTPv4 Protocol driver instance. The configuration data can be reset 
+  to startup defaults by calling Configure() with MtftpConfigData set to NULL. 
+  Whenever the instance is reset, any pending operation is aborted. By changing 
+  the EFI MTFTPv4 Protocol driver instance configuration data, the client can 
+  connect to different MTFTPv4 servers. The configuration parameters in 
+  MtftpConfigData are used as the default parameters in later MTFTPv4 operations 
+  and can be overridden in later operations.
+  
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance
+  @param  ConfigData             MtftpConfigDataPointer to the configuration data 
+                                 structure
+
+  @retval EFI_SUCCESS            The EFI MTFTPv4 Protocol driver was configured 
+                                 successfully.
+  @retval EFI_INVALID_PARAMETER  One or more following conditions are TRUE:
+                                 1.This is NULL.
+                                 2.MtftpConfigData.UseDefaultSetting is FALSE and 
+                                   MtftpConfigData.StationIp is not a valid IPv4 
+                                   unicast address.
+                                 3.MtftpCofigData.UseDefaultSetting is FALSE and 
+                                   MtftpConfigData.SubnetMask is invalid.
+                                 4.MtftpCofigData.ServerIp is not a valid IPv4 
+                                   unicast address.
+                                 5.MtftpConfigData.UseDefaultSetting is FALSE and 
+                                   MtftpConfigData.GatewayIp is not a valid IPv4 
+                                   unicast address or is not in the same subnet 
+                                   with station address.
+  @retval EFI_ACCESS_DENIED      The EFI configuration could not be changed at this 
+                                 time because there is one MTFTP background operation 
+                                 in progress.
+  @retval EFI_NO_MAPPING         When using a default address, configuration 
+                                 (DHCP, BOOTP, RARP, etc.) has not finished yet.
+  @retval EFI_UNSUPPORTED        A configuration protocol (DHCP, BOOTP, RARP, etc.) 
+                                 could not be located when clients choose to use 
+                                 the default address settings.
+  @retval EFI_OUT_OF_RESOURCES   The EFI MTFTPv4 Protocol driver instance data could 
+                                 not be allocated.
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred. 
+                                 The EFI MTFTPv4 Protocol driver instance is not 
+                                 configured.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4Configure (
+  IN EFI_MTFTP4_PROTOCOL    *This,
+  IN EFI_MTFTP4_CONFIG_DATA *ConfigData
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_TPL                   OldTpl;
+  IP4_ADDR                  Ip;
+  IP4_ADDR                  Netmask;
+  IP4_ADDR                  Gateway;
+  IP4_ADDR                  ServerIp;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MTFTP4_PROTOCOL_FROM_THIS (This);
+
+  if (ConfigData == NULL) {
+    //
+    // Reset the operation if ConfigData is NULL
+    //
+    OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+    Mtftp4CleanOperation (Instance, EFI_ABORTED);
+    ZeroMem (&Instance->Config, sizeof (EFI_MTFTP4_CONFIG_DATA));
+    Instance->State = MTFTP4_STATE_UNCONFIGED;
+
+    gBS->RestoreTPL (OldTpl);
+
+  } else {
+    //
+    // Configure the parameters for new operation.
+    //
+    CopyMem (&Ip, &ConfigData->StationIp, sizeof (IP4_ADDR));
+    CopyMem (&Netmask, &ConfigData->SubnetMask, sizeof (IP4_ADDR));
+    CopyMem (&Gateway, &ConfigData->GatewayIp, sizeof (IP4_ADDR));
+    CopyMem (&ServerIp, &ConfigData->ServerIp, sizeof (IP4_ADDR));
+
+    Ip       = NTOHL (Ip);
+    Netmask  = NTOHL (Netmask);
+    Gateway  = NTOHL (Gateway);
+    ServerIp = NTOHL (ServerIp);
+
+    if (!NetIp4IsUnicast (ServerIp, 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (!ConfigData->UseDefaultSetting &&
+       ((!IP4_IS_VALID_NETMASK (Netmask) || !NetIp4IsUnicast (Ip, Netmask)))) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((Gateway != 0) &&
+        (!IP4_NET_EQUAL (Gateway, Ip, Netmask) || !NetIp4IsUnicast (Gateway, Netmask))) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+
+    OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+    if ((Instance->State == MTFTP4_STATE_CONFIGED) && (Instance->Operation != 0)) {
+      gBS->RestoreTPL (OldTpl);
+      return EFI_ACCESS_DENIED;
+    }
+
+    CopyMem(&Instance->Config, ConfigData, sizeof (*ConfigData));;
+    Instance->State = MTFTP4_STATE_CONFIGED;
+
+    gBS->RestoreTPL (OldTpl);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
+/**
+  Parses the options in an MTFTPv4 OACK packet.
+  
+  The ParseOptions() function parses the option fields in an MTFTPv4 OACK packet 
+  and returns the number of options that were found and optionally a list of 
+  pointers to the options in the packet.
+  If one or more of the option fields are not valid, then EFI_PROTOCOL_ERROR is 
+  returned and *OptionCount and *OptionList stop at the last valid option.
+  The OptionList is allocated by this function, and caller should free it when used.
+
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance.
+  @param  PacketLen              Length of the OACK packet to be parsed.
+  @param  Packet                 Pointer to the OACK packet to be parsed. 
+  @param  OptionCount            Pointer to the number of options in following OptionList.
+  @param  OptionList             Pointer to EFI_MTFTP4_OPTION storage. Call the 
+                                 EFI Boot Service FreePool() to release theOptionList
+                                 if the options in this OptionList are not needed 
+                                 any more
+
+  @retval EFI_SUCCESS            The OACK packet was valid and the OptionCount and
+                                 OptionList parameters have been updated.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 1.PacketLen is 0.
+                                 2.Packet is NULL or Packet is not a valid MTFTPv4 packet.
+                                 3.OptionCount is NULL.
+  @retval EFI_NOT_FOUND          No options were found in the OACK packet.
+  @retval EFI_OUT_OF_RESOURCES   Storage for the OptionList array cannot be allocated.
+  @retval EFI_PROTOCOL_ERROR     One or more of the option fields is invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4ParseOptions (
+  IN     EFI_MTFTP4_PROTOCOL    *This,
+  IN     UINT32                 PacketLen,
+  IN     EFI_MTFTP4_PACKET      *Packet,
+     OUT UINT32                 *OptionCount,
+     OUT EFI_MTFTP4_OPTION      **OptionList          OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+
+  if ((This == NULL) || (PacketLen < MTFTP4_OPCODE_LEN) ||
+      (Packet == NULL) || (OptionCount == NULL)) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = Mtftp4ExtractOptions (Packet, PacketLen, OptionCount, OptionList);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (*OptionCount == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Downloads a file from an MTFTPv4 server.
+  
+  The ReadFile() function is used to initialize and start an MTFTPv4 download 
+  process and optionally wait for completion. When the download operation completes, 
+  whether successfully or not, the Token.Status field is updated by the EFI MTFTPv4 
+  Protocol driver and then Token.Event is signaled (if it is not NULL).
+  Data can be downloaded from the MTFTPv4 server into either of the following locations:
+  1.A fixed buffer that is pointed to by Token.Buffer
+  2.A download service function that is pointed to by Token.CheckPacket
+  If both Token.Buffer and Token.CheckPacket are used, then Token.CheckPacket 
+  will be called first. If the call is successful, the packet will be stored in 
+  Token.Buffer.
+
+  @param  This                  Pointer to the EFI_MTFTP4_PROTOCOL instance
+  @param  Token                 Pointer to the token structure to provide the 
+                                parameters that are used in this operation.
+
+  @retval EFI_SUCCESS           The data file has been transferred successfully.
+  @retval EFI_OUT_OF_RESOURCES  Required system resources could not be allocated.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is not large enough to hold the downloaded 
+                                data in downloading process.
+  @retval EFI_ABORTED           Current operation is aborted by user.
+  @retval EFI_ICMP_ERROR        An ICMP ERROR packet was received.
+  @retval EFI_TIMEOUT           No responses were received from the MTFTPv4 server.
+  @retval EFI_TFTP_ERROR        An MTFTPv4 ERROR packet was received.
+  @retval EFI_DEVICE_ERROR      An unexpected network error or system error occurred.
+  @retval EFI_NO_MEDIA          There was a media error.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4ReadFile (
+  IN EFI_MTFTP4_PROTOCOL    *This,
+  IN EFI_MTFTP4_TOKEN       *Token
+  )
+{
+  return Mtftp4Start (This, Token, EFI_MTFTP4_OPCODE_RRQ);
+}
+
+
+/**
+  Sends a data file to an MTFTPv4 server. May be unsupported in some EFI implementations
+
+  The WriteFile() function is used to initialize an uploading operation with the 
+  given option list and optionally wait for completion. If one or more of the 
+  options is not supported by the server, the unsupported options are ignored and 
+  a standard TFTP process starts instead. When the upload process completes, 
+  whether successfully or not, Token.Event is signaled, and the EFI MTFTPv4 Protocol 
+  driver updates Token.Status.
+  The caller can supply the data to be uploaded in the following two modes:
+  1.Through the user-provided buffer
+  2.Through a callback function
+  With the user-provided buffer, the Token.BufferSize field indicates the length
+  of the buffer, and the driver will upload the data in the buffer. With an 
+  EFI_MTFTP4_PACKET_NEEDED callback function, the driver will call this callback 
+  function to get more data from the user to upload. See the definition of 
+  EFI_MTFTP4_PACKET_NEEDED for more information. These two modes cannot be used at 
+  the same time. The callback function will be ignored if the user provides the buffer.
+
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance.
+  @param  Token                  Pointer to the token structure to provide the 
+                                 parameters that are used in this function
+
+  @retval EFI_SUCCESS            The upload session has started.
+  @retval EFI_UNSUPPORTED        The operation is not supported by this implementation.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 1. This is NULL.
+                                 2. Token is NULL.
+                                 3. Token.Filename is NULL.
+                                 4. Token.OptionCount is not zero and
+                                    Token.OptionList is NULL.
+                                 5. One or more options in Token.OptionList have wrong
+                                    format.
+                                 6. Token.Buffer and Token.PacketNeeded are both
+                                    NULL.
+                                 7. One or more IPv4 addresses in Token.OverrideData 
+                                    are not valid unicast IPv4 addresses if 
+                                    Token.OverrideData is not NULL.
+  @retval EFI_UNSUPPORTED        One or more options in the Token.OptionList are in the
+                                 unsupported list of structure EFI_MTFTP4_MODE_DATA.
+  @retval EFI_NOT_STARTED        The EFI MTFTPv4 Protocol driver has not been started.
+  @retval EFI_NO_MAPPING         When using a default address, configuration (DHCP, 
+                                 BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_ALREADY_STARTED    This Token is already being used in another MTFTPv4 
+                                 session.
+  @retval EFI_OUT_OF_RESOURCES   Required system resources could not be allocated.
+  @retval EFI_ACCESS_DENIED      The previous operation has not completed yet.
+  @retval EFI_DEVICE_ERROR       An unexpected network error or system error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4WriteFile (
+  IN EFI_MTFTP4_PROTOCOL    *This,
+  IN EFI_MTFTP4_TOKEN       *Token
+  )
+{
+  return Mtftp4Start (This, Token, EFI_MTFTP4_OPCODE_WRQ);
+}
+
+
+/**
+  Downloads a data file "directory" from an MTFTPv4 server. 
+  May be unsupported in some EFI implementations
+  
+  The ReadDirectory() function is used to return a list of files on the MTFTPv4 
+  server that are logically (or operationally) related to Token.Filename. The 
+  directory request packet that is sent to the server is built with the option 
+  list that was provided by caller, if present.
+  The file information that the server returns is put into either of the following 
+  locations:
+  1.A fixed buffer that is pointed to by Token.Buffer
+  2.A download service function that is pointed to by Token.CheckPacket
+  If both Token.Buffer and Token.CheckPacket are used, then Token.CheckPacket will 
+  be called first. If the call is successful, the packet will be stored in Token.Buffer.
+  The returned directory listing in the Token.Buffer or EFI_MTFTP4_PACKET consists 
+  of a list of two or three variable-length ASCII strings, each terminated by a 
+  null character, for each file in the directory. If the multicast option is involved, 
+  the first field of each directory entry is the static multicast IP address and 
+  UDP port number that is associated with the file name. The format of the field 
+  is ip:ip:ip:ip:port. If the multicast option is not involved, this field and its 
+  terminating null character are not present.
+  The next field of each directory entry is the file name and the last field is 
+  the file information string. The information string contains the file size and 
+  the create/modify timestamp. The format of the information string is filesize 
+  yyyy-mm-dd hh:mm:ss:ffff. The timestamp is Coordinated Universal Time 
+  (UTC; also known as Greenwich Mean Time [GMT]).
+  The only difference between ReadFile and ReadDirectory is the opcode used.
+
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance
+  @param  Token                  Pointer to the token structure to provide the 
+                                 parameters that are used in this function
+
+  @retval EFI_SUCCESS            The MTFTPv4 related file "directory" has been downloaded.
+  @retval EFI_UNSUPPORTED        The operation is not supported by this implementation.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 1. This is NULL.
+                                 2. Token is NULL.
+                                 3. Token.Filename is NULL.
+                                 4. Token.OptionCount is not zero and
+                                    Token.OptionList is NULL.
+                                 5. One or more options in Token.OptionList have wrong
+                                    format.
+                                 6. Token.Buffer and Token.PacketNeeded are both
+                                    NULL.
+                                 7. One or more IPv4 addresses in Token.OverrideData 
+                                    are not valid unicast IPv4 addresses if 
+                                    Token.OverrideData is not NULL.
+  @retval EFI_UNSUPPORTED        One or more options in the Token.OptionList are in the
+                                 unsupported list of structure EFI_MTFTP4_MODE_DATA.
+  @retval EFI_NOT_STARTED        The EFI MTFTPv4 Protocol driver has not been started.
+  @retval EFI_NO_MAPPING         When using a default address, configuration (DHCP, 
+                                 BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_ALREADY_STARTED    This Token is already being used in another MTFTPv4 
+                                 session.
+  @retval EFI_OUT_OF_RESOURCES   Required system resources could not be allocated.
+  @retval EFI_ACCESS_DENIED      The previous operation has not completed yet.
+  @retval EFI_DEVICE_ERROR       An unexpected network error or system error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4ReadDirectory (
+  IN EFI_MTFTP4_PROTOCOL        *This,
+  IN EFI_MTFTP4_TOKEN           *Token
+  )
+{
+  return Mtftp4Start (This, Token, EFI_MTFTP4_OPCODE_DIR);
+}
+
+
+/**
+  Gets information about a file from an MTFTPv4 server. 
+  
+  The GetInfo() function assembles an MTFTPv4 request packet with options; 
+  sends it to the MTFTPv4 server; and may return an MTFTPv4 OACK, MTFTPv4 ERROR, 
+  or ICMP ERROR packet. Retries occur only if no response packets are received 
+  from the MTFTPv4 server before the timeout expires.
+  It is implemented with EfiMtftp4ReadFile: build a token, then pass it to 
+  EfiMtftp4ReadFile. In its check packet callback abort the opertions.
+
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance
+  @param  OverrideData           Data that is used to override the existing 
+                                 parameters. If NULL, the default parameters that 
+                                 were set in the EFI_MTFTP4_PROTOCOL.Configure() 
+                                 function are used
+  @param  Filename               Pointer to null-terminated ASCII file name string
+  @param  ModeStr                Pointer to null-terminated ASCII mode string. If NULL, "octet" 
+                                 will be used
+  @param  OptionCount            Number of option/value string pairs in OptionList
+  @param  OptionList             Pointer to array of option/value string pairs. 
+                                 Ignored if OptionCount is zero
+  @param  PacketLength           The number of bytes in the returned packet
+  @param  Packet                 PacketThe pointer to the received packet. This 
+                                 buffer must be freed by the caller.
+
+  @retval EFI_SUCCESS            An MTFTPv4 OACK packet was received and is in 
+                                 the Buffer.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 1.This is NULL.
+                                 2.Filename is NULL.
+                                 3.OptionCount is not zero and OptionList is NULL.
+                                 4.One or more options in OptionList have wrong format.
+                                 5.PacketLength is NULL.
+                                 6.One or more IPv4 addresses in OverrideData are 
+                                   not valid unicast IPv4 addresses if OverrideData 
+                                   is not NULL.
+  @retval EFI_UNSUPPORTED        One or more options in the OptionList are in the
+                                 unsupported list of structure EFI_MTFTP4_MODE_DATA
+  @retval EFI_NOT_STARTED        The EFI MTFTPv4 Protocol driver has not been started.
+  @retval EFI_NO_MAPPING         When using a default address, configuration (DHCP, 
+                                 BOOTP, RARP, etc.) has not finished yet.
+  @retval EFI_ACCESS_DENIED      The previous operation has not completed yet.
+  @retval EFI_OUT_OF_RESOURCES   Required system resources could not be allocated.
+  @retval EFI_TFTP_ERROR         An MTFTPv4 ERROR packet was received and is in 
+                                 the Buffer.
+  @retval EFI_ICMP_ERROR         An ICMP ERROR packet was received and the Packet 
+                                 is set to NULL.
+  @retval EFI_PROTOCOL_ERROR     An unexpected MTFTPv4 packet was received and is 
+                                 in the Buffer.
+  @retval EFI_TIMEOUT            No responses were received from the MTFTPv4 server.
+  @retval EFI_DEVICE_ERROR       An unexpected network error or system error occurred.
+  @retval EFI_NO_MEDIA           There was a media error.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4GetInfo (
+  IN     EFI_MTFTP4_PROTOCOL      *This,
+  IN     EFI_MTFTP4_OVERRIDE_DATA *OverrideData        OPTIONAL,
+  IN     UINT8                    *Filename,
+  IN     UINT8                    *ModeStr             OPTIONAL,
+  IN     UINT8                    OptionCount,
+  IN     EFI_MTFTP4_OPTION        *OptionList          OPTIONAL,
+     OUT UINT32                   *PacketLength,
+     OUT EFI_MTFTP4_PACKET        **Packet             OPTIONAL
+  )
+{
+  EFI_MTFTP4_TOKEN          Token;
+  MTFTP4_GETINFO_STATE      State;
+  EFI_STATUS                Status;
+
+  if ((This == NULL) || (Filename == NULL) || (PacketLength == NULL) ||
+      ((OptionCount != 0) && (OptionList == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Packet != NULL) {
+    *Packet = NULL;
+  }
+
+  *PacketLength         = 0;
+  State.Packet          = Packet;
+  State.PacketLen       = PacketLength;
+  State.Status          = EFI_SUCCESS;
+
+  //
+  // Fill in the Token to issue an synchronous ReadFile operation
+  //
+  Token.Status          = EFI_SUCCESS;
+  Token.Event           = NULL;
+  Token.OverrideData    = OverrideData;
+  Token.Filename        = Filename;
+  Token.ModeStr         = ModeStr;
+  Token.OptionCount     = OptionCount;
+  Token.OptionList      = OptionList;
+  Token.BufferSize      = 0;
+  Token.Buffer          = NULL;
+  Token.Context         = &State;
+  Token.CheckPacket     = Mtftp4GetInfoCheckPacket;
+  Token.TimeoutCallback = NULL;
+  Token.PacketNeeded    = NULL;
+
+  Status                = EfiMtftp4ReadFile (This, &Token);
+
+  if (EFI_ABORTED == Status) {
+    return State.Status;
+  }
+
+  return Status;
+}
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+
+  The Poll() function can be used by network drivers and applications to increase 
+  the rate that data packets are moved between the communications device and the 
+  transmit and receive queues.
+  In some systems, the periodic timer event in the managed network driver may not 
+  poll the underlying communications device fast enough to transmit and/or receive 
+  all data packets without missing incoming packets or dropping outgoing packets. 
+  Drivers and applications that are experiencing packet loss should try calling 
+  the Poll() function more often.
+  
+  @param  This                   Pointer to the EFI_MTFTP4_PROTOCOL instance
+
+  @retval EFI_SUCCESS            Incoming or outgoing data was processed.
+  @retval EFI_NOT_STARTED        This EFI MTFTPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING         When using a default address, configuration (DHCP,
+                                 BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER  This is NULL.
+  @retval EFI_DEVICE_ERROR       An unexpected system or network error occurred.
+  @retval EFI_TIMEOUT            Data was dropped out of the transmit and/or receive 
+                                 queue. Consider increasing the polling rate.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiMtftp4Poll (
+  IN EFI_MTFTP4_PROTOCOL    *This
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_UDP4_PROTOCOL         *Udp;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = MTFTP4_PROTOCOL_FROM_THIS (This);
+
+  if (Instance->State == MTFTP4_STATE_UNCONFIGED) {
+    return EFI_NOT_STARTED;
+  } else if (Instance->State == MTFTP4_STATE_DESTROY) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Udp = Instance->UnicastPort->Protocol.Udp4;
+  return Udp->Poll (Udp);
+}
+
+EFI_MTFTP4_PROTOCOL gMtftp4ProtocolTemplate = {
+  EfiMtftp4GetModeData,
+  EfiMtftp4Configure,
+  EfiMtftp4GetInfo,
+  EfiMtftp4ParseOptions,
+  EfiMtftp4ReadFile,
+  EfiMtftp4WriteFile,
+  EfiMtftp4ReadDirectory,
+  EfiMtftp4Poll
+};
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.h b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.h
new file mode 100644
index 0000000000..527fd1db10
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Impl.h
@@ -0,0 +1,216 @@
+/** @file
+  
+  Mtftp4 Implementation.
+  
+  Mtftp4 Implementation, it supports the following RFCs:
+  RFC1350 - THE TFTP PROTOCOL (REVISION 2)
+  RFC2090 - TFTP Multicast Option
+  RFC2347 - TFTP Option Extension
+  RFC2348 - TFTP Blocksize Option
+  RFC2349 - TFTP Timeout Interval and Transfer Size Options
+  
+Copyright (c) 2006 - 2012, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef __EFI_MTFTP4_IMPL_H__
+#define __EFI_MTFTP4_IMPL_H__
+
+#include <Uefi.h>
+
+#include <Protocol/Udp4.h>
+#include <Protocol/Mtftp4.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UdpIoLib.h>
+#include <Library/PrintLib.h>
+
+extern EFI_MTFTP4_PROTOCOL  gMtftp4ProtocolTemplate;
+
+typedef struct _MTFTP4_SERVICE  MTFTP4_SERVICE;
+typedef struct _MTFTP4_PROTOCOL MTFTP4_PROTOCOL;
+
+#include "Mtftp4Driver.h"
+#include "Mtftp4Option.h"
+#include "Mtftp4Support.h"
+
+
+///
+/// Some constant value of Mtftp service.
+///
+#define MTFTP4_SERVICE_SIGNATURE    SIGNATURE_32 ('T', 'F', 'T', 'P')
+#define MTFTP4_PROTOCOL_SIGNATURE   SIGNATURE_32 ('t', 'f', 't', 'p')
+
+#define MTFTP4_DEFAULT_SERVER_PORT  69
+#define MTFTP4_DEFAULT_TIMEOUT      3
+#define MTFTP4_DEFAULT_RETRY        5
+#define MTFTP4_DEFAULT_BLKSIZE      512
+#define MTFTP4_TIME_TO_GETMAP       5
+
+#define MTFTP4_STATE_UNCONFIGED     0
+#define MTFTP4_STATE_CONFIGED       1
+#define MTFTP4_STATE_DESTROY        2
+
+///
+/// Mtftp service block
+///
+struct _MTFTP4_SERVICE {
+  UINT32                        Signature;
+  EFI_SERVICE_BINDING_PROTOCOL  ServiceBinding;
+
+  UINT16                        ChildrenNum;
+  LIST_ENTRY                    Children;
+
+  EFI_EVENT                     Timer;  ///< Ticking timer for all the MTFTP clients
+  EFI_EVENT                     TimerToGetMap;
+
+  EFI_HANDLE                    Controller;
+  EFI_HANDLE                    Image;
+
+  //
+  // This UDP child is used to keep the connection between the UDP
+  // and MTFTP, so MTFTP will be notified when UDP is uninstalled.
+  //
+  UDP_IO                        *ConnectUdp;
+};
+
+
+typedef struct {
+  EFI_MTFTP4_PACKET             **Packet;
+  UINT32                        *PacketLen;
+  EFI_STATUS                    Status;
+} MTFTP4_GETINFO_STATE;
+
+struct _MTFTP4_PROTOCOL {
+  UINT32                        Signature;
+  LIST_ENTRY                    Link;
+  EFI_MTFTP4_PROTOCOL           Mtftp4;
+
+  INTN                          State;
+  BOOLEAN                       InDestroy;
+
+  MTFTP4_SERVICE                *Service;
+  EFI_HANDLE                    Handle;
+
+  EFI_MTFTP4_CONFIG_DATA        Config;
+
+  //
+  // Operation parameters: token and requested options.
+  //
+  EFI_MTFTP4_TOKEN              *Token;
+  MTFTP4_OPTION                 RequestOption;
+  UINT16                        Operation;
+
+  //
+  // Blocks is a list of MTFTP4_BLOCK_RANGE which contains
+  // holes in the file
+  //
+  UINT16                        BlkSize;
+  UINT16                        LastBlock;
+  LIST_ENTRY                    Blocks;
+
+  //
+  // The server's communication end point: IP and two ports. one for
+  // initial request, one for its selected port.
+  //
+  IP4_ADDR                      ServerIp;
+  UINT16                        ListeningPort;
+  UINT16                        ConnectedPort;
+  IP4_ADDR                      Gateway;
+  UDP_IO                        *UnicastPort;
+
+  //
+  // Timeout and retransmit status
+  //
+  NET_BUF                       *LastPacket;
+  UINT32                        PacketToLive;
+  UINT32                        CurRetry;
+  UINT32                        MaxRetry;
+  UINT32                        Timeout;
+
+  //
+  // Parameter used by RRQ's multicast download.
+  //
+  IP4_ADDR                      McastIp;
+  UINT16                        McastPort;
+  BOOLEAN                       Master;
+  UDP_IO                        *McastUdpPort;
+};
+
+typedef struct {
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+} MTFTP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT;
+
+/**
+  Clean up the MTFTP session to get ready for new operation.
+
+  @param  Instance               The MTFTP session to clean up
+  @param  Result                 The result to return to the caller who initiated
+                                 the operation.
+**/
+VOID
+Mtftp4CleanOperation (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN     EFI_STATUS             Result
+  );
+
+/**
+  Start the MTFTP session for upload.
+  
+  It will first init some states, then send the WRQ request packet, 
+  and start receiving the packet.
+
+  @param  Instance              The MTFTP session
+  @param  Operation             Redundant parameter, which is always
+                                EFI_MTFTP4_OPCODE_WRQ here.
+
+  @retval EFI_SUCCESS           The upload process has been started.
+  @retval Others                Failed to start the upload.
+
+**/
+EFI_STATUS
+Mtftp4WrqStart (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 Operation
+  );
+
+/**
+  Start the MTFTP session to download. 
+  
+  It will first initialize some of the internal states then build and send a RRQ 
+  reqeuest packet, at last, it will start receive for the downloading.
+
+  @param  Instance              The Mtftp session
+  @param  Operation             The MTFTP opcode, it may be a EFI_MTFTP4_OPCODE_RRQ
+                                or EFI_MTFTP4_OPCODE_DIR.
+
+  @retval EFI_SUCCESS           The mtftp download session is started.
+  @retval Others                Failed to start downloading.
+
+**/
+EFI_STATUS
+Mtftp4RrqStart (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 Operation
+  );
+
+#define MTFTP4_SERVICE_FROM_THIS(a)   \
+  CR (a, MTFTP4_SERVICE, ServiceBinding, MTFTP4_SERVICE_SIGNATURE)
+
+#define MTFTP4_PROTOCOL_FROM_THIS(a)  \
+  CR (a, MTFTP4_PROTOCOL, Mtftp4, MTFTP4_PROTOCOL_SIGNATURE)
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.c
new file mode 100644
index 0000000000..e40561a96b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.c
@@ -0,0 +1,534 @@
+/** @file
+  Routines to process MTFTP4 options.
+
+Copyright (c) 2006 - 2010, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+CHAR8 *mMtftp4SupportedOptions[MTFTP4_SUPPORTED_OPTIONS] = {
+  "blksize",
+  "timeout",
+  "tsize",
+  "multicast"
+};
+
+
+/**
+  Check whether two ascii strings are equel, ignore the case.
+
+  @param  Str1                   The first ascii string
+  @param  Str2                   The second ascii string
+
+  @retval TRUE                   Two strings are equal when case is ignored.
+  @retval FALSE                  Two string are not equal.
+
+**/
+BOOLEAN
+NetStringEqualNoCase (
+  IN UINT8                 *Str1,
+  IN UINT8                 *Str2
+  )
+{
+  UINT8                     Ch1;
+  UINT8                     Ch2;
+
+  ASSERT ((Str1 != NULL) && (Str2 != NULL));
+
+  for (; (*Str1 != '\0') && (*Str2 != '\0'); Str1++, Str2++) {
+    Ch1 = *Str1;
+    Ch2 = *Str2;
+
+    //
+    // Convert them to lower case then compare two
+    //
+    if (('A' <= Ch1) && (Ch1 <= 'Z')) {
+      Ch1 += 'a' - 'A';
+    }
+
+    if (('A' <= Ch2) && (Ch2 <= 'Z')) {
+      Ch2 += 'a' - 'A';
+    }
+
+    if (Ch1 != Ch2) {
+      return FALSE;
+    }
+  }
+
+  return (BOOLEAN) (*Str1 == *Str2);
+}
+
+
+/**
+  Convert a string to a UINT32 number.
+
+  @param  Str                    The string to convert from
+
+  @return The number get from the string
+
+**/
+UINT32
+NetStringToU32 (
+  IN UINT8                 *Str
+  )
+{
+  UINT32                    Num;
+
+  ASSERT (Str != NULL);
+
+  Num = 0;
+
+  for (; NET_IS_DIGIT (*Str); Str++) {
+    Num = Num * 10 + (*Str - '0');
+  }
+
+  return Num;
+}
+
+
+/**
+  Convert a string of the format "192.168.0.1" to an IP address.
+
+  @param  Str                    The string representation of IP
+  @param  Ip                     The varible to get IP.
+
+  @retval EFI_INVALID_PARAMETER  The IP string is invalid.
+  @retval EFI_SUCCESS            The IP is parsed into the Ip
+
+**/
+EFI_STATUS
+NetStringToIp (
+  IN     UINT8                 *Str,
+     OUT IP4_ADDR              *Ip
+  )
+{
+  UINT32                    Byte;
+  UINT32                    Addr;
+  UINTN                     Index;
+
+  *Ip  = 0;
+  Addr = 0;
+
+  for (Index = 0; Index < 4; Index++) {
+    if (!NET_IS_DIGIT (*Str)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Byte = NetStringToU32 (Str);
+
+    if (Byte > 255) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Addr = (Addr << 8) | Byte;
+
+    //
+    // Skip all the digitals and check whether the sepeator is the dot
+    //
+    while (NET_IS_DIGIT (*Str)) {
+      Str++;
+    }
+
+    if ((Index < 3) && (*Str != '.')) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Str++;
+  }
+
+  *Ip = Addr;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Go through the packet to fill the Options array with the start
+  addresses of each MTFTP option name/value pair.
+
+  @param  Packet                 The packet to check
+  @param  PacketLen              The packet's length
+  @param  Count                  The size of the Options on input. The actual
+                                 options on output
+  @param  Options                The option array to fill in
+
+  @retval EFI_INVALID_PARAMETER  The packet is mal-formated
+  @retval EFI_BUFFER_TOO_SMALL   The Options array is too small
+  @retval EFI_SUCCESS            The packet has been parsed into the Options array.
+
+**/
+EFI_STATUS
+Mtftp4FillOptions (
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                PacketLen,
+  IN OUT UINT32                *Count,
+     OUT EFI_MTFTP4_OPTION     *Options          OPTIONAL
+  )
+{
+  UINT8                     *Cur;
+  UINT8                     *Last;
+  UINT8                     Num;
+  UINT8                     *Name;
+  UINT8                     *Value;
+
+  Num   = 0;
+  Cur   = (UINT8 *) Packet + MTFTP4_OPCODE_LEN;
+  Last  = (UINT8 *) Packet + PacketLen - 1;
+
+  //
+  // process option name and value pairs. The last byte is always zero
+  //
+  while (Cur < Last) {
+    Name = Cur;
+
+    while (*Cur != 0) {
+      Cur++;
+    }
+
+    if (Cur == Last) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Value = ++Cur;
+
+    while (*Cur != 0) {
+      Cur++;
+    }
+
+    Num++;
+
+    if ((Options != NULL) && (Num <= *Count)) {
+      Options[Num - 1].OptionStr  = Name;
+      Options[Num - 1].ValueStr   = Value;
+    }
+
+    Cur++;
+  }
+
+  if ((*Count < Num) || (Options == NULL)) {
+    *Count = Num;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  *Count = Num;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Allocate and fill in a array of Mtftp options from the Packet.
+
+  It first calls Mtftp4FillOption to get the option number, then allocate
+  the array, at last, call Mtftp4FillOption again to save the options.
+
+  @param  Packet                 The packet to parse
+  @param  PacketLen              The length of the packet
+  @param  OptionCount            The number of options in the packet
+  @param  OptionList             The point to get the option array.
+
+  @retval EFI_INVALID_PARAMETER  The parametera are invalid or packet isn't a
+                                 well-formated OACK packet.
+  @retval EFI_SUCCESS            The option array is build
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory for the array
+
+**/
+EFI_STATUS
+Mtftp4ExtractOptions (
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                PacketLen,
+     OUT UINT32                *OptionCount,
+     OUT EFI_MTFTP4_OPTION     **OptionList        OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+
+  *OptionCount = 0;
+
+  if (OptionList != NULL) {
+    *OptionList = NULL;
+  }
+
+  if (NTOHS (Packet->OpCode) != EFI_MTFTP4_OPCODE_OACK) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PacketLen == MTFTP4_OPCODE_LEN) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // The last byte must be zero to terminate the options
+  //
+  if (*((UINT8 *) Packet + PacketLen - 1) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Get the number of options
+  //
+  Status = Mtftp4FillOptions (Packet, PacketLen, OptionCount, NULL);
+
+  if ((Status == EFI_SUCCESS) || (Status != EFI_BUFFER_TOO_SMALL)) {
+    return Status;
+  }
+
+  //
+  // Allocate memory for the options, then call Mtftp4FillOptions to
+  // fill it if caller want that.
+  //
+  if (OptionList == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  *OptionList = AllocatePool (*OptionCount * sizeof (EFI_MTFTP4_OPTION));
+
+  if (*OptionList == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Mtftp4FillOptions (Packet, PacketLen, OptionCount, *OptionList);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parse the MTFTP multicast option.
+
+  @param  Value                  The Mtftp multicast value string
+  @param  Option                 The option to save the info into.
+
+  @retval EFI_INVALID_PARAMETER  The multicast value string is invalid.
+  @retval EFI_SUCCESS            The multicast value is parsed into the Option
+
+**/
+EFI_STATUS
+Mtftp4ExtractMcast (
+  IN     UINT8                  *Value,
+  IN OUT MTFTP4_OPTION          *Option
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    Num;
+
+  //
+  // The multicast option is formated like "204.0.0.1,1857,1"
+  // The server can also omit the ip and port, use ",,1"
+  //
+  if (*Value == ',') {
+    Option->McastIp   = 0;
+  } else {
+    Status = NetStringToIp (Value, &Option->McastIp);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    while ((*Value != 0) && (*Value != ',')) {
+      Value++;
+    }
+  }
+
+  if (*Value != ',') {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Value++;
+
+  //
+  // Convert the port setting. the server can send us a port number or
+  // empty string. such as the port in ",,1"
+  //
+  if (*Value == ',') {
+    Option->McastPort = 0;
+  } else {
+    Num = NetStringToU32 (Value);
+
+    if (Num > 65535) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    Option->McastPort = (UINT16) Num;
+
+    while (NET_IS_DIGIT (*Value)) {
+      Value++;
+    }
+  }
+
+  if (*Value != ',') {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Value++;
+
+  //
+  // Check the master/slave setting, 1 for master, 0 for slave.
+  //
+  Num = NetStringToU32 (Value);
+
+  if ((Num != 0) && (Num != 1)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Option->Master = (BOOLEAN) (Num == 1);
+
+  while (NET_IS_DIGIT (*Value)) {
+    Value++;
+  }
+
+  if (*Value != '\0') {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parse the option in Options array to MTFTP4_OPTION which program
+  can access directly.
+
+  @param  Options                The option array, which contains addresses of each
+                                 option's name/value string.
+  @param  Count                  The number of options in the Options
+  @param  Request                Whether this is a request or OACK. The format of
+                                 multicast is different according to this setting.
+  @param  MtftpOption            The MTFTP4_OPTION for easy access.
+
+  @retval EFI_INVALID_PARAMETER  The option is mal-formated
+  @retval EFI_UNSUPPORTED        Some option isn't supported
+  @retval EFI_SUCCESS            The option are OK and has been parsed.
+
+**/
+EFI_STATUS
+Mtftp4ParseOption (
+  IN     EFI_MTFTP4_OPTION     *Options,
+  IN     UINT32                Count,
+  IN     BOOLEAN               Request,
+     OUT MTFTP4_OPTION         *MtftpOption
+  )
+{
+  EFI_STATUS                Status;
+  UINT32                    Index;
+  UINT32                    Value;
+  EFI_MTFTP4_OPTION         *This;
+
+  MtftpOption->Exist = 0;
+
+  for (Index = 0; Index < Count; Index++) {
+    This = Options + Index;
+
+    if ((This->OptionStr == NULL) || (This->ValueStr == NULL)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (NetStringEqualNoCase (This->OptionStr, (UINT8 *) "blksize")) {
+      //
+      // block size option, valid value is between [8, 65464]
+      //
+      Value = NetStringToU32 (This->ValueStr);
+
+      if ((Value < 8) || (Value > 65464)) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      MtftpOption->BlkSize = (UINT16) Value;
+      MtftpOption->Exist |= MTFTP4_BLKSIZE_EXIST;
+
+    } else if (NetStringEqualNoCase (This->OptionStr, (UINT8 *) "timeout")) {
+      //
+      // timeout option, valid value is between [1, 255]
+      //
+      Value = NetStringToU32 (This->ValueStr);
+
+      if ((Value < 1) || (Value > 255)) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      MtftpOption->Timeout = (UINT8) Value;
+
+    } else if (NetStringEqualNoCase (This->OptionStr, (UINT8 *) "tsize")) {
+      //
+      // tsize option, the biggest transfer supported is 4GB with block size option
+      //
+      MtftpOption->Tsize = NetStringToU32 (This->ValueStr);
+      MtftpOption->Exist |= MTFTP4_TSIZE_EXIST;
+
+    } else if (NetStringEqualNoCase (This->OptionStr, (UINT8 *) "multicast")) {
+      //
+      // Multicast option, if it is a request, the value must be a zero
+      // length string, otherwise, it is formated like "204.0.0.1,1857,1\0"
+      //
+      if (Request) {
+        if (*(This->ValueStr) != '\0') {
+          return EFI_INVALID_PARAMETER;
+        }
+
+      } else {
+        Status = Mtftp4ExtractMcast (This->ValueStr, MtftpOption);
+
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+      }
+
+      MtftpOption->Exist |= MTFTP4_MCAST_EXIST;
+
+    } else if (Request) {
+      //
+      // Ignore the unsupported option if it is a reply, and return
+      // EFI_UNSUPPORTED if it's a request according to the UEFI spec.
+      //
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Parse the options in the OACK packet to MTFTP4_OPTION which program
+  can access directly.
+
+  @param  Packet                 The OACK packet to parse
+  @param  PacketLen              The length of the packet
+  @param  MtftpOption            The MTFTP_OPTION for easy access.
+
+  @retval EFI_INVALID_PARAMETER  The packet option is mal-formated
+  @retval EFI_UNSUPPORTED        Some option isn't supported
+  @retval EFI_SUCCESS            The option are OK and has been parsed.
+
+**/
+EFI_STATUS
+Mtftp4ParseOptionOack (
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                PacketLen,
+     OUT MTFTP4_OPTION         *MtftpOption
+  )
+{
+  EFI_MTFTP4_OPTION *OptionList;
+  EFI_STATUS        Status;
+  UINT32            Count;
+
+  MtftpOption->Exist = 0;
+
+  Status = Mtftp4ExtractOptions (Packet, PacketLen, &Count, &OptionList);
+
+  if (EFI_ERROR (Status) || (Count == 0)) {
+    return Status;
+  }
+  ASSERT (OptionList != NULL);
+
+  Status = Mtftp4ParseOption (OptionList, Count, FALSE, MtftpOption);
+
+  FreePool (OptionList);
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.h b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.h
new file mode 100644
index 0000000000..b7fdbf23e1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Option.h
@@ -0,0 +1,111 @@
+/** @file
+  Routines to process MTFTP4 options.
+  
+Copyright (c) 2006 - 2009, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+
+#ifndef __EFI_MTFTP4_OPTION_H__
+#define __EFI_MTFTP4_OPTION_H__
+
+#define MTFTP4_SUPPORTED_OPTIONS  4
+#define MTFTP4_OPCODE_LEN         2
+#define MTFTP4_ERRCODE_LEN        2
+#define MTFTP4_BLKNO_LEN          2
+#define MTFTP4_DATA_HEAD_LEN      4
+
+#define MTFTP4_BLKSIZE_EXIST      0x01
+#define MTFTP4_TIMEOUT_EXIST      0x02
+#define MTFTP4_TSIZE_EXIST        0x04
+#define MTFTP4_MCAST_EXIST        0x08
+
+typedef struct {
+  UINT16                    BlkSize;
+  UINT8                     Timeout;
+  UINT32                    Tsize;
+  IP4_ADDR                  McastIp;
+  UINT16                    McastPort;
+  BOOLEAN                   Master;
+  UINT32                    Exist;
+} MTFTP4_OPTION;
+
+/**
+  Allocate and fill in a array of Mtftp options from the Packet. 
+  
+  It first calls Mtftp4FillOption to get the option number, then allocate
+  the array, at last, call Mtftp4FillOption again to save the options.
+
+  @param  Packet                 The packet to parse
+  @param  PacketLen              The length of the packet
+  @param  OptionCount            The number of options in the packet
+  @param  OptionList             The point to get the option array.
+
+  @retval EFI_INVALID_PARAMETER  The parametera are invalid or packet isn't a
+                                 well-formated OACK packet.
+  @retval EFI_SUCCESS            The option array is build
+  @retval EFI_OUT_OF_RESOURCES   Failed to allocate memory for the array
+
+**/
+EFI_STATUS
+Mtftp4ExtractOptions (
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                PacketLen,
+     OUT UINT32                *OptionCount,
+     OUT EFI_MTFTP4_OPTION     **OptionList        OPTIONAL
+  );
+
+/**
+  Parse the option in Options array to MTFTP4_OPTION which program
+  can access directly.
+
+  @param  Options                The option array, which contains addresses of each
+                                 option's name/value string.
+  @param  Count                  The number of options in the Options
+  @param  Request                Whether this is a request or OACK. The format of
+                                 multicast is different according to this setting.
+  @param  MtftpOption            The MTFTP4_OPTION for easy access.
+
+  @retval EFI_INVALID_PARAMETER  The option is mal-formated
+  @retval EFI_UNSUPPORTED        Some option isn't supported
+  @retval EFI_SUCCESS            The option are OK and has been parsed.
+
+**/
+EFI_STATUS
+Mtftp4ParseOption (
+  IN     EFI_MTFTP4_OPTION     *Options,
+  IN     UINT32                Count,
+  IN     BOOLEAN               Request,
+     OUT MTFTP4_OPTION         *MtftpOption
+  );
+
+/**
+  Parse the options in the OACK packet to MTFTP4_OPTION which program
+  can access directly.
+
+  @param  Packet                 The OACK packet to parse
+  @param  PacketLen              The length of the packet
+  @param  MtftpOption            The MTFTP_OPTION for easy access.
+
+  @retval EFI_INVALID_PARAMETER  The packet option is mal-formated
+  @retval EFI_UNSUPPORTED        Some option isn't supported
+  @retval EFI_SUCCESS            The option are OK and has been parsed.
+
+**/
+EFI_STATUS
+Mtftp4ParseOptionOack (
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                PacketLen,
+     OUT MTFTP4_OPTION         *MtftpOption
+  );
+
+extern CHAR8  *mMtftp4SupportedOptions[MTFTP4_SUPPORTED_OPTIONS];
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Rrq.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Rrq.c
new file mode 100644
index 0000000000..e983d79791
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Rrq.c
@@ -0,0 +1,795 @@
+/** @file
+  Routines to process Rrq (download).
+  
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2014, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+
+/**
+  The packet process callback for MTFTP download.
+
+  @param  UdpPacket             The packet received
+  @param  EndPoint              The local/remote access point of the packet
+  @param  IoStatus              The status of the receiving
+  @param  Context               Opaque parameter, which is the MTFTP session
+
+**/
+VOID
+EFIAPI
+Mtftp4RrqInput (
+  IN NET_BUF                *UdpPacket,
+  IN UDP_END_POINT          *EndPoint,
+  IN EFI_STATUS             IoStatus,
+  IN VOID                   *Context
+  );
+
+
+/**
+  Start the MTFTP session to download. 
+  
+  It will first initialize some of the internal states then build and send a RRQ 
+  reqeuest packet, at last, it will start receive for the downloading.
+
+  @param  Instance              The Mtftp session
+  @param  Operation             The MTFTP opcode, it may be a EFI_MTFTP4_OPCODE_RRQ
+                                or EFI_MTFTP4_OPCODE_DIR.
+
+  @retval EFI_SUCCESS           The mtftp download session is started.
+  @retval Others                Failed to start downloading.
+
+**/
+EFI_STATUS
+Mtftp4RrqStart (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 Operation
+  )
+{
+  EFI_STATUS                Status;
+
+  //
+  // The valid block number range are [1, 0xffff]. For example:
+  // the client sends an RRQ request to the server, the server
+  // transfers the DATA1 block. If option negoitation is ongoing,
+  // the server will send back an OACK, then client will send ACK0.
+  //
+  Status = Mtftp4InitBlockRange (&Instance->Blocks, 1, 0xffff);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = Mtftp4SendRequest (Instance);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return UdpIoRecvDatagram (Instance->UnicastPort, Mtftp4RrqInput, Instance, 0);
+}
+
+
+/**
+  Build and send a ACK packet for the download session.
+
+  @param  Instance              The Mtftp session
+  @param  BlkNo                 The BlkNo to ack.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the packet
+  @retval EFI_SUCCESS           The ACK has been sent
+  @retval Others                Failed to send the ACK.
+
+**/
+EFI_STATUS
+Mtftp4RrqSendAck (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 BlkNo
+  )
+{
+  EFI_MTFTP4_PACKET         *Ack;
+  NET_BUF                   *Packet;
+
+  Packet = NetbufAlloc (sizeof (EFI_MTFTP4_ACK_HEADER));
+  if (Packet == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Ack = (EFI_MTFTP4_PACKET *) NetbufAllocSpace (
+                                Packet,
+                                sizeof (EFI_MTFTP4_ACK_HEADER),
+                                FALSE
+                                );
+  ASSERT (Ack != NULL);
+
+  Ack->Ack.OpCode   = HTONS (EFI_MTFTP4_OPCODE_ACK);
+  Ack->Ack.Block[0] = HTONS (BlkNo);
+
+  return Mtftp4SendPacket (Instance, Packet);
+}
+
+
+/**
+  Deliver the received data block to the user, which can be saved
+  in the user provide buffer or through the CheckPacket callback.
+
+  @param  Instance              The Mtftp session
+  @param  Packet                The received data packet
+  @param  Len                   The packet length
+
+  @retval EFI_SUCCESS           The data is saved successfully
+  @retval EFI_ABORTED           The user tells to abort by return an error  through
+                                CheckPacket
+  @retval EFI_BUFFER_TOO_SMALL  The user's buffer is too small and buffer length is
+                                 updated to the actual buffer size needed.
+
+**/
+EFI_STATUS
+Mtftp4RrqSaveBlock (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN     EFI_MTFTP4_PACKET      *Packet,
+  IN     UINT32                 Len
+  )
+{
+  EFI_MTFTP4_TOKEN          *Token;
+  EFI_STATUS                Status;
+  UINT16                    Block;
+  UINT64                    Start;
+  UINT32                    DataLen;
+  UINT64                    TotalBlock;
+  BOOLEAN                   Completed;
+
+  Completed = FALSE;
+  Token     = Instance->Token;
+  Block     = NTOHS (Packet->Data.Block);
+  DataLen   = Len - MTFTP4_DATA_HEAD_LEN;
+
+  //
+  // This is the last block, save the block no
+  //
+  if (DataLen < Instance->BlkSize) {
+	Completed = TRUE;
+    Instance->LastBlock = Block;
+    Mtftp4SetLastBlockNum (&Instance->Blocks, Block);
+  }
+
+  //
+  // Remove this block number from the file hole. If Mtftp4RemoveBlockNum
+  // returns EFI_NOT_FOUND, the block has been saved, don't save it again.
+  // Note that : For bigger files, allowing the block counter to roll over
+  // to accept transfers of unlimited size. So TotalBlock is memorised as 
+  // continuous block counter.
+  //
+  Status = Mtftp4RemoveBlockNum (&Instance->Blocks, Block, Completed, &TotalBlock);
+
+  if (Status == EFI_NOT_FOUND) {
+    return EFI_SUCCESS;
+  } else if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Token->CheckPacket != NULL) {
+    Status = Token->CheckPacket (&Instance->Mtftp4, Token, (UINT16) Len, Packet);
+
+    if (EFI_ERROR (Status)) {
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_ILLEGAL_OPERATION,
+        (UINT8 *) "User aborted download"
+        );
+
+      return EFI_ABORTED;
+    }
+  }
+
+  if (Token->Buffer != NULL) {
+     Start = MultU64x32 (TotalBlock - 1, Instance->BlkSize);
+
+    if (Start + DataLen <= Token->BufferSize) {
+      CopyMem ((UINT8 *) Token->Buffer + Start, Packet->Data.Data, DataLen);
+
+      //
+      // Update the file size when received the last block
+      //
+      if ((Instance->LastBlock == Block) && Completed) {
+        Token->BufferSize = Start + DataLen;
+      }
+
+    } else if (Instance->LastBlock != 0) {
+      //
+      // Don't save the data if the buffer is too small, return
+      // EFI_BUFFER_TOO_SMALL if received the last packet. This
+      // will give a accurate file length.
+      //
+      Token->BufferSize = Start + DataLen;
+
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_DISK_FULL,
+        (UINT8 *) "User provided memory block is too small"
+        );
+
+      return EFI_BUFFER_TOO_SMALL;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Function to process the received data packets. 
+  
+  It will save the block then send back an ACK if it is active.
+
+  @param  Instance              The downloading MTFTP session
+  @param  Packet                The packet received
+  @param  Len                   The length of the packet
+  @param  Multicast             Whether this packet is multicast or unicast
+  @param  Completed             Return whether the download has completed
+
+  @retval EFI_SUCCESS           The data packet is successfully processed
+  @retval EFI_ABORTED           The download is aborted by the user
+  @retval EFI_BUFFER_TOO_SMALL  The user provided buffer is too small
+
+**/
+EFI_STATUS
+Mtftp4RrqHandleData (
+  IN     MTFTP4_PROTOCOL       *Instance,
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                Len,
+  IN     BOOLEAN               Multicast,
+     OUT BOOLEAN               *Completed
+  )
+{
+  EFI_STATUS                Status;
+  UINT16                    BlockNum;
+  INTN                      Expected;
+
+  *Completed  = FALSE;
+  BlockNum    = NTOHS (Packet->Data.Block);
+  Expected    = Mtftp4GetNextBlockNum (&Instance->Blocks);
+
+  ASSERT (Expected >= 0);
+
+  //
+  // If we are active and received an unexpected packet, retransmit
+  // the last ACK then restart receiving. If we are passive, save
+  // the block.
+  //
+  if (Instance->Master && (Expected != BlockNum)) {
+    Mtftp4Retransmit (Instance);
+    return EFI_SUCCESS;
+  }
+
+  Status = Mtftp4RrqSaveBlock (Instance, Packet, Len);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Reset the passive client's timer whenever it received a
+  // valid data packet.
+  //
+  if (!Instance->Master) {
+    Mtftp4SetTimeout (Instance);
+  }
+
+  //
+  // Check whether we have received all the blocks. Send the ACK if we
+  // are active (unicast client or master client for multicast download).
+  // If we have received all the blocks, send an ACK even if we are passive
+  // to tell the server that we are done.
+  //
+  Expected = Mtftp4GetNextBlockNum (&Instance->Blocks);
+
+  if (Instance->Master || (Expected < 0)) {
+    if (Expected < 0) {
+      //
+      // If we are passive client, then the just received Block maybe
+      // isn't the last block. We need to send an ACK to the last block
+      // to inform the server that we are done. If we are active client,
+      // the Block == Instance->LastBlock.
+      //
+      BlockNum   = Instance->LastBlock;
+      *Completed = TRUE;
+
+    } else {
+      BlockNum = (UINT16) (Expected - 1);
+    }
+
+    Mtftp4RrqSendAck (Instance, BlockNum);
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Validate whether the options received in the server's OACK packet is valid.
+  
+  The options are valid only if:
+  1. The server doesn't include options not requested by us
+  2. The server can only use smaller blksize than that is requested
+  3. The server can only use the same timeout as requested
+  4. The server doesn't change its multicast channel.
+
+  @param  This                  The downloading Mtftp session
+  @param  Reply                 The options in the OACK packet
+  @param  Request               The requested options
+
+  @retval TRUE                  The options in the OACK is OK.
+  @retval FALSE                 The options in the OACK is invalid.
+
+**/
+BOOLEAN
+Mtftp4RrqOackValid (
+  IN MTFTP4_PROTOCOL        *This,
+  IN MTFTP4_OPTION          *Reply,
+  IN MTFTP4_OPTION          *Request
+  )
+{
+
+  //
+  // It is invalid for server to return options we don't request
+  //
+  if ((Reply->Exist &~Request->Exist) != 0) {
+    return FALSE;
+  }
+
+  //
+  // Server can only specify a smaller block size to be used and
+  // return the timeout matches that requested.
+  //
+  if ((((Reply->Exist & MTFTP4_BLKSIZE_EXIST) != 0)&& (Reply->BlkSize > Request->BlkSize)) ||
+      (((Reply->Exist & MTFTP4_TIMEOUT_EXIST) != 0) && (Reply->Timeout != Request->Timeout))) {
+    return FALSE;
+  }
+
+  //
+  // The server can send ",,master" to client to change its master
+  // setting. But if it use the specific multicast channel, it can't
+  // change the setting.
+  //
+  if (((Reply->Exist & MTFTP4_MCAST_EXIST) != 0) && (This->McastIp != 0)) {
+    if ((Reply->McastIp != 0) && (Reply->McastIp != This->McastIp)) {
+      return FALSE;
+    }
+
+    if ((Reply->McastPort != 0) && (Reply->McastPort != This->McastPort)) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Configure a UDP IO port to receive the multicast.
+
+  @param  McastIo               The UDP IO to configure
+  @param  Context               The opaque parameter to the function which is the
+                                MTFTP session.
+
+  @retval EFI_SUCCESS           The UDP child is successfully configured.
+  @retval Others                Failed to configure the UDP child.
+
+**/
+EFI_STATUS
+EFIAPI
+Mtftp4RrqConfigMcastPort (
+  IN UDP_IO                 *McastIo,
+  IN VOID                   *Context
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_CONFIG_DATA    *Config;
+  EFI_UDP4_CONFIG_DATA      UdpConfig;
+  EFI_IPv4_ADDRESS          Group;
+  EFI_STATUS                Status;
+  IP4_ADDR                  Ip;
+
+  Instance                     = (MTFTP4_PROTOCOL *) Context;
+  Config                       = &Instance->Config;
+
+  UdpConfig.AcceptBroadcast    = FALSE;
+  UdpConfig.AcceptPromiscuous  = FALSE;
+  UdpConfig.AcceptAnyPort      = FALSE;
+  UdpConfig.AllowDuplicatePort = FALSE;
+  UdpConfig.TypeOfService      = 0;
+  UdpConfig.TimeToLive         = 64;
+  UdpConfig.DoNotFragment      = FALSE;
+  UdpConfig.ReceiveTimeout     = 0;
+  UdpConfig.TransmitTimeout    = 0;
+  UdpConfig.UseDefaultAddress  = Config->UseDefaultSetting;
+  IP4_COPY_ADDRESS (&UdpConfig.StationAddress, &Config->StationIp);
+  IP4_COPY_ADDRESS (&UdpConfig.SubnetMask, &Config->SubnetMask);
+  UdpConfig.StationPort        = Instance->McastPort;
+  UdpConfig.RemotePort         = 0;
+
+  Ip = HTONL (Instance->ServerIp);
+  IP4_COPY_ADDRESS (&UdpConfig.RemoteAddress, &Ip);
+
+  Status = McastIo->Protocol.Udp4->Configure (McastIo->Protocol.Udp4, &UdpConfig);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (!Config->UseDefaultSetting && 
+      !EFI_IP4_EQUAL (&mZeroIp4Addr, &Config->GatewayIp)) {
+    //
+    // The station IP address is manually configured and the Gateway IP is not 0.
+    // Add the default route for this UDP instance.
+    //
+    Status = McastIo->Protocol.Udp4->Routes (
+                                       McastIo->Protocol.Udp4, 
+                                       FALSE,
+                                       &mZeroIp4Addr,
+                                       &mZeroIp4Addr,
+                                       &Config->GatewayIp
+                                       );
+                             
+    if (EFI_ERROR (Status)) {
+      McastIo->Protocol.Udp4->Configure (McastIo->Protocol.Udp4, NULL);
+      return Status;
+    }
+  }
+
+  //
+  // join the multicast group
+  //
+  Ip = HTONL (Instance->McastIp);
+  IP4_COPY_ADDRESS (&Group, &Ip);
+
+  return McastIo->Protocol.Udp4->Groups (McastIo->Protocol.Udp4, TRUE, &Group);
+}
+
+
+/**
+  Function to process the OACK. 
+  
+  It will first validate the OACK packet, then update the various negotiated parameters.
+
+  @param  Instance              The download MTFTP session
+  @param  Packet                The packet received
+  @param  Len                   The packet length
+  @param  Multicast             Whether this packet is received as a multicast
+  @param  Completed             Returns whether the download has completed. NOT
+                                used  by this function.
+
+  @retval EFI_DEVICE_ERROR      Failed to create/start a multicast UDP child
+  @retval EFI_TFTP_ERROR        Some error happened during the process
+  @retval EFI_SUCCESS           The OACK is successfully processed.
+
+**/
+EFI_STATUS
+Mtftp4RrqHandleOack (
+  IN OUT MTFTP4_PROTOCOL       *Instance,
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                Len,
+  IN     BOOLEAN               Multicast,
+     OUT BOOLEAN               *Completed
+  )
+{
+  MTFTP4_OPTION             Reply;
+  EFI_STATUS                Status;
+  INTN                      Expected;
+  EFI_UDP4_PROTOCOL         *Udp4;
+
+  *Completed = FALSE;
+
+  //
+  // If already started the master download, don't change the
+  // setting. Master download always succeeds.
+  //
+  Expected = Mtftp4GetNextBlockNum (&Instance->Blocks);
+  ASSERT (Expected != -1);
+
+  if (Instance->Master && (Expected != 1)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Parse and validate the options from server
+  //
+  ZeroMem (&Reply, sizeof (MTFTP4_OPTION));
+
+  Status = Mtftp4ParseOptionOack (Packet, Len, &Reply);
+
+  if (EFI_ERROR (Status) ||
+      !Mtftp4RrqOackValid (Instance, &Reply, &Instance->RequestOption)) {
+    //
+    // Don't send an ERROR packet if the error is EFI_OUT_OF_RESOURCES.
+    //
+    if (Status != EFI_OUT_OF_RESOURCES) {
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_ILLEGAL_OPERATION,
+        (UINT8 *) "Mal-formated OACK packet"
+        );
+    }
+
+    return EFI_TFTP_ERROR;
+  }
+
+  if ((Reply.Exist & MTFTP4_MCAST_EXIST) != 0) {
+
+    //
+    // Save the multicast info. Always update the Master, only update the
+    // multicast IP address, block size, timeoute at the first time. If IP
+    // address is updated, create a UDP child to receive the multicast.
+    //
+    Instance->Master = Reply.Master;
+
+    if (Instance->McastIp == 0) {
+      if ((Reply.McastIp == 0) || (Reply.McastPort == 0)) {
+        Mtftp4SendError (
+          Instance,
+          EFI_MTFTP4_ERRORCODE_ILLEGAL_OPERATION,
+          (UINT8 *) "Illegal multicast setting"
+          );
+
+        return EFI_TFTP_ERROR;
+      }
+
+      //
+      // Create a UDP child then start receive the multicast from it.
+      //
+      Instance->McastIp      = Reply.McastIp;
+      Instance->McastPort    = Reply.McastPort;
+      if (Instance->McastUdpPort == NULL) {
+        Instance->McastUdpPort = UdpIoCreateIo (
+                                   Instance->Service->Controller,
+                                   Instance->Service->Image,
+                                   Mtftp4RrqConfigMcastPort,
+                                   UDP_IO_UDP4_VERSION,
+                                   Instance
+                                   );
+        if (Instance->McastUdpPort != NULL) {
+          Status = gBS->OpenProtocol (
+                          Instance->McastUdpPort->UdpHandle,
+                          &gEfiUdp4ProtocolGuid,
+                          (VOID **) &Udp4,
+                          Instance->Service->Image,
+                          Instance->Handle,
+                          EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                          );
+          if (EFI_ERROR (Status)) {
+            UdpIoFreeIo (Instance->McastUdpPort);
+            Instance->McastUdpPort = NULL;
+            return EFI_DEVICE_ERROR;
+          }
+        }
+      }
+
+
+      if (Instance->McastUdpPort == NULL) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      Status = UdpIoRecvDatagram (Instance->McastUdpPort, Mtftp4RrqInput, Instance, 0);
+
+      if (EFI_ERROR (Status)) {
+        Mtftp4SendError (
+          Instance,
+          EFI_MTFTP4_ERRORCODE_ACCESS_VIOLATION,
+          (UINT8 *) "Failed to create socket to receive multicast packet"
+          );
+
+        return Status;
+      }
+    
+      //
+      // Update the parameters used.
+      //
+      if (Reply.BlkSize != 0) {
+        Instance->BlkSize = Reply.BlkSize;
+      }
+      
+      if (Reply.Timeout != 0) {
+        Instance->Timeout = Reply.Timeout;
+      }  
+    }    
+    
+  } else {
+    Instance->Master = TRUE;
+    
+    if (Reply.BlkSize != 0) {
+      Instance->BlkSize = Reply.BlkSize;
+    }
+
+    if (Reply.Timeout != 0) {
+      Instance->Timeout = Reply.Timeout;
+    }
+  }
+  
+  //
+  // Send an ACK to (Expected - 1) which is 0 for unicast download,
+  // or tell the server we want to receive the Expected block.
+  //
+  return Mtftp4RrqSendAck (Instance, (UINT16) (Expected - 1));
+}
+
+
+/**
+  The packet process callback for MTFTP download.
+
+  @param  UdpPacket             The packet received
+  @param  EndPoint              The local/remote access point of the packet
+  @param  IoStatus              The status of the receiving
+  @param  Context               Opaque parameter, which is the MTFTP session
+
+**/
+VOID
+EFIAPI
+Mtftp4RrqInput (
+  IN NET_BUF                *UdpPacket,
+  IN UDP_END_POINT          *EndPoint,
+  IN EFI_STATUS             IoStatus,
+  IN VOID                   *Context
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_PACKET         *Packet;
+  BOOLEAN                   Completed;
+  BOOLEAN                   Multicast;
+  EFI_STATUS                Status;
+  UINT16                    Opcode;
+  UINT32                    Len;
+
+  Instance  = (MTFTP4_PROTOCOL *) Context;
+  NET_CHECK_SIGNATURE (Instance, MTFTP4_PROTOCOL_SIGNATURE);
+
+  Status    = EFI_SUCCESS;
+  Packet    = NULL;
+  Completed = FALSE;
+  Multicast = FALSE;
+
+  if (EFI_ERROR (IoStatus)) {
+    Status = IoStatus;
+    goto ON_EXIT;
+  }
+
+  ASSERT (UdpPacket != NULL);
+
+  //
+  // Find the port this packet is from to restart receive correctly.
+  //
+  Multicast = (BOOLEAN) (EndPoint->LocalAddr.Addr[0] == Instance->McastIp);
+
+  if (UdpPacket->TotalSize < MTFTP4_OPCODE_LEN) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Client send initial request to server's listening port. Server
+  // will select a UDP port to communicate with the client. The server
+  // is required to use the same port as RemotePort to multicast the
+  // data.
+  //
+  if (EndPoint->RemotePort != Instance->ConnectedPort) {
+    if (Instance->ConnectedPort != 0) {
+      goto ON_EXIT;
+    } else {
+      Instance->ConnectedPort = EndPoint->RemotePort;
+    }
+  }
+
+  //
+  // Copy the MTFTP packet to a continuous buffer if it isn't already so.
+  //
+  Len = UdpPacket->TotalSize;
+
+  if (UdpPacket->BlockOpNum > 1) {
+    Packet = AllocatePool (Len);
+
+    if (Packet == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    NetbufCopy (UdpPacket, 0, Len, (UINT8 *) Packet);
+
+  } else {
+    Packet = (EFI_MTFTP4_PACKET *) NetbufGetByte (UdpPacket, 0, NULL);
+    ASSERT (Packet != NULL);
+  }
+
+  Opcode = NTOHS (Packet->OpCode);
+
+  //
+  // Call the user's CheckPacket if provided. Abort the transmission
+  // if CheckPacket returns an EFI_ERROR code.
+  //
+  if ((Instance->Token->CheckPacket != NULL) &&
+      ((Opcode == EFI_MTFTP4_OPCODE_OACK) || (Opcode == EFI_MTFTP4_OPCODE_ERROR))) {
+
+    Status = Instance->Token->CheckPacket (
+                                &Instance->Mtftp4,
+                                Instance->Token,
+                                (UINT16) Len,
+                                Packet
+                                );
+
+    if (EFI_ERROR (Status)) {
+      //
+      // Send an error message to the server to inform it
+      //
+      if (Opcode != EFI_MTFTP4_OPCODE_ERROR) {
+        Mtftp4SendError (
+          Instance,
+          EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
+          (UINT8 *) "User aborted the transfer"
+          );
+      }
+
+      Status = EFI_ABORTED;
+      goto ON_EXIT;
+    }
+  }
+
+  switch (Opcode) {
+  case EFI_MTFTP4_OPCODE_DATA:
+    if ((Len > (UINT32) (MTFTP4_DATA_HEAD_LEN + Instance->BlkSize)) ||
+        (Len < (UINT32) MTFTP4_DATA_HEAD_LEN)) {
+      goto ON_EXIT;
+    }
+
+    Status = Mtftp4RrqHandleData (Instance, Packet, Len, Multicast, &Completed);
+    break;
+
+  case EFI_MTFTP4_OPCODE_OACK:
+    if (Multicast || (Len <= MTFTP4_OPCODE_LEN)) {
+      goto ON_EXIT;
+    }
+
+    Status = Mtftp4RrqHandleOack (Instance, Packet, Len, Multicast, &Completed);
+    break;
+
+  case EFI_MTFTP4_OPCODE_ERROR:
+    Status = EFI_TFTP_ERROR;
+    break;
+    
+  default:
+    break;
+  }
+
+ON_EXIT:
+
+  //
+  // Free the resources, then if !EFI_ERROR (Status), restart the
+  // receive, otherwise end the session.
+  //
+  if ((Packet != NULL) && (UdpPacket->BlockOpNum > 1)) {
+    FreePool (Packet);
+  }
+
+  if (UdpPacket != NULL) {
+    NetbufFree (UdpPacket);
+  }
+
+  if (!EFI_ERROR (Status) && !Completed) {
+    if (Multicast) {
+      Status = UdpIoRecvDatagram (Instance->McastUdpPort, Mtftp4RrqInput, Instance, 0);
+    } else {
+      Status = UdpIoRecvDatagram (Instance->UnicastPort, Mtftp4RrqInput, Instance, 0);
+    }
+  }
+
+  if (EFI_ERROR (Status) || Completed) {
+    Mtftp4CleanOperation (Instance, Status);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.c
new file mode 100644
index 0000000000..c625fda020
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.c
@@ -0,0 +1,634 @@
+/** @file
+  Support routines for Mtftp.
+  
+Copyright (c) 2006 - 2015, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+
+/**
+  Allocate a MTFTP4 block range, then init it to the range of [Start, End]
+
+  @param  Start                 The start block number
+  @param  End                   The last block number in the range
+
+  @return Pointer to the created block range, NULL if failed to allocate memory.
+
+**/
+MTFTP4_BLOCK_RANGE *
+Mtftp4AllocateRange (
+  IN UINT16                 Start,
+  IN UINT16                 End
+  )
+{
+  MTFTP4_BLOCK_RANGE        *Range;
+
+  Range = AllocateZeroPool (sizeof (MTFTP4_BLOCK_RANGE));
+
+  if (Range == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&Range->Link);
+  Range->Start  = Start;
+  Range->End    = End;
+  Range->Bound  = End;
+
+  return Range;
+}
+
+
+/**
+  Initialize the block range for either RRQ or WRQ. 
+  
+  RRQ and WRQ have different requirements for Start and End. 
+  For example, during start up, WRQ initializes its whole valid block range 
+  to [0, 0xffff]. This is bacause the server will send us a ACK0 to inform us 
+  to start the upload. When the client received ACK0, it will remove 0 from the 
+  range, get the next block number, which is 1, then upload the BLOCK1. For RRQ
+  without option negotiation, the server will directly send us the BLOCK1 in 
+  response to the client's RRQ. When received BLOCK1, the client will remove 
+  it from the block range and send an ACK. It also works if there is option 
+  negotiation.
+
+  @param  Head                  The block range head to initialize
+  @param  Start                 The Start block number.
+  @param  End                   The last block number.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for initial block range
+  @retval EFI_SUCCESS           The initial block range is created.
+
+**/
+EFI_STATUS
+Mtftp4InitBlockRange (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Start,
+  IN UINT16                 End
+  )
+{
+  MTFTP4_BLOCK_RANGE        *Range;
+
+  Range = Mtftp4AllocateRange (Start, End);
+
+  if (Range == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  InsertTailList (Head, &Range->Link);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the first valid block number on the range list.
+
+  @param  Head                  The block range head
+
+  @return The first valid block number, -1 if the block range is empty. 
+
+**/
+INTN
+Mtftp4GetNextBlockNum (
+  IN LIST_ENTRY             *Head
+  )
+{
+  MTFTP4_BLOCK_RANGE  *Range;
+
+  if (IsListEmpty (Head)) {
+    return -1;
+  }
+
+  Range = NET_LIST_HEAD (Head, MTFTP4_BLOCK_RANGE, Link);
+  return Range->Start;
+}
+
+
+/**
+  Set the last block number of the block range list. 
+  
+  It will remove all the blocks after the Last. MTFTP initialize the block range
+  to the maximum possible range, such as [0, 0xffff] for WRQ. When it gets the 
+  last block number, it will call this function to set the last block number.
+
+  @param  Head                  The block range list
+  @param  Last                  The last block number
+
+**/
+VOID
+Mtftp4SetLastBlockNum (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Last
+  )
+{
+  MTFTP4_BLOCK_RANGE        *Range;
+
+  //
+  // Iterate from the tail to head to remove the block number
+  // after the last.
+  //
+  while (!IsListEmpty (Head)) {
+    Range = NET_LIST_TAIL (Head, MTFTP4_BLOCK_RANGE, Link);
+
+    if (Range->Start > Last) {
+      RemoveEntryList (&Range->Link);
+      FreePool (Range);
+      continue;
+    }
+
+    if (Range->End > Last) {
+      Range->End = Last;
+    }
+
+    return ;
+  }
+}
+
+
+/**
+  Remove the block number from the block range list.
+
+  @param  Head                  The block range list to remove from
+  @param  Num                   The block number to remove
+  @param  Completed             Whether Num is the last block number
+  @param  TotalBlock            The continuous block number in all 
+
+  @retval EFI_NOT_FOUND         The block number isn't in the block range list
+  @retval EFI_SUCCESS           The block number has been removed from the list
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource
+
+**/
+EFI_STATUS
+Mtftp4RemoveBlockNum (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Num,
+  IN BOOLEAN                Completed,
+  OUT UINT64                *TotalBlock
+  )
+{
+  MTFTP4_BLOCK_RANGE        *Range;
+  MTFTP4_BLOCK_RANGE        *NewRange;
+  LIST_ENTRY                *Entry;
+
+  NET_LIST_FOR_EACH (Entry, Head) {
+
+    //
+    // Each block represents a hole [Start, End] in the file,
+    // skip to the first range with End >= Num
+    //
+    Range = NET_LIST_USER_STRUCT (Entry, MTFTP4_BLOCK_RANGE, Link);
+
+    if (Range->End < Num) {
+      continue;
+    }
+
+    //
+    // There are three different cases for Start
+    // 1. (Start > Num) && (End >= Num):
+    //    because all the holes before this one has the condition of
+    //    End < Num, so this block number has been removed.
+    //
+    // 2. (Start == Num) && (End >= Num):
+    //    Need to increase the Start by one, and if End == Num, this
+    //    hole has been removed completely, remove it.
+    //
+    // 3. (Start < Num) && (End >= Num):
+    //    if End == Num, only need to decrease the End by one because
+    //    we have (Start < Num) && (Num == End), so (Start <= End - 1).
+    //    if (End > Num), the hold is splited into two holes, with
+    //    [Start, Num - 1] and [Num + 1, End].
+    //
+    if (Range->Start > Num) {
+      return EFI_NOT_FOUND;
+
+    } else if (Range->Start == Num) {
+      Range->Start++;
+
+      //
+      // Note that: RFC 1350 does not mention block counter roll-over, 
+      // but several TFTP hosts implement the roll-over be able to accept 
+      // transfers of unlimited size. There is no consensus, however, whether 
+      // the counter should wrap around to zero or to one. Many implementations 
+      // wrap to zero, because this is the simplest to implement. Here we choose 
+      // this solution.
+      //
+	  *TotalBlock  = Num;
+	  
+      if (Range->Round > 0) {
+	    *TotalBlock += Range->Bound +  MultU64x32 ((UINTN) (Range->Round -1), (UINT32) (Range->Bound + 1)) + 1;
+	  }
+
+      if (Range->Start > Range->Bound) {
+	  	  Range->Start = 0;
+		  Range->Round ++;
+      }
+
+      if ((Range->Start > Range->End) || Completed) {
+        RemoveEntryList (&Range->Link);
+        FreePool (Range);
+      }
+
+      return EFI_SUCCESS;
+
+    } else {
+      if (Range->End == Num) {
+        Range->End--;
+      } else {
+        NewRange = Mtftp4AllocateRange ((UINT16) (Num + 1), (UINT16) Range->End);
+
+        if (NewRange == NULL) {
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        Range->End = Num - 1;
+        NetListInsertAfter (&Range->Link, &NewRange->Link);
+      }
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Build then transmit the request packet for the MTFTP session.
+
+  @param  Instance              The Mtftp session
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the request
+  @retval EFI_SUCCESS           The request is built and sent
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendRequest (
+  IN MTFTP4_PROTOCOL        *Instance
+  )
+{
+  EFI_MTFTP4_PACKET         *Packet;
+  EFI_MTFTP4_OPTION         *Options;
+  EFI_MTFTP4_TOKEN          *Token;
+  RETURN_STATUS             Status;
+  NET_BUF                   *Nbuf;
+  UINT8                     *Mode;
+  UINT8                     *Cur;
+  UINTN                     Index;
+  UINT32                    BufferLength;
+  UINTN                     FileNameLength;
+  UINTN                     ModeLength;
+  UINTN                     OptionStrLength;
+  UINTN                     ValueStrLength;
+
+  Token   = Instance->Token;
+  Options = Token->OptionList;
+  Mode    = Instance->Token->ModeStr;
+
+  if (Mode == NULL) {
+    Mode = (UINT8 *) "octet";
+  }
+
+  //
+  // Compute the packet length
+  //
+  FileNameLength = AsciiStrLen ((CHAR8 *) Token->Filename);
+  ModeLength     = AsciiStrLen ((CHAR8 *) Mode);
+  BufferLength   = (UINT32) FileNameLength + (UINT32) ModeLength + 4;
+
+  for (Index = 0; Index < Token->OptionCount; Index++) {
+    OptionStrLength = AsciiStrLen ((CHAR8 *) Options[Index].OptionStr);
+    ValueStrLength  = AsciiStrLen ((CHAR8 *) Options[Index].ValueStr);
+    BufferLength   += (UINT32) OptionStrLength + (UINT32) ValueStrLength + 2;
+  }
+  //
+  // Allocate a packet then copy the data over
+  //
+  if ((Nbuf = NetbufAlloc (BufferLength)) == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Packet         = (EFI_MTFTP4_PACKET *) NetbufAllocSpace (Nbuf, BufferLength, FALSE);
+  ASSERT (Packet != NULL);
+
+  Packet->OpCode = HTONS (Instance->Operation);
+  BufferLength  -= sizeof (Packet->OpCode);
+  
+  Cur            = Packet->Rrq.Filename;
+  Status         = AsciiStrCpyS ((CHAR8 *) Cur, BufferLength, (CHAR8 *) Token->Filename);
+  ASSERT_EFI_ERROR (Status);
+  BufferLength  -= (UINT32) (FileNameLength + 1);
+  Cur           += FileNameLength + 1;
+  Status         = AsciiStrCpyS ((CHAR8 *) Cur, BufferLength, (CHAR8 *) Mode);
+  ASSERT_EFI_ERROR (Status);
+  BufferLength  -= (UINT32) (ModeLength + 1);
+  Cur           += ModeLength + 1;
+
+  for (Index = 0; Index < Token->OptionCount; ++Index) {
+    OptionStrLength = AsciiStrLen ((CHAR8 *) Options[Index].OptionStr);
+    ValueStrLength  = AsciiStrLen ((CHAR8 *) Options[Index].ValueStr);
+    
+    Status          = AsciiStrCpyS ((CHAR8 *) Cur, BufferLength, (CHAR8 *) Options[Index].OptionStr);
+    ASSERT_EFI_ERROR (Status);
+    BufferLength   -= (UINT32) (OptionStrLength + 1);
+    Cur            += OptionStrLength + 1;
+    
+    Status          = AsciiStrCpyS ((CHAR8 *) Cur, BufferLength, (CHAR8 *) Options[Index].ValueStr);
+    ASSERT_EFI_ERROR (Status);
+    BufferLength   -= (UINT32) (ValueStrLength + 1);
+    Cur            += ValueStrLength + 1;
+    
+  }
+
+  return Mtftp4SendPacket (Instance, Nbuf);
+}
+
+
+/**
+  Build then send an error message.
+
+  @param  Instance              The MTFTP session
+  @param  ErrCode               The error code  
+  @param  ErrInfo               The error message
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the error packet
+  @retval EFI_SUCCESS           The error packet is transmitted.
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendError (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 ErrCode,
+  IN UINT8                  *ErrInfo
+  )
+{
+  NET_BUF                   *Packet;
+  EFI_MTFTP4_PACKET         *TftpError;
+  UINT32                    Len;
+
+  Len     = (UINT32) (AsciiStrLen ((CHAR8 *) ErrInfo) + sizeof (EFI_MTFTP4_ERROR_HEADER));
+  Packet  = NetbufAlloc (Len);
+  if (Packet == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  TftpError         = (EFI_MTFTP4_PACKET *) NetbufAllocSpace (Packet, Len, FALSE);
+  ASSERT (TftpError != NULL);
+
+  TftpError->OpCode = HTONS (EFI_MTFTP4_OPCODE_ERROR);
+  TftpError->Error.ErrorCode = HTONS (ErrCode);
+
+  AsciiStrCpyS ((CHAR8 *) TftpError->Error.ErrorMessage, Len, (CHAR8 *) ErrInfo);
+
+  return Mtftp4SendPacket (Instance, Packet);
+}
+
+
+/**
+  The callback function called when the packet is transmitted.
+  
+  It simply frees the packet.
+
+  @param  Packet                The transmitted (or failed to) packet
+  @param  EndPoint              The local and remote UDP access point
+  @param  IoStatus              The result of the transmission
+  @param  Context               Opaque parameter to the callback
+
+**/
+VOID
+EFIAPI
+Mtftp4OnPacketSent (
+  IN NET_BUF                   *Packet,
+  IN UDP_END_POINT             *EndPoint,
+  IN EFI_STATUS                IoStatus,
+  IN VOID                      *Context
+  )
+{
+  NetbufFree (Packet);
+}
+
+
+/**
+  Set the timeout for the instance. User a longer time for passive instances.
+
+  @param  Instance              The Mtftp session to set time out
+
+**/
+VOID
+Mtftp4SetTimeout (
+  IN OUT MTFTP4_PROTOCOL        *Instance
+  )
+{
+  if (Instance->Master) {
+    Instance->PacketToLive = Instance->Timeout;
+  } else {
+    Instance->PacketToLive = Instance->Timeout * 2;
+  }
+}
+
+
+/**
+  Send the packet for the instance. 
+  
+  It will first save a reference to the packet for later retransmission. 
+  Then determine the destination port, listen port for requests, and connected 
+  port for others. At last, send the packet out.
+
+  @param  Instance              The Mtftp instance
+  @param  Packet                The packet to send
+
+  @retval EFI_SUCCESS           The packet is sent out
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendPacket (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN OUT NET_BUF                *Packet
+  )
+{
+  UDP_END_POINT             UdpPoint;
+  EFI_STATUS                Status;
+  UINT16                    OpCode;
+  UINT8                     *Buffer;
+
+  //
+  // Save the packet for retransmission
+  //
+  if (Instance->LastPacket != NULL) {
+    NetbufFree (Instance->LastPacket);
+  }
+
+  Instance->LastPacket        = Packet;
+
+  Instance->CurRetry          = 0;
+  Mtftp4SetTimeout (Instance);
+
+  ZeroMem (&UdpPoint, sizeof (UdpPoint));
+  UdpPoint.RemoteAddr.Addr[0] = Instance->ServerIp;
+
+  //
+  // Send the requests to the listening port, other packets
+  // to the connected port
+  //
+  Buffer = NetbufGetByte (Packet, 0, NULL);
+  ASSERT (Buffer != NULL);
+  OpCode = NTOHS (*(UINT16 *)Buffer);
+
+  if ((OpCode == EFI_MTFTP4_OPCODE_RRQ) || 
+      (OpCode == EFI_MTFTP4_OPCODE_DIR) ||
+      (OpCode == EFI_MTFTP4_OPCODE_WRQ)) {
+    UdpPoint.RemotePort = Instance->ListeningPort;
+  } else {
+    UdpPoint.RemotePort = Instance->ConnectedPort;
+  }
+
+  NET_GET_REF (Packet);
+
+  Status = UdpIoSendDatagram (
+             Instance->UnicastPort,
+             Packet,
+             &UdpPoint,
+             NULL,
+             Mtftp4OnPacketSent,
+             Instance
+             );
+
+  if (EFI_ERROR (Status)) {
+    NET_PUT_REF (Packet);
+  }
+
+  return Status;
+}
+
+
+/**
+  Retransmit the last packet for the instance.
+
+  @param  Instance              The Mtftp instance
+
+  @retval EFI_SUCCESS           The last packet is retransmitted.
+  @retval Others                Failed to retransmit.
+
+**/
+EFI_STATUS
+Mtftp4Retransmit (
+  IN MTFTP4_PROTOCOL        *Instance
+  )
+{
+  UDP_END_POINT             UdpPoint;
+  EFI_STATUS                Status;
+  UINT16                    OpCode;
+  UINT8                     *Buffer;
+
+  ASSERT (Instance->LastPacket != NULL);
+
+  ZeroMem (&UdpPoint, sizeof (UdpPoint));
+  UdpPoint.RemoteAddr.Addr[0] = Instance->ServerIp;
+
+  //
+  // Set the requests to the listening port, other packets to the connected port
+  //
+  Buffer = NetbufGetByte (Instance->LastPacket, 0, NULL);
+  ASSERT (Buffer != NULL);
+  OpCode = NTOHS (*(UINT16 *) Buffer);
+
+  if ((OpCode == EFI_MTFTP4_OPCODE_RRQ) || (OpCode == EFI_MTFTP4_OPCODE_DIR) ||
+      (OpCode == EFI_MTFTP4_OPCODE_WRQ)) {
+    UdpPoint.RemotePort = Instance->ListeningPort;
+  } else {
+    UdpPoint.RemotePort = Instance->ConnectedPort;
+  }
+
+  NET_GET_REF (Instance->LastPacket);
+
+  Status = UdpIoSendDatagram (
+             Instance->UnicastPort,
+             Instance->LastPacket,
+             &UdpPoint,
+             NULL,
+             Mtftp4OnPacketSent,
+             Instance
+             );
+
+  if (EFI_ERROR (Status)) {
+    NET_PUT_REF (Instance->LastPacket);
+  }
+
+  return Status;
+}
+
+
+/**
+  The timer ticking function for the Mtftp service instance.
+
+  @param  Event                 The ticking event
+  @param  Context               The Mtftp service instance
+
+**/
+VOID
+EFIAPI
+Mtftp4OnTimerTick (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  )
+{
+  MTFTP4_SERVICE            *MtftpSb;
+  LIST_ENTRY                *Entry;
+  LIST_ENTRY                *Next;
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_TOKEN          *Token;
+
+  MtftpSb = (MTFTP4_SERVICE *) Context;
+
+  //
+  // Iterate through all the children of the Mtftp service instance. Time
+  // out the packet. If maximum retries reached, clean the session up.
+  //
+  NET_LIST_FOR_EACH_SAFE (Entry, Next, &MtftpSb->Children) {
+    Instance = NET_LIST_USER_STRUCT (Entry, MTFTP4_PROTOCOL, Link);
+
+    if ((Instance->PacketToLive == 0) || (--Instance->PacketToLive > 0)) {
+      continue;
+    }
+
+    //
+    // Call the user's time out handler
+    //
+    Token = Instance->Token;
+
+    if ((Token->TimeoutCallback != NULL) &&
+        EFI_ERROR (Token->TimeoutCallback (&Instance->Mtftp4, Token))) {
+
+      Mtftp4SendError (
+         Instance,
+         EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
+         (UINT8 *) "User aborted the transfer in time out"
+         );
+
+      Mtftp4CleanOperation (Instance, EFI_ABORTED);
+      continue;
+    }
+
+    //
+    // Retransmit the packet if haven't reach the maxmium retry count,
+    // otherwise exit the transfer.
+    //
+    if (++Instance->CurRetry < Instance->MaxRetry) {
+      Mtftp4Retransmit (Instance);
+      Mtftp4SetTimeout (Instance);
+    } else {
+      Mtftp4CleanOperation (Instance, EFI_TIMEOUT);
+      continue;
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.h b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.h
new file mode 100644
index 0000000000..802e3867db
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Support.h
@@ -0,0 +1,203 @@
+/** @file
+  Support routines for MTFTP.
+  
+Copyright (c) 2006 - 2010, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef __EFI_MTFTP4_SUPPORT_H__
+#define __EFI_MTFTP4_SUPPORT_H__
+
+//
+// The structure representing a range of block numbers, [Start, End].
+// It is used to remember the holes in the MTFTP block space. If all
+// the holes are filled in, then the download or upload has completed.
+//
+typedef struct {
+  LIST_ENTRY                Link;
+  INTN                      Start;
+  INTN                      End;
+  INTN                      Round;
+  INTN                      Bound;
+} MTFTP4_BLOCK_RANGE;
+
+
+/**
+  Initialize the block range for either RRQ or WRQ. 
+  
+  RRQ and WRQ have different requirements for Start and End. 
+  For example, during start up, WRQ initializes its whole valid block range 
+  to [0, 0xffff]. This is bacause the server will send us a ACK0 to inform us 
+  to start the upload. When the client received ACK0, it will remove 0 from the 
+  range, get the next block number, which is 1, then upload the BLOCK1. For RRQ
+  without option negotiation, the server will directly send us the BLOCK1 in 
+  response to the client's RRQ. When received BLOCK1, the client will remove 
+  it from the block range and send an ACK. It also works if there is option 
+  negotiation.
+
+  @param  Head                  The block range head to initialize
+  @param  Start                 The Start block number.
+  @param  End                   The last block number.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for initial block range
+  @retval EFI_SUCCESS           The initial block range is created.
+
+**/
+EFI_STATUS
+Mtftp4InitBlockRange (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Start,
+  IN UINT16                 End
+  );
+
+/**
+  Get the first valid block number on the range list.
+
+  @param  Head                  The block range head
+
+  @return The first valid block number, -1 if the block range is empty. 
+
+**/
+INTN
+Mtftp4GetNextBlockNum (
+  IN LIST_ENTRY             *Head
+  );
+
+/**
+  Set the last block number of the block range list. 
+  
+  It will remove all the blocks after the Last. MTFTP initialize the block range
+  to the maximum possible range, such as [0, 0xffff] for WRQ. When it gets the 
+  last block number, it will call this function to set the last block number.
+
+  @param  Head                  The block range list
+  @param  Last                  The last block number
+
+**/
+VOID
+Mtftp4SetLastBlockNum (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Last
+  );
+
+/**
+  Remove the block number from the block range list.
+
+  @param  Head                  The block range list to remove from
+  @param  Num                   The block number to remove
+  @param  Completed             Wether Num is the last block number
+  @param  TotalBlock            The continuous block number in all 
+
+  @retval EFI_NOT_FOUND         The block number isn't in the block range list
+  @retval EFI_SUCCESS           The block number has been removed from the list
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate resource
+
+**/
+EFI_STATUS
+Mtftp4RemoveBlockNum (
+  IN LIST_ENTRY             *Head,
+  IN UINT16                 Num,
+  IN BOOLEAN                Completed,
+  OUT UINT64                *TotalBlock
+  );
+
+/**
+  Set the timeout for the instance. User a longer time for passive instances.
+
+  @param  Instance              The Mtftp session to set time out
+
+**/
+VOID
+Mtftp4SetTimeout (
+  IN OUT MTFTP4_PROTOCOL        *Instance
+  );
+
+/**
+  Send the packet for the instance. 
+  
+  It will first save a reference to the packet for later retransmission. 
+  Then determine the destination port, listen port for requests, and connected 
+  port for others. At last, send the packet out.
+
+  @param  Instance              The Mtftp instance
+  @param  Packet                The packet to send
+
+  @retval EFI_SUCCESS           The packet is sent out
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendPacket (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN OUT NET_BUF                *Packet
+  );
+
+/**
+  Build then transmit the request packet for the MTFTP session.
+
+  @param  Instance              The Mtftp session
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the request
+  @retval EFI_SUCCESS           The request is built and sent
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendRequest (
+  IN MTFTP4_PROTOCOL        *Instance
+  );
+
+/**
+  Build then send an error message.
+
+  @param  Instance              The MTFTP session
+  @param  ErrCode               The error code  
+  @param  ErrInfo               The error message
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to allocate memory for the error packet
+  @retval EFI_SUCCESS           The error packet is transmitted.
+  @retval Others                Failed to transmit the packet.
+
+**/
+EFI_STATUS
+Mtftp4SendError (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 ErrCode,
+  IN UINT8                  *ErrInfo
+  );
+
+/**
+  Retransmit the last packet for the instance.
+
+  @param  Instance              The Mtftp instance
+
+  @retval EFI_SUCCESS           The last packet is retransmitted.
+  @retval Others                Failed to retransmit.
+
+**/
+EFI_STATUS
+Mtftp4Retransmit (
+  IN MTFTP4_PROTOCOL        *Instance
+  );
+
+/**
+  The timer ticking function for the Mtftp service instance.
+
+  @param  Event                 The ticking event
+  @param  Context               The Mtftp service instance
+
+**/
+VOID
+EFIAPI
+Mtftp4OnTimerTick (
+  IN EFI_EVENT              Event,
+  IN VOID                   *Context
+  );
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Wrq.c b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Wrq.c
new file mode 100644
index 0000000000..e825714700
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Mtftp4Dxe/Mtftp4Wrq.c
@@ -0,0 +1,531 @@
+/** @file
+  Routines to process Wrq (upload).
+  
+Copyright (c) 2006 - 2014, 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<BR>
+
+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 "Mtftp4Impl.h"
+
+
+
+/**
+  Build then send a MTFTP data packet for the MTFTP upload session.
+
+  @param  Instance              The MTFTP upload session.
+  @param  BlockNum              The block number to send.
+
+  @retval EFI_OUT_OF_RESOURCES  Failed to build the packet.
+  @retval EFI_ABORTED           The consumer of this child directs to abort the
+                                transmission by return an error through PacketNeeded.
+  @retval EFI_SUCCESS           The data is sent.
+
+**/
+EFI_STATUS
+Mtftp4WrqSendBlock (
+  IN OUT MTFTP4_PROTOCOL        *Instance,
+  IN     UINT16                 BlockNum
+  )
+{
+  EFI_MTFTP4_PACKET         *Packet;
+  EFI_MTFTP4_TOKEN          *Token;
+  NET_BUF                   *UdpPacket;
+  EFI_STATUS                Status;
+  UINT16                    DataLen;
+  UINT8                     *DataBuf;
+  UINT64                    Start;
+
+  //
+  // Allocate a buffer to hold the user data
+  //
+  UdpPacket = NetbufAlloc (Instance->BlkSize + MTFTP4_DATA_HEAD_LEN);
+
+  if (UdpPacket == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Packet = (EFI_MTFTP4_PACKET *) NetbufAllocSpace (UdpPacket, MTFTP4_DATA_HEAD_LEN, FALSE);
+  ASSERT (Packet != NULL);
+
+  Packet->Data.OpCode = HTONS (EFI_MTFTP4_OPCODE_DATA);
+  Packet->Data.Block  = HTONS (BlockNum);
+
+  //
+  // Read the block from either the buffer or PacketNeeded callback
+  //
+  Token   = Instance->Token;
+  DataLen = Instance->BlkSize;
+
+  if (Token->Buffer != NULL) {
+    Start = MultU64x32 (BlockNum - 1, Instance->BlkSize);
+
+    if (Token->BufferSize < Start + Instance->BlkSize) {
+      DataLen             = (UINT16) (Token->BufferSize - Start);
+      Instance->LastBlock = BlockNum;
+      Mtftp4SetLastBlockNum (&Instance->Blocks, BlockNum);
+    }
+
+    if (DataLen > 0) {
+      NetbufAllocSpace (UdpPacket, DataLen, FALSE);
+      CopyMem (Packet->Data.Data, (UINT8 *) Token->Buffer + Start, DataLen);
+    }
+
+  } else {
+    //
+    // Get data from PacketNeeded
+    //
+    DataBuf = NULL;
+    Status  = Token->PacketNeeded (
+                       &Instance->Mtftp4,
+                       Token,
+                       &DataLen,
+                       (VOID **) &DataBuf
+                       );
+
+    if (EFI_ERROR (Status) || (DataLen > Instance->BlkSize)) {
+      if (DataBuf != NULL) {
+        FreePool (DataBuf);
+      }
+
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
+        (UINT8 *) "User aborted the transfer"
+        );
+
+      return EFI_ABORTED;
+    }
+
+    if (DataLen < Instance->BlkSize) {
+      Instance->LastBlock = BlockNum;
+      Mtftp4SetLastBlockNum (&Instance->Blocks, BlockNum);
+    }
+
+    if (DataLen > 0) {
+      NetbufAllocSpace (UdpPacket, DataLen, FALSE);
+      CopyMem (Packet->Data.Data, DataBuf, DataLen);
+      FreePool (DataBuf);
+    }
+  }
+
+  return Mtftp4SendPacket (Instance, UdpPacket);
+}
+
+
+/**
+  Function to handle received ACK packet. 
+  
+  If the ACK number matches the expected block number, and there are more 
+  data pending, send the next block. Otherwise tell the caller that we are done.
+
+  @param  Instance              The MTFTP upload session
+  @param  Packet                The MTFTP packet received
+  @param  Len                   The packet length
+  @param  Completed             Return whether the upload has finished.
+
+  @retval EFI_SUCCESS           The ACK is successfully processed.
+  @retval EFI_TFTP_ERROR        The block number loops back.
+  @retval Others                Failed to transmit the next data packet.
+
+**/
+EFI_STATUS
+Mtftp4WrqHandleAck (
+  IN     MTFTP4_PROTOCOL       *Instance,
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                Len,
+     OUT BOOLEAN               *Completed
+  )
+{
+  UINT16                    AckNum;
+  INTN                      Expected;
+  UINT64                    TotalBlock;
+ 
+  *Completed  = FALSE;
+  AckNum      = NTOHS (Packet->Ack.Block[0]);
+  Expected    = Mtftp4GetNextBlockNum (&Instance->Blocks);
+
+  ASSERT (Expected >= 0);
+
+  //
+  // Get an unwanted ACK, return EFI_SUCCESS to let Mtftp4WrqInput
+  // restart receive.
+  //
+  if (Expected != AckNum) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Remove the acked block number, if this is the last block number,
+  // tell the Mtftp4WrqInput to finish the transfer. This is the last
+  // block number if the block range are empty..
+  //
+  Mtftp4RemoveBlockNum (&Instance->Blocks, AckNum, *Completed,&TotalBlock);
+
+  Expected = Mtftp4GetNextBlockNum (&Instance->Blocks);
+
+  if (Expected < 0) {
+  
+    //
+    // The block range is empty. It may either because the the last
+    // block has been ACKed, or the sequence number just looped back,
+    // that is, there is more than 0xffff blocks.
+    //
+    if (Instance->LastBlock == AckNum) {
+      ASSERT (Instance->LastBlock >= 1);
+      *Completed = TRUE;
+      return EFI_SUCCESS;
+
+    } else {
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
+        (UINT8 *) "Block number rolls back, not supported, try blksize option"
+        );
+
+      return EFI_TFTP_ERROR;
+    }
+  }
+
+  return Mtftp4WrqSendBlock (Instance, (UINT16) Expected);
+}
+
+
+/**
+  Check whether the received OACK is valid. 
+  
+  The OACK is valid only if:
+  1. It only include options requested by us
+  2. It can only include a smaller block size
+  3. It can't change the proposed time out value.
+  4. Other requirements of the individal MTFTP options as required.
+
+  @param  Reply                 The options included in the OACK
+  @param  Request               The options we requested
+
+  @retval TRUE                  The options included in OACK is valid.
+  @retval FALSE                 The options included in OACK is invalid.
+
+**/
+BOOLEAN
+Mtftp4WrqOackValid (
+  IN MTFTP4_OPTION              *Reply,
+  IN MTFTP4_OPTION              *Request
+  )
+{
+  //
+  // It is invalid for server to return options we don't request
+  //
+  if ((Reply->Exist & ~Request->Exist) != 0) {
+    return FALSE;
+  }
+
+  //
+  // Server can only specify a smaller block size to be used and
+  // return the timeout matches that requested.
+  //
+  if ((((Reply->Exist & MTFTP4_BLKSIZE_EXIST) != 0) && (Reply->BlkSize > Request->BlkSize)) ||
+      (((Reply->Exist & MTFTP4_TIMEOUT_EXIST) != 0) && (Reply->Timeout != Request->Timeout))) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+  Function to handle the MTFTP OACK packet. 
+  
+  It parses the packet's options, and update the internal states of the session.
+
+  @param  Instance              The MTFTP session
+  @param  Packet                The received OACK packet
+  @param  Len                   The length of the packet
+  @param  Completed             Whether the transmisson has completed. NOT used by
+                                this function.
+
+  @retval EFI_SUCCESS           The OACK process is OK
+  @retval EFI_TFTP_ERROR        Some error occured, and the session reset.
+
+**/
+EFI_STATUS
+Mtftp4WrqHandleOack (
+  IN OUT MTFTP4_PROTOCOL       *Instance,
+  IN     EFI_MTFTP4_PACKET     *Packet,
+  IN     UINT32                Len,
+     OUT BOOLEAN               *Completed
+  )
+{
+  MTFTP4_OPTION             Reply;
+  EFI_MTFTP4_PACKET         Bogus;
+  EFI_STATUS                Status;
+  INTN                      Expected;
+
+  *Completed = FALSE;
+
+  //
+  // Ignore the OACK if already started the upload
+  //
+  Expected = Mtftp4GetNextBlockNum (&Instance->Blocks);
+
+  if (Expected != 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Parse and validate the options from server
+  //
+  ZeroMem (&Reply, sizeof (MTFTP4_OPTION));
+  Status = Mtftp4ParseOptionOack (Packet, Len, &Reply);
+
+  if (EFI_ERROR (Status) || !Mtftp4WrqOackValid (&Reply, &Instance->RequestOption)) {
+    //
+    // Don't send a MTFTP error packet when out of resource, it can
+    // only make it worse.
+    //
+    if (Status != EFI_OUT_OF_RESOURCES) {
+      Mtftp4SendError (
+        Instance,
+        EFI_MTFTP4_ERRORCODE_ILLEGAL_OPERATION,
+        (UINT8 *) "Mal-formated OACK packet"
+        );
+    }
+
+    return EFI_TFTP_ERROR;
+  }
+
+  if (Reply.BlkSize != 0) {
+    Instance->BlkSize = Reply.BlkSize;
+  }
+
+  if (Reply.Timeout != 0) {
+    Instance->Timeout = Reply.Timeout;
+  }
+
+  //
+  // Build a bogus ACK0 packet then pass it to the Mtftp4WrqHandleAck,
+  // which will start the transmission of the first data block.
+  //
+  Bogus.Ack.OpCode    = HTONS (EFI_MTFTP4_OPCODE_ACK);
+  Bogus.Ack.Block[0]  = 0;
+
+  Status = Mtftp4WrqHandleAck (
+             Instance,
+             &Bogus,
+             sizeof (EFI_MTFTP4_ACK_HEADER),
+             Completed
+             );
+
+  return Status;
+}
+
+
+/**
+  The input process routine for MTFTP upload.
+
+  @param  UdpPacket             The received MTFTP packet.
+  @param  EndPoint              The local/remote access point
+  @param  IoStatus              The result of the packet receiving
+  @param  Context               Opaque parameter for the callback, which is the
+                                MTFTP session.
+**/
+VOID
+EFIAPI
+Mtftp4WrqInput (
+  IN NET_BUF                *UdpPacket,
+  IN UDP_END_POINT          *EndPoint,
+  IN EFI_STATUS             IoStatus,
+  IN VOID                   *Context
+  )
+{
+  MTFTP4_PROTOCOL           *Instance;
+  EFI_MTFTP4_PACKET         *Packet;
+  BOOLEAN                   Completed;
+  EFI_STATUS                Status;
+  UINT32                    Len;
+  UINT16                    Opcode;
+
+  Instance = (MTFTP4_PROTOCOL *) Context;
+  NET_CHECK_SIGNATURE (Instance, MTFTP4_PROTOCOL_SIGNATURE);
+
+  Completed = FALSE;
+  Packet    = NULL;
+  Status    = EFI_SUCCESS;
+
+  if (EFI_ERROR (IoStatus)) {
+    Status = IoStatus;
+    goto ON_EXIT;
+  }
+
+  ASSERT (UdpPacket != NULL);
+
+  if (UdpPacket->TotalSize < MTFTP4_OPCODE_LEN) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Client send initial request to server's listening port. Server
+  // will select a UDP port to communicate with the client.
+  //
+  if (EndPoint->RemotePort != Instance->ConnectedPort) {
+    if (Instance->ConnectedPort != 0) {
+      goto ON_EXIT;
+    } else {
+      Instance->ConnectedPort = EndPoint->RemotePort;
+    }
+  }
+
+  //
+  // Copy the MTFTP packet to a continuous buffer if it isn't already so.
+  //
+  Len = UdpPacket->TotalSize;
+
+  if (UdpPacket->BlockOpNum > 1) {
+    Packet = AllocatePool (Len);
+
+    if (Packet == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto ON_EXIT;
+    }
+
+    NetbufCopy (UdpPacket, 0, Len, (UINT8 *) Packet);
+
+  } else {
+    Packet = (EFI_MTFTP4_PACKET *) NetbufGetByte (UdpPacket, 0, NULL);
+    ASSERT (Packet != NULL);
+  }
+
+  Opcode = NTOHS (Packet->OpCode);
+
+  //
+  // Call the user's CheckPacket if provided. Abort the transmission
+  // if CheckPacket returns an EFI_ERROR code.
+  //
+  if ((Instance->Token->CheckPacket != NULL) &&
+      ((Opcode == EFI_MTFTP4_OPCODE_OACK) || (Opcode == EFI_MTFTP4_OPCODE_ERROR))) {
+
+    Status = Instance->Token->CheckPacket (
+                                &Instance->Mtftp4,
+                                Instance->Token,
+                                (UINT16) Len,
+                                Packet
+                                );
+
+    if (EFI_ERROR (Status)) {
+      //
+      // Send an error message to the server to inform it
+      //
+      if (Opcode != EFI_MTFTP4_OPCODE_ERROR) {
+        Mtftp4SendError (
+          Instance,
+          EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
+          (UINT8 *) "User aborted the transfer"
+          );
+      }
+
+      Status = EFI_ABORTED;
+      goto ON_EXIT;
+    }
+  }
+
+  switch (Opcode) {
+  case EFI_MTFTP4_OPCODE_ACK:
+    if (Len != MTFTP4_OPCODE_LEN + MTFTP4_BLKNO_LEN) {
+      goto ON_EXIT;
+    }
+
+    Status = Mtftp4WrqHandleAck (Instance, Packet, Len, &Completed);
+    break;
+
+  case EFI_MTFTP4_OPCODE_OACK:
+    if (Len <= MTFTP4_OPCODE_LEN) {
+      goto ON_EXIT;
+    }
+
+    Status = Mtftp4WrqHandleOack (Instance, Packet, Len, &Completed);
+    break;
+
+  case EFI_MTFTP4_OPCODE_ERROR:
+    Status = EFI_TFTP_ERROR;
+    break;
+    
+  default:
+    break;
+  }
+
+ON_EXIT:
+  //
+  // Free the resources, then if !EFI_ERROR (Status) and not completed,
+  // restart the receive, otherwise end the session.
+  //
+  if ((Packet != NULL) && (UdpPacket->BlockOpNum > 1)) {
+    FreePool (Packet);
+  }
+
+  if (UdpPacket != NULL) {
+    NetbufFree (UdpPacket);
+  }
+
+  if (!EFI_ERROR (Status) && !Completed) {
+    Status = UdpIoRecvDatagram (Instance->UnicastPort, Mtftp4WrqInput, Instance, 0);
+  }
+
+  //
+  // Status may have been updated by UdpIoRecvDatagram
+  //
+  if (EFI_ERROR (Status) || Completed) {
+    Mtftp4CleanOperation (Instance, Status);
+  }
+}
+
+
+
+/**
+  Start the MTFTP session for upload.
+  
+  It will first init some states, then send the WRQ request packet, 
+  and start receiving the packet.
+
+  @param  Instance              The MTFTP session
+  @param  Operation             Redundant parameter, which is always
+                                EFI_MTFTP4_OPCODE_WRQ here.
+
+  @retval EFI_SUCCESS           The upload process has been started.
+  @retval Others                Failed to start the upload.
+
+**/
+EFI_STATUS
+Mtftp4WrqStart (
+  IN MTFTP4_PROTOCOL        *Instance,
+  IN UINT16                 Operation
+  )
+{
+  EFI_STATUS                Status;
+
+  //
+  // The valid block number range are [0, 0xffff]. For example:
+  // the client sends an WRQ request to the server, the server
+  // ACK with an ACK0 to let client start transfer the first
+  // packet.
+  //
+  Status = Mtftp4InitBlockRange (&Instance->Blocks, 0, 0xffff);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = Mtftp4SendRequest (Instance);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return UdpIoRecvDatagram (Instance->UnicastPort, Mtftp4WrqInput, Instance, 0);
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Callback.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Callback.c
new file mode 100644
index 0000000000..7f35642937
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Callback.c
@@ -0,0 +1,360 @@
+/** @file
+  This file contains the callback routines for undi3.1.
+  the callback routines for Undi3.1 have an extra parameter UniqueId which
+  stores the interface context for the NIC that snp is trying to talk.
+
+Copyright (c) 2006 - 2014, 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 "Snp.h"
+
+/**
+  Acquire or release a lock of the exclusive access to a critical section of the
+  code/data.
+
+  This is a callback routine supplied to UNDI3.1 at undi_start time.
+  New callbacks for 3.1: there won't be a virtual2physical callback for UNDI 3.1
+  because undi3.1 uses the MemMap call to map the required address by itself!
+
+  @param UniqueId  This was supplied to UNDI at Undi_Start, SNP uses this to
+                      store Undi interface context (Undi does not read or write
+                      this variable).
+  @param Enable    Non-zero indicates acquire; Zero indicates release.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackBlock (
+  IN UINT64 UniqueId,
+  IN UINT32 Enable
+  )
+{
+  SNP_DRIVER  *Snp;
+
+  Snp = (SNP_DRIVER *) (UINTN) UniqueId;
+  //
+  // tcpip was calling snp at tpl_notify and when we acquire a lock that was
+  // created at a lower level (TPL_CALLBACK) it gives an assert!
+  //
+  if (Enable != 0) {
+    EfiAcquireLock (&Snp->Lock);
+  } else {
+    EfiReleaseLock (&Snp->Lock);
+  }
+}
+
+/**
+  Delay MicroSeconds of micro seconds.
+
+  This is a callback routine supplied to UNDI at undi_start time.
+
+  @param UniqueId      This was supplied to UNDI at Undi_Start, SNP uses this to
+                       store Undi interface context (Undi does not read or write
+                       this variable).
+  @param MicroSeconds  Number of micro seconds to pause, ususlly multiple of 10.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackDelay (
+  IN UINT64 UniqueId,
+  IN UINT64 MicroSeconds
+  )
+{
+  if (MicroSeconds != 0) {
+    gBS->Stall ((UINTN) MicroSeconds);
+  }
+}
+
+/**
+  IO routine for UNDI3.1.
+
+  This is a callback routine supplied to UNDI at undi_start time.
+
+  @param UniqueId       This was supplied to UNDI at Undi_Start, SNP uses this
+                        to store Undi interface context (Undi does not read or
+                        write this variable).
+  @param ReadOrWrite    Indicates read or write, IO or Memory.
+  @param NumBytes       Number of bytes to read or write.
+  @param MemOrPortAddr  IO or memory address to read from or write to.
+  @param BufferPtr      Memory location to read into or that contains the bytes
+                        to write.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackMemio (
+  IN UINT64     UniqueId,
+  IN UINT8      ReadOrWrite,
+  IN UINT8      NumBytes,
+  IN UINT64     MemOrPortAddr,
+  IN OUT UINT64 BufferPtr
+  )
+{
+  SNP_DRIVER                *Snp;
+  EFI_PCI_IO_PROTOCOL_WIDTH Width;
+
+  Snp   = (SNP_DRIVER *) (UINTN) UniqueId;
+
+  Width = (EFI_PCI_IO_PROTOCOL_WIDTH) 0;
+  switch (NumBytes) {
+  case 2:
+    Width = (EFI_PCI_IO_PROTOCOL_WIDTH) 1;
+    break;
+
+  case 4:
+    Width = (EFI_PCI_IO_PROTOCOL_WIDTH) 2;
+    break;
+
+  case 8:
+    Width = (EFI_PCI_IO_PROTOCOL_WIDTH) 3;
+    break;
+  }
+
+  switch (ReadOrWrite) {
+  case PXE_IO_READ:
+    Snp->PciIo->Io.Read (
+                     Snp->PciIo,
+                     Width,
+                     Snp->IoBarIndex,      // BAR 1 (for 32bit regs), IO base address
+                     MemOrPortAddr,
+                     1,                    // count
+                     (VOID *) (UINTN) BufferPtr
+                     );
+    break;
+
+  case PXE_IO_WRITE:
+    Snp->PciIo->Io.Write (
+                     Snp->PciIo,
+                     Width,
+                     Snp->IoBarIndex,      // BAR 1 (for 32bit regs), IO base address
+                     MemOrPortAddr,
+                     1,                    // count
+                     (VOID *) (UINTN) BufferPtr
+                     );
+    break;
+
+  case PXE_MEM_READ:
+    Snp->PciIo->Mem.Read (
+                      Snp->PciIo,
+                      Width,
+                      Snp->MemoryBarIndex,  // BAR 0, Memory base address
+                      MemOrPortAddr,
+                      1,                    // count
+                      (VOID *) (UINTN) BufferPtr
+                      );
+    break;
+
+  case PXE_MEM_WRITE:
+    Snp->PciIo->Mem.Write (
+                      Snp->PciIo,
+                      Width,
+                      Snp->MemoryBarIndex,  // BAR 0, Memory base address
+                      MemOrPortAddr,
+                      1,                    // count
+                      (VOID *) (UINTN) BufferPtr
+                      );
+    break;
+  }
+
+  return ;
+}
+
+/**
+  Map a CPU address to a device address.
+
+  This is a callback routine supplied to UNDI at undi_start time.
+
+  @param UniqueId      This was supplied to UNDI at Undi_Start, SNP uses this to
+                       store Undi interface context (Undi does not read or write
+                       this variable).
+  @param CpuAddr       Virtual address to be mapped.
+  @param NumBytes      Size of memory to be mapped.
+  @param Direction     Direction of data flow for this memory's usage:
+                       cpu->device, device->cpu or both ways.
+  @param DeviceAddrPtr Pointer to return the mapped device address.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackMap (
+  IN UINT64     UniqueId,
+  IN UINT64     CpuAddr,
+  IN UINT32     NumBytes,
+  IN UINT32     Direction,
+  IN OUT UINT64 DeviceAddrPtr
+  )
+{
+  EFI_PHYSICAL_ADDRESS          *DevAddrPtr;
+  EFI_PCI_IO_PROTOCOL_OPERATION DirectionFlag;
+  UINTN                         BuffSize;
+  SNP_DRIVER                    *Snp;
+  UINTN                         Index;
+  EFI_STATUS                    Status;
+
+  BuffSize    = (UINTN) NumBytes;
+  Snp         = (SNP_DRIVER *) (UINTN) UniqueId;
+  DevAddrPtr  = (EFI_PHYSICAL_ADDRESS *) (UINTN) DeviceAddrPtr;
+
+  if (CpuAddr == 0) {
+    *DevAddrPtr = 0;
+    return ;
+  }
+
+  switch (Direction) {
+  case TO_AND_FROM_DEVICE:
+    DirectionFlag = EfiPciIoOperationBusMasterCommonBuffer;
+    break;
+
+  case FROM_DEVICE:
+    DirectionFlag = EfiPciIoOperationBusMasterWrite;
+    break;
+
+  case TO_DEVICE:
+    DirectionFlag = EfiPciIoOperationBusMasterRead;
+    break;
+
+  default:
+    *DevAddrPtr = 0;
+    //
+    // any non zero indicates error!
+    //
+    return ;
+  }
+  //
+  // find an unused map_list entry
+  //
+  for (Index = 0; Index < MAX_MAP_LENGTH; Index++) {
+    if (Snp->MapList[Index].VirtualAddress == 0) {
+      break;
+    }
+  }
+
+  if (Index >= MAX_MAP_LENGTH) {
+    DEBUG ((EFI_D_INFO, "SNP maplist is FULL\n"));
+    *DevAddrPtr = 0;
+    return ;
+  }
+
+  Snp->MapList[Index].VirtualAddress = (EFI_PHYSICAL_ADDRESS) CpuAddr;
+
+  Status = Snp->PciIo->Map (
+                         Snp->PciIo,
+                         DirectionFlag,
+                         (VOID *) (UINTN) CpuAddr,
+                         &BuffSize,
+                         DevAddrPtr,
+                         &(Snp->MapList[Index].MapCookie)
+                         );
+  if (Status != EFI_SUCCESS) {
+    *DevAddrPtr                        = 0;
+    Snp->MapList[Index].VirtualAddress = 0;
+  }
+
+  return ;
+}
+
+/**
+  Unmap an address that was previously mapped using map callback.
+
+  This is a callback routine supplied to UNDI at undi_start time.
+
+  @param UniqueId    This was supplied to UNDI at Undi_Start, SNP uses this to
+                     store. Undi interface context (Undi does not read or write
+                     this variable).
+  @param CpuAddr     Virtual address that was mapped.
+  @param NumBytes    Size of memory mapped.
+  @param Direction   Direction of data flow for this memory's usage:
+                     cpu->device, device->cpu or both ways.
+  @param DeviceAddr  The mapped device address.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackUnmap (
+  IN UINT64 UniqueId,
+  IN UINT64 CpuAddr,
+  IN UINT32 NumBytes,
+  IN UINT32 Direction,
+  IN UINT64 DeviceAddr
+  )
+{
+  SNP_DRIVER  *Snp;
+  UINT16      Index;
+
+  Snp = (SNP_DRIVER *) (UINTN) UniqueId;
+
+  for (Index = 0; Index < MAX_MAP_LENGTH; Index++) {
+    if (Snp->MapList[Index].VirtualAddress == CpuAddr) {
+      break;
+    }
+  }
+
+  if (Index >= MAX_MAP_LENGTH) {
+    DEBUG ((EFI_D_ERROR, "SNP could not find a mapping, failed to unmap.\n"));
+    return ;
+  }
+
+  Snp->PciIo->Unmap (Snp->PciIo, Snp->MapList[Index].MapCookie);
+  Snp->MapList[Index].VirtualAddress = 0;
+  Snp->MapList[Index].MapCookie      = NULL;
+  return ;
+}
+
+/**
+  Synchronize the virtual buffer contents with the mapped buffer contents.
+
+  This is a callback routine supplied to UNDI at undi_start time. The virtual
+  and mapped buffers need not correspond to the same physical memory (especially
+  if the virtual address is > 4GB). Depending on the direction for which the
+  buffer is mapped, undi will need to synchronize their contents whenever it
+  writes to/reads from the buffer using either the cpu address or the device
+  address.
+  EFI does not provide a sync call since virt=physical, we should just do the
+  synchronization ourselves here.
+
+  @param UniqueId    This was supplied to UNDI at Undi_Start, SNP uses this to
+                     store Undi interface context (Undi does not read or write
+                     this variable).
+  @param CpuAddr     Virtual address that was mapped.
+  @param NumBytes    Size of memory mapped.
+  @param Direction   Direction of data flow for this memory's usage:
+                     cpu->device, device->cpu or both ways.
+  @param DeviceAddr  The mapped device address.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackSync (
+  IN UINT64             UniqueId,
+  IN UINT64             CpuAddr,
+  IN UINT32             NumBytes,
+  IN UINT32             Direction,
+  IN UINT64             DeviceAddr
+  )
+{
+  if ((CpuAddr == 0) || (DeviceAddr == 0) || (NumBytes == 0)) {
+    return ;
+
+  }
+
+  switch (Direction) {
+  case FROM_DEVICE:
+    CopyMem ((UINT8 *) (UINTN) CpuAddr, (UINT8 *) (UINTN) DeviceAddr, NumBytes);
+    break;
+
+  case TO_DEVICE:
+    CopyMem ((UINT8 *) (UINTN) DeviceAddr, (UINT8 *) (UINTN) CpuAddr, NumBytes);
+    break;
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/ComponentName.c
new file mode 100644
index 0000000000..d8b8184018
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/ComponentName.c
@@ -0,0 +1,436 @@
+/** @file
+    UEFI Component Name(2) protocol implementation for SnpDxe driver.
+
+Copyright (c) 2004 - 2014, 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 "Snp.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gSimpleNetworkComponentName = {
+  SimpleNetworkComponentNameGetDriverName,
+  SimpleNetworkComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gSimpleNetworkComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) SimpleNetworkComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) SimpleNetworkComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mSimpleNetworkDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Simple Network Protocol Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE *gSimpleNetworkControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mSimpleNetworkDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gSimpleNetworkComponentName)
+           );
+}
+
+/**
+  Update the component name for the Snp child handle.
+
+  @param  Snp[in]                   A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS               Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER     The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN  EFI_SIMPLE_NETWORK_PROTOCOL   *Snp
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[80];
+  UINTN                            OffSet;
+  UINTN                            Index;
+
+  if (Snp == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OffSet = 0;
+  OffSet += UnicodeSPrint (
+              HandleName,
+              sizeof (HandleName),
+              L"SNP (MAC="
+              );
+  for (Index = 0; Index < Snp->Mode->HwAddressSize; Index++) {
+    OffSet += UnicodeSPrint (
+                HandleName + OffSet,
+                sizeof (HandleName) - OffSet * sizeof (CHAR16),
+                L"%02X-",
+                Snp->Mode->CurrentAddress.Addr[Index]
+                );
+  }
+  ASSERT (OffSet > 0);
+  //
+  // Remove the last '-'
+  //
+  OffSet--;
+  OffSet += UnicodeSPrint (
+              HandleName + OffSet,
+              sizeof (HandleName) - OffSet * sizeof (CHAR16),
+              L")"
+              );
+  if (gSimpleNetworkControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gSimpleNetworkControllerNameTable);
+    gSimpleNetworkControllerNameTable = NULL;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gSimpleNetworkComponentName.SupportedLanguages,
+             &gSimpleNetworkControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gSimpleNetworkComponentName2.SupportedLanguages,
+           &gSimpleNetworkControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned. 
+  Currently not implemented. 
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL   *Snp;
+  
+  if (ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Make sure this driver is currently managing ControllHandle
+  //
+  Status = EfiTestManagedDevice (
+             ControllerHandle,
+             gSimpleNetworkDriverBinding.DriverBindingHandle,
+             &gEfiSimpleNetworkProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ControllerHandle
+  // 
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  (VOID **)&Snp,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Snp);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gSimpleNetworkControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gSimpleNetworkComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Get_status.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Get_status.c
new file mode 100644
index 0000000000..8f2ab9b24a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Get_status.c
@@ -0,0 +1,266 @@
+/** @file
+  Implementation of reading the current interrupt status and recycled transmit
+  buffer status from a network interface.
+
+Copyright (c) 2004 - 2016, 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 "Snp.h"
+
+/**
+  Call undi to get the status of the interrupts, get the list of recycled transmit
+  buffers that completed transmitting. The recycled transmit buffer address will
+  be saved into Snp->RecycledTxBuf.
+
+  @param  Snp                     Pointer to snp driver structure.
+  @param  InterruptStatusPtr      A non null pointer to contain the interrupt
+                                  status.
+  @param  GetTransmittedBuf       Set to TRUE to retrieve the recycled transmit
+                                  buffer address.
+
+  @retval EFI_SUCCESS         The status of the network interface was retrieved.
+  @retval EFI_DEVICE_ERROR    The command could not be sent to the network
+                              interface.
+
+**/
+EFI_STATUS
+PxeGetStatus (
+  SNP_DRIVER *Snp,
+  UINT32     *InterruptStatusPtr,
+  BOOLEAN    GetTransmittedBuf
+  )
+{
+  PXE_DB_GET_STATUS *Db;
+  UINT16            InterruptFlags;
+  UINT32            Index;
+  UINT64            *Tmp;
+
+  Tmp               = NULL;
+  Db                = Snp->Db;
+  Snp->Cdb.OpCode   = PXE_OPCODE_GET_STATUS;
+
+  Snp->Cdb.OpFlags  = 0;
+
+  if (GetTransmittedBuf) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_GET_TRANSMITTED_BUFFERS;
+    ZeroMem (Db->TxBuffer, sizeof (Db->TxBuffer));
+  }
+
+  if (InterruptStatusPtr != NULL) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_GET_INTERRUPT_STATUS;
+  }
+
+  if (Snp->MediaStatusSupported) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_GET_MEDIA_STATUS;
+  }
+
+  Snp->Cdb.CPBsize  = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr  = PXE_CPBADDR_NOT_USED;
+
+  //
+  // size DB for return of one buffer
+  //
+  Snp->Cdb.DBsize     = (UINT16) ((sizeof (PXE_DB_GET_STATUS) - sizeof (Db->TxBuffer)) + sizeof (Db->TxBuffer[0]));
+
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nSnp->undi.get_status()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != EFI_SUCCESS) {
+    DEBUG (
+      (EFI_D_NET,
+      "\nSnp->undi.get_status()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatFlags)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // report the values back..
+  //
+  if (InterruptStatusPtr != NULL) {
+    InterruptFlags      = (UINT16) (Snp->Cdb.StatFlags & PXE_STATFLAGS_GET_STATUS_INTERRUPT_MASK);
+
+    *InterruptStatusPtr = 0;
+
+    if ((InterruptFlags & PXE_STATFLAGS_GET_STATUS_RECEIVE) == PXE_STATFLAGS_GET_STATUS_RECEIVE) {
+      *InterruptStatusPtr |= EFI_SIMPLE_NETWORK_RECEIVE_INTERRUPT;
+    }
+
+    if ((InterruptFlags & PXE_STATFLAGS_GET_STATUS_TRANSMIT) == PXE_STATFLAGS_GET_STATUS_TRANSMIT) {
+      *InterruptStatusPtr |= EFI_SIMPLE_NETWORK_TRANSMIT_INTERRUPT;
+    }
+
+    if ((InterruptFlags & PXE_STATFLAGS_GET_STATUS_COMMAND) == PXE_STATFLAGS_GET_STATUS_COMMAND) {
+      *InterruptStatusPtr |= EFI_SIMPLE_NETWORK_COMMAND_INTERRUPT;
+    }
+
+    if ((InterruptFlags & PXE_STATFLAGS_GET_STATUS_SOFTWARE) == PXE_STATFLAGS_GET_STATUS_SOFTWARE) {
+      *InterruptStatusPtr |= EFI_SIMPLE_NETWORK_COMMAND_INTERRUPT;
+    }
+
+  }
+
+  if (GetTransmittedBuf) {
+    if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_GET_STATUS_NO_TXBUFS_WRITTEN) == 0) {
+      //
+      // UNDI has written some transmitted buffer addresses into the DB. Store them into Snp->RecycledTxBuf.
+      //
+      for (Index = 0; Index < MAX_XMIT_BUFFERS; Index++) {
+        if (Db->TxBuffer[Index] != 0) {
+          if (Snp->RecycledTxBufCount == Snp->MaxRecycledTxBuf) {
+            //
+            // Snp->RecycledTxBuf is full, reallocate a new one.
+            //
+            if ((Snp->MaxRecycledTxBuf + SNP_TX_BUFFER_INCREASEMENT) >= SNP_MAX_TX_BUFFER_NUM) {
+              return EFI_DEVICE_ERROR;
+            }
+            Tmp = AllocatePool (sizeof (UINT64) * (Snp->MaxRecycledTxBuf + SNP_TX_BUFFER_INCREASEMENT));
+            if (Tmp == NULL) {
+              return EFI_DEVICE_ERROR;
+            }
+            CopyMem (Tmp, Snp->RecycledTxBuf, sizeof (UINT64) * Snp->RecycledTxBufCount);
+            FreePool (Snp->RecycledTxBuf);
+            Snp->RecycledTxBuf    =  Tmp;
+            Snp->MaxRecycledTxBuf += SNP_TX_BUFFER_INCREASEMENT;
+          }
+          Snp->RecycledTxBuf[Snp->RecycledTxBufCount] = Db->TxBuffer[Index];
+          Snp->RecycledTxBufCount++;
+        }
+      }
+    }
+  }
+
+  //
+  // Update MediaPresent field of EFI_SIMPLE_NETWORK_MODE if the UNDI support
+  // returning media status from GET_STATUS command
+  //
+  if (Snp->MediaStatusSupported) {
+    Snp->Snp.Mode->MediaPresent =
+      (BOOLEAN) (((Snp->Cdb.StatFlags & PXE_STATFLAGS_GET_STATUS_NO_MEDIA) != 0) ? FALSE : TRUE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads the current interrupt status and recycled transmit buffer status from a
+  network interface.
+
+  This function gets the current interrupt and recycled transmit buffer status
+  from the network interface. The interrupt status is returned as a bit mask in
+  InterruptStatus. If InterruptStatus is NULL, the interrupt status will not be
+  read. If TxBuf is not NULL, a recycled transmit buffer address will be retrieved.
+  If a recycled transmit buffer address is returned in TxBuf, then the buffer has
+  been successfully transmitted, and the status for that buffer is cleared. If
+  the status of the network interface is successfully collected, EFI_SUCCESS
+  will be returned. If the driver has not been initialized, EFI_DEVICE_ERROR will
+  be returned.
+
+  @param This            A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param InterruptStatus A pointer to the bit mask of the currently active
+                         interrupts (see "Related Definitions"). If this is NULL,
+                         the interrupt status will not be read from the device.
+                         If this is not NULL, the interrupt status will be read
+                         from the device. When the interrupt status is read, it
+                         will also be cleared. Clearing the transmit interrupt does
+                         not empty the recycled transmit buffer array.
+  @param TxBuf           Recycled transmit buffer address. The network interface
+                         will not transmit if its internal recycled transmit
+                         buffer array is full. Reading the transmit buffer does
+                         not clear the transmit interrupt. If this is NULL, then
+                         the transmit buffer status will not be read. If there
+                         are no transmit buffers to recycle and TxBuf is not NULL,
+                         TxBuf will be set to NULL.
+
+  @retval EFI_SUCCESS           The status of the network interface was retrieved.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network
+                                interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32GetStatus (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  OUT UINT32                     *InterruptStatus, OPTIONAL
+  OUT VOID                       **TxBuf           OPTIONAL
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (InterruptStatus == NULL && TxBuf == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Snp == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  if (Snp->RecycledTxBufCount == 0 && TxBuf != NULL) {
+    Status = PxeGetStatus (Snp, InterruptStatus, TRUE);
+  } else {
+    Status = PxeGetStatus (Snp, InterruptStatus, FALSE);
+  }
+
+  if (TxBuf != NULL) {
+    //
+    // Get a recycled buf from Snp->RecycledTxBuf
+    //
+    if (Snp->RecycledTxBufCount == 0) {
+      *TxBuf = NULL;
+    } else {
+      Snp->RecycledTxBufCount--;
+      *TxBuf = (VOID *) (UINTN) Snp->RecycledTxBuf[Snp->RecycledTxBufCount];
+    }
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Initialize.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Initialize.c
new file mode 100644
index 0000000000..3f40ef3ce7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Initialize.c
@@ -0,0 +1,251 @@
+/** @file
+ 		Implementation of initializing a network adapter.
+
+Copyright (c) 2004 - 2008, 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 "Snp.h"
+
+/**
+  Call UNDI to initialize the interface.
+
+  @param  Snp                   Pointer to snp driver structure.
+  @param  CableDetectFlag       Do/don't detect the cable (depending on what 
+                                undi supports).
+  
+  @retval EFI_SUCCESS           UNDI is initialized successfully.
+  @retval EFI_DEVICE_ERROR      UNDI could not be initialized.
+  @retval Other                 Other errors as indicated.
+
+**/
+EFI_STATUS
+PxeInit (
+  SNP_DRIVER *Snp,
+  UINT16     CableDetectFlag
+  )
+{
+  PXE_CPB_INITIALIZE  *Cpb;
+  VOID                *Addr;
+  EFI_STATUS          Status;
+
+  Cpb = Snp->Cpb;
+  if (Snp->TxRxBufferSize != 0) {
+    Status = Snp->PciIo->AllocateBuffer (
+                           Snp->PciIo,
+                           AllocateAnyPages,
+                           EfiBootServicesData,
+                           SNP_MEM_PAGES (Snp->TxRxBufferSize),
+                           &Addr,
+                           0
+                           );
+
+    if (Status != EFI_SUCCESS) {
+      DEBUG (
+        (EFI_D_ERROR,
+        "\nSnp->PxeInit()  AllocateBuffer  %xh (%r)\n",
+        Status,
+        Status)
+        );
+
+      return Status;
+    }
+
+    ASSERT (Addr);
+
+    Snp->TxRxBuffer = Addr;
+  }
+
+  Cpb->MemoryAddr   = (UINT64)(UINTN) Snp->TxRxBuffer;
+
+  Cpb->MemoryLength = Snp->TxRxBufferSize;
+
+  //
+  // let UNDI decide/detect these values
+  //
+  Cpb->LinkSpeed      = 0;
+  Cpb->TxBufCnt       = 0;
+  Cpb->TxBufSize      = 0;
+  Cpb->RxBufCnt       = 0;
+  Cpb->RxBufSize      = 0;
+
+  Cpb->DuplexMode         = PXE_DUPLEX_DEFAULT;
+
+  Cpb->LoopBackMode       = LOOPBACK_NORMAL;
+
+  Snp->Cdb.OpCode     = PXE_OPCODE_INITIALIZE;
+  Snp->Cdb.OpFlags    = CableDetectFlag;
+
+  Snp->Cdb.CPBsize    = (UINT16) sizeof (PXE_CPB_INITIALIZE);
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_INITIALIZE);
+
+  Snp->Cdb.CPBaddr    = (UINT64)(UINTN) Snp->Cpb;
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Snp->Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  DEBUG ((EFI_D_NET, "\nSnp->undi.initialize()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode == PXE_STATCODE_SUCCESS) {
+    Snp->Mode.State = EfiSimpleNetworkInitialized;
+
+    Status          = EFI_SUCCESS;
+  } else {
+    DEBUG (
+      (EFI_D_WARN,
+      "\nSnp->undi.initialize()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    if (Snp->TxRxBuffer != NULL) {
+      Snp->PciIo->FreeBuffer (
+                    Snp->PciIo,
+                    SNP_MEM_PAGES (Snp->TxRxBufferSize),
+                    (VOID *) Snp->TxRxBuffer
+                    );
+    }
+
+    Snp->TxRxBuffer = NULL;
+
+    Status          = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+
+/**
+  Resets a network adapter and allocates the transmit and receive buffers 
+  required by the network interface; optionally, also requests allocation of 
+  additional transmit and receive buffers.
+
+  This function allocates the transmit and receive buffers required by the network
+  interface. If this allocation fails, then EFI_OUT_OF_RESOURCES is returned.
+  If the allocation succeeds and the network interface is successfully initialized,
+  then EFI_SUCCESS will be returned.
+
+  @param This               A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @param ExtraRxBufferSize  The size, in bytes, of the extra receive buffer space
+                            that the driver should allocate for the network interface.
+                            Some network interfaces will not be able to use the 
+                            extra buffer, and the caller will not know if it is 
+                            actually being used.
+  @param ExtraTxBufferSize  The size, in bytes, of the extra transmit buffer space
+                            that the driver should allocate for the network interface.
+                            Some network interfaces will not be able to use the
+                            extra buffer, and the caller will not know if it is
+                            actually being used.
+
+  @retval EFI_SUCCESS           The network interface was initialized.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_OUT_OF_RESOURCES  There was not enough memory for the transmit and
+                                receive buffers.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       The increased buffer size feature is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Initialize (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINTN                       ExtraRxBufferSize OPTIONAL,
+  IN UINTN                       ExtraTxBufferSize OPTIONAL
+  )
+{
+  EFI_STATUS  EfiStatus;
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Snp == NULL) {
+    EfiStatus = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkStarted:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    EfiStatus = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    EfiStatus = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  EfiStatus = gBS->CreateEvent (
+                    EVT_NOTIFY_WAIT,
+                    TPL_NOTIFY,
+                    &SnpWaitForPacketNotify,
+                    Snp,
+                    &Snp->Snp.WaitForPacket
+                    );
+
+  if (EFI_ERROR (EfiStatus)) {
+    Snp->Snp.WaitForPacket = NULL;
+    EfiStatus = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+  //
+  //
+  //
+  Snp->Mode.MCastFilterCount      = 0;
+  Snp->Mode.ReceiveFilterSetting  = 0;
+  ZeroMem (Snp->Mode.MCastFilter, sizeof Snp->Mode.MCastFilter);
+  CopyMem (
+    &Snp->Mode.CurrentAddress,
+    &Snp->Mode.PermanentAddress,
+    sizeof (EFI_MAC_ADDRESS)
+    );
+
+  //
+  // Compute tx/rx buffer sizes based on UNDI init info and parameters.
+  //
+  Snp->TxRxBufferSize = (UINT32) (Snp->InitInfo.MemoryRequired + ExtraRxBufferSize + ExtraTxBufferSize);
+
+  if (Snp->Mode.MediaPresentSupported) {
+    if (PxeInit (Snp, PXE_OPFLAGS_INITIALIZE_DETECT_CABLE) == EFI_SUCCESS) {
+      Snp->Mode.MediaPresent = TRUE;
+      goto ON_EXIT;
+    }
+  }
+
+  Snp->Mode.MediaPresent  = FALSE;
+
+  EfiStatus               = PxeInit (Snp, PXE_OPFLAGS_INITIALIZE_DO_NOT_DETECT_CABLE);
+
+  if (EFI_ERROR (EfiStatus)) {
+    gBS->CloseEvent (Snp->Snp.WaitForPacket);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return EfiStatus;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Mcast_ip_to_mac.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Mcast_ip_to_mac.c
new file mode 100644
index 0000000000..23dba90194
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Mcast_ip_to_mac.c
@@ -0,0 +1,179 @@
+/** @file
+ 		Implementation of converting an multicast IP address to multicast HW MAC
+ 		address.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+/**
+  Call undi to convert an multicast IP address to a MAC address.
+
+  @param  Snp   Pointer to snp driver structure.
+  @param  IPv6  Flag to indicate if this is an ipv6 address.
+  @param  IP    Multicast IP address.
+  @param  MAC   Pointer to hold the return MAC address.
+
+  @retval EFI_SUCCESS           The multicast IP address was mapped to the 
+                                multicast HW MAC address.
+  @retval EFI_INVALID_PARAMETER Invalid UNDI command. 
+  @retval EFI_UNSUPPORTED       Command is not supported by UNDI. 
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command. 
+
+**/
+EFI_STATUS
+PxeIp2Mac (
+  IN SNP_DRIVER          *Snp,
+  IN BOOLEAN             IPv6,
+  IN EFI_IP_ADDRESS      *IP,
+  IN OUT EFI_MAC_ADDRESS *MAC
+  )
+{
+  PXE_CPB_MCAST_IP_TO_MAC *Cpb;
+  PXE_DB_MCAST_IP_TO_MAC  *Db;
+
+  Cpb                 = Snp->Cpb;
+  Db                  = Snp->Db;
+  Snp->Cdb.OpCode     = PXE_OPCODE_MCAST_IP_TO_MAC;
+  Snp->Cdb.OpFlags    = (UINT16) (IPv6 ? PXE_OPFLAGS_MCAST_IPV6_TO_MAC : PXE_OPFLAGS_MCAST_IPV4_TO_MAC);
+  Snp->Cdb.CPBsize    = (UINT16) sizeof (PXE_CPB_MCAST_IP_TO_MAC);
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_MCAST_IP_TO_MAC);
+
+  Snp->Cdb.CPBaddr    = (UINT64)(UINTN) Cpb;
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  CopyMem (&Cpb->IP, IP, sizeof (PXE_IP_ADDR));
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nSnp->undi.mcast_ip_to_mac()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    break;
+
+  case PXE_STATCODE_INVALID_CPB:
+    return EFI_INVALID_PARAMETER;
+
+  case PXE_STATCODE_UNSUPPORTED:
+    DEBUG (
+      (EFI_D_NET,
+      "\nSnp->undi.mcast_ip_to_mac()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+    return EFI_UNSUPPORTED;
+
+  default:
+    //
+    // UNDI command failed.  Return EFI_DEVICE_ERROR
+    // to caller.
+    //
+    DEBUG (
+      (EFI_D_NET,
+      "\nSnp->undi.mcast_ip_to_mac()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  CopyMem (MAC, &Db->MAC, sizeof (PXE_MAC_ADDR));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Converts a multicast IP address to a multicast HW MAC address.
+  
+  This function converts a multicast IP address to a multicast HW MAC address 
+  for all packet transactions. If the mapping is accepted, then EFI_SUCCESS will
+  be returned.
+
+  @param This A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param IPv6 Set to TRUE if the multicast IP address is IPv6 [RFC 2460].
+              Set to FALSE if the multicast IP address is IPv4 [RFC 791]. 
+  @param IP   The multicast IP address that is to be converted to a multicast 
+              HW MAC address.
+  @param MAC  The multicast HW MAC address that is to be generated from IP.
+
+  @retval EFI_SUCCESS           The multicast IP address was mapped to the
+                                multicast HW MAC address.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not 
+                                been started by calling Start().
+  @retval EFI_INVALID_PARAMETER IP is NULL.
+  @retval EFI_INVALID_PARAMETER MAC is NULL.
+  @retval EFI_INVALID_PARAMETER IP does not point to a valid IPv4 or IPv6 
+                                multicast address.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not 
+                                been initialized by calling Initialize().
+  @retval EFI_UNSUPPORTED       IPv6 is TRUE and the implementation does not 
+                                support IPv6 multicast to MAC address conversion.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32McastIpToMac (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     IPv6,
+  IN EFI_IP_ADDRESS              *IP,
+  OUT EFI_MAC_ADDRESS            *MAC
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  //
+  // Get pointer to SNP driver instance for *this.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IP == NULL || MAC == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = PxeIp2Mac (Snp, IPv6, IP, MAC);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Nvdata.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Nvdata.c
new file mode 100644
index 0000000000..443979087e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Nvdata.c
@@ -0,0 +1,223 @@
+/** @file
+ 	Implementation of reading and writing operations on the NVRAM device
+ 	attached to a network interface.
+ 
+Copyright (c) 2004 - 2009, 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 "Snp.h"
+
+
+/**
+  This routine calls Undi to read the desired number of eeprom bytes.
+
+  @param  Snp          pointer to the snp driver structure
+  @param  Offset       eeprom register value relative to the base address
+  @param  BufferSize   number of bytes to read
+  @param  Buffer       pointer where to read into
+
+  @retval EFI_SUCCESS           The NVRAM access was performed.
+  @retval EFI_INVALID_PARAMETER Invalid UNDI command.
+  @retval EFI_UNSUPPORTED       Command is not supported by UNDI.
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command.
+
+**/
+EFI_STATUS
+PxeNvDataRead (
+  IN SNP_DRIVER *Snp,
+  IN UINTN      Offset,
+  IN UINTN      BufferSize,
+  IN OUT VOID   *Buffer
+  )
+{
+  PXE_DB_NVDATA *Db;
+
+  Db                  = Snp->Db;
+  Snp->Cdb.OpCode     = PXE_OPCODE_NVDATA;
+
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NVDATA_READ;
+
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_NVDATA);
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.nvdata ()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    break;
+
+  case PXE_STATCODE_UNSUPPORTED:
+    DEBUG (
+      (EFI_D_NET,
+      "\nsnp->undi.nvdata()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_UNSUPPORTED;
+
+  default:
+    DEBUG (
+      (EFI_D_NET,
+      "\nsnp->undi.nvdata()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  ASSERT (Offset < sizeof (Db->Data));
+
+  CopyMem (Buffer, &Db->Data.Byte[Offset], BufferSize);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Performs read and write operations on the NVRAM device attached to a network 
+  interface.
+  
+  This function performs read and write operations on the NVRAM device attached 
+  to a network interface. If ReadWrite is TRUE, a read operation is performed.
+  If ReadWrite is FALSE, a write operation is performed. Offset specifies the 
+  byte offset at which to start either operation. Offset must be a multiple of 
+  NvRamAccessSize , and it must have a value between zero and NvRamSize. 
+  BufferSize specifies the length of the read or write operation. BufferSize must
+  also be a multiple of NvRamAccessSize, and Offset + BufferSize must not exceed
+  NvRamSize. 
+  If any of the above conditions is not met, then EFI_INVALID_PARAMETER will be 
+  returned. 
+  If all the conditions are met and the operation is "read," the NVRAM device 
+  attached to the network interface will be read into Buffer and EFI_SUCCESS 
+  will be returned. If this is a write operation, the contents of Buffer will be
+  used to update the contents of the NVRAM device attached to the network 
+  interface and EFI_SUCCESS will be returned.
+  
+  It does the basic checking on the input parameters and retrieves snp structure
+  and then calls the read_nvdata() call which does the actual reading
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param ReadWrite  TRUE for read operations, FALSE for write operations.
+  @param Offset     Byte offset in the NVRAM device at which to start the read or 
+                    write operation. This must be a multiple of NvRamAccessSize 
+                    and less than NvRamSize. (See EFI_SIMPLE_NETWORK_MODE)  
+  @param BufferSize The number of bytes to read or write from the NVRAM device. 
+                    This must also be a multiple of NvramAccessSize.
+  @param Buffer     A pointer to the data buffer.
+
+  @retval EFI_SUCCESS           The NVRAM access was performed.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * The This parameter is NULL 
+                                * The This parameter does not point to a valid 
+                                  EFI_SIMPLE_NETWORK_PROTOCOL  structure
+                                * The Offset parameter is not a multiple of 
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamAccessSize
+                                * The Offset parameter is not less than 
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamSize
+                                * The BufferSize parameter is not a multiple of 
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamAccessSize
+                                * The Buffer parameter is NULL
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network 
+                                interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network
+                                interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32NvData (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     ReadWrite,
+  IN UINTN                       Offset,
+  IN UINTN                       BufferSize,
+  IN OUT VOID                    *Buffer
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  //
+  // Get pointer to SNP driver instance for *this.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Return error if the SNP is not initialized.
+  //
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+  //
+  // Return error if non-volatile memory variables are not valid.
+  //
+  if (Snp->Mode.NvRamSize == 0 || Snp->Mode.NvRamAccessSize == 0) {
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+  }
+  //
+  // Check for invalid parameter combinations.
+  //
+  if ((BufferSize == 0) ||
+      (Buffer == NULL) ||
+      (Offset >= Snp->Mode.NvRamSize) ||
+      (Offset + BufferSize > Snp->Mode.NvRamSize) ||
+      (BufferSize % Snp->Mode.NvRamAccessSize != 0) ||
+      (Offset % Snp->Mode.NvRamAccessSize != 0)
+      ) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  //
+  // check the implementation flags of undi if we can write the nvdata!
+  //
+  if (!ReadWrite) {
+    Status = EFI_UNSUPPORTED;
+  } else {
+    Status = PxeNvDataRead (Snp, Offset, BufferSize, Buffer);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive.c
new file mode 100644
index 0000000000..ace5630066
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive.c
@@ -0,0 +1,257 @@
+/** @file
+  Implementation of receiving a packet from a network interface.
+
+Copyright (c) 2004 - 2010, 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 "Snp.h"
+
+/**
+  Call UNDI to receive a packet and fills in the data in the input pointers.
+
+  @param  Snp          Pointer to snp driver structure
+  @param  Buffer       Pointer to the memory for the received data
+  @param  BufferSize   Pointer to the length of the buffer on entry and contains
+                       the length of the received data on return
+  @param  HeaderSize   Pointer to the header portion of the data received.
+  @param  SrcAddr      Pointer to contain the source ethernet address on return
+  @param  DestAddr     Pointer to contain the destination ethernet address on
+                       return
+  @param  Protocol     Pointer to contain the protocol type from the ethernet
+                       header on return
+
+
+  @retval EFI_SUCCESS           The received data was stored in Buffer, and
+                                BufferSize has been updated to the number of
+                                bytes received.
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command.
+  @retval EFI_NOT_READY         No packets have been received on the network
+                                interface.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the received
+                                packets. BufferSize has been updated to the
+                                required size.
+
+**/
+EFI_STATUS
+PxeReceive (
+  SNP_DRIVER      *Snp,
+  VOID            *Buffer,
+  UINTN           *BufferSize,
+  UINTN           *HeaderSize,
+  EFI_MAC_ADDRESS *SrcAddr,
+  EFI_MAC_ADDRESS *DestAddr,
+  UINT16          *Protocol
+  )
+{
+  PXE_CPB_RECEIVE *Cpb;
+  PXE_DB_RECEIVE  *Db;
+  UINTN           BuffSize;
+
+  Cpb       = Snp->Cpb;
+  Db        = Snp->Db;
+  BuffSize  = *BufferSize;
+
+  Cpb->BufferAddr = (UINT64)(UINTN) Buffer;
+  Cpb->BufferLen  = (UINT32) *BufferSize;
+
+  Cpb->reserved       = 0;
+
+  Snp->Cdb.OpCode     = PXE_OPCODE_RECEIVE;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+
+  Snp->Cdb.CPBsize    = (UINT16) sizeof (PXE_CPB_RECEIVE);
+  Snp->Cdb.CPBaddr    = (UINT64)(UINTN) Cpb;
+
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_RECEIVE);
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.receive ()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    break;
+
+  case PXE_STATCODE_NO_DATA:
+    DEBUG (
+      (EFI_D_NET,
+      "\nsnp->undi.receive ()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_NOT_READY;
+
+  default:
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.receive()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  *BufferSize = Db->FrameLen;
+
+  if (HeaderSize != NULL) {
+    *HeaderSize = Db->MediaHeaderLen;
+  }
+
+  if (SrcAddr != NULL) {
+    CopyMem (SrcAddr, &Db->SrcAddr, Snp->Mode.HwAddressSize);
+  }
+
+  if (DestAddr != NULL) {
+    CopyMem (DestAddr, &Db->DestAddr, Snp->Mode.HwAddressSize);
+  }
+
+  if (Protocol != NULL) {
+    //
+    // We need to do the byte swapping
+    //
+    *Protocol = (UINT16) PXE_SWAP_UINT16 (Db->Protocol);
+  }
+
+  //
+  // We have received a packet from network interface, which implies that the
+  // network cable should be present. While, some UNDI driver may not report
+  // correct media status during Snp->Initialize(). So, we need ensure
+  // MediaPresent in SNP mode data is set to correct value.
+  //
+  if (Snp->Mode.MediaPresentSupported && !Snp->Mode.MediaPresent) {
+    Snp->Mode.MediaPresent = TRUE;
+  }
+
+  return (*BufferSize <= BuffSize) ? EFI_SUCCESS : EFI_BUFFER_TOO_SMALL;
+}
+
+/**
+  Receives a packet from a network interface.
+
+  This function retrieves one packet from the receive queue of a network interface.
+  If there are no packets on the receive queue, then EFI_NOT_READY will be
+  returned. If there is a packet on the receive queue, and the size of the packet
+  is smaller than BufferSize, then the contents of the packet will be placed in
+  Buffer, and BufferSize will be updated with the actual size of the packet.
+  In addition, if SrcAddr, DestAddr, and Protocol are not NULL, then these values
+  will be extracted from the media header and returned. EFI_SUCCESS will be
+  returned if a packet was successfully received.
+  If BufferSize is smaller than the received packet, then the size of the receive
+  packet will be placed in BufferSize and EFI_BUFFER_TOO_SMALL will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param HeaderSize The size, in bytes, of the media header received on the network
+                    interface. If this parameter is NULL, then the media header size
+                    will not be returned.
+  @param BufferSize On entry, the size, in bytes, of Buffer. On exit, the size, in
+                    bytes, of the packet that was received on the network interface.
+  @param Buffer     A pointer to the data buffer to receive both the media
+                    header and the data.
+  @param SrcAddr    The source HW MAC address. If this parameter is NULL, the HW
+                    MAC source address will not be extracted from the media header.
+  @param DestAddr   The destination HW MAC address. If this parameter is NULL,
+                    the HW MAC destination address will not be extracted from
+                    the media header.
+  @param Protocol   The media header type. If this parameter is NULL, then the
+                    protocol will not be extracted from the media header. See
+                    RFC 1700 section "Ether Types" for examples.
+
+  @retval EFI_SUCCESS           The received data was stored in Buffer, and
+                                BufferSize has been updated to the number of
+                                bytes received.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_NOT_READY         No packets have been received on the network interface.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the received packets.
+                                BufferSize has been updated to the required size.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * The This parameter is NULL
+                                * The This parameter does not point to a valid
+                                  EFI_SIMPLE_NETWORK_PROTOCOL structure.
+                                * The BufferSize parameter is NULL
+                                * The Buffer parameter is NULL
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Receive (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  OUT UINTN                      *HeaderSize OPTIONAL,
+  IN OUT UINTN                   *BufferSize,
+  OUT VOID                       *Buffer,
+  OUT EFI_MAC_ADDRESS            *SrcAddr OPTIONAL,
+  OUT EFI_MAC_ADDRESS            *DestAddr OPTIONAL,
+  OUT UINT16                     *Protocol OPTIONAL
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  if ((BufferSize == NULL) || (Buffer == NULL)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (Snp->Mode.ReceiveFilterSetting == 0) {
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = PxeReceive (
+             Snp,
+             Buffer,
+             BufferSize,
+             HeaderSize,
+             SrcAddr,
+             DestAddr,
+             Protocol
+             );
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive_filters.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive_filters.c
new file mode 100644
index 0000000000..bb98f4c8a7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Receive_filters.c
@@ -0,0 +1,484 @@
+/** @file
+    Implementation of managing the multicast receive filters of a network
+    interface.
+
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+/**
+  Call undi to enable the receive filters.
+
+  @param  Snp                Pointer to snp driver structure.
+  @param  EnableFlags        Bit mask for enabling the receive filters.
+  @param  MCastAddressCount  Multicast address count for a new multicast address
+                             list.
+  @param  MCastAddressList   List of new multicast addresses. 
+   
+  @retval EFI_SUCCESS           The multicast receive filter list was updated.
+  @retval EFI_INVALID_PARAMETER Invalid UNDI command.
+  @retval EFI_UNSUPPORTED       Command is not supported by UNDI.
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command.
+
+**/
+EFI_STATUS
+PxeRecvFilterEnable (
+  SNP_DRIVER      *Snp,
+  UINT32          EnableFlags,
+  UINTN           MCastAddressCount,
+  EFI_MAC_ADDRESS *MCastAddressList
+  )
+{
+  Snp->Cdb.OpCode     = PXE_OPCODE_RECEIVE_FILTERS;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_RECEIVE_FILTER_ENABLE;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  if ((EnableFlags & EFI_SIMPLE_NETWORK_RECEIVE_UNICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_UNICAST;
+  }
+
+  if ((EnableFlags & EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_BROADCAST;
+  }
+
+  if ((EnableFlags & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_PROMISCUOUS;
+  }
+
+  if ((EnableFlags & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_ALL_MULTICAST;
+  }
+
+  if ((EnableFlags & EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_FILTERED_MULTICAST;
+  }
+
+  if (MCastAddressCount != 0) {
+    Snp->Cdb.CPBsize  = (UINT16) (MCastAddressCount * sizeof (EFI_MAC_ADDRESS));
+    Snp->Cdb.CPBaddr  = (UINT64)(UINTN)Snp->Cpb;
+    CopyMem (Snp->Cpb, MCastAddressList, Snp->Cdb.CPBsize);
+  }
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.receive_filters()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != EFI_SUCCESS) {
+    //
+    // UNDI command failed.  Return UNDI status to caller.
+    //
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.receive_filters()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    switch (Snp->Cdb.StatCode) {
+    case PXE_STATCODE_INVALID_CDB:
+    case PXE_STATCODE_INVALID_CPB:
+    case PXE_STATCODE_INVALID_PARAMETER:
+      return EFI_INVALID_PARAMETER;
+
+    case PXE_STATCODE_UNSUPPORTED:
+      return EFI_UNSUPPORTED;
+    }
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Call undi to disable the receive filters.
+
+  @param  Snp             Pointer to snp driver structure
+  @param  DisableFlags    Bit mask for disabling the receive filters
+  @param  ResetMCastList  Boolean flag to reset/delete the multicast filter 
+                          list.
+
+  @retval EFI_SUCCESS           The multicast receive filter list was updated.
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command.
+   
+**/ 
+EFI_STATUS
+PxeRecvFilterDisable (
+  SNP_DRIVER *Snp,
+  UINT32     DisableFlags,
+  BOOLEAN    ResetMCastList
+  )
+{
+  Snp->Cdb.OpCode     = PXE_OPCODE_RECEIVE_FILTERS;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  Snp->Cdb.OpFlags    = (UINT16) ((DisableFlags != 0) ? PXE_OPFLAGS_RECEIVE_FILTER_DISABLE : PXE_OPFLAGS_NOT_USED);
+
+  if (ResetMCastList) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_RESET_MCAST_LIST;
+  }
+
+  if ((DisableFlags & EFI_SIMPLE_NETWORK_RECEIVE_UNICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_UNICAST;
+  }
+
+  if ((DisableFlags & EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_BROADCAST;
+  }
+
+  if ((DisableFlags & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_PROMISCUOUS;
+  }
+
+  if ((DisableFlags & EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_ALL_MULTICAST;
+  }
+
+  if ((DisableFlags & EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST) != 0) {
+    Snp->Cdb.OpFlags |= PXE_OPFLAGS_RECEIVE_FILTER_FILTERED_MULTICAST;
+  }
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.receive_filters()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != EFI_SUCCESS) {
+    //
+    // UNDI command failed.  Return UNDI status to caller.
+    //
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.receive_filters()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Call undi to read the receive filters.
+
+  @param  Snp                Pointer to snp driver structure.
+
+  @retval EFI_SUCCESS           The receive filter was read.
+  @retval EFI_DEVICE_ERROR      Fail to execute UNDI command. 
+   
+**/
+EFI_STATUS
+PxeRecvFilterRead (
+  SNP_DRIVER *Snp
+  )
+{
+  Snp->Cdb.OpCode   = PXE_OPCODE_RECEIVE_FILTERS;
+  Snp->Cdb.OpFlags  = PXE_OPFLAGS_RECEIVE_FILTER_READ;
+  Snp->Cdb.CPBsize  = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize   = (UINT16) (Snp->Mode.MaxMCastFilterCount * sizeof (EFI_MAC_ADDRESS));
+  Snp->Cdb.CPBaddr  = PXE_CPBADDR_NOT_USED;
+  if (Snp->Cdb.DBsize == 0) {
+    Snp->Cdb.DBaddr = (UINT64)(UINTN) NULL;
+  } else {
+    Snp->Cdb.DBaddr = (UINT64)(UINTN) Snp->Db;
+    ZeroMem (Snp->Db, Snp->Cdb.DBsize);
+  }
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  DEBUG ((EFI_D_NET, "\nsnp->undi.receive_filters()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != EFI_SUCCESS) {
+    //
+    // UNDI command failed.  Return UNDI status to caller.
+    //
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.receive_filters()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Convert UNDI32 StatFlags to EFI SNP filter flags.
+  //
+  Snp->Mode.ReceiveFilterSetting = 0;
+
+  if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_RECEIVE_FILTER_UNICAST) != 0) {
+    Snp->Mode.ReceiveFilterSetting |= EFI_SIMPLE_NETWORK_RECEIVE_UNICAST;
+  }
+
+  if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_RECEIVE_FILTER_BROADCAST) != 0) {
+    Snp->Mode.ReceiveFilterSetting |= EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST;
+  }
+
+  if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_RECEIVE_FILTER_PROMISCUOUS) != 0) {
+    Snp->Mode.ReceiveFilterSetting |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
+  }
+
+  if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_RECEIVE_FILTER_ALL_MULTICAST) != 0) {
+    Snp->Mode.ReceiveFilterSetting |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST;
+  }
+
+  if ((Snp->Cdb.StatFlags & PXE_STATFLAGS_RECEIVE_FILTER_FILTERED_MULTICAST) != 0) {
+    Snp->Mode.ReceiveFilterSetting |= EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST;
+  }
+
+  CopyMem (Snp->Mode.MCastFilter, Snp->Db, Snp->Cdb.DBsize);
+
+  //
+  // Count number of active entries in multicast filter list.
+  //
+  {
+    EFI_MAC_ADDRESS ZeroMacAddr;
+
+    SetMem (&ZeroMacAddr, sizeof ZeroMacAddr, 0);
+
+    for (Snp->Mode.MCastFilterCount = 0;
+         Snp->Mode.MCastFilterCount < Snp->Mode.MaxMCastFilterCount;
+         Snp->Mode.MCastFilterCount++
+        ) {
+      if (CompareMem (
+            &Snp->Mode.MCastFilter[Snp->Mode.MCastFilterCount],
+            &ZeroMacAddr,
+            sizeof ZeroMacAddr
+            ) == 0) {
+        break;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Manages the multicast receive filters of a network interface.
+  
+  This function is used enable and disable the hardware and software receive 
+  filters for the underlying network device.
+  The receive filter change is broken down into three steps: 
+  * The filter mask bits that are set (ON) in the Enable parameter are added to 
+    the current receive filter settings. 
+  * The filter mask bits that are set (ON) in the Disable parameter are subtracted
+    from the updated receive filter settings.
+  * If the resulting receive filter setting is not supported by the hardware a
+    more liberal setting is selected.
+  If the same bits are set in the Enable and Disable parameters, then the bits 
+  in the Disable parameter takes precedence.
+  If the ResetMCastFilter parameter is TRUE, then the multicast address list 
+  filter is disabled (irregardless of what other multicast bits are set in the 
+  Enable and Disable parameters). The SNP->Mode->MCastFilterCount field is set 
+  to zero. The Snp->Mode->MCastFilter contents are undefined.
+  After enabling or disabling receive filter settings, software should verify 
+  the new settings by checking the Snp->Mode->ReceiveFilterSettings, 
+  Snp->Mode->MCastFilterCount and Snp->Mode->MCastFilter fields.
+  Note: Some network drivers and/or devices will automatically promote receive 
+    filter settings if the requested setting can not be honored. For example, if
+    a request for four multicast addresses is made and the underlying hardware 
+    only supports two multicast addresses the driver might set the promiscuous 
+    or promiscuous multicast receive filters instead. The receiving software is
+    responsible for discarding any extra packets that get through the hardware 
+    receive filters.
+    Note: Note: To disable all receive filter hardware, the network driver must 
+      be Shutdown() and Stopped(). Calling ReceiveFilters() with Disable set to
+      Snp->Mode->ReceiveFilterSettings will make it so no more packets are 
+      returned by the Receive() function, but the receive hardware may still be 
+      moving packets into system memory before inspecting and discarding them.
+      Unexpected system errors, reboots and hangs can occur if an OS is loaded 
+      and the network devices are not Shutdown() and Stopped().
+  If ResetMCastFilter is TRUE, then the multicast receive filter list on the 
+  network interface will be reset to the default multicast receive filter list.
+  If ResetMCastFilter is FALSE, and this network interface allows the multicast 
+  receive filter list to be modified, then the MCastFilterCnt and MCastFilter 
+  are used to update the current multicast receive filter list. The modified 
+  receive filter list settings can be found in the MCastFilter field of 
+  EFI_SIMPLE_NETWORK_MODE. If the network interface does not allow the multicast
+  receive filter list to be modified, then EFI_INVALID_PARAMETER will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+  If the receive filter mask and multicast receive filter list have been 
+  successfully updated on the network interface, EFI_SUCCESS will be returned.
+
+  @param This             A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Enable           A bit mask of receive filters to enable on the network
+                          interface.
+  @param Disable          A bit mask of receive filters to disable on the network
+                          interface. For backward compatibility with EFI 1.1 
+                          platforms, the EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST bit
+                          must be set when the ResetMCastFilter parameter is TRUE.
+  @param ResetMCastFilter Set to TRUE to reset the contents of the multicast 
+                          receive filters on the network interface to their 
+                          default values. 
+  @param MCastFilterCnt   Number of multicast HW MAC addresses in the new MCastFilter
+                          list. This value must be less than or equal to the 
+                          MCastFilterCnt field of EFI_SIMPLE_NETWORK_MODE. 
+                          This field is optional if ResetMCastFilter is TRUE.
+  @param MCastFilter      A pointer to a list of new multicast receive filter HW
+                          MAC addresses. This list will replace any existing 
+                          multicast HW MAC address list. This field is optional 
+                          if ResetMCastFilter is TRUE.
+   
+  @retval EFI_SUCCESS            The multicast receive filter list was updated.
+  @retval EFI_NOT_STARTED        The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 * This is NULL
+                                 * There are bits set in Enable that are not set
+                                   in Snp->Mode->ReceiveFilterMask
+                                 * There are bits set in Disable that are not set
+                                   in Snp->Mode->ReceiveFilterMask
+                                 * Multicast is being enabled (the 
+                                   EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST bit is 
+                                   set in Enable, it is not set in Disable, and 
+                                   ResetMCastFilter is FALSE) and MCastFilterCount
+                                   is zero
+                                 * Multicast is being enabled and MCastFilterCount
+                                   is greater than Snp->Mode->MaxMCastFilterCount
+                                 * Multicast is being enabled and MCastFilter is NULL
+                                 * Multicast is being enabled and one or more of
+                                   the addresses in the MCastFilter list are not
+                                   valid multicast MAC addresses
+  @retval EFI_DEVICE_ERROR       One or more of the following conditions is TRUE:
+                                 * The network interface has been started but has
+                                   not been initialized
+                                 * An unexpected error was returned by the 
+                                   underlying network driver or device
+  @retval EFI_UNSUPPORTED        This function is not supported by the network
+                                 interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32ReceiveFilters (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINT32                      Enable,
+  IN UINT32                      Disable,
+  IN BOOLEAN                     ResetMCastFilter,
+  IN UINTN                       MCastFilterCnt,  OPTIONAL
+  IN EFI_MAC_ADDRESS             *MCastFilter     OPTIONAL
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_STATUS  Status;
+  EFI_TPL     OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+  //
+  // check if we are asked to enable or disable something that the UNDI
+  // does not even support!
+  //
+  if (((Enable &~Snp->Mode.ReceiveFilterMask) != 0) ||
+    ((Disable &~Snp->Mode.ReceiveFilterMask) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (ResetMCastFilter) {
+
+    Disable |= EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST & Snp->Mode.ReceiveFilterMask;
+    MCastFilterCnt = 0;
+    MCastFilter    = NULL;
+  } else {
+    if (MCastFilterCnt != 0) {
+      if ((MCastFilterCnt > Snp->Mode.MaxMCastFilterCount) ||
+          (MCastFilter == NULL)) {
+
+        Status = EFI_INVALID_PARAMETER;
+        goto ON_EXIT;
+      }
+    }
+  }
+
+  if (Enable == 0 && Disable == 0 && !ResetMCastFilter && MCastFilterCnt == 0) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if ((Enable & EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST) != 0 && MCastFilterCnt == 0) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if ((Enable != 0) || (MCastFilterCnt != 0)) {
+    Status = PxeRecvFilterEnable (
+               Snp,
+               Enable,
+               MCastFilterCnt,
+               MCastFilter
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  if ((Disable != 0) || ResetMCastFilter) {
+    Status = PxeRecvFilterDisable (Snp, Disable, ResetMCastFilter);
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  Status = PxeRecvFilterRead (Snp);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Reset.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Reset.c
new file mode 100644
index 0000000000..2381264c11
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Reset.c
@@ -0,0 +1,136 @@
+/** @file
+    Implementation of resetting a network adapter.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+
+/**
+  Call UNDI to reset the NIC.
+
+  @param  Snp                 Pointer to the snp driver structure.
+
+  @return EFI_SUCCESSFUL      The NIC was reset.
+  @retval EFI_DEVICE_ERROR    The NIC cannot be reset.
+
+**/
+EFI_STATUS
+PxeReset (
+  SNP_DRIVER *Snp
+  )
+{
+  Snp->Cdb.OpCode     = PXE_OPCODE_RESET;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.reset()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    DEBUG (
+      (EFI_D_WARN,
+      "\nsnp->undi32.reset()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    //
+    // UNDI could not be reset. Return UNDI error.
+    //
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Resets a network adapter and reinitializes it with the parameters that were
+  provided in the previous call to Initialize().
+
+  This function resets a network adapter and reinitializes it with the parameters
+  that were provided in the previous call to Initialize(). The transmit and 
+  receive queues are emptied and all pending interrupts are cleared.
+  Receive filters, the station address, the statistics, and the multicast-IP-to-HW 
+  MAC addresses are not reset by this call. If the network interface was 
+  successfully reset, then EFI_SUCCESS will be returned. If the driver has not 
+  been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This                 A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param ExtendedVerification Indicates that the driver may perform a more 
+                              exhaustive verification operation of the device 
+                              during reset.
+
+  @retval EFI_SUCCESS           The network interface was reset.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER One or more of the parameters has an unsupported value.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Reset (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     ExtendedVerification
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  //
+  // Resolve Warning 4 unreferenced parameter problem
+  //
+  ExtendedVerification = 0;
+  DEBUG ((EFI_D_WARN, "ExtendedVerification = %d is not implemented!\n", ExtendedVerification));
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = PxeReset (Snp);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Shutdown.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Shutdown.c
new file mode 100644
index 0000000000..1f51992778
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Shutdown.c
@@ -0,0 +1,152 @@
+/** @file
+    Implementation of shuting down a network adapter.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+
+/**
+  Call UNDI to shut down the interface.
+
+  @param  Snp   Pointer to snp driver structure.
+
+  @retval EFI_SUCCESS        UNDI is shut down successfully.
+  @retval EFI_DEVICE_ERROR   UNDI could not be shut down.
+
+**/
+EFI_STATUS
+PxeShutdown (
+  IN SNP_DRIVER *Snp
+  )
+{
+  Snp->Cdb.OpCode     = PXE_OPCODE_SHUTDOWN;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.shutdown()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    //
+    // UNDI could not be shutdown. Return UNDI error.
+    //
+    DEBUG ((EFI_D_WARN, "\nsnp->undi.shutdown()  %xh:%xh\n", Snp->Cdb.StatFlags, Snp->Cdb.StatCode));
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Free allocated memory.
+  //
+  if (Snp->TxRxBuffer != NULL) {
+    Snp->PciIo->FreeBuffer (
+                  Snp->PciIo,
+                  SNP_MEM_PAGES (Snp->TxRxBufferSize),
+                  (VOID *) Snp->TxRxBuffer
+                  );
+  }
+
+  Snp->TxRxBuffer      = NULL;
+  Snp->TxRxBufferSize  = 0;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Resets a network adapter and leaves it in a state that is safe for another 
+  driver to initialize. 
+  
+  This function releases the memory buffers assigned in the Initialize() call.
+  Pending transmits and receives are lost, and interrupts are cleared and disabled.
+  After this call, only the Initialize() and Stop() calls may be used. If the 
+  network interface was successfully shutdown, then EFI_SUCCESS will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param  This  A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The network interface was shutdown.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid 
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Shutdown (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_STATUS  Status;
+  EFI_TPL     OldTpl;
+
+  //
+  // Get pointer to SNP driver instance for *This.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Return error if the SNP is not initialized.
+  //
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+  
+  Status                          = PxeShutdown (Snp);
+
+  Snp->Mode.State                 = EfiSimpleNetworkStarted;
+  Snp->Mode.ReceiveFilterSetting  = 0;
+
+  Snp->Mode.MCastFilterCount      = 0;
+  Snp->Mode.ReceiveFilterSetting  = 0;
+  ZeroMem (Snp->Mode.MCastFilter, sizeof Snp->Mode.MCastFilter);
+  CopyMem (
+    &Snp->Mode.CurrentAddress,
+    &Snp->Mode.PermanentAddress,
+    sizeof (EFI_MAC_ADDRESS)
+    );
+
+  gBS->CloseEvent (Snp->Snp.WaitForPacket);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.c
new file mode 100644
index 0000000000..5ff294f8b5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.c
@@ -0,0 +1,864 @@
+/** @file
+  Implementation of driver entry point and driver binding protocol.
+
+Copyright (c) 2004 - 2016, 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 "Snp.h"
+
+/**
+  One notified function to stop UNDI device when gBS->ExitBootServices() called.
+
+  @param  Event                   Pointer to this event
+  @param  Context                 Event handler private data
+
+**/
+VOID
+EFIAPI
+SnpNotifyExitBootServices (
+  EFI_EVENT Event,
+  VOID      *Context
+  )
+{
+  SNP_DRIVER             *Snp;
+
+  Snp  = (SNP_DRIVER *)Context;
+
+  //
+  // Shutdown and stop UNDI driver
+  //
+  PxeShutdown (Snp);
+  PxeStop (Snp);
+}
+
+/**
+  Send command to UNDI. It does nothing currently.
+
+  @param Cdb   command to be sent to UNDI.
+
+  @retval EFI_INVALID_PARAMETER  The command is 0.
+  @retval EFI_UNSUPPORTED        Default return status because it's not
+                                 supported currently.
+
+**/
+EFI_STATUS
+EFIAPI
+IssueHwUndiCommand (
+  UINT64 Cdb
+  )
+{
+  DEBUG ((EFI_D_ERROR, "\nIssueHwUndiCommand() - This should not be called!"));
+
+  if (Cdb == 0) {
+    return EFI_INVALID_PARAMETER;
+
+  }
+  //
+  //  %%TBD - For now, nothing is done.
+  //
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Compute 8-bit checksum of a buffer.
+
+  @param  Buffer               Pointer to buffer.
+  @param  Length               Length of buffer in bytes.
+
+  @return 8-bit checksum of all bytes in buffer, or zero if ptr is NULL or len
+          is zero.
+
+**/
+UINT8
+Calc8BitCksum (
+  VOID  *Buffer,
+  UINTN Length
+  )
+{
+  UINT8 *Ptr;
+  UINT8 Cksum;
+
+  Ptr   = Buffer;
+  Cksum = 0;
+
+  if (Ptr == NULL || Length == 0) {
+    return 0;
+  }
+
+  while (Length-- != 0) {
+    Cksum = (UINT8) (Cksum + *Ptr++);
+  }
+
+  return Cksum;
+}
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test.
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device.
+  @retval EFI_ALREADY_STARTED This driver is already running on this device.
+  @retval other               This driver does not support this device.
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkDriverSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_NETWORK_INTERFACE_IDENTIFIER_PROTOCOL *NiiProtocol;
+  PXE_UNDI                                  *Pxe;
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+                  (VOID **) &NiiProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_ALREADY_STARTED) {
+      DEBUG ((EFI_D_INFO, "Support(): Already Started. on handle %p\n", Controller));
+    }
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Support(): UNDI3.1 found on handle %p\n", Controller));
+
+  //
+  // check the version, we don't want to connect to the undi16
+  //
+  if (NiiProtocol->Type != EfiNetworkInterfaceUndi) {
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+  //
+  // Check to see if !PXE structure is valid. Paragraph alignment of !PXE structure is required.
+  //
+  if ((NiiProtocol->Id & 0x0F) != 0) {
+    DEBUG ((EFI_D_NET, "\n!PXE structure is not paragraph aligned.\n"));
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  Pxe = (PXE_UNDI *) (UINTN) (NiiProtocol->Id);
+
+  //
+  //  Verify !PXE revisions.
+  //
+  if (Pxe->hw.Signature != PXE_ROMID_SIGNATURE) {
+    DEBUG ((EFI_D_NET, "\n!PXE signature is not valid.\n"));
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  if (Pxe->hw.Rev < PXE_ROMID_REV) {
+    DEBUG ((EFI_D_NET, "\n!PXE.Rev is not supported.\n"));
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+
+  if (Pxe->hw.MajorVer < PXE_ROMID_MAJORVER) {
+
+    DEBUG ((EFI_D_NET, "\n!PXE.MajorVer is not supported.\n"));
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+
+  } else if (Pxe->hw.MajorVer == PXE_ROMID_MAJORVER && Pxe->hw.MinorVer < PXE_ROMID_MINORVER) {
+    DEBUG ((EFI_D_NET, "\n!PXE.MinorVer is not supported."));
+    Status = EFI_UNSUPPORTED;
+    goto Done;
+  }
+  //
+  // Do S/W UNDI specific checks.
+  //
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_HW_UNDI) == 0) {
+    if (Pxe->sw.EntryPoint < Pxe->sw.Len) {
+      DEBUG ((EFI_D_NET, "\n!PXE S/W entry point is not valid."));
+      Status = EFI_UNSUPPORTED;
+      goto Done;
+    }
+
+    if (Pxe->sw.BusCnt == 0) {
+      DEBUG ((EFI_D_NET, "\n!PXE.BusCnt is zero."));
+      Status = EFI_UNSUPPORTED;
+      goto Done;
+    }
+  }
+
+  Status = EFI_SUCCESS;
+  DEBUG ((EFI_D_INFO, "Support(): supported on %p\n", Controller));
+
+Done:
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  return Status;
+}
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to.
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_DEVICE_ERROR     This driver could not be started due to a device error
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkDriverStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN EFI_HANDLE                     Controller,
+  IN EFI_DEVICE_PATH_PROTOCOL       *RemainingDevicePath
+  )
+{
+  EFI_NETWORK_INTERFACE_IDENTIFIER_PROTOCOL *Nii;
+  EFI_DEVICE_PATH_PROTOCOL                  *NiiDevicePath;
+  EFI_STATUS                                Status;
+  PXE_UNDI                                  *Pxe;
+  SNP_DRIVER                                *Snp;
+  VOID                                      *Address;
+  EFI_HANDLE                                Handle;
+  UINT8                                     BarIndex;
+  PXE_STATFLAGS                             InitStatFlags;
+  EFI_PCI_IO_PROTOCOL                       *PciIo;
+  EFI_ACPI_ADDRESS_SPACE_DESCRIPTOR         *BarDesc;
+  BOOLEAN                                   FoundIoBar;
+  BOOLEAN                                   FoundMemoryBar;
+  
+  DEBUG ((EFI_D_NET, "\nSnpNotifyNetworkInterfaceIdentifier()  "));
+
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &NiiDevicePath,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->LocateDevicePath (
+                  &gEfiPciIoProtocolGuid,
+                  &NiiDevicePath,
+                  &Handle
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiPciIoProtocolGuid,
+                  (VOID **) &PciIo,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get the NII interface.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+                  (VOID **) &Nii,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+          Controller,
+          &gEfiDevicePathProtocolGuid,
+          This->DriverBindingHandle,
+          Controller
+          );
+    return Status;
+  }
+
+  DEBUG ((EFI_D_INFO, "Start(): UNDI3.1 found\n"));
+
+  Pxe = (PXE_UNDI *) (UINTN) (Nii->Id);
+
+  if (Calc8BitCksum (Pxe, Pxe->hw.Len) != 0) {
+    DEBUG ((EFI_D_NET, "\n!PXE checksum is not correct.\n"));
+    goto NiiError;
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_PROMISCUOUS_RX_SUPPORTED) != 0) {
+    //
+    //  We can get any packets.
+    //
+  } else if ((Pxe->hw.Implementation & PXE_ROMID_IMP_BROADCAST_RX_SUPPORTED) != 0) {
+    //
+    //  We need to be able to get broadcast packets for DHCP.
+    //  If we do not have promiscuous support, we must at least have
+    //  broadcast support or we cannot do DHCP!
+    //
+  } else {
+    DEBUG ((EFI_D_NET, "\nUNDI does not have promiscuous or broadcast support."));
+    goto NiiError;
+  }
+  //
+  // OK, we like this UNDI, and we know snp is not already there on this handle
+  // Allocate and initialize a new simple network protocol structure.
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    SNP_MEM_PAGES (sizeof (SNP_DRIVER)),
+                    &Address,
+                    0
+                    );
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_NET, "\nCould not allocate SNP_DRIVER structure.\n"));
+    goto NiiError;
+  }
+
+  Snp = (SNP_DRIVER *) (UINTN) Address;
+
+  ZeroMem (Snp, sizeof (SNP_DRIVER));
+
+  Snp->PciIo      = PciIo;
+  Snp->Signature  = SNP_DRIVER_SIGNATURE;
+
+  EfiInitializeLock (&Snp->Lock, TPL_NOTIFY);
+
+  Snp->Snp.Revision       = EFI_SIMPLE_NETWORK_PROTOCOL_REVISION;
+  Snp->Snp.Start          = SnpUndi32Start;
+  Snp->Snp.Stop           = SnpUndi32Stop;
+  Snp->Snp.Initialize     = SnpUndi32Initialize;
+  Snp->Snp.Reset          = SnpUndi32Reset;
+  Snp->Snp.Shutdown       = SnpUndi32Shutdown;
+  Snp->Snp.ReceiveFilters = SnpUndi32ReceiveFilters;
+  Snp->Snp.StationAddress = SnpUndi32StationAddress;
+  Snp->Snp.Statistics     = SnpUndi32Statistics;
+  Snp->Snp.MCastIpToMac   = SnpUndi32McastIpToMac;
+  Snp->Snp.NvData         = SnpUndi32NvData;
+  Snp->Snp.GetStatus      = SnpUndi32GetStatus;
+  Snp->Snp.Transmit       = SnpUndi32Transmit;
+  Snp->Snp.Receive        = SnpUndi32Receive;
+  Snp->Snp.WaitForPacket  = NULL;
+
+  Snp->Snp.Mode           = &Snp->Mode;
+
+  Snp->TxRxBufferSize     = 0;
+  Snp->TxRxBuffer         = NULL;
+
+  Snp->RecycledTxBuf = AllocatePool (sizeof (UINT64) * SNP_TX_BUFFER_INCREASEMENT);
+  if (Snp->RecycledTxBuf == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Error_DeleteSNP;
+  }
+  Snp->MaxRecycledTxBuf    = SNP_TX_BUFFER_INCREASEMENT;
+  Snp->RecycledTxBufCount  = 0;
+ 
+  if (Nii->Revision >= EFI_NETWORK_INTERFACE_IDENTIFIER_PROTOCOL_REVISION) {
+    Snp->IfNum = Nii->IfNum;
+
+  } else {
+    Snp->IfNum = (UINT8) (Nii->IfNum & 0xFF);
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_HW_UNDI) != 0) {
+    Snp->IsSwUndi             = FALSE;
+    Snp->IssueUndi32Command   = &IssueHwUndiCommand;
+  } else {
+    Snp->IsSwUndi = TRUE;
+
+    if ((Pxe->sw.Implementation & PXE_ROMID_IMP_SW_VIRT_ADDR) != 0) {
+      Snp->IssueUndi32Command = (ISSUE_UNDI32_COMMAND) (UINTN) Pxe->sw.EntryPoint;
+    } else {
+      Snp->IssueUndi32Command = (ISSUE_UNDI32_COMMAND) (UINTN) ((UINT8) (UINTN) Pxe + Pxe->sw.EntryPoint);
+    }
+  }
+  //
+  // Allocate a global CPB and DB buffer for this UNDI interface.
+  // we do this because:
+  //
+  // -UNDI 3.0 wants all the addresses passed to it (even the cpb and db) to be
+  // within 2GB limit, create them here and map them so that when undi calls
+  // v2p callback to check if the physical address is < 2gb, we will pass.
+  //
+  // -This is not a requirement for 3.1 or later UNDIs but the code looks
+  // simpler if we use the same cpb, db variables for both old and new undi
+  // interfaces from all the SNP interface calls (we don't map the buffers
+  // for the newer undi interfaces though)
+  // .
+  // -it is OK to allocate one global set of CPB, DB pair for each UNDI
+  // interface as EFI does not multi-task and so SNP will not be re-entered!
+  //
+  Status = PciIo->AllocateBuffer (
+                    PciIo,
+                    AllocateAnyPages,
+                    EfiBootServicesData,
+                    SNP_MEM_PAGES (4096),
+                    &Address,
+                    0
+                    );
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_NET, "\nCould not allocate CPB and DB structures.\n"));
+    goto Error_DeleteSNP;
+  }
+
+  Snp->Cpb  = (VOID *) (UINTN) Address;
+  Snp->Db   = (VOID *) ((UINTN) Address + 2048);
+
+  //
+  // Find the correct BAR to do IO.
+  //
+  // Enumerate through the PCI BARs for the device to determine which one is
+  // the IO BAR.  Save the index of the BAR into the adapter info structure.
+  // for regular 32bit BARs, 0 is memory mapped, 1 is io mapped
+  //
+  Snp->MemoryBarIndex = 0;
+  Snp->IoBarIndex     = 1;
+  FoundMemoryBar      = FALSE;
+  FoundIoBar          = FALSE;
+  for (BarIndex = 0; BarIndex < PCI_MAX_BAR; BarIndex++) {
+    Status = PciIo->GetBarAttributes (
+                      PciIo,
+                      BarIndex,
+                      NULL,
+                      (VOID**) &BarDesc
+                      );
+    if (Status == EFI_UNSUPPORTED) {
+      continue;
+    } else if (EFI_ERROR (Status)) {
+      goto Error_DeleteSNP;
+    }
+
+    if ((!FoundMemoryBar) && (BarDesc->ResType == ACPI_ADDRESS_SPACE_TYPE_MEM)) {
+      Snp->MemoryBarIndex = BarIndex;
+      FoundMemoryBar      = TRUE;
+    } else if ((!FoundIoBar) && (BarDesc->ResType == ACPI_ADDRESS_SPACE_TYPE_IO)) {
+      Snp->IoBarIndex = BarIndex;
+      FoundIoBar      = TRUE;
+    }
+
+    FreePool (BarDesc);
+
+    if (FoundMemoryBar && FoundIoBar) {
+      break;
+    }
+  }
+
+  Status = PxeStart (Snp);
+
+  if (Status != EFI_SUCCESS) {
+    goto Error_DeleteSNP;
+  }
+
+  Snp->Cdb.OpCode     = PXE_OPCODE_GET_INIT_INFO;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_DBADDR_NOT_USED;
+
+  Snp->Cdb.DBsize     = (UINT16) sizeof (Snp->InitInfo);
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) (&Snp->InitInfo);
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  DEBUG ((EFI_D_NET, "\nSnp->undi.get_init_info()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  //
+  // Save the INIT Stat Code...
+  //
+  InitStatFlags = Snp->Cdb.StatFlags;
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    DEBUG ((EFI_D_NET, "\nSnp->undi.init_info()  %xh:%xh\n", Snp->Cdb.StatFlags, Snp->Cdb.StatCode));
+    PxeStop (Snp);
+    goto Error_DeleteSNP;
+  }
+
+  //
+  //  Initialize simple network protocol mode structure
+  //
+  Snp->Mode.State               = EfiSimpleNetworkStopped;
+  Snp->Mode.HwAddressSize       = Snp->InitInfo.HWaddrLen;
+  Snp->Mode.MediaHeaderSize     = Snp->InitInfo.MediaHeaderLen;
+  Snp->Mode.MaxPacketSize       = Snp->InitInfo.FrameDataLen;
+  Snp->Mode.NvRamAccessSize     = Snp->InitInfo.NvWidth;
+  Snp->Mode.NvRamSize           = Snp->InitInfo.NvCount * Snp->Mode.NvRamAccessSize;
+  Snp->Mode.IfType              = Snp->InitInfo.IFtype;
+  Snp->Mode.MaxMCastFilterCount = Snp->InitInfo.MCastFilterCnt;
+  Snp->Mode.MCastFilterCount    = 0;
+
+  switch (InitStatFlags & PXE_STATFLAGS_CABLE_DETECT_MASK) {
+  case PXE_STATFLAGS_CABLE_DETECT_SUPPORTED:
+    Snp->Mode.MediaPresentSupported = TRUE;
+    break;
+
+  case PXE_STATFLAGS_CABLE_DETECT_NOT_SUPPORTED:
+  default:
+    Snp->Mode.MediaPresentSupported = FALSE;
+  }
+
+  switch (InitStatFlags & PXE_STATFLAGS_GET_STATUS_NO_MEDIA_MASK) {
+  case PXE_STATFLAGS_GET_STATUS_NO_MEDIA_SUPPORTED:
+    Snp->MediaStatusSupported = TRUE;
+    break;
+
+  case PXE_STATFLAGS_GET_STATUS_NO_MEDIA_NOT_SUPPORTED:
+  default:
+    Snp->MediaStatusSupported = FALSE;
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_STATION_ADDR_SETTABLE) != 0) {
+    Snp->Mode.MacAddressChangeable = TRUE;
+  } else {
+    Snp->Mode.MacAddressChangeable = FALSE;
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_MULTI_FRAME_SUPPORTED) != 0) {
+    Snp->Mode.MultipleTxSupported = TRUE;
+  } else {
+    Snp->Mode.MultipleTxSupported = FALSE;
+  }
+
+  Snp->Mode.ReceiveFilterMask = EFI_SIMPLE_NETWORK_RECEIVE_UNICAST;
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_PROMISCUOUS_MULTICAST_RX_SUPPORTED) != 0) {
+    Snp->Mode.ReceiveFilterMask |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST;
+
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_PROMISCUOUS_RX_SUPPORTED) != 0) {
+    Snp->Mode.ReceiveFilterMask |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS;
+
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_BROADCAST_RX_SUPPORTED) != 0) {
+    Snp->Mode.ReceiveFilterMask |= EFI_SIMPLE_NETWORK_RECEIVE_BROADCAST;
+
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_FILTERED_MULTICAST_RX_SUPPORTED) != 0) {
+    Snp->Mode.ReceiveFilterMask |= EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST;
+
+  }
+
+  if ((Pxe->hw.Implementation & PXE_ROMID_IMP_PROMISCUOUS_MULTICAST_RX_SUPPORTED) != 0) {
+    Snp->Mode.ReceiveFilterMask |= EFI_SIMPLE_NETWORK_RECEIVE_PROMISCUOUS_MULTICAST;
+
+  }
+
+  Snp->Mode.ReceiveFilterSetting = 0;
+
+  //
+  //  need to get the station address to save in the mode structure. we need to
+  // initialize the UNDI first for this.
+  //
+  Snp->TxRxBufferSize = Snp->InitInfo.MemoryRequired;
+  Status              = PxeInit (Snp, PXE_OPFLAGS_INITIALIZE_DO_NOT_DETECT_CABLE);
+
+  if (EFI_ERROR (Status)) {
+    PxeStop (Snp);
+    goto Error_DeleteSNP;
+  }
+
+  Status = PxeGetStnAddr (Snp);
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((EFI_D_ERROR, "\nSnp->undi.get_station_addr() failed.\n"));
+    PxeShutdown (Snp);
+    PxeStop (Snp);
+    goto Error_DeleteSNP;
+  }
+
+  Snp->Mode.MediaPresent = FALSE;
+
+  //
+  // We should not leave UNDI started and initialized here. this DriverStart()
+  // routine must only find and attach the SNP interface to UNDI layer that it
+  // finds on the given handle!
+  // The UNDI layer will be started when upper layers call Snp->start.
+  // How ever, this DriverStart() must fill up the snp mode structure which
+  // contains the MAC address of the NIC. For this reason we started and
+  // initialized UNDI here, now we are done, do a shutdown and stop of the
+  // UNDI interface!
+  //
+  PxeShutdown (Snp);
+  PxeStop (Snp);
+
+  //
+  // Create EXIT_BOOT_SERIVES Event
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  SnpNotifyExitBootServices,
+                  Snp,
+                  &gEfiEventExitBootServicesGuid,
+                  &Snp->ExitBootServicesEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Error_DeleteSNP;
+  }
+
+  //
+  //  add SNP to the undi handle
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &Controller,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &(Snp->Snp)
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  PciIo->FreeBuffer (
+           PciIo,
+           SNP_MEM_PAGES (4096),
+           Snp->Cpb
+           );
+
+Error_DeleteSNP:
+
+  if (Snp->RecycledTxBuf != NULL) {
+    FreePool (Snp->RecycledTxBuf);
+  }
+
+  PciIo->FreeBuffer (
+           PciIo,
+           SNP_MEM_PAGES (sizeof (SNP_DRIVER)),
+           Snp
+           );
+NiiError:
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  gBS->CloseProtocol (
+        Controller,
+        &gEfiDevicePathProtocolGuid,
+        This->DriverBindingHandle,
+        Controller
+        );
+
+  //
+  // If we got here that means we are in error state.
+  //
+  if (!EFI_ERROR (Status)) {
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  return Status;
+}
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+SimpleNetworkDriverStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL    *This,
+  IN  EFI_HANDLE                     Controller,
+  IN  UINTN                          NumberOfChildren,
+  IN  EFI_HANDLE                     *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_SIMPLE_NETWORK_PROTOCOL *SnpProtocol;
+  SNP_DRIVER                  *Snp;
+  EFI_PCI_IO_PROTOCOL         *PciIo;
+
+  //
+  // Get our context back.
+  //
+  Status = gBS->OpenProtocol (
+                  Controller,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  (VOID **) &SnpProtocol,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (SnpProtocol);
+
+  Status = gBS->UninstallProtocolInterface (
+                  Controller,
+                  &gEfiSimpleNetworkProtocolGuid,
+                  &Snp->Snp
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close EXIT_BOOT_SERIVES Event
+  //
+  gBS->CloseEvent (Snp->ExitBootServicesEvent);
+
+  Status = gBS->CloseProtocol (
+                  Controller,
+                  &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  Status = gBS->CloseProtocol (
+                  Controller,
+                  &gEfiDevicePathProtocolGuid,
+                  This->DriverBindingHandle,
+                  Controller
+                  );
+
+  PxeShutdown (Snp);
+  PxeStop (Snp);
+
+  FreePool (Snp->RecycledTxBuf);
+
+  PciIo = Snp->PciIo;
+  PciIo->FreeBuffer (
+           PciIo,
+           SNP_MEM_PAGES (4096),
+           Snp->Cpb
+           );
+
+  PciIo->FreeBuffer (
+           PciIo,
+           SNP_MEM_PAGES (sizeof (SNP_DRIVER)),
+           Snp
+           );
+
+  return Status;
+}
+
+//
+// Simple Network Protocol Driver Global Variables
+//
+EFI_DRIVER_BINDING_PROTOCOL gSimpleNetworkDriverBinding = {
+  SimpleNetworkDriverSupported,
+  SimpleNetworkDriverStart,
+  SimpleNetworkDriverStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  The SNP driver entry point.
+
+  @param ImageHandle       The driver image handle.
+  @param SystemTable       The system table.
+
+  @retval EFI_SUCEESS      Initialization routine has found UNDI hardware,
+                           loaded it's ROM, and installed a notify event for
+                           the Network Indentifier Interface Protocol
+                           successfully.
+  @retval Other            Return value from HandleProtocol for
+                           DeviceIoProtocol or LoadedImageProtocol
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeSnpNiiDriver (
+  IN EFI_HANDLE       ImageHandle,
+  IN EFI_SYSTEM_TABLE *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gSimpleNetworkDriverBinding,
+           ImageHandle,
+           &gSimpleNetworkComponentName,
+           &gSimpleNetworkComponentName2
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.h b/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.h
new file mode 100644
index 0000000000..67f65ffc42
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Snp.h
@@ -0,0 +1,1007 @@
+/** @file
+    Declaration of strctures and functions for SnpDxe driver.
+
+Copyright (c) 2004 - 2016, 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.
+
+**/
+#ifndef _SNP_H_
+#define _SNP_H_
+
+
+#include <Uefi.h>
+
+#include <Protocol/SimpleNetwork.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/NetworkInterfaceIdentifier.h>
+#include <Protocol/DevicePath.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PrintLib.h>
+
+#include <IndustryStandard/Pci.h>
+#include <IndustryStandard/Acpi.h>
+
+#define FOUR_GIGABYTES  (UINT64) 0x100000000ULL
+
+
+#define SNP_DRIVER_SIGNATURE  SIGNATURE_32 ('s', 'n', 'd', 's')
+#define MAX_MAP_LENGTH        100
+
+#define PCI_BAR_IO_MASK       0x00000003
+#define PCI_BAR_IO_MODE       0x00000001
+
+#define PCI_BAR_MEM_MASK      0x0000000F
+#define PCI_BAR_MEM_MODE      0x00000000
+#define PCI_BAR_MEM_64BIT     0x00000004
+
+#define SNP_TX_BUFFER_INCREASEMENT    MAX_XMIT_BUFFERS
+#define SNP_MAX_TX_BUFFER_NUM         65536
+
+typedef
+EFI_STATUS
+(EFIAPI *ISSUE_UNDI32_COMMAND) (
+  UINT64         Cdb
+  );
+
+typedef struct {
+  UINT32                      Signature;
+  EFI_LOCK                    Lock;
+
+  EFI_SIMPLE_NETWORK_PROTOCOL Snp;
+  EFI_SIMPLE_NETWORK_MODE     Mode;
+
+  EFI_HANDLE                  DeviceHandle;
+  EFI_DEVICE_PATH_PROTOCOL    *DevicePath;
+
+  //
+  //  Local instance data needed by SNP driver
+  //
+  //  Pointer to S/W UNDI API entry point
+  //  This will be NULL for H/W UNDI
+  //
+  ISSUE_UNDI32_COMMAND  IssueUndi32Command;
+
+  BOOLEAN               IsSwUndi;
+
+  //
+  // undi interface number, if one undi manages more nics
+  //
+  PXE_IFNUM             IfNum;
+
+  //
+  //  Allocated tx/rx buffer that was passed to UNDI Initialize.
+  //
+  UINT32                TxRxBufferSize;
+  VOID                  *TxRxBuffer;
+  //
+  // mappable buffers for receive and fill header for undi3.0
+  // these will be used if the user buffers are above 4GB limit (instead of
+  // mapping the user buffers)
+  //
+  UINT8                 *ReceiveBufffer;
+  VOID                  *ReceiveBufferUnmap;
+  UINT8                 *FillHeaderBuffer;
+  VOID                  *FillHeaderBufferUnmap;
+
+  EFI_PCI_IO_PROTOCOL   *PciIo;
+  UINT8                 IoBarIndex;
+  UINT8                 MemoryBarIndex;
+
+  //
+  // Buffers for command descriptor block, command parameter block
+  // and data block.
+  //
+  PXE_CDB               Cdb;
+  VOID                  *Cpb;
+  VOID                  *CpbUnmap;
+  VOID                  *Db;
+
+  //
+  // UNDI structure, we need to remember the init info for a long time!
+  //
+  PXE_DB_GET_INIT_INFO  InitInfo;
+
+  VOID                  *SnpDriverUnmap;
+  //
+  // when ever we map an address, we must remember it's address and the un-map
+  // cookie so that we can unmap later
+  //
+  struct MAP_LIST {
+    EFI_PHYSICAL_ADDRESS  VirtualAddress;
+    VOID                  *MapCookie;
+  } MapList[MAX_MAP_LENGTH];
+
+  EFI_EVENT              ExitBootServicesEvent;
+
+  //
+  // Whether UNDI support reporting media status from GET_STATUS command,
+  // i.e. PXE_STATFLAGS_GET_STATUS_NO_MEDIA_SUPPORTED
+  //
+  BOOLEAN                MediaStatusSupported;
+
+  //
+  // Array of the recycled transmit buffer address from UNDI.
+  //
+  UINT64                 *RecycledTxBuf;
+  //
+  // The maximum number of recycled buffer pointers in RecycledTxBuf.
+  //
+  UINT32                 MaxRecycledTxBuf;
+  //
+  // Current number of recycled buffer pointers in RecycledTxBuf.
+  //
+  UINT32                 RecycledTxBufCount;
+} SNP_DRIVER;
+
+#define EFI_SIMPLE_NETWORK_DEV_FROM_THIS(a) CR (a, SNP_DRIVER, Snp, SNP_DRIVER_SIGNATURE)
+
+//
+// Global Variables
+//
+extern EFI_DRIVER_BINDING_PROTOCOL    gSimpleNetworkDriverBinding;
+extern EFI_COMPONENT_NAME_PROTOCOL    gSimpleNetworkComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gSimpleNetworkComponentName2;
+
+/**
+  this routine calls undi to start the interface and changes the snp state.
+
+  @param  Snp                    pointer to snp driver structure
+
+  @retval EFI_DEVICE_ERROR       UNDI could not be started
+  @retval EFI_SUCCESS            UNDI is started successfully
+
+**/
+EFI_STATUS
+PxeStart (
+  IN SNP_DRIVER *Snp
+  );
+
+/**
+  this routine calls undi to stop the interface and changes the snp state.
+
+  @param  Snp   pointer to snp driver structure
+
+  @retval EFI_INVALID_PARAMETER  invalid parameter
+  @retval EFI_NOT_STARTED        SNP is not started
+  @retval EFI_DEVICE_ERROR       SNP is not initialized
+  @retval EFI_UNSUPPORTED        operation unsupported
+
+**/
+EFI_STATUS
+PxeStop (
+  SNP_DRIVER *Snp
+  );
+
+/**
+  this routine calls undi to initialize the interface.
+
+  @param  Snp                   pointer to snp driver structure
+  @param  CableDetectFlag       Do/don't detect the cable (depending on what undi supports)
+
+  @retval EFI_SUCCESS           UNDI is initialized successfully
+  @retval EFI_DEVICE_ERROR      UNDI could not be initialized
+  @retval Other                 other errors
+
+**/
+EFI_STATUS
+PxeInit (
+  SNP_DRIVER *Snp,
+  UINT16     CableDetectFlag
+  );
+
+/**
+  this routine calls undi to shut down the interface.
+
+  @param  Snp   pointer to snp driver structure
+
+  @retval EFI_SUCCESS        UNDI is shut down successfully
+  @retval EFI_DEVICE_ERROR   UNDI could not be shut down
+
+**/
+EFI_STATUS
+PxeShutdown (
+  IN SNP_DRIVER *Snp
+  );
+
+/**
+  this routine calls undi to read the MAC address of the NIC and updates the
+  mode structure with the address.
+
+  @param  Snp         pointer to snp driver structure.
+
+  @retval EFI_SUCCESS       the MAC address of the NIC is read successfully.
+  @retval EFI_DEVICE_ERROR  failed to read the MAC address of the NIC.
+
+**/
+EFI_STATUS
+PxeGetStnAddr (
+  SNP_DRIVER *Snp
+  );
+
+/**
+  This is a callback routine supplied to UNDI3.1 at undi_start time.
+  UNDI call this routine when it wants to have exclusive access to a critical
+  section of the code/data.
+  New callbacks for 3.1:
+  there won't be a virtual2physical callback for UNDI 3.1 because undi3.1 uses
+  the MemMap call to map the required address by itself!
+
+  @param UniqueId  This was supplied to UNDI at Undi_Start, SNP uses this to
+                   store Undi interface context (Undi does not read or write
+                   this variable)
+  @param Enable    non-zero indicates acquire
+                   zero indicates release
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackBlock (
+  IN UINT64 UniqueId,
+  IN UINT32 Enable
+  );
+
+/**
+  This is a callback routine supplied to UNDI at undi_start time.
+  UNDI call this routine with the number of micro seconds when it wants to
+  pause.
+
+  @param UniqueId      This was supplied to UNDI at Undi_Start, SNP uses this to
+                       store Undi interface context (Undi does not read or write
+                       this variable)
+  @param MicroSeconds  number of micro seconds to pause, ususlly multiple of 10.
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackDelay (
+  IN UINT64 UniqueId,
+  IN UINT64 MicroSeconds
+  );
+
+/**
+  This is a callback routine supplied to UNDI at undi_start time.
+  This is the IO routine for UNDI3.1 to start CPB.
+
+  @param UniqueId       This was supplied to UNDI at Undi_Start, SNP uses this
+                        to store Undi interface context (Undi does not read or
+                        write this variable)
+  @param ReadOrWrite    indicates read or write, IO or Memory.
+  @param NumBytes       number of bytes to read or write.
+  @param MemOrPortAddr  IO or memory address to read from or write to.
+  @param BufferPtr      memory location to read into or that contains the bytes
+                        to write.
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackMemio (
+  IN UINT64     UniqueId,
+  IN UINT8      ReadOrWrite,
+  IN UINT8      NumBytes,
+  IN UINT64     MemOrPortAddr,
+  IN OUT UINT64 BufferPtr
+  );
+
+/**
+  This is a callback routine supplied to UNDI at undi_start time.
+  UNDI call this routine when it has to map a CPU address to a device
+  address.
+
+  @param UniqueId      - This was supplied to UNDI at Undi_Start, SNP uses this to store
+                         Undi interface context (Undi does not read or write this variable)
+  @param CpuAddr       - Virtual address to be mapped!
+  @param NumBytes      - size of memory to be mapped
+  @param Direction     - direction of data flow for this memory's usage:
+                         cpu->device, device->cpu or both ways
+  @param DeviceAddrPtr - pointer to return the mapped device address
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackMap (
+  IN UINT64     UniqueId,
+  IN UINT64     CpuAddr,
+  IN UINT32     NumBytes,
+  IN UINT32     Direction,
+  IN OUT UINT64 DeviceAddrPtr
+  );
+
+/**
+  This is a callback routine supplied to UNDI at undi_start time.
+  UNDI call this routine when it wants to unmap an address that was previously
+  mapped using map callback.
+
+  @param UniqueId    This was supplied to UNDI at Undi_Start, SNP uses this to store.
+                     Undi interface context (Undi does not read or write this variable)
+  @param CpuAddr     Virtual address that was mapped!
+  @param NumBytes    size of memory mapped
+  @param Direction   direction of data flow for this memory's usage:
+                     cpu->device, device->cpu or both ways
+  @param DeviceAddr  the mapped device address
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackUnmap (
+  IN UINT64             UniqueId,
+  IN UINT64             CpuAddr,
+  IN UINT32             NumBytes,
+  IN UINT32             Direction,
+  IN UINT64             DeviceAddr
+  );
+
+/**
+  This is a callback routine supplied to UNDI at undi_start time.
+  UNDI call this routine when it wants synchronize the virtual buffer contents
+  with the mapped buffer contents. The virtual and mapped buffers need not
+  correspond to the same physical memory (especially if the virtual address is
+  > 4GB). Depending on the direction for which the buffer is mapped, undi will
+  need to synchronize their contents whenever it writes to/reads from the buffer
+  using either the cpu address or the device address.
+
+  EFI does not provide a sync call, since virt=physical, we sould just do
+  the synchronization ourself here!
+
+  @param UniqueId    This was supplied to UNDI at Undi_Start, SNP uses this to store
+                     Undi interface context (Undi does not read or write this variable)
+  @param CpuAddr     Virtual address that was mapped!
+  @param NumBytes    size of memory mapped.
+  @param Direction   direction of data flow for this memory's usage:
+                     cpu->device, device->cpu or both ways.
+  @param DeviceAddr  the mapped device address.
+
+**/
+VOID
+EFIAPI
+SnpUndi32CallbackSync (
+  IN UINT64             UniqueId,
+  IN UINT64             CpuAddr,
+  IN UINT32             NumBytes,
+  IN UINT32             Direction,
+  IN UINT64             DeviceAddr
+  );
+
+/**
+  Changes the state of a network interface from "stopped" to "started".
+
+  This function starts a network interface. If the network interface successfully
+  starts, then EFI_SUCCESS will be returned.
+
+  @param  This                   A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS            The network interface was started.
+  @retval EFI_ALREADY_STARTED    The network interface is already in the started state.
+  @retval EFI_INVALID_PARAMETER  This parameter was NULL or did not point to a valid
+                                 EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR       The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED        This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Start (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  );
+
+/**
+  Changes the state of a network interface from "started" to "stopped".
+
+  This function stops a network interface. This call is only valid if the network
+  interface is in the started state. If the network interface was successfully
+  stopped, then EFI_SUCCESS will be returned.
+
+  @param  This                    A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+
+  @retval EFI_SUCCESS             The network interface was stopped.
+  @retval EFI_NOT_STARTED         The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER   This parameter was NULL or did not point to a valid
+                                  EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR        The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED         This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Stop (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  );
+
+/**
+  Resets a network adapter and allocates the transmit and receive buffers
+  required by the network interface; optionally, also requests allocation of
+  additional transmit and receive buffers.
+
+  This function allocates the transmit and receive buffers required by the network
+  interface. If this allocation fails, then EFI_OUT_OF_RESOURCES is returned.
+  If the allocation succeeds and the network interface is successfully initialized,
+  then EFI_SUCCESS will be returned.
+
+  @param This               A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @param ExtraRxBufferSize  The size, in bytes, of the extra receive buffer space
+                            that the driver should allocate for the network interface.
+                            Some network interfaces will not be able to use the
+                            extra buffer, and the caller will not know if it is
+                            actually being used.
+  @param ExtraTxBufferSize  The size, in bytes, of the extra transmit buffer space
+                            that the driver should allocate for the network interface.
+                            Some network interfaces will not be able to use the
+                            extra buffer, and the caller will not know if it is
+                            actually being used.
+
+  @retval EFI_SUCCESS           The network interface was initialized.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_OUT_OF_RESOURCES  There was not enough memory for the transmit and
+                                receive buffers.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       The increased buffer size feature is not supported.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Initialize (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINTN                       ExtraRxBufferSize OPTIONAL,
+  IN UINTN                       ExtraTxBufferSize OPTIONAL
+  );
+
+/**
+  Resets a network adapter and reinitializes it with the parameters that were
+  provided in the previous call to Initialize().
+
+  This function resets a network adapter and reinitializes it with the parameters
+  that were provided in the previous call to Initialize(). The transmit and
+  receive queues are emptied and all pending interrupts are cleared.
+  Receive filters, the station address, the statistics, and the multicast-IP-to-HW
+  MAC addresses are not reset by this call. If the network interface was
+  successfully reset, then EFI_SUCCESS will be returned. If the driver has not
+  been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This                 A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param ExtendedVerification Indicates that the driver may perform a more
+                              exhaustive verification operation of the device
+                              during reset.
+
+  @retval EFI_SUCCESS           The network interface was reset.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER One or more of the parameters has an unsupported value.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Reset (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL  *This,
+  IN BOOLEAN                      ExtendedVerification
+  );
+
+/**
+  Resets a network adapter and leaves it in a state that is safe for another
+  driver to initialize.
+
+  This function releases the memory buffers assigned in the Initialize() call.
+  Pending transmits and receives are lost, and interrupts are cleared and disabled.
+  After this call, only the Initialize() and Stop() calls may be used. If the
+  network interface was successfully shutdown, then EFI_SUCCESS will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param  This  A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The network interface was shutdown.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Shutdown (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  );
+
+/**
+  Manages the multicast receive filters of a network interface.
+
+  This function is used enable and disable the hardware and software receive
+  filters for the underlying network device.
+  The receive filter change is broken down into three steps:
+  * The filter mask bits that are set (ON) in the Enable parameter are added to
+    the current receive filter settings.
+  * The filter mask bits that are set (ON) in the Disable parameter are subtracted
+    from the updated receive filter settings.
+  * If the resulting receive filter setting is not supported by the hardware a
+    more liberal setting is selected.
+  If the same bits are set in the Enable and Disable parameters, then the bits
+  in the Disable parameter takes precedence.
+  If the ResetMCastFilter parameter is TRUE, then the multicast address list
+  filter is disabled (irregardless of what other multicast bits are set in the
+  Enable and Disable parameters). The SNP->Mode->MCastFilterCount field is set
+  to zero. The Snp->Mode->MCastFilter contents are undefined.
+  After enabling or disabling receive filter settings, software should verify
+  the new settings by checking the Snp->Mode->ReceiveFilterSettings,
+  Snp->Mode->MCastFilterCount and Snp->Mode->MCastFilter fields.
+  Note: Some network drivers and/or devices will automatically promote receive
+    filter settings if the requested setting can not be honored. For example, if
+    a request for four multicast addresses is made and the underlying hardware
+    only supports two multicast addresses the driver might set the promiscuous
+    or promiscuous multicast receive filters instead. The receiving software is
+    responsible for discarding any extra packets that get through the hardware
+    receive filters.
+    Note: Note: To disable all receive filter hardware, the network driver must
+      be Shutdown() and Stopped(). Calling ReceiveFilters() with Disable set to
+      Snp->Mode->ReceiveFilterSettings will make it so no more packets are
+      returned by the Receive() function, but the receive hardware may still be
+      moving packets into system memory before inspecting and discarding them.
+      Unexpected system errors, reboots and hangs can occur if an OS is loaded
+      and the network devices are not Shutdown() and Stopped().
+  If ResetMCastFilter is TRUE, then the multicast receive filter list on the
+  network interface will be reset to the default multicast receive filter list.
+  If ResetMCastFilter is FALSE, and this network interface allows the multicast
+  receive filter list to be modified, then the MCastFilterCnt and MCastFilter
+  are used to update the current multicast receive filter list. The modified
+  receive filter list settings can be found in the MCastFilter field of
+  EFI_SIMPLE_NETWORK_MODE. If the network interface does not allow the multicast
+  receive filter list to be modified, then EFI_INVALID_PARAMETER will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+  If the receive filter mask and multicast receive filter list have been
+  successfully updated on the network interface, EFI_SUCCESS will be returned.
+
+  @param This             A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Enable           A bit mask of receive filters to enable on the network
+                          interface.
+  @param Disable          A bit mask of receive filters to disable on the network
+                          interface. For backward compatibility with EFI 1.1
+                          platforms, the EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST bit
+                          must be set when the ResetMCastFilter parameter is TRUE.
+  @param ResetMCastFilter Set to TRUE to reset the contents of the multicast
+                          receive filters on the network interface to their
+                          default values.
+  @param MCastFilterCnt   Number of multicast HW MAC addresses in the new MCastFilter
+                          list. This value must be less than or equal to the
+                          MCastFilterCnt field of EFI_SIMPLE_NETWORK_MODE.
+                          This field is optional if ResetMCastFilter is TRUE.
+  @param MCastFilter      A pointer to a list of new multicast receive filter HW
+                          MAC addresses. This list will replace any existing
+                          multicast HW MAC address list. This field is optional
+                          if ResetMCastFilter is TRUE.
+
+  @retval EFI_SUCCESS            The multicast receive filter list was updated.
+  @retval EFI_NOT_STARTED        The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER  One or more of the following conditions is TRUE:
+                                 * This is NULL
+                                 * There are bits set in Enable that are not set
+                                   in Snp->Mode->ReceiveFilterMask
+                                 * There are bits set in Disable that are not set
+                                   in Snp->Mode->ReceiveFilterMask
+                                 * Multicast is being enabled (the
+                                   EFI_SIMPLE_NETWORK_RECEIVE_MULTICAST bit is
+                                   set in Enable, it is not set in Disable, and
+                                   ResetMCastFilter is FALSE) and MCastFilterCount
+                                   is zero
+                                 * Multicast is being enabled and MCastFilterCount
+                                   is greater than Snp->Mode->MaxMCastFilterCount
+                                 * Multicast is being enabled and MCastFilter is NULL
+                                 * Multicast is being enabled and one or more of
+                                   the addresses in the MCastFilter list are not
+                                   valid multicast MAC addresses
+  @retval EFI_DEVICE_ERROR       One or more of the following conditions is TRUE:
+                                 * The network interface has been started but has
+                                   not been initialized
+                                 * An unexpected error was returned by the
+                                   underlying network driver or device
+  @retval EFI_UNSUPPORTED        This function is not supported by the network
+                                 interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32ReceiveFilters (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINT32                      Enable,
+  IN UINT32                      Disable,
+  IN BOOLEAN                     ResetMCastFilter,
+  IN UINTN                       MCastFilterCnt,  OPTIONAL
+  IN EFI_MAC_ADDRESS             *MCastFilter     OPTIONAL
+  );
+
+/**
+  Modifies or resets the current station address, if supported.
+
+  This function modifies or resets the current station address of a network
+  interface, if supported. If Reset is TRUE, then the current station address is
+  set to the network interface's permanent address. If Reset is FALSE, and the
+  network interface allows its station address to be modified, then the current
+  station address is changed to the address specified by New. If the network
+  interface does not allow its station address to be modified, then
+  EFI_INVALID_PARAMETER will be returned. If the station address is successfully
+  updated on the network interface, EFI_SUCCESS will be returned. If the driver
+  has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This  A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Reset Flag used to reset the station address to the network interface's
+               permanent address.
+  @param New   New station address to be used for the network interface.
+
+
+  @retval EFI_SUCCESS           The network interface's station address was updated.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not been
+                                started by calling Start().
+  @retval EFI_INVALID_PARAMETER The New station address was not accepted by the NIC.
+  @retval EFI_INVALID_PARAMETER Reset is FALSE and New is NULL.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not
+                                been initialized by calling Initialize().
+  @retval EFI_DEVICE_ERROR      An error occurred attempting to set the new
+                                station address.
+  @retval EFI_UNSUPPORTED       The NIC does not support changing the network
+                                interface's station address.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32StationAddress (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     Reset,
+  IN EFI_MAC_ADDRESS             *New  OPTIONAL
+  );
+
+/**
+  Resets or collects the statistics on a network interface.
+
+  This function resets or collects the statistics on a network interface. If the
+  size of the statistics table specified by StatisticsSize is not big enough for
+  all the statistics that are collected by the network interface, then a partial
+  buffer of statistics is returned in StatisticsTable, StatisticsSize is set to
+  the size required to collect all the available statistics, and
+  EFI_BUFFER_TOO_SMALL is returned.
+  If StatisticsSize is big enough for all the statistics, then StatisticsTable
+  will be filled, StatisticsSize will be set to the size of the returned
+  StatisticsTable structure, and EFI_SUCCESS is returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+  If Reset is FALSE, and both StatisticsSize and StatisticsTable are NULL, then
+  no operations will be performed, and EFI_SUCCESS will be returned.
+  If Reset is TRUE, then all of the supported statistics counters on this network
+  interface will be reset to zero.
+
+  @param This            A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Reset           Set to TRUE to reset the statistics for the network interface.
+  @param StatisticsSize  On input the size, in bytes, of StatisticsTable. On output
+                         the size, in bytes, of the resulting table of statistics.
+  @param StatisticsTable A pointer to the EFI_NETWORK_STATISTICS structure that
+                         contains the statistics. Type EFI_NETWORK_STATISTICS is
+                         defined in "Related Definitions" below.
+
+  @retval EFI_SUCCESS           The requested operation succeeded.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not been
+                                started by calling Start().
+  @retval EFI_BUFFER_TOO_SMALL  StatisticsSize is not NULL and StatisticsTable is
+                                NULL. The current buffer size that is needed to
+                                hold all the statistics is returned in StatisticsSize.
+  @retval EFI_BUFFER_TOO_SMALL  StatisticsSize is not NULL and StatisticsTable is
+                                not NULL. The current buffer size that is needed
+                                to hold all the statistics is returned in
+                                StatisticsSize. A partial set of statistics is
+                                returned in StatisticsTable.
+  @retval EFI_INVALID_PARAMETER StatisticsSize is NULL and StatisticsTable is not
+                                NULL.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not
+                                been initialized by calling Initialize().
+  @retval EFI_DEVICE_ERROR      An error was encountered collecting statistics
+                                from the NIC.
+  @retval EFI_UNSUPPORTED       The NIC does not support collecting statistics
+                                from the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Statistics (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     Reset,
+  IN OUT UINTN                   *StatisticsSize,  OPTIONAL
+  IN OUT EFI_NETWORK_STATISTICS  *StatisticsTable  OPTIONAL
+  );
+
+/**
+  Converts a multicast IP address to a multicast HW MAC address.
+
+  This function converts a multicast IP address to a multicast HW MAC address
+  for all packet transactions. If the mapping is accepted, then EFI_SUCCESS will
+  be returned.
+
+  @param This A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param IPv6 Set to TRUE if the multicast IP address is IPv6 [RFC 2460].
+              Set to FALSE if the multicast IP address is IPv4 [RFC 791].
+  @param IP   The multicast IP address that is to be converted to a multicast
+              HW MAC address.
+  @param MAC  The multicast HW MAC address that is to be generated from IP.
+
+  @retval EFI_SUCCESS           The multicast IP address was mapped to the
+                                multicast HW MAC address.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not
+                                been started by calling Start().
+  @retval EFI_INVALID_PARAMETER IP is NULL.
+  @retval EFI_INVALID_PARAMETER MAC is NULL.
+  @retval EFI_INVALID_PARAMETER IP does not point to a valid IPv4 or IPv6
+                                multicast address.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not
+                                been initialized by calling Initialize().
+  @retval EFI_UNSUPPORTED       IPv6 is TRUE and the implementation does not
+                                support IPv6 multicast to MAC address conversion.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32McastIpToMac (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     IPv6,
+  IN EFI_IP_ADDRESS              *IP,
+  OUT EFI_MAC_ADDRESS            *MAC
+  );
+
+/**
+  Performs read and write operations on the NVRAM device attached to a network
+  interface.
+
+  This function performs read and write operations on the NVRAM device attached
+  to a network interface. If ReadWrite is TRUE, a read operation is performed.
+  If ReadWrite is FALSE, a write operation is performed. Offset specifies the
+  byte offset at which to start either operation. Offset must be a multiple of
+  NvRamAccessSize , and it must have a value between zero and NvRamSize.
+  BufferSize specifies the length of the read or write operation. BufferSize must
+  also be a multiple of NvRamAccessSize, and Offset + BufferSize must not exceed
+  NvRamSize.
+  If any of the above conditions is not met, then EFI_INVALID_PARAMETER will be
+  returned.
+  If all the conditions are met and the operation is "read," the NVRAM device
+  attached to the network interface will be read into Buffer and EFI_SUCCESS
+  will be returned. If this is a write operation, the contents of Buffer will be
+  used to update the contents of the NVRAM device attached to the network
+  interface and EFI_SUCCESS will be returned.
+
+  It does the basic checking on the input parameters and retrieves snp structure
+  and then calls the read_nvdata() call which does the actual reading
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param ReadWrite  TRUE for read operations, FALSE for write operations.
+  @param Offset     Byte offset in the NVRAM device at which to start the read or
+                    write operation. This must be a multiple of NvRamAccessSize
+                    and less than NvRamSize. (See EFI_SIMPLE_NETWORK_MODE)
+  @param BufferSize The number of bytes to read or write from the NVRAM device.
+                    This must also be a multiple of NvramAccessSize.
+  @param Buffer     A pointer to the data buffer.
+
+  @retval EFI_SUCCESS           The NVRAM access was performed.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * The This parameter is NULL
+                                * The This parameter does not point to a valid
+                                  EFI_SIMPLE_NETWORK_PROTOCOL  structure
+                                * The Offset parameter is not a multiple of
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamAccessSize
+                                * The Offset parameter is not less than
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamSize
+                                * The BufferSize parameter is not a multiple of
+                                  EFI_SIMPLE_NETWORK_MODE.NvRamAccessSize
+                                * The Buffer parameter is NULL
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network
+                                interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network
+                                interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32NvData (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     ReadWrite,
+  IN UINTN                       Offset,
+  IN UINTN                       BufferSize,
+  IN OUT VOID                    *Buffer
+  );
+
+/**
+  Reads the current interrupt status and recycled transmit buffer status from a
+  network interface.
+
+  This function gets the current interrupt and recycled transmit buffer status
+  from the network interface. The interrupt status is returned as a bit mask in
+  InterruptStatus. If InterruptStatus is NULL, the interrupt status will not be
+  read. If TxBuf is not NULL, a recycled transmit buffer address will be retrieved.
+  If a recycled transmit buffer address is returned in TxBuf, then the buffer has
+  been successfully transmitted, and the status for that buffer is cleared. If
+  the status of the network interface is successfully collected, EFI_SUCCESS
+  will be returned. If the driver has not been initialized, EFI_DEVICE_ERROR will
+  be returned.
+
+  @param This            A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param InterruptStatus A pointer to the bit mask of the currently active
+                         interrupts (see "Related Definitions"). If this is NULL,
+                         the interrupt status will not be read from the device.
+                         If this is not NULL, the interrupt status will be read
+                         from the device. When the interrupt status is read, it
+                         will also be cleared. Clearing the transmit interrupt does
+                         not empty the recycled transmit buffer array.
+  @param TxBuf           Recycled transmit buffer address. The network interface
+                         will not transmit if its internal recycled transmit
+                         buffer array is full. Reading the transmit buffer does
+                         not clear the transmit interrupt. If this is NULL, then
+                         the transmit buffer status will not be read. If there
+                         are no transmit buffers to recycle and TxBuf is not NULL,
+                         TxBuf will be set to NULL.
+
+  @retval EFI_SUCCESS           The status of the network interface was retrieved.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER This parameter was NULL or did not point to a valid
+                                EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network
+                                interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32GetStatus (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  OUT UINT32                     *InterruptStatus, OPTIONAL
+  OUT VOID                       **TxBuf           OPTIONAL
+  );
+
+/**
+  Places a packet in the transmit queue of a network interface.
+
+  This function places the packet specified by Header and Buffer on the transmit
+  queue. If HeaderSize is nonzero and HeaderSize is not equal to
+  This->Mode->MediaHeaderSize, then EFI_INVALID_PARAMETER will be returned. If
+  BufferSize is less than This->Mode->MediaHeaderSize, then EFI_BUFFER_TOO_SMALL
+  will be returned. If Buffer is NULL, then EFI_INVALID_PARAMETER will be
+  returned. If HeaderSize is nonzero and DestAddr or Protocol is NULL, then
+  EFI_INVALID_PARAMETER will be returned. If the transmit engine of the network
+  interface is busy, then EFI_NOT_READY will be returned. If this packet can be
+  accepted by the transmit engine of the network interface, the packet contents
+  specified by Buffer will be placed on the transmit queue of the network
+  interface, and EFI_SUCCESS will be returned. GetStatus() can be used to
+  determine when the packet has actually been transmitted. The contents of the
+  Buffer must not be modified until the packet has actually been transmitted.
+  The Transmit() function performs nonblocking I/O. A caller who wants to perform
+  blocking I/O, should call Transmit(), and then GetStatus() until the
+  transmitted buffer shows up in the recycled transmit buffer.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param HeaderSize The size, in bytes, of the media header to be filled in by the
+                    Transmit() function. If HeaderSize is nonzero, then it must
+                    be equal to This->Mode->MediaHeaderSize and the DestAddr and
+                    Protocol parameters must not be NULL.
+  @param BufferSize The size, in bytes, of the entire packet (media header and
+                    data) to be transmitted through the network interface.
+  @param Buffer     A pointer to the packet (media header followed by data) to be
+                    transmitted. This parameter cannot be NULL. If HeaderSize is
+                    zero, then the media header in Buffer must already be filled
+                    in by the caller. If HeaderSize is nonzero, then the media
+                    header will be filled in by the Transmit() function.
+  @param SrcAddr    The source HW MAC address. If HeaderSize is zero, then this
+                    parameter is ignored. If HeaderSize is nonzero and SrcAddr
+                    is NULL, then This->Mode->CurrentAddress is used for the
+                    source HW MAC address.
+  @param DestAddr   The destination HW MAC address. If HeaderSize is zero, then
+                    this parameter is ignored.
+  @param Protocol   The type of header to build. If HeaderSize is zero, then this
+                    parameter is ignored. See RFC 1700, section "Ether Types,"
+                    for examples.
+
+  @retval EFI_SUCCESS           The packet was placed on the transmit queue.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_NOT_READY         The network interface is too busy to accept this
+                                transmit request.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize parameter is too small.
+  @retval EFI_INVALID_PARAMETER One or more of the parameters has an unsupported
+                                value.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Transmit (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINTN                       HeaderSize,
+  IN UINTN                       BufferSize,
+  IN VOID                        *Buffer,
+  IN EFI_MAC_ADDRESS             *SrcAddr,  OPTIONAL
+  IN EFI_MAC_ADDRESS             *DestAddr, OPTIONAL
+  IN UINT16                      *Protocol  OPTIONAL
+  );
+
+/**
+  Receives a packet from a network interface.
+
+  This function retrieves one packet from the receive queue of a network interface.
+  If there are no packets on the receive queue, then EFI_NOT_READY will be
+  returned. If there is a packet on the receive queue, and the size of the packet
+  is smaller than BufferSize, then the contents of the packet will be placed in
+  Buffer, and BufferSize will be updated with the actual size of the packet.
+  In addition, if SrcAddr, DestAddr, and Protocol are not NULL, then these values
+  will be extracted from the media header and returned. EFI_SUCCESS will be
+  returned if a packet was successfully received.
+  If BufferSize is smaller than the received packet, then the size of the receive
+  packet will be placed in BufferSize and EFI_BUFFER_TOO_SMALL will be returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param HeaderSize The size, in bytes, of the media header received on the network
+                    interface. If this parameter is NULL, then the media header size
+                    will not be returned.
+  @param BufferSize On entry, the size, in bytes, of Buffer. On exit, the size, in
+                    bytes, of the packet that was received on the network interface.
+  @param Buffer     A pointer to the data buffer to receive both the media
+                    header and the data.
+  @param SrcAddr    The source HW MAC address. If this parameter is NULL, the HW
+                    MAC source address will not be extracted from the media header.
+  @param DestAddr   The destination HW MAC address. If this parameter is NULL,
+                    the HW MAC destination address will not be extracted from
+                    the media header.
+  @param Protocol   The media header type. If this parameter is NULL, then the
+                    protocol will not be extracted from the media header. See
+                    RFC 1700 section "Ether Types" for examples.
+
+  @retval EFI_SUCCESS           The received data was stored in Buffer, and
+                                BufferSize has been updated to the number of
+                                bytes received.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_NOT_READY         No packets have been received on the network interface.
+  @retval EFI_BUFFER_TOO_SMALL  BufferSize is too small for the received packets.
+                                BufferSize has been updated to the required size.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                * The This parameter is NULL
+                                * The This parameter does not point to a valid
+                                  EFI_SIMPLE_NETWORK_PROTOCOL structure.
+                                * The BufferSize parameter is NULL
+                                * The Buffer parameter is NULL
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Receive (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  OUT UINTN                      *HeaderSize OPTIONAL,
+  IN OUT UINTN                   *BufferSize,
+  OUT VOID                       *Buffer,
+  OUT EFI_MAC_ADDRESS            *SrcAddr OPTIONAL,
+  OUT EFI_MAC_ADDRESS            *DestAddr OPTIONAL,
+  OUT UINT16                     *Protocol OPTIONAL
+  );
+
+/**
+  Nofication call back function for WaitForPacket event.
+
+  @param  Event       EFI Event.
+  @param  SnpPtr      Pointer to SNP_DRIVER structure.
+
+**/
+VOID
+EFIAPI
+SnpWaitForPacketNotify (
+  EFI_EVENT Event,
+  VOID      *SnpPtr
+  );
+
+#define SNP_MEM_PAGES(x)  (((x) - 1) / 4096 + 1)
+
+
+#endif /*  _SNP_H_  */
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.inf b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.inf
new file mode 100644
index 0000000000..90fd8d5475
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.inf
@@ -0,0 +1,83 @@
+## @file
+#  This module produces EFI SNP Protocol.
+#
+#  This module produces Simple Network Protocol upon EFI Network Interface 
+#  Identifier Protocol, to provide a packet level interface to a network adapter.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SnpDxe
+  MODULE_UNI_FILE                = SnpDxe.uni
+  FILE_GUID                      = A2f436EA-A127-4EF8-957C-8048606FF670
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializeSnpNiiDriver
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  mSimpleNetworkDriverBinding                        
+#  COMPONENT_NAME                =  gSimpleNetworkComponentName
+#  COMPONENT_NAME2               =  gSimpleNetworkComponentName2
+#
+
+[Sources]
+  Receive.c
+  Snp.h
+  Nvdata.c
+  Get_status.c
+  Start.c
+  Snp.c
+  Stop.c
+  Statistics.c
+  Reset.c
+  Shutdown.c
+  Mcast_ip_to_mac.c
+  Transmit.c
+  WaitForPacket.c
+  Receive_filters.c
+  Initialize.c
+  ComponentName.c
+  Callback.c
+  Station_address.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UefiLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  DebugLib
+  NetLib
+
+[Guids]
+  gEfiEventExitBootServicesGuid                 ## SOMETIMES_CONSUMES ## Event
+
+[Protocols]
+  gEfiSimpleNetworkProtocolGuid                 ## BY_START
+  gEfiDevicePathProtocolGuid                    ## TO_START
+  gEfiNetworkInterfaceIdentifierProtocolGuid_31 ## TO_START
+  gEfiPciIoProtocolGuid                         ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SnpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.uni b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.uni
new file mode 100644
index 0000000000..5d45e3a7a4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxeExtra.uni
new file mode 100644
index 0000000000..aec08b1811
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/SnpDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Start.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Start.c
new file mode 100644
index 0000000000..210f081078
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Start.c
@@ -0,0 +1,168 @@
+/** @file
+    Implementation of starting a network adapter.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+
+/**
+  Call UNDI to start the interface and changes the snp state.
+
+  @param  Snp                    pointer to snp driver structure.
+
+  @retval EFI_SUCCESS            UNDI is started successfully.
+  @retval EFI_DEVICE_ERROR       UNDI could not be started.
+  
+**/
+EFI_STATUS
+PxeStart (
+  IN SNP_DRIVER *Snp
+  )
+{
+  PXE_CPB_START_31  *Cpb31;
+
+  Cpb31  = Snp->Cpb;
+  //
+  // Initialize UNDI Start CDB for H/W UNDI
+  //
+  Snp->Cdb.OpCode     = PXE_OPCODE_START;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Make changes to H/W UNDI Start CDB if this is
+  // a S/W UNDI.
+  //
+  if (Snp->IsSwUndi) {
+    Snp->Cdb.CPBsize  = (UINT16) sizeof (PXE_CPB_START_31);
+    Snp->Cdb.CPBaddr  = (UINT64)(UINTN) Cpb31;
+
+    Cpb31->Delay     = (UINT64)(UINTN) &SnpUndi32CallbackDelay;
+    Cpb31->Block     = (UINT64)(UINTN) &SnpUndi32CallbackBlock;
+
+    //
+    // Virtual == Physical.  This can be set to zero.
+    //
+    Cpb31->Virt2Phys = (UINT64)(UINTN) 0;
+    Cpb31->Mem_IO    = (UINT64)(UINTN) &SnpUndi32CallbackMemio;
+
+    Cpb31->Map_Mem   = (UINT64)(UINTN) &SnpUndi32CallbackMap;
+    Cpb31->UnMap_Mem = (UINT64)(UINTN) &SnpUndi32CallbackUnmap;
+    Cpb31->Sync_Mem  = (UINT64)(UINTN) &SnpUndi32CallbackSync;
+
+    Cpb31->Unique_ID = (UINT64)(UINTN) Snp;
+  }
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.start()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    //
+    // UNDI could not be started. Return UNDI error.
+    //
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.start()  %xh:%xh\n",
+      Snp->Cdb.StatCode,
+      Snp->Cdb.StatFlags)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Set simple network state to Started and return success.
+  //
+  Snp->Mode.State = EfiSimpleNetworkStarted;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Change the state of a network interface from "stopped" to "started."
+  
+  This function starts a network interface. If the network interface successfully
+  starts, then EFI_SUCCESS will be returned.
+
+  @param  This                   A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+
+  @retval EFI_SUCCESS            The network interface was started.
+  @retval EFI_ALREADY_STARTED    The network interface is already in the started state.
+  @retval EFI_INVALID_PARAMETER  This parameter was NULL or did not point to a valid 
+                                 EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR       The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED        This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Start (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_STATUS  Status;
+  UINTN       Index;
+  EFI_TPL     OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkStopped:
+    break;
+
+  case EfiSimpleNetworkStarted:
+  case EfiSimpleNetworkInitialized:
+    Status = EFI_ALREADY_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = PxeStart (Snp);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+  //
+  // clear the map_list in SNP structure
+  //
+  for (Index = 0; Index < MAX_MAP_LENGTH; Index++) {
+    Snp->MapList[Index].VirtualAddress = 0;
+    Snp->MapList[Index].MapCookie      = 0;
+  }
+
+  Snp->Mode.MCastFilterCount = 0;
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Station_address.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Station_address.c
new file mode 100644
index 0000000000..0917baf674
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Station_address.c
@@ -0,0 +1,249 @@
+/** @file
+    Implementation of reading the MAC address of a network adapter.
+ 
+Copyright (c) 2004 - 2014, 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 "Snp.h"
+
+
+/**
+  Call UNDI to read the MAC address of the NIC and update the mode structure 
+  with the address. 
+
+  @param  Snp         Pointer to snp driver structure.
+   
+  @retval EFI_SUCCESS       The MAC address of the NIC is read successfully.
+  @retval EFI_DEVICE_ERROR  Failed to read the MAC address of the NIC.
+
+**/
+EFI_STATUS
+PxeGetStnAddr (
+  SNP_DRIVER *Snp
+  )
+{
+  PXE_DB_STATION_ADDRESS  *Db;
+
+  Db                  = Snp->Db;
+  Snp->Cdb.OpCode     = PXE_OPCODE_STATION_ADDRESS;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_STATION_ADDRESS_READ;
+
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_STATION_ADDRESS);
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.station_addr()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.station_addr()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Set new station address in SNP->Mode structure and return success.
+  //
+  CopyMem (
+    &(Snp->Mode.CurrentAddress),
+    &Db->StationAddr,
+    Snp->Mode.HwAddressSize
+    );
+
+  CopyMem (
+    &Snp->Mode.BroadcastAddress,
+    &Db->BroadcastAddr,
+    Snp->Mode.HwAddressSize
+    );
+
+  CopyMem (
+    &Snp->Mode.PermanentAddress,
+    &Db->PermanentAddr,
+    Snp->Mode.HwAddressSize
+    );
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Call UNDI to set a new MAC address for the NIC.
+
+  @param  Snp         Pointer to Snp driver structure.
+  @param  NewMacAddr  Pointer to a MAC address to be set for the NIC, if this is
+                      NULL then this routine resets the mac address to the NIC's
+                      original address.
+
+
+**/
+EFI_STATUS
+PxeSetStnAddr (
+  SNP_DRIVER      *Snp,
+  EFI_MAC_ADDRESS *NewMacAddr
+  )
+{
+  PXE_CPB_STATION_ADDRESS *Cpb;
+  PXE_DB_STATION_ADDRESS  *Db;
+
+  Cpb             = Snp->Cpb;
+  Db              = Snp->Db;
+  Snp->Cdb.OpCode = PXE_OPCODE_STATION_ADDRESS;
+
+  if (NewMacAddr == NULL) {
+    Snp->Cdb.OpFlags  = PXE_OPFLAGS_STATION_ADDRESS_RESET;
+    Snp->Cdb.CPBsize  = PXE_CPBSIZE_NOT_USED;
+    Snp->Cdb.CPBaddr  = PXE_CPBADDR_NOT_USED;
+  } else {
+    Snp->Cdb.OpFlags = PXE_OPFLAGS_STATION_ADDRESS_WRITE;
+    //
+    // Supplying a new address in the CPB will make undi change the mac address to the new one.
+    //
+    CopyMem (&Cpb->StationAddr, NewMacAddr, Snp->Mode.HwAddressSize);
+
+    Snp->Cdb.CPBsize  = (UINT16) sizeof (PXE_CPB_STATION_ADDRESS);
+    Snp->Cdb.CPBaddr  = (UINT64)(UINTN) Cpb;
+  }
+
+  Snp->Cdb.DBsize     = (UINT16) sizeof (PXE_DB_STATION_ADDRESS);
+  Snp->Cdb.DBaddr     = (UINT64)(UINTN) Db;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.station_addr()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.station_addr()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    //
+    // UNDI command failed.  Return UNDI status to caller.
+    //
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // read the changed address and save it in SNP->Mode structure
+  //
+  PxeGetStnAddr (Snp);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Modifies or resets the current station address, if supported.
+  
+  This function modifies or resets the current station address of a network 
+  interface, if supported. If Reset is TRUE, then the current station address is
+  set to the network interface's permanent address. If Reset is FALSE, and the 
+  network interface allows its station address to be modified, then the current 
+  station address is changed to the address specified by New. If the network 
+  interface does not allow its station address to be modified, then 
+  EFI_INVALID_PARAMETER will be returned. If the station address is successfully
+  updated on the network interface, EFI_SUCCESS will be returned. If the driver
+  has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This  A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Reset Flag used to reset the station address to the network interface's 
+               permanent address.
+  @param New   New station address to be used for the network interface.
+
+
+  @retval EFI_SUCCESS           The network interface's station address was updated.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not been 
+                                started by calling Start().
+  @retval EFI_INVALID_PARAMETER The New station address was not accepted by the NIC.
+  @retval EFI_INVALID_PARAMETER Reset is FALSE and New is NULL.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not 
+                                been initialized by calling Initialize().
+  @retval EFI_DEVICE_ERROR      An error occurred attempting to set the new 
+                                station address.
+  @retval EFI_UNSUPPORTED       The NIC does not support changing the network 
+                                interface's station address.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32StationAddress (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     Reset,
+  IN EFI_MAC_ADDRESS             *New OPTIONAL
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_STATUS  Status;
+  EFI_TPL     OldTpl;
+
+  //
+  // Check for invalid parameter combinations.
+  //
+  if ((This == NULL) ||
+    (!Reset && (New == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Return error if the SNP is not initialized.
+  //
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  if (Reset) {
+    Status = PxeSetStnAddr (Snp, NULL);
+  } else {
+    Status = PxeSetStnAddr (Snp, New);
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Statistics.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Statistics.c
new file mode 100644
index 0000000000..3e564991e3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Statistics.c
@@ -0,0 +1,230 @@
+/** @file
+    Implementation of collecting the statistics on a network interface.
+
+Copyright (c) 2004 - 2010, 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 "Snp.h"
+
+
+/**
+  Resets or collects the statistics on a network interface.
+
+  This function resets or collects the statistics on a network interface. If the
+  size of the statistics table specified by StatisticsSize is not big enough for
+  all the statistics that are collected by the network interface, then a partial
+  buffer of statistics is returned in StatisticsTable, StatisticsSize is set to
+  the size required to collect all the available statistics, and
+  EFI_BUFFER_TOO_SMALL is returned.
+  If StatisticsSize is big enough for all the statistics, then StatisticsTable
+  will be filled, StatisticsSize will be set to the size of the returned
+  StatisticsTable structure, and EFI_SUCCESS is returned.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+  If Reset is FALSE, and both StatisticsSize and StatisticsTable are NULL, then
+  no operations will be performed, and EFI_SUCCESS will be returned.
+  If Reset is TRUE, then all of the supported statistics counters on this network
+  interface will be reset to zero.
+
+  @param This            A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param Reset           Set to TRUE to reset the statistics for the network interface.
+  @param StatisticsSize  On input the size, in bytes, of StatisticsTable. On output
+                         the size, in bytes, of the resulting table of statistics.
+  @param StatisticsTable A pointer to the EFI_NETWORK_STATISTICS structure that
+                         contains the statistics. Type EFI_NETWORK_STATISTICS is
+                         defined in "Related Definitions" below.
+
+  @retval EFI_SUCCESS           The requested operation succeeded.
+  @retval EFI_NOT_STARTED       The Simple Network Protocol interface has not been
+                                started by calling Start().
+  @retval EFI_BUFFER_TOO_SMALL  StatisticsSize is not NULL and StatisticsTable is
+                                NULL. The current buffer size that is needed to
+                                hold all the statistics is returned in StatisticsSize.
+  @retval EFI_BUFFER_TOO_SMALL  StatisticsSize is not NULL and StatisticsTable is
+                                not NULL. The current buffer size that is needed
+                                to hold all the statistics is returned in
+                                StatisticsSize. A partial set of statistics is
+                                returned in StatisticsTable.
+  @retval EFI_INVALID_PARAMETER StatisticsSize is NULL and StatisticsTable is not
+                                NULL.
+  @retval EFI_DEVICE_ERROR      The Simple Network Protocol interface has not
+                                been initialized by calling Initialize().
+  @retval EFI_DEVICE_ERROR      An error was encountered collecting statistics
+                                from the NIC.
+  @retval EFI_UNSUPPORTED       The NIC does not support collecting statistics
+                                from the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Statistics (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN BOOLEAN                     Reset,
+  IN OUT UINTN                   *StatisticsSize, OPTIONAL
+  IN OUT EFI_NETWORK_STATISTICS  *StatisticsTable OPTIONAL
+  )
+{
+  SNP_DRIVER        *Snp;
+  PXE_DB_STATISTICS *Db;
+  UINT64            *Stp;
+  UINT64            Mask;
+  UINTN             Size;
+  UINTN             Index;
+  EFI_TPL           OldTpl;
+  EFI_STATUS        Status;
+
+  //
+  // Get pointer to SNP driver instance for *This.
+  //
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Return error if the SNP is not initialized.
+  //
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+  //
+  // if we are not resetting the counters, we have to have a valid stat table
+  // with >0 size. if no reset, no table and no size, return success.
+  //
+  if (!Reset && StatisticsSize == NULL) {
+    Status = (StatisticsTable != NULL) ? EFI_INVALID_PARAMETER : EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+  //
+  // Initialize UNDI Statistics CDB
+  //
+  Snp->Cdb.OpCode     = PXE_OPCODE_STATISTICS;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  if (Reset) {
+    Snp->Cdb.OpFlags  = PXE_OPFLAGS_STATISTICS_RESET;
+    Snp->Cdb.DBsize   = PXE_DBSIZE_NOT_USED;
+    Snp->Cdb.DBaddr   = PXE_DBADDR_NOT_USED;
+    Db                = Snp->Db;
+  } else {
+    Snp->Cdb.OpFlags                = PXE_OPFLAGS_STATISTICS_READ;
+    Snp->Cdb.DBsize                 = (UINT16) sizeof (PXE_DB_STATISTICS);
+    Snp->Cdb.DBaddr                 = (UINT64)(UINTN) (Db = Snp->Db);
+  }
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.statistics()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    break;
+
+  case PXE_STATCODE_UNSUPPORTED:
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.statistics()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    Status = EFI_UNSUPPORTED;
+    goto ON_EXIT;
+
+  default:
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nsnp->undi.statistics()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  if (Reset) {
+    Status = EFI_SUCCESS;
+    goto ON_EXIT;
+  }
+
+  if (StatisticsTable == NULL) {
+    *StatisticsSize = sizeof (EFI_NETWORK_STATISTICS);
+    Status = EFI_BUFFER_TOO_SMALL;
+    goto ON_EXIT;
+  }
+  //
+  // Convert the UNDI statistics information to SNP statistics
+  // information.
+  //
+  ZeroMem (StatisticsTable, *StatisticsSize);
+  Stp   = (UINT64 *) StatisticsTable;
+  Size  = 0;
+
+  for (Index = 0, Mask = 1; Index < 64; Index++, Mask = LShiftU64 (Mask, 1), Stp++) {
+    //
+    // There must be room for a full UINT64.  Partial
+    // numbers will not be stored.
+    //
+    if ((Index + 1) * sizeof (UINT64) > *StatisticsSize) {
+      break;
+    }
+
+    if ((Db->Supported & Mask) != 0) {
+      *Stp  = Db->Data[Index];
+      Size  = Index + 1;
+    } else {
+      SetMem (Stp, sizeof (UINT64), 0xFF);
+    }
+  }
+  //
+  // Compute size up to last supported statistic.
+  //
+  while (++Index < 64) {
+    if ((Db->Supported & (Mask = LShiftU64 (Mask, 1))) != 0) {
+      Size = Index;
+    }
+  }
+
+  Size *= sizeof (UINT64);
+
+  if (*StatisticsSize >= Size) {
+    *StatisticsSize = Size;
+    Status = EFI_SUCCESS;
+  } else {
+    *StatisticsSize = Size;
+    Status = EFI_BUFFER_TOO_SMALL;
+  }
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Stop.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Stop.c
new file mode 100644
index 0000000000..730e397452
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Stop.c
@@ -0,0 +1,126 @@
+/** @file
+    Implementation of stopping a network interface.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+
+/**
+  Call UNDI to stop the interface and changes the snp state.
+
+  @param  Snp   Pointer to snp driver structure
+   
+  @retval EFI_SUCCESS            The network interface was stopped.
+  @retval EFI_DEVICE_ERROR       SNP is not initialized.
+
+**/
+EFI_STATUS
+PxeStop (
+  SNP_DRIVER *Snp
+  )
+{
+  Snp->Cdb.OpCode     = PXE_OPCODE_STOP;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_NOT_USED;
+  Snp->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command
+  //
+  DEBUG ((EFI_D_NET, "\nsnp->undi.stop()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64)(UINTN) &Snp->Cdb);
+
+  if (Snp->Cdb.StatCode != PXE_STATCODE_SUCCESS) {
+    DEBUG (
+      (EFI_D_WARN,
+      "\nsnp->undi.stop()  %xh:%xh\n",
+      Snp->Cdb.StatCode,
+      Snp->Cdb.StatFlags)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+  //
+  // Set simple network state to Started and return success.
+  //
+  Snp->Mode.State = EfiSimpleNetworkStopped;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Changes the state of a network interface from "started" to "stopped."
+  
+  This function stops a network interface. This call is only valid if the network
+  interface is in the started state. If the network interface was successfully
+  stopped, then EFI_SUCCESS will be returned.
+  
+  @param  This                    A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL 
+                                  instance.
+  
+  
+  @retval EFI_SUCCESS             The network interface was stopped.
+  @retval EFI_NOT_STARTED         The network interface has not been started.
+  @retval EFI_INVALID_PARAMETER   This parameter was NULL or did not point to a 
+                                  valid EFI_SIMPLE_NETWORK_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR        The command could not be sent to the network 
+                                  interface.
+  @retval EFI_UNSUPPORTED         This function is not supported by the network 
+                                  interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Stop (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_TPL     OldTpl;
+  EFI_STATUS  Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkStarted:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  Status = PxeStop (Snp);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/Transmit.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/Transmit.c
new file mode 100644
index 0000000000..e6c5af77ec
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/Transmit.c
@@ -0,0 +1,354 @@
+/** @file
+    Implementation of transmitting a packet.
+ 
+Copyright (c) 2004 - 2007, 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 "Snp.h"
+
+
+/**
+  Call UNDI to create the meadia header for the given data buffer.
+
+  @param  Snp              Pointer to SNP driver structure.
+  @param  MacHeaderPtr     Address where the media header will be filled in.
+  @param  HeaderSize       Size of the memory at MacHeaderPtr.
+  @param  Buffer           Data buffer pointer.
+  @param  BufferSize       Size of data in the Buffer
+  @param  DestAddr         Address of the destination mac address buffer.
+  @param  SrcAddr          Address of the source mac address buffer.
+  @param  ProtocolPtr      Address of the protocol type.
+
+  @retval EFI_SUCCESS      Successfully completed the undi call.
+  @retval Other            Error return from undi call.
+
+**/
+EFI_STATUS
+PxeFillHeader (
+  SNP_DRIVER      *Snp,
+  VOID            *MacHeaderPtr,
+  UINTN           HeaderSize,
+  VOID            *Buffer,
+  UINTN           BufferSize,
+  EFI_MAC_ADDRESS *DestAddr,
+  EFI_MAC_ADDRESS *SrcAddr,
+  UINT16          *ProtocolPtr
+  )
+{
+  PXE_CPB_FILL_HEADER_FRAGMENTED  *Cpb;
+
+  Cpb = Snp->Cpb;
+  if (SrcAddr != NULL) {
+    CopyMem (
+      (VOID *) Cpb->SrcAddr,
+      (VOID *) SrcAddr,
+      Snp->Mode.HwAddressSize
+      );
+  } else {
+    CopyMem (
+      (VOID *) Cpb->SrcAddr,
+      (VOID *) &(Snp->Mode.CurrentAddress),
+      Snp->Mode.HwAddressSize
+      );
+  }
+
+  CopyMem (
+    (VOID *) Cpb->DestAddr,
+    (VOID *) DestAddr,
+    Snp->Mode.HwAddressSize
+    );
+
+  //
+  // we need to do the byte swapping
+  //
+  Cpb->Protocol             = (UINT16) PXE_SWAP_UINT16 (*ProtocolPtr);
+
+  Cpb->PacketLen            = (UINT32) (BufferSize);
+  Cpb->MediaHeaderLen       = (UINT16) HeaderSize;
+
+  Cpb->FragCnt              = 2;
+  Cpb->reserved             = 0;
+
+  Cpb->FragDesc[0].FragAddr = (UINT64)(UINTN) MacHeaderPtr;
+  Cpb->FragDesc[0].FragLen  = (UINT32) HeaderSize;
+  Cpb->FragDesc[1].FragAddr = (UINT64)(UINTN) Buffer;
+  Cpb->FragDesc[1].FragLen  = (UINT32) BufferSize;
+
+  Cpb->FragDesc[0].reserved = Cpb->FragDesc[1].reserved = 0;
+
+  Snp->Cdb.OpCode     = PXE_OPCODE_FILL_HEADER;
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_FILL_HEADER_FRAGMENTED;
+
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+
+  Snp->Cdb.CPBsize    = (UINT16) sizeof (PXE_CPB_FILL_HEADER_FRAGMENTED);
+  Snp->Cdb.CPBaddr    = (UINT64)(UINTN) Cpb;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nSnp->undi.fill_header()  "));
+
+  (*Snp->IssueUndi32Command) ((UINT64) (UINTN) &Snp->Cdb);
+
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    return EFI_SUCCESS;
+
+  case PXE_STATCODE_INVALID_PARAMETER:
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nSnp->undi.fill_header()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_INVALID_PARAMETER;
+
+  default:
+    DEBUG (
+      (EFI_D_ERROR,
+      "\nSnp->undi.fill_header()  %xh:%xh\n",
+      Snp->Cdb.StatFlags,
+      Snp->Cdb.StatCode)
+      );
+
+    return EFI_DEVICE_ERROR;
+  }
+}
+
+
+/**
+  This routine calls undi to transmit the given data buffer
+
+  @param  Snp                 pointer to SNP driver structure
+  @param  Buffer           data buffer pointer
+  @param  BufferSize        Size of data in the Buffer
+
+  @retval EFI_SUCCESS         if successfully completed the undi call
+  @retval Other               error return from undi call.
+
+**/
+EFI_STATUS
+PxeTransmit (
+  SNP_DRIVER *Snp,
+  VOID       *Buffer,
+  UINTN      BufferSize
+  )
+{
+  PXE_CPB_TRANSMIT  *Cpb;
+  EFI_STATUS        Status;
+
+  Cpb             = Snp->Cpb;
+  Cpb->FrameAddr  = (UINT64) (UINTN) Buffer;
+  Cpb->DataLen    = (UINT32) BufferSize;
+
+  Cpb->MediaheaderLen = 0;
+  Cpb->reserved       = 0;
+
+  Snp->Cdb.OpFlags    = PXE_OPFLAGS_TRANSMIT_WHOLE;
+
+  Snp->Cdb.CPBsize    = (UINT16) sizeof (PXE_CPB_TRANSMIT);
+  Snp->Cdb.CPBaddr    = (UINT64)(UINTN) Cpb;
+
+  Snp->Cdb.OpCode     = PXE_OPCODE_TRANSMIT;
+  Snp->Cdb.DBsize     = PXE_DBSIZE_NOT_USED;
+  Snp->Cdb.DBaddr     = PXE_DBADDR_NOT_USED;
+
+  Snp->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  Snp->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  Snp->Cdb.IFnum      = Snp->IfNum;
+  Snp->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Issue UNDI command and check result.
+  //
+  DEBUG ((EFI_D_NET, "\nSnp->undi.transmit()  "));
+  DEBUG ((EFI_D_NET, "\nSnp->Cdb.OpCode  == %x", Snp->Cdb.OpCode));
+  DEBUG ((EFI_D_NET, "\nSnp->Cdb.CPBaddr == %LX", Snp->Cdb.CPBaddr));
+  DEBUG ((EFI_D_NET, "\nSnp->Cdb.DBaddr  == %LX", Snp->Cdb.DBaddr));
+  DEBUG ((EFI_D_NET, "\nCpb->FrameAddr   == %LX\n", Cpb->FrameAddr));
+
+  (*Snp->IssueUndi32Command) ((UINT64) (UINTN) &Snp->Cdb);
+
+  DEBUG ((EFI_D_NET, "\nexit Snp->undi.transmit()  "));
+  DEBUG ((EFI_D_NET, "\nSnp->Cdb.StatCode == %r", Snp->Cdb.StatCode));
+
+  //
+  // we will unmap the buffers in get_status call, not here
+  //
+  switch (Snp->Cdb.StatCode) {
+  case PXE_STATCODE_SUCCESS:
+    return EFI_SUCCESS;
+
+  case PXE_STATCODE_QUEUE_FULL:
+  case PXE_STATCODE_BUSY:
+    Status = EFI_NOT_READY;
+    break;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+  }
+
+  DEBUG (
+    (EFI_D_ERROR,
+    "\nSnp->undi.transmit()  %xh:%xh\n",
+    Snp->Cdb.StatFlags,
+    Snp->Cdb.StatCode)
+    );
+
+  return Status;
+}
+
+/**
+  Places a packet in the transmit queue of a network interface.
+  
+  This function places the packet specified by Header and Buffer on the transmit
+  queue. If HeaderSize is nonzero and HeaderSize is not equal to 
+  This->Mode->MediaHeaderSize, then EFI_INVALID_PARAMETER will be returned. If 
+  BufferSize is less than This->Mode->MediaHeaderSize, then EFI_BUFFER_TOO_SMALL
+  will be returned. If Buffer is NULL, then EFI_INVALID_PARAMETER will be 
+  returned. If HeaderSize is nonzero and DestAddr or Protocol is NULL, then
+  EFI_INVALID_PARAMETER will be returned. If the transmit engine of the network
+  interface is busy, then EFI_NOT_READY will be returned. If this packet can be 
+  accepted by the transmit engine of the network interface, the packet contents 
+  specified by Buffer will be placed on the transmit queue of the network 
+  interface, and EFI_SUCCESS will be returned. GetStatus() can be used to 
+  determine when the packet has actually been transmitted. The contents of the 
+  Buffer must not be modified until the packet has actually been transmitted. 
+  The Transmit() function performs nonblocking I/O. A caller who wants to perform
+  blocking I/O, should call Transmit(), and then GetStatus() until the 
+  transmitted buffer shows up in the recycled transmit buffer.
+  If the driver has not been initialized, EFI_DEVICE_ERROR will be returned.
+
+  @param This       A pointer to the EFI_SIMPLE_NETWORK_PROTOCOL instance.
+  @param HeaderSize The size, in bytes, of the media header to be filled in by the 
+                    Transmit() function. If HeaderSize is nonzero, then it must
+                    be equal to This->Mode->MediaHeaderSize and the DestAddr and
+                    Protocol parameters must not be NULL.
+  @param BufferSize The size, in bytes, of the entire packet (media header and
+                    data) to be transmitted through the network interface.
+  @param Buffer     A pointer to the packet (media header followed by data) to be 
+                    transmitted. This parameter cannot be NULL. If HeaderSize is 
+                    zero, then the media header in Buffer must already be filled
+                    in by the caller. If HeaderSize is nonzero, then the media 
+                    header will be filled in by the Transmit() function.
+  @param SrcAddr    The source HW MAC address. If HeaderSize is zero, then this 
+                    parameter is ignored. If HeaderSize is nonzero and SrcAddr 
+                    is NULL, then This->Mode->CurrentAddress is used for the 
+                    source HW MAC address.
+  @param DestAddr   The destination HW MAC address. If HeaderSize is zero, then 
+                    this parameter is ignored.
+  @param Protocol   The type of header to build. If HeaderSize is zero, then this 
+                    parameter is ignored. See RFC 1700, section "Ether Types," 
+                    for examples.
+
+  @retval EFI_SUCCESS           The packet was placed on the transmit queue.
+  @retval EFI_NOT_STARTED       The network interface has not been started.
+  @retval EFI_NOT_READY         The network interface is too busy to accept this
+                                transmit request.
+  @retval EFI_BUFFER_TOO_SMALL  The BufferSize parameter is too small.
+  @retval EFI_INVALID_PARAMETER One or more of the parameters has an unsupported
+                                value.
+  @retval EFI_DEVICE_ERROR      The command could not be sent to the network interface.
+  @retval EFI_UNSUPPORTED       This function is not supported by the network interface.
+
+**/
+EFI_STATUS
+EFIAPI
+SnpUndi32Transmit (
+  IN EFI_SIMPLE_NETWORK_PROTOCOL *This,
+  IN UINTN                       HeaderSize,
+  IN UINTN                       BufferSize,
+  IN VOID                        *Buffer,
+  IN EFI_MAC_ADDRESS             *SrcAddr,  OPTIONAL
+  IN EFI_MAC_ADDRESS             *DestAddr, OPTIONAL
+  IN UINT16                      *Protocol  OPTIONAL
+  )
+{
+  SNP_DRIVER  *Snp;
+  EFI_STATUS  Status;
+  EFI_TPL     OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Snp = EFI_SIMPLE_NETWORK_DEV_FROM_THIS (This);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Snp == NULL) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  switch (Snp->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+
+  default:
+    Status = EFI_DEVICE_ERROR;
+    goto ON_EXIT;
+  }
+
+  if (Buffer == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  if (BufferSize < Snp->Mode.MediaHeaderSize) {
+    Status = EFI_BUFFER_TOO_SMALL;
+    goto ON_EXIT;
+  }
+
+  //
+  // if the HeaderSize is non-zero, we need to fill up the header and for that
+  // we need the destination address and the protocol
+  //
+  if (HeaderSize != 0) {
+    if (HeaderSize != Snp->Mode.MediaHeaderSize || DestAddr == 0 || Protocol == 0) {
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    Status = PxeFillHeader (
+              Snp,
+              Buffer,
+              HeaderSize,
+              (UINT8 *) Buffer + HeaderSize,
+              BufferSize - HeaderSize,
+              DestAddr,
+              SrcAddr,
+              Protocol
+              );
+
+    if (EFI_ERROR (Status)) {
+      goto ON_EXIT;
+    }
+  }
+
+  Status = PxeTransmit (Snp, Buffer, BufferSize);
+
+ON_EXIT:
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/SnpDxe/WaitForPacket.c b/Core/MdeModulePkg/Universal/Network/SnpDxe/WaitForPacket.c
new file mode 100644
index 0000000000..23d7455fd1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/SnpDxe/WaitForPacket.c
@@ -0,0 +1,92 @@
+/** @file
+    Event handler to check for available packet.
+
+Copyright (c) 2004 - 2008, 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 "Snp.h"
+
+
+/**
+  Nofication call back function for WaitForPacket event.
+
+  @param  Event       EFI Event.
+  @param  SnpPtr      Pointer to SNP_DRIVER structure.
+
+**/
+VOID
+EFIAPI
+SnpWaitForPacketNotify (
+  EFI_EVENT     Event,
+  VOID          *SnpPtr
+  )
+{
+  PXE_DB_GET_STATUS PxeDbGetStatus;
+
+  //
+  // Do nothing if either parameter is a NULL pointer.
+  //
+  if (Event == NULL || SnpPtr == NULL) {
+    return ;
+  }
+  //
+  // Do nothing if the SNP interface is not initialized.
+  //
+  switch (((SNP_DRIVER *) SnpPtr)->Mode.State) {
+  case EfiSimpleNetworkInitialized:
+    break;
+
+  case EfiSimpleNetworkStopped:
+  case EfiSimpleNetworkStarted:
+  default:
+    return ;
+  }
+  //
+  // Fill in CDB for UNDI GetStatus().
+  //
+  ((SNP_DRIVER *) SnpPtr)->Cdb.OpCode     = PXE_OPCODE_GET_STATUS;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.OpFlags    = 0;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.CPBsize    = PXE_CPBSIZE_NOT_USED;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.CPBaddr    = PXE_CPBADDR_NOT_USED;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.DBsize     = (UINT16) (sizeof (UINT32) * 2);
+  ((SNP_DRIVER *) SnpPtr)->Cdb.DBaddr     = (UINT64)(UINTN) (((SNP_DRIVER *) SnpPtr)->Db);
+  ((SNP_DRIVER *) SnpPtr)->Cdb.StatCode   = PXE_STATCODE_INITIALIZE;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.StatFlags  = PXE_STATFLAGS_INITIALIZE;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.IFnum      = ((SNP_DRIVER *) SnpPtr)->IfNum;
+  ((SNP_DRIVER *) SnpPtr)->Cdb.Control    = PXE_CONTROL_LAST_CDB_IN_LIST;
+
+  //
+  // Clear contents of DB buffer.
+  //
+  ZeroMem (((SNP_DRIVER *) SnpPtr)->Db, sizeof (UINT32) * 2);
+
+  //
+  // Issue UNDI command and check result.
+  //
+  (*((SNP_DRIVER *) SnpPtr)->IssueUndi32Command) ((UINT64)(UINTN) &((SNP_DRIVER *) SnpPtr)->Cdb);
+
+  if (((SNP_DRIVER *) SnpPtr)->Cdb.StatCode != EFI_SUCCESS) {
+    return ;
+  }
+  //
+  // We might have a packet.  Check the receive length and signal
+  // the event if the length is not zero.
+  //
+  CopyMem (
+    &PxeDbGetStatus,
+    ((SNP_DRIVER *) SnpPtr)->Db,
+    sizeof (UINT32) * 2
+    );
+
+  if (PxeDbGetStatus.RxFrameLen != 0) {
+    gBS->SignalEvent (Event);
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/ComponentName.c
new file mode 100644
index 0000000000..bb3a39c5b1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/ComponentName.c
@@ -0,0 +1,433 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for Tcp4Dxe driver.
+
+Copyright (c) 2005 - 2015, 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<BR>
+
+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 "Tcp4Main.h"
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     EFI_HANDLE                   ControllerHandle,
+  IN     EFI_HANDLE                   ChildHandle        OPTIONAL,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **ControllerName
+  );
+
+
+///
+/// EFI Component Name Protocol
+///
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gTcp4ComponentName = {
+  TcpComponentNameGetDriverName,
+  TcpComponentNameGetControllerName,
+  "eng"
+};
+
+///
+/// EFI Component Name 2 Protocol
+///
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gTcp4ComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) TcpComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) TcpComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mTcpDriverNameTable[] = {
+  {
+    "eng;en",
+    L"Tcp Network Service Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE *gTcpControllerNameTable = NULL;
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mTcpDriverNameTable,
+           DriverName,
+           (BOOLEAN) (This == &gTcp4ComponentName)
+           );
+}
+
+/**
+  Update the component name for the Tcp4 child handle.
+
+  @param  Tcp4[in]                   A pointer to the EFI_TCP4_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS                Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER      The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  IN  EFI_TCP4_PROTOCOL             *Tcp4
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[80];
+  EFI_TCP4_CONFIG_DATA             Tcp4ConfigData;
+
+  if (Tcp4 == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Format the child name into the string buffer as:
+  // TCPv4 (SrcPort=59, DestPort=60, ActiveFlag=TRUE)
+  //
+  ZeroMem (&Tcp4ConfigData, sizeof (Tcp4ConfigData));
+  Status = Tcp4->GetModeData (Tcp4, NULL, &Tcp4ConfigData, NULL, NULL, NULL);
+  if (!EFI_ERROR (Status)) {
+    UnicodeSPrint (HandleName, sizeof (HandleName),
+      L"TCPv4 (SrcPort=%d, DestPort=%d, ActiveFlag=%s)",
+      Tcp4ConfigData.AccessPoint.StationPort,
+      Tcp4ConfigData.AccessPoint.RemotePort,
+      (Tcp4ConfigData.AccessPoint.ActiveFlag ? L"TRUE" : L"FALSE")
+      );
+  } else if (Status == EFI_NOT_STARTED) {
+    UnicodeSPrint (
+      HandleName,
+      sizeof (HandleName),
+      L"TCPv4 (Not started)"
+      );
+  } else {
+    return Status;
+  }
+
+  if (gTcpControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gTcpControllerNameTable);
+    gTcpControllerNameTable = NULL;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gTcp4ComponentName.SupportedLanguages,
+             &gTcpControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gTcp4ComponentName2.SupportedLanguages,
+           &gTcpControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle        The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+TcpComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN     EFI_HANDLE                   ControllerHandle,
+  IN     EFI_HANDLE                   ChildHandle        OPTIONAL,
+  IN     CHAR8                        *Language,
+     OUT CHAR16                       **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_TCP4_PROTOCOL             *Tcp4;
+
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  // 
+  // Make sure this driver produced ChildHandle 
+  // 
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiIp4ProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  // 
+  // Retrieve an instance of a produced protocol from ChildHandle
+  // 
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiTcp4ProtocolGuid,
+                  (VOID **)&Tcp4,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Tcp4);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gTcpControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gTcp4ComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.c
new file mode 100644
index 0000000000..2e31643821
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.c
@@ -0,0 +1,1282 @@
+/** @file
+  Implementation of the Socket.
+
+Copyright (c) 2005 - 2012, 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<BR>
+
+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 "SockImpl.h"
+
+/**
+  Get the length of the data that can be retrieved from the socket
+  receive buffer.
+
+  @param  SockBuffer            Pointer to the socket receive buffer.
+  @param  IsUrg                 Pointer to a BOOLEAN variable. If TRUE the data is
+                                OOB.
+  @param  BufLen                The maximum length of the data buffer to store the
+                                received data in socket layer.
+
+  @return The length of the data can be retreived.
+
+**/
+UINT32
+SockTcpDataToRcv (
+  IN     SOCK_BUFFER    *SockBuffer,
+     OUT BOOLEAN        *IsUrg,
+  IN     UINT32         BufLen
+  );
+  
+/**
+  Process the send token.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockProcessSndToken (
+  IN OUT SOCKET *Sock
+  );
+
+/**
+  Supporting function for both SockImpl and SockInterface.
+
+  @param Event             The Event this notify function registered to, ignored.
+  
+**/
+VOID
+EFIAPI
+SockFreeFoo (
+  IN EFI_EVENT Event
+  )
+{
+  return ;
+}
+
+
+/**
+  Get the length of the data that can be retrieved from the socket
+  receive buffer.
+
+  @param  SockBuffer            Pointer to the socket receive buffer.
+  @param  IsUrg                 Pointer to a BOOLEAN variable. If TRUE the data is
+                                OOB.
+  @param  BufLen                The maximum length of the data buffer to store the
+                                received data in socket layer.
+
+  @return The length of the data can be retreived.
+
+**/
+UINT32
+SockTcpDataToRcv (
+  IN     SOCK_BUFFER    *SockBuffer,
+     OUT BOOLEAN        *IsUrg,
+  IN     UINT32         BufLen
+  )
+{
+  NET_BUF       *RcvBufEntry;
+  UINT32        DataLen;
+  TCP_RSV_DATA  *TcpRsvData;
+  BOOLEAN       Urg;
+  ASSERT ((SockBuffer != NULL) && (IsUrg != NULL) && (BufLen > 0));
+
+  RcvBufEntry = SockBufFirst (SockBuffer);
+  ASSERT (RcvBufEntry != NULL);
+
+  TcpRsvData  = (TCP_RSV_DATA *) RcvBufEntry->ProtoData;
+
+  *IsUrg      = (BOOLEAN) ((TcpRsvData->UrgLen > 0) ? TRUE : FALSE);
+
+  if (*IsUrg && TcpRsvData->UrgLen < RcvBufEntry->TotalSize) {
+
+    DataLen = MIN (TcpRsvData->UrgLen, BufLen);
+
+    if (DataLen < TcpRsvData->UrgLen) {
+      TcpRsvData->UrgLen = TcpRsvData->UrgLen - DataLen;
+    } else {
+      TcpRsvData->UrgLen = 0;
+    }
+
+    return DataLen;
+
+  }
+
+  DataLen     = RcvBufEntry->TotalSize;
+
+  RcvBufEntry = SockBufNext (SockBuffer, RcvBufEntry);
+
+  while ((BufLen > DataLen) && (RcvBufEntry != NULL)) {
+
+    TcpRsvData  = (TCP_RSV_DATA *) RcvBufEntry->ProtoData;
+
+    Urg         = (BOOLEAN) ((TcpRsvData->UrgLen > 0) ? TRUE : FALSE);
+
+    if (*IsUrg != Urg) {
+      break;
+    }
+
+    if (*IsUrg && TcpRsvData->UrgLen < RcvBufEntry->TotalSize) {
+
+      if (TcpRsvData->UrgLen + DataLen < BufLen) {
+        TcpRsvData->UrgLen = 0;
+      } else {
+        TcpRsvData->UrgLen = TcpRsvData->UrgLen - (BufLen - DataLen);
+      }
+
+      return MIN (TcpRsvData->UrgLen + DataLen, BufLen);
+
+    }
+
+    DataLen += RcvBufEntry->TotalSize;
+
+    RcvBufEntry = SockBufNext (SockBuffer, RcvBufEntry);
+  }
+
+  DataLen = MIN (BufLen, DataLen);
+  return DataLen;
+}
+
+
+/**
+  Copy data from socket buffer to application provided receive buffer.
+
+  @param  Sock                  Pointer to the socket.
+  @param  TcpRxData             Pointer to the application provided receive buffer.
+  @param  RcvdBytes             The maximum length of the data can be copied.
+  @param  IsOOB                 If TURE the data is OOB, FALSE the data is normal.
+
+**/
+VOID
+SockSetTcpRxData (
+  IN SOCKET     *Sock,
+  IN VOID       *TcpRxData,
+  IN UINT32     RcvdBytes,
+  IN BOOLEAN    IsOOB
+  )
+{
+  UINT32                  Index;
+  UINT32                  CopyBytes;
+  UINT32                  OffSet;
+  EFI_TCP4_RECEIVE_DATA   *RxData;
+  EFI_TCP4_FRAGMENT_DATA  *Fragment;
+
+  RxData  = (EFI_TCP4_RECEIVE_DATA *) TcpRxData;
+
+  OffSet  = 0;
+
+  ASSERT (RxData->DataLength >= RcvdBytes);
+
+  RxData->DataLength  = RcvdBytes;
+  RxData->UrgentFlag  = IsOOB;
+
+  for (Index = 0; (Index < RxData->FragmentCount) && (RcvdBytes > 0); Index++) {
+
+    Fragment  = &RxData->FragmentTable[Index];
+    CopyBytes = MIN ((UINT32) (Fragment->FragmentLength), RcvdBytes);
+
+    NetbufQueCopy (
+      Sock->RcvBuffer.DataQueue,
+      OffSet,
+      CopyBytes,
+      Fragment->FragmentBuffer
+      );
+
+    Fragment->FragmentLength = CopyBytes;
+    RcvdBytes -= CopyBytes;
+    OffSet += CopyBytes;
+  }
+}
+
+
+/**
+  Get received data from the socket layer to the receive token.
+
+  @param  Sock                  Pointer to the socket.
+  @param  RcvToken              Pointer to the application provided receive token.
+
+  @return The length of data received in this token.
+
+**/
+UINT32
+SockProcessRcvToken (
+  IN     SOCKET        *Sock,
+  IN OUT SOCK_IO_TOKEN *RcvToken
+  )
+{
+  UINT32                 TokenRcvdBytes;
+  EFI_TCP4_RECEIVE_DATA  *RxData;
+  BOOLEAN                IsUrg;
+
+  ASSERT (Sock != NULL);
+
+  ASSERT (SockStream == Sock->Type);
+
+  RxData = RcvToken->Packet.RxData;
+
+  TokenRcvdBytes = SockTcpDataToRcv (
+                      &Sock->RcvBuffer,
+                      &IsUrg,
+                      (UINT32) RxData->DataLength
+                      );
+
+  //
+  // Copy data from RcvBuffer of socket to user
+  // provided RxData and set the fields in TCP RxData
+  //
+  SockSetTcpRxData (Sock, RxData, TokenRcvdBytes, IsUrg);
+
+  NetbufQueTrim (Sock->RcvBuffer.DataQueue, TokenRcvdBytes);
+  SIGNAL_TOKEN (&(RcvToken->Token), EFI_SUCCESS);
+
+  return TokenRcvdBytes;
+}
+
+
+/**
+  Process the TCP send data, buffer the tcp txdata and append
+  the buffer to socket send buffer,then try to send it.
+
+  @param  Sock                  Pointer to the socket.
+  @param  TcpTxData             Pointer to the application provided send buffer.
+
+  @retval EFI_SUCCESS           The operation is completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  Failed due to resource limit.
+
+**/
+EFI_STATUS
+SockProcessTcpSndData (
+  IN SOCKET   *Sock,
+  IN VOID     *TcpTxData
+  )
+{
+  NET_BUF                 *SndData;
+  EFI_STATUS              Status;
+  EFI_TCP4_TRANSMIT_DATA  *TxData;
+
+  TxData = (EFI_TCP4_TRANSMIT_DATA *) TcpTxData;
+
+  //
+  // transform this TxData into a NET_BUFFER
+  // and insert it into Sock->SndBuffer
+  //
+  SndData = NetbufFromExt (
+              (NET_FRAGMENT *) TxData->FragmentTable,
+              (UINT32) TxData->FragmentCount,
+              0,
+              0,
+              SockFreeFoo,
+              NULL
+              );
+
+  if (NULL == SndData) {
+    DEBUG ((EFI_D_ERROR, "SockKProcessSndData: Failed to"
+      " call NetBufferFromExt\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NetbufQueAppend (Sock->SndBuffer.DataQueue, SndData);
+
+  //
+  // notify the low layer protocol to handle this send token
+  //
+  if (TxData->Urgent) {
+    Status = Sock->ProtoHandler (Sock, SOCK_SNDURG, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (TxData->Push) {
+    Status = Sock->ProtoHandler (Sock, SOCK_SNDPUSH, NULL);
+
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // low layer protocol should really handle the sending
+  // process when catching SOCK_SND request
+  //
+  Status = Sock->ProtoHandler (Sock, SOCK_SND, NULL);
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Flush the tokens in the specific token list.
+
+  @param  Sock                  Pointer to the socket.
+  @param  PendingTokenList      Pointer to the token list to be flushed.
+  
+**/
+VOID
+SockFlushPendingToken (
+  IN SOCKET         *Sock,
+  IN LIST_ENTRY     *PendingTokenList
+  )
+{
+  SOCK_TOKEN            *SockToken;
+  SOCK_COMPLETION_TOKEN *Token;
+
+  ASSERT ((Sock != NULL) && (PendingTokenList != NULL));
+
+  while (!IsListEmpty (PendingTokenList)) {
+    SockToken = NET_LIST_HEAD (
+                  PendingTokenList,
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    Token = SockToken->Token;
+    SIGNAL_TOKEN (Token, Sock->SockError);
+
+    RemoveEntryList (&(SockToken->TokenList));
+    FreePool (SockToken);
+  }
+}
+
+
+/**
+  Wake up the connection token while the connection is successfully established, 
+  then try to process any pending send token.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockWakeConnToken (
+  IN OUT SOCKET *Sock
+  )
+{
+  ASSERT (Sock->ConnectionToken != NULL);
+
+  SIGNAL_TOKEN (Sock->ConnectionToken, EFI_SUCCESS);
+  Sock->ConnectionToken = NULL;
+
+  //
+  // check to see if some pending send token existed?
+  //
+  SockProcessSndToken (Sock);
+  return ;
+}
+
+
+/**
+  Wake up the listen token while the connection is established successfully.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockWakeListenToken (
+  IN OUT SOCKET *Sock
+  )
+{
+  SOCKET                *Parent;
+  SOCK_TOKEN            *SockToken;
+  EFI_TCP4_LISTEN_TOKEN *ListenToken;
+
+  Parent = Sock->Parent;
+
+  ASSERT ((Parent != NULL) && SOCK_IS_LISTENING (Parent) && SOCK_IS_CONNECTED (Sock));
+
+  if (!IsListEmpty (&Parent->ListenTokenList)) {
+    SockToken = NET_LIST_HEAD (
+                  &Parent->ListenTokenList,
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    ListenToken = (EFI_TCP4_LISTEN_TOKEN *) SockToken->Token;
+    ListenToken->NewChildHandle = Sock->SockHandle;
+
+    SIGNAL_TOKEN (&(ListenToken->CompletionToken), EFI_SUCCESS);
+
+    RemoveEntryList (&SockToken->TokenList);
+    FreePool (SockToken);
+
+    RemoveEntryList (&Sock->ConnectionList);
+
+    Parent->ConnCnt--;
+    DEBUG ((EFI_D_INFO, "SockWakeListenToken: accept a socket, now conncnt is %d", Parent->ConnCnt));
+
+    Sock->Parent = NULL;
+  }
+}
+
+
+/**
+  Wake up the receive token while some data is received.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockWakeRcvToken (
+  IN SOCKET *Sock
+  )
+{
+  UINT32        RcvdBytes;
+  UINT32        TokenRcvdBytes;
+  SOCK_TOKEN    *SockToken;
+  SOCK_IO_TOKEN *RcvToken;
+
+  ASSERT (Sock->RcvBuffer.DataQueue != NULL);
+
+  RcvdBytes = (Sock->RcvBuffer.DataQueue)->BufSize;
+
+  ASSERT (RcvdBytes > 0);
+
+  while (RcvdBytes > 0 && !IsListEmpty (&Sock->RcvTokenList)) {
+
+    SockToken = NET_LIST_HEAD (
+                  &Sock->RcvTokenList,
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    RcvToken        = (SOCK_IO_TOKEN *) SockToken->Token;
+    TokenRcvdBytes  = SockProcessRcvToken (Sock, RcvToken);
+
+    if (0 == TokenRcvdBytes) {
+      return ;
+    }
+
+    RemoveEntryList (&(SockToken->TokenList));
+    FreePool (SockToken);
+    RcvdBytes -= TokenRcvdBytes;
+  }
+}
+
+
+/**
+  Process the send token.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockProcessSndToken (
+  IN OUT SOCKET *Sock
+  )
+{
+  UINT32                  FreeSpace;
+  SOCK_TOKEN              *SockToken;
+  UINT32                  DataLen;
+  SOCK_IO_TOKEN           *SndToken;
+  EFI_TCP4_TRANSMIT_DATA  *TxData;
+  EFI_STATUS              Status;
+
+  ASSERT ((Sock != NULL) && (SockStream == Sock->Type));
+
+  FreeSpace = SockGetFreeSpace (Sock, SOCK_SND_BUF);
+
+  //
+  // to determine if process a send token using
+  // socket layer flow control policy
+  //
+  while ((FreeSpace >= Sock->SndBuffer.LowWater) &&
+         !IsListEmpty (&Sock->SndTokenList)) {
+
+    SockToken = NET_LIST_HEAD (
+                  &(Sock->SndTokenList),
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    //
+    // process this token
+    //
+    RemoveEntryList (&(SockToken->TokenList));
+    InsertTailList (
+      &(Sock->ProcessingSndTokenList),
+      &(SockToken->TokenList)
+      );
+
+    //
+    // Proceess it in the light of  SockType
+    //
+    SndToken  = (SOCK_IO_TOKEN *) SockToken->Token;
+    TxData    = SndToken->Packet.TxData;
+
+    DataLen = (UINT32) TxData->DataLength;
+    Status  = SockProcessTcpSndData (Sock, TxData);
+
+    if (EFI_ERROR (Status)) {
+      goto OnError;
+    }
+
+    if (DataLen >= FreeSpace) {
+      FreeSpace = 0;
+
+    } else {
+      FreeSpace -= DataLen;
+
+    }
+  }
+
+  return ;
+
+OnError:
+
+  RemoveEntryList (&SockToken->TokenList);
+  SIGNAL_TOKEN (SockToken->Token, Status);
+  FreePool (SockToken);
+}
+
+
+/**
+  Create a socket with initial data SockInitData.
+
+  @param  SockInitData          Pointer to the initial data of the socket.
+
+  @return Pointer to the newly created socket, return NULL when exception occured.
+
+**/
+SOCKET *
+SockCreate (
+  IN SOCK_INIT_DATA *SockInitData
+  )
+{
+  SOCKET      *Sock;
+  SOCKET      *Parent;
+  EFI_STATUS  Status;
+
+  ASSERT ((SockInitData != NULL) && (SockInitData->ProtoHandler != NULL));
+  ASSERT (SockInitData->Type == SockStream);
+  ASSERT ((SockInitData->ProtoData != NULL) && (SockInitData->DataSize <= PROTO_RESERVED_LEN));
+
+  Parent = SockInitData->Parent;
+
+  if ((Parent != NULL) && (Parent->ConnCnt == Parent->BackLog)) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "SockCreate: Socket parent has "
+      "reached its connection limit with %d ConnCnt and %d BackLog\n",
+      Parent->ConnCnt,
+      Parent->BackLog)
+      );
+
+    return NULL;
+  }
+
+  Sock = AllocateZeroPool (sizeof (SOCKET));
+  if (NULL == Sock) {
+
+    DEBUG ((EFI_D_ERROR, "SockCreate: No resource to create a new socket\n"));
+    return NULL;
+  }
+
+  InitializeListHead (&Sock->Link);
+  InitializeListHead (&Sock->ConnectionList);
+  InitializeListHead (&Sock->ListenTokenList);
+  InitializeListHead (&Sock->RcvTokenList);
+  InitializeListHead (&Sock->SndTokenList);
+  InitializeListHead (&Sock->ProcessingSndTokenList);
+
+  EfiInitializeLock (&(Sock->Lock), TPL_CALLBACK);
+
+  Sock->SndBuffer.DataQueue = NetbufQueAlloc ();
+  if (NULL == Sock->SndBuffer.DataQueue) {
+    DEBUG ((EFI_D_ERROR, "SockCreate: No resource to allocate"
+      " SndBuffer for new socket\n"));
+
+    goto OnError;
+  }
+
+  Sock->RcvBuffer.DataQueue = NetbufQueAlloc ();
+  if (NULL == Sock->RcvBuffer.DataQueue) {
+    DEBUG ((EFI_D_ERROR, "SockCreate: No resource to allocate "
+      "RcvBuffer for new socket\n"));
+
+    goto OnError;
+  }
+
+  Sock->Signature           = SOCK_SIGNATURE;
+
+  Sock->Parent              = Parent;
+  Sock->BackLog             = SockInitData->BackLog;
+  Sock->ProtoHandler        = SockInitData->ProtoHandler;
+  Sock->SndBuffer.HighWater = SockInitData->SndBufferSize;
+  Sock->RcvBuffer.HighWater = SockInitData->RcvBufferSize;
+  Sock->Type                = SockInitData->Type;
+  Sock->DriverBinding       = SockInitData->DriverBinding;
+  Sock->State               = SockInitData->State;
+  Sock->CreateCallback      = SockInitData->CreateCallback;
+  Sock->DestroyCallback     = SockInitData->DestroyCallback;
+  Sock->Context             = SockInitData->Context;
+
+  Sock->SockError           = EFI_ABORTED;
+  Sock->SndBuffer.LowWater  = SOCK_BUFF_LOW_WATER;
+  Sock->RcvBuffer.LowWater  = SOCK_BUFF_LOW_WATER;
+
+  //
+  // Install protocol on Sock->SockHandle
+  //
+  CopyMem (
+    &(Sock->NetProtocol.TcpProtocol),
+    SockInitData->Protocol,
+    sizeof (EFI_TCP4_PROTOCOL)
+    );
+
+  //
+  // copy the protodata into socket
+  //
+  CopyMem (Sock->ProtoReserved, SockInitData->ProtoData, SockInitData->DataSize);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &Sock->SockHandle,
+                  &gEfiTcp4ProtocolGuid,
+                  &(Sock->NetProtocol.TcpProtocol),
+                  NULL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SockCreate: Install TCP protocol in "
+      "socket failed with %r\n", Status));
+
+    goto OnError;
+  }
+
+  if (Parent != NULL) {
+    ASSERT (Parent->BackLog > 0);
+    ASSERT (SOCK_IS_LISTENING (Parent));
+
+    //
+    // need to add it into Parent->ConnectionList
+    // if the Parent->ConnCnt < Parent->BackLog
+    //
+    Parent->ConnCnt++;
+
+    DEBUG (
+      (EFI_D_INFO,
+      "SockCreate: Create a new socket and add to parent, now conncnt is %d\n",
+      Parent->ConnCnt)
+      );
+
+    InsertTailList (&Parent->ConnectionList, &Sock->ConnectionList);
+  }
+
+  if (Sock->CreateCallback != NULL) {
+    Status = Sock->CreateCallback (Sock, Sock->Context);
+    if (EFI_ERROR (Status)) {
+      goto OnError;
+    }
+  }
+
+  return Sock;
+
+OnError:
+
+  if (Sock->SockHandle != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Sock->SockHandle,
+           &gEfiTcp4ProtocolGuid,
+           &(Sock->NetProtocol.TcpProtocol),
+           NULL
+           );
+  }
+
+  if (NULL != Sock->SndBuffer.DataQueue) {
+    NetbufQueFree (Sock->SndBuffer.DataQueue);
+  }
+
+  if (NULL != Sock->RcvBuffer.DataQueue) {
+    NetbufQueFree (Sock->RcvBuffer.DataQueue);
+  }
+
+  FreePool (Sock);
+
+  return NULL;
+}
+
+
+/**
+  Destroy a socket.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockDestroy (
+  IN OUT SOCKET *Sock
+  )
+{
+  VOID        *SockProtocol;
+  EFI_GUID    *ProtocolGuid;
+  EFI_STATUS  Status;
+
+  ASSERT (SockStream == Sock->Type);
+
+  if (Sock->DestroyCallback != NULL) {
+    Sock->DestroyCallback (Sock, Sock->Context);
+  }
+
+  //
+  // Flush the completion token buffered
+  // by sock and rcv, snd buffer
+  //
+  if (!SOCK_IS_UNCONFIGURED (Sock)) {
+
+    SockConnFlush (Sock);
+    SockSetState (Sock, SO_CLOSED);
+    Sock->ConfigureState = SO_UNCONFIGURED;
+
+  }
+  //
+  // Destroy the RcvBuffer Queue and SendBuffer Queue
+  //
+  NetbufQueFree (Sock->RcvBuffer.DataQueue);
+  NetbufQueFree (Sock->SndBuffer.DataQueue);
+
+  //
+  // Remove it from parent connection list if needed
+  //
+  if (Sock->Parent != NULL) {
+
+    RemoveEntryList (&(Sock->ConnectionList));
+    (Sock->Parent->ConnCnt)--;
+
+    DEBUG (
+      (EFI_D_INFO,
+      "SockDestroy: Delete a unaccepted socket from parent"
+      "now conncnt is %d\n",
+      Sock->Parent->ConnCnt)
+      );
+
+    Sock->Parent = NULL;
+  }
+
+  //
+  // Set the protocol guid and driver binding handle
+  // in the light of Sock->SockType
+  //
+  ProtocolGuid = &gEfiTcp4ProtocolGuid;
+
+  //
+  // Retrieve the protocol installed on this sock
+  //
+  Status = gBS->OpenProtocol (
+                  Sock->SockHandle,
+                  ProtocolGuid,
+                  &SockProtocol,
+                  Sock->DriverBinding,
+                  Sock->SockHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockDestroy: Open protocol installed "
+      "on socket failed with %r\n", Status));
+
+    goto FreeSock;
+  }
+
+  //
+  // Uninstall the protocol installed on this sock
+  // in the light of Sock->SockType
+  //
+  gBS->UninstallMultipleProtocolInterfaces (
+        Sock->SockHandle,
+        ProtocolGuid,
+        SockProtocol,
+        NULL
+        );
+
+FreeSock:
+  FreePool (Sock);
+  return ;
+}
+
+
+/**
+  Flush the sndBuffer and rcvBuffer of socket.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockConnFlush (
+  IN OUT SOCKET *Sock
+  )
+{
+  SOCKET  *Child;
+
+  ASSERT (Sock != NULL);
+
+  //
+  // Clear the flag in this socket
+  //
+  Sock->Flag = 0;
+
+  //
+  // Flush the SndBuffer and RcvBuffer of Sock
+  //
+  NetbufQueFlush (Sock->SndBuffer.DataQueue);
+  NetbufQueFlush (Sock->RcvBuffer.DataQueue);
+
+  //
+  // Signal the pending token
+  //
+  if (Sock->ConnectionToken != NULL) {
+    SIGNAL_TOKEN (Sock->ConnectionToken, Sock->SockError);
+    Sock->ConnectionToken = NULL;
+  }
+
+  if (Sock->CloseToken != NULL) {
+    SIGNAL_TOKEN (Sock->CloseToken, Sock->SockError);
+    Sock->CloseToken = NULL;
+  }
+
+  SockFlushPendingToken (Sock, &(Sock->ListenTokenList));
+  SockFlushPendingToken (Sock, &(Sock->RcvTokenList));
+  SockFlushPendingToken (Sock, &(Sock->SndTokenList));
+  SockFlushPendingToken (Sock, &(Sock->ProcessingSndTokenList));
+
+  //
+  // Destroy the pending connection, if it is a listening socket
+  //
+  if (SOCK_IS_LISTENING (Sock)) {
+    while (!IsListEmpty (&Sock->ConnectionList)) {
+      Child = NET_LIST_HEAD (
+                &Sock->ConnectionList,
+                SOCKET,
+                ConnectionList
+                );
+
+      SockDestroyChild (Child);
+    }
+
+    Sock->ConnCnt = 0;
+  }
+
+  return ;
+}
+
+
+/**
+  Set the state of the socket.
+
+  @param  Sock                  Pointer to the socket.
+  @param  State                 The new socket state to be set.
+
+**/
+VOID
+SockSetState (
+  IN OUT SOCKET     *Sock,
+  IN     UINT8      State
+  )
+{
+  Sock->State = State;
+}
+
+
+/**
+  Clone a new socket including its associated protocol control block.
+
+  @param  Sock                  Pointer to the socket to be cloned.
+
+  @return Pointer to the newly cloned socket. If NULL, error condition occurred.
+
+**/
+SOCKET *
+SockClone (
+  IN SOCKET *Sock
+  )
+{
+  SOCKET          *ClonedSock;
+  SOCK_INIT_DATA  InitData;
+
+  InitData.BackLog         = Sock->BackLog;
+  InitData.Parent          = Sock;
+  InitData.State           = Sock->State;
+  InitData.ProtoHandler    = Sock->ProtoHandler;
+  InitData.Type            = Sock->Type;
+  InitData.RcvBufferSize   = Sock->RcvBuffer.HighWater;
+  InitData.SndBufferSize   = Sock->SndBuffer.HighWater;
+  InitData.DriverBinding   = Sock->DriverBinding;
+  InitData.Protocol        = &(Sock->NetProtocol);
+  InitData.CreateCallback  = Sock->CreateCallback;
+  InitData.DestroyCallback = Sock->DestroyCallback;
+  InitData.Context         = Sock->Context;
+  InitData.ProtoData       = Sock->ProtoReserved;
+  InitData.DataSize        = sizeof (Sock->ProtoReserved);
+
+  ClonedSock               = SockCreate (&InitData);
+
+  if (NULL == ClonedSock) {
+    DEBUG ((EFI_D_ERROR, "SockClone: no resource to create a cloned sock\n"));
+    return NULL;
+  }
+
+  SockSetState (ClonedSock, SO_CONNECTING);
+  ClonedSock->ConfigureState = Sock->ConfigureState;
+
+  return ClonedSock;
+}
+
+
+/**
+  Called by the low layer protocol to indicate the socket a connection is 
+  established. 
+  
+  This function just changes the socket's state to SO_CONNECTED 
+  and signals the token used for connection establishment.
+
+  @param  Sock                  Pointer to the socket associated with the
+                                established connection.
+**/
+VOID
+SockConnEstablished (
+  IN SOCKET *Sock
+  )
+{
+
+  ASSERT (SO_CONNECTING == Sock->State);
+
+  SockSetState (Sock, SO_CONNECTED);
+
+  if (NULL == Sock->Parent) {
+    SockWakeConnToken (Sock);
+  } else {
+    SockWakeListenToken (Sock);
+  }
+
+  return ;
+}
+
+
+/**
+  Called by the low layer protocol to indicate the connection is closed.
+  
+  This function flushes the socket, sets the state to SO_CLOSED and signals 
+  the close token.
+
+  @param  Sock                  Pointer to the socket associated with the closed
+                                connection.
+                                
+**/
+VOID
+SockConnClosed (
+  IN OUT SOCKET *Sock
+  )
+{
+  if (Sock->CloseToken != NULL) {
+    SIGNAL_TOKEN (Sock->CloseToken, EFI_SUCCESS);
+    Sock->CloseToken = NULL;
+  }
+
+  SockConnFlush (Sock);
+  SockSetState (Sock, SO_CLOSED);
+
+  if (Sock->Parent != NULL) {
+    SockDestroyChild (Sock);
+  }
+
+}
+
+
+/**
+  Called by low layer protocol to indicate that some data is sent or processed.
+   
+  This function trims the sent data in the socket send buffer, signals the data 
+  token if proper.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Count                 The length of the data processed or sent, in bytes.
+
+**/
+VOID
+SockDataSent (
+  IN SOCKET     *Sock,
+  IN UINT32     Count
+  )
+{
+  SOCK_TOKEN            *SockToken;
+  SOCK_COMPLETION_TOKEN *SndToken;
+
+  ASSERT (!IsListEmpty (&Sock->ProcessingSndTokenList));
+  ASSERT (Count <= (Sock->SndBuffer.DataQueue)->BufSize);
+
+  NetbufQueTrim (Sock->SndBuffer.DataQueue, Count);
+
+  //
+  // To check if we can signal some snd token in this socket
+  //
+  while (Count > 0) {
+    SockToken = NET_LIST_HEAD (
+                  &(Sock->ProcessingSndTokenList),
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    SndToken = SockToken->Token;
+
+    if (SockToken->RemainDataLen <= Count) {
+
+      RemoveEntryList (&(SockToken->TokenList));
+      SIGNAL_TOKEN (SndToken, EFI_SUCCESS);
+      Count -= SockToken->RemainDataLen;
+      FreePool (SockToken);
+    } else {
+
+      SockToken->RemainDataLen -= Count;
+      Count = 0;
+    }
+  }
+
+  //
+  // to judge if we can process some send token in
+  // Sock->SndTokenList, if so process those send token
+  //
+  SockProcessSndToken (Sock);
+  return ;
+}
+
+
+/**
+  Called by the low layer protocol to copy some data in socket send
+  buffer starting from the specific offset to a buffer provided by
+  the caller.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Offset                The start point of the data to be copied.
+  @param  Len                   The length of the data to be copied.
+  @param  Dest                  Pointer to the destination to copy the data.
+
+  @return The data size copied.
+
+**/
+UINT32
+SockGetDataToSend (
+  IN SOCKET      *Sock,
+  IN UINT32      Offset,
+  IN UINT32      Len,
+  IN UINT8       *Dest
+  )
+{
+  ASSERT ((Sock != NULL) && SockStream == Sock->Type);
+
+  return NetbufQueCopy (
+          Sock->SndBuffer.DataQueue,
+          Offset,
+          Len,
+          Dest
+          );
+}
+
+
+/**
+  Called by the low layer protocol to deliver received data to socket layer.
+  
+  This function will append the data to the socket receive buffer, set ther 
+  urgent data length and then check if any receive token can be signaled.
+
+  @param  Sock                  Pointer to the socket.
+  @param  NetBuffer             Pointer to the buffer that contains the received
+                                data.
+  @param  UrgLen                The length of the urgent data in the received data.
+
+**/
+VOID
+SockDataRcvd (
+  IN     SOCKET    *Sock,
+  IN OUT NET_BUF   *NetBuffer,
+  IN     UINT32    UrgLen
+  )
+{
+  ASSERT ((Sock != NULL) && (Sock->RcvBuffer.DataQueue != NULL) &&
+    UrgLen <= NetBuffer->TotalSize);
+
+  NET_GET_REF (NetBuffer);
+
+  ((TCP_RSV_DATA *) (NetBuffer->ProtoData))->UrgLen = UrgLen;
+
+  NetbufQueAppend (Sock->RcvBuffer.DataQueue, NetBuffer);
+
+  SockWakeRcvToken (Sock);
+  return ;
+}
+
+
+/**
+  Get the length of the free space of the specific socket buffer.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Which                 Flag to indicate which socket buffer to check,
+                                either send buffer or receive buffer.
+
+  @return The length of the free space, in bytes.
+
+**/
+UINT32
+SockGetFreeSpace (
+  IN SOCKET  *Sock,
+  IN UINT32  Which
+  )
+{
+  UINT32      BufferCC;
+  SOCK_BUFFER *SockBuffer;
+
+  ASSERT ((Sock != NULL) && ((SOCK_SND_BUF == Which) || (SOCK_RCV_BUF == Which)));
+
+  if (SOCK_SND_BUF == Which) {
+    SockBuffer = &(Sock->SndBuffer);
+  } else {
+    SockBuffer = &(Sock->RcvBuffer);
+  }
+
+  BufferCC = (SockBuffer->DataQueue)->BufSize;
+
+  if (BufferCC >= SockBuffer->HighWater) {
+
+    return 0;
+  }
+
+  return SockBuffer->HighWater - BufferCC;
+}
+
+
+/**
+  Signal the receive token with the specific error or
+  set socket error code after error is received.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Error                 The error code received.
+
+**/
+VOID
+SockRcvdErr (
+  IN OUT SOCKET       *Sock,
+  IN     EFI_STATUS   Error
+  )
+{
+  SOCK_TOKEN  *SockToken;
+
+  if (!IsListEmpty (&Sock->RcvTokenList)) {
+
+    SockToken = NET_LIST_HEAD (
+                  &Sock->RcvTokenList,
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    RemoveEntryList (&SockToken->TokenList);
+
+    SIGNAL_TOKEN (SockToken->Token, Error);
+
+    FreePool (SockToken);
+  } else {
+
+    SOCK_ERROR (Sock, Error);
+  }
+}
+
+
+/**
+  Called by the low layer protocol to indicate that there will be no more data 
+  from the communication peer.
+  
+  This function set the socket's state to SO_NO_MORE_DATA and signal all queued 
+  IO tokens with the error status EFI_CONNECTION_FIN.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockNoMoreData (
+  IN OUT SOCKET *Sock
+  )
+{
+  EFI_STATUS  Err;
+
+  SOCK_NO_MORE_DATA (Sock);
+
+  if (!IsListEmpty (&Sock->RcvTokenList)) {
+
+    ASSERT (0 == GET_RCV_DATASIZE (Sock));
+
+    Err = Sock->SockError;
+
+    SOCK_ERROR (Sock, EFI_CONNECTION_FIN);
+
+    SockFlushPendingToken (Sock, &Sock->RcvTokenList);
+
+    SOCK_ERROR (Sock, Err);
+
+  }
+
+}
+
+
+/**
+  Get the first buffer block in the specific socket buffer.
+
+  @param  Sockbuf               Pointer to the socket buffer.
+
+  @return Pointer to the first buffer in the queue. NULL if the queue is empty.
+
+**/
+NET_BUF *
+SockBufFirst (
+  IN SOCK_BUFFER *Sockbuf
+  )
+{
+  LIST_ENTRY      *NetbufList;
+
+  NetbufList = &(Sockbuf->DataQueue->BufList);
+
+  if (IsListEmpty (NetbufList)) {
+    return NULL;
+  }
+
+  return NET_LIST_HEAD (NetbufList, NET_BUF, List);
+}
+
+
+/**
+  Get the next buffer block in the specific socket buffer.
+
+  @param  Sockbuf               Pointer to the socket buffer.
+  @param  SockEntry             Pointer to the buffer block prior to the required
+                                one.
+
+  @return Pointer to the buffer block next to SockEntry. NULL if SockEntry is 
+          the tail or head entry.
+
+**/
+NET_BUF *
+SockBufNext (
+  IN SOCK_BUFFER *Sockbuf,
+  IN NET_BUF     *SockEntry
+  )
+{
+  LIST_ENTRY      *NetbufList;
+
+  NetbufList = &(Sockbuf->DataQueue->BufList);
+
+  if ((SockEntry->List.ForwardLink == NetbufList) ||
+      (SockEntry->List.BackLink == &SockEntry->List) ||
+      (SockEntry->List.ForwardLink == &SockEntry->List)) {
+
+    return NULL;
+  }
+
+  return NET_LIST_USER_STRUCT (SockEntry->List.ForwardLink, NET_BUF, List);
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.h
new file mode 100644
index 0000000000..52472d4440
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockImpl.h
@@ -0,0 +1,130 @@
+/** @file
+  Socket implementation header file.
+
+Copyright (c) 2005 - 2006, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SOCK_IMPL_H_
+#define _SOCK_IMPL_H_
+
+#include "Socket.h"
+
+/**
+  Signal a event with the given status.
+  
+  @param Token        The token's event is to be signaled.
+  @param TokenStatus  The status to be sent with the event.
+  
+**/
+#define SIGNAL_TOKEN(Token, TokenStatus) \
+  do { \
+    (Token)->Status = (TokenStatus); \
+    gBS->SignalEvent ((Token)->Event); \
+  } while (0)
+
+
+/**
+  Supporting function for both SockImpl and SockInterface.
+
+  @param Event  The Event this notify function registered to, ignored.
+  
+**/
+VOID
+EFIAPI
+SockFreeFoo (
+  IN EFI_EVENT Event
+  );
+
+/**
+  Process the TCP send data, buffer the tcp txdata and append
+  the buffer to socket send buffer,then try to send it.
+
+  @param  Sock                  Pointer to the socket.
+  @param  TcpTxData             Pointer to the tcp txdata.
+
+  @retval EFI_SUCCESS           The operation is completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  Failed due to resource limit.
+
+**/
+EFI_STATUS
+SockProcessTcpSndData (
+  IN SOCKET   *Sock,
+  IN VOID     *TcpTxData
+  );
+
+/**
+  Copy data from socket buffer to application provided receive buffer.
+
+  @param  Sock                  Pointer to the socket.
+  @param  TcpRxData             Pointer to the application provided receive buffer.
+  @param  RcvdBytes             The maximum length of the data can be copied.
+  @param  IsOOB                 If TURE the data is OOB, else the data is normal.
+
+**/
+VOID
+SockSetTcpRxData (
+  IN SOCKET     *Sock,
+  IN VOID       *TcpRxData,
+  IN UINT32     RcvdBytes,
+  IN BOOLEAN    IsOOB
+  );
+
+/**
+  Get received data from the socket layer to the receive token.
+
+  @param  Sock                  Pointer to the socket.
+  @param  RcvToken              Pointer to the application provided receive token.
+
+  @return The length of data received in this token.
+
+**/
+UINT32
+SockProcessRcvToken (
+  IN     SOCKET        *Sock,
+  IN OUT SOCK_IO_TOKEN *RcvToken
+  );
+
+/**
+  Flush the socket.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockConnFlush (
+  IN OUT SOCKET *Sock
+  );
+
+/**
+  Create a socket with initial data SockInitData.
+
+  @param  SockInitData          Pointer to the initial data of the socket.
+
+  @return Pointer to the newly created socket.
+
+**/
+SOCKET *
+SockCreate (
+  IN SOCK_INIT_DATA *SockInitData
+  );
+
+/**
+  Destroy a socket.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockDestroy (
+  IN OUT SOCKET *Sock
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockInterface.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockInterface.c
new file mode 100644
index 0000000000..feed86c590
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/SockInterface.c
@@ -0,0 +1,959 @@
+/** @file
+  Interface function of the Socket.
+
+Copyright (c) 2005 - 2012, 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<BR>
+
+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 "SockImpl.h"
+
+
+/**
+  Check whether the Event is in the List.
+
+  @param  List                 Pointer to the token list to be searched.
+  @param  Event                The event to be checked.
+
+  @retval  TRUE                The specific Event exists in the List. 
+  @retval  FALSE               The specific Event is not in the List.
+
+**/
+BOOLEAN
+SockTokenExistedInList (
+  IN LIST_ENTRY     *List,
+  IN EFI_EVENT      Event
+  )
+{
+  LIST_ENTRY      *ListEntry;
+  SOCK_TOKEN      *SockToken;
+
+  NET_LIST_FOR_EACH (ListEntry, List) {
+    SockToken = NET_LIST_USER_STRUCT (
+                  ListEntry,
+                  SOCK_TOKEN,
+                  TokenList
+                  );
+
+    if (Event == SockToken->Token->Event) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Call SockTokenExistedInList() to check whether the Event is
+  in the related socket's lists.
+
+  @param  Sock                 Pointer to the instance's socket.
+  @param  Event                The event to be checked.
+
+  @retval  TRUE                The Event exists in related socket's lists. 
+  @retval  FALSE               The Event is not in related socket's lists.
+
+**/
+BOOLEAN
+SockTokenExisted (
+  IN SOCKET    *Sock,
+  IN EFI_EVENT Event
+  )
+{
+
+  if (SockTokenExistedInList (&Sock->SndTokenList, Event) ||
+      SockTokenExistedInList (&Sock->ProcessingSndTokenList, Event) ||
+      SockTokenExistedInList (&Sock->RcvTokenList, Event) ||
+      SockTokenExistedInList (&Sock->ListenTokenList, Event)) {
+
+    return TRUE;
+  }
+
+  if ((Sock->ConnectionToken != NULL) &&
+      (Sock->ConnectionToken->Event == Event)) {
+
+    return TRUE;
+  }
+
+  if ((Sock->CloseToken != NULL) && (Sock->CloseToken->Event == Event)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Buffer a token into the specific list of socket Sock.
+
+  @param  Sock                 Pointer to the instance's socket.
+  @param  List                 Pointer to the list to store the token.
+  @param  Token                Pointer to the token to be buffered.
+  @param  DataLen              The data length of the buffer contained in Token.
+
+  @return Pointer to the token that wraps Token. If NULL, error condition occurred.
+
+**/
+SOCK_TOKEN *
+SockBufferToken (
+  IN SOCKET         *Sock,
+  IN LIST_ENTRY     *List,
+  IN VOID           *Token,
+  IN UINT32         DataLen
+  )
+{
+  SOCK_TOKEN  *SockToken;
+
+  SockToken = AllocatePool (sizeof (SOCK_TOKEN));
+  if (NULL == SockToken) {
+
+    DEBUG ((EFI_D_ERROR, "SockBufferIOToken: No Memory "
+      "to allocate SockToken\n"));
+
+    return NULL;
+  }
+
+  SockToken->Sock           = Sock;
+  SockToken->Token          = (SOCK_COMPLETION_TOKEN *) Token;
+  SockToken->RemainDataLen  = DataLen;
+  InsertTailList (List, &SockToken->TokenList);
+
+  return SockToken;
+}
+
+
+/**
+  Destroy the socket Sock and its associated protocol control block.
+
+  @param  Sock                 The socket to be destroyed.
+
+  @retval EFI_SUCCESS          The socket Sock is destroyed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+
+**/
+EFI_STATUS
+SockDestroyChild (
+  IN SOCKET *Sock
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT ((Sock != NULL) && (Sock->ProtoHandler != NULL));
+
+  if (Sock->InDestroy) {
+    return EFI_SUCCESS;
+  }
+
+  Sock->InDestroy = TRUE;
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockDestroyChild: Get the lock to "
+      "access socket failed with %r\n", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // force protocol layer to detach the PCB
+  //
+  Status = Sock->ProtoHandler (Sock, SOCK_DETACH, NULL);
+
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockDestroyChild: Protocol detach socket"
+      " failed with %r\n", Status));
+
+    Sock->InDestroy = FALSE;
+  } else if (SOCK_IS_CONFIGURED (Sock)) {
+
+    SockConnFlush (Sock);
+    SockSetState (Sock, SO_CLOSED);
+
+    Sock->ConfigureState = SO_UNCONFIGURED;
+  }
+
+  EfiReleaseLock (&(Sock->Lock));
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  SockDestroy (Sock);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create a socket and its associated protocol control block
+  with the intial data SockInitData and protocol specific
+  data ProtoData.
+
+  @param  SockInitData         Inital data to setting the socket.
+  
+  @return Pointer to the newly created socket. If NULL, error condition occured.
+
+**/
+SOCKET *
+SockCreateChild (
+  IN SOCK_INIT_DATA *SockInitData
+  )
+{
+  SOCKET      *Sock;
+  EFI_STATUS  Status;
+
+  //
+  // create a new socket
+  //
+  Sock = SockCreate (SockInitData);
+  if (NULL == Sock) {
+
+    DEBUG ((EFI_D_ERROR, "SockCreateChild: No resource to "
+      "create a new socket\n"));
+
+    return NULL;
+  }
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockCreateChild: Get the lock to "
+      "access socket failed with %r\n", Status));
+
+    SockDestroy (Sock);
+    return NULL;
+  }
+  //
+  // inform the protocol layer to attach the socket
+  // with a new protocol control block
+  //
+  Status = Sock->ProtoHandler (Sock, SOCK_ATTACH, NULL);
+  EfiReleaseLock (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockCreateChild: Protocol failed to"
+      " attach a socket with %r\n", Status));
+
+    SockDestroy (Sock);
+    Sock = NULL;
+  }
+
+  return Sock;
+}
+
+
+/**
+  Configure the specific socket Sock using configuration data ConfigData.
+
+  @param  Sock                 Pointer to the socket to be configured.
+  @param  ConfigData           Pointer to the configuration data.
+
+  @retval EFI_SUCCESS          The socket is configured successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket or the
+                               socket is already configured.
+
+**/
+EFI_STATUS
+SockConfigure (
+  IN SOCKET *Sock,
+  IN VOID   *ConfigData
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockConfigure: Get the access for "
+      "socket failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_CONFIGURED (Sock)) {
+    Status = EFI_ACCESS_DENIED;
+    goto OnExit;
+  }
+
+  ASSERT (Sock->State == SO_CLOSED);
+
+  Status = Sock->ProtoHandler (Sock, SOCK_CONFIGURE, ConfigData);
+
+OnExit:
+  EfiReleaseLock (&(Sock->Lock));
+
+  return Status;
+}
+
+
+/**
+  Initiate a connection establishment process.
+
+  @param  Sock                 Pointer to the socket to initiate the initate the
+                               connection.
+  @param  Token                Pointer to the token used for the connection
+                               operation.
+
+  @retval EFI_SUCCESS          The connection is initialized successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not configured to
+                               be an active one, or the token is already in one of
+                               this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockConnect (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   Event;
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockConnect: Get the access for "
+      "socket failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+    Status = EFI_NO_MAPPING;
+    goto OnExit;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+
+    Status = EFI_NOT_STARTED;
+    goto OnExit;
+  }
+
+  if (!SOCK_IS_CLOSED (Sock) || !SOCK_IS_CONFIGURED_ACTIVE (Sock)) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto OnExit;
+  }
+
+  Event = ((SOCK_COMPLETION_TOKEN *) Token)->Event;
+
+  if (SockTokenExisted (Sock, Event)) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto OnExit;
+  }
+
+  Sock->ConnectionToken = (SOCK_COMPLETION_TOKEN *) Token;
+  SockSetState (Sock, SO_CONNECTING);
+  Status = Sock->ProtoHandler (Sock, SOCK_CONNECT, NULL);
+
+OnExit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Issue a listen token to get an existed connected network instance
+  or wait for a connection if there is none.
+
+  @param  Sock                 Pointer to the socket to accept connections.
+  @param  Token                The token to accept a connection.
+
+  @retval EFI_SUCCESS          Either a connection is accpeted or the Token is
+                               buffered for further acception.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not configured to
+                               be a passive one, or the token is already in one of
+                               this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the Token due to memory limit.
+
+**/
+EFI_STATUS
+SockAccept (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  )
+{
+  EFI_TCP4_LISTEN_TOKEN *ListenToken;
+  LIST_ENTRY            *ListEntry;
+  EFI_STATUS            Status;
+  SOCKET                *Socket;
+  EFI_EVENT             Event;
+
+  ASSERT (SockStream == Sock->Type);
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockAccept: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+    Status = EFI_NO_MAPPING;
+    goto Exit;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  if (!SOCK_IS_LISTENING (Sock)) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  Event = ((SOCK_COMPLETION_TOKEN *) Token)->Event;
+
+  if (SockTokenExisted (Sock, Event)) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  ListenToken = (EFI_TCP4_LISTEN_TOKEN *) Token;
+
+  //
+  // Check if a connection has already in this Sock->ConnectionList
+  //
+  NET_LIST_FOR_EACH (ListEntry, &Sock->ConnectionList) {
+
+    Socket = NET_LIST_USER_STRUCT (ListEntry, SOCKET, ConnectionList);
+
+    if (SOCK_IS_CONNECTED (Socket)) {
+      ListenToken->NewChildHandle = Socket->SockHandle;
+      SIGNAL_TOKEN (&(ListenToken->CompletionToken), EFI_SUCCESS);
+
+      RemoveEntryList (ListEntry);
+
+      ASSERT (Socket->Parent != NULL);
+
+      Socket->Parent->ConnCnt--;
+
+      DEBUG (
+        (EFI_D_INFO,
+        "SockAccept: Accept a socket, now conncount is %d",
+        Socket->Parent->ConnCnt)
+        );
+      Socket->Parent = NULL;
+
+      goto Exit;
+    }
+  }
+
+  //
+  // Buffer this token for latter incoming connection request
+  //
+  if (NULL == SockBufferToken (Sock, &(Sock->ListenTokenList), Token, 0)) {
+
+    Status = EFI_OUT_OF_RESOURCES;
+  }
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+
+  return Status;
+}
+
+
+/**
+  Issue a token with data to the socket to send out.
+
+  @param  Sock                 Pointer to the socket to process the token with
+                               data.
+  @param  Token                The token with data that needs to send out.
+
+  @retval EFI_SUCCESS          The token is processed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the token due to memory limit.
+
+**/
+EFI_STATUS
+SockSend (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  )
+{
+  SOCK_IO_TOKEN           *SndToken;
+  EFI_EVENT               Event;
+  UINT32                  FreeSpace;
+  EFI_TCP4_TRANSMIT_DATA  *TxData;
+  EFI_STATUS              Status;
+  SOCK_TOKEN              *SockToken;
+  UINT32                  DataLen;
+
+  ASSERT (SockStream == Sock->Type);
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockSend: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+    Status = EFI_NO_MAPPING;
+    goto Exit;
+  }
+
+  SndToken  = (SOCK_IO_TOKEN *) Token;
+  TxData    = (EFI_TCP4_TRANSMIT_DATA *) SndToken->Packet.TxData;
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  if (!(SOCK_IS_CONNECTING (Sock) || SOCK_IS_CONNECTED (Sock))) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  //
+  // check if a token is already in the token buffer
+  //
+  Event = SndToken->Token.Event;
+
+  if (SockTokenExisted (Sock, Event)) {
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  DataLen = (UINT32) TxData->DataLength;
+
+  //
+  // process this sending token now or buffer it only?
+  //
+  FreeSpace = SockGetFreeSpace (Sock, SOCK_SND_BUF);
+
+  if ((FreeSpace < Sock->SndBuffer.LowWater) || !SOCK_IS_CONNECTED (Sock)) {
+
+    SockToken = SockBufferToken (
+                  Sock,
+                  &Sock->SndTokenList,
+                  SndToken,
+                  DataLen
+                  );
+
+    if (NULL == SockToken) {
+      Status = EFI_OUT_OF_RESOURCES;
+    }
+  } else {
+
+    SockToken = SockBufferToken (
+                  Sock,
+                  &Sock->ProcessingSndTokenList,
+                  SndToken,
+                  DataLen
+                  );
+
+    if (NULL == SockToken) {
+      DEBUG ((EFI_D_ERROR, "SockSend: Failed to buffer IO token into"
+        " socket processing SndToken List\n", Status));
+
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Exit;
+    }
+
+    Status = SockProcessTcpSndData (Sock, TxData);
+
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SockSend: Failed to process "
+        "Snd Data\n", Status));
+
+      RemoveEntryList (&(SockToken->TokenList));
+      FreePool (SockToken);
+    }
+  }
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Issue a token to get data from the socket.
+
+  @param  Sock                 Pointer to the socket to get data from.
+  @param  Token                The token to store the received data from the
+                               socket.
+
+  @retval EFI_SUCCESS          The token is processed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_CONNECTION_FIN   The connection is closed and there is no more data.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the token due to memory limit.
+
+**/
+EFI_STATUS
+SockRcv (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  )
+{
+  SOCK_IO_TOKEN *RcvToken;
+  UINT32        RcvdBytes;
+  EFI_STATUS    Status;
+  EFI_EVENT     Event;
+
+  ASSERT (SockStream == Sock->Type);
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockRcv: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+
+    Status = EFI_NO_MAPPING;
+    goto Exit;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  if (!(SOCK_IS_CONNECTED (Sock) || SOCK_IS_CONNECTING (Sock))) {
+
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  RcvToken = (SOCK_IO_TOKEN *) Token;
+
+  //
+  // check if a token is already in the token buffer of this socket
+  //
+  Event = RcvToken->Token.Event;
+  if (SockTokenExisted (Sock, Event)) {
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  RcvToken  = (SOCK_IO_TOKEN *) Token;
+  RcvdBytes = GET_RCV_DATASIZE (Sock);
+
+  //
+  // check whether an error has happened before
+  //
+  if (EFI_ABORTED != Sock->SockError) {
+
+    SIGNAL_TOKEN (&(RcvToken->Token), Sock->SockError);
+    Sock->SockError = EFI_ABORTED;
+    goto Exit;
+  }
+
+  //
+  // check whether can not receive and there is no any
+  // data buffered in Sock->RcvBuffer
+  //
+  if (SOCK_IS_NO_MORE_DATA (Sock) && (0 == RcvdBytes)) {
+
+    Status = EFI_CONNECTION_FIN;
+    goto Exit;
+  }
+
+  if (RcvdBytes != 0) {
+    Status = SockProcessRcvToken (Sock, RcvToken);
+
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    Status = Sock->ProtoHandler (Sock, SOCK_CONSUMED, NULL);
+  } else {
+
+    if (NULL == SockBufferToken (Sock, &Sock->RcvTokenList, RcvToken, 0)) {
+      Status = EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Reset the socket and its associated protocol control block.
+
+  @param  Sock                 Pointer to the socket to be flushed.
+
+  @retval EFI_SUCCESS          The socket is flushed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+
+**/
+EFI_STATUS
+SockFlush (
+  IN SOCKET *Sock
+  )
+{
+  EFI_STATUS  Status;
+
+  ASSERT (SockStream == Sock->Type);
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockFlush: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (!SOCK_IS_CONFIGURED (Sock)) {
+    goto Exit;
+  }
+
+  Status = Sock->ProtoHandler (Sock, SOCK_FLUSH, NULL);
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockFlush: Protocol failed handling"
+      " SOCK_FLUSH with %r", Status));
+
+    goto Exit;
+  }
+
+  SOCK_ERROR (Sock, EFI_ABORTED);
+  SockConnFlush (Sock);
+  SockSetState (Sock, SO_CLOSED);
+
+  Sock->ConfigureState = SO_UNCONFIGURED;
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Close or abort the socket associated connection.
+
+  @param  Sock                 Pointer to the socket of the connection to close or
+                               abort.
+  @param  Token                The token for close operation.
+  @param  OnAbort              TRUE for aborting the connection, FALSE to close it.
+
+  @retval EFI_SUCCESS          The close or abort operation is initialized
+                               successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockClose (
+  IN SOCKET  *Sock,
+  IN VOID    *Token,
+  IN BOOLEAN OnAbort
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   Event;
+
+  ASSERT (SockStream == Sock->Type);
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SockClose: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+    Status = EFI_NO_MAPPING;
+    goto Exit;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  if (SOCK_IS_DISCONNECTING (Sock)) {
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  Event = ((SOCK_COMPLETION_TOKEN *) Token)->Event;
+
+  if (SockTokenExisted (Sock, Event)) {
+    Status = EFI_ACCESS_DENIED;
+    goto Exit;
+  }
+
+  Sock->CloseToken = Token;
+  SockSetState (Sock, SO_DISCONNECTING);
+
+  if (OnAbort) {
+    Status = Sock->ProtoHandler (Sock, SOCK_ABORT, NULL);
+  } else {
+    Status = Sock->ProtoHandler (Sock, SOCK_CLOSE, NULL);
+  }
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Get the mode data of the low layer protocol.
+
+  @param  Sock                 Pointer to the socket to get mode data from.
+  @param  Mode                 Pointer to the data to store the low layer mode
+                               information.
+
+  @retval EFI_SUCCESS          The mode data is got successfully.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockGetMode (
+  IN     SOCKET *Sock,
+  IN OUT VOID   *Mode
+  )
+{
+  return Sock->ProtoHandler (Sock, SOCK_MODE, Mode);
+}
+
+
+/**
+  Configure the low level protocol to join a multicast group for
+  this socket's connection.
+
+  @param  Sock                 Pointer to the socket of the connection to join the
+                               specific multicast group.
+  @param  GroupInfo            Pointer to the multicast group info.
+
+  @retval EFI_SUCCESS          The configuration is done successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockGroup (
+  IN SOCKET *Sock,
+  IN VOID   *GroupInfo
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "SockGroup: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  Status = Sock->ProtoHandler (Sock, SOCK_GROUP, GroupInfo);
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
+
+
+/**
+  Add or remove route information in IP route table associated
+  with this socket.
+
+  @param  Sock                 Pointer to the socket associated with the IP route
+                               table to operate on.
+  @param  RouteInfo            Pointer to the route information to be processed.
+
+  @retval EFI_SUCCESS          The route table is updated successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is  not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockRoute (
+  IN SOCKET    *Sock,
+  IN VOID      *RouteInfo
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EfiAcquireLockOrFail (&(Sock->Lock));
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "SockRoute: Get the access for socket"
+      " failed with %r", Status));
+
+    return EFI_ACCESS_DENIED;
+  }
+
+  if (SOCK_IS_NO_MAPPING (Sock)) {
+    Status = EFI_NO_MAPPING;
+    goto Exit;
+  }
+
+  if (SOCK_IS_UNCONFIGURED (Sock)) {
+    Status = EFI_NOT_STARTED;
+    goto Exit;
+  }
+
+  Status = Sock->ProtoHandler (Sock, SOCK_ROUTE, RouteInfo);
+
+Exit:
+  EfiReleaseLock (&(Sock->Lock));
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Socket.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Socket.h
new file mode 100644
index 0000000000..8c25c63b11
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Socket.h
@@ -0,0 +1,986 @@
+/** @file
+  Socket header file.
+
+Copyright (c) 2005 - 2012, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _SOCKET_H_
+#define _SOCKET_H_
+
+#include <Uefi.h>
+
+#include <Protocol/Ip4.h>
+#include <Protocol/Tcp4.h>
+#include <Protocol/Udp4.h>
+
+#include <Library/NetLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/DpcLib.h>
+#include <Library/PrintLib.h>
+
+#define SOCK_SND_BUF        0
+#define SOCK_RCV_BUF        1
+
+#define SOCK_BUFF_LOW_WATER (2 * 1024)
+#define SOCK_RCV_BUFF_SIZE  (8 * 1024)
+#define SOCK_SND_BUFF_SIZE  (8 * 1024)
+#define SOCK_BACKLOG        5
+
+#define PROTO_RESERVED_LEN  20
+
+#define SO_NO_MORE_DATA     0x0001
+
+//
+//
+//
+// When a socket is created it enters into SO_UNCONFIGURED,
+// no actions can be taken on this socket, only after calling
+// SockConfigure. The state transition diagram of socket is
+// as following:
+//
+// SO_UNCONFIGURED --- SO_CONFIGURED --- SO_CONNECTING
+//  ^      |                                    |
+//  |      --->  SO_LISTENING                   |
+//  |                                           |
+//  |------------------SO_DISCONNECTING<-- SO_CONNECTED
+//
+// A passive socket can only go into SO_LISTENING and
+// SO_UNCONFIGURED state. SO_XXXING state is a middle state
+// when a socket is undergoing a protocol procedure such
+// as requesting a TCP connection.
+//
+//
+//
+
+///
+/// Socket state
+///
+#define SO_CLOSED        0
+#define SO_LISTENING     1
+#define SO_CONNECTING    2
+#define SO_CONNECTED     3
+#define SO_DISCONNECTING 4
+
+///
+/// Socket configure state
+///
+#define SO_UNCONFIGURED        0
+#define SO_CONFIGURED_ACTIVE   1
+#define SO_CONFIGURED_PASSIVE  2
+#define SO_NO_MAPPING          3
+
+/**
+  Set socket SO_NO_MORE_DATA flag.
+  
+  @param Sock               Pointer to the socket
+  
+**/
+#define SOCK_NO_MORE_DATA(Sock)     ((Sock)->Flag |= SO_NO_MORE_DATA)
+
+/**
+  Check whether the socket is unconfigured.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is unconfigued
+  @retval False              The socket is not unconfigued
+  
+**/
+#define SOCK_IS_UNCONFIGURED(Sock)  ((Sock)->ConfigureState == SO_UNCONFIGURED)
+
+/**
+  Check whether the socket is configured.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is configued
+  @retval False              The socket is not configued
+  
+**/
+#define SOCK_IS_CONFIGURED(Sock) \
+    (((Sock)->ConfigureState == SO_CONFIGURED_ACTIVE) || \
+    ((Sock)->ConfigureState == SO_CONFIGURED_PASSIVE))
+
+/**
+  Check whether the socket is configured to active mode.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is configued to active mode
+  @retval False              The socket is not configued to active mode
+  
+**/
+#define SOCK_IS_CONFIGURED_ACTIVE(Sock) \
+  ((Sock)->ConfigureState == SO_CONFIGURED_ACTIVE)
+
+/**
+  Check whether the socket is configured to passive mode.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is configued to passive mode
+  @retval False              The socket is not configued to passive mode
+  
+**/
+#define SOCK_IS_CONNECTED_PASSIVE(Sock) \
+  ((Sock)->ConfigureState == SO_CONFIGURED_PASSIVE)
+
+/**
+  Check whether the socket is mapped.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is no mapping
+  @retval False              The socket is mapped
+  
+**/
+#define SOCK_IS_NO_MAPPING(Sock) \
+  ((Sock)->ConfigureState == SO_NO_MAPPING)
+
+/**
+  Check whether the socket is closed.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is closed
+  @retval False              The socket is not closed
+  
+**/
+#define SOCK_IS_CLOSED(Sock)          ((Sock)->State == SO_CLOSED)
+
+/**
+  Check whether the socket is listening.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is listening
+  @retval False              The socket is not listening
+  
+**/
+#define SOCK_IS_LISTENING(Sock)       ((Sock)->State == SO_LISTENING)
+
+/**
+  Check whether the socket is connecting.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is connecting
+  @retval False              The socket is not connecting
+  
+**/
+#define SOCK_IS_CONNECTING(Sock)      ((Sock)->State == SO_CONNECTING)
+
+/**
+  Check whether the socket has connected.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket has connected
+  @retval False              The socket has not connected
+  
+**/
+#define SOCK_IS_CONNECTED(Sock)       ((Sock)->State == SO_CONNECTED)
+
+/**
+  Check whether the socket is disconnecting.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is disconnecting
+  @retval False              The socket is not disconnecting
+  
+**/
+#define SOCK_IS_DISCONNECTING(Sock)   ((Sock)->State == SO_DISCONNECTING)
+
+/**
+  Check whether the socket is no more data.
+  
+  @param  Sock               Pointer to the socket
+  
+  @retval True               The socket is no more data
+  @retval False              The socket still has data
+  
+**/
+#define SOCK_IS_NO_MORE_DATA(Sock)    (0 != ((Sock)->Flag & SO_NO_MORE_DATA))
+
+/**
+  Set the size of the receive buffer.
+  
+  @param  Sock               Pointer to the socket
+  @param  Size               The size to set
+
+**/
+#define SET_RCV_BUFFSIZE(Sock, Size)  ((Sock)->RcvBuffer.HighWater = (Size))
+
+/**
+  Get the size of the receive buffer.
+  
+  @param  Sock               Pointer to the socket
+  
+  @return The receive buffer size
+
+**/
+#define GET_RCV_BUFFSIZE(Sock)        ((Sock)->RcvBuffer.HighWater)
+
+/**
+  Get the size of the receive data.
+  
+  @param  Sock               Pointer to the socket
+  
+  @return The received data size
+
+**/
+#define GET_RCV_DATASIZE(Sock)        (((Sock)->RcvBuffer.DataQueue)->BufSize)
+
+/**
+  Set the size of the send buffer.
+  
+  @param  Sock               Pointer to the socket
+  @param  Size               The size to set
+
+**/
+#define SET_SND_BUFFSIZE(Sock, Size)  ((Sock)->SndBuffer.HighWater = (Size))
+
+/**
+  Get the size of the send buffer.
+  
+  @param  Sock               Pointer to the socket
+  
+  @return The send buffer size
+
+**/
+#define GET_SND_BUFFSIZE(Sock)        ((Sock)->SndBuffer.HighWater)
+
+/**
+  Get the size of the send data.
+  
+  @param  Sock               Pointer to the socket
+  
+  @return The send data size
+
+**/
+#define GET_SND_DATASIZE(Sock)        (((Sock)->SndBuffer.DataQueue)->BufSize)
+
+/**
+  Set the backlog value of the socket.
+  
+  @param  Sock               Pointer to the socket
+  @param  Value              The value to set
+
+**/
+#define SET_BACKLOG(Sock, Value)      ((Sock)->BackLog = (Value))
+
+/**
+  Get the backlog value of the socket.
+  
+  @param  Sock               Pointer to the socket
+  
+  @return The backlog value
+
+**/
+#define GET_BACKLOG(Sock)             ((Sock)->BackLog)
+
+/**
+  Set the socket with error state.
+  
+  @param  Sock               Pointer to the socket
+  @param  Error              The error state
+
+**/
+#define SOCK_ERROR(Sock, Error)       ((Sock)->SockError = (Error))
+
+#define SND_BUF_HDR_LEN(Sock) \
+  ((SockBufFirst (&((Sock)->SndBuffer)))->TotalSize)
+
+#define RCV_BUF_HDR_LEN(Sock) \
+  ((SockBufFirst (&((Sock)->RcvBuffer)))->TotalSize)
+
+#define SOCK_SIGNATURE                SIGNATURE_32 ('S', 'O', 'C', 'K')
+
+#define SOCK_FROM_THIS(a)             CR ((a), SOCKET, NetProtocol, SOCK_SIGNATURE)
+
+#define SOCK_FROM_TOKEN(Token)        (((SOCK_TOKEN *) (Token))->Sock)
+
+#define PROTO_TOKEN_FORM_SOCK(SockToken, Type) \
+  ((Type *) (((SOCK_TOKEN *) (SockToken))->Token))
+
+typedef struct _SOCKET SOCKET;
+
+///
+/// Socket completion token
+///
+typedef struct _SOCK_COMPLETION_TOKEN {
+  EFI_EVENT   Event;            ///< The event to be issued
+  EFI_STATUS  Status;           ///< The status to be issued
+} SOCK_COMPLETION_TOKEN;
+
+typedef union {
+  VOID  *RxData;
+  VOID  *TxData;
+} SOCK_IO_DATA;
+
+///
+/// The application token with data packet
+///
+typedef struct _SOCK_IO_TOKEN {
+  SOCK_COMPLETION_TOKEN Token;
+  SOCK_IO_DATA          Packet;
+} SOCK_IO_TOKEN;
+
+///
+///  The request issued from socket layer to protocol layer.  
+///
+#define SOCK_ATTACH     0    ///< Attach current socket to a new PCB
+#define SOCK_DETACH     1    ///< Detach current socket from the PCB
+#define SOCK_CONFIGURE  2    ///< Configure attached PCB
+#define SOCK_FLUSH      3    ///< Flush attached PCB
+#define SOCK_SND        4    ///< Need protocol to send something
+#define SOCK_SNDPUSH    5    ///< Need protocol to send pushed data
+#define SOCK_SNDURG     6    ///< Need protocol to send urgent data
+#define SOCK_CONSUMED   7    ///< Application has retrieved data from socket
+#define SOCK_CONNECT    8    ///< Need to connect to a peer
+#define SOCK_CLOSE      9    ///< Need to close the protocol process
+#define SOCK_ABORT      10   ///< Need to reset the protocol process
+#define SOCK_POLL       11   ///< Need to poll to the protocol layer
+#define SOCK_ROUTE      12   ///< Need to add a route information
+#define SOCK_MODE       13   ///< Need to get the mode data of the protocol
+#define SOCK_GROUP      14   ///< Need to join a mcast group
+
+///
+///  The socket type.
+///
+typedef enum {
+  SockDgram, ///< This socket providing datagram service
+  SockStream ///< This socket providing stream service
+} SOCK_TYPE;
+
+///
+///  The buffer structure of rcvd data and send data used by socket.
+///
+typedef struct _SOCK_BUFFER {
+  UINT32        HighWater;  ///< The buffersize upper limit of sock_buffer
+  UINT32        LowWater;   ///< The low warter mark of sock_buffer
+  NET_BUF_QUEUE *DataQueue; ///< The queue to buffer data
+} SOCK_BUFFER;
+
+/**
+  The handler of protocol for request from socket.
+  
+  @param Socket              The socket issuing the request to protocol
+  @param Request             The request issued by socket
+  @param RequestData         The request related data
+  
+  @retval EFI_SUCCESS        The socket request is completed successfully.
+  @retval other              The error status returned by the corresponding TCP
+                             layer function.
+                             
+**/
+typedef
+EFI_STATUS
+(*SOCK_PROTO_HANDLER) (
+  IN SOCKET       *Socket,
+  IN UINT8        Request,
+  IN VOID         *RequestData
+  );
+  
+  
+//
+// Socket provided oprerations for low layer protocol
+//
+
+//
+// Socket provided operations for user interface
+//
+
+/**
+  Set the state of the socket.
+
+  @param  Sock                  Pointer to the socket.
+  @param  State                 The new socket state to be set.
+
+**/
+VOID
+SockSetState (
+  IN OUT SOCKET     *Sock,
+  IN     UINT8      State
+  );
+
+/**
+  Called by the low layer protocol to indicate the socket a connection is 
+  established. 
+  
+  This function just changes the socket's state to SO_CONNECTED 
+  and signals the token used for connection establishment.
+
+  @param  Sock                  Pointer to the socket associated with the
+                                established connection.
+                                
+**/
+VOID
+SockConnEstablished (
+  IN SOCKET *Sock
+  );
+
+/**
+  Called by the low layer protocol to indicate the connection is closed.
+  
+  This function flushes the socket, sets the state to SO_CLOSED and signals 
+  the close token.
+
+  @param  Sock                  Pointer to the socket associated with the closed
+                                connection.
+                                
+**/
+VOID
+SockConnClosed (
+  IN OUT SOCKET *Sock
+  );
+
+/**
+  Called by low layer protocol to indicate that some data is sent or processed.
+   
+  This function trims the sent data in the socket send buffer, signals the data 
+  token if proper.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Count                 The length of the data processed or sent, in bytes.
+
+**/
+VOID
+SockDataSent (
+  IN SOCKET     *Sock,
+  IN UINT32     Count
+  );
+
+/**
+  Called by the low layer protocol to copy some data in socket send
+  buffer starting from the specific offset to a buffer provided by
+  the caller.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Offset                The start point of the data to be copied.
+  @param  Len                   The length of the data to be copied.
+  @param  Dest                  Pointer to the destination to copy the data.
+
+  @return The data size copied.
+
+**/
+UINT32
+SockGetDataToSend (
+  IN SOCKET      *Sock,
+  IN UINT32      Offset,
+  IN UINT32      Len,
+  IN UINT8       *Dest
+  );
+
+/**
+  Called by the low layer protocol to indicate that there
+  will be no more data from the communication peer.
+  
+  This function set the socket's state to SO_NO_MORE_DATA and
+  signal all queued IO tokens with the error status EFI_CONNECTION_FIN.
+
+  @param  Sock                  Pointer to the socket.
+
+**/
+VOID
+SockNoMoreData (
+  IN OUT SOCKET *Sock
+  );
+
+/**
+  Called by the low layer protocol to deliver received data to socket layer.
+  
+  This function will append the data to the socket receive buffer, set ther 
+  urgent data length and then check if any receive token can be signaled.
+
+  @param  Sock                  Pointer to the socket.
+  @param  NetBuffer             Pointer to the buffer that contains the received
+                                data.
+  @param  UrgLen                The length of the urgent data in the received data.
+
+**/
+VOID
+SockDataRcvd (
+  IN     SOCKET    *Sock,
+  IN OUT NET_BUF   *NetBuffer,
+  IN     UINT32    UrgLen
+  );
+
+/**
+  Get the length of the free space of the specific socket buffer.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Which                 Flag to indicate which socket buffer to check,
+                                either send buffer or receive buffer.
+
+  @return The length of the free space, in bytes.
+
+**/
+UINT32
+SockGetFreeSpace (
+  IN SOCKET  *Sock,
+  IN UINT32  Which
+  );
+
+/**
+  Clone a new socket including its associated protocol control block.
+
+  @param  Sock                  Pointer to the socket to be cloned.
+
+  @return Pointer to the newly cloned socket. If NULL, error condition occurred.
+
+**/
+SOCKET *
+SockClone (
+  IN SOCKET *Sock
+  );
+
+/**
+  Signal the receive token with the specific error or
+  set socket error code after error is received.
+
+  @param  Sock                  Pointer to the socket.
+  @param  Error                 The error code received.
+
+**/
+VOID
+SockRcvdErr (
+  IN OUT SOCKET       *Sock,
+  IN     EFI_STATUS   Error
+  );
+
+///
+/// Proto type of the create callback
+///
+typedef
+EFI_STATUS
+(*SOCK_CREATE_CALLBACK) (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  );
+  
+///
+/// Proto type of the destroy callback 
+///
+typedef
+VOID
+(*SOCK_DESTROY_CALLBACK) (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  );
+
+///
+///  The initialize data for create a new socket.
+///
+typedef struct _SOCK_INIT_DATA {
+  SOCK_TYPE   Type;
+  UINT8       State;
+
+  SOCKET      *Parent;        ///< The parent of this socket
+  UINT32      BackLog;        ///< The connection limit for listening socket
+  UINT32      SndBufferSize;  ///< The high warter mark of send buffer
+  UINT32      RcvBufferSize;  ///< The high warter mark of receive buffer
+  VOID        *Protocol;      ///< The pointer to protocol function template
+                              ///< wanted to install on socket
+
+  //
+  // Callbacks after socket is created and before socket is to be destroyed.
+  //
+  SOCK_CREATE_CALLBACK   CreateCallback;  ///< Callback after created
+  SOCK_DESTROY_CALLBACK  DestroyCallback; ///< Callback before destroied
+  VOID                   *Context;        ///< The context of the callback
+
+  //
+  // Opaque protocol data.
+  //
+  VOID                   *ProtoData;
+  UINT32                 DataSize;
+
+  SOCK_PROTO_HANDLER     ProtoHandler;  ///< The handler of protocol for socket request
+
+  EFI_HANDLE   DriverBinding;           ///< The driver binding handle
+} SOCK_INIT_DATA;
+
+///
+///  The union type of TCP and UDP protocol.
+///  
+typedef union _NET_PROTOCOL {
+  EFI_TCP4_PROTOCOL TcpProtocol;   ///< Tcp protocol
+  EFI_UDP4_PROTOCOL UdpProtocol;   ///< Udp protocol
+} NET_PROTOCOL;
+
+///
+/// The socket structure representing a network service access point
+///
+struct _SOCKET {
+
+  //
+  // Socket description information
+  //
+  UINT32                Signature;      ///< Signature of the socket
+  EFI_HANDLE            SockHandle;     ///< The virtual handle of the socket
+  EFI_HANDLE            DriverBinding;  ///< Socket's driver binding protocol
+  EFI_DEVICE_PATH_PROTOCOL  *ParentDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  LIST_ENTRY                Link;  
+  UINT8                 ConfigureState;
+  SOCK_TYPE             Type;
+  UINT8                 State;
+  UINT16                Flag;
+  EFI_LOCK              Lock;       ///< The lock of socket
+  SOCK_BUFFER           SndBuffer;  ///< Send buffer of application's data
+  SOCK_BUFFER           RcvBuffer;  ///< Receive buffer of received data
+  EFI_STATUS            SockError;  ///< The error returned by low layer protocol
+  BOOLEAN               InDestroy;
+
+  //
+  // Fields used to manage the connection request
+  //
+  UINT32          BackLog;        ///< the limit of connection to this socket
+  UINT32          ConnCnt;        ///< the current count of connections to it
+  SOCKET          *Parent;        ///< listening parent that accept the connection
+  LIST_ENTRY      ConnectionList; ///< the connections maintained by this socket
+  
+  //
+  // The queue to buffer application's asynchronous token
+  //
+  LIST_ENTRY      ListenTokenList;
+  LIST_ENTRY      RcvTokenList;
+  LIST_ENTRY      SndTokenList;
+  LIST_ENTRY      ProcessingSndTokenList;
+
+  SOCK_COMPLETION_TOKEN *ConnectionToken; ///< app's token to signal if connected
+  SOCK_COMPLETION_TOKEN *CloseToken;      ///< app's token to signal if closed
+
+  //
+  // Interface for low level protocol
+  //
+  SOCK_PROTO_HANDLER    ProtoHandler;       ///< The request handler of protocol
+  UINT8 ProtoReserved[PROTO_RESERVED_LEN];  ///< Data fields reserved for protocol
+  NET_PROTOCOL          NetProtocol;        ///< TCP or UDP protocol socket used
+
+  //
+  // Callbacks after socket is created and before socket is to be destroyed.
+  //
+  SOCK_CREATE_CALLBACK   CreateCallback;    ///< Callback after created
+  SOCK_DESTROY_CALLBACK  DestroyCallback;   ///< Callback before destroied
+  VOID                   *Context;          ///< The context of the callback
+};
+
+///
+///  The token structure buffered in socket layer.
+///
+typedef struct _SOCK_TOKEN {
+  LIST_ENTRY            TokenList;      ///< The entry to add in the token list
+  SOCK_COMPLETION_TOKEN *Token;         ///< The application's token
+  UINT32                RemainDataLen;  ///< Unprocessed data length
+  SOCKET                *Sock;          ///< The poninter to the socket this token
+                                        ///< belongs to
+} SOCK_TOKEN;
+
+///
+/// Reserved data to access the NET_BUF delivered by UDP driver.
+///
+typedef struct _UDP_RSV_DATA {
+  EFI_TIME              TimeStamp;
+  EFI_UDP4_SESSION_DATA Session;
+} UDP_RSV_DATA;
+
+///
+/// Reserved data to access the NET_BUF delivered by TCP driver.
+///
+typedef struct _TCP_RSV_DATA {
+  UINT32 UrgLen;
+} TCP_RSV_DATA;
+
+/**
+  Create a socket and its associated protocol control block
+  with the intial data SockInitData and protocol specific
+  data ProtoData.
+
+  @param  SockInitData         Inital data to setting the socket.
+  
+  @return Pointer to the newly created socket. If NULL, error condition occured.
+
+**/
+SOCKET *
+SockCreateChild (
+  IN SOCK_INIT_DATA *SockInitData
+  );
+
+/**
+  Destroy the socket Sock and its associated protocol control block.
+
+  @param  Sock                 The socket to be destroyed.
+
+  @retval EFI_SUCCESS          The socket Sock is destroyed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+
+**/
+EFI_STATUS
+SockDestroyChild (
+  IN   SOCKET *Sock
+  );
+
+/**
+  Configure the specific socket Sock using configuration data ConfigData.
+
+  @param  Sock                 Pointer to the socket to be configured.
+  @param  ConfigData           Pointer to the configuration data.
+
+  @retval EFI_SUCCESS          The socket is configured successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket or the
+                               socket is already configured.
+
+**/
+EFI_STATUS
+SockConfigure (
+  IN SOCKET *Sock,
+  IN VOID   *ConfigData
+  );
+
+/**
+  Initiate a connection establishment process.
+
+  @param  Sock                 Pointer to the socket to initiate the initate the
+                               connection.
+  @param  Token                Pointer to the token used for the connection
+                               operation.
+
+  @retval EFI_SUCCESS          The connection is initialized successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not configured to
+                               be an active one, or the token is already in one of
+                               this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockConnect (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  );
+
+/**
+  Issue a listen token to get an existed connected network instance
+  or wait for a connection if there is none.
+
+  @param  Sock                 Pointer to the socket to accept connections.
+  @param  Token                The token to accept a connection.
+
+  @retval EFI_SUCCESS          Either a connection is accpeted or the Token is
+                               buffered for further acception.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not configured to
+                               be a passive one, or the token is already in one of
+                               this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the Token due to memory limit.
+
+**/
+EFI_STATUS
+SockAccept (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  );
+
+/**
+  Issue a token with data to the socket to send out.
+
+  @param  Sock                 Pointer to the socket to process the token with
+                               data.
+  @param  Token                The token with data that needs to send out.
+
+  @retval EFI_SUCCESS          The token is processed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the token due to memory limit.
+
+**/
+EFI_STATUS
+SockSend (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  );
+
+/**
+  Issue a token to get data from the socket.
+
+  @param  Sock                 Pointer to the socket to get data from.
+  @param  Token                The token to store the received data from the
+                               socket.
+
+  @retval EFI_SUCCESS          The token is processed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+  @retval EFI_CONNECTION_FIN   The connection is closed and there is no more data.
+  @retval EFI_OUT_OF_RESOURCE  Failed to buffer the token due to memory limit.
+
+**/
+EFI_STATUS
+SockRcv (
+  IN SOCKET *Sock,
+  IN VOID   *Token
+  );
+
+/**
+  Reset the socket and its associated protocol control block.
+
+  @param  Sock                 Pointer to the socket to be flushed.
+
+  @retval EFI_SUCCESS          The socket is flushed successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+
+**/
+EFI_STATUS
+SockFlush (
+  IN SOCKET *Sock
+  );
+
+/**
+  Close or abort the socket associated connection.
+
+  @param  Sock                 Pointer to the socket of the connection to close or
+                               abort.
+  @param  Token                The token for close operation.
+  @param  OnAbort              TRUE for aborting the connection, FALSE to close it.
+
+  @retval EFI_SUCCESS          The close or abort operation is initialized
+                               successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket, or the
+                               socket is closed, or the socket is not in a
+                               synchronized state , or the token is already in one
+                               of this socket's lists.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockClose (
+  IN SOCKET  *Sock,
+  IN VOID    *Token,
+  IN BOOLEAN OnAbort
+  );
+
+/**
+  Get the mode data of the low layer protocol.
+
+  @param  Sock                 Pointer to the socket to get mode data from.
+  @param  Mode                 Pointer to the data to store the low layer mode
+                               information.
+
+  @retval EFI_SUCCESS          The mode data is got successfully.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockGetMode (
+  IN     SOCKET *Sock,
+  IN OUT VOID   *Mode
+  );
+
+/**
+  Configure the low level protocol to join a multicast group for
+  this socket's connection.
+
+  @param  Sock                 Pointer to the socket of the connection to join the
+                               specific multicast group.
+  @param  GroupInfo            Pointer to the multicast group info.
+
+  @retval EFI_SUCCESS          The configuration is done successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockGroup (
+  IN SOCKET *Sock,
+  IN VOID   *GroupInfo
+  );
+
+/**
+  Add or remove route information in IP route table associated
+  with this socket.
+
+  @param  Sock                 Pointer to the socket associated with the IP route
+                               table to operate on.
+  @param  RouteInfo            Pointer to the route information to be processed.
+
+  @retval EFI_SUCCESS          The route table is updated successfully.
+  @retval EFI_ACCESS_DENIED    Failed to get the lock to access the socket.
+  @retval EFI_NO_MAPPING       The IP address configuration operation is  not
+                               finished.
+  @retval EFI_NOT_STARTED      The socket is not configured.
+
+**/
+EFI_STATUS
+SockRoute (
+  IN SOCKET    *Sock,
+  IN VOID      *RouteInfo
+  );
+
+//
+// Supporting function to operate on socket buffer
+//
+
+/**
+  Get the first buffer block in the specific socket buffer.
+
+  @param  Sockbuf               Pointer to the socket buffer.
+
+  @return Pointer to the first buffer in the queue. NULL if the queue is empty.
+
+**/
+NET_BUF *
+SockBufFirst (
+  IN SOCK_BUFFER *Sockbuf
+  );
+
+/**
+  Get the next buffer block in the specific socket buffer.
+
+  @param  Sockbuf               Pointer to the socket buffer.
+  @param  SockEntry             Pointer to the buffer block prior to the required
+                                one.
+
+  @return Pointer to the buffer block next to SockEntry. NULL if SockEntry is 
+          the tail or head entry.
+
+**/
+NET_BUF *
+SockBufNext (
+  IN SOCK_BUFFER *Sockbuf,
+  IN NET_BUF     *SockEntry
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dispatcher.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dispatcher.c
new file mode 100644
index 0000000000..5b327af721
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dispatcher.c
@@ -0,0 +1,727 @@
+/** @file
+  Tcp request dispatcher implementation.
+
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2005 - 2014, 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<BR>
+
+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 "Tcp4Main.h"
+
+#define TCP_COMP_VAL(Min, Max, Default, Val) \
+  ((((Val) <= (Max)) && ((Val) >= (Min))) ? (Val) : (Default))
+
+/**
+  Add or remove a route entry in the IP route table associated with this TCP instance.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  RouteInfo             Pointer to the route info to be processed.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_NOT_STARTED       The driver instance has not been started.
+  @retval EFI_NO_MAPPING        When using the default address, configuration(DHCP,
+                                BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_OUT_OF_RESOURCES  Could not add the entry to the routing table.
+  @retval EFI_NOT_FOUND         This route is not in the routing table
+                                (when RouteInfo->DeleteRoute is TRUE).
+  @retval EFI_ACCESS_DENIED     The route is already defined in the routing table
+                                (when RouteInfo->DeleteRoute is FALSE).
+**/
+EFI_STATUS
+Tcp4Route (
+  IN TCP_CB           *Tcb,
+  IN TCP4_ROUTE_INFO  *RouteInfo
+  )
+{
+  EFI_IP4_PROTOCOL  *Ip4;
+
+  Ip4 = Tcb->IpInfo->Ip.Ip4;
+
+  ASSERT (Ip4 != NULL);
+
+  return Ip4->Routes (
+              Ip4,
+              RouteInfo->DeleteRoute,
+              RouteInfo->SubnetAddress,
+              RouteInfo->SubnetMask,
+              RouteInfo->GatewayAddress
+              );
+
+}
+
+
+/**
+  Get the operational settings of this TCP instance.
+
+  @param  Tcb                    Pointer to the TCP_CB of this TCP instance.
+  @param  Mode                   Pointer to the buffer to store the operational
+                                 settings.
+
+  @retval EFI_SUCCESS            The mode data is read.
+  @retval EFI_NOT_STARTED        No configuration data is available because this
+                                 instance hasn't been started.
+
+**/
+EFI_STATUS
+Tcp4GetMode (
+  IN     TCP_CB         *Tcb,
+  IN OUT TCP4_MODE_DATA *Mode
+  )
+{
+  SOCKET                *Sock;
+  EFI_TCP4_CONFIG_DATA  *ConfigData;
+  EFI_TCP4_ACCESS_POINT *AccessPoint;
+  EFI_TCP4_OPTION       *Option;
+  EFI_IP4_PROTOCOL      *Ip;
+
+  Sock = Tcb->Sk;
+
+  if (!SOCK_IS_CONFIGURED (Sock) && (Mode->Tcp4ConfigData != NULL)) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (Mode->Tcp4State != NULL) {
+    *(Mode->Tcp4State) = (EFI_TCP4_CONNECTION_STATE) Tcb->State;
+  }
+
+  if (Mode->Tcp4ConfigData != NULL) {
+
+    ConfigData                      = Mode->Tcp4ConfigData;
+    AccessPoint                     = &(ConfigData->AccessPoint);
+    Option                          = ConfigData->ControlOption;
+
+    ConfigData->TypeOfService       = Tcb->Tos;
+    ConfigData->TimeToLive          = Tcb->Ttl;
+
+    AccessPoint->UseDefaultAddress  = Tcb->UseDefaultAddr;
+
+    IP4_COPY_ADDRESS (&AccessPoint->StationAddress, &Tcb->LocalEnd.Ip);
+    IP4_COPY_ADDRESS (&AccessPoint->SubnetMask, &Tcb->SubnetMask);
+    AccessPoint->StationPort        = NTOHS (Tcb->LocalEnd.Port);
+
+    IP4_COPY_ADDRESS (&AccessPoint->RemoteAddress, &Tcb->RemoteEnd.Ip);
+    AccessPoint->RemotePort         = NTOHS (Tcb->RemoteEnd.Port);
+    AccessPoint->ActiveFlag         = (BOOLEAN) (Tcb->State != TCP_LISTEN);
+
+    if (Option != NULL) {
+      Option->ReceiveBufferSize       = GET_RCV_BUFFSIZE (Tcb->Sk);
+      Option->SendBufferSize          = GET_SND_BUFFSIZE (Tcb->Sk);
+      Option->MaxSynBackLog           = GET_BACKLOG (Tcb->Sk);
+
+      Option->ConnectionTimeout       = Tcb->ConnectTimeout / TCP_TICK_HZ;
+      Option->DataRetries             = Tcb->MaxRexmit;
+      Option->FinTimeout              = Tcb->FinWait2Timeout / TCP_TICK_HZ;
+      Option->TimeWaitTimeout         = Tcb->TimeWaitTimeout / TCP_TICK_HZ;
+      Option->KeepAliveProbes         = Tcb->MaxKeepAlive;
+      Option->KeepAliveTime           = Tcb->KeepAliveIdle / TCP_TICK_HZ;
+      Option->KeepAliveInterval       = Tcb->KeepAlivePeriod / TCP_TICK_HZ;
+
+      Option->EnableNagle         = (BOOLEAN) (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_NAGLE));
+      Option->EnableTimeStamp     = (BOOLEAN) (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_TS));
+      Option->EnableWindowScaling = (BOOLEAN) (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_WS));
+
+      Option->EnableSelectiveAck      = FALSE;
+      Option->EnablePathMtuDiscovery  = FALSE;
+    }
+  }
+
+  Ip = Tcb->IpInfo->Ip.Ip4;
+  ASSERT (Ip != NULL);
+
+  return Ip->GetModeData (Ip, Mode->Ip4ModeData, Mode->MnpConfigData, Mode->SnpModeData);
+}
+
+
+/**
+  If AP->StationPort isn't zero, check whether the access point
+  is registered, else generate a random station port for this
+  access point.
+
+  @param  AP                     Pointer to the access point.
+
+  @retval EFI_SUCCESS            The check is passed or the port is assigned.
+  @retval EFI_INVALID_PARAMETER  The non-zero station port is already used.
+  @retval EFI_OUT_OF_RESOURCES   No port can be allocated.
+
+**/
+EFI_STATUS
+Tcp4Bind (
+  IN EFI_TCP4_ACCESS_POINT *AP
+  )
+{
+  BOOLEAN Cycle;
+
+  if (0 != AP->StationPort) {
+    //
+    // check if a same endpoint is bound
+    //
+    if (TcpFindTcbByPeer (&AP->StationAddress, AP->StationPort)) {
+
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    // generate a random port
+    //
+    Cycle = FALSE;
+
+    if (TCP4_PORT_USER_RESERVED == mTcp4RandomPort) {
+      mTcp4RandomPort = TCP4_PORT_KNOWN;
+    }
+
+    mTcp4RandomPort++;
+
+    while (TcpFindTcbByPeer (&AP->StationAddress, mTcp4RandomPort)) {
+
+      mTcp4RandomPort++;
+
+      if (mTcp4RandomPort <= TCP4_PORT_KNOWN) {
+
+        if (Cycle) {
+          DEBUG ((EFI_D_ERROR, "Tcp4Bind: no port can be allocated "
+            "for this pcb\n"));
+
+          return EFI_OUT_OF_RESOURCES;
+        }
+
+        mTcp4RandomPort = TCP4_PORT_KNOWN + 1;
+
+        Cycle = TRUE;
+      }
+
+    }
+
+    AP->StationPort = mTcp4RandomPort;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Flush the Tcb add its associated protocols.
+
+  @param  Tcb                    Pointer to the TCP_CB to be flushed.
+
+**/
+VOID
+Tcp4FlushPcb (
+  IN TCP_CB *Tcb
+  )
+{
+  SOCKET           *Sock;
+
+  IpIoConfigIp (Tcb->IpInfo, NULL);
+
+  Sock     = Tcb->Sk;
+
+  if (SOCK_IS_CONFIGURED (Sock)) {
+    RemoveEntryList (&Tcb->List);
+
+    //
+    // Uninstall the device path protocol.
+    //
+    if (Sock->DevicePath != NULL) {
+      gBS->UninstallProtocolInterface (
+             Sock->SockHandle,
+             &gEfiDevicePathProtocolGuid,
+             Sock->DevicePath
+             );
+      FreePool (Sock->DevicePath);
+    }
+  }
+
+  NetbufFreeList (&Tcb->SndQue);
+  NetbufFreeList (&Tcb->RcvQue);
+  Tcb->State = TCP_CLOSED;
+}
+
+/**
+  Attach a Pcb to the socket.
+
+  @param  Sk                     Pointer to the socket of this TCP instance.
+
+  @retval EFI_SUCCESS            The operation is completed successfully.
+  @retval EFI_OUT_OF_RESOURCES   Failed due to resource limit.
+
+**/
+EFI_STATUS
+Tcp4AttachPcb (
+  IN SOCKET  *Sk
+  )
+{
+  TCP_CB            *Tcb;
+  TCP4_PROTO_DATA   *ProtoData;
+  IP_IO             *IpIo;
+  EFI_STATUS        Status;
+  VOID              *Ip;
+
+  Tcb = AllocateZeroPool (sizeof (TCP_CB));
+
+  if (Tcb == NULL) {
+
+    DEBUG ((EFI_D_ERROR, "Tcp4ConfigurePcb: failed to allocate a TCB\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  ProtoData  = (TCP4_PROTO_DATA *) Sk->ProtoReserved;
+  IpIo       = ProtoData->TcpService->IpIo;
+
+  //
+  // Create an IpInfo for this Tcb.
+  //
+  Tcb->IpInfo = IpIoAddIp (IpIo);
+  if (Tcb->IpInfo == NULL) {
+
+    FreePool (Tcb);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Open the new created IP instance BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  Tcb->IpInfo->ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  &Ip,
+                  IpIo->Image,
+                  Sk->SockHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    IpIoRemoveIp (IpIo, Tcb->IpInfo);
+    return Status;
+  }
+
+  InitializeListHead (&Tcb->List);
+  InitializeListHead (&Tcb->SndQue);
+  InitializeListHead (&Tcb->RcvQue);
+
+  Tcb->State        = TCP_CLOSED;
+  Tcb->Sk           = Sk;
+  ProtoData->TcpPcb = Tcb;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Detach the Pcb of the socket.
+
+  @param  Sk                     Pointer to the socket of this TCP instance.
+
+**/
+VOID
+Tcp4DetachPcb (
+  IN SOCKET  *Sk
+  )
+{
+  TCP4_PROTO_DATA  *ProtoData;
+  TCP_CB           *Tcb;
+
+  ProtoData = (TCP4_PROTO_DATA *) Sk->ProtoReserved;
+  Tcb       = ProtoData->TcpPcb;
+
+  ASSERT (Tcb != NULL);
+
+  Tcp4FlushPcb (Tcb);
+
+  //
+  // Close the IP protocol.
+  //
+  gBS->CloseProtocol (
+         Tcb->IpInfo->ChildHandle,
+         &gEfiIp4ProtocolGuid,
+         ProtoData->TcpService->IpIo->Image,
+         Sk->SockHandle
+         );
+  
+  IpIoRemoveIp (ProtoData->TcpService->IpIo, Tcb->IpInfo);
+
+  FreePool (Tcb);
+
+  ProtoData->TcpPcb = NULL;
+}
+
+
+/**
+  Configure the Pcb using CfgData.
+
+  @param  Sk                     Pointer to the socket of this TCP instance.
+  @param  CfgData                Pointer to the TCP configuration data.
+
+  @retval EFI_SUCCESS            The operation is completed successfully.
+  @retval EFI_INVALID_PARAMETER  A same access point has been configured in
+                                 another TCP instance.
+  @retval EFI_OUT_OF_RESOURCES   Failed due to resource limit.
+
+**/
+EFI_STATUS
+Tcp4ConfigurePcb (
+  IN SOCKET               *Sk,
+  IN EFI_TCP4_CONFIG_DATA *CfgData
+  )
+{
+  EFI_IP4_CONFIG_DATA IpCfgData;
+  EFI_STATUS          Status;
+  EFI_TCP4_OPTION     *Option;
+  TCP4_PROTO_DATA     *TcpProto;
+  TCP_CB              *Tcb;
+
+  ASSERT ((CfgData != NULL) && (Sk != NULL) && (Sk->SockHandle != NULL));
+
+  TcpProto = (TCP4_PROTO_DATA *) Sk->ProtoReserved;
+  Tcb      = TcpProto->TcpPcb;
+
+  ASSERT (Tcb != NULL);
+
+  //
+  // Add Ip for send pkt to the peer
+  //
+  CopyMem (&IpCfgData, &mIp4IoDefaultIpConfigData, sizeof (IpCfgData));
+  IpCfgData.DefaultProtocol   = EFI_IP_PROTO_TCP;
+  IpCfgData.UseDefaultAddress = CfgData->AccessPoint.UseDefaultAddress;
+  IpCfgData.StationAddress    = CfgData->AccessPoint.StationAddress;
+  IpCfgData.SubnetMask        = CfgData->AccessPoint.SubnetMask;
+  IpCfgData.ReceiveTimeout    = (UINT32) (-1);
+
+  //
+  // Configure the IP instance this Tcb consumes.
+  //
+  Status = IpIoConfigIp (Tcb->IpInfo, &IpCfgData);
+  if (EFI_ERROR (Status)) {
+    goto OnExit;
+  }
+
+  //
+  // Get the default address info if the instance is configured to use default address.
+  //
+  if (CfgData->AccessPoint.UseDefaultAddress) {
+    CfgData->AccessPoint.StationAddress = IpCfgData.StationAddress;
+    CfgData->AccessPoint.SubnetMask     = IpCfgData.SubnetMask;
+  }
+
+  //
+  // check if we can bind this endpoint in CfgData
+  //
+  Status = Tcp4Bind (&(CfgData->AccessPoint));
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "Tcp4ConfigurePcb: Bind endpoint failed "
+      "with %r\n", Status));
+
+    goto OnExit;
+  }
+
+  //
+  // Initalize the operating information in this Tcb
+  //
+  ASSERT (Tcb->State == TCP_CLOSED &&
+    IsListEmpty (&Tcb->SndQue) &&
+    IsListEmpty (&Tcb->RcvQue));
+
+  TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_NO_KEEPALIVE);
+  Tcb->State            = TCP_CLOSED;
+
+  Tcb->SndMss           = 536;
+  Tcb->RcvMss           = TcpGetRcvMss (Sk);
+
+  Tcb->SRtt             = 0;
+  Tcb->Rto              = 3 * TCP_TICK_HZ;
+
+  Tcb->CWnd             = Tcb->SndMss;
+  Tcb->Ssthresh         = 0xffffffff;
+
+  Tcb->CongestState     = TCP_CONGEST_OPEN;
+
+  Tcb->KeepAliveIdle    = TCP_KEEPALIVE_IDLE_MIN;
+  Tcb->KeepAlivePeriod  = TCP_KEEPALIVE_PERIOD;
+  Tcb->MaxKeepAlive     = TCP_MAX_KEEPALIVE;
+  Tcb->MaxRexmit        = TCP_MAX_LOSS;
+  Tcb->FinWait2Timeout  = TCP_FIN_WAIT2_TIME;
+  Tcb->TimeWaitTimeout  = TCP_TIME_WAIT_TIME;
+  Tcb->ConnectTimeout   = TCP_CONNECT_TIME;
+
+  //
+  // initialize Tcb in the light of CfgData
+  //
+  Tcb->Ttl            = CfgData->TimeToLive;
+  Tcb->Tos            = CfgData->TypeOfService;
+
+  Tcb->UseDefaultAddr = CfgData->AccessPoint.UseDefaultAddress;
+
+  CopyMem (&Tcb->LocalEnd.Ip, &CfgData->AccessPoint.StationAddress, sizeof (IP4_ADDR));
+  Tcb->LocalEnd.Port  = HTONS (CfgData->AccessPoint.StationPort);
+  IP4_COPY_ADDRESS (&Tcb->SubnetMask, &CfgData->AccessPoint.SubnetMask);
+
+  if (CfgData->AccessPoint.ActiveFlag) {
+    CopyMem (&Tcb->RemoteEnd.Ip, &CfgData->AccessPoint.RemoteAddress, sizeof (IP4_ADDR));
+    Tcb->RemoteEnd.Port = HTONS (CfgData->AccessPoint.RemotePort);
+  } else {
+    Tcb->RemoteEnd.Ip   = 0;
+    Tcb->RemoteEnd.Port = 0;
+  }
+
+  Option              = CfgData->ControlOption;
+
+  if (Option != NULL) {
+    SET_RCV_BUFFSIZE (
+      Sk,
+      (UINT32) (TCP_COMP_VAL (
+                  TCP_RCV_BUF_SIZE_MIN,
+                  TCP_RCV_BUF_SIZE,
+                  TCP_RCV_BUF_SIZE,
+                  Option->ReceiveBufferSize
+                  )
+               )
+      );
+    SET_SND_BUFFSIZE (
+      Sk,
+      (UINT32) (TCP_COMP_VAL (
+                  TCP_SND_BUF_SIZE_MIN,
+                  TCP_SND_BUF_SIZE,
+                  TCP_SND_BUF_SIZE,
+                  Option->SendBufferSize
+                  )
+               )
+      );
+
+    SET_BACKLOG (
+      Sk,
+      (UINT32) (TCP_COMP_VAL (
+                  TCP_BACKLOG_MIN,
+                  TCP_BACKLOG,
+                  TCP_BACKLOG,
+                  Option->MaxSynBackLog
+                  )
+               )
+      );
+
+    Tcb->MaxRexmit = (UINT16) TCP_COMP_VAL (
+                                TCP_MAX_LOSS_MIN,
+                                TCP_MAX_LOSS,
+                                TCP_MAX_LOSS,
+                                Option->DataRetries
+                                );
+    Tcb->FinWait2Timeout = TCP_COMP_VAL (
+                              TCP_FIN_WAIT2_TIME,
+                              TCP_FIN_WAIT2_TIME_MAX,
+                              TCP_FIN_WAIT2_TIME,
+                              (UINT32) (Option->FinTimeout * TCP_TICK_HZ)
+                              );
+
+    if (Option->TimeWaitTimeout != 0) {
+      Tcb->TimeWaitTimeout = TCP_COMP_VAL (
+                               TCP_TIME_WAIT_TIME,
+                               TCP_TIME_WAIT_TIME_MAX,
+                               TCP_TIME_WAIT_TIME,
+                               (UINT32) (Option->TimeWaitTimeout * TCP_TICK_HZ)
+                               );
+    } else {
+      Tcb->TimeWaitTimeout = 0;
+    }
+
+    if (Option->KeepAliveProbes != 0) {
+      TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_NO_KEEPALIVE);
+
+      Tcb->MaxKeepAlive = (UINT8) TCP_COMP_VAL (
+                                    TCP_MAX_KEEPALIVE_MIN,
+                                    TCP_MAX_KEEPALIVE,
+                                    TCP_MAX_KEEPALIVE,
+                                    Option->KeepAliveProbes
+                                    );
+      Tcb->KeepAliveIdle = TCP_COMP_VAL (
+                             TCP_KEEPALIVE_IDLE_MIN,
+                             TCP_KEEPALIVE_IDLE_MAX,
+                             TCP_KEEPALIVE_IDLE_MIN,
+                             (UINT32) (Option->KeepAliveTime * TCP_TICK_HZ)
+                             );
+      Tcb->KeepAlivePeriod = TCP_COMP_VAL (
+                               TCP_KEEPALIVE_PERIOD_MIN,
+                               TCP_KEEPALIVE_PERIOD,
+                               TCP_KEEPALIVE_PERIOD,
+                               (UINT32) (Option->KeepAliveInterval * TCP_TICK_HZ)
+                               );
+    }
+
+    Tcb->ConnectTimeout = TCP_COMP_VAL (
+                            TCP_CONNECT_TIME_MIN,
+                            TCP_CONNECT_TIME,
+                            TCP_CONNECT_TIME,
+                            (UINT32) (Option->ConnectionTimeout * TCP_TICK_HZ)
+                            );
+
+    if (!Option->EnableNagle) {
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_NO_NAGLE);
+    }
+
+    if (!Option->EnableTimeStamp) {
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_NO_TS);
+    }
+
+    if (!Option->EnableWindowScaling) {
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_NO_WS);
+    }
+  }
+
+  //
+  // The socket is bound, the <SrcIp, SrcPort, DstIp, DstPort> is
+  // determined, construct the IP device path and install it.
+  //
+  Status = TcpInstallDevicePath (Sk);
+  if (EFI_ERROR (Status)) {
+    goto OnExit;
+  }
+
+  //
+  // update state of Tcb and socket
+  //
+  if (!CfgData->AccessPoint.ActiveFlag) {
+
+    TcpSetState (Tcb, TCP_LISTEN);
+    SockSetState (Sk, SO_LISTENING);
+
+    Sk->ConfigureState = SO_CONFIGURED_PASSIVE;
+  } else {
+
+    Sk->ConfigureState = SO_CONFIGURED_ACTIVE;
+  }
+
+  TcpInsertTcb (Tcb);
+
+OnExit:
+
+  return Status;
+}
+
+
+/**
+  The procotol handler provided to the socket layer, used to
+  dispatch the socket level requests by calling the corresponding
+  TCP layer functions.
+
+  @param  Sock                   Pointer to the socket of this TCP instance.
+  @param  Request                The code of this operation request.
+  @param  Data                   Pointer to the operation specific data passed in
+                                 together with the operation request.
+
+  @retval EFI_SUCCESS            The socket request is completed successfully.
+  @retval other                  The error status returned by the corresponding TCP
+                                 layer function.
+
+**/
+EFI_STATUS
+Tcp4Dispatcher (
+  IN SOCKET                  *Sock,
+  IN UINT8                   Request,
+  IN VOID                    *Data    OPTIONAL
+  )
+{
+  TCP_CB            *Tcb;
+  TCP4_PROTO_DATA   *ProtoData;
+  EFI_IP4_PROTOCOL  *Ip;
+
+  ProtoData = (TCP4_PROTO_DATA *) Sock->ProtoReserved;
+  Tcb       = ProtoData->TcpPcb;
+
+  switch (Request) {
+  case SOCK_POLL:
+    Ip = ProtoData->TcpService->IpIo->Ip.Ip4;
+    Ip->Poll (Ip);
+    break;
+
+  case SOCK_CONSUMED:
+    //
+    // After user received data from socket buffer, socket will
+    // notify TCP using this message to give it a chance to send out
+    // window update information
+    //
+    ASSERT (Tcb != NULL);
+    TcpOnAppConsume (Tcb);
+    break;
+
+  case SOCK_SND:
+
+    ASSERT (Tcb != NULL);
+    TcpOnAppSend (Tcb);
+    break;
+
+  case SOCK_CLOSE:
+
+    TcpOnAppClose (Tcb);
+
+    break;
+
+  case SOCK_ABORT:
+
+    TcpOnAppAbort (Tcb);
+
+    break;
+
+  case SOCK_SNDPUSH:
+    Tcb->SndPsh = TcpGetMaxSndNxt (Tcb) + GET_SND_DATASIZE (Tcb->Sk);
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_SND_PSH);
+
+    break;
+
+  case SOCK_SNDURG:
+    Tcb->SndUp = TcpGetMaxSndNxt (Tcb) + GET_SND_DATASIZE (Tcb->Sk) - 1;
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_SND_URG);
+
+    break;
+
+  case SOCK_CONNECT:
+
+    TcpOnAppConnect (Tcb);
+
+    break;
+
+  case SOCK_ATTACH:
+
+    return Tcp4AttachPcb (Sock);
+
+  case SOCK_FLUSH:
+
+    Tcp4FlushPcb (Tcb);
+
+    break;
+
+  case SOCK_DETACH:
+
+    Tcp4DetachPcb (Sock);
+
+    break;
+
+  case SOCK_CONFIGURE:
+
+    return Tcp4ConfigurePcb (
+            Sock,
+            (EFI_TCP4_CONFIG_DATA *) Data
+            );
+
+  case SOCK_MODE:
+
+    ASSERT ((Data != NULL) && (Tcb != NULL));
+
+    return Tcp4GetMode (Tcb, (TCP4_MODE_DATA *) Data);
+
+  case SOCK_ROUTE:
+
+    ASSERT ((Data != NULL) && (Tcb != NULL));
+
+    return Tcp4Route (Tcb, (TCP4_ROUTE_INFO *) Data);
+
+  default:
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.c
new file mode 100644
index 0000000000..30e9406161
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.c
@@ -0,0 +1,782 @@
+/** @file
+  Tcp driver function.
+
+Copyright (c) 2005 - 2014, 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<BR>
+
+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 "Tcp4Main.h"
+
+
+UINT16                                mTcp4RandomPort;
+extern EFI_COMPONENT_NAME_PROTOCOL    gTcp4ComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL   gTcp4ComponentName2;
+extern EFI_UNICODE_STRING_TABLE       *gTcpControllerNameTable;
+
+TCP4_HEARTBEAT_TIMER  mTcp4Timer = {
+  NULL,
+  0
+};
+
+EFI_TCP4_PROTOCOL mTcp4ProtocolTemplate = {
+  Tcp4GetModeData,
+  Tcp4Configure,
+  Tcp4Routes,
+  Tcp4Connect,
+  Tcp4Accept,
+  Tcp4Transmit,
+  Tcp4Receive,
+  Tcp4Close,
+  Tcp4Cancel,
+  Tcp4Poll
+};
+
+SOCK_INIT_DATA mTcp4DefaultSockData = {
+  SockStream,
+  0,
+  NULL,
+  TCP_BACKLOG,
+  TCP_SND_BUF_SIZE,
+  TCP_RCV_BUF_SIZE,
+  &mTcp4ProtocolTemplate,
+  Tcp4CreateSocketCallback,
+  Tcp4DestroySocketCallback,
+  NULL,
+  NULL,
+  0,
+  Tcp4Dispatcher,
+  NULL,
+};
+
+EFI_DRIVER_BINDING_PROTOCOL mTcp4DriverBinding = {
+  Tcp4DriverBindingSupported,
+  Tcp4DriverBindingStart,
+  Tcp4DriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_SERVICE_BINDING_PROTOCOL mTcp4ServiceBinding = {
+  Tcp4ServiceBindingCreateChild,
+  Tcp4ServiceBindingDestroyChild
+};
+
+
+/**
+  Create and start the heartbeat timer for TCP driver.
+
+  @retval EFI_SUCCESS            The timer is successfully created and started.
+  @retval other                  The timer is not created.
+
+**/
+EFI_STATUS
+Tcp4CreateTimer (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = EFI_SUCCESS;
+
+  if (mTcp4Timer.RefCnt == 0) {
+
+    Status = gBS->CreateEvent (
+                    EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                    TPL_NOTIFY,
+                    TcpTicking,
+                    NULL,
+                    &mTcp4Timer.TimerEvent
+                    );
+    if (!EFI_ERROR (Status)) {
+
+      Status = gBS->SetTimer (
+                      mTcp4Timer.TimerEvent,
+                      TimerPeriodic,
+                      (UINT64) (TICKS_PER_SECOND / TCP_TICK_HZ)
+                      );
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+
+    mTcp4Timer.RefCnt++;
+  }
+
+  return Status;
+}
+
+
+/**
+  Stop and destroy the heartbeat timer for TCP driver.
+  
+**/
+VOID
+Tcp4DestroyTimer (
+  VOID
+  )
+{
+  ASSERT (mTcp4Timer.RefCnt > 0);
+
+  mTcp4Timer.RefCnt--;
+
+  if (mTcp4Timer.RefCnt > 0) {
+    return;
+  }
+
+  gBS->SetTimer (mTcp4Timer.TimerEvent, TimerCancel, 0);
+  gBS->CloseEvent (mTcp4Timer.TimerEvent);
+  mTcp4Timer.TimerEvent = NULL;
+}
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DestroyChildEntryInHandleBuffer (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  SOCKET                        *Sock;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock = NET_LIST_USER_STRUCT_S (Entry, SOCKET, Link, SOCK_SIGNATURE);
+  ServiceBinding    = ((TCP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ServiceBinding;
+  NumberOfChildren  = ((TCP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->NumberOfChildren;
+  ChildHandleBuffer = ((TCP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ChildHandleBuffer;
+
+  if (!NetIsInHandleBuffer (Sock->SockHandle, NumberOfChildren, ChildHandleBuffer)) {
+    return EFI_SUCCESS;
+  }
+
+  return ServiceBinding->DestroyChild (ServiceBinding, Sock->SockHandle);
+}
+
+/**
+  The entry point for Tcp4 driver, used to install Tcp4 driver on the ImageHandle.
+
+  @param  ImageHandle   The firmware allocated handle for this
+                        driver image.
+  @param  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  UINT32      Seed;
+
+  //
+  // Install the TCP4 Driver Binding Protocol
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &mTcp4DriverBinding,
+             ImageHandle,
+             &gTcp4ComponentName,
+             &gTcp4ComponentName2
+             );
+  ASSERT_EFI_ERROR (Status);
+  //
+  // Initialize ISS and random port.
+  //
+  Seed            = NetRandomInitSeed ();
+  mTcpGlobalIss   = NET_RANDOM (Seed) % mTcpGlobalIss;
+  mTcp4RandomPort = (UINT16) (TCP4_PORT_KNOWN +
+                    (UINT16) (NET_RANDOM(Seed) % TCP4_PORT_KNOWN));
+
+  return Status;
+}
+
+
+/**
+  Tests to see if this driver supports a given controller.
+  
+  If a child device is provided, it further tests to see if this driver supports 
+  creating a handle for the specified child device.
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle     The handle of the controller to test. This handle 
+                               must support a protocol interface that supplies 
+                               an I/O abstraction to the driver.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                               This parameter is ignored by device drivers, and is optional for bus drivers.
+
+
+  @retval EFI_SUCCESS          The device specified by ControllerHandle and
+                               RemainingDevicePath is supported by the driver 
+                               specified by This.
+  @retval EFI_ALREADY_STARTED  The device specified by ControllerHandle and
+                               RemainingDevicePath is already being managed by 
+                               the driver specified by This.
+  @retval EFI_ACCESS_DENIED    The device specified by ControllerHandle and
+                               RemainingDevicePath is already being managed by a 
+                               different driver or an application that requires 
+                               exclusive access.
+  @retval EFI_UNSUPPORTED      The device specified by ControllerHandle and
+                               RemainingDevicePath is not supported by the driver 
+                               specified by This.
+                               
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Test for the Tcp4ServiceBinding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiTcp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Test for the Ip4 Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiIp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle. 
+  
+  The Start() function is designed to be invoked from the EFI boot service 
+  ConnectController(). As a result, much of the error checking on the parameters 
+  to Start() has been moved into this common boot service. It is legal to call 
+  Start() from other locations, but the following calling restrictions must be 
+  followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally 
+     aligned EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified 
+     by This must have been called with the same calling parameters, and Supported() 
+     must have returned EFI_SUCCESS.
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle     The handle of the controller to start. This handle 
+                               must support a protocol interface that supplies 
+                               an I/O abstraction to the driver.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                               This parameter is ignored by device drivers, and is 
+                               optional for bus drivers.
+
+  @retval EFI_SUCCESS          The device was started.
+  @retval EFI_ALREADY_STARTED  The device could not be started due to a device error.
+  @retval EFI_OUT_OF_RESOURCES The request could not be completed due to a lack 
+                               of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS               Status;
+  TCP4_SERVICE_DATA        *TcpServiceData;
+  IP_IO_OPEN_DATA          OpenData;
+
+  TcpServiceData = AllocateZeroPool (sizeof (TCP4_SERVICE_DATA));
+
+  if (NULL == TcpServiceData) {
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingStart: Have no enough"
+      " resource to create a Tcp Servcie Data\n"));
+
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Create a new IP IO to Consume it
+  //
+  TcpServiceData->IpIo = IpIoCreate (
+                           This->DriverBindingHandle,
+                           ControllerHandle,
+                           IP_VERSION_4
+                           );
+  if (NULL == TcpServiceData->IpIo) {
+
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingStart: Have no enough"
+      " resource to create an Ip Io\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  //
+  // Configure and start IpIo.
+  //
+  ZeroMem (&OpenData, sizeof (IP_IO_OPEN_DATA));
+
+  CopyMem (
+    &OpenData.IpConfigData.Ip4CfgData,
+    &mIp4IoDefaultIpConfigData,
+    sizeof (EFI_IP4_CONFIG_DATA)
+    );
+
+  OpenData.IpConfigData.Ip4CfgData.DefaultProtocol = EFI_IP_PROTO_TCP;
+
+  OpenData.PktRcvdNotify = Tcp4RxCallback;
+  Status                 = IpIoOpen (TcpServiceData->IpIo, &OpenData);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Create the timer event used by TCP driver
+  //
+  Status = Tcp4CreateTimer ();
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingStart: Create TcpTimer"
+      " Event failed with %r\n", Status));
+
+    goto ON_ERROR;
+  }
+
+  //
+  // Install the Tcp4ServiceBinding Protocol on the
+  // controller handle
+  //
+  TcpServiceData->Tcp4ServiceBinding = mTcp4ServiceBinding;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiTcp4ServiceBindingProtocolGuid,
+                  &TcpServiceData->Tcp4ServiceBinding,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingStart: Install Tcp4 Service Binding"
+      " Protocol failed for %r\n", Status));
+
+    Tcp4DestroyTimer ();
+    goto ON_ERROR;
+  }
+
+  //
+  // Initialize member in TcpServiceData
+  //
+  TcpServiceData->ControllerHandle    = ControllerHandle;
+  TcpServiceData->Signature           = TCP4_DRIVER_SIGNATURE;
+  TcpServiceData->DriverBindingHandle = This->DriverBindingHandle;
+
+  InitializeListHead (&TcpServiceData->SocketList);
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  if (TcpServiceData->IpIo != NULL) {
+    IpIoDestroy (TcpServiceData->IpIo);
+    TcpServiceData->IpIo = NULL;
+  }
+
+  FreePool (TcpServiceData);
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle.
+  
+  The Stop() function is designed to be invoked from the EFI boot service 
+  DisconnectController(). As a result, much of the error checking on the parameters 
+  to Stop() has been moved into this common boot service. It is legal to call Stop() 
+  from other locations, but the following calling restrictions must be followed 
+  or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call 
+     to this same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this 
+     driver's Start() function, and the Start() function must have called OpenProtocol() 
+     on ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle  A handle to the device being stopped. The handle must 
+                            support a bus specific I/O protocol for the driver 
+                            to use to stop the device.
+  @param  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer An array of child handles to be freed. May be NULL if 
+                            NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS       The device was stopped.
+  @retval EFI_DEVICE_ERROR  The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_HANDLE                                NicHandle;
+  EFI_SERVICE_BINDING_PROTOCOL              *ServiceBinding;
+  TCP4_SERVICE_DATA                         *TcpServiceData;
+  LIST_ENTRY                                *List;
+  TCP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT  Context;
+
+  // Find the NicHandle where Tcp4 ServiceBinding Protocol is installed.
+  //
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiIp4ProtocolGuid);
+  if (NicHandle == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Retrieve the TCP driver Data Structure
+  //
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiTcp4ServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingStop: Locate Tcp4 Service "
+      " Binding Protocol failed with %r\n", Status));
+
+    return EFI_DEVICE_ERROR;
+  }
+
+  TcpServiceData = TCP4_FROM_THIS (ServiceBinding);
+
+  if (NumberOfChildren != 0) {
+    List = &TcpServiceData->SocketList; 
+    Context.ServiceBinding = ServiceBinding;
+    Context.NumberOfChildren = NumberOfChildren;
+    Context.ChildHandleBuffer = ChildHandleBuffer;
+    Status = NetDestroyLinkList (
+               List,
+               Tcp4DestroyChildEntryInHandleBuffer,
+               &Context,
+               NULL
+               );
+  } else if (IsListEmpty (&TcpServiceData->SocketList)) {
+    //
+    // Uninstall TCP servicebinding protocol
+    //
+    gBS->UninstallMultipleProtocolInterfaces (
+           NicHandle,
+           &gEfiTcp4ServiceBindingProtocolGuid,
+           ServiceBinding,
+           NULL
+           );
+
+    //
+    // Destroy the IpIO consumed by TCP driver
+    //
+    IpIoDestroy (TcpServiceData->IpIo);
+    TcpServiceData->IpIo = NULL;
+
+    //
+    // Destroy the heartbeat timer.
+    //
+    Tcp4DestroyTimer ();
+
+    if (gTcpControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gTcpControllerNameTable);
+      gTcpControllerNameTable = NULL;
+    }
+    
+    //
+    // Release the TCP service data
+    //
+    FreePool (TcpServiceData);
+
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+/**
+  Open Ip4 and device path protocols for a created socket, and insert it in 
+  socket list.
+  
+  @param  This                Pointer to the socket just created
+  @param  Context             Context of the socket
+  
+  @retval EFI_SUCCESS         This protocol is installed successfully.
+  @retval other               Some error occured.
+  
+**/
+EFI_STATUS
+Tcp4CreateSocketCallback (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  )
+{
+  EFI_STATUS         Status;
+  TCP4_SERVICE_DATA  *TcpServiceData;
+  EFI_IP4_PROTOCOL   *Ip4;
+
+  TcpServiceData = ((TCP4_PROTO_DATA *) This->ProtoReserved)->TcpService;
+
+  //
+  // Open the default Ip4 protocol of IP_IO BY_DRIVER.
+  //
+  Status = gBS->OpenProtocol (
+                  TcpServiceData->IpIo->ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **) &Ip4,
+                  TcpServiceData->DriverBindingHandle,
+                  This->SockHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Open the device path on the handle where service binding resides on.
+  //
+  Status = gBS->OpenProtocol (
+                  TcpServiceData->ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &This->ParentDevicePath,
+                  TcpServiceData->DriverBindingHandle,
+                  This->SockHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    gBS->CloseProtocol (
+           TcpServiceData->IpIo->ChildHandle,
+           &gEfiIp4ProtocolGuid,
+           TcpServiceData->DriverBindingHandle,
+           This->SockHandle
+           );
+  } else {
+    //
+    // Insert this socket into the SocketList.
+    //
+    InsertTailList (&TcpServiceData->SocketList, &This->Link);
+  }
+
+  return Status;
+}
+
+/**
+  Close Ip4 and device path protocols for a socket, and remove it from socket list. 
+    
+  @param  This                Pointer to the socket to be removed
+  @param  Context             Context of the socket
+  
+**/
+VOID
+Tcp4DestroySocketCallback (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  )
+{
+  TCP4_SERVICE_DATA  *TcpServiceData;
+
+  TcpServiceData = ((TCP4_PROTO_DATA *) This->ProtoReserved)->TcpService;
+
+  //
+  // Remove this node from the list.
+  //
+  RemoveEntryList (&This->Link);
+
+  //
+  // Close the Ip4 protocol.
+  //
+  gBS->CloseProtocol (
+         TcpServiceData->IpIo->ChildHandle,
+         &gEfiIp4ProtocolGuid,
+         TcpServiceData->DriverBindingHandle,
+         This->SockHandle
+         );
+}
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. If ChildHandle 
+  is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing 
+  ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL, then 
+                      a new handle is created. If it is a pointer to an existing UEFI 
+                      handle, then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child.
+  @retval other                 The child handle was not created.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4ServiceBindingCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                    *ChildHandle
+  )
+{
+  SOCKET            *Sock;
+  TCP4_SERVICE_DATA *TcpServiceData;
+  TCP4_PROTO_DATA   TcpProto;
+  EFI_STATUS        Status;
+  EFI_TPL           OldTpl;
+
+  if (NULL == This || NULL == ChildHandle) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OldTpl              = gBS->RaiseTPL (TPL_CALLBACK);
+  Status              = EFI_SUCCESS;
+  TcpServiceData      = TCP4_FROM_THIS (This);
+  TcpProto.TcpService = TcpServiceData;
+  TcpProto.TcpPcb     = NULL;
+
+  //
+  // Create a tcp instance with defualt Tcp default
+  // sock init data and TcpProto
+  //
+  mTcp4DefaultSockData.ProtoData     = &TcpProto;
+  mTcp4DefaultSockData.DataSize      = sizeof (TCP4_PROTO_DATA);
+  mTcp4DefaultSockData.DriverBinding = TcpServiceData->DriverBindingHandle;
+
+  Sock = SockCreateChild (&mTcp4DefaultSockData);
+  if (NULL == Sock) {
+    DEBUG ((EFI_D_ERROR, "Tcp4DriverBindingCreateChild: "
+      "No resource to create a Tcp Child\n"));
+
+    Status = EFI_OUT_OF_RESOURCES;
+  } else {
+    *ChildHandle = Sock->SockHandle;
+  }
+
+  mTcp4DefaultSockData.ProtoData = NULL;
+
+  gBS->RestoreTPL (OldTpl);
+  return Status;
+}
+
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This         Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle  Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is 
+                                being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed.
+  
+**/
+EFI_STATUS
+EFIAPI
+Tcp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  )
+{
+  EFI_STATUS         Status;
+  EFI_TCP4_PROTOCOL  *Tcp4;
+  SOCKET             *Sock;
+
+  if (NULL == This || NULL == ChildHandle) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // retrieve the Tcp4 protocol from ChildHandle
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiTcp4ProtocolGuid,
+                  (VOID **) &Tcp4,
+                  mTcp4DriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    Status = EFI_UNSUPPORTED;
+  } else {
+    //
+    // destroy this sock and related Tcp protocol control
+    // block
+    //
+    Sock = SOCK_FROM_THIS (Tcp4);
+
+    SockDestroyChild (Sock);
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.h
new file mode 100644
index 0000000000..5fe98d4068
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Driver.h
@@ -0,0 +1,342 @@
+/** @file
+  Tcp driver function header.
+
+Copyright (c) 2005 - 2014, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _TCP4_DRIVER_H_
+#define _TCP4_DRIVER_H_
+
+#include <Protocol/ServiceBinding.h>
+#include <Library/IpIoLib.h>
+
+#define TCP4_DRIVER_SIGNATURE   SIGNATURE_32 ('T', 'C', 'P', '4')
+
+#define TCP4_PORT_KNOWN         1024
+#define TCP4_PORT_USER_RESERVED 65535
+
+#define TCP4_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  TCP4_SERVICE_DATA, \
+  Tcp4ServiceBinding, \
+  TCP4_DRIVER_SIGNATURE \
+  )
+  
+///
+/// TCP heartbeat tick timer.
+///
+typedef struct _TCP4_HEARTBEAT_TIMER {
+  EFI_EVENT  TimerEvent;         ///< The event assoiated with the timer
+  INTN       RefCnt;             ///< Number of reference
+} TCP4_HEARTBEAT_TIMER;
+
+///
+/// TCP service data
+///
+typedef struct _TCP4_SERVICE_DATA {
+  UINT32                        Signature;
+  EFI_HANDLE                    ControllerHandle;
+  IP_IO                         *IpIo;  // IP Io consumed by TCP4
+  EFI_SERVICE_BINDING_PROTOCOL  Tcp4ServiceBinding;
+  EFI_HANDLE                    DriverBindingHandle;
+  LIST_ENTRY                    SocketList;
+} TCP4_SERVICE_DATA;
+
+///
+/// TCP protocol data
+///
+typedef struct _TCP4_PROTO_DATA {
+  TCP4_SERVICE_DATA *TcpService;
+  TCP_CB            *TcpPcb;
+} TCP4_PROTO_DATA;
+
+
+/**
+  Packet receive callback function provided to IP_IO, used to call
+  the proper function to handle the packet received by IP.
+
+  @param  Status      Status of the received packet.
+  @param  IcmpErr     ICMP error number.
+  @param  NetSession  Pointer to the net session of this packet.
+  @param  Pkt         Pointer to the recieved packet.
+  @param  Context     Pointer to the context configured in IpIoOpen(), not used
+                      now.
+
+  @return None
+
+**/
+VOID
+EFIAPI
+Tcp4RxCallback (
+  IN EFI_STATUS                       Status,
+  IN UINT8                            IcmpErr,
+  IN EFI_NET_SESSION_DATA             *NetSession,
+  IN NET_BUF                          *Pkt,
+  IN VOID                             *Context    OPTIONAL
+  );
+
+/**
+  Send the segment to IP via IpIo function.
+
+  @param  Tcb         Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf        Pointer to the TCP segment to be sent.
+  @param  Src         Source address of the TCP segment.
+  @param  Dest        Destination address of the TCP segment.
+
+  @retval 0           The segment was sent out successfully.
+  @retval -1          The segment was failed to send.
+
+**/
+INTN
+TcpSendIpPacket (
+  IN TCP_CB    *Tcb,
+  IN NET_BUF   *Nbuf,
+  IN UINT32    Src,
+  IN UINT32    Dest
+  );
+
+/**
+  The procotol handler provided to the socket layer, used to
+  dispatch the socket level requests by calling the corresponding
+  TCP layer functions.
+
+  @param  Sock                   Pointer to the socket of this TCP instance.
+  @param  Request                The code of this operation request.
+  @param  Data                   Pointer to the operation specific data passed in
+                                 together with the operation request.
+
+  @retval EFI_SUCCESS            The socket request is completed successfully.
+  @retval other                  The error status returned by the corresponding TCP
+                                 layer function.
+
+**/
+EFI_STATUS
+Tcp4Dispatcher (
+  IN SOCKET                  *Sock,
+  IN UINT8                   Request,
+  IN VOID                    *Data    OPTIONAL
+  );
+
+
+/**
+  The entry point for Tcp4 driver, used to install Tcp4 driver on the ImageHandle.
+
+  @param  ImageHandle   The firmware allocated handle for this
+                        driver image.
+  @param  SystemTable   Pointer to the EFI system table.
+
+  @retval EFI_SUCCESS   Driver loaded.
+  @retval other         Driver not loaded.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  );
+
+
+/**
+  Tests to see if this driver supports a given controller.
+  
+  If a child device is provided, it further tests to see if this driver supports 
+  creating a handle for the specified child device.
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle     The handle of the controller to test. This handle 
+                               must support a protocol interface that supplies 
+                               an I/O abstraction to the driver.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                               This parameter is ignored by device drivers, and is optional for bus drivers.
+
+
+  @retval EFI_SUCCESS          The device specified by ControllerHandle and
+                               RemainingDevicePath is supported by the driver 
+                               specified by This.
+  @retval EFI_ALREADY_STARTED  The device specified by ControllerHandle and
+                               RemainingDevicePath is already being managed by 
+                               the driver specified by This.
+  @retval EFI_ACCESS_DENIED    The device specified by ControllerHandle and
+                               RemainingDevicePath is already being managed by a 
+                               different driver or an application that requires 
+                               exclusive access.
+  @retval EFI_UNSUPPORTED      The device specified by ControllerHandle and
+                               RemainingDevicePath is not supported by the driver 
+                               specified by This.
+                               
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. 
+  
+  The Start() function is designed to be invoked from the EFI boot service 
+  ConnectController(). As a result, much of the error checking on the parameters 
+  to Start() has been moved into this common boot service. It is legal to call 
+  Start() from other locations, but the following calling restrictions must be 
+  followed or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE.
+  2. If RemainingDevicePath is not NULL, then it must be a pointer to a naturally 
+     aligned EFI_DEVICE_PATH_PROTOCOL.
+  3. Prior to calling Start(), the Supported() function for the driver specified 
+     by This must have been called with the same calling parameters, and Supported() 
+     must have returned EFI_SUCCESS.
+
+  @param  This                 A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle     The handle of the controller to start. This handle 
+                               must support a protocol interface that supplies 
+                               an I/O abstraction to the driver.
+  @param  RemainingDevicePath  A pointer to the remaining portion of a device path. 
+                               This parameter is ignored by device drivers, and is 
+                               optional for bus drivers.
+
+  @retval EFI_SUCCESS          The device was started.
+  @retval EFI_ALREADY_STARTED  The device could not be started due to a device error.
+  @retval EFI_OUT_OF_RESOURCES The request could not be completed due to a lack 
+                               of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+  
+  The Stop() function is designed to be invoked from the EFI boot service 
+  DisconnectController(). As a result, much of the error checking on the parameters 
+  to Stop() has been moved into this common boot service. It is legal to call Stop() 
+  from other locations, but the following calling restrictions must be followed 
+  or the system behavior will not be deterministic.
+  1. ControllerHandle must be a valid EFI_HANDLE that was used on a previous call 
+     to this same driver's Start() function.
+  2. The first NumberOfChildren handles of ChildHandleBuffer must all be a valid
+     EFI_HANDLE. In addition, all of these handles must have been created in this 
+     driver's Start() function, and the Start() function must have called OpenProtocol() 
+     on ControllerHandle with an Attribute of EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER.
+  
+  @param  This              A pointer to the EFI_DRIVER_BINDING_PROTOCOL instance.
+  @param  ControllerHandle  A handle to the device being stopped. The handle must 
+                            support a bus specific I/O protocol for the driver 
+                            to use to stop the device.
+  @param  NumberOfChildren  The number of child device handles in ChildHandleBuffer.
+  @param  ChildHandleBuffer An array of child handles to be freed. May be NULL if 
+                            NumberOfChildren is 0.
+
+  @retval EFI_SUCCESS       The device was stopped.
+  @retval EFI_DEVICE_ERROR  The device could not be stopped due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Open Ip4 and device path protocols for a created socket, and insert it in 
+  socket list.
+  
+  @param  This                Pointer to the socket just created
+  @param  Context             Context of the socket
+  
+  @retval EFI_SUCCESS         This protocol is installed successfully.
+  @retval other               Some error occured.
+  
+**/
+EFI_STATUS
+Tcp4CreateSocketCallback (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  );
+
+/**
+  Close Ip4 and device path protocols for a socket, and remove it from socket list. 
+    
+  @param  This                Pointer to the socket to be removed
+  @param  Context             Context of the socket
+  
+**/
+VOID
+Tcp4DestroySocketCallback (
+  IN SOCKET  *This,
+  IN VOID    *Context
+  );
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. If ChildHandle 
+  is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing 
+  ChildHandle.
+
+  @param  This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle Pointer to the handle of the child to create. If it is NULL, then 
+                      a new handle is created. If it is a pointer to an existing UEFI 
+                      handle, then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child.
+  @retval other                 The child handle was not created.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4ServiceBindingCreateChild (
+  IN     EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN OUT EFI_HANDLE                    *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param  This         Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param  ChildHandle  Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is 
+                                being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed.
+  
+**/
+EFI_STATUS
+EFIAPI
+Tcp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.inf b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.inf
new file mode 100644
index 0000000000..ba3c377b5f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.inf
@@ -0,0 +1,84 @@
+## @file
+#  This module produces EFI TCPv4 Protocol and EFI TCPv4 Service Binding Protocol.
+#
+#  This module produces EFI TCPv4(Transmission Control Protocol version 4) Protocol
+#  upon EFI IPv4 Protocol, to provide basic TCPv4 I/O services.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Tcp4Dxe
+  MODULE_UNI_FILE                = Tcp4Dxe.uni
+  FILE_GUID                      = 6d6963ab-906d-4a65-a7ca-bd40e5d6af4d
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = Tcp4DriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  mTcp4DriverBinding                        
+#  COMPONENT_NAME                =  gTcp4ComponentName
+#  COMPONENT_NAME2               =  gTcp4ComponentName2
+#
+
+[Sources]
+  SockImpl.c
+  SockInterface.c
+  Tcp4Proto.h
+  Tcp4Main.h
+  SockImpl.h
+  Tcp4Output.c
+  Tcp4Timer.c
+  Tcp4Option.h
+  Tcp4Dispatcher.c
+  Tcp4Input.c
+  Tcp4Misc.c
+  Tcp4Main.c
+  Socket.h
+  ComponentName.c
+  Tcp4Driver.h
+  Tcp4Io.c
+  Tcp4Driver.c
+  Tcp4Func.h
+  Tcp4Option.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  DebugLib
+  NetLib
+  IpIoLib
+  DevicePathLib
+  DpcLib
+
+[Protocols]
+  gEfiTcp4ServiceBindingProtocolGuid            ## BY_START
+  gEfiIp4ServiceBindingProtocolGuid             ## TO_START
+  gEfiTcp4ProtocolGuid                          ## BY_START
+  gEfiIp4ProtocolGuid                           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  Tcp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.uni b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.uni
new file mode 100644
index 0000000000..a1789dcd65
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4DxeExtra.uni
new file mode 100644
index 0000000000..7690fe5a13
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Func.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Func.h
new file mode 100644
index 0000000000..53b7aac8ae
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Func.h
@@ -0,0 +1,781 @@
+/** @file
+  Tcp function header file.
+
+Copyright (c) 2005 - 2014, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _TCP4_FUNC_H_
+#define _TCP4_FUNC_H_
+
+//
+// Declaration of all the functions in TCP
+// protocol. It is intended to keep tcp.h
+// clear.
+//
+
+//
+// Functions in tcp.c
+//
+
+/**
+  Try to find one Tcb whose <Ip, Port> equals to <IN Addr, IN Port>.
+
+  @param  Addr                  Pointer to the IP address needs to match.
+  @param  Port                  The port number needs to match.
+
+  @return  The Tcb which matches the <Addr Port> paire exists or not.
+
+**/
+BOOLEAN
+TcpFindTcbByPeer (
+  IN EFI_IPv4_ADDRESS  *Addr,
+  IN TCP_PORTNO        Port
+  );
+
+/**
+  Locate the TCP_CB related to the socket pair.
+
+  @param  LocalPort             The local port number.
+  @param  LocalIp               The local IP address.
+  @param  RemotePort            The remote port number.
+  @param  RemoteIp              The remote IP address.
+  @param  Syn                   Whether to search the listen sockets, if TRUE, the
+                                listen sockets are searched.
+
+  @return  Pointer to the related TCP_CB, if NULL no match is found.
+
+**/
+TCP_CB *
+TcpLocateTcb (
+  IN TCP_PORTNO  LocalPort,
+  IN UINT32      LocalIp,
+  IN TCP_PORTNO  RemotePort,
+  IN UINT32      RemoteIp,
+  IN BOOLEAN     Syn
+  );
+
+/**
+  Insert a Tcb into the proper queue.
+
+  @param  Tcb                   Pointer to the TCP_CB to be inserted.
+
+  @retval 0                     The Tcb is inserted successfully.
+  @retval -1                    Error condition occurred.
+
+**/
+INTN
+TcpInsertTcb (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Clone a TCP_CB from Tcb.
+
+  @param  Tcb                   Pointer to the TCP_CB to be cloned.
+
+  @return  Pointer to the new cloned TCP_CB, if NULL error condition occurred.
+
+**/
+TCP_CB *
+TcpCloneTcb (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Compute an ISS to be used by a new connection.
+
+  @return  The result ISS.
+
+**/
+TCP_SEQNO
+TcpGetIss (
+  VOID
+  );
+
+/**
+  Initialize the Tcb local related members.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpInitTcbLocal (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Initialize the peer related members.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  Seg                   Pointer to the segment that contains the peer's
+                                intial info.
+  @param  Opt                   Pointer to the options announced by the peer.
+
+**/
+VOID
+TcpInitTcbPeer (
+  IN OUT TCP_CB     *Tcb,
+  IN     TCP_SEG    *Seg,
+  IN     TCP_OPTION *Opt
+  );
+
+/**
+  Get the local mss.
+
+  @param  Sock        Pointer to the socket to get mss
+
+  @return  The mss size.
+
+**/
+UINT16
+TcpGetRcvMss (
+  IN SOCKET  *Sock
+  );
+
+/**
+  Set the Tcb's state.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  State                 The state to be set.
+
+**/
+VOID
+TcpSetState (
+  IN OUT TCP_CB    *Tcb,
+  IN     UINT8     State
+  );
+
+//
+// Functions in Tcp4Output.c
+//
+/**
+  Send the segment to IP via IpIo function.
+
+  @param  Tcb         Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf        Pointer to the TCP segment to be sent.
+  @param  Src         Source address of the TCP segment.
+  @param  Dest        Destination address of the TCP segment.
+
+  @retval 0           The segment was sent out successfully.
+  @retval -1          The segment was failed to send.
+
+**/
+INTN
+TcpSendIpPacket (
+  IN TCP_CB    *Tcb,
+  IN NET_BUF   *Nbuf,
+  IN UINT32    Src,
+  IN UINT32    Dest
+  );
+
+/**
+  Check whether to send data/SYN/FIN and piggy back an ACK.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Force   Whether to ignore the sender's SWS avoidance algorithm and send
+                  out data by force.
+
+  @return The number of bytes sent.
+
+**/
+INTN
+TcpToSendData (
+  IN OUT TCP_CB *Tcb,
+  IN     INTN Force
+  );
+
+/**
+  Check whether to send an ACK or delayed ACK.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpToSendAck (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Send an ACK immediately.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSendAck (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Send a zero probe segment. It can be used by keepalive and zero window probe.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0       The zero probe segment was sent out successfully.
+  @retval other   Error condition occurred.
+
+**/
+INTN
+TcpSendZeroProbe (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Process the data and FIN flag, check whether to deliver
+  data to the socket layer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0        No error occurred to deliver data.
+  @retval -1       Error condition occurred. Proper response is to reset the
+                   connection.
+
+**/
+INTN
+TcpDeliverData (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Send a RESET segment in response to the segment received.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance, may be NULL.
+  @param  Head    TCP header of the segment that triggers the reset.
+  @param  Len     Length of the segment that triggers the reset.
+  @param  Local   Local IP address.
+  @param  Remote  Remote peer's IP address.
+
+  @retval 0       A reset is sent or no need to send it.
+  @retval -1      No reset is sent.
+
+**/
+INTN
+TcpSendReset (
+  IN TCP_CB    *Tcb,
+  IN TCP_HEAD  *Head,
+  IN INT32     Len,
+  IN UINT32    Local,
+  IN UINT32    Remote
+  );
+
+/**
+  Compute the sequence space left in the old receive window.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The sequence space left in the old receive window.
+
+**/
+UINT32
+TcpRcvWinOld (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Compute the current receive window.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The size of the current receive window, in bytes.
+
+**/
+UINT32
+TcpRcvWinNow (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Retransmit the segment from sequence Seq.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment to be retransmitted.
+
+  @retval 0       Retransmission succeeded.
+  @retval -1      Error condition occurred.
+
+**/
+INTN
+TcpRetransmit (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq
+  );
+
+/**
+  Compute how much data to send.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Force   Whether to ignore the sender's SWS avoidance algorithm and send
+                  out data by force.
+
+  @return The length of the data can be sent, if 0, no data can be sent.
+
+**/
+UINT32
+TcpDataToSend (
+  IN TCP_CB *Tcb,
+  IN INTN   Force
+  );
+
+/**
+  Verify that the segment is in good shape.
+
+  @param  Nbuf    Buffer that contains the segment to be checked.
+
+  @retval 0       The segment is broken.
+  @retval 1       The segment is in good shape.
+
+**/
+INTN
+TcpVerifySegment (
+  IN NET_BUF *Nbuf
+  );
+
+/**
+  Verify that all the segments in SndQue are in good shape.
+
+  @param  Head    Pointer to the head node of the SndQue.
+
+  @retval 0       At least one segment is broken.
+  @retval 1       All segments in the specific queue are in good shape.
+
+**/
+INTN
+TcpCheckSndQue (
+  IN LIST_ENTRY     *Head
+  );
+
+/**
+  Get a segment from the Tcb's SndQue.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegmentSndQue (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  );
+
+/**
+  Get a segment from the Tcb's socket buffer.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegmentSock (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  );
+
+/**
+  Get a segment starting from sequence Seq of a maximum
+  length of Len.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegment (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  );
+
+/**
+  Get the maximum SndNxt.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The sequence number of the maximum SndNxt.
+
+**/
+TCP_SEQNO
+TcpGetMaxSndNxt (
+  IN TCP_CB *Tcb
+  );
+
+//
+// Functions from Tcp4Input.c
+//
+/**
+  Process the received ICMP error messages for TCP.
+
+  @param  Nbuf     Buffer that contains part of the TCP segment without IP header
+                   truncated from the ICMP error packet.
+  @param  IcmpErr  The ICMP error code interpreted from ICMP error packet.
+  @param  Src      Source address of the ICMP error message.
+  @param  Dst      Destination address of the ICMP error message.
+
+**/
+VOID
+TcpIcmpInput (
+  IN NET_BUF     *Nbuf,
+  IN UINT8       IcmpErr,
+  IN UINT32      Src,
+  IN UINT32      Dst
+  );
+
+/**
+  Process the received TCP segments.
+
+  @param  Nbuf     Buffer that contains received TCP segment without IP header.
+  @param  Src      Source address of the segment, or the peer's IP address.
+  @param  Dst      Destination address of the segment, or the local end's IP
+                   address.
+
+  @retval 0        Segment is processed successfully. It is either accepted or
+                   discarded. But no connection is reset by the segment.
+  @retval -1       A connection is reset by the segment.
+
+**/
+INTN
+TcpInput (
+  IN NET_BUF *Nbuf,
+  IN UINT32  Src,
+  IN UINT32  Dst
+  );
+
+/**
+  Check whether the sequence number of the incoming segment is acceptable.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Pointer to the incoming segment.
+
+  @retval 1       The sequence number is acceptable.
+  @retval 0       The sequence number is not acceptable.
+
+**/
+INTN
+TcpSeqAcceptable (
+  IN TCP_CB  *Tcb,
+  IN TCP_SEG *Seg
+  );
+
+/**
+  NewReno fast recovery, RFC3782.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Segment that triggers the fast recovery.
+
+**/
+VOID
+TcpFastRecover (
+  IN OUT TCP_CB  *Tcb,
+  IN     TCP_SEG *Seg
+  );
+
+/**
+  NewReno fast loss recovery, RFC3792.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Segment that triggers the fast loss recovery.
+
+**/
+VOID
+TcpFastLossRecover (
+  IN OUT TCP_CB  *Tcb,
+  IN     TCP_SEG *Seg
+  );
+
+/**
+  Compute the RTT as specified in RFC2988.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Measure  Currently measured RTT in heart beats.
+
+**/
+VOID
+TcpComputeRtt (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT32 Measure
+  );
+
+/**
+  Trim off the data outside the tcb's receive window.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf     Pointer to the NET_BUF containing the received tcp segment.
+
+**/
+VOID
+TcpTrimInWnd (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  );
+
+/**
+  Store the data into the reassemble queue.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf     Pointer to the buffer containing the data to be queued.
+
+**/
+VOID
+TcpQueueData (
+  IN OUT TCP_CB  *Tcb,
+  IN     NET_BUF *Nbuf
+  );
+
+/**
+  Ajust the send queue or the retransmit queue.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Ack      The acknowledge seuqence number of the received segment.
+
+**/
+VOID
+TcpAdjustSndQue (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Ack
+  );
+
+//
+// Functions from Tcp4Misc.c
+//
+/**
+  Compute the TCP segment's checksum.
+
+  @param  Nbuf                  Pointer to the buffer that contains the TCP
+                                segment.
+  @param  HeadSum               The checksum value of the fixed part of pseudo
+                                header.
+
+  @return  The checksum value.
+
+**/
+UINT16
+TcpChecksum (
+  IN NET_BUF *Nbuf,
+  IN UINT16  HeadSum
+  );
+
+/**
+  Translate the information from the head of the received TCP
+  segment Nbuf contains and fill it into a TCP_SEG structure.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf                  Pointer to the buffer contains the TCP segment.
+
+  @return  Pointer to the TCP_SEG that contains the translated TCP head information.
+
+**/
+TCP_SEG *
+TcpFormatNetbuf (
+  IN     TCP_CB  *Tcb,
+  IN OUT NET_BUF *Nbuf
+  );
+
+/**
+  Initialize an active connection.
+
+  @param  Tcb                   Pointer to the TCP_CB that wants to initiate a
+                                connection.
+
+**/
+VOID
+TcpOnAppConnect (
+  IN OUT TCP_CB  *Tcb
+  );
+
+/**
+  Application has consumed some data, check whether
+  to send a window updata ack or a delayed ack.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpOnAppConsume (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Initiate the connection close procedure, called when
+  applications want to close the connection.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpOnAppClose (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Check whether the application's newly delivered data can be sent out.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0                     Whether the data is sent out or is buffered for
+                                further sending.
+  @retval -1                    The Tcb is not in a state that data is permitted to
+                                be sent out.
+
+**/
+INTN
+TcpOnAppSend (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Abort the connection by sending a reset segment, called
+  when the application wants to abort the connection.
+
+  @param  Tcb                   Pointer to the TCP_CB of the TCP instance.
+
+**/
+VOID
+TcpOnAppAbort (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Reset the connection related with Tcb.
+
+  @param  Tcb                   Pointer to the TCP_CB of the connection to be
+                                reset.
+
+**/
+VOID
+TcpResetConnection (
+  IN TCP_CB *Tcb
+  );
+
+//
+// Functions in Tcp4Timer.c
+//
+/**
+  Close the TCP connection.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpClose (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Heart beat timer handler, queues the DPC at TPL_CALLBACK.
+
+  @param  Event    Timer event signaled, ignored.
+  @param  Context  Context of the timer event, ignored.
+
+**/
+VOID
+EFIAPI
+TcpTicking (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  );
+
+/**
+  Enable a TCP timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Timer    The index of the timer to be enabled.
+  @param  TimeOut  The timeout value of this timer.
+
+**/
+VOID
+TcpSetTimer (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT16 Timer,
+  IN     UINT32 TimeOut
+  );
+
+/**
+  Clear one TCP timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Timer    The index of the timer to be cleared.
+
+**/
+VOID
+TcpClearTimer (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT16 Timer
+  );
+
+/**
+  Clear all TCP timers.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpClearAllTimer (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Enable the window prober timer and set the timeout value.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSetProbeTimer (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Enable the keepalive timer and set the timeout value.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSetKeepaliveTimer (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Backoff the RTO.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpBackoffRto (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Install the device path protocol on the TCP instance.
+
+  @param  Sock             Pointer to the socket representing the TCP instance.
+
+  @retval  EFI_SUCCESS     The device path protocol is installed.
+  @retval  other           Failed to install the device path protocol.
+
+**/
+EFI_STATUS
+TcpInstallDevicePath (
+  IN SOCKET *Sock
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Input.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Input.c
new file mode 100644
index 0000000000..209127d18f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Input.c
@@ -0,0 +1,1468 @@
+/** @file
+  TCP input process routines.
+
+Copyright (c) 2005 - 2015, 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<BR>
+
+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 "Tcp4Main.h"
+
+
+/**
+  Check whether the sequence number of the incoming segment is acceptable.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Pointer to the incoming segment.
+
+  @retval 1       The sequence number is acceptable.
+  @retval 0       The sequence number is not acceptable.
+
+**/
+INTN
+TcpSeqAcceptable (
+  IN TCP_CB  *Tcb,
+  IN TCP_SEG *Seg
+  )
+{
+  return (TCP_SEQ_LEQ (Tcb->RcvWl2, Seg->End) &&
+          TCP_SEQ_LT (Seg->Seq, Tcb->RcvWl2 + Tcb->RcvWnd));
+}
+
+
+/**
+  NewReno fast recovery, RFC3782.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Segment that triggers the fast recovery.
+
+**/
+VOID
+TcpFastRecover (
+  IN OUT TCP_CB  *Tcb,
+  IN     TCP_SEG *Seg
+  )
+{
+  UINT32  FlightSize;
+  UINT32  Acked;
+
+  //
+  // Step 1: Three duplicate ACKs and not in fast recovery
+  //
+  if (Tcb->CongestState != TCP_CONGEST_RECOVER) {
+
+    //
+    // Step 1A: Invoking fast retransmission.
+    //
+    FlightSize        = TCP_SUB_SEQ (Tcb->SndNxt, Tcb->SndUna);
+
+    Tcb->Ssthresh     = MAX (FlightSize >> 1, (UINT32) (2 * Tcb->SndMss));
+    Tcb->Recover      = Tcb->SndNxt;
+
+    Tcb->CongestState = TCP_CONGEST_RECOVER;
+    TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_RTT_ON);
+
+    //
+    // Step 2: Entering fast retransmission
+    //
+    TcpRetransmit (Tcb, Tcb->SndUna);
+    Tcb->CWnd = Tcb->Ssthresh + 3 * Tcb->SndMss;
+
+    DEBUG ((EFI_D_INFO, "TcpFastRecover: enter fast retransmission"
+      " for TCB %p, recover point is %d\n", Tcb, Tcb->Recover));
+    return;
+  }
+
+  //
+  // During fast recovery, execute Step 3, 4, 5 of RFC3782
+  //
+  if (Seg->Ack == Tcb->SndUna) {
+
+    //
+    // Step 3: Fast Recovery,
+    // If this is a duplicated ACK, increse Cwnd by SMSS.
+    //
+
+    // Step 4 is skipped here only to be executed later
+    // by TcpToSendData
+    //
+    Tcb->CWnd += Tcb->SndMss;
+    DEBUG ((EFI_D_INFO, "TcpFastRecover: received another"
+      " duplicated ACK (%d) for TCB %p\n", Seg->Ack, Tcb));
+
+  } else {
+
+    //
+    // New data is ACKed, check whether it is a
+    // full ACK or partial ACK
+    //
+    if (TCP_SEQ_GEQ (Seg->Ack, Tcb->Recover)) {
+
+      //
+      // Step 5 - Full ACK:
+      // deflate the congestion window, and exit fast recovery
+      //
+      FlightSize        = TCP_SUB_SEQ (Tcb->SndNxt, Tcb->SndUna);
+
+      Tcb->CWnd         = MIN (Tcb->Ssthresh, FlightSize + Tcb->SndMss);
+
+      Tcb->CongestState = TCP_CONGEST_OPEN;
+      DEBUG ((EFI_D_INFO, "TcpFastRecover: received a full ACK(%d)"
+        " for TCB %p, exit fast recovery\n", Seg->Ack, Tcb));
+
+    } else {
+
+      //
+      // Step 5 - Partial ACK:
+      // fast retransmit the first unacknowledge field
+      // , then deflate the CWnd
+      //
+      TcpRetransmit (Tcb, Seg->Ack);
+      Acked = TCP_SUB_SEQ (Seg->Ack, Tcb->SndUna);
+
+      //
+      // Deflate the CWnd by the amount of new data
+      // ACKed by SEG.ACK. If more than one SMSS data
+      // is ACKed, add back SMSS byte to CWnd after
+      //
+      if (Acked >= Tcb->SndMss) {
+        Acked -= Tcb->SndMss;
+
+      }
+
+      Tcb->CWnd -= Acked;
+
+      DEBUG ((EFI_D_INFO, "TcpFastRecover: received a partial"
+        " ACK(%d) for TCB %p\n", Seg->Ack, Tcb));
+
+    }
+  }
+}
+
+
+/**
+  NewReno fast loss recovery, RFC3792.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Seg      Segment that triggers the fast loss recovery.
+
+**/
+VOID
+TcpFastLossRecover (
+  IN OUT TCP_CB  *Tcb,
+  IN     TCP_SEG *Seg
+  )
+{
+  if (TCP_SEQ_GT (Seg->Ack, Tcb->SndUna)) {
+
+    //
+    // New data is ACKed, check whether it is a
+    // full ACK or partial ACK
+    //
+    if (TCP_SEQ_GEQ (Seg->Ack, Tcb->LossRecover)) {
+
+      //
+      // Full ACK: exit the loss recovery.
+      //
+      Tcb->LossTimes    = 0;
+      Tcb->CongestState = TCP_CONGEST_OPEN;
+
+      DEBUG ((EFI_D_INFO, "TcpFastLossRecover: received a "
+        "full ACK(%d) for TCB %p\n", Seg->Ack, Tcb));
+
+    } else {
+
+      //
+      // Partial ACK:
+      // fast retransmit the first unacknowledge field.
+      //
+      TcpRetransmit (Tcb, Seg->Ack);
+      DEBUG ((EFI_D_INFO, "TcpFastLossRecover: received a "
+        "partial ACK(%d) for TCB %p\n", Seg->Ack, Tcb));
+    }
+  }
+}
+
+
+/**
+  Compute the RTT as specified in RFC2988.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Measure  Currently measured RTT in heart beats.
+
+**/
+VOID
+TcpComputeRtt (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT32 Measure
+  )
+{
+  INT32 Var;
+
+  //
+  // Step 2.3: Compute the RTO for subsequent RTT measurement.
+  //
+  if (Tcb->SRtt != 0) {
+
+    Var = Tcb->SRtt - (Measure << TCP_RTT_SHIFT);
+
+    if (Var < 0) {
+      Var = -Var;
+    }
+
+    Tcb->RttVar = (3 * Tcb->RttVar + Var) >> 2;
+    Tcb->SRtt   = 7 * (Tcb->SRtt >> 3) + Measure;
+
+  } else {
+    //
+    // Step 2.2: compute the first RTT measure
+    //
+    Tcb->SRtt   = Measure << TCP_RTT_SHIFT;
+    Tcb->RttVar = Measure << (TCP_RTT_SHIFT - 1);
+  }
+
+  Tcb->Rto = (Tcb->SRtt + MAX (8, 4 * Tcb->RttVar)) >> TCP_RTT_SHIFT;
+
+  //
+  // Step 2.4: Limit the RTO to at least 1 second
+  // Step 2.5: Limit the RTO to a maxium value that
+  // is at least 60 second
+  //
+  if (Tcb->Rto < TCP_RTO_MIN) {
+    Tcb->Rto = TCP_RTO_MIN;
+
+  } else if (Tcb->Rto > TCP_RTO_MAX) {
+    Tcb->Rto = TCP_RTO_MAX;
+
+  }
+
+  DEBUG ((EFI_D_INFO, "TcpComputeRtt: new RTT for TCB %p"
+    " computed SRTT: %d RTTVAR: %d RTO: %d\n",
+    Tcb, Tcb->SRtt, Tcb->RttVar, Tcb->Rto));
+
+}
+
+
+/**
+  Trim the data, SYN and FIN to fit into the window defined by Left and Right.
+
+  @param  Nbuf     Buffer that contains received TCP segment without IP header.
+  @param  Left     The sequence number of the window's left edge.
+  @param  Right    The sequence number of the window's right edge.
+
+**/
+VOID
+TcpTrimSegment (
+  IN NET_BUF   *Nbuf,
+  IN TCP_SEQNO Left,
+  IN TCP_SEQNO Right
+  )
+{
+  TCP_SEG   *Seg;
+  TCP_SEQNO Urg;
+  UINT32    Drop;
+
+  Seg = TCPSEG_NETBUF (Nbuf);
+
+  //
+  // If the segment is completely out of window,
+  // truncate every thing, include SYN and FIN.
+  //
+  if (TCP_SEQ_LEQ (Seg->End, Left) || TCP_SEQ_LEQ (Right, Seg->Seq)) {
+
+    TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_SYN);
+    TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_FIN);
+
+    Seg->Seq = Seg->End;
+    NetbufTrim (Nbuf, Nbuf->TotalSize, NET_BUF_HEAD);
+    return;
+  }
+
+  //
+  // Adjust the buffer header
+  //
+  if (TCP_SEQ_LT (Seg->Seq, Left)) {
+
+    Drop      = TCP_SUB_SEQ (Left, Seg->Seq);
+    Urg       = Seg->Seq + Seg->Urg;
+    Seg->Seq  = Left;
+
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+      TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_SYN);
+      Drop--;
+    }
+
+    //
+    // Adjust the urgent point
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_URG)) {
+
+      if (TCP_SEQ_LT (Urg, Seg->Seq)) {
+
+        TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_URG);
+      } else {
+        Seg->Urg = (UINT16) TCP_SUB_SEQ (Urg, Seg->Seq);
+      }
+    }
+
+    if (Drop != 0) {
+      NetbufTrim (Nbuf, Drop, NET_BUF_HEAD);
+    }
+  }
+
+  //
+  // Adjust the buffer tail
+  //
+  if (TCP_SEQ_GT (Seg->End, Right)) {
+
+    Drop      = TCP_SUB_SEQ (Seg->End, Right);
+    Seg->End  = Right;
+
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_FIN)) {
+      TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_FIN);
+      Drop--;
+    }
+
+    if (Drop != 0) {
+      NetbufTrim (Nbuf, Drop, NET_BUF_TAIL);
+    }
+  }
+
+  ASSERT (TcpVerifySegment (Nbuf) != 0);
+}
+
+
+/**
+  Trim off the data outside the tcb's receive window.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf     Pointer to the NET_BUF containing the received tcp segment.
+
+**/
+VOID
+TcpTrimInWnd (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  )
+{
+  TcpTrimSegment (Nbuf, Tcb->RcvNxt, Tcb->RcvWl2 + Tcb->RcvWnd);
+}
+
+
+/**
+  Process the data and FIN flag, check whether to deliver
+  data to the socket layer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0        No error occurred to deliver data.
+  @retval -1       Error condition occurred. Proper response is to reset the
+                   connection.
+
+**/
+INTN
+TcpDeliverData (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  LIST_ENTRY      *Entry;
+  NET_BUF         *Nbuf;
+  TCP_SEQNO       Seq;
+  TCP_SEG         *Seg;
+  UINT32          Urgent;
+
+  ASSERT ((Tcb != NULL) && (Tcb->Sk != NULL));
+
+  //
+  // make sure there is some data queued,
+  // and TCP is in a proper state
+  //
+  if (IsListEmpty (&Tcb->RcvQue) || !TCP_CONNECTED (Tcb->State)) {
+
+    return 0;
+  }
+
+  //
+  // Deliver data to the socket layer
+  //
+  Entry = Tcb->RcvQue.ForwardLink;
+  Seq   = Tcb->RcvNxt;
+
+  while (Entry != &Tcb->RcvQue) {
+    Nbuf  = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+    Seg   = TCPSEG_NETBUF (Nbuf);
+
+    ASSERT (TcpVerifySegment (Nbuf) != 0);
+    ASSERT (Nbuf->Tcp == NULL);
+
+    if (TCP_SEQ_GT (Seg->Seq, Seq)) {
+      break;
+    }
+
+    Entry       = Entry->ForwardLink;
+    Seq         = Seg->End;
+    Tcb->RcvNxt = Seq;
+
+    RemoveEntryList (&Nbuf->List);
+
+    //
+    // RFC793 Eighth step: process FIN in sequence
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_FIN)) {
+
+      //
+      // The peer sends to us junky data after FIN,
+      // reset the connection.
+      //
+      if (!IsListEmpty (&Tcb->RcvQue)) {
+        DEBUG ((EFI_D_ERROR, "TcpDeliverData: data received after"
+          " FIN from peer of TCB %p, reset connection\n", Tcb));
+
+        NetbufFree (Nbuf);
+        return -1;
+      }
+
+      DEBUG ((EFI_D_INFO, "TcpDeliverData: processing FIN "
+        "from peer of TCB %p\n", Tcb));
+
+      switch (Tcb->State) {
+      case TCP_SYN_RCVD:
+      case TCP_ESTABLISHED:
+
+        TcpSetState (Tcb, TCP_CLOSE_WAIT);
+        break;
+
+      case TCP_FIN_WAIT_1:
+
+        if (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_ACKED)) {
+
+          TcpSetState (Tcb, TCP_CLOSING);
+          break;
+        }
+
+      //
+      // fall through
+      //
+      case TCP_FIN_WAIT_2:
+
+        TcpSetState (Tcb, TCP_TIME_WAIT);
+        TcpClearAllTimer (Tcb);
+
+        if (Tcb->TimeWaitTimeout != 0) {
+
+          TcpSetTimer (Tcb, TCP_TIMER_2MSL, Tcb->TimeWaitTimeout);
+        } else {
+
+          DEBUG ((EFI_D_WARN, "Connection closed immediately "
+            "because app disables TIME_WAIT timer for %p\n", Tcb));
+
+          TcpSendAck (Tcb);
+          TcpClose (Tcb);
+        }
+        break;
+
+      case TCP_CLOSE_WAIT:
+      case TCP_CLOSING:
+      case TCP_LAST_ACK:
+      case TCP_TIME_WAIT:
+        //
+        // The peer sends to us junk FIN byte. Discard
+        // the buffer then reset the connection
+        //
+        NetbufFree (Nbuf);
+        return -1;
+      default:
+        break;
+      }
+
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+
+      Seg->End--;
+    }
+
+    //
+    // Don't delay the ack if PUSH flag is on.
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_PSH)) {
+
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+    }
+
+    if (Nbuf->TotalSize != 0) {
+      Urgent = 0;
+
+      if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RCVD_URG) &&
+          TCP_SEQ_LEQ (Seg->Seq, Tcb->RcvUp)) {
+
+        if (TCP_SEQ_LEQ (Seg->End, Tcb->RcvUp)) {
+          Urgent = Nbuf->TotalSize;
+        } else {
+          Urgent = TCP_SUB_SEQ (Tcb->RcvUp, Seg->Seq) + 1;
+        }
+      }
+
+      SockDataRcvd (Tcb->Sk, Nbuf, Urgent);
+    }
+
+    if (TCP_FIN_RCVD (Tcb->State)) {
+
+      SockNoMoreData (Tcb->Sk);
+    }
+
+    NetbufFree (Nbuf);
+  }
+
+  return 0;
+}
+
+
+/**
+  Store the data into the reassemble queue.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf     Pointer to the buffer containing the data to be queued.
+
+**/
+VOID
+TcpQueueData (
+  IN OUT TCP_CB  *Tcb,
+  IN     NET_BUF *Nbuf
+  )
+{
+  TCP_SEG         *Seg;
+  LIST_ENTRY      *Head;
+  LIST_ENTRY      *Prev;
+  LIST_ENTRY      *Cur;
+  NET_BUF         *Node;
+
+  ASSERT ((Tcb != NULL) && (Nbuf != NULL) && (Nbuf->Tcp == NULL));
+
+  NET_GET_REF (Nbuf);
+
+  Seg   = TCPSEG_NETBUF (Nbuf);
+  Head  = &Tcb->RcvQue;
+
+  //
+  // Fast path to process normal case. That is,
+  // no out-of-order segments are received.
+  //
+  if (IsListEmpty (Head)) {
+
+    InsertTailList (Head, &Nbuf->List);
+    return;
+  }
+
+  //
+  // Find the point to insert the buffer
+  //
+  for (Prev = Head, Cur = Head->ForwardLink;
+       Cur != Head;
+       Prev = Cur, Cur = Cur->ForwardLink) {
+
+    Node = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+
+    if (TCP_SEQ_LT (Seg->Seq, TCPSEG_NETBUF (Node)->Seq)) {
+      break;
+    }
+  }
+
+  //
+  // Check whether the current segment overlaps with the
+  // previous segment.
+  //
+  if (Prev != Head) {
+    Node = NET_LIST_USER_STRUCT (Prev, NET_BUF, List);
+
+    if (TCP_SEQ_LT (Seg->Seq, TCPSEG_NETBUF (Node)->End)) {
+
+      if (TCP_SEQ_LEQ (Seg->End, TCPSEG_NETBUF (Node)->End)) {
+
+        NetbufFree (Nbuf);
+        return;
+      }
+
+      TcpTrimSegment (Nbuf, TCPSEG_NETBUF (Node)->End, Seg->End);
+    }
+  }
+
+  InsertHeadList (Prev, &Nbuf->List);
+
+  TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+
+  //
+  // Check the segments after the insert point.
+  //
+  while (Cur != Head) {
+    Node = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+
+    if (TCP_SEQ_LEQ (TCPSEG_NETBUF (Node)->End, Seg->End)) {
+
+      Cur = Cur->ForwardLink;
+
+      RemoveEntryList (&Node->List);
+      NetbufFree (Node);
+      continue;
+    }
+
+    if (TCP_SEQ_LT (TCPSEG_NETBUF (Node)->Seq, Seg->End)) {
+
+      if (TCP_SEQ_LEQ (TCPSEG_NETBUF (Node)->Seq, Seg->Seq)) {
+
+        RemoveEntryList (&Nbuf->List);
+        NetbufFree (Nbuf);
+        return ;
+      }
+
+      TcpTrimSegment (Nbuf, Seg->Seq, TCPSEG_NETBUF (Node)->Seq);
+      break;
+    }
+
+    Cur = Cur->ForwardLink;
+  }
+}
+
+
+/**
+  Ajust the send queue or the retransmit queue.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Ack      The acknowledge seuqence number of the received segment.
+
+**/
+VOID
+TcpAdjustSndQue (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Ack
+  )
+{
+  LIST_ENTRY      *Head;
+  LIST_ENTRY      *Cur;
+  NET_BUF         *Node;
+  TCP_SEG         *Seg;
+
+  Head  = &Tcb->SndQue;
+  Cur   = Head->ForwardLink;
+
+  while (Cur != Head) {
+    Node  = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+    Seg   = TCPSEG_NETBUF (Node);
+
+    if (TCP_SEQ_GEQ (Seg->Seq, Ack)) {
+      break;
+    }
+
+    //
+    // Remove completely ACKed segments
+    //
+    if (TCP_SEQ_LEQ (Seg->End, Ack)) {
+      Cur = Cur->ForwardLink;
+
+      RemoveEntryList (&Node->List);
+      NetbufFree (Node);
+      continue;
+    }
+
+    TcpTrimSegment (Node, Ack, Seg->End);
+    break;
+  }
+}
+
+
+/**
+  Process the received TCP segments.
+
+  @param  Nbuf     Buffer that contains received TCP segment without IP header.
+  @param  Src      Source address of the segment, or the peer's IP address.
+  @param  Dst      Destination address of the segment, or the local end's IP
+                   address.
+
+  @retval 0        Segment is processed successfully. It is either accepted or
+                   discarded. But no connection is reset by the segment.
+  @retval -1       A connection is reset by the segment.
+
+**/
+INTN
+TcpInput (
+  IN NET_BUF *Nbuf,
+  IN UINT32  Src,
+  IN UINT32  Dst
+  )
+{
+  TCP_CB      *Tcb;
+  TCP_CB      *Parent;
+  TCP_OPTION  Option;
+  TCP_HEAD    *Head;
+  INT32       Len;
+  TCP_SEG     *Seg;
+  TCP_SEQNO   Right;
+  TCP_SEQNO   Urg;
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  Parent  = NULL;
+  Tcb     = NULL;
+
+  Head    = (TCP_HEAD *) NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (Head != NULL);
+  Len     = Nbuf->TotalSize - (Head->HeadLen << 2);
+
+  if ((Head->HeadLen < 5) || (Len < 0) ||
+      (TcpChecksum (Nbuf, NetPseudoHeadChecksum (Src, Dst, 6, 0)) != 0)) {
+
+    DEBUG ((EFI_D_INFO, "TcpInput: received an mal-formated packet\n"));
+    goto DISCARD;
+  }
+
+  if (TCP_FLG_ON (Head->Flag, TCP_FLG_SYN)) {
+    Len++;
+  }
+
+  if (TCP_FLG_ON (Head->Flag, TCP_FLG_FIN)) {
+    Len++;
+  }
+
+  Tcb = TcpLocateTcb (
+          Head->DstPort,
+          Dst,
+          Head->SrcPort,
+          Src,
+          (BOOLEAN) TCP_FLG_ON (Head->Flag, TCP_FLG_SYN)
+          );
+
+  if ((Tcb == NULL) || (Tcb->State == TCP_CLOSED)) {
+    DEBUG ((EFI_D_INFO, "TcpInput: send reset because no TCB found\n"));
+
+    Tcb = NULL;
+    goto SEND_RESET;
+  }
+
+  Seg = TcpFormatNetbuf (Tcb, Nbuf);
+
+  //
+  // RFC1122 recommended reaction to illegal option
+  // (in fact, an illegal option length) is reset.
+  //
+  if (TcpParseOption (Nbuf->Tcp, &Option) == -1) {
+    DEBUG ((EFI_D_ERROR, "TcpInput: reset the peer because"
+      " of malformed option for TCB %p\n", Tcb));
+
+    goto SEND_RESET;
+  }
+
+  //
+  // From now on, the segment is headless
+  //
+  NetbufTrim (Nbuf, (Head->HeadLen << 2), NET_BUF_HEAD);
+  Nbuf->Tcp = NULL;
+
+  //
+  // Process the segment in LISTEN state.
+  //
+  if (Tcb->State == TCP_LISTEN) {
+    //
+    // First step: Check RST
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_RST)) {
+      DEBUG ((EFI_D_WARN, "TcpInput: discard a reset segment "
+        "for TCB %p in listening\n", Tcb));
+
+      goto DISCARD;
+    }
+
+    //
+    // Second step: Check ACK.
+    // Any ACK sent to TCP in LISTEN is reseted.
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK)) {
+      DEBUG ((EFI_D_WARN, "TcpInput: send reset because of"
+        " segment with ACK for TCB %p in listening\n", Tcb));
+
+      goto SEND_RESET;
+    }
+
+    //
+    // Third step: Check SYN
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+      //
+      // create a child TCB to handle the data
+      //
+      Parent  = Tcb;
+
+      Tcb     = TcpCloneTcb (Parent);
+      if (Tcb == NULL) {
+        DEBUG ((EFI_D_ERROR, "TcpInput: discard a segment because"
+          " failed to clone a child for TCB %p\n", Tcb));
+
+        goto DISCARD;
+      }
+
+      DEBUG ((EFI_D_INFO, "TcpInput: create a child for TCB %p"
+        " in listening\n", Tcb));
+
+      //
+      // init the TCB structure
+      //
+      Tcb->LocalEnd.Ip    = Dst;
+      Tcb->LocalEnd.Port  = Head->DstPort;
+      Tcb->RemoteEnd.Ip   = Src;
+      Tcb->RemoteEnd.Port = Head->SrcPort;
+
+      TcpInitTcbLocal (Tcb);
+      TcpInitTcbPeer (Tcb, Seg, &Option);
+
+      TcpSetState (Tcb, TCP_SYN_RCVD);
+      TcpSetTimer (Tcb, TCP_TIMER_CONNECT, Tcb->ConnectTimeout);
+      TcpTrimInWnd (Tcb, Nbuf);
+
+      goto StepSix;
+    }
+
+    goto DISCARD;
+
+  } else if (Tcb->State == TCP_SYN_SENT) {
+    //
+    // First step: Check ACK bit
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK) && (Seg->Ack != Tcb->Iss + 1)) {
+
+      DEBUG ((EFI_D_WARN, "TcpInput: send reset because of "
+        "wrong ACK received for TCB %p in SYN_SENT\n", Tcb));
+
+      goto SEND_RESET;
+    }
+
+    //
+    // Second step: Check RST bit
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_RST)) {
+
+      if (TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK)) {
+
+        DEBUG ((EFI_D_WARN, "TcpInput: connection reset by"
+          " peer for TCB %p in SYN_SENT\n", Tcb));
+
+        SOCK_ERROR (Tcb->Sk, EFI_CONNECTION_RESET);
+        goto DROP_CONNECTION;
+      } else {
+
+        DEBUG ((EFI_D_WARN, "TcpInput: discard a reset segment "
+          "because of no ACK for TCB %p in SYN_SENT\n", Tcb));
+
+        goto DISCARD;
+      }
+    }
+
+    //
+    // Third step: Check security and precedence. Skipped
+    //
+
+    //
+    // Fourth step: Check SYN. Pay attention to simultaneous open
+    //
+    if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+
+      TcpInitTcbPeer (Tcb, Seg, &Option);
+
+      if (TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK)) {
+
+        Tcb->SndUna = Seg->Ack;
+      }
+
+      TcpClearTimer (Tcb, TCP_TIMER_REXMIT);
+
+      if (TCP_SEQ_GT (Tcb->SndUna, Tcb->Iss)) {
+
+        TcpSetState (Tcb, TCP_ESTABLISHED);
+
+        TcpClearTimer (Tcb, TCP_TIMER_CONNECT);
+        TcpDeliverData (Tcb);
+
+        if ((Tcb->CongestState == TCP_CONGEST_OPEN) &&
+            TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RTT_ON)) {
+
+          TcpComputeRtt (Tcb, Tcb->RttMeasure);
+          TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_RTT_ON);
+        }
+
+        TcpTrimInWnd (Tcb, Nbuf);
+
+        TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+
+        DEBUG ((EFI_D_INFO, "TcpInput: connection established"
+          " for TCB %p in SYN_SENT\n", Tcb));
+
+        goto StepSix;
+      } else {
+        //
+        // Received a SYN segment without ACK, simultaneous open.
+        //
+        TcpSetState (Tcb, TCP_SYN_RCVD);
+
+        ASSERT (Tcb->SndNxt == Tcb->Iss + 1);
+        TcpAdjustSndQue (Tcb, Tcb->SndNxt);
+
+        TcpTrimInWnd (Tcb, Nbuf);
+
+        DEBUG ((EFI_D_WARN, "TcpInput: simultaneous open "
+          "for TCB %p in SYN_SENT\n", Tcb));
+
+        goto StepSix;
+      }
+    }
+
+    goto DISCARD;
+  }
+
+  //
+  // Process segment in SYN_RCVD or TCP_CONNECTED states
+  //
+
+  //
+  // Clear probe timer since the RecvWindow is opened.
+  //
+  if (Tcb->ProbeTimerOn && (Seg->Wnd != 0)) {
+    TcpClearTimer (Tcb, TCP_TIMER_PROBE);
+    Tcb->ProbeTimerOn = FALSE;
+  }
+
+  //
+  // First step: Check whether SEG.SEQ is acceptable
+  //
+  if (TcpSeqAcceptable (Tcb, Seg) == 0) {
+    DEBUG ((EFI_D_WARN, "TcpInput: sequence acceptance"
+      " test failed for segment of TCB %p\n", Tcb));
+
+    if (!TCP_FLG_ON (Seg->Flag, TCP_FLG_RST)) {
+      TcpSendAck (Tcb);
+    }
+
+    goto DISCARD;
+  }
+
+  if ((TCP_SEQ_LT (Seg->Seq, Tcb->RcvWl2)) &&
+      (Tcb->RcvWl2 == Seg->End) &&
+      !TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN | TCP_FLG_FIN)) {
+
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+  }
+
+  //
+  // Second step: Check the RST
+  //
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_RST)) {
+
+    DEBUG ((EFI_D_WARN, "TcpInput: connection reset for TCB %p\n", Tcb));
+
+    if (Tcb->State == TCP_SYN_RCVD) {
+
+      SOCK_ERROR (Tcb->Sk, EFI_CONNECTION_REFUSED);
+
+      //
+      // This TCB comes from either a LISTEN TCB,
+      // or active open TCB with simultanous open.
+      // Do NOT signal user CONNECTION refused
+      // if it comes from a LISTEN TCB.
+      //
+    } else if ((Tcb->State == TCP_ESTABLISHED) ||
+               (Tcb->State == TCP_FIN_WAIT_1) ||
+               (Tcb->State == TCP_FIN_WAIT_2) ||
+               (Tcb->State == TCP_CLOSE_WAIT)) {
+
+      SOCK_ERROR (Tcb->Sk, EFI_CONNECTION_RESET);
+
+    } else {
+
+    }
+
+    goto DROP_CONNECTION;
+  }
+
+  //
+  // Trim the data and flags.
+  //
+  TcpTrimInWnd (Tcb, Nbuf);
+
+  //
+  // Third step: Check security and precedence, Ignored
+  //
+
+  //
+  // Fourth step: Check the SYN bit.
+  //
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+
+    DEBUG ((EFI_D_WARN, "TcpInput: connection reset "
+      "because received extra SYN for TCB %p\n", Tcb));
+
+    SOCK_ERROR (Tcb->Sk, EFI_CONNECTION_RESET);
+    goto RESET_THEN_DROP;
+  }
+
+  //
+  // Fifth step: Check the ACK
+  //
+  if (!TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK)) {
+    DEBUG ((EFI_D_WARN, "TcpInput: segment discard because"
+      " of no ACK for connected TCB %p\n", Tcb));
+
+    goto DISCARD;
+
+  }
+
+  if (Tcb->State == TCP_SYN_RCVD) {
+
+    if (TCP_SEQ_LT (Tcb->SndUna, Seg->Ack) &&
+        TCP_SEQ_LEQ (Seg->Ack, Tcb->SndNxt)) {
+
+      Tcb->SndWnd     = Seg->Wnd;
+      Tcb->SndWndMax  = MAX (Tcb->SndWnd, Tcb->SndWndMax);
+      Tcb->SndWl1     = Seg->Seq;
+      Tcb->SndWl2     = Seg->Ack;
+      TcpSetState (Tcb, TCP_ESTABLISHED);
+
+      TcpClearTimer (Tcb, TCP_TIMER_CONNECT);
+      TcpDeliverData (Tcb);
+
+      DEBUG ((EFI_D_INFO, "TcpInput: connection established "
+        " for TCB %p in SYN_RCVD\n", Tcb));
+
+      //
+      // Continue the process as ESTABLISHED state
+      //
+    } else {
+      DEBUG ((EFI_D_WARN, "TcpInput: send reset because of"
+        " wrong ACK for TCB %p in SYN_RCVD\n", Tcb));
+
+      goto SEND_RESET;
+    }
+  }
+
+  if (TCP_SEQ_LT (Seg->Ack, Tcb->SndUna)) {
+
+    DEBUG ((EFI_D_WARN, "TcpInput: ignore the out-of-data"
+      " ACK for connected TCB %p\n", Tcb));
+
+    goto StepSix;
+
+  } else if (TCP_SEQ_GT (Seg->Ack, Tcb->SndNxt)) {
+
+    DEBUG ((EFI_D_WARN, "TcpInput: discard segment for "
+      "future ACK for connected TCB %p\n", Tcb));
+
+    TcpSendAck (Tcb);
+    goto DISCARD;
+  }
+
+  //
+  // From now on: SND.UNA <= SEG.ACK <= SND.NXT.
+  //
+  if (TCP_FLG_ON (Option.Flag, TCP_OPTION_RCVD_TS)) {
+    //
+    // update TsRecent as specified in page 16 RFC1323.
+    // RcvWl2 equals to the variable "LastAckSent"
+    // defined there.
+    //
+    if (TCP_SEQ_LEQ (Seg->Seq, Tcb->RcvWl2) &&
+        TCP_SEQ_LT (Tcb->RcvWl2, Seg->End)) {
+
+      Tcb->TsRecent     = Option.TSVal;
+      Tcb->TsRecentAge  = mTcpTick;
+    }
+
+    TcpComputeRtt (Tcb, TCP_SUB_TIME (mTcpTick, Option.TSEcr));
+
+  } else if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RTT_ON)) {
+
+    ASSERT (Tcb->CongestState == TCP_CONGEST_OPEN);
+
+    TcpComputeRtt (Tcb, Tcb->RttMeasure);
+    TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_RTT_ON);
+  }
+
+  if (Seg->Ack == Tcb->SndNxt) {
+
+    TcpClearTimer (Tcb, TCP_TIMER_REXMIT);
+  } else {
+
+    TcpSetTimer (Tcb, TCP_TIMER_REXMIT, Tcb->Rto);
+  }
+
+  //
+  // Count duplicate acks.
+  //
+  if ((Seg->Ack == Tcb->SndUna) &&
+      (Tcb->SndUna != Tcb->SndNxt) &&
+      (Seg->Wnd == Tcb->SndWnd) &&
+      (0 == Len)) {
+
+    Tcb->DupAck++;
+  } else {
+
+    Tcb->DupAck = 0;
+  }
+
+  //
+  // Congestion avoidance, fast recovery and fast retransmission.
+  //
+  if (((Tcb->CongestState == TCP_CONGEST_OPEN) && (Tcb->DupAck < 3)) ||
+      (Tcb->CongestState == TCP_CONGEST_LOSS)) {
+
+    if (TCP_SEQ_GT (Seg->Ack, Tcb->SndUna)) {
+
+      if (Tcb->CWnd < Tcb->Ssthresh) {
+
+        Tcb->CWnd += Tcb->SndMss;
+      } else {
+
+        Tcb->CWnd += MAX (Tcb->SndMss * Tcb->SndMss / Tcb->CWnd, 1);
+      }
+
+      Tcb->CWnd = MIN (Tcb->CWnd, TCP_MAX_WIN << Tcb->SndWndScale);
+    }
+
+    if (Tcb->CongestState == TCP_CONGEST_LOSS) {
+      TcpFastLossRecover (Tcb, Seg);
+    }
+  } else {
+
+    TcpFastRecover (Tcb, Seg);
+  }
+
+  if (TCP_SEQ_GT (Seg->Ack, Tcb->SndUna)) {
+
+    TcpAdjustSndQue (Tcb, Seg->Ack);
+    Tcb->SndUna = Seg->Ack;
+
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_SND_URG) &&
+        TCP_SEQ_LT (Tcb->SndUp, Seg->Ack)) {
+
+      TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_SND_URG);
+    }
+  }
+
+  //
+  // Update window info
+  //
+  if (TCP_SEQ_LT (Tcb->SndWl1, Seg->Seq) ||
+      ((Tcb->SndWl1 == Seg->Seq) && TCP_SEQ_LEQ (Tcb->SndWl2, Seg->Ack))) {
+
+    Right = Seg->Ack + Seg->Wnd;
+
+    if (TCP_SEQ_LT (Right, Tcb->SndWl2 + Tcb->SndWnd)) {
+
+      if ((Tcb->SndWl1 == Seg->Seq) &&
+          (Tcb->SndWl2 == Seg->Ack) &&
+          (Len == 0)) {
+
+        goto NO_UPDATE;
+      }
+
+      DEBUG ((EFI_D_WARN, "TcpInput: peer shrinks the"
+        " window for connected TCB %p\n", Tcb));
+
+      if ((Tcb->CongestState == TCP_CONGEST_RECOVER) &&
+          (TCP_SEQ_LT (Right, Tcb->Recover))) {
+
+        Tcb->Recover = Right;
+      }
+
+      if ((Tcb->CongestState == TCP_CONGEST_LOSS) &&
+          (TCP_SEQ_LT (Right, Tcb->LossRecover))) {
+
+        Tcb->LossRecover = Right;
+      }
+
+      if (TCP_SEQ_LT (Right, Tcb->SndNxt)) {
+
+        Tcb->SndNxt = Right;
+
+        if (Right == Tcb->SndUna) {
+
+          TcpClearTimer (Tcb, TCP_TIMER_REXMIT);
+          TcpSetProbeTimer (Tcb);
+        }
+      }
+    }
+
+    Tcb->SndWnd     = Seg->Wnd;
+    Tcb->SndWndMax  = MAX (Tcb->SndWnd, Tcb->SndWndMax);
+    Tcb->SndWl1     = Seg->Seq;
+    Tcb->SndWl2     = Seg->Ack;
+  }
+
+NO_UPDATE:
+
+  if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_SENT) &&
+      (Tcb->SndUna == Tcb->SndNxt)) {
+
+    DEBUG ((EFI_D_INFO, "TcpInput: local FIN is ACKed by"
+      " peer for connected TCB %p\n", Tcb));
+
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_FIN_ACKED);
+  }
+
+  //
+  // Transit the state if proper.
+  //
+  switch (Tcb->State) {
+  case TCP_FIN_WAIT_1:
+
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_ACKED)) {
+
+      TcpSetState (Tcb, TCP_FIN_WAIT_2);
+
+      TcpClearAllTimer (Tcb);
+      TcpSetTimer (Tcb, TCP_TIMER_FINWAIT2, Tcb->FinWait2Timeout);
+    }
+
+  case TCP_FIN_WAIT_2:
+
+    break;
+
+  case TCP_CLOSE_WAIT:
+    break;
+
+  case TCP_CLOSING:
+
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_ACKED)) {
+
+      TcpSetState (Tcb, TCP_TIME_WAIT);
+
+      TcpClearAllTimer (Tcb);
+
+      if (Tcb->TimeWaitTimeout != 0) {
+
+        TcpSetTimer (Tcb, TCP_TIMER_2MSL, Tcb->TimeWaitTimeout);
+      } else {
+
+        DEBUG ((EFI_D_WARN, "Connection closed immediately "
+          "because app disables TIME_WAIT timer for %p\n", Tcb));
+
+        TcpClose (Tcb);
+      }
+    }
+    break;
+
+  case TCP_LAST_ACK:
+
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_ACKED)) {
+
+      TcpSetState (Tcb, TCP_CLOSED);
+    }
+
+    break;
+
+  case TCP_TIME_WAIT:
+
+    TcpSendAck (Tcb);
+
+    if (Tcb->TimeWaitTimeout != 0) {
+
+      TcpSetTimer (Tcb, TCP_TIMER_2MSL, Tcb->TimeWaitTimeout);
+    } else {
+
+      DEBUG ((EFI_D_WARN, "Connection closed immediately "
+        "because app disables TIME_WAIT timer for %p\n", Tcb));
+
+      TcpClose (Tcb);
+    }
+    break;
+
+  default:
+    break;
+  }
+
+  //
+  // Sixth step: Check the URG bit.update the Urg point
+  // if in TCP_CAN_RECV, otherwise, leave the RcvUp intact.
+  //
+StepSix:
+
+  Tcb->Idle = 0;
+  TcpSetKeepaliveTimer (Tcb);
+
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_URG) &&
+      !TCP_FIN_RCVD (Tcb->State)) 
+  {
+
+    DEBUG ((EFI_D_INFO, "TcpInput: received urgent data "
+      "from peer for connected TCB %p\n", Tcb));
+
+    Urg = Seg->Seq + Seg->Urg;
+
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RCVD_URG) &&
+        TCP_SEQ_GT (Urg, Tcb->RcvUp)) {
+
+      Tcb->RcvUp = Urg;
+    } else {
+
+      Tcb->RcvUp = Urg;
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_RCVD_URG);
+    }
+  }
+
+  //
+  // Seventh step: Process the segment data
+  //
+  if (Seg->End != Seg->Seq) {
+
+    if (TCP_FIN_RCVD (Tcb->State)) {
+
+      DEBUG ((EFI_D_WARN, "TcpInput: connection reset because"
+        " data is lost for connected TCB %p\n", Tcb));
+
+      goto RESET_THEN_DROP;
+    }
+
+    if (TCP_LOCAL_CLOSED (Tcb->State) && (Nbuf->TotalSize != 0)) {
+      DEBUG ((EFI_D_WARN, "TcpInput: connection reset because"
+        " data is lost for connected TCB %p\n", Tcb));
+
+      goto RESET_THEN_DROP;
+    }
+
+    TcpQueueData (Tcb, Nbuf);
+    if (TcpDeliverData (Tcb) == -1) {
+      goto RESET_THEN_DROP;
+    }
+
+    if (!IsListEmpty (&Tcb->RcvQue)) {
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+    }
+  }
+
+  //
+  // Eighth step: check the FIN.
+  // This step is moved to TcpDeliverData. FIN will be
+  // processed in sequence there. Check the comments in
+  // the beginning of the file header for information.
+  //
+
+  //
+  // Tcb is a new child of the listening Parent,
+  // commit it.
+  //
+  if (Parent != NULL) {
+    Tcb->Parent = Parent;
+    TcpInsertTcb (Tcb);
+  }
+
+  if ((Tcb->State != TCP_CLOSED) &&
+      (TcpToSendData (Tcb, 0) == 0) &&
+      (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW) || (Nbuf->TotalSize != 0))) {
+
+    TcpToSendAck (Tcb);
+  }
+
+  NetbufFree (Nbuf);
+  return 0;
+
+RESET_THEN_DROP:
+  TcpSendReset (Tcb, Head, Len, Dst, Src);
+
+DROP_CONNECTION:
+  ASSERT ((Tcb != NULL) && (Tcb->Sk != NULL));
+
+  NetbufFree (Nbuf);
+  TcpClose (Tcb);
+
+  return -1;
+
+SEND_RESET:
+
+  TcpSendReset (Tcb, Head, Len, Dst, Src);
+
+DISCARD:
+
+  //
+  // Tcb is a child of Parent, and it doesn't survive
+  //
+  DEBUG ((EFI_D_WARN, "Tcp4Input: Discard a packet\n"));
+  NetbufFree (Nbuf);
+
+  if ((Parent != NULL) && (Tcb != NULL)) {
+
+    ASSERT (Tcb->Sk != NULL);
+    TcpClose (Tcb);
+  }
+
+  return 0;
+}
+
+
+/**
+  Process the received ICMP error messages for TCP.
+
+  @param  Nbuf     Buffer that contains part of the TCP segment without IP header
+                   truncated from the ICMP error packet.
+  @param  IcmpErr  The ICMP error code interpreted from ICMP error packet.
+  @param  Src      Source address of the ICMP error message.
+  @param  Dst      Destination address of the ICMP error message.
+
+**/
+VOID
+TcpIcmpInput (
+  IN NET_BUF     *Nbuf,
+  IN UINT8       IcmpErr,
+  IN UINT32      Src,
+  IN UINT32      Dst
+  )
+{
+  TCP_HEAD         *Head;
+  TCP_CB           *Tcb;
+  TCP_SEQNO        Seq;
+  EFI_STATUS       IcmpErrStatus;
+  BOOLEAN          IcmpErrIsHard;
+  BOOLEAN          IcmpErrNotify;
+
+  Head = (TCP_HEAD *) NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (Head != NULL);
+  Tcb = TcpLocateTcb (
+          Head->DstPort,
+          Dst,
+          Head->SrcPort,
+          Src,
+          FALSE
+          );
+  if (Tcb == NULL || Tcb->State == TCP_CLOSED) {
+
+    goto CLEAN_EXIT;
+  }
+
+  //
+  // Validate the sequence number.
+  //
+  Seq = NTOHL (Head->Seq);
+  if (!(TCP_SEQ_LEQ (Tcb->SndUna, Seq) && TCP_SEQ_LT (Seq, Tcb->SndNxt))) {
+
+    goto CLEAN_EXIT;
+  }
+
+  IcmpErrStatus = IpIoGetIcmpErrStatus (
+                    IcmpErr,
+                    IP_VERSION_4,
+                    &IcmpErrIsHard,
+                    &IcmpErrNotify
+                    );
+
+  if (IcmpErrNotify) {
+
+    SOCK_ERROR (Tcb->Sk, IcmpErrStatus);
+  }
+
+  if (IcmpErrIsHard) {
+
+    TcpClose (Tcb);
+  }
+
+CLEAN_EXIT:
+  NetbufFree (Nbuf);
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Io.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Io.c
new file mode 100644
index 0000000000..b98d1e783a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Io.c
@@ -0,0 +1,112 @@
+/** @file
+  I/O interfaces between TCP and IpIo.
+
+Copyright (c) 2005 - 2009, 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<BR>
+
+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 "Tcp4Main.h"
+
+
+/**
+  Packet receive callback function provided to IP_IO, used to call
+  the proper function to handle the packet received by IP.
+
+  @param  Status      Status of the received packet.
+  @param  IcmpErr     ICMP error number.
+  @param  NetSession  Pointer to the net session of this packet.
+  @param  Pkt         Pointer to the recieved packet.
+  @param  Context     Pointer to the context configured in IpIoOpen(), not used
+                      now.
+
+**/
+VOID
+EFIAPI
+Tcp4RxCallback (
+  IN EFI_STATUS                       Status,
+  IN UINT8                            IcmpErr,
+  IN EFI_NET_SESSION_DATA             *NetSession,
+  IN NET_BUF                          *Pkt,
+  IN VOID                             *Context    OPTIONAL
+  )
+{
+  if (EFI_SUCCESS == Status) {
+    TcpInput (Pkt, NetSession->Source.Addr[0], NetSession->Dest.Addr[0]);
+  } else {
+    TcpIcmpInput (Pkt, IcmpErr, NetSession->Source.Addr[0], NetSession->Dest.Addr[0]);
+  }
+}
+
+
+/**
+  Send the segment to IP via IpIo function.
+
+  @param  Tcb         Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf        Pointer to the TCP segment to be sent.
+  @param  Src         Source address of the TCP segment.
+  @param  Dest        Destination address of the TCP segment.
+
+  @retval 0           The segment was sent out successfully.
+  @retval -1          The segment was failed to send.
+
+**/
+INTN
+TcpSendIpPacket (
+  IN TCP_CB    *Tcb,
+  IN NET_BUF   *Nbuf,
+  IN UINT32    Src,
+  IN UINT32    Dest
+  )
+{
+  EFI_STATUS       Status;
+  IP_IO            *IpIo;
+  IP_IO_OVERRIDE   Override;
+  SOCKET           *Sock;
+  VOID             *IpSender;
+  TCP4_PROTO_DATA  *TcpProto;
+  EFI_IP_ADDRESS   Source;
+  EFI_IP_ADDRESS   Destination;
+
+  Source.Addr[0]      = Src;
+  Destination.Addr[0] = Dest;
+
+  if (NULL == Tcb) {
+
+    IpIo     = NULL;
+    IpSender = IpIoFindSender (&IpIo, IP_VERSION_4, &Source);
+
+    if (IpSender == NULL) {
+      DEBUG ((EFI_D_WARN, "TcpSendIpPacket: No appropriate IpSender.\n"));
+      return -1;
+    }
+  } else {
+
+    Sock     = Tcb->Sk;
+    TcpProto = (TCP4_PROTO_DATA *) Sock->ProtoReserved;
+    IpIo     = TcpProto->TcpService->IpIo;
+    IpSender = Tcb->IpInfo;
+  }
+
+  Override.Ip4OverrideData.TypeOfService            = 0;
+  Override.Ip4OverrideData.TimeToLive               = 255;
+  Override.Ip4OverrideData.DoNotFragment            = FALSE;
+  Override.Ip4OverrideData.Protocol                 = EFI_IP_PROTO_TCP;
+  ZeroMem (&Override.Ip4OverrideData.GatewayAddress, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Override.Ip4OverrideData.SourceAddress, &Src, sizeof (EFI_IPv4_ADDRESS));
+
+  Status = IpIoSend (IpIo, Nbuf, IpSender, NULL, NULL, &Destination, &Override);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "TcpSendIpPacket: return %r error\n", Status));
+    return -1;
+  }
+
+  return 0;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.c
new file mode 100644
index 0000000000..c7d3d32185
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.c
@@ -0,0 +1,673 @@
+/** @file
+  Implementation of TCP4 protocol services.
+
+Copyright (c) 2005 - 2011, 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<BR>
+
+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 "Tcp4Main.h"
+
+
+/**
+  Check the integrity of the data buffer.
+
+  @param  DataLen                  The total length of the data buffer.
+  @param  FragmentCount            The fragment count of the fragment table.
+  @param  FragmentTable            Pointer to the fragment table of the data
+                                   buffer.
+
+  @retval EFI_SUCCESS              The integrity check is passed.
+  @retval EFI_INVALID_PARAMETER    The integrity check is failed.
+
+**/
+EFI_STATUS
+Tcp4ChkDataBuf (
+  IN UINT32                 DataLen,
+  IN UINT32                 FragmentCount,
+  IN EFI_TCP4_FRAGMENT_DATA *FragmentTable
+  )
+{
+  UINT32 Index;
+
+  UINT32 Len;
+
+  for (Index = 0, Len = 0; Index < FragmentCount; Index++) {
+    Len = Len + (UINT32) FragmentTable[Index].FragmentLength;
+  }
+
+  if (DataLen != Len) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the current operational status.
+  
+  The GetModeData() function copies the current operational settings of this 
+  EFI TCPv4 Protocol instance into user-supplied buffers. This function can 
+  also be used to retrieve the operational setting of underlying drivers 
+  such as IPv4, MNP, or SNP.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Tcp4State                Pointer to the buffer to receive the current TCP
+                                   state.
+  @param  Tcp4ConfigData           Pointer to the buffer to receive the current TCP
+                                   configuration.
+  @param  Ip4ModeData              Pointer to the buffer to receive the current IPv4 
+                                   configuration data used by the TCPv4 instance.
+  @param  MnpConfigData            Pointer to the buffer to receive the current MNP
+                                   configuration data indirectly used by the TCPv4
+                                   Instance.
+  @param  SnpModeData              Pointer to the buffer to receive the current SNP
+                                   configuration data indirectly used by the TCPv4
+                                   Instance.
+
+  @retval EFI_SUCCESS              The mode data was read.
+  @retval EFI_NOT_STARTED          No configuration data is available because this
+                                   instance hasn't been started.
+  @retval EFI_INVALID_PARAMETER    This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4GetModeData (
+  IN      EFI_TCP4_PROTOCOL                  *This,
+     OUT  EFI_TCP4_CONNECTION_STATE          *Tcp4State       OPTIONAL,
+     OUT  EFI_TCP4_CONFIG_DATA               *Tcp4ConfigData  OPTIONAL,
+     OUT  EFI_IP4_MODE_DATA                  *Ip4ModeData     OPTIONAL,
+     OUT  EFI_MANAGED_NETWORK_CONFIG_DATA    *MnpConfigData   OPTIONAL,
+     OUT  EFI_SIMPLE_NETWORK_MODE            *SnpModeData     OPTIONAL
+  )
+{
+  TCP4_MODE_DATA  TcpMode;
+  SOCKET          *Sock;
+
+  if (NULL == This) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock                    = SOCK_FROM_THIS (This);
+
+  TcpMode.Tcp4State       = Tcp4State;
+  TcpMode.Tcp4ConfigData  = Tcp4ConfigData;
+  TcpMode.Ip4ModeData     = Ip4ModeData;
+  TcpMode.MnpConfigData   = MnpConfigData;
+  TcpMode.SnpModeData     = SnpModeData;
+
+  return SockGetMode (Sock, &TcpMode);
+}
+
+
+/**
+  Initialize or brutally reset the operational parameters for
+  this EFI TCPv4 instance.
+  
+  The Configure() function does the following:
+  * Initialize this EFI TCPv4 instance, i.e., initialize the communication end 
+  setting, specify active open or passive open for an instance.
+  * Reset this TCPv4 instance brutally, i.e., cancel all pending asynchronous 
+  tokens, flush transmission and receiving buffer directly without informing 
+  the communication peer.
+  No other TCPv4 Protocol operation can be executed by this instance 
+  until it is configured properly. For an active TCP4 instance, after a proper 
+  configuration it may call Connect() to initiates the three-way handshake. 
+  For a passive TCP4 instance, its state will transit to Tcp4StateListen after 
+  configuration, and Accept() may be called to listen the incoming TCP connection 
+  request. If TcpConfigData is set to NULL, the instance is reset. Resetting 
+  process will be done brutally, the state machine will be set to Tcp4StateClosed 
+  directly, the receive queue and transmit queue will be flushed, and no traffic is 
+  allowed through this instance.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  TcpConfigData            Pointer to the configure data to configure the
+                                   instance.
+
+  @retval EFI_SUCCESS              The operational settings are set, changed, or
+                                   reset successfully.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (through DHCP, BOOTP, RARP, etc.) is not
+                                   finished.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED        Configuring TCP instance when it is already
+                                   configured.
+  @retval EFI_DEVICE_ERROR         An unexpected network or system error occurred.
+  @retval EFI_UNSUPPORTED          One or more of the control options are not
+                                   supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough system resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Configure (
+  IN EFI_TCP4_PROTOCOL        *This,
+  IN EFI_TCP4_CONFIG_DATA     *TcpConfigData     OPTIONAL
+  )
+{
+  EFI_TCP4_OPTION  *Option;
+  SOCKET           *Sock;
+  EFI_STATUS       Status;
+  IP4_ADDR         Ip;
+  IP4_ADDR         SubnetMask;
+
+  if (NULL == This) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Tcp protocol related parameter check will be conducted here
+  //
+  if (NULL != TcpConfigData) {
+
+    CopyMem (&Ip, &TcpConfigData->AccessPoint.RemoteAddress, sizeof (IP4_ADDR));
+    if ((Ip != 0) && !NetIp4IsUnicast (NTOHL (Ip), 0)) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (TcpConfigData->AccessPoint.ActiveFlag &&
+      (0 == TcpConfigData->AccessPoint.RemotePort || (Ip == 0))) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (!TcpConfigData->AccessPoint.UseDefaultAddress) {
+
+      CopyMem (&Ip, &TcpConfigData->AccessPoint.StationAddress, sizeof (IP4_ADDR));
+      CopyMem (&SubnetMask, &TcpConfigData->AccessPoint.SubnetMask, sizeof (IP4_ADDR));
+      if (!NetIp4IsUnicast (NTOHL (Ip), 0) || !IP4_IS_VALID_NETMASK (NTOHL (SubnetMask))) {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+
+    Option = TcpConfigData->ControlOption;
+    if ((NULL != Option) &&
+        (Option->EnableSelectiveAck || Option->EnablePathMtuDiscovery)) {
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  if (NULL == TcpConfigData) {
+    return SockFlush (Sock);
+  }
+
+  Status = SockConfigure (Sock, TcpConfigData);
+
+  if (EFI_NO_MAPPING == Status) {
+    Sock->ConfigureState = SO_NO_MAPPING;
+  }
+
+  return Status;
+}
+
+
+/**
+  Add or delete routing entries.
+  
+  The Routes() function adds or deletes a route from the instance's routing table.
+  The most specific route is selected by comparing the SubnetAddress with the 
+  destination IP address's arithmetical AND to the SubnetMask.
+  The default route is added with both SubnetAddress and SubnetMask set to 0.0.0.0. 
+  The default route matches all destination IP addresses if there is no more specific route.
+  Direct route is added with GatewayAddress set to 0.0.0.0. Packets are sent to 
+  the destination host if its address can be found in the Address Resolution Protocol (ARP) 
+  cache or it is on the local subnet. If the instance is configured to use default 
+  address, a direct route to the local network will be added automatically.
+  Each TCP instance has its own independent routing table. Instance that uses the 
+  default IP address will have a copy of the EFI_IP4_CONFIG_PROTOCOL's routing table. 
+  The copy will be updated automatically whenever the IP driver reconfigures its 
+  instance. As a result, the previous modification to the instance's local copy 
+  will be lost. The priority of checking the route table is specific with IP 
+  implementation and every IP implementation must comply with RFC 1122.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  DeleteRoute              If TRUE, delete the specified route from routing
+                                   table; if FALSE, add the specified route to
+                                   routing table.
+                                   DestinationAddress and SubnetMask are used as 
+                                   the keywords to search route entry.
+  @param  SubnetAddress            The destination network.
+  @param  SubnetMask               The subnet mask for the destination network.
+  @param  GatewayAddress           The gateway address for this route. 
+                                   It must be on the same subnet with the station 
+                                   address unless a direct route is specified.
+                                   
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance has not been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (through DHCP, BOOTP, RARP, etc.) is not
+                                   finished.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resources to add the
+                                   entry to the routing table.
+  @retval EFI_NOT_FOUND            This route is not in the routing table.
+  @retval EFI_ACCESS_DENIED        This route is already in the routing table.
+  @retval EFI_UNSUPPORTED          The TCP driver does not support this operation.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Routes (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN BOOLEAN                     DeleteRoute,
+  IN EFI_IPv4_ADDRESS            *SubnetAddress,
+  IN EFI_IPv4_ADDRESS            *SubnetMask,
+  IN EFI_IPv4_ADDRESS            *GatewayAddress
+  )
+{
+  SOCKET          *Sock;
+  TCP4_ROUTE_INFO RouteInfo;
+
+  if (NULL == This) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock                      = SOCK_FROM_THIS (This);
+
+  RouteInfo.DeleteRoute     = DeleteRoute;
+  RouteInfo.SubnetAddress   = SubnetAddress;
+  RouteInfo.SubnetMask      = SubnetMask;
+  RouteInfo.GatewayAddress  = GatewayAddress;
+
+  return SockRoute (Sock, &RouteInfo);
+}
+
+
+/**
+  Initiate a nonblocking TCP connection request for an active TCP instance.
+
+  The Connect() function will initiate an active open to the remote peer configured 
+  in current TCP instance if it is configured active. If the connection succeeds 
+  or fails due to any error, the ConnectionToken->CompletionToken.Event will be 
+  signaled and ConnectionToken->CompletionToken.Status will be updated accordingly. 
+  This function can only be called for the TCP instance in Tcp4StateClosed state. 
+  The instance will transfer into Tcp4StateSynSent if the function returns EFI_SUCCESS. 
+  If TCP three way handshake succeeds, its state will become Tcp4StateEstablished, 
+  otherwise, the state will return to Tcp4StateClosed.
+  
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  ConnectionToken          Pointer to the connection token to return when
+                                   the TCP three way handshake finishes.
+
+  @retval EFI_SUCCESS              The connection request is successfully initiated 
+                                   and the state of this TCPv4 instance has 
+                                   been changed to Tcp4StateSynSent.
+  @retval EFI_NOT_STARTED          This EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        The instance is not configured as an active one
+                                   or it is not in Tcp4StateClosed state.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     The driver can't allocate enough resource to
+                                   initiate the active open.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Connect (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_CONNECTION_TOKEN   *ConnectionToken
+  )
+{
+  SOCKET  *Sock;
+
+  if (NULL == This ||
+      NULL == ConnectionToken ||
+      NULL == ConnectionToken->CompletionToken.Event) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  return SockConnect (Sock, ConnectionToken);
+}
+
+
+/**
+  Listen on the passive instance to accept an incoming connection request.
+
+  The Accept() function initiates an asynchronous accept request to wait for an 
+  incoming connection on the passive TCP instance. If a remote peer successfully 
+  establishes a connection with this instance, a new TCP instance will be created 
+  and its handle will be returned in ListenToken->NewChildHandle. The newly created 
+  instance is configured by inheriting the passive instance's configuration and is 
+  ready for use upon return. The instance is in the Tcp4StateEstablished state.
+  The ListenToken->CompletionToken.Event will be signaled when a new connection 
+  is accepted, user aborts the listen or connection is reset. This function only 
+  can be called when current TCP instance is in Tcp4StateListen state.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  ListenToken              Pointer to the listen token to return when
+                                   operation finishes.
+
+  @retval EFI_SUCCESS              The listen token has been queued successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        The instatnce is not a passive one or it is not
+                                   in Tcp4StateListen state or a same listen token
+                                   has already existed in the listen token queue of
+                                   this TCP instance.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resources to finish
+                                   the operation.
+  @retval EFI_DEVICE_ERROR         Any unexpected and not belonged to above category error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Accept (
+  IN EFI_TCP4_PROTOCOL             *This,
+  IN EFI_TCP4_LISTEN_TOKEN         *ListenToken
+  )
+{
+  SOCKET  *Sock;
+
+  if (NULL == This ||
+      NULL == ListenToken ||
+      NULL == ListenToken->CompletionToken.Event) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  return SockAccept (Sock, ListenToken);
+}
+
+
+/**
+  Queues outgoing data into the transmit queue.
+
+  The Transmit() function queues a sending request to this TCPv4 instance along 
+  with the user data. The status of the token is updated and the event in the token 
+  will be signaled once the data is sent out or some error occurs.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  Token                    Pointer to the completion token to queue to the
+                                   transmit queue
+
+  @retval EFI_SUCCESS              The data has been queued for transmission.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (DHCP, BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED        One or more of the following conditions is TRUE:
+                                   * A transmit completion token with the same
+                                     Token-> CompletionToken.Event was already in the
+                                     transmission queue. 
+                                   * The current instance is in Tcp4StateClosed state 
+                                   * The current instance is a passive one and 
+                                     it is in Tcp4StateListen state. 
+                                   * User has called Close() to disconnect this 
+                                     connection.
+  @retval EFI_NOT_READY            The completion token could not be queued because
+                                   the transmit queue is full.
+  @retval EFI_OUT_OF_RESOURCES     Could not queue the transmit data because of
+                                   resource shortage.
+  @retval EFI_NETWORK_UNREACHABLE  There is no route to the destination network or
+                                   address.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Transmit (
+  IN EFI_TCP4_PROTOCOL            *This,
+  IN EFI_TCP4_IO_TOKEN            *Token
+  )
+{
+  SOCKET      *Sock;
+  EFI_STATUS  Status;
+
+  if (NULL == This ||
+      NULL == Token ||
+      NULL == Token->CompletionToken.Event ||
+      NULL == Token->Packet.TxData ||
+      0 == Token->Packet.TxData->FragmentCount ||
+      0 == Token->Packet.TxData->DataLength
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = Tcp4ChkDataBuf (
+            (UINT32) Token->Packet.TxData->DataLength,
+            (UINT32) Token->Packet.TxData->FragmentCount,
+            Token->Packet.TxData->FragmentTable
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  return SockSend (Sock, Token);
+
+}
+
+
+/**
+  Place an asynchronous receive request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet queue. 
+  This function is always asynchronous. The caller must allocate the 
+  Token->CompletionToken.Event and the FragmentBuffer used to receive data. He also 
+  must fill the DataLength which represents the whole length of all FragmentBuffer. 
+  When the receive operation completes, the EFI TCPv4 Protocol driver updates the 
+  Token->CompletionToken.Status and Token->Packet.RxData fields and the 
+  Token->CompletionToken.Event is signaled. If got data the data and its length 
+  will be copy into the FragmentTable, in the same time the full length of received 
+  data will be recorded in the DataLength fields. Providing a proper notification 
+  function and context for the event will enable the user to receive the notification 
+  and receiving status. That notification function is guaranteed to not be re-entered.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Token                    Pointer to a token that is associated with the
+                                   receive data descriptor.
+
+  @retval EFI_SUCCESS              The receive completion token was cached.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (DHCP, BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     The receive completion token could not be queued
+                                   due to a lack of system resources.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+                                   The EFI TCPv4 Protocol instance has been reset 
+                                   to startup defaults.
+  @retval EFI_ACCESS_DENIED        One or more of the following conditions is TRUE:
+                                   * A receive completion token with the same
+                                     Token->CompletionToken.Event was already in 
+                                     the receive queue. 
+                                   * The current instance is in Tcp4StateClosed state. 
+                                   * The current instance is a passive one and it 
+                                     is in Tcp4StateListen state. 
+                                   * User has called Close() to disconnect this 
+                                     connection.
+  @retval EFI_CONNECTION_FIN       The communication peer has closed the connection
+                                   and there is no any buffered data in the receive
+                                   buffer of this instance.
+  @retval EFI_NOT_READY            The receive request could not be queued because
+                                   the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Receive (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_IO_TOKEN           *Token
+  )
+{
+  SOCKET      *Sock;
+  EFI_STATUS  Status;
+
+  if (NULL == This ||
+      NULL == Token ||
+      NULL == Token->CompletionToken.Event ||
+      NULL == Token->Packet.RxData ||
+      0 == Token->Packet.RxData->FragmentCount ||
+      0 == Token->Packet.RxData->DataLength
+      ) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = Tcp4ChkDataBuf (
+            (UINT32) Token->Packet.RxData->DataLength,
+            (UINT32) Token->Packet.RxData->FragmentCount,
+            Token->Packet.RxData->FragmentTable
+            );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  return SockRcv (Sock, Token);
+
+}
+
+
+/**
+  Disconnecting a TCP connection gracefully or reset a TCP connection.
+
+  Initiate an asynchronous close token to TCP driver. After Close() is called, 
+  any buffered transmission data will be sent by TCP driver and the current 
+  instance will have a graceful close working flow described as RFC 793 if 
+  AbortOnClose is set to FALSE, otherwise, a rest packet will be sent by TCP 
+  driver to fast disconnect this connection. When the close operation completes 
+  successfully the TCP instance is in Tcp4StateClosed state, all pending 
+  asynchronous operation is signaled and any buffers used for TCP network traffic 
+  is flushed.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  CloseToken               Pointer to the close token to return when
+                                   operation finishes.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        One or more of the following are TRUE: 
+                                   * Configure() has been called with TcpConfigData
+                                     set to NULL and this function has not returned.
+                                   * Previous Close() call on this instance has not
+                                     finished.
+  @retval EFI_INVALID_PARAMETER    One ore more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resource to finish the
+                                   operation.
+  @retval EFI_DEVICE_ERROR         Any unexpected and not belonged to above
+                                   category error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Close (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_CLOSE_TOKEN        *CloseToken
+  )
+{
+  SOCKET  *Sock;
+
+  if (NULL == This ||
+      NULL == CloseToken ||
+      NULL == CloseToken->CompletionToken.Event) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock = SOCK_FROM_THIS (This);
+
+  return SockClose (Sock, CloseToken, CloseToken->AbortOnClose);
+}
+
+
+/**
+  Abort an asynchronous connection, listen, transmission or receive request.
+
+  The Cancel() function aborts a pending connection, listen, transmit or receive 
+  request. If Token is not NULL and the token is in the connection, listen, 
+  transmission or receive queue when it is being cancelled, its Token->Status 
+  will be set to EFI_ABORTED and then Token->Event will be signaled. If the token 
+  is not in one of the queues, which usually means that the asynchronous operation 
+  has completed, EFI_NOT_FOUND is returned. If Token is NULL all asynchronous token 
+  issued by Connect(), Accept(), Transmit() and Receive()will be aborted.
+  NOTE: It has not been implemented currently.
+    
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Token                    Pointer to a token that has been issued by
+                                   Connect(), Accept(), Transmit() or Receive(). If
+                                   NULL, all pending tokens issued by above four
+                                   functions will be aborted.
+                                   
+  @retval  EFI_SUCCESS             The asynchronous I/O request is aborted and Token->Event
+                                   is signaled.
+  @retval  EFI_INVALID_PARAMETER   This is NULL.
+  @retval  EFI_NOT_STARTED         This instance hasn's been configured.
+  @retval  EFI_NO_MAPPING          When using the default address, configuration
+                                   (DHCP, BOOTP,RARP, etc.) hasn's finished yet.
+  @retval  EFI_NOT_FOUND           The asynchronous I/O request isn's found in the 
+                                   transmission or receive queue. It has either 
+                                   completed or wasn's issued by Transmit() and Receive().
+  @retval  EFI_UNSUPPORTED         The operation is not supported in current
+                                   implementation.
+  
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Cancel (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_COMPLETION_TOKEN   *Token    OPTIONAL
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Poll to receive incoming data and transmit outgoing segments.
+
+  The Poll() function increases the rate that data is moved between the network 
+  and application and can be called when the TCP instance is created successfully. 
+  Its use is optional. In some implementations, the periodical timer in the MNP 
+  driver may not poll the underlying communications device fast enough to avoid 
+  drop packets. Drivers and applications that are experiencing packet loss should 
+  try calling the Poll() function in a high frequency.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS              Incoming or outgoing data was processed.
+  @retval EFI_INVALID_PARAMETER    This is NULL.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+  @retval EFI_NOT_READY            No incoming or outgoing data was processed.
+  @retval EFI_TIMEOUT              Data was dropped out of the transmission or
+                                   receive queue. Consider increasing the polling
+                                   rate.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Poll (
+  IN EFI_TCP4_PROTOCOL        *This
+  )
+{
+  SOCKET      *Sock;
+  EFI_STATUS  Status;
+
+  if (NULL == This) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Sock    = SOCK_FROM_THIS (This);
+
+  Status  = Sock->ProtoHandler (Sock, SOCK_POLL, NULL);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.h
new file mode 100644
index 0000000000..cd904584bf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Main.h
@@ -0,0 +1,494 @@
+/** @file
+  TCP4 protocol services header file.
+
+Copyright (c) 2005 - 2012, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _TCP4_MAIN_H_
+#define _TCP4_MAIN_H_
+
+#include "Socket.h"
+
+#include "Tcp4Proto.h"
+#include "Tcp4Func.h"
+#include "Tcp4Driver.h"
+
+
+extern UINT16  mTcp4RandomPort;
+extern CHAR16  *mTcpStateName[];
+
+//
+// Driver Produced Protocol Prototypes
+//
+
+//
+// Function prototype for the Tcp4 socket request handler
+//
+/**
+  The procotol handler provided to the socket layer, used to
+  dispatch the socket level requests by calling the corresponding
+  TCP layer functions.
+
+  @param  Sock                   Pointer to the socket of this TCP instance.
+  @param  Request                The code of this operation request.
+  @param  Data                   Pointer to the operation specific data passed in
+                                 together with the operation request.
+
+  @retval EFI_SUCCESS            The socket request is completed successfully.
+  @retval other                  The error status returned by the corresponding TCP
+                                 layer function.
+
+**/
+EFI_STATUS
+Tcp4Dispatcher (
+  IN SOCKET                  *Sock,
+  IN UINT8                   Request,
+  IN VOID                    *Data    OPTIONAL
+  );
+  
+///
+/// TCP mode data
+///
+typedef struct _TCP4_MODE_DATA {
+  EFI_TCP4_CONNECTION_STATE       *Tcp4State;
+  EFI_TCP4_CONFIG_DATA            *Tcp4ConfigData;
+  EFI_IP4_MODE_DATA               *Ip4ModeData;
+  EFI_MANAGED_NETWORK_CONFIG_DATA *MnpConfigData;
+  EFI_SIMPLE_NETWORK_MODE         *SnpModeData;
+} TCP4_MODE_DATA;
+
+///
+/// TCP route infomation data
+///
+typedef struct _TCP4_ROUTE_INFO {
+  BOOLEAN           DeleteRoute;
+  EFI_IPv4_ADDRESS  *SubnetAddress;
+  EFI_IPv4_ADDRESS  *SubnetMask;
+  EFI_IPv4_ADDRESS  *GatewayAddress;
+} TCP4_ROUTE_INFO;
+
+typedef struct {
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+} TCP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT;
+
+/**
+  Get the current operational status of a TCP instance.
+  
+  The GetModeData() function copies the current operational settings of this 
+  EFI TCPv4 Protocol instance into user-supplied buffers. This function can 
+  also be used to retrieve the operational setting of underlying drivers 
+  such as IPv4, MNP, or SNP.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Tcp4State                Pointer to the buffer to receive the current TCP
+                                   state.
+  @param  Tcp4ConfigData           Pointer to the buffer to receive the current TCP
+                                   configuration.
+  @param  Ip4ModeData              Pointer to the buffer to receive the current IPv4 
+                                   configuration data used by the TCPv4 instance.
+  @param  MnpConfigData            Pointer to the buffer to receive the current MNP
+                                   configuration data indirectly used by the TCPv4
+                                   Instance.
+  @param  SnpModeData              Pointer to the buffer to receive the current SNP
+                                   configuration data indirectly used by the TCPv4
+                                   Instance.
+
+  @retval EFI_SUCCESS              The mode data was read.
+  @retval EFI_NOT_STARTED          No configuration data is available because this
+                                   instance hasn't been started.
+  @retval EFI_INVALID_PARAMETER    This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4GetModeData (
+  IN      EFI_TCP4_PROTOCOL                  *This,
+     OUT  EFI_TCP4_CONNECTION_STATE          *Tcp4State       OPTIONAL,
+     OUT  EFI_TCP4_CONFIG_DATA               *Tcp4ConfigData  OPTIONAL,
+     OUT  EFI_IP4_MODE_DATA                  *Ip4ModeData     OPTIONAL,
+     OUT  EFI_MANAGED_NETWORK_CONFIG_DATA    *MnpConfigData   OPTIONAL,
+     OUT  EFI_SIMPLE_NETWORK_MODE            *SnpModeData     OPTIONAL
+  );
+
+
+/**
+  Initialize or brutally reset the operational parameters for
+  this EFI TCPv4 instance.
+  
+  The Configure() function does the following:
+  * Initialize this EFI TCPv4 instance, i.e., initialize the communication end 
+  setting, specify active open or passive open for an instance.
+  * Reset this TCPv4 instance brutally, i.e., cancel all pending asynchronous 
+  tokens, flush transmission and receiving buffer directly without informing 
+  the communication peer.
+  No other TCPv4 Protocol operation can be executed by this instance 
+  until it is configured properly. For an active TCP4 instance, after a proper 
+  configuration it may call Connect() to initiates the three-way handshake. 
+  For a passive TCP4 instance, its state will transit to Tcp4StateListen after 
+  configuration, and Accept() may be called to listen the incoming TCP connection 
+  request. If TcpConfigData is set to NULL, the instance is reset. Resetting 
+  process will be done brutally, the state machine will be set to Tcp4StateClosed 
+  directly, the receive queue and transmit queue will be flushed, and no traffic is 
+  allowed through this instance.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  TcpConfigData            Pointer to the configure data to configure the
+                                   instance.
+
+  @retval EFI_SUCCESS              The operational settings are set, changed, or
+                                   reset successfully.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (through DHCP, BOOTP, RARP, etc.) is not
+                                   finished.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED        Configuring TCP instance when it is already
+                                   configured.
+  @retval EFI_DEVICE_ERROR         An unexpected network or system error occurred.
+  @retval EFI_UNSUPPORTED          One or more of the control options are not
+                                   supported in the implementation.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough system resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Configure (
+  IN EFI_TCP4_PROTOCOL        *This,
+  IN EFI_TCP4_CONFIG_DATA     *TcpConfigData     OPTIONAL
+  );
+
+/**
+  Add or delete routing entries.
+  
+  The Routes() function adds or deletes a route from the instance's routing table.
+  The most specific route is selected by comparing the SubnetAddress with the 
+  destination IP address's arithmetical AND to the SubnetMask.
+  The default route is added with both SubnetAddress and SubnetMask set to 0.0.0.0. 
+  The default route matches all destination IP addresses if there is no more specific route.
+  Direct route is added with GatewayAddress set to 0.0.0.0. Packets are sent to 
+  the destination host if its address can be found in the Address Resolution Protocol (ARP) 
+  cache or it is on the local subnet. If the instance is configured to use default 
+  address, a direct route to the local network will be added automatically.
+  Each TCP instance has its own independent routing table. Instance that uses the 
+  default IP address will have a copy of the EFI_IP4_CONFIG_PROTOCOL's routing table. 
+  The copy will be updated automatically whenever the IP driver reconfigures its 
+  instance. As a result, the previous modification to the instance's local copy 
+  will be lost. The priority of checking the route table is specific with IP 
+  implementation and every IP implementation must comply with RFC 1122.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  DeleteRoute              If TRUE, delete the specified route from routing
+                                   table; if FALSE, add the specified route to
+                                   routing table.
+                                   DestinationAddress and SubnetMask are used as 
+                                   the keywords to search route entry.
+  @param  SubnetAddress            The destination network.
+  @param  SubnetMask               The subnet mask for the destination network.
+  @param  GatewayAddress           The gateway address for this route. 
+                                   It must be on the same subnet with the station 
+                                   address unless a direct route is specified.
+                                   
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance has not been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (through DHCP, BOOTP, RARP, etc.) is not
+                                   finished.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resources to add the
+                                   entry to the routing table.
+  @retval EFI_NOT_FOUND            This route is not in the routing table.
+  @retval EFI_ACCESS_DENIED        This route is already in the routing table.
+  @retval EFI_UNSUPPORTED          The TCP driver does not support this operation.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Routes (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN BOOLEAN                     DeleteRoute,
+  IN EFI_IPv4_ADDRESS            *SubnetAddress,
+  IN EFI_IPv4_ADDRESS            *SubnetMask,
+  IN EFI_IPv4_ADDRESS            *GatewayAddress
+  );
+
+/**
+  Initiate a nonblocking TCP connection request for an active TCP instance.
+
+  The Connect() function will initiate an active open to the remote peer configured 
+  in current TCP instance if it is configured active. If the connection succeeds 
+  or fails due to any error, the ConnectionToken->CompletionToken.Event will be 
+  signaled and ConnectionToken->CompletionToken.Status will be updated accordingly. 
+  This function can only be called for the TCP instance in Tcp4StateClosed state. 
+  The instance will transfer into Tcp4StateSynSent if the function returns EFI_SUCCESS. 
+  If TCP three way handshake succeeds, its state will become Tcp4StateEstablished, 
+  otherwise, the state will return to Tcp4StateClosed.
+  
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  ConnectionToken          Pointer to the connection token to return when
+                                   the TCP three way handshake finishes.
+
+  @retval EFI_SUCCESS              The connection request is successfully initiated 
+                                   and the state of this TCPv4 instance has 
+                                   been changed to Tcp4StateSynSent.
+  @retval EFI_NOT_STARTED          This EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        The instance is not configured as an active one
+                                   or it is not in Tcp4StateClosed state.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     The driver can't allocate enough resource to
+                                   initiate the active open.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Connect (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_CONNECTION_TOKEN   *ConnectionToken
+  );
+
+/**
+  Listen on the passive instance to accept an incoming connection request.
+
+  The Accept() function initiates an asynchronous accept request to wait for an 
+  incoming connection on the passive TCP instance. If a remote peer successfully 
+  establishes a connection with this instance, a new TCP instance will be created 
+  and its handle will be returned in ListenToken->NewChildHandle. The newly created 
+  instance is configured by inheriting the passive instance's configuration and is 
+  ready for use upon return. The instance is in the Tcp4StateEstablished state.
+  The ListenToken->CompletionToken.Event will be signaled when a new connection 
+  is accepted, user aborts the listen or connection is reset. This function only 
+  can be called when current TCP instance is in Tcp4StateListen state.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  ListenToken              Pointer to the listen token to return when
+                                   operation finishes.
+
+  @retval EFI_SUCCESS              The listen token has been queued successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        The instatnce is not a passive one or it is not
+                                   in Tcp4StateListen state or a same listen token
+                                   has already existed in the listen token queue of
+                                   this TCP instance.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resources to finish
+                                   the operation.
+  @retval EFI_DEVICE_ERROR         Any unexpected and not belonged to above category error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Accept (
+  IN EFI_TCP4_PROTOCOL             *This,
+  IN EFI_TCP4_LISTEN_TOKEN         *ListenToken
+  );
+
+/**
+  Queues outgoing data into the transmit queue.
+
+  The Transmit() function queues a sending request to this TCPv4 instance along 
+  with the user data. The status of the token is updated and the event in the token 
+  will be signaled once the data is sent out or some error occurs.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance
+  @param  Token                    Pointer to the completion token to queue to the
+                                   transmit queue
+
+  @retval EFI_SUCCESS              The data has been queued for transmission.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (DHCP, BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED        One or more of the following conditions is TRUE:
+                                   * A transmit completion token with the same
+                                     Token-> CompletionToken.Event was already in the
+                                     transmission queue. 
+                                   * The current instance is in Tcp4StateClosed state 
+                                   * The current instance is a passive one and 
+                                     it is in Tcp4StateListen state. 
+                                   * User has called Close() to disconnect this 
+                                     connection.
+  @retval EFI_NOT_READY            The completion token could not be queued because
+                                   the transmit queue is full.
+  @retval EFI_OUT_OF_RESOURCES     Could not queue the transmit data because of
+                                   resource shortage.
+  @retval EFI_NETWORK_UNREACHABLE  There is no route to the destination network or
+                                   address.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Transmit (
+  IN EFI_TCP4_PROTOCOL            *This,
+  IN EFI_TCP4_IO_TOKEN            *Token
+  );
+
+/**
+  Place an asynchronous receive request into the receiving queue.
+
+  The Receive() function places a completion token into the receive packet queue. 
+  This function is always asynchronous. The caller must allocate the 
+  Token->CompletionToken.Event and the FragmentBuffer used to receive data. He also 
+  must fill the DataLength which represents the whole length of all FragmentBuffer. 
+  When the receive operation completes, the EFI TCPv4 Protocol driver updates the 
+  Token->CompletionToken.Status and Token->Packet.RxData fields and the 
+  Token->CompletionToken.Event is signaled. If got data the data and its length 
+  will be copy into the FragmentTable, in the same time the full length of received 
+  data will be recorded in the DataLength fields. Providing a proper notification 
+  function and context for the event will enable the user to receive the notification 
+  and receiving status. That notification function is guaranteed to not be re-entered.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Token                    Pointer to a token that is associated with the
+                                   receive data descriptor.
+
+  @retval EFI_SUCCESS              The receive completion token was cached.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_NO_MAPPING           When using a default address, configuration
+                                   (DHCP, BOOTP, RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER    One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     The receive completion token could not be queued
+                                   due to a lack of system resources.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+                                   The EFI TCPv4 Protocol instance has been reset 
+                                   to startup defaults.
+  @retval EFI_ACCESS_DENIED        One or more of the following conditions is TRUE:
+                                   * A receive completion token with the same
+                                     Token->CompletionToken.Event was already in 
+                                     the receive queue. 
+                                   * The current instance is in Tcp4StateClosed state. 
+                                   * The current instance is a passive one and it 
+                                     is in Tcp4StateListen state. 
+                                   * User has called Close() to disconnect this 
+                                     connection.
+  @retval EFI_CONNECTION_FIN       The communication peer has closed the connection
+                                   and there is no any buffered data in the receive
+                                   buffer of this instance.
+  @retval EFI_NOT_READY            The receive request could not be queued because
+                                   the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Receive (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_IO_TOKEN           *Token
+  );
+
+/**
+  Disconnecting a TCP connection gracefully or reset a TCP connection.
+
+  Initiate an asynchronous close token to TCP driver. After Close() is called, 
+  any buffered transmission data will be sent by TCP driver and the current 
+  instance will have a graceful close working flow described as RFC 793 if 
+  AbortOnClose is set to FALSE, otherwise, a rest packet will be sent by TCP 
+  driver to fast disconnect this connection. When the close operation completes 
+  successfully the TCP instance is in Tcp4StateClosed state, all pending 
+  asynchronous operation is signaled and any buffers used for TCP network traffic 
+  is flushed.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  CloseToken               Pointer to the close token to return when
+                                   operation finishes.
+
+  @retval EFI_SUCCESS              The operation completed successfully.
+  @retval EFI_NOT_STARTED          The EFI_TCP4_PROTOCOL instance hasn't been
+                                   configured.
+  @retval EFI_ACCESS_DENIED        One or more of the following are TRUE: 
+                                   * Configure() has been called with TcpConfigData
+                                     set to NULL and this function has not returned.
+                                   * Previous Close() call on this instance has not
+                                     finished.
+  @retval EFI_INVALID_PARAMETER    One ore more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES     Could not allocate enough resource to finish the
+                                   operation.
+  @retval EFI_DEVICE_ERROR         Any unexpected and not belonged to above
+                                   category error.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Close (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_CLOSE_TOKEN        *CloseToken
+  );
+
+/**
+  Abort an asynchronous connection, listen, transmission or receive request.
+
+  The Cancel() function aborts a pending connection, listen, transmit or receive 
+  request. If Token is not NULL and the token is in the connection, listen, 
+  transmission or receive queue when it is being cancelled, its Token->Status 
+  will be set to EFI_ABORTED and then Token->Event will be signaled. If the token 
+  is not in one of the queues, which usually means that the asynchronous operation 
+  has completed, EFI_NOT_FOUND is returned. If Token is NULL all asynchronous token 
+  issued by Connect(), Accept(), Transmit() and Receive()will be aborted.
+  NOTE: It has not been implemented currently.
+    
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+  @param  Token                    Pointer to a token that has been issued by
+                                   Connect(), Accept(), Transmit() or Receive(). If
+                                   NULL, all pending tokens issued by above four
+                                   functions will be aborted.
+                                   
+  @retval  EFI_SUCCESS             The asynchronous I/O request is aborted and Token->Event
+                                   is signaled.
+  @retval  EFI_INVALID_PARAMETER   This is NULL.
+  @retval  EFI_NOT_STARTED         This instance hasn's been configured.
+  @retval  EFI_NO_MAPPING          When using the default address, configuration
+                                   (DHCP, BOOTP,RARP, etc.) hasn's finished yet.
+  @retval  EFI_NOT_FOUND           The asynchronous I/O request isn's found in the 
+                                   transmission or receive queue. It has either 
+                                   completed or wasn's issued by Transmit() and Receive().
+  @retval  EFI_UNSUPPORTED         The operation is not supported in current
+                                   implementation.
+  
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Cancel (
+  IN EFI_TCP4_PROTOCOL           *This,
+  IN EFI_TCP4_COMPLETION_TOKEN   *Token    OPTIONAL
+  );
+
+/**
+  Poll to receive incoming data and transmit outgoing segments.
+
+  The Poll() function increases the rate that data is moved between the network 
+  and application and can be called when the TCP instance is created successfully. 
+  Its use is optional. In some implementations, the periodical timer in the MNP 
+  driver may not poll the underlying communications device fast enough to avoid 
+  drop packets. Drivers and applications that are experiencing packet loss should 
+  try calling the Poll() function in a high frequency.
+
+  @param  This                     Pointer to the EFI_TCP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS              Incoming or outgoing data was processed.
+  @retval EFI_INVALID_PARAMETER    This is NULL.
+  @retval EFI_DEVICE_ERROR         An unexpected system or network error occurred.
+  @retval EFI_NOT_READY            No incoming or outgoing data was processed.
+  @retval EFI_TIMEOUT              Data was dropped out of the transmission or
+                                   receive queue. Consider increasing the polling
+                                   rate.
+
+**/
+EFI_STATUS
+EFIAPI
+Tcp4Poll (
+  IN EFI_TCP4_PROTOCOL        *This
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Misc.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Misc.c
new file mode 100644
index 0000000000..95f47f91bc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Misc.c
@@ -0,0 +1,939 @@
+/** @file
+  Misc support routines for tcp.
+
+Copyright (c) 2005 - 2015, 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<BR>
+
+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 "Tcp4Main.h"
+
+#include <Library/DevicePathLib.h>
+
+LIST_ENTRY      mTcpRunQue = {
+  &mTcpRunQue,
+  &mTcpRunQue
+};
+
+LIST_ENTRY      mTcpListenQue = {
+  &mTcpListenQue,
+  &mTcpListenQue
+};
+
+TCP_SEQNO       mTcpGlobalIss = 0x4d7e980b;
+
+CHAR16   *mTcpStateName[] = {
+  L"TCP_CLOSED",
+  L"TCP_LISTEN",
+  L"TCP_SYN_SENT",
+  L"TCP_SYN_RCVD",
+  L"TCP_ESTABLISHED",
+  L"TCP_FIN_WAIT_1",
+  L"TCP_FIN_WAIT_2",
+  L"TCP_CLOSING",
+  L"TCP_TIME_WAIT",
+  L"TCP_CLOSE_WAIT",
+  L"TCP_LAST_ACK"
+};
+
+
+/**
+  Initialize the Tcb local related members.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpInitTcbLocal (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  //
+  // Compute the checksum of the fixed parts of pseudo header
+  //
+  Tcb->HeadSum = NetPseudoHeadChecksum (
+                  Tcb->LocalEnd.Ip,
+                  Tcb->RemoteEnd.Ip,
+                  0x06,
+                  0
+                  );
+
+  Tcb->Iss    = TcpGetIss ();
+  Tcb->SndUna = Tcb->Iss;
+  Tcb->SndNxt = Tcb->Iss;
+
+  Tcb->SndWl2 = Tcb->Iss;
+  Tcb->SndWnd = 536;
+
+  Tcb->RcvWnd = GET_RCV_BUFFSIZE (Tcb->Sk);
+
+  //
+  // First window size is never scaled
+  //
+  Tcb->RcvWndScale  = 0;
+
+  Tcb->ProbeTimerOn = FALSE;
+}
+
+
+/**
+  Initialize the peer related members.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  Seg                   Pointer to the segment that contains the peer's
+                                intial info.
+  @param  Opt                   Pointer to the options announced by the peer.
+
+**/
+VOID
+TcpInitTcbPeer (
+  IN OUT TCP_CB     *Tcb,
+  IN     TCP_SEG    *Seg,
+  IN     TCP_OPTION *Opt
+  )
+{
+  UINT16  RcvMss;
+
+  ASSERT ((Tcb != NULL) && (Seg != NULL) && (Opt != NULL));
+  ASSERT (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN));
+
+  Tcb->SndWnd     = Seg->Wnd;
+  Tcb->SndWndMax  = Tcb->SndWnd;
+  Tcb->SndWl1     = Seg->Seq;
+
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_ACK)) {
+    Tcb->SndWl2 = Seg->Ack;
+  } else {
+    Tcb->SndWl2 = Tcb->Iss + 1;
+  }
+
+  if (TCP_FLG_ON (Opt->Flag, TCP_OPTION_RCVD_MSS)) {
+    Tcb->SndMss = (UINT16) MAX (64, Opt->Mss);
+
+    RcvMss = TcpGetRcvMss (Tcb->Sk);
+    if (Tcb->SndMss > RcvMss) {
+      Tcb->SndMss = RcvMss;
+    }
+
+  } else {
+    //
+    // One end doesn't support MSS option, use default.
+    //
+    Tcb->RcvMss = 536;
+  }
+
+  Tcb->CWnd   = Tcb->SndMss;
+
+  Tcb->Irs    = Seg->Seq;
+  Tcb->RcvNxt = Tcb->Irs + 1;
+
+  Tcb->RcvWl2 = Tcb->RcvNxt;
+
+  if (TCP_FLG_ON (Opt->Flag, TCP_OPTION_RCVD_WS) &&
+      !TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_WS)) {
+
+    Tcb->SndWndScale  = Opt->WndScale;
+
+    Tcb->RcvWndScale  = TcpComputeScale (Tcb);
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_RCVD_WS);
+
+  } else {
+    //
+    // One end doesn't support window scale option. use zero.
+    //
+    Tcb->RcvWndScale = 0;
+  }
+
+  if (TCP_FLG_ON (Opt->Flag, TCP_OPTION_RCVD_TS) &&
+      !TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_TS)) {
+
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_SND_TS);
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_RCVD_TS);
+
+    Tcb->TsRecent = Opt->TSVal;
+
+    //
+    // Compute the effective SndMss per RFC1122
+    // section 4.2.2.6. If timestamp option is
+    // enabled, it will always occupy 12 bytes.
+    //
+    Tcb->SndMss -= TCP_OPTION_TS_ALIGNED_LEN;
+  }
+}
+
+
+/**
+  Locate a listen TCB that matchs the Local and Remote.
+
+  @param  Local                 Pointer to the local (IP, Port).
+  @param  Remote                Pointer to the remote (IP, Port).
+
+  @return  Pointer to the TCP_CB with the least number of wildcard,
+           if NULL no match is found.
+
+**/
+TCP_CB *
+TcpLocateListenTcb (
+  IN TCP_PEER *Local,
+  IN TCP_PEER *Remote
+  )
+{
+  LIST_ENTRY      *Entry;
+  TCP_CB          *Node;
+  TCP_CB          *Match;
+  INTN            Last;
+  INTN            Cur;
+
+  Last  = 4;
+  Match = NULL;
+
+  NET_LIST_FOR_EACH (Entry, &mTcpListenQue) {
+    Node = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if ((Local->Port != Node->LocalEnd.Port) ||
+        !TCP_PEER_MATCH (Remote, &Node->RemoteEnd) ||
+        !TCP_PEER_MATCH (Local, &Node->LocalEnd)) {
+
+      continue;
+    }
+
+    //
+    // Compute the number of wildcard
+    //
+    Cur = 0;
+    if (Node->RemoteEnd.Ip == 0) {
+      Cur++;
+    }
+
+    if (Node->RemoteEnd.Port == 0) {
+      Cur++;
+    }
+
+    if (Node->LocalEnd.Ip == 0) {
+      Cur++;
+    }
+
+    if (Cur < Last) {
+      if (Cur == 0) {
+        return Node;
+      }
+
+      Last  = Cur;
+      Match = Node;
+    }
+  }
+
+  return Match;
+}
+
+
+/**
+  Try to find one Tcb whose <Ip, Port> equals to <IN Addr, IN Port>.
+
+  @param  Addr                  Pointer to the IP address needs to match.
+  @param  Port                  The port number needs to match.
+
+  @return  The Tcb which matches the <Addr Port> paire exists or not.
+
+**/
+BOOLEAN
+TcpFindTcbByPeer (
+  IN EFI_IPv4_ADDRESS  *Addr,
+  IN TCP_PORTNO        Port
+  )
+{
+  TCP_PORTNO      LocalPort;
+  LIST_ENTRY      *Entry;
+  TCP_CB          *Tcb;
+
+  ASSERT ((Addr != NULL) && (Port != 0));
+
+  LocalPort = HTONS (Port);
+
+  NET_LIST_FOR_EACH (Entry, &mTcpListenQue) {
+    Tcb = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if (EFI_IP4_EQUAL (Addr, &Tcb->LocalEnd.Ip) &&
+      (LocalPort == Tcb->LocalEnd.Port)) {
+
+      return TRUE;
+    }
+  }
+
+  NET_LIST_FOR_EACH (Entry, &mTcpRunQue) {
+    Tcb = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if (EFI_IP4_EQUAL (Addr, &Tcb->LocalEnd.Ip) &&
+      (LocalPort == Tcb->LocalEnd.Port)) {
+
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  Locate the TCP_CB related to the socket pair.
+
+  @param  LocalPort             The local port number.
+  @param  LocalIp               The local IP address.
+  @param  RemotePort            The remote port number.
+  @param  RemoteIp              The remote IP address.
+  @param  Syn                   Whether to search the listen sockets, if TRUE, the
+                                listen sockets are searched.
+
+  @return  Pointer to the related TCP_CB, if NULL no match is found.
+
+**/
+TCP_CB *
+TcpLocateTcb (
+  IN TCP_PORTNO  LocalPort,
+  IN UINT32      LocalIp,
+  IN TCP_PORTNO  RemotePort,
+  IN UINT32      RemoteIp,
+  IN BOOLEAN     Syn
+  )
+{
+  TCP_PEER        Local;
+  TCP_PEER        Remote;
+  LIST_ENTRY      *Entry;
+  TCP_CB          *Tcb;
+
+  Local.Port  = LocalPort;
+  Local.Ip    = LocalIp;
+
+  Remote.Port = RemotePort;
+  Remote.Ip   = RemoteIp;
+
+  //
+  // First check for exact match.
+  //
+  NET_LIST_FOR_EACH (Entry, &mTcpRunQue) {
+    Tcb = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if (TCP_PEER_EQUAL (&Remote, &Tcb->RemoteEnd) &&
+        TCP_PEER_EQUAL (&Local, &Tcb->LocalEnd)) {
+
+      RemoveEntryList (&Tcb->List);
+      InsertHeadList (&mTcpRunQue, &Tcb->List);
+
+      return Tcb;
+    }
+  }
+
+  //
+  // Only check listen queue when SYN flag is on
+  //
+  if (Syn) {
+    return TcpLocateListenTcb (&Local, &Remote);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Insert a Tcb into the proper queue.
+
+  @param  Tcb                   Pointer to the TCP_CB to be inserted.
+
+  @retval 0                     The Tcb is inserted successfully.
+  @retval -1                    Error condition occurred.
+
+**/
+INTN
+TcpInsertTcb (
+  IN TCP_CB *Tcb
+  )
+{
+  LIST_ENTRY       *Entry;
+  LIST_ENTRY       *Head;
+  TCP_CB           *Node;
+
+  ASSERT (
+    (Tcb != NULL) &&
+    ((Tcb->State == TCP_LISTEN) ||
+     (Tcb->State == TCP_SYN_SENT) ||
+     (Tcb->State == TCP_SYN_RCVD) ||
+     (Tcb->State == TCP_CLOSED))
+    );
+
+  if (Tcb->LocalEnd.Port == 0) {
+    return -1;
+  }
+
+  Head = &mTcpRunQue;
+
+  if (Tcb->State == TCP_LISTEN) {
+    Head = &mTcpListenQue;
+  }
+
+  //
+  // Check that Tcb isn't already on the list.
+  //
+  NET_LIST_FOR_EACH (Entry, Head) {
+    Node = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if (TCP_PEER_EQUAL (&Tcb->LocalEnd, &Node->LocalEnd) &&
+        TCP_PEER_EQUAL (&Tcb->RemoteEnd, &Node->RemoteEnd)) {
+
+      return -1;
+    }
+  }
+
+  InsertHeadList (Head, &Tcb->List);
+
+  return 0;
+}
+
+
+/**
+  Clone a TCB_CB from Tcb.
+
+  @param  Tcb                   Pointer to the TCP_CB to be cloned.
+
+  @return  Pointer to the new cloned TCP_CB, if NULL error condition occurred.
+
+**/
+TCP_CB *
+TcpCloneTcb (
+  IN TCP_CB *Tcb
+  )
+{
+  TCP_CB               *Clone;
+
+  Clone = AllocatePool (sizeof (TCP_CB));
+
+  if (Clone == NULL) {
+    return NULL;
+
+  }
+
+  CopyMem (Clone, Tcb, sizeof (TCP_CB));
+
+  //
+  // Increate the reference count of the shared IpInfo.
+  //
+  NET_GET_REF (Tcb->IpInfo);
+
+  InitializeListHead (&Clone->List);
+  InitializeListHead (&Clone->SndQue);
+  InitializeListHead (&Clone->RcvQue);
+
+  Clone->Sk = SockClone (Tcb->Sk);
+  if (Clone->Sk == NULL) {
+    DEBUG ((EFI_D_ERROR, "TcpCloneTcb: failed to clone a sock\n"));
+    FreePool (Clone);
+    return NULL;
+  }
+
+  ((TCP4_PROTO_DATA *) (Clone->Sk->ProtoReserved))->TcpPcb = Clone;
+
+  return Clone;
+}
+
+
+/**
+  Compute an ISS to be used by a new connection.
+
+  @return  The result ISS.
+
+**/
+TCP_SEQNO
+TcpGetIss (
+  VOID
+  )
+{
+  mTcpGlobalIss += 2048;
+  return mTcpGlobalIss;
+}
+
+
+/**
+  Get the local mss.
+
+  @param  Sock        Pointer to the socket to get mss
+
+  @return  The mss size.
+
+**/
+UINT16
+TcpGetRcvMss (
+  IN SOCKET  *Sock
+  )
+{
+  EFI_IP4_MODE_DATA       Ip4Mode;
+  TCP4_PROTO_DATA         *TcpProto;
+  EFI_IP4_PROTOCOL        *Ip;
+
+  ASSERT (Sock != NULL);
+
+  TcpProto = (TCP4_PROTO_DATA *) Sock->ProtoReserved;
+  Ip       = TcpProto->TcpService->IpIo->Ip.Ip4;
+  ASSERT (Ip != NULL);
+
+  Ip->GetModeData (Ip, &Ip4Mode, NULL, NULL);
+
+  return (UINT16) (Ip4Mode.MaxPacketSize - sizeof (TCP_HEAD));
+}
+
+
+/**
+  Set the Tcb's state.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  State                 The state to be set.
+
+**/
+VOID
+TcpSetState (
+  IN OUT TCP_CB  *Tcb,
+  IN     UINT8   State
+  )
+{
+  ASSERT (Tcb->State < (sizeof (mTcpStateName) / sizeof (CHAR16 *)));
+  ASSERT (State < (sizeof (mTcpStateName) / sizeof (CHAR16 *)));
+
+  DEBUG (
+    (EFI_D_INFO,
+    "Tcb (%p) state %s --> %s\n",
+    Tcb,
+    mTcpStateName[Tcb->State],
+    mTcpStateName[State])
+    );
+
+  Tcb->State = State;
+
+  switch (State) {
+  case TCP_ESTABLISHED:
+
+    SockConnEstablished (Tcb->Sk);
+
+    if (Tcb->Parent != NULL) {
+      //
+      // A new connection is accepted by a listening socket, install
+      // the device path.
+      //
+      TcpInstallDevicePath (Tcb->Sk);
+    }
+
+    break;
+
+  case TCP_CLOSED:
+
+    SockConnClosed (Tcb->Sk);
+
+    break;
+  default:
+    break;
+  }
+}
+
+
+/**
+  Compute the TCP segment's checksum.
+
+  @param  Nbuf                  Pointer to the buffer that contains the TCP
+                                segment.
+  @param  HeadSum               The checksum value of the fixed part of pseudo
+                                header.
+
+  @return  The checksum value.
+
+**/
+UINT16
+TcpChecksum (
+  IN NET_BUF *Nbuf,
+  IN UINT16  HeadSum
+  )
+{
+  UINT16  Checksum;
+
+  Checksum  = NetbufChecksum (Nbuf);
+  Checksum  = NetAddChecksum (Checksum, HeadSum);
+
+  Checksum  = NetAddChecksum (
+                Checksum,
+                HTONS ((UINT16) Nbuf->TotalSize)
+                );
+
+  return (UINT16) ~Checksum;
+}
+
+/**
+  Translate the information from the head of the received TCP
+  segment Nbuf contains and fill it into a TCP_SEG structure.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf                  Pointer to the buffer contains the TCP segment.
+
+  @return  Pointer to the TCP_SEG that contains the translated TCP head information.
+
+**/
+TCP_SEG *
+TcpFormatNetbuf (
+  IN     TCP_CB  *Tcb,
+  IN OUT NET_BUF *Nbuf
+  )
+{
+  TCP_SEG   *Seg;
+  TCP_HEAD  *Head;
+
+  Seg       = TCPSEG_NETBUF (Nbuf);
+  Head      = (TCP_HEAD *) NetbufGetByte (Nbuf, 0, NULL);
+  ASSERT (Head != NULL);
+  Nbuf->Tcp = Head;
+
+  Seg->Seq  = NTOHL (Head->Seq);
+  Seg->Ack  = NTOHL (Head->Ack);
+  Seg->End  = Seg->Seq + (Nbuf->TotalSize - (Head->HeadLen << 2));
+
+  Seg->Urg  = NTOHS (Head->Urg);
+  Seg->Wnd  = (NTOHS (Head->Wnd) << Tcb->SndWndScale);
+  Seg->Flag = Head->Flag;
+
+  //
+  // SYN and FIN flag occupy one sequence space each.
+  //
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+    //
+    // RFC requires that initial window not be scaled
+    //
+    Seg->Wnd = NTOHS (Head->Wnd);
+    Seg->End++;
+  }
+
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_FIN)) {
+    Seg->End++;
+  }
+
+  return Seg;
+}
+
+
+/**
+  Reset the connection related with Tcb.
+
+  @param  Tcb                   Pointer to the TCP_CB of the connection to be
+                                reset.
+
+**/
+VOID
+TcpResetConnection (
+  IN TCP_CB *Tcb
+  )
+{
+  NET_BUF   *Nbuf;
+  TCP_HEAD  *Nhead;
+
+  Nbuf = NetbufAlloc (TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    return ;
+  }
+
+  Nhead = (TCP_HEAD *) NetbufAllocSpace (
+                        Nbuf,
+                        sizeof (TCP_HEAD),
+                        NET_BUF_TAIL
+                        );
+
+  ASSERT (Nhead != NULL);
+
+  Nbuf->Tcp       = Nhead;
+
+  Nhead->Flag     = TCP_FLG_RST;
+  Nhead->Seq      = HTONL (Tcb->SndNxt);
+  Nhead->Ack      = HTONL (Tcb->RcvNxt);
+  Nhead->SrcPort  = Tcb->LocalEnd.Port;
+  Nhead->DstPort  = Tcb->RemoteEnd.Port;
+  Nhead->HeadLen  = (UINT8) (sizeof (TCP_HEAD) >> 2);
+  Nhead->Res      = 0;
+  Nhead->Wnd      = HTONS (0xFFFF);
+  Nhead->Checksum = 0;
+  Nhead->Urg      = 0;
+  Nhead->Checksum = TcpChecksum (Nbuf, Tcb->HeadSum);
+
+  TcpSendIpPacket (Tcb, Nbuf, Tcb->LocalEnd.Ip, Tcb->RemoteEnd.Ip);
+
+  NetbufFree (Nbuf);
+}
+
+
+/**
+  Initialize an active connection.
+
+  @param  Tcb                   Pointer to the TCP_CB that wants to initiate a
+                                connection.
+
+**/
+VOID
+TcpOnAppConnect (
+  IN OUT TCP_CB  *Tcb
+  )
+{
+  TcpInitTcbLocal (Tcb);
+  TcpSetState (Tcb, TCP_SYN_SENT);
+
+  TcpSetTimer (Tcb, TCP_TIMER_CONNECT, Tcb->ConnectTimeout);
+  TcpToSendData (Tcb, 1);
+}
+
+
+/**
+  Initiate the connection close procedure, called when
+  applications want to close the connection.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpOnAppClose (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  ASSERT (Tcb != NULL);
+
+  if (!IsListEmpty (&Tcb->RcvQue) || GET_RCV_DATASIZE (Tcb->Sk) != 0) {
+
+    DEBUG ((EFI_D_WARN, "TcpOnAppClose: connection reset "
+      "because data is lost for TCB %p\n", Tcb));
+
+    TcpResetConnection (Tcb);
+    TcpClose (Tcb);
+    return;
+  }
+
+  switch (Tcb->State) {
+  case TCP_CLOSED:
+  case TCP_LISTEN:
+  case TCP_SYN_SENT:
+    TcpSetState (Tcb, TCP_CLOSED);
+    break;
+
+  case TCP_SYN_RCVD:
+  case TCP_ESTABLISHED:
+    TcpSetState (Tcb, TCP_FIN_WAIT_1);
+    break;
+
+  case TCP_CLOSE_WAIT:
+    TcpSetState (Tcb, TCP_LAST_ACK);
+    break;
+  default:
+    break;
+  }
+
+  TcpToSendData (Tcb, 1);
+}
+
+
+/**
+  Check whether the application's newly delivered data can be sent out.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0                     Whether the data is sent out or is buffered for
+                                further sending.
+  @retval -1                    The Tcb is not in a state that data is permitted to
+                                be sent out.
+
+**/
+INTN
+TcpOnAppSend (
+  IN OUT TCP_CB *Tcb
+  )
+{
+
+  switch (Tcb->State) {
+  case TCP_CLOSED:
+    return -1;
+
+  case TCP_LISTEN:
+    return -1;
+
+  case TCP_SYN_SENT:
+  case TCP_SYN_RCVD:
+    return 0;
+
+  case TCP_ESTABLISHED:
+  case TCP_CLOSE_WAIT:
+    TcpToSendData (Tcb, 0);
+    return 0;
+
+  case TCP_FIN_WAIT_1:
+  case TCP_FIN_WAIT_2:
+  case TCP_CLOSING:
+  case TCP_LAST_ACK:
+  case TCP_TIME_WAIT:
+    return -1;
+
+  default:
+    break;
+  }
+
+  return 0;
+}
+
+
+/**
+  Application has consumed some data, check whether
+  to send a window updata ack or a delayed ack.
+
+  @param  Tcb                   Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpOnAppConsume (
+  IN TCP_CB *Tcb
+  )
+{
+  UINT32 TcpOld;
+
+  switch (Tcb->State) {
+  case TCP_CLOSED:
+    return;
+
+  case TCP_LISTEN:
+    return;
+
+  case TCP_SYN_SENT:
+  case TCP_SYN_RCVD:
+    return;
+
+  case TCP_ESTABLISHED:
+    TcpOld = TcpRcvWinOld (Tcb);
+    if (TcpRcvWinNow (Tcb) > TcpOld) {
+
+      if (TcpOld < Tcb->RcvMss) {
+
+        DEBUG ((EFI_D_INFO, "TcpOnAppConsume: send a window"
+          " update for a window closed Tcb %p\n", Tcb));
+
+        TcpSendAck (Tcb);
+      } else if (Tcb->DelayedAck == 0) {
+
+        DEBUG ((EFI_D_INFO, "TcpOnAppConsume: scheduled a delayed"
+          " ACK to update window for Tcb %p\n", Tcb));
+
+        Tcb->DelayedAck = 1;
+      }
+    }
+
+    break;
+
+  case TCP_CLOSE_WAIT:
+    return;
+
+  case TCP_FIN_WAIT_1:
+  case TCP_FIN_WAIT_2:
+  case TCP_CLOSING:
+  case TCP_LAST_ACK:
+  case TCP_TIME_WAIT:
+    return;
+
+  default:
+    break;
+  }
+}
+
+
+/**
+  Abort the connection by sending a reset segment, called
+  when the application wants to abort the connection.
+
+  @param  Tcb                   Pointer to the TCP_CB of the TCP instance.
+
+**/
+VOID
+TcpOnAppAbort (
+  IN TCP_CB *Tcb
+  )
+{
+  DEBUG ((EFI_D_WARN, "TcpOnAppAbort: connection reset "
+    "issued by application for TCB %p\n", Tcb));
+
+  switch (Tcb->State) {
+  case TCP_SYN_RCVD:
+  case TCP_ESTABLISHED:
+  case TCP_FIN_WAIT_1:
+  case TCP_FIN_WAIT_2:
+  case TCP_CLOSE_WAIT:
+    TcpResetConnection (Tcb);
+    break;
+  default:
+    break;
+  }
+
+  TcpSetState (Tcb, TCP_CLOSED);
+}
+
+/**
+  Install the device path protocol on the TCP instance.
+
+  @param  Sock             Pointer to the socket representing the TCP instance.
+
+  @retval  EFI_SUCCESS     The device path protocol is installed.
+  @retval  other           Failed to install the device path protocol.
+
+**/
+EFI_STATUS
+TcpInstallDevicePath (
+  IN SOCKET *Sock
+  )
+{
+  TCP4_PROTO_DATA    *TcpProto;
+  TCP4_SERVICE_DATA  *TcpService;
+  TCP_CB             *Tcb;
+  IPv4_DEVICE_PATH   Ip4DPathNode;
+  EFI_STATUS         Status;
+  TCP_PORTNO         LocalPort;
+  TCP_PORTNO         RemotePort;
+
+  TcpProto   = (TCP4_PROTO_DATA *) Sock->ProtoReserved;
+  TcpService = TcpProto->TcpService;
+  Tcb        = TcpProto->TcpPcb;
+
+  LocalPort = NTOHS (Tcb->LocalEnd.Port);
+  RemotePort = NTOHS (Tcb->RemoteEnd.Port);
+  NetLibCreateIPv4DPathNode (
+    &Ip4DPathNode,
+    TcpService->ControllerHandle,
+    Tcb->LocalEnd.Ip,
+    LocalPort,
+    Tcb->RemoteEnd.Ip,
+    RemotePort,
+    EFI_IP_PROTO_TCP,
+    Tcb->UseDefaultAddr
+    );
+
+  IP4_COPY_ADDRESS (&Ip4DPathNode.SubnetMask, &Tcb->SubnetMask);
+
+  Sock->DevicePath = AppendDevicePathNode (
+                       Sock->ParentDevicePath,
+                       (EFI_DEVICE_PATH_PROTOCOL *) &Ip4DPathNode
+                       );
+  if (Sock->DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &Sock->SockHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  Sock->DevicePath
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Sock->DevicePath);
+  }
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.c
new file mode 100644
index 0000000000..0da249c3cc
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.c
@@ -0,0 +1,380 @@
+/** @file
+  Routines to process TCP option.
+    
+Copyright (c) 2005 - 2006, 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<BR>
+
+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 "Tcp4Main.h"
+
+/**
+    Get a UINT16 value from buffer.
+    
+    @param Buf                  Pointer to input buffer.
+    
+    @return                     The UINT16 value get from buffer.
+
+**/
+UINT16
+TcpGetUint16 (
+  IN UINT8 *Buf
+  )
+{
+  UINT16  Value;
+  CopyMem (&Value, Buf, sizeof (UINT16));
+  return NTOHS (Value);
+}
+
+/**
+    Get a UINT32 value from buffer.
+    
+    @param Buf                  Pointer to input buffer.
+    
+    @return                     The UINT32 value get from buffer.
+
+**/
+UINT32
+TcpGetUint32 (
+  IN UINT8 *Buf
+  )
+{
+  UINT32  Value;
+  CopyMem (&Value, Buf, sizeof (UINT32));
+  return NTOHL (Value);
+}
+
+/**
+    Put a UINT32 value in buffer.
+    
+    @param Buf                  Pointer to the buffer.
+    @param Data                 The UINT32 Date to put in buffer 
+
+**/
+VOID
+TcpPutUint32 (
+     OUT UINT8  *Buf,
+  IN     UINT32 Data
+  )
+{
+  Data = HTONL (Data);
+  CopyMem (Buf, &Data, sizeof (UINT32));
+}
+
+
+/**
+  Compute the window scale value according to the given buffer size.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return         The scale value.
+
+**/
+UINT8
+TcpComputeScale (
+  IN TCP_CB *Tcb
+  )
+{
+  UINT8   Scale;
+  UINT32  BufSize;
+
+  ASSERT ((Tcb != NULL) && (Tcb->Sk != NULL));
+
+  BufSize = GET_RCV_BUFFSIZE (Tcb->Sk);
+
+  Scale   = 0;
+  while ((Scale < TCP_OPTION_MAX_WS) &&
+         ((UINT32) (TCP_OPTION_MAX_WIN << Scale) < BufSize)) {
+
+    Scale++;
+  }
+
+  return Scale;
+}
+
+
+/**
+  Build the TCP option in three-way handshake.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf    Pointer to the buffer to store the options.
+
+  @return         The total length of the TCP option field.
+
+**/
+UINT16
+TcpSynBuildOption (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  )
+{
+  UINT8   *Data;
+  UINT16  Len;
+
+  ASSERT ((Tcb != NULL) && (Nbuf != NULL) && (Nbuf->Tcp == NULL));
+
+  Len = 0;
+
+  //
+  // Add timestamp option if not disabled by application
+  // and it is the first SYN segment or the peer has sent
+  // us its timestamp.
+  //
+  if (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_TS) &&
+      (!TCP_FLG_ON (TCPSEG_NETBUF (Nbuf)->Flag, TCP_FLG_ACK) ||
+        TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RCVD_TS))) {
+
+    Data = NetbufAllocSpace (
+            Nbuf,
+            TCP_OPTION_TS_ALIGNED_LEN,
+            NET_BUF_HEAD
+            );
+
+    ASSERT (Data != NULL);
+    Len += TCP_OPTION_TS_ALIGNED_LEN;
+
+    TcpPutUint32 (Data, TCP_OPTION_TS_FAST);
+    TcpPutUint32 (Data + 4, mTcpTick);
+    TcpPutUint32 (Data + 8, 0);
+  }
+
+  //
+  // Build window scale option, only when are configured
+  // to send WS option, and either we are doing active
+  // open or we have received WS option from peer.
+  //
+  if (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_WS) &&
+      (!TCP_FLG_ON (TCPSEG_NETBUF (Nbuf)->Flag, TCP_FLG_ACK) ||
+        TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RCVD_WS))) {
+
+    Data = NetbufAllocSpace (
+            Nbuf,
+            TCP_OPTION_WS_ALIGNED_LEN,
+            NET_BUF_HEAD
+            );
+
+    ASSERT (Data != NULL);
+
+    Len += TCP_OPTION_WS_ALIGNED_LEN;
+    TcpPutUint32 (Data, TCP_OPTION_WS_FAST | TcpComputeScale (Tcb));
+  }
+
+  //
+  // Build MSS option
+  //
+  Data = NetbufAllocSpace (Nbuf, TCP_OPTION_MSS_LEN, 1);
+  ASSERT (Data != NULL);
+
+  Len += TCP_OPTION_MSS_LEN;
+  TcpPutUint32 (Data, TCP_OPTION_MSS_FAST | Tcb->RcvMss);
+
+  return Len;
+}
+
+
+/**
+  Build the TCP option in synchronized states.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf    Pointer to the buffer to store the options.
+
+  @return         The total length of the TCP option field.
+
+**/
+UINT16
+TcpBuildOption (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  )
+{
+  UINT8   *Data;
+  UINT16  Len;
+
+  ASSERT ((Tcb != NULL) && (Nbuf != NULL) && (Nbuf->Tcp == NULL));
+  Len = 0;
+
+  //
+  // Build Timestamp option
+  //
+  if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_SND_TS) &&
+      !TCP_FLG_ON (TCPSEG_NETBUF (Nbuf)->Flag, TCP_FLG_RST)) {
+
+    Data = NetbufAllocSpace (
+            Nbuf,
+            TCP_OPTION_TS_ALIGNED_LEN,
+            NET_BUF_HEAD
+            );
+
+    ASSERT (Data != NULL);
+    Len += TCP_OPTION_TS_ALIGNED_LEN;
+
+    TcpPutUint32 (Data, TCP_OPTION_TS_FAST);
+    TcpPutUint32 (Data + 4, mTcpTick);
+    TcpPutUint32 (Data + 8, Tcb->TsRecent);
+  }
+
+  return Len;
+}
+
+
+/**
+  Parse the supported options.
+
+  @param  Tcp     Pointer to the TCP_CB of this TCP instance.
+  @param  Option  Pointer to the TCP_OPTION used to store the successfully pasrsed
+                  options.
+
+  @retval 0       The options are successfully pasrsed.
+  @retval -1      Ilegal option was found.
+
+**/
+INTN
+TcpParseOption (
+  IN     TCP_HEAD   *Tcp,
+  IN OUT TCP_OPTION *Option
+  )
+{
+  UINT8 *Head;
+  UINT8 TotalLen;
+  UINT8 Cur;
+  UINT8 Type;
+  UINT8 Len;
+
+  ASSERT ((Tcp != NULL) && (Option != NULL));
+
+  Option->Flag  = 0;
+
+  TotalLen      = (UINT8) ((Tcp->HeadLen << 2) - sizeof (TCP_HEAD));
+  if (TotalLen <= 0) {
+    return 0;
+  }
+
+  Head = (UINT8 *) (Tcp + 1);
+
+  //
+  // Fast process of timestamp option
+  //
+  if ((TotalLen == TCP_OPTION_TS_ALIGNED_LEN) &&
+      (TcpGetUint32 (Head) == TCP_OPTION_TS_FAST)) {
+
+    Option->TSVal = TcpGetUint32 (Head + 4);
+    Option->TSEcr = TcpGetUint32 (Head + 8);
+    Option->Flag  = TCP_OPTION_RCVD_TS;
+
+    return 0;
+  }
+
+  //
+  // Slow path to process the options.
+  //
+  Cur = 0;
+
+  while (Cur < TotalLen) {
+    Type = Head[Cur];
+
+    switch (Type) {
+    case TCP_OPTION_MSS:
+      Len = Head[Cur + 1];
+
+      if ((Len != TCP_OPTION_MSS_LEN) ||
+          (TotalLen - Cur < TCP_OPTION_MSS_LEN)) {
+
+        return -1;
+      }
+
+      Option->Mss = TcpGetUint16 (&Head[Cur + 2]);
+      TCP_SET_FLG (Option->Flag, TCP_OPTION_RCVD_MSS);
+
+      Cur += TCP_OPTION_MSS_LEN;
+      break;
+
+    case TCP_OPTION_WS:
+      Len = Head[Cur + 1];
+
+      if ((Len != TCP_OPTION_WS_LEN) ||
+          (TotalLen - Cur < TCP_OPTION_WS_LEN)) {
+
+        return -1;
+      }
+
+      Option->WndScale = (UINT8) MIN (14, Head[Cur + 2]);
+      TCP_SET_FLG (Option->Flag, TCP_OPTION_RCVD_WS);
+
+      Cur += TCP_OPTION_WS_LEN;
+      break;
+
+    case TCP_OPTION_TS:
+      Len = Head[Cur + 1];
+
+      if ((Len != TCP_OPTION_TS_LEN) ||
+          (TotalLen - Cur < TCP_OPTION_TS_LEN)) {
+
+        return -1;
+      }
+
+      Option->TSVal = TcpGetUint32 (&Head[Cur + 2]);
+      Option->TSEcr = TcpGetUint32 (&Head[Cur + 6]);
+      TCP_SET_FLG (Option->Flag, TCP_OPTION_RCVD_TS);
+
+      Cur += TCP_OPTION_TS_LEN;
+      break;
+
+    case TCP_OPTION_NOP:
+      Cur++;
+      break;
+
+    case TCP_OPTION_EOP:
+      Cur = TotalLen;
+      break;
+
+    default:
+      Len = Head[Cur + 1];
+
+      if ((TotalLen - Cur) < Len || Len < 2) {
+        return -1;
+      }
+
+      Cur = (UINT8) (Cur + Len);
+      break;
+    }
+
+  }
+
+  return 0;
+}
+
+
+/**
+  Check the segment against PAWS.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  TSVal   The timestamp value.
+
+  @retval 1       The segment passed the PAWS check.
+  @retval 0       The segment failed to pass the PAWS check.
+
+**/
+UINT32
+TcpPawsOK (
+  IN TCP_CB *Tcb,
+  IN UINT32 TSVal
+  )
+{
+  //
+  // PAWS as defined in RFC1323, buggy...
+  //
+  if (TCP_TIME_LT (TSVal, Tcb->TsRecent) &&
+      TCP_TIME_LT (Tcb->TsRecentAge + TCP_PAWS_24DAY, mTcpTick)) {
+
+    return 0;
+
+  }
+
+  return 1;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.h
new file mode 100644
index 0000000000..9676df015d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Option.h
@@ -0,0 +1,145 @@
+/** @file
+  Tcp option's routine header file.
+    
+Copyright (c) 2005 - 2009, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _TCP4_OPTION_H_
+#define _TCP4_OPTION_H_
+
+///
+/// The structure to store the parse option value.
+/// ParseOption only parse the options, don't process them.
+///
+typedef struct _TCP_OPTION {
+  UINT8   Flag;     ///< Flag such as TCP_OPTION_RCVD_MSS
+  UINT8   WndScale; ///< The WndScale received
+  UINT16  Mss;      ///< The Mss received
+  UINT32  TSVal;    ///< The TSVal field in a timestamp option
+  UINT32  TSEcr;    ///< The TSEcr field in a timestamp option
+} TCP_OPTION;
+
+//
+// supported TCP option type and their length
+//
+#define TCP_OPTION_EOP             0  ///< End Of oPtion
+#define TCP_OPTION_NOP             1  ///< No-Option.
+#define TCP_OPTION_MSS             2  ///< Maximum Segment Size
+#define TCP_OPTION_WS              3  ///< Window scale
+#define TCP_OPTION_TS              8  ///< Timestamp
+#define TCP_OPTION_MSS_LEN         4  ///< Length of MSS option
+#define TCP_OPTION_WS_LEN          3  ///< Length of window scale option
+#define TCP_OPTION_TS_LEN          10 ///< Length of timestamp option
+#define TCP_OPTION_WS_ALIGNED_LEN  4  ///< Length of window scale option, aligned
+#define TCP_OPTION_TS_ALIGNED_LEN  12 ///< Length of timestamp option, aligned
+
+//
+// recommend format of timestamp window scale
+// option for fast process.
+//
+#define TCP_OPTION_TS_FAST ((TCP_OPTION_NOP << 24) | \
+                            (TCP_OPTION_NOP << 16) | \
+                            (TCP_OPTION_TS << 8)   | \
+                            (TCP_OPTION_TS_LEN))
+
+#define TCP_OPTION_WS_FAST   ((TCP_OPTION_NOP << 24) | \
+                              (TCP_OPTION_WS << 16)  | \
+                              (TCP_OPTION_WS_LEN << 8))
+
+#define TCP_OPTION_MSS_FAST  ((TCP_OPTION_MSS << 24) | (TCP_OPTION_MSS_LEN << 16))
+
+//
+// Other misc definations
+//
+#define TCP_OPTION_RCVD_MSS        0x01
+#define TCP_OPTION_RCVD_WS         0x02
+#define TCP_OPTION_RCVD_TS         0x04
+#define TCP_OPTION_MAX_WS          14      ///< Maxium window scale value
+#define TCP_OPTION_MAX_WIN         0xffff  ///< Max window size in TCP header
+
+
+/**
+  Compute the window scale value according to the given buffer size.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return  The scale value.
+
+**/
+UINT8
+TcpComputeScale (
+  IN TCP_CB *Tcb
+  );
+
+/**
+  Build the TCP option in three-way handshake.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf    Pointer to the buffer to store the options.
+
+  @return  The total length of the TCP option field.
+
+**/
+UINT16
+TcpSynBuildOption (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  );
+
+/**
+  Build the TCP option in synchronized states.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf    Pointer to the buffer to store the options.
+
+  @return  The total length of the TCP option field.
+
+**/
+UINT16
+TcpBuildOption (
+  IN TCP_CB  *Tcb,
+  IN NET_BUF *Nbuf
+  );
+
+/**
+  Parse the supported options.
+
+  @param  Tcp     Pointer to the TCP_CB of this TCP instance.
+  @param  Option  Pointer to the TCP_OPTION used to store the successfully pasrsed
+                  options.
+
+  @retval 0       The options are successfully pasrsed.
+  @retval -1      Ilegal option was found.
+
+**/
+INTN
+TcpParseOption (
+  IN     TCP_HEAD   *Tcp,
+  IN OUT TCP_OPTION *Option
+  );
+
+/**
+  Check the segment against PAWS.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  TSVal   The timestamp value.
+
+  @retval 1       The segment passed the PAWS check.
+  @retval 0       The segment failed to pass the PAWS check.
+
+**/
+UINT32
+TcpPawsOK (
+  IN TCP_CB *Tcb,
+  IN UINT32 TSVal
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Output.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Output.c
new file mode 100644
index 0000000000..b44e851b26
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Output.c
@@ -0,0 +1,1212 @@
+/** @file
+  TCP output process routines.
+    
+Copyright (c) 2005 - 2010, 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<BR>
+
+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 "Tcp4Main.h"
+
+UINT8  mTcpOutFlag[] = {
+  0,                          // TCP_CLOSED
+  0,                          // TCP_LISTEN
+  TCP_FLG_SYN,                // TCP_SYN_SENT
+  TCP_FLG_SYN | TCP_FLG_ACK,  // TCP_SYN_RCVD
+  TCP_FLG_ACK,                // TCP_ESTABLISHED
+  TCP_FLG_FIN | TCP_FLG_ACK,  // TCP_FIN_WAIT_1
+  TCP_FLG_ACK,                // TCP_FIN_WAIT_2
+  TCP_FLG_ACK | TCP_FLG_FIN,  // TCP_CLOSING
+  TCP_FLG_ACK,                // TCP_TIME_WAIT
+  TCP_FLG_ACK,                // TCP_CLOSE_WAIT
+  TCP_FLG_FIN | TCP_FLG_ACK   // TCP_LAST_ACK
+};
+
+
+/**
+  Compute the sequence space left in the old receive window.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The sequence space left in the old receive window.
+
+**/
+UINT32
+TcpRcvWinOld (
+  IN TCP_CB *Tcb
+  )
+{
+  UINT32  OldWin;
+
+  OldWin = 0;
+
+  if (TCP_SEQ_GT (Tcb->RcvWl2 + Tcb->RcvWnd, Tcb->RcvNxt)) {
+
+    OldWin = TCP_SUB_SEQ (
+              Tcb->RcvWl2 + Tcb->RcvWnd,
+              Tcb->RcvNxt
+              );
+  }
+
+  return OldWin;
+}
+
+
+/**
+  Compute the current receive window.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The size of the current receive window, in bytes.
+
+**/
+UINT32
+TcpRcvWinNow (
+  IN TCP_CB *Tcb
+  )
+{
+  SOCKET  *Sk;
+  UINT32  Win;
+  UINT32  Increase;
+  UINT32  OldWin;
+
+  Sk = Tcb->Sk;
+  ASSERT (Sk != NULL);
+
+  OldWin    = TcpRcvWinOld (Tcb);
+
+  Win       = SockGetFreeSpace (Sk, SOCK_RCV_BUF);
+
+  Increase  = 0;
+  if (Win > OldWin) {
+    Increase = Win - OldWin;
+  }
+
+  //
+  // Receiver's SWS: don't advertise a bigger window
+  // unless it can be increased by at least one Mss or
+  // half of the receive buffer.
+  //
+  if ((Increase > Tcb->SndMss) ||
+      (2 * Increase >= GET_RCV_BUFFSIZE (Sk))) {
+
+    return Win;
+  }
+
+  return OldWin;
+}
+
+
+/**
+  Compute the value to fill in the window size field of the outgoing segment.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Syn     The flag to indicate whether the outgoing segment is a SYN
+                  segment.
+
+  @return The value of the local receive window size used to fill the outing segment.
+
+**/
+UINT16
+TcpComputeWnd (
+  IN OUT TCP_CB  *Tcb,
+  IN     BOOLEAN Syn
+  )
+{
+  UINT32  Wnd;
+
+  //
+  // RFC requires that initial window not be scaled
+  //
+  if (Syn) {
+
+    Wnd = GET_RCV_BUFFSIZE (Tcb->Sk);
+  } else {
+
+    Wnd         = TcpRcvWinNow (Tcb);
+
+    Tcb->RcvWnd = Wnd;
+  }
+
+  Wnd = MIN (Wnd >> Tcb->RcvWndScale, 0xffff);
+  return NTOHS ((UINT16) Wnd);
+}
+
+
+/**
+  Get the maximum SndNxt.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @return The sequence number of the maximum SndNxt.
+
+**/
+TCP_SEQNO
+TcpGetMaxSndNxt (
+  IN TCP_CB *Tcb
+  )
+{
+  LIST_ENTRY      *Entry;
+  NET_BUF         *Nbuf;
+
+  if (IsListEmpty (&Tcb->SndQue)) {
+    return Tcb->SndNxt;
+  }
+
+  Entry = Tcb->SndQue.BackLink;
+  Nbuf  = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+  ASSERT (TCP_SEQ_GEQ (TCPSEG_NETBUF (Nbuf)->End, Tcb->SndNxt));
+  return TCPSEG_NETBUF (Nbuf)->End;
+}
+
+
+/**
+  Compute how much data to send.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Force   Whether to ignore the sender's SWS avoidance algorithm and send
+                  out data by force.
+
+  @return The length of the data can be sent, if 0, no data can be sent.
+
+**/
+UINT32
+TcpDataToSend (
+  IN TCP_CB *Tcb,
+  IN INTN   Force
+  )
+{
+  SOCKET  *Sk;
+  UINT32  Win;
+  UINT32  Len;
+  UINT32  Left;
+  UINT32  Limit;
+
+  Sk = Tcb->Sk;
+  ASSERT (Sk != NULL);
+
+  //
+  // TCP should NOT send data beyond the send window
+  // and congestion window. The right edge of send
+  // window is defined as SND.WL2 + SND.WND. The right
+  // edge of congestion window is defined as SND.UNA +
+  // CWND.
+  //
+  Win   = 0;
+  Limit = Tcb->SndWl2 + Tcb->SndWnd;
+
+  if (TCP_SEQ_GT (Limit, Tcb->SndUna + Tcb->CWnd)) {
+
+    Limit = Tcb->SndUna + Tcb->CWnd;
+  }
+
+  if (TCP_SEQ_GT (Limit, Tcb->SndNxt)) {
+    Win = TCP_SUB_SEQ (Limit, Tcb->SndNxt);
+  }
+
+  //
+  // The data to send contains two parts: the data on the
+  // socket send queue, and the data on the TCB's send
+  // buffer. The later can be non-zero if the peer shrinks
+  // its advertised window.
+  //
+  Left  = GET_SND_DATASIZE (Sk) +
+          TCP_SUB_SEQ (TcpGetMaxSndNxt (Tcb), Tcb->SndNxt);
+
+  Len   = MIN (Win, Left);
+
+  if (Len > Tcb->SndMss) {
+    Len = Tcb->SndMss;
+  }
+
+  if ((Force != 0)|| (Len == 0 && Left == 0)) {
+    return Len;
+  }
+
+  if (Len == 0 && Left != 0) {
+    goto SetPersistTimer;
+  }
+
+  //
+  // Sender's SWS avoidance: Don't send a small segment unless
+  // a)A full-sized segment can be sent,
+  // b)at least one-half of the maximum sized windows that
+  // the other end has ever advertised.
+  // c)It can send everything it has and either it isn't
+  // expecting an ACK or the Nagle algorithm is disabled.
+  //
+  if ((Len == Tcb->SndMss) || (2 * Len >= Tcb->SndWndMax)) {
+
+    return Len;
+  }
+
+  if ((Len == Left) &&
+      ((Tcb->SndNxt == Tcb->SndUna) ||
+      TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_NAGLE))) {
+
+    return Len;
+  }
+
+  //
+  // RFC1122 suggests to set a timer when SWSA forbids TCP
+  // sending more data, and combine it with probe timer.
+  //
+SetPersistTimer:
+  if (!TCP_TIMER_ON (Tcb->EnabledTimer, TCP_TIMER_REXMIT)) {
+
+    DEBUG (
+      (EFI_D_WARN,
+      "TcpDataToSend: enter persistent state for TCB %p\n",
+      Tcb)
+      );
+
+    if (!Tcb->ProbeTimerOn) {
+      TcpSetProbeTimer (Tcb);
+    }
+  }
+
+  return 0;
+}
+
+
+/**
+  Build the TCP header of the TCP segment and transmit the segment by IP.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Nbuf    Pointer to the buffer containing the segment to be sent out.
+
+  @retval 0       The segment is sent out successfully.
+  @retval other   Error condition occurred.
+
+**/
+INTN
+TcpTransmitSegment (
+  IN OUT TCP_CB  *Tcb,
+  IN     NET_BUF *Nbuf
+  )
+{
+  UINT16    Len;
+  TCP_HEAD  *Head;
+  TCP_SEG   *Seg;
+  BOOLEAN   Syn;
+  UINT32    DataLen;
+
+  ASSERT ((Nbuf != NULL) && (Nbuf->Tcp == NULL) && (TcpVerifySegment (Nbuf) != 0));
+
+  DataLen = Nbuf->TotalSize;
+
+  Seg     = TCPSEG_NETBUF (Nbuf);
+  Syn     = TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN);
+
+  if (Syn) {
+
+    Len = TcpSynBuildOption (Tcb, Nbuf);
+  } else {
+
+    Len = TcpBuildOption (Tcb, Nbuf);
+  }
+
+  ASSERT ((Len % 4 == 0) && (Len <= 40));
+
+  Len += sizeof (TCP_HEAD);
+
+  Head = (TCP_HEAD *) NetbufAllocSpace (
+                        Nbuf,
+                        sizeof (TCP_HEAD),
+                        NET_BUF_HEAD
+                        );
+
+  ASSERT (Head != NULL);
+
+  Nbuf->Tcp       = Head;
+
+  Head->SrcPort   = Tcb->LocalEnd.Port;
+  Head->DstPort   = Tcb->RemoteEnd.Port;
+  Head->Seq       = NTOHL (Seg->Seq);
+  Head->Ack       = NTOHL (Tcb->RcvNxt);
+  Head->HeadLen   = (UINT8) (Len >> 2);
+  Head->Res       = 0;
+  Head->Wnd       = TcpComputeWnd (Tcb, Syn);
+  Head->Checksum  = 0;
+
+  //
+  // Check whether to set the PSH flag.
+  //
+  TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_PSH);
+
+  if (DataLen != 0) {
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_SND_PSH) &&
+        TCP_SEQ_BETWEEN (Seg->Seq, Tcb->SndPsh, Seg->End)) {
+
+      TCP_SET_FLG (Seg->Flag, TCP_FLG_PSH);
+      TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_SND_PSH);
+
+    } else if ((Seg->End == Tcb->SndNxt) &&
+               (GET_SND_DATASIZE (Tcb->Sk) == 0)) {
+
+      TCP_SET_FLG (Seg->Flag, TCP_FLG_PSH);
+    }
+  }
+
+  //
+  // Check whether to set the URG flag and the urgent pointer.
+  //
+  TCP_CLEAR_FLG (Seg->Flag, TCP_FLG_URG);
+
+  if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_SND_URG) &&
+      TCP_SEQ_LEQ (Seg->Seq, Tcb->SndUp)) {
+
+    TCP_SET_FLG (Seg->Flag, TCP_FLG_URG);
+
+    if (TCP_SEQ_LT (Tcb->SndUp, Seg->End)) {
+
+      Seg->Urg = (UINT16) TCP_SUB_SEQ (Tcb->SndUp, Seg->Seq);
+    } else {
+
+      Seg->Urg = (UINT16) MIN (
+                            TCP_SUB_SEQ (Tcb->SndUp,
+                            Seg->Seq),
+                            0xffff
+                            );
+    }
+  }
+
+  Head->Flag      = Seg->Flag;
+  Head->Urg       = NTOHS (Seg->Urg);
+  Head->Checksum  = TcpChecksum (Nbuf, Tcb->HeadSum);
+
+  //
+  // update the TCP session's control information
+  //
+  Tcb->RcvWl2 = Tcb->RcvNxt;
+  if (Syn) {
+    Tcb->RcvWnd = NTOHS (Head->Wnd);
+  }
+
+  //
+  // clear delayedack flag
+  //
+  Tcb->DelayedAck = 0;
+
+  return TcpSendIpPacket (Tcb, Nbuf, Tcb->LocalEnd.Ip, Tcb->RemoteEnd.Ip);
+}
+
+
+/**
+  Get a segment from the Tcb's SndQue.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegmentSndQue (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  )
+{
+  LIST_ENTRY      *Head;
+  LIST_ENTRY      *Cur;
+  NET_BUF         *Node;
+  TCP_SEG         *Seg;
+  NET_BUF         *Nbuf;
+  TCP_SEQNO       End;
+  UINT8           *Data;
+  UINT8           Flag;
+  INT32           Offset;
+  INT32           CopyLen;
+
+  ASSERT ((Tcb != NULL) && TCP_SEQ_LEQ (Seq, Tcb->SndNxt) && (Len > 0));
+
+  //
+  // Find the segment that contains the Seq.
+  //
+  Head  = &Tcb->SndQue;
+
+  Node  = NULL;
+  Seg   = NULL;
+
+  NET_LIST_FOR_EACH (Cur, Head) {
+    Node  = NET_LIST_USER_STRUCT (Cur, NET_BUF, List);
+    Seg   = TCPSEG_NETBUF (Node);
+
+    if (TCP_SEQ_LT (Seq, Seg->End) && TCP_SEQ_LEQ (Seg->Seq, Seq)) {
+
+      break;
+    }
+  }
+
+  ASSERT (Cur  != Head);
+  ASSERT (Node != NULL);
+  ASSERT (Seg  != NULL);
+
+  //
+  // Return the buffer if it can be returned without
+  // adjustment:
+  //
+  if ((Seg->Seq == Seq) &&
+      TCP_SEQ_LEQ (Seg->End, Seg->Seq + Len) &&
+      !NET_BUF_SHARED (Node)) {
+
+    NET_GET_REF (Node);
+    return Node;
+  }
+
+  //
+  // Create a new buffer and copy data there.
+  //
+  Nbuf = NetbufAlloc (Len + TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    return NULL;
+  }
+
+  NetbufReserve (Nbuf, TCP_MAX_HEAD);
+
+  Flag  = Seg->Flag;
+  End   = Seg->End;
+
+  if (TCP_SEQ_LT (Seq + Len, Seg->End)) {
+    End = Seq + Len;
+  }
+
+  CopyLen = TCP_SUB_SEQ (End, Seq);
+  Offset  = TCP_SUB_SEQ (Seq, Seg->Seq);
+
+  //
+  // If SYN is set and out of the range, clear the flag.
+  // Becuase the sequence of the first byte is SEG.SEQ+1,
+  // adjust Offset by -1. If SYN is in the range, copy
+  // one byte less.
+  //
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+
+    if (TCP_SEQ_LT (Seg->Seq, Seq)) {
+
+      TCP_CLEAR_FLG (Flag, TCP_FLG_SYN);
+      Offset--;
+    } else {
+
+      CopyLen--;
+    }
+  }
+
+  //
+  // If FIN is set and in the range, copy one byte less,
+  // and if it is out of the range, clear the flag.
+  //
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_FIN)) {
+
+    if (Seg->End == End) {
+
+      CopyLen--;
+    } else {
+
+      TCP_CLEAR_FLG (Flag, TCP_FLG_FIN);
+    }
+  }
+
+  ASSERT (CopyLen >= 0);
+
+  //
+  // copy data to the segment
+  //
+  if (CopyLen != 0) {
+    Data = NetbufAllocSpace (Nbuf, CopyLen, NET_BUF_TAIL);
+    ASSERT (Data != NULL);
+
+    if ((INT32) NetbufCopy (Node, Offset, CopyLen, Data) != CopyLen) {
+      goto OnError;
+    }
+  }
+
+  CopyMem (TCPSEG_NETBUF (Nbuf), Seg, sizeof (TCP_SEG));
+
+  TCPSEG_NETBUF (Nbuf)->Seq   = Seq;
+  TCPSEG_NETBUF (Nbuf)->End   = End;
+  TCPSEG_NETBUF (Nbuf)->Flag  = Flag;
+
+  return Nbuf;
+
+OnError:
+  NetbufFree (Nbuf);
+  return NULL;
+}
+
+
+/**
+  Get a segment from the Tcb's socket buffer.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegmentSock (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  )
+{
+  NET_BUF *Nbuf;
+  UINT8   *Data;
+  UINT32  DataGet;
+
+  ASSERT ((Tcb != NULL) && (Tcb->Sk != NULL));
+
+  Nbuf = NetbufAlloc (Len + TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    DEBUG ((EFI_D_ERROR, "TcpGetSegmentSock: failed to allocate "
+      "a netbuf for TCB %p\n",Tcb));
+
+    return NULL;
+  }
+
+  NetbufReserve (Nbuf, TCP_MAX_HEAD);
+
+  DataGet = 0;
+
+  if (Len != 0) {
+    //
+    // copy data to the segment.
+    //
+    Data = NetbufAllocSpace (Nbuf, Len, NET_BUF_TAIL);
+    ASSERT (Data != NULL);
+
+    DataGet = SockGetDataToSend (Tcb->Sk, 0, Len, Data);
+  }
+
+  NET_GET_REF (Nbuf);
+
+  TCPSEG_NETBUF (Nbuf)->Seq = Seq;
+  TCPSEG_NETBUF (Nbuf)->End = Seq + Len;
+
+  InsertTailList (&(Tcb->SndQue), &(Nbuf->List));
+
+  if (DataGet != 0) {
+
+    SockDataSent (Tcb->Sk, DataGet);
+  }
+
+  return Nbuf;
+}
+
+
+/**
+  Get a segment starting from sequence Seq of a maximum
+  length of Len.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment.
+  @param  Len     The maximum length of the segment.
+
+  @return Pointer to the segment, if NULL some error occurred.
+
+**/
+NET_BUF *
+TcpGetSegment (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq,
+  IN UINT32    Len
+  )
+{
+  NET_BUF *Nbuf;
+
+  ASSERT (Tcb != NULL);
+
+  //
+  // Compare the SndNxt with the max sequence number sent.
+  //
+  if ((Len != 0) && TCP_SEQ_LT (Seq, TcpGetMaxSndNxt (Tcb))) {
+
+    Nbuf = TcpGetSegmentSndQue (Tcb, Seq, Len);
+  } else {
+
+    Nbuf = TcpGetSegmentSock (Tcb, Seq, Len);
+  }
+
+  ASSERT (TcpVerifySegment (Nbuf) != 0);
+  return Nbuf;
+}
+
+
+/**
+  Retransmit the segment from sequence Seq.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Seq     The sequence number of the segment to be retransmitted.
+
+  @retval 0       Retransmission succeeded.
+  @retval -1      Error condition occurred.
+
+**/
+INTN
+TcpRetransmit (
+  IN TCP_CB    *Tcb,
+  IN TCP_SEQNO Seq
+  )
+{
+  NET_BUF *Nbuf;
+  UINT32  Len;
+
+  //
+  // Compute the maxium length of retransmission. It is
+  // limited by three factors:
+  // 1. Less than SndMss
+  // 2. must in the current send window
+  // 3. will not change the boundaries of queued segments.
+  //
+  if (TCP_SEQ_LT (Tcb->SndWl2 + Tcb->SndWnd, Seq)) {
+    DEBUG ((EFI_D_WARN, "TcpRetransmit: retransmission cancelled "
+      "because send window too small for TCB %p\n", Tcb));
+
+    return 0;
+  }
+
+  Len   = TCP_SUB_SEQ (Tcb->SndWl2 + Tcb->SndWnd, Seq);
+  Len   = MIN (Len, Tcb->SndMss);
+
+  Nbuf  = TcpGetSegmentSndQue (Tcb, Seq, Len);
+  if (Nbuf == NULL) {
+    return -1;
+  }
+
+  ASSERT (TcpVerifySegment (Nbuf) != 0);
+
+  if (TcpTransmitSegment (Tcb, Nbuf) != 0) {
+    goto OnError;
+  }
+
+  //
+  // The retransmitted buffer may be on the SndQue,
+  // trim TCP head because all the buffer on SndQue
+  // are headless.
+  //
+  ASSERT (Nbuf->Tcp != NULL);
+  NetbufTrim (Nbuf, (Nbuf->Tcp->HeadLen << 2), NET_BUF_HEAD);
+  Nbuf->Tcp = NULL;
+
+  NetbufFree (Nbuf);
+  return 0;
+
+OnError:
+  if (Nbuf != NULL) {
+    NetbufFree (Nbuf);
+  }
+
+  return -1;
+}
+
+
+/**
+  Check whether to send data/SYN/FIN and piggy back an ACK.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+  @param  Force   Whether to ignore the sender's SWS avoidance algorithm and send
+                  out data by force.
+
+  @return The number of bytes sent.
+
+**/
+INTN
+TcpToSendData (
+  IN OUT TCP_CB *Tcb,
+  IN     INTN Force
+  )
+{
+  UINT32    Len;
+  INTN      Sent;
+  UINT8     Flag;
+  NET_BUF   *Nbuf;
+  TCP_SEG   *Seg;
+  TCP_SEQNO Seq;
+  TCP_SEQNO End;
+
+  ASSERT ((Tcb != NULL) && (Tcb->Sk != NULL) && (Tcb->State != TCP_LISTEN));
+
+  Sent = 0;
+
+  if ((Tcb->State == TCP_CLOSED) ||
+      TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_FIN_SENT)) {
+
+    return 0;
+  }
+
+SEND_AGAIN:
+  //
+  // compute how much data can be sent
+  //
+  Len   = TcpDataToSend (Tcb, Force);
+  Seq   = Tcb->SndNxt;
+
+  ASSERT ((Tcb->State) < (sizeof (mTcpOutFlag) / sizeof (mTcpOutFlag[0])));
+  Flag  = mTcpOutFlag[Tcb->State];
+
+  if ((Flag & TCP_FLG_SYN) != 0) {
+
+    Seq = Tcb->Iss;
+    Len = 0;
+  }
+
+  //
+  // only send a segment without data if SYN or
+  // FIN is set.
+  //
+  if ((Len == 0) && 
+      ((Flag & (TCP_FLG_SYN | TCP_FLG_FIN)) == 0)) {
+    return Sent;
+  }
+
+  Nbuf = TcpGetSegment (Tcb, Seq, Len);
+
+  if (Nbuf == NULL) {
+    DEBUG (
+      (EFI_D_ERROR,
+      "TcpToSendData: failed to get a segment for TCB %p\n",
+      Tcb)
+      );
+
+    goto OnError;
+  }
+
+  Seg = TCPSEG_NETBUF (Nbuf);
+
+  //
+  // Set the TcpSeg in Nbuf.
+  //
+  Len = Nbuf->TotalSize;
+  End = Seq + Len;
+  if (TCP_FLG_ON (Flag, TCP_FLG_SYN)) {
+    End++;
+  }
+
+  if ((Flag & TCP_FLG_FIN) != 0) {
+    //
+    // Send FIN if all data is sent, and FIN is
+    // in the window
+    //
+    if ((TcpGetMaxSndNxt (Tcb) == Tcb->SndNxt) &&
+        (GET_SND_DATASIZE (Tcb->Sk) == 0) &&
+        TCP_SEQ_LT (End + 1, Tcb->SndWnd + Tcb->SndWl2)) {
+
+      DEBUG (
+	  	(EFI_D_INFO, 
+	  	"TcpToSendData: send FIN "
+        "to peer for TCB %p in state %s\n", 
+        Tcb, 
+        mTcpStateName[Tcb->State])
+      );
+
+      End++;
+    } else {
+      TCP_CLEAR_FLG (Flag, TCP_FLG_FIN);
+    }
+  }
+
+  Seg->Seq  = Seq;
+  Seg->End  = End;
+  Seg->Flag = Flag;
+
+  ASSERT (TcpVerifySegment (Nbuf) != 0);
+  ASSERT (TcpCheckSndQue (&Tcb->SndQue) != 0);
+
+  //
+  // don't send an empty segment here.
+  //
+  if (Seg->End == Seg->Seq) {
+    DEBUG ((EFI_D_WARN, "TcpToSendData: created a empty"
+      " segment for TCB %p, free it now\n", Tcb));
+
+    NetbufFree (Nbuf);
+    return Sent;
+  }
+
+  if (TcpTransmitSegment (Tcb, Nbuf) != 0) {
+    NetbufTrim (Nbuf, (Nbuf->Tcp->HeadLen << 2), NET_BUF_HEAD);
+    Nbuf->Tcp = NULL;
+
+    if ((Flag & TCP_FLG_FIN) != 0)  {
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_FIN_SENT);
+    }
+
+    goto OnError;
+  }
+
+  Sent += TCP_SUB_SEQ (End, Seq);
+
+  //
+  // All the buffer in the SndQue is headless
+  //
+  ASSERT (Nbuf->Tcp != NULL);
+
+  NetbufTrim (Nbuf, (Nbuf->Tcp->HeadLen << 2), NET_BUF_HEAD);
+  Nbuf->Tcp = NULL;
+
+  NetbufFree (Nbuf);
+
+  //
+  // update status in TCB
+  //
+  Tcb->DelayedAck = 0;
+
+  if ((Flag & TCP_FLG_FIN) != 0) {
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_FIN_SENT);
+  }
+
+  if (TCP_SEQ_GT (End, Tcb->SndNxt)) {
+    Tcb->SndNxt = End;
+  }
+
+  if (!TCP_TIMER_ON (Tcb->EnabledTimer, TCP_TIMER_REXMIT)) {
+    TcpSetTimer (Tcb, TCP_TIMER_REXMIT, Tcb->Rto);
+  }
+
+  //
+  // Enable RTT measurement only if not in retransmit.
+  // Karn's algorithm reqires not to update RTT when in loss.
+  //
+  if ((Tcb->CongestState == TCP_CONGEST_OPEN) &&
+      !TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RTT_ON)) {
+
+    DEBUG ((EFI_D_INFO, "TcpToSendData: set RTT measure "
+      "sequence %d for TCB %p\n", Seq, Tcb));
+
+    TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_RTT_ON);
+    Tcb->RttSeq     = Seq;
+    Tcb->RttMeasure = 0;
+  }
+
+  if (Len == Tcb->SndMss) {
+    goto SEND_AGAIN;
+  }
+
+  return Sent;
+
+OnError:
+  if (Nbuf != NULL) {
+    NetbufFree (Nbuf);
+  }
+
+  return Sent;
+}
+
+
+/**
+  Send an ACK immediately.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSendAck (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  NET_BUF *Nbuf;
+  TCP_SEG *Seg;
+
+  Nbuf = NetbufAlloc (TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    return;
+  }
+
+  NetbufReserve (Nbuf, TCP_MAX_HEAD);
+
+  Seg       = TCPSEG_NETBUF (Nbuf);
+  Seg->Seq  = Tcb->SndNxt;
+  Seg->End  = Tcb->SndNxt;
+  Seg->Flag = TCP_FLG_ACK;
+
+  if (TcpTransmitSegment (Tcb, Nbuf) == 0) {
+    TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW);
+    Tcb->DelayedAck = 0;
+  }
+
+  NetbufFree (Nbuf);
+}
+
+
+/**
+  Send a zero probe segment. It can be used by keepalive and zero window probe.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+  @retval 0       The zero probe segment was sent out successfully.
+  @retval other   Error condition occurred.
+
+**/
+INTN
+TcpSendZeroProbe (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  NET_BUF *Nbuf;
+  TCP_SEG *Seg;
+  INTN     Result;
+
+  Nbuf = NetbufAlloc (TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    return -1;
+  }
+
+  NetbufReserve (Nbuf, TCP_MAX_HEAD);
+
+  //
+  // SndNxt-1 is out of window. The peer should respond
+  // with an ACK.
+  //
+  Seg       = TCPSEG_NETBUF (Nbuf);
+  Seg->Seq  = Tcb->SndNxt - 1;
+  Seg->End  = Tcb->SndNxt - 1;
+  Seg->Flag = TCP_FLG_ACK;
+
+  Result    = TcpTransmitSegment (Tcb, Nbuf);
+  NetbufFree (Nbuf);
+
+  return Result;
+}
+
+
+/**
+  Check whether to send an ACK or delayed ACK.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpToSendAck (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  UINT32 TcpNow;
+
+  TcpNow = TcpRcvWinNow (Tcb);
+  //
+  // Generally, TCP should send a delayed ACK unless:
+  //   1. ACK at least every other FULL sized segment received,
+  //   2. Packets received out of order
+  //   3. Receiving window is open
+  //
+  if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_ACK_NOW) ||
+      (Tcb->DelayedAck >= 1) ||
+      (TcpNow > TcpRcvWinOld (Tcb))) {
+    TcpSendAck (Tcb);
+    return;
+  }
+
+  DEBUG ((EFI_D_INFO, "TcpToSendAck: scheduled a delayed"
+    " ACK for TCB %p\n", Tcb));
+
+  //
+  // schedule a delayed ACK
+  //
+  Tcb->DelayedAck++;
+}
+
+
+/**
+  Send a RESET segment in response to the segment received.
+
+  @param  Tcb     Pointer to the TCP_CB of this TCP instance, may be NULL.
+  @param  Head    TCP header of the segment that triggers the reset.
+  @param  Len     Length of the segment that triggers the reset.
+  @param  Local   Local IP address.
+  @param  Remote  Remote peer's IP address.
+
+  @retval 0       A reset is sent or no need to send it.
+  @retval -1      No reset is sent.
+
+**/
+INTN
+TcpSendReset (
+  IN TCP_CB    *Tcb,
+  IN TCP_HEAD  *Head,
+  IN INT32     Len,
+  IN UINT32    Local,
+  IN UINT32    Remote
+  )
+{
+  NET_BUF   *Nbuf;
+  TCP_HEAD  *Nhead;
+  UINT16    HeadSum;
+
+  //
+  // Don't respond to a Reset with reset
+  //
+  if ((Head->Flag & TCP_FLG_RST) != 0) {
+    return 0;
+  }
+
+  Nbuf = NetbufAlloc (TCP_MAX_HEAD);
+
+  if (Nbuf == NULL) {
+    return -1;
+  }
+
+  Nhead = (TCP_HEAD *) NetbufAllocSpace (
+                        Nbuf,
+                        sizeof (TCP_HEAD),
+                        NET_BUF_TAIL
+                        );
+
+  ASSERT (Nhead != NULL);
+
+  Nbuf->Tcp   = Nhead;
+  Nhead->Flag = TCP_FLG_RST;
+
+  //
+  // Derive Seq/ACK from the segment if no TCB
+  // associated with it, otherwise from the Tcb
+  //
+  if (Tcb == NULL) {
+
+    if (TCP_FLG_ON (Head->Flag, TCP_FLG_ACK)) {
+      Nhead->Seq  = Head->Ack;
+      Nhead->Ack  = 0;
+    } else {
+      Nhead->Seq = 0;
+      TCP_SET_FLG (Nhead->Flag, TCP_FLG_ACK);
+      Nhead->Ack = HTONL (NTOHL (Head->Seq) + Len);
+    }
+  } else {
+
+    Nhead->Seq  = HTONL (Tcb->SndNxt);
+    Nhead->Ack  = HTONL (Tcb->RcvNxt);
+    TCP_SET_FLG (Nhead->Flag, TCP_FLG_ACK);
+  }
+
+  Nhead->SrcPort  = Head->DstPort;
+  Nhead->DstPort  = Head->SrcPort;
+  Nhead->HeadLen  = (UINT8) (sizeof (TCP_HEAD) >> 2);
+  Nhead->Res      = 0;
+  Nhead->Wnd      = HTONS (0xFFFF);
+  Nhead->Checksum = 0;
+  Nhead->Urg      = 0;
+
+  HeadSum         = NetPseudoHeadChecksum (Local, Remote, 6, 0);
+  Nhead->Checksum = TcpChecksum (Nbuf, HeadSum);
+
+  TcpSendIpPacket (Tcb, Nbuf, Local, Remote);
+
+  NetbufFree (Nbuf);
+  return 0;
+}
+
+
+/**
+  Verify that the segment is in good shape.
+
+  @param  Nbuf    Buffer that contains the segment to be checked.
+
+  @retval 0       The segment is broken.
+  @retval 1       The segment is in good shape.
+
+**/
+INTN
+TcpVerifySegment (
+  IN NET_BUF *Nbuf
+  )
+{
+  TCP_HEAD  *Head;
+  TCP_SEG   *Seg;
+  UINT32    Len;
+
+  if (Nbuf == NULL) {
+    return 1;
+  }
+
+  NET_CHECK_SIGNATURE (Nbuf, NET_BUF_SIGNATURE);
+
+  Seg   = TCPSEG_NETBUF (Nbuf);
+  Len   = Nbuf->TotalSize;
+  Head  = Nbuf->Tcp;
+
+  if (Head != NULL) {
+    if (Head->Flag != Seg->Flag) {
+      return 0;
+    }
+
+    Len -= (Head->HeadLen << 2);
+  }
+
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_SYN)) {
+    Len++;
+  }
+
+  if (TCP_FLG_ON (Seg->Flag, TCP_FLG_FIN)) {
+    Len++;
+  }
+
+  if (Seg->Seq + Len != Seg->End) {
+    return 0;
+  }
+
+  return 1;
+}
+
+
+/**
+  Verify that all the segments in SndQue are in good shape.
+
+  @param  Head    Pointer to the head node of the SndQue.
+
+  @retval 0       At least one segment is broken.
+  @retval 1       All segments in the specific queue are in good shape.
+
+**/
+INTN
+TcpCheckSndQue (
+  IN LIST_ENTRY     *Head
+  )
+{
+  LIST_ENTRY      *Entry;
+  NET_BUF         *Nbuf;
+  TCP_SEQNO       Seq;
+
+  if (IsListEmpty (Head)) {
+    return 1;
+  }
+  //
+  // Initialize the Seq
+  //
+  Entry = Head->ForwardLink;
+  Nbuf  = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+  Seq   = TCPSEG_NETBUF (Nbuf)->Seq;
+
+  NET_LIST_FOR_EACH (Entry, Head) {
+    Nbuf = NET_LIST_USER_STRUCT (Entry, NET_BUF, List);
+
+    if (TcpVerifySegment (Nbuf) == 0) {
+      return 0;
+    }
+
+    //
+    // All the node in the SndQue should has:
+    // SEG.SEQ = LAST_SEG.END
+    //
+    if (Seq != TCPSEG_NETBUF (Nbuf)->Seq) {
+      return 0;
+    }
+
+    Seq = TCPSEG_NETBUF (Nbuf)->End;
+  }
+
+  return 1;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Proto.h b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Proto.h
new file mode 100644
index 0000000000..01d6034b13
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Proto.h
@@ -0,0 +1,351 @@
+/** @file
+  Tcp Protocol header file.
+
+Copyright (c) 2005 - 2010, 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<BR>
+
+THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _TCP4_PROTO_H_
+#define _TCP4_PROTO_H_
+
+typedef struct _TCP_CB  TCP_CB;
+
+#include "Tcp4Driver.h"
+#include "Socket.h"
+#include "Tcp4Option.h"
+
+
+
+///
+/// Tcp states, Don't change their order, it is used as
+/// index to mTcpOutFlag and other macros
+///
+#define TCP_CLOSED       0
+#define TCP_LISTEN       1
+#define TCP_SYN_SENT     2
+#define TCP_SYN_RCVD     3
+#define TCP_ESTABLISHED  4
+#define TCP_FIN_WAIT_1   5
+#define TCP_FIN_WAIT_2   6
+#define TCP_CLOSING      7
+#define TCP_TIME_WAIT    8
+#define TCP_CLOSE_WAIT   9
+#define TCP_LAST_ACK     10
+
+
+///
+/// Flags in the TCP header
+///
+#define TCP_FLG_FIN      0x01
+#define TCP_FLG_SYN      0x02
+#define TCP_FLG_RST      0x04
+#define TCP_FLG_PSH      0x08
+#define TCP_FLG_ACK      0x10
+#define TCP_FLG_URG      0x20
+ 
+ //
+ // mask for all the flags
+ //
+#define TCP_FLG_FLAG     0x3F  
+
+
+#define TCP_CONNECT_REFUSED      (-1) ///< TCP error status
+#define TCP_CONNECT_RESET        (-2) ///< TCP error status
+#define TCP_CONNECT_CLOSED       (-3) ///< TCP error status
+
+//
+// Current congestion status as suggested by RFC3782.
+//
+#define TCP_CONGEST_RECOVER      1  ///< During the NewReno fast recovery
+#define TCP_CONGEST_LOSS         2  ///< Retxmit because of retxmit time out
+#define TCP_CONGEST_OPEN         3  ///< TCP is opening its congestion window
+
+//
+// TCP control flags
+//
+#define TCP_CTRL_NO_NAGLE        0x0001 ///< Disable Nagle algorithm
+#define TCP_CTRL_NO_KEEPALIVE    0x0002 ///< Disable keepalive timer
+#define TCP_CTRL_NO_WS           0x0004 ///< Disable window scale option
+#define TCP_CTRL_RCVD_WS         0x0008 ///< Received a wnd scale option in syn
+#define TCP_CTRL_NO_TS           0x0010 ///< Disable Timestamp option
+#define TCP_CTRL_RCVD_TS         0x0020 ///< Received a Timestamp option in syn
+#define TCP_CTRL_SND_TS          0x0040 ///< Send Timestamp option to remote
+#define TCP_CTRL_SND_URG         0x0080 ///< In urgent send mode
+#define TCP_CTRL_RCVD_URG        0x0100 ///< In urgent receive mode
+#define TCP_CTRL_SND_PSH         0x0200 ///< In PUSH send mode
+#define TCP_CTRL_FIN_SENT        0x0400 ///< FIN is sent
+#define TCP_CTRL_FIN_ACKED       0x0800 ///< FIN is ACKed.
+#define TCP_CTRL_TIMER_ON        0x1000 ///< At least one of the timer is on
+#define TCP_CTRL_RTT_ON          0x2000 ///< The RTT measurement is on
+#define TCP_CTRL_ACK_NOW         0x4000 ///< Send the ACK now, don't delay
+
+//
+// Timer related values
+//
+#define TCP_TIMER_CONNECT        0                  ///< Connection establishment timer
+#define TCP_TIMER_REXMIT         1                  ///< Retransmit timer
+#define TCP_TIMER_PROBE          2                  ///< Window probe timer
+#define TCP_TIMER_KEEPALIVE      3                  ///< Keepalive timer
+#define TCP_TIMER_FINWAIT2       4                  ///< FIN_WAIT_2 timer
+#define TCP_TIMER_2MSL           5                  ///< TIME_WAIT tiemr
+#define TCP_TIMER_NUMBER         6                  ///< The total number of TCP timer.
+#define TCP_TICK                 200                ///< Every TCP tick is 200ms
+#define TCP_TICK_HZ              5                  ///< The frequence of TCP tick
+#define TCP_RTT_SHIFT            3                  ///< SRTT & RTTVAR scaled by 8
+#define TCP_RTO_MIN              TCP_TICK_HZ        ///< The minium value of RTO
+#define TCP_RTO_MAX              (TCP_TICK_HZ * 60) ///< The maxium value of RTO
+#define TCP_FOLD_RTT             4                  ///< Timeout threshod to fold RTT
+
+//
+// Default values for some timers
+//
+#define TCP_MAX_LOSS             12                          ///< Default max times to retxmit
+#define TCP_KEEPALIVE_IDLE_MIN   (TCP_TICK_HZ * 60 * 60 * 2) ///< First keep alive
+#define TCP_KEEPALIVE_PERIOD     (TCP_TICK_HZ * 60)
+#define TCP_MAX_KEEPALIVE        8
+#define TCP_FIN_WAIT2_TIME       (2 * TCP_TICK_HZ)
+#define TCP_TIME_WAIT_TIME       (2 * TCP_TICK_HZ)
+#define TCP_PAWS_24DAY           (24 * 24 * 60 * 60 * TCP_TICK_HZ)
+#define TCP_CONNECT_TIME         (75 * TCP_TICK_HZ)
+
+//
+// The header space to be reserved before TCP data to accomodate :
+// 60byte IP head + 60byte TCP head + link layer head
+//
+#define TCP_MAX_HEAD             192
+
+//
+// Value ranges for some control option
+//
+#define TCP_RCV_BUF_SIZE         (2 * 1024 * 1024)
+#define TCP_RCV_BUF_SIZE_MIN     (8 * 1024)
+#define TCP_SND_BUF_SIZE         (2 * 1024 * 1024)
+#define TCP_SND_BUF_SIZE_MIN     (8 * 1024)
+#define TCP_BACKLOG              10
+#define TCP_BACKLOG_MIN          5
+#define TCP_MAX_LOSS_MIN         6
+#define TCP_CONNECT_TIME_MIN     (60 * TCP_TICK_HZ)
+#define TCP_MAX_KEEPALIVE_MIN    4
+#define TCP_KEEPALIVE_IDLE_MAX   (TCP_TICK_HZ * 60 * 60 * 4)
+#define TCP_KEEPALIVE_PERIOD_MIN (TCP_TICK_HZ * 30)
+#define TCP_FIN_WAIT2_TIME_MAX   (4 * TCP_TICK_HZ)
+#define TCP_TIME_WAIT_TIME_MAX   (60 * TCP_TICK_HZ)
+
+///
+/// TCP segmentation data
+///
+typedef struct _TCP_SEG {
+  TCP_SEQNO Seq;  ///< Starting sequence number
+  TCP_SEQNO End;  ///< The sequence of the last byte + 1, include SYN/FIN. End-Seq = SEG.LEN
+  TCP_SEQNO Ack;  ///< ACK field in the segment
+  UINT8     Flag; ///< TCP header flags
+  UINT16    Urg;  ///< Valid if URG flag is set.
+  UINT32    Wnd;  ///< TCP window size field
+} TCP_SEG;
+
+///
+/// Network endpoint, IP+Port structure
+///
+typedef struct _TCP_PEER {
+  UINT32      Ip;   ///< IP address, network byte order
+  TCP_PORTNO  Port; ///< Port number, network byte order
+} TCP_PEER;
+
+///
+/// TCP control block, it includes various states
+///
+struct _TCP_CB {
+  LIST_ENTRY        List;     ///< Back and forward link entry
+  TCP_CB            *Parent;  ///< The parent TCP_CB structure
+
+  SOCKET            *Sk;      ///< The socket it controled.
+  TCP_PEER          LocalEnd; ///< Local endpoint
+  TCP_PEER          RemoteEnd;///< Remote endpoint
+
+  LIST_ENTRY        SndQue;   ///< Retxmission queue
+  LIST_ENTRY        RcvQue;   ///< Reassemble queue
+  UINT32            CtrlFlag; ///< Control flags, such as NO_NAGLE
+  INT32             Error;    ///< Soft error status, such as TCP_CONNECT_RESET
+
+  //
+  // RFC793 and RFC1122 defined variables
+  //
+  UINT8             State;      ///< TCP state, such as SYN_SENT, LISTEN
+  UINT8             DelayedAck; ///< Number of delayed ACKs
+  UINT16            HeadSum;    ///< Checksum of the fixed parts of pesudo
+                                ///< header: Src IP, Dst IP, 0, Protocol,
+                                ///< not include the TCP length.
+
+  TCP_SEQNO         Iss;        ///< Initial Sending Sequence
+  TCP_SEQNO         SndUna;     ///< First unacknowledged data
+  TCP_SEQNO         SndNxt;     ///< Next data sequence to send.
+  TCP_SEQNO         SndPsh;     ///< Send PUSH point
+  TCP_SEQNO         SndUp;      ///< Send urgent point
+  UINT32            SndWnd;     ///< Window advertised by the remote peer
+  UINT32            SndWndMax;  ///< Max send window advertised by the peer
+  TCP_SEQNO         SndWl1;     ///< Seq number used for last window update
+  TCP_SEQNO         SndWl2;     ///< Ack no of last window update
+  UINT16            SndMss;     ///< Max send segment size
+  TCP_SEQNO         RcvNxt;     ///< Next sequence no to receive
+  UINT32            RcvWnd;     ///< Window advertised by the local peer
+  TCP_SEQNO         RcvWl2;     ///< The RcvNxt (or ACK) of last window update.
+                                ///< It is necessary because of delayed ACK
+
+  TCP_SEQNO         RcvUp;                   ///< Urgent point;
+  TCP_SEQNO         Irs;                     ///< Initial Receiving Sequence
+  UINT16            RcvMss;                  ///< Max receive segment size
+  UINT16            EnabledTimer;            ///< Which timer is currently enabled
+  UINT32            Timer[TCP_TIMER_NUMBER]; ///< When the timer will expire
+  INT32             NextExpire;  ///< Count down offset for the nearest timer
+  UINT32            Idle;        ///< How long the connection is in idle
+  UINT32            ProbeTime;   ///< The time out value for current window prober
+  BOOLEAN           ProbeTimerOn;///< If TRUE, the probe time is on.
+
+  //
+  // RFC1323 defined variables, about window scale,
+  // timestamp and PAWS
+  //
+  UINT8             SndWndScale;  ///< Wndscale received from the peer
+  UINT8             RcvWndScale;  ///< Wndscale used to scale local buffer
+  UINT32            TsRecent;     ///< TsRecent to echo to the remote peer
+  UINT32            TsRecentAge;  ///< When this TsRecent is updated
+
+  //
+  // RFC2988 defined variables. about RTT measurement
+  //
+  TCP_SEQNO         RttSeq;     ///< The seq of measured segment now
+  UINT32            RttMeasure; ///< Currently measured RTT in heart beats
+  UINT32            SRtt;       ///< Smoothed RTT, scaled by 8
+  UINT32            RttVar;     ///< RTT variance, scaled by 8
+  UINT32            Rto;        ///< Current RTO, not scaled
+
+  //
+  // RFC2581, and 3782 variables.
+  // Congestion control + NewReno fast recovery.
+  //
+  UINT32            CWnd;         ///< Sender's congestion window
+  UINT32            Ssthresh;     ///< Slow start threshold.
+  TCP_SEQNO         Recover;      ///< Recover point for NewReno
+  UINT16            DupAck;       ///< Number of duplicate ACKs
+  UINT8             CongestState; ///< The current congestion state(RFC3782)
+  UINT8             LossTimes;    ///< Number of retxmit timeouts in a row
+  TCP_SEQNO         LossRecover;  ///< Recover point for retxmit
+
+  //
+  // configuration parameters, for EFI_TCP4_PROTOCOL specification
+  //
+  UINT32            KeepAliveIdle;   ///< Idle time before sending first probe
+  UINT32            KeepAlivePeriod; ///< Interval for subsequent keep alive probe
+  UINT8             MaxKeepAlive;    ///< Maxium keep alive probe times.
+  UINT8             KeepAliveProbes; ///< The number of keep alive probe.
+  UINT16            MaxRexmit;       ///< The maxium number of retxmit before abort
+  UINT32            FinWait2Timeout; ///< The FIN_WAIT_2 time out
+  UINT32            TimeWaitTimeout; ///< The TIME_WAIT time out
+  UINT32            ConnectTimeout;  ///< The connect establishment time out
+
+  //
+  // configuration for tcp provided by user
+  //
+  BOOLEAN           UseDefaultAddr;
+  UINT8             Tos;
+  UINT8             Ttl;
+  EFI_IPv4_ADDRESS  SubnetMask;
+
+  IP_IO_IP_INFO     *IpInfo;        ///<pointer reference to Ip used to send pkt
+};
+
+extern LIST_ENTRY     mTcpRunQue;
+extern LIST_ENTRY     mTcpListenQue;
+extern TCP_SEQNO      mTcpGlobalIss;
+extern UINT32         mTcpTick;
+
+///
+/// TCP_CONNECTED: both ends have synchronized their ISN.
+///
+#define TCP_CONNECTED(state)  ((state) > TCP_SYN_RCVD)
+
+#define TCP_FIN_RCVD(State) \
+    (((State) == TCP_CLOSE_WAIT) || \
+    ((State) == TCP_LAST_ACK) || \
+    ((State) == TCP_CLOSING) || \
+    ((State) == TCP_TIME_WAIT))
+
+#define TCP_LOCAL_CLOSED(State) \
+      (((State) == TCP_FIN_WAIT_1) || \
+      ((State) == TCP_FIN_WAIT_2) || \
+      ((State) == TCP_CLOSING) || \
+      ((State) == TCP_TIME_WAIT) || \
+      ((State) == TCP_LAST_ACK))
+
+//
+// Get the TCP_SEG point from a net buffer's ProtoData
+//
+#define TCPSEG_NETBUF(NBuf) ((TCP_SEG *) ((NBuf)->ProtoData))
+
+//
+// macros to compare sequence no
+//
+#define TCP_SEQ_LT(SeqA, SeqB)  ((INT32) ((SeqA) - (SeqB)) < 0)
+#define TCP_SEQ_LEQ(SeqA, SeqB) ((INT32) ((SeqA) - (SeqB)) <= 0)
+#define TCP_SEQ_GT(SeqA, SeqB)  ((INT32) ((SeqB) - (SeqA)) < 0)
+#define TCP_SEQ_GEQ(SeqA, SeqB) ((INT32) ((SeqB) - (SeqA)) <= 0)
+
+//
+// TCP_SEQ_BETWEEN return whether b <= m <= e
+//
+#define TCP_SEQ_BETWEEN(b, m, e)  ((e) - (b) >= (m) - (b))
+
+//
+// TCP_SUB_SEQ returns Seq1 - Seq2. Make sure Seq1 >= Seq2
+//
+#define TCP_SUB_SEQ(Seq1, Seq2)     ((UINT32) ((Seq1) - (Seq2)))
+
+//
+// Check whether Flag is on
+//
+#define TCP_FLG_ON(Value, Flag)     ((BOOLEAN) (((Value) & (Flag)) != 0))
+
+//
+// Set and Clear operation on a Flag
+//
+#define TCP_SET_FLG(Value, Flag)    ((Value) |= (Flag))
+#define TCP_CLEAR_FLG(Value, Flag)  ((Value) &= ~(Flag))
+
+//
+// Test whether two peers are equal
+//
+#define TCP_PEER_EQUAL(Pa, Pb) \
+  (((Pa)->Ip == (Pb)->Ip) && ((Pa)->Port == (Pb)->Port))
+
+//
+// Test whether Pa matches Pb, or Pa is more specific
+// than pb. Zero means wildcard.
+//
+#define TCP_PEER_MATCH(Pa, Pb) \
+    ((((Pb)->Ip == 0) || ((Pb)->Ip == (Pa)->Ip)) && \
+    (((Pb)->Port == 0) || ((Pb)->Port == (Pa)->Port)))
+
+#define TCP_TIMER_ON(Flag, Timer)     ((Flag) & (1 << (Timer)))
+#define TCP_SET_TIMER(Flag, Timer)    ((Flag) = (UINT16) ((Flag) | (1 << (Timer))))
+#define TCP_CLEAR_TIMER(Flag, Timer)  ((Flag) = (UINT16) ((Flag) & (~(1 << (Timer)))))
+
+#define TCP_TIME_LT(Ta, Tb)           ((INT32) ((Ta) - (Tb)) < 0)
+#define TCP_TIME_LEQ(Ta, Tb)          ((INT32) ((Ta) - (Tb)) <= 0)
+#define TCP_SUB_TIME(Ta, Tb)          ((UINT32) ((Ta) - (Tb)))
+
+#define TCP_MAX_WIN                   0xFFFFU
+
+typedef
+VOID
+(*TCP_TIMER_HANDLER) (
+  IN OUT TCP_CB *Tcb
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Timer.c b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Timer.c
new file mode 100644
index 0000000000..a8e4a933cf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Tcp4Dxe/Tcp4Timer.c
@@ -0,0 +1,584 @@
+/** @file
+  TCP timer related functions.
+    
+Copyright (c) 2005 - 2010, 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<BR>
+
+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 "Tcp4Main.h"
+
+UINT32    mTcpTick = 1000;
+
+/**
+  Connect timeout handler.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpConnectTimeout (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Timeout handler for TCP retransmission timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpRexmitTimeout (
+  IN OUT TCP_CB *Tcb
+  );
+  
+/**
+  Timeout handler for window probe timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpProbeTimeout (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Timeout handler for keepalive timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpKeepaliveTimeout (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Timeout handler for FIN_WAIT_2 timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpFinwait2Timeout (
+  IN OUT TCP_CB *Tcb
+  );
+
+/**
+  Timeout handler for 2MSL timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+Tcp2MSLTimeout (
+  IN OUT TCP_CB *Tcb
+  );
+
+TCP_TIMER_HANDLER mTcpTimerHandler[TCP_TIMER_NUMBER] = {
+  TcpConnectTimeout,
+  TcpRexmitTimeout,
+  TcpProbeTimeout,
+  TcpKeepaliveTimeout,
+  TcpFinwait2Timeout,
+  Tcp2MSLTimeout,
+};
+
+/**
+  Close the TCP connection.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpClose (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  NetbufFreeList (&Tcb->SndQue);
+  NetbufFreeList (&Tcb->RcvQue);
+
+  TcpSetState (Tcb, TCP_CLOSED);
+}
+
+
+/**
+  Connect timeout handler.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpConnectTimeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  if (!TCP_CONNECTED (Tcb->State)) {
+    DEBUG ((EFI_D_ERROR, "TcpConnectTimeout: connection closed "
+      "because conenction timer timeout for TCB %p\n", Tcb));
+
+    if (EFI_ABORTED == Tcb->Sk->SockError) {
+      SOCK_ERROR (Tcb->Sk, EFI_TIMEOUT);
+    }
+
+    if (TCP_SYN_RCVD == Tcb->State) {
+      DEBUG ((EFI_D_WARN, "TcpConnectTimeout: send reset because "
+        "connection timer timeout for TCB %p\n", Tcb));
+
+      TcpResetConnection (Tcb);
+
+    }
+
+    TcpClose (Tcb);
+  }
+}
+
+
+/**
+  Timeout handler for TCP retransmission timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpRexmitTimeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  UINT32  FlightSize;
+
+  DEBUG ((EFI_D_WARN, "TcpRexmitTimeout: transmission "
+    "timeout for TCB %p\n", Tcb));
+
+  //
+  // Set the congestion window. FlightSize is the
+  // amount of data that has been sent but not
+  // yet ACKed.
+  //
+  FlightSize        = TCP_SUB_SEQ (Tcb->SndNxt, Tcb->SndUna);
+  Tcb->Ssthresh     = MAX ((UINT32) (2 * Tcb->SndMss), FlightSize / 2);
+
+  Tcb->CWnd         = Tcb->SndMss;
+  Tcb->LossRecover  = Tcb->SndNxt;
+
+  Tcb->LossTimes++;
+  if ((Tcb->LossTimes > Tcb->MaxRexmit) &&
+      !TCP_TIMER_ON (Tcb->EnabledTimer, TCP_TIMER_CONNECT)) {
+
+    DEBUG ((EFI_D_ERROR, "TcpRexmitTimeout: connection closed "
+      "because too many timeouts for TCB %p\n", Tcb));
+
+    if (EFI_ABORTED == Tcb->Sk->SockError) {
+      SOCK_ERROR (Tcb->Sk, EFI_TIMEOUT);
+    }
+
+    TcpClose (Tcb);
+    return ;
+  }
+
+  TcpBackoffRto (Tcb);
+  TcpRetransmit (Tcb, Tcb->SndUna);
+  TcpSetTimer (Tcb, TCP_TIMER_REXMIT, Tcb->Rto);
+
+  Tcb->CongestState = TCP_CONGEST_LOSS;
+  TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_RTT_ON);
+}
+
+
+/**
+  Timeout handler for window probe timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpProbeTimeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  //
+  // This is the timer for sender's SWSA. RFC1122 requires
+  // a timer set for sender's SWSA, and suggest combine it
+  // with window probe timer. If data is sent, don't set
+  // the probe timer, since retransmit timer is on.
+  //
+  if ((TcpDataToSend (Tcb, 1) != 0) && (TcpToSendData (Tcb, 1) > 0)) {
+
+    ASSERT (TCP_TIMER_ON (Tcb->EnabledTimer, TCP_TIMER_REXMIT) != 0);
+    Tcb->ProbeTimerOn = FALSE;
+    return ;
+  }
+
+  TcpSendZeroProbe (Tcb);
+  TcpSetProbeTimer (Tcb);
+}
+
+
+/**
+  Timeout handler for keepalive timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpKeepaliveTimeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  Tcb->KeepAliveProbes++;
+
+  //
+  // Too many Keep-alive probes, drop the connection
+  //
+  if (Tcb->KeepAliveProbes > Tcb->MaxKeepAlive) {
+
+    if (EFI_ABORTED == Tcb->Sk->SockError) {
+      SOCK_ERROR (Tcb->Sk, EFI_TIMEOUT);
+    }
+
+    TcpClose (Tcb);
+    return ;
+  }
+
+  TcpSendZeroProbe (Tcb);
+  TcpSetKeepaliveTimer (Tcb);
+}
+
+
+/**
+  Timeout handler for FIN_WAIT_2 timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpFinwait2Timeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  DEBUG ((EFI_D_WARN, "TcpFinwait2Timeout: connection closed "
+    "because FIN_WAIT2 timer timeouts for TCB %p\n", Tcb));
+
+  TcpClose (Tcb);
+}
+
+
+/**
+  Timeout handler for 2MSL timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+Tcp2MSLTimeout (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  DEBUG ((EFI_D_WARN, "Tcp2MSLTimeout: connection closed "
+    "because TIME_WAIT timer timeouts for TCB %p\n", Tcb));
+
+  TcpClose (Tcb);
+}
+
+
+/**
+  Update the timer status and the next expire time according to the timers 
+  to expire in a specific future time slot.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpUpdateTimer (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  UINT16  Index;
+
+  //
+  // Don't use a too large value to init NextExpire
+  // since mTcpTick wraps around as sequence no does.
+  //
+  Tcb->NextExpire = 65535;
+  TCP_CLEAR_FLG (Tcb->CtrlFlag, TCP_CTRL_TIMER_ON);
+
+  for (Index = 0; Index < TCP_TIMER_NUMBER; Index++) {
+
+    if (TCP_TIMER_ON (Tcb->EnabledTimer, Index) &&
+        TCP_TIME_LT (Tcb->Timer[Index], mTcpTick + Tcb->NextExpire)) {
+
+      Tcb->NextExpire = TCP_SUB_TIME (Tcb->Timer[Index], mTcpTick);
+      TCP_SET_FLG (Tcb->CtrlFlag, TCP_CTRL_TIMER_ON);
+    }
+  }
+}
+
+
+/**
+  Enable a TCP timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Timer    The index of the timer to be enabled.
+  @param  TimeOut  The timeout value of this timer.
+
+**/
+VOID
+TcpSetTimer (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT16 Timer,
+  IN     UINT32 TimeOut
+  )
+{
+  TCP_SET_TIMER (Tcb->EnabledTimer, Timer);
+  Tcb->Timer[Timer] = mTcpTick + TimeOut;
+
+  TcpUpdateTimer (Tcb);
+}
+
+
+/**
+  Clear one TCP timer.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+  @param  Timer    The index of the timer to be cleared.
+
+**/
+VOID
+TcpClearTimer (
+  IN OUT TCP_CB *Tcb,
+  IN     UINT16 Timer
+  )
+{
+  TCP_CLEAR_TIMER (Tcb->EnabledTimer, Timer);
+  TcpUpdateTimer (Tcb);
+}
+
+
+/**
+  Clear all TCP timers.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpClearAllTimer (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  Tcb->EnabledTimer = 0;
+  TcpUpdateTimer (Tcb);
+}
+
+
+/**
+  Enable the window prober timer and set the timeout value.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSetProbeTimer (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  if (!Tcb->ProbeTimerOn) {
+    Tcb->ProbeTime    = Tcb->Rto;
+    Tcb->ProbeTimerOn = TRUE;
+
+  } else {
+    Tcb->ProbeTime <<= 1;
+  }
+
+  if (Tcb->ProbeTime < TCP_RTO_MIN) {
+
+    Tcb->ProbeTime = TCP_RTO_MIN;
+  } else if (Tcb->ProbeTime > TCP_RTO_MAX) {
+
+    Tcb->ProbeTime = TCP_RTO_MAX;
+  }
+
+  TcpSetTimer (Tcb, TCP_TIMER_PROBE, Tcb->ProbeTime);
+}
+
+
+/**
+  Enable the keepalive timer and set the timeout value.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpSetKeepaliveTimer (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_NO_KEEPALIVE)) {
+    return ;
+
+  }
+
+  //
+  // Set the timer to KeepAliveIdle if either
+  // 1. the keepalive timer is off
+  // 2. The keepalive timer is on, but the idle
+  // is less than KeepAliveIdle, that means the
+  // connection is alive since our last probe.
+  //
+  if (!TCP_TIMER_ON (Tcb->EnabledTimer, TCP_TIMER_KEEPALIVE) ||
+      (Tcb->Idle < Tcb->KeepAliveIdle)) {
+
+    TcpSetTimer (Tcb, TCP_TIMER_KEEPALIVE, Tcb->KeepAliveIdle);
+    Tcb->KeepAliveProbes = 0;
+
+  } else {
+
+    TcpSetTimer (Tcb, TCP_TIMER_KEEPALIVE, Tcb->KeepAlivePeriod);
+  }
+}
+
+
+/**
+  Backoff the RTO.
+
+  @param  Tcb      Pointer to the TCP_CB of this TCP instance.
+
+**/
+VOID
+TcpBackoffRto (
+  IN OUT TCP_CB *Tcb
+  )
+{
+  //
+  // Fold the RTT estimate if too many times, the estimate
+  // may be wrong, fold it. So the next time a valid
+  // measurement is sampled, we can start fresh.
+  //
+  if ((Tcb->LossTimes >= TCP_FOLD_RTT) && (Tcb->SRtt != 0)) {
+    Tcb->RttVar += Tcb->SRtt >> 2;
+    Tcb->SRtt = 0;
+  }
+
+  Tcb->Rto <<= 1;
+
+  if (Tcb->Rto < TCP_RTO_MIN) {
+
+    Tcb->Rto = TCP_RTO_MIN;
+  } else if (Tcb->Rto > TCP_RTO_MAX) {
+
+    Tcb->Rto = TCP_RTO_MAX;
+  }
+}
+
+
+/**
+  Heart beat timer handler.
+
+  @param  Context        Context of the timer event, ignored.
+
+**/
+VOID
+EFIAPI
+TcpTickingDpc (
+  IN VOID      *Context
+  )
+{
+  LIST_ENTRY      *Entry;
+  LIST_ENTRY      *Next;
+  TCP_CB          *Tcb;
+  INT16           Index;
+
+  mTcpTick++;
+  mTcpGlobalIss += 100;
+
+  //
+  // Don't use LIST_FOR_EACH, which isn't delete safe.
+  //
+  for (Entry = mTcpRunQue.ForwardLink; Entry != &mTcpRunQue; Entry = Next) {
+
+    Next  = Entry->ForwardLink;
+
+    Tcb   = NET_LIST_USER_STRUCT (Entry, TCP_CB, List);
+
+    if (Tcb->State == TCP_CLOSED) {
+      continue;
+    }
+    //
+    // The connection is doing RTT measurement.
+    //
+    if (TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_RTT_ON)) {
+      Tcb->RttMeasure++;
+    }
+
+    Tcb->Idle++;
+
+    if (Tcb->DelayedAck != 0) {
+      TcpSendAck (Tcb);
+    }
+
+    //
+    // No timer is active or no timer expired
+    //
+    if (!TCP_FLG_ON (Tcb->CtrlFlag, TCP_CTRL_TIMER_ON) ||
+        ((--Tcb->NextExpire) > 0)) {
+
+      continue;
+    }
+
+    //
+    // Call the timeout handler for each expired timer.
+    //
+    for (Index = 0; Index < TCP_TIMER_NUMBER; Index++) {
+
+      if (TCP_TIMER_ON (Tcb->EnabledTimer, Index) &&
+          TCP_TIME_LEQ (Tcb->Timer[Index], mTcpTick)) {
+        //
+        // disable the timer before calling the handler
+        // in case the handler enables it again.
+        //
+        TCP_CLEAR_TIMER (Tcb->EnabledTimer, Index);
+        mTcpTimerHandler[Index](Tcb);
+
+        //
+        // The Tcb may have been deleted by the timer, or
+        // no other timer is set.
+        //
+        if ((Next->BackLink != Entry) ||
+            (Tcb->EnabledTimer == 0)) {
+          break;
+        }
+      }
+    }
+    
+    //
+    // If the Tcb still exist or some timer is set, update the timer
+    //
+    if (Index == TCP_TIMER_NUMBER) {
+      TcpUpdateTimer (Tcb);
+    }
+  }
+}
+
+/**
+  Heart beat timer handler, queues the DPC at TPL_CALLBACK.
+
+  @param  Event    Timer event signaled, ignored.
+  @param  Context  Context of the timer event, ignored.
+
+**/
+VOID
+EFIAPI
+TcpTicking (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  QueueDpc (TPL_CALLBACK, TcpTickingDpc, Context);
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/ComponentName.c
new file mode 100644
index 0000000000..cf311055e2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/ComponentName.c
@@ -0,0 +1,435 @@
+/** @file
+
+Copyright (c) 2006 - 2012, 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 "Udp4Impl.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  );
+
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL  gUdp4ComponentName = {
+  UdpComponentNameGetDriverName,
+  UdpComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL gUdp4ComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) UdpComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) UdpComponentNameGetControllerName,
+  "en"
+};
+
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mUdpDriverNameTable[] = {
+  {
+    "eng;en",
+    L"UDP Network Service Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE *gUdpControllerNameTable = NULL;
+  
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  *This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mUdpDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gUdp4ComponentName)
+           );
+}
+
+/**
+  Update the component name for the Udp4 child handle.
+
+  @param  Udp4[in]                   A pointer to the EFI_UDP4_PROTOCOL.
+
+  
+  @retval EFI_SUCCESS                Update the ControllerNameTable of this instance successfully.
+  @retval EFI_INVALID_PARAMETER      The input parameter is invalid.
+  
+**/
+EFI_STATUS
+UpdateName (
+  EFI_UDP4_PROTOCOL             *Udp4
+  )
+{
+  EFI_STATUS                       Status;
+  CHAR16                           HandleName[64];
+  EFI_UDP4_CONFIG_DATA             Udp4ConfigData;
+
+  if (Udp4 == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Format the child name into the string buffer as:
+  // UDPv4 (SrcPort=59, DestPort=60)
+  //
+  Status = Udp4->GetModeData (Udp4, &Udp4ConfigData, NULL, NULL, NULL);
+  if (!EFI_ERROR (Status)) {
+    UnicodeSPrint (HandleName, sizeof (HandleName),
+      L"UDPv4 (SrcPort=%d, DestPort=%d)",
+      Udp4ConfigData.StationPort,
+      Udp4ConfigData.RemotePort
+      );
+  } else if (Status == EFI_NOT_STARTED) {
+    UnicodeSPrint (
+      HandleName,
+      sizeof (HandleName),
+      L"UDPv4 (Not started)"
+      );
+  } else {
+    return Status;
+  }
+
+  if (gUdpControllerNameTable != NULL) {
+    FreeUnicodeStringTable (gUdpControllerNameTable);
+    gUdpControllerNameTable = NULL;
+  }
+  
+  Status = AddUnicodeString2 (
+             "eng",
+             gUdp4ComponentName.SupportedLanguages,
+             &gUdpControllerNameTable,
+             HandleName,
+             TRUE
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  return AddUnicodeString2 (
+           "en",
+           gUdp4ComponentName2.SupportedLanguages,
+           &gUdpControllerNameTable,
+           HandleName,
+           FALSE
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+UdpComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL                     *This,
+  IN  EFI_HANDLE                                      ControllerHandle,
+  IN  EFI_HANDLE                                      ChildHandle        OPTIONAL,
+  IN  CHAR8                                           *Language,
+  OUT CHAR16                                          **ControllerName
+  )
+{
+  EFI_STATUS                    Status;
+  EFI_UDP4_PROTOCOL             *Udp4;
+
+  //
+  // Only provide names for child handles.
+  //
+  if (ChildHandle == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // Make sure this driver produced ChildHandle
+  //
+  Status = EfiTestChildHandle (
+             ControllerHandle,
+             ChildHandle,
+             &gEfiIp4ProtocolGuid
+             );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Retrieve an instance of a produced protocol from ChildHandle
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **)&Udp4,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Update the component name for this child handle.
+  //
+  Status = UpdateName (Udp4);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           gUdpControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gUdp4ComponentName)
+           );
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.c b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.c
new file mode 100644
index 0000000000..faf9ca5e28
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.c
@@ -0,0 +1,590 @@
+/** @file
+
+Copyright (c) 2006 - 2014, 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 "Udp4Impl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gUdp4DriverBinding = {
+  Udp4DriverBindingSupported,
+  Udp4DriverBindingStart,
+  Udp4DriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+EFI_SERVICE_BINDING_PROTOCOL mUdp4ServiceBinding = {
+  Udp4ServiceBindingCreateChild,
+  Udp4ServiceBindingDestroyChild
+};
+
+/**
+  Callback function which provided by user to remove one node in NetDestroyLinkList process.
+  
+  @param[in]    Entry           The entry to be removed.
+  @param[in]    Context         Pointer to the callback context corresponds to the Context in NetDestroyLinkList.
+
+  @retval EFI_SUCCESS           The entry has been removed successfully.
+  @retval Others                Fail to remove the entry.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DestroyChildEntryInHandleBuffer (
+  IN LIST_ENTRY         *Entry,
+  IN VOID               *Context
+  )
+{
+  UDP4_INSTANCE_DATA            *Instance;
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+
+  if (Entry == NULL || Context == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = NET_LIST_USER_STRUCT_S (Entry, UDP4_INSTANCE_DATA, Link, UDP4_INSTANCE_DATA_SIGNATURE);
+  ServiceBinding    = ((UDP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ServiceBinding;
+  NumberOfChildren  = ((UDP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->NumberOfChildren;
+  ChildHandleBuffer = ((UDP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT *) Context)->ChildHandleBuffer;
+
+  if (!NetIsInHandleBuffer (Instance->ChildHandle, NumberOfChildren, ChildHandleBuffer)) {
+    return EFI_SUCCESS;
+  }
+
+  return ServiceBinding->DestroyChild (ServiceBinding, Instance->ChildHandle);
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Test for the Udp4ServiceBinding Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiUdp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Test for the Ip4 Protocol
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiIp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  )
+{
+  EFI_STATUS         Status;
+  UDP4_SERVICE_DATA  *Udp4Service;
+
+  //
+  // Allocate Private Context Data Structure.
+  //
+  Udp4Service = AllocatePool (sizeof (UDP4_SERVICE_DATA));
+  if (Udp4Service == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = Udp4CreateService (Udp4Service, This->DriverBindingHandle, ControllerHandle);
+  if (EFI_ERROR (Status)) {
+    FreePool (Udp4Service);
+    return Status;
+  }
+
+  //
+  // Install the Udp4ServiceBindingProtocol on the ControllerHandle.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiUdp4ServiceBindingProtocolGuid,
+                  &Udp4Service->ServiceBinding,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    Udp4CleanService (Udp4Service);
+    FreePool (Udp4Service);
+  }
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+  
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                                children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                                Status;
+  EFI_HANDLE                                NicHandle;
+  EFI_SERVICE_BINDING_PROTOCOL              *ServiceBinding;
+  UDP4_SERVICE_DATA                         *Udp4Service;
+  UDP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT  Context;
+  LIST_ENTRY                                *List;
+
+  //
+  // Find the NicHandle where UDP4 ServiceBinding Protocol is installed.
+  //
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiIp4ProtocolGuid);
+  if (NicHandle == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Retrieve the UDP4 ServiceBinding Protocol.
+  //
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiUdp4ServiceBindingProtocolGuid,
+                  (VOID **) &ServiceBinding,
+                  This->DriverBindingHandle,
+                  NicHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  Udp4Service = UDP4_SERVICE_DATA_FROM_THIS (ServiceBinding);
+  if (NumberOfChildren != 0) {
+    //
+    // NumberOfChildren is not zero, destroy the children instances in ChildHandleBuffer.
+    //
+    List = &Udp4Service->ChildrenList;
+    Context.ServiceBinding    = ServiceBinding;
+    Context.NumberOfChildren  = NumberOfChildren;
+    Context.ChildHandleBuffer = ChildHandleBuffer;
+    Status = NetDestroyLinkList (
+               List,
+               Udp4DestroyChildEntryInHandleBuffer,
+               &Context,
+               NULL
+               );
+  } else {
+    gBS->UninstallMultipleProtocolInterfaces (
+           NicHandle,
+           &gEfiUdp4ServiceBindingProtocolGuid,
+           &Udp4Service->ServiceBinding,
+           NULL
+           );
+ 
+    Udp4CleanService (Udp4Service);
+
+    if (gUdpControllerNameTable != NULL) {
+      FreeUnicodeStringTable (gUdpControllerNameTable);
+      gUdpControllerNameTable = NULL;
+    }
+    FreePool (Udp4Service);
+  }
+
+  return Status;
+}
+
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param[in] This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param[in] ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                         then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                         then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  )
+{
+  EFI_STATUS          Status;
+  UDP4_SERVICE_DATA   *Udp4Service;
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_TPL             OldTpl;
+  VOID                *Ip4;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Udp4Service = UDP4_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Allocate the instance private data structure.
+  //
+  Instance = AllocateZeroPool (sizeof (UDP4_INSTANCE_DATA));
+  if (Instance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Udp4InitInstance (Udp4Service, Instance);
+
+  //
+  // Add an IpInfo for this instance.
+  //
+  Instance->IpInfo = IpIoAddIp (Udp4Service->IpIo);
+  if (Instance->IpInfo == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_ERROR;
+  }
+
+  //
+  // Install the Udp4Protocol for this instance.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  &Instance->Udp4Proto,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Instance->ChildHandle = *ChildHandle;
+
+  //
+  // Open the default Ip4 protocol in the IP_IO BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  Udp4Service->IpIo->ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **) &Ip4,
+                  gUdp4DriverBinding.DriverBindingHandle,
+                  Instance->ChildHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Open this instance's Ip4 protocol in the IpInfo BY_CHILD.
+  //
+  Status = gBS->OpenProtocol (
+                  Instance->IpInfo->ChildHandle,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **) &Ip4,
+                  gUdp4DriverBinding.DriverBindingHandle,
+                  Instance->ChildHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Link this instance into the service context data and increase the ChildrenNumber.
+  //
+  InsertTailList (&Udp4Service->ChildrenList, &Instance->Link);
+  Udp4Service->ChildrenNumber++;
+
+  gBS->RestoreTPL (OldTpl);
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  if (Instance->ChildHandle != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           Instance->ChildHandle,
+           &gEfiUdp4ProtocolGuid,
+           &Instance->Udp4Proto,
+           NULL
+           );
+  }
+
+  if (Instance->IpInfo != NULL) {
+    IpIoRemoveIp (Udp4Service->IpIo, Instance->IpInfo);
+  }
+
+  Udp4CleanInstance (Instance);
+
+  FreePool (Instance);
+
+  return Status;
+}
+
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param[in] This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param[in] ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  )
+{
+  EFI_STATUS          Status;
+  UDP4_SERVICE_DATA   *Udp4Service;
+  EFI_UDP4_PROTOCOL   *Udp4Proto;
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_TPL             OldTpl;
+
+  if ((This == NULL) || (ChildHandle == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Udp4Service = UDP4_SERVICE_DATA_FROM_THIS (This);
+
+  //
+  // Try to get the Udp4 protocol from the ChildHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  ChildHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Udp4Proto,
+                  gUdp4DriverBinding.DriverBindingHandle,
+                  ChildHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (Udp4Proto);
+
+  if (Instance->InDestroy) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Use the Destroyed flag to avoid the re-entering of the following code.
+  //
+  Instance->InDestroy = TRUE;
+
+  //
+  // Close the Ip4 protocol.
+  //
+  gBS->CloseProtocol (
+         Udp4Service->IpIo->ChildHandle,
+         &gEfiIp4ProtocolGuid,
+         gUdp4DriverBinding.DriverBindingHandle,
+         Instance->ChildHandle
+         );
+  //
+  // Close the Ip4 protocol on this instance's IpInfo.
+  //
+  gBS->CloseProtocol (
+         Instance->IpInfo->ChildHandle,
+         &gEfiIp4ProtocolGuid,
+         gUdp4DriverBinding.DriverBindingHandle,
+         Instance->ChildHandle
+         );  
+
+  //
+  // Uninstall the Udp4Protocol previously installed on the ChildHandle.
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ChildHandle,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID *) &Instance->Udp4Proto,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    Instance->InDestroy = FALSE;
+    return Status;
+  }
+
+  //
+  // Reset the configuration in case the instance's consumer forgets to do this.
+  //
+  Udp4Proto->Configure (Udp4Proto, NULL);
+
+  //
+  // Remove the IpInfo this instance consumes.
+  //
+  IpIoRemoveIp (Udp4Service->IpIo, Instance->IpInfo);
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Remove this instance from the service context data's ChildrenList.
+  //
+  RemoveEntryList (&Instance->Link);
+  Udp4Service->ChildrenNumber--;
+
+  //
+  // Clean the instance.
+  //
+  Udp4CleanInstance (Instance);
+
+  gBS->RestoreTPL (OldTpl);
+
+  FreePool (Instance);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers.
+  
+  The entry point for Udp4 driver which installs the driver binding
+  and component name protocol on its ImageHandle.
+
+  @param[in] ImageHandle           The firmware allocated handle for the UEFI image.
+  @param[in] SystemTable           A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Install the Udp4DriverBinding and Udp4ComponentName protocols.
+  //
+  Status = EfiLibInstallDriverBindingComponentName2 (
+             ImageHandle,
+             SystemTable,
+             &gUdp4DriverBinding,
+             ImageHandle,
+             &gUdp4ComponentName,
+             &gUdp4ComponentName2
+             );
+  if (!EFI_ERROR (Status)) {
+    //
+    // Initialize the UDP random port.
+    //
+    mUdp4RandomPort = (UINT16) (((UINT16) NetRandomInitSeed ()) % UDP4_PORT_KNOWN + UDP4_PORT_KNOWN);
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.h b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.h
new file mode 100644
index 0000000000..db2127015f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Driver.h
@@ -0,0 +1,154 @@
+/** @file
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _UDP4_DRIVER_H_
+#define _UDP4_DRIVER_H_
+
+#include <Uefi.h>
+#include <Library/BaseLib.h>
+#include <Library/NetLib.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/ServiceBinding.h>
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+
+  @param[in]  This                Protocol instance pointer.
+  @param[in]  ControllerHandle    Handle of device to test
+  @param[in]  RemainingDevicePath Optional parameter use to pick a specific child
+                                  device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     *RemainingDevicePath  OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+  
+  @param[in]  This              Protocol instance pointer.
+  @param[in]  ControllerHandle  Handle of device to stop driver on
+  @param[in]  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                                children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4DriverBindingStop (
+  IN  EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  UINTN                        NumberOfChildren,
+  IN  EFI_HANDLE                   *ChildHandleBuffer
+  );
+
+/**
+  Creates a child handle and installs a protocol.
+  
+  The CreateChild() function installs a protocol on ChildHandle. 
+  If ChildHandle is a pointer to NULL, then a new handle is created and returned in ChildHandle. 
+  If ChildHandle is not a pointer to NULL, then the protocol installs on the existing ChildHandle.
+
+  @param[in] This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param[in] ChildHandle Pointer to the handle of the child to create. If it is NULL,
+                         then a new handle is created. If it is a pointer to an existing UEFI handle, 
+                         then the protocol is added to the existing UEFI handle.
+
+  @retval EFI_SUCCES            The protocol was added to ChildHandle.
+  @retval EFI_INVALID_PARAMETER ChildHandle is NULL.
+  @retval EFI_OUT_OF_RESOURCES  There are not enough resources availabe to create
+                                the child
+  @retval other                 The child handle was not created
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4ServiceBindingCreateChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    *ChildHandle
+  );
+
+/**
+  Destroys a child handle with a protocol installed on it.
+  
+  The DestroyChild() function does the opposite of CreateChild(). It removes a protocol 
+  that was installed by CreateChild() from ChildHandle. If the removed protocol is the 
+  last protocol on ChildHandle, then ChildHandle is destroyed.
+
+  @param[in] This        Pointer to the EFI_SERVICE_BINDING_PROTOCOL instance.
+  @param[in] ChildHandle Handle of the child to destroy
+
+  @retval EFI_SUCCES            The protocol was removed from ChildHandle.
+  @retval EFI_UNSUPPORTED       ChildHandle does not support the protocol that is being removed.
+  @retval EFI_INVALID_PARAMETER Child handle is NULL.
+  @retval EFI_ACCESS_DENIED     The protocol could not be removed from the ChildHandle
+                                because its services are being used.
+  @retval other                 The child handle was not destroyed
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4ServiceBindingDestroyChild (
+  IN EFI_SERVICE_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                    ChildHandle
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.inf b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.inf
new file mode 100644
index 0000000000..e1b4898c31
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.inf
@@ -0,0 +1,70 @@
+## @file
+#  This module produces EFI UDP Protocol and EFI UDPv4 Service Binding Protocol.
+#
+#  This module produces EFI UDP(User Datagram Protocol) Protocol upon EFI IPv4
+#  Protocol, to provide basic UDPv4 I/O services.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = Udp4Dxe
+  MODULE_UNI_FILE                = Udp4Dxe.uni
+  FILE_GUID                      = 6d6963ab-906d-4a65-a7ca-bd40e5d6af2b
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = Udp4DriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+#  DRIVER_BINDING                =  gUdp4DriverBinding                        
+#  COMPONENT_NAME                =  gUdp4ComponentName
+#  COMPONENT_NAME2               =  gUdp4ComponentName2
+#
+
+[Sources]
+  Udp4Impl.h
+  Udp4Main.c
+  ComponentName.c
+  Udp4Impl.c
+  Udp4Driver.h
+  Udp4Driver.c
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  UefiLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  DebugLib
+  IpIoLib
+  NetLib
+  DpcLib
+
+[Protocols]
+  gEfiUdp4ServiceBindingProtocolGuid            ## BY_START
+  gEfiIp4ServiceBindingProtocolGuid             ## TO_START
+  gEfiUdp4ProtocolGuid                          ## BY_START
+  gEfiIp4ProtocolGuid                           ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  Udp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.uni b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.uni
new file mode 100644
index 0000000000..6cb5ff0b73
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Dxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4DxeExtra.uni b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4DxeExtra.uni
new file mode 100644
index 0000000000..09a7a35b02
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4DxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.c b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.c
new file mode 100644
index 0000000000..4f077d6b96
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.c
@@ -0,0 +1,1898 @@
+/** @file
+  The implementation of the Udp4 protocol.
+  
+Copyright (c) 2006 - 2014, 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 "Udp4Impl.h"
+
+UINT16  mUdp4RandomPort;
+
+/**
+  This function checks and timeouts the I/O datagrams holding by the corresponding
+  service context.
+
+  @param[in]  Event                  The event this function registered to.
+  @param[in]  Context                The context data registered during the creation of
+                                     the Event.
+
+**/
+VOID
+EFIAPI
+Udp4CheckTimeout (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  This function finds the udp instance by the specified <Address, Port> pair.
+
+  @param[in]  InstanceList           Pointer to the head of the list linking the udp
+                                     instances.
+  @param[in]  Address                Pointer to the specified IPv4 address.
+  @param[in]  Port                   The udp port number.
+
+  @retval TRUE     The specified <Address, Port> pair is found.
+  @retval FALSE    Otherwise.
+
+**/
+BOOLEAN
+Udp4FindInstanceByPort (
+  IN LIST_ENTRY        *InstanceList,
+  IN EFI_IPv4_ADDRESS  *Address,
+  IN UINT16            Port
+  );
+
+/**
+  This function is the packet transmitting notify function registered to the IpIo
+  interface. It's called to signal the udp TxToken when IpIo layer completes the
+  transmitting of the udp datagram.
+
+  @param[in]  Status                 The completion status of the output udp datagram.
+  @param[in]  Context                Pointer to the context data.
+  @param[in]  Sender                 Specify a pointer of EFI_IP4_PROTOCOL for sending.
+  @param[in]  NotifyData             Pointer to the notify data.
+
+**/
+VOID
+EFIAPI
+Udp4DgramSent (
+  IN EFI_STATUS        Status,
+  IN VOID              *Context,
+  IN IP_IO_IP_PROTOCOL Sender,
+  IN VOID              *NotifyData
+  );
+
+/**
+  This function processes the received datagram passed up by the IpIo layer.
+
+  @param[in]  Status             The status of this udp datagram.
+  @param[in]  IcmpError          The IcmpError code, only available when Status is
+                                 EFI_ICMP_ERROR.
+  @param[in]  NetSession         Pointer to the EFI_NET_SESSION_DATA.
+  @param[in]  Packet             Pointer to the NET_BUF containing the received udp
+                                 datagram.
+  @param[in]  Context            Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4DgramRcvd (
+  IN EFI_STATUS            Status,
+  IN UINT8                 IcmpError,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet,
+  IN VOID                  *Context
+  );
+
+/**
+  This function cancels the token specified by Arg in the Map. This is a callback
+  used by Udp4InstanceCancelToken().
+
+  @param[in]  Map                Pointer to the NET_MAP.
+  @param[in]  Item               Pointer to the NET_MAP_ITEM.
+  @param[in]  Arg                Pointer to the token to be cancelled, if NULL,
+                                 the token specified by Item is cancelled.
+
+  @retval EFI_SUCCESS            The token is cancelled if Arg is NULL or the token
+                                 is not the same as that in the Item if Arg is not
+                                 NULL.
+  @retval EFI_ABORTED            Arg is not NULL, and the token specified by Arg is
+                                 cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4CancelTokens (
+  IN NET_MAP       *Map,
+  IN NET_MAP_ITEM  *Item,
+  IN VOID          *Arg OPTIONAL
+  );
+
+/**
+  This function matches the received udp datagram with the Instance.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  Udp4Session        Pointer to the EFI_UDP4_SESSION_DATA abstracted
+                                 from the received udp datagram.
+
+  @retval TRUE       The udp datagram matches the receiving requirments of the
+                     udp Instance.
+  @retval FALSE      Otherwise.
+
+**/
+BOOLEAN
+Udp4MatchDgram (
+  IN UDP4_INSTANCE_DATA     *Instance,
+  IN EFI_UDP4_SESSION_DATA  *Udp4Session
+  );
+
+/**
+  This function removes the Wrap specified by Context and release relevant resources.
+
+  @param[in]  Event              The Event this notify function registered to.
+  @param[in]  Context            Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4RecycleRxDataWrap (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  );
+
+/**
+  This function wraps the Packet and the RxData.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  Packet                 Pointer to the buffer containing the received
+                                     datagram.
+  @param[in]  RxData                 Pointer to the EFI_UDP4_RECEIVE_DATA of this
+                                     datagram.
+
+  @return Pointer to the structure wrapping the RxData and the Packet.
+
+**/
+UDP4_RXDATA_WRAP *
+Udp4WrapRxData (
+  IN UDP4_INSTANCE_DATA     *Instance,
+  IN NET_BUF                *Packet,
+  IN EFI_UDP4_RECEIVE_DATA  *RxData
+  );
+
+/**
+  This function enqueues the received datagram into the instances' receiving queues.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  Packet                 Pointer to the buffer containing the received
+                                     datagram.
+  @param[in]  RxData                 Pointer to the EFI_UDP4_RECEIVE_DATA of this
+                                     datagram.
+
+  @return The times this datagram is enqueued.
+
+**/
+UINTN
+Udp4EnqueueDgram (
+  IN UDP4_SERVICE_DATA      *Udp4Service,
+  IN NET_BUF                *Packet,
+  IN EFI_UDP4_RECEIVE_DATA  *RxData
+  );
+
+/**
+  This function delivers the datagrams enqueued in the instances.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+
+**/
+VOID
+Udp4DeliverDgram (
+  IN UDP4_SERVICE_DATA  *Udp4Service
+  );
+
+/**
+  This function demultiplexes the received udp datagram to the apropriate instances.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA abstrated from
+                                     the received datagram.
+  @param[in]  Packet                 Pointer to the buffer containing the received udp
+                                     datagram.
+
+**/
+VOID
+Udp4Demultiplex (
+  IN UDP4_SERVICE_DATA     *Udp4Service,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet
+  );
+
+/**
+  This function handles the received Icmp Error message and demultiplexes it to the
+  instance.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  IcmpError              The icmp error code.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA abstracted
+                                 from the received Icmp Error packet.
+  @param[in]  Packet                 Pointer to the Icmp Error packet.
+
+**/
+VOID
+Udp4IcmpHandler (
+  IN UDP4_SERVICE_DATA     *Udp4Service,
+  IN UINT8                 IcmpError,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet
+  );
+
+/**
+  This function builds and sends out a icmp port unreachable message.
+
+  @param[in]  IpIo                   Pointer to the IP_IO instance.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA of the packet
+                                     causes this icmp error message.
+  @param[in]  Udp4Header             Pointer to the udp header of the datagram causes
+                                     this icmp error message.
+
+**/
+VOID
+Udp4SendPortUnreach (
+  IN IP_IO                 *IpIo,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN VOID                  *Udp4Header
+  );
+
+
+/**
+  Create the Udp service context data.
+
+  @param[in, out] Udp4Service      Pointer to the UDP4_SERVICE_DATA.
+  @param[in]      ImageHandle      The image handle of this udp4 driver.
+  @param[in]      ControllerHandle The controller handle this udp4 driver binds on.
+
+  @retval EFI_SUCCESS              The udp4 service context data is created and
+                                   initialized.
+  @retval EFI_OUT_OF_RESOURCES     Cannot allocate memory.
+  @retval other                    Other error occurs.
+
+**/
+EFI_STATUS
+Udp4CreateService (
+  IN OUT UDP4_SERVICE_DATA  *Udp4Service,
+  IN     EFI_HANDLE         ImageHandle,
+  IN     EFI_HANDLE         ControllerHandle
+  )
+{
+  EFI_STATUS          Status;
+  IP_IO_OPEN_DATA     OpenData;
+  EFI_IP4_CONFIG_DATA *Ip4ConfigData;
+
+  ZeroMem (Udp4Service, sizeof (UDP4_SERVICE_DATA));
+
+  Udp4Service->Signature        = UDP4_SERVICE_DATA_SIGNATURE;
+  Udp4Service->ServiceBinding   = mUdp4ServiceBinding;
+  Udp4Service->ImageHandle      = ImageHandle;
+  Udp4Service->ControllerHandle = ControllerHandle;
+  Udp4Service->ChildrenNumber   = 0;
+
+  InitializeListHead (&Udp4Service->ChildrenList);
+
+  //
+  // Create the IpIo for this service context.
+  //
+  Udp4Service->IpIo = IpIoCreate (ImageHandle, ControllerHandle, IP_VERSION_4);
+  if (Udp4Service->IpIo == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Set the OpenData used to open the IpIo.
+  //
+  Ip4ConfigData = &OpenData.IpConfigData.Ip4CfgData;
+  CopyMem (Ip4ConfigData, &mIp4IoDefaultIpConfigData, sizeof (EFI_IP4_CONFIG_DATA));
+  Ip4ConfigData->AcceptBroadcast = TRUE;
+  OpenData.RcvdContext           = (VOID *) Udp4Service;
+  OpenData.SndContext            = NULL;
+  OpenData.PktRcvdNotify         = Udp4DgramRcvd;
+  OpenData.PktSentNotify         = Udp4DgramSent;
+
+  //
+  // Configure and start the IpIo.
+  //
+  Status = IpIoOpen (Udp4Service->IpIo, &OpenData);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Create the event for Udp timeout checking.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  Udp4CheckTimeout,
+                  Udp4Service,
+                  &Udp4Service->TimeoutEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Start the timeout timer event.
+  //
+  Status = gBS->SetTimer (
+                  Udp4Service->TimeoutEvent,
+                  TimerPeriodic,
+                  UDP4_TIMEOUT_INTERVAL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  if (Udp4Service->TimeoutEvent != NULL) {
+    gBS->CloseEvent (Udp4Service->TimeoutEvent);
+  }
+
+  IpIoDestroy (Udp4Service->IpIo);
+
+  return Status;
+}
+
+
+/**
+  Clean the Udp service context data.
+
+  @param[in]  Udp4Service            Pointer to the UDP4_SERVICE_DATA.
+
+**/
+VOID
+Udp4CleanService (
+  IN UDP4_SERVICE_DATA  *Udp4Service
+  )
+{
+  //
+  // Cancel the TimeoutEvent timer.
+  //
+  gBS->SetTimer (Udp4Service->TimeoutEvent, TimerCancel, 0);
+
+  //
+  // Close the TimeoutEvent timer.
+  //
+  gBS->CloseEvent (Udp4Service->TimeoutEvent);
+
+  //
+  // Destroy the IpIo.
+  //
+  IpIoDestroy (Udp4Service->IpIo);
+}
+
+
+/**
+  This function checks and timeouts the I/O datagrams holding by the corresponding
+  service context.
+
+  @param[in]  Event                  The event this function registered to.
+  @param[in]  Context                The context data registered during the creation of
+                                     the Event.
+
+**/
+VOID
+EFIAPI
+Udp4CheckTimeout (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  UDP4_SERVICE_DATA   *Udp4Service;
+  LIST_ENTRY          *Entry;
+  UDP4_INSTANCE_DATA  *Instance;
+  LIST_ENTRY          *WrapEntry;
+  LIST_ENTRY          *NextEntry;
+  UDP4_RXDATA_WRAP    *Wrap;
+
+  Udp4Service = (UDP4_SERVICE_DATA *) Context;
+  NET_CHECK_SIGNATURE (Udp4Service, UDP4_SERVICE_DATA_SIGNATURE);
+
+  NET_LIST_FOR_EACH (Entry, &Udp4Service->ChildrenList) {
+    //
+    // Iterate all the instances belonging to this service context.
+    //
+    Instance = NET_LIST_USER_STRUCT (Entry, UDP4_INSTANCE_DATA, Link);
+    NET_CHECK_SIGNATURE (Instance, UDP4_INSTANCE_DATA_SIGNATURE);
+
+    if (!Instance->Configured || (Instance->ConfigData.ReceiveTimeout == 0)) {
+      //
+      // Skip this instance if it's not configured or no receive timeout.
+      //
+      continue;
+    }
+
+    NET_LIST_FOR_EACH_SAFE (WrapEntry, NextEntry, &Instance->RcvdDgramQue) {
+      //
+      // Iterate all the rxdatas belonging to this udp instance.
+      //
+      Wrap = NET_LIST_USER_STRUCT (WrapEntry, UDP4_RXDATA_WRAP, Link);
+
+      //
+      // TimeoutTick unit is microsecond, MNP_TIMEOUT_CHECK_INTERVAL unit is 100ns.
+      //
+      if (Wrap->TimeoutTick < (UDP4_TIMEOUT_INTERVAL / 10)) {
+        //
+        // Remove this RxData if it timeouts.
+        //
+        Udp4RecycleRxDataWrap (NULL, (VOID *) Wrap);
+      } else {
+        Wrap->TimeoutTick -= (UDP4_TIMEOUT_INTERVAL / 10);
+      }
+    }
+  }
+}
+
+
+/**
+  This function intializes the new created udp instance.
+
+  @param[in]      Udp4Service       Pointer to the UDP4_SERVICE_DATA.
+  @param[in, out] Instance          Pointer to the un-initialized UDP4_INSTANCE_DATA.
+
+**/
+VOID
+Udp4InitInstance (
+  IN     UDP4_SERVICE_DATA   *Udp4Service,
+  IN OUT UDP4_INSTANCE_DATA  *Instance
+  )
+{
+  //
+  // Set the signature.
+  //
+  Instance->Signature = UDP4_INSTANCE_DATA_SIGNATURE;
+
+  //
+  // Init the lists.
+  //
+  InitializeListHead (&Instance->Link);
+  InitializeListHead (&Instance->RcvdDgramQue);
+  InitializeListHead (&Instance->DeliveredDgramQue);
+
+  //
+  // Init the NET_MAPs.
+  //
+  NetMapInit (&Instance->TxTokens);
+  NetMapInit (&Instance->RxTokens);
+  NetMapInit (&Instance->McastIps);
+
+  //
+  // Save the pointer to the UDP4_SERVICE_DATA, and initialize other members.
+  //
+  Instance->Udp4Service = Udp4Service;
+  CopyMem (&Instance->Udp4Proto, &mUdp4Protocol, sizeof (Instance->Udp4Proto));
+  Instance->IcmpError   = EFI_SUCCESS;
+  Instance->Configured  = FALSE;
+  Instance->IsNoMapping = FALSE;
+  Instance->InDestroy   = FALSE;
+}
+
+
+/**
+  This function cleans the udp instance.
+
+  @param[in]  Instance               Pointer to the UDP4_INSTANCE_DATA to clean.
+
+**/
+VOID
+Udp4CleanInstance (
+  IN UDP4_INSTANCE_DATA  *Instance
+  )
+{
+  NetMapClean (&Instance->McastIps);
+  NetMapClean (&Instance->RxTokens);
+  NetMapClean (&Instance->TxTokens);
+}
+
+
+/**
+  This function finds the udp instance by the specified <Address, Port> pair.
+
+  @param[in]  InstanceList           Pointer to the head of the list linking the udp
+                                     instances.
+  @param[in]  Address                Pointer to the specified IPv4 address.
+  @param[in]  Port                   The udp port number.
+
+  @retval TRUE     The specified <Address, Port> pair is found.
+  @retval FALSE    Otherwise.
+
+**/
+BOOLEAN
+Udp4FindInstanceByPort (
+  IN LIST_ENTRY        *InstanceList,
+  IN EFI_IPv4_ADDRESS  *Address,
+  IN UINT16            Port
+  )
+{
+  LIST_ENTRY            *Entry;
+  UDP4_INSTANCE_DATA    *Instance;
+  EFI_UDP4_CONFIG_DATA  *ConfigData;
+
+  NET_LIST_FOR_EACH (Entry, InstanceList) {
+    //
+    // Iterate all the udp instances.
+    //
+    Instance   = NET_LIST_USER_STRUCT (Entry, UDP4_INSTANCE_DATA, Link);
+    ConfigData = &Instance->ConfigData;
+
+    if (!Instance->Configured || ConfigData->AcceptAnyPort) {
+      //
+      // If the instance is not configured or the configdata of the instance indicates
+      // this instance accepts any port, skip it.
+      //
+      continue;
+    }
+
+    if (EFI_IP4_EQUAL (&ConfigData->StationAddress, Address) &&
+      (ConfigData->StationPort == Port)) {
+      //
+      // if both the address and the port are the same, return TRUE.
+      //
+      return TRUE;
+    }
+  }
+
+  //
+  // return FALSE when matching fails.
+  //
+  return FALSE;
+}
+
+
+/**
+  This function tries to bind the udp instance according to the configured port
+  allocation strategy.
+
+  @param[in]      InstanceList   Pointer to the head of the list linking the udp
+                                 instances.
+  @param[in, out] ConfigData     Pointer to the ConfigData of the instance to be
+                                 bound. ConfigData->StationPort will be assigned
+                                 with an available port value on success.
+
+  @retval EFI_SUCCESS            The bound operation is completed successfully.
+  @retval EFI_ACCESS_DENIED      The <Address, Port> specified by the ConfigData is
+                                 already used by other instance.
+  @retval EFI_OUT_OF_RESOURCES   No available port resources.
+
+**/
+EFI_STATUS
+Udp4Bind (
+  IN     LIST_ENTRY            *InstanceList,
+  IN OUT EFI_UDP4_CONFIG_DATA  *ConfigData
+  )
+{
+  EFI_IPv4_ADDRESS  *StationAddress;
+  UINT16            StartPort;
+
+  if (ConfigData->AcceptAnyPort) {
+    return EFI_SUCCESS;
+  }
+
+  StationAddress = &ConfigData->StationAddress;
+
+  if (ConfigData->StationPort != 0) {
+
+    if (!ConfigData->AllowDuplicatePort &&
+      Udp4FindInstanceByPort (InstanceList, StationAddress, ConfigData->StationPort)) {
+      //
+      // Do not allow duplicate port and the port is already used by other instance.
+      //
+      return EFI_ACCESS_DENIED;
+    }
+  } else {
+    //
+    // select a random port for this instance;
+    //
+
+    if (ConfigData->AllowDuplicatePort) {
+      //
+      // Just pick up the random port if the instance allows duplicate port.
+      //
+      ConfigData->StationPort = mUdp4RandomPort;
+    } else {
+
+      StartPort = mUdp4RandomPort;
+
+      while (Udp4FindInstanceByPort(InstanceList, StationAddress, mUdp4RandomPort)) {
+
+        mUdp4RandomPort++;
+        if (mUdp4RandomPort == 0) {
+          mUdp4RandomPort = UDP4_PORT_KNOWN;
+        }
+
+        if (mUdp4RandomPort == StartPort) {
+          //
+          // No available port.
+          //
+          return EFI_OUT_OF_RESOURCES;
+        }
+      }
+
+      ConfigData->StationPort = mUdp4RandomPort;
+    }
+
+    mUdp4RandomPort++;
+    if (mUdp4RandomPort == 0) {
+      mUdp4RandomPort = UDP4_PORT_KNOWN;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function is used to check whether the NewConfigData has any un-reconfigurable
+  parameters changed compared to the OldConfigData.
+
+  @param[in]  OldConfigData      Pointer to the current ConfigData the udp instance
+                                 uses.
+  @param[in]  NewConfigData      Pointer to the new ConfigData.
+
+  @retval TRUE     The instance is reconfigurable.
+  @retval FALSE    Otherwise.
+
+**/
+BOOLEAN
+Udp4IsReconfigurable (
+  IN EFI_UDP4_CONFIG_DATA  *OldConfigData,
+  IN EFI_UDP4_CONFIG_DATA  *NewConfigData
+  )
+{
+  if ((NewConfigData->AcceptAnyPort      != OldConfigData->AcceptAnyPort)     ||
+      (NewConfigData->AcceptBroadcast    != OldConfigData->AcceptBroadcast)   ||
+      (NewConfigData->AcceptPromiscuous  != OldConfigData->AcceptPromiscuous) ||
+      (NewConfigData->AllowDuplicatePort != OldConfigData->AllowDuplicatePort)
+      ) {
+    //
+    // The receiving filter parameters cannot be changed.
+    //
+    return FALSE;
+  }
+
+  if ((!NewConfigData->AcceptAnyPort) &&
+      (NewConfigData->StationPort != OldConfigData->StationPort)
+      ) {
+    //
+    // The port is not changeable.
+    //
+    return FALSE;
+  }
+
+  if (!NewConfigData->AcceptPromiscuous) {
+
+    if (NewConfigData->UseDefaultAddress != OldConfigData->UseDefaultAddress) {
+      //
+      // The NewConfigData differs to the old one on the UseDefaultAddress.
+      //
+      return FALSE;
+    }
+
+    if (!NewConfigData->UseDefaultAddress &&
+        (!EFI_IP4_EQUAL (&NewConfigData->StationAddress, &OldConfigData->StationAddress) ||
+         !EFI_IP4_EQUAL (&NewConfigData->SubnetMask, &OldConfigData->SubnetMask))
+        ) {
+      //
+      // If the instance doesn't use the default address, and the new address or
+      // new subnet mask is different from the old values.
+      //
+      return FALSE;
+    }
+  }
+
+  if (!EFI_IP4_EQUAL (&NewConfigData->RemoteAddress, &OldConfigData->RemoteAddress)) {
+    //
+    // The remoteaddress is not the same.
+    //
+    return FALSE;
+  }
+
+  if (!EFI_IP4_EQUAL (&NewConfigData->RemoteAddress, &mZeroIp4Addr) &&
+      NewConfigData->RemotePort != OldConfigData->RemotePort
+      ) {
+    //
+    // The RemotePort differs if it's designated in the configdata.
+    //
+    return FALSE;
+  }
+
+  //
+  // All checks pass, return TRUE.
+  //
+  return TRUE;
+}
+
+
+/**
+  This function builds the Ip4 configdata from the Udp4ConfigData.
+
+  @param[in]       Udp4ConfigData    Pointer to the EFI_UDP4_CONFIG_DATA.
+  @param[in, out]  Ip4ConfigData     Pointer to the EFI_IP4_CONFIG_DATA.
+
+**/
+VOID
+Udp4BuildIp4ConfigData (
+  IN     EFI_UDP4_CONFIG_DATA  *Udp4ConfigData,
+  IN OUT EFI_IP4_CONFIG_DATA   *Ip4ConfigData
+  )
+{
+  CopyMem (Ip4ConfigData, &mIp4IoDefaultIpConfigData, sizeof (*Ip4ConfigData));
+
+  Ip4ConfigData->DefaultProtocol   = EFI_IP_PROTO_UDP;
+  Ip4ConfigData->AcceptBroadcast   = Udp4ConfigData->AcceptBroadcast;
+  Ip4ConfigData->AcceptPromiscuous = Udp4ConfigData->AcceptPromiscuous;
+  Ip4ConfigData->UseDefaultAddress = Udp4ConfigData->UseDefaultAddress;
+  CopyMem (&Ip4ConfigData->StationAddress, &Udp4ConfigData->StationAddress, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Ip4ConfigData->SubnetMask, &Udp4ConfigData->SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+
+  //
+  // use the -1 magic number to disable the receiving process of the ip instance.
+  //
+  Ip4ConfigData->ReceiveTimeout    = (UINT32) (-1);
+}
+
+
+/**
+  This function validates the TxToken, it returns the error code according to the spec.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  TxToken            Pointer to the token to be checked.
+
+  @retval EFI_SUCCESS            The TxToken is valid.
+  @retval EFI_INVALID_PARAMETER  One or more of the following are TRUE: This is
+                                 NULL. Token is NULL. Token.Event is NULL.
+                                 Token.Packet.TxData is NULL.
+                                 Token.Packet.TxData.FragmentCount is zero.
+                                 Token.Packet.TxData.DataLength is not equal to the
+                                 sum of fragment lengths. One or more of the
+                                 Token.Packet.TxData.FragmentTable[].
+                                 FragmentLength fields is zero. One or more of the
+                                 Token.Packet.TxData.FragmentTable[].
+                                 FragmentBuffer fields is NULL.
+                                 Token.Packet.TxData. GatewayAddress is not a
+                                 unicast IPv4 address if it is not NULL. One or
+                                 more IPv4 addresses in Token.Packet.TxData.
+                                 UdpSessionData are not valid unicast IPv4
+                                 addresses if the UdpSessionData is not NULL.
+  @retval EFI_BAD_BUFFER_SIZE    The data length is greater than the maximum UDP
+                                 packet size.
+
+**/
+EFI_STATUS
+Udp4ValidateTxToken (
+  IN UDP4_INSTANCE_DATA         *Instance,
+  IN EFI_UDP4_COMPLETION_TOKEN  *TxToken
+  )
+{
+  EFI_UDP4_TRANSMIT_DATA  *TxData;
+  UINT32                  Index;
+  UINT32                  TotalLen;
+  EFI_UDP4_CONFIG_DATA    *ConfigData;
+  EFI_UDP4_SESSION_DATA   *UdpSessionData;
+  IP4_ADDR                SourceAddress;
+  IP4_ADDR                GatewayAddress;
+
+  if (TxToken->Event == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TxData = TxToken->Packet.TxData;
+
+  if ((TxData == NULL) || (TxData->FragmentCount == 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TotalLen = 0;
+  for (Index = 0; Index < TxData->FragmentCount; Index++) {
+
+    if ((TxData->FragmentTable[Index].FragmentBuffer == NULL) ||
+      (TxData->FragmentTable[Index].FragmentLength == 0)) {
+      //
+      // if the FragmentBuffer is NULL or the FragmentLeng is zero.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    TotalLen += TxData->FragmentTable[Index].FragmentLength;
+  }
+
+  if (TotalLen != TxData->DataLength) {
+    //
+    // The TotalLen calculated by adding all the FragmentLeng doesn't equal to the
+    // DataLength.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TxData->GatewayAddress != NULL) {
+    CopyMem (&GatewayAddress, TxData->GatewayAddress, sizeof (IP4_ADDR));
+
+    if (!NetIp4IsUnicast (NTOHL (GatewayAddress), 0)) {
+      //
+      // The specified GatewayAddress is not a unicast IPv4 address while it's not 0.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  ConfigData     = &Instance->ConfigData;
+  UdpSessionData = TxData->UdpSessionData;
+
+  if (UdpSessionData != NULL) {
+
+    CopyMem (&SourceAddress, &UdpSessionData->SourceAddress, sizeof (IP4_ADDR));
+
+    if ((SourceAddress != 0) && !NetIp4IsUnicast (HTONL (SourceAddress), 0)) {
+      //
+      // Check whether SourceAddress is a valid IPv4 address in case it's not zero.
+      // The configured station address is used if SourceAddress is zero.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if ((UdpSessionData->DestinationPort == 0) && (ConfigData->RemotePort == 0)) {
+      //
+      // Ambiguous, no avalaible DestinationPort for this token.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (EFI_IP4_EQUAL (&UdpSessionData->DestinationAddress, &mZeroIp4Addr)) {
+      //
+      // The DestinationAddress specified in the UdpSessionData is 0.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+  } else if (EFI_IP4_EQUAL (&ConfigData->RemoteAddress, &mZeroIp4Addr)) {
+    //
+    // the configured RemoteAddress is all zero, and the user doens't override the
+    // destination address.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (TxData->DataLength > UDP4_MAX_DATA_SIZE) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function checks whether the specified Token duplicates with the one in the Map.
+
+  @param[in]  Map                Pointer to the NET_MAP.
+  @param[in]  Item               Pointer to the NET_MAP_ITEM contain the pointer to
+                                 the Token.
+  @param[in]  Context            Pointer to the Token to be checked.
+
+  @retval EFI_SUCCESS            The Token specified by Context differs from the
+                                 one in the Item.
+  @retval EFI_ACCESS_DENIED      The Token duplicates with the one in the Item.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4TokenExist (
+  IN NET_MAP       *Map,
+  IN NET_MAP_ITEM  *Item,
+  IN VOID          *Context
+  )
+{
+  EFI_UDP4_COMPLETION_TOKEN  *Token;
+  EFI_UDP4_COMPLETION_TOKEN  *TokenInItem;
+
+  Token       = (EFI_UDP4_COMPLETION_TOKEN*) Context;
+  TokenInItem = (EFI_UDP4_COMPLETION_TOKEN*) Item->Key;
+
+  if ((Token == TokenInItem) || (Token->Event == TokenInItem->Event)) {
+    //
+    // The Token duplicates with the TokenInItem in case either the two pointers are the
+    // same or the Events of these two tokens are the same.
+    //
+    return EFI_ACCESS_DENIED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function calculates the checksum for the Packet, utilizing the pre-calculated
+  pseudo HeadSum to reduce some overhead.
+
+  @param[in]  Packet             Pointer to the NET_BUF contains the udp datagram.
+  @param[in]  HeadSum            Checksum of the pseudo header execpt the length
+                                 field.
+
+  @retval The 16-bit checksum of this udp datagram.
+
+**/
+UINT16
+Udp4Checksum (
+  IN NET_BUF *Packet,
+  IN UINT16  HeadSum
+  )
+{
+  UINT16  Checksum;
+
+  Checksum  = NetbufChecksum (Packet);
+  Checksum  = NetAddChecksum (Checksum, HeadSum);
+
+  Checksum  = NetAddChecksum (Checksum, HTONS ((UINT16) Packet->TotalSize));
+
+  return (UINT16) ~Checksum;
+}
+
+
+/**
+  This function removes the specified Token from the TokenMap.
+
+  @param[in, out] TokenMap       Pointer to the NET_MAP containing the tokens.
+  @param[in]      Token          Pointer to the Token to be removed.
+
+  @retval EFI_SUCCESS            The specified Token is removed from the TokenMap.
+  @retval EFI_NOT_FOUND          The specified Token is not found in the TokenMap.
+
+**/
+EFI_STATUS
+Udp4RemoveToken (
+  IN OUT NET_MAP                    *TokenMap,
+  IN     EFI_UDP4_COMPLETION_TOKEN  *Token
+  )
+{
+  NET_MAP_ITEM  *Item;
+
+  //
+  // Find the Token first.
+  //
+  Item = NetMapFindKey (TokenMap, (VOID *) Token);
+
+  if (Item != NULL) {
+    //
+    // Remove the token if it's found in the map.
+    //
+    NetMapRemoveItem (TokenMap, Item, NULL);
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  This function is the packet transmitting notify function registered to the IpIo
+  interface. It's called to signal the udp TxToken when IpIo layer completes the
+  transmitting of the udp datagram.
+
+  @param[in]  Status                 The completion status of the output udp datagram.
+  @param[in]  Context                Pointer to the context data.
+  @param[in]  Sender                 Specify a pointer of EFI_IP4_PROTOCOL for sending.
+  @param[in]  NotifyData             Pointer to the notify data.
+
+**/
+VOID
+EFIAPI
+Udp4DgramSent (
+  IN EFI_STATUS        Status,
+  IN VOID              *Context,
+  IN IP_IO_IP_PROTOCOL Sender,
+  IN VOID              *NotifyData
+  )
+{
+  UDP4_INSTANCE_DATA         *Instance;
+  EFI_UDP4_COMPLETION_TOKEN  *Token;
+
+  Instance = (UDP4_INSTANCE_DATA *) Context;
+  Token    = (EFI_UDP4_COMPLETION_TOKEN *) NotifyData;
+
+  if (Udp4RemoveToken (&Instance->TxTokens, Token) == EFI_SUCCESS) {
+    //
+    // The token may be cancelled. Only signal it if the remove operation succeeds.
+    //
+    Token->Status = Status;
+    gBS->SignalEvent (Token->Event);
+    DispatchDpc ();
+  }
+}
+
+
+/**
+  This function processes the received datagram passed up by the IpIo layer.
+
+  @param[in]  Status             The status of this udp datagram.
+  @param[in]  IcmpError          The IcmpError code, only available when Status is
+                                 EFI_ICMP_ERROR.
+  @param[in]  NetSession         Pointer to the EFI_NET_SESSION_DATA.
+  @param[in]  Packet             Pointer to the NET_BUF containing the received udp
+                                 datagram.
+  @param[in]  Context            Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4DgramRcvd (
+  IN EFI_STATUS            Status,
+  IN UINT8                 IcmpError,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet,
+  IN VOID                  *Context
+  )
+{
+  NET_CHECK_SIGNATURE (Packet, NET_BUF_SIGNATURE);
+
+  //
+  // IpIo only passes received packets with Status EFI_SUCCESS or EFI_ICMP_ERROR.
+  //
+  if (Status == EFI_SUCCESS) {
+    //
+    // Demultiplex the received datagram.
+    //
+    Udp4Demultiplex ((UDP4_SERVICE_DATA *) Context, NetSession, Packet);
+  } else {
+    //
+    // Handle the ICMP_ERROR packet.
+    //
+    Udp4IcmpHandler ((UDP4_SERVICE_DATA *) Context, IcmpError, NetSession, Packet);
+  }
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of the rx token's events
+  // which are signaled with received data.
+  //
+  DispatchDpc ();
+}
+
+
+/**
+  This function removes the multicast group specified by Arg from the Map.
+
+  @param[in, out] Map            Pointer to the NET_MAP.
+  @param[in]      Item           Pointer to the NET_MAP_ITEM.
+  @param[in]      Arg            Pointer to the Arg, it's the pointer to a
+                                 multicast IPv4 Address.
+
+  @retval EFI_SUCCESS            The multicast address is removed.
+  @retval EFI_ABORTED            The specified multicast address is removed and the
+                                 Arg is not NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4LeaveGroup (
+  IN OUT NET_MAP       *Map,
+  IN     NET_MAP_ITEM  *Item,
+  IN     VOID          *Arg OPTIONAL
+  )
+{
+  EFI_IPv4_ADDRESS  *McastIp;
+
+  McastIp = Arg;
+
+  if ((McastIp != NULL) && (!EFI_IP4_EQUAL (McastIp, &(Item->Key)))) {
+    //
+    // McastIp is not NULL and the multicast address contained in the Item
+    // is not the same as McastIp.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Remove this Item.
+  //
+  NetMapRemoveItem (Map, Item, NULL);
+
+  if (McastIp != NULL) {
+    //
+    // Return EFI_ABORTED in case McastIp is not NULL to terminate the iteration.
+    //
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function cancels the token specified by Arg in the Map. This is a callback
+  used by Udp4InstanceCancelToken().
+
+  @param[in]  Map                Pointer to the NET_MAP.
+  @param[in]  Item               Pointer to the NET_MAP_ITEM.
+  @param[in]  Arg                Pointer to the token to be cancelled, if NULL,
+                                 the token specified by Item is cancelled.
+
+  @retval EFI_SUCCESS            The token is cancelled if Arg is NULL or the token
+                                 is not the same as that in the Item if Arg is not
+                                 NULL.
+  @retval EFI_ABORTED            Arg is not NULL, and the token specified by Arg is
+                                 cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4CancelTokens (
+  IN NET_MAP       *Map,
+  IN NET_MAP_ITEM  *Item,
+  IN VOID          *Arg OPTIONAL
+  )
+{
+  EFI_UDP4_COMPLETION_TOKEN  *TokenToCancel;
+  NET_BUF                    *Packet;
+  IP_IO                      *IpIo;
+
+  if ((Arg != NULL) && (Item->Key != Arg)) {
+    return EFI_SUCCESS;
+  }
+
+  if (Item->Value != NULL) {
+    //
+    // If the token is a transmit token, the corresponding Packet is recorded in
+    // Item->Value, invoke IpIo to cancel this packet first. The IpIoCancelTxToken
+    // will invoke Udp4DgramSent, the token will be signaled and this Item will
+    // be removed from the Map there.
+    //
+    Packet = (NET_BUF *) (Item->Value);
+    IpIo   = (IP_IO *) (*((UINTN *) &Packet->ProtoData[0]));
+
+    IpIoCancelTxToken (IpIo, Packet);
+  } else {
+    //
+    // The token is a receive token. Abort it and remove it from the Map.
+    //
+    TokenToCancel = (EFI_UDP4_COMPLETION_TOKEN *) Item->Key;
+    NetMapRemoveItem (Map, Item, NULL);
+
+    TokenToCancel->Status = EFI_ABORTED;
+    gBS->SignalEvent (TokenToCancel->Event);
+  }
+
+  if (Arg != NULL) {
+    return EFI_ABORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function removes all the Wrap datas in the RcvdDgramQue.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+
+**/
+VOID
+Udp4FlushRcvdDgram (
+  IN UDP4_INSTANCE_DATA  *Instance
+  )
+{
+  UDP4_RXDATA_WRAP  *Wrap;
+
+  while (!IsListEmpty (&Instance->RcvdDgramQue)) {
+    //
+    // Iterate all the Wraps in the RcvdDgramQue.
+    //
+    Wrap = NET_LIST_HEAD (&Instance->RcvdDgramQue, UDP4_RXDATA_WRAP, Link);
+
+    //
+    // The Wrap will be removed from the RcvdDgramQue by this function call.
+    //
+    Udp4RecycleRxDataWrap (NULL, (VOID *) Wrap);
+  }
+}
+
+
+
+/**
+  Cancel Udp4 tokens from the Udp4 instance.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  Token              Pointer to the token to be canceled, if NULL, all
+                                 tokens in this instance will be cancelled.
+
+  @retval EFI_SUCCESS            The Token is cancelled.
+  @retval EFI_NOT_FOUND          The Token is not found.
+
+**/
+EFI_STATUS
+Udp4InstanceCancelToken (
+  IN UDP4_INSTANCE_DATA         *Instance,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token OPTIONAL
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Cancel this token from the TxTokens map.
+  //
+  Status = NetMapIterate (&Instance->TxTokens, Udp4CancelTokens, Token);
+
+  if ((Token != NULL) && (Status == EFI_ABORTED)) {
+    //
+    // If Token isn't NULL and Status is EFI_ABORTED, the token is cancelled from
+    // the TxTokens, just return success.
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Try to cancel this token from the RxTokens map in condition either the Token
+  // is NULL or the specified Token is not in TxTokens.
+  //
+  Status = NetMapIterate (&Instance->RxTokens, Udp4CancelTokens, Token);
+
+  if ((Token != NULL) && (Status == EFI_SUCCESS)) {
+    //
+    // If Token isn't NULL and Status is EFI_SUCCESS, the token is neither in the
+    // TxTokens nor the RxTokens, or say, it's not found.
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  ASSERT ((Token != NULL) || ((0 == NetMapGetCount (&Instance->TxTokens))
+    && (0 == NetMapGetCount (&Instance->RxTokens))));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function matches the received udp datagram with the Instance.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  Udp4Session        Pointer to the EFI_UDP4_SESSION_DATA abstracted
+                                 from the received udp datagram.
+
+  @retval TRUE       The udp datagram matches the receiving requirments of the
+                     udp Instance.
+  @retval FALSE      Otherwise.
+
+**/
+BOOLEAN
+Udp4MatchDgram (
+  IN UDP4_INSTANCE_DATA     *Instance,
+  IN EFI_UDP4_SESSION_DATA  *Udp4Session
+  )
+{
+  EFI_UDP4_CONFIG_DATA  *ConfigData;
+  IP4_ADDR              Destination;
+
+  ConfigData = &Instance->ConfigData;
+
+  if (ConfigData->AcceptPromiscuous) {
+    //
+    // Always matches if this instance is in the promiscuous state.
+    //
+    return TRUE;
+  }
+
+  if ((!ConfigData->AcceptAnyPort && (Udp4Session->DestinationPort != ConfigData->StationPort)) ||
+      ((ConfigData->RemotePort != 0) && (Udp4Session->SourcePort != ConfigData->RemotePort))
+      ) {
+    //
+    // The local port or the remote port doesn't match.
+    //
+    return FALSE;
+  }
+
+  if (!EFI_IP4_EQUAL (&ConfigData->RemoteAddress, &mZeroIp4Addr) &&
+      !EFI_IP4_EQUAL (&ConfigData->RemoteAddress, &Udp4Session->SourceAddress)
+      ) {
+    //
+    // This datagram doesn't come from the instance's specified sender.
+    //
+    return FALSE;
+  }
+
+  if (EFI_IP4_EQUAL (&ConfigData->StationAddress, &mZeroIp4Addr) ||
+      EFI_IP4_EQUAL (&Udp4Session->DestinationAddress, &ConfigData->StationAddress)
+      ) {
+    //
+    // The instance is configured to receive datagrams destined to any station IP or
+    // the destination address of this datagram matches the configured station IP.
+    //
+    return TRUE;
+  }
+
+  CopyMem (&Destination, &Udp4Session->DestinationAddress, sizeof (IP4_ADDR));
+
+  if (IP4_IS_LOCAL_BROADCAST (Destination) && ConfigData->AcceptBroadcast) {
+    //
+    // The instance is configured to receive broadcast and this is a broadcast packet.
+    //
+    return TRUE;
+  }
+
+  if (IP4_IS_MULTICAST (NTOHL (Destination)) &&
+      NetMapFindKey (&Instance->McastIps, (VOID *) (UINTN) Destination) != NULL
+      ) {
+    //
+    // It's a multicast packet and the multicast address is accepted by this instance.
+    //
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+
+/**
+  This function removes the Wrap specified by Context and release relevant resources.
+
+  @param[in]  Event              The Event this notify function registered to.
+  @param[in]  Context            Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4RecycleRxDataWrap (
+  IN EFI_EVENT  Event,
+  IN VOID       *Context
+  )
+{
+  UDP4_RXDATA_WRAP  *Wrap;
+
+  Wrap = (UDP4_RXDATA_WRAP *) Context;
+
+  //
+  // Remove the Wrap from the list it belongs to.
+  //
+  RemoveEntryList (&Wrap->Link);
+
+  //
+  // Free the Packet associated with this Wrap.
+  //
+  NetbufFree (Wrap->Packet);
+
+  //
+  // Close the event.
+  //
+  gBS->CloseEvent (Wrap->RxData.RecycleSignal);
+
+  FreePool (Wrap);
+}
+
+
+/**
+  This function wraps the Packet and the RxData.
+
+  @param[in]  Instance               Pointer to the instance context data.
+  @param[in]  Packet                 Pointer to the buffer containing the received
+                                     datagram.
+  @param[in]  RxData                 Pointer to the EFI_UDP4_RECEIVE_DATA of this
+                                     datagram.
+
+  @return Pointer to the structure wrapping the RxData and the Packet.
+
+**/
+UDP4_RXDATA_WRAP *
+Udp4WrapRxData (
+  IN UDP4_INSTANCE_DATA     *Instance,
+  IN NET_BUF                *Packet,
+  IN EFI_UDP4_RECEIVE_DATA  *RxData
+  )
+{
+  EFI_STATUS            Status;
+  UDP4_RXDATA_WRAP      *Wrap;
+
+  //
+  // Allocate buffer for the Wrap.
+  //
+  Wrap = AllocatePool (sizeof (UDP4_RXDATA_WRAP) +
+         (Packet->BlockOpNum - 1) * sizeof (EFI_UDP4_FRAGMENT_DATA));
+  if (Wrap == NULL) {
+    return NULL;
+  }
+
+  InitializeListHead (&Wrap->Link);
+
+  CopyMem (&Wrap->RxData, RxData, sizeof (Wrap->RxData));
+
+  //
+  // Create the Recycle event.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  Udp4RecycleRxDataWrap,
+                  Wrap,
+                  &Wrap->RxData.RecycleSignal
+                  );
+  if (EFI_ERROR (Status)) {
+    FreePool (Wrap);
+    return NULL;
+  }
+
+  Wrap->Packet      = Packet;
+  Wrap->TimeoutTick = Instance->ConfigData.ReceiveTimeout;
+
+  return Wrap;
+}
+
+
+/**
+  This function enqueues the received datagram into the instances' receiving queues.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  Packet                 Pointer to the buffer containing the received
+                                     datagram.
+  @param[in]  RxData                 Pointer to the EFI_UDP4_RECEIVE_DATA of this
+                                     datagram.
+
+  @return The times this datagram is enqueued.
+
+**/
+UINTN
+Udp4EnqueueDgram (
+  IN UDP4_SERVICE_DATA      *Udp4Service,
+  IN NET_BUF                *Packet,
+  IN EFI_UDP4_RECEIVE_DATA  *RxData
+  )
+{
+  LIST_ENTRY          *Entry;
+  UDP4_INSTANCE_DATA  *Instance;
+  UDP4_RXDATA_WRAP    *Wrap;
+  UINTN               Enqueued;
+
+  Enqueued = 0;
+
+  NET_LIST_FOR_EACH (Entry, &Udp4Service->ChildrenList) {
+    //
+    // Iterate the instances.
+    //
+    Instance = NET_LIST_USER_STRUCT (Entry, UDP4_INSTANCE_DATA, Link);
+
+    if (!Instance->Configured) {
+      continue;
+    }
+
+    if (Udp4MatchDgram (Instance, &RxData->UdpSession)) {
+      //
+      // Wrap the RxData and put this Wrap into the instances RcvdDgramQue.
+      //
+      Wrap = Udp4WrapRxData (Instance, Packet, RxData);
+      if (Wrap == NULL) {
+        continue;
+      }
+
+      NET_GET_REF (Packet);
+
+      InsertTailList (&Instance->RcvdDgramQue, &Wrap->Link);
+
+      Enqueued++;
+    }
+  }
+
+  return Enqueued;
+}
+
+
+/**
+  This function delivers the received datagrams for the specified instance.
+
+  @param[in]  Instance               Pointer to the instance context data.
+
+**/
+VOID
+Udp4InstanceDeliverDgram (
+  IN UDP4_INSTANCE_DATA  *Instance
+  )
+{
+  UDP4_RXDATA_WRAP           *Wrap;
+  EFI_UDP4_COMPLETION_TOKEN  *Token;
+  NET_BUF                    *Dup;
+  EFI_UDP4_RECEIVE_DATA      *RxData;
+  EFI_TPL                    OldTpl;
+
+  if (!IsListEmpty (&Instance->RcvdDgramQue) &&
+      !NetMapIsEmpty (&Instance->RxTokens)) {
+
+    Wrap = NET_LIST_HEAD (&Instance->RcvdDgramQue, UDP4_RXDATA_WRAP, Link);
+
+    if (NET_BUF_SHARED (Wrap->Packet)) {
+      //
+      // Duplicate the Packet if it is shared between instances.
+      //
+      Dup = NetbufDuplicate (Wrap->Packet, NULL, 0);
+      if (Dup == NULL) {
+        return;
+      }
+
+      NetbufFree (Wrap->Packet);
+
+      Wrap->Packet = Dup;
+    }
+
+    NetListRemoveHead (&Instance->RcvdDgramQue);
+
+    Token = (EFI_UDP4_COMPLETION_TOKEN *) NetMapRemoveHead (&Instance->RxTokens, NULL);
+
+    //
+    // Build the FragmentTable and set the FragmentCount in RxData.
+    //
+    RxData                = &Wrap->RxData;
+    RxData->FragmentCount = Wrap->Packet->BlockOpNum;
+
+    NetbufBuildExt (
+      Wrap->Packet,
+      (NET_FRAGMENT *) RxData->FragmentTable,
+      &RxData->FragmentCount
+      );
+
+    Token->Status        = EFI_SUCCESS;
+    Token->Packet.RxData = &Wrap->RxData;
+
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);
+    InsertTailList (&Instance->DeliveredDgramQue, &Wrap->Link);
+    gBS->RestoreTPL (OldTpl);
+
+    gBS->SignalEvent (Token->Event);
+  }
+}
+
+
+/**
+  This function delivers the datagrams enqueued in the instances.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+
+**/
+VOID
+Udp4DeliverDgram (
+  IN UDP4_SERVICE_DATA  *Udp4Service
+  )
+{
+  LIST_ENTRY          *Entry;
+  UDP4_INSTANCE_DATA  *Instance;
+
+  NET_LIST_FOR_EACH (Entry, &Udp4Service->ChildrenList) {
+    //
+    // Iterate the instances.
+    //
+    Instance = NET_LIST_USER_STRUCT (Entry, UDP4_INSTANCE_DATA, Link);
+
+    if (!Instance->Configured) {
+      continue;
+    }
+
+    //
+    // Deliver the datagrams of this instance.
+    //
+    Udp4InstanceDeliverDgram (Instance);
+  }
+}
+
+
+/**
+  This function demultiplexes the received udp datagram to the apropriate instances.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA abstrated from
+                                     the received datagram.
+  @param[in]  Packet                 Pointer to the buffer containing the received udp
+                                     datagram.
+
+**/
+VOID
+Udp4Demultiplex (
+  IN UDP4_SERVICE_DATA     *Udp4Service,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet
+  )
+{
+  EFI_UDP_HEADER        *Udp4Header;
+  UINT16                 HeadSum;
+  EFI_UDP4_RECEIVE_DATA  RxData;
+  EFI_UDP4_SESSION_DATA  *Udp4Session;
+  UINTN                  Enqueued;
+
+  //
+  // Get the datagram header from the packet buffer.
+  //
+  Udp4Header = (EFI_UDP_HEADER *) NetbufGetByte (Packet, 0, NULL);
+  ASSERT (Udp4Header != NULL);
+
+  if (Udp4Header->Checksum != 0) {
+    //
+    // check the checksum.
+    //
+    HeadSum = NetPseudoHeadChecksum (
+                NetSession->Source.Addr[0],
+                NetSession->Dest.Addr[0],
+                EFI_IP_PROTO_UDP,
+                0
+                );
+
+    if (Udp4Checksum (Packet, HeadSum) != 0) {
+      //
+      // Wrong checksum.
+      //
+      return;
+    }
+  }
+
+  Udp4Session                  = &RxData.UdpSession;
+  Udp4Session->SourcePort      = NTOHS (Udp4Header->SrcPort);
+  Udp4Session->DestinationPort = NTOHS (Udp4Header->DstPort);
+
+  CopyMem (&Udp4Session->SourceAddress, &NetSession->Source, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Udp4Session->DestinationAddress, &NetSession->Dest, sizeof (EFI_IPv4_ADDRESS));
+
+  //
+  // Trim the UDP header.
+  //
+  NetbufTrim (Packet, UDP4_HEADER_SIZE, TRUE);
+
+  RxData.DataLength = (UINT32) Packet->TotalSize;
+
+  //
+  // Try to enqueue this datagram into the instances.
+  //
+  Enqueued = Udp4EnqueueDgram (Udp4Service, Packet, &RxData);
+
+  if (Enqueued == 0) {
+    //
+    // Send the port unreachable ICMP packet before we free this NET_BUF
+    //
+    Udp4SendPortUnreach (Udp4Service->IpIo, NetSession, Udp4Header);
+  }
+
+  //
+  // Try to free the packet before deliver it.
+  //
+  NetbufFree (Packet);
+
+  if (Enqueued > 0) {
+    //
+    // Deliver the datagram.
+    //
+    Udp4DeliverDgram (Udp4Service);
+  }
+}
+
+
+/**
+  This function builds and sends out a icmp port unreachable message.
+
+  @param[in]  IpIo                   Pointer to the IP_IO instance.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA of the packet
+                                     causes this icmp error message.
+  @param[in]  Udp4Header             Pointer to the udp header of the datagram causes
+                                     this icmp error message.
+
+**/
+VOID
+Udp4SendPortUnreach (
+  IN IP_IO                 *IpIo,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN VOID                  *Udp4Header
+  )
+{
+  NET_BUF              *Packet;
+  UINT32               Len;
+  IP4_ICMP_ERROR_HEAD  *IcmpErrHdr;
+  EFI_IP4_HEADER       *IpHdr;
+  UINT8                *Ptr;
+  IP_IO_OVERRIDE       Override;
+  IP_IO_IP_INFO        *IpSender;
+
+  IpSender = IpIoFindSender (&IpIo, NetSession->IpVersion, &NetSession->Dest);
+  if (IpSender == NULL) {
+    //
+    // No apropriate sender, since we cannot send out the ICMP message through
+    // the default zero station address IP instance, abort.
+    //
+    return;
+  }
+
+  IpHdr = NetSession->IpHdr.Ip4Hdr;
+
+  //
+  // Calculate the requried length of the icmp error message.
+  //
+  Len = sizeof (IP4_ICMP_ERROR_HEAD) + (EFI_IP4_HEADER_LEN (IpHdr) -
+        sizeof (IP4_HEAD)) + ICMP_ERROR_PACKET_LENGTH;
+
+  //
+  // Allocate buffer for the icmp error message.
+  //
+  Packet = NetbufAlloc (Len);
+  if (Packet == NULL) {
+    return;
+  }
+
+  //
+  // Allocate space for the IP4_ICMP_ERROR_HEAD.
+  //
+  IcmpErrHdr = (IP4_ICMP_ERROR_HEAD *) NetbufAllocSpace (Packet, Len, FALSE);
+  ASSERT (IcmpErrHdr != NULL);
+
+  //
+  // Set the required fields for the icmp port unreachable message.
+  //
+  IcmpErrHdr->Head.Type     = ICMP_TYPE_UNREACH;
+  IcmpErrHdr->Head.Code     = ICMP_CODE_UNREACH_PORT;
+  IcmpErrHdr->Head.Checksum = 0;
+  IcmpErrHdr->Fourth        = 0;
+
+  //
+  // Copy the IP header of the datagram tragged the error.
+  //
+  CopyMem (&IcmpErrHdr->IpHead, IpHdr, EFI_IP4_HEADER_LEN (IpHdr));
+
+  //
+  // Copy the UDP header.
+  //
+  Ptr = (UINT8 *) &IcmpErrHdr->IpHead + EFI_IP4_HEADER_LEN (IpHdr);
+  CopyMem (Ptr, Udp4Header, ICMP_ERROR_PACKET_LENGTH);
+
+  //
+  // Calculate the checksum.
+  //
+  IcmpErrHdr->Head.Checksum = (UINT16) ~(NetbufChecksum (Packet));
+
+  //
+  // Fill the override data.
+  //
+  Override.Ip4OverrideData.DoNotFragment = FALSE;
+  Override.Ip4OverrideData.TypeOfService = 0;
+  Override.Ip4OverrideData.TimeToLive    = 255;
+  Override.Ip4OverrideData.Protocol      = EFI_IP_PROTO_ICMP;
+
+  CopyMem (&Override.Ip4OverrideData.SourceAddress, &NetSession->Dest, sizeof (EFI_IPv4_ADDRESS));
+  ZeroMem (&Override.Ip4OverrideData.GatewayAddress, sizeof (EFI_IPv4_ADDRESS));
+
+  //
+  // Send out this icmp packet.
+  //
+  IpIoSend (IpIo, Packet, IpSender, NULL, NULL, &NetSession->Source, &Override);
+
+  NetbufFree (Packet);
+}
+
+
+/**
+  This function handles the received Icmp Error message and demultiplexes it to the
+  instance.
+
+  @param[in]  Udp4Service            Pointer to the udp service context data.
+  @param[in]  IcmpError              The icmp error code.
+  @param[in]  NetSession             Pointer to the EFI_NET_SESSION_DATA abstracted
+                                 from the received Icmp Error packet.
+  @param[in]  Packet                 Pointer to the Icmp Error packet.
+
+**/
+VOID
+Udp4IcmpHandler (
+  IN UDP4_SERVICE_DATA     *Udp4Service,
+  IN UINT8                 IcmpError,
+  IN EFI_NET_SESSION_DATA  *NetSession,
+  IN NET_BUF               *Packet
+  )
+{
+  EFI_UDP_HEADER        *Udp4Header;
+  EFI_UDP4_SESSION_DATA  Udp4Session;
+  LIST_ENTRY             *Entry;
+  UDP4_INSTANCE_DATA     *Instance;
+
+  Udp4Header = (EFI_UDP_HEADER *) NetbufGetByte (Packet, 0, NULL);
+  ASSERT (Udp4Header != NULL);
+
+  CopyMem (&Udp4Session.SourceAddress, &NetSession->Source, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Udp4Session.DestinationAddress, &NetSession->Dest, sizeof (EFI_IPv4_ADDRESS));
+
+  Udp4Session.SourcePort      = NTOHS (Udp4Header->DstPort);
+  Udp4Session.DestinationPort = NTOHS (Udp4Header->SrcPort);
+
+  NET_LIST_FOR_EACH (Entry, &Udp4Service->ChildrenList) {
+    //
+    // Iterate all the instances.
+    //
+    Instance = NET_LIST_USER_STRUCT (Entry, UDP4_INSTANCE_DATA, Link);
+
+    if (!Instance->Configured) {
+      continue;
+    }
+
+    if (Udp4MatchDgram (Instance, &Udp4Session)) {
+      //
+      // Translate the Icmp Error code according to the udp spec.
+      //
+      Instance->IcmpError = IpIoGetIcmpErrStatus (IcmpError, IP_VERSION_4, NULL, NULL);
+
+      if (IcmpError > ICMP_ERR_UNREACH_PORT) {
+        Instance->IcmpError = EFI_ICMP_ERROR;
+      }
+
+      //
+      // Notify the instance with the received Icmp Error.
+      //
+      Udp4ReportIcmpError (Instance);
+
+      break;
+    }
+  }
+
+  NetbufFree (Packet);
+}
+
+
+/**
+  This function reports the received ICMP error.
+
+  @param[in]  Instance               Pointer to the udp instance context data.
+
+**/
+VOID
+Udp4ReportIcmpError (
+  IN UDP4_INSTANCE_DATA  *Instance
+  )
+{
+  EFI_UDP4_COMPLETION_TOKEN  *Token;
+
+  if (NetMapIsEmpty (&Instance->RxTokens)) {
+    //
+    // There are no receive tokens to deliver the ICMP error.
+    //
+    return;
+  }
+
+  if (EFI_ERROR (Instance->IcmpError)) {
+    //
+    // Try to get a RxToken from the RxTokens map.
+    //
+    Token = (EFI_UDP4_COMPLETION_TOKEN *) NetMapRemoveHead (&Instance->RxTokens, NULL);
+
+    if (Token != NULL) {
+      //
+      // Report the error through the Token.
+      //
+      Token->Status = Instance->IcmpError;
+      gBS->SignalEvent (Token->Event);
+
+      //
+      // Clear the IcmpError.
+      //
+      Instance->IcmpError = EFI_SUCCESS;
+    }
+  }
+}
+
+
+/**
+  This function is a dummy ext-free function for the NET_BUF created for the output
+  udp datagram.
+
+  @param[in]  Context                Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4NetVectorExtFree (
+  VOID  *Context
+  )
+{
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.h b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.h
new file mode 100644
index 0000000000..31edfbda43
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Impl.h
@@ -0,0 +1,695 @@
+/** @file
+  EFI UDPv4 protocol implementation.
+   
+Copyright (c) 2006 - 2014, 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.
+
+**/
+
+#ifndef _UDP4_IMPL_H_
+#define _UDP4_IMPL_H_
+
+#include <Uefi.h>
+
+#include <Protocol/Ip4.h>
+#include <Protocol/Udp4.h>
+
+#include <Library/IpIoLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/TimerLib.h>
+#include <Library/DpcLib.h>
+#include <Library/PrintLib.h>
+
+#include "Udp4Driver.h"
+
+
+extern EFI_COMPONENT_NAME_PROTOCOL     gUdp4ComponentName;
+extern EFI_COMPONENT_NAME2_PROTOCOL    gUdp4ComponentName2;
+extern EFI_UNICODE_STRING_TABLE        *gUdpControllerNameTable;
+extern EFI_SERVICE_BINDING_PROTOCOL    mUdp4ServiceBinding;
+extern EFI_UDP4_PROTOCOL               mUdp4Protocol;
+extern UINT16                          mUdp4RandomPort;
+
+#define ICMP_ERROR_PACKET_LENGTH  8
+
+#define UDP4_TIMEOUT_INTERVAL (50 * TICKS_PER_MS)  // 50 milliseconds
+
+#define UDP4_HEADER_SIZE      sizeof (EFI_UDP_HEADER)
+#define UDP4_MAX_DATA_SIZE    65507
+
+#define UDP4_PORT_KNOWN       1024
+
+#define UDP4_SERVICE_DATA_SIGNATURE  SIGNATURE_32('U', 'd', 'p', '4')
+
+#define UDP4_SERVICE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  UDP4_SERVICE_DATA, \
+  ServiceBinding, \
+  UDP4_SERVICE_DATA_SIGNATURE \
+  )
+
+typedef struct _UDP4_SERVICE_DATA_ {
+  UINT32                        Signature;
+  EFI_SERVICE_BINDING_PROTOCOL  ServiceBinding;
+  EFI_HANDLE                    ImageHandle;
+  EFI_HANDLE                    ControllerHandle;
+  LIST_ENTRY                    ChildrenList;
+  UINTN                         ChildrenNumber;
+  IP_IO                         *IpIo;
+
+  EFI_EVENT                     TimeoutEvent;
+} UDP4_SERVICE_DATA;
+
+#define UDP4_INSTANCE_DATA_SIGNATURE  SIGNATURE_32('U', 'd', 'p', 'I')
+
+#define UDP4_INSTANCE_DATA_FROM_THIS(a) \
+  CR ( \
+  (a), \
+  UDP4_INSTANCE_DATA, \
+  Udp4Proto, \
+  UDP4_INSTANCE_DATA_SIGNATURE \
+  )
+
+typedef struct _UDP4_INSTANCE_DATA_ {
+  UINT32                Signature;
+  LIST_ENTRY            Link;
+
+  UDP4_SERVICE_DATA     *Udp4Service;
+  EFI_UDP4_PROTOCOL     Udp4Proto;
+  EFI_UDP4_CONFIG_DATA  ConfigData;
+  EFI_HANDLE            ChildHandle;
+  BOOLEAN               Configured;
+  BOOLEAN               IsNoMapping;
+
+  NET_MAP               TxTokens;
+  NET_MAP               RxTokens;
+
+  NET_MAP               McastIps;
+
+  LIST_ENTRY            RcvdDgramQue;
+  LIST_ENTRY            DeliveredDgramQue;
+
+  UINT16                HeadSum;
+
+  EFI_STATUS            IcmpError;
+
+  IP_IO_IP_INFO         *IpInfo;
+
+  BOOLEAN               InDestroy;
+} UDP4_INSTANCE_DATA;
+
+typedef struct _UDP4_RXDATA_WRAP_ {
+  LIST_ENTRY             Link;
+  NET_BUF                *Packet;
+  UINT32                 TimeoutTick;
+  EFI_UDP4_RECEIVE_DATA  RxData;
+} UDP4_RXDATA_WRAP;
+
+typedef struct {
+  EFI_SERVICE_BINDING_PROTOCOL  *ServiceBinding;
+  UINTN                         NumberOfChildren;
+  EFI_HANDLE                    *ChildHandleBuffer;
+} UDP4_DESTROY_CHILD_IN_HANDLE_BUF_CONTEXT;
+
+/**
+  Reads the current operational settings.
+
+  The GetModeData() function copies the current operational settings of this EFI
+  UDPv4 Protocol instance into user-supplied buffers. This function is used
+  optionally to retrieve the operational mode data of underlying networks or
+  drivers.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[out] Udp4ConfigData    Pointer to the buffer to receive the current configuration data.
+  @param[out] Ip4ModeData       Pointer to the EFI IPv4 Protocol mode data structure.
+  @param[out] MnpConfigData     Pointer to the managed network configuration data structure.
+  @param[out] SnpModeData       Pointer to the simple network mode data structure.
+
+  @retval EFI_SUCCESS           The mode data was read.
+  @retval EFI_NOT_STARTED       When Udp4ConfigData is queried, no configuration data is
+                                available because this instance has not been started.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4GetModeData (
+  IN  EFI_UDP4_PROTOCOL                *This,
+  OUT EFI_UDP4_CONFIG_DATA             *Udp4ConfigData OPTIONAL,
+  OUT EFI_IP4_MODE_DATA                *Ip4ModeData    OPTIONAL,
+  OUT EFI_MANAGED_NETWORK_CONFIG_DATA  *MnpConfigData  OPTIONAL,
+  OUT EFI_SIMPLE_NETWORK_MODE          *SnpModeData    OPTIONAL
+  );
+
+/**
+  Initializes, changes, or resets the operational parameters for this instance of the EFI UDPv4
+  Protocol.
+  
+  The Configure() function is used to do the following:
+  * Initialize and start this instance of the EFI UDPv4 Protocol.
+  * Change the filtering rules and operational parameters.
+  * Reset this instance of the EFI UDPv4 Protocol.
+  Until these parameters are initialized, no network traffic can be sent or
+  received by this instance. This instance can be also reset by calling Configure()
+  with UdpConfigData set to NULL. Once reset, the receiving queue and transmitting
+  queue are flushed and no traffic is allowed through this instance.
+  With different parameters in UdpConfigData, Configure() can be used to bind
+  this instance to specified port.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  UdpConfigData     Pointer to the buffer to receive the current configuration data.
+
+  @retval EFI_SUCCESS           The configuration settings were set, changed, or reset successfully.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more following conditions are TRUE:
+  @retval EFI_ALREADY_STARTED   The EFI UDPv4 Protocol instance is already started/configured
+                                and must be stopped/reset before it can be reconfigured.
+  @retval EFI_ACCESS_DENIED     UdpConfigData. AllowDuplicatePort is FALSE
+                                and UdpConfigData.StationPort is already used by
+                                other instance.
+  @retval EFI_OUT_OF_RESOURCES  The EFI UDPv4 Protocol driver cannot allocate memory for this
+                                EFI UDPv4 Protocol instance.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred and this instance
+                                 was not opened. 
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Configure (
+  IN EFI_UDP4_PROTOCOL     *This,
+  IN EFI_UDP4_CONFIG_DATA  *UdpConfigData OPTIONAL
+  );
+
+/**
+  Joins and leaves multicast groups.
+  
+  The Groups() function is used to enable and disable the multicast group
+  filtering. If the JoinFlag is FALSE and the MulticastAddress is NULL, then all
+  currently joined groups are left.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  JoinFlag          Set to TRUE to join a multicast group. Set to FALSE to leave one
+                                or all multicast groups.
+  @param[in]  MulticastAddress  Pointer to multicast group address to join or leave.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_NOT_STARTED       The EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate resources to join the group.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                - This is NULL.
+                                - JoinFlag is TRUE and MulticastAddress is NULL.
+                                - JoinFlag is TRUE and *MulticastAddress is not
+                                  a valid multicast address.
+  @retval EFI_ALREADY_STARTED   The group address is already in the group table (when
+                                JoinFlag is TRUE).
+  @retval EFI_NOT_FOUND         The group address is not in the group table (when JoinFlag is
+                                FALSE).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Groups (
+  IN EFI_UDP4_PROTOCOL  *This,
+  IN BOOLEAN            JoinFlag,
+  IN EFI_IPv4_ADDRESS   *MulticastAddress OPTIONAL
+  );
+
+/**
+  Adds and deletes routing table entries.
+  
+  The Routes() function adds a route to or deletes a route from the routing table.
+  Routes are determined by comparing the SubnetAddress with the destination IP
+  address and arithmetically AND-ing it with the SubnetMask. The gateway address
+  must be on the same subnet as the configured station address.
+  The default route is added with SubnetAddress and SubnetMask both set to 0.0.0.0.
+  The default route matches all destination IP addresses that do not match any
+  other routes.
+  A zero GatewayAddress is a nonroute. Packets are sent to the destination IP
+  address if it can be found in the Address Resolution Protocol (ARP) cache or
+  on the local subnet. One automatic nonroute entry will be inserted into the
+  routing table for outgoing packets that are addressed to a local subnet
+  (gateway address of 0.0.0.0).
+  Each instance of the EFI UDPv4 Protocol has its own independent routing table.
+  Instances of the EFI UDPv4 Protocol that use the default IP address will also
+  have copies of the routing table provided by the EFI_IP4_CONFIG_PROTOCOL. These
+  copies will be updated automatically whenever the IP driver reconfigures its
+  instances; as a result, the previous modification to these copies will be lost.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  DeleteRoute       Set to TRUE to delete this route from the routing table.
+                                Set to FALSE to add this route to the routing table.
+  @param[in]  SubnetAddress     The destination network address that needs to be routed.
+  @param[in]  SubnetMask        The subnet mask of SubnetAddress.
+  @param[in]  GatewayAddress    The gateway IP address for this route.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_NOT_STARTED       The EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                - RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  Could not add the entry to the routing table.
+  @retval EFI_NOT_FOUND         This route is not in the routing table.
+  @retval EFI_ACCESS_DENIED     The route is already defined in the routing table.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Routes (
+  IN EFI_UDP4_PROTOCOL  *This,
+  IN BOOLEAN            DeleteRoute,
+  IN EFI_IPv4_ADDRESS   *SubnetAddress,
+  IN EFI_IPv4_ADDRESS   *SubnetMask,
+  IN EFI_IPv4_ADDRESS   *GatewayAddress
+  );
+
+/**
+  Queues outgoing data packets into the transmit queue.
+  
+  The Transmit() function places a sending request to this instance of the EFI
+  UDPv4 Protocol, alongside the transmit data that was filled by the user. Whenever
+  the packet in the token is sent out or some errors occur, the Token.Event will
+  be signaled and Token.Status is updated. Providing a proper notification function
+  and context for the event will enable the user to receive the notification and
+  transmitting status.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token             Pointer to the completion token that will be placed into the
+                                transmit queue.
+
+  @retval EFI_SUCCESS           The data has been queued for transmission.
+  @retval EFI_NOT_STARTED       This EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED     The transmit completion token with the same
+                                Token.Event was already in the transmit queue.
+  @retval EFI_NOT_READY         The completion token could not be queued because the
+                                transmit queue is full.
+  @retval EFI_OUT_OF_RESOURCES  Could not queue the transmit data.
+  @retval EFI_NOT_FOUND         There is no route to the destination network or address.
+  @retval EFI_BAD_BUFFER_SIZE   The data length is greater than the maximum UDP packet
+                                size. Or the length of the IP header + UDP header + data
+                                length is greater than MTU if DoNotFragment is TRUE.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Transmit (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token
+  );
+
+/**
+  Places an asynchronous receive request into the receiving queue.
+  
+  The Receive() function places a completion token into the receive packet queue.
+  This function is always asynchronous.
+  The caller must fill in the Token.Event field in the completion token, and this
+  field cannot be NULL. When the receive operation completes, the EFI UDPv4 Protocol
+  driver updates the Token.Status and Token.Packet.RxData fields and the Token.Event
+  is signaled. Providing a proper notification function and context for the event
+  will enable the user to receive the notification and receiving status. That
+  notification function is guaranteed to not be re-entered.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token             Pointer to a token that is associated with
+                                the receive data descriptor.
+
+  @retval EFI_SUCCESS           The receive completion token was cached.
+  @retval EFI_NOT_STARTED       This EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP, RARP, etc.)
+                                is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+  @retval EFI_OUT_OF_RESOURCES  The receive completion token could not be queued due to a lack of system
+                                resources (usually memory).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval EFI_ACCESS_DENIED     A receive completion token with the same Token.Event was already in
+                                the receive queue.
+  @retval EFI_NOT_READY         The receive request could not be queued because the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Receive (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token
+  );
+
+/**
+  Aborts an asynchronous transmit or receive request.
+  
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token.Status will be set to EFI_ABORTED and then Token.Event will be
+  signaled. If the token is not in one of the queues, which usually means that
+  the asynchronous operation has completed, this function will not signal the
+  token and EFI_NOT_FOUND is returned.
+
+  @param[in]  This  Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that has been issued by
+                    EFI_UDP4_PROTOCOL.Transmit() or
+                    EFI_UDP4_PROTOCOL.Receive().If NULL, all pending
+                    tokens are aborted.
+
+  @retval  EFI_SUCCESS           The asynchronous I/O request was aborted and Token.Event
+                                 was signaled. When Token is NULL, all pending requests are
+                                 aborted and their events are signaled.
+  @retval  EFI_INVALID_PARAMETER This is NULL.
+  @retval  EFI_NOT_STARTED       This instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_NOT_FOUND         When Token is not NULL, the asynchronous I/O request was
+                                 not found in the transmit or receive queue. It has either completed
+                                 or was not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Cancel (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token OPTIONAL
+  );
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+  
+  The Poll() function can be used by network drivers and applications to increase
+  the rate that data packets are moved between the communications device and the
+  transmit and receive queues.
+  In some systems, the periodic timer event in the managed network driver may not
+  poll the underlying communications device fast enough to transmit and/or receive
+  all data packets without missing incoming packets or dropping outgoing packets.
+  Drivers and applications that are experiencing packet loss should try calling
+  the Poll() function more often.
+
+  @param[in]  This  Pointer to the EFI_UDP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           Incoming or outgoing data was processed.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval EFI_TIMEOUT           Data was dropped out of the transmit and/or receive queue.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Poll (
+  IN EFI_UDP4_PROTOCOL  *This
+  );
+
+/**
+  Create the Udp service context data.
+
+  @param[in, out] Udp4Service      Pointer to the UDP4_SERVICE_DATA.
+  @param[in]      ImageHandle      The image handle of this udp4 driver.
+  @param[in]      ControllerHandle The controller handle this udp4 driver binds on.
+
+  @retval EFI_SUCCESS              The udp4 service context data is created and
+                                   initialized.
+  @retval EFI_OUT_OF_RESOURCES     Cannot allocate memory.
+  @retval other                    Other error occurs.
+
+**/
+EFI_STATUS
+Udp4CreateService (
+  IN OUT UDP4_SERVICE_DATA  *Udp4Service,
+  IN     EFI_HANDLE         ImageHandle,
+  IN     EFI_HANDLE         ControllerHandle
+  );
+
+/**
+  Clean the Udp service context data.
+
+  @param[in]  Udp4Service            Pointer to the UDP4_SERVICE_DATA.
+
+**/
+VOID
+Udp4CleanService (
+  IN UDP4_SERVICE_DATA  *Udp4Service
+  );
+
+/**
+  This function intializes the new created udp instance.
+
+  @param[in]      Udp4Service       Pointer to the UDP4_SERVICE_DATA.
+  @param[in, out] Instance          Pointer to the un-initialized UDP4_INSTANCE_DATA.
+
+**/
+VOID
+Udp4InitInstance (
+  IN     UDP4_SERVICE_DATA   *Udp4Service,
+  IN OUT UDP4_INSTANCE_DATA  *Instance
+  );
+
+/**
+  This function cleans the udp instance.
+
+  @param[in]  Instance               Pointer to the UDP4_INSTANCE_DATA to clean.
+
+**/
+VOID
+Udp4CleanInstance (
+  IN UDP4_INSTANCE_DATA  *Instance
+  );
+
+/**
+  This function tries to bind the udp instance according to the configured port
+  allocation strategy.
+
+  @param[in]      InstanceList   Pointer to the head of the list linking the udp
+                                 instances.
+  @param[in, out] ConfigData     Pointer to the ConfigData of the instance to be
+                                 bound. ConfigData->StationPort will be assigned
+                                 with an available port value on success.
+
+  @retval EFI_SUCCESS            The bound operation is completed successfully.
+  @retval EFI_ACCESS_DENIED      The <Address, Port> specified by the ConfigData is
+                                 already used by other instance.
+  @retval EFI_OUT_OF_RESOURCES   No available port resources.
+
+**/
+EFI_STATUS
+Udp4Bind (
+  IN     LIST_ENTRY            *InstanceList,
+  IN OUT EFI_UDP4_CONFIG_DATA  *ConfigData
+  );
+
+/**
+  This function is used to check whether the NewConfigData has any un-reconfigurable
+  parameters changed compared to the OldConfigData.
+
+  @param[in]  OldConfigData      Pointer to the current ConfigData the udp instance
+                                 uses.
+  @param[in]  NewConfigData      Pointer to the new ConfigData.
+
+  @retval TRUE     The instance is reconfigurable.
+  @retval FALSE    Otherwise.
+
+**/
+BOOLEAN
+Udp4IsReconfigurable (
+  IN EFI_UDP4_CONFIG_DATA  *OldConfigData,
+  IN EFI_UDP4_CONFIG_DATA  *NewConfigData
+  );
+
+/**
+  This function builds the Ip4 configdata from the Udp4ConfigData.
+
+  @param[in]       Udp4ConfigData    Pointer to the EFI_UDP4_CONFIG_DATA.
+  @param[in, out]  Ip4ConfigData     Pointer to the EFI_IP4_CONFIG_DATA.
+
+**/
+VOID
+Udp4BuildIp4ConfigData (
+  IN     EFI_UDP4_CONFIG_DATA  *Udp4ConfigData,
+  IN OUT EFI_IP4_CONFIG_DATA   *Ip4ConfigData
+  );
+
+/**
+  This function validates the TxToken, it returns the error code according to the spec.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  TxToken            Pointer to the token to be checked.
+
+  @retval EFI_SUCCESS            The TxToken is valid.
+  @retval EFI_INVALID_PARAMETER  One or more of the following are TRUE: This is
+                                 NULL. Token is NULL. Token.Event is NULL.
+                                 Token.Packet.TxData is NULL.
+                                 Token.Packet.TxData.FragmentCount is zero.
+                                 Token.Packet.TxData.DataLength is not equal to the
+                                 sum of fragment lengths. One or more of the
+                                 Token.Packet.TxData.FragmentTable[].
+                                 FragmentLength fields is zero. One or more of the
+                                 Token.Packet.TxData.FragmentTable[].
+                                 FragmentBuffer fields is NULL.
+                                 Token.Packet.TxData. GatewayAddress is not a
+                                 unicast IPv4 address if it is not NULL. One or
+                                 more IPv4 addresses in Token.Packet.TxData.
+                                 UdpSessionData are not valid unicast IPv4
+                                 addresses if the UdpSessionData is not NULL.
+  @retval EFI_BAD_BUFFER_SIZE    The data length is greater than the maximum UDP
+                                 packet size.
+
+**/
+EFI_STATUS
+Udp4ValidateTxToken (
+  IN UDP4_INSTANCE_DATA         *Instance,
+  IN EFI_UDP4_COMPLETION_TOKEN  *TxToken
+  );
+
+/**
+  This function checks whether the specified Token duplicates with the one in the Map.
+
+  @param[in]  Map                Pointer to the NET_MAP.
+  @param[in]  Item               Pointer to the NET_MAP_ITEM contain the pointer to
+                                 the Token.
+  @param[in]  Context            Pointer to the Token to be checked.
+
+  @retval EFI_SUCCESS            The Token specified by Context differs from the
+                                 one in the Item.
+  @retval EFI_ACCESS_DENIED      The Token duplicates with the one in the Item.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4TokenExist (
+  IN NET_MAP       *Map,
+  IN NET_MAP_ITEM  *Item,
+  IN VOID          *Context
+  );
+
+/**
+  This function calculates the checksum for the Packet, utilizing the pre-calculated
+  pseudo HeadSum to reduce some overhead.
+
+  @param[in]  Packet             Pointer to the NET_BUF contains the udp datagram.
+  @param[in]  HeadSum            Checksum of the pseudo header execpt the length
+                                 field.
+
+  @retval The 16-bit checksum of this udp datagram.
+
+**/
+UINT16
+Udp4Checksum (
+  IN NET_BUF *Packet,
+  IN UINT16  HeadSum
+  );
+
+/**
+  This function removes the specified Token from the TokenMap.
+
+  @param[in, out] TokenMap       Pointer to the NET_MAP containing the tokens.
+  @param[in]      Token          Pointer to the Token to be removed.
+
+  @retval EFI_SUCCESS            The specified Token is removed from the TokenMap.
+  @retval EFI_NOT_FOUND          The specified Token is not found in the TokenMap.
+
+**/
+EFI_STATUS
+Udp4RemoveToken (
+  IN OUT NET_MAP                    *TokenMap,
+  IN     EFI_UDP4_COMPLETION_TOKEN  *Token
+  );
+
+/**
+  This function removes the multicast group specified by Arg from the Map.
+
+  @param[in, out] Map            Pointer to the NET_MAP.
+  @param[in]      Item           Pointer to the NET_MAP_ITEM.
+  @param[in]      Arg            Pointer to the Arg, it's the pointer to a
+                                 multicast IPv4 Address.
+
+  @retval EFI_SUCCESS            The multicast address is removed.
+  @retval EFI_ABORTED            The specified multicast address is removed and the
+                                 Arg is not NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4LeaveGroup (
+  IN OUT NET_MAP       *Map,
+  IN     NET_MAP_ITEM  *Item,
+  IN     VOID          *Arg OPTIONAL
+  );
+
+/**
+  This function removes all the Wrap datas in the RcvdDgramQue.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+
+**/
+VOID
+Udp4FlushRcvdDgram (
+  IN UDP4_INSTANCE_DATA  *Instance
+  );
+
+/**
+  Cancel Udp4 tokens from the Udp4 instance.
+
+  @param[in]  Instance           Pointer to the udp instance context data.
+  @param[in]  Token              Pointer to the token to be canceled, if NULL, all
+                                 tokens in this instance will be cancelled.
+
+  @retval EFI_SUCCESS            The Token is cancelled.
+  @retval EFI_NOT_FOUND          The Token is not found.
+
+**/
+EFI_STATUS
+Udp4InstanceCancelToken (
+  IN UDP4_INSTANCE_DATA         *Instance,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token OPTIONAL
+  );
+
+/**
+  This function delivers the received datagrams for the specified instance.
+
+  @param[in]  Instance               Pointer to the instance context data.
+
+**/
+VOID
+Udp4InstanceDeliverDgram (
+  IN UDP4_INSTANCE_DATA  *Instance
+  );
+
+/**
+  This function reports the received ICMP error.
+
+  @param[in]  Instance               Pointer to the udp instance context data.
+
+**/
+VOID
+Udp4ReportIcmpError (
+  IN UDP4_INSTANCE_DATA  *Instance
+  );
+
+/**
+  This function is a dummy ext-free function for the NET_BUF created for the output
+  udp datagram.
+
+  @param[in]  Context                Pointer to the context data.
+
+**/
+VOID
+EFIAPI
+Udp4NetVectorExtFree (
+  VOID  *Context
+  );
+ 
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Main.c b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Main.c
new file mode 100644
index 0000000000..5ea22acb14
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/Udp4Dxe/Udp4Main.c
@@ -0,0 +1,908 @@
+/** @file
+
+(C) Copyright 2014 Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2014, 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 "Udp4Impl.h"
+
+EFI_UDP4_PROTOCOL  mUdp4Protocol = {
+  Udp4GetModeData,
+  Udp4Configure,
+  Udp4Groups,
+  Udp4Routes,
+  Udp4Transmit,
+  Udp4Receive,
+  Udp4Cancel,
+  Udp4Poll
+};
+
+
+/**
+  Reads the current operational settings.
+
+  The GetModeData() function copies the current operational settings of this EFI
+  UDPv4 Protocol instance into user-supplied buffers. This function is used
+  optionally to retrieve the operational mode data of underlying networks or
+  drivers.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[out] Udp4ConfigData    Pointer to the buffer to receive the current configuration data.
+  @param[out] Ip4ModeData       Pointer to the EFI IPv4 Protocol mode data structure.
+  @param[out] MnpConfigData     Pointer to the managed network configuration data structure.
+  @param[out] SnpModeData       Pointer to the simple network mode data structure.
+
+  @retval EFI_SUCCESS           The mode data was read.
+  @retval EFI_NOT_STARTED       When Udp4ConfigData is queried, no configuration data is
+                                available because this instance has not been started.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4GetModeData (
+  IN  EFI_UDP4_PROTOCOL                *This,
+  OUT EFI_UDP4_CONFIG_DATA             *Udp4ConfigData OPTIONAL,
+  OUT EFI_IP4_MODE_DATA                *Ip4ModeData    OPTIONAL,
+  OUT EFI_MANAGED_NETWORK_CONFIG_DATA  *MnpConfigData  OPTIONAL,
+  OUT EFI_SIMPLE_NETWORK_MODE          *SnpModeData    OPTIONAL
+  )
+{
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_IP4_PROTOCOL    *Ip;
+  EFI_TPL             OldTpl;
+  EFI_STATUS          Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured && (Udp4ConfigData != NULL)) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (Udp4ConfigData != NULL) {
+    //
+    // Set the Udp4ConfigData.
+    //
+    CopyMem (Udp4ConfigData, &Instance->ConfigData, sizeof (*Udp4ConfigData));
+  }
+
+  Ip = Instance->IpInfo->Ip.Ip4;
+
+  //
+  // Get the underlying Ip4ModeData, MnpConfigData and SnpModeData.
+  //
+  Status = Ip->GetModeData (Ip, Ip4ModeData, MnpConfigData, SnpModeData);
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Initializes, changes, or resets the operational parameters for this instance of the EFI UDPv4
+  Protocol.
+  
+  The Configure() function is used to do the following:
+  * Initialize and start this instance of the EFI UDPv4 Protocol.
+  * Change the filtering rules and operational parameters.
+  * Reset this instance of the EFI UDPv4 Protocol.
+  Until these parameters are initialized, no network traffic can be sent or
+  received by this instance. This instance can be also reset by calling Configure()
+  with UdpConfigData set to NULL. Once reset, the receiving queue and transmitting
+  queue are flushed and no traffic is allowed through this instance.
+  With different parameters in UdpConfigData, Configure() can be used to bind
+  this instance to specified port.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  UdpConfigData     Pointer to the buffer to receive the current configuration data.
+
+  @retval EFI_SUCCESS           The configuration settings were set, changed, or reset successfully.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more following conditions are TRUE:
+  @retval EFI_ALREADY_STARTED   The EFI UDPv4 Protocol instance is already started/configured
+                                and must be stopped/reset before it can be reconfigured.
+  @retval EFI_ACCESS_DENIED     UdpConfigData. AllowDuplicatePort is FALSE
+                                and UdpConfigData.StationPort is already used by
+                                other instance.
+  @retval EFI_OUT_OF_RESOURCES  The EFI UDPv4 Protocol driver cannot allocate memory for this
+                                EFI UDPv4 Protocol instance.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred and this instance
+                                 was not opened. 
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Configure (
+  IN EFI_UDP4_PROTOCOL     *This,
+  IN EFI_UDP4_CONFIG_DATA  *UdpConfigData OPTIONAL
+  )
+{
+  EFI_STATUS           Status;
+  UDP4_INSTANCE_DATA   *Instance;
+  UDP4_SERVICE_DATA    *Udp4Service;
+  EFI_TPL              OldTpl;
+  IP4_ADDR             StationAddress;
+  IP4_ADDR             SubnetMask;
+  IP4_ADDR             RemoteAddress;
+  EFI_IP4_CONFIG_DATA  Ip4ConfigData;
+  IP4_ADDR             LocalAddr;
+  IP4_ADDR             RemoteAddr;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (!Instance->Configured && (UdpConfigData == NULL)) {
+    return EFI_SUCCESS;
+  }
+
+  Udp4Service = Instance->Udp4Service;
+  Status      = EFI_SUCCESS;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (UdpConfigData != NULL) {
+
+    CopyMem (&StationAddress, &UdpConfigData->StationAddress, sizeof (IP4_ADDR));
+    CopyMem (&SubnetMask, &UdpConfigData->SubnetMask, sizeof (IP4_ADDR));
+    CopyMem (&RemoteAddress, &UdpConfigData->RemoteAddress, sizeof (IP4_ADDR));
+
+    StationAddress = NTOHL (StationAddress);
+    SubnetMask     = NTOHL (SubnetMask);
+    RemoteAddress  = NTOHL (RemoteAddress);
+
+
+    if (!UdpConfigData->UseDefaultAddress &&
+      (!IP4_IS_VALID_NETMASK (SubnetMask) ||
+      !((StationAddress == 0) || NetIp4IsUnicast (StationAddress, SubnetMask)) ||
+      !((RemoteAddress  == 0) || NetIp4IsUnicast (RemoteAddress, 0)))) {
+      //
+      // Don't use default address, and subnet mask is invalid or StationAddress is not
+      // a valid unicast IPv4 address or RemoteAddress is not a valid unicast IPv4 address
+      // if it is not 0.
+      //
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+
+    if (Instance->Configured) {
+      //
+      // The instance is already configured, try to do the re-configuration.
+      //
+      if (!Udp4IsReconfigurable (&Instance->ConfigData, UdpConfigData)) {
+        //
+        // If the new configuration data wants to change some unreconfigurable
+        // settings, return EFI_ALREADY_STARTED.
+        //
+        Status = EFI_ALREADY_STARTED;
+        goto ON_EXIT;
+      }
+
+      //
+      // Save the reconfigurable parameters.
+      //
+      Instance->ConfigData.TypeOfService   = UdpConfigData->TypeOfService;
+      Instance->ConfigData.TimeToLive      = UdpConfigData->TimeToLive;
+      Instance->ConfigData.DoNotFragment   = UdpConfigData->DoNotFragment;
+      Instance->ConfigData.ReceiveTimeout  = UdpConfigData->ReceiveTimeout;
+      Instance->ConfigData.TransmitTimeout = UdpConfigData->TransmitTimeout;
+    } else {
+      //
+      // Construct the Ip configuration data from the UdpConfigData.
+      //
+      Udp4BuildIp4ConfigData (UdpConfigData, &Ip4ConfigData);
+
+      //
+      // Configure the Ip instance wrapped in the IpInfo.
+      //
+      Status = IpIoConfigIp (Instance->IpInfo, &Ip4ConfigData);
+      if (EFI_ERROR (Status)) {
+        if (Status == EFI_NO_MAPPING) {
+          Instance->IsNoMapping = TRUE;
+        }
+
+        goto ON_EXIT;
+      }
+
+      Instance->IsNoMapping = FALSE;
+
+      //
+      // Save the configuration data.
+      //
+      CopyMem (&Instance->ConfigData, UdpConfigData, sizeof (Instance->ConfigData));
+      IP4_COPY_ADDRESS (&Instance->ConfigData.StationAddress, &Ip4ConfigData.StationAddress);
+      IP4_COPY_ADDRESS (&Instance->ConfigData.SubnetMask, &Ip4ConfigData.SubnetMask);
+
+      //
+      // Try to allocate the required port resource.
+      //
+      Status = Udp4Bind (&Udp4Service->ChildrenList, &Instance->ConfigData);
+      if (EFI_ERROR (Status)) {
+        //
+        // Reset the ip instance if bind fails.
+        //
+        IpIoConfigIp (Instance->IpInfo, NULL);
+        goto ON_EXIT;
+      }
+
+      //
+      // Pre calculate the checksum for the pseudo head, ignore the UDP length first.
+      //
+      CopyMem (&LocalAddr, &Instance->ConfigData.StationAddress, sizeof (IP4_ADDR));
+      CopyMem (&RemoteAddr, &Instance->ConfigData.RemoteAddress, sizeof (IP4_ADDR));
+      Instance->HeadSum = NetPseudoHeadChecksum (
+                            LocalAddr,
+                            RemoteAddr,
+                            EFI_IP_PROTO_UDP,
+                            0
+                            );
+
+      Instance->Configured = TRUE;
+    }
+  } else {
+    //
+    // UdpConfigData is NULL, reset the instance.
+    //
+    Instance->Configured  = FALSE;
+    Instance->IsNoMapping = FALSE;
+
+    //
+    // Reset the Ip instance wrapped in the IpInfo.
+    //
+    IpIoConfigIp (Instance->IpInfo, NULL);
+
+    //
+    // Cancel all the user tokens.
+    //
+    Instance->Udp4Proto.Cancel (&Instance->Udp4Proto, NULL);
+
+    //
+    // Remove the buffered RxData for this instance.
+    //
+    Udp4FlushRcvdDgram (Instance);
+
+    ASSERT (IsListEmpty (&Instance->DeliveredDgramQue));
+  }
+ 
+ON_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Joins and leaves multicast groups.
+  
+  The Groups() function is used to enable and disable the multicast group
+  filtering. If the JoinFlag is FALSE and the MulticastAddress is NULL, then all
+  currently joined groups are left.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  JoinFlag          Set to TRUE to join a multicast group. Set to FALSE to leave one
+                                or all multicast groups.
+  @param[in]  MulticastAddress  Pointer to multicast group address to join or leave.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_NOT_STARTED       The EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate resources to join the group.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+                                - This is NULL.
+                                - JoinFlag is TRUE and MulticastAddress is NULL.
+                                - JoinFlag is TRUE and *MulticastAddress is not
+                                  a valid multicast address.
+  @retval EFI_ALREADY_STARTED   The group address is already in the group table (when
+                                JoinFlag is TRUE).
+  @retval EFI_NOT_FOUND         The group address is not in the group table (when JoinFlag is
+                                FALSE).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Groups (
+  IN EFI_UDP4_PROTOCOL  *This,
+  IN BOOLEAN            JoinFlag,
+  IN EFI_IPv4_ADDRESS   *MulticastAddress OPTIONAL
+  )
+{
+  EFI_STATUS          Status;
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_IP4_PROTOCOL    *Ip;
+  EFI_TPL             OldTpl;
+  IP4_ADDR            McastIp;
+
+  if ((This == NULL) || (JoinFlag && (MulticastAddress == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  McastIp = 0;
+  if (JoinFlag) {
+    CopyMem (&McastIp, MulticastAddress, sizeof (IP4_ADDR));
+
+    if (!IP4_IS_MULTICAST (NTOHL (McastIp))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (Instance->IsNoMapping) {
+    return EFI_NO_MAPPING;
+  }
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  Ip = Instance->IpInfo->Ip.Ip4;
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Invoke the Ip instance the Udp4 instance consumes to do the group operation.
+  //
+  Status = Ip->Groups (Ip, JoinFlag, MulticastAddress);
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Keep a local copy of the configured multicast IPs because IpIo receives
+  // datagrams from the 0 station address IP instance and then UDP delivers to
+  // the matched instance. This copy of multicast IPs is used to avoid receive
+  // the mutlicast datagrams destined to multicast IPs the other instances configured.
+  //
+  if (JoinFlag) {
+
+    NetMapInsertTail (&Instance->McastIps, (VOID *) (UINTN) McastIp, NULL);
+  } else {
+
+    NetMapIterate (&Instance->McastIps, Udp4LeaveGroup, MulticastAddress);
+  }
+
+ON_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Adds and deletes routing table entries.
+  
+  The Routes() function adds a route to or deletes a route from the routing table.
+  Routes are determined by comparing the SubnetAddress with the destination IP
+  address and arithmetically AND-ing it with the SubnetMask. The gateway address
+  must be on the same subnet as the configured station address.
+  The default route is added with SubnetAddress and SubnetMask both set to 0.0.0.0.
+  The default route matches all destination IP addresses that do not match any
+  other routes.
+  A zero GatewayAddress is a nonroute. Packets are sent to the destination IP
+  address if it can be found in the Address Resolution Protocol (ARP) cache or
+  on the local subnet. One automatic nonroute entry will be inserted into the
+  routing table for outgoing packets that are addressed to a local subnet
+  (gateway address of 0.0.0.0).
+  Each instance of the EFI UDPv4 Protocol has its own independent routing table.
+  Instances of the EFI UDPv4 Protocol that use the default IP address will also
+  have copies of the routing table provided by the EFI_IP4_CONFIG_PROTOCOL. These
+  copies will be updated automatically whenever the IP driver reconfigures its
+  instances; as a result, the previous modification to these copies will be lost.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  DeleteRoute       Set to TRUE to delete this route from the routing table.
+                                Set to FALSE to add this route to the routing table.
+  @param[in]  SubnetAddress     The destination network address that needs to be routed.
+  @param[in]  SubnetMask        The subnet mask of SubnetAddress.
+  @param[in]  GatewayAddress    The gateway IP address for this route.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_NOT_STARTED       The EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                - RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_OUT_OF_RESOURCES  Could not add the entry to the routing table.
+  @retval EFI_NOT_FOUND         This route is not in the routing table.
+  @retval EFI_ACCESS_DENIED     The route is already defined in the routing table.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Routes (
+  IN EFI_UDP4_PROTOCOL  *This,
+  IN BOOLEAN            DeleteRoute,
+  IN EFI_IPv4_ADDRESS   *SubnetAddress,
+  IN EFI_IPv4_ADDRESS   *SubnetMask,
+  IN EFI_IPv4_ADDRESS   *GatewayAddress
+  )
+{
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_IP4_PROTOCOL    *Ip;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (Instance->IsNoMapping) {
+    return EFI_NO_MAPPING;
+  }
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  Ip = Instance->IpInfo->Ip.Ip4;
+
+  //
+  // Invoke the Ip instance the Udp4 instance consumes to do the actual operation.
+  //
+  return Ip->Routes (Ip, DeleteRoute, SubnetAddress, SubnetMask, GatewayAddress);
+}
+
+
+/**
+  Queues outgoing data packets into the transmit queue.
+  
+  The Transmit() function places a sending request to this instance of the EFI
+  UDPv4 Protocol, alongside the transmit data that was filled by the user. Whenever
+  the packet in the token is sent out or some errors occur, the Token.Event will
+  be signaled and Token.Status is updated. Providing a proper notification function
+  and context for the event will enable the user to receive the notification and
+  transmitting status.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token             Pointer to the completion token that will be placed into the
+                                transmit queue.
+
+  @retval EFI_SUCCESS           The data has been queued for transmission.
+  @retval EFI_NOT_STARTED       This EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_ACCESS_DENIED     The transmit completion token with the same
+                                Token.Event was already in the transmit queue.
+  @retval EFI_NOT_READY         The completion token could not be queued because the
+                                transmit queue is full.
+  @retval EFI_OUT_OF_RESOURCES  Could not queue the transmit data.
+  @retval EFI_NOT_FOUND         There is no route to the destination network or address.
+  @retval EFI_BAD_BUFFER_SIZE   The data length is greater than the maximum UDP packet
+                                size. Or the length of the IP header + UDP header + data
+                                length is greater than MTU if DoNotFragment is TRUE.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Transmit (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token
+  )
+{
+  EFI_STATUS              Status;
+  UDP4_INSTANCE_DATA      *Instance;
+  EFI_TPL                 OldTpl;
+  NET_BUF                 *Packet;
+  EFI_UDP_HEADER         *Udp4Header;
+  EFI_UDP4_CONFIG_DATA    *ConfigData;
+  IP4_ADDR                Source;
+  IP4_ADDR                Destination;
+  EFI_UDP4_TRANSMIT_DATA  *TxData;
+  EFI_UDP4_SESSION_DATA   *UdpSessionData;
+  UDP4_SERVICE_DATA       *Udp4Service;
+  IP_IO_OVERRIDE          Override;
+  UINT16                  HeadSum;
+  EFI_IP_ADDRESS          IpDestAddr;
+
+  if ((This == NULL) || (Token == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (Instance->IsNoMapping) {
+    return EFI_NO_MAPPING;
+  }
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Validate the Token, if the token is invalid return the error code.
+  //
+  Status = Udp4ValidateTxToken (Instance, Token);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  if (EFI_ERROR (NetMapIterate (&Instance->TxTokens, Udp4TokenExist, Token)) ||
+    EFI_ERROR (NetMapIterate (&Instance->RxTokens, Udp4TokenExist, Token))) {
+    //
+    // Try to find a duplicate token in the two token maps, if found, return
+    // EFI_ACCESS_DENIED.
+    //
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  TxData = Token->Packet.TxData;
+
+  //
+  // Create a net buffer to hold the user buffer and the udp header.
+  //
+  Packet = NetbufFromExt (
+             (NET_FRAGMENT *)TxData->FragmentTable,
+             TxData->FragmentCount,
+             UDP4_HEADER_SIZE,
+             0,
+             Udp4NetVectorExtFree,
+             NULL
+             );
+  if (Packet == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ON_EXIT;
+  }
+
+  //
+  // Store the IpIo in ProtoData.
+  //
+  Udp4Service = Instance->Udp4Service;
+  *((UINTN *) &Packet->ProtoData[0]) = (UINTN) (Udp4Service->IpIo);
+
+  Udp4Header = (EFI_UDP_HEADER *) NetbufAllocSpace (Packet, UDP4_HEADER_SIZE, TRUE);
+  ASSERT (Udp4Header != NULL);
+
+  ConfigData = &Instance->ConfigData;
+
+  //
+  // Fill the udp header.
+  //
+  Udp4Header->SrcPort      = HTONS (ConfigData->StationPort);
+  Udp4Header->DstPort      = HTONS (ConfigData->RemotePort);
+  Udp4Header->Length       = HTONS ((UINT16) Packet->TotalSize);
+  Udp4Header->Checksum     = 0;
+
+  UdpSessionData = TxData->UdpSessionData;
+  IP4_COPY_ADDRESS (&Override.Ip4OverrideData.SourceAddress, &ConfigData->StationAddress);
+
+  if (UdpSessionData != NULL) {
+    //
+    // Set the SourceAddress, SrcPort and Destination according to the specified
+    // UdpSessionData.
+    //
+    if (!EFI_IP4_EQUAL (&UdpSessionData->SourceAddress, &mZeroIp4Addr)) {
+      IP4_COPY_ADDRESS (&Override.Ip4OverrideData.SourceAddress, &UdpSessionData->SourceAddress);
+    }
+
+    if (UdpSessionData->SourcePort != 0) {
+      Udp4Header->SrcPort = HTONS (UdpSessionData->SourcePort);
+    }
+
+    if (UdpSessionData->DestinationPort != 0) {
+      Udp4Header->DstPort = HTONS (UdpSessionData->DestinationPort);
+    }
+
+    CopyMem (&Source, &Override.Ip4OverrideData.SourceAddress, sizeof (IP4_ADDR));
+    CopyMem (&Destination, &UdpSessionData->DestinationAddress, sizeof (IP4_ADDR));
+
+    //
+    // calculate the pseudo head checksum using the overridden parameters.
+    //
+    HeadSum = NetPseudoHeadChecksum (
+                Source,
+                Destination,
+                EFI_IP_PROTO_UDP,
+                0
+                );
+  } else {
+    //
+    // UdpSessionData is NULL, use the address and port information previously configured.
+    //
+    CopyMem (&Destination, &ConfigData->RemoteAddress, sizeof (IP4_ADDR));
+
+    HeadSum = Instance->HeadSum;
+  }
+
+  //
+  // calculate the checksum.
+  //
+  Udp4Header->Checksum = Udp4Checksum (Packet, HeadSum);
+  if (Udp4Header->Checksum == 0) {
+    //
+    // If the calculated checksum is 0, fill the Checksum field with all ones.
+    //
+    Udp4Header->Checksum = 0xffff;
+  }
+
+  //
+  // Fill the IpIo Override data.
+  //
+  if (TxData->GatewayAddress != NULL) {
+    IP4_COPY_ADDRESS (&Override.Ip4OverrideData.GatewayAddress, TxData->GatewayAddress);
+  } else {
+    ZeroMem (&Override.Ip4OverrideData.GatewayAddress, sizeof (EFI_IPv4_ADDRESS));
+  }
+
+  Override.Ip4OverrideData.Protocol                 = EFI_IP_PROTO_UDP;
+  Override.Ip4OverrideData.TypeOfService            = ConfigData->TypeOfService;
+  Override.Ip4OverrideData.TimeToLive               = ConfigData->TimeToLive;
+  Override.Ip4OverrideData.DoNotFragment            = ConfigData->DoNotFragment;
+
+  //
+  // Save the token into the TxToken map.
+  //
+  Status = NetMapInsertTail (&Instance->TxTokens, Token, Packet);
+  if (EFI_ERROR (Status)) {
+    goto FREE_PACKET;
+  }
+
+  //
+  // Send out this datagram through IpIo.
+  //
+  IpDestAddr.Addr[0] = Destination;
+  Status = IpIoSend (
+             Udp4Service->IpIo,
+             Packet,
+             Instance->IpInfo,
+             Instance,
+             Token,
+             &IpDestAddr,
+             &Override
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // Remove this token from the TxTokens.
+    //
+    Udp4RemoveToken (&Instance->TxTokens, Token);
+  }
+
+FREE_PACKET:
+
+  NetbufFree (Packet);
+
+ON_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Places an asynchronous receive request into the receiving queue.
+  
+  The Receive() function places a completion token into the receive packet queue.
+  This function is always asynchronous.
+  The caller must fill in the Token.Event field in the completion token, and this
+  field cannot be NULL. When the receive operation completes, the EFI UDPv4 Protocol
+  driver updates the Token.Status and Token.Packet.RxData fields and the Token.Event
+  is signaled. Providing a proper notification function and context for the event
+  will enable the user to receive the notification and receiving status. That
+  notification function is guaranteed to not be re-entered.
+
+  @param[in]  This              Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token             Pointer to a token that is associated with
+                                the receive data descriptor.
+
+  @retval EFI_SUCCESS           The receive completion token was cached.
+  @retval EFI_NOT_STARTED       This EFI UDPv4 Protocol instance has not been started.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP, RARP, etc.)
+                                is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more of the following conditions is TRUE:
+  @retval EFI_OUT_OF_RESOURCES  The receive completion token could not be queued due to a lack of system
+                                resources (usually memory).
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval EFI_ACCESS_DENIED     A receive completion token with the same Token.Event was already in
+                                the receive queue.
+  @retval EFI_NOT_READY         The receive request could not be queued because the receive queue is full.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Receive (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token
+  )
+{
+  EFI_STATUS          Status;
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_TPL             OldTpl;
+
+  if ((This == NULL) || (Token == NULL) || (Token->Event == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (Instance->IsNoMapping) {
+    return EFI_NO_MAPPING;
+  }
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  if (EFI_ERROR (NetMapIterate (&Instance->RxTokens, Udp4TokenExist, Token))||
+    EFI_ERROR (NetMapIterate (&Instance->TxTokens, Udp4TokenExist, Token))) {
+    //
+    // Return EFI_ACCESS_DENIED if the specified token is already in the TxTokens or
+    // RxTokens map.
+    //
+    Status = EFI_ACCESS_DENIED;
+    goto ON_EXIT;
+  }
+
+  Token->Packet.RxData = NULL;
+
+  //
+  // Save the token into the RxTokens map.
+  //
+  Status = NetMapInsertTail (&Instance->RxTokens, Token, NULL);
+  if (EFI_ERROR (Status)) {
+    Status = EFI_NOT_READY;
+    goto ON_EXIT;
+  }
+
+  //
+  // If there is an icmp error, report it.
+  //
+  Udp4ReportIcmpError (Instance);
+
+  //
+  // Try to deliver the received datagrams.
+  //
+  Udp4InstanceDeliverDgram (Instance);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of Token->Event.
+  //
+  DispatchDpc ();
+
+ON_EXIT:
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Aborts an asynchronous transmit or receive request.
+  
+  The Cancel() function is used to abort a pending transmit or receive request.
+  If the token is in the transmit or receive request queues, after calling this
+  function, Token.Status will be set to EFI_ABORTED and then Token.Event will be
+  signaled. If the token is not in one of the queues, which usually means that
+  the asynchronous operation has completed, this function will not signal the
+  token and EFI_NOT_FOUND is returned.
+
+  @param[in]  This  Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param[in]  Token Pointer to a token that has been issued by
+                    EFI_UDP4_PROTOCOL.Transmit() or
+                    EFI_UDP4_PROTOCOL.Receive().If NULL, all pending
+                    tokens are aborted.
+
+  @retval  EFI_SUCCESS           The asynchronous I/O request was aborted and Token.Event
+                                 was signaled. When Token is NULL, all pending requests are
+                                 aborted and their events are signaled.
+  @retval  EFI_INVALID_PARAMETER This is NULL.
+  @retval  EFI_NOT_STARTED       This instance has not been started.
+  @retval  EFI_NO_MAPPING        When using the default address, configuration (DHCP, BOOTP,
+                                 RARP, etc.) is not finished yet.
+  @retval  EFI_NOT_FOUND         When Token is not NULL, the asynchronous I/O request was
+                                 not found in the transmit or receive queue. It has either completed
+                                 or was not issued by Transmit() and Receive().
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Cancel (
+  IN EFI_UDP4_PROTOCOL          *This,
+  IN EFI_UDP4_COMPLETION_TOKEN  *Token OPTIONAL
+  )
+{
+  EFI_STATUS          Status;
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_TPL             OldTpl;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+
+  if (Instance->IsNoMapping) {
+    return EFI_NO_MAPPING;
+  }
+
+  if (!Instance->Configured) {
+    return EFI_NOT_STARTED;
+  }
+
+  OldTpl = gBS->RaiseTPL (TPL_CALLBACK);
+
+  //
+  // Cancle the tokens specified by Token for this instance.
+  //
+  Status = Udp4InstanceCancelToken (Instance, Token);
+
+  //
+  // Dispatch the DPC queued by the NotifyFunction of the cancelled token's events.
+  //
+  DispatchDpc ();
+
+  gBS->RestoreTPL (OldTpl);
+
+  return Status;
+}
+
+
+/**
+  Polls for incoming data packets and processes outgoing data packets.
+  
+  The Poll() function can be used by network drivers and applications to increase
+  the rate that data packets are moved between the communications device and the
+  transmit and receive queues.
+  In some systems, the periodic timer event in the managed network driver may not
+  poll the underlying communications device fast enough to transmit and/or receive
+  all data packets without missing incoming packets or dropping outgoing packets.
+  Drivers and applications that are experiencing packet loss should try calling
+  the Poll() function more often.
+
+  @param[in]  This  Pointer to the EFI_UDP4_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           Incoming or outgoing data was processed.
+  @retval EFI_INVALID_PARAMETER This is NULL.
+  @retval EFI_DEVICE_ERROR      An unexpected system or network error occurred.
+  @retval EFI_TIMEOUT           Data was dropped out of the transmit and/or receive queue.
+
+**/
+EFI_STATUS
+EFIAPI
+Udp4Poll (
+  IN EFI_UDP4_PROTOCOL  *This
+  )
+{
+  UDP4_INSTANCE_DATA  *Instance;
+  EFI_IP4_PROTOCOL    *Ip;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = UDP4_INSTANCE_DATA_FROM_THIS (This);
+  Ip       = Instance->IpInfo->Ip.Ip4;
+
+  //
+  // Invode the Ip instance consumed by the udp instance to do the poll operation.
+  //
+  return Ip->Poll (Ip);
+}
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/ComponentName.c
new file mode 100644
index 0000000000..f4ef59ce5e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/ComponentName.c
@@ -0,0 +1,365 @@
+/** @file
+
+Copyright (c) 2007 - 2012, 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 "PxeBcImpl.h"
+
+//
+// EFI Component Name Functions
+//
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  * This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  * This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle        OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  );
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL     gPxeBcComponentName = {
+  PxeBcComponentNameGetDriverName,
+  PxeBcComponentNameGetControllerName,
+  "eng"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL    gPxeBcComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) PxeBcComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) PxeBcComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mPxeBcDriverNameTable[] = {
+  {
+    "eng;en",
+    L"UEFI PXE Base Code Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE mPxeBcControllerNameTable[] = {
+  {
+    "eng;en",
+    L"PXE Controller"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  DriverName       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcComponentNameGetDriverName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  * This,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+          Language,
+          This->SupportedLanguages,
+          mPxeBcDriverNameTable,
+          DriverName,
+          (BOOLEAN)(This == &gPxeBcComponentName)
+          );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param[in]  This              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+
+  @param[in]  ControllerHandle  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+
+  @param[in]  ChildHandle       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+
+  @param[in]  Language          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+
+  @param[out]  ControllerName   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcComponentNameGetControllerName (
+  IN  EFI_COMPONENT_NAME_PROTOCOL  * This,
+  IN  EFI_HANDLE                   ControllerHandle,
+  IN  EFI_HANDLE                   ChildHandle        OPTIONAL,
+  IN  CHAR8                        *Language,
+  OUT CHAR16                       **ControllerName
+  )
+{
+  EFI_PXE_BASE_CODE_PROTOCOL  *PxeBc;
+  EFI_HANDLE                  NicHandle;
+  EFI_STATUS                  Status;
+  
+  if (ControllerHandle == NULL || ChildHandle != NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiArpProtocolGuid);
+  if (NicHandle == NULL) {
+    NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiDhcp4ProtocolGuid);
+
+    if (NicHandle == NULL) {
+      NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiIp4ProtocolGuid);
+
+      if (NicHandle == NULL) {
+        NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiUdp4ProtocolGuid);
+
+        if (NicHandle == NULL) {
+          NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiMtftp4ProtocolGuid);
+
+          if (NicHandle == NULL) {
+            return EFI_UNSUPPORTED;
+          }
+        }
+      }
+    }
+  }
+
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiPxeBaseCodeProtocolGuid,
+                  (VOID **) &PxeBc,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mPxeBcControllerNameTable,
+           ControllerName,
+           (BOOLEAN)(This == &gPxeBcComponentName)
+           );
+}
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.c
new file mode 100644
index 0000000000..6c06373004
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.c
@@ -0,0 +1,1938 @@
+/** @file
+  Support for PxeBc dhcp functions.
+
+Copyright (c) 2013, Red Hat, Inc.
+Copyright (c) 2007 - 2015, 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 "PxeBcImpl.h"
+
+//
+// This is a map from the interested DHCP4 option tags' index to the tag value.
+//
+UINT8 mInterestedDhcp4Tags[PXEBC_DHCP4_TAG_INDEX_MAX] = {
+  PXEBC_DHCP4_TAG_BOOTFILE_LEN,
+  PXEBC_DHCP4_TAG_VENDOR,
+  PXEBC_DHCP4_TAG_OVERLOAD,
+  PXEBC_DHCP4_TAG_MSG_TYPE,
+  PXEBC_DHCP4_TAG_SERVER_ID,
+  PXEBC_DHCP4_TAG_CLASS_ID,
+  PXEBC_DHCP4_TAG_BOOTFILE
+};
+
+
+/**
+  This function initialize the DHCP4 message instance.
+
+  This function will pad each item of dhcp4 message packet.
+
+  @param  Seed    Pointer to the message instance of the DHCP4 packet.
+  @param  Udp4    Pointer to the EFI_UDP4_PROTOCOL instance.
+
+**/
+VOID
+PxeBcInitSeedPacket (
+  IN EFI_DHCP4_PACKET  *Seed,
+  IN EFI_UDP4_PROTOCOL *Udp4
+  )
+{
+  EFI_SIMPLE_NETWORK_MODE Mode;
+  EFI_DHCP4_HEADER        *Header;
+
+  Udp4->GetModeData (Udp4, NULL, NULL, NULL, &Mode);
+
+  Seed->Size    = sizeof (EFI_DHCP4_PACKET);
+  Seed->Length  = sizeof (Seed->Dhcp4);
+
+  Header        = &Seed->Dhcp4.Header;
+
+  ZeroMem (Header, sizeof (EFI_DHCP4_HEADER));
+  Header->OpCode    = PXEBC_DHCP4_OPCODE_REQUEST;
+  Header->HwType    = Mode.IfType;
+  Header->HwAddrLen = (UINT8) Mode.HwAddressSize;
+  CopyMem (Header->ClientHwAddr, &Mode.CurrentAddress, Header->HwAddrLen);
+
+  Seed->Dhcp4.Magik     = PXEBC_DHCP4_MAGIC;
+  Seed->Dhcp4.Option[0] = PXEBC_DHCP4_TAG_EOP;
+}
+
+
+/**
+  Copy the DCHP4 packet from srouce to destination.
+
+  @param  Dst   Pointer to the EFI_DHCP4_PROTOCOL instance.
+  @param  Src   Pointer to the EFI_DHCP4_PROTOCOL instance.
+
+**/
+VOID
+PxeBcCopyEfiDhcp4Packet (
+  IN EFI_DHCP4_PACKET  *Dst,
+  IN EFI_DHCP4_PACKET  *Src
+  )
+{
+  ASSERT (Dst->Size >= Src->Length);
+
+  CopyMem (&Dst->Dhcp4, &Src->Dhcp4, Src->Length);
+  Dst->Length = Src->Length;
+}
+
+
+/**
+  Copy the dhcp4 packet to the PxeBc private data and parse the dhcp4 packet.
+
+  @param  Private       Pointer to PxeBc private data.
+  @param  OfferIndex    Index of cached packets as complements of pxe mode data,
+                        the index is maximum offer number.
+
+**/
+VOID
+PxeBcCopyProxyOffer (
+  IN PXEBC_PRIVATE_DATA  *Private,
+  IN UINT32              OfferIndex
+  )
+{
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_DHCP4_PACKET        *Offer;
+
+  ASSERT (OfferIndex < Private->NumOffers);
+  ASSERT (OfferIndex < PXEBC_MAX_OFFER_NUM);
+
+  Mode  = Private->PxeBc.Mode;
+  Offer = &Private->Dhcp4Offers[OfferIndex].Packet.Offer;
+
+  PxeBcCopyEfiDhcp4Packet (&Private->ProxyOffer.Packet.Offer, Offer);
+  CopyMem (&Mode->ProxyOffer, &Offer->Dhcp4, Offer->Length);
+  Mode->ProxyOfferReceived = TRUE;
+
+  PxeBcParseCachedDhcpPacket (&Private->ProxyOffer);
+}
+
+
+/**
+  Parse the cached dhcp packet.
+
+  @param  CachedPacket  Pointer to cached dhcp packet.
+
+  @retval TRUE          Succeed to parse and validation.
+  @retval FALSE         Fail to parse or validation.
+
+**/
+BOOLEAN
+PxeBcParseCachedDhcpPacket (
+  IN PXEBC_CACHED_DHCP4_PACKET  *CachedPacket
+  )
+{
+  EFI_DHCP4_PACKET        *Offer;
+  EFI_DHCP4_PACKET_OPTION **Options;
+  EFI_DHCP4_PACKET_OPTION *Option;
+  UINT8                   OfferType;
+  UINTN                   Index;
+  UINT8                   *Ptr8;
+
+  CachedPacket->IsPxeOffer = FALSE;
+  ZeroMem (CachedPacket->Dhcp4Option, sizeof (CachedPacket->Dhcp4Option));
+  ZeroMem (&CachedPacket->PxeVendorOption, sizeof (CachedPacket->PxeVendorOption));
+
+  Offer   = &CachedPacket->Packet.Offer;
+  Options = CachedPacket->Dhcp4Option;
+
+  //
+  // Parse interested dhcp options and store their pointers in CachedPacket->Dhcp4Option.
+  // First, try to parse DHCPv4 options from the DHCP optional parameters field.
+  //
+  for (Index = 0; Index < PXEBC_DHCP4_TAG_INDEX_MAX; Index++) {
+    Options[Index] = PxeBcParseExtendOptions (
+                       Offer->Dhcp4.Option,
+                       GET_OPTION_BUFFER_LEN (Offer),
+                       mInterestedDhcp4Tags[Index]
+                       );
+  }
+  //
+  // Second, Check if bootfilename and serverhostname is overloaded to carry DHCP options refers to rfc-2132. 
+  // If yes, try to parse options from the BootFileName field, then ServerName field.
+  //
+  Option = Options[PXEBC_DHCP4_TAG_INDEX_OVERLOAD];
+  if (Option != NULL) {
+    if ((Option->Data[0] & PXEBC_DHCP4_OVERLOAD_FILE) != 0) {
+      for (Index = 0; Index < PXEBC_DHCP4_TAG_INDEX_MAX; Index++) {
+        if (Options[Index] == NULL) {
+          Options[Index] = PxeBcParseExtendOptions (
+                             (UINT8 *) Offer->Dhcp4.Header.BootFileName,
+                             sizeof (Offer->Dhcp4.Header.BootFileName),
+                             mInterestedDhcp4Tags[Index]
+                             );
+        }
+      }
+    }
+    if ((Option->Data[0] & PXEBC_DHCP4_OVERLOAD_SERVER_NAME) != 0) {
+      for (Index = 0; Index < PXEBC_DHCP4_TAG_INDEX_MAX; Index++) {
+        if (Options[Index] == NULL) {
+          Options[Index] = PxeBcParseExtendOptions (
+                             (UINT8 *) Offer->Dhcp4.Header.ServerName,
+                             sizeof (Offer->Dhcp4.Header.ServerName),
+                             mInterestedDhcp4Tags[Index]
+                             );
+        }
+      }
+    }
+  }
+
+  //
+  // Check whether is an offer with PXEClient or not.
+  //
+  Option = Options[PXEBC_DHCP4_TAG_INDEX_CLASS_ID];
+  if ((Option != NULL) && (Option->Length >= 9) &&
+    (CompareMem (Option->Data, DEFAULT_CLASS_ID_DATA, 9) == 0)) {
+
+    CachedPacket->IsPxeOffer = TRUE;
+  }
+
+  //
+  // Parse pxe vendor options and store their content/pointers in CachedPacket->PxeVendorOption.
+  //
+  Option = Options[PXEBC_DHCP4_TAG_INDEX_VENDOR];
+  if (CachedPacket->IsPxeOffer && (Option != NULL)) {
+
+    if (!PxeBcParseVendorOptions (Option, &CachedPacket->PxeVendorOption)) {
+      return FALSE;
+    }
+  }
+
+
+  //
+  // Parse PXE boot file name:
+  // According to PXE spec, boot file name should be read from DHCP option 67 (bootfile name) if present.
+  // Otherwise, read from boot file field in DHCP header.
+  //
+  if (Options[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] != NULL) {
+    //
+    // RFC 2132, Section 9.5 does not strictly state Bootfile name (option 67) is null 
+    // terminated string. So force to append null terminated character at the end of string.
+    //
+    Ptr8 =  (UINT8*)&Options[PXEBC_DHCP4_TAG_INDEX_BOOTFILE]->Data[0];
+    Ptr8 += Options[PXEBC_DHCP4_TAG_INDEX_BOOTFILE]->Length;
+    if (*(Ptr8 - 1) != '\0') {
+      *Ptr8 = '\0';
+    }
+  } else if (Offer->Dhcp4.Header.BootFileName[0] != 0) {
+    //
+    // If the bootfile is not present and bootfilename is present in dhcp packet, just parse it.
+    // And do not count dhcp option header, or else will destroy the serverhostname.
+    //
+    Options[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] = (EFI_DHCP4_PACKET_OPTION *) (&Offer->Dhcp4.Header.BootFileName[0] -
+                                            OFFSET_OF (EFI_DHCP4_PACKET_OPTION, Data[0]));
+
+  }
+
+  //
+  // Determine offer type of the dhcp packet.
+  //
+  Option = Options[PXEBC_DHCP4_TAG_INDEX_MSG_TYPE];
+  if ((Option == NULL) || (Option->Data[0] == 0)) {
+    //
+    // It's a bootp offer
+    //
+    Option = CachedPacket->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE];
+    if (Option == NULL) {
+      //
+      // bootp offer without bootfilename, discard it.
+      //
+      return FALSE;
+    }
+
+    OfferType = DHCP4_PACKET_TYPE_BOOTP;
+
+  } else {
+
+    if (IS_VALID_DISCOVER_VENDOR_OPTION (CachedPacket->PxeVendorOption.BitMap)) {
+      //
+      // It's a pxe10 offer with PXEClient and discover vendor option.
+      //
+      OfferType = DHCP4_PACKET_TYPE_PXE10;
+    } else if (IS_VALID_MTFTP_VENDOR_OPTION (CachedPacket->PxeVendorOption.BitMap)) {
+      //
+      // It's a wfm11a offer with PXEClient and mtftp vendor option, and
+      // return false since mtftp not supported currently.
+      //
+      return FALSE;
+    } else {
+      //
+      // If the binl offer with only PXEClient.
+      //
+      OfferType = (UINT8) ((CachedPacket->IsPxeOffer) ? DHCP4_PACKET_TYPE_BINL : DHCP4_PACKET_TYPE_DHCP_ONLY);
+    }
+  }
+
+  CachedPacket->OfferType = OfferType;
+
+  return TRUE;
+}
+
+
+/**
+  Offer dhcp service with a BINL dhcp offer.
+
+  @param  Private   Pointer to PxeBc private data.
+  @param  Index     Index of cached packets as complements of pxe mode data,
+                    the index is maximum offer number.
+
+  @retval TRUE      Offer the service successfully under priority BINL.
+  @retval FALSE     Boot Service failed, parse cached dhcp packet failed or this
+                    BINL ack cannot find options set or bootfile name specified.
+
+**/
+BOOLEAN
+PxeBcTryBinl (
+  IN PXEBC_PRIVATE_DATA  *Private,
+  IN UINT32              Index
+  )
+{
+  EFI_DHCP4_PACKET          *Offer;
+  EFI_IP_ADDRESS            ServerIp;
+  EFI_STATUS                Status;
+  PXEBC_CACHED_DHCP4_PACKET *CachedPacket;
+  EFI_DHCP4_PACKET          *Reply;
+
+  ASSERT (Index < PXEBC_MAX_OFFER_NUM);
+  ASSERT (Private->Dhcp4Offers[Index].OfferType == DHCP4_PACKET_TYPE_BINL);
+
+  Offer = &Private->Dhcp4Offers[Index].Packet.Offer;
+
+  //
+  // Use siaddr(next server) in DHCPOFFER packet header, if zero, use option 54(server identifier)
+  // in DHCPOFFER packet.
+  // (It does not comply with PXE Spec, Ver2.1)
+  //
+  if (EFI_IP4_EQUAL (&Offer->Dhcp4.Header.ServerAddr.Addr, &mZeroIp4Addr)) {
+    CopyMem (
+      &ServerIp.Addr[0],
+      Private->Dhcp4Offers[Index].Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_SERVER_ID]->Data,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+  } else {
+    CopyMem (
+      &ServerIp.Addr[0],
+      &Offer->Dhcp4.Header.ServerAddr,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+  }
+  if (ServerIp.Addr[0] == 0) {
+    return FALSE;
+  }
+
+  CachedPacket = &Private->ProxyOffer;
+  Reply        = &CachedPacket->Packet.Offer;
+
+  Status = PxeBcDiscvBootService (
+            Private,
+            0,
+            NULL,
+            FALSE,
+            &ServerIp,
+            0,
+            NULL,
+            FALSE,
+            Reply
+            );
+  if (EFI_ERROR (Status)) {
+    return FALSE;
+  }
+
+  if (!PxeBcParseCachedDhcpPacket (CachedPacket)) {
+    return FALSE;
+  }
+
+  if ((CachedPacket->OfferType != DHCP4_PACKET_TYPE_PXE10) &&
+      (CachedPacket->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] == NULL)) {
+    //
+    // This BINL ack doesn't have discovery options set or bootfile name
+    // specified.
+    //
+    return FALSE;
+  }
+
+  Private->PxeBc.Mode->ProxyOfferReceived = TRUE;
+  CopyMem (&Private->PxeBc.Mode->ProxyOffer, &Reply->Dhcp4, Reply->Length);
+
+  return TRUE;
+}
+
+
+/**
+  Offer dhcp service for each proxy with a BINL dhcp offer.
+
+  @param  Private     Pointer to PxeBc private data
+  @param  OfferIndex  Pointer to the index of cached packets as complements of
+                      pxe mode data, the index is maximum offer number.
+
+  @return If there is no service needed offer return FALSE, otherwise TRUE.
+
+**/
+BOOLEAN
+PxeBcTryBinlProxy (
+  IN  PXEBC_PRIVATE_DATA  *Private,
+  OUT UINT32              *OfferIndex
+  )
+{
+  UINT32  Index;
+
+  for (Index = 0; Index < Private->ProxyIndex[DHCP4_PACKET_TYPE_BINL]; Index++) {
+
+    *OfferIndex = Private->BinlIndex[Index];
+    //
+    // Try this BINL proxy offer
+    //
+    if (PxeBcTryBinl (Private, *OfferIndex)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+
+/**
+  This function is to check the selected proxy offer (include BINL dhcp offer and
+  DHCP_ONLY offer ) and set the flag and copy the DHCP packets to the Pxe base code
+  mode structure.
+
+  @param  Private          Pointer to PxeBc private data.
+
+  @retval EFI_SUCCESS      Operational successful.
+  @retval EFI_NO_RESPONSE  Offer dhcp service failed.
+
+**/
+EFI_STATUS
+PxeBcCheckSelectedOffer (
+  IN PXEBC_PRIVATE_DATA  *Private
+  )
+{
+  PXEBC_CACHED_DHCP4_PACKET *SelectedOffer;
+  EFI_DHCP4_PACKET_OPTION   **Options;
+  UINT32                    Index;
+  EFI_DHCP4_PACKET          *Offer;
+  UINT32                    ProxyOfferIndex;
+  EFI_STATUS                Status;
+  EFI_PXE_BASE_CODE_MODE    *Mode;
+  EFI_DHCP4_PACKET          *Ack;
+
+  ASSERT (Private->SelectedOffer != 0);
+
+  Status        = EFI_SUCCESS;
+  SelectedOffer = &Private->Dhcp4Offers[Private->SelectedOffer - 1];
+  Options       = SelectedOffer->Dhcp4Option;
+
+  if (SelectedOffer->OfferType == DHCP4_PACKET_TYPE_BINL) {
+    //
+    // The addresses are acquired from a BINL dhcp offer, try BINL to get
+    // the bootfile name
+    //
+    if (!PxeBcTryBinl (Private, Private->SelectedOffer - 1)) {
+      Status = EFI_NO_RESPONSE;
+    }
+  } else if (SelectedOffer->OfferType == DHCP4_PACKET_TYPE_DHCP_ONLY) {
+    //
+    // The selected offer to finish the D.O.R.A. is a DHCP only offer, we need
+    // try proxy offers if there are some, othewise the bootfile name must be
+    // set in this DHCP only offer.
+    //
+    if (Private->GotProxyOffer) {
+      //
+      // Get rid of the compiler warning.
+      //
+      ProxyOfferIndex = 0;
+      if (Private->SortOffers) {
+        //
+        // The offers are sorted before selecting, the proxy offer type must be
+        // already determined.
+        //
+        ASSERT (Private->ProxyIndex[Private->ProxyOfferType] > 0);
+
+        if (Private->ProxyOfferType == DHCP4_PACKET_TYPE_BINL) {
+          //
+          // We buffer all received BINL proxy offers, try them all one by one
+          //
+          if (!PxeBcTryBinlProxy (Private, &ProxyOfferIndex)) {
+            Status = EFI_NO_RESPONSE;
+          }
+        } else {
+          //
+          // For other types, only one proxy offer is buffered.
+          //
+          ProxyOfferIndex = Private->ProxyIndex[Private->ProxyOfferType] - 1;
+        }
+      } else {
+        //
+        // The proxy offer type is not determined, choose proxy offer in the
+        // received order.
+        //
+        Status = EFI_NO_RESPONSE;
+
+        ASSERT (Private->NumOffers < PXEBC_MAX_OFFER_NUM);
+        for (Index = 0; Index < Private->NumOffers; Index++) {
+
+          Offer = &Private->Dhcp4Offers[Index].Packet.Offer;
+          if (!IS_PROXY_DHCP_OFFER (Offer)) {
+            //
+            // Skip non proxy dhcp offers.
+            //
+            continue;
+          }
+
+          if (Private->Dhcp4Offers[Index].OfferType == DHCP4_PACKET_TYPE_BINL) {
+            //
+            // Try BINL
+            //
+            if (!PxeBcTryBinl (Private, Index)) {
+              //
+              // Failed, skip to the next offer
+              //
+              continue;
+            }
+          }
+
+          Private->ProxyOfferType = Private->Dhcp4Offers[Index].OfferType;
+          ProxyOfferIndex         = Index;
+          Status                  = EFI_SUCCESS;
+          break;
+        }
+      }
+
+      if (!EFI_ERROR (Status) && (Private->ProxyOfferType != DHCP4_PACKET_TYPE_BINL)) {
+        //
+        // Copy the proxy offer to Mode and set the flag
+        //
+        PxeBcCopyProxyOffer (Private, ProxyOfferIndex);
+      }
+    } else {
+      //
+      // No proxy offer is received, the bootfile name MUST be set.
+      //
+      ASSERT (Options[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] != NULL);
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Everything is OK, set the flag and copy the DHCP packets.
+    //
+    Mode  = Private->PxeBc.Mode;
+    Offer = &SelectedOffer->Packet.Offer;
+
+    //
+    // The discover packet is already copied, just set flag here.
+    //
+    Mode->DhcpDiscoverValid = TRUE;
+
+    Ack                     = &Private->Dhcp4Ack.Packet.Ack;
+    if (SelectedOffer->OfferType == DHCP4_PACKET_TYPE_BOOTP) {
+      //
+      // Other type of ACK is already cached. Bootp is special that we should
+      // use the bootp reply as the ACK and put it into the DHCP_ONLY buffer.
+      //
+      PxeBcCopyEfiDhcp4Packet (&Private->Dhcp4Ack.Packet.Ack, Offer);
+    }
+
+    PxeBcParseCachedDhcpPacket (&Private->Dhcp4Ack);
+
+    Mode->DhcpAckReceived = TRUE;
+
+    //
+    // Copy the dhcp ack.
+    //
+    CopyMem (&Mode->DhcpAck, &Ack->Dhcp4, Ack->Length);
+  }
+
+  return Status;
+}
+
+
+/**
+  Cache the Dhcp4 packet offer, Parse and validate each option of the packet.
+
+  @param  Private    Pointer to PxeBc private data.
+  @param  RcvdOffer  Pointer to the received Dhcp proxy offer packet.
+
+**/
+VOID
+PxeBcCacheDhcpOffer (
+  IN PXEBC_PRIVATE_DATA  *Private,
+  IN EFI_DHCP4_PACKET    *RcvdOffer
+  )
+{
+  PXEBC_CACHED_DHCP4_PACKET *CachedOffer;
+  EFI_DHCP4_PACKET          *Offer;
+  UINT8                     OfferType;
+
+  CachedOffer = &Private->Dhcp4Offers[Private->NumOffers];
+  Offer       = &CachedOffer->Packet.Offer;
+
+  //
+  // Cache the orignal dhcp packet
+  //
+  PxeBcCopyEfiDhcp4Packet (Offer, RcvdOffer);
+
+  //
+  // Parse and validate the options (including dhcp option and vendor option)
+  //
+  if (!PxeBcParseCachedDhcpPacket (CachedOffer)) {
+    return ;
+  }
+
+  OfferType = CachedOffer->OfferType;
+  if (OfferType >= DHCP4_PACKET_TYPE_MAX) {
+    return ;
+  }
+
+  if (OfferType == DHCP4_PACKET_TYPE_BOOTP) {
+
+    if (Private->BootpIndex != 0) {
+      //
+      // Only cache the first bootp offer, discard others.
+      //
+      return ;
+    } else {
+      //
+      // Take as a dhcp only offer, but record index specifically.
+      //
+      Private->BootpIndex = Private->NumOffers + 1;
+    }
+  } else {
+
+    if (IS_PROXY_DHCP_OFFER (Offer)) {
+      //
+      // It's a proxy dhcp offer with no your address, including pxe10, wfm11a or binl offer.
+      //
+      Private->GotProxyOffer = TRUE;
+
+      if (OfferType == DHCP4_PACKET_TYPE_BINL) {
+        //
+        // Cache all binl offers.
+        //
+        Private->BinlIndex[Private->ProxyIndex[DHCP4_PACKET_TYPE_BINL]] = Private->NumOffers;
+        Private->ProxyIndex[DHCP4_PACKET_TYPE_BINL]++;
+      } else if (Private->ProxyIndex[OfferType] != 0) {
+        //
+        // Only cache the first pxe10/wfm11a offers each, discard the others.
+        //
+        return ;
+      } else {
+        //
+        // Record index of the proxy dhcp offer with type other than binl.
+        //
+        Private->ProxyIndex[OfferType] = Private->NumOffers + 1;
+      }
+    } else {
+      //
+      // It's a dhcp offer with your address.
+      //
+      ASSERT (Private->ServerCount[OfferType] < PXEBC_MAX_OFFER_NUM);
+      Private->OfferIndex[OfferType][Private->ServerCount[OfferType]] = Private->NumOffers;
+      Private->ServerCount[OfferType]++;
+    }
+  }
+
+  //
+  // Count the accepted offers.
+  //
+  Private->NumOffers++;
+}
+
+/**
+  Switch the Ip4 policy to static.
+
+  @param[in]  Private             The pointer to PXEBC_PRIVATE_DATA.
+
+  @retval     EFI_SUCCESS         The policy is already configured to static.
+  @retval     Others              Other error as indicated..
+
+**/
+EFI_STATUS
+PxeBcSetIp4Policy (   
+  IN PXEBC_PRIVATE_DATA            *Private
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_IP4_CONFIG2_PROTOCOL     *Ip4Config2;
+  EFI_IP4_CONFIG2_POLICY       Policy;
+  UINTN                        DataSize;
+
+  Ip4Config2 = Private->Ip4Config2;
+  DataSize = sizeof (EFI_IP4_CONFIG2_POLICY);
+  Status = Ip4Config2->GetData (
+                       Ip4Config2,
+                       Ip4Config2DataTypePolicy,
+                       &DataSize,
+                       &Policy
+                       );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  if (Policy != Ip4Config2PolicyStatic) {
+    Policy = Ip4Config2PolicyStatic;
+    Status= Ip4Config2->SetData (
+                          Ip4Config2,
+                          Ip4Config2DataTypePolicy,
+                          sizeof (EFI_IP4_CONFIG2_POLICY),
+                          &Policy
+                          );
+    if (EFI_ERROR (Status)) {
+      return Status;
+    } 
+  }
+
+  return  EFI_SUCCESS;
+}
+
+
+/**
+  Select the specified proxy offer, such as BINL, DHCP_ONLY and so on.
+  If the proxy does not exist, try offers with bootfile.
+
+  @param  Private   Pointer to PxeBc private data.
+
+**/
+VOID
+PxeBcSelectOffer (
+  IN PXEBC_PRIVATE_DATA  *Private
+  )
+{
+  UINT32            Index;
+  UINT32            OfferIndex;
+  EFI_DHCP4_PACKET  *Offer;
+
+  Private->SelectedOffer = 0;
+
+  if (Private->SortOffers) {
+    //
+    // Select offer according to the priority
+    //
+    if (Private->ServerCount[DHCP4_PACKET_TYPE_PXE10] > 0) {
+      //
+      // DHCP with PXE10
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_PXE10][0] + 1;
+
+    } else if (Private->ServerCount[DHCP4_PACKET_TYPE_WFM11A] > 0) {
+      //
+      // DHCP with WfM
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_WFM11A][0] + 1;
+
+    } else if ((Private->ProxyIndex[DHCP4_PACKET_TYPE_PXE10] > 0) &&
+             (Private->ServerCount[DHCP4_PACKET_TYPE_DHCP_ONLY] > 0)
+            ) {
+      //
+      // DHCP only and proxy DHCP with PXE10
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_DHCP_ONLY][0] + 1;
+      Private->ProxyOfferType     = DHCP4_PACKET_TYPE_PXE10;
+
+    } else if ((Private->ProxyIndex[DHCP4_PACKET_TYPE_WFM11A] > 0) &&
+             (Private->ServerCount[DHCP4_PACKET_TYPE_DHCP_ONLY] > 0)
+            ) {
+      //
+      // DHCP only and proxy DHCP with WfM
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_DHCP_ONLY][0] + 1;
+      Private->ProxyOfferType     = DHCP4_PACKET_TYPE_WFM11A;
+
+    } else if (Private->ServerCount[DHCP4_PACKET_TYPE_BINL] > 0) {
+      //
+      // DHCP with BINL
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_BINL][0] + 1;
+
+    } else if ((Private->ProxyIndex[DHCP4_PACKET_TYPE_BINL] > 0) &&
+             (Private->ServerCount[DHCP4_PACKET_TYPE_DHCP_ONLY] > 0)
+            ) {
+      //
+      // DHCP only and proxy DHCP with BINL
+      //
+      Private->SelectedOffer = Private->OfferIndex[DHCP4_PACKET_TYPE_DHCP_ONLY][0] + 1;
+      Private->ProxyOfferType     = DHCP4_PACKET_TYPE_BINL;
+
+    } else {
+      //
+      // Try offers with bootfile
+      //
+      for (Index = 0; Index < Private->ServerCount[DHCP4_PACKET_TYPE_DHCP_ONLY]; Index++) {
+        //
+        // Select the first DHCP only offer with bootfile
+        //
+        OfferIndex = Private->OfferIndex[DHCP4_PACKET_TYPE_DHCP_ONLY][Index];
+        if (Private->Dhcp4Offers[OfferIndex].Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] != NULL) {
+          Private->SelectedOffer = OfferIndex + 1;
+          break;
+        }
+      }
+
+      if (Private->SelectedOffer == 0) {
+        //
+        // Select the Bootp reply with bootfile if any
+        //
+        Private->SelectedOffer = Private->BootpIndex;
+      }
+    }
+  } else {
+    //
+    // Try the offers in the received order.
+    //
+    for (Index = 0; Index < Private->NumOffers; Index++) {
+
+      Offer = &Private->Dhcp4Offers[Index].Packet.Offer;
+
+      if (IS_PROXY_DHCP_OFFER (Offer)) {
+        //
+        // Skip proxy offers
+        //
+        continue;
+      }
+
+      if ((Private->Dhcp4Offers[Index].OfferType == DHCP4_PACKET_TYPE_DHCP_ONLY) &&
+          ((!Private->GotProxyOffer) && (Private->Dhcp4Offers[Index].Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] == NULL))) {
+        //
+        // DHCP only offer but no proxy offer received and no bootfile option in this offer
+        //
+        continue;
+      }
+
+      Private->SelectedOffer = Index + 1;
+      break;
+    }
+  }
+}
+
+
+/**
+  Callback routine.
+
+  EFI_DHCP4_CALLBACK is provided by the consumer of the EFI DHCPv4 Protocol driver
+  to intercept events that occurred in the configuration process. This structure
+  provides advanced control of each state transition of the DHCP process. The
+  returned status code determines the behavior of the EFI DHCPv4 Protocol driver.
+  There are three possible returned values, which are described in the following
+  table.
+
+  @param  This                  Pointer to the EFI DHCPv4 Protocol instance that is used to
+                                configure this callback function.
+  @param  Context               Pointer to the context that is initialized by
+                                EFI_DHCP4_PROTOCOL.Configure().
+  @param  CurrentState          The current operational state of the EFI DHCPv4 Protocol
+                                driver.
+  @param  Dhcp4Event            The event that occurs in the current state, which usually means a
+                                state transition.
+  @param  Packet                The DHCP packet that is going to be sent or already received.
+  @param  NewPacket             The packet that is used to replace the above Packet.
+
+  @retval EFI_SUCCESS           Tells the EFI DHCPv4 Protocol driver to continue the DHCP process.
+  @retval EFI_NOT_READY         Only used in the Dhcp4Selecting state. The EFI DHCPv4 Protocol
+                                driver will continue to wait for more DHCPOFFER packets until the retry
+                                timeout expires.
+  @retval EFI_ABORTED           Tells the EFI DHCPv4 Protocol driver to abort the current process and
+                                return to the Dhcp4Init or Dhcp4InitReboot state.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDhcpCallBack (
+  IN EFI_DHCP4_PROTOCOL                * This,
+  IN VOID                              *Context,
+  IN EFI_DHCP4_STATE                   CurrentState,
+  IN EFI_DHCP4_EVENT                   Dhcp4Event,
+  IN EFI_DHCP4_PACKET                  * Packet OPTIONAL,
+  OUT EFI_DHCP4_PACKET                 **NewPacket OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA                  *Private;
+  EFI_PXE_BASE_CODE_MODE              *Mode;
+  EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL *Callback;
+  EFI_DHCP4_PACKET_OPTION             *MaxMsgSize;
+  UINT16                              Value;
+  EFI_STATUS                          Status;
+  BOOLEAN                             Received;
+  EFI_DHCP4_HEADER                    *DhcpHeader;
+
+  if ((Dhcp4Event != Dhcp4RcvdOffer) &&
+      (Dhcp4Event != Dhcp4SelectOffer) &&
+      (Dhcp4Event != Dhcp4SendDiscover) &&
+      (Dhcp4Event != Dhcp4RcvdAck) &&
+      (Dhcp4Event != Dhcp4SendRequest)) {
+    return EFI_SUCCESS;
+  }
+
+  Private   = (PXEBC_PRIVATE_DATA *) Context;
+  Mode      = Private->PxeBc.Mode;
+  Callback  = Private->PxeBcCallback;
+
+  //
+  // Override the Maximum DHCP Message Size.
+  //
+  MaxMsgSize = PxeBcParseExtendOptions (
+                Packet->Dhcp4.Option,
+                GET_OPTION_BUFFER_LEN (Packet),
+                PXEBC_DHCP4_TAG_MAXMSG
+                );
+  if (MaxMsgSize != NULL) {
+    Value = HTONS (PXEBC_DHCP4_MAX_PACKET_SIZE);
+    CopyMem (MaxMsgSize->Data, &Value, sizeof (Value));
+  }
+
+  if ((Dhcp4Event != Dhcp4SelectOffer) && (Callback != NULL)) {
+    Received = (BOOLEAN) ((Dhcp4Event == Dhcp4RcvdOffer) || (Dhcp4Event == Dhcp4RcvdAck));
+    Status = Callback->Callback (
+                        Callback,
+                        Private->Function,
+                        Received,
+                        Packet->Length,
+                        (EFI_PXE_BASE_CODE_PACKET *) &Packet->Dhcp4
+                        );
+    if (Status != EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE) {
+      return EFI_ABORTED;
+    }
+  }
+
+  Status = EFI_SUCCESS;
+
+  switch (Dhcp4Event) {
+
+  case Dhcp4SendDiscover:
+  case Dhcp4SendRequest:
+    if (Mode->SendGUID) {
+      //
+      // send the system GUID instead of the MAC address as the hardware address
+      // in the DHCP packet header.
+      //
+      DhcpHeader = &Packet->Dhcp4.Header;
+
+      if (EFI_ERROR (NetLibGetSystemGuid ((EFI_GUID *) DhcpHeader->ClientHwAddr))) {
+        //
+        // GUID not yet set - send all 0xff's to show programable (via SetVariable)
+        // SetMem(DHCPV4_OPTIONS_BUFFER.DhcpPlatformId.Guid, sizeof(EFI_GUID), 0xff);
+        // GUID not yet set - send all 0's to show not programable
+        //
+        ZeroMem (DhcpHeader->ClientHwAddr, sizeof (EFI_GUID));
+      }
+
+      DhcpHeader->HwAddrLen = (UINT8) sizeof (EFI_GUID);
+    }
+
+    if (Dhcp4Event == Dhcp4SendDiscover) {
+      //
+      // Cache the dhcp discover packet, of which some information will be used later.
+      //
+      CopyMem (Mode->DhcpDiscover.Raw, &Packet->Dhcp4, Packet->Length);
+    }
+
+    break;
+
+  case Dhcp4RcvdOffer:
+    Status = EFI_NOT_READY;
+    if (Private->NumOffers < PXEBC_MAX_OFFER_NUM) {
+      //
+      // Cache the dhcp offers in Private->Dhcp4Offers[]
+      //
+      PxeBcCacheDhcpOffer (Private, Packet);
+    }
+
+    break;
+
+  case Dhcp4SelectOffer:
+    //
+    // Select an offer, if succeeded, Private->SelectedOffer points to
+    // the index of the selected one.
+    //
+    PxeBcSelectOffer (Private);
+
+    if (Private->SelectedOffer == 0) {
+      Status = EFI_ABORTED;
+    } else {
+      *NewPacket = &Private->Dhcp4Offers[Private->SelectedOffer - 1].Packet.Offer;
+    }
+
+    break;
+
+  case Dhcp4RcvdAck:
+    //
+    // Cache Ack
+    //
+    ASSERT (Private->SelectedOffer != 0);
+
+    PxeBcCopyEfiDhcp4Packet (&Private->Dhcp4Ack.Packet.Ack, Packet);
+    break;
+
+  default:
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Initialize the DHCP options and build the option list.
+
+  @param  Private          Pointer to PxeBc private data.
+  @param  OptList          Pointer to a DHCP option list.
+
+  @param  IsDhcpDiscover   Discover dhcp option or not.
+
+  @return The index item number of the option list.
+
+**/
+UINT32
+PxeBcBuildDhcpOptions (
+  IN PXEBC_PRIVATE_DATA            *Private,
+  IN EFI_DHCP4_PACKET_OPTION       **OptList,
+  IN BOOLEAN                       IsDhcpDiscover
+  )
+{
+  UINT32                    Index;
+  PXEBC_DHCP4_OPTION_ENTRY  OptEnt;
+  UINT16                    Value;
+
+  Index       = 0;
+  OptList[0]  = (EFI_DHCP4_PACKET_OPTION *) Private->OptionBuffer;
+
+  if (!IsDhcpDiscover) {
+    //
+    // Append message type.
+    //
+    OptList[Index]->OpCode  = PXEBC_DHCP4_TAG_MSG_TYPE;
+    OptList[Index]->Length  = 1;
+    OptEnt.Mesg             = (PXEBC_DHCP4_OPTION_MESG *) OptList[Index]->Data;
+    OptEnt.Mesg->Type       = PXEBC_DHCP4_MSG_TYPE_REQUEST;
+    Index++;
+    OptList[Index]          = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+
+    //
+    // Append max message size.
+    //
+    OptList[Index]->OpCode  = PXEBC_DHCP4_TAG_MAXMSG;
+    OptList[Index]->Length  = (UINT8) sizeof (PXEBC_DHCP4_OPTION_MAX_MESG_SIZE);
+    OptEnt.MaxMesgSize      = (PXEBC_DHCP4_OPTION_MAX_MESG_SIZE *) OptList[Index]->Data;
+    Value                   = NTOHS (PXEBC_DHCP4_MAX_PACKET_SIZE);
+    CopyMem (&OptEnt.MaxMesgSize->Size, &Value, sizeof (UINT16));
+    Index++;
+    OptList[Index]          = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+  }
+  //
+  // Parameter request list option.
+  //
+  OptList[Index]->OpCode    = PXEBC_DHCP4_TAG_PARA_LIST;
+  OptList[Index]->Length    = 35;
+  OptEnt.Para               = (PXEBC_DHCP4_OPTION_PARA *) OptList[Index]->Data;
+  OptEnt.Para->ParaList[0]  = PXEBC_DHCP4_TAG_NETMASK;
+  OptEnt.Para->ParaList[1]  = PXEBC_DHCP4_TAG_TIME_OFFSET;
+  OptEnt.Para->ParaList[2]  = PXEBC_DHCP4_TAG_ROUTER;
+  OptEnt.Para->ParaList[3]  = PXEBC_DHCP4_TAG_TIME_SERVER;
+  OptEnt.Para->ParaList[4]  = PXEBC_DHCP4_TAG_NAME_SERVER;
+  OptEnt.Para->ParaList[5]  = PXEBC_DHCP4_TAG_DNS_SERVER;
+  OptEnt.Para->ParaList[6]  = PXEBC_DHCP4_TAG_HOSTNAME;
+  OptEnt.Para->ParaList[7]  = PXEBC_DHCP4_TAG_BOOTFILE_LEN;
+  OptEnt.Para->ParaList[8]  = PXEBC_DHCP4_TAG_DOMAINNAME;
+  OptEnt.Para->ParaList[9]  = PXEBC_DHCP4_TAG_ROOTPATH;
+  OptEnt.Para->ParaList[10] = PXEBC_DHCP4_TAG_EXTEND_PATH;
+  OptEnt.Para->ParaList[11] = PXEBC_DHCP4_TAG_EMTU;
+  OptEnt.Para->ParaList[12] = PXEBC_DHCP4_TAG_TTL;
+  OptEnt.Para->ParaList[13] = PXEBC_DHCP4_TAG_BROADCAST;
+  OptEnt.Para->ParaList[14] = PXEBC_DHCP4_TAG_NIS_DOMAIN;
+  OptEnt.Para->ParaList[15] = PXEBC_DHCP4_TAG_NIS_SERVER;
+  OptEnt.Para->ParaList[16] = PXEBC_DHCP4_TAG_NTP_SERVER;
+  OptEnt.Para->ParaList[17] = PXEBC_DHCP4_TAG_VENDOR;
+  OptEnt.Para->ParaList[18] = PXEBC_DHCP4_TAG_REQUEST_IP;
+  OptEnt.Para->ParaList[19] = PXEBC_DHCP4_TAG_LEASE;
+  OptEnt.Para->ParaList[20] = PXEBC_DHCP4_TAG_SERVER_ID;
+  OptEnt.Para->ParaList[21] = PXEBC_DHCP4_TAG_T1;
+  OptEnt.Para->ParaList[22] = PXEBC_DHCP4_TAG_T2;
+  OptEnt.Para->ParaList[23] = PXEBC_DHCP4_TAG_CLASS_ID;
+  OptEnt.Para->ParaList[24] = PXEBC_DHCP4_TAG_TFTP;
+  OptEnt.Para->ParaList[25] = PXEBC_DHCP4_TAG_BOOTFILE;
+  OptEnt.Para->ParaList[26] = PXEBC_PXE_DHCP4_TAG_UUID;
+  OptEnt.Para->ParaList[27] = 0x80;
+  OptEnt.Para->ParaList[28] = 0x81;
+  OptEnt.Para->ParaList[29] = 0x82;
+  OptEnt.Para->ParaList[30] = 0x83;
+  OptEnt.Para->ParaList[31] = 0x84;
+  OptEnt.Para->ParaList[32] = 0x85;
+  OptEnt.Para->ParaList[33] = 0x86;
+  OptEnt.Para->ParaList[34] = 0x87;
+  Index++;
+  OptList[Index]            = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+
+  //
+  // Append UUID/Guid-based client identifier option
+  //
+  OptList[Index]->OpCode  = PXEBC_PXE_DHCP4_TAG_UUID;
+  OptList[Index]->Length  = (UINT8) sizeof (PXEBC_DHCP4_OPTION_UUID);
+  OptEnt.Uuid             = (PXEBC_DHCP4_OPTION_UUID *) OptList[Index]->Data;
+  OptEnt.Uuid->Type       = 0;
+  Index++;
+  OptList[Index]          = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+
+  if (EFI_ERROR (NetLibGetSystemGuid ((EFI_GUID *) OptEnt.Uuid->Guid))) {
+    //
+    // GUID not yet set - send all 0xff's to show programable (via SetVariable)
+    // SetMem(DHCPV4_OPTIONS_BUFFER.DhcpPlatformId.Guid, sizeof(EFI_GUID), 0xff);
+    // GUID not yet set - send all 0's to show not programable
+    //
+    ZeroMem (OptEnt.Uuid->Guid, sizeof (EFI_GUID));
+  }
+
+  //
+  // Append client network device interface option
+  //
+  OptList[Index]->OpCode  = PXEBC_PXE_DHCP4_TAG_UNDI;
+  OptList[Index]->Length  = (UINT8) sizeof (PXEBC_DHCP4_OPTION_UNDI);
+  OptEnt.Undi             = (PXEBC_DHCP4_OPTION_UNDI *) OptList[Index]->Data;
+  if (Private->Nii != NULL) {
+    OptEnt.Undi->Type       = Private->Nii->Type;
+    OptEnt.Undi->MajorVer   = Private->Nii->MajorVer;
+    OptEnt.Undi->MinorVer   = Private->Nii->MinorVer;
+  } else {
+    OptEnt.Undi->Type       = DEFAULT_UNDI_TYPE;
+    OptEnt.Undi->MajorVer   = DEFAULT_UNDI_MAJOR;
+    OptEnt.Undi->MinorVer   = DEFAULT_UNDI_MINOR;
+  }
+
+  Index++;
+  OptList[Index] = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+
+  //
+  // Append client system architecture option
+  //
+  OptList[Index]->OpCode  = PXEBC_PXE_DHCP4_TAG_ARCH;
+  OptList[Index]->Length  = (UINT8) sizeof (PXEBC_DHCP4_OPTION_ARCH);
+  OptEnt.Arch             = (PXEBC_DHCP4_OPTION_ARCH *) OptList[Index]->Data;
+  Value                   = HTONS (EFI_PXE_CLIENT_SYSTEM_ARCHITECTURE);
+  CopyMem (&OptEnt.Arch->Type, &Value, sizeof (UINT16));
+  Index++;
+  OptList[Index]          = GET_NEXT_DHCP_OPTION (OptList[Index - 1]);
+
+  //
+  // Append client system architecture option
+  //
+  OptList[Index]->OpCode  = PXEBC_DHCP4_TAG_CLASS_ID;
+  OptList[Index]->Length  = (UINT8) sizeof (PXEBC_DHCP4_OPTION_CLID);
+  OptEnt.Clid             = (PXEBC_DHCP4_OPTION_CLID *) OptList[Index]->Data;
+  CopyMem (OptEnt.Clid, DEFAULT_CLASS_ID_DATA, sizeof (PXEBC_DHCP4_OPTION_CLID));
+  CvtNum (EFI_PXE_CLIENT_SYSTEM_ARCHITECTURE, OptEnt.Clid->ArchitectureType, sizeof (OptEnt.Clid->ArchitectureType));
+
+  if (Private->Nii != NULL) {
+    //
+    // If NII protocol exists, update DHCP option data
+    //
+    CopyMem (OptEnt.Clid->InterfaceName, Private->Nii->StringId, sizeof (OptEnt.Clid->InterfaceName));
+    CvtNum (Private->Nii->MajorVer, OptEnt.Clid->UndiMajor, sizeof (OptEnt.Clid->UndiMajor));
+    CvtNum (Private->Nii->MinorVer, OptEnt.Clid->UndiMinor, sizeof (OptEnt.Clid->UndiMinor));
+  }
+
+  Index++;
+
+  return Index;
+}
+
+
+/**
+  Discover the boot of service and initialize the vendor option if exists.
+
+  @param  Private               Pointer to PxeBc private data.
+  @param  Type                  PxeBc option boot item type
+  @param  Layer                 PxeBc option boot item layer
+  @param  UseBis                Use BIS or not
+  @param  DestIp                Ip address for server
+  @param  IpCount               The total count of the server ip address
+  @param  SrvList               Server list
+  @param  IsDiscv               Discover the vendor or not
+  @param  Reply                 The dhcp4 packet of Pxe reply
+
+  @retval EFI_SUCCESS           Operation succeeds.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory pool failed.
+  @retval EFI_NOT_FOUND         There is no vendor option exists.
+  @retval EFI_TIMEOUT           Send Pxe Discover time out.
+
+**/
+EFI_STATUS
+PxeBcDiscvBootService (
+  IN PXEBC_PRIVATE_DATA                * Private,
+  IN UINT16                            Type,
+  IN UINT16                            *Layer,
+  IN BOOLEAN                           UseBis,
+  IN EFI_IP_ADDRESS                    * DestIp,
+  IN UINT16                            IpCount,
+  IN EFI_PXE_BASE_CODE_SRVLIST         * SrvList,
+  IN BOOLEAN                           IsDiscv,
+  OUT EFI_DHCP4_PACKET                 * Reply OPTIONAL
+  )
+{
+  EFI_PXE_BASE_CODE_UDP_PORT          Sport;
+  EFI_PXE_BASE_CODE_MODE              *Mode;
+  EFI_DHCP4_PROTOCOL                  *Dhcp4;
+  EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN    Token;
+  BOOLEAN                             IsBCast;
+  EFI_STATUS                          Status;
+  UINT16                              RepIndex;
+  UINT16                              SrvIndex;
+  UINT16                              TryIndex;
+  EFI_DHCP4_LISTEN_POINT              ListenPoint;
+  EFI_DHCP4_PACKET                    *Response;
+  EFI_DHCP4_PACKET_OPTION             *OptList[PXEBC_DHCP4_MAX_OPTION_NUM];
+  UINT32                              OptCount;
+  EFI_DHCP4_PACKET_OPTION             *PxeOpt;
+  PXEBC_OPTION_BOOT_ITEM              *PxeBootItem;
+  UINT8                               VendorOptLen;
+  EFI_DHCP4_HEADER                    *DhcpHeader;
+  UINT32                              Xid;
+
+  Mode      = Private->PxeBc.Mode;
+  Dhcp4     = Private->Dhcp4;
+  Status    = EFI_SUCCESS;
+
+  ZeroMem (&Token, sizeof (EFI_DHCP4_TRANSMIT_RECEIVE_TOKEN));
+
+  if (DestIp == NULL) {
+    Sport   = PXEBC_DHCP4_S_PORT;
+    IsBCast = TRUE;
+  } else {
+    Sport   = PXEBC_BS_DISCOVER_PORT;
+    IsBCast = FALSE;
+  }
+
+  if (!UseBis && Layer != NULL) {
+    *Layer &= EFI_PXE_BASE_CODE_BOOT_LAYER_MASK;
+  }
+
+  OptCount = PxeBcBuildDhcpOptions (Private, OptList, FALSE);
+
+  if (IsDiscv) {
+    ASSERT (Layer != NULL);
+    //
+    // Add vendor option of PXE_BOOT_ITEM
+    //
+    VendorOptLen = (UINT8) ((sizeof (EFI_DHCP4_PACKET_OPTION) - 1) * 2 + sizeof (PXEBC_OPTION_BOOT_ITEM) + 1);
+    OptList[OptCount] = AllocatePool (VendorOptLen);
+    if (OptList[OptCount] == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+
+    OptList[OptCount]->OpCode     = PXEBC_DHCP4_TAG_VENDOR;
+    OptList[OptCount]->Length     = (UINT8) (VendorOptLen - 2);
+    PxeOpt                        = (EFI_DHCP4_PACKET_OPTION *) OptList[OptCount]->Data;
+    PxeOpt->OpCode                = PXEBC_VENDOR_TAG_BOOT_ITEM;
+    PxeOpt->Length                = (UINT8) sizeof (PXEBC_OPTION_BOOT_ITEM);
+    PxeBootItem                   = (PXEBC_OPTION_BOOT_ITEM *) PxeOpt->Data;
+    PxeBootItem->Type             = HTONS (Type);
+    PxeBootItem->Layer            = HTONS (*Layer);
+    PxeOpt->Data[PxeOpt->Length]  = PXEBC_DHCP4_TAG_EOP;
+
+    OptCount++;
+  }
+
+  Status = Dhcp4->Build (Dhcp4, &Private->SeedPacket, 0, NULL, OptCount, OptList, &Token.Packet);
+
+  if (IsDiscv) {
+    FreePool (OptList[OptCount - 1]);
+  }
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  DhcpHeader = &Token.Packet->Dhcp4.Header;
+  if (Mode->SendGUID) {
+    if (EFI_ERROR (NetLibGetSystemGuid ((EFI_GUID *) DhcpHeader->ClientHwAddr))) {
+      //
+      // GUID not yet set - send all 0's to show not programable
+      //
+      ZeroMem (DhcpHeader->ClientHwAddr, sizeof (EFI_GUID));
+    }
+
+    DhcpHeader->HwAddrLen = (UINT8) sizeof (EFI_GUID);
+  }
+
+  Xid                                 = NET_RANDOM (NetRandomInitSeed ());
+  Token.Packet->Dhcp4.Header.Xid      = HTONL(Xid);
+  Token.Packet->Dhcp4.Header.Reserved = HTONS((UINT16) ((IsBCast) ? 0x8000 : 0));
+  CopyMem (&Token.Packet->Dhcp4.Header.ClientAddr, &Private->StationIp, sizeof (EFI_IPv4_ADDRESS));
+
+  Token.RemotePort = Sport;
+
+  if (IsBCast) {
+    SetMem (&Token.RemoteAddress, sizeof (EFI_IPv4_ADDRESS), 0xff);
+  } else {
+    CopyMem (&Token.RemoteAddress, DestIp, sizeof (EFI_IPv4_ADDRESS));
+  }
+
+  CopyMem (&Token.GatewayAddress, &Private->GatewayIp, sizeof (EFI_IPv4_ADDRESS));
+
+  if (!IsBCast) {
+    Token.ListenPointCount            = 1;
+    Token.ListenPoints                = &ListenPoint;
+    Token.ListenPoints[0].ListenPort  = PXEBC_BS_DISCOVER_PORT;
+    CopyMem (&Token.ListenPoints[0].ListenAddress, &Private->StationIp, sizeof(EFI_IPv4_ADDRESS));
+    CopyMem (&Token.ListenPoints[0].SubnetMask, &Private->SubnetMask, sizeof(EFI_IPv4_ADDRESS));
+  }
+  //
+  // Send Pxe Discover
+  //
+  for (TryIndex = 1; TryIndex <= PXEBC_BOOT_REQUEST_RETRIES; TryIndex++) {
+
+    Token.TimeoutValue                  = (UINT16) (PXEBC_BOOT_REQUEST_TIMEOUT * TryIndex);
+    Token.Packet->Dhcp4.Header.Seconds  = (UINT16) (PXEBC_BOOT_REQUEST_TIMEOUT * (TryIndex - 1));
+
+    Status              = Dhcp4->TransmitReceive (Dhcp4, &Token);
+
+    if (Token.Status != EFI_TIMEOUT) {
+      break;
+    }
+  }
+
+  if (TryIndex > PXEBC_BOOT_REQUEST_RETRIES) {
+    //
+    // No server response our PXE request
+    //
+    Status = EFI_TIMEOUT;
+  }
+
+  if (!EFI_ERROR (Status)) {
+    //
+    // Find Pxe Reply
+    //
+    RepIndex  = 0;
+    SrvIndex  = 0;
+    Response  = Token.ResponseList;
+
+    while (RepIndex < Token.ResponseCount) {
+
+      while (SrvIndex < IpCount) {
+
+        if (SrvList[SrvIndex].AcceptAnyResponse) {
+          break;
+        }
+
+        if ((SrvList[SrvIndex].Type == Type) && EFI_IP4_EQUAL (&(Response->Dhcp4.Header.ServerAddr), &(Private->ServerIp))) {
+          break;
+        }
+
+        SrvIndex++;
+      }
+
+      if ((IpCount != SrvIndex) || (IpCount == 0)) {
+        break;
+      }
+
+      SrvIndex = 0;
+      RepIndex++;
+
+      Response = (EFI_DHCP4_PACKET *) ((UINT8 *) Response + Response->Size);
+    }
+
+    if (RepIndex < Token.ResponseCount) {
+
+      if (Reply != NULL) {
+        PxeBcCopyEfiDhcp4Packet (Reply, Response);
+      }
+
+      if (IsDiscv) {
+        CopyMem (&(Mode->PxeDiscover), &(Token.Packet->Dhcp4), Token.Packet->Length);
+        Mode->PxeDiscoverValid = TRUE;
+
+        CopyMem (Mode->PxeReply.Raw, &Response->Dhcp4, Response->Length);
+        Mode->PxeReplyReceived = TRUE;
+      }
+    } else {
+      Status = EFI_NOT_FOUND;
+    }
+
+    //
+    // free the responselist
+    //
+    if (Token.ResponseList != NULL) {
+      FreePool (Token.ResponseList);
+    }
+  }
+  //
+  // Free the dhcp packet
+  //
+  FreePool (Token.Packet);
+
+  return Status;
+}
+
+
+/**
+  Parse interested dhcp options.
+
+  @param  Buffer     Pointer to the dhcp options packet.
+  @param  Length     The length of the dhcp options.
+  @param  OptTag     The option OpCode.
+
+  @return NULL if the buffer length is 0 and OpCode is not
+          PXEBC_DHCP4_TAG_EOP, or the pointer to the buffer.
+
+**/
+EFI_DHCP4_PACKET_OPTION *
+PxeBcParseExtendOptions (
+  IN UINT8                         *Buffer,
+  IN UINT32                        Length,
+  IN UINT8                         OptTag
+  )
+{
+  EFI_DHCP4_PACKET_OPTION *Option;
+  UINT32                  Offset;
+
+  Option  = (EFI_DHCP4_PACKET_OPTION *) Buffer;
+  Offset  = 0;
+
+  while (Offset < Length && Option->OpCode != PXEBC_DHCP4_TAG_EOP) {
+
+    if (Option->OpCode == OptTag) {
+
+      return Option;
+    }
+
+    if (Option->OpCode == PXEBC_DHCP4_TAG_PAD) {
+      Offset++;
+    } else {
+      Offset += Option->Length + 2;
+    }
+
+    Option = (EFI_DHCP4_PACKET_OPTION *) (Buffer + Offset);
+  }
+
+  return NULL;
+}
+
+
+/**
+  This function is to parse and check vendor options.
+
+  @param  Dhcp4Option           Pointer to dhcp options
+  @param  VendorOption          Pointer to vendor options
+
+  @return TRUE if valid for vendor options, or FALSE.
+
+**/
+BOOLEAN
+PxeBcParseVendorOptions (
+  IN EFI_DHCP4_PACKET_OPTION       *Dhcp4Option,
+  IN PXEBC_VENDOR_OPTION           *VendorOption
+  )
+{
+  UINT32                  *BitMap;
+  UINT8                   VendorOptionLen;
+  EFI_DHCP4_PACKET_OPTION *PxeOption;
+  UINT8                   Offset;
+
+  BitMap          = VendorOption->BitMap;
+  VendorOptionLen = Dhcp4Option->Length;
+  PxeOption       = (EFI_DHCP4_PACKET_OPTION *) &Dhcp4Option->Data[0];
+  Offset          = 0;
+
+  while ((Offset < VendorOptionLen) && (PxeOption->OpCode != PXEBC_DHCP4_TAG_EOP)) {
+    //
+    // Parse every Vendor Option and set its BitMap
+    //
+    switch (PxeOption->OpCode) {
+
+    case PXEBC_VENDOR_TAG_MTFTP_IP:
+
+      CopyMem (&VendorOption->MtftpIp, PxeOption->Data, sizeof (EFI_IPv4_ADDRESS));
+      break;
+
+    case PXEBC_VENDOR_TAG_MTFTP_CPORT:
+
+      CopyMem (&VendorOption->MtftpCPort, PxeOption->Data, sizeof (VendorOption->MtftpCPort));
+      break;
+
+    case PXEBC_VENDOR_TAG_MTFTP_SPORT:
+
+      CopyMem (&VendorOption->MtftpSPort, PxeOption->Data, sizeof (VendorOption->MtftpSPort));
+      break;
+
+    case PXEBC_VENDOR_TAG_MTFTP_TIMEOUT:
+
+      VendorOption->MtftpTimeout = *PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_MTFTP_DELAY:
+
+      VendorOption->MtftpDelay = *PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_DISCOVER_CTRL:
+
+      VendorOption->DiscoverCtrl = *PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_DISCOVER_MCAST:
+
+      CopyMem (&VendorOption->DiscoverMcastIp, PxeOption->Data, sizeof (EFI_IPv4_ADDRESS));
+      break;
+
+    case PXEBC_VENDOR_TAG_BOOT_SERVERS:
+
+      VendorOption->BootSvrLen  = PxeOption->Length;
+      VendorOption->BootSvr     = (PXEBC_BOOT_SVR_ENTRY *) PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_BOOT_MENU:
+
+      VendorOption->BootMenuLen = PxeOption->Length;
+      VendorOption->BootMenu    = (PXEBC_BOOT_MENU_ENTRY *) PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_MENU_PROMPT:
+
+      VendorOption->MenuPromptLen = PxeOption->Length;
+      VendorOption->MenuPrompt    = (PXEBC_MENU_PROMPT *) PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_MCAST_ALLOC:
+
+      CopyMem (&VendorOption->McastIpBase, PxeOption->Data, sizeof (EFI_IPv4_ADDRESS));
+      CopyMem (&VendorOption->McastIpBlock, PxeOption->Data + 4, sizeof (VendorOption->McastIpBlock));
+      CopyMem (&VendorOption->McastIpRange, PxeOption->Data + 6, sizeof (VendorOption->McastIpRange));
+      break;
+
+    case PXEBC_VENDOR_TAG_CREDENTIAL_TYPES:
+
+      VendorOption->CredTypeLen = PxeOption->Length;
+      VendorOption->CredType    = (UINT32 *) PxeOption->Data;
+      break;
+
+    case PXEBC_VENDOR_TAG_BOOT_ITEM:
+
+      CopyMem (&VendorOption->BootSrvType, PxeOption->Data, sizeof (VendorOption->BootSrvType));
+      CopyMem (&VendorOption->BootSrvLayer, PxeOption->Data + 2, sizeof (VendorOption->BootSrvLayer));
+      break;
+    }
+
+    SET_VENDOR_OPTION_BIT_MAP (BitMap, PxeOption->OpCode);
+
+    if (PxeOption->OpCode == PXEBC_DHCP4_TAG_PAD) {
+      Offset++;
+    } else {
+      Offset = (UINT8) (Offset + PxeOption->Length + 2);
+    }
+
+    PxeOption = (EFI_DHCP4_PACKET_OPTION *) (Dhcp4Option->Data + Offset);
+  }
+
+  //
+  // FixMe, return falas if invalid of any vendor option
+  //
+
+  return TRUE;
+}
+
+
+/**
+  This function display boot item detail.
+
+  If the length of the boot item string over 70 Char, just display 70 Char.
+
+  @param  Str     Pointer to a string (boot item string).
+  @param  Len     The length of string.
+
+**/
+VOID
+PxeBcDisplayBootItem (
+  IN UINT8                 *Str,
+  IN UINT8                 Len
+  )
+{
+  UINT8 Tmp;
+
+  Len       = (UINT8) MIN (70, Len);
+  Tmp       = Str[Len];
+  Str[Len]  = 0;
+  AsciiPrint ("%a \n", Str);
+  Str[Len] = Tmp;
+}
+
+
+/**
+  Choose the boot prompt.
+
+  @param  Private              Pointer to PxeBc private data.
+
+  @retval EFI_SUCCESS          Select boot prompt done.
+  @retval EFI_TIMEOUT          Select boot prompt time out.
+  @retval EFI_NOT_FOUND        The proxy offer is not Pxe10.
+  @retval EFI_ABORTED          User cancel the operation.
+  @retval EFI_NOT_READY        Read the input key from the keybroad has not finish.
+
+**/
+EFI_STATUS
+PxeBcSelectBootPrompt (
+  IN PXEBC_PRIVATE_DATA              *Private
+  )
+{
+  PXEBC_CACHED_DHCP4_PACKET  *Packet;
+  PXEBC_VENDOR_OPTION       *VendorOpt;
+  EFI_EVENT                  TimeoutEvent;
+  EFI_EVENT                  DescendEvent;
+  EFI_INPUT_KEY              InputKey;
+  EFI_STATUS                 Status;
+  UINT8                      Timeout;
+  UINT8                      *Prompt;
+  UINT8                      PromptLen;
+  INT32                      SecCol;
+  INT32                      SecRow;
+
+  TimeoutEvent  = NULL;
+  DescendEvent  = NULL;
+
+  if (Private->PxeBc.Mode->ProxyOfferReceived) {
+
+    Packet  = &Private->ProxyOffer;
+  } else {
+
+    Packet  = &Private->Dhcp4Ack;
+  }
+
+  if (Packet->OfferType != DHCP4_PACKET_TYPE_PXE10) {
+    return EFI_NOT_FOUND;
+  }
+
+  VendorOpt = &Packet->PxeVendorOption;
+  //
+  // According to the PXE specification 2.1, Table 2-1 PXE DHCP Options  (Full  
+  // List), we must not consider a boot prompt or boot menu if all of the  
+  // following hold:
+  // - the PXE_DISCOVERY_CONTROL PXE tag is present inside the Vendor Options
+  //   (=43) DHCP tag, and
+  // - the PXE_DISCOVERY_CONTROL PXE tag has bit 3 set, and  
+  // - a boot file name has been presented with DHCP option 67.
+  //
+  if (IS_DISABLE_PROMPT_MENU (VendorOpt->DiscoverCtrl) &&
+      Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] != NULL) {
+    return EFI_ABORTED;
+  }
+
+  if (!IS_VALID_BOOT_PROMPT (VendorOpt->BitMap)) {
+    return EFI_SUCCESS;
+  }
+
+  Timeout   = VendorOpt->MenuPrompt->Timeout;
+  Prompt    = VendorOpt->MenuPrompt->Prompt;
+  PromptLen = (UINT8) (VendorOpt->MenuPromptLen - 1);
+
+  if (Timeout == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (Timeout == 255) {
+    return EFI_TIMEOUT;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &TimeoutEvent
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->SetTimer (
+                  TimeoutEvent,
+                  TimerRelative,
+                  Timeout * TICKS_PER_SECOND
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = gBS->CreateEvent (
+                  EVT_TIMER,
+                  TPL_CALLBACK,
+                  NULL,
+                  NULL,
+                  &DescendEvent
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = gBS->SetTimer (
+                  DescendEvent,
+                  TimerPeriodic,
+                  TICKS_PER_SECOND
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  SecCol = gST->ConOut->Mode->CursorColumn;
+  SecRow = gST->ConOut->Mode->CursorRow;
+
+  PxeBcDisplayBootItem (Prompt, PromptLen);
+
+  gST->ConOut->SetCursorPosition (gST->ConOut, SecCol + PromptLen, SecRow);
+  AsciiPrint ("(%d) ", Timeout--);
+
+  while (EFI_ERROR (gBS->CheckEvent (TimeoutEvent))) {
+
+    if (!EFI_ERROR (gBS->CheckEvent (DescendEvent))) {
+      gST->ConOut->SetCursorPosition (gST->ConOut, SecCol + PromptLen, SecRow);
+      AsciiPrint ("(%d) ", Timeout--);
+    }
+
+    if (gST->ConIn->ReadKeyStroke (gST->ConIn, &InputKey) == EFI_NOT_READY) {
+
+      gBS->Stall (10 * TICKS_PER_MS);
+      continue;
+    }
+
+    if (InputKey.ScanCode == 0) {
+
+      switch (InputKey.UnicodeChar) {
+      case CTRL ('c'):
+        Status = EFI_ABORTED;
+        break;
+
+      case CTRL ('m'):
+      case 'm':
+      case 'M':
+        Status = EFI_TIMEOUT;
+        break;
+
+      default:
+        continue;
+      }
+    } else {
+
+      switch (InputKey.ScanCode) {
+      case SCAN_F8:
+        Status = EFI_TIMEOUT;
+        break;
+
+      case SCAN_ESC:
+        Status = EFI_ABORTED;
+        break;
+
+      default:
+        continue;
+      }
+    }
+
+    break;
+  }
+
+  gST->ConOut->SetCursorPosition (gST->ConOut, 0 , SecRow + 1);
+
+ON_EXIT:
+
+  if (DescendEvent != NULL) {
+    gBS->CloseEvent (DescendEvent);
+  }
+
+  if (TimeoutEvent != NULL) {
+    gBS->CloseEvent (TimeoutEvent);
+  }
+
+  return Status;
+}
+
+
+/**
+  Select the boot menu.
+
+  @param  Private         Pointer to PxeBc private data.
+  @param  Type            The type of the menu.
+  @param  UseDefaultItem  Use default item or not.
+
+  @retval EFI_ABORTED     User cancel operation.
+  @retval EFI_SUCCESS     Select the boot menu success.
+  @retval EFI_NOT_READY   Read the input key from the keybroad has not finish.
+
+**/
+EFI_STATUS
+PxeBcSelectBootMenu (
+  IN  PXEBC_PRIVATE_DATA              *Private,
+  OUT UINT16                          *Type,
+  IN  BOOLEAN                         UseDefaultItem
+  )
+{
+  PXEBC_CACHED_DHCP4_PACKET  *Packet;
+  PXEBC_VENDOR_OPTION        *VendorOpt;
+  EFI_INPUT_KEY              InputKey;
+  UINT8                      MenuSize;
+  UINT8                      MenuNum;
+  INT32                      TopRow;
+  UINT16                     Select;
+  UINT16                     LastSelect;
+  UINT8                      Index;
+  BOOLEAN                    Finish;
+  CHAR8                      Blank[70];
+  PXEBC_BOOT_MENU_ENTRY      *MenuItem;
+  PXEBC_BOOT_MENU_ENTRY      *MenuArray[PXEBC_MAX_MENU_NUM];
+
+  Finish  = FALSE;
+  Select  = 1;
+  Index   = 0;
+  *Type   = 0;
+
+  if (Private->PxeBc.Mode->ProxyOfferReceived) {
+
+    Packet  = &Private->ProxyOffer;
+  } else {
+
+    Packet  = &Private->Dhcp4Ack;
+  }
+
+  ASSERT (Packet->OfferType == DHCP4_PACKET_TYPE_PXE10);
+
+  VendorOpt = &Packet->PxeVendorOption;
+
+  if (!IS_VALID_BOOT_MENU (VendorOpt->BitMap)) {
+    return EFI_SUCCESS;
+  }
+
+  SetMem (Blank, sizeof(Blank), ' ');
+
+  MenuSize  = VendorOpt->BootMenuLen;
+  MenuItem  = VendorOpt->BootMenu;
+
+  if (MenuSize == 0) {
+    return EFI_NOT_READY;
+  }
+
+  while (MenuSize > 0) {
+    MenuArray[Index++]  = MenuItem;
+    MenuSize          = (UINT8) (MenuSize - (MenuItem->DescLen + 3));
+    MenuItem          = (PXEBC_BOOT_MENU_ENTRY *) ((UINT8 *) MenuItem + MenuItem->DescLen + 3);
+    if (Index >= PXEBC_MAX_MENU_NUM) {
+      break;
+    }
+  }
+
+  if (UseDefaultItem) {
+    *Type = MenuArray[0]->Type;
+    *Type = NTOHS (*Type);
+    return EFI_SUCCESS;
+  }
+
+  MenuNum = Index;
+
+  for (Index = 0; Index < MenuNum; Index++) {
+    PxeBcDisplayBootItem (MenuArray[Index]->DescStr, MenuArray[Index]->DescLen);
+  }
+
+  TopRow  = gST->ConOut->Mode->CursorRow - MenuNum;
+
+  do {
+    ASSERT (Select < PXEBC_MAX_MENU_NUM);
+    //
+    // highlight selected row
+    //
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_BLACK, EFI_LIGHTGRAY));
+    gST->ConOut->SetCursorPosition (gST->ConOut, 0, TopRow + Select);
+    Blank[MenuArray[Select]->DescLen] = 0;
+    AsciiPrint ("%a\r", Blank);
+    PxeBcDisplayBootItem (MenuArray[Select]->DescStr, MenuArray[Select]->DescLen);
+    gST->ConOut->SetCursorPosition (gST->ConOut, 0, TopRow + MenuNum);
+    LastSelect = Select;
+
+    while (gST->ConIn->ReadKeyStroke (gST->ConIn, &InputKey) == EFI_NOT_READY) {
+      gBS->Stall (10 * TICKS_PER_MS);
+    }
+
+    if (InputKey.ScanCode != 0) {
+      switch (InputKey.UnicodeChar) {
+      case CTRL ('c'):
+        InputKey.ScanCode = SCAN_ESC;
+        break;
+
+      case CTRL ('j'):  /* linefeed */
+      case CTRL ('m'):  /* return */
+        Finish = TRUE;
+        break;
+
+      case CTRL ('i'):  /* tab */
+      case ' ':
+      case 'd':
+      case 'D':
+        InputKey.ScanCode = SCAN_DOWN;
+        break;
+
+      case CTRL ('h'):  /* backspace */
+      case 'u':
+      case 'U':
+        InputKey.ScanCode = SCAN_UP;
+        break;
+
+      default:
+        InputKey.ScanCode = 0;
+      }
+    }
+
+    switch (InputKey.ScanCode) {
+    case SCAN_LEFT:
+    case SCAN_UP:
+      if (Select > 0) {
+        --Select;
+      }
+
+      break;
+
+    case SCAN_DOWN:
+    case SCAN_RIGHT:
+      if (++Select == MenuNum) {
+        --Select;
+      }
+
+      break;
+
+    case SCAN_PAGE_UP:
+    case SCAN_HOME:
+      Select = 0;
+      break;
+
+    case SCAN_PAGE_DOWN:
+    case SCAN_END:
+      Select = (UINT16) (MenuNum - 1);
+      break;
+
+    case SCAN_ESC:
+      return EFI_ABORTED;
+    }
+
+    /* unhighlight last selected row */
+    gST->ConOut->SetAttribute (gST->ConOut, EFI_TEXT_ATTR (EFI_LIGHTGRAY, EFI_BLACK));
+    gST->ConOut->SetCursorPosition (gST->ConOut, 0, TopRow + LastSelect);
+    Blank[MenuArray[LastSelect]->DescLen] = 0;
+    AsciiPrint ("%a\r", Blank);
+    PxeBcDisplayBootItem (MenuArray[LastSelect]->DescStr, MenuArray[LastSelect]->DescLen);
+    gST->ConOut->SetCursorPosition (gST->ConOut, 0, TopRow + MenuNum);
+  } while (!Finish);
+
+   ASSERT (Select < PXEBC_MAX_MENU_NUM);
+
+  //
+  // Swap the byte order
+  //
+  CopyMem (Type, &MenuArray[Select]->Type, sizeof (UINT16));
+  *Type = NTOHS (*Type);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.h b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.h
new file mode 100644
index 0000000000..1626060ee2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDhcp.h
@@ -0,0 +1,540 @@
+/** @file
+  Dhcp and Discover routines for PxeBc.
+
+Copyright (c) 2013, Red Hat, Inc.
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef __EFI_PXEBC_DHCP_H__
+#define __EFI_PXEBC_DHCP_H__
+
+#define PXEBC_DHCP4_MAX_OPTION_NUM         16
+#define PXEBC_DHCP4_MAX_OPTION_SIZE        312
+#define PXEBC_DHCP4_MAX_PACKET_SIZE        1472
+
+#define PXEBC_DHCP4_S_PORT                 67
+#define PXEBC_DHCP4_C_PORT                 68
+#define PXEBC_BS_DOWNLOAD_PORT             69
+#define PXEBC_BS_DISCOVER_PORT             4011
+
+#define PXEBC_DHCP4_OPCODE_REQUEST         1
+#define PXEBC_DHCP4_OPCODE_REPLY           2
+#define PXEBC_DHCP4_MSG_TYPE_REQUEST       3
+#define PXEBC_DHCP4_MAGIC                  0x63538263 // network byte order
+//
+// Dhcp Options
+//
+#define PXEBC_DHCP4_TAG_PAD                0    // Pad Option
+#define PXEBC_DHCP4_TAG_EOP                255  // End Option
+#define PXEBC_DHCP4_TAG_NETMASK            1    // Subnet Mask
+#define PXEBC_DHCP4_TAG_TIME_OFFSET        2    // Time Offset from UTC
+#define PXEBC_DHCP4_TAG_ROUTER             3    // Router option,
+#define PXEBC_DHCP4_TAG_TIME_SERVER        4    // Time Server
+#define PXEBC_DHCP4_TAG_NAME_SERVER        5    // Name Server
+#define PXEBC_DHCP4_TAG_DNS_SERVER         6    // Domain Name Server
+#define PXEBC_DHCP4_TAG_HOSTNAME           12   // Host Name
+#define PXEBC_DHCP4_TAG_BOOTFILE_LEN       13   // Boot File Size
+#define PXEBC_DHCP4_TAG_DUMP               14   // Merit Dump File
+#define PXEBC_DHCP4_TAG_DOMAINNAME         15   // Domain Name
+#define PXEBC_DHCP4_TAG_ROOTPATH           17   // Root path
+#define PXEBC_DHCP4_TAG_EXTEND_PATH        18   // Extensions Path
+#define PXEBC_DHCP4_TAG_EMTU               22   // Maximum Datagram Reassembly Size
+#define PXEBC_DHCP4_TAG_TTL                23   // Default IP Time-to-live
+#define PXEBC_DHCP4_TAG_BROADCAST          28   // Broadcast Address
+#define PXEBC_DHCP4_TAG_NIS_DOMAIN         40   // Network Information Service Domain
+#define PXEBC_DHCP4_TAG_NIS_SERVER         41   // Network Information Servers
+#define PXEBC_DHCP4_TAG_NTP_SERVER         42   // Network Time Protocol Servers
+#define PXEBC_DHCP4_TAG_VENDOR             43   // Vendor Specific Information
+#define PXEBC_DHCP4_TAG_REQUEST_IP         50   // Requested IP Address
+#define PXEBC_DHCP4_TAG_LEASE              51   // IP Address Lease Time
+#define PXEBC_DHCP4_TAG_OVERLOAD           52   // Option Overload
+#define PXEBC_DHCP4_TAG_MSG_TYPE           53   // DHCP Message Type
+#define PXEBC_DHCP4_TAG_SERVER_ID          54   // Server Identifier
+#define PXEBC_DHCP4_TAG_PARA_LIST          55   // Parameter Request List
+#define PXEBC_DHCP4_TAG_MAXMSG             57   // Maximum DHCP Message Size
+#define PXEBC_DHCP4_TAG_T1                 58   // Renewal (T1) Time Value
+#define PXEBC_DHCP4_TAG_T2                 59   // Rebinding (T2) Time Value
+#define PXEBC_DHCP4_TAG_CLASS_ID           60   // Vendor class identifier
+#define PXEBC_DHCP4_TAG_CLIENT_ID          61   // Client-identifier
+#define PXEBC_DHCP4_TAG_TFTP               66   // TFTP server name
+#define PXEBC_DHCP4_TAG_BOOTFILE           67   // Bootfile name
+#define PXEBC_PXE_DHCP4_TAG_ARCH           93
+#define PXEBC_PXE_DHCP4_TAG_UNDI           94
+#define PXEBC_PXE_DHCP4_TAG_UUID           97
+//
+// Sub-Options in Dhcp Vendor Option
+//
+#define PXEBC_VENDOR_TAG_MTFTP_IP          1
+#define PXEBC_VENDOR_TAG_MTFTP_CPORT       2
+#define PXEBC_VENDOR_TAG_MTFTP_SPORT       3
+#define PXEBC_VENDOR_TAG_MTFTP_TIMEOUT     4
+#define PXEBC_VENDOR_TAG_MTFTP_DELAY       5
+#define PXEBC_VENDOR_TAG_DISCOVER_CTRL     6
+#define PXEBC_VENDOR_TAG_DISCOVER_MCAST    7
+#define PXEBC_VENDOR_TAG_BOOT_SERVERS      8
+#define PXEBC_VENDOR_TAG_BOOT_MENU         9
+#define PXEBC_VENDOR_TAG_MENU_PROMPT       10
+#define PXEBC_VENDOR_TAG_MCAST_ALLOC       11
+#define PXEBC_VENDOR_TAG_CREDENTIAL_TYPES  12
+#define PXEBC_VENDOR_TAG_BOOT_ITEM         71
+
+#define PXEBC_DHCP4_DISCOVER_INIT_TIMEOUT  4
+#define PXEBC_DHCP4_DISCOVER_RETRIES       4
+
+#define PXEBC_MAX_MENU_NUM                 24
+#define PXEBC_MAX_OFFER_NUM                16
+
+#define PXEBC_BOOT_REQUEST_TIMEOUT         1
+#define PXEBC_BOOT_REQUEST_RETRIES         4
+
+#define PXEBC_DHCP4_OVERLOAD_FILE          1
+#define PXEBC_DHCP4_OVERLOAD_SERVER_NAME   2
+
+//
+// The array index of the DHCP4 option tag interested
+//
+#define PXEBC_DHCP4_TAG_INDEX_BOOTFILE_LEN 0
+#define PXEBC_DHCP4_TAG_INDEX_VENDOR       1
+#define PXEBC_DHCP4_TAG_INDEX_OVERLOAD     2
+#define PXEBC_DHCP4_TAG_INDEX_MSG_TYPE     3
+#define PXEBC_DHCP4_TAG_INDEX_SERVER_ID    4
+#define PXEBC_DHCP4_TAG_INDEX_CLASS_ID     5
+#define PXEBC_DHCP4_TAG_INDEX_BOOTFILE     6
+#define PXEBC_DHCP4_TAG_INDEX_MAX          7
+
+//
+// The type of DHCP OFFER, arranged by priority, PXE10 has the highest priority.
+//
+#define DHCP4_PACKET_TYPE_PXE10            0
+#define DHCP4_PACKET_TYPE_WFM11A           1
+#define DHCP4_PACKET_TYPE_BINL             2
+#define DHCP4_PACKET_TYPE_DHCP_ONLY        3
+#define DHCP4_PACKET_TYPE_BOOTP            4
+#define DHCP4_PACKET_TYPE_MAX              5
+
+#define BIT(x)  (1 << x)
+#define CTRL(x) (0x1F & (x))
+
+//
+// WfM11a options
+//
+#define MTFTP_VENDOR_OPTION_BIT_MAP (BIT (PXEBC_VENDOR_TAG_MTFTP_IP) | \
+                                     BIT (PXEBC_VENDOR_TAG_MTFTP_CPORT) | \
+                                     BIT (PXEBC_VENDOR_TAG_MTFTP_SPORT) | \
+                                     BIT (PXEBC_VENDOR_TAG_MTFTP_TIMEOUT) | \
+                                     BIT (PXEBC_VENDOR_TAG_MTFTP_DELAY))
+//
+// Discoverty options
+//
+#define DISCOVER_VENDOR_OPTION_BIT_MAP  (BIT (PXEBC_VENDOR_TAG_DISCOVER_CTRL) | \
+                                         BIT (PXEBC_VENDOR_TAG_DISCOVER_MCAST) | \
+                                         BIT (PXEBC_VENDOR_TAG_BOOT_SERVERS) | \
+                                         BIT (PXEBC_VENDOR_TAG_BOOT_MENU) | \
+                                         BIT (PXEBC_VENDOR_TAG_MENU_PROMPT))
+
+#define IS_VALID_BOOT_PROMPT(x) \
+  ((((x)[0]) & BIT (PXEBC_VENDOR_TAG_MENU_PROMPT)) == BIT (PXEBC_VENDOR_TAG_MENU_PROMPT))
+
+#define IS_VALID_BOOT_MENU(x) \
+  ((((x)[0]) & BIT (PXEBC_VENDOR_TAG_BOOT_MENU)) == BIT (PXEBC_VENDOR_TAG_BOOT_MENU))
+
+#define IS_VALID_MTFTP_VENDOR_OPTION(x) \
+    (((UINT32) ((x)[0]) & MTFTP_VENDOR_OPTION_BIT_MAP) == MTFTP_VENDOR_OPTION_BIT_MAP)
+
+#define IS_VALID_DISCOVER_VENDOR_OPTION(x)  (((UINT32) ((x)[0]) & DISCOVER_VENDOR_OPTION_BIT_MAP) != 0)
+
+#define IS_VALID_CREDENTIAL_VENDOR_OPTION(x) \
+    (((UINT32) ((x)[0]) & BIT (PXEBC_VENDOR_TAG_CREDENTIAL_TYPES)) == BIT (PXEBC_VENDOR_TAG_CREDENTIAL_TYPES))
+
+#define IS_VALID_BOOTITEM_VENDOR_OPTION(x) \
+    (((UINT32) ((x)[PXEBC_VENDOR_TAG_BOOT_ITEM / 32]) & BIT (PXEBC_VENDOR_TAG_BOOT_ITEM % 32)) \
+      == BIT (PXEBC_VENDOR_TAG_BOOT_ITEM % 32))
+
+#define IS_DISABLE_BCAST_DISCOVER(x)    (((x) & BIT (0)) == BIT (0))
+#define IS_DISABLE_MCAST_DISCOVER(x)    (((x) & BIT (1)) == BIT (1))
+#define IS_ENABLE_USE_SERVER_LIST(x)    (((x) & BIT (2)) == BIT (2))
+#define IS_DISABLE_PROMPT_MENU(x)       (((x) & BIT (3)) == BIT (3))
+
+#define SET_VENDOR_OPTION_BIT_MAP(x, y) (((x)[(y) / 32]) = (UINT32) ((x)[(y) / 32]) | BIT ((y) % 32))
+
+#pragma pack(1)
+typedef struct {
+  UINT8 ParaList[135];
+} PXEBC_DHCP4_OPTION_PARA;
+
+typedef struct {
+  UINT16  Size;
+} PXEBC_DHCP4_OPTION_MAX_MESG_SIZE;
+
+typedef struct {
+  UINT8 Type;
+  UINT8 MajorVer;
+  UINT8 MinorVer;
+} PXEBC_DHCP4_OPTION_UNDI;
+
+typedef struct {
+  UINT8 Type;
+} PXEBC_DHCP4_OPTION_MESG;
+
+typedef struct {
+  UINT16  Type;
+} PXEBC_DHCP4_OPTION_ARCH;
+
+#define DEFAULT_CLASS_ID_DATA "PXEClient:Arch:xxxxx:UNDI:003000"
+#define DEFAULT_UNDI_TYPE     1
+#define DEFAULT_UNDI_MAJOR    3
+#define DEFAULT_UNDI_MINOR    0
+
+typedef struct {
+  UINT8 ClassIdentifier[10];
+  UINT8 ArchitecturePrefix[5];
+  UINT8 ArchitectureType[5];
+  UINT8 Lit3[1];
+  UINT8 InterfaceName[4];
+  UINT8 Lit4[1];
+  UINT8 UndiMajor[3];
+  UINT8 UndiMinor[3];
+} PXEBC_DHCP4_OPTION_CLID;
+
+typedef struct {
+  UINT8 Type;
+  UINT8 Guid[16];
+} PXEBC_DHCP4_OPTION_UUID;
+
+typedef struct {
+  UINT16  Type;
+  UINT16  Layer;
+} PXEBC_OPTION_BOOT_ITEM;
+
+#pragma pack()
+
+typedef union {
+  PXEBC_DHCP4_OPTION_PARA           *Para;
+  PXEBC_DHCP4_OPTION_UNDI           *Undi;
+  PXEBC_DHCP4_OPTION_ARCH           *Arch;
+  PXEBC_DHCP4_OPTION_CLID           *Clid;
+  PXEBC_DHCP4_OPTION_UUID           *Uuid;
+  PXEBC_DHCP4_OPTION_MESG           *Mesg;
+  PXEBC_DHCP4_OPTION_MAX_MESG_SIZE  *MaxMesgSize;
+} PXEBC_DHCP4_OPTION_ENTRY;
+
+typedef struct {
+  UINT16            Type;
+  UINT8             IpCnt;
+  EFI_IPv4_ADDRESS  IpAddr[1];
+} PXEBC_BOOT_SVR_ENTRY;
+
+typedef struct {
+  UINT16  Type;
+  UINT8   DescLen;
+  UINT8   DescStr[1];
+} PXEBC_BOOT_MENU_ENTRY;
+
+typedef struct {
+  UINT8 Timeout;
+  UINT8 Prompt[1];
+} PXEBC_MENU_PROMPT;
+
+typedef struct {
+  UINT32                BitMap[8];
+  EFI_IPv4_ADDRESS      MtftpIp;
+  UINT16                MtftpCPort;
+  UINT16                MtftpSPort;
+  UINT8                 MtftpTimeout;
+  UINT8                 MtftpDelay;
+  UINT8                 DiscoverCtrl;
+  EFI_IPv4_ADDRESS      DiscoverMcastIp;
+  EFI_IPv4_ADDRESS      McastIpBase;
+  UINT16                McastIpBlock;
+  UINT16                McastIpRange;
+  UINT16                BootSrvType;
+  UINT16                BootSrvLayer;
+  PXEBC_BOOT_SVR_ENTRY  *BootSvr;
+  UINT8                 BootSvrLen;
+  PXEBC_BOOT_MENU_ENTRY *BootMenu;
+  UINT8                 BootMenuLen;
+  PXEBC_MENU_PROMPT     *MenuPrompt;
+  UINT8                 MenuPromptLen;
+  UINT32                *CredType;
+  UINT8                 CredTypeLen;
+} PXEBC_VENDOR_OPTION;
+
+#define PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE  (OFFSET_OF (EFI_DHCP4_PACKET, Dhcp4) + PXEBC_DHCP4_MAX_PACKET_SIZE)
+
+typedef union {
+  EFI_DHCP4_PACKET  Offer;
+  EFI_DHCP4_PACKET  Ack;
+  UINT8             Buffer[PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE];
+} PXEBC_DHCP4_PACKET;
+
+typedef struct {
+  PXEBC_DHCP4_PACKET      Packet;
+  BOOLEAN                 IsPxeOffer;
+  UINT8                   OfferType;
+  EFI_DHCP4_PACKET_OPTION *Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_MAX];
+  PXEBC_VENDOR_OPTION    PxeVendorOption;
+} PXEBC_CACHED_DHCP4_PACKET;
+
+#define GET_NEXT_DHCP_OPTION(Opt) \
+  (EFI_DHCP4_PACKET_OPTION *) ((UINT8 *) (Opt) + sizeof (EFI_DHCP4_PACKET_OPTION) + (Opt)->Length - 1)
+
+#define GET_OPTION_BUFFER_LEN(Pkt)  ((Pkt)->Length - sizeof (EFI_DHCP4_HEADER) - 4)
+#define IS_PROXY_DHCP_OFFER(Offer)  EFI_IP4_EQUAL (&((Offer)->Dhcp4.Header.YourAddr), &mZeroIp4Addr)
+
+#define GET_NEXT_BOOT_SVR_ENTRY(Ent) \
+  (PXEBC_BOOT_SVR_ENTRY *) ((UINT8 *) Ent + sizeof (*(Ent)) + ((Ent)->IpCnt - 1) * sizeof (EFI_IPv4_ADDRESS))
+
+
+/**
+  This function initialize the DHCP4 message instance.
+
+  This function will pad each item of dhcp4 message packet.
+  
+  @param  Seed    Pointer to the message instance of the DHCP4 packet.
+  @param  Udp4    Pointer to the EFI_UDP4_PROTOCOL instance.
+
+**/
+VOID
+PxeBcInitSeedPacket (
+  IN EFI_DHCP4_PACKET  *Seed,
+  IN EFI_UDP4_PROTOCOL *Udp4
+  );
+
+
+/**
+  Parse the cached dhcp packet.
+
+  @param  CachedPacket  Pointer to cached dhcp packet.
+
+  @retval TRUE          Succeed to parse and validation.
+  @retval FALSE         Fail to parse or validation.
+
+**/
+BOOLEAN
+PxeBcParseCachedDhcpPacket (
+  IN PXEBC_CACHED_DHCP4_PACKET  *CachedPacket
+  );
+
+/**
+  This function is to check the selected proxy offer (include BINL dhcp offer and
+  DHCP_ONLY offer ) and set the flag and copy the DHCP packets to the Pxe base code
+  mode structure.
+
+  @param  Private          Pointer to PxeBc private data.
+
+  @retval EFI_SUCCESS      Operational successful.
+  @retval EFI_NO_RESPONSE  Offer dhcp service failed.
+
+**/
+EFI_STATUS
+PxeBcCheckSelectedOffer (
+  IN PXEBC_PRIVATE_DATA  *Private
+  );
+
+
+/**
+  Callback routine.
+  
+  EFI_DHCP4_CALLBACK is provided by the consumer of the EFI DHCPv4 Protocol driver
+  to intercept events that occurred in the configuration process. This structure
+  provides advanced control of each state transition of the DHCP process. The
+  returned status code determines the behavior of the EFI DHCPv4 Protocol driver.
+  There are three possible returned values, which are described in the following
+  table.
+
+  @param  This                  Pointer to the EFI DHCPv4 Protocol instance that is used to
+                                configure this callback function.
+  @param  Context               Pointer to the context that is initialized by
+                                EFI_DHCP4_PROTOCOL.Configure().
+  @param  CurrentState          The current operational state of the EFI DHCPv4 Protocol
+                                driver.
+  @param  Dhcp4Event            The event that occurs in the current state, which usually means a
+                                state transition.
+  @param  Packet                The DHCP packet that is going to be sent or already received.
+  @param  NewPacket             The packet that is used to replace the above Packet.
+
+  @retval EFI_SUCCESS           Tells the EFI DHCPv4 Protocol driver to continue the DHCP process.
+  @retval EFI_NOT_READY         Only used in the Dhcp4Selecting state. The EFI DHCPv4 Protocol
+                                driver will continue to wait for more DHCPOFFER packets until the retry
+                                timeout expires.
+  @retval EFI_ABORTED           Tells the EFI DHCPv4 Protocol driver to abort the current process and
+                                return to the Dhcp4Init or Dhcp4InitReboot state.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDhcpCallBack (
+  IN EFI_DHCP4_PROTOCOL                * This,
+  IN VOID                              *Context,
+  IN EFI_DHCP4_STATE                   CurrentState,
+  IN EFI_DHCP4_EVENT                   Dhcp4Event,
+  IN EFI_DHCP4_PACKET                  * Packet OPTIONAL,
+  OUT EFI_DHCP4_PACKET                 **NewPacket OPTIONAL
+  );
+
+/**
+  Switch the Ip4 policy to static.
+
+  @param[in]  Private             The pointer to PXEBC_PRIVATE_DATA.
+
+  @retval     EFI_SUCCESS         The policy is already configured to static.
+  @retval     Others              Other error as indicated..
+
+**/
+EFI_STATUS
+PxeBcSetIp4Policy (   
+  IN PXEBC_PRIVATE_DATA            *Private
+  );
+
+/**
+  Discover the boot of service and initialize the vendor option if exists.
+
+  @param  Private               Pointer to PxeBc private data.
+  @param  Type                  PxeBc option boot item type
+  @param  Layer                 PxeBc option boot item layer
+  @param  UseBis                Use BIS or not
+  @param  DestIp                Ip address for server      
+  @param  IpCount               The total count of the server ip address    
+  @param  SrvList               Server list
+  @param  IsDiscv               Discover the vendor or not
+  @param  Reply                 The dhcp4 packet of Pxe reply
+
+  @retval EFI_SUCCESS           Operation succeeds.
+  @retval EFI_OUT_OF_RESOURCES  Allocate memory pool failed.
+  @retval EFI_NOT_FOUND         There is no vendor option exists.
+  @retval EFI_TIMEOUT           Send Pxe Discover time out. 
+  
+**/
+EFI_STATUS
+PxeBcDiscvBootService (
+  IN PXEBC_PRIVATE_DATA                * Private,
+  IN UINT16                            Type,
+  IN UINT16                            *Layer,
+  IN BOOLEAN                           UseBis,
+  IN EFI_IP_ADDRESS                    * DestIp,
+  IN UINT16                            IpCount,
+  IN EFI_PXE_BASE_CODE_SRVLIST         * SrvList,
+  IN BOOLEAN                           IsDiscv,
+  OUT EFI_DHCP4_PACKET                 * Reply OPTIONAL
+  );
+
+
+/**
+  Initialize the DHCP options and build the option list.
+
+  @param  Private          Pointer to PxeBc private data.
+  @param  OptList          Pointer to a DHCP option list.
+                           
+  @param  IsDhcpDiscover   Discover dhcp option or not.     
+
+  @return The index item number of the option list.
+
+**/
+UINT32
+PxeBcBuildDhcpOptions (
+  IN PXEBC_PRIVATE_DATA            *Private,
+  IN EFI_DHCP4_PACKET_OPTION       **OptList,
+  IN BOOLEAN                       IsDhcpDiscover
+  );
+
+
+/**
+  Create the boot options.
+
+  @param  OptList    Pointer to the list of the options
+  @param  Type       the type of option
+  @param  Layer      the layer of the boot options 
+  @param  OptLen     length of opotion
+
+**/
+VOID
+PxeBcCreateBootOptions (
+  IN  EFI_DHCP4_PACKET_OPTION          *OptList,
+  IN  UINT16                           Type,
+  IN  UINT16                           *Layer,
+  OUT UINT32                           *OptLen
+  );
+
+
+/**
+  Parse interested dhcp options.
+
+  @param  Buffer     Pointer to the dhcp options packet.
+  @param  Length     The length of the dhcp options.
+  @param  OptTag     The option OpCode.
+
+  @return NULL if the buffer length is 0 and OpCode is not 
+          PXEBC_DHCP4_TAG_EOP, or the pointer to the buffer.
+
+**/
+EFI_DHCP4_PACKET_OPTION *
+PxeBcParseExtendOptions (
+  IN UINT8                         *Buffer,
+  IN UINT32                        Length,
+  IN UINT8                         OptTag
+  );
+
+
+/**
+  This function is to parse and check vendor options.
+
+  @param  Dhcp4Option           Pointer to dhcp options
+  @param  VendorOption          Pointer to vendor options
+
+  @return TRUE if valid for vendor options, or FALSE.
+
+**/
+BOOLEAN
+PxeBcParseVendorOptions (
+  IN EFI_DHCP4_PACKET_OPTION       *Dhcp4Option,
+  IN PXEBC_VENDOR_OPTION           *VendorOption
+  );
+
+
+/**
+  Choose the boot prompt.
+
+  @param  Private              Pointer to PxeBc private data.
+
+  @retval EFI_SUCCESS          Select boot prompt done.
+  @retval EFI_TIMEOUT          Select boot prompt time out. 
+  @retval EFI_NOT_FOUND        The proxy offer is not Pxe10.
+  @retval EFI_ABORTED          User cancel the operation.
+  @retval EFI_NOT_READY        Read the input key from the keybroad has not finish.
+  
+**/
+EFI_STATUS
+PxeBcSelectBootPrompt (
+  IN PXEBC_PRIVATE_DATA              *Private
+  );
+
+
+/**
+  Select the boot menu.
+
+  @param  Private         Pointer to PxeBc private data.
+  @param  Type            The type of the menu.
+  @param  UseDefaultItem  Use default item or not.
+  
+  @retval EFI_ABORTED     User cancel operation.
+  @retval EFI_SUCCESS     Select the boot menu success.
+  @retval EFI_NOT_READY   Read the input key from the keybroad has not finish.    
+
+**/
+EFI_STATUS
+PxeBcSelectBootMenu (
+  IN  PXEBC_PRIVATE_DATA              *Private,
+  OUT UINT16                          *Type,
+  IN  BOOLEAN                         UseDefaultItem
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.c
new file mode 100644
index 0000000000..76c140d8e3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.c
@@ -0,0 +1,665 @@
+/** @file
+  The driver binding for UEFI PXEBC protocol.
+
+Copyright (c) 2007 - 2015, 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 "PxeBcImpl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gPxeBcDriverBinding = {
+  PxeBcDriverBindingSupported,
+  PxeBcDriverBindingStart,
+  PxeBcDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  This is the declaration of an EFI image entry point. This entry point is
+  the same for UEFI Applications, UEFI OS Loaders, and UEFI Drivers including
+  both device drivers and bus drivers.
+
+  @param  ImageHandle           The firmware allocated handle for the UEFI image.
+  @param  SystemTable           A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS           The operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  The request could not be completed due to a lack of resources.
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverEntryPoint (
+  IN EFI_HANDLE             ImageHandle,
+  IN EFI_SYSTEM_TABLE       *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+          ImageHandle,
+          SystemTable,
+          &gPxeBcDriverBinding,
+          ImageHandle,
+          &gPxeBcComponentName,
+          &gPxeBcComponentName2
+          );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.
+  PxeBc requires DHCP4 and MTFTP4 protocols.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_PXE_BASE_CODE_PROTOCOL  *PxeBc;
+  EFI_STATUS                  Status;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiPxeBaseCodeProtocolGuid,
+                  (VOID **) &PxeBc,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDhcp4ServiceBindingProtocolGuid,
+                  NULL,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+
+    Status = gBS->OpenProtocol (
+                    ControllerHandle,
+                    &gEfiMtftp4ServiceBindingProtocolGuid,
+                    NULL,
+                    This->DriverBindingHandle,
+                    ControllerHandle,
+                    EFI_OPEN_PROTOCOL_TEST_PROTOCOL
+                    );
+
+  }
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA  *Private;
+  UINTN               Index;
+  EFI_STATUS          Status;
+  EFI_IP4_MODE_DATA   Ip4ModeData;
+
+  Private = AllocateZeroPool (sizeof (PXEBC_PRIVATE_DATA));
+  if (Private == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Private->Signature                    = PXEBC_PRIVATE_DATA_SIGNATURE;
+  Private->Controller                   = ControllerHandle;
+  Private->Image                        = This->DriverBindingHandle;
+  CopyMem (&Private->PxeBc, &mPxeBcProtocolTemplate, sizeof (Private->PxeBc));
+  Private->PxeBc.Mode                   = &Private->Mode;
+  CopyMem (&Private->LoadFile, &mLoadFileProtocolTemplate, sizeof (Private->LoadFile));
+
+  Private->ProxyOffer.Packet.Offer.Size = PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE;
+  Private->Dhcp4Ack.Packet.Ack.Size     = PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE;
+  Private->PxeReply.Packet.Ack.Size     = PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE;
+
+  for (Index = 0; Index < PXEBC_MAX_OFFER_NUM; Index++) {
+    Private->Dhcp4Offers[Index].Packet.Offer.Size = PXEBC_CACHED_DHCP4_PACKET_MAX_SIZE;
+  }
+
+  //
+  // Get the NII interface if it exists.
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiNetworkInterfaceIdentifierProtocolGuid_31,
+                  (VOID **) &Private->Nii,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    Private->Nii = NULL;
+  }
+
+  Status = NetLibCreateServiceChild (
+            ControllerHandle,
+            This->DriverBindingHandle,
+            &gEfiArpServiceBindingProtocolGuid,
+            &Private->ArpChild
+            );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->ArpChild,
+                  &gEfiArpProtocolGuid,
+                  (VOID **) &Private->Arp,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = NetLibCreateServiceChild (
+            ControllerHandle,
+            This->DriverBindingHandle,
+            &gEfiDhcp4ServiceBindingProtocolGuid,
+            &Private->Dhcp4Child
+            );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->Dhcp4Child,
+                  &gEfiDhcp4ProtocolGuid,
+                  (VOID **) &Private->Dhcp4,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             This->DriverBindingHandle,
+             &gEfiIp4ServiceBindingProtocolGuid,
+             &Private->Ip4Child
+             );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->Ip4Child,
+                  &gEfiIp4ProtocolGuid,
+                  (VOID **) &Private->Ip4,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // Get max packet size from Ip4 to calculate block size for Tftp later.
+  //
+  Status = Private->Ip4->GetModeData (Private->Ip4, &Ip4ModeData, NULL, NULL);
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Private->Ip4MaxPacketSize = Ip4ModeData.MaxPacketSize;
+
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             This->DriverBindingHandle,
+             &gEfiMtftp4ServiceBindingProtocolGuid,
+             &Private->Mtftp4Child
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->Mtftp4Child,
+                  &gEfiMtftp4ProtocolGuid,
+                  (VOID **) &Private->Mtftp4,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             This->DriverBindingHandle,
+             &gEfiUdp4ServiceBindingProtocolGuid,
+             &Private->Udp4ReadChild
+             );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // The UDP instance for EfiPxeBcUdpRead
+  //
+  Status = gBS->OpenProtocol (
+                  Private->Udp4ReadChild,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Private->Udp4Read,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  //
+  // The UDP instance for EfiPxeBcUdpWrite
+  //
+  Status = NetLibCreateServiceChild (
+             ControllerHandle,
+             This->DriverBindingHandle,
+             &gEfiUdp4ServiceBindingProtocolGuid,
+             &Private->Udp4WriteChild
+             );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  Status = gBS->OpenProtocol (
+                  Private->Udp4WriteChild,
+                  &gEfiUdp4ProtocolGuid,
+                  (VOID **) &Private->Udp4Write,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+  ZeroMem (&Private->Udp4CfgData, sizeof (EFI_UDP4_CONFIG_DATA));
+  Private->Udp4CfgData.AcceptBroadcast    = FALSE;
+  Private->Udp4CfgData.AcceptPromiscuous  = FALSE;
+  Private->Udp4CfgData.AcceptAnyPort      = TRUE;
+  Private->Udp4CfgData.AllowDuplicatePort = TRUE;
+  Private->Udp4CfgData.TypeOfService      = DEFAULT_ToS;
+  Private->Udp4CfgData.TimeToLive         = DEFAULT_TTL;
+  Private->Udp4CfgData.DoNotFragment      = FALSE;
+  Private->Udp4CfgData.ReceiveTimeout     = PXEBC_DEFAULT_LIFETIME;
+  Private->Udp4CfgData.UseDefaultAddress  = FALSE;
+
+  PxeBcInitSeedPacket (&Private->SeedPacket, Private->Udp4Read);
+  Private->MacLen = Private->SeedPacket.Dhcp4.Header.HwAddrLen;
+  CopyMem (&Private->Mac, &Private->SeedPacket.Dhcp4.Header.ClientHwAddr[0], Private->MacLen);
+
+
+  ZeroMem (&Private->Ip4ConfigData, sizeof (EFI_IP4_CONFIG_DATA));
+  Private->Ip4ConfigData.DefaultProtocol   = EFI_IP_PROTO_ICMP;
+  Private->Ip4ConfigData.AcceptIcmpErrors  = TRUE;
+  Private->Ip4ConfigData.TypeOfService     = DEFAULT_ToS;
+  Private->Ip4ConfigData.TimeToLive        = DEFAULT_TTL;
+  Private->Ip4ConfigData.DoNotFragment     = FALSE;
+  Private->Ip4ConfigData.RawData           = FALSE;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiPxeBaseCodeProtocolGuid,
+                  &Private->PxeBc,
+                  &gEfiLoadFileProtocolGuid,
+                  &Private->LoadFile,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+  //
+  // Locate Ip4->Ip4Config2 and store it for set IPv4 Policy.
+  //
+  Status = gBS->HandleProtocol (
+                  ControllerHandle,
+                  &gEfiIp4Config2ProtocolGuid,
+                  (VOID **) &Private->Ip4Config2
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_ERROR;
+  }
+
+  return EFI_SUCCESS;
+
+ON_ERROR:
+
+  if (Private->Udp4WriteChild != NULL) {
+    gBS->CloseProtocol (
+           Private->Udp4WriteChild,
+           &gEfiUdp4ProtocolGuid,
+           This->DriverBindingHandle,
+           ControllerHandle
+           );
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      Private->Udp4WriteChild
+      );
+  }
+
+  if (Private->Udp4ReadChild != NULL) {
+    gBS->CloseProtocol (
+          Private->Udp4ReadChild,
+          &gEfiUdp4ProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      Private->Udp4ReadChild
+      );
+  }
+
+  if (Private->Mtftp4Child != NULL) {
+    gBS->CloseProtocol (
+          Private->Mtftp4Child,
+          &gEfiMtftp4ProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiMtftp4ServiceBindingProtocolGuid,
+      Private->Mtftp4Child
+      );
+  }
+
+  if (Private->Ip4Child != NULL) {
+    gBS->CloseProtocol (
+          Private->Ip4Child,
+          &gEfiIp4ProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiIp4ServiceBindingProtocolGuid,
+      Private->Ip4Child
+      );
+  }
+
+  if (Private->Dhcp4Child != NULL) {
+    gBS->CloseProtocol (
+          Private->Dhcp4Child,
+          &gEfiDhcp4ProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiDhcp4ServiceBindingProtocolGuid,
+      Private->Dhcp4Child
+      );
+  }
+
+  if (Private->ArpChild != NULL) {
+    gBS->CloseProtocol (
+          Private->ArpChild,
+          &gEfiArpProtocolGuid,
+          This->DriverBindingHandle,
+          ControllerHandle
+          );
+
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiArpServiceBindingProtocolGuid,
+      Private->ArpChild
+      );
+  }
+
+  FreePool (Private);
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  )
+{
+  PXEBC_PRIVATE_DATA          *Private;
+  EFI_PXE_BASE_CODE_PROTOCOL  *PxeBc;
+  EFI_HANDLE                  NicHandle;
+  EFI_STATUS                  Status;
+
+  NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiArpProtocolGuid);
+  if (NicHandle == NULL) {
+    NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiDhcp4ProtocolGuid);
+
+    if (NicHandle == NULL) {
+      NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiIp4ProtocolGuid);
+
+      if (NicHandle == NULL) {
+        NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiUdp4ProtocolGuid);
+
+        if (NicHandle == NULL) {
+          NicHandle = NetLibGetNicHandle (ControllerHandle, &gEfiMtftp4ProtocolGuid);
+
+          if (NicHandle == NULL) {
+            return EFI_SUCCESS;
+          }
+        }
+      }
+    }
+  }
+
+  Status = gBS->OpenProtocol (
+                  NicHandle,
+                  &gEfiPxeBaseCodeProtocolGuid,
+                  (VOID **) &PxeBc,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Stop functionality of PXE Base Code protocol
+  //
+  Status = PxeBc->Stop (PxeBc);
+  if (Status != EFI_SUCCESS && Status != EFI_NOT_STARTED) {
+    return Status;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (PxeBc);
+
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  NicHandle,
+                  &gEfiPxeBaseCodeProtocolGuid,
+                  &Private->PxeBc,
+                  &gEfiLoadFileProtocolGuid,
+                  &Private->LoadFile,
+                  NULL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+
+    gBS->CloseProtocol (
+           Private->Udp4WriteChild,
+           &gEfiUdp4ProtocolGuid,
+           This->DriverBindingHandle,
+           NicHandle
+           );
+    NetLibDestroyServiceChild (
+      ControllerHandle,
+      This->DriverBindingHandle,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      Private->Udp4WriteChild
+      );
+
+    gBS->CloseProtocol (
+          Private->Udp4ReadChild,
+          &gEfiUdp4ProtocolGuid,
+          This->DriverBindingHandle,
+          NicHandle
+          );
+    NetLibDestroyServiceChild (
+      NicHandle,
+      This->DriverBindingHandle,
+      &gEfiUdp4ServiceBindingProtocolGuid,
+      Private->Udp4ReadChild
+      );
+
+    gBS->CloseProtocol (
+          Private->Dhcp4Child,
+          &gEfiDhcp4ProtocolGuid,
+          This->DriverBindingHandle,
+          NicHandle
+          );
+    NetLibDestroyServiceChild (
+      NicHandle,
+      This->DriverBindingHandle,
+      &gEfiDhcp4ServiceBindingProtocolGuid,
+      Private->Dhcp4Child
+      );
+
+    gBS->CloseProtocol (
+          Private->Mtftp4Child,
+          &gEfiMtftp4ProtocolGuid,
+          This->DriverBindingHandle,
+          NicHandle
+          );
+    NetLibDestroyServiceChild (
+      NicHandle,
+      This->DriverBindingHandle,
+      &gEfiMtftp4ServiceBindingProtocolGuid,
+      Private->Mtftp4Child
+      );
+
+    gBS->CloseProtocol (
+          Private->Ip4Child,
+          &gEfiIp4ProtocolGuid,
+          This->DriverBindingHandle,
+          NicHandle
+          );
+    NetLibDestroyServiceChild (
+      NicHandle,
+      This->DriverBindingHandle,
+      &gEfiIp4ServiceBindingProtocolGuid,
+      Private->Ip4Child
+      );
+
+    gBS->CloseProtocol (
+          Private->ArpChild,
+          &gEfiArpProtocolGuid,
+          This->DriverBindingHandle,
+          NicHandle
+          );
+    NetLibDestroyServiceChild (
+      NicHandle,
+      This->DriverBindingHandle,
+      &gEfiArpServiceBindingProtocolGuid,
+      Private->ArpChild
+      );
+
+    FreePool (Private);
+  }
+
+  return Status;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.h b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.h
new file mode 100644
index 0000000000..f436c895b4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcDriver.h
@@ -0,0 +1,102 @@
+/** @file
+
+Copyright (c) 2007, 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.
+
+**/
+
+#ifndef __EFI_PXEBC_DRIVER_H__
+#define __EFI_PXEBC_DRIVER_H__
+
+/**
+  Test to see if this driver supports ControllerHandle. This service
+  is called by the EFI boot service ConnectController(). In
+  order to make drivers as small as possible, there are a few calling
+  restrictions for this service. ConnectController() must
+  follow these calling restrictions. If any other agent wishes to call
+  Supported() it must also follow these calling restrictions.  
+  PxeBc requires DHCP4 and MTFTP4 protocols.
+
+  @param  This                Protocol instance pointer.
+  @param  ControllerHandle    Handle of device to test
+  @param  RemainingDevicePath Optional parameter use to pick a specific child
+                              device to start.
+
+  @retval EFI_SUCCESS         This driver supports this device
+  @retval EFI_ALREADY_STARTED This driver is already running on this device
+  @retval other               This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  );
+  
+/**
+  Start this driver on ControllerHandle. This service is called by the
+  EFI boot service ConnectController(). In order to make
+  drivers as small as possible, there are a few calling restrictions for
+  this service. ConnectController() must follow these
+  calling restrictions. If any other agent wishes to call Start() it
+  must also follow these calling restrictions.
+
+  @param  This                 Protocol instance pointer.
+  @param  ControllerHandle     Handle of device to bind driver to
+  @param  RemainingDevicePath  Optional parameter use to pick a specific child
+                               device to start.
+
+  @retval EFI_SUCCESS          This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL  * This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL     * RemainingDevicePath OPTIONAL
+  );
+  
+/**
+  Stop this driver on ControllerHandle. This service is called by the
+  EFI boot service DisconnectController(). In order to
+  make drivers as small as possible, there are a few calling
+  restrictions for this service. DisconnectController()
+  must follow these calling restrictions. If any other agent wishes
+  to call Stop() it must also follow these calling restrictions.
+  
+  @param  This              Protocol instance pointer.
+  @param  ControllerHandle  Handle of device to stop driver on
+  @param  NumberOfChildren  Number of Handles in ChildHandleBuffer. If number of
+                            children is zero stop the entire bus driver.
+  @param  ChildHandleBuffer List of Child Handles to Stop.
+
+  @retval EFI_SUCCESS       This driver is removed ControllerHandle
+  @retval other             This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL  *This,
+  IN EFI_HANDLE                   ControllerHandle,
+  IN UINTN                        NumberOfChildren,
+  IN EFI_HANDLE                   *ChildHandleBuffer
+  );
+  
+extern EFI_COMPONENT_NAME2_PROTOCOL gPxeBcComponentName2;
+extern EFI_COMPONENT_NAME_PROTOCOL  gPxeBcComponentName;
+extern EFI_DRIVER_BINDING_PROTOCOL  gPxeBcDriverBinding;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.c
new file mode 100644
index 0000000000..89977e6690
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.c
@@ -0,0 +1,2959 @@
+/** @file
+  Interface routines for PxeBc.
+
+Copyright (c) 2007 - 2015, 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 "PxeBcImpl.h"
+
+UINT32  mPxeDhcpTimeout[4] = { 4, 8, 16, 32 };
+
+/**
+  Get and record the arp cache.
+
+  @param  This                    Pointer to EFI_PXE_BC_PROTOCOL
+
+  @retval EFI_SUCCESS             Arp cache updated successfully
+  @retval others                  If error occurs when getting arp cache
+
+**/
+EFI_STATUS
+UpdateArpCache (
+  IN EFI_PXE_BASE_CODE_PROTOCOL     * This
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_STATUS              Status;
+  UINT32                  EntryLength;
+  UINT32                  EntryCount;
+  EFI_ARP_FIND_DATA       *Entries;
+  UINT32                  Index;
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  Status = Private->Arp->Find (
+                     Private->Arp,
+                     TRUE,
+                     NULL,
+                     &EntryLength,
+                     &EntryCount,
+                     &Entries,
+                     TRUE
+                     );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Mode->ArpCacheEntries = MIN (
+                           EntryCount,
+                           EFI_PXE_BASE_CODE_MAX_ARP_ENTRIES
+                           );
+  for (Index = 0; Index < Mode->ArpCacheEntries; Index ++) {
+    CopyMem (
+      &Mode->ArpCache[Index].IpAddr,
+      Entries + 1,
+      Entries->SwAddressLength
+      );
+    CopyMem (
+      &Mode->ArpCache[Index].MacAddr,
+      (UINT8 *) (Entries + 1) + Entries->SwAddressLength,
+      Entries->HwAddressLength
+      );
+    //
+    // Slip to the next FindData.
+    //
+    Entries = (EFI_ARP_FIND_DATA *) ((UINT8 *) Entries + EntryLength);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Timeout routine to update arp cache.
+
+  @param  Event              Pointer to EFI_PXE_BC_PROTOCOL
+  @param  Context            Context of the timer event
+
+**/
+VOID
+EFIAPI
+ArpCacheUpdateTimeout (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  UpdateArpCache ((EFI_PXE_BASE_CODE_PROTOCOL *) Context);
+}
+
+/**
+  Do arp resolution from arp cache in PxeBcMode.
+
+  @param  PxeBcMode      The PXE BC mode to look into.
+  @param  Ip4Addr        The Ip4 address for resolution.
+  @param  MacAddress     The resoluted MAC address if the resolution is successful.
+                         The value is undefined if resolution fails.
+
+  @retval TRUE           The resolution is successful.
+  @retval FALSE          Otherwise.
+
+**/
+BOOLEAN
+FindInArpCache (
+  IN  EFI_PXE_BASE_CODE_MODE    *PxeBcMode,
+  IN  EFI_IPv4_ADDRESS          *Ip4Addr,
+  OUT EFI_MAC_ADDRESS           *MacAddress
+  )
+{
+  UINT32                  Index;
+
+  for (Index = 0; Index < PxeBcMode->ArpCacheEntries; Index ++) {
+    if (EFI_IP4_EQUAL (&PxeBcMode->ArpCache[Index].IpAddr.v4, Ip4Addr)) {
+      CopyMem (
+        MacAddress,
+        &PxeBcMode->ArpCache[Index].MacAddr,
+        sizeof (EFI_MAC_ADDRESS)
+        );
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Notify function for the ICMP receive token, used to process
+  the received ICMP packets.
+
+  @param  Context               The PXEBC private data.
+
+**/
+VOID
+EFIAPI
+IcmpErrorListenHandlerDpc (
+  IN VOID      *Context
+  )
+{
+  EFI_STATUS              Status;
+  EFI_IP4_RECEIVE_DATA    *RxData;
+  EFI_IP4_PROTOCOL        *Ip4;
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  UINTN                   Index;
+  UINT32                  CopiedLen;
+  UINT8                   *CopiedPointer;
+
+  Private = (PXEBC_PRIVATE_DATA *) Context;
+  Mode    = &Private->Mode;
+  Status  = Private->IcmpErrorRcvToken.Status;
+  RxData  = Private->IcmpErrorRcvToken.Packet.RxData;
+  Ip4     = Private->Ip4;
+
+  if (Status == EFI_ABORTED) {
+    //
+    // The reception is actively aborted by the consumer, directly return.
+    //
+    return;
+  }
+
+  if (EFI_ERROR (Status) || (RxData == NULL)) {
+    //
+    // Only process the normal packets and the icmp error packets, if RxData is NULL
+    // with Status == EFI_SUCCESS or EFI_ICMP_ERROR, just resume the receive although
+    // this should be a bug of the low layer (IP).
+    //
+    goto Resume;
+  }
+
+  if (EFI_IP4 (RxData->Header->SourceAddress) != 0 &&
+      !NetIp4IsUnicast (EFI_NTOHL (RxData->Header->SourceAddress), 0)) {
+    //
+    // The source address is not zero and it's not a unicast IP address, discard it.
+    //
+    goto CleanUp;
+  }
+
+  if (!EFI_IP4_EQUAL (&RxData->Header->DestinationAddress, &Mode->StationIp.v4)) {
+    //
+    // The dest address is not equal to Station Ip address, discard it.
+    //
+    goto CleanUp;
+  }
+
+  //
+  // Constructor ICMP error packet
+  //
+  CopiedLen = 0;
+  CopiedPointer = (UINT8 *) &Mode->IcmpError;
+
+  for (Index = 0; Index < RxData->FragmentCount; Index ++) {
+    CopiedLen += RxData->FragmentTable[Index].FragmentLength;
+    if (CopiedLen <= sizeof (EFI_PXE_BASE_CODE_ICMP_ERROR)) {
+      CopyMem (
+        CopiedPointer,
+        RxData->FragmentTable[Index].FragmentBuffer,
+        RxData->FragmentTable[Index].FragmentLength
+        );
+    } else {
+      CopyMem (
+        CopiedPointer,
+        RxData->FragmentTable[Index].FragmentBuffer,
+        CopiedLen - sizeof (EFI_PXE_BASE_CODE_ICMP_ERROR)
+        );
+    }
+    CopiedPointer += CopiedLen;
+  }
+
+  goto Resume;
+
+CleanUp:
+  gBS->SignalEvent (RxData->RecycleSignal);
+
+Resume:
+  Ip4->Receive (Ip4, &(Private->IcmpErrorRcvToken));
+}
+
+/**
+  Request IcmpErrorListenHandlerDpc as a DPC at TPL_CALLBACK
+
+  @param  Event                 The event signaled.
+  @param  Context               The context passed in by the event notifier.
+
+**/
+VOID
+EFIAPI
+IcmpErrorListenHandler (
+  IN EFI_EVENT Event,
+  IN VOID      *Context
+  )
+{
+  //
+  // Request IpIoListenHandlerDpc as a DPC at TPL_CALLBACK
+  //
+  QueueDpc (TPL_CALLBACK, IcmpErrorListenHandlerDpc, Context);
+}
+
+/**
+  Enables the use of the PXE Base Code Protocol functions.
+
+  This function enables the use of the PXE Base Code Protocol functions. If the
+  Started field of the EFI_PXE_BASE_CODE_MODE structure is already TRUE, then
+  EFI_ALREADY_STARTED will be returned. If UseIpv6 is TRUE, then IPv6 formatted
+  addresses will be used in this session. If UseIpv6 is FALSE, then IPv4 formatted
+  addresses will be used in this session. If UseIpv6 is TRUE, and the Ipv6Supported
+  field of the EFI_PXE_BASE_CODE_MODE structure is FALSE, then EFI_UNSUPPORTED will
+  be returned. If there is not enough memory or other resources to start the PXE
+  Base Code Protocol, then EFI_OUT_OF_RESOURCES will be returned. Otherwise, the
+  PXE Base Code Protocol will be started, and all of the fields of the EFI_PXE_BASE_CODE_MODE
+  structure will be initialized as follows:
+    StartedSet to TRUE.
+    Ipv6SupportedUnchanged.
+    Ipv6AvailableUnchanged.
+    UsingIpv6Set to UseIpv6.
+    BisSupportedUnchanged.
+    BisDetectedUnchanged.
+    AutoArpSet to TRUE.
+    SendGUIDSet to FALSE.
+    TTLSet to DEFAULT_TTL.
+    ToSSet to DEFAULT_ToS.
+    DhcpCompletedSet to FALSE.
+    ProxyOfferReceivedSet to FALSE.
+    StationIpSet to an address of all zeros.
+    SubnetMaskSet to a subnet mask of all zeros.
+    DhcpDiscoverZero-filled.
+    DhcpAckZero-filled.
+    ProxyOfferZero-filled.
+    PxeDiscoverValidSet to FALSE.
+    PxeDiscoverZero-filled.
+    PxeReplyValidSet to FALSE.
+    PxeReplyZero-filled.
+    PxeBisReplyValidSet to FALSE.
+    PxeBisReplyZero-filled.
+    IpFilterSet the Filters field to 0 and the IpCnt field to 0.
+    ArpCacheEntriesSet to 0.
+    ArpCacheZero-filled.
+    RouteTableEntriesSet to 0.
+    RouteTableZero-filled.
+    IcmpErrorReceivedSet to FALSE.
+    IcmpErrorZero-filled.
+    TftpErroReceivedSet to FALSE.
+    TftpErrorZero-filled.
+    MakeCallbacksSet to TRUE if the PXE Base Code Callback Protocol is available.
+    Set to FALSE if the PXE Base Code Callback Protocol is not available.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  UseIpv6               Specifies the type of IP addresses that are to be used during the session
+                                that is being started. Set to TRUE for IPv6 addresses, and FALSE for
+                                IPv4 addresses.
+
+  @retval EFI_SUCCESS           The PXE Base Code Protocol was started.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this oper
+  @retval EFI_UNSUPPORTED       UseIpv6 is TRUE, but the Ipv6Supported field of the
+                                EFI_PXE_BASE_CODE_MODE structure is FALSE.
+  @retval EFI_ALREADY_STARTED   The PXE Base Code Protocol is already in the started state.
+  @retval EFI_INVALID_PARAMETER The This parameter is NULL or does not point to a valid
+                                EFI_PXE_BASE_CODE_PROTOCOL structure.
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate enough memory or other resources to start the
+                                PXE Base Code Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcStart (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN BOOLEAN                          UseIpv6
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_STATUS              Status;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (Mode->Started) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  if (UseIpv6) {
+    //
+    // IPv6 is not supported now.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Configure the udp4 instance to let it receive data
+  //
+  Status = Private->Udp4Read->Configure (
+                               Private->Udp4Read,
+                               &Private->Udp4CfgData
+                               );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+
+  //
+  // Configure block size for TFTP as a default value to handle all link layers.
+  // 
+  Private->BlockSize   = (UINTN) (MIN (Private->Ip4MaxPacketSize, PXEBC_DEFAULT_PACKET_SIZE) - 
+                           PXEBC_DEFAULT_UDP_OVERHEAD_SIZE - PXEBC_DEFAULT_TFTP_OVERHEAD_SIZE);
+  //
+  // If PcdTftpBlockSize is set to non-zero, override the default value.
+  //
+  if (PcdGet64 (PcdTftpBlockSize) != 0) {
+    Private->BlockSize   = (UINTN) PcdGet64 (PcdTftpBlockSize);
+  }
+  
+  Private->AddressIsOk = FALSE;
+
+  ZeroMem (Mode, sizeof (EFI_PXE_BASE_CODE_MODE));
+
+  Mode->Started = TRUE;
+  Mode->TTL     = DEFAULT_TTL;
+  Mode->ToS     = DEFAULT_ToS;
+  Mode->AutoArp = TRUE;
+
+  //
+  // Create the event for Arp Cache checking.
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  ArpCacheUpdateTimeout,
+                  This,
+                  &Private->GetArpCacheEvent
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Start the timeout timer event.
+  //
+  Status = gBS->SetTimer (
+                  Private->GetArpCacheEvent,
+                  TimerPeriodic,
+                  TICKS_PER_SECOND
+                  );
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Create ICMP error receiving event
+  //
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  IcmpErrorListenHandler,
+                  Private,
+                  &(Private->IcmpErrorRcvToken.Event)
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  //DHCP4 service allows only one of its children to be configured in  
+  //the active state, If the DHCP4 D.O.R.A started by IP4 auto  
+  //configuration and has not been completed, the Dhcp4 state machine 
+  //will not be in the right state for the PXE to start a new round D.O.R.A. 
+  //so we need to switch it's policy to static.
+  //
+  Status = PxeBcSetIp4Policy (Private);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+    
+  Status = Private->Ip4->Configure (Private->Ip4, &Private->Ip4ConfigData);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // start to listen incoming packet
+  //
+  Status = Private->Ip4->Receive (Private->Ip4, &Private->IcmpErrorRcvToken);
+  if (!EFI_ERROR (Status)) {
+    return Status;
+  }
+
+ON_EXIT:
+  Private->Ip4->Configure (Private->Ip4, NULL);
+
+  if (Private->IcmpErrorRcvToken.Event != NULL) {
+    gBS->CloseEvent (Private->IcmpErrorRcvToken.Event);
+  }
+
+  if (Private->GetArpCacheEvent != NULL) {
+    gBS->SetTimer (Private->GetArpCacheEvent, TimerCancel, 0);
+    gBS->CloseEvent (Private->GetArpCacheEvent);
+  }
+
+  Mode->Started = FALSE;
+  Mode->TTL     = 0;
+  Mode->ToS     = 0;
+  Mode->AutoArp = FALSE;
+
+  return Status;
+}
+
+
+/**
+  Disables the use of the PXE Base Code Protocol functions.
+
+  This function stops all activity on the network device. All the resources allocated
+  in Start() are released, the Started field of the EFI_PXE_BASE_CODE_MODE structure is
+  set to FALSE and EFI_SUCCESS is returned. If the Started field of the EFI_PXE_BASE_CODE_MODE
+  structure is already FALSE, then EFI_NOT_STARTED will be returned.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+
+  @retval EFI_SUCCESS           The PXE Base Code Protocol was stopped.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is already in the stopped state.
+  @retval EFI_INVALID_PARAMETER The This parameter is NULL or does not point to a valid
+                                EFI_PXE_BASE_CODE_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcStop (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  Private->Ip4->Cancel (Private->Ip4, NULL);
+  //
+  // Dispatch the DPCs queued by the NotifyFunction of the canceled rx token's
+  // events.
+  //
+  DispatchDpc ();
+
+  Private->Ip4->Configure (Private->Ip4, NULL);
+
+  //
+  // Close the ICMP error receiving event.
+  //
+  gBS->CloseEvent (Private->IcmpErrorRcvToken.Event);
+
+  //
+  // Cancel the TimeoutEvent timer.
+  //
+  gBS->SetTimer (Private->GetArpCacheEvent, TimerCancel, 0);
+
+  //
+  // Close the TimeoutEvent event.
+  //
+  gBS->CloseEvent (Private->GetArpCacheEvent);
+
+  Mode->Started = FALSE;
+
+  Private->CurrentUdpSrcPort = 0;
+  Private->Udp4Write->Configure (Private->Udp4Write, NULL);
+  Private->Udp4Read->Groups (Private->Udp4Read, FALSE, NULL);
+  Private->Udp4Read->Configure (Private->Udp4Read, NULL);
+
+  Private->Dhcp4->Stop (Private->Dhcp4);
+  Private->Dhcp4->Configure (Private->Dhcp4, NULL);
+
+  Private->FileSize = 0;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Attempts to complete a DHCPv4 D.O.R.A. (discover / offer / request / acknowledge) or DHCPv6
+  S.A.R.R (solicit / advertise / request / reply) sequence.
+
+  This function attempts to complete the DHCP sequence. If this sequence is completed,
+  then EFI_SUCCESS is returned, and the DhcpCompleted, ProxyOfferReceived, StationIp,
+  SubnetMask, DhcpDiscover, DhcpAck, and ProxyOffer fields of the EFI_PXE_BASE_CODE_MODE
+  structure are filled in.
+  If SortOffers is TRUE, then the cached DHCP offer packets will be sorted before
+  they are tried. If SortOffers is FALSE, then the cached DHCP offer packets will
+  be tried in the order in which they are received. Please see the Preboot Execution
+  Environment (PXE) Specification for additional details on the implementation of DHCP.
+  This function can take at least 31 seconds to timeout and return control to the
+  caller. If the DHCP sequence does not complete, then EFI_TIMEOUT will be returned.
+  If the Callback Protocol does not return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE,
+  then the DHCP sequence will be stopped and EFI_ABORTED will be returned.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  SortOffers            TRUE if the offers received should be sorted. Set to FALSE to try the
+                                offers in the order that they are received.
+
+  @retval EFI_SUCCESS           Valid DHCP has completed.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER The This parameter is NULL or does not point to a valid
+                                EFI_PXE_BASE_CODE_PROTOCOL structure.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate enough memory to complete the DHCP Protocol.
+  @retval EFI_ABORTED           The callback function aborted the DHCP Protocol.
+  @retval EFI_TIMEOUT           The DHCP Protocol timed out.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received during the DHCP session.
+  @retval EFI_NO_RESPONSE       Valid PXE offer was not received.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcDhcp (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN BOOLEAN                          SortOffers
+  )
+{
+  PXEBC_PRIVATE_DATA           *Private;
+  EFI_PXE_BASE_CODE_MODE       *Mode;
+  EFI_DHCP4_PROTOCOL           *Dhcp4;
+  EFI_DHCP4_CONFIG_DATA        Dhcp4CfgData;
+  EFI_DHCP4_MODE_DATA          Dhcp4Mode;
+  EFI_DHCP4_PACKET_OPTION      *OptList[PXEBC_DHCP4_MAX_OPTION_NUM];
+  UINT32                       OptCount;
+  EFI_STATUS                   Status;
+  EFI_ARP_CONFIG_DATA          ArpConfigData;
+  EFI_PXE_BASE_CODE_IP_FILTER  IpFilter;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status              = EFI_SUCCESS;
+  Private             = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode                = Private->PxeBc.Mode;
+  Dhcp4               = Private->Dhcp4;
+  Private->Function   = EFI_PXE_BASE_CODE_FUNCTION_DHCP;
+  Private->SortOffers = SortOffers;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  Mode->IcmpErrorReceived = FALSE;
+
+  //
+  // Stop Udp4Read instance
+  //
+  Private->Udp4Read->Configure (Private->Udp4Read, NULL);
+
+  //
+  // Initialize the DHCP options and build the option list
+  //
+  OptCount = PxeBcBuildDhcpOptions (Private, OptList, TRUE);
+
+  //
+  // Set the DHCP4 config data.
+  // The four discovery timeouts are 4, 8, 16, 32 seconds respectively.
+  //
+  ZeroMem (&Dhcp4CfgData, sizeof (EFI_DHCP4_CONFIG_DATA));
+  Dhcp4CfgData.OptionCount      = OptCount;
+  Dhcp4CfgData.OptionList       = OptList;
+  Dhcp4CfgData.Dhcp4Callback    = PxeBcDhcpCallBack;
+  Dhcp4CfgData.CallbackContext  = Private;
+  Dhcp4CfgData.DiscoverTryCount = 4;
+  Dhcp4CfgData.DiscoverTimeout  = mPxeDhcpTimeout;
+
+  Status          = Dhcp4->Configure (Dhcp4, &Dhcp4CfgData);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  //
+  // Zero those arrays to record the varies numbers of DHCP OFFERS.
+  //
+  Private->GotProxyOffer = FALSE;
+  Private->NumOffers     = 0;
+  Private->BootpIndex    = 0;
+  ZeroMem (Private->ServerCount, sizeof (Private->ServerCount));
+  ZeroMem (Private->ProxyIndex, sizeof (Private->ProxyIndex));
+
+  Status = Dhcp4->Start (Dhcp4, NULL);
+  if (EFI_ERROR (Status) && Status != EFI_ALREADY_STARTED) {
+    if (Status == EFI_ICMP_ERROR) {
+      Mode->IcmpErrorReceived = TRUE;
+    }
+    goto ON_EXIT;
+  }
+
+  Status = Dhcp4->GetModeData (Dhcp4, &Dhcp4Mode);
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  ASSERT (Dhcp4Mode.State == Dhcp4Bound);
+
+  CopyMem (&Private->StationIp, &Dhcp4Mode.ClientAddress, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Private->SubnetMask, &Dhcp4Mode.SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Private->GatewayIp, &Dhcp4Mode.RouterAddress, sizeof (EFI_IPv4_ADDRESS));
+
+  CopyMem (&Mode->StationIp, &Private->StationIp, sizeof (EFI_IPv4_ADDRESS));
+  CopyMem (&Mode->SubnetMask, &Private->SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+
+  //
+  // Check the selected offer to see whether BINL is required, if no or BINL is
+  // finished, set the various Mode members.
+  //
+  Status = PxeBcCheckSelectedOffer (Private);
+
+ON_EXIT:
+  if (EFI_ERROR (Status)) {
+    Dhcp4->Stop (Dhcp4);
+    Dhcp4->Configure (Dhcp4, NULL);
+  } else {
+    //
+    // Remove the previously configured option list and callback function
+    //
+    ZeroMem (&Dhcp4CfgData, sizeof (EFI_DHCP4_CONFIG_DATA));
+    Dhcp4->Configure (Dhcp4, &Dhcp4CfgData);
+
+    Private->AddressIsOk = TRUE;
+
+    if (!Mode->UsingIpv6) {
+      //
+      // If in IPv4 mode, configure the corresponding ARP with this new
+      // station IP address.
+      //
+      ZeroMem (&ArpConfigData, sizeof (EFI_ARP_CONFIG_DATA));
+
+      ArpConfigData.SwAddressType   = 0x0800;
+      ArpConfigData.SwAddressLength = (UINT8) sizeof (EFI_IPv4_ADDRESS);
+      ArpConfigData.StationAddress  = &Private->StationIp.v4;
+
+      Private->Arp->Configure (Private->Arp, NULL);
+      Private->Arp->Configure (Private->Arp, &ArpConfigData);
+
+      //
+      // Updated the route table. Fill the first entry.
+      //
+      Mode->RouteTableEntries                = 1;
+      Mode->RouteTable[0].IpAddr.Addr[0]     = Private->StationIp.Addr[0] & Private->SubnetMask.Addr[0];
+      Mode->RouteTable[0].SubnetMask.Addr[0] = Private->SubnetMask.Addr[0];
+      Mode->RouteTable[0].GwAddr.Addr[0]     = 0;
+
+      //
+      // Create the default route entry if there is a default router.
+      //
+      if (Private->GatewayIp.Addr[0] != 0) {
+        Mode->RouteTableEntries                = 2;
+        Mode->RouteTable[1].IpAddr.Addr[0]     = 0;
+        Mode->RouteTable[1].SubnetMask.Addr[0] = 0;
+        Mode->RouteTable[1].GwAddr.Addr[0]     = Private->GatewayIp.Addr[0];
+      }
+
+      //
+      // Flush new station IP address into Udp4CfgData and Ip4ConfigData
+      //
+      CopyMem (&Private->Udp4CfgData.StationAddress, &Private->StationIp, sizeof (EFI_IPv4_ADDRESS));
+      CopyMem (&Private->Udp4CfgData.SubnetMask, &Private->SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+      CopyMem (&Private->Ip4ConfigData.StationAddress, &Private->StationIp, sizeof (EFI_IPv4_ADDRESS));
+      CopyMem (&Private->Ip4ConfigData.SubnetMask, &Private->SubnetMask, sizeof (EFI_IPv4_ADDRESS));
+  
+      //
+      // Reconfigure the Ip4 instance to capture background ICMP packets with new station Ip address.
+      //
+      Private->Ip4->Cancel (Private->Ip4, &Private->IcmpErrorRcvToken);
+      Private->Ip4->Configure (Private->Ip4, NULL);
+  
+      Status = Private->Ip4->Configure (Private->Ip4, &Private->Ip4ConfigData);
+      if (EFI_ERROR (Status)) {
+        goto ON_EXIT;
+      }
+  
+      Status = Private->Ip4->Receive (Private->Ip4, &Private->IcmpErrorRcvToken);
+      if (EFI_ERROR (Status)) {
+        goto ON_EXIT;
+      } 
+    }
+  }
+
+  Private->Udp4Read->Configure (Private->Udp4Read, &Private->Udp4CfgData);
+
+  //
+  // Dhcp(), Discover(), and Mtftp() set the IP filter, and return with the IP 
+  // receive filter list emptied and the filter set to EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP.
+  //
+  ZeroMem(&IpFilter, sizeof (EFI_PXE_BASE_CODE_IP_FILTER));
+  IpFilter.Filters = EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP;
+  This->SetIpFilter (This, &IpFilter);
+  
+  return Status;
+}
+
+
+/**
+  Attempts to complete the PXE Boot Server and/or boot image discovery sequence.
+
+  This function attempts to complete the PXE Boot Server and/or boot image discovery
+  sequence. If this sequence is completed, then EFI_SUCCESS is returned, and the
+  PxeDiscoverValid, PxeDiscover, PxeReplyReceived, and PxeReply fields of the
+  EFI_PXE_BASE_CODE_MODE structure are filled in. If UseBis is TRUE, then the
+  PxeBisReplyReceived and PxeBisReply fields of the EFI_PXE_BASE_CODE_MODE structure
+  will also be filled in. If UseBis is FALSE, then PxeBisReplyValid will be set to FALSE.
+  In the structure referenced by parameter Info, the PXE Boot Server list, SrvList[],
+  has two uses: It is the Boot Server IP address list used for unicast discovery
+  (if the UseUCast field is TRUE), and it is the list used for Boot Server verification
+  (if the MustUseList field is TRUE). Also, if the MustUseList field in that structure
+  is TRUE and the AcceptAnyResponse field in the SrvList[] array is TRUE, any Boot
+  Server reply of that type will be accepted. If the AcceptAnyResponse field is
+  FALSE, only responses from Boot Servers with matching IP addresses will be accepted.
+  This function can take at least 10 seconds to timeout and return control to the
+  caller. If the Discovery sequence does not complete, then EFI_TIMEOUT will be
+  returned. Please see the Preboot Execution Environment (PXE) Specification for
+  additional details on the implementation of the Discovery sequence.
+  If the Callback Protocol does not return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE,
+  then the Discovery sequence is stopped and EFI_ABORTED will be returned.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  Type                  The type of bootstrap to perform.
+  @param  Layer                 Pointer to the boot server layer number to discover, which must be
+                                PXE_BOOT_LAYER_INITIAL when a new server type is being
+                                discovered.
+  @param  UseBis                TRUE if Boot Integrity Services are to be used. FALSE otherwise.
+  @param  Info                  Pointer to a data structure that contains additional information on the
+                                type of discovery operation that is to be performed.
+
+  @retval EFI_SUCCESS           The Discovery sequence has been completed.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+  @retval EFI_OUT_OF_RESOURCES  Could not allocate enough memory to complete Discovery.
+  @retval EFI_ABORTED           The callback function aborted the Discovery sequence.
+  @retval EFI_TIMEOUT           The Discovery sequence timed out.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received during the PXE discovery
+                                session.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcDiscover (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN UINT16                           Type,
+  IN UINT16                           *Layer,
+  IN BOOLEAN                          UseBis,
+  IN EFI_PXE_BASE_CODE_DISCOVER_INFO  *Info   OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA              *Private;
+  EFI_PXE_BASE_CODE_MODE          *Mode;
+  EFI_PXE_BASE_CODE_DISCOVER_INFO DefaultInfo;
+  EFI_PXE_BASE_CODE_DISCOVER_INFO *CreatedInfo;
+  EFI_PXE_BASE_CODE_SRVLIST       *SrvList;
+  EFI_PXE_BASE_CODE_SRVLIST       DefaultSrvList;
+  PXEBC_CACHED_DHCP4_PACKET       *Packet;
+  PXEBC_VENDOR_OPTION             *VendorOpt;
+  UINT16                          Index;
+  EFI_STATUS                      Status;
+  PXEBC_BOOT_SVR_ENTRY            *BootSvrEntry;
+  EFI_PXE_BASE_CODE_IP_FILTER     IpFilter;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private           = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode              = Private->PxeBc.Mode;
+  BootSvrEntry      = NULL;
+  SrvList           = NULL;
+  CreatedInfo       = NULL;
+  Status            = EFI_DEVICE_ERROR;
+  Private->Function = EFI_PXE_BASE_CODE_FUNCTION_DISCOVER;
+
+  if (!Private->AddressIsOk) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  //
+  // Stop Udp4Read instance
+  //
+  Private->Udp4Read->Configure (Private->Udp4Read, NULL);
+
+  Mode->IcmpErrorReceived = FALSE;
+
+  //
+  // If layer isn't EFI_PXE_BASE_CODE_BOOT_LAYER_INITIAL,
+  //   use the previous setting;
+  // If info isn't offered,
+  //   use the cached DhcpAck and ProxyOffer packets.
+  //
+  ZeroMem (&DefaultInfo, sizeof (EFI_PXE_BASE_CODE_DISCOVER_INFO));
+  if (*Layer != EFI_PXE_BASE_CODE_BOOT_LAYER_INITIAL) {
+
+    if (!Mode->PxeDiscoverValid || !Mode->PxeReplyReceived || (!Mode->PxeBisReplyReceived && UseBis)) {
+
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;  
+    }
+
+    DefaultInfo.IpCnt                 = 1;
+    DefaultInfo.UseUCast              = TRUE;
+
+    DefaultSrvList.Type               = Type;
+    DefaultSrvList.AcceptAnyResponse  = FALSE;
+    DefaultSrvList.IpAddr.Addr[0]     = Private->ServerIp.Addr[0];
+
+    SrvList = &DefaultSrvList;
+    Info = &DefaultInfo;
+  } else if (Info == NULL) {
+    //
+    // Create info by the cached packet before
+    //
+    Packet    = (Mode->ProxyOfferReceived) ? &Private->ProxyOffer : &Private->Dhcp4Ack;
+    VendorOpt = &Packet->PxeVendorOption;
+
+    if (!Mode->DhcpAckReceived || !IS_VALID_DISCOVER_VENDOR_OPTION (VendorOpt->BitMap)) {
+      //
+      // Address is not acquired or no discovery options.
+      //
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;  
+    }
+
+    DefaultInfo.UseMCast    = (BOOLEAN)!IS_DISABLE_MCAST_DISCOVER (VendorOpt->DiscoverCtrl);
+    DefaultInfo.UseBCast    = (BOOLEAN)!IS_DISABLE_BCAST_DISCOVER (VendorOpt->DiscoverCtrl);
+    DefaultInfo.MustUseList = (BOOLEAN) IS_ENABLE_USE_SERVER_LIST (VendorOpt->DiscoverCtrl);
+    DefaultInfo.UseUCast    = DefaultInfo.MustUseList;
+
+    if (DefaultInfo.UseMCast) {
+      //
+      // Get the multicast discover ip address from vendor option.
+      //
+      CopyMem (
+        &DefaultInfo.ServerMCastIp.Addr,
+        &VendorOpt->DiscoverMcastIp,
+        sizeof (EFI_IPv4_ADDRESS)
+        );
+    }
+
+    DefaultInfo.IpCnt = 0;
+    Info    = &DefaultInfo;
+    SrvList = Info->SrvList;
+
+    if (DefaultInfo.MustUseList) {
+      BootSvrEntry  = VendorOpt->BootSvr;
+      Status        = EFI_INVALID_PARAMETER;
+
+      while (((UINT8) (BootSvrEntry - VendorOpt->BootSvr)) < VendorOpt->BootSvrLen) {
+
+        if (BootSvrEntry->Type == HTONS (Type)) {
+          Status = EFI_SUCCESS;
+          break;
+        }
+
+        BootSvrEntry = GET_NEXT_BOOT_SVR_ENTRY (BootSvrEntry);
+      }
+
+      if (EFI_ERROR (Status)) {
+        goto ON_EXIT;
+      }
+
+      DefaultInfo.IpCnt = BootSvrEntry->IpCnt;
+
+      if (DefaultInfo.IpCnt >= 1) {
+        CreatedInfo = AllocatePool (sizeof (DefaultInfo) + (DefaultInfo.IpCnt - 1) * sizeof (*SrvList));
+        if (CreatedInfo == NULL) {
+          Status = EFI_OUT_OF_RESOURCES;
+          goto ON_EXIT;
+          
+        }     
+      
+        CopyMem (CreatedInfo, &DefaultInfo, sizeof (DefaultInfo));
+        Info    = CreatedInfo;
+        SrvList = Info->SrvList;
+      }
+
+      for (Index = 0; Index < DefaultInfo.IpCnt; Index++) {
+        CopyMem (&SrvList[Index].IpAddr, &BootSvrEntry->IpAddr[Index], sizeof (EFI_IPv4_ADDRESS));
+        SrvList[Index].AcceptAnyResponse   = FALSE;
+        SrvList[Index].Type                = BootSvrEntry->Type;
+      }
+    }
+
+  } else {
+
+    SrvList = Info->SrvList;
+
+    if (!SrvList[0].AcceptAnyResponse) {
+
+      for (Index = 1; Index < Info->IpCnt; Index++) {
+        if (SrvList[Index].AcceptAnyResponse) {
+          break;
+        }
+      }
+
+      if (Index != Info->IpCnt) {
+        Status = EFI_INVALID_PARAMETER;
+        goto ON_EXIT;        
+      }
+    }
+  }
+
+  if ((!Info->UseUCast && !Info->UseBCast && !Info->UseMCast) || (Info->MustUseList && Info->IpCnt == 0)) {
+
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+  //
+  // Execute discover by UniCast/BroadCast/MultiCast
+  //
+  if (Info->UseUCast) {
+
+    for (Index = 0; Index < Info->IpCnt; Index++) {
+
+      if (BootSvrEntry == NULL) {
+        Private->ServerIp.Addr[0] = SrvList[Index].IpAddr.Addr[0];
+      } else {
+        CopyMem (
+          &Private->ServerIp,
+          &BootSvrEntry->IpAddr[Index],
+          sizeof (EFI_IPv4_ADDRESS)
+          );
+      }
+
+      Status = PxeBcDiscvBootService (
+                Private,
+                Type,
+                Layer,
+                UseBis,
+                &SrvList[Index].IpAddr,
+                0,
+                NULL,
+                TRUE,
+                &Private->PxeReply.Packet.Ack
+                );
+      if (!EFI_ERROR (Status)) {
+        break;
+      }                
+    }
+
+  } else if (Info->UseMCast) {
+
+    Status = PxeBcDiscvBootService (
+              Private,
+              Type,
+              Layer,
+              UseBis,
+              &Info->ServerMCastIp,
+              0,
+              NULL,
+              TRUE,
+              &Private->PxeReply.Packet.Ack
+              );
+
+  } else if (Info->UseBCast) {
+
+    Status = PxeBcDiscvBootService (
+              Private,
+              Type,
+              Layer,
+              UseBis,
+              NULL,
+              Info->IpCnt,
+              SrvList,
+              TRUE,
+              &Private->PxeReply.Packet.Ack
+              );
+  }
+
+  if (EFI_ERROR (Status) || !Mode->PxeReplyReceived || (!Mode->PxeBisReplyReceived && UseBis)) {
+    if (Status == EFI_ICMP_ERROR) {
+      Mode->IcmpErrorReceived = TRUE;
+    } else {
+      Status = EFI_DEVICE_ERROR;
+    }
+    goto ON_EXIT;
+  } else {
+    PxeBcParseCachedDhcpPacket (&Private->PxeReply);
+  }
+
+  if (Mode->PxeBisReplyReceived) {
+    CopyMem (
+      &Private->ServerIp,
+      &Mode->PxeReply.Dhcpv4.BootpSiAddr,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+  }
+
+  if (CreatedInfo != NULL) {
+    FreePool (CreatedInfo);
+  }
+
+ON_EXIT:
+
+  Private->Udp4Read->Configure (Private->Udp4Read, &Private->Udp4CfgData);
+  
+  //
+  // Dhcp(), Discover(), and Mtftp() set the IP filter, and return with the IP 
+  // receive filter list emptied and the filter set to EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP.
+  //
+  ZeroMem(&IpFilter, sizeof (EFI_PXE_BASE_CODE_IP_FILTER));
+  IpFilter.Filters = EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP;
+  This->SetIpFilter (This, &IpFilter);
+  
+  return Status;
+}
+
+
+/**
+  Used to perform TFTP and MTFTP services.
+
+  This function is used to perform TFTP and MTFTP services. This includes the
+  TFTP operations to get the size of a file, read a directory, read a file, and
+  write a file. It also includes the MTFTP operations to get the size of a file,
+  read a directory, and read a file. The type of operation is specified by Operation.
+  If the callback function that is invoked during the TFTP/MTFTP operation does
+  not return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE, then EFI_ABORTED will
+  be returned.
+  For read operations, the return data will be placed in the buffer specified by
+  BufferPtr. If BufferSize is too small to contain the entire downloaded file,
+  then EFI_BUFFER_TOO_SMALL will be returned and BufferSize will be set to zero
+  or the size of the requested file (the size of the requested file is only returned
+  if the TFTP server supports TFTP options). If BufferSize is large enough for the
+  read operation, then BufferSize will be set to the size of the downloaded file,
+  and EFI_SUCCESS will be returned. Applications using the PxeBc.Mtftp() services
+  should use the get-file-size operations to determine the size of the downloaded
+  file prior to using the read-file operations-especially when downloading large
+  (greater than 64 MB) files-instead of making two calls to the read-file operation.
+  Following this recommendation will save time if the file is larger than expected
+  and the TFTP server does not support TFTP option extensions. Without TFTP option
+  extension support, the client has to download the entire file, counting and discarding
+  the received packets, to determine the file size.
+  For write operations, the data to be sent is in the buffer specified by BufferPtr.
+  BufferSize specifies the number of bytes to send. If the write operation completes
+  successfully, then EFI_SUCCESS will be returned.
+  For TFTP "get file size" operations, the size of the requested file or directory
+  is returned in BufferSize, and EFI_SUCCESS will be returned. If the TFTP server
+  does not support options, the file will be downloaded into a bit bucket and the
+  length of the downloaded file will be returned. For MTFTP "get file size" operations,
+  if the MTFTP server does not support the "get file size" option, EFI_UNSUPPORTED
+  will be returned.
+  This function can take up to 10 seconds to timeout and return control to the caller.
+  If the TFTP sequence does not complete, EFI_TIMEOUT will be returned.
+  If the Callback Protocol does not return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE,
+  then the TFTP sequence is stopped and EFI_ABORTED will be returned.
+  The format of the data returned from a TFTP read directory operation is a null-terminated
+  filename followed by a null-terminated information string, of the form
+  "size year-month-day hour:minute:second" (i.e. %d %d-%d-%d %d:%d:%f - note that
+  the seconds field can be a decimal number), where the date and time are UTC. For
+  an MTFTP read directory command, there is additionally a null-terminated multicast
+  IP address preceding the filename of the form %d.%d.%d.%d for IP v4. The final
+  entry is itself null-terminated, so that the final information string is terminated
+  with two null octets.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  Operation             The type of operation to perform.
+  @param  BufferPtr             A pointer to the data buffer.
+  @param  Overwrite             Only used on write file operations. TRUE if a file on a remote server can
+                                be overwritten.
+  @param  BufferSize            For get-file-size operations, *BufferSize returns the size of the
+                                requested file.
+  @param  BlockSize             The requested block size to be used during a TFTP transfer.
+  @param  ServerIp              The TFTP / MTFTP server IP address.
+  @param  Filename              A Null-terminated ASCII string that specifies a directory name or a file
+                                name.
+  @param  Info                  Pointer to the MTFTP information.
+  @param  DontUseBuffer         Set to FALSE for normal TFTP and MTFTP read file operation.
+
+  @retval EFI_SUCCESS           The TFTP/MTFTP operation was completed.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+  @retval EFI_BUFFER_TOO_SMALL  The buffer is not large enough to complete the read operation.
+  @retval EFI_ABORTED           The callback function aborted the TFTP/MTFTP operation.
+  @retval EFI_TIMEOUT           The TFTP/MTFTP operation timed out.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received during the MTFTP session.
+  @retval EFI_TFTP_ERROR        A TFTP error packet was received during the MTFTP session.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcMtftp (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN EFI_PXE_BASE_CODE_TFTP_OPCODE    Operation,
+  IN OUT VOID                         *BufferPtr,
+  IN BOOLEAN                          Overwrite,
+  IN OUT UINT64                       *BufferSize,
+  IN UINTN                            *BlockSize    OPTIONAL,
+  IN EFI_IP_ADDRESS                   *ServerIp,
+  IN UINT8                            *Filename,
+  IN EFI_PXE_BASE_CODE_MTFTP_INFO     *Info         OPTIONAL,
+  IN BOOLEAN                          DontUseBuffer
+  )
+{
+  PXEBC_PRIVATE_DATA           *Private;
+  EFI_MTFTP4_CONFIG_DATA       Mtftp4Config;
+  EFI_STATUS                   Status;
+  EFI_PXE_BASE_CODE_MODE       *Mode;
+  EFI_MAC_ADDRESS              TempMacAddr;
+  EFI_PXE_BASE_CODE_IP_FILTER  IpFilter;
+
+  if ((This == NULL)                                                          ||
+      (Filename == NULL)                                                      ||
+      (BufferSize == NULL)                                                    ||
+      ((ServerIp == NULL) || !NetIp4IsUnicast (NTOHL (ServerIp->Addr[0]), 0)) ||
+      ((BufferPtr == NULL) && DontUseBuffer)                                  ||
+      ((BlockSize != NULL) && (*BlockSize < 512))) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status  = EFI_DEVICE_ERROR;
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = &Private->Mode;
+
+  if (!Mode->AutoArp) {
+    //
+    // If AutoArp is set false, check arp cache
+    //
+    UpdateArpCache (This);
+    if (!FindInArpCache (Mode, &ServerIp->v4, &TempMacAddr)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  //
+  // Stop Udp4Read instance
+  //
+  Private->Udp4Read->Configure (Private->Udp4Read, NULL);
+
+  Mode->TftpErrorReceived = FALSE;
+  Mode->IcmpErrorReceived = FALSE;
+
+  Mtftp4Config.UseDefaultSetting = FALSE;
+  Mtftp4Config.TimeoutValue      = PXEBC_MTFTP_TIMEOUT;
+  Mtftp4Config.TryCount          = PXEBC_MTFTP_RETRIES;
+
+  CopyMem (
+    &Mtftp4Config.StationIp,
+    &Private->StationIp,
+    sizeof (EFI_IPv4_ADDRESS)
+    );
+  CopyMem (
+    &Mtftp4Config.SubnetMask,
+    &Private->SubnetMask,
+    sizeof (EFI_IPv4_ADDRESS)
+    );
+  CopyMem (
+    &Mtftp4Config.GatewayIp,
+    &Private->GatewayIp,
+    sizeof (EFI_IPv4_ADDRESS)
+    );
+  CopyMem (
+    &Mtftp4Config.ServerIp,
+    ServerIp,
+    sizeof (EFI_IPv4_ADDRESS)
+    );
+
+  switch (Operation) {
+
+  case EFI_PXE_BASE_CODE_TFTP_GET_FILE_SIZE:
+
+    Status = PxeBcTftpGetFileSize (
+              Private,
+              &Mtftp4Config,
+              Filename,
+              BlockSize,
+              BufferSize
+              );
+
+    break;
+
+  case EFI_PXE_BASE_CODE_TFTP_READ_FILE:
+
+    Status = PxeBcTftpReadFile (
+              Private,
+              &Mtftp4Config,
+              Filename,
+              BlockSize,
+              BufferPtr,
+              BufferSize,
+              DontUseBuffer
+              );
+
+    break;
+
+  case EFI_PXE_BASE_CODE_TFTP_WRITE_FILE:
+
+    Status = PxeBcTftpWriteFile (
+              Private,
+              &Mtftp4Config,
+              Filename,
+              Overwrite,
+              BlockSize,
+              BufferPtr,
+              BufferSize
+              );
+
+    break;
+
+  case EFI_PXE_BASE_CODE_TFTP_READ_DIRECTORY:
+
+    Status = PxeBcTftpReadDirectory (
+              Private,
+              &Mtftp4Config,
+              Filename,
+              BlockSize,
+              BufferPtr,
+              BufferSize,
+              DontUseBuffer
+              );
+
+    break;
+
+  case EFI_PXE_BASE_CODE_MTFTP_GET_FILE_SIZE:
+  case EFI_PXE_BASE_CODE_MTFTP_READ_FILE:
+  case EFI_PXE_BASE_CODE_MTFTP_READ_DIRECTORY:
+    Status = EFI_UNSUPPORTED;
+    break;
+
+  default:
+
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  if (Status == EFI_ICMP_ERROR) {
+    Mode->IcmpErrorReceived = TRUE;
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+ON_EXIT:
+  Private->Udp4Read->Configure (Private->Udp4Read, &Private->Udp4CfgData);
+  //
+  // Dhcp(), Discover(), and Mtftp() set the IP filter, and return with the IP 
+  // receive filter list emptied and the filter set to EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP.
+  //
+  ZeroMem(&IpFilter, sizeof (EFI_PXE_BASE_CODE_IP_FILTER));
+  IpFilter.Filters = EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP;
+  This->SetIpFilter (This, &IpFilter);
+
+  return Status;
+}
+
+
+/**
+  Writes a UDP packet to the network interface.
+
+  This function writes a UDP packet specified by the (optional HeaderPtr and)
+  BufferPtr parameters to the network interface. The UDP header is automatically
+  built by this routine. It uses the parameters OpFlags, DestIp, DestPort, GatewayIp,
+  SrcIp, and SrcPort to build this header. If the packet is successfully built and
+  transmitted through the network interface, then EFI_SUCCESS will be returned.
+  If a timeout occurs during the transmission of the packet, then EFI_TIMEOUT will
+  be returned. If an ICMP error occurs during the transmission of the packet, then
+  the IcmpErrorReceived field is set to TRUE, the IcmpError field is filled in and
+  EFI_ICMP_ERROR will be returned. If the Callback Protocol does not return
+  EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE, then EFI_ABORTED will be returned.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  OpFlags               The UDP operation flags.
+  @param  DestIp                The destination IP address.
+  @param  DestPort              The destination UDP port number.
+  @param  GatewayIp             The gateway IP address.
+  @param  SrcIp                 The source IP address.
+  @param  SrcPort               The source UDP port number.
+  @param  HeaderSize            An optional field which may be set to the length of a header at
+                                HeaderPtr to be prefixed to the data at BufferPtr.
+  @param  HeaderPtr             If HeaderSize is not NULL, a pointer to a header to be prefixed to the
+                                data at BufferPtr.
+  @param  BufferSize            A pointer to the size of the data at BufferPtr.
+  @param  BufferPtr             A pointer to the data to be written.
+
+  @retval EFI_SUCCESS           The UDP Write operation was completed.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_BAD_BUFFER_SIZE   The buffer is too long to be transmitted.
+  @retval EFI_ABORTED           The callback function aborted the UDP Write operation.
+  @retval EFI_TIMEOUT           The UDP Write operation timed out.
+  @retval EFI_ICMP_ERROR        An ICMP error packet was received during the UDP write session.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcUdpWrite (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN UINT16                           OpFlags,
+  IN EFI_IP_ADDRESS                   *DestIp,
+  IN EFI_PXE_BASE_CODE_UDP_PORT       *DestPort,
+  IN EFI_IP_ADDRESS                   *GatewayIp  OPTIONAL,
+  IN EFI_IP_ADDRESS                   *SrcIp      OPTIONAL,
+  IN OUT EFI_PXE_BASE_CODE_UDP_PORT   *SrcPort    OPTIONAL,
+  IN UINTN                            *HeaderSize OPTIONAL,
+  IN VOID                             *HeaderPtr  OPTIONAL,
+  IN UINTN                            *BufferSize,
+  IN VOID                             *BufferPtr
+  )
+{
+  PXEBC_PRIVATE_DATA        *Private;
+  EFI_UDP4_PROTOCOL         *Udp4;
+  EFI_UDP4_COMPLETION_TOKEN Token;
+  EFI_UDP4_TRANSMIT_DATA    *Udp4TxData;
+  UINT32                    FragCount;
+  UINT32                    DataLength;
+  EFI_UDP4_SESSION_DATA     Udp4Session;
+  EFI_STATUS                Status;
+  BOOLEAN                   IsDone;
+  EFI_PXE_BASE_CODE_MODE    *Mode;
+  EFI_MAC_ADDRESS           TempMacAddr;
+
+  IsDone = FALSE;
+
+  if ((This == NULL) || (DestIp == NULL) || (DestPort == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((GatewayIp != NULL) && !NetIp4IsUnicast (NTOHL (GatewayIp->Addr[0]), 0)) {
+    //
+    // Gateway is provided but it's not a unicast IP address.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((HeaderSize != NULL) && ((*HeaderSize == 0) || (HeaderPtr == NULL))) {
+    //
+    // The HeaderSize ptr isn't NULL and: 1. the value is zero; or 2. the HeaderPtr
+    // is NULL.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BufferSize == NULL) || ((*BufferSize != 0) && (BufferPtr == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Udp4    = Private->Udp4Write;
+  Mode    = &Private->Mode;
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (!Private->AddressIsOk && (SrcIp == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!Mode->AutoArp) {
+    //
+    // If AutoArp is set false, check arp cache
+    //
+    UpdateArpCache (This);
+    if (!FindInArpCache (Mode, &DestIp->v4, &TempMacAddr)) {
+      return EFI_DEVICE_ERROR;
+    }
+  }
+
+  Mode->IcmpErrorReceived = FALSE;
+
+  if ((Private->CurrentUdpSrcPort == 0) ||
+      ((SrcPort != NULL) && (*SrcPort != Private->CurrentUdpSrcPort))) {
+    //
+    // Port is changed, (re)configure the Udp4Write instance
+    //
+    if (SrcPort != NULL) {
+      Private->CurrentUdpSrcPort = *SrcPort;
+    }
+  }
+
+  Status = PxeBcConfigureUdpWriteInstance (
+             Udp4,
+             &Private->StationIp.v4,
+             &Private->SubnetMask.v4,
+             &Private->GatewayIp.v4,
+             &Private->CurrentUdpSrcPort
+             );
+  if (EFI_ERROR (Status)) {
+    Private->CurrentUdpSrcPort = 0;
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  ZeroMem (&Token, sizeof (EFI_UDP4_COMPLETION_TOKEN));
+  ZeroMem (&Udp4Session, sizeof (EFI_UDP4_SESSION_DATA));
+
+  CopyMem (&Udp4Session.DestinationAddress, DestIp, sizeof (EFI_IPv4_ADDRESS));
+  Udp4Session.DestinationPort = *DestPort;
+  if (SrcIp != NULL) {
+    CopyMem (&Udp4Session.SourceAddress, SrcIp, sizeof (EFI_IPv4_ADDRESS));
+  }
+  if (SrcPort != NULL) {
+    Udp4Session.SourcePort = *SrcPort;
+  }
+
+  FragCount = (HeaderSize != NULL) ? 2 : 1;
+  Udp4TxData = (EFI_UDP4_TRANSMIT_DATA *) AllocateZeroPool (sizeof (EFI_UDP4_TRANSMIT_DATA) + (FragCount - 1) * sizeof (EFI_UDP4_FRAGMENT_DATA));
+  if (Udp4TxData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Udp4TxData->FragmentCount = FragCount;
+  Udp4TxData->FragmentTable[FragCount - 1].FragmentLength = (UINT32) *BufferSize;
+  Udp4TxData->FragmentTable[FragCount - 1].FragmentBuffer = BufferPtr;
+  DataLength = (UINT32) *BufferSize;
+
+  if (FragCount == 2) {
+
+    Udp4TxData->FragmentTable[0].FragmentLength = (UINT32) *HeaderSize;
+    Udp4TxData->FragmentTable[0].FragmentBuffer = HeaderPtr;
+    DataLength += (UINT32) *HeaderSize;
+  }
+
+  if (GatewayIp != NULL) {
+    Udp4TxData->GatewayAddress  = (EFI_IPv4_ADDRESS *) GatewayIp;
+  }
+  Udp4TxData->UdpSessionData  = &Udp4Session;
+  Udp4TxData->DataLength      = DataLength;
+  Token.Packet.TxData         = Udp4TxData;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  PxeBcCommonNotify,
+                  &IsDone,
+                  &Token.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ON_EXIT;
+  }
+
+  Status = Udp4->Transmit (Udp4, &Token);
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_ICMP_ERROR) {
+      Mode->IcmpErrorReceived = TRUE;
+    }
+    goto ON_EXIT;
+  }
+
+  while (!IsDone) {
+
+    Udp4->Poll (Udp4);
+  }
+
+  Status = Token.Status;
+
+ON_EXIT:
+
+  if (Token.Event != NULL) {
+    gBS->CloseEvent (Token.Event);
+  }
+
+  FreePool (Udp4TxData);
+
+  //
+  // Reset the instance.
+  //
+  Udp4->Configure (Udp4, NULL);
+  return Status;
+}
+
+/**
+  Decide whether the incoming UDP packet is acceptable per IP filter settings
+  in provided PxeBcMode.
+
+  @param  PxeBcMode          Pointer to EFI_PXE_BASE_CODE_MODE.
+  @param  Session            Received UDP session.
+
+  @retval TRUE               The UDP package matches IP filters.
+  @retval FALSE              The UDP package doesn't matches IP filters.
+
+**/
+BOOLEAN
+CheckIpByFilter (
+  IN EFI_PXE_BASE_CODE_MODE    *PxeBcMode,
+  IN EFI_UDP4_SESSION_DATA     *Session
+  )
+{
+  UINTN                   Index;
+  EFI_IPv4_ADDRESS        Ip4Address;
+  EFI_IPv4_ADDRESS        DestIp4Address;
+
+  if ((PxeBcMode->IpFilter.Filters & EFI_PXE_BASE_CODE_IP_FILTER_PROMISCUOUS) != 0) {
+    return TRUE;
+  }
+
+  CopyMem (&DestIp4Address, &Session->DestinationAddress, sizeof (DestIp4Address));
+  if (((PxeBcMode->IpFilter.Filters & EFI_PXE_BASE_CODE_IP_FILTER_PROMISCUOUS_MULTICAST) != 0) &&
+      IP4_IS_MULTICAST (EFI_NTOHL (DestIp4Address))
+      ) {
+    return TRUE;
+  }
+
+  if (((PxeBcMode->IpFilter.Filters & EFI_PXE_BASE_CODE_IP_FILTER_BROADCAST) != 0) &&
+      IP4_IS_LOCAL_BROADCAST (EFI_NTOHL (DestIp4Address))
+      ) {
+    return TRUE;
+  }
+
+  CopyMem (&Ip4Address, &PxeBcMode->StationIp.v4, sizeof (Ip4Address));
+  if (((PxeBcMode->IpFilter.Filters & EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP) != 0) &&
+      EFI_IP4_EQUAL (&Ip4Address, &DestIp4Address)
+      ) {
+    return TRUE;
+  }
+
+  ASSERT (PxeBcMode->IpFilter.IpCnt < EFI_PXE_BASE_CODE_MAX_IPCNT);
+
+  for (Index = 0; Index < PxeBcMode->IpFilter.IpCnt; Index++) {
+    CopyMem (
+      &Ip4Address,
+      &PxeBcMode->IpFilter.IpList[Index].v4,
+      sizeof (Ip4Address)
+      );
+    if (EFI_IP4_EQUAL (&Ip4Address, &DestIp4Address)) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Reads a UDP packet from the network interface.
+
+  This function reads a UDP packet from a network interface. The data contents
+  are returned in (the optional HeaderPtr and) BufferPtr, and the size of the
+  buffer received is returned in BufferSize . If the input BufferSize is smaller
+  than the UDP packet received (less optional HeaderSize), it will be set to the
+  required size, and EFI_BUFFER_TOO_SMALL will be returned. In this case, the
+  contents of BufferPtr are undefined, and the packet is lost. If a UDP packet is
+  successfully received, then EFI_SUCCESS will be returned, and the information
+  from the UDP header will be returned in DestIp, DestPort, SrcIp, and SrcPort if
+  they are not NULL. Depending on the values of OpFlags and the DestIp, DestPort,
+  SrcIp, and SrcPort input values, different types of UDP packet receive filtering
+  will be performed. The following tables summarize these receive filter operations.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  OpFlags               The UDP operation flags.
+  @param  DestIp                The destination IP address.
+  @param  DestPort              The destination UDP port number.
+  @param  SrcIp                 The source IP address.
+  @param  SrcPort               The source UDP port number.
+  @param  HeaderSize            An optional field which may be set to the length of a header at
+                                HeaderPtr to be prefixed to the data at BufferPtr.
+  @param  HeaderPtr             If HeaderSize is not NULL, a pointer to a header to be prefixed to the
+                                data at BufferPtr.
+  @param  BufferSize            A pointer to the size of the data at BufferPtr.
+  @param  BufferPtr             A pointer to the data to be read.
+
+  @retval EFI_SUCCESS           The UDP Read operation was completed.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+  @retval EFI_BUFFER_TOO_SMALL  The packet is larger than Buffer can hold.
+  @retval EFI_ABORTED           The callback function aborted the UDP Read operation.
+  @retval EFI_TIMEOUT           The UDP Read operation timed out.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcUdpRead (
+  IN EFI_PXE_BASE_CODE_PROTOCOL                *This,
+  IN UINT16                                    OpFlags,
+  IN OUT EFI_IP_ADDRESS                        *DestIp     OPTIONAL,
+  IN OUT EFI_PXE_BASE_CODE_UDP_PORT            *DestPort   OPTIONAL,
+  IN OUT EFI_IP_ADDRESS                        *SrcIp      OPTIONAL,
+  IN OUT EFI_PXE_BASE_CODE_UDP_PORT            *SrcPort    OPTIONAL,
+  IN UINTN                                     *HeaderSize OPTIONAL,
+  IN VOID                                      *HeaderPtr  OPTIONAL,
+  IN OUT UINTN                                 *BufferSize,
+  IN VOID                                      *BufferPtr
+  )
+{
+  PXEBC_PRIVATE_DATA        *Private;
+  EFI_PXE_BASE_CODE_MODE    *Mode;
+  EFI_UDP4_PROTOCOL         *Udp4;
+  EFI_UDP4_COMPLETION_TOKEN Token;
+  EFI_UDP4_RECEIVE_DATA     *RxData;
+  EFI_UDP4_SESSION_DATA     *Session;
+  EFI_STATUS                Status;
+  BOOLEAN                   IsDone;
+  BOOLEAN                   Matched;
+  UINTN                     CopiedLen;
+  UINTN                     HeaderLen;
+  UINTN                     HeaderCopiedLen;
+  UINTN                     BufferCopiedLen;
+  UINT32                    FragmentLength;
+  UINTN                     FragmentIndex;
+  UINT8                     *FragmentBuffer;
+
+  if (This == NULL || DestIp == NULL || DestPort == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_DEST_PORT) == 0 && (DestPort == NULL)) ||
+      ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_DEST_PORT) == 0 && (SrcIp == NULL)) ||
+      ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_SRC_PORT) == 0 && (SrcPort == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (((HeaderSize != NULL) && (*HeaderSize == 0)) || ((HeaderSize != NULL) && (HeaderPtr == NULL))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((BufferSize == NULL) || (BufferPtr == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+  Udp4    = Private->Udp4Read;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  Mode->IcmpErrorReceived = FALSE;
+
+  Status = gBS->CreateEvent (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  PxeBcCommonNotify,
+                  &IsDone,
+                  &Token.Event
+                  );
+  if (EFI_ERROR (Status)) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+TRY_AGAIN:
+
+  IsDone = FALSE;
+  Status = Udp4->Receive (Udp4, &Token);
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_ICMP_ERROR) {
+      Mode->IcmpErrorReceived = TRUE;
+    }
+    goto ON_EXIT;
+  }
+
+  Udp4->Poll (Udp4);
+
+  if (!IsDone) {
+    Status = EFI_TIMEOUT;
+  } else {
+
+    //
+    // check whether this packet matches the filters
+    //
+    if (EFI_ERROR (Token.Status)){
+      goto ON_EXIT;
+    }
+
+    RxData  = Token.Packet.RxData;
+    Session = &RxData->UdpSession;
+
+    Matched = TRUE;
+
+    if ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_USE_FILTER) != 0) {
+      Matched = FALSE;
+      //
+      // Check UDP package by IP filter settings
+      //
+      if (CheckIpByFilter (Mode, Session)) {
+        Matched = TRUE;
+      }
+    }
+
+    if (Matched) {
+      Matched = FALSE;
+
+      //
+      // Match the destination ip of the received udp dgram
+      //
+      if ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_DEST_IP) != 0) {
+        Matched = TRUE;
+
+        if (DestIp != NULL) {
+          CopyMem (DestIp, &Session->DestinationAddress, sizeof (EFI_IPv4_ADDRESS));
+        }
+      } else {
+        if (DestIp != NULL) {
+          if (EFI_IP4_EQUAL (DestIp, &Session->DestinationAddress)) {
+            Matched = TRUE;
+          }
+        } else {
+          if (EFI_IP4_EQUAL (&Private->StationIp, &Session->DestinationAddress)) {
+            Matched = TRUE;
+          }
+        }
+      }
+    }
+
+    if (Matched) {
+      //
+      // Match the destination port of the received udp dgram
+      //
+      if ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_DEST_PORT) != 0) {
+
+        if (DestPort != NULL) {
+          *DestPort = Session->DestinationPort;
+        }
+      } else {
+
+        if (*DestPort != Session->DestinationPort) {
+          Matched = FALSE;
+        }
+      }
+    }
+
+    if (Matched) {
+      //
+      // Match the source ip of the received udp dgram
+      //
+      if ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_SRC_IP) != 0) {
+
+        if (SrcIp != NULL) {
+          CopyMem (SrcIp, &Session->SourceAddress, sizeof (EFI_IPv4_ADDRESS));
+        }
+      } else {
+
+        if (!EFI_IP4_EQUAL (SrcIp, &Session->SourceAddress)) {
+          Matched = FALSE;
+        }
+      }
+    }
+
+    if (Matched) {
+      //
+      // Match the source port of the received udp dgram
+      //
+      if ((OpFlags & EFI_PXE_BASE_CODE_UDP_OPFLAGS_ANY_SRC_PORT) != 0) {
+
+        if (SrcPort != NULL) {
+          *SrcPort = Session->SourcePort;
+        }
+      } else {
+
+        if (*SrcPort != Session->SourcePort) {
+          Matched = FALSE;
+        }
+      }
+    }
+
+    if (Matched) {
+      ASSERT (RxData != NULL);
+
+      HeaderLen = 0;
+      if (HeaderSize != NULL) {
+        HeaderLen = MIN (*HeaderSize, RxData->DataLength);
+      }
+
+      if (RxData->DataLength - HeaderLen > *BufferSize) {
+        Status = EFI_BUFFER_TOO_SMALL;
+      } else {
+        *HeaderSize = HeaderLen;
+        *BufferSize = RxData->DataLength - HeaderLen;
+
+        HeaderCopiedLen = 0;
+        BufferCopiedLen = 0;
+        for (FragmentIndex = 0; FragmentIndex < RxData->FragmentCount; FragmentIndex++) {
+          FragmentLength = RxData->FragmentTable[FragmentIndex].FragmentLength;
+          FragmentBuffer = RxData->FragmentTable[FragmentIndex].FragmentBuffer;
+          if (HeaderCopiedLen + FragmentLength < HeaderLen) {
+            //
+            // Copy the header part of received data.
+            //
+            CopyMem ((UINT8 *) HeaderPtr + HeaderCopiedLen, FragmentBuffer, FragmentLength);
+            HeaderCopiedLen += FragmentLength;
+          } else if (HeaderCopiedLen < HeaderLen) {
+            //
+            // Copy the header part of received data.
+            //
+            CopiedLen = HeaderLen - HeaderCopiedLen;
+            CopyMem ((UINT8 *) HeaderPtr + HeaderCopiedLen, FragmentBuffer, CopiedLen);
+            HeaderCopiedLen += CopiedLen;
+
+            //
+            // Copy the other part of received data.
+            //
+            CopyMem ((UINT8 *) BufferPtr + BufferCopiedLen, FragmentBuffer + CopiedLen, FragmentLength - CopiedLen);
+            BufferCopiedLen += (FragmentLength - CopiedLen);
+          } else {
+            //
+            // Copy the other part of received data.
+            //
+            CopyMem ((UINT8 *) BufferPtr + BufferCopiedLen, FragmentBuffer, FragmentLength);
+            BufferCopiedLen += FragmentLength;
+          }
+        }
+      }
+    } else {
+
+      Status = EFI_TIMEOUT;
+    }
+
+    //
+    // Recycle the RxData
+    //
+    gBS->SignalEvent (RxData->RecycleSignal);
+
+    if (!Matched) {
+      goto TRY_AGAIN;
+    }
+  }
+
+ON_EXIT:
+
+  Udp4->Cancel (Udp4, &Token);
+
+  gBS->CloseEvent (Token.Event);
+
+  return Status;
+}
+
+/**
+  Updates the IP receive filters of a network device and enables software filtering.
+
+  The NewFilter field is used to modify the network device's current IP receive
+  filter settings and to enable a software filter. This function updates the IpFilter
+  field of the EFI_PXE_BASE_CODE_MODE structure with the contents of NewIpFilter.
+  The software filter is used when the USE_FILTER in OpFlags is set to UdpRead().
+  The current hardware filter remains in effect no matter what the settings of OpFlags
+  are, so that the meaning of ANY_DEST_IP set in OpFlags to UdpRead() is from those
+  packets whose reception is enabled in hardware-physical NIC address (unicast),
+  broadcast address, logical address or addresses (multicast), or all (promiscuous).
+  UdpRead() does not modify the IP filter settings.
+  Dhcp(), Discover(), and Mtftp() set the IP filter, and return with the IP receive
+  filter list emptied and the filter set to EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP.
+  If an application or driver wishes to preserve the IP receive filter settings,
+  it will have to preserve the IP receive filter settings before these calls, and
+  use SetIpFilter() to restore them after the calls. If incompatible filtering is
+  requested (for example, PROMISCUOUS with anything else) or if the device does not
+  support a requested filter setting and it cannot be accommodated in software
+  (for example, PROMISCUOUS not supported), EFI_INVALID_PARAMETER will be returned.
+  The IPlist field is used to enable IPs other than the StationIP. They may be
+  multicast or unicast. If IPcnt is set as well as EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP,
+  then both the StationIP and the IPs from the IPlist will be used.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  NewFilter             Pointer to the new set of IP receive filters.
+
+  @retval EFI_SUCCESS           The IP receive filter settings were updated.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcSetIpFilter (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN EFI_PXE_BASE_CODE_IP_FILTER      *NewFilter
+  )
+{
+  EFI_STATUS                Status;
+  PXEBC_PRIVATE_DATA        *Private;
+  EFI_PXE_BASE_CODE_MODE    *Mode;
+  UINTN                     Index;
+  EFI_UDP4_CONFIG_DATA      *Udp4Cfg;
+  BOOLEAN                   PromiscuousNeed;
+  BOOLEAN                   AcceptPromiscuous;
+  BOOLEAN                   AcceptBroadcast;
+  BOOLEAN                   MultiCastUpdate;
+
+  if (This == NULL) {
+    DEBUG ((EFI_D_ERROR, "This == NULL.\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode = Private->PxeBc.Mode;
+
+  if (NewFilter == NULL) {
+    DEBUG ((EFI_D_ERROR, "NewFilter == NULL.\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NewFilter->IpCnt > EFI_PXE_BASE_CODE_MAX_IPCNT) {
+    DEBUG ((EFI_D_ERROR, "NewFilter->IpCnt > %d.\n", EFI_PXE_BASE_CODE_MAX_IPCNT));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!Mode->Started) {
+    DEBUG ((EFI_D_ERROR, "BC was not started.\n"));
+    return EFI_NOT_STARTED;
+  }
+
+  if (Mode->UsingIpv6) {
+    DEBUG ((EFI_D_ERROR, "This driver is PXE for IPv4 Only.\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  PromiscuousNeed = FALSE;
+
+  for (Index = 0; Index < NewFilter->IpCnt; ++Index) {
+    if (IP4_IS_LOCAL_BROADCAST (EFI_IP4 (NewFilter->IpList[Index].v4))) {
+      //
+      // The IP is a broadcast address.
+      //
+      DEBUG ((EFI_D_ERROR, "There is broadcast address in NewFilter.\n"));
+      return EFI_INVALID_PARAMETER;
+    }
+    if (NetIp4IsUnicast (EFI_IP4 (NewFilter->IpList[Index].v4), 0) &&
+        ((NewFilter->Filters & EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP) != 0)
+       ) {
+      //
+      // If EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP is set and IP4 address is in IpList,
+      // promiscuous mode is needed.
+      //
+      PromiscuousNeed = TRUE;
+    }
+  }
+
+  AcceptPromiscuous = FALSE;
+  AcceptBroadcast   = FALSE;
+  MultiCastUpdate   = FALSE;
+
+  if (PromiscuousNeed ||
+      ((NewFilter->Filters & EFI_PXE_BASE_CODE_IP_FILTER_PROMISCUOUS) != 0) ||
+      ((NewFilter->Filters & EFI_PXE_BASE_CODE_IP_FILTER_PROMISCUOUS_MULTICAST) != 0)
+     ) {
+    //
+    // Configure the udp4 filter to receive all packages.
+    //
+    AcceptPromiscuous  = TRUE;
+  } else if ((NewFilter->Filters & EFI_PXE_BASE_CODE_IP_FILTER_BROADCAST) != 0) {
+    //
+    // Configure the udp4 filter to receive all broadcast packages.
+    //
+    AcceptBroadcast   = TRUE;
+  }
+
+  //
+  // In multicast condition when Promiscuous FALSE and IpCnt no-zero.
+  // Here check if there is any update of the multicast ip address. If yes,
+  // we need leave the old multicast group (by Config UDP instance to NULL),
+  // and join the new multicast group.
+  //
+  if (!AcceptPromiscuous) {
+    if ((NewFilter->Filters & EFI_PXE_BASE_CODE_IP_FILTER_STATION_IP) != 0) {
+      if (Mode->IpFilter.IpCnt != NewFilter->IpCnt) {
+        MultiCastUpdate = TRUE;
+      } else if (CompareMem (Mode->IpFilter.IpList, NewFilter->IpList, NewFilter->IpCnt * sizeof (EFI_IP_ADDRESS)) != 0 ) {
+        MultiCastUpdate = TRUE;
+      }
+    }
+  }
+  
+  //
+  // Check whether we need reconfigure the UDP instance.
+  //
+  Udp4Cfg = &Private->Udp4CfgData;
+  if ((AcceptPromiscuous != Udp4Cfg->AcceptPromiscuous) ||
+  	  (AcceptBroadcast != Udp4Cfg->AcceptBroadcast)     || MultiCastUpdate) {
+    //
+    // Clear the UDP instance configuration, all joined groups will be left
+    // during the operation.
+    //
+    Private->Udp4Read->Configure (Private->Udp4Read, NULL);
+
+    //
+    // Configure the UDP instance with the new configuration.
+    //
+    Udp4Cfg->AcceptPromiscuous = AcceptPromiscuous;
+    Udp4Cfg->AcceptBroadcast   = AcceptBroadcast;
+    Status = Private->Udp4Read->Configure (Private->Udp4Read, Udp4Cfg);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // In not Promiscuous mode, need to join the new multicast group.
+    //
+    if (!AcceptPromiscuous) {
+      for (Index = 0; Index < NewFilter->IpCnt; ++Index) {
+        if (IP4_IS_MULTICAST (EFI_NTOHL (NewFilter->IpList[Index].v4))) {
+          //
+          // Join the mutilcast group.
+          //
+          Status = Private->Udp4Read->Groups (Private->Udp4Read, TRUE, &NewFilter->IpList[Index].v4);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+        }
+      }
+    }
+  }
+
+
+  //
+  // Save the new filter.
+  //
+  CopyMem (&Mode->IpFilter, NewFilter, sizeof (Mode->IpFilter));
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Uses the ARP protocol to resolve a MAC address.
+
+  This function uses the ARP protocol to resolve a MAC address. The UsingIpv6 field
+  of the EFI_PXE_BASE_CODE_MODE structure is used to determine if IPv4 or IPv6
+  addresses are being used. The IP address specified by IpAddr is used to resolve
+  a MAC address. If the ARP protocol succeeds in resolving the specified address,
+  then the ArpCacheEntries and ArpCache fields of the EFI_PXE_BASE_CODE_MODE structure
+  are updated, and EFI_SUCCESS is returned. If MacAddr is not NULL, the resolved
+  MAC address is placed there as well.  If the PXE Base Code protocol is in the
+  stopped state, then EFI_NOT_STARTED is returned. If the ARP protocol encounters
+  a timeout condition while attempting to resolve an address, then EFI_TIMEOUT is
+  returned. If the Callback Protocol does not return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE,
+  then EFI_ABORTED is returned.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  IpAddr                Pointer to the IP address that is used to resolve a MAC address.
+  @param  MacAddr               If not NULL, a pointer to the MAC address that was resolved with the
+                                ARP protocol.
+
+  @retval EFI_SUCCESS           The IP or MAC address was resolved.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+  @retval EFI_DEVICE_ERROR      The network device encountered an error during this operation.
+  @retval EFI_ICMP_ERROR        Something error occur with the ICMP packet message.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcArp (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       * This,
+  IN EFI_IP_ADDRESS                   * IpAddr,
+  IN EFI_MAC_ADDRESS                  * MacAddr OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_STATUS              Status;
+  EFI_MAC_ADDRESS         TempMacAddr;
+
+  if (This == NULL || IpAddr == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (!Private->AddressIsOk || Mode->UsingIpv6) {
+    //
+    // We can't resolve the IP address if we don't have a local address now.
+    // Don't have ARP for IPv6.
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Mode->IcmpErrorReceived = FALSE;
+
+  if (!Mode->AutoArp) {
+    //
+    // If AutoArp is set false, check arp cache
+    //
+    UpdateArpCache (This);
+    if (!FindInArpCache (Mode, &IpAddr->v4, &TempMacAddr)) {
+      return EFI_DEVICE_ERROR;
+    }
+  } else {
+    Status = Private->Arp->Request (Private->Arp, &IpAddr->v4, NULL, &TempMacAddr);
+    if (EFI_ERROR (Status)) {
+      if (Status == EFI_ICMP_ERROR) {
+        Mode->IcmpErrorReceived = TRUE;
+      }
+      return Status;
+    }
+  }
+
+  if (MacAddr != NULL) {
+    CopyMem (MacAddr, &TempMacAddr, sizeof (EFI_MAC_ADDRESS));
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Updates the parameters that affect the operation of the PXE Base Code Protocol.
+
+  This function sets parameters that affect the operation of the PXE Base Code Protocol.
+  The parameter specified by NewAutoArp is used to control the generation of ARP
+  protocol packets. If NewAutoArp is TRUE, then ARP Protocol packets will be generated
+  as required by the PXE Base Code Protocol. If NewAutoArp is FALSE, then no ARP
+  Protocol packets will be generated. In this case, the only mappings that are
+  available are those stored in the ArpCache of the EFI_PXE_BASE_CODE_MODE structure.
+  If there are not enough mappings in the ArpCache to perform a PXE Base Code Protocol
+  service, then the service will fail. This function updates the AutoArp field of
+  the EFI_PXE_BASE_CODE_MODE structure to NewAutoArp.
+  The SetParameters() call must be invoked after a Callback Protocol is installed
+  to enable the use of callbacks.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  NewAutoArp            If not NULL, a pointer to a value that specifies whether to replace the
+                                current value of AutoARP.
+  @param  NewSendGUID           If not NULL, a pointer to a value that specifies whether to replace the
+                                current value of SendGUID.
+  @param  NewTTL                If not NULL, a pointer to be used in place of the current value of TTL,
+                                the "time to live" field of the IP header.
+  @param  NewToS                If not NULL, a pointer to be used in place of the current value of ToS,
+                                the "type of service" field of the IP header.
+  @param  NewMakeCallback       If not NULL, a pointer to a value that specifies whether to replace the
+                                current value of the MakeCallback field of the Mode structure.
+
+  @retval EFI_SUCCESS           The new parameters values were updated.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcSetParameters (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       *This,
+  IN BOOLEAN                          *NewAutoArp OPTIONAL,
+  IN BOOLEAN                          *NewSendGUID OPTIONAL,
+  IN UINT8                            *NewTTL OPTIONAL,
+  IN UINT8                            *NewToS OPTIONAL,
+  IN BOOLEAN                          *NewMakeCallback  // OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_STATUS              Status;
+
+  Status = EFI_SUCCESS;
+
+  if (This == NULL) {
+    Status = EFI_INVALID_PARAMETER;
+    goto ON_EXIT;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (NewSendGUID != NULL && *NewSendGUID) {
+    //
+    // FixMe, cann't locate SendGuid
+    //
+  }
+
+  if (NewMakeCallback != NULL && *NewMakeCallback) {
+
+    Status = gBS->HandleProtocol (
+                    Private->Controller,
+                    &gEfiPxeBaseCodeCallbackProtocolGuid,
+                    (VOID **) &Private->PxeBcCallback
+                    );
+    if (EFI_ERROR (Status) || (Private->PxeBcCallback->Callback == NULL)) {
+
+      Status = EFI_INVALID_PARAMETER;
+      goto ON_EXIT;
+    }
+  }
+
+  if (!Mode->Started) {
+    Status = EFI_NOT_STARTED;
+    goto ON_EXIT;
+  }
+
+  if (NewMakeCallback != NULL) {
+
+    if (*NewMakeCallback) {
+      //
+      // Update the Callback protocol.
+      //
+      Status = gBS->HandleProtocol (
+                      Private->Controller,
+                      &gEfiPxeBaseCodeCallbackProtocolGuid,
+                      (VOID **) &Private->PxeBcCallback
+                      );
+
+      if (EFI_ERROR (Status) || (Private->PxeBcCallback->Callback == NULL)) {
+        Status = EFI_INVALID_PARAMETER;
+        goto ON_EXIT;
+      }
+    } else {
+      Private->PxeBcCallback = NULL;
+    }
+
+    Mode->MakeCallbacks = *NewMakeCallback;
+  }
+
+  if (NewAutoArp != NULL) {
+    Mode->AutoArp = *NewAutoArp;
+  }
+
+  if (NewSendGUID != NULL) {
+    Mode->SendGUID = *NewSendGUID;
+  }
+
+  if (NewTTL != NULL) {
+    Mode->TTL = *NewTTL;
+  }
+
+  if (NewToS != NULL) {
+    Mode->ToS = *NewToS;
+  }
+
+ON_EXIT:
+  return Status;
+}
+
+/**
+  Updates the station IP address and/or subnet mask values of a network device.
+
+  This function updates the station IP address and/or subnet mask values of a network
+  device. The NewStationIp field is used to modify the network device's current IP address.
+  If NewStationIP is NULL, then the current IP address will not be modified. Otherwise,
+  this function updates the StationIp field of the EFI_PXE_BASE_CODE_MODE structure
+  with NewStationIp. The NewSubnetMask field is used to modify the network device's current subnet
+  mask. If NewSubnetMask is NULL, then the current subnet mask will not be modified.
+  Otherwise, this function updates the SubnetMask field of the EFI_PXE_BASE_CODE_MODE
+  structure with NewSubnetMask.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  NewStationIp          Pointer to the new IP address to be used by the network device.
+  @param  NewSubnetMask         Pointer to the new subnet mask to be used by the network device.
+
+  @retval EFI_SUCCESS           The new station IP address and/or subnet mask were updated.
+  @retval EFI_NOT_STARTED       The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER One or more parameters are invalid.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcSetStationIP (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       * This,
+  IN EFI_IP_ADDRESS                   * NewStationIp  OPTIONAL,
+  IN EFI_IP_ADDRESS                   * NewSubnetMask OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+  EFI_ARP_CONFIG_DATA     ArpConfigData;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NewStationIp != NULL && !NetIp4IsUnicast (NTOHL (NewStationIp->Addr[0]), 0)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (NewSubnetMask != NULL && !IP4_IS_VALID_NETMASK (NTOHL (NewSubnetMask->Addr[0]))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (NewStationIp != NULL) {
+    CopyMem (&Mode->StationIp, NewStationIp, sizeof (EFI_IP_ADDRESS));
+    CopyMem (&Private->StationIp, NewStationIp, sizeof (EFI_IP_ADDRESS));
+  }
+
+  if (NewSubnetMask != NULL) {
+    CopyMem (&Mode->SubnetMask, NewSubnetMask, sizeof (EFI_IP_ADDRESS));
+    CopyMem (&Private->SubnetMask ,NewSubnetMask, sizeof (EFI_IP_ADDRESS));
+  }
+
+  Private->AddressIsOk = TRUE;
+
+  if (!Mode->UsingIpv6) {
+    //
+    // If in IPv4 mode, configure the corresponding ARP with this new
+    // station IP address.
+    //
+    ZeroMem (&ArpConfigData, sizeof (EFI_ARP_CONFIG_DATA));
+
+    ArpConfigData.SwAddressType   = 0x0800;
+    ArpConfigData.SwAddressLength = (UINT8) sizeof (EFI_IPv4_ADDRESS);
+    ArpConfigData.StationAddress  = &Private->StationIp.v4;
+
+    Private->Arp->Configure (Private->Arp, NULL);
+    Private->Arp->Configure (Private->Arp, &ArpConfigData);
+
+    //
+    // Update the route table.
+    //
+    Mode->RouteTableEntries                = 1;
+    Mode->RouteTable[0].IpAddr.Addr[0]     = Private->StationIp.Addr[0] & Private->SubnetMask.Addr[0];
+    Mode->RouteTable[0].SubnetMask.Addr[0] = Private->SubnetMask.Addr[0];
+    Mode->RouteTable[0].GwAddr.Addr[0]     = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Updates the contents of the cached DHCP and Discover packets.
+
+  The pointers to the new packets are used to update the contents of the cached
+  packets in the EFI_PXE_BASE_CODE_MODE structure.
+
+  @param  This                   Pointer to the EFI_PXE_BASE_CODE_PROTOCOL instance.
+  @param  NewDhcpDiscoverValid   Pointer to a value that will replace the current
+                                 DhcpDiscoverValid field.
+  @param  NewDhcpAckReceived     Pointer to a value that will replace the current
+                                 DhcpAckReceived field.
+  @param  NewProxyOfferReceived  Pointer to a value that will replace the current
+                                 ProxyOfferReceived field.
+  @param  NewPxeDiscoverValid    Pointer to a value that will replace the current
+                                 ProxyOfferReceived field.
+  @param  NewPxeReplyReceived    Pointer to a value that will replace the current
+                                 PxeReplyReceived field.
+  @param  NewPxeBisReplyReceived Pointer to a value that will replace the current
+                                 PxeBisReplyReceived field.
+  @param  NewDhcpDiscover        Pointer to the new cached DHCP Discover packet contents.
+  @param  NewDhcpAck             Pointer to the new cached DHCP Ack packet contents.
+  @param  NewProxyOffer          Pointer to the new cached Proxy Offer packet contents.
+  @param  NewPxeDiscover         Pointer to the new cached PXE Discover packet contents.
+  @param  NewPxeReply            Pointer to the new cached PXE Reply packet contents.
+  @param  NewPxeBisReply         Pointer to the new cached PXE BIS Reply packet contents.
+
+  @retval EFI_SUCCESS            The cached packet contents were updated.
+  @retval EFI_NOT_STARTED        The PXE Base Code Protocol is in the stopped state.
+  @retval EFI_INVALID_PARAMETER  This is NULL or not point to a valid EFI_PXE_BASE_CODE_PROTOCOL structure.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeBcSetPackets (
+  IN EFI_PXE_BASE_CODE_PROTOCOL       * This,
+  IN BOOLEAN                          * NewDhcpDiscoverValid OPTIONAL,
+  IN BOOLEAN                          * NewDhcpAckReceived OPTIONAL,
+  IN BOOLEAN                          * NewProxyOfferReceived OPTIONAL,
+  IN BOOLEAN                          * NewPxeDiscoverValid OPTIONAL,
+  IN BOOLEAN                          * NewPxeReplyReceived OPTIONAL,
+  IN BOOLEAN                          * NewPxeBisReplyReceived OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewDhcpDiscover OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewDhcpAck OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewProxyOffer OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewPxeDiscover OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewPxeReply OPTIONAL,
+  IN EFI_PXE_BASE_CODE_PACKET         * NewPxeBisReply OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA      *Private;
+  EFI_PXE_BASE_CODE_MODE  *Mode;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = PXEBC_PRIVATE_DATA_FROM_PXEBC (This);
+  Mode    = Private->PxeBc.Mode;
+
+  if (!Mode->Started) {
+    return EFI_NOT_STARTED;
+  }
+
+  if (NewDhcpDiscoverValid != NULL) {
+    Mode->DhcpDiscoverValid = *NewDhcpDiscoverValid;
+  }
+
+  if (NewDhcpAckReceived != NULL) {
+    Mode->DhcpAckReceived = *NewDhcpAckReceived;
+  }
+
+  if (NewProxyOfferReceived != NULL) {
+    Mode->ProxyOfferReceived = *NewProxyOfferReceived;
+  }
+
+  if (NewPxeDiscoverValid != NULL) {
+    Mode->PxeDiscoverValid = *NewPxeDiscoverValid;
+  }
+
+  if (NewPxeReplyReceived != NULL) {
+    Mode->PxeReplyReceived = *NewPxeReplyReceived;
+  }
+
+  if (NewPxeBisReplyReceived != NULL) {
+    Mode->PxeBisReplyReceived = *NewPxeBisReplyReceived;
+  }
+
+  if (NewDhcpDiscover != NULL) {
+    CopyMem (&Mode->DhcpDiscover, NewDhcpDiscover, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  if (NewDhcpAck != NULL) {
+    CopyMem (&Mode->DhcpAck, NewDhcpAck, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  if (NewProxyOffer != NULL) {
+    CopyMem (&Mode->ProxyOffer, NewProxyOffer, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  if (NewPxeDiscover != NULL) {
+    CopyMem (&Mode->PxeDiscover, NewPxeDiscover, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  if (NewPxeReply != NULL) {
+    CopyMem (&Mode->PxeReply, NewPxeReply, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  if (NewPxeBisReply != NULL) {
+    CopyMem (&Mode->PxeBisReply, NewPxeBisReply, sizeof (EFI_PXE_BASE_CODE_PACKET));
+  }
+
+  return EFI_SUCCESS;
+}
+
+EFI_PXE_BASE_CODE_PROTOCOL  mPxeBcProtocolTemplate = {
+  EFI_PXE_BASE_CODE_PROTOCOL_REVISION,
+  EfiPxeBcStart,
+  EfiPxeBcStop,
+  EfiPxeBcDhcp,
+  EfiPxeBcDiscover,
+  EfiPxeBcMtftp,
+  EfiPxeBcUdpWrite,
+  EfiPxeBcUdpRead,
+  EfiPxeBcSetIpFilter,
+  EfiPxeBcArp,
+  EfiPxeBcSetParameters,
+  EfiPxeBcSetStationIP,
+  EfiPxeBcSetPackets,
+  NULL
+};
+
+/**
+  Callback function that is invoked when the PXE Base Code Protocol is about to transmit, has
+  received, or is waiting to receive a packet.
+
+  This function is invoked when the PXE Base Code Protocol is about to transmit, has received,
+  or is waiting to receive a packet. Parameters Function and Received specify the type of event.
+  Parameters PacketLen and Packet specify the packet that generated the event. If these fields
+  are zero and NULL respectively, then this is a status update callback. If the operation specified
+  by Function is to continue, then CALLBACK_STATUS_CONTINUE should be returned. If the operation
+  specified by Function should be aborted, then CALLBACK_STATUS_ABORT should be returned. Due to
+  the polling nature of UEFI device drivers, a callback function should not execute for more than 5 ms.
+  The SetParameters() function must be called after a Callback Protocol is installed to enable the
+  use of callbacks.
+
+  @param  This                  Pointer to the EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL instance.
+  @param  Function              The PXE Base Code Protocol function that is waiting for an event.
+  @param  Received              TRUE if the callback is being invoked due to a receive event. FALSE if
+                                the callback is being invoked due to a transmit event.
+  @param  PacketLength          The length, in bytes, of Packet. This field will have a value of zero if
+                                this is a wait for receive event.
+  @param  PacketPtr             If Received is TRUE, a pointer to the packet that was just received;
+                                otherwise a pointer to the packet that is about to be transmitted.
+
+  @retval EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE if Function specifies a continue operation
+  @retval EFI_PXE_BASE_CODE_CALLBACK_STATUS_ABORT    if Function specifies an abort operation
+
+**/
+EFI_PXE_BASE_CODE_CALLBACK_STATUS
+EFIAPI
+EfiPxeLoadFileCallback (
+  IN EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL  * This,
+  IN EFI_PXE_BASE_CODE_FUNCTION           Function,
+  IN BOOLEAN                              Received,
+  IN UINT32                               PacketLength,
+  IN EFI_PXE_BASE_CODE_PACKET             * PacketPtr OPTIONAL
+  )
+{
+  EFI_INPUT_KEY Key;
+  EFI_STATUS    Status;
+
+  //
+  // Catch Ctrl-C or ESC to abort.
+  //
+  Status = gST->ConIn->ReadKeyStroke (gST->ConIn, &Key);
+
+  if (!EFI_ERROR (Status)) {
+
+    if (Key.ScanCode == SCAN_ESC || Key.UnicodeChar == (0x1F & 'c')) {
+
+      return EFI_PXE_BASE_CODE_CALLBACK_STATUS_ABORT;
+    }
+  }
+  //
+  // No print if receive packet
+  //
+  if (Received) {
+    return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE;
+  }
+  //
+  // Print only for three functions
+  //
+  switch (Function) {
+
+  case EFI_PXE_BASE_CODE_FUNCTION_MTFTP:
+    //
+    // Print only for open MTFTP packets, not every MTFTP packets
+    //
+    if (PacketLength != 0 && PacketPtr != NULL) {
+      if (PacketPtr->Raw[0x1C] != 0x00 || PacketPtr->Raw[0x1D] != 0x01) {
+        return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE;
+      }
+    }
+    break;
+
+  case EFI_PXE_BASE_CODE_FUNCTION_DHCP:
+  case EFI_PXE_BASE_CODE_FUNCTION_DISCOVER:
+    break;
+
+  default:
+    return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE;
+  }
+
+  if (PacketLength != 0 && PacketPtr != NULL) {
+    //
+    // Print '.' when transmit a packet
+    //
+    AsciiPrint (".");
+
+  }
+
+  return EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE;
+}
+
+EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL mPxeBcCallBackTemplate = {
+  EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL_REVISION,
+  EfiPxeLoadFileCallback
+};
+
+
+/**
+  Find the boot file.
+
+  @param  Private      Pointer to PxeBc private data.
+  @param  BufferSize   Pointer to buffer size.
+  @param  Buffer       Pointer to buffer.
+
+  @retval EFI_SUCCESS          Discover the boot file successfully.
+  @retval EFI_TIMEOUT          The TFTP/MTFTP operation timed out.
+  @retval EFI_ABORTED          PXE bootstrap server, so local boot need abort.
+  @retval EFI_BUFFER_TOO_SMALL The buffer is too small to load the boot file.
+
+**/
+EFI_STATUS
+DiscoverBootFile (
+  IN     PXEBC_PRIVATE_DATA  *Private,
+  IN OUT UINT64              *BufferSize,
+  IN     VOID                *Buffer
+  )
+{
+  EFI_PXE_BASE_CODE_PROTOCOL  *PxeBc;
+  EFI_PXE_BASE_CODE_MODE      *Mode;
+  EFI_STATUS                  Status;
+  UINT16                      Type;
+  UINT16                      Layer;
+  BOOLEAN                     UseBis;
+  PXEBC_CACHED_DHCP4_PACKET   *Packet;
+  UINT16                      Value;
+
+  PxeBc = &Private->PxeBc;
+  Mode  = PxeBc->Mode;
+  Type  = EFI_PXE_BASE_CODE_BOOT_TYPE_BOOTSTRAP;
+  Layer = EFI_PXE_BASE_CODE_BOOT_LAYER_INITIAL;
+
+  //
+  // do DHCP.
+  //
+  Status = PxeBc->Dhcp (PxeBc, TRUE);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Select a boot server
+  //
+  Status = PxeBcSelectBootPrompt (Private);
+
+  if (Status == EFI_SUCCESS) {
+    Status = PxeBcSelectBootMenu (Private, &Type, TRUE);
+  } else if (Status == EFI_TIMEOUT) {
+    Status = PxeBcSelectBootMenu (Private, &Type, FALSE);
+  }
+
+  if (!EFI_ERROR (Status)) {
+
+    if (Type == EFI_PXE_BASE_CODE_BOOT_TYPE_BOOTSTRAP) {
+      //
+      // Local boot(PXE bootstrap server) need abort
+      //
+      return EFI_ABORTED;
+    }
+
+    UseBis  = (BOOLEAN) (Mode->BisSupported && Mode->BisDetected);
+    Status  = PxeBc->Discover (PxeBc, Type, &Layer, UseBis, NULL);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  *BufferSize = 0;
+
+  //
+  // Get bootfile name and (m)tftp server ip addresss
+  //
+  if (Mode->PxeReplyReceived) {
+    Packet = &Private->PxeReply;
+  } else if (Mode->ProxyOfferReceived) {
+    Packet = &Private->ProxyOffer;
+  } else {
+    Packet = &Private->Dhcp4Ack;
+  }
+
+  //
+  // Use siaddr(next server) in DHCPOFFER packet header, if zero, use option 54(server identifier)
+  // in DHCPOFFER packet.
+  // (It does not comply with PXE Spec, Ver2.1)
+  //
+  if (EFI_IP4_EQUAL (&Packet->Packet.Offer.Dhcp4.Header.ServerAddr, &mZeroIp4Addr)) {
+    CopyMem (
+      &Private->ServerIp,
+      Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_SERVER_ID]->Data,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+  } else {
+    CopyMem (
+      &Private->ServerIp,
+      &Packet->Packet.Offer.Dhcp4.Header.ServerAddr,
+      sizeof (EFI_IPv4_ADDRESS)
+      );
+  }
+  if (Private->ServerIp.Addr[0] == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  ASSERT (Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE] != NULL);
+
+  //
+  // bootlfile name
+  //
+  Private->BootFileName = (CHAR8 *) (Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE]->Data);
+
+  if (Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE_LEN] != NULL) {
+    //
+    // Already have the bootfile length option, compute the file size
+    //
+    CopyMem (&Value, Packet->Dhcp4Option[PXEBC_DHCP4_TAG_INDEX_BOOTFILE_LEN]->Data, sizeof (Value));
+    Value       = NTOHS (Value);
+    *BufferSize = 512 * Value;
+    Status      = EFI_BUFFER_TOO_SMALL;
+  } else {
+    //
+    // Get the bootfile size from tftp
+    //
+    Status = PxeBc->Mtftp (
+                      PxeBc,
+                      EFI_PXE_BASE_CODE_TFTP_GET_FILE_SIZE,
+                      Buffer,
+                      FALSE,
+                      BufferSize,
+                      &Private->BlockSize,
+                      &Private->ServerIp,
+                      (UINT8 *) Private->BootFileName,
+                      NULL,
+                      FALSE
+                      );
+  }
+
+  Private->FileSize = (UINTN) *BufferSize;
+
+  return Status;
+}
+
+/**
+  Causes the driver to load a specified file.
+
+  @param  This                  Protocol instance pointer.
+  @param  FilePath              The device specific path of the file to load.
+  @param  BootPolicy            If TRUE, indicates that the request originates from the
+                                boot manager is attempting to load FilePath as a boot
+                                selection. If FALSE, then FilePath must match as exact file
+                                to be loaded.
+  @param  BufferSize            On input the size of Buffer in bytes. On output with a return
+                                code of EFI_SUCCESS, the amount of data transferred to
+                                Buffer. On output with a return code of EFI_BUFFER_TOO_SMALL,
+                                the size of Buffer required to retrieve the requested file.
+  @param  Buffer                The memory buffer to transfer the file to. IF Buffer is NULL,
+                                then no the size of the requested file is returned in
+                                BufferSize.
+
+  @retval EFI_SUCCESS                 The file was loaded.
+  @retval EFI_UNSUPPORTED             The device does not support the provided BootPolicy
+  @retval EFI_INVALID_PARAMETER       FilePath is not a valid device path, or
+                                      BufferSize is NULL.
+  @retval EFI_NO_MEDIA                No medium was present to load the file.
+  @retval EFI_DEVICE_ERROR            The file was not loaded due to a device error.
+  @retval EFI_NO_RESPONSE             The remote system did not respond.
+  @retval EFI_NOT_FOUND               The file was not found.
+  @retval EFI_ABORTED                 The file load process was manually cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeLoadFile (
+  IN EFI_LOAD_FILE_PROTOCOL           * This,
+  IN EFI_DEVICE_PATH_PROTOCOL         * FilePath,
+  IN BOOLEAN                          BootPolicy,
+  IN OUT UINTN                        *BufferSize,
+  IN VOID                             *Buffer OPTIONAL
+  )
+{
+  PXEBC_PRIVATE_DATA          *Private;
+  EFI_PXE_BASE_CODE_PROTOCOL  *PxeBc;
+  BOOLEAN                     NewMakeCallback;
+  EFI_STATUS                  Status;
+  UINT64                      TmpBufSize;
+  BOOLEAN                     MediaPresent;
+
+  if (FilePath == NULL || !IsDevicePathEnd (FilePath)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Private         = PXEBC_PRIVATE_DATA_FROM_LOADFILE (This);
+  PxeBc           = &Private->PxeBc;
+  NewMakeCallback = FALSE;
+  Status          = EFI_DEVICE_ERROR;
+
+  if (This == NULL || BufferSize == NULL) {
+
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Only support BootPolicy
+  //
+  if (!BootPolicy) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check media status before PXE start
+  //
+  MediaPresent = TRUE;
+  NetLibDetectMedia (Private->Controller, &MediaPresent);
+  if (!MediaPresent) {
+    return EFI_NO_MEDIA;
+  }
+
+  Status = PxeBc->Start (PxeBc, FALSE);
+  if (EFI_ERROR (Status) && (Status != EFI_ALREADY_STARTED)) {
+    return Status;
+  }
+
+  Status = gBS->HandleProtocol (
+                  Private->Controller,
+                  &gEfiPxeBaseCodeCallbackProtocolGuid,
+                  (VOID **) &Private->PxeBcCallback
+                  );
+  if (Status == EFI_UNSUPPORTED) {
+
+    CopyMem (&Private->LoadFileCallback, &mPxeBcCallBackTemplate, sizeof (Private->LoadFileCallback));
+
+    Status = gBS->InstallProtocolInterface (
+                    &Private->Controller,
+                    &gEfiPxeBaseCodeCallbackProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &Private->LoadFileCallback
+                    );
+
+    NewMakeCallback = (BOOLEAN) (Status == EFI_SUCCESS);
+
+    Status          = PxeBc->SetParameters (PxeBc, NULL, NULL, NULL, NULL, &NewMakeCallback);
+    if (EFI_ERROR (Status)) {
+      PxeBc->Stop (PxeBc);
+      return Status;
+    }
+  }
+
+  if (Private->FileSize == 0) {
+    TmpBufSize  = 0;
+    Status      = DiscoverBootFile (Private, &TmpBufSize, Buffer);
+
+    if (sizeof (UINTN) < sizeof (UINT64) && (TmpBufSize > 0xFFFFFFFF)) {
+      Status = EFI_DEVICE_ERROR;
+    } else if (TmpBufSize > 0 && *BufferSize >= (UINTN) TmpBufSize && Buffer != NULL) {
+      *BufferSize = (UINTN) TmpBufSize;
+      Status = PxeBc->Mtftp (
+                        PxeBc,
+                        EFI_PXE_BASE_CODE_TFTP_READ_FILE,
+                        Buffer,
+                        FALSE,
+                        &TmpBufSize,
+                        &Private->BlockSize,
+                        &Private->ServerIp,
+                        (UINT8 *) Private->BootFileName,
+                        NULL,
+                        FALSE
+                        );
+    } else if (TmpBufSize > 0) {
+      *BufferSize = (UINTN) TmpBufSize;
+      Status      = EFI_BUFFER_TOO_SMALL;
+    }
+  } else if (Buffer == NULL || Private->FileSize > *BufferSize) {
+    *BufferSize = Private->FileSize;
+    Status      = EFI_BUFFER_TOO_SMALL;
+  } else {
+    //
+    // Download the file.
+    //
+    TmpBufSize = (UINT64) (*BufferSize);
+    Status = PxeBc->Mtftp (
+                      PxeBc,
+                      EFI_PXE_BASE_CODE_TFTP_READ_FILE,
+                      Buffer,
+                      FALSE,
+                      &TmpBufSize,
+                      &Private->BlockSize,
+                      &Private->ServerIp,
+                      (UINT8 *) Private->BootFileName,
+                      NULL,
+                      FALSE
+                      );
+  }
+  //
+  // If we added a callback protocol, now is the time to remove it.
+  //
+  if (NewMakeCallback) {
+
+    NewMakeCallback = FALSE;
+
+    PxeBc->SetParameters (PxeBc, NULL, NULL, NULL, NULL, &NewMakeCallback);
+
+    gBS->UninstallProtocolInterface (
+          Private->Controller,
+          &gEfiPxeBaseCodeCallbackProtocolGuid,
+          &Private->LoadFileCallback
+          );
+  }
+
+  //
+  // Check download status
+  //
+  if (Status == EFI_SUCCESS) {
+    //
+    // The DHCP4 can have only one configured child instance so we need to stop
+    // reset the DHCP4 child before we return. Otherwise the other programs which 
+    // also need to use DHCP4 will be impacted.
+    // The functionality of PXE Base Code protocol will not be stopped,
+    // when downloading is successfully.
+    //
+    Private->Dhcp4->Stop (Private->Dhcp4);
+    Private->Dhcp4->Configure (Private->Dhcp4, NULL);
+    return EFI_SUCCESS;
+
+  } else if (Status == EFI_BUFFER_TOO_SMALL) {
+    if (Buffer != NULL) {
+      AsciiPrint ("PXE-E05: Download buffer is smaller than requested file.\n");
+    } else {
+      //
+      // The functionality of PXE Base Code protocol will not be stopped.
+      //
+      return Status;
+    }
+
+  } else if (Status == EFI_DEVICE_ERROR) {
+    AsciiPrint ("PXE-E07: Network device error.\n");
+
+  } else if (Status == EFI_OUT_OF_RESOURCES) {
+    AsciiPrint ("PXE-E09: Could not allocate I/O buffers.\n");
+
+  } else if (Status == EFI_NO_MEDIA) {
+    AsciiPrint ("PXE-E12: Could not detect network connection.\n");
+
+  } else if (Status == EFI_NO_RESPONSE) {
+    AsciiPrint ("PXE-E16: No offer received.\n");
+
+  } else if (Status == EFI_TIMEOUT) {
+    AsciiPrint ("PXE-E18: Server response timeout.\n");
+
+  } else if (Status == EFI_ABORTED) {
+    AsciiPrint ("PXE-E21: Remote boot cancelled.\n");
+
+  } else if (Status == EFI_ICMP_ERROR) {
+    AsciiPrint ("PXE-E22: Client received ICMP error from server.\n");
+
+  } else if (Status == EFI_TFTP_ERROR) {
+    AsciiPrint ("PXE-E23: Client received TFTP error from server.\n");
+
+  } else {
+    AsciiPrint ("PXE-E99: Unexpected network error.\n");
+  }
+
+  PxeBc->Stop (PxeBc);
+
+  return Status;
+}
+
+EFI_LOAD_FILE_PROTOCOL  mLoadFileProtocolTemplate = { EfiPxeLoadFile };
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.h b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.h
new file mode 100644
index 0000000000..ce8d8add3f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcImpl.h
@@ -0,0 +1,187 @@
+/** @file
+
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef __EFI_PXEBC_IMPL_H__
+#define __EFI_PXEBC_IMPL_H__
+
+
+typedef struct _PXEBC_PRIVATE_DATA  PXEBC_PRIVATE_DATA;
+
+#include <Uefi.h>
+
+#include <Guid/SmBios.h>
+#include <IndustryStandard/SmBios.h>
+#include <Protocol/Dhcp4.h>
+#include <Protocol/PxeBaseCode.h>
+#include <Protocol/Mtftp4.h>
+#include <Protocol/Udp4.h>
+#include <Protocol/LoadFile.h>
+#include <Protocol/NetworkInterfaceIdentifier.h>
+#include <Protocol/PxeBaseCodeCallBack.h>
+#include <Protocol/Arp.h>
+#include <Protocol/Ip4.h>
+#include <Protocol/Ip4Config2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/NetLib.h>
+#include <Library/DpcLib.h>
+#include <Library/PcdLib.h>
+
+#include "PxeBcDriver.h"
+#include "PxeBcDhcp.h"
+#include "PxeBcMtftp.h"
+#include "PxeBcSupport.h"
+
+#define PXEBC_PRIVATE_DATA_SIGNATURE  SIGNATURE_32 ('P', 'X', 'E', 'P')
+#define PXEBC_MTFTP_TIMEOUT                4
+#define PXEBC_MTFTP_RETRIES                6
+#define PXEBC_DEFAULT_UDP_OVERHEAD_SIZE    8
+#define PXEBC_DEFAULT_TFTP_OVERHEAD_SIZE   4
+#define PXEBC_DEFAULT_PACKET_SIZE          1480
+#define PXEBC_DEFAULT_LIFETIME             50000  // 50ms, unit is microsecond
+
+struct _PXEBC_PRIVATE_DATA {
+  UINT32                                    Signature;
+  EFI_HANDLE                                Controller;
+  EFI_HANDLE                                Image;
+  EFI_HANDLE                                ArpChild;
+  EFI_HANDLE                                Dhcp4Child;
+  EFI_HANDLE                                Ip4Child;
+  EFI_HANDLE                                Mtftp4Child;
+  EFI_HANDLE                                Udp4ReadChild;
+  EFI_HANDLE                                Udp4WriteChild;
+
+  EFI_NETWORK_INTERFACE_IDENTIFIER_PROTOCOL *Nii;
+
+  EFI_PXE_BASE_CODE_PROTOCOL                PxeBc;
+  EFI_LOAD_FILE_PROTOCOL                    LoadFile;
+  EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL       LoadFileCallback;
+  EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL       *PxeBcCallback;
+  EFI_ARP_PROTOCOL                          *Arp;
+  EFI_DHCP4_PROTOCOL                        *Dhcp4;
+  EFI_IP4_PROTOCOL                          *Ip4;
+  EFI_IP4_CONFIG2_PROTOCOL                  *Ip4Config2;
+  EFI_IP4_CONFIG_DATA                       Ip4ConfigData;
+  EFI_MTFTP4_PROTOCOL                       *Mtftp4;
+  EFI_UDP4_PROTOCOL                         *Udp4Read;
+  EFI_UDP4_PROTOCOL                         *Udp4Write;
+  UINT16                                    CurrentUdpSrcPort;
+  EFI_UDP4_CONFIG_DATA                      Udp4CfgData;
+
+
+  EFI_PXE_BASE_CODE_MODE                    Mode;
+  EFI_PXE_BASE_CODE_FUNCTION                Function;
+
+  CHAR8                                     *BootFileName;
+
+  EFI_IP_ADDRESS                            StationIp;
+  EFI_IP_ADDRESS                            SubnetMask;
+  EFI_IP_ADDRESS                            GatewayIp;
+  EFI_IP_ADDRESS                            ServerIp;
+  BOOLEAN                                   AddressIsOk;
+  UINT32                                    Ip4MaxPacketSize;
+  UINTN                                     BlockSize;
+  UINTN                                     FileSize;
+
+  UINT8                                     OptionBuffer[PXEBC_DHCP4_MAX_OPTION_SIZE];
+  EFI_DHCP4_PACKET                          SeedPacket;
+  EFI_MAC_ADDRESS                           Mac;
+  UINT8                                     MacLen;
+
+  BOOLEAN                                   SortOffers;
+  BOOLEAN                                   GotProxyOffer;
+  UINT32                                    NumOffers;
+  UINT32                                    SelectedOffer;
+  UINT32                                    ProxyOfferType;
+
+  //
+  // Cached packets as complements of pxe mode data
+  //
+  PXEBC_CACHED_DHCP4_PACKET                 ProxyOffer;
+  PXEBC_CACHED_DHCP4_PACKET                 Dhcp4Ack;
+  PXEBC_CACHED_DHCP4_PACKET                 PxeReply;
+  PXEBC_CACHED_DHCP4_PACKET                 Dhcp4Offers[PXEBC_MAX_OFFER_NUM];
+
+  //
+  // Arrays for different types of offers:
+  //   ServerCount records the count of the servers we got the offers,
+  //   OfferIndex records the index of the offer sent by the server indexed by ServerCount.
+  //
+  UINT32                                    ServerCount[DHCP4_PACKET_TYPE_MAX];
+  UINT32                                    OfferIndex[DHCP4_PACKET_TYPE_MAX][PXEBC_MAX_OFFER_NUM];
+  UINT32                                    BootpIndex;
+  UINT32                                    ProxyIndex[DHCP4_PACKET_TYPE_MAX];
+  UINT32                                    BinlIndex[PXEBC_MAX_OFFER_NUM];
+
+  EFI_EVENT                                 GetArpCacheEvent;
+  //
+  // token and event used to get ICMP error data from IP
+  //
+  EFI_IP4_COMPLETION_TOKEN                  IcmpErrorRcvToken;
+};
+
+#define PXEBC_PRIVATE_DATA_FROM_PXEBC(a)          CR (a, PXEBC_PRIVATE_DATA, PxeBc, PXEBC_PRIVATE_DATA_SIGNATURE)
+
+#define PXEBC_PRIVATE_DATA_FROM_LOADFILE(a)       CR (a, PXEBC_PRIVATE_DATA, LoadFile, PXEBC_PRIVATE_DATA_SIGNATURE)
+
+#define PXEBC_PRIVATE_DATA_FROM_PXEBCCALLBACK(a)  CR (a, PXEBC_PRIVATE_DATA, PxeBcCallback, PXEBC_PRIVATE_DATA_SIGNATURE)
+
+extern EFI_PXE_BASE_CODE_PROTOCOL mPxeBcProtocolTemplate;
+extern EFI_LOAD_FILE_PROTOCOL     mLoadFileProtocolTemplate;
+
+/**
+  Causes the driver to load a specified file.
+
+  @param  This                  Protocol instance pointer.
+  @param  FilePath              The device specific path of the file to load.
+  @param  BootPolicy            If TRUE, indicates that the request originates from the
+                                boot manager is attempting to load FilePath as a boot
+                                selection. If FALSE, then FilePath must match as exact file
+                                to be loaded.
+  @param  BufferSize            On input the size of Buffer in bytes. On output with a return
+                                code of EFI_SUCCESS, the amount of data transferred to
+                                Buffer. On output with a return code of EFI_BUFFER_TOO_SMALL,
+                                the size of Buffer required to retrieve the requested file.
+  @param  Buffer                The memory buffer to transfer the file to. IF Buffer is NULL,
+                                then no the size of the requested file is returned in
+                                BufferSize.
+
+  @retval EFI_SUCCESS                 The file was loaded.
+  @retval EFI_UNSUPPORTED             The device does not support the provided BootPolicy
+  @retval EFI_INVALID_PARAMETER       FilePath is not a valid device path, or
+                                      BufferSize is NULL.
+  @retval EFI_NO_MEDIA                No medium was present to load the file.
+  @retval EFI_DEVICE_ERROR            The file was not loaded due to a device error.
+  @retval EFI_NO_RESPONSE             The remote system did not respond.
+  @retval EFI_NOT_FOUND               The file was not found.
+  @retval EFI_ABORTED                 The file load process was manually cancelled.
+
+**/
+EFI_STATUS
+EFIAPI
+EfiPxeLoadFile (
+  IN EFI_LOAD_FILE_PROTOCOL           * This,
+  IN EFI_DEVICE_PATH_PROTOCOL         * FilePath,
+  IN BOOLEAN                          BootPolicy,
+  IN OUT UINTN                        *BufferSize,
+  IN VOID                             *Buffer OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.c
new file mode 100644
index 0000000000..8017b73dd7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.c
@@ -0,0 +1,454 @@
+/** @file
+  PxeBc MTFTP functions.
+  
+Copyright (c) 2007 - 2015, 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 "PxeBcImpl.h"
+
+CHAR8 *mMtftpOptions[PXE_MTFTP_OPTION_MAXIMUM_INDEX] = {
+  "blksize",
+  "timeout",
+  "tsize",
+  "multicast"
+};
+
+
+/**
+  This is a callback function when packets received/transmitted in Mtftp driver.
+
+  A callback function that is provided by the caller to intercept               
+  the EFI_MTFTP4_OPCODE_DATA or EFI_MTFTP4_OPCODE_DATA8 packets processed in the
+  EFI_MTFTP4_PROTOCOL.ReadFile() function, and alternatively to intercept       
+  EFI_MTFTP4_OPCODE_OACK or EFI_MTFTP4_OPCODE_ERROR packets during a call to    
+  EFI_MTFTP4_PROTOCOL.ReadFile(), WriteFile() or ReadDirectory().
+   
+  @param  This           Pointer to Mtftp protocol instance
+  @param  Token          Pointer to Mtftp token
+  @param  PacketLen      Length of Mtftp packet
+  @param  Packet         Pointer to Mtftp packet
+
+  @retval EFI_SUCCESS    Operation sucess
+  @retval EFI_ABORTED    Abort transfer process 
+
+**/
+EFI_STATUS
+EFIAPI
+PxeBcCheckPacket (
+  IN EFI_MTFTP4_PROTOCOL        *This,
+  IN EFI_MTFTP4_TOKEN           *Token,
+  IN UINT16                     PacketLen,
+  IN EFI_MTFTP4_PACKET          *Packet
+  )
+{
+  PXEBC_PRIVATE_DATA                  *Private;
+  EFI_PXE_BASE_CODE_CALLBACK_PROTOCOL *Callback;
+  EFI_STATUS                          Status;
+
+  Private   = (PXEBC_PRIVATE_DATA *) Token->Context;
+  Callback  = Private->PxeBcCallback;
+  Status    = EFI_SUCCESS;
+
+  if (Packet->OpCode == EFI_MTFTP4_OPCODE_ERROR) {
+    Private->Mode.TftpErrorReceived = TRUE;
+    Private->Mode.TftpError.ErrorCode = (UINT8) Packet->Error.ErrorCode;
+    AsciiStrnCpyS (Private->Mode.TftpError.ErrorString, PXE_MTFTP_ERROR_STRING_LENGTH, (CHAR8 *) Packet->Error.ErrorMessage, PXE_MTFTP_ERROR_STRING_LENGTH - 1);
+    Private->Mode.TftpError.ErrorString[PXE_MTFTP_ERROR_STRING_LENGTH - 1] = '\0';
+  }
+
+  if (Callback != NULL) {
+
+    Status = Callback->Callback (
+                        Callback,
+                        Private->Function,
+                        TRUE,
+                        PacketLen,
+                        (EFI_PXE_BASE_CODE_PACKET *) Packet
+                        );
+    if (Status != EFI_PXE_BASE_CODE_CALLBACK_STATUS_CONTINUE) {
+
+      Status = EFI_ABORTED;
+    } else {
+
+      Status = EFI_SUCCESS;
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  This function is to get size of a file by Tftp.
+  
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  BlockSize      Pointer to block size
+  @param  BufferSize     Pointer to buffer size
+
+  @retval EFI_SUCCESS        Get the size of file success
+  @retval EFI_NOT_FOUND      Parse the tftp ptions failed.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval Other              Has not get the size of the file.
+  
+**/
+EFI_STATUS
+PxeBcTftpGetFileSize (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN UINTN                      *BlockSize,
+  IN OUT UINT64                 *BufferSize
+  )
+{
+  EFI_MTFTP4_PROTOCOL *Mtftp4;
+  EFI_MTFTP4_OPTION   ReqOpt[2];
+  EFI_MTFTP4_PACKET   *Packet;
+  EFI_MTFTP4_OPTION   *Option;
+  UINT32              PktLen;
+  UINT8               OptBuf[128];
+  UINT32              OptCnt;
+  EFI_STATUS          Status;
+
+  *BufferSize               = 0;
+  Status                    = EFI_DEVICE_ERROR;
+  Mtftp4                    = Private->Mtftp4;
+  Packet                    = NULL;
+  Option                    = NULL;
+  PktLen                    = 0;
+  OptCnt                    = 1;
+  Config->InitialServerPort = PXEBC_BS_DOWNLOAD_PORT;
+
+  Status = Mtftp4->Configure (Mtftp4, Config);
+  if (EFI_ERROR (Status)) {
+
+    return Status;
+  }
+
+  ReqOpt[0].OptionStr = (UINT8*)mMtftpOptions[PXE_MTFTP_OPTION_TSIZE_INDEX];
+  UtoA10 (0, (CHAR8 *) OptBuf, PXE_MTFTP_OPTBUF_MAXNUM_INDEX);
+  ReqOpt[0].ValueStr = OptBuf;
+
+  if (BlockSize != NULL) {
+    ReqOpt[1].OptionStr = (UINT8*)mMtftpOptions[PXE_MTFTP_OPTION_BLKSIZE_INDEX];
+    ReqOpt[1].ValueStr  = ReqOpt[0].ValueStr + AsciiStrLen ((CHAR8 *) ReqOpt[0].ValueStr) + 1;
+    UtoA10 (*BlockSize, (CHAR8 *) ReqOpt[1].ValueStr, PXE_MTFTP_OPTBUF_MAXNUM_INDEX - (AsciiStrLen ((CHAR8 *) ReqOpt[0].ValueStr) + 1));
+    OptCnt++;
+  }
+
+  Status = Mtftp4->GetInfo (
+                    Mtftp4,
+                    NULL,
+                    Filename,
+                    NULL,
+                    (UINT8) OptCnt,
+                    ReqOpt,
+                    &PktLen,
+                    &Packet
+                    );
+
+  if (EFI_ERROR (Status)) {
+    if (Status == EFI_TFTP_ERROR) {
+      Private->Mode.TftpErrorReceived = TRUE;
+      Private->Mode.TftpError.ErrorCode = (UINT8) Packet->Error.ErrorCode;
+      AsciiStrnCpyS (
+        Private->Mode.TftpError.ErrorString,
+        PXE_MTFTP_ERROR_STRING_LENGTH,
+        (CHAR8 *) Packet->Error.ErrorMessage,
+        PXE_MTFTP_ERROR_STRING_LENGTH - 1
+        );
+      Private->Mode.TftpError.ErrorString[PXE_MTFTP_ERROR_STRING_LENGTH - 1] = '\0';
+    }
+    goto ON_ERROR;
+  }
+
+  OptCnt = 0;
+
+  Status = Mtftp4->ParseOptions (
+                    Mtftp4,
+                    PktLen,
+                    Packet,
+                    (UINT32 *) &OptCnt,
+                    &Option
+                    );
+
+  if (EFI_ERROR (Status)) {
+
+    goto ON_ERROR;
+  }
+
+  Status = EFI_NOT_FOUND;
+
+  while (OptCnt != 0) {
+
+    if (AsciiStrnCmp ((CHAR8 *) Option[OptCnt - 1].OptionStr, "tsize", 5) == 0) {
+
+      *BufferSize = AtoU64 (Option[OptCnt - 1].ValueStr);
+      Status      = EFI_SUCCESS;
+    }
+
+    OptCnt--;
+  }
+
+  FreePool (Option);
+
+ON_ERROR:
+
+  if (Packet != NULL) {
+    FreePool (Packet);
+  }
+
+  Mtftp4->Configure (Mtftp4, NULL);
+
+  return Status;
+}
+
+
+/**
+  This function is to get data of a file by Tftp.
+
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  BlockSize      Pointer to block size
+  @param  BufferPtr      Pointer to buffer
+  @param  BufferSize     Pointer to buffer size
+  @param  DontUseBuffer  Indicate whether with a receive buffer
+
+  @retval EFI_SUCCESS        Read the data success from the special file.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Read data from file failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpReadFile (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN UINTN                      *BlockSize,
+  IN UINT8                      *BufferPtr,
+  IN OUT UINT64                 *BufferSize,
+  IN BOOLEAN                    DontUseBuffer
+  )
+{
+  EFI_MTFTP4_PROTOCOL *Mtftp4;
+  EFI_MTFTP4_TOKEN    Token;
+  EFI_MTFTP4_OPTION   ReqOpt[1];
+  UINT32              OptCnt;
+  UINT8               OptBuf[128];
+  EFI_STATUS          Status;
+
+  Status                    = EFI_DEVICE_ERROR;
+  Mtftp4                    = Private->Mtftp4;
+  OptCnt                    = 0;
+  Config->InitialServerPort = PXEBC_BS_DOWNLOAD_PORT;
+
+  Status = Mtftp4->Configure (Mtftp4, Config);
+  if (EFI_ERROR (Status)) {
+
+    return Status;
+  }
+
+  if (BlockSize != NULL) {
+
+    ReqOpt[0].OptionStr = (UINT8*) mMtftpOptions[PXE_MTFTP_OPTION_BLKSIZE_INDEX];
+    ReqOpt[0].ValueStr  = OptBuf;
+    UtoA10 (*BlockSize, (CHAR8 *) ReqOpt[0].ValueStr, PXE_MTFTP_OPTBUF_MAXNUM_INDEX);
+    OptCnt++;
+  }
+
+  Token.Event         = NULL;
+  Token.OverrideData  = NULL;
+  Token.Filename      = Filename;
+  Token.ModeStr       = NULL;
+  Token.OptionCount   = OptCnt;
+  Token.OptionList    = ReqOpt;
+  Token.Context       = Private;
+
+  if (DontUseBuffer) {
+    Token.BufferSize  = 0;
+    Token.Buffer      = NULL;
+  } else {
+    Token.BufferSize  = *BufferSize;
+    Token.Buffer      = BufferPtr;
+  }
+
+  Token.CheckPacket     = PxeBcCheckPacket;
+  Token.TimeoutCallback = NULL;
+  Token.PacketNeeded    = NULL;
+
+  Status = Mtftp4->ReadFile (Mtftp4, &Token);
+
+  *BufferSize = Token.BufferSize;
+
+  Mtftp4->Configure (Mtftp4, NULL);
+
+  return Status;
+}
+
+
+/**
+  This function is put data of a file by Tftp.
+
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  Overwrite      Indicate whether with overwrite attribute
+  @param  BlockSize      Pointer to block size
+  @param  BufferPtr      Pointer to buffer
+  @param  BufferSize     Pointer to buffer size
+
+  @retval EFI_SUCCESS        Write the data success into the special file.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Write data into file failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpWriteFile (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN BOOLEAN                    Overwrite,
+  IN UINTN                      *BlockSize,
+  IN UINT8                      *BufferPtr,
+  IN OUT UINT64                 *BufferSize
+  )
+{
+  EFI_MTFTP4_PROTOCOL *Mtftp4;
+  EFI_MTFTP4_TOKEN    Token;
+  EFI_MTFTP4_OPTION   ReqOpt[1];
+  UINT32              OptCnt;
+  UINT8               OptBuf[128];
+  EFI_STATUS          Status;
+
+  Status                    = EFI_DEVICE_ERROR;
+  Mtftp4                    = Private->Mtftp4;
+  OptCnt                    = 0;
+  Config->InitialServerPort = PXEBC_BS_DOWNLOAD_PORT;
+
+  Status  = Mtftp4->Configure (Mtftp4, Config);
+  if (EFI_ERROR (Status)) {
+
+    return Status;
+  }
+
+  if (BlockSize != NULL) {
+
+    ReqOpt[0].OptionStr = (UINT8*) mMtftpOptions[PXE_MTFTP_OPTION_BLKSIZE_INDEX];
+    ReqOpt[0].ValueStr  = OptBuf;
+    UtoA10 (*BlockSize, (CHAR8 *) ReqOpt[0].ValueStr, PXE_MTFTP_OPTBUF_MAXNUM_INDEX);
+    OptCnt++;
+  }
+
+  Token.Event           = NULL;
+  Token.OverrideData    = NULL;
+  Token.Filename        = Filename;
+  Token.ModeStr         = NULL;
+  Token.OptionCount     = OptCnt;
+  Token.OptionList      = ReqOpt;
+  Token.BufferSize      = *BufferSize;
+  Token.Buffer          = BufferPtr;
+  Token.CheckPacket     = PxeBcCheckPacket;
+  Token.TimeoutCallback = NULL;
+  Token.PacketNeeded    = NULL;
+
+  Status      = Mtftp4->WriteFile (Mtftp4, &Token);
+  *BufferSize = Token.BufferSize;
+
+  Mtftp4->Configure (Mtftp4, NULL);
+
+  return Status;
+}
+
+
+/**
+  This function is to get data(file) from a directory(may be a server) by Tftp.
+
+  @param  Private        Pointer to PxeBc private data.
+  @param  Config         Pointer to Mtftp configuration data.
+  @param  Filename       Pointer to file name.
+  @param  BlockSize      Pointer to block size.
+  @param  BufferPtr      Pointer to buffer.
+  @param  BufferSize     Pointer to buffer size.
+  @param  DontUseBuffer  Indicate whether with a receive buffer.
+
+  @retval EFI_SUCCES         Get the data from the file included in directory success. 
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Operation failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpReadDirectory (
+  IN PXEBC_PRIVATE_DATA            *Private,
+  IN EFI_MTFTP4_CONFIG_DATA        *Config,
+  IN UINT8                         *Filename,
+  IN UINTN                         *BlockSize,
+  IN UINT8                         *BufferPtr,
+  IN OUT UINT64                    *BufferSize,
+  IN BOOLEAN                       DontUseBuffer
+  )
+{
+  EFI_MTFTP4_PROTOCOL *Mtftp4;
+  EFI_MTFTP4_TOKEN    Token;
+  EFI_MTFTP4_OPTION   ReqOpt[1];
+  UINT32              OptCnt;
+  UINT8               OptBuf[128];
+  EFI_STATUS          Status;
+
+  Status                    = EFI_DEVICE_ERROR;
+  Mtftp4                    = Private->Mtftp4;
+  OptCnt                    = 0;
+  Config->InitialServerPort = PXEBC_BS_DOWNLOAD_PORT;
+
+  Status = Mtftp4->Configure (Mtftp4, Config);
+  if (EFI_ERROR (Status)) {
+
+    return Status;
+  }
+
+  if (BlockSize != NULL) {
+
+    ReqOpt[0].OptionStr = (UINT8*) mMtftpOptions[PXE_MTFTP_OPTION_BLKSIZE_INDEX];
+    ReqOpt[0].ValueStr  = OptBuf;
+    UtoA10 (*BlockSize, (CHAR8 *) ReqOpt[0].ValueStr, PXE_MTFTP_OPTBUF_MAXNUM_INDEX);
+    OptCnt++;
+  }
+
+  Token.Event         = NULL;
+  Token.OverrideData  = NULL;
+  Token.Filename      = Filename;
+  Token.ModeStr       = NULL;
+  Token.OptionCount   = OptCnt;
+  Token.OptionList    = ReqOpt;
+  Token.Context       = Private;
+
+  if (DontUseBuffer) {
+    Token.BufferSize  = 0;
+    Token.Buffer      = NULL;
+  } else {
+    Token.BufferSize  = *BufferSize;
+    Token.Buffer      = BufferPtr;
+  }
+
+  Token.CheckPacket     = PxeBcCheckPacket;
+  Token.TimeoutCallback = NULL;
+  Token.PacketNeeded    = NULL;
+
+  Status = Mtftp4->ReadDirectory (Mtftp4, &Token);
+
+  *BufferSize = Token.BufferSize;
+
+  Mtftp4->Configure (Mtftp4, NULL);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.h b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.h
new file mode 100644
index 0000000000..241b079d72
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcMtftp.h
@@ -0,0 +1,137 @@
+/** @file
+  Mtftp routines for PxeBc.
+    
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef __EFI_PXEBC_MTFTP_H__
+#define __EFI_PXEBC_MTFTP_H__
+
+#define PXE_MTFTP_OPTION_BLKSIZE_INDEX   0
+#define PXE_MTFTP_OPTION_TIMEOUT_INDEX   1
+#define PXE_MTFTP_OPTION_TSIZE_INDEX     2
+#define PXE_MTFTP_OPTION_MULTICAST_INDEX 3
+#define PXE_MTFTP_OPTION_MAXIMUM_INDEX   4
+
+#define PXE_MTFTP_ERROR_STRING_LENGTH    127
+#define PXE_MTFTP_OPTBUF_MAXNUM_INDEX    128
+
+
+/**
+  This function is to get size of a file by Tftp.
+  
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  BlockSize      Pointer to block size
+  @param  BufferSize     Pointer to buffer size
+
+  @retval EFI_SUCCESS        Get the size of file success
+  @retval EFI_NOT_FOUND      Parse the tftp ptions failed.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval Other              Has not get the size of the file.
+  
+**/
+EFI_STATUS
+PxeBcTftpGetFileSize (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN UINTN                      *BlockSize,
+  IN OUT UINT64                 *BufferSize
+  );
+
+
+/**
+  This function is to get data of a file by Tftp.
+
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  BlockSize      Pointer to block size
+  @param  BufferPtr      Pointer to buffer
+  @param  BufferSize     Pointer to buffer size
+  @param  DontUseBuffer  Indicate whether with a receive buffer
+
+  @retval EFI_SUCCESS        Read the data success from the special file.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Read data from file failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpReadFile (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN UINTN                      *BlockSize,
+  IN UINT8                      *BufferPtr,
+  IN OUT UINT64                 *BufferSize,
+  IN BOOLEAN                    DontUseBuffer
+  );
+
+
+/**
+  This function is put data of a file by Tftp.
+
+  @param  Private        Pointer to PxeBc private data
+  @param  Config         Pointer to Mtftp configuration data
+  @param  Filename       Pointer to file name
+  @param  Overwrite      Indicate whether with overwrite attribute
+  @param  BlockSize      Pointer to block size
+  @param  BufferPtr      Pointer to buffer
+  @param  BufferSize     Pointer to buffer size
+
+  @retval EFI_SUCCESS        Write the data success into the special file.
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Write data into file failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpWriteFile (
+  IN PXEBC_PRIVATE_DATA         *Private,
+  IN EFI_MTFTP4_CONFIG_DATA     *Config,
+  IN UINT8                      *Filename,
+  IN BOOLEAN                    Overwrite,
+  IN UINTN                      *BlockSize,
+  IN UINT8                      *BufferPtr,
+  IN OUT UINT64                 *BufferSize
+  );
+
+
+/**
+  This function is to get data(file) from a directory(may be a server) by Tftp.
+
+  @param  Private        Pointer to PxeBc private data.
+  @param  Config         Pointer to Mtftp configuration data.
+  @param  Filename       Pointer to file name.
+  @param  BlockSize      Pointer to block size.
+  @param  BufferPtr      Pointer to buffer.
+  @param  BufferSize     Pointer to buffer size.
+  @param  DontUseBuffer  Indicate whether with a receive buffer.
+
+  @retval EFI_SUCCES         Get the data from the file included in directory success. 
+  @retval EFI_DEVICE_ERROR   The network device encountered an error during this operation.
+  @retval other              Operation failed.
+  
+**/
+EFI_STATUS
+PxeBcTftpReadDirectory (
+  IN PXEBC_PRIVATE_DATA            *Private,
+  IN EFI_MTFTP4_CONFIG_DATA        *Config,
+  IN UINT8                         *Filename,
+  IN UINTN                         *BlockSize,
+  IN UINT8                         *BufferPtr,
+  IN OUT UINT64                    *BufferSize,
+  IN BOOLEAN                       DontUseBuffer
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.c b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.c
new file mode 100644
index 0000000000..3016da7324
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.c
@@ -0,0 +1,198 @@
+/** @file
+  Support routines for PxeBc.
+
+Copyright (c) 2007 - 2015, 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 "PxeBcImpl.h"
+
+
+/**
+  The common notify function associated with various PxeBc events.
+
+  @param  Event     The event signaled.
+  @param  Context   The context.
+
+**/
+VOID
+EFIAPI
+PxeBcCommonNotify (
+  IN EFI_EVENT           Event,
+  IN VOID                *Context
+  )
+{
+  *((BOOLEAN *) Context) = TRUE;
+}
+
+
+/**
+  This function initialize(or configure) the Udp4Write instance.
+
+  @param  Udp4       Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param  StationIp  Pointer to the station ip address.
+  @param  SubnetMask Pointer to the subnetmask of the station ip address.
+  @param  Gateway    Pointer to the gateway ip address.
+  @param  SrcPort    Pointer to the srouce port of the station.
+
+  @retval EFI_SUCCESS           The configuration settings were set, changed, or reset successfully.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more following conditions are TRUE:
+  @retval EFI_ALREADY_STARTED   The EFI UDPv4 Protocol instance is already started/configured
+                                and must be stopped/reset before it can be reconfigured.
+  @retval EFI_ACCESS_DENIED     UdpConfigData. AllowDuplicatePort is FALSE
+                                and UdpConfigData.StationPort is already used by
+                                other instance.
+  @retval EFI_OUT_OF_RESOURCES  The EFI UDPv4 Protocol driver cannot allocate memory for this
+                                EFI UDPv4 Protocol instance.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred and this instance
+                                was not opened.
+  @retval Others                Please examine the function Udp4->Routes(Udp4, FALSE, &mZeroIp4Addr, &mZeroIp4Addr, Gateway) returns.
+
+**/
+EFI_STATUS
+PxeBcConfigureUdpWriteInstance (
+  IN EFI_UDP4_PROTOCOL  *Udp4,
+  IN EFI_IPv4_ADDRESS   *StationIp,
+  IN EFI_IPv4_ADDRESS   *SubnetMask,
+  IN EFI_IPv4_ADDRESS   *Gateway,
+  IN OUT UINT16         *SrcPort
+  )
+{
+  EFI_UDP4_CONFIG_DATA  Udp4CfgData;
+  EFI_STATUS            Status;
+
+  ZeroMem (&Udp4CfgData, sizeof (Udp4CfgData));
+
+  Udp4CfgData.ReceiveTimeout = PXEBC_DEFAULT_LIFETIME;
+  Udp4CfgData.TypeOfService  = DEFAULT_ToS;
+  Udp4CfgData.TimeToLive     = DEFAULT_TTL;
+  Udp4CfgData.AllowDuplicatePort = TRUE;
+
+  CopyMem (&Udp4CfgData.StationAddress, StationIp, sizeof (*StationIp));
+  CopyMem (&Udp4CfgData.SubnetMask, SubnetMask, sizeof (*SubnetMask));
+
+  Udp4CfgData.StationPort    = *SrcPort;
+
+  //
+  // Reset the instance.
+  //
+  Udp4->Configure (Udp4, NULL);
+
+  Status = Udp4->Configure (Udp4, &Udp4CfgData);
+  if (!EFI_ERROR (Status) && (Gateway->Addr[0] != 0)) {
+    //
+    // basic configuration OK, need to add the default route entry
+    //
+    Status = Udp4->Routes (Udp4, FALSE, &mZeroIp4Addr, &mZeroIp4Addr, Gateway);
+    if (EFI_ERROR (Status)) {
+      //
+      // roll back
+      //
+      Udp4->Configure (Udp4, NULL);
+    }
+  }
+
+  if (!EFI_ERROR (Status) && (*SrcPort == 0)) {
+    Udp4->GetModeData (Udp4, &Udp4CfgData, NULL, NULL, NULL);
+    *SrcPort = Udp4CfgData.StationPort;
+  }
+
+  return Status;
+}
+
+
+/**
+  Convert number to ASCII value.
+
+  @param  Number              Numeric value to convert to decimal ASCII value.
+  @param  Buffer              Buffer to place ASCII version of the Number.
+  @param  Length              Length of Buffer.
+
+**/
+VOID
+CvtNum (
+  IN UINTN  Number,
+  IN UINT8  *Buffer,
+  IN UINTN   Length
+  )
+{
+  UINTN Remainder;
+
+  while (Length > 0) {
+    Remainder = Number % 10;
+    Number /= 10;
+    Length--;
+    Buffer[Length] = (UINT8) ('0' + Remainder);
+  }
+}
+
+
+/**
+  Convert unsigned int number to decimal number.
+
+  @param      Number         The unsigned int number will be converted.
+  @param      Buffer         Pointer to the buffer to store the decimal number after transform.
+  @param[in]  BufferSize     The maxsize of the buffer.
+  
+  @return the length of the number after transform.
+
+**/
+UINTN
+UtoA10 (
+  IN UINTN Number,
+  IN CHAR8 *Buffer,
+  IN UINTN BufferSize
+  )
+{
+  UINTN Index;
+  CHAR8 TempStr[64];
+
+  Index           = 63;
+  TempStr[Index]  = 0;
+
+  do {
+    Index--;
+    TempStr[Index]  = (CHAR8) ('0' + (Number % 10));
+    Number          = Number / 10;
+  } while (Number != 0);
+
+  AsciiStrCpyS (Buffer, BufferSize, &TempStr[Index]);
+
+  return AsciiStrLen (Buffer);
+}
+
+
+/**
+  Convert ASCII numeric string to a UINTN value.
+
+  @param  Buffer  Pointer to the 8-byte unsigned int value.
+
+  @return UINTN value of the ASCII string.
+
+**/
+UINT64
+AtoU64 (
+  IN UINT8 *Buffer
+  )
+{
+  UINT64  Value;
+  UINT8   Character;
+
+  Value = 0;
+  while ((Character = *Buffer++) != '\0') {
+    Value = MultU64x32 (Value, 10) + (Character - '0');
+  }
+
+  return Value;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.h b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.h
new file mode 100644
index 0000000000..6f41aa671d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/PxeBcSupport.h
@@ -0,0 +1,116 @@
+/** @file
+  Support routines for PxeBc.
+Copyright (c) 2007 - 2015, 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.
+
+**/
+
+#ifndef __EFI_PXEBC_SUPPORT_H__
+#define __EFI_PXEBC_SUPPORT_H__
+
+
+/**
+  The common notify function associated with various PxeBc events. 
+
+  @param  Event     The event signaled.
+  @param  Context   The context.
+
+**/
+VOID
+EFIAPI
+PxeBcCommonNotify (
+  IN EFI_EVENT           Event,
+  IN VOID                *Context
+  );
+
+
+/**
+  This function initialize(or configure) the Udp4Write instance.
+  
+  @param  Udp4       Pointer to the EFI_UDP4_PROTOCOL instance.
+  @param  StationIp  Pointer to the station ip address.
+  @param  SubnetMask Pointer to the subnetmask of the station ip address.
+  @param  Gateway    Pointer to the gateway ip address.
+  @param  SrcPort    Pointer to the srouce port of the station.
+  
+  @retval EFI_SUCCESS           The configuration settings were set, changed, or reset successfully.
+  @retval EFI_NO_MAPPING        When using a default address, configuration (DHCP, BOOTP,
+                                RARP, etc.) is not finished yet.
+  @retval EFI_INVALID_PARAMETER One or more following conditions are TRUE:
+  @retval EFI_ALREADY_STARTED   The EFI UDPv4 Protocol instance is already started/configured
+                                and must be stopped/reset before it can be reconfigured.
+  @retval EFI_ACCESS_DENIED     UdpConfigData. AllowDuplicatePort is FALSE
+                                and UdpConfigData.StationPort is already used by
+                                other instance.
+  @retval EFI_OUT_OF_RESOURCES  The EFI UDPv4 Protocol driver cannot allocate memory for this
+                                EFI UDPv4 Protocol instance.
+  @retval EFI_DEVICE_ERROR      An unexpected network or system error occurred and this instance
+                                was not opened.
+  @retval Others                Please examine the function Udp4->Routes(Udp4, FALSE, &mZeroIp4Addr, &mZeroIp4Addr, Gateway) returns.
+  
+**/
+EFI_STATUS
+PxeBcConfigureUdpWriteInstance (
+  IN EFI_UDP4_PROTOCOL  *Udp4,
+  IN EFI_IPv4_ADDRESS   *StationIp,
+  IN EFI_IPv4_ADDRESS   *SubnetMask,
+  IN EFI_IPv4_ADDRESS   *Gateway,
+  IN OUT UINT16         *SrcPort
+  );
+/**
+  Convert number to ASCII value.
+
+  @param  Number              Numeric value to convert to decimal ASCII value.
+  @param  Buffer              Buffer to place ASCII version of the Number.
+  @param  Length              Length of Buffer.
+
+**/
+VOID
+CvtNum (
+  IN UINTN  Number,
+  IN UINT8  *Buffer,
+  IN UINTN   Length
+  );
+
+
+/**
+  Convert unsigned int number to decimal number.
+
+  @param      Number         The unsigned int number will be converted.
+  @param      Buffer         Pointer to the buffer to store the decimal number after transform.
+  @param[in]  BufferSize     The maxsize of the buffer.
+  
+  @return the length of the number after transform.
+
+**/
+UINTN
+UtoA10 (
+  IN UINTN Number,
+  IN CHAR8 *Buffer,
+  IN UINTN BufferSize
+  
+  );
+
+
+/**
+  Convert ASCII numeric string to a UINTN value.
+
+  @param  Buffer  Pointer to the 8-byte unsigned int value.
+
+  @return UINTN value of the ASCII string.
+
+**/
+UINT64
+AtoU64 (
+  IN UINT8 *Buffer
+  );
+
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxe.uni b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxe.uni
new file mode 100644
index 0000000000..f416ffc2b9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxeExtra.uni
new file mode 100644
index 0000000000..f132359193
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxe4BcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxeBcDxe.inf b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxeBcDxe.inf
new file mode 100644
index 0000000000..fe8f210467
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/UefiPxeBcDxe/UefiPxeBcDxe.inf
@@ -0,0 +1,93 @@
+## @file
+#  This module produces EFI Preboot Execution Environment (PXE) Base Code Protocol.
+#
+#  This module produces EFI PXE Base Code Protocol upon EFI MMP Protocl and IPv4
+#  network stack, used to control PXE-compatible devices. It produces EFI Load File
+#  Protocol to provide one clean way to otain control from the boot manager if the
+#  boot patch is from the remote device.
+#
+#  Copyright (c) 2007 - 2015, 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.
+#
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = UefiPxe4BcDxe
+  MODULE_UNI_FILE                = UefiPxe4BcDxe.uni
+  FILE_GUID                      = 3B1DEAB5-C75D-442e-9238-8E2FFB62B0BB
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PxeBcDriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF
+#
+#  DRIVER_BINDING                =  gPxeBcDriverBinding                        
+#  COMPONENT_NAME                =  gPxeBcComponentName
+#  COMPONENT_NAME2               =  gPxeBcComponentName2
+#
+
+[Sources]
+  PxeBcMtftp.c
+  PxeBcSupport.h
+  PxeBcSupport.c
+  PxeBcDriver.c
+  PxeBcDhcp.h
+  ComponentName.c
+  PxeBcImpl.c
+  PxeBcImpl.h
+  PxeBcDhcp.c
+  PxeBcMtftp.h
+  PxeBcDriver.h
+ 
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  BaseMemoryLib
+  MemoryAllocationLib
+  DebugLib
+  NetLib
+  DpcLib
+  PcdLib
+
+[Protocols]
+  gEfiPxeBaseCodeCallbackProtocolGuid              ## SOMETIMES_PRODUCES
+  gEfiPxeBaseCodeProtocolGuid                      ## BY_START
+  gEfiLoadFileProtocolGuid                         ## BY_START
+  gEfiNetworkInterfaceIdentifierProtocolGuid_31    ## SOMETIMES_CONSUMES
+  gEfiArpServiceBindingProtocolGuid                ## TO_START
+  gEfiArpProtocolGuid                              ## TO_START
+  gEfiMtftp4ServiceBindingProtocolGuid             ## TO_START
+  gEfiMtftp4ProtocolGuid                           ## TO_START
+  gEfiUdp4ServiceBindingProtocolGuid               ## TO_START
+  gEfiUdp4ProtocolGuid                             ## TO_START
+  gEfiDhcp4ServiceBindingProtocolGuid              ## TO_START
+  gEfiDhcp4ProtocolGuid                            ## TO_START
+  gEfiIp4ServiceBindingProtocolGuid                ## TO_START
+  gEfiIp4ProtocolGuid                              ## TO_START
+  gEfiIp4Config2ProtocolGuid                       ## TO_START
+
+[Pcd]  
+  gEfiMdeModulePkgTokenSpaceGuid.PcdTftpBlockSize  ## SOMETIMES_CONSUMES  
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  UefiPxe4BcDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/ComponentName.c b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/ComponentName.c
new file mode 100644
index 0000000000..a5a2f77ce2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/ComponentName.c
@@ -0,0 +1,171 @@
+/** @file
+  UEFI Component Name(2) protocol implementation for VlanConfigDxe driver.
+
+Copyright (c) 2009 - 2011, 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<BR>
+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 "VlanConfigImpl.h"
+
+//
+// EFI Component Name Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME_PROTOCOL   gVlanConfigComponentName = {
+  VlanConfigComponentNameGetDriverName,
+  VlanConfigComponentNameGetControllerName,
+  "eng"
+};
+
+//
+// EFI Component Name 2 Protocol
+//
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_COMPONENT_NAME2_PROTOCOL  gVlanConfigComponentName2 = {
+  (EFI_COMPONENT_NAME2_GET_DRIVER_NAME) VlanConfigComponentNameGetDriverName,
+  (EFI_COMPONENT_NAME2_GET_CONTROLLER_NAME) VlanConfigComponentNameGetControllerName,
+  "en"
+};
+
+GLOBAL_REMOVE_IF_UNREFERENCED EFI_UNICODE_STRING_TABLE      mVlanConfigDriverNameTable[] = {
+  {
+    "eng;en",
+    L"VLAN Configuration Driver"
+  },
+  {
+    NULL,
+    NULL
+  }
+};
+
+//
+// EFI Component Name Functions
+//
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **DriverName
+  )
+{
+  return LookupUnicodeString2 (
+           Language,
+           This->SupportedLanguages,
+           mVlanConfigDriverNameTable,
+           DriverName,
+           (BOOLEAN)(This == &gVlanConfigComponentName)
+           );
+}
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     EFI_HANDLE                    ControllerHandle,
+  IN     EFI_HANDLE                    ChildHandle OPTIONAL,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **ControllerName
+  )
+{
+  return EFI_UNSUPPORTED;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfig.vfr b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfig.vfr
new file mode 100644
index 0000000000..da3fe015c2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfig.vfr
@@ -0,0 +1,79 @@
+///** @file
+//  VLAN configuration formset.
+//
+//  Copyright (c) 2009 - 2011, 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<BR>
+//  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 "VlanConfigNvData.h"
+
+formset
+  guid      = VLAN_CONFIG_FORM_SET_GUID,
+  title     = STRING_TOKEN(STR_VLAN_FORM_SET_TITLE),
+  help      = STRING_TOKEN(STR_VLAN_FORM_SET_TITLE_HELP),
+  classguid = EFI_HII_PLATFORM_SETUP_FORMSET_GUID,
+
+  varstore VLAN_CONFIGURATION,
+    varid = VLAN_CONFIGURATION_VARSTORE_ID,
+    name  = VlanNvData,
+    guid  = VLAN_CONFIG_FORM_SET_GUID;
+
+  form formid = VLAN_HEAD_FORM_ID,
+    title  = STRING_TOKEN(STR_VLAN_FORM_TITLE);
+
+    goto VLAN_CONFIGURATION_FORM_ID,
+    prompt = STRING_TOKEN (STR_GET_CURRENT_SETTING),
+    help   = STRING_TOKEN (STR_GET_CURRENT_SETTING_HELP),
+    flags  = INTERACTIVE,
+    key    = VLAN_UPDATE_QUESTION_ID;
+
+  endform;
+
+  form formid = VLAN_CONFIGURATION_FORM_ID,
+    title = STRING_TOKEN(STR_VLAN_FORM_TITLE);
+
+    subtitle text = STRING_TOKEN(STR_VLAN_CREATE_VLAN);
+
+    numeric varid   = VlanNvData.VlanId,
+            prompt  = STRING_TOKEN(STR_VLAN_VID_PROMPT),
+            help    = STRING_TOKEN(STR_VLAN_VID_HELP),
+            minimum = 0,
+            maximum = 4094,
+    endnumeric;
+
+    numeric varid   = VlanNvData.Priority,
+            prompt  = STRING_TOKEN(STR_VLAN_PRIORITY_PROMPT),
+            help    = STRING_TOKEN(STR_VLAN_PRIORITY_HELP),
+            minimum = 0,
+            maximum = 7,
+    endnumeric;
+
+    text
+      help   = STRING_TOKEN(STR_VLAN_ADD_VLAN_HELP),
+      text   = STRING_TOKEN(STR_VLAN_ADD_VLAN_PROMPT),
+      flags  = INTERACTIVE,
+      key    = VLAN_ADD_QUESTION_ID;
+
+    subtitle text = STRING_TOKEN(STR_VLAN_NULL_STRING);
+    subtitle text = STRING_TOKEN(STR_VLAN_VLAN_LIST);
+
+    label LABEL_VLAN_LIST;
+    label LABEL_END;
+
+    text
+      help   = STRING_TOKEN(STR_VLAN_REMOVE_VLAN_HELP),
+      text   = STRING_TOKEN(STR_VLAN_REMOVE_VLAN_PROMPT),
+      flags  = INTERACTIVE,
+      key    = VLAN_REMOVE_QUESTION_ID;
+
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDriver.c b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDriver.c
new file mode 100644
index 0000000000..7dfea2e32a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDriver.c
@@ -0,0 +1,306 @@
+/** @file
+  The driver binding for VLAN configuration module.
+
+Copyright (c) 2009 - 2014, 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<BR>
+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 "VlanConfigImpl.h"
+
+EFI_DRIVER_BINDING_PROTOCOL gVlanConfigDriverBinding = {
+  VlanConfigDriverBindingSupported,
+  VlanConfigDriverBindingStart,
+  VlanConfigDriverBindingStop,
+  0xa,
+  NULL,
+  NULL
+};
+
+/**
+  The entry point for IP4 config driver which install the driver
+  binding and component name protocol on its image.
+
+  @param[in]  ImageHandle        The image handle of the driver.
+  @param[in]  SystemTable        The system table.
+
+  @retval EFI_SUCCES             All the related protocols are installed on the driver.
+  @retval Others                 Failed to install protocols.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverEntryPoint (
+  IN EFI_HANDLE          ImageHandle,
+  IN EFI_SYSTEM_TABLE    *SystemTable
+  )
+{
+  return EfiLibInstallDriverBindingComponentName2 (
+           ImageHandle,
+           SystemTable,
+           &gVlanConfigDriverBinding,
+           ImageHandle,
+           &gVlanConfigComponentName,
+           &gVlanConfigComponentName2
+           );
+}
+
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to test
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCES           This driver supports this device
+  @retval EFI_ALREADY_STARTED  This driver is already running on this device
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;
+
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  (VOID **) &VlanConfig,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Close the VlanConfig protocol opened for supported test
+  //
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiVlanConfigProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  return Status;
+}
+
+
+/**
+  Start this driver on ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCES           This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  )
+{
+  EFI_STATUS                Status;
+  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  VLAN_CONFIG_PRIVATE_DATA  *PrivateData;
+
+  //
+  // Check for multiple start
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &PrivateData,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Open VlanConfig protocol by driver
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  (VOID **) &VlanConfig,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_BY_DRIVER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get parent device path
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &DevicePath,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Create a private data for this network device
+  //
+  PrivateData = AllocateCopyPool (sizeof (VLAN_CONFIG_PRIVATE_DATA), &mVlanConfigPrivateDateTemplate);
+  if (PrivateData == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto ErrorExit;
+  }
+
+  PrivateData->ImageHandle = This->DriverBindingHandle;
+  PrivateData->ControllerHandle = ControllerHandle;
+  PrivateData->VlanConfig = VlanConfig;
+  PrivateData->ParentDevicePath = DevicePath;
+
+  //
+  // Install VLAN configuration form
+  //
+  Status = InstallVlanConfigForm (PrivateData);
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+
+  //
+  // Install private GUID
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  PrivateData,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto ErrorExit;
+  }
+  return Status;
+
+ErrorExit:
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiVlanConfigProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  gBS->CloseProtocol (
+         ControllerHandle,
+         &gEfiDevicePathProtocolGuid,
+         This->DriverBindingHandle,
+         ControllerHandle
+         );
+
+  if (PrivateData != NULL) {
+    UninstallVlanConfigForm (PrivateData);
+    FreePool (PrivateData);
+  }
+
+  return Status;
+}
+
+
+/**
+  Stop this driver on ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to stop driver on
+  @param[in]  NumberOfChildren     Number of Handles in ChildHandleBuffer. If number
+                                   of children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer    List of Child Handles to Stop.
+
+  @retval EFI_SUCCES           This driver is removed ControllerHandle
+  @retval other                This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  )
+{
+  EFI_STATUS                Status;
+  VLAN_CONFIG_PRIVATE_DATA  *PrivateData;
+
+  //
+  // Retrieve the PrivateData from ControllerHandle
+  //
+  Status = gBS->OpenProtocol (
+                  ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  (VOID **) &PrivateData,
+                  This->DriverBindingHandle,
+                  ControllerHandle,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (PrivateData->Signature == VLAN_CONFIG_PRIVATE_DATA_SIGNATURE);
+
+  if (NumberOfChildren != 0) {
+    if (NumberOfChildren != 1 || ChildHandleBuffer[0] != PrivateData->DriverHandle) {
+      return EFI_DEVICE_ERROR;
+    }
+    
+    return UninstallVlanConfigForm (PrivateData);
+  }
+
+  //
+  // Uninstall the private GUID
+  //
+  Status = gBS->UninstallMultipleProtocolInterfaces (
+                  ControllerHandle,
+                  &gEfiCallerIdGuid,
+                  PrivateData,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->CloseProtocol (
+                  ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  This->DriverBindingHandle,
+                  ControllerHandle
+                  );
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.inf b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.inf
new file mode 100644
index 0000000000..a36c57871e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.inf
@@ -0,0 +1,72 @@
+## @file
+#  This module provides one way to configurate VALN setting.
+#
+#  This module produces EFI HII Configuration Access Protocol to provide one way to
+#  configurate VALN setting
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VlanConfigDxe
+  MODULE_UNI_FILE                = VlanConfigDxe.uni
+  FILE_GUID                      = E4F61863-FE2C-4b56-A8F4-08519BC439DF
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = VlanConfigDriverEntryPoint
+  UNLOAD_IMAGE                   = NetLibDefaultUnload
+
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  ComponentName.c
+  VlanConfigDriver.c
+  VlanConfigImpl.c
+  VlanConfigImpl.h
+  VlanConfig.vfr
+  VlanConfigStrings.uni
+  VlanConfigNvData.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  NetLib
+  HiiLib
+
+[Guids]
+  gEfiIfrTianoGuid                              ## SOMETIMES_PRODUCES ## UNDEFINED
+  ## SOMETIMES_CONSUMES ## UNDEFINED # HiiIsConfigHdrMatch   mVlanStorageName
+  ## SOMETIMES_PRODUCES ## UNDEFINED # HiiConstructConfigHdr mVlanStorageName
+  ## SOMETIMES_PRODUCES ## UNDEFINED # HiiGetBrowserData     mVlanStorageName
+  ## SOMETIMES_CONSUMES ## UNDEFINED # HiiSetBrowserData     mVlanStorageName
+  ## SOMETIMES_CONSUMES ## HII
+  gVlanConfigFormSetGuid
+
+[Protocols]
+  gEfiHiiConfigAccessProtocolGuid               ## BY_START
+  gEfiHiiConfigRoutingProtocolGuid              ## CONSUMES
+  gEfiVlanConfigProtocolGuid                    ## TO_START
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  VlanConfigDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.uni b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.uni
new file mode 100644
index 0000000000..3dc2c6f3bf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxeExtra.uni b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxeExtra.uni
new file mode 100644
index 0000000000..ade7f17af8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.c b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.c
new file mode 100644
index 0000000000..5e0fe42a88
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.c
@@ -0,0 +1,663 @@
+/** @file
+  HII Config Access protocol implementation of VLAN configuration module.
+
+Copyright (c) 2009 - 2015, 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<BR>
+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 "VlanConfigImpl.h"
+
+CHAR16                          mVlanStorageName[] = L"VlanNvData";
+EFI_HII_CONFIG_ROUTING_PROTOCOL *mHiiConfigRouting = NULL;
+
+VLAN_CONFIG_PRIVATE_DATA        mVlanConfigPrivateDateTemplate = {
+  VLAN_CONFIG_PRIVATE_DATA_SIGNATURE,
+  {
+    VlanExtractConfig,
+    VlanRouteConfig,
+    VlanCallback
+  }
+};
+
+VENDOR_DEVICE_PATH              mHiiVendorDevicePathNode = {
+  {
+    HARDWARE_DEVICE_PATH,
+    HW_VENDOR_DP,
+    {
+      (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+      (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+    }
+  },
+  VLAN_CONFIG_FORM_SET_GUID
+};
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Request            A null-terminated Unicode string in
+                                 <ConfigRequest> format.
+  @param[out]  Progress          On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 '&' before the first failing name/value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name/value pair) if the request was not
+                                 successful.
+  @param[out]  Results           A null-terminated Unicode string in
+                                 <ConfigAltResp> format which has all values filled
+                                 in for the names in the Request string. String to
+                                 be allocated by the called function.
+
+  @retval EFI_SUCCESS            The Results is filled with the requested values.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanExtractConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL        *This,
+  IN CONST EFI_STRING                            Request,
+       OUT EFI_STRING                            *Progress,
+       OUT EFI_STRING                            *Results
+  )
+{
+  EFI_STATUS                 Status;
+  UINTN                      BufferSize;
+  VLAN_CONFIGURATION         Configuration;
+  VLAN_CONFIG_PRIVATE_DATA  *PrivateData;
+  EFI_STRING                 ConfigRequestHdr;
+  EFI_STRING                 ConfigRequest;
+  BOOLEAN                    AllocatedRequest;
+  UINTN                      Size;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Request;
+  if ((Request != NULL) && !HiiIsConfigHdrMatch (Request, &gVlanConfigFormSetGuid, mVlanStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  ConfigRequestHdr = NULL;
+  ConfigRequest    = NULL;
+  AllocatedRequest = FALSE;
+  Size             = 0;
+
+  //
+  // Retrieve the pointer to the UEFI HII Config Routing Protocol
+  //
+  if (mHiiConfigRouting == NULL) {
+    gBS->LocateProtocol (&gEfiHiiConfigRoutingProtocolGuid, NULL, (VOID **) &mHiiConfigRouting);
+  }
+  ASSERT (mHiiConfigRouting != NULL);
+
+  //
+  // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+  //
+  PrivateData = VLAN_CONFIG_PRIVATE_DATA_FROM_THIS (This);
+  ZeroMem (&Configuration, sizeof (VLAN_CONFIGURATION));
+  BufferSize = sizeof (Configuration);
+  ConfigRequest = Request;
+  if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+    //
+    // Request has no request element, construct full request string.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&gVlanConfigFormSetGuid, mVlanStorageName, PrivateData->DriverHandle);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+  }
+
+  Status = mHiiConfigRouting->BlockToConfig (
+                                mHiiConfigRouting,
+                                ConfigRequest,
+                                (UINT8 *) &Configuration,
+                                BufferSize,
+                                Results,
+                                Progress
+                                );
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+  }
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration      A null-terminated Unicode string in <ConfigResp>
+                                 format.
+  @param[out]  Progress          A pointer to a string filled in with the offset of
+                                 the most recent '&' before the first failing
+                                 name/value pair (or the beginning of the string if
+                                 the failure is in the first name/value pair) or
+                                 the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The Results is processed successfully.
+  @retval EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanRouteConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN CONST EFI_STRING                          Configuration,
+       OUT EFI_STRING                          *Progress
+  )
+{
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Configuration;
+  if (!HiiIsConfigHdrMatch (Configuration, &gVlanConfigFormSetGuid, mVlanStorageName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Progress = Configuration + StrLen (Configuration);
+  return EFI_SUCCESS;
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Action             Specifies the type of action taken by the browser.
+  @param[in]  QuestionId         A unique value which is sent to the original
+                                 exporting driver so that it can identify the type
+                                 of data to expect.
+  @param[in]  Type               The type of value for the question.
+  @param[in]  Value              A pointer to the data being sent to the original
+                                 exporting driver.
+  @param[out] ActionRequest      On return, points to the action requested by the
+                                 callback function.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the
+                                 variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved.
+  @retval EFI_UNSUPPORTED        The specified Action is not supported by the
+                                 callback.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanCallback (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN     EFI_BROWSER_ACTION                    Action,
+  IN     EFI_QUESTION_ID                       QuestionId,
+  IN     UINT8                                 Type,
+  IN     EFI_IFR_TYPE_VALUE                    *Value,
+     OUT EFI_BROWSER_ACTION_REQUEST            *ActionRequest
+  )
+{
+  VLAN_CONFIG_PRIVATE_DATA  *PrivateData;
+  VLAN_CONFIGURATION        *Configuration;
+  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;
+  UINTN                     Index;
+  EFI_HANDLE                VlanHandle;
+
+  PrivateData = VLAN_CONFIG_PRIVATE_DATA_FROM_THIS (This);
+
+  if ((Action == EFI_BROWSER_ACTION_FORM_OPEN) || (Action == EFI_BROWSER_ACTION_FORM_CLOSE)) {
+    return EFI_SUCCESS;
+  }
+
+  if ((Action != EFI_BROWSER_ACTION_CHANGED) && (Action != EFI_BROWSER_ACTION_CHANGING)) {
+    //
+    // All other action return unsupported.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Get Browser data
+  //
+  Configuration = AllocateZeroPool (sizeof (VLAN_CONFIGURATION));
+  ASSERT (Configuration != NULL);
+  HiiGetBrowserData (&gVlanConfigFormSetGuid, mVlanStorageName, sizeof (VLAN_CONFIGURATION), (UINT8 *) Configuration);
+
+  VlanConfig = PrivateData->VlanConfig;
+
+  if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    switch (QuestionId) {
+    case VLAN_ADD_QUESTION_ID:
+      //
+      // Add a VLAN
+      //
+      VlanConfig->Set (VlanConfig, Configuration->VlanId, Configuration->Priority);
+      VlanUpdateForm (PrivateData);
+
+      //
+      // Connect the newly created VLAN device
+      //
+      VlanHandle = NetLibGetVlanHandle (PrivateData->ControllerHandle, Configuration->VlanId);
+      if (VlanHandle == NULL) {
+        //
+        // There may be no child handle created for VLAN ID 0, connect the parent handle
+        //
+        VlanHandle = PrivateData->ControllerHandle;
+      }
+      gBS->ConnectController (VlanHandle, NULL, NULL, TRUE);
+
+      //
+      // Clear UI data
+      //
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_APPLY;
+      Configuration->VlanId = 0;
+      Configuration->Priority = 0;
+      break;
+
+    case VLAN_REMOVE_QUESTION_ID:
+      //
+      // Remove VLAN
+      //
+      ASSERT (PrivateData->NumberOfVlan <= MAX_VLAN_NUMBER);
+      for (Index = 0; Index < PrivateData->NumberOfVlan; Index++) {
+        if (Configuration->VlanList[Index] != 0) {
+          //
+          // Checkbox is selected, need remove this VLAN
+          //
+          VlanConfig->Remove (VlanConfig, PrivateData->VlanId[Index]);
+        }
+      }
+
+      VlanUpdateForm (PrivateData);
+      if (PrivateData->NumberOfVlan == 0) {
+        //
+        // No VLAN device now, connect the physical NIC handle.
+        // Note: PrivateData->NumberOfVlan has been updated by VlanUpdateForm()
+        //
+        gBS->ConnectController (PrivateData->ControllerHandle, NULL, NULL, TRUE);
+      }
+
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_FORM_APPLY;
+      ZeroMem (Configuration->VlanList, MAX_VLAN_NUMBER);
+      break;
+
+    default:
+      break;
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    switch (QuestionId) {
+    case VLAN_UPDATE_QUESTION_ID:
+      //
+      // Update current VLAN list into Form.
+      //
+      VlanUpdateForm (PrivateData);
+      break;
+
+    default:
+      break;
+    }
+  }
+  
+  HiiSetBrowserData (&gVlanConfigFormSetGuid, mVlanStorageName, sizeof (VLAN_CONFIGURATION), (UINT8 *) Configuration, NULL);
+  FreePool (Configuration);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function update VLAN list in the VLAN configuration Form.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+**/
+VOID
+VlanUpdateForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  )
+{
+  EFI_VLAN_CONFIG_PROTOCOL  *VlanConfig;
+  UINT16                    NumberOfVlan;
+  UINTN                     Index;
+  EFI_VLAN_FIND_DATA        *VlanData;
+  VOID                      *StartOpCodeHandle;
+  EFI_IFR_GUID_LABEL        *StartLabel;
+  VOID                      *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL        *EndLabel;
+  CHAR16                    *String;
+  CHAR16                    VlanStr[30];
+  CHAR16                    VlanIdStr[6];
+  UINTN                     DigitalCount;
+  EFI_STRING_ID             StringId;
+
+  //
+  // Find current VLAN configuration
+  //
+  VlanData = NULL;
+  NumberOfVlan = 0;
+  VlanConfig = PrivateData->VlanConfig;
+  VlanConfig->Find (VlanConfig, NULL, &NumberOfVlan, &VlanData);
+
+  //
+  // Update VLAN configuration in PrivateData
+  //
+  if (NumberOfVlan > MAX_VLAN_NUMBER) {
+    NumberOfVlan = MAX_VLAN_NUMBER;
+  }
+  PrivateData->NumberOfVlan = NumberOfVlan;
+
+  //
+  // Init OpCode Handle
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (
+                                        StartOpCodeHandle,
+                                        &gEfiIfrTianoGuid,
+                                        NULL,
+                                        sizeof (EFI_IFR_GUID_LABEL)
+                                        );
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = LABEL_VLAN_LIST;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (
+                                      EndOpCodeHandle,
+                                      &gEfiIfrTianoGuid,
+                                      NULL,
+                                      sizeof (EFI_IFR_GUID_LABEL)
+                                      );
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  ZeroMem (PrivateData->VlanId, MAX_VLAN_NUMBER);
+  for (Index = 0; Index < NumberOfVlan; Index++) {
+    String = VlanStr;
+
+    StrCpyS (String, (sizeof (VlanStr) /sizeof (CHAR16)), L"  VLAN ID:");
+    String += 10;
+    //
+    // Pad VlanId string up to 4 characters with space
+    //
+    DigitalCount = UnicodeValueToString (VlanIdStr, 0, VlanData[Index].VlanId, 5);
+    SetMem16 (String, (4 - DigitalCount) * sizeof (CHAR16), L' ');
+    StrCpyS (String + 4 - DigitalCount, (sizeof (VlanStr) /sizeof (CHAR16)) - 10 - (4 - DigitalCount), VlanIdStr);
+    String += 4;
+
+    StrCpyS (String,  (sizeof (VlanStr) /sizeof (CHAR16)) - 10 - (4 - DigitalCount) - 4, L", Priority:");
+    String += 11;
+    String += UnicodeValueToString (String, 0, VlanData[Index].Priority, 4);
+    *String = 0;
+
+    StringId = HiiSetString (PrivateData->HiiHandle, 0, VlanStr, NULL);
+    ASSERT (StringId != 0);
+
+    HiiCreateCheckBoxOpCode (
+      StartOpCodeHandle,
+      (EFI_QUESTION_ID) (VLAN_LIST_VAR_OFFSET + Index),
+      VLAN_CONFIGURATION_VARSTORE_ID,
+      (UINT16) (VLAN_LIST_VAR_OFFSET + Index),
+      StringId,
+      STRING_TOKEN (STR_VLAN_VLAN_LIST_HELP),
+      0,
+      0,
+      NULL
+      );
+
+    //
+    // Save VLAN id to private data
+    //
+    PrivateData->VlanId[Index] = VlanData[Index].VlanId;
+  }
+
+  HiiUpdateForm (
+    PrivateData->HiiHandle,     // HII handle
+    &gVlanConfigFormSetGuid,    // Formset GUID
+    VLAN_CONFIGURATION_FORM_ID, // Form ID
+    StartOpCodeHandle,          // Label for where to insert opcodes
+    EndOpCodeHandle             // Replace data
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+
+  if (VlanData != NULL) {
+    FreePool (VlanData);
+  }
+}
+
+
+/**
+  This function publish the VLAN configuration Form for a network device. The
+  HII Config Access protocol will be installed on a child handle of the network
+  device.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+  @retval EFI_SUCCESS            HII Form is installed for this network device.
+  @retval EFI_OUT_OF_RESOURCES   Not enough resource for HII Form installation.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+InstallVlanConfigForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_HII_HANDLE                  HiiHandle;
+  EFI_HANDLE                      DriverHandle;
+  CHAR16                          Str[26 + sizeof (EFI_MAC_ADDRESS) * 2 + 1];
+  CHAR16                          *MacString;
+  EFI_DEVICE_PATH_PROTOCOL        *ChildDevicePath;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+  EFI_VLAN_CONFIG_PROTOCOL        *VlanConfig;
+
+  //
+  // Create child handle and install HII Config Access Protocol
+  //
+  ChildDevicePath = AppendDevicePathNode (
+                      PrivateData->ParentDevicePath,
+                      (CONST EFI_DEVICE_PATH_PROTOCOL *) &mHiiVendorDevicePathNode
+                      );
+  if (ChildDevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  PrivateData->ChildDevicePath = ChildDevicePath;
+
+  DriverHandle = NULL;
+  ConfigAccess = &PrivateData->ConfigAccess;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  ChildDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  ConfigAccess,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  PrivateData->DriverHandle = DriverHandle;
+
+  //
+  // Establish the parent-child relationship between the new created
+  // child handle and the ControllerHandle.
+  //
+  Status = gBS->OpenProtocol (
+                  PrivateData->ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  (VOID **)&VlanConfig,
+                  PrivateData->ImageHandle,
+                  PrivateData->DriverHandle,
+                  EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Publish the HII package list
+  //
+  HiiHandle = HiiAddPackages (
+                &gVlanConfigFormSetGuid,
+                DriverHandle,
+                VlanConfigDxeStrings,
+                VlanConfigBin,
+                NULL
+                );
+  if (HiiHandle == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+  PrivateData->HiiHandle = HiiHandle;
+
+  //
+  // Update formset title help string.
+  //
+  MacString = NULL;
+  Status = NetLibGetMacString (PrivateData->ControllerHandle, PrivateData->ImageHandle, &MacString);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  PrivateData->MacString = MacString;
+
+  StrCpyS (Str, sizeof (Str) / sizeof (CHAR16), L"VLAN Configuration (MAC:");
+  StrCatS (Str, sizeof (Str) / sizeof (CHAR16), MacString);
+  StrCatS (Str, sizeof (Str) / sizeof (CHAR16), L")");
+  HiiSetString (
+    HiiHandle,
+    STRING_TOKEN (STR_VLAN_FORM_SET_TITLE_HELP),
+    Str,
+    NULL
+    );
+
+  //
+  // Update form title help string.
+  //
+  HiiSetString (
+    HiiHandle,
+    STRING_TOKEN (STR_VLAN_FORM_HELP),
+    Str,
+    NULL
+    );
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This function remove the VLAN configuration Form for a network device. The
+  child handle for HII Config Access protocol will be destroyed.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+  @retval EFI_SUCCESS            HII Form has been uninstalled successfully.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+UninstallVlanConfigForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_VLAN_CONFIG_PROTOCOL     *VlanConfig;
+  
+  //
+  // End the parent-child relationship.
+  //
+  Status = gBS->CloseProtocol (
+                  PrivateData->ControllerHandle,
+                  &gEfiVlanConfigProtocolGuid,
+                  PrivateData->ImageHandle,
+                  PrivateData->DriverHandle
+                  );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Uninstall HII Config Access Protocol
+  //
+  if (PrivateData->DriverHandle != NULL) {
+    Status = gBS->UninstallMultipleProtocolInterfaces (
+                    PrivateData->DriverHandle,
+                    &gEfiDevicePathProtocolGuid,
+                    PrivateData->ChildDevicePath,
+                    &gEfiHiiConfigAccessProtocolGuid,
+                    &PrivateData->ConfigAccess,
+                    NULL
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+             PrivateData->ControllerHandle,
+             &gEfiVlanConfigProtocolGuid,
+             (VOID **)&VlanConfig,
+             PrivateData->ImageHandle,
+             PrivateData->DriverHandle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+      return Status;
+    }
+    PrivateData->DriverHandle = NULL;
+
+    if (PrivateData->ChildDevicePath != NULL) {
+      FreePool (PrivateData->ChildDevicePath);
+      PrivateData->ChildDevicePath = NULL;
+    }
+  }
+
+  //
+  // Free MAC string
+  //
+  if (PrivateData->MacString != NULL) {
+    FreePool (PrivateData->MacString);
+    PrivateData->MacString = NULL;
+  }
+
+  //
+  // Uninstall HII package list
+  //
+  if (PrivateData->HiiHandle != NULL) {
+    HiiRemovePackages (PrivateData->HiiHandle);
+    PrivateData->HiiHandle = NULL;
+  }
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.h b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.h
new file mode 100644
index 0000000000..2aa43b4230
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigImpl.h
@@ -0,0 +1,388 @@
+/** @file
+  Header file for driver binding protocol and HII config access protocol.
+
+Copyright (c) 2009 - 2014, 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<BR>
+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.
+
+**/
+
+#ifndef __VLAN_CONFIG_IMPL_H__
+#define __VLAN_CONFIG_IMPL_H__
+
+#include <Uefi.h>
+
+#include <Protocol/ComponentName.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/VlanConfig.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/NetLib.h>
+#include <Library/HiiLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/PrintLib.h>
+
+#include <Guid/MdeModuleHii.h>
+
+#include "VlanConfigNvData.h"
+
+extern EFI_COMPONENT_NAME2_PROTOCOL gVlanConfigComponentName2;
+extern EFI_COMPONENT_NAME_PROTOCOL  gVlanConfigComponentName;
+
+//
+// Tool generated IFR binary data and String package data
+//
+extern UINT8                        VlanConfigBin[];
+extern UINT8                        VlanConfigDxeStrings[];
+
+#define VLAN_LIST_VAR_OFFSET ((UINT16) OFFSET_OF (VLAN_CONFIGURATION, VlanList))
+
+typedef struct {
+  UINTN                           Signature;
+
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  ConfigAccess;
+  EFI_HII_HANDLE                  HiiHandle;
+  EFI_HANDLE                      DriverHandle;
+  EFI_DEVICE_PATH_PROTOCOL        *ChildDevicePath;
+
+  EFI_HANDLE                      ControllerHandle;
+  EFI_HANDLE                      ImageHandle;
+  EFI_DEVICE_PATH_PROTOCOL        *ParentDevicePath;
+  EFI_VLAN_CONFIG_PROTOCOL        *VlanConfig;
+  CHAR16                          *MacString;
+
+  UINT16                          NumberOfVlan;
+  UINT16                          VlanId[MAX_VLAN_NUMBER];
+} VLAN_CONFIG_PRIVATE_DATA;
+
+#define VLAN_CONFIG_PRIVATE_DATA_SIGNATURE     SIGNATURE_32 ('V', 'C', 'P', 'D')
+#define VLAN_CONFIG_PRIVATE_DATA_FROM_THIS(a)  CR (a, VLAN_CONFIG_PRIVATE_DATA, ConfigAccess, VLAN_CONFIG_PRIVATE_DATA_SIGNATURE)
+
+extern VLAN_CONFIG_PRIVATE_DATA mVlanConfigPrivateDateTemplate;
+
+
+/**
+  Retrieves a Unicode string that is the user readable name of the driver.
+
+  This function retrieves the user readable name of a driver in the form of a
+  Unicode string. If the driver specified by This has a user readable name in
+  the language specified by Language, then a pointer to the driver name is
+  returned in DriverName, and EFI_SUCCESS is returned. If the driver specified
+  by This does not support the language specified by Language,
+  then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language. This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified
+                                in RFC 4646 or ISO 639-2 language code format.
+  @param  DriverName[out]       A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                driver specified by This in the language
+                                specified by Language.
+
+  @retval EFI_SUCCESS           The Unicode string for the Driver specified by
+                                This and the language specified by Language was
+                                returned in DriverName.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER DriverName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigComponentNameGetDriverName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **DriverName
+  );
+
+/**
+  Retrieves a Unicode string that is the user readable name of the controller
+  that is being managed by a driver.
+
+  This function retrieves the user readable name of the controller specified by
+  ControllerHandle and ChildHandle in the form of a Unicode string. If the
+  driver specified by This has a user readable name in the language specified by
+  Language, then a pointer to the controller name is returned in ControllerName,
+  and EFI_SUCCESS is returned.  If the driver specified by This is not currently
+  managing the controller specified by ControllerHandle and ChildHandle,
+  then EFI_UNSUPPORTED is returned.  If the driver specified by This does not
+  support the language specified by Language, then EFI_UNSUPPORTED is returned.
+
+  @param  This[in]              A pointer to the EFI_COMPONENT_NAME2_PROTOCOL or
+                                EFI_COMPONENT_NAME_PROTOCOL instance.
+  @param  ControllerHandle[in]  The handle of a controller that the driver
+                                specified by This is managing.  This handle
+                                specifies the controller whose name is to be
+                                returned.
+  @param  ChildHandle[in]       The handle of the child controller to retrieve
+                                the name of.  This is an optional parameter that
+                                may be NULL.  It will be NULL for device
+                                drivers.  It will also be NULL for a bus drivers
+                                that wish to retrieve the name of the bus
+                                controller.  It will not be NULL for a bus
+                                driver that wishes to retrieve the name of a
+                                child controller.
+  @param  Language[in]          A pointer to a Null-terminated ASCII string
+                                array indicating the language.  This is the
+                                language of the driver name that the caller is
+                                requesting, and it must match one of the
+                                languages specified in SupportedLanguages. The
+                                number of languages supported by a driver is up
+                                to the driver writer. Language is specified in
+                                RFC 4646 or ISO 639-2 language code format.
+  @param  ControllerName[out]   A pointer to the Unicode string to return.
+                                This Unicode string is the name of the
+                                controller specified by ControllerHandle and
+                                ChildHandle in the language specified by
+                                Language from the point of view of the driver
+                                specified by This.
+
+  @retval EFI_SUCCESS           The Unicode string for the user readable name in
+                                the language specified by Language for the
+                                driver specified by This was returned in
+                                DriverName.
+  @retval EFI_INVALID_PARAMETER ControllerHandle is NULL.
+  @retval EFI_INVALID_PARAMETER ChildHandle is not NULL and it is not a valid
+                                EFI_HANDLE.
+  @retval EFI_INVALID_PARAMETER Language is NULL.
+  @retval EFI_INVALID_PARAMETER ControllerName is NULL.
+  @retval EFI_UNSUPPORTED       The driver specified by This is not currently
+                                managing the controller specified by
+                                ControllerHandle and ChildHandle.
+  @retval EFI_UNSUPPORTED       The driver specified by This does not support
+                                the language specified by Language.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigComponentNameGetControllerName (
+  IN     EFI_COMPONENT_NAME_PROTOCOL   *This,
+  IN     EFI_HANDLE                    ControllerHandle,
+  IN     EFI_HANDLE                    ChildHandle OPTIONAL,
+  IN     CHAR8                         *Language,
+     OUT CHAR16                        **ControllerName
+  );
+
+/**
+  Test to see if this driver supports ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to test
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCES           This driver supports this device
+  @retval EFI_ALREADY_STARTED  This driver is already running on this device
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingSupported (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Start this driver on ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to bind driver to
+  @param[in]  RemainingDevicePath  Optional parameter use to pick a specific child
+                                   device to start.
+
+  @retval EFI_SUCCES           This driver is added to ControllerHandle
+  @retval EFI_ALREADY_STARTED  This driver is already running on ControllerHandle
+  @retval other                This driver does not support this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingStart (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL        *RemainingDevicePath OPTIONAL
+  );
+
+/**
+  Stop this driver on ControllerHandle.
+
+  @param[in]  This                 Protocol instance pointer.
+  @param[in]  ControllerHandle     Handle of device to stop driver on
+  @param[in]  NumberOfChildren     Number of Handles in ChildHandleBuffer. If number
+                                   of children is zero stop the entire bus driver.
+  @param[in]  ChildHandleBuffer    List of Child Handles to Stop.
+
+  @retval EFI_SUCCES           This driver is removed ControllerHandle
+  @retval other                This driver was not removed from this device
+
+**/
+EFI_STATUS
+EFIAPI
+VlanConfigDriverBindingStop (
+  IN EFI_DRIVER_BINDING_PROTOCOL     *This,
+  IN EFI_HANDLE                      ControllerHandle,
+  IN UINTN                           NumberOfChildren,
+  IN EFI_HANDLE                      *ChildHandleBuffer
+  );
+
+/**
+  This function update VLAN list in the VLAN configuration Form.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+**/
+VOID
+VlanUpdateForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  );
+
+/**
+  This function publish the VLAN configuration Form for a network device. The
+  HII Config Access protocol will be installed on a child handle of the network
+  device.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+  @retval EFI_SUCCESS            HII Form is installed for this network device.
+  @retval EFI_OUT_OF_RESOURCES   Not enough resource for HII Form installation.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+InstallVlanConfigForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  );
+
+/**
+  This function remove the VLAN configuration Form for a network device. The
+  child handle for HII Config Access protocol will be destroyed.
+
+  @param[in, out]  PrivateData   Points to VLAN configuration private data.
+
+  @retval EFI_SUCCESS            HII Form has been uninstalled successfully.
+  @retval Others                 Other errors as indicated.
+
+**/
+EFI_STATUS
+UninstallVlanConfigForm (
+  IN OUT VLAN_CONFIG_PRIVATE_DATA    *PrivateData
+  );
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Request            A null-terminated Unicode string in
+                                 <ConfigRequest> format.
+  @param[out]  Progress          On return, points to a character in the Request
+                                 string. Points to the string's null terminator if
+                                 request was successful. Points to the most recent
+                                 '&' before the first failing name/value pair (or
+                                 the beginning of the string if the failure is in
+                                 the first name/value pair) if the request was not
+                                 successful.
+  @param[out]  Results           A null-terminated Unicode string in
+                                 <ConfigAltResp> format which has all values filled
+                                 in for the names in the Request string. String to
+                                 be allocated by the called function.
+
+  @retval EFI_SUCCESS            The Results is filled with the requested values.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval EFI_INVALID_PARAMETER  Request is NULL, illegal syntax, or unknown name.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanExtractConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL        *This,
+  IN CONST EFI_STRING                            Request,
+       OUT EFI_STRING                            *Progress,
+       OUT EFI_STRING                            *Results
+  );
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Configuration      A null-terminated Unicode string in <ConfigResp>
+                                 format.
+  @param[out]  Progress          A pointer to a string filled in with the offset of
+                                 the most recent '&' before the first failing
+                                 name/value pair (or the beginning of the string if
+                                 the failure is in the first name/value pair) or
+                                 the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The Results is processed successfully.
+  @retval EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this
+                                 driver.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanRouteConfig (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN CONST EFI_STRING                          Configuration,
+       OUT EFI_STRING                          *Progress
+  );
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param[in]  This               Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param[in]  Action             Specifies the type of action taken by the browser.
+  @param[in]  QuestionId         A unique value which is sent to the original
+                                 exporting driver so that it can identify the type
+                                 of data to expect.
+  @param[in]  Type               The type of value for the question.
+  @param[in]  Value              A pointer to the data being sent to the original
+                                 exporting driver.
+  @param[out] ActionRequest      On return, points to the action requested by the
+                                 callback function.
+
+  @retval EFI_SUCCESS            The callback successfully handled the action.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the
+                                 variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved.
+  @retval EFI_UNSUPPORTED        The specified Action is not supported by the
+                                 callback.
+
+**/
+EFI_STATUS
+EFIAPI
+VlanCallback (
+  IN CONST EFI_HII_CONFIG_ACCESS_PROTOCOL      *This,
+  IN     EFI_BROWSER_ACTION                    Action,
+  IN     EFI_QUESTION_ID                       QuestionId,
+  IN     UINT8                                 Type,
+  IN     EFI_IFR_TYPE_VALUE                    *Value,
+     OUT EFI_BROWSER_ACTION_REQUEST            *ActionRequest
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigNvData.h b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigNvData.h
new file mode 100644
index 0000000000..7dd2fc605b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigNvData.h
@@ -0,0 +1,47 @@
+/** @file
+  Header file for NV data structure definition.
+
+Copyright (c) 2009 - 2011, 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<BR>
+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.
+
+**/
+
+#ifndef __VLAN_CONFIG_NV_DATA_H__
+#define __VLAN_CONFIG_NV_DATA_H__
+
+#include <Guid/HiiPlatformSetupFormset.h>
+#include <Guid/VlanConfigHii.h>
+
+#define VLAN_CONFIGURATION_VARSTORE_ID  0x0001
+#define VLAN_CONFIGURATION_FORM_ID      0x0001
+#define VLAN_HEAD_FORM_ID               0x0002
+
+#define VLAN_ADD_QUESTION_ID            0x1000
+#define VLAN_REMOVE_QUESTION_ID         0x2000
+#define VLAN_UPDATE_QUESTION_ID         0x3000
+
+#define LABEL_VLAN_LIST                 0x0001
+#define LABEL_END                       0xffff
+
+//
+// The maximum number of VLAN that will be displayed on the menu
+//
+#define MAX_VLAN_NUMBER                 100
+
+//
+// Nv Data structure referenced by IFR
+//
+typedef struct {
+  UINT16  VlanId;
+  UINT8   Priority;
+  UINT8   VlanList[MAX_VLAN_NUMBER];
+} VLAN_CONFIGURATION;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigStrings.uni b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigStrings.uni
new file mode 100644
index 0000000000..bf95abd8b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Network/VlanConfigDxe/VlanConfigStrings.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.c b/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.c
new file mode 100644
index 0000000000..14d47849d8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.c
@@ -0,0 +1,1335 @@
+/** @file
+  PCD DXE driver manage all PCD entry initialized in PEI phase and DXE phase, and
+  produce the implementation of native PCD protocol and EFI_PCD_PROTOCOL defined in
+  PI 1.2 Vol3.
+
+Copyright (c) 2006 - 2015, 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 "Service.h"
+
+///
+/// PCD database lock.
+///
+EFI_LOCK mPcdDatabaseLock = EFI_INITIALIZE_LOCK_VARIABLE(TPL_NOTIFY);
+
+///
+/// PCD_PROTOCOL the EDKII native implementation which support dynamic 
+/// type and dynamicEx type PCDs.
+///
+PCD_PROTOCOL mPcdInstance = {
+  DxePcdSetSku,
+
+  DxePcdGet8,
+  DxePcdGet16,
+  DxePcdGet32,
+  DxePcdGet64,
+  DxePcdGetPtr,
+  DxePcdGetBool,
+  DxePcdGetSize,
+
+  DxePcdGet8Ex,
+  DxePcdGet16Ex,
+  DxePcdGet32Ex,
+  DxePcdGet64Ex,
+  DxePcdGetPtrEx,
+  DxePcdGetBoolEx,
+  DxePcdGetSizeEx,
+
+  DxePcdSet8,
+  DxePcdSet16,
+  DxePcdSet32,
+  DxePcdSet64,
+  DxePcdSetPtr,
+  DxePcdSetBool,
+
+  DxePcdSet8Ex,
+  DxePcdSet16Ex,
+  DxePcdSet32Ex,
+  DxePcdSet64Ex,
+  DxePcdSetPtrEx,
+  DxePcdSetBoolEx,
+
+  DxeRegisterCallBackOnSet,
+  DxeUnRegisterCallBackOnSet,
+  DxePcdGetNextToken,
+  DxePcdGetNextTokenSpace
+};
+
+///
+/// EFI_PCD_PROTOCOL is defined in PI 1.2 Vol 3 which only support dynamicEx type
+/// PCD.
+///
+EFI_PCD_PROTOCOL mEfiPcdInstance = {
+  DxePcdSetSku,
+  DxePcdGet8Ex,
+  DxePcdGet16Ex,
+  DxePcdGet32Ex,
+  DxePcdGet64Ex,
+  DxePcdGetPtrEx,
+  DxePcdGetBoolEx,
+  DxePcdGetSizeEx,
+  DxePcdSet8Ex,
+  DxePcdSet16Ex,
+  DxePcdSet32Ex,
+  DxePcdSet64Ex,
+  DxePcdSetPtrEx,
+  DxePcdSetBoolEx,
+  (EFI_PCD_PROTOCOL_CALLBACK_ON_SET) DxeRegisterCallBackOnSet,
+  (EFI_PCD_PROTOCOL_CANCEL_CALLBACK) DxeUnRegisterCallBackOnSet,
+  DxePcdGetNextToken,
+  DxePcdGetNextTokenSpace
+};
+
+///
+/// Instance of GET_PCD_INFO_PROTOCOL protocol is EDKII native implementation.
+/// This protocol instance support dynamic and dynamicEx type PCDs.
+///
+GET_PCD_INFO_PROTOCOL mGetPcdInfoInstance = {
+  DxeGetPcdInfoGetInfo,
+  DxeGetPcdInfoGetInfoEx,
+  DxeGetPcdInfoGetSku
+};
+
+///
+/// Instance of EFI_GET_PCD_INFO_PROTOCOL which is defined in PI 1.2.1 Vol 3.
+/// This PPI instance only support dyanmicEx type PCD.
+///
+EFI_GET_PCD_INFO_PROTOCOL  mEfiGetPcdInfoInstance = {
+  DxeGetPcdInfoGetInfoEx,
+  DxeGetPcdInfoGetSku
+};
+
+EFI_HANDLE mPcdHandle = NULL;
+
+/**
+  Main entry for PCD DXE driver.
+  
+  This routine initialize the PCD database and install PCD_PROTOCOL.
+  
+  @param ImageHandle     Image handle for PCD DXE driver.
+  @param SystemTable     Pointer to SystemTable.
+
+  @return Status of gBS->InstallProtocolInterface()
+
+**/
+EFI_STATUS
+EFIAPI
+PcdDxeInit (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS Status;
+  VOID       *Registration;
+
+  //
+  // Make sure the Pcd Protocol is not already installed in the system
+  //
+
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gPcdProtocolGuid);
+
+  BuildPcdDxeDataBase ();
+
+  //
+  // Install PCD_PROTOCOL to handle dynamic type PCD
+  // Install EFI_PCD_PROTOCOL to handle dynamicEx type PCD
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mPcdHandle,
+                  &gPcdProtocolGuid,     &mPcdInstance,
+                  &gEfiPcdProtocolGuid,  &mEfiPcdInstance,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install GET_PCD_INFO_PROTOCOL to handle dynamic type PCD
+  // Install EFI_GET_PCD_INFO_PROTOCOL to handle dynamicEx type PCD
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mPcdHandle,
+                  &gGetPcdInfoProtocolGuid,     &mGetPcdInfoInstance,
+                  &gEfiGetPcdInfoProtocolGuid,  &mEfiGetPcdInfoInstance,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register callback function upon VariableLockProtocol
+  // to lock the variables referenced by DynamicHii PCDs with RO property set in *.dsc.
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEdkiiVariableLockProtocolGuid,
+    TPL_CALLBACK,
+    VariableLockCallBack,
+    NULL,
+    &Registration
+    );
+
+  return Status;
+}
+
+/**
+  Retrieve additional information associated with a PCD token in the default token space.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+DxeGetPcdInfoGetInfo (
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  return DxeGetPcdInfo (NULL, TokenNumber, PcdInfo);
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+DxeGetPcdInfoGetInfoEx (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  return DxeGetPcdInfo (Guid, TokenNumber, PcdInfo);
+}
+
+/**
+  Retrieve the currently set SKU Id.
+
+  @return   The currently set SKU Id. If the platform has not set at a SKU Id, then the
+            default SKU Id value of 0 is returned. If the platform has set a SKU Id, then the currently set SKU
+            Id is returned.
+**/
+UINTN
+EFIAPI
+DxeGetPcdInfoGetSku (
+  VOID
+  )
+{
+  return mPcdDatabase.DxeDb->SystemSkuId;
+}
+
+/**
+  Sets the SKU value for subsequent calls to set or get PCD token values.
+
+  SetSku() sets the SKU Id to be used for subsequent calls to set or get PCD values. 
+  SetSku() is normally called only once by the system.
+
+  For each item (token), the database can hold a single value that applies to all SKUs, 
+  or multiple values, where each value is associated with a specific SKU Id. Items with multiple, 
+  SKU-specific values are called SKU enabled. 
+  
+  The SKU Id of zero is reserved as a default. The valid SkuId range is 1 to 255.  
+  For tokens that are not SKU enabled, the system ignores any set SKU Id and works with the 
+  single value for that token. For SKU-enabled tokens, the system will use the SKU Id set by the 
+  last call to SetSku(). If no SKU Id is set or the currently set SKU Id isn't valid for the specified token, 
+  the system uses the default SKU Id. If the system attempts to use the default SKU Id and no value has been 
+  set for that Id, the results are unpredictable.
+
+  @param[in]  SkuId The SKU value that will be used when the PCD service will retrieve and 
+              set values associated with a PCD token.
+
+**/
+VOID
+EFIAPI
+DxePcdSetSku (
+  IN  UINTN         SkuId
+  )
+{
+  SKU_ID    *SkuIdTable;
+  UINTN     Index;
+
+  SkuIdTable = (SKU_ID *) ((UINT8 *) mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->SkuIdTableOffset);
+  for (Index = 0; Index < SkuIdTable[0]; Index++) {
+    if (SkuId == SkuIdTable[Index + 1]) {
+      mPcdDatabase.DxeDb->SystemSkuId = (SKU_ID) SkuId;
+      return;
+    }
+  }
+
+  //
+  // Invalid input SkuId, the default SKU Id will be used for the system.
+  //
+  DEBUG ((EFI_D_INFO, "PcdDxe - Invalid input SkuId, the default SKU Id will be used.\n"));
+  mPcdDatabase.DxeDb->SystemSkuId = (SKU_ID) 0;
+  return;
+}
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the current byte-sized value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT8 value.
+  
+**/
+UINT8
+EFIAPI
+DxePcdGet8 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return *((UINT8 *) GetWorker (TokenNumber, sizeof (UINT8)));
+}
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the current 16-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT16 value.
+  
+**/
+UINT16
+EFIAPI
+DxePcdGet16 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return ReadUnaligned16 (GetWorker (TokenNumber, sizeof (UINT16)));
+}
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the current 32-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT32 value.
+  
+**/
+UINT32
+EFIAPI
+DxePcdGet32 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return ReadUnaligned32 (GetWorker (TokenNumber, sizeof (UINT32)));
+}
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the current 64-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT64 value.
+  
+**/
+UINT64
+EFIAPI
+DxePcdGet64 (
+  IN UINTN                     TokenNumber
+  )
+{
+  return ReadUnaligned64(GetWorker (TokenNumber, sizeof (UINT64)));
+}
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrived.
+  
+**/
+VOID *
+EFIAPI
+DxePcdGetPtr (
+  IN UINTN                     TokenNumber
+  )
+{
+  return GetWorker (TokenNumber, 0);
+}
+
+/**
+  Retrieves a Boolean value for a given PCD token.
+
+  Retrieves the current boolean value for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The Boolean value.
+  
+**/
+BOOLEAN
+EFIAPI
+DxePcdGetBool (
+  IN UINTN                     TokenNumber
+  )
+{
+  return *((BOOLEAN *) GetWorker (TokenNumber, sizeof (BOOLEAN)));
+}
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+DxePcdGetSize (
+  IN UINTN                     TokenNumber
+  )
+{
+  UINTN   Size;
+  UINT32  *LocalTokenNumberTable;
+  BOOLEAN IsPeiDb;
+  UINTN   MaxSize;
+  UINTN   TmpTokenNumber;
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  //
+  // Backup the TokenNumber passed in as GetPtrTypeSize need the original TokenNumber
+  // 
+  TmpTokenNumber = TokenNumber;
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  ASSERT (TokenNumber + 1 < mPcdTotalTokenCount + 1);
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  IsPeiDb = (BOOLEAN) (TokenNumber + 1 < mPeiLocalTokenCount + 1);
+  
+  TokenNumber = IsPeiDb ? TokenNumber : 
+                          (TokenNumber - mPeiLocalTokenCount);
+
+  LocalTokenNumberTable = IsPeiDb ? (UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset) 
+                                  : (UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset);
+
+  Size = (LocalTokenNumberTable[TokenNumber] & PCD_DATUM_TYPE_ALL_SET) >> PCD_DATUM_TYPE_SHIFT;
+
+  if (Size == 0) {
+    //
+    // For pointer type, we need to scan the SIZE_TABLE to get the current size.
+    //
+    return GetPtrTypeSize (TmpTokenNumber, &MaxSize);
+  } else {
+    return Size;
+  }
+
+}
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the 8-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid          The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 8-bit value for the PCD token.
+  
+**/
+UINT8
+EFIAPI
+DxePcdGet8Ex (
+  IN CONST EFI_GUID         *Guid,
+  IN UINTN                  ExTokenNumber
+  )
+{
+  return *((UINT8 *) ExGetWorker (Guid, ExTokenNumber, sizeof(UINT8)));
+}
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the 16-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 16-bit value for the PCD token.
+  
+**/
+UINT16
+EFIAPI
+DxePcdGet16Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                ExTokenNumber
+  )
+{
+  return ReadUnaligned16 (ExGetWorker (Guid, ExTokenNumber, sizeof(UINT16)));
+}
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the 32-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 32-bit value for the PCD token.
+  
+**/
+UINT32
+EFIAPI
+DxePcdGet32Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                 ExTokenNumber
+  )
+{
+  return ReadUnaligned32 (ExGetWorker (Guid, ExTokenNumber, sizeof(UINT32)));
+}
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the 64-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 64-bit value for the PCD token.
+  
+**/
+UINT64
+EFIAPI
+DxePcdGet64Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                 ExTokenNumber
+  )
+{
+  return ReadUnaligned64 (ExGetWorker (Guid, ExTokenNumber, sizeof(UINT64)));
+}
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrived.
+  
+**/
+VOID *
+EFIAPI
+DxePcdGetPtrEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                 ExTokenNumber
+  )
+{
+  return  ExGetWorker (Guid, ExTokenNumber, 0);
+}
+
+/**
+  Retrieves an Boolean value for a given PCD token.
+
+  Retrieves the Boolean value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size Boolean value for the PCD token.
+  
+**/
+BOOLEAN
+EFIAPI
+DxePcdGetBoolEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                 ExTokenNumber
+  )
+{
+  return *((BOOLEAN *) ExGetWorker (Guid, ExTokenNumber, sizeof(BOOLEAN)));
+}
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+DxePcdGetSizeEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN                 ExTokenNumber
+  )
+{
+  return DxePcdGetSize(GetExPcdTokenNumber (Guid, (UINT32) ExTokenNumber));
+}
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet8 (
+  IN UINTN              TokenNumber,
+  IN UINT8              Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet16 (
+  IN UINTN              TokenNumber,
+  IN UINT16             Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet32 (
+  IN UINTN              TokenNumber,
+  IN UINT32             Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet64 (
+  IN UINTN              TokenNumber,
+  IN UINT64             Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetPtr (
+  IN          UINTN              TokenNumber,
+  IN OUT      UINTN              *SizeOfBuffer,
+  IN          VOID               *Buffer
+  )
+{
+  return SetWorker (TokenNumber, Buffer, SizeOfBuffer, TRUE);
+}
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetBool (
+  IN UINTN              TokenNumber,
+  IN BOOLEAN            Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet8Ex (
+  IN CONST EFI_GUID         *Guid,
+  IN UINTN                  ExTokenNumber,
+  IN UINT8                  Value
+  )
+{
+  return  ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet16Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             ExTokenNumber,
+  IN UINT16            Value
+  )
+{
+  return  ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet32Ex (
+  IN CONST EFI_GUID     *Guid,
+  IN UINTN              ExTokenNumber,
+  IN UINT32             Value
+  )
+{
+  return  ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet64Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             ExTokenNumber,
+  IN UINT64            Value
+  )
+{
+  return  ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetPtrEx (
+  IN            CONST EFI_GUID         *Guid,
+  IN            UINTN                  ExTokenNumber,
+  IN OUT        UINTN                  *SizeOfBuffer,
+  IN            VOID                   *Buffer
+  )
+{
+  return  ExSetWorker(ExTokenNumber, Guid, Buffer, SizeOfBuffer, TRUE);
+}
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetBoolEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             ExTokenNumber,
+  IN BOOLEAN           Value
+  )
+{
+  return  ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Specifies a function to be called anytime the value of a designated token is changed.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  )
+{
+  EFI_STATUS Status;
+  
+  if (CallBackFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Aquire lock to prevent reentrance from TPL_CALLBACK level
+  //
+  EfiAcquireLock (&mPcdDatabaseLock);
+
+  Status = DxeRegisterCallBackWorker (TokenNumber, Guid, CallBackFunction);
+
+  EfiReleaseLock (&mPcdDatabaseLock);
+  
+  return Status;
+}
+
+/**
+  Cancels a previously set callback function for a particular PCD token number.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeUnRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  )
+{
+  EFI_STATUS Status;
+  
+  if (CallBackFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Aquire lock to prevent reentrance from TPL_CALLBACK level
+  //
+  EfiAcquireLock (&mPcdDatabaseLock);
+  
+  Status = DxeUnRegisterCallBackWorker (TokenNumber, Guid, CallBackFunction);
+
+  EfiReleaseLock (&mPcdDatabaseLock);
+  
+  return Status;
+}
+
+/**
+  Retrieves the next valid token number in a given namespace.  
+  
+  This is useful since the PCD infrastructure contains a sparse list of token numbers, 
+  and one cannot a priori know what token numbers are valid in the database. 
+  
+  If TokenNumber is 0 and Guid is not NULL, then the first token from the token space specified by Guid is returned.  
+  If TokenNumber is not 0 and Guid is not NULL, then the next token in the token space specified by Guid is returned.  
+  If TokenNumber is 0 and Guid is NULL, then the first token in the default token space is returned.  
+  If TokenNumber is not 0 and Guid is NULL, then the next token in the default token space is returned.  
+  The token numbers in the default token space may not be related to token numbers in token spaces that are named by Guid.  
+  If the next token number can be retrieved, then it is returned in TokenNumber, and EFI_SUCCESS is returned.  
+  If TokenNumber represents the last token number in the token space specified by Guid, then EFI_NOT_FOUND is returned.  
+  If TokenNumber is not present in the token space specified by Guid, then EFI_NOT_FOUND is returned.
+
+
+  @param[in]      Guid    The 128-bit unique value that designates the namespace from which to retrieve the next token. 
+                          This is an optional parameter that may be NULL.  If this parameter is NULL, then a request is 
+                          being made to retrieve tokens from the default token space.
+  @param[in, out] TokenNumber 
+                          A pointer to the PCD token number to use to find the subsequent token number.  
+
+  @retval EFI_SUCCESS   The PCD service has retrieved the next valid token number.
+  @retval EFI_NOT_FOUND The PCD service could not find data from the requested token number.
+
+**/
+EFI_STATUS
+EFIAPI
+DxePcdGetNextToken (
+  IN CONST EFI_GUID         *Guid, OPTIONAL
+  IN OUT   UINTN            *TokenNumber
+  )
+{
+  EFI_STATUS          Status;
+  BOOLEAN             PeiExMapTableEmpty;
+  BOOLEAN             DxeExMapTableEmpty;
+
+  Status = EFI_NOT_FOUND;
+  PeiExMapTableEmpty = mPeiExMapTableEmpty;
+  DxeExMapTableEmpty = mDxeExMapTableEmpty;
+
+  //
+  // Scan the local token space
+  //
+  if (Guid == NULL) {
+    // EBC compiler is very choosy. It may report warning about comparison
+    // between UINTN and 0 . So we add 1 in each size of the 
+    // comparison.
+    if (((*TokenNumber + 1 > mPeiNexTokenCount + 1) && (*TokenNumber + 1 <= mPeiLocalTokenCount + 1)) ||
+        ((*TokenNumber + 1 > (mPeiLocalTokenCount + mDxeNexTokenCount + 1)))) {
+      return EFI_NOT_FOUND;
+    }
+    
+    (*TokenNumber)++;
+    if ((*TokenNumber + 1 > mPeiNexTokenCount + 1) &&
+        (*TokenNumber + 1 <= mPeiLocalTokenCount + 1)) {
+      //
+      // The first Non-Ex type Token Number for DXE PCD 
+      // database is mPeiLocalTokenCount + 1
+      //
+      if (mDxeNexTokenCount > 0) {
+        *TokenNumber = mPeiLocalTokenCount + 1;
+      } else {
+        *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
+        return EFI_NOT_FOUND;
+      }
+    } else if (*TokenNumber + 1 > mDxeNexTokenCount + mPeiLocalTokenCount + 1) {
+      *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
+      return EFI_NOT_FOUND;
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (PeiExMapTableEmpty && DxeExMapTableEmpty) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (!PeiExMapTableEmpty) {
+    Status = ExGetNextTokeNumber (
+                        Guid,
+                        TokenNumber,
+                        (EFI_GUID *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->GuidTableOffset),
+                        mPeiGuidTableSize,
+                        (DYNAMICEX_MAPPING *)((UINT8 *) mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->ExMapTableOffset),
+                        mPeiExMapppingTableSize
+                        );
+  }
+
+  if (Status == EFI_SUCCESS) {
+    return Status;
+  }
+
+  if (!DxeExMapTableEmpty) {
+    Status = ExGetNextTokeNumber (
+                        Guid,
+                        TokenNumber,
+                        (EFI_GUID *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->GuidTableOffset),
+                        mDxeGuidTableSize,
+                        (DYNAMICEX_MAPPING *)((UINT8 *) mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->ExMapTableOffset),
+                        mDxeExMapppingTableSize
+                        );
+  }
+
+  return Status;
+}
+
+/**
+  Get all token space guid table which is different with given token space guid.
+
+  @param ExMapTableSize  The size of ExMapTable in item
+  @param ExMapTable      Token space guid table that want to be scaned.
+  @param GuidTable       Guid table
+
+  @return all token space guid table which is different with given token space guid.
+
+**/
+EFI_GUID **
+GetDistinctTokenSpace (
+  IN OUT    UINTN             *ExMapTableSize,
+  IN        DYNAMICEX_MAPPING *ExMapTable,
+  IN        EFI_GUID          *GuidTable
+  )
+{
+  EFI_GUID  **DistinctTokenSpace;
+  UINTN     OldGuidIndex;
+  UINTN     TsIdx;
+  UINTN     TempTsIdx;
+  UINTN     Idx;
+  BOOLEAN   Match;
+
+  DistinctTokenSpace = AllocateZeroPool (*ExMapTableSize * sizeof (EFI_GUID *));
+  ASSERT (DistinctTokenSpace != NULL);
+
+  TsIdx = 0;
+  OldGuidIndex = ExMapTable[0].ExGuidIndex;
+  DistinctTokenSpace[TsIdx] = &GuidTable[OldGuidIndex];
+  for (Idx = 1; Idx < *ExMapTableSize; Idx++) {
+    Match = FALSE;
+    OldGuidIndex = ExMapTable[Idx].ExGuidIndex;
+    for (TempTsIdx = 0; TempTsIdx <= TsIdx; TempTsIdx++) {
+      if (&GuidTable[OldGuidIndex] == DistinctTokenSpace[TempTsIdx]) {
+        //
+        // Have recorded this GUID.
+        //
+        Match = TRUE;
+        break;
+      }
+    }
+    if (!Match) {
+      DistinctTokenSpace[++TsIdx] = &GuidTable[OldGuidIndex];
+    }
+  }
+
+  //
+  // The total number of Distinct Token Space
+  // is TsIdx + 1 because we use TsIdx as a index
+  // to the DistinctTokenSpace[]
+  //
+  *ExMapTableSize = TsIdx + 1;
+  return DistinctTokenSpace;
+    
+}
+  
+/**
+  Retrieves the next valid PCD token namespace for a given namespace.
+
+  Gets the next valid token namespace for a given namespace. This is useful to traverse the valid
+  token namespaces on a platform.
+
+  @param[in, out]   Guid    An indirect pointer to EFI_GUID. On input it designates a known token
+                            namespace from which the search will start. On output, it designates the next valid
+                            token namespace on the platform. If *Guid is NULL, then the GUID of the first token
+                            space of the current platform is returned. If the search cannot locate the next valid
+                            token namespace, an error is returned and the value of *Guid is undefined.
+ 
+  @retval  EFI_SUCCESS      The PCD service retrieved the value requested.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the next valid token namespace.
+
+**/
+EFI_STATUS
+EFIAPI
+DxePcdGetNextTokenSpace (
+  IN OUT CONST EFI_GUID               **Guid
+  )
+{
+  UINTN               Idx;
+  UINTN               Idx2;
+  UINTN               Idx3;
+  UINTN               PeiTokenSpaceTableSize;
+  UINTN               DxeTokenSpaceTableSize;
+  EFI_GUID            **PeiTokenSpaceTable;
+  EFI_GUID            **DxeTokenSpaceTable;
+  BOOLEAN             Match;
+  BOOLEAN             PeiExMapTableEmpty;
+  BOOLEAN             DxeExMapTableEmpty;
+
+  ASSERT (Guid != NULL);
+  
+  PeiExMapTableEmpty = mPeiExMapTableEmpty;
+  DxeExMapTableEmpty = mDxeExMapTableEmpty;
+
+  if (PeiExMapTableEmpty && DxeExMapTableEmpty) {
+    return EFI_NOT_FOUND;
+  }
+  
+  if (TmpTokenSpaceBuffer[0] == NULL) {
+    PeiTokenSpaceTableSize = 0;
+
+    if (!PeiExMapTableEmpty) {
+      PeiTokenSpaceTableSize = mPeiExMapppingTableSize / sizeof(DYNAMICEX_MAPPING);
+      PeiTokenSpaceTable = GetDistinctTokenSpace (&PeiTokenSpaceTableSize,
+                            (DYNAMICEX_MAPPING *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->ExMapTableOffset),
+                            (EFI_GUID *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->GuidTableOffset)
+                            );
+      CopyMem (TmpTokenSpaceBuffer, PeiTokenSpaceTable, sizeof (EFI_GUID*) * PeiTokenSpaceTableSize);
+      FreePool (PeiTokenSpaceTable);
+    }
+
+    if (!DxeExMapTableEmpty) {
+      DxeTokenSpaceTableSize = mDxeExMapppingTableSize / sizeof(DYNAMICEX_MAPPING);
+      DxeTokenSpaceTable = GetDistinctTokenSpace (&DxeTokenSpaceTableSize,
+                            (DYNAMICEX_MAPPING *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->ExMapTableOffset),
+                            (EFI_GUID *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->GuidTableOffset)
+                            );
+
+      //
+      // Make sure EFI_GUID in DxeTokenSpaceTable does not exist in PeiTokenSpaceTable
+      //
+      for (Idx2 = 0, Idx3 = PeiTokenSpaceTableSize; Idx2 < DxeTokenSpaceTableSize; Idx2++) {
+        Match = FALSE;
+        for (Idx = 0; Idx < PeiTokenSpaceTableSize; Idx++) {
+          if (CompareGuid (TmpTokenSpaceBuffer[Idx], DxeTokenSpaceTable[Idx2])) {
+            Match = TRUE;
+            break;
+          }
+        }
+        if (!Match) {
+          TmpTokenSpaceBuffer[Idx3++] = DxeTokenSpaceTable[Idx2];
+        }
+      }
+
+      TmpTokenSpaceBufferCount = Idx3;
+      FreePool (DxeTokenSpaceTable);
+    }
+  }
+
+  if (*Guid == NULL) {
+    *Guid = TmpTokenSpaceBuffer[0];
+    return EFI_SUCCESS;
+  }
+  
+  for (Idx = 0; Idx < TmpTokenSpaceBufferCount; Idx++) {
+    if (CompareGuid (*Guid, TmpTokenSpaceBuffer[Idx])) {
+      if (Idx == TmpTokenSpaceBufferCount - 1) {
+        //
+        // It has been the last token namespace.
+        //
+        *Guid = NULL;
+        return EFI_NOT_FOUND;
+      } else {
+        Idx++;
+        *Guid = TmpTokenSpaceBuffer[Idx];
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.inf b/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.inf
new file mode 100644
index 0000000000..54cd8d9b9f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/Pcd.inf
@@ -0,0 +1,352 @@
+## @file
+# PCD DXE driver manage database contains all dynamic PCD entries and produce the implementation of PCD protocol.
+#
+# This version PCD DXE depends on the external PCD database binary file, not built in PCD data base. 
+# There are two PCD Protocols as follows:
+#   1) PCD_PROTOCOL 
+#      It is EDKII implementation which support Dynamic/DynamicEx type Pcds.
+#   2) EFI_PCD_PROTOCOL
+#      It is defined by PI specification 1.2, Vol 3 which only support dynamicEx 
+#      type Pcd.
+#
+# For dynamicEx type PCD, it is compatible between PCD_PROTOCOL and EFI_PCD_PROTOCOL. 
+# PCD DXE driver will produce above two protocols at same time.
+#
+# PCD database is generated as the separate binary image at build time. The binary image 
+# will be intergrated into Firmware volume together with PCD driver. 
+#
+# ////////////////////////////////////////////////////////////////////////////////
+# //                                                                            //
+# //                      Introduction of PCD database                          //
+# //                                                                            //
+# ////////////////////////////////////////////////////////////////////////////////
+#
+# 1, Introduction
+#    PCD database hold all dynamic type PCD information. The structure of PEI PCD 
+#    database is generated by build tools according to dynamic PCD usage for 
+#    specified platform.
+#    
+# 2, Dynamic Type PCD
+#    Dynamic type PCD is used for the configuration/setting which value is determined
+#    dynamic. In contrast, the value of static type PCD (FeatureFlag, FixedPcd, 
+#    PatchablePcd) is fixed in final generated FD image in build time. 
+#        
+#    2.1 The "dynamic" determination means one of below cases:
+#      a) The PCD setting value is produced by someone driver and consumed by 
+#         other driver in execution time.
+#      b) The PCD setting value is set/get by user from FrontPage.
+#      c) The PCD setting value is produced by platform OEM vendor in specified area.
+#    
+#    2.2 According to module distribution way, dynamic PCD could be classfied as:
+#      a) Dynamic:
+#         If module is released in source code and will be built with platform 
+#         DSC, the dynamic PCD used by this module can be accessed as:
+#                 PcdGetxx(PcdSampleDynamicPcd); 
+#         In building platform, build tools will translate PcdSampleDynamicPcd to
+#         pair of {Token Space Guid: Token Number} for this PCD. 
+#      b) DynamicEx:
+#         If module is release as binary and will not pariticpate platform building,
+#         the dynamic PCD used by this module need be accessed as:
+#               PcdGetxxEx(gEfiMyTokenspaceGuid, PcdSampleDynamicPcd)
+#         Developer need explicity gives {Token Space Guid:Token Number} as parameter
+#         in writting source code.
+#         
+#    2.3 According to PCD value's storage method, dynamic PCD could be classfied as:
+#      a) Default Storage: 
+#         - The PCD value is stored in PCD database maintained by PCD driver in boot 
+#           time memory.
+#         - This type is used for communication between PEIM/DXE driver, DXE/DXE 
+#           driver. But all set/get value will be losted after boot-time memory 
+#           is turn off.
+#         - [PcdsDynamicDefault] is used as section name for this type PCD in 
+#           platform DSC file. [PcdsDynamicExDefault] is used for dynamicEx type PCD.
+#         
+#      b) Variable Storage: 
+#         - The PCD value is stored in variable area. 
+#         - As default storage type, this type PCD could be used for PEI/DXE driver
+#           communication. But beside it, this type PCD could also be used to store 
+#           the value associate with a HII setting via variable interface.
+#         - In PEI phase, the PCD value could only be got but can not be set due 
+#           to variable area is readonly.
+#         - [PcdsDynamicHii] is used as section name for this type PCD in platform 
+#           DSC file. [PcdsDynamicExHii] is for dynamicEx type PCD.
+#           
+#      c) OEM specificed storage area:
+#         - The PCD value is stored in OEM specified area which base address is 
+#           specified by a FixedAtBuild PCD setting - PcdVpdBaseAddress.
+#         - The area is read only for PEI and DXE phase.
+#         - [PcdsDynamicVpd] is used as section name for this type PCD in platform 
+#           DSC file. [PcdsDynamicExVpd] is for dynamicex type PCD.
+#      
+#    2.4 When and how to use dynamic PCD
+#      Module developer do not care the used PCD is dynamic or static when writting
+#      source code/INF. Dynamic PCD and dynamic type is pointed by platform integrator 
+#      in platform DSC file. Please ref section 2.3 to get matching between dynamic
+#      PCD type and section name in DSC file.
+#    
+# 3, PCD database:
+#    Although dynamic PCD could be in different storage type as above description, 
+#    but the basic information and default value for all dynamic PCD is hold
+#    by PCD database maintained by PEI/DXE driver.
+#    
+#    As the whole EFI BIOS boot path is divided into PEI/DXE phase, the PCD database
+#    also is divided into Pei/Dxe database maintaied by PcdPeim/PcdDxe driver separatly.
+#    To make PcdPeim's driver image smaller, PEI PCD database only hold all dynamic
+#    PCD information used in PEI phase or use in both PEI/DXE phase. And DXE PCD
+#    database contains all PCDs used in PEI/DXE phase in memory.
+#    
+#    Build tool will generate PCD database into the separate binary file for 
+#    PEI/DXE PCD driver according to dynamic PCD section in platform DSC file. 
+#    
+#    3.1 PcdPeim and PcdDxe
+#      PEI PCD database is maintained by PcdPeim driver run from flash. PcdPeim driver
+#      build guid hob in temporary memory and copy the binary data base from flash 
+#      to temporary memory for PEI PCD database. 
+#      DXE PCD database is maintained by PcdDxe driver.At entry point of PcdDxe driver,
+#      a new PCD database is allocated in boot-time memory which including all
+#      PEI PCD and DXE PCD entry.
+#      
+#      Pcd driver should run as early as possible before any other driver access
+#      dynamic PCD's value. PEI/DXE "Apriori File" mechanism make it possible by
+#      making PcdPeim/PcdDxe as first dispatching driver in PEI/DXE phase.
+#      
+#    3.2 Token space Guid/Token number, Platform token, Local token number
+#           Dynamic PCD
+#          +-----------+               +---------+
+#          |TokenSpace |               |Platform |
+#          |   Guid    |  build tool   | Token   |
+#          |    +      +-------------->| Number  |
+#          |  Token    |               +---------+`._
+#          |  Number   |                             `.
+#          +-----------+                               `.  +------+
+#                                                        `-|Local |
+#                                                          |Token |
+#                               DynamicEx PCD            ,-|Number|
+#                               +-----------+         ,-'  +------+
+#                               |TokenSpace |      ,-'
+#                               |   Guid    |  _,-'
+#                               |    +      +.'
+#                               |  Token    |
+#                               |  Number   |
+#                               +-----------+
+#    
+#    
+#      3.2.1 Pair of Token space guid + Token number
+#        Any type PCD is identified by pair of "TokenSpaceGuid + TokeNumber". But it
+#        is not easy maintained by PCD driver, and hashed token number will make 
+#        searching slowly. 
+#
+#      3.2.2 Platform Token Number
+#        "Platform token number" concept is introduced for mapping to a pair of 
+#        "TokenSpaceGuid + TokenNumber". The platform token number is generated by 
+#        build tool in autogen.h and all of them are continual in a platform scope 
+#        started from 1.(0 meaning invalid internal token number)
+#        With auto-generated "platform token number", PcdGet(PcdSampleDynamicPcd)
+#        in source code is translated to LibPcdGet(_PCD_TOKEN_PcdSampleDynamicPcd) 
+#        in autogen.h.
+#        Notes: The mapping between pair of "tokenspace guid + token number" and
+#        "internal token number" need build tool establish, so "platform token number"
+#        mechanism is not suitable for binary module which use DynamicEx type PCD.
+#        To access a dynamicEx type PCD, pair of "token space guid/token number" all need
+#        to be specificed for PcdSet/PcdGet accessing macro.
+#      
+#        Platform Token Number is started from 1, and inceased continuous. From whole 
+#        platform scope, there are two zones: PEI Zone and DXE Zone
+#                  |                      Platform Token Number
+#        ----------|----------------------------------------------------------------
+#        PEI Zone: |            1                 ~  PEI_LOCAL_TOKEN_NUMBER
+#        DXE Zone: | (PEI_LOCAL_TOKEN_NUMBER + 1) ~ (PEI_LOCAL_TOKEN_NUMBER + DXE_LOCAL_TOKEN_NUMBER)
+#        
+#      3.2.3 Local Token Number
+#        To fast searching a PCD entry in PCD database, PCD driver translate 
+#        platform token number to local token number via a mapping table.
+#        For binary DynamicEx type PCD, there is a another mapping table to translate
+#        "token space guid + token number" to local token number directly.
+#        Local token number is identifier for all internal interface in PCD PEI/DXE
+#        driver.
+#        
+#        A local token number is a 32-bit value in following meaning:
+#         32 ------------- 28 ---------- 24 -------- 0
+#          | PCD type mask  | Datum Type  |  Offset  |
+#          +-----------------------------------------+
+#        where:
+#          PCd type mask: indicate Pcd type from following macro:
+#                         PCD_TYPE_DATA
+#                         PCD_TYPE_HII
+#                         PCD_TYPE_VPD
+#                         PCD_TYPE_SKU_ENABLED
+#                         PCD_TYPE_STRING
+#          Datum Type   : indicate PCD vaue type from following macro:
+#                         PCD_DATUM_TYPE_POINTER
+#                         PCD_DATUM_TYPE_UINT8
+#                         PCD_DATUM_TYPE_UINT16
+#                         PCD_DATUM_TYPE_UINT32
+#                         PCD_DATUM_TYPE_UINT64
+#          Offset      : indicate the related offset of PCD value in PCD database array.
+#       Based on local token number, PCD driver could fast determine PCD type, value
+#       type and get PCD entry from PCD database.
+#       
+#    3.3 PCD Database binary file
+#      PCD Database binary file will be created at build time as the standalone binary image. 
+#      To understand the binary image layout, PCD Database C structure is still generated 
+#      as comments by build tools in PCD driver's autogen.h/
+#      autogen.c file. In generated C structure, following information is stored:
+#      - ExMapTable: This table is used translate a binary dynamicex type PCD's 
+#                    "tokenguid + token" to local token number.
+#      - LocalTokenNumberTable:
+#                    This table stores all local token number in array, use "Internal
+#                    token number" as array index to get PCD entry's offset fastly.
+#      - SizeTable:  This table stores the size information for all PCD entry.
+#      - GuidTable:  This table stores guid value for DynamicEx's token space,
+#                    HII type PCD's variable GUID.
+#      - SkuIdTable: TBD
+#      - SystemSkuId: TBD
+#      - PCD value structure:  
+#                    Every PCD has a value record in PCD database. For different
+#                    datum type PCD has different record structure which will be 
+#                    introduced in 3.3.1
+#      
+#      In a PCD database structure, there are two major area: Init and UnInit. 
+#      Init area is use stored above PCD internal structure such as ExMapTable, 
+#      LocalTokenNumberTable etc and the (default) value of PCD which has default 
+#      value specified in platform DSC file.
+#      Unint area is used stored the value of PCD which has no default value in
+#      platform DSC file, the value of NULL, 0 specified in platform DSC file can
+#      be seemed as "no default value".
+#      
+#      3.3.1 Simple Sample PCD Database C Structure
+#        A general sample of PCD database structue is as follows:
+#        typedef struct _PCD_DATABASE {
+#          typedef struct _PCD_DATABASE_INIT {
+#            //===== Following is PCD database internal maintain structures
+#            DYNAMICEX_MAPPING ExMapTable[PEI_EXMAPPING_TABLE_SIZE];
+#            UINT32            LocalTokenNumberTable[PEI_LOCAL_TOKEN_NUMBER_TABLE_SIZE];
+#            GUID              GuidTable[PEI_GUID_TABLE_SIZE];
+#            SIZE_INFO         SizeTable[PEI_SIZE_TABLE_SIZE];
+#            UINT8             SkuIdTable[PEI_SKUID_TABLE_SIZE];
+#            SKU_ID            SystemSkuId;
+#            
+#            //===== Following is value structure for PCD with default value
+#            ....
+#            ....
+#            ....
+#          } Init;
+#          typedef struct _PCD_DATABSE_UNINIT {
+#            //==== Following is value structure for PCD without default value
+#            ....
+#            ....
+#          } UnInit;
+#        }
+#      
+#      3.3.2 PCD value structure in PCD database C structure
+#        The value's structure is generated by build tool in PCD database C structure.
+#        The PCDs in different datum type has different value structure.
+#        
+#        3.3.2.1 UINT8/UINT16/UINT32/UINT64 datum type PCD
+#          The C structure for these datum type PCD is just a UINT8/UINT16/UINT32/UINT64
+#          data member in PCD database, For example:
+#          UINT16  PcdHardwareErrorRecordLevel_d3705011_bc19_4af7_be16_f68030378c15_VariableDefault_0;
+#          Above structure is generated by build tool, the member name is "PcdCName_Guidvalue"
+#          Member type is UINT16 according to PcdHardwareErrorRecordLevel declaration
+#          in DEC file.
+#          
+#        3.3.2.2 VOID* datum type PCD
+#          The value of VOID* datum type PCD is a UINT8/UINT16 array in PCD database.
+#          
+#          3.3.2.2.1 VOID* - string type
+#            If the default value for VOID* datum type PCD like L"xxx", the PCD is 
+#            used for unicode string, and C structure of this datum type PCD is 
+#            UINT16 string array in PCD database, for example:
+#            UINT16 StringTable[29];
+#            The number of 29 in above sample is max size of a unicode string.
+#            
+#            If the default value for VOID* datum type PCD like "xxx", the PCD is
+#            used for ascii string, and C structure of this datum type PCD is 
+#            UINT8 string array in PCD database, for example:
+#            UINT8 StringTable[20];
+#            The number of 20 in above sample is max size of a ascii string.
+#            
+#          3.3.2.2.2 VOID* - byte array
+#            If the default value of VOID* datum type PCD like {'0x29', '0x01', '0xf2'}
+#            the PCD is used for byte array. The generated structrue is same as 
+#            above ascii string table,
+#            UINT8 StringTable[13];
+#            The number of 13 in above sample is max size of byte array.
+#       
+#      3.3.3 Some utility structures in PCD Database
+#        3.3.3.1 GuidTable
+#          GuidTable array is used to store all related GUID value in PCD database:
+#            - Variable GUID for HII type PCD
+#            - Token space GUID for dynamicex type PCD 
+#    
+#  Copyright (c) 2006 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PcdDxe
+  MODULE_UNI_FILE                = PcdDxe.uni
+  FILE_GUID                      = 80CF7257-87AB-47f9-A3FE-D50B76D89541
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 4.0
+  PCD_IS_DRIVER                  = DXE_PCD_DRIVER
+  ENTRY_POINT                    = PcdDxeInit
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Pcd.c
+  Service.c
+  Service.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  HobLib
+  UefiDriverEntryPoint
+  UefiLib
+  DebugLib
+  BaseLib
+  PcdLib
+  DxeServicesLib
+
+[Guids]
+  gPcdDataBaseHobGuid                           ## SOMETIMES_CONSUMES  ## HOB
+  gPcdDataBaseSignatureGuid                     ## CONSUMES  ## GUID  # PCD database signature GUID.
+
+[Protocols]
+  gPcdProtocolGuid                              ## PRODUCES
+  gEfiPcdProtocolGuid                           ## PRODUCES
+  gGetPcdInfoProtocolGuid                       ## SOMETIMES_PRODUCES
+  gEfiGetPcdInfoProtocolGuid                    ## SOMETIMES_PRODUCES
+  ## NOTIFY
+  ## SOMETIMES_CONSUMES
+  gEdkiiVariableLockProtocolGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVpdBaseAddress  ## SOMETIMES_CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PcdDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxe.uni b/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxe.uni
new file mode 100644
index 0000000000..829eb2b046
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxe.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxeExtra.uni b/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxeExtra.uni
new file mode 100644
index 0000000000..3b4b5018f7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/PcdDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/Service.c b/Core/MdeModulePkg/Universal/PCD/Dxe/Service.c
new file mode 100644
index 0000000000..eb1e1cef0b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/Service.c
@@ -0,0 +1,1923 @@
+/** @file
+    Help functions used by PCD DXE driver.
+
+Copyright (c) 2014, Hewlett-Packard Development Company, L.P.<BR>
+Copyright (c) 2006 - 2015, 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 "Service.h"
+#include <Library/DxeServicesLib.h>
+
+PCD_DATABASE   mPcdDatabase;
+
+UINT32         mPcdTotalTokenCount; 
+UINT32         mPeiLocalTokenCount; 
+UINT32         mDxeLocalTokenCount; 
+UINT32         mPeiNexTokenCount;   
+UINT32         mDxeNexTokenCount;  
+UINT32         mPeiExMapppingTableSize;
+UINT32         mDxeExMapppingTableSize;
+UINT32         mPeiGuidTableSize;
+UINT32         mDxeGuidTableSize;
+
+BOOLEAN        mPeiExMapTableEmpty; 
+BOOLEAN        mDxeExMapTableEmpty; 
+BOOLEAN        mPeiDatabaseEmpty;
+
+LIST_ENTRY    *mCallbackFnTable;
+EFI_GUID     **TmpTokenSpaceBuffer;
+UINTN          TmpTokenSpaceBufferCount; 
+
+/**
+  Get Local Token Number by Token Number.
+
+  @param[in]    IsPeiDb     If TRUE, the pcd entry is initialized in PEI phase,
+                            If FALSE, the pcd entry is initialized in DXE phase.
+  @param[in]    TokenNumber The PCD token number.
+
+  @return       Local Token Number.
+**/
+UINT32
+GetLocalTokenNumber (
+  IN BOOLEAN            IsPeiDb,
+  IN UINTN              TokenNumber
+  )
+{
+  UINTN                 TmpTokenNumber;
+  UINT32                *LocalTokenNumberTable;
+  UINT32                LocalTokenNumber;
+  UINTN                 Size;
+  UINTN                 MaxSize;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  //
+  // Backup the TokenNumber passed in as GetPtrTypeSize need the original TokenNumber
+  // 
+  TmpTokenNumber = TokenNumber;
+
+  LocalTokenNumberTable  = IsPeiDb ? (UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset) : 
+                                     (UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset);
+  TokenNumber            = IsPeiDb ? TokenNumber : TokenNumber - mPeiLocalTokenCount;
+
+  LocalTokenNumber = LocalTokenNumberTable[TokenNumber];
+
+  Size = (LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) >> PCD_DATUM_TYPE_SHIFT;
+
+  if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == PCD_TYPE_SKU_ENABLED) {
+    if (Size == 0) {
+      GetPtrTypeSize (TmpTokenNumber, &MaxSize);
+    } else {
+      MaxSize = Size;
+    }
+    LocalTokenNumber = GetSkuEnabledTokenNumber (LocalTokenNumber & ~PCD_TYPE_SKU_ENABLED, MaxSize, IsPeiDb);
+  }
+
+  return LocalTokenNumber;
+}
+
+/**
+  Get PCD type by Local Token Number.
+
+  @param[in]    LocalTokenNumber The PCD local token number.
+
+  @return       PCD type.
+**/
+EFI_PCD_TYPE
+GetPcdType (
+  IN UINT32             LocalTokenNumber
+  )
+{
+  switch (LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) {
+    case PCD_DATUM_TYPE_POINTER:
+      return EFI_PCD_TYPE_PTR;
+    case PCD_DATUM_TYPE_UINT8:
+      if ((LocalTokenNumber & PCD_DATUM_TYPE_UINT8_BOOLEAN) == PCD_DATUM_TYPE_UINT8_BOOLEAN) {
+        return EFI_PCD_TYPE_BOOL;
+      } else {
+        return EFI_PCD_TYPE_8;
+      }
+    case PCD_DATUM_TYPE_UINT16:
+      return EFI_PCD_TYPE_16;
+    case PCD_DATUM_TYPE_UINT32:
+      return EFI_PCD_TYPE_32;
+    case PCD_DATUM_TYPE_UINT64:
+      return EFI_PCD_TYPE_64;
+    default:
+      ASSERT (FALSE);
+      return EFI_PCD_TYPE_8;
+  }
+}
+
+/**
+  Get PCD name.
+
+  @param[in]    OnlyTokenSpaceName  If TRUE, only need to get the TokenSpaceCName.
+                                    If FALSE, need to get the full PCD name.
+  @param[in]    IsPeiDb             If TRUE, the pcd entry is initialized in PEI phase,
+                                    If FALSE, the pcd entry is initialized in DXE phase.
+  @param[in]    TokenNumber         The PCD token number.
+
+  @return       The TokenSpaceCName or full PCD name.
+**/
+CHAR8 *
+GetPcdName (
+  IN BOOLEAN            OnlyTokenSpaceName,
+  IN BOOLEAN            IsPeiDb,
+  IN UINTN              TokenNumber
+  )
+{
+  PCD_DATABASE_INIT *Database;
+  UINT8             *StringTable;
+  UINTN             NameSize;
+  PCD_NAME_INDEX    *PcdNameIndex;
+  CHAR8             *TokenSpaceName;
+  CHAR8             *PcdName;
+  CHAR8             *Name;
+
+  //
+  // Return NULL when PCD name table is absent. 
+  //
+  if (IsPeiDb) {
+    if (mPcdDatabase.PeiDb->PcdNameTableOffset == 0) {
+      return NULL;
+    }
+  } else {
+    if (mPcdDatabase.DxeDb->PcdNameTableOffset == 0) {
+      return NULL;
+    }
+  }
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  Database = IsPeiDb ? mPcdDatabase.PeiDb: mPcdDatabase.DxeDb;
+  TokenNumber = IsPeiDb ? TokenNumber : TokenNumber - mPeiLocalTokenCount;
+
+  StringTable = (UINT8 *) Database + Database->StringTableOffset;
+
+  //
+  // Get the PCD name index.
+  //
+  PcdNameIndex = (PCD_NAME_INDEX *)((UINT8 *) Database + Database->PcdNameTableOffset) + TokenNumber;
+  TokenSpaceName = (CHAR8 *)&StringTable[PcdNameIndex->TokenSpaceCNameIndex];
+  PcdName = (CHAR8 *)&StringTable[PcdNameIndex->PcdCNameIndex];
+
+  if (OnlyTokenSpaceName) {
+    //
+    // Only need to get the TokenSpaceCName.
+    //
+    Name = AllocateCopyPool (AsciiStrSize (TokenSpaceName), TokenSpaceName);
+  } else {
+    //
+    // Need to get the full PCD name.
+    //
+    NameSize = AsciiStrSize (TokenSpaceName) + AsciiStrSize (PcdName);
+    Name = AllocateZeroPool (NameSize);
+    ASSERT (Name != NULL);
+    //
+    // Catenate TokenSpaceCName and PcdCName with a '.' to form the full PCD name.
+    //
+    AsciiStrCatS (Name, NameSize, TokenSpaceName);
+    Name[AsciiStrSize (TokenSpaceName) - sizeof (CHAR8)] = '.';
+    AsciiStrCatS (Name, NameSize, PcdName);  
+  }
+
+  return Name;
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    IsPeiDb     If TRUE, the pcd entry is initialized in PEI phase,
+                            If FALSE, the pcd entry is initialized in DXE phase.
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName. 
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+ExGetPcdInfo (
+  IN        BOOLEAN             IsPeiDb,
+  IN CONST  EFI_GUID            *Guid,
+  IN        UINTN               TokenNumber,
+  OUT       EFI_PCD_INFO        *PcdInfo
+  )
+{
+  PCD_DATABASE_INIT     *Database;
+  UINTN                 GuidTableIdx;
+  EFI_GUID              *MatchGuid;
+  EFI_GUID              *GuidTable;
+  DYNAMICEX_MAPPING     *ExMapTable;
+  UINTN                 Index;
+  UINT32                LocalTokenNumber;
+
+  Database = IsPeiDb ? mPcdDatabase.PeiDb: mPcdDatabase.DxeDb;
+
+  GuidTable = (EFI_GUID *)((UINT8 *)Database + Database->GuidTableOffset);
+  MatchGuid = ScanGuid (GuidTable, Database->GuidTableCount * sizeof(EFI_GUID), Guid);
+
+  if (MatchGuid == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  GuidTableIdx = MatchGuid - GuidTable;
+
+  ExMapTable = (DYNAMICEX_MAPPING *)((UINT8 *)Database + Database->ExMapTableOffset);
+
+  //
+  // Find the PCD by GuidTableIdx and ExTokenNumber in ExMapTable.
+  //
+  for (Index = 0; Index < Database->ExTokenCount; Index++) {
+    if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+      if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+        //
+        // TokenNumber is 0, follow spec to set PcdType to EFI_PCD_TYPE_8,
+        // PcdSize to 0 and PcdName to the null-terminated ASCII string
+        // associated with the token's namespace Guid.
+        //
+        PcdInfo->PcdType = EFI_PCD_TYPE_8;
+        PcdInfo->PcdSize = 0;
+        //
+        // Here use one representative in the token space to get the TokenSpaceCName.
+        // 
+        PcdInfo->PcdName = GetPcdName (TRUE, IsPeiDb, ExMapTable[Index].TokenNumber);
+        return EFI_SUCCESS;
+      } else if (ExMapTable[Index].ExTokenNumber == TokenNumber) {
+        PcdInfo->PcdSize = DxePcdGetSize (ExMapTable[Index].TokenNumber);
+        LocalTokenNumber = GetLocalTokenNumber (IsPeiDb, ExMapTable[Index].TokenNumber);
+        PcdInfo->PcdType = GetPcdType (LocalTokenNumber);
+        PcdInfo->PcdName = GetPcdName (FALSE, IsPeiDb, ExMapTable[Index].TokenNumber);
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+DxeGetPcdInfo (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  EFI_STATUS            Status;
+  BOOLEAN               PeiExMapTableEmpty;
+  BOOLEAN               DxeExMapTableEmpty;
+  UINT32                LocalTokenNumber;
+  BOOLEAN               IsPeiDb;
+
+  ASSERT (PcdInfo != NULL);
+
+  Status = EFI_NOT_FOUND;
+  PeiExMapTableEmpty = mPeiExMapTableEmpty;
+  DxeExMapTableEmpty = mDxeExMapTableEmpty;
+
+  if (Guid == NULL) {
+    if (((TokenNumber + 1 > mPeiNexTokenCount + 1) && (TokenNumber + 1 <= mPeiLocalTokenCount + 1)) ||
+        ((TokenNumber + 1 > (mPeiLocalTokenCount + mDxeNexTokenCount + 1)))) {
+      return EFI_NOT_FOUND;
+    } else if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+      //
+      // TokenNumber is 0, follow spec to set PcdType to EFI_PCD_TYPE_8,
+      // PcdSize to 0 and PcdName to NULL for default Token Space.
+      //
+      PcdInfo->PcdType = EFI_PCD_TYPE_8;
+      PcdInfo->PcdSize = 0;
+      PcdInfo->PcdName = NULL;
+    } else {
+      PcdInfo->PcdSize = DxePcdGetSize (TokenNumber);
+      IsPeiDb = FALSE;
+      if ((TokenNumber + 1 <= mPeiNexTokenCount + 1)) {
+        IsPeiDb = TRUE;
+      }
+      LocalTokenNumber = GetLocalTokenNumber (IsPeiDb, TokenNumber);
+      PcdInfo->PcdType = GetPcdType (LocalTokenNumber);
+      PcdInfo->PcdName = GetPcdName (FALSE, IsPeiDb, TokenNumber);
+    }
+    return EFI_SUCCESS;
+  }
+
+  if (PeiExMapTableEmpty && DxeExMapTableEmpty) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (!PeiExMapTableEmpty) {
+    Status = ExGetPcdInfo (
+               TRUE,
+               Guid,
+               TokenNumber,
+               PcdInfo
+               );
+  }
+
+  if (Status == EFI_SUCCESS) {
+    return Status;
+  }
+
+  if (!DxeExMapTableEmpty) {
+    Status = ExGetPcdInfo (
+               FALSE,
+               Guid,
+               TokenNumber,
+               PcdInfo
+               );
+  }
+
+  return Status;
+}
+
+/**
+  Get the PCD entry pointer in PCD database.
+  
+  This routine will visit PCD database to find the PCD entry according to given
+  token number. The given token number is autogened by build tools and it will be 
+  translated to local token number. Local token number contains PCD's type and 
+  offset of PCD entry in PCD database.
+
+  @param TokenNumber     Token's number, it is autogened by build tools
+  @param GetSize         The size of token's value
+
+  @return PCD entry pointer in PCD database
+
+**/
+VOID *
+GetWorker (
+  IN UINTN             TokenNumber,
+  IN UINTN             GetSize
+  )
+{
+  EFI_GUID            *GuidTable;
+  UINT8               *StringTable;
+  EFI_GUID            *Guid;
+  UINT16              *Name;
+  VARIABLE_HEAD       *VariableHead;
+  UINT8               *VaraiableDefaultBuffer;
+  UINT8               *Data;
+  VPD_HEAD            *VpdHead;
+  UINT8               *PcdDb;
+  VOID                *RetPtr;
+  UINTN               TmpTokenNumber;
+  UINTN               DataSize;
+  EFI_STATUS          Status;
+  UINT32              LocalTokenNumber;
+  UINT32              Offset;
+  STRING_HEAD         StringTableIdx;      
+  BOOLEAN             IsPeiDb;
+
+  //
+  // Aquire lock to prevent reentrance from TPL_CALLBACK level
+  //
+  EfiAcquireLock (&mPcdDatabaseLock);
+
+  RetPtr = NULL;
+
+  ASSERT (TokenNumber > 0);
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  TmpTokenNumber = TokenNumber;
+
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  ASSERT (TokenNumber + 1 < mPcdTotalTokenCount + 1);
+
+  ASSERT ((GetSize == DxePcdGetSize (TokenNumber + 1)) || (GetSize == 0));
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  IsPeiDb = (BOOLEAN) ((TokenNumber + 1 < mPeiLocalTokenCount + 1) ? TRUE : FALSE);
+
+  LocalTokenNumber = GetLocalTokenNumber (IsPeiDb, TokenNumber + 1);
+
+  PcdDb = IsPeiDb ? ((UINT8 *) mPcdDatabase.PeiDb) : ((UINT8 *) mPcdDatabase.DxeDb);
+                                    
+  if (IsPeiDb) {
+    StringTable = (UINT8 *) ((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->StringTableOffset);
+  } else {
+    StringTable = (UINT8 *) ((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->StringTableOffset);
+  }
+
+
+  Offset     = LocalTokenNumber & PCD_DATABASE_OFFSET_MASK;
+
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+      VpdHead = (VPD_HEAD *) ((UINT8 *) PcdDb + Offset);
+      RetPtr = (VOID *) (UINTN) (PcdGet32 (PcdVpdBaseAddress) + VpdHead->Offset);
+
+      break;
+
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+    case PCD_TYPE_HII:
+      if (IsPeiDb) {
+        GuidTable = (EFI_GUID *) ((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->GuidTableOffset);
+      } else {
+        GuidTable = (EFI_GUID *) ((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->GuidTableOffset);
+      }
+
+      VariableHead = (VARIABLE_HEAD *) (PcdDb + Offset);
+      Guid = GuidTable + VariableHead->GuidTableIndex;
+      Name = (UINT16*)(StringTable + VariableHead->StringIndex);
+
+      if ((LocalTokenNumber & PCD_TYPE_ALL_SET) == (PCD_TYPE_HII|PCD_TYPE_STRING)) {
+        //
+        // If a HII type PCD's datum type is VOID*, the DefaultValueOffset is the index of 
+        // string array in string table.
+        //
+        StringTableIdx = *(STRING_HEAD*)((UINT8 *) PcdDb + VariableHead->DefaultValueOffset);   
+        VaraiableDefaultBuffer = (UINT8 *) (StringTable + StringTableIdx);     
+      } else {
+        VaraiableDefaultBuffer = (UINT8 *) PcdDb + VariableHead->DefaultValueOffset;
+      }
+      Status = GetHiiVariable (Guid, Name, &Data, &DataSize);
+      if (Status == EFI_SUCCESS) {
+        if (DataSize >= (VariableHead->Offset + GetSize)) {
+          if (GetSize == 0) {
+            //
+            // It is a pointer type. So get the MaxSize reserved for
+            // this PCD entry.
+            //
+            GetPtrTypeSize (TmpTokenNumber, &GetSize);
+            if (GetSize > (DataSize - VariableHead->Offset)) {
+              //
+              // Use actual valid size.
+              //
+              GetSize = DataSize - VariableHead->Offset;
+            }
+          }
+          //
+          // If the operation is successful, we copy the data
+          // to the default value buffer in the PCD Database.
+          // So that we can free the Data allocated in GetHiiVariable.
+          //
+          CopyMem (VaraiableDefaultBuffer, Data + VariableHead->Offset, GetSize);
+        }
+        FreePool (Data);
+      }
+      RetPtr = (VOID *) VaraiableDefaultBuffer;
+      break;
+
+    case PCD_TYPE_STRING:
+      StringTableIdx = *(STRING_HEAD*)((UINT8 *) PcdDb + Offset);
+      RetPtr = (VOID *) (StringTable + StringTableIdx);
+      break;
+
+    case PCD_TYPE_DATA:
+      RetPtr = (VOID *) ((UINT8 *) PcdDb + Offset);
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+      
+  }
+
+  EfiReleaseLock (&mPcdDatabaseLock);
+
+  return RetPtr;
+
+}
+
+/**
+  Register the callback function for a PCD entry.
+
+  This routine will register a callback function to a PCD entry by given token number
+  and token space guid.
+  
+  @param TokenNumber        PCD token's number, it is autogened by build tools.
+  @param Guid               PCD token space's guid, 
+                            if not NULL, this PCD is dynamicEx type PCD.
+  @param CallBackFunction   Callback function pointer
+
+  @return EFI_SUCCESS Always success for registering callback function.
+
+**/
+EFI_STATUS
+DxeRegisterCallBackWorker (
+  IN  UINTN                   TokenNumber,
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+)
+{
+  CALLBACK_FN_ENTRY       *FnTableEntry;
+  LIST_ENTRY              *ListHead;
+  LIST_ENTRY              *ListNode;
+
+  if (Guid != NULL) {
+    TokenNumber = GetExPcdTokenNumber (Guid, (UINT32) TokenNumber);
+  }
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index of mCallbackFnTable[].
+  //
+  ListHead = &mCallbackFnTable[TokenNumber - 1];
+  ListNode = GetFirstNode (ListHead);
+
+  while (ListNode != ListHead) {
+    FnTableEntry = CR_FNENTRY_FROM_LISTNODE(ListNode, CALLBACK_FN_ENTRY, Node);
+
+    if (FnTableEntry->CallbackFn == CallBackFunction) {
+      //
+      // We only allow a Callback function to be register once
+      // for a TokenNumber. So just return EFI_SUCCESS
+      //
+      return EFI_SUCCESS;
+    }
+    ListNode = GetNextNode (ListHead, ListNode);
+  }
+
+  FnTableEntry = AllocatePool (sizeof(CALLBACK_FN_ENTRY));
+  ASSERT (FnTableEntry != NULL);
+
+  FnTableEntry->CallbackFn = CallBackFunction;
+  InsertTailList (ListHead, &FnTableEntry->Node);
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  UnRegister the callback function for a PCD entry.
+
+  This routine will unregister a callback function to a PCD entry by given token number
+  and token space guid.
+
+  @param TokenNumber        PCD token's number, it is autogened by build tools.
+  @param Guid               PCD token space's guid.
+                            if not NULL, this PCD is dynamicEx type PCD.
+  @param CallBackFunction   Callback function pointer
+
+  @retval EFI_SUCCESS               Callback function is success to be unregister.
+  @retval EFI_INVALID_PARAMETER     Can not find the PCD entry by given token number.
+**/
+EFI_STATUS
+DxeUnRegisterCallBackWorker (
+  IN  UINTN                   TokenNumber,
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+)
+{
+  CALLBACK_FN_ENTRY       *FnTableEntry;
+  LIST_ENTRY              *ListHead;
+  LIST_ENTRY              *ListNode;
+
+  if (Guid != NULL) {
+    TokenNumber = GetExPcdTokenNumber (Guid, (UINT32) TokenNumber);
+  }
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index of mCallbackFnTable[].
+  //
+  ListHead = &mCallbackFnTable[TokenNumber - 1];
+  ListNode = GetFirstNode (ListHead);
+
+  while (ListNode != ListHead) {
+    FnTableEntry = CR_FNENTRY_FROM_LISTNODE(ListNode, CALLBACK_FN_ENTRY, Node);
+
+    if (FnTableEntry->CallbackFn == CallBackFunction) {
+      //
+      // We only allow a Callback function to be register once
+      // for a TokenNumber. So we can safely remove the Node from
+      // the Link List and return EFI_SUCCESS.
+      //
+      RemoveEntryList (ListNode);
+      FreePool (FnTableEntry);
+      
+      return EFI_SUCCESS;
+    }
+    ListNode = GetNextNode (ListHead, ListNode);
+  }
+
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Get next token number in given token space.
+  
+  This routine is used for dynamicEx type PCD. It will firstly scan token space
+  table to get token space according to given token space guid. Then scan given 
+  token number in found token space, if found, then return next token number in 
+  this token space.
+
+  @param Guid            Token space guid. Next token number will be scaned in 
+                         this token space.
+  @param TokenNumber     Token number. 
+                         If PCD_INVALID_TOKEN_NUMBER, return first token number in 
+                         token space table.
+                         If not PCD_INVALID_TOKEN_NUMBER, return next token number
+                         in token space table.
+  @param GuidTable       Token space guid table. It will be used for scan token space
+                         by given token space guid.
+  @param SizeOfGuidTable The size of guid table.
+  @param ExMapTable      DynamicEx token number mapping table.
+  @param SizeOfExMapTable The size of dynamicEx token number mapping table.
+
+  @retval EFI_NOT_FOUND  Can not given token space or token number.
+  @retval EFI_SUCCESS    Success to get next token number.
+
+**/
+EFI_STATUS
+ExGetNextTokeNumber (
+  IN      CONST EFI_GUID         *Guid,
+  IN OUT  UINTN                  *TokenNumber,
+  IN      EFI_GUID               *GuidTable,
+  IN      UINTN                  SizeOfGuidTable,
+  IN      DYNAMICEX_MAPPING      *ExMapTable,
+  IN      UINTN                  SizeOfExMapTable
+  )
+{
+  EFI_GUID         *MatchGuid;
+  UINTN            Index;
+  UINTN            GuidTableIdx;
+  BOOLEAN          Found;
+  UINTN            ExMapTableCount;
+
+  //
+  // Scan token space guid 
+  // 
+  MatchGuid = ScanGuid (GuidTable, SizeOfGuidTable, Guid);
+  if (MatchGuid == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Find the token space table in dynamicEx mapping table.
+  //
+  Found = FALSE;
+  GuidTableIdx = MatchGuid - GuidTable;
+  ExMapTableCount = SizeOfExMapTable / sizeof(ExMapTable[0]);
+  for (Index = 0; Index < ExMapTableCount; Index++) {
+    if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (Found) {
+    //
+    // If given token number is PCD_INVALID_TOKEN_NUMBER, then return the first
+    // token number in found token space.
+    //
+    if (*TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+      *TokenNumber = ExMapTable[Index].ExTokenNumber;
+      return EFI_SUCCESS;
+    }
+
+    for ( ; Index < ExMapTableCount; Index++) {
+      if (ExMapTable[Index].ExTokenNumber == *TokenNumber) {
+        break;
+      }
+    }
+
+    while (Index < ExMapTableCount) {
+      Index++;
+      if (Index == ExMapTableCount) {
+        //
+        // Exceed the length of ExMap Table
+        //
+        *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
+        return EFI_NOT_FOUND;
+      } else if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+        //
+        // Found the next match
+        //
+        *TokenNumber = ExMapTable[Index].ExTokenNumber;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Find the PCD database.
+
+  @retval The base address of external PCD database binary.
+  @retval NULL         Return NULL if not find.
+**/
+DXE_PCD_DATABASE *
+LocateExPcdBinary (
+  VOID
+) 
+{
+  DXE_PCD_DATABASE      *DxePcdDbBinary;
+  UINTN                 DxePcdDbSize;
+  EFI_STATUS            Status;
+ 
+  DxePcdDbBinary = NULL;
+  //
+  // Search the External Pcd database from one section of current FFS, 
+  // and read it to memory
+  //
+  Status = GetSectionFromFfs (
+             EFI_SECTION_RAW,
+             0,
+             (VOID **) &DxePcdDbBinary,
+             &DxePcdDbSize
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Check the first bytes (Header Signature Guid) and build version.
+  //
+  if (!CompareGuid ((VOID *)DxePcdDbBinary, &gPcdDataBaseSignatureGuid) ||
+      (DxePcdDbBinary->BuildVersion != PCD_SERVICE_DXE_VERSION)) {
+    ASSERT (FALSE);
+  }
+
+  return DxePcdDbBinary;
+}
+
+/**
+  Initialize the PCD database in DXE phase.
+  
+  PCD database in DXE phase also contains PCD database in PEI phase which is copied
+  from GUID Hob.
+
+**/
+VOID
+BuildPcdDxeDataBase (
+  VOID
+  )
+{
+  PEI_PCD_DATABASE    *PeiDatabase;
+  EFI_HOB_GUID_TYPE   *GuidHob;
+  UINTN               Index;
+  UINT32              PcdDxeDbLen;
+  VOID                *PcdDxeDb;
+
+  //
+  // Assign PCD Entries with default value to PCD DATABASE
+  //
+  mPcdDatabase.DxeDb = LocateExPcdBinary ();
+  ASSERT(mPcdDatabase.DxeDb != NULL);
+  PcdDxeDbLen = mPcdDatabase.DxeDb->Length + mPcdDatabase.DxeDb->UninitDataBaseSize;
+  PcdDxeDb = AllocateZeroPool (PcdDxeDbLen);
+  ASSERT (PcdDxeDb != NULL);
+  CopyMem (PcdDxeDb, mPcdDatabase.DxeDb, mPcdDatabase.DxeDb->Length);
+  FreePool (mPcdDatabase.DxeDb);
+  mPcdDatabase.DxeDb = PcdDxeDb;
+
+  GuidHob = GetFirstGuidHob (&gPcdDataBaseHobGuid);
+  if (GuidHob != NULL) {
+
+    // 
+    // If no PEIMs use dynamic Pcd Entry, the Pcd Service PEIM
+    // should not be included at all. So the GuidHob could
+    // be NULL. If it is NULL, we just copy over the DXE Default
+    // Value to PCD Database.
+    //
+    
+    PeiDatabase = (PEI_PCD_DATABASE *) GET_GUID_HOB_DATA (GuidHob);
+    //
+    // Assign PCD Entries refereneced in PEI phase to PCD DATABASE
+    //
+    mPcdDatabase.PeiDb = PeiDatabase;
+    //
+    // Inherit the SystemSkuId from PEI phase.
+    //
+    mPcdDatabase.DxeDb->SystemSkuId = mPcdDatabase.PeiDb->SystemSkuId;
+  } else {
+    mPcdDatabase.PeiDb = AllocateZeroPool (sizeof (PEI_PCD_DATABASE));
+    ASSERT(mPcdDatabase.PeiDb != NULL);
+  }
+
+  //
+  // Initialized the external PCD database local variables
+  //
+  mPeiLocalTokenCount     = mPcdDatabase.PeiDb->LocalTokenCount;
+  mDxeLocalTokenCount     = mPcdDatabase.DxeDb->LocalTokenCount;
+
+  mPeiExMapppingTableSize = mPcdDatabase.PeiDb->ExTokenCount * sizeof (DYNAMICEX_MAPPING);
+  mDxeExMapppingTableSize = mPcdDatabase.DxeDb->ExTokenCount * sizeof (DYNAMICEX_MAPPING);
+  mPeiGuidTableSize       = mPcdDatabase.PeiDb->GuidTableCount * sizeof(GUID);
+  mDxeGuidTableSize       = mPcdDatabase.DxeDb->GuidTableCount * sizeof (GUID);
+
+  mPcdTotalTokenCount     = mPeiLocalTokenCount + mDxeLocalTokenCount;
+  mPeiNexTokenCount       = mPeiLocalTokenCount - mPcdDatabase.PeiDb->ExTokenCount;
+  mDxeNexTokenCount       = mDxeLocalTokenCount - mPcdDatabase.DxeDb->ExTokenCount;  
+
+  mPeiExMapTableEmpty     = (mPcdDatabase.PeiDb->ExTokenCount == 0) ? TRUE : FALSE;
+  mDxeExMapTableEmpty     = (mPcdDatabase.DxeDb->ExTokenCount == 0) ? TRUE : FALSE;
+  mPeiDatabaseEmpty       = (mPeiLocalTokenCount == 0) ? TRUE : FALSE;
+
+  TmpTokenSpaceBufferCount = mPcdDatabase.PeiDb->ExTokenCount + mPcdDatabase.DxeDb->ExTokenCount;
+  TmpTokenSpaceBuffer     = (EFI_GUID **)AllocateZeroPool(TmpTokenSpaceBufferCount * sizeof (EFI_GUID *));
+
+  //
+  // Initialized the Callback Function Table
+  //
+  mCallbackFnTable = AllocateZeroPool (mPcdTotalTokenCount * sizeof (LIST_ENTRY));
+  ASSERT(mCallbackFnTable != NULL);
+
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  for (Index = 0; Index + 1 < mPcdTotalTokenCount + 1; Index++) {
+    InitializeListHead (&mCallbackFnTable[Index]);
+  }
+}
+
+/**
+  Get Variable which contains HII type PCD entry.
+
+  @param VariableGuid    Variable's guid
+  @param VariableName    Variable's unicode name string
+  @param VariableData    Variable's data pointer, 
+  @param VariableSize    Variable's size.
+
+  @return the status of gRT->GetVariable
+**/
+EFI_STATUS
+GetHiiVariable (
+  IN  EFI_GUID      *VariableGuid,
+  IN  UINT16        *VariableName,
+  OUT UINT8         **VariableData,
+  OUT UINTN         *VariableSize
+  )
+{
+  UINTN      Size;
+  EFI_STATUS Status;
+  UINT8      *Buffer;
+
+  Size = 0;
+  Buffer = NULL;
+  
+  //
+  // Firstly get the real size of HII variable
+  //
+  Status = gRT->GetVariable (
+    (UINT16 *)VariableName,
+    VariableGuid,
+    NULL,
+    &Size,
+    Buffer
+    );
+  
+  //
+  // Allocate buffer to hold whole variable data according to variable size.
+  //
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    Buffer = (UINT8 *) AllocatePool (Size);
+
+    ASSERT (Buffer != NULL);
+
+    Status = gRT->GetVariable (
+      VariableName,
+      VariableGuid,
+      NULL,
+      &Size,
+      Buffer
+      );
+
+    ASSERT (Status == EFI_SUCCESS);
+    *VariableData = Buffer;
+    *VariableSize = Size;
+  } else {
+    //
+    // Use Default Data only when variable is not found. 
+    // For other error status, correct data can't be got, and trig ASSERT().
+    //
+    ASSERT (Status == EFI_NOT_FOUND);
+  }
+
+  return Status;
+}
+
+/**
+  Find the local token number according to system SKU ID.
+
+  @param LocalTokenNumber PCD token number
+  @param Size             The size of PCD entry.
+  @param IsPeiDb          If TRUE, the PCD entry is initialized in PEI phase.
+                          If False, the PCD entry is initialized in DXE phase.
+
+  @return Token number according to system SKU ID.
+
+**/
+UINT32
+GetSkuEnabledTokenNumber (
+  UINT32    LocalTokenNumber,
+  UINTN     Size,
+  BOOLEAN   IsPeiDb
+  ) 
+{
+  SKU_HEAD              *SkuHead;
+  SKU_ID                *SkuIdTable;
+  INTN                  Index;
+  UINT8                 *Value;
+  UINT8                 *PcdDb;
+  BOOLEAN               FoundSku;
+
+  ASSERT ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0);
+
+  PcdDb = IsPeiDb ? (UINT8 *) mPcdDatabase.PeiDb : (UINT8 *) mPcdDatabase.DxeDb;
+
+  SkuHead     = (SKU_HEAD *) (PcdDb + (LocalTokenNumber & PCD_DATABASE_OFFSET_MASK));
+  Value       = (UINT8 *) (PcdDb + SkuHead->SkuDataStartOffset); 
+
+  SkuIdTable =  (SKU_ID *)(PcdDb + SkuHead->SkuIdTableOffset);
+  //
+  // Find the current system's SKU ID entry in SKU ID table.
+  //
+  FoundSku = FALSE;
+  for (Index = 0; Index < SkuIdTable[0]; Index++) {
+    if (mPcdDatabase.DxeDb->SystemSkuId == SkuIdTable[Index + 1]) {
+      FoundSku = TRUE;
+      break;
+    }
+  }
+  
+  //
+  // Find the default SKU ID entry in SKU ID table.
+  //
+  
+  if(!FoundSku) {
+    for (Index = 0; Index < SkuIdTable[0]; Index++) {
+      if (0 == SkuIdTable[Index + 1]) {
+        break;
+      }
+    }
+  }
+  ASSERT (Index < SkuIdTable[0]);
+
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+      Value = (UINT8 *) &(((VPD_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - PcdDb) | PCD_TYPE_VPD);
+
+    case PCD_TYPE_HII:
+      Value = (UINT8 *) &(((VARIABLE_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - PcdDb) | PCD_TYPE_HII);
+
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+      Value = (UINT8 *) &(((VARIABLE_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - PcdDb) | PCD_TYPE_HII | PCD_TYPE_STRING);
+
+    case PCD_TYPE_STRING:
+      Value = (UINT8 *) &(((STRING_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - PcdDb) | PCD_TYPE_STRING);
+      
+    case PCD_TYPE_DATA:
+      Value += Size * Index;
+      return (UINT32) ((Value - PcdDb) | PCD_TYPE_DATA);
+
+    default:
+      ASSERT (FALSE);
+  }
+
+  ASSERT (FALSE);
+
+  return 0;
+  
+}
+
+/**
+  Invoke the callback function when dynamic PCD entry was set, if this PCD entry 
+  has registered callback function.
+
+  @param ExTokenNumber   DynamicEx PCD's token number, if this PCD entry is dyanmicEx
+                         type PCD.
+  @param Guid            DynamicEx PCD's guid, if this PCD entry is dynamicEx type
+                         PCD.
+  @param TokenNumber     PCD token number generated by build tools.
+  @param Data            Value want to be set for this PCD entry
+  @param Size            The size of value
+
+**/
+VOID
+InvokeCallbackOnSet (
+  UINT32            ExTokenNumber,
+  CONST EFI_GUID    *Guid, OPTIONAL
+  UINTN             TokenNumber,
+  VOID              *Data,
+  UINTN             Size
+  )
+{
+  CALLBACK_FN_ENTRY       *FnTableEntry;
+  LIST_ENTRY              *ListHead;
+  LIST_ENTRY              *ListNode;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index of mCallbackFnTable[].
+  //
+  ListHead = &mCallbackFnTable[TokenNumber - 1];
+  ListNode = GetFirstNode (ListHead);
+
+  while (ListNode != ListHead) {
+    FnTableEntry = CR_FNENTRY_FROM_LISTNODE (ListNode, CALLBACK_FN_ENTRY, Node);
+
+    FnTableEntry->CallbackFn(Guid, 
+                    (Guid == NULL) ? TokenNumber : ExTokenNumber,
+                    Data,
+                    Size);
+    
+    ListNode = GetNextNode (ListHead, ListNode);
+  }
+  
+  return;
+}
+
+
+/**
+  Wrapper function for setting non-pointer type value for a PCD entry.
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+
+  @return status of SetWorker.
+
+**/
+EFI_STATUS
+SetValueWorker (
+  IN UINTN                   TokenNumber,
+  IN VOID                    *Data,
+  IN UINTN                   Size
+  )
+{
+  return SetWorker (TokenNumber, Data, &Size, FALSE);
+}
+
+
+/**
+  Set value for an PCD entry
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+  @param PtrType         If TRUE, the type of PCD entry's value is Pointer.
+                         If False, the type of PCD entry's value is not Pointer.
+
+  @retval EFI_INVALID_PARAMETER  If this PCD type is VPD, VPD PCD can not be set.
+  @retval EFI_INVALID_PARAMETER  If Size can not be set to size table.
+  @retval EFI_INVALID_PARAMETER  If Size of non-Ptr type PCD does not match the size information in PCD database.  
+  @retval EFI_NOT_FOUND          If value type of PCD entry is intergrate, but not in
+                                 range of UINT8, UINT16, UINT32, UINT64
+  @retval EFI_NOT_FOUND          Can not find the PCD type according to token number.                                
+**/
+EFI_STATUS
+SetWorker (
+  IN          UINTN                   TokenNumber,
+  IN          VOID                    *Data,
+  IN OUT      UINTN                   *Size,
+  IN          BOOLEAN                 PtrType
+  )
+{
+  BOOLEAN             IsPeiDb;
+  UINT32              LocalTokenNumber;
+  EFI_GUID            *GuidTable;
+  UINT8               *StringTable;
+  EFI_GUID            *Guid;
+  UINT16              *Name;
+  UINTN               VariableOffset;
+  UINT32              Attributes;
+  VOID                *InternalData;
+  VARIABLE_HEAD       *VariableHead;
+  UINTN               Offset;
+  UINT8               *PcdDb;
+  EFI_STATUS          Status;
+  UINTN               MaxSize;
+  UINTN               TmpTokenNumber;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  TmpTokenNumber = TokenNumber;
+  
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  ASSERT (TokenNumber + 1 < mPcdTotalTokenCount + 1);
+
+  if (PtrType) {
+    //
+    // Get MaxSize first, then check new size with max buffer size.
+    //
+    GetPtrTypeSize (TokenNumber, &MaxSize);
+    if (*Size > MaxSize) {
+      *Size = MaxSize;
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    if (*Size != DxePcdGetSize (TokenNumber + 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  if ((TokenNumber + 1 < mPeiNexTokenCount + 1) ||
+      (TokenNumber + 1 >= mPeiLocalTokenCount + 1 && TokenNumber + 1 < (mPeiLocalTokenCount + mDxeNexTokenCount + 1))) {
+    InvokeCallbackOnSet (0, NULL, TokenNumber + 1, Data, *Size);
+  }
+
+  //
+  // Aquire lock to prevent reentrance from TPL_CALLBACK level
+  //
+  EfiAcquireLock (&mPcdDatabaseLock);
+
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  IsPeiDb = (BOOLEAN) ((TokenNumber + 1 < mPeiLocalTokenCount + 1) ? TRUE : FALSE);
+
+  LocalTokenNumber = GetLocalTokenNumber (IsPeiDb, TokenNumber + 1);
+
+  Offset = LocalTokenNumber & PCD_DATABASE_OFFSET_MASK;
+
+  PcdDb = IsPeiDb ? ((UINT8 *) mPcdDatabase.PeiDb) : ((UINT8 *) mPcdDatabase.DxeDb);
+
+  if (IsPeiDb) {
+    StringTable = (UINT8 *) ((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->StringTableOffset);
+  } else {
+    StringTable = (UINT8 *) ((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->StringTableOffset);
+  }
+
+  
+  InternalData = PcdDb + Offset;
+
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+      ASSERT (FALSE);
+      Status = EFI_INVALID_PARAMETER;
+      break;
+    
+    case PCD_TYPE_STRING:
+      if (SetPtrTypeSize (TmpTokenNumber, Size)) {
+        CopyMem (StringTable + *((STRING_HEAD *)InternalData), Data, *Size);
+        Status = EFI_SUCCESS;
+      } else {
+        Status = EFI_INVALID_PARAMETER;
+      }
+      break;
+
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+    case PCD_TYPE_HII:
+      if (PtrType) {
+        if (!SetPtrTypeSize (TmpTokenNumber, Size)) {
+          Status = EFI_INVALID_PARAMETER;
+          break;
+        }
+      }
+
+      if (IsPeiDb) {
+        GuidTable = (EFI_GUID *) ((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->GuidTableOffset);
+      } else {
+        GuidTable = (EFI_GUID *) ((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->GuidTableOffset);
+      }
+
+      VariableHead = (VARIABLE_HEAD *) (PcdDb + Offset);
+
+      Guid = GuidTable + VariableHead->GuidTableIndex;
+      Name = (UINT16*) (StringTable + VariableHead->StringIndex);
+      VariableOffset = VariableHead->Offset;
+      Attributes = VariableHead->Attributes;
+      Status = SetHiiVariable (Guid, Name, Attributes, Data, *Size, VariableOffset);
+      break;
+      
+    case PCD_TYPE_DATA:
+      if (PtrType) {
+        if (SetPtrTypeSize (TmpTokenNumber, Size)) {
+          CopyMem (InternalData, Data, *Size);
+          Status = EFI_SUCCESS;
+        } else {
+          Status = EFI_INVALID_PARAMETER;
+        }
+        break;
+      }
+
+      Status = EFI_SUCCESS;
+      switch (*Size) {
+        case sizeof(UINT8):
+          *((UINT8 *) InternalData) = *((UINT8 *) Data);
+          break;
+
+        case sizeof(UINT16):
+          *((UINT16 *) InternalData) = *((UINT16 *) Data);
+          break;
+
+        case sizeof(UINT32):
+          *((UINT32 *) InternalData) = *((UINT32 *) Data);
+          break;
+
+        case sizeof(UINT64):
+          *((UINT64 *) InternalData) = *((UINT64 *) Data);
+          break;
+
+        default:
+          ASSERT (FALSE);
+          Status = EFI_NOT_FOUND;
+          break;
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      Status = EFI_NOT_FOUND;
+      break;
+    }
+
+  EfiReleaseLock (&mPcdDatabaseLock);
+  
+  return Status;
+}
+
+/**
+  Wrapper function for get PCD value for dynamic-ex PCD.
+
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param GetSize         The size of dynamic-ex PCD value.
+
+  @return PCD entry in PCD database.
+
+**/
+VOID *
+ExGetWorker (
+  IN CONST EFI_GUID         *Guid,
+  IN UINTN                  ExTokenNumber,
+  IN UINTN                  GetSize
+  ) 
+{
+  return GetWorker(GetExPcdTokenNumber (Guid, (UINT32) ExTokenNumber), GetSize);
+}
+
+/**
+  Wrapper function for set PCD value for non-Pointer type dynamic-ex PCD.
+
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            Value want to be set.
+  @param SetSize         The size of value.
+
+  @return status of ExSetWorker().
+
+**/
+EFI_STATUS
+ExSetValueWorker (
+  IN          UINTN                ExTokenNumber,
+  IN          CONST EFI_GUID       *Guid,
+  IN          VOID                 *Data,
+  IN          UINTN                SetSize
+  )
+{
+  return ExSetWorker (ExTokenNumber, Guid, Data, &SetSize, FALSE);
+}
+
+/**
+  Set value for a dynamic-ex PCD entry.
+  
+  This routine find the local token number according to dynamic-ex PCD's token 
+  space guid and token number firstly, and invoke callback function if this PCD
+  entry registered callback function. Finally, invoken general SetWorker to set
+  PCD value.
+  
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            PCD value want to be set
+  @param SetSize         Size of value.
+  @param PtrType         If TRUE, this PCD entry is pointer type.
+                         If FALSE, this PCD entry is not pointer type.
+
+  @return status of SetWorker().
+
+**/
+EFI_STATUS
+ExSetWorker (
+  IN          UINTN                ExTokenNumber,
+  IN          CONST EFI_GUID       *Guid,
+  IN          VOID                 *Data,
+  IN OUT      UINTN                *SetSize,
+  IN          BOOLEAN              PtrType
+  )
+{
+  UINTN                   TokenNumber;
+  
+  TokenNumber = GetExPcdTokenNumber (Guid, (UINT32) ExTokenNumber);
+
+  InvokeCallbackOnSet ((UINT32) ExTokenNumber, Guid, TokenNumber, Data, *SetSize);
+
+  return SetWorker (TokenNumber, Data, SetSize, PtrType);
+
+}
+
+/**
+  Set value for HII-type PCD.
+
+  A HII-type PCD's value is stored in a variable. Setting/Getting the value of 
+  HII-type PCD is to visit this variable.
+  
+  @param VariableGuid    Guid of variable which stored value of a HII-type PCD.
+  @param VariableName    Unicode name of variable which stored value of a HII-type PCD.
+  @param SetAttributes   Attributes bitmask to set for the variable.
+  @param Data            Value want to be set.
+  @param DataSize        Size of value
+  @param Offset          Value offset of HII-type PCD in variable.
+
+  @return status of GetVariable()/SetVariable().
+
+**/
+EFI_STATUS
+SetHiiVariable (
+  IN  EFI_GUID     *VariableGuid,
+  IN  UINT16       *VariableName,
+  IN  UINT32       SetAttributes,
+  IN  CONST VOID   *Data,
+  IN  UINTN        DataSize,
+  IN  UINTN        Offset
+  )
+{
+  UINTN       Size;
+  VOID        *Buffer;
+  EFI_STATUS  Status;
+  UINT32      Attribute;
+  UINTN       SetSize;
+
+  Size = 0;
+  SetSize = 0;
+
+  //
+  // Try to get original variable size information.
+  //
+  Status = gRT->GetVariable (
+    (UINT16 *)VariableName,
+    VariableGuid,
+    NULL,
+    &Size,
+    NULL
+    );
+  
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // Patch new PCD's value to offset in given HII variable.
+    //
+    if (Size >= (DataSize + Offset)) {
+      SetSize = Size;
+    } else {
+      SetSize = DataSize + Offset;
+    }
+    Buffer = AllocatePool (SetSize);
+    ASSERT (Buffer != NULL);
+
+    Status = gRT->GetVariable (
+      VariableName,
+      VariableGuid,
+      &Attribute,
+      &Size,
+      Buffer
+      );
+    
+    ASSERT_EFI_ERROR (Status);
+
+    CopyMem ((UINT8 *)Buffer + Offset, Data, DataSize);
+
+    if (SetAttributes == 0) {
+      SetAttributes = Attribute;
+    }
+
+    Status = gRT->SetVariable (
+              VariableName,
+              VariableGuid,
+              SetAttributes,
+              SetSize,
+              Buffer
+              );
+
+    FreePool (Buffer);
+    return Status;
+  } else if (Status == EFI_NOT_FOUND) {
+    //
+    // If variable does not exist, a new variable need to be created.
+    //
+    
+    Size = Offset + DataSize;
+    
+    Buffer = AllocateZeroPool (Size);
+    ASSERT (Buffer != NULL);
+    
+    CopyMem ((UINT8 *)Buffer + Offset, Data, DataSize);
+
+    if (SetAttributes == 0) {
+      SetAttributes = EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_NON_VOLATILE;
+    }
+
+    Status = gRT->SetVariable (
+              VariableName,
+              VariableGuid,
+              SetAttributes,
+              Size,
+              Buffer
+              );
+
+    FreePool (Buffer);
+    return Status;    
+  }
+  
+  //
+  // If we drop to here, the value is failed to be written in to variable area.
+  //
+  return Status;
+}
+
+/**
+  Get Token Number according to dynamic-ex PCD's {token space guid:token number}
+
+  A dynamic-ex type PCD, developer must provide pair of token space guid: token number
+  in DEC file. PCD database maintain a mapping table that translate pair of {token
+  space guid: token number} to Token Number.
+  
+  @param Guid            Token space guid for dynamic-ex PCD entry.
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+
+  @return Token Number for dynamic-ex PCD.
+
+**/
+UINTN           
+GetExPcdTokenNumber (
+  IN CONST EFI_GUID             *Guid,
+  IN UINT32                     ExTokenNumber
+  )
+{
+  UINT32              Index;
+  DYNAMICEX_MAPPING   *ExMap;
+  EFI_GUID            *GuidTable;
+  EFI_GUID            *MatchGuid;
+  UINTN               MatchGuidIdx;
+
+  if (!mPeiDatabaseEmpty) {
+    ExMap       = (DYNAMICEX_MAPPING *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->ExMapTableOffset);
+    GuidTable   = (EFI_GUID *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->GuidTableOffset);
+
+    MatchGuid   = ScanGuid (GuidTable, mPeiGuidTableSize, Guid);
+
+    if (MatchGuid != NULL) {
+
+      MatchGuidIdx = MatchGuid - GuidTable;
+
+      for (Index = 0; Index < mPcdDatabase.PeiDb->ExTokenCount; Index++) {
+        if ((ExTokenNumber == ExMap[Index].ExTokenNumber) &&
+            (MatchGuidIdx == ExMap[Index].ExGuidIndex)) {
+            return ExMap[Index].TokenNumber;
+        }
+      }
+    }
+  }
+
+  ExMap       = (DYNAMICEX_MAPPING *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->ExMapTableOffset);
+  GuidTable   = (EFI_GUID *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->GuidTableOffset);
+
+  MatchGuid   = ScanGuid (GuidTable, mDxeGuidTableSize, Guid);
+  //
+  // We need to ASSERT here. If GUID can't be found in GuidTable, this is a
+  // error in the BUILD system.
+  //
+  ASSERT (MatchGuid != NULL);
+
+  MatchGuidIdx = MatchGuid - GuidTable;
+
+  for (Index = 0; Index < mPcdDatabase.DxeDb->ExTokenCount; Index++) {
+    if ((ExTokenNumber == ExMap[Index].ExTokenNumber) &&
+         (MatchGuidIdx == ExMap[Index].ExGuidIndex)) {
+        return ExMap[Index].TokenNumber;
+    }
+  }
+
+  ASSERT (FALSE);
+
+  return 0;
+}
+
+/**
+  Get SKU ID table from PCD database.
+
+  @param LocalTokenNumberTableIdx Index of local token number in token number table.
+  @param IsPeiDb                  If TRUE, the pcd entry is initialized in PEI phase,
+                                  If FALSE, the pcd entry is initialized in DXE phase.
+  @return Pointer to SKU ID array table
+
+**/
+SKU_ID *
+GetSkuIdArray (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    BOOLEAN           IsPeiDb
+  )
+{
+  SKU_HEAD  *SkuHead;
+  UINTN     LocalTokenNumber;
+  UINT8     *Database;
+
+  if (IsPeiDb) {
+    LocalTokenNumber = *((UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset) + LocalTokenNumberTableIdx);
+    Database         = (UINT8 *) mPcdDatabase.PeiDb;
+  } else {
+    LocalTokenNumber = *((UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset) + LocalTokenNumberTableIdx);
+    Database         = (UINT8 *) mPcdDatabase.DxeDb;
+  }
+
+  ASSERT ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) != 0);
+
+  SkuHead = (SKU_HEAD *) ((UINT8 *)Database + (LocalTokenNumber & PCD_DATABASE_OFFSET_MASK));
+
+  return (SKU_ID *) (Database + SkuHead->SkuIdTableOffset);
+  
+}
+
+/**
+  Wrapper function of getting index of PCD entry in size table.
+  
+  @param LocalTokenNumberTableIdx Index of this PCD in local token number table.
+  @param IsPeiDb                  If TRUE, the pcd entry is initialized in PEI phase,
+                                  If FALSE, the pcd entry is initialized in DXE phase.
+
+  @return index of PCD entry in size table.
+**/
+UINTN
+GetSizeTableIndex (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    BOOLEAN           IsPeiDb
+  )
+{
+  UINT32 *LocalTokenNumberTable;
+  UINTN  LocalTokenNumber;
+  UINTN  Index;
+  UINTN  SizeTableIdx;
+  SKU_ID *SkuIdTable;
+  
+  if (IsPeiDb) {
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset);
+  } else {
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset);
+  }
+
+  SizeTableIdx = 0;
+
+  for (Index = 0; Index < LocalTokenNumberTableIdx; Index ++) {
+    LocalTokenNumber = LocalTokenNumberTable[Index];
+
+    if ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER) {
+      //
+      // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+      // PCD entry.
+      //
+      if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+          //
+          // We have only two entry for VPD enabled PCD entry:
+          // 1) MAX Size.
+          // 2) Current Size
+          // Current size is equal to MAX size.
+          //
+          SizeTableIdx += 2;
+      } else {
+        if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+          //
+          // We have only two entry for Non-Sku enabled PCD entry:
+          // 1) MAX SIZE
+          // 2) Current Size
+          //
+          SizeTableIdx += 2;
+        } else {
+          //
+          // We have these entry for SKU enabled PCD entry
+          // 1) MAX SIZE
+          // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+          //
+          SkuIdTable = GetSkuIdArray (Index, IsPeiDb);
+          SizeTableIdx += (UINTN)*SkuIdTable + 1;
+        }
+      }
+    }
+
+  }
+
+  return SizeTableIdx;  
+}
+
+/**
+  Get size of POINTER type PCD value.
+
+  @param LocalTokenNumberTableIdx Index of local token number in local token number table.
+  @param MaxSize                  Maxmium size of POINTER type PCD value.
+
+  @return size of POINTER type PCD value.
+
+**/
+UINTN
+GetPtrTypeSize (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  OUT   UINTN             *MaxSize
+  )
+{
+  INTN        SizeTableIdx;
+  UINTN       LocalTokenNumber;
+  SKU_ID      *SkuIdTable;
+  SIZE_INFO   *SizeTable;
+  UINTN       Index;
+  BOOLEAN     IsPeiDb;
+  UINT32      *LocalTokenNumberTable;
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  IsPeiDb = (BOOLEAN) (LocalTokenNumberTableIdx + 1 < mPeiLocalTokenCount + 1);
+
+
+  if (IsPeiDb) {
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset);
+    SizeTable = (SIZE_INFO *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->SizeTableOffset);
+  } else {
+    LocalTokenNumberTableIdx -= mPeiLocalTokenCount;
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset);
+    SizeTable = (SIZE_INFO *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->SizeTableOffset);
+  }
+
+  LocalTokenNumber = LocalTokenNumberTable[LocalTokenNumberTableIdx];
+
+  ASSERT ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER);
+  
+  SizeTableIdx = GetSizeTableIndex (LocalTokenNumberTableIdx, IsPeiDb);
+
+  *MaxSize = SizeTable[SizeTableIdx];
+  //
+  // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+  // PCD entry.
+  //
+  if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+      //
+      // We have only two entry for VPD enabled PCD entry:
+      // 1) MAX Size.
+      // 2) Current Size
+      // We consider current size is equal to MAX size.
+      //
+      return *MaxSize;
+  } else {
+    if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+      //
+      // We have only two entry for Non-Sku enabled PCD entry:
+      // 1) MAX SIZE
+      // 2) Current Size
+      //
+      return SizeTable[SizeTableIdx + 1];
+    } else {
+      //
+      // We have these entry for SKU enabled PCD entry
+      // 1) MAX SIZE
+      // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+      //
+      SkuIdTable = GetSkuIdArray (LocalTokenNumberTableIdx, IsPeiDb);
+      for (Index = 0; Index < SkuIdTable[0]; Index++) {
+        if (SkuIdTable[1 + Index] == mPcdDatabase.DxeDb->SystemSkuId) {
+          return SizeTable[SizeTableIdx + 1 + Index];
+        }
+      }
+      return SizeTable[SizeTableIdx + 1];
+    }
+  }
+}
+
+/**
+  Set size of POINTER type PCD value. The size should not exceed the maximum size
+  of this PCD value.
+
+  @param LocalTokenNumberTableIdx Index of local token number in local token number table.
+  @param CurrentSize              Size of POINTER type PCD value.
+
+  @retval TRUE  Success to set size of PCD value.
+  @retval FALSE Fail to set size of PCD value.
+**/
+BOOLEAN
+SetPtrTypeSize (
+  IN          UINTN             LocalTokenNumberTableIdx,
+  IN    OUT   UINTN             *CurrentSize
+  )
+{
+  INTN        SizeTableIdx;
+  UINTN       LocalTokenNumber;
+  SKU_ID      *SkuIdTable;
+  SIZE_INFO   *SizeTable;
+  UINTN       Index;
+  UINTN       MaxSize;
+  BOOLEAN     IsPeiDb;
+  UINT32      *LocalTokenNumberTable;
+
+  //
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  //
+  IsPeiDb = (BOOLEAN) (LocalTokenNumberTableIdx + 1 < mPeiLocalTokenCount + 1);
+
+  if (IsPeiDb) {
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->LocalTokenNumberTableOffset);
+    SizeTable = (SIZE_INFO *)((UINT8 *)mPcdDatabase.PeiDb + mPcdDatabase.PeiDb->SizeTableOffset);
+  } else {
+    LocalTokenNumberTableIdx -= mPeiLocalTokenCount;
+    LocalTokenNumberTable = (UINT32 *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->LocalTokenNumberTableOffset);
+    SizeTable = (SIZE_INFO *)((UINT8 *)mPcdDatabase.DxeDb + mPcdDatabase.DxeDb->SizeTableOffset);
+  }
+
+  LocalTokenNumber = LocalTokenNumberTable[LocalTokenNumberTableIdx];
+
+  ASSERT ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER);
+  
+  SizeTableIdx = GetSizeTableIndex (LocalTokenNumberTableIdx, IsPeiDb);
+
+  MaxSize = SizeTable[SizeTableIdx];
+  //
+  // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+  // PCD entry.
+  //
+  if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+      //
+      // We shouldn't come here as we don't support SET for VPD
+      //
+      ASSERT (FALSE);
+      return FALSE;
+  } else {
+    if ((*CurrentSize > MaxSize) ||
+      (*CurrentSize == MAX_ADDRESS)) {
+       *CurrentSize = MaxSize;
+       return FALSE;
+    } 
+    
+    if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+      //
+      // We have only two entry for Non-Sku enabled PCD entry:
+      // 1) MAX SIZE
+      // 2) Current Size
+      //
+      SizeTable[SizeTableIdx + 1] = (SIZE_INFO) *CurrentSize;
+      return TRUE;
+    } else {
+      //
+      // We have these entry for SKU enabled PCD entry
+      // 1) MAX SIZE
+      // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+      //
+      SkuIdTable = GetSkuIdArray (LocalTokenNumberTableIdx, IsPeiDb);
+      for (Index = 0; Index < SkuIdTable[0]; Index++) {
+        if (SkuIdTable[1 + Index] == mPcdDatabase.DxeDb->SystemSkuId) {
+          SizeTable[SizeTableIdx + 1 + Index] = (SIZE_INFO) *CurrentSize;
+          return TRUE;
+        }
+      }
+      SizeTable[SizeTableIdx + 1] = (SIZE_INFO) *CurrentSize;
+      return TRUE;
+    }
+  }
+}
+
+/**
+  VariableLock DynamicHiiPcd.
+
+  @param[in] IsPeiDb        If TRUE, the pcd entry is initialized in PEI phase,
+                            If FALSE, the pcd entry is initialized in DXE phase.
+  @param[in] VariableLock   Pointer to VariableLockProtocol.
+
+**/
+VOID
+VariableLockDynamicHiiPcd (
+  IN BOOLEAN                        IsPeiDb,
+  IN EDKII_VARIABLE_LOCK_PROTOCOL   *VariableLock
+  )
+{
+  EFI_STATUS                Status;
+  PCD_DATABASE_INIT         *Database;
+  UINT32                    LocalTokenCount; 
+  UINTN                     TokenNumber;
+  UINT32                    LocalTokenNumber;
+  UINTN                     Offset;
+  EFI_GUID                  *GuidTable;
+  UINT8                     *StringTable;
+  VARIABLE_HEAD             *VariableHead;
+  EFI_GUID                  *Guid;
+  UINT16                    *Name;
+
+  Database = IsPeiDb ? mPcdDatabase.PeiDb: mPcdDatabase.DxeDb;
+  LocalTokenCount = IsPeiDb ? mPeiLocalTokenCount: mDxeLocalTokenCount;
+
+  //
+  // Go through PCD database to find out DynamicHii PCDs.
+  //
+  for (TokenNumber = 1; TokenNumber <= LocalTokenCount; TokenNumber++) {
+    if (IsPeiDb) {
+      LocalTokenNumber = GetLocalTokenNumber (TRUE, TokenNumber);
+    } else {
+      LocalTokenNumber = GetLocalTokenNumber (FALSE, TokenNumber + mPeiLocalTokenCount);
+    }
+    if ((LocalTokenNumber & PCD_TYPE_HII) != 0) {
+      Offset = LocalTokenNumber & PCD_DATABASE_OFFSET_MASK;
+      VariableHead = (VARIABLE_HEAD *) ((UINT8 *) Database + Offset);
+      //
+      // Why not to set property by VarCheckProtocol with Attributes and Property directly here?
+      // It is because that set property by VarCheckProtocol will indicate the variable to
+      // be a system variable, but the unknown max size of the variable is dangerous to
+      // the system variable region.
+      //
+      if ((VariableHead->Property & VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY) != 0) {
+        //
+        // DynamicHii PCD with RO property set in *.dsc.
+        //
+        StringTable = (UINT8 *) ((UINT8 *) Database + Database->StringTableOffset);
+        GuidTable = (EFI_GUID *) ((UINT8 *) Database + Database->GuidTableOffset);
+        Guid = GuidTable + VariableHead->GuidTableIndex;
+        Name = (UINT16*) (StringTable + VariableHead->StringIndex);
+        Status = VariableLock->RequestToLock (VariableLock, Name, Guid);
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+  }
+}
+
+/**
+  VariableLockProtocol callback
+  to lock the variables referenced by DynamicHii PCDs with RO property set in *.dsc.
+
+  @param[in] Event      Event whose notification function is being invoked.
+  @param[in] Context    Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableLockCallBack (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  )
+{
+  EFI_STATUS                    Status;
+  EDKII_VARIABLE_LOCK_PROTOCOL  *VariableLock;
+
+  Status = gBS->LocateProtocol (&gEdkiiVariableLockProtocolGuid, NULL, (VOID **) &VariableLock);
+  if (!EFI_ERROR (Status)) {
+    VariableLockDynamicHiiPcd (TRUE, VariableLock);
+    VariableLockDynamicHiiPcd (FALSE, VariableLock);
+  }
+}
+
diff --git a/Core/MdeModulePkg/Universal/PCD/Dxe/Service.h b/Core/MdeModulePkg/Universal/PCD/Dxe/Service.h
new file mode 100644
index 0000000000..4d8ab0f13f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Dxe/Service.h
@@ -0,0 +1,1202 @@
+/** @file
+Private functions used by PCD DXE driver.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PCD_DXE_SERVICE_H_
+#define _PCD_DXE_SERVICE_H_
+
+#include <PiDxe.h>
+#include <Guid/PcdDataBaseHobGuid.h>
+#include <Guid/PcdDataBaseSignatureGuid.h>
+#include <Protocol/Pcd.h>
+#include <Protocol/PiPcd.h>
+#include <Protocol/PcdInfo.h>
+#include <Protocol/PiPcdInfo.h>
+#include <Protocol/VarCheck.h>
+#include <Protocol/VariableLock.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+
+//
+// Please make sure the PCD Serivce DXE Version is consistent with
+// the version of the generated DXE PCD Database by build tool.
+//
+#define PCD_SERVICE_DXE_VERSION      5
+
+//
+// PCD_DXE_SERVICE_DRIVER_VERSION is defined in Autogen.h.
+//
+#if (PCD_SERVICE_DXE_VERSION != PCD_DXE_SERVICE_DRIVER_VERSION)
+  #error "Please make sure the version of PCD DXE Service and the generated PCD DXE Database match."
+#endif
+
+/**
+  Retrieve additional information associated with a PCD token in the default token space.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+DxeGetPcdInfoGetInfo (
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+DxeGetPcdInfoGetInfoEx (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/**
+  Retrieve the currently set SKU Id.
+
+  @return   The currently set SKU Id. If the platform has not set at a SKU Id, then the
+            default SKU Id value of 0 is returned. If the platform has set a SKU Id, then the currently set SKU
+            Id is returned.
+**/
+UINTN
+EFIAPI
+DxeGetPcdInfoGetSku (
+  VOID
+  );
+
+//
+// Protocol Interface function declaration.
+//
+/**
+  Sets the SKU value for subsequent calls to set or get PCD token values.
+
+  SetSku() sets the SKU Id to be used for subsequent calls to set or get PCD values. 
+  SetSku() is normally called only once by the system.
+
+  For each item (token), the database can hold a single value that applies to all SKUs, 
+  or multiple values, where each value is associated with a specific SKU Id. Items with multiple, 
+  SKU-specific values are called SKU enabled. 
+  
+  The SKU Id of zero is reserved as a default. The valid SkuId range is 1 to 255.  
+  For tokens that are not SKU enabled, the system ignores any set SKU Id and works with the 
+  single value for that token. For SKU-enabled tokens, the system will use the SKU Id set by the 
+  last call to SetSku(). If no SKU Id is set or the currently set SKU Id isn't valid for the specified token, 
+  the system uses the default SKU Id. If the system attempts to use the default SKU Id and no value has been 
+  set for that Id, the results are unpredictable.
+
+  @param[in]  SkuId The SKU value that will be used when the PCD service will retrieve and 
+              set values associated with a PCD token.
+
+**/
+VOID
+EFIAPI
+DxePcdSetSku (
+  IN  UINTN                  SkuId
+  );
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the current byte-sized value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT8 value.
+  
+**/
+UINT8
+EFIAPI
+DxePcdGet8 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the current 16-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT16 value.
+  
+**/
+UINT16
+EFIAPI
+DxePcdGet16 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the current 32-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT32 value.
+  
+**/
+UINT32
+EFIAPI
+DxePcdGet32 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the current 64-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT64 value.
+  
+**/
+UINT64
+EFIAPI
+DxePcdGet64 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrieved.
+  
+**/
+VOID *
+EFIAPI
+DxePcdGetPtr (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a Boolean value for a given PCD token.
+
+  Retrieves the current boolean value for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The Boolean value.
+  
+**/
+BOOLEAN
+EFIAPI
+DxePcdGetBool (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+DxePcdGetSize (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the 8-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 8-bit value for the PCD token.
+  
+**/
+UINT8
+EFIAPI
+DxePcdGet8Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the 16-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 16-bit value for the PCD token.
+  
+**/
+UINT16
+EFIAPI
+DxePcdGet16Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the 32-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 32-bit value for the PCD token.
+  
+**/
+UINT32
+EFIAPI
+DxePcdGet32Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the 64-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 64-bit value for the PCD token.
+  
+**/
+UINT64
+EFIAPI
+DxePcdGet64Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrieved.
+  
+**/
+VOID *
+EFIAPI
+DxePcdGetPtrEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an Boolean value for a given PCD token.
+
+  Retrieves the Boolean value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size Boolean value for the PCD token.
+  
+**/
+BOOLEAN
+EFIAPI
+DxePcdGetBoolEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+DxePcdGetSizeEx (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet8 (
+  IN UINTN             TokenNumber,
+  IN UINT8             Value
+  );
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet16 (
+  IN UINTN             TokenNumber,
+  IN UINT16             Value
+  );
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet32 (
+  IN UINTN             TokenNumber,
+  IN UINT32             Value
+  );
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet64 (
+  IN UINTN             TokenNumber,
+  IN UINT64            Value
+  );
+
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetPtr (
+  IN        UINTN             TokenNumber,
+  IN OUT    UINTN             *SizeOfBuffer,
+  IN        VOID              *Buffer
+  );
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetBool (
+  IN UINTN             TokenNumber,
+  IN BOOLEAN           Value
+  );
+
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet8Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT8             Value
+  );
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet16Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT16            Value
+  );
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet32Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT32             Value
+  );
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSet64Ex (
+  IN CONST EFI_GUID        *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT64            Value
+  );
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetPtrEx (
+  IN        CONST EFI_GUID    *Guid,
+  IN        UINTN             TokenNumber,
+  IN OUT    UINTN             *SizeOfBuffer,
+  IN        VOID              *Buffer
+  );
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+DxePcdSetBoolEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN BOOLEAN           Value
+  );
+
+/**
+  Specifies a function to be called anytime the value of a designated token is changed.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  );
+
+/**
+  Cancels a previously set callback function for a particular PCD token number.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+DxeUnRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  );
+
+/**
+  Retrieves the next valid token number in a given namespace.  
+  
+  This is useful since the PCD infrastructure contains a sparse list of token numbers, 
+  and one cannot a priori know what token numbers are valid in the database. 
+  
+  If TokenNumber is 0 and Guid is not NULL, then the first token from the token space specified by Guid is returned.  
+  If TokenNumber is not 0 and Guid is not NULL, then the next token in the token space specified by Guid is returned.  
+  If TokenNumber is 0 and Guid is NULL, then the first token in the default token space is returned.  
+  If TokenNumber is not 0 and Guid is NULL, then the next token in the default token space is returned.  
+  The token numbers in the default token space may not be related to token numbers in token spaces that are named by Guid.  
+  If the next token number can be retrieved, then it is returned in TokenNumber, and EFI_SUCCESS is returned.  
+  If TokenNumber represents the last token number in the token space specified by Guid, then EFI_NOT_FOUND is returned.  
+  If TokenNumber is not present in the token space specified by Guid, then EFI_NOT_FOUND is returned.
+
+
+  @param[in]      Guid    The 128-bit unique value that designates the namespace from which to retrieve the next token. 
+                          This is an optional parameter that may be NULL.  If this parameter is NULL, then a request is 
+                          being made to retrieve tokens from the default token space.
+  @param[in, out] TokenNumber 
+                          A pointer to the PCD token number to use to find the subsequent token number.  
+
+  @retval EFI_SUCCESS   The PCD service retrieved the next valid token number. Or the input token number 
+                        is already the last valid token number in the PCD database. 
+                        In the later case, *TokenNumber is updated with the value of 0.
+  @retval EFI_NOT_FOUND If this input token number and token namespace does not exist on the platform.
+
+**/
+EFI_STATUS
+EFIAPI
+DxePcdGetNextToken (
+  IN CONST EFI_GUID               *Guid, OPTIONAL
+  IN OUT   UINTN                  *TokenNumber
+  );
+
+/**
+  Get next token space in PCD database according to given token space guid.
+  
+  @param Guid            Given token space guid. If NULL, then Guid will be set to 
+                         the first PCD token space in PCD database, If not NULL, then
+                         Guid will be set to next PCD token space.
+
+  @retval EFI_NOT_FOUND   If PCD database has no token space table or can not find given
+                          token space in PCD database.
+  @retval EFI_SUCCESS     Success to get next token space guid.
+**/
+EFI_STATUS
+EFIAPI
+DxePcdGetNextTokenSpace (
+  IN OUT CONST EFI_GUID               **Guid
+  );
+
+typedef struct {
+  LIST_ENTRY              Node;
+  PCD_PROTOCOL_CALLBACK   CallbackFn;
+} CALLBACK_FN_ENTRY;
+
+#define CR_FNENTRY_FROM_LISTNODE(Record, Type, Field) BASE_CR(Record, Type, Field)
+
+//
+// Internal Functions
+//
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName. 
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+DxeGetPcdInfo (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/**
+  Wrapper function for setting non-pointer type value for a PCD entry.
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+
+  @return status of SetWorker.
+
+**/
+EFI_STATUS
+SetValueWorker (
+  IN UINTN                   TokenNumber,
+  IN VOID                    *Data,
+  IN UINTN                   Size
+  );
+
+/**
+  Set value for an PCD entry
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+  @param PtrType         If TRUE, the type of PCD entry's value is Pointer.
+                         If False, the type of PCD entry's value is not Pointer.
+
+  @retval EFI_INVALID_PARAMETER  If this PCD type is VPD, VPD PCD can not be set.
+  @retval EFI_INVALID_PARAMETER  If Size can not be set to size table.
+  @retval EFI_INVALID_PARAMETER  If Size of non-Ptr type PCD does not match the size information in PCD database.  
+  @retval EFI_NOT_FOUND          If value type of PCD entry is intergrate, but not in
+                                 range of UINT8, UINT16, UINT32, UINT64
+  @retval EFI_NOT_FOUND          Can not find the PCD type according to token number.                                
+**/
+EFI_STATUS
+SetWorker (
+  IN          UINTN                     TokenNumber,
+  IN          VOID                      *Data,
+  IN OUT      UINTN                     *Size,
+  IN          BOOLEAN                   PtrType
+  );
+
+/**
+  Wrapper function for set PCD value for non-Pointer type dynamic-ex PCD.
+
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            Value want to be set.
+  @param SetSize         The size of value.
+
+  @return status of ExSetWorker().
+
+**/
+EFI_STATUS
+ExSetValueWorker (
+  IN          UINTN                ExTokenNumber,
+  IN          CONST EFI_GUID       *Guid,
+  IN          VOID                 *Data,
+  IN          UINTN                SetSize
+  );
+
+/**
+  Set value for a dynamic PCD entry.
+  
+  This routine find the local token number according to dynamic-ex PCD's token 
+  space guid and token number firstly, and invoke callback function if this PCD
+  entry registered callback function. Finally, invoken general SetWorker to set
+  PCD value.
+  
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            PCD value want to be set
+  @param SetSize         Size of value.
+  @param PtrType         If TRUE, this PCD entry is pointer type.
+                         If FALSE, this PCD entry is not pointer type.
+
+  @return status of SetWorker().
+
+**/
+EFI_STATUS
+ExSetWorker (
+  IN      UINTN                ExTokenNumber,
+  IN      CONST EFI_GUID       *Guid,
+  IN      VOID                 *Data,
+  IN OUT  UINTN                *Size,
+  IN      BOOLEAN              PtrType
+  );
+
+/**
+  Get the PCD entry pointer in PCD database.
+  
+  This routine will visit PCD database to find the PCD entry according to given
+  token number. The given token number is autogened by build tools and it will be 
+  translated to local token number. Local token number contains PCD's type and 
+  offset of PCD entry in PCD database.
+
+  @param TokenNumber     Token's number, it is autogened by build tools
+  @param GetSize         The size of token's value
+
+  @return PCD entry pointer in PCD database
+
+**/
+VOID *
+GetWorker (
+  IN UINTN             TokenNumber,
+  IN UINTN             GetSize
+  );
+
+/**
+  Wrapper function for get PCD value for dynamic-ex PCD.
+
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param GetSize         The size of dynamic-ex PCD value.
+
+  @return PCD entry in PCD database.
+
+**/
+VOID *
+ExGetWorker (
+  IN CONST EFI_GUID         *Guid,
+  IN UINTN                  ExTokenNumber,
+  IN UINTN                  GetSize
+  );
+
+/**
+  Find the local token number according to system SKU ID.
+
+  @param LocalTokenNumber PCD token number
+  @param Size             The size of PCD entry.
+  @param IsPeiDb          If TRUE, the PCD entry is initialized in PEI phase.
+                          If False, the PCD entry is initialized in DXE phase.
+
+  @return Token number according to system SKU ID.
+
+**/
+UINT32
+GetSkuEnabledTokenNumber (
+  UINT32 LocalTokenNumber,
+  UINTN  Size,
+  BOOLEAN IsPeiDb
+  );
+
+/**
+  Get Variable which contains HII type PCD entry.
+
+  @param VariableGuid    Variable's guid
+  @param VariableName    Variable's unicode name string
+  @param VariableData    Variable's data pointer, 
+  @param VariableSize    Variable's size.
+
+  @return the status of gRT->GetVariable
+**/
+EFI_STATUS
+GetHiiVariable (
+  IN  EFI_GUID      *VariableGuid,
+  IN  UINT16        *VariableName,
+  OUT UINT8          **VariableData,
+  OUT UINTN         *VariableSize
+  );
+
+/**
+  Set value for HII-type PCD.
+
+  A HII-type PCD's value is stored in a variable. Setting/Getting the value of 
+  HII-type PCD is to visit this variable.
+  
+  @param VariableGuid    Guid of variable which stored value of a HII-type PCD.
+  @param VariableName    Unicode name of variable which stored value of a HII-type PCD.
+  @param SetAttributes   Attributes bitmask to set for the variable.
+  @param Data            Value want to be set.
+  @param DataSize        Size of value
+  @param Offset          Value offset of HII-type PCD in variable.
+
+  @return status of GetVariable()/SetVariable().
+
+**/
+EFI_STATUS
+SetHiiVariable (
+  IN  EFI_GUID     *VariableGuid,
+  IN  UINT16       *VariableName,
+  IN  UINT32       SetAttributes,
+  IN  CONST VOID   *Data,
+  IN  UINTN        DataSize,
+  IN  UINTN        Offset
+  );
+
+/**
+  Register the callback function for a PCD entry.
+
+  This routine will register a callback function to a PCD entry by given token number
+  and token space guid.
+  
+  @param TokenNumber        PCD token's number, it is autogened by build tools.
+  @param Guid               PCD token space's guid, 
+                            if not NULL, this PCD is dynamicEx type PCD.
+  @param CallBackFunction   Callback function pointer
+
+  @return EFI_SUCCESS Always success for registering callback function.
+
+**/
+EFI_STATUS
+DxeRegisterCallBackWorker (
+  IN  UINTN                   TokenNumber,
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  );
+
+/**
+  UnRegister the callback function for a PCD entry.
+
+  This routine will unregister a callback function to a PCD entry by given token number
+  and token space guid.
+
+  @param TokenNumber        PCD token's number, it is autogened by build tools.
+  @param Guid               PCD token space's guid.
+                            if not NULL, this PCD is dynamicEx type PCD.
+  @param CallBackFunction   Callback function pointer
+
+  @retval EFI_SUCCESS               Callback function is success to be unregister.
+  @retval EFI_INVALID_PARAMETER     Can not find the PCD entry by given token number.
+**/
+EFI_STATUS
+DxeUnRegisterCallBackWorker (
+  IN  UINTN                   TokenNumber,
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
+  );
+
+/**
+  Initialize the PCD database in DXE phase.
+  
+  PCD database in DXE phase also contains PCD database in PEI phase which is copied
+  from GUID Hob.
+
+**/
+VOID
+BuildPcdDxeDataBase (
+  VOID
+  );
+
+/**
+  Get Token Number according to dynamic-ex PCD's {token space guid:token number}
+
+  A dynamic-ex type PCD, developer must provide pair of token space guid: token number
+  in DEC file. PCD database maintain a mapping table that translate pair of {token
+  space guid: token number} to Token Number.
+  
+  @param Guid            Token space guid for dynamic-ex PCD entry.
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+
+  @return Token Number for dynamic-ex PCD.
+
+**/
+UINTN
+GetExPcdTokenNumber (
+  IN CONST EFI_GUID             *Guid,
+  IN UINT32                     ExTokenNumber
+  );
+
+/**
+  Get next token number in given token space.
+  
+  This routine is used for dynamicEx type PCD. It will firstly scan token space
+  table to get token space according to given token space guid. Then scan given 
+  token number in found token space, if found, then return next token number in 
+  this token space.
+
+  @param Guid            Token space guid. Next token number will be scaned in 
+                         this token space.
+  @param TokenNumber     Token number. 
+                         If PCD_INVALID_TOKEN_NUMBER, return first token number in 
+                         token space table.
+                         If not PCD_INVALID_TOKEN_NUMBER, return next token number
+                         in token space table.
+  @param GuidTable       Token space guid table. It will be used for scan token space
+                         by given token space guid.
+  @param SizeOfGuidTable The size of guid table.
+  @param ExMapTable      DynamicEx token number mapping table.
+  @param SizeOfExMapTable The size of dynamicEx token number mapping table.
+
+  @retval EFI_NOT_FOUND  Can not given token space or token number.
+  @retval EFI_SUCCESS    Success to get next token number.
+
+**/
+EFI_STATUS
+ExGetNextTokeNumber (
+  IN      CONST EFI_GUID    *Guid,
+  IN OUT  UINTN             *TokenNumber,
+  IN      EFI_GUID          *GuidTable,
+  IN      UINTN             SizeOfGuidTable,
+  IN      DYNAMICEX_MAPPING *ExMapTable,
+  IN      UINTN             SizeOfExMapTable
+  );
+
+/**
+  Get size of POINTER type PCD value.
+
+  @param LocalTokenNumberTableIdx Index of local token number in local token number table.
+  @param MaxSize                  Maximum size of POINTER type PCD value.
+
+  @return size of POINTER type PCD value.
+
+**/
+UINTN
+GetPtrTypeSize (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  OUT   UINTN             *MaxSize
+  );
+
+/**
+  Set size of POINTER type PCD value. The size should not exceed the maximum size
+  of this PCD value.
+
+  @param LocalTokenNumberTableIdx Index of local token number in local token number table.
+  @param CurrentSize              Size of POINTER type PCD value.
+
+  @retval TRUE  Success to set size of PCD value.
+  @retval FALSE Fail to set size of PCD value.
+**/
+BOOLEAN
+SetPtrTypeSize (
+  IN          UINTN             LocalTokenNumberTableIdx,
+  IN    OUT   UINTN             *CurrentSize
+  );
+
+/**
+  VariableLockProtocol callback
+  to lock the variables referenced by DynamicHii PCDs with RO property set in *.dsc.
+
+  @param[in] Event      Event whose notification function is being invoked.
+  @param[in] Context    Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableLockCallBack (
+  IN EFI_EVENT          Event,
+  IN VOID               *Context
+  );
+
+extern  PCD_DATABASE   mPcdDatabase;
+
+extern  UINT32         mPcdTotalTokenCount; 
+extern  UINT32         mPeiLocalTokenCount; 
+extern  UINT32         mDxeLocalTokenCount; 
+extern  UINT32         mPeiNexTokenCount;   
+extern  UINT32         mDxeNexTokenCount;  
+extern  UINT32         mPeiExMapppingTableSize;
+extern  UINT32         mDxeExMapppingTableSize;
+extern  UINT32         mPeiGuidTableSize;
+extern  UINT32         mDxeGuidTableSize;
+
+extern  BOOLEAN        mPeiExMapTableEmpty; 
+extern  BOOLEAN        mDxeExMapTableEmpty; 
+extern  BOOLEAN        mPeiDatabaseEmpty;
+
+extern  EFI_GUID     **TmpTokenSpaceBuffer;
+extern  UINTN          TmpTokenSpaceBufferCount;
+
+extern EFI_LOCK mPcdDatabaseLock;
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.c b/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.c
new file mode 100644
index 0000000000..15f1924f89
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.c
@@ -0,0 +1,1408 @@
+/** @file 
+  All Pcd Ppi services are implemented here.
+  
+Copyright (c) 2006 - 2015, 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 "Service.h"
+
+///
+/// Instance of PCD_PPI protocol is EDKII native implementation.
+/// This protocol instance support dynamic and dynamicEx type PCDs.
+///
+PCD_PPI mPcdPpiInstance = {
+  PeiPcdSetSku,
+
+  PeiPcdGet8,
+  PeiPcdGet16,          
+  PeiPcdGet32,          
+  PeiPcdGet64,          
+  PeiPcdGetPtr,         
+  PeiPcdGetBool,     
+  PeiPcdGetSize,
+
+  PeiPcdGet8Ex,
+  PeiPcdGet16Ex,          
+  PeiPcdGet32Ex,          
+  PeiPcdGet64Ex,          
+  PeiPcdGetPtrEx,         
+  PeiPcdGetBoolEx,     
+  PeiPcdGetSizeEx,
+  
+  PeiPcdSet8,
+  PeiPcdSet16,          
+  PeiPcdSet32,          
+  PeiPcdSet64,          
+  PeiPcdSetPtr,         
+  PeiPcdSetBool,     
+
+  PeiPcdSet8Ex,
+  PeiPcdSet16Ex,          
+  PeiPcdSet32Ex,          
+  PeiPcdSet64Ex,          
+  PeiPcdSetPtrEx,         
+  PeiPcdSetBoolEx,
+
+  PeiRegisterCallBackOnSet,
+  PcdUnRegisterCallBackOnSet,
+  PeiPcdGetNextToken,
+  PeiPcdGetNextTokenSpace
+};
+
+///
+/// Instance of EFI_PEI_PCD_PPI which is defined in PI 1.2 Vol 3.
+/// This PPI instance only support dyanmicEx type PCD.
+///
+EFI_PEI_PCD_PPI  mEfiPcdPpiInstance = {
+  PeiPcdSetSku,
+  
+  PeiPcdGet8Ex,
+  PeiPcdGet16Ex,
+  PeiPcdGet32Ex,
+  PeiPcdGet64Ex,
+  PeiPcdGetPtrEx,
+  PeiPcdGetBoolEx,
+  PeiPcdGetSizeEx,
+  PeiPcdSet8Ex,
+  PeiPcdSet16Ex,
+  PeiPcdSet32Ex,
+  PeiPcdSet64Ex,
+  PeiPcdSetPtrEx,
+  PeiPcdSetBoolEx,
+  (EFI_PEI_PCD_PPI_CALLBACK_ON_SET) PeiRegisterCallBackOnSet,
+  (EFI_PEI_PCD_PPI_CANCEL_CALLBACK) PcdUnRegisterCallBackOnSet,
+  PeiPcdGetNextToken,
+  PeiPcdGetNextTokenSpace
+};
+
+///
+/// Instance of GET_PCD_INFO_PPI protocol is EDKII native implementation.
+/// This protocol instance support dynamic and dynamicEx type PCDs.
+///
+GET_PCD_INFO_PPI mGetPcdInfoInstance = {
+  PeiGetPcdInfoGetInfo,
+  PeiGetPcdInfoGetInfoEx,
+  PeiGetPcdInfoGetSku
+};
+
+///
+/// Instance of EFI_GET_PCD_INFO_PPI which is defined in PI 1.2.1 Vol 3.
+/// This PPI instance only support dyanmicEx type PCD.
+///
+EFI_GET_PCD_INFO_PPI  mEfiGetPcdInfoInstance = {
+  PeiGetPcdInfoGetInfoEx,
+  PeiGetPcdInfoGetSku
+};
+
+EFI_PEI_PPI_DESCRIPTOR  mPpiList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gPcdPpiGuid,
+    &mPcdPpiInstance
+  },
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiPeiPcdPpiGuid,
+    &mEfiPcdPpiInstance
+  }
+};
+
+EFI_PEI_PPI_DESCRIPTOR  mPpiList2[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI,
+    &gGetPcdInfoPpiGuid,
+    &mGetPcdInfoInstance
+  },
+  {
+    (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+    &gEfiGetPcdInfoPpiGuid,
+    &mEfiGetPcdInfoInstance
+  }
+};
+
+/**
+  Main entry for PCD PEIM driver.
+  
+  This routine initialize the PCD database for PEI phase and install PCD_PPI/EFI_PEI_PCD_PPI.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @return Status of install PCD_PPI
+
+**/
+EFI_STATUS
+EFIAPI
+PcdPeimInit (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS Status;
+
+  BuildPcdDatabase (FileHandle);
+
+  //
+  // Install PCD_PPI and EFI_PEI_PCD_PPI.
+  //
+  Status = PeiServicesInstallPpi (&mPpiList[0]);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install GET_PCD_INFO_PPI and EFI_GET_PCD_INFO_PPI.
+  //
+  Status = PeiServicesInstallPpi (&mPpiList2[0]);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Retrieve additional information associated with a PCD token in the default token space.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+PeiGetPcdInfoGetInfo (
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  return PeiGetPcdInfo (NULL, TokenNumber, PcdInfo);
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+PeiGetPcdInfoGetInfoEx (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  return PeiGetPcdInfo (Guid, TokenNumber, PcdInfo);
+}
+
+/**
+  Retrieve the currently set SKU Id.
+
+  @return   The currently set SKU Id. If the platform has not set at a SKU Id, then the
+            default SKU Id value of 0 is returned. If the platform has set a SKU Id, then the currently set SKU
+            Id is returned.
+**/
+UINTN
+EFIAPI
+PeiGetPcdInfoGetSku (
+  VOID
+  )
+{
+  return GetPcdDatabase()->SystemSkuId;
+}
+
+/**
+  Sets the SKU value for subsequent calls to set or get PCD token values.
+
+  SetSku() sets the SKU Id to be used for subsequent calls to set or get PCD values. 
+  SetSku() is normally called only once by the system.
+
+  For each item (token), the database can hold a single value that applies to all SKUs, 
+  or multiple values, where each value is associated with a specific SKU Id. Items with multiple, 
+  SKU-specific values are called SKU enabled. 
+  
+  The SKU Id of zero is reserved as a default. The valid SkuId range is 1 to 255.  
+  For tokens that are not SKU enabled, the system ignores any set SKU Id and works with the 
+  single value for that token. For SKU-enabled tokens, the system will use the SKU Id set by the 
+  last call to SetSku(). If no SKU Id is set or the currently set SKU Id isn't valid for the specified token, 
+  the system uses the default SKU Id. If the system attempts to use the default SKU Id and no value has been 
+  set for that Id, the results are unpredictable.
+
+  @param[in]  SkuId The SKU value that will be used when the PCD service will retrieve and 
+              set values associated with a PCD token.
+
+**/
+VOID
+EFIAPI
+PeiPcdSetSku (
+  IN  UINTN                  SkuId
+  )
+{
+  PEI_PCD_DATABASE  *PeiPcdDb;
+  SKU_ID            *SkuIdTable;
+  UINTN             Index;
+
+  PeiPcdDb = GetPcdDatabase();
+  SkuIdTable = (SKU_ID *) ((UINT8 *) PeiPcdDb + PeiPcdDb->SkuIdTableOffset);
+  for (Index = 0; Index < SkuIdTable[0]; Index++) {
+    if (SkuId == SkuIdTable[Index + 1]) {
+      PeiPcdDb->SystemSkuId = (SKU_ID) SkuId;
+      return;
+    }
+  }
+
+  //
+  // Invalid input SkuId, the default SKU Id will be used for the system.
+  //
+  DEBUG ((EFI_D_INFO, "PcdPei - Invalid input SkuId, the default SKU Id will be used.\n"));
+  PeiPcdDb->SystemSkuId = (SKU_ID) 0;
+  return;
+}
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the current byte-sized value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT8 value.
+  
+**/
+UINT8
+EFIAPI
+PeiPcdGet8 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return *((UINT8 *) GetWorker (TokenNumber, sizeof (UINT8)));
+}
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the current 16-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT16 value.
+  
+**/
+UINT16
+EFIAPI
+PeiPcdGet16 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return ReadUnaligned16 (GetWorker (TokenNumber, sizeof (UINT16)));
+}
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the current 32-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT32 value.
+  
+**/
+UINT32
+EFIAPI
+PeiPcdGet32 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return ReadUnaligned32 (GetWorker (TokenNumber, sizeof (UINT32)));
+}
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the current 64-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT64 value.
+  
+**/
+UINT64
+EFIAPI
+PeiPcdGet64 (
+  IN UINTN                    TokenNumber
+  )
+{
+  return ReadUnaligned64 (GetWorker (TokenNumber, sizeof (UINT64)));
+}
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrieved.
+  
+**/
+VOID *
+EFIAPI
+PeiPcdGetPtr (
+  IN UINTN                    TokenNumber
+  )
+{
+  return GetWorker (TokenNumber, 0);
+}
+
+/**
+  Retrieves a Boolean value for a given PCD token.
+
+  Retrieves the current boolean value for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The Boolean value.
+  
+**/
+BOOLEAN
+EFIAPI
+PeiPcdGetBool (
+  IN UINTN                    TokenNumber
+  )
+{
+  return *((BOOLEAN *) GetWorker (TokenNumber, sizeof (BOOLEAN)));
+}
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+PeiPcdGetSize (
+  IN UINTN                    TokenNumber
+  )
+{
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  UINTN               Size;
+  UINTN               MaxSize;
+  UINT32              LocalTokenCount;
+
+  PeiPcdDb        = GetPcdDatabase ();
+  LocalTokenCount = PeiPcdDb->LocalTokenCount;
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  ASSERT (TokenNumber + 1 < (LocalTokenCount + 1));
+
+  Size = (*((UINT32 *)((UINT8 *)PeiPcdDb + PeiPcdDb->LocalTokenNumberTableOffset) + TokenNumber) & PCD_DATUM_TYPE_ALL_SET) >> PCD_DATUM_TYPE_SHIFT;
+
+  if (Size == 0) {
+    //
+    // For pointer type, we need to scan the SIZE_TABLE to get the current size.
+    //
+    return GetPtrTypeSize (TokenNumber, &MaxSize, PeiPcdDb);
+  } else {
+    return Size;
+  }
+
+}
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the 8-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid              The token space for the token number.
+  @param[in]  ExTokenNumber     The PCD token number. 
+
+  @return The size 8-bit value for the PCD token.
+  
+**/
+UINT8
+EFIAPI
+PeiPcdGet8Ex (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  return *((UINT8 *) ExGetWorker (Guid, ExTokenNumber, sizeof (UINT8)));
+}
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the 16-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid          The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 16-bit value for the PCD token.
+  
+**/
+UINT16
+EFIAPI
+PeiPcdGet16Ex (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  return ReadUnaligned16 (ExGetWorker (Guid, ExTokenNumber, sizeof (UINT16)));
+}
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the 32-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 32-bit value for the PCD token.
+  
+**/
+UINT32
+EFIAPI
+PeiPcdGet32Ex (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  return ReadUnaligned32 (ExGetWorker (Guid, ExTokenNumber, sizeof (UINT32)));
+}
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the 64-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size 64-bit value for the PCD token.
+  
+**/
+UINT64
+EFIAPI
+PeiPcdGet64Ex (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  return ReadUnaligned64 (ExGetWorker (Guid, ExTokenNumber, sizeof (UINT64)));
+}
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  Guid          The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrieved.
+  
+**/
+VOID *
+EFIAPI
+PeiPcdGetPtrEx (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  return ExGetWorker (Guid, ExTokenNumber, 0);
+}
+
+/**
+  Retrieves an Boolean value for a given PCD token.
+
+  Retrieves the Boolean value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid          The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size Boolean value for the PCD token.
+  
+**/
+BOOLEAN
+EFIAPI
+PeiPcdGetBoolEx (
+  IN CONST  EFI_GUID              *Guid,
+  IN UINTN                        ExTokenNumber
+  )
+{
+  return *((BOOLEAN *) ExGetWorker (Guid, ExTokenNumber, sizeof (BOOLEAN)));
+}
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  Guid          The token space for the token number.
+  @param[in]  ExTokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+PeiPcdGetSizeEx (
+  IN CONST  EFI_GUID              *Guid,
+  IN UINTN                        ExTokenNumber
+  )
+{
+  return PeiPcdGetSize (GetExPcdTokenNumber (Guid, ExTokenNumber));
+}
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet8 (
+  IN UINTN                        TokenNumber,
+  IN UINT8                        Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet16 (
+  IN UINTN                         TokenNumber,
+  IN UINT16                        Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet32 (
+  IN UINTN                         TokenNumber,
+  IN UINT32                        Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet64 (
+  IN UINTN                         TokenNumber,
+  IN UINT64                        Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetPtr (
+  IN      UINTN                    TokenNumber,
+  IN OUT  UINTN                    *SizeOfBuffer,
+  IN      VOID                     *Buffer
+  )
+{
+  return SetWorker (TokenNumber, Buffer, SizeOfBuffer, TRUE);
+}
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetBool (
+  IN UINTN                         TokenNumber,
+  IN BOOLEAN                       Value
+  )
+{
+  return SetValueWorker (TokenNumber, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid          The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value         The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet8Ex (
+  IN CONST EFI_GUID               *Guid,
+  IN UINTN                        ExTokenNumber,
+  IN UINT8                        Value
+  )
+{
+  return ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet16Ex (
+  IN CONST EFI_GUID               *Guid,
+  IN UINTN                        ExTokenNumber,
+  IN UINT16                       Value
+  )
+{
+  return ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid          The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value         The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet32Ex (
+  IN CONST EFI_GUID               *Guid,
+  IN UINTN                        ExTokenNumber,
+  IN UINT32                       Value
+  )
+{
+  return ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid          The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  Value         The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet64Ex (
+  IN CONST EFI_GUID               *Guid,
+  IN UINTN                        ExTokenNumber,
+  IN UINT64                       Value
+  )
+{
+  return ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]        Guid            The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]        ExTokenNumber   The PCD token number. 
+  @param[in, out]   SizeOfBuffer    A pointer to the length of the value being set for the PCD token.  
+                                    On input, if the SizeOfValue is greater than the maximum size supported 
+                                    for this TokenNumber then the output value of SizeOfValue will reflect 
+                                    the maximum size supported for this TokenNumber.
+  @param[in]        Value           The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetPtrEx (
+  IN     CONST EFI_GUID               *Guid,
+  IN     UINTN                        ExTokenNumber,
+  IN OUT UINTN                        *SizeOfBuffer,
+  IN     VOID                         *Value
+  )
+{
+  return ExSetWorker (ExTokenNumber, Guid, Value, SizeOfBuffer, TRUE);
+}
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param [in]  Guid          The 128-bit unique value that designates the namespace from which to extract the value.
+  @param [in]  ExTokenNumber The PCD token number. 
+  @param [in]  Value         The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetBoolEx (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber,
+  IN BOOLEAN                    Value
+  )
+{
+  return ExSetValueWorker (ExTokenNumber, Guid, &Value, sizeof (Value));
+}
+
+/**
+  Specifies a function to be called anytime the value of a designated token is changed.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiRegisterCallBackOnSet (
+  IN  CONST EFI_GUID              *Guid, OPTIONAL
+  IN  UINTN                       ExTokenNumber,
+  IN  PCD_PPI_CALLBACK            CallBackFunction
+  )
+{
+  if (!FeaturePcdGet(PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (CallBackFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return PeiRegisterCallBackWorker (ExTokenNumber, Guid, CallBackFunction, TRUE);
+}
+
+/**
+  Cancels a previously set callback function for a particular PCD token number.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  ExTokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+PcdUnRegisterCallBackOnSet (
+  IN  CONST EFI_GUID              *Guid, OPTIONAL
+  IN  UINTN                       ExTokenNumber,
+  IN  PCD_PPI_CALLBACK            CallBackFunction
+  )
+{
+  if (!FeaturePcdGet(PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (CallBackFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return PeiRegisterCallBackWorker (ExTokenNumber, Guid, CallBackFunction, FALSE);
+}
+
+/**
+  Retrieves the next valid token number in a given namespace.  
+  
+  This is useful since the PCD infrastructure contains a sparse list of token numbers, 
+  and one cannot a priori know what token numbers are valid in the database. 
+  
+  If TokenNumber is 0 and Guid is not NULL, then the first token from the token space specified by Guid is returned.  
+  If TokenNumber is not 0 and Guid is not NULL, then the next token in the token space specified by Guid is returned.  
+  If TokenNumber is 0 and Guid is NULL, then the first token in the default token space is returned.  
+  If TokenNumber is not 0 and Guid is NULL, then the next token in the default token space is returned.  
+  The token numbers in the default token space may not be related to token numbers in token spaces that are named by Guid.  
+  If the next token number can be retrieved, then it is returned in TokenNumber, and EFI_SUCCESS is returned.  
+  If TokenNumber represents the last token number in the token space specified by Guid, then EFI_NOT_FOUND is returned.  
+  If TokenNumber is not present in the token space specified by Guid, then EFI_NOT_FOUND is returned.
+
+
+  @param[in]       Guid        The 128-bit unique value that designates the namespace from which to extract the value.  
+                               This is an optional parameter that may be NULL.  If this parameter is NULL, then a request 
+                               is being made to retrieve tokens from the default token space.
+  @param[in, out]  TokenNumber A pointer to the PCD token number to use to find the subsequent token number.
+                   
+  @retval EFI_SUCCESS   The PCD service has retrieved the next valid token number.
+  @retval EFI_NOT_FOUND The PCD service could not find data from the requested token number.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdGetNextToken (
+  IN CONST EFI_GUID               *Guid, OPTIONAL
+  IN OUT  UINTN                   *TokenNumber
+  )
+{
+  UINTN               GuidTableIdx;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  EFI_GUID            *MatchGuid;
+  EFI_GUID            *GuidTable;
+  DYNAMICEX_MAPPING   *ExMapTable;
+  UINTN               Index;
+  BOOLEAN             Found;
+  BOOLEAN             PeiExMapTableEmpty;
+  UINTN               PeiNexTokenNumber; 
+
+  if (!FeaturePcdGet (PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  PeiPcdDb          = GetPcdDatabase ();
+  PeiNexTokenNumber = PeiPcdDb->LocalTokenCount - PeiPcdDb->ExTokenCount;
+  GuidTable         = (EFI_GUID *)((UINT8 *)PeiPcdDb + PeiPcdDb->GuidTableOffset);
+
+  if (PeiPcdDb->ExTokenCount == 0) {
+    PeiExMapTableEmpty = TRUE;
+  } else {
+    PeiExMapTableEmpty = FALSE;
+  }
+  if (Guid == NULL) {
+    if (*TokenNumber > PeiNexTokenNumber) {
+      return EFI_NOT_FOUND;
+    }
+    (*TokenNumber)++;
+    if (*TokenNumber > PeiNexTokenNumber) {
+      *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
+      return EFI_NOT_FOUND;
+    }
+    return EFI_SUCCESS;
+  } else {
+    if (PeiExMapTableEmpty) {
+      return EFI_NOT_FOUND;
+    }
+
+    MatchGuid = ScanGuid (GuidTable, PeiPcdDb->GuidTableCount * sizeof(EFI_GUID), Guid);
+
+    if (MatchGuid == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    GuidTableIdx = MatchGuid - GuidTable;
+
+    ExMapTable = (DYNAMICEX_MAPPING *)((UINT8 *)PeiPcdDb + PeiPcdDb->ExMapTableOffset);
+
+    Found = FALSE;
+    //
+    // Locate the GUID in ExMapTable first.
+    //
+    for (Index = 0; Index < PeiPcdDb->ExTokenCount; Index++) {
+      if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+        Found = TRUE;
+        break;
+      }
+    }
+
+    if (Found) {
+      if (*TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+        *TokenNumber = ExMapTable[Index].ExTokenNumber;
+         return EFI_SUCCESS;
+      }
+
+      for ( ; Index < PeiPcdDb->ExTokenCount; Index++) {
+        if (ExMapTable[Index].ExTokenNumber == *TokenNumber) {
+          break;
+        }
+      }
+
+      while (Index < PeiPcdDb->ExTokenCount) {
+        Index++;
+        if (Index == PeiPcdDb->ExTokenCount) {
+          //
+          // Exceed the length of ExMap Table
+          //
+          *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
+          return EFI_NOT_FOUND;
+        } else if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+          //
+          // Found the next match
+          //
+          *TokenNumber = ExMapTable[Index].ExTokenNumber;
+          return EFI_SUCCESS;
+        }
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Retrieves the next valid PCD token namespace for a given namespace.
+
+  Gets the next valid token namespace for a given namespace. This is useful to traverse the valid
+  token namespaces on a platform.
+
+  @param[in, out]   Guid    An indirect pointer to EFI_GUID. On input it designates a known token
+                            namespace from which the search will start. On output, it designates the next valid
+                            token namespace on the platform. If *Guid is NULL, then the GUID of the first token
+                            space of the current platform is returned. If the search cannot locate the next valid
+                            token namespace, an error is returned and the value of *Guid is undefined.
+ 
+  @retval  EFI_SUCCESS      The PCD service retrieved the value requested.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the next valid token namespace.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdGetNextTokenSpace (
+  IN OUT CONST EFI_GUID          **Guid
+  )
+{
+  UINTN               GuidTableIdx;
+  EFI_GUID            *MatchGuid;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  DYNAMICEX_MAPPING   *ExMapTable;
+  UINTN               Index;
+  UINTN               Index2;
+  BOOLEAN             Found;
+  BOOLEAN             PeiExMapTableEmpty;
+  EFI_GUID            *GuidTable;
+
+  if (!FeaturePcdGet (PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  ASSERT (Guid != NULL);
+
+  PeiPcdDb = GetPcdDatabase ();
+
+  if (PeiPcdDb->ExTokenCount == 0) {
+    PeiExMapTableEmpty = TRUE;
+  } else {
+    PeiExMapTableEmpty = FALSE;
+  }
+  
+  if (PeiExMapTableEmpty) {
+    return EFI_NOT_FOUND;
+  }
+
+  ExMapTable = (DYNAMICEX_MAPPING *)((UINT8 *)PeiPcdDb + PeiPcdDb->ExMapTableOffset);
+  GuidTable  = (EFI_GUID *)((UINT8 *)PeiPcdDb + PeiPcdDb->GuidTableOffset);
+  
+  if (*Guid == NULL) {
+    //
+    // return the first Token Space Guid.
+    //
+    *Guid = GuidTable + ExMapTable[0].ExGuidIndex;
+    return EFI_SUCCESS;
+  }
+
+  MatchGuid = ScanGuid (GuidTable, PeiPcdDb->GuidTableCount * sizeof(GuidTable[0]), *Guid);
+
+  if (MatchGuid == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  
+  GuidTableIdx = MatchGuid - GuidTable;
+
+  Found = FALSE;
+  for (Index = 0; Index < PeiPcdDb->ExTokenCount; Index++) {
+    if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+      Found = TRUE;
+      break;
+    }
+  }
+
+  if (Found) {
+    Index++;
+    for ( ; Index < PeiPcdDb->ExTokenCount; Index++ ) {
+      if (ExMapTable[Index].ExGuidIndex != GuidTableIdx) {
+        Found = FALSE;
+        for (Index2 = 0 ; Index2 < Index; Index2++) {
+          if (ExMapTable[Index2].ExGuidIndex == ExMapTable[Index].ExGuidIndex) {
+            //
+            // This token namespace should have been found and output at preceding getting.
+            //
+            Found = TRUE;
+            break;
+          }
+        }
+        if (!Found) {
+          *Guid = (EFI_GUID *)((UINT8 *)PeiPcdDb + PeiPcdDb->GuidTableOffset) + ExMapTable[Index].ExGuidIndex;
+          return EFI_SUCCESS;
+        }
+      }
+    }
+    *Guid = NULL;
+  }
+
+  return EFI_NOT_FOUND;
+
+}
+
+/**
+  Get PCD value's size for POINTER type PCD.
+  
+  The POINTER type PCD's value will be stored into a buffer in specified size.
+  The max size of this PCD's value is described in PCD's definition in DEC file.
+
+  @param LocalTokenNumberTableIdx Index of PCD token number in PCD token table
+  @param MaxSize                  Maximum size of PCD's value
+  @param Database                 Pcd database in PEI phase.
+
+  @return PCD value's size for POINTER type PCD.
+
+**/
+UINTN
+GetPtrTypeSize (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  OUT   UINTN             *MaxSize,
+  IN    PEI_PCD_DATABASE  *Database
+  )
+{
+  INTN        SizeTableIdx;
+  UINTN       LocalTokenNumber;
+  SKU_ID      *SkuIdTable;
+  SIZE_INFO   *SizeTable;
+  UINTN       Index;
+
+  SizeTableIdx = GetSizeTableIndex (LocalTokenNumberTableIdx, Database);
+
+  LocalTokenNumber = *((UINT32 *)((UINT8 *)Database + Database->LocalTokenNumberTableOffset) + LocalTokenNumberTableIdx);
+
+  ASSERT ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER);
+  
+  SizeTable = (SIZE_INFO *)((UINT8 *)Database + Database->SizeTableOffset);
+
+  *MaxSize = SizeTable[SizeTableIdx];
+  //
+  // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+  // PCD entry.
+  //
+  if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+      //
+      // We have only two entry for VPD enabled PCD entry:
+      // 1) MAX Size.
+      // 2) Current Size
+      // We consider current size is equal to MAX size.
+      //
+      return *MaxSize;
+  } else {
+    if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+      //
+      // We have only two entry for Non-Sku enabled PCD entry:
+      // 1) MAX SIZE
+      // 2) Current Size
+      //
+      return SizeTable[SizeTableIdx + 1];
+    } else {
+      //
+      // We have these entry for SKU enabled PCD entry
+      // 1) MAX SIZE
+      // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+      //
+      SkuIdTable = GetSkuIdArray (LocalTokenNumberTableIdx, Database);
+      for (Index = 0; Index < SkuIdTable[0]; Index++) {
+        if (SkuIdTable[1 + Index] == Database->SystemSkuId) {
+          return SizeTable[SizeTableIdx + 1 + Index];
+        }
+      }
+      return SizeTable[SizeTableIdx + 1];
+    }
+  }
+}
+
+/**
+  Set PCD value's size for POINTER type PCD.
+  
+  The POINTER type PCD's value will be stored into a buffer in specified size.
+  The max size of this PCD's value is described in PCD's definition in DEC file.
+
+  @param LocalTokenNumberTableIdx Index of PCD token number in PCD token table
+  @param CurrentSize              Maximum size of PCD's value
+  @param Database                 Pcd database in PEI phase.
+
+  @retval TRUE  Success to set PCD's value size, which is not exceed maximum size
+  @retval FALSE Fail to set PCD's value size, which maybe exceed maximum size
+
+**/
+BOOLEAN
+SetPtrTypeSize (
+  IN          UINTN             LocalTokenNumberTableIdx,
+  IN    OUT   UINTN             *CurrentSize,
+  IN          PEI_PCD_DATABASE  *Database
+  )
+{
+  INTN        SizeTableIdx;
+  UINTN       LocalTokenNumber;
+  SKU_ID      *SkuIdTable;
+  SIZE_INFO   *SizeTable;
+  UINTN       Index;
+  UINTN       MaxSize;
+  
+  SizeTableIdx = GetSizeTableIndex (LocalTokenNumberTableIdx, Database);
+
+  LocalTokenNumber = *((UINT32 *)((UINT8 *)Database + Database->LocalTokenNumberTableOffset) + LocalTokenNumberTableIdx);
+
+  ASSERT ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER);
+
+  SizeTable = (SIZE_INFO *)((UINT8 *)Database + Database->SizeTableOffset);
+
+  MaxSize = SizeTable[SizeTableIdx];
+  //
+  // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+  // PCD entry.
+  //
+  if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+      //
+      // We shouldn't come here as we don't support SET for VPD
+      //
+      ASSERT (FALSE);
+      return FALSE;
+  } else {
+    if ((*CurrentSize > MaxSize) ||
+      (*CurrentSize == MAX_ADDRESS)) {
+       *CurrentSize = MaxSize;
+       return FALSE;
+    }
+    
+    if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+      //
+      // We have only two entry for Non-Sku enabled PCD entry:
+      // 1) MAX SIZE
+      // 2) Current Size
+      //
+      SizeTable[SizeTableIdx + 1] = (SIZE_INFO) *CurrentSize;
+      return TRUE;
+    } else {
+      //
+      // We have these entry for SKU enabled PCD entry
+      // 1) MAX SIZE
+      // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+      //
+      SkuIdTable = GetSkuIdArray (LocalTokenNumberTableIdx, Database);
+      for (Index = 0; Index < SkuIdTable[0]; Index++) {
+        if (SkuIdTable[1 + Index] == Database->SystemSkuId) {
+          SizeTable[SizeTableIdx + 1 + Index] = (SIZE_INFO) *CurrentSize;
+          return TRUE;
+        }
+      }
+      SizeTable[SizeTableIdx + 1] = (SIZE_INFO) *CurrentSize;
+      return TRUE;
+    }
+  }
+
+}
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.inf b/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.inf
new file mode 100644
index 0000000000..0645f914c7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/Pcd.inf
@@ -0,0 +1,352 @@
+## @file
+# PCD PEIM produces PCD database to manage all dynamic PCD in PEI phase and install Pcd Ppi service.
+#
+# This version PCD PEIM depends on the external PCD database binary file, not built in PCD data base. 
+# There are two PCD PPIs as follows:
+#   1) PCD_PPI 
+#      It is EDKII implementation which support Dynamic/DynamicEx Pcds.
+#   2) EFI_PEI_PCD_PPI
+#      It is defined by PI specification 1.2, Vol 3 which only support dynamicEx 
+#      type Pcd.
+# For dynamicEx type PCD, it is compatible between PCD_PPI and EFI_PEI_PCD_PPI.
+# PCD PEIM driver will produce above two PPIs at same time.
+#
+# PCD database is generated as the separate binary image at build time. The binary image 
+# will be intergrated into Firmware volume together with PCD driver. 
+#
+# ////////////////////////////////////////////////////////////////////////////////
+# //                                                                            //
+# //                      Introduction of PCD database                          //
+# //                                                                            //
+# ////////////////////////////////////////////////////////////////////////////////
+#
+# 1, Introduction
+#    PCD database hold all dynamic type PCD information. The structure of PEI PCD 
+#    database is generated by build tools according to dynamic PCD usage for 
+#    specified platform.
+#    
+# 2, Dynamic Type PCD
+#    Dynamic type PCD is used for the configuration/setting which value is determined
+#    dynamic. In contrast, the value of static type PCD (FeatureFlag, FixedPcd, 
+#    PatchablePcd) is fixed in final generated FD image in build time. 
+#        
+#    2.1 The "dynamic" determination means one of below cases:
+#      a) The PCD setting value is produced by someone driver and consumed by 
+#         other driver in execution time.
+#      b) The PCD setting value is set/get by user from FrontPage.
+#      c) The PCD setting value is produced by platform OEM vendor in specified area.
+#    
+#    2.2 According to module distribution way, dynamic PCD could be classfied as:
+#      a) Dynamic:
+#         If module is released in source code and will be built with platform 
+#         DSC, the dynamic PCD used by this module can be accessed as:
+#                 PcdGetxx(PcdSampleDynamicPcd); 
+#         In building platform, build tools will translate PcdSampleDynamicPcd to
+#         pair of {Token Space Guid: Token Number} for this PCD. 
+#      b) DynamicEx:
+#         If module is release as binary and will not pariticpate platform building,
+#         the dynamic PCD used by this module need be accessed as:
+#               PcdGetxxEx(gEfiMyTokenspaceGuid, PcdSampleDynamicPcd)
+#         Developer need explicity gives {Token Space Guid:Token Number} as parameter
+#         in writting source code.
+#         
+#    2.3 According to PCD value's storage method, dynamic PCD could be classfied as:
+#      a) Default Storage: 
+#         - The PCD value is stored in PCD database maintained by PCD driver in boot 
+#           time memory.
+#         - This type is used for communication between PEIM/DXE driver, DXE/DXE 
+#           driver. But all set/get value will be losted after boot-time memory 
+#           is turn off.
+#         - [PcdsDynamicDefault] is used as section name for this type PCD in 
+#           platform DSC file. [PcdsDynamicExDefault] is used for dynamicEx type PCD.
+#         
+#      b) Variable Storage: 
+#         - The PCD value is stored in variable area. 
+#         - As default storage type, this type PCD could be used for PEI/DXE driver
+#           communication. But beside it, this type PCD could also be used to store 
+#           the value associate with a HII setting via variable interface.
+#         - In PEI phase, the PCD value could only be got but can not be set due 
+#           to variable area is readonly.
+#         - [PcdsDynamicHii] is used as section name for this type PCD in platform 
+#           DSC file. [PcdsDynamicExHii] is for dynamicEx type PCD.
+#           
+#      c) OEM specificed storage area:
+#         - The PCD value is stored in OEM specified area which base address is 
+#           specified by a FixedAtBuild PCD setting - PcdVpdBaseAddress.
+#         - The area is read only for PEI and DXE phase.
+#         - [PcdsDynamicVpd] is used as section name for this type PCD in platform 
+#           DSC file. [PcdsDynamicExVpd] is for dynamicex type PCD.
+#      
+#    2.4 When and how to use dynamic PCD
+#      Module developer do not care the used PCD is dynamic or static when writting
+#      source code/INF. Dynamic PCD and dynamic type is pointed by platform integrator 
+#      in platform DSC file. Please ref section 2.3 to get matching between dynamic
+#      PCD type and section name in DSC file.
+#    
+# 3, PCD database:
+#    Although dynamic PCD could be in different storage type as above description, 
+#    but the basic information and default value for all dynamic PCD is hold
+#    by PCD database maintained by PEI/DXE driver.
+#    
+#    As the whole EFI BIOS boot path is divided into PEI/DXE phase, the PCD database
+#    also is divided into Pei/Dxe database maintaied by PcdPeim/PcdDxe driver separatly.
+#    To make PcdPeim's driver image smaller, PEI PCD database only hold all dynamic
+#    PCD information used in PEI phase or use in both PEI/DXE phase. And DXE PCD
+#    database contains all PCDs used in PEI/DXE phase in memory.
+#    
+#    Build tool will generate PCD database into the separate binary file for 
+#    PEI/DXE PCD driver according to dynamic PCD section in platform DSC file. 
+#    
+#    3.1 PcdPeim and PcdDxe
+#      PEI PCD database is maintained by PcdPeim driver run from flash. PcdPeim driver
+#      build guid hob in temporary memory and copy the binary data base from flash 
+#      to temporary memory for PEI PCD database. 
+#      DXE PCD database is maintained by PcdDxe driver.At entry point of PcdDxe driver,
+#      a new PCD database is allocated in boot-time memory which including all
+#      PEI PCD and DXE PCD entry.
+#      
+#      Pcd driver should run as early as possible before any other driver access
+#      dynamic PCD's value. PEI/DXE "Apriori File" mechanism make it possible by
+#      making PcdPeim/PcdDxe as first dispatching driver in PEI/DXE phase.
+#      
+#    3.2 Token space Guid/Token number, Platform token, Local token number
+#           Dynamic PCD
+#          +-----------+               +---------+
+#          |TokenSpace |               |Platform |
+#          |   Guid    |  build tool   | Token   |
+#          |    +      +-------------->| Number  |
+#          |  Token    |               +---------+`._
+#          |  Number   |                             `.
+#          +-----------+                               `.  +------+
+#                                                        `-|Local |
+#                                                          |Token |
+#                               DynamicEx PCD            ,-|Number|
+#                               +-----------+         ,-'  +------+
+#                               |TokenSpace |      ,-'
+#                               |   Guid    |  _,-'
+#                               |    +      +.'
+#                               |  Token    |
+#                               |  Number   |
+#                               +-----------+
+#    
+#    
+#      3.2.1 Pair of Token space guid + Token number
+#        Any type PCD is identified by pair of "TokenSpaceGuid + TokeNumber". But it
+#        is not easy maintained by PCD driver, and hashed token number will make 
+#        searching slowly. 
+#
+#      3.2.2 Platform Token Number
+#        "Platform token number" concept is introduced for mapping to a pair of 
+#        "TokenSpaceGuid + TokenNumber". The platform token number is generated by 
+#        build tool in autogen.h and all of them are continual in a platform scope 
+#        started from 1.(0 meaning invalid internal token number)
+#        With auto-generated "platform token number", PcdGet(PcdSampleDynamicPcd)
+#        in source code is translated to LibPcdGet(_PCD_TOKEN_PcdSampleDynamicPcd) 
+#        in autogen.h.
+#        Notes: The mapping between pair of "tokenspace guid + token number" and
+#        "internal token number" need build tool establish, so "platform token number"
+#        mechanism is not suitable for binary module which use DynamicEx type PCD.
+#        To access a dynamicEx type PCD, pair of "token space guid/token number" all need
+#        to be specificed for PcdSet/PcdGet accessing macro.
+#      
+#        Platform Token Number is started from 1, and inceased continuous. From whole 
+#        platform scope, there are two zones: PEI Zone and DXE Zone
+#                  |                      Platform Token Number
+#        ----------|----------------------------------------------------------------
+#        PEI Zone: |            1                 ~  PEI_LOCAL_TOKEN_NUMBER
+#        DXE Zone: | (PEI_LOCAL_TOKEN_NUMBER + 1) ~ (PEI_LOCAL_TOKEN_NUMBER + DXE_LOCAL_TOKEN_NUMBER)
+#        
+#      3.2.3 Local Token Number
+#        To fast searching a PCD entry in PCD database, PCD driver translate 
+#        platform token number to local token number via a mapping table.
+#        For binary DynamicEx type PCD, there is a another mapping table to translate
+#        "token space guid + token number" to local token number directly.
+#        Local token number is identifier for all internal interface in PCD PEI/DXE
+#        driver.
+#        
+#        A local token number is a 32-bit value in following meaning:
+#         32 ------------- 28 ---------- 24 -------- 0
+#          | PCD type mask  | Datum Type  |  Offset  |
+#          +-----------------------------------------+
+#        where:
+#          PCd type mask: indicate Pcd type from following macro:
+#                         PCD_TYPE_DATA
+#                         PCD_TYPE_HII
+#                         PCD_TYPE_VPD
+#                         PCD_TYPE_SKU_ENABLED
+#                         PCD_TYPE_STRING
+#          Datum Type   : indicate PCD vaue type from following macro:
+#                         PCD_DATUM_TYPE_POINTER
+#                         PCD_DATUM_TYPE_UINT8
+#                         PCD_DATUM_TYPE_UINT16
+#                         PCD_DATUM_TYPE_UINT32
+#                         PCD_DATUM_TYPE_UINT64
+#          Offset      : indicate the related offset of PCD value in PCD database array.
+#       Based on local token number, PCD driver could fast determine PCD type, value
+#       type and get PCD entry from PCD database.
+#       
+#    3.3 PCD Database binary file
+#      PCD Database binary file will be created at build time as the standalone binary image. 
+#      To understand the binary image layout, PCD Database C structure is still generated 
+#      as comments by build tools in PCD driver's autogen.h/
+#      autogen.c file. In generated C structure, following information is stored:
+#      - ExMapTable: This table is used translate a binary dynamicex type PCD's 
+#                    "tokenguid + token" to local token number.
+#      - LocalTokenNumberTable:
+#                    This table stores all local token number in array, use "Internal
+#                    token number" as array index to get PCD entry's offset fastly.
+#      - SizeTable:  This table stores the size information for all PCD entry.
+#      - GuidTable:  This table stores guid value for DynamicEx's token space,
+#                    HII type PCD's variable GUID.
+#      - SkuIdTable: TBD
+#      - SystemSkuId: TBD
+#      - PCD value structure:  
+#                    Every PCD has a value record in PCD database. For different
+#                    datum type PCD has different record structure which will be 
+#                    introduced in 3.3.1
+#      
+#      In a PCD database structure, there are two major area: Init and UnInit. 
+#      Init area is use stored above PCD internal structure such as ExMapTable, 
+#      LocalTokenNumberTable etc and the (default) value of PCD which has default 
+#      value specified in platform DSC file.
+#      Unint area is used stored the value of PCD which has no default value in
+#      platform DSC file, the value of NULL, 0 specified in platform DSC file can
+#      be seemed as "no default value".
+#      
+#      3.3.1 Simple Sample PCD Database C Structure
+#        A general sample of PCD database structue is as follows:
+#        typedef struct _PCD_DATABASE {
+#          typedef struct _PCD_DATABASE_INIT {
+#            //===== Following is PCD database internal maintain structures
+#            DYNAMICEX_MAPPING ExMapTable[PEI_EXMAPPING_TABLE_SIZE];
+#            UINT32            LocalTokenNumberTable[PEI_LOCAL_TOKEN_NUMBER_TABLE_SIZE];
+#            GUID              GuidTable[PEI_GUID_TABLE_SIZE];
+#            SIZE_INFO         SizeTable[PEI_SIZE_TABLE_SIZE];
+#            UINT8             SkuIdTable[PEI_SKUID_TABLE_SIZE];
+#            SKU_ID            SystemSkuId;
+#            
+#            //===== Following is value structure for PCD with default value
+#            ....
+#            ....
+#            ....
+#          } Init;
+#          typedef struct _PCD_DATABSE_UNINIT {
+#            //==== Following is value structure for PCD without default value
+#            ....
+#            ....
+#          } UnInit;
+#        }
+#      
+#      3.3.2 PCD value structure in PCD database C structure
+#        The value's structure is generated by build tool in PCD database C structure.
+#        The PCDs in different datum type has different value structure.
+#        
+#        3.3.2.1 UINT8/UINT16/UINT32/UINT64 datum type PCD
+#          The C structure for these datum type PCD is just a UINT8/UINT16/UINT32/UINT64
+#          data member in PCD database, For example:
+#          UINT16  PcdHardwareErrorRecordLevel_d3705011_bc19_4af7_be16_f68030378c15_VariableDefault_0;
+#          Above structure is generated by build tool, the member name is "PcdCName_Guidvalue"
+#          Member type is UINT16 according to PcdHardwareErrorRecordLevel declaration
+#          in DEC file.
+#          
+#        3.3.2.2 VOID* datum type PCD
+#          The value of VOID* datum type PCD is a UINT8/UINT16 array in PCD database.
+#          
+#          3.3.2.2.1 VOID* - string type
+#            If the default value for VOID* datum type PCD like L"xxx", the PCD is 
+#            used for unicode string, and C structure of this datum type PCD is 
+#            UINT16 string array in PCD database, for example:
+#            UINT16 StringTable[29];
+#            The number of 29 in above sample is max size of a unicode string.
+#            
+#            If the default value for VOID* datum type PCD like "xxx", the PCD is
+#            used for ascii string, and C structure of this datum type PCD is 
+#            UINT8 string array in PCD database, for example:
+#            UINT8 StringTable[20];
+#            The number of 20 in above sample is max size of a ascii string.
+#            
+#          3.3.2.2.2 VOID* - byte array
+#            If the default value of VOID* datum type PCD like {'0x29', '0x01', '0xf2'}
+#            the PCD is used for byte array. The generated structrue is same as 
+#            above ascii string table,
+#            UINT8 StringTable[13];
+#            The number of 13 in above sample is max size of byte array.
+#       
+#      3.3.3 Some utility structures in PCD Database
+#        3.3.3.1 GuidTable
+#          GuidTable array is used to store all related GUID value in PCD database:
+#            - Variable GUID for HII type PCD
+#            - Token space GUID for dynamicex type PCD 
+#    
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PcdPeim
+  MODULE_UNI_FILE                = PcdPeim.uni
+  FILE_GUID                      = 9B3ADA4F-AE56-4c24-8DEA-F03B7558AE50
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 4.0
+  PCD_IS_DRIVER                  = PEI_PCD_DRIVER
+  ENTRY_POINT                    = PcdPeimInit
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  Service.c
+  Service.h
+  Pcd.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  PcdLib
+  PeiServicesLib
+  HobLib
+  BaseLib
+  PeimEntryPoint
+  DebugLib
+  MemoryAllocationLib
+
+[Guids]
+  ## PRODUCES            ## HOB
+  ## SOMETIMES_CONSUMES  ## HOB
+  gPcdDataBaseHobGuid                           
+  gPcdDataBaseSignatureGuid                     ## CONSUMES  ## GUID  # PCD database signature GUID.
+
+[Ppis]
+  gEfiPeiReadOnlyVariable2PpiGuid               ## SOMETIMES_CONSUMES
+  gPcdPpiGuid                                   ## PRODUCES
+  gEfiPeiPcdPpiGuid                             ## PRODUCES
+  gGetPcdInfoPpiGuid                            ## SOMETIMES_PRODUCES
+  gEfiGetPcdInfoPpiGuid                         ## SOMETIMES_PRODUCES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdPeiFullPcdDatabaseEnable  ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVpdBaseAddress ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxPeiPcdCallBackNumberPerPcdEntry || gEfiMdeModulePkgTokenSpaceGuid.PcdPeiFullPcdDatabaseEnable ## SOMETIMES_CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PcdPeimExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeim.uni b/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeim.uni
new file mode 100644
index 0000000000..b25cabc8b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeim.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeimExtra.uni b/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeimExtra.uni
new file mode 100644
index 0000000000..6eee5eac0f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/PcdPeimExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/Service.c b/Core/MdeModulePkg/Universal/PCD/Pei/Service.c
new file mode 100644
index 0000000000..45ce01b1fa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/Service.c
@@ -0,0 +1,1209 @@
+/** @file
+  The driver internal functions are implmented here.
+  They build Pei PCD database, and provide access service to PCD database.
+
+Copyright (c) 2006 - 2015, 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 "Service.h"
+
+/**
+  Get Local Token Number by Token Number.
+
+  @param[in]    Database    PCD database.
+  @param[in]    TokenNumber The PCD token number.
+
+  @return       Local Token Number.
+**/
+UINT32
+GetLocalTokenNumber (
+  IN PEI_PCD_DATABASE   *Database,
+  IN UINTN              TokenNumber
+  )
+{
+  UINT32                LocalTokenNumber;
+  UINTN                 Size;
+  UINTN                 MaxSize;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  LocalTokenNumber = *((UINT32 *)((UINT8 *)Database + Database->LocalTokenNumberTableOffset) + TokenNumber);
+
+  Size = (LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) >> PCD_DATUM_TYPE_SHIFT;
+
+  if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == PCD_TYPE_SKU_ENABLED) {
+    if (Size == 0) {
+      GetPtrTypeSize (TokenNumber, &MaxSize, Database);
+    } else {
+      MaxSize = Size;
+    }
+    LocalTokenNumber = GetSkuEnabledTokenNumber (LocalTokenNumber & ~PCD_TYPE_SKU_ENABLED, MaxSize);
+  }
+
+  return LocalTokenNumber;
+}
+
+/**
+  Get PCD type by Local Token Number.
+
+  @param[in]    LocalTokenNumber The PCD local token number.
+
+  @return       PCD type.
+**/
+EFI_PCD_TYPE
+GetPcdType (
+  IN UINT32             LocalTokenNumber
+  )
+{
+  switch (LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) {
+    case PCD_DATUM_TYPE_POINTER:
+      return EFI_PCD_TYPE_PTR;
+    case PCD_DATUM_TYPE_UINT8:
+      if ((LocalTokenNumber & PCD_DATUM_TYPE_UINT8_BOOLEAN) == PCD_DATUM_TYPE_UINT8_BOOLEAN) {
+        return EFI_PCD_TYPE_BOOL;
+      } else {
+        return EFI_PCD_TYPE_8;
+      }
+    case PCD_DATUM_TYPE_UINT16:
+      return EFI_PCD_TYPE_16;
+    case PCD_DATUM_TYPE_UINT32:
+      return EFI_PCD_TYPE_32;
+    case PCD_DATUM_TYPE_UINT64:
+      return EFI_PCD_TYPE_64;
+    default:
+      ASSERT (FALSE);
+      return EFI_PCD_TYPE_8;
+  }
+}
+
+/**
+  Get PCD name.
+
+  @param[in]    OnlyTokenSpaceName  If TRUE, only need to get the TokenSpaceCName.
+                                    If FALSE, need to get the full PCD name.
+  @param[in]    Database            PCD database.
+  @param[in]    TokenNumber         The PCD token number.
+
+  @return       The TokenSpaceCName or full PCD name.
+**/
+CHAR8 *
+GetPcdName (
+  IN BOOLEAN            OnlyTokenSpaceName,
+  IN PEI_PCD_DATABASE   *Database,
+  IN UINTN              TokenNumber
+  )
+{
+  UINT8             *StringTable;
+  UINTN             NameSize;
+  PCD_NAME_INDEX    *PcdNameIndex;
+  CHAR8             *TokenSpaceName;
+  CHAR8             *PcdName;
+  CHAR8             *Name;
+
+  //
+  // Return NULL when PCD name table is absent. 
+  //
+  if (Database->PcdNameTableOffset == 0) {
+    return NULL;
+  }
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  StringTable = (UINT8 *) Database + Database->StringTableOffset;
+
+  //
+  // Get the PCD name index.
+  //
+  PcdNameIndex = (PCD_NAME_INDEX *)((UINT8 *) Database + Database->PcdNameTableOffset) + TokenNumber;
+  TokenSpaceName = (CHAR8 *)&StringTable[PcdNameIndex->TokenSpaceCNameIndex];
+  PcdName = (CHAR8 *)&StringTable[PcdNameIndex->PcdCNameIndex];
+
+  if (OnlyTokenSpaceName) {
+    //
+    // Only need to get the TokenSpaceCName.
+    //
+    Name = AllocateCopyPool (AsciiStrSize (TokenSpaceName), TokenSpaceName);
+  } else {
+    //
+    // Need to get the full PCD name.
+    //
+    NameSize = AsciiStrSize (TokenSpaceName) + AsciiStrSize (PcdName);
+    Name = AllocateZeroPool (NameSize);
+    ASSERT (Name != NULL);
+    //
+    // Catenate TokenSpaceCName and PcdCName with a '.' to form the full PCD name.
+    //
+    AsciiStrCatS (Name, NameSize, TokenSpaceName);
+    Name[AsciiStrSize (TokenSpaceName) - sizeof (CHAR8)] = '.';
+    AsciiStrCatS (Name, NameSize, PcdName);  
+  }
+
+  return Name;
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Database    PCD database.
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName. 
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+ExGetPcdInfo (
+  IN        PEI_PCD_DATABASE    *Database,
+  IN CONST  EFI_GUID            *Guid,
+  IN        UINTN               TokenNumber,
+  OUT       EFI_PCD_INFO        *PcdInfo
+  )
+{
+  UINTN                 GuidTableIdx;
+  EFI_GUID              *MatchGuid;
+  EFI_GUID              *GuidTable;
+  DYNAMICEX_MAPPING     *ExMapTable;
+  UINTN                 Index;
+  UINT32                LocalTokenNumber;
+
+  GuidTable = (EFI_GUID *)((UINT8 *)Database + Database->GuidTableOffset);
+  MatchGuid = ScanGuid (GuidTable, Database->GuidTableCount * sizeof(EFI_GUID), Guid);
+
+  if (MatchGuid == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  GuidTableIdx = MatchGuid - GuidTable;
+
+  ExMapTable = (DYNAMICEX_MAPPING *)((UINT8 *)Database + Database->ExMapTableOffset);
+
+  //
+  // Find the PCD by GuidTableIdx and ExTokenNumber in ExMapTable.
+  //
+  for (Index = 0; Index < Database->ExTokenCount; Index++) {
+    if (ExMapTable[Index].ExGuidIndex == GuidTableIdx) {
+      if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+        //
+        // TokenNumber is 0, follow spec to set PcdType to EFI_PCD_TYPE_8,
+        // PcdSize to 0 and PcdName to the null-terminated ASCII string
+        // associated with the token's namespace Guid.
+        //
+        PcdInfo->PcdType = EFI_PCD_TYPE_8;
+        PcdInfo->PcdSize = 0;
+        //
+        // Here use one representative in the token space to get the TokenSpaceCName.
+        // 
+        PcdInfo->PcdName = GetPcdName (TRUE, Database, ExMapTable[Index].TokenNumber);
+        return EFI_SUCCESS;
+      } else if (ExMapTable[Index].ExTokenNumber == TokenNumber) {
+        PcdInfo->PcdSize = PeiPcdGetSize (ExMapTable[Index].TokenNumber);
+        LocalTokenNumber = GetLocalTokenNumber (Database, ExMapTable[Index].TokenNumber);
+        PcdInfo->PcdType = GetPcdType (LocalTokenNumber);
+        PcdInfo->PcdName = GetPcdName (FALSE, Database, ExMapTable[Index].TokenNumber);
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+PeiGetPcdInfo (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  )
+{
+  PEI_PCD_DATABASE      *PeiPcdDb;
+  BOOLEAN               PeiExMapTableEmpty;
+  UINTN                 PeiNexTokenNumber;
+  UINT32                LocalTokenNumber;
+
+  ASSERT (PcdInfo != NULL);
+
+  PeiPcdDb          = GetPcdDatabase ();
+  PeiNexTokenNumber = PeiPcdDb->LocalTokenCount - PeiPcdDb->ExTokenCount;
+
+  if (PeiPcdDb->ExTokenCount == 0) {
+    PeiExMapTableEmpty = TRUE;
+  } else {
+    PeiExMapTableEmpty = FALSE;
+  }
+
+  if (Guid == NULL) {
+    if (TokenNumber > PeiNexTokenNumber) {
+      return EFI_NOT_FOUND;
+    } else if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+      //
+      // TokenNumber is 0, follow spec to set PcdType to EFI_PCD_TYPE_8,
+      // PcdSize to 0 and PcdName to NULL for default Token Space.
+      //
+      PcdInfo->PcdType = EFI_PCD_TYPE_8;
+      PcdInfo->PcdSize = 0;
+      PcdInfo->PcdName = NULL;
+    } else {
+      PcdInfo->PcdSize = PeiPcdGetSize (TokenNumber);
+      LocalTokenNumber = GetLocalTokenNumber (PeiPcdDb, TokenNumber);
+      PcdInfo->PcdType = GetPcdType (LocalTokenNumber);
+      PcdInfo->PcdName = GetPcdName (FALSE, PeiPcdDb, TokenNumber);
+    }
+    return EFI_SUCCESS;
+  } else {
+    if (PeiExMapTableEmpty) {
+      return EFI_NOT_FOUND;
+    }
+    return ExGetPcdInfo (
+             PeiPcdDb,
+             Guid,
+             TokenNumber,
+             PcdInfo
+             );
+  }
+}
+
+/**
+  The function registers the CallBackOnSet fucntion
+  according to TokenNumber and EFI_GUID space.
+
+  @param  ExTokenNumber       The token number.
+  @param  Guid              The GUID space.
+  @param  CallBackFunction  The Callback function to be registered.
+  @param  Register          To register or unregister the callback function.
+
+  @retval EFI_SUCCESS If the Callback function is registered.
+  @retval EFI_NOT_FOUND If the PCD Entry is not found according to Token Number and GUID space.
+  @retval EFI_OUT_OF_RESOURCES If the callback function can't be registered because there is not free
+                                slot left in the CallbackFnTable.
+  @retval EFI_INVALID_PARAMETER If the callback function want to be de-registered can not be found.
+**/
+EFI_STATUS
+PeiRegisterCallBackWorker (
+  IN  UINTN                       ExTokenNumber,
+  IN  CONST EFI_GUID              *Guid, OPTIONAL
+  IN  PCD_PPI_CALLBACK            CallBackFunction,
+  IN  BOOLEAN                     Register
+)
+{
+  EFI_HOB_GUID_TYPE       *GuidHob;
+  PCD_PPI_CALLBACK        *CallbackTable;
+  PCD_PPI_CALLBACK        Compare;
+  PCD_PPI_CALLBACK        Assign;
+  UINT32                  LocalTokenNumber;
+  UINT32                  LocalTokenCount;
+  UINTN                   PeiNexTokenNumber;
+  UINTN                   TokenNumber;
+  UINTN                   Idx;
+  PEI_PCD_DATABASE        *PeiPcdDb;
+
+  PeiPcdDb          = GetPcdDatabase();
+  LocalTokenCount   = PeiPcdDb->LocalTokenCount;
+  PeiNexTokenNumber = PeiPcdDb->LocalTokenCount - PeiPcdDb->ExTokenCount;
+
+  if (Guid == NULL) {
+    TokenNumber = ExTokenNumber;
+    //
+    // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+    // We have to decrement TokenNumber by 1 to make it usable
+    // as the array index.
+    //
+    TokenNumber--;
+    ASSERT (TokenNumber + 1 < (PeiNexTokenNumber + 1));
+  } else {
+    TokenNumber = GetExPcdTokenNumber (Guid, ExTokenNumber);
+    if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+      return EFI_NOT_FOUND;
+    }
+    //
+    // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+    // We have to decrement TokenNumber by 1 to make it usable
+    // as the array index.
+    //
+    TokenNumber--;
+    // EBC compiler is very choosy. It may report warning about comparison
+    // between UINTN and 0 . So we add 1 in each size of the 
+    // comparison.
+    ASSERT ((TokenNumber + 1) < (LocalTokenCount + 1));
+  }
+
+
+  LocalTokenNumber = *((UINT32 *)((UINT8 *)PeiPcdDb + PeiPcdDb->LocalTokenNumberTableOffset) + TokenNumber);
+
+  //
+  // We don't support SET for HII and VPD type PCD entry in PEI phase.
+  // So we will assert if any register callback for such PCD entry.
+  //
+  ASSERT ((LocalTokenNumber & PCD_TYPE_HII) == 0);
+  ASSERT ((LocalTokenNumber & PCD_TYPE_VPD) == 0);
+
+  GuidHob = GetFirstGuidHob (&gEfiCallerIdGuid);
+  ASSERT (GuidHob != NULL);
+  
+  CallbackTable = GET_GUID_HOB_DATA (GuidHob);
+  CallbackTable = CallbackTable + (TokenNumber * PcdGet32 (PcdMaxPeiPcdCallBackNumberPerPcdEntry));
+
+  Compare = Register? NULL: CallBackFunction;
+  Assign  = Register? CallBackFunction: NULL;
+
+
+  for (Idx = 0; Idx < PcdGet32 (PcdMaxPeiPcdCallBackNumberPerPcdEntry); Idx++) {
+    if (CallbackTable[Idx] == Compare) {
+      CallbackTable[Idx] = Assign;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return Register? EFI_OUT_OF_RESOURCES : EFI_INVALID_PARAMETER;
+
+}
+
+
+/**
+  Find the Pcd database. 
+
+  @param  FileHandle  Handle of the file the external PCD database binary located.
+
+  @retval The base address of external PCD database binary.
+  @retval NULL         Return NULL if not find.
+**/
+VOID *
+LocateExPcdBinary (
+  IN EFI_PEI_FILE_HANDLE    FileHandle
+  )
+{
+  EFI_STATUS            Status;
+  VOID                  *PcdDb;
+
+  PcdDb       = NULL;
+
+  ASSERT (FileHandle != NULL);
+
+  Status = PeiServicesFfsFindSectionData (EFI_SECTION_RAW, FileHandle, &PcdDb);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Check the first bytes (Header Signature Guid) and build version.
+  //
+  if (!CompareGuid (PcdDb, &gPcdDataBaseSignatureGuid) ||
+      (((PEI_PCD_DATABASE *) PcdDb)->BuildVersion != PCD_SERVICE_PEIM_VERSION)) {
+    ASSERT (FALSE);
+  }
+  return PcdDb;
+}
+
+
+/**
+  The function builds the PCD database.
+
+  @param  FileHandle  Handle of the file the external PCD database binary located.
+
+  @return Pointer to PCD database.
+**/
+PEI_PCD_DATABASE *
+BuildPcdDatabase (
+  IN EFI_PEI_FILE_HANDLE    FileHandle
+  )
+{
+  PEI_PCD_DATABASE       *Database;
+  PEI_PCD_DATABASE       *PeiPcdDbBinary;
+  VOID                   *CallbackFnTable;
+  UINTN                  SizeOfCallbackFnTable;
+
+  //
+  // Locate the external PCD database binary for one section of current FFS
+  //
+  PeiPcdDbBinary = LocateExPcdBinary (FileHandle);
+
+  ASSERT(PeiPcdDbBinary != NULL);
+
+  Database = BuildGuidHob (&gPcdDataBaseHobGuid, PeiPcdDbBinary->Length + PeiPcdDbBinary->UninitDataBaseSize);
+
+  ZeroMem (Database, PeiPcdDbBinary->Length  + PeiPcdDbBinary->UninitDataBaseSize);
+
+  //
+  // PeiPcdDbBinary is smaller than Database
+  //
+  CopyMem (Database, PeiPcdDbBinary, PeiPcdDbBinary->Length);
+
+  SizeOfCallbackFnTable = Database->LocalTokenCount * sizeof (PCD_PPI_CALLBACK) * PcdGet32 (PcdMaxPeiPcdCallBackNumberPerPcdEntry);
+
+  CallbackFnTable = BuildGuidHob (&gEfiCallerIdGuid, SizeOfCallbackFnTable);
+  
+  ZeroMem (CallbackFnTable, SizeOfCallbackFnTable);
+
+  return Database;
+}
+
+/**
+  The function is provided by PCD PEIM and PCD DXE driver to
+  do the work of reading a HII variable from variable service.
+
+  @param VariableGuid     The Variable GUID.
+  @param VariableName     The Variable Name.
+  @param VariableData    The output data.
+  @param VariableSize    The size of the variable.
+
+  @retval EFI_SUCCESS         Operation successful.
+  @retval EFI_NOT_FOUND         Variablel not found.
+**/
+EFI_STATUS
+GetHiiVariable (
+  IN  CONST EFI_GUID      *VariableGuid,
+  IN  UINT16              *VariableName,
+  OUT VOID                **VariableData,
+  OUT UINTN               *VariableSize
+  )
+{
+  UINTN      Size;
+  EFI_STATUS Status;
+  VOID       *Buffer;
+  EFI_PEI_READ_ONLY_VARIABLE2_PPI *VariablePpi;
+
+  Status = PeiServicesLocatePpi (&gEfiPeiReadOnlyVariable2PpiGuid, 0, NULL, (VOID **) &VariablePpi);
+  ASSERT_EFI_ERROR (Status);
+
+  Size = 0;
+  Status = VariablePpi->GetVariable (
+                          VariablePpi,
+                          VariableName,
+                          (EFI_GUID *) VariableGuid,
+                          NULL,
+                          &Size,
+                          NULL
+                          );
+
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    Status = PeiServicesAllocatePool (Size, &Buffer);
+    ASSERT_EFI_ERROR (Status);
+
+    Status = VariablePpi->GetVariable (
+                              VariablePpi,
+                              (UINT16 *) VariableName,
+                              (EFI_GUID *) VariableGuid,
+                              NULL,
+                              &Size,
+                              Buffer
+                              );
+    ASSERT_EFI_ERROR (Status);
+
+    *VariableSize = Size;
+    *VariableData = Buffer;
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Find the local token number according to system SKU ID.
+
+  @param LocalTokenNumber PCD token number
+  @param Size             The size of PCD entry.
+
+  @return Token number according to system SKU ID.
+
+**/
+UINT32
+GetSkuEnabledTokenNumber (
+  UINT32 LocalTokenNumber,
+  UINTN  Size
+  ) 
+{
+  PEI_PCD_DATABASE      *PeiPcdDb;
+  SKU_HEAD              *SkuHead;
+  SKU_ID                *SkuIdTable;
+  INTN                  Index;
+  UINT8                 *Value;
+  BOOLEAN               FoundSku;
+
+  PeiPcdDb = GetPcdDatabase ();
+
+  ASSERT ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0);
+
+  SkuHead     = (SKU_HEAD *) ((UINT8 *)PeiPcdDb + (LocalTokenNumber & PCD_DATABASE_OFFSET_MASK));
+  Value       = (UINT8 *) ((UINT8 *)PeiPcdDb + (SkuHead->SkuDataStartOffset));
+  SkuIdTable  = (SKU_ID *) ((UINT8 *)PeiPcdDb + (SkuHead->SkuIdTableOffset));
+
+  //
+  // Find the current system's SKU ID entry in SKU ID table.
+  //
+  FoundSku = FALSE;
+  for (Index = 0; Index < SkuIdTable[0]; Index++) {
+    if (PeiPcdDb->SystemSkuId == SkuIdTable[Index + 1]) {
+      FoundSku = TRUE;
+      break;
+    }
+  }
+
+  //
+  // Find the default SKU ID entry in SKU ID table.
+  //
+  if(!FoundSku) {
+    for (Index = 0; Index < SkuIdTable[0]; Index++) {
+      if (0 == SkuIdTable[Index + 1]) {
+        break;
+      }
+    }
+  }
+  ASSERT (Index < SkuIdTable[0]);
+
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+      Value = (UINT8 *) &(((VPD_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - (UINT8 *) PeiPcdDb) | PCD_TYPE_VPD);
+
+    case PCD_TYPE_HII:
+      Value = (UINT8 *) &(((VARIABLE_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - (UINT8 *) PeiPcdDb) | PCD_TYPE_HII);
+
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+      Value = (UINT8 *) &(((VARIABLE_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - (UINT8 *) PeiPcdDb) | PCD_TYPE_HII | PCD_TYPE_STRING);
+
+    case PCD_TYPE_STRING:
+      Value = (UINT8 *) &(((STRING_HEAD *) Value)[Index]);
+      return (UINT32) ((Value - (UINT8 *) PeiPcdDb) | PCD_TYPE_STRING);
+
+    case PCD_TYPE_DATA:
+      Value += Size * Index;
+      return (UINT32) ((Value - (UINT8 *) PeiPcdDb) | PCD_TYPE_DATA);
+
+    default:
+      ASSERT (FALSE);
+  }
+
+  ASSERT (FALSE);
+
+  return 0;
+}
+
+/**
+  Invoke the callback function when dynamic PCD entry was set, if this PCD entry 
+  has registered callback function.
+
+  @param ExTokenNumber   DynamicEx PCD's token number, if this PCD entry is dyanmicEx
+                         type PCD.
+  @param Guid            DynamicEx PCD's guid, if this PCD entry is dynamicEx type
+                         PCD.
+  @param TokenNumber     PCD token number generated by build tools.
+  @param Data            Value want to be set for this PCD entry
+  @param Size            The size of value
+
+**/
+VOID
+InvokeCallbackOnSet (
+  UINTN             ExTokenNumber,
+  CONST EFI_GUID    *Guid, OPTIONAL
+  UINTN             TokenNumber,
+  VOID              *Data,
+  UINTN             Size
+  )
+{
+  EFI_HOB_GUID_TYPE   *GuidHob;
+  PCD_PPI_CALLBACK    *CallbackTable;
+  UINTN               Idx;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  UINT32              LocalTokenCount;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  PeiPcdDb        = GetPcdDatabase ();
+  LocalTokenCount = PeiPcdDb->LocalTokenCount;
+
+  if (Guid == NULL) {
+    // EBC compiler is very choosy. It may report warning about comparison
+    // between UINTN and 0 . So we add 1 in each size of the 
+    // comparison.
+    ASSERT (TokenNumber + 1 < (LocalTokenCount + 1));
+  }
+
+  GuidHob = GetFirstGuidHob (&gEfiCallerIdGuid);
+  ASSERT (GuidHob != NULL);
+  
+  CallbackTable = GET_GUID_HOB_DATA (GuidHob);
+
+  CallbackTable += (TokenNumber * PcdGet32 (PcdMaxPeiPcdCallBackNumberPerPcdEntry));
+
+  for (Idx = 0; Idx < PcdGet32 (PcdMaxPeiPcdCallBackNumberPerPcdEntry); Idx++) {
+    if (CallbackTable[Idx] != NULL) {
+      CallbackTable[Idx] (Guid,
+                          (Guid == NULL) ? (TokenNumber + 1) : ExTokenNumber,
+                          Data,
+                          Size
+                          );
+    }
+  }
+}
+
+/**
+  Wrapper function for setting non-pointer type value for a PCD entry.
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+
+  @return status of SetWorker.
+
+**/
+EFI_STATUS
+SetValueWorker (
+  IN          UINTN              TokenNumber,
+  IN          VOID               *Data,
+  IN          UINTN              Size
+  )
+{
+  return SetWorker (TokenNumber, Data, &Size, FALSE);
+}
+
+/**
+  Set value for an PCD entry
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+  @param PtrType         If TRUE, the type of PCD entry's value is Pointer.
+                         If False, the type of PCD entry's value is not Pointer.
+
+  @retval EFI_INVALID_PARAMETER  If this PCD type is VPD, VPD PCD can not be set.
+  @retval EFI_INVALID_PARAMETER  If Size can not be set to size table.
+  @retval EFI_INVALID_PARAMETER  If Size of non-Ptr type PCD does not match the size information in PCD database.
+  @retval EFI_NOT_FOUND          If value type of PCD entry is intergrate, but not in
+                                 range of UINT8, UINT16, UINT32, UINT64
+  @retval EFI_NOT_FOUND          Can not find the PCD type according to token number.                                
+**/
+EFI_STATUS
+SetWorker (
+  IN          UINTN               TokenNumber,
+  IN          VOID                *Data,
+  IN OUT      UINTN               *Size,
+  IN          BOOLEAN             PtrType
+  )
+{
+  UINT32              LocalTokenNumber;
+  UINTN               PeiNexTokenNumber;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  STRING_HEAD         StringTableIdx;
+  UINTN               Offset;
+  VOID                *InternalData;
+  UINTN               MaxSize;
+  UINT32              LocalTokenCount;
+
+  if (!FeaturePcdGet(PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+  
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+  PeiPcdDb        = GetPcdDatabase ();
+  LocalTokenCount = PeiPcdDb->LocalTokenCount;
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  ASSERT (TokenNumber + 1 < (LocalTokenCount + 1));
+
+  if (PtrType) {
+    //
+    // Get MaxSize first, then check new size with max buffer size.
+    //
+    GetPtrTypeSize (TokenNumber, &MaxSize, PeiPcdDb);
+    if (*Size > MaxSize) {
+      *Size = MaxSize;
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    if (*Size != PeiPcdGetSize (TokenNumber + 1)) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // We only invoke the callback function for Dynamic Type PCD Entry.
+  // For Dynamic EX PCD entry, we have invoked the callback function for Dynamic EX
+  // type PCD entry in ExSetWorker.
+  //
+  PeiNexTokenNumber = PeiPcdDb->LocalTokenCount - PeiPcdDb->ExTokenCount;
+  if (TokenNumber + 1 < PeiNexTokenNumber + 1) {
+    InvokeCallbackOnSet (0, NULL, TokenNumber + 1, Data, *Size);
+  }
+
+  LocalTokenNumber = GetLocalTokenNumber (PeiPcdDb, TokenNumber + 1);
+
+  Offset          = LocalTokenNumber & PCD_DATABASE_OFFSET_MASK;
+  InternalData    = (VOID *) ((UINT8 *) PeiPcdDb + Offset);
+  
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+    case PCD_TYPE_HII:
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+    {
+      ASSERT (FALSE);
+      return EFI_INVALID_PARAMETER;
+    }
+
+    case PCD_TYPE_STRING:
+      if (SetPtrTypeSize (TokenNumber, Size, PeiPcdDb)) {
+        StringTableIdx = *((STRING_HEAD *)InternalData);
+        CopyMem ((UINT8 *)PeiPcdDb + PeiPcdDb->StringTableOffset + StringTableIdx, Data, *Size);
+        return EFI_SUCCESS;
+      } else {
+        return EFI_INVALID_PARAMETER;
+      }
+
+    case PCD_TYPE_DATA:
+    {
+      if (PtrType) {
+        if (SetPtrTypeSize (TokenNumber, Size, PeiPcdDb)) {
+          CopyMem (InternalData, Data, *Size);
+          return EFI_SUCCESS;
+        } else {
+          return EFI_INVALID_PARAMETER;
+        }
+      }
+
+      switch (*Size) {
+        case sizeof(UINT8):
+          *((UINT8 *) InternalData) = *((UINT8 *) Data);
+          return EFI_SUCCESS;
+
+        case sizeof(UINT16):
+          *((UINT16 *) InternalData) = *((UINT16 *) Data);
+          return EFI_SUCCESS;
+
+        case sizeof(UINT32):
+          *((UINT32 *) InternalData) = *((UINT32 *) Data);
+          return EFI_SUCCESS;
+
+        case sizeof(UINT64):
+          *((UINT64 *) InternalData) = *((UINT64 *) Data);
+          return EFI_SUCCESS;
+
+        default:
+          ASSERT (FALSE);
+          return EFI_NOT_FOUND;
+      }
+    }
+      
+  }
+
+  ASSERT (FALSE);
+  return EFI_NOT_FOUND;
+
+}
+
+/**
+  Wrapper function for set PCD value for non-Pointer type dynamic-ex PCD.
+
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            Value want to be set.
+  @param SetSize         The size of value.
+
+  @return status of ExSetWorker().
+
+**/
+EFI_STATUS
+ExSetValueWorker (
+  IN          UINTN                ExTokenNumber,
+  IN          CONST EFI_GUID       *Guid,
+  IN          VOID                 *Data,
+  IN          UINTN                Size
+  )
+{
+  return ExSetWorker (ExTokenNumber, Guid, Data, &Size, FALSE);
+}
+
+/**
+  Set value for a dynamic-ex PCD entry.
+  
+  This routine find the local token number according to dynamic-ex PCD's token 
+  space guid and token number firstly, and invoke callback function if this PCD
+  entry registered callback function. Finally, invoken general SetWorker to set
+  PCD value.
+  
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            PCD value want to be set
+  @param SetSize         Size of value.
+  @param PtrType         If TRUE, this PCD entry is pointer type.
+                         If FALSE, this PCD entry is not pointer type.
+
+  @return status of SetWorker().
+
+**/
+EFI_STATUS
+ExSetWorker (
+  IN            UINTN                ExTokenNumber,
+  IN            CONST EFI_GUID       *Guid,
+  IN            VOID                 *Data,
+  IN OUT        UINTN                *Size,
+  IN            BOOLEAN              PtrType
+  )
+{
+  UINTN                     TokenNumber;
+
+  if (!FeaturePcdGet(PcdPeiFullPcdDatabaseEnable)) {
+    return EFI_UNSUPPORTED;
+  }
+
+  TokenNumber = GetExPcdTokenNumber (Guid, ExTokenNumber);
+  if (TokenNumber == PCD_INVALID_TOKEN_NUMBER) {
+    return EFI_NOT_FOUND;
+  }
+  
+  InvokeCallbackOnSet (ExTokenNumber, Guid, TokenNumber, Data, *Size);
+
+  return SetWorker (TokenNumber, Data, Size, PtrType);
+
+}
+
+/**
+  Wrapper function for get PCD value for dynamic-ex PCD.
+
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param ExTokenNumber   Token number for dyanmic-ex PCD.
+  @param GetSize         The size of dynamic-ex PCD value.
+
+  @return PCD entry in PCD database.
+
+**/
+VOID *
+ExGetWorker (
+  IN CONST  EFI_GUID  *Guid,
+  IN UINTN            ExTokenNumber,
+  IN UINTN            GetSize
+  )
+{ 
+  return GetWorker (GetExPcdTokenNumber (Guid, ExTokenNumber), GetSize);
+}
+
+/**
+  Get the PCD entry pointer in PCD database.
+  
+  This routine will visit PCD database to find the PCD entry according to given
+  token number. The given token number is autogened by build tools and it will be 
+  translated to local token number. Local token number contains PCD's type and 
+  offset of PCD entry in PCD database.
+
+  @param TokenNumber     Token's number, it is autogened by build tools
+  @param GetSize         The size of token's value
+
+  @return PCD entry pointer in PCD database
+
+**/
+VOID *
+GetWorker (
+  IN UINTN               TokenNumber,
+  IN UINTN               GetSize
+  )
+{
+  UINT32              Offset;
+  EFI_GUID            *Guid;
+  UINT16              *Name;
+  VARIABLE_HEAD       *VariableHead;
+  EFI_STATUS          Status;
+  UINTN               DataSize;
+  VOID                *Data;
+  UINT8               *StringTable;
+  STRING_HEAD         StringTableIdx;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+  UINT32              LocalTokenNumber;
+  UINT32              LocalTokenCount;
+  UINT8               *VaraiableDefaultBuffer;
+
+  //
+  // TokenNumber Zero is reserved as PCD_INVALID_TOKEN_NUMBER.
+  // We have to decrement TokenNumber by 1 to make it usable
+  // as the array index.
+  //
+  TokenNumber--;
+
+  PeiPcdDb        = GetPcdDatabase ();
+  LocalTokenCount = PeiPcdDb->LocalTokenCount;
+
+  // EBC compiler is very choosy. It may report warning about comparison
+  // between UINTN and 0 . So we add 1 in each size of the 
+  // comparison.
+  ASSERT (TokenNumber + 1 < (LocalTokenCount + 1));
+
+  ASSERT ((GetSize == PeiPcdGetSize(TokenNumber + 1)) || (GetSize == 0));
+
+  LocalTokenNumber = GetLocalTokenNumber (PeiPcdDb, TokenNumber + 1);
+
+  Offset      = LocalTokenNumber & PCD_DATABASE_OFFSET_MASK;
+  StringTable = (UINT8 *)PeiPcdDb + PeiPcdDb->StringTableOffset;
+
+  switch (LocalTokenNumber & PCD_TYPE_ALL_SET) {
+    case PCD_TYPE_VPD:
+    {
+      VPD_HEAD *VpdHead;
+      VpdHead = (VPD_HEAD *) ((UINT8 *)PeiPcdDb + Offset);
+      return (VOID *) (UINTN) (PcdGet32 (PcdVpdBaseAddress) + VpdHead->Offset);
+    }
+      
+    case PCD_TYPE_HII|PCD_TYPE_STRING:
+    case PCD_TYPE_HII:
+    {
+      VariableHead = (VARIABLE_HEAD *) ((UINT8 *)PeiPcdDb + Offset);
+      
+      Guid = (EFI_GUID *) ((UINT8 *)PeiPcdDb + PeiPcdDb->GuidTableOffset) + VariableHead->GuidTableIndex;
+      Name = (UINT16*)&StringTable[VariableHead->StringIndex];
+
+      if ((LocalTokenNumber & PCD_TYPE_ALL_SET) == (PCD_TYPE_HII|PCD_TYPE_STRING)) {
+        //
+        // If a HII type PCD's datum type is VOID*, the DefaultValueOffset is the index of 
+        // string array in string table.
+        //
+        VaraiableDefaultBuffer = (UINT8 *) &StringTable[*(STRING_HEAD*)((UINT8*) PeiPcdDb + VariableHead->DefaultValueOffset)];   
+      } else {
+        VaraiableDefaultBuffer = (UINT8 *) PeiPcdDb + VariableHead->DefaultValueOffset;
+      }
+      Status = GetHiiVariable (Guid, Name, &Data, &DataSize);
+      if ((Status == EFI_SUCCESS) && (DataSize >= (VariableHead->Offset + GetSize))) {
+        if (GetSize == 0) {
+          //
+          // It is a pointer type. So get the MaxSize reserved for
+          // this PCD entry.
+          //
+          GetPtrTypeSize (TokenNumber, &GetSize, PeiPcdDb);
+          if (GetSize > (DataSize - VariableHead->Offset)) {
+            //
+            // Use actual valid size.
+            //
+            GetSize = DataSize - VariableHead->Offset;
+          }
+        }
+        //
+        // If the operation is successful, we copy the data
+        // to the default value buffer in the PCD Database.
+        //
+        CopyMem (VaraiableDefaultBuffer, (UINT8 *) Data + VariableHead->Offset, GetSize);
+      }
+      return (VOID *) VaraiableDefaultBuffer;
+    }
+
+    case PCD_TYPE_DATA:
+      return (VOID *) ((UINT8 *)PeiPcdDb + Offset);
+
+    case PCD_TYPE_STRING:
+      StringTableIdx = * (STRING_HEAD*) ((UINT8 *) PeiPcdDb + Offset);
+      return (VOID *) (&StringTable[StringTableIdx]);
+
+    default:
+      ASSERT (FALSE);
+      break;
+      
+  }
+
+  ASSERT (FALSE);
+      
+  return NULL;
+  
+}
+
+/**
+  Get Token Number according to dynamic-ex PCD's {token space guid:token number}
+
+  A dynamic-ex type PCD, developer must provide pair of token space guid: token number
+  in DEC file. PCD database maintain a mapping table that translate pair of {token
+  space guid: token number} to Token Number.
+  
+  @param Guid            Token space guid for dynamic-ex PCD entry.
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+
+  @return Token Number for dynamic-ex PCD.
+
+**/
+UINTN           
+GetExPcdTokenNumber (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  )
+{
+  UINT32              Index;
+  DYNAMICEX_MAPPING   *ExMap;
+  EFI_GUID            *GuidTable;
+  EFI_GUID            *MatchGuid;
+  UINTN               MatchGuidIdx;
+  PEI_PCD_DATABASE    *PeiPcdDb;
+
+  PeiPcdDb    = GetPcdDatabase();
+
+  ExMap       = (DYNAMICEX_MAPPING *)((UINT8 *)PeiPcdDb + PeiPcdDb->ExMapTableOffset);
+  GuidTable   = (EFI_GUID *)((UINT8 *)PeiPcdDb + PeiPcdDb->GuidTableOffset);
+
+  MatchGuid = ScanGuid (GuidTable, PeiPcdDb->GuidTableCount * sizeof(EFI_GUID), Guid);
+  //
+  // We need to ASSERT here. If GUID can't be found in GuidTable, this is a
+  // error in the BUILD system.
+  //
+  ASSERT (MatchGuid != NULL);
+  
+  MatchGuidIdx = MatchGuid - GuidTable;
+  
+  for (Index = 0; Index < PeiPcdDb->ExTokenCount; Index++) {
+    if ((ExTokenNumber == ExMap[Index].ExTokenNumber) && 
+        (MatchGuidIdx == ExMap[Index].ExGuidIndex)) {
+      return ExMap[Index].TokenNumber;
+    }
+  }
+  
+  return PCD_INVALID_TOKEN_NUMBER;
+}
+
+/**
+  Get PCD database from GUID HOB in PEI phase.
+
+  @return Pointer to PCD database.
+
+**/
+PEI_PCD_DATABASE *
+GetPcdDatabase (
+  VOID
+  )
+{
+  EFI_HOB_GUID_TYPE *GuidHob;
+
+  GuidHob = GetFirstGuidHob (&gPcdDataBaseHobGuid);
+  ASSERT (GuidHob != NULL);
+  
+  return (PEI_PCD_DATABASE *) GET_GUID_HOB_DATA (GuidHob);
+}
+
+/**
+  Get SKU ID table from PCD database.
+
+  @param LocalTokenNumberTableIdx Index of local token number in token number table.
+  @param Database                 PCD database.
+
+  @return Pointer to SKU ID array table
+
+**/
+SKU_ID *
+GetSkuIdArray (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    PEI_PCD_DATABASE  *Database
+  )
+{
+  SKU_HEAD *SkuHead;
+  UINTN     LocalTokenNumber;
+
+  LocalTokenNumber = *((UINT32 *)((UINT8 *)Database + Database->LocalTokenNumberTableOffset) + LocalTokenNumberTableIdx);
+
+  ASSERT ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) != 0);
+
+  SkuHead = (SKU_HEAD *) ((UINT8 *)Database + (LocalTokenNumber & PCD_DATABASE_OFFSET_MASK));
+
+  return (SKU_ID *) ((UINT8 *)Database + SkuHead->SkuIdTableOffset);
+  
+}
+
+/**
+  Get index of PCD entry in size table.
+
+  @param LocalTokenNumberTableIdx Index of this PCD in local token number table.
+  @param Database                 Pointer to PCD database in PEI phase.
+
+  @return index of PCD entry in size table.
+
+**/
+UINTN
+GetSizeTableIndex (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    PEI_PCD_DATABASE  *Database
+  )
+{
+  UINTN       Index;
+  UINTN       SizeTableIdx;
+  UINTN       LocalTokenNumber;
+  SKU_ID      *SkuIdTable;
+  
+  SizeTableIdx = 0;
+
+  for (Index = 0; Index < LocalTokenNumberTableIdx; Index++) {
+    LocalTokenNumber = *((UINT32 *)((UINT8 *)Database + Database->LocalTokenNumberTableOffset) + Index);
+
+    if ((LocalTokenNumber & PCD_DATUM_TYPE_ALL_SET) == PCD_DATUM_TYPE_POINTER) {
+      //
+      // SizeTable only contain record for PCD_DATUM_TYPE_POINTER type 
+      // PCD entry.
+      //
+      if ((LocalTokenNumber & PCD_TYPE_VPD) != 0) {
+          //
+          // We have only two entry for VPD enabled PCD entry:
+          // 1) MAX Size.
+          // 2) Current Size
+          // Current size is equal to MAX size.
+          //
+          SizeTableIdx += 2;
+      } else {
+        if ((LocalTokenNumber & PCD_TYPE_SKU_ENABLED) == 0) {
+          //
+          // We have only two entry for Non-Sku enabled PCD entry:
+          // 1) MAX SIZE
+          // 2) Current Size
+          //
+          SizeTableIdx += 2;
+        } else {
+          //
+          // We have these entry for SKU enabled PCD entry
+          // 1) MAX SIZE
+          // 2) Current Size for each SKU_ID (It is equal to MaxSku).
+          //
+          SkuIdTable = GetSkuIdArray (Index, Database);
+          SizeTableIdx += (UINTN)*SkuIdTable + 1;
+        }
+      }
+    }
+
+  }
+
+  return SizeTableIdx;
+}
diff --git a/Core/MdeModulePkg/Universal/PCD/Pei/Service.h b/Core/MdeModulePkg/Universal/PCD/Pei/Service.h
new file mode 100644
index 0000000000..c75187c2b1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PCD/Pei/Service.h
@@ -0,0 +1,1117 @@
+/** @file
+  The internal header file declares the private functions used by PeiPcd driver.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_PCD_SERVICE_H_
+#define _PEI_PCD_SERVICE_H_
+
+#include <PiPei.h>
+#include <Ppi/ReadOnlyVariable2.h>
+#include <Ppi/Pcd.h>
+#include <Ppi/PiPcd.h>
+#include <Ppi/PcdInfo.h>
+#include <Ppi/PiPcdInfo.h>
+#include <Guid/PcdDataBaseHobGuid.h>
+#include <Guid/PcdDataBaseSignatureGuid.h>
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/HobLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+//
+// Please make sure the PCD Serivce PEIM Version is consistent with
+// the version of the generated PEIM PCD Database by build tool.
+//
+#define PCD_SERVICE_PEIM_VERSION      5
+
+//
+// PCD_PEI_SERVICE_DRIVER_VERSION is defined in Autogen.h.
+//
+#if (PCD_SERVICE_PEIM_VERSION != PCD_PEI_SERVICE_DRIVER_VERSION)
+  #error "Please make sure the version of PCD PEIM Service and the generated PCD PEI Database match."
+#endif
+
+/**
+  Retrieve additional information associated with a PCD token in the default token space.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+PeiGetPcdInfoGetInfo (
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName.
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully.
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+EFIAPI
+PeiGetPcdInfoGetInfoEx (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/**
+  Retrieve the currently set SKU Id.
+
+  @return   The currently set SKU Id. If the platform has not set at a SKU Id, then the
+            default SKU Id value of 0 is returned. If the platform has set a SKU Id, then the currently set SKU
+            Id is returned.
+**/
+UINTN
+EFIAPI
+PeiGetPcdInfoGetSku (
+  VOID
+  );
+
+//
+// PPI Interface Implementation Declaration.
+//
+
+/**
+  Sets the SKU value for subsequent calls to set or get PCD token values.
+
+  SetSku() sets the SKU Id to be used for subsequent calls to set or get PCD values. 
+  SetSku() is normally called only once by the system.
+
+  For each item (token), the database can hold a single value that applies to all SKUs, 
+  or multiple values, where each value is associated with a specific SKU Id. Items with multiple, 
+  SKU-specific values are called SKU enabled. 
+  
+  The SKU Id of zero is reserved as a default. The valid SkuId range is 1 to 255.  
+  For tokens that are not SKU enabled, the system ignores any set SKU Id and works with the 
+  single value for that token. For SKU-enabled tokens, the system will use the SKU Id set by the 
+  last call to SetSku(). If no SKU Id is set or the currently set SKU Id isn't valid for the specified token, 
+  the system uses the default SKU Id. If the system attempts to use the default SKU Id and no value has been 
+  set for that Id, the results are unpredictable.
+
+  @param[in]  SkuId The SKU value that will be used when the PCD service will retrieve and 
+              set values associated with a PCD token.
+
+**/
+VOID
+EFIAPI
+PeiPcdSetSku (
+  IN  UINTN                  SkuId
+  );
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the current byte-sized value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT8 value.
+  
+**/
+UINT8
+EFIAPI
+PeiPcdGet8 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the current 16-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT16 value.
+  
+**/
+UINT16
+EFIAPI
+PeiPcdGet16 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the current 32-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT32 value.
+  
+**/
+UINT32
+EFIAPI
+PeiPcdGet32 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the current 64-bits value for a PCD token number.  
+  If the TokenNumber is invalid, the results are unpredictable.
+  
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The UINT64 value.
+  
+**/
+UINT64
+EFIAPI
+PeiPcdGet64 (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrived.
+  
+**/
+VOID *
+EFIAPI
+PeiPcdGetPtr (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a Boolean value for a given PCD token.
+
+  Retrieves the current boolean value for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The Boolean value.
+  
+**/
+BOOLEAN
+EFIAPI
+PeiPcdGetBool (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+PeiPcdGetSize (
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 8-bit value for a given PCD token.
+
+  Retrieves the 8-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 8-bit value for the PCD token.
+  
+**/
+UINT8
+EFIAPI
+PeiPcdGet8Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 16-bit value for a given PCD token.
+
+  Retrieves the 16-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 16-bit value for the PCD token.
+  
+**/
+UINT16
+EFIAPI
+PeiPcdGet16Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 32-bit value for a given PCD token.
+
+  Retrieves the 32-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 32-bit value for the PCD token.
+  
+**/
+UINT32
+EFIAPI
+PeiPcdGet32Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an 64-bit value for a given PCD token.
+
+  Retrieves the 64-bit value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size 64-bit value for the PCD token.
+  
+**/
+UINT64
+EFIAPI
+PeiPcdGet64Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves a pointer to a value for a given PCD token.
+
+  Retrieves the current pointer to the buffer for a PCD token number.  
+  Do not make any assumptions about the alignment of the pointer that 
+  is returned by this function call.  If the TokenNumber is invalid, 
+  the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The pointer to the buffer to be retrived.
+  
+**/
+VOID *
+EFIAPI
+PeiPcdGetPtrEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves an Boolean value for a given PCD token.
+
+  Retrieves the Boolean value of a particular PCD token.  
+  If the TokenNumber is invalid or the token space
+  specified by Guid does not exist, the results are 
+  unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size Boolean value for the PCD token.
+  
+**/
+BOOLEAN
+EFIAPI
+PeiPcdGetBoolEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Retrieves the size of the value for a given PCD token.
+
+  Retrieves the current size of a particular PCD token.  
+  If the TokenNumber is invalid, the results are unpredictable.
+
+  @param[in]  Guid The token space for the token number.
+  @param[in]  TokenNumber The PCD token number. 
+
+  @return The size of the value for the PCD token.
+  
+**/
+UINTN
+EFIAPI
+PeiPcdGetSizeEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber
+  );
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet8 (
+  IN UINTN             TokenNumber,
+  IN UINT8             Value
+  );
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet16 (
+  IN UINTN             TokenNumber,
+  IN UINT16            Value
+  );
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet32 (
+  IN UINTN             TokenNumber,
+  IN UINT32            Value
+  );
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet64 (
+  IN UINTN             TokenNumber,
+  IN UINT64            Value
+  );
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetPtr (
+  IN        UINTN             TokenNumber,
+  IN OUT    UINTN             *SizeOfBuffer,
+  IN        VOID              *Buffer
+  );
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetBool (
+  IN UINTN             TokenNumber,
+  IN BOOLEAN           Value
+  );
+
+/**
+  Sets an 8-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet8Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT8             Value
+  );
+
+/**
+  Sets an 16-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet16Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT16            Value
+  );
+
+/**
+  Sets an 32-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet32Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT32            Value
+  );
+
+/**
+  Sets an 64-bit value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSet64Ex (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN UINT64            Value
+  );
+
+/**
+  Sets a value of a specified size for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in, out] SizeOfBuffer A pointer to the length of the value being set for the PCD token.  
+                              On input, if the SizeOfValue is greater than the maximum size supported 
+                              for this TokenNumber then the output value of SizeOfValue will reflect 
+                              the maximum size supported for this TokenNumber.
+  @param[in]  Buffer The buffer to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetPtrEx (
+  IN        CONST EFI_GUID    *Guid,
+  IN        UINTN             TokenNumber,
+  IN OUT    UINTN             *SizeOfBuffer,
+  IN        VOID              *Buffer
+  );
+
+/**
+  Sets an Boolean value for a given PCD token.
+
+  When the PCD service sets a value, it will check to ensure that the 
+  size of the value being set is compatible with the Token's existing definition.  
+  If it is not, an error will be returned.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  Value The value to set for the PCD token.
+
+  @retval EFI_SUCCESS  Procedure returned successfully.
+  @retval EFI_INVALID_PARAMETER The PCD service determined that the size of the data 
+                                  being set was incompatible with a call to this function.  
+                                  Use GetSize() to retrieve the size of the target data.
+  @retval EFI_NOT_FOUND The PCD service could not find the requested token number.
+  
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdSetBoolEx (
+  IN CONST EFI_GUID    *Guid,
+  IN UINTN             TokenNumber,
+  IN BOOLEAN           Value
+  );
+
+/**
+  Specifies a function to be called anytime the value of a designated token is changed.
+
+  @param[in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]  TokenNumber The PCD token number. 
+  @param[in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PPI_CALLBACK        CallBackFunction
+  );
+
+/**
+  Cancels a previously set callback function for a particular PCD token number.
+
+  @param [in]  Guid The 128-bit unique value that designates the namespace from which to extract the value.
+  @param [in]  TokenNumber The PCD token number. 
+  @param [in]  CallBackFunction The function prototype called when the value associated with the CallBackToken is set.  
+
+  @retval EFI_SUCCESS  The PCD service has successfully established a call event 
+                        for the CallBackToken requested.
+  @retval EFI_NOT_FOUND The PCD service could not find the referenced token number.
+
+**/
+EFI_STATUS
+EFIAPI
+PcdUnRegisterCallBackOnSet (
+  IN  CONST EFI_GUID          *Guid, OPTIONAL
+  IN  UINTN                   TokenNumber,
+  IN  PCD_PPI_CALLBACK        CallBackFunction
+  );
+
+/**
+  Retrieves the next valid token number in a given namespace.  
+  
+  This is useful since the PCD infrastructure contains a sparse list of token numbers, 
+  and one cannot a priori know what token numbers are valid in the database. 
+  
+  If TokenNumber is 0 and Guid is not NULL, then the first token from the token space specified by Guid is returned.  
+  If TokenNumber is not 0 and Guid is not NULL, then the next token in the token space specified by Guid is returned.  
+  If TokenNumber is 0 and Guid is NULL, then the first token in the default token space is returned.  
+  If TokenNumber is not 0 and Guid is NULL, then the next token in the default token space is returned.  
+  The token numbers in the default token space may not be related to token numbers in token spaces that are named by Guid.  
+  If the next token number can be retrieved, then it is returned in TokenNumber, and EFI_SUCCESS is returned.  
+  If TokenNumber represents the last token number in the token space specified by Guid, then EFI_NOT_FOUND is returned.  
+  If TokenNumber is not present in the token space specified by Guid, then EFI_NOT_FOUND is returned.
+
+
+  @param[in]       Guid        The 128-bit unique value that designates the namespace from which to extract the value.  
+                               This is an optional parameter that may be NULL.  If this parameter is NULL, then a request 
+                               is being made to retrieve tokens from the default token space.
+  @param[in, out]  TokenNumber A pointer to the PCD token number to use to find the subsequent token number.
+                   
+  @retval EFI_SUCCESS   The PCD service has retrieved the next valid token number. 
+                        Or the input token number is already the last valid token number in the PCD database. 
+                        In the later case, *TokenNumber is updated with the value of 0.
+  @retval EFI_NOT_FOUND If this input token number and token namespace does not exist on the platform.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdGetNextToken (
+  IN CONST EFI_GUID           *Guid, OPTIONAL
+  IN OUT  UINTN               *TokenNumber
+  );
+
+/**
+  Retrieves the next valid PCD token namespace for a given namespace.
+
+  @param[in, out]  Guid An indirect pointer to EFI_GUID.  On input it designates 
+                    a known token namespace from which the search will start. On output, 
+                    it designates the next valid token namespace on the platform. If the input 
+                    token namespace does not exist on the platform, an error is returned and 
+                    the value of *Guid is undefined. If *Guid is NULL, then the GUID of the 
+                    first token space of the current platform is assigned to *Guid the function 
+                    return EFI_SUCCESS. If  *Guid is NULL  and there is no namespace exist in 
+                    the platform other than the default (NULL) tokennamespace, *Guid is unchanged 
+                    and the function return EFI_SUCCESS. If this input token namespace is the last 
+                    namespace on the platform, *Guid will be assigned to NULL and the function return 
+                    EFI_SUCCESS. 
+
+  @retval EFI_SUCCESS  The PCD service retrieved the next valid token space Guid. 
+                        Or the input token space Guid is already the last valid token space Guid 
+                        in the PCD database. In the later case, *Guid is updated with the value of NULL.
+  @retval EFI_NOT_FOUND If the input token namespace does not exist on the platform.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiPcdGetNextTokenSpace (
+  IN OUT CONST EFI_GUID           **Guid
+  );
+
+/**
+  Retrieve additional information associated with a PCD token.
+
+  This includes information such as the type of value the TokenNumber is associated with as well as possible
+  human readable name that is associated with the token.
+
+  @param[in]    Guid        The 128-bit unique value that designates the namespace from which to extract the value.
+  @param[in]    TokenNumber The PCD token number.
+  @param[out]   PcdInfo     The returned information associated with the requested TokenNumber.
+                            The caller is responsible for freeing the buffer that is allocated by callee for PcdInfo->PcdName. 
+
+  @retval  EFI_SUCCESS      The PCD information was returned successfully
+  @retval  EFI_NOT_FOUND    The PCD service could not find the requested token number.
+**/
+EFI_STATUS
+PeiGetPcdInfo (
+  IN CONST  EFI_GUID        *Guid,
+  IN        UINTN           TokenNumber,
+  OUT       EFI_PCD_INFO    *PcdInfo
+  );
+
+/* Internal Function definitions */
+/**
+  Get PCD database from GUID HOB in PEI phase.
+
+  @return Pointer to PCD database.
+
+**/
+PEI_PCD_DATABASE *
+GetPcdDatabase (
+  VOID
+  );
+
+/**
+  Wrapper function for setting non-pointer type value for a PCD entry.
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+
+  @return status of SetWorker.
+
+**/
+EFI_STATUS
+SetValueWorker (
+  IN          UINTN              TokenNumber,
+  IN          VOID               *Data,
+  IN          UINTN              Size
+  );
+
+/**
+  Set value for an PCD entry
+
+  @param TokenNumber     Pcd token number autogenerated by build tools.
+  @param Data            Value want to be set for PCD entry
+  @param Size            Size of value.
+  @param PtrType         If TRUE, the type of PCD entry's value is Pointer.
+                         If False, the type of PCD entry's value is not Pointer.
+
+  @retval EFI_INVALID_PARAMETER  If this PCD type is VPD, VPD PCD can not be set.
+  @retval EFI_INVALID_PARAMETER  If Size can not be set to size table.
+  @retval EFI_INVALID_PARAMETER  If Size of non-Ptr type PCD does not match the size information in PCD database.
+  @retval EFI_NOT_FOUND          If value type of PCD entry is intergrate, but not in
+                                 range of UINT8, UINT16, UINT32, UINT64
+  @retval EFI_NOT_FOUND          Can not find the PCD type according to token number.                                
+**/
+EFI_STATUS
+SetWorker (
+  IN          UINTN              TokenNumber,
+  IN          VOID               *Data,
+  IN OUT      UINTN              *Size,
+  IN          BOOLEAN            PtrType
+  );
+
+/**
+  Wrapper function for set PCD value for non-Pointer type dynamic-ex PCD.
+
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            Value want to be set.
+  @param SetSize         The size of value.
+
+  @return status of ExSetWorker().
+
+**/
+EFI_STATUS
+ExSetValueWorker (
+  IN          UINTN                ExTokenNumber,
+  IN          CONST EFI_GUID       *Guid,
+  IN          VOID                 *Data,
+  IN          UINTN                Size
+  );
+
+/**
+  Set value for a dynamic PCD entry.
+  
+  This routine find the local token number according to dynamic-ex PCD's token 
+  space guid and token number firstly, and invoke callback function if this PCD
+  entry registered callback function. Finally, invoken general SetWorker to set
+  PCD value.
+  
+  @param ExTokenNumber   Dynamic-ex PCD token number.
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param Data            PCD value want to be set
+  @param SetSize         Size of value.
+  @param PtrType         If TRUE, this PCD entry is pointer type.
+                         If FALSE, this PCD entry is not pointer type.
+
+  @return status of SetWorker().
+
+**/
+EFI_STATUS
+ExSetWorker (
+  IN      UINTN                ExTokenNumber,
+  IN      CONST EFI_GUID       *Guid,
+  IN      VOID                 *Data,
+  IN OUT  UINTN                *Size,
+  IN      BOOLEAN              PtrType
+  );
+
+/**
+  Get the PCD entry pointer in PCD database.
+  
+  This routine will visit PCD database to find the PCD entry according to given
+  token number. The given token number is autogened by build tools and it will be 
+  translated to local token number. Local token number contains PCD's type and 
+  offset of PCD entry in PCD database.
+
+  @param TokenNumber     Token's number, it is autogened by build tools
+  @param GetSize         The size of token's value
+
+  @return PCD entry pointer in PCD database
+
+**/
+VOID *
+GetWorker (
+  IN UINTN                TokenNumber,
+  IN UINTN                GetSize
+  );
+
+/**
+  Wrapper function for get PCD value for dynamic-ex PCD.
+
+  @param Guid            Token space guid for dynamic-ex PCD.
+  @param ExTokenNumber   Token number for dyanmic-ex PCD.
+  @param GetSize         The size of dynamic-ex PCD value.
+
+  @return PCD entry in PCD database.
+
+**/
+VOID *
+ExGetWorker (
+  IN CONST EFI_GUID   *Guid,
+  IN UINTN            ExTokenNumber,
+  IN UINTN            GetSize
+  );
+
+typedef struct {
+  UINTN   TokenNumber;
+  UINTN   Size;
+  UINT32  LocalTokenNumberAlias;
+} EX_PCD_ENTRY_ATTRIBUTE;
+
+/**
+  Get Token Number according to dynamic-ex PCD's {token space guid:token number}
+
+  A dynamic-ex type PCD, developer must provide pair of token space guid: token number
+  in DEC file. PCD database maintain a mapping table that translate pair of {token
+  space guid: token number} to Token Number.
+  
+  @param Guid            Token space guid for dynamic-ex PCD entry.
+  @param ExTokenNumber   Token number for dynamic-ex PCD.
+
+  @return Token Number for dynamic-ex PCD.
+
+**/
+UINTN
+GetExPcdTokenNumber (
+  IN CONST EFI_GUID             *Guid,
+  IN UINTN                      ExTokenNumber
+  );
+
+/**
+  Find the local token number according to system SKU ID.
+
+  @param LocalTokenNumber PCD token number
+  @param Size             The size of PCD entry.
+
+  @return Token number according to system SKU ID.
+
+**/
+UINT32
+GetSkuEnabledTokenNumber (
+  UINT32 LocalTokenNumber,
+  UINTN  Size
+  );
+
+/**
+  The function registers the CallBackOnSet fucntion
+  according to TokenNumber and EFI_GUID space.
+
+  @param  TokenNumber       The token number.
+  @param  Guid              The GUID space.
+  @param  CallBackFunction  The Callback function to be registered.
+  @param  Register          To register or unregister the callback function.
+
+  @retval EFI_SUCCESS If the Callback function is registered.
+  @retval EFI_NOT_FOUND If the PCD Entry is not found according to Token Number and GUID space.
+  @retval EFI_OUT_OF_RESOURCES If the callback function can't be registered because there is not free
+                                slot left in the CallbackFnTable.
+**/
+EFI_STATUS
+PeiRegisterCallBackWorker (
+  IN  UINTN              TokenNumber,
+  IN  CONST EFI_GUID         *Guid, OPTIONAL
+  IN  PCD_PPI_CALLBACK   CallBackFunction,
+  IN  BOOLEAN            Register
+  );
+
+/**
+  The function builds the PCD database.
+
+  @param  FileHandle  Handle of the file the external PCD database binary located.
+
+  @return Pointer to PCD database.
+
+**/
+PEI_PCD_DATABASE *
+BuildPcdDatabase (
+  IN EFI_PEI_FILE_HANDLE    FileHandle
+  );
+
+/**
+  Get SKU ID tabble from PCD database.
+
+  @param LocalTokenNumberTableIdx Index of local token number in token number table.
+  @param Database                 PCD Database in PEI phase
+
+  @return Pointer to SKU ID array table
+
+**/
+SKU_ID *
+GetSkuIdArray (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    PEI_PCD_DATABASE  *Database
+  );
+
+/**
+  Get index of PCD entry in size table.
+
+  @param LocalTokenNumberTableIdx Index of this PCD in local token number table.
+  @param Database                 Pointer to PCD database.
+
+  @return index of PCD entry in size table.
+
+**/
+UINTN
+GetSizeTableIndex (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  IN    PEI_PCD_DATABASE  *Database
+  );
+
+/**
+  Get PCD value's size for POINTER type PCD.
+  
+  The POINTER type PCD's value will be stored into a buffer in specificed size.
+  The max size of this PCD's value is described in PCD's definition in DEC file.
+
+  @param LocalTokenNumberTableIdx Index of PCD token number in PCD token table
+  @param MaxSize                  Maxmium size of PCD's value
+  @param Database                 Pcd database in PEI phase.
+
+  @return PCD value's size for POINTER type PCD.
+
+**/
+UINTN
+GetPtrTypeSize (
+  IN    UINTN             LocalTokenNumberTableIdx,
+  OUT   UINTN             *MaxSize,
+  IN    PEI_PCD_DATABASE  *Database
+  );
+
+/**
+  Set PCD value's size for POINTER type PCD.
+  
+  The POINTER type PCD's value will be stored into a buffer in specificed size.
+  The max size of this PCD's value is described in PCD's definition in DEC file.
+
+  @param LocalTokenNumberTableIdx Index of PCD token number in PCD token table
+  @param CurrentSize              Maxmium size of PCD's value
+  @param Database                 Pcd database in PEI phase.
+
+  @retval TRUE  Success to set PCD's value size, which is not exceed maxmium size
+  @retval FALSE Fail to set PCD's value size, which maybe exceed maxmium size
+
+**/
+BOOLEAN
+SetPtrTypeSize (
+  IN          UINTN             LocalTokenNumberTableIdx,
+  IN    OUT   UINTN             *CurrentSize,
+  IN          PEI_PCD_DATABASE  *Database
+  );
+
+#endif
+
diff --git a/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.inf b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.inf
new file mode 100644
index 0000000000..27bce7b7b7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.inf
@@ -0,0 +1,55 @@
+## @file
+#  Pcat SingleSegmentPciCfg2Pei Pei Module.
+#
+#  It installs SingleSegmentPciConfiguration2 PPI to provide read, write and modify access to Pci configuration space in PEI phase.
+#  To follow PI specification, these services also support access to the unaligned Pci address.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PcatSingleSegmentPciCfg2Pei
+  MODULE_UNI_FILE                = PcatSingleSegmentPciCfg2Pei.uni
+  FILE_GUID                      = 4F1F379F-2A62-48bb-AC34-D3F135C6E2B7
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PeimInitializePciCfg
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is for build only)
+#
+
+[Sources]
+  PciCfg2.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+
+[LibraryClasses]
+  PeimEntryPoint
+  PciLib
+  BaseLib
+  DebugLib
+  PeiServicesLib
+
+[Ppis]
+  gEfiPciCfg2PpiGuid                   ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PcatSingleSegmentPciCfg2PeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.uni b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.uni
new file mode 100644
index 0000000000..b7272b0f70
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2Pei.uni
diff --git a/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2PeiExtra.uni b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2PeiExtra.uni
new file mode 100644
index 0000000000..394a7fa448
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PcatSingleSegmentPciCfg2PeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PciCfg2.c b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PciCfg2.c
new file mode 100644
index 0000000000..ed53e12699
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PcatSingleSegmentPciCfg2Pei/PciCfg2.c
@@ -0,0 +1,317 @@
+/** @file
+  This driver installs Single Segment Pci Configuration 2 PPI 
+  to provide read, write and modify access to Pci configuration space in PEI phase. 
+  To follow PI specification, these services also support access to the unaligned Pci address.
+
+  Copyright (c) 2006 - 2010, 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 <PiPei.h>
+#include <Ppi/PciCfg2.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PciLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/PeiServicesLib.h>
+#include <IndustryStandard/Pci.h>
+
+/**
+   Convert EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS to PCI_LIB_ADDRESS.
+
+   @param Address   PCI address with EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS format.
+
+   @return PCI address with PCI_LIB_ADDRESS format.
+
+**/
+UINTN
+PciCfgAddressConvert (
+  EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS *Address
+  )
+{
+  if (Address->ExtendedRegister == 0) {
+    return PCI_LIB_ADDRESS (Address->Bus, Address->Device, Address->Function, Address->Register);
+  }
+
+  return PCI_LIB_ADDRESS (Address->Bus, Address->Device, Address->Function, Address->ExtendedRegister);
+}
+
+/**
+  Reads from a given location in the PCI configuration space.
+
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+  
+**/
+EFI_STATUS
+EFIAPI
+PciCfg2Read (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  )
+{
+  UINTN  PciLibAddress;
+
+  PciLibAddress = PciCfgAddressConvert ((EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS *) &Address);
+
+  if (Width == EfiPeiPciCfgWidthUint8) {
+    *((UINT8 *) Buffer) = PciRead8 (PciLibAddress);
+  } else if (Width == EfiPeiPciCfgWidthUint16) {
+    if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      WriteUnaligned16 (((UINT16 *) Buffer), PciRead16 (PciLibAddress));
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      *((UINT8 *) Buffer) = PciRead8 (PciLibAddress);
+      *((UINT8 *) Buffer + 1) = PciRead8 (PciLibAddress + 1);
+    }
+  } else if (Width == EfiPeiPciCfgWidthUint32) {
+    if ((PciLibAddress & 0x03) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      WriteUnaligned32 (((UINT32 *) Buffer), PciRead32 (PciLibAddress));
+    } else if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Unaligned Pci address access, break up the request into word by word.
+      //
+      WriteUnaligned16 (((UINT16 *) Buffer), PciRead16 (PciLibAddress));
+      WriteUnaligned16 (((UINT16 *) Buffer + 1), PciRead16 (PciLibAddress + 2));
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      *((UINT8 *) Buffer) = PciRead8 (PciLibAddress);
+      *((UINT8 *) Buffer + 1) = PciRead8 (PciLibAddress + 1);
+      *((UINT8 *) Buffer + 2) = PciRead8 (PciLibAddress + 2);
+      *((UINT8 *) Buffer + 3) = PciRead8 (PciLibAddress + 3);
+    }
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Write to a given location in the PCI configuration space.
+
+  @param  PeiServices     An indirect pointer to the PEI Services Table published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes.
+                          See EFI_PEI_PCI_CFG_PPI_WIDTH above.
+  @param  Address         The physical address of the access. The format of
+                          the address is described by EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS.
+  @param  Buffer          A pointer to the buffer of data.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+
+**/
+EFI_STATUS
+EFIAPI
+PciCfg2Write (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN OUT    VOID                      *Buffer
+  )
+{
+  UINTN  PciLibAddress;
+
+  PciLibAddress = PciCfgAddressConvert ((EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS *) &Address);
+
+  if (Width == EfiPeiPciCfgWidthUint8) {
+    PciWrite8 (PciLibAddress, *((UINT8 *) Buffer));
+  } else if (Width == EfiPeiPciCfgWidthUint16) {
+    if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      PciWrite16 (PciLibAddress, ReadUnaligned16 ((UINT16 *) Buffer));
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      PciWrite8 (PciLibAddress, *((UINT8 *) Buffer));
+      PciWrite8 (PciLibAddress + 1, *((UINT8 *) Buffer + 1)); 
+    }
+  } else if (Width == EfiPeiPciCfgWidthUint32) {
+    if ((PciLibAddress & 0x03) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      PciWrite32 (PciLibAddress, ReadUnaligned32 ((UINT32 *) Buffer));
+    } else if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Unaligned Pci address access, break up the request into word by word.
+      //
+      PciWrite16 (PciLibAddress, ReadUnaligned16 ((UINT16 *) Buffer));
+      PciWrite16 (PciLibAddress + 2, ReadUnaligned16 ((UINT16 *) Buffer + 1));
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      PciWrite8 (PciLibAddress, *((UINT8 *) Buffer));
+      PciWrite8 (PciLibAddress + 1, *((UINT8 *) Buffer + 1)); 
+      PciWrite8 (PciLibAddress + 2, *((UINT8 *) Buffer + 2)); 
+      PciWrite8 (PciLibAddress + 3, *((UINT8 *) Buffer + 3)); 
+    }
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  This function performs a read-modify-write operation on the contents from a given
+  location in the PCI configuration space.
+
+  @param  PeiServices     An indirect pointer to the PEI Services Table
+                          published by the PEI Foundation.
+  @param  This            Pointer to local data for the interface.
+  @param  Width           The width of the access. Enumerated in bytes. Type
+                          EFI_PEI_PCI_CFG_PPI_WIDTH is defined in Read().
+  @param  Address         The physical address of the access.
+  @param  SetBits         Points to value to bitwise-OR with the read configuration value.
+                          The size of the value is determined by Width.
+  @param  ClearBits       Points to the value to negate and bitwise-AND with the read configuration value.
+                          The size of the value is determined by Width.
+
+  @retval EFI_SUCCESS           The function completed successfully.
+  @retval EFI_INVALID_PARAMETER The invalid access width.
+
+**/
+EFI_STATUS
+EFIAPI
+PciCfg2Modify (
+  IN CONST  EFI_PEI_SERVICES          **PeiServices,
+  IN CONST  EFI_PEI_PCI_CFG2_PPI      *This,
+  IN        EFI_PEI_PCI_CFG_PPI_WIDTH Width,
+  IN        UINT64                    Address,
+  IN        VOID                      *SetBits,
+  IN        VOID                      *ClearBits
+  )
+{
+  UINTN   PciLibAddress;
+  UINT16  ClearValue16;
+  UINT16  SetValue16;
+  UINT32  ClearValue32;
+  UINT32  SetValue32;
+
+  PciLibAddress = PciCfgAddressConvert ((EFI_PEI_PCI_CFG_PPI_PCI_ADDRESS *) &Address);
+
+  if (Width == EfiPeiPciCfgWidthUint8) {
+    PciAndThenOr8 (PciLibAddress, (UINT8) (~(*(UINT8 *) ClearBits)), *((UINT8 *) SetBits));
+  } else if (Width == EfiPeiPciCfgWidthUint16) {
+    if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      ClearValue16  = (UINT16) (~ReadUnaligned16 ((UINT16 *) ClearBits));
+      SetValue16    = ReadUnaligned16 ((UINT16 *) SetBits);
+      PciAndThenOr16 (PciLibAddress, ClearValue16, SetValue16);
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      PciAndThenOr8 (PciLibAddress, (UINT8) (~(*(UINT8 *) ClearBits)), *((UINT8 *) SetBits));
+      PciAndThenOr8 (PciLibAddress + 1, (UINT8) (~(*((UINT8 *) ClearBits + 1))), *((UINT8 *) SetBits + 1));
+    }
+  } else if (Width == EfiPeiPciCfgWidthUint32) {
+    if ((PciLibAddress & 0x03) == 0) {
+      //
+      // Aligned Pci address access
+      //
+      ClearValue32  = (UINT32) (~ReadUnaligned32 ((UINT32 *) ClearBits));
+      SetValue32    = ReadUnaligned32 ((UINT32 *) SetBits);
+      PciAndThenOr32 (PciLibAddress, ClearValue32, SetValue32);
+    } else if ((PciLibAddress & 0x01) == 0) {
+      //
+      // Unaligned Pci address access, break up the request into word by word.
+      //
+      ClearValue16  = (UINT16) (~ReadUnaligned16 ((UINT16 *) ClearBits));
+      SetValue16    = ReadUnaligned16 ((UINT16 *) SetBits);
+      PciAndThenOr16 (PciLibAddress, ClearValue16, SetValue16);
+
+      ClearValue16  = (UINT16) (~ReadUnaligned16 ((UINT16 *) ClearBits + 1));
+      SetValue16    = ReadUnaligned16 ((UINT16 *) SetBits + 1);
+      PciAndThenOr16 (PciLibAddress + 2, ClearValue16, SetValue16);
+    } else {
+      //
+      // Unaligned Pci address access, break up the request into byte by byte.
+      //
+      PciAndThenOr8 (PciLibAddress, (UINT8) (~(*(UINT8 *) ClearBits)), *((UINT8 *) SetBits));
+      PciAndThenOr8 (PciLibAddress + 1, (UINT8) (~(*((UINT8 *) ClearBits + 1))), *((UINT8 *) SetBits + 1));
+      PciAndThenOr8 (PciLibAddress + 2, (UINT8) (~(*((UINT8 *) ClearBits + 2))), *((UINT8 *) SetBits + 2));
+      PciAndThenOr8 (PciLibAddress + 3, (UINT8) (~(*((UINT8 *) ClearBits + 3))), *((UINT8 *) SetBits + 3));
+    }
+  } else {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  return EFI_SUCCESS;
+}
+
+EFI_PEI_PCI_CFG2_PPI gPciCfg2Ppi = {
+  PciCfg2Read,
+  PciCfg2Write,
+  PciCfg2Modify,
+  0
+};
+
+EFI_PEI_PPI_DESCRIPTOR gPciCfg2PpiList = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPciCfg2PpiGuid,
+  &gPciCfg2Ppi
+};
+
+/**
+  Module's entry function. 
+  This routine will install EFI_PEI_PCI_CFG2_PPI.
+  
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @return Whether success to install service.
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializePciCfg (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS            Status;
+
+  (**(EFI_PEI_SERVICES **)PeiServices).PciCfg = &gPciCfg2Ppi;
+  Status = PeiServicesInstallPpi (&gPciCfg2PpiList);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/InternalPlatDriOverrideDxe.h b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/InternalPlatDriOverrideDxe.h
new file mode 100644
index 0000000000..75af763522
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/InternalPlatDriOverrideDxe.h
@@ -0,0 +1,217 @@
+/** @file
+  Ihe internal heder file includes the required Protocol/Guid/Library
+  and the shared function APIs.
+
+Copyright (c) 2007 - 2013, 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.
+
+**/
+
+#ifndef _INTERNAL_PLATFORM_DRIVER_OVERRIDE_H_
+#define _INTERNAL_PLATFORM_DRIVER_OVERRIDE_H_
+
+#include <PiDxe.h>
+
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/LoadedImage.h>
+#include <Protocol/FirmwareVolume2.h>
+#include <Protocol/PciIo.h>
+#include <Protocol/BusSpecificDriverOverride.h>
+#include <Protocol/ComponentName2.h>
+#include <Protocol/ComponentName.h>
+#include <Protocol/DriverBinding.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/PlatformDriverOverride.h>
+#include <Guid/MdeModuleHii.h>
+#include <Guid/VariableFormat.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/HiiLib.h>
+
+/**
+  Free all the mapping database memory resource and initialize the mapping list entry.
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_INVALID_PARAMETER    mapping database list entry is NULL
+  @retval EFI_SUCCESS              Free success
+
+**/
+EFI_STATUS
+EFIAPI
+FreeMappingDatabase (
+  IN  OUT  LIST_ENTRY            *MappingDataBase
+  )
+;
+
+/**
+  Read the NV environment variable(s) that contain the override mappings from Controller Device Path to
+  a set of Driver Device Paths, and create the mapping database in memory to contain these variable info.
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_INVALID_PARAMETER    MappingDataBase pointer is null
+  @retval EFI_NOT_FOUND            Cannot find the 'PlatDriOver' NV variable
+  @retval EFI_VOLUME_CORRUPTED     The found NV variable is corrupted
+  @retval EFI_SUCCESS              Create the mapping database in memory successfully
+
+**/
+EFI_STATUS
+EFIAPI
+InitOverridesMapping (
+  OUT  LIST_ENTRY            *MappingDataBase
+  )
+;
+
+/**
+  Save the memory mapping database into NV environment variable(s).
+  If MappingDataBase list is empty, then delete all platform override NV variables.
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_INVALID_PARAMETER    MappingDataBase pointer is null
+  @retval EFI_SUCCESS              Save memory mapping database successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SaveOverridesMapping (
+  IN  LIST_ENTRY              *MappingDataBase
+  )
+;
+
+/**
+  Retrieves the image handle of the platform override driver for a controller in the system from the memory mapping database.
+
+  @param  ControllerHandle         The device handle of the controller to check if
+                                   a driver override exists.
+  @param  DriverImageHandle        On output, a pointer to the next driver handle.
+                                   Passing in a pointer to NULL, will return the
+                                   first driver handle for ControllerHandle.
+  @param  MappingDataBase          MappingDataBase - Mapping database list entry
+                                   pointer
+  @param  CallerImageHandle        The caller driver's image handle, for
+                                   UpdateFvFileDevicePath use.
+
+  @retval EFI_INVALID_PARAMETER    The handle specified by ControllerHandle is not
+                                   a valid handle.  Or DriverImagePath is not a
+                                   device path that was returned on a previous call
+                                   to GetDriverPath().
+  @retval EFI_NOT_FOUND            A driver override for ControllerHandle was not
+                                   found.
+  @retval EFI_UNSUPPORTED          The operation is not supported.
+  @retval EFI_SUCCESS              The driver override for ControllerHandle was
+                                   returned in DriverImagePath.
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriverFromMapping (
+  IN     EFI_HANDLE                                     ControllerHandle,
+  IN OUT EFI_HANDLE                                     *DriverImageHandle,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  IN     EFI_HANDLE                                     CallerImageHandle
+  )
+;
+
+/**
+  Check mapping database whether already has the mapping info which
+  records the input Controller to input DriverImage.
+
+  @param  ControllerDevicePath     The controller device path is to be check.
+  @param  DriverImageDevicePath    The driver image device path is to be check.
+  @param  MappingDataBase          Mapping database list entry pointer
+  @param  DriverInfoNum            the controller's total override driver number
+  @param  DriverImageNO            The driver order number for the input DriverImage.
+                                   If the DriverImageDevicePath is NULL, DriverImageNO is not set.
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath or MappingDataBase is NULL.
+  @retval EFI_NOT_FOUND            ControllerDevicePath is not found in MappingDataBase or
+                                   DriverImageDevicePath is not found in the found DriverImage Info list. 
+  @retval EFI_SUCCESS              The controller's total override driver number and 
+                                   input DriverImage's order number is correctly return.
+**/
+EFI_STATUS
+EFIAPI
+CheckMapping (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath  OPTIONAL,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  OUT    UINT32                                         *DriverInfoNum  OPTIONAL,
+  OUT    UINT32                                         *DriverImageNO  OPTIONAL
+  )
+;
+
+/**
+  Insert a driver image as a controller's override driver into the mapping database.
+  The driver image's order number is indicated by DriverImageNO.
+
+  @param  ControllerDevicePath     The controller device path need to add a
+                                   override driver image item
+  @param  DriverImageDevicePath    The driver image device path need to be insert
+  @param  MappingDataBase          Mapping database list entry pointer
+  @param  DriverImageNO            The inserted order number. If this number is taken, 
+                                   the larger available number will be used.
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath is NULL, or DriverImageDevicePath is NULL
+                                   or MappingDataBase is NULL
+  @retval EFI_ALREADY_STARTED      The input Controller to input DriverImage has been 
+                                   recorded into the mapping database.
+  @retval EFI_SUCCESS              The Controller and DriverImage are inserted into 
+                                   the mapping database successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+InsertDriverImage (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  IN     UINT32                                         DriverImageNO
+  )
+;
+
+/**
+  Delete a controller's override driver from the mapping database.
+
+  @param  ControllerDevicePath     The controller device path will be deleted 
+                                   when all drivers images on it are removed.
+  @param  DriverImageDevicePath    The driver image device path will be delete.
+                                   If NULL, all driver image will be delete.
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath is NULL, or MappingDataBase is NULL
+  @retval EFI_NOT_FOUND            ControllerDevicePath is not found in MappingDataBase or
+                                   DriverImageDevicePath is not found in the found DriverImage Info list. 
+  @retval EFI_SUCCESS              Delete the specified driver successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DeleteDriverImage (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath,
+  IN     LIST_ENTRY                                     *MappingDataBase
+  )
+;
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.c b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.c
new file mode 100644
index 0000000000..1274d6720d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.c
@@ -0,0 +1,1750 @@
+/** @file
+  This file also installs UEFI PLATFORM_DRIVER_OVERRIDE_PROTOCOL.
+  
+  The main code offers a UI interface in device manager to let user configure
+  platform override protocol to override the default algorithm for matching
+  drivers to controllers.
+
+  The main flow:
+  1. It dynamicly locate all controller device path.
+  2. It dynamicly locate all drivers which support binding protocol.
+  3. It export and dynamicly update two menu to let user select the
+     mapping between drivers to controllers.
+  4. It save all the mapping info in NV variables which will be consumed
+     by platform override protocol driver to publish the platform override protocol.
+
+Copyright (c) 2007 - 2015, 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 "InternalPlatDriOverrideDxe.h"
+#include "PlatOverMngr.h"
+
+#define EFI_CALLBACK_INFO_SIGNATURE SIGNATURE_32 ('C', 'l', 'b', 'k')
+#define EFI_CALLBACK_INFO_FROM_THIS(a)  CR (a, EFI_CALLBACK_INFO, ConfigAccess, EFI_CALLBACK_INFO_SIGNATURE)
+
+typedef struct {
+  UINTN                           Signature;
+  EFI_HANDLE                      DriverHandle;
+  EFI_HII_HANDLE                  RegisteredHandle;
+  PLAT_OVER_MNGR_DATA             FakeNvData;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL *HiiConfigRouting;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  ConfigAccess;
+  EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL PlatformDriverOverride;
+} EFI_CALLBACK_INFO;
+
+#pragma pack(1)
+
+///
+/// HII specific Vendor Device Path definition.
+///
+typedef struct {
+  VENDOR_DEVICE_PATH             VendorDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL       End;
+} HII_VENDOR_DEVICE_PATH;
+
+#pragma pack()
+
+//
+// uni string and Vfr Binary data.
+//
+extern UINT8  VfrBin[];
+extern UINT8  PlatDriOverrideDxeStrings[];
+
+//
+// module global data
+//
+CHAR16                       mVariableName[] = L"Data";
+LIST_ENTRY                   mMappingDataBase = INITIALIZE_LIST_HEAD_VARIABLE (mMappingDataBase);
+BOOLEAN                      mEnvironmentVariableRead = FALSE;
+EFI_HANDLE                   mCallerImageHandle = NULL;
+
+EFI_HANDLE                   *mDevicePathHandleBuffer;
+EFI_HANDLE                   *mDriverImageHandleBuffer;
+
+INTN                         mSelectedCtrIndex;
+EFI_STRING_ID                *mControllerToken;
+UINTN                        mDriverImageHandleCount;
+EFI_STRING_ID                *mDriverImageToken;
+EFI_DEVICE_PATH_PROTOCOL     **mControllerDevicePathProtocol;
+UINTN                        mSelectedDriverImageNum;
+UINTN                        mLastSavedDriverImageNum;
+UINT16                       mCurrentPage;
+EFI_CALLBACK_INFO           *mCallbackInfo;
+BOOLEAN                     *mDriSelection;
+UINTN                        mMaxDeviceCount;
+
+HII_VENDOR_DEVICE_PATH  mHiiVendorDevicePath = {
+  {
+    {
+      HARDWARE_DEVICE_PATH,
+      HW_VENDOR_DP,
+      {
+        (UINT8) (sizeof (VENDOR_DEVICE_PATH)),
+        (UINT8) ((sizeof (VENDOR_DEVICE_PATH)) >> 8)
+      }
+    },
+    PLAT_OVER_MNGR_GUID
+  },
+  {
+    END_DEVICE_PATH_TYPE,
+    END_ENTIRE_DEVICE_PATH_SUBTYPE,
+    { 
+      (UINT8) (END_DEVICE_PATH_LENGTH),
+      (UINT8) ((END_DEVICE_PATH_LENGTH) >> 8)
+    }
+  }
+};
+
+/**
+  Converting a given device to an unicode string.
+
+  @param    DevPath     Given device path instance
+  
+  @return   Converted string from given device path.
+  @retval   L"?" Converting failed.
+**/
+CHAR16 *
+DevicePathToStr (
+  IN EFI_DEVICE_PATH_PROTOCOL     *DevPath
+  )
+{
+  CHAR16                          *Text;
+  Text = ConvertDevicePathToText (
+           DevPath,
+           FALSE,
+           TRUE
+           );
+  if (Text == NULL) {
+    Text = AllocateCopyPool (sizeof (L"?"), L"?");
+    ASSERT (Text != NULL);
+  }
+
+  return Text;
+}
+
+/**
+  Worker function to get the driver name by ComponentName or ComponentName2 protocol 
+  according to the driver binding handle.
+
+  @param  DriverBindingHandle  The Handle of DriverBinding.
+  @param  ProtocolGuid         The pointer to Component Name (2) protocol GUID.
+  @param  VariableName         The name of the RFC 4646 or ISO 639-2 language variable.
+
+  @retval !NULL               Pointer into the image name if the image name is found,
+  @retval NULL                Pointer to NULL if the image name is not found.
+
+**/
+CHAR16 *
+GetComponentNameWorker (
+  IN EFI_HANDLE                      DriverBindingHandle,
+  IN EFI_GUID                        *ProtocolGuid,
+  IN CONST CHAR16                    *VariableName
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_COMPONENT_NAME_PROTOCOL        *ComponentName;
+  CHAR16                             *DriverName;
+  CHAR8                              *Language;
+  CHAR8                              *BestLanguage;
+
+  Status = gBS->OpenProtocol (
+                  DriverBindingHandle,
+                  ProtocolGuid,
+                  (VOID *) &ComponentName,
+                  NULL,
+                  NULL,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+  if (EFI_ERROR (Status)) {
+    return NULL;
+  }
+
+  //
+  // Find the best matching language.
+  //
+  GetEfiGlobalVariable2 (VariableName, (VOID**)&Language, NULL);
+  BestLanguage = GetBestLanguage (
+                   ComponentName->SupportedLanguages,
+                   (BOOLEAN) (ProtocolGuid == &gEfiComponentNameProtocolGuid),
+                   Language,
+                   NULL
+                   );
+
+  DriverName = NULL;
+  if (BestLanguage != NULL) {
+    ComponentName->GetDriverName (
+                     ComponentName,
+                     BestLanguage,
+                     &DriverName
+                     );
+    FreePool (BestLanguage);
+  }
+
+  if (Language != NULL) {
+    FreePool (Language);
+  }
+
+  return DriverName;
+}
+
+
+/**
+  Get the driver name by ComponentName or ComponentName2 protocol 
+  according to the driver binding handle
+
+  @param DriverBindingHandle  The Handle of DriverBinding.
+
+  @retval !NULL               Pointer into the image name if the image name is found,
+  @retval NULL                Pointer to NULL if the image name is not found.
+
+**/
+CHAR16 *
+GetComponentName (
+  IN EFI_HANDLE                      DriverBindingHandle
+  )
+{
+  CHAR16                    *DriverName;
+
+  //
+  // Try RFC 4646 Component Name 2 protocol first.
+  //
+  DriverName = GetComponentNameWorker (DriverBindingHandle, &gEfiComponentName2ProtocolGuid, L"PlatformLang");
+  if (DriverName == NULL) {
+    //
+    // If we can not get driver name from Component Name 2 protocol, we can try ISO 639-2 Component Name protocol. 
+    //
+    DriverName = GetComponentNameWorker (DriverBindingHandle, &gEfiComponentNameProtocolGuid, L"Lang");
+  }
+
+  return DriverName;
+}
+
+/**
+  Get the image name from EFI UI section.
+  Get FV protocol by its loaded image protocol to abstract EFI UI section.
+
+  @param Image            Pointer to the loaded image protocol
+
+  @retval !NULL           Pointer to the image name if the image name is found,
+  @retval NULL            NULL if the image name is not found.
+
+**/
+CHAR16 *
+GetImageName (
+  IN EFI_LOADED_IMAGE_PROTOCOL *Image
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_DEVICE_PATH_PROTOCOL          *DevPathNode;
+  EFI_DEVICE_PATH_PROTOCOL          *AlignedDevPathNode;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *FvFilePath;
+  VOID                              *Buffer;
+  UINTN                             BufferSize;
+  UINT32                            AuthenticationStatus;
+  EFI_GUID                          *NameGuid;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL     *Fv2;
+
+  Fv2         = NULL;
+  Buffer      = NULL;
+  BufferSize  = 0;
+
+  if (Image->FilePath == NULL) {
+    return NULL;
+  }
+  DevPathNode  = Image->FilePath;
+
+  while (!IsDevicePathEnd (DevPathNode)) {
+    //
+    // Make sure device path node is aligned when accessing it's FV Name Guid field.
+    //
+    AlignedDevPathNode = AllocateCopyPool (DevicePathNodeLength(DevPathNode), DevPathNode);
+    
+    //
+    // Find the Fv File path
+    //
+    NameGuid = EfiGetNameGuidFromFwVolDevicePathNode ((MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *)AlignedDevPathNode);
+    if (NameGuid != NULL) {
+      FvFilePath = (MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) AlignedDevPathNode;
+      Status = gBS->HandleProtocol (
+                    Image->DeviceHandle,
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    (VOID **) &Fv2
+                    );
+      //
+      // Locate Image EFI UI section to get the image name.
+      //
+      if (!EFI_ERROR (Status)) {
+        Status = Fv2->ReadSection (
+                        Fv2,
+                        &FvFilePath->FvFileName,
+                        EFI_SECTION_USER_INTERFACE,
+                        0,
+                        &Buffer,
+                        &BufferSize,
+                        &AuthenticationStatus
+                        );
+        if (!EFI_ERROR (Status)) {
+          FreePool (AlignedDevPathNode);
+          break;
+        }
+        Buffer = NULL;
+      }
+    }
+    
+    FreePool (AlignedDevPathNode);
+    
+    //
+    // Next device path node
+    //
+    DevPathNode = NextDevicePathNode (DevPathNode);
+  }
+
+  return Buffer;
+}
+
+/**
+  Prepare the first page to let user select the device controller which need to
+  add mapping drivers if user select 'Refresh' in first page.
+  During first page, user will see all currnet controller device path in system,
+  select any device path will go to second page to select its overrides drivers.
+
+  @param  Private        Pointer to EFI_CALLBACK_INFO.
+  @param  KeyValue       The callback key value of device controller item in first page.
+  @param  FakeNvData     Pointer to PLAT_OVER_MNGR_DATA.
+
+  @retval EFI_SUCCESS    Always returned.
+
+**/
+EFI_STATUS
+UpdateDeviceSelectPage (
+  IN EFI_CALLBACK_INFO                *Private,
+  IN UINT16                           KeyValue,
+  IN PLAT_OVER_MNGR_DATA              *FakeNvData
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     Index;
+  UINTN                                     DevicePathHandleCount;
+  UINTN                                     NewStrSize;
+  CHAR16                                    *NewString;
+  EFI_STRING_ID                             NewStringToken;
+  CHAR16                                    *ControllerName;
+  EFI_DEVICE_PATH_PROTOCOL                  *ControllerDevicePath;
+  EFI_PCI_IO_PROTOCOL                       *PciIo;
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL *BusSpecificDriverOverride;
+  UINTN                                     Len;
+  VOID                                      *StartOpCodeHandle;
+  VOID                                      *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL                        *StartLabel;
+  EFI_IFR_GUID_LABEL                        *EndLabel;  
+
+  //
+  // Set current page form ID.
+  //
+  mCurrentPage = FORM_ID_DEVICE;  
+  
+  //
+  // Initial the mapping database in memory
+  //
+  FreeMappingDatabase (&mMappingDataBase);
+  InitOverridesMapping (&mMappingDataBase);
+
+  //
+  // Init OpCode Handle
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number = FORM_ID_DEVICE;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  //
+  // Clear first page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_DEVICE,
+    StartOpCodeHandle, // Label FORM_ID_DEVICE
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  //
+  // When user enter the page at first time, the 'first refresh' string is given to notify user to refresh all the drivers,
+  // then the 'first refresh' string will be replaced by the 'refresh' string, and the two strings content are same after the replacement
+  //
+  NewStringToken = STRING_TOKEN (STR_FIRST_REFRESH);
+  NewString = HiiGetString (Private->RegisteredHandle, STRING_TOKEN (STR_REFRESH), NULL);
+  ASSERT (NewString != NULL);
+  if (HiiSetString (Private->RegisteredHandle, NewStringToken, NewString, NULL) == 0) {
+    ASSERT (FALSE);
+  }
+  FreePool (NewString);
+
+  NewStringToken = STRING_TOKEN (STR_FIRST_REFRESH_HELP);
+  NewString = HiiGetString (Private->RegisteredHandle, STRING_TOKEN (STR_REFRESH_HELP), NULL);
+  ASSERT (NewString != NULL);
+  if (HiiSetString (Private->RegisteredHandle, NewStringToken, NewString, NULL) == 0) {
+    ASSERT (FALSE);
+  }
+  FreePool (NewString);
+
+  //
+  // created needed controller device item in first page
+  //
+  DevicePathHandleCount  = 0;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDevicePathProtocolGuid,
+                  NULL,
+                  &DevicePathHandleCount,
+                  &mDevicePathHandleBuffer
+                  );
+  if (EFI_ERROR (Status) || (DevicePathHandleCount == 0)) {
+    return EFI_SUCCESS;
+  }
+
+  mMaxDeviceCount = DevicePathHandleCount;
+  mControllerDevicePathProtocol = AllocateZeroPool (DevicePathHandleCount * sizeof (EFI_DEVICE_PATH_PROTOCOL *));
+  ASSERT (mControllerDevicePathProtocol != NULL);
+  mControllerToken = AllocateZeroPool (DevicePathHandleCount * sizeof (EFI_STRING_ID));
+  ASSERT (mControllerToken != NULL);
+
+  for (Index = 0; Index < DevicePathHandleCount; Index++) {
+    if (FakeNvData->PciDeviceFilter == 0x01) {
+      //
+      // Only care PCI device which contain efi driver in its option rom.
+      //
+
+      //
+      // Check whether it is a pci device
+      //
+      ControllerDevicePath = NULL;
+      Status = gBS->OpenProtocol (
+                      mDevicePathHandleBuffer[Index],
+                      &gEfiPciIoProtocolGuid,
+                      (VOID **) &PciIo,
+                      NULL,
+                      NULL,
+                      EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                      );
+      if (EFI_ERROR (Status)) {
+        continue;
+      }
+      //
+      // Check whether it contain efi driver in its option rom
+      //
+      Status = gBS->HandleProtocol(
+                       mDevicePathHandleBuffer[Index],
+                       &gEfiBusSpecificDriverOverrideProtocolGuid,
+                       (VOID **) &BusSpecificDriverOverride
+                       );
+      if (EFI_ERROR (Status) || BusSpecificDriverOverride == NULL) {
+        continue;
+      }
+    }
+
+    ControllerDevicePath = NULL;
+    Status = gBS->OpenProtocol (
+                    mDevicePathHandleBuffer[Index],
+                    &gEfiDevicePathProtocolGuid,
+                    (VOID **) &ControllerDevicePath,
+                    NULL,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    ASSERT_EFI_ERROR (Status);
+    //
+    // Save the device path protocol interface
+    //
+    mControllerDevicePathProtocol[Index] = ControllerDevicePath;
+
+    //
+    // Get the driver name
+    //
+    ControllerName = DevicePathToStr (ControllerDevicePath);
+
+    //
+    // Export the driver name string and create item in set options page
+    //
+    Len = StrSize (ControllerName);
+    NewStrSize = Len + StrSize (L"--");
+    NewString = AllocateZeroPool (NewStrSize);
+    ASSERT (NewString != NULL);
+    if (EFI_ERROR (CheckMapping (ControllerDevicePath,NULL, &mMappingDataBase, NULL, NULL))) {
+      StrCatS (NewString, NewStrSize/sizeof(CHAR16), L"--");
+    } else {
+      StrCatS (NewString, NewStrSize/sizeof(CHAR16), L"**");
+    }
+    StrCatS (NewString, NewStrSize/sizeof(CHAR16), ControllerName);
+
+    NewStringToken = HiiSetString (Private->RegisteredHandle, mControllerToken[Index], NewString, NULL);
+    ASSERT (NewStringToken != 0);
+    FreePool (NewString);
+    //
+    // Save the device path string toke for next access use
+    //
+    mControllerToken[Index] = NewStringToken;
+      
+    HiiCreateGotoOpCode (
+      StartOpCodeHandle,
+      FORM_ID_DRIVER,
+      NewStringToken,
+      STRING_TOKEN (STR_GOTO_HELP_DRIVER),
+      EFI_IFR_FLAG_CALLBACK,
+      (UINT16) (Index + KEY_VALUE_DEVICE_OFFSET)
+      );
+  }
+
+  //
+  // Update first page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_DEVICE,
+    StartOpCodeHandle, // Label FORM_ID_DEVICE
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the first Driver Binding handle which has the specific image handle.
+
+  @param  ImageHandle          The Image handle
+
+  @return                      Handle to Driver binding
+  @retval NULL                 The paramter is not valid or the driver binding handle is not found.
+
+**/
+EFI_HANDLE
+GetDriverBindingHandleFromImageHandle (
+  IN  EFI_HANDLE   ImageHandle
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+  UINTN                             DriverBindingHandleCount;
+  EFI_HANDLE                        *DriverBindingHandleBuffer;
+  EFI_DRIVER_BINDING_PROTOCOL       *DriverBindingInterface;
+  EFI_HANDLE                        DriverBindingHandle;
+
+  DriverBindingHandle = NULL;
+
+  if (ImageHandle == NULL) {
+    return NULL;
+  }
+  //
+  // Get all drivers which support driver binding protocol
+  //
+  DriverBindingHandleCount  = 0;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDriverBindingProtocolGuid,
+                  NULL,
+                  &DriverBindingHandleCount,
+                  &DriverBindingHandleBuffer
+                  );
+  if (EFI_ERROR (Status) || (DriverBindingHandleCount == 0)) {
+    return NULL;
+  }
+  
+  //
+  // Get the first Driver Binding handle which has the specific image handle.
+  //
+  for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+    DriverBindingInterface = NULL;
+    Status = gBS->OpenProtocol (
+                    DriverBindingHandleBuffer[Index],
+                    &gEfiDriverBindingProtocolGuid,
+                    (VOID **) &DriverBindingInterface,
+                    NULL,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (DriverBindingInterface->ImageHandle == ImageHandle) {
+      DriverBindingHandle = DriverBindingHandleBuffer[Index];
+      break;
+    }
+  }
+
+  FreePool (DriverBindingHandleBuffer);
+  return DriverBindingHandle;
+}
+
+/**
+  Prepare to let user select the drivers which need mapping with the device controller
+  selected in first page.
+
+  @param  Private        Pointer to EFI_CALLBACK_INFO.
+  @param  KeyValue       The callback key value of device controller item in first page.
+                         KeyValue is larger than or equal to KEY_VALUE_DEVICE_OFFSET.
+  @param  FakeNvData     Pointer to PLAT_OVER_MNGR_DATA.
+
+  @retval EFI_SUCCESS    Always returned.
+
+**/
+EFI_STATUS
+UpdateBindingDriverSelectPage (
+  IN EFI_CALLBACK_INFO                *Private,
+  IN UINT16                           KeyValue,
+  IN PLAT_OVER_MNGR_DATA              *FakeNvData
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     Index;
+  UINTN                                     NewStrSize;
+  CHAR16                                    *NewString;
+  EFI_STRING_ID                             NewStringToken;
+  EFI_STRING_ID                             NewStringHelpToken;
+  UINTN                                     DriverImageHandleCount;
+  EFI_LOADED_IMAGE_PROTOCOL                 *LoadedImage;
+  CHAR16                                    *DriverName;
+  BOOLEAN                                   FreeDriverName;
+  EFI_DEVICE_PATH_PROTOCOL                  *LoadedImageDevicePath;
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL *BusSpecificDriverOverride;
+  EFI_HANDLE                                DriverBindingHandle;
+  VOID                                      *StartOpCodeHandle;
+  VOID                                      *EndOpCodeHandle;
+  EFI_IFR_GUID_LABEL                        *StartLabel;
+  EFI_IFR_GUID_LABEL                        *EndLabel;
+  EFI_LOADED_IMAGE_PROTOCOL                 **DriverImageProtocol;
+  EFI_STRING_ID                             *DriverImageFilePathToken;
+  UINT8                                     CheckFlags;
+
+  //
+  // If user select a controller item in the first page  the following code will be run.
+  // During second page, user will see all currnet driver bind protocol driver, the driver name and its device path will be shown
+  //
+  //First acquire the list of Loaded Image Protocols, and then when  want the name of the driver, look up all the Driver Binding Protocols
+  // and find the first one whose ImageHandle field matches the image handle of the Loaded Image Protocol.
+  // then use the Component Name Protocol on the same handle as the first matching Driver Binding Protocol to look up the name of the driver.
+  //
+
+  mCurrentPage = FORM_ID_DRIVER;
+  //
+  // Switch the item callback key value to its NO. in mDevicePathHandleBuffer
+  //
+  mSelectedCtrIndex = KeyValue - KEY_VALUE_DEVICE_OFFSET;
+  ASSERT (mSelectedCtrIndex >= 0 && mSelectedCtrIndex < MAX_CHOICE_NUM);
+
+  mLastSavedDriverImageNum = 0;
+
+  //
+  // Init OpCode Handle
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = FORM_ID_DRIVER;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  //
+  // Clear second page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_DRIVER,
+    StartOpCodeHandle,
+    EndOpCodeHandle
+    );
+
+  //
+  // Show all driver which support loaded image protocol in second page
+  //
+  DriverImageHandleCount  = 0;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiLoadedImageProtocolGuid,
+                  NULL,
+                  &DriverImageHandleCount,
+                  &mDriverImageHandleBuffer
+                  );
+  if (EFI_ERROR (Status) || (DriverImageHandleCount == 0)) {
+    return EFI_NOT_FOUND;
+  }
+
+  mDriverImageToken = AllocateZeroPool (DriverImageHandleCount * sizeof (EFI_STRING_ID));
+  ASSERT (mDriverImageToken != NULL);
+  mDriSelection = AllocateZeroPool (DriverImageHandleCount * sizeof (BOOLEAN));
+  ASSERT (mDriSelection != NULL);
+
+  DriverImageProtocol = AllocateZeroPool (DriverImageHandleCount * sizeof (EFI_LOADED_IMAGE_PROTOCOL *));
+  ASSERT (DriverImageProtocol != NULL);
+  DriverImageFilePathToken = AllocateZeroPool (DriverImageHandleCount * sizeof (EFI_STRING_ID));
+  ASSERT (DriverImageFilePathToken != NULL);
+
+  mDriverImageHandleCount = DriverImageHandleCount;
+  for (Index = 0; Index < DriverImageHandleCount; Index++) {
+    //
+    // Step1: Get the driver image total file path for help string and the driver name.
+    //
+
+    //
+    // Find driver's Loaded Image protocol
+    //
+    LoadedImage =NULL;
+
+    Status = gBS->OpenProtocol (
+                    mDriverImageHandleBuffer[Index],
+                    &gEfiLoadedImageProtocolGuid,
+                    (VOID **) &LoadedImage,
+                    NULL,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      mDriSelection[Index] = FALSE;
+      continue;
+    }
+    DriverImageProtocol[Index] = LoadedImage;
+    //
+    // Find its related driver binding protocol
+    //
+    DriverBindingHandle = GetDriverBindingHandleFromImageHandle (mDriverImageHandleBuffer[Index]);
+    if (DriverBindingHandle == NULL) {
+      mDriSelection[Index] = FALSE;
+      continue;
+    }
+
+    //
+    // Get the EFI Loaded Image Device Path Protocol
+    //
+    LoadedImageDevicePath = NULL;
+    Status = gBS->HandleProtocol (
+                        mDriverImageHandleBuffer[Index],
+                        &gEfiLoadedImageDevicePathProtocolGuid,
+                        (VOID **) &LoadedImageDevicePath
+                        );
+    if (LoadedImageDevicePath == NULL) {
+      mDriSelection[Index] = FALSE;
+      continue;
+    }
+
+    if (FakeNvData->PciDeviceFilter == 0x01) {
+      //
+      // only care the driver which is in a Pci device option rom,
+      // and the driver's LoadedImage->DeviceHandle must point to a pci device which has efi option rom
+      //
+      if (!EFI_ERROR (Status)) {
+        Status = gBS->HandleProtocol(
+                         LoadedImage->DeviceHandle,
+                         &gEfiBusSpecificDriverOverrideProtocolGuid,
+                         (VOID **) &BusSpecificDriverOverride
+                         );
+        if (EFI_ERROR (Status) || BusSpecificDriverOverride == NULL) {
+          mDriSelection[Index] = FALSE;
+          continue;
+        }
+      } else {
+        mDriSelection[Index] = FALSE;
+        continue;
+      }
+    }
+
+    //
+    // For driver name, try to get its component name, if fail, get its image name,
+    // if also fail, give a default name.
+    //
+    FreeDriverName = FALSE;
+    DriverName = GetComponentName (DriverBindingHandle);
+    if (DriverName == NULL) {
+      //
+      // get its image name
+      //
+      DriverName = GetImageName (LoadedImage);
+    }
+    if (DriverName == NULL) {
+      //
+      // give a default name
+      //
+      DriverName = HiiGetString (Private->RegisteredHandle, STRING_TOKEN (STR_DRIVER_DEFAULT_NAME), NULL);
+      ASSERT (DriverName != NULL);
+      FreeDriverName = TRUE;  // the DriverName string need to free pool
+    }
+
+
+    //
+    // Step2 Export the driver name string and create check box item in second page
+    //
+
+    //
+    // First create the driver image name
+    //
+    NewStrSize = StrSize (DriverName);
+    NewString = AllocateZeroPool (NewStrSize);
+    ASSERT (NewString != NULL); 
+    if (EFI_ERROR (CheckMapping (mControllerDevicePathProtocol[mSelectedCtrIndex], LoadedImageDevicePath, &mMappingDataBase, NULL, NULL))) {
+      mDriSelection[Index] = FALSE;
+    } else {
+      mDriSelection[Index] = TRUE;
+      mLastSavedDriverImageNum++;
+    }
+    StrCatS (NewString, NewStrSize/sizeof(CHAR16), DriverName);
+    NewStringToken = HiiSetString (Private->RegisteredHandle, mDriverImageToken[Index], NewString, NULL);
+    ASSERT (NewStringToken != 0);
+    mDriverImageToken[Index] = NewStringToken;
+    FreePool (NewString);
+    if (FreeDriverName) {
+      FreePool (DriverName);
+    }
+
+    //
+    // Second create the driver image device path as item help string
+    //
+    DriverName = DevicePathToStr (LoadedImageDevicePath);
+
+    NewStrSize = StrSize (DriverName);
+    NewString = AllocateZeroPool (NewStrSize);
+    ASSERT (NewString != NULL); 
+    StrCatS (NewString, NewStrSize/sizeof(CHAR16), DriverName);
+    NewStringHelpToken = HiiSetString (Private->RegisteredHandle, DriverImageFilePathToken[Index], NewString, NULL);
+    ASSERT (NewStringHelpToken != 0);
+    DriverImageFilePathToken[Index] = NewStringHelpToken;
+    FreePool (NewString);
+    FreePool (DriverName);
+
+    CheckFlags        = 0;
+    if (mDriSelection[Index]) {
+      CheckFlags |= EFI_IFR_CHECKBOX_DEFAULT;
+    }
+
+    HiiCreateCheckBoxOpCode (
+      StartOpCodeHandle,
+      (UINT16) (KEY_VALUE_DRIVER_OFFSET + Index),
+      0,
+      0,
+      NewStringToken,
+      NewStringHelpToken,
+      EFI_IFR_FLAG_CALLBACK,
+      CheckFlags,
+      NULL
+      );
+  }
+
+  //
+  // Update second page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_DRIVER,
+    StartOpCodeHandle, // Label FORM_ID_DRIVER
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+
+  if (DriverImageProtocol != NULL) {
+    FreePool (DriverImageProtocol);
+  }
+
+  if (DriverImageFilePathToken != NULL) {
+    FreePool (DriverImageFilePathToken);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Prepare to let user select the priority order of the drivers which are
+  selected in second page.
+
+  @param  Private        Pointer to EFI_CALLBACK_INFO.
+  @param  KeyValue       The callback key value of device controller item in first page.
+  @param  FakeNvData     Pointer to PLAT_OVER_MNGR_DATA.
+
+  @retval EFI_SUCCESS    Always returned.
+
+**/
+EFI_STATUS
+UpdatePrioritySelectPage (
+  IN EFI_CALLBACK_INFO                *Private,
+  IN UINT16                           KeyValue,
+  IN PLAT_OVER_MNGR_DATA              *FakeNvData
+  )
+{
+  UINTN                                     Index;
+  EFI_DEVICE_PATH_PROTOCOL                  *LoadedImageDevicePath;
+  UINTN                                     SelectedDriverImageNum;
+  UINT32                                    DriverImageNO;
+  UINTN                                     MinNO;
+  UINTN                                     Index1;
+  UINTN                                     TempNO[100];
+  UINTN                                     OrderNO[100];
+  VOID                                      *StartOpCodeHandle;
+  VOID                                      *EndOpCodeHandle;
+  VOID                                      *OptionsOpCodeHandle;
+  EFI_IFR_GUID_LABEL                        *StartLabel;
+  EFI_IFR_GUID_LABEL                        *EndLabel;
+
+  //
+  // Following code will be run if user select 'order ... priority' item in second page
+  // Prepare third page.  In third page, user will order the  drivers priority which are selected in second page
+  //
+  mCurrentPage = FORM_ID_ORDER;
+
+  //
+  // Init OpCode Handle
+  //
+  StartOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (StartOpCodeHandle != NULL);
+
+  EndOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (EndOpCodeHandle != NULL);
+
+  //
+  // Create Hii Extend Label OpCode as the start opcode
+  //
+  StartLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (StartOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  StartLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  StartLabel->Number       = FORM_ID_ORDER;
+
+  //
+  // Create Hii Extend Label OpCode as the end opcode
+  //
+  EndLabel = (EFI_IFR_GUID_LABEL *) HiiCreateGuidOpCode (EndOpCodeHandle, &gEfiIfrTianoGuid, NULL, sizeof (EFI_IFR_GUID_LABEL));
+  EndLabel->ExtendOpCode = EFI_IFR_EXTEND_OP_LABEL;
+  EndLabel->Number       = LABEL_END;
+
+  //
+  // Clear third page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_ORDER,
+    StartOpCodeHandle,
+    EndOpCodeHandle
+    );
+
+  //
+  // Check how many drivers have been selected
+  //
+  SelectedDriverImageNum = 0;
+  for (Index = 0; Index < mDriverImageHandleCount; Index++) {
+    if (mDriSelection[Index]) {
+      SelectedDriverImageNum ++;
+    }
+  }
+
+  mSelectedDriverImageNum = SelectedDriverImageNum;
+  if (SelectedDriverImageNum == 0) {
+    return EFI_SUCCESS;
+  }
+
+  OptionsOpCodeHandle = HiiAllocateOpCodeHandle ();
+  ASSERT (OptionsOpCodeHandle != NULL);
+
+  //
+  // Create order list for those selected drivers
+  //
+  SelectedDriverImageNum = 0;
+  for (Index = 0; Index < mDriverImageHandleCount; Index++) {
+    if (mDriSelection[Index]) {
+      //
+      // Use the NO. in driver binding buffer as value, will use it later
+      //
+      HiiCreateOneOfOptionOpCode (
+        OptionsOpCodeHandle,
+        mDriverImageToken[Index],
+        0,
+        EFI_IFR_NUMERIC_SIZE_1,
+        Index + 1
+        );
+
+      //
+      // Get the EFI Loaded Image Device Path Protocol
+      //
+      LoadedImageDevicePath = NULL;
+      gBS->HandleProtocol (
+                 mDriverImageHandleBuffer[Index],
+                 &gEfiLoadedImageDevicePathProtocolGuid,
+                 (VOID **) &LoadedImageDevicePath
+                 );
+      ASSERT (LoadedImageDevicePath != NULL);
+
+      //
+      // Check the driver DriverImage's order number in mapping database
+      //
+      DriverImageNO = 0;
+      CheckMapping (
+              mControllerDevicePathProtocol[mSelectedCtrIndex],
+              LoadedImageDevicePath,
+              &mMappingDataBase,
+              NULL,
+              &DriverImageNO
+              );
+      if (DriverImageNO == 0) {
+        DriverImageNO = (UINT32) mLastSavedDriverImageNum + 1;
+        mLastSavedDriverImageNum++;
+      }
+      TempNO[SelectedDriverImageNum] = DriverImageNO;
+      OrderNO[SelectedDriverImageNum] = Index + 1;
+      SelectedDriverImageNum ++;
+    }
+  }
+
+  ASSERT (SelectedDriverImageNum == mSelectedDriverImageNum);
+  //
+  // NvRamMap Must be clear firstly
+  //
+  ZeroMem (FakeNvData->DriOrder, sizeof (FakeNvData->DriOrder));
+
+  //
+  // Order the selected drivers according to the info already in mapping database
+  // the less order number in mapping database the less order number in NvRamMap
+  //
+  for (Index=0; Index < SelectedDriverImageNum; Index++) {
+    //
+    // Find the minimal order number in TempNO array,  its index in TempNO is same as IfrOptionList array
+    //
+    MinNO = 0;
+    for (Index1=0; Index1 < SelectedDriverImageNum; Index1++) {
+      if (TempNO[Index1] < TempNO[MinNO]) {
+        MinNO = Index1;
+      }
+    }
+    //
+    // the IfrOptionList[MinNO].Value = the driver NO. in driver binding buffer
+    //
+    FakeNvData->DriOrder[Index] = (UINT8) OrderNO[MinNO];
+    TempNO[MinNO] = MAX_CHOICE_NUM + 1;
+  }
+  
+  //
+  // Create Order List OpCode
+  //
+  HiiCreateOrderedListOpCode (
+    StartOpCodeHandle,
+    (UINT16) DRIVER_ORDER_QUESTION_ID,
+    VARSTORE_ID_PLAT_OVER_MNGR,
+    (UINT16) DRIVER_ORDER_VAR_OFFSET,
+    mControllerToken[mSelectedCtrIndex],
+    mControllerToken[mSelectedCtrIndex],
+    EFI_IFR_FLAG_RESET_REQUIRED,
+    0,
+    EFI_IFR_NUMERIC_SIZE_1,
+    (UINT8) MAX_CHOICE_NUM,
+    OptionsOpCodeHandle,
+    NULL
+    );
+
+  //
+  // Update third page form
+  //
+  HiiUpdateForm (
+    Private->RegisteredHandle,
+    &gPlatformOverridesManagerGuid,
+    FORM_ID_ORDER,
+    StartOpCodeHandle, // Label FORM_ID_ORDER
+    EndOpCodeHandle    // LABEL_END
+    );
+
+  HiiFreeOpCodeHandle (StartOpCodeHandle);
+  HiiFreeOpCodeHandle (EndOpCodeHandle);
+  HiiFreeOpCodeHandle (OptionsOpCodeHandle);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Save the save the mapping database to NV variable.
+
+  @param  Private        Pointer to EFI_CALLBACK_INFO.
+  @param  KeyValue       The callback key value of device controller item in first page.
+  @param  FakeNvData     Pointer to PLAT_OVER_MNGR_DATA.
+
+  @retval EFI_SUCCESS    Always returned.
+
+**/
+EFI_STATUS
+CommitChanges (
+  IN EFI_CALLBACK_INFO                *Private,
+  IN UINT16                           KeyValue,
+  IN PLAT_OVER_MNGR_DATA              *FakeNvData
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     Index;
+  UINTN                                     SelectedDriverImageNum;
+  EFI_DEVICE_PATH_PROTOCOL                  *LoadedImageDevicePath;
+  //
+  //  Following code will be run if user select 'commint changes' in third page
+  //  user enter 'Commit Changes' to save the mapping database
+  //
+  DeleteDriverImage (mControllerDevicePathProtocol[mSelectedCtrIndex], NULL, &mMappingDataBase);
+  for (SelectedDriverImageNum = 0; SelectedDriverImageNum < mSelectedDriverImageNum; SelectedDriverImageNum++) {
+    //
+    // DriOrder[SelectedDriverImageNum] = the driver NO. in driver binding buffer
+    //
+    Index = FakeNvData->DriOrder[SelectedDriverImageNum] - 1;
+
+    //
+    // Get the EFI Loaded Image Device Path Protocol
+    //
+    LoadedImageDevicePath = NULL;
+    Status = gBS->HandleProtocol (
+                        mDriverImageHandleBuffer[Index],
+                        &gEfiLoadedImageDevicePathProtocolGuid,
+                        (VOID **) &LoadedImageDevicePath
+                        );
+    ASSERT (LoadedImageDevicePath != NULL);
+
+    InsertDriverImage (
+            mControllerDevicePathProtocol[mSelectedCtrIndex],
+            LoadedImageDevicePath,
+            &mMappingDataBase,
+            (UINT32)SelectedDriverImageNum + 1
+            );
+  }
+  Status = SaveOverridesMapping (&mMappingDataBase);
+
+  return Status;
+}
+
+/**
+  This function allows a caller to extract the current configuration for one
+  or more named elements from the target driver.
+
+  @param  This         Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Request      A null-terminated Unicode string in <ConfigRequest> format.
+  @param  Progress     On return, points to a character in the Request string.
+                       Points to the string's null terminator if request was successful.
+                       Points to the most recent '&' before the first failing name/value
+                       pair (or the beginning of the string if the failure is in the
+                       first name/value pair) if the request was not successful.
+  @param  Results      A null-terminated Unicode string in <ConfigAltResp> format which
+                       has all values filled in for the names in the Request string.
+                       String to be allocated by the called function.
+
+  @retval EFI_SUCCESS            The Results is filled with the requested values.
+  @retval EFI_OUT_OF_RESOURCES   Not enough memory to store the results.
+  @retval EFI_INVALID_PARAMETER  Request is illegal syntax, or unknown name.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatOverMngrExtractConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Request,
+  OUT EFI_STRING                             *Progress,
+  OUT EFI_STRING                             *Results
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_CALLBACK_INFO                *Private;
+  EFI_HII_CONFIG_ROUTING_PROTOCOL  *HiiConfigRouting;
+  EFI_STRING                       ConfigRequestHdr;
+  EFI_STRING                       ConfigRequest;
+  BOOLEAN                          AllocatedRequest;
+  UINTN                            Size;
+  UINTN                            BufferSize;
+
+  if (Progress == NULL || Results == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Progress = Request;
+  if ((Request != NULL) && !HiiIsConfigHdrMatch (Request, &gPlatformOverridesManagerGuid, mVariableName)) {
+    return EFI_NOT_FOUND;
+  }
+
+  ConfigRequestHdr = NULL;
+  ConfigRequest    = NULL;
+  Size             = 0;
+  AllocatedRequest = FALSE;
+
+  Private          = EFI_CALLBACK_INFO_FROM_THIS (This);
+  HiiConfigRouting = Private->HiiConfigRouting;
+  ConfigRequest = Request;
+  if ((Request == NULL) || (StrStr (Request, L"OFFSET") == NULL)) {
+    //
+    // Request has no request element, construct full request string.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWWWWWWWWWWWWWW" followed by a Null-terminator
+    //
+    ConfigRequestHdr = HiiConstructConfigHdr (&gPlatformOverridesManagerGuid, mVariableName, Private->DriverHandle);
+    Size = (StrLen (ConfigRequestHdr) + 32 + 1) * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (Size);
+    ASSERT (ConfigRequest != NULL);
+    AllocatedRequest = TRUE;
+    BufferSize = sizeof (PLAT_OVER_MNGR_DATA);
+    UnicodeSPrint (ConfigRequest, Size, L"%s&OFFSET=0&WIDTH=%016LX", ConfigRequestHdr, (UINT64)BufferSize);
+    FreePool (ConfigRequestHdr);
+  }
+
+  //
+  // Convert buffer data to <ConfigResp> by helper function BlockToConfig()
+  //
+  Status = HiiConfigRouting->BlockToConfig (
+                                HiiConfigRouting,
+                                ConfigRequest,
+                                (UINT8 *) &Private->FakeNvData,
+                                sizeof (PLAT_OVER_MNGR_DATA),
+                                Results,
+                                Progress
+                                );
+
+  //
+  // Free the allocated config request string.
+  //
+  if (AllocatedRequest) {
+    FreePool (ConfigRequest);
+    ConfigRequest = NULL;
+  }
+  //
+  // Set Progress string to the original request string.
+  //
+  if (Request == NULL) {
+    *Progress = NULL;
+  } else if (StrStr (Request, L"OFFSET") == NULL) {
+    *Progress = Request + StrLen (Request);
+  }
+
+  return Status;
+}
+
+/**
+  This function processes the results of changes in configuration.
+
+  @param  This            Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Configuration   A null-terminated Unicode string in <ConfigRequest> format.
+  @param  Progress        A pointer to a string filled in with the offset of the most
+                          recent '&' before the first failing name/value pair (or the
+                          beginning of the string if the failure is in the first
+                          name/value pair) or the terminating NULL if all was successful.
+
+  @retval EFI_SUCCESS            The Results is processed successfully.
+  @retval EFI_INVALID_PARAMETER  Configuration is NULL.
+  @retval EFI_NOT_FOUND          Routing data doesn't match any storage in this driver.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatOverMngrRouteConfig (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  CONST EFI_STRING                       Configuration,
+  OUT EFI_STRING                             *Progress
+  )
+{
+  EFI_CALLBACK_INFO                         *Private;
+  UINT16                                    KeyValue;
+  PLAT_OVER_MNGR_DATA                       *FakeNvData;
+  EFI_STATUS                                Status;
+
+  if (Configuration == NULL || Progress == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  *Progress = Configuration;
+
+  if (!HiiIsConfigHdrMatch (Configuration, &gPlatformOverridesManagerGuid, mVariableName)) {
+    return EFI_NOT_FOUND;
+  }
+  
+  *Progress = Configuration + StrLen (Configuration);
+  Private    = EFI_CALLBACK_INFO_FROM_THIS (This);
+  FakeNvData = &Private->FakeNvData;
+  if (!HiiGetBrowserData (&gPlatformOverridesManagerGuid, mVariableName, sizeof (PLAT_OVER_MNGR_DATA), (UINT8 *) FakeNvData)) {
+    //
+    // FakeNvData can't be got from SetupBrowser, which doesn't need to be set.
+    //
+    return EFI_SUCCESS;
+  }
+
+  Status = EFI_SUCCESS;
+
+  if (mCurrentPage == FORM_ID_ORDER) {
+    KeyValue = KEY_VALUE_ORDER_SAVE_AND_EXIT;
+    Status = CommitChanges (Private, KeyValue, FakeNvData);
+  }
+
+  return Status;
+}
+
+/**
+  This is the function that is called to provide results data to the driver.  This data
+  consists of a unique key which is used to identify what data is either being passed back
+  or being asked for.
+
+  @param  This           Points to the EFI_HII_CONFIG_ACCESS_PROTOCOL.
+  @param  Action         A null-terminated Unicode string in <ConfigRequest> format.
+  @param  KeyValue       A unique Goto OpCode callback value which record user's selection.
+                         0x100 <= KeyValue <0x500 : user select a controller item in the first page;
+                         KeyValue == 0x1234       : user select 'Refresh' in first page, or user select 'Go to Previous Menu' in second page
+                         KeyValue == 0x1235       : user select 'Pci device filter' in first page
+                         KeyValue == 0x1500       : user select 'order ... priority' item in second page
+                         KeyValue == 0x1800       : user select 'commint changes' in third page
+                         KeyValue == 0x2000       : user select 'Go to Previous Menu' in third page
+  @param  Type           The type of value for the question.
+  @param  Value          A pointer to the data being sent to the original exporting driver.
+  @param  ActionRequest  On return, points to the action requested by the callback function.
+
+  @retval EFI_SUCCESS    Always returned.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatOverMngrCallback (
+  IN  CONST EFI_HII_CONFIG_ACCESS_PROTOCOL   *This,
+  IN  EFI_BROWSER_ACTION                     Action,
+  IN  EFI_QUESTION_ID                        KeyValue,
+  IN  UINT8                                  Type,
+  IN  EFI_IFR_TYPE_VALUE                     *Value,
+  OUT EFI_BROWSER_ACTION_REQUEST             *ActionRequest
+  )
+{
+  EFI_CALLBACK_INFO                         *Private;
+  EFI_STATUS                                Status;
+  EFI_STRING_ID                             NewStringToken;
+  EFI_INPUT_KEY                             Key;
+  PLAT_OVER_MNGR_DATA                       *FakeNvData;
+
+  if ((Action != EFI_BROWSER_ACTION_CHANGING) && (Action != EFI_BROWSER_ACTION_CHANGED)) {
+    //
+    // All other action return unsupported.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  Private = EFI_CALLBACK_INFO_FROM_THIS (This);
+  FakeNvData = &Private->FakeNvData;
+  if (!HiiGetBrowserData (&gPlatformOverridesManagerGuid, mVariableName, sizeof (PLAT_OVER_MNGR_DATA), (UINT8 *) FakeNvData)) {
+    return EFI_NOT_FOUND;
+  }
+
+  if (Action == EFI_BROWSER_ACTION_CHANGING) {
+    if (Value == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+    
+    if (KeyValue == KEY_VALUE_DRIVER_GOTO_PREVIOUS) {
+      UpdateDeviceSelectPage (Private, KeyValue, FakeNvData);
+      //
+      // Update page title string
+      //
+      NewStringToken = STRING_TOKEN (STR_TITLE);
+      if (HiiSetString (Private->RegisteredHandle, NewStringToken, L"First, Select the controller by device path", NULL) == 0) {
+        ASSERT (FALSE);
+      }
+    }
+
+    if (((KeyValue >= KEY_VALUE_DEVICE_OFFSET) && (KeyValue < KEY_VALUE_DEVICE_OFFSET + mMaxDeviceCount)) || (KeyValue == KEY_VALUE_ORDER_GOTO_PREVIOUS)) {
+      if (KeyValue == KEY_VALUE_ORDER_GOTO_PREVIOUS) {
+        KeyValue = (EFI_QUESTION_ID) (mSelectedCtrIndex + KEY_VALUE_DEVICE_OFFSET);
+      }
+      UpdateBindingDriverSelectPage (Private, KeyValue, FakeNvData);
+      //
+      // Update page title string
+      //
+      NewStringToken = STRING_TOKEN (STR_TITLE);
+      if (HiiSetString (Private->RegisteredHandle, NewStringToken, L"Second, Select drivers for the previous selected controller", NULL) == 0) {
+        ASSERT (FALSE);
+      }
+    }
+
+    if (KeyValue == KEY_VALUE_DRIVER_GOTO_ORDER) {
+      UpdatePrioritySelectPage (Private, KeyValue, FakeNvData);
+      //
+      // Update page title string
+      //
+      NewStringToken = STRING_TOKEN (STR_TITLE);
+      if (HiiSetString (Private->RegisteredHandle, NewStringToken, L"Finally, Set the priority order for the drivers and save them", NULL) == 0) {
+        ASSERT (FALSE);
+      }
+    }
+
+    if (KeyValue == KEY_VALUE_DEVICE_CLEAR) {
+      //
+      // Deletes all environment variable(s) that contain the override mappings info
+      //
+      FreeMappingDatabase (&mMappingDataBase);
+      Status = SaveOverridesMapping (&mMappingDataBase);
+      UpdateDeviceSelectPage (Private, KeyValue, FakeNvData);
+    }
+  } else if (Action == EFI_BROWSER_ACTION_CHANGED) {
+    if ((KeyValue >= KEY_VALUE_DRIVER_OFFSET) && (KeyValue < KEY_VALUE_DRIVER_OFFSET + mDriverImageHandleCount)) {
+      mDriSelection[KeyValue - KEY_VALUE_DRIVER_OFFSET] = Value->b;
+    } else {
+      switch (KeyValue) {
+      case KEY_VALUE_DEVICE_REFRESH:
+      case KEY_VALUE_DEVICE_FILTER:
+        UpdateDeviceSelectPage (Private, KeyValue, FakeNvData);
+        //
+        // Update page title string
+        //
+        NewStringToken = STRING_TOKEN (STR_TITLE);
+        if (HiiSetString (Private->RegisteredHandle, NewStringToken, L"First, Select the controller by device path", NULL) == 0) {
+          ASSERT (FALSE);
+        }
+      break;
+      
+      case KEY_VALUE_ORDER_SAVE_AND_EXIT:
+        Status = CommitChanges (Private, KeyValue, FakeNvData);
+        *ActionRequest = EFI_BROWSER_ACTION_REQUEST_SUBMIT;
+        if (EFI_ERROR (Status)) {
+          CreatePopUp (EFI_LIGHTGRAY | EFI_BACKGROUND_BLUE, &Key, L"Single Override Info too large, Saving Error!", NULL);
+          return EFI_DEVICE_ERROR;
+        }
+      break;
+
+      default:
+      break;
+      }
+    }
+  }
+
+  //
+  // Pass changed uncommitted data back to Form Browser
+  //
+  HiiSetBrowserData (&gPlatformOverridesManagerGuid, mVariableName, sizeof (PLAT_OVER_MNGR_DATA), (UINT8 *) FakeNvData, NULL);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Retrieves the image handle of the platform override driver for a controller in the system.
+
+  @param  This                   A pointer to the
+                                 EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL instance.
+  @param  ControllerHandle       The device handle of the controller to check if a
+                                 driver override exists.
+  @param  DriverImageHandle      On input, a pointer to the previous driver image
+                                 handle returned by GetDriver().  On output, a
+                                 pointer to the next driver image handle. Passing
+                                 in a NULL,  will return the first driver image
+                                 handle for ControllerHandle.
+
+  @retval EFI_SUCCESS            The driver override for ControllerHandle was
+                                 returned in DriverImageHandle.
+  @retval EFI_NOT_FOUND          A driver override for ControllerHandle was not
+                                 found.
+  @retval EFI_INVALID_PARAMETER  The handle specified by ControllerHandle is NULL.
+                                 DriverImageHandle is not a handle that was returned
+                                 on a previous  call to GetDriver().
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriver (
+  IN EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN     EFI_HANDLE                                     ControllerHandle,
+  IN OUT EFI_HANDLE                                     *DriverImageHandle
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check that ControllerHandle is a valid handle
+  //
+  if (ControllerHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Read the environment variable(s) that contain the override mappings from Controller Device Path to
+  // a set of Driver Device Paths, and  initialize in memory database of the overrides that map Controller
+  // Device Paths to an ordered set of Driver Device Paths and Driver Handles. This action is only performed
+  // once and finished in first call.
+  //
+  if (!mEnvironmentVariableRead) {
+    mEnvironmentVariableRead = TRUE;
+
+    Status = InitOverridesMapping (&mMappingDataBase);
+    if (EFI_ERROR (Status)){
+      DEBUG ((DEBUG_ERROR, "The status to Get Platform Driver Override Variable is %r\n", Status));
+      InitializeListHead (&mMappingDataBase);
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  //
+  // if the environment variable does not exist, just return not found
+  //
+  if (IsListEmpty (&mMappingDataBase)) {
+    return EFI_NOT_FOUND;
+  }
+
+  return GetDriverFromMapping (
+            ControllerHandle,
+            DriverImageHandle,
+            &mMappingDataBase,
+            mCallerImageHandle
+            );
+}
+
+/**
+  Retrieves the device path of the platform override driver for a controller in the system.
+  This driver doesn't support this API.
+
+  @param  This                  A pointer to the EFI_PLATFORM_DRIVER_OVERRIDE_
+                                PROTOCOL instance.                            
+  @param  ControllerHandle      The device handle of the controller to check if a driver override
+                                exists.                                                          
+  @param  DriverImagePath       On input, a pointer to the previous driver device path returned by
+                                GetDriverPath(). On output, a pointer to the next driver
+                                device path. Passing in a pointer to NULL, will return the first
+                                driver device path for ControllerHandle.
+  
+  @retval EFI_UNSUPPORTED
+**/
+EFI_STATUS
+EFIAPI
+GetDriverPath (
+  IN EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL              *This,
+  IN     EFI_HANDLE                                     ControllerHandle,
+  IN OUT EFI_DEVICE_PATH_PROTOCOL                       **DriverImagePath
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+
+/**
+  Used to associate a driver image handle with a device path that was returned on a prior call to the
+  GetDriverPath() service. This driver image handle will then be available through the               
+  GetDriver() service. This driver doesn't support this API.
+
+  @param  This                  A pointer to the EFI_PLATFORM_DRIVER_OVERRIDE_
+                                PROTOCOL instance.                            
+  @param  ControllerHandle      The device handle of the controller.                                                             
+  @param  DriverImagePath       A pointer to the driver device path that was returned in a prior
+                                call to GetDriverPath().                                                                        
+  @param  DriverImageHandle     The driver image handle that was returned by LoadImage()
+                                when the driver specified by DriverImagePath was loaded 
+                                into memory. 
+  
+  @retval EFI_UNSUPPORTED
+**/
+EFI_STATUS
+EFIAPI
+DriverLoaded (
+  IN EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL          *This,
+  IN EFI_HANDLE                                     ControllerHandle,
+  IN EFI_DEVICE_PATH_PROTOCOL                       *DriverImagePath,
+  IN EFI_HANDLE                                     DriverImageHandle
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  The driver Entry Point. The funciton will export a disk device class formset and
+  its callback function to hii database.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    The entry point is executed successfully.
+  @retval other          Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+PlatDriOverrideDxeInit (
+  IN EFI_HANDLE                   ImageHandle,
+  IN EFI_SYSTEM_TABLE             *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_FORM_BROWSER2_PROTOCOL  *FormBrowser2;
+  VOID                        *Instance;
+  
+  //
+  // There should only be one Form Configuration protocol
+  //
+  Status = gBS->LocateProtocol (
+                 &gEfiFormBrowser2ProtocolGuid,
+                 NULL,
+                 (VOID **) &FormBrowser2
+                 );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // According to UEFI spec, there can be at most a single instance
+  // in the system of the EFI_PLATFORM_DRIVER_OVERRIDE_PROTOCOL.
+  // So here we check the existence.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiPlatformDriverOverrideProtocolGuid,
+                  NULL,
+                  &Instance
+                  );
+  //
+  // If there was no error, assume there is an installation and return error
+  //
+  if (!EFI_ERROR (Status)) {
+    return EFI_ALREADY_STARTED;
+  }
+  
+  mCallerImageHandle = ImageHandle;
+  mCallbackInfo = AllocateZeroPool (sizeof (EFI_CALLBACK_INFO));
+  if (mCallbackInfo == NULL) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  mCallbackInfo->Signature = EFI_CALLBACK_INFO_SIGNATURE;
+  mCallbackInfo->ConfigAccess.ExtractConfig = PlatOverMngrExtractConfig;
+  mCallbackInfo->ConfigAccess.RouteConfig   = PlatOverMngrRouteConfig;
+  mCallbackInfo->ConfigAccess.Callback      = PlatOverMngrCallback;
+  mCallbackInfo->PlatformDriverOverride.GetDriver      = GetDriver;
+  mCallbackInfo->PlatformDriverOverride.GetDriverPath  = GetDriverPath;
+  mCallbackInfo->PlatformDriverOverride.DriverLoaded   = DriverLoaded;
+
+  //
+  // Locate ConfigRouting protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiConfigRoutingProtocolGuid,
+                  NULL,
+                  (VOID **) &mCallbackInfo->HiiConfigRouting
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Finish;
+  }
+
+  //
+  // Install Device Path Protocol and Config Access protocol to driver handle
+  // Install Platform Driver Override Protocol to driver handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mCallbackInfo->DriverHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mHiiVendorDevicePath,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  &mCallbackInfo->ConfigAccess,
+                  &gEfiPlatformDriverOverrideProtocolGuid,
+                  &mCallbackInfo->PlatformDriverOverride,
+                  NULL
+                  );
+  if (EFI_ERROR (Status)) {
+    goto Finish;
+  }
+
+  //
+  // Publish our HII data
+  //
+  mCallbackInfo->RegisteredHandle = HiiAddPackages (
+                                     &gPlatformOverridesManagerGuid,
+                                     mCallbackInfo->DriverHandle,
+                                     VfrBin,
+                                     PlatDriOverrideDxeStrings,
+                                     NULL
+                                     );
+  if (mCallbackInfo->RegisteredHandle == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Finish;
+  }
+
+  //
+  // Clear all the globle variable
+  //
+  mDriverImageHandleCount = 0;
+  mCurrentPage = 0;
+  
+  return EFI_SUCCESS;
+
+Finish:
+  PlatDriOverrideDxeUnload (ImageHandle);
+
+  return Status;
+}
+
+/**
+  Unload its installed protocol.
+
+  @param[in]  ImageHandle       Handle that identifies the image to be unloaded.
+
+  @retval EFI_SUCCESS           The image has been unloaded.
+**/
+EFI_STATUS
+EFIAPI
+PlatDriOverrideDxeUnload (
+  IN EFI_HANDLE  ImageHandle
+  )
+{
+  ASSERT (mCallbackInfo != NULL);
+
+  if (mCallbackInfo->DriverHandle != NULL) {
+    gBS->UninstallMultipleProtocolInterfaces (
+           mCallbackInfo->DriverHandle,
+           &gEfiDevicePathProtocolGuid,
+           &mHiiVendorDevicePath,
+           &gEfiHiiConfigAccessProtocolGuid,
+           &mCallbackInfo->ConfigAccess,
+           &gEfiPlatformDriverOverrideProtocolGuid,
+           &mCallbackInfo->PlatformDriverOverride,
+           NULL
+           );
+  }
+
+  if (mCallbackInfo->RegisteredHandle != NULL) {
+    HiiRemovePackages (mCallbackInfo->RegisteredHandle);
+  }
+
+  FreePool (mCallbackInfo);
+
+  if (mControllerToken != NULL) {
+    FreePool (mControllerToken);
+  }
+
+  if (mControllerDevicePathProtocol != NULL) {
+    FreePool (mControllerDevicePathProtocol);
+  }
+
+  if (mDriverImageToken != NULL) {
+    FreePool (mDriverImageToken);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.uni b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.uni
new file mode 100644
index 0000000000..f4e1ad7cee
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxe.uni
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxeExtra.uni b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxeExtra.uni
new file mode 100644
index 0000000000..66e893314e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideLib.c b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideLib.c
new file mode 100644
index 0000000000..7d880e6277
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatDriOverrideLib.c
@@ -0,0 +1,1938 @@
+/** @file
+  Implementation of the shared functions to do the platform driver vverride mapping.
+
+  Copyright (c) 2007 - 2015, 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 "InternalPlatDriOverrideDxe.h"
+
+#define PLATFORM_OVERRIDE_ITEM_SIGNATURE      SIGNATURE_32('p','d','o','i')
+ typedef struct _PLATFORM_OVERRIDE_ITEM {
+  UINTN                                 Signature;
+  LIST_ENTRY                            Link;
+  UINT32                                DriverInfoNum;
+  EFI_DEVICE_PATH_PROTOCOL              *ControllerDevicePath;
+  ///
+  /// List of DRIVER_IMAGE_INFO
+  ///
+  LIST_ENTRY                            DriverInfoList;
+  EFI_HANDLE                            LastReturnedImageHandle;
+} PLATFORM_OVERRIDE_ITEM;
+
+#define DRIVER_IMAGE_INFO_SIGNATURE           SIGNATURE_32('p','d','i','i')
+typedef struct _DRIVER_IMAGE_INFO {
+  UINTN                                 Signature;
+  LIST_ENTRY                            Link;
+  EFI_HANDLE                            ImageHandle;
+  EFI_DEVICE_PATH_PROTOCOL              *DriverImagePath;
+  BOOLEAN                               UnLoadable;
+  BOOLEAN                               UnStartable;
+} DRIVER_IMAGE_INFO;
+
+#define DEVICE_PATH_STACK_ITEM_SIGNATURE      SIGNATURE_32('d','p','s','i')
+typedef struct _DEVICE_PATH_STACK_ITEM{
+  UINTN                                 Signature;
+  LIST_ENTRY                            Link;
+  EFI_DEVICE_PATH_PROTOCOL              *DevicePath;
+} DEVICE_PATH_STACK_ITEM;
+
+
+LIST_ENTRY   mDevicePathStack = INITIALIZE_LIST_HEAD_VARIABLE (mDevicePathStack);
+
+/**
+  Push a controller device path into a globle device path list.
+
+  @param  DevicePath     The controller device path to push into stack
+
+  @retval EFI_SUCCESS    Device path successfully pushed into the stack.
+
+**/
+EFI_STATUS
+EFIAPI
+PushDevPathStack (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  DEVICE_PATH_STACK_ITEM  *DevicePathStackItem;
+
+  DevicePathStackItem = AllocateZeroPool (sizeof (DEVICE_PATH_STACK_ITEM));
+  ASSERT (DevicePathStackItem != NULL);
+  DevicePathStackItem->Signature = DEVICE_PATH_STACK_ITEM_SIGNATURE;
+  DevicePathStackItem->DevicePath = DuplicateDevicePath (DevicePath);
+  InsertTailList (&mDevicePathStack, &DevicePathStackItem->Link);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Pop a controller device path from a globle device path list
+
+  @param  DevicePath     The controller device path popped from stack
+
+  @retval EFI_SUCCESS    Controller device path successfully popped.
+  @retval EFI_NOT_FOUND  Stack is empty.
+
+**/
+EFI_STATUS
+EFIAPI
+PopDevPathStack (
+  OUT  EFI_DEVICE_PATH_PROTOCOL    **DevicePath
+  )
+{
+  DEVICE_PATH_STACK_ITEM  *DevicePathStackItem;
+  LIST_ENTRY              *ItemListIndex;
+
+  ItemListIndex = mDevicePathStack.BackLink;
+  //
+  // Check if the stack is empty
+  //
+  if (ItemListIndex != &mDevicePathStack){
+    DevicePathStackItem = CR(ItemListIndex, DEVICE_PATH_STACK_ITEM, Link, DEVICE_PATH_STACK_ITEM_SIGNATURE);
+    if (DevicePath != NULL) {
+      *DevicePath = DuplicateDevicePath (DevicePathStackItem->DevicePath);
+    }
+    FreePool (DevicePathStackItem->DevicePath);
+    RemoveEntryList (&DevicePathStackItem->Link);
+    FreePool (DevicePathStackItem);
+    return EFI_SUCCESS;
+  }
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Check whether a controller device path is in a globle device path list
+
+  @param  DevicePath     The controller device path to check
+
+  @retval TRUE           DevicePath exists in the stack.
+  @retval FALSE          DevicePath does not exist in the stack.
+
+**/
+BOOLEAN
+EFIAPI
+CheckExistInStack (
+  IN  EFI_DEVICE_PATH_PROTOCOL    *DevicePath
+  )
+{
+  DEVICE_PATH_STACK_ITEM  *DevicePathStackItem;
+  LIST_ENTRY              *ItemListIndex;
+  UINTN                   DevicePathSize;
+
+  ItemListIndex = mDevicePathStack.BackLink;
+  while (ItemListIndex != &mDevicePathStack){
+    DevicePathStackItem = CR(ItemListIndex, DEVICE_PATH_STACK_ITEM, Link, DEVICE_PATH_STACK_ITEM_SIGNATURE);
+    DevicePathSize = GetDevicePathSize (DevicePath);
+    if (DevicePathSize == GetDevicePathSize (DevicePathStackItem->DevicePath)) {
+      if (CompareMem (DevicePath, DevicePathStackItem->DevicePath, DevicePathSize) == 0) {
+        return TRUE;
+      }
+    }
+    ItemListIndex = ItemListIndex->BackLink;
+  }
+
+  return FALSE;
+}
+
+/**
+  Update the FV file device path if it is not valid.
+
+  According to a file GUID, check a Fv file device path is valid. If it is invalid,
+  try to return the valid device path.
+  FV address maybe changes for memory layout adjust from time to time, use this funciton
+  could promise the Fv file device path is right.
+
+  @param  DevicePath               On input, the FV file device path to check
+                                   On output, the updated valid FV file device path
+  @param  FileGuid                 The FV file GUID
+  @param  CallerImageHandle        Image handle of the caller
+
+  @retval EFI_INVALID_PARAMETER    the input DevicePath or FileGuid is invalid
+                                   parameter
+  @retval EFI_UNSUPPORTED          the input DevicePath does not contain FV file
+                                   GUID at all
+  @retval EFI_ALREADY_STARTED      the input DevicePath has pointed to FV file, it
+                                   is valid
+  @retval EFI_SUCCESS              Successfully updated the invalid DevicePath,
+                                   and return the updated device path in DevicePath
+
+**/
+EFI_STATUS
+EFIAPI
+UpdateFvFileDevicePath (
+  IN  OUT EFI_DEVICE_PATH_PROTOCOL      **DevicePath,
+  IN  EFI_GUID                          *FileGuid,
+  IN  EFI_HANDLE                        CallerImageHandle
+  )
+{
+  EFI_DEVICE_PATH_PROTOCOL      *TempDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL      *LastDeviceNode;
+  EFI_STATUS                    Status;
+  EFI_GUID                      *GuidPoint;
+  UINTN                         Index;
+  UINTN                         FvHandleCount;
+  EFI_HANDLE                    *FvHandleBuffer;
+  EFI_FV_FILETYPE               Type;
+  UINTN                         Size;
+  EFI_FV_FILE_ATTRIBUTES        Attributes;
+  UINT32                        AuthenticationStatus;
+  BOOLEAN                       FindFvFile;
+  EFI_LOADED_IMAGE_PROTOCOL     *LoadedImage;
+  EFI_FIRMWARE_VOLUME2_PROTOCOL *Fv;
+  MEDIA_FW_VOL_FILEPATH_DEVICE_PATH FvFileNode;
+  EFI_HANDLE                    FoundFvHandle;
+  EFI_DEVICE_PATH_PROTOCOL      *NewDevicePath;
+  BOOLEAN                       HasFvNode;
+
+  if (DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (*DevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the device path points to the default the input FV file
+  //
+  TempDevicePath = *DevicePath;
+  LastDeviceNode = TempDevicePath;
+  while (!IsDevicePathEnd (TempDevicePath)) {
+     LastDeviceNode = TempDevicePath;
+     TempDevicePath = NextDevicePathNode (TempDevicePath);
+  }
+  GuidPoint = EfiGetNameGuidFromFwVolDevicePathNode ((MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) LastDeviceNode);
+  if (GuidPoint == NULL) {
+    //
+    // If this option does not point to a FV file, just return EFI_UNSUPPORTED.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  if (FileGuid != NULL) {
+    if (!CompareGuid (GuidPoint, FileGuid)) {
+      //
+      // If the FV file is not the input file GUID, just return EFI_UNSUPPORTED
+      //
+      return EFI_UNSUPPORTED;
+    }
+  } else {
+    FileGuid = GuidPoint;
+  }
+
+  //
+  // Check to see if the device path contains memory map node
+  //
+  TempDevicePath = *DevicePath;
+  HasFvNode = FALSE;
+  while (!IsDevicePathEnd (TempDevicePath)) {
+    //
+    // Use old Device Path
+    //
+    if (DevicePathType (TempDevicePath) == HARDWARE_DEVICE_PATH &&
+        DevicePathSubType (TempDevicePath) == HW_MEMMAP_DP) {
+      HasFvNode = TRUE;
+      break;
+    }
+    TempDevicePath = NextDevicePathNode (TempDevicePath);
+  }
+
+  if (!HasFvNode) {
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Check whether the input Fv file device path is valid
+  //
+  TempDevicePath = *DevicePath;
+  FoundFvHandle = NULL;
+  Status = gBS->LocateDevicePath (
+                  &gEfiFirmwareVolume2ProtocolGuid,
+                  &TempDevicePath,
+                  &FoundFvHandle
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->HandleProtocol (
+                    FoundFvHandle,
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    (VOID **) &Fv
+                    );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Set FV ReadFile Buffer as NULL, only need to check whether input Fv file exist there
+      //
+      Status = Fv->ReadFile (
+                     Fv,
+                     FileGuid,
+                     NULL,
+                     &Size,
+                     &Type,
+                     &Attributes,
+                     &AuthenticationStatus
+                     );
+      if (!EFI_ERROR (Status)) {
+        return EFI_ALREADY_STARTED;
+      }
+    }
+  }
+
+  //
+  // Look for the input wanted FV file in current FV
+  // First, try to look for in Caller own FV. Caller and input wanted FV file usually are in the same FV
+  //
+  FindFvFile = FALSE;
+  FoundFvHandle = NULL;
+  Status = gBS->HandleProtocol (
+                  CallerImageHandle,
+                  &gEfiLoadedImageProtocolGuid,
+                  (VOID **) &LoadedImage
+                  );
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->HandleProtocol (
+                    LoadedImage->DeviceHandle,
+                    &gEfiFirmwareVolume2ProtocolGuid,
+                    (VOID **) &Fv
+                    );
+    if (!EFI_ERROR (Status)) {
+      Status = Fv->ReadFile (
+                     Fv,
+                     FileGuid,
+                     NULL,
+                     &Size,
+                     &Type,
+                     &Attributes,
+                     &AuthenticationStatus
+                     );
+      if (!EFI_ERROR (Status)) {
+        FindFvFile = TRUE;
+        FoundFvHandle = LoadedImage->DeviceHandle;
+      }
+    }
+  }
+  //
+  // Second, if fail to find, try to enumerate all FV
+  //
+  if (!FindFvFile) {
+    gBS->LocateHandleBuffer (
+           ByProtocol,
+           &gEfiFirmwareVolume2ProtocolGuid,
+           NULL,
+           &FvHandleCount,
+           &FvHandleBuffer
+           );
+    for (Index = 0; Index < FvHandleCount; Index++) {
+      gBS->HandleProtocol (
+             FvHandleBuffer[Index],
+             &gEfiFirmwareVolume2ProtocolGuid,
+             (VOID **) &Fv
+             );
+
+      Status = Fv->ReadFile (
+                     Fv,
+                     FileGuid,
+                     NULL,
+                     &Size,
+                     &Type,
+                     &Attributes,
+                     &AuthenticationStatus
+                     );
+      if (EFI_ERROR (Status)) {
+        //
+        // Skip if input Fv file not in the FV
+        //
+        continue;
+      }
+      FindFvFile = TRUE;
+      FoundFvHandle = FvHandleBuffer[Index];
+      break;
+    }
+  }
+
+  if (FindFvFile) {
+    //
+    // Build the shell device path
+    //
+    NewDevicePath = DevicePathFromHandle (FoundFvHandle);
+    EfiInitializeFwVolDevicepathNode (&FvFileNode, FileGuid);
+    NewDevicePath = AppendDevicePathNode (NewDevicePath, (EFI_DEVICE_PATH_PROTOCOL *) &FvFileNode);
+    *DevicePath = NewDevicePath;
+    return EFI_SUCCESS;
+  }
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Gets the data and size of a variable.
+
+  Read the EFI variable (VendorGuid/Name) and return a dynamically allocated
+  buffer, and the size of the buffer. If failure return NULL.
+
+  @param  Name                     String part of EFI variable name
+  @param  VendorGuid               GUID part of EFI variable name
+  @param  VariableSize             Returns the size of the EFI variable that was
+                                   read
+
+  @return Dynamically allocated memory that contains a copy of the EFI variable.
+          Caller is responsible freeing the buffer.
+  @retval NULL                     Variable was not read
+
+**/
+VOID *
+EFIAPI
+GetVariableAndSize (
+  IN  CHAR16              *Name,
+  IN  EFI_GUID            *VendorGuid,
+  OUT UINTN               *VariableSize
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       BufferSize;
+  VOID        *Buffer;
+
+  Buffer = NULL;
+
+  //
+  // Pass in a zero size buffer to find the required buffer size.
+  //
+  BufferSize  = 0;
+  Status      = gRT->GetVariable (Name, VendorGuid, NULL, &BufferSize, Buffer);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // Allocate the buffer to return
+    //
+    Buffer = AllocateZeroPool (BufferSize);
+    if (Buffer == NULL) {
+      return NULL;
+    }
+    //
+    // Read variable into the allocated buffer.
+    //
+    Status = gRT->GetVariable (Name, VendorGuid, NULL, &BufferSize, Buffer);
+    if (EFI_ERROR (Status)) {
+      BufferSize = 0;
+    }
+  }
+
+  *VariableSize = BufferSize;
+  return Buffer;
+}
+
+/**
+  Connect to the handle to a device on the device path.
+
+  This function will create all handles associate with every device
+  path node. If the handle associate with one device path node can not
+  be created success, then still give one chance to do the dispatch,
+  which load the missing drivers if possible.
+
+  @param  DevicePathToConnect      The device path which will be connected, it can
+                                   be a multi-instance device path
+
+  @retval EFI_SUCCESS              All handles associate with every device path
+                                   node have been created
+  @retval EFI_OUT_OF_RESOURCES     There is no resource to create new handles
+  @retval EFI_NOT_FOUND            Create the handle associate with one device
+                                   path node failed
+
+**/
+EFI_STATUS
+EFIAPI
+ConnectDevicePath (
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePathToConnect
+  )
+{
+  EFI_STATUS                Status;
+  EFI_DEVICE_PATH_PROTOCOL  *DevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *CopyOfDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *Instance;
+  EFI_DEVICE_PATH_PROTOCOL  *RemainingDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL  *Next;
+  EFI_HANDLE                Handle;
+  EFI_HANDLE                PreviousHandle;
+  UINTN                     Size;
+
+  if (DevicePathToConnect == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  DevicePath        = DuplicateDevicePath (DevicePathToConnect);
+  CopyOfDevicePath  = DevicePath;
+  if (DevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  do {
+    //
+    // The outer loop handles multi instance device paths.
+    // Only console variables contain multiple instance device paths.
+    //
+    // After this call DevicePath points to the next Instance
+    //
+    Instance  = GetNextDevicePathInstance (&DevicePath, &Size);
+    ASSERT (Instance != NULL);
+
+    Next      = Instance;
+    while (!IsDevicePathEndType (Next)) {
+      Next = NextDevicePathNode (Next);
+    }
+
+    SetDevicePathEndNode (Next);
+
+    //
+    // Start the real work of connect with RemainingDevicePath
+    //
+    PreviousHandle = NULL;
+    do {
+      //
+      // Find the handle that best matches the Device Path. If it is only a
+      // partial match the remaining part of the device path is returned in
+      // RemainingDevicePath.
+      //
+      RemainingDevicePath = Instance;
+      Status              = gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &RemainingDevicePath, &Handle);
+
+      if (!EFI_ERROR (Status)) {
+        if (Handle == PreviousHandle) {
+          //
+          // If no forward progress is made try invoking the Dispatcher.
+          // A new FV may have been added to the system an new drivers
+          // may now be found.
+          // Status == EFI_SUCCESS means a driver was dispatched
+          // Status == EFI_NOT_FOUND means no new drivers were dispatched
+          //
+          Status = gDS->Dispatch ();
+        }
+
+        if (!EFI_ERROR (Status)) {
+          PreviousHandle = Handle;
+          //
+          // Connect all drivers that apply to Handle and RemainingDevicePath,
+          // the Recursive flag is FALSE so only one level will be expanded.
+          //
+          // Do not check the connect status here, if the connect controller fail,
+          // then still give the chance to do dispatch, because partial
+          // RemainingDevicepath may be in the new FV
+          //
+          // 1. If the connect fails, RemainingDevicepath and handle will not
+          //    change, so next time will do the dispatch, then dispatch's status
+          //    will take effect
+          // 2. If the connect succeeds, the RemainingDevicepath and handle will
+          //    change, then avoid the dispatch, we have chance to continue the
+          //    next connection
+          //
+          gBS->ConnectController (Handle, NULL, RemainingDevicePath, FALSE);
+        }
+      }
+      //
+      // Loop until RemainingDevicePath is an empty device path
+      //
+    } while (!EFI_ERROR (Status) && !IsDevicePathEnd (RemainingDevicePath));
+
+  } while (DevicePath != NULL);
+
+  if (CopyOfDevicePath != NULL) {
+    FreePool (CopyOfDevicePath);
+  }
+  //
+  // All handle with DevicePath exists in the handle database
+  //
+  return Status;
+}
+
+/**
+  Free all the mapping database memory resource and initialize the mapping list entry.
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_SUCCESS              Mapping database successfully freed
+  @retval EFI_INVALID_PARAMETER    MappingDataBase is NULL
+
+**/
+EFI_STATUS
+EFIAPI
+FreeMappingDatabase (
+  IN  OUT  LIST_ENTRY            *MappingDataBase
+  )
+{
+  LIST_ENTRY                  *OverrideItemListIndex;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+    //
+    // Free PLATFORM_OVERRIDE_ITEM.ControllerDevicePath[]
+    //
+    if (OverrideItem->ControllerDevicePath != NULL){
+      FreePool (OverrideItem->ControllerDevicePath);
+    }
+
+    ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+    while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+      //
+      // Free DRIVER_IMAGE_INFO.DriverImagePath[]
+      //
+      DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+      if (DriverImageInfo->DriverImagePath != NULL) {
+        FreePool(DriverImageInfo->DriverImagePath);
+      }
+      //
+      // Free DRIVER_IMAGE_INFO itself
+      //
+      ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+      RemoveEntryList (&DriverImageInfo->Link);
+      FreePool (DriverImageInfo);
+    }
+    //
+    // Free PLATFORM_OVERRIDE_ITEM itself
+    //
+    OverrideItemListIndex = GetNextNode (MappingDataBase, OverrideItemListIndex);
+    RemoveEntryList (&OverrideItem->Link);
+    FreePool (OverrideItem);
+  }
+
+  InitializeListHead (MappingDataBase);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create the mapping database according to variable.
+
+  Read the environment variable(s) that contain the override mappings from Controller Device Path to
+  a set of Driver Device Paths, and create the mapping database in memory with those variable info.
+  VariableLayout{
+  //
+  // NotEnd indicate whether the variable is the last one, and has no subsequent variable need to load.
+  // Each variable has MaximumVariableSize limitation, so we maybe need multiple variables to store
+  // large mapping infos.
+  // The variable(s) name rule is PlatDriOver, PlatDriOver1, PlatDriOver2, ....
+  //
+  UINT32                         NotEnd;               //Zero is the last one.
+  //
+  // The entry which contains the mapping that Controller Device Path to a set of Driver Device Paths
+  // There are often multi mapping entries in a variable.
+  //
+  UINT32                         SIGNATURE;            //SIGNATURE_32('p','d','o','i')
+  UINT32                         DriverNum;
+  EFI_DEVICE_PATH_PROTOCOL       ControllerDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  ......
+  UINT32                         NotEnd;                //Zero is the last one.
+  UINT32                         SIGNATURE;
+  UINT32                         DriverNum;
+  EFI_DEVICE_PATH_PROTOCOL       ControllerDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  EFI_DEVICE_PATH_PROTOCOL       DriverDevicePath[];
+  ......
+  }
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_SUCCESS              Create the mapping database in memory successfully
+  @retval EFI_INVALID_PARAMETER    MappingDataBase pointer is null
+  @retval EFI_NOT_FOUND            Cannot find the 'PlatDriOver' NV variable
+  @retval EFI_VOLUME_CORRUPTED     The found NV variable is corrupted
+
+**/
+EFI_STATUS
+EFIAPI
+InitOverridesMapping (
+  OUT  LIST_ENTRY            *MappingDataBase
+  )
+{
+  UINTN                       BufferSize;
+  VOID                        *VariableBuffer;
+  UINT8                       *VariableIndex;
+  UINTN                       VariableNum;
+  CHAR16                      OverrideVariableName[40];
+  UINT32                      NotEnd;
+  UINT32                      DriverNumber;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  BOOLEAN                     Corrupted;
+  UINT32                      Signature;
+  EFI_DEVICE_PATH_PROTOCOL    *ControllerDevicePath;
+  EFI_DEVICE_PATH_PROTOCOL    *DriverDevicePath;
+  UINTN                       Index;
+
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check the environment variable(s) that contain the override mappings .
+  //
+  VariableBuffer = GetVariableAndSize (L"PlatDriOver", &gEfiCallerIdGuid, &BufferSize);
+  ASSERT ((UINTN) VariableBuffer % sizeof(UINTN) == 0);
+  if (VariableBuffer == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Traverse all variables.
+  //
+  VariableNum = 1;
+  Corrupted = FALSE;
+  NotEnd = 0;
+  do {
+    VariableIndex = VariableBuffer;
+    if (VariableIndex + sizeof (UINT32) > (UINT8 *) VariableBuffer + BufferSize) {
+      Corrupted = TRUE;
+    } else {
+      //
+      // End flag
+      //
+      NotEnd = *(UINT32*) VariableIndex;
+    }
+    //
+    // Traverse the entries containing the mapping that Controller Device Path
+    // to a set of Driver Device Paths within this variable.
+    //
+    VariableIndex = VariableIndex + sizeof (UINT32);
+    while (VariableIndex < ((UINT8 *)VariableBuffer + BufferSize)) {
+      //
+      // Check signature of this entry
+      //
+      if (VariableIndex + sizeof (UINT32) > (UINT8 *) VariableBuffer + BufferSize) {
+        Corrupted = TRUE;
+        break;
+      }
+      Signature = *(UINT32 *) VariableIndex;
+      if (Signature != PLATFORM_OVERRIDE_ITEM_SIGNATURE) {
+        Corrupted = TRUE;
+        break;
+      }
+      //
+      // Create PLATFORM_OVERRIDE_ITEM for this mapping
+      //
+      OverrideItem = AllocateZeroPool (sizeof (PLATFORM_OVERRIDE_ITEM));
+      ASSERT (OverrideItem != NULL);
+      OverrideItem->Signature = PLATFORM_OVERRIDE_ITEM_SIGNATURE;
+      InitializeListHead (&OverrideItem->DriverInfoList);
+      VariableIndex = VariableIndex + sizeof (UINT32);
+      //
+      // Get DriverNum
+      //
+      if (VariableIndex + sizeof (UINT32) >= (UINT8 *) VariableBuffer + BufferSize) {
+        Corrupted = TRUE;
+        break;
+      }
+      DriverNumber = *(UINT32*) VariableIndex;
+      OverrideItem->DriverInfoNum = DriverNumber;
+      VariableIndex = VariableIndex + sizeof (UINT32);
+      //
+      // Get ControllerDevicePath[]
+      //
+      ControllerDevicePath = (EFI_DEVICE_PATH_PROTOCOL *) VariableIndex;
+      OverrideItem->ControllerDevicePath = DuplicateDevicePath (ControllerDevicePath);
+      VariableIndex = VariableIndex + GetDevicePathSize (ControllerDevicePath);
+      //
+      // Align the VariableIndex since the controller device path may not be aligned, refer to the SaveOverridesMapping()
+      //
+      VariableIndex += ((sizeof(UINT32) - ((UINTN) (VariableIndex))) & (sizeof(UINT32) - 1));
+      //
+      // Check buffer overflow.
+      //
+      if ((OverrideItem->ControllerDevicePath == NULL) || (VariableIndex < (UINT8 *) ControllerDevicePath) || 
+          (VariableIndex > (UINT8 *) VariableBuffer + BufferSize)) {
+        Corrupted = TRUE;
+        break;
+      }
+
+      //
+      // Get all DriverImageDevicePath[]
+      //
+      for (Index = 0; Index < DriverNumber; Index++) {
+        //
+        // Create DRIVER_IMAGE_INFO for this DriverDevicePath[]
+        //
+        DriverImageInfo = AllocateZeroPool (sizeof (DRIVER_IMAGE_INFO));
+        ASSERT (DriverImageInfo != NULL);
+        DriverImageInfo->Signature = DRIVER_IMAGE_INFO_SIGNATURE;
+
+        DriverDevicePath = (EFI_DEVICE_PATH_PROTOCOL *) VariableIndex;
+        DriverImageInfo->DriverImagePath = DuplicateDevicePath (DriverDevicePath);
+        VariableIndex = VariableIndex + GetDevicePathSize (DriverDevicePath);
+        //
+        // Align the VariableIndex since the driver image device path may not be aligned, refer to the SaveOverridesMapping()
+        //
+        VariableIndex += ((sizeof(UINT32) - ((UINTN) (VariableIndex))) & (sizeof(UINT32) - 1));
+
+        InsertTailList (&OverrideItem->DriverInfoList, &DriverImageInfo->Link);
+
+        //
+        // Check buffer overflow
+        //
+        if ((DriverImageInfo->DriverImagePath == NULL) || (VariableIndex < (UINT8 *) DriverDevicePath) || 
+            (VariableIndex < (UINT8 *) VariableBuffer + BufferSize)) {
+          Corrupted = TRUE;
+          break;
+        }
+      }
+      InsertTailList (MappingDataBase, &OverrideItem->Link);
+      if (Corrupted) {
+        break;
+      }
+    }
+
+    FreePool (VariableBuffer);
+    if (Corrupted) {
+      FreeMappingDatabase (MappingDataBase);
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    //
+    // If there are additional variables (PlatDriOver1, PlatDriOver2, PlatDriOver3.....), get them.
+    // NotEnd indicates whether current variable is the end variable.
+    //
+    if (NotEnd != 0) {
+      UnicodeSPrint (OverrideVariableName, sizeof (OverrideVariableName), L"PlatDriOver%d", VariableNum++);
+      VariableBuffer = GetVariableAndSize (OverrideVariableName, &gEfiCallerIdGuid, &BufferSize);
+      ASSERT ((UINTN) VariableBuffer % sizeof(UINTN) == 0);
+      if (VariableBuffer == NULL) {
+        FreeMappingDatabase (MappingDataBase);
+        return EFI_VOLUME_CORRUPTED;
+      }
+    }
+
+  } while (NotEnd != 0);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Calculate the needed size in NV variable for recording a specific PLATFORM_OVERRIDE_ITEM info.
+
+  @param  OverrideItemListIndex    Pointer to the list of a specific PLATFORM_OVERRIDE_ITEM
+
+  @return The needed size number
+
+**/
+UINTN
+EFIAPI
+GetOneItemNeededSize (
+  IN  LIST_ENTRY            *OverrideItemListIndex
+  )
+{
+  UINTN                       NeededSize;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  UINTN                       DevicePathSize;
+
+  NeededSize = 0;
+  OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+  NeededSize += sizeof (UINT32); //UINT32  SIGNATURE;
+  NeededSize += sizeof (UINT32); //UINT32  DriverNum;
+  DevicePathSize = GetDevicePathSize (OverrideItem->ControllerDevicePath);
+  NeededSize += DevicePathSize; // ControllerDevicePath
+  //
+  // Align the controller device path
+  //
+  NeededSize += ((sizeof(UINT32) - DevicePathSize) & (sizeof(UINT32) - 1));
+  //
+  // Traverse the Driver Info List of this Override Item
+  //
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+    DevicePathSize = GetDevicePathSize (DriverImageInfo->DriverImagePath);
+    NeededSize += DevicePathSize; //DriverDevicePath
+    //
+    // Align the driver image device path
+    //
+    NeededSize += ((sizeof(UINT32) - DevicePathSize) & (sizeof(UINT32) - 1));
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+  }
+
+  return NeededSize;
+}
+
+/**
+  Deletes all environment variable(s) that contain the override mappings from Controller Device Path to
+  a set of Driver Device Paths.
+
+  @retval EFI_SUCCESS  Delete all variable(s) successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DeleteOverridesVariables (
+  VOID
+  )
+{
+  EFI_STATUS                  Status;
+  VOID                        *VariableBuffer;
+  UINTN                       VariableNum;
+  UINTN                       BufferSize;
+  UINTN                       Index;
+  CHAR16                      OverrideVariableName[40];
+
+  //
+  // Get environment variable(s) number
+  //
+  VariableNum = 0;
+  VariableBuffer = GetVariableAndSize (L"PlatDriOver", &gEfiCallerIdGuid, &BufferSize);
+  VariableNum++;
+  if (VariableBuffer == NULL) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Check NotEnd to get all PlatDriOverX variable(s)
+  //
+  while ((VariableBuffer != NULL) && ((*(UINT32*)VariableBuffer) != 0)) {
+    FreePool (VariableBuffer);
+    UnicodeSPrint (OverrideVariableName, sizeof (OverrideVariableName), L"PlatDriOver%d", VariableNum);
+    VariableBuffer = GetVariableAndSize (OverrideVariableName, &gEfiCallerIdGuid, &BufferSize);
+    VariableNum++;
+  }
+
+  //
+  // Delete PlatDriOver and all additional variables, if exist.
+  //
+  Status = gRT->SetVariable (
+                  L"PlatDriOver",
+                  &gEfiCallerIdGuid,
+                  EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                  0,
+                  NULL
+                  );
+  ASSERT (!EFI_ERROR (Status));
+  for (Index = 1; Index < VariableNum; Index++) {
+    UnicodeSPrint (OverrideVariableName, sizeof (OverrideVariableName), L"PlatDriOver%d", Index);
+    Status = gRT->SetVariable (
+                    OverrideVariableName,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    0,
+                    NULL
+                    );
+    ASSERT (!EFI_ERROR (Status));
+  }
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Save the memory mapping database into NV environment variable(s).
+
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_SUCCESS              Save memory mapping database successfully
+  @retval EFI_INVALID_PARAMETER    MappingDataBase pointer is null
+
+**/
+EFI_STATUS
+EFIAPI
+SaveOverridesMapping (
+  IN  LIST_ENTRY              *MappingDataBase
+  )
+{
+  EFI_STATUS                  Status;
+  VOID                        *VariableBuffer;
+  UINT8                       *VariableIndex;
+  UINTN                       NumIndex;
+  CHAR16                      OverrideVariableName[40];
+  UINT32                      NotEnd;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  LIST_ENTRY                  *OverrideItemListIndex;
+  LIST_ENTRY                  *ItemIndex;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  UINTN                       VariableNeededSize;
+  UINT64                      MaximumVariableStorageSize;
+  UINT64                      RemainingVariableStorageSize;
+  UINT64                      MaximumVariableSize;
+  UINTN                       OneItemNeededSize;
+
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (IsListEmpty (MappingDataBase)) {
+    Status = DeleteOverridesVariables ();
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get the the maximum size of an individual EFI variable in current system
+  //
+  gRT->QueryVariableInfo (
+          EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+          &MaximumVariableStorageSize,
+          &RemainingVariableStorageSize,
+          &MaximumVariableSize
+          );
+
+  NumIndex = 0;
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    //
+    // Try to find the most proper variable size which <= MaximumVariableSize,
+    // but can contain mapping info as much as possible
+    //
+    VariableNeededSize = sizeof (UINT32); // NotEnd;
+    ItemIndex = OverrideItemListIndex;
+    NotEnd = FALSE;
+    //
+    // Traverse all PLATFORM_OVERRIDE_ITEMs and get the total size.
+    //
+    while (!IsNull (MappingDataBase, ItemIndex)) {
+      OneItemNeededSize = GetOneItemNeededSize (ItemIndex);
+      //
+      // If the total size exceeds the MaximumVariableSize, then we must use
+      // multiple variables.
+      //
+      if ((VariableNeededSize +
+           OneItemNeededSize +
+           StrSize (L"PlatDriOver ")
+           ) >= MaximumVariableSize
+          ) {
+        NotEnd = TRUE;
+        break;
+      }
+
+      VariableNeededSize += OneItemNeededSize;
+      ItemIndex = GetNextNode (MappingDataBase, ItemIndex);
+    }
+
+    if (NotEnd != 0) {
+      if (VariableNeededSize == sizeof (UINT32)) {
+        //
+        // If an individual EFI variable cannot contain a single Item, return error
+        //
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+
+    //
+    // VariableNeededSize is the most proper variable size, allocate variable buffer
+    // ItemIndex now points to the next PLATFORM_OVERRIDE_ITEM which is not covered by VariableNeededSize
+    //
+    VariableBuffer = AllocateZeroPool (VariableNeededSize);
+    ASSERT (VariableBuffer != NULL);
+    ASSERT ((UINTN) VariableBuffer % sizeof(UINTN) == 0);
+
+    //
+    // Fill the variable buffer according to MappingDataBase
+    //
+    VariableIndex = VariableBuffer;
+    *(UINT32 *) VariableIndex = NotEnd;
+    VariableIndex += sizeof (UINT32); // pass NotEnd
+    //
+    // ItemIndex points to the next PLATFORM_OVERRIDE_ITEM which is not covered by VariableNeededSize
+    //
+    while (OverrideItemListIndex != ItemIndex){
+      *(UINT32 *) VariableIndex = PLATFORM_OVERRIDE_ITEM_SIGNATURE;
+      VariableIndex += sizeof (UINT32); // pass SIGNATURE
+
+      OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+      *(UINT32 *) VariableIndex = OverrideItem->DriverInfoNum;
+      VariableIndex += sizeof (UINT32); // pass DriverNum
+
+      CopyMem (VariableIndex, OverrideItem->ControllerDevicePath, GetDevicePathSize (OverrideItem->ControllerDevicePath));
+      VariableIndex += GetDevicePathSize (OverrideItem->ControllerDevicePath); // pass ControllerDevicePath
+
+      //
+      // Align the VariableIndex since the controller device path may not be aligned
+      //
+      VariableIndex += ((sizeof(UINT32) - ((UINTN) (VariableIndex))) & (sizeof(UINT32) - 1));
+      //
+      // Save the Driver Info List of this PLATFORM_OVERRIDE_ITEM
+      //
+      ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+      while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+        DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+        CopyMem (VariableIndex, DriverImageInfo->DriverImagePath, GetDevicePathSize (DriverImageInfo->DriverImagePath));
+        VariableIndex += GetDevicePathSize (DriverImageInfo->DriverImagePath); // pass DriverImageDevicePath
+        //
+        // Align the VariableIndex since the driver image device path may not be aligned
+        //
+        VariableIndex += ((sizeof(UINT32) - ((UINTN) (VariableIndex))) & (sizeof(UINT32) - 1));
+        ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+      }
+
+      OverrideItemListIndex =  GetNextNode (MappingDataBase, OverrideItemListIndex);
+    }
+
+    ASSERT (((UINTN)VariableIndex - (UINTN)VariableBuffer) == VariableNeededSize);
+
+    if (NumIndex == 0) {
+      UnicodeSPrint (OverrideVariableName, sizeof (OverrideVariableName), L"PlatDriOver");
+    } else {
+      UnicodeSPrint (OverrideVariableName, sizeof (OverrideVariableName), L"PlatDriOver%d", NumIndex );
+    }
+
+    Status = gRT->SetVariable (
+                    OverrideVariableName,
+                    &gEfiCallerIdGuid,
+                    EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_NON_VOLATILE,
+                    VariableNeededSize,
+                    VariableBuffer
+                    );
+    FreePool (VariableBuffer);
+
+    if (EFI_ERROR (Status)) {
+      if (NumIndex > 0) {
+        //
+        // Delete all PlatDriOver variables when full mapping can't be set.  
+        //
+        DeleteOverridesVariables ();
+      }
+      return Status;
+    }
+
+    NumIndex ++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Get the first Binding protocol which has the specific image handle.
+
+  @param  ImageHandle          The Image handle
+  @param  BindingHandle        The BindingHandle of the found Driver Binding protocol.
+                               If Binding protocol is not found, it is set to NULL. 
+
+  @return                      Pointer into the Binding Protocol interface
+  @retval NULL                 The paramter is not valid or the binding protocol is not found.
+
+**/
+EFI_DRIVER_BINDING_PROTOCOL *
+EFIAPI
+GetBindingProtocolFromImageHandle (
+  IN  EFI_HANDLE   ImageHandle,
+  OUT EFI_HANDLE   *BindingHandle
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             Index;
+  UINTN                             DriverBindingHandleCount;
+  EFI_HANDLE                        *DriverBindingHandleBuffer;
+  EFI_DRIVER_BINDING_PROTOCOL       *DriverBindingInterface;
+
+  if (BindingHandle == NULL || ImageHandle == NULL) {
+    return NULL;
+  }
+  //
+  // Get all drivers which support driver binding protocol
+  //
+  DriverBindingHandleCount  = 0;
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiDriverBindingProtocolGuid,
+                  NULL,
+                  &DriverBindingHandleCount,
+                  &DriverBindingHandleBuffer
+                  );
+  if (EFI_ERROR (Status) || (DriverBindingHandleCount == 0)) {
+    return NULL;
+  }
+
+  for (Index = 0; Index < DriverBindingHandleCount; Index++) {
+    DriverBindingInterface = NULL;
+    Status = gBS->OpenProtocol (
+                    DriverBindingHandleBuffer[Index],
+                    &gEfiDriverBindingProtocolGuid,
+                    (VOID **) &DriverBindingInterface,
+                    NULL,
+                    NULL,
+                    EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                    );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if (DriverBindingInterface->ImageHandle == ImageHandle) {
+      *BindingHandle = DriverBindingHandleBuffer[Index];
+      FreePool (DriverBindingHandleBuffer);
+      return DriverBindingInterface;
+    }
+  }
+
+  //
+  // If no Driver Binding Protocol instance is found
+  //
+  FreePool (DriverBindingHandleBuffer);
+  *BindingHandle = NULL;
+  return NULL;
+}
+
+/**
+  Return the current TPL.
+
+  @return Current TPL
+
+**/
+EFI_TPL
+GetCurrentTpl (
+  VOID
+  )
+{
+  EFI_TPL                 Tpl;
+
+  Tpl = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  gBS->RestoreTPL (Tpl);
+
+  return Tpl;
+}
+
+
+/**
+  Retrieves the image handle of the platform override driver for a controller in
+  the system from the memory mapping database.
+
+  @param  ControllerHandle         The device handle of the controller to check if
+                                   a driver override exists.
+  @param  DriverImageHandle        On input, the previously returnd driver image handle.
+                                   On output, a pointer to the next driver handle.
+                                   Passing in a pointer to NULL, will return the
+                                   first driver handle for ControllerHandle.
+  @param  MappingDataBase          Mapping database list entry pointer
+  @param  CallerImageHandle        The caller driver's image handle, for
+                                   UpdateFvFileDevicePath use.
+
+  @retval EFI_INVALID_PARAMETER    The handle specified by ControllerHandle is not
+                                   a valid handle.  Or DriverImagePath is not a
+                                   device path that was returned on a previous call
+                                   to GetDriverPath().
+  @retval EFI_NOT_FOUND            A driver override for ControllerHandle was not
+                                   found.
+  @retval EFI_UNSUPPORTED          The operation is not supported.
+  @retval EFI_SUCCESS              The driver override for ControllerHandle was
+                                   returned in DriverImagePath.
+
+**/
+EFI_STATUS
+EFIAPI
+GetDriverFromMapping (
+  IN     EFI_HANDLE                                     ControllerHandle,
+  IN OUT EFI_HANDLE                                     *DriverImageHandle,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  IN     EFI_HANDLE                                     CallerImageHandle
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_DEVICE_PATH_PROTOCOL    *ControllerDevicePath;
+  BOOLEAN                     ControllerFound;
+  BOOLEAN                     ImageFound;
+  EFI_HANDLE                  *ImageHandleBuffer;
+  UINTN                       ImageHandleCount;
+  UINTN                       Index;
+  EFI_DRIVER_BINDING_PROTOCOL *DriverBinding;
+  EFI_HANDLE                  DriverBindingHandle;
+  BOOLEAN                     FoundLastReturned;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  LIST_ENTRY                  *OverrideItemListIndex;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  EFI_DEVICE_PATH_PROTOCOL    *TempDriverImagePath;
+  EFI_HANDLE                  ImageHandle;
+  EFI_HANDLE                  Handle;
+  EFI_DEVICE_PATH_PROTOCOL    *LoadedImageDevicePath;
+  EFI_BUS_SPECIFIC_DRIVER_OVERRIDE_PROTOCOL  *BusSpecificDriverOverride;
+  UINTN                       DevicePathSize;
+
+  //
+  // Check that ControllerHandle is a valid handle
+  //
+  if (ControllerHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Get the device path of ControllerHandle
+  //
+  Status = gBS->HandleProtocol (
+                  ControllerHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  (VOID **) &ControllerDevicePath
+                  );
+  if (EFI_ERROR (Status) || ControllerDevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Search ControllerDevicePath in MappingDataBase
+  //
+  OverrideItem = NULL;
+  ControllerFound = FALSE;
+  DevicePathSize = GetDevicePathSize (ControllerDevicePath);
+
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+    if (DevicePathSize == GetDevicePathSize (OverrideItem->ControllerDevicePath)) {
+      if (CompareMem (
+            ControllerDevicePath,
+            OverrideItem->ControllerDevicePath,
+            DevicePathSize
+            ) == 0
+          ) {
+        ControllerFound = TRUE;
+        break;
+      }
+    }
+    OverrideItemListIndex = GetNextNode (MappingDataBase, OverrideItemListIndex);
+  }
+
+  if (!ControllerFound) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Passing in a pointer to NULL, will return the first driver device path for ControllerHandle.
+  // Check whether the driverImagePath is not a device path that was returned on a previous call to GetDriverPath().
+  //
+  if (*DriverImageHandle != NULL) {
+    if (*DriverImageHandle != OverrideItem->LastReturnedImageHandle) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+  //
+  // The GetDriverPath() may be called recursively, because it use ConnectDevicePath() internally,
+  //  so should check whether there is a dead loop.
+  //  Here use a controller device path stack to record all processed controller device path during a GetDriverPath() call,
+  //  and check the controller device path whether appear again during the GetDriverPath() call.
+  //
+  if (CheckExistInStack (OverrideItem->ControllerDevicePath)) {
+    //
+    // There is a dependecy dead loop if the ControllerDevicePath appear in stack twice
+    //
+    return EFI_UNSUPPORTED;
+  }
+  PushDevPathStack (OverrideItem->ControllerDevicePath);
+
+  //
+  // Check every override driver, try to load and start them
+  //
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+    if (DriverImageInfo->ImageHandle == NULL) {
+      //
+      // Skip if the image is unloadable or unstartable
+      //
+      if ((!DriverImageInfo->UnLoadable) && ((!DriverImageInfo->UnStartable))) {
+        TempDriverImagePath = DriverImageInfo->DriverImagePath;
+        //
+        // If the image device path contains an FV node, check the FV file device path is valid.
+        // If it is invalid, try to return the valid device path.
+        // FV address maybe changes for memory layout adjust from time to time,
+        // use this funciton could promise the FV file device path is right.
+        //
+        Status = UpdateFvFileDevicePath (&TempDriverImagePath, NULL, CallerImageHandle);
+        if (!EFI_ERROR (Status)) {
+          FreePool (DriverImageInfo->DriverImagePath);
+          DriverImageInfo->DriverImagePath = TempDriverImagePath;
+        }
+        //
+        // Get all Loaded Image protocol to check whether the driver image has been loaded and started
+        //
+        ImageFound = FALSE;
+        ImageHandleCount  = 0;
+        Status = gBS->LocateHandleBuffer (
+                        ByProtocol,
+                        &gEfiLoadedImageProtocolGuid,
+                        NULL,
+                        &ImageHandleCount,
+                        &ImageHandleBuffer
+                        );
+        if (EFI_ERROR (Status) || (ImageHandleCount == 0)) {
+          return EFI_NOT_FOUND;
+        }
+
+        for(Index = 0; Index < ImageHandleCount; Index ++) {
+          //
+          // Get the EFI Loaded Image Device Path Protocol
+          //
+          LoadedImageDevicePath = NULL;
+          Status = gBS->HandleProtocol (
+                          ImageHandleBuffer[Index],
+                          &gEfiLoadedImageDevicePathProtocolGuid,
+                          (VOID **) &LoadedImageDevicePath
+                          );
+          if (EFI_ERROR (Status)) {
+            //
+            // Maybe not all EFI_LOADED_IMAGE_DEVICE_PATH_PROTOCOL existed.
+            //
+            continue;
+          }
+
+          DevicePathSize = GetDevicePathSize (DriverImageInfo->DriverImagePath);
+          if (DevicePathSize == GetDevicePathSize (LoadedImageDevicePath)) {
+            if (CompareMem (
+                  DriverImageInfo->DriverImagePath,
+                  LoadedImageDevicePath,
+                  GetDevicePathSize (LoadedImageDevicePath)
+                  ) == 0
+                ) {
+              ImageFound = TRUE;
+              break;
+            }
+          }
+        }
+
+        if (ImageFound) {
+          //
+          // Find its related driver binding protocol
+          // Driver binding handle may be different with its driver's Image Handle.
+          //
+          DriverBindingHandle = NULL;
+          DriverBinding = GetBindingProtocolFromImageHandle (
+                            ImageHandleBuffer[Index],
+                            &DriverBindingHandle
+                            );
+          ASSERT (DriverBinding != NULL);
+          DriverImageInfo->ImageHandle = ImageHandleBuffer[Index];
+        } else if (GetCurrentTpl() <= TPL_CALLBACK){
+          //
+          // The driver image has not been loaded and started. Try to load and start it now.
+          // Try to connect all device in the driver image path.
+          //
+          // Note: LoadImage() and  StartImage() should be called under CALLBACK TPL in theory, but
+          // since many device need to be connected in  CALLBACK level environment( e.g. Usb devices )
+          // and the Fat and Patition driver can endure executing in CALLBACK level in fact, so here permit
+          // to use LoadImage() and  StartImage() in CALLBACK TPL.
+          //
+          Status = ConnectDevicePath (DriverImageInfo->DriverImagePath);
+          //
+          // check whether it points to a PCI Option Rom image,
+          // and try to use bus override protocol to get its first option rom image driver
+          //
+          TempDriverImagePath = DriverImageInfo->DriverImagePath;
+          gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &TempDriverImagePath, &Handle);
+          //
+          // Get the Bus Specific Driver Override Protocol instance on the Controller Handle
+          //
+          Status = gBS->HandleProtocol(
+                          Handle,
+                          &gEfiBusSpecificDriverOverrideProtocolGuid,
+                          (VOID **) &BusSpecificDriverOverride
+                          );
+          if (!EFI_ERROR (Status) && (BusSpecificDriverOverride != NULL)) {
+            ImageHandle = NULL;
+            Status = BusSpecificDriverOverride->GetDriver (
+                                                  BusSpecificDriverOverride,
+                                                  &ImageHandle
+                                                  );
+            if (!EFI_ERROR (Status)) {
+              //
+              // Find its related driver binding protocol
+              // Driver binding handle may be different with its driver's Image handle
+              //
+              DriverBindingHandle = NULL;
+              DriverBinding = GetBindingProtocolFromImageHandle (
+                                ImageHandle,
+                                &DriverBindingHandle
+                                );
+              ASSERT (DriverBinding != NULL);
+              DriverImageInfo->ImageHandle = ImageHandle;
+            }
+          }
+          //
+          // Skip if any device cannot be connected now, future passes through GetDriver() may be able to load that driver.
+          // Only file path media or FwVol Device Path Node remain if all device is connected
+          //
+          TempDriverImagePath = DriverImageInfo->DriverImagePath;
+          gBS->LocateDevicePath (&gEfiDevicePathProtocolGuid, &TempDriverImagePath, &Handle);
+          if (((DevicePathType (TempDriverImagePath) == MEDIA_DEVICE_PATH) &&
+               (DevicePathSubType (TempDriverImagePath) == MEDIA_FILEPATH_DP)) ||
+              (EfiGetNameGuidFromFwVolDevicePathNode ((MEDIA_FW_VOL_FILEPATH_DEVICE_PATH *) TempDriverImagePath) != NULL)
+             ) {
+            //
+            // Try to load the driver
+            //
+            TempDriverImagePath = DriverImageInfo->DriverImagePath;
+            Status = gBS->LoadImage (
+                            FALSE,
+                            CallerImageHandle,
+                            TempDriverImagePath,
+                            NULL,
+                            0,
+                            &ImageHandle
+                            );
+            if (!EFI_ERROR (Status)) {
+              //
+              // Try to start the driver
+              //
+              Status = gBS->StartImage (ImageHandle, NULL, NULL);
+              if (EFI_ERROR (Status)){
+                DriverImageInfo->UnStartable = TRUE;
+                DriverImageInfo->ImageHandle = NULL;
+              } else {
+                //
+                // Find its related driver binding protocol
+                // Driver binding handle may be different with its driver's Image handle
+                //
+                DriverBindingHandle = NULL;
+                DriverBinding = GetBindingProtocolFromImageHandle (
+                                   ImageHandle,
+                                   &DriverBindingHandle
+                                   );
+                ASSERT (DriverBinding != NULL);
+                DriverImageInfo->ImageHandle = ImageHandle;
+              }
+            } else {
+              DriverImageInfo->UnLoadable = TRUE;
+              DriverImageInfo->ImageHandle = NULL;
+            }
+          }
+        }
+        FreePool (ImageHandleBuffer);
+      }
+    }
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+  }
+  //
+  // Finish try to load and start the override driver of a controller, popup the controller's device path
+  //
+  PopDevPathStack (NULL);
+
+  //
+  // return the DriverImageHandle for ControllerHandle
+  //
+  FoundLastReturned = FALSE;
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+    if (DriverImageInfo->ImageHandle != NULL) {
+      if ((*DriverImageHandle == NULL) || FoundLastReturned) {
+        //
+        // If DriverImageHandle is NULL, then we just need to return the first driver.
+        // If FoundLastReturned, this means we have just encountered the previously returned driver.
+        // For both cases, we just return the image handle of this driver.
+        //
+        OverrideItem->LastReturnedImageHandle = DriverImageInfo->ImageHandle;
+        *DriverImageHandle = DriverImageInfo->ImageHandle;
+        return EFI_SUCCESS;
+      } else if (*DriverImageHandle == DriverImageInfo->ImageHandle){
+        //
+        // We have found the previously returned driver.
+        //
+        FoundLastReturned = TRUE;
+      }
+    }
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Check mapping database whether already has the mapping info which
+  records the input Controller to input DriverImage.
+
+  @param  ControllerDevicePath     The controller device path is to be check.
+  @param  DriverImageDevicePath    The driver image device path is to be check.
+  @param  MappingDataBase          Mapping database list entry pointer
+  @param  DriverInfoNum            the controller's total override driver number
+  @param  DriverImageNO            The driver order number for the input DriverImage.
+                                   If the DriverImageDevicePath is NULL, DriverImageNO is not set.
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath or MappingDataBase is NULL.
+  @retval EFI_NOT_FOUND            ControllerDevicePath is not found in MappingDataBase or
+                                   DriverImageDevicePath is not found in the found DriverImage Info list. 
+  @retval EFI_SUCCESS              The controller's total override driver number and 
+                                   input DriverImage's order number is correctly return.
+**/
+EFI_STATUS
+EFIAPI
+CheckMapping (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath  OPTIONAL,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  OUT    UINT32                                         *DriverInfoNum  OPTIONAL,
+  OUT    UINT32                                         *DriverImageNO  OPTIONAL
+  )
+{
+  LIST_ENTRY                  *OverrideItemListIndex;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  BOOLEAN                     Found;
+  UINT32                      ImageNO;
+  UINTN                       DevicePathSize;
+
+  if (ControllerDevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Search ControllerDevicePath in MappingDataBase
+  //
+  Found = FALSE;
+  OverrideItem = NULL;
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+    DevicePathSize = GetDevicePathSize (ControllerDevicePath);
+    if (DevicePathSize == GetDevicePathSize (OverrideItem->ControllerDevicePath)) {
+      if (CompareMem (
+            ControllerDevicePath,
+            OverrideItem->ControllerDevicePath,
+            DevicePathSize
+            ) == 0
+          ) {
+        Found = TRUE;
+        break;
+      }
+    }
+    OverrideItemListIndex = GetNextNode (MappingDataBase, OverrideItemListIndex);
+  }
+
+  if (!Found) {
+    //
+    // ControllerDevicePath is not in MappingDataBase
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  ASSERT (OverrideItem->DriverInfoNum != 0);
+  if (DriverInfoNum != NULL) {
+    *DriverInfoNum = OverrideItem->DriverInfoNum;
+  }
+
+  //
+  // If DriverImageDevicePath is NULL, skip checking DriverImageDevicePath
+  // in the controller's Driver Image Info List
+  //
+  if (DriverImageDevicePath == NULL) {
+    return EFI_SUCCESS;
+  }
+  //
+  // return the DriverImageHandle for ControllerHandle
+  //
+  ImageNO = 0;
+  Found = FALSE;
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+    ImageNO++;
+    DevicePathSize = GetDevicePathSize (DriverImageDevicePath);
+    if (DevicePathSize == GetDevicePathSize (DriverImageInfo->DriverImagePath)) {
+      if (CompareMem (
+            DriverImageDevicePath,
+            DriverImageInfo->DriverImagePath,
+            GetDevicePathSize (DriverImageInfo->DriverImagePath)
+            ) == 0
+          ) {
+        Found = TRUE;
+        break;
+      }
+    }
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+  }
+
+  if (!Found) {
+    //
+    // DriverImageDevicePath is not found in the controller's Driver Image Info List
+    //
+    return EFI_NOT_FOUND;
+  } else {
+    if (DriverImageNO != NULL) {
+      *DriverImageNO = ImageNO;
+    }
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Insert a driver image as a controller's override driver into the mapping database.
+  The driver image's order number is indicated by DriverImageNO.
+
+  @param  ControllerDevicePath     The controller device path need to add a
+                                   override driver image item
+  @param  DriverImageDevicePath    The driver image device path need to be insert
+  @param  MappingDataBase          Mapping database list entry pointer
+  @param  DriverImageNO            The inserted order number. If this number is taken, 
+                                   the larger available number will be used.
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath is NULL, or DriverImageDevicePath is NULL
+                                   or MappingDataBase is NULL
+  @retval EFI_ALREADY_STARTED      The input Controller to input DriverImage has been 
+                                   recorded into the mapping database.
+  @retval EFI_SUCCESS              The Controller and DriverImage are inserted into 
+                                   the mapping database successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+InsertDriverImage (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath,
+  IN     LIST_ENTRY                                     *MappingDataBase,
+  IN     UINT32                                         DriverImageNO
+  )
+{
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *OverrideItemListIndex;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  BOOLEAN                     Found;
+  UINT32                      ImageNO;
+  UINTN                       DevicePathSize;
+
+  if (ControllerDevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (DriverImageDevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If the driver is already in the controller's Driver Image Info List,
+  // just return EFI_ALREADY_STARTED.
+  //
+  Status = CheckMapping (
+             ControllerDevicePath,
+             DriverImageDevicePath,
+             MappingDataBase,
+             NULL,
+             NULL
+             );
+  if (Status == EFI_SUCCESS) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Search the input ControllerDevicePath in MappingDataBase
+  //
+  Found = FALSE;
+  OverrideItem = NULL;
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+    DevicePathSize = GetDevicePathSize (ControllerDevicePath);
+    if (DevicePathSize == GetDevicePathSize (OverrideItem->ControllerDevicePath)) {
+      if (CompareMem (
+            ControllerDevicePath,
+            OverrideItem->ControllerDevicePath,
+            DevicePathSize
+            ) == 0
+          ) {
+        Found = TRUE;
+        break;
+      }
+    }
+    OverrideItemListIndex = GetNextNode (MappingDataBase, OverrideItemListIndex);
+  }
+  //
+  // If cannot find, this is a new controller item
+  // Add the Controller related PLATFORM_OVERRIDE_ITEM structrue in mapping data base
+  //
+  if (!Found) {
+    OverrideItem = AllocateZeroPool (sizeof (PLATFORM_OVERRIDE_ITEM));
+    ASSERT (OverrideItem != NULL);
+    OverrideItem->Signature = PLATFORM_OVERRIDE_ITEM_SIGNATURE;
+    OverrideItem->ControllerDevicePath = DuplicateDevicePath (ControllerDevicePath);
+    InitializeListHead (&OverrideItem->DriverInfoList);
+    InsertTailList (MappingDataBase, &OverrideItem->Link);
+  }
+
+  //
+  // Prepare the driver image related DRIVER_IMAGE_INFO structure.
+  //
+  DriverImageInfo = AllocateZeroPool (sizeof (DRIVER_IMAGE_INFO));
+  ASSERT (DriverImageInfo != NULL);
+  DriverImageInfo->Signature = DRIVER_IMAGE_INFO_SIGNATURE;
+  DriverImageInfo->DriverImagePath = DuplicateDevicePath (DriverImageDevicePath);
+  //
+  // Find the driver image wanted order location
+  //
+  ImageNO = 0;
+  Found = FALSE;
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    if (ImageNO == (DriverImageNO - 1)) {
+      //
+      // find the wanted order location, insert it
+      //
+      InsertTailList (ImageInfoListIndex, &DriverImageInfo->Link);
+      OverrideItem->DriverInfoNum ++;
+      Found = TRUE;
+      break;
+    }
+    ImageNO++;
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+  }
+
+  if (!Found) {
+    //
+    // if not find the wanted order location, add it as last item of the controller mapping item
+    //
+    InsertTailList (&OverrideItem->DriverInfoList, &DriverImageInfo->Link);
+    OverrideItem->DriverInfoNum ++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Delete a controller's override driver from the mapping database.
+
+  @param  ControllerDevicePath     The controller device path will be deleted 
+                                   when all drivers images on it are removed.
+  @param  DriverImageDevicePath    The driver image device path will be delete.
+                                   If NULL, all driver image will be delete.
+  @param  MappingDataBase          Mapping database list entry pointer
+
+  @retval EFI_INVALID_PARAMETER    ControllerDevicePath is NULL, or MappingDataBase is NULL
+  @retval EFI_NOT_FOUND            ControllerDevicePath is not found in MappingDataBase or
+                                   DriverImageDevicePath is not found in the found DriverImage Info list. 
+  @retval EFI_SUCCESS              Delete the specified driver successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+DeleteDriverImage (
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *ControllerDevicePath,
+  IN     EFI_DEVICE_PATH_PROTOCOL                       *DriverImageDevicePath,
+  IN     LIST_ENTRY                                     *MappingDataBase
+  )
+{
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *OverrideItemListIndex;
+  PLATFORM_OVERRIDE_ITEM      *OverrideItem;
+  LIST_ENTRY                  *ImageInfoListIndex;
+  DRIVER_IMAGE_INFO           *DriverImageInfo;
+  BOOLEAN                     Found;
+  UINTN                       DevicePathSize;
+
+  if (ControllerDevicePath == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (MappingDataBase == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If ControllerDevicePath is not found in mapping database, return EFI_NOT_FOUND.
+  //
+  Status = CheckMapping (
+             ControllerDevicePath,
+             DriverImageDevicePath,
+             MappingDataBase,
+             NULL,
+             NULL
+             );
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Search ControllerDevicePath in MappingDataBase
+  //
+  Found = FALSE;
+  OverrideItem = NULL;
+  OverrideItemListIndex = GetFirstNode (MappingDataBase);
+  while (!IsNull (MappingDataBase, OverrideItemListIndex)) {
+    OverrideItem = CR(OverrideItemListIndex, PLATFORM_OVERRIDE_ITEM, Link, PLATFORM_OVERRIDE_ITEM_SIGNATURE);
+    DevicePathSize = GetDevicePathSize (ControllerDevicePath);
+    if (DevicePathSize == GetDevicePathSize (OverrideItem->ControllerDevicePath)) {
+      if (CompareMem (
+            ControllerDevicePath,
+            OverrideItem->ControllerDevicePath,
+            DevicePathSize
+            ) == 0
+          ) {
+        Found = TRUE;
+        break;
+      }
+    }
+    OverrideItemListIndex = GetNextNode (MappingDataBase, OverrideItemListIndex);
+  }
+
+  ASSERT (Found);
+  ASSERT (OverrideItem->DriverInfoNum != 0);
+
+  Found = FALSE;
+  ImageInfoListIndex = GetFirstNode (&OverrideItem->DriverInfoList);
+  while (!IsNull (&OverrideItem->DriverInfoList, ImageInfoListIndex)) {
+    DriverImageInfo = CR(ImageInfoListIndex, DRIVER_IMAGE_INFO, Link, DRIVER_IMAGE_INFO_SIGNATURE);
+    ImageInfoListIndex = GetNextNode (&OverrideItem->DriverInfoList, ImageInfoListIndex);
+    if (DriverImageDevicePath != NULL) {
+      //
+      // Search for the specified DriverImageDevicePath and remove it, then break.
+      //
+      DevicePathSize = GetDevicePathSize (DriverImageDevicePath);
+      if (DevicePathSize == GetDevicePathSize (DriverImageInfo->DriverImagePath)) {
+        if (CompareMem (
+              DriverImageDevicePath,
+              DriverImageInfo->DriverImagePath,
+              GetDevicePathSize (DriverImageInfo->DriverImagePath)
+              ) == 0
+            ) {
+          Found = TRUE;
+          FreePool(DriverImageInfo->DriverImagePath);
+          RemoveEntryList (&DriverImageInfo->Link);
+          OverrideItem->DriverInfoNum --;
+          break;
+        }
+      }
+    } else {
+      //
+      // Remove all existing driver image info entries, so no break here.
+      //
+      Found = TRUE;
+      FreePool(DriverImageInfo->DriverImagePath);
+      RemoveEntryList (&DriverImageInfo->Link);
+      OverrideItem->DriverInfoNum --;
+    }
+  }
+
+  //
+  // Confirm all driver image info entries have been removed,
+  // if DriverImageDevicePath is NULL.
+  //
+  if (DriverImageDevicePath == NULL) {
+    ASSERT (OverrideItem->DriverInfoNum == 0);
+  }
+  //
+  // If Override Item has no driver image info entry, then delete this item.
+  //
+  if (OverrideItem->DriverInfoNum == 0) {
+    FreePool(OverrideItem->ControllerDevicePath);
+    RemoveEntryList (&OverrideItem->Link);
+    FreePool (OverrideItem);
+  }
+
+  if (!Found) {
+    //
+    // DriverImageDevicePath is not NULL and cannot be found in the controller's
+    // driver image info list.
+    //
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatOverMngr.h b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatOverMngr.h
new file mode 100644
index 0000000000..ca2094551d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatOverMngr.h
@@ -0,0 +1,67 @@
+/** @file
+  
+  The defintions are required both by Source code and Vfr file.
+  The PLAT_OVER_MNGR_DATA structure, form guid and Ifr question ID are defined. 
+
+Copyright (c) 2007 - 2014, 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.
+
+**/
+
+#ifndef _PLAT_OVER_MNGR_H_
+#define _PLAT_OVER_MNGR_H_
+
+#include <Guid/PlatDriOverrideHii.h>
+
+//
+// The max number of the supported driver list.
+//
+#define MAX_CHOICE_NUM    0x00FF
+#define UPDATE_DATA_SIZE  0x1000
+
+#define FORM_ID_DEVICE                 0x1100
+#define FORM_ID_DRIVER                 0x1200
+#define FORM_ID_ORDER                  0x1500
+
+#define KEY_VALUE_DEVICE_OFFSET        0x0100
+#define KEY_VALUE_DRIVER_OFFSET        0x0300
+
+#define KEY_VALUE_DEVICE_REFRESH       0x1234
+#define KEY_VALUE_DEVICE_FILTER        0x1235
+#define KEY_VALUE_DEVICE_CLEAR         0x1236
+
+#define KEY_VALUE_DRIVER_GOTO_PREVIOUS 0x1300
+#define KEY_VALUE_DRIVER_GOTO_ORDER    0x1301
+
+#define KEY_VALUE_ORDER_GOTO_PREVIOUS  0x2000
+#define KEY_VALUE_ORDER_SAVE_AND_EXIT  0x1800
+
+#define VARSTORE_ID_PLAT_OVER_MNGR     0x1000
+
+#define LABEL_END                      0xffff
+
+typedef struct {
+  UINT8   DriOrder[MAX_CHOICE_NUM];
+  UINT8   PciDeviceFilter;
+} PLAT_OVER_MNGR_DATA;
+
+//
+// Field offset of structure PLAT_OVER_MNGR_DATA
+//
+#define VAR_OFFSET(Field)              ((UINTN) &(((PLAT_OVER_MNGR_DATA *) 0)->Field))
+#define DRIVER_ORDER_VAR_OFFSET         (VAR_OFFSET (DriOrder))
+
+//
+// Tool automatic generated Question Id start from 1
+// In order to avoid to conflict them, the Driver Selection and Order QuestionID offset is defined from 0x0500.
+//
+#define QUESTION_ID_OFFSET              0x0500
+#define DRIVER_ORDER_QUESTION_ID        (VAR_OFFSET (DriOrder) + QUESTION_ID_OFFSET)
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatformDriOverrideDxe.inf b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatformDriOverrideDxe.inf
new file mode 100644
index 0000000000..4225fb92c3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/PlatformDriOverrideDxe.inf
@@ -0,0 +1,115 @@
+## @file
+#  This driver produces UEFI PLATFORM_DRIVER_OVERRIDE_PROTOCOL if this protocol doesn't exist.
+#  It doesn't install again if this protocol exists.
+#  It only implements one interface GetDriver of PLATFORM_DRIVER_OVERRIDE_PROTOCOL protocol 
+#  and doesn't support other two interfaces GetDriverPath, DriverLoaded. 
+# 
+#  This driver also offers an UI interface in device manager to let user configure
+#  platform override protocol to override the default algorithm for matching
+#  drivers to controllers.
+#
+#  The main flow:
+#  1. It dynamicly locate all controller device path.
+#  2. It dynamicly locate all drivers which support binding protocol.
+#  3. It export and dynamicly update two menu to let user select the
+#     mapping between drivers to controllers.
+#  4. It save all the mapping info in NV variables for the following boot, 
+#     which will be consumed by GetDriver API of the produced the platform override protocol.
+#
+# Caution: This module is a sample implementation for the test purpose.
+#
+# Copyright (c) 2007 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PlatDriOverrideDxe
+  MODULE_UNI_FILE                = PlatDriOverrideDxe.uni
+  FILE_GUID                      = 35034CE2-A6E5-4fb4-BABE-A0156E9B2549
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PlatDriOverrideDxeInit
+  UNLOAD_IMAGE                   = PlatDriOverrideDxeUnload
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  VfrStrings.uni
+  Vfr.vfr
+  PlatDriOverrideDxe.c
+  PlatOverMngr.h
+  PlatDriOverrideLib.c
+  InternalPlatDriOverrideDxe.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseLib
+  DebugLib
+  UefiLib
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  HiiLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  DevicePathLib
+  DxeServicesTableLib
+  UefiRuntimeServicesTableLib
+  PrintLib 
+   
+[Guids]
+  #
+  # This GUID C Name is not required for build since it is from UefiLib and not directly used by this module source.
+  #  gEfiGlobalVariableGuid                     ## SOMETIMES_CONSUMES   ## Variable:L"PlatformLang" # this variable specifies the platform supported language string (RFC 4646 format)
+  #  gEfiGlobalVariableGuid                     ## SOMETIMES_CONSUMES   ## Variable:L"Lang" # this variable specifies the platform supported language string (ISO 639-2 format)
+  #
+  # There could be more than one variables, from PlatDriOver, PlatDriOver1, PlatDriOver2,...
+  #  gEfiCallerIdGuid                           ## Private  ## Variable:L"PlatDriOver"
+  #
+  gEfiIfrTianoGuid                              ## SOMETIMES_PRODUCES   ## UNDEFINED
+  ## SOMETIMES_CONSUMES ## GUID # HiiIsConfigHdrMatch Data
+  ## SOMETIMES_PRODUCES ## GUID # HiiGetBrowserData Data
+  ## SOMETIMES_CONSUMES ## GUID # HiiSetBrowserData Data
+  ## SOMETIMES_PRODUCES ## GUID # HiiConstructConfigHdr Data
+  ## CONSUMES           ## HII
+  gPlatformOverridesManagerGuid
+
+[Protocols]
+  gEfiComponentName2ProtocolGuid                ## SOMETIMES_CONSUMES # Get Driver Name if ComponentName2Protocol exists
+  gEfiComponentNameProtocolGuid                 ## SOMETIMES_CONSUMES # Get Driver Name if ComponentNameProtocol exists and ComponentName2Protocol doesn't exist
+  gEfiFirmwareVolume2ProtocolGuid               ## SOMETIMES_CONSUMES # Get Driver Name from EFI UI section if ComponentName2Protocol and ComponentNameProtocol don't exist
+  gEfiPciIoProtocolGuid                         ## SOMETIMES_CONSUMES # Find the PCI device if PciIo protocol is installed
+  gEfiBusSpecificDriverOverrideProtocolGuid     ## SOMETIMES_CONSUMES # Check whether the PCI device contains one or more efi drivers in its option rom by this protocol
+
+  gEfiDriverBindingProtocolGuid                 ## SOMETIMES_CONSUMES
+  gEfiLoadedImageProtocolGuid                   ## SOMETIMES_CONSUMES
+  gEfiLoadedImageDevicePathProtocolGuid         ## SOMETIMES_CONSUMES # Show the drivers in the second page that support DriverBindingProtocol, LoadedImageProtocol and LoadedImageDevicePathProtocol
+
+  gEfiFormBrowser2ProtocolGuid                  ## CONSUMES
+  gEfiHiiConfigRoutingProtocolGuid              ## CONSUMES
+  gEfiHiiConfigAccessProtocolGuid               ## PRODUCES
+  gEfiPlatformDriverOverrideProtocolGuid        ## PRODUCES
+  ## PRODUCES
+  ## SOMETIMES_CONSUMES # Show the controller device in the first page that support DevicePathProtocol
+  gEfiDevicePathProtocolGuid
+
+[Depex]
+  gEfiFormBrowser2ProtocolGuid AND gEfiHiiConfigRoutingProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PlatDriOverrideDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/Vfr.vfr b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/Vfr.vfr
new file mode 100644
index 0000000000..52240a1f77
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/Vfr.vfr
@@ -0,0 +1,106 @@
+// *++
+//
+// Copyright (c) 2009, 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.
+//
+// Module Name:
+//
+//   Vfr.vfr
+//
+// Abstract:
+//
+//   Platform driver Override manager formset
+//
+//
+// --*/
+
+#include "PlatOverMngr.h"
+
+#define EFI_DISK_DEVICE_CLASS          0x01
+
+formset
+  guid     = PLAT_OVER_MNGR_GUID,
+  title    = STRING_TOKEN(STR_ENTRY_TITLE),
+  help     = STRING_TOKEN(STR_TITLE_HELP),
+
+  varstore PLAT_OVER_MNGR_DATA,
+    varid = VARSTORE_ID_PLAT_OVER_MNGR,
+    name  = Data,
+    guid  = PLAT_OVER_MNGR_GUID;
+
+  form formid = FORM_ID_DEVICE,
+       title = STRING_TOKEN(STR_TITLE);
+
+    text
+      help   = STRING_TOKEN(STR_FIRST_REFRESH_HELP),
+      text   = STRING_TOKEN(STR_FIRST_REFRESH),
+      flags  = INTERACTIVE,
+      key    = KEY_VALUE_DEVICE_REFRESH;
+
+    checkbox varid = Data.PciDeviceFilter,
+      prompt   = STRING_TOKEN(STR_PCI_DEVICE_FILTER_PROMPT),
+      help     = STRING_TOKEN(STR_PCI_DEVICE_FILTER_HELP),
+      flags    = INTERACTIVE,
+      key      = KEY_VALUE_DEVICE_FILTER,
+    endcheckbox;
+
+    label FORM_ID_DEVICE;
+    label LABEL_END;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    goto FORM_ID_DEVICE,
+      prompt  = STRING_TOKEN(STR_CLEAR_ALL),
+      help    = STRING_TOKEN(STR_CLEAR_ALL_HELP),
+      flags   = INTERACTIVE | RESET_REQUIRED,
+      key     = KEY_VALUE_DEVICE_CLEAR;
+  endform;
+
+  form formid = FORM_ID_DRIVER,
+       title = STRING_TOKEN(STR_TITLE);
+
+    goto FORM_ID_DEVICE,
+      prompt  = STRING_TOKEN(STR_GOTO_PREVIOUS),
+      help    = STRING_TOKEN(STR_NULL_STRING),
+      flags   = INTERACTIVE,
+      key     = KEY_VALUE_DRIVER_GOTO_PREVIOUS;
+
+    goto FORM_ID_ORDER,
+      prompt  = STRING_TOKEN(STR_TITLE_ORDER),
+      help    = STRING_TOKEN(STR_TITLE_ORDER_HELP),
+      flags   = INTERACTIVE,
+      key     = KEY_VALUE_DRIVER_GOTO_ORDER;
+
+    label FORM_ID_DRIVER;
+    label LABEL_END;
+
+  endform;
+
+  form formid = FORM_ID_ORDER,
+       title = STRING_TOKEN(STR_TITLE);
+
+    goto FORM_ID_DRIVER,
+      prompt  = STRING_TOKEN(STR_GOTO_PREVIOUS),
+      help    = STRING_TOKEN(STR_NULL_STRING),
+      flags   = INTERACTIVE,
+      key     = KEY_VALUE_ORDER_GOTO_PREVIOUS;
+
+    label FORM_ID_ORDER;
+    label LABEL_END;
+
+    subtitle text = STRING_TOKEN(STR_NULL_STRING);
+
+    text
+      help   = STRING_TOKEN (STR_NULL_STRING),
+      text   = STRING_TOKEN (STR_SAVE_AND_EXIT),
+      flags  = INTERACTIVE | RESET_REQUIRED,
+      key    = KEY_VALUE_ORDER_SAVE_AND_EXIT;
+  endform;
+
+endformset;
diff --git a/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/VfrStrings.uni b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/VfrStrings.uni
new file mode 100644
index 0000000000..7800cfd3a2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PlatformDriOverrideDxe/VfrStrings.uni
diff --git a/Core/MdeModulePkg/Universal/PrintDxe/Print.c b/Core/MdeModulePkg/Universal/PrintDxe/Print.c
new file mode 100644
index 0000000000..af55acfd2b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PrintDxe/Print.c
@@ -0,0 +1,67 @@
+/** @file
+  This driver produces Print2 protocol layered on top of the PrintLib from the MdePkg.
+
+Copyright (c) 2009, 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 <PiDxe.h>
+
+#include <Protocol/Print2.h>
+#include <Library/PrintLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+
+EFI_HANDLE  mPrintThunkHandle = NULL;
+
+CONST EFI_PRINT2_PROTOCOL mPrint2Protocol = {
+  UnicodeBSPrint,
+  UnicodeSPrint,
+  UnicodeBSPrintAsciiFormat,
+  UnicodeSPrintAsciiFormat,
+  UnicodeValueToString,
+  AsciiBSPrint,
+  AsciiSPrint,
+  AsciiBSPrintUnicodeFormat,
+  AsciiSPrintUnicodeFormat,
+  AsciiValueToString
+};
+
+/**
+  The user Entry Point for Print module.
+
+  This is the entry point for Print DXE Driver. It installs the Print2 Protocol.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval Others            Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+PrintEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mPrintThunkHandle,
+                  &gEfiPrint2ProtocolGuid, &mPrint2Protocol,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.inf b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.inf
new file mode 100644
index 0000000000..9ea06520d1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.inf
@@ -0,0 +1,52 @@
+## @file
+#  Print DXE driver that produces Print2 Protocol.
+#
+#  This driver produces Print2 protocol layered on top of the PrintLib from the MdePkg.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PrintDxe
+  MODULE_UNI_FILE                = PrintDxe.uni
+  FILE_GUID                      = 79E4A61C-ED73-4312-94FE-E3E7563362A9
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PrintEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Print.c
+
+[Packages]
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  PrintLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Protocols]
+  gEfiPrint2ProtocolGuid    ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PrintDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.uni b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.uni
new file mode 100644
index 0000000000..fe3471e577
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxe.uni
diff --git a/Core/MdeModulePkg/Universal/PrintDxe/PrintDxeExtra.uni b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxeExtra.uni
new file mode 100644
index 0000000000..0f1f2bd579
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PrintDxe/PrintDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.c b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.c
new file mode 100644
index 0000000000..718c567346
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.c
@@ -0,0 +1,208 @@
+/** @file
+  This module sets default policy for attributes of EfiACPIMemoryNVS and EfiReservedMemoryType.
+
+  This module sets EFI_MEMORY_XP for attributes of EfiACPIMemoryNVS and EfiReservedMemoryType
+  in UEFI memory map, if and only of PropertiesTable is published and has BIT0 set.
+
+Copyright (c) 2015, 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 <Uefi.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Guid/EventGroup.h>
+#include <Guid/PropertiesTable.h>
+
+/**
+  Converts a number of EFI_PAGEs to a size in bytes.
+
+  NOTE: Do not use EFI_PAGES_TO_SIZE because it handles UINTN only.
+
+  @param  Pages     The number of EFI_PAGES.
+
+  @return  The number of bytes associated with the number of EFI_PAGEs specified
+           by Pages.
+**/
+UINT64
+EfiPagesToSize (
+  IN UINT64 Pages
+  )
+{
+  return LShiftU64 (Pages, EFI_PAGE_SHIFT);
+}
+
+/**
+  Set memory attributes according to default policy.
+
+  @param  MemoryMap        A pointer to the buffer in which firmware places the current memory map.
+  @param  MemoryMapSize    Size, in bytes, of the MemoryMap buffer.
+  @param  DescriptorSize   size, in bytes, of an individual EFI_MEMORY_DESCRIPTOR.
+**/
+VOID
+SetMemorySpaceAttributesDefault (
+  IN EFI_MEMORY_DESCRIPTOR  *MemoryMap,
+  IN UINTN                  MemoryMapSize,
+  IN UINTN                  DescriptorSize
+  )
+{
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEntry;
+  EFI_MEMORY_DESCRIPTOR       *MemoryMapEnd;
+  EFI_STATUS                  Status;
+
+  DEBUG ((EFI_D_INFO, "SetMemorySpaceAttributesDefault\n"));
+
+  MemoryMapEntry = MemoryMap;
+  MemoryMapEnd   = (EFI_MEMORY_DESCRIPTOR *) ((UINT8 *) MemoryMap + MemoryMapSize);
+  while ((UINTN)MemoryMapEntry < (UINTN)MemoryMapEnd) {
+    if (MemoryMapEntry->PhysicalStart < BASE_1MB) {
+      //
+      // Do not touch memory space below 1MB
+      //
+      MemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+      continue;
+    }
+    switch (MemoryMapEntry->Type) {
+    case EfiRuntimeServicesCode:
+    case EfiRuntimeServicesData:
+      //
+      // should be handled later;
+      //
+      break;
+    case EfiReservedMemoryType:
+    case EfiACPIMemoryNVS:
+      //
+      // Handle EfiReservedMemoryType and EfiACPIMemoryNVS, because there might be firmware executable there.
+      //
+      DEBUG ((EFI_D_INFO, "SetMemorySpaceAttributes - %016lx - %016lx (%016lx) ...\n",
+        MemoryMapEntry->PhysicalStart,
+        MemoryMapEntry->PhysicalStart + EfiPagesToSize (MemoryMapEntry->NumberOfPages),
+        MemoryMapEntry->Attribute
+        ));
+      Status = gDS->SetMemorySpaceCapabilities (
+                      MemoryMapEntry->PhysicalStart,
+                      EfiPagesToSize (MemoryMapEntry->NumberOfPages),
+                      MemoryMapEntry->Attribute | EFI_MEMORY_XP
+                      );
+      DEBUG ((EFI_D_INFO, "SetMemorySpaceCapabilities - %r\n", Status));
+      break;
+    }
+
+    MemoryMapEntry = NEXT_MEMORY_DESCRIPTOR (MemoryMapEntry, DescriptorSize);
+  }
+
+  return ;
+}
+
+/**
+  Update memory attributes according to default policy.
+
+  @param[in]  Event     The Event this notify function registered to.
+  @param[in]  Context   Pointer to the context data registered to the Event.
+**/
+VOID
+EFIAPI
+UpdateMemoryAttributesDefault (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_MEMORY_DESCRIPTOR       *MemoryMap;
+  UINTN                       MemoryMapSize;
+  UINTN                       MapKey;
+  UINTN                       DescriptorSize;
+  UINT32                      DescriptorVersion;
+  EFI_PROPERTIES_TABLE        *PropertiesTable;
+
+  DEBUG ((EFI_D_INFO, "UpdateMemoryAttributesDefault\n"));
+  Status = EfiGetSystemConfigurationTable (&gEfiPropertiesTableGuid, (VOID **) &PropertiesTable);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  
+  ASSERT (PropertiesTable != NULL);
+
+  DEBUG ((EFI_D_INFO, "MemoryProtectionAttribute - 0x%016lx\n", PropertiesTable->MemoryProtectionAttribute));
+  if ((PropertiesTable->MemoryProtectionAttribute & EFI_PROPERTIES_RUNTIME_MEMORY_PROTECTION_NON_EXECUTABLE_PE_DATA) == 0) {
+    goto Done;
+  }
+
+  //
+  // Get the EFI memory map.
+  //
+  MemoryMapSize  = 0;
+  MemoryMap      = NULL;
+  Status = gBS->GetMemoryMap (
+                  &MemoryMapSize,
+                  MemoryMap,
+                  &MapKey,
+                  &DescriptorSize,
+                  &DescriptorVersion
+                  );
+  ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+  do {
+    MemoryMap = (EFI_MEMORY_DESCRIPTOR *) AllocatePool (MemoryMapSize);
+    ASSERT (MemoryMap != NULL);
+    Status = gBS->GetMemoryMap (
+                    &MemoryMapSize,
+                    MemoryMap,
+                    &MapKey,
+                    &DescriptorSize,
+                    &DescriptorVersion
+                    );
+    if (EFI_ERROR (Status)) {
+      FreePool (MemoryMap);
+    }
+  } while (Status == EFI_BUFFER_TOO_SMALL);
+  ASSERT_EFI_ERROR (Status);
+
+  SetMemorySpaceAttributesDefault (MemoryMap, MemoryMapSize, DescriptorSize);
+
+Done:
+  gBS->CloseEvent (Event);
+
+  return ;
+}
+
+/**
+  The entrypoint of properties table attribute driver.
+
+  @param  ImageHandle    The firmware allocated handle for the EFI image.
+  @param  SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS    It always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializePropertiesTableAttributesDxe (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_EVENT   ReadyToBootEvent;
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_CALLBACK,
+                  UpdateMemoryAttributesDefault,
+                  NULL,
+                  &gEfiEventReadyToBootGuid,
+                  &ReadyToBootEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.inf b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.inf
new file mode 100644
index 0000000000..3f699f6b5a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.inf
@@ -0,0 +1,56 @@
+## @file
+# This module sets default policy for attributes of EfiACPIMemoryNVS and EfiReservedMemoryType.
+#
+# This module sets EFI_MEMORY_XP for attributes of EfiACPIMemoryNVS and EfiReservedMemoryType
+# in UEFI memory map, if and only of PropertiesTable is published and has BIT0 set.
+#
+# Copyright (c) 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PropertiesTableAttributesDxe
+  MODULE_UNI_FILE                = PropertiesTableAttributesDxe.uni
+  FILE_GUID                      = AA48FBB2-9F87-4DFD-B416-575938F0C8F4
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = InitializePropertiesTableAttributesDxe
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  PropertiesTableAttributesDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DxeServicesTableLib
+  DebugLib
+  UefiLib
+  MemoryAllocationLib
+
+[Guids]
+  gEfiEventReadyToBootGuid                      ## CONSUMES ## Event
+  gEfiPropertiesTableGuid                       ## CONSUMES ## SystemTable
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PropertiesTableAttributesDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.uni b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.uni
new file mode 100644
index 0000000000..e413fe6a3c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxe.uni
diff --git a/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxeExtra.uni b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxeExtra.uni
new file mode 100644
index 0000000000..24841aaa70
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/PropertiesTableAttributesDxe/PropertiesTableAttributesDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.c b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.c
new file mode 100644
index 0000000000..8a2dc8a9bd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.c
@@ -0,0 +1,321 @@
+/** @file
+  Report Status Code Router PEIM which produces Report Stataus Code Handler PPI and Status Code PPI.
+
+  Copyright (c) 2009 - 2011, 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 "ReportStatusCodeRouterPei.h"
+
+EFI_PEI_RSC_HANDLER_PPI     mRscHandlerPpi = {
+  Register,
+  Unregister
+  };
+
+EFI_PEI_PROGRESS_CODE_PPI     mStatusCodePpi = {
+  ReportDispatcher
+  };
+
+EFI_PEI_PPI_DESCRIPTOR   mRscHandlerPpiList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiRscHandlerPpiGuid,
+    &mRscHandlerPpi
+  }
+};
+
+EFI_PEI_PPI_DESCRIPTOR   mStatusCodePpiList[] = {
+  {
+    EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
+    &gEfiPeiStatusCodePpiGuid,
+    &mStatusCodePpi
+  }
+};
+
+/**
+  Worker function to create one memory status code GUID'ed HOB,
+  using PacketIndex to identify the packet.
+
+  @param   PacketIndex    Index of records packet.
+
+  @return  Pointer to the memory status code packet.
+
+**/
+UINTN *
+CreateRscHandlerCallbackPacket (
+  VOID
+  )
+{
+  UINTN  *NumberOfEntries;
+
+  //
+  // Build GUID'ed HOB with PCD defined size.
+  //
+  NumberOfEntries = BuildGuidHob (
+                      &gStatusCodeCallbackGuid,
+                      sizeof (EFI_PEI_RSC_HANDLER_CALLBACK) * 64 + sizeof (UINTN)
+                      );
+  ASSERT (NumberOfEntries != NULL);
+
+  *NumberOfEntries = 0;
+
+  return NumberOfEntries;
+}
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function.
+
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is called
+                                when a call to ReportStatusCode() occurs.
+                        
+  @retval EFI_SUCCESS           Function was successfully registered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_OUT_OF_RESOURCES  The internal buffer ran out of space. No more functions can be
+                                registered.
+  @retval EFI_ALREADY_STARTED   The function was already registered. It can't be registered again.
+  
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_PEI_RSC_HANDLER_CALLBACK Callback
+  )
+{
+  EFI_PEI_HOB_POINTERS          Hob;
+  EFI_PEI_RSC_HANDLER_CALLBACK  *CallbackEntry;
+  UINTN                         *NumberOfEntries;
+  UINTN                         Index;
+  UINTN                         FreeEntryIndex;
+  UINTN                         *FreePacket;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Hob.Raw        = GetFirstGuidHob (&gStatusCodeCallbackGuid);
+  FreePacket     = NULL;
+  FreeEntryIndex = 0;
+  while (Hob.Raw != NULL) {
+    NumberOfEntries = GET_GUID_HOB_DATA (Hob);
+    CallbackEntry   = (EFI_PEI_RSC_HANDLER_CALLBACK *) (NumberOfEntries + 1);
+    if (FreePacket == NULL && *NumberOfEntries < 64) {
+      //
+      // If current total number of handlers does not exceed 64, put new handler
+      // at the last of packet
+      //
+      FreePacket = NumberOfEntries;
+      FreeEntryIndex = *NumberOfEntries;
+    }
+    for (Index = 0; Index < *NumberOfEntries; Index++) {
+      if (CallbackEntry[Index] == Callback) {
+        //
+        // If the function was already registered. It can't be registered again.
+        //
+        return EFI_ALREADY_STARTED;
+      }
+      if (FreePacket == NULL && CallbackEntry[Index] == NULL) {
+        //
+        // If the total number of handlers in current packet is max value 64,
+        // search an entry with NULL pointer and fill new handler into this entry.
+        //  
+        FreePacket = NumberOfEntries;
+        FreeEntryIndex = Index;
+      }
+    }
+    Hob.Raw = GET_NEXT_HOB (Hob);
+    Hob.Raw = GetNextGuidHob (&gStatusCodeCallbackGuid, Hob.Raw);
+  }
+
+  if (FreePacket == NULL) {
+    FreePacket = CreateRscHandlerCallbackPacket();
+  }
+
+  CallbackEntry = (EFI_PEI_RSC_HANDLER_CALLBACK *) (FreePacket + 1);
+  CallbackEntry[FreeEntryIndex] = Callback;
+  
+  if (*FreePacket == FreeEntryIndex) {
+    //
+    // If new registered callback is added as a new entry in the packet,
+    // increase the total number of handlers in the packet.
+    //
+    *FreePacket += 1;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  ReportStatusCode() messages will no longer be forwarded to the Callback function.
+  
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is to be
+                                unregistered.
+
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+                        
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_PEI_RSC_HANDLER_CALLBACK Callback
+  )
+{
+  EFI_PEI_HOB_POINTERS            Hob;
+  EFI_PEI_RSC_HANDLER_CALLBACK    *CallbackEntry;
+  UINTN                           *NumberOfEntries;
+  UINTN                           Index;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Hob.Raw  = GetFirstGuidHob (&gStatusCodeCallbackGuid);
+  while (Hob.Raw != NULL) {
+    NumberOfEntries = GET_GUID_HOB_DATA (Hob);
+    CallbackEntry   = (EFI_PEI_RSC_HANDLER_CALLBACK *) (NumberOfEntries + 1);
+    for (Index = 0; Index < *NumberOfEntries; Index++) {
+      if (CallbackEntry[Index] == Callback) {
+        //
+        // Set removed entry as NULL.
+        //
+        CallbackEntry[Index] = NULL;
+        return EFI_SUCCESS;
+      }
+    }
+    Hob.Raw = GET_NEXT_HOB (Hob);
+    Hob.Raw = GetNextGuidHob (&gStatusCodeCallbackGuid, Hob.Raw);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Publishes an interface that allows PEIMs to report status codes.
+
+  This function implements EFI_PEI_PROGRESS_CODE_PPI.ReportStatusCode().
+  It publishes an interface that allows PEIMs to report status codes.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN CONST EFI_PEI_SERVICES         **PeiServices,
+  IN EFI_STATUS_CODE_TYPE           CodeType,
+  IN EFI_STATUS_CODE_VALUE          Value,
+  IN UINT32                         Instance,
+  IN CONST EFI_GUID                 *CallerId OPTIONAL,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  )
+{
+  EFI_PEI_HOB_POINTERS            Hob;
+  EFI_PEI_RSC_HANDLER_CALLBACK    *CallbackEntry;
+  UINTN                           *NumberOfEntries;
+  UINTN                           Index;
+
+  Hob.Raw  = GetFirstGuidHob (&gStatusCodeCallbackGuid);
+  while (Hob.Raw != NULL) {
+    NumberOfEntries = GET_GUID_HOB_DATA (Hob);
+    CallbackEntry   = (EFI_PEI_RSC_HANDLER_CALLBACK *) (NumberOfEntries + 1);
+    for (Index = 0; Index < *NumberOfEntries; Index++) {
+      if (CallbackEntry[Index] != NULL) {
+      CallbackEntry[Index](
+        PeiServices,
+        CodeType,
+        Value,
+        Instance,
+        CallerId,
+        Data
+        );
+      }
+    }
+    Hob.Raw = GET_NEXT_HOB (Hob);
+    Hob.Raw = GetNextGuidHob (&gStatusCodeCallbackGuid, Hob.Raw);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Entry point of Status Code PEIM.
+  
+  This function is the entry point of this Status Code Router PEIM.
+  It produces Report Stataus Code Handler PPI and Status Code PPI.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCESS  The entry point of DXE IPL PEIM executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+GenericStatusCodePeiEntry (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                 Status;
+  EFI_PEI_PPI_DESCRIPTOR     *OldDescriptor;
+  EFI_PEI_PROGRESS_CODE_PPI  *OldStatusCodePpi;
+
+  CreateRscHandlerCallbackPacket ();
+
+  //
+  // Install Report Status Code Handler PPI
+  //
+  Status = PeiServicesInstallPpi (mRscHandlerPpiList);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install Status Code PPI. PI spec specifies that there can be only one instance
+  // of this PPI in system. So first check if other instance already exists.
+  // If no other instance exists, then just install the PPI.
+  // If other instance already exists, then reinstall it.
+  //
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiStatusCodePpiGuid,
+             0,
+             &OldDescriptor,
+             (VOID **) &OldStatusCodePpi
+             );
+  if (!EFI_ERROR (Status)) {
+    Status = PeiServicesReInstallPpi (OldDescriptor, mStatusCodePpiList);
+  } else {
+    Status = PeiServicesInstallPpi (mStatusCodePpiList);
+  }
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.h b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.h
new file mode 100644
index 0000000000..318a3f5af1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.h
@@ -0,0 +1,109 @@
+/** @file
+  Internal include file for Report Status Code Router PEIM.
+
+  Copyright (c) 2009, 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.
+
+**/
+
+#ifndef __PEI_REPORT_STATUS_CODE_ROUTER_H__
+#define __PEI_REPORT_STATUS_CODE_ROUTER_H__
+
+
+#include <Ppi/ReportStatusCodeHandler.h>
+#include <Ppi/StatusCode.h>
+
+#include <Guid/StatusCodeCallbackGuid.h>
+
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/PeimEntryPoint.h>
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function.
+
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is called
+                                when a call to ReportStatusCode() occurs.
+                        
+  @retval EFI_SUCCESS           Function was successfully registered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_OUT_OF_RESOURCES  The internal buffer ran out of space. No more functions can be
+                                registered.
+  @retval EFI_ALREADY_STARTED   The function was already registered. It can't be registered again.
+
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_PEI_RSC_HANDLER_CALLBACK Callback
+  );
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  ReportStatusCode() messages will no longer be forwarded to the Callback function.
+  
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is to be
+                                unregistered.
+
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+                        
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_PEI_RSC_HANDLER_CALLBACK Callback
+  );
+
+/**
+  Publishes an interface that allows PEIMs to report status codes.
+
+  This function implements EFI_PEI_PROGRESS_CODE_PPI.ReportStatusCode().
+  It publishes an interface that allows PEIMs to report status codes.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      The function completed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN CONST EFI_PEI_SERVICES         **PeiServices,
+  IN EFI_STATUS_CODE_TYPE           CodeType,
+  IN EFI_STATUS_CODE_VALUE          Value,
+  IN UINT32                         Instance,
+  IN CONST EFI_GUID                 *CallerId OPTIONAL,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.inf b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.inf
new file mode 100644
index 0000000000..a3de382d00
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.inf
@@ -0,0 +1,60 @@
+## @file
+#  Report Status Code Router PEIM which produces Report Stataus Code Handler PPI and Status Code PPI.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ReportStatusCodeRouterPei
+  MODULE_UNI_FILE                = ReportStatusCodeRouterPei.uni
+  FILE_GUID                      = A3610442-E69F-4DF3-82CA-2360C4031A23
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = GenericStatusCodePeiEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is only for build)
+#
+
+[Sources]
+  ReportStatusCodeRouterPei.c
+  ReportStatusCodeRouterPei.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeimEntryPoint
+  PeiServicesLib
+  DebugLib
+  HobLib
+
+[Guids]
+  ## PRODUCES ## HOB
+  ## CONSUMES ## HOB
+  gStatusCodeCallbackGuid
+
+[Ppis]
+  gEfiPeiRscHandlerPpiGuid                      ## PRODUCES
+  gEfiPeiStatusCodePpiGuid                      ## PRODUCES
+
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ReportStatusCodeRouterPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.uni
new file mode 100644
index 0000000000..3ab998028e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPeiExtra.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPeiExtra.uni
new file mode 100644
index 0000000000..965c094b62
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.c b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.c
new file mode 100644
index 0000000000..f859075da6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.c
@@ -0,0 +1,406 @@
+/** @file
+  Report Status Code Router Driver which produces Report Stataus Code Handler Protocol
+  and Status Code Runtime Protocol.
+
+  Copyright (c) 2009 - 2011, 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 "ReportStatusCodeRouterRuntimeDxe.h"
+
+EFI_HANDLE   mHandle                    = NULL;
+LIST_ENTRY   mCallbackListHead          = INITIALIZE_LIST_HEAD_VARIABLE (mCallbackListHead);
+EFI_EVENT    mVirtualAddressChangeEvent = NULL;
+
+//
+// Report operation nest status.
+// If it is set, then the report operation has nested.
+//
+UINT32       mStatusCodeNestStatus = 0;
+
+EFI_STATUS_CODE_PROTOCOL  mStatusCodeProtocol  = {
+  ReportDispatcher
+};
+
+EFI_RSC_HANDLER_PROTOCOL  mRscHandlerProtocol = {
+  Register,
+  Unregister
+  };
+
+/**
+  Event callback function to invoke status code handler in list.
+
+  @param  Event         Event whose notification function is being invoked.
+  @param  Context       Pointer to the notification function's context, which is
+                        always zero in current implementation.
+
+**/
+VOID
+EFIAPI
+RscHandlerNotification (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  RSC_HANDLER_CALLBACK_ENTRY  *CallbackEntry;
+  EFI_PHYSICAL_ADDRESS        Address;
+  RSC_DATA_ENTRY              *RscData;
+
+  CallbackEntry = (RSC_HANDLER_CALLBACK_ENTRY *) Context;
+
+  //
+  // Traverse the status code data buffer to parse all
+  // data to report.
+  //
+  Address = CallbackEntry->StatusCodeDataBuffer;
+  while (Address < CallbackEntry->EndPointer) {
+    RscData = (RSC_DATA_ENTRY *) (UINTN) Address;
+    CallbackEntry->RscHandlerCallback (
+                     RscData->Type,
+                     RscData->Value,
+                     RscData->Instance,
+                     &RscData->CallerId,
+                     &RscData->Data
+                     );
+
+    Address += (sizeof (RSC_DATA_ENTRY) + RscData->Data.Size);
+    Address  = ALIGN_VARIABLE (Address);
+  }
+
+  CallbackEntry->EndPointer = CallbackEntry->StatusCodeDataBuffer;
+}
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function. During the bootservices,
+  this is the callback for which this service can be invoked. The report status code router
+  will create an event such that the callback function is only invoked at the TPL for which it was
+  registered. The entity that registers for the callback should also register for an event upon
+  generation of exit boot services and invoke the unregister service.
+  If the handler does not have a TPL dependency, it should register for a callback at TPL high. The
+  router infrastructure will support making callbacks at runtime, but the caller for runtime invocation
+  must meet the following criteria:
+  1. must be a runtime driver type so that its memory is not reclaimed
+  2. not unregister at exit boot services so that the router will still have its callback address
+  3. the caller must be self-contained (eg. Not call out into any boot-service interfaces) and be
+  runtime safe, in general.
+  
+  @param[in] Callback   A pointer to a function of type EFI_RSC_HANDLER_CALLBACK that is called when
+                        a call to ReportStatusCode() occurs.
+  @param[in] Tpl        TPL at which callback can be safely invoked.   
+  
+  @retval  EFI_SUCCESS              Function was successfully registered.
+  @retval  EFI_INVALID_PARAMETER    The callback function was NULL.
+  @retval  EFI_OUT_OF_RESOURCES     The internal buffer ran out of space. No more functions can be
+                                    registered.
+  @retval  EFI_ALREADY_STARTED      The function was already registered. It can't be registered again.
+
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_RSC_HANDLER_CALLBACK   Callback,
+  IN EFI_TPL                    Tpl
+  )
+{
+  EFI_STATUS                    Status;
+  LIST_ENTRY                    *Link;
+  RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+    CallbackEntry = CR (Link, RSC_HANDLER_CALLBACK_ENTRY, Node, RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    if (CallbackEntry->RscHandlerCallback == Callback) {
+      //
+      // If the function was already registered. It can't be registered again.
+      //
+      return EFI_ALREADY_STARTED;
+    }
+  }
+
+  CallbackEntry = AllocateRuntimeZeroPool (sizeof (RSC_HANDLER_CALLBACK_ENTRY));
+  ASSERT (CallbackEntry != NULL);
+
+  CallbackEntry->Signature          = RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE;
+  CallbackEntry->RscHandlerCallback = Callback;
+  CallbackEntry->Tpl                = Tpl;
+
+  //
+  // If TPL of registered callback funtion is not TPL_HIGH_LEVEL, then event should be created
+  // for it, and related buffer for status code data should be prepared.
+  // Here the data buffer must be prepared in advance, because Report Status Code Protocol might
+  // be invoked under TPL_HIGH_LEVEL and no memory allocation is allowed then.
+  // If TPL is TPL_HIGH_LEVEL, then all status code will be reported immediately, without data
+  // buffer and event trigger.
+  //
+  if (Tpl != TPL_HIGH_LEVEL) {
+    CallbackEntry->StatusCodeDataBuffer = (EFI_PHYSICAL_ADDRESS) (UINTN) AllocatePool (EFI_PAGE_SIZE);
+    CallbackEntry->BufferSize           = EFI_PAGE_SIZE;
+    CallbackEntry->EndPointer           = CallbackEntry->StatusCodeDataBuffer;
+    Status = gBS->CreateEvent (
+                    EVT_NOTIFY_SIGNAL,
+                    Tpl,
+                    RscHandlerNotification,
+                    CallbackEntry,
+                    &CallbackEntry->Event
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  InsertTailList (&mCallbackListHead, &CallbackEntry->Node);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  A callback function must be unregistered before it is deallocated. It is important that any registered
+  callbacks that are not runtime complaint be unregistered when ExitBootServices() is called.
+  
+  @param[in]  Callback  A pointer to a function of type EFI_RSC_HANDLER_CALLBACK that is to be
+                        unregistered.
+                        
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_RSC_HANDLER_CALLBACK Callback
+  )
+{
+  LIST_ENTRY                    *Link;
+  RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+    CallbackEntry = CR (Link, RSC_HANDLER_CALLBACK_ENTRY, Node, RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    if (CallbackEntry->RscHandlerCallback == Callback) {
+      //
+      // If the function is found in list, delete it and return.
+      //
+      if (CallbackEntry->Tpl != TPL_HIGH_LEVEL) {
+        FreePool ((VOID *) (UINTN) CallbackEntry->StatusCodeDataBuffer);
+        gBS->CloseEvent (CallbackEntry->Event);
+      }
+      RemoveEntryList (&CallbackEntry->Node);
+      FreePool (CallbackEntry);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Provides an interface that a software module can call to report a status code.
+
+  @param  Type             Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_DEVICE_ERROR      The function should not be completed due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId  OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data      OPTIONAL
+  )
+{
+  LIST_ENTRY                    *Link;
+  RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+  RSC_DATA_ENTRY                *RscData;
+  EFI_STATUS                    Status;
+  VOID                          *NewBuffer;
+
+  //
+  // Use atom operation to avoid the reentant of report.
+  // If current status is not zero, then the function is reentrancy.
+  //
+  if (InterlockedCompareExchange32 (&mStatusCodeNestStatus, 0, 1) == 1) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link);) {
+    CallbackEntry = CR (Link, RSC_HANDLER_CALLBACK_ENTRY, Node, RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    //
+    // The handler may remove itself, so get the next handler in advance.
+    //
+    Link = GetNextNode (&mCallbackListHead, Link);
+    if ((CallbackEntry->Tpl == TPL_HIGH_LEVEL) || EfiAtRuntime ()) {
+      CallbackEntry->RscHandlerCallback (
+                       Type,
+                       Value,
+                       Instance,
+                       CallerId,
+                       Data
+                       );
+      continue;
+    }
+
+    //
+    // If callback is registered with TPL lower than TPL_HIGH_LEVEL, event must be signaled at boot time to possibly wait for
+    // allowed TPL to report status code. Related data should also be stored in data buffer.
+    //
+    CallbackEntry->EndPointer  = ALIGN_VARIABLE (CallbackEntry->EndPointer);
+    RscData = (RSC_DATA_ENTRY *) (UINTN) CallbackEntry->EndPointer;
+    CallbackEntry->EndPointer += sizeof (RSC_DATA_ENTRY);
+    if (Data != NULL) {
+      CallbackEntry->EndPointer += Data->Size;
+    }
+
+    //
+    // If data buffer is about to be used up (7/8 here), try to reallocate a buffer with double size, if not at TPL_HIGH_LEVEL.
+    //
+    if (CallbackEntry->EndPointer > (CallbackEntry->StatusCodeDataBuffer + (CallbackEntry->BufferSize / 8) * 7)) {
+      if (EfiGetCurrentTpl () < TPL_HIGH_LEVEL) {
+        NewBuffer = ReallocatePool (
+                      CallbackEntry->BufferSize,
+                      CallbackEntry->BufferSize * 2,
+                      (VOID *) (UINTN) CallbackEntry->StatusCodeDataBuffer
+                      );
+        if (NewBuffer != NULL) {
+          CallbackEntry->EndPointer = (EFI_PHYSICAL_ADDRESS) (UINTN) NewBuffer + (CallbackEntry->EndPointer - CallbackEntry->StatusCodeDataBuffer);
+          CallbackEntry->StatusCodeDataBuffer = (EFI_PHYSICAL_ADDRESS) (UINTN) NewBuffer;
+          CallbackEntry->BufferSize *= 2;
+        }
+      }
+    }
+
+    //
+    // If data buffer is used up, do not report for this time.
+    //
+    if (CallbackEntry->EndPointer > (CallbackEntry->StatusCodeDataBuffer + CallbackEntry->BufferSize)) {
+      continue;
+    }
+
+    RscData->Type      = Type;
+    RscData->Value     = Value;
+    RscData->Instance  = Instance;
+    if (CallerId != NULL) {
+      CopyGuid (&RscData->CallerId, CallerId);
+    }
+    if (Data != NULL) {
+      CopyMem (&RscData->Data, Data, Data->HeaderSize + Data->Size);
+    }
+
+    Status = gBS->SignalEvent (CallbackEntry->Event);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Restore the nest status of report
+  //
+  InterlockedCompareExchange32 (&mStatusCodeNestStatus, 1, 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Virtual address change notification call back. It converts global pointer
+  to virtual address.
+
+  @param  Event         Event whose notification function is being invoked.
+  @param  Context       Pointer to the notification function's context, which is
+                        always zero in current implementation.
+
+**/
+VOID
+EFIAPI
+VirtualAddressChangeCallBack (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  EFI_STATUS					Status;
+  LIST_ENTRY                    *Link;
+  RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+    CallbackEntry = CR (Link, RSC_HANDLER_CALLBACK_ENTRY, Node, RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    Status = EfiConvertFunctionPointer (0, (VOID **) &CallbackEntry->RscHandlerCallback);
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  Status = EfiConvertList (
+             0,
+             &mCallbackListHead
+             );
+  ASSERT_EFI_ERROR (Status);
+}
+
+/**
+  Entry point of Generic Status Code Driver.
+
+  This function is the entry point of this Generic Status Code Driver.
+  It installs eport Stataus Code Handler Protocol  and Status Code Runtime Protocol,
+  and registers event for EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+GenericStatusCodeRuntimeDxeEntry (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS     Status;
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEfiRscHandlerProtocolGuid,
+                  &mRscHandlerProtocol,
+                  &gEfiStatusCodeRuntimeProtocolGuid,
+                  &mStatusCodeProtocol,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  VirtualAddressChangeCallBack,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mVirtualAddressChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.h b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.h
new file mode 100644
index 0000000000..c82f296f8d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.h
@@ -0,0 +1,142 @@
+/** @file
+  Internal include file for Report Status Code Router Driver.
+
+  Copyright (c) 2009, 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.
+
+**/
+
+#ifndef __REPORT_STATUS_CODE_ROUTER_RUNTIME_DXE_H__
+#define __REPORT_STATUS_CODE_ROUTER_RUNTIME_DXE_H__
+
+
+#include <Protocol/ReportStatusCodeHandler.h>
+#include <Protocol/StatusCode.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Library/BaseLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include "Library/UefiLib.h"
+
+#define RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE  SIGNATURE_32 ('r', 'h', 'c', 'e')
+
+typedef struct {
+  UINTN                     Signature;
+  EFI_RSC_HANDLER_CALLBACK  RscHandlerCallback;
+  EFI_TPL                   Tpl;
+  EFI_EVENT                 Event;
+  EFI_PHYSICAL_ADDRESS      StatusCodeDataBuffer;
+  UINTN                     BufferSize;
+  EFI_PHYSICAL_ADDRESS      EndPointer;
+  LIST_ENTRY                Node;
+} RSC_HANDLER_CALLBACK_ENTRY;
+
+typedef struct {
+  EFI_STATUS_CODE_TYPE      Type;
+  EFI_STATUS_CODE_VALUE     Value;
+  UINT32                    Instance;
+  UINT32                    Reserved;
+  EFI_GUID                  CallerId;
+  EFI_STATUS_CODE_DATA      Data;
+} RSC_DATA_ENTRY;
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function. During the bootservices,
+  this is the callback for which this service can be invoked. The report status code router
+  will create an event such that the callback function is only invoked at the TPL for which it was
+  registered. The entity that registers for the callback should also register for an event upon
+  generation of exit boot services and invoke the unregister service.
+  If the handler does not have a TPL dependency, it should register for a callback at TPL high. The
+  router infrastructure will support making callbacks at runtime, but the caller for runtime invocation
+  must meet the following criteria:
+  1. must be a runtime driver type so that its memory is not reclaimed
+  2. not unregister at exit boot services so that the router will still have its callback address
+  3. the caller must be self-contained (eg. Not call out into any boot-service interfaces) and be
+  runtime safe, in general.
+  
+  @param[in] Callback   A pointer to a function of type EFI_RSC_HANDLER_CALLBACK that is called when
+                        a call to ReportStatusCode() occurs.
+  @param[in] Tpl        TPL at which callback can be safely invoked.   
+  
+  @retval  EFI_SUCCESS              Function was successfully registered.
+  @retval  EFI_INVALID_PARAMETER    The callback function was NULL.
+  @retval  EFI_OUT_OF_RESOURCES     The internal buffer ran out of space. No more functions can be
+                                    registered.
+  @retval  EFI_ALREADY_STARTED      The function was already registered. It can't be registered again.
+
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_RSC_HANDLER_CALLBACK   Callback,
+  IN EFI_TPL                    Tpl
+  );
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  A callback function must be unregistered before it is deallocated. It is important that any registered
+  callbacks that are not runtime complaint be unregistered when ExitBootServices() is called.
+  
+  @param[in]  Callback  A pointer to a function of type EFI_RSC_HANDLER_CALLBACK that is to be
+                        unregistered.
+                        
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_RSC_HANDLER_CALLBACK Callback
+  );
+
+/**
+  Provides an interface that a software module can call to report a status code.
+
+  @param  Type             Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_DEVICE_ERROR      The function should not be completed due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN EFI_STATUS_CODE_TYPE     Type,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId  OPTIONAL,
+  IN EFI_STATUS_CODE_DATA     *Data      OPTIONAL
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.inf b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.inf
new file mode 100644
index 0000000000..0445d94340
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.inf
@@ -0,0 +1,63 @@
+## @file
+#  Report Status Code Router Driver which produces Report Stataus Code Handler Protocol and Status Code Runtime Protocol.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ReportStatusCodeRouterRuntimeDxe
+  MODULE_UNI_FILE                = ReportStatusCodeRouterRuntimeDxe.uni
+  FILE_GUID                      = D93CE3D8-A7EB-4730-8C8E-CC466A9ECC3C
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0  
+  ENTRY_POINT                    = GenericStatusCodeRuntimeDxeEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  ReportStatusCodeRouterRuntimeDxe.c
+  ReportStatusCodeRouterRuntimeDxe.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiRuntimeLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  HobLib
+  DebugLib
+  BaseLib
+  SynchronizationLib
+  UefiLib
+
+[Guids]
+  gEfiEventVirtualAddressChangeGuid               ## CONSUMES ## Event
+
+[Protocols]
+  gEfiRscHandlerProtocolGuid                      ## PRODUCES
+  gEfiStatusCodeRuntimeProtocolGuid               ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ReportStatusCodeRouterRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.uni
new file mode 100644
index 0000000000..3eb0842356
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..89be808583
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.c b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.c
new file mode 100644
index 0000000000..79c8d1e36f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.c
@@ -0,0 +1,239 @@
+/** @file
+  Report Status Code Router Driver which produces SMM Report Stataus Code Handler Protocol
+  and SMM Status Code Protocol.
+
+  Copyright (c) 2009 - 2011, 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 "ReportStatusCodeRouterSmm.h"
+
+LIST_ENTRY   mCallbackListHead          = INITIALIZE_LIST_HEAD_VARIABLE (mCallbackListHead);
+
+//
+// Report operation nest status.
+// If it is set, then the report operation has nested.
+//
+UINT32       mStatusCodeNestStatus = 0;
+
+EFI_SMM_STATUS_CODE_PROTOCOL  mSmmStatusCodeProtocol  = {
+  ReportDispatcher
+};
+
+EFI_SMM_RSC_HANDLER_PROTOCOL  mSmmRscHandlerProtocol = {
+  Register,
+  Unregister
+  };
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function.
+
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is called
+                                when a call to ReportStatusCode() occurs.
+                        
+  @retval EFI_SUCCESS           Function was successfully registered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_OUT_OF_RESOURCES  The internal buffer ran out of space. No more functions can be
+                                registered.
+  @retval EFI_ALREADY_STARTED   The function was already registered. It can't be registered again.
+  
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_SMM_RSC_HANDLER_CALLBACK   Callback
+  )
+{
+  LIST_ENTRY                      *Link;
+  SMM_RSC_HANDLER_CALLBACK_ENTRY  *CallbackEntry;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+    CallbackEntry = CR (Link, SMM_RSC_HANDLER_CALLBACK_ENTRY, Node, SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    if (CallbackEntry->RscHandlerCallback == Callback) {
+      //
+      // If the function was already registered. It can't be registered again.
+      //
+      return EFI_ALREADY_STARTED;
+    }
+  }
+
+  CallbackEntry = (SMM_RSC_HANDLER_CALLBACK_ENTRY *)AllocatePool (sizeof (SMM_RSC_HANDLER_CALLBACK_ENTRY));
+  ASSERT (CallbackEntry != NULL);
+
+  CallbackEntry->Signature          = SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE;
+  CallbackEntry->RscHandlerCallback = Callback;
+
+  InsertTailList (&mCallbackListHead, &CallbackEntry->Node);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  ReportStatusCode() messages will no longer be forwarded to the Callback function.
+  
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is to be
+                                unregistered.
+
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+                        
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_SMM_RSC_HANDLER_CALLBACK Callback
+  )
+{
+  LIST_ENTRY                        *Link;
+  SMM_RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+
+  if (Callback == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+    CallbackEntry = CR (Link, SMM_RSC_HANDLER_CALLBACK_ENTRY, Node, SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    if (CallbackEntry->RscHandlerCallback == Callback) {
+      //
+      // If the function is found in list, delete it and return.
+      //
+      RemoveEntryList (&CallbackEntry->Node);
+      FreePool (CallbackEntry);
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Provides an interface that a software module can call to report a status code.
+
+  @param  This             EFI_SMM_STATUS_CODE_PROTOCOL instance.
+  @param  Type             Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_DEVICE_ERROR      The function should not be completed due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN CONST EFI_SMM_STATUS_CODE_PROTOCOL  *This,
+  IN EFI_STATUS_CODE_TYPE                Type,
+  IN EFI_STATUS_CODE_VALUE               Value,
+  IN UINT32                              Instance,
+  IN CONST EFI_GUID                      *CallerId  OPTIONAL,
+  IN EFI_STATUS_CODE_DATA                *Data      OPTIONAL
+  )
+{
+  LIST_ENTRY                        *Link;
+  SMM_RSC_HANDLER_CALLBACK_ENTRY    *CallbackEntry;
+
+  //
+  // Use atom operation to avoid the reentant of report.
+  // If current status is not zero, then the function is reentrancy.
+  //
+  if (InterlockedCompareExchange32 (&mStatusCodeNestStatus, 0, 1) == 1) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link);) {
+    CallbackEntry = CR (Link, SMM_RSC_HANDLER_CALLBACK_ENTRY, Node, SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
+    //
+    // The handler may remove itself, so get the next handler in advance.
+    //
+    Link = GetNextNode (&mCallbackListHead, Link);
+    CallbackEntry->RscHandlerCallback (
+                     Type,
+                     Value,
+                     Instance,
+                     (EFI_GUID*)CallerId,
+                     Data
+                     );
+
+  }
+
+  //
+  // Restore the nest status of report
+  //
+  InterlockedCompareExchange32 (&mStatusCodeNestStatus, 1, 0);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Entry point of Generic Status Code Driver.
+
+  This function is the entry point of SMM Status Code Router .
+  It produces SMM Report Stataus Code Handler and Status Code protocol.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+GenericStatusCodeSmmEntry (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HANDLE     Handle;
+
+  Handle     = NULL;
+  
+  //
+  // Install SmmRscHandler Protocol
+  //
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &Handle,
+                    &gEfiSmmRscHandlerProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mSmmRscHandlerProtocol
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install SmmStatusCode Protocol
+  //
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &Handle,
+                    &gEfiSmmStatusCodeProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mSmmStatusCodeProtocol
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.h b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.h
new file mode 100644
index 0000000000..83f1bed003
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.h
@@ -0,0 +1,109 @@
+/** @file
+  Internal include file for Report Status Code Router Driver.
+
+  Copyright (c) 2009 - 2010, 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.
+
+**/
+
+#ifndef __REPORT_STATUS_CODE_ROUTER_SMM_H__
+#define __REPORT_STATUS_CODE_ROUTER_SMM_H__
+
+
+#include <Protocol/SmmReportStatusCodeHandler.h>
+#include <Protocol/SmmStatusCode.h>
+
+#include <Library/BaseLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+
+#define SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE  SIGNATURE_32 ('s', 'h', 'c', 'e')
+
+typedef struct {
+  UINTN                         Signature;
+  EFI_SMM_RSC_HANDLER_CALLBACK  RscHandlerCallback;
+  LIST_ENTRY                    Node;
+} SMM_RSC_HANDLER_CALLBACK_ENTRY;
+
+/**
+  Register the callback function for ReportStatusCode() notification.
+  
+  When this function is called the function pointer is added to an internal list and any future calls to
+  ReportStatusCode() will be forwarded to the Callback function.
+
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is called
+                                when a call to ReportStatusCode() occurs.
+                        
+  @retval EFI_SUCCESS           Function was successfully registered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_OUT_OF_RESOURCES  The internal buffer ran out of space. No more functions can be
+                                registered.
+  @retval EFI_ALREADY_STARTED   The function was already registered. It can't be registered again.
+  
+**/
+EFI_STATUS
+EFIAPI
+Register (
+  IN EFI_SMM_RSC_HANDLER_CALLBACK   Callback
+  );
+
+/**
+  Remove a previously registered callback function from the notification list.
+  
+  ReportStatusCode() messages will no longer be forwarded to the Callback function.
+  
+  @param[in] Callback           A pointer to a function of type EFI_PEI_RSC_HANDLER_CALLBACK that is to be
+                                unregistered.
+
+  @retval EFI_SUCCESS           The function was successfully unregistered.
+  @retval EFI_INVALID_PARAMETER The callback function was NULL.
+  @retval EFI_NOT_FOUND         The callback function was not found to be unregistered.
+                        
+**/
+EFI_STATUS
+EFIAPI
+Unregister (
+  IN EFI_SMM_RSC_HANDLER_CALLBACK Callback
+  );
+
+/**
+  Provides an interface that a software module can call to report a status code.
+
+  @param  This             EFI_SMM_STATUS_CODE_PROTOCOL instance.
+  @param  Type             Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS           The function completed successfully
+  @retval EFI_DEVICE_ERROR      The function should not be completed due to a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+ReportDispatcher (
+  IN CONST EFI_SMM_STATUS_CODE_PROTOCOL  *This,
+  IN EFI_STATUS_CODE_TYPE                Type,
+  IN EFI_STATUS_CODE_VALUE               Value,
+  IN UINT32                              Instance,
+  IN CONST EFI_GUID                      *CallerId  OPTIONAL,
+  IN EFI_STATUS_CODE_DATA                *Data      OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.inf b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.inf
new file mode 100644
index 0000000000..36c97ef57a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.inf
@@ -0,0 +1,56 @@
+## @file
+#  Report Status Code Router Driver which produces SMM Report Stataus Code Handler Protocol and SMM Status Code Protocol.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ReportStatusCodeRouterSmm
+  MODULE_UNI_FILE                = ReportStatusCodeRouterSmm.uni
+  FILE_GUID                      = A6885402-D022-4b0e-A509-4711B90F2A39
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  VERSION_STRING                 = 1.0  
+  ENTRY_POINT                    = GenericStatusCodeSmmEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  ReportStatusCodeRouterSmm.c
+  ReportStatusCodeRouterSmm.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SmmServicesTableLib
+  UefiDriverEntryPoint
+  DebugLib
+  BaseLib
+  SynchronizationLib
+  MemoryAllocationLib
+
+[Protocols]
+  gEfiSmmRscHandlerProtocolGuid               ## PRODUCES
+  gEfiSmmStatusCodeProtocolGuid               ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ReportStatusCodeRouterSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.uni
new file mode 100644
index 0000000000..bbee22e73e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.uni
diff --git a/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmmExtra.uni b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmmExtra.uni
new file mode 100644
index 0000000000..92264ec5d0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.c b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.c
new file mode 100644
index 0000000000..aa1ca162d6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.c
@@ -0,0 +1,155 @@
+/** @file
+  Reset Architectural Protocol implementation
+
+  Copyright (c) 2006 - 2012, 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 "ResetSystem.h"
+
+//
+// The handle onto which the Reset Architectural Protocol is installed
+//
+EFI_HANDLE  mResetHandle = NULL;
+
+/**
+  The driver's entry point.
+
+  It initializes the Reset Architectural Protocol.
+
+  @param[in] ImageHandle  The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable  A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS     The entry point is executed successfully.
+  @retval other           Cannot install ResetArch protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeResetSystem (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Make sure the Reset Architectural Protocol is not already installed in the system
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiResetArchProtocolGuid);
+
+  //
+  // Hook the runtime service table
+  //
+  gRT->ResetSystem = ResetSystem;
+
+  //
+  // Now install the Reset RT AP on a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mResetHandle,
+                  &gEfiResetArchProtocolGuid,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
+/**
+  Put the system into S3 power state.                            
+**/
+VOID
+DoS3 (
+  VOID
+  )
+{
+  EnterS3WithImmediateWake ();
+
+  //
+  // Should not return
+  //
+  CpuDeadLoop ();
+}
+
+/**
+  Resets the entire platform.
+
+  @param[in] ResetType          The type of reset to perform.
+  @param[in] ResetStatus        The status code for the reset.
+  @param[in] DataSize           The size, in bytes, of WatchdogData.
+  @param[in] ResetData          For a ResetType of EfiResetCold, EfiResetWarm, or
+                                EfiResetShutdown the data buffer starts with a Null-terminated
+                                string, optionally followed by additional binary data.
+
+**/
+VOID
+EFIAPI
+ResetSystem (
+  IN EFI_RESET_TYPE   ResetType,
+  IN EFI_STATUS       ResetStatus,
+  IN UINTN            DataSize,
+  IN VOID             *ResetData OPTIONAL
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         Size;
+  UINTN         CapsuleDataPtr;
+  
+  //
+  // Indicate reset system runtime service is called.
+  //
+  REPORT_STATUS_CODE (EFI_PROGRESS_CODE, (EFI_SOFTWARE_EFI_RUNTIME_SERVICE | EFI_SW_RS_PC_RESET_SYSTEM));
+
+  switch (ResetType) {
+  case EfiResetWarm:
+
+    //
+    //Check if there are pending capsules to process
+    //
+    Size = sizeof (CapsuleDataPtr);
+    Status =  EfiGetVariable (
+                 EFI_CAPSULE_VARIABLE_NAME,
+                 &gEfiCapsuleVendorGuid,
+                 NULL,
+                 &Size,
+                 (VOID *) &CapsuleDataPtr
+                 );
+
+    if (Status == EFI_SUCCESS) {
+      //
+      //Process capsules across a system reset.
+      //
+      DoS3();
+    }
+
+    ResetWarm ();
+
+    break;
+
+ case EfiResetCold:
+    ResetCold ();
+    break;
+
+  case EfiResetShutdown:
+    ResetShutdown ();
+    return ;
+
+  default:
+    return ;
+  }
+
+  //
+  // Given we should have reset getting here would be bad
+  //
+  ASSERT (FALSE);
+}
diff --git a/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.h b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.h
new file mode 100644
index 0000000000..c3a2a7f127
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystem.h
@@ -0,0 +1,74 @@
+/** @file
+
+  Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef _RESET_SYSTEM_H_
+#define _RESET_SYSTEM_H_
+
+
+#include <PiDxe.h>
+
+#include <Protocol/Reset.h>
+#include <Guid/CapsuleVendor.h>
+
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/IoLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/ResetSystemLib.h>
+#include <Library/ReportStatusCodeLib.h>
+
+/**
+  The driver's entry point.
+
+  It initializes the Reset Architectural Protocol.
+
+  @param[in] ImageHandle  The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable  A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS     The entry point is executed successfully.
+  @retval other           Cannot install ResetArch protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeResetSystem (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  );
+
+/**
+  Resets the entire platform.
+
+  @param[in] ResetType          The type of reset to perform.
+  @param[in] ResetStatus        The status code for the reset.
+  @param[in] DataSize           The size, in bytes, of WatchdogData.
+  @param[in] ResetData          For a ResetType of EfiResetCold, EfiResetWarm, or
+                                EfiResetShutdown the data buffer starts with a Null-terminated
+                                string, optionally followed by additional binary data.
+
+**/
+VOID
+EFIAPI
+ResetSystem (
+  IN EFI_RESET_TYPE   ResetType,
+  IN EFI_STATUS       ResetStatus,
+  IN UINTN            DataSize,
+  IN VOID             *ResetData OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.inf b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.inf
new file mode 100644
index 0000000000..7ef52b3283
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.inf
@@ -0,0 +1,65 @@
+## @file
+# This driver implements Reset Architectural Protocol.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ResetSystemRuntimeDxe
+  MODULE_UNI_FILE                = ResetSystemRuntimeDxe.uni
+  FILE_GUID                      = 4B28E4C7-FF36-4e10-93CF-A82159E777C5
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeResetSystem
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 EBC
+#
+
+[Sources]
+  ResetSystem.h
+  ResetSystem.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  ResetSystemLib
+  UefiRuntimeServicesTableLib
+  UefiRuntimeLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  IoLib
+  UefiLib
+  DebugLib
+  BaseLib
+  ReportStatusCodeLib
+
+
+[Guids]
+  gEfiCapsuleVendorGuid                         ## SOMETIMES_CONSUMES   ## Variable:L"CapsuleUpdateData"
+
+
+[Protocols]
+  gEfiResetArchProtocolGuid                     ## PRODUCES
+
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  ResetSystemRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.uni b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.uni
new file mode 100644
index 0000000000..b47b59ead4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..b20565f568
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/ResetSystemRuntimeDxe/ResetSystemRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.c b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.c
new file mode 100644
index 0000000000..b100883be6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.c
@@ -0,0 +1,360 @@
+/** @file
+ Section Extraction DXE Driver
+
+Copyright (c) 2013 - 2014, 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 <PiDxe.h>
+#include <Protocol/GuidedSectionExtraction.h>
+#include <Library/DebugLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+/**
+  The ExtractSection() function processes the input section and
+  allocates a buffer from the pool in which it returns the section
+  contents. If the section being extracted contains
+  authentication information (the section's
+  GuidedSectionHeader.Attributes field has the
+  EFI_GUIDED_SECTION_AUTH_STATUS_VALID bit set), the values
+  returned in AuthenticationStatus must reflect the results of
+  the authentication operation. Depending on the algorithm and
+  size of the encapsulated data, the time that is required to do
+  a full authentication may be prohibitively long for some
+  classes of systems. To indicate this, use
+  EFI_SECURITY_POLICY_PROTOCOL_GUID, which may be published by
+  the security policy driver (see the Platform Initialization
+  Driver Execution Environment Core Interface Specification for
+  more details and the GUID definition). If the
+  EFI_SECURITY_POLICY_PROTOCOL_GUID exists in the handle
+  database, then, if possible, full authentication should be
+  skipped and the section contents simply returned in the
+  OutputBuffer. In this case, the
+  EFI_AUTH_STATUS_PLATFORM_OVERRIDE bit AuthenticationStatus
+  must be set on return. ExtractSection() is callable only from
+  TPL_NOTIFY and below. Behavior of ExtractSection() at any
+  EFI_TPL above TPL_NOTIFY is undefined. Type EFI_TPL is
+  defined in RaiseTPL() in the UEFI 2.0 specification.
+
+
+  @param This         Indicates the
+                      EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL instance.
+  @param InputSection Buffer containing the input GUIDed section
+                      to be processed. OutputBuffer OutputBuffer
+                      is allocated from boot services pool
+                      memory and contains the new section
+                      stream. The caller is responsible for
+                      freeing this buffer.
+  @param OutputBuffer *OutputBuffer is allocated from boot services
+                      pool memory and contains the new section stream.
+                      The caller is responsible for freeing this buffer.
+  @param OutputSize   A pointer to a caller-allocated UINTN in
+                      which the size of OutputBuffer allocation
+                      is stored. If the function returns
+                      anything other than EFI_SUCCESS, the value
+                      of OutputSize is undefined.
+
+  @param AuthenticationStatus A pointer to a caller-allocated
+                              UINT32 that indicates the
+                              authentication status of the
+                              output buffer. If the input
+                              section's
+                              GuidedSectionHeader.Attributes
+                              field has the
+                              EFI_GUIDED_SECTION_AUTH_STATUS_VAL
+                              bit as clear, AuthenticationStatus
+                              must return zero. Both local bits
+                              (19:16) and aggregate bits (3:0)
+                              in AuthenticationStatus are
+                              returned by ExtractSection().
+                              These bits reflect the status of
+                              the extraction operation. The bit
+                              pattern in both regions must be
+                              the same, as the local and
+                              aggregate authentication statuses
+                              have equivalent meaning at this
+                              level. If the function returns
+                              anything other than EFI_SUCCESS,
+                              the value of AuthenticationStatus
+                              is undefined.
+
+
+  @retval EFI_SUCCESS          The InputSection was successfully
+                               processed and the section contents were
+                               returned.
+
+  @retval EFI_OUT_OF_RESOURCES The system has insufficient
+                               resources to process the
+                               request.
+
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction
+                                Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL *This,
+  IN CONST  VOID                                   *InputSection,
+  OUT       VOID                                   **OutputBuffer,
+  OUT       UINTN                                  *OutputSize,
+  OUT       UINT32                                 *AuthenticationStatus
+  );
+
+//
+// Module global for the Section Extraction Protocol handle
+//
+EFI_HANDLE mSectionExtractionHandle = NULL;
+
+//
+// Module global for the Section Extraction Protocol instance
+//
+EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL mCustomGuidedSectionExtractionProtocol = {
+  CustomGuidedSectionExtract
+};
+
+/**
+  The ExtractSection() function processes the input section and
+  allocates a buffer from the pool in which it returns the section
+  contents. If the section being extracted contains
+  authentication information (the section's
+  GuidedSectionHeader.Attributes field has the
+  EFI_GUIDED_SECTION_AUTH_STATUS_VALID bit set), the values
+  returned in AuthenticationStatus must reflect the results of
+  the authentication operation. Depending on the algorithm and
+  size of the encapsulated data, the time that is required to do
+  a full authentication may be prohibitively long for some
+  classes of systems. To indicate this, use
+  EFI_SECURITY_POLICY_PROTOCOL_GUID, which may be published by
+  the security policy driver (see the Platform Initialization
+  Driver Execution Environment Core Interface Specification for
+  more details and the GUID definition). If the
+  EFI_SECURITY_POLICY_PROTOCOL_GUID exists in the handle
+  database, then, if possible, full authentication should be
+  skipped and the section contents simply returned in the
+  OutputBuffer. In this case, the
+  EFI_AUTH_STATUS_PLATFORM_OVERRIDE bit AuthenticationStatus
+  must be set on return. ExtractSection() is callable only from
+  TPL_NOTIFY and below. Behavior of ExtractSection() at any
+  EFI_TPL above TPL_NOTIFY is undefined. Type EFI_TPL is
+  defined in RaiseTPL() in the UEFI 2.0 specification.
+
+
+  @param This         Indicates the
+                      EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL instance.
+  @param InputSection Buffer containing the input GUIDed section
+                      to be processed. OutputBuffer OutputBuffer
+                      is allocated from boot services pool
+                      memory and contains the new section
+                      stream. The caller is responsible for
+                      freeing this buffer.
+  @param OutputBuffer *OutputBuffer is allocated from boot services
+                      pool memory and contains the new section stream.
+                      The caller is responsible for freeing this buffer.
+  @param OutputSize   A pointer to a caller-allocated UINTN in
+                      which the size of OutputBuffer allocation
+                      is stored. If the function returns
+                      anything other than EFI_SUCCESS, the value
+                      of OutputSize is undefined.
+
+  @param AuthenticationStatus A pointer to a caller-allocated
+                              UINT32 that indicates the
+                              authentication status of the
+                              output buffer. If the input
+                              section's
+                              GuidedSectionHeader.Attributes
+                              field has the
+                              EFI_GUIDED_SECTION_AUTH_STATUS_VAL
+                              bit as clear, AuthenticationStatus
+                              must return zero. Both local bits
+                              (19:16) and aggregate bits (3:0)
+                              in AuthenticationStatus are
+                              returned by ExtractSection().
+                              These bits reflect the status of
+                              the extraction operation. The bit
+                              pattern in both regions must be
+                              the same, as the local and
+                              aggregate authentication statuses
+                              have equivalent meaning at this
+                              level. If the function returns
+                              anything other than EFI_SUCCESS,
+                              the value of AuthenticationStatus
+                              is undefined.
+
+
+  @retval EFI_SUCCESS          The InputSection was successfully
+                               processed and the section contents were
+                               returned.
+
+  @retval EFI_OUT_OF_RESOURCES The system has insufficient
+                               resources to process the
+                               request.
+
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction
+                                Protocol.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_GUIDED_SECTION_EXTRACTION_PROTOCOL *This,
+  IN CONST  VOID                                   *InputSection,
+  OUT       VOID                                   **OutputBuffer,
+  OUT       UINTN                                  *OutputSize,
+  OUT       UINT32                                 *AuthenticationStatus
+  )
+{
+  EFI_STATUS      Status;
+  VOID            *ScratchBuffer;
+  VOID            *AllocatedOutputBuffer;
+  UINT32          OutputBufferSize;
+  UINT32          ScratchBufferSize;
+  UINT16          SectionAttribute;
+
+  //
+  // Init local variable
+  //
+  ScratchBuffer         = NULL;
+  AllocatedOutputBuffer = NULL;
+
+  //
+  // Call GetInfo to get the size and attribute of input guided section data.
+  //
+  Status = ExtractGuidedSectionGetInfo (
+             InputSection,
+             &OutputBufferSize,
+             &ScratchBufferSize,
+             &SectionAttribute
+             );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "GetInfo from guided section Failed - %r\n", Status));
+    return Status;
+  }
+
+  if (ScratchBufferSize > 0) {
+    //
+    // Allocate scratch buffer
+    //
+    ScratchBuffer = AllocatePool (ScratchBufferSize);
+    if (ScratchBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  if (OutputBufferSize > 0) {
+    //
+    // Allocate output buffer
+    //
+    AllocatedOutputBuffer = AllocatePool (OutputBufferSize);
+    if (AllocatedOutputBuffer == NULL) {
+      FreePool (ScratchBuffer);
+      return EFI_OUT_OF_RESOURCES;
+    }
+    *OutputBuffer = AllocatedOutputBuffer;
+  }
+
+  //
+  // Call decode function to extract raw data from the guided section.
+  //
+  Status = ExtractGuidedSectionDecode (
+             InputSection,
+             OutputBuffer,
+             ScratchBuffer,
+             AuthenticationStatus
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // Decode failed
+    //
+    if (AllocatedOutputBuffer != NULL) {
+      FreePool (AllocatedOutputBuffer);
+    }
+    if (ScratchBuffer != NULL) {
+      FreePool (ScratchBuffer);
+    }
+    DEBUG ((DEBUG_ERROR, "Extract guided section Failed - %r\n", Status));
+    return Status;
+  }
+
+  if (*OutputBuffer != AllocatedOutputBuffer) {
+    //
+    // OutputBuffer was returned as a different value,
+    // so copy section contents to the allocated memory buffer.
+    //
+    CopyMem (AllocatedOutputBuffer, *OutputBuffer, OutputBufferSize);
+    *OutputBuffer = AllocatedOutputBuffer;
+  }
+
+  //
+  // Set real size of output buffer.
+  //
+  *OutputSize = (UINTN) OutputBufferSize;
+
+  //
+  // Free unused scratch buffer.
+  //
+  if (ScratchBuffer != NULL) {
+    FreePool (ScratchBuffer);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Main entry for the Section Extraction DXE module.
+
+  This routine registers the Section Extraction Protocols that have been registered 
+  with the Section Extraction Library.
+  
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+SectionExtractionDxeEntry (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  EFI_GUID    *ExtractHandlerGuidTable;
+  UINTN       ExtractHandlerNumber;
+
+  //
+  // Get custom extract guided section method guid list
+  //
+  ExtractHandlerNumber = ExtractGuidedSectionGetGuidList (&ExtractHandlerGuidTable);
+
+  //
+  // Install custom guided extraction protocol
+  //
+  while (ExtractHandlerNumber-- > 0) {
+    Status = gBS->InstallMultipleProtocolInterfaces (
+                    &mSectionExtractionHandle,
+                    &ExtractHandlerGuidTable [ExtractHandlerNumber], &mCustomGuidedSectionExtractionProtocol,
+                    NULL
+                    );
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.inf b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.inf
new file mode 100644
index 0000000000..39482fe1b2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.inf
@@ -0,0 +1,48 @@
+## @file
+# Section Extraction DXE Driver
+#
+# Copyright (c) 2013 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SectionExtractionDxe
+  FILE_GUID                      = A0E8E04C-9B5A-43be-8B7D-C98760492B68
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SectionExtractionDxeEntry
+  MODULE_UNI_FILE                = SectionExtractionDxe.uni
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  SectionExtractionDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  ExtractGuidedSectionLib
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SectionExtractionDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.uni b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.uni
new file mode 100644
index 0000000000..b469cb8ec8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxeExtra.uni b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxeExtra.uni
new file mode 100644
index 0000000000..35844350af
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionDxe/SectionExtractionDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.c b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.c
new file mode 100644
index 0000000000..c4a3508fbd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.c
@@ -0,0 +1,274 @@
+/** @file
+ Section Extraction PEIM
+
+Copyright (c) 2013 - 2014, 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 <PiPei.h>
+#include <Ppi/GuidedSectionExtraction.h>
+#include <Library/DebugLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PeiServicesLib.h>
+
+/**
+  The ExtractSection() function processes the input section and
+  returns a pointer to the section contents. If the section being
+  extracted does not require processing (if the section
+  GuidedSectionHeader.Attributes has the
+  EFI_GUIDED_SECTION_PROCESSING_REQUIRED field cleared), then
+  OutputBuffer is just updated to point to the start of the
+  section's contents. Otherwise, *Buffer must be allocated
+  from PEI permanent memory.
+
+  @param This                   Indicates the
+                                EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI instance.
+                                Buffer containing the input GUIDed section to be
+                                processed. OutputBuffer OutputBuffer is
+                                allocated from PEI permanent memory and contains
+                                the new section stream.
+  @param InputSection           A pointer to the input buffer, which contains
+                                the input section to be processed.
+  @param OutputBuffer           A pointer to a caller-allocated buffer, whose
+                                size is specified by the contents of OutputSize.
+  @param OutputSize             A pointer to a caller-allocated
+                                UINTN in which the size of *OutputBuffer
+                                allocation is stored. If the function
+                                returns anything other than EFI_SUCCESS,
+                                the value of OutputSize is undefined.
+  @param AuthenticationStatus   A pointer to a caller-allocated
+                                UINT32 that indicates the
+                                authentication status of the
+                                output buffer. If the input
+                                section's GuidedSectionHeader.
+                                Attributes field has the
+                                EFI_GUIDED_SECTION_AUTH_STATUS_VALID 
+                                bit as clear,
+                                AuthenticationStatus must return
+                                zero. These bits reflect the
+                                status of the extraction
+                                operation. If the function
+                                returns anything other than
+                                EFI_SUCCESS, the value of
+                                AuthenticationStatus is
+                                undefined.
+  
+  @retval EFI_SUCCESS           The InputSection was
+                                successfully processed and the
+                                section contents were returned.
+  
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient
+                                resources to process the request.
+  
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction PPI.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI *This,
+  IN CONST  VOID                                  *InputSection,
+  OUT       VOID                                  **OutputBuffer,
+  OUT       UINTN                                 *OutputSize,
+  OUT       UINT32                                *AuthenticationStatus
+  );
+
+//
+// Module global for the Section Extraction PPI instance
+//
+CONST EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI mCustomGuidedSectionExtractionPpi = {
+  CustomGuidedSectionExtract
+};
+
+/**
+  The ExtractSection() function processes the input section and
+  returns a pointer to the section contents. If the section being
+  extracted does not require processing (if the section
+  GuidedSectionHeader.Attributes has the
+  EFI_GUIDED_SECTION_PROCESSING_REQUIRED field cleared), then
+  OutputBuffer is just updated to point to the start of the
+  section's contents. Otherwise, *Buffer must be allocated
+  from PEI permanent memory.
+
+  @param This                   Indicates the
+                                EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI instance.
+                                Buffer containing the input GUIDed section to be
+                                processed. OutputBuffer OutputBuffer is
+                                allocated from PEI permanent memory and contains
+                                the new section stream.
+  @param InputSection           A pointer to the input buffer, which contains
+                                the input section to be processed.
+  @param OutputBuffer           A pointer to a caller-allocated buffer, whose
+                                size is specified by the contents of OutputSize.
+  @param OutputSize             A pointer to a caller-allocated
+                                UINTN in which the size of *OutputBuffer
+                                allocation is stored. If the function
+                                returns anything other than EFI_SUCCESS,
+                                the value of OutputSize is undefined.
+  @param AuthenticationStatus   A pointer to a caller-allocated
+                                UINT32 that indicates the
+                                authentication status of the
+                                output buffer. If the input
+                                section's GuidedSectionHeader.
+                                Attributes field has the
+                                EFI_GUIDED_SECTION_AUTH_STATUS_VALID 
+                                bit as clear,
+                                AuthenticationStatus must return
+                                zero. These bits reflect the
+                                status of the extraction
+                                operation. If the function
+                                returns anything other than
+                                EFI_SUCCESS, the value of
+                                AuthenticationStatus is
+                                undefined.
+  
+  @retval EFI_SUCCESS           The InputSection was
+                                successfully processed and the
+                                section contents were returned.
+  
+  @retval EFI_OUT_OF_RESOURCES  The system has insufficient
+                                resources to process the request.
+  
+  @retval EFI_INVALID_PARAMETER The GUID in InputSection does
+                                not match this instance of the
+                                GUIDed Section Extraction PPI.
+
+**/
+EFI_STATUS
+EFIAPI
+CustomGuidedSectionExtract (
+  IN CONST  EFI_PEI_GUIDED_SECTION_EXTRACTION_PPI *This,
+  IN CONST  VOID                                  *InputSection,
+  OUT       VOID                                  **OutputBuffer,
+  OUT       UINTN                                 *OutputSize,
+  OUT       UINT32                                *AuthenticationStatus
+  )
+{
+  EFI_STATUS      Status;
+  UINT8           *ScratchBuffer;
+  UINT32          ScratchBufferSize;
+  UINT32          OutputBufferSize;
+  UINT16          SectionAttribute;
+  
+  //
+  // Init local variable
+  //
+  ScratchBuffer = NULL;
+
+  //
+  // Call GetInfo to get the size and attribute of input guided section data.
+  //
+  Status = ExtractGuidedSectionGetInfo (
+             InputSection,
+             &OutputBufferSize,
+             &ScratchBufferSize,
+             &SectionAttribute
+             );
+  
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "GetInfo from guided section Failed - %r\n", Status));
+    return Status;
+  }
+  
+  if (ScratchBufferSize != 0) {
+    //
+    // Allocate scratch buffer
+    //
+    ScratchBuffer = AllocatePages (EFI_SIZE_TO_PAGES (ScratchBufferSize));
+    if (ScratchBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  }
+
+  if (((SectionAttribute & EFI_GUIDED_SECTION_PROCESSING_REQUIRED) != 0) && OutputBufferSize > 0) {  
+    //
+    // Allocate output buffer
+    //
+    *OutputBuffer = AllocatePages (EFI_SIZE_TO_PAGES (OutputBufferSize) + 1);
+    if (*OutputBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+    DEBUG ((DEBUG_INFO, "Customized Guided section Memory Size required is 0x%x and address is 0x%p\n", OutputBufferSize, *OutputBuffer));
+    //
+    // *OutputBuffer still is one section. Adjust *OutputBuffer offset, 
+    // skip EFI section header to make section data at page alignment.
+    //
+    *OutputBuffer = (VOID *)((UINT8 *) *OutputBuffer + EFI_PAGE_SIZE - sizeof (EFI_COMMON_SECTION_HEADER));
+  }
+  
+  Status = ExtractGuidedSectionDecode (
+             InputSection, 
+             OutputBuffer,
+             ScratchBuffer,
+             AuthenticationStatus
+             );
+  if (EFI_ERROR (Status)) {
+    //
+    // Decode failed
+    //
+    DEBUG ((DEBUG_ERROR, "Extract guided section Failed - %r\n", Status));
+    return Status;
+  }
+  
+  *OutputSize = (UINTN) OutputBufferSize;
+  
+  return EFI_SUCCESS;
+}
+
+/**
+  Main entry for Section Extraction PEIM driver.
+  
+  This routine registers the Section Extraction PPIs that have been registered 
+  with the Section Extraction Library.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+SectionExtractionPeiEntry (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  ) 
+{
+  EFI_STATUS              Status;
+  EFI_GUID                *ExtractHandlerGuidTable;
+  UINTN                   ExtractHandlerNumber;
+  EFI_PEI_PPI_DESCRIPTOR  *GuidPpi;
+
+  //
+  // Get custom extract guided section method guid list 
+  //
+  ExtractHandlerNumber = ExtractGuidedSectionGetGuidList (&ExtractHandlerGuidTable);
+  
+  //
+  // Install custom extraction guid PPI
+  //
+  if (ExtractHandlerNumber > 0) {
+    GuidPpi = (EFI_PEI_PPI_DESCRIPTOR *) AllocatePool (ExtractHandlerNumber * sizeof (EFI_PEI_PPI_DESCRIPTOR));
+    ASSERT (GuidPpi != NULL);
+    while (ExtractHandlerNumber-- > 0) {
+      GuidPpi->Flags = EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST;
+      GuidPpi->Ppi   = (VOID *) &mCustomGuidedSectionExtractionPpi;
+      GuidPpi->Guid  = &ExtractHandlerGuidTable[ExtractHandlerNumber];
+      Status = PeiServicesInstallPpi (GuidPpi++);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+  
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.inf b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.inf
new file mode 100644
index 0000000000..912031a27b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.inf
@@ -0,0 +1,49 @@
+## @file
+# Section Extraction PEI Module
+#
+# Produce one or more Section Extraction PPIs.
+#
+# Copyright (c) 2013 - 2014, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SectionExtractionPei
+  FILE_GUID                      = EED5EA31-38E2-463d-B623-2C57702B8A1C
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SectionExtractionPeiEntry
+  MODULE_UNI_FILE                = SectionExtractionPei.uni
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  SectionExtractionPei.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  PeimEntryPoint
+  ExtractGuidedSectionLib
+  DebugLib
+  MemoryAllocationLib
+  PeiServicesLib
+  
+[Depex]
+  gEfiPeiMemoryDiscoveredPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SectionExtractionPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.uni b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.uni
new file mode 100644
index 0000000000..be3fb2ae15
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPei.uni
diff --git a/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPeiExtra.uni b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPeiExtra.uni
new file mode 100644
index 0000000000..99472a3c52
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SectionExtractionPei/SectionExtractionPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStub.c b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStub.c
new file mode 100644
index 0000000000..22e6c7f4ea
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStub.c
@@ -0,0 +1,204 @@
+/** @file
+  This driver produces Security2 and Security architectural protocol based on SecurityManagementLib.
+ 
+  Copyright (c) 2006 - 2013, 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 <Uefi.h>
+#include <Protocol/Security.h>
+#include <Protocol/Security2.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/SecurityManagementLib.h>
+
+//
+// Handle for the Security Architectural Protocol instance produced by this driver
+//
+EFI_HANDLE                  mSecurityArchProtocolHandle = NULL;
+
+/**
+  The EFI_SECURITY_ARCH_PROTOCOL (SAP) is used to abstract platform-specific 
+  policy from the DXE core response to an attempt to use a file that returns a 
+  given status for the authentication check from the section extraction protocol.  
+
+  The possible responses in a given SAP implementation may include locking 
+  flash upon failure to authenticate, attestation logging for all signed drivers, 
+  and other exception operations.  The File parameter allows for possible logging 
+  within the SAP of the driver.
+
+  If File is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is safe for the DXE Core to use, then EFI_SUCCESS is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is not safe for the DXE Core to use under any circumstances, 
+  then EFI_ACCESS_DENIED is returned.
+
+  If the file specified by File with an authentication status specified by 
+  AuthenticationStatus is not safe for the DXE Core to use right now, but it 
+  might be possible to use it at a future time, then EFI_SECURITY_VIOLATION is 
+  returned.
+
+  @param  This             The EFI_SECURITY_ARCH_PROTOCOL instance.
+  @param  AuthenticationStatus 
+                           This is the authentication type returned from the Section
+                           Extraction protocol. See the Section Extraction Protocol
+                           Specification for details on this type.
+  @param  File             This is a pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+
+  @retval EFI_SUCCESS            Do nothing and return success.
+  @retval EFI_INVALID_PARAMETER  File is NULL.
+**/
+EFI_STATUS
+EFIAPI
+SecurityStubAuthenticateState (
+  IN CONST EFI_SECURITY_ARCH_PROTOCOL  *This,
+  IN UINT32                            AuthenticationStatus,
+  IN CONST EFI_DEVICE_PATH_PROTOCOL    *File
+  )
+{
+  EFI_STATUS Status;
+  
+  Status = ExecuteSecurity2Handlers (EFI_AUTH_OPERATION_AUTHENTICATION_STATE, 
+                                   AuthenticationStatus, 
+                                   File,
+                                   NULL, 
+                                   0, 
+                                   FALSE
+                                   );
+  if (Status == EFI_SUCCESS) {
+    Status = ExecuteSecurityHandlers (AuthenticationStatus, File);
+  }
+  
+  return Status;
+}
+
+/**
+  The DXE Foundation uses this service to measure and/or verify a UEFI image.
+
+  This service abstracts the invocation of Trusted Computing Group (TCG) measured boot, UEFI
+  Secure boot, and UEFI User Identity infrastructure. For the former two, the DXE Foundation
+  invokes the FileAuthentication() with a DevicePath and corresponding image in
+  FileBuffer memory. The TCG measurement code will record the FileBuffer contents into the
+  appropriate PCR. The image verification logic will confirm the integrity and provenance of the
+  image in FileBuffer of length FileSize . The origin of the image will be DevicePath in
+  these cases.
+  If the FileBuffer is NULL, the interface will determine if the DevicePath can be connected
+  in order to support the User Identification policy.
+  
+  @param  This             The EFI_SECURITY2_ARCH_PROTOCOL instance.
+  @param  File             A pointer to the device path of the file that is
+                           being dispatched. This will optionally be used for logging.
+  @param  FileBuffer       A pointer to the buffer with the UEFI file image.
+  @param  FileSize         The size of the file.
+  @param  BootPolicy       A boot policy that was used to call LoadImage() UEFI service. If
+                           FileAuthentication() is invoked not from the LoadImage(),
+                           BootPolicy must be set to FALSE.
+  
+  @retval EFI_SUCCESS             The file specified by DevicePath and non-NULL
+                                  FileBuffer did authenticate, and the platform policy dictates
+                                  that the DXE Foundation may use the file.
+  @retval EFI_SUCCESS             The device path specified by NULL device path DevicePath
+                                  and non-NULL FileBuffer did authenticate, and the platform
+                                  policy dictates that the DXE Foundation may execute the image in
+                                  FileBuffer.
+  @retval EFI_SUCCESS             FileBuffer is NULL and current user has permission to start
+                                  UEFI device drivers on the device path specified by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  The file specified by DevicePath and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the file should be
+                                  placed in the untrusted state. The image has been added to the file
+                                  execution table.
+  @retval EFI_ACCESS_DENIED       The file specified by File and FileBuffer did not
+                                  authenticate, and the platform policy dictates that the DXE
+                                  Foundation many not use File.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is NULL and the user has no
+                                  permission to start UEFI device drivers on the device path specified
+                                  by DevicePath.
+  @retval EFI_SECURITY_VIOLATION  FileBuffer is not NULL and the user has no permission to load
+                                  drivers from the device path specified by DevicePath. The
+                                  image has been added into the list of the deferred images.
+**/
+EFI_STATUS
+EFIAPI
+Security2StubAuthenticate (
+  IN CONST EFI_SECURITY2_ARCH_PROTOCOL *This,
+  IN CONST EFI_DEVICE_PATH_PROTOCOL    *File,
+  IN VOID                              *FileBuffer,
+  IN UINTN                             FileSize,
+  IN BOOLEAN                           BootPolicy
+  )
+{
+  return ExecuteSecurity2Handlers (EFI_AUTH_OPERATION_VERIFY_IMAGE | 
+                                   EFI_AUTH_OPERATION_DEFER_IMAGE_LOAD | 
+                                   EFI_AUTH_OPERATION_MEASURE_IMAGE |
+                                   EFI_AUTH_OPERATION_CONNECT_POLICY, 
+                                   0, 
+                                   File,
+                                   FileBuffer, 
+                                   FileSize, 
+                                   BootPolicy
+                                   );
+}
+
+//
+// Security2 and Security Architectural Protocol instance produced by this driver
+//
+EFI_SECURITY_ARCH_PROTOCOL  mSecurityStub = { 
+  SecurityStubAuthenticateState 
+};
+
+EFI_SECURITY2_ARCH_PROTOCOL mSecurity2Stub = { 
+  Security2StubAuthenticate 
+};
+
+/**
+  Installs Security2 and Security Architectural Protocol.
+
+  @param  ImageHandle  The image handle of this driver.
+  @param  SystemTable  A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   Install the sample Security Architectural Protocol successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SecurityStubInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Make sure the Security Architectural Protocol is not already installed in the system
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiSecurity2ArchProtocolGuid);
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiSecurityArchProtocolGuid);
+
+  //
+  // Install the Security Architectural Protocol onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mSecurityArchProtocolHandle,
+                  &gEfiSecurity2ArchProtocolGuid,
+                  &mSecurity2Stub,
+                  &gEfiSecurityArchProtocolGuid,
+                  &mSecurityStub,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.inf b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.inf
new file mode 100644
index 0000000000..9d1c7b8dcb
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.inf
@@ -0,0 +1,51 @@
+## @file
+#  This driver produces security2 and security architectural protocol based on SecurityManagementLib.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SecurityStubDxe
+  MODULE_UNI_FILE                = SecurityStubDxe.uni
+  FILE_GUID                      = F80697E9-7FD6-4665-8646-88E33EF71DFC
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SecurityStubInitialize
+
+
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  SecurityStub.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  SecurityManagementLib
+
+[Protocols]
+  gEfiSecurityArchProtocolGuid                  ## PRODUCES
+  gEfiSecurity2ArchProtocolGuid                 ## PRODUCES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SecurityStubDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.uni b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.uni
new file mode 100644
index 0000000000..866060beae
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxeExtra.uni b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxeExtra.uni
new file mode 100644
index 0000000000..cd3616e435
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SecurityStubDxe/SecurityStubDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.inf b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.inf
new file mode 100644
index 0000000000..145101b708
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.inf
@@ -0,0 +1,49 @@
+## @file
+#  Serial driver that layers on top of a Serial Port Library instance.
+#
+#  Copyright (c) 2008 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SerialDxe
+  FILE_GUID                      = D3987D4B-971A-435F-8CAF-4967EB627241
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = SerialDxeInitialize
+
+[Sources.common]
+  SerialIo.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  DebugLib
+  PcdLib
+  SerialPortLib
+
+[Protocols]
+  gEfiSerialIoProtocolGuid      ## PRODUCES
+  gEfiDevicePathProtocolGuid    ## PRODUCES
+
+[Pcd]
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultBaudRate ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultDataBits ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultParity   ## CONSUMES
+  gEfiMdePkgTokenSpaceGuid.PcdUartDefaultStopBits ## CONSUMES
+
+[Depex]
+  TRUE
diff --git a/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.uni b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.uni
new file mode 100644
index 0000000000..9df8bea8aa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SerialDxe/SerialDxeExtra.uni b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxeExtra.uni
new file mode 100644
index 0000000000..bfb8420d05
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SerialDxe/SerialDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SerialDxe/SerialIo.c b/Core/MdeModulePkg/Universal/SerialDxe/SerialIo.c
new file mode 100644
index 0000000000..de928d1719
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SerialDxe/SerialIo.c
@@ -0,0 +1,526 @@
+/** @file
+  Serial driver that layers on top of a Serial Port Library instance.
+
+  Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
+  Copyright (c) 2013-2014, ARM Ltd. All rights reserved.<BR>
+  Copyright (c) 2015, 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 <Library/UefiBootServicesTableLib.h>
+#include <Library/SerialPortLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+
+#include <Protocol/SerialIo.h>
+#include <Protocol/DevicePath.h>
+
+typedef struct {
+  VENDOR_DEVICE_PATH        Guid;
+  UART_DEVICE_PATH          Uart;
+  EFI_DEVICE_PATH_PROTOCOL  End;
+} SERIAL_DEVICE_PATH;
+
+/**
+  Reset the serial device.
+
+  @param  This              Protocol instance pointer.
+
+  @retval EFI_SUCCESS       The device was reset.
+  @retval EFI_DEVICE_ERROR  The serial device could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL *This
+  );
+
+/**
+  Sets the baud rate, receive FIFO depth, transmit/receive time out, parity,
+  data bits, and stop bits on a serial device.
+
+  @param  This             Protocol instance pointer.
+  @param  BaudRate         The requested baud rate. A BaudRate value of 0 will use the the
+                           device's default interface speed.
+  @param  ReceiveFifoDepth The requested depth of the FIFO on the receive side of the
+                           serial interface. A ReceiveFifoDepth value of 0 will use
+                           the device's default FIFO depth.
+  @param  Timeout          The requested time out for a single character in microseconds.
+                           This timeout applies to both the transmit and receive side of the
+                           interface. A Timeout value of 0 will use the device's default time
+                           out value.
+  @param  Parity           The type of parity to use on this serial device. A Parity value of
+                           DefaultParity will use the device's default parity value.
+  @param  DataBits         The number of data bits to use on the serial device. A DataBits
+                           value of 0 will use the device's default data bit setting.
+  @param  StopBits         The number of stop bits to use on this serial device. A StopBits
+                           value of DefaultStopBits will use the device's default number of
+                           stop bits.
+
+  @retval EFI_SUCCESS      The device was reset.
+  @retval EFI_DEVICE_ERROR The serial device could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN UINT64                 BaudRate,
+  IN UINT32                 ReceiveFifoDepth,
+  IN UINT32                 Timeout,
+  IN EFI_PARITY_TYPE        Parity,
+  IN UINT8                  DataBits,
+  IN EFI_STOP_BITS_TYPE     StopBits
+  );
+
+/**
+  Set the control bits on a serial device
+
+  @param  This             Protocol instance pointer.
+  @param  Control          Set the bits of Control that are settable.
+
+  @retval EFI_SUCCESS      The new control bits were set on the serial device.
+  @retval EFI_UNSUPPORTED  The serial device does not support this operation.
+  @retval EFI_DEVICE_ERROR The serial device is not functioning correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN UINT32                 Control
+  );
+
+/**
+  Retrieves the status of the control bits on a serial device
+
+  @param  This              Protocol instance pointer.
+  @param  Control           A pointer to return the current Control signals from the serial device.
+
+  @retval EFI_SUCCESS       The control bits were read from the serial device.
+  @retval EFI_DEVICE_ERROR  The serial device is not functioning correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  OUT UINT32                *Control
+  );
+
+/**
+  Writes data to a serial device.
+
+  @param  This              Protocol instance pointer.
+  @param  BufferSize        On input, the size of the Buffer. On output, the amount of
+                            data actually written.
+  @param  Buffer            The buffer of data to write
+
+  @retval EFI_SUCCESS       The data was written.
+  @retval EFI_DEVICE_ERROR  The device reported an error.
+  @retval EFI_TIMEOUT       The data write was stopped due to a timeout.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN OUT UINTN              *BufferSize,
+  IN VOID                   *Buffer
+  );
+
+/**
+  Reads data from a serial device.
+
+  @param  This              Protocol instance pointer.
+  @param  BufferSize        On input, the size of the Buffer. On output, the amount of
+                            data returned in Buffer.
+  @param  Buffer            The buffer to return the data into.
+
+  @retval EFI_SUCCESS       The data was read.
+  @retval EFI_DEVICE_ERROR  The device reported an error.
+  @retval EFI_TIMEOUT       The data write was stopped due to a timeout.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN OUT UINTN              *BufferSize,
+  OUT VOID                  *Buffer
+  );
+
+EFI_HANDLE mSerialHandle = NULL;
+
+SERIAL_DEVICE_PATH mSerialDevicePath = {
+  {
+    { HARDWARE_DEVICE_PATH, HW_VENDOR_DP, { sizeof (VENDOR_DEVICE_PATH), 0} },
+    EFI_CALLER_ID_GUID  // Use the driver's GUID
+  },
+  {
+    { MESSAGING_DEVICE_PATH, MSG_UART_DP, { sizeof (UART_DEVICE_PATH), 0} },
+    0,                  // Reserved
+    0,                  // BaudRate
+    0,                  // DataBits
+    0,                  // Parity
+    0                   // StopBits
+  },
+  { END_DEVICE_PATH_TYPE, END_ENTIRE_DEVICE_PATH_SUBTYPE, { sizeof (EFI_DEVICE_PATH_PROTOCOL), 0 } }
+};
+
+//
+// Template used to initialize the Serial IO protocols.
+//
+EFI_SERIAL_IO_MODE mSerialIoMode = {
+  0, // ControlMask
+  0, // Timeout
+  0, // BaudRate
+  1, // ReceiveFifoDepth
+  0, // DataBits
+  0, // Parity
+  0  // StopBits
+};
+
+EFI_SERIAL_IO_PROTOCOL mSerialIoTemplate = {
+  SERIAL_IO_INTERFACE_REVISION,
+  SerialReset,
+  SerialSetAttributes,
+  SerialSetControl,
+  SerialGetControl,
+  SerialWrite,
+  SerialRead,
+  &mSerialIoMode
+};
+
+/**
+  Reset the serial device.
+
+  @param  This              Protocol instance pointer.
+
+  @retval EFI_SUCCESS       The device was reset.
+  @retval EFI_DEVICE_ERROR  The serial device could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialReset (
+  IN EFI_SERIAL_IO_PROTOCOL *This
+  )
+{
+  EFI_STATUS    Status;
+  EFI_TPL       Tpl;
+
+  Status = SerialPortInitialize ();
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Set the Serial I/O mode and update the device path
+  //
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  //
+  // Set the Serial I/O mode
+  //
+  This->Mode->ReceiveFifoDepth  = 1;
+  This->Mode->Timeout           = 0;
+  This->Mode->BaudRate          = PcdGet64 (PcdUartDefaultBaudRate);
+  This->Mode->DataBits          = (UINT32) PcdGet8 (PcdUartDefaultDataBits);
+  This->Mode->Parity            = (UINT32) PcdGet8 (PcdUartDefaultParity);
+  This->Mode->StopBits          = (UINT32) PcdGet8 (PcdUartDefaultStopBits);
+
+  //
+  // Check if the device path has actually changed
+  //
+  if (mSerialDevicePath.Uart.BaudRate == This->Mode->BaudRate &&
+      mSerialDevicePath.Uart.DataBits == (UINT8) This->Mode->DataBits &&
+      mSerialDevicePath.Uart.Parity   == (UINT8) This->Mode->Parity &&
+      mSerialDevicePath.Uart.StopBits == (UINT8) This->Mode->StopBits
+     ) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Update the device path
+  //
+  mSerialDevicePath.Uart.BaudRate = This->Mode->BaudRate;
+  mSerialDevicePath.Uart.DataBits = (UINT8) This->Mode->DataBits;
+  mSerialDevicePath.Uart.Parity   = (UINT8) This->Mode->Parity;
+  mSerialDevicePath.Uart.StopBits = (UINT8) This->Mode->StopBits;
+
+  Status = gBS->ReinstallProtocolInterface (
+                  mSerialHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mSerialDevicePath,
+                  &mSerialDevicePath
+                  );
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Sets the baud rate, receive FIFO depth, transmit/receive time out, parity,
+  data bits, and stop bits on a serial device.
+
+  @param  This             Protocol instance pointer.
+  @param  BaudRate         The requested baud rate. A BaudRate value of 0 will use the the
+                           device's default interface speed.
+  @param  ReceiveFifoDepth The requested depth of the FIFO on the receive side of the
+                           serial interface. A ReceiveFifoDepth value of 0 will use
+                           the device's default FIFO depth.
+  @param  Timeout          The requested time out for a single character in microseconds.
+                           This timeout applies to both the transmit and receive side of the
+                           interface. A Timeout value of 0 will use the device's default time
+                           out value.
+  @param  Parity           The type of parity to use on this serial device. A Parity value of
+                           DefaultParity will use the device's default parity value.
+  @param  DataBits         The number of data bits to use on the serial device. A DataBits
+                           value of 0 will use the device's default data bit setting.
+  @param  StopBits         The number of stop bits to use on this serial device. A StopBits
+                           value of DefaultStopBits will use the device's default number of
+                           stop bits.
+
+  @retval EFI_SUCCESS      The device was reset.
+  @retval EFI_DEVICE_ERROR The serial device could not be reset.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetAttributes (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN UINT64                 BaudRate,
+  IN UINT32                 ReceiveFifoDepth,
+  IN UINT32                 Timeout,
+  IN EFI_PARITY_TYPE        Parity,
+  IN UINT8                  DataBits,
+  IN EFI_STOP_BITS_TYPE     StopBits
+  )
+{
+  EFI_STATUS    Status;
+  EFI_TPL       Tpl;
+
+  Status = SerialPortSetAttributes (&BaudRate, &ReceiveFifoDepth, &Timeout, &Parity, &DataBits, &StopBits);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Set the Serial I/O mode and update the device path
+  //
+
+  Tpl = gBS->RaiseTPL (TPL_NOTIFY);
+
+  //
+  // Set the Serial I/O mode
+  //
+  This->Mode->ReceiveFifoDepth  = ReceiveFifoDepth;
+  This->Mode->Timeout           = Timeout;
+  This->Mode->BaudRate          = BaudRate;
+  This->Mode->DataBits          = (UINT32) DataBits;
+  This->Mode->Parity            = (UINT32) Parity;
+  This->Mode->StopBits          = (UINT32) StopBits;
+
+  //
+  // Check if the device path has actually changed
+  //
+  if (mSerialDevicePath.Uart.BaudRate == BaudRate &&
+      mSerialDevicePath.Uart.DataBits == DataBits &&
+      mSerialDevicePath.Uart.Parity   == (UINT8) Parity &&
+      mSerialDevicePath.Uart.StopBits == (UINT8) StopBits
+     ) {
+    gBS->RestoreTPL (Tpl);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Update the device path
+  //
+  mSerialDevicePath.Uart.BaudRate = BaudRate;
+  mSerialDevicePath.Uart.DataBits = DataBits;
+  mSerialDevicePath.Uart.Parity   = (UINT8) Parity;
+  mSerialDevicePath.Uart.StopBits = (UINT8) StopBits;
+
+  Status = gBS->ReinstallProtocolInterface (
+                  mSerialHandle,
+                  &gEfiDevicePathProtocolGuid,
+                  &mSerialDevicePath,
+                  &mSerialDevicePath
+                  );
+
+  gBS->RestoreTPL (Tpl);
+
+  return Status;
+}
+
+/**
+  Set the control bits on a serial device
+
+  @param  This             Protocol instance pointer.
+  @param  Control          Set the bits of Control that are settable.
+
+  @retval EFI_SUCCESS      The new control bits were set on the serial device.
+  @retval EFI_UNSUPPORTED  The serial device does not support this operation.
+  @retval EFI_DEVICE_ERROR The serial device is not functioning correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialSetControl (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN UINT32                 Control
+  )
+{
+  return SerialPortSetControl (Control);
+}
+
+/**
+  Retrieves the status of the control bits on a serial device
+
+  @param  This              Protocol instance pointer.
+  @param  Control           A pointer to return the current Control signals from the serial device.
+
+  @retval EFI_SUCCESS       The control bits were read from the serial device.
+  @retval EFI_DEVICE_ERROR  The serial device is not functioning correctly.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialGetControl (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  OUT UINT32                *Control
+  )
+{
+  return SerialPortGetControl (Control);
+}
+
+/**
+  Writes data to a serial device.
+
+  @param  This              Protocol instance pointer.
+  @param  BufferSize        On input, the size of the Buffer. On output, the amount of
+                            data actually written.
+  @param  Buffer            The buffer of data to write
+
+  @retval EFI_SUCCESS       The data was written.
+  @retval EFI_DEVICE_ERROR  The device reported an error.
+  @retval EFI_TIMEOUT       The data write was stopped due to a timeout.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialWrite (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN OUT UINTN              *BufferSize,
+  IN VOID                   *Buffer
+  )
+{
+  UINTN Count;
+
+  Count = SerialPortWrite (Buffer, *BufferSize);
+
+  if (Count != *BufferSize) {
+    *BufferSize = Count;
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Reads data from a serial device.
+
+  @param  This              Protocol instance pointer.
+  @param  BufferSize        On input, the size of the Buffer. On output, the amount of
+                            data returned in Buffer.
+  @param  Buffer            The buffer to return the data into.
+
+  @retval EFI_SUCCESS       The data was read.
+  @retval EFI_DEVICE_ERROR  The device reported an error.
+  @retval EFI_TIMEOUT       The data write was stopped due to a timeout.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialRead (
+  IN EFI_SERIAL_IO_PROTOCOL *This,
+  IN OUT UINTN              *BufferSize,
+  OUT VOID                  *Buffer
+  )
+{
+  UINTN Count;
+
+  Count = 0;
+
+  if (SerialPortPoll ()) {
+    Count = SerialPortRead (Buffer, *BufferSize);
+  }
+
+  if (Count != *BufferSize) {
+    *BufferSize = Count;
+    return EFI_TIMEOUT;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialization for the Serial Io Protocol.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+  @retval other             Some error occurs when executing this entry point.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialDxeInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+
+  Status = SerialPortInitialize ();
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  mSerialIoMode.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  mSerialIoMode.DataBits = (UINT32) PcdGet8 (PcdUartDefaultDataBits);
+  mSerialIoMode.Parity   = (UINT32) PcdGet8 (PcdUartDefaultParity);
+  mSerialIoMode.StopBits = (UINT32) PcdGet8 (PcdUartDefaultStopBits);
+  mSerialDevicePath.Uart.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
+  mSerialDevicePath.Uart.DataBits = PcdGet8 (PcdUartDefaultDataBits);
+  mSerialDevicePath.Uart.Parity   = PcdGet8 (PcdUartDefaultParity);
+  mSerialDevicePath.Uart.StopBits = PcdGet8 (PcdUartDefaultStopBits);
+
+  //
+  // Make a new handle with Serial IO protocol and its device path on it.
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mSerialHandle,
+                  &gEfiSerialIoProtocolGuid,   &mSerialIoTemplate,
+                  &gEfiDevicePathProtocolGuid, &mSerialDevicePath,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.c b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.c
new file mode 100644
index 0000000000..75dc421167
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.c
@@ -0,0 +1,3730 @@
+/** @file
+Utility functions for expression evaluation.
+
+Copyright (c) 2007 - 2016, 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 "Setup.h"
+
+//
+// Global stack used to evaluate boolean expresions
+//
+EFI_HII_VALUE *mOpCodeScopeStack = NULL;
+EFI_HII_VALUE *mOpCodeScopeStackEnd = NULL;
+EFI_HII_VALUE *mOpCodeScopeStackPointer = NULL;
+
+EFI_HII_VALUE *mExpressionEvaluationStack = NULL;
+EFI_HII_VALUE *mExpressionEvaluationStackEnd = NULL;
+EFI_HII_VALUE *mExpressionEvaluationStackPointer = NULL;
+UINTN         mExpressionEvaluationStackOffset = 0;
+
+EFI_HII_VALUE *mCurrentExpressionStack = NULL;
+EFI_HII_VALUE *mCurrentExpressionEnd = NULL;
+EFI_HII_VALUE *mCurrentExpressionPointer = NULL;
+
+EFI_HII_VALUE *mMapExpressionListStack = NULL;
+EFI_HII_VALUE *mMapExpressionListEnd = NULL;
+EFI_HII_VALUE *mMapExpressionListPointer = NULL;
+
+FORM_EXPRESSION   **mFormExpressionStack = NULL;
+FORM_EXPRESSION   **mFormExpressionEnd = NULL;
+FORM_EXPRESSION   **mFormExpressionPointer = NULL;
+
+FORM_EXPRESSION   **mStatementExpressionStack = NULL;
+FORM_EXPRESSION   **mStatementExpressionEnd = NULL;
+FORM_EXPRESSION   **mStatementExpressionPointer = NULL;
+
+FORM_EXPRESSION   **mOptionExpressionStack = NULL;
+FORM_EXPRESSION   **mOptionExpressionEnd = NULL;
+FORM_EXPRESSION   **mOptionExpressionPointer = NULL;
+
+
+//
+// Unicode collation protocol interface
+//
+EFI_UNICODE_COLLATION_PROTOCOL *mUnicodeCollation = NULL;
+EFI_USER_MANAGER_PROTOCOL      *mUserManager = NULL;
+
+/**
+  Grow size of the stack.
+
+  This is an internal function.
+
+  @param  Stack                  On input: old stack; On output: new stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack
+                                 pointer
+  @param  StackEnd               On input: old stack end; On output: new stack end
+
+  @retval EFI_SUCCESS            Grow stack success.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory for stack space.
+
+**/
+EFI_STATUS
+GrowStack (
+  IN OUT EFI_HII_VALUE  **Stack,
+  IN OUT EFI_HII_VALUE  **StackPtr,
+  IN OUT EFI_HII_VALUE  **StackEnd
+  )
+{
+  UINTN           Size;
+  EFI_HII_VALUE  *NewStack;
+
+  Size = EXPRESSION_STACK_SIZE_INCREMENT;
+  if (*StackPtr != NULL) {
+    Size = Size + (*StackEnd - *Stack);
+  }
+
+  NewStack = AllocatePool (Size * sizeof (EFI_HII_VALUE));
+  if (NewStack == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (*StackPtr != NULL) {
+    //
+    // Copy from Old Stack to the New Stack
+    //
+    CopyMem (
+      NewStack,
+      *Stack,
+      (*StackEnd - *Stack) * sizeof (EFI_HII_VALUE)
+      );
+
+    //
+    // Free The Old Stack
+    //
+    FreePool (*Stack);
+  }
+
+  //
+  // Make the Stack pointer point to the old data in the new stack
+  //
+  *StackPtr = NewStack + (*StackPtr - *Stack);
+  *Stack    = NewStack;
+  *StackEnd = NewStack + Size;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Push an element onto the Boolean Stack.
+
+  @param  Stack                  On input: old stack; On output: new stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack
+                                 pointer
+  @param  StackEnd               On input: old stack end; On output: new stack end
+  @param  Data                   Data to push.
+
+  @retval EFI_SUCCESS            Push stack success.
+
+**/
+EFI_STATUS
+PushStack (
+  IN OUT EFI_HII_VALUE       **Stack,
+  IN OUT EFI_HII_VALUE       **StackPtr,
+  IN OUT EFI_HII_VALUE       **StackEnd,
+  IN EFI_HII_VALUE           *Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check for a stack overflow condition
+  //
+  if (*StackPtr >= *StackEnd) {
+    //
+    // Grow the stack
+    //
+    Status = GrowStack (Stack, StackPtr, StackEnd);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Push the item onto the stack
+  //
+  CopyMem (*StackPtr, Data, sizeof (EFI_HII_VALUE));
+  if (Data->Type == EFI_IFR_TYPE_BUFFER) {
+    (*StackPtr)->Buffer = AllocateCopyPool(Data->BufferLen, Data->Buffer);
+    ASSERT ((*StackPtr)->Buffer != NULL);
+  }
+  
+  *StackPtr = *StackPtr + 1;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Pop an element from the stack.
+
+  @param  Stack                  On input: old stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack pointer
+  @param  Data                   Data to pop.
+
+  @retval EFI_SUCCESS            The value was popped onto the stack.
+  @retval EFI_ACCESS_DENIED      The pop operation underflowed the stack
+
+**/
+EFI_STATUS
+PopStack (
+  IN  EFI_HII_VALUE          *Stack,
+  IN OUT EFI_HII_VALUE       **StackPtr,
+  OUT EFI_HII_VALUE          *Data
+  )
+{
+  //
+  // Check for a stack underflow condition
+  //
+  if (*StackPtr == Stack) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Pop the item off the stack
+  //
+  *StackPtr = *StackPtr - 1;
+  CopyMem (Data, *StackPtr, sizeof (EFI_HII_VALUE));
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetCurrentExpressionStack (
+  VOID
+  )
+{
+  mCurrentExpressionPointer   = mCurrentExpressionStack;
+  mFormExpressionPointer      = mFormExpressionStack;
+  mStatementExpressionPointer = mStatementExpressionStack;
+  mOptionExpressionPointer    = mOptionExpressionStack;  
+}
+
+
+/**
+  Push current expression onto the Stack
+
+  @param  Pointer                Pointer to current expression.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushCurrentExpression (
+  IN VOID  *Pointer
+  )
+{
+  EFI_HII_VALUE  Data;
+
+  Data.Type = EFI_IFR_TYPE_NUM_SIZE_64;
+  Data.Value.u64 = (UINT64) (UINTN) Pointer;
+
+  return PushStack (
+    &mCurrentExpressionStack,
+    &mCurrentExpressionPointer,
+    &mCurrentExpressionEnd,
+    &Data
+    );
+}
+
+
+/**
+  Pop current expression from the Stack
+
+  @param  Pointer                Pointer to current expression to be pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopCurrentExpression (
+  OUT VOID    **Pointer
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Data;
+
+  Status = PopStack (
+    mCurrentExpressionStack,
+    &mCurrentExpressionPointer,
+    &Data
+    );
+
+  *Pointer = (VOID *) (UINTN) Data.Value.u64;
+
+  return Status;
+}
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetMapExpressionListStack (
+  VOID
+  )
+{
+  mMapExpressionListPointer = mMapExpressionListStack;
+}
+
+
+/**
+  Grow size of the stack.
+
+  This is an internal function.
+
+  @param  Stack                  On input: old stack; On output: new stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack
+                                 pointer
+  @param  StackEnd               On input: old stack end; On output: new stack end
+  @param  MemberSize             The stack member size.
+  
+  @retval EFI_SUCCESS            Grow stack success.
+  @retval EFI_OUT_OF_RESOURCES   No enough memory for stack space.
+
+**/
+EFI_STATUS
+GrowConditionalStack (
+  IN OUT FORM_EXPRESSION   ***Stack,
+  IN OUT FORM_EXPRESSION   ***StackPtr,
+  IN OUT FORM_EXPRESSION   ***StackEnd,
+  IN     UINTN             MemberSize
+  )
+{
+  UINTN             Size;
+  FORM_EXPRESSION   **NewStack;
+
+  Size = EXPRESSION_STACK_SIZE_INCREMENT;
+  if (*StackPtr != NULL) {
+    Size = Size + (*StackEnd - *Stack);
+  }
+
+  NewStack = AllocatePool (Size * MemberSize);
+  if (NewStack == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (*StackPtr != NULL) {
+    //
+    // Copy from Old Stack to the New Stack
+    //
+    CopyMem (
+      NewStack,
+      *Stack,
+      (*StackEnd - *Stack) * MemberSize
+      );
+
+    //
+    // Free The Old Stack
+    //
+    FreePool (*Stack);
+  }
+
+  //
+  // Make the Stack pointer point to the old data in the new stack
+  //
+  *StackPtr = NewStack + (*StackPtr - *Stack);
+  *Stack    = NewStack;
+  *StackEnd = NewStack + Size;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Push an element onto the Stack.
+
+  @param  Stack                  On input: old stack; On output: new stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack
+                                 pointer
+  @param  StackEnd               On input: old stack end; On output: new stack end
+  @param  Data                   Data to push.
+
+  @retval EFI_SUCCESS            Push stack success.
+
+**/
+EFI_STATUS
+PushConditionalStack (
+  IN OUT FORM_EXPRESSION   ***Stack,
+  IN OUT FORM_EXPRESSION   ***StackPtr,
+  IN OUT FORM_EXPRESSION   ***StackEnd,
+  IN     FORM_EXPRESSION   **Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Check for a stack overflow condition
+  //
+  if (*StackPtr >= *StackEnd) {
+    //
+    // Grow the stack
+    //
+    Status = GrowConditionalStack (Stack, StackPtr, StackEnd, sizeof (FORM_EXPRESSION *));
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Push the item onto the stack
+  //
+  CopyMem (*StackPtr, Data, sizeof (FORM_EXPRESSION *)); 
+  *StackPtr = *StackPtr + 1;
+
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Pop an element from the stack.
+
+  @param  Stack                  On input: old stack
+  @param  StackPtr               On input: old stack pointer; On output: new stack pointer
+  @param  Data                   Data to pop.
+
+  @retval EFI_SUCCESS            The value was popped onto the stack.
+  @retval EFI_ACCESS_DENIED      The pop operation underflowed the stack
+
+**/
+EFI_STATUS
+PopConditionalStack (
+  IN     FORM_EXPRESSION   **Stack,
+  IN OUT FORM_EXPRESSION   ***StackPtr,
+  OUT    FORM_EXPRESSION   **Data
+  )
+{
+  //
+  // Check for a stack underflow condition
+  //
+  if (*StackPtr == Stack) {
+    return EFI_ACCESS_DENIED;
+  }
+
+  //
+  // Pop the item off the stack
+  //
+  *StackPtr = *StackPtr - 1;
+  CopyMem (Data, *StackPtr, sizeof (FORM_EXPRESSION  *));
+  return EFI_SUCCESS;
+
+}
+
+/**
+  Get the expression list count.
+  
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval >=0                    The expression count
+  @retval -1                     Input parameter error.
+
+**/
+INTN 
+GetConditionalExpressionCount (
+  IN EXPRESS_LEVEL       Level
+  )
+{
+  switch (Level) {
+    case ExpressForm:
+      return mFormExpressionPointer - mFormExpressionStack;
+    case ExpressStatement:
+      return mStatementExpressionPointer - mStatementExpressionStack;
+    case ExpressOption:
+      return mOptionExpressionPointer - mOptionExpressionStack;
+    default:
+      ASSERT (FALSE);
+      return -1;
+  } 
+}
+
+/**
+  Get the expression Buffer pointer.
+  
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval  The start pointer of the expression buffer or NULL.
+
+**/
+FORM_EXPRESSION **
+GetConditionalExpressionList (
+  IN EXPRESS_LEVEL       Level
+  )
+{
+  switch (Level) {
+    case ExpressForm:
+      return mFormExpressionStack;
+    case ExpressStatement:
+      return mStatementExpressionStack;
+    case ExpressOption:
+      return mOptionExpressionStack;
+    default:
+      ASSERT (FALSE);
+      return NULL;
+  } 
+}
+
+
+/**
+  Push the expression options onto the Stack.
+
+  @param  Pointer                Pointer to the current expression.
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushConditionalExpression (
+  IN FORM_EXPRESSION   *Pointer,
+  IN EXPRESS_LEVEL     Level
+  )
+{
+  switch (Level) {
+    case ExpressForm:
+      return PushConditionalStack (
+        &mFormExpressionStack,
+        &mFormExpressionPointer,
+        &mFormExpressionEnd,
+        &Pointer
+        );
+    case ExpressStatement:
+      return PushConditionalStack (
+        &mStatementExpressionStack,
+        &mStatementExpressionPointer,
+        &mStatementExpressionEnd,
+        &Pointer
+        );
+    case ExpressOption:
+      return PushConditionalStack (
+        &mOptionExpressionStack,
+        &mOptionExpressionPointer,
+        &mOptionExpressionEnd,
+        &Pointer
+        );
+    default:
+      ASSERT (FALSE);
+      return EFI_INVALID_PARAMETER;
+  }
+}
+
+/**
+  Pop the expression options from the Stack
+
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopConditionalExpression (
+  IN  EXPRESS_LEVEL      Level
+  )
+{
+  FORM_EXPRESSION   *Pointer;
+
+  switch (Level) {
+    case ExpressForm:
+      return PopConditionalStack (
+        mFormExpressionStack,
+        &mFormExpressionPointer,
+        &Pointer
+      );
+
+    case ExpressStatement:
+      return PopConditionalStack (
+        mStatementExpressionStack,
+        &mStatementExpressionPointer,
+        &Pointer
+      );
+
+    case ExpressOption:
+      return PopConditionalStack (
+        mOptionExpressionStack,
+        &mOptionExpressionPointer,
+        &Pointer
+      );
+
+    default:
+      ASSERT (FALSE);
+      return EFI_INVALID_PARAMETER;
+  }
+}
+
+
+/**
+  Push the list of map expression onto the Stack
+
+  @param  Pointer                Pointer to the list of map expression to be pushed.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushMapExpressionList (
+  IN VOID  *Pointer
+  )
+{
+  EFI_HII_VALUE  Data;
+
+  Data.Type = EFI_IFR_TYPE_NUM_SIZE_64;
+  Data.Value.u64 = (UINT64) (UINTN) Pointer;
+
+  return PushStack (
+    &mMapExpressionListStack,
+    &mMapExpressionListPointer,
+    &mMapExpressionListEnd,
+    &Data
+    );
+}
+
+
+/**
+  Pop the list of map expression from the Stack
+
+  @param  Pointer                Pointer to the list of map expression to be pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopMapExpressionList (
+  OUT VOID    **Pointer
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Data;
+
+  Status = PopStack (
+    mMapExpressionListStack,
+    &mMapExpressionListPointer,
+    &Data
+    );
+
+  *Pointer = (VOID *) (UINTN) Data.Value.u64;
+
+  return Status;
+}
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetScopeStack (
+  VOID
+  )
+{
+  mOpCodeScopeStackPointer = mOpCodeScopeStack;
+}
+
+
+/**
+  Push an Operand onto the Stack
+
+  @param  Operand                Operand to push.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+
+**/
+EFI_STATUS
+PushScope (
+  IN UINT8   Operand
+  )
+{
+  EFI_HII_VALUE  Data;
+
+  Data.Type = EFI_IFR_TYPE_NUM_SIZE_8;
+  Data.Value.u8 = Operand;
+
+  return PushStack (
+           &mOpCodeScopeStack,
+           &mOpCodeScopeStackPointer,
+           &mOpCodeScopeStackEnd,
+           &Data
+           );
+}
+
+
+/**
+  Pop an Operand from the Stack
+
+  @param  Operand                Operand to pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+
+**/
+EFI_STATUS
+PopScope (
+  OUT UINT8     *Operand
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Data;
+
+  Status = PopStack (
+             mOpCodeScopeStack,
+             &mOpCodeScopeStackPointer,
+             &Data
+             );
+
+  *Operand = Data.Value.u8;
+
+  return Status;
+}
+
+
+/**
+  Push an Expression value onto the Stack
+
+  @param  Value                  Expression value to push.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+
+**/
+EFI_STATUS
+PushExpression (
+  IN EFI_HII_VALUE  *Value
+  )
+{
+  return PushStack (
+           &mExpressionEvaluationStack,
+           &mExpressionEvaluationStackPointer,
+           &mExpressionEvaluationStackEnd,
+           Value
+           );
+}
+
+
+/**
+  Pop an Expression value from the stack.
+
+  @param  Value                  Expression value to pop.
+
+  @retval EFI_SUCCESS            The value was popped onto the stack.
+  @retval EFI_ACCESS_DENIED      The pop operation underflowed the stack
+
+**/
+EFI_STATUS
+PopExpression (
+  OUT EFI_HII_VALUE  *Value
+  )
+{
+  return PopStack (
+           mExpressionEvaluationStack + mExpressionEvaluationStackOffset,
+           &mExpressionEvaluationStackPointer,
+           Value
+           );
+}
+
+/**
+  Get current stack offset from stack start.
+
+  @return Stack offset to stack start.
+**/
+UINTN
+SaveExpressionEvaluationStackOffset (
+  )
+{
+  UINTN TempStackOffset;
+  TempStackOffset = mExpressionEvaluationStackOffset;
+  mExpressionEvaluationStackOffset = mExpressionEvaluationStackPointer - mExpressionEvaluationStack;
+  return TempStackOffset;
+}
+
+/**
+  Restore stack offset based on input stack offset
+
+  @param  StackOffset  Offset to stack start.
+
+**/
+VOID
+RestoreExpressionEvaluationStackOffset (
+  UINTN StackOffset
+  )
+{
+  mExpressionEvaluationStackOffset = StackOffset;
+}
+
+/**
+  Get Form given its FormId.
+
+  @param  FormSet                The formset which contains this form.
+  @param  FormId                 Id of this form.
+
+  @retval Pointer                The form.
+  @retval NULL                   Specified Form is not found in the formset.
+
+**/
+FORM_BROWSER_FORM *
+IdToForm (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT16                FormId
+  )
+{
+  LIST_ENTRY         *Link;
+  FORM_BROWSER_FORM  *Form;
+
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+    if (Form->FormId == FormId) {
+      return Form;
+    }
+
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Search a Question in Form scope using its QuestionId.
+
+  @param  Form                   The form which contains this Question.
+  @param  QuestionId             Id of this Question.
+
+  @retval Pointer                The Question.
+  @retval NULL                   Specified Question not found in the form.
+
+**/
+FORM_BROWSER_STATEMENT *
+IdToQuestion2 (
+  IN FORM_BROWSER_FORM  *Form,
+  IN UINT16             QuestionId
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Question;
+
+  if (QuestionId == 0 || Form == NULL) {
+    //
+    // The value of zero is reserved
+    //
+    return NULL;
+  }
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+
+    if (Question->QuestionId == QuestionId) {
+      return Question;
+    }
+
+    Link = GetNextNode (&Form->StatementListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Search a Question in Formset scope using its QuestionId.
+
+  @param  FormSet                The formset which contains this form.
+  @param  Form                   The form which contains this Question.
+  @param  QuestionId             Id of this Question.
+
+  @retval Pointer                The Question.
+  @retval NULL                   Specified Question not found in the form.
+
+**/
+FORM_BROWSER_STATEMENT *
+IdToQuestion (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN FORM_BROWSER_FORM     *Form,
+  IN UINT16                QuestionId
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Question;
+
+  //
+  // Search in the form scope first
+  //
+  Question = IdToQuestion2 (Form, QuestionId);
+  if (Question != NULL) {
+    return Question;
+  }
+
+  //
+  // Search in the formset scope
+  //
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+    Question = IdToQuestion2 (Form, QuestionId);
+    if (Question != NULL) {
+      //
+      // EFI variable storage may be updated by Callback() asynchronous,
+      // to keep synchronous, always reload the Question Value.
+      //
+      if (Question->Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+        GetQuestionValue (FormSet, Form, Question, GetSetValueWithHiiDriver);
+      }
+
+      return Question;
+    }
+
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Get Expression given its RuleId.
+
+  @param  Form                   The form which contains this Expression.
+  @param  RuleId                 Id of this Expression.
+
+  @retval Pointer                The Expression.
+  @retval NULL                   Specified Expression not found in the form.
+
+**/
+FORM_EXPRESSION *
+RuleIdToExpression (
+  IN FORM_BROWSER_FORM  *Form,
+  IN UINT8              RuleId
+  )
+{
+  LIST_ENTRY       *Link;
+  FORM_EXPRESSION  *Expression;
+
+  Link = GetFirstNode (&Form->ExpressionListHead);
+  while (!IsNull (&Form->ExpressionListHead, Link)) {
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+
+    if (Expression->Type == EFI_HII_EXPRESSION_RULE && Expression->RuleId == RuleId) {
+      return Expression;
+    }
+
+    Link = GetNextNode (&Form->ExpressionListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Locate the Unicode Collation Protocol interface for later use.
+
+  @retval EFI_SUCCESS            Protocol interface initialize success.
+  @retval Other                  Protocol interface initialize failed.
+
+**/
+EFI_STATUS
+InitializeUnicodeCollationProtocol (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  if (mUnicodeCollation != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // BUGBUG: Proper impelmentation is to locate all Unicode Collation Protocol
+  // instances first and then select one which support English language.
+  // Current implementation just pick the first instance.
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiUnicodeCollation2ProtocolGuid,
+                  NULL,
+                  (VOID **) &mUnicodeCollation
+                  );
+  return Status;
+}
+
+/**
+  Convert the input Unicode character to upper.
+
+  @param String  Th Unicode character to be converted.
+
+**/
+VOID
+IfrStrToUpper (
+  IN CHAR16                   *String
+  )
+{
+  while (*String != 0) {
+    if ((*String >= 'a') && (*String <= 'z')) {
+      *String = (UINT16) ((*String) & ((UINT16) ~0x20));
+    }
+    String++;
+  }
+}
+
+/**
+  Check whether this value type can be transfer to EFI_IFR_TYPE_BUFFER type.
+
+  EFI_IFR_TYPE_REF, EFI_IFR_TYPE_DATE and EFI_IFR_TYPE_TIME are converted to 
+  EFI_IFR_TYPE_BUFFER when do the value compare.
+
+  @param  Value                  Expression value to compare on.
+
+  @retval TRUE                   This value type can be transter to EFI_IFR_TYPE_BUFFER type.
+  @retval FALSE                  This value type can't be transter to EFI_IFR_TYPE_BUFFER type.
+
+**/
+BOOLEAN
+IsTypeInBuffer (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_BUFFER:
+  case EFI_IFR_TYPE_DATE:
+  case EFI_IFR_TYPE_TIME:
+  case EFI_IFR_TYPE_REF:
+    return TRUE;
+
+  default:
+    return FALSE;
+  }
+}
+
+/**
+  Check whether this value type can be transfer to EFI_IFR_TYPE_UINT64
+
+  @param  Value                  Expression value to compare on.
+
+  @retval TRUE                   This value type can be transter to EFI_IFR_TYPE_BUFFER type.
+  @retval FALSE                  This value type can't be transter to EFI_IFR_TYPE_BUFFER type.
+
+**/
+BOOLEAN
+IsTypeInUINT64 (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+  case EFI_IFR_TYPE_BOOLEAN:
+    return TRUE;
+
+  default:
+    return FALSE;
+  }
+}
+
+/**
+  Return the buffer length for this value.
+
+  EFI_IFR_TYPE_REF, EFI_IFR_TYPE_DATE and EFI_IFR_TYPE_TIME are converted to 
+  EFI_IFR_TYPE_BUFFER when do the value compare.
+
+  @param   Value                  Expression value to compare on.
+  
+  @retval  BufLen                 Return the buffer length.
+
+**/
+UINT16
+GetLengthForValue (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_BUFFER:
+    return Value->BufferLen;
+
+  case EFI_IFR_TYPE_DATE:
+    return (UINT16) sizeof (EFI_HII_DATE);
+
+  case EFI_IFR_TYPE_TIME:
+    return (UINT16) sizeof (EFI_HII_TIME);
+
+  case EFI_IFR_TYPE_REF:
+    return (UINT16) sizeof (EFI_HII_REF);
+
+  default:
+    return 0;
+  }
+}
+
+/**
+  Return the buffer pointer for this value.
+
+  EFI_IFR_TYPE_REF, EFI_IFR_TYPE_DATE and EFI_IFR_TYPE_TIME are converted to 
+  EFI_IFR_TYPE_BUFFER when do the value compare.
+
+  @param  Value                  Expression value to compare on.
+
+  @retval Buf                    Return the buffer pointer.
+
+**/
+UINT8 *
+GetBufferForValue (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_BUFFER:
+    return Value->Buffer;
+
+  case EFI_IFR_TYPE_DATE:
+    return (UINT8 *) (&Value->Value.date);
+
+  case EFI_IFR_TYPE_TIME:
+    return (UINT8 *) (&Value->Value.time);
+
+  case EFI_IFR_TYPE_REF:
+    return (UINT8 *) (&Value->Value.ref);
+
+  default:
+    return NULL;
+  }
+}
+
+/**
+  Evaluate opcode EFI_IFR_TO_STRING.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Format                 String format in EFI_IFR_TO_STRING.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrToString (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT8                 Format,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value;
+  CHAR16         *String;
+  CHAR16         *PrintFormat;
+  CHAR16         Buffer[MAXIMUM_VALUE_CHARACTERS];
+  UINT8          *TmpBuf;
+  UINT8          *SrcBuf;
+  UINTN          SrcLen;
+  UINTN          BufferSize;
+
+  Status = PopExpression (&Value);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  switch (Value.Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    BufferSize = MAXIMUM_VALUE_CHARACTERS * sizeof (CHAR16);
+    switch (Format) {
+    case EFI_IFR_STRING_UNSIGNED_DEC:
+    case EFI_IFR_STRING_SIGNED_DEC:
+      PrintFormat = L"%ld";
+      break;
+
+    case EFI_IFR_STRING_LOWERCASE_HEX:
+      PrintFormat = L"%lx";
+      break;
+
+    case EFI_IFR_STRING_UPPERCASE_HEX:
+      PrintFormat = L"%lX";
+      break;
+
+    default:
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      return EFI_SUCCESS;
+    }
+    UnicodeSPrint (Buffer, BufferSize, PrintFormat, Value.Value.u64);
+    String = Buffer;
+    break;
+
+  case EFI_IFR_TYPE_STRING:
+    CopyMem (Result, &Value, sizeof (EFI_HII_VALUE));
+    return EFI_SUCCESS;
+
+  case EFI_IFR_TYPE_BOOLEAN:
+    String = (Value.Value.b) ? L"True" : L"False";
+    break;
+    
+  case EFI_IFR_TYPE_BUFFER:
+  case EFI_IFR_TYPE_DATE:
+  case EFI_IFR_TYPE_TIME:
+  case EFI_IFR_TYPE_REF:
+    //
+    // + 3 is base on the unicode format, the length may be odd number, 
+    // so need 1 byte to align, also need 2 bytes for L'\0'.
+    //
+    if (Value.Type == EFI_IFR_TYPE_BUFFER) {
+      SrcLen = Value.BufferLen;
+      SrcBuf = Value.Buffer;
+    } else {
+      SrcBuf = GetBufferForValue(&Value);
+      SrcLen = GetLengthForValue(&Value);
+    }
+
+    TmpBuf = AllocateZeroPool (SrcLen + 3);
+    ASSERT (TmpBuf != NULL);
+    if (Format == EFI_IFR_STRING_ASCII) {
+      CopyMem (TmpBuf, SrcBuf, SrcLen);
+      PrintFormat = L"%a"; 
+    } else {
+      // Format == EFI_IFR_STRING_UNICODE
+      CopyMem (TmpBuf, SrcBuf, SrcLen * sizeof (CHAR16));
+      PrintFormat = L"%s";  
+    }
+    UnicodeSPrint (Buffer, sizeof (Buffer), PrintFormat, TmpBuf);
+    String = Buffer; 
+    FreePool (TmpBuf);
+    if (Value.Type == EFI_IFR_TYPE_BUFFER) {
+      FreePool (Value.Buffer);
+    }
+    break;
+    
+  default:
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+
+  Result->Type = EFI_IFR_TYPE_STRING;
+  Result->Value.string = NewString (String, FormSet->HiiHandle);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_TO_UINT.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrToUint (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value;
+  CHAR16         *String;
+  CHAR16         *StringPtr;
+
+  Status = PopExpression (&Value);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (Value.Type >= EFI_IFR_TYPE_OTHER && !IsTypeInBuffer(&Value)) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+
+  Status = EFI_SUCCESS;
+  if (Value.Type == EFI_IFR_TYPE_STRING) {
+    String = GetToken (Value.Value.string, FormSet->HiiHandle);
+    if (String == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    IfrStrToUpper (String);
+    StringPtr = StrStr (String, L"0X");
+    if (StringPtr != NULL) {
+      //
+      // Hex string
+      //
+      Result->Value.u64 = StrHexToUint64 (String);
+    } else {
+      //
+      // decimal string
+      //
+      Result->Value.u64 = StrDecimalToUint64 (String);
+    }
+    FreePool (String);
+  } else if (IsTypeInBuffer(&Value)) {
+    if (GetLengthForValue (&Value) > 8) {
+      if (Value.Type == EFI_IFR_TYPE_BUFFER) {
+        FreePool (Value.Buffer);
+      }
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      return EFI_SUCCESS;
+    }
+    Result->Value.u64 = *(UINT64*) GetBufferForValue (&Value);
+    if (Value.Type == EFI_IFR_TYPE_BUFFER) {
+      FreePool (Value.Buffer);
+    }
+  } else {
+    CopyMem (Result, &Value, sizeof (EFI_HII_VALUE));
+  }
+
+  Result->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+  return Status;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_CATENATE.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrCatenate (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[2];
+  CHAR16         *String[2];
+  UINTN          Index;
+  CHAR16         *StringPtr;
+  UINTN          Size;
+  UINT16         Length0;
+  UINT16         Length1;
+  UINT8          *TmpBuf;
+  UINTN          MaxLen;
+
+  //
+  // String[0] - The second string
+  // String[1] - The first string
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  StringPtr = NULL;
+  Status = EFI_SUCCESS;
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index].Type != EFI_IFR_TYPE_STRING && !IsTypeInBuffer(&Value[Index])) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    if (Value[Index].Type == EFI_IFR_TYPE_STRING) {
+      String[Index] = GetToken (Value[Index].Value.string, FormSet->HiiHandle);
+      if (String[Index] == NULL) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+    }
+  }
+
+  if (Value[0].Type == EFI_IFR_TYPE_STRING) {
+    Size = StrSize (String[0]);
+    MaxLen = (StrSize (String[1]) + Size) / sizeof (CHAR16);
+    StringPtr= AllocatePool (MaxLen * sizeof (CHAR16));
+    ASSERT (StringPtr != NULL);
+    StrCpyS (StringPtr, MaxLen, String[1]);
+    StrCatS (StringPtr, MaxLen, String[0]);
+
+    Result->Type = EFI_IFR_TYPE_STRING;
+    Result->Value.string = NewString (StringPtr, FormSet->HiiHandle);
+  } else {
+    Result->Type = EFI_IFR_TYPE_BUFFER;
+    Length0 = GetLengthForValue(&Value[0]);
+    Length1 = GetLengthForValue(&Value[1]);
+    Result->BufferLen = (UINT16) (Length0 + Length1);
+
+    Result->Buffer = AllocateZeroPool (Result->BufferLen);
+    ASSERT (Result->Buffer != NULL);
+
+    TmpBuf = GetBufferForValue(&Value[0]);
+    ASSERT (TmpBuf != NULL);
+    CopyMem (Result->Buffer, TmpBuf, Length0);
+    TmpBuf = GetBufferForValue(&Value[1]);
+    ASSERT (TmpBuf != NULL);
+    CopyMem (&Result->Buffer[Length0], TmpBuf, Length1);
+  }
+Done:
+  if (Value[0].Buffer != NULL) {
+    FreePool (Value[0].Buffer);
+  }
+  if (Value[1].Buffer != NULL) {
+    FreePool (Value[1].Buffer);
+  }
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+  if (StringPtr != NULL) {
+    FreePool (StringPtr);
+  }
+
+  return Status;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_MATCH.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrMatch (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[2];
+  CHAR16         *String[2];
+  UINTN          Index;
+
+  //
+  // String[0] - The string to search
+  // String[1] - pattern
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  Status = EFI_SUCCESS;
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index].Type != EFI_IFR_TYPE_STRING) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    String[Index] = GetToken (Value[Index].Value.string, FormSet->HiiHandle);
+    if (String [Index] == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+  }
+
+  Result->Type = EFI_IFR_TYPE_BOOLEAN;
+  Result->Value.b = mUnicodeCollation->MetaiMatch (mUnicodeCollation, String[0], String[1]);
+
+Done:
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+
+  return Status;
+}
+
+/**
+  Evaluate opcode EFI_IFR_MATCH2.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  SyntaxType             Syntax type for match2.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrMatch2 (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN EFI_GUID              *SyntaxType,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_HII_VALUE                    Value[2];
+  CHAR16                           *String[2];
+  UINTN                            Index;
+  UINTN                            GuidIndex;
+  EFI_HANDLE                       *HandleBuffer;
+  UINTN                            BufferSize;
+  EFI_REGULAR_EXPRESSION_PROTOCOL  *RegularExpressionProtocol;
+  UINTN                            RegExSyntaxTypeListSize;
+  EFI_REGEX_SYNTAX_TYPE            *RegExSyntaxTypeList;
+  UINTN                            CapturesCount;
+
+  //
+  // String[0] - The string to search
+  // String[1] - pattern
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  HandleBuffer = NULL;
+  RegExSyntaxTypeList = NULL;
+  Status = EFI_SUCCESS;
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index].Type != EFI_IFR_TYPE_STRING) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    String[Index] = GetToken (Value[Index].Value.string, FormSet->HiiHandle);
+    if (String [Index] == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+  }
+
+  BufferSize    = 0;
+  HandleBuffer  = NULL;
+  Status = gBS->LocateHandle(
+                      ByProtocol,
+                      &gEfiRegularExpressionProtocolGuid,
+                      NULL,
+                      &BufferSize,
+                      HandleBuffer);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    HandleBuffer = AllocateZeroPool(BufferSize);
+    if (HandleBuffer == NULL) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    Status = gBS->LocateHandle(
+                        ByProtocol,
+                        &gEfiRegularExpressionProtocolGuid,
+                        NULL,
+                        &BufferSize,
+                        HandleBuffer);
+
+  }
+
+  if (EFI_ERROR (Status)) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  ASSERT (HandleBuffer != NULL);
+  for ( Index = 0; Index < BufferSize / sizeof(EFI_HANDLE); Index ++) {
+    Status = gBS->HandleProtocol (
+                  HandleBuffer[Index],
+                  &gEfiRegularExpressionProtocolGuid,
+                  (VOID**)&RegularExpressionProtocol
+                 );
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    RegExSyntaxTypeListSize = 0;
+    RegExSyntaxTypeList = NULL;
+
+    Status = RegularExpressionProtocol->GetInfo (
+                                          RegularExpressionProtocol, 
+                                          &RegExSyntaxTypeListSize, 
+                                          RegExSyntaxTypeList
+                                          );
+    if (Status == EFI_BUFFER_TOO_SMALL) {
+      RegExSyntaxTypeList = AllocateZeroPool(RegExSyntaxTypeListSize);
+      if (RegExSyntaxTypeList == NULL) {
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      Status = RegularExpressionProtocol->GetInfo (
+                                            RegularExpressionProtocol, 
+                                            &RegExSyntaxTypeListSize, 
+                                            RegExSyntaxTypeList
+                                            );
+    } else if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    for (GuidIndex = 0; GuidIndex < RegExSyntaxTypeListSize / sizeof(EFI_GUID); GuidIndex++) {
+      if (CompareGuid (&RegExSyntaxTypeList[GuidIndex], SyntaxType)) {
+        //
+        // Find the match type, return the value.
+        //
+        Result->Type = EFI_IFR_TYPE_BOOLEAN;
+        Status = RegularExpressionProtocol->MatchString (
+                                              RegularExpressionProtocol, 
+                                              String[0],
+                                              String[1],
+                                              SyntaxType,
+                                              &Result->Value.b,
+                                              NULL,
+                                              &CapturesCount
+                                              );
+        goto Done;
+      }
+    }
+
+    if (RegExSyntaxTypeList != NULL) {
+      FreePool (RegExSyntaxTypeList);
+    }
+  }
+
+  //
+  // Type specified by SyntaxType is not supported 
+  // in any of the EFI_REGULAR_EXPRESSION_PROTOCOL instances.
+  //
+  Result->Type = EFI_IFR_TYPE_UNDEFINED;
+  Status = EFI_SUCCESS;
+
+Done:
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+  if (RegExSyntaxTypeList != NULL) {
+    FreePool (RegExSyntaxTypeList);
+  }
+  if (HandleBuffer != NULL) {
+    FreePool (HandleBuffer);
+  }
+  return Status;
+}
+
+/**
+  Evaluate opcode EFI_IFR_FIND.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Format                 Case sensitive or insensitive.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrFind (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT8                 Format,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[3];
+  CHAR16         *String[2];
+  UINTN          Base;
+  CHAR16         *StringPtr;
+  UINTN          Index;
+
+  ZeroMem (Value, sizeof (Value));
+
+  if (Format > EFI_IFR_FF_CASE_INSENSITIVE) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[2]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }  
+
+  if (Value[0].Type > EFI_IFR_TYPE_NUM_SIZE_64) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  Base = (UINTN) Value[0].Value.u64;
+
+  //
+  // String[0] - sub-string
+  // String[1] - The string to search
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index + 1].Type != EFI_IFR_TYPE_STRING) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    String[Index] = GetToken (Value[Index + 1].Value.string, FormSet->HiiHandle);
+    if (String[Index] == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+
+    if (Format == EFI_IFR_FF_CASE_INSENSITIVE) {
+      //
+      // Case insensitive, convert both string to upper case
+      //
+      IfrStrToUpper (String[Index]);
+    }
+  }
+
+  Result->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+  if (Base >= StrLen (String[1])) {
+    Result->Value.u64 = 0xFFFFFFFFFFFFFFFFULL;
+  } else {
+    StringPtr = StrStr (String[1] + Base, String[0]);
+    Result->Value.u64 = (StringPtr == NULL) ? 0xFFFFFFFFFFFFFFFFULL : (StringPtr - String[1]);
+  }
+
+Done:
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+
+  return Status;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_MID.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrMid (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[3];
+  CHAR16         *String;
+  UINTN          Base;
+  UINTN          Length;
+  CHAR16         *SubString;
+  UINT16         BufferLen;
+  UINT8          *Buffer;
+
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[2]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } 
+
+  if (Value[0].Type > EFI_IFR_TYPE_NUM_SIZE_64) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  Length = (UINTN) Value[0].Value.u64;
+
+  if (Value[1].Type > EFI_IFR_TYPE_NUM_SIZE_64) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  Base = (UINTN) Value[1].Value.u64;
+
+  if (Value[2].Type != EFI_IFR_TYPE_STRING && !IsTypeInBuffer(&Value[2])) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  if (Value[2].Type == EFI_IFR_TYPE_STRING) {
+    String = GetToken (Value[2].Value.string, FormSet->HiiHandle);
+    if (String == NULL) {
+      return EFI_NOT_FOUND;
+    }
+
+    if (Length == 0 || Base >= StrLen (String)) {
+      SubString = gEmptyString;
+    } else {
+      SubString = String + Base;
+      if ((Base + Length) < StrLen (String)) {
+        SubString[Length] = L'\0';
+      }
+    }
+
+    Result->Type = EFI_IFR_TYPE_STRING;
+    Result->Value.string = NewString (SubString, FormSet->HiiHandle);
+
+    FreePool (String);
+  } else {
+    BufferLen = GetLengthForValue (&Value[2]);
+    Buffer = GetBufferForValue (&Value[2]);
+
+    Result->Type = EFI_IFR_TYPE_BUFFER;
+    if (Length == 0 || Base >= BufferLen) {
+      Result->BufferLen = 0;
+      Result->Buffer = NULL;
+    } else {
+      Result->BufferLen = (UINT16)((BufferLen - Base) < Length ? (BufferLen - Base) : Length);    
+      Result->Buffer = AllocateZeroPool (Result->BufferLen);
+      ASSERT (Result->Buffer != NULL);
+      CopyMem (Result->Buffer, &Buffer[Base], Result->BufferLen);
+    }
+
+    if (Value[2].Type == EFI_IFR_TYPE_BUFFER) {
+      FreePool (Value[2].Buffer);
+    }
+  }
+  
+  return Status;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_TOKEN.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrToken (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[3];
+  CHAR16         *String[2];
+  UINTN          Count;
+  CHAR16         *Delimiter;
+  CHAR16         *SubString;
+  CHAR16         *StringPtr;
+  UINTN          Index;
+
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[2]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } 
+
+  if (Value[0].Type > EFI_IFR_TYPE_NUM_SIZE_64) { 
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  Count = (UINTN) Value[0].Value.u64;
+
+  //
+  // String[0] - Delimiter
+  // String[1] - The string to search
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index + 1].Type != EFI_IFR_TYPE_STRING) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;   
+      goto Done;
+    }
+
+    String[Index] = GetToken (Value[Index + 1].Value.string, FormSet->HiiHandle);
+    if (String[Index] == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+  }
+
+  Delimiter = String[0];
+  SubString = String[1];
+  while (Count > 0) {
+    SubString = StrStr (SubString, Delimiter);
+    if (SubString != NULL) {
+      //
+      // Skip over the delimiter
+      //
+      SubString = SubString + StrLen (Delimiter);
+    } else {
+      break;
+    }
+    Count--;
+  }
+
+  if (SubString == NULL) {
+    //
+    // nth delimited sub-string not found, push an empty string
+    //
+    SubString = gEmptyString;
+  } else {
+    //
+    // Put a NULL terminator for nth delimited sub-string
+    //
+    StringPtr = StrStr (SubString, Delimiter);
+    if (StringPtr != NULL) {
+      *StringPtr = L'\0';
+    }
+  }
+
+  Result->Type = EFI_IFR_TYPE_STRING;
+  Result->Value.string = NewString (SubString, FormSet->HiiHandle);
+
+Done:
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+
+  return Status;
+}
+
+
+/**
+  Evaluate opcode EFI_IFR_SPAN.
+
+  @param  FormSet                Formset which contains this opcode.
+  @param  Flags                  FIRST_MATCHING or FIRST_NON_MATCHING.
+  @param  Result                 Evaluation result for this opcode.
+
+  @retval EFI_SUCCESS            Opcode evaluation success.
+  @retval Other                  Opcode evaluation failed.
+
+**/
+EFI_STATUS
+IfrSpan (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT8                 Flags,
+  OUT  EFI_HII_VALUE       *Result
+  )
+{
+  EFI_STATUS     Status;
+  EFI_HII_VALUE  Value[3];
+  CHAR16         *String[2];
+  CHAR16         *Charset;
+  UINTN          Base;
+  UINTN          Index;
+  CHAR16         *StringPtr;
+  BOOLEAN        Found;
+
+  ZeroMem (Value, sizeof (Value));
+
+  Status = PopExpression (&Value[0]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[1]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = PopExpression (&Value[2]);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  } 
+
+  if (Value[0].Type > EFI_IFR_TYPE_NUM_SIZE_64) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    return EFI_SUCCESS;
+  }
+  Base = (UINTN) Value[0].Value.u64;
+
+  //
+  // String[0] - Charset
+  // String[1] - The string to search
+  //
+  String[0] = NULL;
+  String[1] = NULL;
+  for (Index = 0; Index < 2; Index++) {
+    if (Value[Index + 1].Type != EFI_IFR_TYPE_STRING) {
+      Result->Type = EFI_IFR_TYPE_UNDEFINED;
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    String[Index] = GetToken (Value[Index + 1].Value.string, FormSet->HiiHandle);
+    if (String [Index] == NULL) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+  }
+
+  if (Base >= StrLen (String[1])) {
+    Result->Type = EFI_IFR_TYPE_UNDEFINED;
+    Status = EFI_SUCCESS;
+    goto Done;
+  }
+
+  Found = FALSE;
+  StringPtr = String[1] + Base;
+  Charset = String[0];
+  while (*StringPtr != 0 && !Found) {
+    Index = 0;
+    while (Charset[Index] != 0) {
+      if (*StringPtr >= Charset[Index] && *StringPtr <= Charset[Index + 1]) {
+        if (Flags == EFI_IFR_FLAGS_FIRST_MATCHING) {
+          Found = TRUE;
+          break;
+        }
+      } else {
+        if (Flags == EFI_IFR_FLAGS_FIRST_NON_MATCHING) {
+          Found = TRUE;
+          break;
+        }
+      }
+      //
+      // Skip characters pair representing low-end of a range and high-end of a range
+      //
+      Index += 2;
+    }
+
+    if (!Found) {
+      StringPtr++;
+    }
+  }
+
+  Result->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+  Result->Value.u64 = StringPtr - String[1];
+
+Done:
+  if (String[0] != NULL) {
+    FreePool (String[0]);
+  }
+  if (String[1] != NULL) {
+    FreePool (String[1]);
+  }
+
+  return Status;
+}
+
+
+/**
+  Zero extend integer/boolean/date/time to UINT64 for comparing.
+
+  @param  Value                  HII Value to be converted.
+
+**/
+VOID
+ExtendValueToU64 (
+  IN  EFI_HII_VALUE   *Value
+  )
+{
+  UINT64  Temp;
+
+  Temp = 0;
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    Temp = Value->Value.u8;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    Temp = Value->Value.u16;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    Temp = Value->Value.u32;
+    break;
+
+  case EFI_IFR_TYPE_BOOLEAN:
+    Temp = Value->Value.b;
+    break;
+
+  case EFI_IFR_TYPE_TIME:
+    Temp = Value->Value.u32 & 0xffffff;
+    break;
+
+  case EFI_IFR_TYPE_DATE:
+    Temp = Value->Value.u32;
+    break;
+
+  default:
+    return;
+  }
+
+  Value->Value.u64 = Temp;
+}
+
+/**
+  Get UINT64 type value.
+
+  @param  Value                  Input Hii value.
+
+  @retval UINT64                 Return the UINT64 type value.
+
+**/
+UINT64
+HiiValueToUINT64 (
+  IN EFI_HII_VALUE      *Value
+  )
+{
+  UINT64  RetVal;
+
+  RetVal = 0;
+
+  switch (Value->Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    RetVal = Value->Value.u8;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    RetVal = Value->Value.u16;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    RetVal = Value->Value.u32;
+    break;
+
+  case EFI_IFR_TYPE_BOOLEAN:
+    RetVal = Value->Value.b;
+    break;
+
+  case EFI_IFR_TYPE_DATE:
+    RetVal = *(UINT64*) &Value->Value.date;
+    break;
+
+  case EFI_IFR_TYPE_TIME:
+    RetVal = (*(UINT64*) &Value->Value.time) & 0xffffff;
+    break;
+
+  default:
+    RetVal = Value->Value.u64;
+    break;
+  }
+
+  return RetVal;
+}
+
+/**
+  Compare two Hii value.
+
+  @param  Value1                 Expression value to compare on left-hand.
+  @param  Value2                 Expression value to compare on right-hand.
+  @param  Result                 Return value after compare.
+                                 retval 0                      Two operators equal.
+                                 return Positive value if Value1 is greater than Value2.
+                                 retval Negative value if Value1 is less than Value2.
+  @param  HiiHandle              Only required for string compare.
+
+  @retval other                  Could not perform compare on two values.
+  @retval EFI_SUCCESS            Compare the value success.
+
+**/
+EFI_STATUS
+CompareHiiValue (
+  IN  EFI_HII_VALUE   *Value1,
+  IN  EFI_HII_VALUE   *Value2,
+  OUT INTN            *Result,
+  IN  EFI_HII_HANDLE  HiiHandle OPTIONAL
+  )
+{
+  INT64   Temp64;
+  CHAR16  *Str1;
+  CHAR16  *Str2;
+  UINTN   Len;
+  UINT8   *Buf1;
+  UINT16  Buf1Len;
+  UINT8   *Buf2;
+  UINT16  Buf2Len;
+
+  if (Value1->Type == EFI_IFR_TYPE_STRING && Value2->Type == EFI_IFR_TYPE_STRING) {
+    if (Value1->Value.string == 0 || Value2->Value.string == 0) {
+      //
+      // StringId 0 is reserved
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+
+    if (Value1->Value.string == Value2->Value.string) {
+      *Result = 0;
+      return EFI_SUCCESS;
+    }
+
+    Str1 = GetToken (Value1->Value.string, HiiHandle);
+    if (Str1 == NULL) {
+      //
+      // String not found
+      //
+      return EFI_NOT_FOUND;
+    }
+
+    Str2 = GetToken (Value2->Value.string, HiiHandle);
+    if (Str2 == NULL) {
+      FreePool (Str1);
+      return EFI_NOT_FOUND;
+    }
+
+    *Result = StrCmp (Str1, Str2);
+
+    FreePool (Str1);
+    FreePool (Str2);
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Take types(date, time, ref, buffer) as buffer
+  //
+  if (IsTypeInBuffer(Value1) && IsTypeInBuffer(Value2)) {
+    Buf1    = GetBufferForValue(Value1);
+    Buf1Len = GetLengthForValue(Value1);
+    Buf2    = GetBufferForValue(Value2);
+    Buf2Len = GetLengthForValue(Value2);
+    
+    Len = Buf1Len > Buf2Len ? Buf2Len : Buf1Len;
+    *Result = CompareMem (Buf1, Buf2, Len);
+    if ((*Result == 0) && (Buf1Len != Buf2Len)) {
+      //
+      // In this case, means base on samll number buffer, the data is same
+      // So which value has more data, which value is bigger.
+      //
+      *Result = Buf1Len > Buf2Len ? 1 : -1;
+    }
+    return EFI_SUCCESS;
+  }  
+
+  //
+  // Take types(integer, boolean) as integer
+  //
+  if (IsTypeInUINT64(Value1) && IsTypeInUINT64(Value2)) {
+    Temp64 = HiiValueToUINT64(Value1) - HiiValueToUINT64(Value2);
+    if (Temp64 > 0) {
+      *Result = 1;
+    } else if (Temp64 < 0) {
+      *Result = -1;
+    } else {
+      *Result = 0;
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Check if current user has the privilege specified by the permissions GUID.
+
+  @param[in] Guid  A GUID specifying setup access permissions.
+
+  @retval TRUE     Current user has the privilege.
+  @retval FALSE    Current user does not have the privilege.
+**/
+BOOLEAN
+CheckUserPrivilege (
+  IN EFI_GUID *Guid
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_USER_PROFILE_HANDLE      UserProfileHandle;
+  EFI_USER_INFO_HANDLE         UserInfoHandle;
+  EFI_USER_INFO                *UserInfo;
+  EFI_GUID                     *UserPermissionsGuid;
+  UINTN                        UserInfoSize;
+  UINTN                        AccessControlDataSize;
+  EFI_USER_INFO_ACCESS_CONTROL *AccessControl;
+  UINTN                        RemainSize;
+
+  if (mUserManager == NULL) {
+    Status = gBS->LocateProtocol (
+                    &gEfiUserManagerProtocolGuid,
+                    NULL,
+                    (VOID **) &mUserManager
+                    );
+    if (EFI_ERROR (Status)) {
+      ///
+      /// If the system does not support user management, then it is assumed that
+      /// all users have admin privilege and evaluation of each EFI_IFR_SECURITY
+      /// op-code is always TRUE.
+      ///
+      return TRUE;
+    }
+  }
+
+  Status = mUserManager->Current (mUserManager, &UserProfileHandle);
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Enumerate all user information of the current user profile
+  /// to look for any EFI_USER_INFO_ACCESS_SETUP record.
+  ///
+  
+  for (UserInfoHandle = NULL;;) {
+    Status = mUserManager->GetNextInfo (mUserManager, UserProfileHandle, &UserInfoHandle);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+
+    UserInfoSize = 0;
+    Status = mUserManager->GetInfo (mUserManager, UserProfileHandle, UserInfoHandle, NULL, &UserInfoSize);
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      continue;
+    }
+
+    UserInfo = (EFI_USER_INFO *) AllocatePool (UserInfoSize);
+    if (UserInfo == NULL) {
+      break;
+    }
+
+    Status = mUserManager->GetInfo (mUserManager, UserProfileHandle, UserInfoHandle, UserInfo, &UserInfoSize);
+    if (EFI_ERROR (Status) ||
+        UserInfo->InfoType != EFI_USER_INFO_ACCESS_POLICY_RECORD ||
+        UserInfo->InfoSize <= sizeof (EFI_USER_INFO)) {
+      FreePool (UserInfo);
+      continue;
+    }
+
+    RemainSize = UserInfo->InfoSize - sizeof (EFI_USER_INFO);
+    AccessControl = (EFI_USER_INFO_ACCESS_CONTROL *)(UserInfo + 1);
+    while (RemainSize >= sizeof (EFI_USER_INFO_ACCESS_CONTROL)) {
+      if (RemainSize < AccessControl->Size || AccessControl->Size < sizeof (EFI_USER_INFO_ACCESS_CONTROL)) {
+        break;
+      }
+      if (AccessControl->Type == EFI_USER_INFO_ACCESS_SETUP) {
+        ///
+        /// Check if current user has the privilege specified by the permissions GUID.
+        ///
+
+        UserPermissionsGuid = (EFI_GUID *)(AccessControl + 1);
+        AccessControlDataSize = AccessControl->Size - sizeof (EFI_USER_INFO_ACCESS_CONTROL);
+        while (AccessControlDataSize >= sizeof (EFI_GUID)) {
+          if (CompareGuid (Guid, UserPermissionsGuid)) {
+            FreePool (UserInfo);
+            return TRUE;
+          }
+          UserPermissionsGuid++;
+          AccessControlDataSize -= sizeof (EFI_GUID);
+        }
+      }
+      RemainSize -= AccessControl->Size;
+      AccessControl = (EFI_USER_INFO_ACCESS_CONTROL *)((UINT8 *)AccessControl + AccessControl->Size);
+    }
+
+    FreePool (UserInfo);
+  }
+  return FALSE;
+}
+
+/**
+  Get question value from the predefined formset.
+
+  @param  DevicePath             The driver's device path which produece the formset data.
+  @param  InputHiiHandle         The hii handle associate with the formset data.
+  @param  FormSetGuid            The formset guid which include the question.
+  @param  QuestionId             The question id which need to get value from.
+  @param  Value                  The return data about question's value.
+  
+  @retval TRUE                   Get the question value success.
+  @retval FALSE                  Get the question value failed.
+**/
+BOOLEAN 
+GetQuestionValueFromForm (
+  IN EFI_DEVICE_PATH_PROTOCOL  *DevicePath,
+  IN EFI_HII_HANDLE            InputHiiHandle,
+  IN EFI_GUID                  *FormSetGuid,
+  IN EFI_QUESTION_ID           QuestionId,
+  OUT EFI_HII_VALUE            *Value
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_HANDLE               HiiHandle;
+  FORM_BROWSER_STATEMENT       *Question;
+  FORM_BROWSER_FORMSET         *FormSet;
+  FORM_BROWSER_FORM            *Form;
+  BOOLEAN                      GetTheVal;
+  LIST_ENTRY                   *Link;
+
+  // 
+  // The input parameter DevicePath or InputHiiHandle must have one valid input. 
+  //
+  ASSERT ((DevicePath != NULL && InputHiiHandle == NULL) || 
+          (DevicePath == NULL && InputHiiHandle != NULL) );
+
+  GetTheVal    = TRUE;
+  HiiHandle    = NULL;
+  Question     = NULL;
+  Form         = NULL;
+
+  //
+  // Get HiiHandle.
+  //
+  if (DevicePath != NULL) {
+    HiiHandle = DevicePathToHiiHandle (DevicePath, FormSetGuid);
+    if (HiiHandle == NULL) {
+      return FALSE;
+    }
+  } else {
+    HiiHandle = InputHiiHandle;
+  } 
+  ASSERT (HiiHandle != NULL);
+
+  //
+  // Get the formset data include this question.
+  //
+  FormSet = AllocateZeroPool (sizeof (FORM_BROWSER_FORMSET));
+  ASSERT (FormSet != NULL);
+  Status = InitializeFormSet(HiiHandle, FormSetGuid, FormSet);
+  if (EFI_ERROR (Status)) {
+    GetTheVal = FALSE;
+    goto Done;
+  }
+
+  //
+  // Base on the Question Id to get the question info.
+  //  
+  Question = IdToQuestion(FormSet, NULL, QuestionId);
+  if (Question == NULL) {
+    GetTheVal = FALSE;
+    goto Done;
+  }
+
+  //
+  // Search form in the formset scope
+  //
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+    Question = IdToQuestion2 (Form, QuestionId);
+    if (Question != NULL) {
+      break;
+    }
+
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+    Form = NULL;
+  }
+  ASSERT (Form != NULL);
+  
+  //
+  // Get the question value.
+  //
+  Status = GetQuestionValue(FormSet, Form, Question, GetSetValueWithEditBuffer);
+  if (EFI_ERROR (Status)) {
+    GetTheVal = FALSE;
+    goto Done;
+  }
+
+  CopyMem (Value, &Question->HiiValue, sizeof (EFI_HII_VALUE));
+  
+Done:
+  //
+  // Clean the formset structure and restore the global parameter.
+  //
+  if (FormSet != NULL) {
+    DestroyFormSet (FormSet);
+  }
+  
+  return GetTheVal;
+}
+
+/**
+  Evaluate the result of a HII expression.
+
+  If Expression is NULL, then ASSERT.
+
+  @param  FormSet                FormSet associated with this expression.
+  @param  Form                   Form associated with this expression.
+  @param  Expression             Expression to be evaluated.
+
+  @retval EFI_SUCCESS            The expression evaluated successfuly
+  @retval EFI_NOT_FOUND          The Question which referenced by a QuestionId
+                                 could not be found.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+  @retval EFI_ACCESS_DENIED      The pop operation underflowed the stack
+  @retval EFI_INVALID_PARAMETER  Syntax error with the Expression
+
+**/
+EFI_STATUS
+EvaluateExpression (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN FORM_BROWSER_FORM     *Form,
+  IN OUT FORM_EXPRESSION   *Expression
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  EXPRESSION_OPCODE       *OpCode;
+  FORM_BROWSER_STATEMENT  *Question;
+  FORM_BROWSER_STATEMENT  *Question2;
+  UINT16                  Index;
+  EFI_HII_VALUE           Data1;
+  EFI_HII_VALUE           Data2;
+  EFI_HII_VALUE           Data3;
+  FORM_EXPRESSION         *RuleExpression;
+  EFI_HII_VALUE           *Value;
+  INTN                    Result;
+  CHAR16                  *StrPtr;
+  CHAR16                  *NameValue;
+  UINT32                  TempValue;
+  LIST_ENTRY              *SubExpressionLink;
+  FORM_EXPRESSION         *SubExpression;
+  UINTN                   StackOffset;
+  UINTN                   TempLength;
+  CHAR16                  TempStr[5];
+  UINT8                   DigitUint8;
+  UINT8                   *TempBuffer;
+  EFI_TIME                EfiTime;
+  EFI_HII_VALUE           QuestionVal;
+  EFI_DEVICE_PATH_PROTOCOL *DevicePath;
+
+  StrPtr = NULL;
+
+  //
+  // Save current stack offset.
+  //
+  StackOffset = SaveExpressionEvaluationStackOffset ();
+
+  ASSERT (Expression != NULL);
+  Expression->Result.Type = EFI_IFR_TYPE_OTHER;
+
+  Link = GetFirstNode (&Expression->OpCodeListHead);
+  while (!IsNull (&Expression->OpCodeListHead, Link)) {
+    OpCode = EXPRESSION_OPCODE_FROM_LINK (Link);
+
+    Link = GetNextNode (&Expression->OpCodeListHead, Link);
+
+    ZeroMem (&Data1, sizeof (EFI_HII_VALUE));
+    ZeroMem (&Data2, sizeof (EFI_HII_VALUE));
+    ZeroMem (&Data3, sizeof (EFI_HII_VALUE));
+
+    Value = &Data3;
+    Value->Type = EFI_IFR_TYPE_BOOLEAN;
+    Status = EFI_SUCCESS;
+
+    switch (OpCode->Operand) {
+    //
+    // Built-in functions
+    //
+    case EFI_IFR_EQ_ID_VAL_OP:
+      Question = IdToQuestion (FormSet, Form, OpCode->QuestionId);
+      if (Question == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Status = CompareHiiValue (&Question->HiiValue, &OpCode->Value, &Result, NULL);
+      if (Status == EFI_UNSUPPORTED) {
+        Status = EFI_SUCCESS;
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      Value->Value.b = (BOOLEAN) ((Result == 0) ? TRUE : FALSE);
+      break;
+
+    case EFI_IFR_EQ_ID_ID_OP:
+      Question = IdToQuestion (FormSet, Form, OpCode->QuestionId);
+      if (Question == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Question2 = IdToQuestion (FormSet, Form, OpCode->QuestionId2);
+      if (Question2 == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Status = CompareHiiValue (&Question->HiiValue, &Question2->HiiValue, &Result, FormSet->HiiHandle);
+      if (Status == EFI_UNSUPPORTED) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        Status = EFI_SUCCESS;
+        break;
+      }
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      Value->Value.b = (BOOLEAN) ((Result == 0) ? TRUE : FALSE);
+      break;
+
+    case EFI_IFR_EQ_ID_VAL_LIST_OP:
+      Question = IdToQuestion (FormSet, Form, OpCode->QuestionId);
+      if (Question == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value->Value.b = FALSE;
+      for (Index =0; Index < OpCode->ListLength; Index++) {
+        if (Question->HiiValue.Value.u16 == OpCode->ValueList[Index]) {
+          Value->Value.b = TRUE;
+          break;
+        }
+      }
+      break;
+
+    case EFI_IFR_DUP_OP:
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      Status = PushExpression (Value);
+      break;
+
+    case EFI_IFR_QUESTION_REF1_OP:
+    case EFI_IFR_THIS_OP:
+      Question = IdToQuestion (FormSet, Form, OpCode->QuestionId);
+      if (Question == NULL) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+
+      Value = &Question->HiiValue;
+      break;
+
+    case EFI_IFR_SECURITY_OP:
+      Value->Value.b = CheckUserPrivilege (&OpCode->Guid);
+      break;
+
+    case EFI_IFR_GET_OP:
+      //
+      // Get Value from VarStore buffer, EFI VarStore, Name/Value VarStore.
+      //
+      Value->Type = EFI_IFR_TYPE_UNDEFINED;
+      Value->Value.u8 = 0;
+      if (OpCode->VarStorage != NULL) {
+        switch (OpCode->VarStorage->Type) {
+        case EFI_HII_VARSTORE_BUFFER:
+        case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+          //
+          // Get value from Edit Buffer
+          //
+          Value->Type = OpCode->ValueType;
+          CopyMem (&Value->Value, OpCode->VarStorage->EditBuffer + OpCode->VarStoreInfo.VarOffset, OpCode->ValueWidth);
+          break;
+        case EFI_HII_VARSTORE_NAME_VALUE:
+          if (OpCode->ValueType != EFI_IFR_TYPE_STRING) {
+            //
+            // Get value from string except for STRING value.
+            //
+            Status = GetValueByName (OpCode->VarStorage, OpCode->ValueName, &StrPtr, GetSetValueWithEditBuffer);
+            if (!EFI_ERROR (Status)) {
+              ASSERT (StrPtr != NULL);
+              TempLength = StrLen (StrPtr);
+              if (OpCode->ValueWidth >= ((TempLength + 1) / 2)) {
+                Value->Type = OpCode->ValueType;
+                TempBuffer = (UINT8 *) &Value->Value;
+                ZeroMem (TempStr, sizeof (TempStr));
+                for (Index = 0; Index < TempLength; Index ++) {
+                  TempStr[0] = StrPtr[TempLength - Index - 1];
+                  DigitUint8 = (UINT8) StrHexToUint64 (TempStr);
+                  if ((Index & 1) == 0) {
+                    TempBuffer [Index/2] = DigitUint8;
+                  } else {
+                    TempBuffer [Index/2] = (UINT8) ((DigitUint8 << 4) + TempBuffer [Index/2]);
+                  }
+                }
+              }                
+            }
+          }
+          break;
+        case EFI_HII_VARSTORE_EFI_VARIABLE:
+          //
+          // Get value from variable.
+          //
+          TempLength = OpCode->ValueWidth;
+          Value->Type = OpCode->ValueType;
+          Status = gRT->GetVariable (
+                          OpCode->ValueName,
+                          &OpCode->VarStorage->Guid,
+                          NULL,
+                          &TempLength,
+                          &Value->Value
+                          );
+          if (EFI_ERROR (Status)) {
+            Value->Type = EFI_IFR_TYPE_UNDEFINED;
+            Value->Value.u8 = 0;
+          }
+          break;
+        default:
+          //
+          // Not recognize storage.
+          //
+          Status = EFI_UNSUPPORTED;
+          goto Done;
+        }
+      } else {
+        //
+        // For Time/Date Data
+        //
+        if (OpCode->ValueType != EFI_IFR_TYPE_DATE && OpCode->ValueType != EFI_IFR_TYPE_TIME) {
+          //
+          // Only support Data/Time data when storage doesn't exist.
+          //
+          Status = EFI_UNSUPPORTED;
+          goto Done;
+        }
+        Status = gRT->GetTime (&EfiTime, NULL);
+        if (!EFI_ERROR (Status)) {
+          if (OpCode->ValueType == EFI_IFR_TYPE_DATE) {
+            switch (OpCode->VarStoreInfo.VarOffset) {
+            case 0x00:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_16;
+              Value->Value.u16 = EfiTime.Year;
+              break;
+            case 0x02:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+              Value->Value.u8 = EfiTime.Month;
+              break;
+            case 0x03:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+              Value->Value.u8 = EfiTime.Day;
+              break;
+            default:
+              //
+              // Invalid Date field.
+              //
+              Status = EFI_INVALID_PARAMETER;
+              goto Done;
+            }
+          } else {
+            switch (OpCode->VarStoreInfo.VarOffset) {
+            case 0x00:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+              Value->Value.u8 = EfiTime.Hour;
+              break;
+            case 0x01:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+              Value->Value.u8 = EfiTime.Minute;
+              break;
+            case 0x02:
+              Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+              Value->Value.u8 = EfiTime.Second;
+              break;
+            default:
+              //
+              // Invalid Time field.
+              //
+              Status = EFI_INVALID_PARAMETER;
+              goto Done;
+            }
+          }
+        }
+      }
+
+      break;
+
+    case EFI_IFR_QUESTION_REF3_OP:
+      //
+      // EFI_IFR_QUESTION_REF3
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    
+      //
+      // Validate the expression value
+      //
+      if ((Value->Type > EFI_IFR_TYPE_NUM_SIZE_64) || (Value->Value.u64 > 0xffff)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (OpCode->DevicePath != 0) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+
+        StrPtr = GetToken (OpCode->DevicePath, FormSet->HiiHandle);
+        if (StrPtr != NULL && mPathFromText != NULL) {
+          DevicePath = mPathFromText->ConvertTextToDevicePath(StrPtr);
+          if (DevicePath != NULL && GetQuestionValueFromForm(DevicePath, NULL, &OpCode->Guid, Value->Value.u16, &QuestionVal)) {
+            Value = &QuestionVal;
+          }
+          if (DevicePath != NULL) {
+            FreePool (DevicePath);
+          }
+        }
+
+        if (StrPtr != NULL) {
+          FreePool (StrPtr);
+        }
+      } else if (CompareGuid (&OpCode->Guid, &gZeroGuid) != 0) {
+        if (!GetQuestionValueFromForm(NULL, FormSet->HiiHandle, &OpCode->Guid, Value->Value.u16, &QuestionVal)){
+          Value->Type = EFI_IFR_TYPE_UNDEFINED;
+          break;
+        }
+        Value = &QuestionVal;
+      } else {
+        Question = IdToQuestion (FormSet, Form, Value->Value.u16);
+        if (Question == NULL) {
+          Value->Type = EFI_IFR_TYPE_UNDEFINED;
+          break;
+        }
+
+        //
+        // push the questions' value on to the expression stack
+        //
+        Value = &Question->HiiValue;
+      }
+      break;
+
+    case EFI_IFR_RULE_REF_OP:
+      //
+      // Find expression for this rule
+      //
+      RuleExpression = RuleIdToExpression (Form, OpCode->RuleId);
+      if (RuleExpression == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      //
+      // Evaluate this rule expression
+      //
+      Status = EvaluateExpression (FormSet, Form, RuleExpression);
+      if (EFI_ERROR (Status) || RuleExpression->Result.Type == EFI_IFR_TYPE_UNDEFINED) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value = &RuleExpression->Result;
+      break;
+
+    case EFI_IFR_STRING_REF1_OP:
+      Value->Type = EFI_IFR_TYPE_STRING;
+      Value->Value.string = OpCode->Value.Value.string;
+      break;
+
+    //
+    // Constant
+    //
+    case EFI_IFR_TRUE_OP:
+    case EFI_IFR_FALSE_OP:
+    case EFI_IFR_ONE_OP:
+    case EFI_IFR_ONES_OP:
+    case EFI_IFR_UINT8_OP:
+    case EFI_IFR_UINT16_OP:
+    case EFI_IFR_UINT32_OP:
+    case EFI_IFR_UINT64_OP:
+    case EFI_IFR_UNDEFINED_OP:
+    case EFI_IFR_VERSION_OP:
+    case EFI_IFR_ZERO_OP:
+      Value = &OpCode->Value;
+      break;
+
+    //
+    // unary-op
+    //
+    case EFI_IFR_LENGTH_OP:
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      if (Value->Type != EFI_IFR_TYPE_STRING && !IsTypeInBuffer (Value)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (Value->Type == EFI_IFR_TYPE_STRING) {
+        StrPtr = GetToken (Value->Value.string, FormSet->HiiHandle);
+        if (StrPtr == NULL) {
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+        Value->Value.u64 = StrLen (StrPtr);
+        FreePool (StrPtr);
+      } else {
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+        Value->Value.u64 = GetLengthForValue(Value);
+        FreePool (Value->Buffer);
+      }
+      break;
+
+    case EFI_IFR_NOT_OP:
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      if (Value->Type != EFI_IFR_TYPE_BOOLEAN) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+      Value->Value.b = (BOOLEAN) (!Value->Value.b);
+      break;
+
+    case EFI_IFR_QUESTION_REF2_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Validate the expression value
+      //
+      if ((Value->Type > EFI_IFR_TYPE_NUM_SIZE_64) || (Value->Value.u64 > 0xffff)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Question = IdToQuestion (FormSet, Form, Value->Value.u16);
+      if (Question == NULL) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value = &Question->HiiValue;
+      break;
+
+    case EFI_IFR_STRING_REF2_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Validate the expression value
+      //
+      if ((Value->Type > EFI_IFR_TYPE_NUM_SIZE_64) || (Value->Value.u64 > 0xffff)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value->Type = EFI_IFR_TYPE_STRING;
+      StrPtr = GetToken (Value->Value.u16, FormSet->HiiHandle);
+      if (StrPtr == NULL) {
+        //
+        // If String not exit, push an empty string
+        //
+        Value->Value.string = NewString (gEmptyString, FormSet->HiiHandle);
+      } else {
+        Index = (UINT16) Value->Value.u64;
+        Value->Value.string = Index;
+        FreePool (StrPtr);
+      }
+      break;
+
+    case EFI_IFR_TO_BOOLEAN_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Convert an expression to a Boolean
+      //
+      if (Value->Type <= EFI_IFR_TYPE_DATE) {
+        //
+        // When converting from an unsigned integer, zero will be converted to
+        // FALSE and any other value will be converted to TRUE.
+        //
+        Value->Value.b = (BOOLEAN) (HiiValueToUINT64(Value) != 0);
+
+        Value->Type = EFI_IFR_TYPE_BOOLEAN;
+      } else if (Value->Type == EFI_IFR_TYPE_STRING) {
+        //
+        // When converting from a string, if case-insensitive compare
+        // with "true" is True, then push True. If a case-insensitive compare
+        // with "false" is True, then push False. Otherwise, push Undefined. 
+        //
+        StrPtr = GetToken (Value->Value.string, FormSet->HiiHandle);
+        if (StrPtr == NULL) {
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+        
+        IfrStrToUpper (StrPtr);
+        if (StrCmp (StrPtr, L"TRUE") == 0){
+          Value->Value.b = TRUE;
+          Value->Type = EFI_IFR_TYPE_BOOLEAN;
+        } else if (StrCmp (StrPtr, L"FALSE") == 0) {
+          Value->Value.b = FALSE;
+          Value->Type = EFI_IFR_TYPE_BOOLEAN;
+        } else {
+          Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        }
+        FreePool (StrPtr);
+      } else if (Value->Type == EFI_IFR_TYPE_BUFFER) {
+        //
+        // When converting from a buffer, if the buffer is all zeroes, 
+        // then push False. Otherwise push True. 
+        //
+        for (Index =0; Index < Value->BufferLen; Index ++) {
+          if (Value->Buffer[Index] != 0) {            
+            break;
+          }
+        }
+
+        if (Index >= Value->BufferLen) {
+          Value->Value.b = FALSE;
+        } else {
+          Value->Value.b = TRUE;
+        }
+        Value->Type = EFI_IFR_TYPE_BOOLEAN;
+        FreePool (Value->Buffer);
+      }
+      break;
+
+    case EFI_IFR_TO_STRING_OP:
+      Status = IfrToString (FormSet, OpCode->Format, Value);
+      break;
+
+    case EFI_IFR_TO_UINT_OP:
+      Status = IfrToUint (FormSet, Value);
+      break;
+
+    case EFI_IFR_TO_LOWER_OP:
+    case EFI_IFR_TO_UPPER_OP:
+      Status = InitializeUnicodeCollationProtocol ();
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (Value->Type != EFI_IFR_TYPE_STRING) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      StrPtr = GetToken (Value->Value.string, FormSet->HiiHandle);
+      if (StrPtr == NULL) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+
+      if (OpCode->Operand == EFI_IFR_TO_LOWER_OP) {
+        mUnicodeCollation->StrLwr (mUnicodeCollation, StrPtr);
+      } else {
+        mUnicodeCollation->StrUpr (mUnicodeCollation, StrPtr);
+      }
+      Value->Value.string = NewString (StrPtr, FormSet->HiiHandle);
+      FreePool (StrPtr);
+      break;
+
+    case EFI_IFR_BITWISE_NOT_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      if (Value->Type > EFI_IFR_TYPE_DATE) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+      Value->Value.u64 = ~ HiiValueToUINT64(Value);
+      break;
+
+    case EFI_IFR_SET_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (Value);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      Data1.Type = EFI_IFR_TYPE_BOOLEAN;
+      Data1.Value.b = FALSE;
+      //
+      // Set value to var storage buffer
+      //
+      if (OpCode->VarStorage != NULL) {
+        switch (OpCode->VarStorage->Type) {
+        case EFI_HII_VARSTORE_BUFFER:
+        case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+          CopyMem (OpCode->VarStorage->EditBuffer + OpCode->VarStoreInfo.VarOffset, &Value->Value, OpCode->ValueWidth);
+          Data1.Value.b = TRUE;
+          break;
+        case EFI_HII_VARSTORE_NAME_VALUE:
+          if (OpCode->ValueType != EFI_IFR_TYPE_STRING) {
+            NameValue = AllocateZeroPool ((OpCode->ValueWidth * 2 + 1) * sizeof (CHAR16));
+            ASSERT (NameValue != NULL);
+            //
+            // Convert Buffer to Hex String
+            //
+            TempBuffer = (UINT8 *) &Value->Value + OpCode->ValueWidth - 1;
+            StrPtr = NameValue;
+            for (Index = 0; Index < OpCode->ValueWidth; Index ++, TempBuffer --) {
+              StrPtr += UnicodeValueToString (StrPtr, PREFIX_ZERO | RADIX_HEX, *TempBuffer, 2);
+            }
+            Status = SetValueByName (OpCode->VarStorage, OpCode->ValueName, NameValue, GetSetValueWithEditBuffer, NULL);
+            FreePool (NameValue);
+            if (!EFI_ERROR (Status)) {
+              Data1.Value.b = TRUE;
+            }
+          }
+          break;
+        case EFI_HII_VARSTORE_EFI_VARIABLE:
+          Status = gRT->SetVariable (
+                          OpCode->ValueName,
+                          &OpCode->VarStorage->Guid,
+                          OpCode->VarStorage->Attributes,
+                          OpCode->ValueWidth,
+                          &Value->Value
+                          );
+          if (!EFI_ERROR (Status)) {
+            Data1.Value.b = TRUE;
+          }
+          break;
+        default:
+          //
+          // Not recognize storage.
+          //
+          Status = EFI_UNSUPPORTED;
+          goto Done;
+          break;
+        }
+      } else {
+        //
+        // For Time/Date Data
+        //
+        if (OpCode->ValueType != EFI_IFR_TYPE_DATE && OpCode->ValueType != EFI_IFR_TYPE_TIME) {
+          //
+          // Only support Data/Time data when storage doesn't exist.
+          //
+          Status = EFI_UNSUPPORTED;
+          goto Done;
+        }
+        Status = gRT->GetTime (&EfiTime, NULL);
+        if (!EFI_ERROR (Status)) {
+          if (OpCode->ValueType == EFI_IFR_TYPE_DATE) {
+            switch (OpCode->VarStoreInfo.VarOffset) {
+            case 0x00:
+              EfiTime.Year = Value->Value.u16;
+              break;
+            case 0x02:
+              EfiTime.Month = Value->Value.u8;
+              break;
+            case 0x03:
+              EfiTime.Day = Value->Value.u8;
+              break;
+            default:
+              //
+              // Invalid Date field.
+              //
+              Status = EFI_INVALID_PARAMETER;
+              goto Done;
+            }
+          } else {
+            switch (OpCode->VarStoreInfo.VarOffset) {
+            case 0x00:
+              EfiTime.Hour = Value->Value.u8;
+              break;
+            case 0x01:
+              EfiTime.Minute = Value->Value.u8;
+              break;
+            case 0x02:
+              EfiTime.Second = Value->Value.u8;
+              break;
+            default:
+              //
+              // Invalid Time field.
+              //
+              Status = EFI_INVALID_PARAMETER;
+              goto Done;
+            }
+          }
+          Status = gRT->SetTime (&EfiTime);
+          if (!EFI_ERROR (Status)) {
+            Data1.Value.b = TRUE;
+          }
+        }
+      }
+      Value = &Data1;
+      break;
+
+    //
+    // binary-op
+    //
+    case EFI_IFR_ADD_OP:
+    case EFI_IFR_SUBTRACT_OP:
+    case EFI_IFR_MULTIPLY_OP:
+    case EFI_IFR_DIVIDE_OP:
+    case EFI_IFR_MODULO_OP:
+    case EFI_IFR_BITWISE_AND_OP:
+    case EFI_IFR_BITWISE_OR_OP:
+    case EFI_IFR_SHIFT_LEFT_OP:
+    case EFI_IFR_SHIFT_RIGHT_OP:
+      //
+      // Pop an expression from the expression stack
+      //
+      Status = PopExpression (&Data2);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Pop another expression from the expression stack
+      //
+      Status = PopExpression (&Data1);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (Data2.Type > EFI_IFR_TYPE_DATE) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+
+      if (Data1.Type > EFI_IFR_TYPE_DATE) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+
+      switch (OpCode->Operand) {
+        case EFI_IFR_ADD_OP:
+          Value->Value.u64 = HiiValueToUINT64(&Data1) + HiiValueToUINT64(&Data2);
+          break;
+
+        case EFI_IFR_SUBTRACT_OP:
+          Value->Value.u64 = HiiValueToUINT64(&Data1) - HiiValueToUINT64(&Data2);
+          break;
+
+        case EFI_IFR_MULTIPLY_OP:
+          Value->Value.u64 = MultU64x32 (HiiValueToUINT64(&Data1), (UINT32) HiiValueToUINT64(&Data2));
+          break;
+
+        case EFI_IFR_DIVIDE_OP:
+          Value->Value.u64 = DivU64x32 (HiiValueToUINT64(&Data1), (UINT32) HiiValueToUINT64(&Data2));
+          break;
+
+        case EFI_IFR_MODULO_OP:
+          DivU64x32Remainder  (HiiValueToUINT64(&Data1), (UINT32) HiiValueToUINT64(&Data2), &TempValue);
+          Value->Value.u64 = TempValue;
+          break;
+
+        case EFI_IFR_BITWISE_AND_OP:
+          Value->Value.u64 = HiiValueToUINT64(&Data1) & HiiValueToUINT64(&Data2);
+          break;
+
+        case EFI_IFR_BITWISE_OR_OP:
+          Value->Value.u64 = HiiValueToUINT64(&Data1) | HiiValueToUINT64(&Data2);
+          break;
+
+        case EFI_IFR_SHIFT_LEFT_OP:
+          Value->Value.u64 = LShiftU64 (HiiValueToUINT64(&Data1), (UINTN) HiiValueToUINT64(&Data2));
+          break;
+
+        case EFI_IFR_SHIFT_RIGHT_OP:
+          Value->Value.u64 = RShiftU64 (HiiValueToUINT64(&Data1), (UINTN) HiiValueToUINT64(&Data2));
+          break;
+
+        default:
+          break;
+      }
+      break;
+
+    case EFI_IFR_AND_OP:
+    case EFI_IFR_OR_OP:
+      //
+      // Two Boolean operator
+      //
+      Status = PopExpression (&Data2);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Pop another expression from the expression stack
+      //
+      Status = PopExpression (&Data1);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (Data2.Type != EFI_IFR_TYPE_BOOLEAN) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (Data1.Type != EFI_IFR_TYPE_BOOLEAN) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (OpCode->Operand == EFI_IFR_AND_OP) {
+        Value->Value.b = (BOOLEAN) (Data1.Value.b && Data2.Value.b);
+      } else {
+        Value->Value.b = (BOOLEAN) (Data1.Value.b || Data2.Value.b);
+      }
+      break;
+
+    case EFI_IFR_EQUAL_OP:
+    case EFI_IFR_NOT_EQUAL_OP:
+    case EFI_IFR_GREATER_EQUAL_OP:
+    case EFI_IFR_GREATER_THAN_OP:
+    case EFI_IFR_LESS_EQUAL_OP:
+    case EFI_IFR_LESS_THAN_OP:
+      //
+      // Compare two integer, string, boolean or date/time
+      //
+      Status = PopExpression (&Data2);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Pop another expression from the expression stack
+      //
+      Status = PopExpression (&Data1);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      if (Data2.Type > EFI_IFR_TYPE_BOOLEAN && 
+          Data2.Type != EFI_IFR_TYPE_STRING && 
+          !IsTypeInBuffer(&Data2)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (Data1.Type > EFI_IFR_TYPE_BOOLEAN && 
+          Data1.Type != EFI_IFR_TYPE_STRING && 
+          !IsTypeInBuffer(&Data1)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      Status = CompareHiiValue (&Data1, &Data2, &Result, FormSet->HiiHandle);
+      if (Data1.Type == EFI_IFR_TYPE_BUFFER) {
+        FreePool (Data1.Buffer);
+      }
+      if (Data2.Type == EFI_IFR_TYPE_BUFFER) {
+        FreePool (Data2.Buffer);
+      }
+      
+      if (Status == EFI_UNSUPPORTED) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        Status = EFI_SUCCESS;
+        break;
+      }
+
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      switch (OpCode->Operand) {
+      case EFI_IFR_EQUAL_OP:
+        Value->Value.b = (BOOLEAN) ((Result == 0) ? TRUE : FALSE);
+        break;
+
+      case EFI_IFR_NOT_EQUAL_OP:
+        Value->Value.b = (BOOLEAN) ((Result != 0) ? TRUE : FALSE);
+        break;
+
+      case EFI_IFR_GREATER_EQUAL_OP:
+        Value->Value.b = (BOOLEAN) ((Result >= 0) ? TRUE : FALSE);
+        break;
+
+      case EFI_IFR_GREATER_THAN_OP:
+        Value->Value.b = (BOOLEAN) ((Result > 0) ? TRUE : FALSE);
+        break;
+
+      case EFI_IFR_LESS_EQUAL_OP:
+        Value->Value.b = (BOOLEAN) ((Result <= 0) ? TRUE : FALSE);
+        break;
+
+      case EFI_IFR_LESS_THAN_OP:
+        Value->Value.b = (BOOLEAN) ((Result < 0) ? TRUE : FALSE);
+        break;
+
+      default:
+        break;
+      }
+      break;
+
+    case EFI_IFR_MATCH_OP:
+      Status = InitializeUnicodeCollationProtocol ();
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+     
+      Status = IfrMatch (FormSet, Value);
+      break;
+
+    case EFI_IFR_MATCH2_OP:
+      Status = IfrMatch2 (FormSet, &OpCode->Guid, Value);
+      break;
+
+    case EFI_IFR_CATENATE_OP:
+      Status = IfrCatenate (FormSet, Value);
+      break;
+
+    //
+    // ternary-op
+    //
+    case EFI_IFR_CONDITIONAL_OP:
+      //
+      // Pop third expression from the expression stack
+      //
+      Status = PopExpression (&Data3);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Pop second expression from the expression stack
+      //
+      Status = PopExpression (&Data2);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+
+      //
+      // Pop first expression from the expression stack
+      //
+      Status = PopExpression (&Data1);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      if (Data1.Type != EFI_IFR_TYPE_BOOLEAN) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+      }
+
+      if (Data1.Value.b) {
+        Value = &Data3;
+      } else {
+        Value = &Data2;
+      }
+      break;
+
+    case EFI_IFR_FIND_OP:
+      Status = IfrFind (FormSet, OpCode->Format, Value);
+      break;
+
+    case EFI_IFR_MID_OP:
+      Status = IfrMid (FormSet, Value);
+      break;
+
+    case EFI_IFR_TOKEN_OP:
+      Status = IfrToken (FormSet, Value);
+      break;
+
+    case EFI_IFR_SPAN_OP:
+      Status = IfrSpan (FormSet, OpCode->Flags, Value);
+      break;
+
+    case EFI_IFR_MAP_OP:
+      //
+      // Pop the check value
+      //
+      Status = PopExpression (&Data1);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      //
+      // Check MapExpression list is valid.
+      //
+      if (OpCode->MapExpressionList.ForwardLink == NULL) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+      //
+      // Go through map expression list.
+      //
+      SubExpressionLink = GetFirstNode(&OpCode->MapExpressionList);
+      while (!IsNull (&OpCode->MapExpressionList, SubExpressionLink)) {
+        SubExpression = FORM_EXPRESSION_FROM_LINK (SubExpressionLink);
+        //
+        // Evaluate the first expression in this pair.
+        //
+        Status = EvaluateExpression (FormSet, Form, SubExpression);
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+        //
+        // Compare the expression value with current value
+        //
+        if ((CompareHiiValue (&Data1, &SubExpression->Result, &Result, NULL) == EFI_SUCCESS) && (Result == 0)) {
+          //
+          // Try get the map value.
+          //
+          SubExpressionLink = GetNextNode (&OpCode->MapExpressionList, SubExpressionLink);
+          if (IsNull (&OpCode->MapExpressionList, SubExpressionLink)) {
+            Status = EFI_INVALID_PARAMETER;
+            goto Done;
+          }
+          SubExpression = FORM_EXPRESSION_FROM_LINK (SubExpressionLink);
+          Status = EvaluateExpression (FormSet, Form, SubExpression);
+          if (EFI_ERROR (Status)) {
+            goto Done;
+          }
+          Value = &SubExpression->Result;
+          break;
+        }
+        //
+        // Skip the second expression on this pair.
+        //
+        SubExpressionLink = GetNextNode (&OpCode->MapExpressionList, SubExpressionLink);
+        if (IsNull (&OpCode->MapExpressionList, SubExpressionLink)) {
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+        //
+        // Goto the first expression on next pair.
+        //
+        SubExpressionLink = GetNextNode (&OpCode->MapExpressionList, SubExpressionLink);
+      }
+
+      //
+      // No map value is found.
+      //
+      if (IsNull (&OpCode->MapExpressionList, SubExpressionLink)) {
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        Value->Value.u8 = 0;
+      }
+      break;
+
+    default:
+      break;
+    }
+    if (EFI_ERROR (Status) || Value->Type == EFI_IFR_TYPE_UNDEFINED) {
+      goto Done;
+    }
+
+    Status = PushExpression (Value);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+  }
+
+  //
+  // Pop the final result from expression stack
+  //
+  Value = &Data1;
+  Status = PopExpression (Value);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // After evaluating an expression, there should be only one value left on the expression stack
+  //
+  if (PopExpression (Value) != EFI_ACCESS_DENIED) {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+Done:
+  RestoreExpressionEvaluationStackOffset (StackOffset);
+  if (!EFI_ERROR (Status)) {
+    CopyMem (&Expression->Result, Value, sizeof (EFI_HII_VALUE));
+  }
+
+  return Status;
+}
+
+/**
+  Check whether the result is TRUE or FALSE.
+  
+  For the EFI_HII_VALUE value type is numeric, return TRUE if the
+  value is not 0.
+
+  @param  Result             Input the result data.
+
+  @retval TRUE               The result is TRUE.
+  @retval FALSE              The result is FALSE.
+
+**/
+BOOLEAN
+IsTrue (
+  IN EFI_HII_VALUE     *Result
+  )
+{
+  switch (Result->Type) {
+  case EFI_IFR_TYPE_BOOLEAN:
+    return Result->Value.b;
+
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    return (BOOLEAN)(Result->Value.u8 != 0);
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    return (BOOLEAN)(Result->Value.u16 != 0);
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    return (BOOLEAN)(Result->Value.u32 != 0);
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    return (BOOLEAN)(Result->Value.u64 != 0);
+
+  default:
+    return FALSE;
+  }
+}
+
+/**
+  Return the result of the expression list. Check the expression list and 
+  return the highest priority express result.  
+  Priority: DisableIf > SuppressIf > GrayOutIf > FALSE
+
+  @param  ExpList             The input expression list.
+  @param  Evaluate            Whether need to evaluate the expression first.
+  @param  FormSet             FormSet associated with this expression.
+  @param  Form                Form associated with this expression.  
+
+  @retval EXPRESS_RESULT      Return the higher priority express result. 
+                              DisableIf > SuppressIf > GrayOutIf > FALSE
+
+**/
+EXPRESS_RESULT 
+EvaluateExpressionList (
+  IN FORM_EXPRESSION_LIST *ExpList,
+  IN BOOLEAN              Evaluate,
+  IN FORM_BROWSER_FORMSET *FormSet, OPTIONAL
+  IN FORM_BROWSER_FORM    *Form OPTIONAL
+  )
+{
+  UINTN              Index;
+  EXPRESS_RESULT     ReturnVal;
+  EXPRESS_RESULT     CompareOne;
+  EFI_STATUS         Status;
+
+  if (ExpList == NULL) {
+    return ExpressFalse;
+  }
+
+  ASSERT(ExpList->Signature == FORM_EXPRESSION_LIST_SIGNATURE);
+  Index     = 0;
+
+  //
+  // Check whether need to evaluate the expression first.
+  //
+  if (Evaluate) {  
+    while (ExpList->Count > Index) {
+      Status = EvaluateExpression (FormSet, Form, ExpList->Expression[Index++]);
+      if (EFI_ERROR (Status)) {
+        return ExpressFalse;
+      }
+    }
+  }
+
+  //
+  // Run the list of expressions.
+  //
+  ReturnVal = ExpressFalse;
+  for (Index = 0; Index < ExpList->Count; Index++) {
+    if (IsTrue (&ExpList->Expression[Index]->Result)) {
+      switch (ExpList->Expression[Index]->Type) {
+        case EFI_HII_EXPRESSION_SUPPRESS_IF:
+          CompareOne = ExpressSuppress;
+          break;
+
+        case EFI_HII_EXPRESSION_GRAY_OUT_IF:
+          CompareOne = ExpressGrayOut;
+          break;
+
+        case EFI_HII_EXPRESSION_DISABLE_IF:
+          CompareOne = ExpressDisable;
+          break;
+
+        default:
+          return ExpressFalse; 
+      }
+
+      ReturnVal = ReturnVal < CompareOne ? CompareOne : ReturnVal;
+    }
+  }
+  
+  return ReturnVal;
+}
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.h b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.h
new file mode 100644
index 0000000000..5660a997b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Expression.h
@@ -0,0 +1,265 @@
+/** @file
+Private structure, MACRO and function definitions for User Interface related functionalities.
+
+Copyright (c) 2004 - 2013, 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.
+
+**/
+
+#ifndef _EXPRESSION_H_
+#define _EXPRESSION_H_
+
+/**
+  Get the expression list count.
+  
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval >=0                    The expression count
+  @retval -1                     Input parameter error.
+
+**/
+INTN 
+GetConditionalExpressionCount (
+  IN EXPRESS_LEVEL       Level
+  );
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetCurrentExpressionStack (
+  VOID
+  );
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetMapExpressionListStack (
+  VOID
+  );
+
+/**
+  Reset stack pointer to begin of the stack.
+
+**/
+VOID
+ResetScopeStack (
+  VOID
+  );
+
+/**
+  Push an Operand onto the Stack
+
+  @param  Operand                Operand to push.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+
+**/
+EFI_STATUS
+PushScope (
+  IN UINT8   Operand
+  );
+
+/**
+  Get the expression Buffer pointer.
+  
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval  The start pointer of the expression buffer or NULL.
+
+**/
+FORM_EXPRESSION **
+GetConditionalExpressionList (
+  IN EXPRESS_LEVEL       Level
+  );
+
+/**
+  Pop an Operand from the Stack
+
+  @param  Operand                Operand to pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+
+**/
+EFI_STATUS
+PopScope (
+  OUT UINT8     *Operand
+  );
+
+/**
+  Push the list of map expression onto the Stack
+
+  @param  Pointer                Pointer to the list of map expression to be pushed.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushMapExpressionList (
+  IN VOID  *Pointer
+  );
+
+/**
+  Push current expression onto the Stack
+
+  @param  Pointer                Pointer to current expression.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushCurrentExpression (
+  IN VOID  *Pointer
+  );
+
+/**
+  Zero extend integer/boolean/date/time to UINT64 for comparing.
+
+  @param  Value                  HII Value to be converted.
+
+**/
+VOID
+ExtendValueToU64 (
+  IN  EFI_HII_VALUE   *Value
+  );
+
+/**
+  Push the expression options onto the Stack.
+
+  @param  Pointer                Pointer to the current expression.
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PushConditionalExpression (
+  IN FORM_EXPRESSION   *Pointer,
+  IN EXPRESS_LEVEL     Level
+  );
+
+/**
+  Pop the expression options from the Stack
+
+  @param  Level                  Which type this expression belong to. Form, 
+                                 statement or option?
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopConditionalExpression (
+  IN  EXPRESS_LEVEL      Level
+  );
+
+/**
+  Pop the list of map expression from the Stack
+
+  @param  Pointer                Pointer to the list of map expression to be pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopMapExpressionList (
+  OUT VOID    **Pointer
+  );
+
+/**
+  Pop current expression from the Stack
+
+  @param  Pointer                Pointer to current expression to be pop.
+
+  @retval EFI_SUCCESS            The value was pushed onto the stack.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the stack.
+
+**/
+EFI_STATUS
+PopCurrentExpression (
+  OUT VOID    **Pointer
+  );
+
+/**
+  Evaluate the result of a HII expression.
+
+  If Expression is NULL, then ASSERT.
+
+  @param  FormSet                FormSet associated with this expression.
+  @param  Form                   Form associated with this expression.
+  @param  Expression             Expression to be evaluated.
+
+  @retval EFI_SUCCESS            The expression evaluated successfuly
+  @retval EFI_NOT_FOUND          The Question which referenced by a QuestionId
+                                 could not be found.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough system memory to grow the
+                                 stack.
+  @retval EFI_ACCESS_DENIED      The pop operation underflowed the stack
+  @retval EFI_INVALID_PARAMETER  Syntax error with the Expression
+
+**/
+EFI_STATUS
+EvaluateExpression (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN FORM_BROWSER_FORM     *Form,
+  IN OUT FORM_EXPRESSION   *Expression
+  );
+/**
+  Return the result of the expression list. Check the expression list and 
+  return the highest priority express result.  
+  Priority: DisableIf > SuppressIf > GrayOutIf > FALSE
+
+  @param  ExpList             The input expression list.
+  @param  Evaluate            Whether need to evaluate the expression first.
+  @param  FormSet             FormSet associated with this expression.
+  @param  Form                Form associated with this expression.  
+
+  @retval EXPRESS_RESULT      Return the higher priority express result. 
+                              DisableIf > SuppressIf > GrayOutIf > FALSE
+
+**/
+EXPRESS_RESULT 
+EvaluateExpressionList (
+  IN FORM_EXPRESSION_LIST *ExpList,
+  IN BOOLEAN              Evaluate,
+  IN FORM_BROWSER_FORMSET *FormSet, OPTIONAL
+  IN FORM_BROWSER_FORM    *Form OPTIONAL
+  );
+
+/**
+  Get Form given its FormId.
+
+  @param  FormSet                The formset which contains this form.
+  @param  FormId                 Id of this form.
+
+  @retval Pointer                The form.
+  @retval NULL                   Specified Form is not found in the formset.
+
+**/
+FORM_BROWSER_FORM *
+IdToForm (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT16                FormId
+  );
+
+#endif // _EXPRESSION_H
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/IfrParse.c b/Core/MdeModulePkg/Universal/SetupBrowserDxe/IfrParse.c
new file mode 100644
index 0000000000..8e8607d4c0
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/IfrParse.c
@@ -0,0 +1,2728 @@
+/** @file
+Parser for IFR binary encoding.
+
+Copyright (c) 2007 - 2015, 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 "Setup.h"
+
+UINT16           mStatementIndex;
+UINT16           mExpressionOpCodeIndex;
+EFI_QUESTION_ID  mUsedQuestionId;
+extern LIST_ENTRY      gBrowserStorageList;
+/**
+  Initialize Statement header members.
+
+  @param  OpCodeData             Pointer of the raw OpCode data.
+  @param  FormSet                Pointer of the current FormSe.
+  @param  Form                   Pointer of the current Form.
+
+  @return The Statement.
+
+**/
+FORM_BROWSER_STATEMENT *
+CreateStatement (
+  IN UINT8                        *OpCodeData,
+  IN OUT FORM_BROWSER_FORMSET     *FormSet,
+  IN OUT FORM_BROWSER_FORM        *Form
+  )
+{
+  FORM_BROWSER_STATEMENT    *Statement;
+  EFI_IFR_STATEMENT_HEADER  *StatementHdr;
+  INTN                      ConditionalExprCount; 
+
+  if (Form == NULL) {
+    //
+    // Only guid op may out side the form level.
+    //
+    ASSERT (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode == EFI_IFR_GUID_OP);
+  }
+
+  Statement = &FormSet->StatementBuffer[mStatementIndex];
+  mStatementIndex++;
+
+  InitializeListHead (&Statement->DefaultListHead);
+  InitializeListHead (&Statement->OptionListHead);
+  InitializeListHead (&Statement->InconsistentListHead);
+  InitializeListHead (&Statement->NoSubmitListHead);
+  InitializeListHead (&Statement->WarningListHead);
+
+  Statement->Signature = FORM_BROWSER_STATEMENT_SIGNATURE;
+
+  Statement->Operand = ((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode;
+  Statement->OpCode  = (EFI_IFR_OP_HEADER *) OpCodeData;
+
+  StatementHdr = (EFI_IFR_STATEMENT_HEADER *) (OpCodeData + sizeof (EFI_IFR_OP_HEADER));
+  CopyMem (&Statement->Prompt, &StatementHdr->Prompt, sizeof (EFI_STRING_ID));
+  CopyMem (&Statement->Help, &StatementHdr->Help, sizeof (EFI_STRING_ID));
+
+  ConditionalExprCount = GetConditionalExpressionCount(ExpressStatement);
+  if (ConditionalExprCount > 0) {
+    //
+    // Form is inside of suppressif
+    //
+    
+    Statement->Expression = (FORM_EXPRESSION_LIST *) AllocatePool( 
+                                             (UINTN) (sizeof(FORM_EXPRESSION_LIST) + ((ConditionalExprCount -1) * sizeof(FORM_EXPRESSION *))));
+    ASSERT (Statement->Expression != NULL);
+    Statement->Expression->Count     = (UINTN) ConditionalExprCount;
+    Statement->Expression->Signature = FORM_EXPRESSION_LIST_SIGNATURE;
+    CopyMem (Statement->Expression->Expression, GetConditionalExpressionList(ExpressStatement), (UINTN) (sizeof (FORM_EXPRESSION *) * ConditionalExprCount));
+  }
+
+  //
+  // Insert this Statement into current Form
+  //
+  if (Form == NULL) {
+    InsertTailList (&FormSet->StatementListOSF, &Statement->Link);
+  } else {
+    InsertTailList (&Form->StatementListHead, &Statement->Link);
+  }
+  return Statement;
+}
+
+/**
+  Convert a numeric value to a Unicode String and insert it to String Package.
+  This string is used as the Unicode Name for the EFI Variable. This is to support
+  the deprecated vareqval opcode.
+
+  @param FormSet        The FormSet.
+  @param Statement      The numeric question whose VarStoreInfo.VarName is the
+                        numeric value which is used to produce the Unicode Name
+                        for the EFI Variable.
+
+  If the Statement is NULL, the ASSERT.
+  If the opcode is not Numeric, then ASSERT.
+
+  @retval EFI_SUCCESS The funtion always succeeds.
+**/
+EFI_STATUS
+UpdateCheckBoxStringToken (
+  IN CONST FORM_BROWSER_FORMSET *FormSet,
+  IN       FORM_BROWSER_STATEMENT *Statement
+  )
+{
+  CHAR16                  Str[MAXIMUM_VALUE_CHARACTERS];
+  EFI_STRING_ID           Id;
+
+  ASSERT (Statement != NULL);
+  ASSERT (Statement->Operand == EFI_IFR_NUMERIC_OP);
+
+  UnicodeValueToString (Str, 0, Statement->VarStoreInfo.VarName, MAXIMUM_VALUE_CHARACTERS - 1);
+
+  Id = HiiSetString (FormSet->HiiHandle, 0, Str, NULL);
+  if (Id == 0) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Statement->VarStoreInfo.VarName = Id;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check if the next opcode is the EFI_IFR_EXTEND_OP_VAREQNAME.
+
+  @param OpCodeData     The current opcode.
+
+  @retval TRUE Yes.
+  @retval FALSE No.
+**/
+BOOLEAN
+IsNextOpCodeGuidedVarEqName (
+  IN UINT8 *OpCodeData
+  )
+{
+  //
+  // Get next opcode
+  //
+  OpCodeData += ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+  if (*OpCodeData == EFI_IFR_GUID_OP) {
+    if (CompareGuid (&gEfiIfrFrameworkGuid, (EFI_GUID *)(OpCodeData + sizeof (EFI_IFR_OP_HEADER)))) {
+      //
+      // Specific GUIDed opcodes to support IFR generated from Framework HII VFR
+      //
+      if ((((EFI_IFR_GUID_VAREQNAME *) OpCodeData)->ExtendOpCode) == EFI_IFR_EXTEND_OP_VAREQNAME) {
+        return TRUE;
+      }
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Initialize Question's members.
+
+  @param  OpCodeData             Pointer of the raw OpCode data.
+  @param  FormSet                Pointer of the current FormSet.
+  @param  Form                   Pointer of the current Form.
+
+  @return The Question.
+
+**/
+FORM_BROWSER_STATEMENT *
+CreateQuestion (
+  IN UINT8                        *OpCodeData,
+  IN OUT FORM_BROWSER_FORMSET     *FormSet,
+  IN OUT FORM_BROWSER_FORM        *Form
+  )
+{
+  FORM_BROWSER_STATEMENT   *Statement;
+  EFI_IFR_QUESTION_HEADER  *QuestionHdr;
+  LIST_ENTRY               *Link;
+  FORMSET_STORAGE          *Storage;
+  NAME_VALUE_NODE          *NameValueNode;
+  EFI_STATUS               Status;
+  BOOLEAN                  Find;
+
+  Statement = CreateStatement (OpCodeData, FormSet, Form);
+  if (Statement == NULL) {
+    return NULL;
+  }
+
+  QuestionHdr = (EFI_IFR_QUESTION_HEADER *) (OpCodeData + sizeof (EFI_IFR_OP_HEADER));
+  CopyMem (&Statement->QuestionId, &QuestionHdr->QuestionId, sizeof (EFI_QUESTION_ID));
+  CopyMem (&Statement->VarStoreId, &QuestionHdr->VarStoreId, sizeof (EFI_VARSTORE_ID));
+  CopyMem (&Statement->VarStoreInfo.VarOffset, &QuestionHdr->VarStoreInfo.VarOffset, sizeof (UINT16));
+
+  Statement->QuestionFlags = QuestionHdr->Flags;
+
+  if (Statement->VarStoreId == 0) {
+    //
+    // VarStoreId of zero indicates no variable storage
+    //
+    return Statement;
+  }
+
+  //
+  // Take a look at next OpCode to see whether it is a GUIDed opcode to support
+  // Framework Compatibility
+  //
+  if (FeaturePcdGet (PcdFrameworkCompatibilitySupport)) {
+    if ((*OpCodeData == EFI_IFR_NUMERIC_OP) && IsNextOpCodeGuidedVarEqName (OpCodeData)) {
+      Status = UpdateCheckBoxStringToken (FormSet, Statement);
+      if (EFI_ERROR (Status)) {
+        return NULL;
+      }
+    }
+  }
+
+  //
+  // Find Storage for this Question
+  //
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    Storage = FORMSET_STORAGE_FROM_LINK (Link);
+
+    if (Storage->VarStoreId == Statement->VarStoreId) {
+      Statement->Storage = Storage->BrowserStorage;
+      break;
+    }
+
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+  }
+  ASSERT (Statement->Storage != NULL);
+
+  //
+  // Initialilze varname for Name/Value or EFI Variable
+  //
+  if ((Statement->Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) ||
+      (Statement->Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE)) {
+    Statement->VariableName = GetToken (Statement->VarStoreInfo.VarName, FormSet->HiiHandle);
+    ASSERT (Statement->VariableName != NULL);
+
+    if (Statement->Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+      //
+      // Check whether old string node already exist.
+      //
+      Find = FALSE;
+      if (!IsListEmpty(&Statement->Storage->NameValueListHead)) {  
+        Link = GetFirstNode (&Statement->Storage->NameValueListHead);
+        while (!IsNull (&Statement->Storage->NameValueListHead, Link)) {
+          NameValueNode = NAME_VALUE_NODE_FROM_LINK (Link);
+
+          if (StrCmp (Statement->VariableName, NameValueNode->Name) == 0) {
+            Find = TRUE;
+            break;
+          }
+
+          Link = GetNextNode (&Statement->Storage->NameValueListHead, Link);
+        }
+      }
+
+      if (!Find) {
+        //
+        // Insert to Name/Value varstore list
+        //
+        NameValueNode = AllocateZeroPool (sizeof (NAME_VALUE_NODE));
+        ASSERT (NameValueNode != NULL);
+        NameValueNode->Signature = NAME_VALUE_NODE_SIGNATURE;
+        NameValueNode->Name = AllocateCopyPool (StrSize (Statement->VariableName), Statement->VariableName);
+        ASSERT (NameValueNode->Name != NULL);
+        NameValueNode->Value = AllocateZeroPool (0x10);
+        ASSERT (NameValueNode->Value != NULL);
+        NameValueNode->EditValue = AllocateZeroPool (0x10);
+        ASSERT (NameValueNode->EditValue != NULL);
+
+        InsertTailList (&Statement->Storage->NameValueListHead, &NameValueNode->Link);
+      }
+    }
+  }
+
+  return Statement;
+}
+
+
+/**
+  Allocate a FORM_EXPRESSION node.
+
+  @param  Form                   The Form associated with this Expression
+  @param  OpCode                 The binary opcode data.
+
+  @return Pointer to a FORM_EXPRESSION data structure.
+
+**/
+FORM_EXPRESSION *
+CreateExpression (
+  IN OUT FORM_BROWSER_FORM        *Form,
+  IN     UINT8                    *OpCode
+  )
+{
+  FORM_EXPRESSION  *Expression;
+
+  Expression = AllocateZeroPool (sizeof (FORM_EXPRESSION));
+  ASSERT (Expression != NULL);
+  Expression->Signature = FORM_EXPRESSION_SIGNATURE;
+  InitializeListHead (&Expression->OpCodeListHead);
+  Expression->OpCode = (EFI_IFR_OP_HEADER *) OpCode;
+
+  return Expression;
+}
+
+/**
+  Create ConfigHdr string for a storage.
+
+  @param  FormSet                Pointer of the current FormSet
+  @param  Storage                Pointer of the storage
+
+  @retval EFI_SUCCESS            Initialize ConfigHdr success
+
+**/
+EFI_STATUS
+InitializeConfigHdr (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN OUT FORMSET_STORAGE   *Storage
+  )
+{
+  CHAR16      *Name;
+
+  if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_BUFFER || 
+      Storage->BrowserStorage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+    Name = Storage->BrowserStorage->Name;
+  } else {
+    Name = NULL;
+  }
+
+  Storage->ConfigHdr = HiiConstructConfigHdr (
+                         &Storage->BrowserStorage->Guid,
+                         Name,
+                         FormSet->DriverHandle
+                         );
+
+  if (Storage->ConfigHdr == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Find the global storage link base on the input storate type, name and guid.
+
+  For EFI_HII_VARSTORE_EFI_VARIABLE and EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER,
+  same guid + name = same storage
+
+  For EFI_HII_VARSTORE_NAME_VALUE:
+  same guid + HiiHandle = same storage
+
+  For EFI_HII_VARSTORE_BUFFER:
+  same guid + name + HiiHandle = same storage
+
+  @param  StorageType                Storage type.
+  @param  StorageGuid                Storage guid.
+  @param  StorageName                Storage Name.
+  @param  HiiHandle                  HiiHandle for this varstore.
+
+  @return Pointer to a GLOBAL_STORAGE data structure.
+
+**/
+BROWSER_STORAGE *
+FindStorageInList (
+  IN UINT8                 StorageType,
+  IN EFI_GUID              *StorageGuid,
+  IN CHAR16                *StorageName,
+  IN EFI_HII_HANDLE        HiiHandle
+  )
+{
+  LIST_ENTRY       *Link;
+  BROWSER_STORAGE  *BrowserStorage;
+
+  Link  = GetFirstNode (&gBrowserStorageList);
+  while (!IsNull (&gBrowserStorageList, Link)) {
+    BrowserStorage = BROWSER_STORAGE_FROM_LINK (Link);
+    Link = GetNextNode (&gBrowserStorageList, Link);
+
+    if ((BrowserStorage->Type == StorageType) && CompareGuid (&BrowserStorage->Guid, StorageGuid)) {
+      if (StorageType == EFI_HII_VARSTORE_NAME_VALUE) {
+        if (BrowserStorage->HiiHandle == HiiHandle) {
+          return BrowserStorage;
+        }
+
+        continue;
+      }
+
+      ASSERT (StorageName != NULL);
+      if (StrCmp (BrowserStorage->Name, StorageName) == 0) {
+        if (StorageType == EFI_HII_VARSTORE_EFI_VARIABLE || StorageType == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+          return BrowserStorage;
+        } else if (StorageType == EFI_HII_VARSTORE_BUFFER && BrowserStorage->HiiHandle == HiiHandle) {
+          return BrowserStorage;
+        }
+      }
+    }
+  }
+
+  return NULL;
+}
+
+/**
+  Intialize the Global Storage.
+
+  @param  BrowserStorage              Pointer to the global storage.
+  @param  StorageType                Storage type.
+  @param  OpCodeData                 Binary data for this opcode.
+
+**/
+VOID
+IntializeBrowserStorage (
+  IN BROWSER_STORAGE       *BrowserStorage,
+  IN UINT8                 StorageType,
+  IN UINT8                 *OpCodeData
+  )
+{
+  switch (StorageType) {
+    case EFI_HII_VARSTORE_BUFFER:
+      CopyMem (&BrowserStorage->Guid, &((EFI_IFR_VARSTORE *) OpCodeData)->Guid, sizeof (EFI_GUID));
+      CopyMem (&BrowserStorage->Size, &((EFI_IFR_VARSTORE *) OpCodeData)->Size, sizeof (UINT16));
+
+      BrowserStorage->Buffer     = AllocateZeroPool (BrowserStorage->Size);
+      BrowserStorage->EditBuffer = AllocateZeroPool (BrowserStorage->Size);
+      break;
+
+    case EFI_HII_VARSTORE_EFI_VARIABLE:
+    case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+      CopyMem (&BrowserStorage->Guid,       &((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Guid,       sizeof (EFI_GUID));
+      CopyMem (&BrowserStorage->Attributes, &((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Attributes, sizeof (UINT32));
+      CopyMem (&BrowserStorage->Size,       &((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Size,       sizeof (UINT16));
+
+      if (StorageType ==  EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+        BrowserStorage->Buffer     = AllocateZeroPool (BrowserStorage->Size);
+        BrowserStorage->EditBuffer = AllocateZeroPool (BrowserStorage->Size);
+      }
+      break;
+
+    case EFI_HII_VARSTORE_NAME_VALUE:
+      CopyMem (&BrowserStorage->Guid, &((EFI_IFR_VARSTORE_NAME_VALUE *) OpCodeData)->Guid, sizeof (EFI_GUID));
+
+      InitializeListHead (&BrowserStorage->NameValueListHead);
+      break;
+
+    default:
+      break;
+  }
+}
+
+/**
+  Check whether exist device path info in the ConfigHdr string.
+
+  @param  String                 UEFI configuration string
+
+  @retval TRUE                   Device Path exist.
+  @retval FALSE                  Not exist device path info.
+
+**/
+BOOLEAN
+IsDevicePathExist (
+  IN  EFI_STRING                   String
+  )
+{
+  UINTN                    Length;
+
+  for (; (*String != 0 && StrnCmp (String, L"PATH=", StrLen (L"PATH=")) != 0); String++);
+  if (*String == 0) {
+    return FALSE;
+  }
+
+  String += StrLen (L"PATH=");
+  if (*String == 0) {
+    return FALSE;
+  }
+
+  for (Length = 0; *String != 0 && *String != L'&'; String++, Length++);
+  if (((Length + 1) / 2) < sizeof (EFI_DEVICE_PATH_PROTOCOL)) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  Allocate a FORMSET_STORAGE data structure and insert to FormSet Storage List.
+
+  @param  FormSet                    Pointer of the current FormSet
+  @param  StorageType                Storage type.
+  @param  OpCodeData                 Binary data for this opcode.
+
+  @return Pointer to a FORMSET_STORAGE data structure.
+
+**/
+FORMSET_STORAGE *
+CreateStorage (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN UINT8                 StorageType,
+  IN UINT8                 *OpCodeData
+  )
+{
+  FORMSET_STORAGE         *Storage;
+  CHAR16                  *UnicodeString;
+  UINT16                  Index;
+  BROWSER_STORAGE         *BrowserStorage;
+  EFI_GUID                *StorageGuid;
+  CHAR8                   *StorageName;
+
+  UnicodeString = NULL;
+  StorageName   = NULL;
+  switch (StorageType) {
+    case EFI_HII_VARSTORE_BUFFER:
+      StorageGuid = (EFI_GUID *) (CHAR8*) &((EFI_IFR_VARSTORE *) OpCodeData)->Guid;
+      StorageName = (CHAR8 *) ((EFI_IFR_VARSTORE *) OpCodeData)->Name;
+      break;
+
+    case EFI_HII_VARSTORE_EFI_VARIABLE:
+    case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+      StorageGuid = (EFI_GUID *) (CHAR8*) &((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Guid;
+      StorageName = (CHAR8 *) ((EFI_IFR_VARSTORE_EFI *) OpCodeData)->Name;
+      break;
+
+    default:
+      ASSERT (StorageType == EFI_HII_VARSTORE_NAME_VALUE);
+      StorageGuid = &((EFI_IFR_VARSTORE_NAME_VALUE *) OpCodeData)->Guid;
+      break;
+  }
+
+  if (StorageType != EFI_HII_VARSTORE_NAME_VALUE) {
+    ASSERT (StorageName != NULL);
+
+    UnicodeString = AllocateZeroPool (AsciiStrSize (StorageName) * 2);
+    ASSERT (UnicodeString != NULL);
+    for (Index = 0; StorageName[Index] != 0; Index++) {
+      UnicodeString[Index] = (CHAR16) StorageName[Index];
+    }
+  }
+
+  Storage = AllocateZeroPool (sizeof (FORMSET_STORAGE));
+  ASSERT (Storage != NULL);
+  Storage->Signature = FORMSET_STORAGE_SIGNATURE;
+  InsertTailList (&FormSet->StorageListHead, &Storage->Link);
+
+  BrowserStorage = FindStorageInList(StorageType, StorageGuid, UnicodeString, FormSet->HiiHandle);
+  if (BrowserStorage == NULL) {
+    BrowserStorage = AllocateZeroPool (sizeof (BROWSER_STORAGE));
+    ASSERT (BrowserStorage != NULL);
+
+    BrowserStorage->Signature = BROWSER_STORAGE_SIGNATURE;
+    InsertTailList (&gBrowserStorageList, &BrowserStorage->Link);
+
+    IntializeBrowserStorage (BrowserStorage, StorageType, OpCodeData);
+    BrowserStorage->Type = StorageType;
+    if (StorageType != EFI_HII_VARSTORE_NAME_VALUE) {
+      BrowserStorage->Name = UnicodeString;
+    }
+
+    BrowserStorage->HiiHandle = FormSet->HiiHandle;
+
+    BrowserStorage->Initialized = FALSE;
+  }
+
+  Storage->BrowserStorage = BrowserStorage;
+  InitializeConfigHdr (FormSet, Storage);
+  Storage->ConfigRequest = AllocateCopyPool (StrSize (Storage->ConfigHdr), Storage->ConfigHdr);
+  Storage->SpareStrLen = 0;
+
+  return Storage;
+}
+
+/**
+  Get Formset_storage base on the input varstoreid info.
+
+  @param  FormSet                Pointer of the current FormSet.
+  @param  VarStoreId             Varstore ID info.
+
+  @return Pointer to a FORMSET_STORAGE data structure.
+
+**/
+FORMSET_STORAGE *
+GetFstStgFromVarId (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN EFI_VARSTORE_ID       VarStoreId
+  )
+{
+  FORMSET_STORAGE  *FormsetStorage;
+  LIST_ENTRY       *Link;
+  BOOLEAN          Found;
+
+  Found = FALSE;
+  FormsetStorage = NULL;
+  //
+  // Find Formset Storage for this Question
+  //
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    FormsetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+
+    if (FormsetStorage->VarStoreId == VarStoreId) {
+      Found = TRUE;
+      break;
+    }
+
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+  }
+
+  return Found ? FormsetStorage : NULL;
+}
+
+/**
+  Get Formset_storage base on the input browser storage.
+
+  More than one formsets may share the same browser storage,
+  this function just get the first formset storage which
+  share the browser storage.
+
+  @param  Storage              browser storage info.
+
+  @return Pointer to a FORMSET_STORAGE data structure.
+  
+
+**/
+FORMSET_STORAGE *
+GetFstStgFromBrsStg (
+  IN BROWSER_STORAGE       *Storage
+  )
+{
+  FORMSET_STORAGE      *FormsetStorage;
+  LIST_ENTRY           *Link;
+  LIST_ENTRY           *FormsetLink;
+  FORM_BROWSER_FORMSET *FormSet;
+  BOOLEAN              Found;
+
+  Found = FALSE;
+  FormsetStorage = NULL;
+
+  FormsetLink = GetFirstNode (&gBrowserFormSetList);
+  while (!IsNull (&gBrowserFormSetList, FormsetLink)) {
+    FormSet = FORM_BROWSER_FORMSET_FROM_LINK (FormsetLink);
+    FormsetLink = GetNextNode (&gBrowserFormSetList, FormsetLink);
+
+    Link = GetFirstNode (&FormSet->StorageListHead);
+    while (!IsNull (&FormSet->StorageListHead, Link)) {
+      FormsetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+      Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+      if (FormsetStorage->BrowserStorage == Storage) {
+        Found = TRUE;
+        break;
+      }
+    }
+
+    if (Found) {
+      break;
+    }
+  }
+
+  return Found ? FormsetStorage : NULL;
+}
+
+/**
+  Initialize Request Element of a Question. <RequestElement> ::= '&'<BlockName> | '&'<Label>
+
+  @param  FormSet                Pointer of the current FormSet.
+  @param  Question               The Question to be initialized.
+  @param  Form                   Pointer of the current form.
+
+  @retval EFI_SUCCESS            Function success.
+  @retval EFI_INVALID_PARAMETER  No storage associated with the Question.
+
+**/
+EFI_STATUS
+InitializeRequestElement (
+  IN OUT FORM_BROWSER_FORMSET     *FormSet,
+  IN OUT FORM_BROWSER_STATEMENT   *Question,
+  IN OUT FORM_BROWSER_FORM        *Form
+  )
+{
+  BROWSER_STORAGE  *Storage;
+  FORMSET_STORAGE  *FormsetStorage;
+  UINTN            StrLen;
+  UINTN            StringSize;
+  CHAR16           *NewStr;
+  CHAR16           RequestElement[30];
+  LIST_ENTRY       *Link;
+  BOOLEAN          Find;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+  UINTN            MaxLen;
+
+  Storage = Question->Storage;
+  if (Storage == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+    //
+    // <ConfigRequest> is unnecessary for EFI variable storage,
+    // GetVariable()/SetVariable() will be used to retrieve/save values
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Prepare <RequestElement>
+  //
+  if (Storage->Type == EFI_HII_VARSTORE_BUFFER || 
+      Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+    StrLen = UnicodeSPrint (
+               RequestElement,
+               30 * sizeof (CHAR16),
+               L"&OFFSET=%04x&WIDTH=%04x",
+               Question->VarStoreInfo.VarOffset,
+               Question->StorageWidth
+               );
+    HiiToLower(RequestElement);
+    Question->BlockName = AllocateCopyPool ((StrLen + 1) * sizeof (CHAR16), RequestElement);
+  } else {
+    StrLen = UnicodeSPrint (RequestElement, 30 * sizeof (CHAR16), L"&%s", Question->VariableName);
+  }
+
+  if ((Question->Operand == EFI_IFR_PASSWORD_OP) && ((Question->QuestionFlags & EFI_IFR_FLAG_CALLBACK) == EFI_IFR_FLAG_CALLBACK)) {
+    //
+    // Password with CALLBACK flag is stored in encoded format,
+    // so don't need to append it to <ConfigRequest>
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Find Formset Storage for this Question
+  //
+  FormsetStorage = GetFstStgFromVarId(FormSet, Question->VarStoreId);
+  ASSERT (FormsetStorage != NULL);
+  StringSize = (FormsetStorage->ConfigRequest != NULL) ? StrSize (FormsetStorage->ConfigRequest) : sizeof (CHAR16);
+  MaxLen = StringSize / sizeof (CHAR16) + FormsetStorage->SpareStrLen;
+
+  //
+  // Append <RequestElement> to <ConfigRequest>
+  //
+  if (StrLen > FormsetStorage->SpareStrLen) {
+    //
+    // Old String buffer is not sufficient for RequestElement, allocate a new one
+    //
+    MaxLen = StringSize / sizeof (CHAR16) + CONFIG_REQUEST_STRING_INCREMENTAL;
+    NewStr = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+    ASSERT (NewStr != NULL);
+    if (FormsetStorage->ConfigRequest != NULL) {
+      CopyMem (NewStr, FormsetStorage->ConfigRequest, StringSize);
+      FreePool (FormsetStorage->ConfigRequest);
+    }
+    FormsetStorage->ConfigRequest = NewStr;
+    FormsetStorage->SpareStrLen   = CONFIG_REQUEST_STRING_INCREMENTAL;
+  }
+
+  StrCatS (FormsetStorage->ConfigRequest, MaxLen, RequestElement);
+  FormsetStorage->ElementCount++;
+  FormsetStorage->SpareStrLen -= StrLen;
+
+  //
+  // Update the Config Request info saved in the form.
+  //
+  ConfigInfo = NULL;
+  Find       = FALSE;
+  Link = GetFirstNode (&Form->ConfigRequestHead);
+  while (!IsNull (&Form->ConfigRequestHead, Link)) {
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+
+    if (ConfigInfo != NULL && ConfigInfo->Storage == FormsetStorage->BrowserStorage) {
+      Find = TRUE;
+      break;
+    }
+
+    Link = GetNextNode (&Form->ConfigRequestHead, Link);
+  }
+
+  if (!Find) {
+    ConfigInfo = AllocateZeroPool(sizeof (FORM_BROWSER_CONFIG_REQUEST));
+    ASSERT (ConfigInfo != NULL);
+    ConfigInfo->Signature     = FORM_BROWSER_CONFIG_REQUEST_SIGNATURE;
+    ConfigInfo->ConfigRequest = AllocateCopyPool (StrSize (FormsetStorage->ConfigHdr), FormsetStorage->ConfigHdr);
+    ASSERT (ConfigInfo->ConfigRequest != NULL);
+    ConfigInfo->SpareStrLen   = 0;
+    ConfigInfo->Storage       = FormsetStorage->BrowserStorage;
+    InsertTailList(&Form->ConfigRequestHead, &ConfigInfo->Link);
+  }
+  StringSize = (ConfigInfo->ConfigRequest != NULL) ? StrSize (ConfigInfo->ConfigRequest) : sizeof (CHAR16);
+  MaxLen = StringSize / sizeof (CHAR16) + ConfigInfo->SpareStrLen;
+
+  //
+  // Append <RequestElement> to <ConfigRequest>
+  //
+  if (StrLen > ConfigInfo->SpareStrLen) {
+    //
+    // Old String buffer is not sufficient for RequestElement, allocate a new one
+    //
+    MaxLen = StringSize / sizeof (CHAR16) + CONFIG_REQUEST_STRING_INCREMENTAL;
+    NewStr = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+    ASSERT (NewStr != NULL);
+    if (ConfigInfo->ConfigRequest != NULL) {
+      CopyMem (NewStr, ConfigInfo->ConfigRequest, StringSize);
+      FreePool (ConfigInfo->ConfigRequest);
+    }
+    ConfigInfo->ConfigRequest = NewStr;
+    ConfigInfo->SpareStrLen   = CONFIG_REQUEST_STRING_INCREMENTAL;
+  }
+
+  StrCatS (ConfigInfo->ConfigRequest, MaxLen, RequestElement);
+  ConfigInfo->ElementCount++;
+  ConfigInfo->SpareStrLen -= StrLen;
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Free resources of a Expression.
+
+  @param  FormSet                Pointer of the Expression
+
+**/
+VOID
+DestroyExpression (
+  IN FORM_EXPRESSION   *Expression
+  )
+{
+  LIST_ENTRY         *Link;
+  EXPRESSION_OPCODE  *OpCode;
+  LIST_ENTRY         *SubExpressionLink;
+  FORM_EXPRESSION    *SubExpression;
+
+  while (!IsListEmpty (&Expression->OpCodeListHead)) {
+    Link = GetFirstNode (&Expression->OpCodeListHead);
+    OpCode = EXPRESSION_OPCODE_FROM_LINK (Link);
+    RemoveEntryList (&OpCode->Link);
+
+    if (OpCode->ValueList != NULL) {
+      FreePool (OpCode->ValueList);
+    }
+
+    if (OpCode->ValueName != NULL) {
+      FreePool (OpCode->ValueName);
+    }
+
+    if (OpCode->MapExpressionList.ForwardLink != NULL) {
+      while (!IsListEmpty (&OpCode->MapExpressionList)) {
+        SubExpressionLink = GetFirstNode(&OpCode->MapExpressionList);
+        SubExpression     = FORM_EXPRESSION_FROM_LINK (SubExpressionLink);
+        RemoveEntryList(&SubExpression->Link);
+        DestroyExpression (SubExpression);
+      }
+    }
+  }
+
+  //
+  // Free this Expression
+  //
+  FreePool (Expression);
+}
+
+/**
+  Free resources of a storage.
+
+  @param  Storage                Pointer of the storage
+
+**/
+VOID
+DestroyStorage (
+  IN FORMSET_STORAGE   *Storage
+  )
+{
+  if (Storage == NULL) {
+    return;
+  }
+
+  if (Storage->ConfigRequest != NULL) {
+    FreePool (Storage->ConfigRequest);
+  }
+
+  FreePool (Storage);
+}
+
+
+/**
+  Free resources of a Statement.
+
+  @param  FormSet                Pointer of the FormSet
+  @param  Statement              Pointer of the Statement
+
+**/
+VOID
+DestroyStatement (
+  IN     FORM_BROWSER_FORMSET    *FormSet,
+  IN OUT FORM_BROWSER_STATEMENT  *Statement
+  )
+{
+  LIST_ENTRY        *Link;
+  QUESTION_DEFAULT  *Default;
+  QUESTION_OPTION   *Option;
+  FORM_EXPRESSION   *Expression;
+
+  //
+  // Free Default value List
+  //
+  while (!IsListEmpty (&Statement->DefaultListHead)) {
+    Link = GetFirstNode (&Statement->DefaultListHead);
+    Default = QUESTION_DEFAULT_FROM_LINK (Link);
+    RemoveEntryList (&Default->Link);
+
+    if (Default->Value.Buffer != NULL) {
+      FreePool (Default->Value.Buffer);
+    }
+    FreePool (Default);
+  }
+
+  //
+  // Free Options List
+  //
+  while (!IsListEmpty (&Statement->OptionListHead)) {
+    Link = GetFirstNode (&Statement->OptionListHead);
+    Option = QUESTION_OPTION_FROM_LINK (Link);
+    if (Option->SuppressExpression != NULL) {
+      FreePool (Option->SuppressExpression);
+    }
+    RemoveEntryList (&Option->Link);
+
+    FreePool (Option);
+  }
+
+  //
+  // Free Inconsistent List
+  //
+  while (!IsListEmpty (&Statement->InconsistentListHead)) {
+    Link = GetFirstNode (&Statement->InconsistentListHead);
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    RemoveEntryList (&Expression->Link);
+
+    DestroyExpression (Expression);
+  }
+
+  //
+  // Free NoSubmit List
+  //
+  while (!IsListEmpty (&Statement->NoSubmitListHead)) {
+    Link = GetFirstNode (&Statement->NoSubmitListHead);
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    RemoveEntryList (&Expression->Link);
+
+    DestroyExpression (Expression);
+  }
+
+  //
+  // Free WarningIf List
+  //
+  while (!IsListEmpty (&Statement->WarningListHead)) {
+    Link = GetFirstNode (&Statement->WarningListHead);
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    RemoveEntryList (&Expression->Link);
+
+    DestroyExpression (Expression);
+  }
+
+  if (Statement->Expression != NULL) {
+    FreePool (Statement->Expression);
+  }
+
+  if (Statement->VariableName != NULL) {
+    FreePool (Statement->VariableName);
+  }
+  if (Statement->BlockName != NULL) {
+    FreePool (Statement->BlockName);
+  }
+  if (Statement->BufferValue != NULL) {
+    FreePool (Statement->BufferValue);
+  }
+  if (Statement->Operand == EFI_IFR_STRING_OP || Statement->Operand == EFI_IFR_PASSWORD_OP) {
+    DeleteString(Statement->HiiValue.Value.string, FormSet->HiiHandle);
+  }
+}
+
+
+/**
+  Free resources of a Form.
+
+  @param  FormSet                Pointer of the FormSet
+  @param  Form                   Pointer of the Form.
+
+**/
+VOID
+DestroyForm (
+  IN     FORM_BROWSER_FORMSET  *FormSet,
+  IN OUT FORM_BROWSER_FORM     *Form
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_EXPRESSION         *Expression;
+  FORM_BROWSER_STATEMENT  *Statement;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+
+  //
+  // Free Form Expressions
+  //
+  while (!IsListEmpty (&Form->ExpressionListHead)) {
+    Link = GetFirstNode (&Form->ExpressionListHead);
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    RemoveEntryList (&Expression->Link);
+
+    DestroyExpression (Expression);
+  }
+
+  //
+  // Free Statements/Questions
+  //
+  while (!IsListEmpty (&Form->StatementListHead)) {
+    Link = GetFirstNode (&Form->StatementListHead);
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    RemoveEntryList (&Statement->Link);
+
+    DestroyStatement (FormSet, Statement);
+  }
+
+  //
+  // Free ConfigRequest string.
+  //
+  while (!IsListEmpty (&Form->ConfigRequestHead)) {
+    Link = GetFirstNode (&Form->ConfigRequestHead);
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+    RemoveEntryList (&ConfigInfo->Link);
+
+    FreePool (ConfigInfo->ConfigRequest);
+    FreePool (ConfigInfo);
+  }
+
+  if (Form->SuppressExpression != NULL) {
+    FreePool (Form->SuppressExpression);
+  }
+
+  UiFreeMenuList (&Form->FormViewListHead);
+
+  //
+  // Free this Form
+  //
+  FreePool (Form);
+}
+
+
+/**
+  Free resources allocated for a FormSet.
+
+  @param  FormSet                Pointer of the FormSet
+
+**/
+VOID
+DestroyFormSet (
+  IN OUT FORM_BROWSER_FORMSET  *FormSet
+  )
+{
+  LIST_ENTRY            *Link;
+  FORMSET_STORAGE       *Storage;
+  FORMSET_DEFAULTSTORE  *DefaultStore;
+  FORM_EXPRESSION       *Expression;
+  FORM_BROWSER_FORM     *Form;
+
+  if (FormSet->IfrBinaryData == NULL) {
+    //
+    // Uninitialized FormSet
+    //
+    FreePool (FormSet);
+    return;
+  }
+
+  //
+  // Free IFR binary buffer
+  //
+  FreePool (FormSet->IfrBinaryData);
+
+  //
+  // Free FormSet Storage
+  //
+  if (FormSet->StorageListHead.ForwardLink != NULL) {
+    while (!IsListEmpty (&FormSet->StorageListHead)) {
+      Link = GetFirstNode (&FormSet->StorageListHead);
+      Storage = FORMSET_STORAGE_FROM_LINK (Link);
+      RemoveEntryList (&Storage->Link);
+
+      DestroyStorage (Storage);
+    }
+  }
+
+  //
+  // Free FormSet Default Store
+  //
+  if (FormSet->DefaultStoreListHead.ForwardLink != NULL) {
+    while (!IsListEmpty (&FormSet->DefaultStoreListHead)) {
+      Link = GetFirstNode (&FormSet->DefaultStoreListHead);
+      DefaultStore = FORMSET_DEFAULTSTORE_FROM_LINK (Link);
+      RemoveEntryList (&DefaultStore->Link);
+
+      FreePool (DefaultStore);
+    }
+  }
+
+  //
+  // Free Formset Expressions
+  //
+  while (!IsListEmpty (&FormSet->ExpressionListHead)) {
+    Link = GetFirstNode (&FormSet->ExpressionListHead);
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    RemoveEntryList (&Expression->Link);
+
+    DestroyExpression (Expression);
+  }
+
+  //
+  // Free Forms
+  //
+  if (FormSet->FormListHead.ForwardLink != NULL) {
+    while (!IsListEmpty (&FormSet->FormListHead)) {
+      Link = GetFirstNode (&FormSet->FormListHead);
+      Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+      RemoveEntryList (&Form->Link);
+
+      DestroyForm (FormSet, Form);
+    }
+  }
+
+  if (FormSet->StatementBuffer != NULL) {
+    FreePool (FormSet->StatementBuffer);
+  }
+  if (FormSet->ExpressionBuffer != NULL) {
+    FreePool (FormSet->ExpressionBuffer);
+  }
+
+  FreePool (FormSet);
+}
+
+
+/**
+  Tell whether this Operand is an Expression OpCode or not
+
+  @param  Operand                Operand of an IFR OpCode.
+
+  @retval TRUE                   This is an Expression OpCode.
+  @retval FALSE                  Not an Expression OpCode.
+
+**/
+BOOLEAN
+IsExpressionOpCode (
+  IN UINT8              Operand
+  )
+{
+  if (((Operand >= EFI_IFR_EQ_ID_VAL_OP) && (Operand <= EFI_IFR_NOT_OP)) ||
+      ((Operand >= EFI_IFR_MATCH_OP) && (Operand <= EFI_IFR_SET_OP))  ||
+      ((Operand >= EFI_IFR_EQUAL_OP) && (Operand <= EFI_IFR_SPAN_OP)) ||
+      (Operand == EFI_IFR_CATENATE_OP) ||
+      (Operand == EFI_IFR_TO_LOWER_OP) ||
+      (Operand == EFI_IFR_TO_UPPER_OP) ||
+      (Operand == EFI_IFR_MAP_OP)      ||
+      (Operand == EFI_IFR_VERSION_OP)  ||
+      (Operand == EFI_IFR_SECURITY_OP) ||
+      (Operand == EFI_IFR_MATCH2_OP)) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Tell whether this Operand is an Statement OpCode.
+
+  @param  Operand                Operand of an IFR OpCode.
+
+  @retval TRUE                   This is an Statement OpCode.
+  @retval FALSE                  Not an Statement OpCode.
+
+**/
+BOOLEAN
+IsStatementOpCode (
+  IN UINT8              Operand
+  )
+{
+  if ((Operand == EFI_IFR_SUBTITLE_OP) ||
+      (Operand == EFI_IFR_TEXT_OP) ||
+      (Operand == EFI_IFR_RESET_BUTTON_OP) ||
+      (Operand == EFI_IFR_REF_OP) ||
+      (Operand == EFI_IFR_ACTION_OP) ||
+      (Operand == EFI_IFR_NUMERIC_OP) ||
+      (Operand == EFI_IFR_ORDERED_LIST_OP) ||
+      (Operand == EFI_IFR_CHECKBOX_OP) ||
+      (Operand == EFI_IFR_STRING_OP) ||
+      (Operand == EFI_IFR_PASSWORD_OP) ||
+      (Operand == EFI_IFR_DATE_OP) ||
+      (Operand == EFI_IFR_TIME_OP) ||
+      (Operand == EFI_IFR_GUID_OP) ||
+      (Operand == EFI_IFR_ONE_OF_OP)) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+
+/**
+  Tell whether this Operand is an known OpCode.
+
+  @param  Operand                Operand of an IFR OpCode.
+
+  @retval TRUE                   This is an Statement OpCode.
+  @retval FALSE                  Not an Statement OpCode.
+
+**/
+BOOLEAN
+IsUnKnownOpCode (
+  IN UINT8              Operand
+  )
+{
+  return Operand > EFI_IFR_MATCH2_OP ? TRUE : FALSE;
+}
+
+/**
+  Calculate number of Statemens(Questions) and Expression OpCodes.
+
+  @param  FormSet                The FormSet to be counted.
+  @param  NumberOfStatement      Number of Statemens(Questions)
+  @param  NumberOfExpression     Number of Expression OpCodes
+
+**/
+VOID
+CountOpCodes (
+  IN  FORM_BROWSER_FORMSET  *FormSet,
+  IN OUT  UINT16            *NumberOfStatement,
+  IN OUT  UINT16            *NumberOfExpression
+  )
+{
+  UINT16  StatementCount;
+  UINT16  ExpressionCount;
+  UINT8   *OpCodeData;
+  UINTN   Offset;
+  UINTN   OpCodeLen;
+
+  Offset = 0;
+  StatementCount = 0;
+  ExpressionCount = 0;
+
+  while (Offset < FormSet->IfrBinaryLength) {
+    OpCodeData = FormSet->IfrBinaryData + Offset;
+    OpCodeLen = ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+    Offset += OpCodeLen;
+
+    if (IsExpressionOpCode (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode)) {
+      ExpressionCount++;
+    } else {
+      StatementCount++;
+    }
+  }
+
+  *NumberOfStatement = StatementCount;
+  *NumberOfExpression = ExpressionCount;
+}
+
+
+
+/**
+  Parse opcodes in the formset IFR binary.
+
+  @param  FormSet                Pointer of the FormSet data structure.
+
+  @retval EFI_SUCCESS            Opcode parse success.
+  @retval Other                  Opcode parse fail.
+
+**/
+EFI_STATUS
+ParseOpCodes (
+  IN FORM_BROWSER_FORMSET              *FormSet
+  )
+{
+  EFI_STATUS              Status;
+  FORM_BROWSER_FORM       *CurrentForm;
+  FORM_BROWSER_STATEMENT  *CurrentStatement;
+  FORM_BROWSER_STATEMENT  *ParentStatement;
+  EXPRESSION_OPCODE       *ExpressionOpCode;
+  FORM_EXPRESSION         *CurrentExpression;
+  UINT8                   Operand;
+  UINT8                   Scope;
+  UINTN                   OpCodeOffset;
+  UINTN                   OpCodeLength;
+  UINT8                   *OpCodeData;
+  UINT8                   ScopeOpCode;
+  FORMSET_STORAGE         *Storage;
+  FORMSET_DEFAULTSTORE    *DefaultStore;
+  QUESTION_DEFAULT        *CurrentDefault;
+  QUESTION_OPTION         *CurrentOption;
+  UINT8                   Width;
+  UINT16                  NumberOfStatement;
+  UINT16                  NumberOfExpression;
+  EFI_IMAGE_ID            *ImageId;
+  BOOLEAN                 SuppressForQuestion;
+  BOOLEAN                 SuppressForOption;
+  UINT16                  DepthOfDisable;
+  BOOLEAN                 OpCodeDisabled;
+  BOOLEAN                 SingleOpCodeExpression;
+  BOOLEAN                 InScopeDefault;
+  EFI_HII_VALUE           *Value;
+  EFI_IFR_FORM_MAP_METHOD *MapMethod;
+  UINT8                   MapScopeDepth;
+  LIST_ENTRY              *Link;
+  FORMSET_STORAGE         *VarStorage;
+  LIST_ENTRY              *MapExpressionList;
+  EFI_VARSTORE_ID         TempVarstoreId;
+  BOOLEAN                 InScopeDisable;
+  INTN                    ConditionalExprCount;
+  BOOLEAN                 InUnknownScope;
+  UINT8                   UnknownDepth;
+
+  SuppressForQuestion      = FALSE;
+  SuppressForOption        = FALSE;
+  InScopeDisable           = FALSE;
+  DepthOfDisable           = 0;
+  OpCodeDisabled           = FALSE;
+  SingleOpCodeExpression   = FALSE;
+  InScopeDefault           = FALSE;
+  CurrentExpression        = NULL;
+  CurrentDefault           = NULL;
+  CurrentOption            = NULL;
+  ImageId                  = NULL;
+  MapMethod                = NULL;
+  MapScopeDepth            = 0;
+  Link                     = NULL;
+  VarStorage               = NULL;
+  MapExpressionList        = NULL;
+  TempVarstoreId           = 0;
+  ConditionalExprCount     = 0;
+  InUnknownScope           = FALSE;
+  UnknownDepth             = 0;
+
+  //
+  // Get the number of Statements and Expressions
+  //
+  CountOpCodes (FormSet, &NumberOfStatement, &NumberOfExpression);
+
+  mStatementIndex = 0;
+  mUsedQuestionId = 1;
+  FormSet->StatementBuffer = AllocateZeroPool (NumberOfStatement * sizeof (FORM_BROWSER_STATEMENT));
+  if (FormSet->StatementBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  mExpressionOpCodeIndex = 0;
+  FormSet->ExpressionBuffer = AllocateZeroPool (NumberOfExpression * sizeof (EXPRESSION_OPCODE));
+  if (FormSet->ExpressionBuffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  InitializeListHead (&FormSet->StatementListOSF);
+  InitializeListHead (&FormSet->StorageListHead);
+  InitializeListHead (&FormSet->SaveFailStorageListHead);
+  InitializeListHead (&FormSet->DefaultStoreListHead);
+  InitializeListHead (&FormSet->FormListHead);
+  InitializeListHead (&FormSet->ExpressionListHead);
+  ResetCurrentExpressionStack ();
+  ResetMapExpressionListStack ();
+
+  CurrentForm = NULL;
+  CurrentStatement = NULL;
+  ParentStatement  = NULL;
+
+  ResetScopeStack ();
+
+  OpCodeOffset = 0;
+  while (OpCodeOffset < FormSet->IfrBinaryLength) {
+    OpCodeData = FormSet->IfrBinaryData + OpCodeOffset;
+
+    OpCodeLength = ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+    OpCodeOffset += OpCodeLength;
+    Operand = ((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode;
+    Scope = ((EFI_IFR_OP_HEADER *) OpCodeData)->Scope;
+
+    if (InUnknownScope) {
+      if (Operand == EFI_IFR_END_OP) {
+        UnknownDepth --;
+
+        if (UnknownDepth == 0) {
+          InUnknownScope = FALSE;
+        }
+      } else {
+        if (Scope != 0) {
+          UnknownDepth ++;
+        }
+      }
+
+      continue;
+    }
+
+    if (IsUnKnownOpCode(Operand)) {
+      if (Scope != 0) {
+        InUnknownScope = TRUE;
+        UnknownDepth ++;
+      }
+
+      continue;
+    }
+
+    //
+    // If scope bit set, push onto scope stack
+    //
+    if (Scope != 0) {
+      PushScope (Operand);
+    }
+
+    if (OpCodeDisabled) {
+      //
+      // DisableIf Expression is evaluated to be TRUE, try to find its end.
+      // Here only cares the EFI_IFR_DISABLE_IF and EFI_IFR_END
+      //
+      if (Operand == EFI_IFR_DISABLE_IF_OP) {
+        DepthOfDisable++;
+      } else if (Operand == EFI_IFR_END_OP) {
+        Status = PopScope (&ScopeOpCode);
+        if (EFI_ERROR (Status)) {
+          return Status;
+        }
+
+        if (ScopeOpCode == EFI_IFR_DISABLE_IF_OP) {
+          if (DepthOfDisable == 0) {
+            InScopeDisable = FALSE;
+            OpCodeDisabled = FALSE;
+          } else {
+            DepthOfDisable--;
+          }
+        }
+      }
+      continue;
+    }
+
+    if (IsExpressionOpCode (Operand)) {
+      ExpressionOpCode = &FormSet->ExpressionBuffer[mExpressionOpCodeIndex];
+      mExpressionOpCodeIndex++;
+
+      ExpressionOpCode->Signature = EXPRESSION_OPCODE_SIGNATURE;
+      ExpressionOpCode->Operand = Operand;
+      Value = &ExpressionOpCode->Value;
+
+      switch (Operand) {
+      case EFI_IFR_EQ_ID_VAL_OP:
+        CopyMem (&ExpressionOpCode->QuestionId, &((EFI_IFR_EQ_ID_VAL *) OpCodeData)->QuestionId, sizeof (EFI_QUESTION_ID));
+
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_16;
+        CopyMem (&Value->Value.u16, &((EFI_IFR_EQ_ID_VAL *) OpCodeData)->Value, sizeof (UINT16));
+        break;
+
+      case EFI_IFR_EQ_ID_ID_OP:
+        CopyMem (&ExpressionOpCode->QuestionId, &((EFI_IFR_EQ_ID_ID *) OpCodeData)->QuestionId1, sizeof (EFI_QUESTION_ID));
+        CopyMem (&ExpressionOpCode->QuestionId2, &((EFI_IFR_EQ_ID_ID *) OpCodeData)->QuestionId2, sizeof (EFI_QUESTION_ID));
+        break;
+
+      case EFI_IFR_EQ_ID_VAL_LIST_OP:
+        CopyMem (&ExpressionOpCode->QuestionId, &((EFI_IFR_EQ_ID_VAL_LIST *) OpCodeData)->QuestionId, sizeof (EFI_QUESTION_ID));
+        CopyMem (&ExpressionOpCode->ListLength, &((EFI_IFR_EQ_ID_VAL_LIST *) OpCodeData)->ListLength, sizeof (UINT16));
+        ExpressionOpCode->ValueList = AllocateCopyPool (ExpressionOpCode->ListLength * sizeof (UINT16), &((EFI_IFR_EQ_ID_VAL_LIST *) OpCodeData)->ValueList);
+        break;
+
+      case EFI_IFR_TO_STRING_OP:
+      case EFI_IFR_FIND_OP:
+        ExpressionOpCode->Format = (( EFI_IFR_TO_STRING *) OpCodeData)->Format;
+        break;
+
+      case EFI_IFR_STRING_REF1_OP:
+        Value->Type = EFI_IFR_TYPE_STRING;
+        CopyMem (&Value->Value.string, &(( EFI_IFR_STRING_REF1 *) OpCodeData)->StringId, sizeof (EFI_STRING_ID));
+        break;
+
+      case EFI_IFR_RULE_REF_OP:
+        ExpressionOpCode->RuleId = (( EFI_IFR_RULE_REF *) OpCodeData)->RuleId;
+        break;
+
+      case EFI_IFR_SPAN_OP:
+        ExpressionOpCode->Flags = (( EFI_IFR_SPAN *) OpCodeData)->Flags;
+        break;
+
+      case EFI_IFR_THIS_OP:
+        ASSERT (ParentStatement != NULL);
+        ExpressionOpCode->QuestionId = ParentStatement->QuestionId;
+        break;
+
+      case EFI_IFR_SECURITY_OP:
+        CopyMem (&ExpressionOpCode->Guid, &((EFI_IFR_SECURITY *) OpCodeData)->Permissions, sizeof (EFI_GUID));
+        break;
+
+      case EFI_IFR_MATCH2_OP:
+        CopyMem (&ExpressionOpCode->Guid, &((EFI_IFR_MATCH2 *) OpCodeData)->SyntaxType, sizeof (EFI_GUID));
+        break;
+
+      case EFI_IFR_GET_OP:
+      case EFI_IFR_SET_OP:
+        CopyMem (&TempVarstoreId, &((EFI_IFR_GET *) OpCodeData)->VarStoreId, sizeof (TempVarstoreId));
+        if (TempVarstoreId != 0) {
+          if (FormSet->StorageListHead.ForwardLink != NULL) {
+            Link = GetFirstNode (&FormSet->StorageListHead);
+            while (!IsNull (&FormSet->StorageListHead, Link)) {
+              VarStorage = FORMSET_STORAGE_FROM_LINK (Link);
+              if (VarStorage->VarStoreId == ((EFI_IFR_GET *) OpCodeData)->VarStoreId) {
+                ExpressionOpCode->VarStorage = VarStorage->BrowserStorage;
+                break;
+              }
+              Link = GetNextNode (&FormSet->StorageListHead, Link);
+            }
+          }
+          if (ExpressionOpCode->VarStorage == NULL) {
+            //
+            // VarStorage is not found.
+            //
+            return EFI_INVALID_PARAMETER;
+          }
+        }
+        ExpressionOpCode->ValueType = ((EFI_IFR_GET *) OpCodeData)->VarStoreType;
+        switch (ExpressionOpCode->ValueType) {
+        case EFI_IFR_TYPE_BOOLEAN:
+        case EFI_IFR_TYPE_NUM_SIZE_8: 
+          ExpressionOpCode->ValueWidth = 1;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_16:
+        case EFI_IFR_TYPE_STRING:
+          ExpressionOpCode->ValueWidth = 2;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_32:
+          ExpressionOpCode->ValueWidth = 4;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_64:
+          ExpressionOpCode->ValueWidth = 8;
+          break;
+
+        case EFI_IFR_TYPE_DATE:
+          ExpressionOpCode->ValueWidth = (UINT8) sizeof (EFI_IFR_DATE);
+          break;
+
+        case EFI_IFR_TYPE_TIME:
+          ExpressionOpCode->ValueWidth = (UINT8) sizeof (EFI_IFR_TIME);
+          break;
+
+        case EFI_IFR_TYPE_REF:
+          ExpressionOpCode->ValueWidth = (UINT8) sizeof (EFI_IFR_REF);
+          break;
+
+        case EFI_IFR_TYPE_OTHER:
+        case EFI_IFR_TYPE_UNDEFINED:
+        case EFI_IFR_TYPE_ACTION:
+        case EFI_IFR_TYPE_BUFFER:
+        default:
+          //
+          // Invalid value type for Get/Set opcode.
+          //
+          return EFI_INVALID_PARAMETER;
+        }
+        CopyMem (&ExpressionOpCode->VarStoreInfo.VarName,   &((EFI_IFR_GET *) OpCodeData)->VarStoreInfo.VarName,   sizeof (EFI_STRING_ID));
+        CopyMem (&ExpressionOpCode->VarStoreInfo.VarOffset, &((EFI_IFR_GET *) OpCodeData)->VarStoreInfo.VarOffset, sizeof (UINT16));
+        if ((ExpressionOpCode->VarStorage != NULL) && 
+            (ExpressionOpCode->VarStorage->Type == EFI_HII_VARSTORE_NAME_VALUE || 
+             ExpressionOpCode->VarStorage->Type == EFI_HII_VARSTORE_EFI_VARIABLE)) {
+          ExpressionOpCode->ValueName = GetToken (ExpressionOpCode->VarStoreInfo.VarName, FormSet->HiiHandle);
+          if (ExpressionOpCode->ValueName == NULL) {
+            //
+            // String ID is invalid.
+            //
+            return EFI_INVALID_PARAMETER;
+          }
+        }
+        break;
+
+      case EFI_IFR_QUESTION_REF1_OP:
+        CopyMem (&ExpressionOpCode->QuestionId, &((EFI_IFR_EQ_ID_VAL_LIST *) OpCodeData)->QuestionId, sizeof (EFI_QUESTION_ID));
+        break;
+
+      case EFI_IFR_QUESTION_REF3_OP:
+        if (OpCodeLength >= sizeof (EFI_IFR_QUESTION_REF3_2)) {
+          CopyMem (&ExpressionOpCode->DevicePath, &(( EFI_IFR_QUESTION_REF3_2 *) OpCodeData)->DevicePath, sizeof (EFI_STRING_ID));
+
+          if (OpCodeLength >= sizeof (EFI_IFR_QUESTION_REF3_3)) {
+            CopyMem (&ExpressionOpCode->Guid, &(( EFI_IFR_QUESTION_REF3_3 *) OpCodeData)->Guid, sizeof (EFI_GUID));
+          }
+        }
+        break;
+
+      //
+      // constant
+      //
+      case EFI_IFR_TRUE_OP:
+        Value->Type = EFI_IFR_TYPE_BOOLEAN;
+        Value->Value.b = TRUE;
+        break;
+
+      case EFI_IFR_FALSE_OP:
+        Value->Type = EFI_IFR_TYPE_BOOLEAN;
+        Value->Value.b = FALSE;
+        break;
+
+      case EFI_IFR_ONE_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+        Value->Value.u8 = 1;
+        break;
+
+      case EFI_IFR_ZERO_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+        Value->Value.u8 = 0;
+        break;
+
+      case EFI_IFR_ONES_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+        Value->Value.u64 = 0xffffffffffffffffULL;
+        break;
+
+      case EFI_IFR_UINT8_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+        Value->Value.u8 = (( EFI_IFR_UINT8 *) OpCodeData)->Value;
+        break;
+
+      case EFI_IFR_UINT16_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_16;
+        CopyMem (&Value->Value.u16, &(( EFI_IFR_UINT16 *) OpCodeData)->Value, sizeof (UINT16));
+        break;
+
+      case EFI_IFR_UINT32_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_32;
+        CopyMem (&Value->Value.u32, &(( EFI_IFR_UINT32 *) OpCodeData)->Value, sizeof (UINT32));
+        break;
+
+      case EFI_IFR_UINT64_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+        CopyMem (&Value->Value.u64, &(( EFI_IFR_UINT64 *) OpCodeData)->Value, sizeof (UINT64));
+        break;
+
+      case EFI_IFR_UNDEFINED_OP:
+        Value->Type = EFI_IFR_TYPE_UNDEFINED;
+        break;
+
+      case EFI_IFR_VERSION_OP:
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_16;
+        Value->Value.u16 = EFI_IFR_SPECIFICATION_VERSION;
+        break;
+
+      default:
+        break;
+      }
+      //
+      // Create sub expression nested in MAP opcode
+      //
+      if (CurrentExpression == NULL && MapScopeDepth > 0) {
+        CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+        ASSERT (MapExpressionList != NULL);
+        InsertTailList (MapExpressionList, &CurrentExpression->Link);
+        if (Scope == 0) {
+          SingleOpCodeExpression = TRUE;
+        }
+      }
+      ASSERT (CurrentExpression != NULL);
+      InsertTailList (&CurrentExpression->OpCodeListHead, &ExpressionOpCode->Link);
+      if (Operand == EFI_IFR_MAP_OP) {
+        //
+        // Store current Map Expression List.
+        //
+        if (MapExpressionList != NULL) {
+          PushMapExpressionList (MapExpressionList);
+        }
+        //
+        // Initialize new Map Expression List.
+        //
+        MapExpressionList = &ExpressionOpCode->MapExpressionList;
+        InitializeListHead (MapExpressionList);
+        //
+        // Store current expression.
+        //
+        PushCurrentExpression (CurrentExpression);
+        CurrentExpression = NULL;
+        MapScopeDepth ++;
+      } else if (SingleOpCodeExpression) {
+        //
+        // There are two cases to indicate the end of an Expression:
+        // for single OpCode expression: one Expression OpCode
+        // for expression consists of more than one OpCode: EFI_IFR_END
+        //
+        SingleOpCodeExpression = FALSE;
+
+        if (InScopeDisable && CurrentForm == NULL) {
+          //
+          // This is DisableIf expression for Form, it should be a constant expression
+          //
+          Status = EvaluateExpression (FormSet, CurrentForm, CurrentExpression);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          OpCodeDisabled = IsTrue(&CurrentExpression->Result);
+        }
+
+        CurrentExpression = NULL;
+      }
+
+      continue;
+    }
+
+    //
+    // Parse the Opcode
+    //
+    switch (Operand) {
+
+    case EFI_IFR_FORM_SET_OP:
+      //
+      // Check the formset GUID
+      //
+      if (CompareMem (&FormSet->Guid, &((EFI_IFR_FORM_SET *) OpCodeData)->Guid, sizeof (EFI_GUID)) != 0) {
+        return EFI_INVALID_PARAMETER;
+      }
+
+      CopyMem (&FormSet->FormSetTitle, &((EFI_IFR_FORM_SET *) OpCodeData)->FormSetTitle, sizeof (EFI_STRING_ID));
+      CopyMem (&FormSet->Help,         &((EFI_IFR_FORM_SET *) OpCodeData)->Help,         sizeof (EFI_STRING_ID));
+      FormSet->OpCode = (EFI_IFR_OP_HEADER *) OpCodeData;//save the opcode address of formset
+
+      if (OpCodeLength > OFFSET_OF (EFI_IFR_FORM_SET, Flags)) {
+        //
+        // The formset OpCode contains ClassGuid
+        //
+        FormSet->NumberOfClassGuid = (UINT8) (((EFI_IFR_FORM_SET *) OpCodeData)->Flags & 0x3);
+        CopyMem (FormSet->ClassGuid, OpCodeData + sizeof (EFI_IFR_FORM_SET), FormSet->NumberOfClassGuid * sizeof (EFI_GUID));
+      }
+      break;
+
+    case EFI_IFR_FORM_OP:
+      //
+      // Create a new Form for this FormSet
+      //
+      CurrentForm = AllocateZeroPool (sizeof (FORM_BROWSER_FORM));
+      ASSERT (CurrentForm != NULL);
+      CurrentForm->Signature = FORM_BROWSER_FORM_SIGNATURE;
+      InitializeListHead (&CurrentForm->ExpressionListHead);
+      InitializeListHead (&CurrentForm->StatementListHead);
+      InitializeListHead (&CurrentForm->ConfigRequestHead);
+      InitializeListHead (&CurrentForm->FormViewListHead);
+
+      CurrentForm->FormType = STANDARD_MAP_FORM_TYPE;
+      CopyMem (&CurrentForm->FormId,    &((EFI_IFR_FORM *) OpCodeData)->FormId,    sizeof (UINT16));
+      CopyMem (&CurrentForm->FormTitle, &((EFI_IFR_FORM *) OpCodeData)->FormTitle, sizeof (EFI_STRING_ID));
+
+      ConditionalExprCount = GetConditionalExpressionCount(ExpressForm);
+      if ( ConditionalExprCount > 0) {
+        //
+        // Form is inside of suppressif
+        //
+        CurrentForm->SuppressExpression = (FORM_EXPRESSION_LIST *) AllocatePool( 
+                                                 (UINTN) (sizeof(FORM_EXPRESSION_LIST) + ((ConditionalExprCount -1) * sizeof(FORM_EXPRESSION *))));
+        ASSERT (CurrentForm->SuppressExpression != NULL);
+        CurrentForm->SuppressExpression->Count     = (UINTN) ConditionalExprCount;
+        CurrentForm->SuppressExpression->Signature = FORM_EXPRESSION_LIST_SIGNATURE;
+        CopyMem (CurrentForm->SuppressExpression->Expression, GetConditionalExpressionList(ExpressForm), (UINTN) (sizeof (FORM_EXPRESSION *) * ConditionalExprCount));
+      }
+
+      if (Scope != 0) {
+        //
+        // Enter scope of a Form, suppressif will be used for Question or Option
+        //
+        SuppressForQuestion = TRUE;
+      }
+
+      //
+      // Insert into Form list of this FormSet
+      //
+      InsertTailList (&FormSet->FormListHead, &CurrentForm->Link);
+      break;
+
+    case EFI_IFR_FORM_MAP_OP:
+      //
+      // Create a new Form for this FormSet
+      //
+      CurrentForm = AllocateZeroPool (sizeof (FORM_BROWSER_FORM));
+      ASSERT (CurrentForm != NULL);
+      CurrentForm->Signature = FORM_BROWSER_FORM_SIGNATURE;
+      InitializeListHead (&CurrentForm->ExpressionListHead);
+      InitializeListHead (&CurrentForm->StatementListHead);
+      InitializeListHead (&CurrentForm->ConfigRequestHead);
+      InitializeListHead (&CurrentForm->FormViewListHead);
+
+      CopyMem (&CurrentForm->FormId, &((EFI_IFR_FORM *) OpCodeData)->FormId, sizeof (UINT16));
+
+      MapMethod = (EFI_IFR_FORM_MAP_METHOD *) (OpCodeData + sizeof (EFI_IFR_FORM_MAP));
+      //
+      // FormMap Form must contain at least one Map Method.
+      //
+      if (((EFI_IFR_OP_HEADER *) OpCodeData)->Length < ((UINTN) (UINT8 *) (MapMethod + 1) - (UINTN) OpCodeData)) {
+        return EFI_INVALID_PARAMETER;
+      }
+      //
+      // Try to find the standard form map method.
+      //
+      while (((UINTN) (UINT8 *) MapMethod - (UINTN) OpCodeData) < ((EFI_IFR_OP_HEADER *) OpCodeData)->Length) {
+        if (CompareGuid ((EFI_GUID *) (VOID *) &MapMethod->MethodIdentifier, &gEfiHiiStandardFormGuid)) {
+          CopyMem (&CurrentForm->FormTitle, &MapMethod->MethodTitle, sizeof (EFI_STRING_ID));
+          CurrentForm->FormType = STANDARD_MAP_FORM_TYPE;
+          break;
+        }
+        MapMethod ++;
+      }
+      //
+      // If the standard form map method is not found, the first map method title will be used.
+      //
+      if (CurrentForm->FormTitle == 0) {
+        MapMethod = (EFI_IFR_FORM_MAP_METHOD *) (OpCodeData + sizeof (EFI_IFR_FORM_MAP));
+        CopyMem (&CurrentForm->FormTitle, &MapMethod->MethodTitle, sizeof (EFI_STRING_ID));
+      }
+
+      ConditionalExprCount = GetConditionalExpressionCount(ExpressForm);
+      if ( ConditionalExprCount > 0) {
+        //
+        // Form is inside of suppressif
+        //
+        CurrentForm->SuppressExpression = (FORM_EXPRESSION_LIST *) AllocatePool( 
+                                                 (UINTN) (sizeof(FORM_EXPRESSION_LIST) + ((ConditionalExprCount -1) * sizeof(FORM_EXPRESSION *))));
+        ASSERT (CurrentForm->SuppressExpression != NULL);
+        CurrentForm->SuppressExpression->Count     = (UINTN) ConditionalExprCount;
+        CurrentForm->SuppressExpression->Signature = FORM_EXPRESSION_LIST_SIGNATURE;
+        CopyMem (CurrentForm->SuppressExpression->Expression, GetConditionalExpressionList(ExpressForm), (UINTN) (sizeof (FORM_EXPRESSION *) * ConditionalExprCount));
+      }
+
+      if (Scope != 0) {
+        //
+        // Enter scope of a Form, suppressif will be used for Question or Option
+        //
+        SuppressForQuestion = TRUE;
+      }
+
+      //
+      // Insert into Form list of this FormSet
+      //
+      InsertTailList (&FormSet->FormListHead, &CurrentForm->Link);
+      break;
+
+    //
+    // Storage
+    //
+    case EFI_IFR_VARSTORE_OP:
+      //
+      // Create a buffer Storage for this FormSet
+      //
+      Storage = CreateStorage (FormSet, EFI_HII_VARSTORE_BUFFER, OpCodeData);
+      CopyMem (&Storage->VarStoreId, &((EFI_IFR_VARSTORE *) OpCodeData)->VarStoreId, sizeof (EFI_VARSTORE_ID));
+      break;
+
+    case EFI_IFR_VARSTORE_NAME_VALUE_OP:
+      //
+      // Create a name/value Storage for this FormSet
+      //
+      Storage = CreateStorage (FormSet, EFI_HII_VARSTORE_NAME_VALUE, OpCodeData);
+      CopyMem (&Storage->VarStoreId, &((EFI_IFR_VARSTORE_NAME_VALUE *) OpCodeData)->VarStoreId, sizeof (EFI_VARSTORE_ID));
+      break;
+
+    case EFI_IFR_VARSTORE_EFI_OP:
+      //
+      // Create a EFI variable Storage for this FormSet
+      //
+      if (OpCodeLength < sizeof (EFI_IFR_VARSTORE_EFI)) {
+        //
+        // Create efi varstore with format follow UEFI spec before 2.3.1.
+        //
+        Storage = CreateStorage (FormSet, EFI_HII_VARSTORE_EFI_VARIABLE, OpCodeData);
+      } else {
+        //
+        // Create efi varstore with format follow UEFI spec 2.3.1 and later.
+        //
+        Storage = CreateStorage (FormSet, EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER, OpCodeData);
+      }
+      CopyMem (&Storage->VarStoreId, &((EFI_IFR_VARSTORE_EFI *) OpCodeData)->VarStoreId, sizeof (EFI_VARSTORE_ID));
+      break;
+
+    //
+    // DefaultStore
+    //
+    case EFI_IFR_DEFAULTSTORE_OP:
+      DefaultStore = AllocateZeroPool (sizeof (FORMSET_DEFAULTSTORE));
+      ASSERT (DefaultStore != NULL);
+      DefaultStore->Signature = FORMSET_DEFAULTSTORE_SIGNATURE;
+
+      CopyMem (&DefaultStore->DefaultId,   &((EFI_IFR_DEFAULTSTORE *) OpCodeData)->DefaultId,   sizeof (UINT16));
+      CopyMem (&DefaultStore->DefaultName, &((EFI_IFR_DEFAULTSTORE *) OpCodeData)->DefaultName, sizeof (EFI_STRING_ID));
+
+      //
+      // Insert to DefaultStore list of this Formset
+      //
+      InsertTailList (&FormSet->DefaultStoreListHead, &DefaultStore->Link);
+      break;
+
+    //
+    // Statements
+    //
+    case EFI_IFR_SUBTITLE_OP:
+      CurrentStatement = CreateStatement (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_SUBTITLE *) OpCodeData)->Flags;
+      CurrentStatement->FakeQuestionId = mUsedQuestionId++;
+      break;
+
+    case EFI_IFR_TEXT_OP:
+      CurrentStatement = CreateStatement (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      CurrentStatement->FakeQuestionId = mUsedQuestionId++;
+      CopyMem (&CurrentStatement->TextTwo, &((EFI_IFR_TEXT *) OpCodeData)->TextTwo, sizeof (EFI_STRING_ID));
+      break;
+
+    case EFI_IFR_RESET_BUTTON_OP:
+      CurrentStatement = CreateStatement (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      CurrentStatement->FakeQuestionId = mUsedQuestionId++;
+      CopyMem (&CurrentStatement->DefaultId, &((EFI_IFR_RESET_BUTTON *) OpCodeData)->DefaultId, sizeof (EFI_DEFAULT_ID));
+      break;
+
+    //
+    // Questions
+    //
+    case EFI_IFR_ACTION_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_ACTION;
+
+      if (OpCodeLength == sizeof (EFI_IFR_ACTION_1)) {
+        //
+        // No QuestionConfig present, so no configuration string will be processed
+        //
+        CurrentStatement->QuestionConfig = 0;
+      } else {
+        CopyMem (&CurrentStatement->QuestionConfig, &((EFI_IFR_ACTION *) OpCodeData)->QuestionConfig, sizeof (EFI_STRING_ID));
+      }
+      break;
+
+    case EFI_IFR_REF_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      Value = &CurrentStatement->HiiValue;
+      Value->Type = EFI_IFR_TYPE_REF;
+      if (OpCodeLength >= sizeof (EFI_IFR_REF)) {
+        CopyMem (&Value->Value.ref.FormId, &((EFI_IFR_REF *) OpCodeData)->FormId, sizeof (EFI_FORM_ID));
+
+        if (OpCodeLength >= sizeof (EFI_IFR_REF2)) {
+          CopyMem (&Value->Value.ref.QuestionId, &((EFI_IFR_REF2 *) OpCodeData)->QuestionId, sizeof (EFI_QUESTION_ID));
+
+          if (OpCodeLength >= sizeof (EFI_IFR_REF3)) {
+            CopyMem (&Value->Value.ref.FormSetGuid, &((EFI_IFR_REF3 *) OpCodeData)->FormSetId, sizeof (EFI_GUID));
+
+            if (OpCodeLength >= sizeof (EFI_IFR_REF4)) {
+              CopyMem (&Value->Value.ref.DevicePath, &((EFI_IFR_REF4 *) OpCodeData)->DevicePath, sizeof (EFI_STRING_ID));
+            }
+          }
+        }
+      }
+      CurrentStatement->StorageWidth = (UINT16) sizeof (EFI_HII_REF);        
+      InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+      break;
+
+    case EFI_IFR_ONE_OF_OP:
+    case EFI_IFR_NUMERIC_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT(CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_ONE_OF *) OpCodeData)->Flags;
+      Value = &CurrentStatement->HiiValue;
+
+      switch (CurrentStatement->Flags & EFI_IFR_NUMERIC_SIZE) {
+      case EFI_IFR_NUMERIC_SIZE_1:
+        CurrentStatement->Minimum = ((EFI_IFR_NUMERIC *) OpCodeData)->data.u8.MinValue;
+        CurrentStatement->Maximum = ((EFI_IFR_NUMERIC *) OpCodeData)->data.u8.MaxValue;
+        CurrentStatement->Step    = ((EFI_IFR_NUMERIC *) OpCodeData)->data.u8.Step;
+        CurrentStatement->StorageWidth = (UINT16) sizeof (UINT8);
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_8;
+        break;
+
+      case EFI_IFR_NUMERIC_SIZE_2:
+        CopyMem (&CurrentStatement->Minimum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u16.MinValue, sizeof (UINT16));
+        CopyMem (&CurrentStatement->Maximum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u16.MaxValue, sizeof (UINT16));
+        CopyMem (&CurrentStatement->Step,    &((EFI_IFR_NUMERIC *) OpCodeData)->data.u16.Step,     sizeof (UINT16));
+        CurrentStatement->StorageWidth = (UINT16) sizeof (UINT16);
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_16;
+        break;
+
+      case EFI_IFR_NUMERIC_SIZE_4:
+        CopyMem (&CurrentStatement->Minimum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u32.MinValue, sizeof (UINT32));
+        CopyMem (&CurrentStatement->Maximum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u32.MaxValue, sizeof (UINT32));
+        CopyMem (&CurrentStatement->Step,    &((EFI_IFR_NUMERIC *) OpCodeData)->data.u32.Step,     sizeof (UINT32));
+        CurrentStatement->StorageWidth = (UINT16) sizeof (UINT32);
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_32;
+        break;
+
+      case EFI_IFR_NUMERIC_SIZE_8:
+        CopyMem (&CurrentStatement->Minimum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u64.MinValue, sizeof (UINT64));
+        CopyMem (&CurrentStatement->Maximum, &((EFI_IFR_NUMERIC *) OpCodeData)->data.u64.MaxValue, sizeof (UINT64));
+        CopyMem (&CurrentStatement->Step,    &((EFI_IFR_NUMERIC *) OpCodeData)->data.u64.Step,     sizeof (UINT64));
+        CurrentStatement->StorageWidth = (UINT16) sizeof (UINT64);
+        Value->Type = EFI_IFR_TYPE_NUM_SIZE_64;
+        break;
+
+      default:
+        break;
+      }
+
+      InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+
+      if ((Operand == EFI_IFR_ONE_OF_OP) && Scope != 0) {
+        SuppressForOption = TRUE;
+      }
+      break;
+
+    case EFI_IFR_ORDERED_LIST_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT(CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_ORDERED_LIST *) OpCodeData)->Flags;
+      CurrentStatement->MaxContainers = ((EFI_IFR_ORDERED_LIST *) OpCodeData)->MaxContainers;
+
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_BUFFER;
+      CurrentStatement->BufferValue = NULL;
+
+      if (Scope != 0) {
+        SuppressForOption = TRUE;
+      }
+      break;
+
+    case EFI_IFR_CHECKBOX_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT(CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_CHECKBOX *) OpCodeData)->Flags;
+      CurrentStatement->StorageWidth = (UINT16) sizeof (BOOLEAN);
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_BOOLEAN;
+
+      InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+
+      break;
+
+    case EFI_IFR_STRING_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      //
+      // MinSize is the minimum number of characters that can be accepted for this opcode,
+      // MaxSize is the maximum number of characters that can be accepted for this opcode.
+      // The characters are stored as Unicode, so the storage width should multiply 2.
+      //
+      CurrentStatement->Minimum = ((EFI_IFR_STRING *) OpCodeData)->MinSize;
+      CurrentStatement->Maximum = ((EFI_IFR_STRING *) OpCodeData)->MaxSize;
+      CurrentStatement->StorageWidth = (UINT16)((UINTN) CurrentStatement->Maximum * sizeof (CHAR16));
+      CurrentStatement->Flags = ((EFI_IFR_STRING *) OpCodeData)->Flags;
+
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_STRING;
+      CurrentStatement->BufferValue = AllocateZeroPool (CurrentStatement->StorageWidth + sizeof (CHAR16));
+      CurrentStatement->HiiValue.Value.string = NewString ((CHAR16*) CurrentStatement->BufferValue, FormSet->HiiHandle);
+
+      InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+      break;
+
+    case EFI_IFR_PASSWORD_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT (CurrentStatement != NULL);
+      //
+      // MinSize is the minimum number of characters that can be accepted for this opcode,
+      // MaxSize is the maximum number of characters that can be accepted for this opcode.
+      // The characters are stored as Unicode, so the storage width should multiply 2.
+      //
+      CopyMem (&CurrentStatement->Minimum, &((EFI_IFR_PASSWORD *) OpCodeData)->MinSize, sizeof (UINT16));
+      CopyMem (&CurrentStatement->Maximum, &((EFI_IFR_PASSWORD *) OpCodeData)->MaxSize, sizeof (UINT16));
+      CurrentStatement->StorageWidth = (UINT16)((UINTN) CurrentStatement->Maximum * sizeof (CHAR16));
+
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_STRING;
+      CurrentStatement->BufferValue = AllocateZeroPool ((CurrentStatement->StorageWidth + sizeof (CHAR16)));
+      CurrentStatement->HiiValue.Value.string = NewString ((CHAR16*) CurrentStatement->BufferValue, FormSet->HiiHandle);
+
+      InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+      break;
+
+    case EFI_IFR_DATE_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT(CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_DATE *) OpCodeData)->Flags;
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_DATE;
+
+      if ((CurrentStatement->Flags & EFI_QF_DATE_STORAGE) == QF_DATE_STORAGE_NORMAL) {
+        CurrentStatement->StorageWidth = (UINT16) sizeof (EFI_HII_DATE);
+
+        InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+      } else {
+        //
+        // Don't assign storage for RTC type of date/time
+        //
+        CurrentStatement->Storage = NULL;
+        CurrentStatement->StorageWidth = 0;
+      }
+      break;
+
+    case EFI_IFR_TIME_OP:
+      CurrentStatement = CreateQuestion (OpCodeData, FormSet, CurrentForm);
+      ASSERT(CurrentStatement != NULL);
+
+      CurrentStatement->Flags = ((EFI_IFR_TIME *) OpCodeData)->Flags;
+      CurrentStatement->HiiValue.Type = EFI_IFR_TYPE_TIME;
+
+      if ((CurrentStatement->Flags & QF_TIME_STORAGE) == QF_TIME_STORAGE_NORMAL) {
+        CurrentStatement->StorageWidth = (UINT16) sizeof (EFI_HII_TIME);
+
+        InitializeRequestElement (FormSet, CurrentStatement, CurrentForm);
+      } else {
+        //
+        // Don't assign storage for RTC type of date/time
+        //
+        CurrentStatement->Storage = NULL;
+        CurrentStatement->StorageWidth = 0;
+      }
+      break;
+
+    //
+    // Default
+    //
+    case EFI_IFR_DEFAULT_OP:
+      //
+      // EFI_IFR_DEFAULT appear in scope of a Question,
+      // It creates a default value for the current question.
+      // A Question may have more than one Default value which have different default types.
+      //
+      CurrentDefault = AllocateZeroPool (sizeof (QUESTION_DEFAULT));
+      ASSERT (CurrentDefault != NULL);
+      CurrentDefault->Signature = QUESTION_DEFAULT_SIGNATURE;
+
+      CurrentDefault->Value.Type = ((EFI_IFR_DEFAULT *) OpCodeData)->Type;
+      CopyMem (&CurrentDefault->DefaultId, &((EFI_IFR_DEFAULT *) OpCodeData)->DefaultId, sizeof (UINT16));
+      if (CurrentDefault->Value.Type == EFI_IFR_TYPE_BUFFER) {
+        CurrentDefault->Value.BufferLen = (UINT16) (OpCodeLength - OFFSET_OF (EFI_IFR_DEFAULT, Value));
+        CurrentDefault->Value.Buffer = AllocateCopyPool (CurrentDefault->Value.BufferLen, &((EFI_IFR_DEFAULT *) OpCodeData)->Value);
+        ASSERT (CurrentDefault->Value.Buffer != NULL);
+      } else {
+        CopyMem (&CurrentDefault->Value.Value, &((EFI_IFR_DEFAULT *) OpCodeData)->Value, OpCodeLength - OFFSET_OF (EFI_IFR_DEFAULT, Value));
+        ExtendValueToU64 (&CurrentDefault->Value);
+      }
+
+      //
+      // Insert to Default Value list of current Question
+      //
+      InsertTailList (&ParentStatement->DefaultListHead, &CurrentDefault->Link);
+
+      if (Scope != 0) {
+        InScopeDefault = TRUE;
+      }
+      break;
+
+    //
+    // Option
+    //
+    case EFI_IFR_ONE_OF_OPTION_OP:
+      ASSERT (ParentStatement != NULL);
+      if (ParentStatement->Operand == EFI_IFR_ORDERED_LIST_OP && ((((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Flags & (EFI_IFR_OPTION_DEFAULT | EFI_IFR_OPTION_DEFAULT_MFG)) != 0)) {
+        //
+        // It's keep the default value for ordered list opcode.
+        //
+        CurrentDefault = AllocateZeroPool (sizeof (QUESTION_DEFAULT));
+        ASSERT (CurrentDefault != NULL);
+        CurrentDefault->Signature = QUESTION_DEFAULT_SIGNATURE;
+
+        CurrentDefault->Value.Type = EFI_IFR_TYPE_BUFFER;
+        if ((((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Flags & EFI_IFR_OPTION_DEFAULT) != 0) {
+          CurrentDefault->DefaultId = EFI_HII_DEFAULT_CLASS_STANDARD;
+        } else {
+          CurrentDefault->DefaultId = EFI_HII_DEFAULT_CLASS_MANUFACTURING;
+        }
+
+        CurrentDefault->Value.BufferLen = (UINT16) (OpCodeLength - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+        CurrentDefault->Value.Buffer = AllocateCopyPool (CurrentDefault->Value.BufferLen, &((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Value);
+        ASSERT (CurrentDefault->Value.Buffer != NULL);
+
+        //
+        // Insert to Default Value list of current Question
+        //
+        InsertTailList (&ParentStatement->DefaultListHead, &CurrentDefault->Link);
+        break;
+      }
+
+      //
+      // EFI_IFR_ONE_OF_OPTION appear in scope of a Question.
+      // It create a selection for use in current Question.
+      //
+      CurrentOption = AllocateZeroPool (sizeof (QUESTION_OPTION));
+      ASSERT (CurrentOption != NULL);
+      CurrentOption->Signature = QUESTION_OPTION_SIGNATURE;
+      CurrentOption->OpCode    = (EFI_IFR_ONE_OF_OPTION *) OpCodeData;
+
+      CurrentOption->Flags = ((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Flags;
+      CurrentOption->Value.Type = ((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Type;
+      CopyMem (&CurrentOption->Text, &((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Option, sizeof (EFI_STRING_ID));
+      CopyMem (&CurrentOption->Value.Value, &((EFI_IFR_ONE_OF_OPTION *) OpCodeData)->Value, OpCodeLength - OFFSET_OF (EFI_IFR_ONE_OF_OPTION, Value));
+      ExtendValueToU64 (&CurrentOption->Value);
+
+      ConditionalExprCount = GetConditionalExpressionCount(ExpressOption);
+      if ( ConditionalExprCount > 0) {
+        //
+        // Form is inside of suppressif
+        //
+        CurrentOption->SuppressExpression = (FORM_EXPRESSION_LIST *) AllocatePool( 
+                                                 (UINTN) (sizeof(FORM_EXPRESSION_LIST) + ((ConditionalExprCount -1) * sizeof(FORM_EXPRESSION *))));
+        ASSERT (CurrentOption->SuppressExpression != NULL);
+        CurrentOption->SuppressExpression->Count     = (UINTN) ConditionalExprCount;
+        CurrentOption->SuppressExpression->Signature = FORM_EXPRESSION_LIST_SIGNATURE;
+        CopyMem (CurrentOption->SuppressExpression->Expression, GetConditionalExpressionList(ExpressOption), (UINTN) (sizeof (FORM_EXPRESSION *) * ConditionalExprCount));
+      }
+
+      //
+      // Insert to Option list of current Question
+      //
+      InsertTailList (&ParentStatement->OptionListHead, &CurrentOption->Link);
+      //
+      // Now we know the Storage width of nested Ordered List
+      //
+      if ((ParentStatement->Operand == EFI_IFR_ORDERED_LIST_OP) && (ParentStatement->BufferValue == NULL)) {
+        Width = 1;
+        switch (CurrentOption->Value.Type) {
+        case EFI_IFR_TYPE_NUM_SIZE_8:
+          Width = 1;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_16:
+          Width = 2;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_32:
+          Width = 4;
+          break;
+
+        case EFI_IFR_TYPE_NUM_SIZE_64:
+          Width = 8;
+          break;
+
+        default:
+          //
+          // Invalid type for Ordered List
+          //
+          break;
+        }
+
+        ParentStatement->StorageWidth = (UINT16) (ParentStatement->MaxContainers * Width);
+        ParentStatement->BufferValue = AllocateZeroPool (ParentStatement->StorageWidth);
+        ParentStatement->ValueType = CurrentOption->Value.Type;
+        if (ParentStatement->HiiValue.Type == EFI_IFR_TYPE_BUFFER) {
+          ParentStatement->HiiValue.Buffer = ParentStatement->BufferValue;
+          ParentStatement->HiiValue.BufferLen = ParentStatement->StorageWidth;
+        }
+
+        InitializeRequestElement (FormSet, ParentStatement, CurrentForm);
+      }
+      break;
+
+    //
+    // Conditional
+    //
+    case EFI_IFR_NO_SUBMIT_IF_OP:
+    case EFI_IFR_INCONSISTENT_IF_OP:
+      //
+      // Create an Expression node
+      //
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CopyMem (&CurrentExpression->Error, &((EFI_IFR_INCONSISTENT_IF *) OpCodeData)->Error, sizeof (EFI_STRING_ID));
+
+      if (Operand == EFI_IFR_NO_SUBMIT_IF_OP) {
+        CurrentExpression->Type = EFI_HII_EXPRESSION_NO_SUBMIT_IF;
+        InsertTailList (&ParentStatement->NoSubmitListHead, &CurrentExpression->Link);
+      } else {
+        CurrentExpression->Type = EFI_HII_EXPRESSION_INCONSISTENT_IF;
+        InsertTailList (&ParentStatement->InconsistentListHead, &CurrentExpression->Link);
+      }
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_WARNING_IF_OP:
+      //
+      // Create an Expression node
+      //
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CopyMem (&CurrentExpression->Error, &((EFI_IFR_WARNING_IF *) OpCodeData)->Warning, sizeof (EFI_STRING_ID));
+      CurrentExpression->TimeOut = ((EFI_IFR_WARNING_IF *) OpCodeData)->TimeOut;
+      CurrentExpression->Type    = EFI_HII_EXPRESSION_WARNING_IF;
+      InsertTailList (&ParentStatement->WarningListHead, &CurrentExpression->Link);
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_SUPPRESS_IF_OP:
+      //
+      // Question and Option will appear in scope of this OpCode
+      //
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_SUPPRESS_IF;
+
+      if (CurrentForm == NULL) {
+        InsertTailList (&FormSet->ExpressionListHead, &CurrentExpression->Link);
+      } else {
+        InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+      }
+
+      if (SuppressForOption) {
+        PushConditionalExpression(CurrentExpression, ExpressOption);       
+      } else if (SuppressForQuestion) {
+        PushConditionalExpression(CurrentExpression, ExpressStatement);  
+      } else {
+        PushConditionalExpression(CurrentExpression, ExpressForm);  
+      }
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_GRAY_OUT_IF_OP:
+      //
+      // Questions will appear in scope of this OpCode
+      //
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_GRAY_OUT_IF;
+      InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+      PushConditionalExpression(CurrentExpression, ExpressStatement);
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_DISABLE_IF_OP:
+      //
+      // The DisableIf expression should only rely on constant, so it could be
+      // evaluated at initialization and it will not be queued
+      //
+      CurrentExpression = AllocateZeroPool (sizeof (FORM_EXPRESSION));
+      ASSERT (CurrentExpression != NULL);
+      CurrentExpression->Signature = FORM_EXPRESSION_SIGNATURE;
+      CurrentExpression->Type = EFI_HII_EXPRESSION_DISABLE_IF;
+      InitializeListHead (&CurrentExpression->OpCodeListHead);
+
+      if (CurrentForm != NULL) {
+        //
+        // This is DisableIf for Question, enqueue it to Form expression list
+        //
+        InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+        PushConditionalExpression(CurrentExpression, ExpressStatement);
+      }
+
+      OpCodeDisabled  = FALSE;
+      InScopeDisable  = TRUE;
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    //
+    // Expression
+    //
+    case EFI_IFR_VALUE_OP:
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_VALUE;
+      InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+
+      if (InScopeDefault) {
+        //
+        // Used for default (EFI_IFR_DEFAULT)
+        //
+        CurrentDefault->ValueExpression = CurrentExpression;
+      } else {
+        //
+        // If used for a question, then the question will be read-only
+        //
+        //
+        // Make sure CurrentStatement is not NULL.
+        // If it is NULL, 1) ParseOpCodes functions may parse the IFR wrongly. Or 2) the IFR
+        // file is wrongly generated by tools such as VFR Compiler. There may be a bug in VFR Compiler.
+        //
+        ASSERT (ParentStatement != NULL);
+        ParentStatement->ValueExpression = CurrentExpression;
+      }
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_RULE_OP:
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_RULE;
+
+      CurrentExpression->RuleId = ((EFI_IFR_RULE *) OpCodeData)->RuleId;
+      InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_READ_OP:
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_READ;
+      InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+
+      //
+      // Make sure CurrentStatement is not NULL.
+      // If it is NULL, 1) ParseOpCodes functions may parse the IFR wrongly. Or 2) the IFR
+      // file is wrongly generated by tools such as VFR Compiler. There may be a bug in VFR Compiler.
+      //
+      ASSERT (ParentStatement != NULL);
+      ParentStatement->ReadExpression = CurrentExpression;
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    case EFI_IFR_WRITE_OP:
+      CurrentExpression = CreateExpression (CurrentForm, OpCodeData);
+      CurrentExpression->Type = EFI_HII_EXPRESSION_WRITE;
+      InsertTailList (&CurrentForm->ExpressionListHead, &CurrentExpression->Link);
+
+      //
+      // Make sure CurrentStatement is not NULL.
+      // If it is NULL, 1) ParseOpCodes functions may parse the IFR wrongly. Or 2) the IFR
+      // file is wrongly generated by tools such as VFR Compiler. There may be a bug in VFR Compiler.
+      //
+      ASSERT (ParentStatement != NULL);
+      ParentStatement->WriteExpression = CurrentExpression;
+
+      //
+      // Take a look at next OpCode to see whether current expression consists
+      // of single OpCode
+      //
+      if (((EFI_IFR_OP_HEADER *) (OpCodeData + OpCodeLength))->Scope == 0) {
+        SingleOpCodeExpression = TRUE;
+      }
+      break;
+
+    //
+    // Image
+    //
+    case EFI_IFR_IMAGE_OP:
+      //
+      // Get ScopeOpcode from top of stack
+      //
+      PopScope (&ScopeOpCode);
+      PushScope (ScopeOpCode);
+
+      switch (ScopeOpCode) {
+      case EFI_IFR_FORM_SET_OP:
+        ImageId = &FormSet->ImageId;
+        break;
+
+      case EFI_IFR_FORM_OP:
+      case EFI_IFR_FORM_MAP_OP:
+        ASSERT (CurrentForm != NULL);
+        ImageId = &CurrentForm->ImageId;
+        break;
+
+      case EFI_IFR_ONE_OF_OPTION_OP:
+        ASSERT (CurrentOption != NULL);
+        ImageId = &CurrentOption->ImageId;
+        break;
+
+      default:
+        //
+        // Make sure CurrentStatement is not NULL.
+        // If it is NULL, 1) ParseOpCodes functions may parse the IFR wrongly. Or 2) the IFR
+        // file is wrongly generated by tools such as VFR Compiler.
+        //
+        ASSERT (ParentStatement != NULL);
+        ImageId = &ParentStatement->ImageId;
+        break;
+      }
+
+      ASSERT (ImageId != NULL);
+      CopyMem (ImageId, &((EFI_IFR_IMAGE *) OpCodeData)->Id, sizeof (EFI_IMAGE_ID));
+      break;
+
+    //
+    // Refresh
+    //
+    case EFI_IFR_REFRESH_OP:
+      ASSERT (ParentStatement != NULL);
+      ParentStatement->RefreshInterval = ((EFI_IFR_REFRESH *) OpCodeData)->RefreshInterval;
+      break;
+
+    //
+    // Refresh guid.
+    //
+    case EFI_IFR_REFRESH_ID_OP:
+      //
+      // Get ScopeOpcode from top of stack
+      //
+      PopScope (&ScopeOpCode);
+      PushScope (ScopeOpCode);
+
+      switch (ScopeOpCode) {
+      case EFI_IFR_FORM_OP:
+      case EFI_IFR_FORM_MAP_OP:
+        ASSERT (CurrentForm != NULL);
+        CopyMem (&CurrentForm->RefreshGuid, &((EFI_IFR_REFRESH_ID *) OpCodeData)->RefreshEventGroupId, sizeof (EFI_GUID));
+        break;
+
+      default:
+        ASSERT (ParentStatement != NULL);
+        CopyMem (&ParentStatement->RefreshGuid, &((EFI_IFR_REFRESH_ID *) OpCodeData)->RefreshEventGroupId, sizeof (EFI_GUID));
+        break;
+      }
+      break;
+
+    //
+    // Modal tag
+    //
+    case EFI_IFR_MODAL_TAG_OP:
+      ASSERT (CurrentForm != NULL);
+      CurrentForm->ModalForm = TRUE;
+      break;
+
+    //
+    // Lock tag, used by form and statement.
+    //
+    case EFI_IFR_LOCKED_OP:
+      //
+      // Get ScopeOpcode from top of stack
+      //
+      PopScope (&ScopeOpCode);
+      PushScope (ScopeOpCode);
+      switch (ScopeOpCode) {
+      case EFI_IFR_FORM_OP:
+      case EFI_IFR_FORM_MAP_OP:
+        ASSERT (CurrentForm != NULL);
+        CurrentForm->Locked = TRUE;
+        break;
+
+      default:
+        ASSERT (ParentStatement != NULL);
+        ParentStatement->Locked = TRUE;
+      }      
+      break;
+
+    //
+    // Vendor specific
+    //
+    case EFI_IFR_GUID_OP:     
+      CurrentStatement = CreateStatement (OpCodeData, FormSet, CurrentForm);
+      break;
+
+    //
+    // Scope End
+    //
+    case EFI_IFR_END_OP:
+      Status = PopScope (&ScopeOpCode);
+      if (EFI_ERROR (Status)) {
+        ResetScopeStack ();
+        return Status;
+      }
+      
+      //
+      // Parent statement end tag found, update ParentStatement info.
+      //
+      if (IsStatementOpCode(ScopeOpCode) && (ParentStatement != NULL) && (ParentStatement->Operand == ScopeOpCode)) {
+        ParentStatement  = ParentStatement->ParentStatement;
+      }
+
+      switch (ScopeOpCode) {
+      case EFI_IFR_FORM_SET_OP:
+        //
+        // End of FormSet, update FormSet IFR binary length
+        // to stop parsing substantial OpCodes
+        //
+        FormSet->IfrBinaryLength = OpCodeOffset;
+        break;
+
+      case EFI_IFR_FORM_OP:
+      case EFI_IFR_FORM_MAP_OP:
+        //
+        // End of Form
+        //
+        CurrentForm = NULL;
+        SuppressForQuestion = FALSE;
+        break;
+
+      case EFI_IFR_ONE_OF_OPTION_OP:
+        //
+        // End of Option
+        //
+        CurrentOption = NULL;
+        break;
+
+      case EFI_IFR_NO_SUBMIT_IF_OP:
+      case EFI_IFR_INCONSISTENT_IF_OP:
+      case EFI_IFR_WARNING_IF_OP:
+        //
+        // Ignore end of EFI_IFR_NO_SUBMIT_IF and EFI_IFR_INCONSISTENT_IF
+        //
+        break;
+
+      case EFI_IFR_SUPPRESS_IF_OP:
+        if (SuppressForOption) {
+          PopConditionalExpression(ExpressOption);      
+        } else if (SuppressForQuestion) {
+          PopConditionalExpression(ExpressStatement);
+        } else {
+          PopConditionalExpression(ExpressForm);
+        }
+        break;
+
+      case EFI_IFR_GRAY_OUT_IF_OP:
+        PopConditionalExpression(ExpressStatement);
+        break;
+
+      case EFI_IFR_DISABLE_IF_OP:
+        if (CurrentForm != NULL) {
+          PopConditionalExpression(ExpressStatement);
+        }
+        InScopeDisable = FALSE;
+        OpCodeDisabled = FALSE;
+        break;
+
+      case EFI_IFR_ONE_OF_OP:
+      case EFI_IFR_ORDERED_LIST_OP:
+        SuppressForOption = FALSE;
+        break;
+
+      case EFI_IFR_DEFAULT_OP:
+        InScopeDefault = FALSE;
+        break;
+
+      case EFI_IFR_MAP_OP:
+        //
+        // Get current Map Expression List.
+        //
+        Status = PopMapExpressionList ((VOID **) &MapExpressionList);
+        if (Status == EFI_ACCESS_DENIED) {
+          MapExpressionList = NULL;
+        }
+        //
+        // Get current expression.
+        //
+        Status = PopCurrentExpression ((VOID **) &CurrentExpression);
+        ASSERT_EFI_ERROR (Status);
+        ASSERT (MapScopeDepth > 0);
+        MapScopeDepth --;
+        break;
+
+      default:
+        if (IsExpressionOpCode (ScopeOpCode)) {
+          if (InScopeDisable && CurrentForm == NULL) {
+            //
+            // This is DisableIf expression for Form, it should be a constant expression
+            //
+            ASSERT (CurrentExpression != NULL);
+            Status = EvaluateExpression (FormSet, CurrentForm, CurrentExpression);
+            if (EFI_ERROR (Status)) {
+              return Status;
+            }
+
+            OpCodeDisabled = IsTrue (&CurrentExpression->Result);
+
+            //
+            // DisableIf Expression is only used once and not queued, free it
+            //
+            DestroyExpression (CurrentExpression);
+          }
+
+          //
+          // End of current Expression
+          //
+          CurrentExpression = NULL;
+        }
+        break;
+      }
+      break;
+
+    default:
+      break;
+    }
+
+    if (IsStatementOpCode(Operand)) {
+      CurrentStatement->ParentStatement = ParentStatement;
+      if (Scope != 0) {
+        //
+        // Scope != 0, other statements or options may nest in this statement.
+        // Update the ParentStatement info.
+        //
+        ParentStatement = CurrentStatement;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/Presentation.c b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Presentation.c
new file mode 100644
index 0000000000..356cd9cd9a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Presentation.c
@@ -0,0 +1,2615 @@
+/** @file
+Utility functions for UI presentation.
+
+Copyright (c) 2004 - 2015, 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 "Setup.h"
+
+BOOLEAN            mHiiPackageListUpdated;
+UI_MENU_SELECTION  *gCurrentSelection;
+EFI_HII_HANDLE     mCurrentHiiHandle = NULL;
+EFI_GUID           mCurrentFormSetGuid = {0, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0}};
+UINT16             mCurrentFormId = 0;
+EFI_EVENT          mValueChangedEvent = NULL;
+LIST_ENTRY         mRefreshEventList = INITIALIZE_LIST_HEAD_VARIABLE (mRefreshEventList);
+UINT16             mCurFakeQestId;
+FORM_DISPLAY_ENGINE_FORM gDisplayFormData;
+BOOLEAN            mFinishRetrieveCall = FALSE;
+
+/**
+  Evaluate all expressions in a Form.
+
+  @param  FormSet        FormSet this Form belongs to.
+  @param  Form           The Form.
+
+  @retval EFI_SUCCESS    The expression evaluated successfuly
+
+**/
+EFI_STATUS
+EvaluateFormExpressions (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN FORM_BROWSER_FORM     *Form
+  )
+{
+  EFI_STATUS       Status;
+  LIST_ENTRY       *Link;
+  FORM_EXPRESSION  *Expression;
+
+  Link = GetFirstNode (&Form->ExpressionListHead);
+  while (!IsNull (&Form->ExpressionListHead, Link)) {
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+    Link = GetNextNode (&Form->ExpressionListHead, Link);
+
+    if (Expression->Type == EFI_HII_EXPRESSION_INCONSISTENT_IF ||
+        Expression->Type == EFI_HII_EXPRESSION_NO_SUBMIT_IF ||
+        Expression->Type == EFI_HII_EXPRESSION_WARNING_IF ||
+        Expression->Type == EFI_HII_EXPRESSION_WRITE ||
+        (Expression->Type == EFI_HII_EXPRESSION_READ && Form->FormType != STANDARD_MAP_FORM_TYPE)) {
+      //
+      // Postpone Form validation to Question editing or Form submitting or Question Write or Question Read for nonstandard form.
+      //
+      continue;
+    }
+
+    Status = EvaluateExpression (FormSet, Form, Expression);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Add empty function for event process function.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+SetupBrowserEmptyFunction (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  )
+{
+}
+
+/**
+  Base on the opcode buffer info to get the display statement.
+
+  @param OpCode    The input opcode buffer for this statement.
+  
+  @retval Statement  The statement use this opcode buffer.
+
+**/
+FORM_DISPLAY_ENGINE_STATEMENT *
+GetDisplayStatement (
+  IN EFI_IFR_OP_HEADER     *OpCode
+  )
+{
+  FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement;
+  LIST_ENTRY                    *Link;
+
+  Link = GetFirstNode (&gDisplayFormData.StatementListHead);
+  while (!IsNull (&gDisplayFormData.StatementListHead, Link)) {
+    DisplayStatement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (Link);
+
+    if (DisplayStatement->OpCode == OpCode) {
+      return DisplayStatement;
+    }
+    Link = GetNextNode (&gDisplayFormData.StatementListHead, Link);
+  }
+
+  return NULL;
+}
+
+/**
+  Free the refresh event list.
+
+**/
+VOID 
+FreeRefreshEvent (
+  VOID
+  )
+{
+  LIST_ENTRY   *Link;
+  FORM_BROWSER_REFRESH_EVENT_NODE *EventNode;
+
+  while (!IsListEmpty (&mRefreshEventList)) {
+    Link = GetFirstNode (&mRefreshEventList);
+    EventNode = FORM_BROWSER_REFRESH_EVENT_FROM_LINK (Link);
+    RemoveEntryList (&EventNode->Link);
+
+    gBS->CloseEvent (EventNode->RefreshEvent);
+
+    FreePool (EventNode);
+  }
+}
+
+/**
+  Check whether this statement value is changed. If yes, update the statement value and return TRUE; 
+  else return FALSE.
+
+  @param Statement           The statement need to check.
+
+**/
+VOID
+UpdateStatement (
+  IN OUT FORM_BROWSER_STATEMENT        *Statement
+  )
+{
+  GetQuestionValue (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithHiiDriver);
+
+  //
+  // Reset FormPackage update flag
+  //
+  mHiiPackageListUpdated = FALSE;
+
+  //
+  // Question value may be changed, need invoke its Callback()
+  //
+  ProcessCallBackFunction (gCurrentSelection, gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, EFI_BROWSER_ACTION_RETRIEVE, FALSE);
+  
+  if (mHiiPackageListUpdated) {
+    //
+    // Package list is updated, force to reparse IFR binary of target Formset
+    //
+    mHiiPackageListUpdated = FALSE;
+    gCurrentSelection->Action = UI_ACTION_REFRESH_FORMSET;
+  }
+}
+
+/**
+  Refresh the question which has refresh guid event attribute.
+  
+  @param Event    The event which has this function related.     
+  @param Context  The input context info related to this event or the status code return to the caller.
+**/
+VOID
+EFIAPI
+RefreshEventNotifyForStatement(
+  IN      EFI_EVENT Event,
+  IN      VOID      *Context
+  )
+{
+  FORM_BROWSER_STATEMENT        *Statement;
+
+  Statement = (FORM_BROWSER_STATEMENT *)Context;
+  UpdateStatement(Statement);
+  gBS->SignalEvent (mValueChangedEvent);
+}
+
+/**
+  Refresh the questions within this form.
+  
+  @param Event    The event which has this function related.
+  @param Context  The input context info related to this event or the status code return to the caller.
+**/
+VOID
+EFIAPI
+RefreshEventNotifyForForm(
+  IN      EFI_EVENT Event,
+  IN      VOID      *Context
+  )
+{
+  gCurrentSelection->Action = UI_ACTION_REFRESH_FORMSET;
+
+  gBS->SignalEvent (mValueChangedEvent);
+}
+
+/**
+  Create refresh hook event for statement which has refresh event or interval.
+
+  @param Statement           The statement need to check.
+
+**/
+VOID
+CreateRefreshEventForStatement (
+  IN     FORM_BROWSER_STATEMENT        *Statement
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_EVENT                       RefreshEvent;
+  FORM_BROWSER_REFRESH_EVENT_NODE *EventNode;
+
+  //
+  // If question has refresh guid, create the notify function.
+  //
+  Status = gBS->CreateEventEx (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    RefreshEventNotifyForStatement,
+                    Statement,
+                    &Statement->RefreshGuid,
+                    &RefreshEvent);
+  ASSERT_EFI_ERROR (Status);
+
+  EventNode = AllocateZeroPool (sizeof (FORM_BROWSER_REFRESH_EVENT_NODE));
+  ASSERT (EventNode != NULL);
+  EventNode->RefreshEvent = RefreshEvent;
+  InsertTailList(&mRefreshEventList, &EventNode->Link);
+}
+
+/**
+  Create refresh hook event for form which has refresh event or interval.
+
+  @param Form           The form need to check.
+
+**/
+VOID
+CreateRefreshEventForForm (
+  IN     FORM_BROWSER_FORM        *Form
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_EVENT                       RefreshEvent;
+  FORM_BROWSER_REFRESH_EVENT_NODE *EventNode;
+
+  //
+  // If question has refresh guid, create the notify function.
+  //
+  Status = gBS->CreateEventEx (
+                    EVT_NOTIFY_SIGNAL,
+                    TPL_CALLBACK,
+                    RefreshEventNotifyForForm,
+                    Form,
+                    &Form->RefreshGuid,
+                    &RefreshEvent);
+  ASSERT_EFI_ERROR (Status);
+
+  EventNode = AllocateZeroPool (sizeof (FORM_BROWSER_REFRESH_EVENT_NODE));
+  ASSERT (EventNode != NULL);
+  EventNode->RefreshEvent = RefreshEvent;
+  InsertTailList(&mRefreshEventList, &EventNode->Link);
+}
+
+/**
+
+  Initialize the Display statement structure data.
+
+  @param DisplayStatement      Pointer to the display Statement data strucure.
+  @param Statement             The statement need to check.
+**/
+VOID
+InitializeDisplayStatement (
+  IN OUT FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement,
+  IN     FORM_BROWSER_STATEMENT        *Statement
+  )
+{
+  LIST_ENTRY                 *Link;
+  QUESTION_OPTION            *Option;
+  DISPLAY_QUESTION_OPTION    *DisplayOption;
+  FORM_DISPLAY_ENGINE_STATEMENT *ParentStatement;
+
+  DisplayStatement->Signature = FORM_DISPLAY_ENGINE_STATEMENT_SIGNATURE;
+  DisplayStatement->Version   = FORM_DISPLAY_ENGINE_STATEMENT_VERSION_1;
+  DisplayStatement->OpCode    = Statement->OpCode;
+  InitializeListHead (&DisplayStatement->NestStatementList);
+  InitializeListHead (&DisplayStatement->OptionListHead);
+
+  if ((EvaluateExpressionList(Statement->Expression, FALSE, NULL, NULL) == ExpressGrayOut) || Statement->Locked) {
+    DisplayStatement->Attribute |= HII_DISPLAY_GRAYOUT;
+  }
+  if ((Statement->ValueExpression != NULL) || ((Statement->QuestionFlags & EFI_IFR_FLAG_READ_ONLY) != 0)) {
+    DisplayStatement->Attribute |= HII_DISPLAY_READONLY;
+  }
+
+  //
+  // Initilize the option list in statement.
+  //
+  Link = GetFirstNode (&Statement->OptionListHead);
+  while (!IsNull (&Statement->OptionListHead, Link)) {
+    Option = QUESTION_OPTION_FROM_LINK (Link);
+    Link = GetNextNode (&Statement->OptionListHead, Link);
+    if ((Option->SuppressExpression != NULL) &&
+        ((EvaluateExpressionList(Option->SuppressExpression, FALSE, NULL, NULL) == ExpressSuppress))) {
+      continue;
+    }
+
+    DisplayOption = AllocateZeroPool (sizeof (DISPLAY_QUESTION_OPTION));
+    ASSERT (DisplayOption != NULL);
+
+    DisplayOption->ImageId      = Option->ImageId;
+    DisplayOption->Signature    = DISPLAY_QUESTION_OPTION_SIGNATURE;
+    DisplayOption->OptionOpCode = Option->OpCode;
+    InsertTailList(&DisplayStatement->OptionListHead, &DisplayOption->Link);
+  }
+
+  CopyMem (&DisplayStatement->CurrentValue, &Statement->HiiValue, sizeof (EFI_HII_VALUE));
+
+  //
+  // Some special op code need an extra buffer to save the data.
+  // Such as string, password, orderedlist...
+  //
+  if (Statement->BufferValue != NULL) {
+    //
+    // Ordered list opcode may not initilized, get default value here.
+    //
+    if (Statement->OpCode->OpCode == EFI_IFR_ORDERED_LIST_OP && GetArrayData (Statement->BufferValue, Statement->ValueType, 0) == 0) {
+      GetQuestionDefault (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, 0);
+    }
+
+    DisplayStatement->CurrentValue.Buffer    = AllocateCopyPool(Statement->StorageWidth,Statement->BufferValue);
+    DisplayStatement->CurrentValue.BufferLen = Statement->StorageWidth;
+  }
+
+  DisplayStatement->SettingChangedFlag = Statement->ValueChanged;
+
+  //
+  // Get the highlight statement for current form.
+  //
+  if (((gCurrentSelection->QuestionId != 0) && (Statement->QuestionId == gCurrentSelection->QuestionId)) ||
+      ((mCurFakeQestId != 0) && (Statement->FakeQuestionId == mCurFakeQestId))) {
+    gDisplayFormData.HighLightedStatement = DisplayStatement;
+  }
+
+  //
+  // Create the refresh event process function.
+  //
+  if (!CompareGuid (&Statement->RefreshGuid, &gZeroGuid)) {
+    CreateRefreshEventForStatement (Statement);
+  }
+
+  //
+  // For RTC type of date/time, set default refresh interval to be 1 second.
+  //
+  if ((Statement->Operand == EFI_IFR_DATE_OP || Statement->Operand == EFI_IFR_TIME_OP) && Statement->Storage == NULL) {
+    Statement->RefreshInterval = 1;
+  }
+
+  //
+  // Create the refresh guid hook event.
+  // If the statement in this form has refresh event or refresh interval, browser will create this event for display engine.
+  //
+  if ((!CompareGuid (&Statement->RefreshGuid, &gZeroGuid)) || (Statement->RefreshInterval != 0)) {
+    gDisplayFormData.FormRefreshEvent = mValueChangedEvent;
+  }
+
+  //
+  // Save the password check function for later use.
+  //
+  if (Statement->Operand == EFI_IFR_PASSWORD_OP) {
+    DisplayStatement->PasswordCheck = PasswordCheck;
+  }
+
+  //
+  // If this statement is nest in the subtitle, insert to the host statement.
+  // else insert to the form it belongs to.
+  //
+  if (Statement->ParentStatement != NULL) {
+    ParentStatement = GetDisplayStatement(Statement->ParentStatement->OpCode);
+    ASSERT (ParentStatement != NULL);
+    InsertTailList(&ParentStatement->NestStatementList, &DisplayStatement->DisplayLink);
+  } else {
+    InsertTailList(&gDisplayFormData.StatementListHead, &DisplayStatement->DisplayLink);
+  }
+}
+
+/**
+  Process for the refresh interval statement.
+
+  @param Event    The Event need to be process
+  @param Context  The context of the event.
+
+**/
+VOID
+EFIAPI
+RefreshIntervalProcess (
+  IN  EFI_EVENT    Event,
+  IN  VOID         *Context
+  )
+{
+  FORM_BROWSER_STATEMENT        *Statement;
+  LIST_ENTRY                    *Link;
+
+  Link = GetFirstNode (&gCurrentSelection->Form->StatementListHead);
+  while (!IsNull (&gCurrentSelection->Form->StatementListHead, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&gCurrentSelection->Form->StatementListHead, Link);
+
+    if (Statement->RefreshInterval == 0) {
+      continue;
+    }
+
+    UpdateStatement(Statement);
+  }
+
+  gBS->SignalEvent (mValueChangedEvent);
+}
+
+/**
+
+  Make a copy of the global hotkey info.
+
+**/
+VOID
+UpdateHotkeyList (
+  VOID
+  )
+{
+  BROWSER_HOT_KEY  *HotKey;
+  BROWSER_HOT_KEY  *CopyKey;
+  LIST_ENTRY       *Link;
+
+  Link = GetFirstNode (&gBrowserHotKeyList);
+  while (!IsNull (&gBrowserHotKeyList, Link)) {
+    HotKey = BROWSER_HOT_KEY_FROM_LINK (Link);
+
+    CopyKey             = AllocateCopyPool(sizeof (BROWSER_HOT_KEY), HotKey);
+    ASSERT (CopyKey != NULL);
+    CopyKey->KeyData    = AllocateCopyPool(sizeof (EFI_INPUT_KEY), HotKey->KeyData);
+    ASSERT (CopyKey->KeyData != NULL);
+    CopyKey->HelpString = AllocateCopyPool(StrSize (HotKey->HelpString), HotKey->HelpString);
+    ASSERT (CopyKey->HelpString != NULL);
+
+    InsertTailList(&gDisplayFormData.HotKeyListHead, &CopyKey->Link);
+
+    Link = GetNextNode (&gBrowserHotKeyList, Link);
+  }
+}
+
+/**
+
+  Get the extra question attribute from override question list.
+
+  @param    QuestionId    The question id for this request question.
+
+  @retval   The attribute for this question or NULL if not found this 
+            question in the list.
+
+**/
+UINT32 
+ProcessQuestionExtraAttr (
+  IN   EFI_QUESTION_ID  QuestionId
+  )
+{
+  LIST_ENTRY                   *Link;
+  QUESTION_ATTRIBUTE_OVERRIDE  *QuestionDesc;
+
+  //
+  // Return HII_DISPLAY_NONE if input a invalid question id.
+  //
+  if (QuestionId == 0) {
+    return HII_DISPLAY_NONE;
+  }
+
+  Link = GetFirstNode (&mPrivateData.FormBrowserEx2.OverrideQestListHead);
+  while (!IsNull (&mPrivateData.FormBrowserEx2.OverrideQestListHead, Link)) {
+    QuestionDesc = FORM_QUESTION_ATTRIBUTE_OVERRIDE_FROM_LINK (Link);
+    Link = GetNextNode (&mPrivateData.FormBrowserEx2.OverrideQestListHead, Link);
+
+    if ((QuestionDesc->QuestionId == QuestionId) &&
+        (QuestionDesc->FormId     == gCurrentSelection->FormId) &&
+        (QuestionDesc->HiiHandle  == gCurrentSelection->Handle) &&
+        CompareGuid (&QuestionDesc->FormSetGuid, &gCurrentSelection->FormSetGuid)) {
+      return QuestionDesc->Attribute;
+    }
+  }
+
+  return HII_DISPLAY_NONE;
+}
+
+/**
+
+  Enum all statement in current form, find all the statement can be display and
+  add to the display form.
+
+**/
+VOID
+AddStatementToDisplayForm (
+  VOID
+  )
+{
+  EFI_STATUS                    Status;
+  LIST_ENTRY                    *Link;
+  FORM_BROWSER_STATEMENT        *Statement;
+  FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement;
+  UINT8                         MinRefreshInterval;
+  EFI_EVENT                     RefreshIntervalEvent;
+  FORM_BROWSER_REFRESH_EVENT_NODE *EventNode;
+  BOOLEAN                       FormEditable;
+  UINT32                        ExtraAttribute;
+
+  MinRefreshInterval   = 0;
+  FormEditable         = FALSE;
+
+  //
+  // Process the statement outside the form, these statements are not recognized
+  // by browser core.
+  //
+  Link = GetFirstNode (&gCurrentSelection->FormSet->StatementListOSF);
+  while (!IsNull (&gCurrentSelection->FormSet->StatementListOSF, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&gCurrentSelection->FormSet->StatementListOSF, Link);
+
+    DisplayStatement = AllocateZeroPool (sizeof (FORM_DISPLAY_ENGINE_STATEMENT));
+    ASSERT (DisplayStatement != NULL);
+    DisplayStatement->Signature = FORM_DISPLAY_ENGINE_STATEMENT_SIGNATURE;
+    DisplayStatement->Version   = FORM_DISPLAY_ENGINE_STATEMENT_VERSION_1;
+    DisplayStatement->OpCode = Statement->OpCode;
+
+    InitializeListHead (&DisplayStatement->NestStatementList);
+    InitializeListHead (&DisplayStatement->OptionListHead);
+
+    InsertTailList(&gDisplayFormData.StatementListOSF, &DisplayStatement->DisplayLink);
+  }
+
+  //
+  // treat formset as statement outside the form,get its opcode.
+  //
+  DisplayStatement = AllocateZeroPool (sizeof (FORM_DISPLAY_ENGINE_STATEMENT));
+  ASSERT (DisplayStatement != NULL);
+
+  DisplayStatement->Signature = FORM_DISPLAY_ENGINE_STATEMENT_SIGNATURE;
+  DisplayStatement->Version   = FORM_DISPLAY_ENGINE_STATEMENT_VERSION_1;
+  DisplayStatement->OpCode = gCurrentSelection->FormSet->OpCode;
+
+  InitializeListHead (&DisplayStatement->NestStatementList);
+  InitializeListHead (&DisplayStatement->OptionListHead);
+
+  InsertTailList(&gDisplayFormData.StatementListOSF, &DisplayStatement->DisplayLink);
+
+  //
+  // Process the statement in this form.
+  //
+  Link = GetFirstNode (&gCurrentSelection->Form->StatementListHead);
+  while (!IsNull (&gCurrentSelection->Form->StatementListHead, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&gCurrentSelection->Form->StatementListHead, Link);
+
+    //
+    // This statement can't be show, skip it.
+    //
+    if (EvaluateExpressionList(Statement->Expression, FALSE, NULL, NULL) > ExpressGrayOut) {
+      continue;
+    }
+
+    //
+    // Check the extra attribute.
+    //
+    ExtraAttribute = ProcessQuestionExtraAttr (Statement->QuestionId);
+    if ((ExtraAttribute & HII_DISPLAY_SUPPRESS) != 0) {
+      continue;
+    }
+
+    DisplayStatement = AllocateZeroPool (sizeof (FORM_DISPLAY_ENGINE_STATEMENT));
+    ASSERT (DisplayStatement != NULL);
+
+    //
+    // Initialize this statement and add it to the display form.
+    //
+    InitializeDisplayStatement(DisplayStatement, Statement);
+
+    //
+    // Set the extra attribute.
+    //
+    DisplayStatement->Attribute |= ExtraAttribute;
+
+    if (Statement->Storage != NULL) {
+      FormEditable = TRUE;
+    }
+
+    //
+    // Get the minimal refresh interval value for later use.
+    //
+    if ((Statement->RefreshInterval != 0) && 
+      (MinRefreshInterval == 0 || Statement->RefreshInterval < MinRefreshInterval)) {
+      MinRefreshInterval = Statement->RefreshInterval;
+    }
+  }
+
+  //
+  // Create the periodic timer for refresh interval statement.
+  //
+  if (MinRefreshInterval != 0) {
+    Status = gBS->CreateEvent (EVT_TIMER | EVT_NOTIFY_SIGNAL, TPL_CALLBACK, RefreshIntervalProcess, NULL, &RefreshIntervalEvent);
+    ASSERT_EFI_ERROR (Status);
+    Status = gBS->SetTimer (RefreshIntervalEvent, TimerPeriodic, MinRefreshInterval * ONE_SECOND);
+    ASSERT_EFI_ERROR (Status);
+
+    EventNode = AllocateZeroPool (sizeof (FORM_BROWSER_REFRESH_EVENT_NODE));
+    ASSERT (EventNode != NULL);
+    EventNode->RefreshEvent = RefreshIntervalEvent;
+    InsertTailList(&mRefreshEventList, &EventNode->Link);
+  }
+
+  //
+  // Create the refresh event process function for Form.
+  //
+  if (!CompareGuid (&gCurrentSelection->Form->RefreshGuid, &gZeroGuid)) {
+    CreateRefreshEventForForm (gCurrentSelection->Form);
+    if (gDisplayFormData.FormRefreshEvent == NULL) {
+      gDisplayFormData.FormRefreshEvent = mValueChangedEvent;
+    }
+  }
+
+  //
+  // Update hotkey list field.
+  //
+  if (gBrowserSettingScope == SystemLevel || FormEditable) {
+    UpdateHotkeyList();
+  }
+}
+
+/**
+
+  Initialize the SettingChangedFlag variable in the display form.
+
+**/
+VOID
+UpdateDataChangedFlag (
+  VOID
+  )
+{
+  LIST_ENTRY           *Link;
+  FORM_BROWSER_FORMSET *LocalFormSet;
+
+  gDisplayFormData.SettingChangedFlag   = FALSE;
+
+  if (IsNvUpdateRequiredForForm (gCurrentSelection->Form)) {
+    gDisplayFormData.SettingChangedFlag = TRUE;
+    return;
+  }
+
+  //
+  // Base on the system level to check whether need to show the NV flag.
+  // 
+  switch (gBrowserSettingScope) {
+  case SystemLevel:
+    //
+    // Check the maintain list to see whether there is any change.
+    //
+    Link = GetFirstNode (&gBrowserFormSetList);
+    while (!IsNull (&gBrowserFormSetList, Link)) {
+      LocalFormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+      if (IsNvUpdateRequiredForFormSet(LocalFormSet)) {
+        gDisplayFormData.SettingChangedFlag = TRUE;
+        return;
+      }
+      Link = GetNextNode (&gBrowserFormSetList, Link);
+    }
+    break;
+
+  case FormSetLevel:
+    if (IsNvUpdateRequiredForFormSet(gCurrentSelection->FormSet)) {
+      gDisplayFormData.SettingChangedFlag = TRUE;
+      return;
+    }
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+
+  Initialize the Display form structure data.
+
+**/
+VOID
+InitializeDisplayFormData (
+  VOID
+  )
+{
+  EFI_STATUS  Status;
+
+  gDisplayFormData.Signature   = FORM_DISPLAY_ENGINE_FORM_SIGNATURE;
+  gDisplayFormData.Version     = FORM_DISPLAY_ENGINE_VERSION_1;
+  gDisplayFormData.ImageId     = 0;
+  gDisplayFormData.AnimationId = 0;
+
+  InitializeListHead (&gDisplayFormData.StatementListHead);
+  InitializeListHead (&gDisplayFormData.StatementListOSF);
+  InitializeListHead (&gDisplayFormData.HotKeyListHead);
+
+  Status = gBS->CreateEvent (
+        EVT_NOTIFY_WAIT, 
+        TPL_CALLBACK,
+        SetupBrowserEmptyFunction,
+        NULL,
+        &mValueChangedEvent
+        );
+  ASSERT_EFI_ERROR (Status); 
+}
+
+/**
+
+  Free the kotkey info saved in form data.
+
+**/
+VOID
+FreeHotkeyList (
+  VOID
+  )
+{
+  BROWSER_HOT_KEY  *HotKey;
+  LIST_ENTRY       *Link;
+
+  while (!IsListEmpty (&gDisplayFormData.HotKeyListHead)) {
+    Link = GetFirstNode (&gDisplayFormData.HotKeyListHead);
+    HotKey = BROWSER_HOT_KEY_FROM_LINK (Link);
+
+    RemoveEntryList (&HotKey->Link);
+
+    FreePool (HotKey->KeyData);
+    FreePool (HotKey->HelpString);
+    FreePool (HotKey);
+  }
+}
+
+/**
+
+  Update the Display form structure data.
+
+**/
+VOID
+UpdateDisplayFormData (
+  VOID
+  )
+{
+  gDisplayFormData.FormTitle        = gCurrentSelection->Form->FormTitle;
+  gDisplayFormData.FormId           = gCurrentSelection->FormId;
+  gDisplayFormData.HiiHandle        = gCurrentSelection->Handle;
+  CopyGuid (&gDisplayFormData.FormSetGuid, &gCurrentSelection->FormSetGuid);
+
+  gDisplayFormData.Attribute        = 0;
+  gDisplayFormData.Attribute       |= gCurrentSelection->Form->ModalForm ? HII_DISPLAY_MODAL : 0;
+  gDisplayFormData.Attribute       |= gCurrentSelection->Form->Locked    ? HII_DISPLAY_LOCK  : 0;
+
+  gDisplayFormData.FormRefreshEvent     = NULL;
+  gDisplayFormData.HighLightedStatement = NULL;
+
+  UpdateDataChangedFlag ();
+
+  AddStatementToDisplayForm ();
+}
+
+/**
+
+  Free the Display Statement structure data.
+
+  @param   StatementList         Point to the statement list which need to be free.
+
+**/
+VOID
+FreeStatementData (
+  LIST_ENTRY           *StatementList
+  )
+{
+  LIST_ENTRY                    *Link;
+  LIST_ENTRY                    *OptionLink;
+  FORM_DISPLAY_ENGINE_STATEMENT *Statement;
+  DISPLAY_QUESTION_OPTION       *Option;
+
+  //
+  // Free Statements/Questions
+  //
+  while (!IsListEmpty (StatementList)) {
+    Link = GetFirstNode (StatementList);
+    Statement = FORM_DISPLAY_ENGINE_STATEMENT_FROM_LINK (Link);
+
+    //
+    // Free Options List
+    //
+    while (!IsListEmpty (&Statement->OptionListHead)) {
+      OptionLink = GetFirstNode (&Statement->OptionListHead);
+      Option = DISPLAY_QUESTION_OPTION_FROM_LINK (OptionLink);
+      RemoveEntryList (&Option->Link);
+      FreePool (Option);
+    }
+
+    //
+    // Free nest statement List
+    //
+    if (!IsListEmpty (&Statement->NestStatementList)) {
+      FreeStatementData(&Statement->NestStatementList);
+    }
+
+    RemoveEntryList (&Statement->DisplayLink);
+    FreePool (Statement);
+  }
+}
+
+/**
+
+  Free the Display form structure data.
+
+**/
+VOID
+FreeDisplayFormData (
+  VOID
+  )
+{
+  FreeStatementData (&gDisplayFormData.StatementListHead);
+  FreeStatementData (&gDisplayFormData.StatementListOSF);
+
+  FreeRefreshEvent();
+
+  FreeHotkeyList();
+}
+
+/**
+
+  Get FORM_BROWSER_STATEMENT from FORM_DISPLAY_ENGINE_STATEMENT based on the OpCode info.
+
+  @param DisplayStatement        The input FORM_DISPLAY_ENGINE_STATEMENT.
+
+  @retval FORM_BROWSER_STATEMENT  The return FORM_BROWSER_STATEMENT info.
+
+**/
+FORM_BROWSER_STATEMENT *
+GetBrowserStatement (
+  IN FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement
+  )
+{
+  FORM_BROWSER_STATEMENT *Statement;
+  LIST_ENTRY             *Link;
+
+  Link = GetFirstNode (&gCurrentSelection->Form->StatementListHead);
+  while (!IsNull (&gCurrentSelection->Form->StatementListHead, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+
+    if (Statement->OpCode == DisplayStatement->OpCode) {
+      return Statement;
+    }
+
+    Link = GetNextNode (&gCurrentSelection->Form->StatementListHead, Link);
+  }
+
+  return NULL;
+}
+
+/**
+  Update the ValueChanged status for questions in this form.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+**/
+VOID 
+UpdateStatementStatusForForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form
+  )
+{
+  LIST_ENTRY                  *Link;
+  FORM_BROWSER_STATEMENT      *Question;
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&Form->StatementListHead, Link);
+
+    //
+    // For password opcode, not set the the value changed flag.
+    //
+    if (Question->Operand == EFI_IFR_PASSWORD_OP) {
+      continue;
+    }
+
+    IsQuestionValueChanged(FormSet, Form, Question, GetSetValueWithBuffer);
+  }
+}
+
+/**
+  Update the ValueChanged status for questions in this formset.
+
+  @param  FormSet                FormSet data structure.
+
+**/
+VOID 
+UpdateStatementStatusForFormSet (
+  IN FORM_BROWSER_FORMSET                *FormSet
+  )
+{
+  LIST_ENTRY                  *Link;
+  FORM_BROWSER_FORM           *Form;
+
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+
+    UpdateStatementStatusForForm (FormSet, Form);
+  }
+}
+
+/**
+  Update the ValueChanged status for questions.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Default action.
+
+**/
+VOID 
+UpdateStatementStatus (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form, 
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  )
+{
+  LIST_ENTRY                  *Link;
+  FORM_BROWSER_FORMSET        *LocalFormSet;
+
+  switch (SettingScope) {
+  case SystemLevel:
+    Link = GetFirstNode (&gBrowserFormSetList);
+    while (!IsNull (&gBrowserFormSetList, Link)) {
+      LocalFormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+      Link = GetNextNode (&gBrowserFormSetList, Link);
+      if (!ValidateFormSet(LocalFormSet)) {
+        continue;
+      }
+
+      UpdateStatementStatusForFormSet (LocalFormSet);
+    }
+    break;
+
+  case FormSetLevel:
+    UpdateStatementStatusForFormSet (FormSet);
+    break;
+
+  case FormLevel:
+    UpdateStatementStatusForForm (FormSet, Form);
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+
+  Process the action request in user input.
+
+  @param Action                  The user input action request info.
+  @param DefaultId               The user input default Id info.
+
+  @retval EFI_SUCESSS            This function always return successfully for now.
+
+**/
+EFI_STATUS 
+ProcessAction (
+  IN UINT32        Action,
+  IN UINT16        DefaultId
+  )
+{
+  //
+  // This is caused by use press ESC, and it should not combine with other action type.
+  //
+  if ((Action & BROWSER_ACTION_FORM_EXIT) == BROWSER_ACTION_FORM_EXIT) {
+    FindNextMenu (gCurrentSelection, FormLevel);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Below is normal hotkey trigged action, these action maybe combine with each other.
+  //
+  if ((Action & BROWSER_ACTION_DISCARD) == BROWSER_ACTION_DISCARD) {
+    DiscardForm (gCurrentSelection->FormSet, gCurrentSelection->Form, gBrowserSettingScope);
+  }
+
+  if ((Action & BROWSER_ACTION_DEFAULT) == BROWSER_ACTION_DEFAULT) {
+    ExtractDefault (gCurrentSelection->FormSet, gCurrentSelection->Form, DefaultId, gBrowserSettingScope, GetDefaultForAll, NULL, FALSE, FALSE);
+    UpdateStatementStatus (gCurrentSelection->FormSet, gCurrentSelection->Form, gBrowserSettingScope);
+  }
+
+  if ((Action & BROWSER_ACTION_SUBMIT) == BROWSER_ACTION_SUBMIT) {
+    SubmitForm (gCurrentSelection->FormSet, gCurrentSelection->Form, gBrowserSettingScope);
+  }
+
+  if ((Action & BROWSER_ACTION_RESET) == BROWSER_ACTION_RESET) {
+    gResetRequired = TRUE;
+  }
+
+  if ((Action & BROWSER_ACTION_EXIT) == BROWSER_ACTION_EXIT) {
+    //
+    // Form Exit without saving, Similar to ESC Key.
+    // FormSet Exit without saving, Exit SendForm.
+    // System Exit without saving, CallExitHandler and Exit SendForm.
+    //
+    DiscardForm (gCurrentSelection->FormSet, gCurrentSelection->Form, gBrowserSettingScope);
+    if (gBrowserSettingScope == FormLevel || gBrowserSettingScope == FormSetLevel) {
+      FindNextMenu (gCurrentSelection, gBrowserSettingScope);
+    } else if (gBrowserSettingScope == SystemLevel) {
+      if (ExitHandlerFunction != NULL) {
+        ExitHandlerFunction ();
+      }
+      gCurrentSelection->Action = UI_ACTION_EXIT;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Check whether the formset guid is in this Hii package list.
+
+  @param  HiiHandle              The HiiHandle for this HII package list.
+  @param  FormSetGuid            The formset guid for the request formset.
+
+  @retval TRUE                   Find the formset guid.
+  @retval FALSE                  Not found the formset guid.
+
+**/
+BOOLEAN
+GetFormsetGuidFromHiiHandle (
+  IN EFI_HII_HANDLE       HiiHandle,
+  IN EFI_GUID             *FormSetGuid
+  )
+{
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINTN                        BufferSize;
+  UINT32                       Offset;
+  UINT32                       Offset2;
+  UINT32                       PackageListLength;
+  EFI_HII_PACKAGE_HEADER       PackageHeader;
+  UINT8                        *Package;
+  UINT8                        *OpCodeData;
+  EFI_STATUS                   Status;
+  BOOLEAN                      FindGuid;
+
+  BufferSize     = 0;
+  HiiPackageList = NULL;
+  FindGuid       = FALSE;
+  
+  Status = mHiiDatabase->ExportPackageLists (mHiiDatabase, HiiHandle, &BufferSize, HiiPackageList);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    HiiPackageList = AllocatePool (BufferSize);
+    ASSERT (HiiPackageList != NULL);
+
+    Status = mHiiDatabase->ExportPackageLists (mHiiDatabase, HiiHandle, &BufferSize, HiiPackageList);
+  }
+  if (EFI_ERROR (Status) || HiiPackageList == NULL) {
+    return FALSE;
+  }
+
+  //
+  // Get Form package from this HII package List
+  //
+  Offset = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  Offset2 = 0;
+  CopyMem (&PackageListLength, &HiiPackageList->PackageLength, sizeof (UINT32)); 
+
+  while (Offset < PackageListLength) {
+    Package = ((UINT8 *) HiiPackageList) + Offset;
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+    Offset += PackageHeader.Length;
+
+    if (PackageHeader.Type == EFI_HII_PACKAGE_FORMS) {
+      //
+      // Search FormSet in this Form Package
+      //
+      Offset2 = sizeof (EFI_HII_PACKAGE_HEADER);
+      while (Offset2 < PackageHeader.Length) {
+        OpCodeData = Package + Offset2;
+
+        if (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode == EFI_IFR_FORM_SET_OP) {
+          if (CompareGuid (FormSetGuid, (EFI_GUID *)(OpCodeData + sizeof (EFI_IFR_OP_HEADER)))){
+            FindGuid = TRUE;
+            break;
+          }
+        }
+
+        Offset2 += ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+      }
+    }
+    if (FindGuid) {
+      break;
+    }
+  }
+
+  FreePool (HiiPackageList);
+
+  return FindGuid;
+}
+
+/**
+  Find HII Handle in the HII database associated with given Device Path.
+
+  If DevicePath is NULL, then ASSERT.
+
+  @param  DevicePath             Device Path associated with the HII package list
+                                 handle.
+  @param  FormsetGuid            The formset guid for this formset.
+
+  @retval Handle                 HII package list Handle associated with the Device
+                                        Path.
+  @retval NULL                   Hii Package list handle is not found.
+
+**/
+EFI_HII_HANDLE
+DevicePathToHiiHandle (
+  IN EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  IN EFI_GUID                   *FormsetGuid
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_DEVICE_PATH_PROTOCOL    *TmpDevicePath;
+  UINTN                       Index;
+  EFI_HANDLE                  Handle;
+  EFI_HANDLE                  DriverHandle;
+  EFI_HII_HANDLE              *HiiHandles;
+  EFI_HII_HANDLE              HiiHandle;
+
+  ASSERT (DevicePath != NULL);
+
+  TmpDevicePath = DevicePath;
+  //
+  // Locate Device Path Protocol handle buffer
+  //
+  Status = gBS->LocateDevicePath (
+                  &gEfiDevicePathProtocolGuid,
+                  &TmpDevicePath,
+                  &DriverHandle
+                  );
+  if (EFI_ERROR (Status) || !IsDevicePathEnd (TmpDevicePath)) {
+    return NULL;
+  }
+
+  //
+  // Retrieve all HII Handles from HII database
+  //
+  HiiHandles = HiiGetHiiHandles (NULL);
+  if (HiiHandles == NULL) {
+    return NULL;
+  }
+
+  //
+  // Search Hii Handle by Driver Handle
+  //
+  HiiHandle = NULL;
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    Status = mHiiDatabase->GetPackageListHandle (
+                             mHiiDatabase,
+                             HiiHandles[Index],
+                             &Handle
+                             );
+    if (!EFI_ERROR (Status) && (Handle == DriverHandle)) {
+      if (GetFormsetGuidFromHiiHandle(HiiHandles[Index], FormsetGuid)) {
+        HiiHandle = HiiHandles[Index];
+        break;
+      }
+
+      if (HiiHandle != NULL) {
+        break;
+      }
+    }
+  }
+
+  FreePool (HiiHandles);
+  return HiiHandle;
+}
+
+/**
+  Find HII Handle in the HII database associated with given form set guid.
+
+  If FormSetGuid is NULL, then ASSERT.
+
+  @param  ComparingGuid          FormSet Guid associated with the HII package list
+                                 handle.
+
+  @retval Handle                 HII package list Handle associated with the Device
+                                        Path.
+  @retval NULL                   Hii Package list handle is not found.
+
+**/
+EFI_HII_HANDLE
+FormSetGuidToHiiHandle (
+  EFI_GUID     *ComparingGuid
+  )
+{
+  EFI_HII_HANDLE               *HiiHandles;
+  EFI_HII_HANDLE               HiiHandle;
+  UINTN                        Index;
+
+  ASSERT (ComparingGuid != NULL);
+
+  HiiHandle  = NULL;
+  //
+  // Get all the Hii handles
+  //
+  HiiHandles = HiiGetHiiHandles (NULL);
+  ASSERT (HiiHandles != NULL);
+
+  //
+  // Search for formset of each class type
+  //
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    if (GetFormsetGuidFromHiiHandle(HiiHandles[Index], ComparingGuid)) {
+      HiiHandle = HiiHandles[Index];
+      break;
+    }
+
+    if (HiiHandle != NULL) {
+      break;
+    }
+  }
+
+  FreePool (HiiHandles);
+
+  return HiiHandle;
+}
+
+/**
+  check how to process the changed data in current form or form set.
+
+  @param Selection       On input, Selection tell setup browser the information
+                         about the Selection, form and formset to be displayed.
+                         On output, Selection return the screen item that is selected
+                         by user.
+
+  @param Scope           Data save or discard scope, form or formset.
+
+  @retval                TRUE   Success process the changed data, will return to the parent form.
+  @retval                FALSE  Reject to process the changed data, will stay at  current form.
+**/
+BOOLEAN
+ProcessChangedData (
+  IN OUT UI_MENU_SELECTION       *Selection,
+  IN     BROWSER_SETTING_SCOPE   Scope
+  )
+{
+  BOOLEAN    RetValue;
+  EFI_STATUS Status;
+
+  RetValue = TRUE;
+  switch (mFormDisplay->ConfirmDataChange()) {
+    case BROWSER_ACTION_DISCARD:
+      DiscardForm (Selection->FormSet, Selection->Form, Scope);
+      break;
+  
+    case BROWSER_ACTION_SUBMIT:
+      Status = SubmitForm (Selection->FormSet, Selection->Form, Scope);
+      if (EFI_ERROR (Status)) {
+        RetValue = FALSE;
+      }
+      break;
+
+    case BROWSER_ACTION_NONE:
+      RetValue = FALSE;
+      break;
+
+    default:
+      //
+      // if Invalid value return, process same as BROWSER_ACTION_NONE.
+      //
+      RetValue = FALSE;
+      break;
+  }
+
+  return RetValue;
+}
+
+/**
+  Find parent formset menu(the first menu which has different formset) for current menu.
+  If not find, just return to the first menu.
+
+  @param Selection    The selection info.
+
+**/
+VOID
+FindParentFormSet (
+  IN OUT   UI_MENU_SELECTION           *Selection
+  )
+{
+  FORM_ENTRY_INFO            *CurrentMenu;
+  FORM_ENTRY_INFO            *ParentMenu;
+
+  CurrentMenu = Selection->CurrentMenu;
+  ParentMenu  = UiFindParentMenu(CurrentMenu, FormSetLevel);
+
+  if (ParentMenu != NULL) {
+    CopyMem (&Selection->FormSetGuid, &ParentMenu->FormSetGuid, sizeof (EFI_GUID));
+    Selection->Handle = ParentMenu->HiiHandle;
+    Selection->FormId     = ParentMenu->FormId;
+    Selection->QuestionId = ParentMenu->QuestionId;
+  } else {
+    Selection->FormId     = CurrentMenu->FormId;
+    Selection->QuestionId = CurrentMenu->QuestionId;
+  }
+
+  Selection->Statement  = NULL;
+}
+
+/**
+  Process the goto op code, update the info in the selection structure.
+
+  @param Statement    The statement belong to goto op code.
+  @param Selection    The selection info.
+
+  @retval EFI_SUCCESS    The menu process successfully.
+  @return Other value if the process failed.
+**/
+EFI_STATUS
+ProcessGotoOpCode (
+  IN OUT   FORM_BROWSER_STATEMENT      *Statement,
+  IN OUT   UI_MENU_SELECTION           *Selection
+  )
+{
+  CHAR16                          *StringPtr;
+  EFI_DEVICE_PATH_PROTOCOL        *DevicePath;
+  FORM_BROWSER_FORM               *RefForm;
+  EFI_STATUS                      Status;
+  EFI_HII_HANDLE                  HiiHandle;
+  
+  Status    = EFI_SUCCESS;
+  StringPtr = NULL;
+  HiiHandle = NULL;
+
+  //
+  // Prepare the device path check, get the device path info first.
+  //
+  if (Statement->HiiValue.Value.ref.DevicePath != 0) {
+    StringPtr = GetToken (Statement->HiiValue.Value.ref.DevicePath, Selection->FormSet->HiiHandle);
+  }
+
+  //
+  // Check whether the device path string is a valid string.
+  //
+  if (Statement->HiiValue.Value.ref.DevicePath != 0 && StringPtr != NULL && StringPtr[0] != L'\0') {
+    if (Selection->Form->ModalForm) {
+      return Status;
+    }
+
+    //
+    // Goto another Hii Package list
+    //
+    if (mPathFromText != NULL) {
+      DevicePath = mPathFromText->ConvertTextToDevicePath(StringPtr);
+      if (DevicePath != NULL) {
+        HiiHandle = DevicePathToHiiHandle (DevicePath, &Statement->HiiValue.Value.ref.FormSetGuid);
+        FreePool (DevicePath);
+      }
+      FreePool (StringPtr);
+    } else {
+      //
+      // Not found the EFI_DEVICE_PATH_FROM_TEXT_PROTOCOL protocol.
+      //
+      PopupErrorMessage(BROWSER_PROTOCOL_NOT_FOUND, NULL, NULL, NULL);
+      FreePool (StringPtr);
+      return Status;
+    }
+
+    if (HiiHandle != Selection->Handle) {
+      //
+      // Goto another Formset, check for uncommitted data
+      //
+      if ((gBrowserSettingScope == FormLevel || gBrowserSettingScope == FormSetLevel) &&
+          IsNvUpdateRequiredForFormSet(Selection->FormSet)) {
+        if (!ProcessChangedData(Selection, FormSetLevel)) {
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    Selection->Action = UI_ACTION_REFRESH_FORMSET;
+    Selection->Handle = HiiHandle;
+    if (Selection->Handle == NULL) {
+      //
+      // If target Hii Handle not found, exit current formset.
+      //
+      FindParentFormSet(Selection);
+      return EFI_SUCCESS;
+    }
+
+    CopyMem (&Selection->FormSetGuid,&Statement->HiiValue.Value.ref.FormSetGuid, sizeof (EFI_GUID));
+    Selection->FormId = Statement->HiiValue.Value.ref.FormId;
+    Selection->QuestionId = Statement->HiiValue.Value.ref.QuestionId;
+  } else if (!CompareGuid (&Statement->HiiValue.Value.ref.FormSetGuid, &gZeroGuid)) {
+    if (Selection->Form->ModalForm) {
+      return Status;
+    }
+    if (!CompareGuid (&Statement->HiiValue.Value.ref.FormSetGuid, &Selection->FormSetGuid)) {
+      //
+      // Goto another Formset, check for uncommitted data
+      //
+      if ((gBrowserSettingScope == FormLevel || gBrowserSettingScope == FormSetLevel) &&
+         IsNvUpdateRequiredForFormSet(Selection->FormSet)) {
+        if (!ProcessChangedData(Selection, FormSetLevel)) {
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    Selection->Action = UI_ACTION_REFRESH_FORMSET;
+    Selection->Handle = FormSetGuidToHiiHandle(&Statement->HiiValue.Value.ref.FormSetGuid);
+    if (Selection->Handle == NULL) {
+      //
+      // If target Hii Handle not found, exit current formset.
+      //
+      FindParentFormSet(Selection);
+      return EFI_SUCCESS;
+    }
+
+    CopyMem (&Selection->FormSetGuid, &Statement->HiiValue.Value.ref.FormSetGuid, sizeof (EFI_GUID));
+    Selection->FormId = Statement->HiiValue.Value.ref.FormId;
+    Selection->QuestionId = Statement->HiiValue.Value.ref.QuestionId;
+  } else if (Statement->HiiValue.Value.ref.FormId != 0) {
+    //
+    // Goto another Form, check for uncommitted data
+    //
+    if (Statement->HiiValue.Value.ref.FormId != Selection->FormId) {
+      if ((gBrowserSettingScope == FormLevel && IsNvUpdateRequiredForForm(Selection->Form))) {
+        if (!ProcessChangedData (Selection, FormLevel)) {
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    RefForm = IdToForm (Selection->FormSet, Statement->HiiValue.Value.ref.FormId);
+    if ((RefForm != NULL) && (RefForm->SuppressExpression != NULL)) {
+      if (EvaluateExpressionList(RefForm->SuppressExpression, TRUE, Selection->FormSet, RefForm) != ExpressFalse) {
+        //
+        // Form is suppressed. 
+        //
+        PopupErrorMessage(BROWSER_FORM_SUPPRESS, NULL, NULL, NULL);
+        return EFI_SUCCESS;
+      }
+    }
+
+    Selection->FormId = Statement->HiiValue.Value.ref.FormId;
+    Selection->QuestionId = Statement->HiiValue.Value.ref.QuestionId;
+  } else if (Statement->HiiValue.Value.ref.QuestionId != 0) {
+    Selection->QuestionId = Statement->HiiValue.Value.ref.QuestionId;
+  }
+
+  return Status;
+}
+
+
+/**
+  Process Question Config.
+
+  @param  Selection              The UI menu selection.
+  @param  Question               The Question to be peocessed.
+
+  @retval EFI_SUCCESS            Question Config process success.
+  @retval Other                  Question Config process fail.
+
+**/
+EFI_STATUS
+ProcessQuestionConfig (
+  IN  UI_MENU_SELECTION       *Selection,
+  IN  FORM_BROWSER_STATEMENT  *Question
+  )
+{
+  EFI_STATUS                      Status;
+  CHAR16                          *ConfigResp;
+  CHAR16                          *Progress;
+
+  if (Question->QuestionConfig == 0) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get <ConfigResp>
+  //
+  ConfigResp = GetToken (Question->QuestionConfig, Selection->FormSet->HiiHandle);
+  if (ConfigResp == NULL) {
+    return EFI_NOT_FOUND;
+  } else if (ConfigResp[0] == L'\0') {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Send config to Configuration Driver
+  //
+  Status = mHiiConfigRouting->RouteConfig (
+                           mHiiConfigRouting,
+                           ConfigResp,
+                           &Progress
+                           );
+
+  return Status;
+}
+
+/**
+
+  Process the user input data.
+
+  @param UserInput               The user input data.
+
+  @retval EFI_SUCESSS            This function always return successfully for now.
+
+**/
+EFI_STATUS
+ProcessUserInput (
+  IN USER_INPUT               *UserInput
+  )
+{
+  EFI_STATUS                    Status;
+  FORM_BROWSER_STATEMENT        *Statement;
+
+  Status    = EFI_SUCCESS;
+  Statement = NULL;
+
+  //
+  // When Exit from FormDisplay function, one of the below two cases must be true.
+  //
+  ASSERT (UserInput->Action != 0 || UserInput->SelectedStatement != NULL);
+
+  //
+  // Remove the last highligh question id, this id will update when show next form.
+  //
+  gCurrentSelection->QuestionId = 0;
+  if (UserInput->SelectedStatement != NULL){
+    Statement = GetBrowserStatement(UserInput->SelectedStatement);
+    ASSERT (Statement != NULL);
+
+    //
+    // This question is the current user select one,record it and later
+    // show it as the highlight question.
+    //
+    gCurrentSelection->CurrentMenu->QuestionId = Statement->QuestionId;
+    //
+    // For statement like text, actio, it not has question id.
+    // So use FakeQuestionId to save the question.
+    //
+    if (gCurrentSelection->CurrentMenu->QuestionId == 0) {
+      mCurFakeQestId = Statement->FakeQuestionId;
+    } else {
+      mCurFakeQestId = 0;
+    }
+  }
+
+  //
+  // First process the Action field in USER_INPUT.
+  //
+  if (UserInput->Action != 0) {
+    Status = ProcessAction (UserInput->Action, UserInput->DefaultId);
+    gCurrentSelection->Statement = NULL;
+  } else {
+    ASSERT (Statement != NULL);
+    gCurrentSelection->Statement = Statement;
+    switch (Statement->Operand) {
+    case EFI_IFR_REF_OP:
+      Status = ProcessGotoOpCode(Statement, gCurrentSelection);
+      break;
+    
+    case EFI_IFR_ACTION_OP:
+      //
+      // Process the Config string <ConfigResp>
+      //
+      Status = ProcessQuestionConfig (gCurrentSelection, Statement);
+      break;
+    
+    case EFI_IFR_RESET_BUTTON_OP:
+      //
+      // Reset Question to default value specified by DefaultId
+      //
+      Status = ExtractDefault (gCurrentSelection->FormSet, NULL, Statement->DefaultId, FormSetLevel, GetDefaultForAll, NULL, FALSE, FALSE);
+      UpdateStatementStatus (gCurrentSelection->FormSet, NULL, FormSetLevel);
+      break;
+
+    default:
+      switch (Statement->Operand) {
+      case EFI_IFR_STRING_OP:
+        DeleteString(Statement->HiiValue.Value.string, gCurrentSelection->FormSet->HiiHandle);
+        Statement->HiiValue.Value.string = UserInput->InputValue.Value.string;
+        CopyMem (Statement->BufferValue, UserInput->InputValue.Buffer, (UINTN) UserInput->InputValue.BufferLen);
+        FreePool (UserInput->InputValue.Buffer);
+        break;
+
+      case EFI_IFR_PASSWORD_OP:
+        if (UserInput->InputValue.Buffer == NULL) {
+          //
+          // User not input new password, just return.
+          //
+          break;
+        }
+
+        DeleteString(Statement->HiiValue.Value.string, gCurrentSelection->FormSet->HiiHandle);
+        Statement->HiiValue.Value.string = UserInput->InputValue.Value.string;
+        CopyMem (Statement->BufferValue, UserInput->InputValue.Buffer, (UINTN) UserInput->InputValue.BufferLen);
+        FreePool (UserInput->InputValue.Buffer);
+        //
+        // Two password match, send it to Configuration Driver
+        //
+        if ((Statement->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != 0) {
+          PasswordCheck (NULL, UserInput->SelectedStatement, (CHAR16 *) Statement->BufferValue);
+          //
+          // Clean the value after saved it.
+          //
+          ZeroMem (Statement->BufferValue, (UINTN) UserInput->InputValue.BufferLen);
+          HiiSetString (gCurrentSelection->FormSet->HiiHandle, Statement->HiiValue.Value.string, (CHAR16*)Statement->BufferValue, NULL);
+        } else {
+          SetQuestionValue (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithHiiDriver);
+        }
+        break;
+
+      case EFI_IFR_ORDERED_LIST_OP:
+        CopyMem (Statement->BufferValue, UserInput->InputValue.Buffer, UserInput->InputValue.BufferLen);
+        break;
+
+      default:
+        CopyMem (&Statement->HiiValue, &UserInput->InputValue, sizeof (EFI_HII_VALUE));
+        break;
+      }
+      break;
+    }
+  }
+
+  return Status;
+}
+
+/**
+
+  Display form and wait for user to select one menu option, then return it.
+
+  @retval EFI_SUCESSS            This function always return successfully for now.
+
+**/
+EFI_STATUS
+DisplayForm (
+  VOID
+  )
+{
+  EFI_STATUS               Status;
+  USER_INPUT               UserInput;
+  FORM_ENTRY_INFO          *CurrentMenu;
+
+  ZeroMem (&UserInput, sizeof (USER_INPUT));
+
+  //
+  // Update the menu history data.
+  //
+  CurrentMenu = UiFindMenuList (gCurrentSelection->Handle, &gCurrentSelection->FormSetGuid, gCurrentSelection->FormId);
+  if (CurrentMenu == NULL) {
+    //
+    // Current menu not found, add it to the menu tree
+    //
+    CurrentMenu = UiAddMenuList (gCurrentSelection->Handle, &gCurrentSelection->FormSetGuid,
+                                 gCurrentSelection->FormId, gCurrentSelection->QuestionId);
+    ASSERT (CurrentMenu != NULL);
+  }
+
+  //
+  // Back up the form view history data for this form.
+  //
+  UiCopyMenuList(&gCurrentSelection->Form->FormViewListHead, &mPrivateData.FormBrowserEx2.FormViewHistoryHead);
+
+  gCurrentSelection->CurrentMenu = CurrentMenu;
+
+  if (gCurrentSelection->QuestionId == 0) {
+    //
+    // Highlight not specified, fetch it from cached menu
+    //
+    gCurrentSelection->QuestionId = CurrentMenu->QuestionId;
+  }
+
+  Status = EvaluateFormExpressions (gCurrentSelection->FormSet, gCurrentSelection->Form);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  UpdateDisplayFormData ();
+
+  ASSERT (gDisplayFormData.BrowserStatus == BROWSER_SUCCESS);
+  Status = mFormDisplay->FormDisplay (&gDisplayFormData, &UserInput);
+  if (EFI_ERROR (Status)) {
+    FreeDisplayFormData();
+    return Status;
+  }
+
+  Status = ProcessUserInput (&UserInput);
+  FreeDisplayFormData();
+  return Status;
+}
+
+/**
+  Functions which are registered to receive notification of
+  database events have this prototype. The actual event is encoded
+  in NotifyType. The following table describes how PackageType,
+  PackageGuid, Handle, and Package are used for each of the
+  notification types.
+
+  @param PackageType  Package type of the notification.
+
+  @param PackageGuid  If PackageType is
+                      EFI_HII_PACKAGE_TYPE_GUID, then this is
+                      the pointer to the GUID from the Guid
+                      field of EFI_HII_PACKAGE_GUID_HEADER.
+                      Otherwise, it must be NULL.
+
+  @param Package  Points to the package referred to by the
+                  notification Handle The handle of the package
+                  list which contains the specified package.
+
+  @param Handle       The HII handle.
+
+  @param NotifyType   The type of change concerning the
+                      database. See
+                      EFI_HII_DATABASE_NOTIFY_TYPE.
+
+**/
+EFI_STATUS
+EFIAPI
+FormUpdateNotify (
+  IN UINT8                              PackageType,
+  IN CONST EFI_GUID                     *PackageGuid,
+  IN CONST EFI_HII_PACKAGE_HEADER       *Package,
+  IN EFI_HII_HANDLE                     Handle,
+  IN EFI_HII_DATABASE_NOTIFY_TYPE       NotifyType
+  )
+{
+  mHiiPackageListUpdated = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update the NV flag info for this form set.
+
+  @param  FormSet                FormSet data structure.
+
+**/
+BOOLEAN
+IsNvUpdateRequiredForFormSet (
+  IN FORM_BROWSER_FORMSET  *FormSet
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_FORM       *Form;
+  BOOLEAN                 RetVal;
+
+  //
+  // Not finished question initialization, return FALSE.
+  //
+  if (!FormSet->QuestionInited) {
+    return FALSE;
+  }
+
+  RetVal = FALSE;
+
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+    RetVal = IsNvUpdateRequiredForForm(Form);
+    if (RetVal) {
+      break;
+    }
+
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+  }
+
+  return RetVal;
+}
+
+/**
+  Update the NvUpdateRequired flag for a form.
+
+  @param  Form                Form data structure.
+
+**/
+BOOLEAN
+IsNvUpdateRequiredForForm (
+  IN FORM_BROWSER_FORM    *Form
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Statement;
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+
+    if (Statement->ValueChanged) {
+      return TRUE;
+    }
+
+    Link = GetNextNode (&Form->StatementListHead, Link);
+  }
+
+  return FALSE;
+}
+
+/**
+  Find menu which will show next time.
+
+  @param Selection       On input, Selection tell setup browser the information
+                         about the Selection, form and formset to be displayed.
+                         On output, Selection return the screen item that is selected
+                         by user.
+  @param SettingLevel    Input Settting level, if it is FormLevel, just exit current form. 
+                         else, we need to exit current formset.
+  
+  @retval TRUE           Exit current form.
+  @retval FALSE          User press ESC and keep in current form.
+**/
+BOOLEAN
+FindNextMenu (
+  IN OUT UI_MENU_SELECTION        *Selection,
+  IN     BROWSER_SETTING_SCOPE     SettingLevel
+  )
+{
+  FORM_ENTRY_INFO            *CurrentMenu;
+  FORM_ENTRY_INFO            *ParentMenu;
+  BROWSER_SETTING_SCOPE      Scope;
+  
+  CurrentMenu = Selection->CurrentMenu;
+  Scope       = FormSetLevel;
+
+  ParentMenu = UiFindParentMenu(CurrentMenu, SettingLevel);
+  while (ParentMenu != NULL && !ValidateHiiHandle(ParentMenu->HiiHandle)) {
+    ParentMenu = UiFindParentMenu(ParentMenu, SettingLevel);
+  }
+
+  if (ParentMenu != NULL) {
+    if (CompareGuid (&CurrentMenu->FormSetGuid, &ParentMenu->FormSetGuid)) {
+      Scope = FormLevel;
+    } else {
+      Scope = FormSetLevel;
+    }
+  }
+
+  //
+  // Form Level Check whether the data is changed.
+  //
+  if ((gBrowserSettingScope == FormLevel && IsNvUpdateRequiredForForm (Selection->Form)) ||
+      (gBrowserSettingScope == FormSetLevel && IsNvUpdateRequiredForFormSet(Selection->FormSet) && Scope == FormSetLevel)) {
+    if (!ProcessChangedData(Selection, gBrowserSettingScope)) {
+      return FALSE;
+    }
+  }
+
+  if (ParentMenu != NULL) {
+    //
+    // ParentMenu is found. Then, go to it.
+    //
+    if (Scope == FormLevel) {
+      Selection->Action = UI_ACTION_REFRESH_FORM;
+    } else {
+      Selection->Action = UI_ACTION_REFRESH_FORMSET;
+      CopyMem (&Selection->FormSetGuid, &ParentMenu->FormSetGuid, sizeof (EFI_GUID));
+      Selection->Handle = ParentMenu->HiiHandle;
+    }
+
+    Selection->Statement = NULL;
+
+    Selection->FormId = ParentMenu->FormId;
+    Selection->QuestionId = ParentMenu->QuestionId;
+
+    //
+    // Clear highlight record for this menu
+    //
+    CurrentMenu->QuestionId = 0;
+    return FALSE;
+  }
+
+  //
+  // Current in root page, exit the SendForm
+  //
+  Selection->Action = UI_ACTION_EXIT;
+
+  return TRUE;
+}
+
+/**
+  Reconnect the controller.
+
+  @param DriverHandle          The controller handle which need to be reconnect.
+
+  @retval   TRUE     do the reconnect behavior success.
+  @retval   FALSE    do the reconnect behavior failed.
+  
+**/
+BOOLEAN
+ReconnectController (
+  IN EFI_HANDLE   DriverHandle
+  )
+{
+  EFI_STATUS                      Status;
+
+  Status = gBS->DisconnectController(DriverHandle, NULL, NULL);
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->ConnectController(DriverHandle, NULL, NULL, TRUE);
+  }
+
+  return Status == EFI_SUCCESS;
+}
+
+/**
+  Call the call back function for the question and process the return action.
+
+  @param Selection             On input, Selection tell setup browser the information
+                               about the Selection, form and formset to be displayed.
+                               On output, Selection return the screen item that is selected
+                               by user.
+  @param FormSet               The formset this question belong to.
+  @param Form                  The form this question belong to.
+  @param Question              The Question which need to call.
+  @param Action                The action request.
+  @param SkipSaveOrDiscard     Whether skip save or discard action.
+
+  @retval EFI_SUCCESS          The call back function excutes successfully.
+  @return Other value if the call back function failed to excute.  
+**/
+EFI_STATUS 
+ProcessCallBackFunction (
+  IN OUT UI_MENU_SELECTION               *Selection,
+  IN     FORM_BROWSER_FORMSET            *FormSet,
+  IN     FORM_BROWSER_FORM               *Form,
+  IN     FORM_BROWSER_STATEMENT          *Question,
+  IN     EFI_BROWSER_ACTION              Action,
+  IN     BOOLEAN                         SkipSaveOrDiscard
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_STATUS                      InternalStatus;
+  EFI_BROWSER_ACTION_REQUEST      ActionRequest;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+  EFI_HII_VALUE                   *HiiValue;
+  EFI_IFR_TYPE_VALUE              *TypeValue;
+  FORM_BROWSER_STATEMENT          *Statement;
+  BOOLEAN                         SubmitFormIsRequired;
+  BOOLEAN                         DiscardFormIsRequired;
+  BOOLEAN                         NeedExit;
+  LIST_ENTRY                      *Link;
+  BROWSER_SETTING_SCOPE           SettingLevel;
+  EFI_IFR_TYPE_VALUE              BackUpValue;
+  UINT8                           *BackUpBuffer;
+  CHAR16                          *NewString;
+
+  ConfigAccess = FormSet->ConfigAccess;
+  SubmitFormIsRequired  = FALSE;
+  SettingLevel          = FormSetLevel;
+  DiscardFormIsRequired = FALSE;
+  NeedExit              = FALSE;
+  Status                = EFI_SUCCESS;
+  ActionRequest         = EFI_BROWSER_ACTION_REQUEST_NONE;
+  BackUpBuffer          = NULL;
+
+  if (ConfigAccess == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Statement = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&Form->StatementListHead, Link);
+
+    //
+    // if Question != NULL, only process the question. Else, process all question in this form.
+    //
+    if ((Question != NULL) && (Statement != Question)) {
+      continue;
+    }
+    
+    if ((Statement->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != EFI_IFR_FLAG_CALLBACK) {
+      continue;
+    }
+
+    //
+    // Check whether Statement is disabled.
+    //
+    if (Statement->Expression != NULL) {
+      if (EvaluateExpressionList(Statement->Expression, TRUE, FormSet, Form) == ExpressDisable) {
+        continue;
+      }
+    }
+
+    HiiValue = &Statement->HiiValue;
+    TypeValue = &HiiValue->Value;
+    if (HiiValue->Type == EFI_IFR_TYPE_BUFFER) {
+      //
+      // For OrderedList, passing in the value buffer to Callback()
+      //
+      TypeValue = (EFI_IFR_TYPE_VALUE *) Statement->BufferValue;
+    }
+
+    //
+    // If EFI_BROWSER_ACTION_CHANGING type, back up the new question value.
+    //
+    if (Action == EFI_BROWSER_ACTION_CHANGING) {
+      if (HiiValue->Type == EFI_IFR_TYPE_BUFFER) {
+        BackUpBuffer = AllocateCopyPool(Statement->StorageWidth + sizeof(CHAR16), Statement->BufferValue);
+        ASSERT (BackUpBuffer != NULL);
+      } else {
+        CopyMem (&BackUpValue, &HiiValue->Value, sizeof (EFI_IFR_TYPE_VALUE));
+      }
+    }
+
+    ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+    Status = ConfigAccess->Callback (
+                             ConfigAccess,
+                             Action,
+                             Statement->QuestionId,
+                             HiiValue->Type,
+                             TypeValue,
+                             &ActionRequest
+                             );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Need to sync the value between Statement->HiiValue->Value and Statement->BufferValue
+      //
+      if (HiiValue->Type == EFI_IFR_TYPE_STRING) {
+        NewString = GetToken (Statement->HiiValue.Value.string, FormSet->HiiHandle);
+        ASSERT (NewString != NULL);
+
+        ASSERT (StrLen (NewString) * sizeof (CHAR16) <= Statement->StorageWidth);
+        if (StrLen (NewString) * sizeof (CHAR16) <= Statement->StorageWidth) {
+          CopyMem (Statement->BufferValue, NewString, StrSize (NewString));
+        } else {
+          CopyMem (Statement->BufferValue, NewString, Statement->StorageWidth);
+        }
+        FreePool (NewString);
+      }
+
+      //
+      // Only for EFI_BROWSER_ACTION_CHANGED need to handle this ActionRequest.
+      //
+      switch (Action) {
+      case EFI_BROWSER_ACTION_CHANGED:
+        switch (ActionRequest) {
+        case EFI_BROWSER_ACTION_REQUEST_RESET:
+          DiscardFormIsRequired = TRUE;
+          gResetRequired = TRUE;
+          NeedExit              = TRUE;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_SUBMIT:
+          SubmitFormIsRequired = TRUE;
+          NeedExit              = TRUE;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_EXIT:
+          DiscardFormIsRequired = TRUE;
+          NeedExit              = TRUE;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_FORM_SUBMIT_EXIT:
+          SubmitFormIsRequired  = TRUE;
+          SettingLevel          = FormLevel;
+          NeedExit              = TRUE;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD_EXIT:
+          DiscardFormIsRequired = TRUE;
+          SettingLevel          = FormLevel;
+          NeedExit              = TRUE;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_FORM_APPLY:
+          SubmitFormIsRequired  = TRUE;
+          SettingLevel          = FormLevel;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_FORM_DISCARD:
+          DiscardFormIsRequired = TRUE;
+          SettingLevel          = FormLevel;
+          break;
+
+        case EFI_BROWSER_ACTION_REQUEST_RECONNECT:
+          gCallbackReconnect    = TRUE;
+          break;
+
+        default:
+          break;
+        }
+        break;
+
+      case EFI_BROWSER_ACTION_CHANGING:
+        //
+        // Do the question validation.
+        //
+        Status = ValueChangedValidation (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement);
+        if (!EFI_ERROR (Status)) {
+          //
+          //check whether the question value  changed compared with edit buffer before updating edit buffer
+          // if changed, set the ValueChanged flag to TRUE,in order to trig the CHANGED callback function
+          //
+          IsQuestionValueChanged(gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithEditBuffer);
+          //
+          // According the spec, return value from call back of "changing" and 
+          // "retrieve" should update to the question's temp buffer.
+          //
+          SetQuestionValue(FormSet, Form, Statement, GetSetValueWithEditBuffer);
+        }
+        break;
+
+      case EFI_BROWSER_ACTION_RETRIEVE:
+        //
+        // According the spec, return value from call back of "changing" and 
+        // "retrieve" should update to the question's temp buffer.
+        //
+        SetQuestionValue(FormSet, Form, Statement, GetSetValueWithEditBuffer);
+        break;
+
+      default:
+        break;
+      }
+    } else {
+      //
+      // If the callback returns EFI_UNSUPPORTED for EFI_BROWSER_ACTION_CHANGING, 
+      // then the browser will use the value passed to Callback() and ignore the 
+      // value returned by Callback(). 
+      //
+      if (Action  == EFI_BROWSER_ACTION_CHANGING && Status == EFI_UNSUPPORTED) {
+        if (HiiValue->Type == EFI_IFR_TYPE_BUFFER) {
+          CopyMem (Statement->BufferValue, BackUpBuffer, Statement->StorageWidth + sizeof(CHAR16));
+        } else {
+          CopyMem (&HiiValue->Value, &BackUpValue, sizeof (EFI_IFR_TYPE_VALUE));
+        }
+        
+        //
+        // Do the question validation.
+        //
+        InternalStatus = ValueChangedValidation (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement);
+        if (!EFI_ERROR (InternalStatus)) {
+          //
+          //check whether the question value  changed compared with edit buffer before updating edit buffer
+          // if changed, set the ValueChanged flag to TRUE,in order to trig the CHANGED callback function
+          //
+          IsQuestionValueChanged(gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithEditBuffer);
+          SetQuestionValue(FormSet, Form, Statement, GetSetValueWithEditBuffer);
+        }
+      }
+
+      //
+      // According the spec, return fail from call back of "changing" and 
+      // "retrieve", should restore the question's value.
+      //
+      if (Action == EFI_BROWSER_ACTION_CHANGING && Status != EFI_UNSUPPORTED) {
+        if (Statement->Storage != NULL) {
+          GetQuestionValue(FormSet, Form, Statement, GetSetValueWithEditBuffer);
+        } else if ((Statement->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != 0) {
+          ProcessCallBackFunction (Selection, FormSet, Form, Question, EFI_BROWSER_ACTION_RETRIEVE, FALSE);
+        }
+      }
+
+      if (Action == EFI_BROWSER_ACTION_RETRIEVE) {
+        GetQuestionValue(FormSet, Form, Statement, GetSetValueWithEditBuffer);
+      }
+
+      if (Status == EFI_UNSUPPORTED) {
+        //
+        // If return EFI_UNSUPPORTED, also consider Hii driver suceess deal with it.
+        //
+        Status = EFI_SUCCESS;
+      }
+    }
+
+    if (BackUpBuffer != NULL) {
+      FreePool (BackUpBuffer);
+    }
+
+    //
+    // If Question != NULL, means just process one question
+    // and if code reach here means this question has finished
+    // processing, so just break.
+    //
+    if (Question != NULL) {
+      break;
+    }
+  }
+
+  if (gCallbackReconnect && (EFI_BROWSER_ACTION_CHANGED == Action)) {
+    //
+    // Confirm changes with user first.
+    //
+    if (IsNvUpdateRequiredForFormSet(FormSet)) {
+      if (BROWSER_ACTION_DISCARD == PopupErrorMessage(BROWSER_RECONNECT_SAVE_CHANGES, NULL, NULL, NULL)) {
+        gCallbackReconnect = FALSE;
+        DiscardFormIsRequired = TRUE;
+      } else {
+        SubmitFormIsRequired = TRUE;
+      }
+    } else {
+      PopupErrorMessage(BROWSER_RECONNECT_REQUIRED, NULL, NULL, NULL);
+    }
+
+    //
+    // Exit current formset before do the reconnect.
+    //
+    NeedExit = TRUE;
+    SettingLevel = FormSetLevel;
+  }
+
+  if (SubmitFormIsRequired && !SkipSaveOrDiscard) {
+    SubmitForm (FormSet, Form, SettingLevel);
+  }
+
+  if (DiscardFormIsRequired && !SkipSaveOrDiscard) {
+    DiscardForm (FormSet, Form, SettingLevel);
+  }
+
+  if (NeedExit) {
+    FindNextMenu (Selection, SettingLevel);
+  }
+
+  return Status;
+}
+
+/**
+  Call the retrieve type call back function for one question to get the initialize data.
+  
+  This function only used when in the initialize stage, because in this stage, the 
+  Selection->Form is not ready. For other case, use the ProcessCallBackFunction instead.
+
+  @param ConfigAccess          The config access protocol produced by the hii driver.
+  @param Statement             The Question which need to call.
+  @param FormSet               The formset this question belong to.
+
+  @retval EFI_SUCCESS          The call back function excutes successfully.
+  @return Other value if the call back function failed to excute.  
+**/
+EFI_STATUS 
+ProcessRetrieveForQuestion (
+  IN     EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess,
+  IN     FORM_BROWSER_STATEMENT          *Statement,
+  IN     FORM_BROWSER_FORMSET            *FormSet
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_BROWSER_ACTION_REQUEST      ActionRequest;
+  EFI_HII_VALUE                   *HiiValue;
+  EFI_IFR_TYPE_VALUE              *TypeValue;
+  CHAR16                          *NewString;
+
+  Status                = EFI_SUCCESS;
+  ActionRequest         = EFI_BROWSER_ACTION_REQUEST_NONE;
+
+  if (((Statement->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != EFI_IFR_FLAG_CALLBACK) || ConfigAccess == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  HiiValue  = &Statement->HiiValue;
+  TypeValue = &HiiValue->Value;
+  if (HiiValue->Type == EFI_IFR_TYPE_BUFFER) {
+    //
+    // For OrderedList, passing in the value buffer to Callback()
+    //
+    TypeValue = (EFI_IFR_TYPE_VALUE *) Statement->BufferValue;
+  }
+    
+  ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+  Status = ConfigAccess->Callback (
+                           ConfigAccess,
+                           EFI_BROWSER_ACTION_RETRIEVE,
+                           Statement->QuestionId,
+                           HiiValue->Type,
+                           TypeValue,
+                           &ActionRequest
+                           );
+  if (!EFI_ERROR (Status) && HiiValue->Type == EFI_IFR_TYPE_STRING) {
+    NewString = GetToken (Statement->HiiValue.Value.string, FormSet->HiiHandle);
+    ASSERT (NewString != NULL);
+
+    ASSERT (StrLen (NewString) * sizeof (CHAR16) <= Statement->StorageWidth);
+    if (StrLen (NewString) * sizeof (CHAR16) <= Statement->StorageWidth) {
+      CopyMem (Statement->BufferValue, NewString, StrSize (NewString));
+    } else {
+      CopyMem (Statement->BufferValue, NewString, Statement->StorageWidth);
+    }
+    FreePool (NewString);
+  }
+
+  return Status;
+}
+
+/**
+  The worker function that send the displays to the screen. On output,
+  the selection made by user is returned.
+
+  @param Selection       On input, Selection tell setup browser the information
+                         about the Selection, form and formset to be displayed.
+                         On output, Selection return the screen item that is selected
+                         by user.
+
+  @retval EFI_SUCCESS    The page is displayed successfully.
+  @return Other value if the page failed to be diplayed.
+
+**/
+EFI_STATUS
+SetupBrowser (
+  IN OUT UI_MENU_SELECTION    *Selection
+  )
+{
+  EFI_STATUS                      Status;
+  LIST_ENTRY                      *Link;
+  EFI_HANDLE                      NotifyHandle;
+  FORM_BROWSER_STATEMENT          *Statement;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+
+  ConfigAccess = Selection->FormSet->ConfigAccess;
+
+  //
+  // Register notify for Form package update
+  //
+  Status = mHiiDatabase->RegisterPackageNotify (
+                           mHiiDatabase,
+                           EFI_HII_PACKAGE_FORMS,
+                           NULL,
+                           FormUpdateNotify,
+                           EFI_HII_DATABASE_NOTIFY_REMOVE_PACK,
+                           &NotifyHandle
+                           );
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Initialize current settings of Questions in this FormSet
+  //
+  InitializeCurrentSetting (Selection->FormSet);
+
+  //
+  // Initilize Action field.
+  //
+  Selection->Action = UI_ACTION_REFRESH_FORM;
+
+  //
+  // Clean the mCurFakeQestId value is formset refreshed.
+  //
+  mCurFakeQestId = 0;
+
+  do {
+    //
+    // IFR is updated, force to reparse the IFR binary
+    // This check is shared by EFI_BROWSER_ACTION_FORM_CLOSE and 
+    // EFI_BROWSER_ACTION_RETRIEVE, so code place here.
+    //
+    if (mHiiPackageListUpdated) {
+      Selection->Action = UI_ACTION_REFRESH_FORMSET;
+      mHiiPackageListUpdated = FALSE;
+      break;
+    }
+
+    //
+    // Initialize Selection->Form
+    //
+    if (Selection->FormId == 0) {
+      //
+      // Zero FormId indicates display the first Form in a FormSet
+      //
+      Link = GetFirstNode (&Selection->FormSet->FormListHead);
+
+      Selection->Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+      Selection->FormId = Selection->Form->FormId;
+    } else {
+      Selection->Form = IdToForm (Selection->FormSet, Selection->FormId);
+    }
+
+    if (Selection->Form == NULL) {
+      //
+      // No Form to display
+      //
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+
+    //
+    // Check Form is suppressed.
+    //
+    if (Selection->Form->SuppressExpression != NULL) {
+      if (EvaluateExpressionList(Selection->Form->SuppressExpression, TRUE, Selection->FormSet, Selection->Form) == ExpressSuppress) {
+        //
+        // Form is suppressed. 
+        //
+        PopupErrorMessage(BROWSER_FORM_SUPPRESS, NULL, NULL, NULL);
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+    }
+
+    //
+    // Before display new form, invoke ConfigAccess.Callback() with EFI_BROWSER_ACTION_FORM_OPEN
+    // for each question with callback flag.
+    // New form may be the first form, or the different form after another form close.
+    //
+    if (((Selection->Handle != mCurrentHiiHandle) ||
+        (!CompareGuid (&Selection->FormSetGuid, &mCurrentFormSetGuid)) ||
+        (Selection->FormId != mCurrentFormId))) {
+      //
+      // Update Retrieve flag.
+      //
+      mFinishRetrieveCall = FALSE;
+
+      //
+      // Keep current form information
+      //
+      mCurrentHiiHandle   = Selection->Handle;
+      CopyGuid (&mCurrentFormSetGuid, &Selection->FormSetGuid);
+      mCurrentFormId      = Selection->FormId;
+
+      if (ConfigAccess != NULL) {
+        Status = ProcessCallBackFunction (Selection, Selection->FormSet, Selection->Form, NULL, EFI_BROWSER_ACTION_FORM_OPEN, FALSE);
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+
+        //
+        // IFR is updated during callback of EFI_BROWSER_ACTION_FORM_OPEN, force to reparse the IFR binary
+        //
+        if (mHiiPackageListUpdated) {
+          Selection->Action = UI_ACTION_REFRESH_FORMSET;
+          mHiiPackageListUpdated = FALSE;
+          break;
+        }
+      }
+    }
+
+    //
+    // Load Questions' Value for display
+    //
+    Status = LoadFormSetConfig (Selection, Selection->FormSet);
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    if (!mFinishRetrieveCall) {
+      //
+      // Finish call RETRIEVE callback for this form.
+      //
+      mFinishRetrieveCall = TRUE;
+
+      if (ConfigAccess != NULL) {
+        Status = ProcessCallBackFunction (Selection, Selection->FormSet, Selection->Form, NULL, EFI_BROWSER_ACTION_RETRIEVE, FALSE);
+        if (EFI_ERROR (Status)) {
+          goto Done;
+        }
+
+        //
+        // IFR is updated during callback of open form, force to reparse the IFR binary
+        //
+        if (mHiiPackageListUpdated) {
+          Selection->Action = UI_ACTION_REFRESH_FORMSET;
+          mHiiPackageListUpdated = FALSE;
+          break;
+        }
+      }
+    }
+
+    //
+    // Display form
+    //
+    Status = DisplayForm ();
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // Check Selected Statement (if press ESC, Selection->Statement will be NULL)
+    //
+    Statement = Selection->Statement;
+    if (Statement != NULL) {
+      if ((ConfigAccess != NULL) && 
+          ((Statement->QuestionFlags & EFI_IFR_FLAG_CALLBACK) == EFI_IFR_FLAG_CALLBACK) && 
+          (Statement->Operand != EFI_IFR_PASSWORD_OP)) {
+        Status = ProcessCallBackFunction(Selection, Selection->FormSet, Selection->Form, Statement, EFI_BROWSER_ACTION_CHANGING, FALSE);
+        if (Statement->Operand == EFI_IFR_REF_OP) {
+          //
+          // Process dynamic update ref opcode.
+          //
+          if (!EFI_ERROR (Status)) {
+            Status = ProcessGotoOpCode(Statement, Selection);
+          }
+          
+          //
+          // Callback return error status or status return from process goto opcode.
+          //
+          if (EFI_ERROR (Status)) {
+            //
+            // Cross reference will not be taken
+            //
+            Selection->FormId = Selection->Form->FormId;
+            Selection->QuestionId = 0;
+          }
+        }
+
+
+        if (!EFI_ERROR (Status) && 
+            (Statement->Operand != EFI_IFR_REF_OP) && 
+            ((Statement->Storage == NULL) || (Statement->Storage != NULL && Statement->ValueChanged))) {
+          //
+          // Only question value has been changed, browser will trig CHANGED callback.
+          //
+          ProcessCallBackFunction(Selection, Selection->FormSet, Selection->Form, Statement, EFI_BROWSER_ACTION_CHANGED, FALSE);
+          //
+          //check whether the question value changed compared with buffer value
+          //if doesn't change ,set the ValueChanged flag to FALSE ,in order not to display the "configuration changed "information on the screen
+          //
+          IsQuestionValueChanged(gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithBuffer);
+        }
+      } else {
+        //
+        // Do the question validation.
+        //
+        Status = ValueChangedValidation (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement);
+        if (!EFI_ERROR (Status) && (Statement->Operand != EFI_IFR_PASSWORD_OP)) {
+          SetQuestionValue (gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithEditBuffer);
+          //
+          // Verify whether question value has checked, update the ValueChanged flag in Question.
+          //
+          IsQuestionValueChanged(gCurrentSelection->FormSet, gCurrentSelection->Form, Statement, GetSetValueWithBuffer);
+        }
+      }
+
+      //
+      // If question has EFI_IFR_FLAG_RESET_REQUIRED/EFI_IFR_FLAG_RECONNECT_REQUIRED flag and without storage 
+      // and process question success till here, trig the gResetFlag/gFlagReconnect.
+      //
+      if ((Status == EFI_SUCCESS) && 
+          (Statement->Storage == NULL)) { 
+        if ((Statement->QuestionFlags & EFI_IFR_FLAG_RESET_REQUIRED) != 0) {
+          gResetRequired = TRUE;
+        }
+
+        if ((Statement->QuestionFlags & EFI_IFR_FLAG_RECONNECT_REQUIRED) != 0) {
+          gFlagReconnect = TRUE;
+        }
+      }
+    }
+
+    //
+    // Check whether Exit flag is TRUE.
+    //
+    if (gExitRequired) {
+      switch (gBrowserSettingScope) {
+      case SystemLevel:
+        Selection->Action = UI_ACTION_EXIT;
+        break;
+
+      case FormSetLevel:
+      case FormLevel:
+        FindNextMenu (Selection, gBrowserSettingScope);
+        break;
+
+      default:
+        break;
+      }
+
+      gExitRequired = FALSE;
+    }
+
+    //
+    // Before exit the form, invoke ConfigAccess.Callback() with EFI_BROWSER_ACTION_FORM_CLOSE
+    // for each question with callback flag.
+    //
+    if ((ConfigAccess != NULL) && 
+        ((Selection->Action == UI_ACTION_EXIT) || 
+         (Selection->Handle != mCurrentHiiHandle) ||
+         (!CompareGuid (&Selection->FormSetGuid, &mCurrentFormSetGuid)) ||
+         (Selection->FormId != mCurrentFormId))) {
+
+      Status = ProcessCallBackFunction (Selection, Selection->FormSet, Selection->Form, NULL, EFI_BROWSER_ACTION_FORM_CLOSE, FALSE);
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+    }
+  } while (Selection->Action == UI_ACTION_REFRESH_FORM);
+
+Done:
+  //
+  // Reset current form information to the initial setting when error happens or form exit.
+  //
+  if (EFI_ERROR (Status) || Selection->Action == UI_ACTION_EXIT) {
+    mCurrentHiiHandle = NULL;
+    CopyGuid (&mCurrentFormSetGuid, &gZeroGuid);
+    mCurrentFormId = 0;
+  }
+
+  //
+  // Unregister notify for Form package update
+  //
+  mHiiDatabase->UnregisterPackageNotify (
+                   mHiiDatabase,
+                   NotifyHandle
+                   );
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.c b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.c
new file mode 100644
index 0000000000..cc9896959b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.c
@@ -0,0 +1,6234 @@
+/** @file
+Entry and initialization module for the browser.
+
+Copyright (c) 2007 - 2015, 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 "Setup.h"
+
+SETUP_DRIVER_PRIVATE_DATA  mPrivateData = {
+  SETUP_DRIVER_SIGNATURE,
+  NULL,
+  {
+    SendForm,
+    BrowserCallback
+  },
+  {
+    SetScope,
+    RegisterHotKey,
+    RegiserExitHandler,
+    SaveReminder
+  },
+  {
+    BROWSER_EXTENSION2_VERSION_1_1,
+    SetScope,
+    RegisterHotKey,
+    RegiserExitHandler,
+    IsBrowserDataModified,
+    ExecuteAction,
+    {NULL,NULL},
+    {NULL,NULL},
+    IsResetRequired
+  }
+};
+
+EFI_HII_DATABASE_PROTOCOL         *mHiiDatabase;
+EFI_HII_CONFIG_ROUTING_PROTOCOL   *mHiiConfigRouting;
+EFI_DEVICE_PATH_FROM_TEXT_PROTOCOL *mPathFromText;
+EDKII_FORM_DISPLAY_ENGINE_PROTOCOL *mFormDisplay;
+
+UINTN           gBrowserContextCount = 0;
+LIST_ENTRY      gBrowserContextList = INITIALIZE_LIST_HEAD_VARIABLE (gBrowserContextList);
+LIST_ENTRY      gBrowserFormSetList = INITIALIZE_LIST_HEAD_VARIABLE (gBrowserFormSetList);
+LIST_ENTRY      gBrowserHotKeyList  = INITIALIZE_LIST_HEAD_VARIABLE (gBrowserHotKeyList);
+LIST_ENTRY      gBrowserStorageList = INITIALIZE_LIST_HEAD_VARIABLE (gBrowserStorageList);
+LIST_ENTRY      gBrowserSaveFailFormSetList = INITIALIZE_LIST_HEAD_VARIABLE (gBrowserSaveFailFormSetList);
+
+BOOLEAN               mSystemSubmit = FALSE;
+BOOLEAN               gResetRequired;
+BOOLEAN               gExitRequired;
+BOOLEAN               gFlagReconnect;
+BOOLEAN               gCallbackReconnect;
+BROWSER_SETTING_SCOPE gBrowserSettingScope = FormSetLevel;
+BOOLEAN               mBrowserScopeFirstSet = TRUE;
+EXIT_HANDLER          ExitHandlerFunction = NULL;
+FORM_BROWSER_FORMSET  *mSystemLevelFormSet;
+
+//
+// Browser Global Strings
+//
+CHAR16            *gEmptyString;
+CHAR16            *mUnknownString = L"!";
+
+extern EFI_GUID        mCurrentFormSetGuid;
+extern EFI_HII_HANDLE  mCurrentHiiHandle;
+extern UINT16          mCurrentFormId;
+extern FORM_DISPLAY_ENGINE_FORM gDisplayFormData;
+
+/**
+  Create a menu with specified formset GUID and form ID, and add it as a child
+  of the given parent menu.
+
+  @param  HiiHandle              Hii handle related to this formset.
+  @param  FormSetGuid            The Formset Guid of menu to be added.
+  @param  FormId                 The Form ID of menu to be added.
+  @param  QuestionId             The question id of this menu to be added.
+
+  @return A pointer to the newly added menu or NULL if memory is insufficient.
+
+**/
+FORM_ENTRY_INFO *
+UiAddMenuList (
+  IN EFI_HII_HANDLE       HiiHandle,
+  IN EFI_GUID             *FormSetGuid,
+  IN UINT16               FormId,
+  IN UINT16               QuestionId
+  )
+{
+  FORM_ENTRY_INFO  *MenuList;
+
+  MenuList = AllocateZeroPool (sizeof (FORM_ENTRY_INFO));
+  if (MenuList == NULL) {
+    return NULL;
+  }
+
+  MenuList->Signature = FORM_ENTRY_INFO_SIGNATURE;
+
+  MenuList->HiiHandle  = HiiHandle;
+  CopyMem (&MenuList->FormSetGuid, FormSetGuid, sizeof (EFI_GUID));
+  MenuList->FormId     = FormId;
+  MenuList->QuestionId = QuestionId;
+
+  //
+  // If parent is not specified, it is the root Form of a Formset
+  //
+  InsertTailList (&mPrivateData.FormBrowserEx2.FormViewHistoryHead, &MenuList->Link);
+
+  return MenuList;
+}
+
+/**
+  Return the form id for the input hiihandle and formset.
+
+  @param  HiiHandle              HiiHandle for FormSet.
+  @param  FormSetGuid            The Formset GUID of the menu to search.
+
+  @return First form's id for this form set.
+
+**/
+EFI_FORM_ID
+GetFirstFormId (
+  IN EFI_HII_HANDLE       HiiHandle,
+  IN EFI_GUID             *FormSetGuid
+  )
+{
+  LIST_ENTRY         *Link;
+  FORM_BROWSER_FORM  *Form;
+
+  Link = GetFirstNode (&gCurrentSelection->FormSet->FormListHead);
+  Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+  return Form->FormId;
+}
+
+/**
+  Search Menu with given FormSetGuid and FormId in all cached menu list.
+
+  @param  HiiHandle              HiiHandle for FormSet.
+  @param  FormSetGuid            The Formset GUID of the menu to search.
+  @param  FormId                 The Form ID of menu to search.
+
+  @return A pointer to menu found or NULL if not found.
+
+**/
+FORM_ENTRY_INFO *
+UiFindMenuList (
+  IN EFI_HII_HANDLE       HiiHandle, 
+  IN EFI_GUID             *FormSetGuid,
+  IN UINT16               FormId
+  )
+{
+  LIST_ENTRY         *Link;
+  FORM_ENTRY_INFO    *MenuList;
+  FORM_ENTRY_INFO    *RetMenu;
+  EFI_FORM_ID        FirstFormId;
+
+  RetMenu = NULL;
+
+  Link = GetFirstNode (&mPrivateData.FormBrowserEx2.FormViewHistoryHead);
+  while (!IsNull (&mPrivateData.FormBrowserEx2.FormViewHistoryHead, Link)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (Link);
+    Link = GetNextNode (&mPrivateData.FormBrowserEx2.FormViewHistoryHead, Link);
+    
+    //
+    // If already find the menu, free the menus behind it.
+    //
+    if (RetMenu != NULL) {
+      RemoveEntryList (&MenuList->Link);
+      FreePool (MenuList);
+      continue;
+    }
+
+    //
+    // Find the same FromSet.
+    //
+    if (MenuList->HiiHandle == HiiHandle) {
+      if (CompareGuid (&MenuList->FormSetGuid, &gZeroGuid)) {
+        //
+        // FormSetGuid is not specified.
+        //
+        RetMenu = MenuList;
+      } else if (CompareGuid (&MenuList->FormSetGuid, FormSetGuid)) {
+        if (MenuList->FormId == FormId) {
+          RetMenu = MenuList;
+        } else if (FormId == 0 || MenuList->FormId == 0 ) {
+          FirstFormId = GetFirstFormId (HiiHandle, FormSetGuid);
+          if ((FormId == 0 && FirstFormId == MenuList->FormId) || (MenuList->FormId ==0 && FirstFormId == FormId)) {
+            RetMenu = MenuList;
+          }
+        }
+      }
+    }
+  }
+
+  return RetMenu;
+}
+
+/**
+  Find parent menu for current menu.
+
+  @param  CurrentMenu    Current Menu
+  @param  SettingLevel   Whether find parent menu in Form Level or Formset level.
+                         In form level, just find the parent menu; 
+                         In formset level, find the parent menu which has different
+                         formset guid value.
+
+  @retval   The parent menu for current menu.
+**/
+FORM_ENTRY_INFO *
+UiFindParentMenu (
+  IN FORM_ENTRY_INFO          *CurrentMenu,
+  IN BROWSER_SETTING_SCOPE    SettingLevel
+  )
+{
+  FORM_ENTRY_INFO    *ParentMenu;
+  LIST_ENTRY         *Link;
+
+  ASSERT (SettingLevel == FormLevel || SettingLevel == FormSetLevel);
+
+  if (CurrentMenu == NULL) {
+    return NULL;
+  }
+  
+  ParentMenu = NULL;
+  Link       = &CurrentMenu->Link;
+
+  while (Link->BackLink != &mPrivateData.FormBrowserEx2.FormViewHistoryHead) {
+    ParentMenu = FORM_ENTRY_INFO_FROM_LINK (Link->BackLink);
+
+    if (SettingLevel == FormLevel) {
+      //
+      // For FormLevel, just find the parent menu, return.
+      //
+      break;
+    }
+
+    if (!CompareGuid (&CurrentMenu->FormSetGuid, &ParentMenu->FormSetGuid)) {
+      //
+      // For SystemLevel, must find the menu which has different formset.
+      //
+      break;
+    }
+
+    Link = Link->BackLink;
+  }
+
+  //
+  // Not find the parent menu, just return NULL.
+  //
+  if (Link->BackLink == &mPrivateData.FormBrowserEx2.FormViewHistoryHead) {
+    return NULL;
+  }
+
+  return ParentMenu;
+}
+
+/**
+  Free Menu list linked list.
+
+  @param  MenuListHead    One Menu list point in the menu list.
+
+**/
+VOID
+UiFreeMenuList (
+  LIST_ENTRY   *MenuListHead
+  )
+{
+  FORM_ENTRY_INFO    *MenuList;
+
+  while (!IsListEmpty (MenuListHead)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (MenuListHead->ForwardLink);
+    RemoveEntryList (&MenuList->Link);
+
+    FreePool (MenuList);
+  }
+}
+
+/**
+  Copy current Menu list to the new menu list.
+  
+  @param  NewMenuListHead        New create Menu list.
+  @param  CurrentMenuListHead    Current Menu list.
+
+**/
+VOID
+UiCopyMenuList (
+  OUT LIST_ENTRY   *NewMenuListHead,
+  IN  LIST_ENTRY   *CurrentMenuListHead
+  )
+{
+  LIST_ENTRY         *Link;
+  FORM_ENTRY_INFO    *MenuList;
+  FORM_ENTRY_INFO    *NewMenuEntry;
+
+  //
+  // If new menu list not empty, free it first.
+  //
+  UiFreeMenuList (NewMenuListHead);
+
+  Link = GetFirstNode (CurrentMenuListHead);
+  while (!IsNull (CurrentMenuListHead, Link)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (Link);
+    Link = GetNextNode (CurrentMenuListHead, Link);
+
+    NewMenuEntry = AllocateZeroPool (sizeof (FORM_ENTRY_INFO));
+    ASSERT (NewMenuEntry != NULL);
+    NewMenuEntry->Signature  = FORM_ENTRY_INFO_SIGNATURE;
+    NewMenuEntry->HiiHandle  = MenuList->HiiHandle;
+    CopyMem (&NewMenuEntry->FormSetGuid, &MenuList->FormSetGuid, sizeof (EFI_GUID));
+    NewMenuEntry->FormId     = MenuList->FormId;
+    NewMenuEntry->QuestionId = MenuList->QuestionId;
+
+    InsertTailList (NewMenuListHead, &NewMenuEntry->Link);
+  }
+}
+
+/**
+  Load all hii formset to the browser.
+
+**/
+VOID
+LoadAllHiiFormset (
+  VOID
+  )
+{
+  FORM_BROWSER_FORMSET    *LocalFormSet;
+  EFI_HII_HANDLE          *HiiHandles;
+  UINTN                   Index;
+  EFI_GUID                ZeroGuid;
+  EFI_STATUS              Status;
+  FORM_BROWSER_FORMSET    *OldFormset;
+
+  OldFormset = mSystemLevelFormSet;
+
+  //
+  // Get all the Hii handles
+  //
+  HiiHandles = HiiGetHiiHandles (NULL);
+  ASSERT (HiiHandles != NULL);
+
+  //
+  // Search for formset of each class type
+  //
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    //
+    // Check HiiHandles[Index] does exist in global maintain list.
+    //
+    if (GetFormSetFromHiiHandle (HiiHandles[Index]) != NULL) {
+      continue;
+    }
+
+    //
+    // Initilize FormSet Setting
+    //
+    LocalFormSet = AllocateZeroPool (sizeof (FORM_BROWSER_FORMSET));
+    ASSERT (LocalFormSet != NULL);
+    mSystemLevelFormSet = LocalFormSet;
+
+    ZeroMem (&ZeroGuid, sizeof (ZeroGuid));
+    Status = InitializeFormSet (HiiHandles[Index], &ZeroGuid, LocalFormSet);
+    if (EFI_ERROR (Status) || IsListEmpty (&LocalFormSet->FormListHead)) {
+      DestroyFormSet (LocalFormSet);
+      continue;
+    }
+    InitializeCurrentSetting (LocalFormSet);
+
+    //
+    // Initilize Questions' Value
+    //
+    Status = LoadFormSetConfig (NULL, LocalFormSet);
+    if (EFI_ERROR (Status)) {
+      DestroyFormSet (LocalFormSet);
+      continue;
+    }
+  }
+
+  //
+  // Free resources, and restore gOldFormSet and gClassOfVfr
+  //
+  FreePool (HiiHandles);
+
+  mSystemLevelFormSet = OldFormset;
+}
+
+/**
+  Pop up the error info.
+
+  @param      BrowserStatus    The input browser status.
+  @param      HiiHandle        The Hiihandle for this opcode.
+  @param      OpCode           The opcode use to get the erro info and timeout value.
+  @param      ErrorString      Error string used by BROWSER_NO_SUBMIT_IF.
+
+**/
+UINT32
+PopupErrorMessage (
+  IN UINT32                BrowserStatus,
+  IN EFI_HII_HANDLE        HiiHandle,
+  IN EFI_IFR_OP_HEADER     *OpCode, OPTIONAL
+  IN CHAR16                *ErrorString
+  )
+{
+  FORM_DISPLAY_ENGINE_STATEMENT *Statement;
+  USER_INPUT                    UserInputData;
+
+  Statement = NULL;
+
+  if (OpCode != NULL) {
+    Statement = AllocateZeroPool (sizeof(FORM_DISPLAY_ENGINE_STATEMENT));
+    ASSERT (Statement != NULL);
+    Statement->OpCode = OpCode;
+    gDisplayFormData.HighLightedStatement = Statement;
+  }
+
+  //
+  // Used to compatible with old display engine.
+  // New display engine not use this field.
+  //
+  gDisplayFormData.ErrorString   = ErrorString;
+  gDisplayFormData.BrowserStatus = BrowserStatus;
+
+  if (HiiHandle != NULL) {
+    gDisplayFormData.HiiHandle     = HiiHandle;
+  }
+
+  mFormDisplay->FormDisplay (&gDisplayFormData, &UserInputData);
+
+  gDisplayFormData.BrowserStatus = BROWSER_SUCCESS;
+  gDisplayFormData.ErrorString   = NULL;
+
+  if (OpCode != NULL) {
+    FreePool (Statement);
+  }
+
+  return UserInputData.Action;
+}
+
+/**
+  This is the routine which an external caller uses to direct the browser
+  where to obtain it's information.
+
+
+  @param This            The Form Browser protocol instanse.
+  @param Handles         A pointer to an array of Handles.  If HandleCount > 1 we
+                         display a list of the formsets for the handles specified.
+  @param HandleCount     The number of Handles specified in Handle.
+  @param FormSetGuid     This field points to the EFI_GUID which must match the Guid
+                         field in the EFI_IFR_FORM_SET op-code for the specified
+                         forms-based package. If FormSetGuid is NULL, then this
+                         function will display the first found forms package.
+  @param FormId          This field specifies which EFI_IFR_FORM to render as the first
+                         displayable page. If this field has a value of 0x0000, then
+                         the forms browser will render the specified forms in their encoded order.
+  @param ScreenDimensions Points to recommended form dimensions, including any non-content area, in
+                          characters.
+  @param ActionRequest   Points to the action recommended by the form.
+
+  @retval  EFI_SUCCESS            The function completed successfully.
+  @retval  EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+  @retval  EFI_NOT_FOUND          No valid forms could be found to display.
+
+**/
+EFI_STATUS
+EFIAPI
+SendForm (
+  IN  CONST EFI_FORM_BROWSER2_PROTOCOL *This,
+  IN  EFI_HII_HANDLE                   *Handles,
+  IN  UINTN                            HandleCount,
+  IN  EFI_GUID                         *FormSetGuid, OPTIONAL
+  IN  UINT16                           FormId, OPTIONAL
+  IN  CONST EFI_SCREEN_DESCRIPTOR      *ScreenDimensions, OPTIONAL
+  OUT EFI_BROWSER_ACTION_REQUEST       *ActionRequest  OPTIONAL
+  )
+{
+  EFI_STATUS                    Status;
+  UI_MENU_SELECTION             *Selection;
+  UINTN                         Index;
+  FORM_BROWSER_FORMSET          *FormSet;
+  FORM_ENTRY_INFO               *MenuList;
+  BOOLEAN                       RetVal;
+
+  //
+  // If EDKII_FORM_DISPLAY_ENGINE_PROTOCOL not found, return EFI_UNSUPPORTED.
+  //
+  if (mFormDisplay == NULL) {
+    DEBUG ((DEBUG_ERROR, "Fatal Error! EDKII_FORM_DISPLAY_ENGINE_PROTOCOL not found!"));
+    return EFI_UNSUPPORTED;
+  }
+
+  //
+  // Save globals used by SendForm()
+  //
+  SaveBrowserContext ();
+
+  gFlagReconnect = FALSE;
+  gResetRequired = FALSE;
+  gExitRequired  = FALSE;
+  gCallbackReconnect = FALSE;
+  Status         = EFI_SUCCESS;
+  gEmptyString   = L"";
+  gDisplayFormData.ScreenDimensions = (EFI_SCREEN_DESCRIPTOR *) ScreenDimensions;
+
+  for (Index = 0; Index < HandleCount; Index++) {
+    Selection = AllocateZeroPool (sizeof (UI_MENU_SELECTION));
+    ASSERT (Selection != NULL);
+
+    Selection->Handle = Handles[Index];
+    if (FormSetGuid != NULL) {
+      CopyMem (&Selection->FormSetGuid, FormSetGuid, sizeof (EFI_GUID));
+      Selection->FormId = FormId;
+    } else {
+      CopyMem (&Selection->FormSetGuid, &gEfiHiiPlatformSetupFormsetGuid, sizeof (EFI_GUID));
+    }
+
+    do {
+      FormSet = AllocateZeroPool (sizeof (FORM_BROWSER_FORMSET));
+      ASSERT (FormSet != NULL);
+
+      //
+      // Validate the HiiHandle
+      // if validate failed, find the first validate parent HiiHandle.
+      //
+      if (!ValidateHiiHandle(Selection->Handle)) {
+        FindNextMenu (Selection, FormSetLevel);
+      }
+
+      //
+      // Initialize internal data structures of FormSet
+      //
+      Status = InitializeFormSet (Selection->Handle, &Selection->FormSetGuid, FormSet);
+      if (EFI_ERROR (Status) || IsListEmpty (&FormSet->FormListHead)) {
+        DestroyFormSet (FormSet);
+        break;
+      }
+      Selection->FormSet = FormSet;
+      mSystemLevelFormSet = FormSet;
+
+      //
+      // Display this formset
+      //
+      gCurrentSelection = Selection;
+
+      Status = SetupBrowser (Selection);
+
+      gCurrentSelection = NULL;
+      mSystemLevelFormSet = NULL;
+
+      if (gFlagReconnect || gCallbackReconnect) {
+        RetVal = ReconnectController (FormSet->DriverHandle);
+        if (!RetVal) {
+          PopupErrorMessage(BROWSER_RECONNECT_FAIL, NULL, NULL, NULL);
+        }
+        gFlagReconnect = FALSE;
+        gCallbackReconnect = FALSE;
+      }
+
+      //
+      // If no data is changed, don't need to save current FormSet into the maintain list.
+      //
+      if (!IsNvUpdateRequiredForFormSet (FormSet)) {
+        CleanBrowserStorage(FormSet);
+        RemoveEntryList (&FormSet->Link);
+        DestroyFormSet (FormSet);
+      }
+
+      if (EFI_ERROR (Status)) {
+        break;
+      }
+    } while (Selection->Action == UI_ACTION_REFRESH_FORMSET);
+
+    FreePool (Selection);
+  }
+
+  if (ActionRequest != NULL) {
+    *ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+    if (gResetRequired) {
+      *ActionRequest = EFI_BROWSER_ACTION_REQUEST_RESET;
+    }
+  }
+
+  mFormDisplay->ExitDisplay();
+
+  //
+  // Clear the menu history data.
+  //
+  while (!IsListEmpty (&mPrivateData.FormBrowserEx2.FormViewHistoryHead)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (mPrivateData.FormBrowserEx2.FormViewHistoryHead.ForwardLink);
+    RemoveEntryList (&MenuList->Link);
+    FreePool (MenuList);
+  }
+
+  //
+  // Restore globals used by SendForm()
+  //
+  RestoreBrowserContext ();
+
+  return Status;
+}
+
+/**
+  Get or set data to the storage.
+
+  @param  ResultsDataSize        The size of the buffer associatedwith ResultsData.
+  @param  ResultsData            A string returned from an IFR browser or
+                                 equivalent. The results string will have no
+                                 routing information in them.
+  @param  RetrieveData           A BOOLEAN field which allows an agent to retrieve
+                                 (if RetrieveData = TRUE) data from the uncommitted
+                                 browser state information or set (if RetrieveData
+                                 = FALSE) data in the uncommitted browser state
+                                 information.
+  @param  Storage                The pointer to the storage.
+
+  @retval EFI_SUCCESS            The results have been distributed or are awaiting
+                                 distribution.
+
+**/
+EFI_STATUS 
+ProcessStorage (
+  IN OUT UINTN                         *ResultsDataSize,
+  IN OUT EFI_STRING                    *ResultsData,
+  IN BOOLEAN                           RetrieveData,
+  IN BROWSER_STORAGE                   *Storage
+  )
+{
+  CHAR16                *ConfigResp;
+  EFI_STATUS            Status;
+  CHAR16                *StrPtr;
+  UINTN                 BufferSize;
+  UINTN                 TmpSize;
+  UINTN                 MaxLen;
+  FORMSET_STORAGE       *BrowserStorage;
+
+  if (RetrieveData) {
+    //
+    // Generate <ConfigResp>
+    //
+    Status = StorageToConfigResp (Storage, &ConfigResp, Storage->ConfigRequest, TRUE);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Skip <ConfigHdr> and '&' to point to <ConfigBody> when first copy the configbody.
+    // Also need to consider add "\0" at first time.
+    //
+    StrPtr = StrStr (ConfigResp, L"PATH");
+    ASSERT (StrPtr != NULL);
+    StrPtr = StrStr (StrPtr, L"&");
+    StrPtr += 1;
+    BufferSize = StrSize (StrPtr);
+
+    //
+    // Copy the data if the input buffer is bigger enough.
+    //
+    if (*ResultsDataSize >= BufferSize) {
+      StrCpyS (*ResultsData, *ResultsDataSize / sizeof (CHAR16), StrPtr);
+    }
+
+    *ResultsDataSize = BufferSize;
+    FreePool (ConfigResp);
+  } else {
+    //
+    // Prepare <ConfigResp>
+    //
+    BrowserStorage = GetFstStgFromBrsStg (Storage);
+    ASSERT (BrowserStorage != NULL);
+    TmpSize = StrLen (*ResultsData);
+    BufferSize = (TmpSize + StrLen (BrowserStorage->ConfigHdr) + 2) * sizeof (CHAR16);
+    MaxLen = BufferSize / sizeof (CHAR16);
+    ConfigResp = AllocateZeroPool (BufferSize);
+    ASSERT (ConfigResp != NULL);
+
+    StrCpyS (ConfigResp, MaxLen, BrowserStorage->ConfigHdr);
+    StrCatS (ConfigResp, MaxLen, L"&");
+    StrCatS (ConfigResp, MaxLen, *ResultsData);
+
+    //
+    // Update Browser uncommited data
+    //
+    Status = ConfigRespToStorage (Storage, ConfigResp);
+    FreePool (ConfigResp);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  This routine called this service in the browser to retrieve or set certain uncommitted 
+  state information that resides in the open formsets. 
+
+  @param  This                   A pointer to the EFI_FORM_BROWSER2_PROTOCOL
+                                 instance.
+  @param  ResultsDataSize        A pointer to the size of the buffer associated
+                                 with ResultsData.
+  @param  ResultsData            A string returned from an IFR browser or
+                                 equivalent. The results string will have no
+                                 routing information in them.
+  @param  RetrieveData           A BOOLEAN field which allows an agent to retrieve
+                                 (if RetrieveData = TRUE) data from the uncommitted
+                                 browser state information or set (if RetrieveData
+                                 = FALSE) data in the uncommitted browser state
+                                 information.
+  @param  VariableGuid           An optional field to indicate the target variable
+                                 GUID name to use.
+  @param  VariableName           An optional field to indicate the target
+                                 human-readable variable name.
+
+  @retval EFI_SUCCESS            The results have been distributed or are awaiting
+                                 distribution.
+  @retval EFI_BUFFER_TOO_SMALL   The ResultsDataSize specified was too small to
+                                 contain the results data.
+
+**/
+EFI_STATUS
+EFIAPI
+BrowserCallback (
+  IN CONST EFI_FORM_BROWSER2_PROTOCOL  *This,
+  IN OUT UINTN                         *ResultsDataSize,
+  IN OUT EFI_STRING                    ResultsData,
+  IN BOOLEAN                           RetrieveData,
+  IN CONST EFI_GUID                    *VariableGuid, OPTIONAL
+  IN CONST CHAR16                      *VariableName  OPTIONAL
+  )
+{
+  EFI_STATUS            Status;
+  LIST_ENTRY            *Link;
+  BROWSER_STORAGE       *Storage;
+  FORMSET_STORAGE       *FormsetStorage;
+  UINTN                 TotalSize;
+  BOOLEAN               Found;
+
+  if (ResultsDataSize == NULL || ResultsData == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TotalSize = *ResultsDataSize;
+  Storage   = NULL;
+  Found     = FALSE;
+  Status    = EFI_SUCCESS;
+
+  if (VariableGuid != NULL) {
+    //
+    // Try to find target storage in the current formset.
+    //
+    Link = GetFirstNode (&gBrowserStorageList);
+    while (!IsNull (&gBrowserStorageList, Link)) {
+      Storage = BROWSER_STORAGE_FROM_LINK (Link);
+      Link = GetNextNode (&gBrowserStorageList, Link);
+      //
+      // Check the current storage.
+      //
+      if (!CompareGuid (&Storage->Guid, (EFI_GUID *) VariableGuid)) {
+        continue;
+      }
+
+      if (Storage->Type == EFI_HII_VARSTORE_BUFFER ||
+          Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+        //
+        // Buffer storage require both GUID and Name
+        //
+        if (VariableName == NULL) {
+          return EFI_NOT_FOUND;
+        }
+
+        if (StrCmp (Storage->Name, (CHAR16 *) VariableName) != 0) {
+          continue;
+        }
+      }
+
+      if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE ||
+          Storage->Type == EFI_HII_VARSTORE_BUFFER) {
+        if (mSystemLevelFormSet == NULL || mSystemLevelFormSet->HiiHandle == NULL) {
+          return EFI_NOT_FOUND;
+        }
+
+        if (Storage->HiiHandle != mSystemLevelFormSet->HiiHandle) {
+          continue;
+        }
+      }
+
+      Status = ProcessStorage (&TotalSize, &ResultsData, RetrieveData, Storage);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+        ConfigRequestAdjust (Storage, ResultsData, TRUE);
+      }
+
+      //
+      // Different formsets may have same varstore, so here just set the flag
+      // not exit the circle.
+      // 
+      Found = TRUE;
+      break;
+    }
+
+    if (!Found) {
+      return EFI_NOT_FOUND;
+    }
+  } else {
+    //
+    // GUID/Name is not specified, take the first storage in FormSet
+    //
+    if (mSystemLevelFormSet == NULL) {
+      return EFI_NOT_READY;
+    }
+
+    //
+    // Generate <ConfigResp>
+    //
+    Link = GetFirstNode (&mSystemLevelFormSet->StorageListHead);
+    if (IsNull (&mSystemLevelFormSet->StorageListHead, Link)) {
+      return EFI_UNSUPPORTED;
+    }
+
+    FormsetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+    
+    Status = ProcessStorage (&TotalSize, &ResultsData, RetrieveData, FormsetStorage->BrowserStorage);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  if (RetrieveData) {
+    Status = TotalSize <= *ResultsDataSize ? EFI_SUCCESS : EFI_BUFFER_TOO_SMALL;
+    *ResultsDataSize = TotalSize;
+  }
+   
+  return Status;
+
+}
+
+
+/**
+  Callback function for SimpleTextInEx protocol install events
+
+  @param Event           the event that is signaled.
+  @param Context         not used here.
+
+**/
+VOID
+EFIAPI
+FormDisplayCallback (
+  IN EFI_EVENT    Event,
+  IN VOID         *Context
+  )
+{
+  if (mFormDisplay != NULL) {
+    return;
+  }
+
+  gBS->LocateProtocol (
+                  &gEdkiiFormDisplayEngineProtocolGuid,
+                  NULL,
+                  (VOID **) &mFormDisplay
+                  );
+}
+
+/**
+  Initialize Setup Browser driver.
+
+  @param ImageHandle     The image handle.
+  @param SystemTable     The system table.
+
+  @retval EFI_SUCCESS    The Setup Browser module is initialized correctly..
+  @return Other value if failed to initialize the Setup Browser module.
+
+**/
+EFI_STATUS
+EFIAPI
+InitializeSetup (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS                  Status;
+  VOID                        *Registration;
+
+  //
+  // Locate required Hii relative protocols
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiDatabaseProtocolGuid,
+                  NULL,
+                  (VOID **) &mHiiDatabase
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->LocateProtocol (
+                  &gEfiHiiConfigRoutingProtocolGuid,
+                  NULL,
+                  (VOID **) &mHiiConfigRouting
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->LocateProtocol (
+                  &gEfiDevicePathFromTextProtocolGuid,
+                  NULL,
+                  (VOID **) &mPathFromText
+                  );
+
+  //
+  // Install FormBrowser2 protocol
+  //
+  mPrivateData.Handle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &mPrivateData.Handle,
+                  &gEfiFormBrowser2ProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mPrivateData.FormBrowser2
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install FormBrowserEx2 protocol
+  //
+  InitializeListHead (&mPrivateData.FormBrowserEx2.FormViewHistoryHead);
+  InitializeListHead (&mPrivateData.FormBrowserEx2.OverrideQestListHead);
+  mPrivateData.Handle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &mPrivateData.Handle,
+                  &gEdkiiFormBrowserEx2ProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mPrivateData.FormBrowserEx2
+                  );
+  ASSERT_EFI_ERROR (Status);
+  
+  Status = gBS->InstallProtocolInterface (
+                  &mPrivateData.Handle,
+                  &gEfiFormBrowserExProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mPrivateData.FormBrowserEx
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  InitializeDisplayFormData ();
+
+  Status = gBS->LocateProtocol (
+                  &gEdkiiFormDisplayEngineProtocolGuid,
+                  NULL,
+                  (VOID **) &mFormDisplay
+                  );
+
+  if (EFI_ERROR (Status)) {
+    EfiCreateProtocolNotifyEvent (
+      &gEdkiiFormDisplayEngineProtocolGuid,
+      TPL_CALLBACK,
+      FormDisplayCallback,
+      NULL,
+      &Registration
+      );
+  }
+  
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Create a new string in HII Package List.
+
+  @param  String                 The String to be added
+  @param  HiiHandle              The package list in the HII database to insert the
+                                 specified string.
+
+  @return The output string.
+
+**/
+EFI_STRING_ID
+NewString (
+  IN  CHAR16                   *String,
+  IN  EFI_HII_HANDLE           HiiHandle
+  )
+{
+  EFI_STRING_ID  StringId;
+
+  StringId = HiiSetString (HiiHandle, 0, String, NULL);
+  ASSERT (StringId != 0);
+
+  return StringId;
+}
+
+
+/**
+  Delete a string from HII Package List.
+
+  @param  StringId               Id of the string in HII database.
+  @param  HiiHandle              The HII package list handle.
+
+  @retval EFI_SUCCESS            The string was deleted successfully.
+
+**/
+EFI_STATUS
+DeleteString (
+  IN  EFI_STRING_ID            StringId,
+  IN  EFI_HII_HANDLE           HiiHandle
+  )
+{
+  CHAR16  NullChar;
+
+  NullChar = CHAR_NULL;
+  HiiSetString (HiiHandle, StringId, &NullChar, NULL);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The package list in the HII database to search for
+                                 the specified string.
+
+  @return The output string.
+
+**/
+CHAR16 *
+GetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  )
+{
+  EFI_STRING  String;
+
+  if (HiiHandle == NULL) {
+    return NULL;
+  }
+
+  String = HiiGetString (HiiHandle, Token, NULL);
+  if (String == NULL) {
+    String = AllocateCopyPool (StrSize (mUnknownString), mUnknownString);
+    ASSERT (String != NULL);
+  }
+  return (CHAR16 *) String;
+}
+
+
+/**
+  Allocate new memory and then copy the Unicode string Source to Destination.
+
+  @param  Dest                   Location to copy string
+  @param  Src                    String to copy
+
+**/
+VOID
+NewStringCpy (
+  IN OUT CHAR16       **Dest,
+  IN CHAR16           *Src
+  )
+{
+  if (*Dest != NULL) {
+    FreePool (*Dest);
+  }
+  *Dest = AllocateCopyPool (StrSize (Src), Src);
+  ASSERT (*Dest != NULL);
+}
+
+
+/**
+  Allocate new memory and concatinate Source on the end of Destination.
+
+  @param  Dest                   String to added to the end of.
+  @param  Src                    String to concatinate.
+
+**/
+VOID
+NewStringCat (
+  IN OUT CHAR16       **Dest,
+  IN CHAR16           *Src
+  )
+{
+  CHAR16  *NewString;
+  UINTN   MaxLen;
+
+  if (*Dest == NULL) {
+    NewStringCpy (Dest, Src);
+    return;
+  }
+
+  MaxLen = ( StrSize (*Dest) + StrSize (Src) - 1) / sizeof (CHAR16);
+  NewString = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+  ASSERT (NewString != NULL);
+
+  StrCpyS (NewString, MaxLen, *Dest);
+  StrCatS (NewString, MaxLen, Src);
+
+  FreePool (*Dest);
+  *Dest = NewString;
+}
+
+/**
+  Get Value for given Name from a NameValue Storage.
+
+  @param  Storage                The NameValue Storage.
+  @param  Name                   The Name.
+  @param  Value                  The retured Value.
+  @param  GetValueFrom           Where to get source value, from EditValue or Value.
+
+  @retval EFI_SUCCESS            Value found for given Name.
+  @retval EFI_NOT_FOUND          No such Name found in NameValue storage.
+
+**/
+EFI_STATUS
+GetValueByName (
+  IN BROWSER_STORAGE             *Storage,
+  IN CHAR16                      *Name,
+  IN OUT CHAR16                  **Value,
+  IN GET_SET_QUESTION_VALUE_WITH GetValueFrom
+  )
+{
+  LIST_ENTRY              *Link;
+  NAME_VALUE_NODE         *Node;
+
+  if (GetValueFrom != GetSetValueWithEditBuffer && GetValueFrom != GetSetValueWithBuffer) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *Value = NULL;
+
+  Link = GetFirstNode (&Storage->NameValueListHead);
+  while (!IsNull (&Storage->NameValueListHead, Link)) {
+    Node = NAME_VALUE_NODE_FROM_LINK (Link);
+
+    if (StrCmp (Name, Node->Name) == 0) {
+      if (GetValueFrom == GetSetValueWithEditBuffer) {
+        NewStringCpy (Value, Node->EditValue);
+      } else {
+        NewStringCpy (Value, Node->Value);
+      }
+      return EFI_SUCCESS;
+    }
+
+    Link = GetNextNode (&Storage->NameValueListHead, Link);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Set Value of given Name in a NameValue Storage.
+
+  @param  Storage                The NameValue Storage.
+  @param  Name                   The Name.
+  @param  Value                  The Value to set.
+  @param  SetValueTo             Whether update editValue or Value.
+  @param  ReturnNode             The node use the input name.
+
+  @retval EFI_SUCCESS            Value found for given Name.
+  @retval EFI_NOT_FOUND          No such Name found in NameValue storage.
+
+**/
+EFI_STATUS
+SetValueByName (
+  IN  BROWSER_STORAGE             *Storage,
+  IN  CHAR16                      *Name,
+  IN  CHAR16                      *Value,
+  IN  GET_SET_QUESTION_VALUE_WITH SetValueTo,
+  OUT NAME_VALUE_NODE             **ReturnNode
+  )
+{
+  LIST_ENTRY              *Link;
+  NAME_VALUE_NODE         *Node;
+  CHAR16                  *Buffer;
+
+  if (SetValueTo != GetSetValueWithEditBuffer && SetValueTo != GetSetValueWithBuffer) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Link = GetFirstNode (&Storage->NameValueListHead);
+  while (!IsNull (&Storage->NameValueListHead, Link)) {
+    Node = NAME_VALUE_NODE_FROM_LINK (Link);
+
+    if (StrCmp (Name, Node->Name) == 0) {
+      if (SetValueTo == GetSetValueWithEditBuffer) {
+        Buffer = Node->EditValue;
+      } else {
+        Buffer = Node->Value;
+      }
+      if (Buffer != NULL) {
+        FreePool (Buffer);
+      }
+      Buffer = AllocateCopyPool (StrSize (Value), Value);
+      ASSERT (Buffer != NULL);
+      if (SetValueTo == GetSetValueWithEditBuffer) {
+        Node->EditValue = Buffer;
+      } else {
+        Node->Value = Buffer;
+      }
+
+      if (ReturnNode != NULL) {
+        *ReturnNode = Node;
+      }
+
+      return EFI_SUCCESS;
+    }
+
+    Link = GetNextNode (&Storage->NameValueListHead, Link);
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+
+/**
+  Convert setting of Buffer Storage or NameValue Storage to <ConfigResp>.
+
+  @param  Storage                The Storage to be conveted.
+  @param  ConfigResp             The returned <ConfigResp>.
+  @param  ConfigRequest          The ConfigRequest string.
+  @param  GetEditBuf             Get the data from editbuffer or buffer.
+
+  @retval EFI_SUCCESS            Convert success.
+  @retval EFI_INVALID_PARAMETER  Incorrect storage type.
+
+**/
+EFI_STATUS
+StorageToConfigResp (
+  IN BROWSER_STORAGE         *Storage,
+  IN CHAR16                  **ConfigResp,
+  IN CHAR16                  *ConfigRequest,
+  IN BOOLEAN                 GetEditBuf
+  )
+{
+  EFI_STATUS              Status;
+  EFI_STRING              Progress;
+  LIST_ENTRY              *Link;
+  NAME_VALUE_NODE         *Node;
+  UINT8                   *SourceBuf;
+  FORMSET_STORAGE         *FormsetStorage;
+
+  Status = EFI_SUCCESS;
+
+  switch (Storage->Type) {
+  case EFI_HII_VARSTORE_BUFFER:
+  case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+    SourceBuf = GetEditBuf ? Storage->EditBuffer : Storage->Buffer;
+    Status = mHiiConfigRouting->BlockToConfig (
+                                  mHiiConfigRouting,
+                                  ConfigRequest,
+                                  SourceBuf,
+                                  Storage->Size,
+                                  ConfigResp,
+                                  &Progress
+                                  );
+    break;
+
+  case EFI_HII_VARSTORE_NAME_VALUE:
+    *ConfigResp = NULL;
+    FormsetStorage = GetFstStgFromBrsStg(Storage);
+    ASSERT (FormsetStorage != NULL);
+    NewStringCat (ConfigResp, FormsetStorage->ConfigHdr);
+
+    Link = GetFirstNode (&Storage->NameValueListHead);
+    while (!IsNull (&Storage->NameValueListHead, Link)) {
+      Node = NAME_VALUE_NODE_FROM_LINK (Link);
+
+      if (StrStr (ConfigRequest, Node->Name) != NULL) {
+        NewStringCat (ConfigResp, L"&");
+        NewStringCat (ConfigResp, Node->Name);
+        NewStringCat (ConfigResp, L"=");
+        if (GetEditBuf) {
+          NewStringCat (ConfigResp, Node->EditValue);
+        } else {
+          NewStringCat (ConfigResp, Node->Value);
+        }
+      }
+      Link = GetNextNode (&Storage->NameValueListHead, Link);
+    }
+    break;
+
+  case EFI_HII_VARSTORE_EFI_VARIABLE:
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  return Status;
+}
+
+
+/**
+  Convert <ConfigResp> to settings in Buffer Storage or NameValue Storage.
+
+  @param  Storage                The Storage to receive the settings.
+  @param  ConfigResp             The <ConfigResp> to be converted.
+
+  @retval EFI_SUCCESS            Convert success.
+  @retval EFI_INVALID_PARAMETER  Incorrect storage type.
+
+**/
+EFI_STATUS
+ConfigRespToStorage (
+  IN BROWSER_STORAGE         *Storage,
+  IN CHAR16                  *ConfigResp
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STRING  Progress;
+  UINTN       BufferSize;
+  CHAR16      *StrPtr;
+  CHAR16      *Name;
+  CHAR16      *Value;
+
+  Status = EFI_SUCCESS;
+
+  switch (Storage->Type) {
+  case EFI_HII_VARSTORE_BUFFER:
+  case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+    BufferSize = Storage->Size;
+    Status = mHiiConfigRouting->ConfigToBlock (
+                                  mHiiConfigRouting,
+                                  ConfigResp,
+                                  Storage->EditBuffer,
+                                  &BufferSize,
+                                  &Progress
+                                  );
+    break;
+
+  case EFI_HII_VARSTORE_NAME_VALUE:
+    StrPtr = StrStr (ConfigResp, L"PATH");
+    if (StrPtr == NULL) {
+      break;
+    }
+    StrPtr = StrStr (ConfigResp, L"&");
+    while (StrPtr != NULL) {
+      //
+      // Skip '&'
+      //
+      StrPtr = StrPtr + 1;
+      Name = StrPtr;
+      StrPtr = StrStr (StrPtr, L"=");
+      if (StrPtr == NULL) {
+        break;
+      }
+      *StrPtr = 0;
+
+      //
+      // Skip '='
+      //
+      StrPtr = StrPtr + 1;
+      Value = StrPtr;
+      StrPtr = StrStr (StrPtr, L"&");
+      if (StrPtr != NULL) {
+        *StrPtr = 0;
+      }
+      SetValueByName (Storage, Name, Value, GetSetValueWithEditBuffer, NULL);
+    }
+    break;
+
+  case EFI_HII_VARSTORE_EFI_VARIABLE:
+  default:
+    Status = EFI_INVALID_PARAMETER;
+    break;
+  }
+
+  return Status;
+}
+
+/**
+  Convert the buffer value to HiiValue.
+
+  @param  Question               The question.
+  @param  Value                  Unicode buffer save the question value.
+
+  @retval  Status whether convert the value success.
+
+**/
+EFI_STATUS
+BufferToValue (
+  IN OUT FORM_BROWSER_STATEMENT           *Question,
+  IN     CHAR16                           *Value
+  )
+{
+  CHAR16                       *StringPtr;
+  BOOLEAN                      IsBufferStorage;
+  CHAR16                       *DstBuf;
+  CHAR16                       TempChar;
+  UINTN                        LengthStr;
+  UINT8                        *Dst;
+  CHAR16                       TemStr[5];
+  UINTN                        Index;
+  UINT8                        DigitUint8;
+  BOOLEAN                      IsString;
+  UINTN                        Length;
+  EFI_STATUS                   Status;
+
+  IsString = (BOOLEAN) ((Question->HiiValue.Type == EFI_IFR_TYPE_STRING) ?  TRUE : FALSE);
+  if (Question->Storage->Type == EFI_HII_VARSTORE_BUFFER || 
+      Question->Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+    IsBufferStorage = TRUE;
+  } else {
+    IsBufferStorage = FALSE;
+  }
+
+  //
+  // Question Value is provided by Buffer Storage or NameValue Storage
+  //
+  if (Question->BufferValue != NULL) {
+    //
+    // This Question is password or orderedlist
+    //
+    Dst = Question->BufferValue;
+  } else {
+    //
+    // Other type of Questions
+    //
+    Dst = (UINT8 *) &Question->HiiValue.Value;
+  }
+
+  //
+  // Temp cut at the end of this section, end with '\0' or '&'.
+  //
+  StringPtr = Value;
+  while (*StringPtr != L'\0' && *StringPtr != L'&') {
+    StringPtr++;
+  }
+  TempChar = *StringPtr;
+  *StringPtr = L'\0';
+
+  LengthStr = StrLen (Value);
+  Status    = EFI_SUCCESS;
+  if (!IsBufferStorage && IsString) {
+    //
+    // Convert Config String to Unicode String, e.g "0041004200430044" => "ABCD"
+    // Add string tail char L'\0' into Length
+    //
+    Length    = Question->StorageWidth + sizeof (CHAR16);
+    if (Length < ((LengthStr / 4 + 1) * 2)) {
+      Status = EFI_BUFFER_TOO_SMALL;
+    } else {
+      DstBuf = (CHAR16 *) Dst;
+      ZeroMem (TemStr, sizeof (TemStr));
+      for (Index = 0; Index < LengthStr; Index += 4) {
+        StrnCpyS (TemStr, sizeof (TemStr) / sizeof (CHAR16), Value + Index, 4);
+        DstBuf[Index/4] = (CHAR16) StrHexToUint64 (TemStr);
+      }
+      //
+      // Add tailing L'\0' character
+      //
+      DstBuf[Index/4] = L'\0';
+    }
+  } else {
+    if (Question->StorageWidth < ((LengthStr + 1) / 2)) {
+      Status = EFI_BUFFER_TOO_SMALL;
+    } else {
+      ZeroMem (TemStr, sizeof (TemStr));
+      for (Index = 0; Index < LengthStr; Index ++) {
+        TemStr[0] = Value[LengthStr - Index - 1];
+        DigitUint8 = (UINT8) StrHexToUint64 (TemStr);
+        if ((Index & 1) == 0) {
+          Dst [Index/2] = DigitUint8;
+        } else {
+          Dst [Index/2] = (UINT8) ((DigitUint8 << 4) + Dst [Index/2]);
+        }
+      }
+    }
+  }
+
+  *StringPtr = TempChar;
+
+  return Status;
+}
+
+/**
+  Get Question's current Value.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Question to be initialized.
+  @param  GetValueFrom           Where to get value, may from editbuffer, buffer or hii driver.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+GetQuestionValue (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      GetValueFrom
+  )
+{
+  EFI_STATUS          Status;
+  BOOLEAN             Enabled;
+  BOOLEAN             Pending;
+  UINT8               *Dst;
+  UINTN               StorageWidth;
+  EFI_TIME            EfiTime;
+  BROWSER_STORAGE     *Storage;
+  FORMSET_STORAGE     *FormsetStorage;
+  EFI_IFR_TYPE_VALUE  *QuestionValue;
+  CHAR16              *ConfigRequest;
+  CHAR16              *Progress;
+  CHAR16              *Result;
+  CHAR16              *Value;
+  UINTN               Length;
+  BOOLEAN             IsBufferStorage;
+  UINTN               MaxLen;
+
+  Status = EFI_SUCCESS;
+  Value  = NULL;
+  Result = NULL;
+
+  if (GetValueFrom >= GetSetValueWithMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Question value is provided by an Expression, evaluate it
+  //
+  if (Question->ValueExpression != NULL) {
+    Status = EvaluateExpression (FormSet, Form, Question->ValueExpression);
+    if (!EFI_ERROR (Status)) {
+      if (Question->ValueExpression->Result.Type == EFI_IFR_TYPE_BUFFER) {
+        ASSERT (Question->HiiValue.Type == EFI_IFR_TYPE_BUFFER && Question->HiiValue.Buffer != NULL);
+        if (Question->StorageWidth > Question->ValueExpression->Result.BufferLen) {
+          CopyMem (Question->HiiValue.Buffer, Question->ValueExpression->Result.Buffer, Question->ValueExpression->Result.BufferLen);
+          Question->HiiValue.BufferLen = Question->ValueExpression->Result.BufferLen;
+        } else {
+          CopyMem (Question->HiiValue.Buffer, Question->ValueExpression->Result.Buffer, Question->StorageWidth);
+          Question->HiiValue.BufferLen = Question->StorageWidth;
+        }
+        FreePool (Question->ValueExpression->Result.Buffer);
+      }
+      Question->HiiValue.Type = Question->ValueExpression->Result.Type;
+      CopyMem (&Question->HiiValue.Value, &Question->ValueExpression->Result.Value, sizeof (EFI_IFR_TYPE_VALUE));      
+    }
+    return Status;
+  }
+  
+  //
+  // Get question value by read expression.
+  //
+  if (Question->ReadExpression != NULL && Form->FormType == STANDARD_MAP_FORM_TYPE) {
+    Status = EvaluateExpression (FormSet, Form, Question->ReadExpression);
+    if (!EFI_ERROR (Status) && 
+      ((Question->ReadExpression->Result.Type < EFI_IFR_TYPE_OTHER) || (Question->ReadExpression->Result.Type == EFI_IFR_TYPE_BUFFER))) {
+      //
+      // Only update question value to the valid result.
+      //
+      if (Question->ReadExpression->Result.Type == EFI_IFR_TYPE_BUFFER) {
+        ASSERT (Question->HiiValue.Type == EFI_IFR_TYPE_BUFFER && Question->HiiValue.Buffer != NULL);
+        if (Question->StorageWidth > Question->ReadExpression->Result.BufferLen) {
+          CopyMem (Question->HiiValue.Buffer, Question->ReadExpression->Result.Buffer, Question->ReadExpression->Result.BufferLen);
+          Question->HiiValue.BufferLen = Question->ReadExpression->Result.BufferLen;
+        } else {
+          CopyMem (Question->HiiValue.Buffer, Question->ReadExpression->Result.Buffer, Question->StorageWidth);
+          Question->HiiValue.BufferLen = Question->StorageWidth;
+        }
+        FreePool (Question->ReadExpression->Result.Buffer);
+      }
+      Question->HiiValue.Type = Question->ReadExpression->Result.Type;
+      CopyMem (&Question->HiiValue.Value, &Question->ReadExpression->Result.Value, sizeof (EFI_IFR_TYPE_VALUE));       
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Question value is provided by RTC
+  //
+  Storage = Question->Storage;
+  QuestionValue = &Question->HiiValue.Value;
+  if (Storage == NULL) {
+    //
+    // It's a Question without storage, or RTC date/time
+    //
+    if (Question->Operand == EFI_IFR_DATE_OP || Question->Operand == EFI_IFR_TIME_OP) {
+      //
+      // Date and time define the same Flags bit
+      //
+      switch (Question->Flags & EFI_QF_DATE_STORAGE) {
+      case QF_DATE_STORAGE_TIME:
+        Status = gRT->GetTime (&EfiTime, NULL);
+        break;
+
+      case QF_DATE_STORAGE_WAKEUP:
+        Status = gRT->GetWakeupTime (&Enabled, &Pending, &EfiTime);
+        break;
+
+      case QF_DATE_STORAGE_NORMAL:
+      default:
+        //
+        // For date/time without storage
+        //
+        return EFI_SUCCESS;
+      }
+
+      if (EFI_ERROR (Status)) {
+        if (Question->Operand == EFI_IFR_DATE_OP){
+          QuestionValue->date.Year  = 0xff;
+          QuestionValue->date.Month = 0xff;
+          QuestionValue->date.Day   = 0xff;
+        } else {
+          QuestionValue->time.Hour   = 0xff;
+          QuestionValue->time.Minute = 0xff;
+          QuestionValue->time.Second = 0xff;
+        }
+        return EFI_SUCCESS;
+      }
+
+      if (Question->Operand == EFI_IFR_DATE_OP) {
+        QuestionValue->date.Year  = EfiTime.Year;
+        QuestionValue->date.Month = EfiTime.Month;
+        QuestionValue->date.Day   = EfiTime.Day;
+      } else {
+        QuestionValue->time.Hour   = EfiTime.Hour;
+        QuestionValue->time.Minute = EfiTime.Minute;
+        QuestionValue->time.Second = EfiTime.Second;
+      }
+    }
+
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Question value is provided by EFI variable
+  //
+  StorageWidth = Question->StorageWidth;
+  if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+    if (Question->BufferValue != NULL) {
+      Dst = Question->BufferValue;
+    } else {
+      Dst = (UINT8 *) QuestionValue;
+    }
+
+    Status = gRT->GetVariable (
+                     Question->VariableName,
+                     &Storage->Guid,
+                     NULL,
+                     &StorageWidth,
+                     Dst
+                     );
+    //
+    // Always return success, even this EFI variable doesn't exist
+    //
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Question Value is provided by Buffer Storage or NameValue Storage
+  //
+  if (Question->BufferValue != NULL) {
+    //
+    // This Question is password or orderedlist
+    //
+    Dst = Question->BufferValue;
+  } else {
+    //
+    // Other type of Questions
+    //
+    Dst = (UINT8 *) &Question->HiiValue.Value;
+  }
+
+  if (Storage->Type == EFI_HII_VARSTORE_BUFFER || 
+      Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+    IsBufferStorage = TRUE;
+  } else {
+    IsBufferStorage = FALSE;
+  }
+  if (GetValueFrom == GetSetValueWithEditBuffer || GetValueFrom == GetSetValueWithBuffer ) {
+    if (IsBufferStorage) {
+      if (GetValueFrom == GetSetValueWithEditBuffer) {
+        //
+        // Copy from storage Edit buffer
+        //
+        CopyMem (Dst, Storage->EditBuffer + Question->VarStoreInfo.VarOffset, StorageWidth);
+      } else {
+        //
+        // Copy from storage Edit buffer
+        //
+        CopyMem (Dst, Storage->Buffer + Question->VarStoreInfo.VarOffset, StorageWidth);
+      }
+    } else {
+      Value = NULL;
+      Status = GetValueByName (Storage, Question->VariableName, &Value, GetValueFrom);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      ASSERT (Value != NULL);
+      Status = BufferToValue (Question, Value);
+      FreePool (Value);
+    }
+  } else {
+    FormsetStorage = GetFstStgFromVarId(FormSet, Question->VarStoreId);
+    ASSERT (FormsetStorage != NULL);
+    //
+    // <ConfigRequest> ::= <ConfigHdr> + <BlockName> ||
+    //                   <ConfigHdr> + "&" + <VariableName>
+    //
+    if (IsBufferStorage) {
+      Length = StrLen (FormsetStorage->ConfigHdr);
+      Length += StrLen (Question->BlockName);
+    } else {
+      Length = StrLen (FormsetStorage->ConfigHdr);
+      Length += StrLen (Question->VariableName) + 1;
+    }
+    // Allocate buffer include '\0'
+    MaxLen = Length + 1;
+    ConfigRequest = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+    ASSERT (ConfigRequest != NULL);
+
+    StrCpyS (ConfigRequest, MaxLen, FormsetStorage->ConfigHdr);
+    if (IsBufferStorage) {
+      StrCatS (ConfigRequest, MaxLen, Question->BlockName);
+    } else {
+      StrCatS (ConfigRequest, MaxLen, L"&");
+      StrCatS (ConfigRequest, MaxLen, Question->VariableName);
+    }
+
+    //
+    // Request current settings from Configuration Driver
+    //
+    Status = mHiiConfigRouting->ExtractConfig (
+                                      mHiiConfigRouting,
+                                      ConfigRequest,
+                                      &Progress,
+                                      &Result
+                                      );
+    FreePool (ConfigRequest);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // Skip <ConfigRequest>
+    //
+    if (IsBufferStorage) {
+      Value = StrStr (Result, L"&VALUE");
+      if (Value == NULL) {
+        FreePool (Result);
+        return EFI_NOT_FOUND;
+      }
+      //
+      // Skip "&VALUE"
+      //
+      Value = Value + 6;
+    } else {
+      Value = Result + Length;
+    }
+    if (*Value != '=') {
+      FreePool (Result);
+      return EFI_NOT_FOUND;
+    }
+    //
+    // Skip '=', point to value
+    //
+    Value = Value + 1;
+
+    Status = BufferToValue (Question, Value);
+    if (EFI_ERROR (Status)) {
+      FreePool (Result);
+      return Status;
+    }
+
+    //
+    // Synchronize Edit Buffer
+    //
+    if (IsBufferStorage) {
+      CopyMem (Storage->EditBuffer + Question->VarStoreInfo.VarOffset, Dst, StorageWidth);
+    } else {
+      SetValueByName (Storage, Question->VariableName, Value, GetSetValueWithEditBuffer, NULL);
+    }
+
+    if (Result != NULL) {
+      FreePool (Result);
+    }
+  }
+
+  return Status;
+}
+
+
+/**
+  Save Question Value to edit copy(cached) or Storage(uncached).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Pointer to the Question.
+  @param  SetValueTo             Update the question value to editbuffer , buffer or hii driver.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+SetQuestionValue (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      SetValueTo
+  )
+{
+  EFI_STATUS          Status;
+  BOOLEAN             Enabled;
+  BOOLEAN             Pending;
+  UINT8               *Src;
+  EFI_TIME            EfiTime;
+  UINTN               BufferLen;
+  UINTN               StorageWidth;
+  BROWSER_STORAGE     *Storage;
+  FORMSET_STORAGE     *FormsetStorage;
+  EFI_IFR_TYPE_VALUE  *QuestionValue;
+  CHAR16              *ConfigResp;
+  CHAR16              *Progress;
+  CHAR16              *Value;
+  UINTN               Length;
+  BOOLEAN             IsBufferStorage;
+  BOOLEAN             IsString;
+  UINT8               *TemBuffer;
+  CHAR16              *TemName;
+  CHAR16              *TemString;
+  UINTN               Index;
+  NAME_VALUE_NODE     *Node;
+  UINTN               MaxLen;
+
+  Status = EFI_SUCCESS;
+  Node   = NULL;
+
+  if (SetValueTo >= GetSetValueWithMax) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // If Question value is provided by an Expression, then it is read only
+  //
+  if (Question->ValueExpression != NULL) {
+    return Status;
+  }
+  
+  //
+  // Before set question value, evaluate its write expression.
+  //
+  if (Question->WriteExpression != NULL && Form->FormType == STANDARD_MAP_FORM_TYPE) {
+    Status = EvaluateExpression (FormSet, Form, Question->WriteExpression);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Question value is provided by RTC
+  //
+  Storage = Question->Storage;
+  QuestionValue = &Question->HiiValue.Value;
+  if (Storage == NULL) {
+    //
+    // It's a Question without storage, or RTC date/time
+    //
+    if (Question->Operand == EFI_IFR_DATE_OP || Question->Operand == EFI_IFR_TIME_OP) {
+      //
+      // Date and time define the same Flags bit
+      //
+      switch (Question->Flags & EFI_QF_DATE_STORAGE) {
+      case QF_DATE_STORAGE_TIME:
+        Status = gRT->GetTime (&EfiTime, NULL);
+        break;
+
+      case QF_DATE_STORAGE_WAKEUP:
+        Status = gRT->GetWakeupTime (&Enabled, &Pending, &EfiTime);
+        break;
+
+      case QF_DATE_STORAGE_NORMAL:
+      default:
+        //
+        // For date/time without storage
+        //
+        return EFI_SUCCESS;
+      }
+
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      if (Question->Operand == EFI_IFR_DATE_OP) {
+        EfiTime.Year  = QuestionValue->date.Year;
+        EfiTime.Month = QuestionValue->date.Month;
+        EfiTime.Day   = QuestionValue->date.Day;
+      } else {
+        EfiTime.Hour   = QuestionValue->time.Hour;
+        EfiTime.Minute = QuestionValue->time.Minute;
+        EfiTime.Second = QuestionValue->time.Second;
+      }
+
+      if ((Question->Flags & EFI_QF_DATE_STORAGE) == QF_DATE_STORAGE_TIME) {
+        Status = gRT->SetTime (&EfiTime);
+      } else {
+        Status = gRT->SetWakeupTime (TRUE, &EfiTime);
+      }
+    }
+
+    return Status;
+  }
+
+  //
+  // Question value is provided by EFI variable
+  //
+  StorageWidth = Question->StorageWidth;
+  if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+    if (Question->BufferValue != NULL) {
+      Src = Question->BufferValue;
+    } else {
+      Src = (UINT8 *) QuestionValue;
+    }
+
+    Status = gRT->SetVariable (
+                     Question->VariableName,
+                     &Storage->Guid,
+                     Storage->Attributes,
+                     StorageWidth,
+                     Src
+                     );
+    return Status;
+  }
+
+  //
+  // Question Value is provided by Buffer Storage or NameValue Storage
+  //
+  if (Question->BufferValue != NULL) {
+    Src = Question->BufferValue;
+  } else {
+    Src = (UINT8 *) &Question->HiiValue.Value;
+  }
+
+  if (Storage->Type == EFI_HII_VARSTORE_BUFFER || 
+      Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+    IsBufferStorage = TRUE;
+  } else {
+    IsBufferStorage = FALSE;
+  }
+  IsString = (BOOLEAN) ((Question->HiiValue.Type == EFI_IFR_TYPE_STRING) ?  TRUE : FALSE);
+
+  if (SetValueTo == GetSetValueWithEditBuffer || SetValueTo == GetSetValueWithBuffer) {
+    if (IsBufferStorage) {
+      if (SetValueTo == GetSetValueWithEditBuffer) {
+        //
+        // Copy to storage edit buffer
+        //      
+        CopyMem (Storage->EditBuffer + Question->VarStoreInfo.VarOffset, Src, StorageWidth);
+      } else if (SetValueTo == GetSetValueWithBuffer) {
+        //
+        // Copy to storage edit buffer
+        //     
+        CopyMem (Storage->Buffer + Question->VarStoreInfo.VarOffset, Src, StorageWidth);
+      }
+    } else {
+      if (IsString) {
+        //
+        // Allocate enough string buffer.
+        //
+        Value = NULL;
+        BufferLen = ((StrLen ((CHAR16 *) Src) * 4) + 1) * sizeof (CHAR16);
+        Value = AllocateZeroPool (BufferLen);
+        ASSERT (Value != NULL);
+        //
+        // Convert Unicode String to Config String, e.g. "ABCD" => "0041004200430044"
+        //
+        TemName = (CHAR16 *) Src;
+        TemString = Value;
+        for (; *TemName != L'\0'; TemName++) {
+          TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemName, 4);
+        }
+      } else {
+        BufferLen = StorageWidth * 2 + 1;
+        Value = AllocateZeroPool (BufferLen * sizeof (CHAR16));
+        ASSERT (Value != NULL);
+        //
+        // Convert Buffer to Hex String
+        //
+        TemBuffer = Src + StorageWidth - 1;
+        TemString = Value;
+        for (Index = 0; Index < StorageWidth; Index ++, TemBuffer --) {
+          TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemBuffer, 2);
+        }
+      }
+
+      Status = SetValueByName (Storage, Question->VariableName, Value, SetValueTo, &Node);
+      FreePool (Value);
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+    }
+  } else if (SetValueTo == GetSetValueWithHiiDriver) {
+    //
+    // <ConfigResp> ::= <ConfigHdr> + <BlockName> + "&VALUE=" + "<HexCh>StorageWidth * 2" ||
+    //                <ConfigHdr> + "&" + <VariableName> + "=" + "<string>"
+    //
+    if (IsBufferStorage) {
+      Length = StrLen (Question->BlockName) + 7;
+    } else {
+      Length = StrLen (Question->VariableName) + 2;
+    }
+    if (!IsBufferStorage && IsString) {
+      Length += (StrLen ((CHAR16 *) Src) * 4);
+    } else {
+      Length += (StorageWidth * 2);
+    }
+    FormsetStorage = GetFstStgFromVarId(FormSet, Question->VarStoreId);
+    ASSERT (FormsetStorage != NULL);
+    MaxLen = StrLen (FormsetStorage->ConfigHdr) + Length + 1;
+    ConfigResp = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+    ASSERT (ConfigResp != NULL);
+
+    StrCpyS (ConfigResp, MaxLen, FormsetStorage->ConfigHdr);
+    if (IsBufferStorage) {
+      StrCatS (ConfigResp, MaxLen, Question->BlockName);
+      StrCatS (ConfigResp, MaxLen, L"&VALUE=");
+    } else {
+      StrCatS (ConfigResp, MaxLen, L"&");
+      StrCatS (ConfigResp, MaxLen, Question->VariableName);
+      StrCatS (ConfigResp, MaxLen, L"=");
+    }
+
+    Value = ConfigResp + StrLen (ConfigResp);
+
+    if (!IsBufferStorage && IsString) {
+      //
+      // Convert Unicode String to Config String, e.g. "ABCD" => "0041004200430044"
+      //
+      TemName = (CHAR16 *) Src;
+      TemString = Value;
+      for (; *TemName != L'\0'; TemName++) {
+        TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemName, 4);
+      }
+    } else {
+      //
+      // Convert Buffer to Hex String
+      //
+      TemBuffer = Src + StorageWidth - 1;
+      TemString = Value;
+      for (Index = 0; Index < StorageWidth; Index ++, TemBuffer --) {
+        TemString += UnicodeValueToString (TemString, PREFIX_ZERO | RADIX_HEX, *TemBuffer, 2);
+      }
+    }
+
+    //
+    // Convert to lower char.
+    //
+    for (TemString = Value; *Value != L'\0'; Value++) {
+      if (*Value >= L'A' && *Value <= L'Z') {
+        *Value = (CHAR16) (*Value - L'A' + L'a');
+      }
+    }
+
+    //
+    // Submit Question Value to Configuration Driver
+    //
+    Status = mHiiConfigRouting->RouteConfig (
+                                      mHiiConfigRouting,
+                                      ConfigResp,
+                                      &Progress
+                                      );
+    if (EFI_ERROR (Status)) {
+      FreePool (ConfigResp);
+      return Status;
+    }
+    FreePool (ConfigResp);
+    
+    //
+    // Sync storage, from editbuffer to buffer.
+    //
+    CopyMem (Storage->Buffer + Question->VarStoreInfo.VarOffset, Src, StorageWidth);
+  }
+
+  return Status;
+}
+
+
+/**
+  Perform nosubmitif check for a Form.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               The Question to be validated.
+  @param  Type                   Validation type: NoSubmit
+
+  @retval EFI_SUCCESS            Form validation pass.
+  @retval other                  Form validation failed.
+
+**/
+EFI_STATUS
+ValidateQuestion (
+  IN  FORM_BROWSER_FORMSET            *FormSet,
+  IN  FORM_BROWSER_FORM               *Form,
+  IN  FORM_BROWSER_STATEMENT          *Question,
+  IN  UINTN                           Type
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  LIST_ENTRY              *ListHead;
+  FORM_EXPRESSION         *Expression;
+  UINT32                  BrowserStatus;
+  CHAR16                  *ErrorStr;
+
+  BrowserStatus = BROWSER_SUCCESS;
+  ErrorStr      = NULL;
+
+  switch (Type) {
+  case EFI_HII_EXPRESSION_INCONSISTENT_IF:
+    ListHead = &Question->InconsistentListHead;
+    break;
+
+  case EFI_HII_EXPRESSION_WARNING_IF:
+    ListHead = &Question->WarningListHead;
+    break;
+
+  case EFI_HII_EXPRESSION_NO_SUBMIT_IF:
+    ListHead = &Question->NoSubmitListHead;
+    break;
+
+  default:
+    ASSERT (FALSE);
+    return EFI_UNSUPPORTED;
+  }
+
+  Link = GetFirstNode (ListHead);
+  while (!IsNull (ListHead, Link)) {
+    Expression = FORM_EXPRESSION_FROM_LINK (Link);
+
+    //
+    // Evaluate the expression
+    //
+    Status = EvaluateExpression (FormSet, Form, Expression);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if (IsTrue (&Expression->Result)) {
+      switch (Type) {
+      case EFI_HII_EXPRESSION_INCONSISTENT_IF:
+        BrowserStatus = BROWSER_INCONSISTENT_IF;
+        break;
+
+      case EFI_HII_EXPRESSION_WARNING_IF:
+        BrowserStatus = BROWSER_WARNING_IF;
+        break;
+
+      case EFI_HII_EXPRESSION_NO_SUBMIT_IF:
+        BrowserStatus = BROWSER_NO_SUBMIT_IF;
+        //
+        // This code only used to compatible with old display engine,
+        // New display engine will not use this field.
+        //
+        if (Expression->Error != 0) {
+          ErrorStr = GetToken (Expression->Error, FormSet->HiiHandle);
+        }
+        break;
+
+      default:
+        ASSERT (FALSE);
+        break;
+      }
+
+      if (!((Type == EFI_HII_EXPRESSION_NO_SUBMIT_IF) && mSystemSubmit)) {
+        //
+        // If in system submit process and for no_submit_if check, not popup this error message.
+        // Will process this fail again later in not system submit process.
+        //
+        PopupErrorMessage(BrowserStatus, FormSet->HiiHandle, Expression->OpCode, ErrorStr);
+      }
+
+      if (ErrorStr != NULL) {
+        FreePool (ErrorStr);
+      }
+
+      if (Type == EFI_HII_EXPRESSION_WARNING_IF) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    Link = GetNextNode (ListHead, Link);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Perform question check. 
+  
+  If one question has more than one check, process form high priority to low. 
+  Only one error info will be popup.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               The Question to be validated.
+
+  @retval EFI_SUCCESS            Form validation pass.
+  @retval other                  Form validation failed.
+
+**/
+EFI_STATUS
+ValueChangedValidation (
+  IN  FORM_BROWSER_FORMSET            *FormSet,
+  IN  FORM_BROWSER_FORM               *Form,
+  IN  FORM_BROWSER_STATEMENT          *Question
+  )
+{
+  EFI_STATUS   Status;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Do the inconsistentif check.
+  //
+  if (!IsListEmpty (&Question->InconsistentListHead)) {
+    Status = ValidateQuestion (FormSet, Form, Question, EFI_HII_EXPRESSION_INCONSISTENT_IF);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Do the warningif check.
+  //
+  if (!IsListEmpty (&Question->WarningListHead)) {
+    Status = ValidateQuestion (FormSet, Form, Question, EFI_HII_EXPRESSION_WARNING_IF);
+  }
+
+  return Status;
+}
+
+/**
+  Perform NoSubmit check for each Form in FormSet.
+
+  @param  FormSet                FormSet data structure.
+  @param  CurrentForm            Current input form data structure.
+  @param  Statement              The statement for this check.
+
+  @retval EFI_SUCCESS            Form validation pass.
+  @retval other                  Form validation failed.
+
+**/
+EFI_STATUS
+NoSubmitCheck (
+  IN      FORM_BROWSER_FORMSET            *FormSet,
+  IN OUT  FORM_BROWSER_FORM               **CurrentForm,
+  OUT     FORM_BROWSER_STATEMENT          **Statement
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Question;
+  FORM_BROWSER_FORM       *Form;
+  LIST_ENTRY              *LinkForm;
+
+  LinkForm = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, LinkForm)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (LinkForm);
+    LinkForm = GetNextNode (&FormSet->FormListHead, LinkForm);
+
+    if (*CurrentForm != NULL && *CurrentForm != Form) {
+      continue;
+    }
+
+    Link = GetFirstNode (&Form->StatementListHead);
+    while (!IsNull (&Form->StatementListHead, Link)) {
+      Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+      Status = ValidateQuestion (FormSet, Form, Question, EFI_HII_EXPRESSION_NO_SUBMIT_IF);
+      if (EFI_ERROR (Status)) {
+        if (*CurrentForm == NULL) {
+          *CurrentForm = Form;
+        }
+        if (Statement != NULL) {
+          *Statement = Question;
+        }
+        return Status;
+      }
+
+      Link = GetNextNode (&Form->StatementListHead, Link);
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Fill storage's edit copy with settings requested from Configuration Driver.
+
+  @param  Storage                The storage which need to sync.
+  @param  ConfigRequest          The config request string which used to sync storage.
+  @param  SyncOrRestore          Sync the buffer to editbuffer or Restore  the 
+                                 editbuffer to buffer
+                                 if TRUE, copy the editbuffer to the buffer.
+                                 if FALSE, copy the buffer to the editbuffer.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+SynchronizeStorage (
+  OUT BROWSER_STORAGE             *Storage,
+  IN  CHAR16                      *ConfigRequest,
+  IN  BOOLEAN                     SyncOrRestore
+  )
+{
+  EFI_STATUS              Status;
+  EFI_STRING              Progress;
+  EFI_STRING              Result;
+  UINTN                   BufferSize;
+  LIST_ENTRY              *Link;
+  NAME_VALUE_NODE         *Node;
+  UINT8                   *Src;
+  UINT8                   *Dst;
+
+  Status = EFI_SUCCESS;
+  Result = NULL;
+
+  if (Storage->Type == EFI_HII_VARSTORE_BUFFER || 
+      (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER)) {
+    BufferSize = Storage->Size;
+
+    if (SyncOrRestore) {
+      Src = Storage->EditBuffer;
+      Dst = Storage->Buffer;
+    } else {
+      Src = Storage->Buffer;
+      Dst = Storage->EditBuffer;
+    }
+
+    if (ConfigRequest != NULL) {
+      Status = mHiiConfigRouting->BlockToConfig(
+                                    mHiiConfigRouting,
+                                    ConfigRequest,
+                                    Src,
+                                    BufferSize,
+                                    &Result,
+                                    &Progress
+                                    );
+      if (EFI_ERROR (Status)) {
+        return Status;
+      }
+
+      Status = mHiiConfigRouting->ConfigToBlock (
+                                    mHiiConfigRouting,
+                                    Result,
+                                    Dst,
+                                    &BufferSize,
+                                    &Progress
+                                    );
+      if (Result != NULL) {
+        FreePool (Result);
+      }
+    } else {
+      CopyMem (Dst, Src, BufferSize);
+    }
+  } else if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    Link = GetFirstNode (&Storage->NameValueListHead);
+    while (!IsNull (&Storage->NameValueListHead, Link)) {
+      Node = NAME_VALUE_NODE_FROM_LINK (Link);
+
+      if ((ConfigRequest != NULL && StrStr (ConfigRequest, Node->Name) != NULL) ||
+          (ConfigRequest == NULL)) {
+        if (SyncOrRestore) {
+          NewStringCpy (&Node->Value, Node->EditValue);
+        } else {
+          NewStringCpy (&Node->EditValue, Node->Value);
+        }
+      }
+
+      Link = GetNextNode (&Storage->NameValueListHead, Link);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  When discard the question value, call the callback function with Changed type
+  to inform the hii driver.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+**/
+VOID
+SendDiscardInfoToDriver (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form
+  )
+{
+  LIST_ENTRY                  *Link;
+  FORM_BROWSER_STATEMENT      *Question;
+  EFI_IFR_TYPE_VALUE          *TypeValue;
+  EFI_BROWSER_ACTION_REQUEST  ActionRequest;
+
+  if (FormSet->ConfigAccess == NULL) {
+    return;
+  }
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&Form->StatementListHead, Link);
+
+    if (Question->Storage == NULL || Question->Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+      continue;
+    }
+
+    if ((Question->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != EFI_IFR_FLAG_CALLBACK) {
+      continue;
+    }
+
+    if (Question->Operand == EFI_IFR_PASSWORD_OP) {
+      continue;
+    }
+
+    if (!Question->ValueChanged) {
+      continue;
+    }
+
+    //
+    // Restore the question value before call the CHANGED callback type.
+    //
+    GetQuestionValue (FormSet, Form, Question, GetSetValueWithEditBuffer);
+
+    if (Question->HiiValue.Type == EFI_IFR_TYPE_BUFFER) {
+      TypeValue = (EFI_IFR_TYPE_VALUE *) Question->BufferValue;
+    } else {
+      TypeValue = &Question->HiiValue.Value;
+    }
+
+    ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+    FormSet->ConfigAccess->Callback (
+                             FormSet->ConfigAccess,
+                             EFI_BROWSER_ACTION_CHANGED,
+                             Question->QuestionId,
+                             Question->HiiValue.Type,
+                             TypeValue,
+                             &ActionRequest
+                             );
+  }
+}
+
+/**
+  Validate the HiiHandle.
+
+  @param  HiiHandle              The input HiiHandle which need to validate.
+
+  @retval TRUE                   The handle is validate.
+  @retval FALSE                  The handle is invalidate.
+
+**/
+BOOLEAN
+ValidateHiiHandle (
+  EFI_HII_HANDLE          HiiHandle
+  )
+{
+  EFI_HII_HANDLE          *HiiHandles;
+  UINTN                   Index;
+  BOOLEAN                 Find;
+
+  if (HiiHandle == NULL) {
+    return FALSE;
+  }
+
+  Find = FALSE;
+
+  HiiHandles = HiiGetHiiHandles (NULL);
+  ASSERT (HiiHandles != NULL);
+
+  for (Index = 0; HiiHandles[Index] != NULL; Index++) {
+    if (HiiHandles[Index] == HiiHandle) {
+      Find = TRUE;
+      break;
+    }
+  }
+
+  FreePool (HiiHandles);
+
+  return Find;
+}
+
+/**
+  Validate the FormSet. If the formset is not validate, remove it from the list.
+
+  @param  FormSet                The input FormSet which need to validate.
+
+  @retval TRUE                   The handle is validate.
+  @retval FALSE                  The handle is invalidate.
+
+**/
+BOOLEAN
+ValidateFormSet (
+  FORM_BROWSER_FORMSET    *FormSet
+  )
+{
+  BOOLEAN  Find;
+
+  ASSERT (FormSet != NULL);
+
+  Find = ValidateHiiHandle(FormSet->HiiHandle);
+  //
+  // Should not remove the formset which is being used.
+  //
+  if (!Find && (FormSet != gCurrentSelection->FormSet)) {
+    CleanBrowserStorage(FormSet);
+    RemoveEntryList (&FormSet->Link);
+    DestroyFormSet (FormSet);
+  }
+
+  return Find;
+}
+/**
+  Check whether need to enable the reset flag in form level.
+  Also clean all ValueChanged flag in question.
+
+  @param  SetFlag                Whether need to set the Reset Flag.
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+**/
+VOID
+UpdateFlagForForm (
+  IN BOOLEAN                          SetFlag,
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Question;
+  BOOLEAN                 OldValue;
+
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+    Link = GetNextNode (&Form->StatementListHead, Link);
+
+    if (!Question->ValueChanged) {
+      continue;
+    }
+
+    OldValue = Question->ValueChanged;
+
+    //
+    // Compare the buffer and editbuffer data to see whether the data has been saved.
+    //
+    Question->ValueChanged = IsQuestionValueChanged(FormSet, Form, Question, GetSetValueWithBothBuffer);
+
+    //
+    // Only the changed data has been saved, then need to set the reset flag.
+    //
+    if (SetFlag && OldValue && !Question->ValueChanged) {
+      if ((Question->QuestionFlags & EFI_IFR_FLAG_RESET_REQUIRED) != 0) {
+        gResetRequired = TRUE;
+      }
+
+      if ((Question->QuestionFlags & EFI_IFR_FLAG_RECONNECT_REQUIRED) != 0) {
+        gFlagReconnect = TRUE;
+      }
+    } 
+  }
+}
+
+/**
+  Check whether need to enable the reset flag.
+  Also clean ValueChanged flag for all statements.
+
+  Form level or formset level, only one.
+  
+  @param  SetFlag                Whether need to set the Reset Flag.
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+**/
+VOID
+ValueChangeResetFlagUpdate (
+  IN BOOLEAN                          SetFlag,
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form  
+  )
+{
+  FORM_BROWSER_FORM       *CurrentForm;
+  LIST_ENTRY              *Link;
+
+  if (Form != NULL) {
+    UpdateFlagForForm(SetFlag, FormSet, Form);
+    return;
+  }
+
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    CurrentForm = FORM_BROWSER_FORM_FROM_LINK (Link);
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+
+    UpdateFlagForForm(SetFlag, FormSet, CurrentForm);
+  }
+}
+
+/**
+  Base on the return Progress string to find the form. 
+  
+  Base on the first return Offset/Width (Name) string to find the form
+  which keep this string.
+
+  @param  FormSet                FormSet data structure.
+  @param  Storage                Storage which has this Progress string.
+  @param  Progress               The Progress string which has the first fail string.
+  @param  RetForm                The return form for this progress string.
+  @param  RetQuestion            The return question for the error progress string.
+
+  @retval TRUE                   Find the error form and statement for this error progress string.
+  @retval FALSE                  Not find the error form.
+
+**/
+BOOLEAN
+FindQuestionFromProgress (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN BROWSER_STORAGE                  *Storage,
+  IN EFI_STRING                       Progress,
+  OUT FORM_BROWSER_FORM               **RetForm,
+  OUT FORM_BROWSER_STATEMENT          **RetQuestion
+  )
+{
+  LIST_ENTRY                   *Link;
+  LIST_ENTRY                   *LinkStorage;
+  LIST_ENTRY                   *LinkStatement;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+  FORM_BROWSER_FORM            *Form;
+  EFI_STRING                   EndStr;
+  FORM_BROWSER_STATEMENT       *Statement;
+
+  ASSERT ((*Progress == '&') || (*Progress == 'G'));
+
+  ConfigInfo   = NULL;
+  *RetForm     = NULL;
+  *RetQuestion = NULL;
+
+  //
+  // Skip the first "&" or the ConfigHdr part.
+  //
+  if (*Progress == '&') {
+    Progress++;
+  } else {
+    //
+    // Prepare the "NAME" or "OFFSET=0x####&WIDTH=0x####" string.
+    //
+    if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+      //
+      // For Name/Value type, Skip the ConfigHdr part.
+      //
+      EndStr = StrStr (Progress, L"PATH=");
+      ASSERT (EndStr != NULL);
+      while (*EndStr != '&') {
+        EndStr++;
+      }
+
+      *EndStr = '\0';
+    } else {
+      //
+      // For Buffer type, Skip the ConfigHdr part.
+      //
+      EndStr = StrStr (Progress, L"&OFFSET=");
+      ASSERT (EndStr != NULL);
+      *EndStr = '\0';
+    }
+
+    Progress = EndStr + 1;
+  }
+
+  //
+  // Prepare the "NAME" or "OFFSET=0x####&WIDTH=0x####" string.
+  //
+  if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    //
+    // For Name/Value type, the data is "&Fred=16&George=16&Ron=12" formset,
+    // here, just keep the "Fred" string.
+    //
+    EndStr = StrStr (Progress, L"=");
+    ASSERT (EndStr != NULL);
+    *EndStr = '\0';
+  } else {
+    //
+    // For Buffer type, the data is "OFFSET=0x####&WIDTH=0x####&VALUE=0x####",
+    // here, just keep the "OFFSET=0x####&WIDTH=0x####" string.
+    //
+    EndStr = StrStr (Progress, L"&VALUE=");
+    ASSERT (EndStr != NULL);
+    *EndStr = '\0';
+  }
+
+  //
+  // Search in the form list.
+  //
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+
+    //
+    // Search in the ConfigReqeust list in this form.
+    //
+    LinkStorage = GetFirstNode (&Form->ConfigRequestHead);
+    while (!IsNull (&Form->ConfigRequestHead, LinkStorage)) {
+      ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (LinkStorage);
+      LinkStorage = GetNextNode (&Form->ConfigRequestHead, LinkStorage);
+
+      if (Storage != ConfigInfo->Storage) {
+        continue;
+      }
+
+      if (StrStr (ConfigInfo->ConfigRequest, Progress) != NULL) {
+        //
+        // Find the OffsetWidth string in this form.
+        //
+        *RetForm = Form;
+        break;
+      }
+    }
+
+    if (*RetForm != NULL) {
+      LinkStatement = GetFirstNode (&Form->StatementListHead);
+      while (!IsNull (&Form->StatementListHead, LinkStatement)) {
+        Statement = FORM_BROWSER_STATEMENT_FROM_LINK (LinkStatement);
+        LinkStatement = GetNextNode (&Form->StatementListHead, LinkStatement);
+
+        if (Statement->BlockName != NULL && StrStr (Statement->BlockName, Progress) != NULL) {
+          *RetQuestion = Statement;
+          break;
+        }
+
+        if (Statement->VariableName != NULL && StrStr (Statement->VariableName, Progress) != NULL) {
+          *RetQuestion = Statement;
+          break;
+        }
+      }
+    }
+
+    if (*RetForm != NULL) {
+      break;
+    }
+  }
+
+  //
+  // restore the OffsetWidth string to the original format.
+  //
+  if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    *EndStr = '=';
+  } else {
+    *EndStr = '&';
+  }
+
+  return (BOOLEAN) (*RetForm != NULL);
+}
+
+/**
+  Popup an save error info and get user input.
+
+  @param  TitleId                The form title id.
+  @param  HiiHandle              The hii handle for this package.
+
+  @retval UINT32                 The user select option for the save fail.
+                                 BROWSER_ACTION_DISCARD or BROWSER_ACTION_JUMP_TO_FORMSET
+**/
+UINT32
+ConfirmSaveFail (
+  IN EFI_STRING_ID    TitleId,
+  IN EFI_HII_HANDLE   HiiHandle
+  )
+{
+  CHAR16                  *FormTitle;
+  CHAR16                  *StringBuffer;
+  UINT32                  RetVal;
+
+  FormTitle = GetToken (TitleId, HiiHandle);
+
+  StringBuffer = AllocateZeroPool (256 * sizeof (CHAR16));
+  ASSERT (StringBuffer != NULL);
+
+  UnicodeSPrint (
+    StringBuffer, 
+    24 * sizeof (CHAR16) + StrSize (FormTitle), 
+    L"Submit Fail For Form: %s.", 
+    FormTitle
+    );
+
+  RetVal = PopupErrorMessage(BROWSER_SUBMIT_FAIL, NULL, NULL, StringBuffer);
+
+  FreePool (StringBuffer);
+  FreePool (FormTitle);
+
+  return RetVal;
+}
+
+/**
+  Popup an NO_SUBMIT_IF error info and get user input.
+
+  @param  TitleId                The form title id.
+  @param  HiiHandle              The hii handle for this package.
+
+  @retval UINT32                 The user select option for the save fail.
+                                 BROWSER_ACTION_DISCARD or BROWSER_ACTION_JUMP_TO_FORMSET
+**/
+UINT32
+ConfirmNoSubmitFail (
+  IN EFI_STRING_ID    TitleId,
+  IN EFI_HII_HANDLE   HiiHandle
+  )
+{
+  CHAR16                  *FormTitle;
+  CHAR16                  *StringBuffer;
+  UINT32                  RetVal;
+
+  FormTitle = GetToken (TitleId, HiiHandle);
+
+  StringBuffer = AllocateZeroPool (256 * sizeof (CHAR16));
+  ASSERT (StringBuffer != NULL);
+
+  UnicodeSPrint (
+    StringBuffer, 
+    24 * sizeof (CHAR16) + StrSize (FormTitle), 
+    L"NO_SUBMIT_IF error For Form: %s.", 
+    FormTitle
+    );
+
+  RetVal = PopupErrorMessage(BROWSER_SUBMIT_FAIL_NO_SUBMIT_IF, NULL, NULL, StringBuffer);
+
+  FreePool (StringBuffer);
+  FreePool (FormTitle);
+
+  return RetVal;
+}
+
+/**
+  Discard data based on the input setting scope (Form, FormSet or System).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Discard action.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+DiscardForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  )
+{
+  LIST_ENTRY                   *Link;
+  FORMSET_STORAGE              *Storage;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+  FORM_BROWSER_FORMSET         *LocalFormSet;
+  FORM_BROWSER_FORMSET         *OldFormSet;
+
+  //
+  // Check the supported setting level.
+  //
+  if (SettingScope >= MaxLevel) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (SettingScope == FormLevel && IsNvUpdateRequiredForForm (Form)) {
+    ConfigInfo = NULL;
+    Link = GetFirstNode (&Form->ConfigRequestHead);
+    while (!IsNull (&Form->ConfigRequestHead, Link)) {
+      ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+      Link = GetNextNode (&Form->ConfigRequestHead, Link);
+
+      if (ConfigInfo->Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+        continue;
+      }
+
+      //
+      // Skip if there is no RequestElement
+      //
+      if (ConfigInfo->ElementCount == 0) {
+        continue;
+      }
+
+      //
+      // Prepare <ConfigResp>
+      //
+      SynchronizeStorage(ConfigInfo->Storage, ConfigInfo->ConfigRequest, FALSE);
+
+      //
+      // Call callback with Changed type to inform the driver.
+      //
+      SendDiscardInfoToDriver (FormSet, Form);
+    }
+
+    ValueChangeResetFlagUpdate (FALSE, FormSet, Form);
+  } else if (SettingScope == FormSetLevel && IsNvUpdateRequiredForFormSet (FormSet)) {
+
+    //
+    // Discard Buffer storage or Name/Value storage
+    //
+    Link = GetFirstNode (&FormSet->StorageListHead);
+    while (!IsNull (&FormSet->StorageListHead, Link)) {
+      Storage = FORMSET_STORAGE_FROM_LINK (Link);
+      Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+      if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+        continue;
+      }
+
+      //
+      // Skip if there is no RequestElement
+      //
+      if (Storage->ElementCount == 0) {
+        continue;
+      }
+
+      SynchronizeStorage(Storage->BrowserStorage, Storage->ConfigRequest, FALSE);
+    }
+
+    Link = GetFirstNode (&FormSet->FormListHead);
+    while (!IsNull (&FormSet->FormListHead, Link)) {
+      Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+      Link = GetNextNode (&FormSet->FormListHead, Link);
+      
+      //
+      // Call callback with Changed type to inform the driver.
+      //
+      SendDiscardInfoToDriver (FormSet, Form);
+    }
+
+    ValueChangeResetFlagUpdate(FALSE, FormSet, NULL);
+  } else if (SettingScope == SystemLevel) {
+    //
+    // System Level Discard.
+    //
+    OldFormSet = mSystemLevelFormSet;
+
+    //
+    // Discard changed value for each FormSet in the maintain list.
+    //
+    Link = GetFirstNode (&gBrowserFormSetList);
+    while (!IsNull (&gBrowserFormSetList, Link)) {
+      LocalFormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+      Link = GetNextNode (&gBrowserFormSetList, Link);
+      if (!ValidateFormSet(LocalFormSet)) {
+        continue;
+      }
+
+      mSystemLevelFormSet = LocalFormSet;
+
+      DiscardForm (LocalFormSet, NULL, FormSetLevel);
+      if (!IsHiiHandleInBrowserContext (LocalFormSet->HiiHandle)) {
+        //
+        // Remove maintain backup list after discard except for the current using FormSet.
+        //
+        CleanBrowserStorage(LocalFormSet);
+        RemoveEntryList (&LocalFormSet->Link);
+        DestroyFormSet (LocalFormSet);
+      }
+    }
+
+    mSystemLevelFormSet = OldFormSet;
+  }
+
+  return EFI_SUCCESS;  
+}
+
+/**
+  Submit data for a form.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+SubmitForForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  EFI_STRING              ConfigResp;
+  EFI_STRING              Progress;
+  BROWSER_STORAGE         *Storage;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+
+  if (!IsNvUpdateRequiredForForm (Form)) {
+    return EFI_SUCCESS;
+  }
+
+  Status = NoSubmitCheck (FormSet, &Form, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Link = GetFirstNode (&Form->ConfigRequestHead);
+  while (!IsNull (&Form->ConfigRequestHead, Link)) {
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+    Link = GetNextNode (&Form->ConfigRequestHead, Link);
+
+    Storage = ConfigInfo->Storage;
+    if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+      continue;
+    }
+
+    //
+    // Skip if there is no RequestElement
+    //
+    if (ConfigInfo->ElementCount == 0) {
+      continue;
+    }
+
+    //
+    // 1. Prepare <ConfigResp>
+    //
+    Status = StorageToConfigResp (ConfigInfo->Storage, &ConfigResp, ConfigInfo->ConfigRequest, TRUE);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // 2. Set value to hii config routine protocol.
+    //
+    Status = mHiiConfigRouting->RouteConfig (
+                                      mHiiConfigRouting,
+                                      ConfigResp,
+                                      &Progress
+                                      );
+    FreePool (ConfigResp);
+
+    if (EFI_ERROR (Status)) {
+      InsertTailList (&gBrowserSaveFailFormSetList, &ConfigInfo->SaveFailLink);
+      continue;
+    }
+
+    //
+    // 3. Config success, update storage shadow Buffer, only update the data belong to this form.
+    //
+    SynchronizeStorage (ConfigInfo->Storage, ConfigInfo->ConfigRequest, TRUE);
+  }
+
+  //
+  // 4. Process the save failed storage.
+  //
+  if (!IsListEmpty (&gBrowserSaveFailFormSetList)) {
+    if (ConfirmSaveFail (Form->FormTitle, FormSet->HiiHandle) == BROWSER_ACTION_DISCARD) {
+      Link = GetFirstNode (&gBrowserSaveFailFormSetList);
+      while (!IsNull (&gBrowserSaveFailFormSetList, Link)) {
+        ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_SAVE_FAIL_LINK (Link);
+        Link = GetNextNode (&gBrowserSaveFailFormSetList, Link);
+
+        SynchronizeStorage(ConfigInfo->Storage, ConfigInfo->ConfigRequest, FALSE);
+
+        Status = EFI_SUCCESS;
+      }
+    } else {
+      Status = EFI_UNSUPPORTED;
+    }
+
+    //
+    // Free Form save fail list.
+    //
+    while (!IsListEmpty (&gBrowserSaveFailFormSetList)) {
+      Link = GetFirstNode (&gBrowserSaveFailFormSetList);
+      ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_SAVE_FAIL_LINK (Link);
+      RemoveEntryList (&ConfigInfo->SaveFailLink);
+    }
+  }
+
+  //
+  // 5. Update the NV flag.
+  //
+  ValueChangeResetFlagUpdate(TRUE, FormSet, Form);
+
+  return Status;
+}
+
+/**
+  Submit data for a formset.
+
+  @param  FormSet                FormSet data structure.
+  @param  SkipProcessFail        Whether skip to process the save failed storage.
+                                 If submit formset is called when do system level save, 
+                                 set this value to true and process the failed formset 
+                                 together. 
+                                 if submit formset is called when do formset level save,
+                                 set the value to false and process the failed storage
+                                 right after process all storages for this formset.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+SubmitForFormSet (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN BOOLEAN                          SkipProcessFail
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  EFI_STRING              ConfigResp;
+  EFI_STRING              Progress;
+  BROWSER_STORAGE         *Storage;
+  FORMSET_STORAGE         *FormSetStorage;
+  FORM_BROWSER_FORM       *Form;
+  BOOLEAN                 HasInserted;
+  FORM_BROWSER_STATEMENT  *Question;
+
+  HasInserted = FALSE;
+
+  if (!IsNvUpdateRequiredForFormSet (FormSet)) {
+    return EFI_SUCCESS;
+  }
+
+  Form = NULL; 
+  Status = NoSubmitCheck (FormSet, &Form, &Question);
+  if (EFI_ERROR (Status)) {
+    if (SkipProcessFail) {
+      //
+      // Process NO_SUBMIT check first, so insert it at head.
+      //
+      FormSet->SaveFailForm = Form;
+      FormSet->SaveFailStatement = Question;
+      InsertHeadList (&gBrowserSaveFailFormSetList, &FormSet->SaveFailLink);
+    }
+
+    return Status;
+  }
+
+  Form = NULL;
+  Question = NULL;
+  //
+  // Submit Buffer storage or Name/Value storage
+  //
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    FormSetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+    Storage        = FormSetStorage->BrowserStorage;
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+    if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+      continue;
+    }
+
+    //
+    // Skip if there is no RequestElement
+    //
+    if (FormSetStorage->ElementCount == 0) {
+      continue;
+    }
+
+    //
+    // 1. Prepare <ConfigResp>
+    //
+    Status = StorageToConfigResp (Storage, &ConfigResp, FormSetStorage->ConfigRequest, TRUE);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    //
+    // 2. Send <ConfigResp> to Routine config Protocol.
+    //
+    Status = mHiiConfigRouting->RouteConfig (
+                                      mHiiConfigRouting,
+                                      ConfigResp,
+                                      &Progress
+                                      );
+    if (EFI_ERROR (Status)) {
+      InsertTailList (&FormSet->SaveFailStorageListHead, &FormSetStorage->SaveFailLink);
+      if (!HasInserted) {
+        //
+        // Call submit formset for system level, save the formset info
+        // and process later.
+        //
+        FindQuestionFromProgress(FormSet, Storage, Progress, &Form, &Question);
+        ASSERT (Form != NULL && Question != NULL);
+        FormSet->SaveFailForm = Form;
+        FormSet->SaveFailStatement = Question;
+        if (SkipProcessFail) {
+          InsertTailList (&gBrowserSaveFailFormSetList, &FormSet->SaveFailLink);
+        }
+        HasInserted = TRUE;
+      }
+
+      FreePool (ConfigResp);
+      continue;
+    }
+
+    FreePool (ConfigResp);
+    //
+    // 3. Config success, update storage shadow Buffer
+    //
+    SynchronizeStorage (Storage, FormSetStorage->ConfigRequest, TRUE);
+  }
+
+  //
+  // 4. Has save fail storage need to handle.
+  //
+  if (Form != NULL) {
+    if (!SkipProcessFail) {
+      //
+      // If not in system level, just handl the save failed storage here.
+      //
+      if (ConfirmSaveFail (Form->FormTitle, FormSet->HiiHandle) == BROWSER_ACTION_DISCARD) {
+        Link = GetFirstNode (&FormSet->SaveFailStorageListHead);
+        while (!IsNull (&FormSet->SaveFailStorageListHead, Link)) {
+          FormSetStorage = FORMSET_STORAGE_FROM_SAVE_FAIL_LINK (Link);
+          Storage        = FormSetStorage->BrowserStorage;
+          Link = GetNextNode (&FormSet->SaveFailStorageListHead, Link);
+
+          SynchronizeStorage(FormSetStorage->BrowserStorage, FormSetStorage->ConfigRequest, FALSE);
+
+          Status = EFI_SUCCESS;
+        }
+      } else {
+        UiCopyMenuList(&mPrivateData.FormBrowserEx2.FormViewHistoryHead, &Form->FormViewListHead);
+
+        gCurrentSelection->Action = UI_ACTION_REFRESH_FORMSET;
+        gCurrentSelection->Handle = FormSet->HiiHandle;
+        CopyGuid (&gCurrentSelection->FormSetGuid, &FormSet->Guid);
+        gCurrentSelection->FormId = Form->FormId;
+        gCurrentSelection->QuestionId = Question->QuestionId;
+
+        Status = EFI_UNSUPPORTED;
+      }
+
+      //
+      // Free FormSet save fail list.
+      //
+      while (!IsListEmpty (&FormSet->SaveFailStorageListHead)) {
+        Link = GetFirstNode (&FormSet->SaveFailStorageListHead);
+        FormSetStorage = FORMSET_STORAGE_FROM_SAVE_FAIL_LINK (Link);
+        RemoveEntryList (&FormSetStorage->SaveFailLink);
+      }
+    } else {
+      //
+      // If in system level, just return error and handle the failed formset later.
+      //
+      Status = EFI_UNSUPPORTED;
+    }
+  }
+
+  //
+  // 5. Update the NV flag.
+  // 
+  ValueChangeResetFlagUpdate(TRUE, FormSet, NULL);
+
+  return Status;
+}
+
+/**
+  Submit data for all formsets.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+SubmitForSystem (
+  VOID
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  LIST_ENTRY              *StorageLink;
+  FORMSET_STORAGE         *FormSetStorage;
+  FORM_BROWSER_FORM       *Form;
+  FORM_BROWSER_FORMSET    *LocalFormSet;
+  UINT32                  UserSelection;
+  FORM_BROWSER_STATEMENT  *Question;
+
+  mSystemSubmit = TRUE;
+  Link = GetFirstNode (&gBrowserFormSetList);
+  while (!IsNull (&gBrowserFormSetList, Link)) {
+    LocalFormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+    Link = GetNextNode (&gBrowserFormSetList, Link);
+    if (!ValidateFormSet(LocalFormSet)) {
+      continue;
+    }
+
+    Status = SubmitForFormSet (LocalFormSet, TRUE);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Remove maintain backup list after save except for the current using FormSet.
+    //  
+    if (!IsHiiHandleInBrowserContext (LocalFormSet->HiiHandle)) {
+      CleanBrowserStorage(LocalFormSet);
+      RemoveEntryList (&LocalFormSet->Link);
+      DestroyFormSet (LocalFormSet);
+    }
+  }
+  mSystemSubmit = FALSE;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Process the save failed formsets.
+  //
+  Link = GetFirstNode (&gBrowserSaveFailFormSetList);
+  while (!IsNull (&gBrowserSaveFailFormSetList, Link)) {
+    LocalFormSet = FORM_BROWSER_FORMSET_FROM_SAVE_FAIL_LINK (Link);
+    Link = GetNextNode (&gBrowserSaveFailFormSetList, Link);
+
+    if (!ValidateFormSet(LocalFormSet)) {
+      continue;
+    }
+
+    Form = LocalFormSet->SaveFailForm;
+    Question= LocalFormSet->SaveFailStatement;
+
+    //
+    // Confirm with user, get user input.
+    //
+    if (IsListEmpty (&LocalFormSet->SaveFailStorageListHead)) {
+      //
+      // NULL for SaveFailStorageListHead means error caused by NO_SUBMIT_IF check.
+      //
+      UserSelection = ConfirmNoSubmitFail (Form->FormTitle, LocalFormSet->HiiHandle);
+    } else {
+      UserSelection = ConfirmSaveFail (Form->FormTitle, LocalFormSet->HiiHandle);
+    }
+
+    if (UserSelection == BROWSER_ACTION_DISCARD) {
+      if (IsListEmpty (&LocalFormSet->SaveFailStorageListHead)) {
+        StorageLink = GetFirstNode (&LocalFormSet->StorageListHead);
+        while (!IsNull (&LocalFormSet->StorageListHead, StorageLink)) {
+          FormSetStorage = FORMSET_STORAGE_FROM_LINK (StorageLink);
+          StorageLink = GetNextNode (&LocalFormSet->StorageListHead, StorageLink);
+
+          SynchronizeStorage(FormSetStorage->BrowserStorage, FormSetStorage->ConfigRequest, FALSE);
+        }
+      } else {
+        StorageLink = GetFirstNode (&LocalFormSet->SaveFailStorageListHead);
+        while (!IsNull (&LocalFormSet->SaveFailStorageListHead, StorageLink)) {
+          FormSetStorage = FORMSET_STORAGE_FROM_SAVE_FAIL_LINK (StorageLink);
+          StorageLink = GetNextNode (&LocalFormSet->SaveFailStorageListHead, StorageLink);
+
+          SynchronizeStorage(FormSetStorage->BrowserStorage, FormSetStorage->ConfigRequest, FALSE);
+        }
+      }
+
+      if (!IsHiiHandleInBrowserContext (LocalFormSet->HiiHandle)) {
+        CleanBrowserStorage(LocalFormSet);
+        RemoveEntryList (&LocalFormSet->Link);
+        RemoveEntryList (&LocalFormSet->SaveFailLink);
+        DestroyFormSet (LocalFormSet);
+      } else {
+        ValueChangeResetFlagUpdate(FALSE, LocalFormSet, NULL);
+      }
+    } else {
+      if (IsListEmpty (&LocalFormSet->SaveFailStorageListHead)) {
+        NoSubmitCheck (LocalFormSet, &Form, &Question);
+      }
+
+      UiCopyMenuList(&mPrivateData.FormBrowserEx2.FormViewHistoryHead, &Form->FormViewListHead);
+
+      gCurrentSelection->Action = UI_ACTION_REFRESH_FORMSET;
+      gCurrentSelection->Handle = LocalFormSet->HiiHandle;
+      CopyGuid (&gCurrentSelection->FormSetGuid, &LocalFormSet->Guid);
+      gCurrentSelection->FormId = Form->FormId;
+      gCurrentSelection->QuestionId = Question->QuestionId;
+
+      Status = EFI_UNSUPPORTED;
+      break;
+    }
+  }
+
+  //
+  // Clean the list which will not process.
+  //
+  while (!IsListEmpty (&gBrowserSaveFailFormSetList)) {
+    Link = GetFirstNode (&gBrowserSaveFailFormSetList);
+    LocalFormSet = FORM_BROWSER_FORMSET_FROM_SAVE_FAIL_LINK (Link);
+    RemoveEntryList (&LocalFormSet->SaveFailLink);
+
+    while (!IsListEmpty (&LocalFormSet->SaveFailStorageListHead)) {
+      StorageLink = GetFirstNode (&LocalFormSet->SaveFailStorageListHead);
+      FormSetStorage = FORMSET_STORAGE_FROM_SAVE_FAIL_LINK (StorageLink);
+      RemoveEntryList (&FormSetStorage->SaveFailLink);
+    }
+  }
+
+  return Status;
+}
+
+/**
+  Submit data based on the input Setting level (Form, FormSet or System).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Submit action.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+SubmitForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  )
+{
+  EFI_STATUS              Status;
+
+  switch (SettingScope) {
+  case FormLevel:
+    Status = SubmitForForm(FormSet, Form);
+    break;
+
+  case FormSetLevel:
+    Status = SubmitForFormSet (FormSet, FALSE);
+    break;
+
+  case SystemLevel:
+    Status = SubmitForSystem ();
+    break;
+
+  default:
+    Status = EFI_UNSUPPORTED;
+    break;
+  }
+
+  return Status;
+}
+
+/**
+  Converts the unicode character of the string from uppercase to lowercase.
+  This is a internal function.
+
+  @param ConfigString  String to be converted
+
+**/
+VOID
+EFIAPI
+HiiToLower (
+  IN EFI_STRING  ConfigString
+  )
+{
+  EFI_STRING  String;
+  BOOLEAN     Lower;
+
+  ASSERT (ConfigString != NULL);
+
+  //
+  // Convert all hex digits in range [A-F] in the configuration header to [a-f]
+  //
+  for (String = ConfigString, Lower = FALSE; *String != L'\0'; String++) {
+    if (*String == L'=') {
+      Lower = TRUE;
+    } else if (*String == L'&') {
+      Lower = FALSE;
+    } else if (Lower && *String >= L'A' && *String <= L'F') {
+      *String = (CHAR16) (*String - L'A' + L'a');
+    }
+  }
+}
+
+/**
+  Find the point in the ConfigResp string for this question.
+
+  @param  Question               The question.
+  @param  ConfigResp             Get ConfigResp string.
+
+  @retval  point to the offset where is for this question.
+
+**/
+CHAR16 *
+GetOffsetFromConfigResp (
+  IN FORM_BROWSER_STATEMENT           *Question,
+  IN CHAR16                           *ConfigResp
+  )
+{
+  CHAR16                       *RequestElement;
+  CHAR16                       *BlockData;
+
+  //
+  // Type is EFI_HII_VARSTORE_NAME_VALUE.
+  //
+  if (Question->Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    RequestElement = StrStr (ConfigResp, Question->VariableName);
+    if (RequestElement != NULL) {
+      //
+      // Skip the "VariableName=" field.
+      //
+      RequestElement += StrLen (Question->VariableName) + 1;
+    }
+
+    return RequestElement;
+  }
+
+  //
+  // Type is EFI_HII_VARSTORE_EFI_VARIABLE or EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER
+  //
+
+  //
+  // 1. Directly use Question->BlockName to find.
+  //
+  RequestElement = StrStr (ConfigResp, Question->BlockName);
+  if (RequestElement != NULL) {
+    //
+    // Skip the "Question->BlockName&VALUE=" field.
+    //
+    RequestElement += StrLen (Question->BlockName) + StrLen (L"&VALUE=");
+    return RequestElement;
+  }
+  
+  //
+  // 2. Change all hex digits in Question->BlockName to lower and compare again.
+  //
+  BlockData = AllocateCopyPool (StrSize(Question->BlockName), Question->BlockName);
+  ASSERT (BlockData != NULL);
+  HiiToLower (BlockData);
+  RequestElement = StrStr (ConfigResp, BlockData);
+  FreePool (BlockData);
+
+  if (RequestElement != NULL) {
+    //
+    // Skip the "Question->BlockName&VALUE=" field.
+    //
+    RequestElement += StrLen (Question->BlockName) + StrLen (L"&VALUE=");
+  }
+
+  return RequestElement;
+}
+
+/**
+  Get Question default value from AltCfg string.
+
+  @param  FormSet                The form set.
+  @param  Form                   The form
+  @param  Question               The question.
+
+  @retval EFI_SUCCESS            Question is reset to default value.
+
+**/
+EFI_STATUS
+GetDefaultValueFromAltCfg (
+  IN     FORM_BROWSER_FORMSET             *FormSet,
+  IN     FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT           *Question
+  )
+{ 
+  BROWSER_STORAGE              *Storage;
+  FORMSET_STORAGE              *FormSetStorage;
+  CHAR16                       *ConfigResp;
+  CHAR16                       *Value;
+  LIST_ENTRY                   *Link;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+
+  Storage = Question->Storage;
+  if ((Storage == NULL) || (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Try to get AltCfg string from form. If not found it, then
+  // try to get it from formset.
+  //
+  ConfigResp    = NULL;
+  Link = GetFirstNode (&Form->ConfigRequestHead);
+  while (!IsNull (&Form->ConfigRequestHead, Link)) {
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+    Link = GetNextNode (&Form->ConfigRequestHead, Link);
+
+    if (Storage == ConfigInfo->Storage) {
+      ConfigResp = ConfigInfo->ConfigAltResp;
+      break;
+    }
+  }
+
+  if (ConfigResp == NULL) {
+    Link = GetFirstNode (&FormSet->StorageListHead);
+    while (!IsNull (&FormSet->StorageListHead, Link)) {
+      FormSetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+      Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+      if (Storage == FormSetStorage->BrowserStorage) {
+        ConfigResp = FormSetStorage->ConfigAltResp;
+        break;
+      }
+    }
+  }
+
+  if (ConfigResp == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  Value = GetOffsetFromConfigResp (Question, ConfigResp);
+  if (Value == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  return BufferToValue (Question, Value);
+}
+
+/**
+  Get default Id value used for browser.
+
+  @param  DefaultId              The default id value used by hii.
+
+  @retval Browser used default value.
+
+**/
+INTN
+GetDefaultIdForCallBack (
+  UINTN DefaultId
+  )
+{ 
+  if (DefaultId == EFI_HII_DEFAULT_CLASS_STANDARD) {
+    return EFI_BROWSER_ACTION_DEFAULT_STANDARD;
+  } else if (DefaultId == EFI_HII_DEFAULT_CLASS_MANUFACTURING) {
+    return EFI_BROWSER_ACTION_DEFAULT_MANUFACTURING;
+  } else if (DefaultId == EFI_HII_DEFAULT_CLASS_SAFE) {
+    return EFI_BROWSER_ACTION_DEFAULT_SAFE;
+  } else if (DefaultId >= EFI_HII_DEFAULT_CLASS_PLATFORM_BEGIN && DefaultId < EFI_HII_DEFAULT_CLASS_PLATFORM_BEGIN + 0x1000) {
+    return EFI_BROWSER_ACTION_DEFAULT_PLATFORM + DefaultId - EFI_HII_DEFAULT_CLASS_PLATFORM_BEGIN;
+  } else if (DefaultId >= EFI_HII_DEFAULT_CLASS_HARDWARE_BEGIN && DefaultId < EFI_HII_DEFAULT_CLASS_HARDWARE_BEGIN + 0x1000) {
+    return EFI_BROWSER_ACTION_DEFAULT_HARDWARE + DefaultId - EFI_HII_DEFAULT_CLASS_HARDWARE_BEGIN;
+  } else if (DefaultId >= EFI_HII_DEFAULT_CLASS_FIRMWARE_BEGIN && DefaultId < EFI_HII_DEFAULT_CLASS_FIRMWARE_BEGIN + 0x1000) {
+    return EFI_BROWSER_ACTION_DEFAULT_FIRMWARE + DefaultId - EFI_HII_DEFAULT_CLASS_FIRMWARE_BEGIN;
+  } else {
+    return -1;
+  }
+}
+
+
+
+/**
+  Return data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+
+  @retval Value                  The data to be returned
+
+**/
+UINT64
+GetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index
+  )
+{
+  UINT64 Data;
+
+  ASSERT (Array != NULL);
+
+  Data = 0;
+  switch (Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    Data = (UINT64) *(((UINT8 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    Data = (UINT64) *(((UINT16 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    Data = (UINT64) *(((UINT32 *) Array) + Index);
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    Data = (UINT64) *(((UINT64 *) Array) + Index);
+    break;
+
+  default:
+    break;
+  }
+
+  return Data;
+}
+
+
+/**
+  Set value of a data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+  @param  Value                  The value to be set.
+
+**/
+VOID
+SetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index,
+  IN UINT64                   Value
+  )
+{
+
+  ASSERT (Array != NULL);
+
+  switch (Type) {
+  case EFI_IFR_TYPE_NUM_SIZE_8:
+    *(((UINT8 *) Array) + Index) = (UINT8) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_16:
+    *(((UINT16 *) Array) + Index) = (UINT16) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_32:
+    *(((UINT32 *) Array) + Index) = (UINT32) Value;
+    break;
+
+  case EFI_IFR_TYPE_NUM_SIZE_64:
+    *(((UINT64 *) Array) + Index) = (UINT64) Value;
+    break;
+
+  default:
+    break;
+  }
+}
+
+/**
+  Search an Option of a Question by its value.
+
+  @param  Question               The Question
+  @param  OptionValue            Value for Option to be searched.
+
+  @retval Pointer                Pointer to the found Option.
+  @retval NULL                   Option not found.
+
+**/
+QUESTION_OPTION *
+ValueToOption (
+  IN FORM_BROWSER_STATEMENT   *Question,
+  IN EFI_HII_VALUE            *OptionValue
+  )
+{
+  LIST_ENTRY       *Link;
+  QUESTION_OPTION  *Option;
+  INTN             Result;
+
+  Link = GetFirstNode (&Question->OptionListHead);
+  while (!IsNull (&Question->OptionListHead, Link)) {
+    Option = QUESTION_OPTION_FROM_LINK (Link);
+
+    if ((CompareHiiValue (&Option->Value, OptionValue, &Result, NULL) == EFI_SUCCESS) && (Result == 0)) {
+      //
+      // Check the suppressif condition, only a valid option can be return.
+      //
+      if ((Option->SuppressExpression == NULL) ||
+          ((EvaluateExpressionList(Option->SuppressExpression, FALSE, NULL, NULL) == ExpressFalse))) {
+        return Option;
+      }
+    }
+
+    Link = GetNextNode (&Question->OptionListHead, Link);
+  }
+
+  return NULL;
+}
+
+
+/**
+  Reset Question to its default value.
+
+  @param  FormSet                The form set.
+  @param  Form                   The form.
+  @param  Question               The question.
+  @param  DefaultId              The Class of the default.
+
+  @retval EFI_SUCCESS            Question is reset to default value.
+
+**/
+EFI_STATUS
+GetQuestionDefault (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN FORM_BROWSER_STATEMENT           *Question,
+  IN UINT16                           DefaultId
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  QUESTION_DEFAULT        *Default;
+  QUESTION_OPTION         *Option;
+  EFI_HII_VALUE           *HiiValue;
+  UINT8                   Index;
+  EFI_STRING              StrValue;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+  EFI_BROWSER_ACTION_REQUEST      ActionRequest;
+  INTN                            Action;
+  CHAR16                          *NewString;
+
+  Status   = EFI_NOT_FOUND;
+  StrValue = NULL;
+
+  //
+  // Statement don't have storage, skip them
+  //
+  if (Question->QuestionId == 0) {
+    return Status;
+  }
+
+  //
+  // There are Five ways to specify default value for a Question:
+  //  1, use call back function (highest priority)
+  //  2, use ExtractConfig function
+  //  3, use nested EFI_IFR_DEFAULT 
+  //  4, set flags of EFI_ONE_OF_OPTION (provide Standard and Manufacturing default)
+  //  5, set flags of EFI_IFR_CHECKBOX (provide Standard and Manufacturing default) (lowest priority)
+  //
+  HiiValue = &Question->HiiValue;
+
+  //
+  // Get Question defaut value from call back function.
+  //
+  ConfigAccess = FormSet->ConfigAccess;
+  Action = GetDefaultIdForCallBack (DefaultId);
+  if ((Action > 0) && ((Question->QuestionFlags & EFI_IFR_FLAG_CALLBACK) != 0) && (ConfigAccess != NULL)) {
+    ActionRequest = EFI_BROWSER_ACTION_REQUEST_NONE;
+    Status = ConfigAccess->Callback (
+                             ConfigAccess,
+                             Action,
+                             Question->QuestionId,
+                             HiiValue->Type,
+                             &HiiValue->Value,
+                             &ActionRequest
+                             );
+    if (!EFI_ERROR (Status)) {
+      if (HiiValue->Type == EFI_IFR_TYPE_STRING) {
+        NewString = GetToken (Question->HiiValue.Value.string, FormSet->HiiHandle);
+        ASSERT (NewString != NULL);
+
+        ASSERT (StrLen (NewString) * sizeof (CHAR16) <= Question->StorageWidth);
+        if (StrLen (NewString) * sizeof (CHAR16) <= Question->StorageWidth) {
+          CopyMem (Question->BufferValue, NewString, StrSize (NewString));
+        } else {
+          CopyMem (Question->BufferValue, NewString, Question->StorageWidth);
+        }
+
+        FreePool (NewString);
+      }
+      return Status;
+    }
+  }
+
+  //
+  // Get default value from altcfg string.
+  //
+  if (ConfigAccess != NULL) {  
+    Status = GetDefaultValueFromAltCfg(FormSet, Form, Question);
+    if (!EFI_ERROR (Status)) {
+        return Status;
+    }
+  }
+
+  //
+  // EFI_IFR_DEFAULT has highest priority
+  //
+  if (!IsListEmpty (&Question->DefaultListHead)) {
+    Link = GetFirstNode (&Question->DefaultListHead);
+    while (!IsNull (&Question->DefaultListHead, Link)) {
+      Default = QUESTION_DEFAULT_FROM_LINK (Link);
+
+      if (Default->DefaultId == DefaultId) {
+        if (Default->ValueExpression != NULL) {
+          //
+          // Default is provided by an Expression, evaluate it
+          //
+          Status = EvaluateExpression (FormSet, Form, Default->ValueExpression);
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          if (Default->ValueExpression->Result.Type == EFI_IFR_TYPE_BUFFER) {
+            ASSERT (HiiValue->Type == EFI_IFR_TYPE_BUFFER && Question->BufferValue != NULL);
+            if (Question->StorageWidth > Default->ValueExpression->Result.BufferLen) {
+              CopyMem (Question->HiiValue.Buffer, Default->ValueExpression->Result.Buffer, Default->ValueExpression->Result.BufferLen);
+              Question->HiiValue.BufferLen = Default->ValueExpression->Result.BufferLen;
+            } else {
+              CopyMem (Question->HiiValue.Buffer, Default->ValueExpression->Result.Buffer, Question->StorageWidth);
+              Question->HiiValue.BufferLen = Question->StorageWidth;
+            }
+            FreePool (Default->ValueExpression->Result.Buffer);
+          }
+          HiiValue->Type = Default->ValueExpression->Result.Type;
+          CopyMem (&HiiValue->Value, &Default->ValueExpression->Result.Value, sizeof (EFI_IFR_TYPE_VALUE));       
+        } else {
+          //
+          // Default value is embedded in EFI_IFR_DEFAULT
+          //
+          if (Default->Value.Type == EFI_IFR_TYPE_BUFFER) {
+            ASSERT (HiiValue->Buffer != NULL);
+            CopyMem (HiiValue->Buffer, Default->Value.Buffer, Default->Value.BufferLen);
+          } else {
+            CopyMem (HiiValue, &Default->Value, sizeof (EFI_HII_VALUE));
+          }
+        }
+
+        if (HiiValue->Type == EFI_IFR_TYPE_STRING) {
+          StrValue = HiiGetString (FormSet->HiiHandle, HiiValue->Value.string, NULL);
+          if (StrValue == NULL) {
+            return EFI_NOT_FOUND;
+          }
+          if (Question->StorageWidth > StrSize (StrValue)) {
+            CopyMem (Question->BufferValue, StrValue, StrSize (StrValue));
+          } else {
+            CopyMem (Question->BufferValue, StrValue, Question->StorageWidth);
+          }
+        }
+
+        return EFI_SUCCESS;
+      }
+
+      Link = GetNextNode (&Question->DefaultListHead, Link);
+    }
+  }
+
+  //
+  // EFI_ONE_OF_OPTION
+  //
+  if ((Question->Operand == EFI_IFR_ONE_OF_OP) && !IsListEmpty (&Question->OptionListHead)) {
+    if (DefaultId <= EFI_HII_DEFAULT_CLASS_MANUFACTURING)  {
+      //
+      // OneOfOption could only provide Standard and Manufacturing default
+      //
+      Link = GetFirstNode (&Question->OptionListHead);
+      while (!IsNull (&Question->OptionListHead, Link)) {
+        Option = QUESTION_OPTION_FROM_LINK (Link);
+        Link = GetNextNode (&Question->OptionListHead, Link);
+
+        if ((Option->SuppressExpression != NULL) &&
+            EvaluateExpressionList(Option->SuppressExpression, FALSE, NULL, NULL) != ExpressFalse) {
+          continue;
+        }
+
+        if (((DefaultId == EFI_HII_DEFAULT_CLASS_STANDARD) && ((Option->Flags & EFI_IFR_OPTION_DEFAULT) != 0)) ||
+            ((DefaultId == EFI_HII_DEFAULT_CLASS_MANUFACTURING) && ((Option->Flags & EFI_IFR_OPTION_DEFAULT_MFG) != 0))
+           ) {
+          CopyMem (HiiValue, &Option->Value, sizeof (EFI_HII_VALUE));
+
+          return EFI_SUCCESS;
+        }
+      }
+    }
+  }
+
+  //
+  // EFI_IFR_CHECKBOX - lowest priority
+  //
+  if (Question->Operand == EFI_IFR_CHECKBOX_OP) {
+    if (DefaultId <= EFI_HII_DEFAULT_CLASS_MANUFACTURING)  {
+      //
+      // Checkbox could only provide Standard and Manufacturing default
+      //
+      if (((DefaultId == EFI_HII_DEFAULT_CLASS_STANDARD) && ((Question->Flags & EFI_IFR_CHECKBOX_DEFAULT) != 0)) ||
+          ((DefaultId == EFI_HII_DEFAULT_CLASS_MANUFACTURING) && ((Question->Flags & EFI_IFR_CHECKBOX_DEFAULT_MFG) != 0))
+         ) {
+        HiiValue->Value.b = TRUE;
+      } else {
+        HiiValue->Value.b = FALSE;
+      }
+
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // For Questions without default
+  //
+  Status = EFI_NOT_FOUND;
+  switch (Question->Operand) {
+  case EFI_IFR_NUMERIC_OP:
+    //
+    // Take minimum value as numeric default value
+    //
+    if ((Question->Flags & EFI_IFR_DISPLAY) == 0) {
+      //
+      // In EFI_IFR_DISPLAY_INT_DEC type, should check value with int* type.
+      //
+      switch (Question->Flags & EFI_IFR_NUMERIC_SIZE) {
+      case EFI_IFR_NUMERIC_SIZE_1:
+        if (((INT8) HiiValue->Value.u8 < (INT8) Question->Minimum) || ((INT8) HiiValue->Value.u8 > (INT8) Question->Maximum)) {
+          HiiValue->Value.u8 = (UINT8) Question->Minimum;
+          Status = EFI_SUCCESS;
+        }
+        break;
+      case EFI_IFR_NUMERIC_SIZE_2:
+        if (((INT16) HiiValue->Value.u16 < (INT16) Question->Minimum) || ((INT16) HiiValue->Value.u16 > (INT16) Question->Maximum)) {
+          HiiValue->Value.u16 = (UINT16) Question->Minimum;
+          Status = EFI_SUCCESS;
+        }
+        break;
+      case EFI_IFR_NUMERIC_SIZE_4:
+        if (((INT32) HiiValue->Value.u32 < (INT32) Question->Minimum) || ((INT32) HiiValue->Value.u32 > (INT32) Question->Maximum)) {
+          HiiValue->Value.u32 = (UINT32) Question->Minimum;
+          Status = EFI_SUCCESS;
+        }
+        break;
+      case EFI_IFR_NUMERIC_SIZE_8:
+        if (((INT64) HiiValue->Value.u64 < (INT64) Question->Minimum) || ((INT64) HiiValue->Value.u64 > (INT64) Question->Maximum)) {
+          HiiValue->Value.u64 = Question->Minimum;
+          Status = EFI_SUCCESS;
+        }
+        break;
+      default:
+        break;     
+      }
+    } else {
+      if ((HiiValue->Value.u64 < Question->Minimum) || (HiiValue->Value.u64 > Question->Maximum)) {
+        HiiValue->Value.u64 = Question->Minimum;
+        Status = EFI_SUCCESS;
+      }
+    }
+    break;
+
+  case EFI_IFR_ONE_OF_OP:
+    //
+    // Take first oneof option as oneof's default value
+    //
+    if (ValueToOption (Question, HiiValue) == NULL) {
+      Link = GetFirstNode (&Question->OptionListHead);
+      while (!IsNull (&Question->OptionListHead, Link)) {
+        Option = QUESTION_OPTION_FROM_LINK (Link);
+        Link = GetNextNode (&Question->OptionListHead, Link);
+
+        if ((Option->SuppressExpression != NULL) &&
+            EvaluateExpressionList(Option->SuppressExpression, FALSE, NULL, NULL) != ExpressFalse) {
+          continue;
+        }
+
+        CopyMem (HiiValue, &Option->Value, sizeof (EFI_HII_VALUE));
+        Status = EFI_SUCCESS;
+        break;
+      }
+    }
+    break;
+
+  case EFI_IFR_ORDERED_LIST_OP:
+    //
+    // Take option sequence in IFR as ordered list's default value
+    //
+    Index = 0;
+    Link = GetFirstNode (&Question->OptionListHead);
+    while (!IsNull (&Question->OptionListHead, Link)) {
+      Status = EFI_SUCCESS;
+      Option = QUESTION_OPTION_FROM_LINK (Link);
+      Link = GetNextNode (&Question->OptionListHead, Link);
+
+      if ((Option->SuppressExpression != NULL) &&
+          EvaluateExpressionList(Option->SuppressExpression, FALSE, NULL, NULL) != ExpressFalse) {
+        continue;
+      }
+
+      SetArrayData (Question->BufferValue, Question->ValueType, Index, Option->Value.Value.u64);
+
+      Index++;
+      if (Index >= Question->MaxContainers) {
+        break;
+      }
+    }
+    break;
+
+  default:
+    break;
+  }
+
+  return Status;
+}
+
+/**
+  Get AltCfg string for current form.
+
+  @param  FormSet                Form data structure.
+  @param  Form                   Form data structure.
+  @param  DefaultId              The Class of the default.
+  @param  BrowserStorage         The input request storage for the questions.
+
+**/
+VOID
+ExtractAltCfgForForm (
+  IN FORM_BROWSER_FORMSET   *FormSet,
+  IN FORM_BROWSER_FORM      *Form,
+  IN UINT16                 DefaultId,
+  IN BROWSER_STORAGE        *BrowserStorage
+  )
+{
+  EFI_STATUS                   Status;
+  LIST_ENTRY                   *Link;
+  CHAR16                       *ConfigResp;
+  CHAR16                       *Progress;
+  CHAR16                       *Result;
+  BROWSER_STORAGE              *Storage;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+  FORMSET_STORAGE              *FormSetStorage;
+
+  //
+  // Check whether has get AltCfg string for this formset.
+  // If yes, no need to get AltCfg for form.
+  //
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    FormSetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+    Storage        = FormSetStorage->BrowserStorage;
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+    if (BrowserStorage != NULL && BrowserStorage != Storage) {
+      continue;
+    }
+
+    if (Storage->Type != EFI_HII_VARSTORE_EFI_VARIABLE &&
+        FormSetStorage->ElementCount != 0 &&
+        FormSetStorage->HasCallAltCfg) {
+      return;
+    }
+  }
+
+  //
+  // Get AltCfg string for each form.
+  //
+  Link = GetFirstNode (&Form->ConfigRequestHead);
+  while (!IsNull (&Form->ConfigRequestHead, Link)) {
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+    Link = GetNextNode (&Form->ConfigRequestHead, Link);
+
+    Storage = ConfigInfo->Storage;
+    if (BrowserStorage != NULL && BrowserStorage != Storage) {
+      continue;
+    }
+
+    if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+      continue;
+    }
+
+    //
+    // 1. Skip if there is no RequestElement
+    //
+    if (ConfigInfo->ElementCount == 0) {
+      continue;
+    }
+
+    //
+    // 2. Get value through hii config routine protocol.
+    //
+    Status = mHiiConfigRouting->ExtractConfig (
+                                      mHiiConfigRouting,
+                                      ConfigInfo->ConfigRequest,
+                                      &Progress,
+                                      &Result
+                                      );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // 3. Call ConfigRouting GetAltCfg(ConfigRoute, <ConfigResponse>, Guid, Name, DevicePath, AltCfgId, AltCfgResp)
+    //    Get the default configuration string according to the default ID.
+    //
+    Status = mHiiConfigRouting->GetAltConfig (
+                                  mHiiConfigRouting,
+                                  Result,
+                                  &Storage->Guid,
+                                  Storage->Name,
+                                  NULL,
+                                  &DefaultId,  // it can be NULL to get the current setting.
+                                  &ConfigResp
+                                );
+    FreePool (Result);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    ConfigInfo->ConfigAltResp = ConfigResp;
+  }
+}
+
+/**
+  Clean AltCfg string for current form.
+
+  @param  Form                   Form data structure.
+
+**/
+VOID
+CleanAltCfgForForm (
+  IN FORM_BROWSER_FORM   *Form
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_CONFIG_REQUEST  *ConfigInfo;
+
+  Link = GetFirstNode (&Form->ConfigRequestHead);
+  while (!IsNull (&Form->ConfigRequestHead, Link)) {
+    ConfigInfo = FORM_BROWSER_CONFIG_REQUEST_FROM_LINK (Link);
+    Link = GetNextNode (&Form->ConfigRequestHead, Link);
+
+    if (ConfigInfo->ConfigAltResp != NULL) {
+      FreePool (ConfigInfo->ConfigAltResp);
+      ConfigInfo->ConfigAltResp = NULL;
+    }
+  }
+}
+
+/**
+  Get AltCfg string for current formset.
+
+  @param  FormSet                Form data structure.
+  @param  DefaultId              The Class of the default.
+  @param  BrowserStorage         The input request storage for the questions.
+
+**/
+VOID
+ExtractAltCfgForFormSet (
+  IN FORM_BROWSER_FORMSET   *FormSet,
+  IN UINT16                 DefaultId,
+  IN BROWSER_STORAGE        *BrowserStorage
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *Link;
+  CHAR16                  *ConfigResp;
+  CHAR16                  *Progress;
+  CHAR16                  *Result;
+  BROWSER_STORAGE         *Storage;
+  FORMSET_STORAGE         *FormSetStorage;
+
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    FormSetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+    Storage        = FormSetStorage->BrowserStorage;
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+    if (BrowserStorage != NULL && BrowserStorage != Storage) {
+      continue;
+    }
+
+    if (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE) {
+      continue;
+    }
+
+    //
+    // 1. Skip if there is no RequestElement
+    //
+    if (FormSetStorage->ElementCount == 0) {
+      continue;
+    }
+
+    FormSetStorage->HasCallAltCfg = TRUE;
+
+    //
+    // 2. Get value through hii config routine protocol.
+    //
+    Status = mHiiConfigRouting->ExtractConfig (
+                                      mHiiConfigRouting,
+                                      FormSetStorage->ConfigRequest,
+                                      &Progress,
+                                      &Result
+                                      );
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // 3. Call ConfigRouting GetAltCfg(ConfigRoute, <ConfigResponse>, Guid, Name, DevicePath, AltCfgId, AltCfgResp)
+    //    Get the default configuration string according to the default ID.
+    //
+    Status = mHiiConfigRouting->GetAltConfig (
+                                  mHiiConfigRouting,
+                                  Result,
+                                  &Storage->Guid,
+                                  Storage->Name,
+                                  NULL,
+                                  &DefaultId,  // it can be NULL to get the current setting.
+                                  &ConfigResp
+                                );
+
+    FreePool (Result);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    FormSetStorage->ConfigAltResp = ConfigResp;
+  }
+
+}
+
+/**
+  Clean AltCfg string for current formset.
+
+  @param  FormSet                Form data structure.
+
+**/
+VOID
+CleanAltCfgForFormSet (
+  IN FORM_BROWSER_FORMSET   *FormSet
+  )
+{
+  LIST_ENTRY              *Link;
+  FORMSET_STORAGE         *FormSetStorage;
+
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    FormSetStorage = FORMSET_STORAGE_FROM_LINK (Link);
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+    if (FormSetStorage->ConfigAltResp != NULL) {
+      FreePool (FormSetStorage->ConfigAltResp);
+      FormSetStorage->ConfigAltResp = NULL;
+    }
+
+    FormSetStorage->HasCallAltCfg = FALSE;
+  }
+}
+
+/**
+  Reset Questions to their initial value or default value in a Form, Formset or System.
+
+  GetDefaultValueScope parameter decides which questions will reset 
+  to its default value.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  DefaultId              The Class of the default.
+  @param  SettingScope           Setting Scope for Default action.
+  @param  GetDefaultValueScope   Get default value scope.
+  @param  Storage                Get default value only for this storage.
+  @param  RetrieveValueFirst     Whether call the retrieve call back to
+                                 get the initial value before get default
+                                 value.
+  @param  SkipGetAltCfg          Whether skip the get altcfg string process.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+ExtractDefault (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN UINT16                           DefaultId,
+  IN BROWSER_SETTING_SCOPE            SettingScope,
+  IN BROWSER_GET_DEFAULT_VALUE        GetDefaultValueScope,
+  IN BROWSER_STORAGE                  *Storage OPTIONAL,
+  IN BOOLEAN                          RetrieveValueFirst,
+  IN BOOLEAN                          SkipGetAltCfg
+  )
+{
+  EFI_STATUS              Status;
+  LIST_ENTRY              *FormLink;
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_STATEMENT  *Question;
+  FORM_BROWSER_FORMSET    *LocalFormSet;
+  FORM_BROWSER_FORMSET    *OldFormSet;
+
+  Status = EFI_SUCCESS;
+
+  //
+  // Check the supported setting level.
+  //
+  if (SettingScope >= MaxLevel || GetDefaultValueScope >= GetDefaultForMax) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (GetDefaultValueScope == GetDefaultForStorage && Storage == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (SettingScope == FormLevel) {
+    //
+    // Prepare the AltCfg String for form.
+    //
+    if (!SkipGetAltCfg && (GetDefaultValueScope != GetDefaultForNoStorage)) {
+      ExtractAltCfgForForm (FormSet, Form, DefaultId, Storage);
+    }
+
+    //
+    // Extract Form default
+    //
+    Link = GetFirstNode (&Form->StatementListHead);
+    while (!IsNull (&Form->StatementListHead, Link)) {
+      Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+      Link = GetNextNode (&Form->StatementListHead, Link);
+
+      //
+      // If get default value only for this storage, check the storage first.
+      //
+      if ((GetDefaultValueScope == GetDefaultForStorage) && (Question->Storage != Storage)) {
+        continue;
+      }
+
+      //
+      // If get default value only for no storage question, just skip the question which has storage.
+      //
+      if ((GetDefaultValueScope == GetDefaultForNoStorage) && (Question->Storage != NULL)) {
+        continue;
+      }
+
+      //
+      // If Question is disabled, don't reset it to default
+      //
+      if (Question->Expression != NULL) {
+        if (EvaluateExpressionList(Question->Expression, TRUE, FormSet, Form) == ExpressDisable) {
+          continue;
+        }
+      }
+
+      if (RetrieveValueFirst) {
+        //
+        // Call the Retrieve call back to get the initial question value.
+        //
+        Status = ProcessRetrieveForQuestion(FormSet->ConfigAccess, Question, FormSet);
+      }
+
+      //
+      // If not request to get the initial value or get initial value fail, then get default value.
+      //
+      if (!RetrieveValueFirst || EFI_ERROR (Status)) {
+        Status = GetQuestionDefault (FormSet, Form, Question, DefaultId);
+        if (EFI_ERROR (Status)) {
+          continue;
+        }
+      }
+
+      //
+      // Synchronize Buffer storage's Edit buffer
+      //
+      if ((Question->Storage != NULL) &&
+          (Question->Storage->Type != EFI_HII_VARSTORE_EFI_VARIABLE)) {
+        SetQuestionValue (FormSet, Form, Question, GetSetValueWithEditBuffer);
+      }
+    }
+
+    //
+    // Clean the AltCfg String.
+    //
+    if (!SkipGetAltCfg && (GetDefaultValueScope != GetDefaultForNoStorage)) {
+      CleanAltCfgForForm(Form);
+    }
+  } else if (SettingScope == FormSetLevel) {
+    //
+    // Prepare the AltCfg String for formset.
+    //
+    if (!SkipGetAltCfg && (GetDefaultValueScope != GetDefaultForNoStorage)) {
+      ExtractAltCfgForFormSet (FormSet, DefaultId, Storage);
+    }
+
+    FormLink = GetFirstNode (&FormSet->FormListHead);
+    while (!IsNull (&FormSet->FormListHead, FormLink)) {
+      Form = FORM_BROWSER_FORM_FROM_LINK (FormLink);
+      ExtractDefault (FormSet, Form, DefaultId, FormLevel, GetDefaultValueScope, Storage, RetrieveValueFirst, SkipGetAltCfg);
+      FormLink = GetNextNode (&FormSet->FormListHead, FormLink);
+    }
+
+    //
+    // Clean the AltCfg String.
+    //
+    if (!SkipGetAltCfg && (GetDefaultValueScope != GetDefaultForNoStorage)) {
+      CleanAltCfgForFormSet (FormSet);
+    }
+  } else if (SettingScope == SystemLevel) {
+    //
+    // Preload all Hii formset.
+    //
+    LoadAllHiiFormset();
+
+    OldFormSet = mSystemLevelFormSet;
+
+    //
+    // Set Default Value for each FormSet in the maintain list.
+    //
+    Link = GetFirstNode (&gBrowserFormSetList);
+    while (!IsNull (&gBrowserFormSetList, Link)) {
+      LocalFormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+      Link = GetNextNode (&gBrowserFormSetList, Link);
+      if (!ValidateFormSet(LocalFormSet)) {
+        continue;
+      }
+
+      mSystemLevelFormSet = LocalFormSet;
+
+      ExtractDefault (LocalFormSet, NULL, DefaultId, FormSetLevel, GetDefaultValueScope, Storage, RetrieveValueFirst, SkipGetAltCfg);
+    }
+
+    mSystemLevelFormSet = OldFormSet;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Validate whether this question's value has changed.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Question to be initialized.
+  @param  GetValueFrom           Where to get value, may from editbuffer, buffer or hii driver.
+
+  @retval TRUE                   Question's value has changed.
+  @retval FALSE                  Question's value has not changed
+
+**/
+BOOLEAN
+IsQuestionValueChanged (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      GetValueFrom
+  )
+{
+  EFI_HII_VALUE    BackUpValue;
+  CHAR8            *BackUpBuffer;
+  EFI_HII_VALUE    BackUpValue2;
+  CHAR8            *BackUpBuffer2;
+  EFI_STATUS       Status;
+  BOOLEAN          ValueChanged;
+  UINTN            BufferWidth;
+
+  //
+  // For quetion without storage, always mark it as data not changed.
+  //
+  if (Question->Storage == NULL && Question->Operand != EFI_IFR_TIME_OP && Question->Operand != EFI_IFR_DATE_OP) {
+    return FALSE;
+  }
+
+  BackUpBuffer = NULL;
+  BackUpBuffer2 = NULL;
+  ValueChanged = FALSE;
+
+  switch (Question->Operand) {
+    case EFI_IFR_ORDERED_LIST_OP:
+      BufferWidth  = Question->StorageWidth;
+      BackUpBuffer = AllocateCopyPool (BufferWidth, Question->BufferValue);
+      ASSERT (BackUpBuffer != NULL);
+      break;
+
+    case EFI_IFR_STRING_OP:
+    case EFI_IFR_PASSWORD_OP:
+      BufferWidth  = (UINTN) Question->Maximum * sizeof (CHAR16);
+      BackUpBuffer = AllocateCopyPool (BufferWidth, Question->BufferValue);
+      ASSERT (BackUpBuffer != NULL);
+      break;
+
+    default:
+      BufferWidth = 0;
+      break;
+  }
+  CopyMem (&BackUpValue, &Question->HiiValue, sizeof (EFI_HII_VALUE));
+
+  if (GetValueFrom == GetSetValueWithBothBuffer) {
+    Status = GetQuestionValue (FormSet, Form, Question, GetSetValueWithEditBuffer);
+    ASSERT_EFI_ERROR(Status);
+
+    switch (Question->Operand) {
+      case EFI_IFR_ORDERED_LIST_OP:
+        BufferWidth  = Question->StorageWidth;
+        BackUpBuffer2 = AllocateCopyPool (BufferWidth, Question->BufferValue);
+        ASSERT (BackUpBuffer2 != NULL);
+        break;
+
+      case EFI_IFR_STRING_OP:
+      case EFI_IFR_PASSWORD_OP:
+        BufferWidth  = (UINTN) Question->Maximum * sizeof (CHAR16);
+        BackUpBuffer2 = AllocateCopyPool (BufferWidth, Question->BufferValue);
+        ASSERT (BackUpBuffer2 != NULL);
+        break;
+
+      default:
+        BufferWidth = 0;
+        break;
+    }
+    CopyMem (&BackUpValue2, &Question->HiiValue, sizeof (EFI_HII_VALUE));
+
+    Status = GetQuestionValue (FormSet, Form, Question, GetSetValueWithBuffer);
+    ASSERT_EFI_ERROR(Status);
+
+    if (CompareMem (&BackUpValue2, &Question->HiiValue, sizeof (EFI_HII_VALUE)) != 0 ||
+        CompareMem (BackUpBuffer2, Question->BufferValue, BufferWidth) != 0) {
+      ValueChanged = TRUE;
+    }
+  } else {
+    Status = GetQuestionValue (FormSet, Form, Question, GetValueFrom);
+    ASSERT_EFI_ERROR(Status);
+
+    if (CompareMem (&BackUpValue, &Question->HiiValue, sizeof (EFI_HII_VALUE)) != 0 ||
+        CompareMem (BackUpBuffer, Question->BufferValue, BufferWidth) != 0) {
+      ValueChanged = TRUE;
+    }
+  }
+
+  CopyMem (&Question->HiiValue, &BackUpValue, sizeof (EFI_HII_VALUE));
+  if (BackUpBuffer != NULL) {
+    CopyMem (Question->BufferValue, BackUpBuffer, BufferWidth);
+    FreePool (BackUpBuffer);
+  }
+
+  if (BackUpBuffer2 != NULL) {
+    FreePool (BackUpBuffer2);
+  }
+
+  Question->ValueChanged = ValueChanged;
+
+  return ValueChanged;
+}
+
+/**
+  Initialize Question's Edit copy from Storage.
+
+  @param  Selection              Selection contains the information about 
+                                 the Selection, form and formset to be displayed.
+                                 Selection action may be updated in retrieve callback.
+                                 If Selection is NULL, only initialize Question value.
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+LoadFormConfig (
+  IN OUT UI_MENU_SELECTION    *Selection,
+  IN FORM_BROWSER_FORMSET     *FormSet,
+  IN FORM_BROWSER_FORM        *Form
+  )
+{
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *Link;
+  FORM_BROWSER_STATEMENT      *Question;
+  
+  Link = GetFirstNode (&Form->StatementListHead);
+  while (!IsNull (&Form->StatementListHead, Link)) {
+    Question = FORM_BROWSER_STATEMENT_FROM_LINK (Link);
+
+    //
+    // Initialize local copy of Value for each Question
+    //
+    if (Question->Operand == EFI_IFR_PASSWORD_OP && (Question->QuestionFlags & EFI_IFR_FLAG_CALLBACK)== 0) {
+      Status = GetQuestionValue (FormSet, Form, Question, GetSetValueWithHiiDriver);
+    } else {
+      Status = GetQuestionValue (FormSet, Form, Question, GetSetValueWithEditBuffer);
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    if ((Question->Operand == EFI_IFR_STRING_OP) || (Question->Operand == EFI_IFR_PASSWORD_OP)) {
+      HiiSetString (FormSet->HiiHandle, Question->HiiValue.Value.string, (CHAR16*)Question->BufferValue, NULL);
+    }
+
+    Link = GetNextNode (&Form->StatementListHead, Link);
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialize Question's Edit copy from Storage for the whole Formset.
+
+  @param  Selection              Selection contains the information about 
+                                 the Selection, form and formset to be displayed.
+                                 Selection action may be updated in retrieve callback.
+                                 If Selection is NULL, only initialize Question value.
+  @param  FormSet                FormSet data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+LoadFormSetConfig (
+  IN OUT UI_MENU_SELECTION    *Selection,
+  IN     FORM_BROWSER_FORMSET *FormSet
+  )
+{
+  EFI_STATUS            Status;
+  LIST_ENTRY            *Link;
+  FORM_BROWSER_FORM     *Form;
+
+  Link = GetFirstNode (&FormSet->FormListHead);
+  while (!IsNull (&FormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+
+    //
+    // Initialize local copy of Value for each Form
+    //
+    Status = LoadFormConfig (Selection, FormSet, Form);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+
+    Link = GetNextNode (&FormSet->FormListHead, Link);
+  }
+
+  //
+  // Finished question initialization.
+  // 
+  FormSet->QuestionInited = TRUE;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Remove the Request element from the Config Request.
+
+  @param  Storage                Pointer to the browser storage.
+  @param  RequestElement         The pointer to the Request element.
+
+**/
+VOID
+RemoveElement (
+  IN OUT BROWSER_STORAGE      *Storage,
+  IN     CHAR16               *RequestElement
+  )
+{
+  CHAR16   *NewStr;
+  CHAR16   *DestStr;
+
+  ASSERT (Storage->ConfigRequest != NULL && RequestElement != NULL);
+
+  NewStr = StrStr (Storage->ConfigRequest, RequestElement);
+
+  if (NewStr == NULL) {
+    return;
+  }
+
+  //
+  // Remove this element from this ConfigRequest.
+  //
+  DestStr = NewStr;
+  NewStr += StrLen (RequestElement);
+  CopyMem (DestStr, NewStr, StrSize (NewStr));
+  
+  Storage->SpareStrLen += StrLen (RequestElement);  
+}
+
+/**
+  Adjust config request in storage, remove the request elements existed in the input ConfigRequest.
+
+  @param  Storage                Pointer to the formset storage.
+  @param  ConfigRequest          The pointer to the Request element.
+
+**/
+VOID
+RemoveConfigRequest (
+  FORMSET_STORAGE   *Storage,
+  CHAR16            *ConfigRequest
+  )
+{
+  CHAR16       *RequestElement;
+  CHAR16       *NextRequestElement;
+  CHAR16       *SearchKey;
+
+  //
+  // No request element in it, just return.
+  //
+  if (ConfigRequest == NULL) {
+    return;
+  }
+
+  if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    //
+    // "&Name1&Name2" section for EFI_HII_VARSTORE_NAME_VALUE storage
+    //
+    SearchKey = L"&";
+  } else {
+    //
+    // "&OFFSET=####&WIDTH=####" section for EFI_HII_VARSTORE_BUFFER storage
+    //
+    SearchKey = L"&OFFSET";
+  }
+
+  //
+  // Find SearchKey storage
+  //
+  if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    RequestElement = StrStr (ConfigRequest, L"PATH");
+    ASSERT (RequestElement != NULL);
+    RequestElement = StrStr (RequestElement, SearchKey);    
+  } else {
+    RequestElement = StrStr (ConfigRequest, SearchKey);
+  }
+
+  while (RequestElement != NULL) {
+    //
+    // +1 to avoid find header itself.
+    //
+    NextRequestElement = StrStr (RequestElement + 1, SearchKey);
+
+    //
+    // The last Request element in configRequest string.
+    //
+    if (NextRequestElement != NULL) {
+      //
+      // Replace "&" with '\0'.
+      //
+      *NextRequestElement = L'\0';
+    }
+
+    RemoveElement (Storage->BrowserStorage, RequestElement);
+
+    if (NextRequestElement != NULL) {
+      //
+      // Restore '&' with '\0' for later used.
+      //
+      *NextRequestElement = L'&';
+    }
+
+    RequestElement = NextRequestElement;
+  }
+
+  //
+  // If no request element remain, just remove the ConfigRequest string.
+  //
+  if (StrCmp (Storage->BrowserStorage->ConfigRequest, Storage->ConfigHdr) == 0) {
+    FreePool (Storage->BrowserStorage->ConfigRequest);
+    Storage->BrowserStorage->ConfigRequest = NULL;
+    Storage->BrowserStorage->SpareStrLen   = 0;
+  }
+}
+
+/**
+  Base on the current formset info, clean the ConfigRequest string in browser storage.
+
+  @param  FormSet                Pointer of the FormSet
+
+**/
+VOID
+CleanBrowserStorage (
+  IN OUT FORM_BROWSER_FORMSET  *FormSet
+  )
+{
+  LIST_ENTRY            *Link;
+  FORMSET_STORAGE       *Storage;
+
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    Storage = FORMSET_STORAGE_FROM_LINK (Link);
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+
+    if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER) {
+      if (Storage->ConfigRequest == NULL || Storage->BrowserStorage->ConfigRequest == NULL) {
+        continue;
+      }
+
+      RemoveConfigRequest (Storage, Storage->ConfigRequest);
+    } else if (Storage->BrowserStorage->Type == EFI_HII_VARSTORE_BUFFER ||
+               Storage->BrowserStorage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+      if (Storage->BrowserStorage->ConfigRequest != NULL) { 
+        FreePool (Storage->BrowserStorage->ConfigRequest);
+        Storage->BrowserStorage->ConfigRequest = NULL;
+      }
+      Storage->BrowserStorage->Initialized = FALSE;
+    }
+  }
+}
+
+/**
+  Check whether current element in the ConfigReqeust string.
+
+  @param  BrowserStorage                Storage which includes ConfigReqeust.
+  @param  RequestElement                New element need to check.
+
+  @retval TRUE        The Element is in the ConfigReqeust string.
+  @retval FALSE       The Element not in the configReqeust String.
+
+**/
+BOOLEAN 
+ElementValidation (
+  BROWSER_STORAGE   *BrowserStorage,
+  CHAR16            *RequestElement
+  )
+{
+  return StrStr (BrowserStorage->ConfigRequest, RequestElement) != NULL ? TRUE : FALSE;
+}
+
+/**
+  Append the Request element to the Config Request.
+
+  @param  ConfigRequest          Current ConfigRequest info.
+  @param  SpareStrLen            Current remain free buffer for config reqeust.
+  @param  RequestElement         New Request element.
+
+**/
+VOID
+AppendConfigRequest (
+  IN OUT CHAR16               **ConfigRequest,
+  IN OUT UINTN                *SpareStrLen,
+  IN     CHAR16               *RequestElement
+  )
+{
+  CHAR16   *NewStr;
+  UINTN    StringSize;
+  UINTN    StrLength;
+  UINTN    MaxLen;
+
+  StrLength = StrLen (RequestElement);
+  StringSize = (*ConfigRequest != NULL) ? StrSize (*ConfigRequest) : sizeof (CHAR16);
+  MaxLen = StringSize / sizeof (CHAR16) + *SpareStrLen;
+
+  //
+  // Append <RequestElement> to <ConfigRequest>
+  //
+  if (StrLength > *SpareStrLen) {
+    //
+    // Old String buffer is not sufficient for RequestElement, allocate a new one
+    //
+    MaxLen = StringSize / sizeof (CHAR16) + CONFIG_REQUEST_STRING_INCREMENTAL;
+    NewStr = AllocateZeroPool (MaxLen * sizeof (CHAR16));
+    ASSERT (NewStr != NULL);
+
+    if (*ConfigRequest != NULL) {
+      CopyMem (NewStr, *ConfigRequest, StringSize);
+      FreePool (*ConfigRequest);
+    }
+    *ConfigRequest = NewStr;
+    *SpareStrLen   = CONFIG_REQUEST_STRING_INCREMENTAL;
+  }
+
+  StrCatS (*ConfigRequest, MaxLen, RequestElement);
+  *SpareStrLen -= StrLength;
+}
+
+/**
+  Adjust the config request info, remove the request elements which already in AllConfigRequest string.
+
+  @param  Storage                Form set Storage.
+  @param  Request                The input request string.
+  @param  RespString             Whether the input is ConfigRequest or ConfigResp format.
+
+  @retval TRUE                   Has element not covered by current used elements, need to continue to call ExtractConfig
+  @retval FALSE                  All elements covered by current used elements.
+
+**/
+BOOLEAN 
+ConfigRequestAdjust (
+  IN  BROWSER_STORAGE         *Storage,
+  IN  CHAR16                  *Request,
+  IN  BOOLEAN                 RespString
+  )
+{
+  CHAR16       *RequestElement;
+  CHAR16       *NextRequestElement;
+  CHAR16       *NextElementBakup;
+  CHAR16       *SearchKey;
+  CHAR16       *ValueKey;
+  BOOLEAN      RetVal;
+  CHAR16       *ConfigRequest;
+
+  RetVal         = FALSE;
+  NextElementBakup = NULL;
+  ValueKey         = NULL;
+
+  if (Request != NULL) {
+    ConfigRequest = Request;
+  } else {
+    ConfigRequest = Storage->ConfigRequest;
+  }
+
+  if (Storage->ConfigRequest == NULL) {
+    Storage->ConfigRequest = AllocateCopyPool (StrSize (ConfigRequest), ConfigRequest);
+    return TRUE;
+  }
+
+  if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    //
+    // "&Name1&Name2" section for EFI_HII_VARSTORE_NAME_VALUE storage
+    //
+    SearchKey = L"&";
+  } else {
+    //
+    // "&OFFSET=####&WIDTH=####" section for EFI_HII_VARSTORE_BUFFER storage
+    //
+    SearchKey = L"&OFFSET";
+    ValueKey  = L"&VALUE";
+  }
+
+  //
+  // Find SearchKey storage
+  //
+  if (Storage->Type == EFI_HII_VARSTORE_NAME_VALUE) {
+    RequestElement = StrStr (ConfigRequest, L"PATH");
+    ASSERT (RequestElement != NULL);
+    RequestElement = StrStr (RequestElement, SearchKey);    
+  } else {
+    RequestElement = StrStr (ConfigRequest, SearchKey);
+  }
+
+  while (RequestElement != NULL) {
+
+    //
+    // +1 to avoid find header itself.
+    //
+    NextRequestElement = StrStr (RequestElement + 1, SearchKey);
+
+    //
+    // The last Request element in configRequest string.
+    //
+    if (NextRequestElement != NULL) {
+      if (RespString && (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER)) {
+        NextElementBakup = NextRequestElement;
+        NextRequestElement = StrStr (RequestElement, ValueKey);
+        ASSERT (NextRequestElement != NULL);
+      }
+      //
+      // Replace "&" with '\0'.
+      //
+      *NextRequestElement = L'\0';
+    } else {
+      if (RespString && (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER)) {
+        NextElementBakup = NextRequestElement;
+        NextRequestElement = StrStr (RequestElement, ValueKey);
+        ASSERT (NextRequestElement != NULL);
+        //
+        // Replace "&" with '\0'.
+        //
+        *NextRequestElement = L'\0';
+      }
+    }
+ 
+    if (!ElementValidation (Storage, RequestElement)) {
+      //
+      // Add this element to the Storage->BrowserStorage->AllRequestElement.
+      //
+      AppendConfigRequest(&Storage->ConfigRequest, &Storage->SpareStrLen, RequestElement);
+      RetVal = TRUE;
+    }
+
+    if (NextRequestElement != NULL) {
+      //
+      // Restore '&' with '\0' for later used.
+      //
+      *NextRequestElement = L'&';
+    }
+
+    if (RespString && (Storage->Type == EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER)) {
+      RequestElement = NextElementBakup;
+    } else {
+      RequestElement = NextRequestElement;
+    }
+  }
+
+  return RetVal;
+}
+
+/**
+  Fill storage's edit copy with settings requested from Configuration Driver.
+
+  @param  FormSet                FormSet data structure.
+  @param  Storage                Buffer Storage.
+
+**/
+VOID
+LoadStorage (
+  IN FORM_BROWSER_FORMSET    *FormSet,
+  IN FORMSET_STORAGE         *Storage
+  )
+{
+  EFI_STATUS  Status;
+  EFI_STRING  Progress;
+  EFI_STRING  Result;
+  CHAR16      *StrPtr;
+  EFI_STRING  ConfigRequest;
+  UINTN       StrLen;
+
+  ConfigRequest = NULL;
+
+  switch (Storage->BrowserStorage->Type) {
+    case EFI_HII_VARSTORE_EFI_VARIABLE:
+      return;
+
+    case EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER:
+      if (Storage->BrowserStorage->ConfigRequest != NULL) {
+        ConfigRequestAdjust(Storage->BrowserStorage, Storage->ConfigRequest, FALSE);
+        return;
+      }
+      break;
+
+    case EFI_HII_VARSTORE_BUFFER:
+    case EFI_HII_VARSTORE_NAME_VALUE:
+      //
+      // Skip if there is no RequestElement.
+      //
+      if (Storage->ElementCount == 0) {
+        return;
+      }
+
+      //
+      // Just update the ConfigRequest, if storage already initialized. 
+      //
+      if (Storage->BrowserStorage->Initialized) {
+        ConfigRequestAdjust(Storage->BrowserStorage, Storage->ConfigRequest, FALSE);
+        return;
+      }
+
+      Storage->BrowserStorage->Initialized = TRUE;
+      break;
+
+    default:
+      return;
+  }
+
+  if (Storage->BrowserStorage->Type != EFI_HII_VARSTORE_NAME_VALUE) {
+    //
+    // Create the config request string to get all fields for this storage.
+    // Allocate and fill a buffer large enough to hold the <ConfigHdr> template
+    // followed by "&OFFSET=0&WIDTH=WWWW"followed by a Null-terminator
+    //
+    StrLen = StrSize (Storage->ConfigHdr) + 20 * sizeof (CHAR16);
+    ConfigRequest = AllocateZeroPool (StrLen);
+    ASSERT (ConfigRequest != NULL);
+    UnicodeSPrint (
+               ConfigRequest, 
+               StrLen, 
+               L"%s&OFFSET=0&WIDTH=%04x", 
+               Storage->ConfigHdr,
+               Storage->BrowserStorage->Size);
+  } else {
+    ConfigRequest = Storage->ConfigRequest;
+  }
+
+  //
+  // Request current settings from Configuration Driver
+  //
+  Status = mHiiConfigRouting->ExtractConfig (
+                                    mHiiConfigRouting,
+                                    ConfigRequest,
+                                    &Progress,
+                                    &Result
+                                    );
+
+  //
+  // If get value fail, extract default from IFR binary
+  //
+  if (EFI_ERROR (Status)) {
+    ExtractDefault (FormSet, NULL, EFI_HII_DEFAULT_CLASS_STANDARD, FormSetLevel, GetDefaultForStorage, Storage->BrowserStorage, TRUE, TRUE);
+  } else {
+    //
+    // Convert Result from <ConfigAltResp> to <ConfigResp>
+    //
+    StrPtr = StrStr (Result, L"&GUID=");
+    if (StrPtr != NULL) {
+      *StrPtr = L'\0';
+    }
+    
+    Status = ConfigRespToStorage (Storage->BrowserStorage, Result);
+    FreePool (Result);
+  }
+
+  Storage->BrowserStorage->ConfigRequest = AllocateCopyPool (StrSize (Storage->ConfigRequest), Storage->ConfigRequest);
+
+  //
+  // Input NULL for ConfigRequest field means sync all fields from editbuffer to buffer. 
+  //
+  SynchronizeStorage(Storage->BrowserStorage, NULL, TRUE);
+
+  if (Storage->BrowserStorage->Type != EFI_HII_VARSTORE_NAME_VALUE) {
+    if (ConfigRequest != NULL) {
+      FreePool (ConfigRequest);
+    }
+  }
+}
+
+/**
+  Get Value changed status from old question.
+
+  @param  NewFormSet                FormSet data structure.
+  @param  OldQuestion               Old question which has value changed.
+
+**/
+VOID
+SyncStatusForQuestion (
+  IN OUT FORM_BROWSER_FORMSET             *NewFormSet,
+  IN     FORM_BROWSER_STATEMENT           *OldQuestion
+  )
+{
+  LIST_ENTRY                  *Link;
+  LIST_ENTRY                  *QuestionLink;
+  FORM_BROWSER_FORM           *Form;
+  FORM_BROWSER_STATEMENT      *Question;
+
+  //
+  // For each form in one formset.
+  //
+  Link = GetFirstNode (&NewFormSet->FormListHead);
+  while (!IsNull (&NewFormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+    Link = GetNextNode (&NewFormSet->FormListHead, Link);
+
+    //
+    // for each question in one form.
+    //
+    QuestionLink = GetFirstNode (&Form->StatementListHead);
+    while (!IsNull (&Form->StatementListHead, QuestionLink)) {
+      Question = FORM_BROWSER_STATEMENT_FROM_LINK (QuestionLink);
+      QuestionLink = GetNextNode (&Form->StatementListHead, QuestionLink);
+
+      if (Question->QuestionId == OldQuestion->QuestionId) {
+        Question->ValueChanged = TRUE;
+        return;
+      }
+    }
+  }
+}
+
+/**
+  Get Value changed status from old formset.
+
+  @param  NewFormSet                FormSet data structure.
+  @param  OldFormSet                FormSet data structure.
+
+**/
+VOID
+SyncStatusForFormSet (
+  IN OUT FORM_BROWSER_FORMSET             *NewFormSet,
+  IN     FORM_BROWSER_FORMSET             *OldFormSet
+  )
+{
+  LIST_ENTRY                  *Link;
+  LIST_ENTRY                  *QuestionLink;
+  FORM_BROWSER_FORM           *Form;
+  FORM_BROWSER_STATEMENT      *Question;
+
+  //
+  // For each form in one formset.
+  //
+  Link = GetFirstNode (&OldFormSet->FormListHead);
+  while (!IsNull (&OldFormSet->FormListHead, Link)) {
+    Form = FORM_BROWSER_FORM_FROM_LINK (Link);
+    Link = GetNextNode (&OldFormSet->FormListHead, Link);
+
+    //
+    // for each question in one form.
+    //
+    QuestionLink = GetFirstNode (&Form->StatementListHead);
+    while (!IsNull (&Form->StatementListHead, QuestionLink)) {
+      Question = FORM_BROWSER_STATEMENT_FROM_LINK (QuestionLink);
+      QuestionLink = GetNextNode (&Form->StatementListHead, QuestionLink);
+
+      if (!Question->ValueChanged) {
+        continue;
+      }
+
+      //
+      // Find the same question in new formset and update the value changed flag.
+      //
+      SyncStatusForQuestion (NewFormSet, Question);
+    }
+  }
+}
+
+/**
+  Get current setting of Questions.
+
+  @param  FormSet                FormSet data structure.
+
+**/
+VOID
+InitializeCurrentSetting (
+  IN OUT FORM_BROWSER_FORMSET             *FormSet
+  )
+{
+  LIST_ENTRY              *Link;
+  FORMSET_STORAGE         *Storage;
+  FORM_BROWSER_FORMSET    *OldFormSet;
+
+  //
+  // Try to find pre FormSet in the maintain backup list.
+  // If old formset != NULL, destroy this formset. Add new formset to gBrowserFormSetList.
+  //
+  OldFormSet = GetFormSetFromHiiHandle (FormSet->HiiHandle);
+  if (OldFormSet != NULL) {
+    SyncStatusForFormSet (FormSet, OldFormSet);
+    RemoveEntryList (&OldFormSet->Link);
+    DestroyFormSet (OldFormSet);
+  }
+  InsertTailList (&gBrowserFormSetList, &FormSet->Link);
+
+  //
+  // Extract default from IFR binary for no storage questions.
+  //  
+  ExtractDefault (FormSet, NULL, EFI_HII_DEFAULT_CLASS_STANDARD, FormSetLevel, GetDefaultForNoStorage, NULL, TRUE, FALSE);
+
+  //
+  // Request current settings from Configuration Driver
+  //
+  Link = GetFirstNode (&FormSet->StorageListHead);
+  while (!IsNull (&FormSet->StorageListHead, Link)) {
+    Storage = FORMSET_STORAGE_FROM_LINK (Link);
+
+    LoadStorage (FormSet, Storage);
+
+    Link = GetNextNode (&FormSet->StorageListHead, Link);
+  }
+}
+
+
+/**
+  Fetch the Ifr binary data of a FormSet.
+
+  @param  Handle                 PackageList Handle
+  @param  FormSetGuid            On input, GUID or class GUID of a formset. If not
+                                 specified (NULL or zero GUID), take the first
+                                 FormSet with class GUID EFI_HII_PLATFORM_SETUP_FORMSET_GUID
+                                 found in package list.
+                                 On output, GUID of the formset found(if not NULL).
+  @param  BinaryLength           The length of the FormSet IFR binary.
+  @param  BinaryData             The buffer designed to receive the FormSet.
+
+  @retval EFI_SUCCESS            Buffer filled with the requested FormSet.
+                                 BufferLength was updated.
+  @retval EFI_INVALID_PARAMETER  The handle is unknown.
+  @retval EFI_NOT_FOUND          A form or FormSet on the requested handle cannot
+                                 be found with the requested FormId.
+
+**/
+EFI_STATUS
+GetIfrBinaryData (
+  IN  EFI_HII_HANDLE   Handle,
+  IN OUT EFI_GUID      *FormSetGuid,
+  OUT UINTN            *BinaryLength,
+  OUT UINT8            **BinaryData
+  )
+{
+  EFI_STATUS                   Status;
+  EFI_HII_PACKAGE_LIST_HEADER  *HiiPackageList;
+  UINTN                        BufferSize;
+  UINT8                        *Package;
+  UINT8                        *OpCodeData;
+  UINT32                       Offset;
+  UINT32                       Offset2;
+  UINT32                       PackageListLength;
+  EFI_HII_PACKAGE_HEADER       PackageHeader;
+  UINT8                        Index;
+  UINT8                        NumberOfClassGuid;
+  BOOLEAN                      ClassGuidMatch;
+  EFI_GUID                     *ClassGuid;
+  EFI_GUID                     *ComparingGuid;
+
+  OpCodeData = NULL;
+  Package = NULL;
+  ZeroMem (&PackageHeader, sizeof (EFI_HII_PACKAGE_HEADER));
+
+  //
+  // if FormSetGuid is NULL or zero GUID, return first Setup FormSet in the package list
+  //
+  if (FormSetGuid == NULL) {
+    ComparingGuid = &gZeroGuid;
+  } else {
+    ComparingGuid = FormSetGuid;
+  }
+
+  //
+  // Get HII PackageList
+  //
+  BufferSize = 0;
+  HiiPackageList = NULL;
+  Status = mHiiDatabase->ExportPackageLists (mHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  if (Status == EFI_BUFFER_TOO_SMALL) {
+    HiiPackageList = AllocatePool (BufferSize);
+    ASSERT (HiiPackageList != NULL);
+
+    Status = mHiiDatabase->ExportPackageLists (mHiiDatabase, Handle, &BufferSize, HiiPackageList);
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  ASSERT (HiiPackageList != NULL);
+
+  //
+  // Get Form package from this HII package List
+  //
+  Offset = sizeof (EFI_HII_PACKAGE_LIST_HEADER);
+  Offset2 = 0;
+  CopyMem (&PackageListLength, &HiiPackageList->PackageLength, sizeof (UINT32));
+
+  ClassGuidMatch = FALSE;
+  while (Offset < PackageListLength) {
+    Package = ((UINT8 *) HiiPackageList) + Offset;
+    CopyMem (&PackageHeader, Package, sizeof (EFI_HII_PACKAGE_HEADER));
+
+    if (PackageHeader.Type == EFI_HII_PACKAGE_FORMS) {
+      //
+      // Search FormSet in this Form Package
+      //
+      Offset2 = sizeof (EFI_HII_PACKAGE_HEADER);
+      while (Offset2 < PackageHeader.Length) {
+        OpCodeData = Package + Offset2;
+
+        if (((EFI_IFR_OP_HEADER *) OpCodeData)->OpCode == EFI_IFR_FORM_SET_OP) {
+          //
+          // Try to compare against formset GUID
+          //
+          if (CompareGuid (FormSetGuid, &gZeroGuid) || 
+              CompareGuid (ComparingGuid, (EFI_GUID *)(OpCodeData + sizeof (EFI_IFR_OP_HEADER)))) {
+            break;
+          }
+
+          if (((EFI_IFR_OP_HEADER *) OpCodeData)->Length > OFFSET_OF (EFI_IFR_FORM_SET, Flags)) {
+            //
+            // Try to compare against formset class GUID
+            //
+            NumberOfClassGuid = (UINT8) (((EFI_IFR_FORM_SET *) OpCodeData)->Flags & 0x3);
+            ClassGuid         = (EFI_GUID *) (OpCodeData + sizeof (EFI_IFR_FORM_SET));
+            for (Index = 0; Index < NumberOfClassGuid; Index++) {
+              if (CompareGuid (ComparingGuid, ClassGuid + Index)) {
+                ClassGuidMatch = TRUE;
+                break;
+              }
+            }
+            if (ClassGuidMatch) {
+              break;
+            }
+          } else if (ComparingGuid == &gEfiHiiPlatformSetupFormsetGuid) {
+            ClassGuidMatch = TRUE;
+            break;
+          }
+        }
+
+        Offset2 += ((EFI_IFR_OP_HEADER *) OpCodeData)->Length;
+      }
+
+      if (Offset2 < PackageHeader.Length) {
+        //
+        // Target formset found
+        //
+        break;
+      }
+    }
+
+    Offset += PackageHeader.Length;
+  }
+
+  if (Offset >= PackageListLength) {
+    //
+    // Form package not found in this Package List
+    //
+    FreePool (HiiPackageList);
+    return EFI_NOT_FOUND;
+  }
+
+  if (FormSetGuid != NULL) {
+    //
+    // Return the FormSet GUID
+    //
+    CopyMem (FormSetGuid, &((EFI_IFR_FORM_SET *) OpCodeData)->Guid, sizeof (EFI_GUID));
+  }
+
+  //
+  // To determine the length of a whole FormSet IFR binary, one have to parse all the Opcodes
+  // in this FormSet; So, here just simply copy the data from start of a FormSet to the end
+  // of the Form Package.
+  //
+  *BinaryLength = PackageHeader.Length - Offset2;
+  *BinaryData = AllocateCopyPool (*BinaryLength, OpCodeData);
+
+  FreePool (HiiPackageList);
+
+  if (*BinaryData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Initialize the internal data structure of a FormSet.
+
+  @param  Handle                 PackageList Handle
+  @param  FormSetGuid            On input, GUID or class GUID of a formset. If not
+                                 specified (NULL or zero GUID), take the first
+                                 FormSet with class GUID EFI_HII_PLATFORM_SETUP_FORMSET_GUID
+                                 found in package list.
+                                 On output, GUID of the formset found(if not NULL).
+  @param  FormSet                FormSet data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_NOT_FOUND          The specified FormSet could not be found.
+
+**/
+EFI_STATUS
+InitializeFormSet (
+  IN  EFI_HII_HANDLE                   Handle,
+  IN OUT EFI_GUID                      *FormSetGuid,
+  OUT FORM_BROWSER_FORMSET             *FormSet
+  )
+{
+  EFI_STATUS                Status;
+  EFI_HANDLE                DriverHandle;
+
+  Status = GetIfrBinaryData (Handle, FormSetGuid, &FormSet->IfrBinaryLength, &FormSet->IfrBinaryData);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FormSet->Signature = FORM_BROWSER_FORMSET_SIGNATURE;
+  FormSet->HiiHandle = Handle;
+  CopyMem (&FormSet->Guid, FormSetGuid, sizeof (EFI_GUID));
+  FormSet->QuestionInited = FALSE;
+
+  //
+  // Retrieve ConfigAccess Protocol associated with this HiiPackageList
+  //
+  Status = mHiiDatabase->GetPackageListHandle (mHiiDatabase, Handle, &DriverHandle);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  FormSet->DriverHandle = DriverHandle;
+  Status = gBS->HandleProtocol (
+                  DriverHandle,
+                  &gEfiHiiConfigAccessProtocolGuid,
+                  (VOID **) &FormSet->ConfigAccess
+                  );
+  if (EFI_ERROR (Status)) {
+    //
+    // Configuration Driver don't attach ConfigAccess protocol to its HII package
+    // list, then there will be no configuration action required
+    //
+    FormSet->ConfigAccess = NULL;
+  }
+
+  //
+  // Parse the IFR binary OpCodes
+  //
+  Status = ParseOpCodes (FormSet);
+
+  return Status;
+}
+
+
+/**
+  Save globals used by previous call to SendForm(). SendForm() may be called from 
+  HiiConfigAccess.Callback(), this will cause SendForm() be reentried.
+  So, save globals of previous call to SendForm() and restore them upon exit.
+
+**/
+VOID
+SaveBrowserContext (
+  VOID
+  )
+{
+  BROWSER_CONTEXT      *Context;
+  FORM_ENTRY_INFO      *MenuList;
+  FORM_BROWSER_FORMSET *FormSet;
+
+  gBrowserContextCount++;
+  if (gBrowserContextCount == 1) {
+    //
+    // This is not reentry of SendForm(), no context to save
+    //
+    return;
+  }
+
+  Context = AllocatePool (sizeof (BROWSER_CONTEXT));
+  ASSERT (Context != NULL);
+
+  Context->Signature = BROWSER_CONTEXT_SIGNATURE;
+
+  //
+  // Save FormBrowser context
+  //
+  Context->Selection            = gCurrentSelection;
+  Context->ResetRequired        = gResetRequired;
+  Context->FlagReconnect        = gFlagReconnect;
+  Context->CallbackReconnect    = gCallbackReconnect;
+  Context->ExitRequired         = gExitRequired;
+  Context->HiiHandle            = mCurrentHiiHandle;
+  Context->FormId               = mCurrentFormId;
+  CopyGuid (&Context->FormSetGuid, &mCurrentFormSetGuid);
+  Context->SystemLevelFormSet   = mSystemLevelFormSet;
+  Context->CurFakeQestId        = mCurFakeQestId;
+  Context->HiiPackageListUpdated = mHiiPackageListUpdated;
+  Context->FinishRetrieveCall   = mFinishRetrieveCall;
+
+  //
+  // Save the menu history data.
+  //
+  InitializeListHead(&Context->FormHistoryList);
+  while (!IsListEmpty (&mPrivateData.FormBrowserEx2.FormViewHistoryHead)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (mPrivateData.FormBrowserEx2.FormViewHistoryHead.ForwardLink);
+    RemoveEntryList (&MenuList->Link);
+
+    InsertTailList(&Context->FormHistoryList, &MenuList->Link);
+  }
+
+  //
+  // Save formset list.
+  //
+  InitializeListHead(&Context->FormSetList);
+  while (!IsListEmpty (&gBrowserFormSetList)) {
+    FormSet = FORM_BROWSER_FORMSET_FROM_LINK (gBrowserFormSetList.ForwardLink);
+    RemoveEntryList (&FormSet->Link);
+
+    InsertTailList(&Context->FormSetList, &FormSet->Link);
+  }
+
+  //
+  // Insert to FormBrowser context list
+  //
+  InsertHeadList (&gBrowserContextList, &Context->Link);
+}
+
+
+/**
+  Restore globals used by previous call to SendForm().
+
+**/
+VOID
+RestoreBrowserContext (
+  VOID
+  )
+{
+  LIST_ENTRY       *Link;
+  BROWSER_CONTEXT  *Context;
+  FORM_ENTRY_INFO      *MenuList;
+  FORM_BROWSER_FORMSET *FormSet;
+
+  ASSERT (gBrowserContextCount != 0);
+  gBrowserContextCount--;
+  if (gBrowserContextCount == 0) {
+    //
+    // This is not reentry of SendForm(), no context to restore
+    //
+    return;
+  }
+
+  ASSERT (!IsListEmpty (&gBrowserContextList));
+
+  Link = GetFirstNode (&gBrowserContextList);
+  Context = BROWSER_CONTEXT_FROM_LINK (Link);
+
+  //
+  // Restore FormBrowser context
+  //
+  gCurrentSelection     = Context->Selection;
+  gResetRequired        = Context->ResetRequired;
+  gFlagReconnect        = Context->FlagReconnect;
+  gCallbackReconnect    = Context->CallbackReconnect;
+  gExitRequired         = Context->ExitRequired;
+  mCurrentHiiHandle     = Context->HiiHandle;
+  mCurrentFormId        = Context->FormId;
+  CopyGuid (&mCurrentFormSetGuid, &Context->FormSetGuid);
+  mSystemLevelFormSet   = Context->SystemLevelFormSet;
+  mCurFakeQestId        = Context->CurFakeQestId;
+  mHiiPackageListUpdated = Context->HiiPackageListUpdated;
+  mFinishRetrieveCall   = Context->FinishRetrieveCall;
+
+  //
+  // Restore the menu history data.
+  //
+  while (!IsListEmpty (&Context->FormHistoryList)) {
+    MenuList = FORM_ENTRY_INFO_FROM_LINK (Context->FormHistoryList.ForwardLink);
+    RemoveEntryList (&MenuList->Link);
+
+    InsertTailList(&mPrivateData.FormBrowserEx2.FormViewHistoryHead, &MenuList->Link);
+  }
+
+  //
+  // Restore the Formset data.
+  //
+  while (!IsListEmpty (&Context->FormSetList)) {
+    FormSet = FORM_BROWSER_FORMSET_FROM_LINK (Context->FormSetList.ForwardLink);
+    RemoveEntryList (&FormSet->Link);
+
+    InsertTailList(&gBrowserFormSetList, &FormSet->Link);
+  }
+
+  //
+  // Remove from FormBrowser context list
+  //
+  RemoveEntryList (&Context->Link);
+  gBS->FreePool (Context);
+}
+
+/**
+  Find the matched FormSet context in the backup maintain list based on HiiHandle.
+  
+  @param Handle  The Hii Handle.
+  
+  @return the found FormSet context. If no found, NULL will return.
+
+**/
+FORM_BROWSER_FORMSET * 
+GetFormSetFromHiiHandle (
+  EFI_HII_HANDLE Handle
+  )
+{
+  LIST_ENTRY           *Link;
+  FORM_BROWSER_FORMSET *FormSet;
+
+  Link = GetFirstNode (&gBrowserFormSetList);
+  while (!IsNull (&gBrowserFormSetList, Link)) {
+    FormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+    Link = GetNextNode (&gBrowserFormSetList, Link);
+    if (!ValidateFormSet(FormSet)) {
+      continue;
+    }
+    if (FormSet->HiiHandle == Handle) {
+      return FormSet;
+    }
+  }
+  
+  return NULL;
+}
+
+/**
+  Check whether the input HII handle is the FormSet that is being used.
+  
+  @param Handle  The Hii Handle.
+  
+  @retval TRUE   HII handle is being used.
+  @retval FALSE  HII handle is not being used.
+
+**/
+BOOLEAN
+IsHiiHandleInBrowserContext (
+  EFI_HII_HANDLE Handle
+  )
+{
+  LIST_ENTRY       *Link;
+  BROWSER_CONTEXT  *Context;
+
+  //
+  // HiiHandle is Current FormSet.
+  //
+  if (mCurrentHiiHandle == Handle) {
+    return TRUE;
+  }
+
+  //
+  // Check whether HiiHandle is in BrowserContext.
+  //
+  Link = GetFirstNode (&gBrowserContextList);
+  while (!IsNull (&gBrowserContextList, Link)) {
+    Context = BROWSER_CONTEXT_FROM_LINK (Link);
+    if (Context->HiiHandle == Handle) {
+      //
+      // HiiHandle is in BrowserContext
+      //
+      return TRUE;
+    }
+    Link = GetNextNode (&gBrowserContextList, Link);
+  }
+  
+  return FALSE;
+}
+
+/**
+  Perform Password check. 
+  Passwork may be encrypted by driver that requires the specific check.
+  
+  @param  Form             Form where Password Statement is in.
+  @param  Statement        Password statement
+  @param  PasswordString   Password string to be checked. It may be NULL.
+                           NULL means to restore password.
+                           "" string can be used to checked whether old password does exist.
+  
+  @return Status     Status of Password check.
+**/
+EFI_STATUS
+EFIAPI
+PasswordCheck (
+  IN FORM_DISPLAY_ENGINE_FORM      *Form,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN EFI_STRING                    PasswordString  OPTIONAL
+  )
+{
+  EFI_STATUS                      Status;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+  EFI_BROWSER_ACTION_REQUEST      ActionRequest;
+  EFI_IFR_TYPE_VALUE              IfrTypeValue;
+  FORM_BROWSER_STATEMENT          *Question;
+
+  ConfigAccess = gCurrentSelection->FormSet->ConfigAccess;
+  Question = GetBrowserStatement(Statement);
+  ASSERT (Question != NULL);
+
+  if ((Question->QuestionFlags & EFI_IFR_FLAG_CALLBACK) == EFI_IFR_FLAG_CALLBACK) {
+    if (ConfigAccess == NULL) {
+      return EFI_UNSUPPORTED;
+    }
+  } else {
+    if (PasswordString == NULL) {
+      return EFI_SUCCESS;
+    } 
+
+    //
+    // Check whether has preexisted password.
+    //
+    if (PasswordString[0] == 0) {
+      if (*((CHAR16 *) Question->BufferValue) == 0) {
+        return EFI_SUCCESS;
+      } else {
+        return EFI_NOT_READY;
+      }
+    }
+
+    //
+    // Check whether the input password is same as preexisted password.
+    //
+    if (StrnCmp (PasswordString, (CHAR16 *) Question->BufferValue, Question->StorageWidth/sizeof (CHAR16)) == 0) {
+      return EFI_SUCCESS;
+    } else {
+      return EFI_NOT_READY;
+    }
+  }
+    
+  //
+  // Prepare password string in HII database
+  //
+  if (PasswordString != NULL) {
+    IfrTypeValue.string = NewString (PasswordString, gCurrentSelection->FormSet->HiiHandle);
+  } else {
+    IfrTypeValue.string = 0;
+  }
+
+  //
+  // Send password to Configuration Driver for validation
+  //
+  Status = ConfigAccess->Callback (
+                           ConfigAccess,
+                           EFI_BROWSER_ACTION_CHANGING,
+                           Question->QuestionId,
+                           Question->HiiValue.Type,
+                           &IfrTypeValue,
+                           &ActionRequest
+                           );
+
+  //
+  // Remove password string from HII database
+  //
+  if (PasswordString != NULL) {
+    DeleteString (IfrTypeValue.string, gCurrentSelection->FormSet->HiiHandle);
+  }
+
+  return Status;
+}
+
+/**
+  Find the registered HotKey based on KeyData.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key.
+
+  @return The registered HotKey context. If no found, NULL will return.
+**/
+BROWSER_HOT_KEY *
+GetHotKeyFromRegisterList (
+  IN EFI_INPUT_KEY *KeyData
+  )
+{
+  LIST_ENTRY       *Link;
+  BROWSER_HOT_KEY  *HotKey;
+
+  Link = GetFirstNode (&gBrowserHotKeyList);
+  while (!IsNull (&gBrowserHotKeyList, Link)) {
+    HotKey = BROWSER_HOT_KEY_FROM_LINK (Link);
+    if (HotKey->KeyData->ScanCode == KeyData->ScanCode) {
+      return HotKey;
+    }
+    Link = GetNextNode (&gBrowserHotKeyList, Link);
+  }
+  
+  return NULL;
+}
+
+/**
+  Configure what scope the hot key will impact.
+  All hot keys have the same scope. The mixed hot keys with the different level are not supported.
+  If no scope is set, the default scope will be FormSet level.
+  After all registered hot keys are removed, previous Scope can reset to another level.
+  
+  @param[in] Scope               Scope level to be set. 
+  
+  @retval EFI_SUCCESS            Scope is set correctly.
+  @retval EFI_INVALID_PARAMETER  Scope is not the valid value specified in BROWSER_SETTING_SCOPE. 
+  @retval EFI_UNSPPORTED         Scope level is different from current one that the registered hot keys have.
+
+**/
+EFI_STATUS
+EFIAPI
+SetScope (
+  IN BROWSER_SETTING_SCOPE Scope
+  )
+{
+  if (Scope >= MaxLevel) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // When no hot key registered in system or on the first setting,
+  // Scope can be set.
+  //
+  if (mBrowserScopeFirstSet || IsListEmpty (&gBrowserHotKeyList)) {
+    gBrowserSettingScope  = Scope;
+    mBrowserScopeFirstSet = FALSE;
+  } else if (Scope != gBrowserSettingScope) {
+    return EFI_UNSUPPORTED;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register the hot key with its browser action, or unregistered the hot key.
+  Only support hot key that is not printable character (control key, function key, etc.).
+  If the action value is zero, the hot key will be unregistered if it has been registered.
+  If the same hot key has been registered, the new action and help string will override the previous ones.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key. Its type is EFI_INPUT_KEY to 
+                         be supported by all ConsoleIn devices.
+  @param[in] Action      Action value that describes what action will be trigged when the hot key is pressed. 
+  @param[in] DefaultId   Specifies the type of defaults to retrieve, which is only for DEFAULT action.
+  @param[in] HelpString  Help string that describes the hot key information.
+                         Its value may be NULL for the unregistered hot key.
+  
+  @retval EFI_SUCCESS            Hot key is registered or unregistered.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL or HelpString is NULL on register.
+  @retval EFI_NOT_FOUND          KeyData is not found to be unregistered.
+  @retval EFI_UNSUPPORTED        Key represents a printable character. It is conflicted with Browser.
+  @retval EFI_ALREADY_STARTED    Key already been registered for one hot key.
+**/
+EFI_STATUS
+EFIAPI
+RegisterHotKey (
+  IN EFI_INPUT_KEY *KeyData,
+  IN UINT32        Action,
+  IN UINT16        DefaultId,
+  IN EFI_STRING    HelpString OPTIONAL
+  )
+{
+  BROWSER_HOT_KEY  *HotKey;
+
+  //
+  // Check input parameters.
+  //
+  if (KeyData == NULL || KeyData->UnicodeChar != CHAR_NULL || 
+     (Action != BROWSER_ACTION_UNREGISTER && HelpString == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether the input KeyData is in BrowserHotKeyList.
+  //
+  HotKey = GetHotKeyFromRegisterList (KeyData);
+  
+  //
+  // Unregister HotKey
+  //
+  if (Action == BROWSER_ACTION_UNREGISTER) {
+    if (HotKey != NULL) {
+      //
+      // The registered HotKey is found.  
+      // Remove it from List, and free its resource.
+      //
+      RemoveEntryList (&HotKey->Link);
+      FreePool (HotKey->KeyData);
+      FreePool (HotKey->HelpString);
+      return EFI_SUCCESS;
+    } else {
+      //
+      // The registered HotKey is not found. 
+      //
+      return EFI_NOT_FOUND;
+    }
+  }
+
+  if (HotKey != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // Create new Key, and add it into List.
+  //
+  HotKey = AllocateZeroPool (sizeof (BROWSER_HOT_KEY));
+  ASSERT (HotKey != NULL);
+  HotKey->Signature = BROWSER_HOT_KEY_SIGNATURE;
+  HotKey->KeyData   = AllocateCopyPool (sizeof (EFI_INPUT_KEY), KeyData);
+  InsertTailList (&gBrowserHotKeyList, &HotKey->Link);
+
+  //
+  // Fill HotKey information.
+  //
+  HotKey->Action     = Action;
+  HotKey->DefaultId  = DefaultId;
+  if (HotKey->HelpString != NULL) {
+    FreePool (HotKey->HelpString);
+  }
+  HotKey->HelpString = AllocateCopyPool (StrSize (HelpString), HelpString);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Register Exit handler function. 
+  When more than one handler function is registered, the latter one will override the previous one. 
+  When NULL handler is specified, the previous Exit handler will be unregistered. 
+  
+  @param[in] Handler      Pointer to handler function. 
+
+**/
+VOID
+EFIAPI
+RegiserExitHandler (
+  IN EXIT_HANDLER Handler
+  )
+{
+  ExitHandlerFunction = Handler;
+  return;
+}
+
+/**
+  Check whether the browser data has been modified.
+
+  @retval TRUE        Browser data is modified.
+  @retval FALSE       No browser data is modified.
+
+**/
+BOOLEAN
+EFIAPI
+IsBrowserDataModified (
+  VOID
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_FORMSET    *FormSet;
+
+  switch (gBrowserSettingScope) {
+    case FormLevel:
+      if (gCurrentSelection == NULL) {
+        return FALSE;
+      }
+      return IsNvUpdateRequiredForForm (gCurrentSelection->Form);
+
+    case FormSetLevel:
+      if (gCurrentSelection == NULL) {
+        return FALSE;
+      }
+      return IsNvUpdateRequiredForFormSet (gCurrentSelection->FormSet);
+
+    case SystemLevel:
+      Link = GetFirstNode (&gBrowserFormSetList);
+      while (!IsNull (&gBrowserFormSetList, Link)) {
+        FormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+        if (!ValidateFormSet(FormSet)) {
+          continue;
+        }
+
+        if (IsNvUpdateRequiredForFormSet (FormSet)) {
+          return TRUE;
+        }
+        Link = GetNextNode (&gBrowserFormSetList, Link);
+      }
+      return FALSE;
+
+    default:
+      return FALSE;
+  }
+}
+
+/**
+  Execute the action requested by the Action parameter.
+
+  @param[in] Action     Execute the request action.
+  @param[in] DefaultId  The default Id info when need to load default value. Only used when Action is BROWSER_ACTION_DEFAULT.
+
+  @retval EFI_SUCCESS              Execute the request action succss.
+  @retval EFI_INVALID_PARAMETER    The input action value is invalid.
+
+**/
+EFI_STATUS 
+EFIAPI
+ExecuteAction (
+  IN UINT32        Action,
+  IN UINT16        DefaultId
+  )
+{
+  EFI_STATUS              Status;
+  FORM_BROWSER_FORMSET    *FormSet;
+  FORM_BROWSER_FORM       *Form;
+
+  if (gBrowserSettingScope < SystemLevel && gCurrentSelection == NULL) {
+    return EFI_NOT_READY;
+  }
+
+  Status  = EFI_SUCCESS;
+  FormSet = NULL;
+  Form    = NULL;
+  if (gBrowserSettingScope < SystemLevel) {
+    FormSet = gCurrentSelection->FormSet;
+    Form    = gCurrentSelection->Form; 
+  }
+
+  //
+  // Executet the discard action.
+  //
+  if ((Action & BROWSER_ACTION_DISCARD) != 0) {
+    Status = DiscardForm (FormSet, Form, gBrowserSettingScope);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Executet the difault action.
+  //
+  if ((Action & BROWSER_ACTION_DEFAULT) != 0) {
+    Status = ExtractDefault (FormSet, Form, DefaultId, gBrowserSettingScope, GetDefaultForAll, NULL, FALSE, FALSE);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    UpdateStatementStatus (FormSet, Form, gBrowserSettingScope);
+  }
+
+  //
+  // Executet the submit action.
+  //
+  if ((Action & BROWSER_ACTION_SUBMIT) != 0) {
+    Status = SubmitForm (FormSet, Form, gBrowserSettingScope);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Executet the reset action.
+  //
+  if ((Action & BROWSER_ACTION_RESET) != 0) {
+    gResetRequired = TRUE;
+  }
+
+  //
+  // Executet the exit action.
+  //
+  if ((Action & BROWSER_ACTION_EXIT) != 0) {
+    DiscardForm (FormSet, Form, gBrowserSettingScope);
+    if (gBrowserSettingScope == SystemLevel) {
+      if (ExitHandlerFunction != NULL) {
+        ExitHandlerFunction ();
+      }
+    }
+
+    gExitRequired = TRUE;
+  }
+
+  return Status;
+}
+
+/**
+  Create reminder to let user to choose save or discard the changed browser data.
+  Caller can use it to actively check the changed browser data.
+
+  @retval BROWSER_NO_CHANGES       No browser data is changed.
+  @retval BROWSER_SAVE_CHANGES     The changed browser data is saved.
+  @retval BROWSER_DISCARD_CHANGES  The changed browser data is discard.
+  @retval BROWSER_KEEP_CURRENT     Browser keep current changes.
+
+**/
+UINT32
+EFIAPI
+SaveReminder (
+  VOID
+  )
+{
+  LIST_ENTRY              *Link;
+  FORM_BROWSER_FORMSET    *FormSet;
+  BOOLEAN                 IsDataChanged;
+  UINT32                  DataSavedAction;
+  UINT32                  ConfirmRet;
+
+  DataSavedAction  = BROWSER_NO_CHANGES;
+  IsDataChanged    = FALSE;
+  Link = GetFirstNode (&gBrowserFormSetList);
+  while (!IsNull (&gBrowserFormSetList, Link)) {
+    FormSet = FORM_BROWSER_FORMSET_FROM_LINK (Link);
+    Link = GetNextNode (&gBrowserFormSetList, Link);
+    if (!ValidateFormSet(FormSet)) {
+      continue;
+    }
+    if (IsNvUpdateRequiredForFormSet (FormSet)) {
+      IsDataChanged = TRUE;
+      break;
+    }
+  }
+  
+  //
+  // No data is changed. No save is required. 
+  //
+  if (!IsDataChanged) {
+    return DataSavedAction;
+  }
+  
+  //
+  // If data is changed, prompt user to save or discard it. 
+  //
+  do {
+    ConfirmRet = (UINT32) mFormDisplay->ConfirmDataChange();
+
+    if (ConfirmRet == BROWSER_ACTION_SUBMIT) {
+      SubmitForm (NULL, NULL, SystemLevel);
+      DataSavedAction = BROWSER_SAVE_CHANGES;
+      break;
+    } else if (ConfirmRet == BROWSER_ACTION_DISCARD) {
+      DiscardForm (NULL, NULL, SystemLevel);
+      DataSavedAction = BROWSER_DISCARD_CHANGES;
+      break;
+    } else if (ConfirmRet == BROWSER_ACTION_NONE) {
+      DataSavedAction = BROWSER_KEEP_CURRENT;
+      break;
+    }
+  } while (1);
+
+  return DataSavedAction;
+}
+
+/**
+  Check whether the Reset Required for the browser
+
+  @retval TRUE      Browser required to reset after exit.
+  @retval FALSE     Browser not need to reset after exit.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetRequired (
+  VOID
+  )
+{
+  return gResetRequired;
+}
+
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.h b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.h
new file mode 100644
index 0000000000..0abb09cf87
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/Setup.h
@@ -0,0 +1,1873 @@
+/** @file
+Private MACRO, structure and function definitions for Setup Browser module.
+
+Copyright (c) 2007 - 2015, 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.
+
+
+**/
+
+#ifndef _SETUP_H_
+#define _SETUP_H_
+
+
+#include <PiDxe.h>
+
+#include <Protocol/SimpleTextOut.h>
+#include <Protocol/SimpleTextIn.h>
+#include <Protocol/FormBrowser2.h>
+#include <Protocol/FormBrowserEx2.h>
+#include <Protocol/DisplayProtocol.h>
+#include <Protocol/DevicePath.h>
+#include <Protocol/UnicodeCollation.h>
+#include <Protocol/HiiConfigAccess.h>
+#include <Protocol/HiiConfigRouting.h>
+#include <Protocol/HiiDatabase.h>
+#include <Protocol/HiiString.h>
+#include <Protocol/UserManager.h>
+#include <Protocol/DevicePathFromText.h>
+#include <Protocol/RegularExpressionProtocol.h>
+
+#include <Guid/MdeModuleHii.h>
+#include <Guid/HiiPlatformSetupFormset.h>
+#include <Guid/HiiFormMapMethodGuid.h>
+#include <Guid/ZeroGuid.h>
+
+#include <Library/PrintLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/HiiLib.h>
+#include <Library/PcdLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/UefiLib.h>
+
+
+//
+// This is the generated header file which includes whatever needs to be exported (strings + IFR)
+//
+
+#define UI_ACTION_NONE               0
+#define UI_ACTION_REFRESH_FORM       1
+#define UI_ACTION_REFRESH_FORMSET    2
+#define UI_ACTION_EXIT               3
+
+//
+//
+// Time definitions
+//
+#define ONE_SECOND  10000000
+
+// Incremental string lenght of ConfigRequest
+//
+#define CONFIG_REQUEST_STRING_INCREMENTAL  1024
+
+//
+// Incremental size of stack for expression
+//
+#define EXPRESSION_STACK_SIZE_INCREMENT    0x100
+
+#define EFI_IFR_SPECIFICATION_VERSION  (UINT16) (((EFI_SYSTEM_TABLE_REVISION >> 16) << 8) | (((EFI_SYSTEM_TABLE_REVISION & 0xFFFF) / 10) << 4) | ((EFI_SYSTEM_TABLE_REVISION & 0xFFFF) % 10))
+
+
+#define SETUP_DRIVER_SIGNATURE SIGNATURE_32 ('F', 'B', 'D', 'V')
+typedef struct {
+  UINT32                             Signature;
+
+  EFI_HANDLE                         Handle;
+
+  //
+  // Produced protocol
+  //
+  EFI_FORM_BROWSER2_PROTOCOL            FormBrowser2;
+  EFI_FORM_BROWSER_EXTENSION_PROTOCOL   FormBrowserEx;
+
+  EDKII_FORM_BROWSER_EXTENSION2_PROTOCOL FormBrowserEx2;
+
+} SETUP_DRIVER_PRIVATE_DATA;
+
+//
+// IFR relative definition
+//
+#define EFI_HII_EXPRESSION_INCONSISTENT_IF   0
+#define EFI_HII_EXPRESSION_NO_SUBMIT_IF      1
+#define EFI_HII_EXPRESSION_GRAY_OUT_IF       2
+#define EFI_HII_EXPRESSION_SUPPRESS_IF       3
+#define EFI_HII_EXPRESSION_DISABLE_IF        4
+#define EFI_HII_EXPRESSION_VALUE             5
+#define EFI_HII_EXPRESSION_RULE              6
+#define EFI_HII_EXPRESSION_READ              7
+#define EFI_HII_EXPRESSION_WRITE             8
+#define EFI_HII_EXPRESSION_WARNING_IF        9
+
+#define EFI_HII_VARSTORE_BUFFER              0
+#define EFI_HII_VARSTORE_NAME_VALUE          1
+#define EFI_HII_VARSTORE_EFI_VARIABLE        2    // EFI Varstore type follow UEFI spec before 2.3.1.
+#define EFI_HII_VARSTORE_EFI_VARIABLE_BUFFER 3    // EFI varstore type follow UEFI spec 2.3.1 and later.
+
+#define FORM_INCONSISTENT_VALIDATION         0
+#define FORM_NO_SUBMIT_VALIDATION            1
+
+#define NAME_VALUE_NODE_SIGNATURE  SIGNATURE_32 ('N', 'V', 'S', 'T')
+
+typedef struct {
+  UINTN            Signature;
+  LIST_ENTRY       Link;
+  CHAR16           *Name;
+  CHAR16           *Value;
+  CHAR16           *EditValue;
+} NAME_VALUE_NODE;
+
+#define NAME_VALUE_NODE_FROM_LINK(a)  CR (a, NAME_VALUE_NODE, Link, NAME_VALUE_NODE_SIGNATURE)
+
+#define BROWSER_STORAGE_SIGNATURE  SIGNATURE_32 ('B', 'S', 'T', 'G')
+
+typedef struct {
+  UINTN            Signature;
+  LIST_ENTRY       Link;
+
+  UINT8            Type;           // Storage type
+
+  BOOLEAN          Initialized;    // Whether this varstore is initialized, efi varstore not used.
+
+  EFI_HII_HANDLE   HiiHandle;      // HiiHandle for this varstore, efi varstore not used.
+  EFI_GUID         Guid;
+
+  CHAR16           *Name;          // For EFI_IFR_VARSTORE
+  UINT16           Size;
+  UINT8            *Buffer;
+  UINT8            *EditBuffer;    // Edit copy for Buffer Storage
+
+  LIST_ENTRY       NameValueListHead; // List of NAME_VALUE_NODE
+
+  UINT32           Attributes;     // For EFI_IFR_VARSTORE_EFI: EFI Variable attribute
+
+  CHAR16           *ConfigRequest; // <ConfigRequest> = <ConfigHdr> + <RequestElement>
+                                   // <RequestElement> includes all fields which is used by current form sets.
+  UINTN            SpareStrLen;    // Spare length of ConfigRequest string buffer
+} BROWSER_STORAGE;
+
+#define BROWSER_STORAGE_FROM_LINK(a)  CR (a, BROWSER_STORAGE, Link, BROWSER_STORAGE_SIGNATURE)
+
+#define FORMSET_STORAGE_SIGNATURE  SIGNATURE_32 ('F', 'S', 'T', 'G')
+
+typedef struct {
+  UINTN            Signature;
+  LIST_ENTRY       Link;
+
+  LIST_ENTRY       SaveFailLink;
+
+  UINT16           VarStoreId;
+
+  BROWSER_STORAGE  *BrowserStorage;
+
+  CHAR16           *ConfigHdr;     // <ConfigHdr>
+
+  CHAR16           *ConfigRequest; // <ConfigRequest> = <ConfigHdr> + <RequestElement>
+  CHAR16           *ConfigAltResp; // Alt config response string for this ConfigRequest.
+  BOOLEAN          HasCallAltCfg;  // Flag to show whether browser has call ExtractConfig to get Altcfg string.
+  UINTN            ElementCount;   // Number of <RequestElement> in the <ConfigRequest>
+  UINTN            SpareStrLen;    // Spare length of ConfigRequest string buffer
+} FORMSET_STORAGE;
+
+#define FORMSET_STORAGE_FROM_LINK(a)  CR (a, FORMSET_STORAGE, Link, FORMSET_STORAGE_SIGNATURE)
+#define FORMSET_STORAGE_FROM_SAVE_FAIL_LINK(a)  CR (a, FORMSET_STORAGE, SaveFailLink, FORMSET_STORAGE_SIGNATURE)
+
+typedef union {
+  EFI_STRING_ID         VarName;
+  UINT16                VarOffset;
+} VAR_STORE_INFO;
+
+#define EXPRESSION_OPCODE_SIGNATURE  SIGNATURE_32 ('E', 'X', 'O', 'P')
+
+typedef struct {
+  UINTN             Signature;
+  LIST_ENTRY        Link;
+
+  UINT8             Operand;
+
+  UINT8             Format;      // For EFI_IFR_TO_STRING, EFI_IFR_FIND
+  UINT8             Flags;       // For EFI_IFR_SPAN
+  UINT8             RuleId;      // For EFI_IFR_RULE_REF
+
+  EFI_HII_VALUE     Value;       // For EFI_IFR_EQ_ID_VAL, EFI_IFR_UINT64, EFI_IFR_UINT32, EFI_IFR_UINT16, EFI_IFR_UINT8, EFI_IFR_STRING_REF1
+
+  EFI_QUESTION_ID   QuestionId;  // For EFI_IFR_EQ_ID_ID, EFI_IFR_EQ_ID_VAL_LIST, EFI_IFR_QUESTION_REF1
+  EFI_QUESTION_ID   QuestionId2;
+
+  UINT16            ListLength;  // For EFI_IFR_EQ_ID_VAL_LIST
+  UINT16            *ValueList;
+
+  EFI_STRING_ID     DevicePath;  // For EFI_IFR_QUESTION_REF3_2, EFI_IFR_QUESTION_REF3_3
+  EFI_GUID          Guid;
+
+  BROWSER_STORAGE   *VarStorage; // For EFI_IFR_SET, EFI_IFR_GET
+  VAR_STORE_INFO    VarStoreInfo;// For EFI_IFR_SET, EFI_IFR_GET
+  UINT8             ValueType;   // For EFI_IFR_SET, EFI_IFR_GET
+  UINT8             ValueWidth;  // For EFI_IFR_SET, EFI_IFR_GET
+  CHAR16            *ValueName;  // For EFI_IFR_SET, EFI_IFR_GET
+  LIST_ENTRY        MapExpressionList;   // nested expressions inside of Map opcode.
+} EXPRESSION_OPCODE;
+
+#define EXPRESSION_OPCODE_FROM_LINK(a)  CR (a, EXPRESSION_OPCODE, Link, EXPRESSION_OPCODE_SIGNATURE)
+
+#define FORM_EXPRESSION_SIGNATURE  SIGNATURE_32 ('F', 'E', 'X', 'P')
+
+typedef struct {
+  UINTN             Signature;
+  LIST_ENTRY        Link;
+
+  UINT8             Type;            // Type for this expression
+
+  UINT8             RuleId;          // For EFI_IFR_RULE only
+  EFI_STRING_ID     Error;           // For EFI_IFR_NO_SUBMIT_IF, EFI_IFR_INCONSISTENT_IF only
+
+  EFI_HII_VALUE     Result;          // Expression evaluation result
+
+  UINT8             TimeOut;         // For EFI_IFR_WARNING_IF
+  EFI_IFR_OP_HEADER *OpCode;         // Save the opcode buffer.
+
+  LIST_ENTRY        OpCodeListHead;  // OpCodes consist of this expression (EXPRESSION_OPCODE)
+} FORM_EXPRESSION;
+
+#define FORM_EXPRESSION_FROM_LINK(a)  CR (a, FORM_EXPRESSION, Link, FORM_EXPRESSION_SIGNATURE)
+
+#define FORM_EXPRESSION_LIST_SIGNATURE  SIGNATURE_32 ('F', 'E', 'X', 'R')
+
+typedef struct {
+    UINTN               Signature;
+    UINTN               Count;
+    FORM_EXPRESSION    *Expression[1];   // Array[Count] of expressions
+} FORM_EXPRESSION_LIST;
+
+#define QUESTION_DEFAULT_SIGNATURE  SIGNATURE_32 ('Q', 'D', 'F', 'T')
+
+typedef struct {
+  UINTN               Signature;
+  LIST_ENTRY          Link;
+
+  UINT16              DefaultId;
+  EFI_HII_VALUE       Value;              // Default value
+
+  FORM_EXPRESSION     *ValueExpression;   // Not-NULL indicates default value is provided by EFI_IFR_VALUE
+} QUESTION_DEFAULT;
+
+#define QUESTION_DEFAULT_FROM_LINK(a)  CR (a, QUESTION_DEFAULT, Link, QUESTION_DEFAULT_SIGNATURE)
+
+#define QUESTION_OPTION_SIGNATURE  SIGNATURE_32 ('Q', 'O', 'P', 'T')
+
+typedef struct {
+  UINTN                Signature;
+  LIST_ENTRY           Link;
+  
+  EFI_IFR_ONE_OF_OPTION  *OpCode;   // OneOfOption Data
+
+  EFI_STRING_ID        Text;
+  UINT8                Flags;
+  EFI_HII_VALUE        Value;
+  EFI_IMAGE_ID         ImageId;
+
+  FORM_EXPRESSION_LIST *SuppressExpression; // Non-NULL indicates nested inside of SuppressIf
+} QUESTION_OPTION;
+
+#define QUESTION_OPTION_FROM_LINK(a)  CR (a, QUESTION_OPTION, Link, QUESTION_OPTION_SIGNATURE)
+
+typedef enum {
+  ExpressFalse = 0,
+  ExpressGrayOut,  
+  ExpressSuppress,
+  ExpressDisable
+} EXPRESS_RESULT;
+
+typedef enum {
+  ExpressNone = 0,
+  ExpressForm,  
+  ExpressStatement,
+  ExpressOption
+} EXPRESS_LEVEL;
+
+typedef struct _FORM_BROWSER_STATEMENT FORM_BROWSER_STATEMENT;
+
+#define FORM_BROWSER_STATEMENT_SIGNATURE  SIGNATURE_32 ('F', 'S', 'T', 'A')
+
+struct _FORM_BROWSER_STATEMENT{
+  UINTN                 Signature;
+  LIST_ENTRY            Link;
+
+  UINT8                 Operand;          // The operand (first byte) of this Statement or Question
+  EFI_IFR_OP_HEADER     *OpCode;
+
+  //
+  // Statement Header
+  //
+  EFI_STRING_ID         Prompt;
+  EFI_STRING_ID         Help;
+  EFI_STRING_ID         TextTwo;          // For EFI_IFR_TEXT
+
+  //
+  // Fake Question Id, used for statement not has true QuestionId.
+  //
+  EFI_QUESTION_ID       FakeQuestionId;
+
+  //
+  // Question Header
+  //
+  EFI_QUESTION_ID       QuestionId;       // The value of zero is reserved
+  EFI_VARSTORE_ID       VarStoreId;       // A value of zero indicates no variable storage
+  BROWSER_STORAGE       *Storage;
+  VAR_STORE_INFO        VarStoreInfo;
+  UINT16                StorageWidth;
+  UINT8                 QuestionFlags;
+  CHAR16                *VariableName;    // Name/Value or EFI Variable name
+  CHAR16                *BlockName;       // Buffer storage block name: "OFFSET=...WIDTH=..."
+
+  EFI_HII_VALUE         HiiValue;         // Edit copy for checkbox, numberic, oneof
+  UINT8                 *BufferValue;     // Edit copy for string, password, orderedlist
+  UINT8                 ValueType;        // Data type for orderedlist value array
+
+  //
+  // OpCode specific members
+  //
+  UINT8                 Flags;            // for EFI_IFR_CHECKBOX, EFI_IFR_DATE, EFI_IFR_NUMERIC, EFI_IFR_ONE_OF,
+                                          // EFI_IFR_ORDERED_LIST, EFI_IFR_STRING,EFI_IFR_SUBTITLE,EFI_IFR_TIME, EFI_IFR_BANNER
+  UINT8                 MaxContainers;    // for EFI_IFR_ORDERED_LIST
+
+  UINT16                BannerLineNumber; // for EFI_IFR_BANNER, 1-based line number
+  EFI_STRING_ID         QuestionConfig;   // for EFI_IFR_ACTION, if 0 then no configuration string will be processed
+
+  UINT64                Minimum;          // for EFI_IFR_ONE_OF/EFI_IFR_NUMERIC, it's Min/Max value
+  UINT64                Maximum;          // for EFI_IFR_STRING/EFI_IFR_PASSWORD, it's Min/Max length
+  UINT64                Step;
+
+  EFI_DEFAULT_ID        DefaultId;        // for EFI_IFR_RESET_BUTTON
+  EFI_GUID              RefreshGuid;      // for EFI_IFR_REFRESH_ID
+  BOOLEAN               Locked;           // Whether this statement is locked.
+  BOOLEAN               ValueChanged;     // Whether this statement's value is changed.
+  //
+  // Get from IFR parsing
+  //
+  FORM_EXPRESSION       *ValueExpression;    // nested EFI_IFR_VALUE, provide Question value and indicate Question is ReadOnly
+  LIST_ENTRY            DefaultListHead;     // nested EFI_IFR_DEFAULT list (QUESTION_DEFAULT), provide default values
+  LIST_ENTRY            OptionListHead;      // nested EFI_IFR_ONE_OF_OPTION list (QUESTION_OPTION)
+
+  EFI_IMAGE_ID          ImageId;             // nested EFI_IFR_IMAGE
+  UINT8                 RefreshInterval;     // nested EFI_IFR_REFRESH, refresh interval(in seconds) for Question value, 0 means no refresh
+
+  FORM_BROWSER_STATEMENT *ParentStatement;
+
+  LIST_ENTRY            InconsistentListHead;// nested inconsistent expression list (FORM_EXPRESSION)
+  LIST_ENTRY            NoSubmitListHead;    // nested nosubmit expression list (FORM_EXPRESSION)
+  LIST_ENTRY            WarningListHead;     // nested warning expression list (FORM_EXPRESSION)
+  FORM_EXPRESSION_LIST  *Expression;         // nesting inside of GrayOutIf/DisableIf/SuppressIf
+
+  FORM_EXPRESSION       *ReadExpression;     // nested EFI_IFR_READ, provide this question value by read expression.
+  FORM_EXPRESSION       *WriteExpression;    // nested EFI_IFR_WRITE, evaluate write expression after this question value is set.
+};
+
+#define FORM_BROWSER_STATEMENT_FROM_LINK(a)  CR (a, FORM_BROWSER_STATEMENT, Link, FORM_BROWSER_STATEMENT_SIGNATURE)
+
+#define FORM_BROWSER_CONFIG_REQUEST_SIGNATURE  SIGNATURE_32 ('F', 'C', 'R', 'S')
+typedef struct {
+  UINTN                 Signature;
+  LIST_ENTRY            Link;
+
+  LIST_ENTRY            SaveFailLink;
+
+  CHAR16                *ConfigRequest; // <ConfigRequest> = <ConfigHdr> + <RequestElement>
+  CHAR16                *ConfigAltResp; // Alt config response string for this ConfigRequest.
+  UINTN                 ElementCount;   // Number of <RequestElement> in the <ConfigRequest>  
+  UINTN                 SpareStrLen;
+
+  BROWSER_STORAGE       *Storage;
+} FORM_BROWSER_CONFIG_REQUEST;
+#define FORM_BROWSER_CONFIG_REQUEST_FROM_LINK(a)  CR (a, FORM_BROWSER_CONFIG_REQUEST, Link, FORM_BROWSER_CONFIG_REQUEST_SIGNATURE)
+#define FORM_BROWSER_CONFIG_REQUEST_FROM_SAVE_FAIL_LINK(a)  CR (a, FORM_BROWSER_CONFIG_REQUEST, SaveFailLink, FORM_BROWSER_CONFIG_REQUEST_SIGNATURE)
+
+#define FORM_BROWSER_FORM_SIGNATURE  SIGNATURE_32 ('F', 'F', 'R', 'M')
+#define STANDARD_MAP_FORM_TYPE 0x01
+
+typedef struct {
+  UINTN                Signature;
+  LIST_ENTRY           Link;
+
+  UINT16               FormId;               // FormId of normal form or formmap form.
+  EFI_STRING_ID        FormTitle;            // FormTile of normal form, or FormMapMethod title of formmap form.
+  UINT16               FormType;             // Specific form type for the different form.
+
+  EFI_IMAGE_ID         ImageId;
+
+  BOOLEAN              ModalForm;            // Whether this is a modal form.
+  BOOLEAN              Locked;               // Whether this form is locked.
+  EFI_GUID             RefreshGuid;          // Form refresh event guid.
+
+  LIST_ENTRY           FormViewListHead;     // List of type FORMID_INFO is Browser View Form History List.
+  LIST_ENTRY           ExpressionListHead;   // List of Expressions (FORM_EXPRESSION)
+  LIST_ENTRY           StatementListHead;    // List of Statements and Questions (FORM_BROWSER_STATEMENT)
+  LIST_ENTRY           ConfigRequestHead;    // List of configreques for all storage.
+  FORM_EXPRESSION_LIST *SuppressExpression;  // nesting inside of SuppressIf
+} FORM_BROWSER_FORM;
+
+#define FORM_BROWSER_FORM_FROM_LINK(a)  CR (a, FORM_BROWSER_FORM, Link, FORM_BROWSER_FORM_SIGNATURE)
+
+#define FORMSET_DEFAULTSTORE_SIGNATURE  SIGNATURE_32 ('F', 'D', 'F', 'S')
+
+typedef struct {
+  UINTN            Signature;
+  LIST_ENTRY       Link;
+
+  UINT16           DefaultId;
+  EFI_STRING_ID    DefaultName;
+} FORMSET_DEFAULTSTORE;
+
+#define FORMSET_DEFAULTSTORE_FROM_LINK(a)  CR (a, FORMSET_DEFAULTSTORE, Link, FORMSET_DEFAULTSTORE_SIGNATURE)
+
+#define FORM_BROWSER_FORMSET_SIGNATURE  SIGNATURE_32 ('F', 'B', 'F', 'S')
+
+typedef struct {
+  UINTN                           Signature;
+  LIST_ENTRY                      Link;
+  LIST_ENTRY                      SaveFailLink;
+
+  EFI_HII_HANDLE                  HiiHandle;      // unique id for formset.
+  EFI_HANDLE                      DriverHandle;
+  EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess;
+  EFI_DEVICE_PATH_PROTOCOL        *DevicePath;
+
+  UINTN                           IfrBinaryLength;
+  UINT8                           *IfrBinaryData;
+
+  BOOLEAN                         QuestionInited;   // Have finished question initilization?
+  EFI_GUID                        Guid;
+  EFI_STRING_ID                   FormSetTitle;
+  EFI_STRING_ID                   Help;
+  UINT8                           NumberOfClassGuid;
+  EFI_GUID                        ClassGuid[3];         // Up to three ClassGuid
+  UINT16                          Class;                // Tiano extended Class code
+  UINT16                          SubClass;             // Tiano extended Subclass code
+  EFI_IMAGE_ID                    ImageId;
+  EFI_IFR_OP_HEADER               *OpCode;              //mainly for formset op to get ClassGuid
+
+  FORM_BROWSER_STATEMENT          *StatementBuffer;     // Buffer for all Statements and Questions
+  EXPRESSION_OPCODE               *ExpressionBuffer;    // Buffer for all Expression OpCode
+  FORM_BROWSER_FORM               *SaveFailForm;        // The form which failed to save.
+  FORM_BROWSER_STATEMENT          *SaveFailStatement;   // The Statement which failed to save.
+
+  LIST_ENTRY                      StatementListOSF;     // Statement list out side of the form.
+  LIST_ENTRY                      StorageListHead;      // Storage list (FORMSET_STORAGE)
+  LIST_ENTRY                      SaveFailStorageListHead; // Storage list for the save fail storage.
+  LIST_ENTRY                      DefaultStoreListHead; // DefaultStore list (FORMSET_DEFAULTSTORE)
+  LIST_ENTRY                      FormListHead;         // Form list (FORM_BROWSER_FORM)
+  LIST_ENTRY                      ExpressionListHead;   // List of Expressions (FORM_EXPRESSION)
+} FORM_BROWSER_FORMSET;
+#define FORM_BROWSER_FORMSET_FROM_LINK(a)  CR (a, FORM_BROWSER_FORMSET, Link, FORM_BROWSER_FORMSET_SIGNATURE)
+
+#define FORM_BROWSER_FORMSET_FROM_SAVE_FAIL_LINK(a)  CR (a, FORM_BROWSER_FORMSET, SaveFailLink, FORM_BROWSER_FORMSET_SIGNATURE)
+
+typedef struct {
+  LIST_ENTRY   Link;
+  EFI_EVENT    RefreshEvent;
+} FORM_BROWSER_REFRESH_EVENT_NODE;
+
+#define FORM_BROWSER_REFRESH_EVENT_FROM_LINK(a) BASE_CR (a, FORM_BROWSER_REFRESH_EVENT_NODE, Link)
+
+
+typedef struct {
+  EFI_HII_HANDLE  Handle;
+
+  //
+  // Target formset/form/Question information
+  //
+  EFI_GUID        FormSetGuid;
+  UINT16          FormId;
+  UINT16          QuestionId;
+  UINTN           Sequence;  // used for time/date only.
+
+  UINTN           TopRow;
+  UINTN           BottomRow;
+  UINTN           PromptCol;
+  UINTN           OptionCol;
+  UINTN           CurrentRow;
+
+  //
+  // Ation for Browser to taken:
+  //   UI_ACTION_NONE            - navigation inside a form
+  //   UI_ACTION_REFRESH_FORM    - re-evaluate expressions and repaint form
+  //   UI_ACTION_REFRESH_FORMSET - re-parse formset IFR binary
+  //
+  UINTN           Action;
+
+  //
+  // Current selected fomset/form/Question
+  //
+  FORM_BROWSER_FORMSET    *FormSet;
+  FORM_BROWSER_FORM       *Form;
+  FORM_BROWSER_STATEMENT  *Statement;
+
+  //
+  // Whether the Form is editable
+  //
+  BOOLEAN                 FormEditable;
+
+  FORM_ENTRY_INFO            *CurrentMenu;
+} UI_MENU_SELECTION;
+
+#define BROWSER_CONTEXT_SIGNATURE  SIGNATURE_32 ('B', 'C', 'T', 'X')
+
+typedef struct {
+  UINTN                 Signature;
+  LIST_ENTRY            Link;
+
+  //
+  // Globals defined in Setup.c
+  //
+  BOOLEAN                  FlagReconnect;
+  BOOLEAN                  CallbackReconnect;
+  BOOLEAN                  ResetRequired;
+  BOOLEAN                  ExitRequired;
+  EFI_HII_HANDLE           HiiHandle;
+  EFI_GUID                 FormSetGuid;
+  EFI_FORM_ID              FormId;
+  UI_MENU_SELECTION        *Selection;
+  FORM_BROWSER_FORMSET     *SystemLevelFormSet;
+  EFI_QUESTION_ID          CurFakeQestId;
+  BOOLEAN                  HiiPackageListUpdated;
+  BOOLEAN                  FinishRetrieveCall;
+  LIST_ENTRY               FormHistoryList;
+  LIST_ENTRY               FormSetList;
+} BROWSER_CONTEXT;
+
+#define BROWSER_CONTEXT_FROM_LINK(a)  CR (a, BROWSER_CONTEXT, Link, BROWSER_CONTEXT_SIGNATURE)
+
+//
+// Scope for get defaut value. It may be GetDefaultForNoStorage, GetDefaultForStorage or GetDefaultForAll.
+//
+typedef enum {
+  GetDefaultForNoStorage,       // Get default value for question which not has storage.
+  GetDefaultForStorage,         // Get default value for question which has storage.
+  GetDefaultForAll,             // Get default value for all questions.
+  GetDefaultForMax              // Invalid value.
+} BROWSER_GET_DEFAULT_VALUE;
+
+//
+// Get/set question value from/to.
+//
+typedef enum {
+  GetSetValueWithEditBuffer = 0,   // Get/Set question value from/to editbuffer in the storage.
+  GetSetValueWithBuffer,           // Get/Set question value from/to buffer in the storage.
+  GetSetValueWithHiiDriver,        // Get/Set question value from/to hii driver.
+  GetSetValueWithBothBuffer,       // Compare the editbuffer with buffer for this question, not use the question value.
+  GetSetValueWithMax               // Invalid value.
+} GET_SET_QUESTION_VALUE_WITH;
+
+extern EFI_HII_DATABASE_PROTOCOL         *mHiiDatabase;
+extern EFI_HII_CONFIG_ROUTING_PROTOCOL   *mHiiConfigRouting;
+extern EFI_DEVICE_PATH_FROM_TEXT_PROTOCOL *mPathFromText;
+extern EDKII_FORM_DISPLAY_ENGINE_PROTOCOL *mFormDisplay;
+
+extern BOOLEAN               gCallbackReconnect;
+extern BOOLEAN               gFlagReconnect;
+extern BOOLEAN               gResetRequired;
+extern BOOLEAN               gExitRequired;
+extern LIST_ENTRY            gBrowserFormSetList;
+extern LIST_ENTRY            gBrowserHotKeyList;
+extern BROWSER_SETTING_SCOPE gBrowserSettingScope;
+extern EXIT_HANDLER          ExitHandlerFunction;
+extern EFI_HII_HANDLE        mCurrentHiiHandle;
+extern SETUP_DRIVER_PRIVATE_DATA mPrivateData;
+//
+// Browser Global Strings
+//
+extern CHAR16            *gEmptyString;
+
+extern UI_MENU_SELECTION  *gCurrentSelection;
+extern BOOLEAN            mHiiPackageListUpdated;
+extern UINT16             mCurFakeQestId;
+extern BOOLEAN            mFinishRetrieveCall;
+
+//
+// Global Procedure Defines
+//
+#include "Expression.h"
+
+/**
+  Initialize the HII String Token to the correct values.
+
+**/
+VOID
+InitializeBrowserStrings (
+  VOID
+  );
+
+/**
+  Parse opcodes in the formset IFR binary.
+
+  @param  FormSet                Pointer of the FormSet data structure.
+
+  @retval EFI_SUCCESS            Opcode parse success.
+  @retval Other                  Opcode parse fail.
+
+**/
+EFI_STATUS
+ParseOpCodes (
+  IN FORM_BROWSER_FORMSET              *FormSet
+  );
+
+/**
+  Free resources allocated for a FormSet.
+
+  @param  FormSet                Pointer of the FormSet
+
+**/
+VOID
+DestroyFormSet (
+  IN OUT FORM_BROWSER_FORMSET  *FormSet
+  );
+
+
+/**
+  Create a new string in HII Package List.
+
+  @param  String                 The String to be added
+  @param  HiiHandle              The package list in the HII database to insert the
+                                 specified string.
+
+  @return The output string.
+
+**/
+EFI_STRING_ID
+NewString (
+  IN  CHAR16                   *String,
+  IN  EFI_HII_HANDLE           HiiHandle
+  );
+
+/**
+  Delete a string from HII Package List.
+
+  @param  StringId               Id of the string in HII database.
+  @param  HiiHandle              The HII package list handle.
+
+  @retval EFI_SUCCESS            The string was deleted successfully.
+
+**/
+EFI_STATUS
+DeleteString (
+  IN  EFI_STRING_ID            StringId,
+  IN  EFI_HII_HANDLE           HiiHandle
+  );
+
+/**
+  Get the string based on the StringId and HII Package List Handle.
+
+  @param  Token                  The String's ID.
+  @param  HiiHandle              The package list in the HII database to search for
+                                 the specified string.
+
+  @return The output string.
+
+**/
+CHAR16 *
+GetToken (
+  IN  EFI_STRING_ID                Token,
+  IN  EFI_HII_HANDLE               HiiHandle
+  );
+
+/**
+  Get Value for given Name from a NameValue Storage.
+
+  @param  Storage                The NameValue Storage.
+  @param  Name                   The Name.
+  @param  Value                  The retured Value.
+  @param  GetValueFrom           Where to get source value, from EditValue or Value.
+
+  @retval EFI_SUCCESS            Value found for given Name.
+  @retval EFI_NOT_FOUND          No such Name found in NameValue storage.
+
+**/
+EFI_STATUS
+GetValueByName (
+  IN BROWSER_STORAGE             *Storage,
+  IN CHAR16                      *Name,
+  IN OUT CHAR16                  **Value,
+  IN GET_SET_QUESTION_VALUE_WITH GetValueFrom
+  );
+
+/**
+  Set Value of given Name in a NameValue Storage.
+
+  @param  Storage                The NameValue Storage.
+  @param  Name                   The Name.
+  @param  Value                  The Value to set.
+  @param  SetValueTo             Whether update editValue or Value.
+  @param  ReturnNode             The node use the input name.
+
+  @retval EFI_SUCCESS            Value found for given Name.
+  @retval EFI_NOT_FOUND          No such Name found in NameValue storage.
+
+**/
+EFI_STATUS
+SetValueByName (
+  IN  BROWSER_STORAGE             *Storage,
+  IN  CHAR16                      *Name,
+  IN  CHAR16                      *Value,
+  IN  GET_SET_QUESTION_VALUE_WITH SetValueTo,
+  OUT NAME_VALUE_NODE             **ReturnNode
+  );
+
+/**
+  Validate whether this question's value has changed.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Question to be initialized.
+  @param  GetValueFrom           Where to get value, may from editbuffer, buffer or hii driver.
+
+  @retval TRUE                   Question's value has changed.
+  @retval FALSE                  Question's value has not changed
+
+**/
+BOOLEAN
+IsQuestionValueChanged (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      GetValueFrom
+  );
+
+/**
+  Validate the FormSet. If the formset is not validate, remove it from the list.
+
+  @param  FormSet                The input FormSet which need to validate.
+
+  @retval TRUE                   The handle is validate.
+  @retval FALSE                  The handle is invalidate.
+
+**/
+BOOLEAN
+ValidateFormSet (
+  FORM_BROWSER_FORMSET    *FormSet
+  );
+
+/**
+  Update the ValueChanged status for questions.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Default action.
+
+**/
+VOID 
+UpdateStatementStatus (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form, 
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  );
+
+/**
+  Get Question's current Value.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Question to be initialized.
+  @param  GetValueFrom           Where to get value, may from editbuffer, buffer or hii driver.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+GetQuestionValue (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      GetValueFrom
+  );
+
+/**
+  Save Question Value to edit copy(cached) or Storage(uncached).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               Pointer to the Question.
+  @param  SetValueTo             Update the question value to editbuffer , buffer or hii driver.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+SetQuestionValue (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN OUT FORM_BROWSER_STATEMENT       *Question,
+  IN GET_SET_QUESTION_VALUE_WITH      SetValueTo
+  );
+
+/**
+  Perform inconsistent check for a Form.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               The Question to be validated.
+  @param  Type                   Validation type: InConsistent or NoSubmit
+
+  @retval EFI_SUCCESS            Form validation pass.
+  @retval other                  Form validation failed.
+
+**/
+EFI_STATUS
+ValidateQuestion (
+  IN  FORM_BROWSER_FORMSET            *FormSet,
+  IN  FORM_BROWSER_FORM               *Form,
+  IN  FORM_BROWSER_STATEMENT          *Question,
+  IN  UINTN                           Type
+  );
+
+
+/**
+  Discard data based on the input setting scope (Form, FormSet or System).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Discard action.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+DiscardForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  );
+
+/**
+  Submit data based on the input Setting level (Form, FormSet or System).
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  SettingScope           Setting Scope for Submit action.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+SubmitForm (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN BROWSER_SETTING_SCOPE            SettingScope
+  );
+
+/**
+  Reset Question to its default value.
+
+  @param  FormSet                The form set.
+  @param  Form                   The form.
+  @param  Question               The question.
+  @param  DefaultId              The Class of the default.
+
+  @retval EFI_SUCCESS            Question is reset to default value.
+
+**/
+EFI_STATUS
+GetQuestionDefault (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN FORM_BROWSER_STATEMENT           *Question,
+  IN UINT16                           DefaultId
+  );
+
+/**
+  Get current setting of Questions.
+
+  @param  FormSet                FormSet data structure.
+
+**/
+VOID
+InitializeCurrentSetting (
+  IN OUT FORM_BROWSER_FORMSET             *FormSet
+  );
+
+/**
+  Initialize the internal data structure of a FormSet.
+
+  @param  Handle                 PackageList Handle
+  @param  FormSetGuid            GUID of a formset. If not specified (NULL or zero
+                                 GUID), take the first FormSet found in package
+                                 list.
+  @param  FormSet                FormSet data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_NOT_FOUND          The specified FormSet could not be found.
+
+**/
+EFI_STATUS
+InitializeFormSet (
+  IN  EFI_HII_HANDLE                   Handle,
+  IN OUT EFI_GUID                      *FormSetGuid,
+  OUT FORM_BROWSER_FORMSET             *FormSet                   
+  );
+
+/**
+  Reset Questions to their initial value or default value in a Form, Formset or System.
+
+  GetDefaultValueScope parameter decides which questions will reset 
+  to its default value.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  DefaultId              The Class of the default.
+  @param  SettingScope           Setting Scope for Default action.
+  @param  GetDefaultValueScope   Get default value scope.
+  @param  Storage                Get default value only for this storage.
+  @param  RetrieveValueFirst     Whether call the retrieve call back to
+                                 get the initial value before get default
+                                 value.
+  @param  SkipGetAltCfg          Whether skip the get altcfg string process.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+  @retval EFI_UNSUPPORTED        Unsupport SettingScope.
+
+**/
+EFI_STATUS
+ExtractDefault (
+  IN FORM_BROWSER_FORMSET             *FormSet,
+  IN FORM_BROWSER_FORM                *Form,
+  IN UINT16                           DefaultId,
+  IN BROWSER_SETTING_SCOPE            SettingScope,
+  IN BROWSER_GET_DEFAULT_VALUE        GetDefaultValueScope,
+  IN BROWSER_STORAGE                  *Storage,
+  IN BOOLEAN                          RetrieveValueFirst,
+  IN BOOLEAN                          SkipGetAltCfg
+  );
+
+/**
+  Initialize Question's Edit copy from Storage.
+
+  @param  Selection              Selection contains the information about 
+                                 the Selection, form and formset to be displayed.
+                                 Selection action may be updated in retrieve callback.
+                                 If Selection is NULL, only initialize Question value.
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+LoadFormConfig (
+  IN OUT UI_MENU_SELECTION    *Selection,
+  IN FORM_BROWSER_FORMSET     *FormSet,
+  IN FORM_BROWSER_FORM        *Form
+  );
+
+/**
+  Initialize Question's Edit copy from Storage for the whole Formset.
+
+  @param  Selection              Selection contains the information about 
+                                 the Selection, form and formset to be displayed.
+                                 Selection action may be updated in retrieve callback.
+                                 If Selection is NULL, only initialize Question value.
+  @param  FormSet                FormSet data structure.
+
+  @retval EFI_SUCCESS            The function completed successfully.
+
+**/
+EFI_STATUS
+LoadFormSetConfig (
+  IN OUT UI_MENU_SELECTION    *Selection,
+  IN     FORM_BROWSER_FORMSET *FormSet
+  );
+
+/**
+  Convert setting of Buffer Storage or NameValue Storage to <ConfigResp>.
+
+  @param  Storage                The Storage to be conveted.
+  @param  ConfigResp             The returned <ConfigResp>.
+  @param  ConfigRequest          The ConfigRequest string.
+  @param  GetEditBuf             Get the data from editbuffer or buffer.
+
+  @retval EFI_SUCCESS            Convert success.
+  @retval EFI_INVALID_PARAMETER  Incorrect storage type.
+
+**/
+EFI_STATUS
+StorageToConfigResp (
+  IN BROWSER_STORAGE         *Storage,
+  IN CHAR16                  **ConfigResp,
+  IN CHAR16                  *ConfigRequest,
+  IN BOOLEAN                 GetEditBuf
+  );
+
+/**
+  Convert <ConfigResp> to settings in Buffer Storage or NameValue Storage.
+
+  @param  Storage                The Storage to receive the settings.
+  @param  ConfigResp             The <ConfigResp> to be converted.
+
+  @retval EFI_SUCCESS            Convert success.
+  @retval EFI_INVALID_PARAMETER  Incorrect storage type.
+
+**/
+EFI_STATUS
+ConfigRespToStorage (
+  IN BROWSER_STORAGE         *Storage,
+  IN CHAR16                  *ConfigResp
+  );
+
+/**
+  Fill storage's edit copy with settings requested from Configuration Driver.
+
+  @param  FormSet                FormSet data structure.
+  @param  Storage                Buffer Storage.
+
+**/
+VOID
+LoadStorage (
+  IN FORM_BROWSER_FORMSET    *FormSet,
+  IN FORMSET_STORAGE         *Storage
+  );
+
+/**
+  Fetch the Ifr binary data of a FormSet.
+
+  @param  Handle                 PackageList Handle
+  @param  FormSetGuid            GUID of a formset. If not specified (NULL or zero
+                                 GUID), take the first FormSet found in package
+                                 list.
+  @param  BinaryLength           The length of the FormSet IFR binary.
+  @param  BinaryData             The buffer designed to receive the FormSet.
+
+  @retval EFI_SUCCESS            Buffer filled with the requested FormSet.
+                                 BufferLength was updated.
+  @retval EFI_INVALID_PARAMETER  The handle is unknown.
+  @retval EFI_NOT_FOUND          A form or FormSet on the requested handle cannot
+                                 be found with the requested FormId.
+
+**/
+EFI_STATUS
+GetIfrBinaryData (
+  IN  EFI_HII_HANDLE   Handle,
+  IN OUT EFI_GUID      *FormSetGuid,
+  OUT UINTN            *BinaryLength,
+  OUT UINT8            **BinaryData
+  );
+
+/**
+  Save globals used by previous call to SendForm(). SendForm() may be called from 
+  HiiConfigAccess.Callback(), this will cause SendForm() be reentried.
+  So, save globals of previous call to SendForm() and restore them upon exit.
+
+**/
+VOID
+SaveBrowserContext (
+  VOID
+  );
+
+/**
+  Restore globals used by previous call to SendForm().
+
+**/
+VOID
+RestoreBrowserContext (
+  VOID
+  );
+
+/**
+  This is the routine which an external caller uses to direct the browser
+  where to obtain it's information.
+
+
+  @param This            The Form Browser protocol instanse.
+  @param Handles         A pointer to an array of Handles.  If HandleCount > 1 we
+                         display a list of the formsets for the handles specified.
+  @param HandleCount     The number of Handles specified in Handle.
+  @param FormSetGuid     This field points to the EFI_GUID which must match the Guid
+                         field in the EFI_IFR_FORM_SET op-code for the specified
+                         forms-based package. If FormSetGuid is NULL, then this
+                         function will display the first found forms package.
+  @param FormId          This field specifies which EFI_IFR_FORM to render as the first
+                         displayable page. If this field has a value of 0x0000, then
+                         the forms browser will render the specified forms in their encoded order.
+                         ScreenDimenions - This allows the browser to be called so that it occupies a
+                         portion of the physical screen instead of dynamically determining the screen dimensions.
+                         ActionRequest   - Points to the action recommended by the form.
+  @param ScreenDimensions Points to recommended form dimensions, including any non-content area, in
+                          characters.
+  @param ActionRequest       Points to the action recommended by the form.
+
+  @retval  EFI_SUCCESS            The function completed successfully.
+  @retval  EFI_INVALID_PARAMETER  One of the parameters has an invalid value.
+  @retval  EFI_NOT_FOUND          No valid forms could be found to display.
+
+**/
+EFI_STATUS
+EFIAPI
+SendForm (
+  IN  CONST EFI_FORM_BROWSER2_PROTOCOL *This,
+  IN  EFI_HII_HANDLE                   *Handles,
+  IN  UINTN                            HandleCount,
+  IN  EFI_GUID                         *FormSetGuid, OPTIONAL
+  IN  UINT16                           FormId, OPTIONAL
+  IN  CONST EFI_SCREEN_DESCRIPTOR      *ScreenDimensions, OPTIONAL
+  OUT EFI_BROWSER_ACTION_REQUEST       *ActionRequest  OPTIONAL
+  );
+
+/**
+  This function is called by a callback handler to retrieve uncommitted state
+  data from the browser.
+
+  @param  This                   A pointer to the EFI_FORM_BROWSER2_PROTOCOL
+                                 instance.
+  @param  ResultsDataSize        A pointer to the size of the buffer associated
+                                 with ResultsData.
+  @param  ResultsData            A string returned from an IFR browser or
+                                 equivalent. The results string will have no
+                                 routing information in them.
+  @param  RetrieveData           A BOOLEAN field which allows an agent to retrieve
+                                 (if RetrieveData = TRUE) data from the uncommitted
+                                 browser state information or set (if RetrieveData
+                                 = FALSE) data in the uncommitted browser state
+                                 information.
+  @param  VariableGuid           An optional field to indicate the target variable
+                                 GUID name to use.
+  @param  VariableName           An optional field to indicate the target
+                                 human-readable variable name.
+
+  @retval EFI_SUCCESS            The results have been distributed or are awaiting
+                                 distribution.
+  @retval EFI_BUFFER_TOO_SMALL   The ResultsDataSize specified was too small to
+                                 contain the results data.
+
+**/
+EFI_STATUS
+EFIAPI
+BrowserCallback (
+  IN CONST EFI_FORM_BROWSER2_PROTOCOL  *This,
+  IN OUT UINTN                         *ResultsDataSize,
+  IN OUT EFI_STRING                    ResultsData,
+  IN BOOLEAN                           RetrieveData,
+  IN CONST EFI_GUID                    *VariableGuid, OPTIONAL
+  IN CONST CHAR16                      *VariableName  OPTIONAL
+  );
+
+/**
+  Find menu which will show next time.
+
+  @param Selection       On input, Selection tell setup browser the information
+                         about the Selection, form and formset to be displayed.
+                         On output, Selection return the screen item that is selected
+                         by user.
+  @param SettingLevel    Input Settting level, if it is FormLevel, just exit current form. 
+                         else, we need to exit current formset.
+  
+  @retval TRUE           Exit current form.
+  @retval FALSE          User press ESC and keep in current form.
+**/
+BOOLEAN
+FindNextMenu (
+  IN OUT UI_MENU_SELECTION        *Selection,
+  IN       BROWSER_SETTING_SCOPE  SettingLevel
+  );
+
+/**
+  check whether the form need to update the NV.
+
+  @param  Form                Form data structure.
+
+  @retval TRUE                   Need to update the NV.
+  @retval FALSE                  No need to update the NV.
+**/
+BOOLEAN
+IsNvUpdateRequiredForForm (
+  IN FORM_BROWSER_FORM    *Form
+  );
+
+/**
+  check whether the formset need to update the NV.
+
+  @param  FormSet                FormSet data structure.
+
+  @retval TRUE                   Need to update the NV.
+  @retval FALSE                  No need to update the NV.
+**/
+BOOLEAN
+IsNvUpdateRequiredForFormSet (
+  IN FORM_BROWSER_FORMSET  *FormSet
+  );
+
+/**
+  Call the call back function for the question and process the return action.
+
+  @param Selection             On input, Selection tell setup browser the information
+                               about the Selection, form and formset to be displayed.
+                               On output, Selection return the screen item that is selected
+                               by user.
+  @param FormSet               The formset this question belong to.
+  @param Form                  The form this question belong to.
+  @param Question              The Question which need to call.
+  @param Action                The action request.
+  @param SkipSaveOrDiscard     Whether skip save or discard action.
+
+  @retval EFI_SUCCESS          The call back function excutes successfully.
+  @return Other value if the call back function failed to excute.  
+**/
+EFI_STATUS 
+ProcessCallBackFunction (
+  IN OUT UI_MENU_SELECTION               *Selection,
+  IN     FORM_BROWSER_FORMSET            *FormSet,
+  IN     FORM_BROWSER_FORM               *Form,
+  IN     FORM_BROWSER_STATEMENT          *Question,
+  IN     EFI_BROWSER_ACTION              Action,
+  IN     BOOLEAN                         SkipSaveOrDiscard
+  );
+  
+/**
+  Call the retrieve type call back function for one question to get the initialize data.
+  
+  This function only used when in the initialize stage, because in this stage, the 
+  Selection->Form is not ready. For other case, use the ProcessCallBackFunction instead.
+
+  @param ConfigAccess          The config access protocol produced by the hii driver.
+  @param Statement             The Question which need to call.
+  @param FormSet               The formset this question belong to.
+
+  @retval EFI_SUCCESS          The call back function excutes successfully.
+  @return Other value if the call back function failed to excute.  
+**/
+EFI_STATUS 
+ProcessRetrieveForQuestion (
+  IN     EFI_HII_CONFIG_ACCESS_PROTOCOL  *ConfigAccess,
+  IN     FORM_BROWSER_STATEMENT          *Statement,
+  IN     FORM_BROWSER_FORMSET            *FormSet
+  );
+
+/**
+  Find the matched FormSet context in the backup maintain list based on HiiHandle.
+  
+  @param Handle  The Hii Handle.
+  
+  @return the found FormSet context. If no found, NULL will return.
+
+**/
+FORM_BROWSER_FORMSET * 
+GetFormSetFromHiiHandle (
+  EFI_HII_HANDLE Handle
+  );
+
+/**
+  Check whether the input HII handle is the FormSet that is being used.
+  
+  @param Handle  The Hii Handle.
+  
+  @retval TRUE   HII handle is being used.
+  @retval FALSE  HII handle is not being used.
+
+**/
+BOOLEAN
+IsHiiHandleInBrowserContext (
+  EFI_HII_HANDLE Handle
+  );
+
+/**
+  Configure what scope the hot key will impact.
+  All hot keys have the same scope. The mixed hot keys with the different level are not supported.
+  If no scope is set, the default scope will be FormSet level.
+  After all registered hot keys are removed, previous Scope can reset to another level.
+  
+  @param[in] Scope               Scope level to be set. 
+  
+  @retval EFI_SUCCESS            Scope is set correctly.
+  @retval EFI_INVALID_PARAMETER  Scope is not the valid value specified in BROWSER_SETTING_SCOPE. 
+  @retval EFI_UNSPPORTED         Scope level is different from current one that the registered hot keys have.
+
+**/
+EFI_STATUS
+EFIAPI
+SetScope (
+  IN BROWSER_SETTING_SCOPE Scope
+  );
+
+/**
+  Register the hot key with its browser action, or unregistered the hot key.
+  Only support hot key that is not printable character (control key, function key, etc.).
+  If the action value is zero, the hot key will be unregistered if it has been registered.
+  If the same hot key has been registered, the new action and help string will override the previous ones.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key. Its type is EFI_INPUT_KEY to 
+                         be supported by all ConsoleIn devices.
+  @param[in] Action      Action value that describes what action will be trigged when the hot key is pressed. 
+  @param[in] DefaultId   Specifies the type of defaults to retrieve, which is only for DEFAULT action.
+  @param[in] HelpString  Help string that describes the hot key information.
+                         Its value may be NULL for the unregistered hot key.
+  
+  @retval EFI_SUCCESS            Hot key is registered or unregistered.
+  @retval EFI_INVALID_PARAMETER  KeyData is NULL.
+  @retval EFI_NOT_FOUND          KeyData is not found to be unregistered.
+  @retval EFI_UNSUPPORTED        Key represents a printable character. It is conflicted with Browser.
+  @retval EFI_ALREADY_STARTED    Key already been registered for one hot key.
+**/
+EFI_STATUS
+EFIAPI
+RegisterHotKey (
+  IN EFI_INPUT_KEY *KeyData,
+  IN UINT32        Action,
+  IN UINT16        DefaultId,
+  IN EFI_STRING    HelpString OPTIONAL
+  );
+
+/**
+  Register Exit handler function. 
+  When more than one handler function is registered, the latter one will override the previous one. 
+  When NULL handler is specified, the previous Exit handler will be unregistered. 
+  
+  @param[in] Handler      Pointer to handler function. 
+
+**/
+VOID
+EFIAPI
+RegiserExitHandler (
+  IN EXIT_HANDLER Handler
+  );
+
+/**
+  
+  Check whether the browser data has been modified. 
+
+  @retval TRUE        Browser data is changed.
+  @retval FALSE       No browser data is changed.
+
+**/
+BOOLEAN 
+EFIAPI
+IsBrowserDataModified (
+  VOID
+  );
+
+/**
+  
+  Execute the action requested by the Action parameter. 
+
+  @param[in] Action     Execute the request action.
+  @param[in] DefaultId  The default Id info when need to load default value.
+
+  @retval EFI_SUCCESS              Execute the request action succss.
+  @retval EFI_INVALID_PARAMETER    The input action value is invalid.
+
+**/
+EFI_STATUS 
+EFIAPI
+ExecuteAction (
+  IN UINT32        Action,
+  IN UINT16        DefaultId
+  );
+
+/**
+  Create reminder to let user to choose save or discard the changed browser data.
+  Caller can use it to actively check the changed browser data.
+
+  @retval BROWSER_NO_CHANGES       No browser data is changed.
+  @retval BROWSER_SAVE_CHANGES     The changed browser data is saved.
+  @retval BROWSER_DISCARD_CHANGES  The changed browser data is discard.
+  @retval BROWSER_KEEP_CURRENT     Browser keep current changes.
+
+**/
+UINT32
+EFIAPI
+SaveReminder (
+  VOID
+  );
+
+/**
+  Check whether the Reset Required for the browser
+
+  @retval TRUE      Browser required to reset after exit.
+  @retval FALSE     Browser not need to reset after exit.
+
+**/
+BOOLEAN
+EFIAPI
+IsResetRequired (
+  VOID
+  );
+
+/**
+  Find the registered HotKey based on KeyData.
+  
+  @param[in] KeyData     A pointer to a buffer that describes the keystroke
+                         information for the hot key.
+
+  @return The registered HotKey context. If no found, NULL will return.
+**/
+BROWSER_HOT_KEY *
+GetHotKeyFromRegisterList (
+  IN EFI_INPUT_KEY *KeyData
+  );
+
+/**
+
+  Get FORM_BROWSER_STATEMENT from FORM_DISPLAY_ENGINE_STATEMENT based on the OpCode info.
+
+  @param DisplayStatement        The input FORM_DISPLAY_ENGINE_STATEMENT.
+
+  @retval FORM_BROWSER_STATEMENT  The return FORM_BROWSER_STATEMENT info.
+
+**/
+FORM_BROWSER_STATEMENT *
+GetBrowserStatement (
+  IN FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement
+  );
+  
+/**
+  Password may be stored as encrypted by Configuration Driver. When change a
+  password, user will be challenged with old password. To validate user input old
+  password, we will send the clear text to Configuration Driver via Callback().
+  Configuration driver is responsible to check the passed in password and return
+  the validation result. If validation pass, state machine in password Callback()
+  will transit from BROWSER_STATE_VALIDATE_PASSWORD to BROWSER_STATE_SET_PASSWORD.
+  After user type in new password twice, Callback() will be invoked to send the
+  new password to Configuration Driver.
+
+  @param  Selection              Pointer to UI_MENU_SELECTION.
+  @param  MenuOption             The MenuOption for this password Question.
+  @param  String                 The clear text of password.
+
+  @retval EFI_NOT_AVAILABLE_YET  Callback() request to terminate password input.
+  @return In state of BROWSER_STATE_VALIDATE_PASSWORD:
+  @retval EFI_SUCCESS            Password correct, Browser will prompt for new
+                                 password.
+  @retval EFI_NOT_READY          Password incorrect, Browser will show error
+                                 message.
+  @retval Other                  Browser will do nothing.
+  @return In state of BROWSER_STATE_SET_PASSWORD:
+  @retval EFI_SUCCESS            Set password success.
+  @retval Other                  Set password failed.
+
+**/
+EFI_STATUS
+PasswordCallback (
+  IN  UI_MENU_SELECTION           *Selection,
+  IN  FORM_BROWSER_STATEMENT      *Question,
+  IN  CHAR16                      *String
+  );
+
+/**
+  Display error message for invalid password.
+
+**/
+VOID
+PasswordInvalid (
+  VOID
+  );
+
+/**
+  The worker function that send the displays to the screen. On output,
+  the selection made by user is returned.
+
+  @param Selection       On input, Selection tell setup browser the information
+                         about the Selection, form and formset to be displayed.
+                         On output, Selection return the screen item that is selected
+                         by user.
+
+  @retval EFI_SUCCESS    The page is displayed successfully.
+  @return Other value if the page failed to be diplayed.
+
+**/
+EFI_STATUS
+SetupBrowser (
+  IN OUT UI_MENU_SELECTION    *Selection
+  );
+  
+/**
+  Free up the resource allocated for all strings required
+  by Setup Browser.
+
+**/
+VOID
+FreeBrowserStrings (
+  VOID
+  );
+
+/**
+  Create a menu with specified formset GUID and form ID, and add it as a child
+  of the given parent menu.
+
+  @param  HiiHandle              Hii handle related to this formset.
+  @param  FormSetGuid            The Formset Guid of menu to be added.
+  @param  FormId                 The Form ID of menu to be added.
+  @param  QuestionId             The question id of this menu to be added.
+
+  @return A pointer to the newly added menu or NULL if memory is insufficient.
+
+**/
+FORM_ENTRY_INFO *
+UiAddMenuList (
+  IN EFI_HII_HANDLE       HiiHandle,
+  IN EFI_GUID             *FormSetGuid,
+  IN UINT16               FormId,
+  IN UINT16               QuestionId
+  );
+
+/**
+  Search Menu with given FormSetGuid and FormId in all cached menu list.
+
+  @param  HiiHandle              HiiHandle for FormSet.
+  @param  FormSetGuid            The Formset GUID of the menu to search.
+  @param  FormId                 The Form ID of menu to search.
+
+  @return A pointer to menu found or NULL if not found.
+
+**/
+FORM_ENTRY_INFO *
+UiFindMenuList (
+  IN EFI_HII_HANDLE       HiiHandle, 
+  IN EFI_GUID             *FormSetGuid,
+  IN UINT16               FormId
+  );
+
+/**
+  Free Menu list linked list.
+
+  @param  MenuListHead    One Menu list point in the menu list.
+
+**/
+VOID
+UiFreeMenuList (
+  LIST_ENTRY   *MenuListHead
+  );
+
+/**
+  Find parent menu for current menu.
+
+  @param  CurrentMenu    Current Menu
+  @param  SettingLevel   Whether find parent menu in Form Level or Formset level.
+                         In form level, just find the parent menu; 
+                         In formset level, find the parent menu which has different
+                         formset guid value.
+
+  @retval   The parent menu for current menu.
+**/
+FORM_ENTRY_INFO *
+UiFindParentMenu (
+  IN FORM_ENTRY_INFO          *CurrentMenu,
+  IN BROWSER_SETTING_SCOPE    SettingLevel
+  );
+  
+/**
+  Validate the HiiHandle.
+
+  @param  HiiHandle              The input HiiHandle which need to validate.
+
+  @retval TRUE                   The handle is validate.
+  @retval FALSE                  The handle is invalidate.
+
+**/
+BOOLEAN
+ValidateHiiHandle (
+  EFI_HII_HANDLE          HiiHandle
+  );
+
+/**
+  Copy current Menu list to the new menu list.
+  
+  @param  NewMenuListHead        New create Menu list.
+  @param  CurrentMenuListHead    Current Menu list.
+
+**/
+VOID
+UiCopyMenuList (
+  OUT LIST_ENTRY   *NewMenuListHead,
+  IN  LIST_ENTRY   *CurrentMenuListHead
+  );
+
+/**
+  Search an Option of a Question by its value.
+
+  @param  Question               The Question
+  @param  OptionValue            Value for Option to be searched.
+
+  @retval Pointer                Pointer to the found Option.
+  @retval NULL                   Option not found.
+
+**/
+QUESTION_OPTION *
+ValueToOption (
+  IN FORM_BROWSER_STATEMENT   *Question,
+  IN EFI_HII_VALUE            *OptionValue
+  );
+/**
+  Return data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+
+  @retval Value                  The data to be returned
+
+**/
+UINT64
+GetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index
+  );
+
+/**
+  Set value of a data element in an Array by its Index.
+
+  @param  Array                  The data array.
+  @param  Type                   Type of the data in this array.
+  @param  Index                  Zero based index for data in this array.
+  @param  Value                  The value to be set.
+
+**/
+VOID
+SetArrayData (
+  IN VOID                     *Array,
+  IN UINT8                    Type,
+  IN UINTN                    Index,
+  IN UINT64                   Value
+  );
+
+/**
+   Compare two Hii value.
+ 
+   @param  Value1                 Expression value to compare on left-hand.
+   @param  Value2                 Expression value to compare on right-hand.
+   @param  Result                 Return value after compare.
+                                  retval 0                      Two operators equal.
+                                  return Positive value if Value1 is greater than Value2.
+                                  retval Negative value if Value1 is less than Value2.
+   @param  HiiHandle              Only required for string compare.
+ 
+   @retval other                  Could not perform compare on two values.
+   @retval EFI_SUCCESS            Compare the value success.
+ 
+**/
+EFI_STATUS
+CompareHiiValue (
+  IN  EFI_HII_VALUE   *Value1,
+  IN  EFI_HII_VALUE   *Value2,
+  OUT INTN            *Result,
+  IN  EFI_HII_HANDLE  HiiHandle OPTIONAL
+  );
+
+/**
+  Perform Password check. 
+  Passwork may be encrypted by driver that requires the specific check.
+  
+  @param  Form             Form where Password Statement is in.
+  @param  Statement        Password statement
+  @param  PasswordString   Password string to be checked. It may be NULL.
+                           NULL means to restore password.
+                           "" string can be used to checked whether old password does exist.
+  
+  @return Status     Status of Password check.
+**/
+EFI_STATUS
+EFIAPI
+PasswordCheck (
+  IN FORM_DISPLAY_ENGINE_FORM      *Form,
+  IN FORM_DISPLAY_ENGINE_STATEMENT *Statement,
+  IN EFI_STRING                    PasswordString  OPTIONAL
+  );
+
+/**
+
+  Get FORM_BROWSER_STATEMENT from FORM_DISPLAY_ENGINE_STATEMENT based on the OpCode info.
+
+  @param DisplayStatement        The input FORM_DISPLAY_ENGINE_STATEMENT.
+
+  @retval FORM_BROWSER_STATEMENT  The return FORM_BROWSER_STATEMENT info.
+
+**/
+FORM_BROWSER_STATEMENT *
+GetBrowserStatement (
+  IN FORM_DISPLAY_ENGINE_STATEMENT *DisplayStatement
+  );
+  
+/**
+
+  Initialize the Display form structure data.
+
+**/
+VOID
+InitializeDisplayFormData (
+  VOID
+  );
+
+
+/**
+  Base on the current formset info, clean the ConfigRequest string in browser storage.
+
+  @param  FormSet                Pointer of the FormSet
+
+**/
+VOID
+CleanBrowserStorage (
+  IN OUT FORM_BROWSER_FORMSET  *FormSet
+  );
+
+/**
+  Find HII Handle in the HII database associated with given Device Path.
+
+  If DevicePath is NULL, then ASSERT.
+
+  @param  DevicePath             Device Path associated with the HII package list
+                                 handle.
+  @param  FormsetGuid            The formset guid for this formset.
+
+  @retval Handle                 HII package list Handle associated with the Device
+                                        Path.
+  @retval NULL                   Hii Package list handle is not found.
+
+**/
+EFI_HII_HANDLE
+DevicePathToHiiHandle (
+  IN EFI_DEVICE_PATH_PROTOCOL   *DevicePath,
+  IN EFI_GUID                   *FormsetGuid
+  );
+
+/**
+  Adjust the config request info, remove the request elements which already in AllConfigRequest string.
+
+  @param  Storage                Form set Storage.
+  @param  Request                The input request string.
+  @param  RespString             Whether the input is ConfigRequest or ConfigResp format.
+
+  @retval TRUE                   Has element not covered by current used elements, need to continue to call ExtractConfig
+  @retval FALSE                  All elements covered by current used elements.
+
+**/
+BOOLEAN 
+ConfigRequestAdjust (
+  IN  BROWSER_STORAGE         *Storage,
+  IN  CHAR16                  *Request,
+  IN  BOOLEAN                 RespString
+  );
+
+/**
+  Perform question check. 
+  
+  If one question has more than one check, process form high priority to low.
+
+  @param  FormSet                FormSet data structure.
+  @param  Form                   Form data structure.
+  @param  Question               The Question to be validated.
+
+  @retval EFI_SUCCESS            Form validation pass.
+  @retval other                  Form validation failed.
+
+**/
+EFI_STATUS
+ValueChangedValidation (
+  IN  FORM_BROWSER_FORMSET            *FormSet,
+  IN  FORM_BROWSER_FORM               *Form,
+  IN  FORM_BROWSER_STATEMENT          *Question
+  );
+
+/**
+  Pop up the error info.
+
+  @param      BrowserStatus    The input browser status.
+  @param      HiiHandle        The HiiHandle for this error opcode.
+  @param      OpCode           The opcode use to get the erro info and timeout value.
+  @param      ErrorString      Error string used by BROWSER_NO_SUBMIT_IF.
+
+**/
+UINT32
+PopupErrorMessage (
+  IN UINT32                BrowserStatus,
+  IN EFI_HII_HANDLE        HiiHandle,
+  IN EFI_IFR_OP_HEADER     *OpCode, OPTIONAL
+  IN CHAR16                *ErrorString
+  );
+
+/**
+  Check whether the result is TRUE or FALSE.
+
+  For the EFI_HII_VALUE value type is numeric, return TRUE if the
+  value is not 0.
+
+  @param  Result             Input the result data.
+
+  @retval TRUE               The result is TRUE.
+  @retval FALSE              The result is FALSE.
+
+**/
+BOOLEAN
+IsTrue (
+  IN EFI_HII_VALUE     *Result
+  );
+
+/**
+  Get Formset_storage base on the input varstoreid info.
+
+  @param  FormSet                Pointer of the current FormSet.
+  @param  VarStoreId             Varstore ID info.
+
+  @return Pointer to a FORMSET_STORAGE data structure.
+
+**/
+FORMSET_STORAGE *
+GetFstStgFromVarId (
+  IN FORM_BROWSER_FORMSET  *FormSet,
+  IN EFI_VARSTORE_ID       VarStoreId
+  );
+
+/**
+  Get Formset_storage base on the input browser storage.
+
+  More than one formsets may share the same browser storage,
+  this function just get the first formset storage which
+  share the browser storage.
+
+  @param  Storage              browser storage info.
+
+  @return Pointer to a FORMSET_STORAGE data structure.
+  
+
+**/
+FORMSET_STORAGE *
+GetFstStgFromBrsStg (
+  IN BROWSER_STORAGE       *Storage
+  );
+
+/**
+  Reconnect the controller.
+
+  @param DriverHandle          The controller handle which need to be reconnect.
+
+  @retval   TRUE     do the reconnect behavior success.
+  @retval   FALSE    do the reconnect behavior failed.
+  
+**/
+BOOLEAN
+ReconnectController (
+  IN EFI_HANDLE   DriverHandle
+  );
+
+/**
+  Converts the unicode character of the string from uppercase to lowercase.
+  This is a internal function.
+
+  @param ConfigString  String to be converted
+
+**/
+VOID
+EFIAPI
+HiiToLower (
+  IN EFI_STRING  ConfigString
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowser.uni b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowser.uni
new file mode 100644
index 0000000000..0f9678ab13
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowser.uni
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserDxe.inf b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserDxe.inf
new file mode 100644
index 0000000000..6aa728db03
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserDxe.inf
@@ -0,0 +1,88 @@
+## @file
+# The DXE driver produces FORM BROWSER2 protocol defined in UEFI specification.
+#
+# It also produces FormBrowserEx(2) protocol to let user register the different Hot key service. 
+#
+# Copyright (c) 2007 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SetupBrowser
+  MODULE_UNI_FILE                = SetupBrowser.uni
+  FILE_GUID                      = EBf342FE-B1D3-4EF8-957C-8048606FF671
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 2.0
+  ENTRY_POINT                    = InitializeSetup
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Setup.c
+  Setup.h
+  IfrParse.c
+  Expression.c
+  Presentation.c
+  Expression.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  PrintLib
+  HiiLib
+  DevicePathLib
+  PcdLib
+  UefiLib
+
+[Guids]
+  gEfiIfrFrameworkGuid                          ## SOMETIMES_CONSUMES  ## GUID
+  gEfiHiiPlatformSetupFormsetGuid               ## SOMETIMES_CONSUMES  ## GUID
+  gEfiHiiStandardFormGuid                       ## SOMETIMES_CONSUMES  ## GUID
+  gZeroGuid                                     ## SOMETIMES_CONSUMES  ## GUID
+
+[Protocols]
+  gEfiHiiConfigAccessProtocolGuid               ## SOMETIMES_CONSUMES
+  gEfiFormBrowser2ProtocolGuid                  ## PRODUCES
+  gEdkiiFormBrowserEx2ProtocolGuid              ## PRODUCES
+  gEfiHiiConfigRoutingProtocolGuid              ## CONSUMES
+  gEfiHiiDatabaseProtocolGuid                   ## CONSUMES
+  gEfiUnicodeCollation2ProtocolGuid             ## SOMETIMES_CONSUMES
+  gEfiUserManagerProtocolGuid                   ## SOMETIMES_CONSUMES
+  gEfiDevicePathFromTextProtocolGuid            ## SOMETIMES_CONSUMES
+  ## CONSUMES
+  ## NOTIFY
+  gEdkiiFormDisplayEngineProtocolGuid
+  gEfiFormBrowserExProtocolGuid                 ## PRODUCES
+  gEfiRegularExpressionProtocolGuid             ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFrameworkCompatibilitySupport          ## CONSUMES
+
+[Depex]
+  gEfiHiiDatabaseProtocolGuid AND gEfiHiiConfigRoutingProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SetupBrowserExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserExtra.uni b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserExtra.uni
new file mode 100644
index 0000000000..53c88fe06b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SetupBrowserDxe/SetupBrowserExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.c b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.c
new file mode 100644
index 0000000000..f29c47b7b4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.c
@@ -0,0 +1,1459 @@
+/** @file
+  This code produces the Smbios protocol. It also responsible for constructing 
+  SMBIOS table into system table.
+  
+Copyright (c) 2009 - 2015, 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 "SmbiosDxe.h"
+
+//
+// Module Global:
+// Since this driver will only ever produce one instance of the
+// protocol you are not required to dynamically allocate the PrivateData.
+//
+SMBIOS_INSTANCE mPrivateData;
+
+UINTN mPreAllocatedPages      = 0;
+UINTN mPre64BitAllocatedPages = 0;
+
+//
+// Chassis for SMBIOS entry point structure that is to be installed into EFI system config table.
+//
+SMBIOS_TABLE_ENTRY_POINT *EntryPointStructure    = NULL;
+SMBIOS_TABLE_ENTRY_POINT EntryPointStructureData = {
+  //
+  // AnchorString
+  //
+  {
+    0x5f,
+    0x53,
+    0x4d,
+    0x5f
+  },
+  //
+  // EntryPointStructureChecksum,TO BE FILLED
+  //
+  0,
+  //
+  // EntryPointStructure Length
+  //
+  0x1f,
+  //
+  // MajorVersion
+  //
+  0,
+  //
+  // MinorVersion
+  //
+  0,
+  //
+  // MaxStructureSize, TO BE FILLED
+  //
+  0,
+  //
+  // EntryPointRevision
+  //
+  0,
+  //
+  // FormattedArea
+  //
+  {
+    0,
+    0,
+    0,
+    0,
+    0
+  },
+  //
+  // IntermediateAnchorString
+  //
+  {
+    0x5f,
+    0x44,
+    0x4d,
+    0x49,
+    0x5f
+  },
+  //
+  // IntermediateChecksum, TO BE FILLED
+  //
+  0,
+  //
+  // TableLength, TO BE FILLED
+  //
+  0,
+  //
+  // TableAddress, TO BE FILLED
+  //
+  0,
+  //
+  // NumberOfSmbiosStructures, TO BE FILLED
+  //
+  0,
+  //
+  // SmbiosBcdRevision
+  //
+  0
+};
+
+SMBIOS_TABLE_3_0_ENTRY_POINT *Smbios30EntryPointStructure    = NULL;
+SMBIOS_TABLE_3_0_ENTRY_POINT Smbios30EntryPointStructureData = {
+  //
+  // AnchorString _SM3_
+  //
+  {
+    0x5f,
+    0x53,
+    0x4d,
+    0x33,
+    0x5f,
+  },
+  //
+  // EntryPointStructureChecksum,TO BE FILLED
+  //
+  0,
+  //
+  // EntryPointLength
+  //
+  0x18,
+  //
+  // MajorVersion
+  //
+  0,
+  //
+  // MinorVersion
+  //
+  0,
+  //
+  // DocRev
+  //
+  0,
+  //
+  // EntryPointRevision
+  //
+  0x01,
+  //
+  // Reserved
+  //
+  0,
+  //
+  // TableMaximumSize,TO BE FILLED
+  //
+  0,
+  //
+  // TableAddress,TO BE FILLED
+  //
+  0
+};
+/**
+
+  Get the full size of SMBIOS structure including optional strings that follow the formatted structure.
+
+  @param This                   The EFI_SMBIOS_PROTOCOL instance.
+  @param Head                   Pointer to the beginning of SMBIOS structure.
+  @param Size                   The returned size.
+  @param NumberOfStrings        The returned number of optional strings that follow the formatted structure.
+
+  @retval EFI_SUCCESS           Size retured in Size.
+  @retval EFI_INVALID_PARAMETER Input SMBIOS structure mal-formed or Size is NULL.
+  
+**/
+EFI_STATUS
+EFIAPI
+GetSmbiosStructureSize (
+  IN   CONST EFI_SMBIOS_PROTOCOL        *This,
+  IN   EFI_SMBIOS_TABLE_HEADER          *Head,
+  OUT  UINTN                            *Size,
+  OUT  UINTN                            *NumberOfStrings
+  )
+{
+  UINTN  FullSize;
+  UINTN  StrLen;
+  UINTN  MaxLen;
+  INT8*  CharInStr;
+  
+  if (Size == NULL || NumberOfStrings == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  FullSize = Head->Length;
+  CharInStr = (INT8*)Head + Head->Length;
+  *Size = FullSize;
+  *NumberOfStrings = 0;
+  StrLen = 0;
+  //
+  // look for the two consecutive zeros, check the string limit by the way.
+  //
+  while (*CharInStr != 0 || *(CharInStr+1) != 0) { 
+    if (*CharInStr == 0) {
+      *Size += 1;
+      CharInStr++;
+    }
+
+    if (This->MajorVersion < 2 || (This->MajorVersion == 2 && This->MinorVersion < 7)){
+      MaxLen = SMBIOS_STRING_MAX_LENGTH;
+    } else if (This->MajorVersion < 3) {
+      //
+      // Reference SMBIOS 2.7, chapter 6.1.3, it will have no limit on the length of each individual text string.
+      // However, the length of the entire structure table (including all strings) must be reported
+      // in the Structure Table Length field of the SMBIOS Structure Table Entry Point,
+      // which is a WORD field limited to 65,535 bytes.
+      //
+      MaxLen = SMBIOS_TABLE_MAX_LENGTH;
+    } else {
+      //
+      // SMBIOS 3.0 defines the Structure table maximum size as DWORD field limited to 0xFFFFFFFF bytes.
+      // Locate the end of string as long as possible.
+      //
+      MaxLen = SMBIOS_3_0_TABLE_MAX_LENGTH;
+    }
+
+    for (StrLen = 0 ; StrLen < MaxLen; StrLen++) {
+      if (*(CharInStr+StrLen) == 0) {
+        break;
+      }
+    }
+
+    if (StrLen == MaxLen) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // forward the pointer
+    //
+    CharInStr += StrLen;
+    *Size += StrLen;
+    *NumberOfStrings += 1;
+  }
+
+  //
+  // count ending two zeros.
+  //
+  *Size += 2;
+  return EFI_SUCCESS;
+}
+
+/**
+
+  Determin whether an SmbiosHandle has already in use.
+
+  @param Head        Pointer to the beginning of SMBIOS structure.
+  @param Handle      A unique handle will be assigned to the SMBIOS record.
+
+  @retval TRUE       Smbios handle already in use.
+  @retval FALSE      Smbios handle is NOT used.
+  
+**/
+BOOLEAN
+EFIAPI
+CheckSmbiosHandleExistance (
+  IN  LIST_ENTRY           *Head,
+  IN  EFI_SMBIOS_HANDLE    Handle
+  )
+{
+  LIST_ENTRY              *Link;
+  SMBIOS_HANDLE_ENTRY     *HandleEntry;
+  
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    HandleEntry = SMBIOS_HANDLE_ENTRY_FROM_LINK(Link);
+    if (HandleEntry->SmbiosHandle == Handle) {
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+
+  Get the max SmbiosHandle that could be use.
+
+  @param  This           The EFI_SMBIOS_PROTOCOL instance.
+  @param  MaxHandle      The max handle that could be assigned to the SMBIOS record.
+
+**/
+VOID
+EFIAPI
+GetMaxSmbiosHandle (
+  IN CONST  EFI_SMBIOS_PROTOCOL   *This,
+  IN OUT    EFI_SMBIOS_HANDLE     *MaxHandle
+  ) 
+{
+  if (This->MajorVersion == 2 && This->MinorVersion == 0) {
+    *MaxHandle = 0xFFFE;
+  } else {
+    *MaxHandle = 0xFEFF;
+  }
+}
+
+/**
+
+  Get an SmbiosHandle that could use.
+
+  @param  This                   The EFI_SMBIOS_PROTOCOL instance.
+  @param  SmbiosHandle           A unique handle will be assigned to the SMBIOS record.
+
+  @retval EFI_SUCCESS            Smbios handle got.
+  @retval EFI_OUT_OF_RESOURCES   Smbios handle is NOT available.
+  
+**/
+EFI_STATUS
+EFIAPI
+GetAvailableSmbiosHandle (
+  IN CONST EFI_SMBIOS_PROTOCOL   *This,
+  IN OUT   EFI_SMBIOS_HANDLE     *Handle
+  )
+{
+  LIST_ENTRY              *Head;
+  SMBIOS_INSTANCE         *Private;
+  EFI_SMBIOS_HANDLE       MaxSmbiosHandle;
+  EFI_SMBIOS_HANDLE       AvailableHandle;
+
+  GetMaxSmbiosHandle(This, &MaxSmbiosHandle);
+
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  Head = &Private->AllocatedHandleListHead;
+  for (AvailableHandle = 0; AvailableHandle < MaxSmbiosHandle; AvailableHandle++) {
+    if (!CheckSmbiosHandleExistance(Head, AvailableHandle)) {
+      *Handle = AvailableHandle;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_OUT_OF_RESOURCES;
+}
+
+
+/**
+  Add an SMBIOS record.
+
+  @param  This                  The EFI_SMBIOS_PROTOCOL instance.
+  @param  ProducerHandle        The handle of the controller or driver associated with the SMBIOS information. NULL
+                                means no handle.
+  @param  SmbiosHandle          On entry, the handle of the SMBIOS record to add. If FFFEh, then a unique handle
+                                will be assigned to the SMBIOS record. If the SMBIOS handle is already in use,
+                                EFI_ALREADY_STARTED is returned and the SMBIOS record is not updated.
+  @param  Record                The data for the fixed portion of the SMBIOS record. The format of the record is
+                                determined by EFI_SMBIOS_TABLE_HEADER.Type. The size of the formatted area is defined 
+                                by EFI_SMBIOS_TABLE_HEADER.Length and either followed by a double-null (0x0000) or 
+                                a set of null terminated strings and a null.
+
+  @retval EFI_SUCCESS           Record was added.
+  @retval EFI_OUT_OF_RESOURCES  Record was not added due to lack of system resources.
+  @retval EFI_ALREADY_STARTED   The SmbiosHandle passed in was already in use.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosAdd (
+  IN CONST EFI_SMBIOS_PROTOCOL  *This,
+  IN EFI_HANDLE                 ProducerHandle, OPTIONAL
+  IN OUT EFI_SMBIOS_HANDLE      *SmbiosHandle,
+  IN EFI_SMBIOS_TABLE_HEADER    *Record
+  )
+{
+  VOID                        *Raw;
+  UINTN                       TotalSize;
+  UINTN                       RecordSize;
+  UINTN                       StructureSize;
+  UINTN                       NumberOfStrings;
+  EFI_STATUS                  Status;
+  LIST_ENTRY                  *Head;
+  SMBIOS_INSTANCE             *Private;
+  EFI_SMBIOS_ENTRY            *SmbiosEntry;
+  EFI_SMBIOS_HANDLE           MaxSmbiosHandle;
+  SMBIOS_HANDLE_ENTRY         *HandleEntry;
+  EFI_SMBIOS_RECORD_HEADER    *InternalRecord;
+  BOOLEAN                     Smbios32BitTable;
+  BOOLEAN                     Smbios64BitTable;
+  
+  if (SmbiosHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  //
+  // Check whether SmbiosHandle is already in use
+  //
+  Head = &Private->AllocatedHandleListHead;
+  if (*SmbiosHandle != SMBIOS_HANDLE_PI_RESERVED && CheckSmbiosHandleExistance(Head, *SmbiosHandle)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  //
+  // when SmbiosHandle is 0xFFFE, an available handle will be assigned
+  //
+  if (*SmbiosHandle == SMBIOS_HANDLE_PI_RESERVED) {
+    Status = GetAvailableSmbiosHandle(This, SmbiosHandle);
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+  } else {
+    //
+    // Check this handle validity
+    //
+    GetMaxSmbiosHandle(This, &MaxSmbiosHandle);
+    if (*SmbiosHandle > MaxSmbiosHandle) {
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // Calculate record size and string number
+  //
+  Status = GetSmbiosStructureSize(This, Record, &StructureSize, &NumberOfStrings);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  Smbios32BitTable = FALSE;
+  Smbios64BitTable = FALSE;
+  if ((This->MajorVersion < 0x3) || 
+      ((This->MajorVersion >= 0x3) && ((PcdGet32 (PcdSmbiosEntryPointProvideMethod) & BIT0) == BIT0))) {
+    //
+    // For SMBIOS 32-bit table, the length of the entire structure table (including all strings) must be reported
+    // in the Structure Table Length field of the SMBIOS Structure Table Entry Point,
+    // which is a WORD field limited to 65,535 bytes. So the max size of 32-bit table should not exceed 65,535 bytes.
+    //
+    if ((EntryPointStructure != NULL) && 
+        (EntryPointStructure->TableLength + StructureSize > SMBIOS_TABLE_MAX_LENGTH)) {
+      DEBUG ((EFI_D_INFO, "SmbiosAdd: Total length exceeds max 32-bit table length with type = %d size = 0x%x\n", Record->Type, StructureSize));
+    } else {
+      Smbios32BitTable = TRUE;
+      DEBUG ((EFI_D_INFO, "SmbiosAdd: Smbios type %d with size 0x%x is added to 32-bit table\n", Record->Type, StructureSize));
+    }
+  }
+  
+  //
+  // For SMBIOS 3.0, Structure table maximum size in Entry Point structure is DWORD field limited to 0xFFFFFFFF bytes.
+  //
+  if ((This->MajorVersion >= 0x3) && ((PcdGet32 (PcdSmbiosEntryPointProvideMethod) & BIT1) == BIT1)) {
+    //
+    // For SMBIOS 64-bit table, Structure table maximum size in SMBIOS 3.0 (64-bit) Entry Point
+    // is a DWORD field limited to 0xFFFFFFFF bytes. So the max size of 64-bit table should not exceed 0xFFFFFFFF bytes.
+    //
+    if ((Smbios30EntryPointStructure != NULL) && 
+        (Smbios30EntryPointStructure->TableMaximumSize + StructureSize > SMBIOS_3_0_TABLE_MAX_LENGTH)) {
+      DEBUG ((EFI_D_INFO, "SmbiosAdd: Total length exceeds max 64-bit table length with type = %d size = 0x%x\n", Record->Type, StructureSize));
+    } else {
+      DEBUG ((EFI_D_INFO, "SmbiosAdd: Smbios type %d with size 0x%x is added to 64-bit table\n", Record->Type, StructureSize));
+      Smbios64BitTable = TRUE;
+    }
+  }
+
+  if ((!Smbios32BitTable) && (!Smbios64BitTable)) {
+    //
+    // If both 32-bit and 64-bit table are not updated, quit
+    //
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Enter into critical section
+  //  
+  Status = EfiAcquireLockOrFail (&Private->DataLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  
+  RecordSize  = sizeof (EFI_SMBIOS_RECORD_HEADER) + StructureSize;
+  TotalSize   = sizeof (EFI_SMBIOS_ENTRY) + RecordSize;
+
+  //
+  // Allocate internal buffer
+  //
+  SmbiosEntry = AllocateZeroPool (TotalSize);
+  if (SmbiosEntry == NULL) {
+    EfiReleaseLock (&Private->DataLock);
+    return EFI_OUT_OF_RESOURCES;
+  }
+  HandleEntry = AllocateZeroPool (sizeof(SMBIOS_HANDLE_ENTRY));
+  if (HandleEntry == NULL) {
+    EfiReleaseLock (&Private->DataLock);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Build Handle Entry and insert into linked list
+  //
+  HandleEntry->Signature     = SMBIOS_HANDLE_ENTRY_SIGNATURE;
+  HandleEntry->SmbiosHandle  = *SmbiosHandle;
+  InsertTailList(&Private->AllocatedHandleListHead, &HandleEntry->Link);
+
+  InternalRecord  = (EFI_SMBIOS_RECORD_HEADER *) (SmbiosEntry + 1);
+  Raw     = (VOID *) (InternalRecord + 1);
+
+  //
+  // Build internal record Header
+  //
+  InternalRecord->Version     = EFI_SMBIOS_RECORD_HEADER_VERSION;
+  InternalRecord->HeaderSize  = (UINT16) sizeof (EFI_SMBIOS_RECORD_HEADER);
+  InternalRecord->RecordSize  = RecordSize;
+  InternalRecord->ProducerHandle = ProducerHandle;
+  InternalRecord->NumberOfStrings = NumberOfStrings;
+  //
+  // Insert record into the internal linked list
+  //
+  SmbiosEntry->Signature    = EFI_SMBIOS_ENTRY_SIGNATURE;
+  SmbiosEntry->RecordHeader = InternalRecord;
+  SmbiosEntry->RecordSize   = TotalSize;
+  SmbiosEntry->Smbios32BitTable = Smbios32BitTable;
+  SmbiosEntry->Smbios64BitTable = Smbios64BitTable;
+  InsertTailList (&Private->DataListHead, &SmbiosEntry->Link);
+
+  CopyMem (Raw, Record, StructureSize);
+  ((EFI_SMBIOS_TABLE_HEADER*)Raw)->Handle = *SmbiosHandle;
+
+  //
+  // Some UEFI drivers (such as network) need some information in SMBIOS table.
+  // Here we create SMBIOS table and publish it in
+  // configuration table, so other UEFI drivers can get SMBIOS table from
+  // configuration table without depending on PI SMBIOS protocol.
+  //
+  SmbiosTableConstruction (Smbios32BitTable, Smbios64BitTable);
+  
+  //
+  // Leave critical section
+  //
+  EfiReleaseLock (&Private->DataLock);
+  return EFI_SUCCESS;
+}
+
+/**
+  Update the string associated with an existing SMBIOS record.
+
+  @param  This                  The EFI_SMBIOS_PROTOCOL instance.
+  @param  SmbiosHandle          SMBIOS Handle of structure that will have its string updated.
+  @param  StringNumber          The non-zero string number of the string to update
+  @param  String                Update the StringNumber string with String.
+
+  @retval EFI_SUCCESS           SmbiosHandle had its StringNumber String updated.
+  @retval EFI_INVALID_PARAMETER SmbiosHandle does not exist.
+  @retval EFI_UNSUPPORTED       String was not added because it is longer than the SMBIOS Table supports.
+  @retval EFI_NOT_FOUND         The StringNumber.is not valid for this SMBIOS record.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosUpdateString (
+  IN CONST EFI_SMBIOS_PROTOCOL      *This,
+  IN EFI_SMBIOS_HANDLE              *SmbiosHandle,
+  IN UINTN                          *StringNumber,
+  IN CHAR8                          *String
+  )
+{
+  UINTN                     InputStrLen;
+  UINTN                     TargetStrLen;
+  UINTN                     StrIndex;
+  UINTN                     TargetStrOffset;
+  UINTN                     NewEntrySize;
+  CHAR8                     *StrStart;
+  VOID                      *Raw;
+  LIST_ENTRY                *Link;
+  LIST_ENTRY                *Head;
+  EFI_STATUS                Status;
+  SMBIOS_INSTANCE           *Private;
+  EFI_SMBIOS_ENTRY          *SmbiosEntry;
+  EFI_SMBIOS_ENTRY          *ResizedSmbiosEntry;
+  EFI_SMBIOS_HANDLE         MaxSmbiosHandle;
+  EFI_SMBIOS_TABLE_HEADER   *Record;
+  EFI_SMBIOS_RECORD_HEADER  *InternalRecord;
+  
+  //
+  // Check args validity
+  //
+  GetMaxSmbiosHandle(This, &MaxSmbiosHandle);
+
+  if (*SmbiosHandle > MaxSmbiosHandle) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (String == NULL) {
+    return EFI_ABORTED;
+  }
+
+  if (*StringNumber == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  InputStrLen = AsciiStrLen(String);
+
+  if (This->MajorVersion < 2 || (This->MajorVersion == 2 && This->MinorVersion < 7)) {
+    if (InputStrLen > SMBIOS_STRING_MAX_LENGTH) {
+      return EFI_UNSUPPORTED;
+    }
+  } else if (This->MajorVersion < 3) {
+    //
+    // Reference SMBIOS 2.7, chapter 6.1.3, it will have no limit on the length of each individual text string.
+    // However, the length of the entire structure table (including all strings) must be reported 
+    // in the Structure Table Length field of the SMBIOS Structure Table Entry Point,
+    // which is a WORD field limited to 65,535 bytes.
+    //
+    if (InputStrLen > SMBIOS_TABLE_MAX_LENGTH) {
+      return EFI_UNSUPPORTED;
+    }
+  } else {
+    if (InputStrLen > SMBIOS_3_0_TABLE_MAX_LENGTH) {
+      //
+      // SMBIOS 3.0 defines the Structure table maximum size as DWORD field limited to 0xFFFFFFFF bytes.
+      // The input string length should not exceed 0xFFFFFFFF bytes.
+      // 
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  //
+  // Enter into critical section
+  //  
+  Status = EfiAcquireLockOrFail (&Private->DataLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Head = &Private->DataListHead;
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    SmbiosEntry = SMBIOS_ENTRY_FROM_LINK(Link);
+    Record = (EFI_SMBIOS_TABLE_HEADER*)(SmbiosEntry->RecordHeader + 1);
+
+    if (Record->Handle == *SmbiosHandle) {
+      //
+      // Find out the specified SMBIOS record
+      //
+      if (*StringNumber > SmbiosEntry->RecordHeader->NumberOfStrings) {
+        EfiReleaseLock (&Private->DataLock);
+        return EFI_NOT_FOUND;
+      }
+      //
+      // Point to unformed string section
+      //
+      StrStart = (CHAR8 *) Record + Record->Length;
+     
+      for (StrIndex = 1, TargetStrOffset = 0; StrIndex < *StringNumber; StrStart++, TargetStrOffset++) {
+        //
+        // A string ends in 00h
+        //
+        if (*StrStart == 0) {
+          StrIndex++;
+        }
+        
+        //
+        // String section ends in double-null (0000h)
+        //
+        if (*StrStart == 0 && *(StrStart + 1) == 0) {
+          EfiReleaseLock (&Private->DataLock);
+          return EFI_NOT_FOUND;
+        } 
+      }
+
+      if (*StrStart == 0) {
+        StrStart++;
+        TargetStrOffset++;
+      }
+      
+      //
+      // Now we get the string target
+      //
+      TargetStrLen = AsciiStrLen(StrStart);
+      if (InputStrLen == TargetStrLen) {
+        AsciiStrCpyS(StrStart, TargetStrLen + 1, String);
+        //
+        // Some UEFI drivers (such as network) need some information in SMBIOS table.
+        // Here we create SMBIOS table and publish it in
+        // configuration table, so other UEFI drivers can get SMBIOS table from
+        // configuration table without depending on PI SMBIOS protocol.
+        //
+        SmbiosTableConstruction (SmbiosEntry->Smbios32BitTable, SmbiosEntry->Smbios64BitTable);
+        EfiReleaseLock (&Private->DataLock);
+        return EFI_SUCCESS;
+      }
+      
+      SmbiosEntry->Smbios32BitTable = FALSE;
+      SmbiosEntry->Smbios64BitTable = FALSE;
+      if ((This->MajorVersion < 0x3) || 
+          ((This->MajorVersion >= 0x3) && ((PcdGet32 (PcdSmbiosEntryPointProvideMethod) & BIT0) == BIT0))) {
+        //
+        // 32-bit table is produced, check the valid length.
+        //
+        if ((EntryPointStructure != NULL) && 
+            (EntryPointStructure->TableLength + InputStrLen - TargetStrLen > SMBIOS_TABLE_MAX_LENGTH)) {
+          //
+          // The length of the entire structure table (including all strings) must be reported
+          // in the Structure Table Length field of the SMBIOS Structure Table Entry Point,
+          // which is a WORD field limited to 65,535 bytes.
+          //
+          DEBUG ((EFI_D_INFO, "SmbiosUpdateString: Total length exceeds max 32-bit table length\n"));
+        } else {
+          DEBUG ((EFI_D_INFO, "SmbiosUpdateString: New smbios record add to 32-bit table\n"));
+          SmbiosEntry->Smbios32BitTable = TRUE;
+        }
+      } 
+
+      if ((This->MajorVersion >= 0x3) && ((PcdGet32 (PcdSmbiosEntryPointProvideMethod) & BIT1) == BIT1)) {
+        //
+        // 64-bit table is produced, check the valid length.
+        //
+        if ((Smbios30EntryPointStructure != NULL) && 
+            (Smbios30EntryPointStructure->TableMaximumSize + InputStrLen - TargetStrLen > SMBIOS_3_0_TABLE_MAX_LENGTH)) {
+          DEBUG ((EFI_D_INFO, "SmbiosUpdateString: Total length exceeds max 64-bit table length\n"));
+        } else {
+          DEBUG ((EFI_D_INFO, "SmbiosUpdateString: New smbios record add to 64-bit table\n"));
+          SmbiosEntry->Smbios64BitTable = TRUE;
+        }
+      }
+
+      if ((!SmbiosEntry->Smbios32BitTable) && (!SmbiosEntry->Smbios64BitTable)) {
+        EfiReleaseLock (&Private->DataLock);
+        return EFI_UNSUPPORTED;
+      }
+
+      //
+      // Original string buffer size is not exactly match input string length.
+      // Re-allocate buffer is needed.
+      //
+      NewEntrySize = SmbiosEntry->RecordSize + InputStrLen - TargetStrLen;
+      ResizedSmbiosEntry = AllocateZeroPool (NewEntrySize);
+
+      if (ResizedSmbiosEntry == NULL) {
+        EfiReleaseLock (&Private->DataLock);
+        return EFI_OUT_OF_RESOURCES;
+      }
+
+      InternalRecord  = (EFI_SMBIOS_RECORD_HEADER *) (ResizedSmbiosEntry + 1);
+      Raw     = (VOID *) (InternalRecord + 1);
+
+      //
+      // Build internal record Header
+      //
+      InternalRecord->Version     = EFI_SMBIOS_RECORD_HEADER_VERSION;
+      InternalRecord->HeaderSize  = (UINT16) sizeof (EFI_SMBIOS_RECORD_HEADER);
+      InternalRecord->RecordSize  = SmbiosEntry->RecordHeader->RecordSize + InputStrLen - TargetStrLen;
+      InternalRecord->ProducerHandle = SmbiosEntry->RecordHeader->ProducerHandle;
+      InternalRecord->NumberOfStrings = SmbiosEntry->RecordHeader->NumberOfStrings;
+
+      //
+      // Copy SMBIOS structure and optional strings.
+      //
+      CopyMem (Raw, SmbiosEntry->RecordHeader + 1, Record->Length + TargetStrOffset);
+      CopyMem ((VOID*)((UINTN)Raw + Record->Length + TargetStrOffset), String, InputStrLen + 1);
+      CopyMem ((CHAR8*)((UINTN)Raw + Record->Length + TargetStrOffset + InputStrLen + 1),
+               (CHAR8*)Record + Record->Length + TargetStrOffset + TargetStrLen + 1,
+               SmbiosEntry->RecordHeader->RecordSize - sizeof (EFI_SMBIOS_RECORD_HEADER) - Record->Length - TargetStrOffset - TargetStrLen - 1);
+
+      //
+      // Insert new record
+      //
+      ResizedSmbiosEntry->Signature    = EFI_SMBIOS_ENTRY_SIGNATURE;
+      ResizedSmbiosEntry->RecordHeader = InternalRecord;
+      ResizedSmbiosEntry->RecordSize   = NewEntrySize;
+      ResizedSmbiosEntry->Smbios32BitTable = SmbiosEntry->Smbios32BitTable;
+      ResizedSmbiosEntry->Smbios64BitTable = SmbiosEntry->Smbios64BitTable;
+      InsertTailList (Link->ForwardLink, &ResizedSmbiosEntry->Link);
+
+      //
+      // Remove old record
+      //
+      RemoveEntryList(Link);
+      FreePool(SmbiosEntry);
+      //
+      // Some UEFI drivers (such as network) need some information in SMBIOS table.
+      // Here we create SMBIOS table and publish it in
+      // configuration table, so other UEFI drivers can get SMBIOS table from
+      // configuration table without depending on PI SMBIOS protocol.
+      //
+      SmbiosTableConstruction (ResizedSmbiosEntry->Smbios32BitTable, ResizedSmbiosEntry->Smbios64BitTable);
+      EfiReleaseLock (&Private->DataLock);
+      return EFI_SUCCESS;
+    }
+  }
+
+  EfiReleaseLock (&Private->DataLock);
+  return EFI_INVALID_PARAMETER;
+}
+
+/**
+  Remove an SMBIOS record.
+
+  @param  This                  The EFI_SMBIOS_PROTOCOL instance.
+  @param  SmbiosHandle          The handle of the SMBIOS record to remove.
+
+  @retval EFI_SUCCESS           SMBIOS record was removed.
+  @retval EFI_INVALID_PARAMETER SmbiosHandle does not specify a valid SMBIOS record.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosRemove (
+  IN CONST EFI_SMBIOS_PROTOCOL   *This,
+  IN EFI_SMBIOS_HANDLE           SmbiosHandle
+  )
+{
+  LIST_ENTRY                 *Link;
+  LIST_ENTRY                 *Head;
+  EFI_STATUS                 Status;
+  EFI_SMBIOS_HANDLE          MaxSmbiosHandle;
+  SMBIOS_INSTANCE            *Private;
+  EFI_SMBIOS_ENTRY           *SmbiosEntry;
+  SMBIOS_HANDLE_ENTRY        *HandleEntry;
+  EFI_SMBIOS_TABLE_HEADER    *Record;
+
+  //
+  // Check args validity
+  //
+  GetMaxSmbiosHandle(This, &MaxSmbiosHandle);
+
+  if (SmbiosHandle > MaxSmbiosHandle) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  //
+  // Enter into critical section
+  //  
+  Status = EfiAcquireLockOrFail (&Private->DataLock);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Head = &Private->DataListHead;
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    SmbiosEntry = SMBIOS_ENTRY_FROM_LINK(Link);
+    Record = (EFI_SMBIOS_TABLE_HEADER*)(SmbiosEntry->RecordHeader + 1);
+    if (Record->Handle == SmbiosHandle) {
+      //
+      // Remove specified smobios record from DataList
+      //
+      RemoveEntryList(Link);
+      // 
+      // Remove this handle from AllocatedHandleList
+      //
+      Head = &Private->AllocatedHandleListHead;
+      for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+        HandleEntry = SMBIOS_HANDLE_ENTRY_FROM_LINK(Link);
+        if (HandleEntry->SmbiosHandle == SmbiosHandle) {
+          RemoveEntryList(Link);
+          FreePool(HandleEntry);
+          break;
+        }
+      }
+      //
+      // Some UEFI drivers (such as network) need some information in SMBIOS table.
+      // Here we create SMBIOS table and publish it in
+      // configuration table, so other UEFI drivers can get SMBIOS table from
+      // configuration table without depending on PI SMBIOS protocol.
+      //
+      if (SmbiosEntry->Smbios32BitTable) {
+        DEBUG ((EFI_D_INFO, "SmbiosRemove: remove from 32-bit table\n"));
+      }
+      if (SmbiosEntry->Smbios64BitTable) {
+        DEBUG ((EFI_D_INFO, "SmbiosRemove: remove from 64-bit table\n"));
+      }
+      //
+      // Update the whole SMBIOS table again based on which table the removed SMBIOS record is in.
+      //
+      SmbiosTableConstruction (SmbiosEntry->Smbios32BitTable, SmbiosEntry->Smbios64BitTable);
+      FreePool(SmbiosEntry);
+      EfiReleaseLock (&Private->DataLock);
+      return EFI_SUCCESS;
+    }
+  }
+
+  //
+  // Leave critical section
+  //
+  EfiReleaseLock (&Private->DataLock);
+  return EFI_INVALID_PARAMETER;
+  
+}
+
+/**
+  Allow the caller to discover all or some of the SMBIOS records.
+
+  @param  This                  The EFI_SMBIOS_PROTOCOL instance.
+  @param  SmbiosHandle          On entry, points to the previous handle of the SMBIOS record. On exit, points to the
+                                next SMBIOS record handle. If it is FFFEh on entry, then the first SMBIOS record
+                                handle will be returned. If it returns FFFEh on exit, then there are no more SMBIOS records.
+  @param  Type                  On entry it means return the next SMBIOS record of type Type. If a NULL is passed in 
+                                this functionally it ignored. Type is not modified by the GetNext() function.
+  @param  Record                On exit, points to the SMBIOS Record consisting of the formatted area followed by
+                                the unformatted area. The unformatted area optionally contains text strings.
+  @param  ProducerHandle        On exit, points to the ProducerHandle registered by Add(). If no ProducerHandle was passed into Add() NULL is returned. 
+                                If a NULL pointer is passed in no data will be returned 
+                                
+  @retval EFI_SUCCESS           SMBIOS record information was successfully returned in Record.
+  @retval EFI_NOT_FOUND         The SMBIOS record with SmbiosHandle was the last available record.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosGetNext (
+  IN CONST EFI_SMBIOS_PROTOCOL      *This,
+  IN OUT EFI_SMBIOS_HANDLE          *SmbiosHandle,
+  IN EFI_SMBIOS_TYPE                *Type,          OPTIONAL
+  OUT EFI_SMBIOS_TABLE_HEADER       **Record,
+  OUT EFI_HANDLE                    *ProducerHandle OPTIONAL
+  )
+{
+  BOOLEAN                  StartPointFound;
+  LIST_ENTRY               *Link;
+  LIST_ENTRY               *Head;
+  SMBIOS_INSTANCE          *Private;
+  EFI_SMBIOS_ENTRY         *SmbiosEntry;
+  EFI_SMBIOS_TABLE_HEADER  *SmbiosTableHeader;
+
+  if (SmbiosHandle == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  StartPointFound = FALSE;
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  Head = &Private->DataListHead;
+  for (Link = Head->ForwardLink; Link != Head; Link = Link->ForwardLink) {
+    SmbiosEntry = SMBIOS_ENTRY_FROM_LINK(Link);
+    SmbiosTableHeader = (EFI_SMBIOS_TABLE_HEADER*)(SmbiosEntry->RecordHeader + 1); 
+
+    //
+    // If SmbiosHandle is 0xFFFE, the first matched SMBIOS record handle will be returned
+    //
+    if (*SmbiosHandle == SMBIOS_HANDLE_PI_RESERVED) {
+      if ((Type != NULL) && (*Type != SmbiosTableHeader->Type)) {
+        continue;  
+      }
+
+      *SmbiosHandle = SmbiosTableHeader->Handle;
+      *Record =SmbiosTableHeader;
+      if (ProducerHandle != NULL) {
+        *ProducerHandle = SmbiosEntry->RecordHeader->ProducerHandle;
+      }
+      return EFI_SUCCESS;
+    }
+
+    //
+    // Start this round search from the next SMBIOS handle
+    //
+    if (!StartPointFound && (*SmbiosHandle == SmbiosTableHeader->Handle)) {
+      StartPointFound = TRUE;
+      continue;
+    }
+
+    if (StartPointFound) {
+      if ((Type != NULL) && (*Type != SmbiosTableHeader->Type)) {
+        continue; 
+      }
+      
+      *SmbiosHandle = SmbiosTableHeader->Handle;
+      *Record = SmbiosTableHeader; 
+      if (ProducerHandle != NULL) {
+        *ProducerHandle = SmbiosEntry->RecordHeader->ProducerHandle;
+      }
+
+      return EFI_SUCCESS;   
+    }
+  }
+
+  *SmbiosHandle = SMBIOS_HANDLE_PI_RESERVED;
+  return EFI_NOT_FOUND;
+  
+}
+
+/**
+  Allow the caller to discover all of the SMBIOS records.
+
+  @param  This                  The EFI_SMBIOS_PROTOCOL instance.
+  @param  CurrentSmbiosEntry    On exit, points to the SMBIOS entry on the list which includes the returned SMBIOS record information. 
+                                If *CurrentSmbiosEntry is NULL on entry, then the first SMBIOS entry on the list will be returned. 
+  @param  Record                On exit, points to the SMBIOS Record consisting of the formatted area followed by
+                                the unformatted area. The unformatted area optionally contains text strings.
+                                
+  @retval EFI_SUCCESS           SMBIOS record information was successfully returned in Record.
+                                *CurrentSmbiosEntry points to the SMBIOS entry which includes the returned SMBIOS record information.
+  @retval EFI_NOT_FOUND         There is no more SMBIOS entry.
+
+**/
+EFI_STATUS
+EFIAPI
+GetNextSmbiosRecord (
+  IN CONST EFI_SMBIOS_PROTOCOL         *This,
+  IN OUT EFI_SMBIOS_ENTRY              **CurrentSmbiosEntry,
+  OUT EFI_SMBIOS_TABLE_HEADER          **Record
+  )
+{
+  LIST_ENTRY               *Link;
+  LIST_ENTRY               *Head;
+  SMBIOS_INSTANCE          *Private;
+  EFI_SMBIOS_ENTRY         *SmbiosEntry;
+  EFI_SMBIOS_TABLE_HEADER  *SmbiosTableHeader;
+
+  Private = SMBIOS_INSTANCE_FROM_THIS (This);
+  if (*CurrentSmbiosEntry == NULL) {
+    //
+    // Get the beginning of SMBIOS entry.
+    //
+    Head = &Private->DataListHead;
+  } else {
+    //
+    // Get previous SMBIOS entry and make it as start point.
+    //
+    Head = &(*CurrentSmbiosEntry)->Link;
+  }
+  
+  Link  = Head->ForwardLink;
+  
+  if (Link == &Private->DataListHead) {
+    //
+    // If no more SMBIOS entry in the list, return not found.
+    //
+    return EFI_NOT_FOUND;
+  }
+  
+  SmbiosEntry = SMBIOS_ENTRY_FROM_LINK(Link);
+  SmbiosTableHeader = (EFI_SMBIOS_TABLE_HEADER*)(SmbiosEntry->RecordHeader + 1);
+  *Record = SmbiosTableHeader; 
+  *CurrentSmbiosEntry = SmbiosEntry;
+  return EFI_SUCCESS;   
+}
+
+/**
+  Assembles SMBIOS table from the SMBIOS protocol. Produce Table
+  Entry Point and return the pointer to it.
+  
+  @param  TableEntryPointStructure   On exit, points to the SMBIOS entrypoint structure.
+                                
+  @retval EFI_SUCCESS                Structure created sucessfully.
+  @retval EFI_OUT_OF_RESOURCES       No enough memory.
+  
+**/
+EFI_STATUS
+EFIAPI
+SmbiosCreateTable (
+  OUT VOID    **TableEntryPointStructure
+  )
+{
+  UINT8                           *BufferPointer;
+  UINTN                           RecordSize;
+  UINTN                           NumOfStr;
+  EFI_STATUS                      Status;
+  EFI_SMBIOS_HANDLE               SmbiosHandle;
+  EFI_SMBIOS_PROTOCOL             *SmbiosProtocol;
+  EFI_PHYSICAL_ADDRESS            PhysicalAddress;
+  EFI_SMBIOS_TABLE_HEADER         *SmbiosRecord;
+  EFI_SMBIOS_TABLE_END_STRUCTURE  EndStructure;
+  EFI_SMBIOS_ENTRY                *CurrentSmbiosEntry;
+  
+  Status            = EFI_SUCCESS;
+  BufferPointer     = NULL;
+
+  if (EntryPointStructure == NULL) {
+    //
+    // Initialize the EntryPointStructure with initial values.
+    // It should be done only once.
+    // Allocate memory (below 4GB).
+    //
+    DEBUG ((EFI_D_INFO, "SmbiosCreateTable: Initialize 32-bit entry point structure\n"));
+    EntryPointStructureData.MajorVersion  = mPrivateData.Smbios.MajorVersion;
+    EntryPointStructureData.MinorVersion  = mPrivateData.Smbios.MinorVersion;
+    EntryPointStructureData.SmbiosBcdRevision = (UINT8) ((PcdGet16 (PcdSmbiosVersion) >> 4) & 0xf0) | (UINT8) (PcdGet16 (PcdSmbiosVersion) & 0x0f);
+    PhysicalAddress = 0xffffffff;
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiRuntimeServicesData,
+                    EFI_SIZE_TO_PAGES (sizeof (SMBIOS_TABLE_ENTRY_POINT)),
+                    &PhysicalAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SmbiosCreateTable () could not allocate EntryPointStructure < 4GB\n"));
+      Status = gBS->AllocatePages (
+                      AllocateAnyPages,
+                      EfiRuntimeServicesData,
+                      EFI_SIZE_TO_PAGES (sizeof (SMBIOS_TABLE_ENTRY_POINT)),
+                      &PhysicalAddress
+                      );
+     if (EFI_ERROR (Status)) {
+        return EFI_OUT_OF_RESOURCES;
+      }
+    }
+    
+    EntryPointStructure = (SMBIOS_TABLE_ENTRY_POINT *) (UINTN) PhysicalAddress;
+    
+    CopyMem (
+      EntryPointStructure,
+      &EntryPointStructureData,
+      sizeof (SMBIOS_TABLE_ENTRY_POINT)
+      );
+  }
+
+  //
+  // Get Smbios protocol to traverse SMBIOS records.
+  //
+  SmbiosProtocol = &mPrivateData.Smbios;
+
+  //
+  // Make some statistics about all the structures
+  //
+  EntryPointStructure->NumberOfSmbiosStructures = 0;
+  EntryPointStructure->TableLength              = 0;
+  EntryPointStructure->MaxStructureSize         = 0;
+
+  //
+  // Calculate EPS Table Length
+  //
+  CurrentSmbiosEntry = NULL;
+  do {
+    Status = GetNextSmbiosRecord (SmbiosProtocol, &CurrentSmbiosEntry, &SmbiosRecord);
+                               
+    if ((Status == EFI_SUCCESS) && (CurrentSmbiosEntry->Smbios32BitTable)) {
+      GetSmbiosStructureSize(SmbiosProtocol, SmbiosRecord, &RecordSize, &NumOfStr);
+      //
+      // Record NumberOfSmbiosStructures, TableLength and MaxStructureSize
+      //
+      EntryPointStructure->NumberOfSmbiosStructures++;
+      EntryPointStructure->TableLength = (UINT16) (EntryPointStructure->TableLength + RecordSize);
+      if (RecordSize > EntryPointStructure->MaxStructureSize) {
+        EntryPointStructure->MaxStructureSize = (UINT16) RecordSize;
+      }
+    }
+  } while (!EFI_ERROR(Status));
+  
+  //
+  // Create End-Of-Table structure
+  //
+  GetMaxSmbiosHandle(SmbiosProtocol, &SmbiosHandle);
+  EndStructure.Header.Type = EFI_SMBIOS_TYPE_END_OF_TABLE;
+  EndStructure.Header.Length = (UINT8) sizeof (EFI_SMBIOS_TABLE_HEADER);
+  EndStructure.Header.Handle = SmbiosHandle;
+  EndStructure.Tailing[0] = 0;
+  EndStructure.Tailing[1] = 0;
+  EntryPointStructure->NumberOfSmbiosStructures++;
+  EntryPointStructure->TableLength = (UINT16) (EntryPointStructure->TableLength + sizeof (EndStructure));
+  if (sizeof (EndStructure) > EntryPointStructure->MaxStructureSize) {
+    EntryPointStructure->MaxStructureSize = (UINT16) sizeof (EndStructure);
+  }
+
+  if ((UINTN) EFI_SIZE_TO_PAGES (EntryPointStructure->TableLength) > mPreAllocatedPages) {
+    //
+    // If new SMBIOS table size exceeds the previous allocated page, 
+    // it is time to re-allocate memory (below 4GB).
+    // 
+    DEBUG ((EFI_D_ERROR, "SmbiosCreateTable() re-allocate SMBIOS 32-bit table\n"));
+    if (EntryPointStructure->TableAddress != 0) {
+      //
+      // Free the previous allocated page
+      //      
+      FreePages (
+            (VOID*)(UINTN)EntryPointStructure->TableAddress,
+            mPreAllocatedPages
+            );
+      EntryPointStructure->TableAddress = 0;
+      mPreAllocatedPages = 0;
+    }
+    
+    PhysicalAddress = 0xffffffff;
+    Status = gBS->AllocatePages (
+                    AllocateMaxAddress,
+                    EfiRuntimeServicesData,
+                    EFI_SIZE_TO_PAGES (EntryPointStructure->TableLength),
+                    &PhysicalAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SmbiosCreateTable() could not allocate SMBIOS table < 4GB\n"));
+      EntryPointStructure->TableAddress = 0;
+      return EFI_OUT_OF_RESOURCES;
+    } else {
+      EntryPointStructure->TableAddress = (UINT32) PhysicalAddress;
+      mPreAllocatedPages = EFI_SIZE_TO_PAGES (EntryPointStructure->TableLength);
+    }
+  }
+  
+  //
+  // Assemble the tables
+  //
+  ASSERT (EntryPointStructure->TableAddress != 0);
+  BufferPointer = (UINT8 *) (UINTN) EntryPointStructure->TableAddress;
+  CurrentSmbiosEntry = NULL;
+  do {
+    Status = GetNextSmbiosRecord (SmbiosProtocol, &CurrentSmbiosEntry, &SmbiosRecord);
+
+    if ((Status == EFI_SUCCESS) && (CurrentSmbiosEntry->Smbios32BitTable)) {
+      GetSmbiosStructureSize(SmbiosProtocol, SmbiosRecord, &RecordSize, &NumOfStr);
+      CopyMem (BufferPointer, SmbiosRecord, RecordSize);
+      BufferPointer = BufferPointer + RecordSize;
+    }
+  } while (!EFI_ERROR(Status));
+  
+  //
+  // Assemble End-Of-Table structure
+  //
+  CopyMem (BufferPointer, &EndStructure, sizeof (EndStructure));
+
+  //
+  // Fixup checksums in the Entry Point Structure
+  //
+  EntryPointStructure->IntermediateChecksum = 0;
+  EntryPointStructure->EntryPointStructureChecksum = 0;
+
+  EntryPointStructure->IntermediateChecksum =
+    CalculateCheckSum8 ((UINT8 *) EntryPointStructure + 0x10, EntryPointStructure->EntryPointLength - 0x10);
+  EntryPointStructure->EntryPointStructureChecksum =
+    CalculateCheckSum8 ((UINT8 *) EntryPointStructure, EntryPointStructure->EntryPointLength);
+
+  //
+  // Returns the pointer
+  //
+  *TableEntryPointStructure = EntryPointStructure;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Assembles SMBIOS 64-bit table from the SMBIOS protocol. Produce Table
+  Entry Point and return the pointer to it.
+
+  @param  TableEntryPointStructure   On exit, points to the SMBIOS entrypoint structure.
+
+  @retval EFI_SUCCESS                Structure created sucessfully.
+  @retval EFI_OUT_OF_RESOURCES       No enough memory.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosCreate64BitTable (
+  OUT VOID    **TableEntryPointStructure
+  )
+{
+  UINT8                           *BufferPointer;
+  UINTN                           RecordSize;
+  UINTN                           NumOfStr;
+  EFI_STATUS                      Status;
+  EFI_SMBIOS_HANDLE               SmbiosHandle;
+  EFI_SMBIOS_PROTOCOL             *SmbiosProtocol;
+  EFI_PHYSICAL_ADDRESS            PhysicalAddress;
+  EFI_SMBIOS_TABLE_HEADER         *SmbiosRecord;
+  EFI_SMBIOS_TABLE_END_STRUCTURE  EndStructure;
+  EFI_SMBIOS_ENTRY                *CurrentSmbiosEntry;
+  
+  Status            = EFI_SUCCESS;
+  BufferPointer     = NULL;
+
+  if (Smbios30EntryPointStructure == NULL) {
+    //
+    // Initialize the Smbios30EntryPointStructure with initial values.
+    // It should be done only once.
+    // Allocate memory at any address.
+    //
+    DEBUG ((EFI_D_INFO, "SmbiosCreateTable: Initialize 64-bit entry point structure\n"));
+    Smbios30EntryPointStructureData.MajorVersion  = mPrivateData.Smbios.MajorVersion;
+    Smbios30EntryPointStructureData.MinorVersion  = mPrivateData.Smbios.MinorVersion;
+    Smbios30EntryPointStructureData.DocRev        = PcdGet8 (PcdSmbiosDocRev);
+    Status = gBS->AllocatePages (
+                    AllocateAnyPages,
+                    EfiRuntimeServicesData,
+                    EFI_SIZE_TO_PAGES (sizeof (SMBIOS_TABLE_3_0_ENTRY_POINT)),
+                    &PhysicalAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SmbiosCreate64BitTable() could not allocate Smbios30EntryPointStructure\n"));
+      return EFI_OUT_OF_RESOURCES;
+    }
+    
+    Smbios30EntryPointStructure = (SMBIOS_TABLE_3_0_ENTRY_POINT *) (UINTN) PhysicalAddress;
+    
+    CopyMem (
+      Smbios30EntryPointStructure,
+      &Smbios30EntryPointStructureData,
+      sizeof (SMBIOS_TABLE_3_0_ENTRY_POINT)
+      );
+  }
+
+  //
+  // Get Smbios protocol to traverse SMBIOS records.
+  //
+  SmbiosProtocol = &mPrivateData.Smbios;
+  Smbios30EntryPointStructure->TableMaximumSize = 0;
+
+  //
+  // Calculate EPS Table Length
+  //
+  CurrentSmbiosEntry = NULL;
+  do {
+    Status = GetNextSmbiosRecord (SmbiosProtocol, &CurrentSmbiosEntry, &SmbiosRecord);
+                               
+    if ((Status == EFI_SUCCESS) && (CurrentSmbiosEntry->Smbios64BitTable)) {
+      GetSmbiosStructureSize(SmbiosProtocol, SmbiosRecord, &RecordSize, &NumOfStr);
+      //
+      // Record TableMaximumSize
+      //
+      Smbios30EntryPointStructure->TableMaximumSize = (UINT32) (Smbios30EntryPointStructure->TableMaximumSize + RecordSize);
+    }
+  } while (!EFI_ERROR(Status));
+
+  //
+  // Create End-Of-Table structure
+  //
+  GetMaxSmbiosHandle(SmbiosProtocol, &SmbiosHandle);
+  EndStructure.Header.Type = EFI_SMBIOS_TYPE_END_OF_TABLE;
+  EndStructure.Header.Length = (UINT8) sizeof (EFI_SMBIOS_TABLE_HEADER);
+  EndStructure.Header.Handle = SmbiosHandle;
+  EndStructure.Tailing[0] = 0;
+  EndStructure.Tailing[1] = 0;
+  Smbios30EntryPointStructure->TableMaximumSize = (UINT32) (Smbios30EntryPointStructure->TableMaximumSize + sizeof (EndStructure));
+
+  if ((UINTN) EFI_SIZE_TO_PAGES (Smbios30EntryPointStructure->TableMaximumSize) > mPre64BitAllocatedPages) {
+    //
+    // If new SMBIOS table size exceeds the previous allocated page, 
+    // it is time to re-allocate memory at anywhere.
+    //
+    DEBUG ((EFI_D_ERROR, "SmbiosCreate64BitTable() re-allocate SMBIOS 64-bit table\n"));
+    if (Smbios30EntryPointStructure->TableAddress != 0) {
+      //
+      // Free the previous allocated page
+      //      
+      FreePages (
+            (VOID*)(UINTN)Smbios30EntryPointStructure->TableAddress,
+            mPre64BitAllocatedPages
+            );
+      Smbios30EntryPointStructure->TableAddress = 0;
+      mPre64BitAllocatedPages = 0;
+    }
+
+    Status = gBS->AllocatePages (
+                    AllocateAnyPages,
+                    EfiRuntimeServicesData,
+                    EFI_SIZE_TO_PAGES (Smbios30EntryPointStructure->TableMaximumSize),
+                    &PhysicalAddress
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "SmbiosCreateTable() could not allocate SMBIOS 64-bit table\n"));
+      Smbios30EntryPointStructure->TableAddress = 0;
+      return EFI_OUT_OF_RESOURCES;
+    } else {
+      Smbios30EntryPointStructure->TableAddress = PhysicalAddress;
+      mPre64BitAllocatedPages = EFI_SIZE_TO_PAGES (Smbios30EntryPointStructure->TableMaximumSize);
+    }
+  }
+
+  //
+  // Assemble the tables
+  //
+  ASSERT (Smbios30EntryPointStructure->TableAddress != 0);
+  BufferPointer = (UINT8 *) (UINTN) Smbios30EntryPointStructure->TableAddress;
+  CurrentSmbiosEntry = NULL;
+  do {
+    Status = GetNextSmbiosRecord (SmbiosProtocol, &CurrentSmbiosEntry, &SmbiosRecord);
+
+    if ((Status == EFI_SUCCESS) && (CurrentSmbiosEntry->Smbios64BitTable)) {
+      //
+      // This record can be added to 64-bit table
+      //
+      GetSmbiosStructureSize(SmbiosProtocol, SmbiosRecord, &RecordSize, &NumOfStr);
+      CopyMem (BufferPointer, SmbiosRecord, RecordSize);
+      BufferPointer = BufferPointer + RecordSize;
+    }
+  } while (!EFI_ERROR(Status));
+
+  //
+  // Assemble End-Of-Table structure
+  //
+  CopyMem (BufferPointer, &EndStructure, sizeof (EndStructure));
+
+  //
+  // Fixup checksums in the Entry Point Structure
+  //
+  Smbios30EntryPointStructure->EntryPointStructureChecksum = 0;
+  Smbios30EntryPointStructure->EntryPointStructureChecksum =
+    CalculateCheckSum8 ((UINT8 *) Smbios30EntryPointStructure, Smbios30EntryPointStructure->EntryPointLength);
+
+  //
+  // Returns the pointer
+  //
+  *TableEntryPointStructure = Smbios30EntryPointStructure;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Create Smbios Table and installs the Smbios Table to the System Table.
+  
+  @param  Smbios32BitTable    The flag to update 32-bit table.
+  @param  Smbios64BitTable    The flag to update 64-bit table.
+
+**/
+VOID
+EFIAPI
+SmbiosTableConstruction (
+  BOOLEAN     Smbios32BitTable,
+  BOOLEAN     Smbios64BitTable
+  )
+{
+  UINT8       *Eps;
+  UINT8       *Eps64Bit;
+  EFI_STATUS  Status;
+
+  if (Smbios32BitTable) {
+    Status = SmbiosCreateTable ((VOID **) &Eps);
+    if (!EFI_ERROR (Status)) {
+      gBS->InstallConfigurationTable (&gEfiSmbiosTableGuid, Eps);
+    }
+  }
+
+  if (Smbios64BitTable) {
+    Status = SmbiosCreate64BitTable ((VOID **) &Eps64Bit);
+    if (!EFI_ERROR (Status)) {
+      gBS->InstallConfigurationTable (&gEfiSmbios3TableGuid, Eps64Bit);
+    }
+  }
+}
+
+/**
+
+  Driver to produce Smbios protocol and pre-allocate 1 page for the final SMBIOS table. 
+
+  @param ImageHandle     Module's image handle
+  @param SystemTable     Pointer of EFI_SYSTEM_TABLE
+
+  @retval EFI_SUCCESS    Smbios protocol installed
+  @retval Other          No protocol installed, unload driver.
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+
+  mPrivateData.Signature                = SMBIOS_INSTANCE_SIGNATURE;
+  mPrivateData.Smbios.Add               = SmbiosAdd;
+  mPrivateData.Smbios.UpdateString      = SmbiosUpdateString;
+  mPrivateData.Smbios.Remove            = SmbiosRemove;
+  mPrivateData.Smbios.GetNext           = SmbiosGetNext;
+  mPrivateData.Smbios.MajorVersion      = (UINT8) (PcdGet16 (PcdSmbiosVersion) >> 8);
+  mPrivateData.Smbios.MinorVersion      = (UINT8) (PcdGet16 (PcdSmbiosVersion) & 0x00ff);
+
+  InitializeListHead (&mPrivateData.DataListHead);
+  InitializeListHead (&mPrivateData.AllocatedHandleListHead);
+  EfiInitializeLock (&mPrivateData.DataLock, TPL_NOTIFY);
+  
+  //
+  // Make a new handle and install the protocol
+  //
+  mPrivateData.Handle = NULL;
+  Status = gBS->InstallProtocolInterface (
+                  &mPrivateData.Handle,
+                  &gEfiSmbiosProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  &mPrivateData.Smbios
+                  );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.h b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.h
new file mode 100644
index 0000000000..10cff8dd59
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.h
@@ -0,0 +1,130 @@
+/** @file
+  This code supports the implementation of the Smbios protocol
+  
+Copyright (c) 2009 - 2015, 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.             
+
+**/
+
+#ifndef _SMBIOS_DXE_H_
+#define _SMBIOS_DXE_H_
+
+
+#include <PiDxe.h>
+
+#include <Protocol/Smbios.h>
+#include <IndustryStandard/SmBios.h>
+#include <Guid/EventGroup.h>
+#include <Guid/SmBios.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/PcdLib.h>
+
+#define SMBIOS_INSTANCE_SIGNATURE SIGNATURE_32 ('S', 'B', 'i', 's')
+typedef struct {
+  UINT32                Signature;
+  EFI_HANDLE            Handle;
+  //
+  // Produced protocol
+  //
+  EFI_SMBIOS_PROTOCOL   Smbios;
+  //
+  // Updates to record list must be locked.
+  //
+  EFI_LOCK              DataLock;
+  //
+  // List of EFI_SMBIOS_ENTRY structures.
+  //
+  LIST_ENTRY            DataListHead;
+  //
+  // List of allocated SMBIOS handle.
+  //
+  LIST_ENTRY            AllocatedHandleListHead;
+} SMBIOS_INSTANCE;
+
+#define SMBIOS_INSTANCE_FROM_THIS(this)  CR (this, SMBIOS_INSTANCE, Smbios, SMBIOS_INSTANCE_SIGNATURE)
+
+//
+// SMBIOS record Header
+//
+// An SMBIOS internal Record is an EFI_SMBIOS_RECORD_HEADER followed by (RecordSize - HeaderSize) bytes of
+//  data. The format of the data is defined by the SMBIOS spec.
+//
+//
+#define EFI_SMBIOS_RECORD_HEADER_VERSION  0x0100
+typedef struct {
+  UINT16      Version;
+  UINT16      HeaderSize;
+  UINTN       RecordSize;
+  EFI_HANDLE  ProducerHandle;
+  UINTN       NumberOfStrings;
+} EFI_SMBIOS_RECORD_HEADER;
+
+
+//
+// Private data structure to contain the SMBIOS record. One record per
+//  structure. SmbiosRecord is a copy of the data passed in and follows RecordHeader .
+//
+#define EFI_SMBIOS_ENTRY_SIGNATURE  SIGNATURE_32 ('S', 'r', 'e', 'c')
+typedef struct {
+  UINT32                    Signature;
+  LIST_ENTRY                Link;
+  EFI_SMBIOS_RECORD_HEADER  *RecordHeader;
+  UINTN                     RecordSize;
+  //
+  // Indicate which table this record is added to.
+  //
+  BOOLEAN                   Smbios32BitTable;
+  BOOLEAN                   Smbios64BitTable;
+} EFI_SMBIOS_ENTRY;
+
+#define SMBIOS_ENTRY_FROM_LINK(link)  CR (link, EFI_SMBIOS_ENTRY, Link, EFI_SMBIOS_ENTRY_SIGNATURE)
+
+//
+// Private data to contain the Smbios handle that already allocated.
+//
+#define SMBIOS_HANDLE_ENTRY_SIGNATURE  SIGNATURE_32 ('S', 'h', 'r', 'd')
+
+typedef struct {
+  UINT32               Signature;
+  LIST_ENTRY           Link;
+  //
+  // Filter driver will register what record guid filter should be used.
+  //
+  EFI_SMBIOS_HANDLE    SmbiosHandle;
+
+} SMBIOS_HANDLE_ENTRY;
+
+#define SMBIOS_HANDLE_ENTRY_FROM_LINK(link)  CR (link, SMBIOS_HANDLE_ENTRY, Link, SMBIOS_HANDLE_ENTRY_SIGNATURE)
+
+typedef struct {
+  EFI_SMBIOS_TABLE_HEADER  Header;
+  UINT8                    Tailing[2];
+} EFI_SMBIOS_TABLE_END_STRUCTURE;
+
+/**
+  Create Smbios Table and installs the Smbios Table to the System Table.
+  
+  @param  Smbios32BitTable    The flag to update 32-bit table.
+  @param  Smbios64BitTable    The flag to update 64-bit table.
+  
+**/
+VOID
+EFIAPI
+SmbiosTableConstruction (
+  BOOLEAN     Smbios32BitTable,
+  BOOLEAN     Smbios64BitTable
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.inf b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.inf
new file mode 100644
index 0000000000..4fd6b97b42
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.inf
@@ -0,0 +1,66 @@
+## @file
+# This driver initializes and installs the SMBIOS protocol, constructs SMBIOS table into system configuration table.
+#
+# Copyright (c) 2009 - 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmbiosDxe
+  MODULE_UNI_FILE                = SmbiosDxe.uni
+  FILE_GUID                      = F9D88642-0737-49bc-81B5-6889CD57D9EA
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = SmbiosDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+
+[Sources]
+  SmbiosDxe.h
+  SmbiosDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  PcdLib
+
+[Protocols]
+  gEfiSmbiosProtocolGuid                            ## PRODUCES
+  
+[Guids]
+  gEfiSmbiosTableGuid                               ## SOMETIMES_PRODUCES ## SystemTable
+  gEfiSmbios3TableGuid                              ## SOMETIMES_PRODUCES ## SystemTable
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosVersion   ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosDocRev    ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdSmbiosEntryPointProvideMethod   ## CONSUMES
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmbiosDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.uni b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.uni
new file mode 100644
index 0000000000..7f9c02e9db
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxeExtra.uni b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxeExtra.uni
new file mode 100644
index 0000000000..b3631121e8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosDxe/SmbiosDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.c b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.c
new file mode 100644
index 0000000000..f9e0196677
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.c
@@ -0,0 +1,629 @@
+/** @file
+  This driver measures SMBIOS table to TPM.
+
+Copyright (c) 2015, 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 <PiDxe.h>
+
+#include <Protocol/Smbios.h>
+#include <IndustryStandard/SmBios.h>
+#include <IndustryStandard/UefiTcgPlatform.h>
+#include <Guid/EventGroup.h>
+#include <Guid/SmBios.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/TpmMeasurementLib.h>
+
+#define FIELD_SIZE_OF(TYPE, Field) ((UINTN)sizeof(((TYPE *)0)->Field))
+
+typedef struct {
+  UINT8  Type;
+  UINTN  Offset;
+  UINTN  Size;
+  UINT32 Flags;
+} SMBIOS_FILTER_TABLE;
+#define SMBIOS_FILTER_TABLE_FLAG_IS_STRING  BIT0
+
+typedef struct {
+  UINT8                Type;
+  SMBIOS_FILTER_TABLE  *Filter; // NULL means all fields
+  UINTN                FilterCount;
+} SMBIOS_FILTER_STRUCT;
+
+//
+// Platform Specific Policy
+//
+SMBIOS_FILTER_TABLE  mSmbiosFilterType1BlackList[] = {
+  {0x01, OFFSET_OF(SMBIOS_TABLE_TYPE1, SerialNumber),         FIELD_SIZE_OF(SMBIOS_TABLE_TYPE1, SerialNumber),         SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x01, OFFSET_OF(SMBIOS_TABLE_TYPE1, Uuid),                 FIELD_SIZE_OF(SMBIOS_TABLE_TYPE1, Uuid),                 0},
+  {0x01, OFFSET_OF(SMBIOS_TABLE_TYPE1, WakeUpType),           FIELD_SIZE_OF(SMBIOS_TABLE_TYPE1, WakeUpType),           0},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType2BlackList[] = {
+  {0x02, OFFSET_OF(SMBIOS_TABLE_TYPE2, SerialNumber),         FIELD_SIZE_OF(SMBIOS_TABLE_TYPE2, SerialNumber),         SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x02, OFFSET_OF(SMBIOS_TABLE_TYPE2, LocationInChassis),    FIELD_SIZE_OF(SMBIOS_TABLE_TYPE2, LocationInChassis),    SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType3BlackList[] = {
+  {0x03, OFFSET_OF(SMBIOS_TABLE_TYPE3, SerialNumber),         FIELD_SIZE_OF(SMBIOS_TABLE_TYPE3, SerialNumber),         SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x03, OFFSET_OF(SMBIOS_TABLE_TYPE3, AssetTag),             FIELD_SIZE_OF(SMBIOS_TABLE_TYPE3, AssetTag),             SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType4BlackList[] = {
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, SerialNumber),         FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, SerialNumber),         SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, AssetTag),             FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, AssetTag),             SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, PartNumber),           FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, PartNumber),           SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, CoreCount),            FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, CoreCount),            0},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, EnabledCoreCount),     FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, EnabledCoreCount),     0},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, ThreadCount),          FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, ThreadCount),          0},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, CoreCount2),           FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, CoreCount2),           0},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, EnabledCoreCount2),    FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, EnabledCoreCount2),    0},
+  {0x04, OFFSET_OF(SMBIOS_TABLE_TYPE4, ThreadCount2),         FIELD_SIZE_OF(SMBIOS_TABLE_TYPE4, ThreadCount2),         0},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType17BlackList[] = {
+  {0x11, OFFSET_OF(SMBIOS_TABLE_TYPE17, SerialNumber),        FIELD_SIZE_OF(SMBIOS_TABLE_TYPE17, SerialNumber),        SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x11, OFFSET_OF(SMBIOS_TABLE_TYPE17, AssetTag),            FIELD_SIZE_OF(SMBIOS_TABLE_TYPE17, AssetTag),            SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x11, OFFSET_OF(SMBIOS_TABLE_TYPE17, PartNumber),          FIELD_SIZE_OF(SMBIOS_TABLE_TYPE17, PartNumber),          SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType22BlackList[] = {
+  {0x16, OFFSET_OF(SMBIOS_TABLE_TYPE22, SerialNumber),        FIELD_SIZE_OF(SMBIOS_TABLE_TYPE22, SerialNumber),        SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x16, OFFSET_OF(SMBIOS_TABLE_TYPE22, SBDSSerialNumber),    FIELD_SIZE_OF(SMBIOS_TABLE_TYPE22, SBDSSerialNumber),    0},
+  {0x16, OFFSET_OF(SMBIOS_TABLE_TYPE22, SBDSManufactureDate), FIELD_SIZE_OF(SMBIOS_TABLE_TYPE22, SBDSManufactureDate), 0},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType23BlackList[] = {
+  {0x17, OFFSET_OF(SMBIOS_TABLE_TYPE23, ResetCount),          FIELD_SIZE_OF(SMBIOS_TABLE_TYPE23, ResetCount),          0},
+};
+SMBIOS_FILTER_TABLE  mSmbiosFilterType39BlackList[] = {
+  {0x27, OFFSET_OF(SMBIOS_TABLE_TYPE39, SerialNumber),        FIELD_SIZE_OF(SMBIOS_TABLE_TYPE39, SerialNumber),        SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x27, OFFSET_OF(SMBIOS_TABLE_TYPE39, AssetTagNumber),      FIELD_SIZE_OF(SMBIOS_TABLE_TYPE39, AssetTagNumber),      SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+  {0x27, OFFSET_OF(SMBIOS_TABLE_TYPE39, ModelPartNumber),     FIELD_SIZE_OF(SMBIOS_TABLE_TYPE39, ModelPartNumber),     SMBIOS_FILTER_TABLE_FLAG_IS_STRING},
+};
+
+SMBIOS_FILTER_STRUCT  mSmbiosFilterStandardTableBlackList[] = {
+  {0x01, mSmbiosFilterType1BlackList,  sizeof(mSmbiosFilterType1BlackList)/sizeof(mSmbiosFilterType1BlackList[0])},
+  {0x02, mSmbiosFilterType2BlackList,  sizeof(mSmbiosFilterType2BlackList)/sizeof(mSmbiosFilterType2BlackList[0])},
+  {0x03, mSmbiosFilterType3BlackList,  sizeof(mSmbiosFilterType3BlackList)/sizeof(mSmbiosFilterType3BlackList[0])},
+  {0x04, mSmbiosFilterType4BlackList,  sizeof(mSmbiosFilterType4BlackList)/sizeof(mSmbiosFilterType4BlackList[0])},
+  {0x0B, NULL, 0},
+  {0x0F, NULL, 0},
+  {0x11, mSmbiosFilterType17BlackList, sizeof(mSmbiosFilterType17BlackList)/sizeof(mSmbiosFilterType17BlackList[0])},
+  {0x12, NULL, 0},
+  {0x16, mSmbiosFilterType22BlackList, sizeof(mSmbiosFilterType22BlackList)/sizeof(mSmbiosFilterType22BlackList[0])},
+  {0x17, mSmbiosFilterType23BlackList, sizeof(mSmbiosFilterType23BlackList)/sizeof(mSmbiosFilterType23BlackList[0])},
+  {0x1F, NULL, 0},
+  {0x21, NULL, 0},
+  {0x27, mSmbiosFilterType39BlackList, sizeof(mSmbiosFilterType39BlackList)/sizeof(mSmbiosFilterType39BlackList[0])},
+};
+
+EFI_SMBIOS_PROTOCOL *mSmbios;
+UINTN               mMaxLen;
+
+/**
+
+  This function dump raw data.
+
+  @param  Data  raw data
+  @param  Size  raw data size
+
+**/
+VOID
+InternalDumpData (
+  IN UINT8  *Data,
+  IN UINTN  Size
+  )
+{
+  UINTN  Index;
+  for (Index = 0; Index < Size; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "%02x", (UINTN)Data[Index]));
+  }
+}
+
+/**
+
+  This function dump raw data with colume format.
+
+  @param  Data  raw data
+  @param  Size  raw data size
+
+**/
+VOID
+InternalDumpHex (
+  IN UINT8  *Data,
+  IN UINTN  Size
+  )
+{
+  UINTN   Index;
+  UINTN   Count;
+  UINTN   Left;
+
+#define COLUME_SIZE  (16 * 2)
+
+  Count = Size / COLUME_SIZE;
+  Left  = Size % COLUME_SIZE;
+  for (Index = 0; Index < Count; Index++) {
+    DEBUG ((EFI_D_VERBOSE, "%04x: ", Index * COLUME_SIZE));
+    InternalDumpData (Data + Index * COLUME_SIZE, COLUME_SIZE);
+    DEBUG ((EFI_D_VERBOSE, "\n"));
+  }
+
+  if (Left != 0) {
+    DEBUG ((EFI_D_VERBOSE, "%04x: ", Index * COLUME_SIZE));
+    InternalDumpData (Data + Index * COLUME_SIZE, Left);
+    DEBUG ((EFI_D_VERBOSE, "\n"));
+  }
+}
+
+
+/**
+
+  This function get filter structure by SMBIOS type.
+
+  @param  Type  SMBIOS type
+
+**/
+SMBIOS_FILTER_STRUCT *
+GetFilterStructByType (
+  IN UINT8 Type
+  )
+{
+  UINTN  Index;
+  for (Index = 0; Index < sizeof(mSmbiosFilterStandardTableBlackList)/sizeof(mSmbiosFilterStandardTableBlackList[0]); Index++) {
+    if (mSmbiosFilterStandardTableBlackList[Index].Type == Type) {
+      return &mSmbiosFilterStandardTableBlackList[Index];
+    }
+  }
+  return NULL;
+}
+
+/**
+
+  This function get SMBIOS string in SMBIOS table.
+
+  @param  Head      SMBIOS table head
+  @param  StringId  SMBIOS string ID
+  @param  StringLen length of SMBIOS string
+
+  @return SMBIOS string data
+**/
+CHAR8 *
+GetSmbiosStringById (
+  IN   EFI_SMBIOS_TABLE_HEADER          *Head,
+  IN   SMBIOS_TABLE_STRING              StringId,
+  OUT  UINTN                            *StringLen
+  )
+{
+  UINTN  Size;
+  UINTN  StrLen;
+  CHAR8  *CharInStr;
+  UINTN  StringsNumber;
+  CHAR8  *String;
+
+  CharInStr = (CHAR8 *)Head + Head->Length;
+  Size = Head->Length;
+  StringsNumber = 0;
+  StrLen = 0;
+  //
+  // look for the two consecutive zeros, check the string limit by the way.
+  //
+  String = NULL;
+  while (*CharInStr != 0 || *(CharInStr+1) != 0) {
+    if (*CharInStr == 0) {
+      Size += 1;
+      CharInStr++;
+    }
+    String = CharInStr;
+
+    for (StrLen = 0 ; StrLen < mMaxLen; StrLen++) {
+      if (*(CharInStr+StrLen) == 0) {
+        break;
+      }
+    }
+    *StringLen = StrLen;
+
+    if (StrLen == mMaxLen) {
+      return NULL;
+    }
+
+    //
+    // forward the pointer
+    //
+    CharInStr += StrLen;
+    Size += StrLen;
+    StringsNumber += 1;
+    if (StringsNumber == StringId) {
+      break;
+    }
+  }
+
+  return String;
+}
+
+/**
+
+  This function update SMBIOS table based on policy.
+
+  @param  TableEntry      SMBIOS table
+  @param  TableEntrySize  SMBIOS table size
+
+**/
+VOID
+FilterSmbiosEntry (
+  IN OUT VOID   *TableEntry,
+  IN UINTN      TableEntrySize
+  )
+{
+  SMBIOS_FILTER_STRUCT  *FilterStruct;
+  SMBIOS_FILTER_TABLE   *Filter;
+  UINTN                 Index;
+  SMBIOS_TABLE_STRING   StringId;
+  CHAR8                 *String;
+  UINTN                 StringLen;
+
+  DEBUG ((EFI_D_INFO, "Smbios Table (Type - %d):\n", ((SMBIOS_STRUCTURE *)TableEntry)->Type));
+  DEBUG_CODE (InternalDumpHex (TableEntry, TableEntrySize););
+
+  FilterStruct = GetFilterStructByType (((SMBIOS_STRUCTURE *)TableEntry)->Type);
+  if (FilterStruct != NULL) {
+    if (FilterStruct->Filter == NULL || FilterStruct->FilterCount == 0) {
+      // zero all table entries, except header
+      ZeroMem ((UINT8 *)TableEntry + sizeof(SMBIOS_STRUCTURE), TableEntrySize - sizeof(SMBIOS_STRUCTURE));
+    } else {
+      Filter = FilterStruct->Filter;
+      for (Index = 0; Index < FilterStruct->FilterCount; Index++) {
+        if ((Filter[Index].Flags & SMBIOS_FILTER_TABLE_FLAG_IS_STRING) != 0) {
+          CopyMem (&StringId, (UINT8 *)TableEntry + Filter[Index].Offset, sizeof(StringId));
+          if (StringId != 0) {
+            // set ' ' for string field
+            String = GetSmbiosStringById (TableEntry, StringId, &StringLen);
+            ASSERT (String != NULL);
+            //DEBUG ((EFI_D_INFO,"StrId(0x%x)-%a(%d)\n", StringId, String, StringLen));
+            SetMem (String, StringLen, ' ');
+          }
+        }
+        // zero non-string field
+        ZeroMem ((UINT8 *)TableEntry + Filter[Index].Offset, Filter[Index].Size);
+      }
+    }
+  }
+
+  DEBUG ((EFI_D_INFO, "Filter Smbios Table (Type - %d):\n", ((SMBIOS_STRUCTURE *)TableEntry)->Type));
+  DEBUG_CODE (InternalDumpHex (TableEntry, TableEntrySize););
+}
+
+/**
+
+  Get the full size of SMBIOS structure including optional strings that follow the formatted structure.
+
+  @param Head                   Pointer to the beginning of SMBIOS structure.
+  @param NumberOfStrings        The returned number of optional strings that follow the formatted structure.
+
+  @return Size                  The returned size.
+**/
+UINTN
+GetSmbiosStructureSize (
+  IN   EFI_SMBIOS_TABLE_HEADER          *Head,
+  OUT  UINTN                            *NumberOfStrings
+  )
+{
+  UINTN  Size;
+  UINTN  StrLen;
+  CHAR8  *CharInStr;
+  UINTN  StringsNumber;
+
+  CharInStr = (CHAR8 *)Head + Head->Length;
+  Size = Head->Length;
+  StringsNumber = 0;
+  StrLen = 0;
+  //
+  // look for the two consecutive zeros, check the string limit by the way.
+  //
+  while (*CharInStr != 0 || *(CharInStr+1) != 0) {
+    if (*CharInStr == 0) {
+      Size += 1;
+      CharInStr++;
+    }
+
+    for (StrLen = 0 ; StrLen < mMaxLen; StrLen++) {
+      if (*(CharInStr+StrLen) == 0) {
+        break;
+      }
+    }
+
+    if (StrLen == mMaxLen) {
+      return 0;
+    }
+
+    //
+    // forward the pointer
+    //
+    CharInStr += StrLen;
+    Size += StrLen;
+    StringsNumber += 1;
+  }
+
+  //
+  // count ending two zeros.
+  //
+  Size += 2;
+
+  if (NumberOfStrings != NULL) {
+    *NumberOfStrings = StringsNumber;
+  }
+  return Size;
+}
+
+/**
+
+  This function returns full SMBIOS table length.
+
+  @param  TableAddress      SMBIOS table based address
+  @param  TableMaximumSize  Maximum size of SMBIOS table
+
+  @return SMBIOS table length
+
+**/
+UINTN
+GetSmbiosTableLength (
+  IN VOID  *TableAddress,
+  IN UINTN TableMaximumSize
+  )
+{
+  VOID  *TableEntry;
+  VOID  *TableAddressEnd;
+  UINTN TableEntryLength;
+
+  TableAddressEnd = (VOID *)((UINTN)TableAddress + TableMaximumSize);
+  TableEntry = TableAddress;
+  while (TableEntry < TableAddressEnd) {
+    TableEntryLength = GetSmbiosStructureSize (TableEntry, NULL);
+    if (TableEntryLength == 0) {
+      break;
+    }
+    if (((SMBIOS_STRUCTURE *)TableEntry)->Type == 127) {
+      TableEntry = (VOID *)((UINTN)TableEntry + TableEntryLength);
+      break;
+    }
+    TableEntry = (VOID *)((UINTN)TableEntry + TableEntryLength);
+  }
+
+  return ((UINTN)TableEntry - (UINTN)TableAddress);
+}
+
+/**
+
+  This function updatess full SMBIOS table length.
+
+  @param  TableAddress      SMBIOS table based address
+  @param  TableLength       SMBIOS table length
+
+**/
+VOID
+FilterSmbiosTable (
+  IN OUT VOID  *TableAddress,
+  IN UINTN     TableLength
+  )
+{
+  VOID   *TableAddressEnd;
+  VOID   *TableEntry;
+  UINTN  TableEntryLength;
+
+  TableEntry = TableAddress;
+  TableAddressEnd = (VOID *)((UINTN)TableAddress + TableLength);
+  while ((UINTN)TableEntry < (UINTN)TableAddressEnd) {
+    TableEntryLength = GetSmbiosStructureSize (TableEntry, NULL);
+    if (TableEntryLength == 0) {
+      break;
+    }
+
+    FilterSmbiosEntry (TableEntry, TableEntryLength);
+
+    TableEntry = (VOID *)((UINTN)TableEntry + TableEntryLength);
+  }
+}
+
+/**
+  Measure SMBIOS with EV_EFI_HANDOFF_TABLES to PCR[1].
+
+  @param[in] Event      Event whose notification function is being invoked.
+  @param[in] Context    Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+MeasureSmbiosTable (
+  IN      EFI_EVENT                 Event,
+  IN      VOID                      *Context
+  )
+{
+  EFI_STATUS                        Status;
+  EFI_HANDOFF_TABLE_POINTERS        HandoffTables;
+  SMBIOS_TABLE_ENTRY_POINT          *SmbiosTable;
+  SMBIOS_TABLE_3_0_ENTRY_POINT      *Smbios3Table;
+  VOID                              *SmbiosTableAddress;
+  VOID                              *TableAddress;
+  UINTN                             TableLength;
+
+  SmbiosTable = NULL;
+  Smbios3Table = NULL;
+  SmbiosTableAddress = NULL;
+  TableLength = 0;
+
+  if (mSmbios->MajorVersion >= 3) {
+    Status = EfiGetSystemConfigurationTable (
+               &gEfiSmbios3TableGuid,
+               (VOID **) &Smbios3Table
+               );
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "Smbios3Table:\n"));
+      DEBUG ((EFI_D_INFO, "  AnchorString                - '%c%c%c%c%c'\n",
+        Smbios3Table->AnchorString[0],
+        Smbios3Table->AnchorString[1],
+        Smbios3Table->AnchorString[2],
+        Smbios3Table->AnchorString[3],
+        Smbios3Table->AnchorString[4]
+        ));
+      DEBUG ((EFI_D_INFO, "  EntryPointStructureChecksum - 0x%02x\n", Smbios3Table->EntryPointStructureChecksum));
+      DEBUG ((EFI_D_INFO, "  EntryPointLength            - 0x%02x\n", Smbios3Table->EntryPointLength));
+      DEBUG ((EFI_D_INFO, "  MajorVersion                - 0x%02x\n", Smbios3Table->MajorVersion));
+      DEBUG ((EFI_D_INFO, "  MinorVersion                - 0x%02x\n", Smbios3Table->MinorVersion));
+      DEBUG ((EFI_D_INFO, "  DocRev                      - 0x%02x\n", Smbios3Table->DocRev));
+      DEBUG ((EFI_D_INFO, "  EntryPointRevision          - 0x%02x\n", Smbios3Table->EntryPointRevision));
+      DEBUG ((EFI_D_INFO, "  TableMaximumSize            - 0x%08x\n", Smbios3Table->TableMaximumSize));
+      DEBUG ((EFI_D_INFO, "  TableAddress                - 0x%016lx\n", Smbios3Table->TableAddress));
+    }
+  }
+
+  if (Smbios3Table == NULL) {
+    Status = EfiGetSystemConfigurationTable (
+               &gEfiSmbiosTableGuid,
+               (VOID **) &SmbiosTable
+               );
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "SmbiosTable:\n"));
+      DEBUG ((EFI_D_INFO, "  AnchorString                - '%c%c%c%c'\n",
+        SmbiosTable->AnchorString[0],
+        SmbiosTable->AnchorString[1],
+        SmbiosTable->AnchorString[2],
+        SmbiosTable->AnchorString[3]
+        ));
+      DEBUG ((EFI_D_INFO, "  EntryPointStructureChecksum - 0x%02x\n", SmbiosTable->EntryPointStructureChecksum));
+      DEBUG ((EFI_D_INFO, "  EntryPointLength            - 0x%02x\n", SmbiosTable->EntryPointLength));
+      DEBUG ((EFI_D_INFO, "  MajorVersion                - 0x%02x\n", SmbiosTable->MajorVersion));
+      DEBUG ((EFI_D_INFO, "  MinorVersion                - 0x%02x\n", SmbiosTable->MinorVersion));
+      DEBUG ((EFI_D_INFO, "  MaxStructureSize            - 0x%08x\n", SmbiosTable->MaxStructureSize));
+      DEBUG ((EFI_D_INFO, "  EntryPointRevision          - 0x%02x\n", SmbiosTable->EntryPointRevision));
+      DEBUG ((EFI_D_INFO, "  FormattedArea               - '%c%c%c%c%c'\n",
+        SmbiosTable->FormattedArea[0],
+        SmbiosTable->FormattedArea[1],
+        SmbiosTable->FormattedArea[2],
+        SmbiosTable->FormattedArea[3],
+        SmbiosTable->FormattedArea[4]
+        ));
+      DEBUG ((EFI_D_INFO, "  IntermediateAnchorString    - '%c%c%c%c%c'\n",
+        SmbiosTable->IntermediateAnchorString[0],
+        SmbiosTable->IntermediateAnchorString[1],
+        SmbiosTable->IntermediateAnchorString[2],
+        SmbiosTable->IntermediateAnchorString[3],
+        SmbiosTable->IntermediateAnchorString[4]
+        ));
+      DEBUG ((EFI_D_INFO, "  IntermediateChecksum        - 0x%02x\n", SmbiosTable->IntermediateChecksum));
+      DEBUG ((EFI_D_INFO, "  TableLength                 - 0x%04x\n", SmbiosTable->TableLength));
+      DEBUG ((EFI_D_INFO, "  TableAddress                - 0x%08x\n", SmbiosTable->TableAddress));
+      DEBUG ((EFI_D_INFO, "  NumberOfSmbiosStructures    - 0x%04x\n", SmbiosTable->NumberOfSmbiosStructures));
+      DEBUG ((EFI_D_INFO, "  SmbiosBcdRevision           - 0x%02x\n", SmbiosTable->SmbiosBcdRevision));
+    }
+  }
+
+  if (Smbios3Table != NULL) {
+    SmbiosTableAddress = (VOID *)(UINTN)Smbios3Table->TableAddress;
+    TableLength = GetSmbiosTableLength (SmbiosTableAddress, Smbios3Table->TableMaximumSize);
+  } else if (SmbiosTable != NULL) {
+    SmbiosTableAddress = (VOID *)(UINTN)SmbiosTable->TableAddress;
+    TableLength = SmbiosTable->TableLength;
+  }
+
+  if (SmbiosTableAddress != NULL) {
+    DEBUG ((DEBUG_INFO, "The Smbios Table starts at: 0x%x\n", SmbiosTableAddress));
+    DEBUG ((DEBUG_INFO, "The Smbios Table size: 0x%x\n", TableLength));
+    DEBUG_CODE (InternalDumpHex ((UINT8 *)(UINTN)SmbiosTableAddress, TableLength););
+
+    TableAddress = AllocateCopyPool ((UINTN)TableLength, (VOID *)(UINTN)SmbiosTableAddress);
+    if (TableAddress == NULL) {
+      return ;
+    }
+
+    FilterSmbiosTable (TableAddress, TableLength);
+
+    DEBUG ((DEBUG_INFO, "The final Smbios Table starts at: 0x%x\n", TableAddress));
+    DEBUG ((DEBUG_INFO, "The final Smbios Table size: 0x%x\n", TableLength));
+    DEBUG_CODE (InternalDumpHex (TableAddress, TableLength););
+
+    HandoffTables.NumberOfTables = 1;
+    if (Smbios3Table != NULL) {
+      CopyGuid (&(HandoffTables.TableEntry[0].VendorGuid), &gEfiSmbios3TableGuid);
+      HandoffTables.TableEntry[0].VendorTable = Smbios3Table;
+    } else {
+      CopyGuid (&(HandoffTables.TableEntry[0].VendorGuid), &gEfiSmbiosTableGuid);
+      HandoffTables.TableEntry[0].VendorTable = SmbiosTable;
+    }
+    Status = TpmMeasureAndLogData (
+               1,                       // PCRIndex
+               EV_EFI_HANDOFF_TABLES,   // EventType
+               &HandoffTables,          // EventLog
+               sizeof (HandoffTables),  // LogLen
+               TableAddress,            // HashData
+               TableLength              // HashDataLen
+               );
+    if (EFI_ERROR (Status)) {
+      return ;
+    }
+  }
+
+  return ;
+}
+
+/**
+
+  Driver to produce Smbios measurement.
+
+  @param ImageHandle     Module's image handle
+  @param SystemTable     Pointer of EFI_SYSTEM_TABLE
+
+  @return Status returned from EfiCreateEventReadyToBootEx().
+
+**/
+EFI_STATUS
+EFIAPI
+SmbiosMeasurementDriverEntryPoint (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS            Status;
+  EFI_EVENT             Event;
+
+  Status = gBS->LocateProtocol (&gEfiSmbiosProtocolGuid, NULL, (VOID **) &mSmbios);
+  ASSERT_EFI_ERROR (Status);
+  DEBUG ((DEBUG_INFO, "The Smbios Table Version: %x.%x\n", mSmbios->MajorVersion, mSmbios->MinorVersion));
+
+  if (mSmbios->MajorVersion < 2 || (mSmbios->MajorVersion == 2 && mSmbios->MinorVersion < 7)){
+    mMaxLen = SMBIOS_STRING_MAX_LENGTH;
+  } else if (mSmbios->MajorVersion < 3) {
+    //
+    // Reference SMBIOS 2.7, chapter 6.1.3, it will have no limit on the length of each individual text string.
+    // However, the length of the entire structure table (including all strings) must be reported
+    // in the Structure Table Length field of the SMBIOS Structure Table Entry Point,
+    // which is a WORD field limited to 65,535 bytes.
+    //
+    mMaxLen = SMBIOS_TABLE_MAX_LENGTH;
+  } else {
+    //
+    // SMBIOS 3.0 defines the Structure table maximum size as DWORD field limited to 0xFFFFFFFF bytes.
+    // Locate the end of string as long as possible.
+    //
+    mMaxLen = SMBIOS_3_0_TABLE_MAX_LENGTH;
+  }
+
+  //
+  // Measure Smbios tables
+  //
+  Status = EfiCreateEventReadyToBootEx (
+             TPL_CALLBACK,
+             MeasureSmbiosTable,
+             NULL,
+             &Event
+             );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.inf b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.inf
new file mode 100644
index 0000000000..c5a779cca7
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.inf
@@ -0,0 +1,68 @@
+## @file
+# This driver measures SMBIOS table to TPM.
+#
+# This driver is a sample driver to follow TCG platform specification to
+# filter some fields in SMBIOS table.
+#   - Platform configuration information that is automatically updated,
+#     such as clock registers, and system unique information, such as
+#     asset numbers or serial numbers, MUST NOT be measured into PCR [1],
+#     or any other PCR.
+#
+# A platform may use its own policy to filter some fields in SMBIOS table.
+#
+# Copyright (c) 2015, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmbiosMeasurementDxe
+  MODULE_UNI_FILE                = SmbiosMeasurementDxe.uni
+  FILE_GUID                      = D27FED59-ABB4-4FED-BEAD-2A878C7E4A7E
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SmbiosMeasurementDriverEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC ARM AARCH64
+#
+
+[Sources]
+  SmbiosMeasurementDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  BaseLib
+  UefiLib
+  UefiDriverEntryPoint
+  DebugLib
+  TpmMeasurementLib
+
+[Protocols]
+  gEfiSmbiosProtocolGuid                            ## CONSUMES
+
+[Guids]
+  gEfiSmbiosTableGuid                               ## SOMETIMES_CONSUMES ## SystemTable
+  gEfiSmbios3TableGuid                              ## SOMETIMES_CONSUMES ## SystemTable
+
+[Depex]
+  gEfiSmbiosProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmbiosMeasurementDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.uni b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.uni
new file mode 100644
index 0000000000..98b0f52a6a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxeExtra.uni b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxeExtra.uni
new file mode 100644
index 0000000000..4e081c44b8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmbiosMeasurementDxe/SmbiosMeasurementDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.c b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.c
new file mode 100644
index 0000000000..3ad264b117
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.c
@@ -0,0 +1,99 @@
+/** @file
+A driver allocates common SMM communication buffer in EfiReservedMemoryType.
+
+This driver allocates common SMM communication buffer in EfiReservedMemoryType,
+then it publishes the information to EFI configuration table with
+gEdkiiPiSmmCommunicationRegionTableGuid.
+Any other driver or application can get the table and know the common
+communication buffer.
+
+Copyright (c) 2016, 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 <PiDxe.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PcdLib.h>
+#include <Guid/PiSmmCommunicationRegionTable.h>
+
+#define DEFAULT_COMMON_PI_SMM_COMMUNIATION_REGION_PAGES  4
+
+/**
+  Entry Point for SMM communication buffer driver.
+
+  @param[in] ImageHandle  Image handle of this driver.
+  @param[in] SystemTable  A Pointer to the EFI System Table.
+
+  @retval EFI_SUCEESS
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+EFIAPI
+SmmCommunicationBufferEntryPoint (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS                               Status;
+  UINT32                                   DescriptorSize;
+  EDKII_PI_SMM_COMMUNICATION_REGION_TABLE  *PiSmmCommunicationRegionTable;
+  EFI_MEMORY_DESCRIPTOR                    *Entry;
+
+  DescriptorSize = sizeof(EFI_MEMORY_DESCRIPTOR);
+  //
+  // Make sure Size != sizeof(EFI_MEMORY_DESCRIPTOR). This will
+  // prevent people from having pointer math bugs in their code.
+  // now you have to use *DescriptorSize to make things work.
+  //
+  DescriptorSize += sizeof(UINT64) - (DescriptorSize % sizeof (UINT64));
+
+  //
+  // Allocate and fill PiSmmCommunicationRegionTable
+  //
+  PiSmmCommunicationRegionTable = AllocateReservedPool (sizeof(EDKII_PI_SMM_COMMUNICATION_REGION_TABLE) + DescriptorSize);
+  ASSERT(PiSmmCommunicationRegionTable != NULL);
+  ZeroMem (PiSmmCommunicationRegionTable, sizeof(EDKII_PI_SMM_COMMUNICATION_REGION_TABLE) + DescriptorSize);
+
+  PiSmmCommunicationRegionTable->Version         = EDKII_PI_SMM_COMMUNICATION_REGION_TABLE_VERSION;
+  PiSmmCommunicationRegionTable->NumberOfEntries = 1;
+  PiSmmCommunicationRegionTable->DescriptorSize  = DescriptorSize;
+  Entry = (EFI_MEMORY_DESCRIPTOR *)(PiSmmCommunicationRegionTable + 1);
+  Entry->Type          = EfiConventionalMemory;
+  Entry->PhysicalStart = (EFI_PHYSICAL_ADDRESS)(UINTN)AllocateReservedPages (DEFAULT_COMMON_PI_SMM_COMMUNIATION_REGION_PAGES);
+  ASSERT(Entry->PhysicalStart != 0);
+  Entry->VirtualStart  = 0;
+  Entry->NumberOfPages = DEFAULT_COMMON_PI_SMM_COMMUNIATION_REGION_PAGES;
+  Entry->Attribute     = 0;
+
+  DEBUG ((EFI_D_INFO, "PiSmmCommunicationRegionTable:(0x%x)\n", PiSmmCommunicationRegionTable));
+  DEBUG ((EFI_D_INFO, "  Version         - 0x%x\n", PiSmmCommunicationRegionTable->Version));
+  DEBUG ((EFI_D_INFO, "  NumberOfEntries - 0x%x\n", PiSmmCommunicationRegionTable->NumberOfEntries));
+  DEBUG ((EFI_D_INFO, "  DescriptorSize  - 0x%x\n", PiSmmCommunicationRegionTable->DescriptorSize));
+  DEBUG ((EFI_D_INFO, "Entry:(0x%x)\n", Entry));
+  DEBUG ((EFI_D_INFO, "  Type            - 0x%x\n", Entry->Type));
+  DEBUG ((EFI_D_INFO, "  PhysicalStart   - 0x%lx\n", Entry->PhysicalStart));
+  DEBUG ((EFI_D_INFO, "  VirtualStart    - 0x%lx\n", Entry->VirtualStart));
+  DEBUG ((EFI_D_INFO, "  NumberOfPages   - 0x%lx\n", Entry->NumberOfPages));
+  DEBUG ((EFI_D_INFO, "  Attribute       - 0x%lx\n", Entry->Attribute));
+
+  //
+  // Publish this table, so that other driver can use the buffer.
+  //
+  Status = gBS->InstallConfigurationTable (&gEdkiiPiSmmCommunicationRegionTableGuid, PiSmmCommunicationRegionTable);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.inf b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.inf
new file mode 100644
index 0000000000..84a2318173
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.inf
@@ -0,0 +1,62 @@
+## @file
+# A driver allocates common SMM communication buffer in EfiReservedMemoryType.
+#
+# This driver allocates common SMM communication buffer in EfiReservedMemoryType,
+# then it publishes the information to EFI configuration table with
+# gEdkiiPiSmmCommunicationRegionTableGuid.
+# Any other driver or application can get the table and know the common
+# communication buffer.
+#
+# Copyright (c) 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SmmCommunicationBufferDxe
+  MODULE_UNI_FILE                = SmmCommunicationBufferDxe.uni
+  FILE_GUID                      = 8FAAD0A7-02B4-432F-8F5C-B880965D8B41
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SmmCommunicationBufferEntryPoint
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  SmmCommunicationBufferDxe.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  UefiBootServicesTableLib
+  UefiRuntimeServicesTableLib
+  BaseLib
+  BaseMemoryLib
+  MemoryAllocationLib
+  HobLib
+  DebugLib
+  PcdLib
+
+[Guids]
+  gEdkiiPiSmmCommunicationRegionTableGuid     ## PRODUCES   ## SystemTable
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  SmmCommunicationBufferExtraDxe.uni
diff --git a/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.uni b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.uni
new file mode 100644
index 0000000000..dfb5579c45
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferDxe.uni
@@ -0,0 +1,24 @@
+// /** @file
+// A driver allocates common SMM communication buffer in EfiReservedMemoryType.
+//
+// This driver allocates common SMM communication buffer in EfiReservedMemoryType,
+// then it publishes the information to EFI configuration table with
+// gEdkiiPiSmmCommunicationRegionTableGuid.
+// Any other driver or application can get the table and know the common
+// communication buffer.
+//
+// Copyright (c) 2016, 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.
+//
+// **/
+
+#string STR_MODULE_ABSTRACT             #language en-US "A driver allocates common SMM communication buffer in EfiReservedMemoryType."
+
+#string STR_MODULE_DESCRIPTION          #language en-US "This driver allocates common SMM communication buffer in EfiReservedMemoryType, then it publishes the information to EFI configuration table with gEdkiiPiSmmCommunicationRegionTableGuid. Any other driver or application can get the table and know the common communication buffer."
diff --git a/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferExtraDxe.uni b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferExtraDxe.uni
new file mode 100644
index 0000000000..14b3a69bf4
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/SmmCommunicationBufferDxe/SmmCommunicationBufferExtraDxe.uni
@@ -0,0 +1,18 @@
+// /** @file
+// SmmCommunicationBuffer Localized Strings and Content
+//
+// Copyright (c) 2016, 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.
+//
+// **/
+
+#string STR_PROPERTIES_MODULE_NAME
+#language en-US
+"SMM Communication Buffer DXE Driver"
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/MemoryStausCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/MemoryStausCodeWorker.c
new file mode 100644
index 0000000000..bdf1d2700e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/MemoryStausCodeWorker.c
@@ -0,0 +1,127 @@
+/** @file
+  PEI memory status code worker.
+
+  Copyright (c) 2006 - 2010, 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 "StatusCodeHandlerPei.h"
+
+/**
+  Create the first memory status code GUID'ed HOB as initialization for memory status code worker.
+
+  @retval EFI_SUCCESS  The GUID'ed HOB is created successfully.
+
+**/
+EFI_STATUS
+MemoryStatusCodeInitializeWorker (
+  VOID
+  )
+{
+  //
+  // Create memory status code GUID'ed HOB.
+  //
+  MEMORY_STATUSCODE_PACKET_HEADER *PacketHeader;
+
+  //
+  // Build GUID'ed HOB with PCD defined size.
+  //
+  PacketHeader = BuildGuidHob (
+                   &gMemoryStatusCodeRecordGuid,
+                   PcdGet16 (PcdStatusCodeMemorySize) * 1024 + sizeof (MEMORY_STATUSCODE_PACKET_HEADER)
+                   );
+  ASSERT (PacketHeader != NULL);
+
+  PacketHeader->MaxRecordsNumber = (PcdGet16 (PcdStatusCodeMemorySize) * 1024) / sizeof (MEMORY_STATUSCODE_RECORD);
+  PacketHeader->PacketIndex      = 0;
+  PacketHeader->RecordIndex      = 0;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Report status code into GUID'ed HOB.
+
+  This function reports status code into GUID'ed HOB. If not all packets are full, then
+  write status code into available entry. Otherwise, create a new packet for it.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      The function always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+MemoryStatusCodeReportWorker (
+  IN CONST  EFI_PEI_SERVICES    **PeiServices,
+  IN EFI_STATUS_CODE_TYPE       CodeType,
+  IN EFI_STATUS_CODE_VALUE      Value,
+  IN UINT32                     Instance,
+  IN CONST EFI_GUID             *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA *Data OPTIONAL
+  )
+{
+
+  EFI_PEI_HOB_POINTERS              Hob;
+  MEMORY_STATUSCODE_PACKET_HEADER   *PacketHeader;
+  MEMORY_STATUSCODE_RECORD          *Record;
+
+  //
+  // Find GUID'ed HOBs to locate current record buffer. 
+  //
+  Hob.Raw = GetFirstGuidHob (&gMemoryStatusCodeRecordGuid);
+  ASSERT (Hob.Raw != NULL);
+
+  PacketHeader = (MEMORY_STATUSCODE_PACKET_HEADER *) GET_GUID_HOB_DATA (Hob.Guid);
+  Record = (MEMORY_STATUSCODE_RECORD *) (PacketHeader + 1);
+  Record = &Record[PacketHeader->RecordIndex++];
+
+  //
+  // Save status code.
+  //
+  Record->CodeType = CodeType;
+  Record->Instance = Instance;
+  Record->Value    = Value;
+
+  //
+  // If record index equals to max record number, then wrap around record index to zero.
+  //
+  // The reader of status code should compare the number of records with max records number,
+  // If it is equal to or larger than the max number, then the wrap-around had happened,
+  // so the first record is pointed by record index.
+  // If it is less then max number, index of the first record is zero.
+  //
+  if (PacketHeader->RecordIndex == PacketHeader->MaxRecordsNumber) {
+    //
+    // Wrap around record index.
+    //
+    PacketHeader->RecordIndex = 0;
+    PacketHeader->PacketIndex ++;
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/SerialStatusCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/SerialStatusCodeWorker.c
new file mode 100644
index 0000000000..1b891b307d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/SerialStatusCodeWorker.c
@@ -0,0 +1,167 @@
+/** @file
+  Serial I/O status code reporting worker.
+
+  Copyright (c) 2006 - 2014, 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 "StatusCodeHandlerPei.h"
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN CONST  EFI_PEI_SERVICES        **PeiServices,
+  IN EFI_STATUS_CODE_TYPE           CodeType,
+  IN EFI_STATUS_CODE_VALUE          Value,
+  IN UINT32                         Instance,
+  IN CONST EFI_GUID                 *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  )
+{
+  CHAR8           *Filename;
+  CHAR8           *Description;
+  CHAR8           *Format;
+  CHAR8           Buffer[MAX_DEBUG_MESSAGE_LENGTH];
+  UINT32          ErrorLevel;
+  UINT32          LineNumber;
+  UINTN           CharCount;
+  BASE_LIST       Marker;
+
+  Buffer[0] = '\0';
+
+  if (Data != NULL &&
+      ReportStatusCodeExtractAssertInfo (CodeType, Value, Data, &Filename, &Description, &LineNumber)) {
+    //
+    // Print ASSERT() information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "\n\rPEI_ASSERT!: %a (%d): %a\n\r",
+                  Filename,
+                  LineNumber,
+                  Description
+                  );
+  } else if (Data != NULL &&
+             ReportStatusCodeExtractDebugInfo (Data, &ErrorLevel, &Marker, &Format)) {
+    //
+    // Print DEBUG() information into output buffer.
+    //
+    CharCount = AsciiBSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  Format,
+                  Marker
+                  );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) {
+    //
+    // Print ERROR information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "ERROR: C%08x:V%08x I%x",
+                  CodeType,
+                  Value,
+                  Instance
+                  );
+    
+    ASSERT(CharCount > 0);
+    
+    if (CallerId != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %g",
+                     CallerId
+                     );
+    }
+
+    if (Data != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %x",
+                     Data
+                     );
+    }
+
+    CharCount += AsciiSPrint (
+                   &Buffer[CharCount],
+                   (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                   "\n\r"
+                   );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) {
+    //
+    // Print PROGRESS information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "PROGRESS CODE: V%08x I%x\n\r",
+                  Value,
+                  Instance
+                  );
+  } else if (Data != NULL &&
+             CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeStringGuid) &&
+             ((EFI_STATUS_CODE_STRING_DATA *) Data)->StringType == EfiStringAscii) {
+    //
+    // EFI_STATUS_CODE_STRING_DATA
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "%a\n\r",
+                  ((EFI_STATUS_CODE_STRING_DATA *) Data)->String.Ascii
+                  );
+  } else {
+    //
+    // Code type is not defined.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "Undefined: C%08x:V%08x I%x\n\r",
+                  CodeType,
+                  Value,
+                  Instance
+                  );
+  }
+
+  //
+  // Call SerialPort Lib function to do print.
+  //
+  SerialPortWrite ((UINT8 *) Buffer, CharCount);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.c
new file mode 100644
index 0000000000..368f2056d9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.c
@@ -0,0 +1,69 @@
+/** @file
+  Report Status Code Handler PEIM which produces general handlers and hook them
+  onto the PEI status code router.
+
+  Copyright (c) 2009, 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 "StatusCodeHandlerPei.h"
+
+/**
+  Entry point of Status Code PEIM.
+  
+  This function is the entry point of this Status Code PEIM.
+  It initializes supported status code devices according to PCD settings,
+  and installs Status Code PPI.
+
+  @param  FileHandle  Handle of the file being invoked.
+  @param  PeiServices Describes the list of possible PEI Services.
+
+  @retval EFI_SUCESS  The entry point of DXE IPL PEIM executes successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+StatusCodeHandlerPeiEntry (
+  IN       EFI_PEI_FILE_HANDLE  FileHandle,
+  IN CONST EFI_PEI_SERVICES     **PeiServices
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_PEI_RSC_HANDLER_PPI     *RscHandlerPpi;
+
+  Status = PeiServicesLocatePpi (
+             &gEfiPeiRscHandlerPpiGuid,
+             0,
+             NULL,
+             (VOID **) &RscHandlerPpi
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Dispatch initialization request to sub-statuscode-devices.
+  // If enable UseSerial, then initialize serial port.
+  // if enable UseMemory, then initialize memory status code worker.
+  //
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    Status = SerialPortInitialize();
+    ASSERT_EFI_ERROR (Status);
+    Status = RscHandlerPpi->Register (SerialStatusCodeReportWorker);                     
+    ASSERT_EFI_ERROR (Status);
+  }
+  if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+    Status = MemoryStatusCodeInitializeWorker ();
+    ASSERT_EFI_ERROR (Status);
+    Status = RscHandlerPpi->Register (MemoryStatusCodeReportWorker);                     
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.h b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.h
new file mode 100644
index 0000000000..d2434292ec
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.h
@@ -0,0 +1,125 @@
+/** @file
+  Internal include file for Status Code Handler PEIM.
+
+  Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef __STATUS_CODE_HANDLER_PEI_H__
+#define __STATUS_CODE_HANDLER_PEI_H__
+
+
+#include <Ppi/ReportStatusCodeHandler.h>
+
+#include <Guid/MemoryStatusCodeRecord.h>
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PrintLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/SerialPortLib.h>
+#include <Library/HobLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/BaseMemoryLib.h>
+
+//
+// Define the maximum message length
+//
+#define MAX_DEBUG_MESSAGE_LENGTH 0x100
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN CONST  EFI_PEI_SERVICES        **PeiServices,
+  IN EFI_STATUS_CODE_TYPE           CodeType,
+  IN EFI_STATUS_CODE_VALUE          Value,
+  IN UINT32                         Instance,
+  IN CONST EFI_GUID                 *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  );
+
+
+/**
+  Create the first memory status code GUID'ed HOB as initialization for memory status code worker.
+
+  @retval EFI_SUCCESS  The GUID'ed HOB is created successfully.
+
+**/
+EFI_STATUS
+MemoryStatusCodeInitializeWorker (
+  VOID
+  );
+
+/**
+  Report status code into GUID'ed HOB.
+
+  This function reports status code into GUID'ed HOB. If not all packets are full, then
+  write status code into available entry. Otherwise, create a new packet for it.
+
+  @param  PeiServices      An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or
+                           software entity. This includes information about the class and
+                           subclass that is used to classify the entity as well as an operation.
+                           For progress codes, the operation is the current activity.
+                           For error codes, it is the exception.For debug codes,it is not defined at this time.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. A system may contain multiple entities that match a class/subclass
+                           pairing. The instance differentiates between them. An instance of 0 indicates
+                           that instance information is unavailable, not meaningful, or not relevant.
+                           Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      The function always return EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+MemoryStatusCodeReportWorker (
+  IN CONST  EFI_PEI_SERVICES    **PeiServices,
+  IN EFI_STATUS_CODE_TYPE       CodeType,
+  IN EFI_STATUS_CODE_VALUE      Value,
+  IN UINT32                     Instance,
+  IN CONST EFI_GUID             *CallerId,
+  IN CONST EFI_STATUS_CODE_DATA *Data OPTIONAL
+  );
+
+#endif
+
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.inf b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.inf
new file mode 100644
index 0000000000..7c4faa360e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.inf
@@ -0,0 +1,72 @@
+## @file
+#  Report Status Code Handler PEIM which produces general handlers and hook them onto the PEI status code router.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = StatusCodeHandlerPei
+  MODULE_UNI_FILE                = StatusCodeHandlerPei.uni
+  FILE_GUID                      = 9D225237-FA01-464C-A949-BAABC02D31D0
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0  
+  ENTRY_POINT                    = StatusCodeHandlerPeiEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC (EBC is only for build)
+#
+
+[Sources]
+  StatusCodeHandlerPei.c
+  StatusCodeHandlerPei.h
+  SerialStatusCodeWorker.c
+  MemoryStausCodeWorker.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  PeimEntryPoint
+  PeiServicesLib
+  PcdLib
+  HobLib
+  SerialPortLib
+  ReportStatusCodeLib
+  PrintLib
+  DebugLib
+  BaseMemoryLib
+  
+[Guids]
+  ## SOMETIMES_PRODUCES   ## HOB
+  ## SOMETIMES_CONSUMES   ## HOB
+  gMemoryStatusCodeRecordGuid
+  gEfiStatusCodeDataTypeStringGuid              ## SOMETIMES_CONSUMES   ## UNDEFINED
+  
+[Ppis]
+  gEfiPeiRscHandlerPpiGuid                      ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseSerial ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeMemorySize|1|gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory    ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiPeiRscHandlerPpiGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  StatusCodeHandlerPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.uni
new file mode 100644
index 0000000000..2e2985636a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPei.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPeiExtra.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPeiExtra.uni
new file mode 100644
index 0000000000..60cc966cc3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Pei/StatusCodeHandlerPeiExtra.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/MemoryStatusCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/MemoryStatusCodeWorker.c
new file mode 100644
index 0000000000..8680497693
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/MemoryStatusCodeWorker.c
@@ -0,0 +1,111 @@
+/** @file
+  Runtime memory status code worker.
+
+  Copyright (c) 2006 - 2009, 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 "StatusCodeHandlerRuntimeDxe.h"
+
+RUNTIME_MEMORY_STATUSCODE_HEADER  *mRtMemoryStatusCodeTable;
+
+/**
+  Initialize runtime memory status code table as initialization for runtime memory status code worker
+ 
+  @retval EFI_SUCCESS  Runtime memory status code table successfully initialized.
+
+**/
+EFI_STATUS
+RtMemoryStatusCodeInitializeWorker (
+  VOID
+  )
+{
+  //
+  // Allocate runtime memory status code pool.
+  //
+  mRtMemoryStatusCodeTable = AllocateRuntimePool (
+                               sizeof (RUNTIME_MEMORY_STATUSCODE_HEADER) +
+                               PcdGet16 (PcdStatusCodeMemorySize) * 1024
+                               );
+  ASSERT (mRtMemoryStatusCodeTable != NULL);
+
+  mRtMemoryStatusCodeTable->RecordIndex      = 0;
+  mRtMemoryStatusCodeTable->NumberOfRecords  = 0;
+  mRtMemoryStatusCodeTable->MaxRecordsNumber = 
+    (PcdGet16 (PcdStatusCodeMemorySize) * 1024) / sizeof (MEMORY_STATUSCODE_RECORD);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Report status code into runtime memory. If the runtime pool is full, roll back to the 
+  first record and overwrite it.
+ 
+  @param  CodeType                Indicates the type of status code being reported.
+  @param  Value                   Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param  Instance                The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param  CallerId                This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param  Data                    This optional parameter may be used to pass additional data.
+ 
+  @retval EFI_SUCCESS             Status code successfully recorded in runtime memory status code table.
+
+**/
+EFI_STATUS
+EFIAPI
+RtMemoryStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE               CodeType,
+  IN EFI_STATUS_CODE_VALUE              Value,
+  IN UINT32                             Instance,
+  IN EFI_GUID                           *CallerId,
+  IN EFI_STATUS_CODE_DATA               *Data OPTIONAL
+  )
+{
+  MEMORY_STATUSCODE_RECORD              *Record;
+
+  //
+  // Locate current record buffer.
+  //
+  Record = (MEMORY_STATUSCODE_RECORD *) (mRtMemoryStatusCodeTable + 1);
+  Record = &Record[mRtMemoryStatusCodeTable->RecordIndex++];
+
+  //
+  // Save status code.
+  //
+  Record->CodeType = CodeType;
+  Record->Value    = Value;
+  Record->Instance = Instance;
+
+  //
+  // If record index equals to max record number, then wrap around record index to zero.
+  //
+  // The reader of status code should compare the number of records with max records number,
+  // If it is equal to or larger than the max number, then the wrap-around had happened,
+  // so the first record is pointed by record index.
+  // If it is less then max number, index of the first record is zero.
+  //
+  mRtMemoryStatusCodeTable->NumberOfRecords++;
+  if (mRtMemoryStatusCodeTable->RecordIndex == mRtMemoryStatusCodeTable->MaxRecordsNumber) {
+    //
+    // Wrap around record index.
+    //
+    mRtMemoryStatusCodeTable->RecordIndex = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/SerialStatusCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/SerialStatusCodeWorker.c
new file mode 100644
index 0000000000..45986fcae3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/SerialStatusCodeWorker.c
@@ -0,0 +1,162 @@
+/** @file
+  Serial I/O status code reporting worker.
+
+  Copyright (c) 2006 - 2014, 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 "StatusCodeHandlerRuntimeDxe.h"
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+ 
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work after ExitBootService() is called.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work with TPL higher than TPL_CALLBACK.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  )
+{
+  CHAR8           *Filename;
+  CHAR8           *Description;
+  CHAR8           *Format;
+  CHAR8           Buffer[MAX_DEBUG_MESSAGE_LENGTH];
+  UINT32          ErrorLevel;
+  UINT32          LineNumber;
+  UINTN           CharCount;
+  BASE_LIST       Marker;
+
+  Buffer[0] = '\0';
+
+  if (Data != NULL &&
+      ReportStatusCodeExtractAssertInfo (CodeType, Value, Data, &Filename, &Description, &LineNumber)) {
+    //
+    // Print ASSERT() information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "\n\rDXE_ASSERT!: %a (%d): %a\n\r",
+                  Filename,
+                  LineNumber,
+                  Description
+                  );
+  } else if (Data != NULL &&
+             ReportStatusCodeExtractDebugInfo (Data, &ErrorLevel, &Marker, &Format)) {
+    //
+    // Print DEBUG() information into output buffer.
+    //
+    CharCount = AsciiBSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  Format, 
+                  Marker
+                  );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) {
+    //
+    // Print ERROR information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "ERROR: C%08x:V%08x I%x", 
+                  CodeType, 
+                  Value, 
+                  Instance
+                  );
+    ASSERT (CharCount > 0);
+   
+    if (CallerId != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %g",
+                     CallerId
+                     );
+    }
+
+    if (Data != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %x",
+                     Data
+                     );
+    }
+
+    CharCount += AsciiSPrint (
+                   &Buffer[CharCount],
+                   (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                   "\n\r"
+                   );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) {
+    //
+    // Print PROGRESS information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "PROGRESS CODE: V%08x I%x\n\r", 
+                  Value, 
+                  Instance
+                  );
+  } else if (Data != NULL &&
+             CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeStringGuid) &&
+             ((EFI_STATUS_CODE_STRING_DATA *) Data)->StringType == EfiStringAscii) {
+    //
+    // EFI_STATUS_CODE_STRING_DATA
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "%a\n\r",
+                  ((EFI_STATUS_CODE_STRING_DATA *) Data)->String.Ascii
+                  );
+  } else {
+    //
+    // Code type is not defined.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "Undefined: C%08x:V%08x I%x\n\r", 
+                  CodeType, 
+                  Value, 
+                  Instance
+                  );
+  }
+
+  //
+  // Call SerialPort Lib function to do print.
+  //
+  SerialPortWrite ((UINT8 *) Buffer, CharCount);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.c
new file mode 100644
index 0000000000..9149ca4efa
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.c
@@ -0,0 +1,207 @@
+/** @file
+  Status Code Handler Driver which produces general handlers and hook them
+  onto the DXE status code router.
+
+  Copyright (c) 2006 - 2009, 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 "StatusCodeHandlerRuntimeDxe.h"
+
+EFI_EVENT                 mVirtualAddressChangeEvent = NULL;
+EFI_EVENT                 mExitBootServicesEvent     = NULL;
+EFI_RSC_HANDLER_PROTOCOL  *mRscHandlerProtocol       = NULL;
+
+/**
+  Unregister status code callback functions only available at boot time from
+  report status code router when exiting boot services.
+
+  @param  Event         Event whose notification function is being invoked.
+  @param  Context       Pointer to the notification function's context, which is
+                        always zero in current implementation.
+
+**/
+VOID
+EFIAPI
+UnregisterBootTimeHandlers (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    mRscHandlerProtocol->Unregister (SerialStatusCodeReportWorker);
+  }
+}
+
+/**
+  Virtual address change notification call back. It converts global pointer
+  to virtual address.
+
+  @param  Event         Event whose notification function is being invoked.
+  @param  Context       Pointer to the notification function's context, which is
+                        always zero in current implementation.
+
+**/
+VOID
+EFIAPI
+VirtualAddressChangeCallBack (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  //
+  // Convert memory status code table to virtual address;
+  //
+  EfiConvertPointer (
+    0,
+    (VOID **) &mRtMemoryStatusCodeTable
+    );
+}
+
+/**
+  Dispatch initialization request to sub status code devices based on 
+  customized feature flags.
+ 
+**/
+VOID
+InitializationDispatcherWorker (
+  VOID
+  )
+{
+  EFI_PEI_HOB_POINTERS              Hob;
+  EFI_STATUS                        Status;
+  MEMORY_STATUSCODE_PACKET_HEADER   *PacketHeader;
+  MEMORY_STATUSCODE_RECORD          *Record;
+  UINTN                             Index;
+  UINTN                             MaxRecordNumber;
+
+  //
+  // If enable UseSerial, then initialize serial port.
+  // if enable UseRuntimeMemory, then initialize runtime memory status code worker.
+  //
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    //
+    // Call Serial Port Lib API to initialize serial port.
+    //
+    Status = SerialPortInitialize ();
+    ASSERT_EFI_ERROR (Status);
+  }
+  if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+    Status = RtMemoryStatusCodeInitializeWorker ();
+    ASSERT_EFI_ERROR (Status);
+  }
+
+  //
+  // Replay Status code which saved in GUID'ed HOB to all supported devices. 
+  //
+  if (FeaturePcdGet (PcdStatusCodeReplayIn)) {
+    // 
+    // Journal GUID'ed HOBs to find all record entry, if found, 
+    // then output record to support replay device.
+    //
+    Hob.Raw   = GetFirstGuidHob (&gMemoryStatusCodeRecordGuid);
+    if (Hob.Raw != NULL) {
+      PacketHeader = (MEMORY_STATUSCODE_PACKET_HEADER *) GET_GUID_HOB_DATA (Hob.Guid);
+      Record = (MEMORY_STATUSCODE_RECORD *) (PacketHeader + 1);
+      MaxRecordNumber = (UINTN) PacketHeader->RecordIndex;
+      if (PacketHeader->PacketIndex > 0) {
+        //
+        // Record has been wrapped around. So, record number has arrived at max number.
+        //
+        MaxRecordNumber = (UINTN) PacketHeader->MaxRecordsNumber;
+      }
+      for (Index = 0; Index < MaxRecordNumber; Index++) {
+        //
+        // Dispatch records to devices based on feature flag.
+        //
+        if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+          SerialStatusCodeReportWorker (
+            Record[Index].CodeType,
+            Record[Index].Value,
+            Record[Index].Instance,
+            NULL,
+            NULL
+            );
+        }
+        if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+          RtMemoryStatusCodeReportWorker (
+            Record[Index].CodeType,
+            Record[Index].Value,
+            Record[Index].Instance,
+            NULL,
+            NULL
+            );
+        }
+      }
+    }
+  }
+}
+
+/**
+  Entry point of DXE Status Code Driver.
+
+  This function is the entry point of this DXE Status Code Driver.
+  It initializes registers status code handlers, and registers event for EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+StatusCodeHandlerRuntimeDxeEntry (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+
+  Status = gBS->LocateProtocol (
+                  &gEfiRscHandlerProtocolGuid,
+                  NULL,
+                  (VOID **) &mRscHandlerProtocol
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Dispatch initialization request to supported devices
+  //
+  InitializationDispatcherWorker ();
+
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    mRscHandlerProtocol->Register (SerialStatusCodeReportWorker, TPL_HIGH_LEVEL);
+  }
+  if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+    mRscHandlerProtocol->Register (RtMemoryStatusCodeReportWorker, TPL_HIGH_LEVEL);
+  }
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  UnregisterBootTimeHandlers,
+                  NULL,
+                  &gEfiEventExitBootServicesGuid,
+                  &mExitBootServicesEvent
+                  );
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  VirtualAddressChangeCallBack,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mVirtualAddressChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.h b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.h
new file mode 100644
index 0000000000..2bf341202e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.h
@@ -0,0 +1,134 @@
+/** @file
+  Internal include file for Status Code Handler Driver.
+
+  Copyright (c) 2006 - 2012, 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.
+
+**/
+
+#ifndef __STATUS_CODE_HANDLER_RUNTIME_DXE_H__
+#define __STATUS_CODE_HANDLER_RUNTIME_DXE_H__
+
+#include <Protocol/ReportStatusCodeHandler.h>
+
+#include <Guid/MemoryStatusCodeRecord.h>
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+#include <Guid/EventGroup.h>
+
+#include <Library/SynchronizationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PrintLib.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/SerialPortLib.h>
+
+//
+// Define the maximum message length
+//
+#define MAX_DEBUG_MESSAGE_LENGTH 0x100
+
+//
+// Runtime memory status code worker definition
+//
+typedef struct {
+  UINT32   RecordIndex;
+  UINT32   NumberOfRecords;
+  UINT32   MaxRecordsNumber;
+} RUNTIME_MEMORY_STATUSCODE_HEADER;
+
+extern RUNTIME_MEMORY_STATUSCODE_HEADER  *mRtMemoryStatusCodeTable;
+
+/**
+  Locates Serial I/O Protocol as initialization for serial status code worker.
+ 
+  @retval EFI_SUCCESS  Serial I/O Protocol is successfully located.
+
+**/
+EFI_STATUS
+EfiSerialStatusCodeInitializeWorker (
+  VOID
+  );
+
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+ 
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work after ExitBootService() is called.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work with TPL higher than TPL_CALLBACK.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  );
+
+/**
+  Initialize runtime memory status code table as initialization for runtime memory status code worker
+ 
+  @retval EFI_SUCCESS  Runtime memory status code table successfully initialized.
+
+**/
+EFI_STATUS
+RtMemoryStatusCodeInitializeWorker (
+  VOID
+  );
+
+/**
+  Report status code into runtime memory. If the runtime pool is full, roll back to the 
+  first record and overwrite it.
+ 
+  @param  CodeType                Indicates the type of status code being reported.
+  @param  Value                   Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param  Instance                The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param  CallerId                This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param  Data                    This optional parameter may be used to pass additional data.
+ 
+  @retval EFI_SUCCESS             Status code successfully recorded in runtime memory status code table.
+
+**/
+EFI_STATUS
+EFIAPI
+RtMemoryStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE               CodeType,
+  IN EFI_STATUS_CODE_VALUE              Value,
+  IN UINT32                             Instance,
+  IN EFI_GUID                           *CallerId,
+  IN EFI_STATUS_CODE_DATA               *Data OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.inf b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.inf
new file mode 100644
index 0000000000..ba22ca48cf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.inf
@@ -0,0 +1,75 @@
+## @file
+#  Status Code Handler Driver which produces general handlers and hook them onto the DXE status code router.
+#
+#  Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = StatusCodeHandlerRuntimeDxe
+  MODULE_UNI_FILE                = StatusCodeHandlerRuntimeDxe.uni
+  FILE_GUID                      = 6C2004EF-4E0E-4BE4-B14C-340EB4AA5891
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = StatusCodeHandlerRuntimeDxeEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  StatusCodeHandlerRuntimeDxe.c
+  StatusCodeHandlerRuntimeDxe.h
+  SerialStatusCodeWorker.c
+  MemoryStatusCodeWorker.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SerialPortLib
+  UefiRuntimeLib
+  MemoryAllocationLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  HobLib
+  PcdLib
+  PrintLib
+  ReportStatusCodeLib
+  DebugLib
+  BaseMemoryLib
+  
+[Guids]
+  gMemoryStatusCodeRecordGuid                   ## SOMETIMES_CONSUMES   ## HOB
+  gEfiStatusCodeDataTypeStringGuid              ## SOMETIMES_CONSUMES   ## UNDEFINED
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES ## Event
+  gEfiEventExitBootServicesGuid                 ## CONSUMES ## Event
+
+[Protocols]
+  gEfiRscHandlerProtocolGuid                    ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeReplayIn  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseSerial ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeMemorySize |128| gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory   ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiRscHandlerProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  StatusCodeHandlerRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.uni
new file mode 100644
index 0000000000..4051091dbe
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..67638c621b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/RuntimeDxe/StatusCodeHandlerRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/MemoryStatusCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/MemoryStatusCodeWorker.c
new file mode 100644
index 0000000000..78d908b16e
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/MemoryStatusCodeWorker.c
@@ -0,0 +1,104 @@
+/** @file
+  Runtime memory status code worker.
+
+  Copyright (c) 2006 - 2010, 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 "StatusCodeHandlerSmm.h"
+
+RUNTIME_MEMORY_STATUSCODE_HEADER  *mSmmMemoryStatusCodeTable;
+
+/**
+  Initialize SMM memory status code table as initialization for memory status code worker
+ 
+  @retval EFI_SUCCESS  SMM memory status code table successfully initialized.
+
+**/
+EFI_STATUS
+MemoryStatusCodeInitializeWorker (
+  VOID
+  )
+{
+  //
+  // Allocate SMM memory status code pool.
+  //
+  mSmmMemoryStatusCodeTable = (RUNTIME_MEMORY_STATUSCODE_HEADER *)AllocateZeroPool (sizeof (RUNTIME_MEMORY_STATUSCODE_HEADER) + PcdGet16 (PcdStatusCodeMemorySize) * 1024);
+  ASSERT (mSmmMemoryStatusCodeTable != NULL);
+
+  mSmmMemoryStatusCodeTable->MaxRecordsNumber = (PcdGet16 (PcdStatusCodeMemorySize) * 1024) / sizeof (MEMORY_STATUSCODE_RECORD);
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Report status code into runtime memory. If the runtime pool is full, roll back to the 
+  first record and overwrite it.
+ 
+  @param  CodeType                Indicates the type of status code being reported.
+  @param  Value                   Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param  Instance                The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param  CallerId                This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param  Data                    This optional parameter may be used to pass additional data.
+ 
+  @retval EFI_SUCCESS             Status code successfully recorded in runtime memory status code table.
+
+**/
+EFI_STATUS
+EFIAPI
+MemoryStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE               CodeType,
+  IN EFI_STATUS_CODE_VALUE              Value,
+  IN UINT32                             Instance,
+  IN EFI_GUID                           *CallerId,
+  IN EFI_STATUS_CODE_DATA               *Data OPTIONAL
+  )
+{
+  MEMORY_STATUSCODE_RECORD              *Record;
+
+  //
+  // Locate current record buffer.
+  //
+  Record = (MEMORY_STATUSCODE_RECORD *) (mSmmMemoryStatusCodeTable + 1);
+  Record = &Record[mSmmMemoryStatusCodeTable->RecordIndex++];
+
+  //
+  // Save status code.
+  //
+  Record->CodeType = CodeType;
+  Record->Value    = Value;
+  Record->Instance = Instance;
+
+  //
+  // If record index equals to max record number, then wrap around record index to zero.
+  //
+  // The reader of status code should compare the number of records with max records number,
+  // If it is equal to or larger than the max number, then the wrap-around had happened,
+  // so the first record is pointed by record index.
+  // If it is less then max number, index of the first record is zero.
+  //
+  mSmmMemoryStatusCodeTable->NumberOfRecords++;
+  if (mSmmMemoryStatusCodeTable->RecordIndex == mSmmMemoryStatusCodeTable->MaxRecordsNumber) {
+    //
+    // Wrap around record index.
+    //
+    mSmmMemoryStatusCodeTable->RecordIndex = 0;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/SerialStatusCodeWorker.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/SerialStatusCodeWorker.c
new file mode 100644
index 0000000000..8f2fbf4f7a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/SerialStatusCodeWorker.c
@@ -0,0 +1,162 @@
+/** @file
+  Serial I/O status code reporting worker.
+
+  Copyright (c) 2009 - 2014, 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 "StatusCodeHandlerSmm.h"
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+ 
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work after ExitBootService() is called.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work with TPL higher than TPL_CALLBACK.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  )
+{
+  CHAR8           *Filename;
+  CHAR8           *Description;
+  CHAR8           *Format;
+  CHAR8           Buffer[MAX_DEBUG_MESSAGE_LENGTH];
+  UINT32          ErrorLevel;
+  UINT32          LineNumber;
+  UINTN           CharCount;
+  BASE_LIST       Marker;
+
+  Buffer[0] = '\0';
+
+  if (Data != NULL &&
+      ReportStatusCodeExtractAssertInfo (CodeType, Value, Data, &Filename, &Description, &LineNumber)) {
+    //
+    // Print ASSERT() information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "\n\rDXE_ASSERT!: %a (%d): %a\n\r",
+                  Filename,
+                  LineNumber,
+                  Description
+                  );
+  } else if (Data != NULL &&
+             ReportStatusCodeExtractDebugInfo (Data, &ErrorLevel, &Marker, &Format)) {
+    //
+    // Print DEBUG() information into output buffer.
+    //
+    CharCount = AsciiBSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  Format, 
+                  Marker
+                  );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_ERROR_CODE) {
+    //
+    // Print ERROR information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "ERROR: C%08x:V%08x I%x", 
+                  CodeType, 
+                  Value, 
+                  Instance
+                  );
+    ASSERT (CharCount > 0);
+   
+    if (CallerId != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %g",
+                     CallerId
+                     );
+    }
+
+    if (Data != NULL) {
+      CharCount += AsciiSPrint (
+                     &Buffer[CharCount],
+                     (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                     " %x",
+                     Data
+                     );
+    }
+
+    CharCount += AsciiSPrint (
+                   &Buffer[CharCount],
+                   (sizeof (Buffer) - (sizeof (Buffer[0]) * CharCount)),
+                   "\n\r"
+                   );
+  } else if ((CodeType & EFI_STATUS_CODE_TYPE_MASK) == EFI_PROGRESS_CODE) {
+    //
+    // Print PROGRESS information into output buffer.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "PROGRESS CODE: V%08x I%x\n\r", 
+                  Value, 
+                  Instance
+                  );
+  } else if (Data != NULL &&
+             CompareGuid (&Data->Type, &gEfiStatusCodeDataTypeStringGuid) &&
+             ((EFI_STATUS_CODE_STRING_DATA *) Data)->StringType == EfiStringAscii) {
+    //
+    // EFI_STATUS_CODE_STRING_DATA
+    //
+    CharCount = AsciiSPrint (
+                  Buffer,
+                  sizeof (Buffer),
+                  "%a\n\r",
+                  ((EFI_STATUS_CODE_STRING_DATA *) Data)->String.Ascii
+                  );
+  } else {
+    //
+    // Code type is not defined.
+    //
+    CharCount = AsciiSPrint (
+                  Buffer, 
+                  sizeof (Buffer), 
+                  "Undefined: C%08x:V%08x I%x\n\r", 
+                  CodeType, 
+                  Value, 
+                  Instance
+                  );
+  }
+
+  //
+  // Call SerialPort Lib function to do print.
+  //
+  SerialPortWrite ((UINT8 *) Buffer, CharCount);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.c b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.c
new file mode 100644
index 0000000000..5e84ea43da
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.c
@@ -0,0 +1,90 @@
+/** @file
+  Status Code Handler Driver which produces general handlers and hook them
+  onto the SMM status code router.
+
+  Copyright (c) 2009, 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 "StatusCodeHandlerSmm.h"
+
+EFI_SMM_RSC_HANDLER_PROTOCOL  *mRscHandlerProtocol       = NULL;
+
+
+/**
+  Dispatch initialization request to sub status code devices based on 
+  customized feature flags.
+ 
+**/
+VOID
+InitializationDispatcherWorker (
+  VOID
+  )
+{
+  EFI_STATUS                        Status;
+
+  //
+  // If enable UseSerial, then initialize serial port.
+  // if enable UseRuntimeMemory, then initialize runtime memory status code worker.
+  //
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    //
+    // Call Serial Port Lib API to initialize serial port.
+    //
+    Status = SerialPortInitialize ();
+    ASSERT_EFI_ERROR (Status);
+  }
+  if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+    Status = MemoryStatusCodeInitializeWorker ();
+    ASSERT_EFI_ERROR (Status);
+  }
+}
+
+/**
+  Entry point of SMM Status Code Driver.
+
+  This function is the entry point of SMM Status Code Driver.
+
+  @param  ImageHandle       The firmware allocated handle for the EFI image.
+  @param  SystemTable       A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       The entry point is executed successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+StatusCodeHandlerSmmEntry (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS                Status;
+
+  Status = gSmst->SmmLocateProtocol (
+                    &gEfiSmmRscHandlerProtocolGuid,
+                    NULL,
+                    (VOID **) &mRscHandlerProtocol
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Dispatch initialization request to supported devices
+  //
+  InitializationDispatcherWorker ();
+
+  if (FeaturePcdGet (PcdStatusCodeUseSerial)) {
+    mRscHandlerProtocol->Register (SerialStatusCodeReportWorker);
+  }
+  if (FeaturePcdGet (PcdStatusCodeUseMemory)) {
+    mRscHandlerProtocol->Register (MemoryStatusCodeReportWorker);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.h b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.h
new file mode 100644
index 0000000000..ee85a69c5a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.h
@@ -0,0 +1,131 @@
+/** @file
+  Internal include file for Status Code Handler Driver.
+
+  Copyright (c) 2009 - 2012, 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.
+
+**/
+
+#ifndef __STATUS_CODE_HANDLER_SMM_H__
+#define __STATUS_CODE_HANDLER_SMM_H__
+
+#include <Protocol/SmmReportStatusCodeHandler.h>
+
+#include <Guid/MemoryStatusCodeRecord.h>
+#include <Guid/StatusCodeDataTypeId.h>
+#include <Guid/StatusCodeDataTypeDebug.h>
+
+#include <Library/SynchronizationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/PrintLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/SmmServicesTableLib.h>
+#include <Library/SerialPortLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+
+//
+// Define the maximum message length
+//
+#define MAX_DEBUG_MESSAGE_LENGTH 0x100
+
+//
+// Runtime memory status code worker definition
+//
+typedef struct {
+  UINT32   RecordIndex;
+  UINT32   NumberOfRecords;
+  UINT32   MaxRecordsNumber;
+} RUNTIME_MEMORY_STATUSCODE_HEADER;
+
+extern RUNTIME_MEMORY_STATUSCODE_HEADER  *mSmmMemoryStatusCodeTable;
+
+/**
+  Locates Serial I/O Protocol as initialization for serial status code worker.
+ 
+  @retval EFI_SUCCESS  Serial I/O Protocol is successfully located.
+
+**/
+EFI_STATUS
+EfiSerialStatusCodeInitializeWorker (
+  VOID
+  );
+
+
+/**
+  Convert status code value and extended data to readable ASCII string, send string to serial I/O device.
+ 
+  @param  CodeType         Indicates the type of status code being reported.
+  @param  Value            Describes the current status of a hardware or software entity.
+                           This included information about the class and subclass that is used to
+                           classify the entity as well as an operation.
+  @param  Instance         The enumeration of a hardware or software entity within
+                           the system. Valid instance numbers start with 1.
+  @param  CallerId         This optional parameter may be used to identify the caller.
+                           This parameter allows the status code driver to apply different rules to
+                           different callers.
+  @param  Data             This optional parameter may be used to pass additional data.
+
+  @retval EFI_SUCCESS      Status code reported to serial I/O successfully.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work after ExitBootService() is called.
+  @retval EFI_DEVICE_ERROR EFI serial device cannot work with TPL higher than TPL_CALLBACK.
+
+**/
+EFI_STATUS
+EFIAPI
+SerialStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE     CodeType,
+  IN EFI_STATUS_CODE_VALUE    Value,
+  IN UINT32                   Instance,
+  IN EFI_GUID                 *CallerId,
+  IN EFI_STATUS_CODE_DATA     *Data OPTIONAL
+  );
+
+/**
+  Initialize runtime memory status code table as initialization for runtime memory status code worker
+ 
+  @retval EFI_SUCCESS  Runtime memory status code table successfully initialized.
+
+**/
+EFI_STATUS
+MemoryStatusCodeInitializeWorker (
+  VOID
+  );
+
+/**
+  Report status code into runtime memory. If the runtime pool is full, roll back to the 
+  first record and overwrite it.
+ 
+  @param  CodeType                Indicates the type of status code being reported.
+  @param  Value                   Describes the current status of a hardware or software entity.
+                                  This included information about the class and subclass that is used to
+                                  classify the entity as well as an operation.
+  @param  Instance                The enumeration of a hardware or software entity within
+                                  the system. Valid instance numbers start with 1.
+  @param  CallerId                This optional parameter may be used to identify the caller.
+                                  This parameter allows the status code driver to apply different rules to
+                                  different callers.
+  @param  Data                    This optional parameter may be used to pass additional data.
+ 
+  @retval EFI_SUCCESS             Status code successfully recorded in runtime memory status code table.
+
+**/
+EFI_STATUS
+EFIAPI
+MemoryStatusCodeReportWorker (
+  IN EFI_STATUS_CODE_TYPE               CodeType,
+  IN EFI_STATUS_CODE_VALUE              Value,
+  IN UINT32                             Instance,
+  IN EFI_GUID                           *CallerId,
+  IN EFI_STATUS_CODE_DATA               *Data OPTIONAL
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.inf b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.inf
new file mode 100644
index 0000000000..1e66fbf83f
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.inf
@@ -0,0 +1,70 @@
+## @file
+#  Status Code Handler Driver which produces general handlers and hook them onto the SMM status code router.
+#
+#  Copyright (c) 2009 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = StatusCodeHandlerSmm
+  MODULE_UNI_FILE                = StatusCodeHandlerSmm.uni
+  FILE_GUID                      = 79CD78D8-6EDC-4978-BD02-3299C387AB17
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  VERSION_STRING                 = 1.0  
+  ENTRY_POINT                    = StatusCodeHandlerSmmEntry
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+[Sources]
+  StatusCodeHandlerSmm.c
+  StatusCodeHandlerSmm.h
+  SerialStatusCodeWorker.c
+  MemoryStatusCodeWorker.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  SerialPortLib
+  SmmServicesTableLib
+  UefiDriverEntryPoint
+  PcdLib
+  PrintLib
+  ReportStatusCodeLib
+  DebugLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  
+[Guids]
+  gEfiStatusCodeDataTypeStringGuid              ## SOMETIMES_CONSUMES   ## UNDEFINED
+
+[Protocols]
+  gEfiSmmRscHandlerProtocolGuid                 ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseSerial ## CONSUMES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeMemorySize |128| gEfiMdeModulePkgTokenSpaceGuid.PcdStatusCodeUseMemory   ## SOMETIMES_CONSUMES
+
+[Depex]
+  gEfiSmmRscHandlerProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  StatusCodeHandlerSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.uni
new file mode 100644
index 0000000000..94507a04c3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmm.uni
diff --git a/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmmExtra.uni b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmmExtra.uni
new file mode 100644
index 0000000000..40f0c6c022
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/StatusCodeHandler/Smm/StatusCodeHandlerSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.c b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.c
new file mode 100644
index 0000000000..30349b5ec5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.c
@@ -0,0 +1,166 @@
+/** @file
+  Implementation of Timestamp Protocol using UEFI APIs.
+  
+Copyright (c) 2013, 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 <Uefi.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/TimerLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Protocol/Timestamp.h>
+
+//
+// The StartValue in TimerLib
+//
+UINT64 mTimerLibStartValue = 0;
+
+//
+// The EndValue in TimerLib
+//
+UINT64 mTimerLibEndValue = 0;
+
+//
+// The properties of timestamp
+//
+EFI_TIMESTAMP_PROPERTIES mTimestampProperties = {
+  0,
+  0
+};
+
+/**
+  Retrieves the current value of a 64-bit free running timestamp counter.
+
+  The counter shall count up in proportion to the amount of time that has passed. The counter value
+  will always roll over to zero. The properties of the counter can be retrieved from GetProperties().
+  The caller should be prepared for the function to return the same value twice across successive calls.
+  The counter value will not go backwards other than when wrapping, as defined by EndValue in GetProperties().
+  The frequency of the returned timestamp counter value must remain constant. Power management operations that 
+  affect clocking must not change the returned counter frequency. The quantization of counter value updates may 
+  vary as long as the value reflecting time passed remains consistent.           
+
+  @retval The current value of the free running timestamp counter.
+
+**/
+UINT64
+EFIAPI
+TimestampDriverGetTimestamp (
+  VOID
+  )
+{
+  //
+  // The timestamp of Timestamp Protocol
+  //
+  UINT64  TimestampValue;
+  TimestampValue = 0;
+  
+  //
+  // Get the timestamp
+  //
+  if (mTimerLibStartValue > mTimerLibEndValue) {
+    TimestampValue = mTimerLibStartValue - GetPerformanceCounter();
+  } else {
+    TimestampValue = GetPerformanceCounter() - mTimerLibStartValue;
+  }
+    
+  return TimestampValue;
+}
+
+/**
+  Obtains timestamp counter properties including frequency and value limits.
+
+  @param[out]  Properties              The properties of the timestamp counter.
+
+  @retval      EFI_SUCCESS             The properties were successfully retrieved. 
+  @retval      EFI_DEVICE_ERROR        An error occurred trying to retrieve the properties of the timestamp 
+                                       counter subsystem. Properties is not pedated.                                
+  @retval      EFI_INVALID_PARAMETER   Properties is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+TimestampDriverGetProperties(
+  OUT   EFI_TIMESTAMP_PROPERTIES       *Properties
+  )
+{
+  if (Properties == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  //
+  // Get timestamp properties
+  //
+  CopyMem((VOID *) Properties, (VOID *) &mTimestampProperties, sizeof (mTimestampProperties));
+  
+  return EFI_SUCCESS;
+}
+
+//
+// The Timestamp Protocol instance produced by this driver
+//
+EFI_TIMESTAMP_PROTOCOL  mTimestamp = {
+  TimestampDriverGetTimestamp,
+  TimestampDriverGetProperties
+};
+
+/**
+  Entry point of the Timestamp Protocol driver.
+
+  @param  ImageHandle   The image handle of this driver.
+  @param  SystemTable   The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS   Watchdog Timer Architectural Protocol successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+TimestampDriverInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  
+  EFI_HANDLE  TimestampHandle;
+  TimestampHandle = NULL;
+  
+  //
+  // Get the start value, end value and frequency in Timerlib
+  //
+  mTimestampProperties.Frequency = GetPerformanceCounterProperties(&mTimerLibStartValue, &mTimerLibEndValue);
+  
+  //
+  // Set the EndValue 
+  //
+  if (mTimerLibEndValue > mTimerLibStartValue) {
+    mTimestampProperties.EndValue = mTimerLibEndValue - mTimerLibStartValue;
+  } else {
+    mTimestampProperties.EndValue = mTimerLibStartValue - mTimerLibEndValue;
+  }
+  
+  DEBUG ((EFI_D_INFO, "TimerFrequency:0x%lx, TimerLibStartTime:0x%lx, TimerLibEndtime:0x%lx\n", mTimestampProperties.Frequency, mTimerLibStartValue, mTimerLibEndValue));
+  
+  //
+  // Install the Timestamp Protocol onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &TimestampHandle,
+                  &gEfiTimestampProtocolGuid,
+                  &mTimestamp,
+                  NULL
+                  );
+                  
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.inf b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.inf
new file mode 100644
index 0000000000..f71d5262ea
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.inf
@@ -0,0 +1,51 @@
+## @file
+# Generic Timestamp driver producing Timestamp Protocol using UEFI APIs.
+#
+# Copyright (c) 2013 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = TimestampDxe
+  MODULE_UNI_FILE                = TimestampDxe.uni
+  FILE_GUID                      = C10194E7-DEB2-4AF4-9EEE-BFFDE4D7D4C7
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = TimestampDriverInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Sources]
+  TimestampDxe.c 
+
+[LibraryClasses]
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  TimerLib
+  BaseMemoryLib
+  DebugLib
+
+[Protocols]
+  gEfiTimestampProtocolGuid             ## PRODUCES
+
+[depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  TimestampDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.uni b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.uni
new file mode 100644
index 0000000000..eecdd61520
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxe.uni
diff --git a/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxeExtra.uni b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxeExtra.uni
new file mode 100644
index 0000000000..08e2f02733
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/TimestampDxe/TimestampDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariable.c b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariable.c
new file mode 100644
index 0000000000..977332e1cf
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariable.c
@@ -0,0 +1,1785 @@
+/** @file
+
+  Emulation Variable services operate on the runtime volatile memory.
+  The nonvolatile variable space doesn't exist.
+
+Copyright (c) 2006 - 2015, 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 "Variable.h"
+
+///
+/// Don't use module globals after the SetVirtualAddress map is signaled
+///
+ESAL_VARIABLE_GLOBAL  *mVariableModuleGlobal;
+
+VARIABLE_INFO_ENTRY *gVariableInfo = NULL;
+
+///
+/// The size of a 3 character ISO639 language code.
+///
+#define ISO_639_2_ENTRY_SIZE    3
+
+/**
+  Update the variable region with Variable information. These are the same 
+  arguments as the EFI Variable services.
+
+  @param[in] VariableName       Name of variable
+
+  @param[in] VendorGuid         Guid of variable
+
+  @param[in] Data               Variable data
+
+  @param[in] DataSize           Size of data. 0 means delete
+
+  @param[in] Attributes 	      Attribues of the variable
+
+  @param[in] Variable           The variable information which is used to keep track of variable usage.
+
+  @retval EFI_SUCCESS           The update operation is success.
+
+  @retval EFI_OUT_OF_RESOURCES  Variable region is full, can not write other data into this region.
+
+**/
+EFI_STATUS
+EFIAPI
+UpdateVariable (
+  IN      CHAR16                 *VariableName,
+  IN      EFI_GUID               *VendorGuid,
+  IN      VOID                   *Data,
+  IN      UINTN                  DataSize,
+  IN      UINT32                 Attributes OPTIONAL,
+  IN      VARIABLE_POINTER_TRACK *Variable
+  );
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+  Otherwise, VariableName and VendorGuid are compared.
+
+  @param  VariableName                Name of the variable to be found.
+  @param  VendorGuid                  Vendor GUID to be found.
+  @param  PtrTrack                    VARIABLE_POINTER_TRACK structure for output,
+                                      including the range searched and the target position.
+  @param  Global                      Pointer to VARIABLE_GLOBAL structure, including
+                                      base of volatile variable storage area, base of
+                                      NV variable storage area, and a lock.
+
+  @retval EFI_INVALID_PARAMETER       If VariableName is not an empty string, while
+                                      VendorGuid is NULL.
+  @retval EFI_SUCCESS                 Variable successfully found.
+  @retval EFI_NOT_FOUND               Variable not found.
+
+**/
+EFI_STATUS
+FindVariable (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK  *PtrTrack,
+  IN  VARIABLE_GLOBAL         *Global
+  );
+
+/**
+  Acquires lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiAcquireLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiAcquireLock() at boot time, and simply returns
+  at runtime
+
+  @param  Lock         A pointer to the lock to acquire
+
+**/
+VOID
+AcquireLockOnlyAtBootTime (
+  IN EFI_LOCK  *Lock
+  )
+{
+  if (!EfiAtRuntime ()) {
+    EfiAcquireLock (Lock);
+  }
+}
+
+/**
+  Releases lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiReleaseLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiReleaseLock() at boot time, and simply returns
+  at runtime
+
+  @param  Lock         A pointer to the lock to release
+
+**/
+VOID
+ReleaseLockOnlyAtBootTime (
+  IN EFI_LOCK  *Lock
+  )
+{
+  if (!EfiAtRuntime ()) {
+    EfiReleaseLock (Lock);
+  }
+}
+
+/**
+  Gets pointer to the variable data.
+
+  This function gets the pointer to the variable data according
+  to the input pointer to the variable header.
+
+  @param  Variable      Pointer to the variable header.
+
+  @return Pointer to variable data
+
+**/
+UINT8 *
+GetVariableDataPtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  if (Variable->StartId != VARIABLE_DATA) {
+    return NULL;
+  }
+  //
+  // Be careful about pad size for alignment
+  //
+  return (UINT8 *) ((UINTN) GET_VARIABLE_NAME_PTR (Variable) + Variable->NameSize + GET_PAD_SIZE (Variable->NameSize));
+}
+
+/**
+  Gets pointer to header of the next potential variable.
+
+  This function gets the pointer to the next potential variable header
+  according to the input point to the variable header.  The return value
+  is not a valid variable if the input variable was the last variable
+  in the variabl store.
+
+  @param  Variable      Pointer to header of the next variable
+
+  @return Pointer to next variable header.
+  @retval NULL  Input was not a valid variable header.
+
+**/
+VARIABLE_HEADER *
+GetNextPotentialVariablePtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  VARIABLE_HEADER *VarHeader;
+
+  if (Variable->StartId != VARIABLE_DATA) {
+    return NULL;
+  }
+  //
+  // Be careful about pad size for alignment
+  //
+  VarHeader = (VARIABLE_HEADER *) HEADER_ALIGN ((UINTN) GetVariableDataPtr (Variable) + Variable->DataSize + GET_PAD_SIZE (Variable->DataSize));
+
+  return VarHeader;
+}
+
+/**
+  Gets pointer to header of the next variable.
+
+  This function gets the pointer to the next variable header according
+  to the input point to the variable header.
+
+  @param  Variable      Pointer to header of the next variable
+
+  @return Pointer to next variable header.
+
+**/
+VARIABLE_HEADER *
+GetNextVariablePtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  VARIABLE_HEADER *VarHeader;
+
+  VarHeader = GetNextPotentialVariablePtr (Variable);
+
+  if ((VarHeader == NULL) || (VarHeader->StartId != VARIABLE_DATA)) {
+    return NULL;
+  }
+
+  return VarHeader;
+}
+
+/**
+  Updates LastVariableOffset variable for the given variable store.
+
+  LastVariableOffset points to the offset to use for the next variable
+  when updating the variable store.
+
+  @param[in]   VariableStore       Pointer to the start of the variable store
+  @param[out]  LastVariableOffset  Offset to put the next new variable in
+
+**/
+VOID
+InitializeLocationForLastVariableOffset (
+  IN  VARIABLE_STORE_HEADER *VariableStore,
+  OUT UINTN                 *LastVariableOffset
+  )
+{
+  VARIABLE_HEADER *VarHeader;
+
+  *LastVariableOffset       = sizeof (VARIABLE_STORE_HEADER);
+  VarHeader = (VARIABLE_HEADER*) ((UINT8*)VariableStore + *LastVariableOffset);
+  while (VarHeader->StartId == VARIABLE_DATA) {
+    VarHeader = GetNextPotentialVariablePtr (VarHeader);
+
+    if (VarHeader != NULL) {
+      *LastVariableOffset = (UINTN) VarHeader - (UINTN) VariableStore;
+    } else {
+      return;
+    }
+  }
+}
+
+/**
+  Gets pointer to the end of the variable storage area.
+
+  This function gets pointer to the end of the variable storage
+  area, according to the input variable store header.
+
+  @param  VolHeader     Pointer to the variale store header
+
+  @return Pointer to the end of the variable storage area.
+
+**/
+VARIABLE_HEADER *
+GetEndPointer (
+  IN VARIABLE_STORE_HEADER       *VolHeader
+  )
+{
+  //
+  // The end of variable store
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN ((UINTN) VolHeader + VolHeader->Size);
+}
+
+/**
+  Routine used to track statistical information about variable usage. 
+  The data is stored in the EFI system table so it can be accessed later.
+  VariableInfo.efi can dump out the table. Only Boot Services variable 
+  accesses are tracked by this code. The PcdVariableCollectStatistics
+  build flag controls if this feature is enabled. 
+
+  A read that hits in the cache will have Read and Cache true for 
+  the transaction. Data is allocated by this routine, but never
+  freed.
+
+  @param[in] VariableName   Name of the Variable to track
+  @param[in] VendorGuid     Guid of the Variable to track
+  @param[in] Volatile       TRUE if volatile FALSE if non-volatile
+  @param[in] Read           TRUE if GetVariable() was called
+  @param[in] Write          TRUE if SetVariable() was called
+  @param[in] Delete         TRUE if deleted via SetVariable()
+  @param[in] Cache          TRUE for a cache hit.
+
+**/
+VOID
+UpdateVariableInfo (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  IN  BOOLEAN                 Volatile,
+  IN  BOOLEAN                 Read,
+  IN  BOOLEAN                 Write,
+  IN  BOOLEAN                 Delete,
+  IN  BOOLEAN                 Cache
+  )
+{
+  VARIABLE_INFO_ENTRY   *Entry;
+
+  if (FeaturePcdGet (PcdVariableCollectStatistics)) {
+
+    if (EfiAtRuntime ()) {
+      // Don't collect statistics at runtime
+      return;
+    }
+
+    if (gVariableInfo == NULL) {
+      //
+      // on the first call allocate a entry and place a pointer to it in
+      // the EFI System Table
+      //
+      gVariableInfo = AllocateZeroPool (sizeof (VARIABLE_INFO_ENTRY));
+      ASSERT (gVariableInfo != NULL);
+
+      CopyGuid (&gVariableInfo->VendorGuid, VendorGuid);
+      gVariableInfo->Name = AllocateZeroPool (StrSize (VariableName));
+      ASSERT (gVariableInfo->Name != NULL);
+      StrCpyS (gVariableInfo->Name, StrSize(VariableName)/sizeof(CHAR16), VariableName);
+      gVariableInfo->Volatile = Volatile;
+
+      gBS->InstallConfigurationTable (&gEfiVariableGuid, gVariableInfo);
+    }
+
+    
+    for (Entry = gVariableInfo; Entry != NULL; Entry = Entry->Next) {
+      if (CompareGuid (VendorGuid, &Entry->VendorGuid)) {
+        if (StrCmp (VariableName, Entry->Name) == 0) {
+          if (Read) {
+            Entry->ReadCount++;
+          }
+          if (Write) {
+            Entry->WriteCount++;
+          }
+          if (Delete) {
+            Entry->DeleteCount++;
+          }
+          if (Cache) {
+            Entry->CacheCount++;
+          }
+
+          return;
+        }
+      }
+
+      if (Entry->Next == NULL) {
+        //
+        // If the entry is not in the table add it.
+        // Next iteration of the loop will fill in the data
+        //
+        Entry->Next = AllocateZeroPool (sizeof (VARIABLE_INFO_ENTRY));
+        ASSERT (Entry->Next != NULL);
+
+        CopyGuid (&Entry->Next->VendorGuid, VendorGuid);
+        Entry->Next->Name = AllocateZeroPool (StrSize (VariableName));
+        ASSERT (Entry->Next->Name != NULL);
+        StrCpyS (Entry->Next->Name, StrSize(VariableName)/sizeof(CHAR16), VariableName);
+        Entry->Next->Volatile = Volatile;
+      }
+
+    }
+  }
+}
+
+/**
+  Get index from supported language codes according to language string.
+
+  This code is used to get corresponding index in supported language codes. It can handle
+  RFC4646 and ISO639 language tags.
+  In ISO639 language tags, take 3-characters as a delimitation to find matched string and calculate the index.
+  In RFC4646 language tags, take semicolon as a delimitation to find matched string and calculate the index.
+
+  For example:
+    SupportedLang  = "engfraengfra"
+    Lang           = "eng"
+    Iso639Language = TRUE
+  The return value is "0".
+  Another example:
+    SupportedLang  = "en;fr;en-US;fr-FR"
+    Lang           = "fr-FR"
+    Iso639Language = FALSE
+  The return value is "3".
+
+  @param  SupportedLang               Platform supported language codes.
+  @param  Lang                        Configured language.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646.
+
+  @retval the index of language in the language codes.
+
+**/
+UINTN
+GetIndexFromSupportedLangCodes(
+  IN  CHAR8            *SupportedLang,
+  IN  CHAR8            *Lang,
+  IN  BOOLEAN          Iso639Language
+  ) 
+{
+  UINTN    Index;
+  UINTN    CompareLength;
+  UINTN    LanguageLength;
+
+  if (Iso639Language) {
+    CompareLength = ISO_639_2_ENTRY_SIZE;
+    for (Index = 0; Index < AsciiStrLen (SupportedLang); Index += CompareLength) {
+      if (AsciiStrnCmp (Lang, SupportedLang + Index, CompareLength) == 0) {
+        //
+        // Successfully find the index of Lang string in SupportedLang string.
+        //
+        Index = Index / CompareLength;
+        return Index;
+      }
+    }
+    ASSERT (FALSE);
+    return 0;
+  } else {
+    //
+    // Compare RFC4646 language code
+    //
+    Index = 0;
+    for (LanguageLength = 0; Lang[LanguageLength] != '\0'; LanguageLength++);
+
+    for (Index = 0; *SupportedLang != '\0'; Index++, SupportedLang += CompareLength) {
+      //
+      // Skip ';' characters in SupportedLang
+      //
+      for (; *SupportedLang != '\0' && *SupportedLang == ';'; SupportedLang++);
+      //
+      // Determine the length of the next language code in SupportedLang
+      //
+      for (CompareLength = 0; SupportedLang[CompareLength] != '\0' && SupportedLang[CompareLength] != ';'; CompareLength++);
+      
+      if ((CompareLength == LanguageLength) && 
+          (AsciiStrnCmp (Lang, SupportedLang, CompareLength) == 0)) {
+        //
+        // Successfully find the index of Lang string in SupportedLang string.
+        //
+        return Index;
+      }
+    }
+    ASSERT (FALSE);
+    return 0;
+  }
+}
+
+/**
+  Get language string from supported language codes according to index.
+
+  This code is used to get corresponding language string in supported language codes. It can handle
+  RFC4646 and ISO639 language tags.
+  In ISO639 language tags, take 3-characters as a delimitation. Find language string according to the index.
+  In RFC4646 language tags, take semicolon as a delimitation. Find language string according to the index.
+
+  For example:
+    SupportedLang  = "engfraengfra"
+    Index          = "1"
+    Iso639Language = TRUE
+  The return value is "fra".
+  Another example:
+    SupportedLang  = "en;fr;en-US;fr-FR"
+    Index          = "1"
+    Iso639Language = FALSE
+  The return value is "fr".
+
+  @param  SupportedLang               Platform supported language codes.
+  @param  Index                       the index in supported language codes.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646.
+
+  @retval the language string in the language codes.
+
+**/
+CHAR8 *
+GetLangFromSupportedLangCodes (
+  IN  CHAR8            *SupportedLang,
+  IN  UINTN            Index,
+  IN  BOOLEAN          Iso639Language
+)
+{
+  UINTN    SubIndex;
+  UINTN    CompareLength;
+  CHAR8    *Supported;
+
+  SubIndex  = 0;
+  Supported = SupportedLang;
+  if (Iso639Language) {
+    //
+    // according to the index of Lang string in SupportedLang string to get the language.
+    // As this code will be invoked in RUNTIME, therefore there is not memory allocate/free operation.
+    // In driver entry, it pre-allocates a runtime attribute memory to accommodate this string.
+    //
+    CompareLength = ISO_639_2_ENTRY_SIZE;
+    mVariableModuleGlobal->Lang[CompareLength] = '\0';
+    return CopyMem (mVariableModuleGlobal->Lang, SupportedLang + Index * CompareLength, CompareLength);
+
+  } else {
+    while (TRUE) {
+      //
+      // take semicolon as delimitation, sequentially traverse supported language codes.
+      //
+      for (CompareLength = 0; *Supported != ';' && *Supported != '\0'; CompareLength++) {
+        Supported++;
+      }
+      if ((*Supported == '\0') && (SubIndex != Index)) {
+        //
+        // Have completed the traverse, but not find corrsponding string.
+        // This case is not allowed to happen.
+        //
+        ASSERT(FALSE);
+        return NULL;
+      }
+      if (SubIndex == Index) {
+        //
+        // according to the index of Lang string in SupportedLang string to get the language.
+        // As this code will be invoked in RUNTIME, therefore there is not memory allocate/free operation.
+        // In driver entry, it pre-allocates a runtime attribute memory to accommodate this string.
+        //
+        mVariableModuleGlobal->PlatformLang[CompareLength] = '\0';
+        return CopyMem (mVariableModuleGlobal->PlatformLang, Supported - CompareLength, CompareLength);
+      }
+      SubIndex++;
+      
+      //
+      // Skip ';' characters in Supported
+      //
+      for (; *Supported != '\0' && *Supported == ';'; Supported++);
+    }
+  }
+}
+
+/**
+  Returns a pointer to an allocated buffer that contains the best matching language 
+  from a set of supported languages.  
+  
+  This function supports both ISO 639-2 and RFC 4646 language codes, but language 
+  code types may not be mixed in a single call to this function. This function
+  supports a variable argument list that allows the caller to pass in a prioritized
+  list of language codes to test against all the language codes in SupportedLanguages.
+
+  If SupportedLanguages is NULL, then ASSERT().
+
+  @param[in]  SupportedLanguages  A pointer to a Null-terminated ASCII string that
+                                  contains a set of language codes in the format 
+                                  specified by Iso639Language.
+  @param[in]  Iso639Language      If TRUE, then all language codes are assumed to be
+                                  in ISO 639-2 format.  If FALSE, then all language
+                                  codes are assumed to be in RFC 4646 language format
+  @param[in]  ...                 A variable argument list that contains pointers to 
+                                  Null-terminated ASCII strings that contain one or more
+                                  language codes in the format specified by Iso639Language.
+                                  The first language code from each of these language
+                                  code lists is used to determine if it is an exact or
+                                  close match to any of the language codes in 
+                                  SupportedLanguages.  Close matches only apply to RFC 4646
+                                  language codes, and the matching algorithm from RFC 4647
+                                  is used to determine if a close match is present.  If 
+                                  an exact or close match is found, then the matching
+                                  language code from SupportedLanguages is returned.  If
+                                  no matches are found, then the next variable argument
+                                  parameter is evaluated.  The variable argument list 
+                                  is terminated by a NULL.
+
+  @retval NULL   The best matching language could not be found in SupportedLanguages.
+  @retval NULL   There are not enough resources available to return the best matching 
+                 language.
+  @retval Other  A pointer to a Null-terminated ASCII string that is the best matching 
+                 language in SupportedLanguages.
+
+**/
+CHAR8 *
+EFIAPI
+VariableGetBestLanguage (
+  IN CONST CHAR8  *SupportedLanguages, 
+  IN BOOLEAN      Iso639Language,
+  ...
+  )
+{
+  VA_LIST      Args;
+  CHAR8        *Language;
+  UINTN        CompareLength;
+  UINTN        LanguageLength;
+  CONST CHAR8  *Supported;
+  CHAR8        *Buffer;
+
+  ASSERT (SupportedLanguages != NULL);
+
+  VA_START (Args, Iso639Language);
+  while ((Language = VA_ARG (Args, CHAR8 *)) != NULL) {
+    //
+    // Default to ISO 639-2 mode
+    //
+    CompareLength  = 3;
+    LanguageLength = MIN (3, AsciiStrLen (Language));
+
+    //
+    // If in RFC 4646 mode, then determine the length of the first RFC 4646 language code in Language
+    //
+    if (!Iso639Language) {
+      for (LanguageLength = 0; Language[LanguageLength] != 0 && Language[LanguageLength] != ';'; LanguageLength++);
+    }
+
+    //
+    // Trim back the length of Language used until it is empty
+    //
+    while (LanguageLength > 0) {
+      //
+      // Loop through all language codes in SupportedLanguages
+      //
+      for (Supported = SupportedLanguages; *Supported != '\0'; Supported += CompareLength) {
+        //
+        // In RFC 4646 mode, then Loop through all language codes in SupportedLanguages
+        //
+        if (!Iso639Language) {
+          //
+          // Skip ';' characters in Supported
+          //
+          for (; *Supported != '\0' && *Supported == ';'; Supported++);
+          //
+          // Determine the length of the next language code in Supported
+          //
+          for (CompareLength = 0; Supported[CompareLength] != 0 && Supported[CompareLength] != ';'; CompareLength++);
+          //
+          // If Language is longer than the Supported, then skip to the next language
+          //
+          if (LanguageLength > CompareLength) {
+            continue;
+          }
+        }
+        //
+        // See if the first LanguageLength characters in Supported match Language
+        //
+        if (AsciiStrnCmp (Supported, Language, LanguageLength) == 0) {
+          VA_END (Args);
+
+          Buffer = Iso639Language ? mVariableModuleGlobal->Lang : mVariableModuleGlobal->PlatformLang;
+          Buffer[CompareLength] = '\0';
+          return CopyMem (Buffer, Supported, CompareLength);
+        }
+      }
+
+      if (Iso639Language) {
+        //
+        // If ISO 639 mode, then each language can only be tested once
+        //
+        LanguageLength = 0;
+      } else {
+        //
+        // If RFC 4646 mode, then trim Language from the right to the next '-' character 
+        //
+        for (LanguageLength--; LanguageLength > 0 && Language[LanguageLength] != '-'; LanguageLength--);
+      }
+    }
+  }
+  VA_END (Args);
+
+  //
+  // No matches were found 
+  //
+  return NULL;
+}
+
+/**
+  Hook the operations in PlatformLangCodes, LangCodes, PlatformLang and Lang.
+
+  When setting Lang/LangCodes, simultaneously update PlatformLang/PlatformLangCodes.
+
+  According to UEFI spec, PlatformLangCodes/LangCodes are only set once in firmware initialization,
+  and are read-only. Therefore, in variable driver, only store the original value for other use.
+
+  @param[in] VariableName       Name of variable
+
+  @param[in] Data               Variable data
+
+  @param[in] DataSize           Size of data. 0 means delete
+
+**/
+VOID
+AutoUpdateLangVariable(
+  IN  CHAR16             *VariableName,
+  IN  VOID               *Data,
+  IN  UINTN              DataSize
+  )
+{
+  EFI_STATUS             Status;
+  CHAR8                  *BestPlatformLang;
+  CHAR8                  *BestLang;
+  UINTN                  Index;
+  UINT32                 Attributes;
+  VARIABLE_POINTER_TRACK Variable;
+  BOOLEAN                SetLanguageCodes;
+
+  //
+  // Don't do updates for delete operation
+  //
+  if (DataSize == 0) {
+    return;
+  }
+
+  SetLanguageCodes = FALSE;
+
+  if (StrCmp (VariableName, L"PlatformLangCodes") == 0) {
+    //
+    // PlatformLangCodes is a volatile variable, so it can not be updated at runtime.
+    //
+    if (EfiAtRuntime ()) {
+      return;
+    }
+
+    SetLanguageCodes = TRUE;
+
+    //
+    // According to UEFI spec, PlatformLangCodes is only set once in firmware initialization, and is read-only
+    // Therefore, in variable driver, only store the original value for other use.
+    //
+    if (mVariableModuleGlobal->PlatformLangCodes != NULL) {
+      FreePool (mVariableModuleGlobal->PlatformLangCodes);
+    }
+    mVariableModuleGlobal->PlatformLangCodes = AllocateRuntimeCopyPool (DataSize, Data);
+    ASSERT (mVariableModuleGlobal->PlatformLangCodes != NULL);
+
+    //
+    // PlatformLang holds a single language from PlatformLangCodes, 
+    // so the size of PlatformLangCodes is enough for the PlatformLang.
+    //
+    if (mVariableModuleGlobal->PlatformLang != NULL) {
+      FreePool (mVariableModuleGlobal->PlatformLang);
+    }
+    mVariableModuleGlobal->PlatformLang = AllocateRuntimePool (DataSize);
+    ASSERT (mVariableModuleGlobal->PlatformLang != NULL);
+
+  } else if (StrCmp (VariableName, L"LangCodes") == 0) {
+    //
+    // LangCodes is a volatile variable, so it can not be updated at runtime.
+    //
+    if (EfiAtRuntime ()) {
+      return;
+    }
+
+    SetLanguageCodes = TRUE;
+
+    //
+    // According to UEFI spec, LangCodes is only set once in firmware initialization, and is read-only
+    // Therefore, in variable driver, only store the original value for other use.
+    //
+    if (mVariableModuleGlobal->LangCodes != NULL) {
+      FreePool (mVariableModuleGlobal->LangCodes);
+    }
+    mVariableModuleGlobal->LangCodes = AllocateRuntimeCopyPool (DataSize, Data);
+    ASSERT (mVariableModuleGlobal->LangCodes != NULL);
+  }
+
+  if (SetLanguageCodes 
+      && (mVariableModuleGlobal->PlatformLangCodes != NULL)
+      && (mVariableModuleGlobal->LangCodes != NULL)) {
+    //
+    // Update Lang if PlatformLang is already set
+    // Update PlatformLang if Lang is already set
+    //
+    Status = FindVariable (L"PlatformLang", &gEfiGlobalVariableGuid, &Variable, (VARIABLE_GLOBAL *) mVariableModuleGlobal);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update Lang
+      //
+      VariableName = L"PlatformLang";
+      Data         = GetVariableDataPtr (Variable.CurrPtr);
+      DataSize     = Variable.CurrPtr->DataSize;
+    } else {
+      Status = FindVariable (L"Lang", &gEfiGlobalVariableGuid, &Variable, (VARIABLE_GLOBAL *) mVariableModuleGlobal);
+      if (!EFI_ERROR (Status)) {
+        //
+        // Update PlatformLang
+        //
+        VariableName = L"Lang";
+        Data         = GetVariableDataPtr (Variable.CurrPtr);
+        DataSize     = Variable.CurrPtr->DataSize;
+      } else {
+        //
+        // Neither PlatformLang nor Lang is set, directly return
+        //
+        return;
+      }
+    }
+  }
+  
+  //
+  // According to UEFI spec, "Lang" and "PlatformLang" is NV|BS|RT attributions.
+  //
+  Attributes = EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS;
+
+  if (StrCmp (VariableName, L"PlatformLang") == 0) {
+    //
+    // Update Lang when PlatformLangCodes/LangCodes were set.
+    //
+    if ((mVariableModuleGlobal->PlatformLangCodes != NULL) && (mVariableModuleGlobal->LangCodes != NULL)) {
+      //
+      // When setting PlatformLang, firstly get most matched language string from supported language codes.
+      //
+      BestPlatformLang = VariableGetBestLanguage (mVariableModuleGlobal->PlatformLangCodes, FALSE, Data, NULL);
+      if (BestPlatformLang != NULL) {
+        //
+        // Get the corresponding index in language codes.
+        //
+        Index = GetIndexFromSupportedLangCodes (mVariableModuleGlobal->PlatformLangCodes, BestPlatformLang, FALSE);
+
+        //
+        // Get the corresponding ISO639 language tag according to RFC4646 language tag.
+        //
+        BestLang = GetLangFromSupportedLangCodes (mVariableModuleGlobal->LangCodes, Index, TRUE);
+
+        //
+        // Successfully convert PlatformLang to Lang, and set the BestLang value into Lang variable simultaneously.
+        //
+        FindVariable (L"Lang", &gEfiGlobalVariableGuid, &Variable, (VARIABLE_GLOBAL *)mVariableModuleGlobal);
+
+        Status = UpdateVariable (L"Lang", &gEfiGlobalVariableGuid, BestLang, ISO_639_2_ENTRY_SIZE + 1, Attributes, &Variable);
+
+        DEBUG ((EFI_D_INFO, "Variable Driver Auto Update PlatformLang, PlatformLang:%a, Lang:%a\n", BestPlatformLang, BestLang));
+
+        ASSERT_EFI_ERROR(Status);
+      }
+    }
+
+  } else if (StrCmp (VariableName, L"Lang") == 0) {
+    //
+    // Update PlatformLang when PlatformLangCodes/LangCodes were set.
+    //
+    if ((mVariableModuleGlobal->PlatformLangCodes != NULL) && (mVariableModuleGlobal->LangCodes != NULL)) {
+      //
+      // When setting Lang, firstly get most matched language string from supported language codes.
+      //
+      BestLang = VariableGetBestLanguage (mVariableModuleGlobal->LangCodes, TRUE, Data, NULL);
+      if (BestLang != NULL) {
+        //
+        // Get the corresponding index in language codes.
+        //
+        Index = GetIndexFromSupportedLangCodes (mVariableModuleGlobal->LangCodes, BestLang, TRUE);
+
+        //
+        // Get the corresponding RFC4646 language tag according to ISO639 language tag.
+        //
+        BestPlatformLang = GetLangFromSupportedLangCodes (mVariableModuleGlobal->PlatformLangCodes, Index, FALSE);
+
+        //
+        // Successfully convert Lang to PlatformLang, and set the BestPlatformLang value into PlatformLang variable simultaneously.
+        //
+        FindVariable (L"PlatformLang", &gEfiGlobalVariableGuid, &Variable, (VARIABLE_GLOBAL *)mVariableModuleGlobal);
+
+        Status = UpdateVariable (L"PlatformLang", &gEfiGlobalVariableGuid, BestPlatformLang, 
+                                 AsciiStrSize (BestPlatformLang), Attributes, &Variable);
+
+        DEBUG ((EFI_D_INFO, "Variable Driver Auto Update Lang, Lang:%a, PlatformLang:%a\n", BestLang, BestPlatformLang));
+        ASSERT_EFI_ERROR (Status);
+      }
+    }
+  }
+}
+
+/**
+  Update the variable region with Variable information. These are the same 
+  arguments as the EFI Variable services.
+
+  @param[in] VariableName       Name of variable
+
+  @param[in] VendorGuid         Guid of variable
+
+  @param[in] Data               Variable data
+
+  @param[in] DataSize           Size of data. 0 means delete
+
+  @param[in] Attributes         Attribues of the variable
+
+  @param[in] Variable           The variable information which is used to keep track of variable usage.
+
+  @retval EFI_SUCCESS           The update operation is success.
+
+  @retval EFI_OUT_OF_RESOURCES  Variable region is full, can not write other data into this region.
+
+**/
+EFI_STATUS
+EFIAPI
+UpdateVariable (
+  IN      CHAR16          *VariableName,
+  IN      EFI_GUID        *VendorGuid,
+  IN      VOID            *Data,
+  IN      UINTN           DataSize,
+  IN      UINT32          Attributes OPTIONAL,
+  IN      VARIABLE_POINTER_TRACK *Variable
+  )
+{
+  EFI_STATUS              Status;
+  VARIABLE_HEADER         *NextVariable;
+  UINTN                   VarNameSize;
+  UINTN                   VarNameOffset;
+  UINTN                   VarDataOffset;
+  UINTN                   VarSize;
+  VARIABLE_GLOBAL         *Global;
+  UINTN                   NonVolatileVarableStoreSize;
+
+  Global = &mVariableModuleGlobal->VariableGlobal[Physical];
+
+  if (Variable->CurrPtr != NULL) {
+    //
+    // Update/Delete existing variable
+    //
+
+    if (EfiAtRuntime ()) {        
+      //
+      // If EfiAtRuntime and the variable is Volatile and Runtime Access,  
+      // the volatile is ReadOnly, and SetVariable should be aborted and 
+      // return EFI_WRITE_PROTECTED.
+      //
+      if (Variable->Volatile) {
+        Status = EFI_WRITE_PROTECTED;
+        goto Done;
+      }
+      //
+      // Only variable have NV attribute can be updated/deleted in Runtime
+      //
+      if ((Variable->CurrPtr->Attributes & EFI_VARIABLE_NON_VOLATILE) == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+    }
+
+    //
+    // Setting a data variable with no access, or zero DataSize attributes
+    // specified causes it to be deleted.
+    //
+    if (DataSize == 0 || (Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == 0) {
+      Variable->CurrPtr->State &= VAR_DELETED;
+      UpdateVariableInfo (VariableName, VendorGuid, Variable->Volatile, FALSE, FALSE, TRUE, FALSE);
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    //
+    // If the variable is marked valid and the same data has been passed in
+    // then return to the caller immediately.
+    //
+    if (Variable->CurrPtr->DataSize == DataSize &&
+        CompareMem (Data, GetVariableDataPtr (Variable->CurrPtr), DataSize) == 0
+          ) {
+      Status = EFI_SUCCESS;
+      goto Done;
+    } else if (Variable->CurrPtr->State == VAR_ADDED) {
+      //
+      // Mark the old variable as in delete transition
+      //
+      Variable->CurrPtr->State &= VAR_IN_DELETED_TRANSITION;
+    }
+    
+  } else {
+    //
+    // No found existing variable, Create a new variable
+    //  
+    
+    //
+    // Make sure we are trying to create a new variable.
+    // Setting a data variable with no access, or zero DataSize attributes means to delete it.    
+    //
+    if (DataSize == 0 || (Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == 0) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+        
+    //
+    // Only variable have NV|RT attribute can be created in Runtime
+    //
+    if (EfiAtRuntime () &&
+        (((Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0) || ((Attributes & EFI_VARIABLE_NON_VOLATILE) == 0))) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }         
+  }
+  
+  //
+  // Function part - create a new variable and copy the data.
+  // Both update a variable and create a variable will come here.
+  //
+  
+  VarNameOffset = sizeof (VARIABLE_HEADER);
+  VarNameSize   = StrSize (VariableName);
+  VarDataOffset = VarNameOffset + VarNameSize + GET_PAD_SIZE (VarNameSize);
+  VarSize       = VarDataOffset + DataSize + GET_PAD_SIZE (DataSize);
+
+  if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+    NonVolatileVarableStoreSize = ((VARIABLE_STORE_HEADER *)(UINTN)(Global->NonVolatileVariableBase))->Size;
+    if ((((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0) 
+      && ((HEADER_ALIGN (VarSize) + mVariableModuleGlobal->HwErrVariableTotalSize) > PcdGet32 (PcdHwErrStorageSize)))
+      || (((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == 0) 
+      && ((HEADER_ALIGN (VarSize) + mVariableModuleGlobal->CommonVariableTotalSize) > NonVolatileVarableStoreSize - sizeof (VARIABLE_STORE_HEADER) - PcdGet32 (PcdHwErrStorageSize)))) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    NextVariable  = (VARIABLE_HEADER *) (UINT8 *) (mVariableModuleGlobal->NonVolatileLastVariableOffset
+                      + (UINTN) Global->NonVolatileVariableBase);
+    mVariableModuleGlobal->NonVolatileLastVariableOffset += HEADER_ALIGN (VarSize);
+
+    if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0) {
+      mVariableModuleGlobal->HwErrVariableTotalSize += HEADER_ALIGN (VarSize);
+    } else {
+      mVariableModuleGlobal->CommonVariableTotalSize += HEADER_ALIGN (VarSize);
+    }
+  } else {
+    if ((UINT32) (HEADER_ALIGN (VarSize) + mVariableModuleGlobal->VolatileLastVariableOffset) >
+          ((VARIABLE_STORE_HEADER *) ((UINTN) (Global->VolatileVariableBase)))->Size
+          ) {
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+
+    NextVariable    = (VARIABLE_HEADER *) (UINT8 *) (mVariableModuleGlobal->VolatileLastVariableOffset
+                        + (UINTN) Global->VolatileVariableBase);
+    mVariableModuleGlobal->VolatileLastVariableOffset += HEADER_ALIGN (VarSize);
+  }
+
+  NextVariable->StartId     = VARIABLE_DATA;
+  NextVariable->Attributes  = Attributes;
+  NextVariable->State       = VAR_ADDED;
+  NextVariable->Reserved    = 0;
+
+  //
+  // There will be pad bytes after Data, the NextVariable->NameSize and
+  // NextVariable->NameSize should not include pad size so that variable
+  // service can get actual size in GetVariable
+  //
+  NextVariable->NameSize  = (UINT32)VarNameSize;
+  NextVariable->DataSize  = (UINT32)DataSize;
+
+  CopyMem (&NextVariable->VendorGuid, VendorGuid, sizeof (EFI_GUID));
+  CopyMem (
+    (UINT8 *) ((UINTN) NextVariable + VarNameOffset),
+    VariableName,
+    VarNameSize
+    );
+  CopyMem (
+    (UINT8 *) ((UINTN) NextVariable + VarDataOffset),
+    Data,
+    DataSize
+    );
+
+  //
+  // Mark the old variable as deleted
+  //
+  if (Variable->CurrPtr != NULL) {
+    Variable->CurrPtr->State &= VAR_DELETED;
+  }
+
+  UpdateVariableInfo (VariableName, VendorGuid, Variable->Volatile, FALSE, TRUE, FALSE, FALSE);
+
+  Status = EFI_SUCCESS;
+
+Done:
+  return Status;
+}
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+  Otherwise, VariableName and VendorGuid are compared.
+
+  @param  VariableName                Name of the variable to be found.
+  @param  VendorGuid                  Vendor GUID to be found.
+  @param  PtrTrack                    VARIABLE_POINTER_TRACK structure for output,
+                                      including the range searched and the target position.
+  @param  Global                      Pointer to VARIABLE_GLOBAL structure, including
+                                      base of volatile variable storage area, base of
+                                      NV variable storage area, and a lock.
+
+  @retval EFI_INVALID_PARAMETER       If VariableName is not an empty string, while
+                                      VendorGuid is NULL.
+  @retval EFI_SUCCESS                 Variable successfully found.
+  @retval EFI_NOT_FOUND               Variable not found.
+
+**/
+EFI_STATUS
+FindVariable (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK  *PtrTrack,
+  IN  VARIABLE_GLOBAL         *Global
+  )
+{
+  VARIABLE_HEADER       *Variable[2];
+  VARIABLE_STORE_HEADER *VariableStoreHeader[2];
+  UINTN                 Index;
+
+  //
+  // 0: Non-Volatile, 1: Volatile
+  //
+  VariableStoreHeader[0]  = (VARIABLE_STORE_HEADER *) ((UINTN) Global->NonVolatileVariableBase);
+  VariableStoreHeader[1]  = (VARIABLE_STORE_HEADER *) ((UINTN) Global->VolatileVariableBase);
+
+  //
+  // Start Pointers for the variable.
+  // Actual Data Pointer where data can be written.
+  //
+  Variable[0] = (VARIABLE_HEADER *) HEADER_ALIGN (VariableStoreHeader[0] + 1);
+  Variable[1] = (VARIABLE_HEADER *) HEADER_ALIGN (VariableStoreHeader[1] + 1);
+
+  if (VariableName[0] != 0 && VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // Find the variable by walk through non-volatile and volatile variable store
+  //
+  for (Index = 0; Index < 2; Index++) {
+    PtrTrack->StartPtr  = (VARIABLE_HEADER *) HEADER_ALIGN (VariableStoreHeader[Index] + 1);
+    PtrTrack->EndPtr    = GetEndPointer (VariableStoreHeader[Index]);
+
+    while ((Variable[Index] < GetEndPointer (VariableStoreHeader[Index])) && (Variable[Index] != NULL)) {
+      if (Variable[Index]->StartId == VARIABLE_DATA && Variable[Index]->State == VAR_ADDED) {
+        if (!(EfiAtRuntime () && ((Variable[Index]->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0))) {
+          if (VariableName[0] == 0) {
+            PtrTrack->CurrPtr   = Variable[Index];
+            PtrTrack->Volatile  = (BOOLEAN) Index;
+            return EFI_SUCCESS;
+          } else {
+            if (CompareGuid (VendorGuid, &Variable[Index]->VendorGuid)) {
+              if (CompareMem (VariableName, GET_VARIABLE_NAME_PTR (Variable[Index]), Variable[Index]->NameSize) == 0) {
+                PtrTrack->CurrPtr   = Variable[Index];
+                PtrTrack->Volatile  = (BOOLEAN) Index;
+                return EFI_SUCCESS;
+              }
+            }
+          }
+        }
+      }
+
+      Variable[Index] = GetNextVariablePtr (Variable[Index]);
+    }
+  }
+  PtrTrack->CurrPtr = NULL;
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+  
+  @param  VariableName           A Null-terminated Unicode string that is the name of
+                                 the vendor's variable.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             If not NULL, a pointer to the memory location to return the 
+                                 attributes bitmask for the variable.
+  @param  DataSize               Size of Data found. If size is less than the
+                                 data, this value contains the required size.
+  @param  Data                   On input, the size in bytes of the return Data buffer.  
+                                 On output, the size of data returned in Data.
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure
+
+  @retval EFI_SUCCESS            The function completed successfully. 
+  @retval EFI_NOT_FOUND          The variable was not found.
+  @retval EFI_BUFFER_TOO_SMALL   DataSize is too small for the result.  DataSize has 
+                                 been updated with the size needed to complete the request.
+  @retval EFI_INVALID_PARAMETER  VariableName or VendorGuid or DataSize is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuGetVariable (
+  IN      CHAR16            *VariableName,
+  IN      EFI_GUID          *VendorGuid,
+  OUT     UINT32            *Attributes OPTIONAL,
+  IN OUT  UINTN             *DataSize,
+  OUT     VOID              *Data,
+  IN      VARIABLE_GLOBAL   *Global
+  )
+{
+  VARIABLE_POINTER_TRACK  Variable;
+  UINTN                   VarDataSize;
+  EFI_STATUS              Status;
+  UINT8                   *VariableDataPtr;
+
+  if (VariableName == NULL || VendorGuid == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&Global->VariableServicesLock);
+
+  //
+  // Find existing variable
+  //
+  Status = FindVariable (VariableName, VendorGuid, &Variable, Global);
+
+  if (Variable.CurrPtr == NULL || EFI_ERROR (Status)) {
+    goto Done;
+  }
+  //
+  // Get data size
+  //
+  VarDataSize = Variable.CurrPtr->DataSize;
+  if (*DataSize >= VarDataSize) {
+    if (Data == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+    VariableDataPtr = GetVariableDataPtr (Variable.CurrPtr);
+    ASSERT (VariableDataPtr != NULL);
+    
+    CopyMem (Data, VariableDataPtr, VarDataSize);
+    if (Attributes != NULL) {
+      *Attributes = Variable.CurrPtr->Attributes;
+    }
+
+    *DataSize = VarDataSize;
+    UpdateVariableInfo (VariableName, VendorGuid, Variable.Volatile, TRUE, FALSE, FALSE, FALSE);
+    Status = EFI_SUCCESS;
+    goto Done;
+  } else {
+    *DataSize = VarDataSize;
+    Status = EFI_BUFFER_TOO_SMALL;
+    goto Done;
+  }
+
+Done:
+  ReleaseLockOnlyAtBootTime (&Global->VariableServicesLock);
+  return Status;
+}
+
+/**
+
+  This code Finds the Next available variable.
+
+  @param  VariableNameSize       Size of the variable.
+  @param  VariableName           On input, supplies the last VariableName that was returned by GetNextVariableName().
+                                 On output, returns the Null-terminated Unicode string of the current variable.
+  @param  VendorGuid             On input, supplies the last VendorGuid that was returned by GetNextVariableName().
+                                 On output, returns the VendorGuid of the current variable.  
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure.
+
+  @retval EFI_SUCCESS            The function completed successfully. 
+  @retval EFI_NOT_FOUND          The next variable was not found.
+  @retval EFI_BUFFER_TOO_SMALL   VariableNameSize is too small for the result. 
+                                 VariableNameSize has been updated with the size needed to complete the request.
+  @retval EFI_INVALID_PARAMETER  VariableNameSize or VariableName or VendorGuid is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuGetNextVariableName (
+  IN OUT  UINTN             *VariableNameSize,
+  IN OUT  CHAR16            *VariableName,
+  IN OUT  EFI_GUID          *VendorGuid,
+  IN      VARIABLE_GLOBAL   *Global
+  )
+{
+  VARIABLE_POINTER_TRACK  Variable;
+  UINTN                   VarNameSize;
+  EFI_STATUS              Status;
+
+  if (VariableNameSize == NULL || VariableName == NULL || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&Global->VariableServicesLock);
+
+  Status = FindVariable (VariableName, VendorGuid, &Variable, Global);
+
+  if (Variable.CurrPtr == NULL || EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  while (TRUE) {
+    if (VariableName[0] != 0) {
+      //
+      // If variable name is not NULL, get next variable
+      //
+      Variable.CurrPtr = GetNextVariablePtr (Variable.CurrPtr);
+    }
+    //
+    // If both volatile and non-volatile variable store are parsed,
+    // return not found
+    //
+    if (Variable.CurrPtr >= Variable.EndPtr || Variable.CurrPtr == NULL) {
+      Variable.Volatile = (BOOLEAN) (Variable.Volatile ^ ((BOOLEAN) 0x1));
+      if (Variable.Volatile) {
+        Variable.StartPtr = (VARIABLE_HEADER *) HEADER_ALIGN ((UINTN) (Global->VolatileVariableBase + sizeof (VARIABLE_STORE_HEADER)));
+        Variable.EndPtr = (VARIABLE_HEADER *) GetEndPointer ((VARIABLE_STORE_HEADER *) ((UINTN) Global->VolatileVariableBase));
+      } else {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+
+      Variable.CurrPtr = Variable.StartPtr;
+      if (Variable.CurrPtr->StartId != VARIABLE_DATA) {
+        continue;
+      }
+    }
+    //
+    // Variable is found
+    //
+    if (Variable.CurrPtr->StartId == VARIABLE_DATA && Variable.CurrPtr->State == VAR_ADDED) {
+      if (!(EfiAtRuntime () && ((Variable.CurrPtr->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0))) {
+        VarNameSize = Variable.CurrPtr->NameSize;
+        if (VarNameSize <= *VariableNameSize) {
+          CopyMem (
+            VariableName,
+            GET_VARIABLE_NAME_PTR (Variable.CurrPtr),
+            VarNameSize
+            );
+          CopyMem (
+            VendorGuid,
+            &Variable.CurrPtr->VendorGuid,
+            sizeof (EFI_GUID)
+            );
+          Status = EFI_SUCCESS;
+        } else {
+          Status = EFI_BUFFER_TOO_SMALL;
+        }
+
+        *VariableNameSize = VarNameSize;
+        goto Done;
+      }
+    }
+  }
+
+Done:
+  ReleaseLockOnlyAtBootTime (&Global->VariableServicesLock);
+  return Status;
+
+}
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  @param  VariableName           A Null-terminated Unicode string that is the name of the vendor's
+                                 variable.  Each VariableName is unique for each 
+                                 VendorGuid.  VariableName must contain 1 or more 
+                                 Unicode characters.  If VariableName is an empty Unicode 
+                                 string, then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor
+  @param  Attributes             Attributes bitmask to set for the variable
+  @param  DataSize               The size in bytes of the Data buffer.  A size of zero causes the
+                                 variable to be deleted.
+  @param  Data                   The contents for the variable
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure
+  @param  VolatileOffset         The offset of last volatile variable
+  @param  NonVolatileOffset      The offset of last non-volatile variable
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as 
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits was supplied, or the 
+                                 DataSize exceeds the maximum allowed, or VariableName is an empty 
+                                 Unicode string, or VendorGuid is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved due to a hardware failure.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only or cannot be deleted.
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuSetVariable (
+  IN CHAR16                  *VariableName,
+  IN EFI_GUID                *VendorGuid,
+  IN UINT32                  Attributes,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data,
+  IN VARIABLE_GLOBAL         *Global,
+  IN UINTN                   *VolatileOffset,
+  IN UINTN                   *NonVolatileOffset
+  )
+{
+  VARIABLE_POINTER_TRACK  Variable;
+  EFI_STATUS              Status;
+
+  //
+  // Check input parameters
+  //
+  if (VariableName == NULL || VariableName[0] == 0 || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }  
+
+  if (DataSize != 0 && Data == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Not support authenticated variable write yet.
+  //
+  if ((Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Make sure if runtime bit is set, boot service bit is set also
+  //
+  if ((Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == EFI_VARIABLE_RUNTIME_ACCESS) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  
+  if ((UINTN)(~0) - DataSize < StrSize(VariableName)){
+    //
+    // Prevent whole variable size overflow 
+    // 
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  The size of the VariableName, including the Unicode Null in bytes plus
+  //  the DataSize is limited to maximum size of PcdGet32 (PcdMaxHardwareErrorVariableSize)
+  //  bytes for HwErrRec, and PcdGet32 (PcdMaxVariableSize) bytes for the others.
+  //
+  if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+    if (StrSize (VariableName) + DataSize > PcdGet32 (PcdMaxHardwareErrorVariableSize) - sizeof (VARIABLE_HEADER)) {
+      return EFI_INVALID_PARAMETER;
+    }
+    //
+    // According to UEFI spec, HARDWARE_ERROR_RECORD variable name convention should be L"HwErrRecXXXX"
+    //
+    if (StrnCmp(VariableName, L"HwErrRec", StrLen(L"HwErrRec")) != 0) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+  //
+  //  The size of the VariableName, including the Unicode Null in bytes plus
+  //  the DataSize is limited to maximum size of PcdGet32 (PcdMaxVariableSize) bytes.
+  //
+    if (StrSize (VariableName) + DataSize > PcdGet32 (PcdMaxVariableSize) - sizeof (VARIABLE_HEADER)) {
+      return EFI_INVALID_PARAMETER;
+    }  
+  }
+
+  AcquireLockOnlyAtBootTime(&Global->VariableServicesLock);
+
+  //
+  // Check whether the input variable is already existed
+  //
+  
+  Status = FindVariable (VariableName, VendorGuid, &Variable, Global);
+
+  //
+  // Hook the operation of setting PlatformLangCodes/PlatformLang and LangCodes/Lang
+  //
+  AutoUpdateLangVariable (VariableName, Data, DataSize);
+
+  Status = UpdateVariable (VariableName, VendorGuid, Data, DataSize, Attributes, &Variable);
+
+  ReleaseLockOnlyAtBootTime (&Global->VariableServicesLock);
+  return Status;
+}
+
+/**
+
+  This code returns information about the EFI variables.
+
+  @param  Attributes                   Attributes bitmask to specify the type of variables
+                                       on which to return information.
+  @param  MaximumVariableStorageSize   On output the maximum size of the storage space available for 
+                                       the EFI variables associated with the attributes specified.  
+  @param  RemainingVariableStorageSize Returns the remaining size of the storage space available for EFI 
+                                       variables associated with the attributes specified.
+  @param  MaximumVariableSize          Returns the maximum size of an individual EFI variable 
+                                       associated with the attributes specified.
+  @param  Global                       Pointer to VARIABLE_GLOBAL structure.
+
+  @retval EFI_SUCCESS                  Valid answer returned.
+  @retval EFI_INVALID_PARAMETER        An invalid combination of attribute bits was supplied
+  @retval EFI_UNSUPPORTED              The attribute is not supported on this platform, and the 
+                                       MaximumVariableStorageSize, RemainingVariableStorageSize, 
+                                       MaximumVariableSize are undefined.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuQueryVariableInfo (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize,
+  IN  VARIABLE_GLOBAL        *Global
+  )
+{
+  VARIABLE_HEADER        *Variable;
+  VARIABLE_HEADER        *NextVariable;
+  UINT64                 VariableSize;
+  VARIABLE_STORE_HEADER  *VariableStoreHeader;
+  UINT64                 CommonVariableTotalSize;
+  UINT64                 HwErrVariableTotalSize;
+
+  CommonVariableTotalSize = 0;
+  HwErrVariableTotalSize = 0;
+
+  if(MaximumVariableStorageSize == NULL || RemainingVariableStorageSize == NULL || MaximumVariableSize == NULL || Attributes == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+  
+  if((Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == 0) {
+    //
+    // Make sure the Attributes combination is supported by the platform.
+    //
+    return EFI_UNSUPPORTED;  
+  } else if ((Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == EFI_VARIABLE_RUNTIME_ACCESS) {
+    //
+    // Make sure if runtime bit is set, boot service bit is set also.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if (EfiAtRuntime () && ((Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0)) {
+    //
+    //   Make sure RT Attribute is set if we are in Runtime phase.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if ((Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+    //
+    // Make sure Hw Attribute is set with NV.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if ((Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) != 0) {
+    //
+    // Not support authentiated variable write yet.
+    //
+    return EFI_UNSUPPORTED;
+  }
+
+  AcquireLockOnlyAtBootTime(&Global->VariableServicesLock);
+
+  if((Attributes & EFI_VARIABLE_NON_VOLATILE) == 0) {
+    //
+    // Query is Volatile related.
+    //
+    VariableStoreHeader = (VARIABLE_STORE_HEADER *) ((UINTN) Global->VolatileVariableBase);
+  } else {
+    //
+    // Query is Non-Volatile related.
+    //
+    VariableStoreHeader = (VARIABLE_STORE_HEADER *) ((UINTN) Global->NonVolatileVariableBase);
+  }
+
+  //
+  // Now let's fill *MaximumVariableStorageSize *RemainingVariableStorageSize
+  // with the storage size (excluding the storage header size)
+  //
+  *MaximumVariableStorageSize   = VariableStoreHeader->Size - sizeof (VARIABLE_STORE_HEADER);
+
+  //
+  // Harware error record variable needs larger size.
+  //
+  if ((Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+    *MaximumVariableStorageSize = PcdGet32 (PcdHwErrStorageSize);
+    *MaximumVariableSize = PcdGet32 (PcdMaxHardwareErrorVariableSize) - sizeof (VARIABLE_HEADER);
+  } else {
+    if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+      ASSERT (PcdGet32 (PcdHwErrStorageSize) < VariableStoreHeader->Size);
+      *MaximumVariableStorageSize = VariableStoreHeader->Size - sizeof (VARIABLE_STORE_HEADER) - PcdGet32 (PcdHwErrStorageSize);
+    }
+
+    //
+    // Let *MaximumVariableSize be PcdGet32 (PcdMaxVariableSize) with the exception of the variable header size.
+    //
+    *MaximumVariableSize = PcdGet32 (PcdMaxVariableSize) - sizeof (VARIABLE_HEADER);
+  }
+
+  //
+  // Point to the starting address of the variables.
+  //
+  Variable = (VARIABLE_HEADER *) HEADER_ALIGN (VariableStoreHeader + 1);
+
+  //
+  // Now walk through the related variable store.
+  //
+  while (Variable < GetEndPointer (VariableStoreHeader)) {
+    NextVariable = GetNextVariablePtr(Variable);
+    if (NextVariable == NULL) {
+      break;
+    }
+    VariableSize = (UINT64) (UINTN) NextVariable - (UINT64) (UINTN) Variable;
+
+    if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+      HwErrVariableTotalSize += VariableSize;
+    } else {
+      CommonVariableTotalSize += VariableSize;
+    }
+
+    //
+    // Go to the next one.
+    //
+    Variable = NextVariable;
+  }
+
+  if ((Attributes  & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD){
+    *RemainingVariableStorageSize = *MaximumVariableStorageSize - HwErrVariableTotalSize;
+  } else {
+    *RemainingVariableStorageSize = *MaximumVariableStorageSize - CommonVariableTotalSize;
+  }
+
+  if (*RemainingVariableStorageSize < sizeof (VARIABLE_HEADER)) {
+    *MaximumVariableSize = 0;
+  } else if ((*RemainingVariableStorageSize - sizeof (VARIABLE_HEADER)) < *MaximumVariableSize) {
+    *MaximumVariableSize = *RemainingVariableStorageSize - sizeof (VARIABLE_HEADER);
+  }
+  
+  ReleaseLockOnlyAtBootTime (&Global->VariableServicesLock);
+  return EFI_SUCCESS;
+}
+
+/**
+  Initializes variable store area.
+
+  This function allocates memory space for variable store area and initializes its attributes.
+
+  @param  VolatileStore  Indicates if the variable store is volatile.
+
+**/
+EFI_STATUS
+InitializeVariableStore (
+  IN  BOOLEAN               VolatileStore
+  )
+{
+  EFI_STATUS            Status;
+  VARIABLE_STORE_HEADER *VariableStore;
+  BOOLEAN               FullyInitializeStore;
+  EFI_PHYSICAL_ADDRESS  *VariableBase;
+  UINTN                 *LastVariableOffset;
+  VARIABLE_STORE_HEADER *VariableStoreHeader;
+  VARIABLE_HEADER       *Variable;
+  VOID                  *VariableData;
+  EFI_HOB_GUID_TYPE     *GuidHob;
+
+  FullyInitializeStore = TRUE;
+
+  if (VolatileStore) {
+    VariableBase = &mVariableModuleGlobal->VariableGlobal[Physical].VolatileVariableBase;
+    LastVariableOffset = &mVariableModuleGlobal->VolatileLastVariableOffset;
+  } else {
+    VariableBase = &mVariableModuleGlobal->VariableGlobal[Physical].NonVolatileVariableBase;
+    LastVariableOffset = &mVariableModuleGlobal->NonVolatileLastVariableOffset;
+  }
+
+  //
+  // Note that in EdkII variable driver implementation, Hardware Error Record type variable
+  // is stored with common variable in the same NV region. So the platform integrator should
+  // ensure that the value of PcdHwErrStorageSize is less than or equal to the value of 
+  // PcdVariableStoreSize.
+  //
+  ASSERT (PcdGet32 (PcdHwErrStorageSize) <= PcdGet32 (PcdVariableStoreSize));
+
+  //
+  // Allocate memory for variable store.
+  //
+  if (VolatileStore || (PcdGet64 (PcdEmuVariableNvStoreReserved) == 0)) {
+    VariableStore = (VARIABLE_STORE_HEADER *) AllocateRuntimePool (PcdGet32 (PcdVariableStoreSize));
+  } else {
+    //
+    // A memory location has been reserved for the NV variable store.  Certain
+    // platforms may be able to preserve a memory range across system resets,
+    // thereby providing better NV variable emulation.
+    //
+    VariableStore =
+      (VARIABLE_STORE_HEADER *)(VOID*)(UINTN)
+        PcdGet64 (PcdEmuVariableNvStoreReserved);
+    if (
+         (VariableStore->Size == PcdGet32 (PcdVariableStoreSize)) &&
+         (VariableStore->Format == VARIABLE_STORE_FORMATTED) &&
+         (VariableStore->State == VARIABLE_STORE_HEALTHY)
+       ) {
+      DEBUG((
+        EFI_D_INFO,
+        "Variable Store reserved at %p appears to be valid\n",
+        VariableStore
+        ));
+      FullyInitializeStore = FALSE;
+    }
+  }
+
+  if (NULL == VariableStore) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  if (FullyInitializeStore) {
+    SetMem (VariableStore, PcdGet32 (PcdVariableStoreSize), 0xff);
+  }
+
+  //
+  // Variable Specific Data
+  //
+  *VariableBase             = (EFI_PHYSICAL_ADDRESS) (UINTN) VariableStore;
+  InitializeLocationForLastVariableOffset (VariableStore, LastVariableOffset);
+
+  CopyGuid (&VariableStore->Signature, &gEfiVariableGuid);
+  VariableStore->Size       = PcdGet32 (PcdVariableStoreSize);
+  VariableStore->Format     = VARIABLE_STORE_FORMATTED;
+  VariableStore->State      = VARIABLE_STORE_HEALTHY;
+  VariableStore->Reserved   = 0;
+  VariableStore->Reserved1  = 0;
+
+  if (!VolatileStore) {
+    //
+    // Get HOB variable store.
+    //
+    GuidHob = GetFirstGuidHob (&gEfiVariableGuid);
+    if (GuidHob != NULL) {
+      VariableStoreHeader = (VARIABLE_STORE_HEADER *) GET_GUID_HOB_DATA (GuidHob);
+      if (CompareGuid (&VariableStoreHeader->Signature, &gEfiVariableGuid) &&
+          (VariableStoreHeader->Format == VARIABLE_STORE_FORMATTED) &&
+          (VariableStoreHeader->State == VARIABLE_STORE_HEALTHY)
+         ) {
+        DEBUG ((EFI_D_INFO, "HOB Variable Store appears to be valid.\n"));
+        //
+        // Flush the HOB variable to Emulation Variable storage.
+        //
+        for ( Variable = (VARIABLE_HEADER *) HEADER_ALIGN (VariableStoreHeader + 1)
+            ; (Variable < GetEndPointer (VariableStoreHeader) && (Variable != NULL))
+            ; Variable = GetNextVariablePtr (Variable)
+            ) {
+          ASSERT (Variable->State == VAR_ADDED);
+          ASSERT ((Variable->Attributes & EFI_VARIABLE_NON_VOLATILE) != 0);
+          VariableData = GetVariableDataPtr (Variable);
+          Status = EmuSetVariable (
+                     GET_VARIABLE_NAME_PTR (Variable),
+                     &Variable->VendorGuid,
+                     Variable->Attributes,
+                     Variable->DataSize,
+                     VariableData,
+                     &mVariableModuleGlobal->VariableGlobal[Physical],
+                     &mVariableModuleGlobal->VolatileLastVariableOffset,
+                     &mVariableModuleGlobal->NonVolatileLastVariableOffset
+                     );
+          ASSERT_EFI_ERROR (Status);
+        }
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Initializes variable store area for non-volatile and volatile variable.
+
+  This function allocates and initializes memory space for global context of ESAL
+  variable service and variable store area for non-volatile and volatile variable.
+
+  @param  ImageHandle           The Image handle of this driver.
+  @param  SystemTable           The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS           Function successfully executed.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate enough memory resource.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableCommonInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Allocate memory for mVariableModuleGlobal
+  //
+  mVariableModuleGlobal = (ESAL_VARIABLE_GLOBAL *) AllocateRuntimeZeroPool (
+                                                    sizeof (ESAL_VARIABLE_GLOBAL)
+                                                   );
+  if (NULL == mVariableModuleGlobal) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  EfiInitializeLock(&mVariableModuleGlobal->VariableGlobal[Physical].VariableServicesLock, TPL_NOTIFY);
+
+  //
+  // Intialize volatile variable store
+  //
+  Status = InitializeVariableStore (TRUE);
+  if (EFI_ERROR (Status)) {
+    FreePool(mVariableModuleGlobal);
+    return Status;
+  }
+  //
+  // Intialize non volatile variable store
+  //
+  Status = InitializeVariableStore (FALSE);
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.inf b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.inf
new file mode 100644
index 0000000000..0dafddee55
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.inf
@@ -0,0 +1,88 @@
+## @file
+# Emulation Variable for EFI_RUNTIME_SERVICES.
+#
+# This module installs variable arch protocol and variable write arch protocol to provide
+# four EFI_RUNTIME_SERVICES: SetVariable, GetVariable, GetNextVariableName and QueryVariableInfo.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = EmuVariableRuntimeDxe
+  MODULE_UNI_FILE                = EmuVariableRuntimeDxe.uni
+  FILE_GUID                      = 02B01AD5-7E59-43e8-A6D8-238180613A5A
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = VariableServiceInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 EBC
+#
+
+[Sources]
+  InitVariable.c
+  EmuVariable.c
+  Variable.h
+
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+
+[LibraryClasses]
+  BaseLib
+  UefiLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiRuntimeLib
+  DebugLib
+  MemoryAllocationLib
+  BaseMemoryLib
+  HobLib
+  PcdLib
+
+[Protocols]
+  gEfiVariableArchProtocolGuid                  ## PRODUCES
+  gEfiVariableWriteArchProtocolGuid             ## PRODUCES
+
+[Guids]
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES             ## Event
+  ## SOMETIMES_CONSUMES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_CONSUMES   ## Variable:L"Lang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"Lang"
+  gEfiGlobalVariableGuid
+  ## PRODUCES             ## GUID # Signature of Variable store header
+  ## CONSUMES             ## GUID # Signature of Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiVariableGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdEmuVariableNvStoreReserved      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxVariableSize                 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxHardwareErrorVariableSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableStoreSize               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHwErrStorageSize                ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableCollectStatistics   ## CONSUMES # statistic the information of variable.
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  EmuVariableRuntimeDxeExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.uni b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.uni
new file mode 100644
index 0000000000..a0041c353a
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..766bb0a14b
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/EmuVariableRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/InitVariable.c b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/InitVariable.c
new file mode 100644
index 0000000000..d636fcf20d
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/InitVariable.c
@@ -0,0 +1,250 @@
+/** @file
+
+  Implment all four UEFI runtime variable services and 
+  install variable architeture protocol.
+  
+Copyright (c) 2006 - 2010, 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 "Variable.h"
+
+EFI_EVENT   mVirtualAddressChangeEvent = NULL;
+
+/**
+
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+
+  @param VariableName               Name of Variable to be found.
+  @param VendorGuid                 Variable vendor GUID.
+  @param Attributes                 Attribute value of the variable found.
+  @param DataSize                   Size of Data found. If size is less than the
+                                    data, this value contains the required size.
+  @param Data                       Data pointer.
+                      
+  @return EFI_INVALID_PARAMETER     Invalid parameter
+  @return EFI_SUCCESS               Find the specified variable
+  @return EFI_NOT_FOUND             Not found
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceGetVariable (
+  IN CHAR16        *VariableName,
+  IN EFI_GUID      *VendorGuid,
+  OUT UINT32       *Attributes OPTIONAL,
+  IN OUT UINTN     *DataSize,
+  OUT VOID         *Data
+  )
+{
+  return EmuGetVariable (
+          VariableName,
+          VendorGuid,
+          Attributes OPTIONAL,
+          DataSize,
+          Data,
+          &mVariableModuleGlobal->VariableGlobal[Physical]
+          );
+}
+
+/**
+
+  This code Finds the Next available variable.
+
+  @param VariableNameSize           Size of the variable name
+  @param VariableName               Pointer to variable name
+  @param VendorGuid                 Variable Vendor Guid
+
+  @return EFI_INVALID_PARAMETER     Invalid parameter
+  @return EFI_SUCCESS               Find the specified variable
+  @return EFI_NOT_FOUND             Not found
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceGetNextVariableName (
+  IN OUT UINTN     *VariableNameSize,
+  IN OUT CHAR16    *VariableName,
+  IN OUT EFI_GUID  *VendorGuid
+  )
+{
+  return EmuGetNextVariableName (
+          VariableNameSize,
+          VariableName,
+          VendorGuid,
+          &mVariableModuleGlobal->VariableGlobal[Physical]
+          );
+}
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  @param VariableName                     Name of Variable to be found
+  @param VendorGuid                       Variable vendor GUID
+  @param Attributes                       Attribute value of the variable found
+  @param DataSize                         Size of Data found. If size is less than the
+                                          data, this value contains the required size.
+  @param Data                             Data pointer
+
+  @return EFI_INVALID_PARAMETER           Invalid parameter
+  @return EFI_SUCCESS                     Set successfully
+  @return EFI_OUT_OF_RESOURCES            Resource not enough to set variable
+  @return EFI_NOT_FOUND                   Not found
+  @return EFI_WRITE_PROTECTED             Variable is read-only
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceSetVariable (
+  IN CHAR16        *VariableName,
+  IN EFI_GUID      *VendorGuid,
+  IN UINT32        Attributes,
+  IN UINTN         DataSize,
+  IN VOID          *Data
+  )
+{
+  return EmuSetVariable (
+          VariableName,
+          VendorGuid,
+          Attributes,
+          DataSize,
+          Data,
+          &mVariableModuleGlobal->VariableGlobal[Physical],
+          &mVariableModuleGlobal->VolatileLastVariableOffset,
+          &mVariableModuleGlobal->NonVolatileLastVariableOffset
+          );
+}
+
+/**
+
+  This code returns information about the EFI variables.
+
+  @param Attributes                     Attributes bitmask to specify the type of variables
+                                        on which to return information.
+  @param MaximumVariableStorageSize     Pointer to the maximum size of the storage space available
+                                        for the EFI variables associated with the attributes specified.
+  @param RemainingVariableStorageSize   Pointer to the remaining size of the storage space available
+                                        for EFI variables associated with the attributes specified.
+  @param MaximumVariableSize            Pointer to the maximum size of an individual EFI variables
+                                        associated with the attributes specified.
+
+  @return EFI_INVALID_PARAMETER         An invalid combination of attribute bits was supplied.
+  @return EFI_SUCCESS                   Query successfully.
+  @return EFI_UNSUPPORTED               The attribute is not supported on this platform.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceQueryVariableInfo (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize
+  )
+{
+  return EmuQueryVariableInfo (
+          Attributes,
+          MaximumVariableStorageSize,
+          RemainingVariableStorageSize,
+          MaximumVariableSize,
+          &mVariableModuleGlobal->VariableGlobal[Physical]
+          );
+}
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableClassAddressChangeEvent (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->PlatformLangCodes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->LangCodes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->PlatformLang);
+  EfiConvertPointer (
+    0x0,
+    (VOID **) &mVariableModuleGlobal->VariableGlobal[Physical].NonVolatileVariableBase
+    );
+  EfiConvertPointer (
+    0x0,
+    (VOID **) &mVariableModuleGlobal->VariableGlobal[Physical].VolatileVariableBase
+    );
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal);
+}
+
+/**
+  EmuVariable Driver main entry point. The Variable driver places the 4 EFI
+  runtime services in the EFI System Table and installs arch protocols 
+  for variable read and write services being available. It also registers
+  notification function for EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.  
+  @param[in] SystemTable    A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS       Variable service successfully initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  )
+{
+  EFI_HANDLE  NewHandle;
+  EFI_STATUS  Status;
+
+  Status = VariableCommonInitialize (ImageHandle, SystemTable);
+  ASSERT_EFI_ERROR (Status);
+
+  SystemTable->RuntimeServices->GetVariable         = RuntimeServiceGetVariable;
+  SystemTable->RuntimeServices->GetNextVariableName = RuntimeServiceGetNextVariableName;
+  SystemTable->RuntimeServices->SetVariable         = RuntimeServiceSetVariable;
+  SystemTable->RuntimeServices->QueryVariableInfo   = RuntimeServiceQueryVariableInfo;
+
+  //
+  // Now install the Variable Runtime Architectural Protocol on a new handle
+  //
+  NewHandle = NULL;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &NewHandle,
+                  &gEfiVariableArchProtocolGuid,
+                  NULL,
+                  &gEfiVariableWriteArchProtocolGuid,
+                  NULL,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  VariableClassAddressChangeEvent,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mVirtualAddressChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/Variable.h b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/Variable.h
new file mode 100644
index 0000000000..81a45681a2
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/EmuRuntimeDxe/Variable.h
@@ -0,0 +1,272 @@
+/** @file
+
+  The internal header file includes the common header files, defines
+  internal structure and functions used by EmuVariable module.
+
+Copyright (c) 2006 - 2011, 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.
+
+**/
+
+#ifndef _VARIABLE_H_
+#define _VARIABLE_H_
+
+#include <Uefi.h>
+
+#include <Protocol/VariableWrite.h>
+#include <Protocol/Variable.h>
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+#include <Guid/VariableFormat.h>
+#include <Guid/GlobalVariable.h>
+
+#include <Guid/EventGroup.h>
+
+#define GET_VARIABLE_NAME_PTR(a)  (CHAR16 *) ((UINTN) (a) + sizeof (VARIABLE_HEADER))
+
+///
+/// The size of a 3 character ISO639 language code.
+///
+#define ISO_639_2_ENTRY_SIZE    3
+
+typedef enum {
+  Physical,
+  Virtual
+} VARIABLE_POINTER_TYPE;
+
+typedef struct {
+  VARIABLE_HEADER *CurrPtr;
+  VARIABLE_HEADER *EndPtr;
+  VARIABLE_HEADER *StartPtr;
+  BOOLEAN         Volatile;
+} VARIABLE_POINTER_TRACK;
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  VolatileVariableBase;
+  EFI_PHYSICAL_ADDRESS  NonVolatileVariableBase;
+  EFI_LOCK              VariableServicesLock;
+} VARIABLE_GLOBAL;
+
+typedef struct {
+  VARIABLE_GLOBAL VariableGlobal[2];
+  UINTN           VolatileLastVariableOffset;
+  UINTN           NonVolatileLastVariableOffset;
+  UINTN           CommonVariableTotalSize;
+  UINTN           HwErrVariableTotalSize;
+  CHAR8           *PlatformLangCodes;
+  CHAR8           *LangCodes;
+  CHAR8           *PlatformLang;
+  CHAR8           Lang[ISO_639_2_ENTRY_SIZE + 1];
+} ESAL_VARIABLE_GLOBAL;
+
+///
+/// Don't use module globals after the SetVirtualAddress map is signaled
+///
+extern ESAL_VARIABLE_GLOBAL *mVariableModuleGlobal;
+
+/**
+  Initializes variable store area for non-volatile and volatile variable.
+
+  This function allocates and initializes memory space for global context of ESAL
+  variable service and variable store area for non-volatile and volatile variable.
+
+  @param  ImageHandle           The Image handle of this driver.
+  @param  SystemTable           The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS           Function successfully executed.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate enough memory resource.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableCommonInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+/**
+  Entry point of EmuVariable service module.
+
+  This function is the entry point of EmuVariable service module.
+  It registers all interfaces of Variable Services, initializes
+  variable store for non-volatile and volatile variables, and registers
+  notification function for EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+
+  @param  ImageHandle   The Image handle of this driver.
+  @param  SystemTable   The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS   Variable service successfully initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceInitialize (
+  IN EFI_HANDLE         ImageHandle,
+  IN EFI_SYSTEM_TABLE   *SystemTable
+  );
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableClassAddressChangeEvent (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  );
+
+/**
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+  
+  @param  VariableName           A Null-terminated Unicode string that is the name of
+                                 the vendor's variable.
+  @param  VendorGuid             A unique identifier for the vendor.
+  @param  Attributes             If not NULL, a pointer to the memory location to return the 
+                                 attributes bitmask for the variable.
+  @param  DataSize               Size of Data found. If size is less than the
+                                 data, this value contains the required size.
+  @param  Data                   On input, the size in bytes of the return Data buffer.  
+                                 On output, the size of data returned in Data.
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure
+
+  @retval EFI_SUCCESS            The function completed successfully. 
+  @retval EFI_NOT_FOUND          The variable was not found.
+  @retval EFI_BUFFER_TOO_SMALL   DataSize is too small for the result.  DataSize has 
+                                 been updated with the size needed to complete the request.
+  @retval EFI_INVALID_PARAMETER  VariableName or VendorGuid or DataSize is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuGetVariable (
+  IN      CHAR16            *VariableName,
+  IN      EFI_GUID          *VendorGuid,
+  OUT     UINT32            *Attributes OPTIONAL,
+  IN OUT  UINTN             *DataSize,
+  OUT     VOID              *Data,
+  IN      VARIABLE_GLOBAL   *Global
+  );
+
+/**
+
+  This code finds the next available variable.
+
+  @param  VariableNameSize       Size of the variable.
+  @param  VariableName           On input, supplies the last VariableName that was returned by GetNextVariableName().
+                                 On output, returns the Null-terminated Unicode string of the current variable.
+  @param  VendorGuid             On input, supplies the last VendorGuid that was returned by GetNextVariableName().
+                                 On output, returns the VendorGuid of the current variable.  
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure.
+
+  @retval EFI_SUCCESS            The function completed successfully. 
+  @retval EFI_NOT_FOUND          The next variable was not found.
+  @retval EFI_BUFFER_TOO_SMALL   VariableNameSize is too small for the result. 
+                                 VariableNameSize has been updated with the size needed to complete the request.
+  @retval EFI_INVALID_PARAMETER  VariableNameSize or VariableName or VendorGuid is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuGetNextVariableName (
+  IN OUT  UINTN             *VariableNameSize,
+  IN OUT  CHAR16            *VariableName,
+  IN OUT  EFI_GUID          *VendorGuid,
+  IN      VARIABLE_GLOBAL   *Global
+  );
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  @param  VariableName           A Null-terminated Unicode string that is the name of the vendor's
+                                 variable.  Each VariableName is unique for each 
+                                 VendorGuid.  VariableName must contain 1 or more 
+                                 Unicode characters.  If VariableName is an empty Unicode 
+                                 string, then EFI_INVALID_PARAMETER is returned.
+  @param  VendorGuid             A unique identifier for the vendor
+  @param  Attributes             Attributes bitmask to set for the variable
+  @param  DataSize               The size in bytes of the Data buffer.  A size of zero causes the
+                                 variable to be deleted.
+  @param  Data                   The contents for the variable
+  @param  Global                 Pointer to VARIABLE_GLOBAL structure
+  @param  VolatileOffset         The offset of last volatile variable
+  @param  NonVolatileOffset      The offset of last non-volatile variable
+
+  @retval EFI_SUCCESS            The firmware has successfully stored the variable and its data as 
+                                 defined by the Attributes.
+  @retval EFI_INVALID_PARAMETER  An invalid combination of attribute bits was supplied, or the 
+                                 DataSize exceeds the maximum allowed, or VariableName is an empty 
+                                 Unicode string, or VendorGuid is NULL.
+  @retval EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval EFI_DEVICE_ERROR       The variable could not be saved due to a hardware failure.
+  @retval EFI_WRITE_PROTECTED    The variable in question is read-only or cannot be deleted.
+  @retval EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuSetVariable (
+  IN CHAR16                  *VariableName,
+  IN EFI_GUID                *VendorGuid,
+  IN UINT32                  Attributes,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data,
+  IN VARIABLE_GLOBAL         *Global,
+  IN UINTN                   *VolatileOffset,
+  IN UINTN                   *NonVolatileOffset
+  );
+
+/**
+
+  This code returns information about the EFI variables.
+
+  @param  Attributes                   Attributes bitmask to specify the type of variables
+                                       on which to return information.
+  @param  MaximumVariableStorageSize   On output the maximum size of the storage space available for 
+                                       the EFI variables associated with the attributes specified.  
+  @param  RemainingVariableStorageSize Returns the remaining size of the storage space available for EFI 
+                                       variables associated with the attributes specified.
+  @param  MaximumVariableSize          Returns the maximum size of an individual EFI variable 
+                                       associated with the attributes specified.
+  @param  Global                       Pointer to VARIABLE_GLOBAL structure.
+
+  @retval EFI_SUCCESS                  Valid answer returned.
+  @retval EFI_INVALID_PARAMETER        An invalid combination of attribute bits was supplied
+  @retval EFI_UNSUPPORTED              The attribute is not supported on this platform, and the 
+                                       MaximumVariableStorageSize, RemainingVariableStorageSize, 
+                                       MaximumVariableSize are undefined.
+
+**/
+EFI_STATUS
+EFIAPI
+EmuQueryVariableInfo (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize,
+  IN  VARIABLE_GLOBAL        *Global
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariable.uni b/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariable.uni
new file mode 100644
index 0000000000..7c11eff3c1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariable.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariableExtra.uni b/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariableExtra.uni
new file mode 100644
index 0000000000..24b14d6d9c
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/Pei/PeiVariableExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/Pei/Variable.c b/Core/MdeModulePkg/Universal/Variable/Pei/Variable.c
new file mode 100644
index 0000000000..c68a41ddfe
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/Pei/Variable.c
@@ -0,0 +1,1182 @@
+/** @file
+  Implement ReadOnly Variable Services required by PEIM and install
+  PEI ReadOnly Varaiable2 PPI. These services operates the non volatile storage space.
+
+Copyright (c) 2006 - 2015, 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 "Variable.h"
+
+//
+// Module globals
+//
+EFI_PEI_READ_ONLY_VARIABLE2_PPI mVariablePpi = {
+  PeiGetVariable,
+  PeiGetNextVariableName
+};
+
+EFI_PEI_PPI_DESCRIPTOR     mPpiListVariable = {
+  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
+  &gEfiPeiReadOnlyVariable2PpiGuid,
+  &mVariablePpi
+};
+
+
+/**
+  Provide the functionality of the variable services.
+
+  @param  FileHandle   Handle of the file being invoked.
+                       Type EFI_PEI_FILE_HANDLE is defined in FfsFindNextFile().
+  @param  PeiServices  General purpose services available to every PEIM.
+
+  @retval EFI_SUCCESS  If the interface could be successfully installed
+  @retval Others       Returned from PeiServicesInstallPpi()
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeVariableServices (
+  IN       EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES          **PeiServices
+  )
+{
+  return PeiServicesInstallPpi (&mPpiListVariable);
+}
+
+/**
+
+  Gets the pointer to the first variable header in given variable store area.
+
+  @param VarStoreHeader  Pointer to the Variable Store Header.
+
+  @return Pointer to the first variable header
+
+**/
+VARIABLE_HEADER *
+GetStartPointer (
+  IN VARIABLE_STORE_HEADER       *VarStoreHeader
+  )
+{
+  //
+  // The end of variable store
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN (VarStoreHeader + 1);
+}
+
+
+/**
+  This code gets the pointer to the last variable memory pointer byte.
+
+  @param  VarStoreHeader  Pointer to the Variable Store Header.
+
+  @return VARIABLE_HEADER* pointer to last unavailable Variable Header.
+
+**/
+VARIABLE_HEADER *
+GetEndPointer (
+  IN VARIABLE_STORE_HEADER       *VarStoreHeader
+  )
+{
+  //
+  // The end of variable store
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN ((UINTN) VarStoreHeader + VarStoreHeader->Size);
+}
+
+
+/**
+  This code checks if variable header is valid or not.
+
+  @param  Variable  Pointer to the Variable Header.
+
+  @retval TRUE      Variable header is valid.
+  @retval FALSE     Variable header is not valid.
+
+**/
+BOOLEAN
+IsValidVariableHeader (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  if (Variable == NULL || Variable->StartId != VARIABLE_DATA ) {
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+/**
+  This code gets the size of variable header.
+
+  @param AuthFlag   Authenticated variable flag.
+
+  @return Size of variable header in bytes in type UINTN.
+
+**/
+UINTN
+GetVariableHeaderSize (
+  IN  BOOLEAN       AuthFlag
+  )
+{
+  UINTN Value;
+
+  if (AuthFlag) {
+    Value = sizeof (AUTHENTICATED_VARIABLE_HEADER);
+  } else {
+    Value = sizeof (VARIABLE_HEADER);
+  }
+
+  return Value;
+}
+
+/**
+  This code gets the size of name of variable.
+
+  @param  Variable  Pointer to the Variable Header.
+  @param  AuthFlag  Authenticated variable flag.
+
+  @return Size of variable in bytes in type UINTN.
+
+**/
+UINTN
+NameSizeOfVariable (
+  IN  VARIABLE_HEADER   *Variable,
+  IN  BOOLEAN           AuthFlag
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (AuthFlag) {
+    if (AuthVariable->State == (UINT8) (-1) ||
+       AuthVariable->DataSize == (UINT32) (-1) ||
+       AuthVariable->NameSize == (UINT32) (-1) ||
+       AuthVariable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) AuthVariable->NameSize;
+  } else {
+    if (Variable->State == (UINT8) (-1) ||
+       Variable->DataSize == (UINT32) (-1) ||
+       Variable->NameSize == (UINT32) (-1) ||
+       Variable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) Variable->NameSize;
+  }
+}
+
+
+/**
+  This code gets the size of data of variable.
+
+  @param  Variable  Pointer to the Variable Header.
+  @param  AuthFlag  Authenticated variable flag.
+
+  @return Size of variable in bytes in type UINTN.
+
+**/
+UINTN
+DataSizeOfVariable (
+  IN  VARIABLE_HEADER   *Variable,
+  IN  BOOLEAN           AuthFlag
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (AuthFlag) {
+    if (AuthVariable->State == (UINT8) (-1) ||
+       AuthVariable->DataSize == (UINT32) (-1) ||
+       AuthVariable->NameSize == (UINT32) (-1) ||
+       AuthVariable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) AuthVariable->DataSize;
+  } else {
+    if (Variable->State == (UINT8) (-1) ||
+       Variable->DataSize == (UINT32) (-1) ||
+       Variable->NameSize == (UINT32) (-1) ||
+       Variable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) Variable->DataSize;
+  }
+}
+
+/**
+  This code gets the pointer to the variable name.
+
+  @param   Variable  Pointer to the Variable Header.
+  @param   AuthFlag  Authenticated variable flag.
+
+  @return  A CHAR16* pointer to Variable Name.
+
+**/
+CHAR16 *
+GetVariableNamePtr (
+  IN VARIABLE_HEADER    *Variable,
+  IN BOOLEAN            AuthFlag
+  )
+{
+  return (CHAR16 *) ((UINTN) Variable + GetVariableHeaderSize (AuthFlag));
+}
+
+/**
+  This code gets the pointer to the variable guid.
+
+  @param Variable   Pointer to the Variable Header.
+  @param AuthFlag   Authenticated variable flag.
+
+  @return A EFI_GUID* pointer to Vendor Guid.
+
+**/
+EFI_GUID *
+GetVendorGuidPtr (
+  IN VARIABLE_HEADER    *Variable,
+  IN BOOLEAN            AuthFlag
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (AuthFlag) {
+    return &AuthVariable->VendorGuid;
+  } else {
+    return &Variable->VendorGuid;
+  }
+}
+
+/**
+  This code gets the pointer to the variable data.
+
+  @param   Variable         Pointer to the Variable Header.
+  @param   VariableHeader   Pointer to the Variable Header that has consecutive content.
+  @param   AuthFlag         Authenticated variable flag.
+
+  @return  A UINT8* pointer to Variable Data.
+
+**/
+UINT8 *
+GetVariableDataPtr (
+  IN  VARIABLE_HEADER   *Variable,
+  IN  VARIABLE_HEADER   *VariableHeader,
+  IN  BOOLEAN           AuthFlag
+  )
+{
+  UINTN Value;
+
+  //
+  // Be careful about pad size for alignment
+  //
+  Value =  (UINTN) GetVariableNamePtr (Variable, AuthFlag);
+  Value += NameSizeOfVariable (VariableHeader, AuthFlag);
+  Value += GET_PAD_SIZE (NameSizeOfVariable (VariableHeader, AuthFlag));
+
+  return (UINT8 *) Value;
+}
+
+
+/**
+  This code gets the pointer to the next variable header.
+
+  @param  StoreInfo         Pointer to variable store info structure.
+  @param  Variable          Pointer to the Variable Header.
+  @param  VariableHeader    Pointer to the Variable Header that has consecutive content.
+
+  @return  A VARIABLE_HEADER* pointer to next variable header.
+
+**/
+VARIABLE_HEADER *
+GetNextVariablePtr (
+  IN  VARIABLE_STORE_INFO   *StoreInfo,
+  IN  VARIABLE_HEADER       *Variable,
+  IN  VARIABLE_HEADER       *VariableHeader
+  )
+{
+  EFI_PHYSICAL_ADDRESS  TargetAddress;
+  EFI_PHYSICAL_ADDRESS  SpareAddress;
+  UINTN                 Value;
+
+  Value =  (UINTN) GetVariableDataPtr (Variable, VariableHeader, StoreInfo->AuthFlag);
+  Value += DataSizeOfVariable (VariableHeader, StoreInfo->AuthFlag);
+  Value += GET_PAD_SIZE (DataSizeOfVariable (VariableHeader, StoreInfo->AuthFlag));
+  //
+  // Be careful about pad size for alignment
+  //
+  Value = HEADER_ALIGN (Value);
+
+  if (StoreInfo->FtwLastWriteData != NULL) {
+    TargetAddress = StoreInfo->FtwLastWriteData->TargetAddress;
+    SpareAddress = StoreInfo->FtwLastWriteData->SpareAddress;
+    if (((UINTN) Variable < (UINTN) TargetAddress) && (Value >= (UINTN) TargetAddress)) {
+      //
+      // Next variable is in spare block.
+      //
+      Value = (UINTN) SpareAddress + (Value - (UINTN) TargetAddress);
+    }
+  }
+
+  return (VARIABLE_HEADER *) Value;
+}
+
+/**
+  Get variable store status.
+
+  @param  VarStoreHeader  Pointer to the Variable Store Header.
+
+  @retval  EfiRaw      Variable store is raw
+  @retval  EfiValid    Variable store is valid
+  @retval  EfiInvalid  Variable store is invalid
+
+**/
+VARIABLE_STORE_STATUS
+GetVariableStoreStatus (
+  IN VARIABLE_STORE_HEADER *VarStoreHeader
+  )
+{
+  if ((CompareGuid (&VarStoreHeader->Signature, &gEfiAuthenticatedVariableGuid) ||
+       CompareGuid (&VarStoreHeader->Signature, &gEfiVariableGuid)) &&
+      VarStoreHeader->Format == VARIABLE_STORE_FORMATTED &&
+      VarStoreHeader->State == VARIABLE_STORE_HEALTHY
+      ) {
+
+    return EfiValid;
+  }
+
+  if (((UINT32 *)(&VarStoreHeader->Signature))[0] == 0xffffffff &&
+      ((UINT32 *)(&VarStoreHeader->Signature))[1] == 0xffffffff &&
+      ((UINT32 *)(&VarStoreHeader->Signature))[2] == 0xffffffff &&
+      ((UINT32 *)(&VarStoreHeader->Signature))[3] == 0xffffffff &&
+      VarStoreHeader->Size == 0xffffffff &&
+      VarStoreHeader->Format == 0xff &&
+      VarStoreHeader->State == 0xff
+      ) {
+
+    return EfiRaw;
+  } else {
+    return EfiInvalid;
+  }
+}
+
+/**
+  Compare two variable names, one of them may be inconsecutive.
+
+  @param StoreInfo      Pointer to variable store info structure.
+  @param Name1          Pointer to one variable name.
+  @param Name2          Pointer to another variable name.
+  @param NameSize       Variable name size.
+
+  @retval TRUE          Name1 and Name2 are identical.
+  @retval FALSE         Name1 and Name2 are not identical.
+
+**/
+BOOLEAN
+CompareVariableName (
+  IN VARIABLE_STORE_INFO    *StoreInfo,
+  IN CONST CHAR16           *Name1,
+  IN CONST CHAR16           *Name2,
+  IN UINTN                  NameSize
+  )
+{
+  EFI_PHYSICAL_ADDRESS  TargetAddress;
+  EFI_PHYSICAL_ADDRESS  SpareAddress;
+  UINTN                 PartialNameSize;
+
+  if (StoreInfo->FtwLastWriteData != NULL) {
+    TargetAddress = StoreInfo->FtwLastWriteData->TargetAddress;
+    SpareAddress = StoreInfo->FtwLastWriteData->SpareAddress;
+    if (((UINTN) Name1 < (UINTN) TargetAddress) && (((UINTN) Name1 + NameSize) > (UINTN) TargetAddress)) {
+      //
+      // Name1 is inconsecutive.
+      //
+      PartialNameSize = (UINTN) TargetAddress - (UINTN) Name1;
+      //
+      // Partial content is in NV storage.
+      //
+      if (CompareMem ((UINT8 *) Name1, (UINT8 *) Name2, PartialNameSize) == 0) {
+        //
+        // Another partial content is in spare block.
+        //
+        if (CompareMem ((UINT8 *) (UINTN) SpareAddress, (UINT8 *) Name2 + PartialNameSize, NameSize - PartialNameSize) == 0) {
+          return TRUE;
+        }
+      }
+      return FALSE;
+    } else if (((UINTN) Name2 < (UINTN) TargetAddress) && (((UINTN) Name2 + NameSize) > (UINTN) TargetAddress)) {
+      //
+      // Name2 is inconsecutive.
+      //
+      PartialNameSize = (UINTN) TargetAddress - (UINTN) Name2;
+      //
+      // Partial content is in NV storage.
+      //
+      if (CompareMem ((UINT8 *) Name2, (UINT8 *) Name1, PartialNameSize) == 0) {
+        //
+        // Another partial content is in spare block.
+        //
+        if (CompareMem ((UINT8 *) (UINTN) SpareAddress, (UINT8 *) Name1 + PartialNameSize, NameSize - PartialNameSize) == 0) {
+          return TRUE;
+        }
+      }
+      return FALSE;
+    }
+  }
+
+  //
+  // Both Name1 and Name2 are consecutive.
+  //
+  if (CompareMem ((UINT8 *) Name1, (UINT8 *) Name2, NameSize) == 0) {
+    return TRUE;
+  }
+  return FALSE;
+}
+
+/**
+  This function compares a variable with variable entries in database.
+
+  @param  StoreInfo     Pointer to variable store info structure.
+  @param  Variable      Pointer to the variable in our database
+  @param  VariableHeader Pointer to the Variable Header that has consecutive content.
+  @param  VariableName  Name of the variable to compare to 'Variable'
+  @param  VendorGuid    GUID of the variable to compare to 'Variable'
+  @param  PtrTrack      Variable Track Pointer structure that contains Variable Information.
+
+  @retval EFI_SUCCESS    Found match variable
+  @retval EFI_NOT_FOUND  Variable not found
+
+**/
+EFI_STATUS
+CompareWithValidVariable (
+  IN  VARIABLE_STORE_INFO           *StoreInfo,
+  IN  VARIABLE_HEADER               *Variable,
+  IN  VARIABLE_HEADER               *VariableHeader,
+  IN  CONST CHAR16                  *VariableName,
+  IN  CONST EFI_GUID                *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK        *PtrTrack
+  )
+{
+  VOID      *Point;
+  EFI_GUID  *TempVendorGuid;
+
+  TempVendorGuid = GetVendorGuidPtr (VariableHeader, StoreInfo->AuthFlag);
+
+  if (VariableName[0] == 0) {
+    PtrTrack->CurrPtr = Variable;
+    return EFI_SUCCESS;
+  } else {
+    //
+    // Don't use CompareGuid function here for performance reasons.
+    // Instead we compare the GUID a UINT32 at a time and branch
+    // on the first failed comparison.
+    //
+    if ((((INT32 *) VendorGuid)[0] == ((INT32 *) TempVendorGuid)[0]) &&
+        (((INT32 *) VendorGuid)[1] == ((INT32 *) TempVendorGuid)[1]) &&
+        (((INT32 *) VendorGuid)[2] == ((INT32 *) TempVendorGuid)[2]) &&
+        (((INT32 *) VendorGuid)[3] == ((INT32 *) TempVendorGuid)[3])
+        ) {
+      ASSERT (NameSizeOfVariable (VariableHeader, StoreInfo->AuthFlag) != 0);
+      Point = (VOID *) GetVariableNamePtr (Variable, StoreInfo->AuthFlag);
+      if (CompareVariableName (StoreInfo, VariableName, Point, NameSizeOfVariable (VariableHeader, StoreInfo->AuthFlag))) {
+        PtrTrack->CurrPtr = Variable;
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Return the variable store header and the store info based on the Index.
+
+  @param Type       The type of the variable store.
+  @param StoreInfo  Return the store info.
+
+  @return  Pointer to the variable store header.
+**/
+VARIABLE_STORE_HEADER *
+GetVariableStore (
+  IN VARIABLE_STORE_TYPE         Type,
+  OUT VARIABLE_STORE_INFO        *StoreInfo
+  )
+{
+  EFI_HOB_GUID_TYPE                     *GuidHob;
+  EFI_FIRMWARE_VOLUME_HEADER            *FvHeader;
+  VARIABLE_STORE_HEADER                 *VariableStoreHeader;
+  EFI_PHYSICAL_ADDRESS                  NvStorageBase;
+  UINT32                                NvStorageSize;
+  FAULT_TOLERANT_WRITE_LAST_WRITE_DATA  *FtwLastWriteData;
+  UINT32                                BackUpOffset;
+
+  StoreInfo->IndexTable = NULL;
+  StoreInfo->FtwLastWriteData = NULL;
+  StoreInfo->AuthFlag = FALSE;
+  VariableStoreHeader = NULL;
+  switch (Type) {
+    case VariableStoreTypeHob:
+      GuidHob = GetFirstGuidHob (&gEfiAuthenticatedVariableGuid);
+      if (GuidHob != NULL) {
+        VariableStoreHeader = (VARIABLE_STORE_HEADER *) GET_GUID_HOB_DATA (GuidHob);
+        StoreInfo->AuthFlag = TRUE;
+      } else {
+        GuidHob = GetFirstGuidHob (&gEfiVariableGuid);
+        if (GuidHob != NULL) {
+          VariableStoreHeader = (VARIABLE_STORE_HEADER *) GET_GUID_HOB_DATA (GuidHob);
+          StoreInfo->AuthFlag = FALSE;
+        }
+      }
+      break;
+
+    case VariableStoreTypeNv:
+      if (GetBootModeHob () != BOOT_IN_RECOVERY_MODE) {
+        //
+        // The content of NV storage for variable is not reliable in recovery boot mode.
+        //
+
+        NvStorageSize = PcdGet32 (PcdFlashNvStorageVariableSize);
+        NvStorageBase = (EFI_PHYSICAL_ADDRESS) (PcdGet64 (PcdFlashNvStorageVariableBase64) != 0 ?
+                                                PcdGet64 (PcdFlashNvStorageVariableBase64) :
+                                                PcdGet32 (PcdFlashNvStorageVariableBase)
+                                               );
+        //
+        // First let FvHeader point to NV storage base.
+        //
+        FvHeader = (EFI_FIRMWARE_VOLUME_HEADER *) (UINTN) NvStorageBase;
+
+        //
+        // Check the FTW last write data hob.
+        //
+        BackUpOffset = 0;
+        GuidHob = GetFirstGuidHob (&gEdkiiFaultTolerantWriteGuid);
+        if (GuidHob != NULL) {
+          FtwLastWriteData = (FAULT_TOLERANT_WRITE_LAST_WRITE_DATA *) GET_GUID_HOB_DATA (GuidHob);
+          if (FtwLastWriteData->TargetAddress == NvStorageBase) {
+            //
+            // Let FvHeader point to spare block.
+            //
+            FvHeader = (EFI_FIRMWARE_VOLUME_HEADER *) (UINTN) FtwLastWriteData->SpareAddress;
+            DEBUG ((EFI_D_INFO, "PeiVariable: NV storage is backed up in spare block: 0x%x\n", (UINTN) FtwLastWriteData->SpareAddress));
+          } else if ((FtwLastWriteData->TargetAddress > NvStorageBase) && (FtwLastWriteData->TargetAddress < (NvStorageBase + NvStorageSize))) {
+            StoreInfo->FtwLastWriteData = FtwLastWriteData;
+            //
+            // Flash NV storage from the offset is backed up in spare block.
+            //
+            BackUpOffset = (UINT32) (FtwLastWriteData->TargetAddress - NvStorageBase);
+            DEBUG ((EFI_D_INFO, "PeiVariable: High partial NV storage from offset: %x is backed up in spare block: 0x%x\n", BackUpOffset, (UINTN) FtwLastWriteData->SpareAddress));
+            //
+            // At least one block data in flash NV storage is still valid, so still leave FvHeader point to NV storage base.
+            //
+          }
+        }
+
+        //
+        // Check if the Firmware Volume is not corrupted
+        //
+        if ((FvHeader->Signature != EFI_FVH_SIGNATURE) || (!CompareGuid (&gEfiSystemNvDataFvGuid, &FvHeader->FileSystemGuid))) {
+          DEBUG ((EFI_D_ERROR, "Firmware Volume for Variable Store is corrupted\n"));
+          break;
+        }
+
+        VariableStoreHeader = (VARIABLE_STORE_HEADER *) ((UINT8 *) FvHeader + FvHeader->HeaderLength);
+
+        StoreInfo->AuthFlag = (BOOLEAN) (CompareGuid (&VariableStoreHeader->Signature, &gEfiAuthenticatedVariableGuid));
+
+        GuidHob = GetFirstGuidHob (&gEfiVariableIndexTableGuid);
+        if (GuidHob != NULL) {
+          StoreInfo->IndexTable = GET_GUID_HOB_DATA (GuidHob);
+        } else {
+          //
+          // If it's the first time to access variable region in flash, create a guid hob to record
+          // VAR_ADDED type variable info.
+          // Note that as the resource of PEI phase is limited, only store the limited number of
+          // VAR_ADDED type variables to reduce access time.
+          //
+          StoreInfo->IndexTable = (VARIABLE_INDEX_TABLE *) BuildGuidHob (&gEfiVariableIndexTableGuid, sizeof (VARIABLE_INDEX_TABLE));
+          StoreInfo->IndexTable->Length      = 0;
+          StoreInfo->IndexTable->StartPtr    = GetStartPointer (VariableStoreHeader);
+          StoreInfo->IndexTable->EndPtr      = GetEndPointer   (VariableStoreHeader);
+          StoreInfo->IndexTable->GoneThrough = 0;
+        }
+      }
+      break;
+
+    default:
+      ASSERT (FALSE);
+      break;
+  }
+
+  StoreInfo->VariableStoreHeader = VariableStoreHeader;
+  return VariableStoreHeader;
+}
+
+/**
+  Get variable header that has consecutive content.
+
+  @param StoreInfo      Pointer to variable store info structure.
+  @param Variable       Pointer to the Variable Header.
+  @param VariableHeader Pointer to Pointer to the Variable Header that has consecutive content.
+
+  @retval TRUE          Variable header is valid.
+  @retval FALSE         Variable header is not valid.
+
+**/
+BOOLEAN
+GetVariableHeader (
+  IN VARIABLE_STORE_INFO    *StoreInfo,
+  IN VARIABLE_HEADER        *Variable,
+  OUT VARIABLE_HEADER       **VariableHeader
+  )
+{
+  EFI_PHYSICAL_ADDRESS  TargetAddress;
+  EFI_PHYSICAL_ADDRESS  SpareAddress;
+  EFI_HOB_GUID_TYPE     *GuidHob;
+  UINTN                 PartialHeaderSize;
+
+  if (Variable == NULL) {
+    return FALSE;
+  }
+
+  //
+  // First assume variable header pointed by Variable is consecutive.
+  //
+  *VariableHeader = Variable;
+
+  if (StoreInfo->FtwLastWriteData != NULL) {
+    TargetAddress = StoreInfo->FtwLastWriteData->TargetAddress;
+    SpareAddress = StoreInfo->FtwLastWriteData->SpareAddress;
+    if (((UINTN) Variable > (UINTN) SpareAddress) &&
+        (((UINTN) Variable - (UINTN) SpareAddress + (UINTN) TargetAddress) >= (UINTN) GetEndPointer (StoreInfo->VariableStoreHeader))) {
+      //
+      // Reach the end of variable store.
+      //
+      return FALSE;
+    }
+    if (((UINTN) Variable < (UINTN) TargetAddress) && (((UINTN) Variable + GetVariableHeaderSize (StoreInfo->AuthFlag)) > (UINTN) TargetAddress)) {
+      //
+      // Variable header pointed by Variable is inconsecutive,
+      // create a guid hob to combine the two partial variable header content together.
+      //
+      GuidHob = GetFirstGuidHob (&gEfiCallerIdGuid);
+      if (GuidHob != NULL) {
+        *VariableHeader = (VARIABLE_HEADER *) GET_GUID_HOB_DATA (GuidHob);
+      } else {
+        *VariableHeader = (VARIABLE_HEADER *) BuildGuidHob (&gEfiCallerIdGuid, GetVariableHeaderSize (StoreInfo->AuthFlag));
+        PartialHeaderSize = (UINTN) TargetAddress - (UINTN) Variable;
+        //
+        // Partial content is in NV storage.
+        //
+        CopyMem ((UINT8 *) *VariableHeader, (UINT8 *) Variable, PartialHeaderSize);
+        //
+        // Another partial content is in spare block.
+        //
+        CopyMem ((UINT8 *) *VariableHeader + PartialHeaderSize, (UINT8 *) (UINTN) SpareAddress, GetVariableHeaderSize (StoreInfo->AuthFlag) - PartialHeaderSize);
+      }
+    }
+  } else {
+    if (Variable >= GetEndPointer (StoreInfo->VariableStoreHeader)) {
+      //
+      // Reach the end of variable store.
+      //
+      return FALSE;
+    }
+  }
+
+  return IsValidVariableHeader (*VariableHeader);
+}
+
+/**
+  Get variable name or data to output buffer.
+
+  @param  StoreInfo     Pointer to variable store info structure.
+  @param  NameOrData    Pointer to the variable name/data that may be inconsecutive.
+  @param  Size          Variable name/data size.
+  @param  Buffer        Pointer to output buffer to hold the variable name/data.
+
+**/
+VOID
+GetVariableNameOrData (
+  IN VARIABLE_STORE_INFO    *StoreInfo,
+  IN UINT8                  *NameOrData,
+  IN UINTN                  Size,
+  OUT UINT8                 *Buffer
+  )
+{
+  EFI_PHYSICAL_ADDRESS  TargetAddress;
+  EFI_PHYSICAL_ADDRESS  SpareAddress;
+  UINTN                 PartialSize;
+
+  if (StoreInfo->FtwLastWriteData != NULL) {
+    TargetAddress = StoreInfo->FtwLastWriteData->TargetAddress;
+    SpareAddress = StoreInfo->FtwLastWriteData->SpareAddress;
+    if (((UINTN) NameOrData < (UINTN) TargetAddress) && (((UINTN) NameOrData + Size) > (UINTN) TargetAddress)) {
+      //
+      // Variable name/data is inconsecutive.
+      //
+      PartialSize = (UINTN) TargetAddress - (UINTN) NameOrData;
+      //
+      // Partial content is in NV storage.
+      //
+      CopyMem (Buffer, NameOrData, PartialSize);
+      //
+      // Another partial content is in spare block.
+      //
+      CopyMem (Buffer + PartialSize, (UINT8 *) (UINTN) SpareAddress, Size - PartialSize);
+      return;
+    }
+  }
+
+  //
+  // Variable name/data is consecutive.
+  //
+  CopyMem (Buffer, NameOrData, Size);
+}
+
+/**
+  Find the variable in the specified variable store.
+
+  @param  StoreInfo           Pointer to the store info structure.
+  @param  VariableName        Name of the variable to be found
+  @param  VendorGuid          Vendor GUID to be found.
+  @param  PtrTrack            Variable Track Pointer structure that contains Variable Information.
+
+  @retval  EFI_SUCCESS            Variable found successfully
+  @retval  EFI_NOT_FOUND          Variable not found
+  @retval  EFI_INVALID_PARAMETER  Invalid variable name
+
+**/
+EFI_STATUS
+FindVariableEx (
+  IN VARIABLE_STORE_INFO         *StoreInfo,
+  IN CONST CHAR16                *VariableName,
+  IN CONST EFI_GUID              *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK     *PtrTrack
+  )
+{
+  VARIABLE_HEADER         *Variable;
+  VARIABLE_HEADER         *LastVariable;
+  VARIABLE_HEADER         *MaxIndex;
+  UINTN                   Index;
+  UINTN                   Offset;
+  BOOLEAN                 StopRecord;
+  VARIABLE_HEADER         *InDeletedVariable;
+  VARIABLE_STORE_HEADER   *VariableStoreHeader;
+  VARIABLE_INDEX_TABLE    *IndexTable;
+  VARIABLE_HEADER         *VariableHeader;
+
+  VariableStoreHeader = StoreInfo->VariableStoreHeader;
+
+  if (VariableStoreHeader == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (GetVariableStoreStatus (VariableStoreHeader) != EfiValid) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if (~VariableStoreHeader->Size == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  IndexTable = StoreInfo->IndexTable;
+  PtrTrack->StartPtr = GetStartPointer (VariableStoreHeader);
+  PtrTrack->EndPtr   = GetEndPointer   (VariableStoreHeader);
+
+  InDeletedVariable = NULL;
+
+  //
+  // No Variable Address equals zero, so 0 as initial value is safe.
+  //
+  MaxIndex   = NULL;
+  VariableHeader = NULL;
+
+  if (IndexTable != NULL) {
+    //
+    // traverse the variable index table to look for varible.
+    // The IndexTable->Index[Index] records the distance of two neighbouring VAR_ADDED type variables.
+    //
+    for (Offset = 0, Index = 0; Index < IndexTable->Length; Index++) {
+      ASSERT (Index < sizeof (IndexTable->Index) / sizeof (IndexTable->Index[0]));
+      Offset   += IndexTable->Index[Index];
+      MaxIndex  = (VARIABLE_HEADER *) ((UINT8 *) IndexTable->StartPtr + Offset);
+      GetVariableHeader (StoreInfo, MaxIndex, &VariableHeader);
+      if (CompareWithValidVariable (StoreInfo, MaxIndex, VariableHeader, VariableName, VendorGuid, PtrTrack) == EFI_SUCCESS) {
+        if (VariableHeader->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+          InDeletedVariable = PtrTrack->CurrPtr;
+        } else {
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    if (IndexTable->GoneThrough != 0) {
+      //
+      // If the table has all the existing variables indexed, return.
+      //
+      PtrTrack->CurrPtr = InDeletedVariable;
+      return (PtrTrack->CurrPtr == NULL) ? EFI_NOT_FOUND : EFI_SUCCESS;
+    }
+  }
+
+  if (MaxIndex != NULL) {
+    //
+    // HOB exists but the variable cannot be found in HOB
+    // If not found in HOB, then let's start from the MaxIndex we've found.
+    //
+    Variable     = GetNextVariablePtr (StoreInfo, MaxIndex, VariableHeader);
+    LastVariable = MaxIndex;
+  } else {
+    //
+    // Start Pointers for the variable.
+    // Actual Data Pointer where data can be written.
+    //
+    Variable     = PtrTrack->StartPtr;
+    LastVariable = PtrTrack->StartPtr;
+  }
+
+  //
+  // Find the variable by walk through variable store
+  //
+  StopRecord = FALSE;
+  while (GetVariableHeader (StoreInfo, Variable, &VariableHeader)) {
+    if (VariableHeader->State == VAR_ADDED || VariableHeader->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+      //
+      // Record Variable in VariableIndex HOB
+      //
+      if ((IndexTable != NULL) && !StopRecord) {
+        Offset = (UINTN) Variable - (UINTN) LastVariable;
+        if ((Offset > 0x0FFFF) || (IndexTable->Length == sizeof (IndexTable->Index) / sizeof (IndexTable->Index[0]))) {
+          //
+          // Stop to record if the distance of two neighbouring VAR_ADDED variable is larger than the allowable scope(UINT16),
+          // or the record buffer is full.
+          //
+          StopRecord = TRUE;
+        } else {
+          IndexTable->Index[IndexTable->Length++] = (UINT16) Offset;
+          LastVariable = Variable;
+        }
+      }
+
+      if (CompareWithValidVariable (StoreInfo, Variable, VariableHeader, VariableName, VendorGuid, PtrTrack) == EFI_SUCCESS) {
+        if (VariableHeader->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+          InDeletedVariable = PtrTrack->CurrPtr;
+        } else {
+          return EFI_SUCCESS;
+        }
+      }
+    }
+
+    Variable = GetNextVariablePtr (StoreInfo, Variable, VariableHeader);
+  }
+  //
+  // If gone through the VariableStore, that means we never find in Firmware any more.
+  //
+  if ((IndexTable != NULL) && !StopRecord) {
+    IndexTable->GoneThrough = 1;
+  }
+
+  PtrTrack->CurrPtr = InDeletedVariable;
+
+  return (PtrTrack->CurrPtr == NULL) ? EFI_NOT_FOUND : EFI_SUCCESS;
+}
+
+/**
+  Find the variable in HOB and Non-Volatile variable storages.
+
+  @param  VariableName  Name of the variable to be found
+  @param  VendorGuid    Vendor GUID to be found.
+  @param  PtrTrack      Variable Track Pointer structure that contains Variable Information.
+  @param  StoreInfo     Return the store info.
+
+  @retval  EFI_SUCCESS            Variable found successfully
+  @retval  EFI_NOT_FOUND          Variable not found
+  @retval  EFI_INVALID_PARAMETER  Invalid variable name
+**/
+EFI_STATUS
+FindVariable (
+  IN CONST  CHAR16            *VariableName,
+  IN CONST  EFI_GUID          *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK  *PtrTrack,
+  OUT VARIABLE_STORE_INFO     *StoreInfo
+  )
+{
+  EFI_STATUS                  Status;
+  VARIABLE_STORE_TYPE         Type;
+
+  if (VariableName[0] != 0 && VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (Type = (VARIABLE_STORE_TYPE) 0; Type < VariableStoreTypeMax; Type++) {
+    GetVariableStore (Type, StoreInfo);
+    Status = FindVariableEx (
+               StoreInfo,
+               VariableName,
+               VendorGuid,
+               PtrTrack
+               );
+    if (!EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  This service retrieves a variable's value using its name and GUID.
+
+  Read the specified variable from the UEFI variable store. If the Data
+  buffer is too small to hold the contents of the variable, the error
+  EFI_BUFFER_TOO_SMALL is returned and DataSize is set to the required buffer
+  size to obtain the data.
+
+  @param  This                  A pointer to this instance of the EFI_PEI_READ_ONLY_VARIABLE2_PPI.
+  @param  VariableName          A pointer to a null-terminated string that is the variable's name.
+  @param  VariableGuid          A pointer to an EFI_GUID that is the variable's GUID. The combination of
+                                VariableGuid and VariableName must be unique.
+  @param  Attributes            If non-NULL, on return, points to the variable's attributes.
+  @param  DataSize              On entry, points to the size in bytes of the Data buffer.
+                                On return, points to the size of the data returned in Data.
+  @param  Data                  Points to the buffer which will hold the returned variable value.
+
+  @retval EFI_SUCCESS           The variable was read successfully.
+  @retval EFI_NOT_FOUND         The variable could not be found.
+  @retval EFI_BUFFER_TOO_SMALL  The DataSize is too small for the resulting data.
+                                DataSize is updated with the size required for
+                                the specified variable.
+  @retval EFI_INVALID_PARAMETER VariableName, VariableGuid, DataSize or Data is NULL.
+  @retval EFI_DEVICE_ERROR      The variable could not be retrieved because of a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetVariable (
+  IN CONST  EFI_PEI_READ_ONLY_VARIABLE2_PPI *This,
+  IN CONST  CHAR16                          *VariableName,
+  IN CONST  EFI_GUID                        *VariableGuid,
+  OUT       UINT32                          *Attributes,
+  IN OUT    UINTN                           *DataSize,
+  OUT       VOID                            *Data
+  )
+{
+  VARIABLE_POINTER_TRACK  Variable;
+  UINTN                   VarDataSize;
+  EFI_STATUS              Status;
+  VARIABLE_STORE_INFO     StoreInfo;
+  VARIABLE_HEADER         *VariableHeader;
+
+  if (VariableName == NULL || VariableGuid == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableHeader = NULL;
+
+  //
+  // Find existing variable
+  //
+  Status = FindVariable (VariableName, VariableGuid, &Variable, &StoreInfo);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  GetVariableHeader (&StoreInfo, Variable.CurrPtr, &VariableHeader);
+
+  //
+  // Get data size
+  //
+  VarDataSize = DataSizeOfVariable (VariableHeader, StoreInfo.AuthFlag);
+  if (*DataSize >= VarDataSize) {
+    if (Data == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    GetVariableNameOrData (&StoreInfo, GetVariableDataPtr (Variable.CurrPtr, VariableHeader, StoreInfo.AuthFlag), VarDataSize, Data);
+
+    if (Attributes != NULL) {
+      *Attributes = VariableHeader->Attributes;
+    }
+
+    *DataSize = VarDataSize;
+    return EFI_SUCCESS;
+  } else {
+    *DataSize = VarDataSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+}
+
+/**
+  Return the next variable name and GUID.
+
+  This function is called multiple times to retrieve the VariableName
+  and VariableGuid of all variables currently available in the system.
+  On each call, the previous results are passed into the interface,
+  and, on return, the interface returns the data for the next
+  interface. When the entire variable list has been returned,
+  EFI_NOT_FOUND is returned.
+
+  @param  This              A pointer to this instance of the EFI_PEI_READ_ONLY_VARIABLE2_PPI.
+
+  @param  VariableNameSize  On entry, points to the size of the buffer pointed to by VariableName.
+                            On return, the size of the variable name buffer.
+  @param  VariableName      On entry, a pointer to a null-terminated string that is the variable's name.
+                            On return, points to the next variable's null-terminated name string.
+  @param  VariableGuid      On entry, a pointer to an EFI_GUID that is the variable's GUID.
+                            On return, a pointer to the next variable's GUID.
+
+  @retval EFI_SUCCESS           The variable was read successfully.
+  @retval EFI_NOT_FOUND         The variable could not be found.
+  @retval EFI_BUFFER_TOO_SMALL  The VariableNameSize is too small for the resulting
+                                data. VariableNameSize is updated with the size
+                                required for the specified variable.
+  @retval EFI_INVALID_PARAMETER VariableName, VariableGuid or
+                                VariableNameSize is NULL.
+  @retval EFI_DEVICE_ERROR      The variable could not be retrieved because of a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetNextVariableName (
+  IN CONST  EFI_PEI_READ_ONLY_VARIABLE2_PPI *This,
+  IN OUT UINTN                              *VariableNameSize,
+  IN OUT CHAR16                             *VariableName,
+  IN OUT EFI_GUID                           *VariableGuid
+  )
+{
+  VARIABLE_STORE_TYPE     Type;
+  VARIABLE_POINTER_TRACK  Variable;
+  VARIABLE_POINTER_TRACK  VariableInHob;
+  VARIABLE_POINTER_TRACK  VariablePtrTrack;
+  UINTN                   VarNameSize;
+  EFI_STATUS              Status;
+  VARIABLE_STORE_HEADER   *VariableStoreHeader[VariableStoreTypeMax];
+  VARIABLE_HEADER         *VariableHeader;
+  VARIABLE_STORE_INFO     StoreInfo;
+  VARIABLE_STORE_INFO     StoreInfoForNv;
+  VARIABLE_STORE_INFO     StoreInfoForHob;
+
+  if (VariableName == NULL || VariableGuid == NULL || VariableNameSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableHeader = NULL;
+
+  Status = FindVariable (VariableName, VariableGuid, &Variable, &StoreInfo);
+  if (Variable.CurrPtr == NULL || Status != EFI_SUCCESS) {
+    return Status;
+  }
+
+  if (VariableName[0] != 0) {
+    //
+    // If variable name is not NULL, get next variable
+    //
+    GetVariableHeader (&StoreInfo, Variable.CurrPtr, &VariableHeader);
+    Variable.CurrPtr = GetNextVariablePtr (&StoreInfo, Variable.CurrPtr, VariableHeader);
+  }
+
+  VariableStoreHeader[VariableStoreTypeHob] = GetVariableStore (VariableStoreTypeHob, &StoreInfoForHob);
+  VariableStoreHeader[VariableStoreTypeNv]  = GetVariableStore (VariableStoreTypeNv, &StoreInfoForNv);
+
+  while (TRUE) {
+    //
+    // Switch from HOB to Non-Volatile.
+    //
+    while (!GetVariableHeader (&StoreInfo, Variable.CurrPtr, &VariableHeader)) {
+      //
+      // Find current storage index
+      //
+      for (Type = (VARIABLE_STORE_TYPE) 0; Type < VariableStoreTypeMax; Type++) {
+        if ((VariableStoreHeader[Type] != NULL) && (Variable.StartPtr == GetStartPointer (VariableStoreHeader[Type]))) {
+          break;
+        }
+      }
+      ASSERT (Type < VariableStoreTypeMax);
+      //
+      // Switch to next storage
+      //
+      for (Type++; Type < VariableStoreTypeMax; Type++) {
+        if (VariableStoreHeader[Type] != NULL) {
+          break;
+        }
+      }
+      //
+      // Capture the case that
+      // 1. current storage is the last one, or
+      // 2. no further storage
+      //
+      if (Type == VariableStoreTypeMax) {
+        return EFI_NOT_FOUND;
+      }
+      Variable.StartPtr = GetStartPointer (VariableStoreHeader[Type]);
+      Variable.EndPtr   = GetEndPointer   (VariableStoreHeader[Type]);
+      Variable.CurrPtr  = Variable.StartPtr;
+      GetVariableStore (Type, &StoreInfo);
+    }
+
+    if (VariableHeader->State == VAR_ADDED || VariableHeader->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+      if (VariableHeader->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+        //
+        // If it is a IN_DELETED_TRANSITION variable,
+        // and there is also a same ADDED one at the same time,
+        // don't return it.
+        //
+        Status = FindVariableEx (
+                   &StoreInfo,
+                   GetVariableNamePtr (Variable.CurrPtr, StoreInfo.AuthFlag),
+                   GetVendorGuidPtr (VariableHeader, StoreInfo.AuthFlag),
+                   &VariablePtrTrack
+                   );
+        if (!EFI_ERROR (Status) && VariablePtrTrack.CurrPtr != Variable.CurrPtr) {
+          Variable.CurrPtr = GetNextVariablePtr (&StoreInfo, Variable.CurrPtr, VariableHeader);
+          continue;
+        }
+      }
+
+      //
+      // Don't return NV variable when HOB overrides it
+      //
+      if ((VariableStoreHeader[VariableStoreTypeHob] != NULL) && (VariableStoreHeader[VariableStoreTypeNv] != NULL) &&
+          (Variable.StartPtr == GetStartPointer (VariableStoreHeader[VariableStoreTypeNv]))
+         ) {
+        Status = FindVariableEx (
+                   &StoreInfoForHob,
+                   GetVariableNamePtr (Variable.CurrPtr, StoreInfo.AuthFlag),
+                   GetVendorGuidPtr (VariableHeader, StoreInfo.AuthFlag),
+                   &VariableInHob
+                   );
+        if (!EFI_ERROR (Status)) {
+          Variable.CurrPtr = GetNextVariablePtr (&StoreInfo, Variable.CurrPtr, VariableHeader);
+          continue;
+        }
+      }
+
+      VarNameSize = NameSizeOfVariable (VariableHeader, StoreInfo.AuthFlag);
+      ASSERT (VarNameSize != 0);
+
+      if (VarNameSize <= *VariableNameSize) {
+        GetVariableNameOrData (&StoreInfo, (UINT8 *) GetVariableNamePtr (Variable.CurrPtr, StoreInfo.AuthFlag), VarNameSize, (UINT8 *) VariableName);
+
+        CopyMem (VariableGuid, GetVendorGuidPtr (VariableHeader, StoreInfo.AuthFlag), sizeof (EFI_GUID));
+
+        Status = EFI_SUCCESS;
+      } else {
+        Status = EFI_BUFFER_TOO_SMALL;
+      }
+
+      *VariableNameSize = VarNameSize;
+      //
+      // Variable is found
+      //
+      return Status;
+    } else {
+      Variable.CurrPtr = GetNextVariablePtr (&StoreInfo, Variable.CurrPtr, VariableHeader);
+    }
+  }
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/Pei/Variable.h b/Core/MdeModulePkg/Universal/Variable/Pei/Variable.h
new file mode 100644
index 0000000000..580d69fb42
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/Pei/Variable.h
@@ -0,0 +1,149 @@
+/** @file
+  The internal header file includes the common header files, defines
+  internal structure and functions used by PeiVariable module.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _PEI_VARIABLE_H_
+#define _PEI_VARIABLE_H_
+
+#include <PiPei.h>
+#include <Ppi/ReadOnlyVariable2.h>
+
+#include <Library/DebugLib.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/HobLib.h>
+#include <Library/PcdLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PeiServicesTablePointerLib.h>
+#include <Library/PeiServicesLib.h>
+
+#include <Guid/VariableFormat.h>
+#include <Guid/VariableIndexTable.h>
+#include <Guid/SystemNvDataGuid.h>
+#include <Guid/FaultTolerantWrite.h>
+
+typedef enum {
+  VariableStoreTypeHob,
+  VariableStoreTypeNv,
+  VariableStoreTypeMax
+} VARIABLE_STORE_TYPE;
+
+typedef struct {
+  VARIABLE_STORE_HEADER                   *VariableStoreHeader;
+  VARIABLE_INDEX_TABLE                    *IndexTable;
+  //
+  // If it is not NULL, it means there may be an inconsecutive variable whose
+  // partial content is still in NV storage, but another partial content is backed up
+  // in spare block.
+  //
+  FAULT_TOLERANT_WRITE_LAST_WRITE_DATA    *FtwLastWriteData;
+  BOOLEAN                                 AuthFlag;
+} VARIABLE_STORE_INFO;
+
+//
+// Functions
+//
+/**
+  Provide the functionality of the variable services.
+
+  @param  FileHandle  Handle of the file being invoked.
+                      Type EFI_PEI_FILE_HANDLE is defined in FfsFindNextFile().
+  @param  PeiServices  General purpose services available to every PEIM.
+
+  @retval EFI_SUCCESS  If the interface could be successfully installed
+  @retval Others       Returned from PeiServicesInstallPpi()
+
+**/
+EFI_STATUS
+EFIAPI
+PeimInitializeVariableServices (
+  IN       EFI_PEI_FILE_HANDLE       FileHandle,
+  IN CONST EFI_PEI_SERVICES          **PeiServices
+  );
+
+/**
+  This service retrieves a variable's value using its name and GUID.
+
+  Read the specified variable from the UEFI variable store. If the Data
+  buffer is too small to hold the contents of the variable, the error
+  EFI_BUFFER_TOO_SMALL is returned and DataSize is set to the required buffer
+  size to obtain the data.
+
+  @param  This                  A pointer to this instance of the EFI_PEI_READ_ONLY_VARIABLE2_PPI.
+  @param  VariableName          A pointer to a null-terminated string that is the variable's name.
+  @param  VariableGuid          A pointer to an EFI_GUID that is the variable's GUID. The combination of
+                                VariableGuid and VariableName must be unique.
+  @param  Attributes            If non-NULL, on return, points to the variable's attributes.
+  @param  DataSize              On entry, points to the size in bytes of the Data buffer.
+                                On return, points to the size of the data returned in Data.
+  @param  Data                  Points to the buffer which will hold the returned variable value.
+
+  @retval EFI_SUCCESS           The variable was read successfully.
+  @retval EFI_NOT_FOUND         The variable could not be found.
+  @retval EFI_BUFFER_TOO_SMALL  The DataSize is too small for the resulting data.
+                                DataSize is updated with the size required for
+                                the specified variable.
+  @retval EFI_INVALID_PARAMETER VariableName, VariableGuid, DataSize or Data is NULL.
+  @retval EFI_DEVICE_ERROR      The variable could not be retrieved because of a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetVariable (
+  IN CONST  EFI_PEI_READ_ONLY_VARIABLE2_PPI *This,
+  IN CONST  CHAR16                          *VariableName,
+  IN CONST  EFI_GUID                        *VariableGuid,
+  OUT       UINT32                          *Attributes,
+  IN OUT    UINTN                           *DataSize,
+  OUT       VOID                            *Data
+  );
+
+/**
+  Return the next variable name and GUID.
+
+  This function is called multiple times to retrieve the VariableName
+  and VariableGuid of all variables currently available in the system.
+  On each call, the previous results are passed into the interface,
+  and, on return, the interface returns the data for the next
+  interface. When the entire variable list has been returned,
+  EFI_NOT_FOUND is returned.
+
+  @param  This              A pointer to this instance of the EFI_PEI_READ_ONLY_VARIABLE2_PPI.
+
+  @param  VariableNameSize  On entry, points to the size of the buffer pointed to by VariableName.
+  @param  VariableName      On entry, a pointer to a null-terminated string that is the variable's name.
+                            On return, points to the next variable's null-terminated name string.
+
+  @param  VariableGuid      On entry, a pointer to an UEFI _GUID that is the variable's GUID.
+                            On return, a pointer to the next variable's GUID.
+
+  @retval EFI_SUCCESS           The variable was read successfully.
+  @retval EFI_NOT_FOUND         The variable could not be found.
+  @retval EFI_BUFFER_TOO_SMALL  The VariableNameSize is too small for the resulting
+                                data. VariableNameSize is updated with the size
+                                required for the specified variable.
+  @retval EFI_INVALID_PARAMETER VariableName, VariableGuid or
+                                VariableNameSize is NULL.
+  @retval EFI_DEVICE_ERROR      The variable could not be retrieved because of a device error.
+
+**/
+EFI_STATUS
+EFIAPI
+PeiGetNextVariableName (
+  IN CONST  EFI_PEI_READ_ONLY_VARIABLE2_PPI *This,
+  IN OUT UINTN                              *VariableNameSize,
+  IN OUT CHAR16                             *VariableName,
+  IN OUT EFI_GUID                           *VariableGuid
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Variable/Pei/VariablePei.inf b/Core/MdeModulePkg/Universal/Variable/Pei/VariablePei.inf
new file mode 100644
index 0000000000..72a440dcd1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/Pei/VariablePei.inf
@@ -0,0 +1,79 @@
+## @file
+#  Implements ReadOnly Variable Services required by PEIM and installs PEI ReadOnly Varaiable2 PPI.
+#
+#  This module implements ReadOnly Variable Services required by PEIM and installs PEI ReadOnly Varaiable2 PPI.
+#
+#  Copyright (c) 2006 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = PeiVariable
+  MODULE_UNI_FILE                = PeiVariable.uni
+  FILE_GUID                      = 34C8C28F-B61C-45a2-8F2E-89E46BECC63B
+  MODULE_TYPE                    = PEIM
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = PeimInitializeVariableServices
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Sources]
+  Variable.c
+  Variable.h
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  BaseMemoryLib
+  PcdLib
+  HobLib
+  PeimEntryPoint
+  DebugLib
+  PeiServicesTablePointerLib
+  PeiServicesLib
+
+[Guids]
+  ## CONSUMES             ## GUID # Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  gEfiAuthenticatedVariableGuid
+  ## SOMETIMES_CONSUMES   ## GUID # Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  gEfiVariableGuid
+  ## SOMETIMES_PRODUCES   ## HOB
+  ## SOMETIMES_CONSUMES   ## HOB
+  gEfiVariableIndexTableGuid
+  gEfiSystemNvDataFvGuid            ## SOMETIMES_CONSUMES   ## GUID
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## CONSUMES             ## GUID # Dependence
+  gEdkiiFaultTolerantWriteGuid
+
+[Ppis]
+  gEfiPeiReadOnlyVariable2PpiGuid   ## PRODUCES
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase64    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableSize      ## CONSUMES
+
+[Depex]
+  gEdkiiFaultTolerantWriteGuid
+
+# [BootMode]
+# RECOVERY_FULL             ## SOMETIMES_CONSUMES
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  PeiVariableExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Measurement.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Measurement.c
new file mode 100644
index 0000000000..2f92fae310
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Measurement.c
@@ -0,0 +1,255 @@
+/** @file
+  Measure TrEE required variable.
+
+Copyright (c) 2013 - 2014, 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 <PiDxe.h>
+#include <Guid/ImageAuthentication.h>
+#include <IndustryStandard/UefiTcgPlatform.h>
+#include <Protocol/TrEEProtocol.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/TpmMeasurementLib.h>
+
+typedef struct {
+  CHAR16                                 *VariableName;
+  EFI_GUID                               *VendorGuid;
+} VARIABLE_TYPE;
+
+VARIABLE_TYPE  mVariableType[] = {
+  {EFI_SECURE_BOOT_MODE_NAME,    &gEfiGlobalVariableGuid},
+  {EFI_PLATFORM_KEY_NAME,        &gEfiGlobalVariableGuid},
+  {EFI_KEY_EXCHANGE_KEY_NAME,    &gEfiGlobalVariableGuid},
+  {EFI_IMAGE_SECURITY_DATABASE,  &gEfiImageSecurityDatabaseGuid},
+  {EFI_IMAGE_SECURITY_DATABASE1, &gEfiImageSecurityDatabaseGuid},
+};
+
+/**
+  This function will return if this variable is SecureBootPolicy Variable.
+
+  @param[in]  VariableName      A Null-terminated string that is the name of the vendor's variable.
+  @param[in]  VendorGuid        A unique identifier for the vendor.
+
+  @retval TRUE  This is SecureBootPolicy Variable
+  @retval FALSE This is not SecureBootPolicy Variable
+**/
+BOOLEAN
+IsSecureBootPolicyVariable (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  )
+{
+  UINTN   Index;
+
+  for (Index = 0; Index < sizeof(mVariableType)/sizeof(mVariableType[0]); Index++) {
+    if ((StrCmp (VariableName, mVariableType[Index].VariableName) == 0) &&
+        (CompareGuid (VendorGuid, mVariableType[Index].VendorGuid))) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Measure and log an EFI variable, and extend the measurement result into a specific PCR.
+
+  @param[in]  VarName           A Null-terminated string that is the name of the vendor's variable.
+  @param[in]  VendorGuid        A unique identifier for the vendor.
+  @param[in]  VarData           The content of the variable data.
+  @param[in]  VarSize           The size of the variable data.
+
+  @retval EFI_SUCCESS           Operation completed successfully.
+  @retval EFI_OUT_OF_RESOURCES  Out of memory.
+  @retval EFI_DEVICE_ERROR      The operation was unsuccessful.
+
+**/
+EFI_STATUS
+EFIAPI
+MeasureVariable (
+  IN      CHAR16                    *VarName,
+  IN      EFI_GUID                  *VendorGuid,
+  IN      VOID                      *VarData,
+  IN      UINTN                     VarSize
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             VarNameLength;
+  EFI_VARIABLE_DATA_TREE            *VarLog;
+  UINT32                            VarLogSize;
+
+  ASSERT ((VarSize == 0 && VarData == NULL) || (VarSize != 0 && VarData != NULL));
+
+  VarNameLength      = StrLen (VarName);
+  VarLogSize = (UINT32)(sizeof (*VarLog) + VarNameLength * sizeof (*VarName) + VarSize
+                        - sizeof (VarLog->UnicodeName) - sizeof (VarLog->VariableData));
+
+  VarLog = (EFI_VARIABLE_DATA_TREE *) AllocateZeroPool (VarLogSize);
+  if (VarLog == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  CopyMem (&VarLog->VariableName, VendorGuid, sizeof(VarLog->VariableName));
+  VarLog->UnicodeNameLength  = VarNameLength;
+  VarLog->VariableDataLength = VarSize;
+  CopyMem (
+     VarLog->UnicodeName,
+     VarName,
+     VarNameLength * sizeof (*VarName)
+     );
+  if (VarSize != 0) {
+    CopyMem (
+       (CHAR16 *)VarLog->UnicodeName + VarNameLength,
+       VarData,
+       VarSize
+       );
+  }
+
+  DEBUG ((EFI_D_INFO, "AuthVariableDxe: MeasureVariable (Pcr - %x, EventType - %x, ", (UINTN)7, (UINTN)EV_EFI_VARIABLE_AUTHORITY));
+  DEBUG ((EFI_D_INFO, "VariableName - %s, VendorGuid - %g)\n", VarName, VendorGuid));
+
+  Status = TpmMeasureAndLogData (
+             7,
+             EV_EFI_VARIABLE_DRIVER_CONFIG,
+             VarLog,
+             VarLogSize,
+             VarLog,
+             VarLogSize
+             );
+  FreePool (VarLog);
+  return Status;
+}
+
+/**
+  Returns the status whether get the variable success. The function retrieves
+  variable  through the UEFI Runtime Service GetVariable().  The
+  returned buffer is allocated using AllocatePool().  The caller is responsible
+  for freeing this buffer with FreePool().
+
+  This API is only invoked in boot time. It may NOT be invoked at runtime.
+
+  @param[in]  Name  The pointer to a Null-terminated Unicode string.
+  @param[in]  Guid  The pointer to an EFI_GUID structure
+  @param[out] Value The buffer point saved the variable info.
+  @param[out] Size  The buffer size of the variable.
+
+  @return EFI_OUT_OF_RESOURCES      Allocate buffer failed.
+  @return EFI_SUCCESS               Find the specified variable.
+  @return Others Errors             Return errors from call to gRT->GetVariable.
+
+**/
+EFI_STATUS
+InternalGetVariable (
+  IN CONST CHAR16    *Name,
+  IN CONST EFI_GUID  *Guid,
+  OUT VOID           **Value,
+  OUT UINTN          *Size
+  )
+{
+  EFI_STATUS  Status;
+  UINTN       BufferSize;
+
+  //
+  // Try to get the variable size.
+  //
+  BufferSize = 0;
+  *Value     = NULL;
+  if (Size != NULL) {
+    *Size  = 0;
+  }
+
+  Status = gRT->GetVariable ((CHAR16 *) Name, (EFI_GUID *) Guid, NULL, &BufferSize, *Value);
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return Status;
+  }
+
+  //
+  // Allocate buffer to get the variable.
+  //
+  *Value = AllocatePool (BufferSize);
+  ASSERT (*Value != NULL);
+  if (*Value == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Get the variable data.
+  //
+  Status = gRT->GetVariable ((CHAR16 *) Name, (EFI_GUID *) Guid, NULL, &BufferSize, *Value);
+  if (EFI_ERROR (Status)) {
+    FreePool(*Value);
+    *Value = NULL;
+  }
+
+  if (Size != NULL) {
+    *Size = BufferSize;
+  }
+
+  return Status;
+}
+
+/**
+  SecureBoot Hook for SetVariable.
+
+  @param[in] VariableName                 Name of Variable to be found.
+  @param[in] VendorGuid                   Variable vendor GUID.
+
+**/
+VOID
+EFIAPI
+SecureBootHook (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  )
+{
+  EFI_STATUS                        Status;
+  UINTN                             VariableDataSize;
+  VOID                              *VariableData;
+
+  if (!IsSecureBootPolicyVariable (VariableName, VendorGuid)) {
+    return ;
+  }
+
+  //
+  // We should NOT use Data and DataSize here,because it may include signature,
+  // or is just partial with append attributes, or is deleted.
+  // We should GetVariable again, to get full variable content.
+  //
+  Status = InternalGetVariable (
+             VariableName,
+             VendorGuid,
+             &VariableData,
+             &VariableDataSize
+             );
+  if (EFI_ERROR (Status)) {
+    VariableData     = NULL;
+    VariableDataSize = 0;
+  }
+
+  Status = MeasureVariable (
+             VariableName,
+             VendorGuid,
+             VariableData,
+             VariableDataSize
+             );
+  DEBUG ((EFI_D_INFO, "MeasureBootPolicyVariable - %r\n", Status));
+
+  if (VariableData != NULL) {
+    FreePool (VariableData);
+  }
+
+  return ;
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Reclaim.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Reclaim.c
new file mode 100644
index 0000000000..616812ccd8
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Reclaim.c
@@ -0,0 +1,161 @@
+/** @file
+  Handles non-volatile variable store garbage collection, using FTW
+  (Fault Tolerant Write) protocol.
+
+Copyright (c) 2006 - 2015, 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 "Variable.h"
+
+/**
+  Gets LBA of block and offset by given address.
+
+  This function gets the Logical Block Address (LBA) of a firmware
+  volume block containing the given address, and the offset of the
+  address on the block.
+
+  @param  Address        Address which should be contained
+                         by returned FVB handle.
+  @param  Lba            Pointer to LBA for output.
+  @param  Offset         Pointer to offset for output.
+
+  @retval EFI_SUCCESS    LBA and offset successfully returned.
+  @retval EFI_NOT_FOUND  Fail to find FVB handle by address.
+  @retval EFI_ABORTED    Fail to find valid LBA and offset.
+
+**/
+EFI_STATUS
+GetLbaAndOffsetByAddress (
+  IN  EFI_PHYSICAL_ADDRESS   Address,
+  OUT EFI_LBA                *Lba,
+  OUT UINTN                  *Offset
+  )
+{
+  EFI_STATUS                          Status;
+  EFI_PHYSICAL_ADDRESS                FvbBaseAddress;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  EFI_FIRMWARE_VOLUME_HEADER          *FwVolHeader;
+  EFI_FV_BLOCK_MAP_ENTRY              *FvbMapEntry;
+  UINT32                              LbaIndex;
+
+  Fvb     = NULL;
+  *Lba    = (EFI_LBA) (-1);
+  *Offset = 0;
+
+  //
+  // Get the proper FVB protocol.
+  //
+  Status = GetFvbInfoByAddress (Address, NULL, &Fvb);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  //
+  // Get the Base Address of FV.
+  //
+  Status = Fvb->GetPhysicalAddress (Fvb, &FvbBaseAddress);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *) ((UINTN) FvbBaseAddress);
+
+  //
+  // Get the (LBA, Offset) of Address.
+  //
+  if ((FwVolHeader->FvLength) > (FwVolHeader->HeaderLength)) {
+    //
+    // BUGBUG: Assume one FV has one type of BlockLength.
+    //
+    FvbMapEntry = &FwVolHeader->BlockMap[0];
+    for (LbaIndex = 1; LbaIndex <= FvbMapEntry->NumBlocks; LbaIndex += 1) {
+      if (Address < (FvbBaseAddress + FvbMapEntry->Length * LbaIndex)) {
+        //
+        // Found the (Lba, Offset).
+        //
+        *Lba    = LbaIndex - 1;
+        *Offset = (UINTN) (Address - (FvbBaseAddress + FvbMapEntry->Length * (LbaIndex - 1)));
+        return EFI_SUCCESS;
+     }
+    }
+  }
+
+  return EFI_ABORTED;
+}
+
+/**
+  Writes a buffer to variable storage space, in the working block.
+
+  This function writes a buffer to variable storage space into a firmware
+  volume block device. The destination is specified by parameter
+  VariableBase. Fault Tolerant Write protocol is used for writing.
+
+  @param  VariableBase   Base address of variable to write
+  @param  VariableBuffer Point to the variable data buffer.
+
+  @retval EFI_SUCCESS    The function completed successfully.
+  @retval EFI_NOT_FOUND  Fail to locate Fault Tolerant Write protocol.
+  @retval EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+FtwVariableSpace (
+  IN EFI_PHYSICAL_ADDRESS   VariableBase,
+  IN VARIABLE_STORE_HEADER  *VariableBuffer
+  )
+{
+  EFI_STATUS                         Status;
+  EFI_HANDLE                         FvbHandle;
+  EFI_LBA                            VarLba;
+  UINTN                              VarOffset;
+  UINTN                              FtwBufferSize;
+  EFI_FAULT_TOLERANT_WRITE_PROTOCOL  *FtwProtocol;
+
+  //
+  // Locate fault tolerant write protocol.
+  //
+  Status = GetFtwProtocol((VOID **) &FtwProtocol);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+  //
+  // Locate Fvb handle by address.
+  //
+  Status = GetFvbInfoByAddress (VariableBase, &FvbHandle, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+  //
+  // Get LBA and Offset by address.
+  //
+  Status = GetLbaAndOffsetByAddress (VariableBase, &VarLba, &VarOffset);
+  if (EFI_ERROR (Status)) {
+    return EFI_ABORTED;
+  }
+
+  FtwBufferSize = ((VARIABLE_STORE_HEADER *) ((UINTN) VariableBase))->Size;
+  ASSERT (FtwBufferSize == VariableBuffer->Size);
+
+  //
+  // FTW write record.
+  //
+  Status = FtwProtocol->Write (
+                          FtwProtocol,
+                          VarLba,         // LBA
+                          VarOffset,      // Offset
+                          FtwBufferSize,  // NumBytes
+                          NULL,           // PrivateData NULL
+                          FvbHandle,      // Fvb Handle
+                          (VOID *) VariableBuffer // write buffer
+                          );
+
+  return Status;
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockDxe.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockDxe.c
new file mode 100644
index 0000000000..c32eb3b1ac
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockDxe.c
@@ -0,0 +1,89 @@
+/** @file
+  TCG MOR (Memory Overwrite Request) Lock Control support (DXE version).
+
+  This module clears MemoryOverwriteRequestControlLock variable to indicate
+  MOR lock control unsupported.
+
+Copyright (c) 2016, 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 <PiDxe.h>
+#include <Guid/MemoryOverwriteControl.h>
+#include <IndustryStandard/MemoryOverwriteRequestControlLock.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include "Variable.h"
+
+extern EDKII_VARIABLE_LOCK_PROTOCOL     mVariableLock;
+
+/**
+  This service is an MOR/MorLock checker handler for the SetVariable().
+
+  @param  VariableName the name of the vendor's variable, as a
+                       Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+  @param  Attributes   Point to memory location to return the attributes of variable. If the point
+                       is NULL, the parameter would be ignored.
+  @param  DataSize     The size in bytes of Data-Buffer.
+  @param  Data         Point to the content of the variable.
+
+  @retval  EFI_SUCCESS            The MOR/MorLock check pass, and Variable driver can store the variable data.
+  @retval  EFI_INVALID_PARAMETER  The MOR/MorLock data or data size or attributes is not allowed for MOR variable.
+  @retval  EFI_ACCESS_DENIED      The MOR/MorLock is locked.
+  @retval  EFI_ALREADY_STARTED    The MorLock variable is handled inside this function.
+                                  Variable driver can just return EFI_SUCCESS.
+**/
+EFI_STATUS
+SetVariableCheckHandlerMor (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  //
+  // Just let it pass. No need provide protection for DXE version.
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialization for MOR Lock Control.
+
+  @retval EFI_SUCEESS     MorLock initialization success.
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+MorLockInit (
+  VOID
+  )
+{
+  //
+  // Always clear variable to report unsupported to OS.
+  // The reason is that the DXE version is not proper to provide *protection*.
+  // BIOS should use SMM version variable driver to provide such capability.
+  //
+  VariableServiceSetVariable (
+    MEMORY_OVERWRITE_REQUEST_CONTROL_LOCK_NAME,
+    &gEfiMemoryOverwriteRequestControlLockGuid,
+    EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+    0,
+    NULL
+    );
+
+  //
+  // Need set this variable to be read-only to prevent other module set it.
+  //
+  VariableLockRequestToLock (&mVariableLock, MEMORY_OVERWRITE_REQUEST_CONTROL_LOCK_NAME, &gEfiMemoryOverwriteRequestControlLockGuid);
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockSmm.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockSmm.c
new file mode 100644
index 0000000000..461b0fc885
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/TcgMorLockSmm.c
@@ -0,0 +1,394 @@
+/** @file
+  TCG MOR (Memory Overwrite Request) Lock Control support (SMM version).
+
+  This module initilizes MemoryOverwriteRequestControlLock variable.
+  This module adds Variable Hook and check MemoryOverwriteRequestControlLock.
+
+Copyright (c) 2016, 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 <PiDxe.h>
+#include <Guid/MemoryOverwriteControl.h>
+#include <IndustryStandard/MemoryOverwriteRequestControlLock.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include "Variable.h"
+
+typedef struct {
+  CHAR16                                 *VariableName;
+  EFI_GUID                               *VendorGuid;
+} VARIABLE_TYPE;
+
+VARIABLE_TYPE  mMorVariableType[] = {
+  {MEMORY_OVERWRITE_REQUEST_VARIABLE_NAME,      &gEfiMemoryOverwriteControlDataGuid},
+  {MEMORY_OVERWRITE_REQUEST_CONTROL_LOCK_NAME,  &gEfiMemoryOverwriteRequestControlLockGuid},
+};
+
+#define MOR_LOCK_DATA_UNLOCKED           0x0
+#define MOR_LOCK_DATA_LOCKED_WITHOUT_KEY 0x1
+#define MOR_LOCK_DATA_LOCKED_WITH_KEY    0x2
+
+#define MOR_LOCK_V1_SIZE      1
+#define MOR_LOCK_V2_KEY_SIZE  8
+
+typedef enum {
+  MorLockStateUnlocked = 0,
+  MorLockStateLocked = 1,
+} MOR_LOCK_STATE;
+
+UINT8           mMorLockKey[MOR_LOCK_V2_KEY_SIZE];
+BOOLEAN         mMorLockKeyEmpty = TRUE;
+BOOLEAN         mMorLockPassThru = FALSE;
+MOR_LOCK_STATE  mMorLockState = MorLockStateUnlocked;
+
+/**
+  Returns if this is MOR related variable.
+
+  @param  VariableName the name of the vendor's variable, it's a Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+
+  @retval  TRUE            The variable is MOR related.
+  @retval  FALSE           The variable is NOT MOR related.
+**/
+BOOLEAN
+IsAnyMorVariable (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  )
+{
+  UINTN   Index;
+
+  for (Index = 0; Index < sizeof(mMorVariableType)/sizeof(mMorVariableType[0]); Index++) {
+    if ((StrCmp (VariableName, mMorVariableType[Index].VariableName) == 0) &&
+        (CompareGuid (VendorGuid, mMorVariableType[Index].VendorGuid))) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Returns if this is MOR lock variable.
+
+  @param  VariableName the name of the vendor's variable, it's a Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+
+  @retval  TRUE            The variable is MOR lock variable.
+  @retval  FALSE           The variable is NOT MOR lock variable.
+**/
+BOOLEAN
+IsMorLockVariable (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  )
+{
+  if ((StrCmp (VariableName, MEMORY_OVERWRITE_REQUEST_CONTROL_LOCK_NAME) == 0) &&
+      (CompareGuid (VendorGuid, &gEfiMemoryOverwriteRequestControlLockGuid))) {
+    return TRUE;
+  }
+  return FALSE;
+}
+
+/**
+  Set MOR lock variable.
+
+  @param  Data         MOR Lock variable data.
+
+  @retval  EFI_SUCCESS            The firmware has successfully stored the variable and its data as
+                                  defined by the Attributes.
+  @retval  EFI_INVALID_PARAMETER  An invalid combination of attribute bits was supplied, or the
+                                  DataSize exceeds the maximum allowed.
+  @retval  EFI_INVALID_PARAMETER  VariableName is an empty Unicode string.
+  @retval  EFI_OUT_OF_RESOURCES   Not enough storage is available to hold the variable and its data.
+  @retval  EFI_DEVICE_ERROR       The variable could not be saved due to a hardware failure.
+  @retval  EFI_WRITE_PROTECTED    The variable in question is read-only.
+  @retval  EFI_WRITE_PROTECTED    The variable in question cannot be deleted.
+  @retval  EFI_SECURITY_VIOLATION The variable could not be written due to EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS
+                                  set but the AuthInfo does NOT pass the validation check carried
+                                  out by the firmware.
+  @retval  EFI_NOT_FOUND          The variable trying to be updated or deleted was not found.
+**/
+EFI_STATUS
+SetMorLockVariable (
+  IN UINT8  Data
+  )
+{
+  EFI_STATUS  Status;
+
+  mMorLockPassThru = TRUE;
+  Status = VariableServiceSetVariable (
+             MEMORY_OVERWRITE_REQUEST_CONTROL_LOCK_NAME,
+             &gEfiMemoryOverwriteRequestControlLockGuid,
+             EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS,
+             sizeof(Data),
+             &Data
+             );
+  mMorLockPassThru = FALSE;
+  return Status;
+}
+
+/**
+  This service is an MorLock checker handler for the SetVariable().
+
+  @param  VariableName the name of the vendor's variable, as a
+                       Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+  @param  Attributes   Point to memory location to return the attributes of variable. If the point
+                       is NULL, the parameter would be ignored.
+  @param  DataSize     The size in bytes of Data-Buffer.
+  @param  Data         Point to the content of the variable.
+
+  @retval  EFI_SUCCESS            The MorLock check pass, and Variable driver can store the variable data.
+  @retval  EFI_INVALID_PARAMETER  The MorLock data or data size or attributes is not allowed.
+  @retval  EFI_ACCESS_DENIED      The MorLock is locked.
+  @retval  EFI_WRITE_PROTECTED    The MorLock deletion is not allowed.
+  @retval  EFI_ALREADY_STARTED    The MorLock variable is handled inside this function.
+                                  Variable driver can just return EFI_SUCCESS.
+**/
+EFI_STATUS
+SetVariableCheckHandlerMorLock (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Basic Check
+  //
+  if (Attributes == 0 || DataSize == 0 || Data == NULL) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  if ((Attributes != (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS)) ||
+      ((DataSize != MOR_LOCK_V1_SIZE) && (DataSize != MOR_LOCK_V2_KEY_SIZE))) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Do not check if the request is passthru.
+  //
+  if (mMorLockPassThru) {
+    return EFI_SUCCESS;
+  }
+
+  if (mMorLockState == MorLockStateUnlocked) {
+    //
+    // In Unlocked State
+    //
+    if (DataSize == MOR_LOCK_V1_SIZE) {
+      //
+      // V1 - lock permenantly
+      //
+      if (*(UINT8 *)Data == MOR_LOCK_DATA_UNLOCKED) {
+        //
+        // Unlock
+        //
+        Status = SetMorLockVariable (MOR_LOCK_DATA_UNLOCKED);
+        if (!EFI_ERROR (Status)) {
+          //
+          // return EFI_ALREADY_STARTED to skip variable set.
+          //
+          return EFI_ALREADY_STARTED;
+        } else {
+          //
+          // SetVar fail
+          //
+          return Status;
+        }
+      } else if (*(UINT8 *)Data == MOR_LOCK_DATA_LOCKED_WITHOUT_KEY) {
+        //
+        // Lock without key
+        //
+        Status = SetMorLockVariable (MOR_LOCK_DATA_LOCKED_WITHOUT_KEY);
+        if (!EFI_ERROR (Status)) {
+          //
+          // Lock success
+          //
+          mMorLockState = MorLockStateLocked;
+          //
+          // return EFI_ALREADY_STARTED to skip variable set.
+          //
+          return EFI_ALREADY_STARTED;
+        } else {
+          //
+          // SetVar fail
+          //
+          return Status;
+        }
+      } else {
+        return EFI_INVALID_PARAMETER;
+      }
+    } else if (DataSize == MOR_LOCK_V2_KEY_SIZE) {
+      //
+      // V2 lock and provision the key
+      //
+
+      //
+      // Need set here because the data value on flash is different
+      //
+      Status = SetMorLockVariable (MOR_LOCK_DATA_LOCKED_WITH_KEY);
+      if (EFI_ERROR(Status)) {
+        //
+        // SetVar fail, do not provision the key
+        //
+        return Status;
+      } else {
+        //
+        // Lock success, provision the key
+        //
+        mMorLockKeyEmpty = FALSE;
+        CopyMem (mMorLockKey, Data, MOR_LOCK_V2_KEY_SIZE);
+        mMorLockState = MorLockStateLocked;
+        //
+        // return EFI_ALREADY_STARTED to skip variable set.
+        //
+        return EFI_ALREADY_STARTED;
+      }
+    } else {
+      ASSERT (FALSE);
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else {
+    //
+    // In Locked State
+    //
+    if (mMorLockKeyEmpty || (DataSize != MOR_LOCK_V2_KEY_SIZE)) {
+      return EFI_ACCESS_DENIED;
+    }
+    if ((CompareMem (Data, mMorLockKey, MOR_LOCK_V2_KEY_SIZE) == 0)) {
+      //
+      // Key match - unlock
+      //
+
+      //
+      // Need set here because the data value on flash is different
+      //
+      Status = SetMorLockVariable (MOR_LOCK_DATA_UNLOCKED);
+      if (EFI_ERROR (Status)) {
+        //
+        // SetVar fail
+        //
+        return Status;
+      } else {
+        //
+        // Unlock Success
+        //
+        mMorLockState = MorLockStateUnlocked;
+        mMorLockKeyEmpty = TRUE;
+        ZeroMem (mMorLockKey, sizeof(mMorLockKey));
+        //
+        // return EFI_ALREADY_STARTED to skip variable set.
+        //
+        return EFI_ALREADY_STARTED;
+      }
+    } else {
+      //
+      // Key mismatch - Prevent Dictionary Attack
+      //
+      mMorLockState = MorLockStateLocked;
+      mMorLockKeyEmpty = TRUE;
+      ZeroMem (mMorLockKey, sizeof(mMorLockKey));
+      return EFI_ACCESS_DENIED;
+    }
+  }
+}
+
+/**
+  This service is an MOR/MorLock checker handler for the SetVariable().
+
+  @param  VariableName the name of the vendor's variable, as a
+                       Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+  @param  Attributes   Point to memory location to return the attributes of variable. If the point
+                       is NULL, the parameter would be ignored.
+  @param  DataSize     The size in bytes of Data-Buffer.
+  @param  Data         Point to the content of the variable.
+
+  @retval  EFI_SUCCESS            The MOR/MorLock check pass, and Variable driver can store the variable data.
+  @retval  EFI_INVALID_PARAMETER  The MOR/MorLock data or data size or attributes is not allowed for MOR variable.
+  @retval  EFI_ACCESS_DENIED      The MOR/MorLock is locked.
+  @retval  EFI_ALREADY_STARTED    The MorLock variable is handled inside this function.
+                                  Variable driver can just return EFI_SUCCESS.
+**/
+EFI_STATUS
+SetVariableCheckHandlerMor (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  )
+{
+  //
+  // do not handle non-MOR variable
+  //
+  if (!IsAnyMorVariable (VariableName, VendorGuid)) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // MorLock variable
+  //
+  if (IsMorLockVariable (VariableName, VendorGuid)) {
+    return SetVariableCheckHandlerMorLock (
+             VariableName,
+             VendorGuid,
+             Attributes,
+             DataSize,
+             Data
+             );
+  }
+
+  //
+  // Mor Variable
+  //
+
+  //
+  // Basic Check
+  //
+  if ((Attributes != (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS)) ||
+      (DataSize != sizeof(UINT8)) ||
+      (Data == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+  if (mMorLockState == MorLockStateLocked) {
+    //
+    // If lock, deny access
+    //
+    return EFI_ACCESS_DENIED;
+  }
+  //
+  // grant access
+  //
+  return EFI_SUCCESS;
+}
+
+/**
+  Initialization for MOR Lock Control.
+
+  @retval EFI_SUCEESS     MorLock initialization success.
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+MorLockInit (
+  VOID
+  )
+{
+  //
+  // Set variable to report capability to OS
+  //
+  return SetMorLockVariable (0);
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VarCheck.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VarCheck.c
new file mode 100644
index 0000000000..ad56a9f5be
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VarCheck.c
@@ -0,0 +1,155 @@
+/** @file
+  Implementation functions and structures for var check protocol
+  and variable lock protocol based on VarCheckLib.
+
+Copyright (c) 2015, 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 "Variable.h"
+
+/**
+  Mark a variable that will become read-only after leaving the DXE phase of execution.
+  Write request coming from SMM environment through EFI_SMM_VARIABLE_PROTOCOL is allowed.
+
+  @param[in] This          The VARIABLE_LOCK_PROTOCOL instance.
+  @param[in] VariableName  A pointer to the variable name that will be made read-only subsequently.
+  @param[in] VendorGuid    A pointer to the vendor GUID that will be made read-only subsequently.
+
+  @retval EFI_SUCCESS           The variable specified by the VariableName and the VendorGuid was marked
+                                as pending to be read-only.
+  @retval EFI_INVALID_PARAMETER VariableName or VendorGuid is NULL.
+                                Or VariableName is an empty string.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource to hold the lock request.
+**/
+EFI_STATUS
+EFIAPI
+VariableLockRequestToLock (
+  IN CONST EDKII_VARIABLE_LOCK_PROTOCOL *This,
+  IN       CHAR16                       *VariableName,
+  IN       EFI_GUID                     *VendorGuid
+  )
+{
+  EFI_STATUS                    Status;
+  VAR_CHECK_VARIABLE_PROPERTY   Property;
+
+  AcquireLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  Status = VarCheckLibVariablePropertyGet (VariableName, VendorGuid, &Property);
+  if (!EFI_ERROR (Status)) {
+    Property.Property |= VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY;
+  } else {
+    Property.Revision = VAR_CHECK_VARIABLE_PROPERTY_REVISION;
+    Property.Property = VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY;
+    Property.Attributes = 0;
+    Property.MinSize = 1;
+    Property.MaxSize = MAX_UINTN;
+  }
+  Status = VarCheckLibVariablePropertySet (VariableName, VendorGuid, &Property);
+
+  DEBUG ((EFI_D_INFO, "[Variable] Lock: %g:%s %r\n", VendorGuid, VariableName, Status));
+
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  return Status;
+}
+
+/**
+  Register SetVariable check handler.
+
+  @param[in] Handler            Pointer to check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckRegisterSetVariableCheckHandler (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  )
+{
+  EFI_STATUS    Status;
+
+  AcquireLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  Status = VarCheckLibRegisterSetVariableCheckHandler (Handler);
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  return Status;
+}
+
+/**
+  Variable property set.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertySet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  )
+{
+  EFI_STATUS    Status;
+
+  AcquireLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  Status = VarCheckLibVariablePropertySet (Name, Guid, VariableProperty);
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  return Status;
+}
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertyGet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  )
+{
+  EFI_STATUS    Status;
+
+  AcquireLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  Status = VarCheckLibVariablePropertyGet (Name, Guid, VariableProperty);
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.c
new file mode 100644
index 0000000000..43360ee6cd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.c
@@ -0,0 +1,4210 @@
+/** @file
+  The common variable operation routines shared by DXE_RUNTIME variable
+  module and DXE_SMM variable module.
+
+  Caution: This module requires additional review when modified.
+  This driver will have external input - variable data. They may be input in SMM mode.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  VariableServiceGetNextVariableName () and VariableServiceQueryVariableInfo() are external API.
+  They need check input parameter.
+
+  VariableServiceGetVariable() and VariableServiceSetVariable() are external API
+  to receive datasize and data buffer. The size should be checked carefully.
+
+  VariableServiceSetVariable() should also check authenticate data to avoid buffer overflow,
+  integer overflow. It should also check attribute to avoid authentication bypass.
+
+Copyright (c) 2006 - 2016, 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 "Variable.h"
+
+VARIABLE_MODULE_GLOBAL  *mVariableModuleGlobal;
+
+///
+/// Define a memory cache that improves the search performance for a variable.
+///
+VARIABLE_STORE_HEADER  *mNvVariableCache      = NULL;
+
+///
+/// The memory entry used for variable statistics data.
+///
+VARIABLE_INFO_ENTRY    *gVariableInfo         = NULL;
+
+///
+/// The flag to indicate whether the platform has left the DXE phase of execution.
+///
+BOOLEAN                mEndOfDxe              = FALSE;
+
+///
+/// It indicates the var check request source.
+/// In the implementation, DXE is regarded as untrusted, and SMM is trusted.
+///
+VAR_CHECK_REQUEST_SOURCE mRequestSource       = VarCheckFromUntrusted;
+
+//
+// It will record the current boot error flag before EndOfDxe.
+//
+VAR_ERROR_FLAG         mCurrentBootVarErrFlag = VAR_ERROR_FLAG_NO_ERROR;
+
+VARIABLE_ENTRY_PROPERTY mVariableEntryProperty[] = {
+  {
+    &gEdkiiVarErrorFlagGuid,
+    VAR_ERROR_FLAG_NAME,
+    {
+      VAR_CHECK_VARIABLE_PROPERTY_REVISION,
+      VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY,
+      VARIABLE_ATTRIBUTE_NV_BS_RT,
+      sizeof (VAR_ERROR_FLAG),
+      sizeof (VAR_ERROR_FLAG)
+    }
+  },
+};
+
+AUTH_VAR_LIB_CONTEXT_IN mAuthContextIn = {
+  AUTH_VAR_LIB_CONTEXT_IN_STRUCT_VERSION,
+  //
+  // StructSize, TO BE FILLED
+  //
+  0,
+  //
+  // MaxAuthVariableSize, TO BE FILLED
+  //
+  0,
+  VariableExLibFindVariable,
+  VariableExLibFindNextVariable,
+  VariableExLibUpdateVariable,
+  VariableExLibGetScratchBuffer,
+  VariableExLibCheckRemainingSpaceForConsistency,
+  VariableExLibAtRuntime,
+};
+
+AUTH_VAR_LIB_CONTEXT_OUT mAuthContextOut;
+
+/**
+
+  SecureBoot Hook for auth variable update.
+
+  @param[in] VariableName                 Name of Variable to be found.
+  @param[in] VendorGuid                   Variable vendor GUID.
+**/
+VOID
+EFIAPI
+SecureBootHook (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  );
+
+/**
+  Initialization for MOR Lock Control.
+
+  @retval EFI_SUCEESS     MorLock initialization success.
+  @return Others          Some error occurs.
+**/
+EFI_STATUS
+MorLockInit (
+  VOID
+  );
+
+/**
+  This service is an MOR/MorLock checker handler for the SetVariable().
+
+  @param  VariableName the name of the vendor's variable, as a
+                       Null-Terminated Unicode String
+  @param  VendorGuid   Unify identifier for vendor.
+  @param  Attributes   Point to memory location to return the attributes of variable. If the point
+                       is NULL, the parameter would be ignored.
+  @param  DataSize     The size in bytes of Data-Buffer.
+  @param  Data         Point to the content of the variable.
+
+  @retval  EFI_SUCCESS            The MOR/MorLock check pass, and Variable driver can store the variable data.
+  @retval  EFI_INVALID_PARAMETER  The MOR/MorLock data or data size or attributes is not allowed for MOR variable.
+  @retval  EFI_ACCESS_DENIED      The MOR/MorLock is locked.
+  @retval  EFI_ALREADY_STARTED    The MorLock variable is handled inside this function.
+                                  Variable driver can just return EFI_SUCCESS.
+**/
+EFI_STATUS
+SetVariableCheckHandlerMor (
+  IN CHAR16     *VariableName,
+  IN EFI_GUID   *VendorGuid,
+  IN UINT32     Attributes,
+  IN UINTN      DataSize,
+  IN VOID       *Data
+  );
+
+/**
+  Routine used to track statistical information about variable usage.
+  The data is stored in the EFI system table so it can be accessed later.
+  VariableInfo.efi can dump out the table. Only Boot Services variable
+  accesses are tracked by this code. The PcdVariableCollectStatistics
+  build flag controls if this feature is enabled.
+
+  A read that hits in the cache will have Read and Cache true for
+  the transaction. Data is allocated by this routine, but never
+  freed.
+
+  @param[in] VariableName   Name of the Variable to track.
+  @param[in] VendorGuid     Guid of the Variable to track.
+  @param[in] Volatile       TRUE if volatile FALSE if non-volatile.
+  @param[in] Read           TRUE if GetVariable() was called.
+  @param[in] Write          TRUE if SetVariable() was called.
+  @param[in] Delete         TRUE if deleted via SetVariable().
+  @param[in] Cache          TRUE for a cache hit.
+
+**/
+VOID
+UpdateVariableInfo (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  IN  BOOLEAN                 Volatile,
+  IN  BOOLEAN                 Read,
+  IN  BOOLEAN                 Write,
+  IN  BOOLEAN                 Delete,
+  IN  BOOLEAN                 Cache
+  )
+{
+  VARIABLE_INFO_ENTRY   *Entry;
+
+  if (FeaturePcdGet (PcdVariableCollectStatistics)) {
+
+    if (AtRuntime ()) {
+      // Don't collect statistics at runtime.
+      return;
+    }
+
+    if (gVariableInfo == NULL) {
+      //
+      // On the first call allocate a entry and place a pointer to it in
+      // the EFI System Table.
+      //
+      gVariableInfo = AllocateZeroPool (sizeof (VARIABLE_INFO_ENTRY));
+      ASSERT (gVariableInfo != NULL);
+
+      CopyGuid (&gVariableInfo->VendorGuid, VendorGuid);
+      gVariableInfo->Name = AllocateZeroPool (StrSize (VariableName));
+      ASSERT (gVariableInfo->Name != NULL);
+      StrCpyS (gVariableInfo->Name, StrSize(VariableName)/sizeof(CHAR16), VariableName);
+      gVariableInfo->Volatile = Volatile;
+    }
+
+
+    for (Entry = gVariableInfo; Entry != NULL; Entry = Entry->Next) {
+      if (CompareGuid (VendorGuid, &Entry->VendorGuid)) {
+        if (StrCmp (VariableName, Entry->Name) == 0) {
+          if (Read) {
+            Entry->ReadCount++;
+          }
+          if (Write) {
+            Entry->WriteCount++;
+          }
+          if (Delete) {
+            Entry->DeleteCount++;
+          }
+          if (Cache) {
+            Entry->CacheCount++;
+          }
+
+          return;
+        }
+      }
+
+      if (Entry->Next == NULL) {
+        //
+        // If the entry is not in the table add it.
+        // Next iteration of the loop will fill in the data.
+        //
+        Entry->Next = AllocateZeroPool (sizeof (VARIABLE_INFO_ENTRY));
+        ASSERT (Entry->Next != NULL);
+
+        CopyGuid (&Entry->Next->VendorGuid, VendorGuid);
+        Entry->Next->Name = AllocateZeroPool (StrSize (VariableName));
+        ASSERT (Entry->Next->Name != NULL);
+        StrCpyS (Entry->Next->Name, StrSize(VariableName)/sizeof(CHAR16), VariableName);
+        Entry->Next->Volatile = Volatile;
+      }
+
+    }
+  }
+}
+
+
+/**
+
+  This code checks if variable header is valid or not.
+
+  @param Variable           Pointer to the Variable Header.
+  @param VariableStoreEnd   Pointer to the Variable Store End.
+
+  @retval TRUE              Variable header is valid.
+  @retval FALSE             Variable header is not valid.
+
+**/
+BOOLEAN
+IsValidVariableHeader (
+  IN  VARIABLE_HEADER       *Variable,
+  IN  VARIABLE_HEADER       *VariableStoreEnd
+  )
+{
+  if ((Variable == NULL) || (Variable >= VariableStoreEnd) || (Variable->StartId != VARIABLE_DATA)) {
+    //
+    // Variable is NULL or has reached the end of variable store,
+    // or the StartId is not correct.
+    //
+    return FALSE;
+  }
+
+  return TRUE;
+}
+
+
+/**
+
+  This function writes data to the FWH at the correct LBA even if the LBAs
+  are fragmented.
+
+  @param Global                  Pointer to VARAIBLE_GLOBAL structure.
+  @param Volatile                Point out the Variable is Volatile or Non-Volatile.
+  @param SetByIndex              TRUE if target pointer is given as index.
+                                 FALSE if target pointer is absolute.
+  @param Fvb                     Pointer to the writable FVB protocol.
+  @param DataPtrIndex            Pointer to the Data from the end of VARIABLE_STORE_HEADER
+                                 structure.
+  @param DataSize                Size of data to be written.
+  @param Buffer                  Pointer to the buffer from which data is written.
+
+  @retval EFI_INVALID_PARAMETER  Parameters not valid.
+  @retval EFI_SUCCESS            Variable store successfully updated.
+
+**/
+EFI_STATUS
+UpdateVariableStore (
+  IN  VARIABLE_GLOBAL                     *Global,
+  IN  BOOLEAN                             Volatile,
+  IN  BOOLEAN                             SetByIndex,
+  IN  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb,
+  IN  UINTN                               DataPtrIndex,
+  IN  UINT32                              DataSize,
+  IN  UINT8                               *Buffer
+  )
+{
+  EFI_FV_BLOCK_MAP_ENTRY      *PtrBlockMapEntry;
+  UINTN                       BlockIndex2;
+  UINTN                       LinearOffset;
+  UINTN                       CurrWriteSize;
+  UINTN                       CurrWritePtr;
+  UINT8                       *CurrBuffer;
+  EFI_LBA                     LbaNumber;
+  UINTN                       Size;
+  EFI_FIRMWARE_VOLUME_HEADER  *FwVolHeader;
+  VARIABLE_STORE_HEADER       *VolatileBase;
+  EFI_PHYSICAL_ADDRESS        FvVolHdr;
+  EFI_PHYSICAL_ADDRESS        DataPtr;
+  EFI_STATUS                  Status;
+
+  FwVolHeader = NULL;
+  DataPtr     = DataPtrIndex;
+
+  //
+  // Check if the Data is Volatile.
+  //
+  if (!Volatile) {
+    if (Fvb == NULL) {
+      return EFI_INVALID_PARAMETER;
+    }
+    Status = Fvb->GetPhysicalAddress(Fvb, &FvVolHdr);
+    ASSERT_EFI_ERROR (Status);
+
+    FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER *) ((UINTN) FvVolHdr);
+    //
+    // Data Pointer should point to the actual Address where data is to be
+    // written.
+    //
+    if (SetByIndex) {
+      DataPtr += mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase;
+    }
+
+    if ((DataPtr + DataSize) >= ((EFI_PHYSICAL_ADDRESS) (UINTN) ((UINT8 *) FwVolHeader + FwVolHeader->FvLength))) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    // Data Pointer should point to the actual Address where data is to be
+    // written.
+    //
+    VolatileBase = (VARIABLE_STORE_HEADER *) ((UINTN) mVariableModuleGlobal->VariableGlobal.VolatileVariableBase);
+    if (SetByIndex) {
+      DataPtr += mVariableModuleGlobal->VariableGlobal.VolatileVariableBase;
+    }
+
+    if ((DataPtr + DataSize) >= ((UINTN) ((UINT8 *) VolatileBase + VolatileBase->Size))) {
+      return EFI_INVALID_PARAMETER;
+    }
+
+    //
+    // If Volatile Variable just do a simple mem copy.
+    //
+    CopyMem ((UINT8 *)(UINTN)DataPtr, Buffer, DataSize);
+    return EFI_SUCCESS;
+  }
+
+  //
+  // If we are here we are dealing with Non-Volatile Variables.
+  //
+  LinearOffset  = (UINTN) FwVolHeader;
+  CurrWritePtr  = (UINTN) DataPtr;
+  CurrWriteSize = DataSize;
+  CurrBuffer    = Buffer;
+  LbaNumber     = 0;
+
+  if (CurrWritePtr < LinearOffset) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (PtrBlockMapEntry = FwVolHeader->BlockMap; PtrBlockMapEntry->NumBlocks != 0; PtrBlockMapEntry++) {
+    for (BlockIndex2 = 0; BlockIndex2 < PtrBlockMapEntry->NumBlocks; BlockIndex2++) {
+      //
+      // Check to see if the Variable Writes are spanning through multiple
+      // blocks.
+      //
+      if ((CurrWritePtr >= LinearOffset) && (CurrWritePtr < LinearOffset + PtrBlockMapEntry->Length)) {
+        if ((CurrWritePtr + CurrWriteSize) <= (LinearOffset + PtrBlockMapEntry->Length)) {
+          Status = Fvb->Write (
+                    Fvb,
+                    LbaNumber,
+                    (UINTN) (CurrWritePtr - LinearOffset),
+                    &CurrWriteSize,
+                    CurrBuffer
+                    );
+          return Status;
+        } else {
+          Size = (UINT32) (LinearOffset + PtrBlockMapEntry->Length - CurrWritePtr);
+          Status = Fvb->Write (
+                    Fvb,
+                    LbaNumber,
+                    (UINTN) (CurrWritePtr - LinearOffset),
+                    &Size,
+                    CurrBuffer
+                    );
+          if (EFI_ERROR (Status)) {
+            return Status;
+          }
+
+          CurrWritePtr  = LinearOffset + PtrBlockMapEntry->Length;
+          CurrBuffer    = CurrBuffer + Size;
+          CurrWriteSize = CurrWriteSize - Size;
+        }
+      }
+
+      LinearOffset += PtrBlockMapEntry->Length;
+      LbaNumber++;
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+
+  This code gets the current status of Variable Store.
+
+  @param VarStoreHeader  Pointer to the Variable Store Header.
+
+  @retval EfiRaw         Variable store status is raw.
+  @retval EfiValid       Variable store status is valid.
+  @retval EfiInvalid     Variable store status is invalid.
+
+**/
+VARIABLE_STORE_STATUS
+GetVariableStoreStatus (
+  IN VARIABLE_STORE_HEADER *VarStoreHeader
+  )
+{
+  if ((CompareGuid (&VarStoreHeader->Signature, &gEfiAuthenticatedVariableGuid) ||
+       CompareGuid (&VarStoreHeader->Signature, &gEfiVariableGuid)) &&
+      VarStoreHeader->Format == VARIABLE_STORE_FORMATTED &&
+      VarStoreHeader->State == VARIABLE_STORE_HEALTHY
+      ) {
+
+    return EfiValid;
+  } else if (((UINT32 *)(&VarStoreHeader->Signature))[0] == 0xffffffff &&
+             ((UINT32 *)(&VarStoreHeader->Signature))[1] == 0xffffffff &&
+             ((UINT32 *)(&VarStoreHeader->Signature))[2] == 0xffffffff &&
+             ((UINT32 *)(&VarStoreHeader->Signature))[3] == 0xffffffff &&
+             VarStoreHeader->Size == 0xffffffff &&
+             VarStoreHeader->Format == 0xff &&
+             VarStoreHeader->State == 0xff
+          ) {
+
+    return EfiRaw;
+  } else {
+    return EfiInvalid;
+  }
+}
+
+/**
+  This code gets the size of variable header.
+
+  @return Size of variable header in bytes in type UINTN.
+
+**/
+UINTN
+GetVariableHeaderSize (
+  VOID
+  )
+{
+  UINTN Value;
+
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    Value = sizeof (AUTHENTICATED_VARIABLE_HEADER);
+  } else {
+    Value = sizeof (VARIABLE_HEADER);
+  }
+
+  return Value;
+}
+
+/**
+
+  This code gets the size of name of variable.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return UINTN          Size of variable in bytes.
+
+**/
+UINTN
+NameSizeOfVariable (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    if (AuthVariable->State == (UINT8) (-1) ||
+       AuthVariable->DataSize == (UINT32) (-1) ||
+       AuthVariable->NameSize == (UINT32) (-1) ||
+       AuthVariable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) AuthVariable->NameSize;
+  } else {
+    if (Variable->State == (UINT8) (-1) ||
+        Variable->DataSize == (UINT32) (-1) ||
+        Variable->NameSize == (UINT32) (-1) ||
+        Variable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) Variable->NameSize;
+  }
+}
+
+/**
+  This code sets the size of name of variable.
+
+  @param[in] Variable   Pointer to the Variable Header.
+  @param[in] NameSize   Name size to set.
+
+**/
+VOID
+SetNameSizeOfVariable (
+  IN VARIABLE_HEADER    *Variable,
+  IN UINTN              NameSize
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    AuthVariable->NameSize = (UINT32) NameSize;
+  } else {
+    Variable->NameSize = (UINT32) NameSize;
+  }
+}
+
+/**
+
+  This code gets the size of variable data.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Size of variable in bytes.
+
+**/
+UINTN
+DataSizeOfVariable (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    if (AuthVariable->State == (UINT8) (-1) ||
+       AuthVariable->DataSize == (UINT32) (-1) ||
+       AuthVariable->NameSize == (UINT32) (-1) ||
+       AuthVariable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) AuthVariable->DataSize;
+  } else {
+    if (Variable->State == (UINT8) (-1) ||
+        Variable->DataSize == (UINT32) (-1) ||
+        Variable->NameSize == (UINT32) (-1) ||
+        Variable->Attributes == (UINT32) (-1)) {
+      return 0;
+    }
+    return (UINTN) Variable->DataSize;
+  }
+}
+
+/**
+  This code sets the size of variable data.
+
+  @param[in] Variable   Pointer to the Variable Header.
+  @param[in] DataSize   Data size to set.
+
+**/
+VOID
+SetDataSizeOfVariable (
+  IN VARIABLE_HEADER    *Variable,
+  IN UINTN              DataSize
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    AuthVariable->DataSize = (UINT32) DataSize;
+  } else {
+    Variable->DataSize = (UINT32) DataSize;
+  }
+}
+
+/**
+
+  This code gets the pointer to the variable name.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Pointer to Variable Name which is Unicode encoding.
+
+**/
+CHAR16 *
+GetVariableNamePtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  return (CHAR16 *) ((UINTN) Variable + GetVariableHeaderSize ());
+}
+
+/**
+  This code gets the pointer to the variable guid.
+
+  @param Variable   Pointer to the Variable Header.
+
+  @return A EFI_GUID* pointer to Vendor Guid.
+
+**/
+EFI_GUID *
+GetVendorGuidPtr (
+  IN VARIABLE_HEADER    *Variable
+  )
+{
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    return &AuthVariable->VendorGuid;
+  } else {
+    return &Variable->VendorGuid;
+  }
+}
+
+/**
+
+  This code gets the pointer to the variable data.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Pointer to Variable Data.
+
+**/
+UINT8 *
+GetVariableDataPtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  UINTN Value;
+
+  //
+  // Be careful about pad size for alignment.
+  //
+  Value =  (UINTN) GetVariableNamePtr (Variable);
+  Value += NameSizeOfVariable (Variable);
+  Value += GET_PAD_SIZE (NameSizeOfVariable (Variable));
+
+  return (UINT8 *) Value;
+}
+
+/**
+  This code gets the variable data offset related to variable header.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Variable Data offset.
+
+**/
+UINTN
+GetVariableDataOffset (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  UINTN Value;
+
+  //
+  // Be careful about pad size for alignment
+  //
+  Value = GetVariableHeaderSize ();
+  Value += NameSizeOfVariable (Variable);
+  Value += GET_PAD_SIZE (NameSizeOfVariable (Variable));
+
+  return Value;
+}
+
+/**
+
+  This code gets the pointer to the next variable header.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Pointer to next variable header.
+
+**/
+VARIABLE_HEADER *
+GetNextVariablePtr (
+  IN  VARIABLE_HEADER   *Variable
+  )
+{
+  UINTN Value;
+
+  Value =  (UINTN) GetVariableDataPtr (Variable);
+  Value += DataSizeOfVariable (Variable);
+  Value += GET_PAD_SIZE (DataSizeOfVariable (Variable));
+
+  //
+  // Be careful about pad size for alignment.
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN (Value);
+}
+
+/**
+
+  Gets the pointer to the first variable header in given variable store area.
+
+  @param VarStoreHeader  Pointer to the Variable Store Header.
+
+  @return Pointer to the first variable header.
+
+**/
+VARIABLE_HEADER *
+GetStartPointer (
+  IN VARIABLE_STORE_HEADER       *VarStoreHeader
+  )
+{
+  //
+  // The end of variable store.
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN (VarStoreHeader + 1);
+}
+
+/**
+
+  Gets the pointer to the end of the variable storage area.
+
+  This function gets pointer to the end of the variable storage
+  area, according to the input variable store header.
+
+  @param VarStoreHeader  Pointer to the Variable Store Header.
+
+  @return Pointer to the end of the variable storage area.
+
+**/
+VARIABLE_HEADER *
+GetEndPointer (
+  IN VARIABLE_STORE_HEADER       *VarStoreHeader
+  )
+{
+  //
+  // The end of variable store
+  //
+  return (VARIABLE_HEADER *) HEADER_ALIGN ((UINTN) VarStoreHeader + VarStoreHeader->Size);
+}
+
+/**
+  Record variable error flag.
+
+  @param[in] Flag               Variable error flag to record.
+  @param[in] VariableName       Name of variable.
+  @param[in] VendorGuid         Guid of variable.
+  @param[in] Attributes         Attributes of the variable.
+  @param[in] VariableSize       Size of the variable.
+
+**/
+VOID
+RecordVarErrorFlag (
+  IN VAR_ERROR_FLAG         Flag,
+  IN CHAR16                 *VariableName,
+  IN EFI_GUID               *VendorGuid,
+  IN UINT32                 Attributes,
+  IN UINTN                  VariableSize
+  )
+{
+  EFI_STATUS                Status;
+  VARIABLE_POINTER_TRACK    Variable;
+  VAR_ERROR_FLAG            *VarErrFlag;
+  VAR_ERROR_FLAG            TempFlag;
+
+  DEBUG_CODE (
+    DEBUG ((EFI_D_ERROR, "RecordVarErrorFlag (0x%02x) %s:%g - 0x%08x - 0x%x\n", Flag, VariableName, VendorGuid, Attributes, VariableSize));
+    if (Flag == VAR_ERROR_FLAG_SYSTEM_ERROR) {
+      if (AtRuntime ()) {
+        DEBUG ((EFI_D_ERROR, "CommonRuntimeVariableSpace = 0x%x - CommonVariableTotalSize = 0x%x\n", mVariableModuleGlobal->CommonRuntimeVariableSpace, mVariableModuleGlobal->CommonVariableTotalSize));
+      } else {
+        DEBUG ((EFI_D_ERROR, "CommonVariableSpace = 0x%x - CommonVariableTotalSize = 0x%x\n", mVariableModuleGlobal->CommonVariableSpace, mVariableModuleGlobal->CommonVariableTotalSize));
+      }
+    } else {
+      DEBUG ((EFI_D_ERROR, "CommonMaxUserVariableSpace = 0x%x - CommonUserVariableTotalSize = 0x%x\n", mVariableModuleGlobal->CommonMaxUserVariableSpace, mVariableModuleGlobal->CommonUserVariableTotalSize));
+    }
+  );
+
+  if (!mEndOfDxe) {
+    //
+    // Before EndOfDxe, just record the current boot variable error flag to local variable,
+    // and leave the variable error flag in NV flash as the last boot variable error flag.
+    // After EndOfDxe in InitializeVarErrorFlag (), the variable error flag in NV flash
+    // will be initialized to this local current boot variable error flag.
+    //
+    mCurrentBootVarErrFlag &= Flag;
+    return;
+  }
+
+  //
+  // Record error flag (it should have be initialized).
+  //
+  Status = FindVariable (
+             VAR_ERROR_FLAG_NAME,
+             &gEdkiiVarErrorFlagGuid,
+             &Variable,
+             &mVariableModuleGlobal->VariableGlobal,
+             FALSE
+             );
+  if (!EFI_ERROR (Status)) {
+    VarErrFlag = (VAR_ERROR_FLAG *) GetVariableDataPtr (Variable.CurrPtr);
+    TempFlag = *VarErrFlag;
+    TempFlag &= Flag;
+    if (TempFlag == *VarErrFlag) {
+      return;
+    }
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               FALSE,
+               FALSE,
+               mVariableModuleGlobal->FvbInstance,
+               (UINTN) VarErrFlag - (UINTN) mNvVariableCache + (UINTN) mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase,
+               sizeof (TempFlag),
+               &TempFlag
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update the data in NV cache.
+      //
+      *VarErrFlag = Flag;
+    }
+  }
+}
+
+/**
+  Initialize variable error flag.
+
+  Before EndOfDxe, the variable indicates the last boot variable error flag,
+  then it means the last boot variable error flag must be got before EndOfDxe.
+  After EndOfDxe, the variable indicates the current boot variable error flag,
+  then it means the current boot variable error flag must be got after EndOfDxe.
+
+**/
+VOID
+InitializeVarErrorFlag (
+  VOID
+  )
+{
+  EFI_STATUS                Status;
+  VARIABLE_POINTER_TRACK    Variable;
+  VAR_ERROR_FLAG            Flag;
+  VAR_ERROR_FLAG            VarErrFlag;
+
+  if (!mEndOfDxe) {
+    return;
+  }
+
+  Flag = mCurrentBootVarErrFlag;
+  DEBUG ((EFI_D_INFO, "Initialize variable error flag (%02x)\n", Flag));
+
+  Status = FindVariable (
+             VAR_ERROR_FLAG_NAME,
+             &gEdkiiVarErrorFlagGuid,
+             &Variable,
+             &mVariableModuleGlobal->VariableGlobal,
+             FALSE
+             );
+  if (!EFI_ERROR (Status)) {
+    VarErrFlag = *((VAR_ERROR_FLAG *) GetVariableDataPtr (Variable.CurrPtr));
+    if (VarErrFlag == Flag) {
+      return;
+    }
+  }
+
+  UpdateVariable (
+    VAR_ERROR_FLAG_NAME,
+    &gEdkiiVarErrorFlagGuid,
+    &Flag,
+    sizeof (Flag),
+    VARIABLE_ATTRIBUTE_NV_BS_RT,
+    0,
+    0,
+    &Variable,
+    NULL
+    );
+}
+
+/**
+  Is user variable?
+
+  @param[in] Variable   Pointer to variable header.
+
+  @retval TRUE          User variable.
+  @retval FALSE         System variable.
+
+**/
+BOOLEAN
+IsUserVariable (
+  IN VARIABLE_HEADER    *Variable
+  )
+{
+  VAR_CHECK_VARIABLE_PROPERTY   Property;
+
+  //
+  // Only after End Of Dxe, the variables belong to system variable are fixed.
+  // If PcdMaxUserNvStorageVariableSize is 0, it means user variable share the same NV storage with system variable,
+  // then no need to check if the variable is user variable or not specially.
+  //
+  if (mEndOfDxe && (mVariableModuleGlobal->CommonMaxUserVariableSpace != mVariableModuleGlobal->CommonVariableSpace)) {
+    if (VarCheckLibVariablePropertyGet (GetVariableNamePtr (Variable), GetVendorGuidPtr (Variable), &Property) == EFI_NOT_FOUND) {
+      return TRUE;
+    }
+  }
+  return FALSE;
+}
+
+/**
+  Calculate common user variable total size.
+
+**/
+VOID
+CalculateCommonUserVariableTotalSize (
+  VOID
+  )
+{
+  VARIABLE_HEADER               *Variable;
+  VARIABLE_HEADER               *NextVariable;
+  UINTN                         VariableSize;
+  VAR_CHECK_VARIABLE_PROPERTY   Property;
+
+  //
+  // Only after End Of Dxe, the variables belong to system variable are fixed.
+  // If PcdMaxUserNvStorageVariableSize is 0, it means user variable share the same NV storage with system variable,
+  // then no need to calculate the common user variable total size specially.
+  //
+  if (mEndOfDxe && (mVariableModuleGlobal->CommonMaxUserVariableSpace != mVariableModuleGlobal->CommonVariableSpace)) {
+    Variable = GetStartPointer (mNvVariableCache);
+    while (IsValidVariableHeader (Variable, GetEndPointer (mNvVariableCache))) {
+      NextVariable = GetNextVariablePtr (Variable);
+      VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+      if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+        if (VarCheckLibVariablePropertyGet (GetVariableNamePtr (Variable), GetVendorGuidPtr (Variable), &Property) == EFI_NOT_FOUND) {
+          //
+          // No property, it is user variable.
+          //
+          mVariableModuleGlobal->CommonUserVariableTotalSize += VariableSize;
+        }
+      }
+
+      Variable = NextVariable;
+    }
+  }
+}
+
+/**
+  Initialize variable quota.
+
+**/
+VOID
+InitializeVariableQuota (
+  VOID
+  )
+{
+  if (!mEndOfDxe) {
+    return;
+  }
+
+  InitializeVarErrorFlag ();
+  CalculateCommonUserVariableTotalSize ();
+}
+
+/**
+
+  Variable store garbage collection and reclaim operation.
+
+  @param[in]      VariableBase            Base address of variable store.
+  @param[out]     LastVariableOffset      Offset of last variable.
+  @param[in]      IsVolatile              The variable store is volatile or not;
+                                          if it is non-volatile, need FTW.
+  @param[in, out] UpdatingPtrTrack        Pointer to updating variable pointer track structure.
+  @param[in]      NewVariable             Pointer to new variable.
+  @param[in]      NewVariableSize         New variable size.
+
+  @return EFI_SUCCESS                  Reclaim operation has finished successfully.
+  @return EFI_OUT_OF_RESOURCES         No enough memory resources or variable space.
+  @return Others                       Unexpect error happened during reclaim operation.
+
+**/
+EFI_STATUS
+Reclaim (
+  IN     EFI_PHYSICAL_ADDRESS         VariableBase,
+  OUT    UINTN                        *LastVariableOffset,
+  IN     BOOLEAN                      IsVolatile,
+  IN OUT VARIABLE_POINTER_TRACK       *UpdatingPtrTrack,
+  IN     VARIABLE_HEADER              *NewVariable,
+  IN     UINTN                        NewVariableSize
+  )
+{
+  VARIABLE_HEADER       *Variable;
+  VARIABLE_HEADER       *AddedVariable;
+  VARIABLE_HEADER       *NextVariable;
+  VARIABLE_HEADER       *NextAddedVariable;
+  VARIABLE_STORE_HEADER *VariableStoreHeader;
+  UINT8                 *ValidBuffer;
+  UINTN                 MaximumBufferSize;
+  UINTN                 VariableSize;
+  UINTN                 NameSize;
+  UINT8                 *CurrPtr;
+  VOID                  *Point0;
+  VOID                  *Point1;
+  BOOLEAN               FoundAdded;
+  EFI_STATUS            Status;
+  UINTN                 CommonVariableTotalSize;
+  UINTN                 CommonUserVariableTotalSize;
+  UINTN                 HwErrVariableTotalSize;
+  VARIABLE_HEADER       *UpdatingVariable;
+  VARIABLE_HEADER       *UpdatingInDeletedTransition;
+
+  UpdatingVariable = NULL;
+  UpdatingInDeletedTransition = NULL;
+  if (UpdatingPtrTrack != NULL) {
+    UpdatingVariable = UpdatingPtrTrack->CurrPtr;
+    UpdatingInDeletedTransition = UpdatingPtrTrack->InDeletedTransitionPtr;
+  }
+
+  VariableStoreHeader = (VARIABLE_STORE_HEADER *) ((UINTN) VariableBase);
+
+  CommonVariableTotalSize = 0;
+  CommonUserVariableTotalSize = 0;
+  HwErrVariableTotalSize  = 0;
+
+  if (IsVolatile) {
+    //
+    // Start Pointers for the variable.
+    //
+    Variable          = GetStartPointer (VariableStoreHeader);
+    MaximumBufferSize = sizeof (VARIABLE_STORE_HEADER);
+
+    while (IsValidVariableHeader (Variable, GetEndPointer (VariableStoreHeader))) {
+      NextVariable = GetNextVariablePtr (Variable);
+      if ((Variable->State == VAR_ADDED || Variable->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) &&
+          Variable != UpdatingVariable &&
+          Variable != UpdatingInDeletedTransition
+         ) {
+        VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+        MaximumBufferSize += VariableSize;
+      }
+
+      Variable = NextVariable;
+    }
+
+    if (NewVariable != NULL) {
+      //
+      // Add the new variable size.
+      //
+      MaximumBufferSize += NewVariableSize;
+    }
+
+    //
+    // Reserve the 1 Bytes with Oxff to identify the
+    // end of the variable buffer.
+    //
+    MaximumBufferSize += 1;
+    ValidBuffer = AllocatePool (MaximumBufferSize);
+    if (ValidBuffer == NULL) {
+      return EFI_OUT_OF_RESOURCES;
+    }
+  } else {
+    //
+    // For NV variable reclaim, don't allocate pool here and just use mNvVariableCache
+    // as the buffer to reduce SMRAM consumption for SMM variable driver.
+    //
+    MaximumBufferSize = mNvVariableCache->Size;
+    ValidBuffer = (UINT8 *) mNvVariableCache;
+  }
+
+  SetMem (ValidBuffer, MaximumBufferSize, 0xff);
+
+  //
+  // Copy variable store header.
+  //
+  CopyMem (ValidBuffer, VariableStoreHeader, sizeof (VARIABLE_STORE_HEADER));
+  CurrPtr = (UINT8 *) GetStartPointer ((VARIABLE_STORE_HEADER *) ValidBuffer);
+
+  //
+  // Reinstall all ADDED variables as long as they are not identical to Updating Variable.
+  //
+  Variable = GetStartPointer (VariableStoreHeader);
+  while (IsValidVariableHeader (Variable, GetEndPointer (VariableStoreHeader))) {
+    NextVariable = GetNextVariablePtr (Variable);
+    if (Variable != UpdatingVariable && Variable->State == VAR_ADDED) {
+      VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+      CopyMem (CurrPtr, (UINT8 *) Variable, VariableSize);
+      CurrPtr += VariableSize;
+      if ((!IsVolatile) && ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+        HwErrVariableTotalSize += VariableSize;
+      } else if ((!IsVolatile) && ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+        CommonVariableTotalSize += VariableSize;
+        if (IsUserVariable (Variable)) {
+          CommonUserVariableTotalSize += VariableSize;
+        }
+      }
+    }
+    Variable = NextVariable;
+  }
+
+  //
+  // Reinstall all in delete transition variables.
+  //
+  Variable = GetStartPointer (VariableStoreHeader);
+  while (IsValidVariableHeader (Variable, GetEndPointer (VariableStoreHeader))) {
+    NextVariable = GetNextVariablePtr (Variable);
+    if (Variable != UpdatingVariable && Variable != UpdatingInDeletedTransition && Variable->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+
+      //
+      // Buffer has cached all ADDED variable.
+      // Per IN_DELETED variable, we have to guarantee that
+      // no ADDED one in previous buffer.
+      //
+
+      FoundAdded = FALSE;
+      AddedVariable = GetStartPointer ((VARIABLE_STORE_HEADER *) ValidBuffer);
+      while (IsValidVariableHeader (AddedVariable, GetEndPointer ((VARIABLE_STORE_HEADER *) ValidBuffer))) {
+        NextAddedVariable = GetNextVariablePtr (AddedVariable);
+        NameSize = NameSizeOfVariable (AddedVariable);
+        if (CompareGuid (GetVendorGuidPtr (AddedVariable), GetVendorGuidPtr (Variable)) &&
+            NameSize == NameSizeOfVariable (Variable)
+           ) {
+          Point0 = (VOID *) GetVariableNamePtr (AddedVariable);
+          Point1 = (VOID *) GetVariableNamePtr (Variable);
+          if (CompareMem (Point0, Point1, NameSize) == 0) {
+            FoundAdded = TRUE;
+            break;
+          }
+        }
+        AddedVariable = NextAddedVariable;
+      }
+      if (!FoundAdded) {
+        //
+        // Promote VAR_IN_DELETED_TRANSITION to VAR_ADDED.
+        //
+        VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+        CopyMem (CurrPtr, (UINT8 *) Variable, VariableSize);
+        ((VARIABLE_HEADER *) CurrPtr)->State = VAR_ADDED;
+        CurrPtr += VariableSize;
+        if ((!IsVolatile) && ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+          HwErrVariableTotalSize += VariableSize;
+        } else if ((!IsVolatile) && ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+          CommonVariableTotalSize += VariableSize;
+          if (IsUserVariable (Variable)) {
+            CommonUserVariableTotalSize += VariableSize;
+          }
+        }
+      }
+    }
+
+    Variable = NextVariable;
+  }
+
+  //
+  // Install the new variable if it is not NULL.
+  //
+  if (NewVariable != NULL) {
+    if ((UINTN) (CurrPtr - ValidBuffer) + NewVariableSize > VariableStoreHeader->Size) {
+      //
+      // No enough space to store the new variable.
+      //
+      Status = EFI_OUT_OF_RESOURCES;
+      goto Done;
+    }
+    if (!IsVolatile) {
+      if ((NewVariable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+        HwErrVariableTotalSize += NewVariableSize;
+      } else if ((NewVariable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+        CommonVariableTotalSize += NewVariableSize;
+        if (IsUserVariable (NewVariable)) {
+          CommonUserVariableTotalSize += NewVariableSize;
+        }
+      }
+      if ((HwErrVariableTotalSize > PcdGet32 (PcdHwErrStorageSize)) ||
+          (CommonVariableTotalSize > mVariableModuleGlobal->CommonVariableSpace) ||
+          (CommonUserVariableTotalSize > mVariableModuleGlobal->CommonMaxUserVariableSpace)) {
+        //
+        // No enough space to store the new variable by NV or NV+HR attribute.
+        //
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+    }
+
+    CopyMem (CurrPtr, (UINT8 *) NewVariable, NewVariableSize);
+    ((VARIABLE_HEADER *) CurrPtr)->State = VAR_ADDED;
+    if (UpdatingVariable != NULL) {
+      UpdatingPtrTrack->CurrPtr = (VARIABLE_HEADER *)((UINTN)UpdatingPtrTrack->StartPtr + ((UINTN)CurrPtr - (UINTN)GetStartPointer ((VARIABLE_STORE_HEADER *) ValidBuffer)));
+      UpdatingPtrTrack->InDeletedTransitionPtr = NULL;
+    }
+    CurrPtr += NewVariableSize;
+  }
+
+  if (IsVolatile) {
+    //
+    // If volatile variable store, just copy valid buffer.
+    //
+    SetMem ((UINT8 *) (UINTN) VariableBase, VariableStoreHeader->Size, 0xff);
+    CopyMem ((UINT8 *) (UINTN) VariableBase, ValidBuffer, (UINTN) (CurrPtr - ValidBuffer));
+    *LastVariableOffset = (UINTN) (CurrPtr - ValidBuffer);
+    Status  = EFI_SUCCESS;
+  } else {
+    //
+    // If non-volatile variable store, perform FTW here.
+    //
+    Status = FtwVariableSpace (
+              VariableBase,
+              (VARIABLE_STORE_HEADER *) ValidBuffer
+              );
+    if (!EFI_ERROR (Status)) {
+      *LastVariableOffset = (UINTN) (CurrPtr - ValidBuffer);
+      mVariableModuleGlobal->HwErrVariableTotalSize = HwErrVariableTotalSize;
+      mVariableModuleGlobal->CommonVariableTotalSize = CommonVariableTotalSize;
+      mVariableModuleGlobal->CommonUserVariableTotalSize = CommonUserVariableTotalSize;
+    } else {
+      Variable = GetStartPointer ((VARIABLE_STORE_HEADER *)(UINTN)VariableBase);
+      while (IsValidVariableHeader (Variable, GetEndPointer ((VARIABLE_STORE_HEADER *)(UINTN)VariableBase))) {
+        NextVariable = GetNextVariablePtr (Variable);
+        VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+        if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+          mVariableModuleGlobal->HwErrVariableTotalSize += VariableSize;
+        } else if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+          mVariableModuleGlobal->CommonVariableTotalSize += VariableSize;
+          if (IsUserVariable (Variable)) {
+            mVariableModuleGlobal->CommonUserVariableTotalSize += VariableSize;
+          }
+        }
+
+        Variable = NextVariable;
+      }
+      *LastVariableOffset = (UINTN) Variable - (UINTN) VariableBase;
+    }
+  }
+
+Done:
+  if (IsVolatile) {
+    FreePool (ValidBuffer);
+  } else {
+    //
+    // For NV variable reclaim, we use mNvVariableCache as the buffer, so copy the data back.
+    //
+    CopyMem (mNvVariableCache, (UINT8 *)(UINTN)VariableBase, VariableStoreHeader->Size);
+  }
+
+  return Status;
+}
+
+/**
+  Find the variable in the specified variable store.
+
+  @param[in]       VariableName        Name of the variable to be found
+  @param[in]       VendorGuid          Vendor GUID to be found.
+  @param[in]       IgnoreRtCheck       Ignore EFI_VARIABLE_RUNTIME_ACCESS attribute
+                                       check at runtime when searching variable.
+  @param[in, out]  PtrTrack            Variable Track Pointer structure that contains Variable Information.
+
+  @retval          EFI_SUCCESS         Variable found successfully
+  @retval          EFI_NOT_FOUND       Variable not found
+**/
+EFI_STATUS
+FindVariableEx (
+  IN     CHAR16                  *VariableName,
+  IN     EFI_GUID                *VendorGuid,
+  IN     BOOLEAN                 IgnoreRtCheck,
+  IN OUT VARIABLE_POINTER_TRACK  *PtrTrack
+  )
+{
+  VARIABLE_HEADER                *InDeletedVariable;
+  VOID                           *Point;
+
+  PtrTrack->InDeletedTransitionPtr = NULL;
+
+  //
+  // Find the variable by walk through HOB, volatile and non-volatile variable store.
+  //
+  InDeletedVariable  = NULL;
+
+  for ( PtrTrack->CurrPtr = PtrTrack->StartPtr
+      ; IsValidVariableHeader (PtrTrack->CurrPtr, PtrTrack->EndPtr)
+      ; PtrTrack->CurrPtr = GetNextVariablePtr (PtrTrack->CurrPtr)
+      ) {
+    if (PtrTrack->CurrPtr->State == VAR_ADDED ||
+        PtrTrack->CurrPtr->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)
+       ) {
+      if (IgnoreRtCheck || !AtRuntime () || ((PtrTrack->CurrPtr->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) != 0)) {
+        if (VariableName[0] == 0) {
+          if (PtrTrack->CurrPtr->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+            InDeletedVariable   = PtrTrack->CurrPtr;
+          } else {
+            PtrTrack->InDeletedTransitionPtr = InDeletedVariable;
+            return EFI_SUCCESS;
+          }
+        } else {
+          if (CompareGuid (VendorGuid, GetVendorGuidPtr (PtrTrack->CurrPtr))) {
+            Point = (VOID *) GetVariableNamePtr (PtrTrack->CurrPtr);
+
+            ASSERT (NameSizeOfVariable (PtrTrack->CurrPtr) != 0);
+            if (CompareMem (VariableName, Point, NameSizeOfVariable (PtrTrack->CurrPtr)) == 0) {
+              if (PtrTrack->CurrPtr->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+                InDeletedVariable     = PtrTrack->CurrPtr;
+              } else {
+                PtrTrack->InDeletedTransitionPtr = InDeletedVariable;
+                return EFI_SUCCESS;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  PtrTrack->CurrPtr = InDeletedVariable;
+  return (PtrTrack->CurrPtr  == NULL) ? EFI_NOT_FOUND : EFI_SUCCESS;
+}
+
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+  If IgnoreRtCheck is TRUE, then we ignore the EFI_VARIABLE_RUNTIME_ACCESS attribute check
+  at runtime when searching existing variable, only VariableName and VendorGuid are compared.
+  Otherwise, variables without EFI_VARIABLE_RUNTIME_ACCESS are not visible at runtime.
+
+  @param[in]   VariableName           Name of the variable to be found.
+  @param[in]   VendorGuid             Vendor GUID to be found.
+  @param[out]  PtrTrack               VARIABLE_POINTER_TRACK structure for output,
+                                      including the range searched and the target position.
+  @param[in]   Global                 Pointer to VARIABLE_GLOBAL structure, including
+                                      base of volatile variable storage area, base of
+                                      NV variable storage area, and a lock.
+  @param[in]   IgnoreRtCheck          Ignore EFI_VARIABLE_RUNTIME_ACCESS attribute
+                                      check at runtime when searching variable.
+
+  @retval EFI_INVALID_PARAMETER       If VariableName is not an empty string, while
+                                      VendorGuid is NULL.
+  @retval EFI_SUCCESS                 Variable successfully found.
+  @retval EFI_NOT_FOUND               Variable not found
+
+**/
+EFI_STATUS
+FindVariable (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK  *PtrTrack,
+  IN  VARIABLE_GLOBAL         *Global,
+  IN  BOOLEAN                 IgnoreRtCheck
+  )
+{
+  EFI_STATUS              Status;
+  VARIABLE_STORE_HEADER   *VariableStoreHeader[VariableStoreTypeMax];
+  VARIABLE_STORE_TYPE     Type;
+
+  if (VariableName[0] != 0 && VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // 0: Volatile, 1: HOB, 2: Non-Volatile.
+  // The index and attributes mapping must be kept in this order as RuntimeServiceGetNextVariableName
+  // make use of this mapping to implement search algorithm.
+  //
+  VariableStoreHeader[VariableStoreTypeVolatile] = (VARIABLE_STORE_HEADER *) (UINTN) Global->VolatileVariableBase;
+  VariableStoreHeader[VariableStoreTypeHob]      = (VARIABLE_STORE_HEADER *) (UINTN) Global->HobVariableBase;
+  VariableStoreHeader[VariableStoreTypeNv]       = mNvVariableCache;
+
+  //
+  // Find the variable by walk through HOB, volatile and non-volatile variable store.
+  //
+  for (Type = (VARIABLE_STORE_TYPE) 0; Type < VariableStoreTypeMax; Type++) {
+    if (VariableStoreHeader[Type] == NULL) {
+      continue;
+    }
+
+    PtrTrack->StartPtr = GetStartPointer (VariableStoreHeader[Type]);
+    PtrTrack->EndPtr   = GetEndPointer   (VariableStoreHeader[Type]);
+    PtrTrack->Volatile = (BOOLEAN) (Type == VariableStoreTypeVolatile);
+
+    Status = FindVariableEx (VariableName, VendorGuid, IgnoreRtCheck, PtrTrack);
+    if (!EFI_ERROR (Status)) {
+      return Status;
+    }
+  }
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Get index from supported language codes according to language string.
+
+  This code is used to get corresponding index in supported language codes. It can handle
+  RFC4646 and ISO639 language tags.
+  In ISO639 language tags, take 3-characters as a delimitation to find matched string and calculate the index.
+  In RFC4646 language tags, take semicolon as a delimitation to find matched string and calculate the index.
+
+  For example:
+    SupportedLang  = "engfraengfra"
+    Lang           = "eng"
+    Iso639Language = TRUE
+  The return value is "0".
+  Another example:
+    SupportedLang  = "en;fr;en-US;fr-FR"
+    Lang           = "fr-FR"
+    Iso639Language = FALSE
+  The return value is "3".
+
+  @param  SupportedLang               Platform supported language codes.
+  @param  Lang                        Configured language.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646.
+
+  @retval The index of language in the language codes.
+
+**/
+UINTN
+GetIndexFromSupportedLangCodes(
+  IN  CHAR8            *SupportedLang,
+  IN  CHAR8            *Lang,
+  IN  BOOLEAN          Iso639Language
+  )
+{
+  UINTN    Index;
+  UINTN    CompareLength;
+  UINTN    LanguageLength;
+
+  if (Iso639Language) {
+    CompareLength = ISO_639_2_ENTRY_SIZE;
+    for (Index = 0; Index < AsciiStrLen (SupportedLang); Index += CompareLength) {
+      if (AsciiStrnCmp (Lang, SupportedLang + Index, CompareLength) == 0) {
+        //
+        // Successfully find the index of Lang string in SupportedLang string.
+        //
+        Index = Index / CompareLength;
+        return Index;
+      }
+    }
+    ASSERT (FALSE);
+    return 0;
+  } else {
+    //
+    // Compare RFC4646 language code
+    //
+    Index = 0;
+    for (LanguageLength = 0; Lang[LanguageLength] != '\0'; LanguageLength++);
+
+    for (Index = 0; *SupportedLang != '\0'; Index++, SupportedLang += CompareLength) {
+      //
+      // Skip ';' characters in SupportedLang
+      //
+      for (; *SupportedLang != '\0' && *SupportedLang == ';'; SupportedLang++);
+      //
+      // Determine the length of the next language code in SupportedLang
+      //
+      for (CompareLength = 0; SupportedLang[CompareLength] != '\0' && SupportedLang[CompareLength] != ';'; CompareLength++);
+
+      if ((CompareLength == LanguageLength) &&
+          (AsciiStrnCmp (Lang, SupportedLang, CompareLength) == 0)) {
+        //
+        // Successfully find the index of Lang string in SupportedLang string.
+        //
+        return Index;
+      }
+    }
+    ASSERT (FALSE);
+    return 0;
+  }
+}
+
+/**
+  Get language string from supported language codes according to index.
+
+  This code is used to get corresponding language strings in supported language codes. It can handle
+  RFC4646 and ISO639 language tags.
+  In ISO639 language tags, take 3-characters as a delimitation. Find language string according to the index.
+  In RFC4646 language tags, take semicolon as a delimitation. Find language string according to the index.
+
+  For example:
+    SupportedLang  = "engfraengfra"
+    Index          = "1"
+    Iso639Language = TRUE
+  The return value is "fra".
+  Another example:
+    SupportedLang  = "en;fr;en-US;fr-FR"
+    Index          = "1"
+    Iso639Language = FALSE
+  The return value is "fr".
+
+  @param  SupportedLang               Platform supported language codes.
+  @param  Index                       The index in supported language codes.
+  @param  Iso639Language              A bool value to signify if the handler is operated on ISO639 or RFC4646.
+
+  @retval The language string in the language codes.
+
+**/
+CHAR8 *
+GetLangFromSupportedLangCodes (
+  IN  CHAR8            *SupportedLang,
+  IN  UINTN            Index,
+  IN  BOOLEAN          Iso639Language
+)
+{
+  UINTN    SubIndex;
+  UINTN    CompareLength;
+  CHAR8    *Supported;
+
+  SubIndex  = 0;
+  Supported = SupportedLang;
+  if (Iso639Language) {
+    //
+    // According to the index of Lang string in SupportedLang string to get the language.
+    // This code will be invoked in RUNTIME, therefore there is not a memory allocate/free operation.
+    // In driver entry, it pre-allocates a runtime attribute memory to accommodate this string.
+    //
+    CompareLength = ISO_639_2_ENTRY_SIZE;
+    mVariableModuleGlobal->Lang[CompareLength] = '\0';
+    return CopyMem (mVariableModuleGlobal->Lang, SupportedLang + Index * CompareLength, CompareLength);
+
+  } else {
+    while (TRUE) {
+      //
+      // Take semicolon as delimitation, sequentially traverse supported language codes.
+      //
+      for (CompareLength = 0; *Supported != ';' && *Supported != '\0'; CompareLength++) {
+        Supported++;
+      }
+      if ((*Supported == '\0') && (SubIndex != Index)) {
+        //
+        // Have completed the traverse, but not find corrsponding string.
+        // This case is not allowed to happen.
+        //
+        ASSERT(FALSE);
+        return NULL;
+      }
+      if (SubIndex == Index) {
+        //
+        // According to the index of Lang string in SupportedLang string to get the language.
+        // As this code will be invoked in RUNTIME, therefore there is not memory allocate/free operation.
+        // In driver entry, it pre-allocates a runtime attribute memory to accommodate this string.
+        //
+        mVariableModuleGlobal->PlatformLang[CompareLength] = '\0';
+        return CopyMem (mVariableModuleGlobal->PlatformLang, Supported - CompareLength, CompareLength);
+      }
+      SubIndex++;
+
+      //
+      // Skip ';' characters in Supported
+      //
+      for (; *Supported != '\0' && *Supported == ';'; Supported++);
+    }
+  }
+}
+
+/**
+  Returns a pointer to an allocated buffer that contains the best matching language
+  from a set of supported languages.
+
+  This function supports both ISO 639-2 and RFC 4646 language codes, but language
+  code types may not be mixed in a single call to this function. This function
+  supports a variable argument list that allows the caller to pass in a prioritized
+  list of language codes to test against all the language codes in SupportedLanguages.
+
+  If SupportedLanguages is NULL, then ASSERT().
+
+  @param[in]  SupportedLanguages  A pointer to a Null-terminated ASCII string that
+                                  contains a set of language codes in the format
+                                  specified by Iso639Language.
+  @param[in]  Iso639Language      If TRUE, then all language codes are assumed to be
+                                  in ISO 639-2 format.  If FALSE, then all language
+                                  codes are assumed to be in RFC 4646 language format
+  @param[in]  ...                 A variable argument list that contains pointers to
+                                  Null-terminated ASCII strings that contain one or more
+                                  language codes in the format specified by Iso639Language.
+                                  The first language code from each of these language
+                                  code lists is used to determine if it is an exact or
+                                  close match to any of the language codes in
+                                  SupportedLanguages.  Close matches only apply to RFC 4646
+                                  language codes, and the matching algorithm from RFC 4647
+                                  is used to determine if a close match is present.  If
+                                  an exact or close match is found, then the matching
+                                  language code from SupportedLanguages is returned.  If
+                                  no matches are found, then the next variable argument
+                                  parameter is evaluated.  The variable argument list
+                                  is terminated by a NULL.
+
+  @retval NULL   The best matching language could not be found in SupportedLanguages.
+  @retval NULL   There are not enough resources available to return the best matching
+                 language.
+  @retval Other  A pointer to a Null-terminated ASCII string that is the best matching
+                 language in SupportedLanguages.
+
+**/
+CHAR8 *
+EFIAPI
+VariableGetBestLanguage (
+  IN CONST CHAR8  *SupportedLanguages,
+  IN BOOLEAN      Iso639Language,
+  ...
+  )
+{
+  VA_LIST      Args;
+  CHAR8        *Language;
+  UINTN        CompareLength;
+  UINTN        LanguageLength;
+  CONST CHAR8  *Supported;
+  CHAR8        *Buffer;
+
+  if (SupportedLanguages == NULL) {
+    return NULL;
+  }
+
+  VA_START (Args, Iso639Language);
+  while ((Language = VA_ARG (Args, CHAR8 *)) != NULL) {
+    //
+    // Default to ISO 639-2 mode
+    //
+    CompareLength  = 3;
+    LanguageLength = MIN (3, AsciiStrLen (Language));
+
+    //
+    // If in RFC 4646 mode, then determine the length of the first RFC 4646 language code in Language
+    //
+    if (!Iso639Language) {
+      for (LanguageLength = 0; Language[LanguageLength] != 0 && Language[LanguageLength] != ';'; LanguageLength++);
+    }
+
+    //
+    // Trim back the length of Language used until it is empty
+    //
+    while (LanguageLength > 0) {
+      //
+      // Loop through all language codes in SupportedLanguages
+      //
+      for (Supported = SupportedLanguages; *Supported != '\0'; Supported += CompareLength) {
+        //
+        // In RFC 4646 mode, then Loop through all language codes in SupportedLanguages
+        //
+        if (!Iso639Language) {
+          //
+          // Skip ';' characters in Supported
+          //
+          for (; *Supported != '\0' && *Supported == ';'; Supported++);
+          //
+          // Determine the length of the next language code in Supported
+          //
+          for (CompareLength = 0; Supported[CompareLength] != 0 && Supported[CompareLength] != ';'; CompareLength++);
+          //
+          // If Language is longer than the Supported, then skip to the next language
+          //
+          if (LanguageLength > CompareLength) {
+            continue;
+          }
+        }
+        //
+        // See if the first LanguageLength characters in Supported match Language
+        //
+        if (AsciiStrnCmp (Supported, Language, LanguageLength) == 0) {
+          VA_END (Args);
+
+          Buffer = Iso639Language ? mVariableModuleGlobal->Lang : mVariableModuleGlobal->PlatformLang;
+          Buffer[CompareLength] = '\0';
+          return CopyMem (Buffer, Supported, CompareLength);
+        }
+      }
+
+      if (Iso639Language) {
+        //
+        // If ISO 639 mode, then each language can only be tested once
+        //
+        LanguageLength = 0;
+      } else {
+        //
+        // If RFC 4646 mode, then trim Language from the right to the next '-' character
+        //
+        for (LanguageLength--; LanguageLength > 0 && Language[LanguageLength] != '-'; LanguageLength--);
+      }
+    }
+  }
+  VA_END (Args);
+
+  //
+  // No matches were found
+  //
+  return NULL;
+}
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param[in] Marker             VA_LIST style variable argument list.
+                                The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+BOOLEAN
+EFIAPI
+CheckRemainingSpaceForConsistencyInternal (
+  IN UINT32                     Attributes,
+  IN VA_LIST                    Marker
+  )
+{
+  EFI_STATUS                    Status;
+  VA_LIST                       Args;
+  VARIABLE_ENTRY_CONSISTENCY    *VariableEntry;
+  UINT64                        MaximumVariableStorageSize;
+  UINT64                        RemainingVariableStorageSize;
+  UINT64                        MaximumVariableSize;
+  UINTN                         TotalNeededSize;
+  UINTN                         OriginalVarSize;
+  VARIABLE_STORE_HEADER         *VariableStoreHeader;
+  VARIABLE_POINTER_TRACK        VariablePtrTrack;
+  VARIABLE_HEADER               *NextVariable;
+  UINTN                         VarNameSize;
+  UINTN                         VarDataSize;
+
+  //
+  // Non-Volatile related.
+  //
+  VariableStoreHeader = mNvVariableCache;
+
+  Status = VariableServiceQueryVariableInfoInternal (
+             Attributes,
+             &MaximumVariableStorageSize,
+             &RemainingVariableStorageSize,
+             &MaximumVariableSize
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  TotalNeededSize = 0;
+  Args = Marker;
+  VariableEntry = VA_ARG (Args, VARIABLE_ENTRY_CONSISTENCY *);
+  while (VariableEntry != NULL) {
+    //
+    // Calculate variable total size.
+    //
+    VarNameSize  = StrSize (VariableEntry->Name);
+    VarNameSize += GET_PAD_SIZE (VarNameSize);
+    VarDataSize  = VariableEntry->VariableSize;
+    VarDataSize += GET_PAD_SIZE (VarDataSize);
+    VariableEntry->VariableSize = HEADER_ALIGN (GetVariableHeaderSize () + VarNameSize + VarDataSize);
+
+    TotalNeededSize += VariableEntry->VariableSize;
+    VariableEntry = VA_ARG (Args, VARIABLE_ENTRY_CONSISTENCY *);
+  }
+
+  if (RemainingVariableStorageSize >= TotalNeededSize) {
+    //
+    // Already have enough space.
+    //
+    return TRUE;
+  } else if (AtRuntime ()) {
+    //
+    // At runtime, no reclaim.
+    // The original variable space of Variables can't be reused.
+    //
+    return FALSE;
+  }
+
+  Args = Marker;
+  VariableEntry = VA_ARG (Args, VARIABLE_ENTRY_CONSISTENCY *);
+  while (VariableEntry != NULL) {
+    //
+    // Check if Variable[Index] has been present and get its size.
+    //
+    OriginalVarSize = 0;
+    VariablePtrTrack.StartPtr = GetStartPointer (VariableStoreHeader);
+    VariablePtrTrack.EndPtr   = GetEndPointer   (VariableStoreHeader);
+    Status = FindVariableEx (
+               VariableEntry->Name,
+               VariableEntry->Guid,
+               FALSE,
+               &VariablePtrTrack
+               );
+    if (!EFI_ERROR (Status)) {
+      //
+      // Get size of Variable[Index].
+      //
+      NextVariable = GetNextVariablePtr (VariablePtrTrack.CurrPtr);
+      OriginalVarSize = (UINTN) NextVariable - (UINTN) VariablePtrTrack.CurrPtr;
+      //
+      // Add the original size of Variable[Index] to remaining variable storage size.
+      //
+      RemainingVariableStorageSize += OriginalVarSize;
+    }
+    if (VariableEntry->VariableSize > RemainingVariableStorageSize) {
+      //
+      // No enough space for Variable[Index].
+      //
+      return FALSE;
+    }
+    //
+    // Sub the (new) size of Variable[Index] from remaining variable storage size.
+    //
+    RemainingVariableStorageSize -= VariableEntry->VariableSize;
+    VariableEntry = VA_ARG (Args, VARIABLE_ENTRY_CONSISTENCY *);
+  }
+
+  return TRUE;
+}
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param ...                    The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+BOOLEAN
+EFIAPI
+CheckRemainingSpaceForConsistency (
+  IN UINT32                     Attributes,
+  ...
+  )
+{
+  VA_LIST Marker;
+  BOOLEAN Return;
+
+  VA_START (Marker, Attributes);
+
+  Return = CheckRemainingSpaceForConsistencyInternal (Attributes, Marker);
+
+  VA_END (Marker);
+
+  return Return;
+}
+
+/**
+  Hook the operations in PlatformLangCodes, LangCodes, PlatformLang and Lang.
+
+  When setting Lang/LangCodes, simultaneously update PlatformLang/PlatformLangCodes.
+
+  According to UEFI spec, PlatformLangCodes/LangCodes are only set once in firmware initialization,
+  and are read-only. Therefore, in variable driver, only store the original value for other use.
+
+  @param[in] VariableName       Name of variable.
+
+  @param[in] Data               Variable data.
+
+  @param[in] DataSize           Size of data. 0 means delete.
+
+  @retval EFI_SUCCESS           The update operation is successful or ignored.
+  @retval EFI_WRITE_PROTECTED   Update PlatformLangCodes/LangCodes at runtime.
+  @retval EFI_OUT_OF_RESOURCES  No enough variable space to do the update operation.
+  @retval Others                Other errors happened during the update operation.
+
+**/
+EFI_STATUS
+AutoUpdateLangVariable (
+  IN  CHAR16             *VariableName,
+  IN  VOID               *Data,
+  IN  UINTN              DataSize
+  )
+{
+  EFI_STATUS             Status;
+  CHAR8                  *BestPlatformLang;
+  CHAR8                  *BestLang;
+  UINTN                  Index;
+  UINT32                 Attributes;
+  VARIABLE_POINTER_TRACK Variable;
+  BOOLEAN                SetLanguageCodes;
+  VARIABLE_ENTRY_CONSISTENCY VariableEntry[2];
+
+  //
+  // Don't do updates for delete operation
+  //
+  if (DataSize == 0) {
+    return EFI_SUCCESS;
+  }
+
+  SetLanguageCodes = FALSE;
+
+  if (StrCmp (VariableName, EFI_PLATFORM_LANG_CODES_VARIABLE_NAME) == 0) {
+    //
+    // PlatformLangCodes is a volatile variable, so it can not be updated at runtime.
+    //
+    if (AtRuntime ()) {
+      return EFI_WRITE_PROTECTED;
+    }
+
+    SetLanguageCodes = TRUE;
+
+    //
+    // According to UEFI spec, PlatformLangCodes is only set once in firmware initialization, and is read-only
+    // Therefore, in variable driver, only store the original value for other use.
+    //
+    if (mVariableModuleGlobal->PlatformLangCodes != NULL) {
+      FreePool (mVariableModuleGlobal->PlatformLangCodes);
+    }
+    mVariableModuleGlobal->PlatformLangCodes = AllocateRuntimeCopyPool (DataSize, Data);
+    ASSERT (mVariableModuleGlobal->PlatformLangCodes != NULL);
+
+    //
+    // PlatformLang holds a single language from PlatformLangCodes,
+    // so the size of PlatformLangCodes is enough for the PlatformLang.
+    //
+    if (mVariableModuleGlobal->PlatformLang != NULL) {
+      FreePool (mVariableModuleGlobal->PlatformLang);
+    }
+    mVariableModuleGlobal->PlatformLang = AllocateRuntimePool (DataSize);
+    ASSERT (mVariableModuleGlobal->PlatformLang != NULL);
+
+  } else if (StrCmp (VariableName, EFI_LANG_CODES_VARIABLE_NAME) == 0) {
+    //
+    // LangCodes is a volatile variable, so it can not be updated at runtime.
+    //
+    if (AtRuntime ()) {
+      return EFI_WRITE_PROTECTED;
+    }
+
+    SetLanguageCodes = TRUE;
+
+    //
+    // According to UEFI spec, LangCodes is only set once in firmware initialization, and is read-only
+    // Therefore, in variable driver, only store the original value for other use.
+    //
+    if (mVariableModuleGlobal->LangCodes != NULL) {
+      FreePool (mVariableModuleGlobal->LangCodes);
+    }
+    mVariableModuleGlobal->LangCodes = AllocateRuntimeCopyPool (DataSize, Data);
+    ASSERT (mVariableModuleGlobal->LangCodes != NULL);
+  }
+
+  if (SetLanguageCodes
+      && (mVariableModuleGlobal->PlatformLangCodes != NULL)
+      && (mVariableModuleGlobal->LangCodes != NULL)) {
+    //
+    // Update Lang if PlatformLang is already set
+    // Update PlatformLang if Lang is already set
+    //
+    Status = FindVariable (EFI_PLATFORM_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+    if (!EFI_ERROR (Status)) {
+      //
+      // Update Lang
+      //
+      VariableName = EFI_PLATFORM_LANG_VARIABLE_NAME;
+      Data         = GetVariableDataPtr (Variable.CurrPtr);
+      DataSize     = DataSizeOfVariable (Variable.CurrPtr);
+    } else {
+      Status = FindVariable (EFI_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+      if (!EFI_ERROR (Status)) {
+        //
+        // Update PlatformLang
+        //
+        VariableName = EFI_LANG_VARIABLE_NAME;
+        Data         = GetVariableDataPtr (Variable.CurrPtr);
+        DataSize     = DataSizeOfVariable (Variable.CurrPtr);
+      } else {
+        //
+        // Neither PlatformLang nor Lang is set, directly return
+        //
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  Status = EFI_SUCCESS;
+
+  //
+  // According to UEFI spec, "Lang" and "PlatformLang" is NV|BS|RT attributions.
+  //
+  Attributes = EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS | EFI_VARIABLE_RUNTIME_ACCESS;
+
+  if (StrCmp (VariableName, EFI_PLATFORM_LANG_VARIABLE_NAME) == 0) {
+    //
+    // Update Lang when PlatformLangCodes/LangCodes were set.
+    //
+    if ((mVariableModuleGlobal->PlatformLangCodes != NULL) && (mVariableModuleGlobal->LangCodes != NULL)) {
+      //
+      // When setting PlatformLang, firstly get most matched language string from supported language codes.
+      //
+      BestPlatformLang = VariableGetBestLanguage (mVariableModuleGlobal->PlatformLangCodes, FALSE, Data, NULL);
+      if (BestPlatformLang != NULL) {
+        //
+        // Get the corresponding index in language codes.
+        //
+        Index = GetIndexFromSupportedLangCodes (mVariableModuleGlobal->PlatformLangCodes, BestPlatformLang, FALSE);
+
+        //
+        // Get the corresponding ISO639 language tag according to RFC4646 language tag.
+        //
+        BestLang = GetLangFromSupportedLangCodes (mVariableModuleGlobal->LangCodes, Index, TRUE);
+
+        //
+        // Check the variable space for both Lang and PlatformLang variable.
+        //
+        VariableEntry[0].VariableSize = ISO_639_2_ENTRY_SIZE + 1;
+        VariableEntry[0].Guid = &gEfiGlobalVariableGuid;
+        VariableEntry[0].Name = EFI_LANG_VARIABLE_NAME;
+
+        VariableEntry[1].VariableSize = AsciiStrSize (BestPlatformLang);
+        VariableEntry[1].Guid = &gEfiGlobalVariableGuid;
+        VariableEntry[1].Name = EFI_PLATFORM_LANG_VARIABLE_NAME;
+        if (!CheckRemainingSpaceForConsistency (VARIABLE_ATTRIBUTE_NV_BS_RT, &VariableEntry[0], &VariableEntry[1], NULL)) {
+          //
+          // No enough variable space to set both Lang and PlatformLang successfully.
+          //
+          Status = EFI_OUT_OF_RESOURCES;
+        } else {
+          //
+          // Successfully convert PlatformLang to Lang, and set the BestLang value into Lang variable simultaneously.
+          //
+          FindVariable (EFI_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+
+          Status = UpdateVariable (EFI_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, BestLang,
+                                   ISO_639_2_ENTRY_SIZE + 1, Attributes, 0, 0, &Variable, NULL);
+        }
+
+        DEBUG ((EFI_D_INFO, "Variable Driver Auto Update PlatformLang, PlatformLang:%a, Lang:%a Status: %r\n", BestPlatformLang, BestLang, Status));
+      }
+    }
+
+  } else if (StrCmp (VariableName, EFI_LANG_VARIABLE_NAME) == 0) {
+    //
+    // Update PlatformLang when PlatformLangCodes/LangCodes were set.
+    //
+    if ((mVariableModuleGlobal->PlatformLangCodes != NULL) && (mVariableModuleGlobal->LangCodes != NULL)) {
+      //
+      // When setting Lang, firstly get most matched language string from supported language codes.
+      //
+      BestLang = VariableGetBestLanguage (mVariableModuleGlobal->LangCodes, TRUE, Data, NULL);
+      if (BestLang != NULL) {
+        //
+        // Get the corresponding index in language codes.
+        //
+        Index = GetIndexFromSupportedLangCodes (mVariableModuleGlobal->LangCodes, BestLang, TRUE);
+
+        //
+        // Get the corresponding RFC4646 language tag according to ISO639 language tag.
+        //
+        BestPlatformLang = GetLangFromSupportedLangCodes (mVariableModuleGlobal->PlatformLangCodes, Index, FALSE);
+
+        //
+        // Check the variable space for both PlatformLang and Lang variable.
+        //
+        VariableEntry[0].VariableSize = AsciiStrSize (BestPlatformLang);
+        VariableEntry[0].Guid = &gEfiGlobalVariableGuid;
+        VariableEntry[0].Name = EFI_PLATFORM_LANG_VARIABLE_NAME;
+
+        VariableEntry[1].VariableSize = ISO_639_2_ENTRY_SIZE + 1;
+        VariableEntry[1].Guid = &gEfiGlobalVariableGuid;
+        VariableEntry[1].Name = EFI_LANG_VARIABLE_NAME;
+        if (!CheckRemainingSpaceForConsistency (VARIABLE_ATTRIBUTE_NV_BS_RT, &VariableEntry[0], &VariableEntry[1], NULL)) {
+          //
+          // No enough variable space to set both PlatformLang and Lang successfully.
+          //
+          Status = EFI_OUT_OF_RESOURCES;
+        } else {
+          //
+          // Successfully convert Lang to PlatformLang, and set the BestPlatformLang value into PlatformLang variable simultaneously.
+          //
+          FindVariable (EFI_PLATFORM_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+
+          Status = UpdateVariable (EFI_PLATFORM_LANG_VARIABLE_NAME, &gEfiGlobalVariableGuid, BestPlatformLang,
+                                   AsciiStrSize (BestPlatformLang), Attributes, 0, 0, &Variable, NULL);
+        }
+
+        DEBUG ((EFI_D_INFO, "Variable Driver Auto Update Lang, Lang:%a, PlatformLang:%a Status: %r\n", BestLang, BestPlatformLang, Status));
+      }
+    }
+  }
+
+  if (SetLanguageCodes) {
+    //
+    // Continue to set PlatformLangCodes or LangCodes.
+    //
+    return EFI_SUCCESS;
+  } else {
+    return Status;
+  }
+}
+
+/**
+  Compare two EFI_TIME data.
+
+
+  @param FirstTime           A pointer to the first EFI_TIME data.
+  @param SecondTime          A pointer to the second EFI_TIME data.
+
+  @retval  TRUE              The FirstTime is not later than the SecondTime.
+  @retval  FALSE             The FirstTime is later than the SecondTime.
+
+**/
+BOOLEAN
+VariableCompareTimeStampInternal (
+  IN EFI_TIME               *FirstTime,
+  IN EFI_TIME               *SecondTime
+  )
+{
+  if (FirstTime->Year != SecondTime->Year) {
+    return (BOOLEAN) (FirstTime->Year < SecondTime->Year);
+  } else if (FirstTime->Month != SecondTime->Month) {
+    return (BOOLEAN) (FirstTime->Month < SecondTime->Month);
+  } else if (FirstTime->Day != SecondTime->Day) {
+    return (BOOLEAN) (FirstTime->Day < SecondTime->Day);
+  } else if (FirstTime->Hour != SecondTime->Hour) {
+    return (BOOLEAN) (FirstTime->Hour < SecondTime->Hour);
+  } else if (FirstTime->Minute != SecondTime->Minute) {
+    return (BOOLEAN) (FirstTime->Minute < SecondTime->Minute);
+  }
+
+  return (BOOLEAN) (FirstTime->Second <= SecondTime->Second);
+}
+
+/**
+  Update the variable region with Variable information. If EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS is set,
+  index of associated public key is needed.
+
+  @param[in] VariableName       Name of variable.
+  @param[in] VendorGuid         Guid of variable.
+  @param[in] Data               Variable data.
+  @param[in] DataSize           Size of data. 0 means delete.
+  @param[in] Attributes         Attributes of the variable.
+  @param[in] KeyIndex           Index of associated public key.
+  @param[in] MonotonicCount     Value of associated monotonic count.
+  @param[in, out] CacheVariable The variable information which is used to keep track of variable usage.
+  @param[in] TimeStamp          Value of associated TimeStamp.
+
+  @retval EFI_SUCCESS           The update operation is success.
+  @retval EFI_OUT_OF_RESOURCES  Variable region is full, can not write other data into this region.
+
+**/
+EFI_STATUS
+UpdateVariable (
+  IN      CHAR16                      *VariableName,
+  IN      EFI_GUID                    *VendorGuid,
+  IN      VOID                        *Data,
+  IN      UINTN                       DataSize,
+  IN      UINT32                      Attributes      OPTIONAL,
+  IN      UINT32                      KeyIndex        OPTIONAL,
+  IN      UINT64                      MonotonicCount  OPTIONAL,
+  IN OUT  VARIABLE_POINTER_TRACK      *CacheVariable,
+  IN      EFI_TIME                    *TimeStamp      OPTIONAL
+  )
+{
+  EFI_STATUS                          Status;
+  VARIABLE_HEADER                     *NextVariable;
+  UINTN                               ScratchSize;
+  UINTN                               MaxDataSize;
+  UINTN                               VarNameOffset;
+  UINTN                               VarDataOffset;
+  UINTN                               VarNameSize;
+  UINTN                               VarSize;
+  BOOLEAN                             Volatile;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *Fvb;
+  UINT8                               State;
+  VARIABLE_POINTER_TRACK              *Variable;
+  VARIABLE_POINTER_TRACK              NvVariable;
+  VARIABLE_STORE_HEADER               *VariableStoreHeader;
+  UINTN                               CacheOffset;
+  UINT8                               *BufferForMerge;
+  UINTN                               MergedBufSize;
+  BOOLEAN                             DataReady;
+  UINTN                               DataOffset;
+  BOOLEAN                             IsCommonVariable;
+  BOOLEAN                             IsCommonUserVariable;
+  AUTHENTICATED_VARIABLE_HEADER       *AuthVariable;
+
+  if (mVariableModuleGlobal->FvbInstance == NULL) {
+    //
+    // The FVB protocol is not ready, so the EFI_VARIABLE_WRITE_ARCH_PROTOCOL is not installed.
+    //
+    if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+      //
+      // Trying to update NV variable prior to the installation of EFI_VARIABLE_WRITE_ARCH_PROTOCOL
+      //
+      DEBUG ((EFI_D_ERROR, "Update NV variable before EFI_VARIABLE_WRITE_ARCH_PROTOCOL ready - %r\n", EFI_NOT_AVAILABLE_YET));
+      return EFI_NOT_AVAILABLE_YET;
+    } else if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+      //
+      // Trying to update volatile authenticated variable prior to the installation of EFI_VARIABLE_WRITE_ARCH_PROTOCOL
+      // The authenticated variable perhaps is not initialized, just return here.
+      //
+      DEBUG ((EFI_D_ERROR, "Update AUTH variable before EFI_VARIABLE_WRITE_ARCH_PROTOCOL ready - %r\n", EFI_NOT_AVAILABLE_YET));
+      return EFI_NOT_AVAILABLE_YET;
+    }
+  }
+
+  //
+  // Check if CacheVariable points to the variable in variable HOB.
+  // If yes, let CacheVariable points to the variable in NV variable cache.
+  //
+  if ((CacheVariable->CurrPtr != NULL) &&
+      (mVariableModuleGlobal->VariableGlobal.HobVariableBase != 0) &&
+      (CacheVariable->StartPtr == GetStartPointer ((VARIABLE_STORE_HEADER *) (UINTN) mVariableModuleGlobal->VariableGlobal.HobVariableBase))
+     ) {
+    CacheVariable->StartPtr = GetStartPointer (mNvVariableCache);
+    CacheVariable->EndPtr   = GetEndPointer   (mNvVariableCache);
+    CacheVariable->Volatile = FALSE;
+    Status = FindVariableEx (VariableName, VendorGuid, FALSE, CacheVariable);
+    if (CacheVariable->CurrPtr == NULL || EFI_ERROR (Status)) {
+      //
+      // There is no matched variable in NV variable cache.
+      //
+      if ((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0)) || (Attributes == 0)) {
+        //
+        // It is to delete variable,
+        // go to delete this variable in variable HOB and
+        // try to flush other variables from HOB to flash.
+        //
+        FlushHobVariableToFlash (VariableName, VendorGuid);
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  if ((CacheVariable->CurrPtr == NULL) || CacheVariable->Volatile) {
+    Variable = CacheVariable;
+  } else {
+    //
+    // Update/Delete existing NV variable.
+    // CacheVariable points to the variable in the memory copy of Flash area
+    // Now let Variable points to the same variable in Flash area.
+    //
+    VariableStoreHeader  = (VARIABLE_STORE_HEADER *) ((UINTN) mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase);
+    Variable = &NvVariable;
+    Variable->StartPtr = GetStartPointer (VariableStoreHeader);
+    Variable->EndPtr   = GetEndPointer (VariableStoreHeader);
+    Variable->CurrPtr  = (VARIABLE_HEADER *)((UINTN)Variable->StartPtr + ((UINTN)CacheVariable->CurrPtr - (UINTN)CacheVariable->StartPtr));
+    if (CacheVariable->InDeletedTransitionPtr != NULL) {
+      Variable->InDeletedTransitionPtr = (VARIABLE_HEADER *)((UINTN)Variable->StartPtr + ((UINTN)CacheVariable->InDeletedTransitionPtr - (UINTN)CacheVariable->StartPtr));
+    } else {
+      Variable->InDeletedTransitionPtr = NULL;
+    }
+    Variable->Volatile = FALSE;
+  }
+
+  Fvb       = mVariableModuleGlobal->FvbInstance;
+
+  //
+  // Tricky part: Use scratch data area at the end of volatile variable store
+  // as a temporary storage.
+  //
+  NextVariable = GetEndPointer ((VARIABLE_STORE_HEADER *) ((UINTN) mVariableModuleGlobal->VariableGlobal.VolatileVariableBase));
+  ScratchSize = mVariableModuleGlobal->ScratchBufferSize;
+  SetMem (NextVariable, ScratchSize, 0xff);
+  DataReady = FALSE;
+
+  if (Variable->CurrPtr != NULL) {
+    //
+    // Update/Delete existing variable.
+    //
+    if (AtRuntime ()) {
+      //
+      // If AtRuntime and the variable is Volatile and Runtime Access,
+      // the volatile is ReadOnly, and SetVariable should be aborted and
+      // return EFI_WRITE_PROTECTED.
+      //
+      if (Variable->Volatile) {
+        Status = EFI_WRITE_PROTECTED;
+        goto Done;
+      }
+      //
+      // Only variable that have NV attributes can be updated/deleted in Runtime.
+      //
+      if ((Variable->CurrPtr->Attributes & EFI_VARIABLE_NON_VOLATILE) == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+
+      //
+      // Only variable that have RT attributes can be updated/deleted in Runtime.
+      //
+      if ((Variable->CurrPtr->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0) {
+        Status = EFI_INVALID_PARAMETER;
+        goto Done;
+      }
+    }
+
+    //
+    // Setting a data variable with no access, or zero DataSize attributes
+    // causes it to be deleted.
+    // When the EFI_VARIABLE_APPEND_WRITE attribute is set, DataSize of zero will
+    // not delete the variable.
+    //
+    if ((((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) && (DataSize == 0))|| ((Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == 0)) {
+      if (Variable->InDeletedTransitionPtr != NULL) {
+        //
+        // Both ADDED and IN_DELETED_TRANSITION variable are present,
+        // set IN_DELETED_TRANSITION one to DELETED state first.
+        //
+        State = Variable->InDeletedTransitionPtr->State;
+        State &= VAR_DELETED;
+        Status = UpdateVariableStore (
+                   &mVariableModuleGlobal->VariableGlobal,
+                   Variable->Volatile,
+                   FALSE,
+                   Fvb,
+                   (UINTN) &Variable->InDeletedTransitionPtr->State,
+                   sizeof (UINT8),
+                   &State
+                   );
+        if (!EFI_ERROR (Status)) {
+          if (!Variable->Volatile) {
+            ASSERT (CacheVariable->InDeletedTransitionPtr != NULL);
+            CacheVariable->InDeletedTransitionPtr->State = State;
+          }
+        } else {
+          goto Done;
+        }
+      }
+
+      State = Variable->CurrPtr->State;
+      State &= VAR_DELETED;
+
+      Status = UpdateVariableStore (
+                 &mVariableModuleGlobal->VariableGlobal,
+                 Variable->Volatile,
+                 FALSE,
+                 Fvb,
+                 (UINTN) &Variable->CurrPtr->State,
+                 sizeof (UINT8),
+                 &State
+                 );
+      if (!EFI_ERROR (Status)) {
+        UpdateVariableInfo (VariableName, VendorGuid, Variable->Volatile, FALSE, FALSE, TRUE, FALSE);
+        if (!Variable->Volatile) {
+          CacheVariable->CurrPtr->State = State;
+          FlushHobVariableToFlash (VariableName, VendorGuid);
+        }
+      }
+      goto Done;
+    }
+    //
+    // If the variable is marked valid, and the same data has been passed in,
+    // then return to the caller immediately.
+    //
+    if (DataSizeOfVariable (Variable->CurrPtr) == DataSize &&
+        (CompareMem (Data, GetVariableDataPtr (Variable->CurrPtr), DataSize) == 0) &&
+        ((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) &&
+        (TimeStamp == NULL)) {
+      //
+      // Variable content unchanged and no need to update timestamp, just return.
+      //
+      UpdateVariableInfo (VariableName, VendorGuid, Variable->Volatile, FALSE, TRUE, FALSE, FALSE);
+      Status = EFI_SUCCESS;
+      goto Done;
+    } else if ((Variable->CurrPtr->State == VAR_ADDED) ||
+               (Variable->CurrPtr->State == (VAR_ADDED & VAR_IN_DELETED_TRANSITION))) {
+
+      //
+      // EFI_VARIABLE_APPEND_WRITE attribute only effects for existing variable.
+      //
+      if ((Attributes & EFI_VARIABLE_APPEND_WRITE) != 0) {
+        //
+        // NOTE: From 0 to DataOffset of NextVariable is reserved for Variable Header and Name.
+        // From DataOffset of NextVariable is to save the existing variable data.
+        //
+        DataOffset = GetVariableDataOffset (Variable->CurrPtr);
+        BufferForMerge = (UINT8 *) ((UINTN) NextVariable + DataOffset);
+        CopyMem (BufferForMerge, (UINT8 *) ((UINTN) Variable->CurrPtr + DataOffset), DataSizeOfVariable (Variable->CurrPtr));
+
+        //
+        // Set Max Common/Auth Variable Data Size as default MaxDataSize.
+        //
+        if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+          MaxDataSize = mVariableModuleGlobal->MaxAuthVariableSize - DataOffset;
+        } else {
+          MaxDataSize = mVariableModuleGlobal->MaxVariableSize - DataOffset;
+        }
+
+        //
+        // Append the new data to the end of existing data.
+        // Max Harware error record variable data size is different from common/auth variable.
+        //
+        if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+          MaxDataSize = PcdGet32 (PcdMaxHardwareErrorVariableSize) - DataOffset;
+        }
+
+        if (DataSizeOfVariable (Variable->CurrPtr) + DataSize > MaxDataSize) {
+          //
+          // Existing data size + new data size exceed maximum variable size limitation.
+          //
+          Status = EFI_INVALID_PARAMETER;
+          goto Done;
+        }
+        CopyMem ((UINT8*) ((UINTN) BufferForMerge + DataSizeOfVariable (Variable->CurrPtr)), Data, DataSize);
+        MergedBufSize = DataSizeOfVariable (Variable->CurrPtr) + DataSize;
+
+        //
+        // BufferForMerge(from DataOffset of NextVariable) has included the merged existing and new data.
+        //
+        Data      = BufferForMerge;
+        DataSize  = MergedBufSize;
+        DataReady = TRUE;
+      }
+
+      //
+      // Mark the old variable as in delete transition.
+      //
+      State = Variable->CurrPtr->State;
+      State &= VAR_IN_DELETED_TRANSITION;
+
+      Status = UpdateVariableStore (
+                 &mVariableModuleGlobal->VariableGlobal,
+                 Variable->Volatile,
+                 FALSE,
+                 Fvb,
+                 (UINTN) &Variable->CurrPtr->State,
+                 sizeof (UINT8),
+                 &State
+                 );
+      if (EFI_ERROR (Status)) {
+        goto Done;
+      }
+      if (!Variable->Volatile) {
+        CacheVariable->CurrPtr->State = State;
+      }
+    }
+  } else {
+    //
+    // Not found existing variable. Create a new variable.
+    //
+
+    if ((DataSize == 0) && ((Attributes & EFI_VARIABLE_APPEND_WRITE) != 0)) {
+      Status = EFI_SUCCESS;
+      goto Done;
+    }
+
+    //
+    // Make sure we are trying to create a new variable.
+    // Setting a data variable with zero DataSize or no access attributes means to delete it.
+    //
+    if (DataSize == 0 || (Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == 0) {
+      Status = EFI_NOT_FOUND;
+      goto Done;
+    }
+
+    //
+    // Only variable have NV|RT attribute can be created in Runtime.
+    //
+    if (AtRuntime () &&
+        (((Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0) || ((Attributes & EFI_VARIABLE_NON_VOLATILE) == 0))) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+  }
+
+  //
+  // Function part - create a new variable and copy the data.
+  // Both update a variable and create a variable will come here.
+  //
+  NextVariable->StartId     = VARIABLE_DATA;
+  //
+  // NextVariable->State = VAR_ADDED;
+  //
+  NextVariable->Reserved        = 0;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) NextVariable;
+    AuthVariable->PubKeyIndex    = KeyIndex;
+    AuthVariable->MonotonicCount = MonotonicCount;
+    ZeroMem (&AuthVariable->TimeStamp, sizeof (EFI_TIME));
+
+    if (((Attributes & EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS) != 0) &&
+        (TimeStamp != NULL)) {
+      if ((Attributes & EFI_VARIABLE_APPEND_WRITE) == 0) {
+        CopyMem (&AuthVariable->TimeStamp, TimeStamp, sizeof (EFI_TIME));
+      } else {
+        //
+        // In the case when the EFI_VARIABLE_APPEND_WRITE attribute is set, only
+        // when the new TimeStamp value is later than the current timestamp associated
+        // with the variable, we need associate the new timestamp with the updated value.
+        //
+        if (Variable->CurrPtr != NULL) {
+          if (VariableCompareTimeStampInternal (&(((AUTHENTICATED_VARIABLE_HEADER *) Variable->CurrPtr)->TimeStamp), TimeStamp)) {
+            CopyMem (&AuthVariable->TimeStamp, TimeStamp, sizeof (EFI_TIME));
+          }
+        }
+      }
+    }
+  }
+
+  //
+  // The EFI_VARIABLE_APPEND_WRITE attribute will never be set in the returned
+  // Attributes bitmask parameter of a GetVariable() call.
+  //
+  NextVariable->Attributes  = Attributes & (~EFI_VARIABLE_APPEND_WRITE);
+
+  VarNameOffset                 = GetVariableHeaderSize ();
+  VarNameSize                   = StrSize (VariableName);
+  CopyMem (
+    (UINT8 *) ((UINTN) NextVariable + VarNameOffset),
+    VariableName,
+    VarNameSize
+    );
+  VarDataOffset = VarNameOffset + VarNameSize + GET_PAD_SIZE (VarNameSize);
+
+  //
+  // If DataReady is TRUE, it means the variable data has been saved into
+  // NextVariable during EFI_VARIABLE_APPEND_WRITE operation preparation.
+  //
+  if (!DataReady) {
+    CopyMem (
+      (UINT8 *) ((UINTN) NextVariable + VarDataOffset),
+      Data,
+      DataSize
+      );
+  }
+
+  CopyMem (GetVendorGuidPtr (NextVariable), VendorGuid, sizeof (EFI_GUID));
+  //
+  // There will be pad bytes after Data, the NextVariable->NameSize and
+  // NextVariable->DataSize should not include pad size so that variable
+  // service can get actual size in GetVariable.
+  //
+  SetNameSizeOfVariable (NextVariable, VarNameSize);
+  SetDataSizeOfVariable (NextVariable, DataSize);
+
+  //
+  // The actual size of the variable that stores in storage should
+  // include pad size.
+  //
+  VarSize = VarDataOffset + DataSize + GET_PAD_SIZE (DataSize);
+  if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+    //
+    // Create a nonvolatile variable.
+    //
+    Volatile = FALSE;
+
+    IsCommonVariable = FALSE;
+    IsCommonUserVariable = FALSE;
+    if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == 0) {
+      IsCommonVariable = TRUE;
+      IsCommonUserVariable = IsUserVariable (NextVariable);
+    }
+    if ((((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0)
+      && ((VarSize + mVariableModuleGlobal->HwErrVariableTotalSize) > PcdGet32 (PcdHwErrStorageSize)))
+      || (IsCommonVariable && ((VarSize + mVariableModuleGlobal->CommonVariableTotalSize) > mVariableModuleGlobal->CommonVariableSpace))
+      || (IsCommonVariable && AtRuntime () && ((VarSize + mVariableModuleGlobal->CommonVariableTotalSize) > mVariableModuleGlobal->CommonRuntimeVariableSpace))
+      || (IsCommonUserVariable && ((VarSize + mVariableModuleGlobal->CommonUserVariableTotalSize) > mVariableModuleGlobal->CommonMaxUserVariableSpace))) {
+      if (AtRuntime ()) {
+        if (IsCommonUserVariable && ((VarSize + mVariableModuleGlobal->CommonUserVariableTotalSize) > mVariableModuleGlobal->CommonMaxUserVariableSpace)) {
+          RecordVarErrorFlag (VAR_ERROR_FLAG_USER_ERROR, VariableName, VendorGuid, Attributes, VarSize);
+        }
+        if (IsCommonVariable && ((VarSize + mVariableModuleGlobal->CommonVariableTotalSize) > mVariableModuleGlobal->CommonRuntimeVariableSpace)) {
+          RecordVarErrorFlag (VAR_ERROR_FLAG_SYSTEM_ERROR, VariableName, VendorGuid, Attributes, VarSize);
+        }
+        Status = EFI_OUT_OF_RESOURCES;
+        goto Done;
+      }
+      //
+      // Perform garbage collection & reclaim operation, and integrate the new variable at the same time.
+      //
+      Status = Reclaim (
+                 mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase,
+                 &mVariableModuleGlobal->NonVolatileLastVariableOffset,
+                 FALSE,
+                 Variable,
+                 NextVariable,
+                 HEADER_ALIGN (VarSize)
+                 );
+      if (!EFI_ERROR (Status)) {
+        //
+        // The new variable has been integrated successfully during reclaiming.
+        //
+        if (Variable->CurrPtr != NULL) {
+          CacheVariable->CurrPtr = (VARIABLE_HEADER *)((UINTN) CacheVariable->StartPtr + ((UINTN) Variable->CurrPtr - (UINTN) Variable->StartPtr));
+          CacheVariable->InDeletedTransitionPtr = NULL;
+        }
+        UpdateVariableInfo (VariableName, VendorGuid, FALSE, FALSE, TRUE, FALSE, FALSE);
+        FlushHobVariableToFlash (VariableName, VendorGuid);
+      } else {
+        if (IsCommonUserVariable && ((VarSize + mVariableModuleGlobal->CommonUserVariableTotalSize) > mVariableModuleGlobal->CommonMaxUserVariableSpace)) {
+          RecordVarErrorFlag (VAR_ERROR_FLAG_USER_ERROR, VariableName, VendorGuid, Attributes, VarSize);
+        }
+        if (IsCommonVariable && ((VarSize + mVariableModuleGlobal->CommonVariableTotalSize) > mVariableModuleGlobal->CommonVariableSpace)) {
+          RecordVarErrorFlag (VAR_ERROR_FLAG_SYSTEM_ERROR, VariableName, VendorGuid, Attributes, VarSize);
+        }
+      }
+      goto Done;
+    }
+    //
+    // Four steps
+    // 1. Write variable header
+    // 2. Set variable state to header valid
+    // 3. Write variable data
+    // 4. Set variable state to valid
+    //
+    //
+    // Step 1:
+    //
+    CacheOffset = mVariableModuleGlobal->NonVolatileLastVariableOffset;
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               FALSE,
+               TRUE,
+               Fvb,
+               mVariableModuleGlobal->NonVolatileLastVariableOffset,
+               (UINT32) GetVariableHeaderSize (),
+               (UINT8 *) NextVariable
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    //
+    // Step 2:
+    //
+    NextVariable->State = VAR_HEADER_VALID_ONLY;
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               FALSE,
+               TRUE,
+               Fvb,
+               mVariableModuleGlobal->NonVolatileLastVariableOffset + OFFSET_OF (VARIABLE_HEADER, State),
+               sizeof (UINT8),
+               &NextVariable->State
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    //
+    // Step 3:
+    //
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               FALSE,
+               TRUE,
+               Fvb,
+               mVariableModuleGlobal->NonVolatileLastVariableOffset + GetVariableHeaderSize (),
+               (UINT32) (VarSize - GetVariableHeaderSize ()),
+               (UINT8 *) NextVariable + GetVariableHeaderSize ()
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+    //
+    // Step 4:
+    //
+    NextVariable->State = VAR_ADDED;
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               FALSE,
+               TRUE,
+               Fvb,
+               mVariableModuleGlobal->NonVolatileLastVariableOffset + OFFSET_OF (VARIABLE_HEADER, State),
+               sizeof (UINT8),
+               &NextVariable->State
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    mVariableModuleGlobal->NonVolatileLastVariableOffset += HEADER_ALIGN (VarSize);
+
+    if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0) {
+      mVariableModuleGlobal->HwErrVariableTotalSize += HEADER_ALIGN (VarSize);
+    } else {
+      mVariableModuleGlobal->CommonVariableTotalSize += HEADER_ALIGN (VarSize);
+      if (IsCommonUserVariable) {
+        mVariableModuleGlobal->CommonUserVariableTotalSize += HEADER_ALIGN (VarSize);
+      }
+    }
+    //
+    // update the memory copy of Flash region.
+    //
+    CopyMem ((UINT8 *)mNvVariableCache + CacheOffset, (UINT8 *)NextVariable, VarSize);
+  } else {
+    //
+    // Create a volatile variable.
+    //
+    Volatile = TRUE;
+
+    if ((UINT32) (VarSize + mVariableModuleGlobal->VolatileLastVariableOffset) >
+        ((VARIABLE_STORE_HEADER *) ((UINTN) (mVariableModuleGlobal->VariableGlobal.VolatileVariableBase)))->Size) {
+      //
+      // Perform garbage collection & reclaim operation, and integrate the new variable at the same time.
+      //
+      Status = Reclaim (
+                 mVariableModuleGlobal->VariableGlobal.VolatileVariableBase,
+                 &mVariableModuleGlobal->VolatileLastVariableOffset,
+                 TRUE,
+                 Variable,
+                 NextVariable,
+                 HEADER_ALIGN (VarSize)
+                 );
+      if (!EFI_ERROR (Status)) {
+        //
+        // The new variable has been integrated successfully during reclaiming.
+        //
+        if (Variable->CurrPtr != NULL) {
+          CacheVariable->CurrPtr = (VARIABLE_HEADER *)((UINTN) CacheVariable->StartPtr + ((UINTN) Variable->CurrPtr - (UINTN) Variable->StartPtr));
+          CacheVariable->InDeletedTransitionPtr = NULL;
+        }
+        UpdateVariableInfo (VariableName, VendorGuid, TRUE, FALSE, TRUE, FALSE, FALSE);
+      }
+      goto Done;
+    }
+
+    NextVariable->State = VAR_ADDED;
+    Status = UpdateVariableStore (
+               &mVariableModuleGlobal->VariableGlobal,
+               TRUE,
+               TRUE,
+               Fvb,
+               mVariableModuleGlobal->VolatileLastVariableOffset,
+               (UINT32) VarSize,
+               (UINT8 *) NextVariable
+               );
+
+    if (EFI_ERROR (Status)) {
+      goto Done;
+    }
+
+    mVariableModuleGlobal->VolatileLastVariableOffset += HEADER_ALIGN (VarSize);
+  }
+
+  //
+  // Mark the old variable as deleted.
+  //
+  if (!EFI_ERROR (Status) && Variable->CurrPtr != NULL) {
+    if (Variable->InDeletedTransitionPtr != NULL) {
+      //
+      // Both ADDED and IN_DELETED_TRANSITION old variable are present,
+      // set IN_DELETED_TRANSITION one to DELETED state first.
+      //
+      State = Variable->InDeletedTransitionPtr->State;
+      State &= VAR_DELETED;
+      Status = UpdateVariableStore (
+                 &mVariableModuleGlobal->VariableGlobal,
+                 Variable->Volatile,
+                 FALSE,
+                 Fvb,
+                 (UINTN) &Variable->InDeletedTransitionPtr->State,
+                 sizeof (UINT8),
+                 &State
+                 );
+      if (!EFI_ERROR (Status)) {
+        if (!Variable->Volatile) {
+          ASSERT (CacheVariable->InDeletedTransitionPtr != NULL);
+          CacheVariable->InDeletedTransitionPtr->State = State;
+        }
+      } else {
+        goto Done;
+      }
+    }
+
+    State = Variable->CurrPtr->State;
+    State &= VAR_DELETED;
+
+    Status = UpdateVariableStore (
+             &mVariableModuleGlobal->VariableGlobal,
+             Variable->Volatile,
+             FALSE,
+             Fvb,
+             (UINTN) &Variable->CurrPtr->State,
+             sizeof (UINT8),
+             &State
+             );
+    if (!EFI_ERROR (Status) && !Variable->Volatile) {
+      CacheVariable->CurrPtr->State = State;
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    UpdateVariableInfo (VariableName, VendorGuid, Volatile, FALSE, TRUE, FALSE, FALSE);
+    if (!Volatile) {
+      FlushHobVariableToFlash (VariableName, VendorGuid);
+    }
+  }
+
+Done:
+  return Status;
+}
+
+/**
+
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode, and datasize is external input.
+  This function will do basic validation, before parse the data.
+
+  @param VariableName               Name of Variable to be found.
+  @param VendorGuid                 Variable vendor GUID.
+  @param Attributes                 Attribute value of the variable found.
+  @param DataSize                   Size of Data found. If size is less than the
+                                    data, this value contains the required size.
+  @param Data                       Data pointer.
+
+  @return EFI_INVALID_PARAMETER     Invalid parameter.
+  @return EFI_SUCCESS               Find the specified variable.
+  @return EFI_NOT_FOUND             Not found.
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetVariable (
+  IN      CHAR16            *VariableName,
+  IN      EFI_GUID          *VendorGuid,
+  OUT     UINT32            *Attributes OPTIONAL,
+  IN OUT  UINTN             *DataSize,
+  OUT     VOID              *Data
+  )
+{
+  EFI_STATUS              Status;
+  VARIABLE_POINTER_TRACK  Variable;
+  UINTN                   VarDataSize;
+
+  if (VariableName == NULL || VendorGuid == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  Status = FindVariable (VariableName, VendorGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+  if (Variable.CurrPtr == NULL || EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Get data size
+  //
+  VarDataSize = DataSizeOfVariable (Variable.CurrPtr);
+  ASSERT (VarDataSize != 0);
+
+  if (*DataSize >= VarDataSize) {
+    if (Data == NULL) {
+      Status = EFI_INVALID_PARAMETER;
+      goto Done;
+    }
+
+    CopyMem (Data, GetVariableDataPtr (Variable.CurrPtr), VarDataSize);
+    if (Attributes != NULL) {
+      *Attributes = Variable.CurrPtr->Attributes;
+    }
+
+    *DataSize = VarDataSize;
+    UpdateVariableInfo (VariableName, VendorGuid, Variable.Volatile, TRUE, FALSE, FALSE, FALSE);
+
+    Status = EFI_SUCCESS;
+    goto Done;
+  } else {
+    *DataSize = VarDataSize;
+    Status = EFI_BUFFER_TOO_SMALL;
+    goto Done;
+  }
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  return Status;
+}
+
+/**
+  This code Finds the Next available variable.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param[in]  VariableName  Pointer to variable name.
+  @param[in]  VendorGuid    Variable Vendor Guid.
+  @param[out] VariablePtr   Pointer to variable header address.
+
+  @return EFI_SUCCESS       Find the specified variable.
+  @return EFI_NOT_FOUND     Not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetNextVariableInternal (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT VARIABLE_HEADER       **VariablePtr
+  )
+{
+  VARIABLE_STORE_TYPE     Type;
+  VARIABLE_POINTER_TRACK  Variable;
+  VARIABLE_POINTER_TRACK  VariableInHob;
+  VARIABLE_POINTER_TRACK  VariablePtrTrack;
+  EFI_STATUS              Status;
+  VARIABLE_STORE_HEADER   *VariableStoreHeader[VariableStoreTypeMax];
+
+  Status = FindVariable (VariableName, VendorGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+  if (Variable.CurrPtr == NULL || EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  if (VariableName[0] != 0) {
+    //
+    // If variable name is not NULL, get next variable.
+    //
+    Variable.CurrPtr = GetNextVariablePtr (Variable.CurrPtr);
+  }
+
+  //
+  // 0: Volatile, 1: HOB, 2: Non-Volatile.
+  // The index and attributes mapping must be kept in this order as FindVariable
+  // makes use of this mapping to implement search algorithm.
+  //
+  VariableStoreHeader[VariableStoreTypeVolatile] = (VARIABLE_STORE_HEADER *) (UINTN) mVariableModuleGlobal->VariableGlobal.VolatileVariableBase;
+  VariableStoreHeader[VariableStoreTypeHob]      = (VARIABLE_STORE_HEADER *) (UINTN) mVariableModuleGlobal->VariableGlobal.HobVariableBase;
+  VariableStoreHeader[VariableStoreTypeNv]       = mNvVariableCache;
+
+  while (TRUE) {
+    //
+    // Switch from Volatile to HOB, to Non-Volatile.
+    //
+    while (!IsValidVariableHeader (Variable.CurrPtr, Variable.EndPtr)) {
+      //
+      // Find current storage index
+      //
+      for (Type = (VARIABLE_STORE_TYPE) 0; Type < VariableStoreTypeMax; Type++) {
+        if ((VariableStoreHeader[Type] != NULL) && (Variable.StartPtr == GetStartPointer (VariableStoreHeader[Type]))) {
+          break;
+        }
+      }
+      ASSERT (Type < VariableStoreTypeMax);
+      //
+      // Switch to next storage
+      //
+      for (Type++; Type < VariableStoreTypeMax; Type++) {
+        if (VariableStoreHeader[Type] != NULL) {
+          break;
+        }
+      }
+      //
+      // Capture the case that
+      // 1. current storage is the last one, or
+      // 2. no further storage
+      //
+      if (Type == VariableStoreTypeMax) {
+        Status = EFI_NOT_FOUND;
+        goto Done;
+      }
+      Variable.StartPtr = GetStartPointer (VariableStoreHeader[Type]);
+      Variable.EndPtr   = GetEndPointer   (VariableStoreHeader[Type]);
+      Variable.CurrPtr  = Variable.StartPtr;
+    }
+
+    //
+    // Variable is found
+    //
+    if (Variable.CurrPtr->State == VAR_ADDED || Variable.CurrPtr->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+      if (!AtRuntime () || ((Variable.CurrPtr->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) != 0)) {
+        if (Variable.CurrPtr->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+          //
+          // If it is a IN_DELETED_TRANSITION variable,
+          // and there is also a same ADDED one at the same time,
+          // don't return it.
+          //
+          VariablePtrTrack.StartPtr = Variable.StartPtr;
+          VariablePtrTrack.EndPtr = Variable.EndPtr;
+          Status = FindVariableEx (
+                     GetVariableNamePtr (Variable.CurrPtr),
+                     GetVendorGuidPtr (Variable.CurrPtr),
+                     FALSE,
+                     &VariablePtrTrack
+                     );
+          if (!EFI_ERROR (Status) && VariablePtrTrack.CurrPtr->State == VAR_ADDED) {
+            Variable.CurrPtr = GetNextVariablePtr (Variable.CurrPtr);
+            continue;
+          }
+        }
+
+        //
+        // Don't return NV variable when HOB overrides it
+        //
+        if ((VariableStoreHeader[VariableStoreTypeHob] != NULL) && (VariableStoreHeader[VariableStoreTypeNv] != NULL) &&
+            (Variable.StartPtr == GetStartPointer (VariableStoreHeader[VariableStoreTypeNv]))
+           ) {
+          VariableInHob.StartPtr = GetStartPointer (VariableStoreHeader[VariableStoreTypeHob]);
+          VariableInHob.EndPtr   = GetEndPointer   (VariableStoreHeader[VariableStoreTypeHob]);
+          Status = FindVariableEx (
+                     GetVariableNamePtr (Variable.CurrPtr),
+                     GetVendorGuidPtr (Variable.CurrPtr),
+                     FALSE,
+                     &VariableInHob
+                     );
+          if (!EFI_ERROR (Status)) {
+            Variable.CurrPtr = GetNextVariablePtr (Variable.CurrPtr);
+            continue;
+          }
+        }
+
+        *VariablePtr = Variable.CurrPtr;
+        Status = EFI_SUCCESS;
+        goto Done;
+      }
+    }
+
+    Variable.CurrPtr = GetNextVariablePtr (Variable.CurrPtr);
+  }
+
+Done:
+  return Status;
+}
+
+/**
+
+  This code Finds the Next available variable.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param VariableNameSize           Size of the variable name.
+  @param VariableName               Pointer to variable name.
+  @param VendorGuid                 Variable Vendor Guid.
+
+  @return EFI_INVALID_PARAMETER     Invalid parameter.
+  @return EFI_SUCCESS               Find the specified variable.
+  @return EFI_NOT_FOUND             Not found.
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetNextVariableName (
+  IN OUT  UINTN             *VariableNameSize,
+  IN OUT  CHAR16            *VariableName,
+  IN OUT  EFI_GUID          *VendorGuid
+  )
+{
+  EFI_STATUS              Status;
+  UINTN                   VarNameSize;
+  VARIABLE_HEADER         *VariablePtr;
+
+  if (VariableNameSize == NULL || VariableName == NULL || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  Status = VariableServiceGetNextVariableInternal (VariableName, VendorGuid, &VariablePtr);
+  if (!EFI_ERROR (Status)) {
+    VarNameSize = NameSizeOfVariable (VariablePtr);
+    ASSERT (VarNameSize != 0);
+    if (VarNameSize <= *VariableNameSize) {
+      CopyMem (VariableName, GetVariableNamePtr (VariablePtr), VarNameSize);
+      CopyMem (VendorGuid, GetVendorGuidPtr (VariablePtr), sizeof (EFI_GUID));
+      Status = EFI_SUCCESS;
+    } else {
+      Status = EFI_BUFFER_TOO_SMALL;
+    }
+
+    *VariableNameSize = VarNameSize;
+  }
+
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  return Status;
+}
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode, and datasize and data are external input.
+  This function will do basic validation, before parse the data.
+  This function will parse the authentication carefully to avoid security issues, like
+  buffer overflow, integer overflow.
+  This function will check attribute carefully to avoid authentication bypass.
+
+  @param VariableName                     Name of Variable to be found.
+  @param VendorGuid                       Variable vendor GUID.
+  @param Attributes                       Attribute value of the variable found
+  @param DataSize                         Size of Data found. If size is less than the
+                                          data, this value contains the required size.
+  @param Data                             Data pointer.
+
+  @return EFI_INVALID_PARAMETER           Invalid parameter.
+  @return EFI_SUCCESS                     Set successfully.
+  @return EFI_OUT_OF_RESOURCES            Resource not enough to set variable.
+  @return EFI_NOT_FOUND                   Not found.
+  @return EFI_WRITE_PROTECTED             Variable is read-only.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceSetVariable (
+  IN CHAR16                  *VariableName,
+  IN EFI_GUID                *VendorGuid,
+  IN UINT32                  Attributes,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data
+  )
+{
+  VARIABLE_POINTER_TRACK              Variable;
+  EFI_STATUS                          Status;
+  VARIABLE_HEADER                     *NextVariable;
+  EFI_PHYSICAL_ADDRESS                Point;
+  UINTN                               PayloadSize;
+
+  //
+  // Check input parameters.
+  //
+  if (VariableName == NULL || VariableName[0] == 0 || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataSize != 0 && Data == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check for reserverd bit in variable attribute.
+  //
+  if ((Attributes & (~EFI_VARIABLE_ATTRIBUTES_MASK)) != 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  Make sure if runtime bit is set, boot service bit is set also.
+  //
+  if ((Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == EFI_VARIABLE_RUNTIME_ACCESS) {
+    return EFI_INVALID_PARAMETER;
+  } else if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+    if (!mVariableModuleGlobal->VariableGlobal.AuthSupport) {
+      //
+      // Not support authenticated variable write.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+  } else if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0) {
+    if (PcdGet32 (PcdHwErrStorageSize) == 0) {
+      //
+      // Not support harware error record variable variable.
+      //
+      return EFI_INVALID_PARAMETER;
+    }
+  }
+
+  //
+  // EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS and EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS attribute
+  // cannot be set both.
+  //
+  if (((Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) == EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS)
+     && ((Attributes & EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS) == EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Attributes & EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) == EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS) {
+    if (DataSize < AUTHINFO_SIZE) {
+      //
+      // Try to write Authenticated Variable without AuthInfo.
+      //
+      return EFI_SECURITY_VIOLATION;
+    }
+    PayloadSize = DataSize - AUTHINFO_SIZE;
+  } else if ((Attributes & EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS) == EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS) {
+    //
+    // Sanity check for EFI_VARIABLE_AUTHENTICATION_2 descriptor.
+    //
+    if (DataSize < OFFSET_OF_AUTHINFO2_CERT_DATA ||
+      ((EFI_VARIABLE_AUTHENTICATION_2 *) Data)->AuthInfo.Hdr.dwLength > DataSize - (OFFSET_OF (EFI_VARIABLE_AUTHENTICATION_2, AuthInfo)) ||
+      ((EFI_VARIABLE_AUTHENTICATION_2 *) Data)->AuthInfo.Hdr.dwLength < OFFSET_OF (WIN_CERTIFICATE_UEFI_GUID, CertData)) {
+      return EFI_SECURITY_VIOLATION;
+    }
+    PayloadSize = DataSize - AUTHINFO2_SIZE (Data);
+  } else {
+    PayloadSize = DataSize;
+  }
+
+  if ((UINTN)(~0) - PayloadSize < StrSize(VariableName)){
+    //
+    // Prevent whole variable size overflow
+    //
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  //  The size of the VariableName, including the Unicode Null in bytes plus
+  //  the DataSize is limited to maximum size of PcdGet32 (PcdMaxHardwareErrorVariableSize)
+  //  bytes for HwErrRec#### variable.
+  //
+  if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+    if (StrSize (VariableName) + PayloadSize > PcdGet32 (PcdMaxHardwareErrorVariableSize) - GetVariableHeaderSize ()) {
+      return EFI_INVALID_PARAMETER;
+    }
+  } else {
+    //
+    //  The size of the VariableName, including the Unicode Null in bytes plus
+    //  the DataSize is limited to maximum size of Max(Auth)VariableSize bytes.
+    //
+    if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+      if (StrSize (VariableName) + PayloadSize > mVariableModuleGlobal->MaxAuthVariableSize - GetVariableHeaderSize ()) {
+        return EFI_INVALID_PARAMETER;
+      }
+    } else {
+      if (StrSize (VariableName) + PayloadSize > mVariableModuleGlobal->MaxVariableSize - GetVariableHeaderSize ()) {
+        return EFI_INVALID_PARAMETER;
+      }
+    }
+  }
+
+  //
+  // Special Handling for MOR Lock variable.
+  //
+  Status = SetVariableCheckHandlerMor (VariableName, VendorGuid, Attributes, PayloadSize, (VOID *) ((UINTN) Data + DataSize - PayloadSize));
+  if (Status == EFI_ALREADY_STARTED) {
+    //
+    // EFI_ALREADY_STARTED means the SetVariable() action is handled inside of SetVariableCheckHandlerMor().
+    // Variable driver can just return SUCCESS.
+    //
+    return EFI_SUCCESS;
+  }
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = VarCheckLibSetVariableCheck (VariableName, VendorGuid, Attributes, PayloadSize, (VOID *) ((UINTN) Data + DataSize - PayloadSize), mRequestSource);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  //
+  // Consider reentrant in MCA/INIT/NMI. It needs be reupdated.
+  //
+  if (1 < InterlockedIncrement (&mVariableModuleGlobal->VariableGlobal.ReentrantState)) {
+    Point = mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase;
+    //
+    // Parse non-volatile variable data and get last variable offset.
+    //
+    NextVariable  = GetStartPointer ((VARIABLE_STORE_HEADER *) (UINTN) Point);
+    while (IsValidVariableHeader (NextVariable, GetEndPointer ((VARIABLE_STORE_HEADER *) (UINTN) Point))) {
+      NextVariable = GetNextVariablePtr (NextVariable);
+    }
+    mVariableModuleGlobal->NonVolatileLastVariableOffset = (UINTN) NextVariable - (UINTN) Point;
+  }
+
+  //
+  // Check whether the input variable is already existed.
+  //
+  Status = FindVariable (VariableName, VendorGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, TRUE);
+  if (!EFI_ERROR (Status)) {
+    if (((Variable.CurrPtr->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0) && AtRuntime ()) {
+      Status = EFI_WRITE_PROTECTED;
+      goto Done;
+    }
+    if (Attributes != 0 && (Attributes & (~EFI_VARIABLE_APPEND_WRITE)) != Variable.CurrPtr->Attributes) {
+      //
+      // If a preexisting variable is rewritten with different attributes, SetVariable() shall not
+      // modify the variable and shall return EFI_INVALID_PARAMETER. Two exceptions to this rule:
+      // 1. No access attributes specified
+      // 2. The only attribute differing is EFI_VARIABLE_APPEND_WRITE
+      //
+      Status = EFI_INVALID_PARAMETER;
+      DEBUG ((EFI_D_INFO, "[Variable]: Rewritten a preexisting variable(0x%08x) with different attributes(0x%08x) - %g:%s\n", Variable.CurrPtr->Attributes, Attributes, VendorGuid, VariableName));
+      goto Done;
+    }
+  }
+
+  if (!FeaturePcdGet (PcdUefiVariableDefaultLangDeprecate)) {
+    //
+    // Hook the operation of setting PlatformLangCodes/PlatformLang and LangCodes/Lang.
+    //
+    Status = AutoUpdateLangVariable (VariableName, Data, DataSize);
+    if (EFI_ERROR (Status)) {
+      //
+      // The auto update operation failed, directly return to avoid inconsistency between PlatformLang and Lang.
+      //
+      goto Done;
+    }
+  }
+
+  if (mVariableModuleGlobal->VariableGlobal.AuthSupport) {
+    Status = AuthVariableLibProcessVariable (VariableName, VendorGuid, Data, DataSize, Attributes);
+  } else {
+    Status = UpdateVariable (VariableName, VendorGuid, Data, DataSize, Attributes, 0, 0, &Variable, NULL);
+  }
+
+Done:
+  InterlockedDecrement (&mVariableModuleGlobal->VariableGlobal.ReentrantState);
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  if (!AtRuntime ()) {
+    if (!EFI_ERROR (Status)) {
+      SecureBootHook (
+        VariableName,
+        VendorGuid
+        );
+    }
+  }
+
+  return Status;
+}
+
+/**
+
+  This code returns information about the EFI variables.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param Attributes                     Attributes bitmask to specify the type of variables
+                                        on which to return information.
+  @param MaximumVariableStorageSize     Pointer to the maximum size of the storage space available
+                                        for the EFI variables associated with the attributes specified.
+  @param RemainingVariableStorageSize   Pointer to the remaining size of the storage space available
+                                        for EFI variables associated with the attributes specified.
+  @param MaximumVariableSize            Pointer to the maximum size of an individual EFI variables
+                                        associated with the attributes specified.
+
+  @return EFI_SUCCESS                   Query successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceQueryVariableInfoInternal (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize
+  )
+{
+  VARIABLE_HEADER        *Variable;
+  VARIABLE_HEADER        *NextVariable;
+  UINT64                 VariableSize;
+  VARIABLE_STORE_HEADER  *VariableStoreHeader;
+  UINT64                 CommonVariableTotalSize;
+  UINT64                 HwErrVariableTotalSize;
+  EFI_STATUS             Status;
+  VARIABLE_POINTER_TRACK VariablePtrTrack;
+
+  CommonVariableTotalSize = 0;
+  HwErrVariableTotalSize = 0;
+
+  if((Attributes & EFI_VARIABLE_NON_VOLATILE) == 0) {
+    //
+    // Query is Volatile related.
+    //
+    VariableStoreHeader = (VARIABLE_STORE_HEADER *) ((UINTN) mVariableModuleGlobal->VariableGlobal.VolatileVariableBase);
+  } else {
+    //
+    // Query is Non-Volatile related.
+    //
+    VariableStoreHeader = mNvVariableCache;
+  }
+
+  //
+  // Now let's fill *MaximumVariableStorageSize *RemainingVariableStorageSize
+  // with the storage size (excluding the storage header size).
+  //
+  *MaximumVariableStorageSize   = VariableStoreHeader->Size - sizeof (VARIABLE_STORE_HEADER);
+
+  //
+  // Harware error record variable needs larger size.
+  //
+  if ((Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+    *MaximumVariableStorageSize = PcdGet32 (PcdHwErrStorageSize);
+    *MaximumVariableSize = PcdGet32 (PcdMaxHardwareErrorVariableSize) - GetVariableHeaderSize ();
+  } else {
+    if ((Attributes & EFI_VARIABLE_NON_VOLATILE) != 0) {
+      if (AtRuntime ()) {
+        *MaximumVariableStorageSize = mVariableModuleGlobal->CommonRuntimeVariableSpace;
+      } else {
+        *MaximumVariableStorageSize = mVariableModuleGlobal->CommonVariableSpace;
+      }
+    }
+
+    //
+    // Let *MaximumVariableSize be Max(Auth)VariableSize with the exception of the variable header size.
+    //
+    if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+      *MaximumVariableSize = mVariableModuleGlobal->MaxAuthVariableSize - GetVariableHeaderSize ();
+    } else {
+      *MaximumVariableSize = mVariableModuleGlobal->MaxVariableSize - GetVariableHeaderSize ();
+    }
+  }
+
+  //
+  // Point to the starting address of the variables.
+  //
+  Variable = GetStartPointer (VariableStoreHeader);
+
+  //
+  // Now walk through the related variable store.
+  //
+  while (IsValidVariableHeader (Variable, GetEndPointer (VariableStoreHeader))) {
+    NextVariable = GetNextVariablePtr (Variable);
+    VariableSize = (UINT64) (UINTN) NextVariable - (UINT64) (UINTN) Variable;
+
+    if (AtRuntime ()) {
+      //
+      // We don't take the state of the variables in mind
+      // when calculating RemainingVariableStorageSize,
+      // since the space occupied by variables not marked with
+      // VAR_ADDED is not allowed to be reclaimed in Runtime.
+      //
+      if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+        HwErrVariableTotalSize += VariableSize;
+      } else {
+        CommonVariableTotalSize += VariableSize;
+      }
+    } else {
+      //
+      // Only care about Variables with State VAR_ADDED, because
+      // the space not marked as VAR_ADDED is reclaimable now.
+      //
+      if (Variable->State == VAR_ADDED) {
+        if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+          HwErrVariableTotalSize += VariableSize;
+        } else {
+          CommonVariableTotalSize += VariableSize;
+        }
+      } else if (Variable->State == (VAR_IN_DELETED_TRANSITION & VAR_ADDED)) {
+        //
+        // If it is a IN_DELETED_TRANSITION variable,
+        // and there is not also a same ADDED one at the same time,
+        // this IN_DELETED_TRANSITION variable is valid.
+        //
+        VariablePtrTrack.StartPtr = GetStartPointer (VariableStoreHeader);
+        VariablePtrTrack.EndPtr   = GetEndPointer   (VariableStoreHeader);
+        Status = FindVariableEx (
+                   GetVariableNamePtr (Variable),
+                   GetVendorGuidPtr (Variable),
+                   FALSE,
+                   &VariablePtrTrack
+                   );
+        if (!EFI_ERROR (Status) && VariablePtrTrack.CurrPtr->State != VAR_ADDED) {
+          if ((Variable->Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+            HwErrVariableTotalSize += VariableSize;
+          } else {
+            CommonVariableTotalSize += VariableSize;
+          }
+        }
+      }
+    }
+
+    //
+    // Go to the next one.
+    //
+    Variable = NextVariable;
+  }
+
+  if ((Attributes  & EFI_VARIABLE_HARDWARE_ERROR_RECORD) == EFI_VARIABLE_HARDWARE_ERROR_RECORD){
+    *RemainingVariableStorageSize = *MaximumVariableStorageSize - HwErrVariableTotalSize;
+  } else {
+    if (*MaximumVariableStorageSize < CommonVariableTotalSize) {
+      *RemainingVariableStorageSize = 0;
+    } else {
+      *RemainingVariableStorageSize = *MaximumVariableStorageSize - CommonVariableTotalSize;
+    }
+  }
+
+  if (*RemainingVariableStorageSize < GetVariableHeaderSize ()) {
+    *MaximumVariableSize = 0;
+  } else if ((*RemainingVariableStorageSize - GetVariableHeaderSize ()) < *MaximumVariableSize) {
+    *MaximumVariableSize = *RemainingVariableStorageSize - GetVariableHeaderSize ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+
+  This code returns information about the EFI variables.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param Attributes                     Attributes bitmask to specify the type of variables
+                                        on which to return information.
+  @param MaximumVariableStorageSize     Pointer to the maximum size of the storage space available
+                                        for the EFI variables associated with the attributes specified.
+  @param RemainingVariableStorageSize   Pointer to the remaining size of the storage space available
+                                        for EFI variables associated with the attributes specified.
+  @param MaximumVariableSize            Pointer to the maximum size of an individual EFI variables
+                                        associated with the attributes specified.
+
+  @return EFI_INVALID_PARAMETER         An invalid combination of attribute bits was supplied.
+  @return EFI_SUCCESS                   Query successfully.
+  @return EFI_UNSUPPORTED               The attribute is not supported on this platform.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceQueryVariableInfo (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize
+  )
+{
+  EFI_STATUS             Status;
+
+  if(MaximumVariableStorageSize == NULL || RemainingVariableStorageSize == NULL || MaximumVariableSize == NULL || Attributes == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if ((Attributes & EFI_VARIABLE_ATTRIBUTES_MASK) == 0) {
+    //
+    // Make sure the Attributes combination is supported by the platform.
+    //
+    return EFI_UNSUPPORTED;
+  } else if ((Attributes & (EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS)) == EFI_VARIABLE_RUNTIME_ACCESS) {
+    //
+    // Make sure if runtime bit is set, boot service bit is set also.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if (AtRuntime () && ((Attributes & EFI_VARIABLE_RUNTIME_ACCESS) == 0)) {
+    //
+    // Make sure RT Attribute is set if we are in Runtime phase.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if ((Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == EFI_VARIABLE_HARDWARE_ERROR_RECORD) {
+    //
+    // Make sure Hw Attribute is set with NV.
+    //
+    return EFI_INVALID_PARAMETER;
+  } else if ((Attributes & VARIABLE_ATTRIBUTE_AT_AW) != 0) {
+    if (!mVariableModuleGlobal->VariableGlobal.AuthSupport) {
+      //
+      // Not support authenticated variable write.
+      //
+      return EFI_UNSUPPORTED;
+    }
+  } else if ((Attributes & EFI_VARIABLE_HARDWARE_ERROR_RECORD) != 0) {
+    if (PcdGet32 (PcdHwErrStorageSize) == 0) {
+      //
+      // Not support harware error record variable variable.
+      //
+      return EFI_UNSUPPORTED;
+    }
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  Status = VariableServiceQueryVariableInfoInternal (
+             Attributes,
+             MaximumVariableStorageSize,
+             RemainingVariableStorageSize,
+             MaximumVariableSize
+             );
+
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+  return Status;
+}
+
+/**
+  This function reclaims variable storage if free size is below the threshold.
+
+  Caution: This function may be invoked at SMM mode.
+  Care must be taken to make sure not security issue.
+
+**/
+VOID
+ReclaimForOS(
+  VOID
+  )
+{
+  EFI_STATUS                     Status;
+  UINTN                          RemainingCommonRuntimeVariableSpace;
+  UINTN                          RemainingHwErrVariableSpace;
+  STATIC BOOLEAN                 Reclaimed;
+
+  //
+  // This function will be called only once at EndOfDxe or ReadyToBoot event.
+  //
+  if (Reclaimed) {
+    return;
+  }
+  Reclaimed = TRUE;
+
+  Status  = EFI_SUCCESS;
+
+  if (mVariableModuleGlobal->CommonRuntimeVariableSpace < mVariableModuleGlobal->CommonVariableTotalSize) {
+    RemainingCommonRuntimeVariableSpace = 0;
+  } else {
+    RemainingCommonRuntimeVariableSpace = mVariableModuleGlobal->CommonRuntimeVariableSpace - mVariableModuleGlobal->CommonVariableTotalSize;
+  }
+
+  RemainingHwErrVariableSpace = PcdGet32 (PcdHwErrStorageSize) - mVariableModuleGlobal->HwErrVariableTotalSize;
+
+  //
+  // Check if the free area is below a threshold.
+  //
+  if (((RemainingCommonRuntimeVariableSpace < mVariableModuleGlobal->MaxVariableSize) ||
+       (RemainingCommonRuntimeVariableSpace < mVariableModuleGlobal->MaxAuthVariableSize)) ||
+      ((PcdGet32 (PcdHwErrStorageSize) != 0) &&
+       (RemainingHwErrVariableSpace < PcdGet32 (PcdMaxHardwareErrorVariableSize)))){
+    Status = Reclaim (
+            mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase,
+            &mVariableModuleGlobal->NonVolatileLastVariableOffset,
+            FALSE,
+            NULL,
+            NULL,
+            0
+            );
+    ASSERT_EFI_ERROR (Status);
+  }
+}
+
+/**
+  Get non-volatile maximum variable size.
+
+  @return Non-volatile maximum variable size.
+
+**/
+UINTN
+GetNonVolatileMaxVariableSize (
+  VOID
+  )
+{
+  if (PcdGet32 (PcdHwErrStorageSize) != 0) {
+    return MAX (MAX (PcdGet32 (PcdMaxVariableSize), PcdGet32 (PcdMaxAuthVariableSize)),
+                PcdGet32 (PcdMaxHardwareErrorVariableSize));
+  } else {
+    return MAX (PcdGet32 (PcdMaxVariableSize), PcdGet32 (PcdMaxAuthVariableSize));
+  }
+}
+
+/**
+  Init non-volatile variable store.
+
+  @param[out] NvFvHeader        Output pointer to non-volatile FV header address.
+
+  @retval EFI_SUCCESS           Function successfully executed.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate enough memory resource.
+  @retval EFI_VOLUME_CORRUPTED  Variable Store or Firmware Volume for Variable Store is corrupted.
+
+**/
+EFI_STATUS
+InitNonVolatileVariableStore (
+  OUT EFI_FIRMWARE_VOLUME_HEADER    **NvFvHeader
+  )
+{
+  EFI_FIRMWARE_VOLUME_HEADER            *FvHeader;
+  VARIABLE_HEADER                       *Variable;
+  VARIABLE_HEADER                       *NextVariable;
+  EFI_PHYSICAL_ADDRESS                  VariableStoreBase;
+  UINT64                                VariableStoreLength;
+  UINTN                                 VariableSize;
+  EFI_HOB_GUID_TYPE                     *GuidHob;
+  EFI_PHYSICAL_ADDRESS                  NvStorageBase;
+  UINT8                                 *NvStorageData;
+  UINT32                                NvStorageSize;
+  FAULT_TOLERANT_WRITE_LAST_WRITE_DATA  *FtwLastWriteData;
+  UINT32                                BackUpOffset;
+  UINT32                                BackUpSize;
+  UINT32                                HwErrStorageSize;
+  UINT32                                MaxUserNvVariableSpaceSize;
+  UINT32                                BoottimeReservedNvVariableSpaceSize;
+
+  mVariableModuleGlobal->FvbInstance = NULL;
+
+  //
+  // Allocate runtime memory used for a memory copy of the FLASH region.
+  // Keep the memory and the FLASH in sync as updates occur.
+  //
+  NvStorageSize = PcdGet32 (PcdFlashNvStorageVariableSize);
+  NvStorageData = AllocateRuntimeZeroPool (NvStorageSize);
+  if (NvStorageData == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  NvStorageBase = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageVariableBase64);
+  if (NvStorageBase == 0) {
+    NvStorageBase = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageVariableBase);
+  }
+  //
+  // Copy NV storage data to the memory buffer.
+  //
+  CopyMem (NvStorageData, (UINT8 *) (UINTN) NvStorageBase, NvStorageSize);
+
+  //
+  // Check the FTW last write data hob.
+  //
+  GuidHob = GetFirstGuidHob (&gEdkiiFaultTolerantWriteGuid);
+  if (GuidHob != NULL) {
+    FtwLastWriteData = (FAULT_TOLERANT_WRITE_LAST_WRITE_DATA *) GET_GUID_HOB_DATA (GuidHob);
+    if (FtwLastWriteData->TargetAddress == NvStorageBase) {
+      DEBUG ((EFI_D_INFO, "Variable: NV storage is backed up in spare block: 0x%x\n", (UINTN) FtwLastWriteData->SpareAddress));
+      //
+      // Copy the backed up NV storage data to the memory buffer from spare block.
+      //
+      CopyMem (NvStorageData, (UINT8 *) (UINTN) (FtwLastWriteData->SpareAddress), NvStorageSize);
+    } else if ((FtwLastWriteData->TargetAddress > NvStorageBase) &&
+               (FtwLastWriteData->TargetAddress < (NvStorageBase + NvStorageSize))) {
+      //
+      // Flash NV storage from the Offset is backed up in spare block.
+      //
+      BackUpOffset = (UINT32) (FtwLastWriteData->TargetAddress - NvStorageBase);
+      BackUpSize = NvStorageSize - BackUpOffset;
+      DEBUG ((EFI_D_INFO, "Variable: High partial NV storage from offset: %x is backed up in spare block: 0x%x\n", BackUpOffset, (UINTN) FtwLastWriteData->SpareAddress));
+      //
+      // Copy the partial backed up NV storage data to the memory buffer from spare block.
+      //
+      CopyMem (NvStorageData + BackUpOffset, (UINT8 *) (UINTN) FtwLastWriteData->SpareAddress, BackUpSize);
+    }
+  }
+
+  FvHeader = (EFI_FIRMWARE_VOLUME_HEADER *) NvStorageData;
+
+  //
+  // Check if the Firmware Volume is not corrupted
+  //
+  if ((FvHeader->Signature != EFI_FVH_SIGNATURE) || (!CompareGuid (&gEfiSystemNvDataFvGuid, &FvHeader->FileSystemGuid))) {
+    FreePool (NvStorageData);
+    DEBUG ((EFI_D_ERROR, "Firmware Volume for Variable Store is corrupted\n"));
+    return EFI_VOLUME_CORRUPTED;
+  }
+
+  VariableStoreBase = (EFI_PHYSICAL_ADDRESS) ((UINTN) FvHeader + FvHeader->HeaderLength);
+  VariableStoreLength = (UINT64) (NvStorageSize - FvHeader->HeaderLength);
+
+  mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase = VariableStoreBase;
+  mNvVariableCache = (VARIABLE_STORE_HEADER *) (UINTN) VariableStoreBase;
+  if (GetVariableStoreStatus (mNvVariableCache) != EfiValid) {
+    FreePool (NvStorageData);
+    DEBUG((EFI_D_ERROR, "Variable Store header is corrupted\n"));
+    return EFI_VOLUME_CORRUPTED;
+  }
+  ASSERT(mNvVariableCache->Size == VariableStoreLength);
+
+  ASSERT (sizeof (VARIABLE_STORE_HEADER) <= VariableStoreLength);
+
+  mVariableModuleGlobal->VariableGlobal.AuthFormat = (BOOLEAN)(CompareGuid (&mNvVariableCache->Signature, &gEfiAuthenticatedVariableGuid));
+
+  HwErrStorageSize = PcdGet32 (PcdHwErrStorageSize);
+  MaxUserNvVariableSpaceSize = PcdGet32 (PcdMaxUserNvVariableSpaceSize);
+  BoottimeReservedNvVariableSpaceSize = PcdGet32 (PcdBoottimeReservedNvVariableSpaceSize);
+
+  //
+  // Note that in EdkII variable driver implementation, Hardware Error Record type variable
+  // is stored with common variable in the same NV region. So the platform integrator should
+  // ensure that the value of PcdHwErrStorageSize is less than the value of
+  // (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)).
+  //
+  ASSERT (HwErrStorageSize < (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)));
+  //
+  // Ensure that the value of PcdMaxUserNvVariableSpaceSize is less than the value of
+  // (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)) - PcdGet32 (PcdHwErrStorageSize).
+  //
+  ASSERT (MaxUserNvVariableSpaceSize < (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER) - HwErrStorageSize));
+  //
+  // Ensure that the value of PcdBoottimeReservedNvVariableSpaceSize is less than the value of
+  // (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)) - PcdGet32 (PcdHwErrStorageSize).
+  //
+  ASSERT (BoottimeReservedNvVariableSpaceSize < (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER) - HwErrStorageSize));
+
+  mVariableModuleGlobal->CommonVariableSpace = ((UINTN) VariableStoreLength - sizeof (VARIABLE_STORE_HEADER) - HwErrStorageSize);
+  mVariableModuleGlobal->CommonMaxUserVariableSpace = ((MaxUserNvVariableSpaceSize != 0) ? MaxUserNvVariableSpaceSize : mVariableModuleGlobal->CommonVariableSpace);
+  mVariableModuleGlobal->CommonRuntimeVariableSpace = mVariableModuleGlobal->CommonVariableSpace - BoottimeReservedNvVariableSpaceSize;
+
+  DEBUG ((EFI_D_INFO, "Variable driver common space: 0x%x 0x%x 0x%x\n", mVariableModuleGlobal->CommonVariableSpace, mVariableModuleGlobal->CommonMaxUserVariableSpace, mVariableModuleGlobal->CommonRuntimeVariableSpace));
+
+  //
+  // The max NV variable size should be < (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)).
+  //
+  ASSERT (GetNonVolatileMaxVariableSize () < (VariableStoreLength - sizeof (VARIABLE_STORE_HEADER)));
+
+  mVariableModuleGlobal->MaxVariableSize = PcdGet32 (PcdMaxVariableSize);
+  mVariableModuleGlobal->MaxAuthVariableSize = ((PcdGet32 (PcdMaxAuthVariableSize) != 0) ? PcdGet32 (PcdMaxAuthVariableSize) : mVariableModuleGlobal->MaxVariableSize);
+
+  //
+  // Parse non-volatile variable data and get last variable offset.
+  //
+  Variable  = GetStartPointer ((VARIABLE_STORE_HEADER *)(UINTN)VariableStoreBase);
+  while (IsValidVariableHeader (Variable, GetEndPointer ((VARIABLE_STORE_HEADER *)(UINTN)VariableStoreBase))) {
+    NextVariable = GetNextVariablePtr (Variable);
+    VariableSize = (UINTN) NextVariable - (UINTN) Variable;
+    if ((Variable->Attributes & (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) == (EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_HARDWARE_ERROR_RECORD)) {
+      mVariableModuleGlobal->HwErrVariableTotalSize += VariableSize;
+    } else {
+      mVariableModuleGlobal->CommonVariableTotalSize += VariableSize;
+    }
+
+    Variable = NextVariable;
+  }
+  mVariableModuleGlobal->NonVolatileLastVariableOffset = (UINTN) Variable - (UINTN) VariableStoreBase;
+
+  *NvFvHeader = FvHeader;
+  return EFI_SUCCESS;
+}
+
+/**
+  Flush the HOB variable to flash.
+
+  @param[in] VariableName       Name of variable has been updated or deleted.
+  @param[in] VendorGuid         Guid of variable has been updated or deleted.
+
+**/
+VOID
+FlushHobVariableToFlash (
+  IN CHAR16                     *VariableName,
+  IN EFI_GUID                   *VendorGuid
+  )
+{
+  EFI_STATUS                    Status;
+  VARIABLE_STORE_HEADER         *VariableStoreHeader;
+  VARIABLE_HEADER               *Variable;
+  VOID                          *VariableData;
+  VARIABLE_POINTER_TRACK        VariablePtrTrack;
+  BOOLEAN                       ErrorFlag;
+
+  ErrorFlag = FALSE;
+
+  //
+  // Flush the HOB variable to flash.
+  //
+  if (mVariableModuleGlobal->VariableGlobal.HobVariableBase != 0) {
+    VariableStoreHeader = (VARIABLE_STORE_HEADER *) (UINTN) mVariableModuleGlobal->VariableGlobal.HobVariableBase;
+    //
+    // Set HobVariableBase to 0, it can avoid SetVariable to call back.
+    //
+    mVariableModuleGlobal->VariableGlobal.HobVariableBase = 0;
+    for ( Variable = GetStartPointer (VariableStoreHeader)
+        ; IsValidVariableHeader (Variable, GetEndPointer (VariableStoreHeader))
+        ; Variable = GetNextVariablePtr (Variable)
+        ) {
+      if (Variable->State != VAR_ADDED) {
+        //
+        // The HOB variable has been set to DELETED state in local.
+        //
+        continue;
+      }
+      ASSERT ((Variable->Attributes & EFI_VARIABLE_NON_VOLATILE) != 0);
+      if (VendorGuid == NULL || VariableName == NULL ||
+          !CompareGuid (VendorGuid, GetVendorGuidPtr (Variable)) ||
+          StrCmp (VariableName, GetVariableNamePtr (Variable)) != 0) {
+        VariableData = GetVariableDataPtr (Variable);
+        FindVariable (GetVariableNamePtr (Variable), GetVendorGuidPtr (Variable), &VariablePtrTrack, &mVariableModuleGlobal->VariableGlobal, FALSE);
+        Status = UpdateVariable (
+                   GetVariableNamePtr (Variable),
+                   GetVendorGuidPtr (Variable),
+                   VariableData,
+                   DataSizeOfVariable (Variable),
+                   Variable->Attributes,
+                   0,
+                   0,
+                   &VariablePtrTrack,
+                   NULL
+                 );
+        DEBUG ((EFI_D_INFO, "Variable driver flush the HOB variable to flash: %g %s %r\n", GetVendorGuidPtr (Variable), GetVariableNamePtr (Variable), Status));
+      } else {
+        //
+        // The updated or deleted variable is matched with this HOB variable.
+        // Don't break here because we will try to set other HOB variables
+        // since this variable could be set successfully.
+        //
+        Status = EFI_SUCCESS;
+      }
+      if (!EFI_ERROR (Status)) {
+        //
+        // If set variable successful, or the updated or deleted variable is matched with the HOB variable,
+        // set the HOB variable to DELETED state in local.
+        //
+        DEBUG ((EFI_D_INFO, "Variable driver set the HOB variable to DELETED state in local: %g %s\n", GetVendorGuidPtr (Variable), GetVariableNamePtr (Variable)));
+        Variable->State &= VAR_DELETED;
+      } else {
+        ErrorFlag = TRUE;
+      }
+    }
+    if (ErrorFlag) {
+      //
+      // We still have HOB variable(s) not flushed in flash.
+      //
+      mVariableModuleGlobal->VariableGlobal.HobVariableBase = (EFI_PHYSICAL_ADDRESS) (UINTN) VariableStoreHeader;
+    } else {
+      //
+      // All HOB variables have been flushed in flash.
+      //
+      DEBUG ((EFI_D_INFO, "Variable driver: all HOB variables have been flushed in flash.\n"));
+      if (!AtRuntime ()) {
+        FreePool ((VOID *) VariableStoreHeader);
+      }
+    }
+  }
+
+}
+
+/**
+  Initializes variable write service after FTW was ready.
+
+  @retval EFI_SUCCESS          Function successfully executed.
+  @retval Others               Fail to initialize the variable service.
+
+**/
+EFI_STATUS
+VariableWriteServiceInitialize (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  VARIABLE_STORE_HEADER           *VariableStoreHeader;
+  UINTN                           Index;
+  UINT8                           Data;
+  EFI_PHYSICAL_ADDRESS            VariableStoreBase;
+  EFI_PHYSICAL_ADDRESS            NvStorageBase;
+  VARIABLE_ENTRY_PROPERTY         *VariableEntry;
+
+  AcquireLockOnlyAtBootTime(&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  NvStorageBase = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageVariableBase64);
+  if (NvStorageBase == 0) {
+    NvStorageBase = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageVariableBase);
+  }
+  VariableStoreBase = NvStorageBase + (((EFI_FIRMWARE_VOLUME_HEADER *)(UINTN)(NvStorageBase))->HeaderLength);
+
+  //
+  // Let NonVolatileVariableBase point to flash variable store base directly after FTW ready.
+  //
+  mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase = VariableStoreBase;
+  VariableStoreHeader = (VARIABLE_STORE_HEADER *)(UINTN)VariableStoreBase;
+
+  //
+  // Check if the free area is really free.
+  //
+  for (Index = mVariableModuleGlobal->NonVolatileLastVariableOffset; Index < VariableStoreHeader->Size; Index++) {
+    Data = ((UINT8 *) mNvVariableCache)[Index];
+    if (Data != 0xff) {
+      //
+      // There must be something wrong in variable store, do reclaim operation.
+      //
+      Status = Reclaim (
+                 mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase,
+                 &mVariableModuleGlobal->NonVolatileLastVariableOffset,
+                 FALSE,
+                 NULL,
+                 NULL,
+                 0
+                 );
+      if (EFI_ERROR (Status)) {
+        ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+        return Status;
+      }
+      break;
+    }
+  }
+
+  FlushHobVariableToFlash (NULL, NULL);
+
+  Status = EFI_SUCCESS;
+  ZeroMem (&mAuthContextOut, sizeof (mAuthContextOut));
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    //
+    // Authenticated variable initialize.
+    //
+    mAuthContextIn.StructSize = sizeof (AUTH_VAR_LIB_CONTEXT_IN);
+    mAuthContextIn.MaxAuthVariableSize = mVariableModuleGlobal->MaxAuthVariableSize - GetVariableHeaderSize ();
+    Status = AuthVariableLibInitialize (&mAuthContextIn, &mAuthContextOut);
+    if (!EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_INFO, "Variable driver will work with auth variable support!\n"));
+      mVariableModuleGlobal->VariableGlobal.AuthSupport = TRUE;
+      if (mAuthContextOut.AuthVarEntry != NULL) {
+        for (Index = 0; Index < mAuthContextOut.AuthVarEntryCount; Index++) {
+          VariableEntry = &mAuthContextOut.AuthVarEntry[Index];
+          Status = VarCheckLibVariablePropertySet (
+                     VariableEntry->Name,
+                     VariableEntry->Guid,
+                     &VariableEntry->VariableProperty
+                     );
+          ASSERT_EFI_ERROR (Status);
+        }
+      }
+    } else if (Status == EFI_UNSUPPORTED) {
+      DEBUG ((EFI_D_INFO, "NOTICE - AuthVariableLibInitialize() returns %r!\n", Status));
+      DEBUG ((EFI_D_INFO, "Variable driver will continue to work without auth variable support!\n"));
+      mVariableModuleGlobal->VariableGlobal.AuthSupport = FALSE;
+      Status = EFI_SUCCESS;
+    }
+  }
+
+  if (!EFI_ERROR (Status)) {
+    for (Index = 0; Index < sizeof (mVariableEntryProperty) / sizeof (mVariableEntryProperty[0]); Index++) {
+      VariableEntry = &mVariableEntryProperty[Index];
+      Status = VarCheckLibVariablePropertySet (VariableEntry->Name, VariableEntry->Guid, &VariableEntry->VariableProperty);
+      ASSERT_EFI_ERROR (Status);
+    }
+  }
+
+  ReleaseLockOnlyAtBootTime (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock);
+
+  //
+  // Initialize MOR Lock variable.
+  //
+  MorLockInit ();
+
+  return Status;
+}
+
+
+/**
+  Initializes variable store area for non-volatile and volatile variable.
+
+  @retval EFI_SUCCESS           Function successfully executed.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate enough memory resource.
+
+**/
+EFI_STATUS
+VariableCommonInitialize (
+  VOID
+  )
+{
+  EFI_STATUS                      Status;
+  VARIABLE_STORE_HEADER           *VolatileVariableStore;
+  VARIABLE_STORE_HEADER           *VariableStoreHeader;
+  UINT64                          VariableStoreLength;
+  UINTN                           ScratchSize;
+  EFI_HOB_GUID_TYPE               *GuidHob;
+  EFI_GUID                        *VariableGuid;
+  EFI_FIRMWARE_VOLUME_HEADER      *NvFvHeader;
+
+  //
+  // Allocate runtime memory for variable driver global structure.
+  //
+  mVariableModuleGlobal = AllocateRuntimeZeroPool (sizeof (VARIABLE_MODULE_GLOBAL));
+  if (mVariableModuleGlobal == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  InitializeLock (&mVariableModuleGlobal->VariableGlobal.VariableServicesLock, TPL_NOTIFY);
+
+  //
+  // Init non-volatile variable store.
+  //
+  NvFvHeader = NULL;
+  Status = InitNonVolatileVariableStore (&NvFvHeader);
+  if (EFI_ERROR (Status)) {
+    FreePool (mVariableModuleGlobal);
+    return Status;
+  }
+
+  //
+  // mVariableModuleGlobal->VariableGlobal.AuthFormat
+  // has been initialized in InitNonVolatileVariableStore().
+  //
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    DEBUG ((EFI_D_INFO, "Variable driver will work with auth variable format!\n"));
+    //
+    // Set AuthSupport to FALSE first, VariableWriteServiceInitialize() will initialize it.
+    //
+    mVariableModuleGlobal->VariableGlobal.AuthSupport = FALSE;
+    VariableGuid = &gEfiAuthenticatedVariableGuid;
+  } else {
+    DEBUG ((EFI_D_INFO, "Variable driver will work without auth variable support!\n"));
+    mVariableModuleGlobal->VariableGlobal.AuthSupport = FALSE;
+    VariableGuid = &gEfiVariableGuid;
+  }
+
+  //
+  // Get HOB variable store.
+  //
+  GuidHob = GetFirstGuidHob (VariableGuid);
+  if (GuidHob != NULL) {
+    VariableStoreHeader = GET_GUID_HOB_DATA (GuidHob);
+    VariableStoreLength = (UINT64) (GuidHob->Header.HobLength - sizeof (EFI_HOB_GUID_TYPE));
+    if (GetVariableStoreStatus (VariableStoreHeader) == EfiValid) {
+      mVariableModuleGlobal->VariableGlobal.HobVariableBase = (EFI_PHYSICAL_ADDRESS) (UINTN) AllocateRuntimeCopyPool ((UINTN) VariableStoreLength, (VOID *) VariableStoreHeader);
+      if (mVariableModuleGlobal->VariableGlobal.HobVariableBase == 0) {
+        FreePool (NvFvHeader);
+        FreePool (mVariableModuleGlobal);
+        return EFI_OUT_OF_RESOURCES;
+      }
+    } else {
+      DEBUG ((EFI_D_ERROR, "HOB Variable Store header is corrupted!\n"));
+    }
+  }
+
+  //
+  // Allocate memory for volatile variable store, note that there is a scratch space to store scratch data.
+  //
+  ScratchSize = GetNonVolatileMaxVariableSize ();
+  mVariableModuleGlobal->ScratchBufferSize = ScratchSize;
+  VolatileVariableStore = AllocateRuntimePool (PcdGet32 (PcdVariableStoreSize) + ScratchSize);
+  if (VolatileVariableStore == NULL) {
+    if (mVariableModuleGlobal->VariableGlobal.HobVariableBase != 0) {
+      FreePool ((VOID *) (UINTN) mVariableModuleGlobal->VariableGlobal.HobVariableBase);
+    }
+    FreePool (NvFvHeader);
+    FreePool (mVariableModuleGlobal);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  SetMem (VolatileVariableStore, PcdGet32 (PcdVariableStoreSize) + ScratchSize, 0xff);
+
+  //
+  // Initialize Variable Specific Data.
+  //
+  mVariableModuleGlobal->VariableGlobal.VolatileVariableBase = (EFI_PHYSICAL_ADDRESS) (UINTN) VolatileVariableStore;
+  mVariableModuleGlobal->VolatileLastVariableOffset = (UINTN) GetStartPointer (VolatileVariableStore) - (UINTN) VolatileVariableStore;
+
+  CopyGuid (&VolatileVariableStore->Signature, VariableGuid);
+  VolatileVariableStore->Size        = PcdGet32 (PcdVariableStoreSize);
+  VolatileVariableStore->Format      = VARIABLE_STORE_FORMATTED;
+  VolatileVariableStore->State       = VARIABLE_STORE_HEALTHY;
+  VolatileVariableStore->Reserved    = 0;
+  VolatileVariableStore->Reserved1   = 0;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Get the proper fvb handle and/or fvb protocol by the given Flash address.
+
+  @param[in]  Address       The Flash address.
+  @param[out] FvbHandle     In output, if it is not NULL, it points to the proper FVB handle.
+  @param[out] FvbProtocol   In output, if it is not NULL, it points to the proper FVB protocol.
+
+**/
+EFI_STATUS
+GetFvbInfoByAddress (
+  IN  EFI_PHYSICAL_ADDRESS                Address,
+  OUT EFI_HANDLE                          *FvbHandle OPTIONAL,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvbProtocol OPTIONAL
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_HANDLE                              *HandleBuffer;
+  UINTN                                   HandleCount;
+  UINTN                                   Index;
+  EFI_PHYSICAL_ADDRESS                    FvbBaseAddress;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL      *Fvb;
+  EFI_FVB_ATTRIBUTES_2                    Attributes;
+  UINTN                                   BlockSize;
+  UINTN                                   NumberOfBlocks;
+
+  HandleBuffer = NULL;
+  //
+  // Get all FVB handles.
+  //
+  Status = GetFvbCountAndBuffer (&HandleCount, &HandleBuffer);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Get the FVB to access variable store.
+  //
+  Fvb = NULL;
+  for (Index = 0; Index < HandleCount; Index += 1, Status = EFI_NOT_FOUND, Fvb = NULL) {
+    Status = GetFvbByHandle (HandleBuffer[Index], &Fvb);
+    if (EFI_ERROR (Status)) {
+      Status = EFI_NOT_FOUND;
+      break;
+    }
+
+    //
+    // Ensure this FVB protocol supported Write operation.
+    //
+    Status = Fvb->GetAttributes (Fvb, &Attributes);
+    if (EFI_ERROR (Status) || ((Attributes & EFI_FVB2_WRITE_STATUS) == 0)) {
+      continue;
+    }
+
+    //
+    // Compare the address and select the right one.
+    //
+    Status = Fvb->GetPhysicalAddress (Fvb, &FvbBaseAddress);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    //
+    // Assume one FVB has one type of BlockSize.
+    //
+    Status = Fvb->GetBlockSize (Fvb, 0, &BlockSize, &NumberOfBlocks);
+    if (EFI_ERROR (Status)) {
+      continue;
+    }
+
+    if ((Address >= FvbBaseAddress) && (Address < (FvbBaseAddress + BlockSize * NumberOfBlocks))) {
+      if (FvbHandle != NULL) {
+        *FvbHandle  = HandleBuffer[Index];
+      }
+      if (FvbProtocol != NULL) {
+        *FvbProtocol = Fvb;
+      }
+      Status = EFI_SUCCESS;
+      break;
+    }
+  }
+  FreePool (HandleBuffer);
+
+  if (Fvb == NULL) {
+    Status = EFI_NOT_FOUND;
+  }
+
+  return Status;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.h b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.h
new file mode 100644
index 0000000000..5c224b9737
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/Variable.h
@@ -0,0 +1,902 @@
+/** @file
+  The internal header file includes the common header files, defines
+  internal structure and functions used by Variable modules.
+
+Copyright (c) 2006 - 2015, 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.
+
+**/
+
+#ifndef _VARIABLE_H_
+#define _VARIABLE_H_
+
+#include <PiDxe.h>
+#include <Protocol/VariableWrite.h>
+#include <Protocol/FaultTolerantWrite.h>
+#include <Protocol/FirmwareVolumeBlock.h>
+#include <Protocol/Variable.h>
+#include <Protocol/VariableLock.h>
+#include <Protocol/VarCheck.h>
+#include <Library/PcdLib.h>
+#include <Library/HobLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/DebugLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/SynchronizationLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/AuthVariableLib.h>
+#include <Library/VarCheckLib.h>
+#include <Guid/GlobalVariable.h>
+#include <Guid/EventGroup.h>
+#include <Guid/VariableFormat.h>
+#include <Guid/SystemNvDataGuid.h>
+#include <Guid/FaultTolerantWrite.h>
+#include <Guid/VarErrorFlag.h>
+
+#define EFI_VARIABLE_ATTRIBUTES_MASK (EFI_VARIABLE_NON_VOLATILE | \
+                                      EFI_VARIABLE_BOOTSERVICE_ACCESS | \
+                                      EFI_VARIABLE_RUNTIME_ACCESS | \
+                                      EFI_VARIABLE_HARDWARE_ERROR_RECORD | \
+                                      EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS | \
+                                      EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS | \
+                                      EFI_VARIABLE_APPEND_WRITE)
+
+///
+/// The size of a 3 character ISO639 language code.
+///
+#define ISO_639_2_ENTRY_SIZE    3
+
+typedef enum {
+  VariableStoreTypeVolatile,
+  VariableStoreTypeHob,
+  VariableStoreTypeNv,
+  VariableStoreTypeMax
+} VARIABLE_STORE_TYPE;
+
+typedef struct {
+  VARIABLE_HEADER *CurrPtr;
+  //
+  // If both ADDED and IN_DELETED_TRANSITION variable are present,
+  // InDeletedTransitionPtr will point to the IN_DELETED_TRANSITION one.
+  // Otherwise, CurrPtr will point to the ADDED or IN_DELETED_TRANSITION one,
+  // and InDeletedTransitionPtr will be NULL at the same time.
+  //
+  VARIABLE_HEADER *InDeletedTransitionPtr;
+  VARIABLE_HEADER *EndPtr;
+  VARIABLE_HEADER *StartPtr;
+  BOOLEAN         Volatile;
+} VARIABLE_POINTER_TRACK;
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS  HobVariableBase;
+  EFI_PHYSICAL_ADDRESS  VolatileVariableBase;
+  EFI_PHYSICAL_ADDRESS  NonVolatileVariableBase;
+  EFI_LOCK              VariableServicesLock;
+  UINT32                ReentrantState;
+  BOOLEAN               AuthFormat;
+  BOOLEAN               AuthSupport;
+} VARIABLE_GLOBAL;
+
+typedef struct {
+  VARIABLE_GLOBAL VariableGlobal;
+  UINTN           VolatileLastVariableOffset;
+  UINTN           NonVolatileLastVariableOffset;
+  UINTN           CommonVariableSpace;
+  UINTN           CommonMaxUserVariableSpace;
+  UINTN           CommonRuntimeVariableSpace;
+  UINTN           CommonVariableTotalSize;
+  UINTN           CommonUserVariableTotalSize;
+  UINTN           HwErrVariableTotalSize;
+  UINTN           MaxVariableSize;
+  UINTN           MaxAuthVariableSize;
+  UINTN           ScratchBufferSize;
+  CHAR8           *PlatformLangCodes;
+  CHAR8           *LangCodes;
+  CHAR8           *PlatformLang;
+  CHAR8           Lang[ISO_639_2_ENTRY_SIZE + 1];
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL *FvbInstance;
+} VARIABLE_MODULE_GLOBAL;
+
+/**
+  Flush the HOB variable to flash.
+
+  @param[in] VariableName       Name of variable has been updated or deleted.
+  @param[in] VendorGuid         Guid of variable has been updated or deleted.
+
+**/
+VOID
+FlushHobVariableToFlash (
+  IN CHAR16                     *VariableName,
+  IN EFI_GUID                   *VendorGuid
+  );
+
+/**
+  Writes a buffer to variable storage space, in the working block.
+
+  This function writes a buffer to variable storage space into a firmware
+  volume block device. The destination is specified by the parameter
+  VariableBase. Fault Tolerant Write protocol is used for writing.
+
+  @param  VariableBase   Base address of the variable to write.
+  @param  VariableBuffer Point to the variable data buffer.
+
+  @retval EFI_SUCCESS    The function completed successfully.
+  @retval EFI_NOT_FOUND  Fail to locate Fault Tolerant Write protocol.
+  @retval EFI_ABORTED    The function could not complete successfully.
+
+**/
+EFI_STATUS
+FtwVariableSpace (
+  IN EFI_PHYSICAL_ADDRESS   VariableBase,
+  IN VARIABLE_STORE_HEADER  *VariableBuffer
+  );
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+  If IgnoreRtCheck is TRUE, then we ignore the EFI_VARIABLE_RUNTIME_ACCESS attribute check
+  at runtime when searching existing variable, only VariableName and VendorGuid are compared.
+  Otherwise, variables without EFI_VARIABLE_RUNTIME_ACCESS are not visible at runtime.
+
+  @param[in]   VariableName           Name of the variable to be found.
+  @param[in]   VendorGuid             Vendor GUID to be found.
+  @param[out]  PtrTrack               VARIABLE_POINTER_TRACK structure for output,
+                                      including the range searched and the target position.
+  @param[in]   Global                 Pointer to VARIABLE_GLOBAL structure, including
+                                      base of volatile variable storage area, base of
+                                      NV variable storage area, and a lock.
+  @param[in]   IgnoreRtCheck          Ignore EFI_VARIABLE_RUNTIME_ACCESS attribute
+                                      check at runtime when searching variable.
+
+  @retval EFI_INVALID_PARAMETER       If VariableName is not an empty string, while
+                                      VendorGuid is NULL.
+  @retval EFI_SUCCESS                 Variable successfully found.
+  @retval EFI_NOT_FOUND               Variable not found
+
+**/
+EFI_STATUS
+FindVariable (
+  IN  CHAR16                  *VariableName,
+  IN  EFI_GUID                *VendorGuid,
+  OUT VARIABLE_POINTER_TRACK  *PtrTrack,
+  IN  VARIABLE_GLOBAL         *Global,
+  IN  BOOLEAN                 IgnoreRtCheck
+  );
+
+/**
+
+  Gets the pointer to the end of the variable storage area.
+
+  This function gets pointer to the end of the variable storage
+  area, according to the input variable store header.
+
+  @param VarStoreHeader  Pointer to the Variable Store Header.
+
+  @return Pointer to the end of the variable storage area.
+
+**/
+VARIABLE_HEADER *
+GetEndPointer (
+  IN VARIABLE_STORE_HEADER       *VarStoreHeader
+  );
+
+/**
+  This code gets the size of variable header.
+
+  @return Size of variable header in bytes in type UINTN.
+
+**/
+UINTN
+GetVariableHeaderSize (
+  VOID
+  );
+
+/**
+
+  This code gets the pointer to the variable name.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Pointer to Variable Name which is Unicode encoding.
+
+**/
+CHAR16 *
+GetVariableNamePtr (
+  IN  VARIABLE_HEADER   *Variable
+  );
+
+/**
+  This code gets the pointer to the variable guid.
+
+  @param Variable   Pointer to the Variable Header.
+
+  @return A EFI_GUID* pointer to Vendor Guid.
+
+**/
+EFI_GUID *
+GetVendorGuidPtr (
+  IN VARIABLE_HEADER    *Variable
+  );
+
+/**
+
+  This code gets the pointer to the variable data.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Pointer to Variable Data.
+
+**/
+UINT8 *
+GetVariableDataPtr (
+  IN  VARIABLE_HEADER   *Variable
+  );
+
+/**
+
+  This code gets the size of variable data.
+
+  @param Variable        Pointer to the Variable Header.
+
+  @return Size of variable in bytes.
+
+**/
+UINTN
+DataSizeOfVariable (
+  IN  VARIABLE_HEADER   *Variable
+  );
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param[in] Marker             VA_LIST style variable argument list.
+                                The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+BOOLEAN
+EFIAPI
+CheckRemainingSpaceForConsistencyInternal (
+  IN UINT32                     Attributes,
+  IN VA_LIST                    Marker
+  );
+
+/**
+  Update the variable region with Variable information. If EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS is set,
+  index of associated public key is needed.
+
+  @param[in] VariableName       Name of variable.
+  @param[in] VendorGuid         Guid of variable.
+  @param[in] Data               Variable data.
+  @param[in] DataSize           Size of data. 0 means delete.
+  @param[in] Attributes         Attributes of the variable.
+  @param[in] KeyIndex           Index of associated public key.
+  @param[in] MonotonicCount     Value of associated monotonic count.
+  @param[in, out] Variable      The variable information that is used to keep track of variable usage.
+
+  @param[in] TimeStamp          Value of associated TimeStamp.
+
+  @retval EFI_SUCCESS           The update operation is success.
+  @retval EFI_OUT_OF_RESOURCES  Variable region is full, cannot write other data into this region.
+
+**/
+EFI_STATUS
+UpdateVariable (
+  IN      CHAR16          *VariableName,
+  IN      EFI_GUID        *VendorGuid,
+  IN      VOID            *Data,
+  IN      UINTN           DataSize,
+  IN      UINT32          Attributes OPTIONAL,
+  IN      UINT32          KeyIndex  OPTIONAL,
+  IN      UINT64          MonotonicCount  OPTIONAL,
+  IN OUT  VARIABLE_POINTER_TRACK *Variable,
+  IN      EFI_TIME        *TimeStamp  OPTIONAL
+  );
+
+
+/**
+  Return TRUE if ExitBootServices () has been called.
+
+  @retval TRUE If ExitBootServices () has been called.
+**/
+BOOLEAN
+AtRuntime (
+  VOID
+  );
+
+/**
+  Initializes a basic mutual exclusion lock.
+
+  This function initializes a basic mutual exclusion lock to the released state
+  and returns the lock.  Each lock provides mutual exclusion access at its task
+  priority level.  Since there is no preemption or multiprocessor support in EFI,
+  acquiring the lock only consists of raising to the locks TPL.
+  If Lock is NULL, then ASSERT().
+  If Priority is not a valid TPL value, then ASSERT().
+
+  @param  Lock       A pointer to the lock data structure to initialize.
+  @param  Priority   EFI TPL is associated with the lock.
+
+  @return The lock.
+
+**/
+EFI_LOCK *
+InitializeLock (
+  IN OUT EFI_LOCK  *Lock,
+  IN EFI_TPL        Priority
+  );
+
+
+/**
+  Acquires lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function that will be removed when
+  EfiAcquireLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiAcquireLock() at boot time, and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to acquire.
+
+**/
+VOID
+AcquireLockOnlyAtBootTime (
+  IN EFI_LOCK  *Lock
+  );
+
+
+/**
+  Releases lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiReleaseLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiReleaseLock() at boot time and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to release.
+
+**/
+VOID
+ReleaseLockOnlyAtBootTime (
+  IN EFI_LOCK  *Lock
+  );
+
+/**
+  Retrive the FVB protocol interface by HANDLE.
+
+  @param[in]  FvBlockHandle     The handle of FVB protocol that provides services for
+                                reading, writing, and erasing the target block.
+  @param[out] FvBlock           The interface of FVB protocol
+
+  @retval EFI_SUCCESS           The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED       The device does not support the FVB protocol.
+  @retval EFI_INVALID_PARAMETER FvBlockHandle is not a valid EFI_HANDLE or FvBlock is NULL.
+
+**/
+EFI_STATUS
+GetFvbByHandle (
+  IN  EFI_HANDLE                          FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  );
+
+/**
+  Function returns an array of handles that support the FVB protocol
+  in a buffer allocated from pool.
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  );
+
+/**
+  Initializes variable store area for non-volatile and volatile variable.
+
+  @retval EFI_SUCCESS           Function successfully executed.
+  @retval EFI_OUT_OF_RESOURCES  Fail to allocate enough memory resource.
+
+**/
+EFI_STATUS
+VariableCommonInitialize (
+  VOID
+  );
+
+/**
+  This function reclaims variable storage if free size is below the threshold.
+
+**/
+VOID
+ReclaimForOS(
+  VOID
+  );
+
+/**
+  Get non-volatile maximum variable size.
+
+  @return Non-volatile maximum variable size.
+
+**/
+UINTN
+GetNonVolatileMaxVariableSize (
+  VOID
+  );
+
+/**
+  Initializes variable write service after FVB was ready.
+
+  @retval EFI_SUCCESS          Function successfully executed.
+  @retval Others               Fail to initialize the variable service.
+
+**/
+EFI_STATUS
+VariableWriteServiceInitialize (
+  VOID
+  );
+
+/**
+  Retrive the SMM Fault Tolerent Write protocol interface.
+
+  @param[out] FtwProtocol       The interface of SMM Ftw protocol
+
+  @retval EFI_SUCCESS           The SMM SAR protocol instance was found and returned in SarProtocol.
+  @retval EFI_NOT_FOUND         The SMM SAR protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+GetFtwProtocol (
+  OUT VOID                                **FtwProtocol
+  );
+
+/**
+  Get the proper fvb handle and/or fvb protocol by the given Flash address.
+
+  @param[in] Address        The Flash address.
+  @param[out] FvbHandle     In output, if it is not NULL, it points to the proper FVB handle.
+  @param[out] FvbProtocol   In output, if it is not NULL, it points to the proper FVB protocol.
+
+**/
+EFI_STATUS
+GetFvbInfoByAddress (
+  IN  EFI_PHYSICAL_ADDRESS                Address,
+  OUT EFI_HANDLE                          *FvbHandle OPTIONAL,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvbProtocol OPTIONAL
+  );
+
+/**
+
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode, and datasize and data are external input.
+  This function will do basic validation, before parse the data.
+
+  @param VariableName               Name of Variable to be found.
+  @param VendorGuid                 Variable vendor GUID.
+  @param Attributes                 Attribute value of the variable found.
+  @param DataSize                   Size of Data found. If size is less than the
+                                    data, this value contains the required size.
+  @param Data                       Data pointer.
+
+  @return EFI_INVALID_PARAMETER     Invalid parameter.
+  @return EFI_SUCCESS               Find the specified variable.
+  @return EFI_NOT_FOUND             Not found.
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetVariable (
+  IN      CHAR16            *VariableName,
+  IN      EFI_GUID          *VendorGuid,
+  OUT     UINT32            *Attributes OPTIONAL,
+  IN OUT  UINTN             *DataSize,
+  OUT     VOID              *Data
+  );
+
+/**
+  This code Finds the Next available variable.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param[in] VariableName   Pointer to variable name.
+  @param[in] VendorGuid     Variable Vendor Guid.
+  @param[out] VariablePtr   Pointer to variable header address.
+
+  @return EFI_SUCCESS       Find the specified variable.
+  @return EFI_NOT_FOUND     Not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetNextVariableInternal (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT VARIABLE_HEADER       **VariablePtr
+  );
+
+/**
+
+  This code Finds the Next available variable.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param VariableNameSize           Size of the variable name.
+  @param VariableName               Pointer to variable name.
+  @param VendorGuid                 Variable Vendor Guid.
+
+  @return EFI_INVALID_PARAMETER     Invalid parameter.
+  @return EFI_SUCCESS               Find the specified variable.
+  @return EFI_NOT_FOUND             Not found.
+  @return EFI_BUFFER_TO_SMALL       DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceGetNextVariableName (
+  IN OUT  UINTN             *VariableNameSize,
+  IN OUT  CHAR16            *VariableName,
+  IN OUT  EFI_GUID          *VendorGuid
+  );
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode, and datasize and data are external input.
+  This function will do basic validation, before parse the data.
+  This function will parse the authentication carefully to avoid security issues, like
+  buffer overflow, integer overflow.
+  This function will check attribute carefully to avoid authentication bypass.
+
+  @param VariableName                     Name of Variable to be found.
+  @param VendorGuid                       Variable vendor GUID.
+  @param Attributes                       Attribute value of the variable found
+  @param DataSize                         Size of Data found. If size is less than the
+                                          data, this value contains the required size.
+  @param Data                             Data pointer.
+
+  @return EFI_INVALID_PARAMETER           Invalid parameter.
+  @return EFI_SUCCESS                     Set successfully.
+  @return EFI_OUT_OF_RESOURCES            Resource not enough to set variable.
+  @return EFI_NOT_FOUND                   Not found.
+  @return EFI_WRITE_PROTECTED             Variable is read-only.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceSetVariable (
+  IN CHAR16                  *VariableName,
+  IN EFI_GUID                *VendorGuid,
+  IN UINT32                  Attributes,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data
+  );
+
+/**
+
+  This code returns information about the EFI variables.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param Attributes                     Attributes bitmask to specify the type of variables
+                                        on which to return information.
+  @param MaximumVariableStorageSize     Pointer to the maximum size of the storage space available
+                                        for the EFI variables associated with the attributes specified.
+  @param RemainingVariableStorageSize   Pointer to the remaining size of the storage space available
+                                        for EFI variables associated with the attributes specified.
+  @param MaximumVariableSize            Pointer to the maximum size of an individual EFI variables
+                                        associated with the attributes specified.
+
+  @return EFI_SUCCESS                   Query successfully.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceQueryVariableInfoInternal (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize
+  );
+
+/**
+
+  This code returns information about the EFI variables.
+
+  Caution: This function may receive untrusted input.
+  This function may be invoked in SMM mode. This function will do basic validation, before parse the data.
+
+  @param Attributes                     Attributes bitmask to specify the type of variables
+                                        on which to return information.
+  @param MaximumVariableStorageSize     Pointer to the maximum size of the storage space available
+                                        for the EFI variables associated with the attributes specified.
+  @param RemainingVariableStorageSize   Pointer to the remaining size of the storage space available
+                                        for EFI variables associated with the attributes specified.
+  @param MaximumVariableSize            Pointer to the maximum size of an individual EFI variables
+                                        associated with the attributes specified.
+
+  @return EFI_INVALID_PARAMETER         An invalid combination of attribute bits was supplied.
+  @return EFI_SUCCESS                   Query successfully.
+  @return EFI_UNSUPPORTED               The attribute is not supported on this platform.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceQueryVariableInfo (
+  IN  UINT32                 Attributes,
+  OUT UINT64                 *MaximumVariableStorageSize,
+  OUT UINT64                 *RemainingVariableStorageSize,
+  OUT UINT64                 *MaximumVariableSize
+  );
+
+/**
+  Mark a variable that will become read-only after leaving the DXE phase of execution.
+
+  @param[in] This          The VARIABLE_LOCK_PROTOCOL instance.
+  @param[in] VariableName  A pointer to the variable name that will be made read-only subsequently.
+  @param[in] VendorGuid    A pointer to the vendor GUID that will be made read-only subsequently.
+
+  @retval EFI_SUCCESS           The variable specified by the VariableName and the VendorGuid was marked
+                                as pending to be read-only.
+  @retval EFI_INVALID_PARAMETER VariableName or VendorGuid is NULL.
+                                Or VariableName is an empty string.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource to hold the lock request.
+**/
+EFI_STATUS
+EFIAPI
+VariableLockRequestToLock (
+  IN CONST EDKII_VARIABLE_LOCK_PROTOCOL *This,
+  IN       CHAR16                       *VariableName,
+  IN       EFI_GUID                     *VendorGuid
+  );
+
+/**
+  Register SetVariable check handler.
+
+  @param[in] Handler            Pointer to check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckRegisterSetVariableCheckHandler (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  );
+
+/**
+  Variable property set.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertySet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  );
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertyGet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  );
+
+/**
+  Initialize variable quota.
+
+**/
+VOID
+InitializeVariableQuota (
+  VOID
+  );
+
+extern VARIABLE_MODULE_GLOBAL  *mVariableModuleGlobal;
+
+extern AUTH_VAR_LIB_CONTEXT_OUT mAuthContextOut;
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the variable found.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibFindVariable (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  );
+
+/**
+  Finds next variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds next variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the next variable.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibFindNextVariable (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  );
+
+/**
+  Update the variable region with Variable information.
+
+  @param[in] AuthVariableInfo       Pointer AUTH_VARIABLE_INFO structure for
+                                    input of the variable.
+
+  @retval EFI_SUCCESS               The update operation is success.
+  @retval EFI_INVALID_PARAMETER     Invalid parameter.
+  @retval EFI_WRITE_PROTECTED       Variable is write-protected.
+  @retval EFI_OUT_OF_RESOURCES      There is not enough resource.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibUpdateVariable (
+  IN AUTH_VARIABLE_INFO     *AuthVariableInfo
+  );
+
+/**
+  Get scratch buffer.
+
+  @param[in, out] ScratchBufferSize Scratch buffer size. If input size is greater than
+                                    the maximum supported buffer size, this value contains
+                                    the maximum supported buffer size as output.
+  @param[out]     ScratchBuffer     Pointer to scratch buffer address.
+
+  @retval EFI_SUCCESS       Get scratch buffer successfully.
+  @retval EFI_UNSUPPORTED   If input size is greater than the maximum supported buffer size.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibGetScratchBuffer (
+  IN OUT UINTN      *ScratchBufferSize,
+  OUT    VOID       **ScratchBuffer
+  );
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param ...                    The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+BOOLEAN
+EFIAPI
+VariableExLibCheckRemainingSpaceForConsistency (
+  IN UINT32                     Attributes,
+  ...
+  );
+
+/**
+  Return TRUE if at OS runtime.
+
+  @retval TRUE If at OS runtime.
+  @retval FALSE If at boot time.
+
+**/
+BOOLEAN
+EFIAPI
+VariableExLibAtRuntime (
+  VOID
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableDxe.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableDxe.c
new file mode 100644
index 0000000000..caf51dfba6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableDxe.c
@@ -0,0 +1,538 @@
+/** @file
+  Implement all four UEFI Runtime Variable services for the nonvolatile
+  and volatile storage space and install variable architecture protocol.
+
+Copyright (C) 2013, Red Hat, Inc.
+Copyright (c) 2006 - 2015, 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 "Variable.h"
+
+extern VARIABLE_STORE_HEADER   *mNvVariableCache;
+extern VARIABLE_INFO_ENTRY     *gVariableInfo;
+EFI_HANDLE                     mHandle                    = NULL;
+EFI_EVENT                      mVirtualAddressChangeEvent = NULL;
+EFI_EVENT                      mFtwRegistration           = NULL;
+extern BOOLEAN                 mEndOfDxe;
+VOID                           ***mVarCheckAddressPointer = NULL;
+UINTN                          mVarCheckAddressPointerCount = 0;
+EDKII_VARIABLE_LOCK_PROTOCOL   mVariableLock              = { VariableLockRequestToLock };
+EDKII_VAR_CHECK_PROTOCOL       mVarCheck                  = { VarCheckRegisterSetVariableCheckHandler,
+                                                              VarCheckVariablePropertySet,
+                                                              VarCheckVariablePropertyGet };
+
+/**
+  Return TRUE if ExitBootServices () has been called.
+
+  @retval TRUE If ExitBootServices () has been called.
+**/
+BOOLEAN
+AtRuntime (
+  VOID
+  )
+{
+  return EfiAtRuntime ();
+}
+
+
+/**
+  Initializes a basic mutual exclusion lock.
+
+  This function initializes a basic mutual exclusion lock to the released state
+  and returns the lock.  Each lock provides mutual exclusion access at its task
+  priority level.  Since there is no preemption or multiprocessor support in EFI,
+  acquiring the lock only consists of raising to the locks TPL.
+  If Lock is NULL, then ASSERT().
+  If Priority is not a valid TPL value, then ASSERT().
+
+  @param  Lock       A pointer to the lock data structure to initialize.
+  @param  Priority   EFI TPL is associated with the lock.
+
+  @return The lock.
+
+**/
+EFI_LOCK *
+InitializeLock (
+  IN OUT EFI_LOCK                         *Lock,
+  IN     EFI_TPL                          Priority
+  )
+{
+  return EfiInitializeLock (Lock, Priority);
+}
+
+
+/**
+  Acquires lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function that will be removed when
+  EfiAcquireLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiAcquireLock() at boot time, and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to acquire.
+
+**/
+VOID
+AcquireLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+  if (!AtRuntime ()) {
+    EfiAcquireLock (Lock);
+  }
+}
+
+
+/**
+  Releases lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiReleaseLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiReleaseLock() at boot time and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to release.
+
+**/
+VOID
+ReleaseLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+  if (!AtRuntime ()) {
+    EfiReleaseLock (Lock);
+  }
+}
+
+/**
+  Retrive the Fault Tolerent Write protocol interface.
+
+  @param[out] FtwProtocol       The interface of Ftw protocol
+
+  @retval EFI_SUCCESS           The FTW protocol instance was found and returned in FtwProtocol.
+  @retval EFI_NOT_FOUND         The FTW protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+GetFtwProtocol (
+  OUT VOID                                **FtwProtocol
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate Fault Tolerent Write protocol
+  //
+  Status = gBS->LocateProtocol (
+                  &gEfiFaultTolerantWriteProtocolGuid,
+                  NULL,
+                  FtwProtocol
+                  );
+  return Status;
+}
+
+/**
+  Retrive the FVB protocol interface by HANDLE.
+
+  @param[in]  FvBlockHandle     The handle of FVB protocol that provides services for
+                                reading, writing, and erasing the target block.
+  @param[out] FvBlock           The interface of FVB protocol
+
+  @retval EFI_SUCCESS           The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED       The device does not support the FVB protocol.
+  @retval EFI_INVALID_PARAMETER FvBlockHandle is not a valid EFI_HANDLE or FvBlock is NULL.
+
+**/
+EFI_STATUS
+GetFvbByHandle (
+  IN  EFI_HANDLE                          FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  )
+{
+  //
+  // To get the FVB protocol interface on the handle
+  //
+  return gBS->HandleProtocol (
+                FvBlockHandle,
+                &gEfiFirmwareVolumeBlockProtocolGuid,
+                (VOID **) FvBlock
+                );
+}
+
+
+/**
+  Function returns an array of handles that support the FVB protocol
+  in a buffer allocated from pool.
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate all handles of Fvb protocol
+  //
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareVolumeBlockProtocolGuid,
+                  NULL,
+                  NumberHandles,
+                  Buffer
+                  );
+  return Status;
+}
+
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableClassAddressChangeEvent (
+  IN EFI_EVENT                            Event,
+  IN VOID                                 *Context
+  )
+{
+  UINTN          Index;
+
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->GetBlockSize);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->GetPhysicalAddress);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->GetAttributes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->SetAttributes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->Read);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->Write);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance->EraseBlocks);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->FvbInstance);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->PlatformLangCodes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->LangCodes);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->PlatformLang);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->VariableGlobal.NonVolatileVariableBase);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->VariableGlobal.VolatileVariableBase);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal->VariableGlobal.HobVariableBase);
+  EfiConvertPointer (0x0, (VOID **) &mVariableModuleGlobal);
+  EfiConvertPointer (0x0, (VOID **) &mNvVariableCache);
+
+  if (mAuthContextOut.AddressPointer != NULL) {
+    for (Index = 0; Index < mAuthContextOut.AddressPointerCount; Index++) {
+      EfiConvertPointer (0x0, (VOID **) mAuthContextOut.AddressPointer[Index]);
+    }
+  }
+
+  if (mVarCheckAddressPointer != NULL) {
+    for (Index = 0; Index < mVarCheckAddressPointerCount; Index++) {
+      EfiConvertPointer (0x0, (VOID **) mVarCheckAddressPointer[Index]);
+    }
+  }
+}
+
+
+/**
+  Notification function of EVT_GROUP_READY_TO_BOOT event group.
+
+  This is a notification function registered on EVT_GROUP_READY_TO_BOOT event group.
+  When the Boot Manager is about to load and execute a boot option, it reclaims variable
+  storage if free size is below the threshold.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+OnReadyToBoot (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  if (!mEndOfDxe) {
+    //
+    // Set the End Of DXE bit in case the EFI_END_OF_DXE_EVENT_GROUP_GUID event is not signaled.
+    //
+    mEndOfDxe = TRUE;
+    mVarCheckAddressPointer = VarCheckLibInitializeAtEndOfDxe (&mVarCheckAddressPointerCount);
+    //
+    // The initialization for variable quota.
+    //
+    InitializeVariableQuota ();
+  }
+  ReclaimForOS ();
+  if (FeaturePcdGet (PcdVariableCollectStatistics)) {
+    if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+      gBS->InstallConfigurationTable (&gEfiAuthenticatedVariableGuid, gVariableInfo);
+    } else {
+      gBS->InstallConfigurationTable (&gEfiVariableGuid, gVariableInfo);
+    }
+  }
+
+  gBS->CloseEvent (Event);
+}
+
+/**
+  Notification function of EFI_END_OF_DXE_EVENT_GROUP_GUID event group.
+
+  This is a notification function registered on EFI_END_OF_DXE_EVENT_GROUP_GUID event group.
+
+  @param  Event        Event whose notification function is being invoked.
+  @param  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+OnEndOfDxe (
+  EFI_EVENT                               Event,
+  VOID                                    *Context
+  )
+{
+  DEBUG ((EFI_D_INFO, "[Variable]END_OF_DXE is signaled\n"));
+  mEndOfDxe = TRUE;
+  mVarCheckAddressPointer = VarCheckLibInitializeAtEndOfDxe (&mVarCheckAddressPointerCount);
+  //
+  // The initialization for variable quota.
+  //
+  InitializeVariableQuota ();
+  if (PcdGetBool (PcdReclaimVariableSpaceAtEndOfDxe)) {
+    ReclaimForOS ();
+  }
+
+  gBS->CloseEvent (Event);
+}
+
+/**
+  Fault Tolerant Write protocol notification event handler.
+
+  Non-Volatile variable write may needs FTW protocol to reclaim when
+  writting variable.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+FtwNotificationEvent (
+  IN  EFI_EVENT                           Event,
+  IN  VOID                                *Context
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL      *FvbProtocol;
+  EFI_FAULT_TOLERANT_WRITE_PROTOCOL       *FtwProtocol;
+  EFI_PHYSICAL_ADDRESS                    NvStorageVariableBase;
+  EFI_GCD_MEMORY_SPACE_DESCRIPTOR         GcdDescriptor;
+  EFI_PHYSICAL_ADDRESS                    BaseAddress;
+  UINT64                                  Length;
+  EFI_PHYSICAL_ADDRESS                    VariableStoreBase;
+  UINT64                                  VariableStoreLength;
+  UINTN                                   FtwMaxBlockSize;
+
+  //
+  // Ensure FTW protocol is installed.
+  //
+  Status = GetFtwProtocol ((VOID**) &FtwProtocol);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  Status = FtwProtocol->GetMaxBlockSize (FtwProtocol, &FtwMaxBlockSize);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (PcdGet32 (PcdFlashNvStorageVariableSize) <= FtwMaxBlockSize);
+  }
+
+  //
+  // Find the proper FVB protocol for variable.
+  //
+  NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageVariableBase64);
+  if (NvStorageVariableBase == 0) {
+    NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageVariableBase);
+  }
+  Status = GetFvbInfoByAddress (NvStorageVariableBase, NULL, &FvbProtocol);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+  mVariableModuleGlobal->FvbInstance = FvbProtocol;
+
+  //
+  // Mark the variable storage region of the FLASH as RUNTIME.
+  //
+  VariableStoreBase   = NvStorageVariableBase + (((EFI_FIRMWARE_VOLUME_HEADER *)(UINTN)(NvStorageVariableBase))->HeaderLength);
+  VariableStoreLength = ((VARIABLE_STORE_HEADER *)(UINTN)VariableStoreBase)->Size;
+  BaseAddress = VariableStoreBase & (~EFI_PAGE_MASK);
+  Length      = VariableStoreLength + (VariableStoreBase - BaseAddress);
+  Length      = (Length + EFI_PAGE_SIZE - 1) & (~EFI_PAGE_MASK);
+
+  Status      = gDS->GetMemorySpaceDescriptor (BaseAddress, &GcdDescriptor);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_WARN, "Variable driver failed to get flash memory attribute.\n"));
+  } else {
+    Status = gDS->SetMemorySpaceAttributes (
+                    BaseAddress,
+                    Length,
+                    GcdDescriptor.Attributes | EFI_MEMORY_RUNTIME
+                    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_WARN, "Variable driver failed to add EFI_MEMORY_RUNTIME attribute to Flash.\n"));
+    }
+  }
+
+  Status = VariableWriteServiceInitialize ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Variable write service initialization failed. Status = %r\n", Status));
+  }
+
+  //
+  // Install the Variable Write Architectural protocol.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiVariableWriteArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Close the notify event to avoid install gEfiVariableWriteArchProtocolGuid again.
+  //
+  gBS->CloseEvent (Event);
+
+}
+
+
+/**
+  Variable Driver main entry point. The Variable driver places the 4 EFI
+  runtime services in the EFI System Table and installs arch protocols
+  for variable read and write services being available. It also registers
+  a notification function for an EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       Variable service successfully initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceInitialize (
+  IN EFI_HANDLE                         ImageHandle,
+  IN EFI_SYSTEM_TABLE                   *SystemTable
+  )
+{
+  EFI_STATUS                            Status;
+  EFI_EVENT                             ReadyToBootEvent;
+  EFI_EVENT                             EndOfDxeEvent;
+
+  Status = VariableCommonInitialize ();
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEdkiiVariableLockProtocolGuid,
+                  &mVariableLock,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEdkiiVarCheckProtocolGuid,
+                  &mVarCheck,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  SystemTable->RuntimeServices->GetVariable         = VariableServiceGetVariable;
+  SystemTable->RuntimeServices->GetNextVariableName = VariableServiceGetNextVariableName;
+  SystemTable->RuntimeServices->SetVariable         = VariableServiceSetVariable;
+  SystemTable->RuntimeServices->QueryVariableInfo   = VariableServiceQueryVariableInfo;
+
+  //
+  // Now install the Variable Runtime Architectural protocol on a new handle.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiVariableArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register FtwNotificationEvent () notify function.
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEfiFaultTolerantWriteProtocolGuid,
+    TPL_CALLBACK,
+    FtwNotificationEvent,
+    (VOID *)SystemTable,
+    &mFtwRegistration
+    );
+
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  VariableClassAddressChangeEvent,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mVirtualAddressChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register the event handling function to reclaim variable for OS usage.
+  //
+  Status = EfiCreateEventReadyToBootEx (
+             TPL_NOTIFY,
+             OnReadyToBoot,
+             NULL,
+             &ReadyToBootEvent
+             );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register the event handling function to set the End Of DXE flag.
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  OnEndOfDxe,
+                  NULL,
+                  &gEfiEndOfDxeEventGroupGuid,
+                  &EndOfDxeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableExLib.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableExLib.c
new file mode 100644
index 0000000000..615b4cec28
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableExLib.c
@@ -0,0 +1,256 @@
+/** @file
+  Provides variable driver extended services.
+
+Copyright (c) 2015, 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 "Variable.h"
+
+/**
+  Finds variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the variable found.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibFindVariable (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  )
+{
+  EFI_STATUS                    Status;
+  VARIABLE_POINTER_TRACK        Variable;
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariable;
+
+  Status = FindVariable (
+             VariableName,
+             VendorGuid,
+             &Variable,
+             &mVariableModuleGlobal->VariableGlobal,
+             FALSE
+             );
+  if (EFI_ERROR (Status)) {
+    AuthVariableInfo->Data = NULL;
+    AuthVariableInfo->DataSize = 0;
+    AuthVariableInfo->Attributes = 0;
+    AuthVariableInfo->PubKeyIndex = 0;
+    AuthVariableInfo->MonotonicCount = 0;
+    AuthVariableInfo->TimeStamp = NULL;
+    return Status;
+  }
+
+  AuthVariableInfo->DataSize        = DataSizeOfVariable (Variable.CurrPtr);
+  AuthVariableInfo->Data            = GetVariableDataPtr (Variable.CurrPtr);
+  AuthVariableInfo->Attributes      = Variable.CurrPtr->Attributes;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    AuthVariable = (AUTHENTICATED_VARIABLE_HEADER *) Variable.CurrPtr;
+    AuthVariableInfo->PubKeyIndex     = AuthVariable->PubKeyIndex;
+    AuthVariableInfo->MonotonicCount  = ReadUnaligned64 (&(AuthVariable->MonotonicCount));
+    AuthVariableInfo->TimeStamp       = &AuthVariable->TimeStamp;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Finds next variable in storage blocks of volatile and non-volatile storage areas.
+
+  This code finds next variable in storage blocks of volatile and non-volatile storage areas.
+  If VariableName is an empty string, then we just return the first
+  qualified variable without comparing VariableName and VendorGuid.
+
+  @param[in]  VariableName          Name of the variable to be found.
+  @param[in]  VendorGuid            Variable vendor GUID to be found.
+  @param[out] AuthVariableInfo      Pointer to AUTH_VARIABLE_INFO structure for
+                                    output of the next variable.
+
+  @retval EFI_INVALID_PARAMETER     If VariableName is not an empty string,
+                                    while VendorGuid is NULL.
+  @retval EFI_SUCCESS               Variable successfully found.
+  @retval EFI_NOT_FOUND             Variable not found
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibFindNextVariable (
+  IN  CHAR16                *VariableName,
+  IN  EFI_GUID              *VendorGuid,
+  OUT AUTH_VARIABLE_INFO    *AuthVariableInfo
+  )
+{
+  EFI_STATUS                    Status;
+  VARIABLE_HEADER               *VariablePtr;
+  AUTHENTICATED_VARIABLE_HEADER *AuthVariablePtr;
+
+  Status = VariableServiceGetNextVariableInternal (
+             VariableName,
+             VendorGuid,
+             &VariablePtr
+             );
+  if (EFI_ERROR (Status)) {
+    AuthVariableInfo->VariableName = NULL;
+    AuthVariableInfo->VendorGuid = NULL;
+    AuthVariableInfo->Data = NULL;
+    AuthVariableInfo->DataSize = 0;
+    AuthVariableInfo->Attributes = 0;
+    AuthVariableInfo->PubKeyIndex = 0;
+    AuthVariableInfo->MonotonicCount = 0;
+    AuthVariableInfo->TimeStamp = NULL;
+    return Status;
+  }
+
+  AuthVariableInfo->VariableName    = GetVariableNamePtr (VariablePtr);
+  AuthVariableInfo->VendorGuid      = GetVendorGuidPtr (VariablePtr);
+  AuthVariableInfo->DataSize        = DataSizeOfVariable (VariablePtr);
+  AuthVariableInfo->Data            = GetVariableDataPtr (VariablePtr);
+  AuthVariableInfo->Attributes      = VariablePtr->Attributes;
+  if (mVariableModuleGlobal->VariableGlobal.AuthFormat) {
+    AuthVariablePtr = (AUTHENTICATED_VARIABLE_HEADER *) VariablePtr;
+    AuthVariableInfo->PubKeyIndex     = AuthVariablePtr->PubKeyIndex;
+    AuthVariableInfo->MonotonicCount  = ReadUnaligned64 (&(AuthVariablePtr->MonotonicCount));
+    AuthVariableInfo->TimeStamp       = &AuthVariablePtr->TimeStamp;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Update the variable region with Variable information.
+
+  @param[in] AuthVariableInfo       Pointer AUTH_VARIABLE_INFO structure for
+                                    input of the variable.
+
+  @retval EFI_SUCCESS               The update operation is success.
+  @retval EFI_INVALID_PARAMETER     Invalid parameter.
+  @retval EFI_WRITE_PROTECTED       Variable is write-protected.
+  @retval EFI_OUT_OF_RESOURCES      There is not enough resource.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibUpdateVariable (
+  IN AUTH_VARIABLE_INFO     *AuthVariableInfo
+  )
+{
+  VARIABLE_POINTER_TRACK    Variable;
+
+  FindVariable (AuthVariableInfo->VariableName, AuthVariableInfo->VendorGuid, &Variable, &mVariableModuleGlobal->VariableGlobal, FALSE);
+  return UpdateVariable (
+           AuthVariableInfo->VariableName,
+           AuthVariableInfo->VendorGuid,
+           AuthVariableInfo->Data,
+           AuthVariableInfo->DataSize,
+           AuthVariableInfo->Attributes,
+           AuthVariableInfo->PubKeyIndex,
+           AuthVariableInfo->MonotonicCount,
+           &Variable,
+           AuthVariableInfo->TimeStamp
+           );
+}
+
+/**
+  Get scratch buffer.
+
+  @param[in, out] ScratchBufferSize Scratch buffer size. If input size is greater than
+                                    the maximum supported buffer size, this value contains
+                                    the maximum supported buffer size as output.
+  @param[out]     ScratchBuffer     Pointer to scratch buffer address.
+
+  @retval EFI_SUCCESS       Get scratch buffer successfully.
+  @retval EFI_UNSUPPORTED   If input size is greater than the maximum supported buffer size.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableExLibGetScratchBuffer (
+  IN OUT UINTN      *ScratchBufferSize,
+  OUT    VOID       **ScratchBuffer
+  )
+{
+  UINTN MaxBufferSize;
+
+  MaxBufferSize = mVariableModuleGlobal->ScratchBufferSize;
+  if (*ScratchBufferSize > MaxBufferSize) {
+    *ScratchBufferSize = MaxBufferSize;
+    return EFI_UNSUPPORTED;
+  }
+
+  *ScratchBuffer = GetEndPointer ((VARIABLE_STORE_HEADER *) ((UINTN) mVariableModuleGlobal->VariableGlobal.VolatileVariableBase));
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is to check if the remaining variable space is enough to set
+  all Variables from argument list successfully. The purpose of the check
+  is to keep the consistency of the Variables to be in variable storage.
+
+  Note: Variables are assumed to be in same storage.
+  The set sequence of Variables will be same with the sequence of VariableEntry from argument list,
+  so follow the argument sequence to check the Variables.
+
+  @param[in] Attributes         Variable attributes for Variable entries.
+  @param ...                    The variable argument list with type VARIABLE_ENTRY_CONSISTENCY *.
+                                A NULL terminates the list. The VariableSize of
+                                VARIABLE_ENTRY_CONSISTENCY is the variable data size as input.
+                                It will be changed to variable total size as output.
+
+  @retval TRUE                  Have enough variable space to set the Variables successfully.
+  @retval FALSE                 No enough variable space to set the Variables successfully.
+
+**/
+BOOLEAN
+EFIAPI
+VariableExLibCheckRemainingSpaceForConsistency (
+  IN UINT32                     Attributes,
+  ...
+  )
+{
+  VA_LIST Marker;
+  BOOLEAN Return;
+
+  VA_START (Marker, Attributes);
+
+  Return = CheckRemainingSpaceForConsistencyInternal (Attributes, Marker);
+
+  VA_END (Marker);
+
+  return Return;
+}
+
+/**
+  Return TRUE if at OS runtime.
+
+  @retval TRUE If at OS runtime.
+  @retval FALSE If at boot time.
+
+**/
+BOOLEAN
+EFIAPI
+VariableExLibAtRuntime (
+  VOID
+  )
+{
+  return AtRuntime ();
+}
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.inf b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.inf
new file mode 100644
index 0000000000..5e7b5c5341
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.inf
@@ -0,0 +1,133 @@
+## @file
+#  Provides variable service.
+#
+#  This module installs variable arch protocol and variable write arch protocol to provide
+#  variable services: SetVariable, GetVariable, GetNextVariableName and QueryVariableInfo.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - variable data.
+#  This external input must be validated carefully to avoid security issues such as
+#  buffer overflow or integer overflow.
+#
+# Copyright (c) 2006 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VariableRuntimeDxe
+  MODULE_UNI_FILE                = VariableRuntimeDxe.uni
+  FILE_GUID                      = CBD2E4D5-7068-4FF5-B462-9822B4AD8D60
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = VariableServiceInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 EBC
+#
+#  VIRTUAL_ADDRESS_MAP_CALLBACK  =  VariableClassAddressChangeEvent
+#
+
+[Sources]
+  Reclaim.c
+  Variable.c
+  VariableDxe.c
+  Variable.h
+  Measurement.c
+  TcgMorLockDxe.c
+  VarCheck.c
+  VariableExLib.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  SynchronizationLib
+  UefiLib
+  UefiBootServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  UefiRuntimeLib
+  DxeServicesTableLib
+  UefiDriverEntryPoint
+  PcdLib
+  HobLib
+  TpmMeasurementLib
+  AuthVariableLib
+  VarCheckLib
+
+[Protocols]
+  gEfiFirmwareVolumeBlockProtocolGuid           ## CONSUMES
+  ## CONSUMES
+  ## NOTIFY
+  gEfiFaultTolerantWriteProtocolGuid
+  gEfiVariableWriteArchProtocolGuid             ## PRODUCES
+  gEfiVariableArchProtocolGuid                  ## PRODUCES
+  gEdkiiVariableLockProtocolGuid                ## PRODUCES
+  gEdkiiVarCheckProtocolGuid                    ## PRODUCES
+
+[Guids]
+  ## PRODUCES             ## GUID # Signature of Variable store header
+  ## CONSUMES             ## GUID # Signature of Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiAuthenticatedVariableGuid
+
+  ## PRODUCES             ## GUID # Signature of Variable store header
+  ## CONSUMES             ## GUID # Signature of Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiVariableGuid
+
+  ## SOMETIMES_CONSUMES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_CONSUMES   ## Variable:L"Lang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"Lang"
+  gEfiGlobalVariableGuid
+
+  gEfiMemoryOverwriteControlDataGuid            ## CONSUMES   ## Variable:L"MemoryOverwriteRequestControl"
+  gEfiMemoryOverwriteRequestControlLockGuid     ## PRODUCES   ## Variable:L"MemoryOverwriteRequestControlLock"
+
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES             ## Event
+  gEfiSystemNvDataFvGuid                        ## CONSUMES             ## GUID
+  gEfiEndOfDxeEventGroupGuid                    ## CONSUMES             ## Event
+  gEdkiiFaultTolerantWriteGuid                  ## SOMETIMES_CONSUMES   ## HOB
+  gEdkiiVarErrorFlagGuid                        ## CONSUMES             ## GUID
+
+  ## SOMETIMES_CONSUMES   ## Variable:L"DB"
+  ## SOMETIMES_CONSUMES   ## Variable:L"DBX"
+  gEfiImageSecurityDatabaseGuid
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableSize      ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase      ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase64    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxVariableSize                 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxAuthVariableSize             ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxHardwareErrorVariableSize    ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableStoreSize               ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHwErrStorageSize                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxUserNvVariableSpaceSize           ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBoottimeReservedNvVariableSpaceSize  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdReclaimVariableSpaceAtEndOfDxe  ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableCollectStatistics  ## CONSUMES # statistic the information of variable.
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLangDeprecate ## CONSUMES # Auto update PlatformLang/Lang
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  VariableRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.uni
new file mode 100644
index 0000000000..48745b62f5
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..e7361c8512
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.c
new file mode 100644
index 0000000000..fb16af31b1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.c
@@ -0,0 +1,1002 @@
+/** @file
+  The sample implementation for SMM variable protocol. And this driver
+  implements an SMI handler to communicate with the DXE runtime driver
+  to provide variable services.
+
+  Caution: This module requires additional review when modified.
+  This driver will have external input - variable data and communicate buffer in SMM mode.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  SmmVariableHandler() will receive untrusted input and do basic validation.
+
+  Each sub function VariableServiceGetVariable(), VariableServiceGetNextVariableName(),
+  VariableServiceSetVariable(), VariableServiceQueryVariableInfo(), ReclaimForOS(),
+  SmmVariableGetStatistics() should also do validation based on its own knowledge.
+
+Copyright (c) 2010 - 2015, 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 <Protocol/SmmVariable.h>
+#include <Protocol/SmmFirmwareVolumeBlock.h>
+#include <Protocol/SmmFaultTolerantWrite.h>
+#include <Protocol/SmmEndOfDxe.h>
+#include <Protocol/SmmVarCheck.h>
+
+#include <Library/SmmServicesTableLib.h>
+#include <Library/SmmMemLib.h>
+
+#include <Guid/SmmVariableCommon.h>
+#include <Guid/ZeroGuid.h>
+#include "Variable.h"
+
+extern VARIABLE_INFO_ENTRY                           *gVariableInfo;
+EFI_HANDLE                                           mSmmVariableHandle      = NULL;
+EFI_HANDLE                                           mVariableHandle         = NULL;
+BOOLEAN                                              mAtRuntime              = FALSE;
+UINT8                                                *mVariableBufferPayload = NULL;
+UINTN                                                mVariableBufferPayloadSize;
+extern BOOLEAN                                       mEndOfDxe;
+extern VAR_CHECK_REQUEST_SOURCE                      mRequestSource;
+
+/**
+  SecureBoot Hook for SetVariable.
+
+  @param[in] VariableName                 Name of Variable to be found.
+  @param[in] VendorGuid                   Variable vendor GUID.
+
+**/
+VOID
+EFIAPI
+SecureBootHook (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  )
+{
+  return ;
+}
+
+/**
+
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  @param VariableName                     Name of Variable to be found.
+  @param VendorGuid                       Variable vendor GUID.
+  @param Attributes                       Attribute value of the variable found
+  @param DataSize                         Size of Data found. If size is less than the
+                                          data, this value contains the required size.
+  @param Data                             Data pointer.
+
+  @return EFI_INVALID_PARAMETER           Invalid parameter.
+  @return EFI_SUCCESS                     Set successfully.
+  @return EFI_OUT_OF_RESOURCES            Resource not enough to set variable.
+  @return EFI_NOT_FOUND                   Not found.
+  @return EFI_WRITE_PROTECTED             Variable is read-only.
+
+**/
+EFI_STATUS
+EFIAPI
+SmmVariableSetVariable (
+  IN CHAR16                  *VariableName,
+  IN EFI_GUID                *VendorGuid,
+  IN UINT32                  Attributes,
+  IN UINTN                   DataSize,
+  IN VOID                    *Data
+  )
+{
+  EFI_STATUS                 Status;
+
+  //
+  // Disable write protection when the calling SetVariable() through EFI_SMM_VARIABLE_PROTOCOL.
+  //
+  mRequestSource = VarCheckFromTrusted;
+  Status         = VariableServiceSetVariable (
+                     VariableName,
+                     VendorGuid,
+                     Attributes,
+                     DataSize,
+                     Data
+                     );
+  mRequestSource = VarCheckFromUntrusted;
+  return Status;
+}
+
+EFI_SMM_VARIABLE_PROTOCOL      gSmmVariable = {
+  VariableServiceGetVariable,
+  VariableServiceGetNextVariableName,
+  SmmVariableSetVariable,
+  VariableServiceQueryVariableInfo
+};
+
+EDKII_SMM_VAR_CHECK_PROTOCOL mSmmVarCheck = { VarCheckRegisterSetVariableCheckHandler,
+                                              VarCheckVariablePropertySet,
+                                              VarCheckVariablePropertyGet };
+
+/**
+  Return TRUE if ExitBootServices () has been called.
+
+  @retval TRUE If ExitBootServices () has been called.
+**/
+BOOLEAN
+AtRuntime (
+  VOID
+  )
+{
+  return mAtRuntime;
+}
+
+/**
+  Initializes a basic mutual exclusion lock.
+
+  This function initializes a basic mutual exclusion lock to the released state
+  and returns the lock.  Each lock provides mutual exclusion access at its task
+  priority level.  Since there is no preemption or multiprocessor support in EFI,
+  acquiring the lock only consists of raising to the locks TPL.
+  If Lock is NULL, then ASSERT().
+  If Priority is not a valid TPL value, then ASSERT().
+
+  @param  Lock       A pointer to the lock data structure to initialize.
+  @param  Priority   EFI TPL is associated with the lock.
+
+  @return The lock.
+
+**/
+EFI_LOCK *
+InitializeLock (
+  IN OUT EFI_LOCK                         *Lock,
+  IN EFI_TPL                              Priority
+  )
+{
+  return Lock;
+}
+
+/**
+  Acquires lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function that will be removed when
+  EfiAcquireLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiAcquireLock() at boot time, and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to acquire.
+
+**/
+VOID
+AcquireLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+
+}
+
+
+/**
+  Releases lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiReleaseLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiReleaseLock() at boot time and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to release.
+
+**/
+VOID
+ReleaseLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+
+}
+
+/**
+  Retrive the SMM Fault Tolerent Write protocol interface.
+
+  @param[out] FtwProtocol       The interface of SMM Ftw protocol
+
+  @retval EFI_SUCCESS           The SMM FTW protocol instance was found and returned in FtwProtocol.
+  @retval EFI_NOT_FOUND         The SMM FTW protocol instance was not found.
+  @retval EFI_INVALID_PARAMETER SarProtocol is NULL.
+
+**/
+EFI_STATUS
+GetFtwProtocol (
+  OUT VOID                                **FtwProtocol
+  )
+{
+  EFI_STATUS                              Status;
+
+  //
+  // Locate Smm Fault Tolerent Write protocol
+  //
+  Status = gSmst->SmmLocateProtocol (
+                    &gEfiSmmFaultTolerantWriteProtocolGuid,
+                    NULL,
+                    FtwProtocol
+                    );
+  return Status;
+}
+
+
+/**
+  Retrive the SMM FVB protocol interface by HANDLE.
+
+  @param[in]  FvBlockHandle     The handle of SMM FVB protocol that provides services for
+                                reading, writing, and erasing the target block.
+  @param[out] FvBlock           The interface of SMM FVB protocol
+
+  @retval EFI_SUCCESS           The interface information for the specified protocol was returned.
+  @retval EFI_UNSUPPORTED       The device does not support the SMM FVB protocol.
+  @retval EFI_INVALID_PARAMETER FvBlockHandle is not a valid EFI_HANDLE or FvBlock is NULL.
+
+**/
+EFI_STATUS
+GetFvbByHandle (
+  IN  EFI_HANDLE                          FvBlockHandle,
+  OUT EFI_FIRMWARE_VOLUME_BLOCK_PROTOCOL  **FvBlock
+  )
+{
+  //
+  // To get the SMM FVB protocol interface on the handle
+  //
+  return gSmst->SmmHandleProtocol (
+                  FvBlockHandle,
+                  &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                  (VOID **) FvBlock
+                  );
+}
+
+
+/**
+  Function returns an array of handles that support the SMM FVB protocol
+  in a buffer allocated from pool.
+
+  @param[out]  NumberHandles    The number of handles returned in Buffer.
+  @param[out]  Buffer           A pointer to the buffer to return the requested
+                                array of  handles that support SMM FVB protocol.
+
+  @retval EFI_SUCCESS           The array of handles was returned in Buffer, and the number of
+                                handles in Buffer was returned in NumberHandles.
+  @retval EFI_NOT_FOUND         No SMM FVB handle was found.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough pool memory to store the matching results.
+  @retval EFI_INVALID_PARAMETER NumberHandles is NULL or Buffer is NULL.
+
+**/
+EFI_STATUS
+GetFvbCountAndBuffer (
+  OUT UINTN                               *NumberHandles,
+  OUT EFI_HANDLE                          **Buffer
+  )
+{
+  EFI_STATUS                              Status;
+  UINTN                                   BufferSize;
+
+  if ((NumberHandles == NULL) || (Buffer == NULL)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  BufferSize     = 0;
+  *NumberHandles = 0;
+  *Buffer        = NULL;
+  Status = gSmst->SmmLocateHandle (
+                    ByProtocol,
+                    &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                    NULL,
+                    &BufferSize,
+                    *Buffer
+                    );
+  if (EFI_ERROR(Status) && Status != EFI_BUFFER_TOO_SMALL) {
+    return EFI_NOT_FOUND;
+  }
+
+  *Buffer = AllocatePool (BufferSize);
+  if (*Buffer == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Status = gSmst->SmmLocateHandle (
+                    ByProtocol,
+                    &gEfiSmmFirmwareVolumeBlockProtocolGuid,
+                    NULL,
+                    &BufferSize,
+                    *Buffer
+                    );
+
+  *NumberHandles = BufferSize / sizeof(EFI_HANDLE);
+  if (EFI_ERROR(Status)) {
+    *NumberHandles = 0;
+    FreePool (*Buffer);
+    *Buffer = NULL;
+  }
+
+  return Status;
+}
+
+
+/**
+  Get the variable statistics information from the information buffer pointed by gVariableInfo.
+
+  Caution: This function may be invoked at SMM runtime.
+  InfoEntry and InfoSize are external input. Care must be taken to make sure not security issue at runtime.
+
+  @param[in, out]  InfoEntry    A pointer to the buffer of variable information entry.
+                                On input, point to the variable information returned last time. if
+                                InfoEntry->VendorGuid is zero, return the first information.
+                                On output, point to the next variable information.
+  @param[in, out]  InfoSize     On input, the size of the variable information buffer.
+                                On output, the returned variable information size.
+
+  @retval EFI_SUCCESS           The variable information is found and returned successfully.
+  @retval EFI_UNSUPPORTED       No variable inoformation exists in variable driver. The
+                                PcdVariableCollectStatistics should be set TRUE to support it.
+  @retval EFI_BUFFER_TOO_SMALL  The buffer is too small to hold the next variable information.
+  @retval EFI_INVALID_PARAMETER Input parameter is invalid.
+
+**/
+EFI_STATUS
+SmmVariableGetStatistics (
+  IN OUT VARIABLE_INFO_ENTRY                           *InfoEntry,
+  IN OUT UINTN                                         *InfoSize
+  )
+{
+  VARIABLE_INFO_ENTRY                                  *VariableInfo;
+  UINTN                                                NameLength;
+  UINTN                                                StatisticsInfoSize;
+  CHAR16                                               *InfoName;
+  EFI_GUID                                             VendorGuid;
+
+  if (InfoEntry == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableInfo = gVariableInfo;
+  if (VariableInfo == NULL) {
+    return EFI_UNSUPPORTED;
+  }
+
+  StatisticsInfoSize = sizeof (VARIABLE_INFO_ENTRY) + StrSize (VariableInfo->Name);
+  if (*InfoSize < StatisticsInfoSize) {
+    *InfoSize = StatisticsInfoSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+  InfoName = (CHAR16 *)(InfoEntry + 1);
+
+  CopyGuid (&VendorGuid, &InfoEntry->VendorGuid);
+
+  if (CompareGuid (&VendorGuid, &gZeroGuid)) {
+    //
+    // Return the first variable info
+    //
+    CopyMem (InfoEntry, VariableInfo, sizeof (VARIABLE_INFO_ENTRY));
+    CopyMem (InfoName, VariableInfo->Name, StrSize (VariableInfo->Name));
+    *InfoSize = StatisticsInfoSize;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Get the next variable info
+  //
+  while (VariableInfo != NULL) {
+    if (CompareGuid (&VariableInfo->VendorGuid, &VendorGuid)) {
+      NameLength = StrSize (VariableInfo->Name);
+      if (NameLength == StrSize (InfoName)) {
+        if (CompareMem (VariableInfo->Name, InfoName, NameLength) == 0) {
+          //
+          // Find the match one
+          //
+          VariableInfo = VariableInfo->Next;
+          break;
+        }
+      }
+    }
+    VariableInfo = VariableInfo->Next;
+  };
+
+  if (VariableInfo == NULL) {
+    *InfoSize = 0;
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Output the new variable info
+  //
+  StatisticsInfoSize = sizeof (VARIABLE_INFO_ENTRY) + StrSize (VariableInfo->Name);
+  if (*InfoSize < StatisticsInfoSize) {
+    *InfoSize = StatisticsInfoSize;
+    return EFI_BUFFER_TOO_SMALL;
+  }
+
+  CopyMem (InfoEntry, VariableInfo, sizeof (VARIABLE_INFO_ENTRY));
+  CopyMem (InfoName, VariableInfo->Name, StrSize (VariableInfo->Name));
+  *InfoSize = StatisticsInfoSize;
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Communication service SMI Handler entry.
+
+  This SMI handler provides services for the variable wrapper driver.
+
+  Caution: This function may receive untrusted input.
+  This variable data and communicate buffer are external input, so this function will do basic validation.
+  Each sub function VariableServiceGetVariable(), VariableServiceGetNextVariableName(),
+  VariableServiceSetVariable(), VariableServiceQueryVariableInfo(), ReclaimForOS(),
+  SmmVariableGetStatistics() should also do validation based on its own knowledge.
+
+  @param[in]     DispatchHandle  The unique handle assigned to this handler by SmiHandlerRegister().
+  @param[in]     RegisterContext Points to an optional handler context which was specified when the
+                                 handler was registered.
+  @param[in, out] CommBuffer     A pointer to a collection of data in memory that will
+                                 be conveyed from a non-SMM environment into an SMM environment.
+  @param[in, out] CommBufferSize The size of the CommBuffer.
+
+  @retval EFI_SUCCESS                         The interrupt was handled and quiesced. No other handlers
+                                              should still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_QUIESCED  The interrupt has been quiesced but other handlers should
+                                              still be called.
+  @retval EFI_WARN_INTERRUPT_SOURCE_PENDING   The interrupt is still pending and other handlers should still
+                                              be called.
+  @retval EFI_INTERRUPT_PENDING               The interrupt could not be quiesced.
+**/
+EFI_STATUS
+EFIAPI
+SmmVariableHandler (
+  IN     EFI_HANDLE                                DispatchHandle,
+  IN     CONST VOID                                *RegisterContext,
+  IN OUT VOID                                      *CommBuffer,
+  IN OUT UINTN                                     *CommBufferSize
+  )
+{
+  EFI_STATUS                                       Status;
+  SMM_VARIABLE_COMMUNICATE_HEADER                  *SmmVariableFunctionHeader;
+  SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE         *SmmVariableHeader;
+  SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME  *GetNextVariableName;
+  SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO     *QueryVariableInfo;
+  SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE        *GetPayloadSize;
+  VARIABLE_INFO_ENTRY                              *VariableInfo;
+  SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE           *VariableToLock;
+  SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *CommVariableProperty;
+  UINTN                                            InfoSize;
+  UINTN                                            NameBufferSize;
+  UINTN                                            CommBufferPayloadSize;
+  UINTN                                            TempCommBufferSize;
+
+  //
+  // If input is invalid, stop processing this SMI
+  //
+  if (CommBuffer == NULL || CommBufferSize == NULL) {
+    return EFI_SUCCESS;
+  }
+
+  TempCommBufferSize = *CommBufferSize;
+
+  if (TempCommBufferSize < SMM_VARIABLE_COMMUNICATE_HEADER_SIZE) {
+    DEBUG ((EFI_D_ERROR, "SmmVariableHandler: SMM communication buffer size invalid!\n"));
+    return EFI_SUCCESS;
+  }
+  CommBufferPayloadSize = TempCommBufferSize - SMM_VARIABLE_COMMUNICATE_HEADER_SIZE;
+  if (CommBufferPayloadSize > mVariableBufferPayloadSize) {
+    DEBUG ((EFI_D_ERROR, "SmmVariableHandler: SMM communication buffer payload size invalid!\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (!SmmIsBufferOutsideSmmValid ((UINTN)CommBuffer, TempCommBufferSize)) {
+    DEBUG ((EFI_D_ERROR, "SmmVariableHandler: SMM communication buffer in SMRAM or overflow!\n"));
+    return EFI_SUCCESS;
+  }
+
+  SmmVariableFunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *)CommBuffer;
+  switch (SmmVariableFunctionHeader->Function) {
+    case SMM_VARIABLE_FUNCTION_GET_VARIABLE:
+      if (CommBufferPayloadSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) {
+        DEBUG ((EFI_D_ERROR, "GetVariable: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      //
+      // Copy the input communicate buffer payload to pre-allocated SMM variable buffer payload.
+      //
+      CopyMem (mVariableBufferPayload, SmmVariableFunctionHeader->Data, CommBufferPayloadSize);
+      SmmVariableHeader = (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *) mVariableBufferPayload;
+      if (((UINTN)(~0) - SmmVariableHeader->DataSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) ||
+         ((UINTN)(~0) - SmmVariableHeader->NameSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) + SmmVariableHeader->DataSize)) {
+        //
+        // Prevent InfoSize overflow happen
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+      InfoSize = OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)
+                 + SmmVariableHeader->DataSize + SmmVariableHeader->NameSize;
+
+      //
+      // SMRAM range check already covered before
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "GetVariable: Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      if (SmmVariableHeader->NameSize < sizeof (CHAR16) || SmmVariableHeader->Name[SmmVariableHeader->NameSize/sizeof (CHAR16) - 1] != L'\0') {
+        //
+        // Make sure VariableName is A Null-terminated string.
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      Status = VariableServiceGetVariable (
+                 SmmVariableHeader->Name,
+                 &SmmVariableHeader->Guid,
+                 &SmmVariableHeader->Attributes,
+                 &SmmVariableHeader->DataSize,
+                 (UINT8 *)SmmVariableHeader->Name + SmmVariableHeader->NameSize
+                 );
+      CopyMem (SmmVariableFunctionHeader->Data, mVariableBufferPayload, CommBufferPayloadSize);
+      break;
+
+    case SMM_VARIABLE_FUNCTION_GET_NEXT_VARIABLE_NAME:
+      if (CommBufferPayloadSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name)) {
+        DEBUG ((EFI_D_ERROR, "GetNextVariableName: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      //
+      // Copy the input communicate buffer payload to pre-allocated SMM variable buffer payload.
+      //
+      CopyMem (mVariableBufferPayload, SmmVariableFunctionHeader->Data, CommBufferPayloadSize);
+      GetNextVariableName = (SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME *) mVariableBufferPayload;
+      if ((UINTN)(~0) - GetNextVariableName->NameSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name)) {
+        //
+        // Prevent InfoSize overflow happen
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+      InfoSize = OFFSET_OF(SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name) + GetNextVariableName->NameSize;
+
+      //
+      // SMRAM range check already covered before
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "GetNextVariableName: Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      NameBufferSize = CommBufferPayloadSize - OFFSET_OF(SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name);
+      if (NameBufferSize < sizeof (CHAR16) || GetNextVariableName->Name[NameBufferSize/sizeof (CHAR16) - 1] != L'\0') {
+        //
+        // Make sure input VariableName is A Null-terminated string.
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      Status = VariableServiceGetNextVariableName (
+                 &GetNextVariableName->NameSize,
+                 GetNextVariableName->Name,
+                 &GetNextVariableName->Guid
+                 );
+      CopyMem (SmmVariableFunctionHeader->Data, mVariableBufferPayload, CommBufferPayloadSize);
+      break;
+
+    case SMM_VARIABLE_FUNCTION_SET_VARIABLE:
+      if (CommBufferPayloadSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) {
+        DEBUG ((EFI_D_ERROR, "SetVariable: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      //
+      // Copy the input communicate buffer payload to pre-allocated SMM variable buffer payload.
+      //
+      CopyMem (mVariableBufferPayload, SmmVariableFunctionHeader->Data, CommBufferPayloadSize);
+      SmmVariableHeader = (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *) mVariableBufferPayload;
+      if (((UINTN)(~0) - SmmVariableHeader->DataSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) ||
+         ((UINTN)(~0) - SmmVariableHeader->NameSize < OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) + SmmVariableHeader->DataSize)) {
+        //
+        // Prevent InfoSize overflow happen
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+      InfoSize = OFFSET_OF(SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)
+                 + SmmVariableHeader->DataSize + SmmVariableHeader->NameSize;
+
+      //
+      // SMRAM range check already covered before
+      // Data buffer should not contain SMM range
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "SetVariable: Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      if (SmmVariableHeader->NameSize < sizeof (CHAR16) || SmmVariableHeader->Name[SmmVariableHeader->NameSize/sizeof (CHAR16) - 1] != L'\0') {
+        //
+        // Make sure VariableName is A Null-terminated string.
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      Status = VariableServiceSetVariable (
+                 SmmVariableHeader->Name,
+                 &SmmVariableHeader->Guid,
+                 SmmVariableHeader->Attributes,
+                 SmmVariableHeader->DataSize,
+                 (UINT8 *)SmmVariableHeader->Name + SmmVariableHeader->NameSize
+                 );
+      break;
+
+    case SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO:
+      if (CommBufferPayloadSize < sizeof (SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO)) {
+        DEBUG ((EFI_D_ERROR, "QueryVariableInfo: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      QueryVariableInfo = (SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *) SmmVariableFunctionHeader->Data;
+
+      Status = VariableServiceQueryVariableInfo (
+                 QueryVariableInfo->Attributes,
+                 &QueryVariableInfo->MaximumVariableStorageSize,
+                 &QueryVariableInfo->RemainingVariableStorageSize,
+                 &QueryVariableInfo->MaximumVariableSize
+                 );
+      break;
+
+    case SMM_VARIABLE_FUNCTION_GET_PAYLOAD_SIZE:
+      if (CommBufferPayloadSize < sizeof (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE)) {
+        DEBUG ((EFI_D_ERROR, "GetPayloadSize: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      GetPayloadSize = (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE *) SmmVariableFunctionHeader->Data;
+      GetPayloadSize->VariablePayloadSize = mVariableBufferPayloadSize;
+      Status = EFI_SUCCESS;
+      break;
+
+    case SMM_VARIABLE_FUNCTION_READY_TO_BOOT:
+      if (AtRuntime()) {
+        Status = EFI_UNSUPPORTED;
+        break;
+      }
+      if (!mEndOfDxe) {
+        mEndOfDxe = TRUE;
+        VarCheckLibInitializeAtEndOfDxe (NULL);
+        //
+        // The initialization for variable quota.
+        //
+        InitializeVariableQuota ();
+      }
+      ReclaimForOS ();
+      Status = EFI_SUCCESS;
+      break;
+
+    case SMM_VARIABLE_FUNCTION_EXIT_BOOT_SERVICE:
+      mAtRuntime = TRUE;
+      Status = EFI_SUCCESS;
+      break;
+
+    case SMM_VARIABLE_FUNCTION_GET_STATISTICS:
+      VariableInfo = (VARIABLE_INFO_ENTRY *) SmmVariableFunctionHeader->Data;
+      InfoSize = TempCommBufferSize - SMM_VARIABLE_COMMUNICATE_HEADER_SIZE;
+
+      //
+      // Do not need to check SmmVariableFunctionHeader->Data in SMRAM here.
+      // It is covered by previous CommBuffer check
+      //
+
+      if (!SmmIsBufferOutsideSmmValid ((EFI_PHYSICAL_ADDRESS)(UINTN)CommBufferSize, sizeof(UINTN))) {
+        DEBUG ((EFI_D_ERROR, "GetStatistics: SMM communication buffer in SMRAM!\n"));
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      Status = SmmVariableGetStatistics (VariableInfo, &InfoSize);
+      *CommBufferSize = InfoSize + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE;
+      break;
+
+    case SMM_VARIABLE_FUNCTION_LOCK_VARIABLE:
+      if (mEndOfDxe) {
+        Status = EFI_ACCESS_DENIED;
+      } else {
+        VariableToLock = (SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE *) SmmVariableFunctionHeader->Data;
+        Status = VariableLockRequestToLock (
+                   NULL,
+                   VariableToLock->Name,
+                   &VariableToLock->Guid
+                   );
+      }
+      break;
+    case SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_SET:
+      if (mEndOfDxe) {
+        Status = EFI_ACCESS_DENIED;
+      } else {
+        CommVariableProperty = (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *) SmmVariableFunctionHeader->Data;
+        Status = VarCheckVariablePropertySet (
+                   CommVariableProperty->Name,
+                   &CommVariableProperty->Guid,
+                   &CommVariableProperty->VariableProperty
+                   );
+      }
+      break;
+    case SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_GET:
+      if (CommBufferPayloadSize < OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name)) {
+        DEBUG ((EFI_D_ERROR, "VarCheckVariablePropertyGet: SMM communication buffer size invalid!\n"));
+        return EFI_SUCCESS;
+      }
+      //
+      // Copy the input communicate buffer payload to pre-allocated SMM variable buffer payload.
+      //
+      CopyMem (mVariableBufferPayload, SmmVariableFunctionHeader->Data, CommBufferPayloadSize);
+      CommVariableProperty = (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *) mVariableBufferPayload;
+      if ((UINTN) (~0) - CommVariableProperty->NameSize < OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name)) {
+        //
+        // Prevent InfoSize overflow happen
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+      InfoSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name) + CommVariableProperty->NameSize;
+
+      //
+      // SMRAM range check already covered before
+      //
+      if (InfoSize > CommBufferPayloadSize) {
+        DEBUG ((EFI_D_ERROR, "VarCheckVariablePropertyGet: Data size exceed communication buffer size limit!\n"));
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      if (CommVariableProperty->NameSize < sizeof (CHAR16) || CommVariableProperty->Name[CommVariableProperty->NameSize/sizeof (CHAR16) - 1] != L'\0') {
+        //
+        // Make sure VariableName is A Null-terminated string.
+        //
+        Status = EFI_ACCESS_DENIED;
+        goto EXIT;
+      }
+
+      Status = VarCheckVariablePropertyGet (
+                 CommVariableProperty->Name,
+                 &CommVariableProperty->Guid,
+                 &CommVariableProperty->VariableProperty
+                 );
+      CopyMem (SmmVariableFunctionHeader->Data, mVariableBufferPayload, CommBufferPayloadSize);
+      break;
+
+    default:
+      Status = EFI_UNSUPPORTED;
+  }
+
+EXIT:
+
+  SmmVariableFunctionHeader->ReturnStatus = Status;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  SMM END_OF_DXE protocol notification event handler.
+
+  @param  Protocol   Points to the protocol's unique identifier
+  @param  Interface  Points to the interface instance
+  @param  Handle     The handle on which the interface was installed
+
+  @retval EFI_SUCCESS   SmmEndOfDxeCallback runs successfully
+
+**/
+EFI_STATUS
+EFIAPI
+SmmEndOfDxeCallback (
+  IN CONST EFI_GUID                       *Protocol,
+  IN VOID                                 *Interface,
+  IN EFI_HANDLE                           Handle
+  )
+{
+  DEBUG ((EFI_D_INFO, "[Variable]SMM_END_OF_DXE is signaled\n"));
+  mEndOfDxe = TRUE;
+  VarCheckLibInitializeAtEndOfDxe (NULL);
+  //
+  // The initialization for variable quota.
+  //
+  InitializeVariableQuota ();
+  if (PcdGetBool (PcdReclaimVariableSpaceAtEndOfDxe)) {
+    ReclaimForOS ();
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+  SMM Fault Tolerant Write protocol notification event handler.
+
+  Non-Volatile variable write may needs FTW protocol to reclaim when
+  writting variable.
+
+  @param  Protocol   Points to the protocol's unique identifier
+  @param  Interface  Points to the interface instance
+  @param  Handle     The handle on which the interface was installed
+
+  @retval EFI_SUCCESS   SmmEventCallback runs successfully
+  @retval EFI_NOT_FOUND The Fvb protocol for variable is not found.
+
+ **/
+EFI_STATUS
+EFIAPI
+SmmFtwNotificationEvent (
+  IN CONST EFI_GUID                       *Protocol,
+  IN VOID                                 *Interface,
+  IN EFI_HANDLE                           Handle
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_SMM_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvbProtocol;
+  EFI_SMM_FAULT_TOLERANT_WRITE_PROTOCOL   *FtwProtocol;
+  EFI_PHYSICAL_ADDRESS                    NvStorageVariableBase;
+  UINTN                                   FtwMaxBlockSize;
+
+  if (mVariableModuleGlobal->FvbInstance != NULL) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Ensure SMM FTW protocol is installed.
+  //
+  Status = GetFtwProtocol ((VOID **)&FtwProtocol);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = FtwProtocol->GetMaxBlockSize (FtwProtocol, &FtwMaxBlockSize);
+  if (!EFI_ERROR (Status)) {
+    ASSERT (PcdGet32 (PcdFlashNvStorageVariableSize) <= FtwMaxBlockSize);
+  }
+
+  //
+  // Find the proper FVB protocol for variable.
+  //
+  NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageVariableBase64);
+  if (NvStorageVariableBase == 0) {
+    NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageVariableBase);
+  }
+  Status = GetFvbInfoByAddress (NvStorageVariableBase, NULL, &FvbProtocol);
+  if (EFI_ERROR (Status)) {
+    return EFI_NOT_FOUND;
+  }
+
+  mVariableModuleGlobal->FvbInstance = FvbProtocol;
+
+  Status = VariableWriteServiceInitialize ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Variable write service initialization failed. Status = %r\n", Status));
+  }
+
+  //
+  // Notify the variable wrapper driver the variable write service is ready
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mSmmVariableHandle,
+                  &gSmmVariableWriteGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Variable Driver main entry point. The Variable driver places the 4 EFI
+  runtime services in the EFI System Table and installs arch protocols
+  for variable read and write services being available. It also registers
+  a notification function for an EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       Variable service successfully initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableServiceInitialize (
+  IN EFI_HANDLE                           ImageHandle,
+  IN EFI_SYSTEM_TABLE                     *SystemTable
+  )
+{
+  EFI_STATUS                              Status;
+  EFI_HANDLE                              VariableHandle;
+  VOID                                    *SmmFtwRegistration;
+  VOID                                    *SmmEndOfDxeRegistration;
+
+  //
+  // Variable initialize.
+  //
+  Status = VariableCommonInitialize ();
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install the Smm Variable Protocol on a new handle.
+  //
+  VariableHandle = NULL;
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &VariableHandle,
+                    &gEfiSmmVariableProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &gSmmVariable
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gSmst->SmmInstallProtocolInterface (
+                    &VariableHandle,
+                    &gEdkiiSmmVarCheckProtocolGuid,
+                    EFI_NATIVE_INTERFACE,
+                    &mSmmVarCheck
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  mVariableBufferPayloadSize = GetNonVolatileMaxVariableSize () +
+                               OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name) - GetVariableHeaderSize ();
+
+  Status = gSmst->SmmAllocatePool (
+                    EfiRuntimeServicesData,
+                    mVariableBufferPayloadSize,
+                    (VOID **)&mVariableBufferPayload
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  ///
+  /// Register SMM variable SMI handler
+  ///
+  VariableHandle = NULL;
+  Status = gSmst->SmiHandlerRegister (SmmVariableHandler, &gEfiSmmVariableProtocolGuid, &VariableHandle);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Notify the variable wrapper driver the variable service is ready
+  //
+  Status = SystemTable->BootServices->InstallProtocolInterface (
+                                        &mVariableHandle,
+                                        &gEfiSmmVariableProtocolGuid,
+                                        EFI_NATIVE_INTERFACE,
+                                        &gSmmVariable
+                                        );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register EFI_SMM_END_OF_DXE_PROTOCOL_GUID notify function.
+  //
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmEndOfDxeProtocolGuid,
+                    SmmEndOfDxeCallback,
+                    &SmmEndOfDxeRegistration
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register FtwNotificationEvent () notify function.
+  //
+  Status = gSmst->SmmRegisterProtocolNotify (
+                    &gEfiSmmFaultTolerantWriteProtocolGuid,
+                    SmmFtwNotificationEvent,
+                    &SmmFtwRegistration
+                    );
+  ASSERT_EFI_ERROR (Status);
+
+  SmmFtwNotificationEvent (NULL, NULL, NULL);
+
+  return EFI_SUCCESS;
+}
+
+
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.inf b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.inf
new file mode 100644
index 0000000000..02f01064f3
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.inf
@@ -0,0 +1,137 @@
+## @file
+#  Provides SMM variable service.
+#
+#  This module installs SMM variable protocol into SMM protocol database,
+#  which can be used by SMM driver, and installs SMM variable protocol
+#  into BS protocol database, which can be used to notify the SMM Runtime
+#  Dxe driver that the SMM variable service is ready.
+#  This module should be used with SMM Runtime DXE module together. The
+#  SMM Runtime DXE module would install variable arch protocol and variable
+#  write arch protocol based on SMM variable module.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - variable data and communicate buffer in SMM mode.
+#  This external input must be validated carefully to avoid security issues such as
+#  buffer overflow or integer overflow.
+#    The whole SMM authentication variable design relies on the integrity of flash part and SMM.
+#  which is assumed to be protected by platform.  All variable code and metadata in flash/SMM Memory
+#  may not be modified without authorization. If platform fails to protect these resources,
+#  the authentication service provided in this driver will be broken, and the behavior is undefined.
+#
+# Copyright (c) 2010 - 2016, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VariableSmm
+  MODULE_UNI_FILE                = VariableSmm.uni
+  FILE_GUID                      = 23A089B3-EED5-4ac5-B2AB-43E3298C2343
+  MODULE_TYPE                    = DXE_SMM_DRIVER
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x0001000A
+  ENTRY_POINT                    = VariableServiceInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+
+
+[Sources]
+  Reclaim.c
+  Variable.c
+  VariableSmm.c
+  VarCheck.c
+  Variable.h
+  VariableExLib.c
+  TcgMorLockSmm.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  UefiDriverEntryPoint
+  MemoryAllocationLib
+  BaseLib
+  SynchronizationLib
+  UefiLib
+  SmmServicesTableLib
+  BaseMemoryLib
+  DebugLib
+  DxeServicesTableLib
+  HobLib
+  PcdLib
+  SmmMemLib
+  AuthVariableLib
+  VarCheckLib
+
+[Protocols]
+  gEfiSmmFirmwareVolumeBlockProtocolGuid        ## CONSUMES
+  ## CONSUMES
+  ## NOTIFY
+  gEfiSmmFaultTolerantWriteProtocolGuid
+  ## PRODUCES
+  ## UNDEFINED # SmiHandlerRegister
+  gEfiSmmVariableProtocolGuid
+  gEfiSmmEndOfDxeProtocolGuid                   ## NOTIFY
+  gEdkiiSmmVarCheckProtocolGuid                 ## PRODUCES
+
+[Guids]
+  ## PRODUCES             ## GUID # Signature of Variable store header
+  ## CONSUMES             ## GUID # Signature of Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiAuthenticatedVariableGuid
+
+  ## PRODUCES             ## GUID # Signature of Variable store header
+  ## CONSUMES             ## GUID # Signature of Variable store header
+  ## SOMETIMES_CONSUMES   ## HOB
+  ## SOMETIMES_PRODUCES   ## SystemTable
+  gEfiVariableGuid
+
+  ## SOMETIMES_CONSUMES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"PlatformLang"
+  ## SOMETIMES_CONSUMES   ## Variable:L"Lang"
+  ## SOMETIMES_PRODUCES   ## Variable:L"Lang"
+  gEfiGlobalVariableGuid
+
+  gEfiMemoryOverwriteControlDataGuid            ## CONSUMES   ## Variable:L"MemoryOverwriteRequestControl"
+  gEfiMemoryOverwriteRequestControlLockGuid     ## PRODUCES   ## Variable:L"MemoryOverwriteRequestControlLock"
+
+  gSmmVariableWriteGuid                         ## PRODUCES             ## GUID # Install protocol
+  gEfiSystemNvDataFvGuid                        ## CONSUMES             ## GUID
+  gEdkiiFaultTolerantWriteGuid                  ## SOMETIMES_CONSUMES   ## HOB
+  gEdkiiVarErrorFlagGuid                        ## CONSUMES             ## GUID
+  gZeroGuid                                     ## SOMETIMES_CONSUMES   ## GUID
+
+[Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableSize       ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase       ## SOMETIMES_CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase64     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxVariableSize                  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxAuthVariableSize              ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxHardwareErrorVariableSize     ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableStoreSize                ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdHwErrStorageSize                 ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdMaxUserNvVariableSpaceSize           ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdBoottimeReservedNvVariableSpaceSize  ## CONSUMES
+  gEfiMdeModulePkgTokenSpaceGuid.PcdReclaimVariableSpaceAtEndOfDxe   ## CONSUMES
+
+[FeaturePcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdVariableCollectStatistics        ## CONSUMES  # statistic the information of variable.
+  gEfiMdePkgTokenSpaceGuid.PcdUefiVariableDefaultLangDeprecate       ## CONSUMES  # Auto update PlatformLang/Lang
+
+[Depex]
+  TRUE
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  VariableSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.uni
new file mode 100644
index 0000000000..60502d0451
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmm.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmExtra.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmExtra.uni
new file mode 100644
index 0000000000..46f984c809
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.c b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.c
new file mode 100644
index 0000000000..0a076ae467
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.c
@@ -0,0 +1,1190 @@
+/** @file
+  Implement all four UEFI Runtime Variable services for the nonvolatile
+  and volatile storage space and install variable architecture protocol
+  based on SMM variable module.
+
+  Caution: This module requires additional review when modified.
+  This driver will have external input - variable data.
+  This external input must be validated carefully to avoid security issue like
+  buffer overflow, integer overflow.
+
+  RuntimeServiceGetVariable() and RuntimeServiceSetVariable() are external API
+  to receive data buffer. The size should be checked carefully.
+
+  InitCommunicateBuffer() is really function to check the variable data size.
+
+Copyright (c) 2010 - 2015, 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 <PiDxe.h>
+#include <Protocol/VariableWrite.h>
+#include <Protocol/Variable.h>
+#include <Protocol/SmmCommunication.h>
+#include <Protocol/SmmVariable.h>
+#include <Protocol/VariableLock.h>
+#include <Protocol/VarCheck.h>
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiRuntimeLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+
+#include <Guid/EventGroup.h>
+#include <Guid/SmmVariableCommon.h>
+
+EFI_HANDLE                       mHandle                    = NULL;
+EFI_SMM_VARIABLE_PROTOCOL       *mSmmVariable               = NULL;
+EFI_EVENT                        mVirtualAddressChangeEvent = NULL;
+EFI_SMM_COMMUNICATION_PROTOCOL  *mSmmCommunication          = NULL;
+UINT8                           *mVariableBuffer            = NULL;
+UINT8                           *mVariableBufferPhysical    = NULL;
+UINTN                            mVariableBufferSize;
+UINTN                            mVariableBufferPayloadSize;
+EFI_LOCK                         mVariableServicesLock;
+EDKII_VARIABLE_LOCK_PROTOCOL     mVariableLock;
+EDKII_VAR_CHECK_PROTOCOL         mVarCheck;
+
+/**
+  SecureBoot Hook for SetVariable.
+
+  @param[in] VariableName                 Name of Variable to be found.
+  @param[in] VendorGuid                   Variable vendor GUID.
+
+**/
+VOID
+EFIAPI
+SecureBootHook (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid
+  );
+
+/**
+  Acquires lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function that will be removed when
+  EfiAcquireLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiAcquireLock() at boot time, and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to acquire.
+
+**/
+VOID
+AcquireLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+  if (!EfiAtRuntime ()) {
+    EfiAcquireLock (Lock);
+  }
+}
+
+/**
+  Releases lock only at boot time. Simply returns at runtime.
+
+  This is a temperary function which will be removed when
+  EfiReleaseLock() in UefiLib can handle the call in UEFI
+  Runtimer driver in RT phase.
+  It calls EfiReleaseLock() at boot time and simply returns
+  at runtime.
+
+  @param  Lock         A pointer to the lock to release.
+
+**/
+VOID
+ReleaseLockOnlyAtBootTime (
+  IN EFI_LOCK                             *Lock
+  )
+{
+  if (!EfiAtRuntime ()) {
+    EfiReleaseLock (Lock);
+  }
+}
+
+/**
+  Initialize the communicate buffer using DataSize and Function.
+
+  The communicate size is: SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE +
+  DataSize.
+
+  Caution: This function may receive untrusted input.
+  The data size external input, so this function will validate it carefully to avoid buffer overflow.
+
+  @param[out]      DataPtr          Points to the data in the communicate buffer.
+  @param[in]       DataSize         The data size to send to SMM.
+  @param[in]       Function         The function number to initialize the communicate header.
+
+  @retval EFI_INVALID_PARAMETER     The data size is too big.
+  @retval EFI_SUCCESS               Find the specified variable.
+
+**/
+EFI_STATUS
+InitCommunicateBuffer (
+  OUT     VOID                              **DataPtr OPTIONAL,
+  IN      UINTN                             DataSize,
+  IN      UINTN                             Function
+  )
+{
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;
+  SMM_VARIABLE_COMMUNICATE_HEADER           *SmmVariableFunctionHeader;
+
+
+  if (DataSize + SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE > mVariableBufferSize) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  SmmCommunicateHeader = (EFI_SMM_COMMUNICATE_HEADER *) mVariableBuffer;
+  CopyGuid (&SmmCommunicateHeader->HeaderGuid, &gEfiSmmVariableProtocolGuid);
+  SmmCommunicateHeader->MessageLength = DataSize + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE;
+
+  SmmVariableFunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *) SmmCommunicateHeader->Data;
+  SmmVariableFunctionHeader->Function = Function;
+  if (DataPtr != NULL) {
+    *DataPtr = SmmVariableFunctionHeader->Data;
+  }
+
+  return EFI_SUCCESS;
+}
+
+
+/**
+  Send the data in communicate buffer to SMM.
+
+  @param[in]   DataSize               This size of the function header and the data.
+
+  @retval      EFI_SUCCESS            Success is returned from the functin in SMM.
+  @retval      Others                 Failure is returned from the function in SMM.
+
+**/
+EFI_STATUS
+SendCommunicateBuffer (
+  IN      UINTN                             DataSize
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     CommSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;
+  SMM_VARIABLE_COMMUNICATE_HEADER           *SmmVariableFunctionHeader;
+
+  CommSize = DataSize + SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE;
+  Status = mSmmCommunication->Communicate (mSmmCommunication, mVariableBufferPhysical, &CommSize);
+  ASSERT_EFI_ERROR (Status);
+
+  SmmCommunicateHeader      = (EFI_SMM_COMMUNICATE_HEADER *) mVariableBuffer;
+  SmmVariableFunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *)SmmCommunicateHeader->Data;
+  return  SmmVariableFunctionHeader->ReturnStatus;
+}
+
+/**
+  Mark a variable that will become read-only after leaving the DXE phase of execution.
+
+  @param[in] This          The VARIABLE_LOCK_PROTOCOL instance.
+  @param[in] VariableName  A pointer to the variable name that will be made read-only subsequently.
+  @param[in] VendorGuid    A pointer to the vendor GUID that will be made read-only subsequently.
+
+  @retval EFI_SUCCESS           The variable specified by the VariableName and the VendorGuid was marked
+                                as pending to be read-only.
+  @retval EFI_INVALID_PARAMETER VariableName or VendorGuid is NULL.
+                                Or VariableName is an empty string.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource to hold the lock request.
+**/
+EFI_STATUS
+EFIAPI
+VariableLockRequestToLock (
+  IN CONST EDKII_VARIABLE_LOCK_PROTOCOL *This,
+  IN       CHAR16                       *VariableName,
+  IN       EFI_GUID                     *VendorGuid
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     VariableNameSize;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE    *VariableToLock;
+
+  if (VariableName == NULL || VariableName[0] == 0 || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableNameSize = StrSize (VariableName);
+  VariableToLock   = NULL;
+
+  //
+  // If VariableName exceeds SMM payload limit. Return failure
+  //
+  if (VariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE, Name)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE, Name) + VariableNameSize;
+  Status = InitCommunicateBuffer ((VOID **) &VariableToLock, PayloadSize, SMM_VARIABLE_FUNCTION_LOCK_VARIABLE);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (VariableToLock != NULL);
+
+  CopyGuid (&VariableToLock->Guid, VendorGuid);
+  VariableToLock->NameSize = VariableNameSize;
+  CopyMem (VariableToLock->Name, VariableName, VariableToLock->NameSize);
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+/**
+  Register SetVariable check handler.
+
+  @param[in] Handler            Pointer to check handler.
+
+  @retval EFI_SUCCESS           The SetVariable check handler was registered successfully.
+  @retval EFI_INVALID_PARAMETER Handler is NULL.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the SetVariable check handler register request.
+  @retval EFI_UNSUPPORTED       This interface is not implemented.
+                                For example, it is unsupported in VarCheck protocol if both VarCheck and SmmVarCheck protocols are present.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckRegisterSetVariableCheckHandler (
+  IN VAR_CHECK_SET_VARIABLE_CHECK_HANDLER   Handler
+  )
+{
+  return EFI_UNSUPPORTED;
+}
+
+/**
+  Variable property set.
+
+  @param[in] Name               Pointer to the variable name.
+  @param[in] Guid               Pointer to the vendor GUID.
+  @param[in] VariableProperty   Pointer to the input variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was set successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string,
+                                or the fields of VariableProperty are not valid.
+  @retval EFI_ACCESS_DENIED     EFI_END_OF_DXE_EVENT_GROUP_GUID or EFI_EVENT_GROUP_READY_TO_BOOT has
+                                already been signaled.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough resource for the variable property set request.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertySet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  IN VAR_CHECK_VARIABLE_PROPERTY    *VariableProperty
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     VariableNameSize;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *CommVariableProperty;
+
+  if (Name == NULL || Name[0] == 0 || Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty->Revision != VAR_CHECK_VARIABLE_PROPERTY_REVISION) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableNameSize = StrSize (Name);
+  CommVariableProperty = NULL;
+
+  //
+  // If VariableName exceeds SMM payload limit. Return failure
+  //
+  if (VariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime (&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name) + VariableNameSize;
+  Status = InitCommunicateBuffer ((VOID **) &CommVariableProperty, PayloadSize, SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_SET);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (CommVariableProperty != NULL);
+
+  CopyGuid (&CommVariableProperty->Guid, Guid);
+  CopyMem (&CommVariableProperty->VariableProperty, VariableProperty, sizeof (*VariableProperty));
+  CommVariableProperty->NameSize = VariableNameSize;
+  CopyMem (CommVariableProperty->Name, Name, CommVariableProperty->NameSize);
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+/**
+  Variable property get.
+
+  @param[in]  Name              Pointer to the variable name.
+  @param[in]  Guid              Pointer to the vendor GUID.
+  @param[out] VariableProperty  Pointer to the output variable property.
+
+  @retval EFI_SUCCESS           The property of variable specified by the Name and Guid was got successfully.
+  @retval EFI_INVALID_PARAMETER Name, Guid or VariableProperty is NULL, or Name is an empty string.
+  @retval EFI_NOT_FOUND         The property of variable specified by the Name and Guid was not found.
+
+**/
+EFI_STATUS
+EFIAPI
+VarCheckVariablePropertyGet (
+  IN CHAR16                         *Name,
+  IN EFI_GUID                       *Guid,
+  OUT VAR_CHECK_VARIABLE_PROPERTY   *VariableProperty
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     VariableNameSize;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *CommVariableProperty;
+
+  if (Name == NULL || Name[0] == 0 || Guid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (VariableProperty == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableNameSize = StrSize (Name);
+  CommVariableProperty = NULL;
+
+  //
+  // If VariableName exceeds SMM payload limit. Return failure
+  //
+  if (VariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime (&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY, Name) + VariableNameSize;
+  Status = InitCommunicateBuffer ((VOID **) &CommVariableProperty, PayloadSize, SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_GET);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (CommVariableProperty != NULL);
+
+  CopyGuid (&CommVariableProperty->Guid, Guid);
+  CommVariableProperty->NameSize = VariableNameSize;
+  CopyMem (CommVariableProperty->Name, Name, CommVariableProperty->NameSize);
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+  if (Status == EFI_SUCCESS) {
+    CopyMem (VariableProperty, &CommVariableProperty->VariableProperty, sizeof (*VariableProperty));
+  }
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+/**
+  This code finds variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  The data size is external input, so this function will validate it carefully to avoid buffer overflow.
+
+  @param[in]      VariableName       Name of Variable to be found.
+  @param[in]      VendorGuid         Variable vendor GUID.
+  @param[out]     Attributes         Attribute value of the variable found.
+  @param[in, out] DataSize           Size of Data found. If size is less than the
+                                     data, this value contains the required size.
+  @param[out]     Data               Data pointer.
+
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+  @retval EFI_SUCCESS                Find the specified variable.
+  @retval EFI_NOT_FOUND              Not found.
+  @retval EFI_BUFFER_TO_SMALL        DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceGetVariable (
+  IN      CHAR16                            *VariableName,
+  IN      EFI_GUID                          *VendorGuid,
+  OUT     UINT32                            *Attributes OPTIONAL,
+  IN OUT  UINTN                             *DataSize,
+  OUT     VOID                              *Data
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE  *SmmVariableHeader;
+  UINTN                                     TempDataSize;
+  UINTN                                     VariableNameSize;
+
+  if (VariableName == NULL || VendorGuid == NULL || DataSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  TempDataSize          = *DataSize;
+  VariableNameSize      = StrSize (VariableName);
+  SmmVariableHeader     = NULL;
+
+  //
+  // If VariableName exceeds SMM payload limit. Return failure
+  //
+  if (VariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  if (TempDataSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) - VariableNameSize) {
+    //
+    // If output data buffer exceed SMM payload limit. Trim output buffer to SMM payload size
+    //
+    TempDataSize = mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) - VariableNameSize;
+  }
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) + VariableNameSize + TempDataSize;
+
+  Status = InitCommunicateBuffer ((VOID **)&SmmVariableHeader, PayloadSize, SMM_VARIABLE_FUNCTION_GET_VARIABLE);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (SmmVariableHeader != NULL);
+
+  CopyGuid (&SmmVariableHeader->Guid, VendorGuid);
+  SmmVariableHeader->DataSize   = TempDataSize;
+  SmmVariableHeader->NameSize   = VariableNameSize;
+  if (Attributes == NULL) {
+    SmmVariableHeader->Attributes = 0;
+  } else {
+    SmmVariableHeader->Attributes = *Attributes;
+  }
+  CopyMem (SmmVariableHeader->Name, VariableName, SmmVariableHeader->NameSize);
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+
+  //
+  // Get data from SMM.
+  //
+  if (Status == EFI_SUCCESS || Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // SMM CommBuffer DataSize can be a trimed value
+    // Only update DataSize when needed
+    //
+    *DataSize = SmmVariableHeader->DataSize;
+  }
+  if (Attributes != NULL) {
+    *Attributes = SmmVariableHeader->Attributes;
+  }
+
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  if (Data != NULL) {
+    CopyMem (Data, (UINT8 *)SmmVariableHeader->Name + SmmVariableHeader->NameSize, SmmVariableHeader->DataSize);
+  } else {
+    Status = EFI_INVALID_PARAMETER;
+  }
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+
+/**
+  This code Finds the Next available variable.
+
+  @param[in, out] VariableNameSize   Size of the variable name.
+  @param[in, out] VariableName       Pointer to variable name.
+  @param[in, out] VendorGuid         Variable Vendor Guid.
+
+  @retval EFI_INVALID_PARAMETER      Invalid parameter.
+  @retval EFI_SUCCESS                Find the specified variable.
+  @retval EFI_NOT_FOUND              Not found.
+  @retval EFI_BUFFER_TO_SMALL        DataSize is too small for the result.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceGetNextVariableName (
+  IN OUT  UINTN                             *VariableNameSize,
+  IN OUT  CHAR16                            *VariableName,
+  IN OUT  EFI_GUID                          *VendorGuid
+  )
+{
+  EFI_STATUS                                      Status;
+  UINTN                                           PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME *SmmGetNextVariableName;
+  UINTN                                           OutVariableNameSize;
+  UINTN                                           InVariableNameSize;
+
+  if (VariableNameSize == NULL || VariableName == NULL || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  OutVariableNameSize   = *VariableNameSize;
+  InVariableNameSize    = StrSize (VariableName);
+  SmmGetNextVariableName = NULL;
+
+  //
+  // If input string exceeds SMM payload limit. Return failure
+  //
+  if (InVariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name)) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  if (OutVariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name)) {
+    //
+    // If output buffer exceed SMM payload limit. Trim output buffer to SMM payload size
+    //
+    OutVariableNameSize = mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name);
+  }
+  //
+  // Payload should be Guid + NameSize + MAX of Input & Output buffer
+  //
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME, Name) + MAX (OutVariableNameSize, InVariableNameSize);
+
+  Status = InitCommunicateBuffer ((VOID **)&SmmGetNextVariableName, PayloadSize, SMM_VARIABLE_FUNCTION_GET_NEXT_VARIABLE_NAME);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (SmmGetNextVariableName != NULL);
+
+  //
+  // SMM comm buffer->NameSize is buffer size for return string
+  //
+  SmmGetNextVariableName->NameSize = OutVariableNameSize;
+
+  CopyGuid (&SmmGetNextVariableName->Guid, VendorGuid);
+  //
+  // Copy whole string
+  //
+  CopyMem (SmmGetNextVariableName->Name, VariableName, InVariableNameSize);
+  if (OutVariableNameSize > InVariableNameSize) {
+    ZeroMem ((UINT8 *) SmmGetNextVariableName->Name + InVariableNameSize, OutVariableNameSize - InVariableNameSize);
+  }
+
+  //
+  // Send data to SMM
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+
+  //
+  // Get data from SMM.
+  //
+  if (Status == EFI_SUCCESS || Status == EFI_BUFFER_TOO_SMALL) {
+    //
+    // SMM CommBuffer NameSize can be a trimed value
+    // Only update VariableNameSize when needed
+    //
+    *VariableNameSize = SmmGetNextVariableName->NameSize;
+  }
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  CopyGuid (VendorGuid, &SmmGetNextVariableName->Guid);
+  CopyMem (VariableName, SmmGetNextVariableName->Name, SmmGetNextVariableName->NameSize);
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+/**
+  This code sets variable in storage blocks (Volatile or Non-Volatile).
+
+  Caution: This function may receive untrusted input.
+  The data size and data are external input, so this function will validate it carefully to avoid buffer overflow.
+
+  @param[in] VariableName                 Name of Variable to be found.
+  @param[in] VendorGuid                   Variable vendor GUID.
+  @param[in] Attributes                   Attribute value of the variable found
+  @param[in] DataSize                     Size of Data found. If size is less than the
+                                          data, this value contains the required size.
+  @param[in] Data                         Data pointer.
+
+  @retval EFI_INVALID_PARAMETER           Invalid parameter.
+  @retval EFI_SUCCESS                     Set successfully.
+  @retval EFI_OUT_OF_RESOURCES            Resource not enough to set variable.
+  @retval EFI_NOT_FOUND                   Not found.
+  @retval EFI_WRITE_PROTECTED             Variable is read-only.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceSetVariable (
+  IN CHAR16                                 *VariableName,
+  IN EFI_GUID                               *VendorGuid,
+  IN UINT32                                 Attributes,
+  IN UINTN                                  DataSize,
+  IN VOID                                   *Data
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE  *SmmVariableHeader;
+  UINTN                                     VariableNameSize;
+
+  //
+  // Check input parameters.
+  //
+  if (VariableName == NULL || VariableName[0] == 0 || VendorGuid == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (DataSize != 0 && Data == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  VariableNameSize      = StrSize (VariableName);
+  SmmVariableHeader     = NULL;
+
+  //
+  // If VariableName or DataSize exceeds SMM payload limit. Return failure
+  //
+  if ((VariableNameSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name)) ||
+      (DataSize > mVariableBufferPayloadSize - OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) - VariableNameSize)){
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize.
+  //
+  PayloadSize = OFFSET_OF (SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE, Name) + VariableNameSize + DataSize;
+  Status = InitCommunicateBuffer ((VOID **)&SmmVariableHeader, PayloadSize, SMM_VARIABLE_FUNCTION_SET_VARIABLE);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (SmmVariableHeader != NULL);
+
+  CopyGuid ((EFI_GUID *) &SmmVariableHeader->Guid, VendorGuid);
+  SmmVariableHeader->DataSize   = DataSize;
+  SmmVariableHeader->NameSize   = VariableNameSize;
+  SmmVariableHeader->Attributes = Attributes;
+  CopyMem (SmmVariableHeader->Name, VariableName, SmmVariableHeader->NameSize);
+  CopyMem ((UINT8 *) SmmVariableHeader->Name + SmmVariableHeader->NameSize, Data, DataSize);
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+
+  if (!EfiAtRuntime ()) {
+    if (!EFI_ERROR (Status)) {
+      SecureBootHook (
+        VariableName,
+        VendorGuid
+        );
+    }
+  }
+  return Status;
+}
+
+
+/**
+  This code returns information about the EFI variables.
+
+  @param[in]  Attributes                   Attributes bitmask to specify the type of variables
+                                           on which to return information.
+  @param[out] MaximumVariableStorageSize   Pointer to the maximum size of the storage space available
+                                           for the EFI variables associated with the attributes specified.
+  @param[out] RemainingVariableStorageSize Pointer to the remaining size of the storage space available
+                                           for EFI variables associated with the attributes specified.
+  @param[out] MaximumVariableSize          Pointer to the maximum size of an individual EFI variables
+                                           associated with the attributes specified.
+
+  @retval EFI_INVALID_PARAMETER            An invalid combination of attribute bits was supplied.
+  @retval EFI_SUCCESS                      Query successfully.
+  @retval EFI_UNSUPPORTED                  The attribute is not supported on this platform.
+
+**/
+EFI_STATUS
+EFIAPI
+RuntimeServiceQueryVariableInfo (
+  IN  UINT32                                Attributes,
+  OUT UINT64                                *MaximumVariableStorageSize,
+  OUT UINT64                                *RemainingVariableStorageSize,
+  OUT UINT64                                *MaximumVariableSize
+  )
+{
+  EFI_STATUS                                Status;
+  UINTN                                     PayloadSize;
+  SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *SmmQueryVariableInfo;
+
+  SmmQueryVariableInfo = NULL;
+
+  if(MaximumVariableStorageSize == NULL || RemainingVariableStorageSize == NULL || MaximumVariableSize == NULL || Attributes == 0) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + PayloadSize;
+  //
+  PayloadSize = sizeof (SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO);
+  Status = InitCommunicateBuffer ((VOID **)&SmmQueryVariableInfo, PayloadSize, SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+  ASSERT (SmmQueryVariableInfo != NULL);
+
+  SmmQueryVariableInfo->Attributes  = Attributes;
+
+  //
+  // Send data to SMM.
+  //
+  Status = SendCommunicateBuffer (PayloadSize);
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Get data from SMM.
+  //
+  *MaximumVariableSize          = SmmQueryVariableInfo->MaximumVariableSize;
+  *MaximumVariableStorageSize   = SmmQueryVariableInfo->MaximumVariableStorageSize;
+  *RemainingVariableStorageSize = SmmQueryVariableInfo->RemainingVariableStorageSize;
+
+Done:
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+
+/**
+  Exit Boot Services Event notification handler.
+
+  Notify SMM variable driver about the event.
+
+  @param[in]  Event     Event whose notification function is being invoked.
+  @param[in]  Context   Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+OnExitBootServices (
+  IN      EFI_EVENT                         Event,
+  IN      VOID                              *Context
+  )
+{
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE.
+  //
+  InitCommunicateBuffer (NULL, 0, SMM_VARIABLE_FUNCTION_EXIT_BOOT_SERVICE);
+
+  //
+  // Send data to SMM.
+  //
+  SendCommunicateBuffer (0);
+}
+
+
+/**
+  On Ready To Boot Services Event notification handler.
+
+  Notify SMM variable driver about the event.
+
+  @param[in]  Event     Event whose notification function is being invoked
+  @param[in]  Context   Pointer to the notification function's context
+
+**/
+VOID
+EFIAPI
+OnReadyToBoot (
+  IN      EFI_EVENT                         Event,
+  IN      VOID                              *Context
+  )
+{
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE.
+  //
+  InitCommunicateBuffer (NULL, 0, SMM_VARIABLE_FUNCTION_READY_TO_BOOT);
+
+  //
+  // Send data to SMM.
+  //
+  SendCommunicateBuffer (0);
+
+  gBS->CloseEvent (Event);
+}
+
+
+/**
+  Notification function of EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE.
+
+  This is a notification function registered on EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+  It convers pointer to new virtual address.
+
+  @param[in]  Event        Event whose notification function is being invoked.
+  @param[in]  Context      Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+VariableAddressChangeEvent (
+  IN EFI_EVENT                              Event,
+  IN VOID                                   *Context
+  )
+{
+  EfiConvertPointer (0x0, (VOID **) &mVariableBuffer);
+  EfiConvertPointer (0x0, (VOID **) &mSmmCommunication);
+}
+
+/**
+  This code gets variable payload size.
+
+  @param[out] VariablePayloadSize   Output pointer to variable payload size.
+
+  @retval EFI_SUCCESS               Get successfully.
+  @retval Others                    Get unsuccessfully.
+
+**/
+EFI_STATUS
+EFIAPI
+GetVariablePayloadSize (
+  OUT UINTN                         *VariablePayloadSize
+  )
+{
+  EFI_STATUS                                Status;
+  SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE *SmmGetPayloadSize;
+  EFI_SMM_COMMUNICATE_HEADER                *SmmCommunicateHeader;
+  SMM_VARIABLE_COMMUNICATE_HEADER           *SmmVariableFunctionHeader;
+  UINTN                                     CommSize;
+  UINT8                                     *CommBuffer;
+
+  SmmGetPayloadSize = NULL;
+  CommBuffer = NULL;
+
+  if(VariablePayloadSize == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  AcquireLockOnlyAtBootTime(&mVariableServicesLock);
+
+  //
+  // Init the communicate buffer. The buffer data size is:
+  // SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + sizeof (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE);
+  //
+  CommSize = SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + sizeof (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE);
+  CommBuffer = AllocateZeroPool (CommSize);
+  if (CommBuffer == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  SmmCommunicateHeader = (EFI_SMM_COMMUNICATE_HEADER *) CommBuffer;
+  CopyGuid (&SmmCommunicateHeader->HeaderGuid, &gEfiSmmVariableProtocolGuid);
+  SmmCommunicateHeader->MessageLength = SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + sizeof (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE);
+
+  SmmVariableFunctionHeader = (SMM_VARIABLE_COMMUNICATE_HEADER *) SmmCommunicateHeader->Data;
+  SmmVariableFunctionHeader->Function = SMM_VARIABLE_FUNCTION_GET_PAYLOAD_SIZE;
+  SmmGetPayloadSize = (SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE *) SmmVariableFunctionHeader->Data;
+
+  //
+  // Send data to SMM.
+  //
+  Status = mSmmCommunication->Communicate (mSmmCommunication, CommBuffer, &CommSize);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = SmmVariableFunctionHeader->ReturnStatus;
+  if (EFI_ERROR (Status)) {
+    goto Done;
+  }
+
+  //
+  // Get data from SMM.
+  //
+  *VariablePayloadSize = SmmGetPayloadSize->VariablePayloadSize;
+
+Done:
+  if (CommBuffer != NULL) {
+    FreePool (CommBuffer);
+  }
+  ReleaseLockOnlyAtBootTime (&mVariableServicesLock);
+  return Status;
+}
+
+/**
+  Initialize variable service and install Variable Architectural protocol.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+SmmVariableReady (
+  IN  EFI_EVENT                             Event,
+  IN  VOID                                  *Context
+  )
+{
+  EFI_STATUS                                Status;
+
+  Status = gBS->LocateProtocol (&gEfiSmmVariableProtocolGuid, NULL, (VOID **)&mSmmVariable);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  Status = gBS->LocateProtocol (&gEfiSmmCommunicationProtocolGuid, NULL, (VOID **) &mSmmCommunication);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Allocate memory for variable communicate buffer.
+  //
+  Status = GetVariablePayloadSize (&mVariableBufferPayloadSize);
+  ASSERT_EFI_ERROR (Status);
+  mVariableBufferSize  = SMM_COMMUNICATE_HEADER_SIZE + SMM_VARIABLE_COMMUNICATE_HEADER_SIZE + mVariableBufferPayloadSize;
+  mVariableBuffer      = AllocateRuntimePool (mVariableBufferSize);
+  ASSERT (mVariableBuffer != NULL);
+
+  //
+  // Save the buffer physical address used for SMM conmunication.
+  //
+  mVariableBufferPhysical = mVariableBuffer;
+
+  gRT->GetVariable         = RuntimeServiceGetVariable;
+  gRT->GetNextVariableName = RuntimeServiceGetNextVariableName;
+  gRT->SetVariable         = RuntimeServiceSetVariable;
+  gRT->QueryVariableInfo   = RuntimeServiceQueryVariableInfo;
+
+  //
+  // Install the Variable Architectural Protocol on a new handle.
+  //
+  Status = gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiVariableArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mVariableLock.RequestToLock = VariableLockRequestToLock;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEdkiiVariableLockProtocolGuid,
+                  &mVariableLock,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  mVarCheck.RegisterSetVariableCheckHandler = VarCheckRegisterSetVariableCheckHandler;
+  mVarCheck.VariablePropertySet = VarCheckVariablePropertySet;
+  mVarCheck.VariablePropertyGet = VarCheckVariablePropertyGet;
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mHandle,
+                  &gEdkiiVarCheckProtocolGuid,
+                  &mVarCheck,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  gBS->CloseEvent (Event);
+}
+
+
+/**
+  SMM Non-Volatile variable write service is ready notify event handler.
+
+  @param[in] Event    Event whose notification function is being invoked.
+  @param[in] Context  Pointer to the notification function's context.
+
+**/
+VOID
+EFIAPI
+SmmVariableWriteReady (
+  IN  EFI_EVENT                             Event,
+  IN  VOID                                  *Context
+  )
+{
+  EFI_STATUS                                Status;
+  VOID                                      *ProtocolOps;
+
+  //
+  // Check whether the protocol is installed or not.
+  //
+  Status = gBS->LocateProtocol (&gSmmVariableWriteGuid, NULL, (VOID **) &ProtocolOps);
+  if (EFI_ERROR (Status)) {
+    return;
+  }
+
+  Status = gBS->InstallProtocolInterface (
+                  &mHandle,
+                  &gEfiVariableWriteArchProtocolGuid,
+                  EFI_NATIVE_INTERFACE,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  gBS->CloseEvent (Event);
+}
+
+
+/**
+  Variable Driver main entry point. The Variable driver places the 4 EFI
+  runtime services in the EFI System Table and installs arch protocols
+  for variable read and write services being available. It also registers
+  a notification function for an EVT_SIGNAL_VIRTUAL_ADDRESS_CHANGE event.
+
+  @param[in] ImageHandle    The firmware allocated handle for the EFI image.
+  @param[in] SystemTable    A pointer to the EFI System Table.
+
+  @retval EFI_SUCCESS       Variable service successfully initialized.
+
+**/
+EFI_STATUS
+EFIAPI
+VariableSmmRuntimeInitialize (
+  IN EFI_HANDLE                             ImageHandle,
+  IN EFI_SYSTEM_TABLE                       *SystemTable
+  )
+{
+  VOID                                      *SmmVariableRegistration;
+  VOID                                      *SmmVariableWriteRegistration;
+  EFI_EVENT                                 OnReadyToBootEvent;
+  EFI_EVENT                                 ExitBootServiceEvent;
+  EFI_EVENT                                 LegacyBootEvent;
+
+  EfiInitializeLock (&mVariableServicesLock, TPL_NOTIFY);
+
+  //
+  // Smm variable service is ready
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gEfiSmmVariableProtocolGuid,
+    TPL_CALLBACK,
+    SmmVariableReady,
+    NULL,
+    &SmmVariableRegistration
+    );
+
+  //
+  // Smm Non-Volatile variable write service is ready
+  //
+  EfiCreateProtocolNotifyEvent (
+    &gSmmVariableWriteGuid,
+    TPL_CALLBACK,
+    SmmVariableWriteReady,
+    NULL,
+    &SmmVariableWriteRegistration
+    );
+
+  //
+  // Register the event to reclaim variable for OS usage.
+  //
+  EfiCreateEventReadyToBootEx (
+    TPL_NOTIFY,
+    OnReadyToBoot,
+    NULL,
+    &OnReadyToBootEvent
+    );
+
+  //
+  // Register the event to inform SMM variable that it is at runtime.
+  //
+  gBS->CreateEventEx (
+         EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         OnExitBootServices,
+         NULL,
+         &gEfiEventExitBootServicesGuid,
+         &ExitBootServiceEvent
+         );
+
+  //
+  // Register the event to inform SMM variable that it is at runtime for legacy boot.
+  // Reuse OnExitBootServices() here.
+  //
+  EfiCreateEventLegacyBootEx(
+    TPL_NOTIFY,
+    OnExitBootServices,
+    NULL,
+    &LegacyBootEvent
+    );
+
+  //
+  // Register the event to convert the pointer for runtime.
+  //
+  gBS->CreateEventEx (
+         EVT_NOTIFY_SIGNAL,
+         TPL_NOTIFY,
+         VariableAddressChangeEvent,
+         NULL,
+         &gEfiEventVirtualAddressChangeGuid,
+         &mVirtualAddressChangeEvent
+         );
+
+  return EFI_SUCCESS;
+}
+
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.inf b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.inf
new file mode 100644
index 0000000000..82ddb00a19
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.inf
@@ -0,0 +1,92 @@
+## @file
+#  Runtime DXE part corresponding to SMM authenticated variable module.
+#
+#  This module installs variable arch protocol and variable write arch protocol to provide
+#  variable service. This module need work together with SMM authenticated variable module.
+#
+#  Caution: This module requires additional review when modified.
+#  This driver will have external input - variable data.
+#  This external input must be validated carefully to avoid security issues such as
+#  buffer overflow or integer overflow.
+#    The whole SMM authentication variable design relies on the integrity of flash part and SMM.
+#  which is assumed to be protected by platform.  All variable code and metadata in flash/SMM Memory
+#  may not be modified without authorization. If platform fails to protect these resources,
+#  the authentication service provided in this driver will be broken, and the behavior is undefined.
+#
+# Copyright (c) 2010 - 2015, 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.
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = VariableSmmRuntimeDxe
+  MODULE_UNI_FILE                = VariableSmmRuntimeDxe.uni
+  FILE_GUID                      = 9F7DCADE-11EA-448a-A46F-76E003657DD1
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = VariableSmmRuntimeInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64
+#
+#  VIRTUAL_ADDRESS_MAP_CALLBACK  =  VariableAddressChangeEvent
+#
+
+[Sources]
+  VariableSmmRuntimeDxe.c
+  Measurement.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  MemoryAllocationLib
+  BaseLib
+  UefiBootServicesTableLib
+  DebugLib
+  UefiRuntimeLib
+  DxeServicesTableLib
+  UefiDriverEntryPoint
+  TpmMeasurementLib
+
+[Protocols]
+  gEfiVariableWriteArchProtocolGuid             ## PRODUCES
+  gEfiVariableArchProtocolGuid                  ## PRODUCES
+  gEfiSmmCommunicationProtocolGuid              ## CONSUMES
+  ## CONSUMES
+  ## NOTIFY
+  ## UNDEFINED # Used to do smm communication
+  gEfiSmmVariableProtocolGuid
+  gEdkiiVariableLockProtocolGuid                ## PRODUCES
+  gEdkiiVarCheckProtocolGuid                    ## PRODUCES
+
+[Guids]
+  gEfiEventVirtualAddressChangeGuid             ## CONSUMES ## Event
+  gEfiEventExitBootServicesGuid                 ## CONSUMES ## Event
+  ## CONSUMES ## GUID # Locate protocol
+  ## CONSUMES ## GUID # Protocol notify
+  gSmmVariableWriteGuid
+
+  ## SOMETIMES_CONSUMES   ## Variable:L"PK"
+  ## SOMETIMES_CONSUMES   ## Variable:L"KEK"
+  ## SOMETIMES_CONSUMES   ## Variable:L"SecureBoot"
+  gEfiGlobalVariableGuid
+
+  ## SOMETIMES_CONSUMES   ## Variable:L"DB"
+  ## SOMETIMES_CONSUMES   ## Variable:L"DBX"
+  gEfiImageSecurityDatabaseGuid
+
+[Depex]
+  gEfiSmmCommunicationProtocolGuid
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  VariableSmmRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.uni
new file mode 100644
index 0000000000..39cf83edec
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxe.uni
diff --git a/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxeExtra.uni b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxeExtra.uni
new file mode 100644
index 0000000000..e79ea93c81
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/Variable/RuntimeDxe/VariableSmmRuntimeDxeExtra.uni
diff --git a/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.c b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.c
new file mode 100644
index 0000000000..b018eb2ae1
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.c
@@ -0,0 +1,251 @@
+/** @file
+  Implementation of Watchdog Timer Architectural Protocol using UEFI APIs.
+  
+Copyright (c) 2006 - 2010, 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 "WatchdogTimer.h"
+
+//
+// Handle for the Watchdog Timer Architectural Protocol instance produced by this driver
+//
+EFI_HANDLE  mWatchdogTimerHandle = NULL;
+
+//
+// The Watchdog Timer Architectural Protocol instance produced by this driver
+//
+EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  mWatchdogTimer = {
+  WatchdogTimerDriverRegisterHandler,
+  WatchdogTimerDriverSetTimerPeriod,
+  WatchdogTimerDriverGetTimerPeriod
+};
+
+//
+// The watchdog timer period in 100 ns units
+//
+UINT64  mWatchdogTimerPeriod = 0;
+
+//
+// The notification function to call if the watchdig timer fires
+//
+EFI_WATCHDOG_TIMER_NOTIFY  mWatchdogTimerNotifyFunction = NULL;
+
+//
+// The one-shot timer event that is armed when the watchdog timer is enabled
+//
+EFI_EVENT  mWatchdogTimerEvent;
+
+
+/**
+  Notification function that is called if the watchdog timer is fired.
+
+  Notification function for the one-shot timer event that was signaled
+  when the watchdog timer expired. If a handler has been registered with
+  the Watchdog Timer Architectural Protocol, then that handler is called
+  passing in the time period that has passed that cause the watchdog timer
+  to fire.  Then, a call to the Runtime Service ResetSystem() is made to
+  reset the platform.
+
+  @param  Timer     The one-shot timer event that was signaled when the
+                    watchdog timer expired.
+  @param  Context   The context that was registered when the event Timer was created.
+
+**/
+VOID
+EFIAPI
+WatchdogTimerDriverExpires (
+  IN EFI_EVENT    Timer,
+  IN VOID         *Context
+  )
+{
+  REPORT_STATUS_CODE (EFI_ERROR_CODE | EFI_ERROR_MINOR, (EFI_COMPUTING_UNIT_HOST_PROCESSOR | EFI_CU_HP_EC_TIMER_EXPIRED));
+
+  //
+  // If a notification function has been registered, then call it
+  //
+  if (mWatchdogTimerNotifyFunction != NULL) {
+    mWatchdogTimerNotifyFunction (mWatchdogTimerPeriod);
+  }
+  
+  DEBUG ((EFI_D_ERROR, "Watchdog Timer reseting system\n"));
+
+  //
+  // Reset the platform
+  //
+  gRT->ResetSystem (EfiResetCold, EFI_TIMEOUT, 0, NULL);
+}
+
+
+/**
+  Registers a handler that is to be invoked when the watchdog timer fires.
+
+  This function registers a handler that is to be invoked when the watchdog
+  timer fires.  By default, the EFI_WATCHDOG_TIMER protocol will call the
+  Runtime Service ResetSystem() when the watchdog timer fires.  If a
+  NotifyFunction is registered, then the NotifyFunction will be called before
+  the Runtime Service ResetSystem() is called.  If NotifyFunction is NULL, then
+  the watchdog handler is unregistered.  If a watchdog handler is registered,
+  then EFI_SUCCESS is returned.  If an attempt is made to register a handler
+  when a handler is already registered, then EFI_ALREADY_STARTED is returned.
+  If an attempt is made to uninstall a handler when a handler is not installed,
+  then return EFI_INVALID_PARAMETER.
+
+  @param  This                  The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  NotifyFunction        The function to call when the watchdog timer fires.  If this
+                                is NULL, then the handler will be unregistered.
+
+  @retval EFI_SUCCESS           The watchdog timer handler was registered or unregistered.
+  @retval EFI_ALREADY_STARTED   NotifyFunction is not NULL, and a handler is already registered.
+  @retval EFI_INVALID_PARAMETER NotifyFunction is NULL, and a handler was not previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverRegisterHandler (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN EFI_WATCHDOG_TIMER_NOTIFY         NotifyFunction
+  )
+{
+  //
+  // If NotifyFunction is NULL, and a handler was not previously registered,
+  // return EFI_INVALID_PARAMETER.
+  //
+  if (NotifyFunction == NULL && mWatchdogTimerNotifyFunction == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  //
+  // If NotifyFunction is not NULL, and a handler is already registered,
+  // return EFI_ALREADY_STARTED.
+  //
+  if (NotifyFunction != NULL && mWatchdogTimerNotifyFunction != NULL) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  mWatchdogTimerNotifyFunction = NotifyFunction;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Sets the amount of time in the future to fire the watchdog timer.
+
+  This function sets the amount of time to wait before firing the watchdog
+  timer to TimerPeriod 100 ns units.  If TimerPeriod is 0, then the watchdog
+  timer is disabled.
+
+  @param  This              The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  TimerPeriod       The amount of time in 100 ns units to wait before the watchdog
+                            timer is fired.  If TimerPeriod is zero, then the watchdog
+                            timer is disabled.
+
+  @retval EFI_SUCCESS       The watchdog timer has been programmed to fire in Time
+                            100 ns units.
+  @retval EFI_DEVICE_ERROR  A watchdog timer could not be programmed due to a device
+                            error.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverSetTimerPeriod (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN UINT64                            TimerPeriod
+  )
+{
+  mWatchdogTimerPeriod = TimerPeriod;
+
+  return gBS->SetTimer (
+                mWatchdogTimerEvent,
+                (mWatchdogTimerPeriod == 0) ? TimerCancel : TimerRelative,
+                mWatchdogTimerPeriod
+                );
+}
+
+/**
+  Retrieves the amount of time in 100 ns units that the system will wait before firing the watchdog timer.
+
+  This function retrieves the amount of time the system will wait before firing
+  the watchdog timer.  This period is returned in TimerPeriod, and EFI_SUCCESS
+  is returned.  If TimerPeriod is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  @param  This                    The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  TimerPeriod             A pointer to the amount of time in 100 ns units that the system
+                                  will wait before the watchdog timer is fired.  If TimerPeriod of
+                                  zero is returned, then the watchdog timer is disabled.
+
+  @retval EFI_SUCCESS             The amount of time that the system will wait before
+                                  firing the watchdog timer was returned in TimerPeriod.
+  @retval EFI_INVALID_PARAMETER   TimerPeriod is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverGetTimerPeriod (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN UINT64                            *TimerPeriod
+  )
+{
+  if (TimerPeriod == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  *TimerPeriod = mWatchdogTimerPeriod;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Entry point of the Watchdog Timer Architectural Protocol driver.
+
+  @param  ImageHandle   The image handle of this driver.
+  @param  SystemTable   The pointer of EFI_SYSTEM_TABLE.
+
+  @retval EFI_SUCCESS   Watchdog Timer Architectural Protocol successfully installed.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverInitialize (
+  IN EFI_HANDLE        ImageHandle,
+  IN EFI_SYSTEM_TABLE  *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  //
+  // Make sure the Watchdog Timer Architectural Protocol has not been installed in the system yet.
+  //
+  ASSERT_PROTOCOL_ALREADY_INSTALLED (NULL, &gEfiWatchdogTimerArchProtocolGuid);
+
+  //
+  // Create the timer event to implement a simple watchdog timer
+  //
+  Status = gBS->CreateEvent (
+                  EVT_TIMER | EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  WatchdogTimerDriverExpires,
+                  NULL,
+                  &mWatchdogTimerEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Install the Watchdog Timer Arch Protocol onto a new handle
+  //
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                  &mWatchdogTimerHandle,
+                  &gEfiWatchdogTimerArchProtocolGuid,
+                  &mWatchdogTimer,
+                  NULL
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return EFI_SUCCESS;
+}
diff --git a/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.h b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.h
new file mode 100644
index 0000000000..e7bb0b2bfd
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.h
@@ -0,0 +1,108 @@
+/** @file
+  The internal include file for WatchDogTimer module.  
+
+Copyright (c) 2006 - 2008, 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.             
+
+**/
+
+#ifndef _WATCHDOG_TIMER_H_
+#define _WATCHDOG_TIMER_H_
+
+
+
+#include <Uefi.h>
+#include <Library/DebugLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/ReportStatusCodeLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeServicesTableLib.h>
+#include <Protocol/WatchdogTimer.h>
+
+
+/**
+  Registers a handler that is to be invoked when the watchdog timer fires.
+
+  This function registers a handler that is to be invoked when the watchdog
+  timer fires.  By default, the EFI_WATCHDOG_TIMER protocol will call the
+  Runtime Service ResetSystem() when the watchdog timer fires.  If a
+  NotifyFunction is registered, then the NotifyFunction will be called before
+  the Runtime Service ResetSystem() is called.  If NotifyFunction is NULL, then
+  the watchdog handler is unregistered.  If a watchdog handler is registered,
+  then EFI_SUCCESS is returned.  If an attempt is made to register a handler
+  when a handler is already registered, then EFI_ALREADY_STARTED is returned.
+  If an attempt is made to uninstall a handler when a handler is not installed,
+  then return EFI_INVALID_PARAMETER.
+
+  @param  This                  The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  NotifyFunction        The function to call when the watchdog timer fires.  If this
+                                is NULL, then the handler will be unregistered.
+
+  @retval EFI_SUCCESS           The watchdog timer handler was registered or unregistered.
+  @retval EFI_ALREADY_STARTED   NotifyFunction is not NULL, and a handler is already registered.
+  @retval EFI_INVALID_PARAMETER NotifyFunction is NULL, and a handler was not previously registered.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverRegisterHandler (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN EFI_WATCHDOG_TIMER_NOTIFY         NotifyFunction
+  );
+
+/**
+  Sets the amount of time in the future to fire the watchdog timer.
+
+  This function sets the amount of time to wait before firing the watchdog
+  timer to TimerPeriod 100 ns units.  If TimerPeriod is 0, then the watchdog
+  timer is disabled.
+
+  @param  This              The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  TimerPeriod       The amount of time in 100 ns units to wait before the watchdog
+                            timer is fired.  If TimerPeriod is zero, then the watchdog
+                            timer is disabled.
+
+  @retval EFI_SUCCESS       The watchdog timer has been programmed to fire in Time
+                            100 ns units.
+  @retval EFI_DEVICE_ERROR  A watchdog timer could not be programmed due to a device
+                            error.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverSetTimerPeriod (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN UINT64                            TimerPeriod
+  );
+
+/**
+  Retrieves the amount of time in 100 ns units that the system will wait before firing the watchdog timer.
+
+  This function retrieves the amount of time the system will wait before firing
+  the watchdog timer.  This period is returned in TimerPeriod, and EFI_SUCCESS
+  is returned.  If TimerPeriod is NULL, then EFI_INVALID_PARAMETER is returned.
+
+  @param  This                    The EFI_WATCHDOG_TIMER_ARCH_PROTOCOL instance.
+  @param  TimerPeriod             A pointer to the amount of time in 100 ns units that the system
+                                  will wait before the watchdog timer is fired.  If TimerPeriod of
+                                  zero is returned, then the watchdog timer is disabled.
+
+  @retval EFI_SUCCESS             The amount of time that the system will wait before
+                                  firing the watchdog timer was returned in TimerPeriod.
+  @retval EFI_INVALID_PARAMETER   TimerPeriod is NULL.
+
+**/
+EFI_STATUS
+EFIAPI
+WatchdogTimerDriverGetTimerPeriod (
+  IN EFI_WATCHDOG_TIMER_ARCH_PROTOCOL  *This,
+  IN UINT64                            *TimerPeriod
+  );
+
+#endif
diff --git a/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.inf b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.inf
new file mode 100644
index 0000000000..bde84574b9
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.inf
@@ -0,0 +1,56 @@
+## @file
+# Generic watchdog timer driver producing Watchdog Timer Architectural Protocol using UEFI APIs.
+#
+# Copyright (c) 2006 - 2014, 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.
+#
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = WatchdogTimer
+  MODULE_UNI_FILE                = WatchdogTimer.uni
+  FILE_GUID                      = F099D67F-71AE-4c36-B2A3-DCEB0EB2B7D8
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = WatchdogTimerDriverInitialize
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = IA32 X64 IPF EBC
+#
+
+[Packages]
+  MdePkg/MdePkg.dec
+
+[Sources]
+  WatchdogTimer.h
+  WatchdogTimer.c
+
+[LibraryClasses]
+  UefiRuntimeServicesTableLib
+  UefiBootServicesTableLib
+  ReportStatusCodeLib
+  UefiDriverEntryPoint
+  DebugLib
+
+[Protocols]
+  gEfiWatchdogTimerArchProtocolGuid             ## PRODUCES
+
+[Depex]
+  gEfiTimerArchProtocolGuid
+
+# [Event]
+# EVENT_TYPE_RELATIVE_TIMER ## CONSUMES
+#
+
+[UserExtensions.TianoCore."ExtraFiles"]
+  WatchdogTimerExtra.uni
+\ No newline at end of file
diff --git a/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.uni b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.uni
new file mode 100644
index 0000000000..628d3fe504
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimer.uni
diff --git a/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimerExtra.uni b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimerExtra.uni
new file mode 100644
index 0000000000..e3595cbce6
--- /dev/null
+++ b/Core/MdeModulePkg/Universal/WatchdogTimerDxe/WatchdogTimerExtra.uni
author	Guo Mang <mang.guo@intel.com>	2016-12-22 15:55:38 +0800
committer	Guo Mang <mang.guo@intel.com>	2016-12-26 19:14:37 +0800
commit	7f05fa00f73038b425002566d3afe6c3ade2ccdb (patch)
tree	297e208d4ade33a8bb3d5d20f72c53e0d134e003 /Core
parent	ed2ecce34b3830562c4239093a41ba92d76d5f31 (diff)
download	edk2-platforms-7f05fa00f73038b425002566d3afe6c3ade2ccdb.tar.xz